From 78b0c2ddc0f8c2fe103f4fce730adc46ef29956f Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 30 May 2016 20:45:18 +0900
Subject: [PATCH 001/217] init commit

---
 rtc/AutoBalancer/AutoBalancer.cpp | 249 +++++++++++++++++++++++++++++-
 rtc/AutoBalancer/AutoBalancer.h   |  16 ++
 rtc/AutoBalancer/CMakeLists.txt   |   4 +-
 rtc/AutoBalancer/GaitGenerator.h  |   8 +-
 4 files changed, 268 insertions(+), 9 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 8edc84ad2aa..454f9c3b3cc 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -16,6 +16,28 @@
 #include <hrpUtil/MatrixSolvers.h>
 #include "hrpsys/util/Hrpsys.h"
 
+//SHM関連
+#include <errno.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sys/ipc.h>
+#include <sys/shm.h>
+int     shmid;
+key_t   key;
+typedef struct{
+	double com[3];
+	double rfpos[3];
+	double lfpos[3];
+	double rhpos[3];
+	double lhpos[3];
+	double rfwrench[6];
+	double lfwrench[6];
+	double zmp[3];
+	bool startsync;
+}humanpose_t;
+humanpose_t *humanpose;
+humanpose_t filt_hp_data,hp_data_old;
+
 
 typedef coil::Guard<coil::Mutex> Guard;
 using namespace rats;
@@ -49,6 +71,14 @@ AutoBalancer::AutoBalancer(RTC::Manager* manager)
       m_zmpIn("zmpIn", m_zmp),
       m_optionalDataIn("optionalData", m_optionalData),
       m_emergencySignalIn("emergencySignal", m_emergencySignal),
+      //ishiguro
+      m_htzmpIn("htzmpIn", m_htzmp),
+      m_htcomIn("htcomIn", m_htcom),
+      m_htrfIn("htrfIn", m_htrf),
+      m_htlfIn("htlfIn", m_htlf),
+      m_htrhIn("htrhIn", m_htrh),
+      m_htlhIn("htlhIn", m_htlh),
+
       m_qOut("q", m_qRef),
       m_zmpOut("zmpOut", m_zmp),
       m_basePosOut("basePosOut", m_basePos),
@@ -90,6 +120,13 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     addInPort("zmpIn", m_zmpIn);
     addInPort("optionalData", m_optionalDataIn);
     addInPort("emergencySignal", m_emergencySignalIn);
+    //ishiguro
+    addInPort("htzmpIn", m_htzmpIn);
+    addInPort("htcomIn", m_htcomIn);
+    addInPort("htrfIn", m_htrfIn);
+    addInPort("htlfIn", m_htlfIn);
+    addInPort("htrhIn", m_htrhIn);
+    addInPort("htlhIn", m_htlhIn);
 
     // Set OutPort buffer
     addOutPort("q", m_qOut);
@@ -339,6 +376,30 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     rot_ik_thre = (1e-2)*M_PI/180.0; // [rad]
     ik_error_debug_print_freq = static_cast<int>(0.2/m_dt); // once per 0.2 [s]
 
+
+    //SHM my2
+	if((key = ftok("/tmp/shm.dat", 'R')) == -1){
+		printf("WARN: ftok() failed retry\n");
+		system("touch /tmp/shm.dat");
+		if((key = ftok("/tmp/shm.dat", 'R')) == -1){
+	    	perror("ERROR: ftok() failed exit\n");
+	        return RTC::RTC_ERROR;
+		}
+	}
+	if((shmid = shmget(key, sizeof(humanpose_t), 0666)) < 0){
+		printf("WARN: shmget() failed retry\n");
+		shmid = shmget(key, sizeof(humanpose_t), S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH | IPC_CREAT);
+	    if (shmid == -1)  {
+	    	perror("ERROR: shmget() failed exit\n");
+	        return RTC::RTC_ERROR;
+	    }
+	}
+    humanpose = (humanpose_t *)shmat(shmid, (void *)0, 0);
+    if (humanpose == NULL) {
+        perror("shmat");
+        return RTC::RTC_ERROR;
+    }
+
     return RTC::RTC_OK;
 }
 
@@ -346,6 +407,10 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
 
 RTC::ReturnCode_t AutoBalancer::onFinalize()
 {
+	//SHM
+    if (shmdt(humanpose) == -1){       perror("shmdt");    }
+    if (shmctl(shmid, IPC_RMID, 0) == -1){        perror("shmctl");    }
+
   delete zmp_offset_interpolator;
   delete transition_interpolator;
   delete adjust_footstep_interpolator;
@@ -442,6 +507,43 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
         //     gg->emergency_stop();
         // }
     }
+    //for human tracker
+    if (m_htzmpIn.isNew()){
+    	m_htzmpIn.read();
+    	input_htzmp(0) = m_htzmp.data.x;
+    	input_htzmp(1) = m_htzmp.data.y;
+    	input_htzmp(2) = m_htzmp.data.z;
+    }
+    if (m_htcomIn.isNew()){
+    	m_htcomIn.read();
+    	input_htcom(0) = m_htcom.data.x;
+    	input_htcom(1) = m_htcom.data.y;
+    	input_htcom(2) = m_htcom.data.z;
+    }
+    if (m_htrfIn.isNew()){
+    	m_htrfIn.read();
+    	input_htrf(0) = m_htrf.data.x;
+    	input_htrf(1) = m_htrf.data.y;
+    	input_htrf(2) = m_htrf.data.z;
+    }
+    if (m_htlfIn.isNew()){
+    	m_htlfIn.read();
+    	input_htlf(0) = m_htlf.data.x;
+    	input_htlf(1) = m_htlf.data.y;
+    	input_htlf(2) = m_htlf.data.z;
+    }
+    if (m_htrhIn.isNew()){
+    	m_htrhIn.read();
+    	input_htrh(0) = m_htrh.data.x;
+    	input_htrh(1) = m_htrh.data.y;
+    	input_htrh(2) = m_htrh.data.z;
+    }
+    if (m_htlhIn.isNew()){
+    	m_htlhIn.read();
+    	input_htlh(0) = m_htlh.data.x;
+    	input_htlh(1) = m_htlh.data.y;
+    	input_htlh(2) = m_htlh.data.z;
+    }
 
     Guard guard(m_mutex);
     hrp::Vector3 ref_basePos;
@@ -530,6 +632,45 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
       m_cog.data.y = ref_cog(1);
       m_cog.data.z = ref_cog(2);
       m_cog.tm = m_qRef.tm;
+
+
+      // my test zmp SHM
+
+      double rfzmp[2],lfzmp[2];
+      const double F_H_OFFSET = 0.03;
+      const double rfpos[3] = {0,-0.1,0},lfpos[3] = {0,0.1,0};
+
+		if( humanpose->rfwrench[2] > 1.0e-6 ){
+			rfzmp[0] = ( -humanpose->rfwrench[4] - humanpose->rfwrench[0] * F_H_OFFSET + humanpose->rfwrench[2] * 0 ) / humanpose->rfwrench[2];
+			rfzmp[1] = ( humanpose->rfwrench[3] - humanpose->rfwrench[1] * F_H_OFFSET + humanpose->rfwrench[2] * 0 ) / humanpose->rfwrench[2];
+		}
+		if( humanpose->lfwrench[2] > 1.0e-6 ){
+			lfzmp[0] = ( -humanpose->lfwrench[4] - humanpose->lfwrench[0] * F_H_OFFSET + humanpose->lfwrench[2] * 0 ) / humanpose->lfwrench[2];
+			lfzmp[1] = ( humanpose->lfwrench[3] - humanpose->lfwrench[1] * F_H_OFFSET + humanpose->lfwrench[2] * 0 ) / humanpose->lfwrench[2];
+		}
+		humanpose->zmp[0] = ( rfzmp[0]*humanpose->rfwrench[2] + lfzmp[0]*humanpose->lfwrench[2] ) / ( humanpose->rfwrench[2] + humanpose->lfwrench[2] );
+		humanpose->zmp[1] = ( rfzmp[1]*humanpose->rfwrench[2] + lfzmp[1]*humanpose->lfwrench[2] ) / ( humanpose->rfwrench[2] + humanpose->lfwrench[2]);
+		humanpose->zmp[2] = 0;
+
+//	      for(int i=0;i<6;i++)cout<<"rfw["<<i<<"]"<<humanpose->rfwrench[i];
+//	      for(int i=0;i<6;i++)cout<<"lfw["<<i<<"]"<<humanpose->lfwrench[i];
+//	      for(int i=0;i<2;i++)cout<<"zmp["<<i<<"]"<<humanpose->zmp[i];
+
+      //debug表示
+//			printf("[L] %+06.1f,%+06.1f,%+06.1f,%+06.1f,%+06.1f,%+06.1f [R] %+06.1f,%+06.1f,%+06.1f,%+06.1f,%+06.1f,%+06.1f [ZMP] %+06.3f,%+06.3f\n",
+//					humanpose->lfwrench[0],humanpose->lfwrench[1],humanpose->lfwrench[2],humanpose->lfwrench[3],humanpose->lfwrench[4],humanpose->lfwrench[5],
+//					humanpose->rfwrench[0],humanpose->rfwrench[1],humanpose->rfwrench[2],humanpose->rfwrench[3],humanpose->rfwrench[4],humanpose->rfwrench[5],
+//					humanpose->zmp[0],humanpose->zmp[1]);
+      humanpose->startsync = true;//test
+	   if(humanpose->startsync){
+//ZMP上書き
+//	      m_zmp.data.x = humanpose->zmp[0]*0.5 - filt_hp_data.com[0];//
+		  m_zmp.data.y = filt_hp_data.zmp[1] - filt_hp_data.com[1];//
+// COM上書き
+//	  m_cog.data.x = 0.0558423 + filt_hp_data.com[0];
+	  m_cog.data.y = filt_hp_data.com[1];
+	  }
+
       // sbpCogOffset
       m_sbpCogOffset.data.x = sbp_cog_offset(0);
       m_sbpCogOffset.data.y = sbp_cog_offset(1);
@@ -686,15 +827,15 @@ void AutoBalancer::getTargetParameters()
               ABCIKparam& tmpikp = ikp[leg_names[i]];
               ee_pos.push_back(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]);
           }
-          double alpha = (ref_zmp - ee_pos[1]).norm() / ((ee_pos[0] - ref_zmp).norm() + (ee_pos[1] - ref_zmp).norm());
+          double alpha = (ref_zmp - ee_pos[1]).norm() / (ee_pos[0] - ee_pos[1]).norm();
           if (alpha>1.0) alpha = 1.0;
           if (alpha<0.0) alpha = 0.0;
           if (DEBUGP) {
           std::cerr << "[" << m_profile.instance_name << "] alpha:" << alpha << std::endl;
           }
           double mg = m_robot->totalMass() * gg->get_gravitational_acceleration();
-          m_force[0].data[2] = alpha * mg;
-          m_force[1].data[2] = (1-alpha) * mg;
+          m_force[0].data[0] = alpha * mg;
+          m_force[1].data[0] = (1-alpha) * mg;
       }
       // set limbCOPOffset
       {
@@ -857,6 +998,7 @@ void AutoBalancer::getTargetParameters()
         if ( gg_is_walking && gg->get_lcg_count() == gg->get_overwrite_check_timing()+2 ) {
             hrp::Vector3 vel_htc(calc_vel_from_hand_error(tmp_fix_coords));
             gg->set_offset_velocity_param(vel_htc(0), vel_htc(1) ,vel_htc(2));
+            gg->set_offset_velocity_param(1,2,3);
         }//  else {
         //     if ( gg_is_walking && gg->get_lcg_count() == static_cast<size_t>(gg->get_default_step_time()/(2*m_dt))-1) {
         //         gg->set_offset_velocity_param(0,0,0);
@@ -914,8 +1056,66 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri
   m_robot->calcForwardKinematics();
 }
 
+static bool moveleft = false;
 bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 {
+//  cout<<"****************** test ********************"<<endl;
+	static int rfuplevel,lfuplevel;
+
+	if(humanpose->startsync){
+		if(param.target_link->name == "RLEG_JOINT5"){
+			if(humanpose->rfwrench[2] < 20.0 && humanpose->lfwrench[2] > 20.0){
+//			if(humanpose->rfwrench[2] < 20.0){
+				if(rfuplevel < 100){
+					rfuplevel++;
+					filt_hp_data.rfpos[2] = 0.025 - 0.025*cos(M_PI*rfuplevel/100);
+					//filt_hp_data.rfpos[1] -= 0.001;//歩行遊び
+				}
+				cout<<"RUP"<<endl;
+			}else{
+				if(rfuplevel > 0){
+					rfuplevel--;
+					filt_hp_data.rfpos[2] = 0.025 - 0.025*cos(M_PI*rfuplevel/100);
+					//if(filt_hp_data.rfpos[2] == 0.0)moveleft = true;//歩行遊び
+				}
+			}
+			param.target_p0(0) = 0.0 + filt_hp_data.rfpos[0];
+			param.target_p0(1) = -0.1 + filt_hp_data.rfpos[1];
+			param.target_p0(2) = 0.1 + filt_hp_data.rfpos[2];
+		}
+		if(param.target_link->name == "LLEG_JOINT5"){
+			if(humanpose->lfwrench[2] < 20.0 && humanpose->rfwrench[2] > 20.0){
+//			if(humanpose->lfwrench[2] < 20.0){
+				if(lfuplevel < 100){
+					lfuplevel++;
+					filt_hp_data.lfpos[2] = 0.025 - 0.025*cos(M_PI*lfuplevel/100);
+					//filt_hp_data.lfpos[1] -= 0.001;//歩行遊び
+				}
+				cout<<"LUP"<<endl;
+			}else{
+				if(lfuplevel > 0){
+					lfuplevel--;
+					filt_hp_data.lfpos[2] = 0.025 - 0.025*cos(M_PI*lfuplevel/100);
+					//if(filt_hp_data.lfpos[2] == 0.0)moveleft = true;//歩行遊び
+				}
+			}
+			param.target_p0(0) = 0.0 + filt_hp_data.lfpos[0];
+			param.target_p0(1) = 0.1 + filt_hp_data.lfpos[1];
+			param.target_p0(2) = 0.1 + filt_hp_data.lfpos[2];
+		}
+		if(param.target_link->name == "RARM_JOINT7"){
+			param.target_p0(0) = 0.151 + filt_hp_data.rhpos[0];
+			param.target_p0(1) = -0.374 + filt_hp_data.rhpos[1];
+			param.target_p0(2) = 0.973 + filt_hp_data.rhpos[2];
+		}
+		if(param.target_link->name == "LARM_JOINT7"){
+			param.target_p0(0) = 0.151 + filt_hp_data.lhpos[0];
+			param.target_p0(1) = 0.374 + filt_hp_data.lhpos[1];
+			param.target_p0(2) = 0.973 + filt_hp_data.lhpos[2];
+		}
+	}
+
+
   param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
   // IK check
   hrp::Vector3 vel_p, vel_r;
@@ -942,6 +1142,7 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
   return true;
 }
 
+
 void AutoBalancer::solveLimbIK ()
 {
   for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
@@ -962,6 +1163,48 @@ void AutoBalancer::solveLimbIK ()
   dif_cog(2) = m_robot->rootLink()->p(2) - target_root_p(2);
   m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog;
   m_robot->rootLink()->R = target_root_R;
+//mytest
+  #define MAXVEL 0.002
+
+  for(int i=0;i<3;i++){
+//	  filt_hp_data.com[i] = 0.99*filt_hp_data.com[i] + 0.01*(humanpose->com[i]);
+//	  filt_hp_data.rfpos[i] = 0.99*filt_hp_data.rfpos[i] + 0.01*(humanpose->rfpos[i]);//足ロック
+//	  filt_hp_data.lfpos[i] = 0.99*filt_hp_data.lfpos[i] + 0.01*(humanpose->lfpos[i]);//足ロック
+//	  filt_hp_data.rhpos[i] = 0.99*filt_hp_data.rhpos[i] + 0.01*(humanpose->rhpos[i]);
+//	  filt_hp_data.lhpos[i] = 0.99*filt_hp_data.lhpos[i] + 0.01*(humanpose->lhpos[i]);
+//	  filt_hp_data.zmp[i] = 0.99*filt_hp_data.zmp[i] + 0.01*(humanpose->zmp[i]);
+
+	  filt_hp_data.zmp[i] = 0.99*filt_hp_data.zmp[i] + 0.01*(input_htzmp(i));
+	  filt_hp_data.com[i] = 0.99*filt_hp_data.com[i] + 0.01*(input_htcom(i));
+	  filt_hp_data.rfpos[i] = 0.99*filt_hp_data.rfpos[i] + 0.01*(input_htrf(i));
+	  filt_hp_data.lfpos[i] = 0.99*filt_hp_data.lfpos[i] + 0.01*(input_htlf(i));
+	  filt_hp_data.rhpos[i] = 0.99*filt_hp_data.rhpos[i] + 0.01*(input_htrh(i));
+	  filt_hp_data.lhpos[i] = 0.99*filt_hp_data.lhpos[i] + 0.01*(input_htlh(i));
+
+
+
+	  if(filt_hp_data.com[i] - hp_data_old.com[i] > MAXVEL)filt_hp_data.com[i] = hp_data_old.com[i] + MAXVEL;
+	  if(filt_hp_data.com[i] - hp_data_old.com[i] < -MAXVEL)filt_hp_data.com[i] = hp_data_old.com[i] - MAXVEL;
+	  hp_data_old.com[i] = filt_hp_data.com[i];
+  }
+//  cout<<"COM"<<m_robot->calcCM()[0]<<","<<m_robot->calcCM()[1]<<","<<m_robot->calcCM()[2]<<endl;
+  filt_hp_data.com[0] = 0.0;//重心前方向ロック
+
+  m_robot->rootLink()->p(0) = 0.0558423 + filt_hp_data.com[0];
+  m_robot->rootLink()->p(1) = 0+ filt_hp_data.com[1];//safetest
+  m_robot->rootLink()->p(2) = 0.976372 + filt_hp_data.com[2];
+
+static int movelcnt;
+	if(moveleft){
+		if(movelcnt<5000){
+			m_robot->rootLink()->p(1) += 0.00001;//遊び
+			movelcnt++;
+		}else{
+			movelcnt = 0;
+			moveleft = false;
+		}
+	}
+
   // Fix for toe joint
   for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
       if (it->second.is_active && it->second.has_toe_joint && gg->get_use_toe_joint()) {
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index e2c03e6e2e1..e8214e339b9 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -135,6 +135,19 @@ class AutoBalancer
   InPort<TimedLong> m_emergencySignalIn;
   // for debug
   TimedPoint3D m_cog;
+  //for human tracker
+  TimedPoint3D m_htzmp;
+  InPort<TimedPoint3D> m_htzmpIn;
+  TimedPoint3D m_htcom;
+  InPort<TimedPoint3D> m_htcomIn;
+  TimedPoint3D m_htrf;
+  InPort<TimedPoint3D> m_htrfIn;
+  TimedPoint3D m_htlf;
+  InPort<TimedPoint3D> m_htlfIn;
+  TimedPoint3D m_htrh;
+  InPort<TimedPoint3D> m_htrhIn;
+  TimedPoint3D m_htlh;
+  InPort<TimedPoint3D> m_htlhIn;
   
   // </rtc-template>
 
@@ -251,6 +264,9 @@ class AutoBalancer
   enum {MODE_NO_FORCE, MODE_REF_FORCE} use_force;
   std::vector<hrp::Vector3> ref_forces;
 
+  //for human tracker
+  hrp::Vector3 input_htzmp, input_htcom, input_htrf, input_htlf, input_htrh, input_htlh;
+
   unsigned int m_debugLevel;
   bool is_legged_robot, is_stop_mode, has_ik_failed, is_hand_fix_mode, is_hand_fix_initial;
   int loop, ik_error_debug_print_freq;
diff --git a/rtc/AutoBalancer/CMakeLists.txt b/rtc/AutoBalancer/CMakeLists.txt
index 76d3149ce01..7f959e0f66d 100644
--- a/rtc/AutoBalancer/CMakeLists.txt
+++ b/rtc/AutoBalancer/CMakeLists.txt
@@ -33,7 +33,7 @@ add_test(testGaitGeneratorTest11 testGaitGenerator --test11 --use-gnuplot false)
 add_test(testGaitGeneratorTest12 testGaitGenerator --test12 --use-gnuplot false)
 
 install(TARGETS ${target}
-  RUNTIME DESTINATION bin
-  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin CONFIGURATIONS Release Debug
+  LIBRARY DESTINATION lib CONFIGURATIONS Release Debug
 )
 
diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h
index e39e77ff191..7f56a295707 100644
--- a/rtc/AutoBalancer/GaitGenerator.h
+++ b/rtc/AutoBalancer/GaitGenerator.h
@@ -279,7 +279,7 @@ namespace rats
               zmp_weight_interpolator->clear();
               double zmp_weight_initial_value[4] = {zmp_weight_map[RLEG], zmp_weight_map[LLEG], zmp_weight_map[RARM], zmp_weight_map[LARM]};
               zmp_weight_interpolator->set(zmp_weight_initial_value);
-              zmp_weight_interpolator->setGoal(zmp_weight_array, 2.0, true);
+              zmp_weight_interpolator->go(zmp_weight_array, 2.0, true);
           } else {
               std::cerr << "zmp_weight_map cannot be set because interpolating." << std::endl;
           }
@@ -311,10 +311,10 @@ namespace rats
       hrp::Vector3 pos, vel, acc; // [m], [m/s], [m/s^2]
       double dt; // [s]
       // Implement hoffarbib to configure remain_time;
-      void hoffarbib_interpolation (const double tmp_remain_time, const hrp::Vector3& tmp_goal, const hrp::Vector3 tmp_goal_vel = hrp::Vector3::Zero(), const hrp::Vector3 tmp_goal_acc = hrp::Vector3::Zero())
+      void hoffarbib_interpolation (const double tmp_remain_time, const hrp::Vector3& tmp_goal)
       {
-        hrp::Vector3 jerk = (-9.0/ tmp_remain_time) * (acc - tmp_goal_acc / 3.0) +
-          (-36.0 / (tmp_remain_time * tmp_remain_time)) * (tmp_goal_vel * 2.0 / 3.0 + vel) +
+        hrp::Vector3 jerk = (-9.0/ tmp_remain_time) * acc +
+          (-36.0 / (tmp_remain_time * tmp_remain_time)) * vel +
           (60.0 / (tmp_remain_time * tmp_remain_time * tmp_remain_time)) * (tmp_goal - pos);
         acc = acc + dt * jerk;
         vel = vel + dt * acc;

From 00b702ba4e5db86b46d2f91e2c926d6973c00537 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 8 Jun 2016 13:56:18 +0900
Subject: [PATCH 002/217] test for human tracker

---
 idl/AutoBalancerService.idl                   |   7 +
 python/hrpsys_config.py                       |   8 +
 rtc/AutoBalancer/AutoBalancer.cpp             | 233 ++++++++++--------
 rtc/AutoBalancer/AutoBalancer.h               |   8 +-
 rtc/AutoBalancer/AutoBalancerService_impl.cpp |   7 +-
 rtc/AutoBalancer/AutoBalancerService_impl.h   |   1 +
 6 files changed, 164 insertions(+), 100 deletions(-)

diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl
index ccbc974b5bc..a278d03bc69 100644
--- a/idl/AutoBalancerService.idl
+++ b/idl/AutoBalancerService.idl
@@ -413,5 +413,12 @@ module OpenHRP
      * @return true if set successfully, false otherwise
      */
     boolean releaseEmergencyStop();
+
+    /**
+     * @brief Stop stepping immediately.
+     * @param
+     * @return true if set successfully, false otherwise
+     */
+    boolean startHumanSyncAfter5sec();
   };
 };
diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index aaef962ff4f..f6c1e1db542 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -2080,6 +2080,14 @@ def setFootStepsWithParam(self, footstep, stepparams, overwrite_fs_idx = 0):
         '''
         self.abc_svc.setFootStepsWithParam(footstep, stepparams, overwrite_fs_idx)
 
+
+    def startHumanSyncAfter5sec(self):
+        '''!@brief
+        startHumanSyncAfter5sec mode
+        '''
+        self.abc_svc.startHumanSyncAfter5sec()
+
+
     # ##
     # ## initialize
     # ##
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 454f9c3b3cc..d3adac1a290 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -17,13 +17,13 @@
 #include "hrpsys/util/Hrpsys.h"
 
 //SHM関連
-#include <errno.h>
-#include <sys/stat.h>
-#include <sys/types.h>
-#include <sys/ipc.h>
-#include <sys/shm.h>
-int     shmid;
-key_t   key;
+//#include <errno.h>
+//#include <sys/stat.h>
+//#include <sys/types.h>
+//#include <sys/ipc.h>
+//#include <sys/shm.h>
+//int     shmid;
+//key_t   key;
 typedef struct{
 	double com[3];
 	double rfpos[3];
@@ -35,8 +35,10 @@ typedef struct{
 	double zmp[3];
 	bool startsync;
 }humanpose_t;
-humanpose_t *humanpose;
+humanpose_t humanpose;
 humanpose_t filt_hp_data,hp_data_old;
+bool startCountdownForHumanSync = false;
+bool HumanSyncOn = false;
 
 
 typedef coil::Guard<coil::Mutex> Guard;
@@ -73,6 +75,8 @@ AutoBalancer::AutoBalancer(RTC::Manager* manager)
       m_emergencySignalIn("emergencySignal", m_emergencySignal),
       //ishiguro
       m_htzmpIn("htzmpIn", m_htzmp),
+      m_htrfwIn("htrfwIn", m_htrfw),
+      m_htlfwIn("htlfwIn", m_htlfw),
       m_htcomIn("htcomIn", m_htcom),
       m_htrfIn("htrfIn", m_htrf),
       m_htlfIn("htlfIn", m_htlf),
@@ -122,6 +126,8 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     addInPort("emergencySignal", m_emergencySignalIn);
     //ishiguro
     addInPort("htzmpIn", m_htzmpIn);
+    addInPort("htrfwIn", m_htrfwIn);
+    addInPort("htlfwIn", m_htlfwIn);
     addInPort("htcomIn", m_htcomIn);
     addInPort("htrfIn", m_htrfIn);
     addInPort("htlfIn", m_htlfIn);
@@ -180,6 +186,9 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     qorg.resize(m_robot->numJoints());
     qrefv.resize(m_robot->numJoints());
     m_baseTform.data.length(12);
+    //ishiguro
+    m_htrfw.data.length(6);
+    m_htlfw.data.length(6);
 
     control_mode = MODE_IDLE;
     loop = 0;
@@ -377,28 +386,28 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     ik_error_debug_print_freq = static_cast<int>(0.2/m_dt); // once per 0.2 [s]
 
 
-    //SHM my2
-	if((key = ftok("/tmp/shm.dat", 'R')) == -1){
-		printf("WARN: ftok() failed retry\n");
-		system("touch /tmp/shm.dat");
-		if((key = ftok("/tmp/shm.dat", 'R')) == -1){
-	    	perror("ERROR: ftok() failed exit\n");
-	        return RTC::RTC_ERROR;
-		}
-	}
-	if((shmid = shmget(key, sizeof(humanpose_t), 0666)) < 0){
-		printf("WARN: shmget() failed retry\n");
-		shmid = shmget(key, sizeof(humanpose_t), S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH | IPC_CREAT);
-	    if (shmid == -1)  {
-	    	perror("ERROR: shmget() failed exit\n");
-	        return RTC::RTC_ERROR;
-	    }
-	}
-    humanpose = (humanpose_t *)shmat(shmid, (void *)0, 0);
-    if (humanpose == NULL) {
-        perror("shmat");
-        return RTC::RTC_ERROR;
-    }
+//    //SHM my2
+//	if((key = ftok("/tmp/shm.dat", 'R')) == -1){
+//		printf("WARN: ftok() failed retry\n");
+//		system("touch /tmp/shm.dat");
+//		if((key = ftok("/tmp/shm.dat", 'R')) == -1){
+//	    	perror("ERROR: ftok() failed exit\n");
+//	        return RTC::RTC_ERROR;
+//		}
+//	}
+//	if((shmid = shmget(key, sizeof(humanpose_t), 0666)) < 0){
+//		printf("WARN: shmget() failed retry\n");
+//		shmid = shmget(key, sizeof(humanpose_t), S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH | IPC_CREAT);
+//	    if (shmid == -1)  {
+//	    	perror("ERROR: shmget() failed exit\n");
+//	        return RTC::RTC_ERROR;
+//	    }
+//	}
+//    humanpose = (humanpose_t *)shmat(shmid, (void *)0, 0);
+//    if (humanpose == NULL) {
+//        perror("shmat");
+//        return RTC::RTC_ERROR;
+//    }
 
     return RTC::RTC_OK;
 }
@@ -408,8 +417,8 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
 RTC::ReturnCode_t AutoBalancer::onFinalize()
 {
 	//SHM
-    if (shmdt(humanpose) == -1){       perror("shmdt");    }
-    if (shmctl(shmid, IPC_RMID, 0) == -1){        perror("shmctl");    }
+//    if (shmdt(humanpose) == -1){       perror("shmdt");    }
+//    if (shmctl(shmid, IPC_RMID, 0) == -1){        perror("shmctl");    }
 
   delete zmp_offset_interpolator;
   delete transition_interpolator;
@@ -510,39 +519,51 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     //for human tracker
     if (m_htzmpIn.isNew()){
     	m_htzmpIn.read();
-    	input_htzmp(0) = m_htzmp.data.x;
-    	input_htzmp(1) = m_htzmp.data.y;
-    	input_htzmp(2) = m_htzmp.data.z;
+//    	humanpose.zmp[0] = m_htzmp.data.x;//zmpはこっちで計算
+//    	humanpose.zmp[1] = m_htzmp.data.y;
+//    	humanpose.zmp[2] = m_htzmp.data.z;
+    }
+    if (m_htrfwIn.isNew()){
+    	m_htrfwIn.read();
+    	for(int i=0;i<6;i++){
+    		humanpose.rfwrench[i] = m_htrfw.data[i];
+    	}
+    }
+    if (m_htlfwIn.isNew()){
+    	m_htlfwIn.read();
+    	for(int i=0;i<6;i++){
+    		humanpose.lfwrench[i] = m_htlfw.data[i];
+    	}
     }
     if (m_htcomIn.isNew()){
     	m_htcomIn.read();
-    	input_htcom(0) = m_htcom.data.x;
-    	input_htcom(1) = m_htcom.data.y;
-    	input_htcom(2) = m_htcom.data.z;
+    	humanpose.com[0] = m_htcom.data.x;
+    	humanpose.com[1] = m_htcom.data.y;
+    	humanpose.com[2] = m_htcom.data.z;
     }
     if (m_htrfIn.isNew()){
     	m_htrfIn.read();
-    	input_htrf(0) = m_htrf.data.x;
-    	input_htrf(1) = m_htrf.data.y;
-    	input_htrf(2) = m_htrf.data.z;
+    	humanpose.rfpos[0] = m_htrf.data.x;
+    	humanpose.rfpos[1] = m_htrf.data.y;
+    	humanpose.rfpos[2] = m_htrf.data.z;
     }
     if (m_htlfIn.isNew()){
     	m_htlfIn.read();
-    	input_htlf(0) = m_htlf.data.x;
-    	input_htlf(1) = m_htlf.data.y;
-    	input_htlf(2) = m_htlf.data.z;
+    	humanpose.lfpos[0] = m_htlf.data.x;
+    	humanpose.lfpos[1] = m_htlf.data.y;
+    	humanpose.lfpos[2] = m_htlf.data.z;
     }
     if (m_htrhIn.isNew()){
     	m_htrhIn.read();
-    	input_htrh(0) = m_htrh.data.x;
-    	input_htrh(1) = m_htrh.data.y;
-    	input_htrh(2) = m_htrh.data.z;
+    	humanpose.rhpos[0] = m_htrh.data.x;
+    	humanpose.rhpos[1] = m_htrh.data.y;
+    	humanpose.rhpos[2] = m_htrh.data.z;
     }
     if (m_htlhIn.isNew()){
     	m_htlhIn.read();
-    	input_htlh(0) = m_htlh.data.x;
-    	input_htlh(1) = m_htlh.data.y;
-    	input_htlh(2) = m_htlh.data.z;
+    	humanpose.lhpos[0] = m_htlh.data.x;
+    	humanpose.lhpos[1] = m_htlh.data.y;
+    	humanpose.lhpos[2] = m_htlh.data.z;
     }
 
     Guard guard(m_mutex);
@@ -640,32 +661,33 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
       const double F_H_OFFSET = 0.03;
       const double rfpos[3] = {0,-0.1,0},lfpos[3] = {0,0.1,0};
 
-		if( humanpose->rfwrench[2] > 1.0e-6 ){
-			rfzmp[0] = ( -humanpose->rfwrench[4] - humanpose->rfwrench[0] * F_H_OFFSET + humanpose->rfwrench[2] * 0 ) / humanpose->rfwrench[2];
-			rfzmp[1] = ( humanpose->rfwrench[3] - humanpose->rfwrench[1] * F_H_OFFSET + humanpose->rfwrench[2] * 0 ) / humanpose->rfwrench[2];
+		if( humanpose.rfwrench[2] > 1.0e-6 ){
+			rfzmp[0] = ( -humanpose.rfwrench[4] - humanpose.rfwrench[0] * F_H_OFFSET + humanpose.rfwrench[2] * 0 ) / humanpose.rfwrench[2] + rfpos[0];
+			rfzmp[1] = ( humanpose.rfwrench[3] - humanpose.rfwrench[1] * F_H_OFFSET + humanpose.rfwrench[2] * 0 ) / humanpose.rfwrench[2] + rfpos[1];
 		}
-		if( humanpose->lfwrench[2] > 1.0e-6 ){
-			lfzmp[0] = ( -humanpose->lfwrench[4] - humanpose->lfwrench[0] * F_H_OFFSET + humanpose->lfwrench[2] * 0 ) / humanpose->lfwrench[2];
-			lfzmp[1] = ( humanpose->lfwrench[3] - humanpose->lfwrench[1] * F_H_OFFSET + humanpose->lfwrench[2] * 0 ) / humanpose->lfwrench[2];
+		if( humanpose.lfwrench[2] > 1.0e-6 ){
+			lfzmp[0] = ( -humanpose.lfwrench[4] - humanpose.lfwrench[0] * F_H_OFFSET + humanpose.lfwrench[2] * 0 ) / humanpose.lfwrench[2] + lfpos[0];
+			lfzmp[1] = ( humanpose.lfwrench[3] - humanpose.lfwrench[1] * F_H_OFFSET + humanpose.lfwrench[2] * 0 ) / humanpose.lfwrench[2] + lfpos[1];
 		}
-		humanpose->zmp[0] = ( rfzmp[0]*humanpose->rfwrench[2] + lfzmp[0]*humanpose->lfwrench[2] ) / ( humanpose->rfwrench[2] + humanpose->lfwrench[2] );
-		humanpose->zmp[1] = ( rfzmp[1]*humanpose->rfwrench[2] + lfzmp[1]*humanpose->lfwrench[2] ) / ( humanpose->rfwrench[2] + humanpose->lfwrench[2]);
-		humanpose->zmp[2] = 0;
+		//humanpose.zmpをrfw,lfwから計算して上書き
 
-//	      for(int i=0;i<6;i++)cout<<"rfw["<<i<<"]"<<humanpose->rfwrench[i];
-//	      for(int i=0;i<6;i++)cout<<"lfw["<<i<<"]"<<humanpose->lfwrench[i];
-//	      for(int i=0;i<2;i++)cout<<"zmp["<<i<<"]"<<humanpose->zmp[i];
+		if( humanpose.rfwrench[2] > 1.0e-6 || humanpose.lfwrench[2] > 1.0e-6 ){
+			humanpose.zmp[0] = ( rfzmp[0]*humanpose.rfwrench[2] + lfzmp[0]*humanpose.lfwrench[2] ) / ( humanpose.rfwrench[2] + humanpose.lfwrench[2]);
+			humanpose.zmp[1] = ( rfzmp[1]*humanpose.rfwrench[2] + lfzmp[1]*humanpose.lfwrench[2] ) / ( humanpose.rfwrench[2] + humanpose.lfwrench[2]);
+			humanpose.zmp[2] = 0;
+		}else{
+			humanpose.zmp[0] = 0;	humanpose.zmp[1] = 0;	humanpose.zmp[2] = 0;
+		}
 
       //debug表示
 //			printf("[L] %+06.1f,%+06.1f,%+06.1f,%+06.1f,%+06.1f,%+06.1f [R] %+06.1f,%+06.1f,%+06.1f,%+06.1f,%+06.1f,%+06.1f [ZMP] %+06.3f,%+06.3f\n",
-//					humanpose->lfwrench[0],humanpose->lfwrench[1],humanpose->lfwrench[2],humanpose->lfwrench[3],humanpose->lfwrench[4],humanpose->lfwrench[5],
-//					humanpose->rfwrench[0],humanpose->rfwrench[1],humanpose->rfwrench[2],humanpose->rfwrench[3],humanpose->rfwrench[4],humanpose->rfwrench[5],
-//					humanpose->zmp[0],humanpose->zmp[1]);
-      humanpose->startsync = true;//test
-	   if(humanpose->startsync){
+//					humanpose.lfwrench[0],humanpose.lfwrench[1],humanpose.lfwrench[2],humanpose.lfwrench[3],humanpose.lfwrench[4],humanpose.lfwrench[5],
+//					humanpose.rfwrench[0],humanpose.rfwrench[1],humanpose.rfwrench[2],humanpose.rfwrench[3],humanpose.rfwrench[4],humanpose.rfwrench[5],
+//					humanpose.zmp[0],humanpose.zmp[1]);
+	   if(HumanSyncOn){
 //ZMP上書き
-//	      m_zmp.data.x = humanpose->zmp[0]*0.5 - filt_hp_data.com[0];//
-		  m_zmp.data.y = filt_hp_data.zmp[1] - filt_hp_data.com[1];//
+//	      m_zmp.data.x = humanpose.zmp[0]*0.5 - filt_hp_data.com[0];//
+		  m_zmp.data.y = filt_hp_data.zmp[1] - filt_hp_data.com[1];//test
 // COM上書き
 //	  m_cog.data.x = 0.0558423 + filt_hp_data.com[0];
 	  m_cog.data.y = filt_hp_data.com[1];
@@ -1059,13 +1081,12 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri
 static bool moveleft = false;
 bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 {
-//  cout<<"****************** test ********************"<<endl;
 	static int rfuplevel,lfuplevel;
 
-	if(humanpose->startsync){
+	if(HumanSyncOn){
 		if(param.target_link->name == "RLEG_JOINT5"){
-			if(humanpose->rfwrench[2] < 20.0 && humanpose->lfwrench[2] > 20.0){
-//			if(humanpose->rfwrench[2] < 20.0){
+			if(humanpose.rfwrench[2] < 20.0 && humanpose.lfwrench[2] > 20.0){
+//			if(humanpose.rfwrench[2] < 20.0){
 				if(rfuplevel < 100){
 					rfuplevel++;
 					filt_hp_data.rfpos[2] = 0.025 - 0.025*cos(M_PI*rfuplevel/100);
@@ -1084,8 +1105,8 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 			param.target_p0(2) = 0.1 + filt_hp_data.rfpos[2];
 		}
 		if(param.target_link->name == "LLEG_JOINT5"){
-			if(humanpose->lfwrench[2] < 20.0 && humanpose->rfwrench[2] > 20.0){
-//			if(humanpose->lfwrench[2] < 20.0){
+			if(humanpose.lfwrench[2] < 20.0 && humanpose.rfwrench[2] > 20.0){
+//			if(humanpose.lfwrench[2] < 20.0){
 				if(lfuplevel < 100){
 					lfuplevel++;
 					filt_hp_data.lfpos[2] = 0.025 - 0.025*cos(M_PI*lfuplevel/100);
@@ -1164,30 +1185,42 @@ void AutoBalancer::solveLimbIK ()
   m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog;
   m_robot->rootLink()->R = target_root_R;
 //mytest
-  #define MAXVEL 0.002
-
-  for(int i=0;i<3;i++){
-//	  filt_hp_data.com[i] = 0.99*filt_hp_data.com[i] + 0.01*(humanpose->com[i]);
-//	  filt_hp_data.rfpos[i] = 0.99*filt_hp_data.rfpos[i] + 0.01*(humanpose->rfpos[i]);//足ロック
-//	  filt_hp_data.lfpos[i] = 0.99*filt_hp_data.lfpos[i] + 0.01*(humanpose->lfpos[i]);//足ロック
-//	  filt_hp_data.rhpos[i] = 0.99*filt_hp_data.rhpos[i] + 0.01*(humanpose->rhpos[i]);
-//	  filt_hp_data.lhpos[i] = 0.99*filt_hp_data.lhpos[i] + 0.01*(humanpose->lhpos[i]);
-//	  filt_hp_data.zmp[i] = 0.99*filt_hp_data.zmp[i] + 0.01*(humanpose->zmp[i]);
-
-	  filt_hp_data.zmp[i] = 0.99*filt_hp_data.zmp[i] + 0.01*(input_htzmp(i));
-	  filt_hp_data.com[i] = 0.99*filt_hp_data.com[i] + 0.01*(input_htcom(i));
-	  filt_hp_data.rfpos[i] = 0.99*filt_hp_data.rfpos[i] + 0.01*(input_htrf(i));
-	  filt_hp_data.lfpos[i] = 0.99*filt_hp_data.lfpos[i] + 0.01*(input_htlf(i));
-	  filt_hp_data.rhpos[i] = 0.99*filt_hp_data.rhpos[i] + 0.01*(input_htrh(i));
-	  filt_hp_data.lhpos[i] = 0.99*filt_hp_data.lhpos[i] + 0.01*(input_htlh(i));
-
+  #define MAXVEL 0.004
+
+  static humanpose_t init_humanpose;
+  static int HumanSyncCountdownNum = 5 * (1/m_dt);
+  if(startCountdownForHumanSync){
+	  if(HumanSyncCountdownNum == 0){
+		  HumanSyncOn = true;
+	  }else{
+		  HumanSyncCountdownNum--;
+	  }
+  }
 
+  if(HumanSyncOn){
+	  for(int i=0;i<3;i++){
+		  filt_hp_data.zmp[i] = 0.99*filt_hp_data.zmp[i] + 0.01*(humanpose.zmp[i]);
+		  filt_hp_data.com[i] = 0.99*filt_hp_data.com[i] + 0.01*(humanpose.com[i] - init_humanpose.com[i]);
+		  filt_hp_data.rfwrench[i] = 0.99*filt_hp_data.rfwrench[i] + 0.01*(humanpose.rfwrench[i]);
+		  filt_hp_data.lfwrench[i] = 0.99*filt_hp_data.lfwrench[i] + 0.01*(humanpose.lfwrench[i]);
+	//	  filt_hp_data.rfpos[i] = 0.99*filt_hp_data.rfpos[i] + 0.01*(humanpose.rfpos[i] - init_humanpose.rfpos[i]);//足ロック
+	//	  filt_hp_data.lfpos[i] = 0.99*filt_hp_data.lfpos[i] + 0.01*(humanpose.lfpos[i] - init_humanpose.lfpos[i]);//足ロック
+		  filt_hp_data.rhpos[i] = 0.99*filt_hp_data.rhpos[i] + 0.01*(humanpose.rhpos[i] - init_humanpose.rhpos[i]);
+		  filt_hp_data.lhpos[i] = 0.99*filt_hp_data.lhpos[i] + 0.01*(humanpose.lhpos[i] - init_humanpose.lhpos[i]);
+
+		  if(filt_hp_data.com[i] - hp_data_old.com[i] > MAXVEL)filt_hp_data.com[i] = hp_data_old.com[i] + MAXVEL;
+		  if(filt_hp_data.com[i] - hp_data_old.com[i] < -MAXVEL)filt_hp_data.com[i] = hp_data_old.com[i] - MAXVEL;
+		  hp_data_old.com[i] = filt_hp_data.com[i];
+	  }
 
-	  if(filt_hp_data.com[i] - hp_data_old.com[i] > MAXVEL)filt_hp_data.com[i] = hp_data_old.com[i] + MAXVEL;
-	  if(filt_hp_data.com[i] - hp_data_old.com[i] < -MAXVEL)filt_hp_data.com[i] = hp_data_old.com[i] - MAXVEL;
-	  hp_data_old.com[i] = filt_hp_data.com[i];
+	  printf("[COM] %+06.3f,%+06.3f,%+06.3f [ZMP] %+06.3f,%+06.3f\n", filt_hp_data.com[0],filt_hp_data.com[1],filt_hp_data.com[2],filt_hp_data.zmp[0],filt_hp_data.zmp[1],filt_hp_data.zmp[2]);
+  }else{
+	  for(int i=0;i<3;i++)init_humanpose.com[i] = humanpose.com[i];
+	  for(int i=0;i<3;i++)init_humanpose.rfpos[i] = humanpose.rfpos[i];
+	  for(int i=0;i<3;i++)init_humanpose.lfpos[i] = humanpose.lfpos[i];
+	  for(int i=0;i<3;i++)init_humanpose.rhpos[i] = humanpose.rhpos[i];
+	  for(int i=0;i<3;i++)init_humanpose.lhpos[i] = humanpose.lhpos[i];
   }
-//  cout<<"COM"<<m_robot->calcCM()[0]<<","<<m_robot->calcCM()[1]<<","<<m_robot->calcCM()[2]<<endl;
   filt_hp_data.com[0] = 0.0;//重心前方向ロック
 
   m_robot->rootLink()->p(0) = 0.0558423 + filt_hp_data.com[0];
@@ -1259,6 +1292,14 @@ static int movelcnt;
   }
 */
 
+
+bool AutoBalancer::startHumanSyncAfter5sec()
+{
+	std::cerr << "[" << m_profile.instance_name << "] start HumanSync after 5 sec" << std::endl;
+	startCountdownForHumanSync = true;
+	return true;
+}
+
 void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence& limbs)
 {
   std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index e8214e339b9..989a2f2f800 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -110,6 +110,7 @@ class AutoBalancer
   bool getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx);
   bool getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep);
   bool releaseEmergencyStop();
+  bool startHumanSyncAfter5sec();
 
  protected:
   // Configuration variable declaration
@@ -138,6 +139,10 @@ class AutoBalancer
   //for human tracker
   TimedPoint3D m_htzmp;
   InPort<TimedPoint3D> m_htzmpIn;
+  TimedDoubleSeq m_htrfw;
+  InPort<TimedDoubleSeq> m_htrfwIn;
+  TimedDoubleSeq m_htlfw;
+  InPort<TimedDoubleSeq> m_htlfwIn;
   TimedPoint3D m_htcom;
   InPort<TimedPoint3D> m_htcomIn;
   TimedPoint3D m_htrf;
@@ -264,9 +269,6 @@ class AutoBalancer
   enum {MODE_NO_FORCE, MODE_REF_FORCE} use_force;
   std::vector<hrp::Vector3> ref_forces;
 
-  //for human tracker
-  hrp::Vector3 input_htzmp, input_htcom, input_htrf, input_htlf, input_htrh, input_htlh;
-
   unsigned int m_debugLevel;
   bool is_legged_robot, is_stop_mode, has_ik_failed, is_hand_fix_mode, is_hand_fix_initial;
   int loop, ik_error_debug_print_freq;
diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.cpp b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
index e4faf4ab27e..e2b75b3c99f 100644
--- a/rtc/AutoBalancer/AutoBalancerService_impl.cpp
+++ b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
@@ -113,5 +113,10 @@ CORBA::Boolean AutoBalancerService_impl::releaseEmergencyStop()
 void AutoBalancerService_impl::autobalancer(AutoBalancer *i_autobalancer)
 {
   m_autobalancer = i_autobalancer;
-} 
+}
+
+CORBA::Boolean AutoBalancerService_impl::startHumanSyncAfter5sec()
+{
+    return m_autobalancer->startHumanSyncAfter5sec();
+};
 
diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.h b/rtc/AutoBalancer/AutoBalancerService_impl.h
index c6cfe9b04bf..cab43ea2da5 100644
--- a/rtc/AutoBalancer/AutoBalancerService_impl.h
+++ b/rtc/AutoBalancer/AutoBalancerService_impl.h
@@ -34,6 +34,7 @@ class AutoBalancerService_impl
   CORBA::Boolean getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep , CORBA::Long& o_current_fs_idx);
   CORBA::Boolean getGoPosFootstepsSequence(CORBA::Double x, CORBA::Double y, CORBA::Double th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep);
   CORBA::Boolean releaseEmergencyStop();
+  CORBA::Boolean startHumanSyncAfter5sec();
   //
   //
   void autobalancer(AutoBalancer *i_autobalancer);

From 125286d2b29019e68e9af4e1ca4c758db555107e Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 15 Jun 2016 17:34:38 +0900
Subject: [PATCH 003/217] modify foot ik jump for CHIDORI

---
 python/hrpsys_config.py                       |  7 ----
 rtc/AutoBalancer/AutoBalancer.cpp             | 41 ++++++++++++++-----
 rtc/AutoBalancer/AutoBalancer.h               |  1 +
 rtc/AutoBalancer/AutoBalancerService_impl.cpp |  5 +++
 rtc/AutoBalancer/AutoBalancerService_impl.h   |  1 +
 rtc/Stabilizer/Stabilizer.cpp                 |  2 +-
 6 files changed, 38 insertions(+), 19 deletions(-)

diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index f6c1e1db542..0f121447aad 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -2081,13 +2081,6 @@ def setFootStepsWithParam(self, footstep, stepparams, overwrite_fs_idx = 0):
         self.abc_svc.setFootStepsWithParam(footstep, stepparams, overwrite_fs_idx)
 
 
-    def startHumanSyncAfter5sec(self):
-        '''!@brief
-        startHumanSyncAfter5sec mode
-        '''
-        self.abc_svc.startHumanSyncAfter5sec()
-
-
     # ##
     # ## initialize
     # ##
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index d3adac1a290..deba4531776 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1079,10 +1079,11 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri
 }
 
 static bool moveleft = false;
+static bool ht_first_call = true;
+hrp::Vector3 rfinitpos,lfinitpos;
 bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 {
 	static int rfuplevel,lfuplevel;
-
 	if(HumanSyncOn){
 		if(param.target_link->name == "RLEG_JOINT5"){
 			if(humanpose.rfwrench[2] < 20.0 && humanpose.lfwrench[2] > 20.0){
@@ -1100,9 +1101,9 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 					//if(filt_hp_data.rfpos[2] == 0.0)moveleft = true;//歩行遊び
 				}
 			}
-			param.target_p0(0) = 0.0 + filt_hp_data.rfpos[0];
-			param.target_p0(1) = -0.1 + filt_hp_data.rfpos[1];
-			param.target_p0(2) = 0.1 + filt_hp_data.rfpos[2];
+			param.target_p0(0) = rfinitpos(0) + filt_hp_data.rfpos[0];
+			param.target_p0(1) = rfinitpos(1) + filt_hp_data.rfpos[1];
+			param.target_p0(2) = rfinitpos(2) + filt_hp_data.rfpos[2];
 		}
 		if(param.target_link->name == "LLEG_JOINT5"){
 			if(humanpose.lfwrench[2] < 20.0 && humanpose.rfwrench[2] > 20.0){
@@ -1120,9 +1121,9 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 					//if(filt_hp_data.lfpos[2] == 0.0)moveleft = true;//歩行遊び
 				}
 			}
-			param.target_p0(0) = 0.0 + filt_hp_data.lfpos[0];
-			param.target_p0(1) = 0.1 + filt_hp_data.lfpos[1];
-			param.target_p0(2) = 0.1 + filt_hp_data.lfpos[2];
+			param.target_p0(0) = lfinitpos(0) + filt_hp_data.lfpos[0];
+			param.target_p0(1) = lfinitpos(1) + filt_hp_data.lfpos[1];
+			param.target_p0(2) = lfinitpos(2) + filt_hp_data.lfpos[2];
 		}
 		if(param.target_link->name == "RARM_JOINT7"){
 			param.target_p0(0) = 0.151 + filt_hp_data.rhpos[0];
@@ -1190,8 +1191,10 @@ void AutoBalancer::solveLimbIK ()
   static humanpose_t init_humanpose;
   static int HumanSyncCountdownNum = 5 * (1/m_dt);
   if(startCountdownForHumanSync){
+	  printf("Start Count Down for HumanSync [%d]\r", HumanSyncCountdownNum);
 	  if(HumanSyncCountdownNum == 0){
 		  HumanSyncOn = true;
+		  startCountdownForHumanSync = false;
 	  }else{
 		  HumanSyncCountdownNum--;
 	  }
@@ -1213,19 +1216,28 @@ void AutoBalancer::solveLimbIK ()
 		  hp_data_old.com[i] = filt_hp_data.com[i];
 	  }
 
-	  printf("[COM] %+06.3f,%+06.3f,%+06.3f [ZMP] %+06.3f,%+06.3f\n", filt_hp_data.com[0],filt_hp_data.com[1],filt_hp_data.com[2],filt_hp_data.zmp[0],filt_hp_data.zmp[1],filt_hp_data.zmp[2]);
+	  if(loop%100==0)printf("[COM] %+06.3f,%+06.3f,%+06.3f [ZMP] %+06.3f,%+06.3f\n", filt_hp_data.com[0],filt_hp_data.com[1],filt_hp_data.com[2],filt_hp_data.zmp[0],filt_hp_data.zmp[1],filt_hp_data.zmp[2]);
+	  if(ht_first_call){ht_first_call=false;
+		  rfinitpos = ikp["rleg"].target_p0;
+		  lfinitpos = ikp["lleg"].target_p0;
+	  }
   }else{
 	  for(int i=0;i<3;i++)init_humanpose.com[i] = humanpose.com[i];
 	  for(int i=0;i<3;i++)init_humanpose.rfpos[i] = humanpose.rfpos[i];
 	  for(int i=0;i<3;i++)init_humanpose.lfpos[i] = humanpose.lfpos[i];
 	  for(int i=0;i<3;i++)init_humanpose.rhpos[i] = humanpose.rhpos[i];
 	  for(int i=0;i<3;i++)init_humanpose.lhpos[i] = humanpose.lhpos[i];
+	  if(loop%100==0)printf("[pre-COM] %+06.3f,%+06.3f,%+06.3f [pre-ZMP] %+06.3f,%+06.3f\n", humanpose.com[0],humanpose.com[1],humanpose.com[2],humanpose.zmp[0],humanpose.zmp[1],humanpose.zmp[2]);
+
   }
   filt_hp_data.com[0] = 0.0;//重心前方向ロック
 
-  m_robot->rootLink()->p(0) = 0.0558423 + filt_hp_data.com[0];
-  m_robot->rootLink()->p(1) = 0+ filt_hp_data.com[1];//safetest
-  m_robot->rootLink()->p(2) = 0.976372 + filt_hp_data.com[2];
+//  m_robot->rootLink()->p(0) = 0.0558423 + filt_hp_data.com[0];
+//  m_robot->rootLink()->p(1) = 0+ filt_hp_data.com[1];//safetest
+//  m_robot->rootLink()->p(2) = 0.976372 + filt_hp_data.com[2];
+  m_robot->rootLink()->p(0) += filt_hp_data.com[0];
+  m_robot->rootLink()->p(1) += filt_hp_data.com[1];//safetest
+  m_robot->rootLink()->p(2) += filt_hp_data.com[2];
 
 static int movelcnt;
 	if(moveleft){
@@ -1300,6 +1312,13 @@ bool AutoBalancer::startHumanSyncAfter5sec()
 	return true;
 }
 
+bool AutoBalancer::stopHumanSync()
+{
+	std::cerr << "[" << m_profile.instance_name << "] stop HumanSync Now" << std::endl;
+	HumanSyncOn = false;
+	return true;
+}
+
 void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence& limbs)
 {
   std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 989a2f2f800..ff748dfff0e 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -111,6 +111,7 @@ class AutoBalancer
   bool getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep);
   bool releaseEmergencyStop();
   bool startHumanSyncAfter5sec();
+  bool stopHumanSync();
 
  protected:
   // Configuration variable declaration
diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.cpp b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
index e2b75b3c99f..0c899349cd8 100644
--- a/rtc/AutoBalancer/AutoBalancerService_impl.cpp
+++ b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
@@ -120,3 +120,8 @@ CORBA::Boolean AutoBalancerService_impl::startHumanSyncAfter5sec()
     return m_autobalancer->startHumanSyncAfter5sec();
 };
 
+CORBA::Boolean AutoBalancerService_impl::stopHumanSync()
+{
+    return m_autobalancer->stopHumanSync();
+};
+
diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.h b/rtc/AutoBalancer/AutoBalancerService_impl.h
index cab43ea2da5..01c0f4977b1 100644
--- a/rtc/AutoBalancer/AutoBalancerService_impl.h
+++ b/rtc/AutoBalancer/AutoBalancerService_impl.h
@@ -35,6 +35,7 @@ class AutoBalancerService_impl
   CORBA::Boolean getGoPosFootstepsSequence(CORBA::Double x, CORBA::Double y, CORBA::Double th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep);
   CORBA::Boolean releaseEmergencyStop();
   CORBA::Boolean startHumanSyncAfter5sec();
+  CORBA::Boolean stopHumanSync();
   //
   //
   void autobalancer(AutoBalancer *i_autobalancer);
diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp
index b6acbf9fbd9..b5175201764 100644
--- a/rtc/Stabilizer/Stabilizer.cpp
+++ b/rtc/Stabilizer/Stabilizer.cpp
@@ -1320,12 +1320,12 @@ void Stabilizer::calcTPCC() {
                                   + k_tpcc_x[i] * transition_smooth_gain * dcog(i);
         newcog(i) = uu * dt + cog(i);
       }
-
       moveBasePosRotForBodyRPYControl ();
 
       // target at ee => target at link-origin
       hrp::Vector3 target_link_p[stikp.size()];
       hrp::Matrix33 target_link_R[stikp.size()];
+        
       for (size_t i = 0; i < stikp.size(); i++) {
         rats::rotm3times(target_link_R[i], target_ee_R[i], stikp[i].localR.transpose());
         target_link_p[i] = target_ee_p[i] - target_ee_R[i] * stikp[i].localCOPPos;

From 427febb8da1469fbd8c8fc8f52d4ee0ff316eb7c Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 15 Jun 2016 18:48:53 +0900
Subject: [PATCH 004/217] add service stopHumanSync

---
 idl/AutoBalancerService.idl       |  1 +
 rtc/AutoBalancer/AutoBalancer.cpp | 38 +++++++++++++++----------------
 2 files changed, 20 insertions(+), 19 deletions(-)

diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl
index a278d03bc69..071bc5d3f62 100644
--- a/idl/AutoBalancerService.idl
+++ b/idl/AutoBalancerService.idl
@@ -420,5 +420,6 @@ module OpenHRP
      * @return true if set successfully, false otherwise
      */
     boolean startHumanSyncAfter5sec();
+    boolean stopHumanSync();
   };
 };
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index deba4531776..8515563c653 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1081,19 +1081,18 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri
 static bool moveleft = false;
 static bool ht_first_call = true;
 hrp::Vector3 rfinitpos,lfinitpos;
+static int rfuplevel,lfuplevel;
 bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 {
-	static int rfuplevel,lfuplevel;
-	if(HumanSyncOn){
 		if(param.target_link->name == "RLEG_JOINT5"){
-			if(humanpose.rfwrench[2] < 20.0 && humanpose.lfwrench[2] > 20.0){
+			if(humanpose.rfwrench[2] < 20.0 && humanpose.lfwrench[2] > 20.0 && HumanSyncOn){
 //			if(humanpose.rfwrench[2] < 20.0){
 				if(rfuplevel < 100){
 					rfuplevel++;
 					filt_hp_data.rfpos[2] = 0.025 - 0.025*cos(M_PI*rfuplevel/100);
 					//filt_hp_data.rfpos[1] -= 0.001;//歩行遊び
 				}
-				cout<<"RUP"<<endl;
+				if(loop%100==0)cout<<"RUP"<<endl;
 			}else{
 				if(rfuplevel > 0){
 					rfuplevel--;
@@ -1106,14 +1105,14 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 			param.target_p0(2) = rfinitpos(2) + filt_hp_data.rfpos[2];
 		}
 		if(param.target_link->name == "LLEG_JOINT5"){
-			if(humanpose.lfwrench[2] < 20.0 && humanpose.rfwrench[2] > 20.0){
+			if(humanpose.lfwrench[2] < 20.0 && humanpose.rfwrench[2] > 20.0 && HumanSyncOn){
 //			if(humanpose.lfwrench[2] < 20.0){
 				if(lfuplevel < 100){
 					lfuplevel++;
 					filt_hp_data.lfpos[2] = 0.025 - 0.025*cos(M_PI*lfuplevel/100);
 					//filt_hp_data.lfpos[1] -= 0.001;//歩行遊び
 				}
-				cout<<"LUP"<<endl;
+				if(loop%100==0)cout<<"LUP"<<endl;
 			}else{
 				if(lfuplevel > 0){
 					lfuplevel--;
@@ -1125,17 +1124,16 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 			param.target_p0(1) = lfinitpos(1) + filt_hp_data.lfpos[1];
 			param.target_p0(2) = lfinitpos(2) + filt_hp_data.lfpos[2];
 		}
-		if(param.target_link->name == "RARM_JOINT7"){
+		if(param.target_link->name == "RARM_JOINT7" && HumanSyncOn){
 			param.target_p0(0) = 0.151 + filt_hp_data.rhpos[0];
 			param.target_p0(1) = -0.374 + filt_hp_data.rhpos[1];
 			param.target_p0(2) = 0.973 + filt_hp_data.rhpos[2];
 		}
-		if(param.target_link->name == "LARM_JOINT7"){
+		if(param.target_link->name == "LARM_JOINT7" && HumanSyncOn){
 			param.target_p0(0) = 0.151 + filt_hp_data.lhpos[0];
 			param.target_p0(1) = 0.374 + filt_hp_data.lhpos[1];
 			param.target_p0(2) = 0.973 + filt_hp_data.lhpos[2];
 		}
-	}
 
 
   param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
@@ -1195,10 +1193,16 @@ void AutoBalancer::solveLimbIK ()
 	  if(HumanSyncCountdownNum == 0){
 		  HumanSyncOn = true;
 		  startCountdownForHumanSync = false;
+		  HumanSyncCountdownNum = 5 * (1/m_dt);
 	  }else{
 		  HumanSyncCountdownNum--;
 	  }
   }
+  if(ht_first_call){
+	  rfinitpos = ikp["rleg"].target_p0;
+	  lfinitpos = ikp["lleg"].target_p0;
+	  ht_first_call=false;
+  }
 
   if(HumanSyncOn){
 	  for(int i=0;i<3;i++){
@@ -1213,31 +1217,27 @@ void AutoBalancer::solveLimbIK ()
 
 		  if(filt_hp_data.com[i] - hp_data_old.com[i] > MAXVEL)filt_hp_data.com[i] = hp_data_old.com[i] + MAXVEL;
 		  if(filt_hp_data.com[i] - hp_data_old.com[i] < -MAXVEL)filt_hp_data.com[i] = hp_data_old.com[i] - MAXVEL;
-		  hp_data_old.com[i] = filt_hp_data.com[i];
 	  }
 
 	  if(loop%100==0)printf("[COM] %+06.3f,%+06.3f,%+06.3f [ZMP] %+06.3f,%+06.3f\n", filt_hp_data.com[0],filt_hp_data.com[1],filt_hp_data.com[2],filt_hp_data.zmp[0],filt_hp_data.zmp[1],filt_hp_data.zmp[2]);
-	  if(ht_first_call){ht_first_call=false;
-		  rfinitpos = ikp["rleg"].target_p0;
-		  lfinitpos = ikp["lleg"].target_p0;
-	  }
   }else{
 	  for(int i=0;i<3;i++)init_humanpose.com[i] = humanpose.com[i];
 	  for(int i=0;i<3;i++)init_humanpose.rfpos[i] = humanpose.rfpos[i];
 	  for(int i=0;i<3;i++)init_humanpose.lfpos[i] = humanpose.lfpos[i];
 	  for(int i=0;i<3;i++)init_humanpose.rhpos[i] = humanpose.rhpos[i];
 	  for(int i=0;i<3;i++)init_humanpose.lhpos[i] = humanpose.lhpos[i];
+	  for(int i=0;i<3;i++){
+		  if(filt_hp_data.com[i]>0)filt_hp_data.com[i] -= 0.0001;
+		  if(filt_hp_data.com[i]<0)filt_hp_data.com[i] += 0.0001;
+	  }
 	  if(loop%100==0)printf("[pre-COM] %+06.3f,%+06.3f,%+06.3f [pre-ZMP] %+06.3f,%+06.3f\n", humanpose.com[0],humanpose.com[1],humanpose.com[2],humanpose.zmp[0],humanpose.zmp[1],humanpose.zmp[2]);
 
   }
   filt_hp_data.com[0] = 0.0;//重心前方向ロック
-
-//  m_robot->rootLink()->p(0) = 0.0558423 + filt_hp_data.com[0];
-//  m_robot->rootLink()->p(1) = 0+ filt_hp_data.com[1];//safetest
-//  m_robot->rootLink()->p(2) = 0.976372 + filt_hp_data.com[2];
   m_robot->rootLink()->p(0) += filt_hp_data.com[0];
-  m_robot->rootLink()->p(1) += filt_hp_data.com[1];//safetest
+  m_robot->rootLink()->p(1) += filt_hp_data.com[1];
   m_robot->rootLink()->p(2) += filt_hp_data.com[2];
+  for(int i=0;i<3;i++)hp_data_old.com[i] = filt_hp_data.com[i];
 
 static int movelcnt;
 	if(moveleft){

From 6674eace1372d06f4f4355b5f7e2fc655109a668 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 29 Jun 2016 21:06:48 +0900
Subject: [PATCH 005/217] add X move

---
 rtc/AutoBalancer/AutoBalancer.cpp | 38 +++++++++++++++++++------------
 1 file changed, 23 insertions(+), 15 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 8515563c653..a986518ca64 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -39,6 +39,7 @@ humanpose_t humanpose;
 humanpose_t filt_hp_data,hp_data_old;
 bool startCountdownForHumanSync = false;
 bool HumanSyncOn = false;
+#define true
 
 
 typedef coil::Guard<coil::Mutex> Guard;
@@ -519,7 +520,7 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     //for human tracker
     if (m_htzmpIn.isNew()){
     	m_htzmpIn.read();
-//    	humanpose.zmp[0] = m_htzmp.data.x;//zmpはこっちで計算
+//    	humanpose.zmp[0] = m_htzmp.data.x;//zmpは外部から読まずに両足wrenchからこのソースの中で計算
 //    	humanpose.zmp[1] = m_htzmp.data.y;
 //    	humanpose.zmp[2] = m_htzmp.data.z;
     }
@@ -686,11 +687,11 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
 //					humanpose.zmp[0],humanpose.zmp[1]);
 	   if(HumanSyncOn){
 //ZMP上書き
-//	      m_zmp.data.x = humanpose.zmp[0]*0.5 - filt_hp_data.com[0];//
-		  m_zmp.data.y = filt_hp_data.zmp[1] - filt_hp_data.com[1];//test
+		if(USEX)m_zmp.data.x = filt_hp_data.zmp[0] - filt_hp_data.com[0];//
+		m_zmp.data.y = filt_hp_data.zmp[1] - filt_hp_data.com[1];//
 // COM上書き
-//	  m_cog.data.x = 0.0558423 + filt_hp_data.com[0];
-	  m_cog.data.y = filt_hp_data.com[1];
+		if(USEX)m_cog.data.x = filt_hp_data.com[0];
+		m_cog.data.y = filt_hp_data.com[1];
 	  }
 
       // sbpCogOffset
@@ -1044,6 +1045,12 @@ void AutoBalancer::getTargetParameters()
       ref_zmp(1) = ref_cog(1);
       ref_zmp(2) = tmp_foot_mid_pos(2);
     }
+    //ishiguro
+    if (HumanSyncOn) {
+		if(USEX)ref_zmp(0) = humanpose.zmp[0];
+		ref_zmp(1) = humanpose.zmp[1];
+		ref_zmp(2) = tmp_foot_mid_pos(2);
+    }
   }
 };
 
@@ -1082,21 +1089,23 @@ static bool moveleft = false;
 static bool ht_first_call = true;
 hrp::Vector3 rfinitpos,lfinitpos;
 static int rfuplevel,lfuplevel;
+#define LEVELNUM 200
+#define FUPHIGHT 0.03
 bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 {
 		if(param.target_link->name == "RLEG_JOINT5"){
 			if(humanpose.rfwrench[2] < 20.0 && humanpose.lfwrench[2] > 20.0 && HumanSyncOn){
 //			if(humanpose.rfwrench[2] < 20.0){
-				if(rfuplevel < 100){
+				if(rfuplevel < LEVELNUM){
 					rfuplevel++;
-					filt_hp_data.rfpos[2] = 0.025 - 0.025*cos(M_PI*rfuplevel/100);
+					filt_hp_data.rfpos[2] = FUPHIGHT/2*(1-cos(M_PI*rfuplevel/LEVELNUM));
 					//filt_hp_data.rfpos[1] -= 0.001;//歩行遊び
 				}
 				if(loop%100==0)cout<<"RUP"<<endl;
 			}else{
 				if(rfuplevel > 0){
 					rfuplevel--;
-					filt_hp_data.rfpos[2] = 0.025 - 0.025*cos(M_PI*rfuplevel/100);
+					filt_hp_data.rfpos[2] = FUPHIGHT/2*(1-cos(M_PI*rfuplevel/LEVELNUM));
 					//if(filt_hp_data.rfpos[2] == 0.0)moveleft = true;//歩行遊び
 				}
 			}
@@ -1107,16 +1116,16 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 		if(param.target_link->name == "LLEG_JOINT5"){
 			if(humanpose.lfwrench[2] < 20.0 && humanpose.rfwrench[2] > 20.0 && HumanSyncOn){
 //			if(humanpose.lfwrench[2] < 20.0){
-				if(lfuplevel < 100){
+				if(lfuplevel < LEVELNUM){
 					lfuplevel++;
-					filt_hp_data.lfpos[2] = 0.025 - 0.025*cos(M_PI*lfuplevel/100);
+					filt_hp_data.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
 					//filt_hp_data.lfpos[1] -= 0.001;//歩行遊び
 				}
 				if(loop%100==0)cout<<"LUP"<<endl;
 			}else{
 				if(lfuplevel > 0){
 					lfuplevel--;
-					filt_hp_data.lfpos[2] = 0.025 - 0.025*cos(M_PI*lfuplevel/100);
+					filt_hp_data.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
 					//if(filt_hp_data.lfpos[2] == 0.0)moveleft = true;//歩行遊び
 				}
 			}
@@ -1207,7 +1216,7 @@ void AutoBalancer::solveLimbIK ()
   if(HumanSyncOn){
 	  for(int i=0;i<3;i++){
 		  filt_hp_data.zmp[i] = 0.99*filt_hp_data.zmp[i] + 0.01*(humanpose.zmp[i]);
-		  filt_hp_data.com[i] = 0.99*filt_hp_data.com[i] + 0.01*(humanpose.com[i] - init_humanpose.com[i]);
+		  filt_hp_data.com[i] = 0.99*filt_hp_data.com[i] + 0.01*(humanpose.com[i] - init_humanpose.com[i] + init_humanpose.zmp[i]);//Sync開始時のzmpのオフセットはつまり重心のオフセット
 		  filt_hp_data.rfwrench[i] = 0.99*filt_hp_data.rfwrench[i] + 0.01*(humanpose.rfwrench[i]);
 		  filt_hp_data.lfwrench[i] = 0.99*filt_hp_data.lfwrench[i] + 0.01*(humanpose.lfwrench[i]);
 	//	  filt_hp_data.rfpos[i] = 0.99*filt_hp_data.rfpos[i] + 0.01*(humanpose.rfpos[i] - init_humanpose.rfpos[i]);//足ロック
@@ -1222,6 +1231,7 @@ void AutoBalancer::solveLimbIK ()
 	  if(loop%100==0)printf("[COM] %+06.3f,%+06.3f,%+06.3f [ZMP] %+06.3f,%+06.3f\n", filt_hp_data.com[0],filt_hp_data.com[1],filt_hp_data.com[2],filt_hp_data.zmp[0],filt_hp_data.zmp[1],filt_hp_data.zmp[2]);
   }else{
 	  for(int i=0;i<3;i++)init_humanpose.com[i] = humanpose.com[i];
+	  for(int i=0;i<3;i++)init_humanpose.zmp[i] = humanpose.zmp[i];
 	  for(int i=0;i<3;i++)init_humanpose.rfpos[i] = humanpose.rfpos[i];
 	  for(int i=0;i<3;i++)init_humanpose.lfpos[i] = humanpose.lfpos[i];
 	  for(int i=0;i<3;i++)init_humanpose.rhpos[i] = humanpose.rhpos[i];
@@ -1233,8 +1243,7 @@ void AutoBalancer::solveLimbIK ()
 	  if(loop%100==0)printf("[pre-COM] %+06.3f,%+06.3f,%+06.3f [pre-ZMP] %+06.3f,%+06.3f\n", humanpose.com[0],humanpose.com[1],humanpose.com[2],humanpose.zmp[0],humanpose.zmp[1],humanpose.zmp[2]);
 
   }
-  filt_hp_data.com[0] = 0.0;//重心前方向ロック
-  m_robot->rootLink()->p(0) += filt_hp_data.com[0];
+  if(USEX)m_robot->rootLink()->p(0) += filt_hp_data.com[0];
   m_robot->rootLink()->p(1) += filt_hp_data.com[1];
   m_robot->rootLink()->p(2) += filt_hp_data.com[2];
   for(int i=0;i<3;i++)hp_data_old.com[i] = filt_hp_data.com[i];
@@ -1304,7 +1313,6 @@ static int movelcnt;
   }
 */
 
-
 bool AutoBalancer::startHumanSyncAfter5sec()
 {
 	std::cerr << "[" << m_profile.instance_name << "] start HumanSync after 5 sec" << std::endl;

From 25a860a18a65e5d1cef22d5c7655c7a9052baa31 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 1 Jul 2016 21:39:12 +0900
Subject: [PATCH 006/217] mod zmp ref

---
 3rdparty/qpOASES/qpOASES-3.0/AUTHORS          |   74 +
 3rdparty/qpOASES/qpOASES-3.0/AUTHORS.txt      |   74 +
 3rdparty/qpOASES/qpOASES-3.0/CMakeLists.txt   |  150 +
 .../qpOASES/qpOASES-3.0/CTestTestfile.cmake   |    6 +
 3rdparty/qpOASES/qpOASES-3.0/INSTALL          |   79 +
 3rdparty/qpOASES/qpOASES-3.0/INSTALL.txt      |   79 +
 3rdparty/qpOASES/qpOASES-3.0/LICENSE          |  503 ++
 3rdparty/qpOASES/qpOASES-3.0/LICENSE.txt      |  503 ++
 3rdparty/qpOASES/qpOASES-3.0/README           |   84 +
 3rdparty/qpOASES/qpOASES-3.0/README.txt       |   84 +
 3rdparty/qpOASES/qpOASES-3.0/VERSIONS         |  106 +
 3rdparty/qpOASES/qpOASES-3.0/VERSIONS.txt     |  106 +
 3rdparty/qpOASES/qpOASES-3.0/bin/example1     |  Bin 0 -> 32801 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/example1a    |  Bin 0 -> 28799 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/example1b    |  Bin 0 -> 31765 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/example2     |  Bin 0 -> 31208 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/example3     |  Bin 0 -> 30847 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/example3b    |  Bin 0 -> 30848 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/example4     |  Bin 0 -> 34008 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/example5     |  Bin 0 -> 39753 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/exampleLP    |  Bin 0 -> 31905 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/index.html   |    7 +
 3rdparty/qpOASES/qpOASES-3.0/bin/qrecipe      |  Bin 0 -> 57832 bytes
 .../qpOASES/qpOASES-3.0/doc/DoxygenLayout.xml |  189 +
 .../qpOASES/qpOASES-3.0/doc/doxygen.config    | 1123 +++
 3rdparty/qpOASES/qpOASES-3.0/doc/index.html   |   12 +
 3rdparty/qpOASES/qpOASES-3.0/doc/mainpage.dox |  120 +
 3rdparty/qpOASES/qpOASES-3.0/doc/manual.pdf   |  Bin 0 -> 710503 bytes
 .../qpOASES/qpOASES-3.0/examples/example1.cpp |   97 +
 .../qpOASES-3.0/examples/example1a.cpp        |   85 +
 .../qpOASES-3.0/examples/example1b.cpp        |   86 +
 .../qpOASES/qpOASES-3.0/examples/example2.cpp |  124 +
 .../qpOASES/qpOASES-3.0/examples/example3.cpp |   88 +
 .../qpOASES-3.0/examples/example3b.cpp        |   88 +
 .../qpOASES/qpOASES-3.0/examples/example4.cpp |  172 +
 .../qpOASES-3.0/examples/example4CP.cpp       |  111 +
 .../qpOASES/qpOASES-3.0/examples/example5.cpp |  200 +
 .../qpOASES-3.0/examples/exampleLP.cpp        |   87 +
 .../qpOASES/qpOASES-3.0/examples/index.html   |   19 +
 .../qpOASES/qpOASES-3.0/examples/qrecipe.cpp  |  495 ++
 .../qpOASES/qpOASES-3.0/include/index.html    |    9 +
 .../qpOASES/qpOASES-3.0/include/qpOASES.hpp   |   59 +
 .../qpOASES-3.0/include/qpOASES/Bounds.hpp    |  256 +
 .../qpOASES-3.0/include/qpOASES/Bounds.ipp    |  120 +
 .../qpOASES-3.0/include/qpOASES/Constants.hpp |   77 +
 .../include/qpOASES/ConstraintProduct.hpp     |   91 +
 .../include/qpOASES/Constraints.hpp           |  246 +
 .../include/qpOASES/Constraints.ipp           |  122 +
 .../qpOASES-3.0/include/qpOASES/Flipper.hpp   |  155 +
 .../qpOASES-3.0/include/qpOASES/Indexlist.hpp |  199 +
 .../qpOASES-3.0/include/qpOASES/Indexlist.ipp |   92 +
 .../qpOASES-3.0/include/qpOASES/Matrices.hpp  |  867 +++
 .../include/qpOASES/MessageHandling.hpp       |  472 ++
 .../include/qpOASES/MessageHandling.ipp       |  144 +
 .../qpOASES-3.0/include/qpOASES/Options.hpp   |  170 +
 .../qpOASES-3.0/include/qpOASES/QProblem.hpp  | 1163 +++
 .../qpOASES-3.0/include/qpOASES/QProblem.ipp  |  283 +
 .../qpOASES-3.0/include/qpOASES/QProblemB.hpp | 1091 +++
 .../qpOASES-3.0/include/qpOASES/QProblemB.ipp |  477 ++
 .../qpOASES-3.0/include/qpOASES/SQProblem.hpp |  394 +
 .../qpOASES-3.0/include/qpOASES/SQProblem.ipp |   51 +
 .../qpOASES-3.0/include/qpOASES/SubjectTo.hpp |  229 +
 .../qpOASES-3.0/include/qpOASES/SubjectTo.ipp |  158 +
 .../qpOASES-3.0/include/qpOASES/Types.hpp     |  281 +
 .../include/qpOASES/UnitTesting.hpp           |   79 +
 .../qpOASES-3.0/include/qpOASES/Utils.hpp     |  370 +
 .../qpOASES-3.0/include/qpOASES/Utils.ipp     |  174 +
 .../include/qpOASES/extras/OQPinterface.hpp   |  250 +
 .../qpOASES/extras/SolutionAnalysis.hpp       |  155 +
 .../qpOASES/extras/SolutionAnalysis.ipp       |   51 +
 .../include/qpOASES/extras/index.html         |   10 +
 .../qpOASES-3.0/include/qpOASES/index.html    |   33 +
 3rdparty/qpOASES/qpOASES-3.0/index.html       |   31 +
 .../qpOASES/qpOASES-3.0/interfaces/index.html |   12 +
 .../interfaces/matlab/example1.mat            |  Bin 0 -> 790 bytes
 .../interfaces/matlab/example1a.mat           |  Bin 0 -> 896 bytes
 .../interfaces/matlab/example1b.mat           |  Bin 0 -> 549 bytes
 .../qpOASES-3.0/interfaces/matlab/index.html  |   20 +
 .../qpOASES-3.0/interfaces/matlab/make.m      |  238 +
 .../qpOASES-3.0/interfaces/matlab/qpOASES.cpp |  557 ++
 .../qpOASES-3.0/interfaces/matlab/qpOASES.m   |   77 +
 .../interfaces/matlab/qpOASES_auxInput.m      |  103 +
 .../matlab/qpOASES_matlab_utils.cpp           |  885 +++
 .../matlab/qpOASES_matlab_utils.hpp           |   92 +
 .../interfaces/matlab/qpOASES_options.m       |  251 +
 .../interfaces/matlab/qpOASES_sequence.cpp    | 1061 +++
 .../interfaces/matlab/qpOASES_sequence.m      |  112 +
 .../qpOASES-3.0/interfaces/octave/clean       |    3 +
 .../qpOASES-3.0/interfaces/octave/clean.sh    |    3 +
 .../interfaces/octave/example1.mat            |  Bin 0 -> 790 bytes
 .../interfaces/octave/example1a.mat           |  Bin 0 -> 896 bytes
 .../interfaces/octave/example1b.mat           |  Bin 0 -> 549 bytes
 .../qpOASES-3.0/interfaces/octave/index.html  |   21 +
 .../qpOASES-3.0/interfaces/octave/make.m      |  238 +
 .../qpOASES-3.0/interfaces/octave/qpOASES.cpp |  557 ++
 .../qpOASES-3.0/interfaces/octave/qpOASES.m   |   77 +
 .../interfaces/octave/qpOASES_auxInput.m      |  103 +
 .../octave/qpOASES_octave_utils.cpp           |  884 +++
 .../octave/qpOASES_octave_utils.hpp           |   92 +
 .../interfaces/octave/qpOASES_options.m       |  251 +
 .../interfaces/octave/qpOASES_sequence.cpp    | 1061 +++
 .../interfaces/octave/qpOASES_sequence.m      |  112 +
 .../qpOASES-3.0/interfaces/python/README.rst  |   68 +
 .../python/examples/cython/example1.pyx       |   73 +
 .../python/examples/cython/index.html         |    9 +
 .../python/examples/cython/setup.py           |   16 +
 .../interfaces/python/examples/example1.py    |   76 +
 .../interfaces/python/examples/example1b.py   |   72 +
 .../interfaces/python/examples/example2.py    |   90 +
 .../interfaces/python/examples/index.html     |   11 +
 .../qpOASES-3.0/interfaces/python/index.html  |   13 +
 .../qpOASES-3.0/interfaces/python/qpoases.pxd |  476 ++
 .../qpOASES-3.0/interfaces/python/qpoases.pyx |  779 ++
 .../qpOASES-3.0/interfaces/python/setup.py    |   72 +
 .../interfaces/python/tests/__init__.py       |    1 +
 .../interfaces/python/tests/index.html        |   11 +
 .../interfaces/python/tests/test_examples.py  |  328 +
 .../interfaces/python/tests/test_idhessian.py |   94 +
 .../interfaces/python/tests/test_testbench.py |  332 +
 .../interfaces/scilab/example1.dat            |  Bin 0 -> 656 bytes
 .../interfaces/scilab/example1a.dat           |  Bin 0 -> 784 bytes
 .../interfaces/scilab/example1b.dat           |  Bin 0 -> 408 bytes
 .../qpOASES-3.0/interfaces/scilab/index.html  |   14 +
 .../interfaces/scilab/qpOASESinterface.c      | 1061 +++
 .../interfaces/scilab/qpOASESinterface.sce    |   41 +
 .../interfaces/scilab/qpOASESroutines.cpp     |  365 +
 .../interfaces/simulink/example_QProblem.mdl  |  803 +++
 .../interfaces/simulink/example_QProblemB.mdl |  745 ++
 .../interfaces/simulink/example_SQProblem.mdl |  797 +++
 .../interfaces/simulink/index.html            |   17 +
 .../simulink/load_example_QProblem.m          |   93 +
 .../simulink/load_example_QProblemB.m         |   75 +
 .../simulink/load_example_SQProblem.m         |   93 +
 .../qpOASES-3.0/interfaces/simulink/make.m    |  239 +
 .../interfaces/simulink/qpOASES_QProblem.cpp  |  447 ++
 .../interfaces/simulink/qpOASES_QProblemB.cpp |  392 +
 .../interfaces/simulink/qpOASES_SQProblem.cpp |  460 ++
 .../qpOASES/qpOASES-3.0/libs/libqpOASES.so    |    1 +
 .../qpOASES-3.0/libs/libqpOASES.so.3.0        |  Bin 0 -> 1269403 bytes
 3rdparty/qpOASES/qpOASES-3.0/make.mk          |   38 +
 3rdparty/qpOASES/qpOASES-3.0/make_cygwin.mk   |  106 +
 3rdparty/qpOASES/qpOASES-3.0/make_linux.mk    |  107 +
 3rdparty/qpOASES/qpOASES-3.0/make_osx.mk      |   99 +
 3rdparty/qpOASES/qpOASES-3.0/make_windows.mk  |  114 +
 .../qpOASES-3.0/src/BLASReplacement.cpp       |  145 +
 3rdparty/qpOASES/qpOASES-3.0/src/Bounds.cpp   |  515 ++
 .../qpOASES/qpOASES-3.0/src/Constraints.cpp   |  500 ++
 3rdparty/qpOASES/qpOASES-3.0/src/Flipper.cpp  |  273 +
 .../qpOASES/qpOASES-3.0/src/Indexlist.cpp     |  319 +
 .../qpOASES-3.0/src/LAPACKReplacement.cpp     |  120 +
 3rdparty/qpOASES/qpOASES-3.0/src/Matrices.cpp | 1967 +++++
 .../qpOASES-3.0/src/MessageHandling.cpp       |  612 ++
 .../qpOASES/qpOASES-3.0/src/OQPinterface.cpp  |  670 ++
 3rdparty/qpOASES/qpOASES-3.0/src/Options.cpp  |  553 ++
 3rdparty/qpOASES/qpOASES-3.0/src/QProblem.cpp | 6338 +++++++++++++++++
 .../qpOASES/qpOASES-3.0/src/QProblemB.cpp     | 3862 ++++++++++
 .../qpOASES/qpOASES-3.0/src/SQProblem.cpp     |  553 ++
 .../qpOASES-3.0/src/SolutionAnalysis.cpp      |  567 ++
 .../qpOASES/qpOASES-3.0/src/SubjectTo.cpp     |  289 +
 3rdparty/qpOASES/qpOASES-3.0/src/Utils.cpp    |  984 +++
 3rdparty/qpOASES/qpOASES-3.0/src/index.html   |   24 +
 .../qpOASES-3.0/testing/checkForMemoryLeaks   |  106 +
 .../testing/cpp/data/fetch_cpp_data           |   34 +
 .../qpOASES-3.0/testing/cpp/data/index.html   |    8 +
 .../qpOASES-3.0/testing/cpp/index.html        |   33 +
 .../qpOASES-3.0/testing/cpp/test_bench.cpp    |  309 +
 .../testing/cpp/test_constraintProduct1.cpp   |  197 +
 .../testing/cpp/test_constraintProduct2.cpp   |  196 +
 .../qpOASES-3.0/testing/cpp/test_example1.cpp |  123 +
 .../testing/cpp/test_example1a.cpp            |  118 +
 .../testing/cpp/test_example1b.cpp            |  119 +
 .../qpOASES-3.0/testing/cpp/test_example2.cpp |  132 +
 .../qpOASES-3.0/testing/cpp/test_example4.cpp |  211 +
 .../qpOASES-3.0/testing/cpp/test_example5.cpp |  204 +
 .../qpOASES-3.0/testing/cpp/test_example6.cpp |  112 +
 .../qpOASES-3.0/testing/cpp/test_example7.cpp |   90 +
 .../testing/cpp/test_exampleLP.cpp            |  121 +
 .../testing/cpp/test_gradientShift.cpp        |  125 +
 .../testing/cpp/test_guessedWS1.cpp           |  164 +
 .../testing/cpp/test_indexlist.cpp            |   96 +
 .../qpOASES-3.0/testing/cpp/test_janick1.cpp  |  197 +
 .../qpOASES-3.0/testing/cpp/test_janick2.cpp  |  251 +
 .../qpOASES-3.0/testing/cpp/test_matrices.cpp |  903 +++
 .../testing/cpp/test_matrices2.cpp            |  113 +
 .../testing/cpp/test_matrices3.cpp            |   90 +
 .../qpOASES-3.0/testing/cpp/test_qrecipe.cpp  |  149 +
 .../testing/cpp/test_qrecipe_data.hpp         |  401 ++
 .../testing/cpp/test_runAllOqpExamples.cpp    |  177 +
 .../testing/cpp/test_sebastien1.cpp           |   74 +
 .../testing/cpp/test_vanBarelsUnboundedQP.cpp |   69 +
 .../qpOASES/qpOASES-3.0/testing/index.html    |   12 +
 .../testing/matlab/auxFiles/generateExample.m |   25 +
 .../matlab/auxFiles/generateRandomQp.m        |  110 +
 .../testing/matlab/auxFiles/getKktResidual.m  |  134 +
 .../testing/matlab/auxFiles/index.html        |   13 +
 .../testing/matlab/auxFiles/isoctave.m        |   21 +
 .../matlab/auxFiles/setupQpDataStruct.m       |   18 +
 .../matlab/auxFiles/setupQpFeaturesStruct.m   |   21 +
 .../testing/matlab/data/fetch_matlab_data     |   34 +
 .../testing/matlab/data/index.html            |    8 +
 .../qpOASES-3.0/testing/matlab/index.html     |   11 +
 .../qpOASES-3.0/testing/matlab/runAllTests.m  |  153 +
 .../testing/matlab/tests/index.html           |   37 +
 .../testing/matlab/tests/runAlexInfeas1.m     |   30 +
 .../testing/matlab/tests/runAlexInfeas2.m     |   35 +
 .../matlab/tests/runAlternativeX0Test.m       |   84 +
 .../testing/matlab/tests/runBenchmarkCHAIN1.m |   56 +
 .../matlab/tests/runBenchmarkCHAIN1A.m        |   57 +
 .../testing/matlab/tests/runBenchmarkCRANE1.m |   51 +
 .../testing/matlab/tests/runBenchmarkCRANE2.m |   52 +
 .../testing/matlab/tests/runBenchmarkCRANE3.m |   51 +
 .../testing/matlab/tests/runBenchmarkDIESEL.m |   55 +
 .../matlab/tests/runBenchmarkEQUALITY1.m      |   52 +
 .../matlab/tests/runBenchmarkEQUALITY2.m      |   56 +
 .../matlab/tests/runBenchmarkEXAMPLE1.m       |   52 +
 .../matlab/tests/runBenchmarkEXAMPLE1A.m      |   52 +
 .../matlab/tests/runBenchmarkEXAMPLE1B.m      |   52 +
 .../matlab/tests/runBenchmarkIDHESSIAN1.m     |   55 +
 .../matlab/tests/runInterfaceSeqTest.m        |  428 ++
 .../testing/matlab/tests/runInterfaceTest.m   |  459 ++
 .../testing/matlab/tests/runQAP8.m            |   36 +
 .../testing/matlab/tests/runQSHARE1B.m        |   43 +
 .../testing/matlab/tests/runRandomIdHessian.m |  428 ++
 .../matlab/tests/runRandomZeroHessian.m       |  426 ++
 .../matlab/tests/runSimpleSpringExample.m     |   34 +
 .../matlab/tests/runTestAPrioriKnownSeq1.m    |   87 +
 .../testing/matlab/tests/runTestSeq.m         |  104 +
 .../testing/matlab/tests/runTestSparse.m      |   33 +
 .../testing/matlab/tests/runTestSparse2.m     |   33 +
 .../testing/matlab/tests/runTestSparse3.m     |   39 +
 .../testing/matlab/tests/runTestSparse4.m     |   29 +
 .../matlab/tests/runTestWorkingSetLI.m        |   50 +
 .../matlab/tests/runVanBarelsUnboundedQP.m    |   25 +
 .../qpOASES/qpOASES-3.0/testing/runUnitTests  |  106 +
 CMakeLists.txt                                |    2 +-
 idl/AutoBalancerService.idl                   |    1 +
 rtc/AutoBalancer/AutoBalancer.cpp             |  101 +-
 rtc/Stabilizer/Stabilizer.cpp                 |    1 +
 238 files changed, 55775 insertions(+), 38 deletions(-)
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/AUTHORS
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/AUTHORS.txt
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/CMakeLists.txt
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/CTestTestfile.cmake
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/INSTALL
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/INSTALL.txt
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/LICENSE
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/LICENSE.txt
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/README
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/README.txt
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/VERSIONS
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/VERSIONS.txt
 create mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/example1
 create mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/example1a
 create mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/example1b
 create mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/example2
 create mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/example3
 create mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/example3b
 create mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/example4
 create mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/example5
 create mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/exampleLP
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/bin/index.html
 create mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/qrecipe
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/doc/DoxygenLayout.xml
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/doc/doxygen.config
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/doc/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/doc/mainpage.dox
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/doc/manual.pdf
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example1.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example1a.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example1b.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example2.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example3.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example3b.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example4.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example4CP.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example5.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/exampleLP.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/qrecipe.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Bounds.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Bounds.ipp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constants.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/ConstraintProduct.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constraints.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constraints.ipp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Flipper.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Indexlist.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Indexlist.ipp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Matrices.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/MessageHandling.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/MessageHandling.ipp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Options.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblem.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblem.ipp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblemB.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblemB.ipp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SQProblem.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SQProblem.ipp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SubjectTo.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SubjectTo.ipp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Types.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/UnitTesting.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Utils.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Utils.ipp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/OQPinterface.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/SolutionAnalysis.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/SolutionAnalysis.ipp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1.mat
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1a.mat
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1b.mat
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/make.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_auxInput.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_matlab_utils.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_matlab_utils.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_options.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_sequence.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_sequence.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/clean
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/clean.sh
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1.mat
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1a.mat
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1b.mat
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/make.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_auxInput.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_octave_utils.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_octave_utils.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_options.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_sequence.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_sequence.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/README.rst
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/example1.pyx
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/setup.py
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example1.py
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example1b.py
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example2.py
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/qpoases.pxd
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/qpoases.pyx
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/setup.py
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/__init__.py
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_examples.py
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_idhessian.py
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_testbench.py
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1.dat
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1a.dat
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1b.dat
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESinterface.c
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESinterface.sce
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESroutines.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_QProblem.mdl
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_QProblemB.mdl
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_SQProblem.mdl
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_QProblem.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_QProblemB.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_SQProblem.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/make.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_QProblem.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_QProblemB.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_SQProblem.cpp
 create mode 120000 3rdparty/qpOASES/qpOASES-3.0/libs/libqpOASES.so
 create mode 100755 3rdparty/qpOASES/qpOASES-3.0/libs/libqpOASES.so.3.0
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/make.mk
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/make_cygwin.mk
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/make_linux.mk
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/make_osx.mk
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/make_windows.mk
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/BLASReplacement.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/Bounds.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/Constraints.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/Flipper.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/Indexlist.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/LAPACKReplacement.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/Matrices.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/MessageHandling.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/OQPinterface.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/Options.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/QProblem.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/QProblemB.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/SQProblem.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/SolutionAnalysis.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/SubjectTo.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/Utils.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/checkForMemoryLeaks
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/data/fetch_cpp_data
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/data/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_bench.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_constraintProduct1.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_constraintProduct2.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1a.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1b.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example2.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example4.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example5.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example6.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example7.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_exampleLP.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_gradientShift.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_guessedWS1.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_indexlist.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_janick1.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_janick2.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices2.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices3.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_qrecipe.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_qrecipe_data.hpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_runAllOqpExamples.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_sebastien1.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_vanBarelsUnboundedQP.cpp
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/generateExample.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/generateRandomQp.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/getKktResidual.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/isoctave.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/setupQpDataStruct.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/setupQpFeaturesStruct.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/data/fetch_matlab_data
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/data/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/runAllTests.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/index.html
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlexInfeas1.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlexInfeas2.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlternativeX0Test.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCHAIN1.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCHAIN1A.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE1.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE2.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE3.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkDIESEL.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEQUALITY1.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEQUALITY2.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1A.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1B.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkIDHESSIAN1.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runInterfaceSeqTest.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runInterfaceTest.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runQAP8.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runQSHARE1B.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runRandomIdHessian.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runRandomZeroHessian.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runSimpleSpringExample.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestAPrioriKnownSeq1.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSeq.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse2.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse3.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse4.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestWorkingSetLI.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runVanBarelsUnboundedQP.m
 create mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/runUnitTests

diff --git a/3rdparty/qpOASES/qpOASES-3.0/AUTHORS b/3rdparty/qpOASES/qpOASES-3.0/AUTHORS
new file mode 100644
index 00000000000..49b20f8dc7d
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/AUTHORS
@@ -0,0 +1,74 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+MAIN AUTHORS
+============
+
+qpOASES's core functionality and software design have been developed by the 
+following main developers (in alphabetical order):
+
+    Hans Joachim Ferreau
+    Christian Kirches
+    Andreas Potschka
+
+
+
+FURTHER AUTHORS
+===============
+
+Moreover, the following developers have contributed code to qpOASES's 
+third-party interfaces (in alphabetical order):
+
+    Alexander Buchner
+    Holger Diedam
+    Manuel Kudruss
+    Sebastian F. Walter
+
+
+
+CONTRIBUTORS
+============
+
+Finally, the following people have not contributed to the source code,
+but have helped making qpOASES even more useful by testing, reporting 
+bugs or proposing algorithmic improvements (in alphabetical order):
+
+    Eckhard Arnold
+    Boris Houska
+    Aude Perrin
+    Milan Vukov
+    Thomas Wiese
+    Leonard Wirsching
+
+
+
+All users are invited to further improve qpOASES by providing comments, 
+code enhancements, bug reports, additional documentation or whatever you 
+feel is missing.
+
+
+
+##
+##	end of file
+##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/AUTHORS.txt b/3rdparty/qpOASES/qpOASES-3.0/AUTHORS.txt
new file mode 100644
index 00000000000..5132d8c105e
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/AUTHORS.txt
@@ -0,0 +1,74 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+MAIN AUTHORS
+============
+
+qpOASES's core functionality and software design have been developed by the 
+following main developers (in alphabetical order):
+
+    Hans Joachim Ferreau
+    Christian Kirches
+    Andreas Potschka
+
+
+
+FURTHER AUTHORS
+===============
+
+Moreover, the following developers have contributed code to qpOASES's 
+third-party interfaces (in alphabetical order):
+
+    Alexander Buchner
+    Holger Diedam
+    Manuel Kudruss
+    Sebastian F. Walter
+
+
+
+CONTRIBUTORS
+============
+
+Finally, the following people have not contributed to the source code,
+but have helped making qpOASES even more useful by testing, reporting 
+bugs or proposing algorithmic improvements (in alphabetical order):
+
+    Eckhard Arnold
+    Boris Houska
+    Aude Perrin
+    Milan Vukov
+    Thomas Wiese
+    Leonard Wirsching
+
+
+
+All users are invited to further improve qpOASES by providing comments, 
+code enhancements, bug reports, additional documentation or whatever you 
+feel is missing.
+
+
+
+##
+##	end of file
+##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/CMakeLists.txt b/3rdparty/qpOASES/qpOASES-3.0/CMakeLists.txt
new file mode 100644
index 00000000000..fa0c2a3d123
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/CMakeLists.txt
@@ -0,0 +1,150 @@
+##
+##     This file is part of qpOASES.
+##
+##     qpOASES -- An Implementation of the Online Active Set Strategy.
+##     Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##     Christian Kirches et al. All rights reserved.
+##
+##     qpOASES 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.
+##
+##     qpOASES 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 qpOASES; if not, write to the Free Software
+##     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+##
+##     Filename:  CMakeLists.txt
+##     Author:    Hans Joachim Ferreau (thanks to Milan Vukov)
+##     Version:   3.0
+##     Date:      2007-2014
+##
+
+cmake_minimum_required(VERSION 2.6)
+
+PROJECT(qpOASES CXX)
+SET(PACKAGE_NAME "qpOASES")
+SET(PACKAGE_VERSION "3.0.1")
+SET(PACKAGE_SO_VERSION "3.0")
+SET(PACKAGE_DESCRIPTION "QP solver using the online active set strategy")
+SET(PACKAGE_AUTHOR "Hans Joachim Ferreau")
+SET(PACKAGE_MAINTAINER "Hans Joachim Ferreau")
+SET(PACKAGE_URL "https://projects.coin-or.org/qpOASES")
+
+SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/bin)
+SET(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/libs)
+
+IF( NOT CMAKE_VERBOSE_MAKEFILE )
+    SET( CMAKE_VERBOSE_MAKEFILE OFF )
+ENDIF( NOT CMAKE_VERBOSE_MAKEFILE )
+
+IF( NOT CMAKE_BUILD_TYPE )
+    SET(CMAKE_BUILD_TYPE Release CACHE STRING
+        "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel."
+        FORCE
+        )
+ENDIF( NOT CMAKE_BUILD_TYPE )
+
+
+############################################################
+#################### compiler flags ########################
+############################################################
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D__NO_COPYRIGHT__")
+IF ( UNIX )
+    SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -pedantic -Wfloat-equal -Wshadow -DLINUX")
+    SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_DEBUG} -O3 -finline-functions")
+ELSEIF( WINDOWS )
+    SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -nologo -EHsc -DWIN32")
+ENDIF()
+
+SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D__DEBUG__")
+
+############################################################
+######################## rpath #############################
+############################################################
+# use, i.e. don't skip the full RPATH for the build tree
+set(CMAKE_SKIP_BUILD_RPATH  FALSE)
+
+# when building, don't use the install RPATH already
+# (but later on when installing)
+set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+
+set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib:${CMAKE_INSTALL_PREFIX}/lib/casadi")
+
+# add the automatically determined parts of the RPATH
+# which point to directories outside the build tree to the install RPATH
+set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+
+# the RPATH to be used when installing, but only if it's not a system directory
+list(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir)
+if("${isSystemDir}" STREQUAL "-1")
+   set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib:${CMAKE_INSTALL_PREFIX}/lib/casadi")
+endif("${isSystemDir}" STREQUAL "-1")
+
+
+
+############################################################
+#################### build and install #####################
+############################################################
+INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/include)
+
+# compile qpOASES libraries
+FILE(GLOB SRC src/*.cpp)
+
+# library
+#ADD_LIBRARY(qpOASES STATIC ${SRC})
+ADD_LIBRARY(qpOASES SHARED ${SRC})
+INSTALL(TARGETS qpOASES
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION lib
+)
+SET_TARGET_PROPERTIES(
+    qpOASES
+    PROPERTIES
+    SOVERSION ${PACKAGE_SO_VERSION}
+    )
+
+# headers
+INSTALL(FILES include/qpOASES.hpp
+  DESTINATION include)
+INSTALL(DIRECTORY include/qpOASES
+  DESTINATION include
+  FILES_MATCHING PATTERN "*.hpp"
+  PATTERN ".svn" EXCLUDE)
+
+############################################################
+######################### examples #########################
+############################################################
+# compile qpOASES examples
+SET(EXAMPLE_NAMES
+    example1
+    example1a
+    example1b
+    example2
+    example3
+    example3b
+    example4
+    example5
+    exampleLP
+    qrecipe
+)
+
+FOREACH(ELEMENT ${EXAMPLE_NAMES})
+    ADD_EXECUTABLE(${ELEMENT} examples/${ELEMENT}.cpp)
+    TARGET_LINK_LIBRARIES(${ELEMENT} qpOASES)
+ENDFOREACH(ELEMENT ${EXAMPLE_NAMES})
+
+
+##
+##   end of file
+##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/CTestTestfile.cmake b/3rdparty/qpOASES/qpOASES-3.0/CTestTestfile.cmake
new file mode 100644
index 00000000000..321a6fdcdf6
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/CTestTestfile.cmake
@@ -0,0 +1,6 @@
+# CMake generated Testfile for 
+# Source directory: /home/ishiguro/catkin_ws/chidori/build/hrpsys/3rdparty/qpOASES/qpOASES-3.0
+# Build directory: /home/ishiguro/catkin_ws/chidori/build/hrpsys/3rdparty/qpOASES/qpOASES-3.0
+# 
+# This file includes the relevant testing commands required for 
+# testing this directory and lists subdirectories to be tested as well.
diff --git a/3rdparty/qpOASES/qpOASES-3.0/INSTALL b/3rdparty/qpOASES/qpOASES-3.0/INSTALL
new file mode 100644
index 00000000000..69852192cec
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/INSTALL
@@ -0,0 +1,79 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+INSTALLATION UNDER LINUX
+========================
+
+0. Obtain qpOASES from COIN-OR:
+
+Download a zipped archive containg the latest stable release and unpack it 
+into <install-dir>. Alternatively, you check out the latest stable branch, 
+e.g. by running
+
+svn co https://projects.coin-or.org/svn/qpOASES/stable/3.0 <install-dir>
+
+from your shell. 
+
+
+1. Compilation of the qpOASES library libqpOASES.a and test examples:
+
+cd <install-dir>
+make
+
+The library  libqpOASES.a  provides the complete functionality of the qpOASES 
+software package. It can be used by, e.g., linking it against a main function 
+from the examples folder. The make also compiles a couple of test examples; 
+executables are stored within the directory <install-dir>/bin.
+
+
+2. Running a simple test example:
+
+Among others, an executable called  example1  should have been created; run 
+it in order to test your installation:
+
+cd <install-dir>/bin
+./example1
+
+If it terminates after successfully solving two QP problems, qpOASES has been 
+successfully installed!
+
+
+3. Optional, create source code documentation (using doxygen):
+
+cd <install-dir>/doc
+doxygen doxygen.config
+
+Afterwards, you can open the file <install-dir>/doc/html/index.html with
+your favorite browser in order to view qpOASES's source code documentation.
+
+
+NOTE: More detailed installation instructions, including information on how
+      to run unit tests can be found in the qpOASES User's Manual located 
+      at <install-dir>/doc/manual.pdf!
+
+
+
+##
+##	end of file
+##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/INSTALL.txt b/3rdparty/qpOASES/qpOASES-3.0/INSTALL.txt
new file mode 100644
index 00000000000..89eb3abb47d
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/INSTALL.txt
@@ -0,0 +1,79 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+INSTALLATION UNDER LINUX
+========================
+
+0. Obtain qpOASES from COIN-OR:
+
+Download a zipped archive containg the latest stable release and unpack it 
+into <install-dir>. Alternatively, you check out the latest stable branch, 
+e.g. by running
+
+svn co https://projects.coin-or.org/svn/qpOASES/stable/3.0 <install-dir>
+
+from your shell. 
+
+
+1. Compilation of the qpOASES library libqpOASES.a and test examples:
+
+cd <install-dir>
+make
+
+The library  libqpOASES.a  provides the complete functionality of the qpOASES 
+software package. It can be used by, e.g., linking it against a main function 
+from the examples folder. The make also compiles a couple of test examples; 
+executables are stored within the directory <install-dir>/bin.
+
+
+2. Running a simple test example:
+
+Among others, an executable called  example1  should have been created; run 
+it in order to test your installation:
+
+cd <install-dir>/bin
+./example1
+
+If it terminates after successfully solving two QP problems, qpOASES has been 
+successfully installed!
+
+
+3. Optional, create source code documentation (using doxygen):
+
+cd <install-dir>/doc
+doxygen doxygen.config
+
+Afterwards, you can open the file <install-dir>/doc/html/index.html with
+your favorite browser in order to view qpOASES's source code documentation.
+
+
+NOTE: More detailed installation instructions, including information on how
+      to run unit tests can be found in the qpOASES User's Manual located 
+      at <install-dir>/doc/manual.pdf!
+
+
+
+##
+##	end of file
+##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/LICENSE b/3rdparty/qpOASES/qpOASES-3.0/LICENSE
new file mode 100644
index 00000000000..9ef3d701d14
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/LICENSE
@@ -0,0 +1,503 @@
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
+		     END OF TERMS AND CONDITIONS
+
+           How to Apply These Terms to Your New Libraries
+
+  If you develop a new library, and you want it to be of the greatest
+possible use to the public, we recommend making it free software that
+everyone can redistribute and change.  You can do so by permitting
+redistribution under these terms (or, alternatively, under the terms of the
+ordinary General Public License).
+
+  To apply these terms, attach the following notices to the library.  It is
+safest to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least the
+"copyright" line and a pointer to where the full notice is found.
+
+    <one line to give the library's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    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 Street, Fifth Floor, Boston, MA  02110-1301  USA
+
+Also add information on how to contact you by electronic and paper mail.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the library, if
+necessary.  Here is a sample; alter the names:
+
+  Yoyodyne, Inc., hereby disclaims all copyright interest in the
+  library `Frob' (a library for tweaking knobs) written by James Random Hacker.
+
+  <signature of Ty Coon>, 1 April 1990
+  Ty Coon, President of Vice
+
+That's all there is to it!
+
diff --git a/3rdparty/qpOASES/qpOASES-3.0/LICENSE.txt b/3rdparty/qpOASES/qpOASES-3.0/LICENSE.txt
new file mode 100644
index 00000000000..2d59e84cd83
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/LICENSE.txt
@@ -0,0 +1,503 @@
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
+		     END OF TERMS AND CONDITIONS
+
+           How to Apply These Terms to Your New Libraries
+
+  If you develop a new library, and you want it to be of the greatest
+possible use to the public, we recommend making it free software that
+everyone can redistribute and change.  You can do so by permitting
+redistribution under these terms (or, alternatively, under the terms of the
+ordinary General Public License).
+
+  To apply these terms, attach the following notices to the library.  It is
+safest to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least the
+"copyright" line and a pointer to where the full notice is found.
+
+    <one line to give the library's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    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 Street, Fifth Floor, Boston, MA  02110-1301  USA
+
+Also add information on how to contact you by electronic and paper mail.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the library, if
+necessary.  Here is a sample; alter the names:
+
+  Yoyodyne, Inc., hereby disclaims all copyright interest in the
+  library `Frob' (a library for tweaking knobs) written by James Random Hacker.
+
+  <signature of Ty Coon>, 1 April 1990
+  Ty Coon, President of Vice
+
+That's all there is to it!
+
diff --git a/3rdparty/qpOASES/qpOASES-3.0/README b/3rdparty/qpOASES/qpOASES-3.0/README
new file mode 100644
index 00000000000..2147a72fb67
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/README
@@ -0,0 +1,84 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+INTRODUCTION
+============
+
+qpOASES is an open-source C++ implementation of the recently proposed 
+online active set strategy, which was inspired by important observations 
+from the field of parametric quadratic programming (QP). It has several 
+theoretical features that make it particularly suited for model predictive 
+control (MPC) applications. Further numerical modifications have made 
+qpOASES a reliable QP solver, even when tackling semi-definite, ill-posed or 
+degenerated QP problems. Moreover, several interfaces to third-party software 
+like ​Matlab or ​Simulink are provided that make qpOASES easy-to-use even for 
+users without knowledge of C/C++.
+
+
+
+GETTING STARTED
+===============
+
+1. For installation, usage and additional information on this software package 
+   see the qpOASES User's Manual located at doc/manual.pdf or check its
+   source code documentation!
+
+
+2. The file LICENSE.txt contains a copy of the GNU Lesser General Public 
+   License (v2.1). Please read it carefully before using qpOASES!
+
+
+3. The whole software package can be obtained from 
+
+       http://www.qpOASES.org/ or
+	   https://projects.coin-or.org/qpOASES/
+
+   On this webpage you will also find further support such as a list of 
+   questions posed by other users.
+
+
+
+CONTACT THE AUTHORS
+===================
+
+If you have got questions, remarks or comments on qpOASES, it is strongly 
+encouraged to report them by creating a new ticket at the qpOASES webpage.
+In case you do not want to disclose your feedback to the public, you may
+send an e-mail to
+
+        support@qpOASES.org
+
+Finally, you may contact one of the main authors directly:
+
+        Hans Joachim Ferreau, joachim.ferreau@ch.abb.com
+        Andreas Potschka,     potschka@iwr.uni-heidelberg.de
+        Christian Kirches,    christian.kirches@iwr.uni-heidelberg.de
+
+Also bug reports, source code enhancements or success stories are most welcome!
+
+
+
+##
+##	end of file
+##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/README.txt b/3rdparty/qpOASES/qpOASES-3.0/README.txt
new file mode 100644
index 00000000000..561939fa534
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/README.txt
@@ -0,0 +1,84 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+INTRODUCTION
+============
+
+qpOASES is an open-source C++ implementation of the recently proposed 
+online active set strategy, which was inspired by important observations 
+from the field of parametric quadratic programming (QP). It has several 
+theoretical features that make it particularly suited for model predictive 
+control (MPC) applications. Further numerical modifications have made 
+qpOASES a reliable QP solver, even when tackling semi-definite, ill-posed or 
+degenerated QP problems. Moreover, several interfaces to third-party software 
+like ​Matlab or ​Simulink are provided that make qpOASES easy-to-use even for 
+users without knowledge of C/C++.
+
+
+
+GETTING STARTED
+===============
+
+1. For installation, usage and additional information on this software package 
+   see the qpOASES User's Manual located at doc/manual.pdf or check its
+   source code documentation!
+
+
+2. The file LICENSE.txt contains a copy of the GNU Lesser General Public 
+   License (v2.1). Please read it carefully before using qpOASES!
+
+
+3. The whole software package can be obtained from 
+
+       http://www.qpOASES.org/ or
+	   https://projects.coin-or.org/qpOASES/
+
+   On this webpage you will also find further support such as a list of 
+   questions posed by other users.
+
+
+
+CONTACT THE AUTHORS
+===================
+
+If you have got questions, remarks or comments on qpOASES, it is strongly 
+encouraged to report them by creating a new ticket at the qpOASES webpage.
+In case you do not want to disclose your feedback to the public, you may
+send an e-mail to
+
+        support@qpOASES.org
+
+Finally, you may contact one of the main authors directly:
+
+        Hans Joachim Ferreau, joachim.ferreau@ch.abb.com
+        Andreas Potschka,     potschka@iwr.uni-heidelberg.de
+        Christian Kirches,    christian.kirches@iwr.uni-heidelberg.de
+
+Also bug reports, source code enhancements or success stories are most welcome!
+
+
+
+##
+##	end of file
+##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/VERSIONS b/3rdparty/qpOASES/qpOASES-3.0/VERSIONS
new file mode 100644
index 00000000000..4681efc920e
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/VERSIONS
@@ -0,0 +1,106 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+VERSION HISTORY
+===============
+
+
+3.0 (released on 29th July 2014, last updated on 17th December 2014):
+---------------------------------------------------------------------
+
++ Addition of unit testing
++ Several bugfixes
+
+
+3.0beta (released on 16th August 2011, last updated on 4th April 2014):
+-----------------------------------------------------------------------
+
++ Improved ratio tests and termination check for increased reliabilty
++ Introduction of iterative refinement in step determination and 
+  drift correction to handle ill-conditioned QPs
++ Introduction of ramping strategy to handle degenerated QPs
++ Addition of far bounds and flipping bounds strategy to handle 
+  semi-definite and unbounded QPs more reliably
++ Limited support of sparse QP matrices (also in Matlab interface)
++ Optional linking of LAPACK/BLAS for linear algebra operations
++ Addition of a number of algorithmic options, summarised in an option struct
++ Improved Matlab interface
++ Python interface added
++ Several bugfixes
+
+
+2.0 (released on 10th February 2009, last updated on 7th December 2009):
+------------------------------------------------------------------------
+
++ Implementation of regularisation scheme for treating QPs with 
+  semi-definite Hessians
++ Addition of convenience functionality for Bounds and Constraints 
+  objects for specifying guessed active sets
++ Allows to specify a CPU time in addition to an iteration limit
++ Improved efficiency for QPs comprising many constraints
++ Source code cleanup and bugfixing
+
+
+1.3 (released on 2nd June 2008, last updated on 13th August 2008):
+------------------------------------------------------------------
+
++ Implementation of "initialised homotopy" concept
++ Addition of the SolutionAnalysis class
++ Utility functions for solving test problems in OQP format added
++ Flexibility of Matlab(R) interface enhanced
++ Major source code cleanup
+  (Attention: a few class names and calling interfaces have changed!)
+
+  
+1.2 (released on 9th October 2007):
+-----------------------------------
+
++ Special treatment of diagonal Hessians
++ Improved infeasibility detection
++ Further improved Matlab(R) interface
++ Extended Simulink(R) interface
++ scilab interface added
++ Code cleanup and several bugfixes
+
+
+1.1 (released on 8th July 2007):
+--------------------------------
+
++ Implementation of the QProblemB class
++ Basic implementation of the SQProblem class
++ Improved Matlab(R) interface
++ Enabling/Disabling of constraints introduced
++ Several bugfixes
+
+
+1.0 (released on 17th April 2007):
+----------------------------------
+
+Initial release.
+
+
+
+##
+##	end of file
+##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/VERSIONS.txt b/3rdparty/qpOASES/qpOASES-3.0/VERSIONS.txt
new file mode 100644
index 00000000000..8824a9dd5ef
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/VERSIONS.txt
@@ -0,0 +1,106 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+VERSION HISTORY
+===============
+
+
+3.0 (released on 29th July 2014, last updated on 17th December 2014):
+---------------------------------------------------------------------
+
++ Addition of unit testing
++ Several bugfixes
+
+
+3.0beta (released on 16th August 2011, last updated on 4th April 2014):
+-----------------------------------------------------------------------
+
++ Improved ratio tests and termination check for increased reliabilty
++ Introduction of iterative refinement in step determination and 
+  drift correction to handle ill-conditioned QPs
++ Introduction of ramping strategy to handle degenerated QPs
++ Addition of far bounds and flipping bounds strategy to handle 
+  semi-definite and unbounded QPs more reliably
++ Limited support of sparse QP matrices (also in Matlab interface)
++ Optional linking of LAPACK/BLAS for linear algebra operations
++ Addition of a number of algorithmic options, summarised in an option struct
++ Improved Matlab interface
++ Python interface added
++ Several bugfixes
+
+
+2.0 (released on 10th February 2009, last updated on 7th December 2009):
+------------------------------------------------------------------------
+
++ Implementation of regularisation scheme for treating QPs with 
+  semi-definite Hessians
++ Addition of convenience functionality for Bounds and Constraints 
+  objects for specifying guessed active sets
++ Allows to specify a CPU time in addition to an iteration limit
++ Improved efficiency for QPs comprising many constraints
++ Source code cleanup and bugfixing
+
+
+1.3 (released on 2nd June 2008, last updated on 13th August 2008):
+------------------------------------------------------------------
+
++ Implementation of "initialised homotopy" concept
++ Addition of the SolutionAnalysis class
++ Utility functions for solving test problems in OQP format added
++ Flexibility of Matlab(R) interface enhanced
++ Major source code cleanup
+  (Attention: a few class names and calling interfaces have changed!)
+
+  
+1.2 (released on 9th October 2007):
+-----------------------------------
+
++ Special treatment of diagonal Hessians
++ Improved infeasibility detection
++ Further improved Matlab(R) interface
++ Extended Simulink(R) interface
++ scilab interface added
++ Code cleanup and several bugfixes
+
+
+1.1 (released on 8th July 2007):
+--------------------------------
+
++ Implementation of the QProblemB class
++ Basic implementation of the SQProblem class
++ Improved Matlab(R) interface
++ Enabling/Disabling of constraints introduced
++ Several bugfixes
+
+
+1.0 (released on 17th April 2007):
+----------------------------------
+
+Initial release.
+
+
+
+##
+##	end of file
+##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/example1 b/3rdparty/qpOASES/qpOASES-3.0/bin/example1
new file mode 100755
index 0000000000000000000000000000000000000000..6a0a92bb236a494c7bbbafe9dd0767a1d92c0e03
GIT binary patch
literal 32801
zcmeHwdwg5PmH%AHc4RBIujM2p<bePu8y<efNt}=n$(9_6$PXkr4+7lSmST(8lCh*b
zC=Ds$WlCs5Db2QQ30>M|yJed$v;;~^c(l-N>84O<fOeY&N)rmdh7{WL0r`E;JftgG
zF5Ta+pZ$xQSTpC$nKNh3oS8W@_ujeJc?0bhO=Ie^u!|W%4_3K2O8hc57~`PCwXjm=
zV)NL!tOS^1{JD4vg(9ZHQ<Aj`ZIaH4un2!G7J+E7xOhZ`vjm!{P)SH3m6yF-GAhDL
zrV3Dnj5#@i@{z31%;q4%4LqX46H*SH1ml;J{}P(=9h3Qv$$To*RX!Q2`cd8JFD&t4
ziwh9JE((~3|7Js?rK|f_sR+gQS91`di$_$rOrV(xRlZA+kL3JYFI^G_WqA|nCEGqC
zgKCHRp;+Id=K7&X?NBV9-del0Wl`;-=DK8}u8G&1_!I5Q&K{mo>2W$Bl6eaLNY#`c
zJ>!K}Cmm`HpFH>6SN`MMQ{Fmo<R>0Vqo$;Mlkm4&-(h2CPbs$*6_wvo#N3oN1Ak8Z
zT~*rl;v=u!ac!&jg}2U|ecP<bOFwIG>U-twudRCR(96%h_Jt!?ubp+(b)UQT;a%@k
zOrG-U7v6mSw-wv`o4$pt6J9q%ct?TM=YT7Oe|rLYYDQ-PKIcq;zaJGY#Q*UL@QWtk
z6Py4apFqw(gT7F??g{AkO+f#`1oT4_(0^wFdg@n&<T+yk`a36}zi0yd128Zh7PQ~F
z0g`2}H$bnmGue?m{PP??gB{GnKLUIe=nqM}>i^%gQoE1m|1j_lR-PGORmt}OU&Sh!
zQ}%b&uMbK3eGV5Vjmiu!OF8r9`7!Xs-&G;#Q^-%(4>=z@J386LVfAyT<gd$ek)2;(
z;P?tQMdB6x)to;d$zS0a#)GRzy4)dehz5ZLbq&lg28I)HBbn+QO&JC=F4=4ZqXV&I
zDmvOW)SFC3lg#jkJB&zlRHDNjZ9|E8G~C-a6cuTOQ!eXVEX!)Sw0krG>fz>CJeKly
zuZo14jJ*HBSa*aO1@g2ECQ@9w{CxR27I%%LVu^UNad9-BOpiv}5T|;?`?q;Fn^`Nu
z-jFxAD%5~VU)H&5oUDxt2cjuYx_2m)7)ldkuMjXpxkS~fwQ(^>yGLWg<A}#`^)%+n
zVq&r&$(h7cf#~Mwkhe2rG&X>#f(z9kpPQ;#n=GY(tV<wjSKn2udxvrb$+Qvj#$r@>
zqc56F#(Lx7Z6i@{j*d)1H;s^lQuE*l152hN{TEz7_O*ztYwK2HBs!W*#CwNgscpvQ
z2BI10?>CY}(#VXS_?B2a0;@}=hoh9(ukv!<8;$<KO$<yE=@j@;y0q5e-dLO&(MWHq
z7YTjIq`(6ekFff5a+IdO`rZ`GxVEo185Pl`^@EAwXnia>7#m2BChGfpQ=4LOV@tBW
ze=rtFjK=Ew(y^gP{ov?Ga$B-~!DwUzU1M9lv}G06qVo{{<kI@YNUDA`k*r58Vgm_6
zes~!XYGWc*Rq9Wsjg9CY=uus$M+73JlTmaDgX*VhCaO=g)RWlCwl;%0aogIp?pD9i
zShs+!4ES5yjHbG#x`mneg1Y9+2*4v2KC9>A1Vmn&#{XGmC?N5x#yf&;;$s`b_z8b#
z-ZndVe23*YM(KT~^yk0635<)_kxaUU{j*G`a?@wVO3BlB4e8{Qq-GFoVZW91r_S0y
zWZcJ7G7zlBbCQ~T7IjT`Zs1Y!NiY><E~g2P;xpHL6J8d{(;7^8bGa=hyxE^~nebG$
zx_l;lnL;A$HsPVb%(d2ppPa!mw!wriH{l0O_zDw##DuRj;kTOb%EzSS9VUF0iT)N7
z-aOy$HsQ}P(eE|krHjl{_nGkITdK>q^DUe2lf~uLPUgGuP^$RYDx~;!K4p7SP@G<9
zLDK2DMfj~e%Y_)hgOqsu=xJPY|KtEnO?~_@hffkrO?><ihu<XFO7H;=ze+GQ>G6FW
zewko0qvN|d{2am5w8wXF_=f~jlO7-8@Dl`+8*zLChaVvrHp`B8bNJs0rlvmb;_z1q
zrY1h#z~L_vOig>-$>GltOig-R=kOf_Q&S#i9KMxcYQp0uJ^*6w^#oJX9Y4z9YY3($
zJAN22bU6luZ>*U3Jh?mcFjeId($9BEDZWp<r2})aQeR$$O8a&_T0=g(@97(Arpx|<
zwAt@NL4<Q=aSLCPxI<t=7wD$G4cy7fsz+<)gAkFsYj*GyV0H_V+0StjefhtOuv~;y
z57y`+OR8oB8B;Y|@%v!SEh_t7k$qvm%)UvKbZ5<eL2-Fa12TkbT)fn01ohNmK|R|m
zsBf#;C#WB+IUpjB)*KX(+iDJr$elGuMC9_CZZ61%V*{i}Xr=)Y9y?zGwDR=rdQc#L
z^}e7W<gu*)K8gBp0-?`iVNT$wart&#UUOk!myjf|>jthD->yK-h;P@OH9JtWZ_HC;
z^X>A~G!T2zO14!zHGFXMja^=&`^JiDp7zvSNVawCKF<Hanxn)(HsUcuB2So+U*$+C
z)-V2Y`t-)i`NzIHAATbm@V*)vB_G2dL3h`jLgXM>44^w}hz}yS)f^L%8+dWY4uVMl
zHP{e<(A_@NjZ{h1ImAPuY$cTsGTVShJ!fU*e+oRPx<`2qDcwC1nIr$394RSg1Ni@z
z|J0vF<B{PX`?h4y+xkj#S?>_pC!V72(s*e7lfJPhRh5ptNJ(SAJQg=)M*6WifPeE+
z<J<nRpIqr1d)2q|^%LD;|I>%ad-py4I4u%<Pd`n*|FNB5@$Sam2Oo28U_HLtg9v;(
zPgVHFeznDN#gk;Dd_a8nipMDF^wG+*b`b6$L5lwIQ&!OX#-8v!{^kmwcG&lmQ)y&=
zga~~%K9_p;3g6iGMgCW1{vFG&u5e=7t?EI}W7{#HT=D(lD}mB}_au$4zMabl?I1|a
z$%rte=dq*MRRR6!!&e;t7_wBJ#f~RM=w9O<JMCWWAN#$#$3OOhI~>?GR|kV0->%x}
z2t$GMe%1Jj@A3DoG}0d{{}U8<<J+k@m1o^RQnJ9<n}M<4d%)xLl%M-{KB@VZ{3iV-
zO#th!a9{4e!hNONcyi;$Iz|7xCs8$;GvpMZ@f`@p%C^E2buM?Vcg~Goh(G5Q7df5V
za&Z}af_uTBo*ONr-jumctPs(oGCU<ic1Yl-xJ1Zgc;YbuAAL-lX}yT{e5miN{p0D=
zw;;R)l|6uPFGA`;lQEL4MR*D!4Yq?&;Rfn@xYV4eT@z&5R$ZGpd6I3nHc2P^Ec|^9
zz4&}0lviPT#%hpG^v|3+eVR;~Rp{*%(=VyCZ?WxQE6%*=`~@}V5DUuh!QWxXYW5E(
zFJ0O--2pDiBr7c?twJ8gE<_rYu^;(p?OIeb)y3Qu`cBL0ihYx<_13Zqy{*FLRz3}t
z@iqJ%f&5W~thGY_l*L;y{dTLj!nv!+TQUE(VqZnWO(njHmYtJ2DqKSqE$)g2cg6hH
z3TJD@bkMg}*top!Qhw+T-8SbseOf!`K?`QH$1TM_EMYHN{?qzu3A?|9fJaJz{{{Vg
zbbX|Ok2LU+20qfjM;iD@10QMNe^&z|aQ*3OfHOr`w}d%>AL#~;3u!NnE|pGuZ)TYG
z(&$n?+;1d2S)eob6V#rOx?e!|=IB!QS7!>4@4->@ObH*6_fwT$$X9Ymd+&qO2_o5R
zaZ!-D=j$sGF!{m!vbuPn-0VNYXQWH{htmbf?<LA84<s35&J7%v)nW&vB9#BD_S(*n
znH%gb9#eQGVI_Biq@xWRx>WgaD|vl(m$^6|t~rO59492~k?EZ1f8j^n$IdTmTDgm(
z=E-o84BKV6T80~CxLJlb$?z^2ep!a!lHvDd_@WHokm2uTSR(CrnhfX3Fke3GPcL=O
z@99g&Q)y>&T}xeaZBv><joTZW>l&Kt7Mv&W{8qVWUeQRZi&RRU`M1jQi@FWATf~tz
z&4Z1zqVFK*lR9IrJW9(Ew0iMRn}N34CXtC-HQPGmwpu2=j^_ugMU#p-vSd9+7Kf&R
zTTwA?b09<Uen6IGpexx)D;djqPDF2@D7Za?4pnR`vF#yHE9nCR%bhUFNe%xD%%sb}
z_lWJib5T#*BG{De_g4bVY~Mq_wEg*$6t(>T_sng7p}21QEc%P>eTq-F%|TUcA5h$B
z`!)F5PE&k->Gy~k(+FrN^*aG*9B3(h5S_$k;ef036O_G(1HRG^&@XJo9Oy260G-rU
z!hyA=>qsh_wjNA3ly;m2piJupFj#s7&0?FZk-#IRmr;t&fmG=%)XL`2hLEzg^e#%7
zrnLgtQF^Qzz!_RMfa^=|!LMz)HUMrrOYeuGY_qg20B$LD5`($g77*+%4N{&uZ4AKP
z(scyXb6{WT1_By5u)lPGfJP1+D2)@)q@9O62TNCyGMl+<he};e0E@JbqnxK$*?)n+
z_PI$9fMU9BA63$}){dWhDavejlUlz>)wS8ylUBY=wbfaP?R?-$Y}AH!YFZL|^7V)i
zmRgfzcPVT&u%8A{PPFC3qvAWLV(CpcV7#h4dos+a^bUGdKwnM+nEsEj5RFyNL8}y%
ze*PpS)I-GW`m@mab$T0`j_Fsx?lk=v+SH<NfN)k;8NsksRQlL?Xfypv)Pw1Vz+2N7
zBWls@SnXQ%w@^Znz8vg|bp~@PVU;x)#fwURb{S=R1)Q$azewY{{sa7K`ZYjX^iQBG
zS@lgo7qQCkK)pp4R}JLTzlip|PQM*QOdmn{ntl>xTJ(Pb!m3{i*^BfV2wAK@jarrH
z<)ECTF96=AuR=vi^|OE}(|?Wv?D~7)Hd+5AjG<g#0Qw63R`9IUqof1<S;$_cKRgHb
z%Jo$a#;Wzl(RHWj2aq~de+V6An*J{MoS{z#hw1u$NHRlT?ZmvLZ$^Bko<w|>?m=&z
ztv?H?=IAZJpQXP98Jzk|a5!6Ej`%tHBN#Hy)hl5EHTrvq&(&{+{^scy<M$u*Rgi7I
zegeumPq(5W&euNx<^ugFq`FY|LgrfiE0D8JzY3bJ*H;76ptnNWM*VA$ut|R(%3Yw(
z2j^zpK(2-QY;ahlKL))m)_)F3TJ(BwUZNjDDNFVDA?-!_uTc6j{SLJ4a{UPMUaTL5
z##ZPla6ZKJQlK7Z`s--dCzyV)3IiU~gBbUoVmfw9*!P%jpp@@3-2>=prY}VkKEw1o
zp{^e=eGC%+km&<J{s+_jkn=|{AwbUx2jIs{|0#I>C)1}P?J(0@A?8nDoM84G)1QP|
zpJ#e8H2PDfp9QgBVESI9y~y<EP~D#~{aR?_2+~lomzX|;dc2Hb4V14i{TpcApEJD{
ztbf7u_t6Uf#q?89+%K8_2I_Z|=@I1nZ>Bdx+*g@C7ZrPr>0d{UUT6AMDB)L3zXvLK
zgXvp={51*!&tptq2a%35{XLZaCQ1Oa-!T36(DYlV1A@1i{wy^6f0#ZAP4Qc%KaYY=
zF#Qjx(L1Ou^m3Bvbo=LbOurQcy~}hTgm@2~1byN6OfN(52d1YHoC1V`{)pa;41Z$!
z15m-Anf`BRmA^3Eg-XBA^m`%12WT8HJI(ZYK<?1=2f*_anoduxT(9Xnp!uDeJ`K<f
zn*JPk-l*ww(EvATdLPttv!?Gw{ch3p8c02+>0gGpw`%%(fIg||m9UuGH2oaZZ<nUu
z3`@RU)7z2ZQ=0xoK)W?obvwZMWs6Tj7{@zMb=iZ&ky#u8$W``D0&Et?9#o+0p(~M~
zTO4yJ<sbo5EROXE${r?Qy2bGtF?fW4Sr*3_<@q)NPD?cvQ?^q3JSucA{-%6^3K7^#
zv~K|OsKQRA95Sg>`w`GT%Ou^$bL}EtLG9N-pHxXxc&=$<sCZe=Qq=paAkxYp!c_e6
z;->Q4)AnH8ExWt~^`+rXt3`54hD302LeXXYH$v;2q#McWiR1uyPW=|iFzsGgS6OT(
z>dQ%zNZy`Da-7sOG6TJnxN3JH`Q8kPC}=yHt8DXn{Bn|qk^EF1$qrKKwe!(hoaE<7
zepQjMX{&%Q+wlk398didQt*VQkd#fUBK9}$femsYrz7QDN+snBHBUQ6RlV&f@^O-j
zklda}@;dUD?Rfy2<s?@kIi5ozSq)(*DEsU;&~NrbbJ|Uy*+X<(YofksBDwcg^hIK2
z`4=RAU9n=*o<RpJ`_fDZ%~PL7%5y4JG~O8&WGnmHvtU3BEi@>Y__K1FkSyOtol1A@
zK_$!o0D`J&AP?JHtUx<HjcVAhp_!Ixj&(?|UrW*!X^zVwoBiVi*fhr!CxGn)Ox7IF
zLCN;(2+%dhzfqnY1WeH!ug?MS2?C~TjvZ)e`}HK(EY0y56lUK^fKzjfU_i6qKx8$V
z<7onJBEjZsjt?***l#AFL32Do<=jF*3m6dD7y&NLaTDrdzm))==BPmbw||m=Zp~4S
zVZ(kK0c$k}-R-sCPQV5LB+jP@7}Okh!b0r32^i5FKfxGd-$TGw&G9j+_8sIf?9dz+
z5b$YAxkYoFAZ~Y36?SWmO(f`NC}pqacoWq2yC`L!=15aHpQV)jn&Slm_7ZSFbDRsS
zvi~Ci2Q|m{(CzG>CmkHp9Cwov?<NLMX^xcy{1XAsXpa9T;0px&NUNrXv#-z&qmD1)
zZ^|>&^#rz6djptv6n5&5dD1Krxn0v>ACvLNGnhsOyMG(3^QR!yYLHy7NJQ>whcIl|
zyIw}m=7zo!$sLM>O}iG|z#fW1cRV$Ulug;x??V#%n)PsJc<N51jAc`=fMwg)51@T{
z>b*$$N;b6`vzR^do=SZTDc{SczDR8n>wrD+yuU!oYn1vj)%XnRGxlTvtwX7nKd5}7
zO00YmQl@W*S{?#}X*v{YeQZA{Ij{48YsTM{qu?cS%$Nz1$)~jcM14376}&b>!#mE5
zi_prGAJQ%YTTXK|(rI3wl0qG(K1zbkpn+!c{o0oxA6bA#9pdwfhMl><iDZlAE?5O;
zaUhe#rZ1So*k(-Pe5fwJ{ZmS*M?hl-e##pp?h_yW6VHe<^L})I@^`iQ2zYrGy_htj
zT?l@QiRU#L+B4sS3d#f8d?j1p!^_N;#AYqUxNoaiLN!>7mYv-U_z$ELt(uZ5mTUJS
z=_|x^655sttV??cm?so=YB?p%`3&r}!lMx{?PvITRi&G(x?<&4;sSc@Pe?9CnVhj;
zan@oaR0KCbr_`%7i*DiE&L*-MNaLfo&?av=R!-^8R-{j_oT@EDxBxA1HvN=1N_?|Q
z&e62lgcg-LhbXlae#Y=O<t(t|!cCb=%$xAT1j%QS{FU(}Y~~EuTIEcwANr=;5Rnmk
z?HT<1m@>2wlbK(FDOWnR$<o$A#)Z><3zBLG$H}H<W*bo@sx!ApF-e{KO_U|FokLl2
zrNzj7ALa+W@>;6wEVS9|F8~(xtT~C{p|V7yA<SkuhX}Q+K$a#x7gJq5BxW@wRPNAj
z1p1RIiOt!EK3BO@Bfi=`{QRp*5)C<bfOK%v$5A(GAx`!*NS;+>Y~HOHc`9!mK!2jb
zKxO4rZ-e4@M8#j6m{*Gdpz`*ABa@?EXVHr|RW-Wm9Q<)Ap^ABrfS~fugXqgtC6E?#
z(u+anQKW3%o#+ge|9A@~Wy!FFQ$;|LP*jrPL+C=4_pFE7B<Unh`YDjzlS3-$w*XoB
zrCabTyQGcNKL*50x%9kIiK!S|=DkEZzJHzaS}b}g&-q8>szy?_mckrNIr~uE%5T1+
zy0%3x<2e@~*X6l6c_u!{a2v|-kiwsYiYpHtR1P2(*PPL{VDhE^5hE_qe^CFCTA0ik
zeGg3D&oJT&$eQ+Fm2E0DC!5Cwd}%F=R`M<9d}m_>?8cwa(f@=mo9BSqDu2BULy2Tr
z!C8)g-N%R}?*$|qn|B_W&53o0)1U(5s^p}f2id)<bT*IXw#xUMuxZIq=Twh_;zx>V
zDs7^$X-%;4%D;RH8Mw8+j^sBL3HyhiQgNqGQBfZNTZ9OghRvtFlS+&BI|!{!&7fI1
zjWApz=XtwBOmDKF>)B=n$MzO$2bP`Rhwi8^;kcJR{_=Z=Pkwjl`FBB;`f`qQ-S*9`
zv%fyobHNUxYh41v-*@4^qO~jwX+x*nhN-2T<`-=H?8UFOs3;7>v)YP&0Xa%+WKktH
za#m|?wDPLu@e&?K4zMV(kseEIR9uOTX7O4ZIk2@nUc%!gHY%sYMg`RoOB$%^fp{Nb
zgY9m3khIrRVxs}Q)<%<2EsvM*c!`a4P+}veuf*1f-^N-S4SFT=m)fG%b~eY*ZxfAY
z)+<4HHU1W?M^$*F0wdF2YNT>9faYpysi~UvW&r=J2zf*iF4zF#{YYZvGys>=OwmMB
zpY_{_{TP3oIghA3aAkmB_&ZF9+PjD?x&!*rE~jcQdWnWj9%YNKLFq*s32m(c?V=_O
zh(#O8hU!_%edw<FyU5zzLC`IE9z4e-UB2`f(5Fvd^a})N{-!FZ+PVSsY5Z{^DYB>=
zqr3I7YY=7S<V%&4JG$%vl%~xmQSx`HwYwiiS6Yq~b{`@aYnT5M1YF@`S}i45_nb(t
za5Js$2OKz@S~0-1dI20wtr+TKT7!U2q*kN>H411)8fnPa^b*gvn-T#o5a7O)s}2^Y
zEfU1L)2{6(R9h^-eQDP(Nuw<SJb+ZxL0cm79!R;qgnp$h72u(i>ta~Db}^Zrb<fc)
zu1lc-%|&)<-LreMtDRCkycPD`mU7jiQ8b_6d?@XD9!;(JkBiEs-K77UDqW-vwut{C
z*?lgw*BT?0;%X(GKZHoz=OrQe_wT}0_h>y(*EWw9rh-}9ewj}Gbvanbbmo0jCXmC+
zkw>>M<|b2Gqr0dK=H|aByJL{bE<%ytf`+_%WqJtR967Z{6jo}b2PMVDnVeB2zzmZ$
z_bVo^5|gzueIh39+oXIyQ9>Wxf^xJbk9H}Q7}OTAcBQ2^6b;pQmK5(0Dz*Y$QC5y?
z<U|_T(LgCMu<kjGU@f$TV4Zc(j<m<Jg_<jc&}QAUk3!v&WC*5fc!Qu(GA8%m$YXpc
z&Dz>!I{D<+<)t4+dPt_z(qL~kowfa3CXnO(-`NDwhC<?f2q5tR1d#ZEkoXWu$b^J8
zlJFgqgc{7rTy>JLL@J#mJUcI)B&?R{B;nF*I%|u{1X{6#ObP9yGGS05(E{z)N#w^B
z5~kJuIf=Z(gk1UW63GQcR_6Vp#Ox5ph00-(Lwgt0TP9x*$SPp!c^uXROx=gWlKzhh
zwlH5a`uU{&blbvrE0(R3{)M9cdElCv;`~bqQj?p%lOQ#%mHScbC4E5cTNTG|sr9gP
z15kJ>^EiHr)=<ahOCE~w3A!FL1py_tZOBk!qrs!&ZVZ_W^_n_{5dEr3n_E$Bw^q!o
z=sBZG)6sh^)0nklsr@3mzZw;-VRrk)NJkMe3J(wK89Qd#5(<cFQnhBSu-hq<1`t!A
z;?M%JQVACOs|hSq{GlXG^H)=j$!9Zvbv+Wx=OKc{(GoCJ>?(_Lxa;%5eHM(M%94Y3
zm`iN}uj)E&#SFow$-ZnxgqJj>7zNN)^;C+$iOCYhEBw`2!ZQjbVEsjfE}KC@&&W^|
z=Vm>#Py)1`iw@X3rq4DNI4fJ7*<94wR3Xw?^9-uaoJ@9mz;4cXRyKj;F!RNtF~hZ3
za6Nk#>fJIuU?-mEh^$nAQqZ{qMMWqy_Q=dlNEzpv%QTlrikW9l%oT`||G}J`EqHzg
z4Rs1MZsat>LlefhY{o21W#<DSty3T_$U#7~3(W|4OS&}Dx~bCG>M>HY2C`jJFI=p7
zfSc;HmDNtWFp>quT=Qfm&4h;l#VT!K9Sop(hLc<RA|e#xVP_#z0dxu{T8%2(BFL*W
z$0XPE&$e>hlG$2gHDT7;*FoT=#jLS<798%2cm$YbW==U-m)o_TGl=iSOBt0r9nlq8
z9a&H{SGN8Hy3K`A&Q>bLf{xxccD;uF-^h*4bL;hZJbU_{R%G$21Z+HuDCJuzq8Puj
z1+18{3_9{Rvz67<IG6AUDzJ(oRC>T%6VYut1S$?CQfBr5#z704ieF9pb459R`4I{j
zr?kK0JAl!xy~86zcun^2$TJr2A4*4}>Rm(eGNVG`C4OU3vwWj}AfB%8i=|MydfySR
zR@RRr!Lt{5J&`lbVaAvgzp<`=WQ4rl4!rc)I}r8t#v?d0V4!XgXlO9=u4`RPAm|mv
zL{r@$W-NL+GZNj1I2Iu(ye*o{pkoO{VSsqiQAO!>QV<9({&*z1bqH_iDujHKRDs!I
z^W;zj^pbCC6fc6}{o6z&-JfC9(VH5L^=C`sguU@prf5tb7PSVjzQVL3i*=ob2_bdT
zI@M|2^!iq7)%DiI^^fkbmf4nBuX$wq9eeKBv2**LZ|}M5?L7lm?b&JFdi`f?=h)63
zxazVZtMz*A8QZEKBL9x-Y|AX6s-0KZR@nkYMb?=%2e2`VTeB>#+PPk0t2p**V66ks
zSZ6--gBPq(+a(qUj5O#C8*Y!MwX3JoW3;>d0k6mKceZ=oA%Cks;1923L=p0LbO*df
zDC`dRgpBS6<_m?5?yiu>+s@N_LjKN`M$o&mC*ThHL+-G@tCQhPSbB$cb1G%vrPg7F
zEGWd=?(g(^%<mUBE*_`X?90;0!Nh2)FTIf$<L_MU4){HWJGio^!`m4S3E_PH_ORjc
zhP}ZKe<v?n)`Y*_I+W<&WRM`H7ksHson3~{8w&Z|okpnJ+vacgL#zUoZRE%`?qDa?
zv0x3BWGpKyxalqLMch2LHa5U^;>mDqIGV)k<#_8ko6j3sV8kMLR~<i*7()v{^LWh|
zF9)aS<y`UB@~C)gnO@q3O8A?{c#S)p@I*KEriW5o&`Y}wcOd9>d)A>H{b4^87DC0k
z14f6}<L~KU8%Lwjj_7b=bQ`Dlb#-)wySmq*alPG2Z={BvPIoKXJlutLb$g6fZ)cmY
z!yR13bGcW;RCxLtf7oXP{GB~(jjqmsFc)ty*cCLXirD$l^lKptH#b$zP@fTxZpnS!
zJEP0SX1qP#nI7(oj=D!jd$)PJyJNiY&>D9)XQ$YZLbi;?Qc<pwT;biF?!dZ`*SNGh
z*oFFZut=gak!sH>0q;Ra{PARTROrvU#-As7chKJ%4tQ6414f(A-MNyicjHj+K#~hX
zN|tgM;Y*o;_p|9-0A>vLj&8!M=$r_R66y-{P`98?6byq0G@&{g)Ewwq;|;P<Pg@&o
zvb`q&rH6Zhoou_X9dmy%y%67k=9hyQP8Wbhsdw_dyx5>O1kH<H&Eo0dU=)>&4oBmu
zP%1hiL=!z<nT)$F>|d=icXX}xng<5X6GsK~1ib;js&m`AHnjdqo`!*>t*bK>4!WT+
zMW8AvMw4iqPIB;GjiU}?sTd}*Ts8D`cS8-34eidyj=mvdBsFT#K+lWF*tF>9N|`u7
zVmKD>CDn%$Ls9gzenH>a6*kt9`OyGIV}|PFP;a-9Q@iY#W_0!h0-<hqTkc5&vL7$x
z0||O?e?}rz*Sryscg}V`42~ZTmPFDcLopl(5XDIbqK9NHsi%{R=Jj;K9CG_dW|qo1
zT%kWPBKkA-9F2}1XtmW#BeXv(o72}7=E{QobwSC6M)~}vg*lU4rY0?5COzm|xz#q>
z(fI@2%os@|Y25MoSNb?3jJTlW<4ot8P>^x;1fxSSez=D^bRl~xM|OBz(i@40k-#g@
zEa=^w?d)B6;jzOV$ab-In$5_vM6XJ2=^YWdg~@ldhGC#Iny={!u0m+)CD{%|;&o?E
z#9$lKLqlwU4_Exa1YRSmOUQqjmqkVsBi^eqk0A$6bx3Vv!q9_o*WlQ2;zGSc=mp9!
zNCu3xU7cP=!(IXtCNXVIq~no1rtJ^~Q*$&4PmCHUZ~n#zXBys!i9a6Akxd<P!so}n
z-u_ML5rfXG;M`<~<`Zfgbr<T5m$l^-N0N%60%GUn!jRJySWQFZc#B_zinS$%ys`Yr
z4|5(H8QhjcZx|Yi#s^Y^qFw=vxS8=JBunH;J>4F6*xTU_2mKf@)er|`3%gs%2f+v(
z1E)mJxS5?<nnp1JZjNS6s*ur%^J8Mv9O4WUDU_I8J#cd{p9q&BJ6H1BW3nk!|7Nlw
z+1btYqA@D#f*Aa?jTpwqYof-8sBLB{UK8|(y+%9SG!{#GV!gt$f?gV`bEd>3<xa7*
zm|*j|F%>9BdK&b4I#+det?A4Wxp84=+wgFd+*&aMwe-c{qDOm2$GI+O&Ir!aLC4H$
zQO!fmG%m@5Z(v6_>?b|cI~q&!k=mR=j;+n)oj^r2qmM>=Bf>0I(Gxel;SNFR@adpQ
zX@BJJiLOYsH?@#^HF*QDA2;ix-j$s&hI_Zt89bMyheudubcP;yxukT;gjt&!51tHW
zlf35X>h^Yu*1<rrHO1ZJ%e=uZ?sRe5yfKz_xYuGthKI4z2zWbJhQ&xZZm`as54PvE
zjTlKgy4%beHMt2NcG!;`L~?qy>Qk)=%%|d99u^^kGmMSH!d1d}&b=S*0%CIM^tgi_
zq5q()EOimHsY_pk3Jj6kPW6^Jll5c={)PQ0d{p)s7&+08o3#BNv;-|x%wtZLM`J_Q
z#l!SLWs9!T)e|<l+6BK5$G7{}N>EK=xh^P8XC7~B&q|z91wVy453y>8x0i_Ed@9qJ
zr+R?&7Q_^vYoa6xd?&9v7$9fe<AwE5Rpi{!HVDlpH*JI0*4v+gDXU(Cz9F1ThAuKh
zYa7*D;41NYWSn)w-P*;yP4j@qNz_#C<ztH&#?<<%9fL!cTE`jf0k?4PGFB<qiI@ba
z{pr9yXWIoAh<7WTwZo}&Ji2@!8joV|0N6X)Ke${R2I#CEaMpG$z+aQI7Og@fO6|sU
zyr28bK;i7I{!O(bqlr|Ms>~(j{x?^6&e~42W7cJA^K}Khp;hY`y@Jm(nr3$~Uno<{
z&zkR|<{jw8nW?oRZ*QtLe8bJqcU^v8B<AE7c>F;xylHs+Qe7DV&1Rx}%G8Ii3i&Zv
zrtawddu{@$jivgWEt>4Yin|AN-~o$AqZl$x9tQeEC%G+DaXFP3=pyE^NOgu5?($)v
zLXXDq98YcJwwokxj+bB>1TtrYeb_l}f4{9Th6w<^7EV^<Q^0n)wBj|RCdXo$W*IfZ
zn0mWuuq}K%8=atqIbHdK7*>LbE!mB(oS{S71i4EfD(o&zXl2rA7cN0|=vAg4r&XYN
z63(s!rEgw%`GB$B<9Dy@!fM*cEdJGg9*dJZPa?q;6=SPupwAyRvQxFxDc_(-;M}%e
zbou4NF!-@_vWLkfFx>8TICW;bG7a*LCNCdn%8}OwUy}<3*}ZEqUSsV|zJZ#X<T}<9
z<m;iFM&?sYh?eTK-_^O!pVK}W%Pf3qA9lMUIBAZ8CJ`(UL9+-J@}k5ZiNDk2;bMov
z)6*7a(pK7gN4fW(gcs-L3lG%Ed`2LK|5SQ3j??wh(segG%R#SjhuYlarD7Y7R#`@3
z<3{wsoa&gAxBw*tuwIppv6!@^r<=8<jKAGVi~K-Hbp6~t!VXm3)IX$2H#P{HrsWMa
z5}qmG6@zQ&iN@LXjEBxul%2)0`yxVjd3z9@UEJto6RLeC>_K>gA&kMw_G!8fc{|`P
zV8YJ&s!Fyd^SVj89Au#?JA`Z>!iF@h=ajY1KaUqGdYjQ6?CLPBi^LKz+l;X!_T8|X
z#kf<WnmLr}O{J5vr3>k7QT~W4WrMTpBYz0fu%|2Q1JHSdd~D^C<<3L}H%a+W!nZ?A
z6Q-&EVy`E&s#C*`@Kp<My7DbsCkA)&maqcnatNo*wD2%BfSMgLo2T2yZ3r_Q0y@BX
znl+TQ+!m!<>ENl^T6sB{9SZ6SbiQFK)}LKx@zscdn-Dm76dU~5*vCGS(ID&zMwGkP
zL^|O1#&<K@x`M%;Zndq%?Iq7z)q<KTpQA{OODh9itr$@|u*>6K={4YycK3w&@(z6~
z9PHtn0sav7!1)#^mx!-=v1;Jk-llef7t>|hx-6jJ#wI@h%I#ulMp(gPC(PZm78`W#
z;5y82l4oH}H_{l9l_;|*i+q_y3KugwQJRkS<S8EiYNK_X!52m};AB_)S+znFUF~8R
zW=cy&63baXd|x_=v(VsT+Fv(qrqg{0I^7ccywR<+4N6^*PgdApkR!cZ4b$2iCxDKM
zWwp>ucHC2np}fk#_=>YzxX;SLqeTT(P7#?08)fT_v?dED)b1DBBD3J+vZ<XVa;^9_
z9&O7SbT5M^)0P(od(5|&V6P8GP3wKWNjfl^*peM`a&6VSw$1DHV1mb@Q?537ze?lm
z+JSg9!aEqY+HgmKuk;c_5z~Y*?s(Y`IV=~85^l-nrW&`c(AbozHymlGfgd$$&SG+{
z^3Q*jr=-gDl85hfZH)FNW0>i%13%tG7791pWE^=1!U}z5#(YEhE;-Dy!wzl13iE~J
z6wl0^EThx@!QRnA7O;dH0PPN|j+k?ktphGZQr-B<n_Cx9S#S;cE+OB;fF1R26`Lw@
z<1xp{<FkXfLqbbLXht<=#vTivW4tjoihFpWbl+82j)fCqs*+yW_%S+0;}kc~>~0Ic
zcf`$rR4a$By!9o&>&Wx9dvO;;Z4$%rBDa#Su~q*Z-=xj7DS=U+_ObZQ11cx8s?M&d
zp)u}Vu<Rhgi&QPFI(KHL*{r2(i<>vJ4C)+Mep88;c6YU2LcRtc5NJsKD<+fKcZ5(P
zo<l4h5?V+IM~Rn^alfeqv*%337j!^WmBfUcF-V%3DNmcbQ%p(nuC3R|Y@`%U#vKv(
zQ<+7JSwT`{+NT$Thd07fgR$IBSl~u)Ki>_e{a-4Q54fQouE=m#cwINPI<v-s2}Lc`
zvHeN(VyU6#c#OIpZwRYy>_KtI9UI|kj0AFc$n0U8tY5i)3-fLYqDPuXk8Ja|C^Le^
zAu1M&>dbD2se|UGjYElExY+z2VH4v+4#Q?gcJ&8O(vLe<*x1PoU9pW=u8fYNo5A$}
zqx7iJkE6hc;|%@>-PVRHMK@2q;x1N>c(N9Y#Evjz2Y>VZ0UGY9E5S<KZ9chWot<jc
z-J+nkE9mhCvEk3}(J5b7d8TCSi<)8X+__XsW89jixfR|%hI_gxow<)7ck2pIpTRD)
zkZJQzJ$`yur3=r};3++ZM{e&?(B~vpV__8?t}+uU=xT}8SdWU{nnS3biTr8~Rzcs$
zW>-=5(GwvMm(BE}#C|9%_R%f(DCpZ6v3peX{TxCC&6E$DLTnYiP@+&SI4Y<iOQ@pm
z96|+ovxF+znM0_cV3trtpU5Fp&?{L&6@4p*P(dfMgqdho+o+5tq2jEzRkR|9P(h9?
zp^9$EAyiN#OQ@pP<Pa)odzMf|O?Cnqm7SQHA6Qe%^Qd~fTjecdnP<1h(W<Ao{}x(z
zg^Tm=mf;#1s;7Z>N%56*`EsczjaJBf>Lr1R=viHI+c5SA5T-^&IH+QnEuy0%tS*i%
z**cv2UYo)_00H4~usVKxeQhkl>IfR_O%AfU$hJ86i4fifPuYw|A!uYX0|wGY;kZx+
z85_Y<EOp!ktV_`ilsb%?F{CB<x%+j|L1W`+?{L%@j3BRqz_h>Lh;HqVj-(8}%^`Rg
zz4X|K;GyDI^&=z7>m82uBVPhV;HQq4E6Tvp{;aM)ftz2LC+i~7zVrYx_r?d{Qc5ru
z-<VMG-oC!k=w=1chPH}BgDMKYT}36Q{3iQ<fbsY<Jwrzi%<-$m;*uc`GT%uOkf2p@
z3eUwKp61Tv4_jP-=zBywU``Qae(}6r&eaIWoIho8@${6;U&Ep7Pkw$6l75dfu9W<H
zEiO*XU-&^R!XZ4SEkiu9opW^p!b6%F`I*%v((#ZWT|OD&F?Fsdl^@S@@@qAIiA0sJ
z&Jpub23+dgFaeF%A3c;#XLhOl>YOeQ9wDWxT?85V6`cw<A)U?uQ?NP*Y`4rmBJ-0t
zB&U*JqS-ZwkWN*8b?z8f3h)h*FkgSF{x{_1SLc{DkWz7}bI&OMWc;b}RsP$7Bm0x;
z%AEUll!$TRt%012WHn#(=)O5WD|d<HauM*eghiD9;!U5N@J>MH{A$!|z*GKoy_#2`
z$}dtG`y3+X{OX*#Db<3aR1wNBj~&|wJmpvVSKsMz%KW#;d@4}-Q}BL7%=!1r*M8J@
zzLqIM8RnP&t-SolWPUmbi>_tF+;ruae+X&j^66M*x@z#^5?%8|konDH$DTt9sakfk
z%(;H*yJ7kI&M*JJkQc8BX7a0Z|Bm2|V7l_@^Wir^Lo%uI>BMllj{c4Le+ydGw<w=F
z_fCD+O?}5r*|#cR)$0`Un9En+4LODPG3iqD0?NV)|2`7T`PI3D%qep0R~cof@~N-{
zna%n4J2!C3{Z5fV$tj?0`3jzl%*0FS?~u%Y&?zWXKUehm{HkR64T?a9D*pv}c#@Sa
z6`m>c3vBlHHK&NXvM3%?`4sxR3G$zNwxHNe6Qt?N&%bbj{13>Ew==K9<>zmkApat_
zDD)vUFi08l^LNSo`TBdiO=P&)DKqBfS9~s$`E#l8P<%vSDpbSf<yZ943G!b*ASgx@
z;RN}sfX^#}Q=XLhlS<GD@>6o2ia6!5QIX*mY3OqSF2SF&du5k2J{Kx~<YOZLH$J5O
z1XbZ*`W{h>#WgO`3W=4UWKOg~66OEo;)^opmFD6xt6E)V6pvdmA}K$_oM?rorTmLr
zyp1WpAr}wr*j;86k6UpppPJ{*iFgw*Gk@peF`27**_2?#_vF-kYeGW{<G&9_={P_z
z^BucfdOh<!%UpaFQ}bCa-huhaWk&J1l~rfvA9JD=r}(RRB^N&x^TY--ipQ;NT4o+F
zCtBGVOpWikcvz!zgBiu+)&k%8qXa9wA~nvMGg;|n#sZghoEI;MtV~XFd3mgCP98?l
z%SkCO)ykZC7)5{f1o(5t;i(@Nv3E}ACy;MoG1&>t^>h&j4SG;-eyV=m0(>DpZJd65
zdvya({Ey1jFhG7q@k@B!;Svb>yH(=z&p{S`?3wHt*^X2ny8a1&S^@ic3V7nL&Q~T{
zx(?${D`1Bc)r<TO9FxbKdG;*)g7NjGG79OZ6as5>s&Jkip2hLwzfXCA#2?6`Z{ZcM
z9@k&|60go}COPQ3obwsqUua%&7O?*j;2k1s&hM8w9_=OlS2ZVmOXBl)EhhRt=o=I8
zF9kzqPKg4TJ^}th;0wvw&hg{Nx4;DSn<n6YGw_AJ>v`7%^mGDgq3@D@2lzsAzBmD%
z&MTqsfbNt1g8B(vbn;yxKJ^pedndqe1D?K<N}mfdUpG!b|FEP#l1KmD3Fu#(0Iy-5
zC;jA?YXiPey_ytW?v5#0Lkh1DGNjLd72<OT@P)qf`ejZ({(Gzc%JBu}+1y9&<Rbxj
zH@nsSy1-)j7?hXwkK$9MX?&=tACEL#(uQ{~=mvqovL8=1Qo}|+eLgjbM}@>^R!t~;
z>@i^s3?=&T3~2;UUL}p*^j3zQFT7xyibm=dEm=$tE)^i+-;NYJYIx%+g*VB>u3RKN
zJiHBAOb~y_B8Sw#V>0dzuYqS>=@YPAav7IZ?`m(yogjLYA1_9M0^fkmP)CvpV~}1K
z#dBb}yd${?JmZ0zk%nhor@O=7#?0Tf%-;Pp-CO>!XQbuFK(k-Ylm%p&{q<X^Iroza
zzr*vld>z;H7Ng6hKEFwy+?3z*%>QI*mWaO5`PV<LoBJtH`VOzMn%uAO=6obHTmQ^s
zaT9$gHuHRsNtb`^c7Nd~DNVQUG9`Y<lL%QERU3(K)|x-#nx)Bb{jdj5`P+sjaTN3X
z`eztU^tD_0Raw)`?M#*l9x)vMp<9>B^j+56ry5`b^3$z_jj%EQxeD<rR<7xYr*3$2
F{a-yIDrEow

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/example1a b/3rdparty/qpOASES/qpOASES-3.0/bin/example1a
new file mode 100755
index 0000000000000000000000000000000000000000..b28dd75d6ef4dfdeaee86c16ddc039298e38467e
GIT binary patch
literal 28799
zcmeHwd3;;Nwf|hnixkJojh)3I2`Iq{0pc}|<Aj7rw&X~}wuB^SfrJ~!l5GXsaxFOv
zk4H@sAf^c<UDG}uHLvg<{gf8kQXX4NC|g^)1lm$4l%^~tp@pWD(zGmj-!qGJCEG2(
z_vz>N&*LW6%$zyr%$YN1&Mf!Ny&w0sG+Q){smsDHWRzOvaB-0MWo*l(JSkxf%+6eF
zAv>ED0FsYC7tf(k#8h}nvR0u@;#m>q;jh6W01XxwkEn2t05cUT2`QxVGM7t6MR?Ct
zkyIgLP7WYClJ$FYc@p6k9#P@FQjViCRCJUiy5o}WxTI5|F7Z{U>PK~>zlh9_SX@X^
z+C>5L@ZU^GxO4?yO+_j0)Oix2i$_$rQh=EX72U<4BRN0pCE2r(EN>#cWZFk?R_##L
z8{b-ASJfM>=#BRe?X1|@P+w7BSD8#y*7AB2f5KhU*3Jo)9?Os-nWy28Y=H6~{hRT-
zchCO8=0BWx@YPLs-f+{}BR9N4c@F#$T_OGs>cciRZ(6Y}FRyrC9&=M(3I1l`Z=1d8
znfqV<<du!ypa1^cx%=l#Uh%o2+O5yOamU)1kNomSFW>y^$2QE__VF)VdEbFQIwnth
z>E_pe`Ul4@|K<09bi%6z!V~}8s}uldE)ePY6Tt7EfX|O7fY(pJCo}<k6!7unynh1t
zYZJhGCxDkuAm=R;z>lE8bhOHVa|<|Tu0H`^XJ@d(Iq+i~K8p?Kz@GqoD)1Ft^{elK
zzjGY^j{{!9iqn2emHc(Ur?M&Rt|=}KQvKye62Dr)W0K%CDW@t|@p&F_;y+v@_<S4m
zbp3+!DPsF3xp-Rr+#>mpO8yd^eTBmv>{ba^_*ZfM=>L+xf;059j|~LeVQ-i^*OJO=
zW*FUliGCxQ>KIHJ1~V?&ZiHgp@nkAC*wovROvaMT@JCvWXlzh|BdtxniT+rmV{30r
z<c*(mN!v16R>Q@?!341T>f-(JlsC9G8m={R{)ghhC^IrN%fpJuQ{&xEIcpWW<tnGK
zW?6SE6&#HBb@YZ4y+f&ZqTf3ao2D9XJl+;IYHED3WHR2-AK5h!^NwF+Lr)^bHS+gm
z9Y><&5Gb&9+q#Zk2n}r~Q_;@z&nIQqld5;_GzMaW$wYrgZ#=cj*j`OI-JP9Al2B@x
z(cZr!-XBF1B!~K9l-a3h2L|K)sV<|l=W+(7iJ=tuQNC=tzK(c5Gh)$>R0lG)CX)gV
zP=AzF4J8N3$5wTu(54kzJCZRGT~XDO=!;dwlRfe7p}|B|XGiMtc)zhDS=HGSk0u7=
zRa=MRz0s<k!GYwiWYv<v=m6|wSCwp76;{BKh<|cLRbn7jHJC_Np%(G(1OfkX8PVAG
zSTCWfDs?7@j4s#;%qxI;L?O~pG6wrFsD7$uqWXkOX2sStH5p{lO&d128~sL2<r220
z#oyRu)K=D3E=|XmRMw@tgN2V8=_Dfg7-#68RfZxZe)Bl&Q@l?z<^*<d;jSX0`IYRD
zgqJYo|MbqwF}=uRucq@Y>@Jy4r4F4Dx6{aV68SX7rDjpu!rqhkr_S0!Xnf8fK|HPe
z3C&&P6otdfq~D`44#lJ|rwMN6v&aNjzKn>fO>p!4sKEqRK9ulXCO9gPzI-P5q%;(I
zGr^(2^tHhRpPYs=w#5XWZG!ih;Ke4mZ_n>-zE9-GraPJMnj@+F<4cg~+w-XH5fc)k
zr<aC+a(Y1ozf;a~Ax7yQLOOBmG_C~~QJPeF;wVoyQkqnF;s{T#ptP0Jcky&BrAd`1
zMtS-?N|Opt9OUWwl%}cGiD8~TgVNMoCkA-Bgwmwi6I*zC5~WF{CxSd}p){%TgbQiZ
z(GGxbB%kOWIT*f=Sko+leTRhfUH$tY3V&;gKGzMQd<PyZcOldF#NKk7v=8JhsGvN+
z^bBn2a*n^Ld@&GKO+%t<%AzL`uP8$tVLnZ~-kLJ?rgE}0L~boF6Om2j14wQv--+J`
z%ZDKbN85)8c)URTXhOzG4vO5n%WvlqeZe<GPO97myrp+2yu$+Tj`C4Nz~uy}uPm>|
z@BVV9DE6lEMJiIQA`K$4zx*yie|P!4BC@~yh=}Yh{~jV#`J)KwLb=5rUame@h70Bh
zIrf)7%MqsDU4BeNZYn=6BGK|7gy}8ca=h#Vw95fcxvS;C{_;`ewj9`7PWAL1Xel4?
z9r$$lFi+gg73UlAl-mfsI@36wa+=qoRX3IEzLC80Cp_hgsmYK3=zYO@3$Z>12(c!)
zeMnOkiTe>A-p$pL<{tbI+&|8_-(7x^GZ2<?d<6%}#IN2veHzL?`ki|*-+-R>m!AUe
zLr^lM_HxR5p(=L#r@&N29{(*Ru6~p(vF6C7k0`ir<dNe~Qr5_?kDrZVYTodVJaw6G
z<R#ypS55{a{wI#mbjSC^!!)b%J@EuhmX2Q!g15V-oV6FFFm~^C&MmCnS22vhx960@
zH}dKZ%jQR@k$J~|d-Fq-b^6$pvxW(FFQpXz!>6pk_l<nl_wehhecDmqQ>TVNOmkfj
zKb?Aevv1^aLI0AZA6|9KR43f_)OJuFzY0BY^W*t90Hpot5%M3tJ*%E|03mf|T7+rs
z4;{lQ5BR5#Za(oZAenL&JCPKjd%b()w0oU@<Q;drf8^)xNXvl*!(h<vJ5aF?VYubo
zS8Ja4J^a3vJlgRE??PMGypcLn)xTxr^_G!$Jm7J9+6%rtk7&N-zZ-g;9Nndx-J9H-
z-IuwIN4mNy75<+dLDk4NN>`}y6%k{Tb_NDg&Q;D!oeN@%@#oxpfzz4b(-=S(#3q@Y
zH#@nSRK(aZ8J?0MJHo+inoERCh9@5q>0=LRv#l4<T9drg!M~k8O+F!rN|EhuK}elx
zF8a>~ggR729)<km7BVqh(?r0uD?&`$scW+*7upVLg*w6K;P2Zoqw^#V1>@s0R*pQv
zXOQ6tg$}*hQFhUkq8+wjw)%_<&RbGGpTI=#!QWxvO(B51I4$DZ6~PuRNuiad+EyVC
zV~deTWek9h=Jt6vO?6?Fsqe9@a~zvwz1}j(p*J~fZZ-B%8F%3CB;=1FWQ`8}MvK=`
zc7xUHa309>Iu`BE_c^MsE$}%S_7t`{T)mD4x1-waSk&lnHag0H-{`P$dEW*(`5+XV
zb)7z~&Ddi_AAHezr*>OD`%FIK_tMWt*GC%oNCO{f;3Exuq=An#@R0^S(!fU=_(%i)
zr!{~TH(di*)X;UzAwr(Ok6ez832E<>E|pJvrRFs4ebS}YQtCeLWC2d!_b8TWbzg(-
z%h9Fouc|#jU4U_CM_kX!`>ASuW|Qzge{ecM(Zd!O1?hXfqxm9D>rsALT|7|BU7J;a
z)jF+AB-wNssz9PKhV2Eq)Y?mJvZ-|_ZS>JKQv__-?&2{856ZNXyISJW1|VIkd@S#H
zeU8~&9F8S2Pb)c2%Jl6rpCkQ$_)+(<bBlU>l8b|$k>PJ;_>K$<q@B%>;X)bK%dlC7
z>txs^!`yOk3t)wFQTx`R{?w4OuCk%BuA+8`r)sXMsjIB6t6Xxffb(14qM2-*i!ya(
znz^^;a*KKtHju}GHmwQvo2TK%X$fv5yK*osx*fScs{`0-E8IH+fo6-r0<4z89WZQb
zUf~D_7Ibr9em(9A=jG+&ZVw3Z_aJ5Y2^v0s5A6(CE`;sp@1-cXT>`><TY>EmCAETn
zFt8+GYz5U{0;KR8$bPo)&u62awi4Jz;X9WB%xsUso(tdo1VwFkK<vWzD6ZQsgQSJ;
zQ@qT!8-1|w1ByFsUjyI5(-dE1r(2PQOru1#{bC4HsPRODoo?b5T6n@`KT70zJmIsq
z!oCahc_L_EiOyD7z!MwnF_OxrT?(dK?DwD>7f#YTkm#}NXqLjs8VNjL|2F05Jdv`G
zqE>|^S}$^T+MlGH8CoL}!}c5D4+>{$K_qtDzmDI+GOZij_Sjbw%^Yn968r48QeuI&
z0|*E0XAn)LHiE=q`#efi@x-Wo5hbd5;&yu#C2Dx$E_*#CYPEAgbFcmCA|&d#Y)9-q
zQb@h_uPEnHHt8u6;R}V|07hBiC{@yy2iH<~3q_f2AF1^}sJb@W->?r;_@7i;ofX*5
z1FXPCZCFH2OJYynjR--hH97Qvf|dgMDI|&sx0rZ1-b58`*M1&yPKl$$JliK927>+y
z08CH8LNqpIHd(Okp;MH>H~4fL1l9Cqz_IAJ!MLsZ6;pwu?*KesZ-8|auqjdS$g{Y<
zi)Pe+0quWWzZQ+i^mD*Y(^ml1qCWtDRj&Z&Jbfn2B47UxvP%5~AcguA{Mz(8VIg+?
zUBoBp+fi7NeioEBS?`4C#kvFd4&4o&Q*<|Qbp29@GF2}}i6weFdTFWtSD45&eLr%i
z>tBEw&(LoGpPBkkKvkxnM7?L}mx1RQ`gahYt^W(+bM#-JFU{4z38~K1hXFrJABFr*
z{mbA#PoIVOeEqYK?QDHE+NWH90G)e*z7XxRP=6Nu&(Rk`wnh4ODBxWEw<z~K{ab*X
zuRjf`7VGaJUZIyj&Px3{XroFmhCJ2!6_B<@UkMJi`a#IBL_Y_d>-0FNmg=X_&Fl5c
zp|@rFL*Ut<mjJR{_oI{*`g4%>0(~!JSgD@}nOEsIgZ4sw5^A(szXP0)p!)*!Fw?(+
zX8JDEe*;(a2-9bxCXX_GGkV}-OkV-n9%uS>NIk*yTfqB!OkW6vf1l}Z!x#O4>0blz
zNv7vR&L1-UY+(L~-<o7UX8QZ!`4gr;4ILe2`lC?TQ%oNO%F|3=2lAgXy%o~^jOpKj
zs6S`=^T>OK=`mFI7fk;=wDBy{EvVRYO#d#l`%9+(7cie^`f4zHf$1l}`d3W955bE}
zzY~i4HPg>TjgB$>QqcW|>AwT;C8ocQ+P=*6>rtaum_8XLyvlSJP=3qwFo3Ty{c%|8
zaa0o`ok0Ci`s<Jd%K9A&f~J4Z^hN}4FufN-yvg)gXo^2T1>kZLtWl#sGX2X4-okD)
zQ2xa9#VF`)q#(qfnLdCL-eLNCp!*BcS0QzZ=^+&KSEj!Jg1<5S9jM@4rVpVG?=k)N
z2;OJ<a}eSKhy)d!hCTrt)^so0;c8946ZX7Y)1N>)?9ueM(DHjV{VDLgM$<Q=0j|~b
zeyHa<O<xF|?bGynls=;A%OU6Wn*JJ6pU`v*==N*+eAMrNrYBL*4VwN21iVqxZ$Rpx
z#-`2z{UZCagDAe_c2vlI_g8_yEG2IO!Tv2u*eoSKLj~;r%{`N)<Uz{0mlD$~C6^+w
z-$#iuOUW0A!Tpq&V=4I|(L6v2r=^sNv9HmV1GXA})4oZC2<Syx0FX-*bUIPUtTt^M
zz(eV*`#9BX;uX@a0r;TGn#QSSd<qq}w^yLv4*^xX9ogShDAPIhj1$nXeba?dJ~=(@
zNn}5hMiE?kplEw1`DBjrCbIuZD35{X^e0J%8CSx(?D41Z%TcC+-5mU7p}<P)1H0h^
zh^tnI?A2)$QP5O0mwo%2K;$Uvk-aqsWf`gT$~vf=qmTo^V{4)yHe)?C#qiA#iRT_h
z&dr(JTS<cJ&VUhdA@4!X!<0+P7iymI8dY_F8K4}6Z1R;Hlx?85AG!^i<tXnVdorvg
z!$q>{gvYXfZa@0X?WEXp;8YPF*P5trFQMG>H2Na3vaCUNt764wbfdG_Z~GdA=D9KC
z45(bucr#xFnf;FG5SkcT_EL6+KP#pI$?|Pv+YbCEDp~v)Ae8+VI?S7h%(@kAG3mdx
zB+|SpPXI{$ZrV4X<mpdQ!=7;|syFGYng_BFvG$tE2cskKqpKnOOtPV(8(u_`^C<#s
z+OJ5dzxc;LccjZ=We<VFL1O5Ec}?E77+sn(+(Zmt{>Qv{aVCbdUw}Q>ikB15i=l-%
z&mjFa)k`a-tm0ML1IT)q*yo`aF#&aHPXY3Rf=-`AS#t-_lEog4cxivc&wDE0)YppF
zTn|(o_}Vm-cNYFQW5MFg`;bu_dJ<O7&C{Y=IJXN3Z3gnV<p`y5?XzOaKdTUGDRWHM
z)&Z{-F5XE$1tkT(IR*1IZH|U)QK@-^soemyFA+;}GF-T6^ND#CewaY{I<g-gi^AqV
zhURt5)*e8E618U08I`KNgrDO?u#}k0?<F*+_Oj|q7M%;H*&$d3{y5t7v?YovQJuL}
zi4Jl$sa24zB$8}tF*j+4dFqZUsj@V=oD)D=)U%wP?Q|4q)Uj-qd4#C#1=@|o=K`>u
z?j|uyDZ??WeG%Z>R2G|i7-r?zqY+>2e*8S9vP45JD22`)*Luj>(Lx;UH$Zt!p|OQ?
zpdQEd1?Ur07_h7yi?m;iziDfz^o2vj=LQe@FYh@PJ&$8m0HZ-+2~{k73%$(o={F!Q
z_Z1dBpQCO7+GPrrEqos8c6@#|%ttaT;8<4yV?<#|hF^kpJHGfL<dCR^9QDgUyDtk>
z;*Y`yIBxs8@&p#$#_^v6?w_*pd7~0j;UE@12}kJo>P_gdysKhm$0<v}ssevp7rYQ*
z4kpUSsCB;e4s<CgCvnPFP;Jkq<V3u4a~n$YkiuUB6+4b3;7BB+BF^ZuVDixa6C*Cs
zlgm+I$!IcX^lLDgiE4_va0O&c`|rv&otl#^<N`jo7Dg-i7IVJKz;oOGiZ5IESu(-b
z)?-F0Svok&>%s0b#FF;{k`1V2HYX3k#K@_@TBmT-ZvpKARXSUEAI2ibpYP8w)H&Ak
z!1%4gnoi3rHsd+;1jl=WAmG;e0kZSJS(yDf%U}wQ)7MhRC<F9N${}gkq923WVbL~G
zccrzX0JCE9FkB<&dV^s$rFIlz+vbEu4(Drktvt_xZZu^%hduYNzx?yjx4yIDJOeF0
zWfh0H_J3>V+&iax&Mzap#vQPN(Z&A`kt|DTsaJd(>7c-N7{AuMJbHrPYRmIMgaR8G
zQ-O`f$O;=x_$qk3fX54LG}SAxkqQfJR8oPB2F(H+jd+zrO1`KHfWINEwk?L{PH+<P
z*%dY#Z@?Du0v<20Q8f!}i=c`ETP1#LDs1G}3gj=TyTWz>htO{=xh(6YK<vlglE=~Q
zdE_zF@G!MQF=@4~f*NDGX5EX#s6ylsg}7u3kZ(g4D<&UXOoKr!jc(R^5qlDUoH>su
z8dgl>;nE*t%+vmeSp5%RK3b5fT~EpoQMT;+=quJP0$Uxhhx#jsZ5NqI6>GQ@k`5HP
zkcpPzzaYFR0KDZ#!E;PjaK)<tpFVxTFA<=#tlS0$+9KjUEZL}s!nK<Uk++JNzJ*EW
zg<A06QRZqN(<&&#dT4aVYB$p=XY$0s?W?<)RwWXn+gJB)Wm>gJ-L-x75K=WFb%gRj
zS4#_b>!G7V$V769NFEz<eHCJ9^`eL)J6xnBZJD4wy503}FeR-)B#&)(T>_D`<sx}<
zyK5%tZG}h<?{Ha&_5#8A<dAC<np|5clEd3wU&kP&xyaV6hmP%V1))E!iR<~$4~JZb
zpmuGoV0CbZn_B3#Vi#!xM~1|uwLXZ(Yor}Gx|)g|E*?3Ant8N#t;VB8>{fbcO<Z1^
zWN-{*K%ioQqSbn|iwQZT)wAYKiSrj!WjXYo!6S_W6Tx=iS`QsWu)%uhE(9B`hmLIb
zSaxuQBD8_^5JKIOWC+T%YS#R)Wb8wQKbgb$h+uq7Fg_|6pA?LTcL>HOi6IydQ$c3N
z&q&5!gT&v+VSI20Ykpbg)4cb|Og{6TA@z041YEOJ0%g6SR&|Q&UZGZ02~%O3Yg6M)
z-i?%1q^V_jS`+DIDvhVl?tn8dxSbYQzEYS_!IvrOcY#aV4f-RLQV!7HLn-C@iv1|&
zqALL!UAqszcI!ds7J%?9<O}$*t|z<U(_)HnCtd|tLkVn>fuMB+X8jj9u<0X!!L+7o
z3mm0IR>y2d`^>4Dj>&{&27364q6>=rFb``vD=NAW`EdL)3ipsd1yHc0fUpWnHLIhj
zh=?>KZN+I04M4NYWWK+Yl9LpFs87@Ur9?4#9`l!0A+vZPB5-rd!BDZAl9$C@Uj*)R
z&@HA~vfw4=QftAhv{G9=OR%XeS~)AqOPZFC0_xylr&9z&Tn4G%;V;b~&KxfTvvVqR
z<t!3<RvIHeoAiwFGBC}}$UB$If=XCXW<yckERuI_nyjd$Xs$_=Gcy@fP)kv3naakR
zA<Z>kaCOc>#Tv?5iiqbtK}rQE4a`q-<6@h+QLWAv*(8CY!akt635oB5G?Su!M=pzn
z=FDt)sf2UP*%`SOrNNL(fU(6`he2Ez<H}ic;4;nwKw75&oSy}NXp7AN479p5(#q-5
z*s3t4VAW*1q<xIe<Vb5PwKb*AB4H#;@_ECMnbZ*+0_3Z_rIj#%x>-(c>Ggyt#KYEQ
zx&r7FPBbc2wn3mz)k+FoWuI&0u;p{Lno@#nDB1{tSLCyr(mAxm;1NJpnmJ{Wt}4>n
zXA<8FS1>BK4AIpY9a&H{SEl}ybejt!%0?>1f{xxavRgy{uK~<+{cb!ATXv`s1YQ9q
z)MkZ2i8Uet&y^`)^{kc9k-v_uDW%4_h(}O?wG^S!Tg){P-KJH5`k_Q3W_#cPEx2_8
zXQaLBD8?^8LQBK%Kk;}@Y-dN`KrfDS{S$e{`#XDwqA_(0NSrfLKpf&U>g(i*&hGx9
zs;%)9N>@jbaO$aQ3<~bq;p7l!n#GJUDSk~yW#_;E&6rwoimRhL=IiK>;?QV!We>nm
zA)OXV)K>OjETVWc)`d9SeF~?&l4)=}fhY_PXO2{qj)MU~aPjv?V>^3s3QhsiN6vBq
zDxE&ArT|Te{$y$pr?PNBEfF2+OtWh3NDan2Gi7naj{a1-a10n0HDR>=glTyeYfQrs
zkb28H(`k*rveR0++uFC=cAUb?H#|6E9olW1_ke9)%kZHaui0kZwfoR#z1uA1rM7wa
zw`$MuA=|v}ZJR7@Q}=ALT#O&v+B~auwygv_yDbZ~?|%h`s%ar_#Bh5&je+(ykJ0S*
zw|G5<zpdHp4*MJZE&j+xMi^m#Yp}&@gd^@qd)Nq8GhaAj1Os7@x0&a+hy879jF5Lt
zdy6~d54$7&KpVrMGdixeJ(V(WeymS0^7)%1hQ}N6hFbk?98Z>NK9g6oY>XZ|!rsu@
za5c^<CVLWtsjWj@oS(mKox8>FG2Edw?XBLnNSGfU>rHfCZcy2#<8xGvwt(UDhQoe$
zn-LCroBYjwuV=ja)o|c?cc_i31<iPy{cS)spHxg6OGYx&X~lZp-aBimVGRArNW3qW
z#Hm6YL|mSx^M;og@hFZ(;wOqjZw4K@OVOb>aT;(?oCc)BaHuUm(dRSzV>`HP7Y7Y@
zOUUc?Y(#7MBYr3$3<0|aW3kp)Ut(|<$MOYQ1Cc;*BO1;dRC*>ww70n%(Vme2+RW`S
z8og~zzE*c=EvIs?gBkGr_5O&@Xz{nTZ!iLFEy5JMp->=X(h0HiLqi)N0XHR8PVZJO
zZPppibmeR6aAdM=sBdd*&^<WVvCA6_#(Cl4_3j{Nr`S;acMQf;F|L7Z;oWWSmW^Sr
zad9vdKz&+SG|`qwHD{E7<AhOve=;_h;>^A4{W+2cL;ki%i+7#3#c1-m+tyGcclCC3
zCpkW;R?1;SF3xKHbkF}#r$+la1~116P|g%h7Y?+vQx~BQ6^fwDaC=h|470ht1q~8u
z54EwYgaL7ikT(n^i_WQBBF_-LRvCi3DdJz}l@zUkbzXC)=3sAIxIN@;@vAyDZEQm8
zuHkub0!@Loa3ti0MifG}C!oH(wbvL(4I1QFIa%8JL|;}FGW8VM6_$5IqmhIs-qG!i
za>}+q#Mns2L#~DTF7!ani@LT4jV#$S-o|KaZ)ph!-A()qZ~7>%EMqC}@91cqY1vdw
za(q+Hkz2EoDtV(r1HEw^jg8?<uQ2kAF|@aF`MjPsv>Mq(T9V8$<*Y-&orwX_C9wfy
zw6>$CG<wN_`XjQ%e1Qnp9ZVt+436uBb6XD<NphK5Z5&HzNB79CyU~pP+Y)5PKtdQK
zydp5Ti)dRP4y6won~r6t`z^Ji=(b*YPPb!wre_9lfUDKrlCke*8dg|WZ=xf`k~=yE
z1gR)C&=`TKk%L_y2(3kEGV-pW-d@(7_I4y&*nf$z?ePqiD}sb4>mj!mX!A1iFbRx1
z=H{tc8s62{7o)LQbg+i4aZDs)9fM=sbDUf=d1g{_&JYPlWAZZU+&CY5wsv%0J~Uv^
zX=_d?`xqbdnv{)$9Z4yAEJV)2M0XFgtRqikvMXto^{7-+qSp)IvIi54A#9*$R}vQ6
z+Z*fePW1@6Tj1Ez-8w8w<XP=Ok2~URbw@&e_#Wk6Z~`0Ti7a22ZvC3tK@2Y2V;Kt=
z&ql;~|2XMi9Ez8ch~dhE=>dGT=<Atrh*ueoe7s86kvYg-Xs!%-kjyx4@B`s-oUy+m
zW(<fbriZ!pA%DbcG-G<h;z@EsJU`?mH<>luB#Ao3hQ#2N)4!-d>1D|!wYRNp3#@N5
z>q<6L9Sy{gm<F%GSVvTtgEF;=)o-}N&;jNtsF<_?n#15oeoD9xlfvBVXy8K|x>?#)
z!MHhdswm}PWG01SWQKpuQAr@^Z4(+nKiiq&Q@TsMp#YymuvYg5xFU>>Ym63e+nR`v
zQ;+dd*^W^5lq}O$hSiuz^v1;d7g&@Uz%aV{#IOtp#79~_-GCQt^SDDEp@0xC*2~+`
z-1KrVqyob<r6f5e4811(&WN<-F6~6&gVKi3=!8FJi1B-%C7SY@J6>kOR#TmsPQd$7
z*}`-J?GYo;Eck^vyxG4&CY86%p6+mE6LB*$`Vvv$DY6@$*kf?^x<f5A$Mbm6TqGa2
zDKVd5Fs)vi=PE11$jpmR&*uzxV}MUE%>9<5sG&lrR`f<SnQn%S2h=p)Xl`+f8A7`8
z<uU;djGB$!zvFB=|9tU?rL&?hHPjzl)g9}P!O0@oG1%F&O1#_0S<&sR2rR*0t+N8k
zB+pyXHPqip@9;<hgx7C$US2UUm`KGsQwdQS)94^vmTG9x0GUBKd$Q6Wi($TD^3sH^
zX8&CEBs``O+o83}oKb<Rh(;ZcKjg)<3R5*%=_X&G#T#CmZ6j*>k^0B;eEu#HQ(u4p
z=?N|$Gw4t*FP?Xa+)}9$((M?H?Q2+i8p^~{hjUBPn9P}(`of2P9m_v&ryyMoEKkU-
zDVJs%)-uzj3Et|0eiKUUVCj{3R^K!Yia;8FP$Z3w+mdqnB*uV@Z!ue!oED8gV}sZ7
z_}yy)Sc4epB@^qX85EU?AzpX~>6W<psY#w}){M8O##W2C1YQ=?0qb8yGqGk2$L0aX
z<c?}gGb+60hQ?b=4S3;@&AS0>Y^*S77*SqS&i*|iKE=#x9_|dov>>4se%nSrt)4rg
zcsUIPwIWy|f;tf_<qX7n(BEd7W?(huX>W>P5Own@++Zv<G}w=~;|$3uk$FNA@`~ZE
z$xV|2tT5;eLq?*jD;a}lGn-1<nb#~qRa>yTAm<@sel8~#nU+k?X&Pxg(-Icb$Ly_)
z)>?-9VO+|qa(%#6=>mC0Gfp+LS`!+`Syi<Kc*9Hes+BQTRo+k-&O{j)%^<?wRt!2A
z1TtfmwCS2!^AvOJ5`-x$S#&NhNSLwF9166Wrj=&>DaO^JOVQMrtmdEB-K*4Qql(CF
zyLb{?5?JUnJ_cvCsgkKa*IUa9V<P0Ep&CPtCy*KE=<P<_-|?zv_wI4`mbeq(>q^sT
zV(OS!5~pX~%3+9U&-i;)d=JWrT|O=V-$cOsvb>?J4J~3M<`|}V<5gpJwiw>4)y;XC
zTPxGg8<hU!oI{OaY6MTO?x_3FTar@o&dgL8DZJ<l+ogDy7j`DGBT4CMVVr4+)v}#L
z=Elo6bD9F7P<v3VS@?LAWBO`wLlw+YARN$|mOvw1V=Gq3?loQmlcZpKgwMdyA0wf5
zzCiPbv6;trfVf0_HG+vJ-z7A)4~F(Y&d!b4CUCcJz%Hdbv=PIL#2;Ua+-GD;OYb*<
zE<K0kB4oxX(;LHhj>o^wXxwP<IWjq*%qlCRGSo8AEPN?b8ZeSrP52G*t}c^9Q^s%J
zBQ$LY()KC6|0+(q(yX8Fm2o-TSkp;&Ef*uS^uasA2F0pIXeQ$am13xImGSTuMo>)q
z)Rcziuv9sPlv!5rVyT<aA{_3zjqhC0W|Ki%tvs97{_yJ<_b$Tr25ga5Ql@Pp<p;eR
zn!H{QhEyyt<jR6~fT8|myt_XZ<vk(2SHz4&Z&Zy2{GDLj3ve?sH}sfwJ^ZKIzky$e
zJK=8&lW#B^&j&3PlQ&ZXh|Z9iU#j_U7Imhd(teMy36z#KK(Tb)I(s_Aq&8<1!Z4du
zTiQ8UONbX{H)1OzJy)cueph@DJM1K8&eo8e)Q)vEaq4cIBD0X>JHT9_#7XwRoFxFi
zFTm-Vz3aUpwO@pZ0!??!4Iz!Xj+S_ESm?O&7D9S0lUd22Vtj~YnN4zDhElXL-1MFk
zl{9v3S4W#`>h1v93YJ7vm5YOc#*1jc<;G6lKTFc=mP>DGKqwK<LYCE{HxG(|lb4X5
z|4|9%DI66)?ue|aBnFVQU8s?y$=xPgsl1WmHPUNRVmUS&TcMa#uvAYRZ)`OAmT~T&
zbn;yz+9x7b+)0Gnxq>2r$i^UcM>4GpC#Y6a*fAk|F_%?i4m^d&8^+oQ%XB_t#D>@q
ze={UsP077$Q)?+-w=`#m9$m#OcBV}llySg_5DjL%?vcUS@i!mxt@>KVyDor3gGMLb
z2-(-qFtXA8r4}rqG0pOd4d$$(q(t>%J4ntq$SLt{M0AQKpWLL%cz3mN8}bH19&d=o
zTbNsY9j(EK`pH>iqKqy|_iNG{fO5BCe19AYKz*jo3-yrG0{I-yBs{|=pJ4hTZW?D@
zR~NZ>`k)LyCqwmMkK&=8;K?FJLfvirxTI5$piP9&DwDhVFg-_@8WdrViot+mg9EIx
zA1n7tyoj+Pg&iJ|!cE{x{yxcyc$8IAs;48_!z!b@`oT|xaArJbJ8qI-ev*|ikT)3X
z?H~df8|Y23O774rQ?zwe$zK~;i94?JBFxHIkI^;Q(HAp%qM%hNFzxI#Vmmuy11W>A
zG6WB!gKj_x9xA@A6ND(Qqc7eGx&(^APbDu`l!13`vdYc`_L4CPtc=FC4t0aLqrV&X
z+GR4{-<44Dj;&h<W7}1V_B~V_8dOn?r79{p<u=*>Dvje}bnBe%lk=;=;*uc`((lI>
zDS@lv6rPPgJo1{Rk62tt(R-+Qz?>pT|Kh%T)>VU)nLcH4@%)seFXySuPcFR&S?}O%
zvy%U?#l?~NGopw^c?yqb$q-L!WnFDZ;o*$5{LJbS`FL1>u2IPt52kTFDSAA|!LN1r
zB~(@ZXugZXM`eNPyf_EqhjeYe=rJ@ptF7qO`Rv<eK@AdF$*=HKcscUv?6*p*^WVoL
z{eYw=`AJSCzXY=@5Fwo^dUf7BXcrjO5>b_}=vDpq=FqG2>%)>>op&et$@t5qzX34P
z3CTy7de8AbJUT-c9#zh|NLKSTf`pm=u1PMQeV3#^!c&<aJnzRZ6@D5iGrjVA17sAq
zYI6!ybRw6rFCb#3SMMjdb&9}{YEbB0*XaZzqxd6wrGNDv>ANKT0P@V2(w~Cej)<Aw
zsk?ZVQwq9LA<8hPI_w_6&GcI&{T4+}nWihZ{3FOS)6;A1=(-6H<<gbgemU%LP83fz
z;!k$B^!pst`{Hx;om>8kpe0o)diB1CXXgTlD;GaE{W@?+CPhzgW~A%bhot`la8=(T
zI`zH-^*(>~-hO4@s(e+iQ=l=Iuio!_>MY_mvHXh9`@k{NtM^SXCsAV@7eV@0(W$Ti
z#Af;d=N67T;1ncEPLUcb&two2FQq?vohDtwP9CJ+Pa#q`(xf6XoGQz&&VeiX^K;-p
z=a&l4kn}ld>?=+IaphzvIt4y=0{Y)M1;+RAptAYOrC&M${Wt3c)t;OVmrLI?0e$@i
zg8n+?7^D!n^Z`kqtG{pa(*ZaXMgzR*QuHdkMADm3coYxx@N02ShN4s8*aY-DCH;Ux
zlwmGCxd~-EX4K=7KAA(1ljOFWqax1cSX6L3Mw1I%3-Grbf6DIF_(lGCyz-CqA_Lb$
zIhncT3#{>f>5btH7T1_eE4;m$Cz&&?kVMUYvf+8@_ta#=F{)Z!W)P2C;gQrl#GGkm
zg-p#avf(zS<_+0!J5%$4Z1|+~dup=bW4}{C`BselYFy67FHWyBvf*RDUm*BozO#^x
zuj6eEE;ES7t#}i<8qds`R#t-XX*~SW^!SsFKaHtzB^y2+<HQy-h{vsLMtU4DXIk-=
zALZY(;jl*OH?!emzjHzPR?~MYvhi~|yUetXnZ=6?E0cqqnF-=nc4iJl;mbiOC)diH
zIS_?EZvy!IF>vZvdF+p;b2Dgcx3G+z$Y~K@cWBUqI_shOHTCiF_;>)<@Rp|B_PSW{
zmuqSy`PGGADt9yozDvS$-+y%@;3UsJ*^X2nx^BjwhPUwM+RtNv6MvRIf<)N#`V7J!
zYljon>m<jY$`0q)^9LM0w!Q$5tDi{_SfkN8#}1twKKA=5izR$#4*qgp@zOE%y-32<
z`?^RDx;Aq@WBW@N;LdTr!}2k}={qT-Qg6!lUoGLe`xE;(T$E-0{R-etGeE}aP5I-&
zC;Fbwap0F=2CL@%ishReu2Pw>7y^&S|6IVw`;JV*1o$DqOEPTJv0lK(`##Ptj$e#3
z9xjd>lN2Kp@VRFK_)DCB=DRtn;5R40pKm33#(tOQ9Kgque+$PSYj52WKUgH>SLN=T
zfX^u4<9*-fZjL|pJ3I7#nG);-sP(7fuO6wDw<{;OZKL*`Tb9YY@?O?Ci0{P=;Z5tE
z*a*C+2~SnhW~0F}U(GR6eMTpJHz$c_b;S2|Odx#NBw=*-Cbr@!o+xhkCykDwoeY~w
zII)q6MJwx<FQYr|;~??xq=?NbJQkh8<KAM!D>~HIw+keu6u%Xig=*j~vAflan+6{G
z_79g_#^pV`Kyx$phm8n-2pkyr_)i);noJlybo>I3C1mrCW&`-AbeQ4U*ye8aH?i@*
z;lelW)Tdoc``4N0Lb4vC{V?AxGClO~a;a~@(Dz^D$7OQApOZnN561lc&mLuemxjKO
zq?$GR`$(qS6zL~)#21bJ-nVSBcEZz7yiD+5jA?T{^VOt@K6hmPG*Cvi5B*3GfAoIb
q0zcH#@9O(RV?TuCa+$u{ll@FKnnixv2mK8nKFV$;aWj=~0sjxaxXAJV

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/example1b b/3rdparty/qpOASES/qpOASES-3.0/bin/example1b
new file mode 100755
index 0000000000000000000000000000000000000000..f1a19d4c515cc9a87fab518fe5968868794398b2
GIT binary patch
literal 31765
zcmeHwdwg5PmH%AHc4W(zZ{#F6kbnqIngD(!j^hN<NVa85iETNOoCh@A$d+u2*p`vx
zJSYtz0ZM>CLfJM@mIli1LbtRjrIhy9E#XlfeFi9nzO$ut6WVT5Ud^^Ze%~_>=}MN{
ze)iMf=kxo6n^-gF%sDe>&YYP!GxyHDS9`lUESkpDWnmv?1l?EZ<|y&Y*zp(#C9Z{)
zGB;bm&SNFO6ywj$Qz#TM6`qn=tI#IttO$$n*J2Tf7K@ulR5(YVnF^JJ6jFKF%PpfK
zoL4146*A`H2;w7IpPkD=gqwLpg~y~Ej0EGCi~kau_>N1y<C0H>y5v)#svp&j{=zan
zY;gl3*i8Xz=fByIXzA*GEghx!uIU^^=;jd>UM$c|g^F)2_(;yb_tGU{P?k59Ub5{Y
z7*spdkHq_%o9aiRwIlJdiS4!9TbgT|o9a@Dy2ZTSlt0mS_VjT^rN?SOB=ZdXk*X<u
z;{Hee^84v;K6=F+$E&th?PAv~XK6~KrX;>;_}j1Vvaxe#RM?7&DsCuZ9!fg{e`n+G
z^3t{+ef`zju4?tZ^t(lKZ=O@W;#2m;{XhNv=hwV?<R{;I_3l@$*f8hvtM9z(Yy1A_
zD4+4GyI+6t4~`xFEsub8%Iix=aTZ9u3Y98^ADM#wS5wfF{H_A|&zl0@3q=$vH#!Br
zW{Pr~r^r7z1^zq07b^FkrojJc3jDXHkmsQ(@N1{QFPoyi>tHZCo6QcoHba8!wHNd{
zJCi+|hu_Nav)TQ5_)Wl12mN7*SN(Z|mD+1^{vQF}$tp7Ar7F1-`01>Y>9YT-emWrO
z_v>y>s)p5%NjdZ7xdM2~e{+SPZwE}*D9^{vs>|IRRzJ&R{!ErD$(e`a9jr*=75xmJ
zKVuCte}!l0-&c$WJRxt0I`xve24)yTqlqyim5wCShQW;0Ta93BD4t5kl5HcAR4SHY
zhCkeGL}N*b4tKYWB*tRlNdHJoq!muNq-U8dYgu4C9Z!s<8kfb!QWMEo8{$;L*uW0&
zR%T2ljD)<wHK7KGlTBN`wl|qTk)y5cjk#Rel&1JtJnijW6Adjk#Q$KtH%hszQL<!-
z8<!2m(!I&}Xk;Xm7?~gmy@Gf$ZOd>X%~h1mT;)8OxUJC}?+F==jlNha6_1RCcZ|op
zg{#uIB*kSIOLxV##zwrv)&Q9lZ01s0%&)$}qgbxY0##j(ng;qWUl$nx32Y#hjt*RK
z0hvHE8Nl}K#&|55N{mHD;^`g6)&`;(8W=EAMAFENzOilbu_z29H8C2a!~w-SPO=Uf
z1H)SwGEGdRksqZ?gBp#*$Cwd|M$!=^^run+57bzc)la07G~?7q(y+AJ{zxh&qATi$
z6Qi;EcxpI4G?7fy4@A;i;$z0PRQ<qkJeo+x>-#6-BhmWd<alaFs(wi_Iu7gIQ7_F+
zg|)D6%0IQDJ~5uIPbO0Js6~7zLCC*dMwD8FNL7^vQWM4?EDS~;Ks}-mX(APac^Onc
zRWnh2q9tQwoo#If^|Q7O8$7Lkqp@xY>+JHkwi%1-7S}D!#Fx}HWjd*dSh7xwDWw?m
zFa5L1P(b3hh~pNa@6$Z(!e8%#9d`1pQWz)bBD!Kz`io!O0=KJ(eKeD9VI4A^=qJvM
zm(q-WJ<=)9^lXAH>^4b%>YU9)mhl%kfFCu^l212FArV%)HgkyPeoO_K%Vol&_{_D?
zgqKC~v<4Gijlq=AV#1sKNw*14Wvk0)!mBxf(t1sJR5){OFyYHHSjIM+@D(QfunF%l
z;m1w*N)vv&39o!PO5SC{OBa%J-C)AII7a;JH{s3myl>C%Y`&X{U#fC3-*rdQ#m5Ig
z@7wc~?MV|eqNkTW0g}`6pTckDIc~%V9wwp_$4=v#|8;_?2~Qm5@IwSs)15fN;m;9l
zCHOFh?<JU;>ck-q-$^hv(TV*WzKvjVbx-W#@QnmhQ=J&+@NR;siB4?h@RbBp)12t#
zaFXCMg54axoM6~KJJG=5euAmVPPjOH3BlA<Cv*-638p4G!8p8{U}~BZC*MDP8amqu
zgl}&#<@4nJ(AP=SZ&H1I*Zgij>h(sYKKE(V*SGH*HM^h}-!s?N97Hl*NSpr<r2%({
zV;CE(T=?c2mDBI4*)Iqx?%@ROHJgb5DfiXfFH&l+MhXxgsac5MXiWnZMI9DUS4|6L
zJ|UoVO)sF82@dH?$yothF(gt)YFs=QH_yd~-*n9e{9aWv%<<#+?W);+{20|2wK$C8
zkAGPL`%zWjzK1yywP=u?7tvls2}1NJ0*W5rWa2u0?7h>cZ+f^!<RFxD{2Rb^?dz(U
z)3vX?#_dC_!M875<Dw!+Az3XJ)c|DAwAaj`#y$QfX9Pq%<HWPsw-5DJ4Bp1)GPGX>
zEoHb1j4J*33mhrM=mUU;IQq?M_)k#P%{4cG_A#mhplfS(A@V4+$BwU&Y+NZ{Fz4}~
z@1m4zo+2A)JaXxizP(Ro%6giT_Wts?q9<mgA0Gq%Z-3nQd;i|&FZ1pFm2c0lPxglW
z&m1A|&iBmY<V5<Od4_z$<F6u%w+8n=`bF1f*5|9eAAxVrDTi<GYuhZFo_rJ;@{au0
zrpGAh^s&lwb`kDTf)xGZr>vm&?R~=c`0J~D+EL&0rzXJsbt3d#_d@!uO}@QP3;th8
z{#`4t&|Mh*r}u&L_)heQO-~nJ29);ZlQbUq_N?4q0fO||84+gmJ$4MMXwaWNy6MD6
z!BTk+JCPEhXT4|dY0o<U-gi8G{=F}G!d?63yO2SjZ(r>~grTlQuQmSE_xO8O8ljH&
z{RN7<?)T}lE6=%>q-0%tU+>!cPCN2AJ>zG-Jx^-B<-eVHod&K;H+eqd+2py*V>~%H
zSf}XUd=gcou}%(c8eefTR<<3UsB5L`QrG<0h4^!Ay2#~9aK{MP`LQxGvdndIm53gb
z;VBuiBLY9eEkY*4laC4b*kjsR){AH@Pou<<Kc7BLW_1*mA`3W%@HoO6bdn7SY5b{%
z$?Zb8nM@BCt)uDEt_(75yRMy8KFzjYo2C<f4*s5k1)Wa>@+wS^GLTO64Uj>nyd8Rn
zqk46veVc6;TXp6|=P#+5M_CYmJN~L6n|VGWUb?g^d%L(K)2y_lunKt?yAWwq#!2K)
zYnY;Uoo?oF=zA<{9fwM-MV2y$-sZ4*XdtGG%J@9~PD1_|AzB^!troAN`WCC#;o4W^
zbu7HO*ym`tzQpHf*)y%%;U00ccpMEL$HG>JtJP5r`c{XH%lj7CtD!>_n{%B$t<C$X
zhN<PaR(xj>yW8@z_D~Uf&q~0JMZiBuKObBlXy5}4e4v33H1L52KG47i8u&m1A86qJ
zKMicgQjo4*INfx)CCmZ*$mPPMkoNfKQt7lOXohKzk1plw-y-31fzI52P<vVG{=#gT
zuI{g%C1G8LXUgyuc|TRHA8Zo;_WP$36g_BhQ;@mmTU{bxS}*a->gIu3j@@k)Xtj>0
z79g7`LlsCe#<2XQOIC~Rl8R94B((=f_ZsOsSmx%T!VgPW$=$$H8SamZOO=o12d~em
zQa4Y)5{|=4j*}98M5asj|L1|akDXu8I(c7dP=;G&c)biiDZ_u2;UhA9T82N8;csO4
zjtomG+`On+GF%|TW*K(KFkc>Qj<0Yn?CYNxOHa6(>RRfWY8Ov%sBvdwQ(Z$--I7HD
z&u@*3<`j*hx<sYSGyhgveo>!-?G|yQO?%CO_-r)hFJQRid6brJSY7cCn}D|3raccf
zt7dx{mTk36>z|9DXxfiBvgF4cSzLo1?xLb%+y?<e@eV+iufy<5_RtE~a`!oiUQ1Er
zb_W=XZ6&q?1ZpM6kbxxzBb?UoSzxA}M)E7Rx6ea8ZOyPD+dG#5&1{2^*Y=m2C~CVC
zHg9{E;<~Nej`({NueOb%Dz^72?y|+uFKwqOzOeLLlo`_qXej*>gt2KHXeoUdox*0}
zfV=c4u@`Z`S2`UkwiR=rxAZgUl(rHMY$)AGQrWaik?H2rCvcnIR;EP&443|vc*->r
zc)aunl%jJWUHW^}%I4HYkg~nhP7=)0S^?}TeY^_58CoxZ-KC$wudP}eLT-CXcM{JW
zZ5x0aO8-c}d~F*D_LsI1Po1_Gz`@eh1k`ijQ0ZC%8aQx&=>`HCIdHhtAYie!2t1FL
zeglJ=t%=Kar1X9gr&*)>v`?|JUyulQP76cb)wV-aN!$1A__>Fo%yuTJ^<Jv3&DKd;
z`B$o~&Pr_O16N|BHndaIlGx?D5g{zKCdckm*eYN@0ic3tD<}`gMX-d@>t|wos{AAd
z;iA&p9tVLwiv}$Ha#)DQDwkn&Dk}Z-8<cP#DzQ^P2AyB6--f1RdL8Ue(~qM~EqWJ(
zv$D#i7^aF!A6tYr(_chAn7$Njn!XHCi~c9{2CM!bD4|Gy2>4=s0)}70D(^)#i%MU<
zgxH=zPFL$L8pric{A&6}lxERKIrA2viy$%7TV!$9KtBBpDD-OmO7Jq>gYq?f2zgud
z2Y|5ZYax4)PERou>jzP*61@VH)AS|4+w?n;b*X+1FlG9)D8Q~?j@-)i!)X5sy$JLU
z{ZiyvsmDkM`rVLyx?TrcbLxXmJXxYQqXskd!$_T}-++!XOFxEu&d^VQt6HB6NoMQc
zM4o5rzlE01(tnEh9Q_GYY_9%2NOiXUAn@nt-+~M-{ddU!Tzw_t^YlwG&YY+3Lo?Or
z2N0jHr?88(KtCV9AJXSQwuO2r6tGDDA<8{p{{b);=zoD!7wWT6daXVOa@Of~*jBy%
zAz&KxuVMsh)DJ?!#rj7f!xH^Dq&DeSf@`V%-)PWg{W0imnf@U1Y|-gf+H(E#Xp$BB
zdyw`b{dtsru|5qluhi3!>ce^;YP3o}ft-&p{d36fai(uX+dlyln9kUfO#g4tJ;n5M
zVLRVqdIRKqn(6BSJ;U^F)ZkgB$Dr`%nEpOSyKgi7N+7=j+ku?lMJ|B8$8QF*?=$@m
z$nytGw;=5()2krn^Gt6=W-p-m!TuuHA>9v|Zim<}G5z0>_9LeEqPj0LeJ%uih3O~3
z_hY92C*=GI)89g+e#-RS(EiVuJ{wv8oaraf3cq0bb5PtbnSLqS`4~ikLjIHK#Sr&b
zOn)2|dzI<CP@`Wn{Wz@nHKx0P{0-CnK>iogPr*`;GyVGz=>*e1iPB$Z`n^!rZ<+oM
zH2pgWkKp%A{~Cn&Z>G;fQ~Uwl4F#QK`UGn9N2cG7;0>mSQKL683Zp7-K~4zqHZnsA
z?=XEnf<G~R5Wy*?Uk$_iAEqw?!=IV{G*s{xrk{t}zRUElAb1Zw9W{EN=|>^;X{LV~
z$X%L#E%LlZ(_e%=@7DBBK=XSveHNf=H9ZC4uhVol8sK_O?}vImuIX{;>;_F=1*!LH
zdJW{fQPXt@ag(N3!eVaL^m(Y?K20x1LAPl7IbgU|(_Mh}Yizm!aADc9H=q&c6Hs;8
z!;64n7U#D?Q1%r9Y!>Hzs6g3Q`PgT1-bE>o5-`K!yc9v%*9fS#I3K1AzD~d#kP*-S
zA;4v+qGHNAwNIl$_uy|vJryFbtF<oy^9_ZaNgOh%NBb_&FK3dLa<0oLub}o{K)<1q
zW^k@q_dyM1eJiM^fJiHY2s81=i<`;0XEmckmwn`;s4oq7S}l@WG9)6Ge?sGx4O|DU
zbCO;pUrHn%<T<mGWSEtNb(O_`hhI*TLh{Z$k|9`0+4wDIGe)^;pG5LK84^*@yF{|}
z$N1$WUqkX!c_eE|rC0qcgytkaL-MZ_37hpCRc6;wG&fKEGg8Q7<&v^lw^H^W{}^nL
z3t5en^C*>+FVsA1FKnvp<~PB|NiITiM;^&h@RuF<J~YcoE<^HI4vEZaCAxIkr*@;?
z+z-uZ*MsH&(Q&Pb`gRb>J*UwZDJ#nZNdBVAip_c$9kA?Q?tsud^%<nRpi)KSonZl6
z+2>o)K9r&5H<X;spH<L=WHC^u(tYz#$%;RLVETnX9<{ev!R7n{s$sv9W?H5>N04B@
zili;loVAe6{xJe<nsYD4RQpZ>$~9*%?A?Af0lMaVoOpH-Fhg^0JR86@1XOFzw+Yxy
za?R14&5*&qhX9x6{0Ig#`?W+?qdC7!!1W~9Ld}^%ueX1kfCkO!rgCl|pha_DLu7ji
zaBI%>sEhqZ0(_ct5&FOVCIWgj=Wfd2W&$>7&OMZJ3jv!o=g&!;TL~D}oY%oZ?E48A
z*PQoajIkdeV7un*qH5np4#O_Z`7V|72}-#^bJ|H&w^J4NYtCaBtnB|pDF-#@k3enz
zB&8hEoUc<kpQ4oeHRo#5+d%>jLkF-b`yB*4syW+WsrFBk4vuKfFOm}Pq70tWoR<*r
z&jdWHIo~4SZUVlmRZ+v)S7}F4#~<Nu#zu(81h!TC4KROH*qJoGu&QOGfDTQAftBNr
zGnhsOyMHyTGX_$v2Fdk`L~zeKf?>lR_;=N(JCWS2NZ2fTRKXr<hVFPbh#_T5HuWMz
z?dv}WcZR3#0k}7t>W5|9FMSOv;Hmc@<=?WYU&MrEkEYQ2JoPc8d@Gy!Z`45XCt$oh
z_2)==l~T7*jnAMyV^2Mf)}d6(pA?^{604Yol<GU6mU0;6tVK|$^|9MQ$@5x-Oq=jG
z;|k;@IA%A2r2LfDj*K`>H_|s`Xn4orLumO8zXX<(j3aV;h9pFJ<^5a@t<CHvfoI!b
z6y=MwpQ%CVK`?%IDrOVkVVGI@SG9Gh3NQ3E@SG&R>oR$tc@U;n{-AaMIw7mmicx2t
z4#)3NsMl7poYe3={LOg?F!>uytD>ZemD)0(R}#l<gcex0)(y-Cg`IhjlI9M=iYnSQ
z%1ax?&vun=vbBoN?I0z8Rojo`Pb(6U#o0ErYDMrROxI-Vnnkzp+`dX=_koFzsX~oh
zZ>)mS&-n#RrP?u5djY_6h`Z>g#988-Q!-D}=DY$(RO(!!)XMO44*q6*4q0;HW*nx>
zX$imtNiC9BP9|aVZb9F2oTb@dw8RY&8I`JC2I43&JV}|%JCn#<+BMPwLB@sCt^>&f
zifm?Pj22a*I&*svQ^$Gd!`cMfqr{RcEgJ7J3`M%*Dyr;57|G^*4X~(Z%^T>njuMRq
z7MtZ<BGkxLwc~Hbb5z$KkeF4J;Mk>|1@yTpiOszkJ;AX@qkOeypgk%{H01oBQRS|G
z19hVo;$)jZGN8!Vf=gk2jvGU;I4TTOR!+4C6nly4B`STvy{Mn#mJ2bnal5qWMV#sZ
zP&}-tgen%)qcF$qwHQaKO31L7ll~B7KT)J?!M~xGI_@Ze`N#}QIMts)QH&nKi<KEJ
zg>^eV(*!vr=`>C{A7o2&NG1JkKsx^Av-sux%%a;keIF2sTzcN9l&R?23*I6fKRBX%
z35#CJId28m1Am?KWpr-GS2n8g(V~}e&ga1OMlL5Wj*l+fhBBo~;m4t3$C178nWXS`
zo{<xhU4*}^US|G{jJQPK(V<V7Q8~}(Qe^UpOh#M*S=0WjS<a;9WDB@}Ki&YNmHAfi
ze7}r5fAhb}mo0deOz^+H1Y3|<I(U{P=qS_iCwc+N#ul`a*_`}7;xwqhxGFhmBgmE!
zsjya>aU5@#!=`11I;UC>ihe~kleRY4tZSek$Ge-sz^(O4B=1rr>_g<CJ5KYCaXYYg
zQVKzAA?;{5EZQ*IUHkV8npMyU!!@$V+Z$q)i!JC4wmHGQ2aC1C7oYz!x?|;Xj{EV)
ze)9IwH@>;z{7*ual`A>Uee+kg&;8=e_6y!7y4Kau%b^ROhE^?0XQI>!8>S2F!yLq~
zwW#Phlx?*Y1re0k$e2oO<UQ8fXem_7<0U*^Vp|065*w+o#6~5R*l1d<wUI|z%i|?H
zUSgvn>L@cBo$5h&1HuLy4!W>yJQEtlYi%?G)!Hat!s8`2s(6X*5l-_B{5ICwXrwEV
zztjM=wy$yw{Vt|K%z6TZ<gzU}ipJ-WU&8VaQp;43o|_&38)gfu6Ae^{KTgOaig3wh
z5HCRztDsT0f~JSXG~ZdtSKp|3ctr8A3YsC8K80COyBZYDqz!E?RlB(dsuxkV>^aC-
zG)QRcT|`DMPthP5PCaXxfTZ~wy4qbo2i@|^@G~iC?TQr8r%zuri~w!Ecsf*V{Vf36
zBjFXJNOLcSbn9cqh_VWDYbwYyy!cv_rhSM+$=@{A?s^?*E0MxpLgd5R+JA<Ct9(qm
zkP@s1PHtN@#I)K!b70rDRU`dOs}s=vZL1~#)eGp*Hg_isNLwtx!`s|Dk(0JWfJe5u
z!=$Dr0Uq7v-U!RoTF7Fo2afG<uZ7^+3R0!@!2a#-2&J}i<sEo;tNSYGQtJ}D$F_M$
zPk2;{q-ON1tqQ_zZGDT>fvc7Dat|VHdn6IL!{>pdUF$=YKH9E@sT9_>U#3&rT%1j3
z-uq+%`G=h8*bXcWsk!Sb-Bbon4T?+a`4p9XE0SJB!+85-dKCQ+oLVCaE49+|mg4Gk
z%d8(!<sI7r5^Zt2wpMFqovL2nQOP9d&m|AJqLpY0DH5;8$!*}Y9@vE-h*}}oU_EeX
zn@`(lh3d2w?Urp!!{hj+QZVvW*W~4KSmbd;<Z%?t$b&O$8;SR9$xE*4mOS2L+gRHL
zGM!vfGB%~M&VHG2heDz@oe7Ejf<i*Moi|G4lP0A9UWw%Sft7i0lbD-?dMznv)jkhK
zPR3Eb8<17NWgy_N##a9i;?wS@eeBg+;dxJcfTC-@fsrILx~`#J1e+RH*Zi6wHKJDd
zk!=@kr5)-nLQ$pGBd*OrG1hYeKSk@wPPwyA5k5E6W4t1u#8wH0z+JG6otWQe?gb9^
zI$fLZsIprfXF2-Ln6Bw)YRfEUb*!*oWcQ;OYYns8Ka6x_E~9WOiYtLdmJ|@xv?|T&
zu-l1ABXKJ-IkbSRRD#9+Dgw(?{!o{u`KySd{9NX*sz+kQ0z@!YE=PtcyUL=R-1UXX
zeNF`?RZ9-uX)bj!@~W!SR?QaKEVf@fJIYI%QH%nb&<Zmtf-x&gH0JPEWeLwHlmMrn
z3cYwX2|YVQQJl+qW}yUl&sllra#>IbE6QxKH_aw_=VsXKUG}*qQO?dLP(fYx?rN2c
zWkn{}Vv#HE7$DE;E<5FUu3)7Clm_Nya^qr~bE8_FCz445#f9w>a}!d&^D~*)#~k@t
zEHEeL%1b4D$ef&&YheZrxda+}`c3c+gfU(`o1Q~I9|&ok0&zhO0-{}LMqswqrIFUn
zl*U$%{>d81c1in~;%NX~T&Hzbx$MG7mK5`bAv0+rJOn6KX-n&108O)9+|rwgP>2W5
zEmHw>3KtrcD%&E+r)$n>?&?pqa@_K{T4NPqHrVO@z=~qlSTzS;-bFkD%*E!Ma#&Z|
zwZ1bb-w&@~RBkn*tFk(>pla@H{R#9?VL@<FVZ_-=rC89>+xG6((El5`Ew<md8&7Ii
zA7}-OS0!MgF^fvvog$5*z6{H4#N`($7}>G{teSl>bhxI8byiX1b@2!)*i8|VuE$(c
z(X9dkH3lUUGrJWdq=l8>SJVFDsK75jU5{~0`v;y?i*1jLj*sBP<v%db_}IY6L^P((
z#EC<93W@XGMst%q>^(F#QQsd=qjYsH4@Us&Cz0SbJ&w`wOmi~BUYZcUvA=F$e4N~}
zZk&IN48?qru_(@y57i9=4Hae%a@NHKg3i(<7S|19vZ7Oj(byp3@RHLwIGM_z;|WAz
zp*WGJqI9wn1R@vzSTweM1m{W>LY_KRV76GvK*vziNt{r`DbPf8Vj#oV9Z4tS1KFH8
zVPq_wDGKw6MLDI`qf9HZSfd(dh}0Wa-DMrWd%LxKw>7=n_L~2;k6U-_Kk)L<HAB}8
zU30B<$L`_(7#jNKB}G>24cfEd`18&;9()kcwc6)5ejD)nJ8c(RE}Xt+lkFw^ttl$9
zo@sLeAG9pcth1hd?j`Hxw$&CV3^3>o8=m&|)<9oRyV2qCcX`_le@}<k6Y{tEyZqse
zj3`3>?%po15ej?4eIcW_f%!sVqc;$0_jd5~zL3AC(+GMy`?@?qf5;Q|2YMJzW70X!
zt?9IZgO#HUEGWd=;qURbn@^-RE}Nvn?8)8Ka3Y!RpBUuD_<PoQy8P{iC)nB7?d=JN
zgm6B8N7!ihhP}aVe-AHP)`TCi97zmpF-Q>8AycYTPr&ebLm|JX#|ZU$+x#7Vh*hAn
zjU2h&6YQZn7OcVYjL~ETHysvk=9aL%u>oc>mI}v5V<{W}#v$D-pEtC`h(~ec7(dZC
zLkmFjIO>W6s%bhPDo)NO#mQMZI0}{U)2}%A8&0&x1|t(AX)fs6Uc=KB^m^Jiq8<HV
zKNJ>1#d^DpZg0E4ubT}fW3leoXd=0T)B6J5fpDOABO2G+tMo={=<D&cqRqnrw5zAx
zX!Z8A`MN#9HJr<{4yMA>*Zadhqs!mZx4{VXbP03u27`g1NmZ1cA5FgjvT$=#<&5+j
zW3g?yC#y5MY;3}L<DQAp{#eqJOh$Hidwb)&@X&fsFV9Y8LkiiJjHhE<CAq?TdOTel
zLtbNTZ!m!RbhBupCz0;RDgkG4qyDi}EGhKoUGLA6yf^6Y33qwddAp1@pQopjtaorE
zGL+)NkdmbwMtChVa7LI8sWW3VlH7vh(3}X35(;$nQMaH@6bvJeP+wac?60G*3knYR
z1$)>|VJGIkVLE5tfTosX5stOPden(+FE1kK4MEGI2eYw>(O?V}i;c#{(xG&0T!<lh
zxiSw=TiCx&F?R>ndCh|W&l3m9`-0vszp7u`#x}HcCr`tm(H7_lg@YbwM-k+X9I-Jv
z-0!_&f)0eoF(IMx6$LD-ueaA5WRMMQ%*ToT5o0`^G-$l$MP%$(^l7Cyyi;H_J{BQ0
zhZ7?)^r-<s-xCNM8_CpYu%hunb!e!!*T|_|cKkAW`ntM8y`DCHwmx&}UG~|fe9%B|
z9mq(e>Y6v&@s8NRhrG#Me|dCbd?bzo=rNp}7rh~CHGMr?G;ezk%ptcgWG0)OqxS=e
zanX0Np=EUUL94A^8jby7*_^&Wm@5mm7J!lqjp+GJ3)3aJOifzAO#0A|a;t50prd#7
zGGjcEqA|wj@AUDEFuH=0k10LtLqW#X6O4_-`THK!A%D|*88Y2|c_bPYz1}O&$Vawj
zJ9YqP9J@VT+3wUqGZ$G_S71HNDYY#!F1XPJP1$EOIn_HDsP=)@Fszw|`SpR|8ib}U
zmF<00m+Z?G*x<y-2pi(#80A4#4*4(fvS>0f?!5w28))&ef%Fb0`a=-j8Qd7Ir9z!Z
zs1G_bsB{d!fgUfTkuZTdlrn8iOpHbIGVOp6n5|<eI9?=0-jt3(&otsuCjPNlj%@19
z6nx6;j|^;?7&quu3OqO2@A(|tM!kwU?ImqF#gU|9M1j~jTo{=GUF&GnoZK;*QL(nf
zh&P@;A7UD0<HI{r=rkiEv9Y1_u&7rT2I0)G6OtwJq`uyEPuSb-2?zZcSJg-en+$te
z$t}U49Y;=y9J9z+UE|^;=ESYBjOG0UooD5c*XO#nX7kL*A>KU_rv?|V+K|;2GkQCG
zD@<a-PsvWuT)&v%3Te2B>{E_9X3f!Hne}@N{+fw62I?zg#<<XDX8vCv^oPAh2Yf>o
zPqoJ*!ghmRnlp0d;S_PF*@T$Y^Trb@P)<2C`u6p#=?Sdw$q~76X=umjXpG!uF%`D-
z$KmqFBFRbq5)3Aam$G1l$aZoyAvVzfDPR2pOT~+5CPpI3c#2OMW(GOPH<3#O6_Fc}
zj76ft@Kw=M8<*h;LFsTLp-E}s<YbD@PPI3+kY^paAh1vm>!<#kow-IM+uJciuAUej
zXPIdNdf?@f(uoQ4CN&=1CroR3%@gSL_K4QOAhbQree+Aa!2tL4IBniw&$>MuFiyd(
z=`^~$J)L1OWKWtkawn#pd2J(x^zPm^vqnw+%HMh?jU1+O#tqeLTN9Yr#Vc=Elnl-=
z21kW|hKYr{T-@)(EZ5WS3APLU2W4fci<nJaIz?1qh}H^JZ;3NmPu3Y<I)K8Hvd_TC
ziGGsz;BQAu(E7+cs%7(NY{>eRn8&DW(NzL{m}fdfej$$U@NbZynoV>4UYb|iy{&zn
zc!LsVIO;r<RR>(mL=^8%GL5RL2S~Ri=dDQDrkUdXMkHgdITHpBpVt%YqSZvZ7nVlV
zmNQw~Fj^_KWd~f%$UququX-5zk??*Qy3+_PzEsbGf5+>bSxp$8)&O^h&BHGzQT~UQ
zk3(X}QwzWj3?u=y&@?)_Ji;f<Sg>5kVoD-gp%=-y+Ag?2oB?pvj;1HZVk?JYV=)Xg
z03*qP;g#Ygb*|bWS8ZSk{uaAx(SkJk)DBLJ4REI(D7?ODU`y?IGLeo^mARyRfxsIl
zXYnc8JnQGR`2t<u&>FEsGC3Gx;!+lw&!rq@u^6B9<|{6MTE}v;pc=!M-3nFL<#$=i
zoE(&Pf6xo>AFC0msf=u9Yf?UN`fsmT>5Ws3re5pK^Yw*umR+ZD*MeT7z{X-R42CA}
z1lHa|E)U5nCkX?_ZysfonYVDS5<?LBDh6J>LW)~jiX28>!ar^jXBej8bKxyte4ySb
zS7KaB(m2-H)OplAVCs|EemBLd!srE0n0b|7h|V8OY|CzM<#ce_B;+=MsIc2KWg|-;
zUrgND0a97s7%dvjQ)PDHC|&u&s|}3E?S4;Z01Is+voD~w@z@x7|0EJzQZWLWhU5GW
zo1IXlPWj$N0<RT|ptG$Mjm+P1CJkJ!n6aMdfOBWI5z|oIXj&9_<J`A`&&h>??A{F+
zRk19m8C=aEa@pG+<jbd=M&=Vi2)&4Jh4pOo)6P;Pir1S_uvi33M9?IHr91<%DdO)j
z`LEcLXzyzaGid=GktFx+Qy4bW6Ui~WXl+6|;AWRM=oKzUn}<9>?61+1%18_jqBrCi
zi%A7bU@%=+oJ#jrEJCF>nYDzB*W5}g`mT`Zgt?o4-Ke6;x}`xjHVBKPMGC1HZdZG3
zjP1<$wOj?+Q9rx+A#{_s(a;m%rXm%uHjc2-;0=Z_a4CzWsW9a2##D^SFzW{@nHHN@
zJ#soHyHe~BvSS3B$Fv$#HnwmPFI4mqqazsTHmz#J0xsK3@f0@Fusy}N!=Rcml#Zk)
zQnDor>8d$@%#*Ugk@S(TfoZEfkexH>C3k#u<C5hL-vxI)`2fK8JWR8Usn=mcC$lC~
zgN5(|3-6-xJy;ipSMo$Kl5;tPLu7L5O%0&N=ge;C&PhAG3@?8i;(3}il(pCvrCaGd
zsVPo*1(|IK>H_rAymWjZyN2POzJVJCc;O^A=dme|%_5^g*b|H>cSDJE!0nB1Pqqbu
z!M<L#cf{=_&sx=Tm@1#6NQ@?(U4d4NgWcH5@pO6(czeBlVZL-ip9%;2_)^{<!lpLg
z)8rEIH7h)FzK?BcCrteT(_Ul&1vf6{Goal6m1czXH@3SxeH-91c!C=-E6F?yYr2uf
zhO9)HT~_dA764q#?DS@OBPCC1_pdWrHyV7=K?6;8y`EJoG!f_!gC|p3GE!J7`r#|m
z>m3UXDy9u{)9yIkSfE!X#_3iK?R8QY<WmfG2juuHSDLi0#cL&#V#z8rlO6ArVu)86
z7+=lx3U^a^MdYwk<rI;5uu-<o-@zG9s4XtEMP@O_WmEe?<lOMRI@)_R=;j1ZrhP38
z@|Y!8V?z%{O>1es6FQVkY|9Qgxwh)v(B}2FV_L?7POjv5znZ|CQHRE2QQpC@pN3lm
zd=-@#iK>Z$?v_F8aLl=Nn$<sP8Q!=!Qx|xYs2YDKrF>7OTnx1HO{KwDBo)VW#5dn2
zZKeuG($tc9uLKp9%M8$lau0HhBRfXWrl_zrh(z(s1jjN87#NNu3pLkr-f?KZR`r^k
zTU_0k&r|BQQ{Du)gvx^V#W&>mVjhhi*)Db$<Q`v+AH}sS?oZJAr%*pG_)6QscoKKe
zLKFR$W33TRh<QaiK9dL3I1MV?ma^Lp{LT$;UsA0cXYv+L{GK1@>+oU+Rc+gb48Cv6
z7n`axP41#iv^vKSO&dCVJD<wQEE%&4Mre$?0xa93d6BAxRZq<9{hGCuZE^F?jzN};
zwI!9fwl~nanj8b(dujapD<+fK;)75ko<l6X2YRiq@Md@k84s6AFgvGId_iwPRY}aA
z8H1#0lz7@aJ!1Zl_e{M;W=ElLGVW2p`7!H7N=h5%qJw**EIl00?Ry38t`6}1R@z~u
zlK7w)>f`zg2f`bBvDKHg2@Lvb1&ZxIq8CoQnujqmwtGWZPGS#+yTsUPp1=SgM}^ET
zvB~I_d$u(1P8>R-c@)aFdW*6i7#gBt8Kz#g%rLyu{4qF^h`{~jH|7>IKGHBO;Ir#0
zczb@_I>Kf}X2gmQV)HqfMDK!ofQ%-R#sJ>oJUYhUBhbBMxI1*m(kpH!<%lP1(JXdE
zAv^Ln@4(ZLOg#w3;c4^9E!*sLsBYT?y@6o6H;65DezQ#Zqsn_E8*gr+9h8YNn)2X$
zW5lMLznRT>xj|KURtpBuD5m|co${H--7?fzSVcF<hX9Khp5pwBf+7;Du|5^OGKWw-
zJ9uXfRzde<*;Q11JwlX&r($w0CE#bIfS@|I<1-5SY)0%p6@4~`P(eS-5~}F`<Pa*z
zmK9J%XJ>1Iq7^hZOQ@m?atIZ4ewI*0FV7)V&{&pGMNO(j(Mq)@jRBjZE&%G;S;bq%
zG7rX1qK#CzIr|nFCS|A|9{rdUNtKvimU@nMlhmbZE%jt?KD~On#gOHwC#9##pVg&y
zjAA{DFr5_Pu!>>ej3vie-59ps>hKcm+BEis1%zjg>gZ94Xl*>o>IfQ+q=s2tbjKL-
z6CoyEp0XA9<f*rt0Rw4C%;Cf!W8-+7qK>;cb!pmEuftdpM_PivguE^`Yz!tNqcLMR
z3SI>v(}4jawtXNro;LWhT;yRy==lYahl*c507jG-8I2EsFM%TPQ^(5{W#B#MtZpEI
z8zPu8>Y}m!i6JmY#)jZsNiaS(m{9RZe}6KzRYA0Isp8O}io)MiQJGVIll?DXoCc!j
zwCJHMezjQKGUNe&cc_SoU!pZ%=K;V|?iv2D#SJL?eH0gmvOjpHE9YtiWadv>+&n!k
z1*+jt_9vge9ZB!t9Xm??gBCX@=BFMIi*g9h3d<0W4d+}vfbb+>Mt){>i*!7QMpv~A
z@su>zlj6rigZx^DUm{WEtM?dH69X>w-XZ~sJXD-QdM29Q8Kn5tdx9Rp=@PnHMUatS
z(W!6?(&-&U3RdqSIwkqXB|nKnaw_>Hnq7$q=~VHn_a5;#X#?LN3G?-*>VIt>zj}|-
zK|E(k*Dh%=#9xj-RlefC1vny+>dL%#s+x##;TiFqi)1xl^!&J)-(BwJ$!^Kd-+60d
zz_|&2sql6{W_~s5^;QTvdTY1&QhW+`Cn9Ek^`5LEoKvGqHiH20n=k(%;D}%8U!U#<
zgyW5pf1E?vAEiHqxgQZT|1Qa|zB6>OB9vi%`H$rBACde=Wce3U=B6vZ{3A#+mrt+%
zrR!asSD|Zx2r|EU*|8Upf~WeqZ4!}subBF7QNF(O%l`#<Doy<Ay<@N72ozoU^!f1X
zpdp!5Gt(Qa={ojz^8W#}s&5gWdheF{u9PktR@t{IU)Adrc+BOi?<t(ZiBGx|y@0Z?
z!oP<EGrxN88FLAagNjjxicf_lU^ep~bZzF82VH_e$tj?0`3f!vGv%f9cUbb@?-CTM
zpDX(O{HDwD8x(;I75@c!c#@Sa6`m>i1vdNpqD#cxSrm^cK80R11%K7Kg5uNk?6>L4
z=U+Mn{})yWrqAYexP1P$Dfri}68!X*DD#!iACUa{`unv<aQ#8rk;+5ytMC%ZuhLcc
zVnoDFD?*u&2eQ}{{4Wd$hC`B11^N6m%qZJYG!!PI0!rmE<bm7{@>IkbhZBN-gEaK{
z0GH!W*}bw$8lMZ5e`K5B@B16=C#VYl(id7<Ebd8(R*d$_PckQ3A&K&Ta`8o(_a)}y
zF{@hLW)zQGF(N5H#GGh_r=|RhT)d4bzabZ2iZ5!o%_ttXva-zk63vM?XqK73bMfU&
z&C9v?ip;^zT>RwkSy4J(XqNfzRxZ7s`JP@bemYb0SuWm*`N?fY@wk;$W#%7qq7`qO
zSMy3PekSIL&1MvjTXFJK%>(8{E52-}#`j!2tWl1ex%kQ7C8Kn!={sh*^yJM?zO0kH
zctK-ja+1qqv9h!CFp55JFp*SN=9+^3+$r$$CgG`H7qLH{&QGAZ-Gc9=DZh+hy3WI&
z20f^QhpJywA1{<oJEx!AUTcA;{Ey1jGeCX~;+ODvf0#hX-yIU4{~p#`fhTz$mF-CN
zq3dq^X$9=(TfkHPoAdI2p3_gZ!>Q_ZlG9ITr}FIieU6`8U!V)>rwjsXY_c7?IDYc?
z^e&Y6LwR<)oL9VRQh!-3@yfp=Iq2HN^O@XV27xd1UA`-TcM8^=-<zhuKhE)JN9ot9
zx#D?=&)>~>Wr}<>FcvDe26$JFBm$uK85Y8KPLU5Cz;(hCR_h0qCA|!@P`TGnf&Vh_
zh06T_@P)ox_=_p%PXS-(yNkuv!tyjuf$!${3Vi3&&8rlX727lg{f$%L?**Q|_eh_$
zF<)Ph_-FI*Uk1LA-o7&h{vDprWIKOP(l=DN&Bd~_;TIIjzZv*K-<e#^>CxY1KI(gq
z0gf*)Z{|I3EbmR{-E~!89_(5spQ7=yfh0atIDs$W4B+1S>NY&#PrKj-%YMDlNRJu=
z^!-M9Rz-a0(S*XM>=MS%NTMGP|3&fOPRfW(Y-iZY!m-43ELzvRd>K6<Q-Fwn8Bc7H
z;Ve%YXOzS?TXbS{bO%^W5Wm}>Lu%l84Nte%z(b1k4N5M#jLWJAIy!K>gPsV-`4dp!
zBa|8HXewb0(-}iNMwFX(G#7z~4RE8)Xy4f5>GroV^H=+_&!A+V`pW;%Vm9S(f3`9E
z6+piIs6GyuEhcmKx!?`NoZBgx6A(FPN&cP>cbZPlx!vlsgY>CE`5nXjPc>$V=-Y&*
zJ3rZ~Pjw38uYV*o_j`%-JyB(RxnC2_`I2H*S=r*JcmmcWs#0v`5f=GD%FGuT3+r}@
zNBjz8EWWdu|1rw~gj3tk6kiBc-^rZ(K~A^Z^!>=(d+%s7`H4w!HYc|Y#S<EQJN*9w
D<qyHa

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/example2 b/3rdparty/qpOASES/qpOASES-3.0/bin/example2
new file mode 100755
index 0000000000000000000000000000000000000000..51984ae9bcd9619f0e177a2dc552a9b7653c4f90
GIT binary patch
literal 31208
zcmeHwd3;;dmH&N`7b%YAXFDrNKuH~%ka&%gI3bH=OO8Zj%aQFY5c;B6iY?-;S`vq$
z5E4Q$C6IKOPKKc+bf!#ZKGPvHr5#$@5=aa5SK6U;ffh<L6quN$4Pj|0OXhp-^7JIz
zl$lR|zu!N+#P8m7?m6e4d+xpGE_(02>wLi$o2D^!+1MIJ&}T~<IZE6a>+R;C#5FJ{
zYh)L*Pp|@D^6}TmQz#TM6`qr#Rp^j(c7%EOYp@AKgRPNAR5)9pnF^JL6jFYft5HTp
zxN@ohRmhl|BZ!Z5eQXW~5pLrV6`q!QFj9<PZhj{;@tu}@rzM{Xb;+kfRX!>k{dLOp
zPFo`&f*UDdUVhJnL`zrui>WBZhs!yLu#rbpxJsaz3Kic5@R6P$_0lC_Smrm8T{86}
z7*st}_r<p_tFP{hR`$gQ(!-U*4a+K*)mJ44tLk{UNj}l8YwO~S%8oMtk<L@_N2aFq
z>A&ee`)2IE#|9EVIQ!zvFTQZ<#M%F#G@>ECLi`=l_d3|zDaDSwyyBbkn1|9x)<yWc
z%GvyjZ~gZ3*EIQl{_4Uxx6Gcr@~?~Pwm<XQKd=AoiC_Kbw_ket)0<~sb=@7;eDmPz
zuE|rL|I*7p{k>~XVCOf$I^jj;caM`gZvy;gC|WiSy=?;g`U&vIV6^e%TsZ-K-vsz&
z6X+S90Dlzt@$y|Xfj;Lapl_XkzHb8hZ%u%AOi-@D3FOyJK>sN;B%RG<N8Q_yQ|3Ah
zdYxUs9?QXB%keYW@f>^%_)^d-yz1xMAm2TX{3!5ctl0FUD(4XJrL2T8*{@aq-6`q!
zmNjzHm}EF0^;G#PIeUO7`6r46{iEQgE6wE;v7yP099FlrQvMk!AME@JaJ-AXDe;QF
zgv&?2lkydwq2GUcDCFtzbx>ztQdPqYqo;pxz(}SdiIib5<FZ{wIMx$SrecZazDP0|
zOEM$S*=j^%35o7(ZSET!h;>G`_r*lo_$imSEth#UY-mpmg1WyxJ`hj&+Sf-r>WrNK
z;dpzL85y1x9g4|Y>)S;s>y^0LWgUZk=~R4hz%vl(+mnnZYa4oEsn*Ew`t_Z=;)8t=
zO7pe1br=~bzVHO%n`?dXI7n-2{jp>+9vSG|GZgb#RT-O~QfXq1AeAevC^CvP^bV%D
zwLjbrA5z-7T9kHUBmrH!W6guRWGuJ9?a_`U2Gw{n742TMh$?&;iZu}F$2}Tphki!S
z@USryOC$$j$ardxv8#p{db+!fB+=J0qibMyd?1Q8kxci;D6v}#GKLcIfz%G8yLTrW
zN~e-^>+X#t7_u8ory!h&Wy9)^#0QuWi$+orBy3M61s<q@D638<6Eq@KM^b2UmD?l9
zn24^d?j7upRmYRP@t$;Iu(~^v+8G}(b|<U5d*ji;M7(-?I^Gwp?oA9O_av*AB%(uT
z#e1q{vr}Ou+BwNjuB;v$N>wKYlhr6vyl0S*e>#t7Y*(z0NL7)#lWAiIS~41c2<3=E
zrF1fe=4DX%RLMl~iIy5YTi4udP=9OQyxG$fFlwuouyw&e6F{KrMck-Em(0ZLs+P)*
zYr|+uzQ)E!RI@kp2)T{|jMnteCPM*<JI%K!R>6G{gTKQ(<>Ea>#Pt`%jEgwmfN!R8
zzXpFly>}<r^4Wf*6E3yz^KhAVcB_TnBjq8PrDn#RG&8tE(x)#V*v9UW^ye<xMr3?0
zhHT7B`9;!kx{1Zyz`cz_G<UGlyDfMrn$s<?;FV9Ngc=K;@=#ZU1#g{qHCphLx4Qfm
zyc#<wt=)o$LCtHk1wYxuGPccvFSg)&EqIp&KV-pU)G@DN3!dsnU3)F~QiVi#lLc>`
zCmgci=UV8GSnzTx%TxXPUv>CDm;ae^H}fAjk;*?k3uyoT?>SBi=JQJvNIE}1g?q_G
zjffH4ONpbW&f}VY6~R=cqfc;nJHa%Zjh^7}<pkRaKF;AV!BnNAM>%{M!Bmx_hdA6s
zFje8`UJkD$n5u4ch{JURQ<aTw<M1T}qiwR$b`H-YcoM;l9KL{Hs>;zC4wn&3RXFPA
z@Faq%>PB@A+X$vA8)Y2+V;Nwos?jqa05Sh8!Bj<~rvSrt?*QQ+$tS*(hddj<NxFZF
z4DwyFf~U*V+125{{?$g5@ob4+97gCr`0a`skoX_Hq2e5f=tA1mHIxRN>LS3+C;A;F
z3tq?H9|6x<DdbdMz{Ap8D$a1`(%UN>ka)JFG*~gj@58v?QgM??JtXodei^o>yhK<U
zt*D_~p5@rmf3DaIGGz5T&U}RPK=e@&xwGP!h-|HB1i^h3y2$d|6*E-Cts)CV<i3jI
zg5~ck?ni`5_XI+^xP%itqOaa5)LPms)$11FoJ)mb_f<S5IKEx+w21tD#VHZFz2dZp
zY^`XA$i9kgr(b#>HFNNqibbbi<uPwXqyOO6ip9Z$w^SSjaq!>`6=V<pK^V(_@b-$m
z1dMnq9R7peiW>jH`?ysz)#R<10Sb_8t<e1=c@>X(D;855JAEgY-&xU1JiJPIwtodQ
z%<8S^{cav+mJh-f?0&kPe&sz<&0DdJbU6hK=|UFq113|EIwyE2%xkLE{t?t!J-#{x
z6RNWn>O4rMJ>8^YfsyZ>UJ4*E@<{F5fk*S|kqVdah{J#V2Sk~E1tqKe92$34nS&#*
z2S=Vi>nSN+TtWA~ikj0dk~tFwIQ_GC&z}#Dyyf{0S}hz2t9^#dGlzY*GTS_lgU-MI
zq{F}e-394qALKTEn&ZB|z_b4~E&WLAfwxnR;K(0NdmubG@?WP{AQYAT8zPil>-56}
zT>m}lcC{z2xYD!LbEW4~C;cNQO^TyLG4h+!3zPsPp8i`PUwO9nwZO<vKII>I-oO8a
zGwq#$M^Dg1-T&xAv>xz3`Y28FPq%~g%^g)G7u`S>LaDr{<bQW>V_p8r;|Tow&$;{~
zFYdNod6Jqwcd&0>`5+~oKUH$kUc%i^kfML+oE`N3k%#>cy}Z`1J>mb!xipw*O$O#C
zQ*U1BA9+OZKQH<BuD;&oMxB&)f%EjWa8Or1l0N{H_Qpx_*Z%#hZz=*o>Oxb6DP0eq
z!mbJE&p&bH=wE;Z^)Z?hp=Xn4<h*BNVB~F2S77Alp3dOG`85#G<v&>2fUqOD@Wt9^
z{13fvr;+3Iciw>s54@JTP#GaO@^WzGZ7*b;pYkj!QuD8PCH*oDD5?rP#>pKSqrGtw
zMWZoD4mmV$>Cc|_uXN9kO=6SU6Y>5?pW#j8w$T%IuXZ!{{MwowWE^#!Su0|vWOz=7
z?1aEiX%r!o;h6^oeCk1Mmi<!N!y)HA1BJQ?VZ$HKpXWUiA-R|m`1{QW9WYe`I`c5X
zz0|33(f$Wr+SOsE4eQ#h$%T$XTA@z(+4#E`J@67Dkk|P1j8!0==x;iA{ye!-=F(eS
zGcGGB+U?lO)?RSwB}*#ikqF{Pb7Om<D-BNSB3`<*tJ{NIlR`T!8SO$JcpIcq9-F~O
zYt+0cQyW>6OTX2&!8PNcy~*XiCC}qpaAUs5RkOcfo$Jgb`)>OrS532Pfyd=;a?Jp3
zlgr^zb0gB>ar`}o^2HF6tlMlp*Nj{3K9~Dop3k-5mVCdf=EefQt6_g(tE;ik)d1PZ
z19_S9czLFvyk{T>o+ax#e_oq+z>a==n?3(%A^W|3)OLR%d#`|ikwW0VMz@czk1g=A
z1wOXG#}@e50v}u8V+;KMwLm-833MID<cqEv33C89Ic*#h(w-e%DxLQDtT64-(WT~m
zUyv|uXwap8XHa`l>UV^hBH8?%m9}~4(nWw@QpEMN{5@68BOMa|)(7VYDSE`#NP+pA
zZ%u)KY2M5)dm|4f35fk0=9qM;`Qi)#GWoNxgv4);9B^;rftmvzk%mz7WwnQ=_7<O>
z)X3=-{+NW7-Zeax;rDTIsr)gO=jB--=`oe(uvC|wk?`}*M#=gACQ!d)=jL{g{Qc+~
zGJHgazmVZeGJIQx1;s+|X)?T6hRbBwBEyX`+#$nVGQ3fSUy$KFGR)Nro8T+m3%a(a
z2U2NweN{tMePvymL$%k|)>qZkS1nm6@cbuoQLUnKRM#kxGIM`A%gyQ#+Hf96I<za%
zzVqHiW!(er#vDr9&uHuVXZ1ka9fhx=sn|8go2Yrat?&*se0yHu>l|6|Hb>@v39TzH
zFCRaCfFb`CK(^P=A@cXripFN(QG@&&C<<v0gE8Mx;5ba6R`5Oq*q%aTE~xoCU<xk)
z{B+@4pFlYsWl*p1?N0&C9M7SD7QXvAiaMr1e&Kr**B#r@PYU0s_zcG^6s7P3in|@F
zQP#rq6kp)H8^Q{iMnH`-0c8p`4m3FLM&~HBaiGx|CH6cH_?=}iabZ3O+MPc^=PWGX
zz-DKRv~p-yK<GB-Q|RV}le7qcUgyt=XR<~L4>_Ns6rBSprvs%bEYtdsGVFYnQl@E5
z0QNdvWdNpY?Ev;U{}uPb8Cnmd?RWkI3{^N=+YR6*=VTHvU)v3WL(b2Z0;tkP0331N
zN<cLSjynH}fEo@QbKXfnEeDP}|Biq<Z6SE>cm6FIv!3gA!Wk!nEYm)Ne7?sf{e)Dw
zqwsN1%qTocg>*QIfV!Ka%rTwJ`eiDv!*Lr~<sK@n&I%lt09W9kIxISj8>u~cA0mXM
z+T_@S3R@2BUjis5+G3L7+JPcEZhRbimh4B4d5+IN1OmMq24ecNXdxOBrcuI!=P1Di
zBBsxSqME)OQJdb1MXX)-Vs}1IKMZ`n{vO(K0W0|}gyz{AABMvEIavQX{YF$G({F<~
zP45J$O@9CgyIu*&dHQrTi+p_&QVaA^U<!5m_1K{|qlGy2cM+eYUx>_#^hOwOvi?I<
zRk6McI=b|K1Xqdf0gbNDhbpD|CgfPAKa7E*T>lOfnxfx=)Tz1+&3Kx=405LH%OPQg
zJ_(x4)USig3-qT^yjl94h|kvFLSLGre+^n)sNVtnMfy?b@79x$KUbfL_&og*==KTy
zpJ2oay~M@XeEr9ee6hY4_fP7#LbnBa5uE%&y$$(ZqR)Yzi}bay%wj!?c%|-xo>ltY
z(4ksS15=}~fwr~!3-B3r`c=?jiSB{hsMmjivMklNLFO`jC+xOd|0HBK=vU*uLVpfs
zUa9{Q+Fq(V5nrWOLFd)_S@5pW7okLJ^&v<;f!YJ=A*SDpPW&*_+aUHN(~m>@?;#Dn
z>HAFoGIDu@=|Mn`GJP3J@EFr?g}Hvf^iM+L|HJfVAb-g8Vd(id(|-==M`Bq1G1ISt
z%>Q8e3$W1>;Df<_!t~`3_9WByz?J=!={?ZyXH4&asy}D?QKbEX>2IQ;KgIN~!5U9P
z3JUg1rf&f9S4>|8%4eAFgs^9sz7^U2n(5y}4$m=tIyCzY)1OEAPBHx?`2G{3fqb6n
z`%$ppGW{${^a9iW7Q$X+`Z18a#PkP%{4b{a(ArM}f=Z)k;xNz4OkW6LuP}WSmVTA#
zO$c6N`s=9I-!c70l<)UUp9c0bO#doM^g7cYM{t(uw?of2APgD3iDE*Dx0wEW)cD&>
zk0ba4)Bg#<Ii~lanC~$C(_r``7+`{TVII`VdrUur;C-fl5lVc3GC<gQrvC!Sy_)`2
z$h=<D??rpwr|I{=^7}RYENcD+O@9S44`}-9XqPu?`T)%HSxrBK^4+BAe+R8cG<_@d
zycr!3N_<Y!ZQ#2_)90al2LU0YTQ$8847X|e96*OOR{9da1<vJ%piG&L%$@h00yDFf
zeFg;1uM^;~mHiD0;QV*)nQUe2Ddm0wrr64^K;Zl)0W)l6HWKhH0%qIF4inD<1h{SG
zl#O$pwgLrOi@zx^Q6>U=nHB=(a)q5r95Sg*y9(&Ene-~>dXZ#>wF5vOQb|)d*EBph
z!<=1}DEC>!v|~vAnj)FXxu<PFhjwnAjPjDx(|(BLUzj99%03v{*-bu~ll%_J?-0p3
z$ej8C=`igDG!JKdB|0@HDMeXl<IhS`g#PUu`Z{_PN!99+yw)TU8P%h@oV!YK=Omku
zygi5HE@a`n<|LHnBv&IDkGYAA*tD0ZFni~~AUyR5QofW)y_Pii?7e7%T*+@B<snKX
z;|nuS)6gNDxBMDdPV!4cUdSOi4}Rz2GVpMc_mE81%Se%;eu&=d{OdQ-`;L)eD?n3C
zblhs9ygwt7yI)0LBvH0?NN!c4*tA#C0i9pD2}<+S7*fcqiHvyVO@9t-&VQ~({g6Q0
z4V0Xb&x&b4vOR@l$H8Ah;o`pnLFrG?Q?yA9vyp8%#V4)R9s>SRV%A|giEYxJ0*3qy
z$4;Hblb$4o7VRw1@2ezBW1AFs92T4nQf(@Fup56C?r9IB*-YB725ySmt^vt@BAEw<
zsSD^<MY#yNE&nKsu5AO&kV#jcLl?OV4wB2e2{cDcx|JEa4JhlRz9~xHanO9<q~rPK
z5i#A^G)}yGD^%q|e+|O3CNa-ELp+@m?;8OlCoVunqBK1B3?VE23d}WQ3jET{PI#={
zAe>HpujtkUK+fDu%shihq|JxlI^lJM!fa*-Bs@j}ozxhwT8uj40&gdQT#*l##ZxT;
zFW3p;j^Y(0a|EqtRt)fZGLBYGNyV$RH<0uWiNAu-0^6uLK|F>22s<@GNwc3u4=DC(
zBuiU}w0f0pX*R{{ZU$)&=(Qk{Hz^V!V$Oc_lj86P$}iY-8<&<OvVO{mTEDOtuR>N#
z=@%YFJDcH}s@)2@189~P(XF7Yz(2cSo~F&Z1IeOLZlcs40ogMoG6|7fxhZ={_>H(R
zK{ATucgB*ixj#Syam~^`feIyV&89QTRl5K=y74#V77{X-8nMf*IS}v)vguqojb=SD
zMK;y!!J<gKaJ(gPuQBgulmVBXB9?4zF|)e~E>?G4Lxmkcr=PVCuqfvzevFa8RiKeW
za@gENsC^q`kCL3vL+sQeq-HrKxb|vK0sU*0#AY8svvTd%NUlaxt@l-usK|=xu(|6-
z>dz>?jg!rUT`$5P7tZG2fbr0E^S{8aQD&gBbE>tV@DbIQDEIldk(^t9igrfs3RHQV
zDh7%nMJ0Uf{BMB3b-N2QKJH^}dOjyT1hOwEQa1lv=*q6YS%c;y1r~6sZ-C+<MI{B+
zAy?O3$I#v+X(1>5PmsNyMJnlw(Vtykxe#|*pAJr6iVnF5f4oGZb@ECjp`z2yZ$Q<!
zz8XP?m7Gq_xgK1x59fS|YUk?*(9|U7B+hvNT>p~I$+P2~o3|k|cd5Jqf9N{#lp3^c
zdJz}&V+i@f|40zm=!Z|jKvJX0T+nowC4@g=1#W;$)BaH1rc!mX`CP$Yu1BMla*Mg#
zLCE~_|4J^K{{c0@|2nTm44dxaB5CpTBNEAb0qMr(FQR61rVDZEd}xOyob)x2y`gev
z^XJ07x!&52HZ673IaN8T_d@)MveUAZO{;`~T<`q~jf=O|Wk_D7NZ7@rl->Duq-yQJ
zZln~_hJEtK;C9)x+o-$tm}pi^K7d<ffv>%TmDC-Ds*c&=kt6xq@l^}U(2Yt~aNI9H
z^Q*U>IQ!j|3x9{^U9y_v8gKdf@SJ<6dM}}QKuOc<Xii5LO@egWQd%No`FsR-dtTl<
zm_yhdd4GZ23mnvr3LG@8SnQxV@?st@;PC<n&5jEkWW53hWm4dvL2t2xvRKUH1w3Bh
zn9DJO@?yjb95f(Sl6Z1eRX}V&SnU`^uWdxEz(Fp3v4e)A#XMfX;{^_~LV@EtPICbF
zn#B%sdj;}NwXxWd<QTfwl1H=u8wkITzq)rIpGPXuFpf|)6_e4I#6X3i!~P6_KPW;T
zQG|8dK>Qw(STVWaVj3%IY4Ed`pbqEZj|=A!#lwneV60z;*|O#b#nM}0A8j`kd+Fbk
z<K$7c>`HV$`wl|eV?;&+xqSz<qAIrhBsy&F&YX7VJ)mp&ByMApcCT0k^!f8EYZ0I?
zUHT*hXbVYt?uMRr=k;g@tB}I_5LvD5{vz^R<7ZkGCD;$|-Mz-cwCckG*tMpIX*B{k
zv};Y@cBa(|=;*FBX+U)XI!<ZeTXHw&JCQ~rz<L2bkzV^%sHH6zSsdNHc0Y`yH3;4l
zyVlmAXJ{(~_{6TYmqR6Or2tRuTKf!{@KOPu*|qk1;$0;qpGvQN234-D7T}rm+JD6u
zrg^BX*$<!Cz1B{=P1H#3haX6<{SLad=Hq5R{O$DG#c1YQfEua&@VRs&p8jUqW--CQ
z+GPM`(YA2c@F~w^G$ZW_;#`J54-KdS{2ah0s_Gw>G$M-OnctDTek)o*^JXO73zYXo
znO=l0>eITkTA$YGw9}($;^IOs*9B+_>9|`7c?M+OluZ9nAx}yov;ux<JAVx!Eu@L}
zS($dR(86h_N5jO$6?j>uUnr5<z&wcIr6@!ldh<#JdA0)d1ev(F5SJu^JLH!(v=Kst
z0yAV9ZLg9UR^o-H%cN<p1{H8rb1;rp=hHTjnZnvKwysms(2g6Yc_7XF_eo?m>ie`x
z;LOyL!EQf%Xcq(kLa^C>_$Y!c_QS__`E0wXg`^QW?1!JAP`4!+f*D#3^Q+3F`CJJc
zhs+UL3FRR89P)3L*4m5wdli|?|ES3S36cK^k^d=?{~4M8DVhJ7-27ELp!{!Bf*(ld
z=6|is|8f}f9|W1r@fh=e&Vql?f>$krmOHPEC)RciiW;_Ei)CB2tf)^zwjEsoCgAwx
z7W^!UzXp})k@y;cqyC0h*(B1ZkSBJtbwd()xdj<GpeU*S!OFbD5_5xSDX1MRrf8K*
zu3+*$Kz0FB=jX5{;D1#xW0y@qyx<sZZCrK?y}jTcDY||bQp|yL{VfEkA$0vwg4Ezy
z96&a~`+z#ydIEQ+{e*iPP>i)bi<^BDc?9Z9Vh$)Ac<BCO>_*~`5~-a}<<o=E4<O*D
z_#-r5j8Oz*7X_h>P_z*4X#@b7d}=)<w!aP6crB912vaGIZ5Tm6ybIG20Wz&ro9`+w
zvb$!vx~7+Ew5Mg8#_X<@MVA%@%AsQgD=J!pbWC$(6m!!25@2Bu3W%z(T(i51iik-A
z=qNTNG=R)0!Tdluf!Nm;@=+n07APl<$#Yqtocdw$#fV^RT>*hgTuEM*bbSH%XTw0H
zwk&*^HP<@GDzDPk&J<$midM~x@|>pRBZGRf2SqUL$`B2>0_7RP>Ek8Pz8!L31^$_o
z`>L6w{RJ6C=I~5rkt$T$`kAE2?2M>j(Hu)2b215(d9bK;hDyf#(3F}lq+T=|8aK=c
z7LiQ1V5JOH(dL@c@BnGHG}3aONG1&w7q;xJbwhG1Od&-BE~`Q$V!kzTYz`M&lQVjK
z(nLcqfyORsJ*JJKm93gN8y;mL5VF|`#3fk>sJ6(8K-1S{>#Lk9TTvC7I;*BeN!G_)
z?_$8URoc38caf<5x_n+SRR2o|4+Zj7T74BNw0@?WH{qp3DAdECZ59Bn-Hl2mTPzpk
zrCM2G<BY#<;<$!6T5UODHWzJy!YlGwZTW0;Hazl=2r!pgC1tU$D$=^9libxS8Ra_z
z(KQ(x*-*5#ner3ZXh}u4^;l8?Hd$36xy_^lZ8>{K_G#$$G~M&vybsUo&N$ozaV?Y%
zPb^Z2!><Sl2nYb4Fq6;PnbZ;1<MpU=s=*+Spp>l?Ar;%K#T9)eBv8~q>%mT<+Z9S7
zHdqm;LNBIeHa18TJMCTkD52p3nr4N6#@Eqe!;$`>KD=D{XOtNq=<Z8LW9qdx@m`%m
z;uUXWS-pJUyJsL>y*-Y1_RLrB@XBBH7!v$Kk2mGG&@5s97nQD#4W|;3BsLy|f$FNd
zhla@bt#~;z(i8JX2BLUpy{D=dXw(tCem7WG)r$d^;?dX+#Id4I;U&(biH;8<imri|
z_*9hM@&tj95*UcahWqf+t3u?yV9N0rl+`<_CS5L~d2k?!0^s2Qyz&}NcbgJhBdJ8Z
zJCh|Rj0~hq9WZp+)OyMOHq-KK_U~S&VKhpgwNJ0I$6t8dUcS%Xzt1sx-TL)=_d4c2
zaM)44f2VzTpWS)LK783>$Go_$qTDg>n}@HzVgGf94?E`eT(#xPwvN*MTl4Jp3mrQk
ztH*K3wxM+Y6+lemh#|+JyEd(6wmhw1Gw!y6t#tq9U)m?_v+r_L_1K-MJo~0RbX;(M
z+OZjoB~^Kv<41Y+NS^kqe!*F=-`=**UjL;ZI0E*d<1$+bdQI5ZX?VQerchU#*J$ws
zf<CViXlwC#Is#3BV4!mgBZ`hdYkSaVbac{tPewcDvmKpAd#J<fYvJi#9f7uWM%cHm
zE9eOaIy{|$P#eQrqV#_0u2jmv3!nW$kU!AUX?T5|zHn=xjnm0ovtH$0KBj^q9lr4T
zjvBoBoa`M;q_(GbaCw2Yjh<k@Yk0!zx>|j0ogMsb&%VL#od)G?dHI!!(H1iNzK)K7
zr_JbS_caGv0zU6}<*Vh$O`dQY6$_T}wFKHgYJKt9Y>XMrEH6JV;|*!Jwg#<iAlVu3
zk0tRUF}><+^7-)IZ#;^(tZ|FtEmecwl1<TDs^VSVgm{;i-d;s%`75}7V<5Jh>$aiY
z@C3s?k9P}dE6^E$5jvpYjzlch8tWfS?BP`YP;00&)V>83=WADXCPQ?!d74n4ogvhj
z$7?kC+M4~Xp746k<=Kd4z|%JcI{ij4(AKrt2(<-8Q}Bhup|Hg!BrX68ZH5NCDXDz=
zwsUQ*uY_j`Ut5otqubK`+hYk&A`#i+Yj2P9%sV!D+POF-hRVM?5l_Xq1+ta*w0VMC
zI(){4_HYR0X=TyDw!u_O#t3--HyRj7#u6zm+_x!^qj`Hc(AF9BZS)0=X1}Lx9aZv<
zzDQ4!)01hX9!BSetm-%4d;dr;iuXqnJMsQD7mBLu2nD;Si%^FOcOuV@uI6Sm%$BYo
zDx|Y3+{UgI4Ty7ueH}2e=$y*q@&wUqRYUMJcLp~4*tNM{QOalyZS+~Y`JZ!mP~#-V
z1wLSk@qjAJ*VfS$_5}mVjLln`(V*6Go#1zxLv0<MVGk-n5o9|bYwH>Ng8xT6P(U<M
z*-M1aQHHnlXxU{~M558oL2o?L<BM`F+d`ej7HZ_=0?EgqzDOeKq^sS?vS`LD8f{&{
zU`M;BnIBPLo)RGQSjznp96~pjWNl_SJ~ZdWeycf@zR~nhUmWk;$8eN^DycGNR~y&Q
z=WRplV%>v7!Y^R+*J$lRcWv^K7Y}rz*$}Hg)X5Elb{T4KA2Sl<yu-y=a_bk}gd|z&
zHK%z}Y8U!!c0r96IF(>KGlm95FM+ED1@~KRn>xbgaRrv6CrnRDRVI9iPrmIR*_Ck@
zA-w9>>Ir5#T?>tMY)9W<B*l`uBSV5!WE*PgL}MhczbO=6kI>TdcBK3ISdZy~Nw<!`
z<-&5~HD&S_X3dXnydQQxgn1N)PaF?HMk6y&qa&{!YV$F2po17z$DXOdYanM(hikO>
z#X+1V5lIau+|gKHEERKKaV1mdUGM>Pd!&13ddQ$N9z=DDUdspWX4L_Z1L-2%4iw9x
zgwF~EH<CNG7(q7KWl*_!u+JCI9T71QvY}oa71AB)>x&Kaq<TeBg7D6!qv()1@}#bI
zucy=3>gfyz;6Rm+#JLY(AIx&tW^1XfOJIcD6*F5*gda~5ON>u04nq-xlWNLdOmQ&Y
z2tSY+OL>Vg7>!rvdTKqghg!=*-X${^G59GIah&pTb<7wNB{YZXP2oVN&uGCEjm4AX
zpm=)NM~*#f$V?J<ilxN>oYNgCLpf5AyXtCN-xk``mZfs-(vCg-{V|#ei&3aydmOX*
zSR^rK4udL;;z$^DyG+NFwOmh=5jtK419&19iHg>wBu!j<hNlBI#9R%Akxhu^aX3dL
zCA^o#;GT^%_@fDUnCS+gI~Pudq7=+d$l#crz;}ymO_PS&eQm;`=+?t2KCQjn7Y^}>
z6l?Wth9|@fVVx25wXN&)bM7%tGTX_bZDZR%=g=3<nA=}eYi$}F?2Cy5ZCI45)i8GS
ziwO(7ARip~L=3LE&Fcw!g;~N}rjHlJ+Mwh#foi>jCflUF#98VnGZS0djm#6WWuhq&
z{g^>5;DzmIF<|Y@85wwE*PLj<1ykOlL58|IjZlk_*TL~Efz1+BE}#5ZW+KR|fS6mD
z(LWd!?kBrBka)}?e4cQSX1`t^Dv|W#Eltd{7#dt3tpHTp8%ia3_U262@HB<^)XVA&
zIEm`y!nC4~t7S$DdQM0!aEz9qM@(eQCLtG4@QPGzbnu$HdC?;Ath~FjKb0Pct?r2p
z#L%+<MiSk<tHq&i?#do_WoQZh>fDtuCUv*U9qEB?IwL6w6wV^*-dQ=67)-^w;kYf!
z1%9eeB0W%N?So^78*`aL=CcfL!h@<NBC0Mk>&>2x4a8!Y8(GE?YjYTz<;2r7wH|Mx
z^PVVAwJhT`PqxFnX%{N3D#sIXVj6k9fv^wrUCg1S&71w9ps!<nb`w?et<;}hfbp}X
zEL{-_m@5x+9ORubH$&22x>2L4YaKI}223nJxu7IXNw^$KFEoewKXu~PpKtq#8<2{D
zB_p|6<zo4PQ|OA}!QJNeNmEvLw+!AO8h`L6Q;b_Xa#|<`tBkv{Haj`p8-HdBN9YZB
z)`hU(GtBiV8=#pX<q6j+9FFvKyh*9atgNrhup*1kSIlZAw3=p^Etob~dy&P%)mmnX
zIEjpAU6{h~%%PiaGd47^oTKqdIcB-;@rL<SI?L|dzjn~-kG50VwghOaA`-<pUKF4s
zQ?Nt?^&(ix1&GCWpv^KJ!qV5<)ePrn?jNWVzVM4VpR6WgsdQpsW27%FCv(=RRM;m*
z+GY>UXRujA=fxR=J9Z>vaG#dBo5dDOQ1~D=cjOF8EIZ_!C{wrQY^;gacEJuYQO@2;
zYOQ1VnSaba!B|)FVUy*|p>1-ud~L4NxYnHfaVus9fy}yFm^Wv!+7{xiO4gm)<G^Co
z7w&-fQLUHeQ60Wk43ZeM#>|6i>#S4nv5PdOn&5(kJR{LZjFxby)iSrXR)y?|wKbx@
z(kg(OWITzjA8e>FKB8w;p5j=R>%pZum~8oJ;K#t_4P{16I`)nGFs@j3#~ybNkN1DR
zF1Cy?mOhDn0CQoWe1(`=jlU<z_xjw}rsWFo%^DnY=nH3U9Fib0W3kL_uN||4$Z#BD
z50_=NWu{p-DErA7j2e>Ee860=Qa_>N<5KbN%ygX}W@upB8%Ho=YZ+V07`TlZ(R56W
z)wTnv%$pY9G;0op!(HuaS;fbgoQ5fPO?X$$G9Y}<x?rdYUa=J`e9t<cfhkveS0`U3
zpyPFhyZA~m(1A@;zSG7v;tM8BZuxGkrG7BnhjMnv#5RYgYcqC(J>e}FG9>-@R;(rG
zkjc&5*8`t9Pv$CQ#vaRojy%O1*l09uG59>3JWXaHm@yej8EO%3lqm}sNvwDRhB(B~
z;<;4gx9-JSHeP9)nhplU3^q2*2KZEz>*2w&Q+jH-V4~$2PEt&Wm6xzg#^EW$Q01z|
z!&fowViKulC^RFc!YQK6(uHSB-Hi4XP}yyKbA~qc4BFD>$+VpT2aX}{GVI`>Ez)X_
zen;V{84LL~H~V~EjHXz`$khw)0O^5byk{U5<vqdN>te=WUsSn$epn*+0=yYnD|*b*
z5Z+DgKf&w5o$zB6l}mGDznV4|<WY~T-Fn^+9*$BcT#l`+rDDovsVC8AGE-AE>CSR=
zneM#8Y&uYi;^u(GOcU^fFiCFCsDyzyseWO|Nm@d(F#q9OFPRx4tuJ=O6ZpY{#N}*?
z$*JsEj})g~$vHA>TD~FA?M{+pN6lGx@SkZoUyE;(FRXUTFqNPQk+r^L+pni}CHxxt
zxBNxNTtH=(Pbe54C$j|n4~OE+#9nM%Wq$tP>Z&?ZUB%qEqdd(+Sk2M8o^~wN?>?3`
zGFGVxmm202o1ZL8EgZ{2vh0TTP}5~Ji1RK=V-~|A3N>%ZC%ZM78%8jch-VQ?Q!l4f
zJ|gpM%vmdCW1V=C4&x5>$~I!`Gux^f1e-l=Bq}o+8Rp`gM2=0yE;ME<7_xSyw)Yum
zs0Q~Isoprw47L=ucI=OC-F%OZcI8Mk_jes#+-jYn&MocOjLNh`_;a;b#0C}7b6;fs
zOa-Us^>tv?hW!gZ@5PQ}nx9R~*Lm{i4ofpoZhL9Yk2>hiR<$#AM5hb$rbD~H=-PCa
zE&na2j&WxKWIADV<51%M0fr%&et-;OCjqlJpV-;Y5-jsyCidEpE0u}7Dc^`iH*NOI
z9l(s!Rh#NzUnuPLg=wOI7PqXPwj$C4G|FIdk7XtOpl2>B<)+K{14%dpV_EiaZj=w=
z-7G_mwX5hArHyz3f}Vlc%0tWTw4_$gO6IerpJB7_QlMLM=CfslR^XK;np0ck@u(82
zp76A>n!9po)MJm^q^L=FKD@k<Q;nfjk0^c=T9v1TRy{I0M()GKBEz4^@HrW(M+v*7
zZt8Okxn)p4_dh84)FaRn(X*=Lo_?&O5T+6$>{T)J;#gvcRSjStrwS(wSEjJvCm=lS
zQN<5Ru8c=n6+yj`WG}0V?iqkQ5n`0#DZB7vBIcJ_fPu6`tS>?gGB(tgVpV)Vu1eAN
zW)(jVxeAXw&<W2~v0h_GBGMl-dZXY~5QKJj8?oW;*ig#g%RwQ-h|mwULWYW8)eT1E
z7wM08gKrR7;8w-+6?xzgX;#%ei0x!d;;W*u?dcvcM+SQEP>lrR13Ly)JhFXzBDPCG
zv<s%<u%L=!E~KJTQf`&~?_fNUP0z5<11tP$ur<n%2j+J&1SDuxoWf7w4^LN`{GGN&
zKzKA$3UqTQbHf8LSywF}D}Tz?$kS7jzk)-VTQ0vBNpItf6Q%zVTO%jtFDW1v<q#g#
zlp&tl%(~hD;ZZG9e`aqK>3C{~t{NHQ2^(%F#gC`D__YytB2oFPb7X3W0hc;AMnGfj
zM~|)2nJtQ6ozp^Zl+s1zqf6<p=v259X><mRg4H=N3-BNsU3>W*&!{3KJ(d0v&8|iy
z4}XeZojY?(^4Ca0mA~Rw<-Z|^U!7y~wB%Rk-Vi_D`Y`iX{I>!}?N6G^Jojgah;gZB
z_yr_%RdEXGX<jS;p~;PeW``udn-VP-9uDT03U3Ew<yVgHI3BE}t4S4HfaW<v5@dHE
zV&zxo6uns{C}cG%a&BuAK;$U?h+o;C$&rGU1Bq)`1m<1YUm=bmV&xx}{KJx;zonvx
z5mH@SuWtab^4~A{@0a{rDbaG}=6?cdR(?8$lddUvS%R*MMPS}@#IYxlf`_Vk+ax0O
z-5mAZ7*$V{MpthB&jDA0KiN&0I``>myzNL=E`2WiGH6IA#ZM>7(sk-1^8X&Rs&5gW
zI`>F@*GGN7N40O2zbe-`@L2O#=h2_Lh@?%dzmoGlXsrCqy^YuybMrfIUm_;%icf_F
zV7BrfmHbB~ztU65P<#rW3}(V9`_VDLbRBo|fPYIz#Kb*UUa8E##!7^^;$M`5Ctc}M
z;RTXkU^Dk;+#=qXLGhU4Q|N^g@XwknC~n7-z1Az2f9VAL_pTINU&-lkx%|x&@OQ2i
z{C;IvsYEV+Nb={}@6IN{a9p-y$!^+B$+>)j{Da#B#}Y*-1>}G%HUa;25y3Dd`Bae0
zUy6jBB68B-Oa5dILk`HxWBN{&3BuET0vM7Fc0Rxr_*3myjX&g{$IE{+UY?=r(4VM3
zK{fu}4(%Fjjbjq+@b+q+WKFa~6E*+I#^;&m)n(%`IN2MmC?2=NBdK|aHPMclmYQE=
z;~h-R8?y0Ersf0L_(@ES^V#^Z-v^>}J4SvrE@#sho9|&}<HvqCNXUVwm*Z15y>5Qb
zEE`|S)OePSFT?oMXhre39fPhKf2@gioa(H`m2CV}j1$|eC?2=7Y34X!O|;`nBFevK
z<Ix(W-^|93{k{~X+c7Irem0w)X7gh&`<Pk0ps}+Y=O9__?831^fU)zhqK!?mGxt~|
zr=L3ke%=^7_3J$L`uW@hnj6^gJt{RXqp_YYlAys3C#2m}zit42JUPvrer$cU15ff#
z$OSP#e#LMn{Fxm5u*Bz{gDK{*7qI)KK2#pMzKB0<oc8lQ;7PtZUz2F*dIEpiIPGwv
za?$()2Qo1|r#*{#!PxRr9^=`^357Kpt#jJpMI1l&yJhtKHVxk=JC;LF=gFg!j_EG}
ziO=m{G{4lw=`S>{xX1ba*%0vbeX=uUjg%2=D6YK{pSwZvSuRHsS%NQ2fd3~h$3iHh
z-<|;f9~0n5C&1T3;WEs4)jC1tw36c$lnFx<;0J(rXUNRh&A^ZM9k{=lfc|R}$o~=W
z<9*-mR};{`$MMB%udEm9KXgsDQ+dZ;4ZzcP@^bwcz2h_<{f-In^j)X%=zlW-{te*A
z`##^f3FvDv{*xVY^=ts1@;y}Cm{qtf5}$<x@M+-3`_A71PCxd0dpC3ZIOA#7)8-TW
z45)rp2ricoc=%X%0-ttE<AW95*q^<u84n)Q9<aeOAI~#V{YE!^J}-$U$;4;&EGT?I
zY|!ZG8{Cd(%%XT0D``a1!+5%yUKLBlqE*XQET@Mz#v$S#Bomuwc%31I*EGZiTr}O^
zzXvQ9i2t;lMQY%g08gtAzb<*{6M$TE8JE8Shgw>&NpE!Wmp4FxZvdLq(d3}fORr_&
z372fy(QE|&>K!w@TiQIWfo3-T$I|#lqWKNAKlyD!dPc3Wk$*`IUr(birtvS_NtFD!
zn)P*^3^9Fb&AQ(|_P3w^(X%1qlW_78nM^KRrQA>9<s^UT_Z720F-N4T;beb^(eg8w
z`J9;ePGQz(=`v(v%PYz8mRz@Cez?x^!%60On2A1`X#KR`hvk&}c|`Nav_JK{+J{%z
zgg@40G#B52q|YDbv=MqFW!$oUr1x;trwzw`P_ePm^2xvKw=ST8{3;+k2)>z^-7Li8
J27HtKzX5|-s!;#{

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/example3 b/3rdparty/qpOASES/qpOASES-3.0/bin/example3
new file mode 100755
index 0000000000000000000000000000000000000000..b10c54a32773d7cbd8045c64e10d34c3cae0bb92
GIT binary patch
literal 30847
zcmeHwd3;;dmH&N`7s*>*>?9;10YL%;l6Xm+#My{sOOBMtl9A-Zfx?Sy$+m(mxmpst
z10f_plakP+8v#m5*J)?kfthZUwh%~Lx=zzVTe?CQXhLDyLQ69R>fiU=MS7C$c0TiC
z{^NaY-Fwe=&pr3t^}ToBO&))zMbntNEbK}~sqfY}IY|66HrmgV64u7-%*ifgi&zC9
z<@k4U422@5!gErz3T=|kim(j-Z59D&vp9J~g$o3jsZeQ1A>~)NoH8oHT{A^eg^W2k
zfcQw)uP@|Dggbdeg(syR&&W{mQIh!1NWL?YPldXqSD`8&m5u%*5+AWRk)pJd0_Ntw
zg^+0J>V7sGqxe%ZcoLzLM^v~$fSC#v-&NovJ^!bdE(t?2zv=8!s2{<g>Y-^k(YLy_
zX*jlgIFZbaEgx%Jy?k|RV<y$Of|r}*6Yb_e4`)<%oQo9cJPZHi1BCy1?aa;|%Yi$;
z{K;#Ye)j!i_nq1FdVp|b0^+O0|51IPja@XW)>c+ld$5eT2-krB`S{;u@A%<Ezkb&Z
z?VcaJaQVW+3u@NgS-qm~r!RhT>#tA#<U7Cqz*BGSU9jt>yKnfykw4be%=*;_o`2%^
zb>qJ4KL^%nFOuz;0(<c^@NpEjehT{GY2bUOfghO$zIvK`L({<R)5!Tg=%>>23)8^=
zI1M~Jjr`$h=x;(r>TDhxbL<2~;rb8A(NT*ZFM;33;q%zR68LR^&jkHZ30LjyX2^F;
zA^%pu>sf8SKT|o60zMNS=B#sakZMPVCH=`-0gp?Dk4ZgC^|>2xl0RB4=r<!x*Za7f
zYPPS+$<yj*r<C6;^F@+hyEwd#wMw|6Z|3qDb4vLN&d~neIvR9^Jz;8eD;t}cVGNF>
zl13&QO=k^*8C!N6q4;1TlZ~f4hNGEGJi`oMq|1oK(-Iu%>KINX<B@3Ja9rS~##|j(
zBlBummChxDS9SNrll?;@(e(A6?ydcSu+h@eo=OeJqshqlXxtOp3ecM1Xf}~bW;~%p
zC>CC0h(Bf&gtd)~cc&A{>;MTdBNS^}CZ3I?y1F|&yQf0CTc(D05FGZ&WMlnHmy&r_
zlX1qzjL~>HlS)R16WMWNcQerp_V*hZB57epPjXKp8H3R>xsf;_`xWmfSJUVpx}J^Z
zvKjj6ABv_Ka!BQ}5KhF>@*~kik{R(>G#dq?FOv~)fRZuRl*^>4>uic<Q6<a!qM5je
zu4@`fjl`Q0nW4mBE}d%Xk7ln=B#k|prv9NsER{|)_2m-7v8JK)Xl6Xqv@#tVMKzB%
z$$C-YaySCX&#Y@ojb@wDsZ0}!lo(7A@E_+9i|>vP6R9dve<o)PB$5euM-b(RL8V+K
z9s`=nr%EP@PqZM-vdtYG203I$Z?CJ}XS6h~WSjlI_6}nO{IoS6U(vWqd9wv$fra<=
z3L-{%4P%G~xCqU^s=X4-!`mif4ls9LI$lk@@0SgW@E4izPuzb!rVnN8BYC`q^+`PC
zot>Ak)5vr`@HB?y=2P0jz9{L>UAU9T_?!T2`AhXjG&eE#uXANTMPrzm-eH1U^HLaF
zVuGtO61v$0H_w0COmOq~?=-<tfc)h(!K?C6*v$lo0rOX{30{+jGPctMS93H%51HV#
zCi+nmyv_vo9(cj#y}f*7wu5<ZJ((>(b3TyX17ES75=0ZL`hc3aB!=G^7djE6^bjG>
zJ~M&qlB+3AragO_r$dyc+B$oZr?*hrO6do9x}DNw+OsEkdL5<7q-T%v^a@IoDbMcX
z=_@Er)pvH3r!S^74LfId^7MR4lZnrE^K?C>$+TykJY7X;GU?f7q{H5uU(lhg_s9*4
zooDs}B4W4jn0s;COEdJ^CaC8>;$OVPf5g4m;YDnL_mF$B&5LBS_ef;1v*ndX+!!ey
zcQ1BAGV>lPTa5UEUX=T#88iKh9cK<eAPjo(Bl|#iW))Hgz9sW&qr7hZ3aN4EscTL>
zawAcE`fn2xk91S)6BHYy*nNmWANS%V4>Qa|A0aD0NIz%Zt1!(EHy306NnzR^-dT)^
zl3X&(nL$BQ9z?Y~e6)z_r5QU=<TI;4(el%?er^?5%6sV4nVSLg9(wxBT?hweTzCs4
zq2#xhf1|+x1C)Os!Ks!PeTTkxo%hhMya#^shwg~)@sl*rdLMt3#&hrEkJB)H=G^NO
z6Mvd<;XdMj@SSkX9`AA**S!bM)p-v+yT@|vsRs#14*92RA0gDlGm4&4ivH1aR?vG7
zJ?4G%`AuH!wD)`Ga$tT)Fn=G+-a}s#{J)a?`!-$#Jy~vM4>)=KU;DN4I)Jp7PmzCn
z4{Y@6Ajn>jS7BDqBhO&f0s4v4*Pi_+m}JI<>}*DauI;Wv6RvH(L$A1ce20GEiujLQ
z@`o7?sCZ=gIfP;V<<GYK)cfe`R%&Tyj=TmN-uhzp0%bq{q38XFUU5Ul#H^or51i7x
zYk!w}o*Lja*Sg-~y4H1_%Q!VK(5UENJ_Wl_o0UzJdgn%rQ5f{vn)fs@%ncmvYI5M9
zp>#N#S;sa`dBGWcNk=M;8HppCa$suV$POhkj`&z~WOO*b%rTzIIg;^sjIgoPp5$;U
z8dF$DGS%p4UQsoWPK|&q?g%Ea6p1^E6zE6|56Amy9^%*&@5>~zamPR+oylIL`o59q
zSYjkM;@F)?4M#a0=1&wx)0iZzbI^53<I4E-^aJr|CefFmIfWqq2lV|ZD%(griFumf
znM!^~caI~R7>P@fTo10vfMX;vJe<hH`%}qSrb_8WF2|VY4Vd$tPVIc_dRik=NA~r<
zPD~s`c>L9giPH$rzzNPF{0qWfN~1kcXV8m=)l1DA*DMh*?QJ2Zjp^F?HI=rbTBS~K
z>hJFa?-i1Uf~o1D5pYD`1|2AGR;PE?&D}DidXH@%+cbau6)P8COkg#JXm9KG^BgO!
zH08C5e1#m6brN!Dj$P)T>13`t{eWe2-3g23?NxPpN1e@8O?n*0{~45()`;qIBk~da
zpN8IX0=C!b@3eU8=H6lT)H#lndFqxNF89_o-&WzRYdcWcRp%V8Yjf2#yXuy-*E!nj
z=7PSx&c@660@%+%j+_#ZG=5D?X!C0^lAIU&4+}e>A$DAQw}$v*78+mPyxz3Hn-+M}
z0&iO2O$)qffj2GirUl-#z?&9$(*kdZ1&+%56({9=+fkWT!sW6qzXz$(7s-f9(_SiF
zDouN%bg8w}=ViJ^fb;h^YGqp8znCY}>i#Ni>(NDfs&vtPJG!2d_fysSk2com`t#o=
zQWPGyI4Q{A^L?;fq-kBtFRPOWY90KzT$HIj%(;S!&6c4GtSW--9J<t6&LPXE*0Z$n
zN7s2GU_0$j9#imUnO1r?OFFgZs`AIunwRIKP4Hv)hNqPte~{^Y5--L4p9ktbc4<bR
zs}g#CO@=>|;d3&4MTQkvq|-G=hL_54wG2CDxJ`xwGUQCAAKWuo=UCFymrG`Ij@HJu
z#@6L4ay-?tx23hQxwUcS<pR!ceT(X{aRH^b-c%D%E^Eq#M;qXqWjton;_%zDBr0hL
z)H_QkEsO2IKG_Pe)mHg1o-D9xwo~A?S}Grc!&=KKPjO(yS2?i!UU*GeSvhWLfuZ~*
zq%8HID?dOx4weTlK=c-hLfUHNS#GPa9iyaH@ofmOJO?MOX#OxDmA?b}smebuLOE?$
zz(*=yxej1vdk*cc^3~fZYHNV}%GW5a+n$4_m9JBLuI)AKuUGz!;ttzYXlIoZ6klTh
z0)$mEjS|iFQ&6T-<B2x=`{4AI7M^h0uZATm%Xq?TkHF6>%Xy;Pz6Oo1vVtdi?QYV_
zrd<P}JMB;7>4?fIEsDgDy%E(?S)-A{qxKgFqw_@8{$rG?vR)eoX3V~lG?=5cBeBnZ
zK|K=ZY28Tdx4#F!m2<U0NIPIZpLiB%dyqJ2|2ZWt(e{AgsQn(;zOqp}gv4?ChbYm+
z6DRDSphPoIeBAy3C0cmm0sH4Eu|m5XJP+E3$e68Mx0CjVNS)Q%jmYOKtm-?Y!rhfm
zf?{sv2`Z%RpV7rsevqQfRzYU{5Ea*Edm4K?l^>(h>a4<c1z;66s>AAI_#w4x_9H@2
zs!a|(qM!|cz8i^JqOBzvb(g^tY`0yC=MQG=sD?Y)-t{O5^b8Ed^xwflG&bX}Flw3Y
zo|g!5HB{WIFM`c)(if1+>;DYD(DZMjPA&Rxpq!P>xEK1C*?zPLb*3Ar#hdi=sYBM=
z@vG@);4l{bGPDV+z7yawHsfX3sm$tp5wyAozIBrx2QSlqFozfxB2SC{@0`I8CCc;)
z<W{cF1GYlf$TRdtz-{_sXk^zL0IAaVgT7k70d}s@yHJs}`mHEVo&I}p&CqvJVf0r~
z%$fSNC||w)Fvf}oy%xF8(&={HY+Xai=ID1r&UyN6=#l5@&%i|U^wW?zU;j76&(~i>
ze1Se6&2FK7KeW0)9|inE{U+$((BrV{MS2V37wgZX?Jv^DVeiGd117jcZ-lij)t`m@
z%k+Dp+Y&ts16;0mA>S+X`QTluhhdpz`iG(Oa(yfGY}9XuHJbFt0cqCnKs~hRAxK!E
z-v;BZ)P2aMRsTN9vPw@u=4$-|FytEjQpjx6$ML&XcO#c|`qR*My`F#$8}z@x**5A{
zXmzDNh!SnmPeSrZre6VZk1~BF@Q)#Y*i%fu5p-W+dJK*At4#lQ<nlG9-;LDcOuq}V
zzs__E4E_zKFNen8Wct?t{1(%H4?BFD=@)|XJ7Q%1Po@VU^Sey{5^$%Pei()R9@B3H
z$@d`)W_^O`8)4BWnf_s@`U9r_7jQph`ZXx-kC=Whtnn1n6DZh^nf^WK`4dP1<xiRZ
zG-~%}On(BRe-3s8zhL^csK}?8z7+NR4AcJ&e7|J+hXDK))4NfyUo%~UZGXe`U&D)^
zW%_qvg6Ek2O%(IDOz(lrGfY1QmCiDK4sg#ieH_O69n+tJrC(tB8U!yg{VFK&UrgVR
z^8KFa3&H*erhf+|`Xkd{K=2X*ki5)vD~j?brhg7f{F&(!sPR`|WCVX<`lnE$b4*`_
zV*WSN9|gl-nQljZuQL60l;Jg|zY`>{GyO#X|HkxhLF);o=K$QN=@+3MZq{^~rtH`B
zr%?|FG@TxfyG7GqhRj<v{YPjsw`qD8%=30l{~F47P}A>&)`v9x2I%<?O&@|Tw`=+z
zQLw|Beldb0NWqiu00|i0sp&Py_Nc~YehJAX_BCIGYW3StAp2+5p-M3@eH8@u&r-r>
zssAnN(Ej=BfY2@VcM|47O3bp<cO$TWff92q^<EP25G58^>YpZ_FH*u`X`pQEn>BL5
zUHG5nq)Y^Ki}p4^$b&d^HgQN)KzlF1C-SIH&h<r-71AC6_=^fPi*wC+4Q8<SEP%cI
zAl04#`WK31Hs_vmG0b3p%UfW4>h82xfYy*O<vnLB3~lcpM;&mIc|b2Fk`QFh?j;@O
zybSlSC&uA;oMZ#gn@dR2@Dlszm(ij~H_ZTgSDr+u8$os1cemh|lk5ljND0Za$ijZZ
zyPz~D`6$r;sz}(J&r)IbEr&rk_7pJRC}8g<4c<N*PQ;b`B`_}#mW(gVJjad(VL!YY
zP)<??H<*EcGYQ@7wja9`F2zYM1A0vni4^rYNVVVjDjMj=VL2@bnyZP9TTPVr5hD5E
zV`z&c%8~|pOo?K1{AeupkA4J7bL>%I?on7#dFQ<X$@Wj4g8&j}xu4Jl`K*=(B+JvN
zKHHI%$gB1w2xdNp26MWq?b`s?SD+YGZ=)fTY4s0+y6OhfwoI%480f2Rq=ZeYzvluZ
z_EMrotKR^Bueym6x>jEcf2rC>iCJ3xK~!$l&6Jp{)h{HT{iN3dt^RY!tm*(I99sPx
z^!HV_5ZPj_euNUYkzz}<`YyEks<%_3S*u@0`5dG~n^wP`$PQ7$snvf56<qZWN_e&U
zPop?hw^O29tA7g#I82FNt^P&A+(C(*T75aG^G-?(Y4yLNVjrc%s8+uTeN5FcN{ngs
zU!`Kdi>3<uwE7$+-c6W;TK)BCOI7coA{^D~9|ElEy@WZg)gMLWRlSceC$#!{%I8kP
zd>raf;y5K9(CWv@RCiJ0L9M<Co?3Me+2EvB|0)^&{UqQkFgzvhrNq~@dMELGfD+%<
z8mQu`HfeXEy6?mPtX)u#323`^KOhe&=<HFBdXyMCwQmCa1BEj63{}2U@Xp;J)m{Xe
z+=$B*+;cvJZlfx=OtonpRX7L#BHKB1tF$Wo1bR5$3>txHEnt^IR@L@a<j=8wU_u3K
z89ck{n(x5`9J>pc(E|2LjAB(W1GUew2Z6bxfOSCSszee1j{O)SpCs%JRO0if%~WNm
znGn`;Qt^o*vD(wX%>6V(+p5=o56E|+z0dm;((fjAt$|S08@1PfvH&&f!`zM>>V&gn
zmZYGwKS8MZPeYw*w??wG6~Jv&c$1y0H;;hye$Z<@K<`u}Ld5wu15q97hMlRNHH&WH
z(rzHK&jQE$li4vI!w;G<;TMd+Omk~yYj=X~4#XGIPepx&cR|I)nl^6`DN(2kh*G0z
zz;8(87a@ZyH|tRn`JW_Gkh}!61+*n3>_U1zr{;X^PiQE_4HX&Xsx1QX<;3tL3Ayl3
zFm8=QyGB|cWL!CI3rLcRY<9kXl0^dN?CVGqDm`<|hvx`3nx>Vi!secbrmNT7K!x3d
zZeZR_q(wO|dJ+y>Q=w6#vRN)5LhY*{J56$aLODD`YBms}W}o&9z|ScZo4*JCQgc8f
zx!SAvsetctE>V#ef17M@+qY3Rsv%BxF-VpuGPdX(+Ct4c>fnKt8K|tB$_t7BQ9Vbw
zFN&jlHFsQwUY&XZP?d42Bq+uel`zGk3bN;WHeDb`5Q|>UN$&#LeTtMVdKnF|=C127
zXi9+<oazx!d{a?LffvEMYwkM+vq{oQPWn8^{#ryT>0bk|=A%}0e7vn$bQ`Ci4?rva
zd5Jiws8kZltCB79lZ`)hKYB08Y3G~~a3%jf=btc!)O^;X`UZ<$#X0W)*XN5ld3K^%
zz_7FPxl85$2_~*Nxlnab7QLDa`VoZSQD(F9|3DDe=v()}CuCV_xF83F^yUR|uP(Uu
z-xWEVJd`cs3jTN<oL0)M<#NX$^I!j0a@nHqqJ`G{)(#U&k#$_;w;=9^B$BrR(v2;8
z54p`BX#PZv4}Lg<lfDA7*HrFo5%sn;e|{W;uN0_rss(VN%kVEefY#^O(IRJg?Z*&C
z#nUzc?NKCb@k+Qt&BPkYY6qYOVMrTx$#bBtv1oTfX>BwQX0_DAaEn~(=?=5n72kr|
zwgsU>$IG<`HeB`$y35+N9QNZIfAZ(kFFm~OvN4#lb|Z&54}W%S;r+ASOP)out!@7R
z>~iAr3s5fWs!NgU6<<cqb*H~7=T<sEv#AukX82e!TZ0GA-cN|)AGWj=EqF%%Y><;p
zI3I0S9EV72S=n<aht*bgGlB{mb!ingnuRU1(UM{rk5}+`g>4<ASJ<dJDr}^6g^h-%
zWj30bE#vVD87G?MB#e5xMiAbGu*uerdO662(1@_iMx)s>8^tSlyuwCCtgt=KX}*Ks
z=4CeOH7n#VRpv4qErBZdZwqyZ*1v+V78So@2@1g@8MLC~C@`y~I$N0p6-FNGA`op-
zggl}MSM-3`4HT=T5u=udnid*6t=j?IrFeKm@vvGNZCk_eBg+64)_NFpt9HTMmI11r
zMz-36b%ypS0{;S@tA&Yd)e_XPh_W?vp{cf%z@__~+WQy63)+BTHbmBH;d>!--HpW1
z%v<F8jU>H=D#3c}#NPE^Kv$uy;+h<L?Z)+2LpyCXRfYA~zP;<W!g;hcB6)D{`h)0O
zRr2WG^`C)%s^sy#8&Xi&TH~b58MNh>=G%w-Hr@o(m9-N0AT-;g_1LZSkg2#hufpef
zw|ptjdtNCoJ(g<XRroyb)^mB@i%NOvK~@v5!lQ8f()=4yFem9u*OlufKXp9NTWi^*
zMeNu?b8<AcW~ktbC@-XIzY?;Hg!D`)gdUU?7Z;+4@<IZ%g~T=KB_XLPh3qUAqKNXQ
z2u>&=w~&x=Ddc%soOO)3s024lcr%E!ZYNxZXZ!&1+%sjy^q8)=+V@wBXb;$6BXTX)
zaaaOr&YUzOel8JDqES935Oym)yi6CYt2J-Zt|A$3Eo6mTTB~|3o3vhX$xYf0>#>7-
zH(B<;LyuBuvmQH+%zh_jQa|@gC6gX>=2!bUiGIbThv$A-ETu8K^79f>iJlQqO9tGX
zcJP$mLD1fR9qhN6TJI!O*!w|%CnkLFLxBEz%d;S@_&DtiZ>d8+S@8*qZoL_$&v*7)
z-%Bag&2Rk#rBnxB>w^~lF9UR<>l^sBTTeK4@?r%})?x;Pe`>(wpQ5J@(xk|;oq$x!
zV)`O_m%wQZ5I?~`A$k8#5p<9gbki$nyJ%22L_ns^)Gn!OsJ7OfU)OWqOihO!EOVH(
zZe8{IY9H)uUCgShuLK^eX&FV2RXzhyl#&9Xs%+4#b=B3xq#<dm%}Zzl8FnMMz;a&$
zC99NttQR!R*FYRK7cpN$6Ogr+B7#9}Ed(lYGs=pj>q{VR0W31pQUu4kM(N#Fy#lft
z8nsRH1ZYL|hIuib)2wo2&`JiN2nPHDQL@h0P#`>SDg^C^<uhH#1<s>vsbsD5DAV}`
zHh=X(lf3f_2+GJ`-8EOCX=6=EEf-Q3E`WAzbN##^7YJ6$Ko#S{ytMLCX=G1_K$8ZF
z3tQmkdLUgd&I_qd)|nL|5sS>oVtom{*o-ddbx9r!y#yG$>#dke2#?w@ZvlF(%K(rr
zCICx{08s66GXOK4yjW=JYuGT4R9IRHgp=#iBbU#Xp4Euf!J5cXsg^KxxD@G@Ms0J0
zqgr^xigI3o<P$3i4n@lquC<Vb4-Bn5-&MpQii4f~M$}yEJO_9DHD>aeT79K+?w#$N
zr>$D+IggZHyN(g-TtwFu%D4bUUSCMkzWs)UT1x}T=&jztg>NipE#SKH(0&cAmZF>9
zu^&(3&pp--awlOeXudAu3n4ef*De?+^A5#J33s0RT_nIGaOez0iHg+UY*q&QRg~ER
zED?P?0_C?9l;1RO15CLUuY5I7UHf?iBwZAtk_Cj7s3KcIOS%Njh7+J}MMX5IvnSD^
zTG#-7HLPrF@vEULqs~`*1K*;PFCk>aJ9~*_|8OoAZ^C=d#_Cr2((_<4*VLE5`+#p?
zwkjj}Vqf8nLA<FrId|MC#|wU3wIV(L4>f9vk7d)*3}ezjD;xVqN6Gl~dSGfr;}GhG
zb;UE8=wRF%O~&w2#$e+RNU*j5Ni04963p|ncvCbZs5vL1a3j2Xm`I5zy-ErK!RbrJ
z;$wL2A*%rL?NXKOz?-H}3%B08)2UdlKTp>c&88Fm1wouJn#|@U<Bb(?#nS`P{y3M0
zLAxiL7|s)5*s`d(w{@xZ_WP{*2dvp%Yt4S^TP>Gp)*1V)nJovbH3QbMUTf8s`<Mlz
zRP6=|?klrcXMYmOA<J$J@wxjc{u_!7AhxU5a!Es()!L_7X26R>o`~UcyW4|30k_fV
z^7%b(!x!lExWc}6pWheR!H6R4>+1G<jBvyi=?NR%%^cVh_60T@A<yO>zboVmyCS||
zfZ+{OdN*}<Hf!Kz(GeDo7?AAg^aVU_^ZT?dYbIGQ8pcb!;by!=oEb``vwgV%k+;{^
z88O_Rh$qzL3-DZ|qO9b-VE)4BaH{`$Bg;7hLBs0_hkdSq5$^VM_&R;iVT$6kaNu@V
zC_n|8Qr)$AUnppKbzrM{MR_%^@v)X>xI;1%!8<e=ykCqLk_&vE@Jb^Q>)sl}Pb|S;
zW7rn&>f#OCEWP0>-UCjH_kiimSCpT>zRTZ7cE<;zc&mvkdR4dK@`pSw_YTyQFXD6g
zePI-=+i!Gv+`gVJHjs|TyW%6M^f;&Y2D^fhVD}DGpQl@yj?B;#aJ8dWBSBQ6%WbrK
z0v+BiS7@ubo{Ha>=H9XmUcwo-`yyV$?+f(w8o_{Hc#0<!3WiKJBn}@OpchJVPg4ru
zo$S2%3RY@q9gJrKxskqj+Lca6$35NM2~lO+UEN$xo*yRKgO{1&+(^a6aRprd9bu1g
zRd*<e5_YjzDv-){7R-Xzi(|fICY}}+^=$W*sN5a$1tNaWHjm%v@VWw<$$1Bcqk|c)
z44GZ(VMMNC2ECMyx4c#DgoFMbY7Ep~LJ_crdpbJch@CxtSUS=Z3b4JxVK_&~6NZsQ
z+hfVxNGJ~V_{&=1Y<yHm741`1yQ?GO+vbtXUBPW0bMxh3PaxbA^7ws9%Z?o#sHn{x
zhvwc941^;g7s{vziraiVNw4R7-kPJ=))VmzHDg5qkLl^|_JkO8Lrw9<-ZyNFX43|B
zuRM#qUyA0c?0_C`B$4E=x<&BjKH5sZpbrEi#t!ln>Snq;0Wum@DGbxoZA>biFR%?x
z?>1ba%{@dNCXdZ~lWdHu_zU#>HGkoRg*q$gvv>>Z<Q>;!F>7PF(cuJMjgR96|58Cc
z0j`_J9YFQ6{?w>waoB4!x_V%<c8}2=4ErLovb@0vHw7Fc2op}#)0S2o+>0_bRoE0x
z&;wsDE|bxTR_O0$#%L--J%iV`*~<l?PXs0J2?E>0A;!xdiVr9Fc?=j7`Pl-6^AZY8
zb8R#h6D`vtU(Jv1E;PO%UVQCx`Hj5qchZPLe&Y{rhqGk%L`Ma;aOhxr1nx)u?e<`3
zD?(En7|0C|i|R**74}{2VX<^-)bm!17&yBD=MrScnefmM-r2-Z&xxmUYf)sBlRSw!
zx?sS=sE0{mXd<EQsa!HvBD53PW8{lxFv%j@mkf{SOiVoq3GpT4MY{3cJKBGJZq%T|
z7Pt`E0Qor9LGDeh=jx84Tu4FDw4rSg7rKa`e;ai|lO1$5%7a%<J&Do*2jdJI9U9NT
zdGSJOaxgn2isMJ0mT&)InIlK_bh}*<PnRna@}a*{T@u_U;%cWs9(`Z}l2YaYqTnDc
zE7BNrcE<}&GL;*((s2#&AZ}SXjJYv;KtC<od|_1LWkx4HRjFIaNo0#Omxg+h!eDRk
z!yyvr#oiV-MnxI(gWmR#FXAyeG38;2j5`q(ZWQuRhgvk$WQbdg6osq|%1L&_a%$qo
zU<ilzh%T&1>y}mF@sW`@O$$ZO-$rlmCgaicq<I4BF_xeyg6Y%}bvQ{Ybun_51P8)V
zE4g93(VO92s+mFdfvq$ZfR)H2)A49b_@l~t`Z_mUVc4A)Q8}K0?o>2AsyS07xwg?j
z3&(M>K59*co*nNwyD@@n$&HM%d@l++@O;Vc#DsB$j0(e{BQL3;V7DhAD#%D;1;DXl
zg5e98d>Es=yQd?<=RJa^c=Tgku3mJx7}7TzeotU?M0ACddgJ2Wd~ZpOiLMd{!!UJ(
z49^R_Ls7d`t;TdV4U3U;8OFefC<6LmK7-)%CkzMyw=3kPo^u;IO0uqp`)I+}@=cMD
zVVVz9QRIxl<R*o-xT+rq<D_N#fI|~~(J*T6ND6E%&4rnfY@?zj1bZSzuv3T#a~(T<
zy)voBn&O!ojXG{md(UQ^|AS%~BdHk9`!RK2%BSTxEe6+Or=#Lx+TwAA{502fd*Bal
zaud<}cMM??k-2^x6Q*c?7S5^K0lZyI$rzk|7)>|t|Cpt4?elY8!_^+-b2M|`#YxmS
zD=I_uO={xNiH09kYaXN1?-Dbsyhq7}6#8?jXgYAr(Xn)?c$VC;d?cGo#y1YelW}wh
zNJi8BLmS0mW{%~9j^)9X_+Q~zj=G>eV);NW$&VAr0ECkQ`mbL;noebL#6T)-T0!tb
zg2-Zg?#iuRv{)h(cRQ(Ehd1c=gtv-myJ-*>gMjj`QZ6+M6l=qHGLuWkX(dNTJ|+9*
zf<rF4sK&7HL;W*jH!R;++M-A}O-0<kkOxy*%$Q4cGdql$*U=FL|6qAF>2#=)39Oit
zkXMK$1Rtj9(1(b5Y=;L2Xu?2h_5`*Dg4+WI9NOH^DknCd8pY?G1LE`|nj&%GZ`^JT
zL$`_<%ZTh)_sUHPc!J27pBSa84U0qS$Ym!^LoWtHHETkH4W;%JHA-n?no|HZ^_*hb
zT9b2CF@hF)FXc)}TH2Tgtitj}PD!VpHKV(9`&^rYSkxIVwfJO7n!S=L=zxnxYl4+F
z#KLe!?x<GGDICoeMI+kCxaItG5S8>)xje&sxDzuTbB&ui%NEn5){{uk#1a!Rt`v0k
z^rGLx8qnOW%SD+x#Hat9QYxhSQVa*Q#S+-zqY912a6}jdD@3qT1g#=i#c-0^pd*LY
zH<-kMC5a?$y{H`pbzYdP^v-CSPj)lt6SKK=a$9sbC#R<7$zaGMM*R*K&Bw5hL+b(~
zH86mMLXoGK?649A_hb1aCw`(Qmvg#;8{`MEc3O1$!=hak?<{qpoTec{dSgqoa7UW8
zlbtd1amSNvZ+=gP8?w-)7na)`QM#lG1i8OR>#MB^tc5+HFuEb-uH4P}`qS0Zi|)=9
z+JTlddC*l#t}w3)F!G|A;y@MZtn&4&xKV)4;lwCkx+o`HaycgxjmPK=1-ndZ0Wsbc
zsxXnk{tos?7$4@Dtl}_Ek;!FbEl*{~)ummWoFHH->!le2#w&NQFj&&zalB=4&5Aqf
zDfi<FCNqsE(qe?eenfsH!)ej?iwW7(`+IyJkh?XyV)WTu4>64~&Aw4mwGXGd<NPMw
z-buTRqRsH8V)C?tH@7L<%K4AlK~&R${5l)W1IOLbX@h8V`~aW+!fJz0$PMg7<K#o^
z5o2!{gofBRrTI|tz6jZZ`y^jKbOb}8o^G`T!iVe<k5-FGGD49-(d}&Z2iwt6c3}*5
zZT1+LxOMkL_#y}GE)weD>tA0O8?1bTjhmFO_%KE0JFTYr!eAdXZH!G}=#~}c?MYf`
zn2b8b9&bNir_##QOjxjf3-u)h?eg1yXnFbBId56T2O9BG6vjESr`xy9Xy0L6?Fj{`
zWA+yozy(XgIKfWtPuM|{hBGp`KEsFkBb}&dDvWC8<}F^+elG2M(<z16Sd5R+E*`Z`
z(!+)2w#Y)Q7HLt6K{`E%WoLLanwC2RVy~;vU#iZ6geWJZ3J-USxu}}I(Y%~WsfbAH
z()Q4@vS^<N^^{+aaV4p3(?$!;cld@CZ7&+MmCn($xr0FhL+uvqYQgboMa(z*2GglM
zg>I?Xk3GE|9*-MiI#yqD+4aWFf^bKOpVyU95phchdsLAWU&|Y*;g}k?=w1(M6VpB3
z5X{v&X<^>7B5xW@S79mYT$#EvCOTp_-}DiOaA5^9m7lB<vs9CZlpOUbY*pBkR8y8B
zVTFdVHt)kwiQ@V3mgUEPoQQ@6!Q|Rs%PXHYIKfN#mJGU>V`TDoTZ-=VbgiVkFcah(
zW_)>%y~602*l&<~a7A-EZe=m4rnS}-+O({hv}%A_7%e*shng{dU>XbO`gUQN6G@4Y
zP0r0Gcj*c0&UncR`x5+?2Cr@^vFzbW)@}TD7w7BrV3ShqjAHsllS{tmEq8d#E+cz_
zR$A3#kN}z34@VJ5Pj!wPmz!U`7S^sXD4)2n!o4C+svJPI`}}sR*@y*)FmDB!to#NX
zmP@ThZg*YP9c<r1b0XgH&}8WExMzOf4<!`wB4TN_HH768IbQ1g%${;EPtK{<rko5Z
z`-sUw-uY=nC!P*hKn#oWwxq}K(UyU_lQ|h3?sj3`XEw6bls3FYXX1%5IwO$F=iPL;
zhnpo5jO^&f7GlAVFb1g=GPV^-AF(!3vj_CmZciA?UF`SpnLc)tb9552XiNDWVN;E&
zfoE08O)NATvsV^8qfL1joD<^=4P*m2#1PYWeg|s><6RTO8ZwDtk=8ueXeg`-F*)_&
zMh!Mqq*Cs5VgL)9bXpv02${LG(VrU5jZ8U0t`?iAP!E#K`{*<dP{TsCx;ngax3@4D
zs~aXEPcY>6gs?--Z$hbwvYOVCcdl-wJ)K;V23)Kd(67_Y()?b#-0zxtC=CT+chmNn
zdN}h#^1-|+dUx50M?3I-0RJ`psAn)uwCVxNNwn%wOB1bnj&c&MdZN-qs~%mQM5~^z
zG|{TZBq!01$!8MPvx(~AS@jU2sc1Z^p3pohZSg)Cs;4tc>D3boN}hU9@w<|*l%6$a
z#z(NCM3_yBa7e}AjPdj+YfNHis}V;yFVAAnRHX38OCzmnW6KjU)<~(LXl95t#>SJ7
zCqj%h9J3quVbKE@B@E!w7`BK(#zt{3y^)XXjak}RZbTDE0GHxNXg9`(jDd7?ByJ4F
zz^hUa+TU-)$NJ-=S%WW(g$yG~x2lB<72nklM&uVAN%Vs+g)H#X$nzC>;FNCGh=V!l
zR8Ek_`*MR|jwS~&p^(W$av-JR(Z0TPe78!`zMzW3f+~s`n~F+Fld9l<0l+ib^sEg%
zW5cgDi&KU?$e*JrQi4{+DO`kqyt$C)k64^Y(RV3$z#JmT|KdTNqN@cdGk@0N<oK-Q
zU(8d5pHhA|P_N*u6Q%!gi<1-chw%}M@f4mbl_4H6ExH0o;R&+5{><tWcsyN1*Mm|p
z9^&D4Qv7(>j9=UEOQb4)b)L+FGDCHKjG&%uKYDVO&Tdis>bw?woq}I@z?xr5e?_Ol
z>w%@SU{v~~&B^KM^?tfWMZidZ(o^X#!R&2_kWCf8I)A3wE-2`MZn{+dieHug7U0bM
zJMB&)#~B_xz}u`MBz`=}ozGwK-vJnrkbZRSt8&Va2l;b-L`vkP;uO-e#%BH#lK+H!
zg8j6a7;#;uRrsC~e%0;KArEvtrVJyJd48E>cOzoXU!7OfS|=!^85B9URX%~p3H%ek
z%El=lI(JI`<AgL_%Ki%UaYW4g>boxLdoLRlp$w_6%-83TF!RsVog6(^8mRRj<bSe+
z-#=6E)BA07T_A$|Z;3b@0)a=e@h_X3={q`tMb(oMP+I<90D<S#^Ze@kr>F2{4qc`6
zrRnEELprJa>11rWp7|g0{~oleZ4sY3|44n`M}5af`M1hnmFpaM%=xSD0i3&#q)o5C
zlJh!f%>2x;lh_z@@Lx72g8Z-IQz0F+V&+%hQyG)|N>3#~@u_qTm<gurcU0zoQ1YvG
zuINkU&6N2!D*_oR{-q^w(v>b1&X@cGTKN66L&Tj05RWN71-^V5{+Ats;yijl*mRZh
zubPIxN_O1;Qqtf``8%fJ|IC#l)2CF&Aay9^4@&-0`yF-(22J{rDuUuy;nk8~;Z=BK
zr-=Pb0c5E76gWN&e<CU<jw-@w_^F#vzGEi+Z^@r2$*?5J{h-7|T+APag|u@txxjS^
z{@3DP`LFUz>Yt~||4bGLTz8frOY;|0Q~z3_U7N)@2}$23QS&4-(h5z~{HGXRmOr<y
z7>-fZ>NJCR+=?Da%|pycD`r}1eo+j!F*R=}hTCznm(vX5aVx9JpIc`}S}~K_S2Y!V
z4LdO<yzrf)VmXt)TSRy)-tym9DyG---!&_S&%}{VPBVzdt@xsf8mG)iD{IJ)KgIA_
z`Evz};j=ML>@<UT+{)(U#{o0aic|ko|6UA-H_Cpq7(V&?R)n{jzE@RDPqX>Smvz!C
zUeH*X9OOz^tdmA<MPJgHNIolblt@tY7fl1dcoLl2RT=x^L@9#i1{PL3={-4;purC6
zWj58W+oqAz!RaU0SGSbUYMqD`t~jEUZ*vKJOu|dg!@L7<(&xCWM=B3p_u^l}DgSC-
z!h!ht3jRs{(GvNmIsIfmoUU9n|G=qC%q!<zNb*a}3nrJBkW<;2zVD{7$$oethfn@)
z+ENMMSK_yAyx<L!%IlMGb$+MNlCigNIg{H9jVn`qe{B?S`aT+chJ`NG_wSSN((Q@2
zb2*%}=!cFdbQD2E;$OI&$%rpa1OFk1PwuaNHVyqBr-9QvaVmY50Y24t<<?I_-vjtm
z-^J?#oa}Z|`WyK-U1<&%ar5sxxSaWuzL!Vep_}UadiP8t=b>rfPfP><9hX1Z-{`=h
zdMN0ucAC{>bQB=LORuHVz{dce>U)2OIelR~qHluGb&SKO7}tv4R+{Dpqq<e+Un3tj
z@v#1M7B>^|0UUgw&e+m{2f697QiByfqGx1BjDGrz9zD$=KDTEA;X`RDV{kathiBhn
zc)lfLL~~<!I-R}&la0q3SFc?|50Xql#J{^Hwy^M8MHVlRh;6V~Ze(N}ET$B{*Ih(v
z;Hd>ymj_QTy6IDZTyq(h_f~_Qo%}g${`Lqc@GZbRbu5!IhUnETJZ)1f8~2LM0NiTF
z%^t(OBjD=tb+D<w3CFkd@}G+Phu<iqXXl(w^}#s$NSyg;sse^SB`3a3C!ek<pac_M
zB{P4MuLS*fzl2!)tve!BRa^YY#J~3yJkxCh^(X}YFyW+c0!~3_euT}0E`9;XgislX
rFCwaM4^IB#q0?#lCSUPQP?$%4w$CK@|Njn_c#EOf_Tqj%-|_u_*z%6W

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/example3b b/3rdparty/qpOASES/qpOASES-3.0/bin/example3b
new file mode 100755
index 0000000000000000000000000000000000000000..25466df8c7532a88f8cec57d92eec167ff812b3b
GIT binary patch
literal 30848
zcmeHwd3;;dmH&N`7s*>*>^LMLfFKD3NW8^KoQ+7f<VcAu8A;AU3oo)I$4YF;NOEE`
z5JJL|v;+ceA#{OsI_-~c1GBWHWd=e+p?gCME!}BLX+oi;P?-D*#q&LPk)C9^{O0rP
z{Kxy)y7!*#o_p@O>wE9M>plK%i>5JkS=eQaQs1j_a*+6CY;=GpC9H$lnUh_}7O)CH
z%JJ{y7z#y9g=eH_71|`76=50vJ1hdwVR7<^3g-(jQ=!t3Ldq|HIb~FYJ7<cd3K?^7
z0P&HoUq6Q@5pL%Z6&{y*JSRiNM@iy4CHYQCJ{9VcUWKZBR5tpLNPNWNM2gZ*3YeS!
z=0l>TtM|9r7{#BR!IKD`Jfgz20?bsX_$~(@>G}V9>5?!c^PA2t`T7wIsveq$6a6dN
zn}=gdhZD)M@ulM(E0(TkZ_1>a+IYE1KGALn^l?UI$2mxm&a?1OK0x>*&YM2%+4;=#
z!)L$erp$*HTz9pl<1>UK6A)h|{*UPUZ0!75wYIXd+Jj}xMYsn1&%^&td)E)Y@|!!a
z?eskJ(#7Z8I=^Q1-PLXVKYjUgn|^crC*S$az0Y2~b^gxlKYZ<%5C5sIX4bFoeenl>
ztef!d`XX4Ty-2pB1onby;1eipeF^&EY2bUOfghd*zG9kuL({<R)5!Tg=u7GO<!Ru5
zng$-8M*i?L^w*;zbvBocJGO%&fBh@u=%~f}i{STh_*{0d2!1o*GeLhu!c}{_0rDLs
z<lh8%J*&<2XDa7Wz-Pk4oOMnPQtjwgNq@Xnz~hqPQ&P`jeLf60$ses2^c#?->n<**
zn(eD{^0fNdF6Fn#e39hWP7beQ?Gmo&Tey72oKn7mGqk^}M}w}gCrpiQSyKx$j2$DX
zq>;%+(^<n{#>U-7D83_+$;Q)N!_iD8o?(VB(qqKpX$g+>bPcDH@kq3PI4*FdF;@gu
z%Dh^ar^k}P%X|Cd$$_DfXnL2Ychf*1Y_zs^rc%T4XfiS}8ux@Y0kkqWnoXpV8BZt?
ziiKAi;*S}5VI3nAz3D_UJ4iyz2*uiwiDx6Jp589c?ow!XYiW2F!C{|FHa2kSrDUEJ
zWSsGFV>F)5q>|C$M0Ud1-9j`w1_q1_k+d?SFS#d?jKOG`v5`0-2NdrpSJN06+Qml4
zvKjgr7>cGDa!8G3A)JV%<wv54Bs1c%Xf_H&e<mZ~03~Crc`TEruCqCsMU^b=k7nW`
zy1IENH4<-5WQGzu#?q<gfoOJDB5CZ&G!G0VVySeZxqmD%9BUp*k7gz^&CAlUQB?Cp
zv#b{tE`=kI{LJd+)M&Ojoys($NQoUO0={z|vH0%zFp;Vv4P?fQ!9+3v?+BtCF{m__
ziN}DZ@~M)E;u9@Mvus0Gmq8BMwRNkj(`U3cEn^$}zRoVA4Sw35i?=l`SKe&FSYY9O
zy@H5QUc(ro0WL!GuWGLZ^YFIGm;=nc7fw_Y??+|BBK-L#{0|=3h3P{X`==b<!ulni
z^3Kjp*lA>X0C*b1#^zDl!XB3NXU^MBWPDBlw%n!qBbu9-``0<LpQ16$Oz$wktvM--
zEjGbb842BDf}7_*9VWPW{CAq*C_wJ=n&4GADC}l}!+^PKs|j9{gEF?=1Xpu3LJyhX
zwI=#e6THp@_a1o3=6!$pNTY*!Z#teWKXo>c-UE-@P6(pO<^4cSE{frI#(7S}C_O~T
z)6Y%fT66`a$+V|W@^pyOR9mNy^YlhaTPgh@Pj^z9OndqmPp_slne_A#o^GQwney~L
zp1y?ARDGvMdHMoM)39@TJ5SG}G@1BxFHhG~noN7z$<tMoCX=3SK|1Wc;Uyi~dJkW_
z(0OVvAR=}nkGU6iygEa#ZH9XO!~TVf{fFHP9bUxddk?u6+Pp}%cn?PwI$Pg()Qyqi
zN%ul0Bs1@!vW1AxUjoUm&Y0<6=s0x%0%6b#9^D7JQ_GP$@GY5F2jz9c<D|x+XWw(;
z(QAp~-`<{_d^AY0Pg87=V)r2iecTHdKf*8%eUz;H5dEC`kixV)(o%@|2ZiZ)WP2ed
zN^;3Cr*;UM@*t|^ks}3Eug=(pBA;3giq@Z=_H(PiQr<%+PTc^Q_s}m--Ggv&#(6hF
z5=#F5@^3UaV1V-PBRJ9evhUDSS9uTp+I!&nS9&A9Cy&!W>wWSu8qd8?K1sv$sWX3_
zoc!~Q^Y#({L$|{*`@BnOT=yP0Q|CSO+dY;mPdr38a>zel`6!_#pHuXdQuL3Vv4Y-v
z=&RnxUR>|hPI{j@GY00b2<GpD*?Z^-!T)Q?zi-|7(36eL>;osS|0|y;uLDSX?F9L^
z_rN-@4ub5tITdF0J^CD09iX2)dFAPUfJtVY$4+NN=-T2sH0j#xJM@OD&v)n<SHypK
z(JM0?Q1S56GYG@}i+|htQ}1JcwNgtvb@(mV@TQlu=PLX8554F=^oAQUCTIQ3d*FoT
zUG@917pVch=StUmU01rUav3KE2b&cAYbRhAYO}J5Qt#Y^F$#lTM@uuq)WFfH<^~QL
zNr$tU)ofkKi+sVCbfnUlkvOs`2c{N|>`)@(h>u4{Mu+1|922QAM=~Cd5jK|ElN?S(
zV+!j?rkWfrZB>Km)Ckz(j$jf?k+`EkgRa!@aD0H~A&x!q{!AhpcMK-dne6$h?;DAZ
zCq~9b9J>>#;V7rW{E5P78k2<84!RaKEsIZ2KNyc@68#C9QwZ{RpdUz4*+$|?%+mx<
zDfwN!eU5BmBrZjAJ-8-=j*-Oha3T{QNF`&LDy0{>9Alz4V9wi}+S%4Mv__<k?CXD-
zoIHr|=$n(1ClQ{46P!W#UkJBS8ts8PgRN*-Td8^Dnk53JT@zy3xUQXDQ)xS*Rq6z%
z{{C+8ULt8IC`}KIfFt@2=s<b1I=#DY&c+$ldu;pI`gv<ES+?*30;@4ZyQbIAbF8$|
zl-DZq6>><{amb-LcA0;sley~j1C|YS$1Ijxs_OKvI-9GS^tcuOr%+Z}BdW`d$Uou#
zB=n9Gu(M9T-Qua6bDPyu=Qv#Esat$&xwo$6<_d3J$AQY8I_Gd*hpVo|Rkyga&e2&n
z2lSnFHeSw`z<vgD<dlG<@oREYn^%jG<gD1gTG#;%v7_1tG{nDZq4DM2>s<@HYk_wy
z@U8{kwZOX;c-I2&THsv^yla7XE%0}-z!7=B;<&tTJ1WykxLnrd_8?XId>K(`+DoNN
zrD<=JF142Wl1$eKaPIy_txT)?7jtD=-Cw0`J-TR5l`gt(N7u9ReyUpk(Z(8GufIK+
zqVTB2NkQ(O??dGxP3u~IS)Dvk>)<ElqD<{!&Jk3sQHCn8stC4o=u&Grhb*63&(g*p
zU1y1aZMQplOu<`ZTIt;)>C~R9${$N>UY_GN!H?Y=o>qFiBGdaMUW)l&57d3^;*7pn
zCG>nkhCh_y3o?8|h80+((=}U$7s_yj47+8xS%!l$<V>a?+%s71Slrh?mduVh+M7C>
z+LyMC@l@;H*7l~B_NHYQ3pl^^Evn1L1r*<UQ%yj*tSJ{BZGdl<@t95PhToR8Vnh0~
zpx$0YX<29o_UU$jt+vWXP?c8A_Bgn$mdc0Wu-3B56C7Ca1P7MC46i9GE5|J@FqC(~
z=PV7ND?dOx4wip87ttFj3TZckvD{W+yMvNi#djdUvSK#&Yg#@5NaagFKU?|w0+iEs
z34EmTjjI4=wkOf<D&KrRMQsDft@163>$Vr5Y2{xjKF78mMX7w7;tpE~?W}T=;*0HH
zhOkPeQKH5EIFzZ>c%s987o5J*!V^yW$B4a*C%pCu{JgT9CwlEG(da5Gcw(#FO<LKs
z_dw`&`*V0YqOwYhA~9rdLbX)ZXr%C{{bj=FJdw5k1f{C1*M@-^x3`l9v$akn_Suv5
zNSvkhBC+58LHt(E(RM)E0sAiEnXm0Z;-LK(lvt$g0l^XbFYu^GWs`OYiKF)4QlgnB
zj@e(LL<>*cZ-0#vtvvCd{V$Yg(=G<jLv|k-vz_a9-2O1BvqHNL`8>|5zC$W}xbh32
zm{WO-3TgX4=wd2APEls7B(we_71w6_CH8hIKSib0S%vKqz$$E1ht+rBht#gwj|f4j
zHaYaLf;Is90VHaPww7enEruu9ZoUxDAIw-?4R^BL`4|ZF6EG0d*T6$GHsehgwaoUB
zR|&BOD(=-6z~<NM2D!ZcU+@b}|0e3xqCXGitZc@;(6`L?qXnol-9Rl~ug|9rS?|KH
zre6v+i+&N>gjL@Ta2cD?h3v|#&X+-}d*EBw>v8ZheG&RWO@AATS@eJ541Oq4rdJ@h
zay>x~tZU>MdK2I_eFsF_^#(wy^!=c()~|z|YxIwyB5U<$&_e3;^^iG3-%f?mH=*1!
z^{Y_6dR;?ZHR!d-eU?tQ>l*dTQL@?khau-I{fFS1qdx{s=IT#D<~;p&#Lw3MfcSj<
zY&5%b^ar5Tx%%aRpQm3B9US@>Apd;574Zx7Z=vlk(652L7wQ_ifJJ%}taYLOTgbmi
ze;K+h*0V6c#rnsP?<M+9@LsCl1+A9ouc4$%bwBiM(uYw4&H7o;r$xUF_0Xz^AfZja
z1;$;bUxHlP^+s?l*B^z<75cp}<Vt-vWOnG^!0#&Eja*jizl64H^aONRt3L~8Tc<w{
z-plkMlxV$v9FmVSy&9m$n7$17uQGi%hSL*FKMw65XL<~c^=nMmpxYBn|1eTdGW{ON
z{yNh`F!(o^-UN-m$@C`x{1(&y2s?b6>F0s+J7Q%1F4OOX%<nP%KY%;Q^q-;7Pci)_
zkbEC9Vb&io{aje|X{LVysy@T?KLGbbroR`({Snjm!Wz#q{mUrWkD30j(DNruzZH}}
zWqLD&{fy~9fasrt9l<Y{{&^VgmrTDD_52*u-v-~WnEon&zh?ReP_W-H{c@D(d8R)P
zFa9mlAA|{BVEQ}&f5-Gs!c$K%Jq?vk!}pQ<i%g$@v3}3=XJP4=n7$Ih%a8&k{=oFF
zp?rU2`UvXc6{dd|CHfQ7Uq<jM(_aF~YfLXgQU1*I3fSy*rU#M38!$4e@V{VPq|PvX
zHZuCZOn(dve_^^E`Mt^XzoHCpG5vNF`mc});M+|9Hng5(`WS%wH2rGGyg}1xnzCQh
ze~EfHpy~8*+>M(4G-TeS>GPoA&6?f=^W388vthG?n*M754{7>!(DQwo{#oeqeog-q
z3U;feUx47SroReLzD?7=4Fzx4^jc(lL}N2+z`xkO@?oe}|0!72{_m>+VV3$QKw$p@
zC2W@Z-=PleU%Cni-BN#)Fb`2;mZiQIf&I&rm;($6_zETFTk3yFJP%XCVQHXj>>D(4
z!JYV@<)lmmbfb0+Aml+D+DIG{70^Be@Ua}Kn{!o@tdRB~zz-|bEY3B1Jqm8`n-6>W
zL8Scv=wB$3M$SEZ0nA{3?|Wf<>h82RfYy*O<vn`|3~e8nKpk+Bxj-)@k`QDzZY3RN
zcfq^tiErSSldJ`LLlH>^USc181TBhm(+r?@=17FPeW)(`?l$~#lKns*E+Sb@CcX9p
zP@0o`66nt=5;psbRG59&p-MUS1Tf#oV?RO~+|mdq;!6Gsn3o7k#usLu{UR0h))jzq
zk}|l#4E&o(=w`S5jsz^rNiG6<WdVs4Re`2yzk3tf&Hb>P76i=|M8~Zr$~%KdKK==`
zMG|F613j)pvDtn!7W*eZ38gvq2rwT}SW$Ury#dMg&;1wzNTB5bLg(ePS{jfn$5AHR
z;bkaf?QsyyEJxcoS=I4vfa`rIM%6VmWHPP(Ay8LcOWKxc^>2W_>N-l;wEB;pi^N_^
z)M)kV;O|w}Qv$8K7XDJTj}o)A`a`JPsv9UVN2@=Vc=nTC^R@cBky+INN;tIo+34@9
zZX~jWTKxzmZYISRYxO;7^;Ne}qD8A;O8Fe5M2A+thR6<4!l~7lk#6s!gcpip%&dAp
zC3>~`_mY5HDX~?nZ$Uw;ZllC@t-gZPxt$V2T74Tlr0NJIMz#9e(Z^KXL5Xp#z7bVY
zbtg>~_G$HBqkKL<n1fpVKa;c%QW1`5^<M$3>O+J%s?{Gs<yGB9m}6Rf1Lbo!VeZ%J
zucX9LN<665PmrnZp~OSb4W3%{5wgK?t^Q3i{6|T^<68ZvDDg2$d|j(|63@Mq__o$S
z6<4)hy9d>MAO2_22+IVtQ+oiAuPA8aC`XkML$~%#fS*w)Q_oQ4Yk+s|2C4Qk&}S5h
z;GX@D=r*c?*Qz$HqY9}T7un9HTcuUu1+X1&22H@U=dm@ARkdXgs)l3zz=ZPHGI(~?
zd%h17aO_TCM)TP7Fp5>hwxjkr_8>5~<*}nwL5Uy$9Q!FmK1bMVsl;bdo2klBGa)Q@
zeXg%4601E4%$$FRXj}EFr@(e62FJPojPwVHU27mz^*Ze>pe#Vm`UJNlhdSZxm?bG_
z<EIHVuNfAsc55U{YXfeb!kg?|y<r5T4}f0l1A4n65hBih3W(~^G1!^vS+nRCF6~+(
zqX`)APa0!9h95Lz!q0yYW|~vesND^^+Ymp8ek$rKyz?tA(6qUqK}r<rT%y#z2Qr!<
z&3YIzxN@@|Baz=Fk%Ht^pe>*+B4Ovz^EowVYp<iSi5n_1%2itc;){vlX%ceYpJCh@
zhxQ(6eUNeGw2dH1Dze60|0Ih9&c>@q6DmD(yotO7+jYcJs4Yr-1$r#K=2|N39<<`Q
z5u`;q&wm;YTT`J?qq14fB|`0MAUjENenL4sM`|_@qGq4=9KbIq6q~mP{!(*5Be~j}
z_^E*JaxPJk7kr*<aPzlOHmV^`b^%BhD>Amg3-i>xuMQqanSsj6sl1>F5Y-Em`+{z?
zjGEgPqgSV%090k1DhY}SMI}tJpn~lALFc)01hMGlob(=$-KR*|g4fUxYwj^HXi9+<
zoa#|fd{a?LffvBLYwr6M%qB@IIq8ca`%3|-q+gHzqvn&{_~mWQqT4t<&A8g}&r8Hf
zMWvEZUX^UY?PTN6KY-p#a@sj(1YF6#&G|aUkeV-atG>aaS8>kU!1bj<PM)1;7BFmM
zE_bQ?wJ>qb@e#}yqzcts(2pPlk20H;e+NNaqi_8hJ|P9wa6t|T*_so?y*lsOe^X>5
zc_>@J75wpPIIWah%jJ$kCQVp{b^j-F*@Bbgg1@VR38lz7F7jIt_d^oNTLJ0D7JQK0
z<`tShQR9Oj&fuhPfb1=mJ6k}#ZO!XXV(^s$bxt)OE_4z8g^sj7XS2!2Yu>sT3{*UA
zJ<uLS!WJ%rE7VNhLRoDC)F2FL!xp^&>Kco76iREOIWViG9)?@wLQij))wX>LYTM?A
z4jnDm9$b4-3+lCY6^H%!x}Us$^3_LHUo;L=)~@3)=dE8DKj(o)_u?neY->C3g<Xza
z97VaT%P&N(m;5Jkt~>dJoLlJtHBu>f&G50JQG*9I?k7ay54*GxEqKO2Bgn}noR79E
zjzXlhtn7Cvht*cLA3=qUy0i)#&BB)0Xi2ez$18Ze!nPXHD{NF96*khk!bZc>5*y9T
zmhgCmj1$dL5=K2;6A14?*lg=Wy&U91Xhc|IqtR@Mjp7wNUST66R@k28G~dB*%Mu&)
znicYwDszeLS`MM#R_YL~e*s}F>ap!&6oN-iz}iPqU{*_Ywk!!Mj6Bu_AX={oc|;Mm
z^?}$86sx5kyq1QVRvJ64TL9dtcz8tduv!{z+wX!OSq7=F)>}chd=t!V8Kl~2Vk<s`
z;a2-Rfqwzd)xt!!d@&6fJjzxMK~wEg0vGRdY9BoZUeEyyvmvrd3x5nUSAT~XT6l|G
zbC{&JQYBdLIJS4qTj(mZ<y@0H-nwqhXP}+7f~vxL$G*L5{BRy^rAQv!yXFx3R+T)m
zcTE|ImDr<u*QTJdwZ=)AGib{%&9@Kvt-Btm%W5U=A!xQ<>$6+wAyaX2UWL!`Zv2lN
z?^(sX^jNBiSK)KKo6h8T&oAbs2U$(L3Xj6^OY^V8JjF>m({<Sz$^Si}x7NB|i`cP)
z=HzH>%}~J=QBFwDekEiH3F#{-gdUU?7Z;+4azX-6Dj{1*NUEfe?S(=VQO*>>NhRb)
z5;7r$yeNyanlTrZ;06hA0g=}0MBQ=|JV-nrDVZ@nrYo+_{naAc2R76#xfbiXRRU?w
zoHQeTE)h?oIet|j>{fbsnJ!pYYgw;dPBPqD$O^Z#RxQ!iYg@@B*K6CXcO2Zi-m(WC
zdW1ro^^T*+?DtY8^>e>cGU-8Qes!La=r>Gycpi|&QW~?%z9b=)=otaEWWe3M4Lrvs
z0u7g6+DET~{WehRoq`H`KMwHZr0*^S=&v`v0Md&4X>WLA9s0?NPg8W$4JduCv)}X~
zN~vys)2At=I`~>2vh@EaK*xH%fnU4zm}5KFBJlBA45av{22B1bdg@>;TD4^h0jZY7
z^hNY80S5$Jf`3Bt{+}Y~ASvjjSI~CRpm2zQOq;1Ks%xmW)}39~ch*czhaD`lnYC_p
z^_prQ>}*}gs;e&p9;;~?MUPcJ15lKb0-~yH(5!XU)x@MBX{*gi=m42rCd+*dl&n(n
zv0l(LUjuQ}oX>m>%|O;(hzJI?RS>Ae%_u97t}ll4`LM`LO933~8l`tfbsJ<gG->PS
z3Q$}1+PN{F)2wo2&`t)R2nPH-QL@h0kS9E=6aq8%e5U7cfpaNaDp~tn%5+|y&0l?v
zN#5Cc1ZCu}?wO;|w6Ug&QZA&PGauS@%<=ProGVx<167Rka?;9+rI9@y0!<nyE^L9D
z>w$E=ASa|cS!Y&=L@Y2P3-u-RLNhwA*P<L4dI>Og*V{3d5FWL5?tJuG7Xct$OaK-a
z0HE5%W&qqdCl;Fe8rIGw6)r6X!pU{%kxLt;XEmX9ux4^pswGSvE=0PuN!!ris1_d4
zR?aJsd}0~Fp=i0nwdb?&fuWt}yPOzAaj>)Bgqmxg>)?*R(o8;6tFLs<xx16|bX047
zXOYsYRx@IqgXrpf8Rw(OYw}48lwNy|*4jWawpMTB!q=6vR&ZT*XupP5OVRc3+m9#l
z=iJc=awlOeXudAu3n4ef*C`k%^Df0p33rbBP7>e|ICO@hL<Q<^Fe`)oD#~m=mWVzc
zf$|&k%CDch7N*>USH2pkuKhd$k{*gs$pXSkRFUnVC0&AM!wFEYq9Pj9*)!-+EsSmt
zYFOFU;#WggMxC$rcYKRZzJ!nw@9ZU#1H)sncr)H}HdeIDm!5Yd$C~>ScpvcZn61i4
zzSx(4V-RmDPR$*6%JG69SFJ$L|3!_O<Kx+MG{cxQ(6auffzeU2KD{59YHJ!o<*=T3
zCKKHe_ePU3yp^$|X$T}(T!17N9|Q>|`dPdxni15T6H)jPUOr5uM3mko1%cr7C1de%
zy!Vh*0Qq{UN_OE@Q|N_z@4e|%Y-}J$*AvaA69aicoG_Zq<|N~t6>!DVgVBLFmxfWh
zFPj+75n$Z1sL8kWQtg)etopsy>{e^de(TkiMVfWSersmq0c*{mb$qL}YU6#(f?=w5
zEd}?LS*(qpLvqNnTSI)#eu_U&u|dRkZnZ3GD6?AoHOmaRamW)fTyA$~urJ^?x?MiM
z$8Gom-5yui*Xi^7BHI{Ignd1|evc83xFUUFqql_v`@+7!1|#Iz(C2rBd|_9_7Ys1G
zVoEQk?#^Znye&Gy!Vv?KJ>9;5$8CP0wsqwc>qWzOi#ObY*N8JisdTn~Y*6Iw^>s%K
zw<qEW_4ooj7pW*KdNG*4F*=+Y*kxonXCP>JJ>js=6)?iRo-SXv4?2`6PAdm)afJd@
zppxpY%K1WG%PRt#)H}*6c#V&@w!j^dnFwB@$>0TJypf#e^MscfiCFKZ7=B_21{=e+
zcv%;(*k<V!SMefnTD%BMufC%E{QX`2Lb5wP7{z-{T+z#W4VORUak;mlrhE~f%kK-L
zV7-2$$K&?(^{~NoJl+!@Nu?(^y*Jnsj0AhPq53?%%5-FgzJRL}wHgVc5?yYi(-Y|O
z_P9cu%=J|G!Zi1m&F~V=xWyOo8h&4(Z>tdu_=Tr<LZM*DWJBWc!2z~HN$zP%0lbu*
zGhg0Ht?fJF*}&LHe?09<r=t^|-rj_$vMsJ&E+@wi6Yas<OmS|c!s56BF8{W$$GE&V
z6hsMoSS%GtWxMlc!TZHAUosO<3yXTT_=;5S4fz5Qzh|?@Z*+NGfeqxmgTv7s8LkYO
zUFu;(E@uY4m5$fERqcd>{yu69)Ludnu!j4(y5NZ2eSTOv(iaM_y~1HQN5~U~kwn{L
z$+3}89P076wZhr>sE{h!r>b^WSH!p3Bbj@Gn?2^{%fX&NxG&`K`;?Yl+qzIu8#oTl
zy(<_9M?x-?Q4tij`FN7v&-YwCM(?dB;u&hjiUJ<f*W2p}G3bVx;*Gt3*ci>G4eDNb
z7CFBZ%~ja}J>Ezn$=`L0;MIM!l>tE?2u6%;<SEq6^mqbfG^$b<rmxqSQaWE?Go0RS
zxI!EHh&oIjoAV~w7?<-m==pp8!U^+rR@7(l7S_!>uBl>H#l}X56L>d1jyL>^1@#5E
zZXS04)yoD_qoT!Ov(4z~gUvcUMsG0ei^$6I1|!@QaEu^KSgNNjt~j_CWooLh5>C(u
zUoR|^(T!H<?`6hlDnmVk*SEpT1))y_CGQCWTf!m6%N~jkC-{L37!>*80{H_I@=bG9
zG!_#r(<9%_kM7Plz98Ow?Q!{yobPwjh(dnj4{m|8WcEZy1-EeMU}psGNB!-VU}zIU
zQyUl@8y*(bj}9yByTZd_>C~v_YK#~-ya5LjWG9&L&=6kQ#8A(POS!cuGRjGwL>*l)
z;9=Cmq%bs*(9YCYGFBwC8`)#zi)S#)BHI@YkLXNHJqZc%CF2FU@!mT+uxo78pwkw(
z5ZM6vIMzk(O|9pOu7X@hLD96KZ2=d$h@gKnbwX1ebOp+TcTPQt;sFQa3>zJq$iR8=
zMrv|Lc1RS*k3KEe{=+gyj_T`myCR+*S0v;^f2F!4xKG5@NrOE4zyu_v%mYN;L0a3=
z7<6{W^G;IAjoRtF26zy+tQ^MNm_4AM7HvL1D)BO-6E9Wjc5)KgBF&|to+Llm8~k*L
z1bVS+;>M^bV{Xvf67oenMmMHBERk_1qQZ?r9_mmFhMEj<i;*Iql|eblj#y4j{1^=3
z@E*~H6=>bMJUlTn5~pdQ==nS7_1$DVnw~OGKt0A1G(|8STcS=UX{Rnm&XV9jIBR8W
z7_anZc$aEskbPh~O$A^j^2l^N8WaAgvYx)q4ObX;=S5VGXP`S3O^<5MR7tMQG|<9v
zT&$m3Q@&@%i_UJ0AREU<Mp>>Gg&lalWOri1xI#vS;n0y6)ljh46A%?-B(VhG6B|xa
z)Jw7+*H-kY7_&DReotUSMD%r2y4%9;dT&vch`ta<zc6)349^SAzo22NW??#ghQ-Kx
z3}bLacnLaLK1JXYB=q<Jw=3kPu5mMZLNcF+J7M0La&3)}VVdw#QRI}s<Oli2wtN6b
z-=t*&fX5Pj!8mE|De`QsE%~XBY=pwqgMASr*eyhaxsKhwtum<wlftPP4J&R>XWs^#
z@PlF*BdHip^fC2M%4y|5EJn{lucG2&UgB|u{4|+$dr;wS@&nPmcMV~dkl8hX8Buf~
z3-42o4tExFE(Wh2M%&CeIHn<7``l#KaCHXxB+T47aS}C<ipmh3kD6I@qh$xxqQ>a<
zyTsHf=TUN{gbtf3nvN86bX|I>cv{@CbR;{LjIY}fPsY*Sk&LDXhSrJW${b5~IF<&N
z;lItX6m>zJz|z67BtI}90}##v7}&LRG@Z)g5P?+Mv_#;?0FlM`<dj>zU^PT2?sihS
zE^pB932zefa?>a)dU@qt#awEdCl-J4WM(WKr==Pl>XaOq_<OGM)piG#_x$h$xvVFR
z)Vhs)0&7Qj7&CUGZkvkRED_Ue#_bDvFe}ESySVgbmsC?jI_BUVR%laBm@1kBi)jt{
zomhqNk)95Yh?u7bXd?ldbC8;SflYznmVkjKWA3U|Gclh$#iy==;shj`WpUwuza1Zj
zo*9##5!pBImHQg-HIXqtbxJt~)|WIE$Z>#1Z;YpE;)O0Dl-g6!?4^xqS^`w*Y00#;
zCa1e%5YG3H%2|`NIx>%W`4y6!)s~)kqeFK4TpNN|7aA_Leq~9T0Fx@{;tK|Rf)zLQ
z{D?^Is21HR9PbrHGhN5H<@|J*mGo4(!o!rh8<Qq;jhnjTR@1!LlSt6q6mvPQ6m<4%
zMbC<5q<P?w>pFLcuOK+3R7myK7;tEpCa}#%6&j7<m@x|4M6gT*?IKvt1&BqvFJPJ%
zV&&}a>xzI%?NF!#$fO5!N7H=roIw|y9ZM%SM~BDcRM$MY40*&XqRU0|JM0V5I>Sf}
z4r1X^;5H_sEQ1yNSdPhwq!>8l95L?`xiPbo7P0=YXk>*uRXr%QX+)B)+1es}lU4y_
zbj+mO@g&=u+vDLz$&VoU<-4#}(b_r?<jy0jLTz<mZS4t#(I+Y2<v!2Xv#!3a=v`f*
zZD>|g$9Sb=n|YmqfgWuZC$vz{C3DKHXQ=?)!--M86jC0!_+m~bT9VNn3ig=R7GikK
zS79Q9{UYqSFg~kbvWmkvmS!v?Yq^viR}}Ymaz28ov6m(!7#`ih{MbyV(eY-&H7o43
zOYZOGO=cRtq{Rq_{f*qph|^*q5c9v%`-Oa8(UC}IY3_m!oa-TGO{UpAN~-qtRNtK2
z^xHdS*OK8l!yR0v*+O}5?ohUs(=9c!so6(v-Hzsg^YiHVLNq#lBG5p7^};9k26nb_
zlp^+=vDXYjL+r!S^s8{6g>1onlCMX)f}v1fui8@KGlU|KR*O?ILV-ci2W{{NJJD<Q
zV1vW8!DC>a*xMK3izc+YNT`pmr+r~;*zyfKZc@I2#EhKp+?whObBmy9qpgIYTieXr
zowRZ>HQy3@#shraODkP7Vcz<!)R*M7%WWs3<>eLwyk!-hki<)oAN0tcZr^63bDMF6
zClsV!+Mi!g=PeE61iQIEVMj_D&d7}Q8$L`|>4-*CVN^3WZyB5RhiM<3jy=RiWqh1=
z1*vtC9xg23MHXuHNsC;J*y$Zu#)e0uX}RMd_RjMCrRr)(h;l-z@Nln~`l{(7Ed!{O
ziior>ZV#=?3igdqPr3CSSCZN`ZQRi0hi`z<cBMgE^Bhf^MHnP7?ry{`862Nh*nIPF
zM>@49-z^pTv1e<S$K%ETkHwx`<?*&RhRNxUBu?z2hK)_CNQy7!jnr^V4K#GK2j<6Q
zjeD-S9;U3!TibGF!DJIfqu!9Idt;*Kbn{&waSRt$D5WZPxtL>`s<h~^Pf@sBC!U*d
z6bQ?E@~WKEK_!ak21u4O0M10if?;ZPt>U#sJDlL9e2Yd&%n&lU+bspRdU}>oUYN)6
z9W%bZ$7W%4Tx>YVO}K&?8Mm^S64P3&gf^`!r>rWV7KXR({IO<?ADPC&WBog^42q=0
zP$XwoQ~Tfq^*_91`HcyFPlH!Cl~{I3Me90#zl-yAd$3EXwni~|qNyR@^p;yZW|xso
zx1Cn~=+!|cHpEc`(o>z}#^vT#xB0a#49aI3EPu0zlNQZ5$(6+n$ZW*CLzwr1OjiCo
z9G6S2M(%fA-W%-PNRt@eQ_$??Z@8zNukl%IUi0F8`IRWYn0fJ1=jP><gL!IAwN`TW
zqB=4$n&+IKT0il0xdLJwllLV(hL5%s)UC{^=y0zK6E(AuWd+lYx9ChfF-E5Za{0Vp
z4fk=gM1qlRz1Ty{`w<2WwSvaJBIzSm7HYUh=j!%^u~@}^5TB=GJ9&)GLKbZ)w<T<<
zQ8iL5FS?6`Mq~EMyk~SM4})`JV4#s_5XTr|zRho8wK3i`F{}WS7{_SEf{lp$nhvu`
zAMVs((?u%fPA3MjEJ>%uv4)U2mNo`b!($^QhqM)9Hx=qZl6fPYI&NxMs8&~(S8n&_
zM^AOfB;*N(+@27&$oX9;HM3RoQ1Z?d?X>4JmZT99Q+)L6bhk9O*)I3PN{@Y^Anb11
zUQ>@}{-b;XuZrGYcH-d<ydl7UO+V@>OcSko#BvI)df3uLtDdBsLaUysG|{SuSEtab
z=POOL>OsjVwBz!rMD=u{dVE$rhG;4pkE&-hk4RhGB}4UmW-+~bL_x_@k1Bpo@)gsw
zrp&|$mVgMeX%P;o7@RSl9%W5Q?0hxh5a*>??5T<r9(rk_WoT?^BF361H5APZv8LEW
z67ocdv4&%I<3=oc;DUq!TpGg`G04~`Zl*W!k-aHPTgy#o0tw(!{1EM?_>eJ}j*i5Q
zp%{2o3PJ}4jQIFKd^BtDm8_6qMCo3&kfGu`2f&E@q9cg`@THIiewui`A`hI?&6;pD
zC!Na)(s=*a4lqZPJ20V;$wYE6rQ*^4{&ak|O3}Wdio=2`iYb<gN=Z|y;C~FjGurg1
z4LxGRuMUe-hCIlfqbX8?R>dh?fPcKXkmHY7oJi4kDR{sfBFO#XIh}&56)7`+*5c&&
ztmI$FQ~94_em78W;H(p+|51yR6Z41h5sUE@o-36h9x*Mr0!ZNrvYh_R>J)f9TtwGH
zQZSz3;dWB|c-V|zoAFDeDt~pJ%tJCmdNJF4k*&=aJvmEfw<t1oUJJfX!7n^u%`c_D
zqEq26VCgIvl|F8Ba(a5bpRQ36Fw&p&RQgLWy9N=msp41X&$QSD1wGJBm&#x9tMcCn
zoS9#pXR}@MJ7j^0A5U`U@>l$~0Y)UGA6@&ZoHFD=?pz;{5_zdOh4iekng5vNKPI1G
zKWQdL9M3lMONAdq%FM4iy-{)sTwhg&5y>3COtKFnV&+%p6}8t13TXyK&TW-TAaV@<
z#ILe(%7@OKlK&_nO_#F20^N^@nO}X^MSbsOts;~m)s^}BA`)i)Il7ah=STy!zeE1V
zi}?LB1-~D!647<82y(wg;&2EA9?8bPY;LCS=m-{7Pf9>>`F{ZfJyx&y)%j1);>{eo
zis_5fFM@`2Qv7r>HeGZA4PC|b#pyqSMzt-XSLYw8@B66l_$dEY{Hk1Mz+=u|eGlNw
zc_eN6{FR))g2v3x9NUSFF$e!;<08oYDn1p`K`Ul{^*xnw$*=TO0u-N0*MON|%6>;=
z{s$$$YUhf+Sl&#Te~TiJq2j-^2u`}vrNViVUqJJ}|K<>JXCB03icf(ro`(N5hoCr%
z9uPKN#r(^s;jfY%_h*V4Trq#wH2nX5naK3Vs$-Bk6!Qlqf3f`zy99$p`jIMv;#c7n
zl3(Ffc>8t{`<VjBQ1K~nd>Z~;Q9*G;5l+KT-GuTTGwGX>KU0)pQIh*Xk%_pNSB8bO
zGc>utwFv*K@UQ$=`6czwrSd<O1p?R6B4ly?f~xed720)JoKukWZ4xz4G9#_fM9qH+
z;bpmV>k8o*Rjp1lh{vtyk<>iIjI?5=rREofa2r$ehC;X<Cwn=~ARf1}s@%DCW~3D}
zseM(Y=xf-qlJNX@jtb>W{caKAv2@FQU#XB@&wbad5Iz$}IyucC9=GC)Dr%fEBdx3<
zH~tjDXXVZnEQB{=oY-y#@wk=E&W!_Rq!p+Bss6nX4sVqGW+8m)_pJzTHGQwDke+7q
zQ!ne3S-haJGC9Z<u~?^!+KRrYGm(5&<|vY&=+B=9e!&ztwW~7rr^#XjjqMgzJLNsO
z1%PX?gL;`wwQFkQrR2Cd{nYxpT*_y)PQ>!pAfl9SOA&lR!i&$tyd7}T=cuelDi2-v
z;$Ool|7u>sf%y3v{z?9kBKc2o`l)_6UAbQ2^fQ@P&byH0*W38ja7tw<|14#nDk!Y6
zseb6-@TuQTqc6i~_&(acBEMb53*IoLyc;E4o!=?dWLI)IQ`^fR;HAF5b~WJieYE25
zo!ube#hVicxg3>SKD>Jx_-DDCJe`XD=QQviaro5!>gUtYzd8+kCI&Hw%FGll1-#UE
z<<?F^zZvjS-^CjMoa}a7`deJ)k>PMthBEqoE@$49@8#Vs;rAESJAEIkeySM$ovTvt
zr>CKRiOZkrZ?8#uXSK5+ms!@*`7W6TJ^^^C@BJO-^!f2feRuCp4lgmT6+YH9%?&|y
zYt6q>J}Kg11L-Vo{^1Ka_(Gkru?r7!(_^Iu%YQ}B$c`8T^c_8V`a*nf&jiAk(o)8b
z;Z#2!jf>&ol#CG_8^_b>^bweBJl3>g)k=B_qy!QF@S51c!fO>-yg(v$!D3@0BNJdT
zrTD$+0#XA{8Mt~pc(~C`-vZ>C%ecH38tm@o&tdboM?is(0p_S<nUpa^uWsQ{nL^pP
zw`&IA);R9<818KWSC6lYmHsFk-_6T?EAAaXQb>>1Ii2c@arBiq^P^CC41G&Ze4I``
z4U<O+CcH{!{wQA&`fq*;vG8MeM5?N`@SBN$>oa(!y9Vmf1^#8iDIWzaL1=#d%!Dp{
x0my_<8Hi6Js*ev&{p6w3Y5FK%;XP28M}D`@B=`US29|h>q0si?{yX3B{coQ|g$Muu

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/example4 b/3rdparty/qpOASES/qpOASES-3.0/bin/example4
new file mode 100755
index 0000000000000000000000000000000000000000..564bfb13c1d076357ad3aac2d760341c21d17216
GIT binary patch
literal 34008
zcmeHwd3;;Nwf|hni)<ybZ=A&mSrl++0>oRK;DiJu+j68twiH=T?6Am=CC4JR<yvwU
z9tA@J#gOol0&UucCf)Pe*I)DcSkjgjLx2EnA1N&mN=s8#hc)bZY<}M}cjoF!mMQJ$
z{eFM_ZsI#LXU;iu=FFKhi|(C!waZ&?QWPdH6T6U6>W--n4$|(7ja<%?0#?oPn1h|o
z&SE)$WaH1lb12keGCVC<%g`e5%m}maS8dV&)g}jz$Z)0xW-^o#Qb^?`FNcU~;eQGO
z88T+)`9w#uK0J#j5w`J&3{MC-j*(#evhzEEiEc#DjR-m!7798U%KA~==r1VpgC+-3
zly*?S>i9hw5-wf-7ZYKM`)BYZLI;n?Fsy-@3?<zf(2<;<_bL>K0a4y~dP%mAMj+dv
ztUJ14NmW^Qcwu+6cVO$nt<_5wE~zSw$4V=Cy@@~Ju54`Pgi?>iNRiBw@kgqr{GJQ@
zcf_9j_BPj_Iv?A4Vn=Szmb-3Lkv9o{giAV|TR5pLN6E^nHeJKmPRBL&1+2WdZO|I1
zuq<X-IUKhLc+>DV9e-!wkDAPmzccYyg1@uzw*Y_V;E%3_TEOpRxZi%mv$wtShc|71
z-udip`?l<Pe$&<8xN`QMXK(xYuD5KgEBgmef3aoezps9N_?ORr<x0D2sP*!^+9w~m
zCjW8&tN-W0ZJtect=;@YN3H9L*CtGU?pqJ;`~FbnhG$-{JLk<U&;0KBZ(UdV=9US|
zzGgifesJp1$~U+49y>bq_KsgYxM0>!Y2pTHlF>_`MAAe0^_6kpqyu|8GSZ}jOz=)L
zOeX%fjsyS6IPfLo@DGdwuN_DJspG&8fKMhlyT^e)J`ViOarhq`r(XXu4%`DLbS9g~
z?8Oe0#MlIWXY588^(1CvcL=xuvljs;`~%Y*93=h39vJvsR=^HS*WmL7!7q5Z6WK9U
zgTFu>0oU20+;l!SA%7CHvfYIajwf$#iuyVP9|?a5{0rF(cF^7?K|FjCa;lJLSjaDm
zWseK|y>nCXcLSf~9~AQ21vd)_3(*tzrtvp{&s>x{EZ{8y|0Cd&eujm9LIOS?_|)zP
z(&Yb7;8VGNQ7-uvy6)w2P9IaRLC84?{11tG0gYcz2|g#%^lt;4@Z~rt(AhE}XL(w?
zUnB4x0v-_rQw08@w06%2+@3){A;76#<wBm>f={<-_jEpY2>sgyzVz!Wgr4nl+c*cQ
z=Whe9LjQvTF71%PpMSB>=WKE{krQ83TFyeDjXkm6P(0Dzp9qCmXw~LWAhIzUPel4_
zyW8XONSuW{!G=&c(l5ZlhT85}ZzR~hp*y1GWzJdOc)lpBdX2w72JD`yXm2#(@~;jz
zRff|32crHk3uU0G?usS2bm?^CQdE}5BZ=BrZ#>c89_>w_?C?MbifwEPRh&N-PNlAv
zqAr!9)>NFoF#_6XPkVP$ta~64jrF?55wWhqwb>}Ufyp9>0w%o)Z)9_%+eIRj8`9&H
zy%a_JHe9~8y*rh$uE^R7S2Rk@D%_EHJlftH+}0OyQS-(V;f{09rPf`7w(Z#39_kzD
zkAyZy`x680-6$`(wy~m|a<;Z_h;FW^I3ICp=uiSILebvNn2U^J>()?Tq(2_(ZSRgI
zwuLssQqoB)!6qKfK)pWP?cYqoZ0zU=#YvV57HaO@673DcxZ(pn5z6e4LiUk<Izt^@
zn;4{z4J259doPS3L`cFid(e_B6bZK{+L5sV;&Ph+2$*F9@qU`Bl(i>NlZ6}F;}I>o
ztgI{66Df<vyP_Kh`eS7s?TJm%-q4nKSw~ki9P5vkZ5W7lhs(P9`{LW;WsCa5edq?;
z%7op@a3MMh@sBSni}fYS`eX4jR3*AGM!>&cMwptOP-T@m;sc>hbOlmrlio`zmNsn5
zXq`b%5+|emdVJ7E7!@0cM^ZXb26NYJHAIS(Dol2|j3T64cV?t=ZmGuVWE53vQ&IrA
z0?npm-^(B^OmV%j`s1o-ZBj+!;UqhG#u{{VyQ1(+sa-sSNOhH|eKHe@ri{-PiSt=m
zTN@%TywdBbsSQ<@R+cW-<BLkGSZzy-v&IvuC|#t1TUvl#B}NMhOqgF|FiE?zu$oa)
z{$(3d6gQ^RF|#;s55^E0XYBa%pS{gWE2KRbpXgFh?u{b99O;1>(L9>Pe;xTmlbB9v
zGyA>){{-eJa@_jEPd6baoBfBxU+@(SQ6_eez&|~w4P3M}8HPUnlJf~do1;S^qZsE1
zUTT2L@sHp%_m@|(n3v=N0I~GTZh)hD`ZeDGH_B6PfD6$%O|=1TT(3C{a4K6~ZUa1D
z0ulNRa5R8^wHV;E4w6@!0WRmNl-Ff|Pn5_A`wVcK0lw7$FEGFd4RG0(lzoE%KG%T1
z+W@Dvp1j=KU$eMx%D%L~&fM1xC$dK_K#qI+1C}Ei#^~YzvPS0xaW9zTK#bB|lzH;R
zD6V;{C{0aw@)%FoP@0<T<S<V!qqLdQhj_Y@($rB-9^mP7C{0avayL((Noi`TlY=}x
zgVNMQC;NDM5~XPxb+V18^C=A*XD9tUZK5<a-AM;ee`rIRn(SmbPrprRYLb(7o_>YW
z)D$NRdHOj@+bGR=`cITDp!BIvM@ONnpQ8ZxQ1<-@j_h`>bv8SL%}wrWUZb_Jd)JPV
zW59Lq+I<6b(z)Ax6e-f^u*JRf*ubf`3knz00^7UmdnJXC0_lmAcJI2igc!VCpx#m9
z2dQ`H1ds1VoJWqy2yK06XgpH5;xM#Be200c-oqsT#?F%CD4ItsIx!;Dj_GoFhhB75
zyk7B)^Vb_Yag`R#*}(-Lc?Ok!yWpH*_x1;?-!8Z;yF)9)_3+4jB4_Bh^Mf~rj&DDr
z+;@5o<d`=fDEED6T5!K}SKSnobNhq2?!81xa7g1iw*8|@gx`CB6fkhRdskhF-@VIU
zok=eDu6-qKqMoN<6{IgMRIeneTS!0dq2u7^Eotf8jr@fNfkz4ltm`ys{T27FdrD5h
z27e6+*w4wheo={=`)FNB`6<lg5mr+c!sV3V9SWB?Mvi@g_7;>C?~f=vnJ6iju#x?M
zcz1P|%y;8f;@%Z3@uU1H^Qrt@cL+n!O7;$QmlV5kE2Kind?EGB5?93|u8Lvr(BB|y
zuw*{6O2Wt*Sq#o#HESYgmEe3AA4e~dWg~ONFw(@=;vPDWIC+MSS3FYjI9Odm1ZtwB
zMKVO{%98mb|MfA|=#G-zRJ^F!$k#bg6LaJ|BsvS`AOH2kSXiNuTT2cA_g*NOQaelb
zB665|FH$==pOFP1K`J{6aejRoWE}gjM$%e`Vsk|aWavrl^Trt2WXOY(_kF~fP0ZqT
z6$%|ifl{F;Y1UDmzNdtDBwex4K$>oaV!b7PgC2q<EnGwMM~-P_Ek4NEX-ZxZH}FPT
z6E8+OvywRh)M`|+7qyLotx?G&<wz9=Y9fyu{qP@F@&h0tC1*jL%u4<aFsMy=ad(OY
z8U4sXN?h{*xtEIJi!XIv;=I&(*%9~95#0o?Cyb%Lj@S%RB7dY8@K>L%c-=Gf`^(%z
z&$+k1aLOO_937@rz5D2WwD@)(JxY5fBa49gZs+d9Kee~9X7|E_2;AFG+uTDhZZTbY
z<S;CT4>0dudM{;-o+y|zNU+0{lKA(XHUr;1biez)S1xiZ$K1a^Jpkfg5Tg6K#}n^f
z>K=MXqkm4&53Z=lx5JC5&7d5)3Lg8?hq7k_q`Y&4`;+Y}x^jS!n5~O2x%u7`*i{Dp
z=&?&r{tHM7=CG4-Ep#8vJp_jU+8tCXs+XD`I^{e}9ugXHelRk03NEkUr&}!C-MMdC
zS?KM!|Lxj>LhpTrFtR*%$CKM1QrsQyqv^7az3nV$Ro*UWxer+g3n!ea7$%qpo~q?x
z!asuXf|qylfzu?GLzaK?4v}`Qa}JF<*LsHDcQ$*5o^S@eyXN&}+o8f;3%4R{@-BF>
z;u-gSpO|T6ADIO*_jRu)X3P3`hhFgxy<Z0&qm%#ahR=2{eHH!@1HLp2XXuE*|G#qt
zI-og%m=e%@ldk-&zP^Ngh5cgtyvTXt&t6(uYQOXXyFJDi%*dS=k#ne8f3rOi?TIY2
z+vk-o>a_PnJFs|*bi{hY@qCE{zQ<q=bjkgg<jg(OA_jPKHGF+P42}0$SCU`m#fhYq
z`#W;q&kj?kD}7&&12f$GLJqlqB=@i6e%r3s?>z9qLx(QZNKaj)g(pPF9E=UaAphzV
z`S*%^5wc-Q5AG;+DrcB42qR7toVNEzM-L(F`(SjGrVxV&X%-TOiyZ{qk3ofI1Z@aU
z5gtOC7|=LpM|c8ZHNsN}k0Wfu0B_ftoBw_Q5~f@cV9M4)<%|irmfcEjA^omE8|r_C
z=QxKD#6@trQt`Fl1qlD(d!wU;D1+Gw>utrW3anc!gY2Ri7o4-G<V>O?`a1meL3Tb`
z5)c=`FTr0|26zJSHo!v&<wf{A@plk#%r^9k;NQdFfei3_0566vY0@Vzl9?{$3cr_Y
zAlFP=q-ISI7$T5Yg1=L!7wsKq?VRagHMYW=O*OXSUFI5_eP@=_Hvjr;r>%T@&Pv;%
zQuBeVd|P>~ZNAfH2M%y+Y!)ZABVAPYI_Rem@<j-M{C_pMY{fU5T{io!ESGKm&TO}>
z{Q4ZXt$KTIgU!)xs|IhBfwFXdTwgoEry6`jQxZ3RjgBg3J!8f|`-=G)(=YPaV>$Pl
z9?xT6&C5pOyLrg?9o_!_l|ci2Vt;v9?34RNTB4F^)0G~c5C93MwY?FK*79^o+j~l+
zrR~b!8|2zi{_Y{yp7QtAGc-2(I)8>pAIHc?m$YZux8D17G)C~fCI<!B5zsD+#la&q
zXQt~0vxA4yWj`zsrQH_uT*jt|PzGih5&m7;;-F9j^+&pB7ETxau1Hs5frEz<e#oY!
zrQB^im*Mwu?ULogb@BR?3w$_Jo|bZ)66s?T9fI=zO(1{APA}~t@jKF!B78}N?~5=;
z*x6JOo-M*9BCHqTS`l`NaI*-n7vWb$_yZB%CBk%hupPF{KEHXxKyPBeUR7FMTD7op
zfTt?1s;DY0uPR-%K!bDlsx_z9D00Y@O(cgvnU+JKbklNZl_oBSN|}~jtU$<^{u5og
z=y|Yhcwk%>MS&e=U4+i@4wAzJGh8NG2Ic&*3U{+5_pe0=6w8-j>1I>zv*<wPtlSqk
zFz0Fx%=W>$va+(VM-GDQHP|FDRl#s`w@1Lj^xjlNcTg1EzJ?&%l4IFJNhRlcVqnSk
zf+Y7cfO0H9!abMgT5|t{QgSRb>{v_}!$K|P4<nve0ixr1@42De{O=Y5@IH~Ud>f2C
z@1vUtJ--hfJMUwP7v_hdvAj<xUY!4R5aoSJaeICZ6p}Ye@%h$2LV`S|P@>$r5h~47
zc%s_ci7uCC;t7W}0V(pbc*1R6i;CrC^Mv0zix}tdM2ocuZXnO13<B0>rKL$;zH&Vh
zUDi)Yt_cbW+-F@$f)(;a!umX1SKcJ08#!C8XAsR)r3Q&X>qN9u-ZaIJ#I@E=DzsSH
z32xi1W)f$nvJZ(HtS=LTdCC@`?Y1r^y_70LNbI#Pr$iY~9I)0>qMRoVT31n`f+r4H
z*HEHTSpb^D*5j0@;<61}e@TKaQNDw69$*v7V6l1M$Qyx-#d!y)lKF3vRNtm3%Womd
z?@)Cu`5|hOA5d)z(ULyEaxBz_6RCwsY}>Vn5R_VzLw_crMXX=~+*|(jXHEjS0wrYS
zf8|N!sw?S<3Uw)3NnwS1QCwF3H*=v1btd$FmAVf)zFOT%wyrLKA68T!;3oA&@G-MO
zdSoFh|K1CUU>!hARUoCJu0TVZR3EBpR$qjPWvM2_v(;XhMh+{?L3UP_!vc}i-@)Lo
zR$ZLfigFaShnJ8Kk<98el1Obw$=T{u+;h}Fqr2s*TS013?*Ubw+6RT@tB*obtGWYy
zYJz$LtZ|}x40oHl9KEMNJp$T7^&aq7)nSM@N$o+!i`28w(390mfH_6I9VJdx4?-)`
z)VI)Giq&s|YPvcZWzA5}fmCOx%g|mk)h_~nmU;zbo2{M${5fhjDrr{_TJcM-Y6qV)
z)oVd}mf8vY67?adX`cEJ<UCuw9Gt(Pwn1a_)!zbhf%-I5bB;O)+0Iof5kF5|4^1vq
zPoSAfRR!gisilCFt8W8Rp}vK(D%Gz;m5bE-QQs;x8$1`QO^|bmdKc<@zB&>2YIPrK
zx>S7>+Fqu1U`zG_^$lomxjKwmuTXyu$c1VhxLu?^i@FRmH4jR>kEw@IkNcV0il#Zj
z)Q>>_08^g?`h)nP8P`KhU5?-=Q=fs<4>R>A5b_bGmO+k3nfe`6;x|nFPo#d!)axMV
z@0dCRlKvM{AB935V`?c{?if=qhk(Cl>SZYXai&fI<{y~)9EA8IQ{MsWCz$#%@}6XB
z6$*ZeslNr?aj=GRpJwW6kpGFPFF?J|F!e2z@MosBK={AF9Kq#TrcMLhUzz$F)ba#V
ze~gO%jj4XL{c}tm0NwN80+bh+dJ|AyWa>*0@Fk{Rga&$<sTTregsImeb&{#&DE1Ym
zz6RyK%G4`C_ZoDDyw{ofDpGGSwE>lWlc{&0Elx2t8wI_^)SsfA-)8DT2>%X*hxNS6
z)H6Z%9#g*yso!U+8%q9wsXHOyY19Z6`){Ux6<j`KY8y~KV(M0qe~c7@Pnb$i5q-+k
z*O3}URnSy}idqK&uTj)5q3+izYAaf0yP`gVf_5nCizwkbMO^|VU$3Z*sOFaym7Yzw
zK~WDOHKeG^QP7QwIsyT2QdD{lVW*<*N3cs#*P_xlD=I9I{i~vGLJHfWlU{^P&$pic
zB;+j`McjJF5(vahMa$6&);lR-F%`WAko9ivx=clnqd~2QDKXhp^jnB%{W&FyO+`<D
zsr46>m}x3{foOh73A<@B6=PkgTn&9(hrcPysSpjiO1TY?0}?uwC`4AHatPpG>RC0M
z>ILEzP<{jOlQL@xr<%45CSYxT2^Dq&Re1y1|1D9ba_VW%L4DRs{s<^JJw=7$X5x=a
zq?J7_icD+A)qrx8Dnu?Klt%EJ8YCH}T}Rc9UIiU-ly%77kcOh7Ev$VigeBROE0BGi
zj-rWcC0REA5RJ`IZb$aPG?e?%->p}czzjLcFtQ(#C~Vq9G_-Z_M`#e9`z&%^NaiM}
z3BUXi?2HTfF>-Rz&c>>~59epyxeP|jQRX1~oHUd#le+gf(E~V24YC6%C_-zyiPhIC
zpu1lorjTbCAUv)$t-cSD*1r8sbTndR+J)>}BrEuA*uM4q@4@%*+#e(7E}5$}UU3fT
z^xyvsL}F-ql(LijSuPDorX%Pl`Mb){PPxwjVfuwAwFQ4OZiAjCoK^;5rMxN=0W8Mf
zlul|`8co@ZGoaB4cPqaKnB!C+vYc=}K+#jzQ{$h}2)$1DiDE^93Q*R_d=wT&OBEo0
z#zHWf_@QzD1f0<oKq9M|(oKbZpBQmloOtu6fO3@E5jm!#keitL**9{SLZ;_Ysfk`?
z7TgHu_2S>>lcZ)dAB5J6O=Xi2(BCY&F=ck|T4w4ZRa;E6duy2~O7RJ%*-mD<oODrW
znoN|YMM^8`9Kzp}eyW)UU82MQ*&(6aPP7}>qlV;EZUw@A9f_PIoAUs1U8RsFl{;{|
zSLPc!x~buAIJ2vOuRMk9=OoHh&cc2-dX8!RBp3q;t(Xc;oZF{_wgW_(BwWE<95!<e
zEWn~HrMf?XzqzZCzKz6DCR3KOLb(WF7yhR3_S2var5TVm37z_5UV&D4BBf3tUdjM&
zgEHSx0cB+b9q&J=0(+7DU5TQx;1y5;33NPJ0nY7SLc2Rt1#+o^xrB7)f1vnc+Z5%m
zNIZ!`&Z1k+q#XCmoHG?=?vIhJ)vSatm1)R54}VjB1-@LuDMzS;M{r{r3i*gtV^P?=
zsQ}u}P;P;nAZjR@QK`x%z`sZYj}eo3YjCsKm437bH&9a{7f$&KQ0|gwQ}xcQSw2;q
z8@T3U&VH1dfpdS5NK&PRBYzLjLfe&8*?Zw-=DvxDR?jccLx{E<g?y96R6>Z#M7ZB#
z{7rd+>iP_cIhitSgUWn>OJx?Da{z{B+pZ8_r54~unWeSn{HxIsZP(v|K1X)O(Jlu{
zpG0E|D$z8y8}9&s3ImpzW9<gutAzD8Dt*Ck;52ME&&Pnk9gC?ji(}mdjQb>(riulv
zROGF3REN7;Q(-no{Tt9;mZ)sOC&cjU)o2^RFo$F1!l)+Uj~6Q#K0^xH|06gMftt%v
zD}h#%f-3MALczB0p8*Fb+S9`EHvsp_RD9m3#8mTD3*zv@wx1|4JVBYqDffZuuFt0Y
z5r#0^ov#5&Q08;WCjp?H*kpC4^5S^s<~F34E`;BLA;dP^4?7i%tenvtNan`h=V!zv
z`b{r<jbJo^GwK4913DwFfTU@ErfgHGIoSd(;L~r(X^N?EBIi2{p6~oqeA$Bgpm5vE
zw`0O6M7432MQF#__!GSV%>+~!jP2CF!WPIi!deSB>V-h7BUH^=Z$s4f-t%(EF%=ea
ztT4bai8b|m;K!!@4Bg)L@wYG);nw<PWbcwF?3`K{ylr$QI+^lqKz~R%)J@pAFOgB2
zlnLZe4(ni+OFn?x=XoxF6Dz2E31VAj28Q-#D~FaZyco0vOF8W6Fa7DgV{iX@*+Las
zD_Fr{j-7XIo%Pcxb)}b}*$QfIM^8FXW(G~xVp?A3zDhdCvHS{mb5_<O7?#<R^%Hce
z919s!j)kTf<rbPVm-BcIkLOruwwz<3=FPECNjVl`m1CjVM~;QObGd~^mU14?;qeM$
zNj|C)DBnc5$kKsI-^f{!FE6+JiQg%n!{a#?s(y~;E>M<R$kFA9JL$jN@&Jd>y^4I8
z`91Km&<gW*knzaB!7c0uHOnP6FS(1H-W0_=8%WC}B9BPK#aDpQMaW{g<aBdsbf}^+
z&l~_SB58O;(y&|_C97y?H*W>TO%jVol2|ljs462~-vY)#iAB8F`9ruXkKk7QF@!Y#
zkhE3(II-kWRz;Hz^HYR{84#}_v0oaZsml_=Mieub^R*}@F4cUrU*<-eWOowOJR6kN
z^aDtCCs}JHyPys%$cv_LQ!4wM!1ds7In`TfW*5+Qz+WIk`VOYDuN!E$5Gqw(X<k80
z)3HpeBqMeqO<>aZJ(Ybw1BVascM&xnMU-Yo4KPjXNp`xNTbb$NQRFN|qQ;M=HC>Vg
z%T@xllwB8Prgp-OQ3|_{)PCd{b4d}rs>|UM(sx*uedmEkAO2ia)n9_No@yZR&;|IV
z)U7w-u$AW(J9x(3=vHo}+58T)q^W5W;OQH;%D!7bewtX2n%)IUJ*iaU$zn{ao-^Wb
zWsb$+q7o0h#I!t<9G>-P)%49`W#3ZxyA@=4=~?C<uKE?gqob>Th5#nwjZ%+VNcv3Q
zV^;RfM3o!Lk(G-`qxpxleFycMc^_X0>blECVj6OMZf33`67!w|O+F_xFFubaPDFhh
znR&?yEhpgX-hjOG8+h{2TK^)*YVILtV%{?t@Sh96X8t0181tUtR(~r7HuF|;LFPTD
z*Z8j>s%_lL_q@Bt|6R1W`Ff4>iGaTf?QXtVqdnGMPu*-+kpuE$MBsO1_cX$FyWU3@
zxuw;wB2u~f1n#*m>UBQ_kkwP)J2=_FY04z!4Yv@bimXq8wB{+1|0qO)T__c9C75TX
zM~Jj5KurEBklFw<5B*UxsK;_~&3|WPKo1jX7nfq<R4u7X@!jbdpSTn^6O-j4|1+hi
z6G*$k*!;cqlEE*C!DSg4B&BG&bCM>(i>PPQV}?ZVkkIYd!H{|e+6y{R&UY(oOqNQw
z5>S?~=6eOwyJ*6@QxVHq^Y2A&JKFoHlw2|f)_kte5v?>AU_4D0Lq>*z5Cl+?=}Kg^
zDCKUY6*tosW{NWe7M>_HZKg!A=}U|?Un@A$!n8Y`BUr5=CIPy&5D%RnXJO$iz9v}E
zT6<?2ixUv~dm^9K+22p*v*45|nkpZaz+>%W6;>jUNMR$Xh)~#WkxL3$HbW?cv-xKg
z^0MGa3ZZ>Eu42jY^C{$G!GaWWdm0N;NS?4UQpjD&d=}g!GJazOZWX{25;)w-g5MCp
z_YA<bKNCP9GOYPK0`iI0Dba%1Mxd<|az*_2h;l7WK4aar$h}n20OXH2LXMWcRHiFf
zOBhXIIY_^Hw0ua>f1_yYBPybDj@J4YO3B$->syqP^RiqI%2}TcDGpq$;+|*TJ+}=Y
zJk{}A3<7>siMsF9Ap%|c1vs(nb>!lx$MN|+pLABT%i1tyDBR3{w<#A>x0&D<+5w>s
z!c;U4*aTGO*@~=Y+ZndzX{u5<4I}7OX0|P}USRdW$C*o*)p{ZFF+&tltV6O30L9dc
z0>a8Yn^9mn8<OM|DQ271N(>ah@+a!tszG8E$!t#%B`3(Tpn65|6cL4OE|`@evtTJW
zNj`;HDQwmGU^^3aoMcLY7a5DAh3Di_<)Y~tn@a2Q>0w^Plx!4HMOvT;=IBYJUYn;V
ziCCN|16y5G=<?|#?F=0wJC*c|Oc}I0tjjx#%Q9;wM5wlstg|7!Mbo?nn4|Nsdabhz
za@ms^RI1n7P%N{t^d}t<&#LLf^-R6QY^}tzc!|YcUS^5LiBP3>=IP9Nm5j`(#%F8U
zB#@-SuR_LlrBdhXOsu`ObQTMYnW^Ga3FjEIlX9J_gCUm&MrW<kb+&vu{qDCA0Aaow
z0B@lf01&Or2*7l!P*`!r6k&mt7+o0FK33R0nMYNrva-l-)of>RHrFm$&k};8y|QKA
z`K4&ws_Axa`PGD|wI=4LNQkC|GuA7D-eX5IQKc^^RVs>lZK^UU*HIiU`dST#F3)Ba
zMJC9zVivL~v&BmLjTchUX}A|j$q3((EZu}o?M#+W$r?j0kZY|<GoW=zT@sRumyJlh
zv22oc<<PYXJjZP{(<_j2-*_z^do12l15O@I1Da%_4y%aIOia#J^9ZE(8WoU&+dz^~
z5sk|j>8==2pFu1X;ZHUnRdkIe3keb+IRjuoIwB@b$ivUNlo!;{2-++uD6v*cz{7L5
z11VdQ{4bim9L?NX#a0%PX|3lG$bT_KsPId)iu9t<HFt2Sv1S_RvQj2I5S$+I3mJ)E
zX^&xUWnx7bCluu)+eF+I%$I4#t^6;(6&UU9=pG11<eP%*f01=$YkN;$cO<U8-Y5&g
z5e%UvRpLB`jlBb98=?t;f;SAy@Wy4?7!*87gBKRLpef8?qUmqRmUi{^k&Z;+Rki-o
z4nUwq36bdrym;EaG2+Innm8O{qa>sk5o495U3eCN-sTKPIuXZ!N^h6Ob#OF>D69e^
z%P76U3IwfqPj5J~wHt3COMw1bZaP4w8`=~7(T*esUbFVzgw7Jepfs&mjHf2K5;q@#
zX_zdyWtjk$>uK{NOu^uIg&|8WrOi)cfBQ>L(^*q2FQ5Cg39rv7Ae75kjo$>xPcA8S
zep#j~OnCcF$ufgTt`5yV03geJ^VI;Gx4q3wStfIl<pm0VjGJ;X-QK0!DZ1TCw`#hX
zN{T4Bj&4`eZBNS0WS;mi0Hz=3wB!t(GMzO!=Ctg4(0LQV%!L~NN4H<|4yB2QX|xR)
z9!uw*?b{(qRZHtjmb0JRVJa`&xc!nZ2d^L8VOg*-dQI0QmepC=<~GYDAk8(EDM*B_
zrT7aJ>qPAA7Rygw<`ky+6EDBItLtI|`Nf>v#mSdz<aI>8oXG1ma&yPS<|PlCS6Nn>
zXIWO6CP8HZS1?rDTI=;Rt_(H$f}wz`$>&|`s$-PV<Z1AGU7@BRy%-g0sN}#k{*VDh
z(&}^@qA=Y^d%whOZmjV&H`alp;Z5|6Z;RH^E1vP_6_HSa2V~wxBK=M6fAnSazOHTY
zXh(Z@ccgb?qU(RrpYS@?d77LxURSNJu_+jEdK!aGp?assD^%FrNSUFaFSIV;3A#e{
z$l+veS5uS6*%<ObH}#%I<l$B3&YlD_ynRl!*I%Pw!n@<viY2{~NH`d)?~g<pB0aJG
zZO|Gu7V!`zBVO-vHhF41UQe)<=i)W_=77uVkr=@>M%6CWox{@haJX@xXG5glg?Fjx
zE$H^m5&lLvmC@+aidp9gy2&gW>zsi)%^U(Ow7D<QUpmJ9p<0_08@fZt<nP00s??Hb
zye`_l5jBa%Q7M$p%ihu-O+-TU?m4eRlPlQl4{>vnD(=*^z^-cA*3%OqkEJP!;f4P|
zY)fOTzlVimjj=>hW2_&F+Z@sA$wJ++j!nFP<|gU}0oTfAuQPyV4SIZyJfqg_^SYW=
zw}yQ6q4lnSFC<!zXF@3}y}lZ!H`IVOb*^-Ue9b|Bb5L(;-ZD+iwYA8uZ}ukDs@KfL
ztz;Y%O6`UIa!nU0UDIEo$*&t*z~!t9)wmjK-3`vb>dZ~u-sB3bZYswC6&+o46aY1F
zoo8*Rrj^$b;DP>72hM5e!7%|L!-)c7*fMe*?~QHZsf!UZ=)muVF*RykL2FlMa|_JX
z8EA!r5go|jF@*J#c?M(LR>E9h>OL)APZQaH&=qKM)yhtwsVRt`9z5&Ow_wUW?fsiz
zStL_Z)gh;sl+_w?)&@OmU92P4w@q-a_l3xFp?9J)Ltw9O9nX;zTB~WU(udcn-~mF;
zx>e0hLEv}v;d~36%0Q*9a|Rm8vXb0zj7D!fi2S%HE7^CYt?`To-&!I}v3cT%j;woJ
zN+;FetgADqPxl_l9$rz7a}vUy-gu-xL4ACzi&=u6Qyb&&?xRLYN*8~VAByCb9m81`
z?TJ|b3QbUFOQ>;OQvhN)Yu%{-Rbw3i2@0uk7wtRH8;@@6jfCwmK&@RGoWX#n1^Kl>
zpX`AWTk|jJy<9g4E{369X*3gU>V|nVxf(oZOI_U_C#;6|aB^e;YBF&|g;DVaeo33m
zpww#Vx1dqp9<aq!=Fq9;Z=C)8?V5$*m1B>W4-ed*VhnC>3Hchmn#XQ#a;3SVP`xh@
z^7?rJa`=$8s5>px!_aM#ji;Rxld-)k%9G}Z6Jb)@ylGJ=8pa_GxP`S5f!rmAXx^GW
zOby6D4I`d5KWL`zplen$(WH&LeL=4D6zfaMT2Ym%EtlUPWz?Ir-U5eP=W(v|HNxfU
zI@IKGp~D!4VxwIC06YZF-H{C=L?+vSVKfG5q$DrVA88NMV3i`9a6i;DFs9I{7O7gI
zo<WsN8)jLkFBYdcyKt`oE?=lV;A;@JiQWL`N=K$7=&%v(K$m{)Ko?>NUxF!E<4TOT
zbuP%>;AzzC%Aj%H`E@^rA*#;R<O#SiF!PBU7pxw=ihB{vPBbfmRCF#36@n(C-Xf-T
z!cWG@<<+@rnpbkAal|IKr{37!`VxX^!vm2txavI34IG_21nTdY{zzx935yYu#!^ip
z8mT=^vHpbCH4Nq})LuoyyqM;YLdc`j@ZtA0c{HcpO*2j$R71y~B&SS<LxVp5$wPnd
zK&9|NUpHoxi3p35%os4}R3tuuqKar|4Dsrx8H1c>GkuDcDy%*St5}qx5N`cxJ&5y9
z&MljqYh7r?I%m+S4b?b@C$Wv#>j%MPdP|a@m%=*$4Rc0xIoVvCHggh<fF9^-fi<Fs
z(`o^8zs~OVjqyrG%+M;}7HVwvdYk;tT5as*y^7Cwj144<F*+cFw@GwGwH}W_ErTZ>
z=%vGXD%n*T4*Ai{l}{Mg`2wrqqYUGYK`%+CQ$-`cm=Wo1jM0X3(mJkYyUBq-DmTfb
zqS83E#9*KuyJTpGv2VZX^Dyo}0s(YSPorUm;t%-hnrnkx4s9Y}7}|(NdQ<dwpWlyu
zudlfpG2>*RSa&!S>+Hm_Sz?YZCdeUvU{DmNQDM#1N5(5=#KbhCt1|jin$=<uCFda=
z2aYh?Xy-S2u|`gcL-BzPArG7id?1-ebK~kp-?~P=EJ%cKIFzPgH1U{Oj$SdLayTr0
zK$@Y83kzDRw-_vxytuY>pp&e`p-9P|Ft!DFKhS#ET1*S-Le9X-<_3%jO<d{5l@-=a
zo!zna1gn8p3pB&HlWl7-gNpc;_C8IUaU3hgMcOdXsjX#<Js`uxIeBUliG;Rx(+CR}
zTIb{9)H)l<0Fonq2;B@bU!Li!31SUO1R2d<pSlRWE#{-D%h2brbd*a*Zi>k{tC;rJ
z`pLz#hjIQG1(jN`NDHd8U@;5DQC%((O-LcUHWj1c4t9pwCC`KH$O()vCr=Y5FewMT
z3E%QrzS0ZFuPGa?-x=+vVW1}#)+XOc$I+yXa(-8>r{05NNWD(4*U;54f7ez4GD{4Z
z0t(47`g1x=)?Chjw-qC#-v#g4sP#`Vf92z(v9q{+4L)?vRz98^GvG<(RIW)Ej1Q)n
zEo*TH4Ryi|8=AP8oIkb~k~<G_XNIN?wKTn@I$`V#HDG~)MH%^NPY^2}Oa$tqbnc*`
zTO~)%lymclh2|Ji`f74rhumOuppkcUS}5~EljD>Rx=L<=Xfkt^VQDO64>_CC<|A}K
z0p|l<!RToMz?T|=H}@tXOjCGTZx`~_vSlzUaeG#}(}dw`WPOpI=4%XM7%h_&02^-h
zH#OI+f?9*VV5>hx?Lxw_Y9>;nC(-*apU<J+;Pkyjv?J-0k-{mG*bBx%k(goNyhTcD
z%l}LRBDfoOeUjc-UuKb8)@xued!yNqdh0NIL_fd;KI5D-M5BpV0q`M_ufH*+^VMtp
zMJps_hoV8-p-A1!Xn@x+xKDjHS@HLdc;uC!xoBnqX(C8d1FUl+TbVrUl1nAsjo7uT
zqjQw)wdbCzy#i}r*pnFOjjY%h>5X6`3CZ^Uj;<Bj8O`>E8|@2yi||)zUx+m{?Q<^d
z9O&ik5(fwk*s*D0Uw<qS>A=F6HDFDQJwbBD+7v#ei=}$t^aVxAvI}dCii(<83|(KG
zN+}oL^B0gi^=ccN5@-)cx+9pMl9pWSJcextg6M`WXA7xjli~&RDQPuI;R#G>dbZIJ
zU~m?qCN#yS-8R}(zzDnn${6SqBVN1<Q)M|K@RgX?wbljK>vlG-B#)mQE>l=nEKXbR
zS5)dFJ#xt14|Ho|iLq~`PJph`tHDCOk*ydU$bgqr!GwGHRwwoY69fIdI2&{z)eUCc
zr%sN~$=NQ~wQv-2ou#e5LXE8+efX#Oh*5K6s#h(y&&RC8=^#a%=Ne_{+rJRLy<7A~
z+J$PsoD35(Z2?|Sd+%Jh<Sv}E#>clicqdBf?b_&pM$vXVNmMz-)i>Wi`|McW&UW3;
zWa5q)6qQVlm)0xrS~Nmq2am>3qjH2<RZ$DmtZuBuRg`NZvSxKrCf3K;siO@+m)Ave
zxJ-nL`3MUgcQ8Y@!t6LfC-CZ%<z)4REU>=RjracxzfG2<{3>{roU5mv^=fFDj5eCG
zrob#q_Hps!LvlpsQ+Euw!hp5uZ`$^o*SRv2605?rWgo@nx?yKMq#aaCEv!uuq{9_c
zD1C>8x&mP(MR8(3rxlxCYp~$KZzc`2nGkf=&|Z8>@yTg6%}vC-Puq5*d2uV4@}Y+s
zU(WK<=Ad%G!;!K0Q7DEvqudcCcgo!b7cOm4qN_3ayNbRMMMWED3feCkS{3=+ENz~{
zda+x|Xk$aDUhZSd=~=+#3)Hy+TJJSZVx0ln{J=b<)+v8w(tWz<!I*E8EKKW;W5)mV
zjfqAdAIAAmqEDqM<xe@QndaqVcXgAiE!s5Gc0)8z${ox)r$1%cOu#jMAD=yG)u8EA
znhsnowJuj3=1>?x<<N<V5q_xX-^fwxUF%%5*UV>wnAejo$sE)8)cOK}X1}xoz1npF
zzD}kZ@}GxDrm?H4<gTE>^Q0P|Ht@nZVKrFq^oli@oY3<|^4Bprui?VMq*C-De*u4L
zngfqg(8Cl#8J$zCfwI^;N|3(^z%j&468@J?xJE`Lp~6~?6cm3$VCM<mCRV;*#%JrW
ziv2P!S(#Yn7zyOh^XCdW1&_9hsC>ShL~kBLD1X4eAW0~r@`3Qogz{<bH&h3&7)j5>
z)4K~qNRrC<VjZ?<zl>j~!>abn_(~nNc)yJMb=Z>qGQLBHoxlH(i2p@|3Tu+l6FSH*
zquJt_U^9zfy<a9~3Lr>%bqqJj{ZYNd^8GR{$xW<6XC;r@&(uzvMKRZj=SUTHiHzPT
zKv`_hiRgZrlFWc;5Q#@6uLlHA5Do_R%hWNwLO~goyfnmgcJg_2S#mzpA2#1FAA6M?
zjCtvN<a6r=KJsbrOnmgbbUyOQbORsx@cS4mUM_TJz#W&b>~=O`@r5Fk-7H;>Q^LUI
zW5c^dSyC72`0|nbH^fug?}|`9j-QS%AD(X#d|O2*ADmCemrwQ_6z)Sl(sNj(r8AQ5
zLb?X&2V{?z-CI7S^O&fo?AYVgkCn!^^<Z*_Fww7tT{4E*W~9H5mG)xYRf?m%7bY+c
zXem6*RZ73DgcnA`tdvq+?eQ*F8s63mep-k@i|1^{(fn9orzAqi>yLD|6M=~JbthOU
z-%~72&|0FDpE_TP4<gVx`lXSsP-lO8PbAb82CYniX-7vWvb7`9mk9AWrp6=G&VLN#
zJY@Xx4iKWecATsTx)_SUt(2Fmm4T!BS!qWMPdi{eUmA{V7}yBn_TG*7eM}_jn*%c5
zzF|XuWV1}ss#V6JK^eu5dNL|FrJKM%na1PZ^za)!?#8cblS70&(9bW^QW~y|Q+O8s
z==~x|A2d0T!ef#GVCSji4bR1;Top(e=@TXg&rb;Y5}r!l(&_7vg(C!Gvi!X!2S?s3
z0I%^Bo-`LB9y(9C8j-?-n7aJ(+_f9<FdtomqCmX&$@L`Z@ysT_*5Xd6vV3`tn?)eX
zbK5lRvHGKD?&-`lNiWYyJBCNn>1yNmr2G<3hMSO2XRyh%Jcq3u58Bf;$nSW>9wEso
z<riRf1tL^MNiWZRJ0$4a1Y)}WWc_!f(aUq>3h~A#U4vv?xQHGv-|6K``kMhG6q1iF
zdG6lsLM=T*3-r5a4n0lpFBs_$6*_p{Awkd2zSObtDuam2@K&Ua^wRSUs~Y0lQm`~S
zk!0UUqnGCd-XSW8w<J=obomb;m&%v=9~L9eu%O5JGAWlmC52ZokT%lG@2UBP;=*b8
z>E+**Mt^vQgHs$9^nXk%Kiz(Y)9C3qPP*jRzrMgz$y*vboOpy6HSj08o8kL)8jWlx
z$sxV`XOTgsCh6t5i^p+>2wmy;>FHO1Lo!KvI<c6p6Q7g*P2kGDMRfAqLHS)h`Taa;
zMzVZauhXD0mM^~pae5AM8()6O=M&%<>6yKa$QZNpJKL)T`d!k=FbBj&`n`gFub`K5
zN(PcnrYC@yU{Zg>g8rbOKQ0kPD8M>Y73G)f5G^9<&rO4qtaQn6hM?D=$@?GdT6}jB
z#AA|9f)|WK-!WIipwoejS33RTap?P2XjD6-;}lHN>FIlM=`cclw?-rQgH-G|^gcnK
zuD_%88o@Qfjs>}1mMnAqIOV&A<Dhe(jF+U7u*f*{c<P*Ajx>U_BtJ?mtq4wsPX+NS
zo<@+C<n@uhQ=`!CT?s9ubMEMxhrgxxllEIoKwOlasr>m@@oaWTc3dr~>rW&<GcW_Y
z+T<96V}`ev^CV-Y8Is8PPbxf1KQA^Fo~@q~n+nfiavqWj&t-CckqWmkId4dX=P@}S
zNQLL?=f$SN$9|`e^353e<+z-RkMH7%6-Fw2>~{t=KFRO*rQ#Rr-xE!Rt4xk(sqje{
zpBzRIkDFPMKK>Xp%{bjzjw`9~DHtc(j36F2v#I(xV9Ye*v|Z`nQ{k{idz%r&<K_(C
z(W4AAOMYk2NMvR-n;(0b$IRk295ertXeRvGV}XE}Q@%iyL@_gaGGU0B@x{MPmzkY8
z22A}ri@i0Po<Va16TVj{?L<r~IR^!LkZ-lheqB8dpIX2bHnzR|g8zxB4#bjI1W_ut
zP^_S3&Q<~Ur@^&(><o6p+%}FX{nYJ9DH-hN0l}v)jn6TTKh_S%s~62b@M;>nA<dq(
zdBNEF0zIRi^B}Oo#@gW=4j=p7$#Vt#K$_jsx%M~)aZG>l2)H~qo3QAj6C9Nc{e{LA
zdj|XO101g(kGX!p;bXspd6$5v?_B%}@J!$L{M$JE@un2Bk0}*39S8mez%$8t!8q_{
z4j=3HE*=N}3cxeT^Hsp9Uh=yNg+k`TfM@#t=~2KleZTdUaromMF07%(e*aVjocPF{
zC)ppD0G{c)sPv^O!k0Twv^Jz`7vP!n_5;pm?Ds<-=J*qtL#&&q-_mt_96s4t01<yj
z+PG5!IO*-cbO({M`TUMoRt3ID>H(bwO!)Hq6X-eo+6Q>1@6Xb=Z%7aJxosS;MMGzQ
z8HYa|<d{hh&43gC^zYTK2b|<NktY9DfRmo(4q!->`)>j+cMfHH{TA>{<$f>@JP-3D
zvIqLSjPWW2ocJ6_(=(m5P4d`9d&zbW3Vb6Eiqo0GnZD~f$nnR1NBBApA3G1BgKaa}
zXX;CynI4KX{FduIUp$P3_fY!riPiypz@-C^E3B%;ga7oKc8DcE8XHRV;6tJKY-}8l
z0BE0=HGo2)a4fX3JGKE22ZZr7N<7p)uoXYg(fhB7NVs&#((~z=y$neFJDS>VBwqYV
z^l!tjl(eH69_Z=8GsR+mlc)H@N-3xzJlyVVaN$AtI{Nf2mt4ffOH#i2dj3c_fAIkr
z`1Y-i9gfFBUG(-V9>YlGjprte0P&^Ly4FT#gQu2d{`6+@A-2E!K#}+`YVs?hqJYo!
zG0{v9oTofC_jx`D%zrm`IQUm9#V09^FI|Zoe9w|TRw+Jpn*KG{Bock}@-x3Nobn0K
zu}{R}xmG;F!(UwZ{9iz&ZyHNWO#PzqKk;qU&+$!Nhr{qKPeU)t_(TMMyVQWBKhPq+
z#rltbW;Ii@kN1V+F<%W%lO45{PugU7A{ecad=y0&{Ij3L`CK1)HGcK?bAF0AO>kXo
zU97+N{zRs#{G)I5@)uJ1$CGufWqQAX>rH>;Iip!5ALcU1m7xjvBjN@Wy}ILkHkzLM
vPwS5IOUq-w13jKjGs(ffgPZa&5Q-6>ywzUmNiA0U%JZ0q;4`U<*YSS<@Uk@{

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/example5 b/3rdparty/qpOASES/qpOASES-3.0/bin/example5
new file mode 100755
index 0000000000000000000000000000000000000000..67c0493f8c2aa81031ac0ac699c9d76e239fb470
GIT binary patch
literal 39753
zcmeHwdwf*I+5eo~To#rjd$@+HD+UW{E*laED!Q9w!!G2;CV_zB$!$Ycb8B`3p;$#x
zYgAgWTB)@X`&MhKt@N#}ms&+Z(e~{tRco=@O1)IDUMOB_>xJL<nakO;n~iPX&-?rR
z@%wOK&&)jYJkLDy%rno-nKNf{slU3)rfE!FHg*o9)XUSn93<|Hxi8~M2`gjcn3tW(
zPGK2<q~qu1ITVVR3O|ypRp^j-c7$p8mDvQK%;x1070wc1ra~nlg;ZYr^2(?P|9iGb
zst}TM0MU`GPv-L^!WJG;;bAGq0TPT~Zhj{)(H)j_hb5f~U6M|Ps(w^A`Zdb@Mw=HY
zN_#0_mHZwL374+=*JB+N@1Mz&2)#U_!VUpuDpYhUK}T}_tCve6hGcnT=_TGif<U!H
zVRxi$QAuHUM}BvtcW@|wsBBUGqLPAWUqLaiH}NOjWwlM5Q0XxjDUx|Iexz#3Z!0Uf
z_wpaVcJEE+zrL(9M=P9v*Kg<Q$eV;8;gU{Ic1>)_(9+V%Y?m>1o%b^L0#=mUvc(zl
zI7(Ss2FE3y#D4~UWH)p0I~l)I@SBI<7w|g`zti!ft3U+&UWEI%um0nWZ$A5O_Dk3O
z@y1&=?s)a0OK-k-&W=Cc_`tTmX0xvJJ6`x=^Q^m9yt?P-uU>z#+dsVK!f_SPKXuvo
zXX^j*qsKP|FZ$W44Zm%#@c;I$36o#B_3>M7A1-ct@$Jgf-re}(uV20O%7S+{PB`->
z=lvazPu*Yq?#A8&`=@@p{TGifn0=ixaf>wZ=#QYdiSeWv8$m1;oOI^KFZr4afIAt8
zbaV{<w~YaRbPV{SG5CkZfPY_0E&s)1z+VENRQ2s1170=;pU1|4KQu<Yvd6%`dJOz^
za87es7OS7`g=CCP;CIGug|Sa$+3Yn5mtgjHzzP3Au9t(9|F|87e=^Hqdu9mud`a-%
zyxc5SG*Q6sk<Rc;S#C0)UEnj3IoUy%z?W>;C$e7K^+fpZ!QX`*w7aN9q4Mxe$f-jf
zCgq`iN!Onx{!UK|PpjK^flu;}NPM@<m<2)?o6bg(_~(Gn$tZV^gf~n499gfUQa^@-
zmvTKIA@v}^Y`@f7zbrRbW<1N~oIa{vH}H1JVuxhA0F7UNk$jl+ze@je0iTHWQsbjU
zXSIM+JGLaX`;8L6U&6zZV1dLxl+^BX0C%U*PZV&{XT7W!`8K+?Nc|-9c~a`%EAh!6
z)3sdc*)7|vS%BG30oS4bEfTKmkipM?vA@pO;BO%3zObN(8OHjazFs35YaNIg1~Zm#
zFhb$=k!UPDP|@8QjfSJl2sYLj9pM29Zmg;3?&}RVwzhSLMPBNhwY7_7S!FBh2l{~B
zQxfTo#QgOuIvR?Nr2nBveFrmA(0EFtecc<v6@vo<;og{kWxYR=#Gt;TVId?>L0i_<
z7vnWcE>o4ZVw8xU()Hn3<zQ=fLtpn`EYjEOA49I<qG&i)(bpS|4YWpjW2j-rU^^;T
z+hBMWkA{<cW@$^5LdrdhQC$5%r01U(Sn2U^NLkAYk3SNjVm*OyG#Y8`ZQRr!_ETF&
zV;$|MpH5A^2o2pn)N1q(4up*jk%1U$2xg6|YCT1iGt}A^+2HXkMx0vRh=GL>>Fw<E
zlZ6Zo8U5jbXkTw@cO<sS*Z{LhCM^b=Xe0&o+K%q}4J6F^_I4vmvUr%$)Vnd#+W||9
z4)%m8vt0?<KLBNR8tq*dv7T@bRLoF`zQGt9Xzhi87=$aWwFi1<M!2Ij){2ZaNX^?C
zAYc{_Mh9qGQ`j0qh4R~4qhS#}v#_hLCtMhbc16|?4)hhax5h4t^cowZh3#FDj=q6N
zVcTG&yQ8pcpg+1PTDWkaqaR&kQ=znF73QP!5dY|zg?;_8!hybMA!-y^-$%fIUPecF
zL%5qzRh8PKgGML12Whs!>^Yt#HJeh}Qe!ZVlhWFPb<jo!DmEAmCv>b7#wjezAVn$_
z#ye9=5t3~&HBu2bSZj4siYnTa6hQ7ln3n37DWru(uC-QwOckw)t7t5oc$Z9BgZ6Ij
zY!W>{3Xv*{6T4<A6rqgI7m4#(R#9P)XRc^&_LT<>Pr*XAtU6eZB)}yiZWP0L$K%BX
zrSSY}w6kNLt>SV(khvL{#asWSTT&Farmc9CLmUbAB}8caapPBi>Lw>WQQeMli!KdV
zZ(}Y(`9=6Wd(TD4NoPOAonWyA*JCKMv->UhH%WfTX0hp!arBh1PvQ^GptOx0koX@J
zwh$VBhQp9(UTU5}Xl@gVxE%ioUTA@<agpF8r@C_Gd?gbARKvX77PwWj^DS_zJVh3`
z6rJ-bv%sy-cU}ve%2rpv0w1q{2<t6yG=O<ETj2EEq^=eVT+LxAuge0@QpgDVE%0m$
ze8>XF?9sfoSm4yo>blwjr)NNQZMVSZDj>q07WhO9Jh1gGN8sA@DJQvE;L1I*^ura%
z3~b%!*lR&T^mwTaD97ie;huAn7cokA5z>)E$8pX3#~h@oX^$M>>0^|pCOxu;r{APB
zHRX}rJpBr#F<WCtMtJ)7l%^&<vYn@&p)@t+ku5y^OG;A{9_i=l$0$wHuOlry{UD`b
z)9gq+PyZ*SscDaRdHM&GrY1d7#M9rQG&R`~H&5S8X=<t?E}p)g($qvp7*AhAX=<7y
zM?V8#-sP00COL8lY3Ohb>2Nr`4Z+^+zSX`}z9wH|Q$yggw+>I$SYX=~d2XmDux<O*
z6zm5jX?l+%u;jqt(PKHT{5_~mVB6h!M<FHBOYcYeSdM;o-Uz3=@hEB&7(UEFhXCRc
zH&o0cEh;i1A`kK6i0=^R<60_0@SCrQn?>ZIycR@qPO2O&S8(_(zvpewi@t}~cj79@
zIq3>6_~Bon(#LX6+Y{Wnuk2XP1?i_yA-){FGVjU5_sG2AgTBALGkkFCUhUD3PJ$#;
z{*3l}^dGhbkNLJ$PO<s6KAsuaNyG#f#0R#1T#V}6u#c25_|3q!%Dk4qw)(PUeN}J!
zZr*NQQCI$MqRw%Zics&)YYA?<A#XdXR6TqM%wl<6+mSz;tcMg1Q1eHm^*00C9?Cle
z=)?H22gukSET&f3Uzz81;fV%efU*!)^Niu{y!yj?K1F+Pi{*J0`QckRq%seha4prS
zm~D+D_r0>oMJa*d%Dh~vQ3(|QeUX59d4A7Re$O7tbOnZwK)8$Z%8*~uf&9ZIob$bT
zBS@IVz559XaBrR$-Nytj2WB;Zho&;-+4I02R6nromb^oGD21x&1^C12;lDjdYhv#O
z?g|XQ+zBkis)vssJ_9Tve10XA5Zv~aLsKZYZ+lo87cqrocjvjM$w-TejHt*V6>;NX
zk|VdMNY|+na;J(MR+0H%kXc<Sa;J(MMnu^gWSe&<yh9*;Cn^T+BVE+%mK+^?oqE=x
z@5VJzJ$!av&G6rY!v_PyF9@{~2HLNB_}Ifx13D4Ze>nK@@#CF2tB-r$<`&KsKVOyS
z+0)7UgKt}7I*N0ppF)MVKXWW+%@xlHZ8TTzYk@gwfvq29f>?kD|GQ`p`A0?e7B!I~
zL)ozM=Cpm1j4T~w01rMb>_Xb3X&<nSn&FQ;PX&h4@;uK}4}WMHh7;104IHBSNV3Dv
z|3d&>Xhjp~0j38+Z2=}z^|@}}A*85c%aw%ZU$<`rFp^Ew5*ibAJO9znoW}GuKs=UX
zROZcx*tie=q==Ms1!x%CHrWnl*X^T{iR=6D^t|5S%FWc|`d!X-$*~;4)t=1tr(5?f
zA<q!p`b1gqvi;PJw?1Bc*~eb^;+!ixxNC8t_=g>MWPb#??0&i({=wg=?xLq1{skp2
z+eePqv*)~ZzVm(Sd>8nPy$T^Py!Y@o31|4_!yYKj^LB9fHx~qkUkPk|?Pz^taQ_}!
z=m_k8l-45x`}fl_(&1(hyf5{5|9u!xn*#Z}5kN`Vf#KIT+Scv8pL9V3>HF&*p{(PF
z6h5UC{-Yn+fgc!tEb!=?=LWO`f!};I2;v6?@iQO}3_l_0Uy<}%mTJ@8uoJxrl!q_D
z0JQFj^mlUIOnYxH*;`=i(y}Q)h|MuYnB4Tpp?pX;{QB_&>yB&&NzO^^NK}M@{h0@#
zKhTQyOHL0T_1#Y%A8PUa?eOqXIPjc%HafWT4qUs;Ro(vBv5Fj5^`kBr!?-~E^IM<L
z0_`87>C+Ct#@E1vnjb|LLf3?&o;?H$!t*viN%(uW%kqxA_BRp>@(X#6JSWo_KZlR|
zRt1MY^fd*Cf9q?k-ZsxW*$owL%MT!Is9x~8=f%LIpW0~#bGQ^_fh*sR%~AEK9)1(O
zr4l@jPkt$Y5h$?aFYx!6*jTK=GM>G+7wtxKq0v(xY86KFZ@Ow)hax?LJ#IX-#|8)5
zT4^1_-M1k;;BM{ic6USu!tDf#p6Q+!w$3XLp!w-i>r(Njl~fOIm^*s}zFHSl>r<Rb
zid&pt>IX7KqWGxwPqqG<yuPXMlf!-(4P9y-O|9dpb;oUQ|8(SUPwYO2BeA3BitvyO
znU}FWFu<36GXD{o{|JC|T`JPh_e}d)9Z1lGZTknukMBmf`@`eM>(STuAf!1|AI7XL
zfVW`eJBY9!;ZcNJ5IT^52%#I{QG~S4&EPu@A?!yuUo<Cwzk(d5eJRAWA(u9DLZ)N8
zmg%B*B=o}J4LHx!2ti&1rz;Vku{^*C|KR(gOqT7c%FbP$<J{=j!p@y>)@cj#<`Nyz
zSK_w?vhx`L0dW!heEfz|z+-^-18yKx7vW!r-~E7FpF0TtUHo>ZfIkFyGjwTvz95<D
z(!Nw*%{7o|r=?=M&;#DXA&=@-1UgznPrG-vmz8I`zGf@W&i$&rJlnl3tvq}Fb?LtB
zqN_4|*=1WZmt~LqD9yepeSCIVMRt)ddp?kW45IREhmRT*7wMoHZIKH(G09+pr=NKG
ziKm}<28d^Xc-CZlyR*wsMiM)&w;RC63qG<*i5tI;AJ?Y7Xot{m+lMo6a<b3HBmM&?
zZYQo28aSbW6B;<7ffE`yp@9<`IH7?P8aSbW6B;<7ffE`yp@ILOH83Lgk-4$RLDv?U
z<_X+rnTW3Bklwvp;q<QFnx=Q{bg8wKzsR&&n^F4;)w^J|zj3-CGxu4~lxdd?)w||{
zSjnYJt=FivqYpki-be7AHZKL(5g;zN!^<OBj_23ab}tXrn&e9oQ7*@@T!F^$-Y4P0
zGQPOv`lVW9p|>z}(RveInId3#2gEOLj+ciDekfa{mE0{nmtlXrxK#OAn&b6pk@#2>
z<Y^_xQJMZE%PT4W-vnwOdva-0bs@*8GF&9XDjBYlVW$i?$nYu|enW<L$na+}d_soL
z%ka-K{7{A&(tfAPFj+2abve^Lzo~7oH#X=lDJUx_$uAz{DbFRIl7ga=f`tnNoG&Aa
zW)+R2mWEYRsij_(R!hBVIZ`blD^piX!YZx0T8@-4c}s4x=)T!vJ3BVCYv;or)4E|~
z?*YrDI;EtrInri;seL=rj_1z8-R8*50kTbVTn)pw*)l&xf3l@zPUgUjYdA3d7cjQ8
zv~=u-13~)3c&}vd0K&Mf-C$vFhY63nf}-Gd6M}R{hGQoswTvCaz>)p{NHW6!WjKC}
zdnV6yWR3@u48C!~VZR6lJHF^?<c?bmqJz#4>Y(HCpP^qnKO|B%{uJ20^W*Cgb&S6T
z{oMHp#a-hcf#RH>QapG3_d(?RjN<O`t?2vC;}oCo^r7BPrct8E`Fl!eJW=L+4&pm)
zJmGa-0V$kmJP~mI0`G90={!;IoK1`~c%s=kA6~)f(7pm#i*s@s663XPNOU=~pg-pX
zjRfv@euo5e@kGp71symiYEk42IZr2=sag<;EzZsGY0hcdY9uaq{+bHS)xHjHTb+|g
zoLSm`AaS*GIWd^0T>`Z2&Ra<@1sb*FPUr2EDCCI|=iQVj;)y$*KcR$&Cw4m@q(re+
z1e*Ju3$Y2tS;A%8<D3dF>s+Lf^7gR_l`vfA&Er0RjJeJcs^s`}q@>#@%EmuRY`#O)
zb&QWuliWeIb)h9!0hZyQHq4?HCb6@J5g{nGCWk(tpp#h6-_X(%t}34h@;)dbZNl|`
zK(5{mgJ$|2XeEugu7?q)O?YH7RH1(jm`tCFLN)y^h-A}$02SNy1K^XUuR}ate-UcR
zV6GUl)6%?uM2>zIcwef2nG?@Mtu+0=(1135BTBIA*PuRW`ucPtrkB<k`q^lMOnn<%
zu|t0!RO9sD0Ww~Hfs9W74>a)v{d+L3Ed5>Fv-MxYj&t;<K<m=?fxoW*79vj6Z$dpL
z>6bvcll9HOoT9r>)>Qp|RC1brGN^L(+dws4KMpZx=-VLGOuY{3pQUdH{%rkK;Lp)d
z1O7?+M&P)0J1Tjyz73Mk)vpHaDf(vM=jl&CP4n~;)cREY=1KS_Mem2k=IiHz!veho
zGMuJg4cSiDw}HbM`dVl*U;hU*Q=m^pxrKTGAVvBF)X}5AgtChD43x4^{}bw4qL+ea
zseTT0vPjqAWESg7A%B_vW7KqsUIq_wrv5N&>MXq$x;k4w1<kNjpA7Atqt}7kx%v|1
z?qT{VDBw}1Pl6PWF?~M>_cA?#U?0;xKz|(D#c@5s^s^D{XZov<`bnlwz{vX)(+$Y+
zOQwGc@?RkjssCd7F%<S|rZ0e`|IPH5p^&GU-U((0n0_e){0$0%9-d+Pmx1{#)Bg$~
zo<l)U;BT3}8YMmt>q785rl*1DL8d=|ycd|>0`lK8{WYlfMW(yZDlakpCy?b2O#dRd
z{E_JcpnI9=KSM1KF+Ce4{E6v<X!}<XfbLb;GEiP)dJh!+I@3RffPZFs9UABjrk?|p
z!%V*(sUu8Zgks-h`ZB2aFHHX(=-y)bbmYCw^eWWo9n=8=zRUEd&=yCTei{n;E7Rwq
ze#e-82q^C{{TaylKGTaq_W{!%gI+#_fH01~G5uZG!beR1J1X{frr!lF|A3Z&@-fqQ
zgZvYw??mt^)6a)wpD}$l$~z8Sqm8y``f><(nWo=_x?is8_oGHzHT?w?bcLoj0Od+e
zzZELDO4Hj>&97*BAKLF~O@ADzVNG9wg09i@EJ%H=rtbsYb(($;f^C}q1}y5Unhu*}
zU(@tYkiuq$i472Ye%9i<A@8If#Ix?LgLup~>04-pte;ZCVVm?8Kw0;Nad+7!<wI>*
z_fulBZPMw4_5dYvZIg<i*Q^IAG0QgTH3V5dr-a)!nTp9;rZqty=ixV{i3$<W<=RDn
zY*5gtL?N?kwW|Q$VP>7jsY1jnq}>Ma-70Ger<&%6m1H%2hzhp?RojQ`rxnUnPCcy-
z-7f3=BY=|A)80Vjn292|T!M5~`wdVjM;VXE6#A_P&#65m!?f4XGFg!qP<f7$kL<D}
z6gS!;tA7anmAGp4$Ue_R5e4<4DYG{G42{iEqR74^31u$mvn~#zK{?7dkbR3nVbjil
zC1!0&M}zR(dy(^CJojm8o3DHfJL5t=gPcE5E-7EAd0GZ^kagXS<U~muA0V41!B&)a
zNP-<dg4#GrF0$t*ph#A<(J$+!S<u}+#56B(RuCT7nyBwQ66m%MAuh49wIiGM{R(TC
zHYXjqx7SNqY?mSDI+ZILFLw&*^zK)HNDOTwlpW{KGHF1vd(clNY+C??O!A-8zlXN^
z3nDYlgtoHp(~4B9y-egbsCH_U8kJ4|D&kX+JM$bEa!$2&EjaN?XMx2c{H9z=s@pWy
z?GqFrc+KjAq&e?vjZg>YU~{FD4BBdNX(LV!v)q~0Aj|oOHUbei?q=j)WtMh7$;Qn#
z=d0_K=(i&KHzvxM0>v}pqRm)==E>Qsc|gvK`?JZ1lsb0pNJ&ytv)yQ=TwCD`3>5U6
zLpP?K)Vqq=x`>Ozc2aKzvqdOA!FH05*)Ak<mu)gp+7@aCY7)k8%9T_-0bQgG0&<Ol
za=Q{Y-aZ1!r`-&M+fAfviQat@ab2!a<7p4#_PENoI0jqIZ{YlP0AKq9vJWeishq{h
z6Jh7JwOim^NNCOGvT<%{sM(a8KqMsL8WrX;yA_IbXiKQ>FXA`%Jf!a=akR;lr7hL`
z0Iwhhyb%S|tDOf(r-Dv>m{)*41J)`v;-ziI?JAXTsercZNmO$mRp3@+e_x>p7Q6yl
z=nZsKRRPZJaYFlLstROM1v3fhlv_}Iu4{_+Iub9TkUY9&Ow0((%9yKZbH9OXQL}l3
zsm(@iF@94HfG?MD%JWphOSmzC;zc&~<s=j~>x%%oW@;J~P1H~{qf#}3ZzO_Oh{>##
zxVhXKZR3<3hzqCf1j<7SZK~P%h1pWYxrqv&bSixpDad|BB#F{u3fK$>>~dX9m3<su
zX>JH<QO_?t4N_NzM!w8pn@5P+B;4lUH{~$Z^&Jv(GUu>GD*(7eWij`?Ff`XzjreLo
zfE!hoXwCVz!aKOGI);u%cE-{AfwDoNu?0KOG_Gq-Mq^T8z_N3!Zvf*K!lIcoTks}4
zhwH257(mF=0V|DTJp_#X3QMSBK^GNy!&X#>yJz?#j`{}B-d3n=!CZJH*Ee^eD@cYJ
z94iZQ<l@JRl?-W3$+asNHI=BD9JLH+fdo{Ee>D{By1fW;%Jy_{eA>i*c_Kb<RAMT;
z&4QOO`*Qs-6NV=#$8pN<fa;;or+ftCnd_$p=q2TNPWck3(gBQDXDTm_cW!P&X6aJ+
zFQ<dio?pXGB_k(iG!IN_|1CybqF+6Q@kKJ4z!~*|$sHymu7J2{f39p(sX5sKF5n9p
zsITOk#rZw~o`3%z@ns9{hQeKM&?n83Wj1Fy9qpKhpXdc78(UC_Ug|pfDTYgOjj+QU
zj#>${<%BA%^(dmQ4_3lrBtsX+x)9(&g*Ejq@MF{5VCDMcR!m*EwQfW9*A)soZ5<5W
zb^IDI(!K}i4=IPb2|N8mGD@4ao*W82FbOcrBoD*w^NfGMwlls)V!wub-tH{lU*CZF
z{6hdbW`%}#rfa*;F8Dea>PtB6g)jg9g9FDNKC|Eos8V0bVczS0IyC#9DV2ryp)K|D
z|3pt5DY^hOX{GeYo%tx3Wzvw3$Mb!-+tSiTV0IY}vabvW%~U)Nns$45JcGyaBoA7w
z;{cxFpmH)C#3RE&uG-_EVaCJb89ZK0%*bIa1j>&PmN@8WfNygq<k&rqL;Ozh3?9#L
zkS;PDzW}AjL0&II-l;J>j$d*J-Al=#*&TR3nTg+`k3hyFPok=KfSP5J&KExfGK?H{
z50GjUB9AD<MdV~c$YPn~fir2SD5U|=9tLno(eQ|(VVN{!meM$HzZMv`Dl8s}W6|`X
zw3K-L7#KfSSj3B!J%GFR5^hTjs>)u{){=Ze=22EkGZ6cmK%gZ#UPEGkra|+VMTCtg
zrZI~}6tkEzKJ=gUMHo?f^>!fGt3bKrhcHEvUUDx=O0VW6Wx|=JR~G?`o&7WvYF`W7
zGl!)4v9o>*|DV}OsP>J-<vN&qW+!!kg>30+2+|0HNnZcdZn+t_-@)%3(w^4DmeM0@
z8ETchG^*Y5IMCiAR8pbVbS^PX#<DF}jF^|^M9B-P+AU64cmsYuG69NcP37MMrfn_B
zPFKYcvz^zBoFzz9_Q61G=dUBgke{-vBFxrGxP6pjnKYfsBxjpR>f}|ehMP%Vy47yE
z5<JLPE~Bdc9HarNfx^SwnqOMwPpmjBc>Z)R&-gD0vRrGjzXvS|HC+UF@`|u_%Ud8{
zi6&h}YI+|i0aB^LQ^nYppJm12${dZuMI|2iWTiWm94lyKPF{u9Zn+-3&n5d!&a&^V
zJ^=9X<28>XfQi&TPsW^2`b=KD)^2$R!+af?+zW`*+xOCfAk=U75PCkSt1gs@X_RxJ
zO0`p}gV~G8_0X{kD+A14O5D<RY<X_;N*}W?`i!Rz9ND~bJ+m*)fpXJ!?0IJM%I-Gg
zmYu}Y+kdw?bTg`A?<J3wwqxX(&7n%Pt$l#}81VnJIb@`h#Zy6PJ9Z-fo8W67A_3EO
z+<^3pFbey{g8!ChH;0ZA{l$fx|H$t*H{6DPW51kS5N)yz&8FhEo-4#TzB%NDYqeh?
z)8D!{^bk6y{mOcg|Lnz~=g_O{SIPYQE)LxWLG53m^=ydutxKvRP}eX?lZS`Y#-GEz
z*>{q#RlNf}%>GS^lC=iNvAjLW3q)~f4TbhzTBTVw4t<W+Dr-IiPI1k9xMwaSKl2%Y
ztckqM9h1F8z*_LLUvm{voe6LqbhE4wkwJ>&A@WT`1j$b&$q12L`+y>OQ<2cuE}WzU
z5kc|`NpgTluA2Z&@KSmSlS;Wkk=&2Ss@Ej;%g_b-jqOt08?~b4+DiL)`c7;0r&GLO
zM2k{9U@i>Ap&>HI?@Dt(a}C4o*}p5A>*yag2gpMnpX%ilw7x)>{Rf>SOFgoVk+2-8
zX_}Yv_{l*O+V8nmA+4oO*nFep@;;j2X=Fi0Zc&iZb_r4|N!AeEnz`(25VdU-R)vTI
ztMko2kUV|~9)C^b0bb8;W~&~N81%qzDIskKDg#N$iC-rrMfLe*|49p?%PvILV~}*!
z9Wp-~ssIbk0}033=@TKkmTRq<JFb3a^M{TFjIEg>O=tmd*LW<6wJz1(;B65^Ta238
z9K}@CMXY(Q#G+M&#}YAl_hZc$OAK05n2Ek-;l(>Q*rDprYz9Ny2IMxA2Bq$77vt~|
z(j4wC-aQe?<^7g5?~sg1mrF-6hAx|xm6I-wWDN9W*8DqZQnZ?}I}yX?@KQ~wWka?o
z3C8bhsBO`#B5*+T`z{vJq4%=2KaskB!orN5_d5wybw?wq!(d89BY`&-m(U`N{2YI^
zQY6x3+UE}=W1S`dCjr0_GDMneou@q_O}>Sv%S4)d4`+Z6#CQqhY&hW32?8*n0Ow}2
zb(^5h@psZYpmjOW%J}b7^a7f$nzPdjKBAPGmtHUd9@C*_q?z>))o=qe(sBmw<Lui#
zEdXIr^o*&DT?iYY4nB1|f!b&)LovQUL=pbPGnJ}~*<cv=498+{$#9&F`x%Sjz%o}8
znCw;H-i6dc*1i{>a^eR5(yQZ}sKR6DU7`@C>Ds*PNlttA%<QIVy5_>;f-R3Zo#z0A
zem<4iv(I#%<qX0@rb#$FMS2cUP%;ISm3b<oz;P<d8#hU_XFHulr2#fR%Vbdo5~ob2
z2PaW-f+_$FtZBhXM3H?mm=z*3X9+kdKCZL`w)%XqokjJsCBP?Hiz^0`$pzZE(*>Ji
z=h@Rcco9?5QA7!8fg*USi6ix92Pef5b5mttJ(3DNdpb!w!^B9BlcG1L0u%nM#EfcZ
zP3~+<fwSY)naxF=GYb-yIjJgGO0zpeYY2eb<mIe(TDhDY&mf_!g)lakR75;WrW4mw
z%o5W@iFv%l+-hFtJi&=jmGVv%ASzLT@!rkauEch}RRpU5q=W_5%tZMp`!s8Iyx`MK
zFjOtTuzM2-OB79#Ut;R%?CG=6PYVE$W-I`O2>?i2WCfsmxujtiPmxBs5Mw$kA$up?
z;mM{zTQ<q<6y~%jookc~YB9mlM(HZAtN^A`GTqHP!xBOiEr{n0BxG~q{h_rQZZr+)
z;cQU|MVA(6o=Lsgx;8P>o7*wzrgDyOPCD~UvO%<SXCs?3o1JS=9WN0k;rf)2gkK&P
z#D*?k5f_D$m6lu}`<+@7pjC0T64EkWHX;FQ*(7Ul_;L+?<;L>q^yInxn#(n2%iU29
zPAh~mgk-V~)x>8OW=b_Y0_kh5x=6sSBT1-;`ZF1+F&EL57O_x7DBhA((FRKrvNpz(
zn1_+zA#fz65!Y2haFTkp6E&EaNzi5mrNkN}m<~F|)(SX5&l6D$9o%=JGV9_6ojd((
z7{&!SD`pZIqroGnW(!3~##T|^UYLn6;5Gpu7bO7g1xz|I_)X&0VI?7_3+HcR`528f
z?c?k$+%=jA;-~#D9<~(eZSNlJ2&)5_*#9Ey@K9?{e|I=4PK{Cp;S@|`QHeYfbA9h%
zVOu07QE+fmA<nZZ9EE~SOgIOM3!1<TR-SxRSV32RKj}yoUQ$tC&<+T+C?TaPU4vu2
zTGxjIH~|aibFNo}IC%`Ig#}#<4<t>oNO!a#B3RPNPkqHA1M!Y<C*o)%I@K#`f+Ky1
z!dg%n6{VBIfFQU8dpp8I-8c$P0nAg&k^w4R(+ZK=;~aRMTYF<BO9+F~M6s|Nn_9)N
z?}gFW9Jr;~09I=e_NSPJk@riMEVT?_e}QS!zU;G|GR5)6=`YxDGLZ&CwFDseO;G&Q
z<G9H$&Gsc*I_26yq@Jwpj{%ToKXwVg_D#pIG-tC<cDzR6UASrI(d|9DT}HPL=~hNJ
zTizrJcF^q#x@|F&1hd(*o~PihjOL8tqqcdI`+SaD{^YxsXSwd?S-;$R8CKMQg!yZl
z?RX1_0*=ldTem`#(&jaeQ&(MKJJYp(>-mlqY3cS=j^%)#WjjZ+HBBZ0+cF+FR&yEc
zk6dbNg4+X=r?86yp<_SYSJ3Ti83hz%+?EV4wtYe0dY)s2?Ubpu(_GbQ8TN~)l&P2B
z_jj4@r1Z?o@3Z$fI@2=kwGSdW-m%^O8}_9Ao=Y8P!<}eY+G#W@)>KpnYnK_db&W>I
z-%wY*%3sMSqaj#RU+p&<8tKd#qo$YxSJoRA6h&*&wMAg`;no3#+f-X#*Hl{xj+P_;
zQXbz`NoQ_FBVP&|I7JQtnZK3DJeTm_J<G4ZYg07Rj&JG1z3XFL|BIf8*Re9#;4828
zSJc%uG=_Y^+QtT>$``DbDr~ByOrx>RSRD#B`i&~&aI%2Ep&{t2HG<GhRj?L$I9sl>
zC&nzN0#fbGQwSIF?mFaI)Ef?WH1<^ugu^xAp1y%i&>A%s@sK4WUgh^S1j~cf!NxT_
z*I(Pv6!KRG6-MJqt7?~+&SObyM@Q{oPg{7vkHhWgSi05?VSYp(l~G$Kidh|O43JsW
zR{BDf!W=>@>EK6ZZ0L^-6pV7DsOE-PTepEser{o^N-l~-D<iGzQL{)Cm4q~0mW>0E
zSlFP$0ePJo{EbcZ1~)II@=jAL?2?8}Jw0Laa6)kmM>mH0HrDoG{ima^wl5afARB-p
zH-tsKnbF<Xei1LAse!sh$iJ+q+807IHwNo!c}7K`uG-(QVvSK(Wvumw>I~VMJQGS<
zR$W)_t2S!T#=d2Kqpqp3zNyh{dfrkEO%)Z$u4<}|tJ$oXpIglsCY9JL>x+ag61r-=
zN62p)U&!yPG|K(86@eOGXhrI#Z*B00Rx}jhY|i#BI;)WyyfV1TC||?t2=L&5(T;OA
zdvK1UVLACx4uDp!V;!kSJWVkSiw-D5w5PAB8D{GXt%0MF9m(Q3WG^5yZS3PV(!m9#
zZq!^IY@mM7=npmcD^zC?>S@Hwq2O9S+f>(&GYetYJ*@*5!K|p<xSkDPHEC*%;j3s2
zuJW_?zWz;;eN~-7ZVX*j_H2bJ+DbUgIvn!?KVbMOmp3&u0<paxr;_5#Oe$uzFH}qR
z6la68NPDA=$dAfG;yqYd7|*DwTSbHkmQEbeolU<>>9`Vnm6aCdnLZ@m&pkyrzOp0O
z8^xI;)YC^hnnma@6@C0jMbs!x>F=Ne7NJ3I&uusblJ()_lGa$?z)~Ttui2<w-4KFo
zzKQ^paLH&NK+-~V++`CF_C_P?d&3=W*d97NcOA(c*lcrNjHT6R;F2vCfCV)8Yl3Jg
zQ>Q^641;%G@>wBj7kNCWRl62`akERIxiaOFpf;{_m|G%qt1`_&E9FVGP_b}2zJY;O
zVQDzxEm+OR4DMtxFgG<Db+y&PjW;#;ll+lURTna<>$xauOi`w3dM?!1(ud-0DNguJ
z+5DcOxOL(@-b53?3BHjIoa>8QhZrWvXJXvu?OKQV0@<r&7!;F+Ch8q#8wiVyn|z?I
zkvC6*F~()}lq71)Utb?#)VW0Gf!D1J`j*wz!ta_o6!Q4de=Os(Rj&FF+yzcdR!t~H
zCM$vo)i%=bNwz)^Ztb8ED?v8ti>RYuV4<@|6SYJgg({ge_A;ZtFG}a+N*^2I@)=d3
zx*BQ4=oj#?bg*rVj)WDb?+%F5cZnff3+83D%P<I6`XPHwuvS>0MdRETnEnjoRHeTm
z81iGZ=Ce62SXHoDI2BA;gcU(5I=z<)L6cFxl5;)jFLAtd{H$4vZ6}TWV;uTSY-@at
zArxDM{#vKTRBMYg1cFso>*+NHx}wZ{mSJMt;SzEuLp}tvtGZe-trFG7fJ;L&cbE+{
z@CO_E24bR<St=kKu!Lp}au!9ZrGWx<udT0Z2#R5#n<lh4n3#^cjnBybN9RKSvqQBz
z2K&1)S&fBRgk)x$3~G>e{y55KX{k&~P2rh2drK75oZfjBCMbjVA!-%N<dO4_Pg@&&
ztNgHqN?)T-jQTipIkw4a2B|0WbjB|4Ml@(z(bYV*G-(nh8vy;--wd0EpP&bk5HmWv
zTh~X6IWx2fxEZxg)zuC4z6wm4Ro~=OCu`%VVyte=;<I915#cv5Ql{|ygS~VRb1}Ol
z#evRTL1M15x-PT=p3X9kS*$DWx=LssAtzU6`(kwCoJ98*b{ijf)QQS*CZ1yJEER)~
zJT6QgwH@!4Ddt}0)Q;NP9}1zf2Wu_!p88N-Wm82Xmq*MpEJG^MDBle4QCD9NFJeB{
z)nXFMjK1y;qp!1*4geQpx14hu{E+Af4kE{6T|f0iImsTXmy@rQ-p}d~$TTs6lRr|v
z2oqF>L+#hQ`HjB0QZsHNI@o3e;jG|W$@-dVSJc+6uI0}hF#|_~3!S6UM@^>8D#}KH
zBjiWsTROb7sU>C`!e+?=#d8UrV=j-xj-Q-1`Xn}fYP9Hct1vOCG<>0DO*I&68o2r`
zJ<-5JT4#4(YmAk{$Ay|;?(tT(7=|ahv9(|5HHx$4*${UbBv{*Er+8el_KXzv5kEPb
z1Vh^gX#9p-t*qlhRrqSjAmZ~01KkgkY@S(H-iXIuB1qXC%qf)AUMU~5{g%Fnr&aZo
z%1tvqLB?Z7YX=UXr=VB_3q?>Of>LHgQAsWb&3PfRm<ZAk3Twjzmgm8E)TBt-Q?P+1
zMclW1p0BKi(-*2mTX#kVXb9-(>ku>gxZ`LLV`aU+B3Km!V^X89y4upk@K7M0DO8pi
zoD>wYVh+LSFy-_6Le*<9V%PiOJ!?h3lmjOpAFcf(P*+ojPPvB93alCMq-sLgU<$^E
z&(wBQ+D^lqbi<aWY$E56m5cbIhFbKY$wviEr>RaDH;o!R_2Bu=;3FrVpD>-Mis0`~
zP`>Es3EeLK{D<7ercf>Kv-CjCDdK}r9TX8?IuQbKec%yV3TF5klBO&}^r%NALlLlC
z8rk`CiR8^4gHhhJjHgYdlOwjOJuU=)hBhDKliY=+c&mX?@qDOgXewV0T{YG<uBlJZ
zrIc*6?ugXt49w2Qr+P4d{P~1fq&@Cn`MHq>R-N(JKd{)0KW~9UBMq@UkZP0zck3!q
z+@YG!V&qxP8dz-EY73;+N=z(a+?d;^oQ@haG~nUj|H3PP!i2?lp)$1)fJtComFQ)n
z770rvHDZY*aWSQ4p}?d$XzIGqB#@>Ccz6yEG4%&aIF)iRazU_?{=SyG;`GzSC(-Ww
zp4eb-c<K6ZZy0M*NVX2NcP$lvyUU%w-ko2!5Wiw~J|4_z2{pfSuveV>2@u-4{i6K-
zfxZ~d0__`MHF(Cxaw9oi@gR`k!xBAh@?%ND!yBGhJf8BtK6HKghe*`J{QL!MJw4pR
zGgXU)Q<dZH2zQ4uK_zwjR|hQ%8U!&rhMFoQnwk7FA=UvYK9rTAFg{ns^rmN1MSZGT
z(Vt+UMLb%ypdk*58SIy%T(k>QW;Gu0XS-_uDnHzCz*oBre#$aXQoVd_{CUP4p**EY
zOAel5bEroSjR1q)VnDI>%*46SCFW$C8NJ~lj2C47i%QS|dilaCRvU55YwxPo?m@o3
z9WOrBF{|;RndUQM0?aip-HdvG6OVO9?V6xa3r2pLkXZFRs(NK=v3}G8KK;=i{E43k
zOI~b-@U7kQL7!H;YA`dyv`jqNSJ6s87cRcW=PR${D=yrRB=`z3prCEUTB>zWGM_9z
z|4#z(=60K2DiwE3p{QhX+w=$npGSi>)(L4?wJJyUnG$MYn%0e;_IZk6GI9l(rdjgA
zyBf=IXc>RCpC)ms2ut~(3mvyJOHac@IYxh!$(%RG=XO#SSYP5Yzrj5_9!luN-4JW@
zQU6T(>pT!#Jv*d)FiUt8z_dv9PWf6QKHTxiHilGbY+|mO^w?hQTb8QAB^|VQAHg!a
zWsThse_DuILremczm)SL^HGk>mayWY_^_z61}k4H@tlIUDK!aY#=XZ9@(k+T1#zG`
z>l%dI@l)%lnOV|IQH^K#lRkJy)Q5SVS{fxU!TU3pDCx1m)Lr~Fi@5|vMO!BV;)MpW
zv;MUzQBOX<N}A5FUaX)pT9}afSEHJmVuk#5p-O*9bT{in)EA<~3rssIeCj2X>5^ra
z!z`I(W1|0z8t#+UtH6yvo$`^uoDNf}KH)E6u~dIy%kq3g%V%O8L%2R_p@!AI`h-Vc
z0<NsD<C7&(4SLCwqyv9*h2LL^DHBFPHT<C~;IVLEJx8tbulCdWGoSQfZce%+n@r+U
zQ5Om|)st1=FI9!b8xOPcl_CD5OZDXMj7YN4&q483LW{#lG)Xaf!l&RVuF6*}pPJOv
zoHtc{CEG*~#A=y~%L!{sP>p(*o;lr%?HaUUqftfcBq)u&r#0^4xt5zE{4CeYp&DDM
zqKZ&sYb*pjI!E$cB10Q1+NI*PCd{)-#T6?XD;_0*deMKLq|;cFimF}yBzn^*LbVm(
zqBx<7s%`nH3DutZ`iY{>Bz=pFUuMFJcd7UdCT!s@75}ygE7_&ucbKr!T`GQ$30t&F
z#a}gHi+8!?cHnbmsIdkWEjK~+DjJa>JBwbrOC`3Lcn!(zq`03UQK+UWuE=f7W3o~k
zW>dA(;VD8}$7D#nRD7n~P_5zb8OdlU4ti2X--(0Va<guw3_+MIyJ{s>?s#^q+FPEC
zYHqdPFWK3#S9X-WsJ-uYO9fPXf(i5NQt>G!taz7-&oN;Ocd7UnOjyY-6)!SjrMpyo
zi3wY@OU27gSkW#OUtz*LyHvc<gca{n@pUF_;Vu>LG+`yXRJ_lGmF`mUArrP}l%ABr
zCrM?dG1|(mQmW-N)~ceB1Voj(-E7)c6;;h6I3~B^^U_HFAZ63oDiu|Bna+0L<CIYn
zE6OQyw>r@9Rmv`vJXwO_6jjv1kT8=)xkFOK1s>0`|0x#rh_N{RC@$Kb<C}M>q|&v<
zE>KZbCMrRg$qb+Qmqbg(N+p`YdBxOsgNj;Z_}0avWOyls3?2UzE0qk2ld8PN3>CG?
z@a$+AlATHw`!efia~<wHhx^opc{|wL@!r>F;RB5K7dX@(iI~&Rc+Lbb{!R<!sq_|^
zo;7P!EtHWfPgTXDqskc=s~ne}Sk9q|BAucf^}&g%_IPaSo#ExJKT0Mw{GKC6KCcYb
zaGPv1YM@gePN*_eFG$8$=C3|S+ArJtmoj`>hRTAH=@p&&Hc8>B)=tJ(cw?2L=#&Rf
zrdN9p56Vf5+H`nWrqv`zO+(a_L(MPLOhQc?)HcFGIZ&&iXe@oPg6O6mJg^~*4T!Ky
z#qb0g9_VKUy;z<pz+ZvPk724QQrL)EKreDT@*^FrfKpwp(Jofdv8fmQM2LAY&%s}O
ziDJnvAz>hIAl%(b1Txm&9b*OjMMFW1))xx!xDr8LAOF*;1>r8EbD*^+Y;<*iR;9qS
zz1;{8wTJs-27m4pJd9Sp^MLbE@eA8Qi1J!{BJH5-LlL+Y@Nz{N`0K5#puG?KtFS^?
z&=GDMTo2;b-t~AHFO&4wXjQzmt!*H@L8WMMOU0o<6~zt$6_uQlP2k^6&jbPOY{nj4
zewEq0GUS1I?xjcxTotGA6#VFmHAUZO^CE?f;S%8HsrU_Bb`vfSQdaty&CBy+l0J{8
z;<se_N@U^B()6kFciOxhd8Y)t#Z&mgMuy2#r&_?Vcifa;o!5CaHXzfrQx-^{?vb7p
zJ+_(iYZdN<s>)aAf4U{2I`30pkJjG>$e^<}6}>uN^AmiELsy#!O!*a_3NJ!Fo!zO@
z>io_Yd~HP67JkR4MF>ewCBFo-FCjuzRP=|ky*%rXq;KJ=(fU*MzXJJIdUgJ&7oVQe
zrOqoQ{o|WzvwTJWRlrEEBp+P|U0xaTfXxtrd8e|h7k#~DrDwWVWa}buQ>Nu|$+!w{
zK-x;LTpoQ?Mi+hLXuTAjg58XWm0q1M`x-tfpbKAzC0xn!k06)mmHycbFH*Q<#ru=w
zck|5n?M_6k^!+ovJhPuVHm(j4nD^xJe})_@{Xt28P}0AYRDQDk>`9`hKT|~4z4&I1
zuH^PhV#l5V4w;(lZsvKsPv#Rf&RY<Hc~36?kH{fYQ}pV*-h+}p89zDwCTK|}MNfaZ
zjjlugBK^C-Reg)-)OodsmHe_{m3^!7RlPm}jkSFB_kcetByREkB2w|R;`1potn{p?
zMW7amU_=DwUD2s91H@MP5lKHH>6M&{fud9C2_Pnz(%&9QzgyBDRERQkC#5kl<mF3r
zGgr}{ZbC$aWTi`mGbFu$#_ummhTG#H9#eD*ykHFai)diN<(GCWQt@(Cx^xWs-L(Q^
zzZ!TXL2~&OW6*DG6a-mG9X6T1PSPjq?}}zYf7clDD?V$-D8F!npnoUH|0z0I9Tpyg
zem0#iiwh?&CR|BLoShlB9V_AwFBPe15<yau+kxpjEeuAk5s8D+0nbBn34Y3cb0sci
zE8CSI9t_dIfa^D^<BFtdcQX08fgRXoHt#4LJG^~kO1PcHQo_^BbH=UA?3fj(`AQ-@
zgQ<B)B0Q6+`9&h!!PLAV5k8Ko`9LCkJX7O*BHU??--&RH{AygbWY}4jc?O6DjP}T8
zY8*|3!_&|3T0uN+$0(u3J!_^N)}_X?MEFFEPhKmC$L(yAIsRBP?Ko{+jVp=pDHtbO
ztRNn@v#I7dV9m6%X-xU|L^!Nb`E5&volVC{ODz_Nh}to#DnDz@v@<zqTY-q%N6q3D
zpCsoX>FjJy5(kBU(im{}81R$FfX^KTr+%Hr{(3w)gXRV{i=EI|PZx2}pa=EAzv|az
zfT!Y9!SP48S3Tgw-<9h{EPjO%CAc>UJ|y8=lHgwjoaDLM(}GxhocK16kG7wEfD`|b
zBt8c?{%AWKt6nt!zz4K!calAS%HgBy3vf#Pj6(xxY_uJo#NpG~?j$=rUBd57g7d%k
ziGGyaUxL)YDf$<k4XmZ;FEp;WQ`kSvhvn3q7@F4=W5D-tcosV}-OF*+SaCqYlb14n
z2l&aDQI5<I_-;;xLyZAfaS7tVcrZ)_p96R*Ig7`DS9ADizZV(<|BHa9Dwht>Ao;yg
zewP&X9>8^)DNDGTRy+*&T-c#nI#T`UPh;@^7;smLc5z@3B|e9wJu5!>fS-(hq5M^|
zl*f?xYRQS74e6qj^Hb^V+ni61`FDJkhkAtLXR&_SzSM8&dU^~#pGbJ}-`%D2+(~bH
zq<-cLHf$;2spO&4s0m;FUEW-Y|8>A!sBdzA*~Rf^Ft?|LV=6s7HwJ$P=8dV!#n=6u
z|4zwA7Ry!wPVzAM{HElm!<DFA)Y6|J2}UG5x!v~yo~qn;#(>kq57|R<ed!F#RD3)N
zKGCa0=V1tNl2a{7Dtj1I_zBoJ<{NMwdfp=SpzQ57j-NH^ys$eseDwT;j($vKw~1e@
zr`kGg*+f#kSZ?CP*Kh+dY<|NZxNgV$^5qry?1(nL7_1{a5MGZD&%^kjv&U%1MnWtD
zrT}64N}sX5yRQv9h&r%~Eo!t54q;0Geas&VcN8pIvY2-Fra;2RjNXV?p2V)E*uW<2
zjG!gXj=`QDe2X9#Jb4QH03xvjR0E&Z_-g$445t#p3dv<$ey3SiRYhC)8~JBfz-X+g
zFtIzLeMT33%8$KTiM+A7$_l_1J$$ZVRIaJ@)dVXTEtJw0O~AyKH?MbDb+Ej`C@v^2
zz_#W1HXU=blc)4w`-}#g6(VizMtgLS>jhh&MbT>;8$N&U4>ph+>wB;%loOCLmTxdZ
zvh6CcwK0l@6Z>?ew9U=NLY#<V{oFHN51zvppQS{CEp5J!Bk{@x8*8w2Ntp!HOE%I-
zyPr@TzV<P9EREhS51FyckAE#?(8f!v=2Glruk>J}1wQDi!6#e7CKF3DEbU@xgOppP
z1h2J?t7<((hS5GW)Y=x=;PEU*oOH#%UWNs9_W8Zuisoi4{2QJESa7^)Q?sS-+$~6E
z`>XOSt(a0D@#g1m0l>=KLio==NkTnZjsC%bu(2V6KR4gnog_PI`}wUAubnCQ=kL->
zY~fUU9LX;6N+E8?i5f44_M?%1S?Q)&|Ljw%RBiC@o-H6ZSz5I=Mhz{kl+rBX8$B&@
zrLu7PvdV&DR=2`~U5R`@ujL2<KmR-^sXK=GTO6z1m`qZBrV6hyG-Tj=qiA0*KIMsR
vGBy+?na$@J4L{f+ahoCps;Gc#DWXpV@PS*Rfc*0v?pv)DO{q2hHO>D4xFENg

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/exampleLP b/3rdparty/qpOASES/qpOASES-3.0/bin/exampleLP
new file mode 100755
index 0000000000000000000000000000000000000000..87ee8a1ed82c272ebc3adca06654f738e83a0d3d
GIT binary patch
literal 31905
zcmeHwdwf*Ywf{adNth%gISH?zprZr|YF-c^D9B7Q6J}tNi8B)(iamtPgp4FJaUKw?
zVhYrXih}i4+S-b}y)CxhTie=dZLeUlmEPOiVp|`*ww7zF1*?@>we+F+eb;`>oS95&
z`}y?q`^y8Feb(A*t-bczYp?w{uN&RI4vVHSby(QtjG(hs4vrGPj1Ba2P~uuyDRZ!S
zYz`{{rWk(?o<pfPQ|VbLTBSBgXGL0szgCMtv|1eeM5VI?nyFN2NGX+<Jsk2>q`RgF
zP^FC7IfD2|*W<G}h;$P_QRzvk2P4J!VduYuCcaaW@08?IsV@0cs_I8|qrZ^M4_O?5
z2zF4yT>Lki5-lCwFQvkCezKZ_NFDq{rOO4HsZ{Y@0Y1|6?>%%$7?9;nWS4CF2nN*-
zjf2t6OIjKS!}WvFSbC&>q;*OCl9q;KykQ})H_0d3Rb4%tQQ5H;5a~P>e`IRP|Kg<k
z7y0I0<5>F8pZ%*}->_(4)f3YwkD8MBCgE?tK5AnZOs%jL6;<3^#GI5j1Aph?@0!x~
zXCD34o!7UypMG`0?AvBdzVz<$g`1y$?SVDFI{Lr=^Q+H1ckTLF*WCEY>%Y10_mz{U
zzWA9}e)4+d4)4})fOW#-YA9b*Aos_>6~Z5zfS&ZU7m#z|1o(}pa3T4_6X2IjASW;Z
z-Zg=qe*t}=a%W6He`o^wFHJx{I05~~CZK<L0(|=fcGwJu(b-IP(7p*JWsiRWz0S^I
z$Mf*JIDRHOoQIDBUk&;PC0_OCu$9_zy!-*+Ygk1lUaFFVz*n;>HZ1$E>Zfr@e@1t3
z(uic(EA`CR=LX<O{&0n$zZozcJGh*3Hh;2%!|JD9%HJf*mE^3G<11NM;uXD}%V*3X
z<tscx|GswE?+m(w)TtLWG%>^2G8B&)$y9G5Wf;s@z0C+jwnUSuNTPkPH<^qinc)p}
z8sSJnqC=hSgYj4-)Vp~wBJv97T-mi$meqPicOnkzp_XVYnsRrq2?rM%dH(~^?l3b7
z@U#xZQ(U`zzI=|Q{^3+K9!oYajl`1aM5G<(RF7ES4)->*Xhql?bO+W1n^5U1yVfXO
zmnmJ_nwM^gq`DK)q29q@d@xP2-4p2OYR)aeBxF&N3yGzCk!_JdcURD8Zh}w+7t$+V
zn$ovjmQq01Wl+_>`I@!8gSm=iEDzpjlnQV5M3Tv9Z!EN9IN~OMNT$Ml7hg>7u!Njp
zWW*SbB$Dx1?_e~w!`RkDG+X-mj3kjXGovTAJsJzcNs{TI2xayu-eIa%ztK0al|g7c
zoq{~dm(DfR8;vm|67Eg)B4cwhDeyqW!mKf!Owj1l*qega)o<=iM#Sl*jRWzaNMkfP
z5Z#ha#2fp1Q(L1kV|%i(Zy*|uC!&p;)6v0j<3M6Kxg*)QC=nipC+}#KuBXy^_&Ui?
zUfLKRPBkXt$wt&7x+PA?_bww$jYp)aN`1++(GPD%2k@gFVW^Z&M&Q8))lbz-RG(<c
z@!6{Oc7ys{`}+0HHm}j#u!ybldE45Jg$)ZE7H7^EHMC?pES*@g)0Z54F~(y0XO*dd
z#IJ}<biNp|fW~b*{<`PwD5sg$jffj`P=2*3|0iGGifLF8`)DTL!U8g%=+oy!OKF6^
z6Zs@FHIrZq`;4SNd;TUO<8vz&8wpl%m*#lW2_cVK`zC%ya{&wunZs_vqxj4*--MS%
z^1LP!-dt|032&Z9IZSveTOA$~zDyyJcAM}pVCGnF!cWd%8QWySSD5evCVZs{KWxHR
zneZbfyqeol_NWOjr<9!QW)t4dG2&;x32&Yccy_;P^W0ke-6?kF`N+{!@u?u_J-eT<
z9WycGbZqfgK{7V?A^cXI@4y+s14MNC<QR^*UnH2C@bn1|f0kfsy3<EF{7HhX1V6~(
zI|-(yI(>-4w-8M3ae6<8cM(iYc6yY<*AYxjb$Xb?34&=DKfQ^=*ANU}WT(41yqREX
z!qW~8Ur8`E-RUL{2MDGnJ8kFiYJ#b$PU{?QBbb`#G~@841XI(TKJ)I_7;NSR!n3!S
z<Q&@{Z2tbERQboK#-0zqdIB|hqe`DW6*cti`*z(?WO|<5Q+E{EbRci89eGIW&%!E5
zSBSKFW8Lr@Rrnpj?>%)l3*y<o<wVsF)QxgP^|$Nxi=6uZlkE44wBnnB@}9awg7R~9
zhs8;_&Vgcz_p1C8LcrXc1>@{PBICBYgMxmrt_do9yY4{&?x}l3oIFr>T%6of_nbHh
z*L8#P`npZ09wOVIzGskeYP$qZz$l)54+-Q#6Y|t|-x(Xb^`SaJAgpxiKHxn2Ty+l5
zzKwO4`1ak#E8^R?hu7G%&sR6>*>_jnC`$0`b=BF3wkd04R~@ZIkhQT+Z+_dex2Wz(
zSKTG#2&euC$v*IJ@`0t>1Ic6p5_XiQ5Amv+MUj6&)U2$Gk8$lDsyoR!q{=allz;}H
zziR%8$3`$GgTZgBI|bS!<Q;(aaLz|yYUuM<ld-Nb%Xo)!KKulEQS;HOj(PSTQ(B#R
zk+Syw@|2<{OC$f(Md1JK&zfKJ?tSWmp1m)6cK`ZJcgXwXQJOJ&o_w4prk*FCr1|El
zJ0RlC{)Vda_kfr2{`|<7?VDJSr~WV!&+fC8p1m(^w_JUUTAT-lH?KZISz{-w&L1V*
zBLpe>$In_p@7enu&*QJG^k^qMPn}JJ`B5VDeB{TeH?Q{W{hr`|QSy(jsMhTmPO5vr
zd1@zm*wx=Fez(fbv_Bl9_~O~UqP7AAsq-=_Ozk;x603C3kDa*s^oPJwbv`?t6sdEa
zbMKgQt#|L+&K~dHr=20+zPU#y+o9sV`r}B0z6CEeKks?`9V<n=Q^kLQ5kK-;>O5rw
z-`-bzd*60J#@N)Kdv+hwJj;HYeuV<zRaZMVI<Izq&}kg&&)Vz{$51tjoibEv7z61j
z8^IjZzQTT$eQx9u{MoO*%x;hKc^R;CBW316g`HW+nb=vGo|Gv&D)3VsB4si?b40)=
zk7(ywFQdHz3XsEp9vizE=_o2ieRV%la=BXcwDm|Sp4GzrMv-nJH^f2fbUL)_0!$mx
zwR0y=vhCL<>4cw!zenIr7ZHIx3ez)IhkT+x`KPflon%+)9hJ4KtID_AM%l`9F1u(^
z-GwBA_+9ur4qeUT8}ZViUDxg7noP3NlEo_YVeAs*Q5lD*jxcf2#wiZwtkic~f|V!B
ztsR!KO1-_(=2T-cmGJ=n&OrYNQr1?f-(hiA*4}P)SK9X#xhv=2R_v*4x~ard*}8jD
zXQgAXvejAH<gA?ER%vgmtOb2rrH$+RCfLb_qGd^#IL5}b3qNW>*SyP8{6Z1C-?HEG
zb`g8Lh=3m!0snjYdH;Cd0`FVkeG9yAf%h%&z6IX5!21?>-va-*1&(6bM#m;his?8c
zVGiI&PUpv^w5Lah%BMX*GfaDWbf~%bCnY>tpflGWXhVb!b$wx`05jKF&y}z)({p6{
zoV=c@=J$Ldjl8$s9g7pmL5qWu%r)QI5&>uS=&S-(%cU<`iJCDr&#x6An<i70NHWH-
zJf%Zci;YS{sCA6m^P_8xbQ~^o@KoVPB&_sq;<*gh$Hk$_$MS&J$0q5q#Nx2h<BWvA
zQ|geM|9=v79Xr3YetBJKn@n$#>BnXId6|Agrr(q4GctWyrf<u%q(andx=iQEbcswm
zWV%+S`TAf({8Ib;p3Ui4Ds69RXl-bzUzp}l^Umg$hNhN=MGFL;Um6$9DH=s}i7F{G
z|593hQ5V2Bi#XDz&8fuszruOD;kb@GO3TA&<>H^V0ByBR8iC7OHQN`#ZM96g5%&vN
zizeN`ktGjsWbxPFg+)chxb^{tVjK3iEziR7OLo%=)bhmnINd|1kmdkmv8}{*fIzL}
zYY<?W3@4n_^sm57`Z2Pfv%NJ3^|UR3582-SAkfVAIrKx@Uv8yS+X9qk`zxL6wr%Ji
zws+{f*7hYB!uBqm+ik6=nr)2E=a)W0!k9)tQ|Yr%#-?$gwe$<<6gCS79Ho`8gsq4J
zp3+~SAJ~dH&|P{bI;E|I1M5qDq?Jv(3PLxPp2B5%Tbb4iV4(D2G>dJrMhXv?zC}4Y
z2U4YXp;opUZ4fylrOl+lbgd1*XsKrkfEijhfL*2c;@4KIZGp7irE`gAmbM+h&7}tk
zn5%6E!T!?k)BtGE_5wIq`h5Z#IdG`-1OZJPI9&QH0nHqEu=E827HSK?^GNADGG+_c
z?P%!_Nu4Fy4JhXcR`vp^@X1Mcf}+-Th$?AYT8^Ll=#<%BLvz_aOVzd6E+wmco@%SJ
z65B<<mDs2a%c*He?a8}vLRe}|j@_rQQ-J*#fC{3mAQ_dF5M6px3*u81Ll7=1z4LJp
z=%0pxm|hJJ(OA_xD6**ZQ*Tg4B~;w0KLDHGsGp!Np#LZQLerl|n_BdhptQ28pM$Tc
z^ay#9z5?}N`n`~?=}U2H(O*Gtu<AY(S)|W`xMKa6aQqThm4^2el|FkVu{{b&H|i@W
zj_YCkYWm$M&7yDO%v*sjVpX%DWs${E2d~jrz@RtkL*QllDbibi3T0aK`+>0PF6dsQ
z)7=Nf`by-M=+Ki*(&qth)1QOrQoRP4GW}a9pj_V%X_NJJX#WcROXygYx&@)PN?%Vl
z(CeXlwH|`6)#xu(<IWQO5Na@0|5xNr(`UdS)AgS~&J2A&xN7x}LX(;LuOagsy$9##
z>Y5$<SNbnevDx}jXmy@G1NifG+Mu=TAA<Y~^d_8NsDA<>V~)NF%~YrV2<LP4e(dth
z(`V!N1Nt%OHede{jI}`TL%A2}+fciU_3Pkxm+1eA^Lo7vdN$}@Sh`XFAuvt)1<<xx
zzZDuT)DJ?3MfxO2ZqfgQx-8ajfXpTO5!h|1z8o@J^*ivpOurYUT&kaewwLKop!DVX
zwP@QF`d7hwx&9&4ZlxZC<fCW+6!tjNk0DHahv@-`J;wA)VC5&6{u)~AyG(x;rF@U+
zbKpr&GJPhH$C*AE2LC?O>!9&}F}(xG|7QAp==lS<5TO6z7ysA~nI418A2Izc*yse)
zKMR9B1?PmYA2a=f2$nx#`mdqgPnrHMRDGK1+mZJS(}z&qXPJH#6nu{9H=<%cWBM9U
z{x8#i1Ip*&^$_-R=z?N@!StV_gcq1TjE4Lr)4z-Qon-nG;QK$OKLF&5*yn@+e#P_;
zz_!0;dNE3PiRn9me3|KIP|e?<Ab9F2rnjPzPc!|eDE$?b0Aasn`e|7DRi?Kgc?~k5
z#P67XAL{oy(-(pL4AbvKjegJcuONAY>8sEXe_;At6!a$3k3xyJn7#ldyv_8_q2d3?
z^p}vFg=m!bC#KH=!=E7qCin}}_n}q(%JlP5>35hu3MJlU`o~bv7?cNcRMRg;JA7Ev
zzXyNbrRfGNzgyEw0qxQBuR!^aX!;Sv?VB{c2j=;xrk_RWH*5N1(0Z??JE7+-ntlhA
zxK-0F@R-{){X*1lpQb+tPrhB#KM#gGH2n{N_Ty6U?*Ps(Tlxl+sfnUOWe<7K3d~aT
zX~cxGuM=Ri)C{8nWe-!_(k(S{%6Wu<sg|0nkd%FsfLZ_~;86l*S!(E7UD>w?uv?~3
zF=eZ?PoYBh;csde6(X>!wXXp4ZH1ji95Sm*`vK6;X0k$@>ot-U(0&8-8!Brm=bAnT
z6))?lN4>8Esa6IRrs0nlH;r>oFGa=6HogQbg*&YtC#@M0A!Q7WSJpQIt8<cWWM4%j
zn;>&qKj|=iCS0y8`Z4@+k|eTs=8^1ymy``ZbRJ_QRr@%y@5_*gf@Y$*%C@}?LQe8c
zWIvHda+V6Zeg~B1BtJ*?i;9FzzlSO_x(){6xqn6u%~-jnZ2AMF!AFPTgIvj4<jkR5
zGQKeL^e5p{Ww#vxA1ApC*&TT#$G~59pc9tmBp*a}EQdsj`T?Yt-CYfLJ`BrgH-Y8=
z(Q&JZ`aVr0_eEh|5@op`*<V(o*z_-;1D5^UH=s1neG)l8R=J|_X1oQ-We+?60VL4!
zGG%AwvkDrJET2Z5O7}TYR>dDdQ2hofaH71`3Up09s!@I&jkHXwX$5up^`vc)R`YGp
zm)}5uO{@7bVruzL0w!xUbKvjgHxi(0HT#HXlz^#P&5h>)_%H#rTFtq{vy1earPZWR
zSov-O>{^W<0j+!wk=1E6pC{lZQf$6f^9w|T@{bbGq}5EJa&9J|6#|HCF98m%=4CW+
z`7H!^w3<($|CirNK(|)&Dharafc0991wo+vb^<nOHC9sR4gv<Wnj#XgpMYVl#)}wJ
zet>`xt>$^E_MJ3g7}aWSCE#O}bF)@+3rV|+s<2<HSxx2q3*{WtYQ7KZ@{d!_A+1KI
za_**_!&=QS0S5_q5bD6I%0EHCBU;ULcxw5l$OcEXnh%i??;!zCXf^W)_%s2>wVI;@
ze1?D@Xj7=+%2#S95E`Gs-_#MP#{{-bdl{JDE9|tLJnMckO^2qzKPKakGngVnx%X!n
zKK6iAt3!69A`#rvzlN|;?hmLwy$ac#iiAzSij)cd0uhdPg9viAW^?}ynv}0Qg@Kmm
z?nchuZ0>S+cKKD0zyv(^KID8MoBJ9DtnzRNTA$}0LC$xxxl5pOdGr$a6VLqxa(+d*
zw@{5|P@gGJUX0eET+1I7pQsY6n1r0#TD0Ur2$=pl>R>%WxB75d3xI3E-_-q(B{*jO
z86=a>YX5=yaGFl!ug}o%jx+O_^FZ^k_B5=`X|6^7^+Z#J8crJ|#m=#y*(d+Iwg>u=
z2WSUWKJ@t^!rsNGG>^p<x6e{?z6qLFiSFa1PvX6QD3;6eehSB_@M(wO+gv9N^4~*F
zmYbb>E39v;Tt-^GioaPO0sIB3h&F|?DpzQ0f!;_$A11WGI<!Gxwkzzk|D>$hZ=uC1
zT^h;KZo|*rD&ORam8(WTdJOd1my!LbA`v3ayBismfxo~`)D<*~ZsF3NA+qlykB3}g
zA#QF~LHXy;g8$W4P1Al4fPB$TKP5FKo>?UqYTB&nfJCJ(AWCfkelEw~)E_}4S8ggr
z=xHzFhY1oNvNw(=VHci-V^y82Jr8>mH&kR)s&)g2cN4=Y5^~`lBC~71g$wXzvFKbm
z?Q<YGuE?fkLb#|B)tNV|7+L1r1!oj&&k{?nwwTmkj-akrT~C#L3W07`D_~L2x|h(W
zt4cJAJ2uM&M5vt)ce@CGQ-4QweUsFjLK#(~+ND4{R2G{}_smu8)<~|_1N0`9B^q*W
z2lT4CX(OsgEyT&L2gxo)#^zaIo~m0q&<Ut8P+2+EXF+k8sQyf)&!bpXb-Nb>AMYv_
zy@*pC2gOeml`zG;Pr#5>cLfn<sY(!7%t`+MvOg(OHZO_JQ1yv9a33kKgj3B#9H_$|
zFIEb)!Mm&O{T9q7Nhfhq7sx_6q>{cG$f|$45WliLZJa&@#O=BCyirN0=%MqHWaEFQ
zxt8QC<(&TkuH%23^Bu&Ss;^(7!ly+q<D93!RSlup>P+Ls@xZ}-C{wyrejiL+b+k#v
zaEo5f1vNv+mH&_+uF-#Y!7*fACUZfw`1@)`5H~>9wg0AW)2KPwJg(r+*3kSFzK7<m
z;BtQqnZ;-#QPY1!E}M4?x!`Y}g)c~vm0aXph-)H|ycdveY@UtW=1eQjDX5_IDo)xB
zvbCyoHjf6Rs<*~4Kam1;PPGjbA68V;Xy=1XKLP_){q=PS<KFrSWZ$bu*ayBw#f`0}
zqP_;~Hz|j-Ve?-Cca=rE9qq3DR|d^0D8g`yEO2)RnZD41u4kJS*n6;8dvN(hbWe!B
zjN^WG!~ec@;*G~Hy=X5?sjuKT$8BF9nf>Kyu8R$#Yx^nea_AB_ENfXzyD$|tj6AHw
zb`ZbTqN3MPw$)ZNf~3So&QxNf$z{EbR!a5!yo8^Z*l3MZVj~ll*r=ou8;!d4HkxqO
z^YapZUSgvnPy&7%NF>FmMj-wjX_M_ku;@Kp6vgs-8;wKtHaaii=Os3(eu<4{Y$dk;
z!*6rFjpAL2{H127w>`r#^t+HknY9wlbpiesy$u)OCpW?W4^sP7khNQmQzK2&tV=-@
zRD}FQ5iZ&U;`PX46%>0bXpmS)qn>pukUJF*KT$lag2u_kK`aNf&w*mewXlyCplUC9
zgd!$CWlJYP$D)2hTOTK~YQ(~#esZ5i);bHC=I{Dy_gElv*=`Vz%L-h25a_Y7%l08b
z%P*{ksjWKb=i-lBfKHas!xh#e7vYpu(A=kjrjN@nMrqo7QYC+9S-U3%Z&`sHX5i#<
zE%0e5xYEhAddjdK*q>gxg=q}}IFw#FxS45<0(vmLG7YFnKu0MLe9e0}--#450WK8a
z$&{l4PNppu#3#~@Yf+B2M1UvLj>pJ|O9gl)?RWvLthEZ>GbzV30+$JJH0^i}@ljhr
z-ex^;a=XJt5oIO0CiLCrpa;`57dQNYAEX?Q!V9!cA$fG0lWe@V(m^$5OZYFcoeyJf
z+%}WUhNF$_`Y=w~KO+fg9J?7LF0IFU;LHw}7NU|^`<G-s&3W(3<}>#ZnL(3K&UA7+
zV@`6H-Bk`M1CJ((L-YR?jBO7g>ogkHeZS2A9n1($tr>-tTImK%adccSMcdJ+QZz`k
zg)Z$1A`WOxtV30Bv7#YmXG+QUL8seP#>wr(+yrLpfkSDqQ<J;2_0|IqBH3U)a5U|*
zY=>b_q>$QRZ%TDbk|C)@S?yO#u{6JZI!7#+AOnI=3c+WD;4>*Mc$5nkf@~xxAqCN-
z{H;7eqtO3GnNJh)N?{#pe%A2~nK4HpQQMAV64|PdMEQb5cAAi@-j+yiOt3Qd%MvpU
z12eGD5o-}zc9REA-UUb#@LdYV`gIEYd(vUrQTIf#)R=TXoqC^#=Vk)A_jQ6)2=`hc
z(WZiTg%@I1&j9MsnmPC_wH~!^0*W!;S1FRLBZuY_dphC6P$PmO0VTEuFm&Dl_ql@B
z4SRuuiq+cO$|>d6%5y7wW>jlBybQi?t-Q4SvT`r<u-38i^2?DA;qnwSnBpp6A(9fJ
znlweTR+g6&lSax{WF)kLtW<)ygPg!JB_HO}H18DRn0x{APH9AD#XOv#<(5I95?57}
zBVC^l>9l`eZOOscm`hy<SyLLcl{1Byh2_g<hIvU-i%~!eT4WlXV35ud#VWm1vV=1V
zWng_lg)W~-O3%zt6z8&@Qz!!y^{l?Lxh|-L6=k-Tx6CAcXJ^>TedV)Fs+^b2pn`nm
zowX_(tDcP1Vj<N&3l(dv^_7#%3j`|_pe%4<MjBV!ERAY4M`V)*iVM?ba}$!>xfvnl
zvC4cA^URsK`cerWFlT4=nx8>KFM-C@g%(UhgflLmISax2A|Rx93dF@Z2&i_68G$K*
zE}gVtnsl~Cgbmh2zDw4}Jc6R^!Uk>C6nnXFl10V5VaQEd2oD8{Ro>zTI6%uxJNNV@
zL@3n5Rf|jo&?)R_RH|&NAg|VHCOK;FZsWLRv$f_agjrv{0SaGQ%$ldn!XoK1ege#L
zv!oo>73Esb43c~KrHsn0#p%kdjp$;LBU^t0o#w)bvyDozprg0%-KC-bH*;Tf-Leb!
zbk`ne1B+W_U^i5p65lFuipZ5MVCBr^u#vZgt(roOvzniv0&D1mO81#-BDzhdK*eB0
zVrHL01hlZ}_|>$(;4+_vgCZ0D{}Z2ti;VOR4G-ek&VQoJXsmBA9ge6c?!+T~3W+Dl
zjU_GevGOgkbmQh|3Z<*3{_ucd<2VxB5P*mDxX>J7j7jx<-3@)i!(^LIJk;2`CF1Fg
zh4JM2mWBbKVZzL_oDETdpeOa>3mXO)raw3jNBVJ&sdfraLnbrmXdI{TKs@iKPU&$;
z5C|#WSU55=h-XU`LOyw_z-+O3dME;VG&PmL^N@JHG#*a(WdwEhrV`P<Y)PE3H<rp2
zjq$^xX1dm=m{w%5_GuUqQg2wN+O30+j#%|w)*ZX7*F8GAd-Pj(jvl!4ChK*(cH1tD
z?m6&b>&UJ<ud!Wl;J}t^t}L=zcWcLOYyJz&qc_@?TRN+EZ?r6}-hH)gO;M3`rmY4U
z>n!s$Yt8ZRKW&ZKR$FS|i~)DZaJpP={+=$E(c$#^+%Ci0)!}vqy=`8fH?)BfMbO*X
z?Q<K!kTcX1G`gFZCm1ri{Xv(zgXi}Iy<Mw}fO}Ps&l&IroguHki{ZIXdSY~2DrMji
z&LIXC6yomicDY^V$7GwAjx%5ON#5i@JdxU*?&rmLyVg2=UYFqvtm^4>cZGsNIghs^
zWVqZRcc9bT#mkm8;SX~T#{0G!DNfntH$3iO(Ch3ng5B<RZ-*C}6sTV_N3L@Qx~N(O
zm0p(dnXIm+r;eBKh97Bef>*?nq3BQ~iN~Gsyl$4y9b9BY!+3TYKjA2Y(P3;nM~kOl
zQ}pzycq}&|9?PYtQeg%DXe}N<4#i!O{@(PUsMZzThSL{tJ6#*lbl#8`HVUF*-9Dq!
z?eg|?vi?LQ(is_wCwB0PdHkLJkiUBa+SA>wOhsns>2kKAnL~awrPF1!xx3muozB1-
z&gEPS@8J3CydjU_^LF*DH~d{b;UVroz#lM~io|)*+UubU_b^q?;ASHh*`E7|cE*;?
zEqLy^D?PM1l5i#xy*u39-BHm%>zv(OoDxF@*`A1|BHSdo$~(K9z70XQaYc8)kNR}7
zaJ(y?>c|=a5Bi3^v1BA6?B`zR&C|R);Oz?e+-u!Fqut}|T15`qKiIn^$(11^OFfLx
z70kc`$n=~#GlqH-Tk)(lCqknH{k|UR3Dj=_A;<{!w70|mI(mFCaHuEH#dZomF?R>k
zv*t}`Y8i^~WIMb^Jvr{?MFiYISXp#h7E2EWBB)qoC=yErQ;}hzhUnPJJ)G?!?^?y&
z>0j$M_j@iA5486L+&-_WU;Bo3wDc;Thd9yh?+S(jPFP0~<OYjKjNT&PzBWxyn@2Go
zq45<3JgcX>+Z|xg4Q<Rr!{$L_IF&FcT=OC_{wun%G8|rBFcgjTl9@yC!3g?PpP=vZ
zhl~y6Y80O+B&ZGz^L86KwabPqqpQc~3wAr(-8pV3`|M)=*gJY_Uq&NU*SrA7J7Ncq
za^t)HvT%BMFp8(!BY1*e^oFe0^mK96+^#OTLyi*J*3WqbKwo@V^j&O?8J#__YMYw^
zus0-|)8h|uW5L(_Fmj<Fp5L@^UDC_cqy^lh2mL6w+C~RDy04oV!|^1=7>{?AhYLb<
z1tpIuUF(7Y#?2Fm3`Y5jCDa=PvTuaQhQejN;jrlSZutOy@3w5m_T$0FPNy&1ojPc+
zBG2;q*TJ2V+k1xvw{UrXTL=zHk$as#um-8Amt;E>sh53k1M5!@4zevgH1VhZd5x$p
zLGP7r7EZ*6-PdB60ta4EklMk7qX#hS!t{g_7wR3tE-;2cIv}q4yWEUITpZ&e32lp~
zW8plZ9Z&@$ZzPEc6*W-a@Qi3?3S}h38;j)Vre3hYhri9eeOuGR2EBlROOqX%kC*M#
zU8px+*`8AzX(~bm)Xw2T$npEuQaBv%@k>y#_V}PXnm^ECd}G4{JCf)PgM*RSmehc#
zmk-f36HkJ&M4r{t?Q(|Poz76ei-4&@9GorWY@;a#qHz?G;yH0MJBln!V6fX3$+}b_
zrxWikiBfZjH*H9z#6ar8ECyqTm>y)uMP7RhEQRXdLOvurySZKzqq5TtgTFc=iuib4
z#26N}%?!Ki0^X3@=)f$DMU$>*ukfsZn?iNY@Rua+6ibW2G_M;|filul(Cg`1)8${+
zm7{X=;^2;<p$JV*#R$~8If{vPq&G2cnu6vG<6SQ3m{}{Taj1pjl6>h3{0J|aNe}iW
zqDda9%?vWOw$Mz3-e`i+I}zy(3%687Pu%o|GYF#_+>+A&41{UX6{+^77ILnoksto!
zWSgmXWe1F*-VqlDrq$`8VU~%`umdlbj804#uBh=aYr$xe*F64icb8}#1d5Rqp9Np(
z4*2;5i__-CSk~!WkBE%f#45w*?phTRk#bzH&K(bS=CzH8q@CUEW{YMAistuvC5;Rs
zIlWr-skS)AQ}Ol~7AA)?jQ$}pEkZo!GaWwN!{E~8at2%!h1VkJl1w-ETeGXnxd>$j
zX;x14mN=95WCQ==J`|pieFjcW^y3C?uL~_fD-m<d$;xPM%1-And{Eh<tN43DhQC9|
z3vzsicfADFAeKAbq~XlvZtGcvHzc8s)Oko$2WEEhFy4)1ig~IB$k~Dz;&WY;G{M}-
z?F{&6V(xOod#EaM>}Vf=<&#@?VAj^#mx3#+UW2|NCXx(YWDvbP(_1iA;`PW(%nfIo
zpU*7K0gscYp<J|)2xDsb)PdmOSIab`!{-#!yNp-LWgrFtYJYn9o4x(wi^YBW_WGez
zIu=>6B@&AucmV88^bM>KFP5{{Z?V_=7vXQAy&kPX5v9IA9qZ%sW1#Tfr@pQA!-;q*
zLRIFP^0_uQc+S#Iv}1P4)b8>7+`%<swP70A#ZaNVET2owJ;hRX+<T<B0%}Rfo0@8j
zY2h}Qx*@+Cl5m<5xV!;3CeoPLOG{;RGkb`dPNv>_@yFleWa^3TylJzT=VX^Wd@!e1
z6R=n$f&gcl55R-FXp%v?%D`{Hq0Ny)xo#_;pdqZF{~?&-RZra0k~H_?C78Nw=Dkzz
z^~SG%-u@FqxW>!~Z|mX#d#79oAwUUb`Fp8Udt=Q@14Mzzi78GNe*YJ}$qB=;n#v&}
z1mfGXJ54#gUbZRCbU;*irZb_J$oaMy&9hNcxo(UWaOUwdyNHw1>cTUBglU)8xyp}4
zuaQ}!s~t5KqnS9Vgju8ri>4Tz-{Z5REOi65j}XWE!+OzMR|qfR?>&<}M6O;jWA4Cw
z%<N>Q_}pxopyQ=wG~vQDkt+q=-Rlu*u|zfxD{=|y3h<>2r<4k*@QvY!cA~mAc*!4n
z!+4JwB@0EeNF*&HS<D594GM3UY08Q10as6Zh)ECV=uPnHcoNepCtnF9BB^vDhL^CV
z<=ojkTMf9ye5Bn;(?o1@(K5$~_xGb)<v5MW6pK)I9~O*qW-3MvIn~U1L}tR<MyqUJ
zQ1s^9J-AL()ifMQ$82sAo=NKqGBvJ7^NL_@=8DAF&dd~<n<P7oWH%;+t@1Vjy8PUA
zWJ9P;9c%))13^SW<-s&w2Hl+)uQ6g~CyPqAh318e9P_DJ6+4vd7{tCVEvS^w&0oL^
z6@A9&2>3fq3lp&h%QjmyiA^qS9Wg$uP|X@l^`_EE*|LRfwj@7@O5HHq^w5L^<Dko*
z9faxac02-c&2q<sf}4;$An^SN)39agh1hJ#EUQ%T5R=8io0@z})Q)&f(-jO2Tn{m8
zGR^!=4WI&kW~XxJxD8i^H$ZOTGR+psdTy(-t(;-0F;h)5GFt@H73ghwsc2tzp~II0
z2KMst)=BJ>V}BeQIYyK4Cpb~=W)ImQJD=w}k$go&c<zUMOR?P_2=sKT{T}W|d0wnm
zx@43b10(9J^7-2kFFUdQ;#}o6FjegC3Gq_U=R$!VzDoB7vG>gPA-Se}Ig05#-)=Uw
zALbZ-C|qD?TN%E2As=exo~?8*EUmFc<?LCHxrsBd0i&0cS-1t7DRyKv%Iu7SFSB~!
zYG#K))0-iAj?25&Xxm`$6$=HT>{2{yR#?K{!JP^FQqoFB66;JaCZ+Vg$3j8Jw8?GS
z(Wd=>dWRx5TO%X1KS^Da8qtY`zbHa2Q)y9*@jM~cyTUTrc&H3Ryvp(TQm|XhYt{6N
zX8Tk*MPv>_nWmt{QYfyrrO+0cl_1wmZTZkFitn${eyKs%5O_B2MPU%YXu2AkaBy~7
zi1XdcEs6N{Y$(e0RrmUKx7&r`8>>ON4CMVSjTf43iABP^<6#dB+xmP779R|o1`_oq
z)65*~J>H;-^#k{0b5o65?>8^Z)Em=Wn1R30QodwUu25WjZ>c}hn~Y*;!~XX8wqGn}
zx29Ild!MK<L?*}^YRZu#EE{KN(^L2*RHF0DV97GZ>Ko`y6l&UKyk%)WR`sx)t5%(u
zBP7+8q`bj&5tW4*9p9JZy9RKO-Vw2lA-DQ+=4O025Z51QQ3lJXR?0Y9!ME1-M-#Xx
z7EEux1`D82T#QX}GBrLZM=1dPGcW0?%fyYj7hI{4WOU108}bW+-19r!*lSfAyqI~>
zyppev<xW!p=V+k~1q>Lp2gNu3shrG;IJ+K(?fJxnW#b4hQu&JN|CwE4v%RyvW8RE1
zsQ<{Tepht++g8&wg~tqvx_`s@GJA$lN}T5q%ef4#B*YwxE1H>qQwip&GgY|Y+(lJM
zjO7{Era_%}+MQiu)RGre-9}~;q;NK_W?(9oS+$rABvqz8b`f9PVU`+*=62@-*I@hj
zPA=`;Qjt6+2Ya|7L;lc)ZfsL#odaWzT4`hJlIX=+Lro75m|gB5md)5d;xl(_Vy6*P
zWT46HKb!iInl&!YyLgAbZFaS6^S3HDg2&-hEFIO0o*70L8ov4m<Gq-j^J{qv84q3z
z8x`5*A10w*Ty4UpOeP3M`>|e0B+y|oYk{D2!sx^MqlaP)Qy048j9D06<aCQGP&w+!
zS}YN}xzHW*&3pSaB2ZU?mpI!!a=SS@8mlWk0k=QkatE+k&#$kkX|kHslCv*qp`D_1
zj7D6n4KN7M_2SI7zua6aJg5fzXcW_4ow{}M6Y@68B8EFX?^V#JC01i0b@~-~v!EbU
z(AOnaV?FBhhdG4m9>d?|U={RMmR+5m%^}2XG&zS-`_Zi0C%50LpvN+5_o&meIfM$T
z&6cZ9+q30DqJlPN3DxOP4xxgs%@V59>v9Mcv@=VnP9Mu5RM2;_gzD5}TIi@uYqA)y
zIpzYOZp2l*Wh`@-?Ks+ND;%8t2AS@Wsk+JZDXEewF~2NzmrGposoSt8qGt`s9Ya`D
zBTXelI-t%FCnJer))2!+Vgp{iU7y0vwSaIhQUia3czra?8VDNbO%AYz@QxVdi4-F?
z&)J5H_0*@$fPuUO27h9ZXT!M9qJhtk8d9|F-++h_MP8i0`n(}BVDu+?ha$#67`zIC
z(7rw+GSU|rP8ob}K*%t9>Ba^jL!DpK2S$|FI~45$UmQi?r-7F%%D~&zSwmkOmuxUX
zG=w9Y(_6sY8{2|unFOP;{<u2t-Ml#w*`^@chE?aVpgP5*R-H;o`Azl@!+6|??%twX
zwfNC$ambV>neW61NYJWtO6TAYw-9IeLly@h`W_2Um|Y~9U)<@Hb2I}o^QSBho}ZHZ
zbsWn6<nz0b^)}w|qx3&$ad2Y(Kn2di9Ks#KGR4ipIY$>D+#i_HpIIFuAGfE`Q7cp2
z8_n&c_;D*CKi1-xNL2aiJxaC2fJ41ENkHT6M|VQgJBt**dQZ`J@MHlUZ6e9&ujo{|
z75VfIBn7MYAlY#DBOSvc;qR#>J(d0v&91`<*;MhX_b!b{{w7J7Z$DN4J$d}<Jx+%u
z|0p>Z4&tATKUKcsza2Otk><+0_i8>7<G`KaIS1)#KIpD-Gk^DF2hZ-7{QRA`CI;M;
z&JUH|1<1^=V&0|-L1)P;Q1OXe#y*+Huin!Y#^W4x$YvA(e)Gh!L%>n_%KqxRCc~0{
z7<uMH*-zmP<HXFbzGtMqbF^F$$~3?HZ{+dYYaE=?jt8;mSWd!CM}GN7k!LQSUJp#i
zM%<uF2i?kSKJvujEhV_Q9)Gf%W!`J1zMGV9@BH##0B@CvU%mJ2IXqoOM?QT%{0eAD
zCsjVZ;hK(<e<%Ozpe5f`{OY}5>bq9z`&G)nRr#u3XTfVOUwyyfEFLMPL(vN;3oHCP
z$T0J(_pUL!;5eulWvcj8S^{P>|3UjEPI=HS7?hp@%9gL-$zUc~s{Rj3{=;@bq58R^
z&zDy%%WqNyGFAK+=iy0LI#ha&<QLfN?@#RF+>u4`GsUOS3nt*7a)F?jn-_rc`4>;X
z-+Gzg-<{Xt^7-2*;Q!W2!GEs`3{v5Ie!t|;x8Id*f?=9nGUo9sIaf|letfg!e~<Db
z6Y%c|3;yBU;xp~0&=lsB?<gFlZ%Y28QdDN<fm~jmik$LPTrjMcjy@OQGW;pOSAI$H
zxls9z5y9X49_=Tn3jfkqVOlMYahX;`do@ooXIi0&n*ZeDi!$$P%*A6=wK~iwer`oX
zQu7dVrWG?SHNVKk+wc}ShZ)7str)%3e88M(Wo4Q7HRj^Se>aQrtr+>$xSUI0k$Gk`
z7eD@cT0&0tyI;BVdggn6x%g_P#<N^}4aO&j8O6`7_%hyc8UM|hRyLKXaU~Z&4dcWn
zGm4*E@r4~V4wy5oYz9;DJr@selyNf`KmNODly5bCM=h70X7l3@>$q9Gps_MJ$mOwE
z*?D;wMK1@Xyj&}@=V2861ry*e9EYcVUBrGrmY+dmy9M8AQ}Z%{>6n8*4R$yx?WX!Q
z_3=V-T%3M<dtCuM$#2pfILjXWIOXN$;de-U{(E5W0G{-DMCwEJq2n|7(+c>{cY!DQ
zNAu)A#p%cU;Y9U1!|AKp`aFMrm*dCR7wCfaDTBfq8}Em9jvxO$ze^<kP@dl|;}xGW
zuD`67`27Aw--FZ&^p}3%3w;;xTHtG_5_t!`N|V0lm;Zgfhb2CL&*HHO<osp=d>IBA
zdtR9Y&YS>W4}2j#S8@FK_|`cA{on-hZw9{5cM$KMfc`7M7y6DPeJ`nyp3hEz{{zQY
zu>9{VTCIiU(7PK8$*1p17Q$Z#JbhR4uzgdGwQrh${!vMPJdgg!1oY2NfTwjH*(bl;
zQs7BXM};FtpNk|u2MM4J_(I>gT+8Xle{b?CjxR87=3eia;KHuDgymZ*Z|HHez68G2
zn8ufK`fyo)bvthSr=4ztWxpP2q=t+>T+PHrl=^I>35Cz+#f>e4@y)oAF^qeNl16WO
zgkd8K4=bi3;f5v4meS2S1&H|9_{0Vo9`Q-x(Iv6T7ETWh?Es4j;x`)PkQ%sE#M$Y_
z9jY$+1}4{Bp37_6{*De@(xCgv@mLBd@Da=mbvPL}2I$E{+|iUPJDiKaeGa%jXSg<W
zIXk`W%>1Rm>;*y74QTImqptkCV)k2vvVg3xzkPKv=fX(lp~ajBN&cSCgPLxzb2!uo
z4e29>@(YRiAA!sg(N_=u_SZ#oKdwk0B2`|K`x(-l&n{-`pScD;(Kj_S_qdpB$+vC`
z+vXqmCg^)yb<gUk+DLrf)BIt~EKQ~!?{z;Vf4t3fSxgDfuYX411Rn;?e5Z5#2SFVU
a)AuWLFW;kO<tHx1<2$)6Ep7<m*8P8{MImGW

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/index.html b/3rdparty/qpOASES/qpOASES-3.0/bin/index.html
new file mode 100644
index 00000000000..eb319306ec7
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/bin/index.html
@@ -0,0 +1,7 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/bin</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/bin</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/qrecipe b/3rdparty/qpOASES/qpOASES-3.0/bin/qrecipe
new file mode 100755
index 0000000000000000000000000000000000000000..49be1d1d5539fadb962463eb7fdf0024e54f7b21
GIT binary patch
literal 57832
zcmeHw3t&{$we~(UNth4_ho|AC0|J6dUIYjVGLvMI8Oe*xB!rhIlVp;NB$GHZfnX6M
zP-{e6wXM?DE3~)vTC1(S*ILw8;}dIbi?+3DU({aPV68>1iq^{it^GK2W|CCAw)ghl
zp9Aacz1LoA?X}ll`#I;FT<)nXw`m$vmyKP(D79_6i=)JyG4COsl(=G+$y{s!JCkJq
zlMZn4912BDg~z366*?rH9bp=v*d`FgHW!bmaF#$b6)FuWr1BD%OGZWb+UX*xLdKjN
zL42g^6N`BgVH1z2@S4>EU+fd1;v@BlZ%FbDNj??oQhyby`cd5o0x~~fb0I}(7X_@0
z-xDFx(pCFXyp`gIWyKHAauFJ75mxdPV=7d9Yr#i)p6sPd!a7;rXm&}ok6=*kkk=V*
zURIRX*}Awh9O>;_+*iD8@v@@aSa)tAuQ$mj+KTFW&Zz8|gB0mJ1wcMP`JcPN|MRLp
zE`4F}`Ay#`2)_2&OTV*w#eC#V1rROSbo{I-UrN&gX<yw{rn4zyzseRm@^hSr&#=vL
zr1wv;*@%{|X#gsNuIVCRvk^LFd=A1h0rLP00A~TzwMa(JL3l18S7ji~0~7!X0ZRaM
zEd><wn7FM#a6VwA#C@z`R(t1|`?|*8V`G<n^Qx@#p1d#b<sbhk`*_u?_U~OW_L;lC
zY%6}@2fw{#>E5fdYEG;ffBf7lcWlWlefDEZelTv_c{gQEf8_Z+i)LRlYuuC<Z}B(m
z+Hr7s&gtLYbklwJ|I)j)&$s2CEth|O$N6=$+F3{XSDwG5an}96y<x|lYkoUe*!=T9
z+JC)feBUpAfBh5wAB_7w<d%AV`icuOKHayTeRunMcJH;XWlJ?j3~ol_Od26)GH}ij
z@N-AOJ4cZd83q5%QSfg<{z&@#22M5-e%UDc)Qy6FViY+uN5Ow#6#Rlw^#36`z({&t
zIST$8qtv%^6nw`h<?bE@AA$aJ*?877!v%?=k5M=Tede$U?8r<PC7Bv0{)~<|5k1Pe
zx{2ct$U*p+#Ipi{e-8X~RdG2pSW{OMPcw<X9`ut~HcON8MJ8kW5Fvf~r9R_i#$xC{
zhs|I|x|=wrQQ`}vd{<Q9528MF9pv)Iv+JaskU+Daay@4-XS|7Hl%D@7^&e~#_<pJ9
zO%mUKvB3XC;=cyG4m%9y3p^Kw+gp<Uh{UUT;7;J3BhY^w`jVZUa$KkS({(%QI}!C|
zQcor46Oc0r_F>X)QXTdKRc=cYu`{J-SlXva+O3%9GS(=|JtpNSpZunzcl9)JN;O~m
zrT-k5?2-~h@Bqh;XZceEzC!wg8}di;tMO8fD@D$e!0V`QK>3L*<ynbm+nOXs1UK{g
z!fsN3B>DACX&<$2k!bd^w9imVyF3p9@~Z<W_IX~?50*7?5eI3a!u4y3cZHjH+J$=3
z^$6E<`Y`>Uko5iQn>gL3u{>bUDEtb6SN&*@#2;CbjDHyTNi2)?OmYbU>W=yMlAap3
z-{YrAcS&wOGmQ4G?uZeK2cvPrV8(@8jk-{KI2I2@OFM(HSSZE}U!clp4Min7P*vL5
z9SH@3&7C2UH*(H~>gBSmf<k{!Fd7S01>@0hUqMkzG!%@7%EG~RU!<+uvy~YM+GT>M
zEMK74C>h0TYopyLv8!NddnjJk8|?IVclO4^-4Rc1D+B}riQ)?U+q=p_5niCjDm+<{
z(gII7TpRWm7+gYvKS|xv!jY+zq6J*hfZt@78A$~z;zB{(Fe0&8*A=R+B?Ze-+1hBh
zE48|&0MjmILXqJaMd3&|?y0Tz8wE>Yn63~Sp+#8PQ@f^>GL*{xBE$S&7Z!F+=vq*q
zjEYvI>IJNMDK@vZM<Mb|4@k0$sXcECQnA1rip9dgNML(UD7B2y<rL(@PNlW=@o-m2
znAm3;LRF5z`D5|cmUGS_-(LpLZ|MsfJ-yM8u{9ix!=q58NU#g{)?h6;CF)m@Ph@?;
z=J3{ng5`))pD^N3&Ipt5Qvd1eGkQYNSa&4Y8IEr^wxUyQh-~9>Iw1$ewzsqxF)Foy
z8TFBE;YcewS**7!M42s0owlB6I1+C&S~|8cDB9f{hjb#4y|XJAjxZzC8jJ^#(F{F!
zuLUa7%JO<+QCeQ-1>;b0adR*h64CSWI=Z_;dEr<`xV<;po!1hKZwW_?ZLz$Tj&N&t
zG@RGm8}4k)>xlNmw#V|8L|f5U<J<FO-&Nsa3>G9mc3xh0PdqQ$9m_*C!tLFJ{NOTL
zLt8_gM9O-bRjpcLy+#|HlX;DcTQHK;pgyfotv41zk2a{Ds)oW0#6Zr>+(tNx8@M`~
z@!yU3Ry3cN&|VphL@q^HZq|-WidU4D8Z_XQHa5CTd`3a;5>`>^D?t+IA`v$Va|?5q
znsLJ860@cqI}Z%2Nf(B3dD*ew)8s$e!)ScJsW2)`<2hoFlP;-L9!(92IK@P|m(LZ9
zIgzeiusw@*mhVYLH0bs|)T8xQKH$gS*n$}~ou%PUxcH(QuoSSf338q#`WvJ?WV7BG
z;Y`{s%~bT$DQ#nCN&4e?O+?1`vcd{HX|B(RY`Q@4yIMC8UhRu0L(P|jr~Qw*j>vhG
z@M_Oad1^gDc-mK~ORd`oPx~|TlItGA)Ba3dIdc9U1B9G<d6v_Hx7KT+1#fNZd<&k+
zP*<@9Z{0_`EO;tgU0w@bt<fm2)`CX^n^&U+KhDH5)?~qBb!T237Ch}e)zxFcTQ_ih
z7W@Q7jIiH=CqGixRTlh2g+zFr1+UhIl)cY_Ki5Kkz=EG_!QW=V%Ow=g_3nPf;oX~l
z*=#5Ceta;VK15F8-TkQJ5y5<7X%tx}=ErfLaE1#pN_SA^(IY2t&A*t^<N`+z^K>(%
zsp*dn^7IBuQ+Gak7f;twnq1)M0iM2)($w@vujA<wN>h^`?dR$9C{0a$w1=k)DNRj$
zw27zBrZoB%J6g-rb16-$yQ40io<V79`lI<gJ&DrP<VT%6J(kkc)JJumwo#gz@+jl!
zcV{6@O?dR!`#{Wpozm2FN00FI%ao=jJ9?O>U!*iO)zLwuVcSJbPWJi)J^gto{rZH7
z>5I;Stq0P_wh($^`q@ZQcwY0#hmW*PIAafrV9dSlL0Yo98{GBoK)v64#Vc2#n4w-2
z{a_zr-hoGkIw(O@-hs!4HmV%oz@tOel<-1~4><*>UFF_YFK!_99`YV?73X6!bjanR
za=d$&K8=jZYcFwl2g>F--S^T?%zcp7-<1Q!zP%Mg_uUP;%7M4N14WwmP})4&!re=_
z2l_A=4V9oQ?_RJz=puQCSYuBU0$1@}m+;WB59y&hK;^!dR{QRQM_M3&7VKQ|0~?8L
z;1CtRaOly$LfL`0hc+pxRPbJx<3w$rcW+?cLZ2Wk_U^TL2Yv?ZgS6u!-uxjiczgqo
zA9B&aj+&+CQI+>j%|}pmZG-bc!b8|l<v?Jb*EcXF12i)Q%`BoB*qP%#RPLa<Asj~`
zkt42C+&2)&aQY5q^kEO>J5;{#xd;!P3lR=|ABubT?wFTP7KoyP<m!i93vnlFID2BK
z??#Suh=9v~fEO~*m*c&5$GoOPE?Ll_LuK<e4WV}pErHyj({c0d9YZvmDhJ;2?v2m$
zy6>eujQe0eb~yLZaRHpIY@W+&_YQdncFfbcKriIv2o=iaIq2|!T97v9Ll?gX!H?l4
z2nT+}`MpEC5~x>sK71VJ+FLnqlXvf`bK&&&EySkq0U8=!pRg$~&&AIK+9oV~c<7$L
zkht&6>%lm{4Zv&6>u6S}naZuALa05dDo({>t&*$_HS^QnYXc~pS1d43=au~%6}uiv
z483B}q;lYQvTk2V!oE86JET#)O=k6~3{<blQoT)y>OC^_vv*INXq&KZuwd{yaE*K9
zb@zl#;~w?y9@M<6s2$rT1jhduReybg@dWJgQ1A1w*t-`&<P%CHx7E-mO+l)^Ltf=$
zj07SO5+}gIB=HKX#GgUpZ6tADvc#2;NbcwK?j3WYHix3HEuxqOcFZgGe*6UHfJ2ff
zUnCDA>D!xu9tJYFSLMJ7M5;di$GD?%;7>!JLw422|C18GAG(HEDaVVP%X#AGl=$^f
zloGPdhvxGHJb7p)#jbdiMy-Oujho$@+?(AOxs69egAX8_+SuS^G{962T;QeQ1^w|*
zIUk&c{^cETL-@xn!fD8RjPTS`hVGXXEkrT!$RCc1DuX5{b9~4t_@i&&>5IGrFM4<X
z=ds#=@6aF}?0647MCVrCLx<>CWyl4FKey$orkHR>8P2y1bn<%d;zk7C-N&=N121i}
zZGPk+?@O3a|GfD@$~tjm!WsR9J4h)-|Il$e=)D6EdmnmvwO2drefoGWnC}zJ&w$xG
z@VMZAQS$e%`ptSLmX{Oj!8x=OL-yv!(|-t*_QoSLKYMqt+BX>l@zYHerqn-pWHGcG
zc<IF9%||~5mI-IDqcIVB4~?Nw9=v;~RMeHHkCq)LG8g6-_rM8vgKyw1cfD`mr|v-I
zwey`Qpx%4!;)MwPm5W{~___C?zuIXfF?8qKuriwebS1BH;N{AJx5^;n#FSrncR!+e
zSNx&(Wm;fZ+PW=a)Hfc14zwmoEJ3(l>PG8qx^P6XHPq^CrLzcUk2t?@h9l1Tt<EqG
zSb}tX5j)p8KX*x+GZt#;j<m*dX;C2gV*J$PLom{@h%*$8c1O>1&R>yR7;-unc7@wJ
z;?78S+}Rv*_C~^&^oAD6V<jd|7Lqu4)-g<lbgA>c;d^PG{eib5a8J6_d9gaDRp)vC
z?cp`w>YM$IsgJ^|=(?OMhnqIjetsoFbZ<8J=7|&gC<9J%3_%mZg&4g44TF3K(g!em
z4I;b?;d2NF5%wcLAM-W+9zfR=5isr2I;QpM+RSld9M@@Mbi&U9EQhSKB@G25(~IAm
zh~5dETqHYNFVD`oa6;BLM?YIV<NULi%$uwD(aqU&kV|z?7s=fOI1Kzag+xgBIKc7N
zi4*HAcq)r7?b6yxuK5@{9pT!A{`egddGi4I;G?tKw7XAtv65{4Q?`oioKM<IvYpqa
zm1HlxCf%K#e`SU{yLk7QitNFO_IuLCW*3)c=ex5Pf~F)p2RtR&4!7FtknU-yUorFy
zAueQkNT!EmdPt_1WO_+vRko`$yBKAp$m8{@gq$YG5j~KEiR;7(ZN;<|jD1y0e?F7_
zNQ3%sYw7<!mYvW(3EFGZp4RRh%YK!9vwiPac28zH5`&p*7`tuk8A`;d@RS8kS>Tif
zPFdiT1x{Julm$*%;FJYUS>TifPFdiT1^!oBz%|3g^*)SEK3z_k<_X;7amKKaesiWv
z<<oD})-?UbOqcpi@#iwF&LGrtN$R&t^<3X{!Dc>ZJyWK28P1U5b2uraOPyD!Gmkgl
zKhaJ2eKr>b410Wj9n5s`kbcGBSDK3l>Ktr|Bvj`SIf9BmODv;2kk!BeNE1)1GltZ2
zAN8B>X_EOi-Nj=HKQuw4mEMCqm*M$%ajEigq{QpfA?a~u#nVcUV>10pwo7vU_XO%W
z_SDk8GD+xhj|?A|;j=RQoebZSVTSa(X);_O!(}oomtlhp+hn*^hF8k)Mj3uZhWE(u
zaT%uS`9bNMWW&~e5aFr(Qx^CL7I2C4=(2y7P*vrhMW6rcdMJB~aGi^X{iPxttQR3$
zB}1NzTTx1SrA$lw;98fm8|QE}iTwU@8D1#EH6k3W6Cv9qLnbbl!tWG#SBnfU5+U=+
z@B)eV%XF%Kg%Z!?g<mQ8HC$h%_eTx6c(dNJo-S!$RxQd`^&U*A_h7BaXCG8=segZo
zq|cQhlUHimDf9nH>h0Q)tgotf{67R<wRitUNvGQTgX+B^SuXo;s<+uMq<+KFOkOJg
zWYdc7qlX{WuND7C*W{Fh|N9o0=Mr_CBE!_QvVg*?G?VlyJuF{k{F8=C-w#`FMX#!*
z<f!yNd#LnP^+-)C{S{uN6}?KQ<}3WE@RSAqsVy)lwfnDzs&a~-z4z7Eo~deXTK(a|
z+EM9M0aD(BA1<sNm0rpHdxamazREtQ>YZvg)gB+NottDk|M#``=RZf+y{rFTp{lo(
z_u$_vRC515;jssWoMWpyGv7JTeolkysmr|&Ue>PmiTSQJe#!mIV53XW2V}TVhI3_@
zkhkUsr+1{M1zewaKQp)Chl+2#i`!xPQc1r)Iqcu$TEef*!{qffKfCvfchtG=SmDaA
zcq5?n8f4-U^!*Kz-!9T@;h?bl;z2={&~M+r9{N+)Q-0UmPu;+l?ro6#9?7TnYf`^K
zWskWsU#$-l@(!NA<=dOT?05ak#9P1n(B<mfV$mSu*Rj<+a4I{<!*EtO$j$q^rHamM
zkIYB^@NWA30oOh4H>Tb8T`A9{_T`IQTppA9^-H<^UV-l~k@*Sz{`$~h>`%7_Tz@L|
zR78EMJ_`pKzdpQvzsN26S=i}!y}R?`@5HZ_=N^OV{6qOgs{Pgqz53<(154;v)G@UG
z$g~F6CBIu2>%UIw=Q2b)e?`%!_=~ey$Wx(3-ioKcUh?Fv4KCaMGs+#k8(cqlimn_%
z&t&LQc2nm^x}+N?=@R;#>K_a*#hvOOumY|R?!Tw{2ifV={6SV=r{<4AZU@Ww{Qp`1
zQ1g0fDAol`&c7caJb3!Y|FHJp0}s~m$aAusBdWZ=TRE~FQ{|<W^FN(d`i-W?X!sA}
z`$y$}IJx~5V%^v_NA#=y2HC%SGMtwjy4H!bOVOGBChYK`<w-fNdRfj#lsBmKQlVOR
z9T~+gsrb~i(kB(4njS_sN_k2jMSm*I`1KL(@uB_hlphPbpYmhre`5Sf%$xr+=~I53
zGH!mLf68_`SvqsPRqgxF8YbJ}lwTy<gPm-9e-NGM{n-f41kn4q6#y@w9?$?-2WSM)
z^SLKOXFEwcdhYaO<fWY?9X)n?GV(G`l5WgN($RCbCsQsxk9#t7Amh(E&qufb-~p5a
zY5=u>wSYPRwZ{a&2hmY`oD3bc$H~x9dz=g%wa3ZOQG1*W9ks{F&{2Dw3>~${$<Tp}
zfB)<(gbM+sfHHs&a3Np~pb|jsp#wgMj@sj7=%_tThK}0fWay|pPKJ)!<7DWlJx+#>
z+T&#Cs69@GZY=1s0OJ7anSZi9*_CWY_Mo~_JxE6?pJb=<<pGxuC;$`!mH>(XO99IO
z%K^mzyynlpy?q`+qCX!%JSzdjw+cYK7XXN#25OSw0+1XxfMk^basecBG2kC5m+DGm
zO$ERUNFB@91GfP{cBDSw0gN=p_<^VIG?1;SZASC`D&$oI&IFL$c>q<$e8fq{0)Uc7
zeT3+#KPldG5hoj;10Wlp4ImpY0+5a80?5X50Ayn)zy&x1KsG)dKsKHYAUn?jke&Zg
za{o8lc&4<+41{Fo=>W2G4uI@@8i4FP4M29D3UC6Z0Ladh0c7V%0J8H$0NME;CHH@;
zjmf@bTe2(Jl<Y~iBs-D~$$n%zvfIhh{%^Des>d(Vo$5}$Lw%mcP3qe;=F|c<09pYN
zKp$Wi;2OY<fCGTr0e1r)06YPB2Jj2O?*PXD?*izXFWG<`z+Aw&fW?4ZKpr3;Pyi?d
zECCb&mI9UmmII0bD*)#KE&!|sxB&D$@e)8OpbX#vlmjXNUVsm<22cg40jvf10rh}&
zfc1clfXx5{5CpUULV$Ka7_bG<1?UD`0*C>60ha>?0G|eY7O)TSdBDE__5;2I_%h%t
zfZG7K1MUEP9dH-m-vM_6?g7yE)$Rj)2SA^w8w5NIcogtBfZjWM67Uq@hk)k*F9Kcx
zyb5>?@CM*5z;VD|0DlFX0BAPMy?}JU7(gZ<3osrq0Wc9T888)a8elqLCSW$;48R<~
znSl9#vjFsM{qq4HKn37JKqa6Cuoggn)1)5I09XfT1gr;a0Bi(o0&E6c1TX+ifFPh5
z&;n=$gaB=Tb^v|7lfGAfF(3}u3AhSyHQ-vnrvNtqJ_AtibHU{-*F2X<nCEyjMd4EE
z|LOdH7Ck=54yW||ulS7`U+01u-I1^BXuhHK7+jXG-)Ih^bs4RZ#{+0yqO}>Vm$Lyh
zS1kb0_5YhZvVjAj*10rSjRa&PTJJakWJ{XMMgm%M(q5qmPz<2C?qt9Px-x(lPzj*9
z@f4i0!H4w$e4qY2=fe8t-blRHS(ICxTeP^am!}GL78K>?7v(NlB=G!`el$dg4v_e~
zB6SD4`e(R!NL_<2^?Lt0ksS8tS0vgDc4ADT?gjbU4<RJ&77R9iz!6cZNoaGV^<q72
zuf%3O<0t0>Z*z=k#3*Ic98KtWHrtq8SS#Do#%$)ujAo8ZpM;K@mX?mcb^wNS11b9p
zSbLAz-3bx)E$FUe_D~eka>1DH$Z+hVq?Qqe0Q(w@la72^V2-&G+0SLZc{UVt&{qjE
z-wFZE9Lu0~=3j20sN*Uu-!tE%xbFBjXqx#~isv}SqehwUQ{3tJD&%IKp!mYfPmmm@
zQ6fKcCzQ$5c%nG-GZ?5dZ9L)1^bmU*Pk1vwfn$rzbe^cqbY>ut!4r*{g&4du9U3iT
znlgV2Yh{krXzkLG*?{)S9H)`OJ((|4j?NSD%s6V5IZ2Blr!Vt3<xJCjNc3l3gDptr
zY1%p@c4dAO_skqE0%^N5Clk*sjW2^T`zSGA+X;f}GA9sEu67*~`!c6dB9A8yWX`5U
zK2O}1Igb(rJaJd%Ig}{W^1*X3^Y=J&$SmTz4QBR|L6&KIQ0Sv<?9-&e=f}*0xpOiP
zP$eBzn29pKNKxk4O=kTPRoCITJ00<_P;GUV;h^PWhJ)HLi<*|y9yfppVW~Aamey_@
zI~mx|BQc(6$CHfgqp0H8E4M(;3HPJKw6Qll0RsIuKrp=)9-^@c?;%JV`yhP_Qul+1
z=}oALrY}d-re6Xdx9k5p2{ihTfKS)2AU|dkf{>ABcYP1dsHdU2m+SNgjo>|)k2U=U
zklOV7fw1c{AURDR3%5wuccQQi{inc;(cc5cp)Vy5(SHxjSiJ;=W$BfWHcnp-)yL~E
z!|k*6xsW+Q&m|4@Jy2z$z8390NuR7^`K>R7LR0i8a;NHB;l|VS4Ulu1elxgo^b%+?
zUEd3tGjtvCnfh&r&(dE+Uz)8SfDWhY&!F08=v~m?sXqhxbM!M1pR3P;ZfEM#&_46@
za;z=p>+eGH0{zRlpQYPSmxcN_VSq*Y!zlM`{d!=|(XWG6=jzWPzF1!kJ#+PUP;Q?7
zDlqx_J#gp(U4w)|{R_}xiT>~Kiz59ja4pq;g8DAgx4>@8^)EnXvHmLVEA+)E<vg9X
zGw16|;Ug>c7<68xFM?JV=*v-~)%p-54>JAJ5cd$%{|u`<%=G;@2783*Ujp5uOuq>|
z@G+)OgC>tN{UM|dG5u-CeuC-m!d%~D`W$HdB-4Kd<oB6Qe?;mj_ys6`z)#B951D>H
zWc~=kkaw8rZ@^$rGyM*bJj3)LhO!@{SZMbXruRYBpE7+aG<%lmVN~~LOwWRX&!GfV
z?0Kd?4(q-EK2ZLg=^G*J7fk;;ME{cMI}!Yf={um=ubJMC8XaN!a+v5hOg{+Zi%joD
z#eU25Hq_`p;T<U9C8o~;$?uq+2juUW{s=sE2zo-Lqo73TFEjm%Q0Wg${{<}l3e!sv
z{E_Lip~S09{~7A{C#GKwDaV-pS=8t?rr(X=b*A47k~f&1kAnWp^rxW2n@sOQ32!m|
z5Ss99rWYc0oawit(cfYEjbM0}=_^s*Uzq*?>hK=ZmqM4n!Y)X?&-CLk!3n0%1hQY#
zr^E5D(Dcv3pLc2c+h~W~n*M9F{2om&h4LTQ^r=wrN=-i>=J|xCm%wIMY5Jdm9MJSK
z=y|oK-wIv!YWka~*fp9y7s0if9zj8$)by93;HNZw0IBOVHnAA|3$vEr1Z5`mqC#1B
z_kn=fCcO@VtZz}mVVjhV3S@mdgbdv_X^?UbQeuj2(nbVX_fjIqHmRKi+((I7wn-Vp
zb3Y}Vw#igXR)yw*pOgcp?4&{j_Cl=zn4rQ=B@UTYtz8224l`>H=UPs(>a=TsCYR>2
zrf{xlYf<s6dME64HAuB@Ap1T=GL>^r`z|b;wP_EOqd`x52HC$fNraR^G+tIq1+qEG
zo5-ex^pxu$bLx$x!?Z?tS62AzxO0*mWS^Nr@(FlJR?j!kqe!ZDKC;VA5>e1BG*{Nv
zcR<KVE<$!Vg(Qbex+4!J=OkAk`xA<UO{=4(=>Gy#;<@{gb88~^8>GP}-bt5Aeiu1U
zP%ar?n0eZI_*B+4R4}i{5k&rwLUIe7FzfmjSeBF6;VF{=2`N(4YINzWn>x^MzCngP
z2Q<rwj$4h?W7E7u^2K>DFNv~MBRil(v1vI_H0#!Tp)}9!M9wyqD;n>#C&89=M;8Q;
zK-*r*PRM8DX+pB+p-yA3T?B&hH-jK2gud}CBGaEnTa5d*b`H|KDo+x-e=dx1Gc_v9
zc^d6Ovmkep@jYHh<AiU6@ND3x+(wogqT83~HU^$3Vmz6+RhdF(tOeotKWi^3``Pq#
zQbwyFdFxF&HuDJD(viJ_%AbM?&UyjqK`=9IGG%42(sm+i4`m-Fw7|Nw&j3RsF2_!N
zma=A7q7|~sG?Jy=joX7N-_lRAE3O9VFF>#T4B0O#5+UOBA=E9q?l>$$rq*n_jZ1r<
z$X)~!Z*@_5VZHH`f5r_Ml5@1F+H_PwM?*O2mN6;AJ1b+Zrp>wr*`iW&h*Db%vNpgJ
zGAdVY%Bv*&FSs#55=Zu);UsKsHp<gxYM(-fCT^(6s8sD%+`dK(8mhqNev7Q_)Fz`B
z^TxF4TsiG2kQ`HFQ%x@tRiZj`uMp0C<^qTkY$u2%SzD|tcEPoDZ3k8M9k|!59Y~9M
z&buF@k(QxRr*PQj5TRB9vO2&N?m**7&B;8YUuy=M{;UnpVzWaS<uyzJz-X7@c9qH!
z4LP6w{-Ji|wNyp45GVUGNN!hTY{4-MAllV0V97y+fy&OQo&?2VqMA&lFE~VUKIui@
zqb>ugG){F46n{}v!W0W~iQqFcu<GFb9RAEnXQDah0HnYym|OeY0=SP9n8B&spr}+-
zQs6D{Zf*Z~j17`>3@7ab*^VSqNq-RxtliqJda+G+aQd4;drvYwZ&VU0I{1PCbVBXU
zXA_=~$vK||*Rj9N`8qY*w_by*Ngo=^Id!ZNR{(gWxq4H1aeO2YKBP*Q%3lW)YlAD%
zC{j=s7qlKic6}&8T%+$l3bm!6aa_=sAmjy85H~==wg0AWQ>i)G0<Pfm^jm?HJD$sZ
z7cv*a<f5h@MlM@$G0dj@eimF%ip=IBt01nCMDkuhy0Ha!V93>u{Qy%C4Jz<M{M{9V
zeIVPZN@oi`3t!RRyfPtB=Tx_V;;V{kDs7F}v{o2Md#@S{+*=<)_8~>W&T1zwJ@F0d
z7(WO0H<Ux#u!Z-5TeE2c)b6jFXf~ck7;cfAOQF~WaHvHT(N<~NbR?!c1#ZhY#(f}h
zlk-eI>bIR(DWW~AX#ry010_F*_3T&BqtZV`#`+|9xVN#KUjz9=l1CA3m0kNX$$J}<
zz=-nxij1lJCiJo22XZY!YUXT%{_i6>J)dkGxBVa;i&fBTZ<EN^>Gx80j;GeoCKS?1
zfn!$Pz`k_tu9fGm!w52A1;;&q*$ZzTe*J;-&TWCSPguoq>ccFHgG5*I%pCj%mb(!$
z?MrEgn)fsKi2gu2hC8a;_&exE^CY&l`*F9WrR{@jWH?C43<u5g`3_pN=JR+4k7qb&
zxtif12^st#Bf~*53Tz9$jOHvPUK*5_Aop>EMUK}{-nTdpO<ws9nt1aa6wl!C3<tG$
zhU3qi=3U&E<~wMB&yaWWyL`v{97FeIG<Mt1f)URHEPn&-!6P@I!|x}D98d05O!E#V
zNqa34or;i06yfqF5MP2UHl8M%@iZ$gqiNnwd*GWC505AwHvTYCkr&%<2gTirid10B
zXvVQWj41i2sT!(cM>8xNM{RKgIrGu*JeNn5Y_8AQ$Xd}w&Q7LW5n<W|BGy^PwACWI
zm1!=G`_*_Vhbvx8Q#{qz1p%9s{NeSbEWvT!?c{URBhJr23))`h!p}dNh(mAceak9H
z?|mTrrIO1dNqWDCEYkZ~Q}3T8^nT9N`+2E170dNr`3Q!XG=nN@p8$nceH&9%nn4b`
z>M68Enn8oi5_SP~X8W0-SluUkCv#0l?cc|!ss6MJx;nqKo6A914RDj6X!U}00^<EM
zT$}_A#V_sVOF;H5Knao3RZ3zN9Y!yHX}kAZX;|4cQqU{tW*)8H{su~6<$hT2@MITf
zStmG|b~6nV6+u)vH48@fZUY{FHEIh2%m^6{8gw!oG}bJ;2{+qB?0|v*?l3%(WO&+{
zuZToxOcEZ`0+$g{AXnRIWLW{6OT^$iZoBXu(u%jig)g8F*>2^Hl=@e(wVP=`T0>R2
z6Oqa=v;CXaiPQ=}RW>Pj8)vj%e{**w{_GF4eO05;;X&kA#=|Wt{U%R$n`!!Ky|S->
z*>2}9LBD2KZb$NKJoyKnTgYs8@MIBupmIBsck<+Al=L#&T^iNlaZ0(F?VB2@IvL|l
z^@|wlY~SLvRg}IRgPH9fp1u?5N^dK(9pt|M3?<!2-m6hxa=;s_ZiL_2zN^u=X21SG
zcU5CL(oe|rUE6BxF!|+ET&NI!=?6fMH&CFv0r;`@!L}ygFc!Hhd0PD)_WkY>o>qT{
zd%tfv(s%pLrF#jr@>&#LlLtRg!ta6@t;BvhgxA&qUqYRmsMuN>cm|{n7>xJ@(jZ7n
zXaJ(WHePo#BG*eI(~<jYY2ZRhs}W>QcX3v7J+NOzB{c(ILc`{tcuCY77DzI)gO%97
zP9;&7;eV&q|0`Jxx`J&dI>cI=<8F^u$M21qc2#MQwm#FYEaI_kW0|JCpwZQ+-j=CC
zBYNxtZS*pRgOX=}51|oC4k7aI6#1@-{D+Fr`1A_0xMh?Ov(hhUA0t6znWgXrZkb_3
zre)}iGAi|GBFns>(J@-d43x14>?IQrp<}(0bQSpvSO`=5IwB=B$kXzi1*kc1l%fWO
zs6<l+xPCQM+)I*%es+Z~c4J!S#cc-(FYZ4mLdEe2757KNP)X#p732_aNg-S;O4r;P
zuku-FH(upo1ggrc=Jyc&&@^HDCQ{;2?fayJ4Lxit_)Fdf!2^iYzbvUv%%PDlpcSCC
zGwt*j4C(S{!SUB$)%})Z(e?dw$0ml$K5&}IKR~j7q@9H!hs!>yWYb|?{r4nQvh1eO
zWv``4<0Z)jQT(#~M5`kC(~;+bShkjG_qL>3q^O?NZdO_x02Qy{Q>4YD92cia)`I@x
zptyLU%O#On==0jcs!%%35QX|A6&AVtvi)3}h&Zo0t><{5yAi2x9kEdQqlDsWSR=dG
z8FO(|0vR;xj6IOf8X7Gbr78n|(zGFD$=IYah$CvrpdZh;DB@5f&oY#8t;n!BysRNj
zdKTetRd^R$cdo?0DsXAn4@ENI6E??2a|l3xU*E*5;L$J=9BEMrj0E)8OZnAMBn7>Q
z86^WgBgPT7u2kY_GpumyB`#m!c%iJJLt;rqucUSqvj)|(2;Zde`E1=P>Bd(I9F6o`
za|~M;yr6%bwv08FNGdvGYPeA*evRf$$8(HiHo=w#mq*3X51gx2{ZLu@OQLk~VUPZ}
zC{OTVkN#q)OgS>`tiVj^G?QXz%)t*|jbE@9|8<$5<CcP?;%zZ#cfZW0W3<}j?5%j*
zDO<Y#R+V+vwh`((SyJ)(cFP3S_st}>wCj&hL;v0D8@CpJwM<Zb=im`QZf>FV4%GMa
zGMnn_O3uCv_3f7ZsX=6MjoG?OCAL#x;jpY>Kw=H}$dxMRIOXiK<Qz~r$B?t`i!$eK
zfyJ!H*4-{~Pe|P5Q1jas2}dLz`ZYXi!5^1+s@BsMyp9D8c|F>%;a3*?LWxJSuls|<
zuM#->^>=M!>)w>eI)yyY%^H?WcX5}aC2Ob1kXBhEbFNV2B%@v;Z&XOgsPDGOfM%la
zv4-;{Itl|mCg{0OZnofmnZVa?v*4+Ms0bFl&!h9N?>`MQ+HLf6?fU-dh<=r#8+?&A
zW;;7+pPNN%<U*(Xb@pzyu^E11TRaz-#2SMWty4rbY~#_XAaYl5BDE^Xq6O;?3(+63
znX-91Igwh=ZG0Gt*{X$jHHvZZ-v-^L0fvd*#x~BxBEYu!E1Xpgd|du7tyCMRfL%{<
zDm4glDjV8{|JwW?Y;!xaHEXYP8a1kH?rdhZ?GmX*m(9IE?v_Y3%5Z75O4<enPOOoy
z(_BDdl#-(lSb@DsV$~>g(TiA2*lywL{7m~$eiNhXqPx)$wl7NT5s5YED8oFXFeYJ~
zbg0t_V*#bqxy1Nd2x&SGr~|=r+%xURw>FVr)_elVqtmbtf^t)@Bh=Z?!$N8GAw+4u
zn@Sr(ar!6rgDr>98g#Tp)HJkDJ)hWGuEfUf4X_fL|3tvi`SMVs2~u#tfqQcti=^|h
za3YTtMYitH@S7}9BFVIg+WhRvS@!Ii+4ZMQ)O2e8c`PgI0-&%nn8xhc=VhIr<(mve
zB_3;m^a&tADHKrFn8}(wJ1dJQH6$J5O}1i?Wy)l_Z!#svDxp|DYMO5{ag3Y8e3SE#
zIeq~m_{Cra1S)Y8(vqa>3n6_L&f_N9lJJwPr4~Zg<Xmm_bRniNYvuG-Uec6w6i|dd
zH<cnd*-H>bvVD^igr|*^fs-vNbmeqXdb&xGo?yiz0?Fvgtg4*ZQZ7B%YwA3k>w}8e
zQEqWo(R9*zw#l4TnWfmMu+tM6R9LdqGfb)JLaK8XDp#CSnME?^2v#aUSz)dzjjL{z
zMunXzvPlEQg~u1H%}8?Rn?kZ8*{LEHSTmFLr4r7vW+(JoXriH)K;t+vfmEooO%(i3
zw8l9_%<d4rx^ns~Oqb^ZA)Qws7AGN4fm|yBhljaL`3sqNC7dK*mmXel9wXT~P`z-f
zblN4@F0mr&1Jr`pbS*%7S*}(w*_kDrZFxFxTynQ!!b3#5%3Fav(ign~hkLnji=ydH
z-hIv|E|s&=T2m*ygsitpQt-}qfhdvitEEhicjamYlOx#^wMk=KIjxg#D&ef|bXG9g
zMjJgIL4Plu4LYJ~%v#UOFB{mUVO;oZ$@Buq^jy75W44^@OTbeu)D$A6zzPdHwx(VZ
zI|~sXkHE?onl(UGlUUbeY4xX(-j%ZLKwyJLRkB?uiBhXAN#xd0fpy@7zUjnWOC@0|
zVpW9jbruBl@QZ3;ziJhLO^Lvhhz1oU_H`mb(MCksL==npI^i>`r<03pkh;U><aHaZ
z`K0|OKD5Cgn<XJ?wR$?`TqH?Ngi*vQxR53ufzCmS5N)$CVgybi+`UC1XfzY3R)r!2
zT=7i|^EQ@^2~E@fl06=G4TC=eXn7soU7@^itRvjs8|}_(3C6dCBgVE^UQ0)~wL2Ql
zYwitqw&rz2dt%#Tc}t?L_ygJT?Rl5<)VTc~zX}&G$<5CTM_M|2TSE%R{*IzUeZj7t
z&QOfy^~R!RL4C!`jAccOJHwIQzQyg4-n`~;T#|H#oAcURTJnaG;4x3dpCk;v7K}&3
zEumO$M^6u#=8|ZrCEOFrZ9x*2s|v+p!S;|h7->a;?TRWEZw+@B=5{bU{3jmnjOB&}
zJ2F~BZODM_<J)^kF41s$MBxU|G2AVp0YM-*eUa8sUuQTLS7ofN6@SOPBo(33rTClX
z@n{eacY?CDx5eb-8No>0tdvku)({i2O&uoN(bF}X1Fiw2It~`daX_ZofL7<Z_M>(!
z&2Inx4xsJdx5W|v)@84YxP8hl`y|_WT47*l{km1+|Hk^&n7Cis*$9#^XmP|3U$zpm
zciAV$?RTcdfqr$zg+Sk6Z?ymTWA_mb;Vy(1BJWQ-9Csk|dOPtWfu(Vp&9-<71$*fB
z65ZC(Z8zO+pj!joHl$&|0@IT_ckysF9)3mgbK0}055)|W$|F}@-YBy!<l*mkigZ9E
z&wZ9-1CEtEJXc@?Lh5506ytcq(Uy=WaqqGFB<?XOb2Tc=$=_qUWY(U`m5BG)vm@Y?
znhCn&_-o>*!D_nkTCb-Vzd2r`XuECk<n>pyuYWQv&3?;eZK&*vHphjQL?e%rfq!Rf
zbcA-VU+*~7ZY!R={%h^)|BSp(bEZEECY2zjPhUn%uiNhXT3WjO;mdA8V$im$A32}8
zEEkElZMH?)A>@6R=RGF!uBJRFrrG~ubKG+0ofOTuC4)5P;q}yPT1I0A)n=s79^7T0
z^Td7#_}Wg#N|f^j&C!U|OFLKEU)@P6-D%rp&w0W==XJZ+(T3+KHLcDQFx+KjB{lWc
zWk$K%SLrD;eAVS1x8GOdtMmodGotYOs%k4ehCkpA)ccLveCG8BjM^H1nWvoR*ZY0d
z6-J$>qQ25y=kvP*zM5**8H~l5VQh`Z4Gf80LXg*29x%#00Z(0(ubR`zQkk)#dbu1l
z3ziS-uR*`3ZjC?RU`DK?I~s59ZR7HM)eY`SUzy>qtEjK?R0sUb@YNWd-7Q;;I47vC
zF}xnX-{-D2{I#A^U%AgyCNxQ|QUOP<bJtZ<O<)X9xvv_e7Ohs8!%jk(4b^Lu(#!bp
z*jJE`aV8QAgu6m9Pwkp&zmeec_?H;r*4j0#xV4618=5-em`!>k+rp7nBN~eJc7=Eq
zP(dRUY2{k2tu@@0bsl%wdNhqM;6si5P^~Q*3RQ)=x})1UmA9s<CQwtm9<Ai5RYs-8
ztFLyKpz#7VXdri)QR1mC^;Ws-)^IL&1KOPDuk!`GMy0R1zR{?ut`zO=sjI80vsi<~
z`CywyXu$nOl>;-GhDg-6pr}0*ukP(?4n^J3XmGoywl>U5^{;c+ayceHT5Vf291oe5
zPH63}c2}<VdyKWUbv39+6>}Sr$fc~cySh7Gp0Gh1Ea;2GLQ&E1o^`$y#cS())qzS+
zgQwCc^}4GosCC;qgY7YEBNyaD<<eTr0$m}`R<11BT<U2A)-t0j7~K+z#&{l@#$Que
zPySB5qb>lU{`%5VxLkRCB@7;@ud8M|g==t*I*%Vl6g`b2MAuUd>@E%X8a$G+s;0qX
z?U@|ysrJ{`c`AKMrPB4KXtfHShn`$oQ|%AbxlvU`kldZALpFCBJ@KePlP+g7y-IXQ
zRS}EtOXpq@Y;6s6mxY7vo>o|`HCQW5R9zD=){}40FhTtgX5ixn>R)RlRUt8Q7}fQa
zmHt|HsfQZCbRSvN(w-=0YxJNNQ$<ywlyRnwQ?@aqUi6|m*^7qDT+!Ow(;3DJZltEI
zujYDs%Bs<Ltfjk0bSpe~VN}(_+e<t&;`suyb-XnJZYMN*O>OP4QB$ss{9O6eCW9A|
zbu3LaLi5+d<&$e_l%q3N)-t1~TX-J^H&F2YUcJs=m*mHVehj*?kQ|hXTB0FTn_3Ph
zXWj5@OXDUwEVZxbubwcvSvbnaqF8WiqF>h327FcS%7ojO)6`)VSFBcJMd3&oEr=no
zV5xt5S67IZk3u1|Yb#~Q#`gG&4D)|oxVDwGb#@2iEVeD!BlL+uownZ2PHwoGk^mf>
z2I_S+b!!l&bSbik-?u?@qmlXqmqI2ph!Z1YO|^&7Sk;Yr%Ib{66fbTLV{sb_Mu&~<
zVH($ISf{2<nNl%WSw^bvo=`Lx?~XcKL!F^`$hmQ|81jQHTY7tp=3q?pO4+gbY+0&$
z6a<sLVwiyPyfj6Lv8AT6fyTArzQ2sdweC(2)J&d}Fcq<$j_on@i_Xqaq&?mtYEX$`
z(i}_tvP6b<EDK?jMKf};`r0ygz*FT8)cG*zsd3F{3qw@5)hiNCR!|tlthF^{I@*Y?
zD4a)HQ_g;6SXy8P5MxVXg5Z_L^e|Gji^yr@KyIxLjaZ2VfnkuVhJ&5qOG8GFsF*ox
zuB-C}JVrT|Ml2ko9>nwOJTy8dO=2<P#*(L^vZe$*#EA7a8$QEpTr7sLMDYfdE=PJA
z59_PfRM)Jlwwg<Jj3S!kskdT&L=SEi-l2SO^cFDOei($8qg=wkFp2GITnuX#>$w|f
zZbdV?nK?e9nYnPPHRWK|C-pJwV{lKgPfe|-TA0U(g!<xqZMeZxSHl-5tjgVpArez>
zg;D9Lt_XNJ_ps43dGwW|r)cO@msd_!(%sz|3PxyW$Xcmc45O_}cqIl#K5O$e4TkCJ
zGIw1W4OtDII$pkqw{D_EOwZ-B7A;aqVL3~W=$}hlP<T{&7F>bolO{U1u{GGqjo~YU
z(P$}c?Ut7Qke^t(VAv%Mgj?0r2aK9>AyrHy<-SIlRC7V{0!tJ}i&rv+m=}$fjxAWU
zbhn0>m#buKh-?$Bo9rP}Fjf*CcU>hdG|N0_SJIq2pIG!T_+TfwxN=7<Be=rmqSA1e
z)bLf1wU2WWHN^?jmoYVrM8kc|91$a-ZPEZ@U0;q~QloYhMtP-MtaD8FkvkTQh}7P!
zqPpH$dd@lGl~m{Au6S=Gw5mN631Q+vG8k>?ScL<R#mkDEi`$)xYnA{Cor__A8q^oJ
z^+sCaVH_0z6^mmyUfdJyj)z(>R&%W^vyB+{l!u5mODq$Umt2uh2&*Q`1VH3!S3-uy
zRs%x}Gq%E<xhXRxFPJ8@GGCnsi$g3lWu;5KHI*L!nq;?9t0!8&e!%{N=H&uQCxQaz
zD%_lecsEKdko1>hmQhk)!TI<ifwTSnPPP6K2V{BHQ!7i)mWC@UlwF!Cj-3{bqG|-S
zOsXr?8mFXSIl5qNG~5;J^mljmlKXftqoR`qyJ$&K*S(FI#7QH7ERiN=5RE)Blch&&
ze7S@avru9jvpS?)8jiet#RysEb63=0Z)KQ!Ko+4@B$bIVH)(3)9;;To(j5|0jI<Ts
zm8!11iVpJ1u?(=zYL-E-z_PaUgu`SiEKhjh(9P3`69R0htRuGE_LSA}^)`#~^(`ju
zxZLWi7UQF^pSELkSW&&+mt=Kwby~0#MV9rZG#id8S_?3EA}AEW5)l-MU@6y7?2CQX
zmK72<r)Bk}0jh^pC`Qb9Z#2>n?Ch1RI_sLM&LgJiQa7z?urmubcZQ7awzgOZgRIrx
z%(I7rB`Bd1M=NsqBsO4jWt4CPb3!YjePE?utc8=$ajFUv1Fu+$TH8u>h<tR2ovDM;
zPE#LlkHqkw*!&7ZrEFHKYq(o6>*j<Lax_0xz|PfE=f_Z|e3@2Aeoqx<QOs(ILn5hP
zL7{bRZRxxwJ3lQ@Zn<y~FHZCuqr9%B%CeTWT3m^z^8j>D+8L0WChw!LAjAm_jyD)z
z4J0+060j_F_?J3j4d|ss1Lm%>n#7DshdzAb=6WXg{SnX2_z=PO*_P=^bWtt>Ck5ud
zKn*8iB{uRo96w)kqFZrperRG~cB)G{tRq2UwPRVz?i_Y%$2z+s?Oc}Cw24umSlLgm
zd(@<;)(YnSmwJbRDH#tCCzk3+bw>>R++ehZaJr9EeM;wZy%QR%(+pBM;bo;Yb#?W%
zYFEW)xfIt`n<}bck^wP1$rXMTP72%=9s>)t+WG+BA)vbj>gxGs(&xuPB|rD#8u2|6
z*24S*)Y3j!E7YW%RAFC-V-t6MBTiu5b?Y$&N&1nk$cKbPY33;#_{?QB)_&N{lMNCR
znioIVgyjO7a>{%SM#*}Euia^oNo)oac0*Nb%EbsfqPJU5KP^X{bQatj3$@~mF4V^&
z{78^%<Hj~q4%l)-M0+u;S)*c`B}|bR*OVnlU*#>-LjGE@Sx}22S~ych6cLnd?Zj!7
zxx?Y5Q}?2S0*u1d{BVcP-uUq`&!)2u4A+<qFT@EDe2{i{mQyoz5b9|x^?1rK(PFP7
zH#yw#dn2)MdnDA#dw_X##*FUHRyD}euU!$|`?>pA8+h2+1BN<veueQE!w79!aV~=W
zr5fIxSU}Tx0-8@AVxr;sxrsblAF0unip8R(xkO({tY_7FJBd5djaQi70;MQ!PEX7<
zK}$za_+QE#gvmChPV-Z;mXItt_FC4awCA9~0DF-x))tQ9R|e8I<&aITriTwnVd|Eg
zBe4hO2jjc}NRsTSDSHq8+X&|?_pI~OsgpOXFyzr&>Ir5MZAvk)p@YkxQOw;{VsC}^
z@9vFLMfr@%5<edBqLks4+neWe)Dr5KheSgZ(Xplap@dvaE{)dK)|6aGQ!4K&G`jqa
zmNd_0pp=Lw5zBdvmc3$9<cgY$Nh-m*9wdW~INPc!iTTF#6E*Rax~s*=D}Ph+80I#b
zL=Ml!NivoV=31L|;n!X|t}>SzWMp@9jKw>`{K(&IuMvK3YT*ZEbTmeV@j=L6&rKMp
z39PThaZth?Frcda9*&5Jo)4GiZ#Ec-$~=DT-f#lJ*Q7WN?8Q1ydZ_$uz|x*-<*+p6
zCk}Kj>-dmp_G0Be@HRw~cOwjgDVak2)=|iKX9u!3YP4X((iLIy$GA%D{;|~Zh!gyz
zqNGO4#Q7KWpjy&s$InpFdrH0XR4y@7s6+EQPfcB!r;a9YIOMV-IzH%)&}4uGHa48}
zOP9Iblm{szkN0&oFrVdIM*RrCcZLhkYS7af^(v~~#!ADdChPa}T+8jSd{X$=GSpbD
zioPjPcGh$Gew7$6pPmJsQ$`CCC>5=cD9)-9MuN_w0FNpyCYmHcCA}qqO65`ypoitt
z-FAleD%4Zj%T4WpOyyM1j(;`5c~!T1!206}&Z~xT`lq<Kuyrzo9aQv$si|MRJ2X)~
z%d0WJdJMQcfl^NZHziQbGJ0_Wm8yG_EV(tIfr>Jdt3hd?o+j4#A09}O)g=X7k`Sb#
zHdD~1;es|LNkQULDPU(pkcy_6f;JBqv^iDK2`Run)y1=5ToqN^XyYRhWlL2#j!YG%
z-oLPL4w;<F&fpx1q;l5E;{6FVR5V{YjK&NVwa7m{lKkVt<g1r7TBLj^acmgT5t*4`
zTJR!OZKb@%E=pBhDYeqnvsOhNrXXXuAR|@KZL%ruPqe9u{=`Jpsp#Kqh*VJ+6pHt!
z3bV6l@%~hCW2mt+&MY1_YN>uzXx1s9qN;8q`h<E}PZ8T$ym-G#D9%w!Rd2;9BYz|(
zP{I=K<G6djQbDz_odKEJG>UV3!aY^=!#PW;AT-Rv{i&ktZ0Eue^a3(fFZEjB-^Dyi
z*oQZd;#3vEAflol&OA!c4T)|#d_GW>P3VcfJiHl4=-|USPu2lO;M_Pu6FD{xYa+#|
z#ssvubH7Tc0m9BMca9(q$kY~BuT`nak7ee&j-~+h{){zmG&x7*sFlgg_a9P8)tgP0
zBGoI9!_-r>Psj=VZ-`Y35ViK1#(LP2c`-H@uW3?j2anC#ukzJ02y~a5bciXsRJGL$
zmKL?uJCnoIR<vp{^f%O2izKz~5^8TX)xJ!st@70pOsFmCpthn*Ra?EHX;E9fbvaCJ
zMXMHUe?#p;IfE>hVYv*|t68czHLaworC!CnQu3*{z5i9F)r|97nI4j%T3;ynXUOSe
zt_*W!_(A3Elk)b<@NOBZN$nAtJ|x3bd83uDrl(YTHBj%7Bhi2i)fyp{UX5J+DfHLM
zv>MUW+t*)~_=7V1jtn1_VXFL-<@??!{OY~a*JQqW_c~qn|FJU6k>P9^&XM5)87`J#
zs($Ka?Pq2FFJ$-|8NMV#^}2Q{zj`72TXNvIUxtG+d{l;N)JWx5&q?>m-I&^)sjZlL
zHu@~-AL`2zqxn0_jcxD34<ZQTQ4w~i7!C<S(H@o?!LPHq?h@bPIDY;YDLmz#%b&<!
z9ByT~l<Ek^I#_P&_6X#O5C=#+XDc4#!;x`P!a!a$)EOiO8SCkcvs`}GmK&!B9CGn{
zaTs~st-*MZ<%T+pwrH>`WOTHGSEV4drNs#KwS;=`0~P(QBV-ssdJ<B|Q1Odfz=-mK
zUHCpmZhJS1z%7@TE6Qk&#aM1jH=d!wxqNPGsJXWt%)v-Ip7@u^FrMpD@nCavG_+Nv
z=(lYZhXqv>PsysNl$6?}|E2UyaM8OHcq4#c#Wt4=dBDGWE@I+NwASlPB<O<$ia%g;
zA%#~_B*Mv4i5uP?NV*D;vhv4mE}kEk{PTD!aZBYdL)Kd-PIRmC_t{*Wc%MYHN`>)$
z0>9Mz{xZq`9uZzgG4*F@E|Db*Iw%?OUMRPd;?EIOe9Ma0AWZ&)nJ%7l5bvbYwL}#x
zl3a$2@_@ESCoKBElI`O7SMZV|U6+c0-ml=7s;3IK0Q<g$zhi=n({;$tiuCgo-e^Qf
zdMf=Tn#-rEDtSYae@OBVN<vk>;#c+GgM2IhZMutd+@=dg`uieu5kGykCzbz`z!3@Q
zM_11zmkfEp|4xpGiMwnT6GboXTKTV&{MX?fSh`%Ye38N@v}8nupF!Hnug1N@cq59g
zLUK;arT7%;^N3ja(`LGORvL9IT+$6h0{2w?4**B}%KlEt@09%X_gt)()0#m1Hh`7?
z@GKXnI6R97tWpH#J+=IMkYnX{&USHH=WM}3-z&FXs{Ish5D_c?-KV>F7QI(b*D4X1
z_Y`sXC^FuO1(0vj^~B;P8S=nvFOf?4k4pav5pq?<|D5E1Zn2<9)i*W$GRR0L#eYQd
zA2~_>KY>^EEh=$H@()RVT{f)pZ&kkXqvPPQmVZ32iNvtuc|0)ti%2EvujKp{8CHIl
z-y}%$MR1o0%)8=KVFsA3{DYEzQ1X9frOC=8&g3K;2WFC`?AO%Q#3=$@f{;GVV!fQ!
z1mY8A`LZ}OSMi@?Vnl>=rAvh~B)`BW?w8B*n-VA<Q+x`&XcYcmOa5JR6{QGM`RQ*-
zrQ(P(c%7jCg&H_Tau_ZBeW{W7SKTNW-bv}OskHRBrwEs7zhz$#{NIt|fGplDOUc<F
z`BlCOU;nm<eI0LpS}(<?aP;@AQgM`b;9)_LuN*@%q|)MLe$#g(2@eiGCQ|e_v8-20
zl9#Q<PAlCLhehhR)PFvbD*(#x)%->7nW7?R9Q-*C*h1BDMRLTt%B*359n{4(*9dqU
z)<+}Zao9H^K8@`g5fA4|8%a(E{^Y_)_%TeaFPP=R5gt?P1`8T?$z*Chkc=P8)I6Vz
z&obxlWc)a$=H+Dkc=MZj$@pxh=Fw#Q1g7T4WW3JQyqAoh$kcq6jGx5RJe7=}Y|cN)
z_$f@yE6Mn&OwAX`_-W=mkc>YK<9d@7#p8AcZ&c&9CD+cTGc_Jt&}f$#OpUY2c)4h>
zA`!O_Tg3|kJO68rDa4#!PTnb5cK+8OQ;?kAIULFHb4J0Z462}_ex1f%JCVvT9ta!$
za+O+_t&;>KS%V$aM@Uq^UJm?7a!NS;@b;<!p5)&(-Gx}<YDJXrM^f<HB;Gm0l^{m!
zS^(+eh&OS(8mDgN_~HKZ2=FA|l_KXyoPM|;j#jT%IsHU-mt1!t$*=ct*YMXTNAk~+
z?BjsK+6aDlI>%3EjdEU>;@Q~}|3r%4(%)3X2TF$Z7q7&t^_`MKe<M+2!}|;E+ngi#
zKmD!5iNLE5<s6XnYZUl7)_x%;g--%M33g_3U9IMdTO>a9JmXd_X97DS>2;|neWYA&
zDU|9wd=_|T5=JCm8U_E(D00R@v619EIez$fxL_1|`rC=z4&6;$kz%PE{V|=9<X<%k
z{yE^u&gyHAKS7$VUm|q!Sn};1PM;W`)w0Eb$(8gukfP5#;75{k0r2=t{jhOw4e)c(
zj{UN~jFZJTOZi8n{6>lYjKsTonmC63b|qck1b!qt(4TT4JzdczPDg(?ldj*5LO%`_
zB)_^&>Jt*=j6UMc?Y6#&W9a+ybj<;t^jGI@YP>4uas-X_9+G(1WS14q_DK0hTADas
zwaWnTI`j{u^wZBudiAv_)nE2;{0yc(B}MH`7kznjB>UVY<*3hH9h@TQ?*X35W!str
zLf-F}^y(vBE@{s};OAh+c1-p!Ro1h>tGHFrF)2rV$E#oJ`If{F$?-?o^F1lwDfhwq
zq_9aCC#YT2N3v9XrvX1wx%iq4_rp|s;wvyC(nm+Z-y-n`QrhvsQRtr;1^+VeBl(Yp
z^(oa$ePT?tuU+Dgq}V5u<I!Gno>F$34Ltd?I_IrGd34R?_z~81NiSxM@D82j38~8E
z^5p^#9>|X4(bBfI+!j1wbYUr8kfn#14AvTohT8FokPzN;?lM~NJSL7xM?e`yYq!zf
z+1-p6n_BUrPs|AR_Tlwk`kG2S)SA0&#d3NBU<4w(<QNHyBV4?-9>;6=;uyHKx2tPA
zSS%_2%yJT`foIR%RUSOGSO#T<<}xm;UQ=F<=gy1(e>oo%fvQrIx;55qbkKKJ@DN$D
zY`l45Mc|QXJnU$ct*>@h`AQibCevfoz~LKl#)d$H`4C}20iKmp?_}X2SOXtlX)#)2
zy}X)uNK;7KfCsSDbBFS&Fe0|RYvyuQRQgIvjl$d|_-K@2w0Gg5Y|c#;y>P2hC)=eI
z4+Y22DEw6!$<^3sa4!C^ZVHZ{x=X!;JbJ`0QCwl}QmRtG)UTk>`uHEzOgu7)EX(WB
zr3HAjzc%bIF!-B0rU3I@k&p5wGk^aRPtaH4DRt$4XiIX)0RLHl9)jbK0UPvCTuK#E
zT7|@nEX1(1t)-%}jmqdP4&L4c`G(Qb*B5LKZ!IWTjySaf{}cfH25*MDT&0bT_({bm
z$c5i0dC2JIG!0c&pvqBBQc$3r3NQNotu`pMwn4)C<TK9nDte;%RITV`EuhUOIY0Qd
zEYv+{^z=qU#@28&j<<i3>dT*{#%DQT5#jInydr%kQGDtk*&oIGo&0+h{MmZg%5Qp3
zqH83SPWiNicp1~n-%d%gv&l=}VzgA+G%|Nr{`Q{b34WM1#D6QY+QVcS`DG`IZb|*2
zV1&;F80~$123}l`bw}`;bbPz9H9yI8{H@Yt3nhP$0ePjR=-v6&I;n;vw`rFj_7IOm
OC--aqFfSk2*#86JBkkq@

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/doc/DoxygenLayout.xml b/3rdparty/qpOASES/qpOASES-3.0/doc/DoxygenLayout.xml
new file mode 100644
index 00000000000..927d94d2e54
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/doc/DoxygenLayout.xml
@@ -0,0 +1,189 @@
+<doxygenlayout version="1.0">
+  <!-- Navigation index tabs for HTML output -->
+  <navindex>
+    <tab type="mainpage" visible="yes" title=""/>
+    <tab type="pages" visible="yes" title="" intro=""/>
+    <tab type="modules" visible="yes" title="" intro=""/>
+    <tab type="namespaces" visible="yes" title="">
+      <tab type="namespacelist" visible="yes" title="" intro=""/>
+      <tab type="namespacemembers" visible="yes" title="" intro=""/>
+    </tab>
+    <tab type="classes" visible="yes" title="">
+      <tab type="classlist" visible="yes" title="" intro=""/>
+      <tab type="classindex" visible="$ALPHABETICAL_INDEX" title=""/> 
+      <tab type="hierarchy" visible="yes" title="" intro=""/>
+      <tab type="classmembers" visible="yes" title="" intro=""/>
+    </tab>
+    <tab type="files" visible="yes" title="">
+      <tab type="filelist" visible="yes" title="" intro=""/>
+      <tab type="globals" visible="yes" title="" intro=""/>
+    </tab>
+    <tab type="dirs" visible="yes" title="" intro=""/>
+    <tab type="examples" visible="yes" title="" intro=""/>
+	<tab type="user" visible="yes" url="../manual.pdf" title="Manual" intro=""/>
+  </navindex>
+
+  <!-- Layout definition for a class page -->
+  <class>
+    <briefdescription visible="yes"/>
+    <includes visible="$SHOW_INCLUDE_FILES"/>
+    <inheritancegraph visible="$CLASS_GRAPH"/>
+    <collaborationgraph visible="$COLLABORATION_GRAPH"/>
+    <allmemberslink visible="yes"/>
+    <memberdecl>
+      <nestedclasses visible="yes" title=""/>
+      <publictypes title=""/>
+      <publicslots title=""/>
+      <signals title=""/>
+      <publicmethods title=""/>
+      <publicstaticmethods title=""/>
+      <publicattributes title=""/>
+      <publicstaticattributes title=""/>
+      <protectedtypes title=""/>
+      <protectedslots title=""/>
+      <protectedmethods title=""/>
+      <protectedstaticmethods title=""/>
+      <protectedattributes title=""/>
+      <protectedstaticattributes title=""/>
+      <packagetypes title=""/>
+      <packagemethods title=""/>
+      <packagestaticmethods title=""/>
+      <packageattributes title=""/>
+      <packagestaticattributes title=""/>
+      <properties title=""/>
+      <events title=""/>
+      <privatetypes title=""/>
+      <privateslots title=""/>
+      <privatemethods title=""/>
+      <privatestaticmethods title=""/>
+      <privateattributes title=""/>
+      <privatestaticattributes title=""/>
+      <friends title=""/>
+      <related title="" subtitle=""/>
+      <membergroups visible="yes"/>
+    </memberdecl>
+    <detaileddescription title=""/>
+    <memberdef>
+      <inlineclasses title=""/>
+      <typedefs title=""/>
+      <enums title=""/>
+      <constructors title=""/>
+      <functions title=""/>
+      <related title=""/>
+      <variables title=""/>
+      <properties title=""/>
+      <events title=""/>
+    </memberdef>
+    <usedfiles visible="$SHOW_USED_FILES"/>
+    <authorsection visible="yes"/>
+  </class>
+
+  <!-- Layout definition for a namespace page -->
+  <namespace>
+    <briefdescription visible="yes"/>
+    <memberdecl>
+      <nestednamespaces visible="yes" title=""/>
+      <classes visible="yes" title=""/>
+      <typedefs title=""/>
+      <enums title=""/>
+      <functions title=""/>
+      <variables title=""/>
+      <membergroups visible="yes"/>
+    </memberdecl>
+    <detaileddescription title=""/>
+    <memberdef>
+      <inlineclasses title=""/>
+      <typedefs title=""/>
+      <enums title=""/>
+      <functions title=""/>
+      <variables title=""/>
+    </memberdef>
+    <authorsection visible="yes"/>
+  </namespace>
+
+  <!-- Layout definition for a file page -->
+  <file>
+    <briefdescription visible="yes"/>
+    <includes visible="$SHOW_INCLUDE_FILES"/>
+    <includegraph visible="$INCLUDE_GRAPH"/>
+    <includedbygraph visible="$INCLUDED_BY_GRAPH"/>
+    <sourcelink visible="yes"/>
+    <memberdecl>
+      <classes visible="yes" title=""/>
+      <namespaces visible="yes" title=""/>
+      <defines title=""/>
+      <typedefs title=""/>
+      <enums title=""/>
+      <functions title=""/>
+      <variables title=""/>
+      <membergroups visible="yes"/>
+    </memberdecl>
+    <detaileddescription title=""/>
+    <memberdef>
+      <inlineclasses title=""/>
+      <defines title=""/>
+      <typedefs title=""/>
+      <enums title=""/>
+      <functions title=""/>
+      <variables title=""/>
+    </memberdef>
+    <authorsection/>
+  </file>
+
+  <!-- Layout definition for a group page -->
+  <group>
+    <briefdescription visible="yes"/>
+    <groupgraph visible="$GROUP_GRAPHS"/>
+    <memberdecl>
+      <classes visible="yes" title=""/>
+      <namespaces visible="yes" title=""/>
+      <dirs visible="yes" title=""/>
+      <nestedgroups visible="yes" title=""/>
+      <files visible="yes" title=""/>
+      <defines title=""/>
+      <typedefs title=""/>
+      <enums title=""/>
+      <enumvalues title=""/>
+      <functions title=""/>
+      <variables title=""/>
+      <signals title=""/>
+      <publicslots title=""/>
+      <protectedslots title=""/>
+      <privateslots title=""/>
+      <events title=""/>
+      <properties title=""/>
+      <friends title=""/>
+      <membergroups visible="yes"/>
+    </memberdecl>
+    <detaileddescription title=""/>
+    <memberdef>
+      <pagedocs/>
+      <inlineclasses title=""/>
+      <defines title=""/>
+      <typedefs title=""/>
+      <enums title=""/>
+      <enumvalues title=""/>
+      <functions title=""/>
+      <variables title=""/>
+      <signals title=""/>
+      <publicslots title=""/>
+      <protectedslots title=""/>
+      <privateslots title=""/>
+      <events title=""/>
+      <properties title=""/>
+      <friends title=""/>
+    </memberdef>
+    <authorsection visible="yes"/>
+  </group>
+
+  <!-- Layout definition for a directory page -->
+  <directory>
+    <briefdescription visible="yes"/>
+    <directorygraph visible="yes"/>
+    <memberdecl>
+      <dirs visible="yes"/>
+      <files visible="yes"/>
+    </memberdecl>
+    <detaileddescription title=""/>
+  </directory>
+</doxygenlayout>
diff --git a/3rdparty/qpOASES/qpOASES-3.0/doc/doxygen.config b/3rdparty/qpOASES/qpOASES-3.0/doc/doxygen.config
new file mode 100644
index 00000000000..6433bfd0933
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/doc/doxygen.config
@@ -0,0 +1,1123 @@
+# Doxyfile 1.3.8
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project
+#
+# All text after a hash (#) is considered a comment and will be ignored
+# The format is:
+#       TAG = value [value, ...]
+# For lists items can also be appended using:
+#       TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (" ")
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded 
+# by quotes) that should identify the project.
+
+PROJECT_NAME           = qpOASES
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. 
+# This could be handy for archiving the generated documentation or 
+# if some version control system is used.
+
+PROJECT_NUMBER         = 3.0.1
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) 
+# base path where the generated documentation will be put. 
+# If a relative path is entered, it will be relative to the location 
+# where doxygen was started. If left blank the current directory will be used.
+
+OUTPUT_DIRECTORY       = 
+
+# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create 
+# 4096 sub-directories (in 2 levels) under the output directory of each output 
+# format and will distribute the generated files over these directories. 
+# Enabling this option can be useful when feeding doxygen a huge amount of source 
+# files, where putting all generated files in the same directory would otherwise 
+# cause performance problems for the file system.
+
+CREATE_SUBDIRS         = NO
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all 
+# documentation generated by doxygen is written. Doxygen will use this 
+# information to generate all constant output in the proper language. 
+# The default language is English, other supported languages are: 
+# Brazilian, Catalan, Chinese, Chinese-Traditional, Croatian, Czech, Danish, 
+# Dutch, Finnish, French, German, Greek, Hungarian, Italian, Japanese, 
+# Japanese-en (Japanese with English messages), Korean, Korean-en, Norwegian, 
+# Polish, Portuguese, Romanian, Russian, Serbian, Slovak, Slovene, Spanish, 
+# Swedish, and Ukrainian.
+
+OUTPUT_LANGUAGE        = English
+
+# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will 
+# include brief member descriptions after the members that are listed in 
+# the file and class documentation (similar to JavaDoc). 
+# Set to NO to disable this.
+
+BRIEF_MEMBER_DESC      = YES
+
+# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend 
+# the brief description of a member or function before the detailed description. 
+# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the 
+# brief descriptions will be completely suppressed.
+
+REPEAT_BRIEF           = NO
+
+# This tag implements a quasi-intelligent brief description abbreviator 
+# that is used to form the text in various listings. Each string 
+# in this list, if found as the leading text of the brief description, will be 
+# stripped from the text and the result after processing the whole list, is used 
+# as the annotated text. Otherwise, the brief description is used as-is. If left 
+# blank, the following values are used ("$name" is automatically replaced with the 
+# name of the entity): "The $name class" "The $name widget" "The $name file" 
+# "is" "provides" "specifies" "contains" "represents" "a" "an" "the"
+
+ABBREVIATE_BRIEF       = 
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then 
+# Doxygen will generate a detailed section even if there is only a brief 
+# description.
+
+ALWAYS_DETAILED_SEC    = NO
+
+# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all inherited 
+# members of a class in the documentation of that class as if those members were 
+# ordinary class members. Constructors, destructors and assignment operators of 
+# the base classes will not be shown.
+
+INLINE_INHERITED_MEMB  = YES
+
+# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full 
+# path before files name in the file list and in the header files. If set 
+# to NO the shortest path that makes the file name unique will be used.
+
+FULL_PATH_NAMES        = YES
+
+# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag 
+# can be used to strip a user-defined part of the path. Stripping is 
+# only done if one of the specified strings matches the left-hand part of 
+# the path. The tag can be used to show relative paths in the file list. 
+# If left blank the directory from which doxygen is run is used as the 
+# path to strip.
+
+STRIP_FROM_PATH        = ..
+
+# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of 
+# the path mentioned in the documentation of a class, which tells 
+# the reader which header file to include in order to use a class. 
+# If left blank only the name of the header file containing the class 
+# definition is used. Otherwise one should specify the include paths that 
+# are normally passed to the compiler using the -I flag.
+
+STRIP_FROM_INC_PATH    = 
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter 
+# (but less readable) file names. This can be useful is your file systems 
+# doesn't support long names like on DOS, Mac, or CD-ROM.
+
+SHORT_NAMES            = NO
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen 
+# will interpret the first line (until the first dot) of a JavaDoc-style 
+# comment as the brief description. If set to NO, the JavaDoc 
+# comments will behave just like the Qt-style comments (thus requiring an 
+# explicit @brief command for a brief description.
+
+JAVADOC_AUTOBRIEF      = NO
+
+# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen 
+# treat a multi-line C++ special comment block (i.e. a block of //! or /// 
+# comments) as a brief description. This used to be the default behaviour. 
+# The new default is to treat a multi-line C++ comment block as a detailed 
+# description. Set this tag to YES if you prefer the old behaviour instead.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented 
+# member inherits the documentation from any documented member that it 
+# re-implements.
+
+INHERIT_DOCS           = YES
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC 
+# tag is set to YES, then doxygen will reuse the documentation of the first 
+# member in the group (if any) for the other members of the group. By default 
+# all members of a group must be documented explicitly.
+
+DISTRIBUTE_GROUP_DOC   = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. 
+# Doxygen uses this value to replace tabs by spaces in code fragments.
+
+TAB_SIZE               = 4
+
+# This tag can be used to specify a number of aliases that acts 
+# as commands in the documentation. An alias has the form "name=value". 
+# For example adding "sideeffect=\par Side Effects:\n" will allow you to 
+# put the command \sideeffect (or @sideeffect) in the documentation, which 
+# will result in a user-defined paragraph with heading "Side Effects:". 
+# You can put \n's in the value part of an alias to insert newlines.
+
+ALIASES                = 
+
+# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources 
+# only. Doxygen will then generate output that is more tailored for C. 
+# For instance, some of the names that are used will be different. The list 
+# of all members will be omitted, etc.
+
+OPTIMIZE_OUTPUT_FOR_C  = NO
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java sources 
+# only. Doxygen will then generate output that is more tailored for Java. 
+# For instance, namespaces will be presented as packages, qualified scopes 
+# will look different, etc.
+
+OPTIMIZE_OUTPUT_JAVA   = NO
+
+# Set the SUBGROUPING tag to YES (the default) to allow class member groups of 
+# the same type (for instance a group of public functions) to be put as a 
+# subgroup of that type (e.g. under the Public Functions section). Set it to 
+# NO to prevent subgrouping. Alternatively, this can be done per class using 
+# the \nosubgrouping command.
+
+SUBGROUPING            = YES
+
+LAYOUT_FILE            = DoxygenLayout.xml
+
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+
+# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in 
+# documentation are documented, even if no documentation was available. 
+# Private class members and static file members will be hidden unless 
+# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES
+
+EXTRACT_ALL            = NO
+
+# If the EXTRACT_PRIVATE tag is set to YES all private members of a class 
+# will be included in the documentation.
+
+EXTRACT_PRIVATE        = YES
+
+# If the EXTRACT_STATIC tag is set to YES all static members of a file 
+# will be included in the documentation.
+
+EXTRACT_STATIC         = NO
+
+# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) 
+# defined locally in source files will be included in the documentation. 
+# If set to NO only classes defined in header files are included.
+
+EXTRACT_LOCAL_CLASSES  = YES
+
+# This flag is only useful for Objective-C code. When set to YES local 
+# methods, which are defined in the implementation section but not in 
+# the interface are included in the documentation. 
+# If set to NO (the default) only methods in the interface are included.
+
+EXTRACT_LOCAL_METHODS  = NO
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all 
+# undocumented members of documented classes, files or namespaces. 
+# If set to NO (the default) these members will be included in the 
+# various overviews, but no documentation section is generated. 
+# This option has no effect if EXTRACT_ALL is enabled.
+
+HIDE_UNDOC_MEMBERS     = NO
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all 
+# undocumented classes that are normally visible in the class hierarchy. 
+# If set to NO (the default) these classes will be included in the various 
+# overviews. This option has no effect if EXTRACT_ALL is enabled.
+
+HIDE_UNDOC_CLASSES     = NO
+
+# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all 
+# friend (class|struct|union) declarations. 
+# If set to NO (the default) these declarations will be included in the 
+# documentation.
+
+HIDE_FRIEND_COMPOUNDS  = NO
+
+# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any 
+# documentation blocks found inside the body of a function. 
+# If set to NO (the default) these blocks will be appended to the 
+# function's detailed documentation block.
+
+HIDE_IN_BODY_DOCS      = NO
+
+# The INTERNAL_DOCS tag determines if documentation 
+# that is typed after a \internal command is included. If the tag is set 
+# to NO (the default) then the documentation will be excluded. 
+# Set it to YES to include the internal documentation.
+
+INTERNAL_DOCS          = NO
+
+# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate 
+# file names in lower-case letters. If set to YES upper-case letters are also 
+# allowed. This is useful if you have classes or files whose names only differ 
+# in case and if your file system supports case sensitive file names. Windows 
+# and Mac users are advised to set this option to NO.
+
+CASE_SENSE_NAMES       = YES
+
+# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen 
+# will show members with their full class and namespace scopes in the 
+# documentation. If set to YES the scope will be hidden.
+
+HIDE_SCOPE_NAMES       = NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen 
+# will put a list of the files that are included by a file in the documentation 
+# of that file.
+
+SHOW_INCLUDE_FILES     = YES
+
+# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] 
+# is inserted in the documentation for inline members.
+
+INLINE_INFO            = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen 
+# will sort the (detailed) documentation of file and class members 
+# alphabetically by member name. If set to NO the members will appear in 
+# declaration order.
+
+SORT_MEMBER_DOCS       = YES
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the 
+# brief documentation of file, namespace and class members alphabetically 
+# by member name. If set to NO (the default) the members will appear in 
+# declaration order.
+
+SORT_BRIEF_DOCS        = NO
+
+# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be 
+# sorted by fully-qualified names, including namespaces. If set to 
+# NO (the default), the class list will be sorted only by class name, 
+# not including the namespace part. 
+# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
+# Note: This option applies only to the class list, not to the 
+# alphabetical list.
+
+SORT_BY_SCOPE_NAME     = NO
+
+# The GENERATE_TODOLIST tag can be used to enable (YES) or 
+# disable (NO) the todo list. This list is created by putting \todo 
+# commands in the documentation.
+
+GENERATE_TODOLIST      = NO
+
+# The GENERATE_TESTLIST tag can be used to enable (YES) or 
+# disable (NO) the test list. This list is created by putting \test 
+# commands in the documentation.
+
+GENERATE_TESTLIST      = NO
+
+# The GENERATE_BUGLIST tag can be used to enable (YES) or 
+# disable (NO) the bug list. This list is created by putting \bug 
+# commands in the documentation.
+
+GENERATE_BUGLIST       = NO
+
+# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or 
+# disable (NO) the deprecated list. This list is created by putting 
+# \deprecated commands in the documentation.
+
+GENERATE_DEPRECATEDLIST= YES
+
+# The ENABLED_SECTIONS tag can be used to enable conditional 
+# documentation sections, marked by \if sectionname ... \endif.
+
+ENABLED_SECTIONS       = 
+
+# The MAX_INITIALIZER_LINES tag determines the maximum number of lines 
+# the initial value of a variable or define consists of for it to appear in 
+# the documentation. If the initializer consists of more lines than specified 
+# here it will be hidden. Use a value of 0 to hide initializers completely. 
+# The appearance of the initializer of individual variables and defines in the 
+# documentation can be controlled using \showinitializer or \hideinitializer 
+# command in the documentation regardless of this setting.
+
+MAX_INITIALIZER_LINES  = 30
+
+# Set the SHOW_USED_FILES tag to NO to disable the list of files generated 
+# at the bottom of the documentation of classes and structs. If set to YES the 
+# list will mention the files that were used to generate the documentation.
+
+SHOW_USED_FILES        = YES
+
+#---------------------------------------------------------------------------
+# configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated 
+# by doxygen. Possible values are YES and NO. If left blank NO is used.
+
+QUIET                  = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are 
+# generated by doxygen. Possible values are YES and NO. If left blank 
+# NO is used.
+
+WARNINGS               = YES
+
+# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings 
+# for undocumented members. If EXTRACT_ALL is set to YES then this flag will 
+# automatically be disabled.
+
+WARN_IF_UNDOCUMENTED   = YES
+
+# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for 
+# potential errors in the documentation, such as not documenting some 
+# parameters in a documented function, or documenting parameters that 
+# don't exist or using markup commands wrongly.
+
+WARN_IF_DOC_ERROR      = YES
+
+# The WARN_FORMAT tag determines the format of the warning messages that 
+# doxygen can produce. The string should contain the $file, $line, and $text 
+# tags, which will be replaced by the file and line number from which the 
+# warning originated and the warning text.
+
+WARN_FORMAT            = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning 
+# and error messages should be written. If left blank the output is written 
+# to stderr.
+
+WARN_LOGFILE           = 
+
+#---------------------------------------------------------------------------
+# configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag can be used to specify the files and/or directories that contain 
+# documented source files. You may enter file names like "myfile.cpp" or 
+# directories like "/usr/src/myproject". Separate the files or directories 
+# with spaces.
+
+INPUT                  = ./mainpage.dox ../src ../include/qpOASES ../examples
+
+# If the value of the INPUT tag contains directories, you can use the 
+# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp 
+# and *.h) to filter out the source-files in the directories. If left 
+# blank the following patterns are tested: 
+# *.c *.cc *.cxx *.cpp *.c++ *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh *.hxx *.hpp 
+# *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm
+
+FILE_PATTERNS          = *.cpp *.ipp *.hpp *.c
+
+# The RECURSIVE tag can be used to turn specify whether or not subdirectories 
+# should be searched for input files as well. Possible values are YES and NO. 
+# If left blank NO is used.
+
+RECURSIVE              = YES
+
+# The EXCLUDE tag can be used to specify files and/or directories that should 
+# excluded from the INPUT source files. This way you can easily exclude a 
+# subdirectory from a directory tree whose root is specified with the INPUT tag.
+
+EXCLUDE                = 
+
+# The EXCLUDE_SYMLINKS tag can be used select whether or not files or directories 
+# that are symbolic links (a Unix filesystem feature) are excluded from the input.
+
+EXCLUDE_SYMLINKS       = NO
+
+# If the value of the INPUT tag contains directories, you can use the 
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude 
+# certain files from those directories.
+
+EXCLUDE_PATTERNS       = 
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or 
+# directories that contain example code fragments that are included (see 
+# the \include command).
+
+EXAMPLE_PATH           = ../examples
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the 
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp 
+# and *.h) to filter out the source-files in the directories. If left 
+# blank all files are included.
+
+EXAMPLE_PATTERNS       = 
+
+# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be 
+# searched for input files to be used with the \include or \dontinclude 
+# commands irrespective of the value of the RECURSIVE tag. 
+# Possible values are YES and NO. If left blank NO is used.
+
+EXAMPLE_RECURSIVE      = NO
+
+# The IMAGE_PATH tag can be used to specify one or more files or 
+# directories that contain image that are included in the documentation (see 
+# the \image command).
+
+IMAGE_PATH             = 
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should 
+# invoke to filter for each input file. Doxygen will invoke the filter program 
+# by executing (via popen()) the command <filter> <input-file>, where <filter> 
+# is the value of the INPUT_FILTER tag, and <input-file> is the name of an 
+# input file. Doxygen will then use the output that the filter program writes 
+# to standard output.  If FILTER_PATTERNS is specified, this tag will be 
+# ignored.
+
+INPUT_FILTER           = 
+
+# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern 
+# basis.  Doxygen will compare the file name with each pattern and apply the 
+# filter if there is a match.  The filters are a list of the form: 
+# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further 
+# info on how filters are used. If FILTER_PATTERNS is empty, INPUT_FILTER 
+# is applied to all files.
+
+FILTER_PATTERNS        = 
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using 
+# INPUT_FILTER) will be used to filter the input files when producing source 
+# files to browse (i.e. when SOURCE_BROWSER is set to YES).
+
+FILTER_SOURCE_FILES    = NO
+
+#---------------------------------------------------------------------------
+# configuration options related to source browsing
+#---------------------------------------------------------------------------
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will 
+# be generated. Documented entities will be cross-referenced with these sources. 
+# Note: To get rid of all source code in the generated output, make sure also 
+# VERBATIM_HEADERS is set to NO.
+
+SOURCE_BROWSER         = NO
+
+# Setting the INLINE_SOURCES tag to YES will include the body 
+# of functions and classes directly in the documentation.
+
+INLINE_SOURCES         = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct 
+# doxygen to hide any special comment blocks from generated source code 
+# fragments. Normal C and C++ comments will always remain visible.
+
+STRIP_CODE_COMMENTS    = YES
+
+# If the REFERENCED_BY_RELATION tag is set to YES (the default) 
+# then for each documented function all documented 
+# functions referencing it will be listed.
+
+REFERENCED_BY_RELATION = YES
+
+# If the REFERENCES_RELATION tag is set to YES (the default) 
+# then for each documented function all documented entities 
+# called/used by that function will be listed.
+
+REFERENCES_RELATION    = YES
+
+# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen 
+# will generate a verbatim copy of the header file for each class for 
+# which an include is specified. Set to NO to disable this.
+
+VERBATIM_HEADERS       = YES
+
+#---------------------------------------------------------------------------
+# configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index 
+# of all compounds will be generated. Enable this if the project 
+# contains a lot of classes, structs, unions or interfaces.
+
+ALPHABETICAL_INDEX     = NO
+
+# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then 
+# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns 
+# in which this list will be split (can be a number in the range [1..20])
+
+COLS_IN_ALPHA_INDEX    = 5
+
+# In case all classes in a project start with a common prefix, all 
+# classes will be put under the same header in the alphabetical index. 
+# The IGNORE_PREFIX tag can be used to specify one or more prefixes that 
+# should be ignored while generating the index headers.
+
+IGNORE_PREFIX          = 
+
+#---------------------------------------------------------------------------
+# configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES (the default) Doxygen will 
+# generate HTML output.
+
+GENERATE_HTML          = YES
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. 
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
+# put in front of it. If left blank `html' will be used as the default path.
+
+HTML_OUTPUT            = html
+
+# The HTML_FILE_EXTENSION tag can be used to specify the file extension for 
+# each generated HTML page (for example: .htm,.php,.asp). If it is left blank 
+# doxygen will generate files with .html extension.
+
+HTML_FILE_EXTENSION    = .html
+
+# The HTML_HEADER tag can be used to specify a personal HTML header for 
+# each generated HTML page. If it is left blank doxygen will generate a 
+# standard header.
+
+HTML_HEADER            = 
+
+# The HTML_FOOTER tag can be used to specify a personal HTML footer for 
+# each generated HTML page. If it is left blank doxygen will generate a 
+# standard footer.
+
+HTML_FOOTER            = 
+
+# The HTML_STYLESHEET tag can be used to specify a user-defined cascading 
+# style sheet that is used by each HTML page. It can be used to 
+# fine-tune the look of the HTML output. If the tag is left blank doxygen 
+# will generate a default style sheet. Note that doxygen will try to copy 
+# the style sheet file to the HTML output directory, so don't put your own 
+# stylesheet in the HTML output directory as well, or it will be erased!
+
+HTML_STYLESHEET        = 
+
+# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, 
+# files or namespaces will be aligned in HTML using tables. If set to 
+# NO a bullet list will be used.
+
+HTML_ALIGN_MEMBERS     = YES
+
+# If the GENERATE_HTMLHELP tag is set to YES, additional index files 
+# will be generated that can be used as input for tools like the 
+# Microsoft HTML help workshop to generate a compressed HTML help file (.chm) 
+# of the generated HTML documentation.
+
+GENERATE_HTMLHELP      = NO
+
+# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can 
+# be used to specify the file name of the resulting .chm file. You 
+# can add a path in front of the file if the result should not be 
+# written to the html output directory.
+
+CHM_FILE               = 
+
+# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can 
+# be used to specify the location (absolute path including file name) of 
+# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run 
+# the HTML help compiler on the generated index.hhp.
+
+HHC_LOCATION           = 
+
+# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag 
+# controls if a separate .chi index file is generated (YES) or that 
+# it should be included in the master .chm file (NO).
+
+GENERATE_CHI           = NO
+
+# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag 
+# controls whether a binary table of contents is generated (YES) or a 
+# normal table of contents (NO) in the .chm file.
+
+BINARY_TOC             = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members 
+# to the contents of the HTML help documentation and to the tree view.
+
+TOC_EXPAND             = NO
+
+# The DISABLE_INDEX tag can be used to turn on/off the condensed index at 
+# top of each HTML page. The value NO (the default) enables the index and 
+# the value YES disables it.
+
+DISABLE_INDEX          = NO
+
+# This tag can be used to set the number of enum values (range [1..20]) 
+# that doxygen will group on one line in the generated HTML documentation.
+
+ENUM_VALUES_PER_LINE   = 4
+
+# If the GENERATE_TREEVIEW tag is set to YES, a side panel will be
+# generated containing a tree-like index structure (just like the one that 
+# is generated for HTML Help). For this to work a browser that supports 
+# JavaScript, DHTML, CSS and frames is required (for instance Mozilla 1.0+, 
+# Netscape 6.0+, Internet explorer 5.0+, or Konqueror). Windows users are 
+# probably better off using the HTML help feature.
+
+GENERATE_TREEVIEW      = NO
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be 
+# used to set the initial width (in pixels) of the frame in which the tree 
+# is shown.
+
+TREEVIEW_WIDTH         = 250
+
+#---------------------------------------------------------------------------
+# configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will 
+# generate Latex output.
+
+GENERATE_LATEX         = NO
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. 
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
+# put in front of it. If left blank `latex' will be used as the default path.
+
+LATEX_OUTPUT           = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be 
+# invoked. If left blank `latex' will be used as the default command name.
+
+LATEX_CMD_NAME         = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to 
+# generate index for LaTeX. If left blank `makeindex' will be used as the 
+# default command name.
+
+MAKEINDEX_CMD_NAME     = makeindex
+
+# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact 
+# LaTeX documents. This may be useful for small projects and may help to 
+# save some trees in general.
+
+COMPACT_LATEX          = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used 
+# by the printer. Possible values are: a4, a4wide, letter, legal and 
+# executive. If left blank a4wide will be used.
+
+PAPER_TYPE             = a4wide
+
+# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX 
+# packages that should be included in the LaTeX output.
+
+EXTRA_PACKAGES         = 
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for 
+# the generated latex document. The header should contain everything until 
+# the first chapter. If it is left blank doxygen will generate a 
+# standard header. Notice: only use this tag if you know what you are doing!
+
+LATEX_HEADER           = 
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated 
+# is prepared for conversion to pdf (using ps2pdf). The pdf file will 
+# contain links (just like the HTML output) instead of page references 
+# This makes the output suitable for online browsing using a pdf viewer.
+
+PDF_HYPERLINKS         = YES
+
+# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of 
+# plain latex in the generated Makefile. Set this option to YES to get a 
+# higher quality PDF documentation.
+
+USE_PDFLATEX           = NO
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. 
+# command to the generated LaTeX files. This will instruct LaTeX to keep 
+# running if errors occur, instead of asking the user for help. 
+# This option is also used when generating formulas in HTML.
+
+LATEX_BATCHMODE        = NO
+
+# If LATEX_HIDE_INDICES is set to YES then doxygen will not 
+# include the index chapters (such as File Index, Compound Index, etc.) 
+# in the output.
+
+LATEX_HIDE_INDICES     = NO
+
+#---------------------------------------------------------------------------
+# configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output 
+# The RTF output is optimized for Word 97 and may not look very pretty with 
+# other RTF readers or editors.
+
+GENERATE_RTF           = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. 
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
+# put in front of it. If left blank `rtf' will be used as the default path.
+
+RTF_OUTPUT             = RTF
+
+# If the COMPACT_RTF tag is set to YES Doxygen generates more compact 
+# RTF documents. This may be useful for small projects and may help to 
+# save some trees in general.
+
+COMPACT_RTF            = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated 
+# will contain hyperlink fields. The RTF file will 
+# contain links (just like the HTML output) instead of page references. 
+# This makes the output suitable for online browsing using WORD or other 
+# programs which support those fields. 
+# Note: wordpad (write) and others do not support links.
+
+RTF_HYPERLINKS         = NO
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's 
+# config file, i.e. a series of assignments. You only have to provide 
+# replacements, missing definitions are set to their default value.
+
+RTF_STYLESHEET_FILE    = 
+
+# Set optional variables used in the generation of an rtf document. 
+# Syntax is similar to doxygen's config file.
+
+RTF_EXTENSIONS_FILE    = 
+
+#---------------------------------------------------------------------------
+# configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES (the default) Doxygen will 
+# generate man pages
+
+GENERATE_MAN           = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. 
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
+# put in front of it. If left blank `man' will be used as the default path.
+
+MAN_OUTPUT             = MAN
+
+# The MAN_EXTENSION tag determines the extension that is added to 
+# the generated man pages (default is the subroutine's section .3)
+
+MAN_EXTENSION          = .3
+
+# If the MAN_LINKS tag is set to YES and Doxygen generates man output, 
+# then it will generate one additional man file for each entity 
+# documented in the real man page(s). These additional files 
+# only source the real man page, but without them the man command 
+# would be unable to find the correct page. The default is NO.
+
+MAN_LINKS              = NO
+
+#---------------------------------------------------------------------------
+# configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES Doxygen will 
+# generate an XML file that captures the structure of 
+# the code including all documentation.
+
+GENERATE_XML           = NO
+
+# The XML_OUTPUT tag is used to specify where the XML pages will be put. 
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
+# put in front of it. If left blank `xml' will be used as the default path.
+
+XML_OUTPUT             = XML
+
+# The XML_SCHEMA tag can be used to specify an XML schema, 
+# which can be used by a validating XML parser to check the 
+# syntax of the XML files.
+
+XML_SCHEMA             = 
+
+# The XML_DTD tag can be used to specify an XML DTD, 
+# which can be used by a validating XML parser to check the 
+# syntax of the XML files.
+
+XML_DTD                = 
+
+# If the XML_PROGRAMLISTING tag is set to YES Doxygen will 
+# dump the program listings (including syntax highlighting 
+# and cross-referencing information) to the XML output. Note that 
+# enabling this will significantly increase the size of the XML output.
+
+XML_PROGRAMLISTING     = YES
+
+#---------------------------------------------------------------------------
+# configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will 
+# generate an AutoGen Definitions (see autogen.sf.net) file 
+# that captures the structure of the code including all 
+# documentation. Note that this feature is still experimental 
+# and incomplete at the moment.
+
+GENERATE_AUTOGEN_DEF   = NO
+
+#---------------------------------------------------------------------------
+# configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_PERLMOD tag is set to YES Doxygen will 
+# generate a Perl module file that captures the structure of 
+# the code including all documentation. Note that this 
+# feature is still experimental and incomplete at the 
+# moment.
+
+GENERATE_PERLMOD       = NO
+
+# If the PERLMOD_LATEX tag is set to YES Doxygen will generate 
+# the necessary Makefile rules, Perl scripts and LaTeX code to be able 
+# to generate PDF and DVI output from the Perl module output.
+
+PERLMOD_LATEX          = NO
+
+# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be 
+# nicely formatted so it can be parsed by a human reader.  This is useful 
+# if you want to understand what is going on.  On the other hand, if this 
+# tag is set to NO the size of the Perl module output will be much smaller 
+# and Perl will parse it just the same.
+
+PERLMOD_PRETTY         = YES
+
+# The names of the make variables in the generated doxyrules.make file 
+# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. 
+# This is useful so different doxyrules.make files included by the same 
+# Makefile don't overwrite each other's variables.
+
+PERLMOD_MAKEVAR_PREFIX = 
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor   
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will 
+# evaluate all C-preprocessor directives found in the sources and include 
+# files.
+
+ENABLE_PREPROCESSING   = YES
+
+# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro 
+# names in the source code. If set to NO (the default) only conditional 
+# compilation will be performed. Macro expansion can be done in a controlled 
+# way by setting EXPAND_ONLY_PREDEF to YES.
+
+MACRO_EXPANSION        = NO
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES 
+# then the macro expansion is limited to the macros specified with the 
+# PREDEFINED and EXPAND_AS_PREDEFINED tags.
+
+EXPAND_ONLY_PREDEF     = NO
+
+# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files 
+# in the INCLUDE_PATH (see below) will be search if a #include is found.
+
+SEARCH_INCLUDES        = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that 
+# contain include files that are not input files but should be processed by 
+# the preprocessor.
+
+INCLUDE_PATH           = 
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard 
+# patterns (like *.h and *.hpp) to filter out the header-files in the 
+# directories. If left blank, the patterns specified with FILE_PATTERNS will 
+# be used.
+
+INCLUDE_FILE_PATTERNS  = 
+
+# The PREDEFINED tag can be used to specify one or more macro names that 
+# are defined before the preprocessor is started (similar to the -D option of 
+# gcc). The argument of the tag is a list of macros of the form: name 
+# or name=definition (no spaces). If the definition and the = are 
+# omitted =1 is assumed.
+
+PREDEFINED             = 
+
+# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then 
+# this tag can be used to specify a list of macro names that should be expanded. 
+# The macro definition that is found in the sources will be used. 
+# Use the PREDEFINED tag if you want to use a different macro definition.
+
+EXPAND_AS_DEFINED      = 
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then 
+# doxygen's preprocessor will remove all function-like macros that are alone 
+# on a line, have an all uppercase name, and do not end with a semicolon. Such 
+# function macros are typically used for boiler-plate code, and will confuse the 
+# parser if not removed.
+
+SKIP_FUNCTION_MACROS   = YES
+
+#---------------------------------------------------------------------------
+# Configuration::additions related to external references   
+#---------------------------------------------------------------------------
+
+# The TAGFILES option can be used to specify one or more tagfiles. 
+# Optionally an initial location of the external documentation 
+# can be added for each tagfile. The format of a tag file without 
+# this location is as follows: 
+#   TAGFILES = file1 file2 ... 
+# Adding location for the tag files is done as follows: 
+#   TAGFILES = file1=loc1 "file2 = loc2" ... 
+# where "loc1" and "loc2" can be relative or absolute paths or 
+# URLs. If a location is present for each tag, the installdox tool 
+# does not have to be run to correct the links.
+# Note that each tag file must have a unique name
+# (where the name does NOT include the path)
+# If a tag file is not located in the directory in which doxygen 
+# is run, you must also specify the path to the tagfile here.
+
+TAGFILES               = 
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create 
+# a tag file that is based on the input files it reads.
+
+GENERATE_TAGFILE       = 
+
+# If the ALLEXTERNALS tag is set to YES all external classes will be listed 
+# in the class index. If set to NO only the inherited external classes 
+# will be listed.
+
+ALLEXTERNALS           = NO
+
+# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed 
+# in the modules index. If set to NO, only the current project's groups will 
+# be listed.
+
+EXTERNAL_GROUPS        = YES
+
+# The PERL_PATH should be the absolute path and name of the perl script 
+# interpreter (i.e. the result of `which perl').
+
+PERL_PATH              = /usr/bin/perl
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool   
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will 
+# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base or 
+# super classes. Setting the tag to NO turns the diagrams off. Note that this 
+# option is superseded by the HAVE_DOT option below. This is only a fallback. It is 
+# recommended to install and use dot, since it yields more powerful graphs.
+
+CLASS_DIAGRAMS         = YES
+
+# If set to YES, the inheritance and collaboration graphs will hide 
+# inheritance and usage relations if the target is undocumented 
+# or is not a class.
+
+HIDE_UNDOC_RELATIONS   = YES
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is 
+# available from the path. This tool is part of Graphviz, a graph visualization 
+# toolkit from AT&T and Lucent Bell Labs. The other options in this section 
+# have no effect if this option is set to NO (the default)
+
+HAVE_DOT               = YES
+
+# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen 
+# will generate a graph for each documented class showing the direct and 
+# indirect inheritance relations. Setting this tag to YES will force the 
+# the CLASS_DIAGRAMS tag to NO.
+
+CLASS_GRAPH            = YES
+
+# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen 
+# will generate a graph for each documented class showing the direct and 
+# indirect implementation dependencies (inheritance, containment, and 
+# class references variables) of the class with other documented classes.
+
+COLLABORATION_GRAPH    = NO
+
+# If the UML_LOOK tag is set to YES doxygen will generate inheritance and 
+# collaboration diagrams in a style similar to the OMGs Unified Modeling 
+# Language.
+
+UML_LOOK               = NO
+
+# If set to YES, the inheritance and collaboration graphs will show the 
+# relations between templates and their instances.
+
+TEMPLATE_RELATIONS     = NO
+
+# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT 
+# tags are set to YES then doxygen will generate a graph for each documented 
+# file showing the direct and indirect include dependencies of the file with 
+# other documented files.
+
+INCLUDE_GRAPH          = YES
+
+# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and 
+# HAVE_DOT tags are set to YES then doxygen will generate a graph for each 
+# documented header file showing the documented files that directly or 
+# indirectly include this file.
+
+INCLUDED_BY_GRAPH      = YES
+
+# If the CALL_GRAPH and HAVE_DOT tags are set to YES then doxygen will 
+# generate a call dependency graph for every global function or class method. 
+# Note that enabling this option will significantly increase the time of a run. 
+# So in most cases it will be better to enable call graphs for selected 
+# functions only using the \callgraph command.
+
+CALL_GRAPH             = NO
+
+# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen 
+# will graphical hierarchy of all classes instead of a textual one.
+
+GRAPHICAL_HIERARCHY    = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images 
+# generated by dot. Possible values are png, jpg, or gif
+# If left blank png will be used.
+
+DOT_IMAGE_FORMAT       = png
+
+# The tag DOT_PATH can be used to specify the path where the dot tool can be 
+# found. If left blank, it is assumed the dot tool can be found on the path.
+
+DOT_PATH               = 
+
+# The DOTFILE_DIRS tag can be used to specify one or more directories that 
+# contain dot files that are included in the documentation (see the 
+# \dotfile command).
+
+DOTFILE_DIRS           = 
+
+# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the 
+# graphs generated by dot. A depth value of 3 means that only nodes reachable 
+# from the root by following a path via at most 3 edges will be shown. Nodes that 
+# lay further from the root node will be omitted. Note that setting this option to 
+# 1 or 2 may greatly reduce the computation time needed for large code bases. Also 
+# note that a graph may be further truncated if the graph's image dimensions are 
+# not sufficient to fit the graph (see MAX_DOT_GRAPH_WIDTH and MAX_DOT_GRAPH_HEIGHT). 
+# If 0 is used for the depth value (the default), the graph is not depth-constrained.
+
+MAX_DOT_GRAPH_DEPTH    = 0
+
+# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will 
+# generate a legend page explaining the meaning of the various boxes and 
+# arrows in the dot generated graphs.
+
+GENERATE_LEGEND        = YES
+
+# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will 
+# remove the intermediate dot files that are used to generate 
+# the various graphs.
+
+DOT_CLEANUP            = YES
+
+#---------------------------------------------------------------------------
+# Configuration::additions related to the search engine   
+#---------------------------------------------------------------------------
+
+# The SEARCHENGINE tag specifies whether or not a search engine should be 
+# used. If set to NO the values of all tags below this one will be ignored.
+
+SEARCHENGINE           = NO
diff --git a/3rdparty/qpOASES/qpOASES-3.0/doc/index.html b/3rdparty/qpOASES/qpOASES-3.0/doc/index.html
new file mode 100644
index 00000000000..9211a5de060
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/doc/index.html
@@ -0,0 +1,12 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/doc</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/doc</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="DoxygenLayout.xml">DoxygenLayout.xml</a></li>
+  <li><a href="Makefile">Makefile</a></li>
+  <li><a href="doxygen.config">doxygen.config</a></li>
+  <li><a href="mainpage.dox">mainpage.dox</a></li>
+  <li><a href="manual.pdf">manual.pdf</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/doc/mainpage.dox b/3rdparty/qpOASES/qpOASES-3.0/doc/mainpage.dox
new file mode 100644
index 00000000000..c945b356e0c
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/doc/mainpage.dox
@@ -0,0 +1,120 @@
+/**
+ *	\mainpage Main Page
+ *
+ *	<p>&nbsp;</p>
+ *
+ *	\section sec_copyright Copyright
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy. \n
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *	<p>&nbsp;</p>
+ *
+ *
+ *	\section sec_developers Authors and Contributors
+ *
+ *	qpOASES's core functionality and software design have been developed by 
+ *  the following main developers (in alphabetical order):
+ *  <ul>
+  *	<li>Hans Joachim Ferreau
+ *  <li>Christian Kirches
+ *  <li>Andreas Potschka
+ *	</ul>
+ *
+ *	Moreover, the following developers have contributed code to qpOASES's 
+ *	third-party interfaces (in alphabetical order):
+ *  <ul>
+ *	<li>Alexander Buchner
+ *	<li>Holger Diedam
+ *	<li>Manuel Kudruss
+ *  <li>Sebastian F. Walter
+ *	</ul>
+ *
+ *	Finally, the following people have not contributed to the source code, 
+ *  but have helped making qpOASES even more useful by testing, reporting 
+ *  bugs or proposing algorithmic improvements (in alphabetical order): 
+ *  <ul>
+ *	<li>Eckhard Arnold
+ *	<li>Boris Houska
+ *	<li>Aude Perrin
+ *	<li>Milan Vukov
+ *	<li>Thomas Wiese
+ *	<li>Leonard Wirsching
+ *	</ul>
+ *
+ *	All users are invited to further improve qpOASES by providing comments, code enhancements, 
+ *	bug reports, additional documentation or whatever you feel is missing. The preferred way
+ *  of doing so is to <b>create a <a href="https://projects.coin-or.org/qpOASES/newticket">new ticket</a>
+ *  at the qpOASES webpage</b>. In case you do not want to disclose your feedback to the public, 
+ *  you may send an e-mail to <a href="mailto:support@qpOASES.org">support@qpOASES.org</a> 
+ *	or contact one of the main developers directly.
+ *	<p>&nbsp;</p>
+ *
+ *
+ *	\section sec_citing Citing qpOASES
+ *
+ *	If you use qpOASES within your scientific work, we strongly encourage you to cite at least 
+ *	one of the following publications:
+ *
+ *  <ul>
+ *	<li>Reference to the <b>software</b>:
+ *	\code
+ *	 @ARTICLE{Ferreau2014,
+ *	  author = {H.J. Ferreau and C. Kirches and A. Potschka and H.G. Bock and M. Diehl},
+ *	  title = {{qpOASES}: A parametric active-set algorithm for quadratic programming},
+ *	  journal = {Mathematical Programming Computation},
+ *	  year = {2014},
+ *	  volume = {6},
+ *	  number = {4},
+ *	  pages = {327--363},
+ *	  keywords = {qpOASES, parametric quadratic programming, active set method, model predictive control}
+ *	}
+ *	\endcode
+ *
+ *	<li>Reference to the <b>online active set strategy</b>:
+ *	\code
+ *	 @ARTICLE{Ferreau2008,
+ *	  author = {H.J. Ferreau and H.G. Bock and M. Diehl},
+ *	  title = {An online active set strategy to overcome the limitations of explicit MPC},
+ *	  journal = {International Journal of Robust and Nonlinear Control},
+ *	  year = {2008},
+ *	  volume = {18},
+ *	  number = {8},
+ *	  pages = {816--830},
+ *	  keywords = {model predictive control, parametric quadratic programming, online active set strategy}
+ *	}
+ *	\endcode
+ *
+ *	<li>Reference to the <b>webpage</b>:
+ *	\code
+ *	 @MISC{qpOASES2014,
+ *	  author = {H.J. Ferreau and A. Potschka and C. Kirches},
+ *	  title = {{qpOASES} webpage},
+ *	  howpublished = {http://www.qpOASES.org/},
+ *	  year = {2007--2014},
+ *	  keywords = {qpOASES, model predictive control, parametric quadratic programming, online active set strategy}
+ *	}
+ *	\endcode
+ *	</ul>
+ *	<p>&nbsp;</p>
+ *
+ *	\section sec_moreinfo More Information
+ *
+ *	More information can be found on <a href="http://www.qpOASES.org/">http://www.qpOASES.org/</a> 
+ *	and in the <a href="../manual.pdf">qpOASES User's Manual</a>.
+*	<p>&nbsp;</p>
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/doc/manual.pdf b/3rdparty/qpOASES/qpOASES-3.0/doc/manual.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..5cadc179aacfecee0a4bac7b50679b85ebebdae3
GIT binary patch
literal 710503
zcmeFZ2|SeV`!7EBERjM)DEpqB$b^t&-;yoMVC=@ePLY%>At`H;?E9MRvWJkJ?EAhC
zGsb!J`IPki_Um`PuXFzAod4@Hd3p^V*L`32`&!=Db>Gi0soap|;^h`3VEX)}zMp`X
z{tCUFu_b}HI6aR#J&&B7h8;c64SH5tHDx6Y6-hZ6?kjBc5)uRuThpW41pdCARTgGv
z>qKvFWDa5b{yV|nh%&J-vUh^OxOsmTB=TL5ysZ<=&eYk&32JBiLzL)mL^(pf-N(&)
zQW!7acWLS-cJ>f@J2QGG3kbcsotcxX5$t$|y!?NZ%lGpnd4>LsT&N?xqLHn$k@b(+
z@(TY^EdS49`L28yE9(pcCJLihceb~;gE{?>$oD%~ju$}Sgv1}XNJE{Vw&wH>_R5m#
zGU~?y{{+qnz4?AZ$I;2i+S=#@I(&bEhwmgj{KxP(8QGc|!A$ApZU2p2{y)LP$9-a&
z{6FB4c6M~Kvw=E7ekb{N2z|HB6Zi=Hz()?^^ldrSos3{k5Yr#aDexzJ_)o5Yzz=+s
zot><qw#OzZ@H=3>Bh7zOXMrEkC>lX+>D8Se_KwHX`V%nx+$R(j`~i$4y@s=s9Smw@
zO)ukSWMglAJVU`hp~BC7!WIR8prZcw>S@~3>)1KN=#^b<|5f^*kU1e+=m#?DcGk{E
zMoF(_1kBpj$&ud74)7y;h{=%-sw(sv?)H%5l@a<AU;-xr6Z!!dFp$fUN2#hf(tlg<
zzb#nS4rcRX;f4N$qQFTMg?^y;vv^milf{uf?nmP(8actBCO@n2CvXK$f-C$3-0x$k
zLjhpj>80$PZA}4B0W|udfbgF{7B~sA@G;0gt<N_FRO}pq7~}$kUx*Eq>juOOY72FO
z(BFhOIs!obrI*|Bz`w))yZsAr3;rDGM1Hn_AHaMYSOW%iIRgJTBZ>Uk1cXkE^e-l$
zWM|7IZD;EOaXYbgBEJLwdvH0azz^spO<jy^O#mQdoo&CRH6v@NllzaA5&09|f+tN@
z^atL`#!f(NI5Hw2ZaFzS0?`(54x5ufT=Y+f3!XGt(I1E_+aEdJ-wt<-{*Q@^{s~gS
zlZMKB<p-uRZuZu8e`PRrdm|WNNbW~Y_Z?Bd_2>Zr9Gzf*et^q=^O0j);=S@Ghy_nt
zG4GXQjE^FR**DKK`T@wdm?Gn1WbF*B9k857q~fRXe`w5m<##6V-HHWIS~2gHA66g_
zcn4s$-*VSYI~zMEJNsiB;Jxx^BRFBSygyN)pEZ(J(WKXa+Cb=ULv5f=-}C}v<o6(Y
ztjHf3k|4M6&rZU7Op(5G8BIrsnX~neH9Tq5ynp0Mg4`k}1~?{5lBT9y%75G4ul$Dh
z_l(Ke_!tC{lP<%{cZ~n{d?IOUWbN(<Jr0e$e7^<vr05geg!h<3eGk7H5Jx9^6_}l|
zHN*z+9+;iYH-l2PJt720DN71sYhqzz1hWE+%-Z@ds(TF0Z&}y(Fmuunj!755upuxr
zBOsT2!(GDy3Nz(W0SJxzzhlUM3JXHNEa5Q;`!UY9VF2H>HZne*KL4Mhg3!r$!+Xra
zP6`IHm@}|qR>z|M$i{?z2{^}e?4)RA6DK2=W9GyAM{*|gOFTJdXD0<anm|u1hQOcP
zTIiQ}am>_C3RZD<viPZP;{7986XHH;CdX_|&Dr)_&iLE4=mBnJs;(j_Epxokf`7*F
zWOO*@YyU>Nn~JoC5zHKNT=ww(k){d#5*?0tnx^AXg#JGHBSHWqj-!ArDJ^+JnO?)r
z&e{t4^H9I_>c4ADA#TwVf#{gC{oA1wHPxk+Z*a-WA1{~CpHb#LQC;&MbGCmwhK}TI
zMR}Fu0e;KczK4X9XdIKa8xR+WHBc$Tex?TWf2CI&dnn*u=JbE3`JYDn6Z*oxgob0T
zCT(rx=tzGP3V|8HOe}uNZ+|3Z!oNfXkslFT&f3lxV0+*4@lgSAJkj40vhRsN_?LJf
z@*^8d+a0M7tc$F*k@?Re{{*G*FOlGwjcEc2p8hEI1B-e*$KTVh<D>$}{AERtN!Z^4
z|Dq%u9Q0ZcfGzzT34YJQP7ZJ)Yw;aZv7ZO{E0zPc@^>Qnp$6Y?8Q0$y$j&WrqV2<X
z%($eW#@0|fbC{9+PX@vFTP9^>Zx8gKpl;lfKYJzLF_HT2`ro@g0M#)866$d{=ldO*
z(&ghlVxVgDJhD(H{hzvBe7t{K8@;$V0ncrStvOJj0W4FH{@aTHxZfIB65#KE4hiH2
z#Kg`NLI5<&0D=lJvLSFA4AxX|91<a_L~73!Z`}U0c1cxDnOdxQrZetsQ8!Jo=QF0>
zi5qY9u7wv@`AGU&E<s&f8u=+s-3LjGG06MrU-tu>ZGkIW+m^P%As7m$%pJ(AE3so`
z9&WHc<Mm=oZ5hj1TSTu|d*{U;R=meJ-1=<z?#6vFnZ^JE=c?^Z|7Y?DbK8b;pOLzo
z6ph0xIv?5OUlCD=Uy^@5(W~rOBh|BACRl2CCa~80Lbs!4su`gbfkf)ebD4X_LUlY%
z+(P`gn5XYl-?(AQh;>Qr?4}57^gtigcp4~fTjWgK6Ng(P>`B&L@dm=2B6j9<W5V_N
zw;!Cx3)7dwU~(e$g)Ld!e)$|jnIXIdyVGF0q4-Jb+Pk({LJA5;3a!Cgrx$o;#W6DD
zTI4iRF{<1ZbJLy>@rBEC+fXoH@?iFTY0u7!7sF$GA-)tIqH7JobG*lQ2^w2Xk4>sJ
zR$MTB_FUFoU8$0Ux>%{_o05skNeh}Q8Zyqtn)0Q%*gV8Fpe~Z0p7(4Ah=RJp&qC3r
z&z^^~Zg#IW9#k$r8okzib7(na#>3-P!vjX0%W9c=*XuETL5fKNaiGJ&s8jow?%lY-
zH~PZ+>yXl|ImRz|QZy^G;-|&#o8l6M(LTC)|A~XRDr%0CCgsh;s{P0Xot`Wk150S|
z6E9gRiY%(D6&C|~lUGzTf?YFqH7o10{J6)+L<QSlYmA7Ab#s=p#-|T2?~1fv?e3j^
zlGRrHn3UGxjEF;=Yh6~4Fl%kxrFj3=ywsqDCG7Dn{i1E#{8%f^nKsj=(?MS-@MlR*
z>1veK)K3rB6mVXXY&%N|UG?q>If&?)`s$M%^eRaBy=Z<Gd%v(nlNqUsVcG0-7H)Rh
z*C<oF^O}>zLwQRLKKZf1$x;UuV5vbZu#O^r{Z46VN%K%5Pr4r;eKGEW@ndBzu*AUi
z<$@fTdQltMxGI(vs<>!y=J4(TQ=ZagFQ+F9HNw{jMl*WLi90>!hzYvPX4Bl7JehCS
zKG?ROmzB5z_she38+wRm1&O|$nZ=oVs&slFg^0VT^L}P_)nvZ9pnz&*DJ-+NmUNb7
z^8V-$9;#A%a+7RZh9q)lLxqiOWyjBUE7Z%L2ZLEowyxmPIJ@Y=qW=IiNy8k8^O1a<
zfS5P@PHEKSijQMn{_02k1p{_{4zj1DkF9X^5Q8xZ1afK!q4z=*C6UX5)1NOj;}Lc(
zl(@`$UoF3&?kPA?g9lmhyZhWOcXF^-Z=v!t@~-DqFS)Czk63zpzR=EGZh@Cu=5Dxz
z0y%?bVB&uK2Xi@e;4<MZHa*fY*JP)(g{%*KnkCZHew`gVdl%2#Vwc*OY4upEMPmiC
zUP?IXhyNV`{_dLp6<>h<`L{T7)S05^Q2|=#1Ux|Ya)KO{vVVmdH3A+rh$GM|J0k0R
z--<EdM~V<rsF9SN8@=vPm=Y8PDsv!}iwN*?i-_tI0Fq#U2ztJ6#SCz}q^+$TAW;_(
zaMTh|qt^#w68(><{9PmeG?hEb#+Cr-A>dJWHg@_pIeD9Jv(tu}9tAugD-iJ9gh0(L
zoajZb@DlJyL7f~`Ah4rkX=i&hw#bj@C@ch!m?OQc?O;F&ZvtovkPRRbtI0_n$IBbi
z8vhb45Bm?tKxb~r%FBW<FhC#-;2-F42qX={!o)oK0$$j_7tSdh9BgbH0z5q2Q$z$r
zM1%x{gr|wmo;giSN=!(2hT;tAIWlr`aw3xRloVu?XUWLPj)Y)f0e^#ygO7uQPj;H{
zG}-_3>#!O`dJ4-QI~NP%G6<6t1B(>n@FR#0m?sX#w-@N|7X~IUM_jy9_ymN&4Mk@_
zm>5`CnAliHvj*<=27V91CdD~>iB}T$oT?GtWd}082Vse)7^L!Q$kjU682OE1zW4+b
z=P9W!Ffp^Rvat&Y3JHsdUX{KfBP(}P{+7Cirk3^{ox3Kc5HoWNsHLNmvx}>nyNBOH
z|3?9jp9F?SJdb=4_3~Bpo20kN?^06J((?-ni;7E1-<Q?aeXMV2Y-(=l>i*Kx+t)uZ
zI6g5sH9a#sH@~pHvAMOqvy0f<Khg^Wg!PwN!2f?K_KRMmfL@r`*jU(jM|xpkx&jv#
zDK^d}Ufi>is(3~Y=PvU-I7KEEmY7$A&%m#?Ms5u2B%ojv7-w2PQtg{&|2@Tg|0B(Q
zD0ZyZAczPH0{{<;6a)qxsJ{th1^sjVvj+dzz&|$dj}81|1OM2-KQ{1>4g7y=138~o
zhn|i{J*wpJ`C6tFF%c(>R96lde=Dq7H`_|gbm8p+HE(`=NF7xp_|^e$PKVtf9IZxY
zbO<ud2Oo$j7*KesIl)@C*_s>VEqa5!I)wRR)5h6v5w~#%JcHL!dC?q#62XTccGm=y
zM<~LC$1h=v<q$Njw0Q^$)h<#sybrgUG2A)_MiCp%_?T=j8KMrN(AbBdPwR%i+^u>D
zB7&?rq8}cDP-$P`>kB*StmwW^RddX61WnZ^iHfI(poIKGP<6syJjbv1g2#g@iQ%6U
z4ndJ(9q4NYJu-BIdr=429fu&SsJ(!`Jox60A>!O2h`%0w@>V$%Z&XdkKJcrJ6!$|=
zW5!@=64H?G5cK-hA!rPC2)eSr!-Mu|Jp^IGXWVjrxjXd`^oS23dI*w&qcv8+h;2_z
zvY#}jGpIiVbqv7Alu;ezzuxNO;w6o+O7{XIBGVCkH>WDUaB<fcVp7(4CVNrps4CoR
z3JN;M7~C%&c;KBA2)A1ekiPN7OKhD({`nn4@%a>e@zc{S-Ry-gHrX`s-0qAhylBEd
zh2>`msU7&rG-*J0PP182$5CjRDSgy*TWM&{n?mlThFh`%sW5BnhssLrjjA!Pp|OlZ
z5bnJ2>9=k}wcW|X`~qrUGrQl039S*d5|0O03EkS<cu*J{y{kw!Hd<!`zg_ahX_<|C
zDf_&aqnx{gFL<@wmsk?-nIvAAL6qGXoWSSHA?UQY*o3~PQh2v!d|wOFIc&r1NtaZ9
z`c$b!Ytk|?HNjMHrlQyQ#OOo#nM07-;%*;j%K1Tp+cB#)lrI@^;#6%fHF1QEMyk^j
zARp%pl<J0Vv=qS$Hg5}%->lS%HStWbVX{*09MoReN9TR?I{k9oBGU&mGeuV6mAtT(
zAlyTE*ITnMae;2_H45(#be#tk3)n>nU|$3z)$lFMLr`<`AqX*T<Rx+N#wqlixXax^
z<^Ug!1POUi(KCOly(c*MV!2si)znYuwX^%S%m_)sXUGn+F9eJ7tjR9++?_)yld4~-
zZ4ED+N<aYC2>(^`FC%NeRy7FNk<|z0vIf{M16oeFFVFD1K>{W^E{PyP-#6S71w4r+
z2~9?K{}42Xa|nuvI0UV?Y!R@cn_8>pXz38x@Ft0hz+*r7Z;UMW^T3kZOWNNZ5b#ih
ziVqfG=;zT&%7Epw9)kSw#=!d+@Tn=l2VoVMf#7_;)qT7GGzQ%UNse*V>{8Vl3)&o5
z&04=Tx&u(v1`LiCl|!+DF%ChW2@gRmfbUO6&XK7eghjzWi-Xrr9fF>Ddjh5ph8%((
zfH%btLBFa8KB0Zi^*5nQK_Ac$#t%XF<xqZ${W*I`9u)o|=+*2Y=nsf`;E=&hA){L)
zBq&Vq6ySXDzkLr_*AuFRp46P<Giwr(fg4x~H~1LoA&7o|hvU1;12N$j{hq^r*52TO
z4BY^g>j75XAqW(@hxZS%KMeODWdHHme|+{h0|9y9pJ)s)p?{+BiDdLoo;{-0|K!<!
z7}UQosP+{J9F#;s$qV-_C*M=g%fp+!ucot7F-9<huqxbzmqwh8+>u6JtT76ONaE<P
zBAw_qTZOY>2W4kIJmtUm5$4XoI2lB+3WkyZb*hn@*)@V*o07ql{gL2D3idwM;}59Y
z&p(q?r3Z>2g39yo+5$uolEXWDw7E~vj%wiRsGVZIT+9$1_qj+d>hn}u?S0TI_Ezg5
z{EcH*UM^7C(#D#w3HxY7RB=Sf<SVRN9GnZV7b({|TY2?uOwd`6;w_k}q0s2?E9ANi
znRX*>sj3Va=s&%*XG@lWz{y6EdkD(cp+m<d=X{Pv?Hr^4)%K+wx*Y|ELr`~N(Pp{y
zA?R|Z9735x;1C4MnA3d|$gx+7ie-Hh2>SWrh(aWcQG)|%L4D#}UTBRAk?(`e>gbqN
zq-B_~*la>lhhP{(el>z$RPT1DUF8qHW!v?%%saY2RY_wi<-A#u1m;0#xrzH&=tV1_
z7~lSg6#N)<PC#Yau%#k^L)`tkWQ$x}eQ_vDP*hGX#}v;HN$sW=Ib!;?m(<!~z2)GC
zqVW<n74P)3zV<B-;I8aVF_p&c2G-9`<z$EpXV*-G63=_IrORK;PO>5&mcN40!+3px
z67#wUg9yu_!6AtJv%kA)+gyfhx${P>6THu9;Gkzb-mTzv#=@r0_BCyUq4M+W3eM28
zhzjIoH@k*4!P!R)#cSF5A6{nil0qFej8v1<AHUb!`|N70*-W|DjdYyATbj&-8(C4v
z>a#IBkU1t%Tlu#XR225(CDOpinq29v0}t@iql-h0a-bXimM&U@!9k^)T-g&k#$IRY
z*sJ|~G4>zmmy0&OdMB+WfsECPK{k6VI!~>XS$M=sSL@iiQr(Kx6s>F>@ySpC3!3)~
z@8wh*QWTQV=~ln!lZU3~`!UCf8OvCol{)u;9z(umi;|JsK|k3&svg|mFsb0uTY1qy
zX_xm?K697@-`lXbsg*}CZEC)k+Oq_W?jm>{<t*#725Vfy(JUbXLwj%iEV#rCG^Yrs
zr=^qpM-!e+Kel@ru1l}IBS+{K8D;+bi<Kgqv!^afb;Kj1;UIJheR<5fks)nWa-*Uw
zWu7Y`sW3DtHM8n8B1vK0<z?BUFt?~;|3EtxjSS+%=l3<XMCk1$=VUCkm!72&<f*^w
zRyHfTjT%&aP8}2(O~Cpl^V$E4t-jP1?#oBvX8I=m6NP5ZAad2~&5ZFtu#q>ohP>%E
zizKY$ozp6?sq6vwE?M*G`fZvMo%(8D?o03l$9>?D5S0&w+pA^r-O8sk?U~%PchWnP
zXsfzKM<1f;qILohyUbbMSk2=jg8pCfZa;kOPfUN`_afhN>m~Xtn~!snBmf_7?nZwC
z?}!c_g65rrm65zCpe*$_Uu*APc0nt>&oJIWdf0@EhmO?3((8;T*#`!fnTbzJuG68F
z5_W*LU0!?NAxI(93V}Zx(IL_jXU@^U4Rx>7(MOGrg&jn4hKsV`^!Np*NXVip5CdLO
z69Y&wAUG|tOc=?KsbKu4mtp*9Ay6p4m8e5Eo5J^9UQ|h0Z4T`Kor329{qO<K&_j??
zS6}-&2`Vs;Zd^RG`=BK!(Ke7ZXJ&H4{@)l1Rtwyc<dFkP4Z)v1zerYKsYM#6tbB*T
z-PG!F<YO5b1_x&zN<0xhQ_Sl*301}<k9jf>>Kbd2U-I^%rJp_(<YRU5)iPc<1G*{C
zr*IbD$2x|QP_($2)41ceV}6h=(PLUQU~r)_UaTSjmnV0aMhV+~5}(Fb|FInHZS&Lj
zl?SF((LJpWR0_QcyUz|mXGeH6Q`!o)f!@~U>QYb_yX|LB0G2uQEYvHsXzooQstLV9
zFez~vMYonxFD6Y}l`JRtSuxD=40>gOmfCgzj}ddJhMFz@R`k+b)|__6qtx`z5kXNJ
zOYI&A50P}+NH;c_mFO}FGDI*^6frawpXfaSCNNO+PSmxHTGq-po;igf`*D2?PsL-I
z=&h`dFEMVy{VJ#~m+=H49qHh^SB@Omy8R^kLh@iQ-cKegLDP@qSuDtR>fIXaR|l+W
z4?$$+Hc~xR<J5h`6T2=dY=JK3!)*e3cD9ByL|HHPo`OIJvk%KP8X$bZG*-JFAN0ZN
za#gX`sL)!Gq*|1O=&efKs*RFE(A(CnsIS^3rGXn?*~TJSd^JgE@^T^04D`Mt^dZ|!
zUiBGJI-rAM#-+GC`-Hj6+!TJz#Jv#tgo+v@N}$=$+k(pU%Du_q>+M=SN{tepv|}D!
zrW^1`8uw{*lUg+?G!{nYys*Zoqd8Zoy+bQgKVa*Ai$6t>3J92U`}<;GhCMsMag?pP
zmd2vwd}VZw=KezHbs)tpcN_(s`~UY`IhQ-pmk^`%@vQ3-VQ=~^0;3ftrMOpv(i5?o
z5_L46nA^Obx_3G_p`5oSu%Wy5)|qIVw!(024-T}PB8~WB+AS`oiz~}h9?Rr4C;>z$
zPoO{>M6iFkutHZRZldaD|KlXpSc!?7Skv}CCcF4e*xJ`j*1-+&?73xq<=4hz%+?Mv
zjEu@%k@_1MaWfaIST8v=P|sjvPTA1p2-hM-G=fL3+Azkr7?O7lcF4`X`5?3(Yzvhc
zfDLJN-CfRkwOFVfVVQ7Y6iu2y==P$SbF6cfr~XC8#rAo#BGKld+#OBFre<9Jl5qCb
zdb+0p)wHZ@D=}2X8{9ADQw!H`ww-?OtTJEtAQs=Pj_$d`B42y1N(y_n&yu_IMnY|0
z)U<hj>KO0YXy1_RL(m=&;`$QjpKFs$RM8khN6&cij0f{9*$E|9TGcp(n~9u{O=81x
zz-OiEpf?9?4r>EFY^CxADLSKp{Q+?8!Yt$bEr5x6Cp*u3`aA%lnf&8lSQf^``j1*J
z8y(_l*raUkOkuv?NMtq0H3x)H9^Zpoz(Zk{eV@hSFLvM2S0HP;bxLL8)Tv9|+{5{2
zu}DM$=c@k1AafiJUZTG-NOS1U2RMsfPoAfDg-|oC+{(D9rcs_lr1i2(1RicxG|F%y
zbO}ZGg;n9Jhu7dJdL(qB6PfUcM)V*p^iFi=E<?tavdkgK4qU_EyV&USYfA7WcU%1k
z-@6JQ2tYTA86I%w<xI5`AA-VW=+IveL0yb+rS%fjV_v$QS#;F79Ty<R&L{2Z{Y?G-
zXWw)DYb2jj^zroR2KG#zAxV%-VL-APyu0`H5-<%d1{6zVH2l5h7FVYXIcnHPiA+@n
z^y@{wX}3JGHfTuY#q6h_SBR8N9B=i5P2cvfPYNzrmcgbWCCsz7xf44DBk9sSy&YVf
z0&y^Rw<8}({%AwgyZPE}T9y(55kDhI?^lCcK~zP>iOU|(PF+9R<7_CH*zKRa){}Ji
z8XZb>lVmAo5`luQg(~O2m~HP703)J?8LYKPgZ8l5W0yB}UK?X@QE%6<3@lE-{goY%
zV06!g6hD?|U2ocz^{uujZU~eC9^WE8jjA-UeQcW3!fY=!HMtzv9e9PeW`;P6ZoEft
zeHm)|!Qv1^NH?d9Gqc{#=b>IPO(?TBzyXdfu2=z-_j<B>lwYxhU)Wm;MGTHeUTn*p
z>r}vQy2$uLk={x}L=KW{zC<hPZs-H^@P-Wm?lQqI@uvJ|$Z$fm#Gb{9?zLim!c}#r
zQ&%-;(2bddMaOsJi@EC4r60KJUl7;pX0Dzkt9Zc|9c5k^l{7kCu<&Y)`02V~1N>A`
z$!C;f%II)I`KeX6QkhLkfv_gI)#u~YVu5i<Rv^(`Fs|FC%=$`v-(8u$xas9{*B7+z
z+zGnh0w)cNWTZLgXV5)q7~n&Vc$$)x@OaD9MvbiFMVD`WefZm@u$xZJn(bYDxWqDE
zPuB@|Zmh%ab5Hepk-~)vtcZ7SlsMuhVvcWsu<@@v&}Nn9wLZ0O_+|Lg#D=Aab@LEP
zSsCe~Y$xx*GPD;F=*5*20Q(48jF(W*TqLi`^uBwoA#e{_Jz6-sAEQh*=)toS9DN8X
z<7b5LcAt%}X7X%LNFB874I8;LBJFO)3`nAPvOY^pX>Tt{=MPt#-pf8G5Fm8H)ylcR
zAY;29?NNU(V{F<ouOvbC4v;tcM5F>)Ut|6sy*A;&yB<*iTX75@J-l{(ud$;=xtDEL
z%3r2DcxieQ{B-72QZ`lzw$pT_kQ-9169r9{dE3gbJLkZhlqO^2IsHWSUGNPxVEg92
zGv1SeAea}Ct%CyAvUjH8`_@gh%8ahADumAb_!|F57))^0aN0IOET??;!uqPO;RYB5
zZ2jcEJOp)?kY`L>Q``Lrr|@D*hDJs4LREB%a~!Yv&-AZc=6YO*QTJ|2b@Z}&5X|Z`
zr;`o%c4=9Bd&6Lt=etn{&NJx(ZQ-yfqP$jG<+SZLqlWdhg#y7;f@qG~K@U+MBA#u_
zvkyyBqguKa7ftFbY3E^!(Txq>Lbx~LH{5L|q4)$;gVT~C*pn<>?}2^N`PU=_ZMZp;
z@G}TQ)$R*2@4K~CkbFF!8h2GAnL_na_e#t|WPP6Gbrv4*O)PkdQCrEc#CU(ql}hav
zl5c&+*U0HEk<y#75KrFe-0Gj8*105z-{ZnrqLs#h(=3AXK%`mRGkTLkT0|-)DB`($
zZ%Kh#MEJnFXp~abGHZ!IxBSMkRR)K%zB2n-)Wa?KIn=qntx;m>OhVme(-#<Nd-9y!
zZyL0%NTQ6~dgQl>`FI<J$1k2&MYJX%TJ<ZDx1(V5pXZAM3tsCm2dimO(qo@ybSTE{
zp|uK{4coF&@7l5fOP5#(3{l%i;2JNJXL6i<QB8H}I#o3dlHjX55&Ko%*ovn526+xP
zi!b^`MemdQ()mLmtZ9W6Gb=H@Lx!vQ;!no}dk#TC+g7V&3tar(;kBbO#jcTtK0Vqm
z^BN}VeJhnwTMR5To{InVXzzhQ^6&-W=?j`nZCtwEL#}`om50#iJ3mgJC5Lpn+ys)v
zR{NEoDf<7>8tFquGuRC5Kf0EQrW*XKQ)6xLtxpq%ELGCHW330#71MuPQ<I~cvk49=
zXr6slbgNxENLQh4RC^QdE4PnR<IV`XoqoolaMlb}*a?(C{n{ZYIbgRn<=`cJ$*~AX
zqKg-H*w;rN1NqJA5L7^N5aLp`uWxc^%@q}T<q)*9iw<K@5(1JPE(3ZPxP8K>9msVD
z0&ot0GS%xi|LOIwDfIyVUruBgihTrcuz!R<+$d1qWv}!vV|`N<6WUz+n+Qe{44h&}
zOgNof(d`AEjNwq`U(zicku~Mx=e#}i+8D%|nhjBF#4c<SZE8&OI=7C3pRJ^t^XM4#
z>GryJho{w=nEM${6fCiJ1uR12m7RhmlB0<T7#p&C?1R%+F6d-@vq(&o>tVrl<~#b&
zm!4l@V4}7Pk!%S%<DL@$H|aPxj5t5R^F-p}qOpQi>l1yG`=3m2W-sqPNy31=hhRrF
z;WeqgXqjAMp<A~0Rr&gkYQ)vv^I&M7X$mfbP)P9BrrN%cziYmN+IB$MkuQYir2@6q
z7j*5<JYT$OVs~kC9R4&{U{~c31hMjSWX^zGc%olyqk5%rRl`Vx@d}|o1bcR><E)n$
z;we&a(KTu(tl2T~Jb^Q-@g|oo1JMVssb=1@dOU4@;Wd;Y%5xXVcSZ3iXLPV9my+8~
zEyWZVj=xx3H;}8DMNz+)?R=g>dn<IAsS5F;W6j6B5si89dW+<oZjD{Wr)cJkLVWSf
z;z8LO#<vg0WzUOv!KyJwta_|s5GBY9a|X<THqtfkz$A;n`HHvMqec;&_3a?DAXFWO
z&-F8`l5usloG9IOk}--D+^C5-z70h&=$$McJ<6d|FE_YB_oOiIr@v4QKZQL7H?~g*
zyhHPq{RO+h?g)=IZALrTxnr7`X$?si-rUYxE|z67?-SZ$=)N^V-VEUI=)pQZ;|2Y?
z<YB8y1+nJ??A#OlLC@+^*p-C2?Ugt(b4F5)*^Hs-g{H~pd*~uGlCSdFr)Ks6^*M**
zi3sp>H2B~A9*d{`Dv<%}+D!7>wmh{ByvO2UPq3}!T9C?WDMX?)51Mjl5pruYySi%$
zPewm#<ygT}tE2V6cDX~0q%s7Vwu!knJEG=4(ND$8m311(vT!Co{a8n;zVt4ub41th
z#z=+Eh#ZxgKbu;BRvc0nwNyFTSY@6=xS%C8m|c*4x6LL^VX9x|;z&vayCjk2dF=bQ
z!g8IaKH@WOrI6HjTyTq#>qE(mFSFcICYjHa%Gz^uyqx~Xaidhv|I*`0zx#TDTceNR
z8}x25=$a_QeYT7Q1l}QtF-qzXRD1~9m$<dg@Gy?xIV_zVLYb<hq~I?0HfJ-!$CLJH
z&V^_3d~eU$ss`$x1wxX-MHXLi&GW(K94U(`)jQ2jnYa1Q+~UjAqPi%nF}5BSf_G~;
zhxQ-}K6U*Ic!`At9T-^gsA4$doVXcoutx&fo1DeIRq<gP%FiE_p1a&;O`RwWbbZAT
z$>^qQ8kmVMJ4k(c&c4Oo^Ni=0$c1<DjTANirWJ(kdY)8gs8xL9?U@_NQ!iu*QeT^f
zD}yk;mSc^B1EvoW(bt+Y46kpvxhfhtbE;ory(ImZo<89A2UychBJl~1HwSOU9S{7+
zUv6>e6pvaaEV>9$an<Y<Ji@jUF_6T$;aeGiz**}?lN;7u6PAA^WEnBDsWJ<mC&tx&
z1lc2FbXWH)a1j+mbxe|wAA-z{wDhG6P*p-j@SO#?SCq-F9U;OBsZG6I*1Bt7;Nvk@
zwLDWfSI}wkCJ=oQT<BZwIsrz-ftTtIK_+oI=%_n=dH*KzBPD*Wro6z^@FqW?qhSl5
zcL19oiHZhC{w~^L{w4~a_<7Z}DkxC&-t-PQ2yZ$BZ7p;8JnZ||?ta_^-5$9yxYk`K
z1sQ25HA1Fp*w#2T{%>HxIQp3(uFv3C!Y>lkD6`L%EYQxF7|;2v8M|vb6M;MZ%wKF0
zrIH#ioDrrlj&K5Le|4Td1QkZr!V!9g_(l2@r*;>15I~=@!PW%N64p|6wV?5Zh#|j=
zgsP#;v&p3UjZ)4M_@2PwJK&5D7hTg1UyxO`$t4{zS9nhIKo~-<UWis`t^HN@e7O4y
z;Wf_n>F4>Tu;9XHR7#qUl&Eel-`(Cg1TD#|^B~phcG37ByoRAMIh<~bwyVhcafgkj
z>gksev-gOYyd@1qW2d`X*0=5R2YHl5D%6oBn`Pk6*a*H5o2ah(`bDOk=P$wyeZFb|
z<&<`i=YyY@Lx+}aEhKo-hFOtczniN_;e7K(_8FlDc5N7KYBfKoMVe?$#;Q{rzdiQ(
z*ivtsMN^s*x$(FvbNu|mtM!uloc>LXwUPSS$6h!tyIP-l%R=RG>PIRGp3yr*u;ICb
zmhX{Y-=y4ZKnU+V=@AEd7X4qjyJyRUOosqU$-@8{CTIz{ZiZ(W8_9qj*HDSfrk;S;
zgc723yHUZmt&qVbQcITyYAZ}?N}ZR~BgJB-?`hAlP=P`R5QZN=H8Mp%x~!yN3#q6o
zFOJF%-?T;<KJgN)F|*XPXOq(1sK}Yx4SkoladUHo@Z$x$Cxu&;T{%XTMb;#I6z$#j
zU2+$)eECwa&BN}JZ$<^9vAwP%a55FL+<c(h`s}PeHg_99E^Ja=_^+8-(w7+XT1cL)
zoUUGwp!4b6Hz*DpbbH1emRdl5o-_@~F@4`ao{2<!WzR>=m1qoZvbK5=@@koh+nHj2
zbOi2`=c87VhRTSuFE)zMbXw^4aI!Mouxit%ZIzL;b0D8CSFd4L#!jadHM!TaAS;P&
zFL`m{)&Y}1Pv?5fe@u=3Q%d#i@3e(8z2w)5eWP%94ReE+hfH%DMZz_0hHh^3aXs@0
za%lI^60}lFq_xafHLx9h^_HzYsqma#(>%w=M1<U=GSS@rAt=D>S_V{k{6PMRRuwU~
ziIz&Xga$|WYc}yUKFo8v(Iv}tV{KwBNb%k1cc>L4Q=TCYWrJ63oa4|nVuuE|1G$|{
z;6Uu#+_l-=hv)S44_pdfD<`s354n2m5q0o4bBG|V<tnJ(jt#kl_ZSFkd3bd=;y?Vv
zHagJNIW(b=)VbKdJxVn9!jK4c`t<WCJEj+}up<;5FT-T-%2QiTO)}-;DqRs3Pl)vL
zT(>e@-~Kr7=(H29&dWRhS%^WiiC&bF?~DdbiF7#TEP3K2IHdCOdV?Q#nEPq}qPL&2
zcm|B`wUez7DDdAi#cCkIiZ!JZ%X!dIFlRijj2BgMfmh4QYD<2PX}4lWu50?eb!v;|
zI-6+8q8(VPnQMiqpfu2p$&KZSUn8frX4r!!`VdLH3U9)-^OMBM*$Z`b(aRk}jBZbL
zMnn$gS(AA4ux9q8g=S{yr}B(K<1!qpgV_<Ibq(0^EEj^`2Q$I~+h(#c?%`uyTXUGR
zs%hRN*z@6zotGada_-D3Mt$J#Y8;17q~7AcJ0#bSj6P~_o!hwoGYk3OQ=hPVo$7=j
zX4h9%>!Z>E-!CI%`ot?nTJ+#@uQ)zm3VT;9M1cYG%Jxo$dtWurqlC3|G}V0B?dAfF
z1mWla;Dqo^d%kjho~A^Gme`9idCfU8Ih!m`f%;^iNKYYhB1WM@->_|DU&jgjIsQyD
zLl{J3PGk_)YoM5QzFEg(WE)_-l~<4|ITsPYUJ8pc5~xG^XMwHP<X*ug&B-btLru*C
zeRnz}%`$wetf8{QuWHIY;1E<&we^IKq+xV}{v&)SVP8*qU=XMn-i1yBwC+DYivF)<
zNuuE1G7{(|v`8<3mtFwnR0`B;)-3=nb0>5yUJHtz*8W@qgzQa+J&|9Uvp;W%W9)6N
zhj*wPygCF~h9YiFAa4)ieh^#WmYvyr1N^LOT%<p9ZmR6vO}*+n5r#$sUnNrqC7rFr
zU4;tXw34lrI)za}bne(oVxN(_yE!^!Hx5Q|@;vOa(h6JYLGjKQHns~7ossB@%~kc+
zj1VzF#-&SmOv6o;pTO5*z4$}r%e)6u0*ZC2C`W81H>mvyuB3NPGNKn9z=GZejyTZ}
zGJe*@tA-yEQU>73$PZ57Y;lGO5*sBQ8y(ASgA9135S-aF63EnXbX|!*r>5`k4M^4v
zcVyR6f^E2S#IM=WJ#rp-Sihs1->!`z_2e?>A*C8WrGZ`^v75xYj0Yu69r=r%o&LKa
zJgw$ay=@-xwqUMWv*&R%`hrZeWt+-QsLs84XW@_FfgFRPZs(VATWM6$94^llpw1UQ
z;9jX>=2Nh3Ab&K)U4Gv?Vtlv325SPV5lUF`D6{2G|1hyG0im@~9Y(}lznaT^j`=oS
zx!k_OJ!?TR4jb79J@*%V4xD)sr;H-+N!EZ4uID^Qv!Xv5uCJ%Mojn90yWl1ZJZPj%
zpdE6Y+>|!?ZO**GnRVDZa=Ftl#dyRvs^l^PCTT0>Hbcn}I?U6LTkQmglpuR-sLXx$
z_*XJst>ele63hr!4=3<>=x&rT1+GjL$wd@Kaf#TTQDHdweHQz@`ylNqH@Su~x!|pj
z6<xMn_L_HZ5@K*-&rXLqMv7|fh%_-e8uq$tc|T~Ih{H#=8Fav-WXn5I1HrAlBKU$@
zS6KiDNibiqRh1#%l6(GZ6Z(JI_u4G!UOH<op|%Ce6Jq$JG509)pFV2GpEeo3KAFwi
z{Y9HA-KQ&_xO;^{<TU!7zbjRiO>L?um~E4a*sbF>Omxve(|QOa0L|o<6J0g(en=VT
zNyeJG-IPZb`DSq<QcJ%Xdbu9~qxMV+4%gFvr2^xRJoR=xVS+C7)^w5GAh^LJG;+s%
zX{+MZ$d1CjuVxNGd;a?-#aClzgWSkVtJss*<U6`}01lfs-`%ucfCvxGI0SJd=xF(5
zO>l8s->53w7P}_a))BSy;o(ys-dI}(ktS5Vmu@ObYF#30C((K0^sPH9uJ}es^~W!T
zT)C@V{6!foC>ts3A@d$WiP?Gr7i@yt6R)PZ1ol!y`c4r&dmU>U;$811=r$t1VU)XE
zGJ<{r{<LLO*V|j9%fqx^u$W~snc!P!UixJF?T2{sk?DbkO?={Ee{C!ymXxp6*KXEi
zi)kFBZQZf?GG8WmFpSJ?c524-ubJYDVw|Nhc|RNC<*9(+ozyoTdFNNR%ljrziu)DU
zkO-dtvowbI(ndpR{p<uJC&E{uv=^z>ZL_T8+H7kY*@#D}mQEsqzJ%DygcplMTtu%*
zM_x<dfr~;n`@0uJmFN&f+y`)(R7<)d;m3?0!ZMxWguZ^6G<C(4*R$t;LG*evVVo}X
z?qm^3@K@n4S&YRohOZdBAXyV3=PBve<`M&x8i$fSR-%xc7P&*YUy5Ro?|0AUSf*k6
zSC3`jG0uw5mQIFvjEIf#_zJA%m+S}+n)yLDxw#qPyiqh|EB!Dz2HWoW5a7W7gr42{
z;>Ku-V9DrB4zFU89Eb>fyt}IP5EL#U-y;|&tyikE&9N9K<*fgd>r30WzUGr}ea+9E
zD8dOYZ?38CLIq?gH?^rN=}^gYrRAzc-a-%zp9Kr=8ppB?OI?}PgBc)WKwo&c#)R@O
zy_ly!nNtnoIz;S6f6hv?m*b!PI6-aa$<tG^M=EhCe>(Zr*PJbdZ3+1<p!QO}uyFn~
z&5p~UF%BnlS(&UFOhR>fRdn_mZ_RA7a~XUo#g*x;jrW9z%}R)Cqn!mrvblk>ETRGY
z(C3=fTr|f>Qq7(wPY7Q^X4M|8dRtojB+FV6oebJlJdk7{pw|Aqd0CG+j*d&S_&rRG
z`@^!tlA;&$aV4qnX6??qacHl}zT3H5lvFLv*C?W9982WYS$w6AsB%(-o|a?9GA9+<
z8(sAFMM-b&*SpQZYkIO5sj*2T#c9rvjH|!0e@Aj&o|8}GZCP^C_JNpH679LGIZ`&g
zUh`<$fa;><s)E_t#i0rhy0ezWJh*X|m5`UEx7|0M5r}BfyvDOP3XOqck4=tPSJEO(
zls*V+eWmL*D&5qAgxRR`mVz*2y<QHIw1&Vu!j&ZrY&B~Z@gIjUkgF<0jxZKhDU5EQ
zl*-LK7Q38M8~R3af!4}a#8mB1W#g&eoj`G`st%?w41v?GpmaQP4xYZw*Q-01M}6lk
zmq~R5J1mQ>K_mYP1Buol=#P7qiD(Qj;B=V3_F9KxmziSp+r9EHq8<Cv4<g53`N-hY
zYK$$7geb!K+?wm@RBJNxA6Cpmce4yzcvi!ge748UiQyKL=nr=#rmw*-o^Q+8OD+1c
zbr)b2k4cusj#$P2-$}}!HYBUWy+GaQw$UP`#IZaFrkP+*muRRaY^k%YXqnMygDuRf
zsZOX11eNj&Gkg=d!Gj_r90%`IJt`cf8<2~MW3hRZ*zvX4^`LvB+AI9M9(%YKD&k>I
z_W0nJS*>^akKrG(-+MFd7>>T#v_MLnYg>R&#a;dEt^KvktEcbN?jXl~#JP1GG_x0X
zr*p<DlL)nbn^oVNnK#zsGCz@l{Ht|@#_rwZ4fFNfM;(IB1*AO$sEFpOWZ{8U!2Tuw
zkKGL_kC-FM%_0nfkG73xuloLEo|r$qCzhL#tF1bX(&|#H`zQpq*l>-pdKvq+_fu<O
z{uLB)cO;WGyFJHL!sF5W*&rXXMHQ>cQ!h3R^ZkQTck)7u@5UiDFI<s-Nq!Gsv!-rE
z#-Iu~E~y#ez-<!N&O;(rBeqddsjt51+~=9V6t`BXA76i9Oc8@rfG5b7>1+))*1<{O
zPz&bAcioD-o;<>|r06^wgt=iV8BQ72gG;;;h^t&WzaRWA3wp69_>1t1d~?&C4LsQz
z`tblfY*K_)RDBf?XcEp%L@x|dqQoZfX+*y)jW<Qin8AhUTe5W>&!QxxrXN!)w|u>{
z7WFY_Aby9+%wFKt%wU0T^K8GKM30%;Tg)J-<z@|I(&BIo!2|bN2~MK|92yB;<VZr}
zhNq%;*Fwj%66@M1_45*gz;f^_FR);_k89|Sj+D6^{<ZUsZCDLaXYLGV%nfFKIxFTr
z<OSnp!C(XtjS*9FD}aQx-)V2jo}&(ERNpCurpdK#>!wU~)Gv-NjHfhf2_1DnjN1QW
z5~K0EX5Ra2x|4W*0)`yRw3iDa+S*6+dEYni#_kyvZ?6VzhCF(|Xl!8=xu_`PURcF{
z*U3`TC57UEyuCJXgUe(mX06C?SPo(bPb=d#ej)PE;pPww_s%2Lc>IC>F<O*Q8d7uN
zoNg+x8DRsJSSlFN6Ijs3yPtl>FV-=>F~n_+W1U-5YQ9%-j~O_!l!b_o6HlU6w{+PC
zdQZOI)WH67s?X=I!sQ>rEu2}cO{|T-e(mLZQcZK7Xn1=nzA{#uF4e8|<AtNxKe&<%
z><0_iIJhz--Sg#pJ2rM7P5{WSB)*`U%;qt`!=1CSM;j?Yo5M4q$WsCA=Ij)wO=O*y
zF1;hjWn8Bm7v)j!>=eeU7xs+OFUYSWFB^OZ0c@sZF>wo7+NCNJT5oWUxp2GQTZUnh
zj*8`8pA^?_xvCuCtPgKPf9mEDq~oA9AK?0`|Eit8kj5NuL$DV(B7h}sm6=?g_NL<P
zIiXR$jT;Xegzptx605%GvvPBeSjN3-QOhc*8^?dNMQvjqlC*ab-rvtow@O|`R`qFH
zUw0?1D{-T$KtQbZ)9fdtUK;M#mklF(tMfs`?<YgQz=6Kvx#5E}F6+V}k*VR!c3fZY
zJZSF@AJPe+iL7CGCz26uxRjTrXkVw+Yt2PJc&j44d`XwVH}ece&}MvHh}$BN6$Z@f
z)(2%v28)v&ocJph9BFR4i_}IMi!`p&)g{x!Y6tgJ0>@Kq8#i|SVwmcdSHUsBmZ4j3
z+Im7mS$$a@_%XMMjLq0!x^A((92cdU+%^7?c;ENeq*XukWKWe<mPKzY+_sCKkXq>n
z5#>#dfxrYvLS+QQSg_EF*h{BV1~=L%CUlskpUJT<X>6}t+wap!o*VQDu>A}z78>*-
zsvqRIqs7E^pHJ#O|9#ONGI+XZ2+zSTIUGd*kMl!163QKX4V|WInXNAa*V&ORcGz+j
z67HQHcdZwAMmYf)Dy8GiEoDgT5I4zkj#ge8pS&j5hP*-HVerWYso!MBFWN4id%!z-
zQI@}5BL7XrU7B+b9u-;MQl5&M8l7;^m>{}cc`A9gYH>xvr@IPr^u+HW2q?atI!;%l
zbi<OO_V3qw@9bn3C>%tiE~X3%L_9rEB#1mSqKBgblQ5c2r&$jvom%gz&<fn0Tbt38
zzVlhRcLOyfKKLqI=-oD5t8&xy7Lb}N&ve@`6bICKST($=)6?Z_hsjS5Q(W^*u#Nnp
zD&zSjRqb`wEdq{arafBg1*N3&snJxR$jN*8U)0v$>g?Zo{aax7;1AvT@})XzqAEi1
z+dF~CQJGD1t@9+Dniid*hVzjU*cSOHN5rn{L~9RxNgpSChBEH)V0D-kE%qz*NT?ED
za@Ye(52Qi);@J5*WrQZ2XlsPobRb<hrcF8)I8`V@Wk3}*r4lx+MBLUt)%T`vFQR^6
zYIA8Yo2g}8F?lvSXJ+MqvkW+2bILTVDL>tb#zxeNhq?FZRAz<kKCtU*Na+nM02%1t
zH1z-7R<5CW&FF?Wun{mcx_=`yVA5{yj4^n*<A9rGX&DgKEw}N1)nR=g4gWa2&vCv1
zFphHgIG1i+S)qY#LYi(-d8sDnE1Ije*m&xX)FkUzuVq*UmL46{$1^LE4vM||+*NLN
zb?H_1SCkmy!vy@%e)5!37S~7CFe+N1ielqz<2f<`t4o|M_@uIYr$7f!4O!76mG}wO
zqi4kh*BNeX-g{r^e6IZx4~_N<BoQBlu{P&@P|WrE4|FkHp>MqiCrZT~$CjxLi%By*
zf-_xlMc$o#92**ct(xe$O^X{%S`%L2J~vwujfRD}KyhXk4_l+$?vS#=w%jh=BdU#0
z8y)G2$j?0<!s&ar4nfSpK!d6hbny0PrGI_TPTeXZNG-o?`#NN%*U2K;*OWM%fru|Z
z{uKQ)$whTlDI2jwf#?DWLqsEzs^5xk@Is%j8M@;OwHMGf3&)}8Drw!28g>`0dqzCn
z3KB1(aSM66PS-GsEy(g&Tt0cYl?d+As%9p4L%-7xJJyd@+ZXFnzsn?J-<-3NTSxW&
z`Hx(rg`sEWI~TNw7FmN^*|ZIUGT-vW-)v&=m6MV@(0zoG`jme)Op|e%yl<Ilvr7Tk
zhTQ{`y17Tql0IK+R*#wA(cQD6yK~U^2q8AX{9y8f{l4P9yoi3+UHJ`VI>Ok*srz?M
z*Kx#S2zF@)pwxkfEo8=qdV5f(1@?R6Y$;41wwzaa5UIe={rSypavT?<jcOc`VMp<E
zNEyTV3lGbbL%81RTyiATXR3-4;KRH0uyeh3mX7@qHE+?ANhHG~FPT=y7N|zFa@i32
zt8U}Ef%(P@@0wSS<W(E&T{aid=L=A%(ARUB2Y=da*#E+EAxzz`%v*I2^=4wvG!rOh
z(iwln>-hVOg$u#&BA`q?+h2rS@+Q4@&tl6ogCL<;j1Lf9HZnpZ;TIG?T&^dtUr3-r
zktYpv@*{i)<se^Ar&rWYGQv6^f!4c4=NC9adcl@-#Dh$=^MlUlL~)y9e)>Dc&utr)
z2RL#0WYw=>V3mX#UTw|o+lib^F?ZeNC@71^BG(>!hhFqC&+jx8wgl*ThH&J;tHgnU
zNeT18Y4=^H2BD=!xZadgqqs}b`eMU==wKAmeCcyXsk_V0ddiEec+%okN<6<0_S+qT
zo7;#+&g8eEDRWss7xvE9{FF#DO6#xAijkH?;v^5ryfEzUs8yLw+!wb|ocdvl#b>8l
zBEp_YqO2z3)a&o|zPHV_yjzb>#4O$pT$rC$>1Uwl1s-roaFmT2*SFFufo_e^9)eUJ
z+(}ZfvfN0u#`4u5jU{a?%{l93b2WUrc03z3atOk%QvV!E(m=4CJ~X{w>@Ab-)9FLd
zz1(|~huc}0tFX?>a-sSjs_|1lZ^PBuu*SR7MaqLY%8$lnW5$s&r9#v33*-0h4T$5Y
zj0Ks*?ut8f!~i@6ByN>)t@40Z0Ku$}_MVzc#2pT9!ngM{v6U2`j9zp~7|_*>X+wAS
zaW1Wg@+^gkBg<yRicLzgVIC&2y^wm!(Xme8Qv^!XfTrybx|nC-l>{9kQnV5{U(|Km
zTYBf!y7%<kXLS@YCJZL$DR!3hMe**hw00;-E-tpg21^=9s1^;h2gPOF=_2E*OpR<}
zCq*}ESLR1WW-lswhQuMSxET*E<v4q$8OYOiv+q3)yCo}3gfpD(J|=Yinq!-DnPE*R
zj>Yb20b07^uz_2oP9658P2!yX_r;wXJocl(xrl)C==CSl`tK8_3XKkuB0r<aIjUs#
zDwKn5xtYpKfk!~xXCt2ikAUdJJpmp8k!PuJzF=5uN4a?ZG!ZRLRdA0&gfnT0o+<GK
z#n<s3=GVMWd&i~B_eLwebtx9g9T;qkd#2k!dY&=DpN1%0c~XnxO(<4(#X9-Wmew}V
zQK8b9U|YTwZIBIoVoB)p!m5FshO7P`<VAA3X?Rsn_tcm%V9n6pEo~H8j3>FsQ3xEh
zg^1l;nY3CZZF8{XuN3o?JQq)VrM{oo0mZVmXs{{cxh0~&p0qC6wpX&R<AWEYF4pj6
z@Rr0=)QWq26!NMYhjNL<Y{C04!+A!6J>3}?4>m%1?ln@)$17js$m~AqWymAhiq_CE
zWG{wpt9wThrE62`x>G+T!YYlOjh!hY;22GMaW5D6=!Yco4Q=GEc+1Oj8N-kyxDZf*
zc*xQJiFo|x_rE`xeI7yjrggnza%A)=k0$V7ixY*yOJf%XD@>V8x3dr>&18|57qZw!
zi*h>;*PV9mGNt%$B{J?e6i@J69^ztqN^e7u;$mt|PkAZe<<-8=0Q0V-NU5a@=#8dU
zxLA+;pl_%r{lPS%7-NufOH<>zJ%51<2BY>ST~Mq>u83yj=cp>49!s?9w{rzmgd)%x
zk=2~u(3Sbc#R0Fu4;xSQmC(T6)5JNX!1vh3@!vYET`;z~M+-ctv%Gq6S+4H?fuq_#
z7nts=jjNA3y|fUD6Q>E660E+Q{-8HsbK7!a<;q>I>`DC6;2^g>{RUY_Lr!#^1UP(E
zxlf$V)v;>n<rmJ&HcPG47YjfzO%2{4e@d}r$95%&xttEeK`i-F1Qt?j08vZm&Q+**
zYhF;((~0jDIa*<^CSrP%vM{Sqh)j_R2gbYOF0N(%;G^lrSA}@GiBRR}<*wNU^u^f@
z=C*rN2ff={9tta63T<>|ohtw3)=+d6A2h`b?(r_V4A@tD@Z;1Td9{}Y;vI0zVFktA
zJ?}7Vv0wZq+PgPJaAPR>fw9mB%oO`UCSw?AV~^o-K_!Qmb1p(>VieyZieje^Jfyz#
zK3;d)Z|Th5V_$Q%6n0n9W_GaC@OERGG-v&UcS4^3-j`VCyBjEdr=nsPI|bWN=1MES
zTBNd#delH^n#I;k36G+kcTZREPQf*)z%g-~bww1<I$jO>Y+l!7FVFVuvz9k|MxG+)
zaNe<MtWe*0OO);P4vQrPuIFsOZBa|k&cT+ErIo&HFqEY}>JtFrRAk>wQ;PZES6JvF
z#$vU!Wo0#ZM3(htRep9y?P!!X0%Ke`a3@O5;7+4ab{Q!}fn|)DcC9}j==yU|V5!$Q
zj4Vww=hmCD+|n`xdwTH&TRwg5?evxSpsD)tD~JW9N6Z8c-gMhZOT8J1d)t$t{@^oJ
z<`?C+2?^h)bdGWzq@^SYR(`HOqbFOPF5G^Fnj>mY=ds9&*J<QUi?Y=StL(;uCtm%A
zV5Y3NO4Ya#b;i6puh+x|m7j7FB)HIxm9*=b1(pU>;b0=mWDrH36i!gG=KVb~EGm;+
z3cVmUF~YOTTXgt{oF}b!6N6p!cb{&Dtot~Orh|b`=&2oai|K21MlW_LbScB-y_fpm
zRBGq<qUq5c7t*1FGf|G@K`#j*UiHhvm>1j118?DejD5}sd`{36Mk3FY`;}Z4v=Su`
zxUzu{G=ktNUUm5x*w6To1@k&)MC9C+n|c#fIr#>xt(&DXUD0Zu9Pbhkkish9L8Sn`
zW?pyGX@bulc1emJDd}Rj0A5?Zh8dWWZgmwXXbTp!dx3&>h(b5*1rwLAJ>ePt`))ge
z%?otY<)rUMnr(!t2%BEE>ee$h!7#=<Z^c)HmfW4n3aT@z3f1D>hWeOo&1q(!M1<#J
z!~!`!0$Dvl=Fx~!Op@V9#|NUQ+s|0_Xr<X;N)8QT@ouwGJzz_3ODq4Yw!ubF++jX_
z{`UHOccNqvu#L0iJ{y{lRWGV|@mdc5h9X+1WWvK~!^=iF&rYjqyd&8nC)oMF+VFtw
zmpm@ghqs`+zpMgn4++D;6<YmQ4GyKT=2#o#V_K6zoNzx`<(}j{N7^`vi@;HcC#pOO
zRT~mrUwkgP0f|SYY#sL~@jT2uNCOg&Sk%4$LEd+VHMypJ21V&02-1tvl&T;_f<#5S
z6h%Nlh)6f|-lL!(y%(uUkxrD}dk;l=FQF<mp#~D-zMh@gotZOd_I&&8Uf1q7e}oh-
zqP}_ZJoo);H#n4%ww!W3%%XbR@F)^(k+$la?%l8L=k6ObiAW4Hl{3+(*-j6VUWSNU
z6R0p3f1!L4@A8cedeUNNrj@@>^03`xlIDtRty330a>0~*au^!4wEfKWq!m_}ZO+fV
z%3-{p?mWWA;iX4&icB+gX+f3ydYYX{4Ee4r9ce&=#vANAMBAKefHJc>eusOwUT%+X
z_P(R11O3j-SeBLt$rrzS8{%D@znHySqr&6TRDnsTDuCHT-eUU2>~-E5e1yKUX>>6W
zC-CaB;n&gn`vBs##2{=t?JD@D$Kv|^0go<|L0PwC_*OrP0$jb8Y@~WBF<`qRLeGs>
z;-xi=sHYtn36U%Ac(!dR_Ygk{lup#Oo|_vS6MSs|m+co$@bU{9oDS*`W!%tndfvaJ
zuEOJEZo6YZ&jfOK{~#hQsL5b$dwaFMI)wBZP|Wx%(Z2$UnPw2R3V(%fP7a>$7iIaa
z1%)J|O{$T4Oc6K$LS+M#<!8r<(TT_Q_6%wDTKDPh6RXlmXK59LTm*p}K<xP?R2@s4
zlHv^R`-vsRh<Z)APbvoQID7o`<Zcc7(O?G+_WcG?Gu`+iJDfqN&AN0~lAgQ9R2TDx
zVDS(@H)+x@<kTltU150%e)%jru15$~M~u3!E4@N>)~URGD9o^>wOHSnUSWO%@-%S~
zflZYV$3HFiSHln*ms^24NgJqKBXX7~fXX#0A7=OErcH#}EeA#MPH=?BdNEKZS!NLH
zPb*PQ+e{3LI6FGJa=EXhKVUGHaUjt&h7Dgwe9US)G3H!s>o~<NoazjUeYOlM#OT0<
zA%aY&@Bkl3_FR{%=_l(-_s8pIM@P|FCHF}vFDl;W#uNkRy&XMD#US;Gu>prm?v1*h
zS>Wvd7WIqr_&;&`7s3@4+yjqVZ(+KT)>Ew*UsFRz#%SAwxQ-~vciMGgl9wniwe_*X
z2KpRF_qKQ+>8NKmafI&5pp&eZHap)+*`yq~qw!75a$>I<A)_DNhFmPjayw+`K0klS
zb^hY>%r!;w=AuYeYAWA1&n;U3t&^6I5Y{)xiuV2V2G72|WhAk>!)*6VW&R7A-kd-;
zf%C(4GQj4DEd=LhtW`M1#*a-KCoZh*5ZoZ&b{Kk0qE?@N$v4ogZ-8pXl>TNtJK^30
z+QR>Cv-vNr!u+2)51f^cWN23g@gW#nEPDn8peBr1Wi;h0B;E2hxqhIVr(tSAlzyg0
zH3-+iFyr3=aQ?Z$x%CEqjnKQ{=);_#LYA*{;0#{BF6brgjtq)Kg=Br6(Rw*xfkz>^
z?^v;wJ(%JKfmm*SOeGDvcMsu#2q{;dIC^nb<jO8~G!3Sq-k@tJqWkL1rYl9wL88wq
z7!{TmvGRrEd6tkOY};UCDV6>vcDLvof!%zNGktS+qW1^qcmDI1xBB<I?2hAaXPyb~
zG}UQJ0+1pDrtACtzx>xo()_*|ll}y~#i2OgyL|t&kNEU@-hjAje|10eq_wj_UYd$S
zNjeJc#hLnI#8q_KYh;Ngj6d)`Z{Qo4Y3{vRJG5OCZ#$-0{&Fmj)5p~e^Xc`l8#s-c
zh4Vgl(LX_LMtrv7Z3&!H<6;AL#-cJR&R2DwOu3m3(VI#yHI@kuLdqOUa~W_QJc|dV
zb)o5JC)X-{-8L~NCHK#AL*J>Wj;0zUkMCKl$lJ~Cd6%Rdpckf=_3N-m49jL$Z8j^=
zNZeLarz2Y{CAX+GUvL(v=G6&czIcgdZlH!DhIP1J0jKOEjV`J3%zkCe7a@p~+RKd)
z)L$cAF4%c!-u?X^;5lp|)~Q5~N20-~$lx_}L<)PHn_HDZ=1jj(8TUYx*%hJHFFqB-
z=VaVuCz1($Ks8y;?tJ8&JiW63Jb9b7&1n~xZJ{v5-NDiNblIYLv(9b8!eoMB;k~Ed
z-O)bS4aK+5Z?xVjwbrll<tbOGbG4gLN(<DFc%E-w{+#!Hs^t@>L~-@cF>22m3{}tQ
z8dBr}%IX6im-}ywd+gX+DkR)MIsCYGv$yoiqCY8DM1_P0WXq^zun^B=s2?K{sWxg^
z{`MnR>7`h@DSm#JKItxyV`}V>k0Aj6-aselm*YP0jQQMq&t&ap8aG>{g}i#A#U`R8
za=wt5Ya#&3-`;gAKhb1q{d#4&5x8+qbcj<vXy8jJ(%RzL2LZ<zO}(+=C%fb0dcKjm
znT_($nQ2FNK`DPz$(&0{OrzbdK9x&`y)DG_&C|CkYQV3dzF0#{S8E1iX1oiyhP&yV
z>{K~$c+|Zganzb5E~F;8-XD%krMqAoiJBb|&fWfc+aS_qL~Xo(rm~i46|WYz7VjHz
zrPg_CB-Agp2ilNO2|m4E6b|oXp?N18x!qu|F+>=u>mhuri=Yv=ld_#xbgZ^y7rP5v
zN0E&t3Nb90`Lb<1h<m<qJ7(iR;yZ!|IPV7oWt3*L33QOkS%X~;^|b6^&%T-yQ(u3{
z9ww?f>u&JZe$VkHocnjld8H%%cZclqZo6P$gWh&?nW?f&sgqp?JHDBa^}Wa$B<`bK
zA$z+KmWYXuOl`+Vs=w9C4~gvA_oJ<o`<U6tg<;(8TBbO$zpOlbyO?kM=@f-3!qJgP
z(0>LgaEmHNoI~auD~X(Y31JvQhgYylm@pP;2$68)f-yu4?Gdrn3xp~0`#XMH=F%HR
zVJRdEEp<9MN_Nree*DOnJ`Z}W=AMMDe~*ENYN8>^n}K7Eckp+ITu$soWGyp$D4)$%
zCI&m-=mEWyMuMHZ&%cTBxY#67Vf*qY$as{uX>`}lmGy_?+Vi>I;a)6}qJA(xi0I&4
ztuQBwG5m1%t3=S-ihNYV<cn$?Whh1AGR*TWG5LKG$E%J5j>ZSl6cj<K&t{DhV;%!|
z$MMXH&jDV(W63b^Jc;|-Y{Tsks4Ky896}3!rTJFo?9))iU76#KQ=9zydPxwEojiZ9
zh1HxHxdyuuYqiZfCkJ+Tw{2qnT3t%(sq0Q+R#{ER{OO9uqW#P2tBOPAdDCjq0f{zi
z4Tks5!sU2i9PsJw@@)#_Ocn0Ip<(}kb5#$WOY^{%E7PF#E*^{{(ZJ{<OR$yF*$XKt
z>N4M|ud`-8+NzJ=F<nd#BaxdYVQ8zDNzMv&+FNtd+qEh&Jefp)#cJ>_>89CSi_8!R
zs2s1)kf=wjdAM+L)Z5$U&P;a!Rc(Iv<%W8D!4i5<U$1j`_WCYe7Y`-^{uuji$=7iE
z!Dc&4+}GJ1y0)WS+M6~zMz6QYofN;_intYm6QG$4Xr@(gDE37>8vgjuz4kqm#ws|G
z7NkK0O`;8ywIl|BYmDBJ2?pBwAh{x8@UqpnswBJOVzt{um+n|EUiHtV8PIoVlALSi
zENQ{4u}T(vSujXDy&)NUP*>G6)g->^h^I<2zn#wa%%ftgvmaq3y052J9LYX%u-Ra+
z12$yUM~Gw(bPF$wY`<1%jnDw`qX81@UrMZu%N9kajeEU4gz(yzr)LY^*{TvRu)RIM
zFLS*&_8sT-7b^VZg0r?e*XpYPg|NzEygZ-~W;Rj=6vCw34So}G=mGttwrP2~^*Nr*
zi2|Pmzrjx@ic(S?D08HMfShe*Moy7We-;4=eD2<Eqpqw}a%7Ay{$e9)v@>aS4)m#u
zY;rPDnto*}XYu8EJ`3A-o%Z&n_Xs_7_p3^r>-@aEOi4;Sj#&h^zB`m?E9t3yH~~yI
z6ggOq3y;=WwYO)6M6u3;2fMEuNk<xc7bX|1lc{q1s;i^%ozI;%>r8ss!s|3YO9Phy
zT@w0f_V%e=q)F~T@8g_`3gx7d`z-=Qo%fa}b#oYYhnh`pF0|w@o$^FVe&%a{E~z)^
z0tHXWa?)SHq5oU=5lc&gU{A~Rr@zHxb+_oyQ^u$)g1{4regi<}%rbwcx?b059QCwF
zSj%l}?t`buu!o=*$hha-EzcODmrYvo;8vQ9n&z1X9#B;}%_5lHwLRg|;iS@w-wNC<
z6kCb>nLGoEefiOF)F>x~9H+Gb@k`0tVA+|e<(A9OrNkt+3!a_d%Qs02d3}Ib-DSpR
zZ6xW$p;mV_;dRg&FnnfS>44@P+nqzab017;HWx9NyMzGf<CC;BfM;v(I+Ff7B;wB*
zG5<ei@BX?DPDBahWat5PSPP(TwOI=7AL5a$>f^r8ZP?e#{c^?sfyz5lqBS>J(9|Z}
z(*FGo3@b`5+-JB)_$-D>?&NcHwzI72$2u@kb!}W!x?>YF56dxx!NYB)W1eaJwqRF+
zp!$;m*p2ja#33<P6$O;J*lu;+XWoJ>!lzPnYq2*lb?uxq^@`Ec{qXCh;$N<RUb@yv
zSF{QQan;#^Rh%#{K<d74P0kR}Puq62sZX-j88M;>h#jQ^#&^s=L8D)hBs#dVW4|-t
zs6Y$f<z4a30K_P3xku0b=F=ofTT&;dq4#iqI#iObyzR=WZ_DeUOr3v?{$;j5QHyjp
zMD;`)mD@Fny^5}Tj<yfB$sWO*ev}>hF-~VFUfdFusJ_KYOyc(PzLx2At*;V?s|-7f
z;w;a+#M@q4d#gG*#@EysI5rvHWwDj8t+YamQ&v{;)!%w%Fh7uOdVL}BgLLnlD7&^w
zuJ7?9^uimj3kxlfsp(_wQ5Rg?*cFNfr%jC-hcITXEe)a@NlRf-&820MN~}sPr=7*_
zh*uM}Y4}Fp!>wIs*}4b2(UWD8Adp#%1Bl`D4TiCG0iEIjFk^IGwmcI@Vl5=o6SsIe
zG!xbX!VwjvL|l2?(a=Px1m_<U&Op`!Z^bC}udIj1%dXNuXa!J|1q?SmUkrQXIOdpC
zn{ZSXW8VXw5*3{DiQn;)=t|OGJ^1oW#{EbuRIR_zs0pl(pDR)@n7@p%C|rbJGT8Lu
zotUgpGa@`97^qY^(c8zjXve?2!@kK^QO}J66wj<4DUZIDcjJ+pC*!1hMLJ@hZqsDs
zSwdYT<k?4vkwToPImFgZ4ZHv|G(0hM-Yejkn311^rf-bT9}myFPu3LrC}Hdxa*-za
z*erul8}qIYw(fcALYn@7xc<E~7iS(r+9wZtU5vua^!I6_onn{w=2k+s^OjGE&viVd
zyQs{3%u#V_l!d>BRtz}e`+`mgB_21VWaKmn|E&1P*8Zmd>Ko7wPo?JotOU`O+jVd~
zmy+WOvOfpQ0|c}m75K5g6ztJt4XxW#|FJP$HS6bS4JqSq--c<1>@Bpq-)a+aE1pHq
zsNPh*6QQJMtmGheQ;AqF#|ru-G1yLI&NYM1TKCE)GJT;ZhPk5AuXWzOptMZu`na4L
zS^gc7-tM?&Yp@X$c9&s+@Ms!Olhx+o8#`21u&O;a1z*X(>kG}_7kP&x^LRBsG@*~-
zD}=i|kB{lNR&g(h>B2K*l>yILnF#5ggJS+8GpuRm2CcCnR%2Qvb6~749hL-mUv718
zg}3ZbQ<JWcACe2MVc6tTULv*~#U{#D4NerhPs60BR<LT`LbqKuCd5Yu^eo)op&IyG
zk0N1N)@}ysg*EXXzg8(fPJK<mphYANvYhnN4X4+8b3@h1DYnk?TPtlY1Ct={B(Rgj
z{A#LcE(56{*FChC)aS<YE%rQ|jjEl6s@w|K<i%s^l9g3JsTWtt%~Q+lB=-7uqA}b@
zHe-=9LMxH#?q<3#N317=rik7=0XcEc63y_9F@xx6zQ;cuEBB`I)=X&AF0K5_$4`g6
zyco!=OAJV8N6F7<g0Kr>-5W{^&i)k@$B9{I>=WuV1JVI+_GWj)Qm^HYx{ryvHtmXz
z=^whw8Df1gZTB#(O2osTBMZR{SM=!2AbsbyqFytlwXLGc(n*=+f{mRq_D46=f-Oet
z&&#^?Ub_5gUb&sFU~2F4OO0o5iJ)O+?29jGc+m?T8CQI+jsuzLOqjdbG1EVGgxAWd
zA0LIfFn1*^q5_$fdjy_dfATQ^Iyo!2WJhwx|D7ky*0>)Y0!axUJ5t&UqDAvTcv9>*
zwhuNL3Cw;mWk4Zb(?sI)@SCi=9j8i@3O_}ynu?stGRiqlHj09%+T6LBuU7S}B>a`f
zdx5xL-e_>vKJ!T04QX}#697#&Wd0xg!GFr8_6NxUN{)^I5LY%R0042uDrPSLAWnq`
z4FXJcdz#aiJ4U9g64Dum-^OmT%)GA;wJHBB^rl=OZ7TZ9>lc9>FE)+cOe1m=O$1&R
z*fL-bpq>+N+cX)CZ(pr5TPSM^TE+HYVs$Wtob#2TSvS??ZhVUuTc!muiLEk<-PFBc
z<}v92+X8~kXz(%tD9g5O!Fyqf6OZ{*yk$ME!2;J5=PJ}demX+pU$r>mrzTE<I`ror
z{RBxAhNYI23?DT91WCE?e<cJtl&Db#3FN#}9j;j2IQ5B7>_q|RMGvR(-;zrVzjrY;
zg!~>Yn)=%m1{8LN@Q^7I=L|TcgMUl1I~c;3{O!B`M;|l%-o|vm_j}8(&+et4pzcCi
z!sXt6$ZWysmG6Kv*bc(~x4{H|_!w({w1M?@U(8_t&PiV`iIYtf@1W|drdl;_mAcQz
z9^tOUA^CP%HY_y}Q6m_F9YOCCu2i)y(0b@w0uIBY<*mKM=}Z^%7~ypod&-d1i_2Bg
z>kK<9K#I%OmWu##(Qm6xx?%T6Q}>}*pV=4bYTeMv+EaM*!irN9H6CV-VQi6syx*EY
z1`>$46=Hnyi3E8(1?nnmOLv+{?0~IQ0V+V~kdqn}wvzzRz}j-gn}fND4gP{}&0#h&
zU9v*VrAclNE?qwf%TA4k>ga{0n=edxzI!f))vkxP9zp}tb5CzYS-_-_R!h6P64vlw
zKv2N07YkgEdhYKJ{t5&F_mq*09-rG<<d{bf=QK1$I$JNMKN1V{p{8}eL~*M%uN!%j
zeRFTKJ$Z|89djkWXpYBm?v>P&x!x#$T4N=Vmis(!VjN@qDh@@r>o7_LSy{5&S-9J=
z_TB;k-#TNsc~uvBveZ10M(EC<f}o_aA2^C#Qp%fXZ0h|SYZqfzL@Aj};<E=x&&G%4
zUY&Klb2!)&$}F)DHs~>ezw;3s*F1m_FU_h0EBa1W49Ym#%^o+n<QJ_5{b1L1#E*UF
zUe%wXP})b^>w~KT>sQ&m-wJOuYo1j??o$=bE`J9kt3Y~)R8Zq3VD!VQlwgU$OE$Mc
zH{ZANa3u`Ja)Qa8(8u(7NDDB(IbspqpY00Se>4#LN%z%F@4P~2PlHiC!gBPCQwT+w
zD_BrC91u4?pmDUXrT_;Uxd4~4@g5EczWe9Ma|v{brVUEtqcOxg;(8_rj=$bR^-W7e
zSnWkZa5pctj>?wX>*lbKjqAl$<M}~(1;epn3H??>;jeCE=vP(CpJpCd1JO}U?YmQY
z9lz3DdB^aK5`?-Jbsx;WUJxyInt8w=U;C(#CjHjbUIC~d9<)$i_aJ)8qm(~2fgAT^
zj{Xt_rF|IaCXxLI&*t;icldY7_cB7^W$<@$!fKMAoY|7~-m{+zf6}*y)A1)Wg$TZh
z??H7dXf2YTA>a8t?lKqG>S%y`a0N8LZhl|OjM8Qx)Jyi14wKm08n?(3LqJ4gUVGqf
z3^Pq1It&&5Z7j*=+q%+fBTPT%14t6r$|X<YOP&l)jYboGta#P4)I-R&`1#7@uwvfr
zfb#LnMCG<g4o@wN0{Y0EG}=00!+souZ^@#;OIN06+CIIK2i=bF8<avOyjU^0L$Yc*
zch7=9a!V*6Tx@G6&LH`TpL?jEX4an9!B_@TzlmzOG`TeQ35`a$J|a<Pt=v1k4A|Yi
z^Y{GUIu~eXpDS3}3+?wt-KR^8(Pt9Kd=sN03nKB_+nriy+ryh-dkO%l@{>0+3PVYJ
z8T8*(KRCXBP08#O`hiLCT&b*$v;@sI+0u`#ay3ng9}iRx!)@D*6)Fw1uiTqv(Aesp
zymwQc=B6tmoA77$eSE_*gx*)1yfjs|;m7VV?3mvk=iN<sgU$+)Yl`p;x5C98o*du-
z4afF^cp2b$!6p&9ez!ONK5oaj!x;R#T+=Nqn4pvd#{sHa<!ChC76?10R645x6j<$N
zX~27*S`qYq_ul`JV94J`DxY5dUAk$b`+N9`&ikJrvQ>+W9UWf^m&XclDbCnG`+;+2
zWRy1r0TBY0k<*Yzn4L)KZSb?SL*7sa)|`QvNvDqPd7il@D~ffLQV;Zn{=ag$uE1?H
z-;FQOHd1a>y;k5TM15b-si<}!nhbe_Nto^r47*(Zf>C+TL9h>E%K5$wTZTFuf8%AA
zu-P^ctpr{PtcrCER=dIzf8*LRFgEo~D$8l#G>t@0-tuw2{A@-lw3VP&RcLt_`BwiY
zNG8Eh2X6=u5j+eBbW63qB)~FX=Ra8dYtyHz6k4xsvY0`+;kA>?pJ-}wUXzbmUXFyc
z?)`-%)KfF_B<e#1l*ezTi5|Y()|~9yb!_hn5&j9<?GvtOEM+kN2}&2D@~+|27n<SJ
zB(L=9ISN_8C#ER9$*_p-**i_Ds?`nI<*zzv;u%cD8!e^$*6V0bxZaTkdYx&{n|(TB
z9tyK6<XPld{Nx|q5JRG`p}@|sgBKrh`4DT7FJ~`7?M#STH21K8Mn6!Bmj+9j#zJ}i
z)cJthX}Y&uKXA)sFI6u3U24q1v0e{s-zajx2hzLBt0&zKyHT0BfM^g+8Ck&r3jrF<
zS^8(z!=-usmb__Am@6@0V$M<!y37Tbm`&p2(D#Y`X$(%ffN&$r9FKIj(RxOSsvty)
zW1?o#o$?PK_G^@Tf<RTh1$ys@v}63@G8>-PNpza)_?FORg>KYBbjr?TIONRZUX*zm
zBbn7zTzvJi=KM5^2M3HeyEvq=4Ef<CdLwIr(|i^(7Fo<%PaBz0QE;;Bi(&~6)Yv#G
zb_wLbVrk;A4b^PevyXs(vulp9U-~X4kIu$tFPcgtKFMmM*jYQLA0=DVeKvfRAR4>+
zoXa-sY01jn$FD7j;sTi$LrzGR5L=$b9ydRiErRR%?MPYsCBt+?Z7aE6d`Fh6x(mJ#
z2qgYURKtDh{Z`h{W!9m3!PTmme+Lj|m*V7NYPa+8V;ypgI@vkF&GWDxZ{#=xHZF6l
zd$%Cu1knO1=PO3tSsj*zTD$rlx!)`g`y{g6doG9l8P&Q2^Q`>&I^OE@-_`%MgcUlb
zZ|oY-|EWJHp+S2Np1iN8v|lMM+Wx{EsVn0Sc<RruSpgK-PCD@Oe?PwcAAk2>=<o%w
z`%7wNxj!0dhMK%fJOW)RpT*n@UoE>pu7RrZzEoo$he)EnWo&02k(&V2^&1V>5CmxO
zR6y$>voZaR!Wu9OP<6<Kini|tPan4<R~=1q%dAfyYP8$DOyWN$UtXl%8}+n&OObqH
zOkw~JM(gCZA&9>d*vjn1SV@U(MM(F}c*<j5l-bU1^;hvwO|lUgF~AdN)zY30?yF9o
zMr>$mYGP&TTx@=V6r?SA1Lfm~TNLzjJmJB+$5ydhQNYF8CmFr)kG;_EVoP8CGn?R%
z5BpCLcOT@e0dNH<WBnxmzerg-_ct}|Z;zW~s6L)0L15P9n+~@1Ql2u+#V)c*@80$3
z607hnJz;DX%}AR0!t2!R7dnB2R86#Q+F$YkS7M6JU3V2e_b||%OHdOg6!ON2K0%QC
zs@AjXG&feFq55q}A+XCrRB+V9^T;v0==iC~M9pF9t>mY@Wdg4GbznrpT?lf`Tb%)j
z_bIcCT)_sRBb6KKYH+&wmUWs9&h=koWNn28R4lCe?E1(qq#mr$Xd&KqpmP1&pSC^4
zew>Dj?th18R?F(hy0N4SB(~j7K0)-`v;t7J{Fh(QWjDq>#76cqDoxX9bY*YLhRb@T
z?Yd<%iqR2N#wHLJRz@`6B>HD|<~)e)LI?zdT2z-?##Pm3B>V&&uU(m5&?g6+UV~RI
z*6$uY!~%9Evaa^-Ls<4)4%XXzc|UxDIOD3vi#Ez)J_n_p%IJN)9X8>>+~`#X=ddT^
zs+)L0;0UpYe8V$_I_fy%*CN#3c_~Z*awEL@i6R8&cv$u4qB($&Ia7w<{4j*9f~BWQ
zB>_Gjw8QjBScrktB(G4BjCb$M8`#>Pcm17=?jOJ6|L~k>9ZE+a@(^G8d{SX6wKbh(
zO5jWOyfph0M0yfI-m~}daf9J`u;X~6ECv`7_Y=hFH+bv>t8^EZjJ;ey1yy$fX<wZz
z>-5x8{lpa*DO6Y!+~N23G1M{|V__PjDv+A`u@PyniEdcl8|lV|9lv$j+7pXg?s<Hc
zozS#osJ6c^Cy4dNDL;e7ghJ@Ls3Tn*=yt>wETcnBiP@6S<a;W{OfmK6w<?bTK&wNR
zE*q`-X*ofXvLxH=k`giPe4dZ-S+uOwvIon48|{pTp1x6^F4Oz+>XVy!L;3B`3-I5;
zUj<gee0pRTE<r5Lyq4AKs%{#ph%=}xSqmXl^Ek{jTV!P}mj@inzg8SNgSQ$Bm^dqU
zM)1xXd8zwU#_Su+=psJhbK40VrcTx}(Z=hF6`^M@@#%?EGuj&)f`fP}OjDbm<en{I
z=q#~7j&aNVt;i8rksDrnV$sZYs|Bp!b;0i}u;W`N;Fk0R+>)mwYkv0)-l@{96tV*G
zb%J^Ae+A$FGxvW;xKQB4zn9`QuxSXNM~!+{SiB`OvL9|KcHZ!J-~p?)&*ooH(`bKD
zZ06&M;hh>eSRnbn&@dq@FJmaA!lk%3<^PV~VW#2z^QvCGJ`yHGZX-KZU<5Vc?*gaR
zulhjs_MBuQv1cirfyzz&${jb0nV5@P)F8wvoTWNzW72A++S7Xk+aN=rp7$9nfMm+@
zpax4!jbX-sN4u$GWTyh3Y%~z^ORzkG`yWoCzr76q2IA$vB0&sl&xdUADw!KB8d)^c
zw{bX6VLOvq<l4gxoe6>a%*0{-G&x3@`BGPM7%c)*G``~x&e9M(SYDjGM`W{1#pqW<
zBP3r2TjC|5bDmjFwmDY^o~khWzEPRA&}@iOni|^!1|iU&Z&v@fInRU*%2wiyA00dr
zL?YdlLk5`^Y`JEF#2EAy@u{SyoFW%}aj1oGTe)jkekBZh@`=MjGmllNW;_?KStPHX
z+k~#sBxt*PG2~Q(|MS;!hm$@=45JV}mmNZ1zNd7w{_ev@pVN9k%JHXy1N(Vg4&tod
z<>_MuKR>@khUleT`biJ|HktIg{GQdx$$KL`fm(xaT1A$KETnaqnJvyYz$SV)D}nrW
zEE>(1;dnMPr-_8)_9<-bi{9zO*F(=6@2DiGNp_927N?i`W>)<K@vaRmT25s<`80kO
z-ignw>)9sQ_lN_B4FKc(1a;2y{f6U+of6#AABi(ON~d+Fdyv<f*VCHEUU;DPr>uWH
zkWzlYA*dn8r&MUfw=LQ>Bg&a-SIu)m(RPu^+*;fp;#wY=+=xSblin2%Gm_1v$u_a%
zX}CMKXMmqD-Or%et5MMzxW&?In^(2DtG0G<BKHexTv6Kk^jC4#f8<_&{u&T$ztm5+
zuRON3+%#f0f=r%&+(w-h-Ocs-J`->wK5rn=>GK3zh1z{9!($Lpcl%u~Tx7m?Onz3w
zWA3ZUd2(tM9-`&}{rf*!hY<$lR|D1i8-yG14N^_&1J8{N=&x&hzLBFYY+q4-=2m4C
z`9V&O(BsBIfhOcEAt+z@X=K2HEr$=8>202@%ROxmTXquYoU&+X6Tg((D=Ut>hiz(e
z3%8lLxd9v-=U=@VmT^)!X%l2y#|C$Tn1HhN3YxK9bVgJw$Y25}t5K|2+Y2XF!rZ78
zE%K`9J;Gt&T%f+3j-~HwR2Jte5BU}Yn%B3Vm1j`@kgS!<;Gl_xF&S+!=y|7?tO)pT
zsj*SULK|LqG;y|$>adKZJy$Z;v(v$s#wW)bM+}zq5xAFSH?I{&I3OL4{pV~DbM{C-
z<mVqYejf0)XYf(LDHYF27&8P;`wITR<autn4+UL!w9J%;`E_iZ&NC8+9XAhtL{vgh
z%0731$-?Jbfgz=Ql|MmCf>DiUk&TyLF;+1?!AkNZc(Uj{6D|1^cID31$GqeL=?wP(
zaVo>nZCQ&Bg{D{80M;-GLB`>CH<_PSW$}LBJt<qi3polzE39TatLLkma$8CsUnynO
zP+s|HzK$S1JY1@}-4rTEJuW<LC97n@B1nI2<FVaTv#d~x3fp0)%qh!3B&}ZyoT8#|
zG`-(7?Hq5|)yfe>KxY@vE}u(usS_X)r3Z9W-Pl+H*$@heC<B}_kfk&2M>BPUgyE`)
zeYkp^n<v<D-x6FFJ;HV-!rJLYx2lsFvmX|FGKECn?};0<?v*Pz!bNTKw;Sj-{mN!}
z<pw1C0NE^wWFmd*%+ng;0Vgcq5ONqs{N+T0%2LH|_0}f&Mjt>P6|DT_VSLojWNv4t
zZZl0^B)hXrLd`j;y)+njt+sJk;*3F*?n=9tTb3i`$eY20qL(_zOSfwL*O|GCeI+?N
zW<O^>KLV<bWIP*O>onn_*GM}e8Fq?`G|H@(*Tj<Glz&t?&bC?R%Lb<kch<nf*!=G(
zCFO6<dLI_SeASlmjI_!GCX8?^_eCo+^vpX4nmMou2}v$7=L5kXUkE&A@gv6-dvTnE
z*(P8{fZ0Bryq<R-fR^RnXEvHOp5^#hem0)fpSL<S&zY+a2Lk?Q9>qM|aOmwd>*n$&
zmGu{d5m?#?IrjKGAbq*uRMp2Q?x~(D%VFrm&Z7@z)nhOPh+>oWcNW1;V1>QXq>`vt
z^cB~MaQfICovl$YxpGm2jCuD>#(l$D_%_vIC{lSX<w<g<A|SRFHWNvW?zx8?G$z~J
z?C^@)pDisccceY3^K+A4O*FmQz<&(gyL&BU<=|hhj@e-!g&Y{c{>UQNGVkQszFdz(
z%+`QoTK2LuBpRWSAWCB}-<xgjTQq`?P#G%Jz++y`4FNln-sAWGP$&Lo)BeZT{{7m;
zU+v?+Rm%vpkU*fy?i+4w3VN_To!KmSbyR3hxNlEJF_B`wc}qK0C?w2*`d$mMgP+7%
zFy53epadJ+>{XM{xxFH#JFa)D=uvFf-g8BH6MUbLr3E#yhDXcb0>cRor;GL4T}<4*
zn(cwpd@GVx(9e+x<b+l(2pL}4pnDpNYnB&}Cu;oGMin?XwPI}{{ajH~3a`*lgXw~H
z2>ZlBG#4o`D^o@M{5uO!^&Bh=$l<msNR}(MMp#Q4q`htQ4c{VjCazQ64IdS&BqcVx
z<Zw^Iu1P%ZxgjtM4AR8Sdpn2GrSH#hJwHJm^Ea9>RlyZ^-L+%;8k^E`V<|hd`QI9~
zM-|*G5OXLXd373aG>6Ul3F6=&9@&A^dFP+l{{;D&*cKl~6^i09FJJ?}7NZFyFsAxE
ziqx%DpN0li8bhS#98~rg11WFz4qFoYZCZ0m_${<fzVhUFtS8+eY*y0LS_wVcKci{E
zRx#*727roMFWU$(54A7EnF2mmPD7t_zxq*f1S&lxyDvpTW#2Q;IpZMcykMIcK7>v-
zzh$b9Eg6VGLUi7GmYmeP->&N(mtwzuX61)^+dAB$7041&{vrk{I#CS(Bz>;G!$<xR
zFZtbby*~KmCM#R-uG6vO*Nt@6RR*1(_$G5Egve||49x<sU8E&4gO-b5V19XPugAF2
zFn&o~wIuCA=J*pYMZr4wh%XJvodLA|73L+!*wXK{Ra5YkAy-QnX5=VwsFBX|R3RHa
zJ%7VWs_Rwt&6!)BZQygH-Ms#yefO9rnGL@BQTX*542D<Of|p1-DK^udoE)pu1lh+`
zb_h2@ApE2s%c`d0AB5%E_+Tb734<A>9g(ef6_?wmk3Pz3E*pN)uOOq3s}N~wpiSUS
zn+gpOH~F9;Sps#hpBO_j2leL5iag!r$LMU}@(^X59e4m=4)y4Y#H7mW3}>EL3&#*D
z{NPS%PCyHj2)Xt<gnzwW0lnGkC(@wS@7~5DS>YD_{>k#=k)`TrExHGRt~``wY%V==
z72k#o9%TP$w35t-ejN94m(di7SHa!H3u2&Cgqg)~5gKl{!L3mk+fJvu=LM;=q423L
z=}rw#PFlY<DA{2lkN==r(>GrI^C@Xyt>_gKe@WUq(GE^D<kU}{!vc0kfx$$Qeb=Y7
z5w{aHEbVKLWKa<u+Ev2()@y@t6$mW9yZ`!#g*tzG@?pY9w-fF8B)gZ;Buf_<n|Z0&
zMYb1Fo}DzJfXQd>C#WD8n4G3b_>QQsLTF@MUOO1eGQy8S*FH?JEwf)R*G6mZD>chN
zx0o@8^3~P{3-nejK3h2~hDHx|_ynUIRGX>ANH|){7>|6;vdV>DDweCw-81a|!QL{y
zz3Ok$yGwSv-v18kH`?fC?#Lk)!MQffk!v89uZh9_E{hDL5<}Ni{?>dWvcF$Zbegrz
zV5p)VJzwR`TgR=OFfBG`*iA}R)&GY43)kYdPlV!qHf;=lXH#$^6%K;u;kXnjkJH%Y
z#;HB3t=e~6cAMLgGId)2rMW(63^}!dgq+L?I#=RwgXkuS0G!6#<f9}jd^tp-2~`By
z&;=4iBY`u#7zv+_ZBw_W&3H6snwlrZV|}@x@(xj`zoPe|<u|<rxodzXb7_;OY{JEx
z>eUd>W!}atn?%~e*Y{qOkdjcg-F46`c9NOc!l04HBNkt4(0l>(blbB>x;J3g+b*)2
zzZXPsSyGCL(GZ86`ar_4*i8ePozSyuyunCwyI0h5F->beS-FI7^Q86V5!Mwqip*@*
z=9k7F{ODneqpUw%OmD2$T}#ARwoG@_sgu0l;#PS1)=2fY1j1PB|DHf-JO)sw=4fZa
zTc9D#pNC^Mmf)y)I5qsc#8M=l9H?qnynxFQ?vfsFZPip~LHHqo73BgP0{Qd9slHF(
z(UIBMP}K?i!dAb=;N!ioYrZik-l^1Ouu7)`oZEn*ZvoySEu7s@4TEqZFJviqGUW~v
zmtM16)pZmL<a$BJa9?)OrNjw3$+^TIBvBYFE4qC5hH%JRwXXD~-X(>rY8RD<npuO?
z*QH4!0)C+9Z<#aLvIIj*N};K?*vO3}vZlEuKWtOqL9$?vgy>WmP(3GnJ#SqbfPGJT
ze3m)$OrPtC`wd04%nw}7$D}JSRvb0;(N){rD5m0F!~rXE6Mjd#yR@pJLBvzqtZEje
z{Di_W>`jz1;&U4|ElN*yzs#5ZBS5Y5V@Zz8%<V0WY1S=|IR+IxBr7nGC1487!Bhpi
zVe@5cFCVbaOx+yI9#a<b0&7|lQOrH&yUGmEzpDui*i2)VF2}*F>QnHsG+RD~dj3xi
zVEDn0RtT1=vwYdx;~3_He%(900oN3xWeQEBn5ua!8!!4^umd3eaw&gNVeVvGRTwkt
zovP}Js9Gl_-t(e&J@^p_hAr(ur{0pGcBQrLlgZTtqp-k(j5&DsL5N`NFhK|lepY4u
z5ySr0sz@s|U!l$tgf6w^ah_n>{0?(g!CZ+)oHR`(r=@4Dn)cy%Th+?$bh^ccEfN$P
zWS-UEg?S4^#~+nx?%m;6EC%m)6uK({z@m{N&mV<zRYRvT6cU%*+qb=!%=fClj7*iE
zUE_Z#dF#>TH&>^1`Bs$$4BWUcz_^zOgGB~1K4T$@XFD}Z%)?VJRnAEz^wHk{2hMFh
zSZ#dL_(HnF_bz(3MK^=G{&E)9slUG4hKshmn!7(LW?03)zsk8xayClpg~J87Aa<;S
zQ@2ftw+%U;(eYyf?Y6(>`}m9&#ZT?)%(8EQIli$?5Xo>wtPJn`0wCa5GA4=ssLlQ-
ztnOst@4fTB86@!TeHsAIh5w+F;|y<FxepMoRgi<~jIwgzuTbawB_jHNXJq9cS^yE(
z&I;vN)i997^ePi`>!W_-F1s8PCb27G9cjHGuM^$g_l9I&((lqBJ)9OSq<EiBSAUEC
zI8<{-GE4WVfMSum#(F3Gt7iK%iKcU!DCRIn==YN0)&Nkyy<noSfBRQ`=738eqD;0J
z*o@ybkMUoip=%`VyeI$~UQ`I>0YJkm(t8X5Xs9#ie{n0F2oZ}{<T*=#!@xBw&x-Nl
z1>Gat{yY&9BRkJy7%ts;Oc`=di^zloM3N6+^uh4ybgnOG^{lmpM>C$jU4}uLa|pqV
z1WA5@c1|#{x!~4U&vSkIy*k$svlGTcjYtjGO}7TN=(DnDjIXpqVY{<&&h1YO8x4as
z#z1(Q<-oiMM*Sx!yk0$jvo<x|{1Cq_n`9yILH61X&%4L(9>>1Z0_%NG{8KP}3=rJ>
zzb^B9j}0;NmQ1yt8l1|l11etLg)0>&C0&|9C(pAB)|tZ1B}WYpk8}3Dd@_JYY{5P5
zBdtG@)%?-xMZ2^fq3Y=LV9&yh<LF82%HTD1#~Uc6`^_sc${<SMMt847X=$M|gU09T
z5MRxxooUUO>cxtor8tUUq#e0qkMdse2h!&0dq@+_P^{o$P(;(KZ98N~6P<ZV+x#UQ
z-<iyKbCLZ`o7X1c$fU85AZs4t>Pbd{`<i)3C#B%r!#=aK_zPpZqSr+bgXWEuR`~T0
z><m%c?NqbTf`jm*Ljbxf+%xzv9oVoOIU{OVh29M;pUI%YUc582J)ON_EH&BkCO(iM
zRZvlP?is$n`l}YZTn+X<TDuKG>;sWZL{0JvJMyQv!rgk0rFbl@+yIK5jQk@=Ohc?s
z^Dzoe@xzz`eHIa%`K;sf58cNnT80ngbl6pR_EdOc+GBtpDiZm|fnaeRYatFgjsgE^
zLHBk3F#^RWyh=NpTEr6Dqr#=n)4s9^GuC?sKEJ}w>o5OZEb4Ep+CQ>vfAsuQ<JZd3
ztrDSK;ibCr>hMp=^DZ=Od2N(Mx&EX)mpr&eN43>+U0B)l@Hc3JAO~C?P#{HTd;zcE
z#JKSFxRvU`(b5L=VtdRWx9aAF!_iy*T|5FjpR}A?^FY*~a}6;eySYir*5Hw1-XDBT
z+MlJ19?G$V`%LZIRp5y5Vq+3_N{VwQ$~|~yDtmhmS+1i$vVVRycfu8$AR8M-N!0BT
zN+3gK&dz698Q*Riu_b;XYY-k|)D7hqVrSvHC>dtU&y~9W=7@@6<H?fjbbO5?jm8l?
z+0sn>xs7B2!HZzouGfUt1cqRHyt?X&qIs`XnZ3Q*v)&?-E}PYJwqk#S-$64j53s&~
z*5>?rGdN|%&%e^;5G)(tS?okIRp@71yR+v~II^@mkU?&-ORt}6+|6vLmttG5HoUGo
zwE%<vki<(!5+>V`*buS95Fp;q&&#jx=(Ms1rwpT7raP9q`mIRhWt((IIi@#CCNFG@
zi;S1eT;=6#I&5p>M6*ke0)@a{=h3T8R<ne8p?S+oAI?~aSuf3zzi^_gQEz)KZx+Vp
zt8XmIqE{T=In<?VTU)*?Y_~PHQ*u0^w*iM?P2cB2uRf1{{K_C_ov8^+@eSZAu;&lI
z{Lwe?pHa#Gg<|pl874#o@C@5HKpIM?`L0s#MucxH)^2${c!7ck2xhi@QyznA2Tb*K
zr>T1DW2-Bt;j_nj+C9#$;PTQz@kCGd2>lO{z}J%J5K#<Y{0O}Qslcm!#-8haG-D06
zKctpBMCR_q#*?f7_3*QR^~JCp56vyiSv=sMDr@DOfna_dtxg^P1T8OH;~;bEXYTRa
zXFKB1dh~H6at~s!@2s!-Sk2F9dw<R=>#&izb*xx5f+f2~M}-Gb`ovBBZ2MGvNApjJ
zbBsfQA)=ch?W3J~>DS8YvuEItO?g}v*}EM7Pm~f2RxLb@P{gl&J89#0P2nHPzVhrv
zxkiflYirZNuxGS637)Cf6hp-N`aD1&75*GD$*s53s$0qR;AP8gLEA-Gs67q#fgt=c
zwsXS~R30SWWjAv~?;5sat#&SxOa6RMS1)H2CbA1~Xu>ez8E^ckRk-xORjkzxl^dE0
z2EEXCmuInKjwM2FlMToY+MJ~WSx#^opDU%MW1`nW^)Pwi_-(C(Tg}fwL(RFbK^J^l
zrM_6M>QvIywwW{el+boqZ0vh=Ch9e$0_eG?-D&X<mW0LQLlBZ%h(k~0Rf@B4)Aj-q
z&_WS4ZoUR+s%x_wwH3SJeqs+L5OhWZJfW)TCOnTBOoEa+-Q?U}f2HiqZy^C|Z<1dx
zVRJKq-pn7=|6Z<Q%~UdE#CR~c!e$4iP11G6(A9aJ0dsi^HvrzuGxwjZ!2jLXh%lnt
zWwdDbu8hER$%gjYr`F_2FV^4(h6ouAYR34qbNJb)sV?F(u<?bTpbL!Xh3R@m6wM{y
zT>D8^G8?(UGB6%{ygQdJd6vS|GC;^;!$}1ICmdE4&zlMJQ<{R87rm(8liJ<GcaI(I
zG!ICbN;7w<o>Fdm+vE}%+0`r&)TI&Cgf_H2<MvI4JgI^*yjcdS7dE88fX+SmLr(v{
zezfG@62iY83npSSmNy~q2%rgBZJ%@-7TcO=6C{nylLs9f555}hbk}{kHy7J1F{%fo
zMxeDsLpi4jydyqD=6ts0X;r5|oL;LV$11%6dk<Gji|+5E6`wS0#R_|VmSKXI)i<?P
z;f{=L4obJUJ703Eecl~{>z-r?Ef~LQl)~KY7%ddZE#5c|7c9E8rWP6u<q_c8=$jRY
z$OY+r%iw4-uUmMNLG5#ODNSTY<<OgPuc8ilB~Hk>EYaTs+bfVU$0$^DjgDSi;@R0-
zzf15|ljRe<N#2ZSvQp63BYR2N?1>_|M|Ka2S4*Cw0tA_qH=&L`nrNzw4jK?F{YoQT
zfJ$|?v&<wi_WIS#SfaSfuBWy&pzW5$<TF9=5@(*pZHMxV9h99*-Cf~5jiX(=E`WQq
zeA}s=qdtABN}{PG6bh9el%vD06RzmBrBSro(nL#aaW;jCj9untTA7xTLN>74sem2R
z0Si<#+!TI!0W06x$d9!xCf&ABG5^5jnbqg+@Fm1!f_a}zha`}Tlnmsr?&}JTv^;S_
zij&82qaj`ploz7TKKg91&9h)YtoB*qyl#0@dlmfd#4oLVqsQrsKRPe}{`GxmSO!P^
zU9`n8!zgib;8&NhqiOSe)@~4Lg?~r!neCjv*S7BT<VGtBw*0Dq$>Wa7R&-Bj$G7I8
z1n$ccOu{oV3(VN%2Uzb+NyHMn;$6uWqnC8j7DC%YI!~{dwn&4}WZ%g+#TEUo5~gK3
zb4@?>%HBdxk9)-E+a(5*e}UY3{J~;+-ZG9!vCb5EzG4Q7f^OlK5YwusBV7yh65td#
z4H7afben$Of9V`#HEE6zNnsmyj>wb#N_TJfg5~#Gjx<z*Bs*ATWZR`dj(uJyCr*&^
z@oFf0-bbPs6=`-AiDQR|%Rw%m$DZry)!%5^S@87SY6Nn2W26__dusv`NQ929w-)p^
zSI_3^s!G=zneo@rgRPo09XTHM>LEi9tzzfJZq(EXRtF{DBXUlB2^LwRzuwI5O0U;9
zR`{fTPPNod)9o0NwtnYnZ0g($aP>>M{_)%NpM8$zoJnk}oSxV^E-=>E%dS;eE#5jO
zcCM`4{~RaRif!5;(;bQdF)Es=Slznu#tR-AOM6fmGk-O6+fR;c+A8$0*CY=Wm^U0_
zV`Gi8Fgu+yT3GJo$&%uwnqU`jkN=}LqcRT$!iaW?3Yrfu+-D0p**)T_PhUq-9XM<9
ze4W|6tUwM<puHNxDDd%sH7(_Q-R05bQ`Ln=R1bfHOg=t`Ro~|jl3mZzA#Gal&AlwF
zK@NGwIscnTxH)6O<d1swe=|7X{GZ%YPq=+O3$P3A$Ba*kYSXI<=WmyMk^dkf!s`>2
z7f>kfP#}{^1l5MO+-np9>g7ar^o6(&mBVfe8*ILtiJEi#0yI6B6MDar47M<<cKq2r
zu+)s+B-)%oTxN4Vx%L1jNSOCJm6*iMmZmz*QhtrUQaZ6Z<a3ORENdk35vj!9MGq>W
zXCLs@g+OEoOI)PsKThJSY@al9#;Z}y&=UPHS_Om`I)$3Fx;h2mUiGSt!FTyjbSyU1
zHxA`4oPEKIO0_^E3`p8Vsq1x%3+s$oU3p2SKV9di%ws%uDhjS!6IxyC0fn3*mBxxC
zw)v3SUlIquHoOsek!Wp%sSVU^0fqTflU~dtjB-*gV{_1!MkC`lEJlmCWzy5XDDHJn
z&<b7(2~k|yFuqCcVAf6i1`6?O=ez{fLx0Xk3Fe#b-wtBD^2$tWo4BZBlnWFUW0n9o
zP)i;+pNFU?wc!l8b3>u+ZH@Y#EqKw@Ox)C&8tMvsS53HpAl&dW1?uB+7_#Fl0g$-s
zB!}1kk+S{WbFF#f`?&_1uCB6f<6pSdg!ZK$hSQsWNJ4`^F`B9;dYu_pWQ}mD8;Kmm
zSkvs;u1po_s-_937k6v(zM$<{yLzR3Kw{K&Enr@~x2Lf-^mfWhxpmi43if3^t`X_}
zBY*$S@ukITZ%PbP4Xii|K%ZlF=0p3Z<m#5;+Yp*Sd%OCpi+SebKIS~ZR;4YMk_A1J
z;;$EonJ2M-<hmd)Nye*Bqk=wvq?*!Zu$>Fch}#hJi0r&5y%sY3qi@sn=AwA7e*F)I
z2Nxi&q+oXwHZRd;#8g`w<eX5h?`>rMrn_U2yFtY?BB_}uX;};6&TLX=r^3(rp|a$z
zL1Rn_gyu~q+VjaPQQ%*kyLvWxBANmCc<3G7*-fhx%0hg~6MAgzH{I*@3;=2jdfV^s
zyn#Rs3i>~FV?aZ;#2sp--s7D=LbEHq29WNO+U-5pNR6My-Q->s82%{W;N<1tc--M5
zyhs}P8g56!W-SxQS2J#WSUf$mrm+7(G}hCG_Wc_Y5Pp^t5F=#?8P5p!-JE;QEs8&<
zhpQMp^KQi(v{@b`B$rq0Yd~=B^S=7OmPzih$M2TKUqt2qUe)FQqm|{xMDs3SrXc}F
zU=vv1Kb&w>T8GX)B)U8Y5v%-5&_&%#uDgQEoRdl@5<DLVB%_lhyy`=?hc~CizG<(z
zSKCVGiom?V>Hw42*S8TBrwmYNr)NysGxO0#rdQhwnJ1!y-NzmQS8}9LCgrYv4U9d0
zz{#k$rqq4G&(lX~#1xo84g(v`P`oelxszWDl_h00lhmW`k+o%el|}l4Tk-WxqHDNz
zTIi>W{fzB?^Z<m#hs)NBG$&REJ(}?<OrEv7-){AZD;FEk%c?~=T)j)g*O(jRcrz!|
z!vKiaDyre#AA&UPGz@-5_4R(%OByLwpPKXi@dHbsO3zU6=-NtI#^SBfk(#9m>jPuA
zA=t!OZW~?NuoGf|)a|{ak1v7k=dgu;YIEr;_RiuhotqU>0kDa|wF;$E(ZppSCRf#?
z)=-`Q$DCJg7ElINYu@{#x<@b#P^?8B*axOF*G#z1WLz8OjHkV6p8kM(a`}09pM%GD
z5Kj~lcOdo$PTp#P^x(txwngQbERZN&7w|~`yeH+;n=ua5kMEgZ^|0(de_p6wudQhc
z*1o`kS9e^7hkf2?z2Wn@^xI;T;`SB!w+><^@6YKWyW`64yety?3DQqAi%#Q>=riB?
z?n1EM&epTvwTVP})J5KumC|_nadgw|dxLiczR6qZPL>C}6PVWw1gzo0RR=a(p_E%+
zC0BV9+=#P0HF}AMV@m2Ow}bC@&pPr?aZ)w1T0zM0yw<2)8Ux?tUYZXpPArDO%B(r@
zU!=e<oSQ))kQV5Bk&SIRQqUU=_9zw>UVdg$_2!%s-SyUjyhY&8sX+`9uBQ8UUVdVe
zAjQehVqa7q+86FyV&vR2mYugAePybtf^QO7#?>-`OI$C${vSd|{|Ghxqt}6KXe1+E
zaCfG#&8yRI;jFeYd}SHAC%XcpX&BhD*_WYQx99X~L?PY-K$5O43eM?s7ZaG~C?qo&
zFQnWqsKi_u&l}ttzp|6IpXz@OGXc?W6P#YQd<Wk<x*Rl**qixuIC~@1gKa+C<;WeE
zoCuZQU;(B{-GL0D|GrQ8pJvy8BfO6YiPyv&W&lb^pfkPEs3}P$IothZH7M=I)t8Z1
zpOqY&8BKE8^(?aBqZ{kTeC(IJL-TcwG!4gg0g2?*zC=d4M&+mX$39f+uxo%2c@TU1
zu*!LU%*BjFqiZ{Yh3p*i&iR0)+izz7TD!(Pr79HsEUZ`OwG|fmr>n-5+gGDj8N`Ji
z9A_i!MN1p)p^9~!-U%ZShHnx?3dRlawW1j(ZgK-yc69B`;&Izu&NfHV<elsIP2Bs}
z*SvGrbpMhgAZ7EAB*#7OF*IttSXMz_H*fcb<U*m`l^hkFlzgeeQD7@+ew}AuWLp6i
z3<V=SO14F~tt_h2&UXpiY!ke(=ubw>K`J3ZKVm(1v1>i3^-km#4)+A+N6`RFDzBSK
z7)D*ztdH8_=+&e3FWSms%-r6eOsh75TI&sDptS}Ahv$DZa$@@9b3YsjPupdQF*7Id
z*?zkPqJOF<mT&5BV8*s$Iu3*(C<H8J0AUEwu+FL{x$)k_9_Lu!Eh)^@g!ucJQ#yZ*
zO0PJ;t6Yd|MeQdEo7M0Wu5S@wXtn&ae3XXoA)fM!1ZM-Mr{5E(Z}R3Y`>!CR^o)9d
z_r3S`q99%O_w@>&v6>I-#}2)_w{Fio+~=!~LuY+lbW-5~0Vxn5GA^A5bosobf>oJn
z`Il3zI|l4}Ud>E#D>iy!b8*TG#vU<kEf8{(DvVUH=wO2Vi*|IoKH0fz)Sc#>VDM#@
z3nyj!Hmmsi%Y$rwV{(F{cy@GuU>fQ=m(}i}+Q*kF-wwp&hB1Zl0?~Q1%{7iy4w|0{
zWPXDj3|RKhh$Ee*>BFhwaf-QDJ||V2l1mIK*LVixbT$~I7zJw^cLMbTdp;Jf!O{!$
z!bK*g{u_Jm85LFAWs9OHh$Kk@5())Ga!`_l5(7~{a;As^mgF1@K|r#CfMk&@LXoo+
zIU|yDs3Iw`$ORR=+qZ9z^Ys^Qzu!IGXY}YkKNwW)!GPME=ULC3Yt1#+_|8&FAbfJ^
zTYw|VQ}#wO=^gK^Z?Lr)-`#V5|DLQMV9u0!d`R+KSBp#xG)p$wHHhL6aCd&Ydi;$$
zU9cg~u~oz@qwM5dBZ_~pd!=NZA@k+qvU4xBN;Age23$&1mwvadCscI@c#h`2{r^#5
z@gMyjhJzFwz0#AaY-D`+v0a2$4)hDT;q&<=0qqV><@#?_PeobZy{n6{+1Q-jN6e9Q
zZ#=_i1lTf^HrI)L@fxEKBQJm47`gC<ElKY~RxmHmEA!naM&Ad~hmgS($0NL^l3?-n
z{`>s(l%<c^<ib4TK}?v3o14BuPK6`eYe2rbsdwk%_2<@oKn7n9S_Egpzpa6)D=q8C
zIy(!eioZUUnBuRF7kYG8m66N+KsADsbG_%;gA>2A!O74L>#Y&kb2qcbCWrO-qjcU*
z0ewhBS<CpX)0SF;slFe;9sAmS`Fr>MKgXbBnIMJJ`h(vZ?sFCV=C`-o7qtO?i;48w
z1^8`hNah?(FTihu{0Iw6jDW0OWFN;)JysSl#Mv=})l9mr4V?tZdo}1EOkEMnNs4Jb
zhUfN*O{H1qJb}AHg`W`o*~^SxpReB*fnA+AhqX<gRS|B>^GqVvw)7{C$>UE7vetcB
z_U*i^cU8pCI{>Ru&$_m~a6fQ3ibi~1>ul<jhUoFB=3r^li)13)XG5pyH`J<_o)a33
z0?;!T5x=n?p6j^oz|P(?(>#5UXkLCM+Hkclxxqc0;)^9`X;7HK#D0OzXT3Ox3buUG
z{5K2@QmW}>YH|1?AQ1<jDx61%07aEUy48P$7F7NyP^e?eO$?(sdSWE<*7){nE$t9d
zs->4s<+Il@?;XpeN%=$+)>}A%Q8m`S%kgZ06BWgACsw>Oh3?+7HAw&02!2`L2MYa4
znO(K<JTWP71QZFL^KbV?P`8L+>`>+@wk(g_zROlhC4FBq5#e1w(xas9WYBx%%=ai*
zfoI!I=&2{)nVi|@lCF~sPY4X6=4tnnl`LzRd&pr(P8gmxtG2;%elPVnkl`H?1oXbl
zb=(I!lU3FYms+cOlC}HZ^-GLCu$9(lma^rnfljR-zAheWe=%e~l?MLK=gFymHIHq~
zK(i6R<GR=X*ZMaX{c{S*rgB48MQbBVboqKBp$sc5;fvW0ViEkfFV#@g1=Ck6IZ$%n
z=R_5T6X<{P!cZ)GRcCWuHPy=I&}6-NJ^CPG*Y=QsWM+oLjj-m+`{XAB<2NU3un3GI
zfkh$6m|Pj-OMkI+H30F|vd%JDGs$SlLdw3!5^Zv#JsIFw;;?j0UIb#gnKImH56GqR
zYk3{_&(5TtT+VD6{7i5A$a*8LrM{w~c1)mjdwT~}U0oMz8NPUlj8!@J>gU{G0CY+b
z8O$mAJbfi)F<bm*X1r#FZTDj~WG`?oyJsGX(&v<PYg-0!3lC4_?j{cm&3r*z@!?lM
zlSE%nL@wN(QW{%nJ8-ln{~Gdn&wXM(?3r1l^{WKvizxneo`1=SZVtC`^Y9Gucma?0
zLC8M>oBxs58(>j>f*X<YOc;d9SPoV(2bQ~NJol)<U@!^k`DEZ4i3%`uXv^#gL+Smv
z_bb{m1=;huTu5&2@0i>Zx+#l#G&u5}t;*$zf=_jB(!`c3pLCy5xxcjt*kACKUHSR>
z?CyzjG~t?N4477yEW0i+H8W@_Uf6p^=mzLGU<H=hwC7sKy1}`1|46DfE!<V#1!FhB
zz2O6&+&JfMlS+!}bpa{|jo7>$j3Iz)&WLv({=J+3TLYwuzmqD!%2NT2((53?B4CZe
z6#!;|^@h>_HjA*Iu3$TRz?vuxoYemJe*K@l_O}j5L9eGa>UO}th<K@isW6pBAVOw7
zJQD>vW3U3Z`x+HulOPWl0BAGRv-)RrTBM}=J*e0}f?Q7|x9IO6CvpAYEfC~7)#}`U
zAU8<AI~}K_iUZ20k@1no@H4-On!txEz+{PrEJd8r!Ut@EdLR$hI{*-Kyzb~h`+zkX
z4)4WN+wI8frmDaV1I}jGOa@Fj`os0_5>LkeNdk}6S5jmq(Ufg(1hDM^mVd(^n2YsY
zY}0i>gtkl9cmQ$$0gq#4mc4-v?$9NQBN%1pEogo2A$<21QRHrRs)^Txz=bp2@`Z|P
z>`9A2Yo4@m^*)2GaV}8G4*G)OOXAjd%3Fj#1A^_ve+{)4o99lVFm^m$*{^S$ghJ`H
zH(LxF#|Gc!iqywM=soPbBt-lK!Ff^fo1y}s&CI1X_j&Ohu8E3P_sdn)<5tXj@yKI0
z;0cx{K#eet25Pt=PD_$Eo^K7X-=AwD?E9DoREkJH=Q^7vCWAaP=w4zLIHsP-J^@;5
zUY7}14}FAIwR~E2a=y=-jTfSejrta+_Sq(E@N%U1iMn5gL3lxh$lx8}1r(zs{_9D~
zQpw><!WsHd#;8S;NzzxR5|HRYI!=vx2n)YUP+a(bq5pqIAq7tsr}Xau)tc8iBbO*0
z>f~9_6z+9%H7euH&K04={1p6B`Z1-A6=&(LRv+*C&zNf(h&kmZgx$gE_zORoI8hsb
zX5Z{I)qI6)mMMu!+JF`v!!lE{=hMH>VJZMH>@%zj3|54pKe1$LLRvCrYKx)4dDiRL
zyLR~PL9fqzcX>G?AeQ)Sux&KxwO?#cWX;Y4pX}~Bi>ep2yu2^WV1AqeYBE)fB+om<
zG8|1?c2AL{ZJHD~8Z-`QF|c@F)0yQsfuG}_pMRH`GtC!shl?+ms84D@RBp{5lhGY}
zm6|yDNqZtDkHCB05{)g<z)lmmP-S;}_65B*RmV?M9Gx|?(>Kp=0UGxVuq^QSg8xr`
z$<vMr%(X7neQU`7nL>+C(M?8~<cR3o(aqv3zOyx75Sq8nxN|AX{WvV30;@9Jg<P6b
zYW1FVNa)|wAq=G*80K5pK5y{%+G`c*F4DE2$%xYkDunw-fqlw-ve)zJv2`p3Gx#B-
z2JckwJR5#SxSRi29R~y@cm&w)@80PD3`-yv7;Dg-xd3*I-4~w(9XhivP@<(X?<GwS
z+~X#1UJL5MHjSM(s~%+s2sxP3UNlf>|KLNQCjB`j|NI?t8(-L;$Sb}@oh@Tp;tPGv
z{H(v*!f$G1l^HrD`u)>XeND=zUqo{81_JGY?sQDxuormn@^%1Vi9iRU+kzY%m%KO+
z>%r=1y3595s+vSkxzg~Ke~zXHSYxl?e#_N6dJZuiTD8Apt@ZZeOW)tI_QQM_5NjJc
zNc2VCO(GtaY+}a5^!R#&^YGJ9Ve{x!U_Q<)iUM+P@;Zx>!PeL=<O9$Ya~64U;jgNM
zq{EQfrFIy`5JHwWS<1Okg0UV%#Ai6yas=i}TjX9Xx$V+(Dx({4JVfYSx(4Y+knL<c
z<;TL0cy9RLdFNeyK_RT4NFp|`o9x>jZ=A|E`P_d;8aTDl6Y@JA#-HS6x6=g{IFcJ=
z3giejG-GZ%9~m`FfBDhLS$~OoDeThJv?V9LG;wlZv4%J3fIZtm_}jwL0yZTKc(^A{
zwybhS{t08NOF#L+io?~w0goFe%w1iU7X*^4r|oRC&`ia3nM~b}P&0Pk2lW}X%Z04(
z<N0RZ#!%)_)Y9)GSilY@&cvoBKCpHg6HIouq|TemdD2-jUQ6Y<U+mmf4H;-IGO=sO
zKim29lAmoCW^q2<WEpX%drKV*`@<t1@!qFSshCq{pU~IHA8OQLFFae0L5I*?GtZHI
zb1IA+Aj6;C>xxIg*#VcPFdWS#8;4{q$^_N8oW$P6JO++j*)`=L21U|c!U!4>b_BnS
z3F>(~B49P<QE~@YB$xx@i_1&!1z4)pZw)9{+W4m|@ZI8At$c<)RS?DX0o;<;F*y1#
zV1qH!iu+C1P%Y5=0^d?k`fVb!lEw6oiY5Pv*DvhH<LHCB*VTlH%mtfE9q3cO_qkmA
zFf#u@OU>i*Yo`aYZ)4-aTf#?<q3ZrBmE+sohuh8;Y9F{~4Fa5UlFA=&a)gFlfQ#c1
z_e_R#mTkWdeUX28!>PjA(_`+vuPc4>wP_xnjbzbRiHCL{xu5|Trz`+J#&}XQ6Rt>B
zKM(!aszfr&>}pwL%hfNHDwv7(Ay3kT9p57PgwMRCdwvmJJgqq#)8srKXp7PIC_z@w
zfaQRi<r%nl?*|}}ynFr<SZaUc@bEvt-QiD$QFz}T<^v`hD21eyniz}=foIeFQ&*k4
zGH2o&xTZ#7b>3ZolI*E0j5PFUEBmf8`Xp#<P33*sal~+DnO<{EJ4KPLwTPVkpUsWd
z*mzjJwK*K*hx-AuZGqgVH)6OKPb^f(T6IA{@5z@MK$(4r*Ze06>MKcqSx^u8*Md55
zf|mb{<b;0}g1N)k4%>lII<$Z*<0bEF%uznd{`ztcTeuc{#_d+G$8~nbLk3atL7?Ja
z%U)QAziC#>|8dyk<kT|c=@Bi{DH+gr-MdA#{DJ~k2#F@c@-L$62<TEM(0`S*7vc1L
zLRP(S^W<P|7@u0`2z4Tv`bDHNEgACY=7riyn-yZP&fBq`@7>y6mc^<naxXK}{I_c7
ze5X&urcn;4o5t^1RJLr#;K|*mZIH+c%psCJ6{W7RJInuu!{=B!%Z8l(osDdQ1z%6Y
zgA^wsJ=eKQXP`K+y)a#X$^NA*GR$3Dp?t2BxUtR60x~@x0V{{AS00eseZX*MT$+7Y
z70|cLf?aTfIukDd7{so9whJfhFE}S!6B}_+i5R`9r(!rz`I-Vz45J{>``96QB2c5v
zk_BD%pYEybG1Jd4B+Wk>;vi}ms9;NRy-IIE4H4}xXO)S}x|JuFmp4~P?*9(5NPx=G
z>&zK6)*bQC`lQC>ri`{(CdMkUsG6A=OJqMj^_s*d6MV69gnj}$M$p#wv-y+j@D(kV
zv)LR0>)`qexvw;dh_0T!cD5MqFL}wn0u0;nX*LV+G=R|X@Pao7Di6SEB~!(3(u?&L
z$^n)T)B5)|_<#Pj=2Ku}RP%JeF8Y>y@##^E@+qVIQaf3R|Ll{`QpewQ#oLsDBbkUe
z*q1~9$X>5gEg^<A+a?_s%R827FrXob;C74OkwYRdsstj;;=m5r_MrL%u~Zcl^liYk
ziW%hJKbJLE6&eHLz{iRM%I5eGugNT>M0{ePs~sX}Pf~zqA%+sMb+VG(lyMlm7X_}}
z>kx!=v~Z&rYHGihD!>Ydp4WLj<8IbID^I#<uJcvIu-KZW?NXR!%$f7{-4)D^IHh~r
zske;$RP4j{+tZ&%@63si-b{^p)2sEamPgiNWBc=(Pp`|4&s5&ZfYcCA?(ER%h;v{{
zDxMT6XWZcBu)R;#rQ2vwr<+~2uQoZ*aKZqv+hovm+h1XpKQPRHcU4mv^-6pf%Wuki
z98ari1yJ6Dmx5~?0Ofs?;<N@(-VeMB6|8Ydiv}C;orQfEfsPu~wv4~OF*qtKmtB3P
zZF~Ex<>3Z2$yv}Pl#khPklOfMHuSePl-?LH<~V|ERBWz&&@8jECPkM*3UO@l8QU+9
zAuO1z#mU<#I+Mr#U6D6@tGK&5;lW1p$IqRN;&l?B$4$tH;FknO4Dlm$P4rp0LoG@o
zokI4filD#;fiCYR8OJ_iFH#}{pqz`=Tmt$s0ln&YGt8eI(5p_~fPsJORpA}P2al01
z{R)Q%RN5_Qc>aoPpMIy(1r==dF$@J;MM%U$z+G6?5=zQk1RQty?DwubbO*Xs_@nl-
zyAc-y<givgp<RAvVMAHN@&h{;)vA~3oOkKOSi^PNkEDMNIjY909<1gw^33ofTI+Q(
zl?E=&9mp8-uP-tF&ficTfh6SE#5L(OPu#h*jpux?F$XzTG(FXudMh@-Ld0av1vqpS
z1uZS!-52k?<|EUpA1V7{&y}HLsHjRvmsjiBn~#?=5}RaF3Z&0Xg~^{|y5HXs4>}K`
zhO{2@u1WPrrzZ>yYYyw|s^*tM)4czs&GK2z=n=;2tj@biKvXIDs-z5%9V4<i<}ldU
znF2aM?3dVQXJ(f$afd2ZUW^w}yd32oGE~Iro<=V2v9CS59{@97gTCEBM8c=^_94+5
z<Koh(>T^l{kz9K1!94FOHM*XWncuf~HT+`+_PSwFuu#tpuI5E>g7lr=b>7~V<H6z{
z!*=F+vD846sYz)^ken5RqXj)>lIkRB+Hy<GeG&dwxMG+l`b;esT;FP3AZd>7%tmX$
zsEqO11R&FO<waW7&LZq2sng5<zT;fgw}tF<>A4uHhLx~Uz-CqRvct&^>xuXM^kjkC
zjg6ibBJx^m+e|BRcl$Q-y>CFmJj|$&g=0W3vAmadtYGQq$6anzWY7NSqDDL&z%k^o
z$l99V=5UO{g!g#G{ABG}+Vn#&NO&Q!Csq&Vx8DWKEsf1F{t<hZYz0v{I-{>UtwBCt
z)kz;h<---$wn3UUE=#%}fnumGLB-!#AZL5%EOw~TFrJ?v&i;ItLvw!l8nc$NRv599
z)Ttl;5f0H25Pf0qj@dc)Ib@$7s}2k{^v(ym0SG5P$thR?`|`Q+@tSmO!znHNMe)V3
z*5wjXt_|6Z`jt5UOyT98kZS+%G-~QO<?Z|{jdt~>Htg3QwzA(}kqcD!dT%g~Ja}41
zj^m>_D?xl+$VhUhFmsN}#UVI{2|NKDV*$ROo-llI`8k{a{-F%-h{Q*Q(c46U!%wMG
zigs^k&P1GKeZ{yI_n?<hkK-Mzo!VzRw40n>QlmTgJw#|?6YQ>B*%;vHSu`NshajJ%
zbjFI@r>~wY9gl`qN^!zdZcE?M4En60q|Jiz84f9LRtige?WpB7dNR9^*XRLNDp)Wy
zHRYlHj5-dTIgZ|qZ@2>w2dr>Fvwo+@?m|!pERt3(KemY+N~yZjP!0yX0Kc~?N~mOT
zaApgHg65NOjOlAk8R@j>>7*anUqp#!29LgVTWnfvx@qd31l|B}QuCwN|M}--MS#jm
zJe7~+HV|swLeI@3PBMXMVM*J-qMvt~`x?IOWV%wIRBC#YFJUdD>44R0b%(*rQOlef
zpj>lyNJyCMc&L8kKEt2^$)fT-fBgV!FH@D<y$fRX{W5kgoSa;~no;28T>U=5BY8Ho
zY1NZ8)aaq^SDJFSY(Hfh6AlR<+O^=#y!|7lAod~G`|Y7>!wa}=zxS2Rj?UrfXFt!T
zvG=1ev!EM<DKK5lmF;Tq1v88>)IQ6B`AEx}e&9Wm*o#X!7MfRIi(ZbqVp<lSi6sf}
z!*raev~c?z2y#ActWVxt(QIr<AV5`=KF}o0%9_M%1SbR+-|W6JW2Dz=A)=NpGRUiQ
zpuLeoAZGJ(a8@7AYexs!zIm}ZFhZz>L>@t+<b^SyHn$~(QcRuVnA3Ldp<QL#3Anmw
zruo7ZnLa*+tMrNQg5CK0WmT|VIR5V~fRWl)olbM7onJ&VwG>$zqOps<(b9e0^Z2n0
z*G$mWUqo%ZvNU#uy;<D*1&9&|vx-V}(Z`BN>(=%Ut45J8cV+4>gpiEHWP76F+j?lF
zw%OMywIJ$y1Ce=e+70x4YnVb3s2+P$-y-Rc|3KMlV|OWZMIS{)*S2%NX6e)_2b3+I
z2OnLiIXs`2j5zS}+yHF06JV+Pwg6)JiGne|`1uKUALDa+o;O8(qFXPvr0IUKq@t_(
zf)T`?!ey79-xJ$z6Mxc;DRn3|cgSLCLGNZtOT^kZ?s(KipT1z~I;k~zl{rI8B4y=O
zKKJTe2MPE=ILOnE3LceGt5uFvHw7-L$7MUcRbXEinA0NCN`B5moS?F2EWV<@Pfo2c
zCph5H4S3{h?S1_^=do4LShXe6OqcRqEz|5AmO1=Q3bve{eZ4ubNHLp6p3hj_8cLUL
zlz`gPPvWvl)nY|)M77$Kzg(ktlv66k0lEDUGu%D&-n5XcR>_vUaC5{U_A$HFjFG9G
zi{?U4s*XLMPkOTHkqmS@jenyeh-06fkG;=IF2nz!X#D)>`qomQw|qgI`l}o#V;Jnr
z5yP&8gv@}Jr$VdG1ir|#{9wG+$NQnS#9o~F(KV7wDeTr^?eXu~KCZ5CCGHR7=rxzQ
zmpYbMeu9~r843d9CXFV|tW0EFtZT-vKN~UYC8xon+BAC^11u_pCBb=mu`9wPKM>af
zgo`lVoyH#bYom)gEgPyc*vcAuIbs+r9@yI6ObX@*xlkq>5iL5S??4fBJMVLkp7`yx
zA?5%zgO(xR{6NZmsT-d5jUJ=X#s$??CNBeEXgKOgM3?iWX-s&V{P2cToWEj{*-n8@
zxFRRo@-3OZ&Z`UqGYB(=&C_}M@FZ+9fCg1x5U)6HYhr^)<qVNV9vT`ig{WK<Bj3G1
zQ2)jv$Uch|L#PgmZTe6=EDlQ%zgm)(k^D`#aKH&GU$})62l~8TTD<@K)0lmGDjSwX
zs51j91Mm#_KO9*9GIITQru7C{7{tkIjPtUlxMw`mPghNbwwmPGNl1+*eXiG<PY%^q
zNFHq~kqb4~)dLVSu<su<lYI)T1X2HNER1xrnpssZH#XMH%y01?jIO@U+SO<3AyYEl
zVRIHPwAlSREzr^Sv`M&gH$S+`fWRg}Fb>>%G$a3~s{WAowGw{tSe&LaluoU9%kL|y
zGgfvc0fjPX71;Hwk`(AE?i1f!s<zvaF2cbGy)^*!YaP9}a}XFQd;@mhYh}>AhJHsY
z?98nx?<{Vc3W8Juro%G%gzRCV?WaAD6GpP`P0;R_E^y5f-}-Q{{64WvM1fPT<b*(n
z+%~79=5r|wc)YvwwY1Xf=fruody1vLQCO1dj-n<)V}QVf7eL3QAhUx(6i1XmJTxvJ
z^_XLt9EZG+BzI)%>TpU>WVg;>bQV1!kzyu(Wf~s?tTsqk5GB?If_@8>49(|$8#|9!
zLgOX9#tzgw1%qe6%`2Azkx^$tQzuq|O3!XYya~{Bg}J*7iSH#{w@uK$RB@R3p<>?6
zC`E9XC`(2Vv`iXO{l0FgKt4xS5HZ}GIo+`EuHnKJrI(Iv#2JU?ljLn*oNniq#+Q_M
z`{jEroV&N+Ta*S>vmngU6D54Ho7<)E;iLPSfUu(N$<dGI>1IJBc}eNy#z5nw&~<7d
z_j-yBcKi={9YTdlA4uqetXB?JHRyZGz4{O~VWsE}&5KP9b)%hXBHRz=r);~GEb`b*
zq<h$-^MA|&bJxE+)Jgzz*Slu<4fVMNgT)>O-wl>NVQ@nz58!U~mDhTH-S`Q^^Y#T@
zQDY9v$Fbf&PF%-PU!5Q@;<PaD0nKW=-HL;gfZc~h6$kP8I&x=Tee7FT+P8*f*MdK0
zHoJJ-gU$GN)J8hZ#^u6_G+CV9D(W-kvsBeNkza@*ju_Uyva?-n@RLcNwhyhfR`>)A
zIT#7Lq$$zW=hkgnQmMk4m4Z%ho0V&NL>eK$rD(Of@K}gNia0#cl{hZ72clzUOK&i)
z)iKghW-Ov*t|Huf$fKWc;FsV$@p~Ng{NP_kh~KZ53H-P_m6&%0qz{wRCK}&4dMRD<
zJXr1FCfl1(>D_z;s-oVJ^GC1f#3K=%k?f=03t=_k`CjwWAY{1y4+~Bfu5Z_9hLaWI
zo6D$Ci1ujV$Qu?Oybh%%#n-27T$CU}Pu-qn;dL|O@+8;h9ol@HxH?p|;`hK{@maOp
zt*S9+JMySPKA3LTmiqR2*QvSE{Ck5A*q{BQyQ&28p@SF=^+=zWsz-spi2BM(ayC{(
zJViXEc|QBg#{!x;^PBDeORw?2+bHmC31Z@qY`~b3LSt-f(mkf`D)Tyy3pi-4TZt|L
zQ#N?><|--sir~WgJUen|czd=T);ad50f(OhSDC>p>X;W+NoP*OC9P#wq@)gnf3M+3
zvN!Y3Dlpz1=LKJ7y-C1gKS#fj-C{xb#2zBJgcx#l!lm1o<R?K~gB6hW9AW)0EvGhq
z^p!X>=a6(P@FwTjkzt=NXcEFgxNZExAe)-NJR~30IosoHnYXa%W<0P~{Bkf@(eyeM
zLvAZY1BJ8<nlQXn%h);eP{z^ST%IO;k|#l?ZeqDQN|BqS{T}|?P@LlN?8Q2c3`zBu
zSxmxbOsJn)2k)mO`}zGH+<%D7^QT(99Dmv3e4vlhEXF-Sl`^(VsQ8v`UB4li+j~<j
zR&h;m#g5}`PDZc>L)`skkm>=@$`Le$Aj1g?9ye$4MQb<J#QTJ*g{UQc*q*1A)(iNg
z@c_F_+(8o>C)}2eR^7_fIH5E^@pMW%?e1~`&O2j@!OaOxjI#s2(tTV>UV>CA0xj|8
z!$=5*hqMhu?QmcM`7xUvx$f{ZWKVD6wu+WAvBmXuuGuR*NrK6;D?~mMRY7#EO}d@q
zWm7n8Skm627vi(uf>E|f*z(mwmC2-*HDK+4WPB0{SA~%SkU#u6k(4?*3%~t2>*I#)
ze*1IIRRR8-zlZl)IV`cl9Kdm48xS>)Tqv~wVyecSTmmh)9){8sVOreXz_YG6TrPjz
zNF<^$c77KC?Ce+2LOI-kZ&5SbM<2g)<rr$ud<Pd3)sY0I9Q$$Fyw4dP{qd@sC7mTb
z4bRz3ga>p|4t3L0mgp03o)F*T59XI!M;ykd_Hhdc;zM<?4P>?ul~j=Lje;<BAXmK9
zznrTR>i2tO&5et1vD`}_iXqdM;tJ>X?b{9D2mC8C(Z^XioR}2U^}-Uv4{ww`O66sk
z^`FLwCMbPi=tkDrOLOcpwPC+Sq4(K2?T)RG5gGP9HBwK4gi}jAs^sC+Z*4K2_8sR(
z`bxuKz;?$17Sck5y|jb|=zqIP(X4Ow&lZl%vgVz+h&>L}LOBcZsi~vRY53;J(p8Lb
z$KvI&)pGP?Xn~Jkx60E!XueU8Bc1XC?KNwn>o;}n%IN*{hZxY9?In@H^khkz`zZtm
ziKZXvm1YSe9&%}he)sf>+k_tj+t`=q!C#ekE*v0s@K{M$`#Q#A@g|xOf3nAlfxUbi
zn9MdMS?eqBSbhf3DY2rCTCF4!Tj?__U>*Mz6pNz+g$uJdRegN$j`z93NwjHFY?31N
zZmX^nHJY}wf%vEZApnNLUwgwU3wOx#aF2mGg(nb_7j&haqQ;$^wRQD;1MW6t!c~J0
zb`gBR+_tf<WIemAj31Qlw+@F_XrE4OD74@$TZZ)!I~${S)^cK!mEyx~sgVN<MTkbi
z)aPGBuO_m!wT#$l_SGX|o9`At8*Q>|V{zoNzP19XthtU{Bsxi!iQ(EZ`d?i&XbUCD
zO2l)`?eT<6<%y7DQjdwT-QQ)=%l}3h{NKkDlmEB*o@M|)x)3nmTL~0`fdTWqDd@H%
zV7_<ZBoG-pJuLN&>b^a)1Z6w$kV>;p^6bV~zIIiOR4p!P_}Qlx?R^-I{%-pXNIEDY
z0Q?y`*5hJ*D3mF%oN%-0^v#mY!9%36xRiAUO=$Zjs5YVt&Qx949(1K<>}YQ7;N`CI
zB((UZl?3BqpJ60*{aT`ZHxnyuP$6kM^Q8D}CdpS=pBaI)m^JtM)V-DdrrH!7-L7BP
zK2Myi^zzd0=%E1r=?9wi_65xvRLq?<xi&eSuv9d-$EC8@pdB{}-{mFWLtX?)HI9^V
zLT*}wYDrkWcVn^rw9O-+xN_(k+RBibPv}nzkfiVwzGk}6Rvo5h;`u7n#?2@?L^Mlu
z-n|E$w??>TCzXDmuz6HRcn_q2`XId{7B1C*f6^QCcj^QgkTw94XD|jxO>vHivgRl5
z3~hA@l<X7PObaQrDwUE?%f#Yl=CAbjv8$sHm^%S=DRk2q;WlU4gK7s`&(GhCG9%5}
zN<T4kW%lRT5&$E#=T-M$ud=R=WG(8BXz%(M0Q0*z2f1WijNWFx&O&$-9T8t_QU_~P
zk2-SJRMdojo0Q!SSrmEGz?E@l*(L#Y11~9rhLEa`c8wzzv0kfYShyDUX=8kwst#<?
z3l}f-6@$krV*0iG3yeCD13puIP(uNWH!@UesO_fJaAJ;~Gau$5H@}Ehz}^#sLS0w^
zu?`d~yexBEL~Ww*xzG9Q`TBM5+*DKsw>Psdo<0dmPr>-Uvz>Iw_db9mXu|cot5n|u
z`%XjT$QZwUAP73KLDz)N_4&KPiW`Nvqf{QdD$luS(35CoJy@Ey$vTne_!)E^qqgiI
zT)lI(o$U$Jlfogci$>bWNL!fbM4S%Aifd0VBWKjH@tNew>wP$mn_VJMo@6L4zH!C}
zJ7(is+8-i)K{P@BY5Z67aCZulN3W8eir?wZ`#RkO^`6`_qeVdH8YY61dBi%4CemGv
zzj|P6r<@-R9qq^mnmhv@V*F5JLy~2b?5l?9;9Q<p2};J;=r#g#Xxk$5ST*$d5?h`E
z#RtwoH@CeI4M|b!nEebY{x-yO@a4cJ{ic2KcEb9A#ANevYRZNa3r4FV<cho<YiS^-
zX$DKOKZR_?1mjmZMvTE3qV2Y0wiP~KFCuzW<{a>PhNJ_jFodGK?qB1@zxV#XB8F^*
z5F;EA^f>bU%kiP|T;!3>B5!*rsBe8qbm&X07#X{5AMTYjz;N}V_497*V)yx+PGwzl
zRejX%C`vqy<qfN>TP{TjKh+h$VR|_f(k?xz6x~}v70xFgHw4+6DJv-~D5Sc3S%;K+
zaF4OdMC`?HM}<Q`uk;aM<NxH>YJk>0td&5Xd`{`y)hLnfZ3{}5dp}iy&sQ8;zM`<G
zqJ8CAUPcV@J990R=3E`pJnRHkVYZod>(JYoFjQ`H;2Ps}IB4`uLt<k$V<qO?C!mTY
zs;)ejSLnnGdvgD$#XbWfLLHH^8A$#lKm+CsWAd5Z98*F|c6iqBUpk;o?l!97No8Qp
z)MiSM=q2`YdvJB$n~@eg$AYRU$oDPqal&>qdaTZhzpJYmT@+p@vSyr}aU$`yQD#C$
z&UQGFmgq!mAw7F03l(5Nu8@s`Be`ub<#Va*a1eaWeY^h<s9b!{ro@r0msPB=3Gt%p
zB`W8P2TqDn@15r1^fwn=bVCe@s4lF&GUuVj(Yd3h;%lyqp?JE~RK7S`xkJTF%{E8#
zVs!X8Gal0zQY2Gme2OhBItE7o$x}5%corM>a!-_apk3GL+p}!@&_NS^{+L}dZ2cjh
z<7G`cw}Gq(N1qATdEr+E@HkJ%!B{AcTi6?yzEgbcV}7w~++i{)r4N>lqay7B)d5YA
zU)fE5+$#~ZgCyTMbatg|Y;%;^?$sU@c}KERoyg32DY%1q*%Dt*xE1htoU<I`jUEKt
zJm`i!Uqrf=s9Im9momIA7DaB6SWmk(hFFkiz`r9<`p9S|^miazUb#{vq(;A}4b^%t
zN@Stpx-MmU=S*RiN<&OVzQJ7FNwT+m)}r!EH*iy9DZtue53c8w1u+U=7T}h-KZ*d#
zKYc-@>-w4KjguD4%863uZZplN#d$SX)~UNndH6)<4;Sw%%Fk4R`6BecX~ikB*LeVX
z1@7UPX7efI!Z^a6Iiart^$vnN&nGA<KHt&zEl9Y&&}P=8e8FISgZRiQY9H#8m{Bl!
zksiH;eQQv2-r#O_I$}P@xg|0$nPQogOFec_aR3X0EvIIaG|LsKxCb=?f?K40jMbRl
z_`}CB$}(+r%GoYNafW3~`x!Jl2mmVDWrt^z%eN93(HgM@=7zr6cH=?)RptfwIY5nU
zv%uTyWp0Wqbk^Yi{sSJ}<I3)u%0po;<R1NFb!QGa=ghf~AKEeET?llI!WRJq81QrY
z&(JxB_5eqW!Y7<N#<>KeW)gDUq$u^v?%JE)Nl69eC?t`s%-f7*k@@`hnO|B@Jq#2<
zGzMhGOi{G18O9E!V<uueSH`1XPmF;9$ZnT?4GO;4BD#gR(rjp5tN1mU{(1ZWq?Ra$
zODWNssUX(ixo+|T*)vI2yuYaujy~eVCb}z&5f&B1Rv9h_Rg5O^xGFg9h~vw&^sVbS
zJ&0%r`@x<GrdRYdNVk{v9SMQ~@^Kr_(xhb-3K26km!Ieg`Ssj+pg{}|xFb|~ndmAZ
zyk|1Mli5?~W`_fhy?*?`xLF|gd0A=xQ3hM88nw2O(9IoRoxqhz`4ZeYH%uQWIj(9>
z{F`9(Us))=3c)d`b);<oRN|#0`O^{c)O(QX0C0%RPwUjNp=!s6J2o+|vCF%p*4GMI
z(^k6HYTW&3Tc2S*+s9D^aO#jt^_v!#V%bYj!f#sW(6O6FTP-|*yH;Jy)8zBG9ERqY
z*Ke8l%eia~?Ftl*;4A<$pF_hRV!_j8$C@<0O~9UiJ}u@e)h+3}F0jGF;(n^(+8yK!
z6b5Z$ud{gFNUnl;%cJym^f;c?voct$W^81fvbqkt=i`{r`tdGf3P`L2+xr~Gr>EpG
zq3ykf#66~V@Zwxyw0=o-MG}l2aVZ}dKq;Jhc2Jl+ipl3iaNA-w4JX0NIrp=w#3!=>
z(F{>4JnNyr^X3MJneP(ba?Q$As>E+AuN7vLvmTsBbnVSb_LK^|ecEzk%=7XA#dWm8
zOTqWCF|B=#Nb1XV7Y*(BapIfTPxg)Gx-QXhU3;9v86|$T=%WP@Dba=5JouQ{9$BRT
zdyT=}qa%WobNiAId~4bd(%R7li#os!lYGTzH|wpaJUNZVIZNaeE4Dm!`)1p6>t-*z
zO<W%3;4h*(OdHaxQ!4p{p&+WU-5V-hgn}^DH%?_2i=;s!2NP62J*I9xKg~;?1Vx7-
zV{)$~6e`GgPoe#SNZf}+6ZJPL55^8ej$jut-VgkJ@9ogGkNSJguyuNEt-q>sJ|A@A
ziV0Ea>l-$n{}gqu>QOurFDRAQoD?si^L}A{7g)xnR{${bhum2TF#eVY1R$@e+aw!b
ztJj`Gj-VQMvFoIir^;s!?An-QTo+r!$Ewec9Uo41Pl6>j*Y(2h9FS~SU5v>hQDK9Q
zg6%bL0PtLE#cpi*<@671X-fQ838l38BF?vT%$%#e*op6i!Jw;HuTpOmGz>qK>{#l%
zbyxV?yd*l;`qH(4Z_>s%h<LI5t#7#@dH7x#$k<8>(sps6;S_VHhks!47IZNgfr}#4
zHpy-LdqB-9ivL8_1F1+;sBX+_B+Olb)_GsZBP~-41UP%;zv8+7$WIOE00z933UE(G
zC}YtFWt;=Xi?`O6-RJ#^Tl-IYtS+!>wu0l+mbRx*;b^7UNuy?NuJqg_g}2kYJg4aW
zl(0B84rWpTB9|Y8ucnAj;fO}(PP1#%OVoTQ*_u+6kU6Pb&*uyZcz_oN&DjQ{OB`dQ
zOQi<?tjXxfoO!GVjHh6+g^uw`8?u7$XlaRY0R}+_^;fh24x6QW0=u_<J8V7=N0I(^
z*yQW)2m$bv6vz0)*&&*GWxWn7j>&1JS@BGECR4fnW9^xZgiItJCu@tePUt037$9Q3
z5B_mE_PDmt*4fw2m0=;j6M9ebnv~C)BD20e>otW-G;Jh^S5@=T3~y0cplg|B+dI&?
z-0dm7b5@^udUT@kqOje_r{CD@<-dc?2KE`Fsv%cqxYwJ$7~0pybA$<rv%Ig1%?NoI
z?sh>+Uur8j5TK-5SaTc?y1H)iz~{4|>h7c*ic*M7NZ-23MR`r{AX1umFPNG_kxIti
zqQVfqto{Bg9qip#S-BTW9ic&yRWo<s?*df*aIIT|143XV2pwKryt}aRi^#!L$*!tk
zp-FaU=Bz5SzI7{06d-cZh#JYRHy$Mr3ZILP9FSL@jwL4c36Gp*%}s!vpxpdCm{W*J
zLcxljcY}2H7m<N$HFC#Q<DT`Ro0fTypC9`oh+|MxXr$12tFlRc><fg}XQd<S4<Gg$
zgp?e7q4zs{zCRlxy^f`J&$5XnMe66fEr?*@b~IKOr(Hc>hErQ%3XpZ`tI1YDOtDLC
zL=Wh3J$jz-faVpS(xtbZaA~<=01G6xuvd+9!Dw+0$B&_OG-Omsg+11Meec4>ln*b~
z&r9r(a<O$V$=kljka_is$bT@GXEwuh?<aTRQp2&7)8!t$s-m5Py7e&}8SV+z4aYO`
zGQnR?`xg-;DesD7^^W}GcZy_-Pf_Bnz#uni%Wg5H6ihDRs`$%<rANr6%LDh!%>{+U
z)kauIc~A3RF;o)Uy%I8{`m70p_(%dRkDSX5M_WbB3{<6ocGqI&p^|saN;_Uo4+~Ir
zU$;(}SELv14{PnI#2B;%v7?2uxg)}>+3t2simBT9oAG^<u}^z=F@>LrsN!m_oJ;O#
z^NVKd4nqUNKxnZE&i&aIehydGxEWvC;d1%PmazeLX{|rVrDFv$yMgk`VM~r!=0}jL
zJJJnt%0~%UrMiWZs6TiwK!$u4&QZ}uo-_im_GO@DiM8ccN4^Q}0Va{+hCr~HCe*#y
z>_y6Tw=>6TUxDklT}&+bg@^?7KDfYCir}>HNaoj!LXe33M?*?!=5<MvnfI_G$Mxn6
z1LciHOS_O{L?n6-_`kf*x>7Z^s!59(gG|6<^73YNpBZ*mEL%Qx6X>k<W}6FcBew6>
zY{VFx@BvS0=b_phbwrwN=jdZg{?E;`F$yZAw`3yT5!)&ak}Qke4N*9m<zEwRhGefZ
zo_C9R1=6iSJYFJ*fAHF3RKb_U&j_}TG8)w*M_3bba>JJ|9jy!ncN}wRx+q>;{Cq3s
z^0PmjE1m<;#Kg_pKPwy%#%cdUSL{#!V08Kad;MZ<ZlN*sM&Lvk{)^<Kq}Dyg>vf`$
zx6&jo8?k9T;sr+IUb=zH!hBseQ#xVA=UaUpOZ<j5uW5m5u{G6Wi3cfaU*$PHk9c7b
zNygdtoI_a~VO-LUc#%Cb+i?<yvGn}3U0Jg)Sh#m&roSlxwoT_VBIljwd}m<o(rOEV
z-h07+Bt(w7VpG5Ih-okEOT!DHD6AS`A+V-q$J_c_w|3FEE0gz&>ID@3e!%eEs^(Cw
z>X6MzpIt}0C&|1>m!G#p`YX$qfmj$<BOXwSWeZSk8QT*Fg`3}u{UYqKp7h1BEa#d!
z-Io|A!Ff>%Q&9m%2g^t|mygt+p*vLpwZ0l+K1NOXp94`Ru#FA(llc_!!i?pd=z`L~
z8#~+pAQ#&FUFO*zcKKJb{$HXLe6U2gO81M1X9(yynUMy_n})ae`8!B5z~Y`|pUFEi
z68Z+g=k5*gkASS<?1tqpqE27mY;30Hqh5QUM==)6|F0<E-+W&IT=u63*T9z+H)$m1
ze!@mdrH1!Duu3j)u3gB!WU<rZx6Qx0G)vcwK)?5yIl{0=+?U_7UD;EAw_eA0W%W|G
z@|l?Yalr7kb=b@YW)144lqQhD)N0Lfo%>WjI@_ePsAjhmauzTw`Tqecc7;hX&#6g$
z>@-G7(*wNTT@tg17wsEpA(NW#io4tGA_XVGf50)E>=^HLPLX5N1?G$oC!a$y>z$E`
z-B&oyGTds8Vgr5=k*sBPPz!iI+0%SNU?4c7RkE@Iqfe6b*@cz&wg3;;px*+1Jk!~;
ze`kaI^~M5x<-*XX>X_(HEG)=1Q>EmZ<|hNK8}-LJuafrSIxk9P2g5+(h#7bn<#IgB
zmnt7s6-gz&(3=(WntR9<VRF{w2Vzu)M92}mUjyt7gxX{2P2<gq7IwW(U#Ll^X)~37
zyq*-ZIcEC-=ixUYB&#qj(k8>8x-NgG_Unxw1Wom?0k?%{LOZPVej-|g8wrSIe3Esb
zWATj1qS5>lZwJ>KLzJCV?9y7{dPxzNfP>vdp}6F5V;R&l<a`3%f8@j2y(O?no1i|@
z|FQR_Hysw^Gtl0{{gxs$nMle5_MMz%%Tc3$@!%VZze(-Qq`eb|%YJ{1*q1L-K>mg^
zgc3$<*KnQxW|yq|O&PP9*{M1uAcb=zwO`MRyj|05KAih_9AK6+8~>6_H%Rx1K1sls
zA)~n(oA<xP-qYlGBb4bTY}0pXHIHSQih5A-?yHP<Sy57c(-8xT|B`&x$=5Eak*fPq
z=BLhZC>aHI1}2#K$<}>$&^P{u5YPhVgRppp%zDqf&Cj#EMcLzUTTGGcLipX*4y-Hg
zF($G|$r8PX{Ls=8&Xct{LA#>=zWeGOmvRAH`s-~xF5FXvFu!c7=kD><nL^6PZZCNK
z7M9R*FQg^zH>uv6Jsv|Zq2;HFoZ+Q_Z&38Wz?<K`K@J7=neBjY(7;{MWugmQSwsv6
zB-!X_Kwnfsl>{wXGR0UwZM?YSNgh35CZ2<z6N^k956pMZED(#JP+VhiYx0BPXg9je
zg`uT7JurI56!T=$d&wtNFTxEc==o*GRLGr{%NFv8N)Fv$_2?(7gn0El_n5*6>%dRF
z4z~j}SVG)yEU{+`AFjzkJF}}#+Ywg-vv*n~M>h^nUlNAATDs@$yx)TC7MT`e4r7*A
zx^YhcY($UFtVxgvLz>xXH;EDLzYu%k!1R>0+`~addRDZWg4Vs8y1~bQK$ePzb^!V;
z$t^f0>-w8+L2<LkT*{rH%`6^~r$krQqLA^-ZR}giORqpwK_JaSQ;c`(EJ<Qu;p3aN
z;~Zn3u*O2e0*TT(oTA!~?utI|>ys8o>7lZ&x54K}OU%t3AIhhhn>YHF^-LlGDmvK=
zjNAn<N{}zlj_p1IWeeoR&;WqT82j->{foEP4fVxB?k_FI$%DO@Y}+i5#<D~MZ#xnZ
z)Z2KhWx&&M&e|#Q6(p0Rl`m&gpgFm6p4GFLoSO67)2q&AANk$LI)dJhn2S5p*l9j(
zx}ch!kyy<AW1)F3x8c~wiE6;*#7QzJ`nU<FIgB6yD#kpHoY@-LU9f8f^%4CfSj~~@
zeLAwMZWX4=J4ptd>-KK_@|%gjAq|174XxBM8<0G8E>Vc`P1E6Mn($Of(A6fIzT6Ng
z5kj`6W*aZn3ep^b+>Kc&<LUt~T5};f>|sC2U?$n>n*42}vuQz%1aE1`RlxI9>6Is$
zFiw1Vhqz;E^s$A1DZg6CtbWYfE^@_>N!+?C1-gC)dO(YUCsYqE<X7nyI0t-WRnZuR
zPrn^Qtp1^Q@}Fcnf6ag{oDBf$<r+S#00Tq$Q^y8Tgs@y0TIJCXADxH{$NI~94|OOq
zOy81!7=PDNX7)K}Ek<--ehjq~{H3N|Uah9926sy=nql;FDubJb?k+_k&zfbH0`U%*
z1{2iL)xth;PfmYn$Jo3ODws6SG9%k-^DwRn7+kN<$q{n5jp0l%PpKpbSlB`(%X_qD
zfrTC3L6FQMC7Wf_xg=|6PbRQr|6o5-E%<T2WMwgY3FFR+Ddb5u(OY5&H;C{Z+X6|l
z2`()hTJ^hAZ&P$>IF0kms8!pJLN?Zdda?3=sEY`{`aj?V|G%7`{;yZ|U%vkj<oD)w
z*hfE341gvA;XS)L6`^fop4t!i9|NjyrsZF=D`{NVBmqmTm|t(RtG}A`qI)#TvE?T}
zek=D(PU57TDU9D@tIS?~Zb)+CH_1m#wn*UQ(Z|iBOPDEz(Ed7;+-u{ve2s6%D95}j
zVGQimdw+*Jl)~;-GomxQ&bj?7IUshnYMPLJvc9{7Pq*k>U13_-_)wLR;W)e?-NU>B
zv(?1#0HRX)dpO#W04T}~28qJzC9meUBynBqqHrBVhsoP$Gg?_aUr1uIkia#iZ^d6F
zbU`i!uobEJTcQu2)~6}QaC;i0YrWy+SXFX4;CNYd7CcX)=%NhXXt7L3x2V4<4*R@0
zS*%`P!@$(NKT<H)a$03D2MpW#WPu@{?H1?LRI)=TttR3|_C@huah5C45_M4P66NGd
zCta_q3G?&uEF0bDp2}9ra~za2eMJ1O^QOu`+R`~{GA(I671^{$^>ZxIeo}F9O3er1
zl@kMj+SW5KyyLJA=Ydkzg9`3@Wjf^kJgL2Yy?lClwzJ%1c21EqifR0^?h__pVhRx}
z5#Lb28syp!Ef_j8StlMJ;8r|ESg6xz^p@|SY%}r%MMBmH0X-Y6V6^&hw7L8^r{<e%
z2Ep4EHIYMK`dqzBR1-v>7R9+<dtxDGDB%_*k(E1%V*wN(2#n6h;X>0bzi<S~H|Gs`
ziP3Jt;^_Lrbfb{YXg;Bp23=L<*2^F4z!oDA>F9mpHx;fYP@3(PS+Ym_s(=+^&N02)
zOUldkV-WDc^a1!Pp%;Lw&1{8zwwCCn2le49PrGVN6pH$pf<?IM4_Sri13vDM@-TB{
zn*yDu8Tyh8aVheAUp~tpBqria<m|NzRg5#VkMsd7Umtt%pRSqxvo$v0=UeRY%?LY(
z7MXzCCOd4BnA~=PC;t6IX&e9gr@mPeG-QMEYu&m#R>tBA)iNUI=26FPUj}FrR~)R-
zWn%@qh}+qT2AX{z400)+@M>ob;s)?<<;8$hqAfBT^H^Nk8+u=Wc)QDTw$iZZv1$2o
zB`rN#ET4MNJMH{-%X54!fgTE~a_%VsN1I<i_?CzwpK%JQrd3X!`jY*VMs-m6R^UNz
zy{&9NgYIJ{?8YmcT><VPx*!w9b~6u3zADU=Dslg9myw}g2|sTeXB-(5&uPGB9Eedh
z**h3g-^o9B#&hzC;9ci0<6ymu0ZV4xj;Wm2!5@QgX>6bYyMxW4Ss)Ge6o2jaiSpl)
z4;JkT-Kwff_FYrvh*awnh+)ur&<B8C;;)o>WgTv+|2|JdHmhTw**7}QcH}yYLABx$
zE(a_&vKJ*n^5eCs9v-c5tT#hZc3tF4ktsK9QY8AGC%e2nuy*Vpt-!B_a&oZVnN{gt
zY)-?-u0)A=xE%8$`P}ttlHj&Mb#v+y>Eq{5sWi>=4K<ntDS<wAekV6SuWb3!BID`V
zp7jOO{_-yN2lLwxv=UqUfU(P6KvjUEZF!I4=d5^@bKEFL_*0s7X=(2^9+nRk-9Fz)
z8336HD5Pbf#0-;$gt^<ML(TmL2M+5fad)QME`jdG>9@QqGp8Jo3uianj9$!&#_{8p
zmJ=n0D}qKpcXa(^!^4(^hXB3<$p!KKnS$Ei`xyoxa|r^kCTt;20Fkm%kNGQe*#03w
zMW_jgiQuCE&$Z{jhz^80fv65USCer8@984CKtmvVSQ{9&Iubw%%y0WebPjQcJTe#t
z15I~l_aGO54wqj<gp~QI?-K;!eK0P*;uq2Jc_AT%^L&n&u%(6*#r-w_I8?I#|A`y_
z@^t-YUtj1Bu*O(h?x!Fl-r$&v!HLc##vc*_?)uVQWM}p&;dM;llV50+m}e)04{8Pn
z@l7D?_M&H(+MOBWt2)o4jk~RD1ISTYobQZ68p3U^=tX6O(oudkoZ9jXPy@g$KR&Zy
zfB3ay%k3Y<2wn6<aj15!;aY-PI^FFxkE^Qigkk==ezCaiLD?G8<1FHDfRyBb)xU<~
z(Fo8fuAa-iWy^Tbk|KM4%x?jGB-W1eIE4(c7Xzu($f*%?6|6*nEnw8pw48l(=8Z{I
zo4cM{3lapmI`irE3>@-hyowR4>!&XZbowgmL{&>nBP;0X@C3+HryW4`R)daTb&m|X
zeD6XoAjLfinnQA+-}<v|@Bcc3m`wm~^bodsRt>@Oi2Ne@`E4Dc&Hwk(PDiXj_X_7z
zFeYC3?k^(u;vc@0z(yD;T(kVkZe4Rh>@%OVK*^5#Ua?nEavJ)+dA)15?mV=J+wOO}
zg8i^cXFg`mfYtVSVN!Zmd5WV>QBBd`KUI(HE&p=2;j7aN6w<QHB=Y<4G?bFQw7R#F
zzi{Iw@cj2y{bp1{yT>+wlznV9PBWtjNN;(AwdFQWjP~zs6}?zww_SL#AJ}sDt%TH_
z*f|~-o4xLD8Nbhw7x<KBalxrK$82tD9#A!9%4WCT*YEDWb&Fu?ebBnRN=Et|o)0};
zr1Lb_)0{o^IzpVy<HqhkGn{erh5R;1#S*vwo9x*?V<U73pn&WH70)Fskekb0fj@wQ
zF=xMsxKhjj|6|a({QZL}*cqc0u($Etnf0L4&k$7ndVDo_IU8|c-$D#{Km8)Y3pc=X
zN;a}FJbSn_;b5RK>KD;3BDG!Uj~Bm)7%|xdrD-4_;B|m&@9>VVei3=*6PO6fz$ZV;
z{q2+TH|7=pC$8Z>U^dXeom$1uhIeR>2=0!xo+m`7E1&iWZRxkq&!#xOTD*Ohq$Y8p
z&dy2&{>3BEc+7nCWsMA30G4H%cAd;CX^ExC_T{xJy&TKI&}oDN?3EX&TDaX&v;9=5
ze_ThhQN1tqld^Dvq+x0=<%e%4`Ya}EUK}J>l{N~uJ@@%mI?l^-(F2{L3p1N!uKb2`
zW{)_0xF2+*7CJx+@5;-~a@P;tysuFoX|S*mYHgyKcJ~c+b9G8G?rVOXtUIgwzKhK0
zz;c4{nOd_+c0OmCb;GtB)?2CoA~3`k{dYtIg<tGXj0vf(g)bz2dfU1*afNHVhKVS&
zU2?l}$&GJ~K`h3Ok(s~X-n`l5#<#`W_++Qa9g~jPYT*|P*5iE!8l?NT-p<G%!d!dI
zYcx9`l_zjdSLfReMRSI;3<e1-+O`?2G2Lz#i@TBa*NI-WF&vdL90<QYC^VS$m;CAN
ze2CL@wKJTc(s0hK5ykN!<s{pTx$c{%P$xNq1cnCsH%c0Idq5wFiHIbC|3PJ@lvW5(
z1LlAH4yECik&(AM4Mf(Qn|~RM{+ZW*bqN3Xn`2rZICXDr|3SC%s_KXd$7(n&+{j4;
zj_#D8&e#e&`MoLgPsNYK1At9`2@f>NK0$-<2q2~g4h2#|fG$+JUqroF5OA`5hW&H{
zmzINfs}UWbKA_(Ma?k7EXp;ZNx%jW0jQ<1Q2l>F-)LJgKL=-z9n49mVc%8<(GSt@H
zqwRev_WARynGpTi179l%$9_p)76W%taL#ci3QRriQ`a$D5b=2qU!pd?V_Vr2>d%u(
z@kVRP?QI8Nr_HthhrREPr@DXtw~LIjDmxJw$Jx$7_8!?gNyy%NS0qJLB&!rf*?W(q
z5S0~$L}aAwk?r?7(&w&Jcc0JQ_n+V6?(w+qxA!^c^*YzNUf1gy&+B?!Z&^?Fs^sAO
z?h4`9_IfG3OitF}&|d$vH4O`T1bMEjkzjJKXmheC-*3T7rP*O^or4R!Yxmr$DXiOi
zBX+*d)hcnjsw&i|Tv%97c2+KIHN5c-0fY`8wP5aFdeeL^$L+T9&PmBU&A=J&o(|)Q
z!sZT7Q><4NWk*4#%FHvbxqyXF!!bW-{J6lgy-S+WDl@QsPsP?MH}LMrj|@62?LH|s
z-n<6k`Zx|2uU&4Y<e&Iex<l2gv|Q_|@c_%sbD>&Kn6Q~=uLk|0`frQP_;w4dMaBfo
zjkp*~0Eck~3H}!{Gb{E<=;ry$tul0L4E{Pl@-zSGYuh%!|M!0C7rz#Nws|~z1r<;$
z?F&4j3tB7=myrCQ7$6@K4`Dr`X8V25rh3bwBa12TU)L5$yi#o{7TV^SSUc+}pkep2
z8%bb@jlJ>4#%PhJtARz|Oub=AU)=a|<Z^&<Okc5ajWBrnC+psVzOduFRS(K{6taqr
zD!4y2?O=MscTbmRUAZAkp1fAn`Nl<4%@t#!Q)<tkk#7^sG6TFr15SAtGcR11>1}dv
zttUS=bIcW?s5rf74Vm`y(*eG0+emB2U^Qsn@D5jZ&bq=h9xzu{S4sG`?~<J;`y!Xz
zG;`dNW8VV~%RJR<Y@B8hoVxBWb&Ou&jW_gP1Ud^m40;CCKIT)cg}YZT8<`p#K6Fi$
z3gK?Mm&sk4QjQ`$6w$i)FoSn$$hzRhASX$p>j-`9iZFQC#@zkYl7Av^bW1?F5|aXz
zIoulXF#$+;s&L$ze0Ed7BDd;lB06*f&uo}=4GZEMNAT$POJG?BL@Juso~Pb^hXhZq
zmQB3&f1*CUHt_13mpIBM*K#)STBl#$(oI|i8ON(iSs>t|T4QDQ(yv|(%>~}t`qk&J
zsa84!rsZbkx>>hJWwXuO?L<K6swz+`9|P9>LAY_ZVZG?V23{Gcp(79A0NnI#?fxe`
z{ikE;8do(IZIV_`Op)aGZ{Y2|(|pJJ%FZElTg;HGj_#$_>WZwiTDJSV!Q`Yn(vpgj
z4N_P#Y}6yelG;`5<2q2ovG9ut$IP-~mzbvvHD3aOj0i8wei*Tr?*L@-V3B*UM>KcV
zenP`0dVJzjx*<$yITZWt%E?~l`N{z?NCgjV`RD>#d9{6Dm{VQkZvNN-o|DQ8_%oG@
z#0Yv`uhq$1=>H^4Z3lsI<eBK%!DXL5hiYAeQ{AX2WaU{suvp3UnDvrzK3&$Wp2BA|
zb?)@V5dGfNu-;con)!m$=$90;G^M*7_u;R3)oQ<=KEzA<h?ZKL?yUjuofjNYnSOWg
zRvFoQ&gXLlJ#o_K_ed@0aVQ%Jby~7sUyl~MB?RjHZNy;zb5wwfAXeXj2>;Cbb5>Bu
zvffrEc*&pM^H9&DWwYaZ&dc5O$&~FRR*)Hb`t&wu$%Q9mtxTuop312?Eep!n$rdUf
zk<p1Ph<^I~E~aC%;&xy4TTl~V8?embEw)a{7qBE0v1nf1!@7QfC1L|FFqC!uXm(dO
zF8~Sj5XQ?puK*0<)QJtekv6g48_B&j@N&Sv!?u|DFH5$`*S{?JmnDC%g?>Xyax08i
z_WQrALQajpx#TyMsB*D$<kY9)&VnhBTneM{meXcVU+BKrnBo<y6LkMY)@y_O<EqYY
zubUj;#I!3U&lrP8;u!Y?^e5KdI9TJ&T9ZxQYvpZm<K44+)<qG*UXdc1B`LSk{H^wp
zbN5qL+T^e3j)O`eX{DO#ML0-o4c+jMYc-D5?f)1^XW;h1<ly0FMQ_TACf=;+&#66i
za=aL1jl8mSZ6Uko<$B2Vz<?=pyA8Z1A<>Q%Z{Q|SPdTiUXI3v+7^f8IiY-Uf2Fwvt
zm2Tk026THiFD*Ae%`WtRQ#~hL!3Z=nufw{2>-Q6_p^_rzSwWAcQ8HGS4Ls<2dtE?t
zr@y&h`ocU&yK(mSt?oArn0X%Y-9#_nXVM1TTNT{E6IWYFTmo&S5bOT=v%KTR#o23n
z1V9-?e<Lsf(XQW5RIOr}ye9XydYRz->kYi8Vg=KQv-63u#@1_<0izc@!Gt6_eXIG&
z-%sSWY$L-uCPuM=*Fq~gvTm}L3VL58W~?@DV<_u)Q)Ras069=!bXY%q`;*u#)wqbo
z>YBx{Zg;hV@iO~yFk{U%0DbX|Zkq`#joe>MjR~+C6dU;<2Cw>#`EuXS^kJ^^qSy<6
z!1P<(e$A`3E2{f9@Wk|gYZ=wolc)OD@TDdKhFSkz#Q(^{zl-?qPW+F0`*$b)M=$@Y
zPJe9`e?y(VZ0;iz>%S3DAaG91vhVyGz7GF=Qnx(=RHlz7rWs`xm74PDxVOHM;|(Gi
zQ_xqZ?Wdj)AFIzEFQl6=kEJ(^#8y;YOnqk4z1+P}eYdw#zsZ=+kjBA|_Z7%}d^-By
zmI8lY8vO75ug@C%%b$Pw^REp2D+B+^z`ru^uMGSv1OLjvzcTQDDFcKXU1a9=W-hMI
z<|cNyzZ_0llfhwdHYgkTi%mp?OhC)a(VR^{!rtD&l}td=!P(5**$@iehZzaT3n&Ok
zfg2dOArsIrH+5w*gd<T9I0nTAM?xVOIGhcEMnYg{BpVzh2tlI3$28qex`HVMl&$P-
z1SCX6HorhX!qnBu!CpX9KwCor_aC>NiIuIZgOH1xqoaeftGJV+s)VMDCd9$n;;^VF
znW!k+e~3kWMXWR+0EVMZgH1qB-+&E^fxux1HVhOBK?t(hyV=?reKQvQtFhS4v4SWx
z1pJ=uV+DU>EE*1hqJHqj|6nNiIe#sVtb@HPSS2<ASs0cL4#R-Az%Q2tf55>DpKnlb
zHaL8XC$fTQa0A<V2e(XMi9UY}gL?-zw+0(vuz;TGNo(AK;wD$HgMs-zkHWpD=Imgq
zX$}^fO+ZarmQ6s*+|w2JDH98GDYkDI4F8%*%I5YKu9j?QLAI@bWCF5ak<Fa}#kMA{
z=F;Y-4rb<HeaN=t(X$(&Ca()sa~y$-C*P1Pnd>FzrzNMD<D=lCpp$H)4dZ({udG5r
z$EQ3`9>z~6sX?2B3PYMoRrJRllPbaUJF}bN2ht7ydeJ{iw;G_FOh5`S)7;+G1@Hk!
zwgyh5T^!t;P0evH;lvEQq+)JnWg_X|$+o!(P*@=CfTtKF41&P|=~gpw22+5KBR4<(
zm7KzHi}Phu;pB9ajx9OGAR$l$_NQ{X`Q=~6ezoR*E%Q&8<(uU|f4x_>mIDUIZY{{)
zFGsJwic1fAcjb!is60!<Kq#J5fswFNYS!y339(1J2@k5nQfY5rymfMI1+w#UlCA7j
zuJsV#T+7R6FBLoV^%y19yu@el#Yjxd+WO8kafa^*I>%1$3B3uPGe??2_&oXYqZ~N1
z@4|EK+wGFq*Tn|=#ztUdoLA>(Eq6PzoTToN%VllY>7{$_IFHz^hIk$S)0Gz%a<AWi
zNEH&L;d!N3acV**=6qDU#A*G8*p+rn5HpPZ5u?frf+`lD!ylFIr=A*)^jdz-ANE3d
z#c?Bk>B^$Cr&38R=98pY$ZqdiszXB&lLR4!XBd4cp*s?i$F=8ni{$OC$SB}_si4kT
z%DNNM2RmNDnoR;3>n}snrbUZq-hFJP#ZT`4P!pA1!0PzmW2WljSyolNY)(d((Hj;*
zyj9)g5yfzkf!n2$3yd55!Kuli>WQ?_{Ymm@Di&VCMB<V2XBkhNt=-8X#3sTasTU%j
zTz>cHj7V!?rv@$Q+b2?m(Y6ITT#u13g`9+Lq7okQ2TBKrgwCWdsGbNK6(&_S=6-jy
zF;d64Ib9>&ODBc@STAO0UAPek%h^DDrm%(G9Znfdn$}*+2RaI6iK2GZq7zT5X$P|G
zp^l<!mr_tNY@_78VQ~(I;fDI3vh(x%ke)09+DO{n_q}0uj`yBx8MLEDePGe_o+$By
z=n$28M{CrHkM&Q}3)0Myh{9|7l_jdfp-g%_C+DidGWQ~?cI2m=+E;iMZ(lEj4}Sx`
zGp(`XM6E;kb^f3PS-JEZ1Mj<^WVnZG9mfXjm|iuz(>|8!G(!Fq3(GX{b4l^FtReSr
zA-+VO_DYE>Iy7fy<dUE5N2P(xUOUqnf@Lff>oH7@%G+Br*LEE|H?G83dg+$!b-EX{
z-EJxrye?uPFj0e~CZn-N^d&t3@jkep#dT4;xR-9HtRdH_UtTzViiU>iP*?NeJAOI4
z3lJOn>9K|-Vep2@!Xt~ZX&0@8#_{M|P4jNb-Gi30(z&x4+LIe<k*;;}xLvXqYCdb+
zUcKj(Oo~zQ28_Rn@34?^nP4d-kw4N~D#F7;+O>awjqKsYkw)xjr<J7-!!&_ig9|N1
z&;%)JGVm?c!J)=G0VlGae^8=1`LWmIG<9>*b%(*I$`iR<F-eRV*-l#3)bu+sFNE-e
z-K|!0cn;MaiWF%d)@`AAsB9y?K*ieI8Q(b&8l2{ttSZ#1-R0_y7%|T33XQ=mKtJB(
zk<E;y*gM=nxpRjUBGfPH1-Iw|BuTe%i6w&}u)M_h9Z%1Tl7kJS=TP1)-~nXaNgcFG
ztSUtwcPXv8eDIys#yEFtT<>Aur@qd!8sc%-r@%=-(D|lWzME3;BbL6SdUnCxjLhre
z*9#P9$f(ma@e5tnGY6F<j&nYBR*hj4EGKwLoN9eY0upAORbYUW@K3X5?a6rM7?gD6
z^faNov%kv+>(jOqXE%J%=WV_CFmAl2=ju^%J@+ax!If++y%*T4K9#*KQnt{3QzSnf
zeI3Q;&E;~qaRJI{U&qsK1-0!Z396|Ieect}J6}zD`oQG=6Fzm|JbLY}mi)c%cA$wv
zWEVIe5@vk3x&zU91CIAPRlL&8>J43;aB#ZZVJ%;e@H)4`!QJ|W3r19*+S(LapK16#
zX)nzm?9zBPer=X-m|~Y;+=P6GzJc<@a1qgBac+@_>J6?sx0)9#0uIgLRk{$<a4A|+
z?(2h>^W4|ptc^Whd>247aZC0G#szpfe`)z28<$Nb{H<|8Z`$4e*0_9j%3OdMhQmJ_
zdpP2=orfbn8*4b?vvr3fJ{xv8V$&?K!4aF*3|w)xPy<{y4Ia3D_8{QE7Hz(c+?o%$
zH6L<oKIGPX$gTO1Tk|2e=0k4HhuoSExiueZYd+N0e84x^{2tWSe5kGYP+Rk%w&nvq
z)aTD_&4>Ct-#5l>lLudn<TqaHmdnYu>2v<riN(<a{f`s7={BO-P~Zv!Ukwu!gkXV{
zL_-B3f^Zc0n8c=Eq{${A=b+`lCLqnmZE9)a2wWKm>@d!K`pWKaF3F!E3kE|%P#888
z1_nW3aV|FSE-~O9ivU;T56N-?J{)kYA+R41hWpKbmM}CLA_$fch!I2(IF~3v3<Qb%
zJHp`GIEE97KT8-24uJv384ehTfdRtM2nYs>_#2Lar*MC_lsGB<vxFf8AuvHe7kra(
zC?pmHOYpy;3%QN1?Mi1WUi+UVABKP+U}zxuFbEnaa&R~VgZZ`OZ(ql+z0H5NA-5zS
z2{t4e7s4RW5S)%eK|y@?Hv$G$ejC5GM*#mTzrGN*>A>T*q#y(Zlq3>tN%Y?c7#y;l
zV>mVUX9R3Zg&|QW2pVX27#64w5Z5Eof)E587l;2`g>9GnuS1tVvyeyx*pN8QiiBW6
zK!^kz6#WzV`l7<Xh;0jrGXsByUtbmyj)owB^?_mGn<^g(R2dZcW5NH6ux$&uX*d5Y
zVK5j(5U4I-;(^A%vLWFJ2om~h-3s>b_Z-_a<bRYf1Xd6N6$DBj1C|g+7}%47=wItr
zK>9Yq{+!a^5`Baq6e5U4vmt<TMF7GOfIuYX*P_3jtUs>u5dcqu0lx;00ptNmLx3HL
z`5OrX5ZX3={W+b#CHe?}VW5D7A)vrn!0`)(fWV=@7JcM)9k%HT{#gkFdlG|_FhLXq
zII;+^B~j>KOFr@klK;n*K2ET39yJ1sh9Ci`h5%a<3;(s`BYz<Ie@^Ldtt3EnAiyaE
z1OYyv0bx)8Wn%u0Fw`~?`{QaKrz5a9JQC~y1Qvjbz$&4UzxH>)C$<sx=hXg|uD}t5
z7Q|^+oGT4rH8cir?#D9r#na!$ug&1*k4pZghYutSC;2cCL&3q86omhntS{CGeDZsK
zZAM#vmM}PQBd~xlV7Gx=jlcq~{aU^NPP&b(Kd17)2>zxY4Fn&9f}jKeVPFFw|3<!m
zPT5A-A6NP?AZIY(ej$L;#|0p8up=?hUkg5JyQ<g>kN@aG!Z5%V3xXYq0DJ&m50D--
zE}664JN!$&whR6rcl2Rk|6u@#fdKLa1z-aV@D25M?AoR$HqnDW%Ptf!#b6(zfkMT3
ziD(oEi~fdPg4@LFk1Ko_*nlAP0O1F0(-Of@FkF1}H-urf5%vcaKA;FK2oi%}2V$W(
z&Gp$7!9e7O!v2IZx08Pf8Q3q|$oq3@AIGpw@?cO1(DiIcpkT4MFbNI=2I?m(`Yn0e
z)EW9~2LqSj*~9{WFA)hspuYl{ZDW6(h};AZzexEHlS2aErTxCm^!+fK_YJa!`I@>f
zX(K2I-!}jKecEU!M#ZIDes}QXDjBofwKlUVv2e<w{G*SF%lk7GOPw!EoaT{azxC)z
zbsr0<$-z2eg|52iH^q}X?~T0IJ<=<6)=K>BryCx%&wE^~2^4cUqe*HN*f<_*5qIl7
zrQH=g*w5e=TA@7SOy|$yKbk-xY%EO6!i7%}_9)0OhyIlTP1x{_56v9=YAdPmYnX!V
zd7Hf0xE}|RE+!HOKJR0m$=UVnj^xQf*@Rs@VHq5w1Wfuz2h{V(4rU?aXb+@X>Kl_=
zymw}r@0DkAJ{x^^WhK7~%YCS0D6^bKSC!L5nqQsC>4VO~!m;!fd$n_=)DqIz=!aRo
z4`^A~!+O04+Fi~ZIv^v{fLzp2ks<9exccA)RL>FBWX^O}&YZq)@eMC6y{&0ztvvNi
zhS<aI`kU>fsTVvQnbpa1Gw%0M5L%w4>pJk9<;)`6jew@M)zFAyqH$_J@;A*y22;!}
z^fabDx0Vi`p17o#I*vh-_^GZp&(P06y+Y+~RWaP9;-JaO9a;`4h@iLuQ+^-*V(9p&
z<1_UQ3oNhmNTvUH0p$scqVozAD(I&#;=088W<;K)7Ir_4?xwvwbm>(U#G5NH|BT=c
z$BZu7YvQlCoOm8IAY}(7`i7z+FYCWJGCS<#xpZSqDLgWw>W<wI!$r;^yP+|zFv+`1
ztZVlBV0JA~Boj3jVTn~y)XRWpA8o%{c>m#Y><hcJQ!4Y;^LU!v)G^3v@_97B#d`)@
ztWt&0UV0_|crNQRi3tn+2lfUfHB^b~DAq`5c{gcE=Duw{yu#e_aN_K#9KNbZkMVZr
zg||Z!i0=N%3I<o7)K+5o*Gaqj9vPV4K5l>=?Qk#8FeSoEXSUNM$Y+XNO6741sHhnA
zX-C%v&FID#6$iH1o8PW^@yc?%Y6sstp+jZ%r&J>+6?FLerBb=?d+QMsU$C*d?Qx^t
z+~BU+zOY%R-be1COYF~%?MrWZ^RP>tmz=n=Q@&BEN3YV#v|PW!pyP3~u~lA1`<f+*
zO8(Hhkokc~hXZd;C^`_k%4J%~^jSPsN$9Yu+_i_>)I*5iQ$wiff@`C$&k5AH8GpB8
z|E;`PzMDcdq{|lX-#glSQx&HisAz1@J4JFWM*K-yhN!RQferafYiRL9xk_%I;^|K7
znPYgQjq1=-aY7jnyZvTUSe9OzSDKZkQD@y*9xYok=Qx#^lJ;VrjdGGS@NstxVFVwi
zt$k8%<x4GItK%2hDqnxR*=s1(V-w%o^P<V;`WrJVm{HxXVio6;8M0|S0nDFPqPeQ9
zZ7Nw;(lR?0D)5XPD4~`0FSB+o#)MyeoqH;QCR8h(=MKRbuj9@#QUV)&*3?k`<b0D6
zu_q<Vem_W$fFgsx^xTirBb&<R+w=&4k^%bv57Hy(t+WG(g*MX&=&iH^(EOk8Kc^Gm
zf?J;hhHLY6;23?rZlxzc5c>IkD?I_A$j|pU1qsq9f?M-}=y~%Vvo#-PYd*}@e3-5I
zFrVl9M%ixC`bC}pGq4Q@N&@Fe5abC2H{)jjHbc>D0HOz_I6saez63Yhe5p;={TZ@g
zpc)4W6=V}cfIJ4s7{TEn*ur7~olpoC_G7xfI0gVa`Q9zqT+BaA84M(PK*9$X4q~Bj
zHbGptgZ?!X0ubqKl>I?}>I=`H0CE9YHBdbQ){spQ;Pe>Sk1?&kShg*|LH|KdYKt&5
z779UQHw#jLCj}N0ybebT{?Y{nKjC|pZSKH7yO<!Zhd@E~4XB8MKyg_-49HT!e&XY8
zrOSYmx{b0wm@fOmGc*oq0~<gPAZ?%=2o0_v^7$K{ZQGigTIvt;4CFD;pb83TG!Ul(
zo}qxh4Eu@O{vV!gi*!H;{=K^YpA#)#c!mH<O;C^x0Wt_U*cpvPgX`b0Y+IzWsmT8@
z%iy>W6^I!K^#OFshDN}_{jXzbGz7a%%>H03{e@*v@IAmJ!3D7pB*>hjVHj}#>u4J2
zS?D&>wj1yNbv3sv6AIvg7*HyQ!0`%f1k`4h>c>FR7t6F=$i9wF{+Yr2Vwq65_#VK4
zFaV??LB1W>rJvZ#FG?0z)@|(i^V%N;ifA#oI3Egd8n8W4Aa{U-{n{=GZnsOD?#Z8(
zevtIsRAo2>34zPZ0;Cdy{<UEOHuZa+Z8}VUm@?p9;8Yn7=>kp$3OE@cv;9kj0hqdt
zus^8%;Q-kKmJ;MzfaU@U1L#<s=ZFF(UGQfX^DpcH5Vu{{HXX}9#J4Zxfiw^b)YKrL
zU}b?*jsmU*s4)I9<Gv+tn<KR8@BSI`wloJ4l;i-r1Ve)KEO2R&7+livhqP_!4IZ#j
zwk_(P%b|TC3x|i};4YA*6$EPv+-z|FV<TYV=m=`qtvn$T+vxgQbKuaR&9LeBN`*IL
zr7x`h9W)5&Jdlh09Iyz;epdzlx7ZK@`n8GQ!iGRe2JCmSp=bIEkspgFs+zM~)Q!BJ
zIks9S9zS;Uqn2&8E~0pv2}xaUcp=)~;MDz@2r7C-{&`C#-2p9UD<^!3-9(~zw(psb
zyk8rVEt^c7uwP%axaMhak-P1-hsRW#mZf<Gx1vni!O5(ti8Y6#U0yl8_6|v}bj#0|
zSI?x0KT^nWyf(LRn_9N6qJQPor~#X<KWw=6=#qKQ(<Uu8<=yYC^x2bwbPwNl_qls#
z&S7>zxru*b|I+xGPb^2H8H>kFUe&B3*3E<uu01cKFzY{jSR?l6+4pKOm?3_Dy!);L
zdy5D};d9Nxqi#jqv$uWoM52qikL)2@Z-z&S`7V=)PP}$1Fv~uxB~Mts`%|6;rT$=*
z<IKQe8)w4`6XxZ$7W2R(RnC)3TEkq$8EFPlAG&fG4WjSnIFh<`wJ20p_ApjSstRhO
zN1RTZpXt1VTs^gleTdI!KXYH7u5_Gv&9RZBct#}3t*giCHHwnohPQbcUv*A2U2d#L
zb3v=fl*@fZPhn%ij4PUNc!$t=S1!ktTAv4JH1pN=y24A6iq6Qah#zv~rheRNt}Uvw
zpY+_?^Rx4#uZl5Rl{5XXu0Lzd9Z<bd)7qaJG#_7QdMh)f$<Ie)zzKGbVm9RNW#?f>
zy_*z;iZ3mxUlE1q80dAI_uL{-!8=ZDu|re9KuF=1X0?Mlt4TkE^bnOJ_H|?2*$YC3
z*SbuHb}kn&Tsi!yLB4-intz@^kHKYGj<>EM0;W2y_YlWN3Y<UYmC4>mN9A~s>jaN=
zp3y>C!{H}r7CYGNbjEAe`KW|78+*DJlrK+9zq#ZUt1Uuem|SFwnBpJ|Ua-NVtsdV|
zoF_Cx7gcN3z2JP<KL3@&c^VJisX|*Ag+B#lrFo%B58nm1lfL2<XEGmD2WopBc{Nd=
zaA}8vn&N8N-RYC>1)8~f^e5K+9uc;jJ;gZWnjw?Xc1if5cBt?GK2c$F;+w}v^B^({
zHJc3n<tJKFR~~u$xk__o2EI=!A(UMRdJ|$NO}E-jjvsz1fBD!I3e#-c<pVas#?C=1
z>>RheYY~jWS-wo?FH9D|VnxxEczfDg3f|urxw#uZWU^U92D|^s4A~8ttL6JL=_k%s
zc2jbV3?7L%#z!KQC^r3RK$~Q0Jg7l+nn-Bju33d|ehD5oKc8e=&8!MR&fM#~yUD~m
z@SZ*;*{hIEv74D>2oYj$sSqtZFRxB);78$f?rg~cVwA|!108IZ{`;H4-yvt|YGC2^
zkIe~Zw3r?ySKK5O^%_BBJW;4SZmY0FvsB_5AAGosVT6v_wUd?=^E!X+;Tys-2hvLk
zR`k)YXXSO~`SS-%HD4rU9UQH%Uqii;7bQ@eceKpeasHt5TP!2J^Ch<vw6LYNOqW9p
zdJN<Va;dGfJx)bE>Ke(b0xjZxvySbQuko0?ZW!28D&J&0f{{Icb&P|c_CxR7$Aj|l
zCFX+AT$hm5Iz4W^do2P7ZZ{kYx}ece?rz(P(T$A3JBN2Ge{GOx=`ecuMlNC@idx(T
z?ey`MI8){=mjVh_JmI|!VO#_A^8-7rKh;<Jjd5j^iIT36Ggdb3cmTbjzHd-f#~%MA
zOM0G|%7RKizG7C1kCW0|l)|Y5e3%5GlEM|D-EoXI?HW}lq*anWaL~ffJR)5iT5YfI
zad0Tk5rj&nzK18iuo6#wAaLmMz3EeZQU=!yS@>%-?I?7f_CJW)r{C6Ma*$lSlZ4jH
zu*F3KQ66m{uG4y{<cN`M*fDS6DYm;AK9nR!UtBw{hEA^yn#e3JVM_QY>e;Ve#&7#t
zj>_1D`y~FEd(T_Aobvd@J6;IgrWxd@H&-&jFB@^HQsQtiDdyRu=y;+^MWsaCREBia
zfi30a#nXgd*XF~ISnUedxn62w$8CZ~x>V_p7cU+!U5!uDXWB@-e;EN6zkP*lpgR_y
z&L@H>TA4(!E-rNqx(HKQdDSy<VLI|Km672r(@KW)b+t2@WH(diC_e1dks#@BSGOiH
z4T2gv(pM8&9CFhQx>TQ=!95@m|B%6Vf6;9d;{?v($ku7(<4n#2+SEi@4L&@1w^R<D
zyL7hZaarchow-+}L@?d_`-U$ED`dAH-3QepF_Fik9JvX90X=3R7R6KC6t$m$;y`Z`
z-u2jq7y+8#C(Ajo!7Rg}##`o&8d@K;D+ck$!gKT23fLHnUKm?!yn^{MEJo2zx4&nX
z@aDFBm0=v4#Hw^%hNFp(-3bfv+WE3Djt_ghqfoElX$LLCdmgRHzy~M%hTR_yFnmzW
z8{V&{FXVCT>Ji~?kA8cv7URHjacL;LrkBV}OGaa%mN=P1*)cgQBUVz3@|wzXGW{~T
z7ygxo6+`wh-K8bL4ZGjv_tadhf!&-8*%NWsX#Ze>Xr}e9y+U=}?lH`*bPPQ7d)3KK
z2UGhbl<JNV77y&0W*vJGqSq{B^ax&yCA3!=YK1Qe7t9UsUTH?;=I1W+>N*9bKST$`
zb+q)5X{9{BrSz$eu)LoA5~sQ>dvtDbg!H~3<&iXHJ9;e%jynpD=Q-3SP&=R4S5=lT
zIS0}e%+W?+sf`mv_zzxT7#x~^wiY-?7DM!j`pkt23--(-Ep!c>m5t?<OHU_{rBeuG
zoG|J?dP_e~3lhOVcJKBPH6BrY{vqQ;jriRmZ7oNTy>_`5r8#a>9>46j<8tz$a2vXG
zX=&@MhwC|yBjRbEOdfveq?Du<WCa}$-s2y-ms>REuK$~oF+GmOR|nV!kICDQ)15Z5
za*#k-oc1vnZ$X}s<)Ma37R}n}KOJ(KNFTSmdP1jv>W$Z2_Keys(IbfaEX5^v2pGfP
z4boqiz9+pwKQz`SS9(gRG$+xZw6tYflAGDq;5PN`IQ)^pCi;VPf!AdmutrwSkF8$6
zxYH*2enUWe%tOm-$P^y;c-=d+<M<iQ{pSv&MvY`5k5qaPm=pES1$^4+kWsU$WOKS<
zcN(v98JvVdWi3AJK*he=a%;T1GBq1>hekFuQzqlN)R-k65nS#+{T|wMWP1PIV{AHY
z8_yT~6tvSH&JpG<?Y((xgZ@NDKxQfjYk5L8uP?(}gDh9lM2)PS=}kv&-g)pc`_>tz
zHvS;x@R!fad0DT%d%uTHPgKHtlKNR@TTdm)5!3Dy>F-JJ8JpAB%-s<}XHB^~+<Orm
zM$6ln=cM0aE_FSXwp~gzCBfx>M6>6m8&}hcO1quZ!2@4D5UA`Ui(EH6<Z{j0C_YS2
zpKzn6&~<8OBxy(i{%O5|^?5Q~8@9eBF8e2BE8DSp;P-C>Z@xPx_u2XT2CE0oE)M$s
z2UtCJ3o{4hL7Nylb_+8H^-Q1de}I{9j``yB{<jbS?)>UsrT;JxA0k1;GXjK-0L6g;
zAPfryMRA}|8!h-_Y}(Yy)f^&g?hMY*y1}8)ZD7;pFZn~%fqGd4NHl+;4x9wQfV%eo
zf;#lc@1u=P2LB=QKz%(Fhv$GR0*c&Brz1f!1y>n|ME*E-+~l65xr-|di`@ooZZ7(t
zp=}fW0`V*yS9%N}6j0y`iU~Ihn0}IT*rZI&!PUjo(#8Z1-42L>0;vBT-uzbVa3y<C
zz_Bgr0P}zp;tJ&emA@kIKVk>_9<spk|1au56#02W|5iQ1*FfL4k-w})*gSmwWx;+j
z6n88{5TxF|KT`c!cYGQAlh_poeij03NSrSCd0hiI0*xzV{hXc?kOkN}0-yz-$ACl5
z2<YcToq#OR9JqM>^E+UpBDPMC|JHe0#OC4YF9d%Hei0yp_nU<gm-LhzUSp`MR&*^L
z;Qc8w>Z*jh!nhb!ulAmZ%+0+Xi*2DUk2h8L7;@F;65EDfx?a>W4Bp9=yU5-7A)SYZ
z3@t&s>O4JL`*@yNfPf_z&f_(eLn(bTpO0SRcog6Bf&SZ+{=53@hV0eWh7`@b8ut6=
zPninNkpzom)LXZtl&=oHZGXISKGb0^Q;8ft)x{{8B%<S_yiM%)x<mWcL(VdEiW2Xk
zffjlQ_prT9kZ~dyJXBJ>=)LELQVjIUEzE~ltE+hg=Cb(MPyyWpIdxCt*c&v8SE5d-
zXiyp2S<Nk7J)aji{+63DYH1nL9ImpPI2~ioE)eo5P<a1!^H+_O5hc?|3&^0zN5Q;y
zoem`DT3a`gf^YBJH*bz*QSV!2nZ8q8OShBSobBpNg^+MV%9ZCKkK9y@Uk`Bb9pwmX
z3ZWw)awG8XSzB3HUt7D%bKwNLO3J0qj2&E5jeJfod`R8%yd@3nU;EW4h6GKslj@vU
z#5^Ar8B5?$zqej)b7w-WPx{s(@`xNMHB4IJ=7lMD3QeE-9wR-&7SShD{vIW8BjS`~
zslq(%Y!9;?3F5*fyd(i`ys15%2N!c)&Tugvxuq!JpYJ9leS9$?sBhGQSWX6#@L=&s
zkQ>W+f=Z1L$Lfxb=+5!ZFclTkjwki9t(|)-o`y-22cyH>yR=Cy5~`o*o|r}^q9;(&
z8P_%2QsIO4)HUZ&PYcT!cbM<@;wP~^UB%_i8pYQRtx+8wU)@<HAZc#(WFjO38H{-@
z5?NpHkwj^v6OwQwJKK1b@1nQ!fQ$#Df%Q<!2Sse63yVSTMBLtPrQ-P9bXfSI;QH`c
zbuMQ6_7>tR)(lCO=xL39xB~vcbc4z?y0e+>fk{uO8VxSx)JsL`#SpO_)l?5Ep3nct
z!$tP-c!Z0YM3hR+IXT9)peXi65z5_{?@dTP!oPRERbj7GgI7-><<LGGeNvcI^jQkX
ze(((7+wO#WqAE*5@z^r;wJ&qaE1IS4n4&jlNq804Nq%{wKWT6C3Fm!bdx}VoD(m{M
z<#Er4Cs@uyrxnfxR>=?MbQUr28^w5Y)XW`dI4$3LN1bVX<g~Y)Y_qsWjsyRV=_`SO
zsu!0(R2^R`k8luGG<bZ#>TTnmJ?DklGYHn_uO8>NPPo29{Yt~WL&G8!?fupvPuWSc
zp$Azw^GiY=Cb(~~+`XboC0|B;`8tHlL~HP<Rs=hP?x_m4X~l(e*9?xN*Lkka(`UUc
z5Lb+~pg3)GHW-zu)W1J^cV$<Z-aL{4O`^5L6M1DwIL-LI@zb~!gUctKTjIm(b_Xb7
z`NQ?DjyOEA+EaU_&vPiYjL1PDE%1V3kP+3uLIFICd2}#|PJ!pbdoAh%UFLg~@&{ft
z<#p|rY9JZsjE1REQd3bxXoT#Jl030M9$$d*<H}Yx+<C;&nrNz(zM-1Xynj}Q_%&zF
zCB+no%pzoHf?6m_LElz!qMo^spF|fAZ=+hERO-wrmOJ&FpD3Q>^5msk;c+%P6ix_b
zoMdsZFS@!Olua(30h4Q%tzmJnex5Sv<r@X#4c*72nFF5<@1m@o7+&_S?nAH&Je`aG
zz<H!}H=jyd^1Yh!)G~7C1#9gO$F;NgNUuxLC%FzSPUYiCVEK0DeR!7Ga7J6!gSTDS
zs=_Q(<MFW^rqd&LXXy4WW1>T8MGopEq!cXd$#*}y|EV^HqtTH!kxs?osd%8nL1{|K
zw-=rDvJE5$BJWbTECf558}R97CRvB2_p)PTYRGO@&?VICklci%&@|e~8;(wwUrO&1
zzfb9XnI-N~oF!a@J25adIp024H|T~`$X(K;;=8SbT3RN@<`tLAq7G%fs!7%K36Rxv
zNfV)ZUnQFVmS{BO!%<a12~5E2Gk3D+ho<zV1r<I}F((jaSPxDjYxnH1^o{1Zz^lL8
zU2hlT;Npc?<4K8Rt+f!9Lc{o@mmkGw*VW7F>T+>e-MywcE)|nT>O}+ZAWKt0xcKd2
z@Y2ZP6Ht8{Li%8?R#i^#SP**{!ii<h$pBuX+1#3%+*6knO-#jFPeH*=;GB4sWbZJi
zJIvJhaJR<(y$1`qw2L1@Y{_QMva;cs3hT58E)okbN@uS=y3LH2gmS-irFjXDbzU&!
z96Oii!$z}D=N<}`vno=~b1$BYN}kJSC%9?TxJ(gnpAAz=u6pxY7}s>q8~c`KRYodv
z>ZE!Pg|aqb>^%jN>QTwix+=r?G=6Q=YF53;$5f$4)98e1245vOmcV7BTdtS3Hvfa~
zS<#A%k6PPF>jwv0T}+R5A&T#EL_0_CecY1~Rqw)kwttF_LFCT(={2LP!JicQPZAeI
zoUBWB&eg+`Xt5^L?Ds>z^i<&$F2?txIv1$HY_``&<rF`$Qqo7-gXcfdurs%p#s<>U
zTs-?yde)j3KP~0`YVE=4%tyjwqwnRtW|E1PlUMoC$Chqzjh~ZJ;0w>wRE{{dFCq1q
zUTk0b`GZH)i`sU}FeIANF}shlKTCvq4!n1gYm!58(VvgYXA2KyU(~Tw^d7?>8YA>&
z<QzXqAb7)@Gli<!c9@%81beI}K9a4hig5OZczjKKi_tX|9iq(Hd?}0lofKSLYiXTn
zJ1!YZJ$YX(bgv8>p}Ib?Q`wXKtXYWN9Rq1{3z2xAnRs36oe?u^7UnEV$1kR;?c90j
z><Zsf`*=1LYv0<3wNJtwpIW?muyy@IycRAw#dxMs8MJTcotcVK>POI28%Gl-SzmF<
zO1;A;4{_w@rF>J`fm!9Ms$H=TeZpdO!67iNuJ~r{Uc@Ww6^>>--xJk~6k?2g4P2p#
zYi(i$-Dt6_6ie(??-s&!S1kQ<d3{?1V_eDI=A#RF+KU!7#u^lA{5te2^m4U5kE)d}
zA5$$_L^qb5$D6Ich3LgQ=xKHOV!TZ3I+1kAb?{)IL{7ABgN4Bo+@;&oCeB@ua&d#E
zb}6c`zAp5t1w`^XL~+_SF82sRrKlvIu}99W&OMf^$acXsIb@cu;>x3>64fzthvnsu
zA)lH%@{FU$DRd;HjFuJ&+Daqj<$WfsE}5PvMD6qAP-T!Pf2JB`sgfCYp)_s&>CP4A
zRhu`fw;1sKw8Cz5wRW*ubPdo>i>Tu3I1<G9u98lKn{mm#5j(a@MMNvI@q<`(v!%@!
zN9>1nWuJYuZ)4TZU2*<Nth!YP22PoMj!n1fz(98I^ZjSf5&=>yxO?1*+RxX|pF?al
z>_Kex>iMP)3>@(O%YFZEV_ID7{8dZ~4k&>W|AJ6lc?pPsz!g{BB#1i=jrws>$CqsT
zwp8BcPyLqD|7#HdXbS~_g6d^7NZz4A&KP&t7#vl>RStpTiC-o2wkM9k&-r8A!kwA|
zr~W~>59)-#8E8<Zh5=Cru5JkVlk;$2*!6u728u!c7+s*r0|W~ab-2P3C}`XOnl?a?
z;EDzT?ca4J*jCL1GU0!WU!Wof0dk#aP&5Zb3qU4-U!Y?V8h29lZ}|0n!2fxt{2B2A
zeXc+k0>Gawx^Vo%9oPFEy1p+m`Mh&~NY~%DM8ma@fC7e~K-C5k)MJ7e6Cwz%C_ork
zKp^P9*%Qsu)zwi*K)}Pp1M*crG=Xm$q9Fjd@sp+fO;Q2wfW(3WBw!*0sOrOlJ&6J;
z1Dsj<??`n4v^qLF;M$0}KujI1?D-v>aShxAwus+G_)quB-xCfxoq$aM2**`nf$TG`
zJPZZ8pCG~JMQ^t3_)Q-FZr{1jI{P0;1QML*!yT~&340{qE*hNe0>~aX5{%P)ze((t
zK-gNjxPHT6+>fv`u@}I-VP${H0dU;G#Kqj@dya4Rd;9VmaA?h!)^NDqdAO1Wpp}2p
zI}i3fy7bLZP}zd30sV37yiKe3({KGe6jy<T1p@dzM)VEhvW?=;V-dKbSisir$09cE
z{g*HP3BH6Y$NchBzOIJ&uEX9}P!V_sU`ybvE!cLSd*}Uq)x>6lxi6G_IZ*=+VE-;~
zRH@tR02;9cd@{Z*xBs4{MjsjU)_t2|h4@2CA|G#$2eP+DGw5q^3LU3^7tnV_jx##d
zq!;>;Q}hk<<ch!6y~5mk^Y$~RP_g{_wlGENIH*lw(bb@mr};_y{H0zR40T$>?@3f<
z76|+3g*7kY`|xb;!<cVTSZ!^(guEofSrfnaJ}zF~`6cQXBOhDO_&pSt<X;%5;ZmSB
z>~f!3;w^N(e8Vux^Fg=Gv&<oc?7a_!M*4!0iCiy&RH$lq^zDE_=`XX9Wj0XBXXx=n
z2}H>Vg|83T?x$dv%Zpfz%l2ye5GM8fri0RL|09O<O!+MId>0jn-p7<DWi`f`=C&zE
ze~P0V4_7jC&r{*-6S;Li8T$Hkkh;T|+3p#o(J)yZ>8l}Q!r@AXP5jA&<SH-^%d=Ip
zbuDHiLhCut2N&|IdfeJut#ODyvE-%$mQ<l)Uq|~5K51b$f`~4$x6Ok-&POjFbGE~y
zvbu3_`ep@UMv%Q+Gs-5lpL{6G&*YAS(s3=-(byzinmc^vkIx@fwc%KXTSugXUF<a1
z>J&zuPq}K4RCb4G;J8J?1%A2UAXAgkwAW+!N6in9Jufjx4%_I|V&lzkD=8^CJLtQw
z^!`i1T$>LC40ZlGN|eXU%l6)29lL$-lyPW}5m!Xo5-H-S3e9j*bml$T8)70h^Eb6^
z<g_Pc3VT?t#XL@acF&{7(BI7CwPmEer^lTJ$@&XNgwg`)Sr?L?MP%8^JXbM^iyj%0
zh(su_U%QfGS2mQ#j}QsyyXzHiy{|DxU4^&51iz4<jV5@v8g;(Jz5s;AM=wNT0X`RA
z%*Cq`mu`lOo9#RjF0lu17xl{Y2IY~J!yGjZ+CFs;9(zzRud$Ut>8;cx9AVhXDolB-
zn6>ZuG6DNzjjLh@l;$R{3`LzV?pP=7GJk9&)Z#c^TDo!DHeYhwM^MP?Al|Iy&AVJ8
zlex!}i+ANZxEE`8(TT7!9KK_F^i=+uKULO+TzTp8*tNxnvf;^N=(5m3M$^@p!o~WP
z<XnoTR2}&<X3Ke!W8CYrbYwf{&kH<jZ(>4RB|4K{|J0P@mUSa_L`#d-TV#3qxw!X!
zk7p_RW(?e$11Szk32I%x?K)+f*WjC;LEGtLfAYH9VWo4GH|55C-ngG`;#L>W@V~1=
zY{k{-JXV(@F~xPG>`E`gL7DCYPQq^DbzYYQ<FxJ<<#~%cJ)iXAWYB87QY>Ju&fwQ2
zSE%!>#Xo4l<<s5+Mgnhc_?0;=zdZpVC7?o7lpU`)QeJZG0Fuw$Zc;GwN{40B0OK5O
zMvaL4yUAUxP8|5rt3sIwO}4%X_e*)yu{pFANw+>u$)=UxzIdnnN@Q+Jw4%vNNqXqi
zaq0y(^-9e!JQ`yBLuAs23VL2M<@Srewno!4g&z_%LTh^(nPrmH?<nwlCLG?|ME36K
zV@l@?wfR~BR#oo>0{zgTFcO4fZLS%Fku-5v^@kmPV~gr1t}nkV-aAjSaJqO*SZ~+x
zvC1{>RY^i`uR?>TXHIZ;mfSTHvUv=<UAHf7V6;u5WGP>)&mWmYM*aYAP@&6sJ*_~X
z@cfmAPig@}AtZGNDaP&6l6OQ0n~gJa5@$Ofln9;_QJxpEx=R_W``A#+U^eZc#OTF5
zd^Sp{l%gzKb<utNZnKsJcc!%&Q%`yJ*mb?Vm?k_mb!iCY$8uUY;i&p2Dl@UvHTa98
zJ6Mg^q`WKM!t62f41T-&JgYe^8+7|<tar_kWS;5P_U@8^myw-!8128}e%R~*p8OHT
z*RSf8gZqf4CR<tM*n@(86g_^O@AygdxEZm1-4PVA*_89QGo6Uft=Ycn3<h#|UpZ=<
zwrne3@%No;5#KaB+cx%diUv4vo4L#FcJS-a@+Writ3p7KCHX5c@?Fm5zc+oGt?0fp
zegC&DVfnsl2|Kcz?FRy|*(UD`O+OZZ&mHu>5rEH4_I?*W_<;aycH{f<wYWvxYS*{f
zy7<@b7h?0vzl{Ci8vb4i>zjr^Upx^EsD1y<6xLlm{RrTR`1j#F5u^4BX>|$p6Zw`G
z3-H_6SEAoZ@U|Bj=sUZ9Y&GuNe^uxs{8^D-uJlEw=f1PgB-$-mTSrrQ#}czAv(OGP
z;TTTEX^yfU8GTs{%@vLPt&%bxJ)@?+J`JjsR=PH~+hVds`_?|XD_k9mdJAF5q>+`5
zE~Rkzkky>dGuf2l-lRzfQ-sH<G;x~nxHU~ul9;t~OvD*g4+!}`O7wl)UNUcd)NA_u
zjn|A^YKf#~QG4#gciR&CV=JqdrUuWqv})`oEn7KfKJrMc$h<GUn?iBp=~{m2MlbDJ
zS?+RF%EYryKg_V3or%6}+1f<8Q?ao&Qm*|rPivflLc!7LbB+_4&FLv`vO7hH%bV8w
z-?&R@A1T*;6)<;xAjc-S2s+5+Ip+D{d?I(`wf7Cy{G}I-<Fj5rd}9C7`q@dfM-kpF
z5IGS;f%(oOX)|x0mpR;RXMAupDa!0{;m(WFG?a+5W*0sQ&L-S)CUsNPxF-GKaaq~^
zOq&%#HVcV8ihJS`Oi$iC?cfHAS#0mf2(?NY8uag#3#GMcEscF_LEJ1L)s>}Z+p=mG
zLZytjs(_^?$kE&%+Lw5_4Es@9&~lG)p!n^RE#Am$kh_)=F*k!M%9q^ky7}f0==-iH
zjcT2!-hJ{|gchm7P?%b5c>3|`a#FYf2L}z!d>{+aWlfnoOtB+_WjW+~*{h0?aC^0D
zg^yq4G1ZmVU33nNH0)Y_VtU~Pog~4AT!h#DL&mmE>6N9$(}b6Y?d1@Sk}-Si*(PEP
z#FY-veJYjY77xYVz?$?c;qk2Z%k#x>!ctQ9-eJCZnPZy6Cy?xU-E9hYv2NN1*u}`G
zQ~@Pe=sBrMH|33Tp938!G^B5?%}>$J32?@#zAmF&soCGc`gAluEzg*xGLYy|1=cNl
z>h$3?LPl!*_s0n#1da;6$jE%{fKZ!5(Lp`|eIs{Au80}e^%J(w57sRC4b5Atn9n>X
zB2*`cpb@=j=UyWc;Z(O#Nab$lB9d=)29Hkqv>zUq$?K8(QhkIv@z0)|?K!bLDc>43
z-(90}jQ^E`IAMY~KU!Sr?gKIOn_FS*&Cl<|wH>NNHA4qZFdkY|t}|N4!^`uW45pVp
zHf!M}DVS=ykQWI_yC}7^8xnLmp<T<EjR+e<ExWvyDDmzv=Y_b_*}W3YXd#kphan!7
zGx*_yrj$G{j;o)qi#4Gqx@oq%WLcze?+(pXSZ?0_<y~VpHFxmWulDcfFiq?o8|Xh+
zbNL9pNUU;<K@x@A+BHlt-veZ8tSWae;l6~aNA#M$N1uy%h+vqRDANU5UR*l3K$5ej
zn5WNTde~CiN46lY#?TY0EdxnzBYvf?HWMnt`jYhBgW__9lm+;K-0l#jc8>{`o615r
zM_R3-<zLp!P}m+4X?0=Af4m^_e6Ou9-9rXya;~%HBYGdtGrVxJ+_B%f#?UdP*|2!o
zYVYf7#R5|gDDLrTAB&gsRT#AEI_qO3E_OcZ{M9G=^yGJ2oDLVroNR25FxM_692p=x
zPoIhJ?{Fp8pp5KY$eB1)){|U9a;euRd{_pD4f6BH4=D$}aUY{mxi?Fqe6oF%QN<E!
z#{OJgWayO<SsJudGCnXqHhz>Ui$({*b2#Z_>QvZ$WsRZAh?CYE@OCN^sgY&Vp{Sr#
z3a{$Nb;87ilv(_>pANq>snO!UXYgn_<9&iL+5ztUenQ4&*014ioc!pq`r*E7E2tQz
ztEnk%t5OZU+}2%a#lwTeqOo);mkzvI+9yc0V2#gO_Lk-8aZ{b18YN8$hZdu92RDCY
z$E?izbNSJ4i4NRv&pqZNb^C*=fV<K0@F_Q~`6}`SSx4d*vy}~PX-;E4rm{`psd)@8
zAztvuCzyv-+^)0O;z4@U=KANw7`b9HyY>~Z4{kguvD2(*!N_@@xt76ltcOQ#9)=|;
zUnS2Xn^ot1KA1XENjea#_VDd90TS|Q=OR{vmkHfcuieIpALP(Za9>zx2-ye<^J<Jp
zwXSF}LMrK{TptbdP{yZ+OvGRwy*Q&P&`h!VxSe8ng@9bB{ZrPf+7p=4k=F)#7m8Ff
zd;5!xd)qyZKgbwVx|fV+cV$iBbYt1Q;LIg=#09)_&b!{R96sJ1cXYh7=AB_Xgmh2N
zr~2}A0bTh+w+0ufC}^%)-c1_toS#NqDw?7gI~0?hELT3Wla473Pv&v@Xl;kX#3V~Y
zK#OJSEgH!O{i4Jvu#cKFUjFeTv{#R%L=_b8PU_Dh=up+{d(Lt@;d1RhZtq<BAg9uj
zT%t!cs4&$FB_E2-Y3MmI&_qAYs5qHI@!}jlQagM1;kwrvo$Gy3kL&1dSI^(x#e5Lk
z`lO)TX9Df#g*3(<=5(#<s>H*)Oj3A{N-0oFvdb4HY1DIBKG?1zj`6a0kBGamI`oWE
zv+L^frh-nXk+9&>nmzV5QZ}9IszfChQ^;m_-`Hrc*+}6t58TV=5pR6z<WcInkB@g6
z&~veubF3IQeqho1fqU|UZi3%e;%%DoZ?hW!fB}HSf9sz7)l?X<)g&0P)g<`q?8N8y
zzPKO%EIWY&C=m*j4WhtFPAqV$k)Sy%?lde44Nefle_S*sf6BqZ6+Dr`eA`i|&(!{q
zvfn@v!PzG$I93fBD?&hTGH?Z*=KxlN1fDbu`(Glp|1JEv*<1Mw;W%$_tNHTpJjmkf
z0A}0JUlweA)1LSzLs8%e7!=e*ZVMy;i2WDT{FA|(tvvx+xP9>V2;w)t_cPG|kOXJ8
ze*ooe3ilUg{m}jWE>QU<o&n(CUk|?cwxaCY<4`{<4+EGV022{gZLe_%AgCAxg7f<b
z;5V(jzLbt3f%yNX-~CKq-{G|^wg1cP7(WBDse<MH_k8p%3`K7rT(}`0O60gIe&vWp
zEt9hdhui9kmKz=RT}<9J5e~2Y3%%MM+Q!TWbzkz9di%{(H$~>_@)#XH43E9vF?7mY
z<6z#ajH^f*q0aOs=lKJkPLY#B<zhzh7}A0V4onsIdm!0q6)q}%%hNL}k=L(tC92Xg
z@#$J$la-7k4=0t0zMFp1XXb74v1`yzYl>uqQXc+8?sPnycPMWgWkQbn3tdr8NT4Qt
zq!OYfM17lxy;lAXCP;eykoLg)edwE&ii2H{zM#X^uR9*J9ulw(jemZeGrw@!cH9Ae
zHFDm@RJjGUND(+uH{LW45$mwZnVkA`hQ8ogoyX)+H|mxGzdFs%w&B1NCdgo$7x@S2
zhr_aII5j+T3KieFJrsNFSuPNb>1<0Bc^jkiEQ0JD<+Gg8*@@nz$4#Mk+a(WDGe_v6
z*Q#<JajG%ua7&-NeovFmH*`q(k^WOYxqyp9&x>xPXR~r5WAps36=$&-taQ7rHo87s
zT~l^6*_FNSufi_*PX6gm!%R)ifqkPdWi8cw0wVSpr?S|#n5ph@;T{N#VU|Z}IjNNu
z^^~Vq<#STZxsf`9!(m6lrEe8Rr--i<Rb+NewLm{D*Kf3s6&KA@o$~r*W7y+&yfy99
zn)W$@+66=1S7n9rW7f|eWzum|guIIMOg|@ipibY>WAGxQEl<uq=#`YzqgRqeOvh2g
zJ3ra!w%<v$i^<@7!V!sh`}p=wQ`F8OF(vDSi_lAy^8IIQjmp;AymCp17+D9a`$Q%_
zj>qp62&nKpT()BW{uwp@fHYM)BNh3h_9c6dvf!*tB@flrn~~g?6oamMLzFg-3rP`K
z-!FXjK{T7FH_Z%Fj;2!=iJMqy9L~IJ<P(2AKQfG*$G73^-kc1-@`mZmg+L*@Tdles
zgyUrdGm#JM8LERlMA*(Rmb~ina%XHlWm4(?uuAx4FF9?_47Bu-qlnMF)<ItriV)gl
z{`|~ge*F=763Vi1N{p+~ym%xw$EorYQ?5CU0?TNnnqvnyEmIZ4ZBg=Ncj@b;CoK=2
zQ=Hf2)4xb~eC@+d$$nA27<jN*0k;T29MuQ;#(oO!bNEUkIqB4{5>}UAGOKts6DRxG
zT(^7<pUQmS4izPndt-0(yxxcA-0V`OgVBXaWlb03SWo<ikDivY({@b3?@v428;XjJ
znZ|$aAXdtJIJ7PHvf{I6fulPIPqb@Y6U-O(<rI72?*38(@o6{VB?0Yr)!DqZBH|M~
zU0lsxYG)^FLZ!HZr?{2frYI-+`bwiOaK|w)$u}R#Hj*z(u)(Bn?5*3?R6wopaAHs<
zzZY9Xm>ss07(Dbw^zb8Rr@W7<;gGoU+D0$_zV^T)e9U$Q@@=kpXH@qt!O5&lD7mHc
z<QNG*DKfoUVX}3>ax39uhepS53S1!^OA4BKaOEOVmA{obMChWKb$r0NM7`9*j|nu7
z^bRL}q%OYd)xU4`>IwO#)Cxr+b&t8lWZ$=!3mE!*PZs*J?x#jxBuPvee3O#xHx;RC
zPwr##DxH1O;FhwvK~SsaTh1fWm*ef9RY6I1N3|Xc338-03tW4`{pvu<-jLD6)z^i@
zwP6Pv426#EZexO^SrG~g#1Yx@oi}MVWMJQS&R~bmv0e5$S?lj$#Kjm<!RqJgnk7<+
zb`#dt6dHqXs=72j>~VU6jW^DxoborJr6eG1;T=Ej$RVpKw_M<PAV!roqH!w5er%u4
zrHDw~Ox0la>;qXzw1;Jft#bObqq~W;rY)yh8`?Zx5ij-iBu&onigE~v@Yvf3U%W0t
zP|GoJPtJsfS@JxTu94~ej#qp^W6xfk9PZrJed^BA%Go;J;(cwFN1mC6YwjYpN^6_V
zVG}yaAHLi3rs3-Amnbh8@@qTK8is{`WQut<byP{3XXkPDyA}_^22xvNJ8w8J=Ix!*
zA7wLiP&jekcK7*_R2kxiSJ9#3)Pb6zbxN@!h0?|x`2Cb>&h2yB3iW;OifyMpcG*4q
zB&kOJzTIPAqGZtPS!h=Y>1b}tVGDMZ@Q2RXW}N2p2VRn<TxH5*P7Hf{)#}*FDvWk<
zM_IlM#qmr88;8d11K9+>@iXCzvoPDMURHy9U9@y-eF|PQ3)0$2<d27*jck|VIjKQP
znOw&CxQ?)dl2C$!Vs-9j)yI$Gy+!L`7NM789<l4ZZ*OzTl)Lcyo${n~am;YdHIr6O
zq|2IwKl<s;6rMTdB9WtHuP$4-om-^ZzfsP-<nTO7eq=x4A-edpCkL*UbDnQl>+J01
z*h{D_FC8tPbx8d-*+ggzx{}H9;-k}ehdx9P3h~zrd1fxxGfH-Wu5M6F((w9Q2GoJJ
z)|Ne&nQULr)9?*4p#rEsM1>??(TL;_zQ8T@P!{om=AP0yW=r2>&!oI+ZBGlq-XieO
zFsn&o^j;%MyvaKcFvFU=nS&+oJBEw$F$WtGU0jW}rp<~^ne|bmF%%|1r0O<Z_W$s)
zH_1lvG)d;oxcP`(8@-g*25S}{IxzOBjVshp>}udg#VXya(0h4^plLu7_Gk&uS+TKS
zhU&_T&Ly_JHp>oBa#2WD{}nQKId6H1gW|lDh*KQ_8v%HJk^Or;ONVLGh4@uZ?2y@^
zKTU;=);W;j?{TINvpByevmS#SXcU$GC~Ixp7sO^4B4`pcS8ioEa*%{+%#&>OTzMgj
z%uq;D(;1?IXEdSR!<RpJ`@c<swO*?~6VF1IXhyKVw)f;HixpMC?A89Z`($kvM<A;Q
z>JL_m)6iSy@wMy>dwS>my{gmqVu!Bfs~Odk5VdMV2i4z`5pNsdA{|Sql_fv3IP%8C
zF+AC8El1v!{(@BJdS}Pl8ubN|W4Bq_ue&w1Xk&=d&)T1!I~o7docEp{=B31)^Hrol
zD>+TCDl>%{_FZ;NToEH!9ay+M-ZJSzN*X16<YZ5-g#FAziRgHbD374XfyB3SF;3?c
zRi3C>pM9VC?8Cy+Q+|Op_5a7(R{+(OZQJ7R?(P=cg1cLSyGw9~;10nx0YY$hcXvr3
zXprFU?(h%k?tepf`rdx;U8<<msgqf2?X?eQ&oSqiW8|_XUd5K(D{fS%Q>+piKSVK=
z+m#=I8HL6|K;mJSYuNIrJFYpu5}2jy<&O}UA?c-LCrPmUvZ;sn2G#wjqkm#mzW*~n
z?A0SE9`<8<2ZEWN*tZY5O`>X<Cs@;a!cw&9FY8N&VG(S0@WpR9Td--DN0(<849W%H
z-p}DrSal|gZHSAKRFQsaB7zDS5H`CM+>SbhvOtj>d7uC<!wco?iM52uAG*n0JAV0y
z(9W#L^9ywL!@J~<RP@)y;~&Z1b9DBb^U41Oo&C-s``xmI<#($Umfx*bSbn!!Vfi8c
ztMts44KObBQ{Mi6L~l>B5P-DxNn{M5UG#&1!46O)0!W+xoi5KGWU0T9e|}ub^OOAF
za?1d8KL!rKU<U_)bMwS4dlC(22Y_UN@DqR^@=I>nzocCL%;Ng(Z2vnOW959376Y)5
z01CH&V`6=p`+3sj0cgyy{vg==ReASsg(7~t*Z-Y;J*nkCN!qdlupS5CpjZH;B@Vy{
zAUl90$olUUdH!-A|BFJ{Ph1YBCyjN0^$r(+n*pHRJaHBQbPE7MiR0JY#eWqC{>)zd
z?T_JqdtlEewg7W&fN}*9K-C!_UI!2$0qhgd&;!6ifIi!=_w{3*%*e*o&D7Z1%*M+2
zPlI?*wiD0M^zW`n|D+xCOzQXvi9d_|jTZyBod7V+^;abR!(Hi*Q~8@<c7Sj%z;ogk
z!GG>z{$gcTHb%g78}l!Mf2JY+&B`nQ@jVVeE$XiYgnxHjzg;rmn+YH{{ELO3sVx4@
z!cXc?T!6Fr%fdgtyZ<<^zbQUEYneVPK0IR>Ju5IguSEQ*Nb$2u%m2LG@GIv>fZQe5
zKP@-(XjnO<ilID|>UM}<48BZLg-}veCBNP44ozJ%SXeKN^=Ka|?smgm11H;E)V(}w
z?+lxiwp8Vtf$bV)8Gj+qzYrphi5%6+>t^S5a#tfk+M(Xdg8Z>8i_TCfzd!dVrGKkA
z#Oob-oMpx9&<I^;*(M~r7J6LO(b1Gb^V1Ouo1sRHJG+Pbfuhs6C`ypEQ;EGE^F(9w
z>e;lr3&<!K*9*_?4}KZA{0_M0#8^o<r5s~Tao!(~%EkG<7s>X%MHgbuitD%NQo`#T
zf!S~wYlGNabw7zU_%OKi;Zp?Uj(pZKtIa7Q?1$b17lyN5HmaS^g{fU1Vs$z>-sPY(
zKb(J6rXmSdeU}sBF#6Txl?o;q@kfbrXciNs@)7>}2B+-qA+M>bp0Bn9bxD!zQn=Zd
z%zR$*lrj$A+Ac2};In1&az81?0w;+!q@X6XvsdclKDsK*JMbXRdA)xb>nlwh6xiaP
zl>_2w6dIN&#6pegJ`>|FiCV|@uv2e`TW4Wzsccg=7%_J)Ka25tA>ca6O?s6p*?zOr
zR>#@`hiZ!nn%z#(k2ztkfu>nimp*X|tO>6th|M}>%4)0{zT0n#gs<&vCwP3cm)^=l
z+s5o8uDh!2Mc#y&s;xs!H2e3-Y2)QE8w5N|NA`2gW(}#XYM54p+givMIgqW~7a8Bj
zbBSq+O?c21V+B)B*IKcZ%N?0T@}v6w&x?&AfWVFI2v|Y%M?JY$kg*yVy${#&(Ez{o
zoh7H%<6pOSDj8H6WYpuR&2ij>73#v83T-<Qh*c=(q)|=A^&42`XXZ=MObdcD%ZqOJ
zDqm|c8k4&U_D)P!geTC|Yz(PE7?HDsecQPJX+Bdc$D#O2tR@bl3T7v`4dmD{619uZ
zJ5;DMQD&^{!-~B)yi%knV^aHRo((jF04gDonh(lzJBoSNKrc;%clJHeq>=9Udt1J&
z^CTvUex=#bN{;-gl#LKJ)c#VErZ=*@@Ps7?9EP27eJP5kXNo2h1|c<d>lVSxySX}b
z#MAHl%ZD?+bTV<G9;F36vbe8eCX-k7chbq3#LK8Ll8{;Oz8@W?a2TK~ZUs})^J{;M
zmrb$}vQP>LMGf$tb{19VUy|q`Z_On1U8Aef?Tr$^Sjp6_wT|?ysWhZH+9Yy?0k?5r
zWp9V>*1j8md2@fr#ER1#5aW~$QV4n=&&3oy>8)CxM89r7GeA!<(>Mpejp1-OyB`96
zrUp>}Zab_S)Sarjd|g_!!5P($0@bo-PG&77dkh4F_f0OY(F4bKfmTNofoN3c^UeZ}
z?HMtiZYyu4ZUR#)N7QER6bt@~d(LgvSZeAA?yuX3jw<X{U9<B0^8?I40&K3Mg{4P~
zm<ylJ>0al^K(W9(*}o~Y@<9P%P_CMlCBhi#ss^E}>-&PlSH%N<^yW((A&7<#Bd?5C
zl#-2I70vg?W{b|&=_*+4q2ghJO?3Oi9%*>Nb6T-P(qq1CGex6G&Iz4HH}PSh6hbXz
zRMew*OARzM9p~m9##VpjgYphGu%-}A!YNq7FPWS4fn7A|`uc_nS1{!%G4U*PL~$bG
zdXmX!3W!C;`J*it-Tbnv-rrTOYYBO27hj;!MZZ9ca;bAjg8}#J9xjmB=qvgpx7gy&
zA+3y7<#Cjyd=N$1p4*Dof%`3q51f~VR=1};!N30!9C>)aJb&8-QTQ!<=z|?oDrlOG
z)FIc)LOE;m@+96_9|3N8HhW}@rhca6ZT)ihYB=!^B5}8CD%vS+i)UDKu{}TrbN0I)
z+BvJ3Yy$7Sy*}RV<aZl1THq%0P=6BB*BV`|OrAjUSuBNKpD&~@6V|s=*!aQ?hFGc>
zl>Z=b4WD|uceW`?vHFVjW{7YUH8z^5om?mN<7bV!OMd}L9iP~m{u`3bZ>hTiQ$uG!
z8kcSeBJZ%5@X=<YZK;x{rAJY&JlEgikwg?V<^;d2YseV}=D?D^+U2XURs{w=aS*rm
zO%zb3r{6qwVMCaACD&<MU}43N8X~GM+pAEHwdFbosT5maCFk;kN~mX4I~xZ|=&|Oq
zcg6|~#0j|$q5#bv8xr>sGx$V-tpF<v(ebu18asJUVmTvdK6oRw$Hd2-7ifk_lXm25
zhxPaj2>Dmwi+pg7d~~02ZLE2S#&C4WK_K}&0Vw@N+yRSli6lt%Ss^VehWWQ;9jOq#
z@bIIAGmPNDo)KjeU2hevr+vlq>yOdXY`AmY!pnbr<=43fYKi{#B?NPWl<zXsxw@rt
z4xUo|ab-4?;#nQ3mU3=%<sRR{ts#P%f~qJd4HqGSn_igz>^NOY<H5&Xq$TM^tj^s*
zN?g?dImC`UoGwbNy#BJ;E)&F9ZdXonKgf^6TO5nPmRfH|$r}*7)_t8KsmbXU=C+em
z`9FPZ^>CVgtt8wm5bNg`tA6UX00DjU2}n{@eP$;c%Hwo(`&(Vr7&8%OzGb8(ZEe}v
zHC+?#^cAtAxCpL=So*txYNhDYoBb-$1_&Q^zxQxj`DxV<E54+v(I4z(R9*dj0^0h3
zpqu1*BtHR3KvXjjf{^u78|a!5f@d2I=bIuyyL^umZ~hFN{-Tf+-*Qx;6Xk=P;`ZLi
zyAST@N^#8GN_cXgJWW~(eakv?R%VXmA$ik2@7idTA>6#QRsJ0C%`~)uRH8tQrW=#X
zPb3-o+R#40P%Nc@;BZvMSJtT{xA{D{P)x=BRiF*4?D#n$3!i_A$t}zwvv@cQC|h?F
zq3ayCK$l-B?@NwuvE|yGTKy9S9^LW7>PH07q_xDrJj`w$Jl+PsrCk;y{}R18B&s=N
zs;*eD!4UybyG>LwL?hWqjSm70L`*n()iyNlVv@GIox{y`Ub0jHs@B2bFoZx*qt3n-
zE-6R~^yJrI+nar*^WeyCit$hc@9U5Cb?(aEXwaE-T9(BcQKccY(HG}GP~)YXuf-cn
zUSL&1xoILa#JH0@po*l=9KQ6au^(qRzMH`+FuOC6$Ks>g&UR8oUoMdhc9oEObEmb{
z7BaK^4vK7~azEtV{NtFRd2ExLU2cTQp?OxzG51Jr4G%(b1xEHzri0`1EA|JSMz7e%
zjkVwt=SN!%Nr5tVpV-{SSzvk1$ItM=-YXKS&J(*jE|V5*EzEl(<4(<HjYaJ`{fk=1
zbY~qXpx4JfMf2{4PDg%~uccN*>kcEA>6yhFzxsJq_)c7o<?&+cS3klWD@2L&*Y;(l
zjV@9bP3F?hpDs;)`C?O+j?n-XILwBaoww+o{4pp}M|!=4;7UE88HB&fK9RSy;iU<x
zcPDKP<Eyf6-=zc`niQa<qWNi2FXyP2u2tY0%e87NuWC#nAT73>sS=JUUVFMvMF5e;
zbr|-x$H{0Qo9ywsN<a9kimlk#e|j|QP%?jLBv_8~fZ_yZd5cM|6Si0wGcseIFH3c;
z4FV>ZCgcdCS3%(kQbzEO@d5TaPITBVAFSS9Y)+9^_v$$BgJ+%}Hht@G=G7&kLZmE)
z=l#XmT@!XKc%ibEM`UX@(2(>!Wt~@l2T^~Oe@WEC1JH0JVbaMF@)?rx4UGanh;goW
zsA0k-W60}V$0Jr^iZY+?4~UtN6-vJ-#r$XYU4V-BQ<Lr)kMDoxzWWOv-=Cd70M1cQ
zixB;T&Mm<G?N2D?S?s@W-9D4?egZMSh-Cvzt+4~hi+=$z&y=?RE*OBa0TVR82!0wc
z`~v~~P4E-w;{wdj{dMJ^g@yhmnB|EC0Jt%K4gPb>`4`s=5TFGBw_gSSx#j$eV1U>r
zpu+GA)b~I3S@{#3dm1eHO~dH#Wk;Sm$v*?VXBhBFjP-ZNf`7*C1juCn>n{D?V}6=w
z{HLI=MOoHnjuEcqR&{F&D8fRr*7&d%et%RaC<EAe%U!{coN1#>!@272RxxwFN4p$@
zsxgi_hGg+=c20<;q)02+CzXzC@*${qW(4cu#q1(Rej5cu%tO@jT<Yj+?03zt7SeBf
zl(L!ECcMUv3Yq*1Y1rbes_HehAu)@Vi1ng5Ny3ewBSa9)4U#vp)u%i5wMAf#wBtfx
zW@elEle@#mLi+Sn*C?(qMEaJD+~KUHa41=;USiBCgW94HlqQT#p)BinRF&h~X>#m3
zTO#pXO<64@5LBHe^@h);SD8c37fZ2hf$V=gAbtV+Wlt$cJI9ll;G$8J&Wc{M{L(b|
zV=2=@%L1RPBfDsuUdB*PJ65rx-<TznmO8TUb!<=kDcUzScSIfRVBv-b^V4nM=|S&|
zR9+Xa4<iXEtg#@K4RnYL(@`9l99W!5tdZ2xRn)eJQu8hrMHxfTj@P$x9BKt-(vavf
z;~H$0oW<yVCv%2}CH(Y0qMK{xghN7KDm1A#S$%kt;`@e+h>)jpOK2!h2#7<ly1}t!
z8+w($S(ZcByxE83h#DvQs0%VlI!Kp85-|`9J818V?j<6__mXuQdPI`)Ub=+dp14zd
z0XLPReJuQ(H$0%5{Ss@;`e;{lTmJlAXgmd-n`{?YLi#(zy_%H1DsL2>%2dU~at=l7
z*CjE*7BEI{Mef0*F~2(Z*$WH><sl@4-Wi=s_ak>b0$GcXx*bQ3-iISL#^W-7vSw{x
zzd!!6jK!-49nt3>zEp}mG?<c4dlUQ-M{X1=B9F1UuTOrMT;86hYLCqUTk*|xJEjqG
zdAa#wrgR1*c%$o7<KZ$y^f<q2&0JfrlAIxR8ms5yK?g5zoQVn19ji)<N>2wz<_k&+
z1)_)Zv!ts6v@3;?1}tr>Ef%`d=~PKMuIBecoR|k1PVcck;rqGVa>z*BjQrwR0Vu=#
z>yP8tW&0n0KhK{P0Alq2f>iY{JMgDL*?%qj^yT+sAHP2<{sZUx$Kro{>;6sM=~;E@
zS>5TUiqt>*`gkTP{RcIi9dH3Ze*UxV(?>PwxH)#X)=kwbm3^7v6XKn*)UPtJwJcW7
zlrI~|cx{c{OH>l^-hJ<Ddm$iDxo$Qm`$9U5OaSTX2w10~Uc0~N7BjZ*6F}JJN)8>C
zUNq6Xsb1&Bppuy}hPp}^$q43jQP1YuNbkPiqAs0mBIU-)m)%7P8g#c9+W6RaIm(@A
zbVuKV73%89j+p9Uvtm7uXi-M00%J4d*Qt_WjMdNFUtJQJo@V3N1R_3`ztLJaQQ0!{
zXdV^)ihq<Paj;unpgC*HT71tdTkG@|DlLZtKWsr>`CQv!QxX~0o3|b|q4<Jf0^Mp)
z!FqAwV8Qv_7ys$>YZ`P7Q=<n=^YuycJ7ua&$e5H~8{ODh*XuOrRauILMaPJ|gzw(G
zSG6l;*0P2(;VT?e4-!L~hyA1w@?wh^sEZaMnNre>%s7`$m&@mUb%dLl(Ne^H4t^7D
zy+t6JwiT}jAnum8`4&s%(tvJ*1rUq0QSU2RQKF5coDs#R`Rn%?S|{I=LCj|FkuKh1
z5Tn#}_JYsOPt~OeylYuptjIT1?Caw-bMobw){`XAh9iVSeh7PcN$;tpePIB`qGtY8
zhJ{3y!Z9&+L`;A#A(r{t7D{&EhEk!pq`cR@H=1QMo!OIwGuH?1)t6BC_w2nC-0+Sd
zWG18t=0#_=S39xk9C?#c8L#<Li*cr_Hg09Lt<1Q!HmnVXrKGn(XnBL@K<Qqo8^DdN
z!|*f*oDasar_^J_g=`2%`g%;(xIFkqXENK`v4g{qEk?T)yw3YDJwBKb1?K<CQ8d-j
z3Qj(?iDtru=g@OW66oW@#WA`EPRc}cvy*q07k?qaSqbFz%Qi@@xRoy9g$V|nGfr{f
zv4og6K+OG+Es&m#bWuEEgj}%!0>WR{_J!A7e5a;K1$|uG-W{Bw=PJ)1?AijgIa);p
z5LHq7Mw=5`-QbaB=R}LTM}pPkcSX5OMZe#NaX@V&0n@t-l3BG>m7;nj<BtW3I$ml;
zz%_2<F7~p3pbVnVA#Ek%wx-C$fxe1_?Hdu;#xRdAHgUv_&dP0uDFjzF`ppa*P)vVa
zpd?t<Bt|A3nvA`^{YHpOj4`JzlMK)~l!A7zpC4#*=>_iyvW9EqDN*T1Jp%r`_kHNa
zwqzHgg=fAXN@Q&Um6Oq?j;cdN>DbE%X=E`au(vnM>tGd|b{a0SENJ53f<6;1<eB^a
zixm#@5?1Bk-DR9U2Yphwp3%~STjD{=LRJ}*m6D0_(9sZ3s<|X}Yf3~DE<uch$zXa7
zWvG30Iu-;WX7UB>two-mH&=~$0Zh9{uXEM1`_xfm38<0Dg2N^wMhZ6WDM)k1Cv$^R
zaT9<D_lfe-=YT>60q9kSJvAyHMnSE;w|t&~!3@FhOBpdTNAFFQ`+_oogu?elpf|>Z
zz~DvRXCS8gt}yGrWQG=UfK*Xh-jDg#YFCI%?hKkI$5thWUkJ8!#-Kvt(}nzDLbdlq
zR&y0p&kAxG4{`L(5?BfZk};GxjDLjpyQJPiqw`C{FpyqL{d8K+))a(0LB!AmfMyUK
zuNpczPbqLRhbY%sQ#IUj1eE`I`M0-xJlRIRV6xJ4akat}w1ip<0-*!(l6=bu>*fpi
z?%Z~Lu%+<Dp`GgX>jPtq2@2;O`-A6jK`by_#NfgsD{0B1RPPy~?P_uQKZ@<mygzK1
zJ`C1FB#(Z@SL#0fsiI?)Gt=6Pz)(G4C~bPe+<M20lB181W@}VYCG@L?r{#gvoB9Uf
zMjY?&BAtVkYty9q9Tu#}y)osl-3k@eX|owNJJ?$0L2L()*HY9znljc;R3d!{f+<eu
zx4eib=q_OqsQXavcHwQuXSJ@XJ;cOqV$A&@$3=R=RAD$55^-%tjb7FjU5lEG)q54G
z6pyWh%qcJcXTjIZA9#~0r|W;_Qc*oiQ9-bheSdc1u?`<0p<9+_aWbY(PECko4F848
zIa)05j$6$JQPVo^q8e!tk<>&d@`DR#l#PM&Sycl4P$1qL!CUjsbP2sdB55d<1X^%Y
zdU7)R65=l-wL9<53sBr&!z0{$*@f&`-H-b;)*X5@B%WvLw3a<5SQwMn7O+gq>}+Sr
zy$z)jhK`g<zjr)sL417z0&TaesZ$(Wkyo+r>N{uvnc(lWk*dWpLGr%W-oap+4qho3
zV~F@t1aAB6j33z1K-u4l*ZE|eSpv*<WwA1x95oz_ef3@ZhyLakuVX~*u()4=#UG|z
ze>_cpodEpi*Yyl6{$}a*kNfrjFX12g!jC?-1Cb`6ulk!N%riFif76fyplb<Gx*!5b
z@c%H)2B>NRgi8UGW`OAz$G^8oVEBvb#WUZAr)1@KBeQ=3Z2z3+JPZA|xb`nZ=wF1g
zF##lP0QN$E!G}LA)&97vf7$ZytYR*JcMxD^|F6M+?!f+H<)=nD6F}zVufabjlz+D}
zK#qdx7s&Jnb@)H*ni*jJ!U52e`Rl?z!@R#)_(}W(p#SrW;3sLKKUV%9=akPO?vpd_
z?~+FUEW>;zp!$1;32;^a$Ht;18tM-7y~tjhWi!oWdSNfqI);~`)69Y@_w4Z`v)8gM
zRbjx&$qeYqJBdRd`8?rD$@7Q2W*ef?%9`4&i|6O9*EX8sY|7+Y-EuP{Vgb_Gvs#Dj
z+zLViW~gEnN1)ygAFaHO84hBFlQ{A;@dMH@u5eu!*R@q&*Uh_5lILl-$tIcRwP`=v
z+Pit4T!?f~*k`fFpeyy2XCvF;S$(}9c+oZ>3*pIcnLRpsmL_(<Ax2H?3A%3Y;B;~#
z!hk?Ca+lpz*c%3=d6cet@AWD5CT4oRtD*XM2misMtZAs>sOWXB`O5>Vrn@7sFSjFA
zjPJ|nIW&y}4k4TOyyZyuCwXNmW0EM)F$`W<on#L+*qPB5)=C$$Zi;60WDZ6Jh<q4L
zz796>raG-zh(NZ-kx=!aKZ2nxvWR}q-nEqUBKanF__AoIm*i|!-$;YPYAD!C*U@F1
zV4}Kcs8UR@b(6C9?KIIG&0A&y5)3;Zlv%6V)^kxT9Lk!bEjrEJx#iY2DcXh%cLx=1
zTV$NcS2~zu`68Ajhu;M?Gvb%g+seX9B?dEP8#7C>f^5kkI~w<bH==VO7QVoES(&+C
zuf*hCB$PLIuzg=djGx{xkAS`PC5yLx6W6x?j>4v4YI0r|F3;W}u5wLC(f8|X$$_p)
zFvkGQ!(_hw8#`hi+DO;4z!INR?<rJq5}+LXXou4_p8fd@8YP)%Q=lynEV{I?#f9J|
zIiUa?XvLmam)X1uz;0r_s9M*x6mWu}=b$hg<qlg7A0)%B1VcrsLU~qAAbDp<?I3}g
z6Ryiqq)?=(c6tO8ue#si7wnamDK$fNI8~E&9U^;SgH|{(zOi>r-H}zg;95o1bp>G{
z0u}WUEeBfnZffH<^Y#)m4U15q@bSOy)Yv~e7_YMV(u?c)t^2g>quO_m{o#j=-AkUw
z8`<eRzs~z}M_g>`mYvNa)gr^GDX@LN6X58!FVh>td*e!rwOz2c8em@CS&NJ4)P6X!
zA}25X(9k~?g6UG>Ng(cE!1cYH4!E*7^{nU_VK#f6Z8b?fgM7m5d&v7`u9oQB5BfIy
zO&Fp_q1tEpbxqoEeEfa~4l(-F7z_%WkG_XNGd)PuBwWcKRe8FnQxv*0vXL`FI$g5_
z9SjIwx7cN^e4{Ggq$%&>Zj;(BqeF+njd%+`L>CCZ3+4*}%f9atlo>+N@SR;!Wlg!S
z#zpVGUxM|G5{N$vjQPY@>-kb>S_r8%Zz&c1Gq=nzzRSq5y~qw*@qzbNv3}Ojr8jnX
zRM=td;hdbhJ;0V%=6D|43k*|wK(;53U}$PssB~nkVZ9AtN)16N?+JX~-+<EMI|<we
zDVLkx5Si+k<D;s>5RbOo_Esfuya9;<$C!`KdozTE{6TT{eq`sJ8Z$ZJ+nACVTb!aq
zxx06)XA=3oNBZv?EMk_zZPXee;4hRKXIK%ErKjP75(H)+BE*N+Bb+D?kC5GNAtJLU
zG5c>ll$Z22v%QFpa+0rvu0M^U=*rY<siG_)`ra@bx*bEcM(Kd4&#VyREjxOAw#ev^
zd69lpD_;pAVIMXV*|9)W`ldYcZTg$`k``$!MGXAf#nboS-q{HYMa4Pf%pfwnd#}m3
z6}4-fr)EoJUQM<S%QG!oT4{%bwVm5P$F{@XZAso?_|?SBz$&OyD22MX%080IF%xf<
z308EzW}Z>iF%tgG0hVDm`?sdAFV}nw>?A0c9Wrx~7cfHGB1!s=D(n_fOF~w#p@XD$
z*(tEau;@=-1Q)~K&)3b1Dfx)LMC-iopszw9xZ+!JVWsJ}AqZO1@JB`*?v)TZ5jV6Y
zg5bs?wGC^IT0w@l2a;LU5Zn@&Ez!rucIN!xK|7$D<JQCw83aFk!5g5obl%?Y76^1~
z14`Zk8`NZD*toyKqyb?cTEz*|r@NnN54BL_kwRPtsuUPd*aITQg;l(aROJB@OkhSJ
zWT>{gR2||CgL#9Ch*_$kRwB9(+k5NPhwlTYM3_2!$%E_nhJt)zePyMJEm}E52Ah72
zVU<)y+C@LKQHFgA-XADJ!^P5cx%O?dtL{n*6KCfs70d97BwlMrG3X{_dc3XtB<e=R
zyU7nLn${i7kf9vDH7o@3P(iH&U(2w~BG7JpbtCqM?JGZgQWvSgxeZfz@r{_4VF@ZR
zY~*Wy`a|iuS^I8)Av-wy{;Y}@k)NyMm~ejRS7zcPrj0u_(3EJy6QH^A8wI43j06!-
zE4(EkeKBQoAkd9kNHZt;4v4Ja?^ZE;0!KzzZdx0O9|=$(7RV3hvGt5G0uV{glaS`v
zxFW-#OTiBa-q?IP++}))5d~yE+dQAa?T45t1AW1DBYBFEr&FVh)2!!+_8#w=pPHg{
zzY+s6v7?`i`(%btGiWjjQEL+a(-bc}>)5!Fkv&OSiMZSR%M?^G5-_P+q*RvUB{J8J
zvB@<p$-~$kc9(-et|4m@h0hp@FStv2_O<Ck;C)~>ORn|_2FD=t_*c&MmvuK)lT?c|
zfbFq7{jm-}tD?|XlD`EKEH~HM_@M4lPLiTIle~WAUc3$ub*4r&3RLyY3_>$}e^0!E
z)p+p8F8tiy4i*S5fxu2&Tn=L2uAoEzD&$kv{4u)<N%rvY{JV+ure^IXB#Aj0t#~&z
zqg+cY6_a?_B9_4(cu(nh$akwCbU5{H1LsZdx-MIKLPo}{B>p<4Y8iqo*r{cTs>X^9
z%pt%`-ObTonw#|_gOxGXVIFob2u^47N0&UV?-O#0*XfU4^BJLyB6RAKkmK{B_W|Fq
zQAC|v$Q+Rf8|J<!WNPgy3${L<CmA!BZ%bd_KbVBkrTc;bwPlOlZM9Vy9qYLD)v4MM
zRUaoI+1M)-SHD0tE%E#`K==^pICUAZIimugq@xChLz;LB&y5*LVWP%<h<Rn~A-XbE
zPoP9NTwq|c;1b;GFh2hR%Ll=A*OqL4KXMV@^d)jYxW7pRRWyI-Ky3hQh8}54W8^C2
zWuKf6oCwG|ZxohUuj}#Ni~c|)5;2kr^(s*n81Iv%D%ROS7x*mkD(&iKN(--jxqdV}
z+_w{9_Fgo(xcM^z>M)m|`O1UaXLDF7>~=<F)XzzAktB#>v!L+>gI~qk6YQG2aR~)M
zPC5RH`9h~@B*fStluA8+AqmVSG4wi6xEVO`(#47EGtDRk3P1TfPS0H^Y;img*~`wN
zV4)y!x8iS2e!_Ny49u5`#r9sg&Iln)+d4S=8<ToY^X)}EPI*3I=)h$x3u0MwX3<CX
z<U`BAgU(^8AWUE|GI+(J{Gd(Q^+Q<)y^wO4UJ$q`#wMI@izfuoEXTFdjo;aF*tV56
z<OB;FR++_-<sel`@Lk9IXVz8jKI*M&`KAT(h1Fr%cVT?urd2BM+#ijLyTdCc&aW#X
zG_OwM*#Uxgg*EYK$h5je^DB0qPLgj(g4bB6?UCKSf)1>;ZeA51vLQ#bC(=_S_UnE@
z&RpKr^V*e_nmbqjxSPLJM^{TrN1ONU{c0DKa-g0VP!A1AFUXf`agx{Q7RVriul3=^
z4x$N~n!WCK@{}*FmrPa-dEI3ClUIAa1WHhk<9(<KtM|0SK*i-3zg9b|!?QdBl@JRF
zo27cbKF}m5o`>@B5=t+NxS<*7T2~b*?En)ydLul_a@!|TS_9P%yQHD=lJxA$sdw2P
zj7sm)8o#0H8<D-;^-_K=6#MWp2{rUbt~s%@Q1^!YqM_4wn@NvXWYFH7*LPf(UEJWi
zM=4XDlGN<GAh2dBAh7vJvLp53+tBu`zW86QeO+MjMr-J2P}wJ5Zn78+FQ3e}^Q6B%
zBMfcE_2^IKrDJNka6tsOKnL&W11lj)W9%l_%&f(QwX=}WH8F%*D9LS;%U?b%#+i4k
z20p>|krl$n0DgEB5Z2qmgU{9X&_hIO?oCDQzzM_lW_du9{j?Nglb&A@O@f=jIsm-M
zcBjmrwk{6&D8oW}F#uVcj?b}=qXkTvabzerQJ!g20~d^X9DNZ<h&L3^9l~2@$Zsk8
zh!{eRD9nOTO*3k0kUDzIV8>Npk=}Mi3=XQoWh_i#W93mse*sAXDfWf=6pVJu^Ez{#
zL8{20-^P;j7841MX&7UQzx2hUQuYcKD`Gt-PbNFUn?XF#t;bkCrtDwz0)AmL@#g{S
zA5i6aFW@(4lK(>~;CI(O7C>eGsa*^ZTm13wv(mti|MLev`#)0k0o3y+R|pmWbNPuu
z$_j8>WqfKX0@@C&OusfM{1=z{&t9Fs?fSp7GiHE5?b8s!lM^^Ifa(jtfB={bFzEmk
zF_>8Xy}K`f-~VSK^^bG>FYOE<V*2EQ$;8M7I5U7D2S9X{>FG6~GY-%``sE<OAM*`=
z86$Wm>H7rKfA_TcCj{?jy1Rdg{f+I+4md_G04d-v6@q6LFMk8_pF{b7(?<Fm#|%JI
z8*t-(=<ou%Sz^yzHl99Yzjv+y9qyl<Ii5AR|Hrgu)@L3?e@szx0*Lhgutim(A?Y&D
zj?%iTdS)KPtJ0+g{ngE3l+$^yna-Z<B_!pGsGVFglg0rA%%S8YLVjf`+c(0MDA#&y
zp*afqH?2Y$X07!aM?D<kk6irjH%er%9qNOxDmdG;vZ>>Q=q*9oDzs8<Fb+w3$TUk|
zTV@?r8Bk}&>U^H{SfumjeZ1J}xdw=mfry+0Vv(CA<_;LNymWu0)wN7NoC7z7Nw%Kv
zUl4-rsGm4$)=tH5;abt5=Tk<UNY1AW(C_VqErORWhd#dzgxx*cqU7`Z&|Rf|fZRBL
z^hqbxyRc?7nL10*cGz}`*Mc@znTKjWo<~hi(`4B$cWswYCpfXjub|uOgK>#T=<8Es
z-L&S1gP!U7qwyqXb}jN2L9gW?Rp}2RY@8|uR9%Gl`}y>Fy|Uj;de|2gbakf3v3D&R
z{OV*8GP|qRrQYD98;@apt2L{v&6i4c-WFkeZCZgMRG~(v0&`IiQTLU+fO}DWP>Hu5
zaXsjQ6uxRTEzJH^aTU9b08?CNB8UOJX}THz>fZk8=8Mmtbz1KaUU5j4ykGG_@FA?<
zQM8U-vXIV%3)vyQzQ`l=(*aTIEu21n?>xRnc)~r`DLtGuRH~H~crSO#+%&Us=JAf9
zkQ--OWkOBchg8~~VNlwBH8NGiOA>4e(u*zbEA4x2PDoX4X#|@xUb<w9xrm1n2E<0+
z<VI_a9Z!cw8OQFe^=87#BAgf1GyCS~c}gh>UOLK|xX>REV!j~Ecp;G*m{umdrmH#j
zBnGiXUqwJ975P}Ft=&3Qi@#--9a?&FQO)4|?PAyllUi8Y2_f<dj1dM5<_$(g&62Sb
zzg~d<J|1J11&kTUV=5K6trJe|W9H=a(q$q6|3m$yyhNwD3y{jv8kV0Z2qHD=Jmk#<
zw{YAW+=Yq2)yS_EE)=XPw-{CeC2u#w9v0+$fm#Sp*p_59Fo~A%_iLz~-M+O)n`Xx_
zvS@Tuk2^2fTr*^|3n=Bdqtt=X>%@l~e5eFVn7yA5ny~>U@TW3uR#1p}rG<wo_`35$
zg2CtLlwAW)S*n)LI3T63U~ocs2oAqFOOK&+jpyctE~{_rGVJm!UbXl}!_IcZt8(uo
zH$Uv?U=;IYw=u%E9uUZZKo|I+yCX|~&Xyt+1z}J($VTls+-Tp|UG?da(D%oanC$nh
z=HTH8$CEIw-rh3DfLI>MB8(CTNmEy|1)-F%>rk4(tMAFK&V?&TVn!(VX2HC7O2!QR
z?iJVasMIW{yC0-a_~2%SakY`y0H{<40Z&eEi*$B;OOg^TIT<6#2bR?f-+6l|LbL3*
zI0bE@q`Y}C$)gfcedC~J*=W+Eh*24eA$UTR;j}G`Oz#Ch^hFC}0S8CeXBMAu*Mb^*
zH(%j%^2DAKPedp8NM1PtRVnJ3A%t@&f$B1NL871$dNl9Eq3zuH5C^&efqpcl)ubp%
zox3fb+owvq<&<#iHQJ{LZgK77uzlf%F^m3b^+fq1gQ^Tkm|Md?4lmpYWRymDsrNRV
z;By{Fo)fs+6oW)TK#tHE=>Q6GX!O-Aam^6FNfR%bHL=_L0z3s5(Mi>LvMD()(M!5Y
zP~D)ll#2jyWTIv6GV@>#(YLbuNcHZtNaG98lw3E9DMJY9d9UT9LJLxF&;7EaP-Yy}
ziCH?-Imj@Rr_is;=6P_#89PeX={7dnVc^aQk#BddcCOZ3-`dS<w=bez0onxKe7D=K
zEy4LsL*iQA>w@4e8X=Vsa&Rzb)lhJTwp^#_IKu2&Cis~veBM4mNz&vbb?80!D&>zO
z<_?h6WJV(ct`WXuVAg{PLwRJ`<#U=AgZUZ2p>wIp4_lS8@X%MuI+qN?Dym3kqSaZ;
zm<-1Rd5c=A_j?qNSwllk2bd?cS!vayY$8$?xT}<MpnaPu{4D(82d_azn<HTM66Q5U
zvym9L`~7Qk&6*x8AbFFzJdtjg7q9NO9W2r<$Cx(cQ6M6WA1ZR}peQZv^m>DVhpox;
zXn0-V8$c-F?QfQt+c4&lQ^sCFw3i(8+m#m8npe5|wMu&VX57^Ye78H-{IICt9D)vW
z5DUYp7NNTiOj3)P1^19->JpU<pXxrp;vyf+x<WXloXJ&>i;ate8VidSU@Cvuo(la9
zv8wSqux%_9;m(5btIlr%(Q%I0PHj+jcUBj?Ya-PQj57_RT$1g_ZlBn|1UPEW9efLM
z>B1WZax{VB1Sl=ipa@=r5NYB>#TT`+DQPCQKM;8uYVelwWD`mI+zk)J4u0BRs$JrR
zl}YMXpVY6^Cj5RJ^->{yz*zfgbblLR(FG}{sPr&+!gc)%R{ha{$S`PRoeC-|rYSc;
z{HzX`Jq<XMni`{JCmBanQ7&~CSvtJ2o%I15rWr<5AX&AjfTu)`(wkdBZHarZM38<j
z<nvb@ta#qu!k*#RotfUg2nTyUb*+=W0-<Y2U^5~G9B|2gK#TZ!yKn}Ar}7rtLz4I5
z6*@@HEdV2$iLKi>`I0<TEKtbDr1CU5^wnOTes@=KgV01s@G3^A_KC1}$Y7MjNSe7m
z9!SDKj02>!V|Y+8-0D-gpM(W*=FDYhJ`T=`H5He<V@vSA>6FqnHcz(R{A}WyffN35
z1N-}>dgx|>-cW+v%2v$_X7mvS?wipetWxuO>$C(fs2nzDrFS%*SiWW^MDNl>W2_V#
z-wNUwcj1E-<(7B@(bZm0(zl#rgGXL~YDqGS=+l0v6s`<|)EJFrGD;Cvb3w}}zgX1I
z>BUFzYT(v2opY+ov4l}#nJ0<r1-TVw?Q*QswYRtS3RCG1eC4Fa*vPlsm>5^Ge0jaC
z$rDUGM&q1sC1(>7-ierThe6o5KlOH|UGdm(ThyuiBStl}y<qo2;icP~Tn$f(dJxwU
z;o34bRC_$Ngx9d*+Rz%-FJ6A~n(`OS4K?0+lO(a+3C6TUP#LDiGO`b>J<_>UA>cwR
zVGC9-LWZHmN(&F&jyg}Xs@9a1+5_bV^>W_fW-~`7)UGsN5HXFeiWb|ewJFp3PJ@(H
z2d%(jPM07kl;(!Pe<HVerzbqgb-##tbf*Webo-6rX!UNbRmCEP+rpVBZ3kr&FxrwS
z5itE%n@#lWaGo29temu23E!;oz#zt*3eEFLa^PO9oK6AYf_4k$6G=0tGhH6(4g_*@
zlRo4F4J0H`3v!F>yS%VIM^JF8TpfG@9T}|lDb7LVH+Oyp%t!}}<CXZEE>_k8x1rO6
zpQ}N1WMmu(Vhz+XHxirSEfrYO*ZlAN2u&$KA0zq#v4@%Fa%5)iWHf}bQ)WdzrxFoW
z?Tfd;47~e*zY3;CPbDVbb=fycVPcRLU^GnUeU7AjrELJQx6ry7UU9M@b%!GoV6W>)
zKy<7l0W{yi0xZ-2MQ_FP`g6f$wg;aDCrb*r9Kv@V+grMJ!I_>^3j(w|OyTdr7$L6X
zWl<g)&Mqx{%^VXc-T_AK%KA$=G%dx=mfDKQL581`l}3XR;NF{mTIAcmkhWvgOYH1?
zyD{Bhmoc+G5;xVp!#P}5dPUXTws%r-<efi6)}+E@jT#pu=?5Q?<^0|e8}(ymNFnu&
zM}K_}>`i5uP<dPFX%LznGslGZxeJSA?n;ByM8sLD$&Kg!0;jQ3o{WRc_|P4VLnlZR
z{`m@Z3H?<)d-X^e=3Qx^{+f}#MLI1E-{{2Sx|1k;3++<HmzUs&Z@y<y(YdVbCi|$Y
zmUuIM3p#n37NiWb=j8ZGIM{e&ehG0rGux`_C5-0jV<RaMCKD2}0Ld%Qgjeso=GPxh
zDZg?36)>~@aQS?Wu>J&?e@o4u0cO_UY*79Wfcb}O#h+%`|BFr*Pm`vs0FoUWVDOXq
z38ny?EuJQz0Z@<m*E8e)g2#UbdjL)UpBk$G!1mjj{UcI1Kz{7s#s1dBW(9Z${Hmk-
z+bREXZ~g}C{lo=)@m%Hp8SwjeQ0-?M#-D*30N?>U6ahvlKWZ5OAoxQ@?e`Y|7!7#q
zsbBqs5t*2n0Iz;9B%Tl>KxYl`_U~`~B(L?4ddaK+8`OU_vj0C@5dfS2Ar!1p(*#KB
zqj_DHA(dk`lw*G<Q#4m?A4<&?O`)jphnILKP+=^dLdVreMjv*6iLXPZS+O_q{?aIx
z<co_d!y*@J?FL5ecPB4gmq@wLawsvri%_;S^<nZ5J$<)M@rj%%SBr%i%FB*Qs4DAE
z4QV6McTW2*`ide0S}!8Q(uYDmqAyq9I^AWk;JFZRg9((2(k&AWdms(O9Kyb-+?x03
znL=yzEN3(*Kg)Jzpp%2@jPK&C75g}ZprbnBFpXXkU=Io1KyvP}qokjwz4J{yX~vs&
zKVQ$80K;BjpDq>bI6K^*L@G|J4E@sg`i4wCNIl=QBb&;f^P$vK`Wlqbw4l2m!&bh9
z_JfLn5d0KRQXrynymFH~L+QwY8M2gc$No(Ksg$-SC@DesVExCu<E&anLycBEvgJA<
z@3!VG80*V3GFXQKQY}3tktpINe};U(h?TOgp-1>bB2flkoy4io0XH`~{H?t_!g4Y9
zY55%w7QDE-;W%<Cag_UHulfg>c)PcA0Vfu_o*HYS(dZ>Yp4}haO))TxDelP+39(ku
z1QnBWBnz8a;B|K!oK=!H$FwbrFR;3e-|uGmR@%81lhnXeWW5s58Bdqz3DcUcxk2lk
zVyV6L3%%S^I}LJIqvmVm<HC*<%bO<TsPc2gQjAh7Me09o);8}zGPVAQMuVM};l2dc
zuR%QotPXQ$!P(l%ZBxat8q!d#bfTsSl_{yd$;YVDGEPiL;ZBpqlzO4Vh~wO0w!)7M
ze`Z~Q(G>z!o(RjaAWQN>^BvO~iu{26Qo7JR>s1x_BymXBvVS_a9?D>H{-$?$>8udw
zyu@8sJB*)4pLwSaJS3%Q*P<p1Vpaleq%jm#rs4#<<dAD&Qf&1oC@Oi7nR56RqP(IV
zA-TI{f6SaidalT!>|h{Q%C^n+Fdi!%0#_lGmJ$|NViZqrvSop*cCSg?v~IW}y49m@
zh@W0xUNE#`bdN`_;upRs@zoAx+-e{*DMV0uA_crQXisj}#|mB@EQLm0+znXgBX?-n
zYHtkp5!+){IzOsJJCeEp^;+(V-gTldfjhecEDNP&WhunnZ!*w=8q|Jn;{(IW)DuT1
z`C4?W*WS}J%xgG3=tAr%IOQNyP!lKygmv1@h4VpHUqD_JHN26pWTcw77jT;-?w_Oz
z4WkOlYJT{tLK0dW6Yv_MrMX{;%HSQn=vQQ?b=kuQzpslFvB06r?z&g>YILhdJ@ZMP
z;Fl5R0-ED4?V|B7^^8%-+%q<S8Z_*D?tpScj9)l`i-e9>&(K8}u3zWf)vL^jx6+Q|
zR%0kgroD+M%oT*ts5?~XOS8!Z&1p~Rw&&S0CXRJ60o6Ak#Z{34nk4D~4*{XYo|5d$
z)A5-0+kqm(vqMtb>hx@Xi=bKCaf7G7M*uc;iL~6jUh6NbMbQM+f#^HrBZ8M+HnLm0
z?2hNgzzbDMnW9zcn<AimAe)%8^Hn)olG)wrJ;ZluTL%V0aRJteJc;&t#N)xxf-*R!
zB?P|;-qFJ>yXib<_3WJ(GIMee4QPXVyLw*IieY>ad|^G%4`zI}+4H&q?keQf$-~2^
z;<7uC0jX#-la5x(uu=LK3w!TWKnMo)k_GWUZfz@#8FPQ}6D?I8oJU)SPxkdmFRHb_
zjPpHdY>EokTfn7ijgGYQsl@R>CWo)ws5CnrwB*w{r47zLw;*{tF2q1^;@Uzi<)gGx
z&mU2Vp^uQ?X<@u*jZ9GpFW!sq#ztgTsa31)i{#qbXfXMv<dQIg&j@@nPiBhco1SnX
zjNu2}Oe2(}SExlc;p=1f_Sd!UKuVAVKGsf)LWf#lij^cv*sJGnV}ua(EFKYSBn0gw
z#(I->$8Jwu>Nfr5yT*E!j9lNkDK_14ZH&>vGyCPuD<S3yB5%deb)C3i7&j>HpMY$4
z6lJl0OmFY4GuizH?<v~0b6j?_8T>#Db$JF=7m3GvwRtX$<d1to0v7J2GJj{1!f5KP
zf4qQUUY|IX$N!>qp5k+Xjnt(gg3L5csr<#5c}hHn@<n7LPil6h_#DgLMx6qUwN50=
z=B8DR@CuFPCCfZQ3~lhmF`ouP{4l-F0_|64NfJ$-T}xM$N7w=~2HF?Nx$3v-Y`rh<
zXL<BO-u8r7i>K89Yl6zMx8267kMva@n$7qvsnDv7lzS)^SM%(SyanBwOt*@~PuWXz
z|FS-XsYc5<!tXQbjd*13SLb8S9WuuWG&0i$6)DU5iyVyU=P{~Z@)v)5CO*%>0FT@s
zghW|mBXfNr8&@LDr=|lN7aKrMg&pAe17Hn4sh23|+Z$Uu0$KyV@fZI!z`*)v;~&-^
z`}ju#49|37o>HP`p}*Cz*;oO5ML_=Z=NdMkLGjb`{cqBmXUWUo4L>~1SpA&5{3OBg
z4>OIQ`F4L4otXe;)BiBj=ux+_S!G9gNYmA)(-5FG2tuO{m3kYTJ7@Hw<s#P7-i6B9
zUnpi<O<Ucbp6i>(miHkXS1h@x8#P0{SA_&XJO7r4^mJe(K>qu_hf711H{F?tEyk&u
z48vv|r%7vcoJ1GP8D&z{0Hi!~k8>V#cr)y)>2IY+VYnQRmxH|C-y=NB=8G5h&Pr|l
zv%U=2C!{PC4G}s=GPronA=}b_3fZR*>#0b~9{Px=>cQ;q!Y{)aSMF%)!aykn)$0kq
z>S15u(ApJxzC0e5*(Eu=GK#2(vg|;Soh(A5hZ88Qc+ocd5yERpJN~-Fu4^#cn8{)F
zlw|`#cb{}zz>UTO11vjar493N;kK<IcuDv2LbC1)XT!J5;v#PKX+pR51>Y217>Bwf
z-MPMvWRqZ4C;2*ux0tCI`)pqAV2HP!%`7#zWD$5pPAa1s${Q)%w6Gksn=dev1JqN<
z9s@%>`Lrhq?}{h`Tba(kacfdsgD*5E5JW}wPvjVIeX@yazJ_9_3+j`+C!AlEu9}9M
zN5x8oAzDtDZhN2{cx&3qnwWX-R%NP=#ABi%<|$U3e`oQ9uV+$6y!n+hx|;}75Qwa%
zy|!kFY>Rpv-WuyG+tC@!`#0Wws<(ZLGw@Q?Z~Fr%ICy}$rC`eD&ui+^KGdX4k@Gbq
zWfv6CXlP<_ytfHMi-=iMkLWuH?VkVG;dx=O&@{7|Dk=p6da<&ok{66Aj%i3o(?e2{
z5~0jhS2w>bU%4wz)YfE{kp0OvZGN=VD$!CMUtcifeW2o~0w2F-(sYA*<S`Hzr5>M|
z2MyZkeg8*^)9k`_!R&ZBd?gwieUu^&t-JofLGi;#VTx-7_gb}*UTZ{QjKRb&#rEA*
zBqJOwEQV{i8D9(vLWNHvhQ#Av2;|Ur7{{U;PWu@;I<uJs4S|N7a91!Wk+KMGEf}A7
zwtT>jZ|t`u32t}=XZeBLwwEE}&5WuU!<+uE^z+H*yko(PpuP6_P}gXE%E@cJEid_t
z7)ziI@ISw)YONA;LC|a{PoWv{%eN^unt+oLT_Hz6vh7u#*b7+VpeBPWs9_0?Y(}e8
zw=%IlwvNep_waU9Sddm!#T1ENxvGhQ_zZK(T#vK>Ws11tJ2&qNMUl8`)Q+~Q<L&jg
z(nNv=0i-H{t=5@7Y%mkAs3z_-?#wWON-8sQ%bg)J0lz5_wiV2+_=I<U5vT}dv<p@C
zPRP1S#r?G^OZAkYODKc&TQ5())Mw0d($iI?fN!_EDJ*Bo8LA+CKR-aj<Tw{_GjGt!
ze7Q_e??y*!1~tNH-97hu3Z&MW0wCy~n4u&fczganEC7XWqQsP3n7x0jWM9=9+OIQ=
z!E?lQlw-vZn$ARtAGZg2eA%BW2<R)%iJT7;IB}o}XiDri+ch+^uV9JuF!eEWmg8w1
z-Fej~)+Z<2865CVnI?ju2-e!JCeQ(@>$S5dNcnvtDTN`|7U@BG@4ygy#zlpK!omWa
zML{s}OuPM;EPT|i*(LLg(}X89+k*Vn$B@bq9gAZVYa^LAa5D4+=aot@r?{f^MB@d<
zOeWZbp<vKkvyFsmEqLoCLwP_%=x&JvJ3{R`CA@(4!XyKa<_$ohf$BtekU0Dtf+ubn
zc}3nc;Jhb@>^x&yLuSO@w#xC_jW>;@VPS9S$Yx3xMXzUc>F<^;OpFt7Pf5^ht^?!;
zntRSe@UhPGdIWF=Wh9M;=g3N0RI_L<h-<^Ev3ko9FL!DuO4!EQqyg_<$0mL#uAFbm
zhs?$Z^K}~ZJ24VcXUmAU@Ij|yN9vZMhMhgyFLoR(A4rV79}GNBKC8teTjIK^el>Ps
zX2mF2N+dW-;-UApI2St^YCsQjMvfU7wGyqkZc`Ai<AYi)j5{oL6x^(F&LreWaU)`H
zv46czRme?gyb$?D3oR+E#rB|0Z@*b??G^=X`uzH}V`52P|GDhlrRl&v>SIEcHQYRy
z0T6*|Y51sI$5-?c$N3S43#XSrx!H4Rym$P@QCaFLh>Vs=w~+P_bk?M+d24YWOjq@N
zM=H#Q#(~kDp+{Lpd12FH<1j!O8n@i^5C&||ET$5^XsGa(?>)S#6n5w(X%l@V32Gm!
z&SFa<^h!>si5ra5wi?GkD9mx<m1dtTLO%7ryPP@t!L3C0q1&RDpcK-*HI%{v24%QV
zH^ae8_uHs(CT>U4{YGq?=Ejce4SEvOkUNgg_ia~+qkBLPEeLZqUp|WsoSpQ*))N?0
zY~kFy>9q25dtCt!1xrMVfU!{-z|eG}>XxTd43*z|O?u?*d;>v^s;u_NH!b~2(CY2t
z$kp2#FxwUp_!x)MPSnRw(AtDWIw?>5LVpYQBx95J)a~=<FB(3R95H^V^O$7@B%FvY
zVxFk?t|w*Qtx}fvh!7Trh59Dxp!+P)8P_!2KFKJ+8+cnwO_#xk@{wZrq(E>be!e6S
z3CJbco^44i$3wH16VVvP`>~Arq{A&ABhg2v;21}TBdU^H8;NQu*Od6a$-jeYo3)e%
za+Eh*z&>=ps)b>^y)O%H&K*Va*HLbp8sb;pl`m(dlNF5pZun-coO!rb%?%b9fx!0w
z|Kr1&dx$!_xCCV<14!_<Rb`&esP`SZQDYb4K+|181SQ|fHi_olUxhD_)<lHXvOKcF
zpxlgw>*@4<*B68bHo)^f>KOB2-_Gr2m{l*Hqq#`P-~yAln%=(ProH%Np0eU|t(a0!
zDB3UpjW<>YDI|rKgI`vu8#z^{Zb885eMp#BkeCh*l)~+oOP+2*zVp#H`vP)1FDB_%
zIpEJT2Y-_T{;0P6Jj22YDC_(mbHG0hcd!D6%74^t{<Cw{Gu`taIpEKsf0VWu88}&g
zl>`1sF7EldzXh{!vH|RR|0)*;a0dM;f&8150lr#*v68>)#j*adg#4qZ_BX*#hO^87
zO2=PQ*Pq8q{w8&OmaRU^WB)c=edf#c&r;VPCY66&#_y>sfSB?7&wn22Wl@k&WmK@;
zWERN!C{%y807s7o2Y*HfPX~`K^c5gYEpsL#3y)4Ga}F0yk1nKymdOT3|HFN&QA@ZU
zC?Xt5?iU~8AF2p{Z0gq^;@`|QpM8jbIDrDN&Hoz60Qg4#Di}W9(m&K0{<HZh>oXhD
zU*6Gwc0YbtZ2WQQe-tgb0OMf)5K4@wOUI?MBefouE%m?lE5=0Wrld6P8|e8yE6EUw
zxYvH5pdZA<c-vnlU#G$7^X*$nx8yoA)R;w0t94E5)X@_AQb!c#Jb#<6ZnHaOtbn~q
zc)0($Y;>*+3_4aEbC#bI!5zj!`3QpcSTTvQWxndCDgJSy5laJbgcU(FSQA$n(9)fq
znS}E+vAaeklHnuB*CFLv9doU3wpV3alz1E)WnG<o@}S<yeo&rpxJJ$szL#Q5koelB
zlf&$B)=7^Y|6ag0@x+l)g2k8({>tO(Xk%v|U2--wdrF231A?;yWuy{fI8Yp~R@-6T
zu`f3yELd1MNxPx#93-W*nKrX_O9BPX>E23v0Ucgbv;uwEeQtLSD4_hHQ~(SICl*s%
zMMXzXv&;zP`*>1m?q}@<XY2czI%bq)N%MiJ6&Hr#T)GSGbEp$LnYdS{cH~xSgt+ca
zcQZ<>t$Vf7uFxe|@~Z?v4zARRI18{kS(9ED@WsS_(~acu>9n|eJFCFo518>L;*r26
z#;jNtXN?z3b{S7Dbo|9&`7RiNAj12$(SpTaZzdKJ$IpVof2&sm4PmXWmg(|Osc9_@
z!<TgaeBoc!02HO3)9E6Y6n7@{Ines9g6rxWeNjlYc-in3+8JA4!Rs*}j>Mw`y?QD4
z2?l=mmx)Ia>Lfv~;e{uN)wx7(*p;Vh139)@7iIC;YXhQ{!a}Q~(1bNCpau#*pOENg
z2-Fu`0)#5WB&$g36A%Viz$J}ckkYVc27>4jr4rb`9x@uM?{Q!SP+}2t_O*D6KLjtx
z+CkpdpEWEM4qb4q`=z>n|9`A~by(Hgx;5RM(p}Q9XrvpYk?xl6?hquT8>Bm=ySqyo
zrMp8K`IdV2ceZZNx%=F6|MHQC>6&Z)mUF%19pfEi4W7bJlvj@sr#Z?2{c(!08gAVS
z)X-QBp$t3d1Eg6C@u4N0U&OHF2iq##Roi<M?s7?A9cyY{=l<shF!C>l18hDyqZB`1
zoD2bqOuvwm1{#FQdeCZsL9y)=xE-QrKI<JZKXmc-ccnv<jQ@b)0M!AV^)j31!$;We
zJ9=q)DyNZi@(>XIBeLyI>7wX3_Hbc@Zv0Rza!UEb^`pD>3rMhWa8!xmGc_2%n{b0^
zz3S$hBXnf@8yL4Vb7OxGswQ}1Axx8)=M7B_xz%PY`V~@nyXq+lG5Rz_6>KISh`>&Z
zqm?cS!s4DoW!0$G>L?m7(_lsPe6GkAxx_7a78Z!ZN!yCoramP;s<Uaa=+tbafFOW4
zc<VxVL80y2Ojz2cds-8O0f7Sn<11I`Y0kj;H6B4sk?H-Z8*Y_+ijYKT9K~|KPP1j$
z`o1Oc!5RBrhQ>SI5@8TedwE4|MsrCVV6n&{t%1fFuqE9QuM4eJ3&Ai#)#sle$-^SG
z-%E`_hb7$Saq%HnLM^}cR&g3(Ff@OIG)9Pp<Vjb4h$=Jfh}jydT9=Vh1vEMr0u7}@
zJBcqNLj!cvRbq=#)8B0pZRBDyGb186DO7F5ev0lHIbbWypM(#xJC+H=jB4!Wq~kQ4
z>P<|6&)B<<61d%p5@?P(?oDeq_PI6A%CGuKDy1_UB%X@kRX<@hvj|z(8*RDkIzu9Q
z=+@9PX~#ggFU28CLrME89{63g>T+Ky4LpK}bg0(WuE}xa-uC#}DXNBdOsf!`e!;T*
zbkFcaM;HY#5qQK`$VU^h7<Z|hxk$62$&-e~vS+49@YdMOfwh;&&Fa{WRFTD~rFVh;
z82;X6p~GlxxRSoGxh<<8N7i~I36@^L+;@oljzWtYs?Ba_FOqlKD7j$5ZPGer3LD4F
zTXqGN!V@W;v5k5=nkj0<l9yhH&-ZcEYflQ?$mU2Oz>hGpWn(vwDk8($MCHKEL|3<l
zqBNw5SD0!s;|Tl4Oy>k%v7w~@((4uBdLTz^`e|=Qzjm>n7p^pBdwXDdL^3IF9!CvY
z^5W#%GelK7r9#vDqyVecu5z<PE_8Wu`Y~%LKjX2eOkx9_+hQxh_fz5Tr5-rK_K6#M
z%?Vyanzb;h5~XugZsZrae6%0f=rZ%0#Y8x6{1R~8D=c2trr38?`DG|PRtx4_;u)yA
z*{eOBbi^xH&-^>Reu2z~O~XyNA^hT=HTc55v@ds-Y9dkCm3!-5`98EvVt5_$oO%lL
zs+{tYUsUK4svlbwWanVL!bkS0NY+AF0#vV^>UULF-y0M*b9{-k29+A&<Ahe}Qs-J0
z?;OKk`_vFTx*cQmkgQcs9!Gy?*|Nrl95y=A-facPIn*zqtHRJD5zgUcp8?(e^tv)j
z2L%rdNt*ohiP$_`36eiO@#Y-RS7g*TZ_3&eT4Ycq8vl%{oSA2!(!$eu<NnL!@a+Bm
z*`UJl?vugpho#1A@)=S52GQ(%opDb>#&dgzCdFqfljM)Ewqx3N6vw2S?QiRqNBR#b
zU+mB$Zp~HSE~o6V(d|A}o4w|hH^fXpIN5b9HlakBlYB$6ke`D`I+v<os&X7w+ON>u
zEFi)A#VR1QKV8Z@VcLt5$T?GK<5MDt^<W(M%ZID2Pm9YouMas8y~Cay+9gj@n+?CT
z1w;aS6ywbd1X|GHgDV9`no~#CldEn9^z)5hB3Ph<6`f&bH+r!6^RM6FK7fA0dC@n~
zO)Ef@j*oe<;C1WCvsa`k3$Pp%v<$A5$giz7LX)$gcy{vSh_Yuivh<}_SEG+U^EYMC
zPidgPFN1z0a{W{W{gF%h_sSr^cjtFt)%5@8(E-4ut#73};Cm$bw?BXIu>hcE_xsO(
z<kk{@vjO||_dko%f2{qZeEG5E|E*y=8xs=)&@&!r?ES-soQ(-61qaG|fkCGXTmZIz
zE42Pqn*QUx{NL=0g&hEtq64Mr>_A&EAk1L_8nrWV0;TC3z=3M!-=~xP;Uo`K5Bwrc
z|8ctiH#=ixVP*JcANO4}4<ydNxt_5A)ezrB^Z!E5|IeNM_00Z^t^T`b35-r-;9v)O
zqjP@i<+B3i{7ek&zyS*2^8_Zn{a#7&&z=26N%2!e*?+mSZ{QH<YQX%>n+~WAU}XaG
z9Y8qD478i$<ochUCfl!1^QREr|8i#l;O7ho0{`JB&jJj;V+PLBGXo9GS-5^boAJ*h
z`|HO3+!y$UZN6VFe@lD<%z6767X7sJ2gc?AM)m=f`@aHR;3NMZmwvO&X9j9)eq9Ur
zDHr;;km}zmReqv$e$uS~FF0U&!1v4Rm$8IDF+cwcEd5g|;~z8-E3j?xH?do-x~9!?
zA0{xtjrb15R+E0T@pKyla+5d|FH=Gb771<%My$EI*|KpnWm9Ul{e;88q(S;^OnQ^s
zB+{F1u4{)j2Mp*XVkGYtS0{(Fb?5|Wy2!T;dYz+`tBN98Rm8(#AqBh*>d+@$C(ip5
z2hNok=2$}m5BY0ON(A>+MdsH>LLcPmMeytLiti)GEODEK#mkXzB7|F?tBCU0cxM@M
zg!u;{A3N3G*j`NcUUOg*F+OVu2gll!#0Uf99W10i*Q;%Jy(U4Lb-h+X5eL(}zS`uA
z3KwNgR8#Hz(wiFQ`n)JS5C9xqmNrJ)5_2Xi48JiLRKv&HJ05aBxOF^VPJBF$Zb9yD
z^eOc2%=E>CJzIZ6_hQZppUw>}l#a?kRi)h`Qqw^{E%;G3RTPcgNo6*LeWsZ4WVva&
zRr9iJ|I3wM<O-uJR`HmZ1>vH-<}6&e@(j3F<*asN*_V_IJ&X*^oYl@05Z!e!to?W5
zp82Cy>?|mtF*v3x{W9KN^eCePVA}Q>F?1D9bDhVWRBz>Hf@8wFRg~~dhMNkoJ!Ukw
zv{L5i=ssNy`M5T#`Ln(xI*@BEpGz=r*~hzXcBoJ-p006fpDxoZF1FZZ8@s*E*bg}g
zzi9*7!;8rY1>+gEn#du)Vin>XLgR?B9%+JdxV-X8>q&QBI*CE9@la$Y#At=U<B8zk
z8fxO#>7XmA<82`B601JT9g1`ws(nS=na#ERbj)A<O4nn$cE>a9bbfEI>{TtnfKF3R
zu;zipajKUgeI-N0pz4tAyqkWwUI1?9>{!uhUW6u*Ehsu_WXFbj997ISN@iKp&-Cbl
zi!Nos{EP`Pcyn1RRl$H+Dvyo9Q?W%3T$c7eM9aXf92i5=Fu3SdXX%r>QAR5V&Amr0
z03{7z@NrTluPO4Fosxd7L8ELubUS2(23yQNw!U<3>^xRnkx6co%UK3j$lsnSVl=d`
zEKh&*9X=UZvdP8-k2Cv|1SJh8IXc5>WJ!ir06YEM@khD}mZ1&CS?#n<rB7?tcz3}`
z**nK-S8(+ssB|&ZlF;tP5spm4Z%w0llg%AURr~N10;YIjb=g9s6cHcj<@(MhOP=Ut
z6Z9+bKMm*93_P^np(~cXOr*qj9&6{Ropa#bvWrXWp?X>&yiVCC<Wt^k#c)D|Ty=9D
z#{<do|ANgWjXEe2dC3n;-(ZA9xkiisev1L*))S<-I7Px5U0@pxWJ`~W+Q;$DQ|%On
zLNsnW;B%k=od5x3y+nw6$-SXwyUGlzGFnDpbN-3@%oXLjSwA-eeKcnA${j!dqR*pd
z*$HFW4303OoP%1r@T;|&vs>D_0^0SJ$FrF^PpwbhqjUXB6**pCoO<4W#mD@b+RKt?
zn}0WV;4_wy+>D(GdjM`r4!L-V7Yo&%4)43A=N_pXhsD5yV9YTn1cLmLQ&sD%C2J@$
zesqJ>ZnYoZYPrJ6hfc=pY4(FEibQ7uq;fjH8H+TQMo3)l&dR6hu8pi@ChP&;774G@
zHg5!%JOJ^lRcy9Pt{Pf-ttrq0jc}fP3JN$e#ragVLxFOAdxlAWGxl7=Flv&4*3rxX
z<Tzt={jhE7(PHZuUI}<1nbY^g<X3w|>tt_9a<pH(f`c|U8USHK_4H2H;M`9fjG*3E
z$Qaz2L3=-E?Bp+)UtTql>lK#hG1MXdveVB~G4(En?hK48=ny&nI6Xt&ab$yRfZFTj
zwMM$qGZD`Cik&bgw92%XZ)>-LZR=|qcrNChr}62%`$54Uhusadl~6qe8P}`hkPG|C
z5<XPAJUFd>bz})+QBQ;8e+_grpEKrhFpm)pefF?!^S=2oXxe-STSl~`TVIwQAz_OO
zQw)hB++E}uI>`Y1#_cNEwRU#W0a}GLif5+SS-e7aS!%i0gD20;LDv^P(jenP(t?k?
zxoiD78uj@unf(DS?Iuglmnw|bJbL7J)H`rko}ZmjEeaJVAMh*<Y=kZ{Vz?E5VNcd?
zws!9iJhR{|{W4s2QHsd?VoClJp>}Cbgcs*EL$}2B6b%;+9vBMa%QAMBwQM`xrcGEf
z@M+V^7b#-u2oeFi@eCid*aho&(}mE9E5Smwn$r5W(WG$mWFT5V4%gqIy_F>CRwh{&
zibNj@S+!NkSmk+F3uVLiPP9ebdq|Pp#D|Vt93K23uA$1A)digzY=U7tvQ-Sy<ZEB(
zTZdB)kxAX{<D5MH^QTP<o_^G>B8jo4bk}2sU4ni4H^-)Q=_DN8Pua>1Ce#@@6l<?|
zIeCnmX{BZ<!IYFH-~jwcPL3BXA+D=iTfE5+aUpjFn``5rJ2UzCv=l$_<>Aa}KiL(7
z3Oai;g}GA7Uai|Fl=N&rRU${62!>J{e7(sI#Z`;@(D@=#u7o7$%U3-%nHC;daj3Mt
z07{AYg2Ipaf%BK!T8F_V%!+~Octj0Fh4@h&85xBVhIZ`utmtXNGjXkDOy{NY(z=DC
zqXnS!8OpVJ9(IoWq=0SVilR*Y1J|Twn479J={t~9XECEbNm0u_QuP~KYK8Zfh<<t<
zWBIs7&KZ7*HLotK6`bV-ON{_J%<YfmwXwA$5l>ZJ{_l7qj`;<Po0$XnsRHY27L;s5
zcLXACQPhZ#`E|8@d|oVR;59<c9i&7MMQ$Noaz!G&HZFjFT|&T)6(UK#5N#s!V%9Xk
z?}B8#=Nu|Z-I5*tokGs0Bg^Ujg_#W?q~-z;riNhaB!dvr=`ruC^#OcyUBN0t{iao9
z0Q1}hk@(shVI%X%43{*xl02&5)nlbo#oQ=_i*-tI)zosg;Co#5zE?M<!4Xdyo5WtZ
z%`W@0@7}b~a)gnG^P9qwGYvjrD*@`CW=lSg8xgc6>hLiqjN3V|_p!cu@NXQB$&!<r
zcI0)#ndg0}VX0(-^+ofJ!2PH|X@Th@*FocsxRh$DPCcJl$KB!xbP%lPf&QhQQYt{A
zM(ln0^DySoumbmtnvfpz<Exfq1)O!uVh<0u>KWn*H1oTY;Dr<31db(uBwHaaY{8K`
z99U#ioZxs4%99p^8;WIrL-|SZbNMD-Ht*zjICHgHvWHAxi3o-{Gm|QXBV~IlJ2xgd
ze4Cyi^&L(beg~BQid23TpZ|yw{uwC$P%`~ZzTh`Nne|(B`wIa6{<HrroclRX>KlyX
z0wxQwa{(idfxgr%z`!L|Ae`d>0%I1Y-$Rq{iBe+5_V&j5PAou2GyPuy3gFM8sqZ-8
z2de)Y6UV=rnghN^F8`tP`bVPF?{NU&R|mJ>tp$d}G5m%r`!!Pfw@bebUjql@f5i~L
zTAKfE?f2n#U`F1rYXQK*{TJ}^=cD@Zewl&hdK^IB=&y@^<&u7LSj<3#$IkIrGj+gE
zv3LIh`TWEP{=^mj8+!2X!XRMU8&I<U?Zf@|SgY>|Uw?e+|3C5p{u6%_`LwDpMJ_j>
zdcCjMEf7F&FjvSHc@7H}&f)Bg4P)S85(|A|khQmcNuBV~N_!jZu>XVxD=)zvpwka~
zRYarnws*ZqN#)SCRx_xi{e5c_x5uqmN<4xnmWjQSQ}qxF>Y!^LqW83!Q?0faOq~3>
zCqV&s!*ZfaZzIA!fls@~lKn>8u60p7l+E}C(|o<C>azNAV~tAX*VISmxMBwGVfeOM
zE6q(dp9iv&l=ZcTf|8SQ9o=*<?$`+it-%}?DWha{tKw>8{-6^2zBb(;Qn9(#RQ0mB
zIMQ&8#jXAG!%ynFa;|p=vX)aSlvMz!y$%+sAeKR?5|L+Bpy>^j+AANQhK33EDHYH#
zCLa2kU~es|E0!WtBSwecH}muDJzYT(n8wqP#Ost9lgIR=UckRPadq0o?$d4g(DHRL
z{_%V$`}nJE8~byFfV-8%6YL$j(4N|CuifVCDrpNnnOjp_*ITgyC6IFHSkXJq`+cly
zlswA$xh=B#I!F-ArM_7_e&^{!T5G5J1ku!FGn_XaN>D0k6D~HXk51+#6!dYZ8iV@V
z`6EjSqS!}v@SoXM9EDZ+>Q>3TdHLTtP8{A1&8F)v5uTQSH`7=X!Z)BLo6^^UQl6EF
zV3>cNvqI}<#;YB)!#QFM`e3l<sQ%p12r8a2K{j45n9!s}4K3y^fWS0vE=(~OYifWI
zd)h}##t~x!DP?Yn!tGRntiO6@g>EQTh~jn91^l74uMAp=e=*3?M;_DqXn2rHCNw+T
zoUA~FVAVdVYQ9o*-zUV-C7xuIqMS^M=Z?G~$Zgbs*Y^{42dmpG9a_)|MhXu1ladFm
zg)TO&68A+^oU!!byl|}xiFr$R-pi+v@1UW+EFIZIrqBuL8riXn&mFZ{G`H+woxOh<
z;Wj;&T0TGLDZUA2^zng#aR^_cALj{By>RLwRBOTLMBw+@=?Y;6>Gh+kWAlAu-L;IY
zA|<KJsH*&qLf_oPPIZZo4;;ZurmcxEf}4O$Ok=^xGn+&oa(wkBg*z{hWYo?kW@!G)
zIyd9SFhLG`9K`LT!9zqINkf8RR70B={V(kHWx_6Ds>Y|7eOX6JES-+Xmx7a7UugtF
zl+ZNnMAahWdF(!`<H*4_>|ia0<KEFd+#C^M{j6h8Qzq&>6o2tR5!t~wAf*iU$oi!b
zf)eYwZ2zUPgc9XauPyGgAQ4gZ_0WmWKCso^B!cgEGhS!a+&=A=PbTcs#$kK*Ml*Lk
zBi&acXTcFldlxa2Rg>N>E|A3MW@bP0d{@&%&gVdyBPDmWnCiB(q*p#gX66#sU#Xv>
zcK#$K-~ptGFnaXmvyBc6?>PKKog%t4dlQJn#L%>yd&Zo*{$yO7;b)}qOcFW?2Ue4G
zFfRq*&vxZ~yu{|sLJ~h09!*5e88~p#ebx!Ypk=K-*gz8wRZ|cYyo~F$g%=^qT@HQQ
zPr8BM0~ynTPj*n*CD9KjtQ162fElb)%%rINEDF0OL{2)WZ|~C^hUdMDRG{yXo?03_
zy&4BZCot0X<#xLtv@0plC?biHM9^d9`<a!F8Uhp>$By6H^)jIfg1DRDBnUh4F|}#9
zpyAu`aFNpc+Q}#7NkL*84{D33X1-`P?mrtE>!{|RKwz;*YHHn^o4u`dNR@9Uw`5PU
zd75qx5%B`%w_oJ%*iQGw!4rMx&!^#4@^C}CQd2wHVdWu5yTD0w*0XV>BuxR4d^-}a
z=xK^$$22Xta#Yen%kDXIa#Swv3|0sbP4&=6dJ*$XkUIl8AmY<igm#l@Ok^C(FeL?y
zxv=WQ2R1Yjjup*tcH75@<|^%Xqf(LN;uK7W3LD`YY^Qyr%|$Iem6|J!d8a$#gv-l|
z53AQ3%~DW3(aZ0Sa)%Qrd{A+O-Z94}5oZrs#8?U`%G{U6XT~RCK=+fRaSuphWA-=Y
zC@|5iSZf}SHRhDr7eKlfw;jS!njtilVz!P1?E2z?wvPq1h+*QyRE>o0WNY--zTvCO
z)cF{q>KoMlywNe8&sQd&%kiE!6&)6x|0Q|fo43^P3yUAk2>hiAoy6(SLH4(cQ6fyM
zh&g7NSUwqlFwVO`!4;;_!!Sc9VrgjDv)r&pyDGK1_lKfJc8Ik>5!CKJnRK?07mls=
zU`!e^B1Nn&D$1r%GS~OLD=LB-H(@2Qkrvt*jYHxcph-q<jOU3O3eJ93mW|sGV;L8y
zEcoRR4UlHT?iFi8cw0j6%r==X#OZ`X<78mGL3J^nH4@4sF|Vi0dIsQ_KJiusXrJ(!
zfj+URswxZm!4e5rET?g;Ow2fMKI2AeYhEiiXHL8`h)g;}vKvvzG0~XV+nj+X!h|N9
zyNax6-PlOf<u2^4fb@8S?(XSeXS0S;mL3En^@UAvN>V0==9n@(qzc_p(G4yxD<#Mw
z_AND`g;xRww1@>6FhPUQ_C#+@Ny3;04vmZcDl5ffP>qUtuJ69>q6;5KKXuef=L9&}
znc+qGjHEMUd)2U>HQI-2kK<F+jlxc8L{XbtDax{x+1N3pm9>@CNwAwv!=;@=WsIPA
z>Mk?hrrHpw8Q{x$7$jWghDV%-2FoSF1fr)=;E1duKfYCC8*+cz=FH5rqmGzFM+elD
z{Fw3A0!N+fQb4q0l>u8%|22P$4mY_JF+lG8R)jR7z;sgC>}W-cQL3v0i3@osLWyZE
z;oJh(_Lhy!^K~Z?yAAv~_G_~D0#|zVj4+x*JRpomKnu~vld6|HoIPG6%U+$`A-*W?
z_3+EZLRs>AFF9FtB8BX@3tpYvj=w^>{X{4@IfpbStmg<>jMx8cxg5`g_Y5ROc=Za|
z^3`F_L#4@HuQn;`xbhuW+yF+AaiN@TOz8)C3vcy;A;|n#=D_Ubq&}0-8^M$GB6~l{
z+{IoKboY|O4m%@6OoH*!U>A1GaZgmXei5wV^Bc8<CE@1HH>@w_#NhCa%IqAu;KSMz
z6P$EMYg-O8Z)^&=g%@nh_cI|QqEfP~7pHYO8M3nuQkPyxHNVrjE~8*1Cfq~|o6&V$
zNPV&j)KD`*eQ^Z3Hq<OlF6Vben|s+8@*7<9Q>^F@HsM!X^S$!=d7$G*zVqMJLH!2T
z0Dknw0sjb)2K*yH8W`X6SG@E6cmJsT{yV}0@Xe^21IStdgMNThCjX!B16Ijg9KSbK
z`h)NQQX+o=I)7#@{>zPh117*7v9NP|&yEEEnLcLVbm=$dkCW^7Oyoc3fd4|pd_T_r
zqay<vcyR*h3>M(%@3$bUZw6j$z~}#sn*`=b|CS^D=gxk$$@=kt|8I82{Ev8TW;UP%
z5=a;^1K5B&1hQE`_LS{k7<m12XTSQ61HH<B!D2ts(*I7N768cA{*2>(TKj{)Vh57U
zK*sb}7{~E%U-lpJQsBpy9rz4>T?_a%ff?|d*Zr1S4V-uW>)JoV^nZZ>e*%3!!NH$s
ztX~YZ{ygaP%arziJYoG)N8k_G#|3=BzX|(lHKZMuIMLefRZk^U)L}sUD;AX2?VMRu
zQY8(Jl#h(;Gey<b14&<a&Yyj~I}!sY7oC1vACm?HNFly^WFtR5;yr%6IGou&?6rt?
z^{<XA=vrEKRfLpzzSb)_WJKQj^4RN=@oK@UAa#a2#6XwXQ|zmX_La7Rs>q`kCJetO
z<g(4%(tVBf#v;TZwQ}pVL9bT&su=2%71Sk@c(|udhWZk@RNZj~4pX1^u$&avfr)|P
z&@^e1m4KHmM{9>0RL5H%u1fmVL2^C1-A?0B!lfZMrkc4ueEQ0AFAAc0LtEnVqQm8F
zuMsh;AXNP9+kNIf&gJnv3<|etykWTvE6x%8a(|m<uTRsyTL(#CNodW6dgTCiNcGjv
zpv{#)T<OTch8|No?K6kRr`>FBs@csD6<8E}NVc?TIl4nUQ93x%{+7`RGr?7tg`IdA
z&j{A7xp+!@FT+Gti?tKgF6QNr#_AjJ*P^v#3C*R4PWQ3Q$<s$+bElJo{c}Zn3K)K7
zF=v2G)`vk+UXKGcKHMg<C;M8rLoCu&8S<{MsW@1alR#Yv8Jyce+O#hP@8Q9)LZ4kp
zslk<Qmz;Pv-`$>ni4g+t^*5XNDwm63;i@=Ga*(nPPme+WqO)7j!%5SzH+{ybTwyR1
zQN=GiY)occ?f|_Mr!REU6#pxXS05@o+O5|mtVd`M;raf#u1$t~;W~o&O0oP_{hfTN
zgB1X7|KaU(!$B~@uL3H;nNZ2zL@L1pQbb-CSFkUJjBxkHXABiyXd0rL4!%=<#v6OX
z)5~&**~{$p<=rrHJN<JR+3_o_phs1XFB>YQ=w)@0Y6|NZUlSu=X}0#dhYmi`aw7>-
z8l8g2kDy=pon#vkR%1M8Xxti4wBfK}e^L*(r7rbl`>H!W;xvQ#NviKFxI)IO2Av5&
zca8w8zi0ZG)I@4(Uej9;(x~x@lA>o-30w|IH}o3XE`YuL-hD(JB=xhn%3KUOq&RF!
z)U@4HO9~23Wja0D4}!#P8R3{4o+!g{4<-t#C_GYW31F`s;A%H{Bv5VjK#{p)*4^~I
ztgR}=RbiE$niz+ywUtGzFUuKO9V_#FGugI>_4Fp`PwSUj`Q}~1613ptSz=-1^s|!U
z*oSMbMWI``K7i%2RTM0bbEl}*rti4<rQ+cuTv~ZG95q{#+Y#3ifLMhvJ^3R{?9L`M
zKWAiWJUkRXc2t^GKjv#HE2*=%vl6eEG`~G#uC6MhQNB#fKEiJy6Gp#UI-m&apTO)1
zSkd9~Z7~9sF&JFMSC3~rq9P#)60T`H((&?Yw@R)(;!1a#KnQ+JXCQhdxw_Pm$ZKqe
z7I1@7Y;H!j0-(>_EEUZW>vD&Rnqaj*M@Z5>JW#BEPZUBkXGVRah!6#y4$+$D0KOjx
zVGQb8ZxbP^7g&{=Cv~t~?p&+gOqq&1<8sB*n!(&S=aLM{4UaV%W-0!eOi9y4AUcXi
zK|C$ix5071S$^w&SthUZPU85=)jD^?S?IukyPF*vHnMC|dqtU=R52n%g0jdi@C}Ew
zm#XlX4r6-Lyoqt>;!DyF;w7<IK|L`jKS3{xE?Z_Lswk0k;oYUHfY6P6v+A>WLn0|d
z-=lzLeb2KFA2;yUqZhcutgi+lbo2J?o??4sV+^FJ9L&LWl0DJ1khEStH>apA>nU3+
ziKwjSKEZoY<^ad8Sx;a!i__l8SGu@3mGJ3v0<!S5XdUUY829ie%6GCNOvz>of&sbV
z7y(x8dgd^b;t{4ORrBdrR@&-;;M#YhE6c_N>GNy{O-}S}PRH{;+F14Vj$%oz#rMx`
z-tXH%R!861*S^Z?@z>tqGg)R+FK|Hy2s3t>mQRw|LEt!_eCGDQ6Y+d1?*%oVJ^WBl
z>tzAGdvQ~m7RR{|shCn99w=ReULS-I#p60>ow~kA4haBf(7%#rIp-geTQ#Uuo*$T=
zB5?=<v1A$kjMf_h=TJzrG%m8Q-B(~PT&cE#qYct2K{^R@`MNGo48~luTcgo`mW^p>
z#0t_rQq&7&V(~L&Zd=vDfp>*q(5ONN#@PEO|G0yxWyV~jupM9E2=Zz><4{bYu~jO2
z^YqJ;S1F};Bd5XSLY5|2OLu7xG$s3I7|QG6Oz>Ft#xj(9(LfQ_0{nZiXlTU2@@{|q
z5c!1KE!IK!_mo!aDKn*ki_4^SsrGxtY>@$mXwc%+ND<V7=XUkUS5cc$r~U%D;uGO;
zQ<7$2fFw>UV$y?}8iQ~U@F`zoCM==Efy|NZNYGg#GlUo?j4mb;&F<Ab41^MSl{X$J
z+G}k#PYv@8yUEZ#XfjLF#+EIwv_R%6RogT=5~4;?Q6UtDguX<~VWy>8LtVpZ#kSs_
zo=AIiOn?g-@&zS;l8*;In;{K>x<nF^nWrP~#Cv59I>@_HOf8z@9~5i^fnlMFNnxa#
zi|EYu@y=E|$sk;sU04k3blulpSEM`yLLwz=c+R?cz982MCiyYbNyTM4`=XaS^38hX
zlR}8x#6pn#*A+E1eY##$6C%rPuqrO-cQg)XlG|OXeW#Q<;G^ceh-Qm~A{OQ{Q(c)a
zKuRadytJ`-w3EYz@*GWpRs^gxS8!?GSlJS|DJq*TgNWEN+fW+Tm$`j~)pL0@DU&f=
z_zEU-swO2bGSvmrZ@Ge-V_{t*6i>s23HhV%`pDE?!Yc8u)+S&<J9T$Ctu)#!33nVf
zRSrhGp5_y(o|g`XD9)$X-Ip$MA=W-R+srhMAC?DsC{m<&dKER1vc~!QKcovE+xezG
z=f#Rzmsevyd}3P?d@Yj+41POR;1pS_uVXVa<GbS;En;ZIv?SRN@S3s2&KF;b@54q^
z^K990U17SYL<FrPcZ|ReA85Z!&nJ@hH6XF!JmS`a(tcC4`9=X1;!y7>zX%zCD<^l0
zCqcY`s~^9{u|ODl^>XQMew;zgz|omT%rtSi3-fHije=bQosP0mTI2)Ri+%8jwEOP!
zE+oU!I+%1<fM0wAANLnh&YZ)fb4lmG!Iu3ZWds#Dg%<_I1&U#|QSRAi66Emx&&5>6
z(=W_dX+?%&0G!$n0x4>UywWOipwcK%83<WOUQ41hJ*0sCqJ`&?+oC5QgW$*rT1zF&
z<qqH$-$se*yW^uwlp;E7H@ynzDv#NCUlJ3lU}vy&AR?cbwpuo%+uArR(GW(l){I~Y
zo-k|qmIBF9d`)hho2u@No7}DeyU@_QbD<JCb>Dj2dov*x4sh%DazT6a<XOKN?kpuo
zPNy$O`mkIkcr6wsw(N6vR|GFRuSHc*&KaL;(HyIUqh(4+2phrlS+cldggzH_egBi_
zxwsB9Tvya0Dn7+=I_5}NaDpK=DHgsh1@n$B*WI-vzY1<&>7$}~bh=2x>xB42q?8W{
zZEsix)$S%<@}n5Hpt@#K1Y8+1XFs|&u=~v%_$f6A3+n2hWD0@A?^ViWuIkdw%0{4f
z5px~4iLi7E(c9tLVCS>a;I1cs@|nC_KJx=`0>V8sRX?TB7jyPx!)>J=*F~)Y1XWMF
zKIOb^e3Mp6+6lqZ>1uOIfk1bRSeV0bXspDewNhcY+oziVB2jGF@=4ecLvrPaw~1cH
zS)TWBTNq&@Fby9{SP<D@e+!{f+gX0@vyy>4YP`Is&SV7w09HK&e2eJqr#7-hax9_L
z3R{sZG$il94L6$bE=fyea^wk^R$A-|DAAsA)(*M|1b#C5$4McHT@6>y;<^wCW{|n+
zeKdEH8c92&lSM^pC9v1ww%8SQ3TV7%Xw*8<EHBB!41yvQ!M^m?35O78B3Co9$WLjz
zgC2=BCtwdms;O=hqEattzlu|fHed6upw-ybgyX<egN90J6q{u?h8d*lk9jMcExF?|
zEsX}U<4S|Xkl6HII?^T36NBH*iEl}+iWsh611ONWxmY<Pa+w9o7F}bkh9$?d@S@qN
z`$}XR!8q}o&cyf7<e!n^ubqkS;Na)Z#E)>@|DrR&{-Xl{VE-XK27K2S0wu~n0-Jx9
zB7gtgpSSq;rN}=svCQm1lm@g+27J?A0y{3u?3@fhL<-bU0twmQcP73Qu|mcUj=;FB
zzu@fe+yCEg?b};t0wQ}wV?#$`O=cFLf)*&K1@4Fym>C6hq6Y2|D4GVgDA-v3rv?D$
zuT7Jm^qT)^)8ywf1KNjj0>{=^zuRL2@juYOn*$i01>7Vs#`*Uf`+jEfHjWO4W)}J^
zOw3Gw+1{TWz5dhf{c>)ctPIRRtu!+L=<EhmZv!6_@DT%TyxD-H`~TQ&`b$se*U)aj
zk5us={fU1c;tKfggZ;;+;Tt9Y)7l?AGk^ow&;gE|{-sgy{lC;7i~sXR!9R&!pqBQV
z-TDtn!#771&Tr2O_*whz%l03)27pOy|7^Pa(Hh_Yc0m4SYoH9rB6g|n?EVq%CEuQj
zBa@EjOP+MDdfB7!*ffXNtX46|yl62Kin$~G3v;?F+H?YnaO2L4o~VAkVDs9Cnx3_~
zXEVALI@T3Bp7c}kt8^R5YE~{KbcTwBDluIg2_;sIcjjg4J>@paX{9Hr`;$6hPOSyE
z&CQWeM3z1s3N4>?ObtFQil?eVxLP%hZL_U;rCygkx;_qHrIx5Jm0YWW?XJ=?YnP}e
z<dv+uewk+UROwpepdG{T<>MULkoxA)%~byCTJ@y-hJw-GSb=<}Wj7>mfZ+VR{tlEH
z^}wW1S_v&^08;*P{eV0p%)i1qS5Go#a*Sg@D1T1$GY)={lMfR_idx`cw3_3yIx;mS
zRXG5+_R4_KC&#+_cit|hkyrTueupfJrPR?c5!(xnt{10_N)VQ{pAp$cQf!ky5xL{?
zHN+y?`{%-s=7xt}f+i>I4b;f?xX-bR<4&d&rC8i}3M6SgjgVO<>j$oxb#^xM+$A6$
z?ZGrd#hba~5}k{`3kU18ao9gXtp!bN&c}`NY!K+!Gmk5;hh9Q85!BoyWrRI5;KP~`
z&<c5m;ulVZyR;G7a7edL<qsw&VIiH_a;5nOJ?jl*6T0X}R1o7sXJt%8{OeP%k+Y83
zZRGep-%!rj0}dVyk0xjPD;|Q3v+J24^ZVslX)Q0#XDBT4q}9}EYo@NW-tQWBFsTFu
ztnF`GU>w5l(RbYhDlnkq4`D-C`fcXwU$#~(H@c@MzN-shP2++o-?<{LjvK_FcEOlC
zU4<7TW9d`Dh`Q@jns-x7r)&%hZUdFB42p$PGwO;98JnqNyU#kWI<~L4Bp!?fkA}m_
z$JeOAp}4)%--QJ~>8&U_i!eJh-h9Bw#^=R9WT+~LoleI2?CYNG-<5VWzopY4y-Pga
z!f{`ar&{MUm)c^a)3RC$P~uMbytEr0DiOhVeJ2JpHn^k1x$NrL0L!h#<<Ky=yw=D1
zrg`%-DV8{m_RU7;$L&d+fN%kaHC6{pPS{Aqx%6JJ!#J1pYEfUmiBD{MK|&IJop0PD
zTz&4oUVWW;L=#M5<y*Jf^*k30m6#NI-BbV)Kn@Qb=y@hPrcNqGU$gouks%H??4#?(
zrrMd-hxZT}NpY;(GSW`vX`OC|15=TgA_{df3UtXyfwpFM<>O1D*#fX=1StIYrAjMF
z0k9DqnWPNM(GpM~?H6A^X)pO66D1ypkmhfYXJTRUt_OdFYz6<k4Th~{eDJx34dx+q
zwYL-F$@#d3T)<XKRvs-we*Z>mZ_tvIZF+b>T<qp5xAAlUm9v23@%HuP$8`Fa2iB<{
zUQ9){8D%n^DmZJ!^o%{!2@Lxn+#%P85~fT@oyPf+qzxbHbClkaN#1ivn`lSD)#P7*
z7BWck$L@ra#?xguK7M(;n!}0qJ*0p-l<<qre=ie?G5wqnoiq_9U6-oS0IaaL+HoK8
z>-!4t8}G^Iios-Zsqe_7#h}no?qYcd)R=pHs0i1SDE9r|aQL`=`E)gUMHD8*cOK<(
zi6yR-<EI1#!B=`EC$jJ|gm!z9Jz!-LX9tZ*L6Wo2RnZ~33Py2))kmN>?^b41MeHrK
zKt+R@u|`Mbs#!X4U=rF*+L-yCRxA9#%(VJ~r@OHe!yJb<6Q>)cbbID=gHhoq?1X$i
z>~)|p>VYE|y5x+z`nxx6R8%<=myrcoE~$I6hzrU{CJIyJMmiy>Z;0pbxv~z=vyt6k
z{0XE_olLf3s)8dN$z@DQru(#{hSUjj^OE{W_cq>L+Cvu0*JvE5e}ydJ<LS$~g%M(~
zd`Ac-cfr*|=8EkzMUrNIKtL%o#)yA`g4V;DUs_{uW(58ItqpFt9>AS^NjveLI5uC8
zcK<<?IK4U(bL^4E{*4z8XAz}f@iQb20Kjs5c{ox<;fA}IHKDh#oaCwm<zBe7qm&4r
zx$nBEK=AQ;lwxz7aGl8L-jM}@Qwt0zJ?wU96z9b>c0F;BRTkn}@NM)xzukP5HVAs<
z2r*hxo)vv-)|?UCO#D2J7oF2M?nB%#BYb)a1sSDSMsHldo3%H?a^g4vRaKb8;TIB<
z<d{U9%0wiUz?oRaoEoY08R{%b_zp?ayDbP<NBt3CMsb}0>5Vr1HqPUeTa>#eWd;o)
z>!9Jl`3E8SCsjImTd`7#wD>CJz44yvB$}dj;ta`rrw4OF0W22Gc4TB`a(7a$oi(L1
zgyv_Y>0A9bZBD`~>bAZMllY<I1@hWi2%C*I3y^cATq5Kriqtl1S(`f!^}}1rU>*Jr
ziTI=C=XuY#pVPf@YWkWG$!gZPn_K{qVR+r48*l^y_R>0aSbYIMk|@k|WK>E{zZ#?a
z197JTlaru?$$@*vIRWVB3Qh&LxJcXGu18;3urfWjpj$spMCcI>Mi8;~gNmvz8)~_o
zaYF2wGDRQ)?<9DFpAV6B>R&u9=(oEHB4H&#X7i+6zFBVGP2<ZUa$wyjo4hHxE~qfu
z)H3FfQmmp@&ODJrfG?n-JRS9RXf7gT?*@PP+_mGZ^yK=~o$o8sN$8U^-rlPk{&-=Q
z5jd9QLl_tp+}PE8g`#z7o)Y%9;c$vaEjNvjB-W%m0j;`??=n=6&FH*5=^<Xc_YfwY
z^HmmkpNoaLh5YmmCL=|!wBT9FT_5dxaM|bE+@5h1d4(#Vri$3}e08Y$oyO6?n~QQ{
zUlEMA{tA4_gWO<-EaQYIm7zRS(+nelScT45xQm(gDNU`ft@oo*5PF=+^GMIrG#A>>
zO*+g&$o48*xgmP;!(ORGUW54k3}4$@g-3Bv<!#z?Y44}l9F1^;`ocsVI&SHf`e`&+
zy-y<Q_0!Dq$5kckEw$&-u|C6`n+M@VyiiQLB3uR?CUnV&zu)M9C#@%j*#W_yhTnIf
z^cQ*a5Z42O5(LYEnFGsxvz&M7JAa53kh~scxxcdLr0=+WklgQt<hd3MBkbMe+9|q%
za;}5=&Tq!OsnTcHb<mto&Po66IcGHWMGmMEb(&@2*SF{iyO&x{E1&g!ShMELu({&H
z3)V?}ub@l6*czIB$Q&1#a^(&hu>H_4vBus`3s?ocVoJg9i2e#*s_=IEU0LNI5q3fs
zf+Y>~yJOr0-asS}-#L39RuId4<w&pb{^Yx~c%P{8NGn8q4_xETjB3hNhI-71v}5sk
zsl2NiaI8A|V8gi`r#1Iw4Womj&YC`|kMs8NlGLfT9y$rL>R5f2U`H~ak)I8m^&C$i
zs5Iu@=d2o~+)yG#21i<An-Lr^r?`kS^`;^%C#`H|fI}-w;<w^VhmkIu;~{w(Y+efv
zltxF>>A1AD9=|n&=0b2<NSS&c-b?E}p7L~A2XmFzktgiL1;IHrcGa!?$wDSLD!5k&
zFK$XysmEb9JFXrJlUoh0xpK|2FnAT%O;^0bQlYkYQC?lL!xDZr_-?FqZAzwpqcKt(
zL!waTy2PQ9EV$l3q-ioN9+SNB47-s;B5BJk?f4xrud9iY#B-+wH{LB%UO1Ica8IB0
zNFx{sN+#kNEm`=aiy&rIb9U}GB)$gv=}@Mo4Co<rc$+T;b(30Bl=$8kj1ydS@e5)@
z*_^1^anpazBISxz=XTq5t8=|QIYJDNr3NL+>JYyXku9!%AwT(m4I<aJ{~N>%^pN>y
zLHq-|{EC>r*UUd7W}rG0Sb_f+i21t$_7{!vPpa7e4n}>W#JPY9T^68GHYX5;vjAZg
zCvcdL1vq-j#>V<v0{Rcq9+)xpE5Q2;-}?R3{??S@H=FK%9`H99_0!saj41+#D1q$r
zUtko+pTz?IVl6umX>)M>buAE@{m(D_?VP?jVE+ZbeSg6J1AhC7+x=PH`x6KJC))S#
z$##EA8UF`JV`c*SdHzk17V&B+a)}+q>soa;8jLhpRXeJ{s%p>LmlBZ<f_e&MM(Q%f
zmqACmYR+c`CuVUGuYPw}F#`w5bPS)6A&ig=>#^xBPpkED;39-o<AcBW7A2aLVwP*%
z79?g-$Pne7>}Q*jPexaEq|@QlZ!M%|g?Z$hcU!+`(s30T3=VvL^$5628eOI1GinWp
zM^#!X{*qA9e*b|5sL6o5e)dGoUuHc#GyPULo^D;CoF*3XW3HLProyMPwkJIW@HRQr
z95fkm>;=)c4nu+dC+r#(ZRPU^vRw2vSo3=^%H}DCC-c;ojvpR36t#EP8wkWUtXxd+
zv-qF7p`L0GKi~IV#-^jZjjexp_0r=$oS^kFDQ`!hni@5&IQx{!r&=Ygu!UH^=nW?-
z4vxW1wT-3jODF=a<SP|^>y{VWxnbx%>9Mf<be;&0shpFfRNH!2RPU_<pYIpFpl`en
zaJyRJ1T*c<XL<rpgZF6tvh^YDkZ0O8=cs_><D#MIJe;a>$Vk0GhKWa^hA$ZNgnX1+
zP!3<eLVF)70;o9W)h2zSmBGdpj%y(%6~_^$t1rBB=O&<_XjVSGWS^0U%qVV>)Q~|$
zp$vc_r<8n0ZD*zY)<jXnR6&x6_{_<ru0EZhWQ*Ob%Hen|i!D8eb%Z;>kR(j}`KDjd
z*nP2+gFfV;&@O@4Th?*@8tCPTD}TJNA39HL9fIMPCuX?5f-0I*N;am8uLh$rbr0Ta
z!_7D1ztAY;G9tiAJlFymta#~>c0jMlCc3@uAKv8zL73FWenZ=BiL~$lralHxajP5I
zHNW06@oH}42AAYQmtGY%n@r!ZfL32fTmB@P69B<~jDSYzyUlBTV6ETCis-Ff=u<YK
ztUz&N|MgSTgL)puTn$Ksqyq}h*pqZVxk^W#Xl@#(c1D4#AVn0mgE8;G>EoNF@hz|0
zeicvqlrV#$6SIP{FNv(tTmn00-t25XGr8pPHtw8Bxx(ii##R$K@)QE>+wSLD*K2Ga
zzG%-c)bZK+8g;fN-*YefqwwA(weCH~8*$LBJxxgpz7vyAz>I&6DTtkyzmOgFHlImg
zY(9_OhwyIGD~h?r-&OUis1aMd`f;pR5<&fA59ajRw(pu;U+t$OEpJB%<?S{cFAxDx
zv<KI>4x*p6jDZ{_gwrjNGStG_YuL*!cq-*WL(?~JhPEg77_fL+J{I5Um0UYlv9%TK
z!q(3>U1NXhrYuArH)E)lM!NJ}JI7enTDCx_=Ei1}N6N!!Jj)nUB9Sj(Rx(}+Vu--W
zO8Ue<&95PcqSrf7XT3A|@?lHC`>`FG5l#RKpjaKwf~m&%v6r&J-UqzIg6r{jUu}QB
zJ^#g5+xO9lpT=)E{$WG+pZQe$$#<CZTQm5NtMmJ%{e7V4Z}r@OoydQ)_8+q>|KiJn
z^UvSpe|g;>YuSOO#mvBN*I)1ZZ}0k#P5+C2-B0)ZpDy)ZhSmRc_y4`?9@yaf=WG9Q
z-E#niY=85*Z&j0a_$Y<~l<QD)SiL0xAe)PAigd_EXEW(x$79$=QP9(7$78*)yjm&n
zaL?6V@L!$3`H<&!wQ-PK;lX~&=jM0audv$I{+{Rs-&!9!Xc#G~`wlH*Wb*1%Say;*
zELrmPXP-LRI_=MN2`p3I4L7b|sg4w7@J8)aSLubvl<}v73l@wO^W4OEWU*rIz2cok
zwJD4z^<6lm*4|2Dh6=vbe?=|@i=#`&g$(v|nJxXcxR5Wpqncsw4r?j;DO+O(9_MD1
zQt+1il{g`?8h%fJ9jOs_xWwTtw~hULsW87f%!ZQqZEEh~<mnL3(i8-BhcQ5vC<)Br
zNDod1GhTB~ChH3({o$;v=_1EV9VdVWNU3CEbuUYJxQ`;hO7$2^?C#wJY7PD>YL|MO
z@SYR0X8hH3ZZ?#9pw&iA$LcJuqGe3Xon*WM*})r<f~C=V@>RI&En_#%;%fsXTlxD{
zfo5&GEEQE8LH(nfv;q{Sc3Or{=b!_jtSM=YGDC!kl6$(Diak2d+ie6_$#7j>cVuE1
zCp1%0oAI{3lOt|}w%bOHjSke4#(EQ6;|bHyRyQc3GqMgNsNHzPWT?vFHpel;Q$>f7
z^+a=`qlq!TUnMTb2ca*Z^b$!RdvoI=hfy3~1cLCI9A=aIGHy+#Z&8{<{DdD$2P)PV
z1Oew{h~blKjGcL=io2<}&F*tqnOH5B8+8R<=dX_KF;^I}rsAU&1)6hU8#im0BigHJ
z)iBon4mXo7pxQ&kP_ZWjQ#+6Ez@Pg@0aP>K*pyh>&@=t?C^25e$*=S{Twk1GRO(k^
zxO6)pQIsRVlV_4s1YFo-$8n-?%}u>U6XQ(kn@`vA4Awi&xgvW8iK-N(od!Kp)fodm
zBaJRw=~^kIG3k+yzG#QWglIyX)}nDz2wL-TyT-)Go<lZ*eW76%G;e9E43DsofCu^`
zU$mkP52@nvLX(5E(qJ)OsnoW_@u-C!3*nu`aPCCIhnnOaOeev`yqB@dq&T^>5|C23
zPjqtHNPajl5MAM=0~-N!zF@OXx*DKx%W~+$%S@lk(6QlJLxUzr!h)U|oEdxET^`<8
zGQ5_mvL&i%8%>7}y(s+rQM;8O2~P})&dRY<K0hua+wU`43ybrqkfBV-ykLhJyB%5F
z;OkWADn3zGTh1)GRG&QuYPLeviroW6O%Lr?2Gy#^uips}+*vG;um^PCXLh$eG7GL@
zN~~<E`whmXIcZW2-i;nOBfa80Qpkm-msDT6gdL2ur!(rX80h-QzBhY3+i+AUX~&8M
zur+HCq*OqhdgjcHA)xvO3aK2#`J%$mIuEpz>d>Y0v5LGy>6AJ3g}7}2_yEF+F$In9
zG1%Imyc*aI11$~CC%wGxQ;AM;6zm~f=C4qnKx`tyWqKyak!0-wIbusQ;8toqQ271x
zOQPLDR1a$!)MFL#EWT^DLUGLAmD!9mED}0f*4&~^GL|-vYoVlky%esPrxuOic+ks!
zJfS?^-BYMTGAQ#}m{xqK$dgvM&ld_)XJ3gWjQ3vRy0pIFdoKyls_xJ;^Q*0GP?F%{
z<q_iSp_aB_X4!t3hjLFHej;Af6M*+JzKuKGfepe+!IfX5(jE3j`Cwsh*rk7bClcuP
z`jJc$W1xl7K2JGA@NEc~<eW~5dAt|8VW;m-TCUC32@ZOV-dO|kxzu7}PoWuXD`lx<
z-Rg-S3+}xZOB}~uc~6=-rmVSLizW-w?%-+$`~(3(W$T)s@4+%^AA8osSgc}4IKFfq
zc7^QNQ$`T>Udp@EatS6>Cct(D`PjT8lCIPa^L0s6PV|~JDL%N004Su>#|VTFP{%wl
zEs!#TFyX#8xTda;ppzodDkur61LN}&i#~;w<5HxKd&(Wc)yI~L{*}cmS}6U^qU`)#
z*D-395)4wQ<oc<IMbCyBpy0B~o$i8@-Mp>_#m`si%~q}xUYiz^D8IX<8^yae3UZvg
z!nTTRvW3d(H?mTVKsM&$i}SI&Ec{4~fUxjz2u!0nf?riVu~vEL%c4M~+J#-<OVd9{
z2}4UG6saPqfI6X!zI!(ZIlCZM;^A<)c|<?#=~?jAM^*J@#!31t|2_a4fws}Z;jRMO
z30JzV;tJB&`=nxu-fQkk!9{iD{bNn+r-=>;5RXW=RaUQuL%(kLBBN^hu$m73t3$N}
z!wK4E-hiIHDfkfo`jHHN8~BeeRssr;;gwn9KtD`yAx@0kM)=?m#nIpo;ax5=6=|iQ
zt{K5&I6Z;!a@4y2rc&Yjv!3_&mCE-o$)7<u&>rYN0pUP4@%yj;(^2bt#qbLV|EY@j
zFF`nf6Bs=V<XVA)!mPk1!Z(L1Cg7kiFf|G&sQ<k_{SOch)Y|_F!hv<gKg)z~_09KF
z`&%IV-!J_K2LJttFtE<~H?R6*DH{hX0~=6R@RzFU`xE+)s;ZxePoRMQ53ARoq9FcW
zS;h6|^7jv_lM6U%`!~y~S}je7)B!YL&eW<x`CBRnqJh-(`Ox~7v0mBq;+N;X6!@_L
zQLl(iTid^0GJzA)F{AlkkRu4Y$oc5%nt*Jqn)(J@i<{PqS1?LELkTCvk6yjv(~BeR
zG|TLQQIHH+Qa`mizar5|7~H=Jlw?Ddcp4hqJ7gX!7#vA!<uq}v8Y?h%m!S;f9h*{o
z@5cSr?!JHW_GofN971{B7a~g_Ni@?phZ-(m?_qZd8GmGTkqA@_LH4m+Yz+K+=9C@Q
zGQJmI*R17X3bF~$Ho6Q-Kb^c*ru_S<ZJ&M-73Yr}ZEY~dC|MGU=GIf!-SMv-Qxf~B
z{NE0a=eD%*dfgMiTgrBzGK)`(>cnYm9>7y9-BGL)&kVUccy@8UFO9Kp9fCi_*ogIf
z(ayt{^;(e&LzeFN3nO?hbHfQUvyaO2SIlZk#AP&`B;h1DqP8d`_{=$L<Gsv?f}QKk
z>aFg~gV^QKA(GZ7+W{{wR2kbvz>MJDYntrw4u&~Kr|!+k(h$WgTB)1JVh+2eH00bE
zz1g4^jtlC9eXuv-@&Y{`t18%tda09)_r8V-+k$rMCQ!y;)!v#7<B1LXIHG%0-Gbm5
z*zr3&@Zkn6Y%AN;==c1|@QYqy&AOQ+lg0UYSL>-ytp289q6s~>N7F3KzInaI83{Cj
z7SSVj%-W)b5!6_S*)umE*fP*DDk+)9tU1uZH_-G6C1_BTh7z9v!mA|VRdCH{4S9v|
z4`-@E;3CGB7<I?%M+4ztXlJAFL2`s-);;r)$;qn`1l(DCKC04ke(?*g#&x=;O-=~5
ziQ-V8#sKL>%^Mo<3V{~}A$V9Lmptd*$9uZpxb6{h6M{wakAQd@CG|KAYXNx@j|P7c
z_7Q0}521AeJdc9HFBcVUQs9teh}l(lc#7k=O9y4LD?=2^gA?TN{fL5BQ5T*!k)t(;
z2u(kF>KI%0ouWdcBXm+6t%y;?wri*7`yNsEM2Z)N$-*=ChRF(ynH@G#50wZ>;-pcW
zfl1ro0XY6ipj;n>NOpKY;zNR$A>;JnsmbQSise)iS7qYgt(~lJm#H$tQJr90G;~8$
zhoJaHc0p7F?1x2Z6AmQG(^sgX`u)FJco#$*;|-&k>;zYJ%!l!c2i{b3D#19yh-UfV
z!|{S{gwxTvfD_p-fa##Yw~(`)+A8Z7`@UH*U0r*JbqZGGWvjchEw|zAARA!Yt}*hm
ztR7Zq&P`i7GeACe1((3cBwN!qGV`lKEFMO-1}k!O;VlD7&=PFsg`K)kqH<ClUK6Sq
zWgY8WBk!nB%GJX~fkA5Ohm2<*=LX!Xiw!%XRu=Qsn%B)~kDvKx4x;!l@@ITbTd|Bq
zE)qNqmREWHQ9TAX;JL~)Cx<C+{VxCdLWPsU>!&^DA_7Hn6~kKn984!ug!14q65`V5
zpB(pwjbAhurw$j#wZ_k4U{NRoE=sgpth}G?k?~BhC*iXwVVtq6G7L8nr8Cy9(NdrF
zhMAmtM;iwtLUC}i$!#ZS)AHP1cV~Zid1{zeb`UwKQ`XYhIY>%lv5$jX>%Qj{UHaLv
ze8e~S^Z-#ilEeyS5#l5tf!ZeO-iyUnZ;6cRE9|AuaWUwuj|e0_7=`j|+1wfLf&mYV
z5FLWJqKtkKq~OQdha!#tN84LR)s<}9!??Rca0u@1?(V_eo!}7M9fG?BcXxLS9^BnY
zu;AZG`u4B8Z+E}j_r3A`0|UmXQ?+Z?-ea!0=9-HDO*Pmjn1piO`oxUknvn#%FXsYt
zioYS!JsE9QRr9M;gg3rm7RD|RXd^MSJ)&`d0Pg4@AvPQj%8j!L_qY`wBi>mfvv#F^
zfg895*;TA1*cyC+_Fi^nBL%n_v~~F|F|LXrWkM{Jl!Wxr&Ctn}?I=FWyi2q-)QP1R
zBrWCEeFPd5><Q=G#J7Pw{`;ZAs)1JRN@>KN$F#N)Y6i9YH;Hn{@cbTQm&jntLfRKf
zZqOj0l-C3KNd2lFH(&YJo;a*a@xUA!K7T1Igi4b|DYU|^msXvaO+8N}5{+A=as`#P
zZ8Pf66;>J+P5@?Dkm>QTQh_&;vcu*p)IW5uSqz|@%GMtrG6*lbp1g8R)_XlA8v}}A
z?S_vv@B<wp@yR&tuV{iow5IXLF5#u}#V?-YK+h?-?%R7r%*=tmREt^yLS{7%C&5J+
z8+|tUE}+ctwnkj-3uq7)IdxOQdRCTK2iNmV)Cy4YkMC3@A<whCazuLHVe&mTo`Jnp
zJZTOT8;@MNF`<*2|M9^)LRTPpH10YWjB<S0gWhhE*9qHKSvVP(3Aa_kC%MhhB8MG{
zvL4x}9R4~U^rVwC#-QNtZ5F+=-b@PzmZ+!j7qgocwgsHhUWTI!&%2`Ix={~18*T%H
z^n=^alZqa?ElYH;t07i(_4bwJa0s^=I@al(PD%Z1vU)!8>@5Snm{aF)Qf7eFVdY_}
zhM)BycX+Ef;YK%0(aTlY^007P*W$4_+<Zx|`np4$xpYuvF=PF49&yf8GPQi)!WI*C
zpU8$?6k=0LS%jWeg(H#})OarXg0}a-t%GUDrK*G?zTm4((k}lP<jxnnz^6-^=HT&R
zhjN9!#w;uvf>8aPx!~hHgl)Bu2sh`#yJuK=FRVU)vIia1_Gv`W8`OElybaakD&&xf
zhJb*|W!^H8y1LHFrtuTrg2%<xIoZYaM!VRkU>H#+^@|m~;1D`AH80ZT{M;!}4HmhJ
z6{TZ(3`jW_ZaqzpY=RN=iX^|(q#iT~Ed@2nB#@A-tdpU+x;+lC-8Ay>Tt6C4b)mES
zERYZ0?>3PvnlP3Tg|9wU`?ch2;_?v;6@bJGHVE+_4@pHTQ5Oe7?Ut)j*E*GGE$V<9
zp)seyZ7y$305K?BIxG()DcSXK*jM6-IL#x_VQnxXLoguPc&2wra(z}D4snhFoEogY
z{A0YBwMaj|$WRBY9jY}&W`uNWNE7W2xryqx`Nh)y-iww5)sx3fUT``YW=Ph($aimB
zfEBo{<36ZM%V0yG@6`7v^d#h~Nk6^4Z~459E~2rQe9poij16z0!`=QV$nSkdRD6=0
zPZBrac}Z}dznyqba1F{zF=~<F>9)MGDjSXLmnVTRW(m0UP(ltje9N}}-2!w=9B>Ya
z=fjpEvW)@6mFueLG3czvDJ+v}MTjHq-cVh<)6UGsK<%u;41!W(eOOEaVrO^j#2>sv
z(5X!go*Vj;Lgu((`hD*?Y;P`xFUk?YFgOCQ6pumu^yJLT;C0QG=J#xr5^l=eh<(8W
zd-wSSF!Ew@B^KYINxDK~56o~0*UQ|PK=CSkbl}<p>?1s<edS8nvQnEFd|3LVm1zmt
zwIqAY<4UWfkf@?aaKiv#FAsssv{BBdF;4twaEoYdDQ_@iLCIHo5Z_${P~z8-F6gnQ
zxXLcvK9{#VKaFR|y=bJ(#9`h#((qBn_tvE{f$gVJ5(h@xE}q~C8_&J`IJ-WQ*{C^J
z1K8=t7FqrCC;c}}#z}K>kb#9^MeT5@1v}RG(*`%-Ul`O!9gbnaV5-L}=ofrC&Zy{!
zAk^Z$BF3;x*_QemX484ErC+TxZtDu>^t2JutsvcX8ay$QKg7MJm!Eo{I(2wNj@^u2
zEmn5*q_ccNhFPkHsPBR!5Z5;luU-xyVTd*0L=e|5;HDZkh(99?D{Cs>o?m*ul|`GZ
zi+6(*i<dR5t0z2{ZEu4NUIOXa=16QH#PA6|6H33MAfX=bJ*m1tsE7Bm;Hj$!2ON-E
zNwW8*B`G?h92QK?gbTph<DE$*JD8zP4`)z*J;MQp^oc4*`tr48u@k8nT(wna%_!y`
zTLTb-9bar1!46Br1SBfq7ruf6kG6T9>?)dSG1<Ci^J~L*q|nF+=~dx;KAN-wyHD2X
z9&%;+@A;>ez7GM@zaI5KUzc%J`&x%ONJ~*^-GOwxK$h{EkpD#>2*~687H#}>Aovs3
z|1l5*_{07m1HoSyY8HT15#Wme<+}gUGXuKs{_^)P@=F##&yXK~|G`@MXXCwJW1Rn6
zzzEPw{}AC40{XrHY<U4)b^&T(fV%mIu`<VB%b9--7XhFDQ@Hr!TK>1|`lDtT&}aqV
zrUXzk|ER_Vh?oK1ylj9*A3u7qFmnF2OYncx4F4gn{BO7I-<Ai6uYYVCpf4goBMjIu
zKzz*zuv_L}1C)ca{bjEEKQHh1iHQGpc|XSfCIx3<1?1cT`D7**Kn@;|djMp_0p`j-
z=m5ZOvi~Q?;6LT!0ebiU{eAu8%9sI!1i(xHU`R&C4&W!40d?~~Iz2Km0``%U=`T0#
z*8}$#B7vU|$N%=g{c&Lcivz$N1gO6OR%;vp83PMop+A_EAJys%Z2$ere*Y-^U`~F%
zW`1=@1CTNR6EF_ICj5m00bqvx+iw49=r2hnRzUdyGvFQcC;H>(d+FC5`J1HDKW*^+
z2ixTTQ4-DuuxR+@{RNPV|F;719|!Zd3;q{t!py?>j~m_0>qtAUvY~XIyc>5=K__yH
zA2%p799rf`l*N@^s#qq4^bnC8|13<z*_f!;1&<O41RsVBZQZzUsK~6~n*VqLnFbVz
z=?dHa{c)-6q>i3mvXv0ago7mFWJNTINNSN-a;xZ5#7X)9y|f<%SC>AM#>@Aqgf^QB
z@3^s*HO{`V(#Ji^rWbYFi&g{vj<QLgwwLqMiJK(?GcffnaQD1%#iW7=KxKOJ#^p9d
z=cBD3azG4U0|`6qPtN4Z!@>D>7W>OK{tOC=q+ol!eXtT4T!k=B2C-C73`FEU1KBjG
zW<|lw7n~lDI5>03e&#P06^r`>Uk2D!-B;SzRaI-0O1V{FO`wUD2IB(W2SXiH67DrB
zT6eX}tkz%^=zFfhKcK^|M)})SD0aur774021%%oc%EuDc5(I@bL-&I>rNjx#GJvEZ
z)zNN$GeTqO2`vjN3%P<#ql-|zrzUGx;4rmbTE#r}1H=g<wf<(cPoP=111>j<UNvoJ
zV(Sz}_%L)pMkz(iVEiH^vfbRZ8l-R^LCmq<$P{kX0fTl7f2!-TbVVRLe9k>bdx9nE
z@|y1%1b6yKnEF=N*d$c)ON{yL{Ow5j8ymiAw#QzhCZM1j7ZSL)(C~FyK=4LQBP<Yk
zp*3$L1^F`#M-=BTPPPTE+-JR%cFP{mORO~^%es5Mu@N@zgqsHO+(Iivnh?ZQ=g1Ik
zk#9+6$oOGzBZ*YVe0}h4qC0-&>7=I{tP?@QmPx7(;%J~@R7NF19{nO>p47ZnwXma~
zMR!MpqzWyXzo{UUDLzx5{@te2(nVF{;6=)W=@EEmBCV(TEQ3--lsk-V;4sYm(fP|(
z2j0@RT)g^6JQPKTotlNGrQ3Ze22w_hgf~LwSOxNhTeBV@mT{kEADTpUlJ8v@0)%Dz
z86FADE|G4o)%+X4k|k3T0lP&lTevl{sbrYI@by)GizuxIPUM+1y2xHmVN^0>3Yr?$
zpP4(T>)t54YOIjPQvN!UNGQ7p4_j9U(H_0pP2Pk%*1#?d7CTllzm=$vSQNT|ZuaDk
zzw0aMk$XHIG$-4}7U}a*C(y1MPMkh`x49J(xH@lP=Oht-Qw#wY4*sHC(ks&Y>gHIH
zz=6aOmJcZg3`GeYK+M(FeUwoF*y%@m3mba(0?c-eBsn$M2Pg7WM+e8*;vLu4rNr;R
z1z<z9jle8JzvWlnsVNAts3P1roWa@yn>NPw<_aLd*azkvjoog2hu8<YIx5#zNq@f*
z{Ai_KsB-V3>Sm}7MhT~F!`Q6`TELo!@^(#v?ju9H5Ro9tz4bSvuz1Ash|9y)Lpz_I
zJoBV&ah@Tyo=3yXJGvfg7ISi@6hMC?cU+UwP)ICH?0z(CHTjUK!$jW9VEL75kO7&6
zS?4;>Ll*b0mD4$T77f0vq7O%50z}HSEYv{rM931|9%0oiQcx%s;T%bsN_?lcE9sbE
z*%Tnx(?fCHZ@lIt!bB4eckFpH51Ey(`Vh7bH!Ex_P6?cr-W~CDbR2<9;lOlce*4JQ
zbEj7|CcAXV<C6~~YTD_CUN)3SCcircZyd}d)a$PTILPsUZDsQl<(G5<Utc{~@vc$-
zB8LxTRIW0h^T9wZK4w|pW>)l_nySOXv-gfwcc6JLJA_I-)1JW~RE|L;V`}gu7RWX6
zHMM5L7Y!?$<YIvX%nVrN&lqd1^|YRC@|stt3AvUxbM+DK6b@eZZv}vp@Z6|+zroUs
zREH{W!_Xn+hb8K%WM17%yfTM_k@YMRe@BIjL}QV2Ke6Y{1!I=KB6%@7**t5s9@TCF
zFC|Cfs4$wRlSqOgBU?R65}F-goNU`V$|E4oma>oPZe{lkuIV+#;iK+;$k5lhOS618
zMA@kVpTRej+!)2Tf6}3Av+8o@{p?a$7dV6jjB^MNER7Xrl0pPH?81Oo12Y(K2kC>2
z-2#?X-1!(3uw%8#h=aOvvKjQ~42P_aA7y`iXUU_&YxPDXxj#juEO9mGBPwv@E>BcE
zly&8V+ioW3JaX}pj9{Mv6I&yqtLmq3PnF?3_Kr}~R6HlsqGxC0vY1{-29h|9laWPm
zjL2r$*z%~Pl<XAGg_jA`idc6P&K#bKrDBB%qJkxT^C2_t%6MWWeP}H0R$96w9A|eB
z;{%7rJ&<2TMbz(7#?ao<Ac&wY+g@2WG>!0gm5s`FzSwrTUlOu{c!H{=cM%jrKZ#<H
zXK-oD7gc5zeMfGpQN4_La%@?$<MR2ggRS@O{VfI@cw}d>GHphw3g3-^jvE(C{9C^M
z)=i1c>MQ{$`20t}5gC0~wSuzTb*9<0RcsZu)?B)A-spposM}S3J3rLEbmx_tKTV~-
zoOFwgaKhpU0s4_6)ATcEAiPEIJo{sQ4;6$fYK8Ufi#a$6cCtfW;>1m0-aOAlj9X+y
z0Lj*|sU0L0ddoKwMa$YxA%sOBlE{rU9D%`#4}d#aFe`Ib!i`_>Wv_9#;$!LRI@$v~
z5Kn6DiFr4Sg0BfAT`wDw>w)IBHQ~MpVOnZOn~mA7SEsvznf;)V8XU09c<tNEAsVa^
zW_)tfC2e^M(}~%q(8ootW~L7DCVH1@tfT7O$HZzj%5(fl<$ce59-TwrVIy_$;2T$U
zGUOqL2X0&=(gIuF)n5Pe=2;N~yNtp83s)Sh)KQw~*+p&nAq!JW?|utwJZ-A6A=i{6
z5Jdk>L!5mEN<UTK$ZZCiL<ZLL-PEymkNFO8Y}Yg_;%m8`;Z0{8){oyfQU|v@gyaBc
zyiacDu-jc+E!hVHV0pb5v^=IuD3k1rbV3X*!=O|`C+BYfEtql0;P~#0ucKeX@RI$r
zA?&6r1{S4c#$a;aO_d`C$}!Cq3;#AvBLdw@)L@y}^Y&*>AdI!}A$KFr0Xwn?USId6
zbA0G;Zv^9V;=V-!$(Y(^`OP#gEr@rJ2cr#oS~FJir0>jk(-<kT4ROnCyj<R#oHV^&
zpT=Z!ZXr)`wP!+YSZ+jpmA~X!IH1fzfGTmsQeA5{3#8*A%;Jd>2C7BF5Hcv*E1JFR
z3F*sth(Fi;-rX%i=CEnR{QNTH)8T&6^SON~T7SrN^*s4r>Wq6w+N)da#vw~isk%T%
zH=XF#7hh_^OVOjzJ}{+29AOWN+`!YKaQ2cYq%hRs8^~_AK+g`)(Ej~Zo2%~Tc={=n
zz4f5Wqdd9P8;Z%MCl^nTu0z+@FNkM_<!sCK5}E`n<OGHu0r<f)-78>NaGP<qsQSnL
zV2<hC!d@+O1ko(4hW6}>4NH&Ga#*Qx9O=7wmE`q&=|%U*;<e896|Jb}f*E{v3&#}3
zZC9G?ixD=;M-|=AbVDyM&@l%x7|%dAECCc^eT@TvS9=9RX7CV9Yy^_L^OleDzAN{v
z&V!TQ?NDKQX?q(rl@6<518IFcUs*X%F17J~5-<b&%BYBPqzHx?HL5J9R6!+w)8oYB
zwr$tv$F?_>IH9IHiZtPbUTBV_oDC}qrj~|alZ|8<u6g=7h;nS_O=1g=^HMq>U#A^Y
zJX#||h67l7jn#Vyg!+m-nc%Zq=l9UZae)=SH(k&ly))W`?wmqmpq6eSEsqxi_hXsd
z5rI%oH58In+!2G2R|q8vfQD&ATAvY%`t)KXMNemgTUn&MJU;gn`VR1vXzp3X0}MMF
z<{yZQ+Wkrgx%T>#({VqZn{`ANdxeY_MtFyMb!QVAU=9aPF{q`;^qGMpct|??9kmlY
zHnu4XuRYFYr@7;|eAuGW&r9k|?eg;C(Nzqek$7j(gH@fYI=q^+Vh9?`qt@Ra+tW`i
z9U5O|uuo|S)|^DFtJa$k#TBwyrB5d97Rr%9Oxaf%0G(@)z?&K@zjQ1IIru<R!qtO!
zU9l`G3acjol@ec|N!RDn9Un=8TjWio5soD%`}_^#5?s)L2dRH^Z3sA|rO8&21U%ZY
zd~VBw%Xy4v0I|(K-=AHFOPl|8q44^y&}L4!MfGu2oL=#$?yPuB2ICRw47wQ<`ouqE
ziUH!s^#QAQY^$&*XZyESDy}Zht?`^fOB2=ACD2GNs<{%2dJdT+G?Oyh!&_|6+}Tyl
z-)eZd=BtxZNZTI>LFYk(-%SSE+A7LlhZE&N>orEcT!NDngw86uG-oM%PXipjiyCK!
zDMgnrKpvhh+iO`9(YBLWe15=?TNKiD4CJU-(AWpx-yq|r#1%RAzk2iTn{<B-!p9Ki
z+c9YL^+rSm+2T0hz$+x*)xSdH-_~zLh~`XjmWpUAs@L6AjW58zhGIM8tDb%>3+R4g
z_Q@>CP}hOjB$o6oVQP@BZlD$g+kHFA^o-zEiVURsE}W?ZXCcRy6t%HgpW@68?%AN|
zn0|2n*54%`xafI8^m6H35YzC|qGT;W!9sCdy=_U5#I(&ScoC%mnBxR;kKpQk#b+pI
zC;l}|fE2s)yj7zDO0#sHs|w!8dRArO@=Y2bIB^khxUvlGXtmY|_cn)&UMGH@Jb2w1
z6HTj!^^LeKXtx!f?Z4LoTJ!;~vrx<YGh4<A=(_b=to92@@o%*Yzl3vtV9Qt;e#F}U
zymsNwvWcHD@eh{lhi>uT*swpu%l|EJ@^7>bJD_#}z;>_$_^%)SgPZ`C0`T?hEI(L^
z|1InEd#UCBZFww=?10jK07n5RtzZBI<Sfhp`*#4F^22(ZiHY+s*^~c7I{|ub{qBwe
z=!Rcz@judd{BmFa1KaRBZ7RLkAN8!iS6=+VkNgix*Z)76hu?F*zv?v@0oDiqP_I#;
zt`P%Z9=x;lh<4xd3Z!9(X1bsdp4^<;>JQ2BEPC1dgNqx=TUA&&jRWrv+2MU^ohMld
zAf?}y#VG51D5cnFyZvxMVCK`t<LN@AEdM}!#eAf5&Vvp{tmK#cDHUqFyN#wDF+JIH
zEvk@zqm*Mw1Z2$ZO4y++eHCrh)1mTdVs6*SosQimzwCW~R>;)`oez(vv%8BU*_Omc
zye<y3o{7BxMJ>(Ew};a{yUusri!oXBXijU><IAuGXx>Od9pBcopFivqL}d048Mevx
zNqkxiP<pj;?S!APXFi92M}}m>u`vp-`&MGU(7a|f#cwFxWp{K+Fx6T{_Z+`joV{_z
z#b}AGk9}&iR~DEXXVn1`ombW6xuTM+se}*UYfIjv52K(5rGvS+F+&)$=eeexJaI#F
zt&hpe2j&D5zBsA?CODqFM<Pg5fVQDtVqa%f7XG<8Kgz)xOP2lekOt@WZr_y3Q<;Z6
zf>YH~2|DH229)MZLy6|#!=hMcm;2Hi-n?NAxZ_^zNxGS!I77Wp$Z7E|?qg}YVbWH?
z_UeRd{kO+HWCUx2;O{Kibb2uIkhLsawQH9gcEbxBn;Lb}A>`Ry$gLE)7n0N*M)qyh
zSLYL*8f1(ak9`honDxMN@YSh7_%(Fx-+)gSdRI?i*4=bZ6AvgKD1N1lxQefwxNHB$
zO?TSeHT`*`OC8F&w38N#7SwK$O2O^pSLB}3>2)HoYklCa0x20DV*TY<{wib2GypN`
zfvXFK#=*UE{cb;(+Dx6<2i{nN(kRt0%?tTw`yhyYGlIY;dTUO44il@%eH=8`mSHD_
zr~%j&wmvr`ul-o7fx!2M@F4;q#1d%KObz8T>coTxl#w^<ahF&%G5c?)2)KoD@V=98
zG@00*RJ<j(PXj*uR>~ApV&p1=*jl>k5{-x}NW%#oqvgPdK1l4>#xwG0yuZJPo(HPh
zrb?Nnq_JLeYvprOz|<icW*WPDHm8a~?xsk?eO<V24GT&w^o{TI^9sbuf^wB)&H?!_
zL|n8;e2oHQEpk%QzJdGuT61^D%2rrle7cpcbUc(JVC3vT0SsUekR~iAMP2S>3OCeF
zre&MPv6?Z8M%>R9+$jdKH!WJW1q86Eqi;`M;yhmrOHpcW0wbjKgf64Z+AdP{SAeIB
z^RB1i^ZOqt5z<UkC_W0<Ra}gDl)C4TLr)PPjUlc|rsUiNbBD_0;4ae|O-R;tYqI8k
zQ!jDLw@49aLZnOavz6x<U3JCXOgHcwjP--*nGzF!ni}wEn%Z^3Dm{r9ByM=M(CTo%
z`vz?)^+{seX-!Iy6!8fe{0q;bw5yf&LxAb0NU;6!!CtZTFKa9xNshzcA*cnQ(dM4r
z1}5&^WGTm0pj%qd<VTnsR&#9})uuh5x}-2;=;$z?B<^(X*VNXO8$Ku|e|c#MriSHP
zueoNV);=dshM)MP0!kUh%;LoBh<2%}dRab7bxjhZvM1g<=<(rEs6BlTMl|Ck_0uNv
z_wUzvmGtRf8(LWBiDTUE9=fVFUd)`&^w4Qz&MuWtSZqgx@XT6cW+K~W;`WO9HLFM+
z6;npeAWU@9^8$g!J|mK?f4u_RfqTz#AuFQ*n}nXtnd?h|l~Zni#ehCzLKLU4iG_la
z_wG(-K8fT4s4C;`kRg-r-6j)grv+&oR=XQin8M3!w_oelQ9`!ig>>`GBB=I|ANRVx
zA;az7v!*?SgTF+e!9LvA+mO4ebsu65GmY&hpDJ2TJq>!BW{sRBt&>*pyNM+*WE=SL
zmOZX}@`Pe#@fG395zaigW_NoPK7nSViA`@WD{@dKO!2vR69?+kkC^q$uA7rC@u?<2
zhtyJXlnm117`>gqb(C$URd#3N)ZPWt1-a01g>-LHB;?DdAQ5ud(W1Y;&3PY+ZNHD8
zOCuvn0gKrN9r8|B9b7z40UR>kDBrY9NT>F`M%mE;SJ&g*f5i8t1i-T{CQs^fuA|t-
z6-UO3Jb8gar3p||@SfE-ZLglKjM9iSwg$t?%Bxg<0;98v<W?O4D*2+VsP(GL=FW<4
ztwmYEn6ph6a+CR4+ynK7P(m_Jq%CmY)q1D!Z7o%2HIz$r_*^iA;`ewUwMm9R!CC8t
zT?p@C#iRxnHuNnb>HCcQ?;ut#+Y1y~h#;;ys{3PsV67wIIqhswN0NF|K;!ygUzPnK
zoPb)TF5kFWC)}r5Ult?Rq!BdRMX!2Mru(~Z;r3Ut<f0*L8mE_@E-7%jv5{}ZntpO<
z3?y4@X}Z{=Z1TZk^7WBKgm#;_z07E@<HzmxCTP)m2U;P;);GtfQSC6;<Kh~L>22Ph
zP-wuPOU&+q(KTYxbw43v<7RXfIgQWU5~Zycmmg9Eqz|krl?z=;*eXl>fH6}yxLE9v
z!?yXf1)K*uI%FiL#+VJNI4Zgm5g?~v_6>=*+Tnwj_0CNnVM@~zgoW5vOi-t#vtQ_t
zMN>P>D(+1vEW=n2@(1YhEwV5|vrt@oAd?*&OTu=smq@y~hh<$Q3C=$6jc9?)=VO;R
zdLd3bcIpJhq|M0|dkZvgQ<CNx>-{eb+$~iw$`4h+Z=V(VD0#^PrUu4A5``B7AwpzR
zQs{fhBFDkdmP6|v>SaY-+-iFb=65!t4bX%OQ3X+up!B?MyYf+T!w<TmubUj)^|1s$
zhD`b@gsUv&bd>JOi1grAXvp)TBVOfmp&7j2o_vS;d=3M2o<six96QoyMAj6LvU-VB
zNW8eSP2cog9E-_bZ~fawr7vYL&z*Vjvns}ixGl6f<&vy^dqpf-Y+Jr8_4(|Mq#K|k
ztWQWqq4}iG*V*0QVj9K;MwoozU|gWiMhSJwfk~eT2yN~y{|b=*UZVRqfcyu_`8PoR
z6G#8!V&Z=R^4}`10FK=Mjv9X^#Q#af6)PaC_FD|`6LJ22?7u3mm;hGaf6>N=^=C7h
zUm@q;3}#~m;9=H3*XRC0p8GdrnE^>TPKG~c)>r{q=-*cTH<`6Rz}r6n-{0lj{*jcJ
z6;O-y+jPG|Q)WPu=YI%IGc_a~cN@_GNwLd>JndN|V0&k!5~U1mPwl;L#EAzCE(MFE
zniGN2h?tU@df83BTIo2{juwXQK<h7jIlKowUo-5gUL1U!ujB>+{xI8bXIl6<B#9H|
z<k0%qRX%&2e{cfbia5zcAh|mS)0<?~aNxka0YxnQ4!!2Oz`#7Gp~u1+gIno3V^i7S
zq*eIgF(pZOc_lG-g#cdu!|a^SMxiL+gIyawJ~y{7$FqrG_`KquY3_+74av(Y`CiUB
zImf*&+=Tk3P!-jp=?g9s5YJ{>05-#;b?<e(CC@pWP~rlqw8PIJiN!>77AuZI8eh=W
zA?QHSn?-f5yWIVTI@%mnQ#)yE36#Ejy&QWKeL85_=n#a?=*OHv%*<3FhAPKrPWXt~
zL`M5M?=z4&@Ex@3_EcyBwg|Ns%iNaTLbt}f<#i^p?`3SV{rQ`4Q?h^Z)1C+BW6tSA
z-x_^7i<YN<zThLvwWvws7hj7#J%{am9W**4Lv|eHpv|{3PZcGy=}G$6-L8(xuEFW(
zX;jR3w;)jP=%DmG^EdFWj_3U7&o#xV)j$Zd%avDa``vmlNY=(C%g*Ck?c1`V(18L_
zwMbg8B65p5jik`%feK70U49)TlV0lKGnpv|(W>>co&L|DQwj75;ci<r#+xOQ(cJ;&
zRqiSup*W<8H@hQ<hZSJ4!ARb@7S%tO!_|cPB1^-EQGP|0;Ok~7OO81kJlySDz^!s;
zO4mM1NFt6-OysE_&n#0MNN*P<1~g}Nmp=J0Ypf)JF29{sCb?ZfOF~;uUqmY&@-5Lt
ze$E0vVs%$vdIM3=;|NElZ4EjKHno)=U)H@dnzfYbBhC%CW{OLyY5;Fa#3^u&$g0Jy
z=6fh;zFk?%u))~C%a70^nw%<&YWVfkrc=J*j`(kc3B4HFH@m$y1#^J19)TZ&OSBvr
zXlHx4aD>L<J`&DXI^np12pOm-Bdk!}-NRLO6etrWc@Uqc<xY%K02R{)`|j_23$9Or
zN4M;q^MZL;TN=LP*%JP$P1%dVUm-qHz0($w3;tOvYeY61!-TvL7j5FjIpSuXd@Zq%
zef0!VMN4FmmO-Hbvw5FW6sM44^nEvorF5AYGbCH^XMls?*S1jm<JTutJB~+ShuANx
zi}6A<XSvwN8R=}t;@C-CIr)N98$3tJUvx)CwLQXbyKF=yXr2}cC@ENz_UP!BD=K;^
zS6^Oc)MgFX)hyN~iZB?)M?z1Xmfs_lOl9t77%v_GQQjj(g+@>af`N1oevT2LA^!vx
z?a$yF`%&Z*m{=Q_G_wEO7QoMkF=+ux96x|YHG%FPLb_PskfUZ@h5gh;>i#?oZplff
zhJG|poPgsB4g%AAD8t~}n*BDB6GEDQx2bN;yG6Ssr6uJ?eM%Z1R-4k6LrppNxaY!{
zO+a$6o;o{P!5d$;?b|2&Ey+%}tsJH|qY0edz&wauNyLU1Uh7K<!-71<&t%Tc`q7W`
zdCS_QCtyhaY3~Ql3Y&cE0+mI3yPJ7z&9(Z>MA@t`(ZOmbNTft0-QpBHDMmV$wk52P
zzboIndHWHS6awWb-172<FuAmX5a)Iyv2yONGfbU(N-q%1W@(x0m(xgT8tB8JNO6B&
z29FTx2c`7Mp+m<GjdQ$X;Cym|vFKhPj-;c4IUu-1?90_}9`X}dkA#k!WqzXxI+KqZ
z!WXO?8dZ>MVfggz065&JV>NbM$jb_tN_ytG=JRRKS}fi@L)#GPycm^8_Cj1#IaVDo
zHLf^lFC>eXlucM92bTNC$Aqlhjnh~7fu{*FUnw5sVQ5fqu4gyTyIb2a#t%TXiVU8X
z8sCVtNjK4kkJeb`&dZZ}s!1xVwLcer%BpZSmM!o??sX`W$O6G)#_3NGZ5+uU-vnYo
zLC}G47?Q3=N?|!bc#E4ek!~@Tzz_Awk<K9r<HGvUU>R2jVunyEpy+&zd2)`@4OADz
zEpdWE%hU@@nr6(CHNgvbIzcg56Vs*57+k?&V#>i~!9%4ri-a^2g=MpmoyS#7iv`p*
z`Y4~ac9(v+>3!;Lw?ZyX6B`LGr8L*e2!6yJvg2`Q;p43vqYG({Y{hh_{Va5fGvyZ+
z0$?sr`&o0?ZK7RsfB9f4Jb?lXcC`VYk+Qz8FG3T-D=MqCs_-Px<G#8A4sf8JR5Vw;
z75#8`zUuOF{FsB;%fTqUrn(7)>-A&}xO=97PQcoi3QwwJRI^9R7!oZ&d_KO!eo*t-
zB<Y(C!=}y}p5U#N$K|;19j_pDLU&*p$o;9Kwbu&rrk)wr4?Y<$7_a%VKYn8>pXRE%
zlBZ!W%%f^{8Rv`#Qe|#+;1bd$_Ta)uaJD#zmdqVhk|X}$b+i`Qh0TQOqW5_Jy~7P$
z=z9W`yvC3Z&9Zm5M|TYWtwtUvvMa4A`Z&f_#FR)c=b2Y#x|w%otnVn2=u?+G;bfKf
zYD&x=WEti<J4WD&JRWM%asZ93!>%Vw8dze{t6;$lo?!G>YGg$JPmA^FZk*zhet0NV
z^u|6pBu%knCqWrjUY}VKnwAJWc<#c^gLe|ozu}Pv-|mjCC)tO^3SN^ZJ3_B-%1@QN
zm@->`x;(gpQKPP%ywP4>jsi{KSWZxGfzKEDp4$nP4`G`mI??$?y}SUF6;%9`#+O7f
z3M-CG@!coSltaP`+vDK?q%ZLh$9j6S+Bp>BuB$Z1vr{qo!?S(9ersQk%E41SE&cjz
zlR~Hc0_!ui{dwskD{44ON*q0JzAoqkx#^c<=FR#c+Gr>UF2S2!w;_a2c7+N;z&UNM
z6WH8nkQOO8z7HST?N7uFwQOcsk`mH|Pk3@_AJM@Lqe#fUQSMe@>@ccIVBw<y0xHnr
zN(Y5d-<m2856PFUN&=ROF3~Yxc7Y&Vh9CnqPZv9C{I24(pSOJhgh9%s(Ff814eQ;F
z)&nd{5fckv|K1;#(^ZTmc6iSQ$!xGm#5x<A37+=ubRN%+MAv*n`qBTHpeI|WpCR?G
z&<(34hsAoJnc`wiGcIz-z4&lJ)tX8RY;21=vxqVIYbJop4#M+tE6E#s)inMDWSgUD
z^g*@SQdZ4seO>Pvk&&WS=r3?IK!o-i-ui1C{S%V?5l1uruZZp6;b?%v%5T&DfTTay
z@sHS7#y@cJe;WHsog525k;n-!@BDY67%PBp{U0X&WiSA40tU1E#o*ru9e*?PkCv89
zfUEiQ%s=+*R|NkzK>82Z`46a?5g;^U`j=>yUhH>7@jrvr0CC!Hlm9<h4fxJ~2&+4=
zbsTpZzpi!pcZ9&rbM}BiU6R;v?X-=sPRPVdPtNj@rXtV^V!R2T3;SgK{CH}ZoLz{z
zka-j#0MxcP;7yO_cBi}fd~>{ZHh%+|>c)g*5i51!au6g1jY>r?r<6;)w`SWx@O<}p
zy0|!Ni6>TL2NoUYQ^#wzH)v~d(?<pO)VRuWFZH0BitZjjojgC;v;k<d;CXW|1v1hd
zuWKlRR45=20cc7xv2{F@4QdDLx#8glWpLCZh|fb?0F@AY(fjCh)2G^PJt^)QMfy!C
zPK>HoOQJ-CVhdwA6pTK&sku|PB8tgdZQ-ov3ya0AM&B6A(`DS?@j1UspOzgT!Nydw
z*Z4`{pfvCvs{&)N$@SgXV`+K1a#HfdkaB|!_io?b^H4Sj>J(h(ZZ_F>qAef3@MSjn
z&sYpy-2tW{+CHKAHMd`=FsKj=BK3_w+X@I999}Hvb<>}*+bCD4Y-fKf&*6Pc!HW(m
zkkmz^Wt!gI<0U!0N<*yCn9ahfUPeY734|<&lu)$zP;p_$=k;t6rhyl(K7hALMtwBb
zHjm?Ggwj?@+tlALj9|?mQ+M#34N~G~=jwqYBZ^!=9Bvp}ive@ULFJ>n&4J9th)2Ll
zK}a2b!^TgG_a+E$Q!k6VwCj;IqT!scei1iDIxm)@j^zvqyeG%Z64}y8xLpBAKykaG
z0Yh=v)F@D+q5ux$)y`zySLiBDo9SCu34*FJo+o<-DmW-utwF|=B*7GEH=Q8?9xwb;
zE7fOXkP?xLZs2y3>6ev(vNuFV_b45GncY@c4Al6s_QCwgX~n(}8D4BW;+O+*a|q?C
zLuC9MJEDiB=IQNJ77$`b0$6Gf;;sq22dY|k$vH+n^;4)lh{^^k9IS~XOIsG=o$res
zDix48PBof`Su1?yr$Yl(i6O}xob^%6hIgUVIFo{fcQ?TzJsY*4+Heva18^*%O2zqC
zP#t?h@7y)=&RYeTTa)wgiq450wij+4t%#y$iG$NaI%t#8GAwgv%`Z_yFJYHOrSUUL
z3XRw|6<s0>B0}@3xbc^V+J_~VFg4$Q^o>(M_1PgN77=2xYEl%U@&{99=<Vw)O?{Nx
zE}s)H$CIV!FBTjdo}QZd-jyIXg!p!tLn_e+meX23eL6=hBl0W!@VQeXT+&w!Y)}`n
zQy!>yQ(1;xAh#BuT|`lfBt<UHsKnZO(HX*OFi2XV#Ji1mVHmJmKy1&<PYo2?mI^KE
z{8jv{ID_ERay&yib4J==qqN$_mh;>r>gp%KN3`7|F9yBTCM6tuf!nleB_S)xo&tcF
zFdw!^?A!=aCMS~&Z2H>TEf9x-pOK5)mgM@O8cPu5%8S|yi-U`i60c;}(RichVv<U-
znB_jY2Av(c;`+@tqYTAqHBS{z?qqP3)i&o$i6Y`b$J-xvI4d!Y(o?&7hQIjXKPLJW
zQ{F5-2J73Iel_o*orOY?jPb7J_AG@)6^?$xO}40T!qqjlhL=f+kb_ceN1@12{P-D1
zx{WRl^jcqop~cQ-0*t$loSxM$RHoK=bvZKf4H?PKl*h1&CgNp>7h!W9qPDfE!cG9(
zV0?I|hcDkg+?)$@Y{^%8_3q(BAqD$)0@KCMHmb{&>jvpT7x269LkVZ-!#lr^nwRZ=
zs6UEmT<2rVBP6}&Ruz{H(kKp^?jq>!VQ?nsajh(@8l~4E;;(8qGhv_8R;cM?R45Bo
z56P?%98rH^rq-0+qgKK{)+D!n;8`mF?s_#e;sJ%R?XyFLFfK47dabzjgmV4ZB#nM2
z&0wb+rLo$9(~y~4`Wk#Z@haegjfuMp`_2gD!+n+ft2&q_7b{FI%^Ggc0>bUQHm|pj
zwQkf0ci?Z_){Mt*%X(r>n(ALYi{PSGPAQ&t<HZ=Hi+DHYom~(+T9R<})|a?840V-s
z_yY;hP|kC^oKwXWli7l&y|tR?`e_EBEQsUUbNXk+K9hjWL5>ReT7_mvcZsidD@953
z8<RYOyPtB9PhT1@fZvQ{7W)#QyM&>@9Lg&V7I%Jup6g3D9cH?c77GgvRtbk_bdwIs
z#zR}D+x}kVbZV#dZt28Wi1VCcXe^-sE-2a)STBpj#<vHdxXd6{O&))A3}OXad$$w;
zWy4u_1~NiR89o$h4wFNfjItC7r7EG03-06%Ua`fqKWvJKbGK$skm6f7=H1-J_5ofA
zK~Yv!-V-RMSJ!iB5J8Gm)ygC@m~&wLz|X!KXO_56=M#O5b9h1Q{m2|;XNC@(x6r3~
z#zN@HOdd(c4g-?o_AR~yS7hKx+VLPHcknDrnsU<=MqH&C1gwto>c@1U8x#X;`n*?Z
z1a7=5OhvY|DpL>T-wVJxPj2lu{d06>UX=u2O&<;-B%QRFdN~{}oO<-JrCCJ@jS0=n
zK5wta4@DEAD6YQ4n%m&nQ~Tt=#p!lbjwIxFyn%v$dv7YgJ-jsDhh>Xbpd3MQPYIVd
zA01JSLQKrhVo<`jewOvA)Kq@fXkTS6@s`xNa{uzMvSns&p}AYFntg6oXf(hV$hT4-
zCX(p3SkkR>7$nkZDk4IcZsfxadQ^`V4*lf0JuTmoriuz$guUC$J&M3Yaa~Z^TT0ah
z1_Z^2G(!@)T3X>D`-`menQq>l>HTKCzJOwvBBaTLqK-D|L(T?$l%0cdaxe2?FY{1}
zvzp1WfVl;l{)UeX?=cH?-`?^+@%Q6Xp>RW^4*14U*hXTK5)8?c61YxKpW4uhtO=jv
zAf=e5-`GiHzM(=PgZ{8u)4qgu{}pOt^v$i|&cJ4i;DVEw;I7}U%xGc9cS4G71-~)`
zePw6bXD>o@U&ya^fZrtc&xGf%d^ugvk!HSKi$^TrYDC0R*s!T-&>>6`?O+K67MP}l
z`WCwk&?$Y|>N0O3`kO>+EI&)+B9OOyp^e?Wp{dwLXvnghYj$}bJOI(w^E}Gi`SA;K
zZ44U&=YeWmOERNK|6H7sJX{cZ#yb?WH&D1v_Rl7QV>@hEt&)pbDzdN>>(^Dp<$X#)
zHPpztrjF17Eyv#A)71<0e}$QUPvrhJX8wt{{tYw#tbF=!xo7(S=b^z0D75)^5y&t9
z`xR9Fw<?Q2R)R1w0E{dF?)N_{L6|rJwJCt!>_1c&EKGEa%zs@8@>{<`&VMV4Vg*?B
z|Ash!V&xyZ_Kzw-7y&%=|1kDn{$c>Kn}PMu4GUR+8u<UZC4ZXum%)sj>~w5^Dw;om
z^q(de|7kFw7Y3lS#GeN<{ysVIUnBhAVfr7<2Y%nB@NcCc{}BBDh1`Sjzvx7;roQI5
z!UpGar<Z$e5bs(e6h8OP)63k^YGK`0D_Q$)>eeG%bY<CkWLjM5u8Ke)wsv>eruFur
z8WR-+8VTeQl#to%h~eOV_h@hT6gvj*b9><1)fR0I$?bb>4n;VtL+<3%t3mko?)1+h
zSS|G7A4b>6<QLp4n6{slY)1=M<-lECXI+!&$@%r<D#>x_^bs~r;&lOb=^vQgucA4z
zPdl7a$bFs5<ZW?BT82JAUoW?$w5Kr`k)FJH6Xa#YdDkHjEJq4TFNUE>&0`aU&h3d2
zBDeiEm%tKv>n*V#p4V;P#fm8x>{GYbQ;M34)Jf$EC+DI@V`(K~;N>9qU{D-6v2B$H
zN9M?|>j_!rLyeZ-dm@rMHaj(mfpfLGE7{h<%r{)gMenRsQKfa7T$(5?o{I~a&()9;
zP{vV#U72}6_c@rYx+A40hnmEh5#}Exi_{pDV9QOs^I?eX=5U)SgvDn-=*B`ntK^r;
z>QZo}eSQ;5(y9iZsbRjucCBzQ*1M`nXFhTGzP-e5IbRTI3y;E<dE@RQMUHFS2FFZH
zjQGPYED2tn;3p!w5{)|tKISR}8GC!MaTDE?K2fp25yTv8342$*Hxc$C3+Y%;f{dHz
z<T{kJ{jU+!@|~dOy%eWyN~8T8;FLRPq<8L`s73~L7z7W8y=G_nq|rTZaY7<uZDH=q
z`#_xtfHp4?fT~S^2!VhSp7X}M7w4A{Y2rZyyc}*rHaLto7#;%*GEr+LjF90^GMcpx
zd<(3WS(}$Q$O~J+xG}&eiUt++k26Mslu?T?>f@N)@5E~8{d(tPDZ^fhsf3XU{i^AS
z?;2Ob9=-A~z4*H`pJIT_*68`RybU`|fTd4b*HWTP3*g}1pw5;Z!BsL%Wu8=HI_t8R
zn#&2Kc^VfIxO?xO90tv(ySFeICN6(+kHeI171<k0WVP3|t^7u8^KB+2HQktcbg+HV
zqe90ef`7Dq%W%V5F{H-4GhfHL5C{J-A<eoGZE37~raK@kl*X&{m`7NCl<<9zQrcR5
z&#=X%XsmiFCBpeSGUOYKN*%iKWm<=7w-Lfs*sg~@TyR{!ieofx{nLFhVcSai1!qY#
z3jg4DUz`yP*pd>yJb82N4rDyl%=Zv;*;seb4pZ-U$QUZ@pUg;m%?Enjlu&uK)h@Aw
zR67BS5=EuC`Qi`^Y9PZzq^U0XCIiuu0L6!qgIdVbEgME-f3^30AyEaI_-cqRC}IE)
zR#L1ot2S`rv_{q2(1FLC#gW2RNT))j(`Dz4a|w=^()K~Gcm>c>^D}5nkUf0#>}}fP
zp}O8#Vl_RcU6<SkubW-!Dt@|b%n3wa%FhCY-%qwDr-hBugnLrBHyUL-6|?J3EtW7e
zcro6{7$i==9Mor!Xb7TPBB(gq2$*y*8!cBa^WsRaYb4_+D1)qM>FGmCAmQRHL9eEK
z33oH8Sx1g31+Au82f%VM6fwznL0@ypQp((4k{n8ytWAlr*iexQO+QL7^I43%UsEa>
z0PR~3M0pU5N=$L8IH}^R>Fe#$Uzd388O@aj;YmYAV0S_`xOJ3W+cmK=u7=vSr1g_G
zr4%wpcjwg<%Bdoql0SbZ<TfV@suMzk(^~zu$V36-1eCf>Da55@kga}r2Wc!9bH8xP
zwD5L`kMb_4u0r)D-(qy^iwm)qbsO?Xe>GF4wb7kqbw!(^)uN-5xc24-?6quLj8x)0
z{yq3>*`_WE7~cap2qh<)#OyI$BNTy5)&Rn5;nW4P*xcL0N!Z!9y;EMctzefuay`z;
zP_n`z+hNXlxSb054n1mPys4y4uXPp+xeQ0Wx@-Z0tvko71q~+}ZZp?3yYvq@aKZlg
z!9sAK21TKC``&1A)k>WK!({-c+b(gviYqBd>`Q(rAWTfJ!9)+k465OU^y6a|i>Am7
zOA%~^=)2~k5D3BSCZ)))%uIUc1M1C-D+=0+*q@sE5jfDFNI_NPVhobU*u;QBh(^hS
zCG8ytJ*&sC7IK$kotohweh&NF+>!`vGh5fMkXVDUWe&!X_#a$+fkQnh8Q%~$@sD9+
zNn%QCsj@?6dj%Ps)@w^6zR4ixzQpTxP+Y#R2{>KFvWup>wxVbMzRWPcZgr^NXR&h{
zi31%qvmcSCQBc_Rsb2rAFOgXMI^g6CG_QW;n8YS28m($VoPu^i%dRACL-#^Ftx^>4
zodaLt_ERX#p;<;yK>r21jNzF{g_rmQ*04K{yZo0Ey)iA-wLX77p6KhdEZT#+x&rc5
z?CZQ7DJDZ@dZJ(-=qu8%AP5^4-lx!y;H2LL_7BEJx?m<d-~0uz1N1Qd4R`zvu=}&9
zfa$*y6|e$w**|0nzav8dSmL*7e;}5h>-a}xC@TP{{SRY*;WOF)+HZ`N`S+yYpXUA7
zSU|-s0C@fhC^7##y!*qNe;Lch!2u{{{|ge9`S+yr-%QNL3J@U!D)aw5^Y0Y`e>a%%
zhfUg_&Bj>)eYpSQb}=&mWE6k~Wq+Fa=Z*dq9R3Zy`vYnFJ39BDAvNaTpM+m&S_Ve8
ze@fF%Xh^%Rh@h<>yn|9>BF{3%pNSkTy=KXH4})en3ZhPN%YiZcu}pmC@}$Mp^tu*(
zh?jIKiWO&KXZ@nH3iM(Sw%u;BQzJu1TWN3Fw_i}EV_<nFGA@=`zBHve)IB2cF8$Rq
z{?t<E^FGf<Z;4Cnd-``xlAUwZ+@72}ejB@ET$zep!|^l~4*TA>L)&e23lVjQW@wdg
zJjW%mU_8OFU|~jkv`uDR&&Y1n-)}Q9kr0$!P4~W_CVcdT#xh%<Uy?WLiM7UtAF)M5
zCsO1HP|+(iQO%y*ml5^$chc4_ohCy__Z{e_do48orfRYfg?RAsvRFq=`?VyQHG@jn
zN5k5cx$udpV&-Y^^V{!8FKdyKT?_=T?>7SVCidqR8oVgKD6>p65Iab$tbDsu22&Ii
z>DtH9xTd|?->tVC87phGPGW@_nM!H`MVi4OwJer_OH6XM`wT`7Baq$qHd6F9oOI08
z5qbKn4u=7|)dvIbp!O|54-IH-I<Grc;JxN~Tdg+x{Mi}r&v2Tq-}9v(S*4pS;ExN{
zPxUPH&A_+O7<~jXfFbAMwbXU7&*NFD$b)bwr?497RohveCF~_7u_QwR>tTmd1{w9!
zj0a?=RhGQCr@aNI)XPut(Y&Cmwfs*(L<kP*y{cH~O3%mwk;rMcZAH_xkbqHpII-|x
z`mzk!WOsUrQ79cgW{Jz-QgE$$+R<6!(!~+-D0nuLa&my=SXeCX<D&5_xWKTi@dz&7
zj<pVF)JIutHZ^?31Db=~VboOaY6hDLP9yN6^ncHU<ZN}&1*vjwvs_2XY<*AVyq0kP
zNNVMq78_LDFmY~@q?4riiXZ5g8HAi!-=Pp&yikit2u<`9eWYcDWbr=pXtefAYN!Do
z9cR-OCFA?RHIlOTTAx;WxpT7#L`~)LV*|HStKGF$kG_G~6JKt2NI$FC`)Yoj7D0Pb
za@7o=1L?YGfqH_W1Brsjk7lenW=!ZVPW^=N_O_nlxpu`1f`YXz)47e-@=%o8Fe;hK
zV$Su`TM~Z5i}XtpxL*BkdFQH4M}z>fEH67^qgjs#=q3Z+JZu6tgmJpatJv&wo_(c{
z3c-^kkL;hN<dOJ467>sj>mm5AVR~P{DJ&2uI~NejZf4gBa}1U-XpGJTiu7HQbAnHO
zm;VNK?RX6)4=i67V8Wd)!gG$4q4>d$`JK+{@wZi5OJyL(7q*V2<}z(L*abLR^!<}h
zwxu=LC&n!``Fz_t*D|fvxw3C6r-v_MWb!6~2rl_e;K*W*MXe8q1lO&r;4QzupU0YP
zqNv2hHGR~7iZ!(-%6&POsQ$)LImM$AD0Kr@e%twS|2@$kOsdf-Lu(HWcwolrQ+h#V
z2G<*LD2t|TJ}SzfQmgzPE;2(*ik9P)dNlr6;4wZ+=eti>?`3Ha42!lHx{W;A{Gs#r
z*I;n!KS_y~X4bP#=N|WoiC}g!`88*8KN=Gm?@e>8oD*lWx^2ByjBUdPRNN3M1jytH
zNTRu{4~Ch*K+IQ4mc>2ei5PpQjKNCuZ{Do?-KonIDl{<OT-4uMIxL#zz(p8ug%L7^
zJAaz&4r35%(t()OS!|2KzCjk0K#R3J|8Uhe_AY|`LQj4xL<g!ALiZUtIhl*!-LQ}z
zo78Ztj%W~_gg812gpntFK+a<0<@?D=cG0GxWeRl`!n<^6coIF$cjOB&G2OVqflMz|
zwFCJK0d3KOk@CRIxNo;891rL()}KQd<T2jRHTZ%_`|d?uU+Wa2o;pGeJhd0E6|awL
z%R}k5%7E(0_~}E&qGMN@6An&P$4V@_vu65RrMa9(g5{OzU5jtRda}83v^GKeU`wG4
z>6C2jyXeW{;fsk3X0EoOAXI1Mr3xz%?-|(hTeBg_^+8HD28{4+XTXMH`bY1GWjD)w
z1ELH$^=(fP0aI~>lgNG}gI8im;oJ&uHFQLQjmF0&&+H{a&}E;?U>w}tVqwn74rk>A
zGEf~m2S)w@#GoNe?7M#|B_Fw!Cd{FNhZLf^0CBJU(EitR5?-2^xN&C<Ydp*`rjq10
z<K_PL`skM<dm9|}w5zKREE&ZSl8Nce5Dx^RJA>zq3~p63tow6BH?q74^3RMFG{HQ$
zi$hr+LZ{JONj0hH)YfG6nRc$b@8geG&S{KJEKKki#sdSQfyz6jWp6<9ZTxhUS8Hh+
zSBNKZ!%`v>Z&5>^S|MW3YWwAAVP_Lu%PQO#c|gtaQiYTIuj%9#$!?8T-Y+E-*mms5
zU&zr&h{X|;F_Jm@>Hwkm0ikjTYHKe(ag>yrmg&@L+o2>Ili&I!Z`Y|};riSg+!f$q
zr!MU~`-YJ~=ROb|H)uG|9+u8fbKl&UH^9CpaM=}Ga1qSDjJdrT`QX|co1S}%$d0(*
zm6bQU&ZF*Z|J*Zz2md&V9r@HRedii-{Cduz^hHR!>{@rvVqOq;<O5_BMN_@~0hxQN
zRqdxQ;>%Xmej<K6C9(u}xr3#>5kpdaQ2xN4%QN;`tH)#4`MR{>4ksU~Sa@{5GwN$i
zD{AH$I<mb;_onoCj;_EEr}E@}dc%3n=|<paNnm!`=FoO~0pCr5kO#>#(DUk1xR86|
z;LKt<jeqUvIZzG3H&@y4$Uod55cPuRR6FRiq*M%v6(iUjZodV!Z(R?`H01-d#u>Ku
z7l;+$qw*V``D?`b6I1+=ie&yNJpHFs<X<3GfQ!p-)BeC<KiBb($UWAdlm9PK6d?2c
zhvKQ<v1I=YU;!L%ep~pj0PBZU&_B$BS7=JQ{^GfEU2~HKjtVPT@A&>{J9XE@G^3fB
zkCaovx;_n+4hD1N;Z8Y-%HBdT9Pg3X5YqCsRC9XY6xHU%;p^GU_MVmnI}=KqIXU~|
zj%gFB)UmAPR(kQoOZC0;n+OqY-6PVq&aNQ4dS`cs4;065aK<m=tvA{qgB~99-p@VM
z6o+S-#!S4mwl2Av<>srT4CypK9^1I%*Z$Dd_5Dhyw=FL!Bg>%Q){NcV`RmEW^gfUy
zzcq2L!QpyUIh&XiG9~EQ`>U7jqKHt7h<z3i8)lq>NYM<k^tt}tZ6uU?EmGyz=<2U~
zTIn<{_hd5ndle@DovED~f`C=|w$1!%OH+m8<G#(;X*}<U?NGAt)*>-pm4!+#!OXk9
zn^*1ii}X9MFNk>XE_H0UgkO2-Hwq)n4psb&2VL8HS<}lHJl;krQ04xAtbJv0ok_N&
zm|3!znJi|O#THx4%*@PSvBh99Gc!w;#mvmi%;3H4nU3k6_Pp)4vp??_QBhKT_f}-)
z$&-iE6D6&{s^(NS3PEEE4fNcRBW$hFy%3oTYhLS?g6rn#>f@pn^9<w>!E(XZitET{
zNlibWC4O-|-71G7G`3Z?{;JDQ)_R|R1c17vtba0`-xU``jT&<|4_YPU1jVHgUBIjp
zVkY?HZmaMtR3};>dQUN5!Q2CbRVqL>tRIQPR#qHbQ5X>VE{ZE77NqIBaJ@w%8Mof_
zW#0XBKU)?FNi4@|9@D!}nM`Jo_YpFps29|oZPH@O!d)0V!$81qQv^S*H(N7RsKAUq
zin+x-%$Y`iB$argg_oc;H19xw4}BLJFNy8#c5j^y0UrM9X1SVmlt*L^SvCQIy`d3b
z@Yooakpl<MQUA_eiTcKn-sWq|@iN?rhmv$G7(UV$shd5Sr%T&nfqTfK9W^{`BMFy)
zj;ZK*+lN@r?vZLL!QvqZ#3l+4|HEy${R7cf6%z!@!@G(pv=!}<z|_k9JMErRmyL&x
zO6GO<se9h@MMV&52CNO5x-%_X{g1^I>B-2&ANeQ4Q-Uyh6s`+8^eM1uka5}_b7=MV
zSll?G!NP?rT{)7;5ZU7h8H35MQ`VwgQwfdFm*hZQHRNS+>hBHcGcXt>k4wWjLY&vz
zytTjUV6RC9o<7jF1x?Og^g-0*U>R>3@Y8o>i$wYBANMfa=7HqJfgJS1FeL5h6a#@X
zWz7uK3N++MXJW39+A=8Jq2_47gjn2L1~O|vPLA^8%{h+;gT=w*-;(=bNIVA-5k(P#
zoU$=)^rpX-=)HG~f<Vm&;W?Zq1imZ{%m#<fB$%xOmWfClmp|1J&IJ_`>|xPH+@w+S
zik#6E(XN72`Sc^P0T*VD5f{|ybU7Gf2g(encG&71dAUEMFZ|;40M><X#6?j_K_T-6
zxBem6(ph6)5H&@xj>XqsNm<kw`atCCfPABDZ#4zagE%M(k=U+b<XMca&G)fQP=l^y
z9x(Z@5EI&3d>1%+S;HmEQyScNFuJ0vZq9}aC)@)`w9z$U4m>qzav*aiAHbMxDTGWf
zIB47fo`4xl#7>c{cIz7M>|0Er+NUb#g@iRMWHIN#iScg?h-=gEC*MG<2Zt)uh%xby
zA~?kuu#rdBgXndSsg)$*e@rdxIT?{9v-an@Y>o#}hq?|WV`&4c2vg)hCv$`>(WvZ^
zeD@<4KhN&|$d(O>d&7<+goS`06{>t6JHortj<lzyD0u2`g*Y>I-~dXksZa~NcR03e
zD@%eV81^E(s?fJD=lF4t5eQ+&V1vtLTm1+(tTiXqmW9HM%P{b*jE|-`6gWl3az0|!
zCNgPeTwJ)it&0m}NBx^$cQ4bH#TK-MDT}P5vKY*y(jF{=w{(;4N??lN@@?tFy1piE
zWW=ojD|g_5^PWLpBV<-FifVirh%~~U2!^ic@uC|+nU$mTBBkc3m3ImiPr6Y2A}ZRD
zAu@huwyh?!{+ElD9w2MR@vc|z>o!IjDY){d7V&!U)uYvQR+CacZ%$^DTw*>H)aXGc
zNRNiK45cC$N*sQO0FHx7WzFg?zjq#c!x-3?P-NQM-GVzeSf`6sysdmY&4(jeOac6w
zHLoARSGe6vg9;jLUOo-_3t6)4%aj0JM8s?Wk(QkHf5uU%tVZE+s#A^F*%R75;!7r2
z3AP{l0%9itlLHd|e%MakpSMXvec>HJv7N2f8u|${LS5O$^jccYG9OZ&f3#orM`(|}
zh3%be9-t3))y=q?neU7!lA)bJpX<6m9w5J>8ivS+@K%d=xq)OB(Nb5_hDGg_F?|gl
zGOba@lJ+uE{A9$k3;0kJk>SAp;TZTww(>^|YVdwIc~^L8@$4;RE_A1A^A&1z?vxks
z#At40*{+?L8^TuSTMmODC4&&Sn2fbhTVQw^Bg6;;5ijl=7E$2qK8w*K5&?AgE0NpJ
z_x$`46Gh=7V)u#yYICUb_;lrqIoKtJ&ri0L7eU?fV(NMO#?a%z@7=?@DL&!fmuN|e
z_1q^$v>t~YqZBV;+30Z0PNvakPK%3XQa`II3iTSia|K`?4>*qQQY3^M?I6!^M7)42
zSD_ZC?YFNOnXZDzePXUGttc+?knmsejtQ&Ct5I%g@owSv5#fo@J7)?pU=F*z;c?48
z+e=YhNW+5!CEEE29nY0oLU=}|yFHd^($n@un*ciiO`UgCRW)6|6XXc5&{Q%qrDml@
zDG=UB5ks&g&n1_Csw*%Q`Gl^SBas+Emr8Yeaj8*V>I*Tb=Fk^R679>94D=v#s%eVC
zp`eZU3B;rdQxEU>gEnD>9i*b2LBl#h;Vs{-tKe${-}dt>D|6r;B=#ClxWmSkXHlxf
zCtntPRqUGz-IGUwvvv*CmIVC|DzZyVc_ght4p)UHH!0fIwAhDOF4DH&8IGG<_Y_d7
z?xCmlHzN8HOroU6B_;9Sy1@CcN%##0ePJP5Q|KX+{27Kb^7Y~!9nu^>6A9;5*jQt!
z8ASkdsn3G(JP3=zS&t8K%BAKCX1=-bDC$$HkG_@NB3a3hltFro$evyJBeZ<hTw85M
zlBAF`XK;Z4jGgn7Jc_6Q<%vmdS4CEP1GXjrqF(mJvLY;Chp+@<2Da8V!BdM`3@)}z
zC(!k&#!}kf32r!eBG!3Vj#7=6BqO2*i9-z%@KAs(*LXIbZQ8?)-@X<&6L6@kE*`Q9
zE0kReWIflB=3h=03q~R&l});gMmr#@=C$3?jli%Y9?)L-<FQ~L8GrEtLWR9&G<1G)
zJ4>m>;7g67yL)d@bw^`KS`$oyLYYYPYv@*4I^&Xy83(+5577ruHUXgFY`S`)8NpI}
zHOP5llnn<j&A@;@EzO1&ny-FIJRi&LqO&+GNzddtUtb<yLz(*3jFWd?H>T%YhjjIl
z1MxmrMU)TycyMvZc}m{HM&vZ6#Z6$CPeQDF0K>E4exs{2oh}~?L+vT6-&^eIQh9<x
z!mr*6HaUS!<`k+Ox%%c5(fLC{1j`3m3AS6_&2pRF@unS%@^p|e9uf%u<J=cZ8M2MT
zk%IsxzEX28Ya%j>#eiFc*hbzrFYwFYZ4&VaqEa^EITOYP6PLj^vjN%Rz=UqOVJg8O
zJ*Fz#y|lO08@`TygG(kfJJKKf0R)%ozU8qOykl%mj{*T5hI{iyKDNZcyr11t>3eoO
znE+#5nV%dHqhXaC)wOFhdf|O8uA-9G1&?CgW<pn6iBs`ga?sh09Ma1RVJ6##Jh$Dm
zE{X+`b*K46A2u;Ub6VcFAAv)<>D(0N@X^5PTT;#o*_Vj0(3~|qO1i=tnm(SME_r++
zk+Q^B8ZaRX+U0N4lz?A^E28oxiD9y+(>?*-dGv|gKL-B=Nn!b;-1}>i@)MW;nWO;5
zcl>PM`+L*TekUn^@Jq1#BbxD#5wfg5pZ!ig{yV<}3nw6K^tZ%JMnGr=z!1&K@LQHA
zAq${`kCE}$b_oMhJzGa3`@akvVFj3t|3yoF<5NG6>~B>782{24_@}l1bTt1QQu+%q
z{pTV7ejfip(*6_W`h%qXZ?P7Cwk-V7ulzTu*)OzjKqdvp-=$`!sv2?AY-rx&l@|*A
zhe&#A7|R!RU7r$x8;p{e@N_Y)a!AiOoG-|R-=6Ug2ei5ET(bQd`9xyBM2$)BKgV}z
z0EV+I?+otlY&u|jun<Z_%bnXFRx^<L2Wg{Gkj7`bG+*#N$j+N<9NTjlFkOK&c24oD
zohb6n!)ki*83EMqYp(Q9odZgVbH=;IPls2#J2&=MMjti@x^lH?#k){AeB($%+aHau
zHWfDBZ!AmF6T&4{l<)Ly$Hx-xFpRZ@4-&-bu3O4r0<v|v^EIOvIP}ZVNxzO!!h;y#
z-lo=QH}bWW-qZ|o4O6Lin6PCCN)QL~l3W;P*FmV=+SIG(J;EgLDAa$eS`diu?Hiqv
zQYoU1edWM0iWk<47|KAqT{;o>WI);7_T$+n4I^1F?w!Ux!JUM;BSV6q$iM1M=Td{i
zI>?(B(V8v6ogGF+xo%3(Y%x_T(vt33pl6i=Se2byMquhE&JK=exFB?#DU(XtD%5Kg
zBiF5!Fuhb#6qGUgFl|JQ1cblqc1d7<tjh^}2*5O$<HfX9eVWS4cOHfv3B~H(Y8$hb
zGlzZ`2JIgjw_r>?YUgTyd1J{1>M>^f#+b!3jVPxX_s-4GqUC+x=W!z8b~$;8<Q=wF
ziIX$Lq7aDGJ0B}!lvwfYm8oKj9<8Gms%w6wIp0)bG`42~P~;6ugwUc@+wvhDL}eXM
z?SejF)S*DsAu=!&>t@}eAKKuh8U7pG_Zhg=?5$k-4h~K;gA`(TUj1h*l|CtB#N;6S
zK0cSuiVz>UpEVKR2{8v>HWW`s;-4IvOHK5<DDOo;OppaV6xT00$D9|9d%MZQkPkil
z`0xlF^z9DDsB=)r1s+(AY6g-7<>-4@rr)P|Gm=i#txD_wRAj-p#Kwf|8lj8APqERT
ze4PbJS#r21GLgp&Kfj`1f8Li*@r_zI+T!_UAht<E#Iu1%tzJfTIzGx`++lTEEf|Ay
zBjdBG4g@q}x?0b#q(!{Zv#^OBf%_!vUBTv#S#UOuq(J<ciX*sg7Kl%@7M$C364<J`
z;&zA{1%jk>tif>~9ex6s3YEpo4T^RP`JD_~WHB6bi~s6Ib_(f+oL^U~?b7LuyK}no
zQ?;M&Q7LVY4Hx9OhF70QcoGAhJ?}zfoVgauVoqr%zO_#FD^$g-^5`vG^*bxO=!%N3
zQk>Vu32XXuiscx2h4FSOGf5U&YEy*wZV+XZKrdy~wmF>zO3!+Wc+jX3c3>`I^(7Va
zTrSl1FUDEyN?3Fi*jjkzDCOh}v*gfy2^DtH%9Qo>++kYgO;h4&QjkgsBn&NIlZwOB
za@hw|ntZa~WA%s#1~#wn=seJTmQ3Ina$B33AdT+OK4=LBc|k9DN@`{j3Fse9;8uD6
z)`tQbu%R!v&RC=FQ!bxl`h__~JL(pZTK_}0qKj;&<+v=J487<fC8!4%(~9Dm@?G*q
z%>WZ^|CvU=I>Bl{>_sAyB3$bR@JD=1e&62pZ26hW8yORXEl00bN7zVmrPnpyEox~H
zl<Bc~NS9@N2y%rh%4HXk7J=LnyHbxayqkNZk$1LKLJ0Kv9DF66pP@-4;(h5{XL_O4
z8(BY_G{)2u;fAJ0Jd^qJf^z8y=S2i>HftX1`9;hYz)3(MA{AOkk5y0SCZ9Nvy2Mp$
zY~dXw(Q?zOovDN22BOTJdJJQDGFNil$iFpSd9nJh5K(B_Qi_LII%*o5VRkdGmY~EO
z)7Xryr$zCZ^>i40VL)-)^j~0oXDWDVlep?2xxqRdyG=KkjvNM-t@j{AjAj=bh_J1l
z-brwZgq&^Hgpnn<+f`dn`+U#>yz+?#c8@%Svn|5FTgS$)fX3+RItQYtchmYUq{y#C
zuBFA@>*?fV^|-o=aRV{-7BCgPm4G7y4g^wXxeIHE64N%F!QOM~qS3jTnid)Ckwpi+
z$kd=1q+Zi7f3)ZuBKnr#%~H{1qSdYPI~id7y{^HkaKT6N;QV`NeRT^$F+BL9ucM+^
z+e4{$vT(?>H=kHjTFJPt4Vg!;yS;2gQ@b(~orXd}dJzY?iu9{3T<OQWJuXLA4J}B(
zLl4hjYt{l%KUGRUsiPPyE_kjrNjkO5FwYNZv0cr2+kXC0{|SNz0t)c}nEaS&@Ie<*
z_>_QsMpy4wkmmocy8;5Le&Ux;Mh-Sk_69}{gsgyur2i7q{PYg|DHWfU<&Par{~e@Z
z<OJlnvjLoktSofwfbcRv9}^t|AX=Ll03X;G{^wcdfTZ<*w}7(zyGjgjlLG$#zY~_g
z_?HH;Kdt>|Si*nsHL(7>ui;l~nOW$5YdZT&k)QS7jSat8{97D9D<J;<FN=S^;QtGp
z`cDplKWW>44>Ga-@zwvXW-|b$0{>k#8!*mxSrnz?TIE7qIn-c8_u?v}EG*4wsA2)R
zJ&6<&3O8&492f!-AZ9Pq1A)PtrN2*Sn7%$bdwe>^cO8DtZ+vmHYu9RXtK81NQ`{nR
zZ(gO36UI8F&&BM>R<m{)URUu8n{nU%JbCoGG+$a_jaXsaJ>PfQ3bIUu;9goWveZUB
zO>AR#HvKeyN*|tjx>XqzOwyi@FIz<|v1R0}`SMoSxYE5qFDd&-=$@C-pFgYa*7u=R
zb&alrmPdY>TZLeTPeJEc09?K)!ZJ%AE&I*Km5Z47DadB6f79?Ct6oTf&)Y-$)Y2Cv
zQ{n}7#ij%BaxbG-+lds%HjYv3-Jmzc{?pduN`m#*V2VyR0_i=CFHvZxYZdH$eWk|j
zlcg`o7>7&WOTPwL^&S`QF9t@rb#ob%PhB3XnplEmv{ao<kIE8scz<Oc`x1}w*lzEl
z6`n|1x{6(9A!Djl=RPzHnEI<4Q!hi_%r*+!xLw`0qQcS2R}j*rO_?F+l3#VG)$cJ^
zG-96{fH_f=t(U+^WKq4Z5T$~VmGZ%gq-YUS$4&6+N$y(zoyX>}0Q?;cFXMS(>TxCi
zcp8q`ZhxhtwV{$F-PmhYdrEI3<363BC*)vBiY^Y=^#TG<Q^SLZ<>!p447Yc+QU|47
zUkZ)e(VBHrI)K=dYfugQd20ms@HGd^nIj-kBZLqc5HK^k()4i2sG)nPB9Nau&MTMV
z1!XI4ytp?sF7;8VP_Z7c-$B;gJ(;yvcdd&hCBn0fmMqmm4P=6QY(6!6O!k$CFR$J7
z)n^{JS@Qd*^rjUG_~0Sn-UdD?;St2;lM$?>bwOVa89i#(m43XBus*~aiBz5qSjN=~
zRriNoR3BHdLklS-=DQF^00v()051v+lxJD-;b}q<X1#Z=7_?|qu!Jm-EsrH+F#X)P
zqB{V5W46AItGYHtX3!vIBw^n{3fHaBzN^DGKdZGF<=_Dc)L|GBr*r;xTH`#HgkuFX
z|6NLbQ8@Xqfvbg$pNnD8mzq<bF=DZmvdLiIzA&yX$_k3-Y0eau{%F3*#!YB$l(0+p
z3W6(z^@LbT|J|cdvZ5g-8tz>3>|Hdwr!ievCgz}3#%&f|hU?Bo7^?Z`Jg&eT^R&j;
zJ;!9k#kVmM=c$@qe4NtB1!aU-Gsut+)@(|bU3ReP@%uiHv<7FDV+koEFFmk4g=d54
zHVJ1Mfk@@lkFX&QtZ0&L(0UcYK$V=|)ZE2!GBhXQjnq^?pL8vx6WtYX2(Itsocy=T
zoQty%zwsIA9pX1LaxFn@nWCNRNq@cxz(ZGujy@t<CwjLzfOm=U<<r9CQVC^<CjPFX
zk8WsVTpfqRmjNwtbu>N!+@)GesZ)WtD;=}R?l(VZ%zy)>vmDen#?O>1kR@~CX;;AX
zjCXFF?#vK2XyCBpXF%?)b<)kVbxujy6i?v0QINi_$1gaI_hxfUseAC3c0Kkf5fz8R
zicL4&5;%BlsLbA&l-oi4D9oweXFGR~A1@<9Br`Ds&>ZtH{Sa2;Lwl6s@bSWd4$4`e
z&8nk8JqbAF!th+g!n&IMjTdN)Bp~ME#vyLLsX};@zfb@%s&EhTp1}mi9nhlb1x!Ip
z=;&ex#BNBrh{|P1bIG+Yxejo;rHoJILM!P)5+F)QU3estf95D8T@u2j=Fwn(FWX%u
zmdz3Hl?<`jhVHm8ok(ajhEy(-4fL70ckB@EHkki_C)JIOZ{`sh>5LRueh%*nu^5(~
zwd>u^r-38{S2V`Z5B{`cPq*I%yvZxZ?1#8-l$V!R;{?6YRN#}sw+y^}qKo4!xi3(<
zD28#=)OXs$8SB4<Qx^lfw=dQzpd8WXBSPTfE{5rW(Yd|*z?F@4ATxJhBd0(cgpP*F
z&D0ChGu*RuG(11@?xPosRGO=@;LubJ_ea+u2+wqesi`^zf>y_|jLuJ_PP;PBSm<`N
zg<V8n*JUD2fsjszt42wc^KC61iY_wdy6jXi{HCWA&V<EU4p$Kz-VU+77b<R%#Ccay
z2A=%k?(Ih)aupk@h0RvM@J#{~T#LFx#LZ}ut|Y`^hWfvr$Ux<>o!CLYztih-3j{-9
zn?V+XaJAH~jX-gs8YVyzV#1Q2=IBq?n7>9Yg9s}W9zPx=nP|c}XX<JSp-fIoOJ;Mx
z<ucyeyY;1)Vhn#eKhmewAz8+#wE8%Xnw7fz;`(h0hV!7IljCJxG4GzgOK={EC8YSG
zIS<AW(+}kb4GkDIV-Ky&`uO?b=ZzwwY1|TYB$dYTnTG00`daCpR+8Wg$z4Lv;xJ*i
z%)VT49{TGCX1+iMETh`<O0YIOFav$8N}GmcAl6IS<vL9|7q_|jFEO7h2{#MHwbd6e
z<>3`58w4l~rF&uF<&eWk{Ixt!a&R4JfiZ%=(hU>7We4_~gNr?X*lL}|aUB`Se&Ofr
z#Y0<0%eZiI;@xos?PWoc>^Ti95<fDd4&bE=*nfdWsCg25g_h+?g+s)S(wH!c&^Q3<
z0co|J`GApAyrn#}^iDX464M7*SGFmVfK%6Ft5*R6LJI8gRq2QQXf5cqw$A#O9~eba
z5;zj!UI`+%3_4jBl^N2RKOjTp`oz9Axx5$S?SCF1U<T)h!58iRvNv0MKa%&Y^e|@D
z`C*89*G{Mt+1R0X#3MNL`82!}!l~$GBJ)lDG1Uj6ofD_K<I-ea1CTN>R@P?FYm87j
z{3B_a{}o#fd!tg8lFo|;cn30$m3_I_2Y9FjPvaM`m-UZ2)vuMjpP1y&N*?QPMR&lX
z|7Rr+P@n$|H~vY<`}yp5sP*5$UKR!pI%dE$7G`D+z>pr6pAsGuz^2R03YaF$@$0ch
zzfb33{i~SA`bQV>Z(`ohQ~O(D9xEWd>(4^qpVt1<RtrF%oPaSge?gzDe^k)^vhhC_
z|285C&=mKJ#s3c0f3fp#W0g4pyymYv|It?QU&@HTb)NmCPK19C1hf5d@83Z%>)*F@
zjH;<QtTZ8e&sT0JB*wYw`T{{sxlOsBo>(m^YbL1MG#6n-1*;)YT~Ea5WLmRy5sA&{
zmD40(z(cW3Ms{?Zp(Bog;=gPkEKld|SBjuOjeW`p+i*l3pww3v^^^F7-Yak0g5V9S
zV_h3gXM-+@l`6;l64|qTYouI$#d_MIEslRTTS9|QTvl2}%eB(1_Tv4tJy|-PJ1z#I
zdL;lBYOGn%B~Z;G?LT{V(75ha73{(fktVUwQWM`Q0Nw4Hr>g~V{$&l{j)ILK98v$v
zcMMI2UL)OjV`%H_2ZCk>WrQD_*YgrQuP{j$L$R=VUT;fM;w;QDmuU3plKF}ccVK69
zN=+rtD6@n{s<U|S$Iyo<p&f`#kFS_q&{RYk-&3o*Xo%TrYwlTB9=V(?2eTc)zoP5<
zmC+oPcsbjq6vHGE8&^jJ7?J9Z^GJXXtKs=_#?Q$r_>+>+CK5X;9<Q|@m_LnDa}^hD
z6l@6ArLs%6`zm~ew1)SlF}U5g#`C%?51+#-T-s67NK0t$9R3Q`Vs$QVO(U)bo>H9T
zImjAUsHMR7_|;2<y!_ex6mLmO2|>lHjJ^zO&<|Riv6x}G3*HDMI}uD0eh?_IVM~A^
zU>|CX=(sG&rX(?TtUy~Fne8mu(K7-Fyw_I9C376ZCThLYo!{j$pF&IYtHidWd&?*e
z8rz(nw^p5&qy&BL`=$+%L+#~<z;!s%QVFI=J}Wi61tPYH(gWlVRAaWgP@QJH8aU9L
zgEiHMbGzybopxo7YGS4OkRQ&jN*r#YQ>=iDs}7#XZDN=;Q*p@iz(gp~Md%z4(8#tf
z+6r{`OW9!`G;&81Cg!3=oMg}r-amNg<(Cm*I+rU1Aizn&Q@qC%KmGs_o4>T^!ghhi
z3?w{^j72D$CaQ~bBepYhXJTcYAh1<-?Ht5zi4>I9&-rPf-bzLiN~xv8i<<gr)ooWj
z9|{yp$E+|u`JwWfj%Fxk`2&)<)oWw`zJhTC7H%T+e$DxD4F&R~$(L?z_V+{cV_ILA
zC*~Z7EaHnqgGkUZ%~r=L<ru!^SQE*j-?Z4CA6_I5l$o4$)^uB1UbwkM)L^n~m&EvT
z5n>^YuWv3eCnNKMM}~Hrs2I(0af;&~zHZfItpM9FiTUBH#cYwus)C7h316aVfNG8m
zl#X!p@^aN+ra#u$V28wA_2qLX)dmBZBVM>GB;`9L(Hy_rrF+$N3*zwcEMwf8IEFy@
zTMV4UbTH?pdN4h?aJqi0C}J|1%QlzOHpDusIo6*)KiI4(8un9#C+ib<nU*a)8x>JG
z>4WYAQLCpjwq_H|jL&CO|H7lrxEo$g{&}I4&yeg}XOyFBhD)d6e3u>45y^boT03tx
z%aw}zi>Kb?<et14Sb=px{SBnaZs8fdaC2&gY3G)L`v!3cCf$s)Pi8;FxP*Cu(ouD{
zI}3Y2<YDpfn`%=;z!$97Cf$0a$lOxiZd_5*QT+thX?OUM7W*M#9sy0SI~(yJ$E(=}
zRV%YYks#9(7q&A|6ia5>LEG7gZauAdc6=N($-uAr0rVybEDfeTI&~@S#21t`bQGu5
zDM-|+AMXd1K=UmnEspKPS(_-%qE=7|CV9oa2d|)Zez?~}=))t62pt!_956Y=5-v9r
z;UgfbGjQfQf3G{{?=mtIjbws*GQl%(pc~pS&hk-Y6ET%{8H*f6p292dGC$NBiy&&v
z7-1ow1<B=b#<hV^VdCp)`9Qm|TT_m(IHGQ|aPT;S_!7cIt<z3@@4_ul;6?H&^3H1M
z^m04lN2n;2fn;W@8L!bS!;gJJG^QGzptFL;DqV=<v3+DyK5y<Xr{k5zn4oPIDV}(J
z<bF${(pZ@tDSBUF$c0~=k_*E04jJ{_Uyd)wf4K8IXn}i!zgwHvEvw@DvQ4$!NO8XA
zo8gmkn&!8UUjlQUN)8doLCFP;qiJ9(8z8xHFIVb9px;sEg5kAqFO`RyYH%^%;+8jk
z5ql-*#~ig=g3ph#OLzQr>uU&NM?s#+ms9QyAz>V9hC^^5u{8nK!v&0Y(O#@PuhB7C
z^n#Y^yl};#hPdj5SmI9c`<Y{W>3BX*wl8OkxM?z`ve;$Vd|vZC1P>dB7svTw`o3KI
zmi)Tz{vWw$G%TgJ4B^`DdNtXYP>2b=rHsKpO9yd|l(|*~C>g4`YU3-H<FZ<zlx-oH
z_WBpE;xk#F*rGx~u_sj=0`-VKRiScsDGm@T4i%;ynLN}xnP?|YYx%PgcFrfobfNoh
z(~selfC9lX{BUUZ5%D6F0a9NQ(9QJI1%V?P?0|1_pZ6BxekS^0F-Zo(TaPhQA;{~L
zq7ey9WZvJ8#a9U{u#fJ47j)cx17@o)thxIhW3<^1UzW23(V>4qV!(E2eNRRL$n37L
zjWjkuf^;AU^5(}tbJ}*0-fLFd5%Sd$eUgjuTkhSJdAIm}p~gIlWH})wqIvhTmSn%s
z*YEXC%%>J~rxf7HGzTfh5>=}A-WD9l-|F+rzrE@cEiN_}<5Ra>9(LrUDu>8iv<~qM
zuo(M74;eIGDq{A6WmPgzg$rL50x4mFraPODZ+)xBOy;zyjpRT>(jBPmMyGk^Kv8I*
zQz3W~{jSDjzw0ISF@+B)jtb4_>IXS-P$HsO-o%j)TI>-rQE@-fV}#<_GPkO-oAosV
ztyxqCLY%VsMi&sVOhHHH<_$=wvM;p-f95UTo2ltUqZ{-4zP+~u5|-~$_BLu%`jl!-
zoVMcdEmD!yM$+blIv+X0nXPj|pTHgE<@EcbJ*uvZ7~`uX2T}O(_{{80zn@&F32Y{^
zm)qQM6yS1^Y~?aDct<3sqvJ75PrM=219e$qPibOC@g0ZTOJ5V{9`ExPr~l|WuHon@
z?AxV3sa)n%1N(xxu2OK368G820mba#xlg-{bAtvwHh|R0XkRtvv%>;5eAFg%A-d!_
zzMSr0pt(7}@hW1%^hF=*)kF(d8h<=NMCH^@!^0!1uQ#_(D5;(u7X;2(7^kj+VrdOz
zyZ^T_v(MDf@xG!b2x$>>Kr#Zx7L2a#bW0T%pjaziG7EH?;yc!zuenXOyM14sJ*{Mn
zlKrJ(L>2M{pyNzhr;bw!Wm<WN#Io99E$rdw<lNU98jj~T?^(}u7Kz`zp!TR9R4q)1
zJr7EA9TlA#XuR|@W^>A58%HtZT78)CRJq}HDZz2n_`0csf@{#8v-;|zzmYw{-s7h&
z+}eUA;E)%{>|PuHG5C>n5xRaeGVpmKwAg-aHv22PzHI`#jqJ&H)3|c`29SUe4w$+*
zdqiFr*DMC~?lVlKbte|mG*kMKf?^la&Q@`jWTio&+O3O>)yY!nAL$)|Z0s>0An<fK
znokLrdLLiuLpB#xe?e{l^AG-oy?;$^eqzl(lN&&z*ndZE02!NqAvZst{r@31OsoL0
z96(7}IoJU)4uBD)fb;@(4n{&YK;SP6%dcmm{wvmjk>TG(eL#8OU#IjN$@qC}e^b=|
zcY^Y#wg1d@U;zyK_(jto<6m+eezBMpFzA&7Fmm)S1m)*j`u#xvzY!FA(LX0M04U?n
z=nBAT01_NH2$}y;@cHf2|9Uzo`yY!Kztb8<z|+60?PFOuEH_=Y`ULT<%B4e*LtV_a
z9!A8GTUi%4*?(G29W>%%fYz|2{In+&_2%VLy&;6+38<7|SX3`)>8`^r>G06L^sZNE
z+f-DnUmA9MB|cH<Se{{+4S~?1+kte-S1s&>%W4mxG-!rW-dSIHI%(85g|t1pQ+ZaZ
zd9S^=zS4yP8mp4Y^gv87-}+xqN1slyp)1#{EAT5hP99+S@W|D^l80Nm+#c3;4(Oft
z2aSVeWx(xP6LFe&69Us=9G8AkdD9Hwus0*zTng#NY14oRRoTuoZiWHbytR_&CB$@H
zRc=)bdD(nAZaHbqsj#M2!e7oIJE|2TyY6w^82C8bK>FGQr(bzyCU2I$EM)QYzNGBL
z!3sl5vT@lw;dro$N-~%5n}I84wQ#}sYJ}Cj$NZ`CH`>_VFiG?*${jlDwjd-Bdg#4F
z98HNzaLVbr>+o@-Vx7rgVol}qyZkfSptllR3AB_FC=bXm(hRcZ%4ceds-h62NIq^~
zy7VVzB`=S~WV+{o9&E<&T}(rfS?3a36}L%_ydXx{AY9WQR}oc!gc;>hXCRQoX6a$K
zffq`AOsq*=8N0+_J>8M<xWGrGQ5q^t#K(B0Zu{Qm^IQ@{aze&Rw&8e>mQ&N)?Ber}
z?G{wP=c(>uNNPw>)}=K?EJXa?S?lvHLk-;ZXe$;SNJr9IiCjIVcE!MR2pr*rCZns?
z;*O=Y*gX3K2vQZ4PNWaby3nC9fuPy`Px30HZB<5K-{e#{$%Exf8+t{IMARHhYSp8T
z8rb&Cl-4yGCbd1{cH>5Km{Ys)FldUb%_wC_EbB;w^Q%c-5S)c$bf8#kNy<RaQGdYo
z$`4IEmmZ-3(sVz}vDa0pUDuTlGar_FpKJn!dr`NXM(bh@D~Xk+AAQv~I`c|IO%a6`
zIxDVW>juqA0EsRphnjgNyBEOB0$L!-0-`i?v8hl$QG{+fZc{<$uVpSn$YPou-xQCV
z1d28oVN8$t5Z?MDI|jiS(8}63fm2^)bdWWKKVT?8<kx8>2Jt<M;UUcXJ|DX)^)~gs
zps%fvbv6cF6IlqDxA?v(J$uf{X+kNxw1Nkv!H>&g3IQWayi_(&a4{|$5ht<%Cjh^E
z7!Qq{4b=)0Ka8=)=TfM1w&GKgK=Fn_)XPQr)=j@M*Ro&5`di!Mp|O|O^Cc*`A?Raa
zrUeDhp44}JBs+dg?0UtUxMP`jyIJm0t8{lvVOt4M#N_@8+qc{o;UAX$(;qTN%NSVP
zCZaRb5Km%!!^l*joS9!XY91mE>-3R1R<L*xOZLEVmk|)!C#E4er0H)1A?5@c4u}#c
zLu8(oq-xYsKd@3HhXH+bJArh22Sl!(ef;(nIm^!W=&%p&$|VJuCaDKV{yQ-;=M4t=
z*m%OY6);)@SC+HFJI2*@<-YW<(S}%zs-oKQ{Wc3k!M;hG=V1^{yMh6DXGJSQ#Xlk^
zC!$_)5(+aRi@Xb`C)DVhXX53(FM!Ack^?>>!=fH9`b@aB@Ez>ED~OXthy8|h_9@Z$
zIkZ4sSWiH(8heOO9p}=jv(|-D-I5@r)_zBI4b4$Ik8_I89jdYa+EPR~jd*oKnPx;+
zcdogV1Ehm@^ayo6ZNv+h5Ap#iTWekMM*%)EME3m6?23Y_l39<VG_1AKH3i6h$?{hq
z|3xf_G!BvL8U{4dHz#;4HKo+;wJ`dnTW!|&;OX*P^F`0fKibc-aM`tvcapV^oNc)J
zUFu5Ab`7bihiepcY`SV?xqj^X%<*%Y5d{qYa9)qRNec7}ki)~*ro9~f>?3F}zg_^I
zO<w@sY(afrLe;Vzj=5$wi|)elt|Q_t(ycJL!{4MywQAn;+><eKk{Bh~(Tbr1vQwOP
z>j<_0a_<YSITr|~CAw0km0)cFGEB*OjGU~->H#y8N<KquVR0f-5N~U}K?+vFMWpd1
z$2XSp_DZIHzMC;B6!<`cI06p~=JV|Z5-Z+Bzb}H;1(Y)5++a(*tIRZKUjl)Re7mP2
z%Tzaz^o&)RE3nVrN6>>soeSrU*$P;2O{>{|z~WX9jcRT5sENf|@#LoQA#*int!ofl
zY_Y;BcnhSNZa_+|^Xr{6Y}))5qyA(+(-jsO3E>N=AdOOT33UX&2R098S%wx=%U1PK
zSfARPN69m#ukT%{X08K`1Bz|r%EI@Hp%~l}SgUB|nDT2V0eiGoZ?QV1&e{SqQ;ebZ
zo*CI`@z<c47}xD6ASi1kW3S?12jIrcypKmtA$Wy`LOJR+nAWNdIZO9=mvDCBMCPK;
z9DXL)F|%Hu9itkHLx9n@UCqODo?Ao6c+<C6W!Jh)crXcS6a~`hwYzg1)Aoo8xJ7h$
zU-Z6j#(^SPd?rgS=<+!dM7{g|J|v4XREG;BrVm-%=_o{{ZrPOEP0XK+fh1jcJ$d*+
zGayNxn8#4^qB)|OWNTrW`Z_i|mneM15EO^{c%wJuoN5J^$J)WN@LEMi@+!KQIGuCg
z4t^gF#wOCR=DD^b6ok`ZtP$)=^2hhub5LTb?!`&9Y}|B+p_pkx9*RO>^e8glmMmbL
z*NC71$0}O}b<BZ1bhgJI3m64$+j}fd-^N*-hy#WizHg4{Jj?V8Tc%vc$$WpI>Kezc
z8;+LX$jna9h*G8QNf}Usb@>>Gma2J892!OL!`Vcl;C^_im-j9^nJ?T^MAx+_42a`#
z(L|vntqagKR=m-T&8!$Kv=6s?Az_GHfczP+A(?R!62~x?A^|!PGa%?r8WOI0GLGHU
zxe<jb)(|B3*>rJHjZK5D14o*Tc`E7#o6{qtd-wza-spqPn`-s@xUg{Bes*pTuk^P#
zze1|E*$clo2BLDU-ox<I)4ih~Caj)qzXH~Ol$L)Dtbd}Ne*@P4^w0dwssX@`@e_yt
zD=&@hr`zVwRfC_;e#e3T+p58zhvqQ>+$W5HJP9^tb~+XS>|<gBWKR4BtXbITSXuw`
zP&ylHOEYUDdAawdw*QW;+5g>p!uH$A{jJIY<Int!-~Y3pxcrZ6e-Gqf0}S^2g?`WW
zM_~RR;PHQU{{AVs><@0?zo*{={4oC_6#oReY=Dx{-v+vhk~Yf>a2?Mo2Sq^PIY|T{
z!N3;PaL4)Zi5l|(<}~$;Vq-6+Q$GAt_DgxN47v*vGZde8#Jo``WCP<WCuh0TX+=cA
zOoDL)=WufJ469>FN8#c^Nss+krjMu2+~>#ks3pusY^4wOw-%pP-T`Zkp%n&<Kbw=A
zZd+;HhO~{~&#_M?Io;5pM8~2E)+E~*5&9YT8IrDx-Xo@=**XhxC;**`jlZA#z(B{k
z!1{<^PVBAj{537nt4*<=RtT(Ad(2QYHARX_kq%YxbH$Zx_sMVxY~uScx9xyeCQ&%O
zZ9IFNbR12xa!+kU@CygM^&~ou_%U?rk5>V(THJZ<s>aZaBcx2-C`Pm?$S{}@v#S+n
z)QHln3oF(3jj+h)s$>kXUJ*d|du3x(29U~p#-tAUaMN654%sQ++c1+2R7GL$ftB$T
zV8dsCY+UM(V*(Us@Mx_INsd9EI7DR*3a~tpFNPo~>wtU}mVz^<&>=)o2qZk)CQ{<_
zm)fdN+yMpP9CP{=^umdld3^7uWux#=B^MU7VfsdOC^@-UJWw^$!ab@GMcGrSo<;Tz
z{P5hmzV2=G&#16M%X(ah*g~-GqzB#p_6??|T}5JW5GWK+(k_?;jxO#7lN*m~^O5gC
zuDEC&ui@LoQ;g%ofh?DdabEf^2_27*hae*_9N0>^FXX7&?fK{%(V6S{K^<3T>H{z$
z$4Qi-S4s5l7nF*yz|MMwF1{Z&Y7wxZi}(j~-;|F&Z)n`EI`R|@3?D6m6CPlf%h6%3
zUU00lu-@It!e2Nz{Xi5s^pR84UK%p->}(>XFh~a3QIxJ+pMR!TDUh<AQ2q9f*<C-s
zrWW%-&<C{G+*<M%cOUy7En5HK?)#@J@DD~o_TNbM->;bd;_my0)9z0dQ$U!`zkX={
z?cMjAL6D7*{kNzbK%_bo8z3VR@S9}^^g=MR{tpv5|EJ6EkAL287ue6E`&+^S>(BH0
z{qp+{_t*d6yJG<G_J7=#fA7iy-17fA$=|s-10W{pZ*uc?!2HZ@K*{l3CtD0_+4wrX
zh!CnuP0AmdX~A$=9AxAyKt0Dw&uB0=clGsL!B9RnU-{gXQBd3Qf^zP1Rs<Ut1pDPS
z>ghhy#Rbtvk82}d7+Iy;nt&8~QwmHzKAI!UUkl$8!H1(@e~cxyJY<Vu>80iw=PZ_V
zj<L4Yv`Q&rKzuv+%RAcj!W1*R@s2epba#+egUOv`pM@Al64M38yX$up1jp^q?F%V0
zr%bN8xZn}7f#lnMv9`$1p!Zc;!U944daK*xMm<i+%6M;1^<MpY>+b&UqH<!wA_8+U
z&-QPZ&;5vBEFdL;-Sv9D?GuX}IT=NJo^l>WLGa%>sm&#pOUmN~bC&u!m_LFZZa#*k
zhRfUBbJ%|Lsh`e3GmQ&+?K}J~?b2UYD+QrJPKlh>4QU$WQQo%Hu8hN@x5?<mb{8IO
z;tD(pQKpZ@k%Ft@C`M>?(OR@b_kOsJ<_N~6KAp3XW6(B_o6X6GV=K9<+DjqRRu?Q1
zZ_ltcL3i@}o*~e=n%ZomYi01`dyPb#DikThzA%^wwaucWq1?+)K)Hk+n2xj<D@#-?
zJx#)a&AAKc@Iz4hTMtF{FVf44S7lTgxI*LvGS~;`-bTUpv6Jt8RftH=<f(~K!ofJd
zK$%7{M&EAF7Bl~t;CYF6HhN--F(8#UC+zow*bYe2l4w68NkblOTjNs~i`HF2Uxx~4
zu?jDo+K9eJcJ16IftCxmaYTXFd<t@#OfI9*iD_WXoX+7rKNts2r%hula9{XAwdxy1
zo=-dWNjTK5yf`DgphBkLh<1nAsz;g|1NTNdUL?sGUngrOne@Y0`o7tXB$n~*A)E$v
z*I4&<7S5)E$IgbZh7B`A*yY93%_jdt^?B(C_7Q%PR+Pw|d|E!fg6u&OzAs<S0CFr}
z^ufi{TlW>O=%p_-2&@2t9CsVubHvEJ=Nfg_ssTG#I%~b8Xnd5lqK9zZ$KZOi1P!a}
zs>4#&9o3;h@kc{#HjQ4TCM)Ur%=rPaG$?T)?|?6guENe`y!KiG^+Hq~-VbNoo!=LS
zAK>{FMBq82S^HEiYeTRRxC|Gu+0#JjM0q@n=UAHF3CQ+)BIL`8%vo|kMp-!v(J5w$
zqx3K;tA4~hNxHa+*`m<wjRT6tQpg*H4v*87gidNYG8vk|ZjFcw0l^0oMv%SG5m_!I
z139cd1~}KtX&Gx~V=2k@1-42Bo-XujI+(*bcm!wmx%8y%KpY7T*G&mH0ymnBptJoq
z-PEFJP`MgW_1x>a2vnFkV%%Bs#-TJqcrA8-v?Z8w?qzkz?iq~sK2Kw223UbuQn8K8
zM%S*>sHLSfneRImh^Q7IDYcC4;$#B1joGfbqtzAz^a2cqlssKVCx`D+vDJaqoM$WL
zX1)}3^Ai%Z_UgG5EW#VEs_epS(IjGGtSTdjSAHHXZ6cmv!TK_N=4Wgn)o3*et$i6F
z^Wr<iI5Dgi0bO4Ym%9zLTU;0i(6d65e2qV8iS@F!GuiIcv$)^ux!muLWdQ9%d|>#H
z9fgE;1CYSDSH0eXAULHyNDB++1cYn2!+^znL!k}mZ}rhv;^#+6ixXeaHG@97UH(en
zUd&Wm!VHCA9=o&b*yr<kY{_6Xi7iE7l9gQeIgYi@_{0M-84N6R-?bTcbb@vy#dj&0
z>dmK`|N8-z{EPC|W|%lxCDT^6r<zMeeO>PQ_h-tE9H-D7>i0!bBzC=cIAL1rC<1AB
zyADCXt1d{91bAGKwyQ)Pjg=FaX~9QHLuzb0iZM`EZ?xGjhBB&wFD*p|0_&FK$~z0a
zqY6Xe7^&#!wwVDDm`2U8oNQegMDgebnO#BW$Qqh~f=#EImk>X^5?U)T_>jX`9q~&y
zRRUHEFP`snj&#<TeGP5C_4s2uZYxF@Ke@;Y;)0{oonlLXuc3ZvIIkwJ!4(=k{a(G8
zE2NhxJ?+0!?n0Cv4du&nO>~f|48C)wywPw&T<KL@iU<MHc?z0;)sB;7KV;}Y*$*v&
z`TUU+!c1{EUx(xAd{?YOg2ip>M+N>gdx8NRLQ?5&@+?@Q!N3o>rlqwYs$}Y+EANjn
z)td3x$-37dQHd!5^~ZKBcbymBVxytN@!_eTxKm3!b*MSM6>izyWGbwK@W1P-u$t`F
zu{)q{Bjf_K1dASRek-`ly$0I0VGEQbVouhdY1RQNW*rWAUoT}%7g3>KbC15hs`RKX
zoOW`n;RombhWYZ@M}e|dMN{+Ocxw>h`@|T!QMiMf%|#De{)!gTBQT{FH^0$&MBiY&
zWvuVej3R;ZK19iSyR<Q#Q5iUq?SQG{WA<$Cbr`yAsrk6Ta^~wSXhRNLA!y6?Wy?@c
zdTg=?Y@5e~SsO&Dr()x32VOqW%>l$dSr4+$YE|M`g4Jw?_SS%g2h3&bQz$^Fz1}G8
z{JeU+ShwS<Z51qN)m5UYDd#HwL2LEFB5afTTkaIeeKh*){6(nv^PE(x_AyN23X-Wl
zO8=I0mR`>-)V7Kqgs8_!D5myEcUkKv|4`JNp$*fo{DpFxdmQYVJRcU!R)g{oTyRXK
zw8vfh8>xW~z-IVuomqCV&gs@Hu7Q+7KiOE*ojg8w*cDFzYx)jo(!2NPVAB~Do*fJU
z-Em#C;0KjiE}INkBk{)giyM1n<v!3?JD!*R0tNy+`Ts%;zlMQ7LB*dbF#AtX^0$q1
ze}#eU|1eVh2?qXr_B&Sk-@?G(fDZ$J+kP@Ka3s_K&|W$QKobR^v4xHu0Do8jv5NqM
z86hhxz(~jNYvbI1?1AO@WBKhj{P*+B{vP@LOOoZE*8YS1{)ZII-zfaQUd><j{l{9s
z*dRJ~K*rHu>Pj4cG@t#4#Vmlz8w+4)(O(z;jh$HlW<S8!;IC`{$SeNC#>||Ibc}#R
z^S><q`Mvu6EAYRD%YSU`{kIAjAdQsyr<U+r1?*3<z@NK=|JNb`AgldfFX?y0&B^{x
zl>mlb)XWmlSPj_JQqR#y*vP=f&<K#Q3gh5tZ=`1h<GPfuuI8}AhTwf)dC(X`vx<wK
zLs?)>kXAg>S8O>tvefz<o!78q?WBQ&m!7zTr9%C~3)p{VlEfpbO_fIw2&@ay^m<mW
zSH-9E;p}c}*CM6@%p_5xx@EP}5QX_&c!I?HxKPd2Gi83U%ClO7q)a+?YDW2Q7pcPY
ztZ=8bR%=ra9nFfwSK-#@;nUXFcb_gP9eVKIyz&N9jn_9vJaBUlU#{kt3FtdI+aF#(
z09kmFDHj`Ss_Bv%&lQwfIbB{?de#Rt6R4z7VY;6yE0?N5^%=pP+B-R1p802nQzXx-
z=eYWrmZ^n-<T?fr!rHH}><Y;fh;NO2=@vM|==2DW==8LTEk=1XCjk#H&j+oz%i}4<
zg?^}eMZcLldyah)S#?%(qsn1A4lB^svDVM<Z!yGX<$b?s>|`2GhWB+0AwjbynTBHp
z@9Ft+bawQfUGk*YwwGZG!??U~c?Qufvfm_w1__~q$y^Xx(D${txM+5DUs%(YFTW$k
z?ps84X=F%Be6Y-;m^1Xz(a?!?49Tc|IjET1t6bXG77*(V%KRLiradpo`z-xJ;UKsq
zZgamYIi<p=J%^3E7~__lA-%?jxkVL*hzlp`x)E}w%QWV4G|zXa-<aR7Ki|F)(ab-7
z&RI@c)YIxRsu(V7SvEQ@`*Bfl9_4j^yele+_ZrC;!N_BiP%r`m9~LcT!P<&}7K;9(
zim$A4Zg;h%GoVxDqWQ^S759>OiT@(-7#rc?T2?*s8?BiwNt01AlMEE96wV5gwaO7#
zypobyI}VqNNcuBXn>aS#?$}Gw{K;jh;4*9<IjzaJWLtytwT!Q=OqJ;8Y(}9_u&J#-
zc9)|NFc$`2E=?DRVKiAyR!*PYkDD~~DH@~(S<*huh%^?Fg0mB^L`q4aR_2UOW;dq?
zave}IrAwC?@8QduEd@eb_r+~im@=pAWQ#Fd^G>@PC9|$rvDnJ;s8NMT^U2U^(=v?s
zZ4TDlLBUAmdTN{y98wmNSiS%W=G*##a4F5Dhan<K#)tHieO73o8K}*=kG7#_2T6z=
zk(tYRv<?&mmDxY?=a_UZEE7Pf{@7=*B|OE*q0SX{PM;qtm&&nohJ*7uDY-#$riloW
z9%33Q*uf)v8W#@=>>#rUhe}d;3QK`<UqebdyvSI!9glc)8uq!(vrS3~j9wcA`TPhd
z{)J0lj5K$s+Tlw#8jfjuHG>R`?q{x31Et-pviIdg71@H2Y*v00Q*x;)y~dE)lHYZQ
zA0u3R5bM%=QbDXi;j!|7iPW)+o*Hsl^S08s%9A(|!Cvf5V5CFu<@9RbLTIT!3?D2h
zyjOpUok~S#ukXckujU^*1nw}(;C;w0K!EQDtEU-{$y}s|K2gempy=5<F#OJtY~PAT
zXgO4eho5YJi42m=;1)P1X$U8|d-3f~Rw0yMf(+lSS)J4%S$vmbz?4SJ7<~{3j`6aJ
zvN8&f`UE+_Bo}0+zOdjkHpLV*-^;`5bh)C};W{|(3ag>yd#B?*o!$mJQ3pqjFt)&G
z#cEPUhsIppc-4h%Wki|z8SXC)Q$@5TIYQyn>cTcrU{V7Fj51gkg=aH1t!&XH+_tiv
ztD>XGk-!A<>-TfPR<4TSHH6D<c|;}(e(~X1T^}yKf*bG{Tcw^aOrc$d9mOzCe(O8%
zo8a7W@*d*a_no+1fJp6OcDU=p>i(3A(Y1=pB59!tqMWVtt;q|4g$4rH!FL3Kdi;x`
zCIhfdiE>E7>-DLK21J4=T-H<%d=oEbgm!l?xY|0#s@L|43UO>tmHh#`AuZD;?XJ5u
z?b)kl?p@b4GFP(UgpQ;Z7?P+M-J2st(|v|ED|SKEo<3*;lc_7Wod=`4KFNS+?py2d
z@@esfOJi<zPQkf!$E_(e*H?*Pro)4J2QmswX{+<gfhS)vy3h4&6m>8hwd3;2!qux6
zBdqvyZ)+G;`U1V1i@qVSb1U!YkOS-lN>0Hz8tGV`P%}VIQGnv{^ruXGZ}m>SwDCjl
zylokEo#75R>W918hwoscIkIvW1~}1t9%C68DC_ukX4ZC}%jmg}qTh;M-}owD?IA5L
zJF3|=nVl*9!J5YQ#oOIInYE6q+g;j!xO0(2`PlIQk3B%ge!1#ZuXxqfHUWyzN$S0X
zqV=4=xvlW3-g96$M-hb=pihBaO5;Pvw64lQ2Q%v8)ER!L#aW~rjtKVjo{z1nef306
zkTjL#9M`Ovtke%dYSr3or=5#O`;I@l)pbzE5H=Rp5QVIJR9jVXu_KxpBzU0~L4&VJ
z*9x9Dbmj00BE^RH;U<u|hFRA<<Rhi(A{)!0PQ_fmfVli8YmBTfEV!Qxy1qOTf1g}2
zf_ZzOezrXw-6r9Cw{1S4iX=)$?g}S&AGlO%-~&5&gCZER^l72W847gZoJHl5DCx@-
z&|=OJ&C!gP6MG&w#|DbrgZ{U4`05nMC`R5GgfF9djC3D!Rcw9)!EaLovFrc^`G{xu
zKUPZ-nvD^dohN>4(y>I!K%%PZ$%l!E?dp#{p8L`KxKB(zJyNnW{WO0G#M%fXlOg9F
z0X!A;SeS(U{xrk3Nui&8U2dC*zdq|Dj4KX!MG_3Rk8S0BIGi22<g?pe5C;iE;1?7D
zcSzLg$-N3c-;iiZgr^flV3Z*(bT0DxOUx}hKiJDPm0MgQwM6oOQgd}s2j=fQ%^5lz
zDw2U?=w=;x@N)C6+_#+=Sf3O=tn*|m6pMMd)r%V^%JL|^`DqGdmU?=u*SSTCmfxTz
zl@N9Q;2~J2EKi=aKeG(_jO*BXanf^I(SzhA1mq$jqPmI89KTnziyLPQ+MNS)um>IO
z6;!2O>Oe^#<cvKfRQ~cTWA$N6UE1FMKq{t585Cvyii(s@5Rq7hFZBDnIT@7tPo|Jr
z!<}Hg3eY%ZxxLa*jvY)*jv5!}m{xoc+f>QDPWxW`GI(`g#m1+SRMe>j6~^G<9i{kZ
zCNQpP#Qlr59fuWE+-PcIxgdcIyuJC!pF!mVnu!DFb8_GzVE+$$Zvhq8nr)31?(PyS
zxVsYw!CiwxaCZ;x4#9)FySrQP;KALU;QA`(^!fW(_dVTx-+1r7cdG_fquBe~wd&ip
z=9+8GHQFZ2>WVAfz$?3oaSUUlPcdCF7Rq-@(WxoaA)jwj8YPB|H!<JfHo~dvAOPDa
z#1fIdTd<Pgx<k|=t`!jC8(SsY1Kp?f>ezn0F5(KOl^qRZL;T?xJSfeiKgWFhl}j^T
z86Fa)6sXVnQ9S--@Yv23=T!$7Z|K#>5u7E1)oa~h4@EXDdw61eKK|)7k0b)k8lFX2
zP;@Bw+|<~HDIs$H0K1;)Z90~7xe@L1PKm*ds{V<_kGu(_m!Gs{Br1*jluN6n9(zHU
z7I@Hak-tO-bnlJmW${<3s&Xfw%_&7)3SGvDZG^3<9=^uzp_AO2f6c4ZvK}F`KVv`F
zwOm2S2!4NZpRfQ_(5>;ypm6NjgO^(efKorofW*hJ57ld(O3+wcZ!W4n7zTUV1n_qu
zD$w=rE`OlT3H=IbMBTkl5sm#}YtijRP|hgIlywl^U(D!5Qf`N?c-XTg_gf^Nim5|H
zn`{wSn+Mv)(ucA*p0S!K`$r<we1M)s=SE{_PZ;gUe0~&xs|h5Y#)vm7i(O^6wYbyG
zCf?am_WN1HX7|nqm&ic`o1{)RUrpRAWvNgGL%YSx^#)EP2y!xGtXKG(tUef-$e@AW
z)<7c*7;6^S@27jMIt0KbJ>y~7H>UcQJF@+?2&tLdo6)2iJ9Tmn*Ld&t(Ob9GLf$F_
zFg+ihx&uW=)<8;U++2{uS|-DUe7f&6Rs&bf#vRe3I4!0}HHY%NppX?n?02|0$xy|5
zmu2k~1@gMQ%b3br4G?P7eQc1Q%hKmt*Y;t3@EW;584^BYd%5?7FXb3Kf(*njY#_Vn
z?Hg3dVD8CTety3d;8mc%AH#ybxb1kDh-l4SyZE{-Q#aC6HHQv6oDO`m)Ggbc{e=M$
z#Ct*_T!~7t3Tcpuf9~W3<QTC$_D|3<*RP@ApQGiU0Ksq3G7wb!XQT;`;rzk20y8E5
z_Wm<^X8T*)RW=}?`LX;rX!@ryzd)(~3{C&&9LojdA=p`f8JfWAD<)uvD;8kpCOaz|
zJv;lKt1tY5rdfVZCI9pr{(J0V2C6}D{;=b3cKyg81(urqGqZk`K+5z7%YMx@|7SDC
z3WViMKnX53CQhIf7dsIv&_YHaZ3OIWLC?(c=kbSsQn>fie*SNoGa&S41*&?nZ~`p?
z9;+<C=4vcJ28xA?8JK<h=lk-XSoYJA_upd~6HvB{9T*f~=47P*QECU&?Esc+GIO!e
zb29$9YT4iI`h&s;*Y6`3Kb^UMjkDwWeFWn-WB;b`0nF$9p)LJ~2*yw6`Y#jmA4V{K
z8=CmX8NI&>TKqP>_b-u#>_9!0KZY<kfzh(RNE()@YeX%wV|r%j=w6aBQ?ulrz*Ztn
z5zXfdd^KQMOeCdbtv{L&Nx+v(h6{bZNyl@v-4ahedDl-WOh9n4)NYU6_KogxV-<^E
zON-aTG$vqzdZtKC(-O<Hn0J$51KjhqwQZeRt22q=wB5F7)%Rkv#Qwpk;dtBndG3LU
zFY~jKIy{Vf6zSi&&4-UZ-kl#LhGD5-WiY@W<pP^C;uo??V;(;BJw$!1^0Drqt-a9Q
zi!?gH@?ny21F0SEYYA(>Qg58mFZWqQnd|qb4$l$B_owb6HhA!3XFAu#dgGBK6stV#
zFU#t&y}4Duu(^+)<I7lUpH8Rp7Mas}wNve>lL|<Brj<8qm$&v7wV1_PyU>>8POLZ}
z$m+~#QTZt^+xcPbESVl$u&+{X>4PK|=`m=R^2(C=;O{%O`OgouwvR^9pP}LsA%%f4
zI1QDWHYqzFXT>^+z&S0ysW1Rf+F`hysvWSR%b0bXF%ni*;z^oZ#xm?8CFrjW5Sl8p
zHpmYr;`q|0T`HByyQU+Aa*%EIT|Yj48_Z04<LG<oe&iiwJ5IZ6j$e^WII}H7#;*1A
zFx;YE#&`(zbY^u`+875Jjg_61DuK~Wm;wg*(>;&oxX_!4fHh#Hps{nLfKaqnQXNH$
zuWCzuAq`_TqgW~pk7qMP$a0u|d}k5GP0&L*=xXIz=QrjP_T@VFNtw27ruWdPHCADZ
zn&b;==HD0s-Iy|kP5MSb(2;x9EmaT1GIxsnWcVTa0DK@b$kd|Qb^~dP?#mZcxa5`F
zD=X=P)bB`d{6Y`84CS|RSl&H>TI_bt0_YZCM0{);>xJChEjhuRUg6`R^o9ug!N;zs
zm#iAmaVvOEnk#GXnBabK-J3cF><+Z*%9ye0EybRIdnLhmIdCE1>P>oGd_y+=40coC
z-eG)lhi^|q_-KHg7VPyQ247<I#Q$now7kp#Hvo|u9m7vn$Bc=V)UdN+)@vIBAAkJy
z3Jn0q$IQlFBauwL(L6n^aSDwB9t-boqq6n%qRVKuxf6+;g5WmXDrVENk8APa05SXh
zK-F+x-AT@_T9*$AAzx*M!>tF@<Vz5kcq#Oj0k%59^Pu?SDO}V}O3&shN@U?|j9;d0
z`zI92jn^ROGYPx=pVFPB+!Zsx=c!MxnrU6X^oue34Sg(yU#=Zi6eaEIt(LeYqo3fl
zsv<U;jcu!E?c?-Y(Ks)U*Xn#mbIRPiuat5hz|dFXVT&FXdQ&9v+97vMMgTV5=MXwh
zrdPhT!?~eB?G6Pr0*m`)BV#;BhheqNC5Y)Wl&6#PNuGvw%W~br^Xtd9t@D~_mzh(D
zVjAs7{=0Oat^#BGP!w<?T73*Z4-Si}<6^dz^Hi_!&P}p5FZA;9iIZJJLq(S3@JDn_
z^PVSdZy&5Gu%NfTs=RT^-z$6OsAU38#tsqDi(qGC^&)AMWIJ@Oi}d=+T`I_5mV6T9
zHN53UaS^Wf0w$*CJ3RdIqru0tCRS|V9QV?du(>mH2lUeXSRh&77KE!wuP?rEPqND8
zr@%*r=%drG-F#vb<V3sYaHO(_ysiuq1N90El?G4F!H%LNk4VMB`OuNH_h!UDp4{Pj
zBnC!A4vRZNDqFFumhD5XM_n@wZjzkRBk?9MQ=UM&7Z9$<1<B1{<${Kvp6<W!RmB^k
zlyPS}Z=>?2+hI!N6W@_UG|D}XfLy#GA^pA2qH#SNE;G*Y%ai5ZWrT?^culN9F$QVu
z7gf1^rUJ&Zu3MiaE=rT{J*kFyYtK;nOKD-^eAkY<!M(`bA91&@=1q{@dE!qp<Zx7*
zQu9!0VqKyXLX>c!z!-K1Kl{KhbL~12fnPt+CWwIh^-I)4^~}D%3;PTj9_A>L9#4^3
z7G8sk?1oAsW>9cZ(fo7;-6vFvd`463ca01=X4sR1$}isW_)R3|%N>_>vai0Hf#USU
zu4!XrY8!%+i97ENv}Z@5$W%?SqZ>2Qckc|^wDAzD;BbP|XnK*FGNd6(q9yi8y|x1X
zAS)OF3#yDx&`uVrlUCG+Pp-2KT|?2<hx<6^)hNbFoDoZ;mrx7=i}HdecISR1qf&M>
znuo+3*F6sICd)0+Vo|TshPTl!jO+)cHvEV$QMNs0_!Kb^b_}9on4pfBUB&cZ!e^nr
zWYok2A>ohRw~s2h9(*YaHp*Xn@8}+r63N#@nCfEZt0G-BXiWDNkN_ubJru09BeO3I
zE4aM%*+0vqBZo)~7f;nmc8F2hlK69{I2y2xjK8UZWj`lbf*^=WV`a(}74i*~3CJs7
z%(E7%$V#3Bj><W<>iQ@Q9V0)V)u6%#>&Pp%Tvz_IB8FaFen%|@QqctsJkzQv^asCX
z5A~>S6eSlbMiawH1G8*s#5s*Vl}mb^Fx1W~$RP?%%F5JEY8{ieE9-z29|>;8hLIYL
zLFOl7zHQPBNx9J@WtWid{M6HnhYhse#;kiq&lHXq+$5H_AZs>yF(AYX@PRV-XGa=e
zABNsF9*%cAb968)SSB4Wf!2Pdt9iO2x34*QR<UE`YZkR750OA15o07xnI8r9^nLJZ
zG<jaDdl9g!Kwf>i+oihYFA~E}EB22W0D06oA%e^dfH<-RJGQv6oQ<b*UPFE*&QFFB
z!Br+HO!1+cH5Ed=TkT*igbL2wrS;Qlq#tDMnJUMfd^l|CfGVT6gTq&_N;!e#B)y>N
z!?XjavL{#X=#M%jucZ}qs!+CEcuq&+39oGoGL}1h{l>^S0LW7Y8;57@2eaf|S+J8+
z7n$gsvk<;W&4u4yXXuO=sWXwv@$F`#)U=wgp#zX_-UT=2y2I_qA{Vmd4t-r$!ol4`
zxUknI1gaBD1|s9M#+serGi4m@+6sl#eykb437WlU`cU)jf*}jb%yP?Hliyp!X^pz5
zOr@GoI()K<<0{8)x1RiMQW84bQH{^)oM;R4W;LU^oiBBKpsMKtNxwJm^v$~>d-!wc
zEa>(|Mzy_Guumiy2f^pEwDTbDHEYH@??1qee`2nZ=5P@?U;I``Lpx!&I2%*7EC-Vt
zkJ#DAzRG7)fsBj&hU*f!PkqPkMEdep^aHL(uHBGvD}A<XaKyK77pFcb7@Y>*Y!P_~
zx}$txLt;E^V#n=7OJ)0`U|x`JIO(9|A)29;xz4XEYREM6JZA5^#RSFJ+=3>tI8Aci
zywQhZsY_|r0x`&+!1TZNs|+4wtrN3pyih%D>U)FMM9`Up`B)Kzl^B1nk8y<rl^q3t
zcCnaUA0=0*ZFQ@e%ABq(pcInbX$JH8GhJhw*F%3iWnNq|9qW@7$_=u2I=bwV?F>ve
zOG28+I`n65tZ1=Xk$`(riHm1~;1fLTM%N9{-4>RIjFZnF0tTfTJp6BQL%hhs)nhg(
zsRN1yLxsx9V)$OpMS<-;Me|<5AqdY9;b-*5ImfN5!5~rZ1qBdvUi<b=h6dih9uuIq
z{j?IAxq8Z3E-QQ?MA=t8swpuy+k!6*|CCcTFI+0DrdwDTidkB+<_s=QqUO3zvpZTv
zK(J-v5?hB8^}WQvd8B9wU1UtsE`K)Jgd#Me^5E;MZIm!Rw2c_fSE{b1J1!e+BEkdv
zhjr&5&3S8?9vI~7`W17*2RYBj$t%UOpqN6p0CpjDPt<v+_V1p!t878dlp%yCQ8sNd
z+b3W-qA`KGMa#POE}-Ncp}wCHa*0U=O_AU_4LY>n>rVTn)Xo>Q2d;i;r!=tmG~2}x
zhNU)Y06X_&N)VC$Mqld1x|>>}bB4qSk`W{L3n+^kn8E{`2$9&)9|H=&TE}1c-hUcU
z_}M-GT|nVKrQd;-h(84sfGO912ps&h>=y*{pTS@zAV3Fd*Zd4NfepJkfasS42sK&g
zxj6ngqyvuoJv{%(*8d*6Sb*sO9KfUmVA&~95*rA9nSUr{a{&VzKv2xY@#iV>f0M}m
zy<O};0{!ps2}pSVyifm9dH_(m@}G_Unc`>W;G|~;ia-7lYW_G1ewmPe05yLD4gYbX
z{BM!wZ~5`x){Zg)@%hggssBdpC_Ce?Q)z!;%Ypjje-&zOt4T&Ja-z21RGiT<uk8er
zTn*_V@%b50fWec$y`e*+HA%5D7eyL~Sz5*(@ml=|Z1`Om3xHvSJL)3|GdNeino)6R
zY}D}&^U`{9alYy?PoguRykJZWYgs0dsFhQvpQOc-N2NQd54-7-b+g|)vUM$sVHE@S
zj9rsP$WEL8mayGoRAjKmy?h+Dd3}GZ5a2^>8`L4Y<g$3gZQ&|_!?Cy>)S^?ay{zTY
z8dF(oLyV<HD;VD+So`I>MO9JxiWfANhRK5=3M@o6)PWjZZ5&BBYus?(@k~pJO=}w~
zj`cXrOw|`T-xhO3t%>P;+KhxG*!RQcuXf%pzIu=j(inEEG(8wYca&e%=W}|ksb3S$
zyucnO{h}HdKs<<cLwE4*^y-nIU)P*YrM`X{A8xXjZ&U&DaGNdMQU<eGf^!etjDMlt
zd(1_!vdp|l7@e&3(n@xyJJZ@tQIKiaFaPv|2t5|tl0+Qjz|b0REe8hrD<tl(+$m_L
zyH#~lDoRk=Fpm3D$LPt&^=nfbo~$^fi>6y?t2gmwEs4X2oHC{%oACv$cATT-jD{1q
zZ@eiZ{1BEv5+mymqg*=Wlfm;1*H5#i2kJLNSIHc)M~BJ+`P~$)C5N}m*T)<TC|z!5
zP1RopteP>bI1mV~`;5k4m8^{SB{H`}_?c?71dJZy_p7%Ka>8FrJ1;x6mPVFAE;tY5
zZ(LjH)Y94ybYf_tus2B0Zx(ev9rD6jg%wjqG%(|@*m$*jP}VX+ouJ`~!+c@SucJXX
zwc%+U^c?f1o(#*!4SZ`l3RCC57Vm;aiC0RMx?;tH%9j}ufiXW2!Poe<rHfy{vH?oN
zR`GV2nC&V7u9Ykm3qOb13dN#ux0ye)36HDG@FGsH{LTof@XGtRHN`TFBcr4fxA|5I
z(z3(qcpyE3D(suwS_=~!Rx6(z#H^hlTxYa7Z!iUY%?m*ALXVmm4FAI|OBD*{)$!Mm
zGIZFM_LbYR=QHjU@A*D0Pu@o-`-g*TMbdbb%rq&i5E&u-gYmaTsOn^3pHRJJcH>Zb
z15~5wG5UE~2<Q4QND6|(#Ndd@x6)0biCvEPI`{cAY4hv@DWTlxshuB!!5s>TJSdy_
z5+MPd^!_EZ8MiMZT*8$vq+w6E+DAGgbgZxevps@?B=UD34d(Wa-GS{ElIQ9aLOzM}
z)>U`azlJ2h=t?&)kM$EwvkfwYwxu+u^G~K5UV=2;Gx39l<%V5pj83})!z)wld}c}B
zK0+)Pt+5C5ko9+md2<vr%85Y7(B~bYh{7r`nt52r`oij+{ne&m)=4wdb;<^vM;yY-
z`3Ys#ek*j<<)aU$Hq$G)d+}QdzK^+Y@W<Ac0mc$dYoG7dIN`APQ({}rq6>Cc__O42
zLs6UjU~V@=D70FgOTPuDBa;FmiXu4nxCIOH`I8@XV*)!`a_X?S;YfAR1FGK77u%-l
z8zK2Y&`3uH;3c1<+U&979lf|RiocHb-W)KcRl2uSjUguiFApg;pe|7L)r7$Y2iBI4
zhG7uUCq!e8a#g^`L)&+_v=5mlnIns+A~&&k&q*4wQ3d4E=4fmqBg8vfYtVnH(<|{i
zM}RW@<lvAjbMz7K#!zCnyC=a=cyyH|C@_?q9CNggze*TA@ZFIKmBkY0q5$fyl^Zm+
z!KR<b`gRns$0n+-5L5*mU>bwNEm;7J>YHmAG0j=&f)}xUaJMidMT!YlM7CJ&(DLSk
zv$aZ8$?{+v4RJ42d%M@g0L~mR%F*mnb?@rfK|NRE`LsLa)0aU*G;58F`Fw9~236|@
zBR_il{A4gB=G)=nI>xs!M-i(%2JR|WoW1XmA%Bk{;&!Yo=ssD+)4-w=x*2T?8e#4e
zmnggBP3}#pU_Cgt@h!<}L_9F)E9bL+ZQtjyPfvt&NrZ((4OXaFWreZ`RLDZ61n5z;
zFp0jzIRJVUT@V#E{gAw6M?Z}Gt|SM(rY%T>Gpp0^@8Kq#dj2L$3)T2pHp6`t{&iw+
zp9?#n&8cC8aSH@g^rFcxQf#E(BGTClH*ah`Y$gwppqTPSSTqQ|dcWfKf>_X|i{!hC
zce})U*na=eY%P59&;;>#T>fa0s%saPR0sB4kcCtkqhD2328Z!Ui{Mz8^j6ajIZq+?
zE^)JINHJQh4X{8OH`G3O=12*FkGvm4{bFZ_95lUZe^PC^gb4M17%~GOFE)*?eT&}t
z&HSO0a!oe851nIu(bd1`IH%_|$pBH)UiO%Io>Agg9<PhW&2p)I`8Ui$3%jw%5Tv_O
z6`dx=%X}Ejv56<?V32@tDHACs*Gju6kX)2hBfDN32M2T36O|gN$xIrbcO%<oAz#H0
z9x!vD-b5sU9jVOcl;~!D1sq~N)^VgcPPWm+Aw<U^fO5||doP;T-1XEoJA)Nl&=D9!
z?1sVemAg4gZFfU!UUh@A^YvCPWbnIm4{G6)zM^k^ynF%ocN)=u-W6aR5_KmZs*^R^
z0Fbj7PBgiR4tBob#F(US3grvXO03uO@p=V7^i~3|lw;=cS(h_-_I=fWS=w}gyV*5T
zdGtOTDlB;rzs<w6EY%~=6n*IfbNF53=9a)0^%K7u{rphF&yLl?anjJ3W;U`>-}+sz
zCr00Myt=_=0GWb~tLpLbmhDdu&9~nD_U+6F5}Y2Xc*AqmHo7Qx8y23SE2#n0T784X
za~L5dznJrT;7~nR0l!=xL^1X^tw9`_qgC=v_mM+A_xBWqog_XC?)(I6xt{L=gY!93
z0y=zhsv(gS`&bb>(!a|G5Jrm~VTHyQGqh}YGcZjT6>_GM6Sw)~j3C*MFI#Gq2Av>y
zEj|mhrVL#hMERStD$lE29rf_qIDTH3#<cR|<Z0s==?jpGoH}`!r=*&s<m<>Br4$#k
z>p!-`T<jY4iSm>mMy9$y&4v)zo9D!eAcR2aTA$qsmSXl4IO-6|vWaHwx!SSCN8ujS
z3?-E!$_ykKVs-YoG9fC_%eE~-*9Y}}?d%XDAtvvIZPVpN!gRD$tw-e@KSCv2UD2fM
zQ8dO|z%c49LAJR&+*AXuP5xan?XY5M{~01@vAg3aZ~#t-Y>R~3#&Qi<yFpSYNFAre
zE0$CbsGYUlNc5J$F4$G|k@nv}X8;hMH2K*|gGs+5c4>X^^CmQ|66G*RMSu&o)wf2(
zrtA)LC!Gb^U|JlTgu%;rS~S=@rc1=h*$fPgbv$I5k+&c+Q)h9sC*5XigTB`UfkJBX
zoiw)N&PsWdeZ~43Q<n3noJvyR8bGZr^JRQHqm6l7n3m~-v7o6l4|j67kHLCJ^xfj0
z)T6Qgq)+mTH~SBe&`+r4M~?3=!u9NoKe!qoIQn;MaoK^5I)9IVe*WKYP5G&K7nVWM
zQP08E#*m0X%GAmnmO;?k&cM)4;|GRg(qa%}5NCL&@!LdX7FM7n=1+;oKLqQ6bQ{~x
zHMwlSn%sXxxBWt$vi=SMu>*x${_gPqgfxEUg&9=U)QLD*=^26DPdS0=Ie)@S{ZQ8Y
zCu4smwU{_K=(&J0qrXQQKhN?n6Y>v`#y_rN`?bW(4}R+J5aO@#Q%t`GNxy&w79dRh
zi~LlZx<>3`J%;D1P8WwF8VtD=cv60nEw{{}6iZzV&%Ogb|5tP}Td8PfhLvTnRU~lA
z7}VO9@&?x`Qe<Avxl?zj!_LKzJ~uZd8CK=m&r+|WAW0GR4wfxiImyHAWQA|xlbma=
zS00FU4XQS>E>`1?eec!UROP*5bCa|Yo}}y6e4>O2m|nP7orSeM)!iHKwhyjGXLe~q
zYe1tSvmrt@zewJwobrRO;EsEqgLYRrES@g8`6{3?`kV#;FA*oymG_W}ZOWW8DGDOF
z(0pX1A(t`lQ)9Hl*4a*<Bp`jfmdW8T?;J}AyPt_Z!=i#@q4Yd!cUKI88Z`)COjx87
zhSvC__pIb<_CTaQ%xfh$l{q0|dA1CX9OP4nN)WoIBx-!8ExUGWopHDJ7tO+o<uu4j
zXj6#uuex~8N+PA|H(5820~MR5r1$z&?QIvaBnsJv?mKOEO9k{2M{OQ|abZ&a<jYO{
zZtWezlo@V}<irPqv)G1El5Fgt3tz_kk(F$THKIXM{J0||VG$VZB;md3E}9|n5q;}E
zY+YtnyU#qu%IAF_7n(J1y}*^GrOn^3wy|XpVoBZII2A+=JiUe5TKc+vWC|#Lm!e{*
zKXO}gck6yTS8_Nvw+l0F63iOeCTA;{<W9L+<s&-ae)`=lA1;R}E6bNsL=ST;?(X#`
zpNXx$H%)GSF0HBxExyK=4CJItuZdCJFk@~`SLja@QW(UmMouoA6iF8i5kUwc!LCwx
zNr_+SnMSo+*aLb#NKk_NRIeJ?cZLYOF_9OvwYQ1&IH_hHB^TLcNO)1TN~}oe8!)^<
zd1>z+DA}e}T0lZFRu4aQPd9lS44E}BWm9@-*iH&qx_QYc6zlv*o4p$xikauLrl91+
zkRVoG_AU|IX=qn204nZ`GPy*-s6ou<yFN;w_N{#?+^Qr&dd6bY7{Y7T{M{;*27j@}
zBM(Q<N@x;+0GCKVh-7mB`x@gn$I4(Bq;EC&v^Xe;ejU~ITv2afds~|WYMQ-HirzXc
zv_7z_kO$n=jbF!?pWHvI9!$s`P<=d{I(*dYoBWjHM`F1qMZDj@868L*A~+GI7o%PJ
zO-bcwDKGF9Y<Iwp@0mfN7&OW^A<(SIOJsLsR*9*1?;zndrdvNXyJVqAiqjf0unQQF
z%2#3B_A|x_4SW-Bxpa~cvOwG54-0-!Zcv30S%02%-%mcm_8V*=mVnN~OKo}yU(E6n
z9*7{4R9nEj5xKTpIk|Yop9SI<Q4+&w#GVKH!(9q-PTyi(<M*C}hs8_AhAq7?prbkJ
zrJr;#I2&O}UW^hscJ^?YM!E(LrM{o{?*%S<v>1a*$)sJnA&<S&hUZ#NCKa_(F(P9k
zl&+gpOD!AyZa%Ej!aRebmYmVh!n>@7TjF7LAV6c*S5Y$QDT!NSZO|Eb9$6PO`E^Xa
z%3J6HuaSq$E`0v=(ko%&O&n*@m<LCd=1CFB$f%f``Lv=~<=DFAF0wZU{!{w59BuP;
zTqMi$-t13AnTe?1&l^tLYc!-%kKmd`SgOhB`w9ht1!#y1`9+d=5pGAE)2fD=1e&&A
z!epj)=6K7n#2Sl>*k`{($C6el@OArtPC9Gv?5HRAVrdcZ#BhQTVSlXz@&ekKF%RKM
zuXT{U><=;t;!Rc7x%xdHZ|$b}c-A$K>3~%9JvLn)x-1Tsb`;Bk|3KQP$r4kJG=^h1
zbS#b~X2HJdou8ztkPs<h^5o7w#=!aAXD9I`2N`p6Rqz$KKGtm6%{JLLhFIRtX8Y2o
z?B3SVfPjViAY!<iS<9U!`eK62%P2{2aVA$$eR6(o*MZHE#au^E6D^q!*eQDx#|!k;
z&V!porjNm0VD(**bwx84*^eIxscWs}dqK;m^+QwLE{x{vVaTt_yq%)PxIYU$=>aUC
zM!L&rdgS0N@V_vyxI?6SL&ART(maFKLH2-3{T@&>MPh)praBCI#X`~HrlCR@X7Pe%
zyPc!tUsNG>dQ|SwD&grMY({Rh&kbFK(jUbQ-xl*#fH$lM9P9+Tp=0SJ`$_7I_9Ppr
z8P1y?2Cm0d{b0y}49~=AgwXL-1Ud|$f);WhWQ(IIQtt$l>CkW@>qdwFsFj~?PC#DS
z<x;e!q*?Ga?!v$q2T+?aZpj6UvJm-90D)Q>7gFw8BlG<fI=_``Fu%9+cNB-W7WI8;
zXhd%xAsm^4Dc!vV{6(7@vTTO>Y*0c&@~5i@jy`CH&ziq4p1j<1^KhD(ahkCz0qJ`;
zY;}@y{h6n^zu<lzq*G3M@r&p1b>MqXZsL3!zO*|w|9bm$Si~HH)HkDZU4m0AYO4Yc
zV4Rs4-`-@;b(PeS@afO?1el^)%J!YN(99*HA5@Tol{oEM<$K$3_g`hW&M#}(y<pG2
zrcD*|YwF@0e&k`8z{u*h+{8C#j4IZrIfftHerk--D`WzmYzO%nYto0kmAOG>I3mh;
z4ThyJG~H9GzMm5mH7r>!t#6}X=ErUBQ?OwMd1EK!L<4PoRKOT>&Y9hb?$jU25_(8-
zS0{L6NoJ3zdbvNMR?Idrq_(Q_bf1~!yfFHF*h5#^kk>{Zj^3lRRagroFSGa9PA9*1
z3Jenqs(NW%+g6S7KBquo<&HE@IJ@$_xpOk^h%5{zB%3TVmC{;G$%85UT#7!_8TxgO
zLrP6`qQqWx|2RMOrjut_!H;bi&BRWapel81Xzo#W5Hsvm{%{=})PM`0zYb04PM3hZ
z5!$4R;5R&>xN{Q;pDW)B4smVPo%!bBk;LZ<(D9EOrC5x9wbv8$rUOAT+<J^3p+CA7
zGWA<H@Zd2N%5*|O;1JMwHdZt4ZUbqvQeabHXGUH20CRQDSTmdBPbVPqm*A;^xnYH?
zAYGDSJ8ob$@@x8?LnHQYM?>3K?bsZOG<G%}<aix9w{+OB^MxD6wAJfxE_R+ju^Bkq
zteACL$|(oivzzXi*b`Gft_zwvt;Sizwz|{uOXnwDKCbeVhV999n?8bP$_r6>LbAp!
zn$r)FGx`zUr=vvcr0BSsUY|%y%<~Q2ODg1hpcT9eI;{xDL#X)Di{}yGS`jOMXLXE|
z5$V%X%J~_uP%?*GfjZ&h=vh3YWB;HX;d*{lmmy4M=cTol;b;KFdE@Es1v^thwvgKh
zrO`j0==SJUIMNG5F6ZQ*DDkrctM2~pX#Tk$|C2-a8%Bd2*rxJd@Z;Hkl3Y{(7E%!z
zS{VQxQ&{%jw`Ka-j=vU#2kJ!p-Q*vB^-sUym$Cn}NRJ)J9{ks1fjM?ez^t-A_|?G5
z#Q$<IJCLJb1}5zNaq#caz@J#n#tD@E`D20K54p|%@{8GkBnjuAsQI%4|My=GX5|DH
zVE)NBvjgQO|LeiPEJ?0EVLaHGf1RoJ4|dcKc8L?H!1>4d`t{(y0%CrRRDk7#Kp`by
zcH$4P_`3?i-z4V!rqJ+rv<edu%TGN~75-WS0d^o)_V>B^8}MP`0;0gb2z*A>maP}r
zk-ctysCg9P#E?)`VMEqx)YJ)7fg@H1)gy)WkqR0PR*_Q7>vU`;8gNN1(Sg(_ekPH1
z9(Y(T@Xa}d7VQ{a(HW7E5ywOcCru!2ZVWB_K&eYA!4guKIU$VV5#rS}$dR@%h7}^F
z?{irZ%s*S{7+;}eep8X`DyS#wV5?{q);G`FWouQ>^K{ciK%Y_~ld-hV=T^G2O8;i`
zjpm4zF}P9>N(ghL4ar-Sz}%7SnUB}oc#*K259qpaowI3Knxf&z{#1UB7<9(FEbcCR
zzNEWKI^!&c_=fl0_f9>+wTKi2Mc*2rA~EBL$n)~r8s#$&N!D54-xw~=8#>o0TQyMb
zl0JuuGwXc9HZgxExRJ-#esHL)!d~Y=eIpG$UScFu<I<vhY}D?ptcYdkVTt1qk(dW6
zSl9a&H`VkFQUVNPklTJ3av-Jjv2Zd?rc})Bqtr`vqjE!AaT$4~Wp3ALp5eMlvRMJF
z+Zl%a67JpEClyP5aS>S`OGu%uvbVGEP@};?USVSZZ@ls_$zM}bjnJUiFkn9GO1ih)
zZ7yH*TvT-+rxVe>CT7Zx;ooI|(F?(D?Q<FQ5u!&f=sxRKQ0>|^UfiixIK26CAJY3!
zafUt-_mRx2npCkIM-S%){h%GLikOOIzH?{ylPGE^G+FxAP;)q&OGz4{0JXVIw>gx0
z(~1((g9*EbmJ9ay_yUKbH|9cQ9BKb4nf`lt59MH?z|K4!D3Uc{&NM-Ekzk^+Td#;u
zZwKCj^*6LQ8#-_Hgfb#*v_YAqIWMAiU!X|PLKya;=WQv=>wz+*wj)Y(hHZ?%YQI66
zdSUxSkkz-LWwqjJ6C7!hlma3KT0TB$TF*0c>RmYvr+7D%bsEdO<&xtgSrN;eHFd0Q
zz=P_T5rt%E4#O_eVx)q&Uarh=_%@iV#jF@q!KpJCifxk<+i=;*Rhcf7`HL(mlsB(Y
zA8+5F%OP5$sZRXW4N++3y0+#!dr+wj{@rqmD)i#QK`*J^x$UuIjN;+p;=0Axmo|E%
zpe@qJCO44*-$B2zX@8JU$OO?<)Yun51g~OpS&=~oERBY`1OWzDW<+%EPAV(D7dKhm
zW1fKD2)(m}lEuTrWdvns@Lj>x_+Dtv-NU?ceX=Ar1GmeUNdP7f$a0<N!|0moL`6SG
z$sfPN=WZ?^VtBW<S1HS9s*R<cs(zK0OiA1`8il~`u~c5aonLcmvibhxX{nwASr2EZ
zQS|L?W;u1wb$Qw~v231LagD4V*er1qb>wbhq9|klxe7r9q9-kPb^v%^X1z>mmDvT!
zr(HC}k(yu%^gM=<?;-w0K6vkW$=<Lmre3Pr1QLIrR6$843%2LNI69R4@*0cTFO+rk
z3sye_RfI;O5Kdi0)e4OrG*r!`sd9#i`N0=<x4IDO179`kyU!3h&ns4!s=8xkH;M-^
zXs3N3g;bOwHCrZmko3?ehyk>H4CVzyCs_<wfmj1U$J5P)zV%Yr0opu#BkuTEW{Tz9
z8P67dy62{@%hp;$Xu*5?AJi*xU)`COnki>~XuQ{{^lk8&R@M|k-%$n$^+T~T4z8k&
z5By+l8p(`JLB%m1F1T_{K|wgjdS3@}82&v6tICEM(@iiO0f{<#E%@!;J_@en+!6lL
zoh7u@UIQtd_Y-9bIpwRV{q_t5y1dcAs2KLkb}GUZ%{PiNT(qw2a_W6g)3}hrix4?V
z2StdJfbsr9*t>eGo<9EeV;}PCdJZ{)e!LoS!T5(!trDl=>#@fD7Iqg$AAfA{c0Q9l
zAEVVP@u5$j_Hz{k1PY+6U%3RUeS~>;H;1x$4?&1Fg0Q21id;=b46b&{4)2Au42z6n
z(E%RB*p+X@ne99<AASrYWUkyao6D!i{5IT&;`MMxHBQ$o?^%mt$r$9@uYy!egkVdH
zb8o+OjY0-_q3MCzf5MjVaKt$gB)+_hvftMUkaWc9HCssG?F&4j`p$~ST+MXd-a<86
z3p%W<z1Vjf4NdSdr6<cG`4Wl8)!1t}6^usF*mBI5VhVRAEhCA!P>2LU$%N9xR=!-i
zK(m1x2AQvrOktr*yxB;>x{;D2rD;Ed4{U}?CCQuiv;`8hRP(L5(Y@(1C>nvz^>XJ~
zSIo*5&p@|3o?qJeLS>VgKwkZn&KjuBqJuw;Ls<}7fB&H5YX3@alvftf0DfrX?Kx3J
zJT-a7`Db~ytBmu&)~S{2`l*Hhb=f$tB4cG;WLjD-T$y*MGy5a!61td17TksI_0c8C
zX$!vFRGa61MP_`w0wo5P<tjGOatRclKYK2R(8S5))Oihf66H1DfvQl2DyT%ZR%^7h
z9_$+R&T6CEc`YlTGO4pU>u1gw>(85v=d_nhg%z|khi8Wy45V<I6lb!5=qvii9vNDu
zJ&z&qz#j;aAT>A77@@Hqkf2~2#z~1%?H*fUJ6Tnm&8Acj4TQGdK=jZjoZ!8y$Z^bz
zunc~89e5oQi8iMDl=hrLQq=|F!CL!N9>>V;3K4$Kk(To0{wEnezmY%ubAR}!`}}YH
zVdj4+uKkNY{P!O7?0<7KeiT&vA->K0>*9!?O;VNBGXpAT{3^mNZmDZ*2+N>iYT#f(
z#KgqG1<N32XliWYK*Yw*4h(&pI@rq@+P$;3w6V4_v~nQggk|`fi^9du$qHQKowbFv
zouZAdJ}_J^Wawn7Z>S(D_;*1Bp?6AukVAO+_Ob>*mJksa0e}FXAAnE5%QJvbO8Bi1
zk&>ali4}00#-_lIwc=L#^hED}Wa|K476JYMP*6~CP;f{vFi1#naBxT{NZ<qNUwD9k
zf`dbVLHyVN6ePsIH1y|hdFcho1^_@HKz@1v{`LR?1p^0e4H^a(4g>)9^D^M;&r5%`
z5E%dp0tN;O4h8`M4i4Om2XGkx90dZEnCUGfn!GL)i7h&_PjnXaYr(26426j^QWiZs
zUl>?SENmQHGI9z^Dr#0Xb`DN1?sr1MBBEmA5{gR7?^S@jfWCpDk+F%XnZ1Lfle3HK
zM?e37z@Xre(3se`_=LpINy*tcxq0~og+;~HHMMp14UJ9B-95d1{R3YHhbE_{XJ+TV
z%`dEPY;JAu?C$L!oL_vuyt=-*y}L&NnhcyYFmR|JCWC-F1OLEKz#)j4AW`4SL+RS0
zkudu}qYFl7RdvC<W>Gl9(6gI>#Uy22Cp-UP^-o6sHJg3^Eu;UB&Hu;bmrei*G$=cG
z3K$5`dK3^a6p)t%06Z87@B<hMfFJPmm<j3)3!nu2@7w=Q!T%lw07|g!Rpdv>^Zt^J
zW5up#$f~dTem&P8El0i|%r(@XoEARSn2EkBiyt};SAR7!lADN)j|<2+egWWk?e=te
z-LlN`-zf?}Ej}~90LFuSUjS#urCt~L-bf9PhA)5&B#0Nl+PLZqU|SkMXni~U0(e6J
z_X6ljuX+ItB?I29o+m!L^J2XK{P^@=0P(B<kB&`-$6FVM7XakPffoR^3E;u2U+3=Z
zOy?Q?D(YG3E6DW=AiU%I`dY^$&LQDrQ6|{$jlsrs?w7^6)##fgKs<j+2(m_O$UnfE
zo!aL}tf_9#u+8s2mq>i?lqKXR|6W9Jm#LD-Mht4U@(Y>3W)8rM8<6n-tLp<&lIN=z
zz@Wl;;*%dQ(+eQ;kQL-|27nHb1Gkq-_%b=v&AADY^x3;O(;!9`05VA7&BY3`qb@pt
z1YtgIh?%&7fO;)$x*a8>;UO{hzRd-ZT=Fgch?P33K)mjXJ_RBK;A6y(t$_#-0z9W=
z^l#JFj&my>#UDjDH=ng1xdaHEO$pBrv7YR8wgqrmf2;`c^ZNhae{&x^JAIZ&4DUF?
zS&w>xQp-fj-}`Y@CTecf;7oUhe@|vijIw2ijw(2SThsEvy)bla07NMU0^n)sPBO(%
z%0>ujhIK<g-{QS;2yWW&OqjedF2UU83T|~Vu>Qa^Y+!Fw)q7ud89j(!LBz0qSLIW6
zulDowLhYO$2Ur#_y9=M0xqRH2>^^;<=@lJmt4+A3%P>2;Rv8Su?z+KTcz1UTVEiTt
zDTkDMS%K)T@W86U`)*yX1q*PiqDTtw%zu^y|NL%W0C)<o<KGs%0Nx=5@Snxa@ZVKu
zf<2>*hk9Ky-YUKTV5EUBDbxV||6S|<qcZ{j0zkNp?N28>&x`Ws)$+(T$K>u0DLD|k
zw~BVnmxf;eij!IukJ^@-4YDedmk|?*!o3#s=YcfLWPkJNf3i0Qwj&$lb)nP>yx)Bo
zhxgj!9tC)C{O%$~n6X;B`xv(h`2zSx*z0w#vc3R#=n#DYM5%ThCIjztfXRPx3hrJ2
zy})mM<a+_=6@<P3F89|K0Qdi+eOsiMb_#cpX-6+)ZI|Kh=ADN1hbQ&fvDpI}7}~zp
zF#YQ&;!sUBP0mjq`Eq>*AU-;Y+nMaVvCrs<z2o<IkK%Xx9gk(etIq$kpAi_KpGQA_
zzxAE#(`J}uvqoMvO_*a(DDj+a*JN5AG^af(u{O0@oG)F+F(RUTZ6!tocB;YYIgTnk
zrRQ@83#K>vuigHj#-Crm0AdQ(PaiONRbBuEvXEb)zW#K_{U3b?B_#J;sM<rl#mPBW
zN~WttC^6Ys(fUY9Q59)lv~c1|+?c#<s9g6TDT^x$)|T0%NNbyf4{&i@3Cr~y^MWHi
zcM2%IweH5fF2D!6rWPiQrkKrds#s;6$-?E;rD19ks4RE%HpYiK5;!ZH{h(VnKqhqD
zr98OlxGLJo)F}e$qUxn7LNjF%MUmU<HB4RrRi&q74Na4DYtgw)&5<-;7#tbs=a2gy
z`$*#eE)9_#CQ&G+hxqq85g$TI-TBH7KJ^oS&5crv5}VHPlbOb4uV=}{jYw)rKQ7$~
zT{)oPqIc-hg)x#tYVk52$K?n)VncYdtD=LpY@(jmm!ci3M^EbSpFS(;Keb+8_l^{`
zL})@ou)1se3s}q~CsfD3=O($o05<sVs67dQKS#08>)zqa65#Pa+TntP3VBq?yCvz@
zGVfJMXckE^Zy0N7OQ$h+X{gS=i<KJ0wQ^F@8sT7}4hQ;8_8(RHR$&gEXBX&G%1k+u
z6ACOCERKmf>S8u_X5!PVEZ4==#mna9ze(fP8Y%}{P!WNB-2(&beM2+(NQ|#&-jL9&
zsp5>XAaG;fXH~s4A{Mwp9{-8=;**dq6*Ct&b*dcX(=TVQec~I&N!mJ2cP_r;RPNC&
z23zY7Hz;&VU;D*kOv=JSazmhrw}dEjU-imCiT>_(@fS{9LjK#RXNioUjw7XWo#!-O
zJOM(1fA669f5)W)WSjq$gWBV}wKiv&N^$a*4DmM1^X`^y*S^*W*nV%+N!Bb-01T}N
zS0BI~!o1*7q^ToOVUF&qaV<kCQC%a<p)vhIrf%+vkLchdeX2Tz!!&E#o9?qNb9gVa
z2rG2;ch>>IU-1Yga;43IpoMKRrT=*SwXTV-#@?gxQ+0EWxJ@+zUHK*fxd|*?n5IYC
z${4e1UzLVjCK6MII8YkZ%<yYDuYRdZhg_mSeL6qt{xgY7U=C)Mv5OUMw;P^NeNCO%
zwq!~hJC7unUa&{`hQ_S;H;{Hh^WIwz>LpEgfy#4{oT7J@rAtfSZaWl_M`D+jB<?-p
z(7x#mtlzaZ_%3YrTr6nkmCnKxitoZqdh^b`onk{mJm98w=~1}#am+bzPA6ve2(-rT
zvvoAI#Ei4hc)(uPa!HY{^*vF`QZQ_u&a<INKigNaL>I95I2UPn{%S6$wv5l<oGv}m
z+R@aq4u`hY*tHl=%m`&aAvqu_$lAffW!M%zW)Nex%n_FqbHdAn^get;tn*t`kNQf{
z1_Gg2>_)81^~wqzhUG-98xqR=vX{j>d)rq>&2hN+3T6mI^_X;eH?od7OJGhL8iQ5D
zfbrXc^$tv?NfHm0zGV8qWN?&)Ve+I6OJk+y3DnW+A~vsELAGUntb8=1^QUIGQNGY|
zd_kP|E<1oL56Ql3ZM@l5I<G?Y!;kG1;4tJhr3)?`+9BmP`)FO(d_xQS3c&`Z1s30G
zBgoUI#L=U>F*?EYPeT9oc;5fL?h{uBwo7wQr@CQ(N151}>Aky=S~{muTK5)G44lh$
z`g>CEB6O=GG1%N$yO@HZv{SC`R0SF7vC>T$6bY*OZ7vZS>6K<Pb(I~nK|z+(6f-Kp
zRQz4?MRCe*dRgtXaqLMivkpjeI$NMpUo)RSmNRPZ^Q&`=8jDJk*>gRO2x{_lbA47%
z$68QH{}q@pz^84>sXSVPWTniMxbo7N;=Lhtvvrwbn-bP0Q_kT9&ceYg5gFSo^t){`
z@UrV|@i~7UDy2LuZd)fuj3)Hd?2NCNSQBP8qON9(YmE_x5tSlaj_?KWVDLpy;Go>d
z2oBQ)w7lDb#kK1cI5Y*O!t*611x5|h<pXTo(G}3Mxu-CF5)8^NgIYX1Y!3IiUmVgD
z==7Eu=w=NfrH<9sk{9Fysh!{1;ww@RZMvH`*$l^jkbzUiDa*qibZ|;lii!EyFV30=
zL&hm_ma0!WxoEWcSRJQh-Y-KxZYo@RXNcEtTbsR&nKF?NDbi8C;G*5XsXAk*VU7oF
z(AXGXEo3W{xBNv;b^R@;oa&{VIA~33Jln|W1Q~Xdc>QF!K6C-qn1UIjzhd0hv|ehm
z$S1!dGHCVF7J11Q%i8RinZeqa7E99kFD}j-2}v8~ICXBEY%XQTq^isGmwaYJYc-aD
zK4aH{#pSUK&Sg^ML0sLiW`B<sjd&4tb$dDWk|GKDP&<`YV%L)C3_8v}35A$&^35P-
z8A%;GL~GG%1xw+DskH248LSj5CV9J5^fH%+jrXx<{VO!5$asxq#__tSi>f*+xfZ>E
zIRt%68#>)Vz_^{RH!8>{w9wBFS5)gc=QDA-aQj$PGU%HG&L$#?shij$z$2<%>6k{T
z!&$~9#2tN;N@Af7yI(P`NqK7%yIUbPTs>5M;oxZ-3>GlfG(%F;7&thqPd1lSnTy6N
zxm{@V-AHT!n;Gfk@$U6^4I0dAaPWpLjlw3U_X_^%?MrjXW-#A}sO{x#3*>t}4;1#c
za(10@B$evqYhP{6QOAaBD5Rp-ntK0yPXdvZou<1|ece`-z>{L`uSSwgp%N)%gQN8d
zMnwc1!;lpDEqJIU=iW>QV7~Dx%+|a(P9r>fP85ec`D1?a$Lwt)+>_T3p97R}NY4PT
z#QD4n<KkI&+2a%}>yKj8>lY_T8w^Q-d#`*Ygl_RDisKBbFO6_R_tPGjyo;Vtym9lS
zqXwL5PV5`aghZ9=_p@_Vjd(N1WNGT`;vg5xICbKU4<e}+leWnY2jo*u@l+SnVeidM
z`#2>gGH8=!3?G^vuu5*~qS^=@bB{S!5N|Z=6I;tS9a6O86+i^=2_v7$_JHtr)90#)
z;=6UO$#Gff(J33;aG)@=_?GJa^^Hm_tr*<MdFLRk`(!m0WR1OiOm6wsRD7If8TPz{
z-L`BDtb#xOwu$|8fz0C-F^+O+({hZo==&U3)`f7ZIo2h8E<2HnE&p9BRq1lv!|Rwc
z$M9xN1+AsHbCz6nk8acC+=M8WK~f##<ZB-C+M4E2F++_Mjlts+I(X~r&sht$t@`8g
z>+4pA7;MGcsic|f-1Z%0s!G>!GhB*sISR_A-G~ZB5TsK99KoiA6qLiFqfiLO$HeDT
zTM{x>xq8`I@<S_Q1q`yA!00P3QV&+-D<{b+(h2c5!*u-e)9REY)TE6~THLdH3mU{9
z5JtZQ!k-WrDOZgg2xhRP$VYG%$;gT+hV9f8ahxbBY^6l;=8P0g+cp<);BK2yi?c79
zF@^3Fv6qb$O+_3i+3xtp6yT_hrE29Vz-n9DK4{(&%p1f>mke85T3WlZjS###N58Jp
z&=RC4YK%E~MJ;I%D26wJTAKwLv@|5&ebtW|FB;T9JL4Q^roCNKBtkISq(;TT?KC?X
zAz>e8lS;aw({hk;GNb%uMsmtbT81}uX~I2LCE{sMh9BJ^Q6Km#qgF6)b!^Epg01K?
z*{euzn7i^1oD*iFzo-gX(;t<d&{>4%$;Krr3SJ=OVfayTDLx&|;JP(`%1yb^I~eT~
zj7cqnQqZ1EEBbE#e4UWd8pINFZ7QLq!j@1iCtaW*uM%fUfwNK?lONArHb||E?~&m1
z7ref|z6w5j%UMs4Hs$oOyJ+b?w^2^%z9B;V(@cEroKrprb5=uR40|#{3>t~6Op9hB
zwth0;#N^*)gur%R04Ts9vf|mzQ)oKP3t+0i&jf7mzl&xI+<5QH9*vT3pwP{_fb#_h
zHg6u8g#hwKdGIbtL@a~@x=DmyntWQhf;?e3sh`7*-jvK^K|K*YcUnVUI?vgSfx}=q
z;bgFVunj4O7n)b{1pvJoYH9t*R$|)FaHF2L9Z`G3jW+1QhDH@inLel1=*y*d=YCo@
zYKgqku2j8#eb<=ETM+*wm?BdX$(}*o+%RkFl6;n8Nj8YBr$27AlS)n5t-V<^A5?;?
z7Bokm3`|fd@_K;k(Jhj|lzE&=!*S!UF~;H&JXvvECk&8#<TW`yatCp?%`;2K30>8!
zi)iJbnznwt4GvRyFlx+^ZwWz|8gzCfJ+B_=vf6ewsL1Y>!6@D&%5mo{UN6?ooZJq2
zUBMj_&hGyLa0^n5k1Lji-5zCwMJw8n3y`qsY8sj;n(Py-Jvlw?_w~1`bJ#wj7D**(
zGVMW;L1f)Lj;HtQ;>K*VJnR16+3*H!mbJ;!a;rA9pJo+Fqpq)R8<?mf6N4agKgLmA
z8|xKanAeuO{eC(`*OmQDIjSxzFv7HH4E;>0IL~xQAxjcTrgW>g!uWmMHcXSCW%Kl1
zjsjfzc(b4C$C*57N<n3&x}rz9P-W`b5|Q|Qw)b&c1=!Saby+mi{!{*>moESVFW>RA
zWG$;>)|N8RDHS&qc=d2?!iF}Z`bb-1_7x18EMXJ)A<GfDJ}=Tym$@=hF9VvzQ8Sd>
zg^s!s9^q-H@S?B2JG+)5Iq_U8=$4}M4^)eb<H*~?=I`x%%?O5)t(Q~^i&gAQg~^X)
zlvMacb{4i|V^UEsWfy9?DuAatn3*<V{nzoZQ-*byGE2aeY^>dc7;!6wQgdxXxJDDy
zNTf)kU{`iTyM=2x&BFSD4E|6$NrY&w4lOYob$yFYZ{E1b@Wyp&bsd*<g+yEvhS2s<
z8kfkz!|}YZN!iJGLP48B6Maf@s3+bz&3a#Y(OTZuN4m2y4Y<How-O%gvd34}PifA0
z8idF217%?*c}?NiImu&cR#kAchp?`~_pW*HB4{X>i&-j2ib?FLRPV50>-L0OSPukK
zV#B>0`AspnzS|etG!}n$*M5~pXf9L7hE*pK&saw_S(Z0bBx6ReSf@JQY!hd(TJmTy
z`zX4J1X&jtyRKzrDXR8fq=w@Sfw+ZxO7Oh&QdD_a4V=Hi*;A-nHxybh;dY_<{9Mn_
zn@cJ^MwbwAOEb=7<zht%&Fbq=MpKibCN{H#*7&+{Q8d~0xII0}xBRt->%~*ym(6nF
zJ8>t|_x-4%0grCC97T`j)5pB>WqaBr$$gir)xx=p1>RPt4lSQgtZ>bPaPOvR^*Fw^
zwTRm1nq`>k)R1WQ!}e_%WgVF**~Z!>7f>qa-;qgEwoL6xq?y?laB=lJY_oC6e4M;S
z&(kQH-pzy6ETXnA8pS!pO+5gLn(=$ai<PShN)-DAehm<tE#AB__NFD9tfk`~+FPx?
zo@Jj9b5yh3ot7k}EVVAEbEV5~B{ka7a3#PYUpa4~g7<(IikGOPKom;NovTrZ;M02g
zGDs!DKa?y@M;pZ*W8)jPGr4bHyKS&sKIJ?)YuUtR<veP?lM$kBc@@-)z|prr=<A4}
zz9}BL0oP_W*vbMw=X8UaLgLG1?N~x{N79fqc3Z9&$YyBlF9us<yYA(zUYc=IGz(1B
zZy(F){jdu=CAYH^ZmN4AEHPFr!z5SZbWnkzkOtvMHC2Wi=GL8@7hXVZI!MDMQ^%bU
zwq<raoMx9U{2=uR(RMqyAzseEPB3FlE>>DrYCV#Uks)rVK<?D+wxff-FPB?AjBw%d
z(ZsOHm!Eq%c|%cs9W^UB!s*V{7QI+Pq4{Z<UvITKo<{kji04p%^|hDPVhNN;n<G`P
z_;e4qjY@3K>~^TB1lvw=p@@728~7%fgf)sZcYRD~ipxj*j$F3Sgwd;ztsLtaSUQK8
zF90NW9z%~Z(`UNogsI+p!|MQ2b>aVuz4r`iYH{1W-Nk}#MG=&uu$7w7lnzq1D1ihC
zCM1;5L_kO|AfZd!TfqWEG$gPM2?$9D2?PW}mGYM=p-Gp}qy(f_1B$TU{Ll04InO)q
z`^-7#+nLE^)`vAKGb@>_`@XK<b>BII@;3dm1Byd>dwx&U-_~GB2^F%BY!%<xR1Vj>
ze+)_dP2WusF{2Q$W`@3dNp?_qSs56COIR;uJb9|;(A3gP)uTc1et9~5#mNlR7KP=I
zSXiAum@Yv^qB4_^fa!e(hIDz{+6Zb$vM~whjXN6i`-a7zwZZ6Vm+gy?rj=e#tKzYn
z^(RaFXRE6LI`%>G#3c^K0Bnl;=CBkIfl9ITr;M7GP1ybPG`OiT6u<&1I0~3zo9rZx
zF^p(D1uisIS!m^`LF9WuNZ+GylBx;GTLx2ZUSf~RMiEC}bmOgDz<_~rQ1v{7KDCz*
zhR@-p)Os2ACA;8t#p`uL<tiKJo0jnMCptzmGu4xYAN@)yD$*y6l)07MlA&UJ_GpJ&
z<`?XJ0)OB0Kt)^^3zNJg8~G4%kp>Jdq?2nXs!1F;UW!g+ePlRFFd^YIORlQpBV;uO
zTEMeEg9;9Oz=9KLYW1!VlI-!Y@o!2@$GRf|m@0~4?2zag3^~1KaZmio%v1t6cXU^$
zqfy+?OR6ZRux0<1B$?Csn}lJd-PIy3Elv8*0Z%g#(I@^U+aT}HMFq-^GVHT5spIBB
zKKrRi{iEHBxPuj(F|MDe4C^7&Fa%hbPJwi5tpE&Ab(NU8rj!E;b6WtN-&vjEQO%t#
zkDr*pf>g_6Fza9ElT3|6nCQ3dG2Ox*Gor(Zh<a|)96(z`BAg(JFUG=MgXg*=b)vEm
zn{6jPEHy7P#1K!pd5pDwE{YILbka$K5z*Z+jtOoj0F;@dHw6c8TQ0q}jN=7r6XG@n
z>5JWf?o(hR;9Dr;yfTU``VD{EYiu~V!d<cOJZFA5?Z)~l!#CjQkIVNgPW?KdeP({E
zsIPkznQ=4uN?l=)(#~zW5TVyx&YnIi?Kd}7PcaAat{#Bx&J}FKoxWrwg0bE*7@VJT
zWS{s2Qf{>8c5^M%SK5h*)V?<*!zPrL`x{0&?6{^8ohlyVlJGXuiefzk^6avO&*V?8
zKTOxhWS!UlUYBPE`7%crb)rzR7ol?};<aqEbwTxY=*Et@Gs<geSGSUZ5`Es|%}{+`
zKyM6}iYiUibSi@fqc<7t^1l9ir9nI41n~6+6ZLdE+{k9@)(%$FcXh36b@402{JQ|V
zr9AW_Zk^PYtf6rmT^dP)lntDVwYS8t++GZy^sB;O@~yApmQa6xN8oR)1ZgQ&zQ%S(
zd?;x>up&@=;il^XGNczHb&&!rS0PqQ`W`C)H->q8sPk_>_DH#y1HYP~#K%(w=ceh^
z3oI*R>ny`MZhr0;1crpc$ANb-GgH(_e2&#eJ*<Z<C&al@Ue?a3$zdXvDth*dTjzb^
zhF}619ybnuuG*8w@8=4`b(LuL;=xS1BJKFVmtLVkaaUrqxjzcCBs&i(tRC<+ZW670
zbP3_<QK~OOBuJqp28o6;2UCSmd0-~jPrCKVWvR=w6MJfdEiFzIw~TVC6>zM)z(n;+
z>f>6OY4Il~y{7TnmhWn~Nf+VoO^r_ukt_Lj`0;J0a2UU?<;O!;CCkGvru#I(mnsrh
zrb?o`N?xcM3&1sDN=r4|FCb+4B0@EZq^xf!0|rVnp0Zqw2S{%E##ICOhot;1J+>j7
zEQH~~5WVdbA2zJXvXF>oSyQL4Y;vHQjpjXPP;erU0gAIO_@TF(HgocZ1B|Ktv@md4
zKY--I30-kKUp*K@5DwiFm-63oT?}>|b<G`jyCFQAZ^t@g;(APLb$6jLR1F6LBSfMB
z;xoxg^#$hz38kA|4UC)M0MzaRTDpyZQUa)yI-(K2n4t`Lo9UE@Dk0hQpfG3N_r1UF
zl;U)4e?Xo6G6(pwLJP6x;*jY3CGL{E@<w@mX7F_J;ax`9Gd-e<{>fjOQI4dri(978
z!-7osk?D=bSi|dwSMsCrmOrbGC>QFqU2bp>r9!^q_HGl&LZ_Vt1{#uG0Z$P%U=)E=
z4~QL1vX*0)B~-JtP@xNV$psyNR%{y~5CGqwHCBEYAW^)6?&lm8>wPqrPlFUjsf!=q
zZEKnU{BtOw$8CN1*$=zog>)0#=s5a&(NjN$=VTMzeZ<2y*ekebwCw_}EBaE>Ec)*1
zY3Z8<KLte90HRI8u0oQ8p-a;RxUC7;6M5Lb8YRHS-4eL>)#Y;C2;XA^^DHUZAQxT=
z+aTwHnc6j1KFDMAtRpNfwG{7n%qc;Hu|v2(C<V~cYX7lb(_jAg;qrn7uK20lr*=bw
znp1tX){KCm2WWy(qEgblQ(q>kbkEJv5Z?9cN4Eu8nmzi==~WlFWYZ}k24uHqRI@PX
z;5HxtLr+s?q<S#r>%Fscw-cSq!O_k&h(!lw^?s&F$$Uo1y9C)pXG9$+AIMRf5yK5U
z4h)T29GIgiEw&c-E3Cf^=$Q8X_|Kuzkg29Txx8Vk*+VBbEHd_~`_#uFoeB#I0h6<-
zUzN_!5jB;OJ!21uQbs>K`gD>*NP|Q(3IR_qzBcZ18TBAhg-j6uK)K-e4k>pi_lhQU
zSO(K?G9Q$WqO<;jU-@sk(EkNNXX@H!!S(#BUS{#=jSWz_Hv<EfxP@h=Njwv1DB|8o
zNb$`39<CTI{Ba@Zhk;+?<ae3Sp1s}4A6ADH)<?2W#DDSaw^*Br-;g{M-*hiV^PfXG
z8Mpp9)PNWH-M4=di7c<zD-WKy>mD2!iki+pj{YCGM!5WueOk=P4dCvV`tw7DVRqLq
z1w0P=HQ~c0>zh#qq@#ChOIT*f`D=g351ay7Lk^+-d-Ah*_79QquWa8mDFj9q|07yU
zbMlS&oB!fY{_pCa|Ic6gTjtc%KZlYTGnW?fBEA1PbbZL~P^|8MGPM-M;y!-ok}X=q
zkC|SO(X*ETML!5KHHarCi5S0IA<DIZ4~Y)nZy#R#zqd1OYVrq3TPFUKZj-|IQ)PcB
z*nRxBOKWAibRVzx6a!-Xz4_o!yU~U9qk;av2((Z9h~2P$R<3!(V10#A@puNN+?^d4
zmL8Q`VtenMQ*D{M`hBOe(J0&v^vUtg3!P=2jT@J{kF*UT{)4uQ3q1Jbp<1z?(d>C-
z(t|Np67L)N++f7T!?PnTY-%R^a_fob!YQa{$EQ<+8ZyMw|3<<1_Dv<DH}iU0T#nIF
zkL=dIpg#Nf!QyuC{{X=lwZ<aQGfpG~HR`tfbLdB|2+ELkZOY!7pKkb}XzD3qKmSdF
z{om96KW~WmAG^^T*KBsPPY$;JIqbcswYF>+?CI>eka5~AHfv(pTDgdUMdRLaidOQh
zH!Dma44LYG9ewkkjNtzbh{(~-?~^OBpT)&5czAkXy)Dv|Ow7xJrT8d1!M(>zzn-dr
z!gJ`s*T&sH)wOU+@1y5Zz%ns2YVF3VJ~UZ;$2g3k>OYi5BKq?dTDcqu1Qwj>kQ}`S
z*fN1R5i*o%#cSFf%g7>X=(n^t4Ka08*Z0w~wPc|Mcjs}rtvUzNI=yOBc;9D$Cb7Uz
zWR7waf%W0UouQ$(i?4Lsf7Te1==mui6YhhoPRx#KWazDknIwg>qP4^IV>wYcSl*QJ
z_4k9UND$KaQYV>6>rkowJfm)?EE`{2XC3b+1<?u%GRiB4o)Oh7QJaletIc<J7UN}p
zI88lMmRI~3Iu6_n02FgqzR#A9)bN2_QCOCH3r+WvN0YzDv|f4npr-1ks#V?Xq)l8m
zl;S5{j7E`+9Vq}7Plq96NfA6vOYnok3gARDDk@Y}8AW-+z--X^A@cmtnL;mAF3tY6
zHPw-#YVx2!%JA4$>)G0c!2;<$lZ^QH!cmk$>r(rV^U*UG10Yyk=U&B^lM5$8=98n+
z)mFW&Yxhh&YYfUp$LuXe8iLP$m~g9i>KT4!dy+S$R>cd<HYFz-@r<5(VGu0+p7y7S
zHc)zjOGAP7{m;c8s`D*^*zn+4q~h|l09Znj9!~LZb$h`;gCGRv>OY6LEZAH>Wy9=-
zux_w0)M>-SdDn8rIvHNp4=p~5SIh)$X(eQNnIxd(+E2J-mn9Qx$||P?sB=W^o*v`q
z^C11Rj|!LQbL1MDC2|HBQ#;Y8A4`3B#$(0m89irNa6Bous>mP7Qi$pc%}g;{0x@jh
zF*||>6lJ)SvLr*?OWA?H#e%2iS+uRVg4bO(CMxq5!cPR7y(vxGyLW?b>l=NM`w|oJ
zkj_Vzw`Tk8?Y!{hqCM{8KyO;xb&Muz;|fe7qxMJ_*p|60e-Lq@W@0&y(jTS>0pjaT
z23fabL>U>k^Cpy;wbrXCwB?2<01Kzy5?zpfDlp84LR51Q)%N+D4Yzi6H%@I$$Gr^e
z7!5e#OrEyk-0S=5te<(}lj5<H<DBKwqpQ!ETW`cPPIq&Dkx{gKZ^e`EvRs6v;m7>s
zo#c2BZEIz<fxH})ggq)uv6H5+R;Z{IFw!zo9{H}V*C6*BZ5;tsPpA7ySHB0*_wSYH
ziJtwx%r~5gxie&{J)>r-9me2WJY97htLL0!sJo5o&pzsF4-6gZhwGhx_+-ytRugoe
z9;BOa=aRDIA`ih<71MsiZ;gM~>=?HtsMOTZx4tIww5Z20?5#G)E8znsoGR$*iX34@
zlhpdNFT0KL6r34wD3r=#_mv^)_P)2RC{8JeFP+`>5JrFbDkpX)X<6%4q3OBYS@vjG
zNYus0E~BVhosQ}wOj|?$dC|V}t!Lv>UsFGeQUGmMA<@qV-74smZ$a>wb>Rb#ubyaI
z(MO?TMh5>m<m)!b_2417I}KsKj?EDR$L^%f2YklBNQ`qCHGRL#twc|+J^yiS`gx2+
z2o252b#(!=!SSb9`D323j{uGUSu>T<rZUsv>iZ0$Ax`WNzf$f4s!`POdE{yC8BOhH
zrX__q7Ht3@^UrxMcMS(YfKE_$vTwJ<9L@GOqQEmd<24%{tx&KR&=BPw>lLm2ZFhR0
zj`Xo!W}jm!HgoiertGG0rKrE_>Ah6Wp^d+@{Cy5{zy72(1W2>LYq_Y~WLExHb=4E+
zG)^PTZq&cp1v9B}(Mj4Wz$7V2c)&Z3j4M{VYiyP>1;72zq3{_Qz3OUgIKgdD(Rw2K
zI*2<1qGbaaj@#@_s7_M<y3>mGl)`iE!<C7vel;0zu=#t<ZYulJr|~YyAjQ$^`R>lv
zXw6Pb<G9{{-w5NQ-_BdwJ`h%l$v$9p0AMg8$pS4oo69)Lqro)+#_%+XMKo0Rsm7FY
znA-S+-t3es87L(s(gy=6*(2m$%OL+&+*3V@R~SGsda9?nBm49M`2}tIU4Y%su|EqJ
z`?jyg?4&=dD4`ddjCc7mEiwf4bCUt`T^xe7ekM`TGV7`_j;EjLR@2hk?9|k*d|p+(
z<`k^dj-}iYY^h6)`aBS1#3Oq9#RXw4@sX<}YJplgyCzWRcQ!w6SpG<F&K`&n%y$0y
zW5SEIKQ<0?H#M6dNi&{I#A+GORYviz#dBNaMme3rp)R$o#R+3qs19Dv0u8{aDUE&O
z2VQyLvF+!O9>>#xAl*&agiHnuG$ZdLI4^&*y82r#YjXnVfRvjh8HIq57=RHxVFpgm
z5d+!hh}tH!_R+{ELtC(t+A4$Zx4;@JDpc0L+B!*Z&bV6ZQ*a(`jJ`|&M`I|uAbLY6
zHuYltnF!kPUO8V|og>x0R=MJe@Re>N!x^Z)P<5pR#}r=^2Z|y<&9gZ&Vzmoc<TkOG
zT9V@Hng!?HSpYpCS*eIXg;~Nq?AnUm=Q^#?mC8z4e34deX+RX$4yQhlHRZyA+nL)L
z2j!k;pwy2N#=RS=94pGAR)0#OtyofG7ZAcEp!rT61jw*4l*=E$8?b9|<@R^?OOqLE
zFUCr!F3}}<G=On*MM7p0i&Jb*);g72D|K_B8nDOM4-n$lCz9VVlN&L^;S!#0d4F@b
z?k;ON)r2Xm&~2f$D~MyD`=q6-)(*(E4J@+_#$FiiKwF+b!i+(%ozDY4Gs<13{8=E(
zZK51JWy;8iWr<M=Ly~(xCKLO2+)2tVE|6h3Q?1?hGmJnHNPF1cTbqe{?v;1FsN+<%
zFnYv8I>+<W?q6%7QCcf1WU?#fO5@e$EmYetfq#@rEYv7e%^a<z0SYigZ{?$%g98`&
z+<P+s)h@}I{PYr{5Jo{Stxn()@`yo!@!>ky#syrv0olKE!*k{2RL`-|1=+_Z?Ga@g
zH%KL!Hhp25Fq_lh;F{}R0S`6^uil;}8Y-=MiIWZwymU=Y8JA}R(R6_r2(m#0S{aFc
zQ4ARnRO^(keL4)wc!+D(lvJXrHG~JJv1Fk-2_ixFY=*|b0E!&?&!MCls8e}y%J*=R
z3jSxSNc=Q<I_a0Fyd_7LY{#pLs8b}R@CcvJR(`CCtW7~fWab0J-EsdZscMR{A$u2j
zyqxfOS5=Q?fwr?lFiu5PRk!LfDfArhbXY0n4)>uUih6FgaQ;{&EZVrr2$8LUAo>Dd
z_HE3!+Wj(ByYXpB!oxO7Lo-+yd(Cd3+-0WH@l&AcV_R-*I)AJl=2vAi72$L<+~+~{
zmeQCj3<l=GF?tKkI43xr10`o@@WYfP2Y}#c?%d4B8~|>!pxRK{%9V^oEuP+Gc61%q
zLJYVquv`WnWyq<nfImMg9<{OUsLo4W<|w^IqiTi0RxzhX#_i%yCVy)34pq2<kr-1Z
zzEc@_5Z7aoQF{r{-Un`laWt+u?2i|;FqUHvJSV`s3om!s(U7<<TfW1bfGEP5id9!3
z$<0=rAS-J!Qo<?bt<!w}7zUQDk=ytERH?q{UkBHA?`nD<oGIqI$u*QG|MH+FE|-s)
zs5n#>>yAB@c5&oo;m?C>jv=`A0KtCFkRqa=^TWI!bFgMnHYd#LAWsaZ5tyRmSs^lt
zv&_a-9|$~RjkSZKn)pVo*(UgL^LVsf8x^#{f(DFHlzuccXLG572E29C<VpWw*~g=D
zR`s>D6&q6Yt8&$O_*)YENI!>Cf|Sk0gW-r+FJHx&S8i2Fb2hq74TU^-oItu8F_x7!
zmq%RWpGC~jeb0lVp%1&pL3;C+3=Bu1;Jwk_plnU*2)8gNs)<^_TV%W$RH~a3nVzwz
zWyQlU9?(pGwwM3YCvEvB)p7v%y79Xam2JDaYf70xrQpg^CkeMMtFV4Y!nexYuMZUr
z7P7hZ)aiW-Sj{@;NwMYuNotR7Qva&RUx=&7R(b`&vwG+q0Vl43M(^$m`u5wga74P%
zMswGc8lrW?sE@hglQ|pnyeJN-RGmkQkk)?A@jb?Znn^B%OnYZkqIto2WVhaV7F7RI
z=Wx@}&a}+ZfQdXdz(Gh=ZR+96H7T6oDAm8MB8*_~pM_D2INDgv10Jts6|1$TiRFAf
zVZ^%<&~ZNN)5#Y;$aJ%btjpv^lP+^is84@5CZ_mxBaX(g0hS~%pc^~i?!B3vQjZ!?
z*03&lgl7+@zWeoChG{IBWoH*%6guyCni9y60Rybe&h7jar}IVOjFu;~PkvMGL(cij
zldyh$Ghh_9<5$CT>x=2(ICocCsZ!dgM#;_1o24e)k|%Ai=(7hMFhb4T;|Rh0)0R_l
zF8Rhvc?$!ucTD|@QKY`)8gx~AfEXj7%Fg3#rJn0-S}8?N^cvZHI^(;3iX0~O$j)*R
zox?3Fd;Us(Zgmd%Wa9MZnG2XcBOTuS{VRoOS1uhh?ZX91`qLK&u-CzxZQ}`xPzM4$
zIMD>Lh|Hf6Y5Nk0g~9wO=tDYz1i|ND_po!R^U0BJfxwcf5JI8}#v#ES+yZ@opR2vM
zBP1kGb^dc`BwnhpVd}!bJ^%XF(BhSg54$(~G%uik6Y@pF49CAMYIbpjXQI;V{iNuX
z@Ug;xU?R|daXV@p8R)-hUcxe}0*6>aZqr;gD`g8lIT2aF+li<&r3LFzgaEMu0EB3Y
z8j68=Xt(JLjyg|NTl7O{nUqUo7>jeBF6%8l$);jK=Qx_rGdsD1q9DqRP=ir?a|r#r
zd&^humx3B2zqj^!dO>xUUn!gJvQvMtno%OMKxbL1GFE%Ufp68uK{8PN-xqiiL}L}E
zOjY+O@fg!{D#ZiSW0(+2F(WQ<5r|9GUmkEbc||57Rw?%jS93NjTHo1=?P8i*oZi|h
z2x}cvWi@0;UA3CH665nusBmf=_VV>hKhI51sL3?aOrtmJ%O6@ewZ{9lOxT~!2k3vu
zUYoMwPFAsPsnI|<C6>N&DzXUO#-q88dRcJEg4DNR6o<Kh9i4)Eg!u=l>OzvD7|wcP
zHYi<UcsFsQpy{7OCjvQo<FRY$YmO4JNpfb|hyjnO+@I(#r1}e2le=HddH*~YRMw{1
zmN9!n<N13T02J|iCdF$-R`Bq~>+3vW8H`AiaoKwj`i--4sq2eT%>&_P=~JD=)q2DA
zmZeaEP&+W3PK%InJqzEbnW{2IQ4_C=S91rjaih{Defr4@mnz0S_|`q)HBP%5-+Zog
zU;oz%UxM7T58^>4ZFB-tRm><qzcG+TM9M>+^Uex)gr<Z?Ar3-Hea}wDEaoPCEGjoM
z<|`myX`jXdL8QE79fbs<%772;C>nBg0%kv=Hx8L}sTAdc?9;Cm*yi{7t0oTFFp{jq
zFTDDYKx_SSLy-mK-2nXl7^%jiP)(k<!mxV%c6!VG_K5yoCff%DK|@a(amna$Jt3OY
z|E1^H#`)Hh-i-%3j~jPT?zG)F0k&hY7XvTvD)X?>)JUJV!|E$ukH`<I_$znd9uKr-
z1p4``q(cD!o=;J#KM^2vuy)2}uP1$s`*!fcGP467dWl`obCGR5(ny6wWEkqt++(aF
zXwaDFAt7q9cS&GllkmI(NEduZSR21*xoba_*gO;`N=7~}yyTiZqRHKd2v11UfAfyD
zoNMiyp0t+$mRSo&?&$ok^;ZVDTgQQzbAG-2+~hk2|2_gk=A9*yOw<Hv4#o{7-LAv9
ztytQ%p4nxv@xz9iW3hBvbR%iz+8_ZjT*e+-fiN}gPaJ&wel)qfAoVwA*QMOm?k^}A
z#z+0C<=0)c5tj}oh7`P$6fYE%+P;1x@uT#{+;#ohvM?ZyKUO@^=P~2<yjr!VW6i%W
zwRt=@Ll?Yd+zOPiC+lB=(%piz!wVVG@6_LU7!o<{QlYbq<0Vh_i(4|)vrkX#x+qq;
z=p~<xx*DzNXsq;8V|Ga)8N**Z{pTNB_q79kwo+dSV}7&|bfMJyR@utp_s9?^L+~|u
zf<&^lxU-*}fB{bIS4VlRgcAfTTXCj7(jXIU*(*F(Zo7A__<kLm@@64A13s5s_OIRs
zwD|FWh7rs5uON}xLY@n-GpbK4x~WVad!_TixBoV((950dDw;&@=3sN7?YBbe>13Aj
z@o?-%XA-;Y%{U1fu*r9N3T)7;;~Yste%YD<0MR^@vdhPZ*Y<oeA5`psq2FCh!qOsW
z$Q!PWlNW1sXHHt_+37^*gaps+r`uAm!OP0)H*tOOexC09>%J`PAN}HQ-Q3nHovQ3U
zD%z-=;9HCGr{3mHt33p_itI8Hvpwp)@#bbL63}XA@)FKR_ogw-1}GQHlPEzC17WpD
zs}76c`gmMk_PcCt)}mqd$k68aeM0<Y!?cdX2;QD##fN^yP&_`rTITK%P(W7UKHhjp
zAetTwz`ggt3G6mo&{5Y=!0+!AArFLQcrE`nGQ_kmJSA`?%KVjUWv_zDkE{Ev;Z}46
zYw_imSN;!u$!=pF{^ZT2G}M@#M(`*)gB)rS%lTnKYF)5&y>apbI(JV`TW>Z0+J#uk
zh1qX)yABEYOh>$*%0OMBO+nZ7J(W@BVKpbF(RfI{w_G>$4D{UNOuQ?+0CE)yk9-E+
z?BzP}>3t*<Uf}W5VX-P}!j$#FqD0niuEh%2Ykk(zbxC0v55EZ4^U*#_)mrSH*!TKd
z{aXb5k8~ev-SWn^pa&cG_6KDG8LGd|tSRdeDSqu;%d@#Nsxq7PF8w5qV`lm`tLCfJ
zu?5w8)kVcJhEi$;9_V(6`UtBUqMP7A@s*m`Jv%M+wAt3Lt}l0hGn3eOWq`}EhrA@@
zl)l1*r{f@h{~lGM-ttbp;lX9@g9mPW)atc@34^+s4P|y2T9xe-OmNd%1hR}Kom<@p
z8PO=ndRo7S3xsdRIJaSb^#}N$Ly7h|^$mhiaH;?Ald%!P6UR)yIa-(>{B!6-&E7?N
zbVHQI$Xt`8UAFmDI{?Q!-5b(ggf@2ueWdI!xQz6qjldoE1yOyB$gM4{4{fiZj%l+=
zF>h1##s$&~a>d~nCi)7C_i9cTIQGp7;%?C51`R4|urFBCvF3Nkt2t)rREsBvCzCa_
zW-2Rn^=F;PR)2Y&ZNJ!j=DjjEqVNvE0dwia)aRw(vfJDlb7&Ok0Wr#fIH0NAB?f$%
z5^$a6IWq8=;-xZO3f?mwu66TLKHs6#kE7h-#5q#j1~4I@t4MDXgaEmx>NHU6vN73Y
z{!{<`+UwKcRW4o^dFgJ|yZ$gW2^DOP#9k-YA+I*2;{rH8*)bh{DMgRz>#L0mZd!u}
z;}X2rjLXH*?WbI~27?<kr6;(vjAND#$w`W9mdNy%2}B>k>gJYxT9(^TFrBgC7|u9W
z)$ITy`fLU^i1KSEoKY?L-C;fzlw0l%Z}nG(k^}ctVj)-aqR4R7$w`)GuHNrsih74W
z$iKp9wpg{;Qh7i^GV${$d$KfDX<-15n4<#z4OHFIM=7gn`AHp?F(D8Vf$4ZX%6y~~
zLv6EVYtJTs{N35ze&L@VV~(=<{)Qiy4AU@<xXBv^8M&D6oa!g4i55c(=}um+Q+S_J
zjGl*=zkk|Ou{Eil!ge4KeZiaU5S?UuTh86|jIv?t$c89AGc(H7E25eQ4ARm`uA4Gu
z(nj*HX(xfVKspJ2YN(C#ZIdT<p*wrJh^*D4c&~uxt*_Z&UmvZXxi??~m&>A0UQIHw
zS;}`C)w^nn>y$7A>E#LxEe+-zgaHT|Fl!|TirqvKDO1uwTpDFw`UV-7;#S#lY$yUo
zG|P32;ZbKP^WrZ~yQR$$XQZT#zpzuYV`o|H=l)P!Z=4HIIPLD%C`8M-wFmth4;HRm
z={)V`?&31$dOad!GdaN6*Y;hS#))5b^qYUa_4__e7`iH0ky0vLGvQc5sAd*X=BSrx
z3C0ZR4l)?D5};NSEG#WeN?@BIXP|~qI%!QdB89+GdLs~P0aw|ntIo_nNrTo5%aki|
z`}L1;-~B0?#9YRi?Ky5G!M|wg4k_9>ollnbIN7JRWn7eaRNE)#UIFiRUO&_@fhlGT
zHb(`gEOeg=yWrJzUOwQ9S0{m0Bh;I5#B>=o-bOl<7tONT5k!fQ1O?Mv8f$s*mDhjr
z@c!TU<Da%vn;O0WJw#E<w|-A7wlCiM(>(J(yyRbIi+?%jNERuB(rq(m<2X$8qir<G
z&0<teV`%$V|C0Owepa?5;M^ZiA${9_cRYJ>PTFzouad8v{|M6kuO_F(vky*+{zqD7
z8unyY;=i6=XZ+8J$N$AF-LD}x%V;-e)IPH=>KuE2A*8>rRMb%A0QHemMpf;YB6{KA
zAh{;?Vn+(q7|>*Wj{Frx+wz_kxnl~?Ozes>r|_32>}C8>TG^}-^a67PjfxrC82dtg
zl6tQuag{ZV2&kTNeP-HP+JAqFpsTm~Tk^&7TLRNtCE}8q1+FJdwCD&$r~?O;2uU5O
zk5Y}X(0zxPS&{TUp0VG<u%urZWI3T=PcG}%iB!nT3l+J<NhLF~hzEH=C>-<Za#W`d
z3mbm@abe?p>UOk$oZ8r%M;Enb>?_|sg+3OgHpsp&ehG;PLvxl;F*{(%6Uy(L8Jrr^
zJjcJBtq(UapJ5oNzpHHba|3no{_fEROYoLx?jk!mOFBtXg)(l#1h>=-%#GE@i=T(!
zFKFZ_ctghq1ZyFiy#-GMA<6g!DE=YOHeHuMlntf$9;I%GHj8d#wAd=Y-8lMud+)EG
zW3zs*xj0^nbG30=!G>y@-oRd)bhnZn)gPpGC7d3$kEsyCx`KZbX2p2#eYopKAFQsU
zFrfetN8_fHcpMTidIm&t9bkE2Kjs`(Yg&S}L@=e_Pt8^@@ZH!<B?@H$+0;J7RTMjz
zE_YVzPm~TpQW!F*Bw8^eu18*ZU4NAm@j2*B9_dJ>d=^h%-QCrr`o$d2rjyVSRj~VF
zYA$JYvb&5hp;UhLkD--c+s)qDnQvq$#ZnLmWRzni(2aK+Zz#(_g`_2#s8b>5gpgiX
zpo&{bEf8#|4?^U9qh?|X7{K6|UV8pu{$lGnawf?N*#(j3)mQJCN4|5b2|C?T^d&g*
ziEEcmlwWGoaI&A0?DcnqK9w?aWu4b4m1R-Gnh!@mpMHPgYScYYyVdokSE|wj&4XkD
z+#yT#=!Fw29mhV-)q3hut!0eWZj4l38%SUwMp{$6u@{gUw}PQ-PFfi5!X{Hxu2#$@
zQKLFnSK<^RMPNc-Zpqt?bIHEZ=-3M{ljBe7pwW44zCz!=Vf%woL$yZ_v|2hJ(s7Ew
z(Ok{9o8A2xdNdg38zveWF6c47)`~82I)Avgnq@Fnqa4v-VZl$4lnoE2V79t(4LaG}
zr>hjKl=R+4xox{}@<hx}A)lR#nS({x;;-)E`(Nc23PnOxxvpGvG;&j8wzeVC`c;4!
zv3#scZ_XuIYyFwWRi?c-SYH;Cw_+f0EJtlU1R1z}_3e=JFAs#{(%QEmxB^%nKLJVN
zNM8p5QT0*&`#WnmoP#41Lh)wKWGBj>+ofFcl>YMF>6umfR-pbhIoDpXw4m35*gSw~
zgYMm+k;?FvaVkw3*J8_e2|*2))}_@rO$=j2yO;veYBlJ3$Nu1VWoW%jS9gG3)`B5+
zq|GC)v*F~|V{a<ntuvo6pET;8<l_AKA@nnPrO@6+&V5HUKHB&1+`|qU1RK!K-O#vg
zilC63gk2rDOOu=srodC2Z0PCF5?70?=us3-KN$jqSB!Om0CoHnhhD{DF8*?V)6m-m
zvfJrTpGI!J9335PeRbnESia44l3F7@IQP%|XF13}jvtaPpa|el2G|(<u2N+byE^)?
za;|em$`1ElsXQFQTN!_+wNkNH88Q=p@t~cY(R{5g+IdjF#m?HzpdhauL*A#z#yH+T
zS>yhRAIUzU>Hg*$Q?w#tOmn%k>#}WNyPtxY<a#etZDi=hWuw;+FfWrO?$=?|n<m0B
z&08O(qy?m>T%eg`u)nj7kR-O{c5K2IBbE_@qP(J@RNye1#(X$folEqYfwf8j8lr@<
zmGAAPm%<;e)J@dU_L<Eeotu?Y|9FR}4DcT^9&3)d+3IET<m&I~3S}jS$)5p^RR>F?
zzW$7hzv1fH+k>Z2L4E;K4}SF3@sR{u{`=n9;CVP6#7{pJX|N3!WO5#owLAju27Jk-
zbuO4ms>(Lp=4-GdH2am(5@3q0PxSl_1^VE#KvtTBe4m&3|I%s>G_fVM*P!#qM+`C{
z-&OjOu$rPT;Ni%9qb)TCjH#~9EzDf(3$#-aLZPGXt1<J_7K!x+12J|Mq9LpiH@KCA
zR(FFFE<Y@ygO$QJZB&lfCTu8|cIbSd>1fufwUf@MpPqR<64KW^wREiz;U#8N<l)@+
zy^qlmVTK5X3-RN2ty(~GpsFp2?X?7V{9a4%3;Z!LI7Rj0!^#f|>$0-n&BaU%efYfX
zInvfF|6656b>$#O{{&UO%`(v|*(OvpQ~#esqi24->mF`26FurmJ0#Iv(iB>N>z6*^
zTFf$;tdH!r))umNE01w0S*qI37p&BHj!C#umL;w){cV1Mb}Y^$uryR)^u9Y$!{QoT
zB&)P_$XU{oYRReBm?+>Xo4ZhBKPDO}@UNi6_|x`Jw(E3TbVvPRk-^mE?WHc@glkFI
z1)XG<Ky7ZZ#F)$Yr|bIfy(Gi=*sqPFm+kd@=~*+^qK$efZtfVwB`AgHD`m&l7BlyE
z2N-)%ZHmBZs&IWpX`^WAHDeIR%~BAY<9gPl3_oo8X2VuS9$di2g()6ZRYR0IJaVYT
zWaUhG{#7HA)AYN3Y5iV7;(W={JkJR`{3JPq2_;NgU67ETuNwO5oT4sq#W`y*yVEzs
z7;&}1l-20$|C!p)1VAYJZM^yCoaDgU6jiaRp@K+&7Svg7AknpA@htU%^)Q-C7ufN%
z$zAXqdvR%|3<w6tt5v_N7|qt!9M&c-WS30P-k3`G<95&AOdnco{XD*qb!6}Gc86Cq
zHs)ndaKN<MD*_sOSGO>HqqRBkh1&XG(P)G)BoWN?H54U$DS&bl{d};pAu=z=BaY`l
zvOc;IZOhwjN1aX7PEH?~hXHsxwQjyzR$%Y~dJ_l%*L&Pt4&7-e_2Cbud{-%EiMWtT
zv4_9v`cK2^-O=S8XMt{*v!i3y*auxwvpTNJ1HU&Gf9jr|h`F>pb*$b6nx&x4I5!iy
zCD>m2m@+08G&0mQOf`9#GUKSbzjDC}fWu+CI^f9P1=73<a~ilP$nS3m74i~Zsm*0L
zlBqyK0&9~mrG_XYrk*(HG$~GyJ=g2I4t*@ZI67^`d+i#yYm70nvrJ#7cqpbtE0})M
zshA49m0g|Dc`e%893m>}+Z$5~+MCEck4bS2T|uu1l4m(?a;q83nGijJY;F0a9;m3X
zVA)*<RE@Wd3^AU$I|PTR_0KsuB@ygI{=;_2dW36De^{OQ7P8dPq`UZBF|+5?YYnuo
zCU+=p)H>uhGQD+QYWdAp3jC7{f_L_V7XF;xSpMxC%ahR%olG2h(+=5ee->`~oGHiS
z__yp?cO12kvV^lft^ljZcniO0^ab4HMB;kEk!-WV6{wyd&?<^mr>HL->|wS~DW7~E
zE-ap=y58J4yh5*p4)NQbw5%pPn!01d@3%e}dUCV6!dW~mBA$i8<<vGHHm9nR<RTxW
z>JX)h?FsOhQg*g!o&XYP-c5)Z!$->)TapeNgBuGAzOeNeN9k@0?I6uoO-qSnq<%=Y
z9$mCR2F-F|$B6!TgLdOTgUY7t#r!AE_3KAV4#p@)2=V6MwjXJ9-c>K~eKLQma)7fF
zpF7S=;Cr|<tDNbOJae<*o*BGAbvY^`Q4PlG>asw~hQ_9#0^NE^Bx5iSd4VZoNMDin
zx3ZK@g4?E-Tf>0>QNi$>nS>caxA84uU^PC4uuJPwY_$-J37fXX>hKjL(D&?sb0<OQ
z;#v!hHM3>ijw@$0v#Rsjg6<?O{Ay5qv5Vg#@%pR!>(_JlLlgb)0=}>S`KpE4+$LmW
zH`1rt8^F_N!ndbD$HN7AZ>lf01NvAXlu23&m7C%Xt&`o(fohX&(-CkA)8s{PTBTx{
zyFtnNUQ6#p-n*$TZ}8i}z-7(CuMSbU;~pqCuz$a)!^mh$(TKh8mb=@LB-f60?<+;0
zH)_)W=oP^2TD9&#x&xPONzakRWtb6tyN-t<smhWITgq!)1|0wDf&u(YCpVedDL7VD
z?-yxo#{$9TglThjKuk8JY0o(Fv0a~j{3#JjLY1m7K{NTTrhNV8QdbVeR<ac8;F_pK
zufE+`4|h?LFtD^qQdGiPzk)%oGV#o0=0#MBzqR;ry^E82MDNFqEJP4FrhGiD;HVX1
zf15qPqHz)np0YX^^B!S~$O5@7+(cxpE#3?fWEG+hi5p!Cup(Wys_`K_NK%UY*qZYB
zWaq204<8t%$lnN3(uY6cTLrw?alL`S<xD$9>{+yxLHdfQa=2e{*^~!Bpn1F%b(Qbj
z%rtOaT3W?$bhfF*`?~lpth|}WzaovlTC6DE;oq8v%N4ig376PfMd{NjwPU|rRL*mC
zyz=eI9j%1^9Ww%F4R{<#POWB0Z%$Pg_jD~AqYVQ+1C}!^+RU+jek*240$~XS1Bh;C
z_daOGjS^r&Hd3h=5N+I5wf8E~=!<iV@kShV=0wlAw!$y-wu9%o`L4g@jPEn<FZ<LI
z)cSS<rZ3?pu|)6#e)P@oR>$5e?|BwADJ<j;k&P@^-NQpdS@5Lc?C~U;T{~(kM)&}p
z^teFP9BDl!xSd+J=1&)xRcp)&%<X4u1KGIx_;~vDEvw1*JyLH5xZ~Q*2I(kynhG;G
z2*Wp|n)I;s`b3c<JC<5a*^}tkb5G=qL@~sU`-Dlbq(fU+mq+%{`qwL@iSkP(ZtX50
z%G_AC=IRnCO^BH9W~uY&Ul3_YPu(zk8aIQ985+bdcH`q)w{woelEO<91dy}&6kl@>
ze+QObeQ10AFN+U1UbHpWkP?L{2QryWl@&wZ-G;X8g?}b%b{a}7RtEo3^};ND6E#M<
zwgcVE@C(0ed*0L+!|stXHM0tIT4s9*9O*v986&%B$X9xfQPt?D#+XH_wP#OL%jfh{
zdY*ga0;%Y|b=ERTWUx*TJlMX=f7o!pFO~a<;q*Cm`HQ*uqCHEq;c?DM%upltL}T!m
zhslpN3<9<94hH6?8*J{_{ThB7v=xO@C6+wHv+<Upg2-x!4n~5AR;jLUrp&H%?NtI_
z+;(fPu6Bbzs&xyy9oUW0p;o&&%q20U4C5cdn9}8)#LHP(!yDr7)n~f&7CKDHVFS81
zBK)ExXmy-%Zh?Mnqw!BKtXxiu-3_krN7*?4Ir{<tP(40?!lSUfQ?_LDZB=M6#Uc3t
z3&MaPCH=%UMPQQX0BFt7N5k_o69(y|v_42M(T%3Y>SswK@<j3oBEfCSXhOVM`{3wq
zvD*OF6kHI%zyH03Bq&Iee;zDFa30nDFf>%bbSL<01u|aNbuI5?BsHr0B=spj|FObS
z`&j(jEr;^i#k{ZCt5>Bl!H+97jT{)s&Fvm%DFw)RDj3d$Cg9jq4E)V9m5{eWw}`!Q
z#!ziyiv;=G&D5Q7jE(r_b!>pLSoi!uDze_p!nJ=nj3W=Iwa)Lsm3HK~^U{Tt=Nd;=
zI&G(qp#uIaY3vABPP)>eb^pUF)1u?V*!oj4zD}!~cQ3(VjyUI}a&^OB$Eq)2oC*Si
z(2*v=aofr!R3bapQ_A7Wqd{K6LQBIF#3cwZif@~x!vKlpvxE4?cA|PQc)mRQ(S*s6
z>8103c{ZE;7H|O^p!{K7AoSq>T5*o|D!vi62`&B&Z`ul0eC>1d#l$aXP3v187N33o
z!i*uq-rmsEq+*;CpW$oR*w!;dt=Yab<ar&~mRj!%IdP3&13!PU6+a&q1`WmB&YwsR
z38uKo!cu(Ccge7n<-gni6j}PuAy{u1)!L~~9(yLpYWjWdcyq?R1qcY+v{aUP^|!lM
zra^UElxu(llU$tZ$RPD0D#Xjz5{TOD4q-SV<=kp}g;ftV9Uz(x!k1wen6kw0o^{9I
zS-s7c6=aVqOn8Q>E4Z~hC>}vyFbA=^_2c5}_fzr)Z=U~NTYq}^Ykn?yBC5Rj*hx_D
zo9NNZFL7O1t%^#6v=&VP&-V8L<tNjM$D(}+hhH{($zDw~0jRa&kHc%Ac2i&4#Kl<3
z=c>Q-)7g!2edc{+s={5Sqm(uo?l2!TQS8Bm_o*-G3d=9|&jqCyq|v9R{GqUXcDrTp
zo^7Lv!Vg6gWY(*SKJB-zH-btVN0%J~1Rz$$P)}F(a>b1FlgXaU_SE}v-Ya9#z#jep
z$uoKaySF)?)zGDG{dy<dX*tBwyxX*Xamao=6*y$K5>ZsY)ljF-;|V<#ch`is2I?=3
zFY#MhML>+nRJpr>En;|Cw4%N24z_Q#wZG-I3ufhLm#Mbixv3!EbhLzPxUJFd7v>YZ
zF{qmxe5RQPha$#pqRm}eQz$fOuuxCZC)164M<6p$=QIjyEabq?)0S1Qw}2bzQ`-LD
zlu_AA)N4bWE=iiI?29WbyS<)%#;*5_D}Y(`@y87&x`*@3chH5aN<HAGza6oSV4NR1
zcTGWOV&azz=QjebwCGuaBwG6{zw-fmMq8jq*?M<HR;?mn4?-NYK6+Hlt~(gV05l>o
zs>bi(i5duo^mRx->sZxZ`<}5f0HXEaal*H3Lrv@BZ&PNZ_l#f}rpzL9ncbu&&rFBD
zuusd|Sm<2e*6j+cj&Yb+UU|i%L4usH`dqqjIGF#hO3@@+A6ug%Gs_u{#Tq)UK{7>X
zlM?802a*wJBL(p}mu4_FZ`9~Ec&P&zGB@i$nUVqJg+O6Rk1=%r#$=Ydq5n{U6CVVy
z1ky<^*SA2S!_BPabVT;a7);C^(>pa2&nm>ux|}%s#zUt5U&FQ!ybZJ0UnT6_8$6TT
zP;%_Liv8PHdo}Ypd)erfx;0EbYjcnM(K-9xOkA}3^u4)kT|x@N?c!)nJ3<)OHWn&!
z$TBB(3N$RZxJu<=19`18Zbd+N3d0bsmDc2WWF^LUTCV5Jx7L6FQuear5q;-E`2%Ft
zARYf$;cB_u-_u!^Uu0Fc9$f{G+{`a7=r{}u0ojg>SB70T?$%4M1VajEBCA?@nF)*!
z^#w9KrYfASUL4lg7aq9D&N+USr5P0jO$3I1VMwWk-1V8)E(BY*)l0^;r)MAgTHtOC
zsaTIJ>Hm!tWa@D?X|nlel_VRQc+quMVUUrNnAwNE!J=FVjU*+KpeRXne|fq=<lDCc
zsVsm?%fth4%NA}6dK$5?XWa#r%#rSM@Z8x9v7OE~%Oft2Rr6Due9wo0JIKKh)6D{X
zK*wVC*-sveWbAl4K5RcNf6UcRinB(@Q1GSB_$JLPi+GHKTZulKZV73Dt-qgjOM_zu
z=(+vL@0=Xrc6miy0@Tn>KbjeeUw}x?%M7KON~RIDW;%-tqtZ)i`j(K@TSuQa+@sn~
zlSSl(MA$_Iml}7AWdl3EH|2%L<3@e+g9bE%!mub!kajs*$Xsq){`k!<Ou_Zf4Yb<(
zkZuaR07$%7ouRLnuVI;DtWY~ZdtM;c!3QRt+V%q#uiaYc4*dq;Vf#}sE&Sg6ac9se
zNO*8E#B?mH8^MG*kWgV#vcawj;_-gRekjtHP=}k%&-Ht4C}+CVW}RFAw7!=ZS#<ec
z)i@h-cjYD2uIln7u`S&>u{b-XakPRr1fJ4Q%pIL$${4i(qO~I?pypfiZX2-FcO4N3
z<*)axH2gF?%h3FxG%igYu+(wBgDr>%kH6P56n8N)sJZ5N=fRc1m9Op8yDyF{hxX>7
z{&)pA+zfYLG{m`eci!3r$6$rl$4B|DZ^^dF6q?eQH`*8ikEn4fz5S?EkbsSk>ZVch
z#-UJ~8;_{kUb3NYR-ja)eh9yvxzRQ0g6f4FKU0crg-19ZT>6rwICQ5%{<lHR&=Va2
z46<Ks{|iP?NRZ$bK0rw<irFQt1D2fj*UEi`I`lLYXTwaJaDdz4)Y^@>UvZThX?A*3
ze!hk7U!fmXp+Mb)v^k7vDRc}E)d}p<S5^Z!p!vL*UJO{b-D_8lgK{YsR1`4ZPgjRc
zuI$vgt9a$<x4%8$`5f>LhWGx!FURd^6)Ht{j_Z<C#<l^`YB1VUx3_t079hGij?Z}5
zUzVz>%pRJ1LFhBc&|?6Muw9PGa$yNgs$;c^Eu}W_)UahmE;P!G%2*yl=jHrcKosfs
zPdu&;Z*z3*xf@5a_j8yY%KHMr!Lywc!KS*L<};(NBa8gIe3a$;9A^PRC|GH>)f~$Z
z)9n1VaD0115=n#TrXcDTM49@*CTN&a^<9CQBXQ+;=x6pwD=>StxsL=Kzyvvl&Q`~M
z>Q6KoSZY0!JeRh!-~e@?WNUMm)N10wHd2Ky4ZGdFV@nzJ3bpTIuZDh`gso2siH@a#
zujzFkIjdj8pMtBiyMOkpa);3khP*}ylO1soqPM=|=~^kILX_*MRlb^NC8?^cmJ<ct
zc!lB)DIPVhf@Zs$^wC)?Wo<v>4$`<ohHk31*|ojCl)JPpfKhpz^L1!0YXQY<>^XbF
z{Tx=@MO5tU|J;f-ab;~kFS@^asmd0nZhg0=u6AhixdvxK1#2mgjAAL_)*MrZi(RGz
z5JhXp9OuFm<w96ISQ=F%ZKP-nLY)VgNM)m~kSY(4aCypuPY<09st09>^G8<G@W3z;
zcmy7wkg8j-ye)G35r}Fh;%=MJ?6K#d?=VeXYA7h|*b4ru2x(GAb0+LUut8F+EcImD
z6)Egc<eTS$iqo}XMg-fizkL0d$Z5_GYqxV*{lcO`Z@Vh!g}!;BH?rEGy4VOBOtpdF
zpdooIIO83Ta!IyS^!8!2G}*0>z}1W_@16>bIDf3)jg4VIxb=EJOg0K8C;ZKiBzJxM
z;bq1dVyb`o1eW47o^S;}tIcjUh;aN^$043tIQsN!)Y%<;kXrgp*RboGLpib?J4P{9
z8aLhf;ZKzB&fC8ZsqPv?s>=I2B$NF6!!G?g4*?bnUHTb3cF#%I<1<qJ9-+!0DPyw<
zo6P}OOgYA=kw9Po#vQlCcdg5@5!0%C<vx8)i>A}6{P}9KopNDuxvSM_XLsuhvh?98
ztHRPF=VfE=_q!~V#(n0C8#VEGTy~L@+}>eLABH)`=sCbTJ0mLr&AtLpFr(ggEjWy#
z-`+6aq(QRlqCwpBZj!MER6bjy8oRs!XBl^&pmsSVF5bpAjf^(&*M2C?xXIQ}p2X|%
zsp8AGLt=%gS1=ZQx%$4VN55N)7<^OR4`2C#=-(g1v@&Z2Y9>KbLRbola=+dJt-|Qi
z^bXLJ(+Uq}$_5Z%l(C6}J}rRdxra9h;EvY;>YH_ECgDo||ALI-Ph*hN@n?Qq+&E~M
zzUFyqBJ!!t@b<ohQJ}_}-WQ1NUHvnrK~jUAI@{IH3V@{hZ@V4;{KWcqva}%nBZ(tg
zSa9!^XqXZA-}zI_<hF7^{H_c=eiQred@8weUko*Qa0IRJBlF+c(|e-REgdWWIrJA<
zl<8FY_&)-w|3@zKlR&mODQq$|&h+Egp^w@z#?0MlyFrd1eRI0iQ^$1)f7#}h#L%>p
zv@IZ6SIy(G@$aX_`>{H@Cu{v5veow0P9_ihN80bd9{2gvmS`(O!#C;Lh<^@wyB8ni
z%1i#wq16A*oF`+8i@U6tlQfT|E}80s?n;8!u{%#dNM*%|8^2C`f(x8~zg9uHGp`lU
zNRp&N$^o!E`k1$9S{)u3vk6N}F`k!?QnPC{#7Lm6`UFWN2!|ay1ux_~5ttAO7OYjX
z1p+yfmf}Y1TrMgOY4zE#eo=o>d?oclM-JwpA<?jj=7PDhs2;6LHz0@69o#3Z@?X!B
z?w%(Y{Qc8US$#&cQJUNrsh{2rWG3ScrKC*;quzXF0(g3WMxr_*lk~78Rfhy2vb;nx
zj0u;y+V3_tC`n<;A_i3BTw0GKnZOs6yEG7m!J84arsX+MYHYlaT)7^X8NrTx{Kv+T
z*TM7-AFa@x)LT`x=Z3fB0;0cIrL)4YVS$0jH<4H^zX6FqbiFHV$dK}V3R*PDhVH+~
zxd23j(_}($3n-|l%`qLIj1id%-p_=NxogVm*MSa&3>Bj9ai_3q6czw`B*Gfa9dp$5
zBScc{<gR~qNev2<b#ZvRV5jEoUn_y}B|9xgXvXE04d^#RDrM2g8U?>Bqd-ReBkSbO
zQZZ9;72?#2pX_bI1T1rCfXoEG6s9{txLz<A1u+F@827=21uq-nvw2J0J|T{EIOt(;
zLXIr^>V#Gj3mO(%Ty~pj;XkPagviRP?0!3+$oa|vxt~YXl&&+z94kZkMVV?Ok6)|E
z(?i-T%2iqF&a=<#&l4&+N{`e7Y@eS)re<jxeWcV4dNjO!@xsmmyR2xyVpUs)725%0
zzAEag4KR<&xJGGNJBsCf`6ygN$`x5V-mV?<KK_bM1+i;_L&sEQAo_sn=}6j);NbO}
zj^d`Nm1wAQ(Iv~BiW>AL=iXJ~fKquZuG{gi!OnXlzV41gp1XVf-uyG$DXy%iG?h1t
z!wgUade6AKTsE$qcM4pSREqe27<;d%CeSZ#*Z<5oW3O~6BM?exQbLn5pac^pffPcA
zK}1MmK$?KG83j>_NJwA|0fZ1j5{g1XFC)z%2uMPaE+rr(RHcY8C+GH@wZ8S;<RWY3
zA}eLT@7}-tJev|2mGiP;CYlYX;!P$1p7-!|6B#I}Wmi06<Ogr69}M{SzhjZB(aVU^
z&qE~tpX%d^sB!AEzjO?37!8jRvU5xKGMoo5VT)e#JO8G7j7Q&|UA7<8)7C-C<OeFZ
zdp;{BMh(l*JCgJlcB%LFeHEj-wMy0Y{lY8FpusN%i7IBjD7Ho?D-2J!-4sKuwh$?i
zyTxCtsyN7$AXIL;>A{;Y6ccW^Kj(1B`g#`g#Rog~b2amG<hOexHQ6uR+{Txk=y`)R
zyDlzaUMnGPI!%qJ)#1?7F!Z(i1b~*tpJ_BWh&AgkBS_L#1fby?j2n_?Znj3z`hi(O
zn%qEC#C3k@>`Xu0g?tj72Q(6Y)$7U(6I(P#+WQiHs%u`9<hcoQ**!{F7zn}Q3^v<$
zDk)SN#i4!|7@a?dJ5uM2gm-gIgI3iCK3!44BVONN;V7|*NsZ}3J>lxMvTM>?6Q#SW
zd71-xILX^{K!f`aJ4*!B)ySakDf4po2c9)q3yE_s=j|0B2VDA3Suj(UxbK(#rPdHV
z4)Hc#SfJi1l*|9`SnyQ}D086jt`pRX|L?7Cj7DE>PT|=5dL^xkk3xw~V!9;}MhC=P
zkMQh<_D6&lgy$#O)!RWBQvBG85pPE_H=?>4UyVog9s#1J=Gq*Kml|$&s?lb3+|Qby
zH@z$eR?gqq!8k{arntII%KYY=`}o8kPOkv~5wI(DFB*u>_2|koUCVJX=hJRfs+5a7
z{pCU(A&gU?kc(5NC@Dcb`CMH1?df>Oz~wqE^Gt?@rcKY7ec!!UyUo*G+GD#()9tT6
zKl$e2dQb0D>w%c@F=1IYl2Q6LZ2p~=5whp(pZs}@jeIEXBIo%BA$X?{)7+dFVOu@V
zM{j&3X{lC5U^t&a;fHA&Lu~xW6Qx{9kkTge13%mg=aB2#*K7g|rVNKKwN>FA2|&)$
zww^z5r*KK`eELG&rWNzkp^cy0z4EfJTI$cdVtcN%nopE9z9d&%ZQ|5Tb-y`t+5P)p
z9ce@GubO?q5F~qbzOn7Tv&LY}ChAb6<qhdSi;V3l7)9}m(38e3a<8*8;vXVY*jE4?
zXXs_*gI4Z+$bpI3L;s(Ybgeu7J0=lz!?V6>Q=6hl!~UFAiof&`iw#rQ$qByKv*XB_
znDFdY*qT0XtTBHeMVbn~^tW$)iq@0ISz`_1`XGiP)_2L4&^bmaR~3GB3lv5kHxpFS
zXTV6NZB`}pbd5wD7bk(Wx;0$J6qB-o@|m-jo16<laF9lqp)gzd;tn58UH<stNPRD(
zjStN?GxuLFcr~&2XvioRpX(Q_kol*V+wZ$Bk+z%3W1<e}(rjg~s+>2;!XFtIju?(o
zE#`k)IrMx=4-R}I-gKFx@ixR2{}Oiy&DnuFM_<c~PTZ(wtRXs$i1A;T#!H}Rbx9Wr
zsGM%@S&<c)BZjbf8Fo%tQ+#I*ax=~+5LO=EUT(O@^ea8>AT!fg_2g)3&9IcykY#uF
z3jrT;hT>FDmT?FUOgr_3SD;e)By@K}P$-KQa&W-P3k5ogcz%eOpwRrVc4xq6OW5!c
zNhv=S)hSgvOh-B;jeyKkE|3AhY|fks1{h@Y$0sJwA#W%RySAr3YfPW{2x?q^)==?S
zD(jW^jX+A7@m63>VcIpLS>HI;GUCsy3Hy7*Lzd<Cv)lSVI|sa@P-t*A)|?FNI{z}1
zXCz$Z!0cEl&Dy^Mu5_EWT~6JlxR=6)$#VFYV3N9O(SD&Z0|QxJmrM@V(o`intLE-{
z)nv`~CU+dj3T6}=XUhGYI8k=J|1@SGYu`Mvo$m$J6oe9?t8f0j`q$bj##Z5oU!<II
z8Ob>AH6{_Piz7OcHEg=+Ww$5yRkZr7`YuNhtn8q4yLY*x-<{=lf2F`3Df+?K>vj$l
zo?brGXx*`3tUYxgUTZJeW22cYVq>M{x@y@Fx-0HJy?x2#LRsyVh(OWt{xP|e1VS<8
z3aHgT-%-bJFujZ!<Dxn}R4B0o=g2S0Rz#k22jSTsquanQOSboev(bLuR{YCMaQK27
z()C&Ic2{aiR#IUvkQ;~y!F@IJPcHXHFRPbBoXBLoz~=mTu6V)0Ju><0>0G~}9(I%i
zk~o5?$HxrT<V)1nHl>Zarq*O@2L0MpoBsU}isrm<`1Jsn^epc>I{y@Ap8mA*lwtZl
zBiiri)xLkviB2fLW82h`IM5zCxDfTX9|A7!m2o085p4I|TJ31x#B8mp^o99BVtXC4
zJ<sg68FSQEI2!+<K;fR%iKW+ZMJsRt7lw1;3W|p+E)BPtAq};wCwo<2|9H0PkJe0i
zrsc$w>#lB!KE~F@+yKX2^u6U!czJ&1DF6Nr^-TjA(%mPq)Fz$9L|!V;IWf$3Y@kY!
zam*NyDQY*oY4QCmb&Q04mwlPE+-ur(E%pu59bG!~<$U6ik<Zf>8Ge%!(PKiv!?C6z
zlJm##(Y@ES-`^-~eSfAIe(KHxuSU1jAMl^`!)ZyCm?W(O(>~Ai(Rx(#5^o6rFg5Ll
z0A$sQIe}aEzbfFIn8wrz7i-;TFf8cq{9KRG?Cf|5wzIJ$+9c%&gtt1kNq?94iXUu&
z2>J%Apksq+<2DL||6?@PHsBk39njSRisgs)ohZ`V@C6swnR-_`QP2F`a;?~(621iK
z)_Wka3@$>DTtHB={O3VZ9LeK;lb6v<KWcy)Ev1McdXJ31?VNtumsJSR;E;UJL5Fjh
z;tfgbrsi7$qfdvl@D;WW3R5G9aMS*xqdSgq(ek@IJwB=Wo*8gvcN3oV4j{e>w<k!7
zM?Hie)<aVGh>#R2GK(CDc$AvD(=syJ;EaWS&!S;KOdg4*4y+AH^hX*zddluaW;w-@
z>FU-GjOfXvLU%GG78p+dMI;xLXIh75A|+TTYV({!Q=IMC6=q>J4>Ajpn@Z5`IW5l1
zKz)gUzh`E>`OfsuxPM(WqFvgQ6-tsI5eZ@7?^V^@A^w9HcCo0z@rI1U7#O5I_dbh5
zF0QD=>Zact0ms_J9Q?M&8b=48y8)<P{;~7*5>PC7OWFI=^{g(0mi`zgF2hGXM;~s@
zZ(B@rS%!?!&g5L>q@G`n1YaHT49>Upn%cGXua4y&AGfED1j}K~&6Vo49*3ApZdA=i
ztJ{vlrzntvXne)-e56bE@cg$ycgNAN1L@jf?17$B^C7$Gddc3u-;L^0?hMQM5spbW
zsBpFq|DIHJ@Kvn%*71Yu&?8T8Z1(WWQ@!`!IGiEEq&D@brFJ}oGv!e=Y4Tj85haMK
zq+`|_<0imBby&nS#UYm=y37#nS`c|*jV9@lX|qgP28heEo}b)vg9Sl$9>m^frd4!S
zy2F9(aQDO4$)CqD(#pJbR0jN307YOOSc~QiR;*<T?sTeUk6pi>_}4tQo?`OX!qi1)
zWyVP@UZTGK^IGz${H`G>D*JFYX+~YHpc!UUIym1}+?+;LEok(bDIO}98%T1in>yNp
zrr#8bH*GsRIejs26S%F(-y`RK-ixHWPeV%Wbygh>dWzq_Nc~~OA}2e0toU0^t}GdI
zA{QmN_dcuVjBn&EEpsIiI$tgSg;V;?F@h=23E?@gkcfl8U<hiV8yFP*iViOG<@$eh
zV}Xo<JlTxqe;14Wdo=C33~*&CKm({jI>eT)iT)OV8WeFHun}i%UCUNQD$$Jm09SFO
zY_N$4Q6b+<myn9QEs4I=aSWz_eppkK6AdZt)LlbjRk?&>u3wrzf)y?Z4eACw7#|fs
zwPXL@T+D0ok&ODKBebG9p?Eqf^tw-tT>#^=cpIe7#01RiYCHY7(($JpJ&PBb)Q`mm
z!diQ*tf8IJm7E$HQa6HGX7op)7>8Z?wd6}K?ixd0Rj3ZkO!HI6(A~p7_wsP2M+@|m
z8b}r!!gHadX~bbfp{fhZad!s?l^5n|Cu}*3GA(0D&p(V~zm*@=7cb{4HEoDk(~gYg
z^Tow{chjt%x+lvD$T)Rgz>~nq#lcqxhBv*pS2voF|FWP)uh^Pu^)qmYl?&V%AoDJF
zEL7J`GYlLOnzoO#J?D%?Gp-~8%nnp3&1QqCwI=2z#fv92!l2G7fego-++B`WIp*x<
z(donT?`E4n-pX;=IBZ^^N1rQH%%*syH8>x>A0fWoDV=??;yAeW<u2-8Wr%hqOV#*I
zr)Cpz@<lmbgQqVZ@b%}T(u?Y&<0y$NGqWu_@C7E^a~W<(zU5T1m_mHDnF>cGHcto0
zzz*46#d)S6x!5hZvqT{ltb5XYNaEUm$6Cb9%Xf#jd3|Ty1-8RR0(`j*eH!cjBu_qP
z_tC<ofplOD%xOvV%o~6gSoS{p%b-`Mp;LPk%CAwR63io}l=En7a6|eNwzpxtfDEPv
zK%EMVkjyJXbcDc(?2*$5FKao8Oh5J9aVi<$`FkKwsc|8cH3LHGKPD9kPlc8&_c<{t
zXMHk8q`c*`Ixr8d*m%pl%{Q%&qB!+J{-|o2MQx^9BjN0OJ^9S0fZto1UYw+bU6CMO
zg=nJN+AhKK)yhqQ*~ARP&UdGQ@FjHOq%)F)14d}JIYszz82_?_W-+F>iv_556m=rJ
zxrP0ej==Y5bWsa6ZThH-2?5fw-iX%CfSuIpJxit<nYko4^hM9tm;v~jh(V6qVDZ`r
zuWn#>!r3W8lUOzMd>9gNS=o*LzU@{PkT6Uwwbg;@ra7;Y7N2G@Y%fICf^a>fc95>V
zuj<{lm&+Av9f<|Cq@p4k1}>^_23zrI+1V-Sx;Uz6%l+IrXxS2qxioz~Hnhe1wR!T#
zwJ*^#b(G;*-offLww59d?7muE-^9I_UZweD!Fg<1rtGhQ$Pnl?_DM}!944f>@>CWL
zX|<Kw1QalT7WIfbgHWdsQr(;u4?W@OX{K9TCPI?UKRlT9&U3vkhHXZ5t5K{T9@4G!
z_mXFfX$*VXq}%9HLwdQOdm$<kHKOh&TGK8<!k)h=xq-j8^3Y_$TjtYWMgLnn?P)L9
zY1%;cwK2+b8(^F+DPmwan%)st=AmDI9n=v4u)q+eT2TXVYoOgxHsF8>>Gd+6k+*z!
zG>>c~erw7a$rhlp7^lLr7C_?Ep`Xp3S!^s|Q+b(32zI#R<-QB66KIQczVB@RdWZj2
zA^70oZSCUP{HQMnmMFh}H*aWffj`&U=SZ9K+tsLaII$76&|!)~rNvP~op!du=geO)
zWQr$UPP7%z876LQBFpcN<=v!7ON+1J8Yb`eJ@<bdPgK|Jt_(dthD>X&>?%-jeEhX_
z&#q9MfqV*LkCwxPw58V{9fBjy#8`d6L`TS|aE*^{KLV-v?c^wH{TQi<Htn1o65ln?
z6>tb9J*QX<ots5;%?cbUxr!k>dj;-P2t?%F;zJIkPuj7TgL09W(8ncIj|YfdTpNS)
z;8oF}zcZSI<^6s5Y8M|CbzA9NR5m`>xR!`T4!<9#lY1ndf9D0iTicT1{^=un{dDWB
zMpxQg)QvGn*D_FYF`1Oxq=ErbU)9szi}wN9CxNdQ=wKZONegq5XT8wsmgeZ>%-qv7
z9c&#4MX{)u5a&^lB{Nno$P$ue6;jL(o~dm**P^BP6Si%_m~og^>k~BKc6L8&b@u$~
zmmv=?r=Zq>rd__&N9%QpZ=l`V{&OvLS+pG*(#c_JF&$(EVL0aN!>Q^>-QvPtiK0p*
z-PG~n%5tPLXWl97z#;`_;IbfGsI`bbEU*U5FY=f}_dkC>d*Bik`K|@fxB*B**_XSP
z&#5XA26Vj>-rQ2ji_NokX|8`0TSb6*p9MXiP1MfhWiCP*z5r4SrFGq*mCDswiB!L0
z!HxwlEKz&8WMW-0(6;#1vK|}b>fUSn72uFJY5!95dg=IB9rQxptv;g(`P*DL9bh%r
zo*?^aQ@+6nU$TtJjG~;Je}X-FD;)0)s<OzAw7s%Vm%gH$Nfyo4aju8#yy~<WK8eoJ
zF{VVhyZ-CoEX!woRxP3|ppu6J>zQ4r1iJteSwJFvl~2lYTmjv$dY+Ty4y_KtJh-1T
zv|L(ecKzKH0+|}$gA9!R?^q}7!FpSZ!XPNmcwt2RwJy(}_w*j?j`=}b<^2oGG80N{
zmSI=9JT52XqLxbRr|95b=Rt%ICWQ<h^mPotgzjSbj|m!mbbA@Q7XJr1S_B0nzmqU<
z<2(av2I~eJ%@&Qb&5ZaAm^zZaTCbw_(E8G*NNMgld*=KpuN6hBH~94nQK3<P42fc`
zPy8B3|21DzM5xq@T~c-pqG4|4$2_~65>cCC|HH*`=eKa`{aPySwxXobFgdQ8=9vGz
zDSw)h6lmLrUb_9+JiVR5?1n&#{ejt!(3q=X2gLB^0*mo<;lH1Gpd|<o0(Zp`lRIVP
zHjh3mtHgMu|L9$_`NKpz;auVzBxWVU>ZC3*Y9Seu1ARVm<9H7QFia2f3zSnw7}X5)
zOD$B<);Y_}(xuhr6q;sz{Z<LFW2S7Wo@u_37z{#QyX+G@;oX;at6r`Cuxwe}`I4X>
z(#WU8Se?`~%|0>2;u2BVWO)Ggar^APo)T+Uyll`u(;MU=t1Z$le00<|K+s-$Sy8??
zbocp=1ExMOKMpu6jQGj5!f4#hk8B6vF+VL#P60~Iz=k2He6<QirAA={Z5@1p43f)A
z2k$;%7ljCSsvL4gkrAE%jgq-j2&NXgp?JTr*IgG`Quxa%3*>$YUYXu{FYo+JXLD1F
znjivpX$6(-_{MEH>$5w$)_CBZMEXPGGG&NN4LSOzF~*-=KI?8KQ#Kavp{iw4jsCI-
zg%_KB5<(cqd$b^|6wf0e^c+`KJiaLvLW|P2M$J=>dP6n(vtz5nc50xT3y)Lins5*e
zjzH4G^W1Unlxb1IjF(oMdvJ#XY~-79qv3d4+rf=D7ke+hRD14_=jS2_-;JU_a(#o3
z4q22ibl>0kr{CG-Fu&~D)_=!xNQY%g?Y%HoXsS~I04<bzlcQBaKkMg=q6pPa=U-+k
z?MEU|RF|e1wUV#*7apa|*IJvX)-P>Aam60Jn~Nf*cx~%{f#2+((JxPajH2)y4!|Di
zYCXFbPxQ=i;~Zfr3xD`K>6EE9)dx4d<^*5<*Ks$(TMmdKGT=o{X;k{DZeD8A72r1c
zR>`0WWbm;t$yLBaQq`CY_kx;7W5rDO%_`>4bEZ62P@<0NoGg@;<Nyu#N9LJVCl6X(
z8BTs?wBqIETUX_A5Ex|FJQ45wUO8~sMzX^#7#OvCFWd6noxgI_Wy_ZZ;XrsQ4t|Lg
zEFIN?8JhjZz%zz$ekZ%iR51B(h)6FZ+{%WP$zK1#Z!1dYEZpK=B?Lz~+HyDpt6Z4X
zJJ;$u!VPqGbi@h&Hyko(9JcjR+X+1UEn+ZbBvXB~BphL_qu$4Y*bP&)Lha8(d7Qab
zCK{?!uZzo~7XCcb6V7=MP-q^4Xz%VtB)Pa7mZRL!Uh4MK*_;t|KoHh$FqNxm6;3~B
zy*(0dZC?m6sCZVsEJZ=zZ+R0FAerldg{f+WsD_nyinzk@agTqqCe+9Q@k>{Ze+*ET
zTPFFzi)sCmc5w8Z*Ag_y;C?8~8&N_J<Z+5hZ9SJG{8sS|-f<@8q(aayB03Bmt`dnf
z$uM}r(vJY#ErcEod(Z7)or{*O+kE7>dJ-$UQwok}RxY?<Bir6DPh&q!y{Ea`JjgjO
zC%(}UTo)h%MXm&0pPncR;!H`P&cd|^a8`IOvgs~2t*G+fefh(M!)YNBw}oiRCd-w~
zmU1zM;?J9;_#6a8Gv3$NqaN)Lrk3G+#A7Ix71}T&3Z4;3cV&Jzl&Y<DZ>oLm8b9l$
zTGefV)hBZnhg61--u-kgAb&r+q}mlBgiKwNnGx4mRH`sh60>k5!ve;x1zAjWhS{P&
ztsH>N8gM$SL4V|OC0m0R<B-*}M|8^2$^o*Yp_&Iu*ncNzR&K~L=9e*(cafWRL**4O
zU+q1zyzU;YBH(swi;4xRr{#%&!8e_y34!Bo|Ejz<n06Zl{2P^cF81?Fs-Jmgh)*3`
z%=!o2UL&Srd(}vH*3a)m4>F#?V~LEDsLE9w=G8bQEKOYoazhL$NTJdc11C!lwjxM?
z8^l~{fN5U_cfm^Hd-RY+STLTBqeM)-WZ|w&-N2Xi1V}stO@7Va)Tj|NOB$-$j4eua
zF(-nQpV$PK-jb8Ob6*UqqQSZ6CkL62X({OC7!kNwL$iNEn3|~NQX<9!MFgB|gX)Cw
z%z6$fWlWdDdEZp_lTgN*R-3aJTo~q#q}jrekC-RWO#rO=@th5lox+5a;hKSW^r>ay
z_(wbA$}()~od!Vp$_?#mSDGV>yW-U~@Drs^CTM|!6&($478kj*h>3yy3O~)*8qFc)
zw^AceG-z?NINj=hT$5M8EvTl8G@Tz)$j0e$q(YzNvt7=X&F=2%lE=^d{<+IJXsl#H
zLMwC|ill=~!I0vJ=^qoY{~bH`J*K`<5dP~2A8e+b#p~Qjx&Mys8X6W^7Zfg2xMhSE
z6U?LN*?O7t^ZWNIBGBWBY&(VwRG(Z)qhvi^s8uEH17j~`WdIh67_k1Vg6t5w>YOGR
znL^Iml}8I6$Met0Z?tj}gM@+grf8>c=C6${T4#NpalKSl%}tUzi|}$nfk(ULHwT^a
zvJe}c_q&(g{`A5j6r+*Sc`rw5>Rv@|h3{{DhEQi<%qf_CHgyHY5#qv^5hY?HSg_e(
zCG=#y9V(IGvHP4F=uGG$NkU5YSzywo*p~0&Vc9sRTt+~{Jha>9>C+$LXl%!Sn^1H8
zO5gRw<F`%Khj30hd;DXWxkA!X<Dkx(N+!BJIIQ#pXS|ZLk3U#xw(b4Zo-(^UsW3H0
z(;6zY>73$hUK6I#)>u&x49yy=YZrh_!eUYr(VFN)KO~X{BtNt3%5#51LSyaE>tZ$W
z{m;^Y13MuL*`#Fw+S6ZLKh+Kk)_vjmYs`aN<DVaTx(}@-&HnTYf&knameCHX_;S!Z
zP^@X$oL_i<MEP^i#UxI3X_DKIp3$eOB$|a_iMaocm4T~^=ZZ+5V}h}e`mIoi!+CE-
zK0{X_#>8wmU0se?kc0`_jY}*AK3k)CAxJ=7k8L|`JFPa8vWa~xpDIYHopVf=%Setj
zwzy@a<LCId=Luck`eX+R%<L!Lli3B;5kg@@ZRRcgnwJUDzivx#ejn&LfmL@o3pViN
zE)OJIiYYq3+nFo!AavwqDqW+EQI%&}%s$%6Vu)@dcfvO%@2%VBLB<`U?au>`_~u)1
zBfjTGSfUUBgmmGMNeLM~oVC9WEI_QN6f-)(R&xQCV-EZK;`sh;SY;XR;hji>1?SH4
z)Jg9PHV38{+X)`Fci<6;F4G0qhog2UND`V|UvWJ6We75H45y!?%|bex51E<9xQcpt
z4O(MIUxYaHIzx|}XO$4$PBCJeV=H1~2E$hl)!H34N?O^Br$#S9&iW9Vdw=lju^?{#
zIC7}WOZTdDpzdRXr+Ktnux`WLpe0l|1C!P&9ITHnnw`0RJxoj#$o{4_o68mP1hGq9
zXzc7#x9K4DHknduY0g!NDIY5t^YF-}Ac$$eh=!bw)E*^?rFdy#+g9I*^P^eE8V7~6
znynrQ#Skt;d8lP1*yz}0EvuIy1Y=vf`^CX#Het@?b`W>6=HT=67(2O-?(xIt`X}J6
zS`4HS3(^e($pwLKhwg-b9;)`{7yq#n-^<HZW>ixeJbMufDDj{e5^6Lt$lSEP)QT%O
z`(1q#Q2V&)x5}S(r~OZc?DWbWGYONqI__)CmZ}YH36d<X;#U-TW;H!!Y?UM*IJ+b<
ziVusifL5bL8B?tr6a+FXO@gIicfBvjD616fahOf%aQw6rS~@lWzDOE8Tng>vU3{F&
z_yzb}_mn<q+4d5*=x88hxuKAM_XFtAd|1bsgN?XO;&+lF>#fiU^+Re+6tnFM&u7Q@
z`0!}H*0WzJT7QdpN}>yNM9gyz-aI6yQ$2ZSYD{s_$Pbv{Qdw49Xnp69x*&wXFk(I>
z`IdFesJX@3b-+`y20y2{XEO$e45{U$1xTCT{H8c_CvcQ7ibNKxnCCd(i>1FXkaD^Y
zO}1_SlLx;EfpoG6nr>S)RN{kyI`ObEhOSY=X_(*}!^}$NBEs`>y^sWmIw2Q6KDrvK
zzk#1CNQ>9P4ZG`*|2vi()cM%caaNPYU)LG>1CrEa{O~rUvR}IvZ-k-?mmE@0yZ8p}
zHP>YGy<YEhAm|y@XrON(x+?y^W5ZgVy@0hP+dP(7VwlCoB+l9NQ(6bB${$|NzU+vX
zw~O)#WSK@En3-u!4zniBGfN8AJ`bluS^C+svq#Ar!5ux*7pGThq|FH;>Xg#8ePcs=
zMtR4Iwt=~Y$pyUhgK8ZC`1c>?4nUudBZa>h=w1kNjZA^m1FIRUL!?hLH_p0@LPY}7
zf5#FMuyQTEObvHkDhoY8m$z-Jp(zEN+-|G`nSV@)A8-pRjbE5}USKW^ZE7w`G@Jf8
zexN2^=Me!Ocg6d|mlzp6)%w8*EsrQsVfH_-8*8lWCOft`zFx$&sgs<Z$5%QD2}W}~
zc9S#cX=@`8%|<~D!`N_E$=vFvqD7HB3w)8L(beoT>qN(-vp6($P;#31uZ&<}L7u}!
zTm@#v@)v~j%_a{sNrfk&VpghTbfKd9J0(v18#<OhIl33=^`W>n33R*wBT#PS)>S*C
zMlB>sLx+m%xP$5`311vMI#M>wYgSI43B7r#$=?p<vAc(hz^Y55axQx3D`k^!)jNrW
z6VD-nyg>J?#rx^*Al2*U5$ceYRW_6~7Lp7CN`}aP^#z^cEV&aCn^`mf@U;+mpM7G-
zq?J>XGt+z-y{VM43Q{_q&}O+4Y}JpBGccR*COfD-4+LqYTS_h5y{1{;QoE&UkMFOx
zZNY}&sD2tegP`j6EvB96FqA4hhplV#<gf%R;mJP5-bM(|Fb#Kl3>{7jCo7eOnezJ5
zSM0@G=0lE@B)e`Pjuy+bt5>~GQ4+lq+jnh;EFeKXR`SHY>peGHJ}@-&Cf;FHa2QHw
z*W)wDhJWd65m6RT(#roy><Ln19xY2Oud&c%jl(3-)#Dh%0<&8h2ZNdN5V`SPx%pSk
zHEqIp=o-==%YaQY8!OBpcDo0Hz20daa`@Hzck6XW>>gmVz;SPd*80rIBZlo@x>mwL
zg${=n_a3uRt*JGSoBT?ie4R<T7($591_$0vGxh^<m<=I`bcTf|5r-gpZY4L%f_1_|
zx8II-(amg&0UB_fIaxK~@g_NVpKfoLp?fE{%wK9~zi2#dFhJHWY+7D?%u+fZ-#^Q<
zzHkvRTyggRRM*VcJ`Je5VN?=_NAk-a6=nmr#FC-0-)<)j00Q^wN37pGU$$6xt#E!{
zR6!*b7zXEL>n<Ax@w8WV2&jmuOr(=LWC{kwU|?EcomzF7$_FM3VhY*(VJU%|r~ccf
z-Xo(p`_6BXsk#g<Sg>aFH)YHts$$AjOb*4o?ltHMu<(7Aw>o*161XmZ#ko;L)<hLl
zhvev0r@QtIi&3Wizw}r^Gn?e^%$bLfqQ%RyAXEE1(*|d)Fn~BIu6}y53(1COHPL!c
zdlw1slLp5VbJpl0+Ls4OrM+yo_UH(_RgTeT1(B}%B;&6PS0Ns<R-RgE>sv>$UTZwn
z2eBK|vAo>Siq3T}UYFYU&Tfb;@9KT;fk)4=F%LeEIf93Zn}x%@VXA96iQgj*OTH+L
zE<2z%jY6YM26|1Xdf^^+>Az|&NT#BPI4imDoDQ1uj889Z{IKtR=XG%tuQ;x_v{#I7
zG6>x(mo;N=-N<JP=LuVa$Ofkc@@#{bnFhE_+Qf)1L8h(S+0Zq6yGUmHqH_;39kU5s
z8K?suxmi(-pDn*6zGAl}oQcXl{piJE%8VY3cp4R6eY!nvo`XEgo;Rit)m8f(f2=SP
zI$V7E&n%e%O?Q7I#D?;+Renh(sc&jd&Z+}6>Hs~GSiE{CFf&iyO7F}ekBk*WI!>iS
zWmuckc<Ix?3mMwtj$GdGvl#N7p#NWD?zmRD`ROV3-{z+NG)S1aC`eeGvKFU;(!~kE
zr%SEd>q_$pU4L5G?<7)!lk;V>yf+3PT=Vjna<#kKAPA0d@@;H-)aMA~iR<+mRv<d9
zjcD7lC^7uOwjbDVEL!hzyqY)G`$F!lOG;VMrqttKAv#^D#SV0w@rQRhgC<o|OAYyr
z5BuI2IdMY9CYlfSGn96sGar2T<w2_yXUT$nD^_JzeQ4dG8%yWb6dWHMM!dV+Qgo+&
z=;|A8ElT_<>D_Efis!A@klRGzZ&&0Kp0;8$PDcg&i_bWR=idO3g<l6VJw9F&2w?RN
zFu2yozLM&);mh4}|JH1J0zPIN3l#URiNk?b^3zM!7maoXT^#E8YPIzg{w(ji$6?7-
zloG!9N#DZSP?d4R-$DLf6+cSe|NKf}{kyVx@b`mv`%0T)AHYQglcTt~?-_d^j^_rS
za~)Oq(DpPg`M+b{e;WTbcFpvMKc)WsKlv}L686OY4;AU>{~cS!bN@Rwc%zN<zc2q^
zJk9^T?_Mkpel-@A-!%swy6usBW$#XHT~gIgnNYgINu*2=5{!PLfPVD9vPDS{Z><K~
z1vf|)|Hm1mihr1@^pl-TTL@GKqJ}+bjzlF@G!~CgLa<CV&j0%$fEj0;v#ws5>iB^7
z25jn9D5qmOCOom5agSVuPojc~Jai2B*LzC+Ym;VkTwo4C?_`u`KRb#$HW8P(cjpfJ
zT)kAM&67Y%zQ>&*ZIu9xS_m)A6AN`B1vYmoP2vw~<^@fjeh;6+8|PuXvqP;Mr*d`R
zAQPQ@e$##@VdQJ@Ih<U4I?Hn}w?nRja*q&K;qAWUNC;p|r4A?*k4ZgZO5_Lsjp$8<
z-XTE%?K+%md}R2(WeNhv0rIe8!J}Ke(^IHK65#xUOBu>LTyg(3+V){58=da*KJV0*
z!e1n2^OeOY<{Ii<Qqk-p)+*Q^u0Ky0^r$U3@!ck%(3?{&Cf#6Xy@I;5PA832ipmJ(
ze`skrNYbZLXBuz!$Zs1xGsK2WOh`V<_ecP$KG)jTXmCB}3h9Nogm7;)KttQn^G!3F
zPOSZ<c8bMwIs{0>X|m3O@3T`qHyN^eet`|4&R(FW3{BI{It=4PTdbHV;48xlL6uH%
zYu{cMMgEhWe{wkDdatxoRQvMC!{4)A47?_x-X6n-%kDS2#HLNHiN<%nSmhQeJ>ai?
zLGN!Qt_UiV77{ghq$QitEodsk+>;;6&!wBH_iK7r!y}3&gp!@M21!6p2!?6?fekOt
zB~Reg;hqf)iO;S6GA#*y@DGW(j8iG=X;we`L`~mtJcIKFZk|Xrh$~qzZ8EQwPFW*@
zv&1vfKgWy4NgZJR)X+!6xWdKj(5=?zd9I(Apw!vysbR9Sd!BvF6oU9j*jgQuh<Y3p
z<aPQJ8|=<{LsHm9FC)o7(i~pbtqU0hGf#1xpkkg_cLXOM0din(>NZNsV<KmJHTr*8
zuFZLf$+Z}c465?9xo$V(<pcLWf0mi9GN7Ebdtw?3HoWo(#$+O(7ems3c9yEIa?_>_
zsMI-iT@UER6t+t?&ojey)HZzC_)uv6FxH?dH6`ReM~wdl08kX1v`ZSmQEz5_buwlq
z&q{I-bm$EFdEhR;LTX~E>g|<@ex1QueTT^6?g-2GVZR>))CDVtT6AlxoS!dtro-vx
zb*9s!&a}k#9=Olsr_ZcwP8miM5_7<b0W!2Yr2`Huqr=moA<!@eGGBAlnc*Sn6|uV+
z;f3ACN~)g(8iypA*4pZgb{Vqaae2G{RGPr};VTM}9{lxK6FeYIj$XOrGOorYUxi$)
zgM4AGo(QfUH+gxSbXnz}ba`fjk6{hh?KZCeq6h97>jkU}Bu@os)=IH#gAl=#bZBe~
zA1BWy+sj>Wg<fkzLE1<&=CR!-(r}RPiP3|qG+zvYd#nGYM!gwSbYPWEw=Z<R!SLYl
z?XZ_@GeV3;3=O&pZ#ab9-*U7_JkwNtYwO?9$OpF}ePI!*%SKs*Mlev{MI=*9TqWLe
zO0G&t8n1P`UNcqR>M{V3<RBq{x8I=~mhV!PAcNsuDVFx_6pmb)Vu-RyTBjv>^>-f8
z;H9>nZWB>r_LHq^;+dQLcwQOaI+;i}qc<@JpZ8QJ1rAgS@&<O`Vj~%ZU@QbzD4LtG
zf=pbX;cQg7#6j4BiFlYNNQc0{VhVxhz@Gf;>E^D23VcIQI%uXf=Vmpf?}}8O@Ze@=
zkkq?_+b1e?!$K^ZYuIO-Dz>@?;_{;lv&<bnn#Ps#kL3L$W!P4(oU&!xvr~EI6?D^1
z+mwvg3^>UPyu1xs<KYCPNb-nQ{H{SF{d`0pF>$}r4RRX*>}Pz4y$Gs&_SI1acUCrg
zXRIGqb?aq+jvs!}Qz|EvRo*d@6M1Fm$5)T8_)zOJodwOnu*Z8W=N)2P;R#?kb~;)k
zey3a{!Fa|z=N1~{{+xyi=kH-{p_cp0Jl(R2d_MZKjos8#cP~gJud)QE|K$WF>Sv~9
zguDRT*QpH3JyTbm%}vF!*<R7?l8RavSS0M^b)&K7hU=sI*Nu)A3SMp(w29#&Jf$Z;
zuc=hv%@3bf1SuW~oKtUP1_SE<=N;3?_L34H8pwW%H}B5w+;q5~J!A2A(Ji4M@%6ec
zTM~FKm%*DLlgj(`A}Vm?h#jtaFR+Tb4C9s6^TLRgUzdZ4EK1&7aP|9{<L`^-;JG<#
zg;HdYOq=nekT{daR;N6oyyyfM#9*xB!f3)>+lJEDql7;ahTUzmrKB`Adu#M9a<oE1
zG@ksr{o?O(EkJ4#&Xq1wRGW6CR?yu52!O`BY$`#-ww@GAV_Adsv{TT0wD=t@!Yi)q
zhR_oHPJB12_%y8*No`Gr=wZ#aSoj=rA3Lu#x%GtLpGdW%MfBizn-B%o$1yAV&i{6_
z?EYAon!Km2Em4-1DeGV7k^0i&>5V?y|9LbJb2xeSy%-J+$W`h@h&kLQlDH6I1lwUZ
zeCGW!NH?&J$S6>~E#A!ph|jWELB?B{L(WUP#zFFsd5AYZOz7lEt%&eEs5}+rJ^gkk
zVRV?F`t)vHT{HlAhlpoC3kt~Ov2?w$d3DamF1$K++&{ACgh@gGJnwq5aCJr|tfy(Q
zryo&F5;I+RJD`WW2*qtu@%M_ukGf5ynI1TWP$wQXMfQX}*&En+az5Q1y7yq$Y0xOf
zCPaSw^!eAX8(ep953ecM@{WcV`oLF9xY?6M1-FX}=O)r0CoRI+!MlWRdwWH^yCIel
zjcNs%>;OZD(pdtAX|QZV2vwO&K`b;Pp-WsTN)^SkdYW?1DSm$bW!MIN{fkDEWn39f
zoOTMM_g1{##GXtbXgfBjKQRq2<-8S^d_G114ZVtl{Ftrd(R%<vl{JrXCMc9)*kC#|
zLcV^VyeQ5TK#H43!@xt<QDLYQx-6n9e7>k&F*D_H;;W)Up6C>52BTS1KbzHL*`7W8
z-?4yoQTi#zGmNL8aqC(~P!}WN)aTx4Cghs4+UJSD?eUuzRi2w>w+(murI^nBc>6C@
zked@(FVr+zQ}AdQ$m-BWvL^OR=HJMFHH$vXSqGdCSS6d<(AS0&4;Q_lm$L>A%#h&<
zMuk1crR}=pc~*i6R9FN68TO_6)4{tBx$XQFIZ{r><&KCa8NW;acg#>~+kz`_`F)BN
zOaJfKv!LETrNCJR@^2*OQXdzMsH)OrH6l_VFcaOo%`6y<4eHmsyTp}Nz(Qu`8JkAq
zo|9KvwRWT9e-@;#6#y3(Jx7{9Ii6(v$-(QLyAcwSAeBXsEQ^%(w$I;eD^xK!q?`^s
z{gbojyPy8}^W^bo>MdNCr%d2B1Pb5cuFfB6y07&in<$tRLTQ&i#|2=HOOw>784P!+
zN%>}flas<hMP@<F%%EHGezWU;WkqFlT-s}gZz{IVtQ@3jgwH8b!ygCL3_!HUe=_-I
zx1qZY<;%|NSp;YXYhUZu4h`uH1->BEp+iE`Ma9Z<GF?tW>ZL`dZeTZzM_b5sDq=o>
zdQNq_sXG*q$G#2T4<lS>yl3*$%XuV8S~RRbeK`K?a_jh%Vm1HXs`5t06XP;L4|__%
zc1;ycF=bQ})B@Km@}N_F-(hO3YM9aSydnR(lz5i2JbthoDdwY%HiXYWVwi0gpDV%@
z=l$d@08b0S<i%5nGIskeXYP0}#^hBgD##h&GUiMb$%2q!*}#jW;I&x~hWQfs0;_Np
zaQqj!7vHL08(BpsjP6sU#H&t;$zBMxhK8e06tx+Hx=aE%9NBtjThiw^Y#4o>OVBPW
z)nmuGPwfshx9XXUf9vP$ObK#wW_R{dLqkIkZ^zcm7p{{1MMtk&&X>hp**eVdT|LZE
zH!*2n4fH9<KkAZhwG76S;|>fuL1Pmi_mVs0yz*Muy4+yKSf0Fldz=NoKSa|oaNBEA
zoA2o<_55gg&B#zH>2?&s`^=2A9a5OJP<T>6R}WZS>Z5+H0~^ot$@a&QP7#q=r9xyG
zi-7SS&x5G4V$`q4G3*3n<MALUpAOq9c}4O&E(zCvG`=zYkM$n7_013Xu}OXu2G1;V
z8v%DtkF~SuGs*TzHDKbc%)=LBR`M?u?N9VH8<(2a*}2QOewvexiP4C$@@|^g+%b)a
z89g}RWn#WoDK@T4(9{IkYOW<il_LNCUj9VY4tK3QLd4*W=qUMHCmtQIP3B(Fad8>>
zI(<3)av#C`tQ)7s!R_ud(O8IOeojDKh(=9z;QyK$>ygC91~EVbA&Bo3>tGw8sij|X
zsu$>HTK3t}OmicKS!kHI;%X5C;RtPQAs?MM2=F1A+rYXwNS7wkRk^PcztVdo#iS$f
z)Z1CT^P_v!jtw8a26pxpFg<$kVJEeVZUv+lkxyTB@%sF}F_Zixv{+uQ3xV{kSh-6H
z(lJ`2%i4*7I9M^J6`4jPxnm7+5rAYW9{GT(N|(N~Drk5R>FAwq^etb-|G;eY-P!4#
z?8@@xZ|U>K**o9TCu)4~g1Oh!Nw?Gb!M?%xmZ{H&uX-a15g@^zIaOY^Ki%-gg11tI
z<-km)IqMoKH_iXJRDB#3yM?Y)@)tWfw`R2}JkSr?^b^hgC~8hOh3A=@<fy8kawz;(
zZs_5pqI(t-_#zz3yOcINFU;MEOGZ~W)r@>C_w#6NH3opRIDDxE!GZDo)adCbf>ExQ
zS)xhegk-~;`pYeYb(e3*BZ|LURM8I_UNA+Hx4sj$VnEWpy^5}F&JveW!iDbT&>+wq
zhJuRv6=uwb`%Fh7#0|*Krc*fo9g8AK%+Ak%;QAy_&pGW}z0pz+NMu6DtmDNgTy6EZ
zw_2g$l5&hgV_<QOL+Y&=zA^{46WUB2u5$13)z)4cF2H7E=Mhk9w5G1(6qG(R-VD9Z
zz!)qnpu|J-k_nvVfhLk}Vf}^-4AQJw6sealfm1Wu>U|MICF_cAnzqGstu6=}Mb!en
z&U#j>_Ybq;c8y2nrKptUH32FaO<cno)y6#g)kYp83JR}$?LNl3Kd_YjzyK;Np}6TP
z2=PfwG|gQzUN}rlZA6iyprncUX-@4M>XPjRs%D!Zsqi>vNGeQxU?-`LIZJ}ViTBYg
z;0FXGv{ppInGL2b>beR@c`__}%UPw-5}ylW%jWn!M#qDihoUdFBZwPH$@krDPPiiI
z8{ex`ucbzHxF(o>+803Kc)5_BYwdhkD;tI`nXD8^sjNUlt03H`p=`W{H~gGcFZd$=
z0aN=X75VLbv>JJ}a8*&x>3J;_ZKhc>PTaXq@|SHOQd<Crl-3!8Mej}Zr+Sl|7?BkC
z&WGCuHVHtO_IsT#rf5&_#&|Rsbjx<EL-o-OKz9N9kV?no)T>(nT)CBi-zDuZq*;3S
zcy8$eqgRU6D?T#iRKJYb2v;vR12+K_>Svycy~dAmy#VQ6X=zy|J=%0iZ=~t`C&Z_)
z=&GG<gk0XZ_q9f;Kkc--p6QWyJUskrTpZ03OORF9Po#z1dnSjIn6o{p(G@{gI2`ik
ztTf<6Qx}*Rh6;Jv9Yb9V9Mj&P#2O1V_-Qqsed!tq6c&JmOU(4q0=-QVuSWY^?rc4y
zwrSk#JDW=ga2^oyuVx?TR>O44E0psC)#@Ne<0amn;&H(02e2qO<VR)ugX|P=w5%YD
zuCBgrS4;&Ihc`CQ^;~ac9}Mq06L;%51?!sH=DP&zz~UAF;3O(_cym_;Nzc*A%?(}P
zn1y|}-*_^I9ApfKpH1-CD5Lz9zJ68Ua*J~MWl_4XLvjX&A7Lmlbu;9*h0#}TDo^zE
zWj|QjSx>YsJj$`I&S`%jpuoh);jyMd*;38=!AI%t+<@z(`au*X#FNRM&R&Cn;vE>8
zRzcnmC~0&JTdMER^&1bjF`IHxEz=#81<FQ8&59pEDsea;{P|Ay<(<4sv)hD@7i`{t
z$MD<e>eJD`!dP^@ooKkvB_=SG>dzsoI}4H<djK-5ug(G#&ihMlntK<1%r@`(7LNo3
z8)6okeFFN8^Jd;3l6A}^q?I|+M*Dg6SfwU@EF@%hep9htqc!u)w|kj^KE8oP+fAu|
zbQjUTb^anIgajwG&&2Kv-*(#{=AX;j6_A##2v?Zq=q0lQYBo-S>85470qfj$q_B>5
zpk;C08mN-`1R`}2l{3Atw}@nCP4A}Lb%IZwD)=Qp@Ks$L$g>esNeG?e-o3Q(bTR8G
z8*8!hN^u*~FzEYy3#FO`s3|-K0gy%MjWuBC#rdd*>@j2rNkVl@n3(0sa=On9470vV
zovuzv*Kkj~3N<k^Yl%9@;<y-$xQdC=*|CEHL>dZu$^hFJLWT@d_kDgj>dlNYbmD(K
z?Umq75wz=zvDHgNF)BqTO^OIs#v`-8-Msw!UzaY_UhdcVwyz@nH|4Wugl%Y|(3`eH
z4uGNqG~LW>8p6C+&91__=%z3rf+00012<sE*y?-iev3Ul9(Q}-zhea%W11$KVNlNj
zHqsz0G;5Kx#<B{9N$-R@p2yX=54E&b_KTrRVO9ktJ-QU_Kc27mZHrFX)3vwSjsns7
zv)FYJ|62BLh-%JhYfG(}Uuh6co{Z~^is*_99d1O{sFhqZHJ1=)u9>j6G5#3Mn-Rzk
z>zG~atRi4b>u4v4!O3Zv92~-Rzin|nfW%ne9rTQ`{_E(cp%%rxcuVU}x%9633eR3k
z(c$}DL(i^SX{4<8R4?Pyz{l}BHuy`Or=HTz&v&iO*6v~jv?nDKWI6OwjQGglOmb<(
z&~tai*EUROj<%<7FF<0#`eFIQ|BjtvyVGyIHR*xpK46^CDI%M`d#M*O7)IpVWu*C>
z^4;s9DV>@2@y0$t!6a&b;OiLshQ1IVo+^}wj1IwL4gr}6`qO_bKINm6UPa%|A`8+M
zPYyy6j~8}AA#KQk&Tdf5+loM=5mgb=n0W$fd98FwGZd?-u1G*ZAxuC(@I#%>Ny{tT
z4Ef-X>o){S2%l3p>8bOtM|?RcbB-TYV@(3?%5`^S!ZY)Jwl_I<<eTkQOse7y{Q1WI
z-iz=s=!XnLuEkApzv31;an4kO9ji~(H0h0Y9JPTXGYoR{w4<Y=T?}dFlInPP84!#p
z;NV7Lt4Mx4np~`atkD)mrGH%Usb1XaDz|>-(jh0xXXtbB(SOKYJGks&1A2oG&$7=1
zNMD$kDBGOYnFo|?Uwtm?%Z*aAZdly$Ugt0|p*1ZcX9$$I&?**Bj&9V5(`(OWn621o
zmk4G>OpIepHrX6^xCD2;>g+zjjb_bW4iTn4QRAwgzdLinPia&<d-TUO$Zpy{JF%%V
z>*F&~yK{NMqG*ypM^6`W^zH6vdgMGeMc*V8c{o;s1@;CGnNI1}5x&a}gQCh-(Xg7%
zwiow|y`^h;B@!*{1sg318n?k^z=Nq_QYr6S3^9$#<5@6GY*^CJiA-m?ZygNpxctJ6
z&=n^xXWPQAqNKLu%3o47sWia0O{h~ZUi(M;qweg<G^cClOJtotrPz^f(MOx*sFv?~
zx~~%uVP<x{4ll!O8g1`8QC0e7xN<{7&Qf!-w%%KzC>KFy>DED0Q+Ww>U5>gL<T<>M
zqz*5s-ms1H*VgsuaG#f3$dH_D!0W#&jH|)Nj4BhOR8wG=U?2%@bDbOEo=(LN|NQxW
zd|~_NEE4^Aw``llvaK!z?bMhWDz<DjmHc-s0!j7A+YdhF-A7ZGIWSoVAkY~Gjzp-g
zoR$U`>)^)h4uk3w(-vjJ<RVuEA+J8R)0Px<H@%Ybxa`q=-!Be-Y3mpX&YOtF6o&62
zuG<D>`40|gCtxYj7a-7(bXRT5C~&y599rReXcu}3jGoP9bZdBLrEA7$qTtsaL(p)7
z*bZeK3>zyd$21gpKjbG0BR0B$nGwn*#WO&%gnMGW^oJ?=jYCG_mjkc!4vxz2ey@GP
zAG4^w_vFN|MpoFRUhYK~Z3p=~#OMkN*Z&#Q!p<dnki<~GQ>ke@UNwN*ahDIrYJ*ID
zXCmc7(k=(B4M4P7DtcUo^2I^kQ+czSvH6y@!ys41<>4`2oW8V!dF*TVIOZ#`0XCyl
z)2&ihT}Sf!J-#VMP2$!Qqv<!!n%@yj6D0q@Fnmd{keZ>jhkXYz1Mfcg+`zKN%Q@}E
z<ycZ9<M~cxhwU)EI27ZTJLNJJ-fm`4*CU-hISW{HbO^qD#T@XY=9iuPT))F?La-w-
zn_u0$@G>$cXnX9PLQ}b4bl`)oDtqCC)XSoiwjWB#P(*<_-AIfyWSD{!#X=SJUh@~B
zI3yF%It%@>AaK^S4!~#vkV*=3@ep^<4`E3nSyP|vYl-zV_{H><%EuQSDvVpPtQEP4
z;%yJlNvu?uhsVc>33ogBYY^>cfgmG<yws9Tc?<10ndedFVl1!&yboJHDONfyTL+Gl
zX#K22#_^LzWYSs|(K)Ut9nz4AC}r|w`hZU98UPrW6gYLNf3&S^xs*|<(hRYu6sYbi
z0f~m{dlC_6VG(Ctd`2QHb#;En4SXJf(VNpEHLb@8Luy_+AjEO{KM=yFBY{4HEDu+l
zFw#lWIUUxC#yY2Yj<m7Cbh9Nz*PdMv3*kKg|Bxfr;-)(vio=ixTx-Agg+=jg=m3TF
zo?i27d2ny<XtUN@tyZzy=|_)tRGUt$jX3$9Zd(7EZ*d?|UCpz&U(|iBK&mkEIO|qV
zFrMk-<9_C*|0C2-Kbv`WU*{pkdKuHftOaT`F~hQ$3Up3$CcAI2P1KfqdDRh$#2lmj
zF^Czn54e-Nu!!4TWXQt8Ob7B*X%F><uO`*W^rL=9noaSz$6l7tFYSb=jaw*NJwxx}
zr?7vHI=1>>RQ>zwg<_S3_p_lt1Faist9PUCr~aa}#?Me!C9gkxv11xz=B>B9v5;23
za&V8bu=lk^b9VAeGbC)gb4=W6DUYFJ^@6S3?1>pwi?Qkl%AfPk);96)adrOq+%Qa{
zU}*GeY&SNtX0-h5_oB1LvLncBsqv7oGn=QAo}Var_NlfuMNJGW+SuI1cDA9FiTE>1
zEQ|cN5J;#d!dU^>GE8Ej%f+Q#yz~ui(%+3AGoUgo^L((#au0NfFn*rJg^CZ$;+sI4
zSj^j7Wc3r?>I(ia!G2Ti4SO$T4qWhiwOlpU$+c_Tt!%W2|HZ-QR}WA9tlpZZ5aoK0
zzwW>BP&FMAOFlXfV!73Al8Sk47W1TCFPL+M7|*8yhlGq?IV33;C#kvuEL;^c`Qfa?
zVL7BY+MwuW8%zB%;a;nKNVbgEC{-;0%;@>=;Xm#FKkU5+SX1qqE*vW$Dj*#Mq<0YM
zT|ruq7J65a5<)<FCl;Di>7P_70YViBgpN`Lq&Ml(r3a)$2;nUE%s#Vc?{D^*|NPU=
z%y(T}R|u@Eth{;G^S;mh+|PY0uzaUsW~v1wh6+aD@$@c-B+GU6^@-QJ<wn2*>|!yd
zF9ei$De@@{$dsDY$5Fh)uL^EZJuS_9nrPz{-&m56Uh3Pk?^okn>!Tel)rzhwg7M1M
z)aA|O<pvs1I*Tn5=y<x`4>03yvM`$~Q`FTDmpgHr7Yi!2w_=PEf_<Xip5-v6+22wY
z_@GJM(TLTgDl4b>R(>fuujoN-H&<C{iLJAPhS(AhxO+-tzyEIWO|xHRx%Wt0J~%fm
ztiaPQV~JxC_&JpY%Z;ug!Je$baw*>oCPd5yC*H`ZczQ!oZABVD6Qg-k``bG){9YIt
z<FA5nLn()YURiohw-GIYl$-lCgVlVAFcsmM`e8k9N4IhkNLqT@QSy;!29FYDh?J^Z
z;+>+YoKEOy0VFsLoReP`uYD~^#q1idkkMIXw3<v@`bsZ@TS6|idfQlw-OocH?Ccqb
zZ*`29dFFiRS{S~y5|G8I-;c$2bz}C~Dfm;o*0ygw(uVPA3`P@o)L=I%gEPtrgT?;#
z{G2pvhY`E#NLs`F!a{xBVXwl?9z6|XkK6H0xrYe*tx?aWl#1*id#<x6@#9T@tAa(c
zPif`hsT|A<je9h8_Sap1dVAAUe196Kyc(Uf(|AVX!|vg5Ed2FOrIO|AT%keqA?n)R
zabS93KZW(RVkKA9wbVInt6_@toaObJlJ_|?8fFkwSW07&Y91$dUQ>oiKV^QM@6((G
zy{@#BDNed-aUpwwGvjYDr9}ddaa|qc>Vjs&3rp5Flj@blhTm{u65967RAfpz$s~H_
z#`0cCJ)C>?X7yIy6Z6yz6IBDCw&+pLNzajo+~wt~@HEImFQce)p6j|Aji;}uv;uN{
zBk2Vsrk0XhTd5d<Tp}&-?Y)G^43!|$+SEB#94D;M4C<x8GU};Ff(hMb)TzwUgmEP~
zW%9838#81i=TfrL)O_Kj5bTlTgLvM!6BJR+8D1=>;x%8rcJx*!UerC^Zg!y;azkv?
z7VNP?&1+g<SNFZ+iNi?i11m8kSl8`14+#i#>p2s)b{Q&H^`jCqGGe^jS*lh#D+)~=
zy{vfmgp%tyf-Hk6BqF^&Tym-zuCj2f7s^F=3VcWAC!g%EswT~jrQ9KoN(TzG97tXO
z>ZuHtPv00W<pYb2Qau?^%}9rf<=V>Ix+nR|_yoeW!2x|3UIV?-D-f-k@@}g<>8H6`
zT&0&6-;u>2>^{rcyCdWnb{ev$VF~85^JdU4u*q6Ob;KNxkQpq4Z{vW<T=OwJ<ZH!S
z<w{Mp2=nmOR?<l+H;kiUlAp$Oa*~RkQN7VGY$g316u8X*cSECw42}}q!zwc|@3B9U
zhckG2QCwq*LPgqyZhhm#ycDrDUK8ud?eUo@$?(Hf!7{GKXS~@&qPZkjnrR<WoF#rP
zX1yrlngp^4=De9$8s_6omD(n!9N~>_uG*F_%1HGJnx}9!re$%FRdfrhYz25LuvV=C
zOWoD@>P0MJV6t|-*K^B3@=IP;Mf6yKslmyuMXpqdXr^p?NY2A4QxdxC*<*v1nW_i2
zpMNHWKIqrz5fVv=gd#a%9o6-#km&KvR6%Qx@z6a-K@B8Fs+V^aM$IlGv%gQFPhN)<
zi~0BrTV0dllsZ@C?Ygkp?JT&n$^aSQtZrRgdQ?j@W!co28n8oSuXk~lBsbJ#XDB<&
z4z;hEArrI(U1%=JDH{p4As$YHte$PtjpU>H%r*oK4A~)-s5FbYAss8fUCO{Z$L>-0
zL|>=WteO%eJxYbI^0{6jPfx~;OW%aD7tSENRywpk@r!GQ;cCyTlov{)Y(h=oc^8Fm
zw)Y%0yQGm{XZNMJGHjvJ%pG2CukFE}lRm3T7%p5=y_I?@nIuECjvjes>>81_CYGjO
zRB;>SR@kB9?sh6=h%$tX)w#JF7N*xVl_K$)h1C_`tY48XFx}g2OU<t;_Q_Ho&B>%i
zk5OtojCRhnuM=rbL9SP=y33_CPCg?h-hb$mUD%WOyw6mL3fiidxFqvD`-*maa4z6;
zFA~Lo!W;6ax~8UxCVuKkvM%{FrR@nwG}QJW%`(UJ9W~A{+&!hFS_L$T2EK6XBbT~~
zQWl-jM$5(C#IF_i`-Gxe>{XI^)%TZ%Af#RH*EQER;;d&LW`Chn<~SzfElPw*o_-!h
z8mA6f9WrE&)8yx%l#hF;S*i-I0s>B!x=CPtEg{n(@Myuc#2I032f$-R!(T&VEM=zv
zLX)AvJ+RGzG|S*apA^r(9CS=ED_zYl+L%zb3hhzkNh3Du0f5Mw6fcN56l=lxMe@@2
zof#c>io|biD_5d*Ufl~(bZvZCArg;6jV9UvvRI9_TEhk4#B3vRc1VT-xN)Nt;VA6o
zS}N%4ARw18q@A%koe(RKE6G=l8~_{4*c#Tlx#=Bw+Tz4Z2Atppdsz`y9<`y1j7XiE
z$^u+snG)fgEnzv;wXa<7LUS5x>Mkt2QV%FlGMgUe<K?E;z%;}s-j?P-`s{b&_gtgw
z3hciOYmjO`JA_0SP4#J0Q{{~1A$Q#5eQinl0bqEf9_NWYVhJ;<nnsR|7yA~@HXUvs
z?oNhujJ4(I{*32Ls?Nx9dPRK45K|o@p;WRljE<L=7BbNbHel7#P~|)A&)YOe0Ne-q
zOTh_#lhSUVRIiCdnf~Z6$(t#XlJqxqgA}Op0V*T9Nb1>!?waXm%<?$*a}Y^h^2E4V
zO!mU8zV;6Y>Dk@Mcfpbu<W9XKb*oyclIc@<n1XA{);TETbQpQLvr>{Ruy9MaZhkX8
z?R5>Yu_eJL;*M`2C}Kc?$&6YOg0|!;E6b~fqBw?ONyTNR8dANfH#fB9umw`jV2anc
zCp;pxxPDlSGwKhQHBZ7zi`IDy1-@p={gwh|&_rQslSgx*A&<0bW6>9+<{$w>8H%%j
z#wiofuJC$wZ7t~V=h+0K7B{m&-J_+QP9(4Y5URI1y)lV*ELK=pXaq^S>uxE{ovY!k
zK#m>624&DB3YCjXgt_`RDY?74%f-uTaKuYri)@;VXuJG;YC79Q5dIAl!viVc<f?m`
zO$Su8q7U@IeESKB%UezB{ScI-uQntB-l`=H*23^g%T97kPl-h61w)2ZMKn15Eo<-%
zi;^G174FOppxTNznOPF_KWK^SGMGa*Tm+W<!(wPtf9N#USATWhxu2k&*`vQkV}FlB
zhvhN~pyGsO%<>JSdK^V&v4ttT0b6ZEJWoV|hhURaNL&4SairF8hAi)p+w-Kd3DMVH
z+IwcL!tYcxrX&+|3%dK}g8Gdyk_?_RLBf0DRo}_Oor0g^9v9h7!fm~N$Vsw#XNq%a
zE$1|dgs@o_HK!G=glEd+=mT^W(dM{Bdq|RFoTHG0K1ww`Eix)8H8_`MXiYU8s)v02
z3e8i^<G9;LH&$?U+HW6HDx=}Fn9GYY&~_PbeDIA0r!HQ~-L3{1b3!bzNUCi~q$n}o
zP~O%JM=aVqv9o<QU+SyAkW2nW6F=GUglMY(f=oC{98)lb(fMx#V6E-)(_@T-mRxHv
z;(kLXRenE4vd}{YXR~K>&*F7ah&dG%UQ@Gi`HOkKYokV#5uk?y5>ZlFO$OFc;e)#v
zBMib}BqS~?FmK)cS{7!~`qKvs6{?cU7k=w3wC;scd=$a^WAk0YnHibM8^;e}x*Kmi
z3(dLWg@uL;LQUJJEXLJ2pK`cEZJj<1J+u=Oin<V27(m_7647t7Xz3^D{)m<$zv{Lh
zc~kP#daf^=EBJeurjO*OOMnx?47(E2o)P&2B&B6@1RkkIJCs#7d6aghpr9$Th{_9f
zGSxkE)>b|9lZ+wDDSDsyd{z0?LmG8E9Fhso?t%QhUo^9nzfhQ7nYY?dHfVrN(7qb<
z)mIO%^0l@_SIgRY{IImWw;lJ}Lz_q^@YSyOgBSkK&T>Qp=b)?Spyj7~^JmdsEhphV
zkQWLs{=roa^yde%EIgpR$f?sdf5hig4wzPD-uPSL!vB5r#lK4`{Fe!bWUmf%VkJ&!
z+o#KKTKP2Jfb?t{5ZWW#j@=x(y`jE>sAkG(H7FET*zFKchc4{swa60B^R<h6;bEdP
zuRK|>ZP8r00FiXFHe8O%HdIliN;~rTsnt&+u67O8bNJJ7oVW8eWH^Q6`OJo~UP5^v
z6$-`8JL>Gi_omf3-Vg}<QHA>>$3Oxn=~rF;Mo#(tMt#N{2KM-fa*FYD3A?{%Lj;uL
z<&BV}w?4W#;a@hzzL90xbENY4fAw|Wl<_G_l%9)Ew+Z<;rVTZ_J#)Aru|Ui(R+=MP
zyt7DUv(xJ1=UGQHm{>vA16OJ2O-YUxhk~`&8#dY&qUTCsi#p`rWy%V?OQ*id2Oov*
z^Apd_IxnVb+);jF`;3{2Cr4DKPwLXzyKb%s<|Af{lm1LT<W29q0=k9gK(o40n54X4
zbgk00K7NN+<qhG0S~1DO`uMWB5~SURx0+^G$jVG?49P2-zgsL{bhZ{l!raZCx?*E<
z-R<*`>i!j38Vw`qR581cP#Stus$_~f#z#Htl9$s<w$dtHa^PIU&D-4012!i6b(|8T
z&Sk?5n#CHm`NNaa{M(*&KMDXqAYx8xZ##E`WFd`7fgJN(X=HOc*?dzYiu+78iN7mf
zyPK`eZ|JPW?<t&tZqlyU>X64MG;=(Lxe3rx^)?gBUjnj+_@WRsNV?57Z+|+%*DS=;
zpE~Q4G{mpovskxqsWOSzC`?Ye&LVq2n`-S#wp35<^ANM=M+p@R1Cf~I%uDD!%k74_
zP1*#`D5Ge1l3Vk?$by)VUA7OEZvcy(_x^DOszcMSxhIkD0_tcxZVNmo{Aky09gWPY
z{ptAR$r}`;Kv)J6A`CUk$uH$q)yvg#*Uz&Q?~~cU)EADow}P3^k_1%1z!tqux-Q-3
z8aOYvF|gW_+h89-6<~)8Im@Ycb2vJ&Bt)a2HTz~$Fmj72pw)8<>UK-i)i=9MzB-tc
zp-*r|(<LCGO-}FYY-e0x3G%cj(e{Ro46Z~BH886&+`AIxnV#Lpxiw#r#M71m2m{6y
z6C3D7JuTnKhLEW-NI5B`dnOev<%S%I1lK*C)ijDqPnYIGfyJe7FE?FkSEQG-gfG;I
z%ZsWH0|;?Y11xvmbjA^*+Hs|0rQ{+2N}=QA2+zFVB1$W&?~3-aYQ|QTNSZ4tjC>u<
zE#)gJ@7U44hcwG))=Y9QpoK)M(YtdkbaP+f1(Q1tos9B9<>V#x+&1!>3$B|v<4dJl
zp;E#zdag%rwG7M^$rS{Asuub~V5m8tAkuBCjqUfz(~s)M#rb6&j?~x164vJ!8pasE
z5R*F%Mv<^zPF}qWh+nU8(krD4m)P=3q(->L1MEos#yli_8N!y?WXxh_sDJ@OjA2Mf
zO5u;?)3Pjp^?<c$H0FfF&Poo0>*TYlGnkLRb+z*n_<S6DJ0z2B)HCr~&0n;Iu3zPU
z^Z4t&udYn*X-VJZ!3VA|UVqqcPisRBtWclyS5~|s1>pQWucO2TNRMvfdY=%CFL6!A
z^Ok;rw31#zt6;N3w`r><Z#7idFMUPZ2olASB4%ze7+mV?&@n6IzZlkzpuh<mjCjP}
z{9bc%0xK14geVjWWRm{26c3*eX-e!7xW&_}<?15Xu~9L7^_`n<nTfjrN=^=<EoN(~
z(c(2&s+y$2&&`9`Uf8O$5toTgE~qHzGn3UxPLeW$pN=KbQdKvu8={DqoX$b7f$HHx
z_mw^}m;_3Z2bXZFwK;d80W-MWD6WuOB9umxjd}o@aJhpK?UfUG0Q1c8u{Sjji`#al
zXFe1|I<#xGq4%a09O|@n^*x?^Mn@Obfv@GQ7lQS2HXu>!DDzwEb$y(ApZt||>^X^h
zwD}+zCSB$udIg+xqlxjJ><mR<S3qE(5aq6-p(=_5SkI;70oQ6$Qtd;^cLanzHf4_k
z)g4paKRS2caB&rr;mXZh-#5J;*tjWElUOq^$}|}A?!-pt4E4>HXVN(2rVo0wZsp20
zw9ZWlDcPUMel4cSwAEulZAwO@^vk>;S!stnt5IIgSh^v{id1b^Y5=|ht}$Z3ka!zb
zdup#b-;S6n*Tq_gmy6wCZ}ZD+vB}YNxp8ph<%d5e;}~r7WHV8-S(x4y@{X8Wq#^Y$
zg(8;)O4uTMGcADj-|brwDRm^H9!rAB@dmRRr#kj@!y#5a{d$R0A=3RK>2=WF{1PU*
zjxJM~py4TVU*zS*<yqBi4<wz^k5@_R<xsxB%>cu_;5p7lY;)E^uX(YhV5z-7aN@XI
z;?X1ZWxc*)D`}^?0&bc|w4{&qo=^3Fjo7}IJEU)%#)Y!^_=@lP_fYYv=0RYo=})Zg
zwOe*RA9}J7=?s<|<H+6_YD5aixspd}-<X{UtIC+D<2A@x(wNf7Uoqb}0$U)DyIN?z
z4hY4ay+ktE$@|H(@r#yOk;zPHiUxUdzV-1S?8xfc`V322sJN1+&$ibL+kn9a>&ETk
z+H$q;p86tu4?ROyR)^er0`P;&_4)JT&n<csO;aj{E01pGK{xS4N;`Kwll+o+%gW<M
zJT}0tjmRx5t_UULf$DFTY#o>yK3=?ua&hrq=+n_2tX8+LMURH0gkNjB{=E9jJx9U7
zg;_~0jAl4__*|RO7}ZRpZl`U84q8y-%qql5mH*f+8|d)&s9ae&N=%V0kTerA)-(8N
z%iw9AZt=JXE&~UYNqGAY10#lq-P~NGhAoB+keqv9M?L)l-Uq^t>kiO)YB}3-Tx2VT
zHr4OD0^=UXM5s6h@DwmxygKZ>3r{e7WqFUJ#D(^AsKeEy!g;f0%QkP>myW}por#4;
z?N3C!453eE>S&vYM?={{WYUvv1*d0kOLG*E*#o>u)6QK^K2K;7&UipoKbBKdN*n09
zmFkT*21l!R3>Y))rBrBBK+5R?Ut6??{DXb&Uw1_OO)ty8W*-)dM~r&PQ-1R6M=flk
z4w^KzKBVq5ra5feCf%s~xkWAfxUPonVRWv`J=)d^-s;v5AlyLPJUSocTi$h+g+Of#
zLMyB$xe`Q%ykPN+ef&B@;`-h<6YP#$H{2cFv_wMnV7Gu&z<E_N^f?)-w3!!|Lwsl{
zYB+?Rw!ZcKNO!|%d<aH4P+&O)-p}UNl@XIP`tk7S#~U^{@8If8m)I)}ty-dJ+|p7@
zBw~RGuoDj}G|~75m!Tc1rhE2kXT+#3vwuyu=GFc%;R|%6L;;Ro^+#+?mX&en2#cD|
z5TTz-$O^I?z1%Y@__nUVN_M@CGh+dhqKc^;brENbEardLqZa6UB{WRFFHg{Ea5mft
ze(2w>0k$x|kL~+#=_NL7`^l0pqt%qdiGA=E<cD7E=(d5yW=is_tHZ-RrpTsOYi)6k
zM!J1WTL`<DB^}LUyO6n7S=sPi5F=Ap1kjDvr`&d8nU_5EP!t#8KRwOZ#J$Z>+WYXt
zciLSXHHcR%owqUz^`RFse^mX3y*Ajg8rfS~403cpplRy%ZVvY#E_^5P_f$x@S{it@
zw6G>okY?39{0=&*Gz33lr6HK{l|;Nck0S5RjKu?vNMg`3KJZi@_xe-9)H=BjWM|Dx
z#(*IcX{T7@LepLLEK|y9sUA>H3iCYm-61ubu`Tdtw5gUutz6Y7z8V&dcP-tJtd{_w
zBFAI(QT+XSrxWjB`{7c+T+Wv*P}|a%BXmu#M$_S$NAl{_t`?^99UG=&do-@Y?ZT1x
z|7lkKm)Y}QD0}%yKb<0mgeXht?t|mY6jM2R`r0>-prwqF6Iu+=g30{US=q!aeu$-P
z-b9}~OYR`H9A~#ndV{`E@Ek-LyVWh+S!hhS#X{~o5BqVQ@VJ36dTNHM<@(-Q70$3r
zYsdcla&?ijLT`%B)4C^cK}L?mc-*Lg!Q+CyZWb>It(>|vPmaT*ise+OdEDGyl+{~^
zR;NXT+#rxFht->6Q?`y51}<+DwELNF+Uo`Q#(FL)lFxPBrD#+a65RFFjD)>pP+z2w
zgAkj9McNd7&^w#qR+Ujj{%n{7Q@HCaMyoM+EN!y^o(rmSGew>HR>M}qh+CEQ$4<G@
zS}JZ+gZk+!-B>HP<G?jfY}VG#UvCDOm;J+=MSCs^lh1WXP%J%=uiinq!k@Ei+*aZa
zeL%vxNW4+PNA^|KV0}{67%rC8ILfxR%QZR!)sHMJl_ch=b1S}Cu#4Pw3@{5&Scy+B
z;z8atcwZo0t;I<eWVM~QyE&Dg!CVd(3|=*eSmk~*MS{d63Pc!2=}$N6f4XxyF`1q{
zS)#3T<J;X%J$;L^b!e5VpX@G8R#|;1(}=_HJ&)xAMk_&ak394)JvU85erllJ(j3cK
zW`Ohpj5LGsQHtfO0o`jH`is^S4cV0Rz2*RU_J?U>d@?kzDxN4^l%tPLlox~TOdbm3
z<}-6Edj33^(#SFW@lQk9eY3lbfuG&hSlpZ|aari5L#;7y6gRC;U5{4pl=uL>Bu}?S
z%_cOfsywVwG<zOr^N_X|TBL&J$}WbbChCGq_S@v^XB|UfYB4JvZ5jnLCNpwH@ub12
zQpjkXcf--w;L4NxrxHo&@~N-#65lT<+G{CY6B`{hFcvV-_pWyq+wh0xk2V#cjg1P6
zB{to8d0Qc<J|U<!rAw+-K_3VH9JDa7_0D#oBYCsK``B;1KqwbpSaOs?V68Mv?q{d2
zNGfZ5P_#j7YnXm-ZbOnLUr~r>`{_g3<ESJ+AsRe_gp?Z0^IIIEhI17?RaEsnpsFm5
zhTPEcFvSYgR&F1UzYdfkb{3_b!Id;o2S%C=qRUq3`KoOdXKU3`#R|H&pKNn(k>km+
zTQp|*i8f8i6YVKn`#<eQ4>i(fZk1G-YYHy)KHfvSqPae8H-<)Vh4nh*^7p53@|l=O
zWUE>XqEi!X`-?E!lt=MR4>vTxT3|O6FV}{F+xM1TzNcbqrL@;lg-lgOJW`i2Y*yW;
z@@%D7h=sk#^;YxF-pSn=Ql`>#^cpeKbd>^VpPtijRjBhgRmRStY#>-(71!uBK15by
zsZ+`5!y5Zcw==RhA)w~ifSre@YbHz}zX*(tem_$r4CNJD;fv9S_qaJD($b81^%LBV
zk$$=vG69uIGX|^>*QlqXkVN$8A%NcaSap5!3y=|trN>lOK`M&n;_Emg8+yeCIDq<G
z)Nr5Nu!h(^@I02iik4R<W~Zk|1T3N#WkXlkDgxL&vl1ieRC!f_ziDtpEm3RZF~>-0
z0Mc|vN&b3p)=$iv_N(cq>}t7;7JfrJe$rfdQfc{>RnzOkgm{x6`;A6X0W-T?C|T+z
zd+rzAFMDy6($R*h6cGiy2GOHPW}_Vf%>p-7xh*Z<^Mr^>x8P?Z@sDP^0i6<#IvXL+
zonq8}t-I9(Ton@Kd^RRsIGyQTwM6zhl_iGb)utXpNYiX0*PLvrqS2T~Lw@kvxktxl
z@NHzrmj|O#n(u54-=bd-dD~%1R;;1gGc(Q0w)WhNtrD-7-LtBW5ne?Gx-0srt}Jd8
zxs6Y-b^gQA!w~d}Z&{EDz~?!RA$MPW{@S8xersYub~887EwyBTZCbEhrySM?_Yq`^
zAV#1C`t#u8x1NMbt-)~44BN)Es_?#yT!_Jj+(RcMP_Q|+>1BAM!aEXD$J-<KHjzoH
zE8aC)eG01nro-TMwrp*^d(NSwcH#;IlE0iK8tGY7EHNB4L!F!5v^emJfhekc+N^;v
zQrZ0OmUP~5b*a7Dk9Jdvw8Y`^t@xECBc6EeO7*x!@y9QCI<s~-w|T$MBu@E#0*ImS
zMv}fQFA*zgb=0+z2mj0^<}42ARy8^{=JT@Xk#qy3@+A2ZkvyIFio`V@pq;sgN%EV~
z%RE@6PxIJe(wH;H4_m?V&0(|8YVWF<TeUPwnCb8n`|zo+pKk8Xs{Tgj0dSxuIE6#%
zB(j=(wJ89OBy7%^BG`8xAaUE(u{*6MX<NaU7@liM()McADG7G=mD%%xw}13_aw^zE
zoGq(x?BE159h&NTCP#jW59TE`3r@ptqRhR0?T=rx_Rv#_7JV@Gd@4MN>SO?fH8lI>
zjrV`}H{xrKGW$~$B`c8KZjGa?Tt6SL@Dw-jYVg#FSH(xTMx{Dm^Q5|MXqG<X64p%Q
zgJ?B-JMIy?Evu%M{1B}LGbQvYOPqu5a>aOF%FBE9d8>l+ZG<#&yqE^H$-`R(S__K_
z`QK3c-Mob(MtZ@%qjl_bNMcn9-TWk}sjPa+vrYHpc(*lYg(bHZ(Zo7mU!=5^A;-?f
zFK1dLujBv_SXo{lz?B9}(P0qHj9UmrZROI<Sol+LC0)r@mL5#j+~`ddnJutb%Qp*7
zUAkCeqb{lSBY);6r&do8eumT8p~rnSv22N?*=`zj&C_L@w^h&GxdbV_D_tM2IU9-M
z{ZXG@fi{>)PmuvzB=L>3RFxWu9*|uO5FTu%m&hS7)SQy+T}8SUZ?H6FTFab#zmtiW
z*W^xVRL!an>kV<;SFMxQ3+hi2;P61I3Q;ZLE8G&Nns?_CGnS{vC#v6S)mJlf<yUiF
zd+y@mmFNpC=I88X5{5M`Ps?sR{5RXOUq~wZp4`k;Q9E?V(|%+Yd~4d`p(al}eOzuF
zDXWp?=Pz|eM)aTWd>bT6#B6PUo6R&^FJ{4m1<`kg20U*^7EEfcO+Ad~*ksd!OHn@X
ziNaKXi`guQ4DVbtXL*syG_oa(4(~79gK1KEdsD)uwvGpGU+<&hMQNcl3O`*Gi_c=4
z+u+g3kb1`EtNgOUS_fErMGBxII4G?_78p~HZj2z!aJQ%@;fzzQR`JhJHN8@zbK6+}
zbM_`v%o}5;a_2@=vCE|Qr*SiXhvdNxuOoX1-14&7PQ&fD*6PmJ6Q=r8<7dq1wPxG2
zm`Xl$h<@4SlO?~;$m|Yya!uZvHXTF5nwWp0XSpB5@B1#rTYABtZUo-{nBsk@UDqdK
z021L<jfolN<&yMg6}(<7k4b^k5tYsd4KUL*RaTG4l`74OEx57OR*!q8X|B@7zu6gI
z)2nU?CG0udlZtJ6yUX<6%PXTFG6C645qcF!N1V1j`Xp|sJ$kJfV<MQ^W-%d95Hkmr
z8%IkTr6x)W+r<Z9O}j;>rf<#LhYI$+y)kQi1mO6{8%b!I2^ppDM=ej$a7Mk+11MyI
zuXS>D*hK)D3ff#1&~FT{Oi40O-}!PO0e*T92kA9L+%ji!z5(&9(tNWB(A<|K`Jd25
zj9TV6_A<>UzjzI#t$o7$TB~BiRWJO{{-4=w|NqA4|8H^qf9>mjIUl`7?2Gv95;_~K
z&mPM0-e<Y5`_FS{q5%T?IViqhVfU0;&iEWupmTMQXz>3g;r{36{ZD)r;8N%y)6zi}
zZgppfD7<=}Y<=oxMs~N0OJ1+-EqdV=bkr_uT}8c@&5^^ag+ve;k=O;G<FHNI`W!U4
zxuk<99d7xl1NdZ#0_sL*H=D)IK_8p=&q0X?fVb9v?m^5dMViF{H*v3X9AXy*-K|ID
z9^`J`>%Ceom|m1ZW9-NFfJ1evXDIQfcgnSe$XD0CUA%Yy>uGbpivmZ&X%E>d;HM?_
zAL^&|FW092wR8AC<|b2khVusMahjdhN7u1kgx4j5L?a%*Mv7RQsJt5F<K4O&lRSUx
zP+qxh80uFLR?S3R(`feF$miNmdi-X+1&$2C_evY&G+f`2cW`vmUulnc!MyShwL+q`
z<85i++>)PKj)?YH2uV1v7Ye&TyTQ`9@X2aR&kM1_zxNscr{2Te*j)lRC8}kMej)aZ
z$n-rK&}9r%Penc7x!gg&JNRv(uGwT}y2BMvmfe5h#ZlFDrMH7o;?n2jANQ`uQO2QP
z1n^^bzX`XyT(ovzzLTaKX3$(!JKA4u>t6IxfQHG)8f_&WoIFQXGwPfPYY{qR9d&Ls
z7jN{+fxS2fIR@W|ZTUyvKK$(2hei?j;y`%wjfy7!+NTW0;aQ$Z+baOk+lra)$CiCf
zmNVUoTwn0^Q)H4l=tKv6mL?}NJ|xnxr^`vTw<Jvaa^wTUw@crz@?1Oz;YD@A6xV)q
zAaJI*7NA?;mtmh6&Yghx6?{XoBF{lT{eQ)tf5)RPEVH16KM4wdoV&0m9Dbms+~HN9
z8M|<1bkgsFdn9Mdo#YCC;wpB_*KJdWax00tMm;7vN~SWNm}vUopGS{>bN{{nXX1qx
z)+O9Y%sL`dBv!W$gSH>W-!-&Lmg;8ojC;$J7?*jE=hF9BoHknLgxCb|W_+ZGw*+*S
zG90oF$SZ*L<TnJa9;CP+Go<*P)duikyn7DX^8g`QJ#!F)N?qrmi%-r$i{C*&+w$ha
zbI|*cSi)tXB;4`KsgY#-%{k}+DGQz?>>LycXcAGDQW9R#DB>u|&Ox-m)ZsScKD#V{
zz;dw=gkF&S)inh0pDJ$}`K`6@5+m$%ZmcEc;(}Y^*fgs==fk{qzM4BfcTKk^V&b`U
zK&HMp&N@O-zW-iA%;7I0lmg<c1hJg#(@W^k0%E3?x54i!7-3xqs=`d?psy3Kb5LPe
z2rwRh{pJ1ygk%u=26*{%(9;3{Pd-@xHU&EQueC7z^)>%vQ23V~$cS-ir?NQX#iXIp
z!#@5;O)U=N?C$RNE@A{T=dYQt_F*TbFNWSvG(|wq28qHEZC8D`2`~M02p7zO8BhdF
ziPv(J=b-yd+~*(=;Ms*613jN7N7%B2wveLtN?|9qIi~LxYOW3_XXz&N)w<s1Uxpq@
z>c8X&d1@@;bg$8MXP%88WRD{{dWL(l#|6~U7{Q_Z(RY<5$Q>1!8i(^$YJUWzkONlL
z>G5i;p}HK=O!uNlFLi+YbntiWUm?EJ<^i*n8VF1bz|5i({iJw6I*h<xn+J5lbb-lt
zo&H<a$(40Q+!dE|P-h`9K|`-&b<QpxWZ^GjfF;cUh-QEFk#-0ygzg|ZoU8vCD%WKF
z$lLsSkY`hgp2kXr(X+X@Vkf(YPwS?@D0V)cOB)V0<nN<Abct5(J8Av})#g{6*#_<t
zjw)b*=7Y7I!0P{&F7rQhrTo)Yotfkm>`)&Tj^{hkjxDg>OqiKpkLGMn^M>1ad@$dj
znee`)=ILGt&D0Es&$9Ul#V^?Vq~3cWR`J5;xIOFj=N6JhtU}zvtZ_S~Qy(&Vr9X}f
zSug|sp)Hi(Ts!(6W1eOlw`AVSzrHiJY#gDX^G*u1>e5WSZnOuT0QSygJG)|J2Ll+K
zkuRFpHt=C7+N*wcnO0^dRU%g!6S_+8Tqe9W`Y46Om93jMUj3t-d6#vSQ1K#X^fzqq
z>f%7#)Rx(!&?Oa9G1QJY&kkBp+MX~@Npdi)UJx;C5-=?<WL~>aQEx)A7Jg}Ge)31F
zyU)!w-N6)fm+R|q9aAWwHSfQc)=yakXF%ro8Q%?Gpo(SFWGs=+`r^}GxM+OIX1?$*
zBG^CP(|-iAze6j#$+&17lZyj{(0p?I%}ceJgoI@@{e`OnvvaR!Pby5fLS4c~-?0R&
z@3X^aPW4{MEw&wnj_ioT0&lIh?srO;>^hf~uhb1as~oY`qR3Dk(Mz5%u8p7D=+1vB
zbg_f89^IqT#?~|RvSM<5o}rPkc%7D+@52Pzyp%{AA+!kz)2oW_nI-7N`{(`kiIQ-n
zn5;oX*pvD>ap(4Kc#DJ1_3z{R7VE}$aZ!_2(4&;Wby~l`FXNZ=-`zpiHoPfkb@=8T
zFX!}w+r#A^ovn(O?u`bQm`vjOSA?VacA4ABQx^<JSG2Udb3X`e-y)r$OsW>-LpyTw
zn2x=*eMxlz*oEB?!Okc-w=D2u2Vd%cxrcCCpT6j#ew=)&J=pxLB7bWsAiQp7gP2IG
zEWidE7IM(*LIDH18w(t!GUqGiXEtcwPFd_Kvz=uwY9!NR?g%xT*DPNNz0dVRz$bUH
zD<pu-1%_CZw+9bxi$4mCfK6|jJCs!xEp6><@o`M58ZqlR74rka<5yV71Dvq-!B5cj
z6XIvInwYWCrl=IZq;wLmVzj8aSQT6U`}$|?7B@Dt8t+{QSsI9b8=%nW?_bJqT0c?(
zWPXPxg5P3VJA^DaocAqf%?A8vW%DiB<{|MHLf+j{i;)B!)r0=|Y6dLJN5CE>-gHU$
zlv;fLOng@NZvcV+u^T@rod^ZWgsx16u}1L=(RO3#=?1A(r=Ktmtj2ApQ{}rVUPe!g
zzJNYo-n-92v3?G!I1cxyTW<F6lZC*dWyW__hu4v(;`8A#1qF^W#o9DuFGf6tw~E!H
zUU4OJ-4LltT%o%($BK=O7G^Qu6P(M)Hr=eRNrNJ%O{O<#<ITWtHN5)V{R<^tUfyKm
zToP>5$nPgt?XkPK{2NGa++`BjQJG=d(h#8lq!`449qX%tCR;;?sx|XOzM5N3Tg}ay
zKT5rl_`#~r^^zu2{hg=*-a7x4xVzs-bMZ8L`B~r0T`tT)GZ`8Jx%HwKeLS0Nw30W*
zrMcg^Wa*8d)BO7st)S736W(4!GirgN^N;4#rUH<cY&w7no)s<<u-%9V5I<sKm^T~)
zl=7`cA3+eUDku|^=2GSiTjJV1x9^gedsd&}W!wq6fSa6pX`Vm56{$G74#YfJh9}tz
z9+wF1c6wdCQ(kDbE@8zw?m_C{?8zSjDEN$vMT<Eai<lSwrsFt%L_Rk&tKoasID#(m
zVpf~*D`rWY&XZ??2fL)3DIdP)KB#-OsXzKSMDTq>U2TN9dAAr`bz!N=JO^_`OU{xx
zHP&}Fq~rkFOWEoww$!mN{A#w2={mhOuhKN)Bz_K}z)5Pqgd=N{R1*ETr^n4TuFQOB
z%9vqkpitq^7k($kaY?5$>q6s$MR{3GY*TXVf;mi(X1JA!TR-&}QadIm!olxcRr5})
z-XFl0$tIWLhu6!|s##9Db060k(H~iy|B6Tai#|{ti;o|mM|?$<orBiPHhzyXOt^6l
z`rNV9a>}rcI3&j+L4kh_vau-QVhP}^u$C=@J!?XS9OK{OeSiIue+#P3f8sd-t<+N>
z!Ri*+OF4^3-6f>%6#j0a@ogR-WhD#Tk7X0*AU(2czaP@V0p2zW=TGR+p$5K%kx%Hq
zO>O?P-TMDDXcyMbLDWHPW_S+l9CArZTiRIw%_v=N9|6iYj=3zfN$2%Y$bcz3!yx@)
zk2Hwc8_Rx_j}_i#fgZJ7+<0r%AI=P;Crb!)*<79Kexvky|7RidRYf=B+k-@Jd?=6J
z;8OM&Rjc-Z-COyMHrt~681mAt@ZI)omiKWs*y;B!x@B!M6RRf8{2y1UHy*=A$^dhp
zd#=H^of{<eWc~8|#jxUh9R`unsr3!s_?U76O;$(N32guXc!n?oT4!Izd|NilEYCp-
zd%&(=kzfWp%P?+KI0mL-7+z2@Qt?0tXAJbE%WIy4-W4fMSTqm<7lDicfk!J=6&;7o
z0e}WMaIS(qdqT+kH6T!^p%6dB)lN235Ce@s;-hm8N)9{nw8EYONd_AW7EKTUaM*?g
zYdp`PFu|_y9AxRX`|}t`dOz}>gZ2jr{l#-E05xz~`EM_k<)2+C%jB4U+b#khj%Xdh
zRh@&Bc0&-@+|ibwskvvKy)A2Vzmlwib5O7HsZ6;~;O;pnjtpmpph0}M#Fi7N{89kj
zHv>djoj3jz@PqzF1$+_*guUS|Src#_tUwW6XK~<+v>BKxt+9}E(5hqX;e-$%BjN)j
zLs{n_L9(rlriJ4c!b8OcDBkoOl<nQxg45fu#`ozF>R0A}>;jqI3;lD@O7<T^`p1y|
zF-!l=Z~Qmr0H4!10_eo0n#3t~nJFY7%gPJpt<5c<^`(8s5~AzaZmNnIs98|o;+Z9r
zmCm8lwW!zDHc9p-06~1Jk;+l+>%P0cdPhLOI7VOvUv`Yq>c58bU62mUHJcl6(e&1=
zJCZM1Toq69yZgDkOp5gHtq#5bfM*?^+(IHJiSilIk1>xFyxI?&TztWswDgj+zLP1A
zG>OQ;-jEwq8FQZ~15bR0Y9u{Xn+h!dSf58*l|z);sk}UIe)6-X@~Q57e6u6b%h;Ku
zF0l(6150ODDXS0pFSA$Fzp(ic{{pD!g}hlf2c3|t&*NwTsHTetfNK5~f4b=J!tCJf
zCO~r%9aZ^RDTj0P`j@U3)XMQ1a-gePAWy!JQK-xQx|e<@oxTS8+0~(VyE;&@J*Alp
z+Y(ydsHG4Mvvv2Dqpi2iQx_(TEZ?EU=*rdz+6w4_WA3>*Tui|Wkud|&{xu4SuWR>#
zP#6NBq_uKDcvrd!MCR8Smw?rQ3IImG?d_fs%VZI5dXN!Jej!n$ue(|f?rb1%(u0aW
zp8&Auzvr1GfqBH0N8qTzdPfFG`!B#Uv|INH!_Zxi>Dm=d-jeJL#X)E8XU|=}Jz2eS
zT@udm`UP2j?(+?LKZ>~fWPfh@`h{%%+a*`M_&}_`GA_XrirQqEBA<uqS$u-K=5I`u
zCEQc?883II)I?fM%2j}mjLK}@<>?OQ^61CkmjW<FyR4gtfi(bd#a8Vh&OyQszhFEl
zb`4MX)rv-{NDC})M*wpC$8mp*%YUDFpa4CqRJiCc$A~jsjMmYv1DYHp>g7ixQWW_1
zRAw$;{)wN4mD-s@2e}r92B_lh(s?|2d;cq*<?N*#E4ILT@3v$XRij#=kG(!R!&>JR
z7hGn_n2G+dF#q0k=KFjB;-%ikryU@ogA1oTHU#RHFxSt7FR$k-cC9<!0ad?f5yask
zLH&%36wiQIP<UPbR<`x;z;C}>1R;9`aPD`>9xPgiGgDU9$L|oIgDMYKXKnp`p9rb+
zzY<ydI^ZHPr@_1G#KBL^X%PEe;mS1Qrt&$+orV7$qPOJ~h^ugg0qpVC?816UwpZhx
zuE2|QpRSG`MNmCZpafB${@h>f`gbip{>PU)|GnRlS>#lO&|?Is>Ae)+7Hu8&NzolI
zJ7_2@0Tc`D-#D|TE-czcNZ)B5&+Ip_yK~Qy{l03zCydwyto0VK#9e-o)=KNuEQkZ_
z_patTY>LTvg}DkgC@saUIlBI~cKrft^5=&JQOKp&_gNr#R;){#VrbSCu~-(L;?@(R
zyyR5F63kU5nx37LMblMvayGQ=S|${!(%WpB<}Rs!pUEZdo8k#aY6{`j6bryY(7mvV
zBO|=w0y4YcnlK=7HURlH1B+~q1`4qtm<>dJLy#|bhY^-2Tuva94?s5Ijr_$p^Uivq
zEEx-jafYC5xF=Iz$jBAg^JR|Y<L8z2nqkIw0JJL?s1A9CjicM^5?Eto&UkLbs{G}`
z_h*dp(yDbBQ1olGJqJA=xQ_TP(+3I^|BF)3uL3j6#h~E{GWeeCr%pEA4951yOM|nx
z=Sv4v7mkhM-C-2xpnMp<@RI6Z+(4JwfP15fYtsq`U+fco6ly-bIe!Co*L?aDqo&x1
zW`RYXp31GOzMOHB5UC+eS@I&1JGxg9crgr6j2{4s@v1&80K?~3`AXBroe+QrQrcj9
z00#c40&Fe*^N+f)%*;C)MCP+duykn;y4f%;E?b)ExpLfw0JI4;S8_U7-pS1|`dU9z
zkIP?`)uX;jeIIliz;U@hW}T2De$c_P@LzN<oV~$v6M|XroeXP0Y71<uo(7NrX(`b;
zNF=llNcB5FEMK!OBECNWet%Mi_#twi=#Mk}F%JLj<^zj-&^Uwer)8YWA{Tc4bxXye
z?eH2*a{y;t-<h@yeou|S^e&wGl*+&<HRROa^~yx0Ub5MbB#?~l@wZxC-+N?+2**<^
z#(2W&3?}DHRgWi(?!y^V9wkUr@=0*O-ak#9ePC<1exIcYQ3>EykuD2!^LqlXe=-mR
z1_0m+3dEK2@_n`MKGxqx07AzBQ^3o)zn3}w?*9TPPj!9b=UZFI<v9KQQXnd4@5;HR
zWn~-I-JCrrv)Fn_-_$H<TlapwdEv3?1H$WXmKDut#bc5Ex)glFISA8oG_p;0lQ68n
z6xvAz)1g|ixwHbLH@^j4ucs&4Ao<f)VBo(%==-l4l7H{In3+x=1&vC`c(?76&*0#4
z`uUF7bx9X{4+71eR{STK_D*Sroi(-4xmmpzT&Bnvj@u1a`anTmL0#5Z!+BbtT-|D{
zPOO+_Iqgipl3HN4xxH=|sN2<s&V);)|5+-fBl~^R+_%evM3e6Zwnp+Yv%aSi5i*?s
zw$9?OweZru@!~Q5WdIBil{HsA2aVW=6ka+I{Z@c~X@gfx0{I>O9=Dq1UVto6?n46Q
zzN<bq0Jmt|;nMn_ih^WO2xmd#PpRIomzl-WR9@?;njSnUl25fD4KKj_upJ=`KYZn|
z<hm-4=?`yny*Bg0H@3?<f|+6z4_#nFsP1`1FVqSjY`gIWC_{yk+|EHZ8ctry)Z0^*
z(4)O>@9Tqk6LC5&tC5iox>mi6cZkMYE(93*E|HN5o^4MYeSvv;-vA8vI9R5Pp7*cA
z5i9L+9|Le76+8h4xmozBWuEJ_&Mh9Isy|gzf}9Cojb%y;+O4m)gq4;G?*Mz_#)opn
z({)ZclSk!bwvFGN%IBZM-*d;kB4zqkX!MJyw@2H-3Mbbg^D!t?`J})%`l(Yk7N@MO
zD0=vw6iAQb@BliS?cbV(Gu+6+cm9ce#nPd8LfZ%+(IHj<Xz>LO;1z!y_Q!zyF+={i
z7eFNc9fLza_o+QBi@Yp|+9yZQUm3fSnLefRF%zb9%aK;JvzE@GbQy~*alZ?1Y4HDz
zI`l!30v6aTDH2FtBK8U%wIl}cjg$!C`CXsq8l)M$xl=g@B7z@4k5)6t@fGH0DDn@!
z@Nnt(WRupDY6OjzF!zbmdt}QnT$LhFj>{J&G`DoKWG^6I>)^QuJ@<)xfb{TB)mI{>
zJu<Tv?D)&Vje3rYr;v)u+RX8p;=9QXi{8=KSdE{50r5luwRBfHk*f_K&<kcweBvwJ
z+WJwpg48)vTC?BL8)M14%Dfoj9$mA%DJ%O=-t$zRnx|=})<n3UcujadPSbFLBeiGx
zep1e;pjKtu_41qV1V|iP)jTRaPXiu`)YiXazLS;HMFdWV_A(|H-O3b-zCzziNwuz%
z(W3z_O2~X8^X62tK!9zeC4NJ;8jQK8n~2uMC838UFv(}ryz>E%vf8HtktNLh3eqE<
zr2hWp)KimP&l1*+G~A!L$kiVFCU{87?;Yd2y;HK=H=cd;ETuQe7E|{_EasM!Uqbo^
zgaL{KETy3}>RPJTT2R$c$_qhp0AlPOKY87&eX4m(rDr*c-18GsjT#Nd2vn(N=#d7D
zC0(=p9-?iY7;3hTHFKa1B5xV_hNf|C{w<TGZtwcY%V#w?-mw~z&)Xt1bUQcV9^6Qu
zh^3Eyrhh}3CA>JyWGy^Kh?-NutLV&*&X#3PdcVLt9&*exE3v}cNiv(SZGh?w>P?Bu
zE8W?bp4YUVtvBoK=IRe>>twz@DCXbtI*7HQ63bZ{vXkMS_Z1gvDXd%L@0(4l<x*q@
zohmunwiWDa>T4_Q&vs<oPJU$i=mW`#tun1qiRc#r?nuBUOx!*!+7SYYs-r6GFQw}?
z&`W%p&Z|9>M2$`n;MEVdVu0&wbadz>6^r|8sA`$5H<``)TC;R`SzXt;YCffiXHE7(
zU{gOGX=!CgAa2g>yqcMroief55cpuGLL*QvQ0htP2rzAWd5j!J9Pf1oy6MMG*eaql
zGvgCt__CUW)V1??xq0h?_NlowGf>5y>yq~j&Qh%#G^hAwt;kfo5|Sfx!A64{dDbDy
zZBxc!+n&2uOth#+X66O^@@Si>M#W9g4RoGF=C*YsVncj_9VO$vF7BW=a}dE)7j!so
z;wZJ;oKj9gLZd@9OJGHZ9voF{kR-OeU{snGwIs;{UKb8ji$4ikQN635U)@lzofaRV
zquD91Zv-1YR`_z31{oH<YND2J{@{r{qh*OERIF<1QO?oQ*}Z;^<z$yi>-4#)++Oaa
z%X{sW8VnrK*t&Ab2|k(1#MPgSYf&~*E%q$4eJZMQ2Y{A_M0{h;v_?$|QmH3ZPeahl
z+q}u}M1CSc4zoCcXH~=|%mBsj6+)k-0Q3XmN%WJHtq{z>S&q}Vlk!<1<5uon=N59h
zp#D$HD&;m;4m(1_j%d+G&BIpnUke6cYLo7@r*oaaDX39D+}$1HLUio%302PKLRRql
zA7&@+@S2+H*i1asC10ZA+q4dyiI2Pa_zqgjhy%S>y=9alco}noID+VK_Gc8$^cMQ6
zs^^9}$Q}PS@0>DV+IA*LqzCEJxuj328U-5yROy^ufe!3bd+KZGCk)0_Zc%jxff>0y
zOU}~fsquyHS9%5cWMw7lTztCcyNl<}oV~Zp2foRBYvJJTdlv`Vc5lWqSehl|bj#5e
z!k5rBAK*%~wXp+~L?Rm`2=TtkmyR`2tp+~=6I~7XRzLLGo67XHT5n&wm;W<;O%D<u
zxw2BdF5?<J-_oDLKSk)*%ic~5oHcInIH)u>9zS!Qn(8v#<6cD0hk4#Gx}n*TOrRS(
zFbF$kR~CVWi{}WkrGv3^9SYe#DaWybQP0;UC1PpgpO`Xl-ZN&VCAx<G-jjUIaNW@D
zsCw7*%zEB+1(Lr6zL|cjaK(%gvbqXYR@NAO?glB$=9x}b*lj`{c{}fFs+3J<%3^aD
zRhlCvKHzNHlf8~37mL3-$uZi}r0!Xo4-|={zxW`LEt8n)OQ}1ub7!QK=Eg|Jotj3n
z9h<xHsI8UA*1on4^z5T&vjz)1Ll!MiNK8e9N0F+b-HJo?0I9ovYV??N-p^T7nWcVm
zlhAR{z9Zk%)&LB>xAsC#cF)Qcxxvg-HFn6vxh+h?SWN6J<oTdbg~8k+(NzHRmD#N0
zj{l3RgXvEjX+ygVdzQALPdULEhEi74oOF6eT6L`(Jy}L$xvL(4|D1i#x4b3$BCVcN
zMaA}gMNoj_p5Y--1P#vW$YdJ+Qd9qO^q^ZjTJlr}oXqbF0HDgltJuEaqpJ1&jyEZW
z&0~*G*&c1#D_HjVo1|Z*Nmu-eHHk>hP_)(4E#880^OOgnT|L>QMk9|c89Z$J7gK~P
zq!*05k2fAT>@qw2P*X!&P-@BlOiwCrUQ(dOdyePT4Lcvs)2g7REv9intBc%`DzE5c
zYA|mt*&AtwdMZq2r<Jc@lEmBum*YFU1wZ=0vwgnQN7<OsIZ+ny^itBH0U`2}hT!+Z
ziBryb+`O8pf>J!SJX4T3?UIDZB+cRnXNgUl1(n~2!L#1Yy$9nh-0~m)33FB8#+?F)
zFMBKE=-LF(T~U0${C5iiikgk0=b$?{#m(Q!=Lb?G9sZUo{lB{)_I9VP&9@y}5l^u0
zeMvC)fgVgJjjmv>+s1t93^<BnQ$G#%Wq;;;4w{90rMl1+)Jvek*(}W8>=OROb5ND@
zh)TXQy;rQVJ|-ohO1`)MJ-*w6;DqofS^{1A*vR@V(0q}5Yd7z^0(}f)hLNSHX6hW{
zo8|$kXea{iHH4nCXg2kDnPk%WQQGn={hdqG0Gr4zKo`i-*Z>^mnz;+X_*afbD!LeU
zMnaQ?gG?j#n}I^6X%>~@cXfaU(3%XqB<bM7zHmni2`=(J)~LF~r>Exz!*K@-`9ZW1
zzpQ_SA1h}6K=bk~3xT`*>bp-kLJaRlY)Fc9zb9)^xQ6&lquBOSm#F;D+mOGczFqji
z-PZDx&+6Ve=!1Qz;sKN03x%|Q`ab?Nh-3r1Q|<(?%WK3I*`neZKXw}wI1fPQT_x6s
zeTXCw^e<2B19Ux*E~WwI5I#ih&Fn**r$Pm}ID}zyRoYXH6$^6{3$4qU7Dams-FoKF
z!oPoQE9SlP!W~Fl^YJRc^q&7!;-zh6!T0<sm|t^gw?2@^0Ttd00jxmgiUcy(KaTrj
zT>h8`f7~DcKJg%+_tY*xXhO_7B#5V}eh(3T=yPXSh<<Aa-YE3p(cKHCW)p2j$9%74
z-@Nj9M%U95whD3z;yVX@5^JXFT|!U^9=%&aW2x~si_bFb*|`b|3#Ipm`3om>YQy)v
zB&R3A37o%KNQ2J$&k|J?7;zq+fHUtimd_J=b^xn|{gL%qAR=B-88E=UnuXJOzqx!9
z`%l4GF02!A5ih%R3fQ~DJ?Xh?(aD`EVJ72(k0q*tO&4g!G1zG~ZT6_XO3r(g?CPe8
z6x)MeMCf?<q!4zw^hFT|JlYm}Edq6G+7M^S0F0j{@>xF=P<QR)-zd$?OUm5nMwVNO
z%M-ORGuB`OgJl%b?Img=Ad2XjnK><Vg_C^W^2khg)IvbF_0uaVt$c+)_{Ngi+)9m;
zz-G-RO6ftm!8X)u4#f?aVy7t)U9d<~ir4h%w9{;v+n;y?m-_IQNL=&IvSP@gJd0Kp
z#fu^9@5kQz@9s_irggO;KPMaVUiGovWs!S#9~W=vX5ONx5lVvgpU8MFfF-tj1>}tb
z4_B>HVZn*hXEQk|>E?KYrJuKxbFtw0MFEQP4#SZE9dA2(bG2%8<H$FR#;`b)9g|`>
zDlyNh^kkGhnLq9M*BS?t&--3&ECScFjzm#m!F9fXjX-5Wq@OgOSh?2xT;Ae>Fs3iZ
zv>u#uc9u7`{cJG_Uzgd0tY5bomlItm7QXuRLC!kGS!Pf7Wmi`3xEI}=nHr`m$!brn
zG;F<-jqT#q?Corp42$I+tyonxF7M37TXV!~aOP-mB*km6=ZF{#M|r6Kc|`tt*FyFR
z;dq02$sG5;*!%9drq*ojAS%+UfFQl8bO^nwR1rZGq$9nAfJ&DViu5KZpddw%-lRk5
z9Ymyqgqk3|1w;rC;<q_>=FTaebMM^pyYrnH|4{eN`|kbjx2|WcXRQ^0<c09A7c*#W
z^m{c8*;L00$d6=1ziO;%2({-FmcN6|YsagV*kd`f8U5|;cTiny!pG*txf6hqTAgm^
z=`p_@X4pk*b{Uf+WJ#}Eg^nS3KZDd$c+E5043jsW1Qfv%D8VD<Q;V^G6Toy{(g7rn
zdw%SIRI&{imWh$%IP0I-cmFE(vkt*;Rd8?)OuJHxd5ltn`aD3T`q>U`)5?hzVMW?@
zlDUZ;_4?YiO=^<WG^<CHg+Fn)IWpi|e!aI3&;<jb*!$?FY=UUFREKS35f6mX=qjzr
zttBCk3tOx*8=27+sY#KpA10{`f&k-6w_jH~3$d(_G~gSPL;eM@ZA6?pJdsDkoMI2D
zHk&sd4k_#*NkQJWty+g?x-qb8pl@rxa*6thydW=uiAGO#!RaEZ6c{kX`jziZ+z0Md
z*H*%Zn?B>)&p{^HZwnSj90qO%u|)-xsX1xll`R1@(sah}pcVdiuzA1~cxVfl)N{K4
zW+t&c3x*wrGZ@4>$=_+l1tL^&7t#T;qbmsJ-~%u=Gpgf&FXaI!k*Oe-|BYwRxM~GA
zN$<>^(qk`T6@h54A)&V3_7KvXH;(?tuHzzo9CZz~b2S@R3Q`DWc|7P@Su))zPSR*1
zk0S_H6&;QKwweAXX^p!Cq4JCfAFUm&C?~P?Z3^;@W(vd42xUtk1f+c;+6a)(g^^Pg
zm)+q`x%Q6rB~Dv59+b`=36T|#{mH(AaF=r1)4v{~nz4LXGk~>^0KBD>eWbpF6w-Ks
zsso(>&8iXbTK>l<HZSWo&k_P7ZivO)ZqO*ecx=wvPBvHKSTV_c1H08;O<D*aWxCgk
zhh>K;5{_S~-6cvCWdUav=XtFY^0~to-v)5@D<Iqyh8mFM4{%l|fLx#a6r@m%I|~qI
zPXK2N1!G{dN%JS)Yz|P^Q-CdLO(-1ZX5jzp3IFPczh^!ie%&t_Yoh2w&vo3zJm^YC
z7`6^?oh#18^2wRq{W|A-RQi4ZMI7dIP_nqo8L0b0#$EieqNAUJU)uw-a~TtH^5Ro#
z_huVb;Ex&K4d0A^dv2PxK+y0W_Ze7m)kBysJ>aLb$M_dcr28JOFNh_s6!j5U&ML2(
zkB$JJe!tLQ9H|02{rW@?6Y^I~u>a7}il9}YiXfu0`8-w;oq%ihf@B-QY#_bMrdk&p
zjQT$Xw)*rMP(LQ>5O{LqMHIf-pbv4i#CRxfN&W$V(vx8O2RMZ9TjU&~0_oNmCEUBf
zLd;S0@FLXZ_F0x2q~pqdIG?%-z!}&QU4aV~B!bN!XaEF`(>M)9ezdT`MWhy%c@>6D
z07znDv26oKmrpIjK{JAYJw5M~Rb7TL0G4l3umai}P&fhHIu`V}0<pr%6Lc<)+_f2Z
z3)rt1x{<agWTrHL;JcOd6C)4Su8#$rpx*WYmS?$P;MBMVh!hZ(;PgEy&wJlN1aM&c
z1DJgOpzR`v^YJ~Y@S#S=o?U-h6WCycOi8CSu&)udA8apJP0~?!jY|xsnsmDS@#8!m
zy$b&uyksEq_P#Z6+_l@8O2w0tV|B}&$=52%m6C%4H6ELTqi&QOKVC9HRFo0D^&5ZP
zCmSaSPyuZ>ycSFt=8&+E%tTx-6F5&dp@dCON8|!)Eo)HgSu1A1doTTuqoMpdA`1qf
z%uVgsB;<SE7#{z$JB;#@y_tD7MhfL&dGeOBt)R@}5#hWm4c?wdtM%2)t_vKwrZU<D
zB|q2M{F1BVvweK`vxzB2x$8C8s6=nRJu#?iOpNq$&&XP0Vkt=t!jd>IBbhgx{biBM
z1_`%5vwl_!LHy^HCPB>AdENm!!yahE9S)7ieqO^du~<s`9{%h)Gwu&-U+>57j+LMi
z@wp~aWHlDEQiasvta#H}D6??Aqv-T!0!}`{_Jt0EGY-5yd~TOEp_|3$-aT1ewL&Bm
z!GnjVP4ekbM{VNODhZW6Z^HZuA^@vH6ebuscB~nJ@j?w(Ha^+DtM2&ijDYQ+m>PJs
zsa1OI8e5RI#1&neK;&L`jNRlnkt|jgQ!LAld?`v(eo28^?9EY5Lf*{kS55P?)zu?j
zWa7O{l?M;X+h1Di>A_R@vfHniGk~5p{+5IC5AOfU4B3f9cEIQwcndJ>UWhJ!zb<dO
zS|uv5hBS@seZIG^uXCPhu`LF9rf+N>Q(|RLUR$dB@aFP&l@x}KmF`cQm2r@k>l7q@
zXb;iQpI4!urT?nPBI#+?`W-}zBeK&or71HbS$kwpQC#UlGa5HTlUnrQvj1lecQV3W
zdV-6GDiY7$*Cn!-NM}2gB^bB>Np({;J!iXXw3wuY?q|^>uFb$c)&o2;=Pt8DG<pDc
zAMo)g_^L1(#d301?>p!N3>)6OAFZ_*yMQzQb+Mn93WM2=;`DI}ODyO_EU+BY$w>h5
zQwwnF&N@*88at@$jLkju`~26#{=H}a)o1^j8b4cgf6nD(BmV+@(49^tYm)1k)HP)9
zo_NHR@U2LAwQbwGac`VTRLzNyQColhTx;K(<(et0LoQ|}T|I_(%bRtA1AVzXV#Oa4
zv7ZihWf7Qgi#o4(;~K;X6sNkn6zSx+$^Pt{(wU~gul=`v1iF7Rnw;^Momil255oz1
zKF~!geXWWzJc-&Dd=$oMm*{%sQ=Y+1kwCR`{sV@f9firMeGZJ*u}D`u=D{7KXp;xz
zg=U+z%@!(*R@u7z385s3Wi3&{4Z=(QvhAn`tt`RfhHy`JO|nskdqwxahGroG+v7BK
ziE7CBEvTqQU0}aW{MG8l>E|K(i!Wp-KZ5{h{_6-=KM3081qd?$39~fBRKBihq%vB3
z$Z1bqhquQW%o{DQhEVJhS8je^Evn3_Vz=zCPMr5a?fO~&r)iPL6d0Ks^^w|DBjDQH
zDz|tZ4ati`ql+f=N<`&px-SFCSqBLw3~6JMFP}m1LCtRATiX203F4P3F#H!gN&c0B
zVbzxEJEp1TC7<~suT+mhAJ+S;y$$SPuT|$Dd;5xaa~fZW;DzEP>4`Q4BF|X$8gx@}
z=BmUIbDB0{bwZrB!eHvvP`v25qBt-m{m>+JpE4rqL+#lJP;dIYL_mq_s|J3FP}i$q
zUW4Q%CaKDLYHOMedhih96$v3P!J}b5^B$Yt!x%+EKYULHGrnHr3}I8h6~a*lD7pCg
zy&L4pSAt%<KF)~ozA&}8*4v_1_~q#3z)EL=OECfF*4c~`>H_lJP}1f%J6&ZQeT(c~
zQE}`lQHQ(?1!n*s2S?Y@Uq&SS$$dltH?FX&AqyN~7CFGdm{VWy+pu!-FpLDCC8mq{
z4%&cJeFu%+Y<v0JXb(O>7(Ru=Ykq*%sq9`S?8p@j{cY5Z{}817+(`KE%{>dHs?jDN
zCBipvwv9b5JR}0i53n3O*rfsarYUTdez^iVmW9oj^=RYm19m`o^YaY+jh*ixHyE2D
z!Q=vN4R^zzyrTPL3>yq$`SAnrk?C|k<~wce%#9h%Aw}Ru)lOlIDPlmoydmvqCZ@nO
zw<%fwn~Y>hj%|cd@UUe~wN+Idm-NdJx)^P7ULR4+&_bh2`;lPuH#hWbp*<=zahP9F
zy{bN9iT7p5W)_eZTc<w6r_Tzt)ic?$ug;k<duRN%lj7&aXk021Mrx-(At1JCbhlyW
zhR}E>ZnV+PX*0VrZ9$(_PuQ5#5RAAIVPKJ@9@DipuwjE(vW-{Q@HaY1_Iczg1E_*X
z*Cn3rCr?PrKzhAV^_n&c8@;=$`G(oc;T*vw#jXi2wA`j-2RH5CCEjgJZIOM^?MFQ!
zOZd)#*08aw3R={M+^*Kw%JrNuZ5}D*svZS@-g4%S_v9Dci_AA%!Xpufy5v+99jN75
zg%K*s$D-%_oqafvnidn<W9jYk*T#?(Cy9W<5FS!$pSv|GSPZU%3RW2`kMhTqGTwBW
zP-BbRy2y;^n%$zU206N=S9}V0K|*bf2E}T>F+y)#aGi>SD1-@OHFG+h6{d7}%9_WZ
z#Z~kr+=;}oHAd4}SAbaW6_uLRLgMnp|J`B!{dq#fs<cv|fCt3V8=3kgF8RR~3(;yb
zagjMDD-V-%fZR^MxL_Y?TPoHZoZiz=nRc?}=2%^Ll1ul7r@}2+<{pe^vitR!%DZRq
z%<i4Ux#nYUA?{0`kC-K@AAQkDU4bybKgm5;SFH(FVi$?@%H_>#I5u%-tj7=Ip{yl%
zgC)htpwdB}OWhqKb_@xjQ|!%=9ds12=O^vAU0ijn5@+SEDUDY#UfJS!-YH+>=8@OM
z8Qx!^AvXQq-<KRcrN2YvIbty81|wdV7Dy=N3hegwa6v9dOzRsN^F=>Ne7ZT)VsI(J
zLa0@$&*m;w$V0qUAf{#$k15t1y=ThbIC3Z0IZso@en+gO)@y_IMs-bTm^npCPj}jL
zaxkaXrLXB8Am+{=>mryh9^dhFpXy{G_ql2AO(GFT%DwgYVuuFUM#C<7iQVGi&5Mbg
z)(as-s_gy>60YQw{kA2lDDX(-Ho5UCf+jg8uCOG@nr>eqxT;_ER%IFgWy+3tfnW<U
zFYyokBo}bd7CQ#lN8<8D!}5GnFl$WghM#Pt(IB{<SmRZL9yfZ@SN_OLfCDPG1uddh
zO8Q7KRjw))P#3ysZRH^$Zo^7WwWAsx8ZCcG6;-%RV>d@x<>l-nz7Ky2votb7K$;Qa
zFH|Ac{O{9HpVo|Q6J-+MiAVF$G}O838WBV7^V^{~ITI5f`J|50g=0z-sj5y$7t)Pq
zsPb6u{PsXyqA!;-{~iRhX>H`mT^)IU3|gfj^A*a}|4umjiz7%<ixlg)BUC$V)Hz3y
zq&n#4z3Gn7G^IgLAGx&WcINcixNbUWk|DaD>`rS!<=?vGBBs=(jqL|Nb4|M+=-`#D
z`blAqiwr%zZ)uoOA@uoYwu)GH<i_7-u2eH*$D|7d5=hiMr3>SG*8u-|v-5mPc2`mx
zVrhS7lmCoS4;~OE)9%p6D{GZ*mS?9pv>hV=)=fE9IXAozdUtGi2{pSvxjepUT@!mO
z$5x%VpcygSch|jX)^J=hiWOf0mEJ{uX(d;!DyY-YG$?m^g~Fu07;+DR(G`n4NVKlC
zgaVF`rb-kl)#a(umt2ptBi28D<xzQ111jDb=tyIWUvKl{+p%5_a;mHol$8)j_<{*R
zeVY8nQ(x(O=jg*FaDDJ7o0SVwYHuT{bxtohgf1SX)54PftHt>LZ?H=r<c`d~DWFJ%
z-eRGe3P27}7|mRFKWa+KU9r_k)36QEcr3P+^=^&&W%zs~F=O`<`=O}U^K=1!7W9LC
zBXoMp7xl_MJ2!p{rk>>f#gI7W0yev01-4SxP4U*nsB2F~2zh)vtzHkZXD~A5WTB~B
z;0*^gHMq-zB_(ceqi+~g)?eamJfrlAxW<#%7!Q26?-=zR)DX?G&Hfy8+#Fzfp$Xz8
z;3N0;<B{9zkxfHQk>2RW7o2fbZ_Z_K^WM8${<bF8QJE|5micE2l0vh|3glRD(UEmc
zu#XgJ^iFsQbVPMz{L)9RWNNU66V*LxQLyq@l=Z6;LNFdg^CS(6pJiaOf`ElIDA?yZ
zw6XcxlO>olHGU5G_&oBJ&a|pzaIa^>m@|K`RqK+!^uSrHBzn7D#^XIYBq`U^Z`@0E
zk`2hou(9AJ9{iPA#&bc>x1g{w0GdHEPlRt-`EzT_UwvtRDewL83O@<h4ff3NIjJzQ
z+T}wFO{x6Q<ozW^>onBu!$&u4T&ZqjWY{%7eU`aDKZu7+XIyClAFE<H5W$JXC#jG2
zlgupYLKZd-gujD|YSMc>c5QECchswuaa=Tbqrb2B`s<X&Tbkv-Xl7zcjo$A#WeA28
zX7X~SOszdlExJ<5-pj6T5Ah<+ZoAz}pX&55!B%Y{=HX5zD`+qI$57x9P|ub9^F}5l
z2ke7(0DTJ+g!1!mqD8maw~7(JqCHwWLfHxMZR#k+ZB=GnOhA<s6W--|w)-q*c#)&F
zBlwIqL4@Z$MNBN3w>`0mWN^dP`}nw-lD{}7JLPzK^`7j3#M7FvD0jG>bXg$B^7K0t
z@8%89Bbf_}_?BD04i5RZsYe2kRG*rL^Tg$u8X`3Or?}IuOcA_AY~&J@bzQ+1#nJMF
z0(MWHT-dxx^LNl?K+*k=b%&Hmv=ecsf-Oj^v9kUpX(wCSRr)Ke)K!<?SOvVX<a|0v
zaC7e$i|#bOFrpi$ZVLpZzdr%nn{fMWbiIF=M1Vis4E~EZcLpv38Qhx##emF|k&&;g
ztA!#9pN&*q-Qa6D$r0Qny+f?-I-_u{m%|snHrE%Tw)DbLhvWTut*B;u*m<l1d=xzM
zbZc#R{kjotLzMl~qFR$%&K3HV=NYfkqK%DhE{fYcDlg31w0e}m#2mU~F<`bM*Bw->
zW0zA_M)!m<MN?;3^<yFP!(-Ewa|vU$WN&k;nqLp=Sa{OeKLvH50gfcWThCb2dUSAc
zH9YAEy8PKH(FEU6gF`TmscXCD<k@5=r8-F6$tJ*t(K_g3P4cY0zwFmGmdeiMSgfVO
zGgl^Bq-(1DX3>@y<1vg<zo%T)o@q4O;zc`u4r@o=^X#P>Td|goxX#6X2Pcr6L<^87
zQH;s*-UL{S>N8?h#M5f3hqIz7`Glt$9%En`_bk~hsQubI)sMkc$eWirhb=itBWGza
zbI4{o{;4S~mxr!}=3cJJ)#-UF!`*gD>8pNv#Sm*=1o<_^3AN}m<sy{<FC#!bXsykQ
zNUbo20#Ar?GIp)pNNm0aKJ+b;mNv`IC<!t}>>4v`F~y%7m&luIz)|gGdiA2B+z(@l
zkVoz{homuLf_Ahzi&3dOn&XH84fJq}8?g)|i{fz+9p&qV=NV55^aUg?Uix$n<gI`e
zb#5;`y4V01*8_bO07smaQ)3ns12C*^01*?=T7VjOR}L^{^*56>ujrwdafEw-MNjqA
z`d)*aI3b$73|O<|f&Hpy5Ac%}#{%Sww?^&h7@iTpn#Ft?TWBuSY>(FyFlwJYf&n=E
zl($PVg<i)6JCniY(*dRP7r@w!Oz~ra4HhE@u+)G6kBtS)Y`<w4{vu0w6ZN1xxzAm6
zLmK69<tF*D>Wv+e2P~80iTi1G8gpcoe$H{1{cMwxChKK9s;8Z#@GbXv@4W0k>-4nz
zDz+1^%>8Fdlz-Y~j+sD7Y{c54wkb_LOS-1o=tR~@Noq7uW<)njKE7xtai1!7n$Q{H
z4!Oh@!Zf~nlC5X$m1&1vrSR9qyglZJ%u&gz$U*@F!@2B`!na;5=m_}AZutrf7<)Uq
zFo&J?0`c54CuwF?UHOgyhGXoHKF%{OHn+_&k<)fAXheJ8@XV^h4R|Y|H+Mx1#D<M~
z3Lp7svT9^iSQyhYbADQIz_fx^Z7{YdMg=Mph_pZrZAzxYfqPBOzU6K8(ljH89sg~;
zqh~cx=(*yLdhhw$<dfuj%oH{z+;`P!66Y^9li(OI?XA*XS@{m|PqF#-h)ryAbbnP-
zsE57uN5{>gEPbbUJLkUGQ*pf!Slf>WM_D6w2(HsJUiM+Z_kp_j7MnWwaSo5O&>9qH
zPwd+Sds}*dBF4SFy&Oxv>@dv->LbkgV)nn%O@>9+Q?i2|%M+hOH6t0*?;AzNk{j6j
zaSU+|IrBA0A>U?_7B_UdSHK`<y<EeuSw2EaD~lr7DT~hDo&b@col<x1EZiHHwJe=1
zT$6@{@{G9JF$~-uN#6aIBS9%-1f#+PimS$7=@>SdUERsSbchE@P-zp4r;}7hJ1e43
zf<q&>0SAAv75?XDb6!G<JartdMbD?YNv<-KU8UC_eKn@B*e8r{3IE0M_HT#7;S1y(
zHbD>wL@V0Pazs|4$f7xTs=o3%2y0>$3>EX%B2QtQs;km~)NL;LPlg$4fFD5aEt|Ao
z8@Tpfs4UFz8`X5j(}1$C`^Co^4nk|D%qi-_&Q(kbK#q~}Xl7oBWs$OPj`4Ns(0XaO
z9eS#u0SD2Pj1=Zcc)F`eLw3G<2zO?uu{m~cr@*lNvBfH1^#N^CSlKjUx|Ks&u3??A
zj}|j?hxw9Cpw(5ehT(S3Cqe|ar$8O=u>RQGvtAwj0Pp}`V`git9CrN41w74nW5-Cj
zVaC%d(CdXXH>%*23sBaDmqGq+uM|;_bkrN-@a%3Y_z-bHdg;{#$u0S@PLSC@g6*D}
z?N}2x0wb;34w>Z}0T0l6I_w`Ev>Se;-A6;DT`M%faJLW2G<UiBWaE1_@-N$Er-tdC
zeSQUh>*!bVGaxws9|pkvca`T4^`QT)n|Li)tsUq@fU8U+<W4z!EVnXv`lcN}gA!3x
zmwU{C_6S?z6~#GD4KJGED_7}0semL|F8SEN(H_q#=jcDLN~Cf7YOkt2EQc^kT+df`
zruxw5>sQt#@u}c>c4sfO?BiaM>r1mrc)eKFlOosHXL{~*%)%qvu5ucN-T}_pb7mNa
z;dy09MStp6i5tc>fY~~X(>`CW@AdhqCsda7=jvb%{X}LY_I?UOMO9Gm8xy(5SCTYB
zX19!tVgaHeVe#<xK&?$ZxnpULYM#=^5OcOlyRFOk(+!tU_6-lL1WWdgH_yzg?etpb
z71!2AWj&<Y_mD`8e-|05&cBrtRS4xP=gkDUx&E++f8QNtcRQSgL;?9wE@7-XH{a7m
zd)Vsre8VuVa~WFutQ~wOIXK~@LS^lyZ4XseCin>(0V}hN<uQ+P7R<Uj2^C8-se#NM
zrPE5V9$Jc{yO&@MH$beb)E+yz2jBA%(0?o=YI4o^3c(=r8MIt;`=+eDdxf!Sy#{}0
z=gdOk^o;QUhlX6oCWA8VAyW(${o6nqLed7|o0lKw{dFau9N&TU0vUEr0<fyzLD_yx
z8RYeQFwD0|Tpf^(hV<I6X7}GP$!XZMKkr;`^-)DD%~7~^53!DZBPY-9T1@tpa@|i>
zGnI%@V?0)A*ABLPbjW}Cs*trweBLxYe)tHEfWI`Du_XyHUY2i^JK5ChD>=N-Sbj`x
z)N8x%Vw+=#Xr9cej<Aw&Pf}5xt~qZPn+W=x#nU1#eDg6&Sc58aGgPc2$VhZ`*A(ob
zL8WB}<r%RavaERQ?ASItryfiTafO@IXvuyBX`z|}AIu7lRGtWIcQUg^n6R&y`c_|Q
zkQETfjj>Li+|u2nt2#6Gl1~E)aV&Xv^3GV|BS`MW^%0w)4P-eSo_;_1?1cjl4X27v
z?M3m$?`W%Ar)ooH=}#!M85u@@Ji*aTr8_Ef^Q9f7RBH^l)Z%{gBKu@e75x2hsUUQw
zSS3$@qR*K+vzH~6(%OQI3E<-bq;Asw`MJZr9Uj~TF$EkD;R-7u5d>Qv=}5=0jsnDq
zPC%-io&_SUr)yEc=7Rvb)dCPGtKG#@0_Q8M4#>l#fNUD|6Zz_Y=|RkR3oPN|>C8BG
zjE6S5F|C*>Co~&!cSaMIGc&1DZZcyReb(WClD!^2b$jamvIF%981Kj>n=J;xB});B
z%_EKei_!FV{`u1`kRMKxpWUQ~mjce%an-9H<X<-xFDGHIrPAR5+nOL6z%3W=M1BX!
z!jdrB1hGDDl_NL`&!<{62Kt{(vWxrQJzzl?h7WI=hpa8h+<VN0_-yt}jWbyy$>&-o
z9$(r+tNYLK`y1GH2yQx)^(_oAd<LXFI(FKMu}Y)Ho5mGs5|C$U_7N+FzWoV}g+@XA
zi=BC8yNl}QW~T=UbXht|Lqv_TXDVi=PcqG}A!g^)#+Do4%xbmAnyJsEU1n$X=?$z+
zZjH}E&a3Fu@I2}{?^m%Cij)YA&YjvSVbi=buJM@@_i@aS?JU(Rfi$o9*33$AQF6h%
zF{{WIntp=ubL{?dp)ClWXHD#%NiA>0*j9_E6B<QAQm!t<b6@3jF*Kc;no(R+aqlOh
zy-|{pRY*bB{Id=LpczrF@^?_0LLc@DzQ!GlZ}uu_6M{b!z}`fljj<b`EPqKF(Aq!e
zw*0dR_WOolf<){?v`YRCfhrLGv8ifkD{;STW5COjAM06GAMuT2m%Vx96<zX$AO!>&
z`&Sz70|O4wBGwIEsJ|l^1Va)JmAXb#ts!OW`@;6@llv16WS2&?V)d#adWr5-h3ej}
zbjxI19$#tBG-aa~x+LQD*9X@PWtp#xpz13UmygLd_f|JbzSO+zJbB}`<3h933&H1V
zTPu3zRWSZV+DnmH6D2r#&v=JO>vHn9XJ!a#l?qGGc9nFOne_){W&<AN*gN~}&S%HR
zhgxm=_|k3F^)x2AUuw}43^kMbQ095^94k3^Bp!v0i4h8p^i;%;^x6+ANQ=*Tdo1NA
z7@$~{ETf+}aaoJJXJk(Ou~qGtUB{w;WdyVik=?L;F~oP!8XNM~k1Y5*EZ77Eav*00
zs`*6PQ~b9%Gfw4!UrGo6z*qPmnj|n;_t6T^s>~=bZpB%nt|{sQ3lfQ6O|Dmys9*79
zt*cBpxKTn4?eI1(tX|ZK!u!ho`dG&-+TJ{QX+(S2wQ{tmN(KUDSmM96egA#!h~vDp
z8pF&Nn~D4IF=+4`myV6F7#sM70kK+wCVi}vcch7WNhPUMyZ+#O#Fi$*{3?rMZPhDy
zc4yI!!|<w3H~i|@Sk;xK@VDliYSz`%{$8pP3J$!leFOoJ-xQrY>*;c<Ch1n4#4J~t
zKEKo6RNso@Rx*pAP6(>a$nDi_?$Nj53}V!+&K?lfT1B$weq(UA<`97fSe_kXJSoGd
zk3=s?qFb#x_O8K}X5|CgVU5Bf@%=mQnoL`T+$j*ThCz+9!{OQ#WUB27)t^!qxGMmU
z?YVF_DY4hi8SllLKZ{;><4*bFThr61=~l#iL1}@m*@0H1_j2{%bgz3MK3>@rrhC6|
z5KEph<(Z{ZSsx%!YBQ2k35O5v^=@o!Vq3);Kf=4pa`i7!W<3lX-(TLmpxl|CHhQ>+
z=T45W)SPfEa$_N|e>xDUlAF_A_-e1#vE6X;)~7i2M+XPvLg4sbnw@qiM6Cbg5@8y>
zRk&;$Uc1lrlN#(*^hf>9Vxw7vnS-H&L#es<V}MeV=sqm^&SGiKXl$<=5@NT=VqI};
z(njAC4;Hz>39@NEzG(`C=bbdOuX+F36Nt93SFA#M%Z%7FNGD{Aag|4uZk%7B*md)w
zMF2G1_YVF(tkP?Ocrl9$<eTyzt*L+7bWMN)lh)H|uFFt&J9IlTyQI)w?oB@>F>4Hp
zC-{S~9ud+wflL2<>orvxysw?<8^K5@f1<Cn(J^nHs}U0eZ<w{eI^Dk8u&LqNRrutK
zV|6Es2z4%3h*yW*3&RGl8+f0Md??a*O_T*1luJz4;QbBa&_Ye-5JCQ8b1y5}{h^zg
z?YY-vv=oxR>T8M~Hm_fgm~O+1huKPQ%ZjoDxJ7#yj_sv*M-tMQsJ;yqKl1M6l=ePJ
zSFVZPWSS4pv5A_MpUK>$Ow}h11CvjF&>~!k!&oZssD>9oi%R__EIGBZLd#>RV>edr
zR@zj*xP4ciy`sLRPQxt(A!AAMNX4Ht_Uew{#t;gkK(TU^^z55PSXNcKcKz75UN>S5
zs10iQo)M$HmC;#r=Th+F%e3MYotJ^cWS2|InxI?e$5#H#^$HscPPD@dcgDXJOBlV!
z4bBxQf<qhOxY6XR+dg~nVJAlpk>0b6rk_3xA!BF)X=6Z^te4>A8(nGSmDN?~<!~SM
zY~x9zsua_%Bc%g6@jx#p{>`o6{a81s;3Q|-{M3wZs&iPHx!m+|Kgi86wxxw(d|7b<
z&@_|J85*eO_Io+YrGD%D)*!HZLolYsvQQQLzL#&Csjd6+!X`D_^Nuz#P%%ouKey8$
zRC|X!EUj&KmKdtjq4kizN;%kF&DLyVfh(Xs_Dh2%nF;sR0Gr)$6<b#3lYcFl|GC5Q
zpB7QDa6uTlV`QBdq9JXwwBtoSGP#~lyVc$h?!|5r+9cgQ_Im!B^dv+1x?RA14%^3^
zQ5OE<%}5ih&Z%?HZNO>K9CqjeKv3T12(vvY`*h50;Aj=F1B@-f9y=PiP<~P1qzwj0
z`27GXEx}y{JdSc;NTFtI-6>+3>l8e?TseT{rUP7=z(A6WdQ3LpZIt*Lh<Vcn7-*IN
zJIezYP4mi=5e4)sGo10Mk%ed*m2^U8Xp5r;B3$+*12&dlH|H`PmK!IAF<OY`ZJQMs
zcl5cL3pu(|=-#9f4_;+UYg<+imw)W*&Z$nVoy1#3^zvTfClCQbZ}@PiC<x=PkP6cL
z!?pF#Dcdb+{U++U#A!F-Esf97x*DaL9<*O@PP#wLpS<g7c6)Sz$|#>`*x<uHge*hi
zxxf9caoH1!Ggxf`4I~TgDo$#6vq3piZGyom`;4(s&yWVS>>Y`#LLcM$FL4y8Gf1ah
zx{mUfYz{SJ^pV-QwRNn4#!i?y4>5ons~bnE_(eQ#7Z@tcIOrwDOJ2jLW~kNPNxa4t
z`-q_ECq{=K%x@%p_nr?Rf<J^&0*TPfLhbOkPFQeq7T-Z7r;*oh{h?g_e@?CaZ>g03
zQ+V!&Zy@-X4!9|Q$p=znx2Dkp{xWXKpd+W#uEPHkt!lg@mfOKm+Kyy$U%nNjk)%#{
z*TmD2FUC3Etu98&=Q<5)EbB)5#}Jvg=M0lZtRSBqw8H#S85%dDR2$oJkSrKc$-)1r
zxG3_33=(|{@9g?X66wdMg~l_Csry5!wt117yjnurfnQD{`j<efI690G3iiwmO5Q0w
z{$?T_LB{}x=!uU%)vTI%`82!D1YGkp2m{|j9PvVGH#?udFEi+WrW4Ll^QyS0g(Y+E
zv=>-XnyYCeT}rUZXlQZt$!z$Q;RdBSyK)VQq%-U7?KXnNf)5euBw8<;uhl-P4+8n}
zWcys+p{x#zW%A6wt~_suxrrYWB!7|TE9c-jK(+R-pa}mnQ6lhxP;KZ|KL&v;&g7W4
zrL*ft{-dRWVr&)_;66*sT=oL{acSQIr9`+d+`k$bN#Nnm-$Ic0E3y{B0FDEoQspAI
zk1xssu`6=5&JX@3&Fg=CE&OnS{4d}1=|ykjZY2TqoJ+{q69R(_@|=}lQeN=C9`C;a
zzB*-o?ie2v`yF%(E2GHbu>#~?U#B~9F<Vq*xc~Qk^Per_Cf+U(v}6)?6BApgiE`Jd
zhnj9(;LoY89Iav$<<y(C4=~|(LWVbFf$R0(I;RFmW{?91L<n0f4(0~KGCKeyjdVbs
z5hj6sJuU={j$eV9_2mfk1#5jQ=m|r1z8dp+P&&OLeqK4ucuk&rl%MtMWnuk}8itpT
zw;vYiN;t9U`#-Wf6B^KLt3ZUkqn?d+4!7DNaktiUACcAdefqv4<O-KK;!U6kQa{g!
zmZ-z_yuVB{#sb)Fmk)q<wJwzc9@M9t6N+viFa*3AM=l1#X#vs_eTc&0$run2%?oJP
zt}h@@bpV{kfi0kXP(NVqeen~;<(D7Al+-@=8Q0$WwA?cov9@(Idx;;FxUJpTu#0?+
ziodT6uX|1H-AZYrOX80A#)^Wxnm_Yc7DB)95PvsAocjmX^*_=+0YY>KkQfCk2Y9XI
z0+HMHR_77FReJq@q|f@x(!*bdA!58Z(2?y<V%Ncl#JF-z7OmRRZB~l4#3YgFZm6?$
zp6v$uI$vvURpObIIJnv{rw#~6ORq)V9O6e8ZqcxFrw}C*mUS#%mb-d1)71P}GJeTh
z6b8wiV(f^S<Vnyh$#w#NdBTx6b~gMRURktn&ZK^4u5qvT4%I<xQnRD?Oc|!i&1Nji
z$Zp;XSydU-p{GaPCCE*o-))~ooE_^(sf6yBcl4}@DfZEFE$hoM2H200EaZ?H27_SP
zJ82N3wYxQ;A=&gn8zT-kwT!C8C-V2#J86g3c8sTc;?(dCoHq{n#uFJrVg>ixZ{|28
z5!!NB!n2I!-n`|hVc*uf=i>R+>Y;JJsA9{MRCMO<bNnCWqyJ9~tvu{crt*BTGZ4ME
z9m=5F9rkTJnP0k9cgd5##F<mq_WoI;$#%JR|Bn*%C}?DoZ(dOg{;M)f@7p<tarsMT
zoAhFG4*lE39CgED-uFE@w1*F0On>flb_QR6t^s|__dpmN$N9XlkD-<ze~^G}L5s;g
zogA7uD%PDyY!q4O%@xlJNqJsjqQ>U0v2SN1q_kV!<-|=nl?CUE&v)w~F#9WpVYXjL
zkA%~;7lwg14UJyA5s!MYm5&q&e$(g*5uw!x0&J+WCS2t*oX$_s#c;Kqu3<t><%i_z
zDrER1^HIAO6v8A~k16A3{h9vy#9ykqfB))FctU&CbSaQbd-k;%jt4zVgDz!d&rVHc
zJZ4_~QWIxxvY~rV<R#<lz?dS%evl7guHPq|Ol9)gf=<=3TUvfDlOgUCaqUB|Z*oTW
zhPQl_-*yTj?)AlZYYMfK<lL`#HKTF1M!22hJ?L912hv9zZWgvM(m*H>joj308=jHn
zwnPtVtnbAyd~DXx@HI4{B8_Xz@Zy4ppcXbV8XU)~(;gDw+oHMmr;w9_*sDmWR-2z-
zqc7QqeVlhH?b^~kTW`R7p!l)WcxgP2mpj7Tu_l%u#j6T&w&U9k!%IN|`M*S0=g70s
zjROIt#bS{&w=PZyxK70q`&Jm}=hoHZK3k8LY`d<1&aJ($y8`dt;_KXaZon63sPqMY
zc>tp;e-WdOQXpMvx`v)f%XTT8g^YA(m`-~dPCbBE*O>^AN;&hz?~k;5sBR7!Gct*E
z8F`<J=BTTw@zt9}S^?=Sx)t!nioM$rdf^J8B}?d&s;y!VvS<kTvnt|~tJbeb1wIOf
z+cXSXRK!ef-EOuxHXaWyE-EtM%X(RR){g<B3>=J>s}w$(Xp`@tn5>aX-QmplX~)+a
zQtrTO>S{%1Ga5%XTn~<YixR_~^9&mdqsu3Oj8hdcCjO_sGaGdtrKzm5cFEb}v6dOv
zbB$IR_;561bE>s525A-oK2V!W{T(_jLE2@t5$HbK@L?s|-7dU(RB50*|40?r#9jbu
z=FZY_`%|aX+;T{w-U9~H+=u1Q;5#Di`3NJ(^)$C+y=QCViy;|7S}%=NIa^q=21D=P
zy-G4q+Id}<KSoXG<9+HuS_=ZkSVK_!9|F`L?6l<Rr&JRzt@gS7Yo}Q-@1&7tkZ1hS
zdHqlE+n?>g`6V3qFJAo#B{lL_4XQh4T^>!mqYaGnCXA}1mV^A@^_0led;0MAe27>C
zwlabd%5ry_!cf@5>)v)AkypzZK!K^g6N>{Blowz~jRM$LK|uEBOb3MV6*F`wjs~9l
z9V7w><NpAq=n=L+)IB;3tkYedbO#rgwHjG$KXEW#!(dwM2K$sQpGp#^!Sv}xuH)QE
zW=u>h&r$U`mFpSgMQqP+1$3)`aISd#zYSdTM}YQEczpZx{RUv?ovxv4v$O3vH@RlM
zggZMsMRS(-v~t9N&!x_M4pve=_xhE829+96Xa%vD6jp>9b4>{oED~5S$f&DH;0n}#
zWo;FqKbaLr|D+`9e$4ewkPGt#Og1t$+|LW`SDKCS*DvQE*oSUbEk7T2`ds0gW+0n+
z%<|ATyINedavs0_Czyr7-%oa7>DVTKa=}nqDB*6F-2>zY&~d%mcTlW;^Lp}k&^6G|
zzYrPuYk4LBAnWWTodn4O_%~x6jJG#)I<X0s{hCqU0|aK=QTt62aQG0`eE+RnRdaZ0
z@%d;Mu!!35bmy|sT4I`U2>pg`>0*UNCzKx<XkY=FhuP*kHC)PGp>C&(wU}@`8(V6)
zrpL_2;Am+4Nb42r`}n!ztBZ_Kg$mTr+ZRTO7U_;8MmpMfHR#aJrmKSu%GsXXCWAH=
zRXeF&g=AS=#p&U;qZHGcml56$$v#aCm9d(d!sbD@4VIhJF0m&XY|cWg;!&*RBhshb
z4)XrFDWWM|I3{v_6#cC~J9uvYL0If-Z+rd?Bl$7pM(GRE&7dv*%FwT9koo&Wk_;C0
zRnyCu?0vgx&(OBF?gDt|k{mVqEf`Yd5;k<$Ul7>Tq;|A$AuL2N<W*o#vj+Awj&vY8
z+lo05w2dkm*we)C1AE%P3jKMyAQ*QEljm75(Pb(!wpV=bSn$?pWgi48ssbN@ec^I@
zH{Aywo?cI`dA;}@WYrbd71xQYnoO-O_?O1?@7I|AKH&YM)AS$aH=mRpo_VJ_Zxrzy
zJw~*Hp*d90%^M;EsdYpR+XU*CeZ_s6a^CBkq@36k2@&ooXo)KQAcVKrj4o&rfDNve
zz~;$A6@JPpe62JxeNP&aj=@I`;O?F>iirwQ%O_;X%Qz4QaEv?6M%-3_sRK;9Ily23
z@_<j>;zdAl`M>rg+6KX68PBxi4)L^WJD+OuJ>!>eRo7KkX!L-4EQ19(D-+$%v};w^
zxI-jCBBH@~)p>lHAm0UB@=25$+)-PBP0`Wd?>uAw3@GYQbbl;XF{!1WCF0!>Gy`g+
zO1ry0bX_jWCH2mPOD}21`1-ZtQiP%BuqV^zxeu=`qk87?+e`LLIQCsI20W3K{I>0W
z6cbgF$fRS->p^N&L*K$Pz3ccaB8KfW-;`-Mi_+2h>0-+|gGcF=mnq01j!~AYL-zf?
zJJ*I}`HOtZ?PYb(cs`K~{#HJvdJ=8x#ozcT&iQOKSO4f^)9c>(UhV@<f=gF9Gw?eb
z+>VtzBenFGhp~{enwH2-kNY9QWWk(IKR#8-zRcjBF;rES$~x-U|M3873)VZhYtZYG
zLwrF)#NT)7Sl)A}LCZYP{y-XzNn5`$CcuZ-OQC$s$;r82Zp@udT2*j~HNlWO0&y{s
zTtM-1VqA;d(cg>1|K~_4|93Nsgq#{vs)1IHf=`5%^1DWhPdKM~c+2XI?>Jj41C-5p
z@bpGr{VOLM-AkNApryUi$R_IX?cz!k3BzG$G5$NBO(zl8VGyp){z6{q>8ZB($`?lE
zS?`m@>^>AyeAZDUcsqEWAk61MHs*$8&V)0+Yi-FEM`gg)a>R^v-K#j#22Ea8uSM1{
zl#bPt3%ZgSr2p;?wp&W+<AzoZ`X-1`b=#&`d20He#BxL8X=*~_xDBX7et>UP+?pgu
zxMG=vXc-`l>;2OW-)?#goDau_Hm%Rq=X_x@@NPEJ6KS&BfSOeawp1jvXV4A+olWGp
zjPjR1Q#FF)FrMqnha?_NKhnz4DCX~k8*BOI8m=FuJ$I9%yTeZncVIA0H}`y46{1k3
zQbWYgc6_75jx^S8n7b-cV~1f?bHN=Q_r6l7c+XE1H)(SBU9G|S#EHkT)ZlZpoh9<0
zwrwx*S$r<e(<XQ|@NRAvD<-n?fLVB4)+|pW!$^xI-1R$%QugNENbQOS2~u;p=u9I6
z#JeZi{j(`d*>B2}<R>2UuXApI=E$p1KJp1p0`jc-o>_NC-&T)1IXU{6<%ij*Z#i-K
z%6}l-5L`fGXN_8+kCBH^L7j2P*-_4~&sG(gAsMU5p`mU}7iG=!eG~x?beRE)*YfQW
zQ5LoE^6JQ2;`}F3P9+VRIf&~Xe1rKSGY&en)B~Qbh1C(P;&PB~5lqZFpp#51_e>m6
zm4H`PBcc*bMH|M>y%W8#*Qa@-yS)Z3-(X!EqHYzjt_P2%BDQO;r1DjH%=yq!6<O?j
zf9HJcJM|s)25)C)+KB2Egj{IY##VDRTU~9;zR4|8va4d}pBoX=mU-vGhqmzQAM=0t
z{M_Fbdk;MqNxw=dUx@0byQz8R!lw0$+dJYF1u(mCo8$Y*%~&>ut@Owd!U|8^%(<ns
zPTLqZl0sP*kgrCb=aYqwWPzZ!pUNGcKn@suKQ-rzCmumex^ofTk2w$abyQ02`S+?5
z-J#ZxXw{Gjn{ChhGXu)@#{NDH!9R>l1HixkW?KkIORz%di3l+nGs<=8JFoNBr44Rc
z!`!T)Vl(;CAM>DFoY6T!R?m9U?v+e%b<r>R%OwB*!_5CL>IwhvPM|--Df%b<2K?`n
zMAlALF0N0k&7r6NbGC3G5*HR?6=DVc$0{pJBzWKJku|H}btflhS0cfCZWgXTUEj8M
zf)EMbaDHNC{lw_BxUh+!nxMMiO(S98Ldb+jP{-QRmDNa82rM8WA<QZvBrX63i?E6c
ziwKAaOR`D`O9_aAMS*hHfw!;<-eVP1b-wS+DyYPI#nR6Fk*oC+0q|8;d3hpvdDg$S
zOGNa4tX;wqq5@JPBCKF3uz;ADFsrbnlz^ldm{m+jLI5oG<96BD+qylm77!N@{!POq
zf7UQ1RwHo{ao~mytDwGtA*&=<KuA=IRZ>VuKvaU&$<5Kx<QE?$^+$EZeyS@WE+ioK
zqspQ`AF1EG_%D@(#DGr%|MtzIr$hDEVg8-U;*z2QlER`ts4ViIs4OllASo{OgUUZ2
z$lrOhh=hcIIQT~$Cib&8t2jHk{xpG9fJr4HeA<^lx2s5qvx<nEe%|SY<mtuf8&5B!
zgjhv|{#r~}NQhNL@^4pQ;J4GK|5Q*In5?JWt8+RF1@*No98PES>10)him-}{fdBnB
z?I+Hb_pE{0&MK&_q{1qA-`dmlw3NB6^-b1a3^p)^f9vww)=svrcB~?zB9g2>{UH)m
zv3CS!KF~}@b60C6YfEP<YhbPr{bhZ6B03G!W2b8`HXW4avC>_fM}-yU-KJ<E+z2Nq
zn@QVZ4eh=BnX1spGU5LCu!f&6vjntw))C)@O8xGaskwtRRR1FPEY@}R;Oh&PS&O<>
z;adwd&Yuoaz{+PwG})0?dzNJ9undGsL++}|RE>pj{utMa8KhmQ2vkqGva|*q^Dyof
zmG4Jsx8j=S)bBpKOV}@cgdr&?Qf6|SE1HYkGH<X)Mh!qz-GLPPyZZ1l8FI!D;RwQ@
z$8}ONhptrK4hvVu6gQtaL`k8-KXXQL*e8-qevby?YFuE!q4gprAGlvA-Pl)Herk5V
zg^EWOa6ZB9nQhBsU2$6{j4tPm=?CPwgxLuR=Wt`2uUP~sU&wCM>RFdg++n98yw+CT
zT*u$rPH<vfZyBXLlRznNeYcRTdpO2^^ddpC;r7Xggl3v<mm}f40J4Vki=j<?1p|;8
z7y4b!jfN}hn=w6Cj<F7GQ*|otFDW@WXErX#QXg$zJ4DCEvV4LaUNDq8IXIjw(7B>Q
zNC|tc6ZYs9TYgvp?h#&A2Ry<+=tyXws05xPF!L@i#&_b51bW4Z*lf&q!KV0eWqtJs
z!Ob?C&=+U#-g?+V!KO}eEw)jr<8HpL>HQ=~kxRG*H-0Y!W&B9(u88BnTKmworV>}{
zF!3ZQ-pDNE{?P^Qyjv1dPUhaCpQhj&^_eBp8-}(fP><TWa?8y_D|&VIWq@F?o^osX
zQ@QRszi&f);Jh?NKBL(y{OxIOE!OBy)0T4&UbCH33Qd1+=^QsV5cS<<zc0C$i$wSn
zj&<tdb)_g30ypBJ`FZZy=h<Rb6%TdE1{fi^dba7&jgy^v9!`TRw-XG^u5o-BFTNzo
z|6IkiokG!4_`##0)h=K2JrutFCU|A0S2HH<T_eXlg(FHbo~S}A*GQcM#-i@nFiR;^
z^s?@FHn}v2RD>wDsen(^#^RjC`J?ereU%sU=}z(aAE2v7jhMV|l~(8IiAB#QlebXZ
zH*<H21k&Jg+#Z&MubeoT*4v&7uil5G4QOmGv(mKRm!(Vp0Nb8&3OgonKKk~Ga3f>L
z>?m_6biT{7ytZO@r4i%MzN&eV6LKL>@RqM)`^?p|o?Baf(AneqDndh|o`H|^ANUdA
zMUrzi%46;YYqU}h$eS-Y7)BoEn8mP|r8gRo-X}S8@sh}BTPML0b;fsik6Zoo?j`|^
z?sW=@#~*E8J_`IoPjrwK=2&b0pu*<NQnV_Ix9kIM3gak=+r?jluiKC>RjOR*DDExc
zdiqLrAaaiF3f%;5hWWlECA-V{;|1y>ZBD$q@JEMkZ>CE=xD=qc=wdnL@SgiO^R24C
zk&XZ6bD^>vA1?qCSeL{msX4-&nk^@;?yYr!iujEGCD=XGP>php;Miip$fLr%I;Di}
zm#J${2l$P96FjC;3fb$Q7&)#~_`Q>7bPtr%eR_>kdhNpW#~!;q@G+vZh39sJ{*2)5
zYdL}xN?79;fp5(78--=_-3RBZKBc!2w&j@k+o`?#Qkd5<kSWrYc&m*hd82Qd<VBuH
zU0czi>n3;1lbVH`m6C`L#WER0K^p6ulDq2_uFkZ4e8J0&uFhH$sWMN~w#?YC9O(uk
zif<8pQ0Y3vC;FBtvR0Y-<icY*z6svRu(s_F-g3&UAjw7ox{EC@uX)xuG4Lev)Sh3+
z4)lHDwb2t<CxCeFd|+7BseFY-H-X^cWh}PIpEhINz!WX_`JGw6jhR%!xNicv+l_=!
zwb2^EvS4gDsQ;PeH%5_EyavI6NtfI5;Z+jM1KjxBxp*%|F5V_n%`aNlHJI&3e!Wl_
z*L`!iM`7`e{wJ@f_h<R~EjnJKLGPb1WlAvfl|H&?<g<XcpfFa&ao2)?-q~&3L(l#_
z$}@q}p(01py%0@7A!mKF_5S#^<~djPkoy_QPXah|W8G4(SX4Vak>i=NOg2-KR5rfl
zojA{D^*Lasv;QjoSBkmiCGd3-?yo5dsnU{7$y4`UlI73X2tU8dYDV0Xa%T6vM8>qb
zfs|@#={H_|HG{mf(?&^a$Y<3c(`_HQPEHhi>H}7P#-c^~$OTO65;eZ0+?iulZ!LOr
zajWL>jr+_Rt%uxRlje>QCmpoMvaTHSjrk3?PBL~l9Xr&??c&b(yn6+86eX_3((>PU
z54sYuX~3t(XGh7jtGg%gr7|n{cJV8v<(O8_YZQevkL;BkF=DrN(g>A{tNb}%#yOp7
zA|7W|`eMf5Ij{0TE~JY+%GdI)wBo#c_ma!!^Ge~bVlQS)`Ykq#GT1q45~aJ-9@Wu{
z&CX{jo{=Kj7#44k4$5JgPGjC*9WBjsFUB^SJ-$boa_{>6e1&tg%;FK?^NsI_Q(9Is
z;9~u+;-kq)AM(;tD%kDP+?0}hh4rpF*XzcGnUNOJ`uu*PEqV~bGG*mXwM#?u{P_wz
z?yQVIgW1J|(#RG|`-pe9X>Ty<`iGWJhh3tEQSc>wD4@8P^az=tS^J6%GaSX}>-y;W
zb^4mC=jwA}A(A9XSS^Vv1?!cAT8R$|C!L(|f_mY1w{^EP85K1rB%Yckk_P*lBq(1|
z9er2ABw{hu`t4}gwz+M&qiL*eRYCI!%d;okVFTvR4_;@qr&9{)4^|zDiWKz189EPO
z;YHa>WN~Wng*TQX;15SRLnR9FDqeWcXz6ZJzD5X$<W;+gE<X4MRhtjC-;#Wm{E6g3
zPKk%>fMPgJSQLm@O`N-+T1RF59V3Lno1I&PkgD-3r)@wEVc_jzD|}7)hm-+A*)Ay!
zpm)j9c~(ccH?{m@lJ`7xk2|)>uHg~P+aAxIb@<ew3Z*?~9kn`=2~q8Qo#y(+zf6}H
zVXkgc(akW7j*OOTe~u`kw=><#qfOj<yLCI5CtBuJ%e=~Z8|CHs>5N|0tqjFQ438Z<
zy$k%H)4idO?DmCSMjt`-1m$I2RZa<~^{q4Cw8YM1u3jwcZ$78%e?zqh`(ehqKHo;$
z^J&A)b9*-eF2?QF%`mO8RVSzEJiY?=R_@Ri-0*hJ_$VIE8b#WfFz_z+6-U5sShU<x
zwv@o7FCz;0L7%@@UI8zCf9w$$DE8_*E!FJ-mmbUpqrdcb0!qju&L2P+(Nh8Zud?mG
zK^Sd793v9E2?&hVPOdJj;y=k^I;TLz#o6tNrS<8ppM*amK}~Bbd-EI4o~%DX8!;&{
z0TDo=lmr9ftP}uQw9TIYj{ro#QyCa|!cXAH1rYzlMgL7G{H4rydheG_`LCiW@F{8%
zl3*2<5*H8!J`pSmv<$!&B0`b^LLy>6L{lz+)N1eS^ds0J`fJ$oQ?CKS?<s7Nk_3R7
z=<mW7;H`iAs#6jE7j>n6sw*lbA|N64TlrrY0D}KvWf3tk0b%jqU<YCF&qwxmUMwsw
zB_RHT7ysJ;|8*>W2RlgpP4NFC?C=YS@kc;HSVV$V9Q-#V@k6xmix~s{`HcAsN)VP3
zmiW)2gkpUom#JzhzlqX$qx?&RY!}dpY3^N{oY39jNI4hgCn8J{fmY)USJ>^0zBkqD
z`%_&KuYU2BJwCG2LU7JBqh-oF;Ghqco6=cWBm2#>rJQwPw*M2cW$&{urtvm>Q8%}3
z)_u{irhalmrX&a@kKaf1rQ>Y|*3I1;Zc1}i8m+#4t}aH*HS2Rp>}Z#p;~4ME&UN?P
z$BTIOl|_jx&a;BE(|q#$l6kbA(;6JKVI(whe0@5ld2}rD=K;UfS#LaQ#iv4Vs(7Sq
zuU|nn?v~KO_yY62-dNwI$@IFD7o{I@KFe)8`OdO#!g@JWJNkk13(PQAKn>YV+VHT?
za{}$7&PnDLju+du>&qucx?C$M$<*6u_EUV*$(l_}n(NYJt`j|sVp_PF@2pWsNFJ!6
z(rVB_&{cA!&_|R<`vP}n$4;@@0QK7R{@&x1Io_uu{PNi_{StGg8;Hz+?&Q3q@$yYX
zfe0_TU9rJ>rL=3Q=9f2pjbcd|D%>r$l*FRy43ET@j^4)vwtTNECbwQ3t<|mI=3#rm
zU9I){a(fxR*=mb=n?xQayizZ=V5Lgzh9v&t@_C|WyLD;jYO4h>e4Woiq_+hX6>x3o
zvxUZ+$)2-mLTMp)s=j|D)OdY6t77;3^L41#8)~}HhxSXaU_Lt2g{_xn3M;-V!nAo&
z^un|uggfJhsSLpmYfEBud_g3cXd=YLPay|}%jeafikE<15a-HLK0snv8EBFV9JjbH
zq;^arkLLV4*#L8p@741AE@sSAIeO7)p~_co&?=7Ab)2(ss*7ps(G9r%VEp2mci~)~
z&UKM;p#a%H<HUT|>@`PJ<rmYMF*!E62p1=N#XP>REX}a|S_9>|%|r;{9hukc6C<h*
z#zWVXw#oICc2!2`j_A{UbC-Nt@i^I^-obmxM@rDuz>%*!H64`lmT&pu-Invi7aTkN
zo^5|8&h)?a{_x|>tOLZ0fIeC49HZ7tkSrbvcU(Vc9nWN(NNKk&)8I?AcmA~-5AwqU
zrDOZPyc0>ICh~b%#@B`ab}o>*ZBR)jMnnhMzTn`I{($$J{+da`_5&^@U){xT|Btn=
z0H|_nx~97ukp>CrI&^n8(%s#S2uL?brw9_#(hbrf9RkwbT_XQcuirJe@Adt@|0pVQ
z=InX)6Em}B?X^u(pOWF+G6<sA`@*!8oEH_TRH>igT{ii3PZVAqoMpZZbs4OC%3~H&
z!6GgZmg@MiXCwfn>@=;`*m0B9C32VoaB%Ygr2E#S*E@oKel7&o?<UQ2+EY;%b8TO^
zLRu00)2*(`)Qe4*SCe(T2~m%YA|Fa^9z<PLD@WuZUK9?Gx8-?(yp|<AsTeUdS4L2R
zif|5xn1d<UnDt$b0>zJ(#&1s~7~p<#w)dgStBR)_;XzyDa4`eYCyrSCH%Y-fMGM{l
z7*S|V$Mi!1R?SO>&TjJ0<!?N#4nj^-n?FuB%LTZIy@%#n^IhaBq~_nnc!{6pr5p_^
zBFBtQK(pAG^I$o@-xCrO+$lhxF_iSKFek!$3cM^N6AloJZxH^_d0~sAs<)`R+Vyq4
zDoT@ItlcJ@lh5pnd|zV~=em!Al8!S##U8e4?&bg)F&a^1E^e(1D<N2Y0K-D$7-65R
zE`V%FkgTDMGNhdX4dD?12icQZ%Q*d5u_S$1V;o;Sx?;sNmcTk>lpIj{w}}khhAlk&
zA<U4N`Ou_CpAer{0UXQp@z-K8%F)rZ;Xp%DTc}p6-$3Dpby%fIH#Ad;mw~v;&wChC
zTj^<FMrTRyH0QX&s6rS=O~O9nuX>Uj&hOVo&0WrohIYhtav})cS7x|W*iQnOD%|U<
zz(oNW%!o!*9c;Co-N2$U#g6c|!s}`=eilC}oMx2NI9M)xj%90f^6LHOCnRtq*QyB5
z86MU&c!#4`&~0SBVz(S7Bnc}D_^0BJQXk38H*$tUIx@WTeDB*nWyDo&Lu{pQzh!WJ
z>2fAQK9yyRaMEkG?3JDHDOUr_T6bpL=EM6P-*kSOpr*2jQglz4=G=&9T<_$D8Ca<+
z^7PDuUfIWZH^(*8#qX%ow~14eUftP^RcA`CBdwX>*l76_tc;M4W77sQQ5hV`lsyTe
zuddxq8{&H5y4IzwYc1B6wi~3w;EXf~)jQ2W&v^0}{BiS#fo2XlX1YkB6Q?}h5b(s!
zaj5L8)B(KYQyT!Yt?g{cfGE|@dbWMTXET3;u*TkG%yJ~x<gih=oWuv8L2-?&)3S)e
z?5)`A1zIO{CF1BXnXV1n;xFrTBDKkk^{wu>u9YTO$QPP2nxLN3EOlsK^wrzoL}~}l
zjXm@wjY}@yT7V~=vMI-|W?mjT>cHVba7A}|AV<YhCu=*idpk~jBXgky4$xA{^q9cd
z#sw&+k=YLs+Q3*Vy@>FBZ;rrtkk?J^`M9-Lz34WiS@JQs3|R|KNb#D;8x8kUw_5AR
zLT#wi7Luk1o9<cp>n4<YY5aDl7A#U}^jKo>JhjWtlUJGDSJTUP$`-ZiNT><XxEnqr
zygM&p30^SXb%m|#k`D~iH(5V8A@+;8{Gep3iMX<Kw7hyd_WGDzpJZvJGW{LcXv-FI
z90}*u<Frb4R#yY!Jq*f~kw^d_>?sH?@^R!nsVK{jVCVg*=+}V%b1M2ZMFRfUf0v5>
z0G6}-2$O!vlfL@#S913+nDlQs)=#$of=L50+n<xW`-~Y`>6kg#f6MOJSbj}O?=fa$
zWv62W&d+a9I?Jy)>3zmP7@m>+9zdJ*S1A1+V<tubuv>7S+dn3>|BzSylra8;N&lQZ
z{(?~d2Tc0kGf5!q|M$80AG{gZkNHEqxk`ODa)k}e`=Au%oZRN!dax_?w00In(nm7>
zveZ4%bA6^KJ=Jrh!U;SH<B2T`)L2wf1ZR+AO>(col#di&7s`&9JH)-}<s144gwq;U
z>6WPaN5d(mQpx(07MaSCG)QB2oR95Ik_<NB&3AV<>UlOQm+hJtD`lFg<t<E7=j)_R
ziQ;7kQdHE}`cLGhTGG60d2bxL^mAF3#5zNblciK=E2A<;jSvl6$f)9o>Q9(!LYC8V
z1qE6pONM;7q=Fi%Oe|_7nS;>g_Cy?;<ptmOJ<w#f<ijSrJi0lMP_(&FDW~0Lv+dWG
z@qhY?Q`>qmN%e3hD)YoU?DacTSRpSY@3oh7;A3^1EVP4{d%9$E)|;*^21;IAp&ppc
zR2^L@+RmD9jsh2-z?USU1Xh3Cw}HFFu^m00(Vp??(vgHyP@Xi_T-Wo1dvS#8vW38&
zqJ}w0Z#nIqu12P_VJ&-{#{;A$aA6=mRJLSp;Kp{T=E5G*d|w(6dQ{}?I-N;xS4*{S
z_Mwf9O}pv1za96Zc72S*atsarR{!TrY&F7?<}UC=ToB^&IVRFQCAVm4iLsov9KA*|
zH3mp5VQooH4V==@b3kA?gEagc#Jt;Qij3w~p?;u>ms{LRqB(q|3CrzaT7J{ig~VAK
zDZXu+hJYQ1j~w#8dadXCZk<mP&JbZ!U*-;FKY4R(F`+^b8Ufweo;M)L)7&ldHdaz8
zil`L+DawaYn8?sq1aZUbh*ic-2Oo%zQ*1a-eW9KW(xxLpU-;O$YL<irj)TFptraY=
zYHR6DRW)^0AkEIMqhh@t7QM58d+O+rFs8C+1Aj`(mO?n>Sq=)bkgfP(|HHv}tL!nR
z+w<5(vH94CZ*mtt_Y8aR#hFz1S)$SKM8d~H9l(QUT|c|N(Q9%;tJlTpS#dQhp!T_j
zPTW4Bdo%a;ZSOg~lGxURiiQMGE)DK20VEU)72(YsBr=p<QHm}it;-^A@t*ixs*C!!
z2+uGgp`p2?Jy4;v#2@NM;m9a*+|=cExWBGgRlbZvqU|lnYNA!8op%rdJ9yl&QxMj0
zMB$#ag{t62PhQ}PV}r<CzyX`5ldK5N_ngp>5AvBVo<^zr>T4o0zUM;)nI8HC9~L!r
zoR&2elTkSI>~=LN+jwM2x*BlnC7qae<5FifO->f20q*K@*TZrgBCs(b1n1Ki0RMFA
zZj3;lMpLUJHc=%5xQ@=&F+wkv9kXyGyhrV?v|W2_uSHzOZIDFlU!Ejo98OhFk5=(X
zcSdZ!bQ@KXc2{}6kz$8$oaB72KDD%HFj@cZRV;_zgHlOeb-0JZp4y^j8ty8X?(=&T
zJkZ><T%1TtYG5n95Kg}Dr(5gkqI}<-rsC%`J0T%V%6G3|e|Q8@_CgMn()@M;(mp9Y
z2n(f_GrkRdl1!*Q!1}$<VNiQnYAEbkjnQW&=vPrSf*C<YXZAjQZyBN3K)X?86eG2a
z2cta;W{hD2EM+^0q5Gd9u{aoomY-FFrRdxBB5DW-M5CmsGdx@9=)SpHXDeOr^q#RP
z{A8lj-n*gFtX*;<hce+?avPBXl6Pm2(Bu)d6~4+hXuCe)@R4@o>}kno8Kwb?+Yrbx
z4WAtgMkr)8F9RMM8gM^09oB8QtVRz$NU7lL1P+*6cw*ecOU9i;!b9Z<Uu2{Hl;Bkp
z$pVvtaP(*=K51$+Tt~6QC$f+GJW4-0pM5aZr|5x?kIT>z4qfpws_@2nG!hyKHkTOP
zSTq&Cykr@!!txp_6k+KtqJ^jQvYC+BA-GDpd9Q1gVS9Qs>zz6oz03I_Wq%#DUavFE
zD(Lw3tiYZQ`1Zn6eE(~b);AhD{EkOYpI-pp+@j^*ecpl3qLYKE4zF#GEN#o<w<L>4
z<+-+fJ;@l2Eu1DG?y-3?8Nw+JqH)1nmae#w#OysETNC7<;uaVK>iR6DJgG0pP7I|n
zb1(?aOWhKwF2rBXw6rfsk)P0kVH=GNPpQ}3ALoh6`R8=C!AQENPtxjoNVScEhGzmT
ziR3V3+apfwW3!p87PXx12{$n&+@5(O#YwJmizPzVXSFNPMccGBh4C+72@)}P2sZNj
zIJA~*un`g3;a$>tnfuYkzGRgNfq?FOqQ~8CB}|R8yXUS%odg{Z73vO^_Q^R9A^|x;
zPz7y~(}-<MLRbn$%3H;^(RG<@u6*}uTXZX&DtBS^VIRyoQLYTW(SsP25>^@lHVz6e
zsuECLi-&DDUSO;;?wb5QzPl(m{j5BBJ;C5R_KV40(MsuW53X+OVfwPvYE+WE5oE;U
zHbcPLT!YV?gdKJ&5qV}8@sLFxB-#xz6^poKJo8E|h^FS9+qlGGOKwuiQ~}FrdbA;a
z+pl)l*2@DwDfZ3}!?AjZ0$zM2iKpxRmC>PrXnJ8k;g~7DRFyZB2TICixkUtLBK;U_
zay|Lf>!8%q-4YZ1-D;RB1g$EAhgqC-=Er#filXM|HNI9tvu$`$z}*oDaC-fBFEe1;
zCv2ox>a5y@g<xQE-(aH%`OTcB1e3g7Y%-N6`bfYW;=mSro%CAvs!y-Fo1`hE;8HoH
ze2lL|k+1COq9y2lgU2f2%9Plr+ETl~rCK0G6O%Zy)HSCyQb$omX9hq2QeQY7jn4Z}
z3j{=~3=FkmB|Zus@oZ1Ut2acQlc!+5c4utsDE9_~eY9*BXQiE1qRgdy`@{0v0Nnl5
zuGR|8%fc&txOebb1Yo0UU|>{a*&su{7c@NCoBp?-osY{ysHULn(KhMdC3e<xOBeVv
zq!{x^T-{V|f4mYTyrVGth)@4m-KdP#JN9*vcC#qb<vfF(7xv6Y-%lA19O4{I;Qeq|
zDk0|m%iuk**f_+Gp+<6%UwZR*qFu+NJ)9zVuq^Tr$5ucyOU%IwR<7}kDlZpmbSG)`
z+S%5-U67T$k#1=4sOpnB+|U>!=+kzxXo?pMq^UjFv#*ZGM^cl7o4d%)b00k;uks{~
z(@P5lrFK{sSmG-W3un%mpWP+#_uv^@(FU1<f{{LyhAo$G8a!%yljZ-k$qrD(&^jr8
z#0Ia~rnr(P>%}M_kv-}pZ`sn>2PX-)^lm)nwJMZRp6%N*iR8Y4DR{o6g_Wnl*$<Wn
zz%+>Km<#+eDY>p;s_*z>vs2XWsRvm9y&m|Qw*6cWu>OTL|9ADk570ErH#!6xknHk>
z)9^Fk_#+eKTZ!_&g{YYsnSq2KCL#a}6A->-Ct_w|1Y*G)L;xV1%K*3+q5jVIVg8j?
z^VQYwXzl;eEjAW9MrI}=R(7B}K&YA-z(@y7<XM3THOD^y=ilA>jfTYfYi0HIjQ$yH
z&iX5Q{gdrqusI6@a0>4s?6Ce?S>0pI1UxVnAh7>iaRfZtze|BH_27NRj6jwakOA|%
z@voKDJ;sbcwhtTNKI8vmar9#a^pB;|7mm)q*F$VSy7C=aXJKOf(|Ty|iKgu=8-~wC
zDM}eOZZS@5J=zm!GWCkw>B+a})j@A@a6%$kr(>ymWo{2#ktBL!i)ytM^gQj}^)I;&
zT@8sS=btRfZ@ixAbZ~JQL<>?N$=4flSUaR7@@Y+DM-330Q#Tn(_pD1$DcY{v5G%(+
z^%C8e0hM~3T-f!dw6E^H*5kKY8_w(VL*CnxO>@f&O*2~YMOdx`_JdYMR}E|ZGZPNy
zK3%OW+YA)2uxF4sXeh*J6!Sy-9~CJ4n-Y=8d1-hodW+E+L@AN=8tt(vr<g6~bA$jZ
zW}A89!Gt%m4+F^BFi)9?J_PT_J8nFWub*<v@b!k1z5uV$-g$PgzUfU2FsW9Vbr>?N
z`IvgeLr}4zY{TY$<U4IT)!PGy1-~q2R?D{NeYWjoUv(J|mq|`$1e%10%I)C?p9$eJ
z&YaCdxO2)uN@8R<H5L)uS;E8?nG8pnze__V3Z?`#261Az=EfZfn>%;Z4l=+Jiwe6z
zV92kRNr7(e&|o`dY!4zbCWt5r%0^Zmj5sGV^0qa{>O|JdiaeAV4MrcmnkHpFFWpl>
zT4uoRFrh*(XTV+{sJrc1ba#5<f?Ovxu>hJ2S{-9||FH)g-EwpVk2#+g);`UF<<^_g
z__U@v&by<j$BKZx_y!u2fJ$C_UnOb9m0Xr6=_;m$d<2si-{c_n$c>YLcLPU6K53c`
zsc(lK55AF|c5sg0NiJL`pp+NM3uT#tMDDNXsn@`P5F?~z)e_1Gej^7f_2zgXwV6E$
zr|(Mst~@C}!myR-k-#vttxDpoT%4tJW};oNE6Y$ad40e4NV>oM@_Nu}XK^E=oVm-a
zvrf|U{?Fldgq9qpHpU!9gCwz`?_Cf*vG$H<nBlo5{kn@^O6!ke;fRt>Q&qpTBxlZb
zL~+N5njrIJevy$D%YbobT>Uy#J8wvok!SmTxUi3#85cjftF49?7{EIHDF_&XRM~KX
zD8Dz$tHcN<_&|zi@qVzrwnLbmBJG%M1XiU1BW-+XyLOTVCS2!11-1y;Yu|c~cX<p#
z1##N71C~-z$w!iGmaXJZoSP-6fb=MK*{X6Bb$`VoQR@RhUcRuJ6B~!T9%WRoej!j9
z{#;u9$s1)XrGjXed6cjTYM3`*G`2ZJ^VXQrPMhG~Y69kCveF`!Uin$W2Pw?8{j{_G
zp#0#P?!}7XDwd1qN>pTmXd{bv<4-5V>WZ(gIkuaL=OR2T+h#so(++C4U4G0*E)0bC
zGjH(WM_Z3_Z%C9u8n=r~wjAdPGDjiE@8;RFiji%u#c3oXJwja2`V7!7#w+tw+!L!Q
z+%JfML`7A_{YXs7j@HReWS3dlWmAM^6RFc9R5NW2Dv4}tm{?d~#X=7<_f+?VA~v0D
z;FFr0sTyZkk(wqPQi6mQtjm`)M|4$lY8D7QbC(*2o%^3{UsU%yfI8P(cELpgT2y<B
z`dqq*-D=DPh<9+R_KvuUbgJy0jdw^_kA@!>2fbH`#Kx6kOcuPt)ey!B+7F|93*3Hs
zt|gba$$$3Nu~M2sEE~5GTwgRRvwbSG-T!jsJqO*2ZD>qToP|&k7y?dyQv1vtILgBm
zh?hLk-LT$hhOTg=ySHm`U7u!yS>Qgx&Gbfde^SZsriX)PaQ4H6KVjj*5AT+(IKfQ=
z7b4)PFilCrqR;Rc9dR1Rl+>07Y8ibtS=D)%KIwiDPq<drs1sh;zrK5kLx@jW7@f7^
z5vPWk^i*L#)GC~HlX>;}Q)1@?EUCE8sx=)T&(KLvC@?AouI_5@lY{$7w}kxWxa~L`
zou*snledKyT@ox3VdU{Fh4^I(=N)n7GyuZ!M7EUW_noPz`r(l}^4)hR`hu-uv|e-V
zL2-oBcq-B1L#^G{e7?G_i}z$#Y`>>6zc&8A27#ZmE4Hug+5ZFD%JySY_a(Z1_2bXb
zR<<8Oy5DU7LR(qcfc)g&0yE=psO3-NekT9}=m0>#@wceX_G=<@k1-oN12AE_$M)Am
z=000S7CL5N+xqu`f5ck<Yq<1hfE37n08)Ry({%qJ1^6+Q`JMuN!J_{V5-ol*Ycnf~
z;ytB?f(RxbQMF8WAfwdE>L3Ej^<tKnU6en@GK5xSv~gOYrMZ%;l8LJS`OY=*x{k^F
zwB+<;{LXUK1jDAxPhPyqEh{md_*h(wHAAIu*iHNzRB_q>D3k6c+S8yXGfwMJ*|+7p
zd2<PaPF;INWG^gO%SsCicBT<!hpOl6sb42<WInQePcem#l=3ia0o0QwlJxPsG#wrO
zWB(?(gbuAI^%Jpp{VQo4?f&-qw2v|=hN7)8o-l@iqayTr@P=V{i>UfYu{jf~>#WaX
zcf0aFlre2S+{5$^BS-XLro4Od2`=_i6$xUBi59~G#q7=_de$LV-P)ZOl2~$0ZSVV~
zOLzDdnpotaBre0bKuMZf!NUq`W7w^XHD2IG@@19_1?_LG3?=1pReNYVTPL!l9Bo@t
z*}AGCKf1N6kaG{dUI^z4cWs3{Fb?v6_oNgvMyc@SfkQA6Pf-cMUZL%4XMlm)<gV?E
zaxoJ>x6w=cIY}2&nP!Pl$St+k7^|e-j&e7Ub>alcpg#1vLdUI)4H}=XJQc%lsh~f<
zBJWsfuOlPlSz%Cm6R^R+cTUK%t!$}4)Q55+ho9a8@2|i9-bEhln4YXOBZ0bCdrCG=
z5s|xLOG;~N(mgDP&Dz1Z`NRHfqt@0bz}>Rhvv((&gb*{=slf`#=S4&z-CpBI%s9{e
zZhY^6fi)>r+Yu&}G4?QLEK3=>cpQS|kp%>y=&&G%5@aV5lqrL$v#q1$y=9avY^L|{
z5qdfX_R>XzO!VjoIx29sCpHn96kJ^U?mS>Q5S;U2hQeg_dzz=^rWEYjwuwrpGcp-V
zjpBVOnQzG8XkRm<eu&mNyio0xkYF$c7f*MQ{1mX%-W|yn`f^lE)3L+p%Dc#-nR8Sn
zry;!soEd8U(6{ax{3HyPhtVK*P>^R(7jJ8}y(t{cxtpSJ>sVZ2Vvsl}<}FWhW~0Y*
zKl5Dt=-#K~9Hh^gmzaW}<qVRIoIvGm8o0?fC=+chF?C1-U56+_Q57KIh|#lgxOTPb
zv;%m7Unr6?AA7ODDIbi$K|;_1-ZlqeT)I#WX$^t7b`yl3!L^QQ99Qfa#b%RG)<VZa
zpstkQP;<Njx59DT6rX`De>*hBN-gQMDyckv-mj{2QL!6JYvU@TSUoI7nH@pKtNlEc
z;&R3vInx(6iU;fZArt{R3Cm~Z=jL&raw~luC||RevWibsUsd2hqQ6qFA6ko;3%g^R
zkq=x_2o_yVj;HD+d!?k}ix<nA{ei<|FMP`<pa(@Ti77B6B0byGdxF@#tCEkP{$;Rx
zjH_BK5lxsu!rCA)dyhQYr!8X21@SrhZby<2Lt`Upd>Ec20gH|<oaGElx9&*F6GTKo
z^3(>Bpif~C61l4BOlKchrfMuZF_)>Gr)rqumly`KURPQxK=cQHkZyy~fbc~u**-3Q
z2O}@YHv9~%UqVGF{d8^^0&ABaFv`k-@d{5nnu(jMw(VAGQdG@bK*cabUzom-#%aLJ
zQ{^otXRVFQQBKw#9tRDDSityLho&mVe2t~j>5@p{iJ(ctXcKl89n_|_Ldy1XpY{1P
z^qg_|7RIrRHtRTef#!^2)KrSi?l7~@gfiY-OG(kI7i8S;IA?u$Evd!F2XX9bu9c^m
zC)#eodUe8|LGQ>t4@5$&_jL)k#6tGXi*or`wlYZO+&vtdjC26K3rBnfL1AuDrbA;i
zHRVPmRc|z>$2Y9}nhSqeCQ%)%psLANU-fM9rZoKuHD)apu#84Qh@^1|k+pIM{|?sd
zOpCwsxGws7?ZlqbhgY%NlXm!$584f<jUFO6I=GGilZ~DL|JlMPpx$?kV99-P{>O6m
z{;n!;<;hoi-VX#nwl5X*pLbP(IL4Q^|Ik$h{1~~u#_Dge=T9hez_#1Z6*v$c`4YFj
z*?z12SvWX=wg2ydp6#pW-?5N?2$?@M6@O|s0s)xsQ4+WW=0}3t55e@$N@$>Se;>v7
z_{ju}{(l%h)77kjB{a(CeO+Qb9HJa7Da8ZGbK<?xDv_91AtU-y{`}SEn#nX&BrgW=
z&KF!)6(TVyWurkN^H=5_OB-|YD;@GrW;bu0PR~iD9*RmxDy>T=B1M5MLl1<+#UBQ9
zjXihV@_Axfg*G^Sk~19<>?69rfhOK>MMpN8I5BNnVw_TPVzQJT;k|7V_~!JE)<#EJ
zD%W+N6|tuC(&F0Ki5q`D5+_L%f1Qjt+XpTXf=mY8v>8`hN5K@-Q}>~y7tuW4P{g+i
zTALj^$CkAIKnIP3N`jQY`~U}0AMxWA?-A&n+lBdVIRnzXZr7K>;f=&3%gi}6;ublG
zT(mT_8ZljoZ*q*NI86Mo-*q>IR@|AKC-fut@HmE3g}srm@%HU3I2ZBG?W|Bc`A!sB
zMsOmO2M{m0h@#N1$wWP2NawElb@c{N9S}g>-*b%-#6H~xMTX5mh5H=v30ttBQEN#`
zbLC`KU1<=mysgH<g>*d>;n@JTL5TG7ft3>WOH(Om|GKIz_r4hD{6bZCl|F)Ts!^+4
z1h6NJiU9Y5=1?w$Q<NbWBL@c$2krnpZz9Ef9`6YocZNt6d($S}^$2BchzwY~X`_?#
zn@;!lI-<zQNr`o%UEUS?+A|w5-dM~HW~N+yBa!39Z$}ev$Y+x=YKkdchkCI&6Xk3I
z)6`WX<_|89W{lSQSRh5uGOG{MV>X6oI6T2t*QugQjx&Tbv`33{;Epp#tZaiD`rp+n
zDjG-hc5R=HT!4wjw$@}LD2R<E++aqHMZ~Zm)a~D)4Ee?dclCJZnabr7@wuxmS`|80
z4TK6VhxZe)3}`R*liUd;(?3wyH_0fplPZQp<85^J591mUXNNF(<9xEYCze$1S(za6
zE(*-fH-@OH14j8>RKRjRg;7G8@$_ra@`DCb%_=D8G9)L=tzK@EeXUem4h<@@F0w6f
zhZdp7vocNOYwf2(t9y^5z|jH(y6I5a>2uL$z?{{j$k=9*C>WLo;aMcy3c|HxABMv!
z%YrG4n6Hrqcc_W13BM-K+ZP2G^^~#Y&X8E^Ow>u}zA1ggJqg?x&`(rwG1{9H3%x0(
zel%-qHq7Ov$iA}+Mf4j^;~CwQHbvvvr9$Vt`WN#w#H_`P#ginn>8x4U;5GBCa9Aiq
z)=j)mKw?Uq{c@qo-9dz)M)3mkdaR6WcBp431o#4;$r;ES;f~;Fa^{6ovy*4pPpB5B
zWx=w+Xt=5co%FqX`|K(RBKeLqT60#@#n@mGdtDY*`g(vZ(B&Yx*6br*0MkWA2Wtlo
zD=kI#uFBaceg;uMhmJnJ_-)^Og`&7_sC07LHZ~{)B-~8c7{-2E-X~%*f%dHBa;Ud7
zY3xP|-GFi$0tX5d3j3&Xo@P8x)WuDI9tM!C7B`J6y}X>EOi|y0wTm}qBJ^;`Bb)P~
zJkh7M$nz}a=^B=;$%nqD2hltM(F8L-3~7XNrB81^-Co}kPCX^a%#_t3vFN=+oOrZ1
zn>ZW#etkGZ2B-7oj!4ujv7N*l4jfMEOwANU8D7g5#TvUBZ92Hv$?^E(b;H!k@Erb6
zHeY&~8xhVn6%=*JUN*&DvO}!%=0ydp5q}8KCg{=88%$%iJ3rPJ-V=%D;RL-!=D~7o
zUqXio#pk`W@`p8-gKF;n49)f;r0Jfp1%!3~etO;?w!S`Ke=bh}Uso^wad~=A*!s0v
z`|8J^m8XDTQRkm*zm=HGtiWZW9QTB+ub%%mVe6;h^g|5$v*5)3V~G79z6%iV|HI%^
ztS;pU<hwNAsO4MGI~hZ+NJJNqsZ@7Y+IKO#&<vWhK}QN}&dI+bQOvkpumym``axX9
z97zztu+`(~KAlG#8MzIP{Ost&#>46UJ{C2HESCyT=0Mb3k{z8iz14O=Z0u@N3L`G*
zVU)<5oLLza%!%70!m$O}j}%AeLFJD$;884juPmLtIqQ<9pMOSDL`!km+3thOkh6(Y
zB1k8Ie~?aCxe3<7U7tj7>-<iS7QbA8Sz?q0cQ8Rr_j6VbN8^pPaENb#t;w1L87^tO
z6i#AJ@5q6e)2!B>`JkJ$(MwSc>exjLYF6#ltQRN>$)+dZ>(nyBG+fL!^m#XMj(x#U
zvsK}+K4$@4e&VPX*HEXcEW{52GcrUBC*CBA%&t3nMJp)G^a2Td>*d-wfYe0m=H`Br
ztCqcnynGWI^^tmV*-g$&+d_BMHkK&~;lLatGL_IARYOswmrhI%PU6Yzx6_vR4Un)V
z%5?K)buC2Q9B^nm1Pemq>osc&!c!;#wi*o|UB{Cc592{1aAM3q5N<vgh!4j*IRsSl
zsS-to^W!{IViVa>U6|<R4w<y+Y#($XU_IuH`p|t%2Z2=r0vm#qvQtFArD})r*zv<&
z81?zKBfc}+3)VCRv_U3tu%Z{38H(=#+{R=h57NkBH$LniD~Yc_ynHF<s=#Wkny}(t
zjuaAc<M?SUnsTC;ed~52?M*nIT(}Aau_*%;OqTv(V5EqXcJ>4bq~l|*C$@G(1zbc)
zTMr=}U3WaT9yQcdt}yy1;jSbsq9djDj9-yed(u|I@!*c-(+v19d;p<ky`*AK6cJDy
zMnN<`h$ysicpb7#&8g=;d`QOl5bz|!4%fR-^R;9{j|P{Au-7EsP1(TV)(9_61+-Ri
z>){9W2NYQZCvpqv#of3}8pf4;3aT!d&`}se%4dgVli?$C(Tw2S)$GD(#D?9(C=V&E
zX)s=;)yi=!T8I{8Iz$Tx$|-$XWC%fkSCLliD1E<?rmZBvBa4I5oAj8(cRM3OZ_#1q
zrps4Z9^adlICMYvFpgE^%1jsMbh$MjlAhTV^vo|@0m*(`2+UayZ8-h)$d!nE6vp_z
zb@l8N6wyxI{9$&_Zlp+1dpeH_97hTAyhQmNKg%cF57G)~maK=JstKTi+R55GK?WHO
z#fZL$l#nU`ogv7QWTuy^pMnVOvT|^{7{EDDyo@6|s=W25AKG6>V;Y}V4aATwy4RQ4
zvIIQSTt*@)-6ieH(3iq-r6D{9#SnlHTv_vhR-;S_xZPKF@0pTO;1a<AEegO#_V6{t
zp$IYJrKb(`6gTcUf@{C#wY_<5E7D+h;@vebfHFpQhB$)sDF^}8yRSkFbd-L{>R?LU
z@+>F>neAC2j0Ii$o?TB$Ujf1f+UvJc#&B9X2__Q&>{&gE;kfCxHZPq;+2obia5%#+
zD8f99kxy-uQsn(Oe6<v?tS33KcBF_Fmj<`Rl(L<x7K33OOIq*b<zX8LNk6C18$u$m
zw3QYUiPh-m^FvDOF6qL^7u;wqaDx#fa@~M~clqXxn@K2bU=oZUJ0$k_Q~Q;7WYCkK
z7N8?TP<{jl^?1NvR>}zxIoiCvEHMFo3O^{w6b~XdR<h2cbti?%^Ugw^u#3fqxF}Z=
zS{>0_OLLa-o%1SD&D&wuTFn`1YgHEaD;Uml9*TkhhLlU$SOJRksea5@-F)n4(YT{W
zW3r*@w$Zr=ZIH*0`*c>~AT}6Z%tK!%V|b8viODOWkgxXNBnt#&wMHJUPmoAUt!mQd
z6BSf%!(_jaId}wkz42^aAs9nL@JPGfkQJ`Dhi`124~9}YSTdlLE8!-#Zq7Ke69d@`
z&)|@u^f9;qC6%Lt+}vT&<6FDVycFvfHPZ^at5e20Rn<ZrkEWDsVRQ<15x_|21Q>cI
zVRv9xUz_gSZfHOwIon(pp6jdHtY3wIU-h2azW^AOYdpV!rpzfg?|Waoh<?Z!=zr=b
z*Q5>SmZ0IG;|Mu=cdYI!N^D7iFLXL#FWzgJh>A#2w7CLp)Mjh~clB_ldq~Kwe>9!_
zZIMX{ppV<SV9fia*=LZf86&!x!t9e)=vk}9d_xg2Dwhg72MgEPta^(96&+QscaM$M
zlwWbF@UzyG1Ua-S&?or9=clIO`mm1V)R)^<x@?FV#h9L0-yEs9?&&w37{Y{LjDywh
z+jM?5o~>_euOEvvO(L=%wo)zfu$I!=);w`{uy1%ThX5jgPK3MY&{%y*U|=MP0x5xR
ze6$a@x_h!V(a_o)Qp~hqv_SSq%kt^MNowlwU=jp%#DbBUY*n_R?bKLm>SIqdzS+zj
zIh}XA^A7|bOLWSXzw}pd(O|234ky?+a1lDl!ypOYMU>~!NLn@>hT^9eYYUEoB9c4n
z?BL0Jq*bjVwwspWJM@68`Pm~avsk<4oZbV#&I1Dr36s_xxgC-$-%;BYOwit7A?MYt
z<pKlrLV02MedsfRTO1>Z#+=snAf_Yl?!4@Nr~vRjVpu8Nw#qhj&^!DL4m=uvja2_9
z@5?GZlm=6mDo-AuU32<txLG<>T!q#dXdo*kPH^8f(4<sj%iYSJd#k@u9mYP_^|A?b
zkXk>3YX~b2iBfN%GjXq^C>uN7c5+B9m82XlZ^Y$o<a{1`D0Dc{-DqBM^GU6T0p;aI
z`lhza^h_@cHW^^;ff4qsV{iVmPczzivoxcp5|1#hF!1EBwkPx62L@f^1<Q7hyPg8H
znLakat@sJ8J^sXU$)agbCqX@tI*i#js%Skj@wr+99SX-M&KiAFCB9{+p<gqKv{#}s
z1M)7=*GkxM&F~{MQv5p<mMU}9c}P(lst1MBQ!8X3-Tu)1M0=g(K7(x)7-d<@0FEc|
z&($DJX8Ywf)G`+!PmmlaPwi~WS%WtH*ZUW)*PX6+hK}leR_ewk8Q;D<rb?OS-Fw;a
z4ldL6WpM0K5%R+#jj?va+_{*5tB#jlEp(IXwowd3)i;PJec8Pi)Y+@Ocp)XUquS8K
z8D?hr^c;wi&GX7!PsM9p44NrEDp;NQtq-iqb>405sci<@T(nrrgHd8OLKhp4*2FhQ
zfsWll>%Wbsz9*k${}B(lKcD@Yll_v<{)No^A(`d(e3t#kKK>UN=Bppyp_Ko;auG=W
z{yPQ#&Gs7%17K#L`vSTA+PPr-iBSCy#%#<$QVGz*Uya$=e@y=W!I%}e$drxko@NcO
z`}41EvjW$Lvfd}S0z`)X)tH3?D18H*>fhb|wE*~Ovj4)Z06?9edq@5er~8Lv2J%(t
z06_Bo?<4=%tNABmAZLXGNXhxl7>I2B>oGI40!4X%{kPwZe=H>aVPwWHR2v|M_q*|b
zygI*47$XY@9S5+q`pp>d<1zmqfxdszy7{wW3b=sw?+N@~OtAvb(;pU7o$5Qbv+QVh
z87+c|a>v;Ql#0uS_D_fscd)c~Nv*x6!5wb1G%AXVY3gDhc3(^_AOQI?q_2DTz@XnU
zW-@mqFK~-1*sduY%y%wqV5{*3^ube&C<lG?+G0dSC3v2poWju7=)OIF1K2T|H94bM
z6ukCZEInLEWOyr$rEb`rI1wJytjVIg;rw83i2V$m8OWFMPj4(rpdH$^ACNVzlsh4$
z|2Rb7tP?==;7m}iGr$4SPgvM;jg}4jA~Qd+eJ+ZC{9)j^$y!%fd$c^3K*Qla9k15#
z*veAOAx(dV83y`u=&mtX5>o*j#qFnaFN}3c<gupngYcXYwio#)t*+{C&#w3P(lyza
zQQ%+J?P96s@*J=1A+nh5zBf%-N-oeXt(ZiUH!yof6ppK}(+V*h{^+uI#DN_a8Z2^w
z6Vy^z@={_%c{=p=eWfoIU%%vRv1_ijLftbl_Qt4XMp&0rDx~E$DAEk|+Ui@UD8B0S
zndBhua+c$2`}!=WfV^OIs8FZT`L$H-m_*&YA<!VKEQuPXgH}krF%}4J+yFOFy*P$0
z0V&i^a19>nm?F>p_C`EFH&A<Uugz0x+*KYu$EPlu)O+FhRNJ~(HKE}p`!wTX0=9eV
z+vL5Poq>JJ3YQw^gis!dx3Bi?QhJ`ar<q+^Lg>d~k;MA*K=(H@n;q!E<pl~oFZT^3
z0b#0qM7APwA%FnW#9V=Mh2*wqp{7Yp_+EWDZE^VR%z6M3qzhx2vrg-yt0wwXHmXpn
zG*MIqF@K&Qd0Da@p5+CWyp2V)_M-wfRE;d$ehM@96=}5@N7B>WgMr!t&duf6Jp_|&
zs~CW~JxH?b1fov9xcI~q2`@znz=TOE_Dp>2a*{Y^v#>-f`SwabukF;Ly59}FR6~~z
zL?8T$(HV;>Q}CpugB<^G@I+->Y+P^{uQHOj8AyIYDTV&a-OF)lsV4@VOOmt9w>#%1
zKDS5nvsX$-9B#2F%TdAa4f%1)t{2`B&iZwazDtO?L<z>of<e<<p^1y1q~z%<i&G_k
z$uP<o8NjQK8Z2xg+$zbx*apP}^*N`b2a?G=c~`RR;?ZMG?Cx4*q{Tx#DlWh29;q;5
z9v4D77)+iIpV=VAMTm3=Z@b)Fqu0+cc-lZvy$?TsPF*cIc2L+I-R)cYWZ^Syf~am=
zqZEux3c2?&NbWXD3NjkY@GeZ}A;#<Wu;}Pdx`f)*k8G4%OvBD4qG5>fDLJs1cqv|C
zmJ4*jK6+$1HR;>FM%q9|FQ+I^|0cKWiD|0U!)>|+UO|KaEm!wP1C=Gy9FHFw4j98(
zfo}<5wQ<1kq6%}2iB{LHAKP0Fx3qSm$b;@Y_P<GZ+qqmZi=zZ&eWzQBf$23@7ISeU
z+c*$ogfqZBiZ2Uxp1kZQOUS~Fq_M>07C@56?i>LWj1XJ^R6;0p_qH=0B@m;Sprm&&
z8ChZ`STQk(F!#=>BG8+DW|Rum%gCupR{m@Y^)awN5h^SRmO#Eh5x2#!Tieu8kD-F&
zG<}d_+Sk+Kb4m`kMlgLO4GM`;L81g4@%6i+)#U3}gQ`^pGfDDLTIkQD1q&RMd+%uJ
zWY@h#jG{yrp#{1=T<FQg#}`=gW12ib45)d|Rmv)7$R!V{LSeyPTna&XN{P)Wxp?@{
zKlG9vH$5y80ZnZjeajD((;+3B-`K_<uPX?<?lW1lU`S_{7h>ccLJRWyx+bcU8n!;3
zl_g>wNY<ytVO)>RWCZ2!dJraW7$@%n2U?bHV^L*wJ3g2Aqr2T8bNHG2QD?0@No?`m
zBqvroEG>ejEB*x4KW#TA(^Px?Sly~7F%FWwR=uZ7fS*V6DYV{VTNR9F&Ky1^H3G>3
zy_lsnh5rXjwG>mN-D>Bm8!In(&zq_`KB1x^FwMABv_17m+i(@3{GvL7*|){W9jAj4
zq*D<^D5GzBA~6>kC|BnpC+!(gS?6h)UO!ROLkffD9Uw*$S$Sw+J(jUs@vtJO@dK{7
z9x<b|iz=))KKR+8zQQy-j#gEhP-us;9)kLTzD#-X)68}>(|}%rj^Gqn!SDx9oEW_=
zRX)QreD+!PBc?P0*%Hy0Ch7e=ycW0ukBhM{9wsqKUegJo1_`6O`+lj=fJBgIU8Ckk
zLN~s(S`ATPgOj$zT$scqR{!jkqxW1oR?fa;N@R3!U(FakLEr&;qi3c(`T@GvWjk2|
z_9LdVS}b2?P-J=(KVc0Q@?wVr*E_4B#*YGY*&|Yf2k#ECG8&+{Jw1p&S$Z`z)P3$Q
z-Ool}I2jHRYN5`$zKyMkE5IfTdTYMVd|866i8RHW$7K+Eb5cm{0cMzLA@Hu8w~Z2n
z3;kwn(UZE_CxT$p&LpJ3g3rzkVC5OvQ&P;f*bP6ctxO)!+&-8bj2M9t=>k%^uc|W3
zwy-L*jF$Mq27@s28L_)Kk-QgO#Y(>xm!QVQ4U6hKVU6g}o@Y+;)w2gBCFeIh5?B%%
zRjS6X3f1CDK(O#%c`xsOTucfVF}!T)-jMB!a5%zVqp_~^q^0pMxolS$AmeLlxb3~f
z4rh3B7(JyDg6-DuN{z)O<xQ0WK9`$*CvC`H$u^C>CjlDvwkGyK1ccO0s*7lAlUaZs
zMryj4)$!fwGxgfnCELttuNhn`Weh1I8GMII7|&z&kEu9`8gZlQ;b0cQswVZgsB_*(
zdX<!Qs6J*lE^&EL_ejvBa#hPk;zT5s&xkb8rLE~&_!0irYQ7M%;w~P+t7YDzR{PZ6
zx9#EsIoQn>eZ0zKqPfvwmBtm1HD<5uET5(py7gG(^kg5j4Jc?=-&I~ogLEHkCI=^B
zcDiAhi#Le5bn;?Jr?uXd<7wccc<H*zFHK*A79~Nw+qDcg*0#3V+3&C(COPYHIdw~^
zvQ)EXF#_Sh<>Z|TMn>eVuwTF%9XEo(*#=3^Qd<P^buVP{eJc{BvhG+2BwNl<;#jOT
zyXnDO?L4_a)q0HG#@ddwojo}zG<@4hJum;Hj;Tl+k8tFJ@Uy%zTNtkioToTE{wk^_
zZm>M6`VLQ?%jk4cHmjXqY$%Y(%a7zy;?J>v#2sZ8=k5+09CKw6@RTfOjXNXVU)Ygb
zdCPkif#?{Awp<5B9#BZZG*zx2g67Fc#u^QMD}{#m8IuW;k@bN$a+|A@$Xk>cD&t_^
z$F{8@FZZp_HeLxkGs104_R&XJroJr{fRv}gumO!nZkKCLlCH6XAHjbsV_Npu@`E8}
z>!vTG&eO9xPCXg}?oZYFU{ul7tl{lZZif_%($<LJah9_OEwSc_0p^Lqa;tMGE@#oN
zC*k$YlvpE^&QN;fvi&sBQG$?TWz~Ht(F;lQQ$c2<rSTjdrAPAZ1==-)4@STRiz2l!
za*1?51-08Zo4BV!1Tw4s&LZ!x5WgmxKUatx|Eos*56c|@-+Jq89A8@}L~I;iYA;3L
z-(Sk8UzR_7eed@s#WxrK*UKN6fsGJ`FM6jyol^#8B4!|$pB1PE3IHy6W@TWzcm0F7
ziLI>@P+rxTj*;otB@kcTzt^?<v{!*LtUw{w@7k+O3_wj&po9+-usOrQ0n}b)`=Y^0
z#Kgu5EKga0Z~IHlmHvyEE4=_W_gAe~dV#NcLG()WDvA<cz9`L{oa{L1>0Mo2>GTct
zjclE4ZLKWLo#<>GOsRmC#1HI<uakVQw|^iU`^EXcg=1NO!lS?y*k4v$GXR$}uy8QY
zu>;E`CMKZ#Dm&A?^Jr)eoI-J1XGcqYCI$u$qu=K8#|rwtJ<UIR#|#u?<p5UXjI2PR
zA>dMUW;S-9w@gI96)#_?mjCV^<F6XB0H74<-}lLviu3D@^G7W@#^2U<{ABwd8KeN<
z#rhw{{bmcid+3;0e=pF1SNR_p1I3sa8GbL50l?ei-$!O)rUR;S{aPgh7=DFP?m1#M
zp!P7+JzN^VSB=;2m+5a){q2xh0UUHJK;@?2NB$K`xo2cn)-NOf4&nfS+Fk#8$gDut
zEEDi{{e9#gpQQibHVg2=WMjW?vVSb*|H&9Ae#~%>HUi^M%Vz$;_{)=y9jLkctJ`c}
zALZX?`yT*}pZH1tfyVTcUN7+eWBMk+_N%V%Pfg@6*B<j%MGN4~B+AGH1Vz4TV|>%~
z{a3;;04PHC_c{9u24Uy;tEEiz@0z_4DpHO>!c+5(nw^KAi6jI)+A5=SoN}^Bn)0y`
zQ#CRRY;V#rvC=E8iz(M<<RT+EEF7TlyL2us^S7;j+O2-bVGC?wfCG0lJ5i;CW4%hm
z(ResI@buOPVR}jsHDq~I+JQ`ytLu7+Y2yuRx9cmZ7ZP4JxKs~za6WmYzP>%1J!@d4
z9?Fytt#BImte(#)9=Um)dNy%(umndYsygI!WK-#;VJB|_nrWcKA`?ecidAz#L%Fzb
z*qR@ZV_K>X8S2;<;OC9|23r?v*SS4Za&0sfO%+u}%S))eKiaWNtKG1gn+GrkYs_?O
zQypgjG4$$lol{#vxnsT{9kCieD+cNdP63LzY;iFmo}%X_%AKz@ru{t`h;iv|a>d1G
z7Y$?*uLj|D2vV#!RyT5oA#4Zq64st_am*q@&vxnN$X~oLx6XKxlUe~JFXfC|F|L>_
z%vdP*rq)}gjy@+05Nag`p%4z&N4DpU&vSsK6t(iU>dPq##)kLou4O4M;7Wa=uev|r
z58p7DC1JfE&(0|4y-tFwHP`34X^i=}Vq1P`#GKIX4kX&bKsv%eKQa#@>ytiXA-^*H
z<{M9z+5&0JYvJi=zb=flmVl(mI&3g|6t6OWf3Ez4531hDh<a`6s@S!4E@m@N<<!!=
z%u;>s`s?T!uQVpo&EY+J_L|?GDJ$33GwmaJ*A|*H!R*YXTpO&Wcc{7}u+~`=^2DRt
zNA==|O>gh$GLYExA2IC6U*uJbiBG#wq_K3t?GSch<vrwgS>GGNBvml-7q^JLC@!La
z`xqlfKmbCKC@Kt=E_`H;_LLiM$&Asm`VBYBVPt$WJ@d|u2z5SGhDOjLTR&o`(t*D$
zU(CBSG<O;^8hXcNZc6e3EQ*94q)SO%>^xds6gRicepBwCEiki9@_Javu!3-76lxh~
zjEI1)nJ66E5Z`>CgyChWfOG3)xa0Hpaz*bGTS00eWtP>n&YvdnyHCT$i6Mthu1H_u
zPQYS7b=7p(d}yCcgb~Gl(XehXW(Pv`%n52?Awj}{;F3EsxTD)8bS0tkMz_t~&kGJ!
zIU)*%v*EOD_po+gTRX}K_L($d4i6U+4HRJ5R_p|MG)vFV0`t8XDlBN94&W?nw;?<E
z1e`LBIqx8Htoz~`dHrxtBuyys)kArLP}JUJRIlbcP@z-g(BCssU@G>*4SjD$`t6?o
zDI*0ieoY|%TMo_-87c6Q@g?&F05h`RGSII-`~HmlKc*(kz?H4QF@RAH7)FSgf!I3m
z(}C^-6O=DW-#vQ!U-Bklb7M0g&ceWG@JrhC&ENk|z5ydLBXA;Ffol6Kz^og%IE0P?
z_<UsricJF5H2>K<X<HKmV+T{9f4{EA1pwEV{QY<a#-?wl_m5gZjKDt2f3W?l74#1)
zZ+>M0|2*zD+b^9UCg7^wUt{98i|PBE{zFXsDGdG;Ab(y2``?nJ0AFL~_rd=PXuvkc
zU$1`;Xw35BY7Fvr8%+E;>4LSV^9ZyU2#BXNh%|_pf}<GWG}5QiGKiQo(q{<aw3vd5
z7%y3+WmpV_8W-!egla(|qETe;xhcP@^?!Hj{+sf<-0I(aZU8Wt1Mj8(_NM&1VgdZL
zrNo!3=liY!|Gsm>x52-=eD6v8qdp7ZtLp6c&-v?o05463Zzts6&qsusdfY4<g3p5P
zQcWK-V~5bo9^R_D6sJdVCN2xH+-AlfOh~07BY8f1gZjlrnT@5Vt4k4q`Vsmfqv%>;
zdmT+-MzxgMl;||0n-Fu1X_$ni4z_DNdahw+txh-EIp}dj?%wnmsmU_&aqbD3u1~{N
z>RZKxRIMy5-8W2aO!^W_5e<$ijv$aR(HXM5<EbVEZln8Q9|5eeVIC3d^185j;o;D1
zH~ytwO}W~c36EnQdbD6jD7iZMlwkWY_wPCR^t}q%SlIP~F6?`>wp!2Bt4}2}%I4~`
zx9lLm=UU?Zinc~7nWESg&*RK_fvq1izc`4g9`WWTFySVh)OH{lz%)TK#o>n8cx8Wi
zx^XtU!_lGyiDI(SRWftBxy<wVu^&Yw5?Ih}wMoB9p~x{Y=S47A$}^o%yPCxz19DnZ
z^+(u`L9=?&-gIbd?bLrP79g!TjBI&?%L(DJ{-BFrc*4=<+3R5Ym8Jt%quG~XP+C@7
zub}Xoqq;|pPqr!o>EI<9rSIx1C<M?UUa@KDD1|CSwl;WfYR_LK5!yMDtCx&A!m#C#
zn~N-aUgLZ3YE`*|cCk}rKBTQFu{!sJ4tb!%czUVXE+I4#m1m#|HzfBA6^lsvEO_8S
zWMXf}%H%{PgNDf~j+t<GH6%M1LMf>C*L}=7HVl|qNnZ13A}Z-LLSD#>u!<eZKF|0!
z2oFQeD*_E1o~UWhTx$*w#zo_lVGylvzuE<gscH!v!*=R*pFJ-XS9!Mm2*RDlAqGOM
za+P$e7D^IP0L$xLI!4qC5`76BF+rz`6J<PJ4>D*>o+GJgK<ZXfCitpLFyFZUOD)T-
zI%iMGvIBcmgrU_2U(?*z0SAE{+2%KV2FsOs^Ftysb{*kKq3JTGFhd2(QgfBD?Mx^F
zc}y9ivHiG<u%^eW&MHp$3iF-_A%uowSW<e!)pI<&0v=3;brB$1hV5p_Rxth8I7;5q
z#>KJTOFP>!F8ZpV8ZeA>)`Bt1?H``534$t(^U%K7OyBXzH0Ff8kfZ>+`6M?xr)tBO
zI*;4RVtf(CVfmnyF|k^QR_(#@^|e~{b9tI7B?JStI+hkiHe+1C2IkS$@C`-1WFKx#
zcdH=D#;}G~kU8}@ta%%JHJDrUa5jno2B;lM4;_ZO8GD&W%gI_~f?PdnF`_mlTXt|E
zK!n2r|1nY&t-xmt)5ir#s>>X&yglj`NG$WT1mfSh8(Rl*o;JTc&gS8vi-5CxMeV*I
zjo9Bg3Z((Yfa1?=kjPe$n(+z(OPu6Mk7K;CLL<k1YJdGh6UbDa(iV%XR^1CVHx7)o
z3!*vEZkwPC6a^APt=W?_j*oCXWXjt<uZEq8Xff55sU9$9os$qh@9T2jqz>xi8Qvah
zNC-D5hIU&zHJ5Gp<n8JBoH$Si{E3;K`@=4>N|Z~7r)Mjn#&zS5cG=w#q$XnxeCd01
ze5Q*!Q`fhdKSe-?PpaiyO@!Psu<BqCm9)PB;Q)_O2pHvR!55A8-8F$FQ_`E@4dbJc
zs-%iHel<Yq!l92XJWcAYf~dWK#d6?_vJrblhd3T@GQ-^9)Rmm}@H5FOz$*M?$!`}_
zD_n5-eUi)>dpZdvyHJ4CKvwKJzv<R{Y4#1IVd~5z`tT;bx>mtTZmb6|HU-Y--jx&r
zl;vai8&o*LFoqk;*BCPLer8?vAn)5pU<m2DEVT-W7K+%TL8~a~=%6Yx^Nn+*Gscmq
zA{${z;;ig02|YQjwo<ku1Nq#jUJeLJh@c;}3&}vj#u5rW6GAo$pzk$ta+}5S{J=D*
zdg-v&6+)<!l_+Q5oW^aFp-0umKCt*+!n)%P7Ipz`n*ia{sgg6-Y!y7bXfwv}OSHfz
z)2MzZ%F!xibz&+>NL%{tEXYZLRc9ZdPle)pE`5bGL*m(rFBx9*DdISq;mL2F7*;V|
zvoNH9<t!cJ^AK%4Z)#3G=T8?{S6K;s5Qg<Y4_bd%tIMJ-_jRGjgQ0+w?Tf@XFrQq4
z4NNOFHXqnF*xTXBI5X&^i*j1(OZEwDM<EbZ*mNr`(9rQh)(Ol{yd(ADoVUu?gw3SO
zyenMUhD~i9B26I=Urx2xS)W?wRm*#&kvmm@8y<0KhU<fIk*bWEDqHNjw=Z<|k%?H@
zh%9)dsAg5-x5$s4z6#ZYwE8rVz(SMh!=iq<k0G8Ry^f2YY;4Lo3~~o9Od%6?PaOZM
zfc`z0-5<xltE~SJ$C<z8#D5yc0bhm7fnooDgx3Lo?ZyKDN_l)uvVq9Wx4*x^zyM$-
z{Pp{<8z%h>{{p^l*7UQ>f9>}3KM?6(yZOM;tC<@Cx9s8IfTI^THa9hM0!p#70{6i(
zcXE_Bb`Y|)wzIYQA`Q<DNB>tK2QY8|zY(&vvUO0h(>DZer6p|aVs2=xC?@!KWEU1v
z{%y0LyXm_{5EMyK2~iMGP*4zY;4hH7SrDNw-~IRk`})n7Kf%7gz`-HFzr4V}zWf8?
z%L@z~5)uLe_!|P^%ZL2+0YED-2yo!L{_EFWD+nqKNC$`^I4B7S7%C_@D(Kxi5JC`8
z5OAQgVBnx2f4!h#VBw&^AwVJF!GLezLIDARga8490fPm)^yN+95Rg#NAfR9{u&8M0
z7&uSVlP29JFtLc)ITCAX$KkL^NEroinOs?b#(3X8`F}q8i!~%D1QZxF@P~A$AYfo1
zkf2bIkl)_`3Oq1KC{&>62O~3^eK|BbiDRXL{xiRr!@Cb42*A<7P{C0__&|2$94;75
zRT5c8LVKpn2~Ekh(_>UggD2S{(g)d?V*Dv-MP8>Q`@`9glbsIx!Lou1>z?Cw(;UN1
z>swX!BQOPU9ooK+ot*F}oV)|^nd)rh3x<ZY4OKThU!hTbGmNXY;P2y=P+c^kT61%r
zehJv<wX|qbv3%NMAsk(s!cQZnl@^fFGXtw#3|WF>CC9W&Q96VxN*1IX4DGwUwQcdP
zt)Dqiu^k;0L+Yhp7BTX1>kv^$p#I9mDlZv|C&&3%l)7}tj={voXhpW(Y|0%PSqoEJ
zqyf8hd7{wD|Hs{1#>KI1?Y@mW!D%G8JB<f-cXxM(;1b;3J-E9|fCRTdaCZyAJy>!(
zYrp&5S!?fg&U?<ipY9iaRdjXrs5whU&G~=Evx*QsJwhHtg+D@6ySygH7^Zc7q!SSR
zr;ulRVV@ru?KLZF9PdvJt#I68S`8>H<u9&|(viI%7|hg6$Kb=W>0u;EJZE&WyrYai
z260LqK@BJ*vdeuDj?w$X8a##U(sx(a#3NT7>ltCj(<9ANs2mRDMCQ%4P*J_oP=U_W
z)g^U-Zslqfu^jseu;A?F{elJc`|tiKqWK>g78YW_6-Qp;AuL()j(rO~P->Qmzl`WX
z>ks!LQ1+(-BCeK8I1Y}zXBeEH03+f3Jdc*s*i)4jh*ZDdn%MgHwr^1>pWE(5Zzkm;
zT%6d^&q&`Z&PXJ4aH9IC6ctjd-M*=>=}bnM!IP=?=o!URTDbrW48uG0!D%_Q-K5tE
z_)9-;rw-`Ywhr6+e>kB=tqGWaZVjE;HJ1dDZ&*Z9gbmKE%C#YFEt`t#GMSTqx((3{
zXZa}iX+WS}=+yUgvKbaVtH?e&GQ-tZXs4-?Gf4@`g#+hGo;}L!Bi??3SBP0;ejmHq
z2xJ%wR4NE)3SfZ@qXJrnX{`#hg<k@;YA$6N{MXE*pfs#yl<?&Le9A4TI=<S0_@uj&
zruEW3j$OqL3oWXvQ_*h#<#TM8UAbz{Fv)s0AfSOK4=(zr?-KdC&{1G-e^{H#)_zhb
zU__HQ$~Nl&Yc@4LnO$qgc><;t!b0z3OS~|9qpzXovGHE!ti`Q)t!_v^ASw9TU|F6}
zXDAK3I1_KgIEXCxb5=dTlnaS5nK@D<5Q7`dGdLq-6}fMisQ5_8X!bCi-mgT!2ALPw
zu=z@A24Y4(#E;bw!@2~{RhFm3L@I1uJ{;+@%=S)>v)#^Un`uHjHB>7XyV!E$Ah<}~
z8gg#Z7HMu)Q8ceYi=^cW=1aT1lwB=Z0TLWvhT?MKl^c4aJ|nA}wNQdkH<2mNN6k0l
zW&vX+uyP#ePCS8Z`R;MkM?x-@ZOF}fO(GamuTG6}cGew8DZhOheW~^^AWGP=&3p`O
zW*rYPpH{*i!D08({;))H1Uhutf?v|Yq`2bC=M=vhAr^!XW|O}gO)GtX@WOOb;i41w
zm~d>3f5ii+E<84cFTBQz)a3i&^$kf{J|aKF7`{+O8|#_}6H6@sksQ{U?@NGNsti=>
zy3uwxK5bjOJ6byX-ouu@FV=#*y^C)+UL_(_mJt;+6~S9tWK3fhmSed=3o!%=m}oOU
zL4cx(B7i}Z0Qaf}HYRE|#jRkqfu2v3RMl1wt)-RSmG72V+pI%=ru9KmIa!`Jfk;wC
zJGw7XTW(Y@U?)w`#UdD8lcDLzkooc$lmhGHRBa)pyb&rCqs^MoLCnNH-mdh8VDg4#
z6@(Yojykx87galO0_u+DPDNvgC~d?(w>v2<^r`+x%ABDk{?z|Hz)9nKNt&PoXwc@f
zIp@U>5<+lYT=V$Vm<A!6&$Kd-u?_~A)u%ECodTHznB>QVRn;nV1M&EytWDyj4=PON
zUggabs!YbL16lc5G$N&S<)J_Nmd(RNR{5CYJh$^$!ku3pu1i+xWI8ub=;p!ay5`zD
zf8Qhbs+rohC&b+0`@_j}n{K+}clA~3v_j^Fzi)n_Qrhg`%LN=s+C7d<o2LR59)Oy|
zPL<9oY8E>v=1j=Tb1@dWP#3x{q1)42I$qjZYJLLT8aX}>--_7hI}yJUJV_#JH+!FL
zbqf)sRZexHt%);6Wxk*{^lA7#Utf=BA=Rtcnz8M6X4|Vnec05I)49%gm_+rSdu?EA
zr=;H@^DzwVJ7~?7ic*c9i0T~deZR9E=Q<GmgKD(H@DK)tJrl8g7tqQH8u5Y#fkV$(
zTUH%@CYi!iE6A3OPW%FLM+?nG1_;^)7y<yVOuORGr*fI*z16{w8DmZ0`<bIw;-x%N
zkx$1_npESfRIF)~pRi;hCE-9sP&I9GNU7&{`w7O3)G*=nH=%lco0cy>0aA}A0xZ_v
z&8_VN+x4mC#<emlGuxB`6zN}6=4SM9hI$NrdXY<*Zu@@%x{<5S1bYSEcBvm}>b`_3
z>^@x|%C;D!y7expSEkgZXP2tq6K}qMqhEPbRr8~-uAe?!ZqYAp;U#QIfvgLn8PDdr
zWau`|wCw{MhQw(!ZHKl44o?$RET=AQS;)7M_DRbCuh;}XBjb(lhN(6+G%&O>5HczO
zDk=Hcz!9e$Hm0aJzB=?Fkmu=%<RHKrB_!n*8+FLLXV%1v5TF}u?48i*XDC@VZ#?y0
zVy*a_TB@!+rp7Ea9@9a#;)?{nyxoM;Hf`;j8>(_P;y$5^k@#lDlx$?uZrA89SMEjd
zsXS7}vurdaF6;S(k?9P2045^$cG3VM;9KW@0$tN%j&`Y+kuUz_hjoh`pytxL7W@0n
zil$e&T#R)cjH^D(nM^n@Bhjcyr36gTODZX~Z!^G=W0m}N7;L&B@oTYgI|AM>KFRbl
zwtwBDn`CAaaj(Z-A~q2;KW=>qt(sV2N(rEZR){`viPLV!wt<n$uIwiu`N*G(%EdP&
zQ>xmcwOQDvTs~zeCF9Jh1P3sE=X40OCy!img*MWMkmhCSbLj7TTuZl?9lp4}41NH3
zzqlvSAlO#zm>wJpK58xYn$G^VycoZ{Y$9PzC!!3^mTk_dvE=bE<U&7qkir&NM_61`
zP~NV&7$yhr#KvbtHu^n{MzIbK0)-|pp-_utaG0(Gq>#;65h#!~gB&rJDePg(kWMF(
zBwo~b;=v}q^vQAAO3iI<=75dNGq7>jy>JNdu|04v%Lu&zzb}M|L|>pTGCsQ+T;Rdf
z;3Zz+_Z98w+OGUF54n}c&yJ@#wCy7**}~>F77l}UAv=@&-Y7hcplS2q%1Am1BxpT#
zG}%JE!U*9gny~;g9QSYHeRupt1LEmqZRCR^n})6SH%0W#u*u|iE~?Va3CB*&GAWXT
zvyDqM%-9T&?&LM8Bq*_{=tV<NID4{$kW0V`1Vozf`&v}<z}JI>s%5P{3{8=4OOR+f
z+ptVq_|che(i5GQ&3Tq_Cik4>wZ#tGYqDY3pY>Rr;0d9S5D0#2-FZft9lksl$y99J
z#DOl5@CX_bs++u8G_V+?88@TxqgCJOIs_XVQo<EWu;63gD1|6{Nu2dzO(fsrhX*Of
z*Bj(nIaAKbxFt{7%X{C0b%2-nGM=>$2<&@8{fRCUq>4FIqG)}etlwT#B2q;Kp!*8<
zlQQP^eUx_WS(Bd~*rC`rTTRJ6x9?tk*X5fbElyLrGji)I=eutC6CjI!f8y=-`${nE
z4iquoV{_6%c-j3#*oT;Vb(P5MSTnNtA*sZbBz)casQIaCYz2TzUpr<K9LXdi!~{&=
zz67iD*p6URU>i?xJyTe}bLXeD6&MOBkEaLBH)c3jCT#v1HYU~#8Do)JfPAB`IPl`l
z(Q7a&0}>M)0C9L$k@4#Cm?SvOf#J^!?`|6E{D*5KwvIVJXQTA*P^NHFNwP9cz!qDp
zCw)u&%z>^An=8z2K0GT<;f#@lh^+v{r<yR^*BuQJ+Ssl+&Xnh6nWwf>b_UfOrl$E5
zut~kSMpz<bhYh~5=p{QnK6TOis!SQy&WO7;>6so8^$E#&%8x8~PohKdPoy_DwawTM
z%cVta>MI-{A|QX{_hOycqVY2lg=XUCv!D5>2WsOrw_QUruCG^qh$;5&ne^GmldYfK
zd<w@BycQZ<NlAIDw)=Gq8heI`$0tutNl}$gX;f(<4fiE53Ozs^nw7QG)|;#0YWzyU
z^Mq))e#4Bo9_bF-j6DloCfd9Xkm+4ST8OVJ3g{axm~&ylQ1HrNdizlWaW^{BuCE~&
z&(h6j$!0223+g(t>v_zhe*Qg1J=`I=$3qHq-m>+Jz7f~bdT*;A^c2nPqiW-w|NOEv
ztYWj<Yral?rf|iA*<L2!PH?ki72Ch?q?m9XG$KJy*rhCEK+HFvDmpQYI%_Y=(n(4v
z(u4<^+fpR!(3l0z985cykz8_fYeN^BLA>$!76W7YcR|2V+h06ol7X0twb;y8ttv3y
z;s+y0?T$FrI_y8g%Ak)KU3Z|ljEe&x#J-%h1Ul0gK}5m%j$aSR3HOi@eqheR%MStD
z%o6g5*(xAhO?b}b8edRG3_#B~^D59_xAvrvere>NrCWfJqGVyCqjmMd-Gk47q@r@L
zX6%`V0l2WJT9EL48W`C-Pe}R(;j;<ah1WCCI<Q&n;hbPNjdO34<keX=^=;_kF+9Eh
zSL3CAbINsf%36^Pdym2~9Wtw12phs40UtX#=;db~H&VexHp&>(z8JaH9N{1UvJiJ>
zU1Tt4WC>VkFgYRt$x??0cI9xpLII<Aeq`vQ^A}TB^e)dc>MK3|1l&e<S>z&BbSusq
z77BE_1+g?ezAIUUImziMb~)h-KfHPW6HxXdc$)Y(O`5ug`J%W6_YS$d?kAu}vA3N^
zrX1G?Mf;K-;P1lqE>p8BK%`OU9!bmG8AY1k9Noe`cRq0eLjkrhtx|2^Cx8Z7Z`2fG
zV>=41;4sV+A@n>>>(F-PzU9;#$_)J4fLLwgEnjMDBZEZ$CcfHc9C0{QRb5>*5Wj3L
zWg-KURFcKWcVv?l29xi4ij@uQKfqAuvvJ>juHljbqkcwREP!&WIXf=7fAib&txbiH
zy1|aO=+>sXA78V-p*pVL&MDfgAJ<ZMnqXO0YG8XUNT7Ow$;V&?ho*FkBaZfSBblq>
zPr%$v#lXycFa1^hBKXg~&ZyqaJ7uco^SpiA7|EJPeZ+VglDd&yL5qE-;-Q>(QQB`o
z?XW>9B(ij7XI5-sYB4!u`;UG~5ipOM!_`BJUvI(;13q(bB0*)XBi1)IGL34}^dX`@
zmgW@CC@3qiu3u9nt#?N_W9GgcQ_>xG;5<yG`w1w?SZJ^_lWoAi<Q7t5Pp2A;2VG^$
zkCKVW51ST9ckaAuB&5J=qPQktQB8r@(!Hr0M{4gMCcbQ`iPz#ExC#H%Q~Bs+_1x2*
z{Nw#P;*M$UQ!2W>w?$VLYYl3sWDFGBo2c<>rE3W8sLo__N4{jH7IO$EDHkpXVtRm<
zT#QL<>iCi!3=-c@Kz@sRFTRtu0)hUHvo^==vihikOvjLZG*4_ox_Qq;m$Iw%$&VsK
zrqraq5)<oMo^PGln(cWB-?`=u6&Z=sXMbCsuPpJ+*`TX)<|n;ytnA6t5oE3cjF_<Q
za`u^$aa**I-<eSB->``%Y*5YFQMe~VX0t9!VI#9Z9!s)tFW<1V$q9!{K3|R9_g4Oq
zozQ|$9hDTwloE)nH46VdD?#+@#pAV19aZg`KFKqQ;7ekD|L)P$5)PkAzRwviPrVco
z<rk!1yF+PZ3Mv1YA;nQ-#*u}osxDX{sBlLq&rj*I(rQTiQn9M4O)T++cDU^35;8#J
zEE%Wy8Q}Kl6FU0q)%eWuY&J>xn%UHqTf|S$AgRk#c0aL)4@p!$hFLxL<^HxD(4tmZ
z_1ng@zb(_iL;X21!|>EOqbg!+-baU7Z(iA(*L!7Tam$LH<LYCMq6Qkn^o__D8WJDk
zjYxwZRTY;DqCB0(!X!lqz3S(4=?U{VnIXo*ulj~pbdA_OPh>IS|FW4Rrsj!uV=!1*
zqHge#CAv$l?kCFbZ>;d!vYJ@QUp_UcNo;-7K*ckQhdwlJ`y6nsd)yOd)kr0=TQga=
z935phY)+!K6&yVe&~tA3YV03i6q);A8hdU;{iu=xE8q+?Vcq+@v^6)9xLhL*k<r%N
zm_Eyh+vS;EQ(w2-a-hO#*n-9m3_zMk)q|)%+V&3!2+!>>kFB$9T2qwrCHPiXmKHz$
zt^6-z2QZi%HP4`kUg<$gBYjM28XD||8T5|wRzap0K9r00Rv?yd)Y68hA+pO<S%gQM
zB*Er;dd)|_V@u?v&XN0MBS!4F>K&f9cQ2dHrVvHuv%nj#0L7NnSG5@Hd^}skvdq`R
zCr|cTKC~eGglEcjUg;FKs@2b>!VXa9MwC2+8PRW62P}}B$)yh9v=n4jxrK(1WF~IS
zo|Dm|_pE!Y)NYz*N*W0W$JYiA!&Y_Lua;^VT+)s?Ov;K=UyTcqRc^RiCwQ+g2FVOF
znuiU}U`_!zqS~gAjaPiXl!&&%&Ye^R+@q<qek5VKUex|>HT1C*FVXEu0!X=VMn%)#
zZIXthhV2t<bcHkM!mh@P^rgPQkg4vyM4$0gI<M<3yti?F?b{@z({80XI7EyJ9|Bn&
z(V$G5Q95D6+-h1^QuGm47V!OS9J$W1>4$B$R!7fHLWw?#^~Y({ZhJM7rSyBAmU2e>
z{6s~Pez=|Da0*{___uCb*$T?BXmSMDST}qpVnQLZG>VlR5!|6^9^G{?_N}>;wrjBc
zDaJ2)Z!ckXOrBgm5UgwrQj?2yZY;3E@~`8yW(c)oj0ccB5XDAUuYDgCm(|zyyvQs?
z)Fnj%Lg~Uz=Mpf$mm*sqlmqktjX>uiu=GbLIE=GL+cHm+bZlv5q5ItDC9S3Err&z=
zRU^3Ip#+De0>BD3_BKXR>q{^N)spqPqV?$mQ3q4u;A7q0?fq22m!ANpbmrDZ;MuIb
z@R9@lvL@9K{JH(f!Su*V()MgW>#8tE4KX=&Bt?Ge=hs@5P71Oei431%q|L;k4y2+3
zWWGaZi4qD;;N#LstKR5;`i5yse+nQ#06K8NL^5O3<Vz4JBM_ksmO(>zW>kJrk)?pF
zAKg2TVBDwYmZy!NoevV(qGhOaNrx)0{p|^)@Y}N1J*z|MJut5|cN#k>X@>l>)~ZF?
z2Qrq=*^6m$HC@RHDJBG}kIy~dT+sb-bab2AZONm815J#V#;CYcG|@zXhu6&q^50Tk
zKMs_m9`oX%te5RDHtM*SPuq*2cg!Rvi!f;9#<Z#H4o#5)*|MWPikYikb61k?twmRD
z^36blU?zZ2hCY!;uY&#5Z?2{v{46;?+gfz?=SX~K=`f)|#g1CDyc5wC;oFKM8=!i#
zJVR#NTxcX>l1-sT>Iy|Fi-SqhIQHETw7u`++E1`@b#anPu^g?os!$QS9DOGyoVpJT
z^}4=+fbA$NMSm{6HbK+i%o8Jt1XyizF0;Y1P@z#iUWT;`g@#_f0%Cf;LqkQ}#A+Nq
z9n#9@umIoDQ&#^nq$7xKAu(*=yggOFa$j@Soj<HuHe9tJ5x0y%&&1{W0^E-@IB^M+
z$#SJHO~kC?+ojGk-r^K-Gmi8z_E*31*xGzgSyOqdUi0H6pONfG_>pR<X}>XHR#9Da
zlRJ{RKAsHXG>G&tGE{}LZ$N#RC)4ME_G$r{9^k2v7+{Cy2f_N)bo}nRC*6kgOJxDG
zokjDfYnNlQFl%^Om$z0)ms$#BG`UWrOZXsIleL7xcNrm9V_OL6s~KK~r<IYv5B~l>
z@NR`PN@y$5JwTadPEKRMVU`|0(Jm($n9q)Y`D5MWm=o`w)Z+B9$J$&?5&(gWMd~CQ
z>9Vb}ef0{-l&Xd;i%Hxf>N>J|%|I`H5!SUnO(S8fk{*N=Q&LU_RFVQQ;lay|RE&)k
zvk~@aA!Mqv8Ew*sv1FKMuK(S@bU=ILid!WD#&w*(+`?C}n5A>Tr3YWJ4x1FpNr$<}
z5~ljZ=#QeB3DLoR<c<PI|ICEo0ah1GWc%0;$GjppQT&r)qp{^#3$utimcY2;J{p!H
zHxAm14W6ks5}M?XB>{Ns^XI-3Xyd4pro1<UulRb$D$o5<9<Fp&>eO{M&sYA`PfWID
z%D49CAg;UDyLBw9d2TibQWiI@U9U?jdS^oAj|HL$IanduUz~~B9mY7W+Cw|H-{b@z
znNJEUh8K!Ow=_yen}QR_bGK2})VFsc#`H~VaXvMBdrp*_0Q+UE)Y^ULwacwHP}yC9
zvbAUm3xe8t{D0drvp7{eg#(gZUU3T!Y)VYE#ao>O>c~}IM28D%^40Wt9VXw6h^_d1
zA9jZ%zTOOjoSenRNuj6c4-|eMi!@~Z{b&OhuszakxXQw=_fPx73>T#0wMv!|rIL%6
z>5YtsJIL~x`s-Rvq)jRq7iEGXmSYNg#P}MKk(PKZXU5ZTrdU3%js21T{2HqH?_}t|
zR)~Kt|A$%8Kc*-DJjM8bt1$Vc>fizkE!eriN(j&&0tpb<+v8W^NS41t=&X#L-kQ5G
z{Z(D^kJ-}y7}ozpT>^ID0}C^_z&;;fz>w{iH8l@7Dlp;4{mUf#e<&`oH@3EW>-fgi
z&B@8p<*ksjlcKPyxaz-aOSt}+(fyCH{-0}0*ujK77{v#Hez_2Gk+5@c{h?3*v9W`V
z1%C->{sxjXbM*AGG<UFav^W1NB?<Z+&i_9pIDbhnev`iZVl~0FB3Qva{A*&H9bAlp
z9Sp6qg1eaWZ_G+8tS#MK%$dN)@o$0s4*vhY42=7i%{Vs+E4a_W)gnkZSir&}PH>#R
zn(<ek|9i9j(|sIV2>h>s{V^5)f7_vdZZQy8b@hiY`Y)9gxYQ}Qw!$x4bXE{pmBaI&
zvC@A9_HS3kA5P}~4D1&Z_+PH>|AQK@tbeNS_ix|*-w>t0pF)50UG`tD=>JI^@`s!K
z-v<4U9|pGx6VHDz&gA+7zxd~u|0BuBEb$v=`WIXD-*MDm#N&U#6C`|mzpx1~iTnp(
z@xPE;fVVQXG#CAoClS{l_M88V>AzqKu=?wNT&F!(`xh1YhY32l<#-<&K#{sdy@73)
zTfM85ty1mujsI3c!azv)glOKfVeh&Xpb#r=cr-Zxc4__lBQ%yLFZ0DlwSm3H;(vRf
zTO3D{m|ORCQ+&>d2yLV7j{kA&=MPkW*OKPELyi2-^rxhonwv)e!u{#Ws{5+`rw@)-
z4c~Og4(z23Kb&QWs%xIceLY8Qw`_lGe(-W@()KfOR-(ITPnTdwz$-^<WRUGH)b@ME
z@}#VOIf0RbJc`~}_9Q)IKqHN@J$JY@#-4bUHgFxRU1~g_QoPmSUv8^JIri*ar&I=j
z?N$+^|K<x!(KvtP<ljvdXHXzdDDLp}-1N9_u`!rlaBV=hRQ}r9qqT$6#DI}Lk>(kS
zC%Xl2LEYE+Q68EhrD6T;J%_*YKp%I5ZuWCLec~f*gf$DxTU|1{GWvRY!2B%Vd!mnN
zJpMC+WFC%;Niiixu^g{5(O4TeSb9F_gbVL%YP_)Yv<KB-2)_$aPI-KKF1JdVRfJMK
z<GOEG=EcQO#ta-1$&827(G3c)1avy(Pd%R~Mj!&;)DOi$qp*B5#mi;Ka=LS?@JI4o
zs!aMyypesX@Avji5`zUGkrWtAAcSFxq5jq$0T_bujKXscq0I=n1&F`9*VmsZSH^*=
zfjA*>61HM+>PLvaL10J%5Gt*Z_k1gD)e8|3SqraNmjp0?QW$nnR~Ma@8m6k;^~~_8
z8RT*=HiKxsH`Yb*ALZ#LIIC3lyrFYFvESrye+7{#yjHf%lOVpxo?+U@;6)V9Z3^Mh
z<IlDJvo^|kmY=vWpPd>nH4U4<PKLb$XB}PJM_vRj=%{-kbZg-Wx!AiQ<C8?RuFi-x
zVr{C-d-PLvuCv(km!0JW!XSYlGd~<pLk>4!=rvzsn)D5pM<y%N_cj7PDgCIK2E~c&
z51&28<i6PeKco4HlmXSYv@O#hWL0qM99LENN$MP1$PgR)T2sCn?*Tr3^;e?o$iYfT
zZ~D5nAGg<}j!KKoNIh+`CJF1U7lUolsDxk3UA}_(FmaUuX%b<>k)c=GHXlE9BR%l)
zxu-9m`Lo7Y#>h}HJmYW_-p~g|b_b_#Y%MJwFx$yEI7;~XFr773{ld4=M<BE5Q2WHw
zg${SLj=hR;zS;wcR-2K=W;>+tAEg1Dkn)PWhb!OAt*9QqeHwvei47nv`%rsicvq<|
zsj{j|%^JNY&evS2)NizpA5#*NG<U#JNc0Hrilvhweqe;^3sQ!2SA8*Z@sP}xQY5kB
zM>+)tA(m~FrUxp?`PHo?mTU1PNI3-Qz4_Io;$?=+%U5b4X$*Kx-?5g<v_h%!bD{>h
zw;>ig$hl1oGw&En;9tM3{<w#lJ69|86yOZQEumei7&3>6$n7dkL|t2-#{N?!)@P)0
z#f-2qY3nj{xAi+EP+GEvT_6y8HTOum0#!W_+hr~%FsMVvX7`MW@pVt=Q0i0A&;?9M
zW^1uOmU)Jwki`6~6VQGg<|Y8W2?u=%g6f(Hv@OvAZD*DAb9$m8*E$d-sLU<ow2b^?
z$YYJz>Lw#b3E*Cjvn?UXybt^IUBSEaImIp2&T)f!&bPOjNXEErG)j_2o3H7i;Ndm=
z6e;cNX_fodN%*;xMMYS%#UXdlmiHd3aDc`P#g`R#+Zo<cgc=#VZ4~_*K#>ci*GTmW
z1yE2iu=vU!J{s{JcC<+@G#Lce=-?Ayd#h+fpK1$belYr+JJ94lj@GKGn?Vr_BjTfu
zksM3*&W0Wtr;*z3>{{aJI@#u(krBn4$XCs~)kG?7_8Kt{rkZ&%%!6A6szu(AmY-ml
z8lhglX!6~dc%_wP!)J~+n+8lWN%jNFU2d!iGd_gdxb}9!1<!xePI(<I{yx-Tu;b*g
zzl-?H;S48QzF1e_xM~3dV1ok>P{%6^`H(o6UQ;07f^p?LpMMs<;=YVhy>v{=pokS*
zQL82e{FrD1Mf|Go+CiuB<Nl)l`ugFmNjN1*dWUG|O!cxYQkf#ou2?+<+B?$X0uEG=
zvqoBPe^!PzLV$8kBS~j@xP(vcAw%aQlLPbg-Mu1?98hnQ1P&30a=I|@r_g!Sb1wd-
z745b8ys3Brik3i+?wdJpX?U-k(FY#ND=&Rc%0f0uBeUq?hF&g$`>g<62?Q=%ylq#J
zFlN;EnqP!N9+QYYn+gnqe5DO7;yhnk5xR-H=EIliL(F?ypBLF6kQA|UBx9)aVbyRy
z^s4Y4&}D=mQW-Ek$U&QL-LPD=WF#@5HxUTls@LXW1U$)P-c>IN&d}*pR~{p*B50a6
zc(2kROcqg?gPbZ#L*CLnhY#o4;1b*hL{6+T3%FcHW<{B64lGtCjH10SZH2d*5OI8c
zotzGkLfg+z`Z?t7nWMy<BIs;e5X4l_JbhZ<AH*nRJ%X}YQQ8oTOG@E(nU!teB9E$Y
zPzPPFzQ~PFwoaAC(C+JoS*H7VFDnryyL$9lx-6yBAYHw8fEmZh!x{B*%5qs>(_UGz
zq|=?H;bpsii3w7PZQ-DRd|;rDndl8?nLwr5s&A#_RUNrOX1T!@miPD&yTNPa1CA68
z@6=cNNNr5(?Y$&BFJ?f2Z!eh2**0xlHzsw=mc0AklH}!}bu(Mpz7va!tBFg1mcRRR
z#whpP8AjNh&1iD8Ba=gow-tJqS1&M=&zR3m`<e2nLiU}yQW|Q-*yNKjx4nT7T#X&m
zoP(AcRPhzAAwOJm8FF}rVH3-vF7?CtXXf*?xF80_uA6@1_rx$|2Q=Kp$3*u(%e?KL
zNE}#KGinpk!fbL}X8B)<&BU`_*b)FKV#CDF@*N#my1C8~yg@CKsmSIh6PTFReh+<b
zx%G{{u<4I=dFuBKoBNoQ4^Qa=UDw#C#)|ztNihfCjf*+{^y_+<DvVXZd2bNeyS!ex
zu!j2C?R8s03)bqksXDh0(wEEm7Ta4Tl%g)v&jy2OXxu-UgG4)`AQn01+9=pZd7`Fz
z0_lFd%f?o$`48`x;H2(fLZ<&r-TuSQ{8w!NS4TG&Q*&3aMdPn5`hTpR`47}B+aEh1
z;Q}k)|DbEx|B&PSIgbP9;eQS7e?!-Ty&3<<crAz(1Omgj|9|CcAzc5Rul+^Wf=Sqa
z@wKpsV9pf@@)v+B`47Gp3QXL>!N5Vm0KglA8CU=W7{f)+_ye$=$JSoJA?5Kz#Kj|{
zU~MM{aSOLmN_%)kwhptgD|3j7s|5ZnYYPbt`-`>Z|HayZ=~@_QSa?{dKLP=-p+m!9
zkg~!GD;qmwk_8sPfm{<mZBvM<n3y(9UtnRAiv(S=aW?%Ky*<DD2l4aIS)o7hpMR}r
zuvkno&G@=DWh2x8yF@vmj2CTPpWT^x-;034Kp_$+`V`?T@+t|byw8D<pYE;pXpw*?
z@}D+v%&X~N=9_NkZ|VjXqkEry2MuQr81orDPD7RiQ%dLE5P}36%YI*}9Bg<(iEYtD
zm1%0#*E$O(7FFk<@Kqa_unYWB0Dh4%6<;T(e-h=XbdfjK!s$LCh<dIYPxXE7Z)y*R
zBMwnjAXd9yMM>4Jzhi<zgb;YPp1-?q#Ej|i*wbP1pc|sceyZ1(#yRxKAkah7eKO{J
zwNKlkFJcs5UGe)$RpjEUP{l$od=$b_7EPDE3vdb_sb+lC!Lz`43R!4Zz`BRU5;AoP
zAXy_tvt!<9>Cm}>?)ZHVf7)Fod*p~r+F+4NBmVoPh-4P>grrdhi#I&97Lap;6^NQt
zyEUdNm2^W(P087|$VC7a#D#W8L;rwQ9ovDyDeF5#-J020sR{34{#fIBiyM<swsQ9U
zR*5gvjTw$+2g3%0klL+EM!nAB<Gp5G8w*dTKHuEjHfERyznT67FmAo87}PYu|1YNN
zU!D3_OzMCAiJKq&CCASBPXIBi=FLO2Z8P>?{$mBd=x40(fMl4xQ#~yyT@lU6hRp01
z&))c&lt)wrxgW;Ki|?icVA82Jf>EPV{0Xj$_Kj%_n5l!Mw;=UY<&#=>CP(haGYgg1
zVash$=-SatZE!oT<2NB^W98$W#3WeX_=b*-9=+R(apdp?UI`Dvg1{EbDRhY_1y0``
zm``-qg!Xy;Lc9=lMt&eqj1!{1TYm%t;pno0fJw|mMC_Qp9iMYJ;;FDCu^)qtQqq0E
ztWtz59`dbU)%B_J!W|Di+$$=N6Iu$#NaJ<t5k&J7)TP@E0(_$}8jAL`BLPsv++LDv
zS(GG+UecHdZ=R2F<;Xd7)-aTJg^=5={gAemlRT-Wop^41@FzFiS51!3%Dk9Wsy&7>
zne;*#T8T}-2%8PU-RNj{CqKEUEk1jRayxFiSS=M3UKrg6-Sn&dd%v{`8K`(`+?`w2
zpMbOr!MUBP29*CVI}#iJK)bkzV8<>o!I=AYiea3TrHob=metIaG4sr~1X5KUXC_o?
zoCTm#)p-_q3VGKN3{yA$p4TT+kj~VO7AC=Lt?NzHuALvYxZ4$8%;%GcCn#n2UXuGL
zR5Z~QU@aP;%M~veO#~1j(o&c6zE-|#qv|_~fz7+w7k#IfK_Bxp<1}W0eKGQ<GYhsr
zZd}LEQobJjxiBU5P$wFM$Q6Z?kZ<iPcU>*=wv5VgX&8F04eg8LD8sDxB~-o3I;j~4
zX*g=GS@E4cZRv*Ri-`-O4F2hN9?p%8ET!2do}`0u`_!tbAc7&)ADZ2@Z16&|f)+mk
z<M#|k9q*nDihFRfUIwzG(u9}wZ$=4Fw@a7a8}HkrR;HQA@#1+IrWLb#9*ePuq7PhQ
z#o<aV1R}dZn$w|R1kF{_Tm>M-&`F48f?jYfv=uQ99)M<zn9X=%+HF({X(Lp`IO7;c
zq@ov-4XQY<hy^I~GS0lT?)9<_W*vFbTo9jV+|LM5!v~#D6u6*+B{O%FMqKf=<}{dz
zg>-cZ{kqXf6cX{p23I~Z6h<?E)fBcm3n(>)MyHcM0XODtc50_DL93PCv0_+dX0wfI
zL$W!TEmOE2oIQnyxC67X{%JLKr!MpI+XA_4_oj)FKLHKrMnCL*)Mq!~A35LuzS5?W
zdg1crNPH-q(6R@J{}6YDDTv7!J%)!%x&PMLDhE+Cf@Mt`VI(_>b$NMi9-hpc7TwxG
z)ESqq|15@eE`U&b0DdOCBBL&hN_!$>`1Sq>Gj49BYp6MpO+hr0+so(UUFX7L3I%r|
z=&t$AUCV$>(7BK6Q=QN1b5-@LBk|Ml<JAadE&kZCV^Z&ISb69MN<7nqIJxhTM0+xb
z*md{tlv!hIRy?|mw!qW-pMZ3;_-UV2k?X@(f^*5M9k!NP=^O%(CA4EFC3DK@4dq@j
zRor|J0aeI~_93u$`zE`0Ege^N?e6JcPfrcSvNf%X3^-67#`Acd_$Bz$3`5jxr9EUu
zJyV;%eZRd^&C|!NJpTSNaQH-Y_v&D=MW25wzQ}gS5iT~Er2%$L$34TOjWmGT_U;t~
zB$}f!?2rd5S!_Gc%6fjlM?K*~hxv}{nxzt72~4-o3jC^5Vvk+|iqlsN@spyUGfk`w
zglDlb=FGVX=lZ1k5AweHP`P`AE_$xSW>RRIK{u(RA+6|~YoY!$fjIdsrW)*}_k)N-
zu!H!FecOqTl3r}@+zko8t;7;j_K31{mLU*Hg&dG$4U@_<9>u71uiJ{G<t-u5ePpO#
zIj(a>RTCEOwfm^0D?U&c&WXRHT|4>RlnP50xx1*xuYXya+!JSnHF$r#UMRo*8f8kX
ztX`X5Y0VfhD2O0LoJhqDddCr=7OLY(!C9<{nQ8L;&2jg5*((OtMXQ8_eL0p!!aysP
zV$0;=$&<8i8+W#z`dxjfBHgUit!q>iDl$Qi_bc~xg*8tNxHca8FmL=0Xf@igQ$I~g
zU#u0#8tn9^>=b_28AhcfoPTpWZ=+_L>~?$R`+?{F`@7|pyVT5!)9-!*jcE+R1H7*}
zzx@#Qb%H`>=4R{4C7X?ztM)j-&}I0r1+a^r(<5)!+RK+JdMY5fT|41Da)O0|L=O-K
z!V(}u0jf#j7Tb6Y7JmYG4Z(EXcmH0y1^bz(dCB#3?okdJdCxxQ3A1#`@X;$z>%uY2
zm>(4{U|bl=`G1-D{QE;v6D4ky5U;Y~E<L838H2h)=eTjV+7W20$~Nw$Klxm(tER&e
z1Ix`r2MhtM(T<@Z)}N3|D7&|x3S`SxEEvA#Olsq$oMDx3`<l*%kz&$bd=X8oy_&`~
zm)dmr;xmiv>9BHQi}*fNPTIt~3AWdWZTMbmEt03fqD0Kv{WSAJ;-cDvw*$>CrV4Q=
zjQd5?f$Huj0Ixm{6iYL2NpLr7YrvKkkefeV4)+{AqG|@K*ZwFDsP|a_P8X^>5gFUO
zJ{=L$6}#jl8C)WR{uEt52T2=qd1A~JY8uh69cgMVZu@e8w~j;LKFO{(WPP9~k;Mvc
zfoy_3=+k@gq%l3Ew@{?gtBLon1ov5%42+At>rhqx`ckVJh+}CvzxToC2U6GWJysR&
zSLGSnIUhksDluj|*Ap-MyseXHJZ9nrdw*3QgF9qYR5la}y2wK#?Uyy%*D+@SBzakj
zXZA&JOVp3sUR&&&G->jcS4+^+^dZ$DlVTNN=XplS@;GMOC9%ZdkkQI$B7i|{A=7Z2
zZs*0qTM<hEW~l9?L&q-KkU0T);pnA)MlaSy9@An6Q;mJI4U%mJg%V-7wND#|I9>t7
zR^-@q<=oc@+c{#kpS~BN?-CmMhETvfpLjvqgyKJ#tsjOpej+ES$~EcQ^?7}0+prOz
z&NTlpqrkYi75j8e9QbbWgQbqDLAGY~{PMCrrMoUQu782?XiKO;IuhR3u5@Q;Q*7TX
z{qctpm6SS=0(f$g(8d%w!i>9XkIWAzUcMm7{jzyv$7RUu3b=ZuYCke?j7|iR<ZSZP
z5%6(iasRoB9j-E)vXmV2AV4;G&yFcZQ)#y#^U<i>Sz?Cq=>Rjj%<}%&G|c~;EbBEx
z%ll^g6aDPqr@MI>^}JyB`LE=3#*x#^HF}HKF4xXyX*{%pL(b#dN$k1K@3|I-d!HV#
zj`cj=p`5554{<=K<nY&DLc41VF6b?N0Seee;!bXN_9eiZQcgE(w$$o&@r<JZ(Swts
z5nrP9bZS;#S9sFfWGt_OZiHls3+b6I<6O+N=jEuGO~Q#ZPIw0Q;loseZ2}@lu+MT<
zaU9*za<uagkY?)^P``a$yjU(NRqlRlUw<mL)U(8);Pr_75_|@K;M8mc=_R8A;p7oF
z-8bfRf@K`mOzc112F^H{SMLpdEYq7hew2<fIeypRqxBN;Wjg|PnNKXnIqjQ_T^dF`
zYch+fS13#1OlJ6)Y%yv}!tPm>Dh2Ybi5Djh<N|h5k^_zIN9NP-@l&<Au!CI0Nf@pq
zC#Y~BbnYq)${k8i`5TLr;qV5xOS7zNE6P4|Y>i^do4>^>p<^TahZf+k6U_fW@BTUU
z{9VOM%m$mtD_yRb!I`GmxqZ<KI!o?E6w5D?l(c<PrNMX?f+9uB-Wk3h8ZB($&}XH*
z$7FM9oc&=JV6!3UyY@QQ3Oi|pb%Fh@!fM<lLhzwcZlWdZl6(xH&CKM&q%&O9hqHn4
z;I77d+qniQ-4YLpQ+bCgafK5k@?}T&MpTFNbc}q(I0_q!9)+hwQW1Mj2Dhx&w(*#%
z<5SD9Gqq6ET}zFR!e@NCxH_MvrMVgBI5xZ4uiFM1WRDd~+pvvW2a5=pP0V2>=~srt
zpM0HsRa*~@+DLYba`+0ROt}-Tp<{Rb*{Zd*SOcOCT|N_m!ZTfPWwBsiy3!7crIr~$
zUZElvo1NX!i?4|XbZ$vJ#^^N0nk<IL6PB^=E!j*nT^R-`>F(P@Lh)-)5HeT#eXeM8
zQ&J*i;UQikLWtJSNGXzXl9J148Zi^Jq)CEg8^n?TE1%#21bz!GHQ<b>ZQWMqLuzfA
zmw%tm>)Kx9GT$skwyYnF847+=p)?Cw^1=d+biF=tj?8=AxOdNkv@*pM|9YqS?C|Gn
zSG#7g>lwY*-}Hj{@jm8Q#^zr6C!qRBU{m!cU`j@u`1$H4{yO0|EeN>(v5?rmQiv!E
zx-FqN=1IB1Srb-c5`i{o9TjS6Qw9tLPC3QGUN74wq2F(;UnQjMsCRgvLhEss-$wgV
zzj?e_jPp!1O|wI681#}$w66=GQzw}vOv&F4Y2H%1@cW=A><k`=ws@d6<{yC23g_@W
z)p=(~dm9?3G<Hmo%*H-aR9B8-<74dSK)If}O}o`$Nu`$ZC89`%g$zwhJFs2cc4HBY
z6nB?-X{x`bx;1R8q(@J4w!`>ZU_oTkRlvbbnTL(@JtEiBSN(%v@~vT1KmT0R46{t8
zHw^7UfldzMbhVU|T8V~oVEs6I5tL7e5Sq9#v~c&SkHKQg@lth*dhv3)dMa(o#nvdJ
zGL2tK#@=}`-k{6ji;?$f+yS8;$4D3cF@lo$kW&!sPXN~R1~PwX!ph?75?}J(rImNQ
z!hzRzD(V$JGR0j>F-SSEnIDc?#=C=TG8i%vBlA?^TUJgoW<2MhNxG+`vYtQL={I)K
zrPWzBt<GX+QZ(4%`ZRuMO*+pZJl0Fja)i(h?>Fgx5R`mz8b6d#DIXR8Z`sm6IvtAk
zd$j!Bo8{qffe*(UBQm$XyrS=^>Z@bHvsmA7lPG=ykYi1>KWvkkRhWH7m6AQjl|&D)
z03>9tXP8&>y4F!AzC1k4oN9svZ#USU{}6jVl#CP4#%a}P=Y#4-emp;e<!>eWj*p0W
zlJ|#TO_n}b{`N&(&KY(2d{0xH<0l|eLvc#0j(Hcs#xtdg=lHVoSqx=8ij^vn)YwHA
zJB93Rdk@JZ9Do~;dW1@8Z55~Z%oCQD-;HUqBOhVtLQZJ*!8niLQg4{K$M%kQMG{y=
z<91FaUo_NJ;u7DQr`x_Md(aTL4{0fWN~@DrCG4V7r$jDwE)@3xtWX2X@j8Dg)bcC#
z-ue53)nj0x-V2N&LU2rLd)+XDcA9Zyt0m@k$3_+Z=?}`$4h-l4T_S^Mi5b>vAJR-D
zpOs;;GlYIJ+p{rGi4BpDuU!+%)v^|py(W4{ra66;c}FuP_3x4qdHYM9V|<4r4=*%u
zV*=CjsD|?;(ZmsS2JQ}tya)=*u#Vn^x&MG3C-5K!Xq&Q-hztDIR%%y*y@Fuj8d%u&
zBdD_E!<zP$KWotTG7;wBte$@2<&RUYk6hG-u4}>t<J8T&r5~gdT#;iK4jMaoIB1pj
z22ofRhN(i2BQb(d6#Xr6ya4rh@!pL|0<_+$mF=-+;{M(8<cg>Sc;uK?hJ<-*X53HY
zawIf(gpdNGkTMEvPEOiJS|+S~fmKfh>|8*?w%_}-ol17Qro+qo73r{Q58G;1DJ`uK
z)nbx~rzzsj@T@fP8VSZ#Kc>)*dZ>dD5K#D$1quiY5u(}h4lg)5k!6i5r^N0}a?{0!
zM7&w;AKS;fuwIek1@=5Mf-sEvZCoq`RB~Jv#a~g9SQvCx$rL8f-_j{`nn{*39UY&X
zz+v8LVZqUf{{(1sp~p7vVM;}=o+N^VBa3)csH>~vPG;RuYLg%tsYsE6lG6GJRUvXQ
zX5}<&Le`gq6tKS~%8TC}Im8u4nmBtYeyWZ2sy&CB=$DawRWQhMjwJtnaS2m^sZUSK
z^^SS?gtWKc<jX1lLaF<g3|_cy6%P}r6@KYOlrWc~0k$||)(oRTpQx+w->(KOIL3>9
zMqNmL4UMUU(|QRri*i#97AAoN$17Ma4f9v|BjuqxPl;lxXd4$I>BcdTw61SLhzMmR
zQt6^AbXMIuqs{%UTypYs!CqzGR()6f87PwazM7`BRIZ-z50uJR<8`@T!LII<`4Lj{
zvrvqDH^krNYR?H_TOE#PDQU2v`b0xA;LbowDIRKsdVrKsOXA@}pl$mDEcJNQ`3cz1
znyZW%sb;gZ+0ZZF$fIDfQba%Lt<H$oYCPi~`aX<js_);Q{8n1ZufJY8yD?Q^hNdYv
zvY%)0#W@fmd}DCQ1KtbrBZN<;wtUfM1RVbR&rH-ma-8`g_J2QJ%YJVWZIs6eylf~N
zf8{YxM6y!zWb(3l3Xx0-m|1$7=$4Mr%q+8{fWd4zsNkvPJGaTH_lOcKIZa#ZBaXJ2
zljUhpn_x$d*YOicuZIT-)@y*oudtKL&cAnJEkmrt0QVx57yTSlgp<puKH4^EA-mDc
zM}A>av`F2LK&l1RYGIuJ9tz*@*M6n!c#af!EOa7dOTx}A+f_lqDO=()7>V<BJZo3m
z2*p)~+9D}eeZtPpStzMw(kBF^of?sTDT$QugJ5|zj5xl5HCgyenL)2k?a1B8da5tG
z?sl`Z*``1N;W4z4S#}P&T|&xt>RiEjSn=O-g!UG#sxt~08d=#YctVY>^|2y)G@-U0
z!0J@6UiNJjSk)`6`loBGS}{h0|2^k!IFKs)oUdNHPJd$Mgeb5F4$@IOEuTi#n&9}!
zZ+?7~77c&kc}TlrBW$C<C{Jm<Ih}-t!Zf*rP}M2<tyh$3T!X^E;gVO@I^Jq^B4s~|
zML2r~FVO@#gG;=Cv?>oTsh0D%8O}~RBzgqYxI4=BS*D<m=w$5hn)Da_s|>qHg0J`e
znOEgmLMswkWnY<a>9RnfAv?$Pw2o$^d{1F1#L_u>jZdJy)P-mAxl<R%35CJ}i-+J(
zhq9`OJp>#6dNSF}jx{-|Z{@OnsSSQddx+Q4D!0obU|+({o%`r30R~@QgK*{I6j~Jt
zKu-Q_VQ>8<BYjzgtV0j%uq*3!g&~>aR?maP%y0LohZoC18%`*?+c8FW=*tf`8kbnK
z7^mvFQ@E1QUFE7zqIVLCCsEf4fZxVQn4jr7O=X>xQCuGEpj^vIQF~vd9OWM|gyY#Y
z1^4&o2gH=GB8bn{%+s7{cskt5z1ebWZ(!9LX+jBIf(?hCC(a}K5{X+L<ugzzGEYBI
zHPUMkMTG4O6~C|yvf?^jFQfc`AT?a%+35D5{U+vYx94o8@5r802L5QPcD5N1W5VK;
zhBc))jaB6;%3(ADFVap*X@X<A!rJ&0Nc7PJB=fhAgYqZNOEbT~k07+7EXRiF>f4w@
zPjS9z*aJu-_+rZeS1Pe5jCo3#ibj)?#ydXyy04fe9A37VJl|gt#=a5GDee%Yyu^+A
z(MZ|uPK`TRAFe0ZtQ8Mw{-L=Iy;1g3R`)<Cai@zU6h>+@Im@Ii?Kv)yWs!tdvk#y%
zXo)4z@606R{kO4ILBw$NBkzCs;?Om)eD%6X&F@a<bE@-ojvtw34vw7CUMR$Z9!V9R
zv{Fy#bjlsMDvDIDDj!yU(3vz+th5^qa@qDQ*V6GJ5jp;KeKslm3gFwFZ>UJaq*(TD
zxdB)5Ge|bl7&Fv5l`^K8vw*EjpIi$71c_O?YrD6fkF}UIW=r;>p>haj=_$T&?|9E=
zL1<Y_&VT4q?&gy+`jSV!{7##dsKI>AC-yQ9G}QSLPW3e@;VIlGnai9mw^-Q{4l~R(
z`f(Q$^Hq0r{TbR7OaSCP=r22k8vi|SP$sh5mTvSRg*J@2uEVZ=O36Q?f)S1ZtqU#;
zLr&Ac)S09e2%ccaNmXZW?9iGhr+A6H>`cJ?c<j!;&0yn%oqH}yxxf;a8>f;~G?EGh
zNg<VLDC63?g7ex@N{DFA3^=&=O((~>?**`8jCm|SbtP;pvgInpkPfgGqQfrd?6a@i
z33jdY)_vFGk?<fJ`cnE8b>28V#pc!bWaKdTl%1b|5Vfd}YB92yT-6kFGLf&-wE8Bf
zbR4=A?Hp%64>^0OL|d13vM;hfPlqLI`JtM|c{(_R(T3KWe%HiDnWSUI6Ttz$Yu689
zk0lXcy*gVKU454KNqp|@B)~7+20)?(khXMaa{m4VsC5dDWDo$qHE{B9HaH*?(~FX!
zuLWa$diDKN;wIxk8lLKdvb&{Kqefdxxt)4W3YEf#vQ+y@yr6uzH%V(?|LFG0T6$nh
zOL2(8%6W^Gt8g~Ee?j@e+XncQY<ALIC_;i%<+hlGBm9hund*2zsCY-hnF)xgT!7&w
zfKoZ~+VAdLZCc@d=IDMe&SQ`MdG7#4VSOi2{NG>g8lRp;oqu>{)>*}E^GC&*?Br5a
zBX=><N7f@j2y*W(QlfyGC2ECkYNj)+Qwp6Rs747^Ms&^K^hw7AUE5Tz7jX_k9t-Lp
zxNuEAtVe)d!!2n}Ez-?C979;h#<-L6QIQl1Wlm>;B;9OBU;<oqBXssE@D}MrtR@+0
z#u>J85t4L%Z@@o2LX}@?cd{F``a^DT*>?xNm}Sqk><CALK)6(|BKvkgghFzW{`#I>
zQsK6NAFfCy2ix>M-7(ObqE)joIBT*gUV&3xvHD3(@LX5`r<vzNFJoW5TDtj+C=ks^
zYpsl=%GfccO>3}D`hCtQhhAnZ>6x57D#{TSBc2R47W=m#A@sk$OIC&N=F!_I428;q
zHmVN~9wrvOL1F2}cABxkA~w7<ap$1wJQ@kKfDY0~%-oCbY(eJV7U<+6(ji}ghC*8x
z3?1yo2Nn@_3H1g!r&Kz=Xq(S|SbrV4=k~pZK{@mC7<Z3W79wMwz?+(5s65G2L)|(H
zGkwAZW4Ce+10w~<4g<0Md#jbd-mda;GxYX!6fRwpZ2a+ghd+0`1Bqbo#5}3x2~`B0
zjx-u|^1z->rAyM}^BF}<Iu0}yJmq^^JR^7U30xrZDsZ+l652WNK^JLFU3HaC(~UKf
z1|yw34p<yCi4O-T<-564$=mIaSPG7#8^U4wR0@}Hp27QvAmw>xKmKT#K?%PJJZw5<
zzx^!4P492*GJLa}Mz3K@%pIe!^?k~nn?wGJZ;Up6J0@E;rH73vdqjDV9EFJ5z$HoS
z2d#Gz5=rgNEi?w|(&;M8&L;;G>>8h@!zL*|u%b|D8f-nwc8%fGv6P0463l+i>EOUM
zvJx0dSQIsH8e?2R?{uIrZndLGYld9i=;gB9wT<h@>J1HLplWJ@nTC;L=CPOJT!32;
z-u`CT8P?G<6_i}|u8JnjF6EGBtr(vTsM}F%IkF(|rYMegA0{eA=W7;jkKPR77p@Wy
zRyI)^+iLpy5|M6<>*ZpBMh?w#tN3=+DT{>1k$HoHLV;}|HPLnp>D79w%pDqo;gXoK
z=3Q}vo9QV3-JmQ>Mz&PcX(@Iy5ane$M<v>KS-<G^xrXx2`U=vm8HVOW9Bin7Y)_Wu
zMZ`lpSEdwOGlK_as@<IKIHw-61$9Pm$>{g2{ZZ*GME=Zg-|2{4ncb_IaP5tzv|)Ku
zd>yBts8HOEosH+u5<MnY_YRAA3gx@AepMZ-{tch}3=4*6V2x&FC(biZnM$+j%itIN
zS}xOK&sojp{a^^=^t)^m+A~^3G(@$rp`CoqC(o@bpRSoU(wNByS_Jem_SaJdfu3T~
z@Xbbza7{_eW_8F1nF2CwWba|2)E0!kv>|pHMesr(p6xh>ykR8-@zL|3jyZJe(iIJ5
z^2EVLRBiv(fpvM&5wMZmM0=T3hc*r2O|?y~ks?d*1>|yWqcl%&3F*2mwrO4>_5BlE
z^*zU%sSox*1?e%`=8j&<l*VVC=58HW$w6*XGn}vj4yN&y&Y_w^Ntel3GoA&OxkX@{
z#`qCP%`38*2iBZIomQ*mn~2EwS(AQQ9aeOg#Wy(G0r&xg>I)-$Z%Va22pv=jkJ*O^
z%6K#Tcd(0HxM=cN)!>UE-b&?EYd3~StMSH)G-5|nyCNBvCV}im@2lT@dl%kf;KLri
zAW?aXf_{(a1s0ZS<5$hGORmu?iNt%;(tEC9@phtwdWdE$6-SaPm(GxeZNBE(evjZ7
zyR*-*iCzEyF!$C`Rc>v=FQRmJN_Vq>MT3&k-OZxArA4~COS-$HyHmQmq)Tb>EcgEQ
zdD!)g_q^Xf$A1{ZdtGDBd)9r`ynf2Gj?Q?o7!>oknAkgOV?!j9S?BQVswI_~W2E0>
zO4@`)yt*@=H39kSVtL0?V#l3HJzaGv{$4n5%Izd~lfiLy1fa!{8sVJOO)tXK+A&WC
zCMDt2z^H@+ZO=bD7})6hVr{;y++%5_)YuHl7#QVELTK@Uz46n%eed^Dj-b?$8uz0h
zu851w2;-Hz0DJ8S??FTH8P><B_=T)9OZk+Fg(hxG_;>XRV&Nb0QzQ)7NgJ8i_n_zZ
zUTAh&$POE7^qcib<=_smN!lTczoi1DbzOSu`{_uUJ4CgTxynWyWWmR2EGG1Hg}h9Y
z2!(_1rB_Y>h2Z+wJ2_Jo8;P~%#(m~>$>Un)l1SsA6E8D19Fv?ge`VGr{q}CG_)<tI
z778WGR*@QtiZw^}BODn`o+x1>6sB7#BG25&vu!8$Y|+0+*XFsk8?SC}KL3%hhnJpT
z%44u{eDRq(0P`e+?eWs4{03FJYR7@)ww@;%J;vje?pt)RYz0--mq>xh^1?$h@fc>C
zeEjVL6mv!$<?F$BPRrq&bwBnOh+(b=zK=5WGk<irjc%px`U#=M*J{i5Xxb@wec-8g
zK0W_$(+Ay^&i@la@hkfz%F+81=6G)cn^yjZ^(sM_ANr5dxOv2uErRHroi`s!i9W#Z
z71m!&{P^n0d4cII-uWF4iE9#3Oti?ca4v$qzuAjF!&)`8#>(%3=Ax7=5c+OSKz8HA
z5N`!5U)|JuYX3rPGx=i`2rf}JtG+qK>uC~!8A0bpOR-`HV3RX;y|Cb#c;oV9qFu`6
zQikkVzTDR2*j7G~abj`f`JclAtMw?ubvsN$LqmP|LRZ%WLoQos?tIyuM=E)Fsis#}
zFcg42X<0BWN`7*iMGLNs(@1!%{xY|xS>-^B>Ek95ttG|BT#>}Fe%>OixMCXLxUj|K
z^P9mUN5W;}stViSe#g+0W{1UOZZ{Nc`ecxDx!kP-Qg9yXelM*qTA3n77o7CDa1IBp
zv}|EdBE>K~!$xu4)3g2Xe9H+_%4eJ}B$%oW8nlqs&^!1yQI+gRI1B=?k1dqnF~d80
zhN6&AE^%qtkYn(uM=j(HrYayxVFrJCWl$kN${Iu$-eKgQ$F(ADbW#^Vr0F(8*yB^!
zR31kd$xNOm<H&x^8Dr#z&?0&HQUSx1iX-HBFh$&EFatt@$-FGS8XWWRaPlE-tZc$B
z9|Z1uYr&yoJ)J!<8b5*~6PIo51P?h(A>HM*&h@liG?jJ%LX43?bW}I>ce<-tjcY3>
zADY48Il5IfkYBy%c^TEk5NA5o<N3vQAfVQdDeabb7uxU2u~*is5Etx6^4siI>R|!q
zG#VaT6U7-Y)LUhB?lo~cfK+%jwhq&ydFT}FEk%eS$Rg_FU}1Ecpg%IjP>n;XCB~v}
zrZol|BHin<e<2MZv>Me^MQ6CJev1*&>#BVg6iC7qy1UAlr;0RHbe@D>y#6mc%M4Zt
z(JZeL&Fb-!wP;nvWq4X@LL2E4RbO<ELKmP1jl9u5O^QHeRT|OLF21vdy~Ky(v#wIh
z`79<SGE}j6PQA*rHHP`@3o4H#{1=O5V4U!(Q9yb%5>V@D4+u4a1+7hixu8<n=$H<T
zJspAb>6$U(0lM)$u$;~O+&fWKyw`{noqExJ9$NZe|9^GRxh}UXq1CzZ!rRz%?xth{
zhnE>izNDBG7~%@#9X+&j!}p@YxTt+EN%tnX|F(0z>uiOTZ7a7-$pd-X&`EQJjqh9^
z_Am4C;uCjTpv{<7>>btH@nx0JJZ^;sa?F0*%@RDH%#8&BQ5;Ze{aD_K+l$bt+Vt09
zNtwel5$?)1G_h2&Yw8d}(aBjb9;uVvu@Zic${%PLhj5Vs8F7~%Y{;3Y<}*O;$UgOr
zWu55Ks0Rc#wl5E3(Q>MpteK6=s*w10X$ghwn4PmY81J9W<DWDk1z2VTS__#hopc9T
zxo%m+Fu6h6y$M%SZDF6<@$tHGtu_29!0zXU&vrF!@%<?))u?3+hAOnGg9{~l1!xus
zDldh(aPvVuDZJ3daR<E|DcjQfsU8p`TxvlpdPMs-9St$rB7><$aDu%usykb2Yvg4P
zKynROC&f1NVKG!h|Dk#VTT~=;TmE`L+xV>EgsYCr-j-h#ns8v=5xV&e3Q0wW$sB{9
zsC`(pwf>Kr&%BQJM9&VH#cpuyUtKQsN@IYT$h=Y+5Q>-5C|GY?th?Lc$6py$YYl8|
zGo*3iTCvia#zu@8S=fxZo0+6+r;?e+{gy=79pFUV?dqt|jAu**HzscCe{2ee7?X0v
zq&9q_p{K3VGJMShQ*%2_vS)2<H$ngtisIWWw1(c9V%!~z3O(2zAb%9zy`)gUC(_PH
z@t(yHJ&wKbUlxJ|{Cb2Fqz>93?yxjCD`wk9VTHbzlj)2QEBn)HI-zghq8P;irJ92+
zH9fDz<dLm)5>$&Bhq(Fu;%cJb#bUGdXBDlfbVta(>APUyWm@ExvLb$M;@MJ%d%18(
zL1UE3A78l0qZC6j&6aMnW2d5t;3io!S&uIlpxYXoSo{!5ipL51m#)1q%rV<3zc8DE
zOWRh?)DQWHFQJ_fDqtiLE3~8O(Anew%q~%8HzKl=VXw<o&J*$J^xaRhyYU_Ulkq?|
zfA>tw;%=}s`aaKkL9o$-->Y#2zbk={6g^^FntQuVTP;g8Ny@BK6Y7YKAA_DN^Wp`E
zq+zd_pT4>U0@B&2L4D^6k7E2Q`&hd@veB{f<3&HL?eMbkFU_acG8$CKi#*m=N)Ej$
zGt{K!&_1;`FM?lhFdDdsdN}TSEM-rSxeI??S8;a>e(=&Pk;hUS%#6P0oYl=V6j+!0
zUp~{pbIsdnBu3Qbnz~yN)~aG(hmq808IXu<8e-LYu`u}~YmNeGoEXuT6)z+W=VmOZ
zn6!(Q6c-bAIT>S^TLR`OIAN+b>H}Qc*m?z(`sTU@E9n_aeQG6<P0DVuaH?x2OL$X$
zoOBomT_L&XGWT0^u{r(NTw0+t?4s%$PNWCb`-v~R4_ICWGY2hFbaR0x996YT9Jm~P
zaGMSDeBhfp7xom}W8^`0s?AD;d=drK7re+8{x1U>4mJjLyY(3jX6{7bjp8y!*mMns
z*0)B6VfifSyQRpmrkUTdE<ne51asDR8i3g2;MrX`pN*G#+&#A^5BlL8A}0vlDo`PT
zfA6|JCOoYYWGRZ<cRMFV^qxO^6(RP;s$@P&_|Y<9+VO_M*G_;8Pq(DLh<wkYZ>yf=
z^;n_l2~nl>BJ+Fm(zI}6n#Li6&Ax&}ZCy}rB4s2+qFpolRn0Jn7n?v-<q}*ADdM|Y
zVRF>x>1Q2LElJo533eo~Z0ImZqJ~Kq^gZ9Bb$u^vGN7hy9j-xAA!}ZyFH_?TlOruZ
zO4kD+z8ZOhVg~GBa;ZNAfeQ(~!SIru4NswVOxy7?pMU(w=5gu`wrMvTcPeDhHsj)B
z1VeR;@q*G;QPX|jF28<&idAPEB&{zOoboAY7x@-6*<SMP3hnyd4P%N}?7nn*RDAhx
zR&CV7!z6;Oc`(oBBpmIUYI}{Cm?$ZJZhSO`)sR~#Us48DiY7J;qMr>B%NN4;rtQR1
z_j`PLYPhNvwAm?9!Il-MJOxB*(Sf|mFv>mr8V?|h*kALA_WI2-Q?tRV*^rR)@*fw&
ztt`q&fH$xm=&lIV?ewi@FYn5&KA|FhuN^O}r5i}S3DiuesD|CpoG+iW65g0-N~{m2
z9V25ENL%VKdOa1e4-Th{)ZVOr`npl#oXnM8qCnJb8^?#xb2t!z`PpEpB8NN`c$*cv
z!Cgog{1!gYtSQ|e3O(i({0by};u3mj<2-x|xB!-Ti$e8J2y|Ggqqq3Dx$1#05Z~=|
zmBTJV8#z~!aA?PuD=Mnsnw>ZW6fz(%g}*uTri+KK$m&c52;a!%j$Ku+239H|FPI&D
z*3CJtWZ6!q#H^NH9yQ=@w?rHBzO0dy^W{iw{*GVvAt+Aa>qG5ctJ>8m*E+N;d@@MQ
zSt9$vip7O>xP>G2IMXdAYy=fM-N&0r)?5w4N&_ni)l4ZlIoHPU0>e^!wlEbT*<K<J
zmgize?wuF76ptJ$v2Ce}<Q?)u8<^wNvfgH+I5s!J+{F~isxukdYuFMi?xoV5I&1ee
zwDg%sVGKX%H`|gnzFx2PauvF)=szul1Il}`bjE>#CWSQDdPP7c(#Ol(W*6-^RNuPz
zX}sR4PRMy#`**NV!Ib4YuYz6Zc5k4{Dc?bPKxryke#0q><JUj7u4%L-*SG2*CX<V}
zjMov7RsLd2LP#c8U!fV_f7v#qocsknpT|e5LE>PD1*gh~V0p;}rGD<3^FyV5yjMZ0
z_wM())C`%jG67kI`^gFSo}D$fdQIn|;-0)D_D8(CLpz^Uuf&D6*uMMHom^p;Y{Z04
zYRR2NYGpE3ZX(_Y3N$@Tcjd?NmA9(2q(VO-W_1%ZN|4!oAxR0{3~;=jpQA>yNl#IU
z<TKK^<?Y<b##Sw<sqW482rDb@k;i~!P6tE((ehU{>E<D-0Q}%LwYBsC<D{K^7FOgh
z4P$WAM8IKmyFD5QDVsu@I^!Ue6M%nVMwC%FbcPjL11wtLy!^!rdj;=B%TO4j*~tXo
zTVHyV1{CbH*wk+w@8+*f>ssnlPrJ>J0yE=hwqbd8`S@Dmd3Y`!Si`sC5@Hn0qrcA1
zbO|dj+ZMQiY2<=SRJPBKc|LG~m|QQFgFW&Sa6vUaD&E0u0)tV}KrL;&swesZ-29AH
z=k!(0>blo8@Jo9Ieg361@<L?rF9H`JV0pWiwvNG>KEreM@4$KQMUb||OVx1{r;eUc
z<vd`>8I`TqM8EO0CnsUT(CQ<=A(r|=YL=Yk$|DCO`W-K*L%&7}s_7N5zVQBk{uxpk
zvD_eJtgD5V0b&RoAs6;-$j_u7Yq(z_%m0L+)io2}NYR+mLAA+uulC)bNz*2+FJnW*
z6G=4Bnsx`5B^68OK|f(aq&A#A4pazj+r+N3_XJhGu@t55$77Nrbs^2perW-r1IRmz
zLReW&JKZopf`F8y>XK9!GV43=AZ@VboiJxlGr937lrfqyrs*4&V}_@P+Z@{Bli?t|
z;M;!16Efax%rmFSVjIvP_3?sdI|4Oh<a^6YAr>dimuyZ!mg%-wRTvSt`0$3hE9)1)
z%gMx)u@9bmLUAit=%f*EB%2IKa+FZT-725WlC$atX}7JT6HZ8?`1{4ycOLAFM5TjT
z*(nxGdzl*H6yp|oU#5m769%(ghU5+9d2lgH;bjYU^I5#c{A^|)is$R@K^=y*bxYQq
zCiz-lrVC229)S6;YPq**TU&>kj4!x>yA#yFwQ}X5)l6!tHE*c@@};>1wf<?Z|LM?I
z{PPE4*DeD5VKJq)O52VAe^@@S8J)EwG5H%b?kLJmZYibyGJq2BOcU0kvo6IKUbh&h
z>obp%wN-*YEPu^QWrIOpx*zG~<<SRy@a=0UCls`+8Ul4Vd~CKGIbV2EGnVnVIzciw
z71?oN;QY*{){3Y96T<y_;cU9LjNv78IyL4}tNUE>Jgr%SxOnX`<B8AC2~8xhP;oT|
zl#jgL=E2>kZA9r`>UdI2igUWQiFEQICpP$Ti<uy`cNM)|H2)ZO7p<_4{0xncvyqxn
zlS3rzvD2IyTi^D{<#owVi1$A@#q-y2=Hk5)KA9_q$~~#@tm)0AR^I^Vek@9Mgns*w
z>5VJ!gH|%@_sVoCATyVJ0*hq^yPz3GLchcsp)2xRIlYT!62aY-;X??Q-h}LqIs)uu
zpA{+wM^&=uZ0ZM5m?TTz?ypnk96To?PR;MQHJC!YPVOjbmR%*NmL+iG<JB;m(Y<ZP
z)pND&%fguUic3Y{U*J%XJ9xO@_H)4C(_^Q=Q{?-Lm#XKD_{V>Z_34?x*9T{<pq3Hr
zTQXsf5{eSx*9ffleyV!<1Z=_}>Eq$K=A}qB-lS%y{`{_1B@C;_p^OAGpPi-}j+?w%
z$N@i2>b&n=qqt&?nB<VcYVA5l%w43lKA)_sAcQzJ+}TTu7u3+#DBV{2LO3aJ!f=d@
zvL#$lMeWgj@``&#Pbf<lE$Wuxovq_kEd^H{_VIs0xCE_JuD_3>t4gX%*9O$@dFF0z
zAN>Fap`bP-69AY)lVq|&BIdA=$|2~nm<O@(>Q;~=;1uyLSRn#+Agnbwjjq02*Ilu7
z+PbyI;6ItqLLM(GZo^D3S`z6;F94Ki&Y?r^t>qPSQz<54M0}ef&cc~`Vd0mxCsVvH
zcy>&%?S=LSwf#xftn#HqZ@{T0g)*E7WYdZ=1*u=BwWnF8EK#!KE`IopHlyB-h2F$U
zf2KO}$W1{T749g^W~8m&q+y}p6tC5EfI!z$z3x?XF!uLB3hO1SD5JrIRpV40<O>i%
z&u8tUk<QA>%y<i}zJ&sDVgRB4x_$GNYFnk{@@ZS89`G%(K_V-k=q^J9;N46>)wSB~
z)s;OnVpQ-<BAIfYYBV@EE`+^0{3T6iFd1y6<<fNr-LR7Vn*C;8)_yB=7jR(}0WL>{
zV$S6%GQm_wne3)<OfNjp?$mL`wQB7KNafmFj{DXCh^x<?RsRzLIqJi{Vg%3G;x2y0
zhn6W0^`8)?362LuaE5GQYo(Q3kMrO4IQCa7*oy{`O+3}A)b}_N&s%r>$hMm-1abSQ
zBTFx6CXPclxctJf$}F5IaM9x3RUjBHkr<3q)uGX#r<~m$EE>X$MKA2rwPe_*WgN^4
z$Jp-+BvUQvv+`*bQhueinIId~U)u(IBX#zDAPB3=0g7$x8Fu}sfx#PD?@eYErb}B+
zGBOLL(jad#Y`en`NFJZKvDFl$k002WG-sGoKTHUvN72`yjpMH%b_&U8YvT@`E8)`H
zfefMOf%<S+#jK1FsIB?t$bZ*EB|0^M!<g~w;1SUK&B|9~n@QKBRHw0pLmsorhq+oK
zm1Z9K;SQ$mxJ12L%W$=}r|Em_FmPsJIqn19)6^`*`lE#^Uxj^I`ArTz|AAOX*f+gQ
z0h36@_!3Bk|17-!leCxcwW+D02H42g+DK|{YA$4%R$7``^99&IC9596FXT7i0Z|hM
z0nM7syFdzS>#;UhfA+>>rlj;)%%Sj^Z}zvUlmGUtP70mE?p-nDx1nEIkLK99s_&F3
z!C8-uh%{4umcOzd?Sjeb!C8;jZKWJs=4C2j+inA*pJs|Uj>Kz<t7uS@2q_UV?46>s
zCG34iQasjrAWiB`2Ttif+~m+PdT3s^segLrtu{sWUO|%V)fy>nk%3u~aq<q^4XoER
zt~nsrXkh<l>1cR`IcUFAZcf42;gXSko`l^qNATc}2E-k(^J!U|f8acD_mUlBY7mbk
zwp4Toy>jrYX{@*#7kH)WQLeE$bT2r8GD#34S<?Q@v+>M*O6L;~F2PysbutC9w1vHM
znD1xl*mhPmNdQi$y<7j0tq~M`oB0-aXiV1a-qf%_uUX}R5WE1GJD8{mHx%TEULY*1
z&P5hakN201d|l~K$z(^pS$vnwPRW&wii^P5Vbx*hP%R;p42m3b@W&VjOj!?Yo_EuZ
zt9!D#x=uOD@6NUdx3*w3MU`{IiIp7^^5^oO92RV;w$)bHaS##C?R`(|2#VR6&tV`X
zr`kLrOi17~pB!nalAx&xa2CCKtvjtsh<ZgniidA_^yzwll7x{+d10{zGM2ww`o&G(
zKrT{NV_cQE5}wB@FAwsYwr()Irs&C37cigxx#y+o#i6HVpv!w@&}gn5_mjwJi&4(m
zH1}0^%Yx;4o~<NdVD;FT`pGi4MJ*zWUNeC<2?PY*^fOO~j(gj9sMF=Li_x}pldMB`
z&5Gkq)sjD;rX<Xg%Q1czx4uk&dAqjK;Z*O_r%Of^x{_yp3_1`P*@|XjG7q*n6IlCI
zP0e(EvpKn}GJp-VO$NZB8KU5;HyWQIx&@BkSGADU-#N5ZDvRVr{cVF}kH%H={0P)e
zTdak4>PgPuTt!-0O0AKVHS%rZE~xgUJH&{tmk?$tMTCTLMbPHYT3fkWx-Qy&_9}1Q
zq(sR9pg9MpWb(eeFFNHIa}fQ6KiWtoYhHf8-Jph44tjN;>=Sxi!|Tr%H>@?>G*3*z
zL}^ae7MAAhVQ+su5-5X4z#fKXg$!tVccogtGc{y6+}I@n4y0|<K%y}*(%BB0eIf{S
zzzRUbdC@>@mGk55*+x)~_!MWiyy($y+`W3@)|>KxuRhBKGP-aX5Y}+$XXTq%WAUAV
zGYhe^Q2MZ{KDhPrODO}WYN+Y~+V6`><H38n+}w47tx!pv{FFW>Ngz{$Y7*uBAcGyZ
z7~N+K|E$}R+scEDC%HXKBzi166zpReyifUaZ6g!-y-DOcEPYWfj%d3koQxI|=5U`+
z@$<eg@mz2&2d*yzc+(7#Sjd{8LVZ}`kszLVJ5^jdY-Ud8#BetIrC8E+4Y<B-g}2N*
zYNu=3$12J&ZgOxW{Dcq%SC7?u_L|Q^vRIu~YBrRXSjM<}NRuW|;<4Xbxs@9!Zr{nG
zJCCV1v&$Pb@f(NN-$Br&p%+Fi2<_WkYiELwHV@T=ymv8eWXiA@)FJ`sb`&BJq@=P5
zq@sbpE(ju(fpbxie8IN}eoKol6YS*mE7ovy14fze6Np!YvxdvhB}qHyQzQZVHDv6q
zY+3n&NQYz49hTpRtga*5SdnVzHM*h3_7em{Yu-#gpCY|8!5s5%%k1v+MBG2hO8~K#
z_P!NO)KhjzAz0p5F0S9eXSjw%v5MNWW%WEVCy$<3lJKuF0PK<kLXg!!a4nmq-#GDb
z@-{tN9aAl+v;vq8lugsZT&YSkIz$n1%VJpeF(7u~UC@cqd~DL4FD81X?y6Y2edrR{
z^j57w`H^>~npiS-`4hs|tNXFsi1<Nb&8PHUA&*CUzo0pGXSc|`ePe60EShp;zM_*=
z)KZPeN~NermTNMrF_1M_2sMy?ZL9IOTG}#k5z3oG;mODeI~g`pRvtLAZsy7o)VS_&
z82TjBwjP-3b9C=CNKwx$7f2hMzyWK`?_=zij~EAMyOzWsmSnJ`IIl3%60fIagd?dm
zkI$;r2MBlWD){HT8V2MPt!PsgvD09!q6Iix^B9cl2i0NDbCl>90uH5wX`MfRM59VI
zgZOOKtdor{mbia=T4;gsfYA+D(IiT-6JedqpXAQ28s-PYMhDkQ;o4gI>tpr&%E>>|
zlY8dnQa=g@9+WDu!XGE=vP4<vBF)QEI9Q?e%3<{G(bile1eK)r%Qxo6Z{Okgh_RRT
z4O7ZHA_++Xm#GEbJSxWf@MRi?6!L<4v%)Dq<9Qu{uniU~s?P^MUSQbtzO(Bk-E0%_
z^&p!-e|3)c_DC~Q$)%uyM%taLVyr*ko*f+H!qiGJ(Gd@OX}{C_zULBmuw8~_HiIiv
zY^n1l;SRbsDV(8^4O}2ZY_EImBt*_JIU)mgW1o#73-I&0hi1DrrwW(7k<0e##r0=%
zhVS`wmC*U)1VLG`i97q6bHq+;aTF{sUYFa2)p}g*dgvv9*3Jb?;v1*EowfM)J|iUt
zy;*fyrekTByO@zAX@j~Xa$_x#?4&3wAB?3jh21z9d#AY-U&inmV_3f4)QC#0rB}>3
z^49a1p2>2O&maVplEawGUEo}0N2vGODpHg%XEcNy<o~X){2g=xSBPayDbv>kMpo_|
zNsl{<5}MLtJbi1yXFi_HXKudUJK*dEL0{mzi_n87F>}XcsrsR2tQeQ_G;N(LL`q%R
z342dd(q`8{ptpL4vQA(1rrq)^cRs14GZKAhZAlN4M#x=-=y=*D#t^rq>LxE~OwCmw
z=W;>vNj@4d906_JybfyuK0A}q%D;9{ON&GfK2=u@)4f`!41r-7qvZwr<-^0rO7CYq
zx)nY6j8Ziyia@X8v^7<CtRDledV}P@BqXdKg|0Zi3KjFdOU5w_kCeB1bwwZ5ipiy|
zlp@jQbk2n3AdtB0YBemi;*(|jyI~C}fIc5%0IRoW@?iDMY9VpCu^P@$B}oJjduAiQ
z6CE<us_IHb<Mo%2Hqy_)5(Z+P9SW^8f3Dd5g!7{9Q-LpDZhanLR|I?#>tJ<FGEimc
zfdEDGN!X7!E$)qirtD%kMNkC2@62E<X9d(uFeU6p&4rH(t#Bcqpbn89C?d78P!@pu
zIiNKq02w8K-YwE~%io=uC?2-L;sdI>h=I;m;WY?ORJWy-qm#S;cw3Juh#HIz_Lx&H
z&=$7#ruoz|ZVm-Yxm^I&ez@c#Zg0`Rj#QIZEWv3EZiAOvkk{ag=QaXw&%`vE(EHCY
z>vG{JW_pszQ{*fhfu{56jtP#L-J&|qBM|^?hG3<BX(m-rY&Ew<X}dTi?d%pTwkWb{
zhE&`zswYMx)1a>P7JO}}B4*yp_s_ja5{-^sB)>AHhPzpNfxl-;g?S^P`HRQ=o+)K-
zQBQ;gsW|d0Qwk<spp$W#WgEuZ5bMm#udHD5toLPv++GsKPY7AAOrP1*zm!3R<}fN_
z4ssO4r|Mtn3ArdxvoYHA<`^9WD5F(4g=AfUIZ`Y;`QR{RJS+4ZY#@T>O?{K@W6Aet
zV|MtTjFhtL@4oQf3D!A;pW=47qzGG%!p8#bQw*&}b^y}SGn+(qBF&N!_+<S>(**GH
zhRf>F*X5gTS4idd>ab5(;G*R<lKa=K&L5p*J@+wHj1&{Usxl5-CQRm5kOuZPq9@S$
z$?^tQFIPGHdFx{}dCeCiJUiAj2tqC%lXP7K-&K3db>4@drWuV|a&@J~HQmG44NAO@
z|AM8+K`~nGFr&uM4^j|^caSu<nXukzDzJeaCOed1S5WjOPfJRxJ~ogA4obV`Xa(e{
z!$>zYO)VVcXs*e63@#&J_2c2e!C}RC_%tp{fx3OC63HM9++MeiT;aV!jON7DB+uQh
zFXQK!W!3DiDmSp~eWO}`;_cG$6GHCErDI*;zT9>*KF~c1tGu(BDvE}SvStQ%75t6|
z`=ud`OxBT<=@M3D`i+pJ*WEfNi%4e^38idtCSI!pY*smYH_aH@Nf`6G3=2_Wiaooo
zruZD*jiih0t$gK+R&5?eUU*$DwhbEWiQiez{Hb9<y*i32Ir3(H+JJa6+Mx5qjT4gU
zxk_7L3RWXVo6oS@YrH6MrFK)O!IT;vzcqr?l{pf5m_jp;TGuqZxDaJ8h4K^YVkOv(
z2>uzs{mUwxe>uel%c|=i%zA9dd9iyMVADYE)QzKDiAcpeAXtasIe7!kzKGYDtV1gk
zwuGVMQR31#*~a|E7DT)95@$ZUmLc4_WsWXDXWf~jl}!G`D_x;^q^K2!;>sd{xh+JQ
zi0|=Mlj@mj&K6|w5*CO<!#Ydzo$`h1-@ZTf+nuh62{~oU2UmTpoSS4M`MhckoR!l0
zB-o;B2z>hB^%LUJ>%;S~N;lVOuRT@F5mSP3fR<LQ$#~j}JSZDgZJXIqrH`unR`MRL
zE{Gm4t_Nyjhd8!i!!3!#PCZygJ@{t_Df9Uu6$;*c%G_uCw!D%?mq9msmngwZw7Ck`
zzifar<XS=MyTz48$B<b!w2lyO<O}dmcv-8>v?D&qK${y_sGU}G=FCjW5({5N0K5L{
zl*(A&KG6i9Qnr&yN*FXowq57>kr&lvWW$Ot-1W(tLVDfEs`D2%0=J=#Nv=Mxi^*_r
zK2Y9vERLy7g9%f$XQusn?QnjZvK_A+q!un@4j=M_PL<=<aMn;wjXGU=mXMm3ns48N
z^T0D~aZod!2E$2h4aRF!JsFd-q)~c@2qrQI>WSfZt@OXFwXp^kDmjipD@0LpeZ<8o
zHD)BC>4Kf=wrwgrxO~dnvYIYqm99;~YtW*<P_Y+{Ryo>Z3)nf{OwOY^D&WjC%FTr@
zFoh->rbG!Wb>f~AHl_`gSf{3mp^<;v`*FeJ*&cl!88v)L-?a{27^~hq6CBa_Oc8yD
z6J+B}oz&Yu5v5p=5f|<#O@zIaBV%GjM^N!KA1Sq+%*jn1^|pWnsrevSu?m5gQXcoN
zCld~)-=n%;6)lpqTKWT`(UP{-xmh?FUnDKA*#*y4m`c%k5qhEq)ce9BzRl|wHj6>H
zyjQ4|9>=f$nghoU24wtkOSe>O<O%Ku;j%vZ!wI!}UHyv_BRZK_=l4d{GqknaO0?H6
zle=@IpICX{_oc6b#%biKE?|;>HIgaM+Y<=cv%E*@4{SSz!Je!<mn-Z@C<CuXb9Ob(
zarfEE7S7j}A;&<R{*)ZP-V@Vn=#>~`VFo6i_D(ca?G?5W)G3<Ne!!`NJ;eJLeo%dO
z!K&>VmVsjzm_7FwAFCOX{FsI-Vz!Q1u?G~P%CtZgB^uaFMj)YuvKbQsE}F?`HpOJJ
zUIWp{jYb<1$j2V@PZQ9Ok6@Z97#4be?UQts_;KqvqvGPVQ2$pFTo^UU^g2FcX~8Ia
z0tLhrY@UV7)HfA?aH#P3F&Ag;@GvG_%tJYLzqwb~3|GEoX?her3z5d2AKkne1^_L)
z!9v=`@n;nNKmk3<{x7R5??9Kxk1>y;_53?1^R)2o=f0=c9W^?w605BjS6v-~;L`Gk
zrCLvIg_%fqzW}D-TISzRUU(JWKYRpq!yTzAb#nz#a_p)CUHj#&KW(d;$09~A6KNI?
zeQbM7Fb{v}Ytm<#L?<THf~yHiBr<plF^XF`z{*r$(Eu4ayv0SW@%;h5ci(80eTy~3
zZpM*N+(dLG_g#}sQQBmzWYtRTICD)F`DZZ2@ecmFjp5&WWW$=6V1bNl;1p(<VbJ*n
zc6+5M(DQf*w@<n}qI4~%bh=qodz6Oufm434noLC|@B{EF4wHR#5F9=`+O?LPJ>CmB
zf!3dI<&Fu7m6RXXi%i(=|Ez{%+R%T*#>q!iCUUc-ozT9HLaQ^djvaeLLSC)b^uB<|
zm2-cmzFr56nw_tE-nlHsvUQ2N2a4@?$oLsY$lzNH;mXKoxZ9ttw8uRKi6D32Xhw|Q
zZXPR=jgpw4nw$GVq$>D99FqjU?cjEtXLR=fMfVfg3E;htkg`wH#Yz}Rzwp4n=H}xF
zmwjm*!bQQ#F|0VcMs2D<X~{(<nR#k)s_~|&-=!$(`84r?p}AlXZu2i_ZlxK*2=AP=
z^U!fE?x}E>Ypl^&bP8^6T)ZNp=gbzKA#6<oz2N~)xQNR*V~S*-1%WT`dS`mxG{%iE
zZh-4*+}m%NfKT0G^-7Qf=s&8Wj0BOp!c$Wu>wKv+yFrx=a1?=ac0x*_mtk7AClB?7
zK4P#`@bhK5|2jS#ihsG3a(M>^8GsSL&MM%$WV-WFLDJ-)@^sg5D`2;!XQf(C9rq;`
z#{k!<ZH$2%ncVr{9Hsx;*<$4q!0C>E`-PLeey934T*hL++Qw&|m#Tlg(vSE23L(^>
z0(Xx3(oO`upQEo5YUP_IF034FiL54*SbL%RZWcuFt!r@g0IY!Du16CC6`0lyPAOS4
zK6Ud*L<85cc)xBgVExeL8pEB(YwO)1&0G?CYmyV+-T?O&n!MT<ZRnI9;ynXjfIUjf
zzQFSlfXeH|XDi~_dHM;#G5^}QQ=OMY06=5#U>apl&*!#^t~3BT4UQ?K$xaO2_s<Os
z;2I_|q`_&lw)0oLO<gXKPp1v1_;MU*1>zY4sCF|iJ=Sx_fL!LpQe56DK1IG+c?t>K
zQj~mAlRinrSftAJotE*nEoUEYUP}>{%k$Ow=gpiwTy;&ZNnOp@*SM>mw}@BCGW4nh
zbs<X;&JkCmbBI>Q9>e>`YAfp=M7{%OT|R&E^07wNd9Mqp+7FbeVp$M$w*-f{9<iex
z(bO<h{Of8qvV+hdWgt{^OJ<e%DVthBwXu3Yze_xZVc0SFW|*5w2Oq%|n2S(RQr~g3
z<o0~pzo6&A1g=#NI96yb!d)}=X*dGpu1P#zAPhPpD$f*HhRVKj2ljugZ~L-#yX^^A
z22&|nm|`gFSbalN6STxWMA8O5C1e`3i_Rga7q=6D2aBh#ly_U49-;t{r(?Ct9MVQ-
zFqa%tv>!&{%6=t@#MPCy25Z=MMEqa24Jg(rNVXPH3%~0!utHyAlnsdmnldCOmQZ=P
znLA?9H-V1W7eLnReL$ceM=F#q)BB26Op#&(O$|<TLEuSg+FEMtlttXLr`(m7iW>xd
zxp;Be)WB*QLx3hEkw>KDz;ydjy-uG401}E^Ur_JuL+-owAz1qKr|mHPr!PWB)B+B)
zr1uxE!tUEl@#|U=w%ru9f$C}AjwN$tH(pIKIJO}7bY0Tfsz9KSLSx2~Vse8MDNQF7
zHxLGW2tT28@niOE5RK&=etKQ|4vX<XUMz#dO#Ap$&N8eKV$8c!ajOB|h633o%AU3f
zbikWr;U?heVe+45>d!s;EdPUfCf)r2)!paF1;?*x`yjVp<YP%NsDJQFwyh#2&9a_0
zzHfRK6m2A2S~O-sulfs~!QjSjQf!2!F;$iS@~#(VH(M|I3$LWWwXX^Z<uq$G<a!Fw
zs^`Y}#DD|_9PMhQn@fau@TuGB?>PsnT4ShhjV#f-Ne*X9RPf$A2<f>tb5-dxn7$f6
z?mn~A%CPABLt*%fUG+?r_+LEvXC%dMwLAZ)ul|3>1z-c$+2`P5BLRZ{vT?AJuz~ps
z9AIDpke!tYzybWPU;qv#_P>Dv{$9-Ve+|pvU}Itja*_bSNQGZz`~kpUlnQQePk~JA
z0JeYZ+5d}D_!}7DxALa{+aY5i0T)XJH>hX~aw5@UXJ-SG3cy1J01p)a%+25ccjy;J
z157dia{hD2K;}-yO#cNz!Os4y(&{gMz;9Jy6-j>M7J#`NEZ|bCTr4b1U{rvOvz3(&
zynq0Sv5gV<h2S~aIsTh0$8WN1V0-`z7;x}cVOFl^6ZW@;e-{RD0hl<s{wDi-4cb4(
z^nZaK01qL{ufBs{Q|vj2fLV;21^nn&@!4OGxLLu3lHaS-{vA=k4z6JP`wxFTlXtKM
zDH((3h=f^QM2v)4+1S<T7s%ir5CzXm+x~$l_%*A){ex!~1EWoh9l)(y88{h>7=vt$
zjKO0L@95-UY+w!Vy40t+`aZ1@{lkRbrnqN=>Ld>OL!!y*48M8o-1yk_@hXh3L^mV!
z{F}JP8{T_}m!z_f)%yasB!28{_+yVI?6JI!1dV>vxhS={H^kM1`Z#wLQ82a3AJ52r
z58k|UbvI&3LwRy|m1D-0lW8Yg(+40jqz+Nt>l>hG90~d`AwV2qDlQns673A*Dic7u
zp5$j||80K#R3CX=07YA5bJp*)36~{2MhdqsuIt;^qnKr^2-4wr63a<kaYZ;wQG!So
z7aej<VW#M*(Ceq%^He1#81gOBPg>~nHwy56G|oaYWU^<YG>iC@J_+ImRYcz$-R${Z
z%RLOT;S?vH+>S?k<D+jH<?mvs2VQ;cU@|e!dtxnYgh{b}07}JLz014G>NS5?NDcd1
zpCst+M}@N<?a2>u{S{PWf1R=bJOvzs2~M&u`ANLF%o1}|Hm<aEy&sF53Ry#3mCZkF
z@({bpp0F@uX8Fpb9bsWuDT-+a%<pa?jf<^?-V^MOY{Dv|1yn1lnc=P`(pC*mk%S=R
z7Zpk}uS9%#y+9AjnsPP3Y7X{o)fNq-Va!7Cz6o7}eXDH7n;m<c*Kjn%O=+u`hW-t!
z-rzh%lX%{q2WFXMT)5JWjo5ANr_%dy66j0>;jW=ugkj4rC@M&Uw=hI8gSk!^y7Ik6
zYPq6*ulj;-*RCnpzc^vX0w{{mf)q?l&Uam1Tpl;MXFh6?lHSDW_!MYY=N_NT4s^rF
zQClz8$4qkK;#GQGNwK`bWsKb@nD1wat?DHRN4z1xn-jyfe00n(&eFG6=`~1sRWfmy
zaiF{+UZAko5VLyy5zs}~IWt}tOe9`Hu#gFZ+8taYCKAl@iCN8FbLINph(C#IEQ}_Z
zK=8nH*l2npi5bIqEcgO@BKy?_S_dR=TV_xGjDZu%#A0eM^u(qjgs;{*N|HoMsotkp
zE^E4>90UA-s^f(rM-Y}Ac7u~h-HY>rnUKMie(}BZ$Lq8MptYa2uBb9w<HWZRdQ^jY
z%xPBzA^h{)`AbqHl8CSE_HSv+BI#3f<eO)G-ErLz&+4<5V;AbO-h{DQ#(t1yj*{zV
z7Ty%7(V|(Ax%Q^98meFz?TZ)<WASNv;NmQIj?I5cp%|&dGAS-(&2^j29~^Zg+V7H4
zoDRF63?-Lyw;dvdqp1n3XIB@w1Hk4=(w#ydtU$irnMfW2e%VE_M_VJ9*q9p9Cz@gR
zqhO67b7zo7Fx&5X{VE^^+SF>T!VBq1gtqr*>WU2NldHU5gyZ;_R(?}fu<aJ~ieo~D
zkmmBfV|*x8_;_KGFm3uasXr7*OD7||1kI!*6AGtA|ITv3BDPkCZ}NMz)17W+BV&k6
zCTtzw{P@$vNZjJZ=jn^G0X)iMd%pIhTY+PuycLDVNVfYd*Euxe^hu?5C0_E}6`ov+
z@{E{SUZN{e?p$|Zx&q3W^!yUutd#ZTeJRlGyH~bV8_>ff*N<h=fCUmau60`x38%`V
zx}v%R9mH~tHc)>hsGHe|OPaAL?@L{-Ebsvb%PWw%GuY_0!<xnUs|IgK%r;>_7avNO
z`w8emD{evmVstxtXVICf<14E(^fm2*L2@@X7EAruH*$)fpWJVSd@g8~rSxLSy80q>
z-fK(W5BB9THiy3V=iJViC~<`;7i+zmXDV2_D0eGJabtpna9EYxA~<1iwkc@Q^6jc5
zk+q`uHcyoOldsrCb?L($hS3tjk^L+C^SZ==zJ_;(e2z92<O_Kplv|qJQ{_z&Xs&hC
z_e2|5PF$;{zm0QzY1#lP81jB1C44*Ak~Co@Wf(UB!3h7w`f_pT?qD&{^MoXv)Si67
zmaYBv+|6?eBQ2O7jp2%jhdlXBhmI&tMxFGV3fkoy`i{-Ar4RRW#xx1#Jn3aRVmCvY
z{G*?;m@$-7!TDn<7%zzDHi#LLBnT~(fHMYXDIB*&Ld3#MCyk28A4djXo;qE1&@dd5
zkxAun-uGVb64Jdgd@z;;58wtyD0Z=8lwdz`h3tbi<jg=@o5&5t=-Rr1)5U11GFBnw
zeQj(u?G#>KM9Hwj3ogR3vCm};iN*%)Wz$~YjddX_Dh~O-%}a%qXeiQ=d-Sn4vV=@s
z+etLhJ)Ef=ET{HTO>clRvNtXdFE5u}dH4l>dfRifv>v`>DFK=peKJRdziVnaV&{Q{
zeh@w0yk;*;+1Yg{CZ{QD(QrO|b)b|mP*629=}4c#8LI=rECa$<C993>6J-I`jX#UR
zy<}M%VQR>$8e;Il1l@igy+Y-)-dfypbFjbrvApw5$rdpnJ>(>pC8xn~EfW4HP(Ivq
z3eDT^Mc|^MZJO%EYFmV;xoU5=Xl1A7W&|z$dY`$b7RaRqL;2u@dYv*9ZGBcHjbGZq
z5}m+Ganf)q(09A`X!~qBiLJ$tr6ENp>|)0gkVg1PreIsZu9pXb$d~)0hDY+}6g|0n
z7m&ZOA{??4YD4oQP0O&r(I(w!!xM=%<%{iOhKt)Htgmihh#v!+F$=DXr$;gk7t~Hz
z%kVn)IRq@ZfXzsyJj6Ufmpmy1n#s?C%(pd6lmd&k_V`r0d?h>-*C#@|?bn_`sG)f!
zHtd!k?PH1>1Dj;6h@G!rmLY{28S_kxA*k@pu1a%Yu89ba?6G3)@c^((AucU>2Thie
zmwRY`&@pXw#=3r?esqr&L{#q@;iDT(hH)I~Y^6KKdg+Xjol=lALdQoCjG*jrT?A~q
zFCqsP8p}B=C(hi>mt^`o44zy=wlUILb@P$zYJ6!w^dlUFyVUkX&0KjI6wR1fE4SY7
zrA=vt+ty>3fDWfu*XZ5EllGm9zpJW)1ZGnbRxJH<Q>N?_o4^@pL}gR;)Bv~F(SYMU
zW^0^n5qi=$P2Z!}ybVyKab~bBlsG=j_pE9#Gwhn8#p5h|+x2gCy0L(5=jkl(X0n|O
zu8=p=`#W-G6m^WT16iQH!L(R*MKf|N^2~baW29sGNVk>%vKVACONdH~n#~lbM77V(
zf3}e$9Xr;o8(cJC0=Yhu?W%Hbi=7F)QaV~?EMT4+IKCSoy}d`Y?_TyaNOA+D$TE$4
zv?2kqOPE?8B+=?|RurxyYd?;&oDAwx-k|XC%@WPijLvaT?=i@jlZ88KHRfC&voM|F
zpzZt^Buc3}eTbMbyz@sCi{4`)Eg|?Yx^~;*dv0m-Fcv6apVR8Pd8oVJy}oBbps{WV
zR~sO!M)5`@mX<_b#w*8Mw<KtP<&_&@DrY{nL_Iux<Wp^hZx3-oB*ZwTr2S2x?>Nm2
zr&rKh4sTKVT>3HbU3!&{wOZwz4f)oSQ)IJI@!s843lFq%8$Xts&mATWYsKh^RYgfB
z{S0Y5G8@-lEQxkE>43E*!;&|9r-svH5D+64CrTb2RPGEpPP(h!;n+nlT~A7j7u1Ry
znu*A}#L8A4@m7AkEU6h=`X|uCb4J*|fF6ERW&aF%;QXZq!!yem8<`sj*}9Tw{ZfTF
zxH*^rV5}Dx*i8Ek*(Yz{U~J<AR)&8yqWIiA;Q00XKeY7UH2mLw^1Erl&BX*@W&1bN
z0!*>{Z?gX#i3Zl;|4X~S%l@L#fnk1swMsbIf4%e{eg6M!Sv)fs{%lzMtBLR@dy0eY
zS>u102CQJH-~Z4w7}JpYMK5cgsN66>QKs}gr+8yar&4?EIzl@j3mBeLHFn&1hZ|-b
z8Y?k;`S@_s?}tt~9!{*3#t--EZ5FZKhgGD>9UY;#54w*aZ&r^X>28`K^YPd_>+WJw
zv09O|^z(4)E8|P?druWB-eu_mvgu-~+q8V<V{veCwDZoX+fx;<mXuD%TBc?PrH7#$
z-FxOt+qvZ$x}*2rPbb*(b#a3Gf;!q`%mUghsG(F0tfBHTez37SO%m8gVS-P2L+EQi
z;^oQoEzVRcFotlmwYwW7E1138vI;|4W9j=8svC=yd$UT4lBi;<hT>Tzr|&5Ci>+I9
zTWySgY}~zHcf0$^frhDUt-yxreywOaP>?$m&qj8&V*BKw;}S<ZVL-jm@LiY%h0`2k
z@8T6<BZg~cK5@^fjf;YR7t&+$O#f>aGdXRYIqHWMn-a>#?xDo#k6LW9uQ*9^<4#~Y
z`Vn_HMewoI;+>bVR5)}Jb03V!m>w_M%Z;>Ik6JZM!%kdAZInp4UtGk|#`!y*bX*oG
zKoFa<ehUDYxA^gY!B3kDyjku4d9B**JXUdCt6ye@c7AzLExk*jq!EAVtZ^m~5kgJF
z+NuWKS(z-$TnZ#4NDmWssu0`X5(ZaHfy~jT8((Oq4;!8+KUw<Vy#I+_tMxtUi@8DC
zWL9;`s{vkVSklP40GP`1XmhtCYE(234}qjve-0BdHI-c0p^lPoxj4I8x_tk(MIBqH
z-W|EBC~tJH7}_D6)Fzfx3#6Eev3=_|OU}3Vyx$H?=5OVeS{iDPe8>~bVl5AK@+gNO
za3OQJM6d`jl+hWvHojJczxob?2_fAVoYRPI_R8}x@u{KoXgX^m8d?2N+ru`G{^yU<
ztho2v=uR&9oLv}cAHzhrUev8|d)q0wj^B76vl^zo!L;lEdQz}-)8Ck!iHaRLyDSSh
zGT<LD8~Aq8N5^8kMiPgXLdADLMb_uv7}&6qn~mvJXwmwBzk!{OpVvGg5N<+oW^ip4
zPFi3v7LONuL9%Vg-OnM7VRAdIfD=>}D=4XV7~k$Gzef^oC|sh6b9NG9FNe#7`+gXT
zB0F074u^Z^bMPnFG?>v(v}8t4Rd+q9AMb_`<wMOapCHbo1RLqFONW#Jku{CcH*yh3
z%KI$g--2r&5}*h<Z%L*A46E3TEZjbN@R?wMNI+dAf?*Ed!&D#SIMZWnKXC-L1xa!~
z0^(=Xx^wy@C_A*~DCF1kj_Pf2z&869!k84lSRG_9j#K#yQRMHlatZ;BdS%C3ZGv@`
zXvpAO>umZq(QF%w`-oAIK$7p__uQN!2gi<0diZ3>r30+J9R4iwM{vREbj+M%6C@^%
zH#}5z6Wxk=!nPrfgqsgUNaNUFJxo#9OrrRghJgGY%}WQJb8R;RS5Wi*mS|I=Sq~$-
zH51sj$4|Ajec4P_ClXX#<j$KCZE{5lhVu4!SZDGV(b}QLUrJ!%xtqovMwO^TESFCj
z#9@S)eAC-u4}Sy!G!|L?^l80|=rc12;VdKH7IYD(BatID%%PxH5MK|KRg0R!_(CIu
zu!gMqdM%b4zG}Y5(DTMec$xS%hRjKiiqw5V6Tb);97~5rN1k3B<c|L~Z}2{{-x)v+
zRjg0(GH**aKA;<Qm+{C^e{~w=jXfNkj|s~lEXJK)y4IN`5<r-JK4|ITZC;~qCY?CF
zvKN-3?rfOF=U$u8t1qfu`o>`%qp+{(u#P9EL<DryoLO`Zj5E9z+S^*&n=|`*k@hG`
z?HvKZtqf<MY52Z_UM9BOy>(!iUmmU?J%dQhgU^b^Q|3iBWpQpP#mN@(FXUQ2C5-7Q
zIwwA6_0h(8kMo(XyC5<{U*e_o)XYipl(sNVo;%7|WySvfCS^=En;B*i)gFSMrE5B-
zFD?oH3a$SfbJG008nc*|urThKsN<MgRXk@Ygrg(yctgmAt$^7}EM65FJ<3zv6q#k9
zIe8xvz2%mwoLgLSXjNAzsQ>HkZt48ZVY!3A7lLmWu&np*MD_H$I@Mg%_Y|=0-;3qZ
z_|HYTrqba+$z|8)_3tKroMUF%PXa+~zq)@a<i^&Ip=ag09$RKL#`1{~j0ns3H5AKe
zzCAg!?dKm28JdX+;?Ghb>JQ>WI|D6U7QjQPg9ML<`vY@&y$`LFaI(#v>Tl1XH`@=F
z6D*@*+V^cI?v9V%{dA%Eep6k!_Kj1HT2r4pU{{SGwA4>lFFDYBuw(Bh$<J}aBqs|-
zGq*lcwhR{``7sE!YTE;AX3(8`(c^ag&_thylP^EhSg3jrO6l@OeL}f!)RL%@O$%$p
zd8`)wRSFhOq9h(g5^Ydpf*urWW%eXY<Tof9#Cu50{<cIaVTAD2fHzXH7st#Uh$$aI
z$JX4s>%5)c4!U9lWTXs-jNrd(d{uq3T7yt0M2WgJ^ut!<4Y8=oq6_HqZK1nU;oT8)
z1wOt+(gr_GYf9JCxF@K=|IxH=S!#e_z$3P9D>|=lny@rpYRV5(`E-)N@8`OAkL2!|
z5;>3PMNa{$b*-f@#r^P9@gw6z%!X{3i9PQ+PUpCJtDq(ddk1e4?;+9m(e2|T0}{f#
zK;Xk*7asA`X-3Dv$4MgdrgWBdqDp=Fxm<e+(zxBlpO-(QuW;Ru8zNp9VVz}rZA*P^
zHw;4Mxf`L-l|k$&XaJE-8XUpYjAmeAsU56iN4u>Od*6x-9v76E3a-3R+Ds+>DPXs;
zp!Xgz_O0x9VY`6_4Mi_V*93?G3l_gZG=@0bwV!(yc0VSHUZm}5Q0Y`d8t3*NeGtBK
zW!p@}Dq?N6d9VU5zeK(ExMbCKMT@dzPuLNs83@wBU`pCJLjO6w<Mznw2z!&eV-hHX
zZAUlYfhFvZSfQ{tWAYQKu9foTKPaFa&waiBtbqQe`+&90XQVNX|69@+`!6L@@y}$j
z-~R6p1@Hedg2Bzg#0A!2f!qKlE&$k9%+1Nf0ak-KS$;vb+5a(4@Xo~6*2%^e?2~8x
z8`~Hh1o@Bd`Ac8^ZP>t$Xmt%uu)h7@T=A@6|M7p3{oR|+2@V%<ar|B9<@l=`|4;4y
zE(_#hVFD9*|E4c<{8dl?Cs{U5CN3~7`0v8cmF>U1@b3+C{CaV4Fy`;V&yAXY69!|?
zIoP@XLHM~7{$GUI!QJKt2dMtmGVrhV#Xr0=8`w<Z01wXJg?}yTKi1#Bv<zV50xu8N
zzYG7iBmNNnpI9e<3TJ?ADgfY@iSaA8@#nAycv%1dzc%UbkDkXz{$kT`fdh5FZ>#^A
zO#^(sxc-k#!^-|YCo<2fNkuIKQQIHXzGCC4+uy~~<eU4jzYBdO^U*}-Tl7v={nvU3
z4HpW$@uvr5?mQx0?nU5>&`c49+)d6(+d%e41hey_t)$tldk(3WViMpwv1P!wiD=^@
zS&FVyeCk+TLGox4i81e#Z%uVvOcMM{jr)aGnKTv>zOP<RAD(Q@T3onwy<tUK$P$lb
zkn;O(bFqnm1b=%C%o6FG8<ISQ{LM~jhPT1~!0?ruORR=#4`c_RD*kxXbz$6#n-7f%
zBaa=^w`|SP?UnRi#XBs`_E45H&9fZo@MxY6Z&}VSn2oF!u|Bse{i^C^Te(K4-AmQ8
zB<+BRhdSpHh#vJMMgPfH%9<)?m-%mv^heX_yWoe~tQLjqV9KuWar9rv34d-`BtC*n
zGL9H5Yr8F{bMk{R#*k-_eMc5Acz`#YhrvP{Q%qP@r=zQ)pfG+Y<&Wd(EunHuS;31q
z{++2~C+J--{1&5M@JI2+ZemAJ5h~eelKuyfFP+f>5ef{8TEGLgi~3iiW_|$=)0Ct=
z^=&zESY2yR(7L?Iy0z)cK}FfdL!`&@vM9a1HwY3&#HN1EvS2!EpO^nnx+6%3;GjYm
z;A=qaH~f%k&SI#}jA-@#4OU4C=luI8^?8)qbu7zy-SJ!8pC6QRTdwCO+R|F3GH`Tq
zG~SbuCo?*BVpX7gy*Kf5caa^LgZh71d*|>zzij_IPSY5TZQHhOr?G7}Mq@i^Y}-y_
z+h}Yzd@`LgXWH*{=Jfo|lRtA^>)QKs?;Ef6UhlOxaCJ?!j&$&BZ}oK0ZP7P)82~^;
zkX*nFTuQ`uQPCk`7u9M0l2MWdYxXQyZ+n*GzzY?=9hWzr^i?%N--1fb`s!h<3Fy6G
z1xM|DyG&nlN-$D`5`0x0sfhlOUcIW%o9ARyJsd})1oJEFW4Rla>r1Ik^5%sFI@L>L
z_RX4cq73<gsxvXRn56&ls|Wj$5eb761L67SVs+0v<S<&tWACMeqZ9fVPME7~fGv|%
z38l?!t~KH9%ETq{k6oxf5@kMQ5Ei)D$?gz487N#ktH{k&dZzc}cl%+f=NrQMCwLiS
zfZUc$xTdnXwX*;|K*HjMN%SXp)*4!k^lLkYd5~cs<h{?@tN!ojNT>Ga_zEFa*17>}
zm4K{9zEGYFGh1J{x1AB^mT3_;`p@-&!YFo*ugGN$H=zhkGHjeZT$_Mlx_On1Q@VGm
z0DXVokzk*8upCvJyzPdPVn)Z6HHI_p+023cG-%I8iaVXx8s}@=T=2Bl1L38bI=0L!
zJ1pRg=PE*VeUJ?L=For>5@fG?ATT^E8S#_w?j9E2OUHKAbc_5<XW{3z8VAnMnLSf<
zT>q`MEu>;ZE|9#F@g=A5`?7vqEAQ|;idYO^K<WePIDBII_?K!_CnD7sioK3SEb_ML
zI$N^vbPO^F=j}t7`MH4^n*v>$n%M3B!I{XD`Mbt-OZ$RChgasPX{nRflnX=@%$Vl0
z1Dk&JhLa=mj#Nv02>4bMr<v_?23W?_s#=`X#kA%y+2q@cH5UTLsWLc-FCi&YR7Qj{
zH5o3(5rHA;Fmk!}A_at4yCC+pav=?XHTyDE{=*DrMObz?d%F}h1%ZQ(Zk+mbK3?DI
zL~82&<OZX_v3<-zEH&1Y?;2<K$3^y&K-&*!Y$l7Y-mkx3M&*<~VoDIp?oFPjc7${%
zco0r#@qaF-TyHd8NVfYXvhZxf0Z%X;<rY@J^^8;w9LhEq{R%r*#XMpNBuI|E8*j=A
zRo8Ql@%bznQRhB_UGl?=Z}qUd@$EY1+i~1E_+%JT(a~}A9;O2=g8|?8z|rKE;Cj)(
zMP7n0;2tsqEg1o0SC64J4GAUkgRQwJmR-SIa`tJg+9Hdq=T0!wYQDJHn+!ECd*sq-
z3DZFp$UI1txVk%a@Gyg|+4FxKGsH#aww1Xbh>28U+w!9u)ga>8-gfy!ILp#Azd12U
zqp}B%LFOK}%2JX!HqDR@bCqVJ%bC!5-<_Ur%l!S|?TyL8Ju}wu8xCatnNohwfj_<%
zzj7e+k8jrhL5O7jE2R9G$A5v<{~$!N{S`BQiT#g4B+DDN{Q9l`O)N7TBQ+!KZ#)if
zpUD3?n2C;>ndvut!~9pi`Q6BGaK!kA-T#6pEPv&j-vl$!Gf}g=EzG|L|CMik6U@lU
zKut^cubvPVmY?&4zroROf`1rFe#1sT?vnov8~sW^|B00T4G#So0I~cAfc}9$xVZnb
z--AI`RE1X7W{;jXC!N3XdKHQa2@3j}5}FeF1OF6K7^UR3q!jcAO353jFscvy@<`cC
zBT$U`0<9a(ngWdg;o5K#zj+Yf3_1V2xqkm3{+Rc#4<gI|^+9~A8~k(Fzpd|&dHhj^
z!}4SB|G2(?xE?=08viY_zol(|D6)sEuG=p&LwTL)Y&Hx!*YJnVRJ5~+rRILdppkA^
zc666D=${ix;1sh;+0}i0004@iQaQNkL*R?@_kqRX4Vg3}6h^TI;gLC8OKVM>{5oxg
z8er#eM;od?A8UpLvIM>L(V+p;(@DDKk&{A2salIC^0n!4qKwBC2aJGhMYY$d>dZ_h
zlDOST2z6>{{4#SbW^ylt#@}9HM@_dMkp`*#>T#vrWvqy7>V_E2Jz9ZFJ=tk&h?C{O
z1wWmb0uuR!%ELA@H?oDQbx>yj@mt@QZ3uWtPCPD@$lCrsD@+%_C1Qbl{WTN(x+7PM
zm1fg=YZ>W`jEoiU1{RlNhYPFCgt<?%Vy59BM@y?PFU8mKC0sZ!v8#h8uY(~_0T_ig
zj2F4f8MliV4t*of8{O+u_jgBuQi=RH8OZ+S!7V-c%a$3*L{uq6qe~Vs#1iHRgrMUi
zgV7N8G$ey+O^c@~rK-@+1Tv)auuBY`1=_f24(S#c3X_+nUE>}?)I%vZfr7AvS%9ei
z;GLNwB&M^@57{HmNaCR2aj^GsPT`)(+rk##`x%uIVxI1_s!L<&x<3(Jy{Bth+;}ww
zy=z*;#q^z6B&JZ9_Qz@W2V`Bmt7XvPnO94ZEDi9L($9MJ8Au_QQEm*9A0yftGmwZ~
zC%F6=VN^@7&3ezh4^q{WRHC8>(<1oAJ|o!?hQll>y`_M%jTxYY#2ga)GqD;LL=`AF
zrI`7l^eSkDYVJ`L(wMhC79@!)?T4?xU(yx}`W8pO?+l&V2{mV$Da>|EJW`T5iH!}Z
zc(28%1j-v~CsXycntWx6vb6Jkx$wB1E5LsQayK6Jp(^!QWTQJw6V-SML%hjCq@l9}
zTCX}lG}+@4s-Pc)nZ)QX{gMtj$4O<0M3u_vOQIG|4|Z)-YTV;nt`x%yTaBPKo(tMT
zXt^;y<!*_tuEU}<l{Cu`k{Xjs#M+0gW(6g-1aiQZLYHKmS}I1%2iIC9f(9gW^`V-3
z6diqZ*SJR!FW$-VxX#r>iT457se`Z}4`)bg7J6ybdiYUm)Lp}SV^~D5)8AgB?>%X*
zNijVRA)HQQ^yM9t9viPvdkJm~7vtbNG)(WEWl=DiXasjrK^|a;S0zzWcn+M2)u*+f
zfEZrDZP7)wPX|)7A3s7<7v5eqr*}w)D%D8aO-pNE)k_d6%``G`3RuOat!7PiiwgAl
zKB*anq0NXbMk>Smo~y4Bfyh;IX95uU!@yg%6Mm;>s5871WzAQ$l?qSSvQ&$n7@gL^
zP@sgSp=<%dJgb6I_cw%dQmt=C=TCXtR%TTx52(o>c|C<D*|iuAuyo$fRav&x`xD7Y
z3m+Ud65)&rLiD!Uj!TuB96vn)uwA$Q4oC(@fGrMN7sb~qMuhjh5?@`=5Q7<Oco&W|
z(oB!)W!WL^Qf-jk-+!8Hr}Q0r%(aSdMhGU5<ntJdEXFRcQi2`ne0Tx2st<ts&Mz{C
zsA%X`<H^5d3n3a6bG|4*5VlU|aGp-!+Zc{t2*(8XmFDE3uBZkb>(m|G0oI7;?vM<_
zXz-iJ@v8z6;}c(J9QUqg9`Ih6vDa`h5~b0>jQ9s9)F@%8^l}`_imvIzQ&1Z7;oh8d
zh&v?}g^*6;#N{teFm3}44!v=4^_Y``B^K+Btg>Nb&?L;jN=U3BmNnh=qx^C~Br~t`
zPJ%NqT`O~kKnQTu7&{EdVSx%L>KrOgMRnGAmP((>_Ex!_a-gs(w=yCUZnq>o^FWtm
z@gJ)w2+l~OzsSt*^GL0F41;UM`k;A*!I*3*mOkzWgH8IBA<Yo%Rj<ry#c}PwXB+pl
zU7-fSy94BgXXS3;=H+b(I5eKV!boYRph_8jf~TMt*WTAn2FhU3@a2F-k#$h$A<>Um
zektE688c6Ai)=vQnATFTVqFuL&EVIxfcK$`$C=>prj7}p_3aw1_X*re4IiaY{S=57
z+T6cRUCH`s)>tdP`|8nQOKw{*YlF*eAtTvHN?UzSpi;K#=x8HpJ$flX&nr?p?wZ{z
zaUHN5c4Z>+;Zv{<sDDHbH?GGA23$r>9N6H@z4K-t?Y;npwX8ltsD8atxxnaT&7^MJ
zyP6j&<>MA84ephv;w~}is4i8U)#7uzYu@iNe%nS+f)gN968#NDr^rmhQ^lpw4rmf7
zO#$5ft-@emFhFfp1~ih0{F<OXC=CSh`SniXfl0j&MP~${(frP}8AyNF(|v{SJr1}Y
zFmq)wdlsM=+La`~0krqfK?gG&RNnp310d5|_pOIELu+(BW;YI7Dz&w?v-z2#?*Nb?
zv@N0-L%4F#QXnAP9~YjLC_nU@i0U=8&c2sAa;=JT)S5-8(~6di%wl<o9xL3_e56?m
z6E2f%)n_HC2y;k^K8%>{uzge8HAf{I2u2|@unsMMfDTzK!!<1QWCKYJH661>a<bca
z4R4jE?FFt~BetiD0pzWuiic<Z(g2xRjMIn?wR!x$#pUT{%jqMXqP$&SXpHpgNNiW|
z_D)>>W^Ei{8;LPAa*1Yqfq5^;XsJ_s0gl<nNJKjCGyt;PNtCNuJvUO}pa4|)aB<c7
z7$1KP`ZSFD>dE?bR+&B=ZD#Nt(+<rAS(Aa>Bi|!Nlg+$V0h?JH_9c#W`jTmja|-ZE
z7d|82#A1Hl&t%6e*dFUDn5dLfQ2EszUla>Um%RA8bFzEJ#=J1`^8ET~{rllcjZR9S
z$vhZ26iT6^2yg`JSao2SqRE3%o%krZ_*~A?&CjeBzS!h7x;S&BdOdws@$`SZ%Qp;+
zaQ1h<CPFkRPQ(X1PlL6I&@_P=jN!eVYi)7A;@Q;G;QSq>{hR{)8%q0E-v-P7&bPt(
zca{AoTQ}=p`Txy={X-f2n-2Y-(<OS^zY_c}vHvk$V)}*fe$e61<NiICnf}cJ|7Ozo
zYu>@|ro;aQE&nx`=}oYI1HHe-ve5l}o&U|iOmBV{+TSSP40L}@cz*MXnP}hC;5Wnm
zUq@#Ar5f>XMrLGsbKLw(C<EOeTxrI)aT(w4hrf=?@YZMXKVB_{w{ii)?~=<u`Na%x
zH4xh0q=$bp>$mO?YKGsLbLf6X_kVM>=xN_n``_LmKc_tZCYX-pEuUul&9r|!Q~xpf
zpb>scZs^~tK>w<t|5;x0X7l;ylKfX*^0%w-r+JB1w1!MKFI>R-oyy2|%);X0i4#q`
zSd@k&(#hu{PB?WRQNX+^^w(Am>i4bbBWVm*Coo-5@7zk$=_li12)~ff;CX^dVQb-c
zu}2P(irRm5ai3_%Sh>Tt#(o$tvt>7EAaggSAHj{<K*(JP@)thCl9WgYhq#=;0Mf-2
zzZp~WO+XMk$Vxo#6%K>JbQsbfB^$SGl%8OU2P26!Ywcm(#r=$czt*LUWIoD6QX<^j
zs)MwAz^Pa_+gf8qHqg;-+-GK;S~0B0ko+O&rL~xOo`1$MeA$fNvu9SGQA3dBm<vyy
z+a%S$k0S{s)YQ5KK;+~_@_V+XA`WEqD?@NeV7JXd+p{ScZ*D2yk$!NlaNY=aF{#JW
zqB)HWvB1ay2!0N9ivCz(jcnrr4KtDh=x<)QZ`GcEuEFnLxIdQZ*R=RYC(GNv{?I7*
zn-}h1pT-~a_@lI#f%&hmfnQ?(bK&oo7MwpSC;i$G^V_DEUmR_$KfhW3QT<_gGY0<n
z`=7;+bk$|Mx9X4QxXONXN|h?YO!#Q*Ni^4KVF-!c(qOLVCsNVwBz#4<+U$<At?CV-
zIEItY?_xUj?v590va-9Z;GnU21W(=WE;kc{T)1)NSkq$$Hw-yMr=xlYKe9{p#7TnF
zq;}nH+`3>!#ISzs3w-@F>43fh^Xl~-dM+QB9>(}et#n@4Z;}uu*2F0B+5X9+MZI)o
z#MR!29X3`C2y&!1exVp|fd@L4G|P=JdCdr!(zQP{Obzks05_tWQ9L{(x`r+#tjC1;
zY|(Bmg-5!ZEO|4b)gSsR>%zIbSbI6r@&dS0mF^VGthPHJv5(&6#_ng!qg7Uyr|Vfz
z)bP~hz5`ZF8T}RO!6Ag>+xz$^8%!D7H3EvH+@`u`%`*Fp#VWpR2bkv%QrX|OX<;%F
zX~XIkyMjluZGfc1wroXd9Tgwb3A^M{$?u4V3@}TB46xHqC@|hbVFwz3eQn{0MM`p<
z;+bi`yxR2v$ST~*zE?UsMRPnUzDR~P(q;cl$ti~rv1fnI9$<d#3r;D5DEewwrB2np
zaIw8Ly)}F?wZ`e)rop}1A|`Y9S*9caBBNIqY6Kn76j3CACyBC)izCthA{>jWvaYm{
zbGe08O5wA1ivNW_fQu<>rc`N&yDg_g`!WTGT8@|Xd4HYBOj6Y@w3l1%1&7?hDK@9J
z)u2LjfYDyzHzsuNs9KlNCVTI^&eHC8_sS=$v?%<|1J2osYY%4)J#v^*VsO!{5yG|z
zeY<=J9$J8wR_;#Pt#q!)0aq&XLG9Q+51Pq`6Aj`E=3rkJ;Hg$hnq!W+k~9Q5!8<<H
zR2yq($@X7jH;hCd^cR0E2Bx5OqCGo4F|FF}8*?pL(nfi+RRfONzf(zD==bIFof0KJ
zq4f5?(Ab`h^T)PmqD;3Ov=f#9*(w{P#sG#uIyO{6P&l_KsS{l1=B5!yj3F_O+RYQG
zA&&zc_&yI1?Bq#aLJ7Rh&&*=o>Z}ApZDaG13TqMKYe+jw=#~yfp|zVHK8w>_#s?#H
znZu`uvH&4RRfdCM>Zj$>Fupj1nA&LXgfQ&6raCU5?7&G*M};)6whiov4=`a=!rpL6
znbk_xUX{8;>mRgU$R9!xWoEkGjv&A>O;7f7DJU~(koCo|HCZORyNOVJ7nq_dOzz<5
z?A))3B0hxxu{x@*=?swLTo24x>D=bH=-b251qmwEjBx@364{Nx0s-W>|2&^W%~pBi
z7IYseY_h+>M@><yTGJR$P1(AXHcT5wOGq?+l0?B$;cKM66yua^7Mq_#q*bZK(jMB7
zKBF!XIgm!MhD~n!5yit6jXaWaJBw$J7`8}79;GAU9maCBQ3d-P76oAURkZQVR@xE$
z?t@g#3eEJz)$O!cDF6q|JN2TrDC+KM?W}dQ^-8A&C0D*PYgx|3Q-j(@RiAu+L{yps
z5}5?7k6D~5G;yfvLKlZF5b#zMk%(H5)|TUvCZ5%^%cin&oMQef=iQk|Uc#|!U_g^}
zqGZ=HiwZpxG(NB`S4=jZ<zrvrq)_^5$O{m*??ANl?eTp+nFg4Pp+Y~}j*y+hY6C)w
zUys!fl*C^bB)mE~MZBHD$^nZ^1t{;wHr$fBOD|7>wDVaEE69AU7kmwYpjPYPzMQoB
z=+Z_sIttjLB9p?RDc@dqYR%N!5}v48vCeye$wCLkX`#Xt;K<O8%kl2Iti+3}8}iXC
zaw&+^*}^QHzFwG-b4%l7b&#V1wn$z0{T>Ht!wOpb0sL9Ym8fP^9}B=Voy{^iKSEkq
zkh>??OYoB~iQ+8Q3=y0D(<p@}5d+S6zJirDzLVrl<Gzd&O8LGgWPs<UKEVge&2tRK
z$IE!;9m7c!yv|G}g!2eLmnU74L}JgRPDZS}39+baS+CM%^xeXNBVmysN-Pf%Z<6-#
zeLOr6^=fry=QBnrpFTpQVa2<`kL`X#sr9HxrJggiH*yV(5cF2!TndG3j_*E2DML2x
zrkaDE@{oCG$hT8`*n@!y7cHy6Qgeoxx~yxFz4uKR@p_)WrG~HTm$TGHZ#gts{?c7k
zGE#I{J02z`6irik_zv<uWlz&~l}cQ&pu*L)f&=ue!(^5OYZMmAGk=1~UkCi%Ou(k!
z!ud&))&1LUyPv=oVBJ`E7XV&@GQ%%x!85O<S_(AUKJ5+;n-|bH^h;nm7Ml4lN{Lj!
ze8`}3dg3qP8)0o!#mzCva5iSVS5_fK9+V#TwkD1iT$i1J5uoBaBq;-95m7y-oSj_N
z4zSFWmh50YTzP^Wcv72xqA(ZUC_y?Wx}rsEagd3fOX+)Sp9JhAbAI<b?)bS-^l!N1
zM>_M1KkA1^{{O@sf8}Y`AF1P?81(6B|0?DG68j$$J(ge0{r`qL-dLahM_%?fN6IhG
zm4D0Ge>1hP{H;0q2ZR3^b+i49PyeU}&@(XoDWJrwN!f2RBeWf<RN94T64?rh%?+%z
zYA~$j5-;ZV7hzVj@e-2a5d*^TisyKDKqEi{;U2iqP>bUGoX$UMJNK0AW0qy_E%N9X
z;cycLk*f6#%+iHuUz*g2Z+TXS-v;Tou9f8^Qa<RvC@R4YOVJl_>aTySTOQ5a3gXf6
zdOAO-v|nH2>L1{;SZ%+ldl;WijMQn~ds3)hR;?KE(-G9>>AY<S8a0!?7MJ%N@_I?m
ziz`sHrhB}ne`KFNE6pZk=j<;Tg)d#{AK1ndA6~@qF3*PYdZbhD!?-`3<5m-1CJa$I
zqALBoKYTpDICSM%7kT^fvbc_1Z0>?sP2}-J-(w=sY+F8h@?JN&)amFu?Dw8a_>q>+
z!ZdSNz8CYS6mmL+#|aPNR*w2Nc^X|6tJxn(#CAM4E#pRpg=bfbW44B>=L{?DUBe<%
zxzy#ALCZ#kx2{p7Mmg9h3hBW^Hh9Gpl<9`d4aXF3A>@~h&-xv@IR*%+uw$AotX5CK
zgqwx2*mLr=j=7^+g*C-5hwc|K9q^twbs0K~$DLR<(Nf~OWj+B@Tq_IMtA<y(bvvIv
zKqFRQ8#%(WxSN@(4BZ0=CzAI<%DH_TYZ-ANS^O?vlJH>?Z|yW<lAf(U=vm)*;cO<b
zq2AW-+}%1zO<;tKrAm>h3@WU)J@^=BHR!b*TLt1tY@`INQ&G{mh|gb(&iA}(xHyk)
z+-G!L9N4+xk$nYgV#GeI$dJE<M?v(n;vS@m!=$IJxwjO}Y5_xwnf^#m0x&37tR7+h
zhhAT@K9IX<Ar4c@v7}-m4kl5U^9{ewT12$rvu!qZv~m<W1Zwdq37d>4NSsuR%{pu1
z+_vxG$*P(~5u>aA#aV*vVQIlp2`DT5I}j)?xp6kdwsa`;9~G!gG29W+Z7{jb)|up(
zuL}4fwn2v1v+2_~prk)j=Md{kJMTz}fwH7d@K54ak!K^d6Wrs|HrAmi4NujP(Fb`6
zNVsvegp)1@XmZ`pp>w-r%{Ppp;^J>d1)h++L&{AbGl(6?Swd@NL$aCz(EoP#9<<5K
zOn%v+y(+u&Q;L*KFbW)1nSc!*i43Ta>!B1`^vMYr!O~ZAvT9=JJ9CNoV7rs%d2Z8+
z(3Uc6S7?YJGvPyS;rL2hVP89?JC8<D@xu^up$@>DC3$NkqfX%782t>1C5c0|7=(aV
z=U5?@cZ|KOATGSZvss8-zH_J5)qA{RGqz~mV#^zq6jZ0Es#NbylTgEf9Lh9K*?>`2
z8cX-sFQl1tSCpmb^}sTQ$`fiP7<38k$i&Y^sPzI7v%eM!u4vbGe{!4PokUfUob}=V
z&a7IwvdYCFc=XOFW0rL5GOPuc@2kT#=B#g|aOe{ITI?O;8fBf4hPm)VTr1z!0k6S3
zB(NgBWhGE5#R^-f;fXvNXZ4EOnpnaIF<k#~p4VLlh`PiAq1Z)Pb5KX7B)Q{t*%t-C
z$^gx|F)2kjJY8ps-MKL1p(4Sq$2>N^h2|S$BhBvXPCu!SD1d2B1LH`&S`ocFXA#}E
zImPdhZv)BV+5IHH=Y^)TRSlWo`Mw=lHkW542sB!*gArH;6i2gfWRY?0ZyUD?e46g^
z1?qqiokAHf1(1%@+5N5+1f-=BS|DyodGIpw?wJ`WOO69fV_RDD{2OoPnu#?{+#~xo
zx%wG}cV7~yR{3)BLcZJ`Agc=B#cU`*0K@FJcX+%}OrWEJv@wq$E${H3)0%?Rpq9LS
zP3C<t*<^TG*072r1wl<TR2F9T5k&)?KreJ6$(^0!`UJkt2Y}-VCTfY%90N-{8THlI
zDvwE_O%9pT-@oqVa#fc=Tczll&wW%mHcZ<<C2T0cB+F?vvU~rulA(uE1ac@Y>>(NE
z3qGbnb(zFbt2<>qJ~{QE6J@R6uX_8yE4c<0HrUOvhlokSgz{vEL>jt!(E<~CwpO6G
zAe`Tuz{kv%Y5;U2kO@y3!@=vy1%KP4eGvfW2Rq!M{BhJ;<27^n;_`X27KL?T|AV;8
zJe3(71riT#O*^km%UT%!Gzt0gT%9&-kzos$V*l~`QCc<v_X<z-xDREP-I8JzQkI4i
ziPi2Aqeg;LU50RSPN!MgnKdww+7x~7Smr;a`m34y<1BF=aRYsI>)dISE9X$j8Ob|^
zUN7X>pX=y@mW}^{+46LAxfGcS?5BcX6$S%slFClVJ7lmF=ca6WMwLIjLSn%BEPQRa
z0#rBFX%3HKD()arf*kQ_>zy5JKBoo)V;B_QE|LVWjj69IEnSH4Qe1WADkEbZ)-aNv
z58b|*pPE7LW}`bwU|@7F<LZcXN>tnSVvvE_E};<H1gf#15p(M39bp@-TV7in)!G)n
zi!nPp;3s;1X~T9*CSa}Wp<SaFBr~qGm%6GGJVB<$05r+SvL*#IYyNeL{5k=6i~Cmu
zlA15wV$9P|-Mjpc02&)qU;w!X!jE!Ad1f-@qfg+@256>&!ONpT^OB$?<|Ekt7Z&n{
zmf(@Td|w!)k}8Nj?>XPult45-xr41rg3Qmeuwzfw%|S`&Szz$BGP*^$95gM#R~&4P
zR$_13Q?T&{vavhzERtq!C~<W0zb+vOMyys`;O>!#p4EJ^v#V}+B?cXT$Oo1BCgs}A
z6w{ua{a)Yf2%w8tB)dFK(CLoa=t=bkzP}K!jpX{$8os8=;{}cPu}tF?QldF>G3vW@
zNFcb1DGjxFK|{KK|A&Roq!WUbaKh_&l+6OTo@y%2O4D|m@*2i;x7LIf4i&t*q-F&A
zw?X4!Ecw7EuxOjI?pp@xJRfL3Fz*dx5Z!rN;$t{?=O1BUX%Sa9w-G<CnM0tQ)-_S2
z*j#@-NuSrcRWWHwoL$pg>fk8;yuED(8<!f4g7Ch1etzloWAbay#;0X%$D=m*oh2lV
z#oA=cS1q&`aHhqI?He%M6^rd>uW7W_Ww$dAkK||Axfpig-%#kA)AxT;zkcA-uj<#2
zZp;4*g}&*n{~Y#j!t9TE{E_;_@=L4dzahzA;PL-~5Pt^Ue-I+w+pa!;C}D-GY1uE=
zBY56bZ0vtqVqhjXoIdolLs(W4ag^6c=15=Tej`L(Nd$5LbtP|46cA*>`^_C9VgZB&
zF8AAGYDSpbE>@S;`y+hcj&0oc$|W`kQhwh~C^g@jhgm!Bx5sUxI%{-%ZCL|e*_Et!
zyC_4uFATuu)z0lMmirdUZ*1K!Hu2DD{0tk_BXT~bf=wF0k6G<?*)hXftDNhIoe$)(
z!#O%KWC|yWblv&x?B>UnE~HB6z3g!6lI4Unx##fHYbRl3q;wqe-t_I*^^|81r{iv6
zYCktU6E#8`e^#<VEbCtWwsPFUbarTII+EKar?@j^rLj<^r%fo?Wpv9o1j@~t(KTdl
zpGaE-vK&8@oToC;vc7PlvT5dK?!+?Io9o5)O|4&Mb5@E`TavmXre>93+XO={6!```
z)Lq&fcq;C_dhe~5$ZUvwZC<s(F&Z7a?S<Jo5kj=%(RMRhNff4Qr6et7xX__&Ld=kl
z(u6>`cTa8}9Yp15kza$XKi(mo%!Ca20)~3(`;5CYgC09}0ODNzaIopq*hKl8teTug
z+jf1>Q)j)yiH^)B-2lEIU_c?g`lLqHkZM|gO={c@<b-I*n$7KLmaDnlmv7%*7BzX+
zcvgAVEk{P@t#Me6WN!0r3LXm*-32Q97}ErrnWI<McwQg$wA<VtEU39XT_0N4$~!n0
z!C`eWF1qH|=Vc<1tH+NLPp%%eS)Pr|19Un(w0TN-R=Wsn&IgaVAxWl_&xTnU&#Ylb
z_&apzsG^3P-T6#mEb7zPu^Dzzd!kK{<p&iU@v~tqM+9NI-~^g>4~7mpw*+uHDAOU!
zQHdw$2<ZAfovp+qp<V=DAnSQlX<#@dq7*c*qn1@oohW8~Aa7x*y$SihwCo(`MlR;r
z$N6DdhCoxxB3oIDj07h6@H?TEA8yE!U0jK%%kt3=H`*muHIn$g;*1Xv#xg6@4bs;d
z<Sx<SbPdZb=y!5;sT%A`wXk~0#0Z)U9}gw<<uK5Q_;q_3>s%UA6y$z5Km|$cFIEur
zXwEZ)XQ{=RIRlQN#tVhAkKh-bwDAwAC2bd=zSx4^7lZ^Oi%y4+*{UmJS9kQGjG`Ky
z(uJUQhnv$ilWp%k7t5G-dOLkD|EOk>qiISgc;DoqI$!DSxStMFP4a3#76$ST9S;!<
zj68EekkG6)WTc_REYi6BY1pp<O4(rev%GtsE*6`rzo&CvErFF<ID@5~smxn7G7F^T
zmCnH=uBUSAVDT#@gCw+0g1v*rX>J*4&oF%3xuT`AMwyehw7ktJYprjH0;zZ3&ieah
z`!}u_*PZOtFOpi$az24J;A^{OUt4+y>$K;kRAjg*0Br>Hd1J9HK7k1e04su5cd3UA
zPD3bq<R#XV@;<ZfE7k|_5a53M#JZ>;5NR48!y_Og4=5m?HCETrag}&u$VjnODw)#V
zDFKB#oNVT$bBzR&>O2bOfTT>u6;+ahq8*wvr?pTx3+9Z!zEMVzh#R~?O<0R4gcL5;
z;%QACf7w-?5v!s?a%E)@{Y6q&o7)9UCQNak8JY|#JwtgujNH6-#z$z+a!}?NniZl!
zNGCW8wH`Bu>KzwIcwwqF@lrhfG*nE2cJXYPVB*}0zVeXr7$TYL2pB1pU>-hEm3fZB
z*Rg(pZr-Yjsw=W(teP-T4)TLT(XF1+QAAFAs;g)<VrTm$XyuJ?pweJ;^qsUcXULEf
zhO;c$HXp<V&dYqsn5|Vd=%`u@8M9Q%*kW-No>|H&+aM4eJ%bTG|7K|Y5{!omdw9RV
zI!FXHdO~o8YGOQ>jq9FSX@rvY-LY(_9N|gqFG0k|URp+T0A&iA$>t^lj8?vVomEH5
z-klOIxe<AuEVJ|wWU~bM<t}$<{VTompJGv@trcK#1-SN84>^jGn5%mkPv6BA4QREn
z=Trm*AzOjvU5RdH@<Y?k)vFo-Nv@Z;m!8wgVosBM^PK%o_-XIv<3~5M9E(>D*~q&g
z*enEg+PTspFRhMaf$Q%(+(!+}n~Lp;BV6n5awpg@CYoj~%+ldy>24kScC=Rn9J((s
zrB}v;>8B@S@oGz_lw?cKKpqO_q7NC4uhBgRnJr6ngx?o*GbYXW^bge;=ND*6B#bGG
zASkZPgN`t!M_HvN<g?xMwwWi{Yz=G&C6d!M3N&clA-#~AO*U$Bupb!j!87sWW^(MI
zZFSy^Or`7A4hf@p%V<D!p$y%2t0eC_=srb2x)C_mPkk3ZfVY)b2ldVcN#!z34V#Uc
zq%=gA@^mAmJGYIePMRHC8N0w(9DF#2rkV#l7BgJWBnBxSl$#Q6QBGac{}5w~6x@7K
z$9OuWf0e$p6srb~f7@<JR@d3+R4M)1qsA|j;8>!xdU_aGsg+mTGA|&uTgmQRN>G2A
zR>JO&#Lka4AC68x6tD~in2}y3^i4dLrW}%|q$Ahc@9x2A-Ko*q3Q!fyHXVvwW!57t
z-MgE^v>GNDuj^s63(mM0(4Rvgfj#bK(D_uvJiCN^;m*uzjYD|NWuQ8I`X*o9Ioe`R
zaFxTwc%NFgE%_BZvcI^4X0#`(P)5ONBFY8fIOQV=Nksh3qN+3*w!L2DSp!%Mv$rv=
zR?$-tJ+8VUugkUV9*x}U`-hHjMq)b8+VE9`QTf+}5Gb6i{Zbner%_WIl{4+D`4a?0
z=k6V1o+JwN?XY@9_i@b=JCR@@G{_C0Q3rD5A*R)blBA)v$+SXYie(7U9V$Z0DLr1M
z)7r*;3+doub8m%V3`phk#>^IOPQeB~Mi`at`0r)r+^_}PVm1uv0mI*BRGX9B*H^4*
zUO<&JgLHm}a6eZPe=j7y?HBk~qW(ol{Lwu4e?qvQ^~)a!_qTcc7sCB##|X<Wor^yq
z+)q;8o3O|FrUw47y5RniuJ^WM;ZIQc-_-WzcK<_a%Ttqz-L8j!)Ai0tmBeaAO>^b!
z)K#3coTU21Mt3%<JwJ)c@+S~70+5!yc%l=c<MNucDzQPc5&FEiJU=kiN=py2e>c6|
zUOo<@S+tKSo9=mtBC=MhRx>`}UM7Cr6G%C`+$odv%Jgp5L@tqHF0dSI@0zt@d5xof
z@p$|eL^IxUG2c>fT^W@3Sg}Ua;pO)H#NN7hR=t}4Fw}ZKL{azYoAu{WH|gv6EVsnx
zr+^Q&RE64lx8E&qxu@4lV<}y|#OG6iRhy(>gZ6Swj5=PMq{UrsC90)a&MP!tjbbfo
zD7E%98p~tn7dwp2$M0^q7ySm0)`}^?GaD)o!B$+BVER9Jmu6P?9YT~oggf^LO^Y#U
ze#nh=XjI&L!OvVD`)o00Za+YAM&~lvr*cKZ{lt|*Qg^F7W!Fex^f|%&u4{J|kINz>
zT*(+;dPXO3hRLeWUu}oqUr!N+MN2~-UNxuCB)1A_q&!eUwaep*ejzR!dJ)fa?F@`S
z93>?)Jkj939luh!%Pjx2=T+@9WU42Ic6F$nOBrTHYh})aPROGTjY(eSd;K&=yL$yk
z%P*+bK^)ZQo22rOU7|t}b&H4!atK0tXkYiLfpd=pW%Xw#7i31HckFy9;TO=g8Mw7T
zcy97p_6hP~D)9&gYxoMk!sX;u!D25wEnpdb^kH>`w*0WGqTabOhLi)<kBfeT-6jzg
zT11v)G^hPtMV*?8eZ=aHYzN~q-ffWERy&(VGCfE_T}hLaX9yrydch3O42_`>8bG{a
zPEBuRx~krl_AC)oZq3dx2BLzG`{;4Mo;9@)z-t4$W1Pt!`=c=$s=pY<X|A5?Tk!!@
zPLX<2y_Am}7dgC_vWr7yq^4X3a!iyl?)%|X2&70pd@WunlMK4J`rAf^<q>mlfb#%3
z2mUOa^7)2TT^!Xrb{X9_7gAwmplD<d)Y{>T!+XfRwxiGUEAPjcz@$zxLc%~21v%~W
z{5t)282HR<p2O#EigQ1h!(T(Oo&b3c`WlQhOn+*Au$=BA*EqZUj)8;~0HUj^-I9|V
zpt9*2;^TeRiH?RYvGh3LUgki{h>W&`MM5i~%w_}bfjAYlZTR}w&R&50QGXNVnY`%L
zNDsN5@G^1MU_^vjWjJfE=}880)o(L;QWxIzBbuV&bFnC@+lS;^ED`IfA*W{;>;=5|
za^VL>)BL!x!4?@&T^8~l7|X%=vtwWf=H??IGi7~vWqVKKPYOrxyLFM^8J7^S>Z615
zN6V2}d|CL8bu7ZE^$i0V6+eQrEy2M<6te0mzF!lk)#jw6YKhD3)DR2@3AZG;aGDoY
z*P&u)zNMc;!*f;#I(8gFm_!o{W=(J|n$4wW>fhmipt7IY7blMYDDs)lvJEyc`0gta
zkCT}{lEr}ANfA+W_{c-<g%82=b6dt{Ppo-+J$>8<<4j4hcl895hq~|4v7I(iK$2dq
zkNotjLsq!#OMxlxSKt*MMI*RPf+GR!IhL`3PU>~#n;VtBmJ@gy<}z1fKIfD2TS>ww
z00(+8+EEFXvOCj9ae*iW<$s-%b*lgba26I=N_+2ypM88q3f%nZc_d&XRQQ{31z<)L
zG6yBrn|YHWVP+98s16RgEP}sYZ;FI*d^DSQL^1SZ0Ru8M7|sG{tz4-DpKI`eb7Lv+
zkrZuSrM9Qmr)T(thZZ?pNG?9U_8w`_v70>8G<~ZU(drI4Ia)Mi6KkUlP8SlF=hbhM
z_Uw!+6m@bb%DP=g>LL&9L`z?6wwVmfn8?Asg<Vw=BsJ;;;X{&$3u1*gX7r{ah6afG
z?9O}fy`j*;ou^sXj~C$B=~hhOKeO7M;U9ro-0I4%^bTo6B387XqND3$>y(T4fwIZr
zEKhT?)Ea1A$x32@irxYwsaxy$*Qv0p>VzX7gWth|iK%s-!z0)@_<Kt?wD?7aNTFj}
zN43m`Q^yK_k2F+Ft9v%qj4Q26emcl}r9-I=Fdal1>1ZZ^yUaQY^U3*&@hGP&DkSzD
zr5y~Mz({)5{`^BKz2L~(p*3b20!G3c%C5ccQ<s~Kl4b!)4D4BSY}Su-5Ecmp$izcD
zfDi#vt-wQ#rlhpG!yL>>LmBkvaDFMw1mA}o+ZioW;=V=EH=fiO4Qb%qhmH805?eCu
zT!`@`h`qqI`R&XpZeTI<DNSOq@0=Wt$$IE>ry<G#c=DJPG;$!a*vU>@;b4}i%2CtQ
zew=;o!F027+3X1!D|gqw<b;wCSa^XK-<>3rcF+8p1H|u~jbrY1u!{t}LDq(Pb5X+G
z2_17-7731)oBN{sW$-LSi_S{6_6}S;5Q6U!V6m!%UdmCDEH)`ZIqu255K{chJnXuU
zpI2%A6c5`ZAsm{-^_L!ZV-YIaVK37>LsE2Mw@y%NPTYuekEBeN)cDP$PFVA|-iuk-
z0;2F5vCk9BT<A&`Hjvs6C(J4NN$3EedpKaQ;FVHO@5pe%Lkgco9!RHP8(<&5*oS3>
z1<h4FIvGSU<eET_yPv9OnN^;*+iYTiaP9jd_Njsz)8$M*&sTAer|))kPc3cT>$nW=
zNMI;6BYzaeS9b`-yji5fCbFLttbE)Ye?K;%Vx2T938j7q`3cX+ZfBo>@ODUCJutcM
zoFIUkCKRUwk58~S@1-z*XbsTuE2Omq#Sl8`qN;=n5b7MH&rD6|sYCF!e8c`Xm#qx-
zt=>*LOsI*5SDL`Q`9)zs2YlzHt$u_$07zE>5CPM9H6qIb<3S3?ngRtkVI<&i9!kRF
z>Bs?GD>+tTe^U`w_H;00xq=Kp04GTpG|^X+;Evk>9$upsspfT(l2POr3ov_Uj<eWt
z>zY?Uw*5=5-yz)3McLmX+~48mF9`Qv<>CKV4;m}&&*I+?g!?;g{lVrFEWh+f{)BM<
zgtotwaR1jT94qZ_+>viq&p%e--s-e}+eV=6tOCxi2xY;M^2E)up#VvA2?;b5Ujs=f
z1(-^X88O5`)J*I35vI>NbjV6Nfif9$tku=psFfV1ulWAk;gJ$VW=S@sQ;VU}+K1GW
zXS;@FWJm9ti|JyMjYaD|Ggok(^Z}N~t#-=9l0)qBmaU%IOcjcnx$#JqiuL#ssVcQo
z=8|d`i_g_=s%zHseY;+5E2kSJ`S*<`Y)2K=^UmLZ4kE!I?XpC-gx53K6}K@wsUnjO
zDdy^v5Zxx6?~iyhWP^$rC1#nByPpCN##7R%t>TYlB5k6!oElbEEo<?7p<DZJSNk<x
zuVyOxA${C1lk%xc0vZFgh~TNZ@ZCDp&&@4y2ih3m!roIZm_efB6OuggZ8;3|GHydZ
z8MNU=TCifCFm28`21^*jZW)0N85muq+>op|&K<c{NR{$o)zGmbx=Koq-Db`%nipAj
zIXJdpI<Hfo;W+hKHfdO~m|J$c6?wn*tBx=F5bSvpmI*21^9OK+X}fDC1HU75?MB9b
zfcF)-&aRLNlV~IgLx)^OMQG?R?WF67<q~<)^MIM0L=p72ky>>!dXhL!cz-L)e0XFl
z*Z8f!yv_C!n?thX6C4U`KLg#-yJtO}^HY*bV&*Dg?sCzyyCIByE{<T3mOQQ}ieSkm
zw?br`zy}kQl%*WxI(G$g;3Hh(Fk)shxIi!_IL>nsR339Z%)AN88scmz;>=CM2n{2-
zbzj7hHH#=;##ln&LXod%3StY;=8{5S&9H7YI*Vx4(HMS;nwpr<Wu6rS#<7Ra-wH0$
z*?Q+NfjyUMS$sNQH{GI4);DL~xeH%g5Esp`fxS~sGlqXUGizJgHV<HCjen6=BWL&$
zqM-VX5I``uJrWD(D6zi{vuL4Ne`4=RlM0Nsx3O9LVEvnX*tJND_aW;R0rFOf5uA6b
z;o~q1Akx_r;JYa}`6d_Ik8#|-2v=*G-A;jyrHYL>>Kzb?U)F5sB$~WA_&gyb_u$Tl
zT+~9cJZpl0Nqq?lr*&!%4XN_uS5JcF^a&P$7Dw^i-YXMXLa^3cyKU>2udjJCyCD^E
zZbKZlVz})zE-qz6pAzanjqja5SwdvOd&Mee2-v&$CIx=ieap$e3Ha}rec2+EZVZ%l
z?5EB%%_U;028A>=#ssP6eqTCSrD+&Hgj9ph-m}MyI+*d{xJd7T=oTm%$k`G^FLqd$
zTxOLp67w^`Q3?czs65-L?QxS?u^tJ`DKH0hJ*f$V)L@ptp4zhu&!D!1Io`(sq+|s2
z9IO>|?0kVprF_0)$*uH$$xqoIf(21gSVXhMsu%fD$JB<q5Hy0Q#cA7Sz&<eBLpFnF
zgCi}D6tN**jYEJM?C?*+)WQ(*iG8kh&Gy3AOLHinBLNE>!b!s3p_7GP=IA9fswIyt
z=)XWa5+w%(1|<c=nV+at4eti;#jsA+dch<n5fd%*!)Zbq8M1BPt>#sUa63}tDNfYl
z+PX;J>sDalq!^jTj8r|5v<MDxNG$vO$cuF_u+*TqnIaL*6SqDVWxz5{J5`%K2@zdp
zq6a2#YJddHd-mO1IKc>`{!9gJFBUaJ{86cRz|m%_PLg$PVmS9*lAm9^vNhBph`a5C
zm0cE;oR!SUR|IUW><mScQU!?Mq+(1Tc|+&#8PwupfGbWXl;hkB$oBGB^Y=TjM2ikJ
z7c<5FUx?97Dt7iE_5flV;oX|rvYRX8hiNMihR%xn<#oaUm}eJq9Yw`qOROf0Sp1Im
zj8Tar(-W0Wi})pLi1{~WJ^{G#QCTzqcccd0M8mU?`|5M?#m?utH|p_6^e$njWc2IR
zk+5~2sWHs)PwR2Sew`+)0LMcMzJ#4SXK?gk6wR(|^5ItT+$lUwt_70<Mg{ewS%=$r
zwqb2N^TdKjtr%jYOGSTWee!L4INZ2LXkF|Xt4H@&Bu1vkHbz~PdY&=iuVoYD;qer^
zR$gdqrt_=KNP~3o^}554#t#*yC6&-<UJ28kS~n%iU{#QL7c^beB8Vk>SY+oldN*yS
z3P_YlHwrf*ECOmX5pc}GCr0vgn}!)-s|m=1C~u#ITYDO4;}(-jw5yyh7RgG<b=B?f
zE`im*#Bj>aGmRKc@C7C4<0QH<c!{_K&|nYs4kxidW-T%G?3Jx6Fr)W&BdMc`zOoTx
z;sjvyyFGbY#V6KwP42J?Vb{=pe6bSMvOxb}ZKFj}2A<*p=BF9mgNUIo9$a)wq_Bqj
zuv6%EuL%M*<xlMmQtuoJLK#oqt%NyCHN2j%n!_~(r{QUctPPKZMg(J50s{=#BSjrZ
zDsg!z&mt8`-AqJ=$2yFf5sI3r8&becq{-pz?lJY}(r?BNt(^IAD&~6GjZ2BQ<yxgD
zFD47K6HL~qdo%a}bLrgT2Kyc}OCCjrF^|j8#_o*H_iiN4Ba<_Dzgb+vzSw3YVQg8Z
zG1Vhia%0|{UE-6SW8vcriz<XGi52beMev7=f)R&jm1KR47RU~>O3uZb<g{nW{@y0Z
zAwwFT)%sD&she(+_blsbaxP!j*EIpP1a!YSFE~ImoOy--Vdv9fa4ZZb4HSb`_7N1R
zvI1zYm~)5-P1j0_T04t(6d{QqZdJYV@H=LSUa$VgZ2%FRNtV2lq~N=Z;8(jkfvg+C
z;1*VV6)}e|h;F4%GJ=_3^{-L*)7n0UJ|^DWwAg9Hm3*_RoA$0MpC4{fQd(JCrC~|s
zn7+2iUT^hmeZZ;GXB7HZ_+ou)S@~xz^54MM4}00q`Wfqwrl&vF&scw0=YHNPm-WYC
z6#r;1d4sRN&*P8uv%e4ikJz8!>;I^q{Y;_%t)IO~aDS+u{f)P_nW5VDR6yDo0qL!d
z4K62_krqzCkx4_*kVu!Xku=0+>I^FkNMgNprjUswoT^VuvwIW14v1XJ)ud(Q4<AIv
z;;a#`dxkkryP%yUKDs<OrIjbx)`rhdpIG4#r5ZdWNWG@DDb7Bbei&~Y>E-ZPvM{2V
zd^&5;X@6{7zFuGB6nUijn=LNy;pW>-kNI7i_mCUbCdt)?xohcO<+EeE`WUL0Jf_Uz
z7<WkAaNIU>EUa}b4WfrJTX;dso=MY^U;L3t$GX(C-sRr<uqjx_Grg8D#r@$Dx0>X<
zEhbeglgC&=MeI7U#Q5Der)JRD#4;sYAnpp?`5XIM0fEUznXRm(W}&MBmnJbInyUh1
zT3MECqo6eB65j%2;sEOAyouFy`IPk|hLQ^f`bYEctzmm~FX1iL=%b_Cs|*_+1f%BJ
zv>3F403%*YB9F8643tzQscd_q14#o{V@6k2<*dUa+47q9YO$Vc?k12){j&|t8u^BR
zN5w@=hwYu2*3G+p+1BIbH6nBiB5FaSYbJS71@J8q;gaTZ;@fRej%bZt>#j?+v@~H9
z^heydVPc;*Cn8F|+HdRhb&*PEY}2y@=?Hleo3y|sB(uJ^y6v#Z1MRYh)z*(=jX>4S
z3^*sZlbxp=$71<Pb|s~aAUU#O%|h1?_@H(tVT@C1*Z|fK4FMEv=V(CYEq*bXtyfnW
z=LoQ{zny=cc7YLTTv-aNQJ(i;!5#e#bl9y$*g(8B(rGyyj{=@Mqb;|yBy>>YlbN+X
z59{D@@kDv^Za=-lp39DzE>tGLa6s3tDtW3*Pv=hBL5j5UN}A`kq$wh7_NJJo%g1b^
zfJ`#ov`?1em6%=_kiq3HO9aWZPi~elDGfyvWh`Mywxl$@2z&8^B@7BA_N9~@9PfLD
zF67wh(e7gs{DChprhui%Eb8Zs$qkYx>JWm!EHR<2ce@|2Z12a?Iql$40xu%Oz%|~E
z@0cS@$k#OIk+#Ib^z~FU+{8<;rIJR#H`u7K6Ac$SLsD>PXWZ(x(;z&<K6sq!Hv{>O
zyW0y_^M%2#lTLqwbY-RMnZv@l^}8i8HU*DyG5G{+R7*Y)p(yHkFz_-1&6`8!^Dcob
zxO6-@<DN3J0m!$w`FKqUyEKAf#9^1p0)cH)lrk0$oR~;vo}KhvT{<~bJ)t%HZ)fLM
z82F^Q_n6@oJqh8F>Ys15H^Qy%`+M8ISRm4mV(2xM`J$DmO>E=Z+sQ4rYh@r&j8w&3
zl^VZ_iXsKER1=>NU1b#oA|Ww_q-73KbR=CKqQ`9plt*!sB{VsWu4}v~ChdO~DWlDT
zAe#082le9=pegEH1%%z-!X&K!K02X_hc`0eY3)45&&PL@1W97xuvi%P*vkFxKoH8}
zD8_a|aq__X3E)Ju?ZXEX05x-C051}|hyzphW#e+fd}FJ*1k7eav`uN8^M+|2WV}^z
z7K-P<+@=g6x0m-?ylL_|76zx}IYR5~!nsqNJWKLRPF`I%1R0G4Ee<SAv*Wj!=KOcU
z0eQuZm<fEv#Ke1$<|^t#>Q$1tb=ZX*No?1&6_!SLjt@ieHZ)k4vKHiB=EJel^z{g1
zp&Zrp6u=1@<uH7D&b`wng=FbEsWT`oW3J0-kYitQG|Ci+C-c92kOhz;kK^h_;RCGc
zMi9q)j|eR6n+$sy%F*ag==KQ3yL$yk&1h{BGrW&yn$aRsFW1n8vy*8kvurvbjFg}U
zjqK5Bm)}?+TNcMgD<zc2qr-KIKx~G;Efchi_;N#pp98)_hVypj{G&}X{Y(xa7mq*n
z_0%OTsF?oa&L;BiOaZlA_u`0<ukbg($M>XPD$go`zQ=wrm=szNX34f<tVl{de@WcB
z?zzrFcZIK(2IJ@vogxQ`p>*xMCy<us9wKx-=qs7fe}KPi?i?h6)NKck7+otpE~!7=
zp7jEkoLX<#>q|xUrcqj$A7H}h!@~gYaU>;*rE6O5H+t8Z?r}KKe@n2QpMhrZ{5k?o
zo@JeIB|WejPZd70!t@RhR&GWBB+$6y(#zO1=)E@(Eg*X%z_ZDoVit&`b$uTn_UE{D
zC1O}0){o8u>uw&^j01e+V8D5_ZfcF?1K4|;Ip^xTSI&j={CYJQP0v06_BtQ2g(jlY
z5NuTx@LaLOKu?i3*hQ4?T@A?TbeC2w+48@3Ck41GHV9e!kAKY8XvApPjdG7?MyYm|
ztKR=SotsCHkWmh>e!#xn7v7N*ddI}`5!zQ|uc0b?o9=Dnf!lV)h#}NM6VUWoVndA=
zFoD}_=T{-fXPpbw`ov16@NM1<O>w%y=2N?9CPxUwaM4voA2sUS6A{^vfw&HY0)~*W
zSKln2Fx`YjY&)*RP0cbF?>KO%&CRN!u)yT86d_i~C2D#-ym#!a5l4#ELE^qDa<@Yh
zXrQc+--?bV0Y7a(8l|1<)zXl0vhuDo$V(EEMJDkYq&H2BfyN$qrZ>XI&V_QN)}BC%
zzaoEOR<V&)JuetqOSv`E!BEk(6Su-!o@~+x*NlmJz!uYO9j-<RaRzgCSsfkVEHFt&
zs?jfCV@$3S;M%BIZDaZFpHv570`l37v;)TwIa+$+qLa$jhp_pYk+<SVeY3l@ys)48
zOBQ2#AK%Ou?a{^AwdqKR+};DN*29v+irj4sX4HKcMNb(0n=%SjWcSNuvYD>BupmTj
zu*S#IWfk}Qrc881BqN<T$j_bLCZA>7TFqwt02K%=J^>PXwGp=!4X+u3zHCRlY$LxQ
z(ykH9RRgKN9iQyfa(KLY-0IBTD)j;yT7M+{J3#xnq43`T+7IFCS3vt;wZ#7h(0-f(
z_7BMV0cbz;wm;dLmGuq8{>hX-0ownM>ew&M@Bd9}e}1M|y4tYS5i{KC9)-VE3P4B&
zj+jMG3y{<#KB)SN1+`U67SWJig<x?m(t!8+roL!&_H-7-vRl*@tNkui8)Zaa_5Jt5
z%#sft)Ss2A)@kP6l&;lPn%hS$IVTmJS9RnME01-m)XX*q1{uepkF#b~n(IAx_4gh(
z1G7_QH`~Lr1qHKq8Wt#zCw5A$Gks>yjg9MENXw5weO9oE#al;A{~vpA9anYJ^^Hm+
z9U>v6C@Bq_Qd&YJM39n@mXt<mM7lvjLQ=Y=TLh(BP`bOj&+NUqu8Zrv@B4Z0^E~H0
z=RBX!^9PIB^P5>Svu4d&-!;FzXI_oORr_=<f5dKcCCywNRr$(vl76E`7m09mEc5-h
zTW7IbGkIsq?A%iJBxn`-1(P-jnW}l8YK~S=UeK@a2Y@eF$&!$nP`x!olb+>s-MKr<
zQ@-Lszl=rj!Y#jX`MsJ|d4=03u|v+8Zn?{s_WUT!lU4aNvv|{78;yi!<9ey(Ap7#|
z`O`{g)f8#3PiyNLx6BuKPLOinefrr@%Gs0u8N0^jrb)fSPvLUG+?p!cvZycm4^y3A
z-wJ-W_@m)*-`AUYBNUV=n-R5&2CR4|w(XDGK4@e`r35hhy<@#Az#;d0DQ9tkS~R>L
zt6#(UoturHu2_W3@%v1<CstbuGgFr8I9ubJlvUe;Y`1^q%*`Ah2Xw9;SJ7CQvW$Mr
zT|r@)w?;=GySI9qb6+akg_*AK6D@aF#PPURlBH4<77xDN@dE*fs*$Ahp}Qk0CjCr6
zAdxm+mU!PcDz-wjk>TWYX*ES0Z5i$N>5Iov_*zv9ID!mM_>tb;L6aD~`=L_Q03*Vf
z+P-rCdH%j@7KI$TVHYjhHfoRZ?x(P~Ic8rT1j-=97HMBl=IAZtG)BK1>7(gg@=FC*
z><T$0FOwA9L-y_(%85Ak7C<Fs`6)gn@e$LRK>a8CCy9XHdv>^8YE1WKUTF_XeaPE;
zqyNlxud*yOPxtW6cAs}V8CmYPZ*sbH`j76>PSL+wJtVhmSs)Zv9r0(o{f@O<B;5aq
z@sUmj4)QErDA@CQk|!>_H%fN=JeYBAHKt$i%f<1r+Ou@AJVSqM9Zqn=_XEpEzwbo)
ziUjGWUkkz@l`pa|L;IRVqU^M<QTfDAto^>@7#U3gTR_VqpHyN-%{nO?+RsE$_MOm$
zLKSqO?tyeay%&-EygTk4m3w-cH>{A;1JoS1<M$-o)A6WtgI7Ne*qTWXtjg({<$PoF
zMY)m5q-#aH_PQ=78Ov}@dj`p%Ik19;6{m82?)^Z|A+3#x*9PH@<1dyOiLaPY%e^7J
z4+6gr`AGP04Ic(RDLD*IBp@f|4YTpGqt;>8eDUSywb5I7!Q}QL?dWIKB0(R7EuIgQ
zEK+5%2ENeFyZ4e@VDMUgN<wn+yT~U?4?F!VKlY<~m9BxWiE)(QD1O)XIGg;zwVp=r
z6q&&K@F-=g6bTCf=6h(&9hoj_(Hy6d`3YM#w9=m^qV!}Gq&U@r6A<fqyoVW8(X?^X
z-tXt_&7rt6s-s@?yhTMsGRxA9y5lmDXla9obG<9?<CoXh9ByG9H$)va1m=5g+*3ZX
z#VOLuuevSF!!Mb?6s&llsp7Ko1lNC5WF%?0g~ObE>a#K?f^4OdiG?CN$7i$q?`dau
z#1Z-HHNIBOK2*X>&lmPhw+^l|d4zDNR}rit^PnKzYQ8A$bkl;KY~qLWG>WI1&kaN-
zrIfw~CJN_TthNsQubZBxNs_U{4}UR+77^^d-tIH%y0)B)`ulb%$1a;iR7KAEJ@sYk
z8&AphXl$D}I2rcB-C0pR?86V4L(zyHnyh+Lhd89Q$tp><G4zyvq^I3QeDS)FNE%Nr
z_iM2DgHbjm;%UUh&a+_kM|c+fJuguZ*W?Tlv5KExD-?@)2rfR_><FW4<<_t6$92ra
z8(JIoDJHt!+!#87ew4-0K+J)og!1mD1bVuMrA@x=4J~0sA$Cp{x(pl=`Y>V^tHRXR
z^Jzc13PmCwZKgyI%>5J_R2dRC;X%~uE}oL9^Pk9=40hgeLZ7+8Vf{;SRg|v&Vaukl
z9Ture^E>Rgsy7dc=CldIrN;`AA0j$Y*J{QS&(qAmyTv}_`t6CyD>?Ge82LJb92r!P
z#2(F&)|#HaRX+D#G!o<%)}ZKnY_-HZY~D-Dl0}lgoE6(4A#5#sM*XbDM2bwNh<EKy
zDCb&y_+5!qASrth-o%+MIm9mVyPtPjUtAm|yDKT9&La~|#L?c+_og9|ls;~WH@~-3
zeSN|CZQIXq+pl75WMz8XuLSFSori7iNcw&0n|BmBSY2iFI&z}bRhY4B`AM_C-pjQS
zdX%K{vDemPH4SsU2cx>1b$C~ljxJ*BS4e3`rf6`A59t|w(n}fP=O3TisEW*o<Ivk#
z^h%pdyjitUNK1JfiQa+f!Gp!of!{rcF<yb;+Q|K>=Efm;(I(;DFCSyp!VEuFniR@u
z#|kmUMm@;LkeNGvoZgl|rh}r8j<+YYQWs2p=8|$Jku|>jxXEz97PXh6Ghy3hU+rhh
zN7kZOJ?+08%M?Rpgap}C=(2~Lq#!SPNFRpATCQ36jCw>S`phV(<mP7uHYz-wtb*7R
z_-iZLj2pN)xe2pT3WuZM-#8a@r@j3~>&OSZT{VA^EE$vihAT=5)k{4x*d0qAw|B&O
z6-Vj#aojY|E@GC^^r?s)u1VT{vn<Q{CY8}k&U>a`ZYo;|V{at??8aYl(2D+|u+33C
zUp=B<czv6joNza*LPLAVu*|fXf#3l0M9E=Y&Or9}TsLP%0&8E$&JVv?_6s9S-i)0Y
z5|w(+heZgqZ0fk;I(uCVotswKepC2M$iqu`ww@C44Hr76l@Tb{NBL*6_T9*}3^pTZ
zyNPLhn~sD?dl9LJ^}mH36`Ea0s<I>;P0)FE#JOzWSYz)@el!;IjMVe1#wTBD&W1dG
zQrtW2i2PC=X%(b*8+k9N7J;YFS7jzXnHxVxIId7LD{mf~rxb+zT>kE|+a}Mda)B5o
zEjaQ|fZyK^j``=n4~ByN_F9w|c2v!OWoHhYQu-Hog91NTKmNU)xxby0a|QVQ*LLRE
zufoYcupT(8{@>l1%Tt@MndT*M?ba6QY!A!+rgw`V$<QQ+oUO6?o1t066BA1+a=gaU
z7DIDF+KaQ&TnkcCBm0&0#L)X$gF<!ZX*b_4l%_hYv#x&)=zgX*%x-I*77{2|v9xO|
zc8lb!ONY0wL!*Q+U!dM0@9ccL%E(r&D%eeUWxveyRkWK|y=x+lrM$|-xN$0iZ~pVM
z(u3RvoNo{8qFuFTnBz(G<F*ODQL#F`<(bZl-_k@KdsCyk<Zp#fZ11==grc455ZJXT
z(eFglQ(O=v(bQ$)uysQ9)(Bg#-rhD3?Njhzhh~N)$w!O*<OEyK088R3-D<%_EMbG%
zzBaZec}6h_A{P?3tRGccpFD8tOx3h{<I<!&YKTJEY4;-X&8ap0%zYy5=cI=jHC$Dj
z=^riaQ^}3KFENg+y}Zvj6OAlHIb+Q<-^f}~uA?%l)U)w6E$A&$awB_)%nJp>!|G<-
z-sDlkqb2H9HTPVhM^0kzoT4_SQ+!|2%apd8iq)^#WpJqrBL^!E%|NuwpRP?SB_}Hw
ze^{H?IkeL#E{)mS9#=5^$Z3Y+Kl{pFY6$;en8;#}aD?&pLbco7EzHOI`hCG4wt`<y
z+V_6qZE(#Uu1musEe&Whe}Uc|qqz}UZdPlUQxQ5P*dpz@pSn0CR&(A!ho}4c3}0;b
z<D=`g_;-Jy>O~?UZkO}EPgePg;y*4Ga??PG(*I_BXpHZGddUM<q=}j^LSqU0AIe#V
z(RvbAW|8@EzYVH$MIC-C<qu2nUA$dqkNnDcEbJ$SsN7Rq966VC?DXh<Xn@$-ykSoz
zMB?}B*GyoOCgw!{sBX@?d$7Y4$@dXe@ayHE7?V+B-SvcrWl<i?{8L!BY!<r8PmOx5
z5RZAw#u^f-i=A)LIX!#1;b`b_bYAOn?|2HUs?0KgQX1hNQ>nN5g?%QkST^&llWVnN
z3|aCJNm;#5*)9M5ulrQ-Y<D#`6h4=#y5s*Gi@&QbuP&psx}oRf5uRvR7GU^Q?mR-Q
z4ntI?&Ea)h#D-U}(}aTADs!UCf~z-y(YUkj$s%@pf-|aEcdz=IcZ;g);&6q|lv7C8
zY{u5HKFW;>`OIj-t{Q`3;qTI$6?eLs6AwGB=oT$Uvbc-uS)Bz!6_TX*a3{6mZt|`>
z*2TLE6uL=bRA@=5i-k%_Eou%*B$#>@6A`98`{9c=f`(U>Y`4&X;1tZ(H>)L~z?Y2a
z&x2=}aQLPsUc<HhJRrS(6EP0GP=X?JN^iu$b3#GaouLPLsXU$A;m3g@vpW<0j!vbX
z26yHZ<~Ipb`)!hdpC#pKg!jQ!_C9mYqNG`qDBe9H<jTBUMH&0Nv*S)_;;#8#Nlfo_
zj(Tn;5munRMAF!)R9&?eB@w%pw(qef#6LrJx30F3T>Jgt@Q#Z@>(+~~+#lo_9Y&)j
zCbSrgT-A`(L*Fkt_R%apD2NSSopn}C)UJ!*3+gmP%EBte?|;pi=F#;=^;Dvf#b*X0
z^K+5nAZ2VeMbM1ivEI=8T%lNdn~sNMI`$BQ*{#q-kFv&(Gp}mFN(@zWLq48*mqKdr
z69*RllXr%B0=Vo=-9+jt{mgvm*M6t?KKbsqr!zJp5+Ol0KQWTOrePCLbbqyo#;FjM
zm%ven#l@j+V)$Lt>PR>pTPLBwyHq^toBCX69>e4*by(dyB_=;;l&q86rKLXe#|H0o
zxP||^_-VQ8z30}ReAtUg*1=mOD)WfNWh!}kyPINmgr%WCa3*)laY^H4lR_Qw*6TvA
zXT4T&-&sP+i)rKe&eP^z6V6N?Kpa)OoR>zZ4z;bwD;trVAG?^kilfIqc$a<4e-ULo
zvY#@Z8R9seq_^=j{^GSqY}r^@^vthU#o6{{8H<5(XmUIdzjE4;cw40D6i&ja!|UY1
z3^5XDw`8*iczp;B3Hs(0ZjoY-e_28fuNIL&3+lg4U@-mq7#q1@FBxk!4*Yg3ieiuh
z#bg?};BYFccMRgik)zOFWb|glp0q9IJ1gI4Ol(E7_JQ}uWS$!io8hO46pI`eE#YP%
z(KfgF4Qf8)t$DS3S)2#^e5`pZga4hLxhR)OSx+l6=cu*woZIMWtD}{It8LDhk;3z~
z@aN;(-EEa;Q}_H-R5Fx5{_1@53lkWjurh_rmmVS}qC^_9sgK{+-+ZlSZLcUGX@L2l
z!QjX#Dg`lzK3!e>=ekJ|l1<su<F-YD-6x3=GDD;c$<N;?ztN0Zd(_kFYK2<$Srqj~
z{*$Og0uh5C>aGwq*GI|}foaOzXm<$wInbx-T5nkigo)1Gq!JinZXdEDn=@{yrT@xC
zmE(@Dt8$;2WFcrr0OPZiub|&@hw<-6WswHrtXOvM<L*h@M;6JK5bWPqZ&XT7(4)AK
z5tsCgk@Y6CFmnnv6Ip|$aq)?cKxXBUeY!e6XXO`na_uk*t3D|YO^K1>PVYQN^hOc$
z4aTKgrh@5(R5`N>Li|1B*DlEWj?QF|8cO9#DVrezcX{iA#?g0-Wxw7Dn;;Nb_~;TY
zFR1$Zp4IOs3Fw@+1Ny_~veAMyB<$*Tm*yL+;;AD<z9d(FEmE!PyjHg*GFjGUieja0
zU@>&u6IHNvO8^PmtTFcBOq*UP$ey=lMuwHrxD}I)>#cxRN#@jr`M_kKT?@uqbeR*$
zQGOTrS=-&-v*WImQ{ubv1!)SkVUAM~)*rk^!ap%?XWWhI%J~$ynLZ~oBYb$BCR`bB
zyjyKVW0*zbdz^z^ahov}8~#Ik|NYtkl?*;64E0e7u>~?}y5z*>R)%M07e3L_wgdq$
zW#gt^e;!yeHfiB&PvoHGpsZ!zo_sdF#pb>e_H5q~g2!=)5Zp!=_Jn@)#R==Af~2FY
zd>{6Q@?Tn?1N|*Z=86dAjyrExnrz{%_HN&&!MyeM(~+W(r_mQgxz*3lw=6z=OXKWK
zv^_prr>YH8KtEq1^jpj+xPzdFy~gXR(H38iu)`oqR?t%&NL^`YaD(nWnQ0AWTb#5&
z`GQ%q`-PI>Jo$Me_r^ukdufRUp0o1X<AX#mbSGzm9xMqR^9axRM~yyoIQylIC0Er~
zC><|=f^`;)cm9rGHXNhhQS7aJ#FO#5^-1(-0;>C7?<~l?UCnoGk%aCeVNRsU%QFSW
ze(1?#S8u=JRk4wB%zF2DEPKxTm!`7YXoEqz67Ogv)^~ymwOOL~=<W$h*G`P(MaB@)
z7{yzKzJ9#uBt?40=|n>>bNpMUhp-CQ)*y&N>lE$jCg18mgSS_IW%@7R?d2xsUpHj&
z!j5tLFTvZZ+o>>k3;T7*zqPOVw=*QKz}x=^_cdY1DgLqTdWm(xL7Z@?`){0`Z}3br
zq@NAXxx0Yvz&cAjC?SLv)eG5orh;zWH?+r!;MFwkQ^M-u{OnUFh~`3mx;*7bwLnP;
zga-TNtm!7tWFf~_KRm8!_SS7LjIan3bjnX8SySu5Y-CNnQzU$$UN&th$gkyHkq;1d
zN(G4;<8F0vgg*^qm-EM->h-66^SGEd{f?~E(}?ez(c`R9uR6p=mR+=ISe$b3x%+OD
z?2S{0H{}nd?p9CSHyAVgivR7tV}+!1kKsyyTK#k}XBGP#n-o$OdG{B-+iw_?B#Un-
zed9CvqMT!QL8cdq)s_WtV8Nh$kf^o1o$}I{F+bkm7aYswuCHaw?KsFUUJj!!rS>!`
zGvObM{cb4Z`avLD6Wtb>n^uY^PAt>){HY^T&@R9HNU*2|j)KKdyMvalP*H1a<(^tL
zd76upe%q^J0{vE$&JtV7pP4I9r6&$~l%L{iOlpSsCV$s^SK?kL>2Bf3B%EX&s{hGh
z{E33~uZ3AuTff_G7J*vY^??)BE;mbJC6{rENA0s4$DS{hy2PF|+IC5IKWMLyyDxmG
zkgbmEYuRqXgq_e#VXl^3b<jU6Q~oJ$=-lr{-Lz5OAev>Q(Rl@DLiu~lMmdeH!o^p`
z-Df!VcBM+|@$*J1$wuw{VQ<VIX;iIT&`tdgF!Br`N&K~YUrTl_v7X~}CjPYq3gKZ$
ziLV6vW*S4rQZCPok+in|Bvs53RrEhugB(|XGxX2bApCsKzr9<4|19a>J^k^Yyjy_X
zyY+{XL)RdzAOB{EdG%`=SJvQvDa7RY<4j2m4G!3g56~BhSGJl^k%4|<@ci*M<X=4J
zhW*40TJoRQ=l<hPuz&R&c7oHtuFv(y?I!=CJ{LH&78bPs%jcI??_WN@46*;!^Z(8G
zIsf=R^uMUj3Ay~P^}l)!j<5ZX`p*ly#0@4Na19IffAhaT)r$A8Y3Jr-W8tIb279A`
z!qYIy3GTar-m?fUWC17jvR|os`MXC94Ia+FKe+U(-~k`F?uMF!7o5aQ4Nt@Q|6>}S
z^B+@oHV$y9;U8Uub@V^3@b5ppWPSPXYQ4WdxfBnq?Y}?yTh+fkx+1YZb&L}T1{|9P
zqgO=&7ZoQL2MajZ7$&x>E&k`ya6VTlY<yR`_2<%rutVqiKS!Z@0xl}NLisqMhiG&2
z@PeVjDLjXl51chk&C3ndH(nk#pdozh;ASIgUUpunp74U3JgND>(cd5kR+0-Q4hRPq
zG@k>?jfa~PP<A$Ow)h`~E?L7^T&BT|N8qAR9_~MzxojB5l%0bE+Va&b9xf<5_z2)K
z_{a3s95`!kF6d~kra8g&nIH!)OIY)lQi11lfz#A4TLIT0v0YZZS{wo^!o|xD3UYzt
zd||08^Yy2x<pI(I(%|8NQcg~wG`w8kK2#tdSf2pppawtV0P_NsHaj&J7a!;uHv}es
znB*a_70L&0Qw4Lu0Z!ru^5=l+0K6_c8#D*-hfqV{iw~3sX+YVjx#4vIA8ybtuo^(H
z+^}aHm#TeL&HuJ0sJXywa#3@0aY2U&<jw^s7tHc-KyyHgVA$YS%uovLIXCDb;0m;h
z2h@e-0PO;H37mP(L(K<0>mBrv4U~qCpBv~CA2skvpbi%Vsw-ST3xRe*Eg0a%4s8$g
zo&!h?0>;A)#tbOn#qpPzIWM6JFk>8`@hcDhbS1nT5a=j?`2+g=4{3PLe@xlgAYi~(
zcwe54h@rs^TOV-B3wjB36-GI@!1?mj96T^P4Y$_+f<nMb0wV`A36&xzpgdqDasJH`
z^8zEo3sw@)L%7w0v*!d?jRAqc81n)KP?16#gr|9UplWn!tAH5)zp!NR9>V*~1qK1G
z1c6Zqpd8fjp#ut2WT3)a&;`i@N^$c-4G6px5129#Ie}9&xnM?z2dE8nd4Owofnfmm
z!BWFZ@Bl>u^|^U~;4jk<sDg3xfD&v_#e`lz%zN1?9}hI=&mKTq14fITngekBL#BUA
zgC2vcU||$KO>p-MpSY`OPVi>r(g(m?^WRhf^aIQZOgVVjp$q)qbm*7mxPU?d1%ZtQ
zp1!2OyzySq&&w}s1O5E(4}<>y3gfPb`|?#75I4-o{15#f3H%!pxS~LRf^=@EiHG^s
zzf$ne|1pL6256Q4d)598rf{zV>k_;)EaPwO{grd2@L!o%k?LQ$SBi3g8x>(6Apr9L
zLky$5zy$zT2BT0P&kZhn0frey;U!>0g*tuU0ATb=C*f^^-`_$J2h6u}12+ON7r++4
zJi|~DxZMV{#tp6+1?|8v4jkz~;Cw)=58;KnO?D187zhS{1{xqifiDDHVh&&&u;2jM
z!oVB^fE&;~zy=D!pza#N&H*SNFBBy40KW`;6NDE~9ssKT806If|1Et5;zJ=99C-bm
zUe5LvE&4N}<A99-PT`aGms-H8!5kCxwh10?7@B8?c8M2Ag%4mYUZ`UP;1;+|;BbMH
zg&8$2PAI12=7wTHfFq&9;f3N(K7cIQ0e*$zWw1X29XJ=55r7AQLx;O$FnADQ09?%l
z>Of0y!$3bDY+$^=?E>He;{|;Rq2`3m`BnOt)^Nc@&JF570K$YJM@|?=*yL~lhY#|>
ztN{vcrs3v+_Ma0rw_HG^pk+`F1Z_Ys08#_@5kbJz0`v*`21<i5K+!i8#c=@)3q_?+
z4FS6nAfFom1}M;nmV`nO7_j&oDEu>S{5RJAB^>?7k{m#@peW=odI?$CprD!?M&UU;
zpbXF~ZfGC?F4Tn%2&OgA#Q+8W(82J6a09Bd(2|^R_zbWIm_VSRU@`#chE5<ftbmuf
z<P76*nFjF<n07FoKyzWOT(%4234%M&IJ_e;3V~?`y!-IM{bTxS4xBL;42)e(gR{z^
z!-cz3SmW?om-ztv0Z0g11VaRn46JBqnJY`-O0W!773flju1F{a%ESQ`77Uw1z)S)m
zLSqt`mR_ZB)DOx-!9O(@Cuju%TZ*tXzy$-K(6|x+Q&?oi4dO6R24)DLi=PvwIp8VK
zQ!o|~4Fg>S@D{|!U@bw{32cm@U7-J*(D;vw2gV-&chDY)e*g!tQh>1ly#y-=)CJHM
z)*Egp@VzqLKZDJy%jT~|4>tvzKpO!1gc*lR>jCo@|GV^6b8^+Fz^4sPFFgs=q_DxQ
z9LyvG4dc5s6cC`PK-BOB1j=15D=>VZeXu?Oy9qWKpaO-NCU_pqFkEs6p%qw&Fc)xX
z@?j>74a^bTqF;W5TmQcX`oE_B`V40VH?ddKoN#jvb*QlB;59CDVQs;4pbHFuc(`x+
zvozelaDo|x23^oOg-;8t++VW>j!M1U&E$rU3`Tij5`j|=V2r_W|1gL6&t{2-6B<Zg
zdI-3ruDC*I4PKb>f=4@WDZ+VjvO_ohU?t!~fR=@&pf2GLE%`sKP4Hg8`*l^D{^kqe
zpa3Qb_^fk-?E|nJAkY!OOTu*yJ|%EIu-n6dKZb2k0eo?(l`siI;Jn~zU=M*?fWwMQ
zCj^x#oG;u({jE1J8_NN6QBdh!NnLTeTrf`DFkQY>t}E#)$_2~4I?G@u2|DBO9>P=y
z&ILC6u+s3)oUoBW%>&%bfOP?H0XE+dSUwjl4Wn0-?$3P@PFRO86#=$5x&I3k&IeAR
zXbM=^E0i1DP6*xig5_T>d8iGC?<4ZUJUI_+NIbxD0tW^BFVvwyjRW+qKH!4js1UkE
z07nlnL0v8u7+-k(%QnE&Tw*}jroq)k#m){Gf+nu4GYCwVu8QIx)9@S!s0H1h0EPi9
zZ-4~hTV!C@0ca7}>;j$_3<J773!52uqz)?sfjx!p3;<6Ga~tq7m-#Rb@H7No3(gbX
z3al2qJTJVJzjCh3?O*#+{}cEBzrAm8Wg7n4e)&)B2f%^NCEx*83TzC+w=J(=z)O1u
z_q8wzfo-0`F&YH;1pwl~jgP>W!pi{H2<HgLkWkM7g|N^)FFxSV0E~k=PndVU<OENH
z_~pvZC)ZzF9k5mSpLJYtllni7>;I(V`ooD`y08D9jth#rff7UAA1n%l`9bgj{J*l`
z{@g#|-~kvLwpgI4%SaF&5B{eplnaa}kRCJuf(Aoe0D*%D5k}#l76RT&Ln9vmbzxib
zJm75()Jecx1=xLnzNvvl#=sW=XazhO=o^TQ!3+V!51<T)XQ6u#upC$!IM9IrFb5Ve
z4AMbB%nnL|1qcCQ7>FL=un4rw2fk<l?*Wh#EVAVQS6#x#ekFA|5_n9@4O|Q;16&E<
zc_qyOd*1@D3(w~O?Sei7X9R#Icuxj`Q&<iU=s6f8ye>?(@HCuq!lH6cP#(Y&5cvTA
z4KE2cOF(ZyoDPMp(DWq*F7nC-_8+(ZVrW3UDHs;aiNh%`3|7M_2OONkp91>@Uc~(E
z5Bj<dDhjB0!IvvQ8l0*N_5E<b4oZU?ae)Xxj0crHJR8oG6DD777#M~JB~a!d(15;F
z;{_rB8x=5M!UYQl;1pT}&IJAp&g2sB{V#p{U;BFX^JQ%qe>jiJ9>Y;PjKatEHwr`y
zAQfzcaJgMVAULjv<#WP%&JE`Pi_YL%FMKdC2(NoduW*CDae&6pP-Ju2BCIYv&CYfS
zsA13s)))j9(R0Ffk>E1?BMkyS_^@CU_I?5mQ!gh8w$}n{h!Y0TVPb)m_}gT|h5^sv
zh0heM%wMIsVV^l+{on%cOhFtDRwXoSfVBWj2oDT3{$WtKz+Ms86_fJUZVuQS1M2%f
z^uLD$u1w+8uW-V@Bgsz9&c_SEV1<7I>z<39gZGb*0JZ&*q-N&^UgY0A(I;ph>2;dM
zK?COZM3M+U{xbF=a{)qovyn3HB6GRG>vUyn`noIg@x(Uc7YE8?8k&P=r$4ovUKNyn
zqP-WXl0w8*%zn-Px2ixNM<4rZL@#}>NG}r7YgkwvwpdtCUZ<tC83so+T(C5RWh1Up
zbeQGxT_oSp#~pAu4ZXf1;A-49tcr9u;XXoO$2AFvTuVVb7S=UdRMg-L`rpGgh+caM
zoq^XlY_Cb*GEG7v&k7C7i>r<G%bM{?y?B5~kHvrxNKUT(>U<mruMqKf9O<$KlKw7*
ztjBXIO6)ymgvStXLt)AC3wDu4^W{b3c4_&I>1ojn%Q;~qJ<G2$(nzn_ZMabA5h>jF
zyM$>Pw55^s_ww8?T7z(cZzP$S%x@MD-MghHN$lc8Jt?O1$`JjF{o(M8zB%$2H0KI5
zdTA-7Su>BH7m88m(TLcS6N3nBeeCB$qxB9K20uEEH#(2Ky&=!bj(=Jh{-nL8$0<WC
zET{Cbt9LLAHObO^H#(&*{axpGR~I7}P8pYXxbN%rhR}R5=&$VxDV{ob;&|^HmIdS3
z;uuD`-IKnEjt$`tP`!2im#P{<9tFkZxxf5y2c_}hQ%9cTi}-@~r9lCu-`#qLJAVH-
zED#TjH(OH9?Ocs_C-{2t{1~tCEG+gK1wt;?s<s+Q%r#+wYdwQKk>{+$Z5Cw>^~9tX
z1aA6Mb1zNxFxk$fkQWUMu}%*?c3$^5pdym5CAe!*Uc5Ne_SDlyCW$5GN5pccHT?P7
zE!WN0qvc{c<Ypm{4T>?lvn@4^`&2=<+b3JWj~zc+e)p|~98Y9CDagNjCznO?>{I+j
zH6y+8492F8K0*&i5yv$W(mGcZ=QGF6V{L(pyau=83k}8s%l(bT^Op)=e-EQ>M*Gwk
zAJR{_aaf%@*eIMj+0r9itcd=cPtEYS<}XB()Y;IcbGX%fc<~1Be0=+&;Nqfq`$7=$
zg6Hm2I+^x*BKq^Zt&7woO9R8DeajCapY5$rN$^PHLfDJW>$6Q7+=5E6dVX}YE??w7
z=3REf@^>%wn{zXLHBW6akHHnrW5dODrb2hNKrh&#@lg$F&YZmCyvp&~mq0A63!^o?
zT`Xon7Gj?eq?4iWx~A{s{O2xZZtR<Sedj#L_@-jFQtKtc{XSqPSy~ospdR$%Qhht_
zrKBn`MYcVwo<i8Q{Rs)C(zr&o^cEq|e0A!a-vILbT5W@NiW~kdcdboa7nB~%v)lI9
zYB#B-uk{3+M%Qs5E;gUzak^_w<GMJ89*a>?rd{;-pv4O9zI?V`|E=p<QuKuw739M?
z9;Un25h$Z@1U?R)_ec&byDc`oYjN|N5qoxl>talHnv5&kbkV0x_sDV=_w)zxA+A1R
z&#en2Azkhx)?#OIqh&+WW2?3E-owz?GbACzqC+%Saor=_Q}pI#T-OfzGodFgFCOed
zE*e7GhCwxQzEdQj?2SX*)6EsYaC7ApO19Br1y8Hak%UgG&VnxbL%iko_o=W?4w$!l
zb1$aqCy0fUn0oLmUX8beGWFE36rLqZT>Jjs-ZK8y_Jci<1V8(l$y<Rl@?o#W@*LAU
z0u8b_dedDcdM)QQi#_de)mAWvD6997u`TX9d_hj}p+!b5+SzbOD!ER1P$9k}#Y|mk
z(&~8_-@Gp*GP`vrwEE>y;1gHIusd(UhxV9hD?*c_eO*6bnMALx<}<X0wtSmYcsTal
z){L7(aa^zV=t*L&IT2wYVHQQyojAF>&0|OC#2bo1HGI4rU;O(*h6tzVmR9Q>L<m-W
z?|kL<V(LKD%&Yq7a~Q6<>Kq=t{F2(c#Z7m&BoNiSn@|cNNpVbp;_dvv`a&uefwTZq
z1%gFdFqiPRCHa02<Z8W0F%tSO-wK|E(Wd&l%V-6X$DsO|98;24r1yVG8Z%^77AL7J
zj9#sL?JsS1XI2-b$|k!+!97?9!YU%J%5yz_;bpt0jUQ(W#=x&bin-g*V|$lo6vhvV
zsM;5Cr_9L<C--;yZ(7{Y)TVm#Y+vVV#fm^Zt=v!INM|*6;<Zten|75=15?h?H+vt+
zk&iEsb&OFl1(|nn&3<n9@!U?^z#Mn>)|<yO`wEH>ekvzwE#JNYpRAbsrJ2<M9m`|O
z60X;|#0vxmv4jPpb7SU$ZGDYN)~oBLY5TY^c0=P*>#ND%<CJeZ1Wz(T2&81WAK~`~
zy(tyl3&?t%ftW>0!`&s1dr0tz&J5Sv0^iSB|Ba<uKORzBEF1EjY#bUP+Oe|SpUrQ@
z+hSOjt_zNHF3R4JR6Bkb_)dY`+i;nUPL_^sr+qWcINK>fw9*$>dn$3+fTff5u4dg`
z!w?_Cb^QJhn4-@+R3f>mZn-Xth#8W%DviGPn5dQXiW9hXYei)`OYes?%gbm(R+q7a
zdb?gcT4Tz{z4nnir|hyadNC?IE(h6and4-F3EihjY=&8NM)KX(-?d8ZBBUR*KB=*?
z&X%^v6{p081oie%{^s6yeR5orYG0k9lOlfCki}k5LC&hk-+61~S=Ye6@^7!T=;*26
z*|NsV&mnIMOW6nqTp0?R10PWb9-0<Pj0PQPJ)}IDG4~=lNk15IC~A)?8>hNoGk+48
zC;Q?4$m3mavf!`@719#U!0mNxV-j$cp;}ddQFnC2=Y8ULL+?kuUx`T(5AF==uf!y2
z-gtgvV$^F^91&YT-i`*VG_dal#SUXPzVEx26&zJPBQM{z{|b``J)*mVp-Aj4e$Al3
zY3BC&5Ail^{<7fZssSgLo7IY2u2u}1J_9H(aegsvnVP(7B{U@%<cKaJF<H*@&+}<s
zQ&W23j~;w{9(u37#V~kaR}~BL$(~10zIL&$(>}Sq{Gof_3OCN1M9P%0$q5y{j2icv
zk}nO{)RbN;Z*9ri{32?7xNzbTa{KNb?K}9DvXVAsq;-;#IM@1K7imVE9V*zEYw8yg
z^O8N!UJwmA>-2m?i;VUxUNyAm`gLC1eLd=v2*n{+wB^r*Uj?&lQ9u0{cN3YI59<&Q
zE%3}RsN!NUxX#<4Wuv-1;bs+r!|zQiRR5`MC+cNZVvx<vm!HC^com|avqa4z<XJZn
z4Vf$+-`2AbG^D0*-7vbqx>eXnEE*b^D@wLa7{_&?#>(Pp>Lc=nO6S87*+oTER}B||
z3P1Qc)02gKr6Iv=X@><Yq5WKum7hn1&R9olPIH;DBl~nj@(+Z+P$y3wVrM6Db3E_Z
zRUwmiT${~7PWC(Keu`9x`Kw+b(}%tRUkf4j@stYj0`Hbt9cSOX*Hmg_<v~PkVWWB0
z(K63#(d#2!ItZ@!QFya{l3foSu}E5&*8cX8mVvV3Q2%HBtVW-!mOe*TJjUrci@Es#
zfynl+)%;@4kk^Y9=r*&Wft};+!ge+9!F4|Z8yOG7<OZE@FzS1B^!-TCLJzjkCg8yu
zwoN|%`RmsZmvg2?^EM0X+g9nQ7b8KTw`296iY-V<d*o>iOq9zw#}qO4At2c_`#nY5
zxr3%`D^WsE>sa=N6@eiZ$Fo7Rc^I6%4#94h8S_Uk4tLZG;7ofMKwX<%xSX`Hw5LZH
z(PZxYHj(ps$*yaBoIxVwvn3IxLKgN<(frBrmeDS3r=CV)b^*-Fa5D%xp;*=P$7DYV
zam=vKFlb(o-yC*Gl3a@;^m0JC-tp_4Gqs(+%4qXTDw|JJC+-rav464DTwQco#_e<^
zvo0pAWtnK4E<D0|!?)Zcxe3=xnlZ$tB`~M7(vaI`Gnw`<HjVkTyt|%Gg>`&n=peey
zlxa+BF;nP>P9{uoR3B&{67fal_D$Z&xTqMurtYvb^uq=O7S@3W262lgx8vQuI@XrI
z#5X&cBRN<_U)IU=eTuLt_2G%E_)gmLWPIVra;vSDV(Cdn-AMk|Pr#{>IXyq*7?RPY
zg#0*V#~-vVlmGVbyr{^3Rkr<Ah;F_16M2Q&F1lUEJwGvHac2kZ4(w^^e#C>CU-QfA
zsQ1WOdnJXEh{n5gw2sr3&uhl(-Z@o}lile=d{;F*6;yvdS@KCycuSMtHrTVI!BdJ{
zMsOlMP`Oi$0--ulLDypqa|H2uCFa@SmwTW6_-+adMe)-pyeBLo_Iy;nFDJgpHCrnX
zEVEa#pwpQ8^cSv8W994-goYuHg>pt7<^3#~T=R;_GY77>W_>-c>5sPKXAQXL@sc>e
zuapV~ooS8N<2dJA(VKtun}@ugqzd~%ZE1Lr(A&<4W$}#c<<7Jde|Dy7&Wa3gCEsT(
zM@Hd8ZcFipJ4;D;{t;?Flz*QW*Q88FCw$<x(%<j=Ih|<Yf`hu7I%7tnDNUWS^>gUS
zqhArhW{lc(bqS+M_&AcGBviVRH3QKczaF9SJ;bfV+7(hNpBWSonck)M7JJs2i}+Oc
z7dRiLDGQHM49x&3Pgn<erEM&bDPbu;A=7Q+KK{c6y|;*miv!<%t(jXxB&~|)YwYYr
z3YXT-45>Uh?Q9p#!*mpdmSiCh3Vx?72R&9Kd^1Kn#-rGoF*!}N5g=I-`c;S~lg!NA
zFf6z7K^Yyfh<q0sy>4gY7zQ`jy%D{vn5dlB6VBtygI`VVbF`B^2-TU?mh39R$j~i7
zUnCpkc1^Ai)Qu9Ei1SRn6PnM+qc5|zk-1%*m_zgQtIT%D4ZmlV6%ohpld8?$2@*Zw
zs@-{WD}`9j0S6J82yvF;(`%0TleDS^W+neA9ks`s<`VPjF;uiZ6bL6V^^aa7ZU_As
zdzLoIi=8r?oScp*YSH}hv5s2!6fRow*X_q)_EK_ek2w*n4rLxK%8jrQjG4W-{kvQD
zm3@*x5Q%5EQ7GZDXU48LQzW}C`Uq;~tuR~Tjm;Y7!`XB4F9k&!Gx!DV+Qcm-7~mSr
zueK$mjz5lAJtp?Q59JXD79$R&i#9pj$u~97e^!Y*@p{o#tZLR{@Uf6?)x3U;r3NeN
zM>7*%2v;;|tk-O2Q6)bPPF4F%Fme=eP(+{Y5j!?wZKMyDPR~MXfHs|$sJ@t^{Hq%l
zIHnd;FKi`0yJB-_SbSi7x}3BlmpnHe{efq@R92&hx+jF(+dA2cm7DjTN#~uOg$X~2
zNMg@B%-J%AxJ9`mA&LieqaJDv>yCjhNZiBoX_S^JF>8<S6I7{Au--XBCUkL1Jy$n<
zzrxs(rV~+!`dyktD^K5<>D7z8snPZNi07H|_>XTWC1s2`jg!A%6(Q^*uSojhSer!f
ztY>5)tA+=se=dSY*D7-$)k@c5(&uLAy%x+q?%a#KiJPxbF>kdcO&L2zCZHirb@G$6
z-^~rkeN^(I;>PA{@@JSE-ZMc)5L(F|?K`No!G(TzEXw_t<G)qJQmrvP@{-(7=~sEZ
z#Kw6u$b!J(Ti<UwBrWQ2fpe1*p8CP+_PX0i`7|$1%G&j>tv?fdc5J&#-&40E+NX6p
zZ;-}La>6hs;+gDNP)Ioc9OA-EA|DTX3Em!IE=l|*G41(%btyH5*%Z>o2)Ucvy!d24
z%6|xmwxuh5YyODw`7u6PoEKh^H)kOG$@kIgW!PE-nwv$apF{e#^=>RH5g|P3CD2j9
z2|v3pD|(w`J`~F=Qp<_Ru)KN2@!7ZLdo3gbOP%O+(R&J>NTK|8b4XhpX{7Q^GnQdt
zbB?L*cLyR}gzQEJi^qCD6Z}@vHf|XU3u^JkM!T-4tRG9ny`%f##gSUycM&4r5g+mg
z{Odd_g%Kv}#y3+P8{Y;QrtMP9;=SN#+O2x5Hu1n69b4|i#fuYi-xp2m#D4AE?sIsR
znlZUAGZq=KoibioS42y&FpfIcGZ$~paw4y#d%+lA9;i>Go!XdxjKy=^Vyc>(BvIoa
zBYgL^OHj=h9pPh~rz)2S%Fu1m&1swOtASL#Z)!Bf4L)+B)29w8b%*s&_`QrbNq#AJ
z5W@Cs^P~B*?+K#64N#aDv#Wkhkcqv~_jxZvx6{J+q;5nw=Pt9-M`X37i`|kX5s&Pz
zjbc7-gvI9swKfXoo)~7URJ6H;jtOO0OGFN4ET2a<a*1tLg~QWuvh`^2tJcB>S!c^>
zeiMGsOc<1V$jKn%{#)8r-PExLmo$lIXDXyL;{eldg8tzcyD@s+NogC;CYgFJ2THo2
zxl13u7}9s4-1t%&!h*%Vut3j%#h&Z2*j$>%S+9yF*J#5XPwVc~$wWs*SC^j1PJ8eR
z*ZdTJh1lZOwX(3Wm(+ZF=&^W^moEjn6xMPZA+u4KYkBL|y_b&_!LLzw`%lIy5Ir|7
zMF~s?PKc;Cn4`xfD0V6P6INCjKYg_w@RTPZqMhZW_p-*JR1on?!oA^I6s3Amv;IlV
zm_9DNEW5Xzu75)NK8Fv*Ic18ALe0V1V9sQE))e0L)rQp$?uM!BUzAb&+l4EOSrA3`
zUkU}=G4RpOdcRE@pz%;?A^b*s{Cl%KZ*k6mp;RY*K|~%y^#MkrB4Ji{F@ED-gzN$S
zgB1n@qNmRJO`bNU?+0UZNtyB~;y=iqwjGOquC%ch8%Vu476_wLs^2BO@$#B8j-%Xm
z<Gjj%wJdF+YvEbpXTe8vhcPsRoJ1)xBrJVSg4O8v<40m2R?tXcrGBgR&XcWkYI--X
zDR6yv(M9CfQ+)0AyLib`>-rFd6+=_+sSKUSmyQOlB$0`q&hDjfY1I)qwISL^oXMmX
zAgb7e4zBEBRO{rv)HU<w_cRVTD0;R2$i-7u4V9c5IX7B*{~+kMU1iN~w@_Mp%j~E6
zrh$hTb>R?MrVk-mHL-?oTPR&pW5&6{J5m>rwHs&LxNco+(F^1>(@Jy?b|Tl6uzlys
zX6&-oEhecm9B!AH`jRGv*dZZ{&M-3fWMkK*E>9n)Az;t+czX4H)YcPH5jUfi3Bi{h
z5M>>0B3JyM9$zWK`l)tJjX(VmtxD;h8Y=tw41JuuPF#@SZ9j(F>(X1BcURMs-I?l2
zi?Q19>u3{k^lsQ;2&?cQC<O}X-n<C^_HoeXzK0{y60758Sq6<Lo#yjgw<t7~Iuv1b
zSG9?U3t4kpwZ2rU@%<U=k|i4hTdzGC@6HJKt5-0rk<EG6ZyEKgf7H9c$n*^r@x=0!
ztI?Yl>8l%Mp=x5oQQbMqb}6QG#2`6RbkRMbX@0M{w2+?ap0b|dxuQtRi;W_1kum+Y
zHM?cbkK-KGUO^;$L*#8KVxrinKYy$KWXC<tfard4p|VF}rS4K|%wmg}od)WbZ8=XL
z&%aJBjI<bT8Ig$FAVZF)D2cs!K=Ne<t<c(l<%r=NZJC>nlGdoJvh`8HQ_r)7xg)PC
znWEr*g)t%q)TGbPA9u~=)mseV8kR^<Xj%?al14gJeRiC#v3*AG*kSKTLvQnP1`oBF
z_k*Ne<O5G}DwC;s&193UkY9%Z@5X`~(X8u9-f-P?$-cw?DnIG1h@yFax4_O~r6sGp
z$~s-s5U-ixstSJ6(+(V0=jI2rD$jX#`^6o)+#g=cwa(e|Ai8ia7?Q*^J$3o=Xl6X*
zt6c7<ab$)adY<%e#DzX;&dQcL+a)f&X{?O7D}@{Pd|9oKm2Q`j1+$0MEYb>EN|TTg
z=1>!gTDPP;No9OL#JxfKk*JFXSNsz%k)C=egSy=D?S$yHoV>?Ra0%IoqwR;;yX`Tp
z$vf_<rmADsiiBAT{KB=cEwI*RJ7lWjzF|G3kJ4d@)panj&)-c?s&JEV`7RseetEQM
ztVsFsoe~tkvR=0e$}YR)X|DIASZ=*aBn2q3s{Ov_*<;7?`)|@Xq@M5cmDeymRC8VY
z*2Iv;nc3NN7pE2Fc7z4Vm;~$aVe5YN1|@5Z=~qQsB`pHKqp?87mpYGg5Z!NN`%l`|
z(4ogkSSDn=Y_`B)xZoyqG7hZ{Ks>T?#}7H6{BBJ{CnLaEqj?eJnJE%-CNZ{4c-Yuf
zkzNwr^Smij^3jWvri}fq`)|&SI-52*9?P$T)7B`D1^vp&aLZa*jSv}1ZqhxU?<d3*
z#qf{!A|oz*gK}-2v71NYW{t#N)_UhzpHpoj-_47{v!w;TLsOMnqdPsZPYr(TmMU>D
zki9}VWcbqHgfE(*O~2mwCR1SmOPkxe7>CX6F+<ciH)KJzk}!v4FXvP8(#%q?DgH@I
z0%O0if1C-etYNFTX=|m4xDz^=5IMJ7mlU3Li)x+<q_42Xqw~{BoMA9E)%_$v9(H_D
z&RhM(wu!7R8Vl2Jt8~BIw{e)P8%kp47IY0VB=FPlC2O-pes*C&xMP2Wp3*gB-mghI
z%7V&ni)q&x=g^EXPtovJTY5HeQ|gV7sR{vuHV)BGnxR}47volxcBGs73Cnlb%_FTB
zK5e}y*2E95M{Syj9zHR*dpwmt{-R9yzLBaxZ$I+0YE_@ZY_*pS$aEr#8P~TZ7It@b
z6IAE#X*^9G?INU*-(M3Ml|x=q8a!&T#i1V|SrPU2TAR$=!{ob8*Tzp4=s_eREl;Op
z`Tfc8l9f~zW?vf513Imz+rs^#p7>j(<@YK=GWAFtM!Fw#be?=F$vV&y%6RcY-Hxd<
zJo5<>73wwtHUq0nX%?ze-?zSTEfSU^yGC@~&o!lbD^8?z757QzD+3t<Gp)mQ>PL*8
z^A?;57vDtZda@o+136CnrZk{nJoaJumuQ?8K~vIK`8)LH5|~cOkdINCrv}__>L9OD
zD;n6qRcyGaEd0BQ6{Zo&-1^N0DN!W;CIP3fi!1m}^sKZs)Kgk2inok+Db3iBXmgrb
zQ?jnd1&`k^>&RB?GIXcIz?yL~Cy?Xl7Qv1&iqT$&jHx0lVquXX_Nv&@=u#t+bKe{&
zAzOT(GGXo*=eD}qxOhXg&v5eQ+?yuijwOqdnT-scLCm9C_t8|e2HVjmAtg~b1k!6+
z=mj`LLX+`b^BLmT1yqVeva`N&NAG=Zfb8=dm5!xUc0NG0dXPN%R&t!Bj8Pp8^T|$K
zb+Bu|E#n!iBmMN2i?c*y4x+$2_UogSc2+S(-hJPyV^uNsbKWlzX36-x!T)YvUb{JP
zltFUxLanR0PI$_8gvd-GU4JmXZ1S6fnll0&1NBl#&$&T~G?k9KlJPC6{duh8*0V&4
z5BH?K91(O6h9_#*;%sC#$1KCVD|-TOc)uYtBe8jP#FiC4?XR76G!@Oammnv}{?k5E
zH9?D`KE>|g+;z87+7|XfoF~23ZDk92MMK-h><YFrxUB2##cmr3d4XwcLN(#x=N!fY
zJN?e0YU~wp30(n7W)+QtX1`iQ27|+?o8zT4-}R}H^E@>o(rkKGF0K~R#z_Bod_YJh
z3>=SJJH{Gyih7z>VP&j)RL-@XHextGdz@WqCvA@^bsX_KJxe9f?6b1hv}W`;=H8*K
zC#oNoBVoM7SA#oieBZ*T*oR(ukdR9k`0s=xeOxDO?K%s4aq=T=vy<C{4lS>lriJ1|
z4r$9yt;4G?`AKz0>R!^$s5f?(#l=&+4^<AW$Qy!3MhOCYoW(6sqb$&jiP>*8D@T2I
zK%s4^+iKl)VV6I3;4uhrc=j|m_*Jpw^G2ulJ;I8F()YE3b^HwN<x4wL?zJF(c!@Wm
ziKK_K(vd;qvppf8^353WKC){|-~u`1g?k*|B4lSk1Ff)2qqeU|y`CId_Qu0{#_}@H
zTvoGY9`7aJtvIYX^V6mP^;{ciabI06<)6|fQG)AAY3F7L6c{^Q)5KNOabH?;e)*4l
zD=2Jy`^|&8D^dw5yx2vM+}AI=H3`y8b6s##;+kmmo$_~d6#mT<6A*#HI!cyWfZLe8
zi&rQ;Z@8LFXAI&G|M+fx9N8&1*JfB>kQW$Ni>6I05&!6E7uz($KGFM%AJy63HtL-k
zso&Udi169z`0<PrKCOR-W@;C?pVLZ_McniJ8>>yd6ZS?wg+6+f{QQHdtU7G8_(TZ<
z<a(^r?FZ}S8>Y1-2~|?jcmnY!rFN97rl?45%xt0e6i?L({AoH%bu=}0B52w~8~Y2T
zd(Q<(V<4@ct+GqHX(LNh#~Ya#H#}oT%GcTA6i)?jZhJN>X}T_m1n?5H?N#5njdopN
zjEDctNIkouXMk}wl5$P99POJABtzHl@VpM}ZF}4i&iF%zRPT}Ge34)L;RCy>m=0B1
z^SbM)Kg>nGZ+ulh>r2UJM{xSR<}0X=$VW@;{Cu0zihWf#K<86>l3%Q?#q&q!+lq2G
z;++KoADlxl<c2o$#CCW2IIyV}+`Ua)zIaYP^T|1Ejg?Ar#F}BFuwwXnUDW53a!EVG
zq>7O8qbQEC&EWX5sd?^{#A3XWd=w1yrh?;-%Lk`obsJXWWMgOu3}cKa1X;?cUnG_`
zPavnmyYs(k`B&_!QzG#4W3$hUe~5{luRM*vxwW^EW?ee@{c|_u)$KF`@ikga6J(E~
zVd4)cs8nu=iAk*39X+Sbokfpkl8lI)ztu|XE6os~g*Kb8-pw$J@9KESv!^I}(;$Mm
z?1p&fwc+gk!Q;CAuV3p5(s;PGB=Vp65&S;Zn0rlmgHh=7%TG^Y(4~BqxlB$q94w|e
z=0Bn{E_{EMN8#Hl`C9|C;wSZK!|{Xk_X1?rG*&;G8F$qld@>EQtvOzNai&;fxfQct
zw;+-m?I0E7#q>Khmq3u#OLI12=%s{LGaB1(Gkcui7Z?hoA{C$ezupQ&m6+Xf*7(7U
z`JuwQxlZ=_-H$?Z!fga^b~_TgS?X&o0_!Mm<0YE>D%V_5R+>tF-|0Jpiaw;BsjIrM
zhM!Y<#QZ)W>0NjCt1UA59e%2j8MTef+V38pl-|(1JV&2sUYi*TYVUfF2KIMK7tA$h
zw$$%DGQ@o8Xq9aLYlO^zvTHL&!_*gf+TmKvsC{FN;iIp|pZY!*s)W}0+$B}CQq0DX
zq8;<VCgtN&7I!V;;|$^q61*u_Afl!^AJF5@YEBgRv@upUQAYh|LbM!@Lz7U2sbgn~
z_w^@A^`oj^TWiSWI}4)y0?zW$jEKZ9Mto4s+KCrU&mS|BGyHn@T`k8u_#=ypuG9-_
zO!?}M6>+l9E>cra!WTz!JUgDMkA$$_w+?}n?vKCfYiY2i<T0bMvl=svK?pKaf2xss
zidFqRd9$ZZE0H$7gEqKNB*?FrKaIUZr$g7WsqRO%szv^<kEY@U5)}F(dLuk6is@--
zn!~a!gUX3bPmCgcEFWm(M7J<vTE#KxG%1$!d|SV%TdBc${@C%BuV7I&+YKx9;gDyd
zZ65Y#iMH)wp945=TMvG&|Gow;OHNQ{&s*#{oQZkF^3m$uQW~vcEMd~yo>T;4D%S8y
zrK;TZWpjKC?_vCN{|zr2S6>rZzm7~2^eL3Y%+&XgltsI=kQSj|ZzHf<zeyyR<c`a^
z3exwTAIkPARpFSQzUmBA^>e?&Ea%d_=T6YBF?--|=AqgAS!BC=i8e6BGC#=X{=m1p
z1NWc4_YRP099^+E897CE@$;S}%h1=bTxvYra2|`lcZa5AuAi9I+6*ZMuK@D<aVkp8
z*UV*?%z=AO3*Xh=-qhcA-I&3heP3IjIV*~9?wFf?7X`A`Mk0rt(}t%KSMwtOeLi;A
z+Xh{XugDuskLeRrcagc>ZH;eJVBL7q{!}uOqncwXli^rbQ2t2ImmcA%fPPiob(VMI
zM4bddysvPi$5&W?XXOq_2=$MZ<SB{OjXm0v$Zai{q<iBRQ9n{`I?_FHJzTWHe^uxU
zhIRUF??WWF)arvU?lj!ZLrEsQgNB0zh~A^&?nkWL;}v-?SlAbiKVW32-qtCR`Bd7e
z6mc(MI6tDfKXP7brH)f%{2n$7VVNy|_Dz>D-;v&`U|FoZjpTtmVq(vZLvvPMGY3tJ
z^orySIgvwwCSEIp&>_Mm|N73zUQJU&%4FKcBS%JE&r~F-nb-2tYZYkabB~qP9-pdn
zFlv5m*VFI{HXsSY+%pwpJed4oHgIyww4@e;fsk_#QrBXU;C|q4_&y53ZNL>v_D2v?
zYcDDq8tW)=@6LU_I)k2oSVKGm#_3>7A>3VQA)lb?W|8S5Nx@2%H{U%f{31DS{&>7^
zyv%Ho^YiR8S$OWN4-GF?Ci(ZZkF!#H&(4nu5xu%yQdADg3|>j^&u!UR=J7Nt3e8ei
zy&;BVr1&_f=RW_GHFMzg;^}idYeGqUne!uSDGX%Mc@}xnQjBgy!#=xa>qsjBw6&_Z
z*9e&<-W8Qin5VW{nkNrr=6CwT8<1%>#5{LRk+4=udb26bMQH@i&HU^$6x1*#Wis~U
zO6R<rPQEQLVlrCYJA2UbTyLs^V_)_tiKoG#(~AQ6n`)lt!Q-`E$FTA5h(=HKCvi3u
zM#SnD%LmJ*4K^J_4&o97U+hyve_oGTW8ji2n|S|BM-o$Lo*SdY%Jkd8>AhN<A#Ws+
zw?Tx}i!t#u0k&opv-xQ?RjhY^31XYP#klo>5ZP$pyX7od0b0o_BZbd6eKU#dJbED7
zk#=O<&hn^tsz7)D+Q{z_HT$g^{`jXYD@F|`xwq**PY{GR6^%({Qy|Y~-1{c4kh4aS
z5hq6H7Q2i;LlhlzkjWh}{l%`JK4551RE87VF9YK?&UJ&xmxXf-xrEQUd$^;RX(}yG
zJ}6n4VbYr>BMjYBFl=UAV!H2N(Il&j7L6*ZVS0_yF5_Locx^=o-P0Y<M2b#c>^0ql
zHfN5_LR3;mMQeTgY#Fj2Iei(0v@Y%AHUk0)PCqzE?O%j56y2eE#lv-z;A8imL1@Ah
zAwHSn2Va4J6E%_-m4u_uJ2JMBB$)XT(9*+;sPo%i3I#2(=!MK$GQKXHlwc1Ei0xtu
zZmTb@>Pewd_jS4H5sUFa=icr$6?B{LkBiI|9occS&?jjT__R@*rQ14r5&WV%qjPR6
z4L|=JXWkqt5uTAafRv_=EoD2qKFE8`xuj34$jav@F$Irv76P+9%8!Jnk4|4{+rA-5
zYf<s4r~bB2UV>UhUY%<?Bkv-=^h6Bu4o4!6$<!ed-^G>dZ5!FcRG%;%zNwiULtf7A
z!=q5a+xJ@K-W&-cj_F23-ZD?}QfP};zRhh=gG8|YtCL7(!YnqV!?l($kUYr4D{Cy;
zF~4v~>=F5x$;&>|4&?J<PqWO}zP0UG)wf1>zAf%K$Ji+|Ro6x%_4tj7Gb^sh^oW}M
ze5lGsT7k-5fO~z;<FmyrX4FaEFU<3Z@u{_J-a#ez)LYwdAK0P^+tak1OxhOd565}z
zJg3#|HT%C9JICP2o`-EG+1R%2iEZ09H@0mX8`~S(wry@~Tbnn(=l`(k)yJ+gXXf;L
zIMr8m-`8DUea+k@sM0lCM_2)q`=Ej&v|oK7%(1%4@!@E{n`E(iGMT5b7u__H)Cr40
z4!u!^kS(6(gR*^vX-a;n{w<1LD6L4}&_eXXZo=&5WNmXecqr8>-GIYfwK^xQD}g*r
z``u19Ca~cunX)`+S~eTiQl64Qql>i#ys8=S#kOJosZw5?!$x}=XeH#{y(jXQ#w-3H
z62emr05(q~P7*9KX;9L)&`$a)H_J>z7--p;?qijmWDG+$g|bzT#4=@rCM=I*`nEX-
z=YITMdQ)6#SnKXBy)ml30hJ5UqPe*2lu}`F>`O<+iK?{Ct$UW4M!e_IG<4BD0f*p=
zP9l`Ln>%GIw}l@9bFPM^nAhxxkc;!j8{^Ej?cq0Kt>Dx9Vk1e_>u>l9p#=}2M(^c`
zyuFd>%*L{i;^&EDA;ke-JBX|-$YI8qizqtKXP{X7u<v~nL-f2NWHlA5H#`?TTH6JU
z)I(<ObtmAA0sDd`5y7{YH!{wJnSB@u_6!3EqzSXs(w2r5K|8BXGC>-CTR@^EX#cN8
z80|4xxGGonL=R1sU5T4DCKahpZ;k~si92LtQKWtq7!G0g4e8>(eQZYa_0t*#6l|W~
zo2ZlN;X<im69z2?(QljMj3*l8krlr$5rd>!$L1w4`zG~%fM#8EvL3g*jh@L~lZCwt
z8ixu=tk;<icUdCZ$f^>HUESG0EkOUaoF5^9zE_&2tGwP^$F>-nzYl_F*GtVhNzE`~
zNJYAYeSp*M8IK*stmpGI6u4$L8Q=*N?e0-4$miNc2o=bjIXB2v65~t~LywJf;bAOL
z=z%uUAH$KRidU6u$T%kmAqp(*upW&}d(2^%G~jof$dl4|=Xr6d3|!PoV>6L+DW3{p
zH}gqaS8X#*6y{z(xvV>UnA}@V*K=Zjy)?u9N!%3-39}u?!-w*kb!kJig3|(^+G4iX
zfrf-ky_@f+yh7%I^R2E#K5EXC7z4xbL+4kwr(&tkih~Lq#Q&LLPa>gh%}RmE^ja<(
zYp=fbX|A+Lmu7Vx6m)9@Wj2hki%I)>sm%~l*uH^1(Mx?_W2M#3E2Ief$xi3R!-)W;
zh?NVz%0xCoK%G^cylg;!yT2OKY!8ljz(3M6^P+y0Ow{g1BvXyKg=BF=2?3f2J&kA?
zNdH24VleC9`MoT@`Z^iZd`@FRB>=-%2l*`jt;;p)*5B0)RAR6?d}TxcvjqJ3cW5Qi
zmYnvzFH1GY)nNB{UIoST$=PuYT!k7CU`@5amZ6el_f%|;IP$(w;Ag({ZN7aqW1fi;
z?5reC32QaAIa|cnpASYe4``pCGuN9{BCBd>5$p+X=t|IaN-S3<I^Oy#&ZoDHE6=0J
z^W&3)+!<;!pU-z17^c#=Pjmljef<KJDhjIf8S@g?U8HoKrSw6Co2y(4dF7yn#8d0F
z5uLQDHZeNUSqJ0hQ?ETB_yCF!MchY_&lz^`d8Fu;QOaB6NHc^(DpSxf4TL$Ex6|se
zqQ-Xscvzi*%VQBGGIAiA2LAW8<OyP~P#=`KI}YWoNW>nYzt~#9Vs6`Op0DB5s(bTB
zJ?5<g1`O;IdO^hlrynozB~Jjg-d}28y(07H=JJ*xYWvBHxp!`GUa+&-U1t329Cip>
zo(h;x-<)J9Bu;ebR1v0XJ7~#2+~a+hg}v)(L-hgsd_s%n+QeMRo+M~w9BC5n$x1s8
zbAKl4@+XAX+T4dG<XA&1qV-*Y0w)w>?(U8i<e%sF*KtSr5cCy;yBrQGvoxZ-^HZR-
z!t9(#`HD(P$vv}}LSuI=J=oPkz>z#SknT4`jUzX3uOlhs46>8i{w9Y*VSX3^YjITE
zFY^_xrCBSTlNDw{GJEJKNrO5`q1COL?rVu_wc}*o{MZvNF@2yptsU}KyA^93`P7<9
zG?{5n=F~!%DJK0W+jJ*i_GAWg8DpT|zD$IQu_7wU^})RxN#ZhdMqqdo+2xTUumiyx
zWN3sGE)A>(*?Nz;{poW}dh)i#yvpO4BDh)a3z=c7mi70_!Ehm3c`S9CV|wQ*{w%0I
zek#{AZy6C_qYyEOb=F1;cPoMKmrx;9Kg66fwR*<C@3MG-z%aI9!*Hsu*nJ&t&=<E@
zof}y}^YVFcON9#}JMyN@Zv^CD1TUQW%$7zT9Oce)qiJ(&#j$B=qlL(gT&z%02Mpzq
zT+^j*Fioa$M!U^HEo@`KqIsR-Aa@W@(>jOn>w1o)4XGubDt3l;x6^6vyvezC&|Da5
zJR)N&K%DS84Os|G%F5mnK3GNLSu$f=GQtCk5npPaY@7}aqm;!c<mS(x!WkE6-w*Eg
z=6tR`q@;;Nv(-{NRreI`)5_=%O4%g!*<^fhqpdd8j(i6##q#-=Uxg(MGT=<eZD!#2
zNaoa8obc!&<3lyM6u&O~o)N#0TmPz24=N@9H1E&~J$0JVKj6|Qy$~`8P~i){)lg11
zNm9lgIv+C05YfRnHaK9z{~H{I<vBiS-lj6VupK)oixcq+^=eu6OeL{$AbF@4UQ?ni
z8L!O>>Ow-SLmK4z<3kErk|58x9GsCtam%-KHiExN>EO{|s1oK?=SpkJ|Hz$20RnxP
z*n#H+H+Sk?PQNll2sAWJIFF_Hr&3(fDaG^8w8K?3nL-{6{j~v5edid7w*7NR6wM&|
zOJ#ur>mXpL!n7gRM$&Ks;~b@N*>bS>sT8&|-mfBi#zJQh&qM7Efx?HsU1uUwca>be
ze!YH1EXh4<avvgt@TYs6f}jp`uek87n=MwBmMa)qxN+$WL&-Y?B=x<J=c44WnA@%}
z9NhBHIiy#-1$(pQU%$ZeuYCVdzUHh+IDZ7?mxjyf!U9y%QN1(v1oeWTH$3X9iHRFG
zU}1Xp>vyUsp3{T4?Q%3Lt@mM=opU)P>$I3qrhdgNh3IOz)smFh98P(>rxdxbD*gO%
zr8W5jq~xcZK;U|JP&Ly>r*iV3D#%*^%nXvprbe=2D=<e&#2*>_uqDH!tSk5%xznM)
zBT&2>lA6wL74X8>*VjOcO>{rO7EFgqtR94cc5-IVs=8__kEH~p6iA~dW_~wwr?c{M
zT^{>%VLtrH!MgK7Gs~Aw$Y7~`(EB;|Gc(Em4IItz{PRNP`!pANwzMGxPZH+|d}lh;
zR=BVhp*vSQGiQPyOqM9!2|mNv{WC@yrsVyts+3tBPDWh-==~&0KMHz5%`qYowS?ZR
zCr%ZPowc-5(JGsEvpBRz+f4c!Kv*61ef;F*LsC(Tb(Omex3eg3xBqZkvs-&TE-_}@
zL49m8HN6E{Rjr=!m+m<OM-T`D6J4mbtD|-3a3fZX=RzqWD#c1Mp2|`-VU6YVj2*3Q
z1#cC43Qq<olw<$EAuqRSf`@DUk$uOwkm=hkhYRp=Apa;!>aU_kt}5Ubnc&fn-G#bW
zB^E4Ot;p6z<jvA<!7OZY9U~<9-6$#qUC<*`iN1vr-SG5~8n}}6WSGNhR3*Lxp(dm<
ze?yXn+G2z4+H#kv7UOc;P8M!4ObDOCEix^az;8!;#?RIp$FCZvKI{F^Tzh`|zM|+}
zmGHz+4BKuf17d{&)itPQ-MSzlKjL!L`QE*H<BE!`C;iV#$LvT1O3Px^))|DWBIS`f
zo>cV%_(>*oV1*>qJQ5vh$WX2vcw&0`^laE(ZZ5|X8^P2k)P(5!;4V*$K@J9Tdqq1F
zr>vkSpg$<l>Cj<&PWHI#e*8@->Y{;Bl$%lOppU8EhQZmX8vC_K$0CS~Ij6d%BlK1V
z9U$@<Y6AH`M-*geFUWa@gn7nJm0NjcelU?&yam6B|7ntP(b849%nG^I*G>`u)q(g^
zy-Z|o`WcDC+-NUIn+CX9<a9d5J|yOvD|RKcBYT@1BH@cQ!jn$mVP|$byW^gxUeW}E
za2W-o@}*$5k&(jkM4FfQZ1B&O6p4q5n_ulK|3M>km47YNf@o)B2ZQOK10^hM<}31p
z#rqceQ`Q^#N3`8<raeN{Ei=CzsZR<5%PKwjb-C*0E%rN>ro48ZSEs`^32F2r65j-v
z&yE3|W;t?6`Gf=fmYoVEB#r$abedH_81yM#`xR;%JVxedtlbaDlEo=IGa^?k$U2VZ
zx6%;uPmIuE?VyXe?w{YAeqrIddW5M-hR#hIbUt|#xMvj+88g$$i92<T=h>73cSH>9
z3izo$Yy|goLtUhKMth|@R?$yroN5Q~Grtw<lEwdYR0-)-FZj-d>a2eszbo{j%r-w~
z$qmEj8Y>Kdp^kuhBdRm5Wvy$2xvG?aSA?KB%Y|DV?`6zcj&-xMPU$LZ6`2EKV#!n#
zUiC?%d|XF&1!})Fjg?wXuJXl+2~pQLx8Yo>8jh}Ku|juzp#rw2=Tq6Bo=|RQD00T>
z=vaK>g4^&aj1vr9pi?eo;i5SK^=!>z<C@M~8PoR+P-+Us0ZhE6{js8D?8M)?2-`YI
zX11*lTzF~)Qub4PZR+q}zyHY}Y=-tu{V_upP#ldcG=FL;l)epS4Cff4)`q>(<0#wa
zWZLCedr}VqMfHaWJ@dMT2NhdZRn1HT$1lSThGB^^VIz#RMgnWstwC_GPN^{?4kN7J
z1=F(`Su%VWi;9@?!h`6AHg^Tm@V6nqo{kwKPCjG4XSkUr`-|i~#Rd0_w>YrdCJvR5
ztaTq{=a92>{wmIckkR)kZSrV6psq+QP5&6$1LOn4Z`O@-Fq?NaP+N8Hdd?gq>Zcg|
zj=Nx|86#QTuh35<BTHhyy0k}3Ag6I79M45pmfeL7R^=0iXF8>Ghp?biY4KtUIigt1
z{8oEV082BsnuL8`T!KtWvAlreB!ATX8S{qpGnJ|(AH$#<{&`G{15Z%q>=7vX@k~Q3
zzUmSL?!HosprdI{*=+1olyq>|b5_1Fr3WAl{Q#`pQwVW5+A%Rqh?q?*DcXh-e`-zK
zLYylBN;Hh)pL-X3&O3RYL&%SHszquEA$_P|E~oozxI<sJvJE80eVc%RYihxsl3cZw
z+6o$eL1`yFSFwtZ6SDJY-755b@nQ`cd>3Nr2^ry+z%3Zw__epJZ>y~MmGuEvFK%Ld
zqUaShHHrkM#KGK;>{?QBP(~>>oJ=?Q5>Vwm)@sa~J%#-ZgOS~Z@3$+~m*-5uBhK;T
zJ%#v<)DA@0-+l46l<C0>eI>-Agg%_}%Udgc!Uandu=npMOKXq2V4I@R&IDCFu>fE4
zjA_(-vLf|M30s8r(GdG@MIVG<{?Vze^^YQTE41SWQckb0@RAtCEw_zJfx{<Rfj_8x
zaI=jMbQ?@eYLCxEn&46QY+6uQGFdvPFR0pb723f+w)ND{2bu3p%Lv<QZ{P0pSU9o@
z@*rsoN-q~ZU#f7Wr-0Ki)#S1iMCMcDpT^HZ=6_`C)RYP&%+vUa{n&`S>`EhD#<2sK
z(^ljWjDq@y{bCr%U=S{bU>K|c+Jn@D$jSyuqu-X$t}XzGixsm4oEFhnhmHOo%U#`v
z*<jzmvvlz2E5(mU%o=;|R~0QV+3#ead0Blb3Uv;Q?#*eg7xZT}ykwc$p<JjzSvCtx
zpdD5XN(?+3xl(th4$|v2m~g19v~F5f$QP}c2(v);R@qZ#k4I!vYih`&Z@^HK-EhYl
zNMc1!)35MNE)^5#T)!8UDOTL<yi!%KG=LbS_0}XwSOp5Tj1qt0+d0cTeo^(EE7}R)
zBkfXH_LU5=Hb~R;SgT+b+XiEX*Jp#VI1-1rdB576%Ew^jlb3A5>_g3?lGEmgZW~R9
zC`X!sZCeUQi&nwT&SWlV@5og6WtW}+QZ!fR{-IJ6eN%~+00Qf-dCI^PJHFi?*IBa9
z*K{bA`!*{s`cQg|)$P57`r2rO&P7lE7be?|Y4B<a^Ibh<f!bjXO}E?btgh*n#a*gr
ziZN-V`>Al+?B3bR%gv)zuy23op0|^e<n~qeUT&f<*8zZ2$Mf#e5w(YW!&3l2!{50S
ze|NqPmVuaN?Z%Xf2#;UF+IU!&;AQ<>+zJZn#MMa5Cx6dh=FP&(;}|Xl7g2E(lJwy*
zW52^1YGlUo=Zfu{bEa9Neqg_$WyK3V=#s#b*s@VQTjqHOMU|McwVQax<cYD0_(aOP
zsU`yCX_@8SSG*(pQnoVImZ}w@WBw?sR>IaQ)$`35gEv^}w$XUgbg(rN#1S`+O!Q)w
zr`w$ym<gfMIg*pHJ{hA`2U%E9<prWAj;6o%D#@;(<5*h^8f7hu7!rBN&<;k36c}Pf
z2OpW)@oHq&3+Q2x?aPvf7-t!8yY;y_SkN+OS#n2qvR&xUgcj4}6|<yaZ|%eC0#w7x
z>VS#cszfMJg!;Sk-Lequoj3KoV~7B1bR9Z`;AO%e?Q@zG_iA=?!lz(3ylYW4ddlRM
z1&BhyFe<51qam*+>Ygkio<A^H^R#)Jx|Q|1;CwDQG?l{XJtvM22-AUO`oQX+v&1w4
zL^dEaj25I77z|A4eyXc=D`6!u2E298U_LJ+tsq0Nxl5=jYdD}F_lt-b)WQ0q_`5mc
z@}iFf-4XilCC5G7-sc?~bG)k!JOfF~d8kzP;{`u-$XzT6Y^80`6Q<VcI_z)#A%h!k
z7cS74W}eu3XUYS1ir|Y#^@+^N%lUhbHW}z$Pe+gOZ1(eS{jBrk=z62i;!N@nbD?PL
zg3Z~eSUO#9jRJ5u5GA>m%1M&`wcCuEq%~SR8n3#YGkwQDT~8GB%f>Nq3582%-b%N9
zmDX-E)fBV01|o!k(nwi@5Ue|x4^#2Tn<Jo6N;qh9kiDVa4|*ewW0({c^QdPH>UHz|
z1jG|~Qo}|)(pppG0*7dh454&LYQl;o4ihrH_jW>I*E7{w=swC!Rso?|DV0|6SteGy
zmt5XeM@&4KVAy59#3f}W*5D&gt)oA!otBY(Q2?5%kY9Cb=CaO;t{o#S)ykk_?@%Ab
zyygSzpD?UJD{>Oa2LVQxvOHrt;p@<?b4<xF#p@g(W2XDW#CPs^*k`73^NjC4k|PQ}
zf~;y-ETwc9j`jKXKR^OGhN=b?^LB=wRBchCLN5jmUs6o>4HS(ih8xMUxI8~FC2*Iu
zkUc?R%c*iQ_}gz#JL2b_?_sJ2sVtsm<u6;df7K02UGiT_-uT(SS)Mogot@CrGd+f&
zo7}{}^2W!3Ho+wAuK$Lj`U{Sc6@EX-0rrT=a&B);Osb?%$LccHBUG~4i^7rNuy4*<
z^(2YGhu(p5`~|$Pc<cJVDZ}`7X#ZF7;lIi-{^^$xF?BL_w6J%!bA$%`GnO;7G5v0f
zQ5F@^kf#xr6;>8z{EiokSsR);5wiU2BSJ!U?u6Rkx?nn{e^U#58w<ZBEV|GDK__EV
zTjzf==g<IQLwgBR3o~=)f4oP~fPa4GpKIUWCFyKvZDA~EYi4cw&rsRf)JE-F76A>=
z`1c!Fm>IwCH#c-tHgzVX1PB6z0KxzffG9v5APJBL$N*#k@&E;ZGC&2O3Qz}V05k!H
z03(1gzyx3l_z5rrn0wfpo7w^_09F8NfDOPFU<YsnI02jiE&x}68^9glLG`bhh*^9;
z0|(1LfommbK0d;K@5A`^ng6%{>OUjc-x`Vk2wH#JHJF*%|6}yu6WGj5?Eea2|9`aE
z#Hc`VsJy=;m|{s2*AF>VY5wdpupS}|ssosw$EF+O4o{|m2}=Zjzv_~Rg_3DQMl<@C
zex0ZlB)7VVDyml~yy6M9ynpe}Tt83VXK`M?`)*&pe|pD;mS>iBvxXqSt}~zD`oJRF
zfd7dA1pr{N1q$I3%o~A)ga=^uW_z1}ga#l?>WIL%E(j2LU`QPda_J&|kVL?8{TVF=
z{%(^qwhm=v6fOk*!GhfaZBjc4q^*Q10vs;Un;;0W1_{R#G-fQgr~tod{)je9Zuf(M
zBP(Ltk;?!NxSo(H0v=H^OMwC8N`ib2NFE#ugtL=C%3M3xkwb<Vm70qSTQbss1dFzO
ziH(;KdoLISpAg4HGyv7W!lE|>6}b8;RKy%eFCXz;doT&9KZ3kY0LXQgkV(R9fK!JH
z%mmopt2gNnY%7!C8++|NyFY%dBNq^OgeFZ(VwVbJ0e~WD)I<z4N#t12nHmvfPM~*b
z8*40L_XL=f%M?nYpbpEPk17&$PNI*&z{3^U!0r-0{tucux=E5IV$y(EJ<<AnVp%cM
z4d=!jmAjY-sKjwClJxzW5N$jx0S`ixXV0#)7z?lM6l}mra(uBUr4IRcCcLPlVBjS*
zq$^SPdl?}_Z^4{73JNZe7>EHZCu-ATf7A$QYET*(d#9*3Zc-np&>zuWcS_LUFGL4x
zB~(az4IKu)tK0wrI#BQ*3WQ7$kl?=&@ou}gF2IGm*BHPw6kz|vg<^#R&r0_+wFsn{
zpn_))zr*Wyn<m#pWmjbvUY$)_DdOOW!6HJqEq!}jK%-+E{28Y)!@ue#*MTePdoEl^
z1Zf673-JT+`+@9tT>TO3T;j^mEX`{m&2VmvAuzxv%@Jq+()$Ckpv#65q<v|EPf&dk
zyr-6bxB-87k$Ta`G{es734Ra&QGpcPsu402x8Z8wi-6N2zxCvI|6x}|5riI$d2vhb
zCF*0UD^WHqKR1q!H!rmF4YxrU@<(M47lzmUIeli+OJ|OV=WJY?Uo4N16)g5e10EHJ
z3-715dXr5TCZM6s-`rG~gq2K&gl!N5j2tDpM>NYY16!++hFGgbWiZJ!1dN(E{il}-
zsMClCvEP6w#daAJCa*G8XNMNozh1k+ATm#9g;rS@=&<dlAp<y2<SPP7tQdVJf3)~f
zy+&<lY6R#^w<W-jL4X0Alms81Y4nK7u!*WFmtK=b>^$xn#}(VlPv@W<lZeT}dnWz^
zkKS?jFj7pEKqfA?+2tIrsr(6p5c;P0C)K)XhDg}H^?JOJ(mX`b3m!bR49PjI&g9_F
z;HEG4peq%Q6e72R8?c;}%bOO<p^+_YwHH3B8I@3(y=8e{l%#~}z{qhlGF9U0q@WG^
z`ulhN4z5|`%_&d_&+7Gpfv3`JL>FtjR_4;f6N+^PUK-sc^2htnK6)Gn>pg)pInYv9
zat#N!wurT*Zr7mKS+-I<_1EgtU-(>wthaJCjb=~z4Fax#85yI}WUoq=Ty~M}Cen-|
z-h6dFGCH34Oxd?qvhM}&BcImA!P_y{A3tCxC?cHOK3C9bLo`#o%Ds(f^A^&<QP~f?
zIg1xA|7=4Tsmj!mR-dHec`Jx~<elt;@?MLYOgu3kC)-B`DpirR4&r#wAuFYq+mwiz
z$#@S{7oV4&J+7GQOL*B3ZYHXAY4`f7D{A5+cu)<O=Z;^r^J!NiJ-Xd&-JdV;Y65p{
z9iAeWo_jfO|1$q9+McBg4)UI-`AH2YHu)9Vwxf6Viu*N_r*{4BrYnU;R417kcP@pe
z#sKi?zHZvdwi)V*YSXXJ*`d+wT+}deF0Z>jydv?$!j$M5U_V!Bm~kQdkQ{Ct&B2z>
z&W#xw)P^)XJBA7qSGc9JDd(W$(i)guIKB(tIDMhq?TeKVTzj^De*sso@xp39*S^<v
zS&xQU$8LZ!(?0<{*}21H>$JC66+JWT#`-mHJ1z2>mh{oB$DBh8Nc6T&QMrTRrdZFG
z&&e#4zVHCp8(44p>YM$&eHV46VY9EJzFx_3$nHOfw^de{>|XT!Ti}nmKe?`C8{-@$
zqk(vQuM8+pxzx}^FQP*H>ss@N=3*aX)K<5*8rFs)k7BF+dO}&U?G(KxjDnqPGsC^+
z$dM_1QH``gy6nw$8dB-YbJ(ooI1_J5FVR?zz4K%yJse8c@3u1P1p~mk!nT&!P+++m
zF!dBv^^S0n=yZBS&v4Tz?pF~R<$Ho7lzZz+HPKozq_BalF;$q4%8Ik;jEw$)LYpbm
zp=YlXB1U19ZE{@{ZYHL*^O#-9f3vrLvMSp_`BfkC%HP9w+32*Yl=#GrcNyQm;GW5i
z<UPE-PkiaH$MxKss112-h9*P-akbppgrls)2w##pF3)58cd4q?LKu_Cc7iuV|Hh9@
z{7l7dQU;1AlN!<Za#HYj2Zv^M!<w6@W5w^J0w>8{yuaMl6b~pWpHDNvZz13wxIP`C
z6v_eVJ$THSy6W0~g9~1Tsh(i#7EdnOHxt|0Xe8uN1IM|baILNjfwW%jchgO(#EZ$~
z$+$ny9GK)Y6IL)c!1Sg}quFT|jlPmG=-3!vq&M1|@#^1d`_!OKaifRCrw;iZedkkk
zeHhE!B~Hq7_2kAGn^&)Wqoumq1dqPgo@9+=E6y|R-g#t0<}V))bM$NI?~VrU6cqa^
z_HCUczVn6WYYcA%A>*WebP;4`>ZT@V<*t@Jpz&vp+gf#9pZ%|M&)EyYEx~k4O{CJ?
z#qW>-RdteZN368eiSa6DPPHdF3K@5`i6CZv<wjxSseF{BR0eMMM_zE_E{6D-JU%p~
zj!!CEo~1COt1UbT#3CPyDvoa~?-?$qPWaOk4&$TQ)P*QKryDs2+fLsFqu9y}g*x}c
zQh>U^w%hOB{Uo?WsHqT|@C)F3v%Q$2fz+&mP!%?x)jt8L_6sU!?V0UJlXXitHC?tX
zD+g)TFSC5QBnd<<T^&PD7hG@UzonSvZ*B|gA-5ck8+M^Scn{h4a?lT|2(X$BjSJ^(
zbgX+!{*+q5H0;qNyo=OJur>G0k+-0iS7?2>%G7MYo%1Vwmhsd56*z~;)rSt2rp<|&
zh-v2Q=Q(w=Bwd4YX(NHPU08@(CllpshQRi_JJo9K$5;0KLqC>XXp7$Xm$s||(Ys4q
zPq(vBQRC29Z92wf>~jXBv|rz=0ma6bYeheMLAJ}77~RSw=!vN7ns4h}8A6wKXa$ep
zXj9JQfRm5uLu2Q;Oj(zxNmzqpZqKxx<FtN9!Ye<%zD~_9#wo;ct+=MAjiqE5pTBwE
zG2-Y$V*g6=VfDf$R`Qqc!;=HrE(7_!B9d8(JK++!;_HAcf0nbMANo`F5*XU&4cKPP
zZPnD6Vo6gBf+c)R(a6tNn~Li-M{zEW;V=fAA|P+J`*IXsA|wOtO4TfsmMD^VFQJI*
zG3TNWob+**9#uTvk_0gg{t-XYOaT2fmX$?{_8#L+Bo+6Iw{hCws52D(dWNXc-sSlO
z-QE3<;4%v{f)-;E`?1E#XKlzgj6LOFW}&!H$wPbaAE|VHS7^C+olZO!BVoQ`@HU0C
zb8B#yHO(VYd-s%(q$0-lenWA``BYhZ<8T>heztD&ce?y}97;1u9Q-m>R-1;{G~+(j
zPU-?Ba_cP!r3UVlm({7i_*}-{{!S}8er+?~B0<a7FW4b`#Ik#GHc;K0V5fgs%~p@i
z@q{67)w1oOrFc6|eCYU+G~GxyWwdq(8680u{e3SevfiD-EtpZ;LOJ6sf$Flsq0Dp<
z9kM*)laZ3z@J_@@8-FR@>J9Dipxs<6c|JV|j@0Rt36gWHM0s6^`dNz?mzshcU!fjT
z221DTzCw?}Fh+(B3yOLF(S~q@*!r{Xw~oW`mb9m0N74hkm>NrmZNbIe!|MY$n%HG>
zyWr9$;=bRz?}DiGb<vRMX-m{(v*l$*rp@<}DExK`w4S6^Hfjq&13u?*`jdv7=ZK8N
zb!N35xq1D`@Fdha2J;67VukMho7rDrRI|6;1s5T5aUep~cF08XTi(f+c{;nHyr~!S
zELrvnd+H%2*hmq(QSTu2D1D7-{K%v~OQ~n%3{XPAj;D0-i!g;!C%W->gk~ER;Jw*C
z9!F^;4(j=-H<`F`R^J&Pi?INZ9AOnI%7`49r5=Y(F>w-=T?}e2NygXlheRL=HPe5J
zyt6H$#w)x$yZjW~ZeP|LZ>Z!+p0CU$?j(BJKiyw4@d~{TkcwJDH26NH`(ZHr=+oHM
zR)2dj&(tlgbv!TL$5`ty4Z7iP*gI(-T`VNbtLuJ-HB7)W>m87kg>^q!=5DOX*1sU2
z7;t&~Z+MgS--W$|JUq~V{}*qn%ZZAJN`3RDq>9S_i8mR)d6W5{HiB;vA1B*?;pVsA
z?SJ6rKg0jPO*$r~Z?OFD#7XxLoc@bB**O0%%qjZKn92ZqfW!Yl#eW0C|A~tK?ymi}
zY5pfFemfNZ0}lTw72x=Oh<{!C@2JShz|8co=F@K)wXk+Jb^NYnv^I1$6)`ooGco-J
zQfMb<M^i%^X!rFPH!zijuV?moBIG+!QWHW#BF6R(ZA`MZK7U1=jg$Czg?4UI(fDN4
zQzb-F5iZiA1lt$+p2zR_#&Om$C%4)ArkDQn_Vf9$7Y<EQHD5~>94?4zXh&XK0FU<`
z5EvaDD<Oe@oV<TnTwHG2FEmhLZo#i*lm={}1WL?J(GNc&)OZC3>_TPGLPi0)Rv3Y;
zDJURRAfS+;0OCDwZy;YjzPg?O`yvhjqzm{4VDnp`#a(&@RK^Mr+})7iwY5S<kH5a4
zwc1WV{zgW`aef^?4zltk(pXo)^1#G4gtHm3s`})Bd|FvyBcI;l5HAv+%vpr8Qdb5B
zf*>xU_X0>Ri#YLr@EDkT`QxL4iM9*d0QFG*XzZJw?J2uOYyc5>Vpz-t_i3eD{}CMw
z!T?Q?#<~a=;^5#^7eEZ0b^+wWD(epqa&||daWCI%+^atX4CLYe75qN&u1=UXVP{s`
z#JY(E?0Os6whC-*DEIBxsV7HRNBP7Aa;<v>1Le;DgXvexjd=tbY5>#|2M<m;!3*rq
z2>Z1cRxj3%XpV{iee@7Ta$Uoa#zfP$O4*y8olkF$dRuc()gP~5C9~D#{$*KR2(vjb
zbZj?<eq?0)9u*pc6S&d~c60`=s{8>Hq=@uY(GXMsEaY843{oHjR7L}IW??h)P6Ip&
z1N><N`d7)A<K>m`52Qa`DEK16PnaQZ#P_`3D+M6zQ}D<8*PYO><nRCif4E6(IDdgl
z1Ny-)v5~U;X20>Yza)k^K+1fRFTlXwJ$hd}>Bi)f;8%MGkAM%iD32yMG*y*fZGQ_s
z;uQ7pub^&r(f>f$-^1Jj`+El#03l5V?f4D9mV)>-d{r%h4gBTW`id+&GJ=8lkh+Zm
zEAjSYcKEVlS^Zkaf$#MznGH1t$A&iea{D{=68uuhXxIC7P4{wn=ZEuGU-575{nrkK
zlB<*Bn~m8Q%8lP@-!ub7O%GyPfUB@^iZD#9CQ|BGD7N6&ra4_1+~m}2PnD%P!%xWr
zoePGbX*kq_U6c=H1YxQHo_w};hQ_AP+<^x-b_UKY19}Br4(O-zy*~Hb+s{<E2CwDV
zv<a|~)3sc32Iz+Hlby>?GGWGwr`%%&$GXsskM9gMks%Of1gIC1u^0#m6tK5{+F?_G
z=ieL;AV3@pZj1x40mnPA^{&L8ehfM~$j$ht(r1du3vg%omxPcJkly4xchJ`y&?_iV
zr|VaEKW6Jz%{h|tfFDs>c&#6iH;`Vbe(+zla42I`zir?za%8X@Vg?eBmRELI#H7uA
z{R9G(k^7bxf`_WDcnKLF-x<0RHou9UYYNIm6iZniPWYA-_bY|Z&eukz*y$iIQw3x@
z_Q%DvT(8>RK*X&o7Na)In%rNU#kXP}^)O}DyR+qV*Pa=?&zHSsZK1c>sec&R3-2=U
zNJw2jj$W2E0%xFj?>RJoEYlR4yh7{^joQ-chC<%45o6(W*!rk3?T9aBqd?(XtJM4w
zT@=n}(q5<5H?ve)h{Ce<SrKLzdJ~`#3S)_L?B*T;1&C&JNzBQaEyr7PlEglj5$d0O
zF0u>H4b9>sm0jUC-X)Qt%I1^KiPum{C>U}ixg9f=uKF-%&qEYAs=F?fbU5dbyf$ii
z?f825Eo4RT*2xZTQ_ry>w;uWo3k0O)C(4T+9o2L<rcc8g5B|RXCC4F%<yz9G!hN*u
zYi6&405f-!fqjmv*Xq0=p~#GP<}v+OgGTSH-P&WKCgo8$xx<Fib)j(FB+`_O{!Ato
zj(_7%Cq8;<WxDZGJR^y(@|?>1&v@s#=+*lYAY*7?U?n#V%eFqe?RA1K&=B$wzU{yj
z4+uKCJ8R`?G$eY(<b-V)R_5+gu$pPNfKGxP%~DlSYIiRV0RFq0JAH=L6W0LD=e8$1
zH;fIyyD&U9IO+#xgEX6#K3`nHu7zjchpXprnvaw3IaI<eTU~C>-wz0!&G6p*r2Wpm
ztpp?-XKt%!uATzf7!7jUb+WhnNnpk{u0uT^xg;4#%ky=wCD}^NK`CH3r|jdj&ZCs!
z*c}v|-COLA7usr-pcZPt2DDYdT9DD7k0!3YJM3itj3b9D@SMl!#3w*mqiwNYO+)Ep
z)DmTH_UU+SQXwRY(-@1JoDv4+=2{Pu`OO_&LYETF9*@&{NWg!13I}sE;}!S@b@mjK
zhShRy3|KKWb*A-yUfGrZrO4%gJA(qF;b!yljKfRZw9|v9`ioELf%{F8BO7hSCegXn
z<6ReRcGtqAcI%w`rAk0T+PLHmr?y5l%14Fq086|hO!B6>i3%rIcyGqim=>u<nR~C}
z_tW#^x$xcUP0p(#adn$GoAn4C(4Qy=={;L63PHPp#~@aWuAmS#@CnK?Yf(6g<`z3g
zokXPQQax*bl6H1FVLlapPkb^4RFwU+_f=wK<lMgt9X$z%GV=t)=2US#jSd)bjz<M%
zH@K}6tURKOo>A5lWz891C?Gk%-4it5KuU*{-;yrbmol7`#hl{j9Xf>D^>(EF^r~SJ
z4^7YD1U#u<E0a#@TW)1|3f>qfNTh#_-I$Qimc;(17w8vb01&81uu+lzbwAzAKFe<s
zgFT*X1x<aPB>){xKug{(iuLyblNUSYZ7=St=0)5^Ge}<d*v|v!)DDMzm{>{T_<aMn
zG0ZiV3y+;TS4bNRCpY3$nG!Zldvsu7B)zHI*GBV<+n4b<V{SIBBq-c2kP8TKv-qv!
zs4wC-rIvFS;LiukvauKDyEE-`elO%?l0?^874wqhkg_^oS`c!dl)dg@k>1;(JaH}1
zhT?OB#~b%BC+e_Q3xPE|tZTIGQ-C3B1^aNxvZ#l#0q6FSa~ztlUK)KLn3*OD@}xUA
zgJC`MxlXAbhQdiLEb8$1C>E2{d8H`zvtav{oAgwJ1pv;T(9paU%Q$KYt$CAxAGJ3{
zS3#+0A39TW16QB&+9f2j)9|`$EKf)InRrJQ5E5oIC$D*uB%H~u%YkE%7Bf~m6liT#
z1xvKYCL<8o&85)Yp%@G7>2RC(d~?D!&2X;`cmAj<ZL}wRIuVyj_)@C^O!2uD8uxju
z2L_ykfaG?oIS&hd+9wLU1gt3}|B?$GzF@Mn@F<y42Kx>qZ&wMobfvxr*`Q6_lqRpA
zCu|J@5@@z#ZWnr&sXfpeH4+HHuG9gWn_zLh)m1`sLC3M7%1U10Jq1r8qvyU^RlKx5
ziQzt}x$!u!I~sQ^o3Am~HIDn!(Egq~l3O9^(YZ$hcNf{0KgOUNPkKP=tnWDibm2$m
zP`}OJPr%C)-r@d$S9P{)5%1n{G&^ZeMTUs#(;v@vW#86wjM~b{sl>i4;Cv&annza^
z?*a%x>BWa_O*cyOlW|8<&E;-C+80+;{Ttd#`|h7d!-BEQ1LL-&7h{>_mc&m51jwHO
zzvLz0Fmq`L39WKX$>g}vY6tVqt;b6)kOn<ciOlNEM45tE6eTKCH-1mari2|5fufQ$
za|Mez>DQ*}9e28N?`QHxL^&SB0L&^7x!OM+4jwfxSD*@dsjoa_Q3guw2(<%;*GhhP
z+x~!GoR>0=T>@T`8Q!wVsH1Z!t?}H4Rich#P65&~lshI=ed9|_u&RlsFB6oXnIfYW
zfp;UnC*u+EcbHwtLQ*iyY3`Id-ooUl(49^Z$t=5D0;1H$86ThtrJLifbhR>;SmRD8
zE+gwT2vq-pGjf_)Ra<&4ME@fJ!=WimUp2m*+76E5{FQ}tj4^+2zGxig>a}<}-<x8H
zxku|um^wCHoi1#GH@(Dqpo#FY{T*TYrVaD`1h<c<tz>bV`A73_Rp4Z*9qT0yGg+H!
zX+tYTC75W(TDI$7$w?Hg-BdLTh9g$2`@Z9-9$$h_C7u*@)DXOPV-D7I11S0XA7+Hu
z;Uxq$4|(w<hv-ugizsnC*;YuF`4cx`&U=zO_{UfpN>*p%gG*1o?WI);-Q_hdA85_*
zznrDGPT@soW+h+7FMsg@PusyG#BepBvWu(`YdnejJU67D{nDTI!**Y@m+jyyPAB|U
zRjwXM*0*Cz>nb~zN)o(-!ZkJIoUd;sdAlfQxchB5IO5igj1~8@?p$kv%-oBA@&b`w
zvrC^m66}uw8Jh$`Nx}~w4!0k7WvvFKu-LJ}=kFXd?Ok9GDqUGuAvJ9QTvG6Mi@L+X
z6eEV@XjqLfuuoSV<jLY%#1mD^t4Y)>uKZPR2^df^JTP=z;_qXl`P{1y48o6-z`PxR
za3R4}$*?T-Vjw-REM7bzqH$@9KOVBng;7vBMmH+MFr|KZ$wSLD1q?5JW~$3pXj|6w
zKx^o>9qW<k#P@->G{MGfo8aDmv|9DY=t)cK)_7}AU1vYK0IocU)G+Icx*4!F&KFs&
z9`O^q-;ilap`>k+Jk$CTNzN_S`_jmgbxvdXG8$)lwam{O`U|W@5IeQQJ6DvIZ=EW1
z&L-N;)VJ0}Vf7!r1wKd{>CL=36(5loV}d{E+m@8~(-&&f%h-hn$Py^$r^k-U{#+mP
zjB@r%WkEnlm5^<6n5sW&i@Plc>O*+?N$5ogrtBx!&_>syj`tAF4IcSmy$g2==Da~N
z@IA-S353vlnIN8!PnyNnLzBkx%iPJ@Ng31>piia)e(HZw>H7@H?yi1xGj>p{)!E<P
z>~#$XIUa{=u5nI)w24>jtmm-BG%F(W{hSLz=8U7t*z1h3j1qcz{-XVQ4FuhBCl|bu
zTGX8Ox;qgkua=E!2EBL8;D`Hb|8b5!=zZ@nP5}3erV@DtN^0&m_B;A2<M|hmIpPIm
zQp?y>ZuFy;CS40h_<f?m&VZ*D3W~jxjl$r#2fd6Ln}Y&HPkR~z8pW%Hu_${YSit@b
zTV9pfRv`*rKbRLU1PwsJk2yuxj}!dGKw)+33#IAjeDWdUUaGkDa)6rZD&gv<^AllA
zs)mCD*v(&*1O(XiCTDKAQkgqdFx`pa*<W@Laq^KarfCaQ`p&)_UW|{6%wYX0)!8!-
za1TlHIMqnBK+U|4_Ka)X=*8@}<K>Fe8o2k*N6?@*<FogkG_|l4=zMDO;w3a8EVn>)
zj|3?}o7bPXpR8e~e%7fj@lO>JE}F^f^49hoY9=iG;UYwZ4yE;!QC_7v)a*PC`N1Zc
zEL;@%r}8AOxYQ}hzXh15^d$%y{M~{q9!qFu6q_Ti?_kI$vwl?K1>8_qFPQ}!{;0Tb
z1-_KRF^t~|AAvrshRC+gS08p`U%F%V99D8jAebyU=L+Q8_U@St>aW_|$H<Ildq!l^
z7B)UaOK;Xk1OMu+p~kXf>E{*;j>Y5HI|f5|8arf4Y>>~eqTBfr6BHAXbKya;LV9t1
zsgP6pwQw<u0ib#tSV7+wl1d18?$WFddaG(VetE-t)F(FL8~q$?28b0%2=N7L@Ba3a
zYjuq_x5ZUZtS2zmPI@Kb0FC!^%g>sorm~V$qYb1&vn8)c2LdlnJI|MF9_`eo!Jl(%
z&c&1ncBFw@s17FW-3cPxG1Cr7p*hnA@ZV50OQhObxVJ+)qtkmgieNBzPHDt_?r~c`
z#IR-o`~)mjspOgqs1nRlcZ`5Fl__%urt_$E!F4m|@zScWlFB&JPd=Z{+HxBdkGP)o
zs+J2j^Vr9s!EU3bVzWBWL|8|aG|BTaJnEF|Q<Dd@bCXU7X<U*^I4(sd*D`-=2-@NW
z5rm@`tu)lmTs~`JLT*;s%lfgqHg-7Y`H$#$?r&qy*&o=;2B488WjOII07e+JdZ-8Q
zvB|cLwxucZ$Mx~=^0kJ4Kk7EIq8;9`rr7R|5uqeA_<085z(wc$DL%Z636cmUC!5sa
z)VryEncS~Ipy+1?ce>__d)*9bXjG*v@0Yp-VO;%iXeiaQlJ{mg-8?K08q(r7NRkUb
zmGyQ-(Vw5VR<0+K8FTqN5h+rLAWCM-@745mbp#YL5pP_lNM%}j!wk66{Zl!)wtmEJ
z2NFRkeINX@i2n%i+frx<M(=@4^0+UfRwyDoQrFq|Y)zcYp`O`;iZO*GuxncuI|X*y
zbGNndp3jSP4M~{+iKJ1+sOn3uH(Zf@#tP>P<(Cy-@#2smab+N<<{w90!(@ueyW(A(
zl?{g<aA?I5c4H$*{1?!Tb{_8OGjW-w(|Iqt*>2UQ^&aHVtY_14WZto{0x^Mt2l+Of
zc#K2q9v#L9FfVkmnJ~I+Fgcf&Pf|)<eM7q;N4G~3K$P86ag7$@bSFmKL4HrZufp^F
z44aMTF}BJD<EVQzx68KbdNJUh0kJR~b!qyfO}TorzkI)4oIY0@)|K2TRxkOPK3iV4
z?gMKhpTf?iY#GdE27s*$TxrHCOMkYUK_!q&4z`i(geIcVUM(G+u+on=Yxc{(mkVo`
zmh0Laj$E7<hxui3&8n3b%oyQ<Ll&w(ec3uyYzL*0V|2g9G2)Abfyt@Zi-Rc6BdCHe
zG9wd_S>b=_=AO+QQ!J@XzbLA}dbr$UI9^`}sUq`Poj-RsUhNWeSigl99YAI4BEOa+
z+T;`Fc3{@2Ns_TfMdLL~ZI_NaJNisa+wpB=l)x2F%xg#=R2Z%%QsD~%CCNWY&=x2?
zUn_hH8Wx||_zh4NGkTE}IyaXKrY7{M4Y=HV$3%zw<p+)4=n_|)aEWFEn_jmD|EQ*0
zG3e1zG^^4hD>xF1-?8-xG?upD;7>#dY?bhns8aK|I?{%Yw)5<AVaCr!MLCbf`t@D2
zqG}0#de={1_r0^mJVL<<E8GkyDXtHVUXTVSx<`$aR$drdl37V524_vz%TYBIK4J`=
zb*b_5lVdxbcw9{n)r{;pdmux>-OH@?_)4r6m3jJ+m?hRAY_u=;bR}qY-8?Co57-*y
zx6;tN$me#Y4qtRwnL<e>Wo7}0QvYUmzZXB=ix{K2AqaMkE>{o_N9wlGkBT|j{q2Y{
zEoD-pxEAhquH`=y4=kt-Y|abS+`9N%9z8$6%bS{zM0yk>Iuh2v8FpL$)^^-|MvBWG
zR{pcq`9;My4n=$u&IFZ=6XY$)Z3>2|8rn5+`zspo&eu|XSDaeF3ye^~hc{SXQsUBi
zf)?<LUW#tCV(5>y=FS^~__$PBGXw$-0AAM9O=0nN;fKaHb~ot(EfsU=E&8nzye!Ac
z4=Ad6Se;*|W;30lW1<fUkj=A}DHyTh6S=0TkXBJjB)wX|yLe;IW%$13p>KLc_(Ig(
z{1PN>G0U(hc-4E-uc%ktEbVgz_x{G)j}y>sj>s95niLS+`r{4lwZCxyMv=}1qmE75
zXXF7Nku=3dBI4F$D${oUN58Pf3R1z=jH%=&MxT^^bUriM+}8xVKn~y@l%|N73Yp}5
zX($eQ5rm+L+hO~h6sK{lN&T@_F?nUcM+WYee0uzor_i89=(Pd$Bgm(Zw+vyE=S-6j
zv_T*@i*}<t^&-sX?7C2}^W}wT)n?3odmr<Gfi8kVa=s5~1&Fm&VypTr{_qrKOc*<l
zTZ$r&5!1Qi=lX4hLSzi%8ILAwJN4@D93)Zw-dQm_@!Gq=T$0FHJof@G?z56<_;}5H
zk;BLam(4}3QF!K$(5_=^4!#+^lJuNT6B7(Y$8&5>${}sh<#BKo@K>tzYgU-f48XEk
zvDt#<P2@PcBBcarU=|%Ks;C3fn8!zcg1q+~u)r4^eG;X<q<ebb7$g>X1i;;={BbOC
z85cF@=!<fxD{xonC<3EyKxj1eB5-KX?2`0%_vWwVjDzn4#ZG`dm7{Ct=Q|~3EwkMT
z%Z!Dv=rHt7xjnDV`AkAwWrp8cJJFKB=olL!Br+A+qaQl@JA;c!e(4g`Z{g(IIIiKd
zD+IWwGmJ;k%eDuP3wTBLcr-c7?^OrsmhpK4DBrj(U+LS{xLdAbVyG;>hG|!C!eEkm
zbXGLu_;b*+a;v_Z+)9=r&wVHi*fZL6^73l(ZafXIbnDCDvLqTyz>czx{hLfff_Uil
z7VYy&F_jNZ$cBa>akYYiC$LK$X;D0AfPnhSOpHo_pct20{sRd=n1}Osh;g6SPvJr|
z7n^-^hVHxtWl15wesj0N5E?g(z!L)t><jlKEaG_&ht=tFzr=izrA03`$e!0XIZtOm
zz?AMuuz&P-jL#G(n*&Kws_vA9gFjgZJ6KZ&+k66`zm^q}Nx~EW+MRKEVigtDJznZ-
z64s%|B5zK}{2fOrfiT4DP74%YhA66*>_H{hJv)0mmLQvtprEHaWqm??o1EC}IXO>z
z%E0r?4N3he*VQN*Qe;T9D|oHw#m1YvjP#MuYlPW2z24O07^MwJ)oqV=y2&y>cvK_k
zQE2tKU$;%F!H$d9?pQ+E`bfGZgs<pIOs0_cJYQY|K*@ok1e+&X(nbay=Rhc0OsV${
zU_pkca8#M_ol{wLk?Wqow*KWZXhH1?FmK4t7;)`u8@5C`15R8^Mq?Sav2wOi%CJQC
zVr@n4O0ugkK27#<=Kn~#;Yyl20=YG~XqzW+;kAtE=r4@}EDBuG8Y->?>0u=qaznMK
zBq7xfFX0GVy<0nBQ#7Bxs*h(k?8V?6aDkoEBE0J%Gf_qM3`gemLK`0TlIMXe(3+Zi
zkn21?GHF|uWKGPvttvR?M1{rNU*2U#x!2pVLJ>=hyNG6To_g4Cg<#ezej<()o?tso
zQRI7fWR<#LZ%FR6o|zWs^K2^Luop5~tw29dTqo_4_nbl5zln5jFk2t_>BpTTpg+sG
z*BcqGC+nW@>^7<YSrfa$ez|I_k(L7qV_|qzJq$|p2AN>i_j-mD&9^+_cm8gQxD2aq
zWa<9XR-9PcC|p*<XR?^fauceSsRI9e`fO-2y7o*~5_fY6epUr?lx46_bC@7^wcr!0
zqk|5U#2uyDU5n|jCmDxZJsX1>61z2Q28K~v#%_l7h4`S=Y7pA%O?x6pzSsMf(4CIm
z+wykXqPUX4ufyUXnVT(^9M!1u`!3lLV=9&4_B?1_wr4&!Z4Qx2=?+>r7<DthYyK<^
z(C4#2W;S&#3O*0O8kFqt%efv_v`kPr`7Q9ka{s|Dq+u%XAz=S$04r)QJN+F-qZx2z
zRmhQjK}bf8Y9dkJxvw%i<|W8xwh&3l5U`4~s1b$laZr9=*LHdiAF}X?QtR`>2(GfG
zu0)*R_rzN+DZeC3*58YJtW}r1w-J~b*c7^|`dM3{2eBq^PoMG|e75sI@2Ra<t^5&H
zuULKNYA9lxhV@IYdb;txFoG+>Du#5cqLbi6_3D|zMI|AAQHkIZH@Id1Pb%1c`;?{l
zWR#Ina9fvQiy<ley+J7*a8c04y=A;6Emfg>7UB_7Kr=KUoP=sZ-<Nrmq<xu*E?W8$
zg4{G%j$rc<jMKsPGZSI6Ktjzok<0k<660Kpeu1A5zuX<%C@UqfzZzOuG$<$4`fu41
z!>=lP7d|)sRaLF{v=ILRL|^lWL*Y|T?bqkW5^wa3hGsV5)1zGOG|5hz_Z7j+<|OSc
ztN7QsN3#OOl#U-+mJ=3SQo)&u(SwWQRG*OTpCJ#mG^PkWp3{5D74O(~vE9GB>*W1R
z;!n(5F-8Z$psZeZtDf(D9$pJcyLLZY{c$+)V6KF3pM-S-GrX=#YS4*F;YgA;O<{(v
zZw4ZXmcUu_Uy>xuwc<<ElJ2I_1&8V$3MBe1dykIyX9f?>BbqGlL>&^>l_aBLrit*&
z$9&Ndf^;u}Lb(o?<}dI%aL5R+ljNQJ<$^g=UUr_5bOM^@F_iW}s$nW59g>~{NlaB%
zhO%0iE1n!EfC-Mu`iHc{K5$s5J_x#0=Ma&@kX8Q|Yv&XtTexWFwr$(CZ5z97?zV00
zwr%gWZQHhOyE}bygL{&5lbbQ}SYtiZsP#~_>i_2aCM#pqrIooCXmDuFegvL~?_V9(
z&50BB9$ikFW4((XUxz^`RK0&C6Vq%4sQm7JldqrPQ@Uy$b^S>60<gB~CmKfsa7PIQ
z!4@eA(AZQERE+f0glAfvcXq9#2UtdTfKm!VU5O=d33^>alenE1P!#?vMPKKhf@dCq
zoWWqzSWbuB!~~fQ(9VVULn~r3AuXj5?B}4F%*$PsO-u;+O1TJXe)2$!B&AkG61>kw
z+hd=MI%`E`p!2dlqfsl)8g6|5;A%H^Tfr>~7wd^GlVJ;@junKmvfCqBiGly^S`sAJ
z0|Z;#q6xkI0e=fK?O(U!;)B}t4`Y`z)Tr*0aCbfN5yigoAk0~+!p~9{m_ZE$X=Etf
zzkhukMnubXzxp{f5=WIimJ_Ty?oXQ@(3Q!*rj{)ychLgM_=Hc8kQ#X%0_Xw}KNuSA
zvsy}7wC-!JlJ*#8ap0J!bKs_DA;|!Ve93Miv`E3_4QpP*3Z0=@yY?!D9EnvU(#QA_
zW|u6aS9}&f!PJh;?Whx_NccIZOBZS5ZoYOU&tkJY+wm`-dL{}JG2PI*X?2uHsSw0p
zqVLuhqSiEKl;u&cU$6Iu*rOsLIB{uFR5;{A!J2PtOJSk~DBDz1hB&v^R4yvQL@8e@
ztu}=+oY9MX5hO^&w{;m3&$sxjgy^dW)Il<+xC#Yk*5c!AaHEY<wFmY$QH(XxR2Tpd
z*U^H)e6@t&jPe8}rMZ|vP@lbCtt~7KyT(ah{1+&^E_w8-gM3-OsM2;e%^P&agA)}?
zFNpLeVP`dk={{$4Jk+X9Pc27lR<zv_EL?KFNT7<tj@rG6ZgVjf>UUMOTh%xj{Z#PO
z`uzPe{aGW&dSi??z`C^#<~KYhz6FUC6#4pNtsfV)mD&PgCfg^1Zx#wlk83ZmI1^AB
z%uU4OUp8*XReZ=xOP)?qU%2e=O65HfOHi|aYonkf2o0(A1<X<E-kCPx78fYrUW&~N
zE4n#JL6Q~qCEH@KXuBsI?mh57bu`aoTU5a9@L3>ad%ogOc*y}dx-bt{p=lw?zTO1S
zZzzlxYKm!<#|_qgt0j~`R1Q?IA^#bSyYrF4?2EQE6KVYw4Q^Z36<#k>p>k>D>eqb)
zWWORdEWiIovxW^=cY-&nexWajd}n*hw(=8iN_q2MVNdma(cK2KL{WrQ&^0(}dy3X{
z>Q32hG_nxN>A3(l0KV`byRcYO$3g4}k+;kaz&Dikve_QNsrPsR+|utHVA#O6$`whM
z!p&Mi_{5`G(ohF~Fzxibo}lbTj@5^0+2?AX-9vDER%5`Nf;ZgKKb(=K?*<%d$if0C
zON=|uU)_d#uRBeA9G(68<!xiaUl-EIdlNdL2t`mFCbdR5^{|)AyAvHk+dwN=t7uK{
z%IX8&sW{r!L^HaNdqbvgPp)5zGpeT2k(4AHOPY658{ae9@w?0Ot?fFS_7RrO#M7<&
ziCE~qaMJ)TVO0n{@#9zhxr+OMiTN`mta{kSPQp&hO18X6Q{M5EVT;&4vv!IrpD2vn
zLE+FH%UgYt9E@~vvG`Hq+eL7r3#G0uRwy6s)j?>(RvQV=E=b3A1ZMZr+#eX*ZWb4v
zZ=nPes27_}0VCrfa?8{x2xHpQt)?J;Ki8G892~vp;{skmo|8AG=y0iUuaB3Y`fF@Z
z)+u+G`LR$cz*-{5`wlnZW1Ct#-s83>{7PhAlj4sNk>`b)m4mS3yxp^pCw$P2+s5CV
zE$k9Y&87yF+l!ZM<SIvf?(vcdfsmGc#-k;kr&YK7Wa$33lB4)gGEfeJ6-vAEy~aoZ
z<)-kX#7HFwY9<xsw>rcHeUs=PdQ(UX%))b#I_z6wBve6h#Nfg%B0~3Q<H`YsE7#Ay
ziO*1%;A+&8E(e*YZAwXXHsU9b<qYRoqea<`85dcO#=3=HxnZ%n>vtB$gl{+yzo)iX
zmQuP#psPmJRT+w1X?H>dm?7o0+)K{ItoJBgOg}DmKtTq5@Niojy5dh;{~A&G)`0Sx
z+rYP3YJa-i=U>0ALY2z|hsjy#>pw&#IuBV2IbNswMtKimb1zZ$Q-h_zk@MMGo3K97
zvhY2c8)EJpd(soqQNBy{^A153Uw$AumS+y7!cF$bTH7v*Q^tgc{;>fOK;NBF+(~E}
z`vxQ@Og;KnF{PhG4uP$q<$o%sB&Q}KE+O_4Q<9bZ4^m3BKarmQR*U(mmHbmJ=4aad
zpK?n7z7|8v_>YVKK~Cxa*o~2(H?p<1Hu!11nEgbBelkQqwHGTBM@M?Af9v(o+nG36
z*c$(%_VUk3rhnv?{yE9?-!_u}ImyJr@b7j9*%&zgW$ERoaKQ1OPBJ-xv#ejfH!mvv
zWB?-a|0b=JTMkiJDmREA4M_Z15v@#K^Ng9psI^SiY*f9>r)acHQnFlnE4jS?$o{_f
z+;-f0KFGAa`mLAq{-(_iUy_{{iv(i=3|<h6K^V)RfR>wIP(TR@05llnFQDjvher%D
z1o@Sco;eH>?IN5TTlYOK2nrjjf5W8;0d`#yC<N57kp)1&2q2+^MnVb=0wm%u@RK7}
zG}rK>8UO|aK7jykP>^WB=8S<F8^n&PsUO}M|L_L65qt$Cz{I5N-QEQtHRHen3?=~N
zLWtzuz|=#8vIoNWslsv$z3WlF99%MDplcu^%FD|e{Hf1ifU+*>p@+B;(Z2u$vt#F2
z1BL_s0D_qb=IH+>Kqfl|#8@TX_&R}AY{HO-z=G?c&O}h)n1jXNjSlLa1@N->%PVHv
z>4L_29oBe{2_)F5ZU^)W68x5YR)44#fjGF<hd>E;aPcSTM3mt6qwRx)Yk^-v8*(!M
z0Yo6VNFg2=g$<4gbrV34L%{Cnzn&rlsEOwT2mpUU=V7P7+6}Nrb`j?I44}B9hW$!j
zjAcMgtY^jvvnT#u%%K8;4c)8W)H;|p#*L$pFTB<Zpap`g{{#g(x*{@+6lrPeXI1@n
z=q(@lo?Y!_1c1zKNK9pc0B{BE>CAm1>Yl@PZ}0KN_AS4K`0&zEpaan2foQ?+LyP_<
zes&IY>jA`Q;NF;i{s{`v#svxhU;u>!uJl|YNZk3>?JwG`^E3Ipm<OTv0R1_}g8=w=
ze4jog%+_T<nHzn_f8M@FRaBa9sV*SC0l)RN8XKda_oMGBj{OUQ0Dy!F4p?{t-tBAl
z3jftz`@M7~h~WDO_CcOUHirfZ0Qjxnqqnuq!Sj_5x~)gu5C7r{se_U|ZU?Z<zt9XK
z1(5dwfAZ}K=o9*}?e<MQ{vGl5y@{~0IeOZhcHR8>%^;kKD7WRyxl?)(-CG?D2i1);
z^6kPJ{H<Yv%K@`{^|7rc3j@-PCndqnGZ-we*C%kx?-7KJu?re{O30w_&M)%jiS~je
z2SK6@e;D%exVKZ_*YjPs+YER1$5(aZ2Kn76pk-Gb;oh^Q+nB>=^lLK|3n@I{b4^SU
zQW5|pD#R^eAJRXC<mRKNnil5jG2<El(SQUh5FT)c@h88eXnv<>l#B-c2jf#I?wgu+
z0XJ%B`EwG?YqI-r8yh~<vp4TR8JXg*_8(zYiTPP-QZ)IzL%~<+^-^)ue4#*k{5XTQ
z1+hxBJ=l7tlIz_b<t&05;!rgmljVYoX?))opVFhhw`lwbdjQp|_A3K&+BgW&q%U}N
zTba3K<ZcG(6%%w?f3ApTj3QdTp2l=iby5i77mc>cqDMidwHieADJy5lHj|sH!<iaS
zHB4mK&|w*1**e9>kvt<iI3uPQd!^%&cPZOW*{?^NaT10-)wFZ2PtUVjXGfRN5dR(`
zHhg5gQyzt+YtxsPaaaV8%4{{xX9~qcIa%sqJ7b}G2xp;Zf<+#5Qn`o0u16-9XcwBo
zrR_dJ9$-hROuC<z|MFZ&NHk&$d(uBm%&g6rzQ}&>J22}7lh?%J<2%PT;mB-b*?t}j
z+q<+Q)+1XsdY{YR1o(cI3PFS`JydWcG?>?Z0hx@DJ6q3IAQ&_V<f0JWqHaGww6T=_
zTT#nfeJBbSUEse2&^yEyRhJiu+q|<5a^ZnH9LI(XUK|xsq%V`cwF{(4HbSt%T>P8x
zs{QdI`eGm)#=!)%U5$Sro|68%&=GemuLeVx_^u(>20m5TmZkg-99{f*lT34Y8`ttf
zgss1_slO#SYY<J8*PR!F$0~_JnPk|+OG?yIQOKIltbv~jlT=#Zw7NK<cnReGNyD!p
z5r1hTGu)&buObLyxfgMxcsIdgPa^Gk!N~igper;UE7Sn^Z>WXR;IrD@#`E0ib0=Z)
zJYuuRND|9d66Jb4ZwGuIDs?)^!J{l~z5vE5;L)Aw@5Qoxdc}1fmAr9~-kB|Yc-nhT
zxR1*VcB&KogOA~7e#obY!`d@))ao2R%6_AYgYg<$(|gKs3V>3}QS>o{*8|R>HsY?3
zn=gsxP~5Hx4C^nAp7E^17_-}g9A`HJ8s-ewJClMSUdk3>{M+us0;yZb{Ssui1PL7B
zV$NT}qe(uaDtd|!I!F;*c|`XtyA3N<QS()1?MtCi*~?}c8br+mKP8R<NA~pmd`(=3
zgzd~As6o#gaZ;e0=JeXxvIyXxdscY$bjewPY$QadE(ei7!FwL2dw$^&Xcg7k4r=q|
zPKGi?*bY|zKpl;9QHfB!5idEbaA<gl5z~T=EA2sAMn!nG>c;QbiuPM8`K33(J4>**
za@LxCk$Zmh)+FQxSht*e^hNBvVDeDtw0Zm15qkE|oBQ}EgOg30lWrSI65%1{ZGO~t
zsPoERI>4W{EbQkmu3g+DvF}_p>?KbJkDEj<{xb0sfv9PLkQbx)Ou${uJS179^WNaH
ztx{oj$<2W^R7_YI1gEyx*krm#6|%GPzs()z@9Bp#+^pau;^1wY6zo}3tq@mfjGqG>
zpKlp+b)a!-;&_z}rw;y~ne;XYW=2h7B2nmivz=&)n&i#h5chNU3qHH`Xs->I9SPjd
zH($&1phSb>f<g%_s2L<x`DCi%o=*J=;M1}Gl^p~^ypHwlNb7MV?A;+bte7qlnT;NU
z)gJke%y1G=!U*>?yy!vas~<Pj;KSzL{>J1MYY(acp{hXfE&;+sSl5p8oYm`Wz-X+$
z<sHF|_Gx1Txf^(}Zc)TQuKs4;B%~dRPj7Ohj`PkW1;m~IW`Q40WdM#%?vu$tIJ0uM
zjv!GdO;5H#V_ki`J6}A?q1aXQp}7;e&exT9j(HLHulLda{XQo1W{#{K&>K*}_3RcR
z5%#9a6ESyTEwO<KkF5#1jESZo-5K*END-=@)Jk9YV{?D&_PZYC)KaSRBVWOcD{r&6
zCg7BK!}XV>w?`hI&?tXX;^YO1;A)y2c~>pnE0CF4xohu?O1E!*P99b@?RrzCsLz0&
zH3^6IiDjL8c;n^VuRv^i--ztq(r42Bmc9O>-$tfHa2j-7kl2H$G|*?QaS&C^j=JYv
zNW*J5N;+y7fvb~?lAk?lUFT=6F|u{;Yl!<AwO^?5q(*W47x;O(T5rtETNaM`Ih*UJ
zcSVZe(ji|;QT?Eo;ejhU=5J!I#J5jwG_y#6H&+Z)cV?Cl@@U>7sX$dXa}?s~E<2d_
z&0uKHepr7`!nq#{Z(qUYp@%)&_j^{)gsN)}zREA694#970y)Nb2nOyg4qAfH!fWWl
zTqpw9&)lxDnO%q`8ywV!d1k(XN~Clcf4&0rM06f>OaU&UvP9VKyQ;Y7II&lR`$wG?
z-&jn9(Va?qEy>T{3MR}rH=}KhyVSD=bo{E>>>{Y1Jh*fbg5rCG(o|gkIy&tfZp|d*
zrb!$5Rg%|dfEE}u>$l=7HQRm`GLHKYq}@3Oe>7CO(4}9hE$=)S=w6R#bT@&(kIQ>D
zB7lS4Ub$C^k?pU)nswB;MtOR>(42xV=B=u_YvDX;lil0WFb`xN(`Y-JRgX#oanjdq
zMI=+atULSyLA->}kTGpIvla8;nrbg>8SU_4tjdRF+kn6Hs`ols`@NzlusDRO6;0P-
zD(=vqLY=V5wp{FUh&=x*uUn(A%oIt!w1PF`-iupP+*rwR%b+Io1?$Y($yHgzP_On`
z{!-@{adKBP{ug4sbJUmW>=C@|ee)Ctb=NY}t!uVM4|MQ{tXTPW1-irbXG>3%<$gZ%
zYq8<;d@SXWPuW{`=a&)LQ33~2;265nRfAUfRb1lXZJAD0hLCRD9Zg6(e^h2S{rRO7
zo`p^DG4zG3i`>8rdKW~=QYyO@e%IiOk;so8$ByU=Qymi%!@-5Xo!&}BkQc;^W8jyT
zP+aXw$lvnRZ?E?Hc#cJ%w$*k$hNjUS@X3@ldgxzELfk8%ZfoYI1@aF2QBs#(@b-Nc
z>7MU_df~F=wsfkI>rSI<Oo^$RGZyE?>&nU;vZ-^8P>G0=kk?0r#D8;TTlw{de3@m<
z&c~3AE7hd#M&LP<c`Psm?}IPOr=?3W{+?7(p$>qQ4s}JU4Bl84H#<=Xu*wgEO2dnL
zz6TsPr)nXmoaz?SqXoZ!&2IVjpXG>b7?0T(Iwa2&4-`dW7Y$)TFFc1)Ydv-76B9Do
zNL(ObHIukC@cfvZDrUBYEg0i$-uP@D?`)!%C{LUSq9=oUN$i6Au{Al@6J(3)zBi8D
z8p93J(_Z^m*e`xZlM}ht8@vmZ>^aBrU9GBiZj`Tmo~;L@92hl&_C-a6^To>E6-r@n
z@volVC)oOmx#j?U9z1C5F^aSK9;<F0q0Nt_zuMlE0ja;$kE&oi)~buQU-<+dbTX1@
z6<a@2Yjb7TeHZQxffu2ij}=%uMfbzMNmY|@9c?&2e540u|2erJ!7brErFErfp&VuK
zje&~HJ#5wDbOd+hpfj%&?7}*31p0pR0v^vXV)<3AQxyl8GH!rpQKfhmPNltCh~Vmm
zHY8^$b0l#XF<IrD#-$K=!#2_g7=d|KbwwT|(aQJzgu~}hV6&zuJ$z-xr*oG*;uN}Y
zk_eTFC>35F^44gF9!!_y-x=S(r>}Nt&j7fUM;@Eh+(pGTtinC63%$K{=ZkZ9=;8h#
z$X`IbL_hV|Z*TE?JVd-qp7&VBjZ0noX-5DW_<3^wegm$k5ivagX5Pnt9HS;HvOsGp
z@sEvQU45Oi1xxqr^6*Uw)2*^X{!5J(TjlI8)A<gkZH@{?X!)(YTIn9CS}Cxi$+R~4
z*pF}sOnzbvQ?eVv?=dJHo<LSFb}>8a+yjkR^(?6Lut2mX57F!L{7{jX*mPLY<yuyY
z$p%k8D?ERb?SnQOpHtzhFNr6|Ef!j$`h5tWof^s7#;@aXj>v53<NNtE5mEAYzUNUm
z0Y1W5I)U>!BcLbw!Bs`V0B1FgH8F+R9L#liNN0ap%UX4=WzEB<ZPq=#m=I^9%nIiX
zsRFBO?0U+iVGeY1zdPhz+2j!S?FhTcoHeV{ebIPm%5(sn*ZXpV2l$mobThaZNaFI}
z94j=2*R}x#2}y}7Ey2W#{L91c&5nY_qEde>`yP9YJ!;wiA|Bz^<bm}+#V}R_>G|W<
zF7_a%3upqL=VABtrXyRnd%qQlyYv-~M2n${a!>8~;d|o&qib3=*jz%YB3?l2KUzg<
z%<TN}dkF2oDJzj@x0Ads8`x%yO-=!uB|%{v&o`}e6y#<htZZio^Oh40Go<lIo4fL|
z9y*-2@xVl@rmZU*N!K5uO<rNgNY3Ph(L(Sc-}cBshA!{5BV`MnAhnfg%TAzcwhu>_
zhvL@qzWYL1QIvJ1;$BxYd!Ry#qZUJ#LCzr|`;B9hI>zg&`f>`HHI*oH*_6bFj%~4G
z78WsU`MaWj<2>CI--N$iyx0&B20QR<YekrvbHl3|AYa1xmM)D5_l5lLdlQ_;i3>uv
zh@Iq3>JYUiFciof^GrQii<0=QRDltBv+?F+D=MenD$V42Bb`KD!_j@$48tyF*Cmvs
z?p%5e;c;<9%G63{5SX{=z4&X-ox-ptck%UCuwmR`&2T`(3V3L5;sn3B+F1OYYelGU
zcvGncc?ta_q8&0GYZ_a{*$u~VMZCq9foK=b$BPlcMyx->``5bAn7Yv6gSvKP55JD|
zP<{$p`dT_kPKS>Jvky$?(*fS=j_DAlYF(v$h<;Z9M5hT!!S|u=ovuMl^aU8w;Qm)2
zi4NL6_dvO>d`o=?9Ktb(6X4T6wq@k>Z!Ule=9YtWY9)1U&X5cxt-8)I!;_nl=9;H<
zQ(@ed3)P?DZsAN&>Hbht@ozK9@+5d0a9CZs0`oUM)}Iv;JfRHji+XsIXWgm&@aMMn
z_p^3)g+!q=eZUE=uru?D%Udo^@S#Mae86I+&{QjqNk#Q=VkV*ss8zlwtw&FOgQ7V<
z2dMH$AsL^K4&h@OQ&c2Fj`U{#0l`xIE6k-v`!UPF=~plCQ~2iMi-)eZ131(EF6tZC
zoRA@J0_G{Fh%!FXk)wAuBwHvUo@!*g+df=%1fNbkRP6-#*fi1E6txyEuFeSfZgfH)
zHaSKG#KiQ<#0TC4B0L{D%#O`<S@7<CJe8w@L3BaG`MZgTLb#$9_JT=4U~~Rzckfn5
zFAf{nsU6wul63w?0w`pv`ERE8B;y4A40V56)?B3!9k<EA1Rs3`%RDBly(hxPXWH$?
zm}*(uk-~5Nr!5Jtu|>&q^4Ju<mL2~qd*3z>B!3-47JM7YIMOxls(VaR*9oCEiKYw&
zY%*=Vt9a}HB%Y@pDv*JhQ8Ut0$IA<-=YnmXK`u_fwH2SGob&i+GhH+fkMxnH+zM4!
z3FW<YB70|6AuxAsUUlN(8IW<p)GlYk=#}w#a0_Mb@wvk;9@I^R4<dox%j&okpLjLr
z=}%3a$Wri5I+Ymi$lefzGc`9~#zlCdHmX|E%=;d2Z;9WSOEh*(-ak*1&!#$Hjv*=N
zh0UQXJD;lf?nJ1`v{j9^Fg%sJiEwT7LZKJuv$OW`@s7b~q@Px54~3Bz@rw9?5+0kT
z)2$pjMdh}o%gG4E*;$dgmS<-c^ELxjV_>w!oMHg!wizq+beN`F>**I#2;-1kd}3W-
zu6_lT?C66!Y$#Y6>?inFclCVXHgh?uP}B4t+L`UlS&UEhUBh@Xc!hK=CKih;Y8&ri
zVR>tL3Q9GO_|4i@wM)UZbPZv?#=nRXq@P-Tz0N0<G;d^`a$CBA28qSAk<txrt~l?Q
zu+|w(Q2$!0t-gi~Rt%wyOF4<bHjc8xJjk*%g~ZDWob+&Wyz6y9xpo;{C+)r>SrJpG
zUE_2gUmSFr5VzdAge_TybdBhfffj!L3tBh;MG6tm;;ns0{%RJVDpb5Btpn9iYkS(9
z`!O;Eb%kcmk$c2jVeWCmKv<Qg>AT(1AkM_IWK&#aU48k7fw=WPJ__*m9R2Lrhw{@7
zYTSvmxIA~)9$SHK?^E?@@Jl47$5ZOKHMrUf`qFB3vlQ-(yC52xCOi(#XC^srRo$#V
z9?TU!U2J6nH)<5?I1IgS9*nGF3dzxn)`k3CD(0!67=lKR;dz|uBsIZvqN!x4L#8YZ
zi$Dpe+}FZG-a>{#fz-&~?~bR3#<0z>WEom{!6D&F(_GZ;%;UKIV2W29lx35Ii6Xg1
z*TjLNoB8H-sYX(NUYGZ3Wa%>(3PXA00D?S(jCZ7%p3h^Asz)id9M*NkHjU0$93IS~
z8c~*c$$zmz){+x6d$ZeGCE}2v4s%i&j#Y8`<gD?XG3?YAVQ%=`6qi&4v8l?qs?wwM
zk#MrOCD<7BDFYtU+9Rdc#&2t1%)-;h^*k~GND3+FpiVKdZn{q@xeL-Hs{Q+!x4apv
zWE79+l9g^)721S{WTCUrwpfdQBc_;1stG_#_f|hLdO30D_GIK&%M?kCHv-ILIPau<
zQp=xxb{dgQ<OH~TCecRrEs8jgoK{`NI84=5D(+W(l{_<nGlkEeZE3}@n)|g=M@7<g
zNdA|;a1ZXqN6_^h`WQ~CsbjUngT+RU><alM3?TO*ZTr~s98PvCc9gt#U_MdZi%M9U
zoF!e0YFM%fn+ecILM*Hi3=s;pH2gk^Qut1ZkD^yR=t0yQ1oWZ_7`Fojfbmf(rMqzZ
z<$RvX0^D#}asS=wv#P_vkZdRu&}#m@8J;_>KXIr!WxZlY7@r8yZ2@VC`(F<x14oPP
z%hAhJn}KCysZKQph=SvP7xNpW>2r%7G#+g_cY?7dgU=f@`22YkUbPE6(3;t!kjFd+
z)mA3LSbC(2)X4W8ht|PR(V9+bnHL*|J7S83<}#l7&3zERe3BxkI&eicECH=-PDLtJ
zv+6o_Fce6gcI|^gc~l#C_kB039YvGEhV3rAjEgz3`Z8&+<f=^el)3&$ur!2_!I@q0
z#tO=9NOD8{l8jbHa~GDd8qjlN2Is!15XjjKl9AIK$uHAu|LrQ|RrAa`{a89dvWyXM
z);M3;aJJ*kS|tGB4>wud^}tL%L(RG<jCcA~=Rs<pL|EpvMz35?sqOx#k>ZOJ1h<$U
zpnD@3UINBLWM|kIsjr7^`7JP!GMX*DPDAWmnKYr<+x<7ocCJFAzsysMqA;%BuT4$(
z59cq-*HQ|t@5uT{Cc2Pw02E<902};98O03#;>|cGj=5H|E_>$Kb6uN>c5X{475>C)
z@V$YV=XUn6JI%LA)E>OnJ!a^@YinNa!%3lxNrWx)T{H7|Tl2%OrH4PT7yGa6G}T#A
zlT?vAoXsB<E&0hR{p!Q0iI>HCM`qR3r3Ytc;q0&eTO)C+<>PS=i*YbZ6s=0#Btvg3
z(d)0`P<sA!UB*8`p!8^LS-zNr=Ip(NiR%%!?QVD6)nPlO4r1+_FQeKrMD7-C;xWIt
zwkXXRFUs4{OgC@`O$Gb+*C;$v#jcg;T7jV!-MZ_-OpZ&&F|W2Ui}80C|0&x7Di4BR
zeTn5P@SPJRU9D#b3{;bBg&1-G6oVpc5M$D0gb~Tb;k5+^^tA^8m;*kAdtzGhF(_w_
z$b9(qZYrmSM)Ay;e%Hj?QRL!?!4|Dsg~NS?+xSgjN?Mt8BWN2hXwmT-RAFSVUlp}0
zds964+qfISPlFdl2^(1jU%<u46<C+c?>&{I73yB`!sN5C;nXK{2=yVYv2|xs6fr$R
za`&6-+8~Vsj~jEi{7ggQg<^|#^)SexUiWDmAGo2OHeBhy=Y$z8h$Dxd;;t0?8D~l@
z$OFNrjzjnNq#}rZlrRhOtJ#xST~~y;zUdRGMW&c%2$Wj~BQAUOCrx}MIW>8tdD!`K
z<+`J?#dCQEQ``jRcUh`Dq6bNQjpwblCBN_D*b~y~fWP<m8d5oZ6bZ+RhDIRjda_Bq
zpQ_yE#Ymx?Ssl&y;Ixq8cP+jb<uRxwo})g#s56bCI_ITm7)dT)>zEcNF+;$DoC%6b
zG$(K>Bdv3cKek7;tRq*qOKhIYAkDrwX%aueC=9U=2IUN7l+e@)fp4ImlCK{q6Fm4f
zVhXcLpjytUTZnJ&5(~9(%U2*^9rE>MHragn2}rA#bBhtwmX>K0B{3L0z6KtfT6^38
zjd^~K@NCUTQ+m$#Z-fgG^n^I#fXqi)-Uc?gnu#8WKGwV1*$$<i57C-2w8xOvYobJM
z0fkubJ)Xw2>h@{N%}E{IXJI@-^qFQJH&d;w%(qhLw>1V(NX{V3wOxS)a@x!49$JPY
zv7y&tSqo)4Sm=uY4&ch9^*-SZfvcka6$xeie;}cPq7w45vj3HY{_w$nr3d_nuHheg
zKqeN(|4$s0<43>uA2<S4{+Vt5gJk|Q+hqH9pY4BG8d&}zhxoTwOayF996#cS|J~BS
z&cynkw=_6GvMl_t%|t5`TIuAjNbjs#lkqO&?PZ!MMvNgCQ-+Z)(<rM|*L(y~(WVRO
zCsM*78p#p4FJ<28>mPU0Gw-qYbmo25`&#w1w|7-Wg6y`0!YrOOxVmsbO#lL64NGoC
zK>;D8UvIvUehmaBc?ly>M9_B;**;6Kr#^yt;={Lt!jqqHty>lpO2qTLU;%)vJP80I
zC;`%3tk#@90|5}2^ylTDr&utRTwMrH0da^Kk%AozCOMOaj2Q5~S;z?b`)9-*|63pg
z%2F+$?tVZ~^%ig-Fi}7*g$m>iR6Rs6Q6MpM44~-8S9K^n4RV?#S<PQ7>+4z&XW@uK
zo*ZMhPykyH70CJ!!#%tmJ5N3IJ$`fxs9)e|2$<yh7ePHAv)FmKHCqbs9DeEqhzuN1
zfiX8CtNpw`XtPH`7<<Msu;7d1_qRO)z*ieK01?Qmd|P+EFR4(j54f-aLqj<>%3+`Y
zClDM23C@i2JX+sg4G;i|$@e&7_(1OMtpYs-5M*ON_s10u)L%+000o|&?i~YtJP39g
zF^E8cuK<dVe}GL@#iucIb#)G?2*UTp9H>5ExPHC%n46r@w%nO5itcU@aFDUBU33Uf
zd3FP2l)g>ms=T`tXkF2#m?^|)(1G6gW)V_Rz!q+R;oi(3-y-%W`@nBDA)nG8viZ3&
zW8(*m2eN^`3o!g0_iB)<YX@+?3;F!~z8&H#ZHxe+ALC~L$lXE=B>E)p(t&k#BR&Mb
z4eSqm6>z%|2(Y8q{he8H`%oud5X<-7@||5>Qx_H=5|8waKk!{kNttKwmy`PkgMfkv
z4vYW+0;46E|K*!|(wA<>_NKe>O<_Q9(*Jw(ufKx;@C!}1;EwIJErG7@ZNPDL)-LFm
zju3hjIWx}AYv{Y0zC8oXxBrK)ZO?V=H}<Ws$*ZsZcQ>}uy$F$ShP%)DH$3wKs{h05
zIN-F2JdF>%)6C1Iz-Mzg=EpSMdCw(ccGb7_Vvo@6d)^<10Y_~FA~F!P_X2VK1q6<k
zPCg)L$G85%ZZrRCf05jsH*=W1<%GZ(puq3(5Iycx=r4`l9rCAPkii{Sg!g|Vo8v!Z
zlL{)N>ANwSXb%Zsp&$56ID{NT7YY1>;H9%Mbl(RV4*^Xmul5B2AZ8WcPQWwv6RW-u
zgTU%T`cW(3j~}4HhV8AMs(v;e-V2*g0`PX<_T4o)0Nik|fiXVW3_}{jaeB>AzojUd
zZSUc8oru<H=n+xCb%d2f4Q?566SSn*s>ZzLp(Xwrp(WeYWSi7gD5qmWyN`4uyWY#6
zOXw{XE$0r4XS+SG@4gk{ZeY_V$T4-0SJP6din$^@T#z>!Mitt^yQgSevCPEh*56yQ
zymv=!YQ-+n%pHo&rA2Kt(izbjV-M%~ginn+&9^`59I&em(dD3-=~^}TIVa|oM&pd=
zk@$}CzenvJ?Ph=1H8hO^SUZK1yB$UJOI1zU(o_fjl*^bwQOG$;cd{{=e_SkBB%MY&
z-L1l4j<h8~$LrZL{>lpYGnrM7$H7aquAl3r6?<E>0*RaGLPE8LMwOP#J%o3PacM@+
zU2cl8@ZiBz=Nk0213c?>jTGNu^4Fgn+G+dLVpUFGASYij($Rx-LP=qS2A)qaV_UBO
z7#27Ize6pwaCFasE8Jx+qpwId(+_xb9gG}l&4Qcn`B!J!I32kH6qCDUON9cp$6?cu
z`w-?J9Si}{9qGe(s-#SoVQRH+^oOEc|C|0Ea{)`r8Od`Hk-SCWfm51Zl8uw0z;F`d
zUZ+{ba-oeOh0XS8t(pCEy_QPLYuXSIMM^II0bq`Ap;Ww`TSDM8IxF4FgHC^9C;=*z
z3Q-9+W_ITr0P^4`ZED1bwnMXjuwVM_@Tqv;S-STu`|GJibx=meHLrTH4~55hC}n&n
zySc~Z?pGYH;wwidopz3>V7dPaO0p3KS?L}LrD~UboqFu?(K{!-uqU-LE%%Iv7iDMd
z>?2*m;Hl5o+3XASx$7u+&y`QZWpu)6rzGr1@zK}ZYES4L65pyzvo=5dYaq4S1nk+Q
z#Zu_+-x>~70F=Zoej9}#5026fetasq4sRe|P5P=j2s>GR6gd(8YDH+n=ZQdbV+rc2
zhAbBnK36(Tq$vo0x@nn_1JN+%Hs@zX9em6^&rHI}ejQyDyNuCavI%Gd=$eIRj88cy
z^NaEUnVB;OhcqTUT`G5{txL5vNsvIxdU)s4XT9zX$PtFlpo;W(OMJV#4jJ5tPB6Q?
z%w=<6U&96Xvpt3sJ1w~?+QOA_Chr?U$rz7~jlH2mO}&c>@;ij+@!`=Q^K_?|iVx~O
zF0L=MgH^n~m^Gn>T{<;d?gJ@rvWI5X$Vquy&ao;~11@zIZ7IWn9UY$pA4=IqNBF;q
z!oFq!(Y<IRp!({%H&Kw~qdpbVJq&i8-@R3&RU;_-?=xjMX6_YXzdnnDckUI<?JT5h
zJinWAdoNtIHGAh?SV<F$aD^sRUec!^@r?b{&3Byf+_eK*DMD!~O{t_&ODSDHm@?%K
zOgrW(1oDY5T$_8ZGSEbpX^t;wq33HSNs`$liV}-L_xGbGggh(!dNf|@AGt`Of$e7>
z#=mI6n$sCV1cbM}8PUw&&r^|?%Kom$C&&{}x}#|k(gR%KVjcuh{27!z?~R&>=x4u}
zZGcAtKt$_S`;>bP!%QjIs&7eA(agRi%M6-d9tXl6K(zu-%02bc5;5{2lZ=8)%VP=9
zsWydNgeIaQs9ckQACT2t==n`fY~XOt74o1?C!+)>=ytj32$u^~mq5r4tE?=xmMBFF
z#J_U1TzItDjmI4{f|AO2%GmBRVP^d$@^w)50SL+MpL|&UcLoB;3=~-MO5sHt*k!h3
z*W~7mdKHr^ytIHFo05)N#pj(DLr}VJ!%N-u*)GNf7HfHx(G{qo28D%P=vi~ZL_GBn
zZ8KG~P?$7g%-eXi?MXPIK#lYmc-U-scQLK#>$$8e)N88DWiNO7*Y1b}L)Sc^L_h8O
z)U4X%hWeWGY3}z<A!ec-dxl~gpWj83YTDV?+?IK;4mu+u>FrSH9jNWlD*7#7`>R=|
zaLnol&KPijdxvf6%ehmCWM=#f)utB=Sti}e#z|`V5tYF6sS35fDr5)bNzUHYz&g{<
zq06#-M}&Wv-dXRZIi4O4-V6p4Ujn2=@@{?;m)rs2V(SI>zTp6}^X`digXu2Az|esE
zs0}I}6(LezpB^dgx8<{?YBR<1Yy6JuV)GGqpDXq>RHr7h_c>}qQ-&d~z&Wg${{X+~
z3!^2x$=heIi6&U9sv68BGib7khK$wc7+|?!Cb%Fzf0i=ibh%<y&R>4);yGSajcx-Z
z+)NVG{98MjQ)z^VmI1hv`_XyM*uzW?bZ*yyFQ+7aXSeeG`F6c75CstW1vZ+k){*mW
z3l=P{+E&h?gMRgyQrM9utVbAU=I!H7^aXzvJ%${vGmWy<ZTC3pgKeq{*Z#JXU6A;^
zy68lomc;a|=>8sQXXAwj+Iz1O(JG;BQ|w)IUsE5spJ`HNWXl^kqvMiRM_i24A17l-
zO*2A9eHnGag3%M|1RBh9t1v!pu-ZuWS559zk#f--b-XIdL5s?LY`Ym-Q<Btlc-6&U
zNlD??IVB1GNAK%jqrsX96SK)3n?%CF$5292tzRZ7;YOrTLV>~}9?((!4`W<DOu-CG
zwt9`al1z)2O4@sK80YAWK^NIFal^9HREdoU&y<Hub@D!{a&@qy`K}Wgk1BANOwvrQ
zZS#nqDq7}_aq}2Qh&_>~UFDg11}oDF_@DVHo_$_pU!5YPu4R+a+As!tKKZ*|5hbT2
za19Bh$NO0KrbG$9zwYp^CJ#3%Q;qXOH7fWeyzYp!qtDW(hPCZn#caaXhOA_GF?=n6
zr<!5h%~?h`Gq`l@xhG$l<Tf@Ygh<QvlTv9duajuxZxFruPP0Td0{c?r9^IW2q@2m2
z29`02@6|*emONpq?`tX<&2{$6vr7%5_@#Z5BE6S?-v_&e8+s<_guqS=FG#zj0hH@{
z3N1Ow#Z<&EG~{}@@msu+t!6;k)0U>zfFxJir>9-wF4GuB%#!PT{{8Kz`Rt#W%VUe{
z>3VwTK{<a=H9k?`MQXn_r9N!_jsyQ*tc;;-G^?>TUYUso7?jNAZC2^`u(+3r4zKs9
zXL0f|Cj<}#@5Aqwf$vFc))*(7Q)z*K2@d#P+Gx1kI=LL-fP~#;j@VRwrk$CHqw#C#
zO7=nYHWTRp>(7=`Ee$cbY)tyN_B8s>X1dUd)GTbB!z+rIfk#}pQKv<|mtsT^6bCvW
zXy1EFauMckM-6;`L1$?~hX-<9_^x&2!V$mBu&fciWT-enW!CyYYzQ68aAJ%8b10{%
z+HW%ziLX%w-r^x6_eSFgG@-w%J<Z%zd8_BItZE~fkaJoNHRN98RCbGoOYennC5!Ip
z3!M}s=9Pc?h(akeWl7jqwwui0YukjLfbz<Bm`h@AKn6X)53lTf`YI{|Lig_V-rEMk
z?q_~qJ7p+!T7X#@YS&>ES@4(Y@y4+-0d3w+J;$FAou3m`x!(i%iNu2>4BM~{l_uEs
zn+XCFVgvaNO|q?j4dfWPs3KL0z~EMxnI;6OJEmXsHE!ulYdVy2d?CI8qhNl;hg=*0
zJ(=hE{x<p2y<AuYJZGdO>#AB1JP{PdW;`oLzOdMK*8sFsSLK;fq5&Kb^g5#-+f-8e
zuFyI5Hj)(h`K8U<U*V(S-qdfr7+llI3l5zVx`>qT$FhvDH=dZKOP1x&o|d<FK$=-A
zYqdx|+otP&X~WhK#N<Tg>F(v-95v68nEXDqv35JvaR1vdK_@7l)3JrEcboJZT*X7U
zBb6H_OC2U;<0P5>yMfcZTD)_)%7mC=s@cr^(Ii<Gt#cKPAG96yt@9L`95k(Nb`yLZ
zpGqN;3cHY-k({WxxJ4wp?j1{d)77VHQ~h}`YU*vp?(LO8xU21H!!ZwIlu&(KeA{}O
z2=)9E)Z5IZicDtRc64OthWdMjw(_?)7(}W>3NGfy*lGvHu0L|RWjxhx4l7(02mQMy
z(`CtuB*Bo|4)$;cw6s4OxR^QcROXy59hAu0cnV@JDEWLCivlmZH_0mPh9dICF-cAc
z9Q=8jN(h+}9JUk8P!=T?@iaGPX7HYJO#KsTof6U~Y}fb2p6jG)J0)usQ}*`rf#)I8
z=^_V-q98Nwdcvs*birCZMB>%pUQz5y(x3{$K!&iyUN9J4!kc0D=47tVDPK}{>23{q
zGxl=pH0%jmw^)w^*-NCg4v;9H!E$-Y?qcRMaMpDYYVA8oAHN?c(qQPMR5c#N<0W3&
zgBd=#vFO;4hGL?5Sf$H@`Gt17zGafJk^~BZf&reaZx`+8QX4(ivhISzO$CFP)M`RK
ztpNLpjw^0VFI7AJdToE-m*MSiOPO4$2r=Y+2{r%^t;{-F8J({@*M&L&<|-LeUvKz_
zHwgB0Pa`?A6UTYtJcYD3@eja4I`zL^WnL+dYun0Q-zSeaADDY~#+-D2*^JBit5rX8
z#kHUnAa)rn+AF>^6Rg?9j}6BKH^!2P!j;}$=Xm9>*spcQ<>_d+ku*E>(KPQbC>L6H
z2MsZ`?iyF}a-^S)i`iv5pJqb+6b!7zbPtW!S#lXN;*fO?bsuO|Bk|_BpWPM+9^Kp}
zddo|xL;9nvFEBe5|B$@7kp$ld|KWK^xc|E|0A%3TpnSI<Njw5z-I=PUN0>5(KAjt5
zL=!PU85Aiti*~PapD5>(z_}ZkImz83a;{J<!nbPN5?0qPBu=xuW@eIyY?G*=coK!H
z%t`%*%zz@&ljz7=#+iPb(an0rGB7@qmyNC^q55amr(4sBsXTHtx}H@T0;I9JYoa@N
zM%vo)ik1G8At&<iju@^^I1+`0<yWqZ)+T)?)ai7F88f_zOiRQTPK(Nkx8*ymeziiX
z9#(N9Uh%{{%6Nn}{w;%&H=kW_POj3=G75*Yz?p?)S@^1aBi7R&iNkw9(1D4u%~}?v
z;D`*s_}<Z<zxuzbLpcwHW*gwh8gbM|t?e#-s(ynsJ`?Q~#MlO?1=8}<LBm+(U}k=>
zRE&DEap<^~-k$~LQjAFvWH6_)PW;7gg48wtIjH!Bn2x6t_v-e(qjl%ec)&MgR?r&l
zxjdIHg{Z#%GagoZHBtD*BradTyr<?ACeZ!D<YDqXT5@!3T}Mm@MVg5LTkiRc!U`mr
zT@G2iu}bhEGj8R2ecA^`W&ZLDNoHQ9WAb%&$2l`f>qy_M-3(%gt940&M!6+9>`OSn
zte)vs^Rf!Vcj9p6w^*{^)%NBUtU42#4}7#(naUJ-3=dcYB$mgtou6{yw0ZH;dR5E^
z2<)4WZ4{cm!ZO|r8R0D%AMWV%OkLUxZa6=E^Saz_maW_`RtfQvhw$Z(7b~PxZ))Si
zdrnn7l<nMnkF;Od1N(WuKh|CsC;cndpF{XUEJv=CZqxYMC+EpE>>&2|5A>c|8_8ng
z@%K~|Vfs+Y`7@+b5GPx^Zj?XrVRIZ^m$IUW6U-qP0mB=GIkL*uGHRZ(6?IJGjaq?_
zA1K*PgI=T6iKA7V4|Vn%U~R{+n`5PkAq(0yw#VH#qvtKTf1NDTblxo{Zka3pEs^vH
zQ(+0r-8(YY18W~B?sxE!z+F3kqWEK~DVb*Ah{RMz7TE~@7r5I|%9d_6%9u$86)q}G
zFLA8^twHW)zm8Yaf1{@T@Om6o5fG`~rNzGTID|u<Rq1IoX&l&?mP$KdsbLZQPRt~m
z{LHx!Tk3*^7waRgsVXQsmQqFvTu6DJ(;_GreIV#3ad`c9)v~%)vPg&JOdCey>)_wD
z7$biFCiF>s094W)RMh_0Ru=ScVFbHvL`>cx1Zo8;Yr3|z_!%BA>89oPkhZEym4*s#
zz!#RaV=4J)qYpy>D(*ZA@p+RdPK8DBO<P@#Up=w43)^rofcNA)hoOZj3*DbEb9xv?
z$08xsmUq#I^21IkMzaNnL_y}J&&$N@wNf030L^WYTcGyW(8me&PjW@(J}EZwN*;7%
zLuC{S1)SUyM5(t*?n=k{a?w%!_-T4998Gc_5VGV8$Y1DtE^lTlIic28t=+r_^23>9
z2?gO}=38I`F8HM+iY32?)h=V-$S8dQvP}jvX~+hL-x4XypFTI1K2#W3gUT^V$7Tj_
zjO&uFDzfijY6~Na#%bla4J+Wcr0X^kMhjbco+76bcb>{uLC90mZ!W|khX&F^-;cOh
zyeQ*8+3>yC^>2plSy=mb%Q_B2DAm(f=Sb+B-IlG7L+Nd{jmL11Ws7{qCuw`5Pc0mD
zpiO35T(M#HopAbaL%F<4<WYXenU&Hfi6-Mtem7kHSK2OG5xQ2h=j^x;;Wga;YpRan
z&Zt}B+h!nb^ezmw_3c)`!td*t68Ry>{1KdR$l!Gf(!Y;S&))IKq4+^qzE>$y-T1DC
zmF$8G&h)FVt81Z}5U9Q?#v!YZ3O4UZn~~G_TekzrMEuSIW7J6=#i|%q73fgd-jXga
zT8)EcgcgXPEMPIR<{P7i8spDZrDi-zhWge#-CXx0DskN38MC^lrSBBZW2k1;hy6<m
zTW)QdShco`>nH^%!i-Uo2RHBg6Mv#j$qsns&lGl3lZ=U+0!uP4RVo<?tD_KSYY2XG
zjIDpm`0<P>Jtv(#5*|YHm%)wDj_kvP?a+I#4?T59u;Ce={n`i!iRR|0^5RB@#AoOr
zmZt|fXb(;~2+(|WE?qmBk2c$Nc;gPngTZ%YZR7ODV2ehfL*x;JR8v7$Z=%Nv<Ncmd
zV?R>=_93cT*?ZRkdvo>J&g=_U&G8Z%II7Z6WWJlF*u_Jjl8501<2^lw!~op9vg%|^
zb#uO+ZI^Q_pSyz0aSwcU-utaLZauxx0yDUdhhZVN0)Tm)GIYl4Q<%zL?Iy=0hCVQ@
zGb#0!sne4oJf8;RPL-?TagJT&vEtZi|6>|a)qm}jo`2^nU-Rj_xtha;9{Lq5Os?V)
z&a&-tIu>CL3j7+GIFs^L216w%)~=??I4r;9zgpT+w6S+qLv^SbvcGwwgG&w^DBeoU
zk=5HUOr@*5^t%?;J>h*giI@}>I{nbAItm?Hfcc`GSPnm}kGvVtv}v!QGe+5lL7)*V
za(vA8!a*j02$Dz-8nMzBIDmV;D?v*;2&vTv(!(|5W;eG#dX?yJRy3Od?E2}%VFb)_
zD!aP=fGL=(mj|m}3XU6bOZv6j`MQ>nO{*uKb>C414W<gctUDXM;mjd=|6>0dOax?K
zvzElr?gsrDTFiYoh{N0_>E7<#fz#f|YeG){+xAw|qq1GmhKC)AS0__MEt%0bb=ny*
zPL3!U>}{_a=J0}p*07c2xX^~VpyZb6)KzoO-5*+sx)*nL;pC)^$M{+A63ZDV<6o8h
zQ&h*<d(7y2bj0q*>aJ+YC55k1Reu+xweGhm*{%>ux|h)Q?RaO$SOnL~mTy<_T0G0|
zMu}o87?lDa$vvNeD(|L(9l#ywhWVF-hu5l@oXI#HX}(7JL0b{pF%3y_YgF|~7usyI
z+1b}S={ngcgprdJ*#-EH;fWq)6KYesJKK&7szlN>W=gNqpdFjuo!T8OxKfGfKEulZ
zXt;zA^7PCG#Zq>pc!L!63-L2dgbqGbRAA~gP{rWV-~+sz99Z4he!S638;WSw3(@ma
zHkkQK(X)S8BrcaY#|iAA&b`FmojPb3Co^X`sD%>68<2DuF%e;}D7_M8*OVIWLz+^r
zdRZC=9k13cK*OTxMT;?jEc8mIEK@B)@6rdnI&v7SZixl5*TZ6*GV(W|Ph1J*4k<3i
zrrjJDUNI2oj1{esJxV1KYXcJ=G`W=`i)|t}R~>%sGF75~XnR!?8<8KMax!(xrLsTW
z?w3p1FzpI68fZQMMRu}0gpY<BF6e1hvA<(3Kt~Q2w&95~cfp|WYFC+?(`&KI&``%O
z&LjVdR=*gtc9)@bLK^lc$<k+*w-{sj@;3WihW61+rOsLIvaMgMX%$z@%r9Z%2W_IT
zrx1sC4Mt=p=CkpW9$G<8Dh#D35jZb`8m;ZUk!5Z3w~CduM5KXiwqUV+bSdUZZERj1
zq|E5v+;Y{R*mymnQM@tb=^eWS@06^na)7F%{F@c{{K$hcRu5QiC8bLEbWRYAJV-94
z6I;&TP9{N6CTNLR!uu(CDnEkr8X#h4nnV~Kt>el$^3W2b2IW)3BFCEZmAde#C16Ll
zu4*sv!1`zN1D-J0J7Rudv;NUaFnl&PL;<x(tu>t!VZNNbMfENO3xBpakArg>+Q2@5
zI7%hj3xwgBEQLM}gpTvH^z}%+=hBe#r+(tB1+%M7mykH$Ir`w}1Bz|B3{v3mM3Z0>
z3EJqz&v7EBi2Eu*rs+<XAZ;SWi>JAuHadEgT*czpXGY}^DIyBKEwV?p=Lm8X@e7WJ
z=G5+5*Hy>ux8w8^_0J;59_z#ClOYv}lg(N#mDbGEd<msq;waX6$}QFY`aH{dEhD$J
z0}ioP`@QTMiDi>?`aYJU<6FLkJ)#;NGVwRCI33LFzv8s)|Bhw;-^BXDszQ=NLjRT1
za{Nn9OUv}%cib2m8U8<kTK1o}`j0^E|5`)+2ble5RQr!Lw*Owp{hz4zf5O=xp*}P7
z&v4xTj%qoWnf@PYsGuqfznAGLNp%7sgr%4hgPdF?q#*hkdJvcyU`Pp~BvPCtC`q%V
z*d<(DB_IMMhC@B3JiG3<+Lt<iFTG9I->-Ie`o0?!FVF49Hw9@4pzJV^pwqJ}pb(T-
zRnY(e0KK;Q3-D>1n(9!@;oYTRrD)&>*oE{N<o^^<(EtGnubmtS4zhB%aKIGoYr${s
zfZcNb+(Cr_`2+9~C>+>|a|odjfI9?h0xh%xQrTe)V5VrGU7h&*X|5AoSG~SN=!4b+
zx`~PearlM;7vd5?P+))n{|$^~LkMlh?j`W_gTsLdCgA-_3Q(N@3v)s_+_iUf+y~?2
zaO_81LbA#PV5gV^yMft*5o{5n0qh0<W)Xmk<sWIWPXlaYQ#k%{ue6}f!3OyOT%kNE
zFn}C+q3;6K2N3|crT{x7E&`-SBdp+?HS)t{`gMB%?;IX|{u`(b=<@*&?9p!tB!)Nw
z4Auay$bCZaD-OV7pTix61n8yl<qWvU+I77pP)At6GqyjzX?H*n5Y)s5XaoM~p#B%u
z&ap?dux-y{+qP|Ek8Rub9$R~C+qP}nwrzW+^E7GuHfhr@UH{-tR&uX(UB`KxW(+{*
zp-#sx!RtQ(qYu<mIw?m6$@TW&;1DopkslR&oD@Jn-BumoGdW}HeY>^{Gd&>G6qEDk
z=nzuqVtBk5i({bqG%qO=Sx_(dS*#$C2p~|<chJxP1hN33+^fMSY92c9VxxA%H}p@q
zKz4Fst;D_Oa3F)&mY{7P15Zw1ZGr&e|DXpq9{!U;-MhaBqRG=I=>;-`4chxo_>zUK
zdnew3#Rqf-#{j-*9f1b;e*Zc#_7J0&;+Wh%_)iS=$eoOqx+1Ud?msb9CMJ7VKXUXO
z2)_t91c+TwC`h1x*KpfEvJW|6pRnI8s?Z=`6&$<#m`Mhzw;epbe>R<ex3$&3+uEm^
zECb-r4=C7I+%!UfW#3jiAQX^*8@|q;9qe1~!ynO4J&kX@&7Ye96z$6^-wX}k|Ez0A
z0WELtQU-8SbYfjS^Wb_m;8uM*u>O2s)^iOao$uf7sG5+R+VU3~EW6^I?4$02eJ|;h
zHUKT+8kT{I1lW8toZV<_ZW+J_2{i6b&+5rd>!2XK>NR&X0dH*aQct`7Mp1z{cdiKT
zU+jIyVmoL*TygIi7&h-$xb7kO0R0dkV!^L*39m^6{K#*%wlxJeeUb5c2Vet<Zv+8c
zQ@a3F?Ln?<82J%FZpD9oW%~bK0eB>T;@tc{0q`z-g>?}DVmA1R@DX|szI%jq0b)je
zx4EDJItZcMAU*ERYG1$G+!0_PhY&vpGqSzi68~mYe^@h<dHAhXHn1TE`1om<w^lGm
zJ$+*CBe5w}L+AMgQx#}bQ!Y1D-BmWLQI0GV8riQ~?V7@hdjyWXe5F@Q88bv)ch3MW
zyc2iH0R{>6ka&|=0wL;mz`3yeCyK~YP_g;>cBPQC*$W4e52s9QXpQcGSV{RnQ6(u2
zpL8tHsL=22b7tc%k(le?-Mhe%v<f%{K&w46J0w;`4CSHpX$PY%$*|;(50B$8ZoH=8
zx3boEJ1;XRQKSj}tGqbmUaF;>8M4;#&)xP6!F`K7ajGV(HI2r>WP8;-2jd2xMk~pg
z>%};Qk6d6+dN~=!XBA0RW4+47KhkS47ztzhE43m_uG@X45*;*pFN)<pTeG7NyPe$x
zm_91o!M`LeCg{TTxb#?egZ?=<jAt@aH}T7Kwcoy}&;@w}n?#7tG8dUO)PgR}7QOAP
z?rCX5okKVD13v1=9Oo3oERr@wn);^dz}TNQF=VtV(yqV@BL!<`yG}nvJduY}bKbZv
z{WjC_A0#EefqBkDts7!zs8>qEY4*?2hl%6yXE_4BN;R_~R$q&?F09a!l<x9K<BbQ|
zQql6l7*XiF37&HNsz21Jy>0WkU)kR@&X3T$Na=-HuWx>QW2rIEU@UcHet14h@Izmc
z1Co^*R*1;gYzV>YNJ4S;OchM&)U|YqqlZr>XyF<h<XBuK!X>e^)6V}+-CSejI+o|6
ziGTF>v%F3&Nb>6``cFy4>60abBjub%X^ou*()^MX1<DS<V!E!AoTSTF-BUdWJ+xa5
zo-)M*LM<t=Y)&^!&h|q#6!kXX#rZrC`1oq(dQ%b&RoTns<YdWxTUYJI7yS6O_9NyU
z`}Yf^EsEsFAH1}wO{D)#eQyh#C$}G}QPGb+(8r*+>7ic!Gh^`U3H(r(@Cp+!Ky69z
zv@ovDofPxOo1H7@x3QkFCq3}NhSei8?F}@(gB;LSHO-Sa`%uym$>bjaZhTgOhVGk=
z8{ieJylB#9%ZCqt%Tdeb|NVL2+LXhYAm;`0b|x*lE+E<HlsWqZ%KvqG18O!5gTKHE
zKU-<&uEIDnROScA&RcbA1VaqcE~+egC8WcwoYC`3M(xX?uxIR6;?d|i7Fl(Yg5@T?
zm#4=%hMV$@Z&92pgNV`CU7wtf-su+mRx|C_;OxyBC-YdwGyB{*&l$)k$E!ETcpBMV
zDjGnvEgn7$%J_AqjuB0<_UI|Sxd)UwX9^<>32-MrOIu!%$J8xb5v`uci^O%a4{5=}
zVmij?We7`<%{r%eNh=8CSq$)b6^+<--oNP-_DLo5B>`2BI}4<+b;KO_g?h++Vh0Bv
zUTeJ}hH#?V1&|$xSuh+q)i+lLW<PjlLDz|w<%?PvVk$2QplVFgJIlr2I6GhKZ$4z6
z6quzpb;8RgAMGNiX!z<VlNZV9{J53!PI}ozd`u!mDWc=QXjE){u%CXgM_ZY}%|Suy
zVMC^AG7=F)2yhzG?A3ui;-XB49QYSyoaV^$zB*GUHJZap;3^#ewQ_MIt@%B#vW=(m
zc<mTHO;VMQXaw9<cO3<#!|5+-l`@t-NB}nGuithJ*n*+ttADj_L8GULCX)a$%1;7c
zhy9zidV#v>Dos~y7#h7vIcrDe_e|k|7%pWv;;$QbMcbm|8obb9aqxnbjIgyeL3@O#
z@98tGyk;uSu@NYbJq59dI%B(ht*2A-n#r9xid&cenYS&U8ybqL6$(+*l0*QZZ&gqh
zgo2QOX}k{uCxroK28(n2=XM0T(EF!g$=E}g&|7k8G;iD1z%PG^c`zlP;@WxJe{Tq5
z6Wo;ECdGN)2E*f5&dQWE<6Ie}({62H^HOz!e)w^PM#@yLU8qe$Y$Pw!AFkKGGTW-D
z++kZz?AuQ!=?;a;mFSEF=6qvxDJSB?g5yQ5e2Ht`j0$&0+{yV@!}K~fQkDv+L7J`e
z^z6`hEKD@$+PyLxDFL4^#Sj%?hljO`-v6u*m$+CRUOvzpwY2l;ZL>?@FO}CAt!=RH
z9`8j~MY-1l;R0hqc9qH;e8tFoX_9KYD-v_e9{A{xnS@N<^mALz+&mkP%tw>{5>GZ?
z7wi-|c^8CWRiR)b`}<ij=L?*d6gyS3bxUsN!%_9g@qxuSHGh(1Pv<rb(#AH^;<;<X
z4@N^gT3>pJ3hOs=b=}FyIN}J1piKfDvn$jUji$DYJk--9&Z;W^A^KecEM<a54k=V-
zA{C~kRp^@+E%s~UEuSh~N|f1gnx0MzD%V?zS7wA-a*kWT{7UME{{%NuH(&=C<Xh?E
z&eAB{ChSxtFH?6Igy)I(kuL5O4BwN;iYYA7u}>>`e}Ly{Vwe)OL;!Oj(r%AU*0g!2
z<Znss?N+!urZA5?E&?()DR+4crr_(E3NUU+d^oW_>_FHCyM=;0ov*hlnfQ`NY6U`-
zIT4|ESJPJiwro1}MII;gXzI}-CN^Y15XrwN6PRxVj6E<%-KYXnexgnH(>ek>h<rDf
zCiM<iQ4uL^0+o|@Vl|=HxVMgogpA!WHF{c~*4_mFlg|>;{LQO4IcVXe9pEp#OSdsl
zs!2`zG*5Tm%TQgGWP>-B`3c<=d202dS94OqzT)w0smT?Pk_tGQSfj&@!xT)GF7qx`
z1qlw-)L*3jUJ)Qb#Uyh`qs9DG*Xn9nTlC?4IJU+z;D#Gsi75yMop76(&c-Ty;j7JM
z#T(~mg^n;2JQX-v=XAA%(N8NX6tZtAWR^W>T{2yNBem(EAp}z?s))bt-XT!7h)>}@
zthE3Xgy7Fsl0<%)(;!OwxAcq45`9nCt$~Eq<2;u{`58a=Kx}SbE>@Sv89L-ho$I%4
zkJ0*p+mAXFXFKV=bmBl{K^7MmvXs*hav7Db8*?|BYQRI#koJU=s&ulTt!GFLPvhJq
zR8rpi2h^|k*7M@)kOt12Z|3*fvpx)XgqlfDlR}%<cD%`}C0mCTOS%_3cu#>(AYXm8
z_e$~jr{gTzHOPlN8%_&naCM<5)?H#Lj1SJZr+O!e6Ej&-cXZ3u^Bs=h%CQywLf+wv
zJy19A0bCYUndBViLV_~H?(q;6RMq7Z{0TX-*n*$qlU`wZjrgh=*3~e9stpu6qS|?m
ziqCL-3{Hj?d3&`;H~83(kCIH5CU6*D1)2VJ2;hNKwF{I(Jj}@am+s!`1lcf}C);z2
z${jJ~XcI-tAseI3BIOUD^tu?mib!2E9t#?;k6A?V4s4yKS-3R-w$qB326h1dbm=vw
za?FX`9Hb9g^b+CQVF%kj<HvGxd06fQ7qJ>8JIqaL^jKPnhLDk=^gXYZWo4<d0Y9qy
z8PQ!^g5@A6X6kt_RZ{LbMUdqi49-Zuh90lvy$scQ^(&huz{4LSR>upA;51CQMT(Qp
z#p9*c#q`sliQmDkWaxr^A_Y8Ih~lQ)z8%!pBq5u-^PXHXR$^p3H-2r#KFq3wHYqEk
z24yyZa$4;?7GHC$NHPN$!#j9bnCyv8>64Uidw!-P6k!P^vQ#Or0*;>D()N=19G93s
zB>b54h!-79$Ne1hea|&a(|zY|HM>GK%xLwY&m~UK99OIeoBY52C?yne`rwurT$Cd!
zIldplRn#VjfyeGGFW(80e!X!9CWHl5yVIt7b5+4m&<h0{8_mXP1tEiw&ty6;=PaJ3
zkO~(Gc9qKJr!Dtnps5El;TEm6$)GmfNur1{cK4$sjw1}A_8Ver<dzBIjZ+IG)c{b%
z;wSg1emMs(215IhEb|G{x70-&DeG*xzd>la*>#7?5suJc{U}RxP<hjG#}lvMbxZb+
z7?;H%fHX+S;IKItg7enJ#rK=!4$uAcUTT{VU*l8k5Q4VIE9aSQUL;$ulYXyilP3%K
zgmX(#;i_gkl8diLbSXenPR?sn$!Sh*!z98OvKV2BC7sJD9P*6=WKg)W!20*cws3C9
z9aLi1WxTo?f}mPy`I8SW%Gd%R=!X&6>-9~|fM-eF0_$7kI|k5jv?QV=P>rk6X2AJ|
zVFwoA&OYXI9Wv~?E%4c`k!eRI!6*|)VM&sl-OSo`#3pn1ZbXkJQ4NbjifS5m=I38h
zNh_2wBV4qCRo2V&VzVg|W5z`xmGKUj@JXMGcZQhRx@pOWEb$X)^N@e@W}F+dnq0|m
zfu4K9KFIyh)ACD_rcN*1B;V%D+jov8NRHpDsk{HI*dx}=+WMv|mKmAZClOFSrm4$p
z?;+pgrJdG2><zZNh7L<*B6yR^A0wsdRpV|9CVi@072n9U1;3{A_*fJ)1iXkOZ$W*W
z(|-xmK(6DN3Wa}(oLR=tX!cPpjiq`NijrSY-mN8PzpcOU-xv;x`w(qB^!Uhh;?Xhb
zf98I2+gSjPk%6v6;M=pQJ2LTO2G6dNm{(YyDg?QqJFF=2c_`eGuPkO^s&#X~5HF9f
zXLKMFun^K2F}deYVU-1-A0G2q&0myLjknc!kiZ6vWtdAAEg=|>tfFV%%?6f=A=(L`
zvmJ&Xxyw`ewtO$dn}8#%Jy3wFU5yf|KMa=%wR>G#Es&85ZHNG~#3X%_M$RUdF;YWo
zBoeZ~4xAV&D9L-fybn0q&34C*-uAvscLxvvRtvw^C4m1z5HoaK<hynNaIXVyE?I(0
z+TFC?f22vfwqFCVzpKlN{Y#6an2I6H4LTjUi|OFeqlz34K!a5UnV&vT+^jDIZLYmt
zM-Btw(fP%p%b&?a-j<n+BOveQ-VtD`)?(_N#OT&%q9WP{cKrMMOW#pyzWz#J?XOpp
zi;2u5OHqRsJ;MzKXeX&5581En1&m<8N1y=R60U`i4mPsF$5RNdlobyldpQ(Qz3yXB
zYdj2~sAW7LLEe5@%MuzvM^QzH2h89TVMGFuidQhfJ0?;@_WkK;Gs(09->MFxMY(+v
z3b}d0a0g6MTUrKxgJa6_V$(|~V;?s@m|TMZj^4OzPDJi<&1^^eG;<9nExR19*RC`N
z7)Eg*8#!_>4@jz|0z%4ke!o8n6eLN-Y(%EYbuga&(|IVDr6ymO`P2Zn_zng}iyEyC
zSjHS--UY4M=vC7l^UYfZ3J6)roZt+=_aP+oR6~-k?a*snxdHx9*}$bek&K(JyM!GC
zsXJdH@6G10eDeNebr$sKi(S}JGNcUOOc|^}EK2rixuqvpF0;$%Ca`l9mVemXBL4EP
zK8sB;;u$m<-UrI~3Nr=WR&L2dGpZ8M1)pX_2Wo2Oe1~Q`NKg?K%<&~p-MvefjP{sC
z-I7l<?WfJyG9BYe_QX@Lz-|e|+21m3XGUJu-)duNo_%~Hb)H9jq#7G@E{loFqYph=
z-t4=9V-6}OIig)2UwVeL(1RXxxlQ<KNhCxbC9vB!9FrF?Ef0g<YiM(pBwa=05jjE*
z)Gm>PiAFl+To&UZ%(mK-Z9$&8NYMkLr<k1HET2LGwEfRy>@xi%&kwB};R-?5sQm30
zEZT#pI0@;G(rC~khh-k9elxuh*UINw5#d{jw?|{HU7LSp=e8la4kJ(mql-tY*}c=7
zjElt`!3-yKA@ICS%Sr(Eg~7tVxoK(c_^8PjGce-2QMw>TIGYEC4!xMmS9>^-2DHS-
z>T&owujE=yuSwsn7Z%<+-WWVNfd*A~|38K&d`G3}Mz`lEm+y!f5p6lyeXa<Zdb>!j
z09oIX^2R?+G8>%V=jm*8rc93i2H$c&N}=>}t2Lkc6)Z4$%zZ5N9$_`T1!Qvu%V-`<
ztV~lJLWCBM6Jn^8p_l^;B?pF~m}HUN?1{AlD;}N_#zyOxAs@YeMJQzJj~}E~ErzV1
zUM=`@NgLyd%UQ8`sl|JDH%DVT^gQ}(689~W`tS3Zsl$^~mp6*Jz@+(9`JSc3G3!6f
z-c$$_V5))>QQd@_eo}g#T;Ri*L{6P1_RQUiXhc<pjvA=VC6TyBBmG#Z3771B8W&5i
z(}hBi{pA>^oR}io4QOS>^P>IOO`cRRWHa<yO5I|INm}oTBM(4|_+=gjf3~CHSU@Ld
zFpqufjn3_c5u~0WSB~}6K$wF&!XVM#D|{D>zb1*oPA_Uzh8j{Z!T;P+#vvOC%<f%u
zp*&V_VrjRgU~TKZZLp_Lyy;EkL=BcdydiDvp9_w%#rK)sEw$WrRJDKE%_>1;+`(lN
z^G8DAuotFADy6RIbovj`z%O`$;dQQQYW|kpr!az!U*iYQtvnb%y0<Ss_COIy&>7`Y
zh{v|X({r#^dFrW@*OT(aUmtqWi}Z`AJKLRp?Oyp*VLrfZgr(PJFOCZ%W^AhMr+Hd6
z=yB;x$-?c_p-_!`KbP*0UZnLHN<X&B9XyjXOL21nG)~61OG!ZBYk9QYDt{OQGWRki
zHSMdkWaV2lilYZsPOX$uY?$wrr>xt8T<e*ns7Ay1$$FDr(MVcM7s5-YH&?WIz%(7o
z!|VN&=H%EIyiuwES!J6*VNfpSR8qC|nRM~L2ig>^f9=<Otw8;Nx`b_q{ezNt(#gDh
zeV>8;Hf&lTV#D_*X9;j13b9<cFadR&XdA&)nK2fsUAwvoBd#WXkk`Nnvk;g`C>`_n
z2yD3_tYD<?D!Zcn7uy_VJl<lcEb6FeG_|jSk$x&zeEU!B1;^Iz-gZ-=n`927`L^_a
zI+>M{z!1M+7_P5WxN6pu`Myl|V5)*PXp^mRhIF2lrICS87n-A(Z>2=CsOjK(^yEx-
z?!*>sDSpvIhF1rm1EhAIhxF`z$*YEIi}vL9un0YBm-Mp@w5@V8^s~+DI(~H)($m5C
z6uHV3Ef4aqrOrr9$T2P+7P<J9g{^{K`_xndXK*qNEBi4H%EDvS!LX`IOKkOD{jJvO
z79pLWAi&kFFr*twWrrn=9c?8mdY@Bbyy@s@q_=H5!+g6p(kG^-+u%lzQa>peZ@E7N
zvnoujZzhRxfj8{ePxbYzh_9nQ5l0GAun)&WyzQd>8&7t6*gjCrJp*bPZwRexW`5nY
zC&wB|+t1ex={F!6S=i%$(;;3=gCc#pBvwLcG&!JmB=xz@iTBS}ux?;POeDFE^GIpY
z(f6zSpNCWyvYgT*bL!vg=aG}TR6mW1hU?~)LY<02ovrxTn?%l;*<$&)x%Q0yWop=y
z=(!&zn5_R@gc*vdLFJ4?az`qk>;8VVKL1d?!edJ?+jZI4m>eM@BxSM_GlkZcdSxha
zy^4g+dg8rcZJ7l^q1J0g%L=NP9BHP0O-H26OT;YC;~nxaJ_eT6az>!Q-ac`wi2{bn
zl5kwVTYyo-f7q-|xK#PI#i>|cZDfpFHuPeZ)Z1V;%CM?wJM(vvZ~5(%>D|-#Nk`q@
zj-!@JdxHq`LMWsrg=&5kGYU=Z%Cz<{7CiNGCscf!nTW}SWEX2W88FPL+`MF?RJT26
z?sb0!HWwvg?(6*A=9n@|Nu`qKEIvDDlg`ORzL*Y0ysdzkcK6+M?;8LNzBC+%v64|C
zrik0_pf$|4<)Ttv93B&7!zX!rt($%?ehs_!D}ZUGGG60$cPXPXDjov;2G~Y=36^nE
zJz;$i=#J(CEO|rKN4#YjbO?HIOjyx?eCzJt$}Rs?iH_sjDW9Z8K0E60P*&H5aO-g2
zTWamQXmE-W5fx-(qME-H5QjzOvssDbuie1gnUpqLYwBP9CH7VW;Ef{nT#q7QlHsO0
z+%SnDc^1kqJ`SMf2q4}&C}3=9X9J5_g8Dc`?wa7dRsvgww!S{8^-L}VU#YYn&&9ST
zAHK6Q!YaU2HnLL9>a4Hb9$2bKWtMS5jWsFwh#uknJE^=~aJF^~D=jRY;;K1lOn*Fx
z^wL=xizn%7D4&|`ME>R1nG#5wDcTi5AO|gDm#;s(kcd-v$m9UMCaXolY2SR-+9CyZ
zm6|zrH_19uU8X{RdLIwnyg=S!hI2eAdxOk=`-7334J!`E@S<iD;J}P}!>m-!mg5);
z%V(;&NGfBQFJ&7Y8JvjuphKyIa49cREq+jJ<b_xlpAJ{zycj|420kM2%GKW!24-dF
z2ol=h0IaVBeF~Remv{<O166;V%;C-(&JxND&%zP<vO{_cN;DFDOzHiB$I}#BWYUrH
z>7&<|@xnG%89-<ZjUc|GlLEP_Ylsp#xPZ4zk?oDb;NGlg**S@KkVEiLQ13Pjv+y_3
z@^zADYo$kxyKVnYO7o!{a4-hCQg>@e<YT3n?X$hlqNX+^Kk8vxq{WKRcE!Y9MmWW-
z9cva@s%A1tKv2Q>vfUe`4Ml&lmx)5pr|*pi1!RPyc(8EtL^AwXr0=qrb^yk+y?S>U
zv;{|Q4u8(QT|+W{KdNAQ65bbX<8PdY^2tBQL>sx;5d9lpVC|9}xnnRDno1xjs5d;?
z%r6$11%hk6%*ZKZh47lNnui#Tfhq4e9)?$xl5dmkKIW^lYHDB~{M|oU37~n(5Hbvu
z?R)I76fDnqmP#_i${?O{T4x(2Bj1DHN&#?mI!#)9-!;-(uCK*t#{`>9<C&);`!?@(
z&xMBUq^{#ZSieL>b*c%x5G^<Ls82d|Nk*`qUF_DvwAtzgrf+j`(vG}zSNkqjOK+{k
zH78%mGYtSK*JH_Z2>D_;DT;rgdxiwmgT`MX<f7%MidU$j`bBD!w6%~HQg%d`*_l-H
zmZ=)SO&qiU!4LBu(zTVs@h3X3O3MW}kyI8wklgOBtfP2kBlz#}*jXYN{*h1zn^oDt
zZJIGWU(ssYxuMFC5ijxD^%tO)P$x{e>SNlXVIg~zG6R!k0c1)=Rq~WJx4%rqytU#n
zim4r&qQG4p6bA%lB#SPrp3V+{1NaebY_1zh?}{Uo9w(q1><c+}E#^S(CNzr%FA<j5
zU?wQ#p}RGsL4qMM9ZptxS>XpQ=GsG(AR{fB3`ro1l1QTag?|!uVRC1+6&y>OOYWi>
z`>oBch{1KatVd^+0_c*`6=_0#Cu6+PzqK!G?8OGt9Y?JajOP<}<5E#gJM#Kyh17y5
zl#T>jbYUsQnqucd)7%o5(uA?{NE^nl3CD{&TD7JwTc_iI2_ps9<1@WuMry~5=k^Q-
z4m!5c{=)Kwhgu1bjMJctPw`pDNedaJ_AO=rbwTU_+Joj4;40lX!u{3}tH5L#naquz
zhm)wOOjq=18o(&qSbH9SbCd>eoC<?&Ls~6Vjh{C-C!$yMuCcVmlXWuxT-aKVrj_o{
z;`k@h{n|K~kh5X)K;qvueJ_gRaQ<R2DUPYTY0^#bTsG_cRj+s4eKnv^5cH&1v%oTZ
zX!sINFpoLEnur-{)+<kMMcYYl*{hnJ8TF&YZ`px~VR1zWDjJN?%83}QvgF21XCSMG
z4I>K=(SVp(bx{<c!|9QJclA0DVv0j8gFT9M_1f!&^0hBWgh2H+<v@tRVWd~_6&kB$
z5A>TbQ5B8d@aAurmnO><3WMZ^R%O@z;M%ayUc%KS4WrKEecH!%^i{A&3-ZvopFwtw
zAgDC|VKVS#eo|Z`PMC5?XgvuGw#Fxu3_-)wWEQF;vfDzBZM3<37;qMw^ipCR>d&m|
z<g5?EoSqA%ZLnRC&8T(Z@(ujB6n>H=&r{EN(|luMSBI<fjOlNY`JLq-!@%DuBc!Sv
z{^rR*nn4NhH^D}$8H7bTl6E|9kbc)MX@3ebSL2jYE~79UFhYs8qT!+jd?~rdnWp<E
zyaZ3GD+dO^i(}sJii{$^IE$<t?jlo@=ApfX+ApA5Oe!zS3+s9<(3GyEajHX9Pwacx
zi+1dsG`kw9e!v^R$T0s;cn9Zy;UE5Ac*k!eu#l|mf5$sG|1Wq4=l}4Lv$OvH!aIId
z<^LDFLx5iV*F^s78mD*v-xK-&fja&-%!7^LzZ~;_VjgTP|HU@W$iVTx0{Xw2_Wyx=
z5HK=waI*h*-}vvDEv$c~=LG*z5IC6#n;6*{n?UjLK{-18+R$yF+}5L7K;<)Rkm({(
zX|M4rTR~iZ9|%?P*xhT|YsF1mC=qBI2TH}ws#dFriM^zoufrd+>9k3bne(od>!ugx
zyaL4)!=&0^^#Ef+eG2N>C?N!bqI}3h0DF7;ItP3E{P{VH4Ix|s-wK5@CxL3)XN1~a
zUk>rM0GeESE(_?>h#V*o7<ngG5P&v-Ke_@0IRxO~ehGm>yL!<GY65<ZK_OkNe}Z6h
zApiq6<<ANUrnjq~n_Xs8jJ}^B^lJ@406;-r)xTlD#X0=7uc~Nz0Z^e!aYxUNgsGv$
z{Llu+@j#xwYfxbNUF1s}2)H{+O2{*U60orcmsAtuKzA+Rn}E#&Hvd%P5m@!&`vI1t
z$>#RN*5%CuHeOwBea~A7Sm$P=RTK0^^v`WWfw*{v*@J5ZY6H@T05_r}0$NTJ-0Dq_
z`*PL-d|$%?jQ&cqw|3@yN$SCWFs%(tKpp-z19)x(+5oaWM-%v^S2AHQLXQL00%-m0
zneUx~xrepf*4DZ<)_OU-HvEP5tNaJnyAQg%bGuooeU+7GYi;`+?fb|0hj>aV#iVyx
z5rLc>0pmRQUCev_xtgFNrP;ILhjpz*Ka_m+#jbA{&)Vhz9Ky=g%muf$D}q~G;N30-
z9re7&xtV~%FC;K9P|gn;;4h$GXXWMUk9f`170mkr#HY|#K<~~9<~~fVJ0ReFOABDv
zXYPBuhMOONyqarl&y6qEx0WC%5a9B&s=OcR=rUg9`^_c2W$cvR{lu-#RTqFL4*e!D
z5XblH=Y#RRAiWjD>fFc9weNI_j*g0yq+<9R{ov0k2`Qm2fS#%?8~`;rJUGBB_}Hs1
z-sf*HpC9%W{guDz9PQZ`{RJNrwU-USuU8L4H~obk2hVpQVDBc$0Oa!pvOE@@E)W3b
zr?{ID96n@e0{!;K9q=3G?dRYNU*p?u@n<K%cncTzhlTmm;^!xoHuDm<>&L|{ub%tS
zgjSh=egg1YPmOn0uc8vDzS-6FU9SRYao>cOq_;Wxr*UpuW_cUhxXKp3ss1BE=qJ5j
z%0tYPK^%AA^k>8bSl=6S^=DNVZTj~H2-<&Us^FK&P4RxeAvWn%MY!Nci%AGU*WWw0
z+>3f=dPfff--Y&Cledap$yo%eZSW_YO+n~|=JEA|9YlSK3A?`sTrK*R|DwEm09tka
zi352ByiEQP=g|gm!TvyB{*eMa+y$)N_z8L4zkbEf2k_fE+u4U!SnK-<-J~B*$++LS
zk63y;@&Muvu+IHi2Krh47RmQrq)+1DZtwUKyHAn)R`T+{vwKNdt`p)V{4wz9yZpYf
zrZ@TU)BdUdQM0Ss)X|#`{vP{I8UB&_eh3Ek@BE`$3f+J}oyQOoTpOJcgH)JZxnV@4
z<h0>|8Cls4U9Goy!w{>8c~LL4tX)H*7}8#s%x;3=6G5-Y_m0=hN+jYVyKLU`;>jSj
zn7L{zmW5A#`lH}QaaR_D6BMbU<u!6MHzddYO6!Gv$u83Fh3@JD^CY;5J)_H6Bj=qY
zMU2?6fyM3IDQOO$lu%j6#W>t&vfu=o%*iag7%H#&*O)AmnSSzYM@@;toj;or6**1e
zE5@)*(hGKl^T5{3OSQa-bBim?A`CWcbkFx7Qo8dlX;VsxB=domUgJ9F#S)n=Z&Jui
zLeF%_aN}UIykR90tmSdhGm^#&)b^+ZsQ*;r!=WYqf?D%-QoC&F_F{c1-y!cw+fLxn
z^H)o(##QaqVU06Ch6YN;&kI~U2YU$@sM+ClgRP?)-2Kv^D^}>+jcNJTy>?WiMz%~Y
zYI*@_9AC#AuBnSONhIg10*M=Ru?S4LYfwkyQFzbaSr38!49l6)NepO<;%JX?-*fJd
zOUS)9_QD<WP-DTR^wrK6#d~rZW`y*PGhXp-{YLwBS(&;TEj7%;9gs&LRea&yXUWqQ
zEDv02!POZ5W!)9*C)3`ap~z_wX!h;p_e<LeRGWELyHJdC3~ubH=7wGgw}_VT`j?0Y
zT3%43J3G}-y2&%KbgJ!+d)OV6mrKs0_~Pe#da540$^NCdE@9JC@B64p7jVzAwM_eq
zG-qG5lEKVNk<2o(9*?IiOM92<u}<WgQIJ&xpG}hlHShFD3QgrL`OzQe15NfYPs%%p
zA2<lP{bLKcZ~dC@G63+vg>6L|P3LI_nLT_tAs=!5h8#vxmi#P37&XHsJ+pauO15-6
zNLE#26y<>FYDfJil$;ECoAC~IY14EzWP%kjIJJ~KqjXsXS1xQ=r2f6~%x89eZWkhe
z1a=;s4vuH8j@bQ=1E}|N`Cmxp?FZ|>)dRtD(XY5BUKQe184d$f+je`mREP(%!0PL)
z7Im&%;1mbk@SSDu3A%qlv3C8b=HV3vwVs`w8YtV89g$1NGa3FpiMHX_`r0IeFewhL
z{<{bv(7G}ZBOb7ZnwdR-Wg9HXvf3|q97B;0s)JfKS}>~NkCM`-FkG(d(mb_^Rm2HY
z_@~<6d3JXsB)wZDRs^|>dxLqylbM#3LnzZ7X<G9dO~`F_AwfHlfOO+t{S>`ibIpm7
zPsr2Eog5v^W`H+KlPLsvSb8fo?S*E$<I43L7Rj2?#jWSonu6D=Sc4a|wS0eNl(G(8
z)xM0UzlsHzvL^XSQaE3c@}YM&u-%1uD}s0&iQ<_gB5OymGu5i-sfg#c&$;*xj7FKH
z{(uLGx}QxC4x{{`wj&B(D*>Q};yPeWSs*x=$#HF6YUM4Bte>n#<wN)3`BHxI+vzbv
zmFce&)pgFxcdLju>s8=Iy4joP5KUyV@>&4)bDVO-eE%~gLh~0kC%%B!B6qO4QrL-M
zaCJzK;zeg{Wj`x1uTc9MT^Z`{4JlY81?hVzo-AuL<<_I7E~vAn=E%oa$GPgValdnc
zZ2o}7?5X_Y6aXYs-Tde#wk+2M$^=)=sMQeSzBHPvxSu4e8GYPLxu>G3&mjx_LWoPT
z;$ye%V|kdFzn#&Mm0_2NBqfMyr0@WLwJ%oA+OdGucBqQ=@Ym^=GrZ^`-{WU|=Yl|n
zU6m!xQo0IHbEymKZ*#eD%tFN9n4KWWHII6i1eCuRwV;b8F_?)ch&4aXuF^wsize~_
zcWp(y-MBq?5cwBXZ)}YJZr&W9$V(s#>&;;0t)3~0_1S-;+G+%6PDByn6ssVEf3oEo
zM5{3#kGtT`w6#HFC-PH55Ge!Q8|ip<lxy*1y(=e@9@;3;IS7z23+2`gG2AcZ-D;J(
ztTLar6bAtHjRE$Ocq>gaZ}ryLlU%wn3spI}?9Wl{<}$)c38|_bG@A<#)JNThoXo!W
z5XxEz+-cLcj$P%V574=sF{gn9pmZQbX(ddaarYzxknuJp%e2Gtn=Wv~@-%n$WLdz{
zXCB^bdh0_a7Dwk8Fc}O!yVD<HNiM5(Z3Kg9nY)-dq{2%6kn)E@rDI-SD!1c3uOn;h
zqdVQuCUu!@=azqZEYPo@&1y>;JFxB=0D8hjUUHg9GN-VE${ZJ?7Ke{-wrc0>9fTux
zDm^`z<=`{c`-f6+L?^YVEN+G8Hxc}E*BNu;m0<eO(HRE$XHwNi>S>r9u&}#SuOx#L
zpc?S|TG!VB-zMoTI<7u<QxHpoxNi|8BBfzQon3Sfkm0m`7RBj>R9Ag}ZV<ElprJly
zk&eAt#E~qr&eFYAv#w?B@(|JtXRghqfV^Q9I`C%bli3Q5PKXfo6x;4G?g>>ghub|f
z+M(3!)Iy2(DYvmAL@1%x^z-PS;h+#6b|`WNAZZu$iPV-<P_yXGGQQhF0ppgaM#<2+
zZe<q*=JZ%56Frua6K%4|4XPXKWNNy6$x1wncJ$u!5#LqV+Gi367TDORP3IpO%UnXK
zT<9K2uTR$A&5US4+rkp;jl==`N$L%bu$8b{6Ne-in|i!*$jflnM#C5&uY<-cD>iRY
zw@;SlTnSkvf%~YON2M(<tYumhQhB#(49Cd<s0yh6TF5XCbJv>B(HGOTTNM{cbkk@m
zhBhEgyb^kvGi>63gv`IeLmcz3NQe%ybIGuUX+FU^Lm$?EL>p-l+Hper3qlQ;9eT%K
zyHT)8FWp|CejM*4m3RLewiO>unLKpp%lBx|!8r14vZ?&zk95tN#95O?>Qjs9fNnA6
zsOeG6paqzVIz(PhfJDU2bQTwk+Q@e<O%?7}MMY@JXLevHM{kG|2+OIwfQpy4K6lh*
z?1fX$({R+x(pG$tWA-5cgMw6pt7T6v>vufyd2O9?JxQJcz37^fFK9;N&nFax>{J^2
z+}`9x|0)m-(}HlhCSZuen_IaO^Xrp&Wc9mBP%^YmNU46gDHgd5!?8rh$^poKWHO71
zpnJr*4;`GWc<cVlrY7P4()hZo3S5RoxvN9pIIo#`+@6VV`dMXp=+rnbw)#n#mp>hN
zW;~+5#tkOi_=d4GQL3n(_mo4rEVs1vGEytSst#`@jjPcARRmg=;oWM`nsjTMRRq*8
z^Y`vlk?DA^{UgZbLND0k?3Oit#8`^pT+f4@tX2rvf*QG7Ue4aZS-S#y5)ub<xE)fs
ze?G%*Sw0uPjqZF4s=PQ62ew`23s2l44@ImI(mTsWvbqK7J2+tjxuky%+$O{&UFim;
zrO%PH^G95!_rA?ML$itn{^g@o7e0X|dJM|5tj*OVQj~YRBM-$WYSC6vqj=oJ(<b0y
zl~+lnH>pV@F5KPR;U8MoGx^etq}QGE=wy0YgP3SFsRn!?SkH%j46OE_Z``K`;~J^_
zniE)}r5m5T=&}CuQ4~BKzM)JR${?3%{qd1mc9yVyEX&Xs11g$Ip|kMj?1g&nsBz3U
zj~y@X(H3mrlg?`LZT8$8-TG78<!jE86<63y-3epx41c9P)Ffz#*;vf^K?Q7=hmaBe
z++~)s_z$Wvwkq1JwC<^T?JF>R5CohI>J`*90(uRU3zXjGr#HUNfxA@>6<w|GPNmVT
zPv}$u?9hvIk~^>c<nWa(Uw1$@?Lf0}YxJ_#@9XCenD`(w%Il0%-iTmnQc@AjD=G7!
z$zpW#RWB#Wx&Z+QQLPpyxE5!nnc8BLEQaoK?2KuT0-DQDSiln01;m#oJ*_{g#6R1(
z8}fFV%nehkelf0XViZSm4PX*E>o;-kh&>!bJjL9xh<k8!+&(iR&!+>LC<=GTGbr47
z#k3+wYaVp+gn2|ew>A^`7@?k4X&{*)Cw(T%n_Pk<W-k>93KR=}c}h8KUo`WLl>~Kb
zDz~zfM=Lcjx}fXY6khh>;^AjTUbz3vdzzzY#uuax4KVc<9mjtTHQ2w^IzA|Er+So+
zL)?L-vL(A)4;6S2%b(GVkCm*_y}7L&4#W8(Em=?8R`f&b1%d)oSt@t*@u$<AEU#eE
zh?hxF>-R^z9B*YMxY8DQBReFH%ytXCB<Z?0{;er4q--lL3_55Ls*y8&a|GiIfaRew
zafYzsCYN$6Q4nu+xMEF}DfB;jxr7_qyYPkvRsOCxrX&xUw|g870Hba(WqsT*M?6{?
zcVQIEg&JbTrSK1AwuS~XKG}cJ|D;-Bp=r<)+dbN~HGZtIgwK$NTvDqXKizFSb*fV{
ztpF)D-IHo|laK*y1NpiWtUE@*ABmYrA|Y0raTDm8(wRtf4y!87r}T{8VaY@$7B7ba
zF2Ud?PIt7;kfd8R3W{wEJ=4o*{zBUfAfj024#nSfSMi2QGT4p%pksN)^pzL@rK>h1
zII3dv-q4^+man!CfTncwnc&4ZFQoyMUcLdnjf^iWXeXI|YL`w1iLeog`2Z+tOywR6
zU-ah%M9C%Opq*A=siqoYNgBE_D2s+UQIcsKMadBm!rHi$oi68*^s4oU$)(^Q&MtaW
ziJ*9EOm`k=F5|L&QOEtU^X}^4?HNbBa{@qC9<Tr_69Z>xQC_Hp8eIU}I8t&Slyve`
zC1rB15<_{>Ac}`3h9WBz&<yRh0+>2lG_&T8L?n)aFzJv4UjF^Oqg^4lgU0g<Z}<Nl
z*tQ}B0SMBhK?)9m<N?A6IJI)f$<L3kg8)OG%edrIXv)f&PsBEM3K<hF7h`u*`-HCi
z#X5)6YB}A1wbs$;i)lG)iiqwKw3|OS|L2O%@6`!F%CIhNJ@#&#)&kQQifT)x-WO^$
z)+2%J<Fgd!6aa7!FmQVSb}||qi)nYhtoHXwM{`kJ-rdvH8Y)|k@!&{P0o6ULOA@;7
zGw+_|<D0xAiho5@b9vAHte31wTLjBximodvd;?^jrKmf5y5MI(MM<hcz)q=>QgikZ
zCu`soAn9{u!JPHY>@aVRL$YDx*gUUmG`KFJcMx_+H3ld$SU<r8OL-l%P;!#ggOa;W
zKU;c_fi_?4{`p`pM(1RIj8&ME0-vTFJ5c?Oi4>?RpXeEv(4#hr3I_cyw3>m|ghlGu
z@jxF~FND;Qj$e!e{$BH8<`F9TEyn-iNqHj*9<--qv7-n7tr&%9e9<H`Jd#uEGA$o;
zW&^jpD~Nr@f^%%$sk_ZnXW-_#OZvhiuK91;d9>w$-O>EO<iZi|pQlzsrUXYZj|KOC
zc>{{TAE@awtw!_|zHvV)w4JVDdCM?`Xq|F&6(3JyCOC0_8rQw;x>8hRZ?|S1baQ4m
zZ)x0xjN09@jAGtoF`ynDa%7b9pA5#-?b-=cuA-Jr=@9cZzXqJ^;K+G~3bBEfe>ZPd
ze?O);-S3Y6>1W{Q#n@@FuE90<@O#XH!*bN^&Bj>TxJb3c&~SM0%Ol;9BBOM(z4B5&
zl@-re^o%vO$xdgDNYg=|6<YwRd&EGR#H5LOYR2HMaR}+0_~^k`0`@R>q@X>yiW+a%
zl{KQ)7LfK(-vqKE@~gD5=CEg3Cn&We?UxDCJmYX!i>)f!J98#coYcobXfXFSyVk&+
z>#m)*y)rp?u!I@yhBwL4eSJtW@VU&T14%T1_<+XJ01o+hk7}m%28^`ct>xFdoi=O@
z!vhwaF};`>SLqM|7Ln{TT<Y2sK|y=3)5>k+9hx@D7r=7Vm@B6E&l1^zfCe|7z=6}?
z9o0()$ySj0@^gpFt_$Oz!D?sWZ$V~Xfz?gw;Z?wbj>x)t`l7s;0QcyyaDvhKaY=yd
zPi@%^?umaQl*drqnWgMpvHF~{>l~8u8q7Rpx3afzQ1?>e3db;&V@LEe<$Q7xC1@>P
zNz6Z;j|K!hI{VK=e+AWxU=T+447|j22<^X!C~SOt`|GB1mv~Io#4@$?#o0Pw;^>pX
z=tXjgF%CfGOq?#>aLnabb$pH_PFId2+kr(~@5v=iiU1x*9XV|sTLh6)?*wmB4e@!x
zw7g_s@pqa@_UCvnzJ51oFU^x>x|bbXSgO<g!y<LblLjvuFx#Jh?sZu-q^9fZQeg8X
zQfdWP`<eDEYO43ZxsEOaww--GPt%glSt#W8@6i4pFgIA<90Fgcc0e~lC?w<-I(cQ<
zeR7sioae|nMKtogGb!fbAVj}*bdLhTf9kGr`d7@rP5!7*WVgFgwc%nh`=td4`lTnq
zCW5+#e2YE9k9U*ifG+uD5<Na&3$_O5qv#y{BF{`oXK`oXM2986NSJWd#pSFnNbbwr
zMo?Gsn%-u4venY;Vw@?06Sreg4!Y6Ui3h7mvJSR-W04$cnY578HZm^jGE}ecGpmQI
z^Rfvxj-+`pPB~N0aD(()?l-}UT9Z-VqT0&xOaP<Pw?6b}pt`e-rKtSzPeSd_ke;JX
zpgo`Q(N)=GL<AQD)WuqwU71rSvUfH$jtAT;q$dAzl4|Ci)X%2|{(^75^f;?!Y^|QB
zyrDh(RPZSxT>%B9$d=xz3d)mm$ovw@lzP644|MS5Q`Q~b*g?~o6B7?7Bu~0LnPkb1
zKiQx4R(24bZSV9Kje;8oo<KgaK&x5B(THN!==h6xMJwUj`97o%x3Fr|^3>2ly7#U>
zS8pU4n<}oXtr@Emhep_jAruG07P@K)60t_pv^?;K)X8(UeXW%UXc6Zbe?7E6!YK|s
zRHEpDpx@5s^bQiU4A%oj_8;m?{&kekn2T$^>rmr7^)E$Uxm<WOg!bvR+N>KFX<hE8
ztRC}Q3^1l!cBShVL4bR{^VjEgF)Q07_2H)ke5trbnfjqsyg@A<F7SFg4=*uVM6UC|
z^ZIx&y^a~-$x&eICm$`LVnfv=T%gO;<SeovV3LtX|Dtt66@I4DX_(C&UZn2?9EDac
z@a!a9N>sgty?b&DOUBYy+%Yj;<3$ap3%-1Z-J}^d!2I>{INw^{RiE4i(*4{%ld<4g
z@t53k!|Tjio23R`DHeUsR^aLJ&Fv<it}fzqz~-cDxV8vR%WQWf%A{_b-LEp|So~3U
z;;mWZ9X5V@K`|M6ZlS&kHTTj5Xtv5>`mr)ckxF73Eg&lvpO<N9SY|a44*QjJ<QK_r
zhu}lj2S;)sD6GclAgW>!!UFIusXn0tvo&zNVc#@iIx8g<pX<|M>*V{0A~CjTq>}vV
zi4nuM_+{-CkdSMonD81{22r9D75!W~?*YaTgb`APs*`F?7f%!;<yRB&nmj;=?0Gsl
z`bN}_WuB&<4g71@7B{Yy)@homr%!~$bc`$vb3leXh!u9v7<UDR@$3UKcgkhjRvs{3
zlK-TLwh`Kkp(WI*#pBW&dZ3@hOq>CF@82=txpnoUc|=X99TFI#z^LnxW~xq;%xx;f
zVwNS9$LB(xksj%G5Ph(6aN;<RGVxQ?PA-Tl3pm(T=dlfKVsZQ96eKd4-xw&~e28Pv
zz=sbr6J*s3Y|M!D!MCR$$%l@=N9RU*^8wR`0QD{u&1S+F{$hM^e*jBD)caZ8mqydC
z{_057CJq&ax2zvh30}Pu*m8x0HZ%%^RTfHV&NX2bKTAu7wZ=@hej0*>Gc8@|yyf~6
z3zO~DAgf$7%0)uq+Ig6;x2L}?#Uk2NYIf#B4-*bIT<=(9J)|y!&vSrQ9@S|?!X>T)
zFB?jmnl8_A<jf$Afgv}hbgxW}TV9BHe95`sp?_NMol#1HE)(V8JL=n~yz0NiP@PVD
z1!y&8;b^iQtY@km2cnT`ZfS1%e4bDjqIAsC+S@}`Sp+&wnjC9X_Rj>UO5bni+MJY)
z+IeveL=8AN^({4&ACX)UKXaK?LY8PkP?Vr-qqsv61(--WRBP~@ljH<7F-US+9z!db
z0^A^}Z;QMJ6wHL&W`&iTuzZ~XYsB{ee(T4cL@i#PSN6;(5x=EtPP8Cb5wVpF7t1ij
zhwPKn9mN`DD76Q3;k@5kHc^~WA6O!RP!<y2Mk^O1!;X5aW_RMwo^(Ht%~$u@J>mh-
zN5t}^i+TlRe}F5*rz};y%W_*c0rgFc?(cRLzRfftsPelA*Wqtv5HRqpHN75(pvkZp
zosqLP5~d%+CjnRoj|TR$IarD3wo1<yjxuC6k_&dMuq}T(Esola07G@^ZZqs!3GKNn
z4Fn*(%R^~fc4{0)C3Nq25G$c_X$&0$+@5Z``C#M@)A+zb)X6}QPjYG8v1GSXk3OK1
zgqiFqx>q=*p1G4z;e%|tc~YMB+D`uU%<s(^8|l^<zxA{AZg$P9b=hHH#xaITrbb&{
z@Uh%MpQC3dH6c`fE|@4OE#9JwGX=;D_rLhN^6c`mQ3XH72x<s_3RMoD6`Y&W?P8O!
zWwl)Z>%=!-2)V(HpRo%a<ZTOU$(^dF_xQ}aqIFLrXy$IP%I&IsSXy_Ztk}mSH$o;^
zP6tscrK5Q}N|*Cj=y@9W7^AldQ7swHN-2gtea6}@In^=j_(#J&Ys8FfG|FGYf6qZS
z9y0T=zTY>O2yD$OkEN!)vF#t8UVO6uMX9fIim8C-<xuq(HnP;aYeA@HfzOz*Qn9t5
zZjBYNBq29<FCXAFNtE81PQ`ypHJI_<;5X85niv5XQ2X$-lQlt6AXVkR@p&NRcyaxy
zwX!8Y=eX~F|J~QrOsFwxatPv2-dL)H8}nQhZN`^9bIlYkA%-fdZ6x8!^&S(`$3$C{
zC*(|*>C|~}gI%ei@FNW~xbRKoI)iwn4R9C*T9$2e{G*<`K~RT>$~r6oGh58BuCbzB
z*z4^tq*01vg(*R4UqExU;sDVKKYMRcbVBXNd9&L)cYwu7en=K3riP+4C)bwKbHR#)
z-O<z2f0RHZQ58wdnz}oCjfLXfVWK-Xf!dO-5*t?OX5@|fM{`G}iR48kT59~v>NDiz
z=f?>JQGn_@aTJd8NE??Psw>@sd5nJ1X7!FDKx>fOm{Q{xzqD+5410tqsD$3|3I(we
zFXVGruqeRc!X0&_rc|l9srxTfR*K^|zr%#=*wmYB$H?mO{oW*)Et*e>P%Wxk@7k=(
zI<DJ-p)FK2g@B-$-Tu6^Dx~+P3$v*^(?aUA5mA~$JL&<PZCmY-?$p^vJKv4Bmyklm
z<32vc^I$pmpy_B=%T7BA{>8?|IBJnk&vyq^^QmQau^h{Ry?dM<0d)8VqV}by9t)mt
z1gjeU`Vg`jmZck<c@6dKyH&%=jA4EC(Ia*XZXbknBSNRPw~U*)^Nhr;qWGracbrtT
z=X0)+)@2=FD&h%u76#qH6st(%+wATJD?r`gnW9tU*cw31I1qid9vFEd>&zNm`d+04
z(Dp(ictU>h>rrLHt!fiuJq@3*m)5f>09vEUtix~-E<GQUhmgF1bf8-@ec$;*wrU>_
zC{qY4;uL-Jk!o;Y4DZ@~imXcB^uC3}x{#>kV%TZIBU)?jnc=`xQc?{yv;-#-TYWvR
zmW?YMh59c%RdTae_C+~-%r?LNO5Hg|-HzrSs3L%`lK<~x*?KO_50yr6mH%z8_B^|R
z7YlFiX5>mzyuQS(KQUBOs;MR)bsMQp-1a15_28K`7dP#@eJdDZF2QKZP#ObEyFn{5
z)h3N(P=cd+IqI0iPoi4?Rd}If@_R+=(0;$T01RZ9(KmU_Xvos{%K)oY%?i}{(h>$8
zTWr{Xm{W3!Mz;<pa+2SAuz4eI*}Ejl4Q78i)Y*QvbF?jjqtLq8c?B1r!?lBy>nupV
zoQ%%idq9=xb*dUHD@rGmnd7+z6;KhxE_YdBVh6q_%u@1>O@L3{(dilyySng1DSIoJ
z@V~?)%@Pk60>W4_2%Y8dGp2F#%1v=_hEhFZpXXgeK*aK(jryQR>|88PBEieadIbkz
zi0KYgq#f84WAm`|hNfJ)wiTGf$%fpQ8m<m&@Hq&YL5@BLC71}&(mh<9*HBuIldzv;
z2xU>Ad?ANh=wW7<wNulqiEu+0>A1|V|Hj%o25Az8YntUQ+h&(-+je!?wr$(hW!tuG
z+xAzsw$Dz??3tJ|F&nY_@6A6Mk(m*h`99YTd@QqYq}&gW(+EfLkHSZS-QgOsRuZ8X
z^{FUrbAZl}=pC6ebME=JBMMVgW<jRN+qqxB8FN-|?<l_f!o+BWPPK07-d}9=O$kNx
zA-za`GBZIEHoI_4=zjws=~1fbO+g(H5vQtdk3I6Z7*cWh#Snc8ec84;kB#RcP3d@v
zE$O(;w}lgu-wRQdGo}Zi=MOaK#TPKrzNCif7qybCF}-w|Z#ietg3J>ZSH)<_$n!C(
zI?srhGa7w-tX%=6Y&JzZ`oje%Q$ik%|9Voi3{|2|clI8$<=WpcP)!olri*6N0inl5
zfH!qg_-o?3BVgwhAr_@y*jQYq(q}p{3$lPF0?D7K4RAnMFW@5!P4#hS9NXA-vzamH
zx{60T!DaTNz%y%4fBD7;6J7%~S$h@2*!NWJ27&9hqTP%^&eI+^6l*Rc*cFY?0WB*X
z;->w$(l}*_2U_Fk<00FJk2aXzAR}kOfR8qH!jUBvw*j0Nx3sOLZd3pZUTJw87&e+I
z4T;d)h4yk8glakkY@nmVhjxfG-tew0m&SR`>jq?W92$zPa_oR~nPfscYc{=~XvCOL
zS8+opzOWBfzImE|G+f$j9`^*p{jzduL}u$mnBkguBsEnF`)`>_fM|tf?Y$w#jm~=d
zFRG(N3yMQcyoGWXWCZzYpjwR+cPnq#rb+12B>1CS6edS?6~G6Gq$216(R6n{Y!!DK
zjzihgsL{%pWhb7c*vZCKZb$_m0@NRs+ufMI_TEWL{TlYtw;V_D4{40gy?If_h|~aP
zZbPvmS(ZMYLXJ)_@)6VM5|+jEb`Ddg$}o+Pay~no5w_BPR3ah@ao>$;baPz%9vSP1
z{4t^`pD~gA1cfp145!=1@1;wVzUaaHyuX#gpX0vxDQjC#o3*;PD|`=;!+-1XM$WJD
zvaTenrKKL`=ts=FruQ>HG9eld>CWx7_KwH_0x=apv{Y#M99XjN%4!B5EKO#6JvKEz
zwCnw_Nitm-tH*r6AniC4{sTFyS`rDdQ(cb%rdj(gu%y4=fkB@Wi2JxO*5~kr<j<OZ
zR{<V^vL6}H(0ar}a~0eECD0lJhY9>7FuLw;un|H3G`6s4Mg8~&Eakj$86`?&{KDNS
zKzix?)P2~9w6YGL^}R(Brp|>L#jjE&3&=IXnZYt{<*{ppI&$<?EeKRS6omWgcNJtM
zbiNEk$dyy3v#UB;xj;VhwYMNk#kQGL-x;(y8osKcw7X6n!I{;zqb)sWw4-*8ahwk7
z*^3ayahjwu`gICZ4IOEh2<|8;aY|<KFr1z9PE&sWIK6-}Oc-u5DtAS=*URcR=Z?S7
zElQd8>~0S-1;d_R3B$<fv+|~<SyVE6gOLOcT?I8Gg<raxtAZyJAc!9xU!lzL>wDz1
z<4CN@wewM+)3mA|M+Wb)F{uO9F+nrFtq}u8g}eTE73|O8p-en_*e6i;%rMM!{C3pe
zAbn||zwYv;{OZc;1JB~7U6xllu6iWjU7*%7wNmHr1t?lDGg<9M*{Wua%ZU44m%Bpz
z^9`d73&x9p(dASrk-cpEZDpvxHhb9Tq-HvHoP)OussyNe%2vlquW!YAIc70W0k2!S
z>;qfSW<~W_WB{{Ve2}@6mx8;@(JUPStQXN41Ue|CgMl!2O5x6KFfz}&j&9^@jOf*A
z!3&<ZW_L&$eBIL0-A=F3)1<&)9r6rNZ^K;Gor&?N2VRAzWAwRu42KNpltTmY^GmK|
ziNDtqt>^717Ng2$HyC;3Gh-;7_QJ;$EU-0IC>Z@o#ETxYyZn}8+NKdRYHgflK2cPc
zE0E~`n7J9GKeyVLWGpxRhe6T14!;v%dP|RExy5QZgS<6{VA?)?fiZ3nH#^mevV@t!
z7aINBagZVA{|>CpypukzJLdg#@5f%U@rH#^r1V&#ebmZ<xuD2JUqw#37|<$SVEYCv
zx1DEis~26o?;B*B%mCP5bZdX{L#~m5t*UodR*2&^nzs#US@vg7>7f`ax|QE^3`LFi
zRbvWTPV*S&bCtc*QnD0af)F7p#8m3_i;5<0rQ=0`F6vk3zVU%4w>My2%;7*}u*`nz
za$SuxG8SChNow9Wew(t4mT1_^@vqJ<O*>^t`wi89Z5GaaPs-02VgG=*52sk%L;raI
z3x&5Pt$RrOnqyHj?yKtvzW<G8eope$XdX>fKYc_vONVxGbXodv`thdMH><jup{@x@
zKp?ZYMMd=$UuT`M-ted)E0jW6u;Awww=1x=Ec)xtSNSI4!2FN7zJBLr9@E)0JC?c+
z+RAB80#UhzOK>p;TNZ7TBhwJfn+RdDWSHr(QB|Zz_XHAEy-UGH>M~n^)GFzyi}zt_
z1CRdClQ6NeH}9ZKq}mxMfTX{6r~MC+mFS<`FKww)e51d@Etjh_GMN-h5`{1egSlcP
z5oR>V%5SclO$N916Z;Q^uB*~bvEz_D1K|mpQ?PKfO>KG(zcug*5f~kHX5sInj-L`d
zV!BeWYbb0OQ0C-9)VomcSqY?^`#`zVGa*VhI--RhJmjqqxl7bXg$fT)e!m{E#cd?6
z>g|)kU5!*#b#`7CWbDBeGU9n*B%??UCnIRP;q=;)5k^pcNFH1+#AfQEIqVy(>xAwt
zMo6a{y!wwRQia_QaZ+*X*D*1AQ8pd-^vP;klFF`rnwT1<3O&RaVydPnX7>o^JM_h<
zDJ%k3&b@MVL)n-lg>$|OA{k#tHSB;VWe1C~*Di{uKpG#44KN>4JyxmNvAvVArn)^5
zW+iL(*bQ70+fr#}EOyaodyu_$lkiGRi@NC14gm2A+4mABHU{suu9;vg2ny%V!8wF_
z@e-rgpA06w<mQsy5uJUmtL~*yr4b~=lxG8PnuLw)R5(D&UWt5$+2XV8n>h?G=RBYO
z&2d9cf^dQSGQM}p?&~5yPE0Ql&PMKtjirO0{WWoJ^V1)N#REB_24V_Dl#}1S+xJUO
zN~yK#o3g%xSjk_=YZd?Afvy7dlJgGD2Vh3M^WA{ak^qwdavu(}K01*8&p!6&e8jM!
zywBew_nJ)=GX#2Px`zd%Y&~Q#tuS%{OFrf`>iUJjeQyAm^?qfeJ0IyULcMx3QA*Pl
zE1h2qO<(}gkDx~%WDs7o-4+puHxFGy@hByq_vx%V*&6B%)&^fB>VdroXD!sLSTOOO
zk%)?m=x^gU{G5LBo2e$oWvgzEm2Up|3X-Ty?(Ql5`}$eOkBX*3Bc`^&Mc`O+Cm`!N
zbesuwTR7^Cc{C>ys%FB*j=Sal_3)`HF^lG;I(%q{BKAHHf+_0Le@$}XTBcvy<Yfp&
z@2hN88`Xp<N{G|AuEWMlUq+TbMAN=ps<S;{lf;ORiiO6OCDi!&bhsX0QAEOY>zaJ~
z>-CF)Qv7Im_o5#~WI+mUB7mX7vgNrQ*N_oXLm3hO*$-}x-a4#YB^L}e;0qX!CrLfp
z<DTR({~7G!g)_ChmpcmSfxSm!TcEcCL^0^#I(et5@JD-UC5FgARuObD<%ut|Y$Rwg
z;(KEY@0QpDO4^lIJ9iOQrdnW;2boIWCPQDU(>Cg}K_X>AaOt!bHyAo>$<^30HDc_S
zIID5`21cYpoC#0cDdPpyERq1VT8PHg#lWR(X1!c1xUT!LwDH~gD2dNA5FJao8N>3q
zyaGcxJwet4r8OioXw(h?&<~trCzVjz)0G^mp5-kR@~P%zII=wSOY1-%gsS+0%ElHX
zIu=o>i>~~3!@rv;ZmAFZNk&CR<{0Ua9v-~P(w7Z&Np)dIcfQDudI>S#Z`7+kO3^3E
z$yoKbjQf&Co!m(T2?8jIjI`LJG$L++bk?cybRA<;&U$;}FF4=j)WlCiy5jk26eI~6
zG4ft8N3HJ1wNY;`Ws*mn8szDBrqvY6IOw~NBicJ~y~?!-;(K?+oL~@XUL-E;n>dc1
zK0Domvi;fG(bUp(zCH*KfRivM>tuw`_~P;3{AWhjaz)~CX!Jj`TP%e_z?3L~f%Nm2
zh1A-gO^w=a*N}9a@5n4ls`3v>O+gRKp=fAD5@Rgltr+%f$|k?W%SJhRZn9BcBA+P(
zfB~DERDZ*UF(384otwD1+YklKr0GpqZTle<Gc(J4+P>^lkv&NoYi;f>6tPxcb!ti<
z+FsY3s7+Jl0uz^GOw~w@cA5*%l`5GM61J7_cW1)ud8lkb9XZShKo5{JtYlt__gU|a
zD%h3}Y2~j#=0{Wt4WgB3P%Z|#*{Kc5UcBl+CW6gN6_3tocq?6;WJ#Vu`6Cq27ZQay
z$R8}{W1Mf5*b*04)WeC`(qwg#x#AE4kBsBs0d^kPZjAO=!F(O70wM6S7_vMip_A_|
z9QpasqC8Y*POZ$94y{=F#n;T&JOOqKd{Xc?#Ee#UL&>Jwxzh^z>{>>d@4eyO)m^k{
zSFWl74*45MhuDP~`<d@CBQCom1L9l<Z2lrO8y{TogI7O56ja|Z|AnxhjqyJz-v3X+
zeg!#YVIkpv!hR*jf4cYoqF;}eh2g&o`x!X@zX<#PonG15#72#f?Vp3-e<SM`rdOjk
zq&Ky2HKDhlx2CtDx23nUHKDgRa5S;CHZgVnZ!1R&GxL8-YZoVaCwgakXE!@~7kW<<
zN4x*Y3h;jm|Nmzz0MmcEG5^B~z{2#e?(e@H{VV5VX8mV6_+Jwr*_j#tn-PHU|AiC4
z4O|5Uu*#nevOvUW=o}58JBJ_(W`vN@4`S4JCfO!}L`Gg9QX)2(%O);|T*%K09OC|3
zx%v3`nf2IhbZR+0apHgR1vt(4UyYawRmee#m!X5iA|s<gi~}#h$rJ+-#<wv*l8S+V
zWeh3+=J=aP1*9n!a6<hjzru9zpuh_mxZoI|4^933L7jcJfC~<Q6e&3sEjSS&2_Znn
zzmLNOCV*M`dKk$40~iD_fWthKfR>SRz9s%fQA0aT>WKr<2O|(tQB%{ta^vP*0E-R~
z>T~n~MLULg>e~kUZv&e`Lc&Jg|ENRjf~%uWD9ecq&CbeWUhUIH3v^K2L4kbgQ>gn>
zF5yHzhHwG<_Jd;v^5*phKqUbIpVTni@GqG{nk5rpLV@d}0vI4s0Wnaw0o;NKKvUa5
z%y>Tj3FN5XVU6De@PYq=&p;8;(7(91bLV@J`}cZ-`Vk=q22o&N!uoZ9W*FQk`Xr?F
zFhj(kK>F-U^<wqi?7)8GU%`a72jMZo_rm-FZBmg1(ueKgpZ5@sXOK^Btz)11!VpT(
zvtW>23Fa;*;o-@%Pi7yleP_{^pa5HR@k`h>atR{&+zar_$~LekKh;Cb^5zkx*<+w5
zD2pA*#m7V&=jXzpfF^_j2`Dl#23DX4LWOfcdrR5c+C_Qaq<EK(Dd*>yL)nM29})0}
z7|hE5OOPmFS4WTPFZ3=D1p2dhYZoD+g1QG5lm`^6fPtBHlzWBAy7Y#C#q2=}3s(nh
z5Kaya^!5Jf(=cd12?gx-@-zDNVlUbhqeANf3+@a0y;)QeDgX)+G9&;*#3+d<prWFN
zOBllm_QO57`}#BRtz`up`t=9rEH|3V-V99O83FR|&KLOLi@e<JcRQW8zu&HYc;781
z^xjw4a|@v)BG?1jyWj7hf8Bfpr~kV7^vr*DpUmIdI)7U3znOmc)#`ptZU4w~hYs^m
zOo?F<GI#<1*r`*W5lCADxdgtsf3~Xr0w1vsrP><)y7@iO)vRw`mc$4P?(h{P;5QAF
zw1Wo`hE39C(3^whA5Q`KsK+){Be=yNAHi;_)GMnnMfDG8%W{D2KU$p*kro5#lOkWq
z2V(JvC4zxP8m2BnMSf3P`G+&$1`GrPG28|M&p?un*Lp-6zETFh7Jn$Cf`+?)hf#nd
z3VzW_jRA>(<c;xf{is3={fZc>I<d>pxBQg;{(=|CH>U_THh>3Ci0jxzU4APRd`iuo
z8~J;{Vn296l&i1@a$h{l$!UPD=i@s>b)RXuvR9cYeVNf!N_DeN2kN$&2}R&+{I}~B
z%h@2;l&k$N@Ms%MXV?9_=AyBcW@t+2gep@oO;<2=-?r^~ZM&n7n4SR3@euifz}K#6
zes)KVHg;&r=CAdN^m&4=iYye_AJUCb^(mkzJQ!0f4oC$d8j(*Gar4OZMY~0nki%8$
z4hs;{elN)blYkW~!*R1l;7QgK!yVpe?dTO)nzm9ElBBHwORe>e$A<v<NwowL&PHc?
zaejB>$PT+Z2y4uYZvk7s_E9+8v_lhP#Qx%WNeS)iwrqf6e~uiXqJ2|QXXMHo@iyR$
z);t-$lg%;BcAzqy&TQAY$bc`ev$eB>bDigRQ(?IXoo+zl>Tem9wRkuFR4>`j!l^Nu
zOVjaR%)TVYs}0$rhm|k{D~@v5WzhfJI<ibKI=p>9e<W`Q5%z8d2!o1md@xv7OVTAT
zTsr$vR4Q0L{k!~vjHTzSY|I#Yc#{UGn~6e0+eU&GGzaB8%L=#0WF<Cz*Pjd58C-&T
zUBAn}50fURy8fyN|4w&G6ZbOw4c70tSu1U43z1zJrHz4x#umr$m4zka)XSKD_gnJI
z{LfTAInAG4ac}2z!)x&D5llWL{^2T5zhctM$?+|?n{>%92^cYUh`7yV_NFfM>Flto
z_7=HV%hEu{G`7tlu{praWi<ZlRAJ|WPE)H$U>(%aa~s@#zPNTrP{!}fuy2f9bz-ub
z-kE>%^Y;gDLo`234<CXSnl5+7=_NhiUv?U7q`p4ZOGqLYjeqtiYE{Vwt*Trw$jx)a
z24VjKYTeNeLCn~pT4#J2?rt4~Q;@DKtfyM1H%iM}!HxKWit*$cli04S(PGPWpku2$
z>D|P|%QGtRg`m`6V1$LuU`V<e3`K<?0S{~Dk#s1#*pE&~xO0_olRQM??o*UlV=R;G
zgw()y7jEYEe>63e2{e>C<`>$!P>>@kIM*3f(+#yC9r;~{ln^k^aS_7J=crAaM=L7^
z1EB7wB4RUfPo;Jz5pU+Ke;9RK+AuQuh$llfjlM!WSH^c|@P;#@yZ(rjYmCLGu@@yv
z$QEKi;HHD@qWV~P^+gqEl%lK{!%5U1#2iO6FyKmBC3q4H@YhoEuln%j?~H0FA@q>H
zZ|gM}wr7quZ||lT&1c@9cp0SM-a(9a;6p|$WlB4)fjgI(0enL)Js4{n@{C8_uRk4t
zM=dX$mI|6ABU_GbPc-4W9!<yW^9%64TU;uHiA}}@#bAGa%Z0O?qmLRz5*H-DM8r?K
z?S5$p*bH-^-KC)ixWViV2tS)=+a4BdM0Otkc9nsjU8K5uhlO1yhh#dF^WxvS+f3!&
z@HV9)2&$O&8ja5-(mzdHsS~#06=uft5p|8YeXg8XW2%QdowQ4lEW-gB^;7Bo1$a>v
zdLiq^>cN}0Kp@bB?1!F}g*4?c?$m{rWe`4wv%naVUfue;aA_PLi+1TqSElOs2@kPQ
z@`U)L7#m)cn-YFF)3W*X`EAg>c-E}8mRLFp&uWkfuF4x?r8V!YY(P(vrk8HhU4xPz
zEfDu7p~O&hHoMKr{j$yk+ul>rqr$gF!D~#6nCY&0gk$&MTLoHB^H#1%Xu+w6H}BM?
z0%8v4oGs!N*(EG3ld!t77d#L{KC{B!u4IrxNmu$;Yg5~@3hztfPCH`k!}-_DVo+AG
zKjQ#K^>7kOm9+oob~nuGlZaga>o@Gb6xGjv>5R~u>}g$7-I3UAa^6%J)B-#zYZhz-
zr;8zv96=$ts>95Z!ZoF^1;06Mn^9|)GwsR(|6U5~-Ty%lfKM|&<wijbQf?e6_5|!l
zzoy&^_?Twh*{vpdG!%x&dqQ${t<RoJ<#_g3-Lno6V~^dXjYGddOnH40dIrXh`0g5s
z377QJqPK4|yiT$9&rXc?xcR7Eq`>J&cNoG*HkkC{J#1gFF`DR;OWKTX;`O*A@h(n1
z&k>d#^dC_8qS-oRk+k?MxxkEBQ(IDKHIVRWYCr0<juLt*Q+|wG4Vo4PI!SGW;;(RD
zv0Wm;+(h0c;>|$#+-3EGf%`G=%9Ew;N#v10?iZ5Y=5N1r*H?_2C}wY(WZrH``Lf?J
zE+*rbJ%=vA)U-E5P6sb(8nGX@7Jk!z)*6u&aF?Oo_R`kk!j*baR#-U!$DTEAs$`*i
z;@oKo_rO;~W{ioHL4#M1a7J>0EwSy4g0PgD(x>WaZ0^t?0mSjub||3VmTtDQC-1f#
z!ggks_}SUha5(dTJI*U|&DbVel~D*y>dIdk!`>r<O1H<%sXr<Z7X1zc7z1c(G8f7F
z;h-O^=ofln|8RO+ab<eQ@=$CPjX5P}&~vn2X}-Nn7isBD4AgWqn)<dKwWsQQiUj(x
zZLx-X3`K<J#On(Mw{i!6TsXx!2&ZRUIXzi9YZTCouTQ``K*#S7x(Tie30Y4#kPvrw
zpZoZPhDp2kOtG|rqgW)DBo@jer-K(~FgtL7vAl2e5bHdlT|ugX7qPkMo%5xAb=9wB
zdkWxCJ%9CpbcT0oJSAnR=el-~rN1{|dui9u)-QfUoU^}1Sg75bIOAW&2wThFzeKOm
zK~ou)C<g^iH@fvK()|>r((14LJ=n$%-IJNbN9TVGonaQkc)iZ#i%-xswZpa&DDo(6
zW#%YgvI{Ot!A!+&{5s?9@EJAHPmWi<#<Gu2WtBalDPe(mu4icEb-FnkD7H?J;bvx*
zrBZ&sC^3%>q#V^oj$`gplc3qfomk!IeG^%(pr_E*-v7|)AN@mXh8>&c>*A1qXka*>
zXEhmA?4e)iqJv*f45K`qG9%#ZSHIc*KKz|8BODx=D9KQ1)TCAqw*&vQI^kc-lOwui
zN?-p<o6)Y3+dXOO6PSVv?8=b6q?qM%)pV0-G5b7|>M~!Eci*|8IcibJ`y@S{8rf=}
zxpk-tphxFUqjl477AIJQ)msww9_dIB?@{66bWP=wSHCp0o`#3#a8A60Wb+vphUt!g
zTIdFNo}fhIZ0blu=o~^2#q5k4gc%4EZdwv<)Jl4$3O<N-G9vG)3ta4d<Pc1{b^kI^
z`DoEaJ@WpoN3iYTtK*F!E@W=!sov{x_z+`O_+%GruU`~4`Aai3CZLax1wbq7{ejc`
z_6d*jE>=tX+>`idcHy#Wi2&E&{wqYy<BZP(JlR;gMxKD5#!cy|SWUmHS8Ww-KA7G(
z<i%ajHbqDwLG2nKH=QExu)HzIOYutySyWeAQ5~7B&F{OV04mNyUebQr4!GI~fg^<P
zA=aw+lcB+XhE3EUg(WRr;E?N1=eQPFh)w2gV}>SOS#hlke)!XDl=w26OE$n)9Jww`
zOg}bv)h-d<rW|H<BOpMCA1rbfufimizih16wSf+v*A!cSxV_mCM(1@oGR)w5OcagV
zCI)7;b<_$ZTsS*lqEty1n&G*${-yHoTFn_WOmwZq%T>yRUiMZe@#-ig<5<t51nXWG
zDI;0LPR|ELQ#*67XQEYsc`e%&!wJ}pnw~<tikB_orkL%8K%^^ljGSEMAdVS(t=67C
znhH)2s~fQV2KBJK$bpF>bF4cT8r>9n99aZ$zCkmnPg+U^{bfBr8n`l)u8Q~mhYa~I
zBhrf?+;+{$b~n3KdC@I}xrwBleiWqmH_cret*%Wg%1;`8aDz+N(qzQQ?LUtl@bdl_
zlDia1$$IIUFf!e%i)Tm{BxoTk3U;wXnuE4Y3IQqKDL{8RR&(5s8g!F=@H?P5m+|f4
zQ<3uXI_)_V{5@PPQsX6|l|HMURAYJBG;n-*H_fDN+^xyqP8h>7&(Q48nSy2XB9hTF
zkq_nWaAyy63;vbpGTyQ!!l(Nm<_DAQ^KuxY%7Vxj5}Z54dGEHP69&b=RjSAKOV3Sx
zB?)`M#^)@Q@F}d#VwG(v#s>DVt%{AMc!-lNqq4;60UtanE4j!;%XJNTSnX0-f~^LO
zRPSJB4xUZMlJzo$s^c1SFSoK)e$YS`;mWRuub}6nhh|y{K#QA0Y@cplPFw;wb>EtZ
zYe+x$srAmsc|Y3fS*7%qS|M*A2_8<$z<XL2RY5SsEhS{nZ6hzRGyECN6hL$v)K(hc
zBfL<C6Q%+edFbN$gYU~Ga^pEf|5Cc;A_N!B&uVG_`E=hd79B2*V$09X8#Y9)Rfs<`
zlwz~%39seYS5F0HYt8MRr9Mm(Knd9k52|+y%^yt`#h<)gG#zUraXXH8OKB?=zDz&!
z3!YYO(9`qx&qH_lBH_W?c=zkE(~mVbGzcb_#?xrJGpO*q3!D8u$h-;^(q@GyuXC^3
zl!=r|iCas*2By6fL|<&Mn^caq1lFCkRVe&W+h~0wMqqpm5KsXFPBg)qXm|>0DdB?C
zb6(hMNK94o6?R0=j;(xPbUkP5w%Zd3zlqPDgJ3kV2+-!2z<CD7<)+edQ5a%GMYUpg
zICa%=$(pn8DQ^@eiM>02mIKwdTM<^}-=2By{(W2_Q^gD@L9v_t`0&;p@M|ryY?=K_
z`YLkJB`qVV(5+(rZK+T-tZb)?DM)j1EpbJ}<0Oh&d}~SNQx@8<7yRe44m}j^;dhSO
zJT7CZy{^N3JE+0TUIr9{yQJvXY(|-GEJcIXO2f4ymGvDm1<?h=^_*@kQJ<q@BUgv@
z7Pe{jUj;X3`)YiJlrGbZtbaQgM^xtpp30TxK&jCQy_d(Dgyih{F}-inK_K3C{8f`c
zsuo1}81=M#Ow7@<SWKgy&<v+lxAqM$@>;3cvHvsk&g$~ac0Q^bY{ry$dc~WQx(>(#
zN*|u=-zTdfp#3t!TZ%5auDOR9Lg88)CJNp+S}#U&tb&Xo07a*_!eXhMZQA%my3c!2
zH=q%pppH_G!XH7AlEEF!`ugK@3TMnRev;U6^(n(I#C|qAb*4%M%T~;3(sueBJV8AZ
z`8l|MYG+JPot0%GBiX`cF3y@!J2e9<_9qlMM<Kzxc4i%;ZorbI(ST{=4zX00`Pn{l
z6TPSkeT)SrQ~#{Hii2&JI@aN1-_LVVB_Jx<^>NV=^X^x@6$8@CQY%}>z^z-P$py3M
zE<6q&W^BUvf@G_`8mkX1)&;W!2k*2Vliih~tr|?;7n2<q=Z3C|q0l0#f)1yhxaf7Z
zf=Q2;Gm}k~A#RGa9Bwu3QZEQY3hNpCCS=+6D|Smixm-eqoBrHeOx>01wUN+cwfbr*
zPw=_PK-c09-08j5pkU=QnAk~3V;t1YmE1U2o#mnUcH16PizY?Lzb!70ZkCGk3fN3C
zHY4M2FD{E@TCXfH*gf@`?)r~8Ih@5u?;2SCywd>P5+Hodu$6_Fp}0Clz|l}%P~Di3
zqpsm=!9|S;hS}ZvXlzTDYfw}oN#{Mm=NH+7yzwff(Vc=BY+{x~+zWvR%W3`*WKDfn
zVr11ZWdIOXTX+G}=l2nF=x9<`bMS**s0aA@{Y1>b@TWv8T2W;4UOchlGu-mCzg8+r
zN$WrAbavFU=CMku4_FDxORM&3qY$s>GB@!;zs#iaMj7RoQB7oXYP?CA#Go<bKK@Yh
zUiCQ>SN)E<3Z+aWgQ5#ubTkBq@~Ho%DVd$zH_O+U0sqV8Lu1iW>rA2dMx<-<DTvWy
zYx?Nrz9&`;Rb-A7Gc{K;){d2qj}A{APl3b<WpBizm}!ie?~P&PL)BPDJi6Qamw~M;
zwQScaWv$P#iB`atZ|Q9y?yevhUgb0?V-i!&A=*WC-M%8Hpu%kQU7)z$Z6$t!M(#VS
zoP{cU)Wxbdt>Q_`!^+wArm=(wgOK%|-2PFfLETvT&E2FuoCAAQ6-s7Kxd-s?{_TsB
z_SGs_*ucRy5wW5V<i6!!+OJEk9&|fw6(|ak3aom~q(%GN5?vJM*>K1-cdFL>usgBi
zX3BBZ6SFUF@g35(Q@br5iGi5`+UE&GU<+Wxq=e}OnRsHRqn5N+auvZMN}bKFY65Ba
zW1$}NX`d+rsZ!)!R%AgM^*hgAp)bM+V_r6}W~vR5uhqGJM(fF^&MrC&N#9eHzx8T+
zwv>?X8Wp3b<g=-Y`8Z>YGOA}w*7XWC-2~#DmKYyZvs_fF$a-0TWZT~rZjwF^gO4I)
zehhZ&X#P;*@!`Q6(mgk^HxkRw=1e+g6EAqgNPo(@d1g|Q&`#xW#4A(OWzX0BVj96P
zPm_NX;sz%sX7^Yo$$2Yd>$)E>%Cg^O3VU5MPWm3};4l4KkS=b|qcukHEUW{<@!8DT
zR1%;j(+6eYIL9ceE<{4So2i!k&Ua0t@%56ckTRENWh=3?2^b`u&*cK;m=f#9TT3K>
zZyP6GLyL!a#)2CKTqME7@hJy0!W}Hm;3(OrzLfsvPa>RExkp1~(BMShe%m@c$1>~H
z&FxZ$;R?hG(}^)P!TT-dyn@RwVdF%3*?=A+n*b99zaD&7(n&s2OWonI9B^qB8|)=Q
z$#>kVwV#l5nyFQvq|c4Kl~V8`D9m?BU&a<lf&r*k&pAsnVjV7aLVcU#HT8N-FlOKv
zskhel5#pIBfgz-!a*F77);qo0+B(aI!itbQVx`?Yl=&gsVUpGS&dHV9X630wP$MBT
z8rRSn7b>I~bWFtqYnAg!I5?phKUoZ@T%{kji0MG5M}WD~N=!s^9W5wfeFh7suBlho
zwow0jkTLY9JdC&ujnBF-*e&+cmU*02CeFbHIl|&Lv{g5iT5T0yMZWQZ$P#+14H}}1
zs^jhW5SCnw87S+Yz+?-IMJ8pJv(%Swv%t77MS}RBHbX#@8esv!(f2qQHy2$IFOJ_3
z2grIah@EW_g||11)cq(MtcZ7a@zBO_E;t9sC<|kX?K5k}JIK+w6MDc-MTo#yK@XE?
zNiPIX7cx*rp`3xHW@xV+GL{fNja@9x`9eYjn}Fho%>rBfu5r57(GH@`mM`CYdhu*2
zMyCW#?u!i-%D{Nxtd;Lu0(sfj2CkE{W%1RA^|0D%XO<WTT<w9+Tp5w@!Y7e)7gL<0
z=VE5kjX<ROwRtsAp(ypErOGu#z5%+fb0S#}6_cShz{oBKx$gUR9h31sq9!TE$XoBx
z!t0~peR4T<>L}J1QhwoU*e;6fm%5L8T(T7JG@Y=WrlaZO`xs@nvnwX~X`MQ?Kp*q^
z7i_1a#ORmk5?O9E30}&sQL)4iuChk~4WbQYI>RJakJqJ+jtygkreTxH=v=GM(Dv%6
zZoVC0S;|z?3q&&^P}=hR@r7?<AG>FHxbWP<SHl(VovNA(zgIIrCbJrHC7A*nR-t-Z
zlc-fnFxDG{Yie8fwh8bGDCfY`2#55c-9mXyPb$55OpgOTR%`G$+(=vfV|Qmwtle_~
z*5kUM&MI>QLk90UWD*J(4mYPvDsNfmd`F#ngMSWHPxGWTE7}OBj+VwOOHW(y4AfeU
z52d%U6r8Wb5HnapZiy=9(0XFD4DV@4c;czXUPvzG&;U=15|=t3<gtUwzSujH8_ZIX
zTbm|k_gYR7r)~WAB&51HkGXe0;Jk5U2S?DUGxg5@EcrfUn-Q)u=?&Oi_p_zAja%P1
zR!WW-YSEHOUE|GMEa8v7tA%M==v8hvFCFChqRXxm!`uM&qhi_6Cpmt^+qNyjp^guu
zA1QQy#on9YH8iCk)vfQX+8y?y<<m_&Vs3Q?K@XAhCbyn=;!k^Co5|w=guzsItd=zm
zn!Vwe|6tWPVH}iz^7U`VThXxk*@_-4WJ)#RuXDNN21<<atq*ILUoUn+q*xM+?|Iu|
zpRQprg{Jx9^WFO*r2kIWO}n!_EBCmvjDJ|aMWu$kT6oYdC(8kEy?GsRgD*QftK7-d
z?y;0KSYK!V3@>WSuk=EQG+oTfd`@X%u~W!?E7`H9PHLxq0NHKk#xqrFo>9u&VKF8g
zIY=;-AGdSZ=KnBWsF5lVk-qFHGPLJtC&b|2kqqK-+HLY1#4{v_hs+mV)oMZFg}B6H
zy7SWB2h7RWF`-EFt5+N$;C6^$(b_a-<0eO{&-!)re!II`=Y{I|GUHWUTXRiV-S}o1
zp^KGl2{h!kZtG@VSy+{Q`!hP&qv8$2QW*V#Yh`o0-q)bpGoqs<F%H#c9}Bs{;X{7z
zk*HRFXs0gi+TF%woMO3y<ffMq=;U2$3GvRV*(+VCO6n6-VODRb?AYlOhvd#zA`3I^
zBnyz;zyOQU3H^j^4Z#(~yKW8ddhH*p)r(x9G811pKcQ6)>=YF6nN|2?a4S_-S=Vq<
zbv_s9LtG&|{>4`RG^FM^0T!$7V&u}~EH5ObcYI`eLLW+P(HG5MsI3c+Gv@Ocv0ug1
z2VgeVC#W-0Xg9+7Qp7H?51%WFOwQ$b$#HF8oEDuo9`(CK{65~Hb?wFT6qv&F+3!X@
z9O4Q%GfLmTPMD48I(pumZG1!{P);oI6H?b8eZcBvY<eZgEJ5S@uPE`qptJu`fA@c)
z#NzUTYLfpE|EBaWHtoNH#7zGk5;Jl#|NjDsIhg;qkl2pi{vY-IUwQ9;(B1!;_p<z_
zOZ<O$FYABiy&V66z5ff|%g*utp7*N!YxuiPgGd0_VkvNiB_{&l6Gng`FfmQ_F*`&%
z1VxmJ$iRRT;4RM4<KcmU6Kq`{E5E)wPp@(~PuXv#U-)i(0463Yta~YqAzDEx`wXP?
z@$v#7d3nV@i2uUphR6vC!$&79Aq9o{{mkz)V)`K2A#s1-?Os7cGC)QwPzJ_Ru+wtD
z<^kZrfkJ=;6ru9|{;SgyP!JIKr2p6M2a*SQ@4p7p^a^xgkGTdHrwR->i1A<JCRA+w
zbOEzpvj_1%I56bt9Rzlq(FYrX00T7-h=<oVTZ&;Lr04^(1qCMQ{z(ZGo&b(;PS`tr
zd3-zq<?OucM|zIq@Bqp|ECLlExPlOK_qPG=-2-V7oY~v$d%e{F4uwZ-<zIrZ=Pn|L
z1ct~%RS1W}JPC!i4_gmJ0$SPz0%eu;kA=Z}4r@Fifame&&xHUw0{zYg%%15*5Z3MX
z6XM{sHNrrizy+*>UJ)uV2#EhlLO2gO0`}K$>BSXPVCSI1KLQRE5L}@M+YJc<nsD?4
zLWJJUJ=?NIM8qh<$;APChavi@WhoO^3tXq_%g*j+D8kvPeMcpNtb@B)?tb%YU50{q
z34Z%_XbI}GzV;QYb8*}EgAHoy2wFz{c@!wW{hnVBN&rFv1_owyAOyOC2J%|xxc!l`
zJ-Y_^w)w0UTfVuu1aSji%O~{zB68JVBFL%583TnCAl~Z#?)lliwSz){1X~lbjt8=;
z;{^+Sk#|kTwtfkVzTg2l0B1lEKtg~-e0}=(Fb+6KrNxDK`X2pqb9#$$UXpo~ZTI^J
zdr9dLUO-;|qpr~%ToepQ0GJWjAM;}G=Z41b@CW9@u9^<=-pmH%$5<t3y&w3%Pe7o1
zTaSU~`v`0__izB}^#yLj9yP8XDAv#1j*gI6Kj<CkXZPp_@4naJTRrtB==tYQTmv`i
z`{u;m<R^a>p&8`Thdg)WFhF8S#Jpe83HU>Pm3)ssR0GBu#=-4dz7iWbI0KgRFYn`x
zcyJq~a93YT2VzmoAOYBW<*%|w1xVN`7`LELO=ck7JpVqwHHAjg!(dG^+p>RuIsuA>
zk2lmQ`fT=_dd%b$#Lz%Qh=g`LDu;@RkbM~8*m7ERt9H7euW$PHq*P%3fu{&{K_s`n
zPs%E=eYLmJPg<T7k;E&z$cPj`dtqO+&{3eTieA@tSgOeU3n<@lUqt@9zoK7-?`nx{
zwvZr~_P&23LHFz3-_Qa>cm>$pnwIRbCC=hi=CEh0PRx7V*C0s7HP-NZ43A%O2bsI}
z5)R1iSB-hMgmWZFpMQwlyQoGSYPhDJZ@hT<z`42$H|QP~@2030e4lxe?4%)80QNs`
z9G7*X+hDsw8*qGAYkbA}<uISlsxPWIYXArl&Q9aAJlv{X6`fKi5CbJEPi#~othcpN
zBko27qkn&4$kz4L&tw!C^b6fH9oQ#adm+i==0+!V<M)^-$sdh8>_(tb-LmQ+M|0;s
zrqeT{I$IU`2v|uChC9fxF-)9L_r57*dv3J0=_4`5b4f1qv|W9IKyDx1{#jk&7q-UJ
zXsCymwx3(LHBu-O9i?F_`^|~LNNxaScI~IC_o&2;SlQ}&I4>Evbs;cZ5njcGa4lY`
z$J!YoWn1LEAPYAkRb5W~BA)26eA`s+Q6>)Ml96X}<4xDx$JN)F{M~`0pfs5*_C_o1
z0uG#_GMs|BcPq7Yyn5Z~?NFI6<~PTTX=PtB0x<0s*!|0nhxk`2?znU_az9b1*rrhU
z&r6NAZQa@HYqxi?;)~@kn9_V<!~MEq5X$=1>@*hK_sShg9vj=Nt%|7m#L5}-+_o~4
z9$uwsYf~r!{pDDjWbZLsAC9QPKv8_Fjf90%>usY>5p$?@4+Y)UUHfF9e9#1cdEK^S
z?<;K$c*T=osn^}kYD7)u5$PdP<E49!K?+mV`5n7-K*0$Xwe<;tI~TIYL}GnVeyn6-
z9tp*xJsWT!c=&#^g0hEc<BBzT3mcLOhdm(&uS6P0=BXVdVaMBw>}!kgGh*6wRAOL)
zWqrm~KIo3q1lB({bPVKRfvLi55xG~Njz#u6i7YnG2lIBy%O-xr!A;5svo{^ZAIY4Q
ztOUMaA3EY^MfY)9Hz6u>p{?HSTRUt6v~Vkg+L8@bwU@A797sD9!{w?w=-4PNR3T2I
z9(T6Z^+$cJX%VoJL&x7-EdnsgK}3;uFIg_aR}dl8<k%codT&;urf2BSHXAUpDuMye
z^`O)fqrzI}BK+u%I*`gSbA0avL$537lIaObOyL4cE;3U1aB`9Q@mS&s=`9Rkylb3)
zfFeo15QX)i%S!uqSf<4~2X9^3Ox#=JIdWed$_zis8mI8`^E0wrXV)EEXDmoG?=RFN
z$73Wy+n>zNKvipW8oJTSu~fo%yS#M*$E;b(0Vg>iyqRqL%~Np+^Data2P>3z78aXM
zn0eyWKm#VfIc@+fV-jrRmI(+)*ArIsspU<W1f$eK;jR0@m<%97+iuJdvbnI)r>Otx
zv3qidhBjaRUK25Ia>I}B^0ySFNN)XXv0IX3s-|uKC|^C2-uuR{0*sobV@*#(+fEKk
zW5L;c<X925$S&$0r728>3JIll@!8{9<a)4(J!qR3o0Pc6%`66<zn8T!g+aUnbhoY*
z`Sv~h#Eqm42f1{Ig-6i|G@I|*X+u=h+z($>Ewnq}uZX?^dcMBW5+h~lKh7J)CBRTP
zm)eEj5xz{tHXT%$lA_qi-I#loX*eTh)nr2ya09!Y&L#fIExm@@J>089crJ%?NwXrh
z?`AW0azGIi0KPBj+B21AWz=n0xPH<vYabhP#|3blK7TMU-WD&YL>1azaU`6lsyQe>
zanP7og5@CgzJZLnPWpwK-x9a-VGaZXOUI%Noeq34%thpap<vH)ZVatjR(oMwEJuw)
zFK((laKS`jMm0P_5?O6Dx4i_yOpKook*WVIKXQGj6doWO06acRC@JT%U1lq&bkgUL
zdx3?y?BHk}l88-+E7@g;rfS~MsBa#(MprDq#AT@!Ju%%v!xI|Bzj(8ip7x=`GpI9)
zd2_pBnvI9&=hShJ$n}3BZJ@Amiw#(?iu2}Djl8HpEEWcz*JZ^nQGiqD5~S3H@tk2~
zX&&;>d7#?^q8sO#s_Nyc`;UyO+maRBDHpBir$bZE#_EAydc4qyV$=!?8+Re+DTl;g
z-=&O4`FEsC;{f|%x%qBpmArZElV6qth1lq#YI6(4Y#$N~KB4{(UNfqLn{G?h;H)~p
zeDk`D1=S$|GvoOxYa`?nVv6)~sL9S<OM`T`xGC8%x!m>(*e|R`T?T(~q2hcsa25tJ
zc{`sL+?Me=gBZA$e#cqqG_(=WFk5x^tOY@^Qgz;O;}NG!-{2mTO`F(%(wVgqf9DUM
zO}wAUveK;&a)+ilwW!9j3`}W>$->pH^r?=9Qz*wlDdX2E?rW^+U;*<iazAv+cc|Rg
z&+yuw=CK|DYk6%mBc+%ULr&ZKspAg%Aef1Vls_q1Rm(9!eOw)HKgZspY{`*i-=N0l
z$JmWGk`28~+<e4%a_8PdBi!FV{!9(E!l?4yc{&QBZfb9Q60HnZ&-~?7TS{U7ZbxVQ
zz<+znta)7*Mc0>SpEU@|RNwu@8q~GaiItshW-Qi9liC?;cqx=FmCGAlx3ItgIKkEg
zSY+LvaE%XrUVS9+<qWmbSX@qWwqFI3+z7-B(rUM0-F#zW<W^QVKg=`?g*Z6|6rqw-
zZZ^J5b-9C6`yj+HL$vFx1<T_uQmmR=$ve|z`rp97>hKt%3DYX0P;hbAryDMuLdJFB
zcfHq5+Wg|xe2|hV@@XVm(Ix5@f?+}Q5$TP~>zXS-cmal_r-jzoLM}Oh!!8Y>LZy53
zGa=r!9YrgG8@jTsbL>QOP^W3^(og%NGt})0)~S?jW7w*Pi(!DQ!sZaKe8`d;M?QN-
z*}hrt>9cWUZ`v;?o;G?~C)SqHw$M?i1)EaAwDB_KTrhvs8GcN9nkwD3nlqnN*d%JV
z0hgJ@BKd~(DLfl780UDDv}}<li0^0jb1tmVFz-Q6&F#GsbX>2G>$EPHfKnI~o+I<Q
zpNTvZQ-<or_Qa0x{5O(9BIx(KN2=!uE*uTTIv=Q8vW_@Jn`dkL$$Ru-`@sPb+1xEC
zDEBqSQF%{I32YnM6=nmkbc@@pzQPFCt`&NW+;E}x<7SQX7Nx;aR=y+;W3&5vb$xFg
zs74&nhAr9ekHDqmeTUOc^luQ(Oo$pJ$EH@t>yv@##EruqRt=ef)=s+`KqPKjb+d(9
z)2swDPQ>kgtMMDj9BV2)RynkGZZ%esfqng@o`e|+xMb5KQObPvXOK&@yT!A4kf}t?
zVhT#JU(FDslw*32ZM09%Tn>t80Z1Hh?g8bnEcN{u?;0>HK~Dz9?hct)b4g*8Ysjmz
zIF}-1tY-YHI#fv(^zr^!obzXuyhm2KR3*_mHz^K;G_|cjlS;Mt++hD(OmG*>V#@fc
ziUzlu#8Z`$)M`$XtMyai6+3;_me+B2CBP|7r3btMU>?|`<+l<I+NBXEBmINr*tHY$
zGn7%j5U5O_>bx-zABwkobATw*5KAMUgv%uVitTi-V3;||UQi+RjxC(6K`AnKHO@T@
zp*KudUmFvBa83Srbs-3|UP^MA_7WgIC@`uLp@{jySRzF?>%7V!4}Hw#LAZ1+R%Y>?
z-mp=ff1%VxC49C>+LYuRH<g-vW4BuTXY@?~XXgyt;4a~KNAv!a;lvcwjoc0aJ>kVE
zlFwaPRPTH}<osO@BWy0`@(rm$7LEO}CC1NsuCyM>(GbTjLvePT2+<Xvn1|(K!8CR2
za*y4(|F0xnLkovLd5!KSj+S5Ti@T#Gu6$mWx#LswS3kB*CmMbbxpn)pTcxBtfaEu9
z;h9Z$q)$V}#Uv(-HQB&}RKcK#yMCJD%Jt||-fsh_So<vB2h~Aul;76u<trCi9mQ#Q
z8&6Eq?wYk;P_9s|Yt;N&%l9QX4cjhi3@`S7OnIo0%Bv+BGYV8mNV|`n(iQ=w->CGT
zwy2a|&42cCaysvpj2!|iTJIXSt{vxyXlfT-+N)^4vnTrEyz32em`*FWE2Ts?T?!83
zltyYJhgAm8j+DqPGWrX}k9#UfZX69wqDMZOAKbN_<zbUB@`P*euCE%&mTfaIHkFMX
z|16vwjh2p}b&FR(vbZ(j?^8}cI74ls$~Ax3?9acEBa0#$)JW@jR~D7wJr*>rtf6#m
zbS|R0ShV+;y&XLQj5tZ^-E=*H^(Q{Oq%J^RKZ(!Y14Y4Nv0!*bSWy_(xhIBD&MmEz
zTYTXtx5`3HZLn5W^K_;XOo0Bn5cA6Iz6ues%Hlln1lQ4&5#cGEa81NX-X~C52uhf8
z^>Fj%q2V}1X7S|i(B?B$GVc)1?NTU5bDt-k9WbmJVV>@5zIH9%ueS21dudtd`t3Zu
zfJj88<G+T*KN+uBn`o_d(R|ME9AhdVQy<QX2^!jD{Z!1jU>Bg$io^&4QqH`2QT==^
zsJQ-=Ig_SpRoBhX<|lB}<E4N4zIpQrtUuSZ_&_0wo0J&@J%N!x+1PWny$*+Ex9^8E
z*R&(%y-2Y>4REPZiVg|XnKgo-I=K}VUHLrwQ!BLV%+mdGcPp=C<PzQDIUa4fDlLh&
zlW{X$<?JBFw4^m!z)HrviB-{ST<Ja~tW$o8z}0B(3d>a-%u&(_aWeZM$>+-xvuEIg
zee8;;ASZR34URUanrZ?}&o@&FkG+=pl-c|O7~MI1tM;;sABqi5hpHK?JXkv;R54AI
z2^9xer12dIQ!P?W>hflSeYz3<sj!5hPL_|`|2w>_#G@TikG7;I7j6cS&-@IQm!=6^
zL&ilru?X~{?ttEMvxsHQ$W2vw?(~Zs%)$fLzs)u7&~pz=j;~*r_5V;fA^b55gDQTV
zTwQ-UV^q(N)f*UOA}NbpYwvr9zw8>S_kO{ph$J(@t80SZ+t!<~raFiSlke!F<M{hb
zWW%DE^Xig(x&i6}pr+J=lm}n|@D52OFbdhjH9?{mbhnTA%LXFX*^ML`q&09hvNt?D
zh^$W7j)T$NsjiB4+UO^LW&}!&F-;3>XAZWcTYF9qVZ4=NBndta2H{LkxG*40gWpp|
zc4QKFbz)PsHKIyE-c<1N)Z5~&V+&QK8>O($LG(f-wck>j*L}O+M+OT}Gkh57(Ai9z
z^78Ymm>L>2T7L2SsLs4VADZL{WlHM0CEJG%Ni@891Y01@v*_nGyBKHY8lzxg?ok{g
zSq!{nv%S@jaEUiJo*S(j=N2SIV-<~t69hfH$64uqSHWK5;l-9nRm;|oPeEpdv6vmc
z^g+fJ%xTSOWxQx$w#V8<GI~>RG;ubBanVULEaU)+o#G8yQ=>V1yo(T1ppTT!r0I24
zNG7HF^6SKOi3jj*){6KXY&b<B#{ZTJ{w>nBNgI8T;dzmhSg^9}Q3B}{ZZnZy5z2YT
zi0yC7=DKun8)0RF6mP5D!rp4*q?=EfQ1%SiGSD3F1Wu{Cyt-X@1?Jne{UQ~rVN@G-
z)Xd4hNFCj@GG^}BzTSTfiPfM_CWELrMY%cy9At2Q-TGkP7yVdv7-Yxmq7Jd)%E~sI
zgmvBNE0!$yybi_@N8k*S3vjoY#A<2sF(^evJ}xZ+AU_@x#*Xi0-J>ZE4K4DvtUEG1
zS-iUFYjK+{0)mbZPCt%4U0y`~THAo1ZH2T+|1Cka4wR0S9gPqxB>(iUzRyNa#2IRy
z<O~i)dZEG*`_d{n4a#gm@M(`l)?H;Xy3}}%utt(XZ;7CDm3OXqA|EN@d~o#|$KS!q
z{lL9pGhLwyUqOw)DV#>EWia}>M{~nBCaGg2N$aFPsd7day8!eiKXILG`%dMMG({W?
z!we%V_^4s7b9^T-Kk2pmD+)1DAJ}~$5*vM>jVnRy5KL7Qyrdgk2{>zY$Zk4|`W*0K
zA4Qk@!t4Z#(6gPw;CggFNHg%97vc5tFL7fq7rvzD3ynbU)#$cW*ehs|Uv&K&e1UO7
z$MzDk$CaAZ*i`e_bZ}Y@EW@-H7cB>*ii%XZX`WV|KdQyYO*#2GfN*m`jYZ50`o)L8
z-OKZi5*SEGj)a={E~jK%n1`LD5{NcpTIXm$k65~gddXx>N><QDPfM)w-Q;zGs!=}Y
z0<s3-($|4wEP8NFNs=UC&rd~>&8v0H?hR6UC%~J8uRkH()E82Z35@d{H1$mrx3{fn
z*(>Snc9PLOnoPgj_k1+y|8<Ab3Lh2Z*%M469DlsQV5c8vv`T5;Yt7hU^>`kf&TQU3
zs&d_SrFPh@o(+2r`q(Kdi*S$+e>hgE62Or^`)Xh02WaVNQ@lVvCm2ClIl0~3FC9Oc
z{u*%k`oCB^#~@kSZcVps+qSvewrz8_ZQFMDZrir8+qP|6)9*R)&76t(20tc$RpqlH
zqcW<ZGUB<`b+0B56IDAa!Nms2y{8@4{OOX$^Ocfv?y~EET%RE#)*C5KQ*Z^Q$n<RK
z^m@?SF;DZbP>6Rh7}Z+puDp=yY!tew-0?`{ur=R^08E62*K0TmE7LFnzpvp;jcBHX
zfpQ+y$f$NMZOQ&h)%`%%z{gJFu~&e;jb77-xX<Y?=)RGJK%pbBnmjDYdXrp@vby%W
zM|HB=Wi&Wfjc643ZP65w5msaQkm%nFp4Uub;!yIaoV<sg;}47`nfuzEXXk&DY3Uqi
zp#jHUUQ$0yPqJr2W3M{;)vWhv>YebK3?$xf98$JYme!bZP}lhU-}(aVSja2ve5<KO
zsDC<U(!lvwPi@18bI2c+7U;h}7#nkTeA=kqUT9lu&#CO>4zs<WFN{9OVXOQ{E2(jZ
z$J3l{W*2{%y3DxQ?LEU0V}iGKhdn!{zp}(;O>xa*YdmG}T(Oy5s}Wh%Ka*(Jk5F_x
zwb*H6<MTb@4I*pzyE`KDkCSn$7ept)gpajxCJtki4{pci@`U)>+OqD_&7S0pL&U@?
zUY!VrygA+ZKFqp8LH%mE*&PdU34``FtmtcrBBm$`>w4_IFs`6x9`Quh-S5jjHt3Yu
zy4lnbxB(Kn)Pb9YQ|&z`=6a87Bw;^gMuSUr?kdht#6k1!YOW#zR?Aa6;3u4Jq0Q6L
z3rhdnfCf#^R@{d&$6vBsgC(3)9kB{o?X{PbtwcXZ^OyL!hYQ=~1&+^A92m?cLzJ|8
z@1$Ezz~wSNe8HligZy^bf+RJBO^5?Qwqv{66PqDkO1}N|i&fcy^v$W;<Et2d<-PH3
zR7-}9hbm^(=DWajMIk=He2b+_c@*LKquk6?ma9j_4zQ)}$slbu?npKBPezo)wJ2Ft
zM<Rt1yWv=OE8qhWPAb$XmACAjj8yxc=u!V?rk!6P#;C&KQx*B!!g%e+SLaycU#s&_
zy^q0WE+F=ns!^p;uGvF44_=6No`g*iFZ&21PXMi445*UO<mpD<uEH=poBBohg=XkU
zgK~9f6x0(qr0(revJ5Y6TAJ<`8~u+XH@k4_MfJn%seP?Lr~-uBmeq?>>%WZ8&-n24
z<5)vDtS!V>Ie|@_=y4*axzSQ!;Nmg$TC-l5RjEd??D~sQ8vC14^kBSGjIXnF)V1g8
zdRlN5_z4}qC|=#Vh!6=eWX|AS3;q@eTL(Y-*W50bL2(5)2NGk|ulNieY*)$NuasRC
zQbhGqLm^^Tm+H}Zt53|AYx`~5MR*-@rZk2Ey6I`%7ILbDnE3jYLcv@|?s9gojRMfm
zG6)LK1L!=`4TDI5>4I;1z8_?!tZz#eZVdp<XYaoO5xGu|c+*gr=R<X=b(g-Cc;l>g
zjXkKrR(EBb=v5!#LB-QAt`X9yf3fMZw3%zCRpPXI65nzf>qQ6RH$>MV`)TBwRDzrR
z@xzK8Y(%#(g3N~JCT8y=p(n;rT%+lSG4R%VUr~?c9G5&{sh+Zmu1jq)If@C!a<VrU
zf(3IkgGQp<Zb7gk#>lFy6{T;?Azz>|8lzPR2Z3|rkQcyt6JKyxKn=f#7e*O{ooZup
zw=0}?KByW*W}6}9y~rHh(c<M4dv`Av^c9`<ZcZ?@IIQ=d$IA_LXQH;jn-&_bVq-tW
zcX(-%^?qEmEE1JD1-ue5aFc>pAKwGX@PA1Pzecv{E^_x6+-EhiFZy5#b-jq*m<x10
zrYQjdC;DmsS!i!l)Tl7t5zT0vD*Rfb@TDsQ%#cK-R3R5QUQUnZQ%zG`25jV$lPTpr
z?a{RCLb{qq<gxFd3B^m6fK{ch#^$c)S~`07L6PiMti=bxS{a`7l~BV%@}V(P9JaM>
zrp(h|Bw*p*t{N(GfPdiaR~(@ZY-;eM6^}07pH;)r>XT>EA6%mywHxg7I~k~JI->hD
zve>7GduhvMhO8)kg-6Qu@h_x3<%`qg-JI2QPoN>{3HuYDHH4rwnq5Vv`6P0u;FTlH
zJA$oZBIq@B6SO<%^z7=$6{6wYW}n5*Q{FL^ZisXR)X>e0vD^{;N3kMH)SO8v@&L{V
zT%ApZhAnm<k_VG(T9zB8w^%JyV$6|+KF!X#HYp~zL_PcRaEjB?3_HSk>8)jUCOK3U
zoc$<?+bE7=9m(|}Lnb|KV)RA)XL7H5cJqO3w$ekjI8HW>{^;?;kf+1})Nx)TiAD`i
z+uNnzRWWwHN5HmQ@xnp0zQlVK=pdpKXvnz2Sv`6pPqsJX<#a`@&Bpe*Ij3~Aim)wC
zF>wdFsKZ1yn*jLl!%y9@(U)D!5w|vF0ba&D@=Ol0B40Z(s?`N&Mk@!i`MH=U*fMaT
zdyL5(ta+2KKg-iso_%vpu}*3PjT-^hxO~#!(LLAx5VpW(L4tu*$tBT|0W6AcfA2wC
zSC03WB&Nh?bCcDJybsERfW1aze`Z_(1X$+bRvH*K<4mw;pM**pX+8+D05iJBkS;5q
zwm6*PqxJNBJH+Oqa3!cqeAR&*An&4dW#eoKFbT`jebMo!Y0BQRQ2Er>d;zc0fn=t|
zq~un2ezxZxLjRcyOz;w#wvioYZA3j=<NTerOyM}gp`I;duA6sT&aMjC?%^ZY5>%5B
z8UkAHnnNAtyb&zakW?rMQij>3iQZ{@+>Y>7+;Ry0+_g$M$?&B57#n?t(u`g0N*7M0
za$z`#xh7cgh8y#_-l39w{N|uD=7bjm@8MXbtX`o=y3KrH!<3cU64IHYzVy|ck=I-S
zVyfDsouA=RJ>YoVJ3xN_NIc5M(X~vR`x)eTML^#lkf-oHfRME3W2*HKu7O85j|Oj4
zS-_Qe#iLEh5DzOza<|&;EutBy7hww$B(-IfXs~+C;4tzPV4mmioNcQ}bVFD0kS69C
zFTsTmIE*Aj$TsW?XgokQ^zSG>>wiJq|2K-SDkq_#sQC}Y=lJ&&pPB7nsy92s{}+nS
z{?jh-e^9;u)9(Jq-~L}|{y*&gf2R3t|K(Qy2hC^u@BIG9-ezI`_x3i+Pk+LH9RCmY
zw#q;Dc1MaVpv72x%+|?O+*Jw&!*Cevu%80apE+@ASVba5B0|dOq@<uih`9j20D_Iv
znyYKyYtHAK>;~`c`{8Ha?f1@YH;yRjU}fz%S$P=}AX*SKurZ*82N~lZdpqsN-j@73
zU<4-SDfrDV;ZBoBp~rzl_`ZE%NkIV;J+we*UYW^@1UPeh58^ijpf90On?wTvf&l7o
z{N*5=Zwz>b;M)q#uZNKrD9rCI;l2>J<xjye0UOP8SVIbMG64=SJ}M6R1qWX0CZJ$v
z1_6`@FVG>-RZGq#^b4?9+>8SS_oW7f-<_RhO-ogngn)n_WABhUO1KZ@6$B8_t`WNz
z<P2KmRY(V*=L86bKzC+)Fi653z|_WWn@?Y<keUl1AONV=k9-3&;${f!g-|<>0^rHo
zudErbcLqD+WkBN^9$cU|R}B~dI`CWe+3cZ4*lzGzKdgayZX7!RB8XTYK&_P=MZdI+
zJZd-(6c8b3oL(%fgdPbS{x-Cihfr4i2j~j{WKazQK%{KvUU+3I*x5GL)Y5O<qYM4m
z+qJ2c{&osN9_|8&=6Z8_k7WZ2@@5xR;1M4-72rUaeh^<qmr!D@tzY>&d&jhfF@Ena
z0!@qG550ng-}(#`SOkDS_O`JwpgcYR%Kin}9d(;V9{WQJ;s^Bb5<b>N^aEI{Atk@i
zZngXcyhSbjAU+g-!AIVoz8|1}jZ29EW9QFZ04OVufg|uH=RyPH{Am<>`3oura3!F&
z9~hXwhu62}qIS(efan+SxBd5~HX@G>wGb+;|8~$fTUj{~qn|z@<Bttp3IYKD7A^r)
zSQ|OxYxbJ?t+(M@Ad2vk+vNW>RvK+50%ZJ^=hxNIWB&482D$UY^TA)9A;H8eWBvf_
z@YVMKCjf97_4Iz*-hRWqZnJz-kAD-se^-jlPtV`CpuYUW^AYG<&31BKDkDB;R>Eh*
z43B|sbE{Kket16q8PN0nn_LAORzNlkD#*nBwN%i%Wj~`lIx)xE>Lonj=OVDRha3lx
zWza+Lw}&Z!pgzJ0pY@7{<5Mp$Y4^$x&ljls;rYa@2vg#zKRlm`5YjJCK8|0}dcTev
zlAi&CJBv<!!d3@ZI0#Xkg$=~72p!5!BwyV3Q3(kK!T;^Wz!n$!TXawW0MYh>*#9P|
zJ^x#v&o|%K>E2cy2HNxO^$S=WSU4b$;=p!CFPZE8Pwno>wq*a@i`DmgnV%q|yxCma
zv$$Cs#dCY<(mWRG5x}2Eg<y#s=kXCQ!9K@QwjO*G;Zv_gPQ>w2TdLDQ9c5@MtX||p
z_ov)G?#V}Cc)b#=maI^14wB5MnwGe!?t*J<>MaHL)p5ywBIDvt#JhR&L81-d8q5-*
z(30K}xs3kOup^c{G3fjHmc4<Sz~}Q}?8St3V{}wxLCB+9*Z%QhH>hlf5=>6WS;5M|
zj|Fgyzh#NGi3_hrY8PB%K!txgAPq_T*r@Zv&E^FnlTGuxv)S8h*BI*evgP4p@4+f;
zOivglKvGj_9wrSl4y9J^YoJZ(AQT{NBTUr>WcCoEH=Di&94~j|Ku`TjwI;k-auuy|
zS?9R&IABl9wQ%;#XPON0<d-I0)E0-9Y|(7BH<>k#OvrR~0gH<`6s>BGhd`oeG{;;L
zB2j-rNI|?XDYvA2DyyWfyFIQv=I;V_OW5XrCQ<jS&D0hR<O}~6#JmVBnv^I$WFLs^
zfOf%Y&r0*Z`dUr1Pui}YQkLjUI2!Fdm`azW3zVB+73tg)Qh6`guSRJ(5Lr*mPT(u5
zP3tCr$)6rlL$P>L-|}np#<6Di=sWdULCvY}4bz2eD#|&|A8&lz64!Ojb0Q#K<EF`1
z*s3R6%PjaDW?b>QdcPW`=-A#2#`KCexA3Ou$SCEvyYqx0CS~{iv=JYB?FWYM-WbMu
z#P9eFJTSCGuCu5+!u-ub6LbbBWCpHPMikpVKVqarl`FaXT9h84i~HU}^D}WbVLN&Y
zLf993&Ux3X<La+LBDag4gj>g-o*rZ0m%)j1mH0Ec>FD(lPEaANqh<xR3`FN?oak8K
zSDe&gOf*>ox|z(3wRBPn$3MK;9cmNyX~-)%FTgq#pXkep0oxP7D%~GB8Oc68mO_@1
z@AW>PH+6sNzubqEinv#Wg7{5&)-XO|r;!@M0^ROzOVXWSo#+&CJ#*0Ig1TFhtf|f@
z^_2-_84QaPSXLsYkD`(ttSxlJA2Znqye->pi$0R^c~^<u!e_tY25c$%(`O}?#3R7Q
z?L9?dVDxUauR%%By7``usFYDDpt93@mK}T~T`Ovus}}|ry7Pj3=FCqPkw=9l6RWU@
z^xro(xQ5?sQM6Gev#0a((JZL>oP<>-%7I3jG~B|II&2PXE2F&(;iC~Xy=kyJ@k$+{
zJI17*Q22<BmeyzH#+a~N-?`V-cxq6Wy?lK_OksLG)B+(mK+tt*BMM(}S?VEnM_FEi
zi<9^h^(?@VqTJ=!UK|?8+PzG1(-;}h+}SCWD5n$T7<XR|C!N7ZO38H{?)Q9R38tc6
zFczR=VnhsiR2-V7mc)FrP~dGD&dP1cu@nfIhqbb&0}gm5esLou*&J1_*=N!7roZN`
ztaLWItWKvetRsL%N_uj+Z|exegKA@^6kn3w2M*kBgtW|b-hgG7Img^0AbA*h=fxNo
z8ztfts+2>#Xk=EAbWnS>-lEi+?98c|rwT0FTjmfce0B;N^r*W4`D&hE-tQ?RV0Vz1
zIM5OETKi`#yHyQ~u0JJrnzR=v0=v2*-qA&)kB}GWX3b{vI0ugw%z{Kne~$<IOhwC)
z<9lvR%(E}UrG`e%V_LthuCP!XDxRggVPAXy_C9mbM4kFr(y{R#d0l~n(spBY_&)ML
z9pj`+p5L2%k3%yA{Nd}LxsFc`o%axYX+x5)d-F=aK)1cV&J4%6oef0RIpP=HX@bP6
zkH_VhG@-ZLV5=F@HXvti$uXI4zf>Jt;X2=UpnHW<DjB%?vK-bSXVjIR6<jQgS&JCv
z$2>)hWc6dwoc2LuA{rtWNoB9@t$DE2z&DLHL%Kd!bHlm%IdUK&|Gew?XWi?nWaGeT
z-<x2RgkNtVfi<Uokok9~TWoOG%My9gCj&0YDQ}&9^_D=o(B^7&zpIy9CVX;_VcwP-
z!fj`o^=f1pIEZTQ>n5bhdP*#r(k|A4@v@38XYo#_$*L#zca&%?@A-CLKvvMW%dA~b
ziWDf=`I6V|IxB)DfW!4j+cGDb5?<Y*AMC=WWF0OXvD_)KJd@;7h!}_ZjIICuUg=T2
zb;>j!w#{^#&(5<WWQe_M?(!DaeIhRM%z~VwYTAUiL)u?Yb)qlZ#5uE~DM9mM6KzNH
zOPOxo)!>kk-g^b<mVlGGY@6n!7HJ0023a0PKcR+LW(%ypb6H|vqa|wBm*dgP-ufW8
zoRQ8`xw_&FQJ80`xNx@|TZMWu8#|noZqk>BQ%RO>prNMr;0dE6h_fdR>7#?_p|3)G
zor<G5tM{lQiw`!mmV4t2`n{kZN%d-mPqi8<cN24H@-ecq;J;62!(<d-Z7;rQf9v|3
z#8sL1)Tm@2@$mDd=y9PM&0#@w?F=VI2}QVjs2y^yTBCawG6sqrz7Y1(c1kM@p_;;d
z1>M*kX-U`p%B%|p+dY6mxh_(-D~aetwblN%49`CWKyHH7C1!?L%1lHpRk--la3Jwp
z=1GXRpChr#;dqD+vmLoSkMmk3^6+`0vkB);+u*2?6sLKRcvE@;WmeQw;v?i1_4)#~
z9MSJnGvJR5aiz_LUP+Y#J^noBZZBBj1S#HHUrELTe&(n`fZx^mwHR+<vh*ni26g5?
zX&B=H;}cY8D9t=6gnmy;UYA>uZDvAB1U3n<i(m5W?yqA!z;9)M7=oM$7a7;e0Z5sA
z8zz4Xl^^0+cs#B(c|gtTUp2{sPc}H^_G-FJV#*0S+0`Hj)}@PfwqP*6ZO<mF^u9wx
zZ^S2R_#AO{+)tcO8bW${IurhRN&RuW9l7omk@*p2bWgr_EKhXhVH8?*D@esEMW2*Z
zAll$1DrYH6yXN7q6_;qGJzWH>Mw50hWP6|9;9~VH9&d9~iZ6MF$;inE8PAOeON7#z
z&4Z(AIBuN@zT@YMu7aBaMTs!z=ysRJ*+cgW)QOFbq!K?i?OtvPnPjTSXPPyRk<Gds
z|4jJZJEMDHN4jUv*a1Uv)rGg<{gs&1<^}W1;WWPPj!JhRc@DFw2k~U;@}@`?<a4v%
zdq(1Glj~Ls8#yvtQzpB*a9I*#$hY4lM@_ShFt^2oF*+PuW(ibg)o*)J<FY0i58x{B
zY7rJ};lUM~gn|bhc^Egt!Nc)gqz0fmuP+36Xv*6T!ddOYd%Jxt44%NCp|9vmg_0v%
zVf}Ww@B}7yx^%pR^X5<V#n^;Y2qtIev15$o(*p4gU@CI#)VZKwi2v=AAt(DiEYU?g
zR<td}bS|dS>a<az>>$xv(uHt=@>Y}SI1*<U?NeDo=J@;R>yDLcs4riK$iu*joCM$b
zxLVD8kq_Q0(XJ>rXSiPPZyDB}2D5ggYSut($QxFkp7BCiB}G!Cv7-4_gqrQhF~pNY
z9g{LU#tw4rw2PGW)Ds1qLE&zUnRyy{ey$8zNkSo%UXt6vj>7^&@gRQ-=9-~~irl+N
zYfFxf{-Q$>YmN2fDve!(4p=ma<H#ZvT|i_L&3J-#_JjwrCC&q_7IzbUMg?J!2s6hW
z1CI4P6KB(wW<@<y<?C^OMTH}9om2272meGw29nQJ?bY+Odo=B549%BgvW^7wl;dF)
z9e+671a@cWUT?U+PAxwnRJ*xl2@GPag>bc8E7`i2qD~XMo-V%)wHt%f;N^Ie4m}~v
zYZ5{ANTerV@K9<a@n0=sd&wVui-z}&4Qs3({vDUf`i1rzwRGwEXUIEJ)q)-5Fr{eG
z_`Yl0t&8M5x&i9#%%k4~IX}IR$a;<vZTjR}#wYt!zpNq_h~u-pkX~a?9_uI60!0qq
zY}>mmPgMWlDR0Q&EF|Jn#5X}Z*(a$)!BRBtl5`=Jo(}JCmS7ZIu?UNUC*{+V!+PQb
z?Qrp<N~+=s+Ls2?fS;aaSVbPY#pi5vfAKuV0#zpVlQHS&>=o$h-`WB~*@~xe9To3c
zw|c`*nI-QXu;+0M`f|{w_XTTA*7Q}u^1l|sU?oMzA|I?xK#Dp)eJbufh~?6n_F@$x
z(SQ$Z+`g@~7wXKmi9qW-a{_RVq&`fhBlj;_bPSWuy@DKZG{sKZQ#90YJ2IdB2gz+k
z0?}V~DC`0n#Y3cGoUi>@)j`Q@eb^G&E}BhzFg-d$EG>H*(C3~J!f<suj~bum-m8y8
z4*(zyB4RW#A(Az)?Gx@iyZ4SatrtLXE|uZvT8zbu&kHdKm#>2)bnGcdEeE~geMWEd
zpCs$HOI}?Hb8Q!;S7;gAs=vC##wdl{$*{r93HIWNC`rvgL0`2L$%Ot^EXOQRrQ45U
zW0dsKV=c#$*Vn`PbA?CBj5wbN5LG>t`ml#JBqDs6gOlfNDgUGx(?ORGF0P2y4zMb^
z+j+=X9t;3uJzB5}2vp;cWknS){_RWGp$>-E!%W`KR@z^!xQb0EilBa_@QK)8(MkQ=
zU1Z86s!RUfn|TR$15sq6P!aw8l$^ODZmhvd%CSl45L0)P^swtRV!UwcXCGCQ=nGAm
z!RgW2(G;r1Xs2f&c49PoydwDs_Y}NdK25I&-@pnN9_Hk&sU1t;7wy@_H=xFJX)yDP
z*|>PlEif+QLT8OtPd18GZqp@OizdTjF)z<6^!2sBqOhprv+pBmAQ2qI<ZOs64mTxz
z-eho+z2IXWs-(3Qzv44>K2^lG_~Oa7IxA@^uSG+QS16oqFH9@7N-*JLV*nWtsUY?h
zJ5Y%kel~3wP?Eg;*_P~=y^=faRw@98N3>MS9(>?g%n2&zOOC#^V-6jy<W04+ZvED-
zVzFORAKtExosv|PLkFTr%vhJ%bGuoL`!M=lq3dSh!g_Mhwx%P+CmXCx2|Z|{JP9SY
z@8I#1E@&T%jeS5~wlh>L1$itmns1RgE^{=&Onox&M3Uo}b)8UwH<k0CbNprgSDiB%
zW+dfjcY!pV%F)NTp`Lbr>-m;i$<$q!?YLR;!pJ~L%w_p>w@FoErR$8DPT7+c<?OPA
z<C0@Yasjz9i>wUhD_AMHhYFv8aeUn3<$HDJ;G+FmXFA-(U25;SBDvZs7^X*y?!7dZ
z-!%8cjcNsn$qi5!t5Ih66GgQ=?uJHpQAl4$h1pC$`|SIWVIYA8<>kR4I-Af8(XqRX
z!IJ&NfedPI_QRGLc#;7y51>XWv6er%09ck`S^QWq5W+eAT2>ZGGvoBcI`CjqyD6S2
zfS8)vz3!AMb57WP`>0m)?!bB1B_y7>HvgvA6eR<U;3VKd^1vc=+<u^uh~UCUAazE>
ztAmU9liHU#lqRw!VCtl7pWj2kS1g)|F6g6#t5&S?Fn!6(ldMo9ALvn15UVL8HFF#q
zPrKr-#3&Hq#-BlW>;vSXh4f1bYKpXd6m@6#Vk&FFSLNMvZU61;TW2@dU#g#5w(lW2
z{id{#Xz480t1{}CKsTj_#V-+<K4})3^r5k$-GHx<35+Kq$mu3@u%uUHK4zCkKR@ze
zY~UPTrrBN8*mMdld~b|GxDPtmreE%j$<?#Q5TgVeR$yG4Y;mjWbj3^EY#vzzXUwI!
zNh6q5Lf4<!?yscI)LtBAQnp~k4ZvUtZDOEC7tu-Ka;DwVGU~Mb(ytm%Hd-cLu4e&v
z!@M2-dJYGs`}*qLy4D60$t_jH%S!GIh6rL&f5jYBm#H>Z9#_{55+Vt`%N<_V@MGzl
zDWWs9w;o%6%b)(zw}=!6>RqP9bOJj{u8QZHfFB<NU|&9lxF6B!I3`vB8u_5v(KZs(
zqM1|M>s&mmkERRh<jBsr8`cNE4#n%De?L&cf4Z$Mog_c!p3~+<j{vEvQ6t|B8sp3%
z`@v>`aC}f2O3J7Jq^C-?gLqpKVdQakiqDNY_15{8WA@vM3mVVdAY|z@Nj0707v3vX
zzTLUR=oZx)_cpe#iw&!25@t^DH>HkYRrjQS>Xub0a#OJ-pk+SMCePX2fH9it!ve+o
zOpeDHr{0m!9F3a03+@li-(~&8HE&$T73}YZBx!XTo6|PZ+=#3nSJY~sHitc6jIJ`t
zZFw)+Fy0ev8$E;@uC`nmP{UxN1|-TTyL?MAevNzeR_h8@P=_p24<ZRWN?m>xbeZ=V
zlf$K$p|t2!Jt_}he?5eMeB=N&J`1d{QY)a???s_RRRqvVDi?sY;jwr_uSVRJgGtaJ
z!dA%<ILCB(+Kk|rFR~MT28=zamqpS?_x^dfme`5vG+pM|Lb1njTjAFV-6q<p%WLoy
zn|mhI<uyQQ$kCX@u}{jUegsb)qv<kPT3yg!L8R>#{*%s%1^M7{wy0@T{NeQ%Y>pAh
z%hDSkqyZCHR#s2OIGnmsM#A9?{15W|1#jue&FX2<5kh|d-fI&g=Hjd6Q*Jb8<9g{h
z#z{1-qJt$|&S3S(`ht!;_ejK)93NAOhx{*EcFD@0`Pu@1PMu0Qb+U74ym5y8Vr&2U
zCYMcS1k~h%*T(Wm99>6@#t1^$nryrpwD-J4TX@6qjcKmivd7K-eGW!~#aw6Lh;3@v
zKha6Bu0s<@mU=Y9&$=@Lw!u{g?Sl`>xh~OH!9Q<txniH!Ytz-Nm@05VGJ4LJ?e1U4
z42>Z<>y+7t%<WO|)KJ?YmcsHUCl2)ILS9Rk1_Li!+~e`8LNR|?d+53e>5afl-4BNI
z^#<{EPbRc97EOVnQC=<w{j;~w=wL~T<h@~4N=X2(Zp12iSA5C7-_Q*fB&@D|nozGp
zUG}?90+Sv`mP0KjM%S7>X|n1)sJuyTBW{sC`L>Fnr!$xDssLxGCg*P8tqxdarSOCG
zH@QfYQ0b+QVZ&Vq6L${8(wx8Ngm$~4{Y9J7Iv*Rj^hToNFJYB>)Ma-KqRVqMcH0b)
zXzJFju+_gi+`(-XG{sx}znm0V9pY<rsq1pZQL+y5F6bU|b)Pdx=Xb@c4^wN(Q5mZS
za8LSGX7E!Wn+%&i^@*C_ghZ5k3Fb^f?+d<JWh9WI6H(Ub`;&qT64}7O5YHwmt#oo%
zH!)84azkpB@K1bHlhVSFQvlAYzw$DbQ3yN`C<(TZII6T|TZH2>Uu2y<wwE%x%2+Rh
z3KLXDZ}&vKJft=6cEy>*B+}(a8<3Hqy^~17p`T%&gRZB6%k!jPYxGDi0NsOdgL}NY
zVy>h~osECtKtxWhf2igL0n~H=QXmTK<L8mJQ9rjLnCa%4r@~m-(I4Y6fH{F;yxn3y
z<SM#p4{(ee@wU~waXpF_-?^m5Iz^x8->$&c4dl<(=$Z8wH^fE~1rNwHK!Ac_B)L<r
z8zx>qM6Ej?acR(@JD_0FB)d(|3ba29P*zJ+<w!(^rAR2kK7!nN*yhWyCS4dzBA!P-
zmgyK#c?Fl`R+HM!`QS`7c@0I0(c?~}Tojq|uK)CVM|B(PjyF|BG-*`lb?<Jas^A+&
zo6$`1*8U-rGXuuW+%)6&(ft176C15Xz+Ln!QZ=|%x1r#X$Kq8&$Me^SG_755kQciR
zZR@k5X07noBklMO)Yvk4Qsr!{58LCFiKmD)p!*5-b$s%5b8ubwFFA%(`rms89%B0x
zZK&7~<_Cy+cw#)ujv6>KGNiI*sH(kM*(4lU5);Vn;+zR*D?BWS1qn)wQORf#(P`l$
zsIe!#9Wy=5&QM3;faY~puxELayv@G0S==$svFO-s9GTfNW<EDw`u0+4VDWV>RkS04
za}|!9a%N>4bbP$J<^^8l6b}7)eEV2+%G)g(@u9sYDeJwM8<eO4Z2|N8HYI3%UD@59
zAw-0IsD0j_tN2}b^rzKMtcgcY7E>Lauxa0Q3E>>b3{!Pmqs&-6E5tF`^l{t<>z*!c
z&DPs`PR>E8YyAv2SJl^A>nyv#DG|7rsq|}jaaG4t&V=J`JxVq$cNk74hi`fVftxl2
z1nOD8xdfZhc;|1HO%B#?Q+PtICAIr8+bqrO$ilSXoz97abM(u()#g+sFH4BfZa<s`
zo4~aN(S<M=axgQ|?s&Lb%x1UobA6Rb$jeEMHr%X>nF<Gv1M*V~kFDp6pjnW%;2`qi
z2kocBm+~&%{rB3E+>ND&3OHsc8eVgLMyW;iu+0xARN#%MX@AuSmqBZV3!Np~_A*6L
zq}3_N`6ItrVd-?33<}-l0C=W3DXX<s0)pfUPBT=~7YwYezhs~l?+32s*Lot=e4D8i
zr9a@%|1Q6)R63=}O^c^?WF%FNT7T#KbtTvDSi|8=J9=SmyDM`FGCzs-Xq&;jD@>_@
zy11$$XFsjFxd+n26MpU<K!O+%Dr+^&tbn%vhO9|erR=SpAO5F4%?n5y%OLO)$Mwl2
zZ1r+;EGXO)T<i(ecNcM`_EkEUX7bwVk2#cQH*jWvPg%kTNsHZ~m;8y-eMpGYrIJng
zIZuXmma&Om*$A1Z*ue#)xnzQuKnf@Zk{1snWJh1~Ds98)!}-$>$Jg4fron~9Vt@P{
zo>LC~a)%WSZR_-H*L<uICQ}8!-hF#^iT<eq+>@;=$M_yz#}Ea?N-TkP6l`^MK)vhK
zpA=Zg!jMRxh4MbKFmOh@SvzoP?DHO`N3@wyECI|V$Eef1?#uo=S7zfudf?bT<q^I%
zA<ZA?weQLDwaiY?(^26~%}3qL+61ziZImzmbYr&%AV)za@898N_W#0M|8IC%T1s6`
zK<pp9{1fo~?<8?%&VS+MAJELs%Af;9FW_ioV&n7^3I4-vg$(S(P5z1YCSd!={uVa*
z&ox@ce-<U246H4T1Z;jjHo*_^{m&$E_Me6S4PG{}cQ&y8KLm&WufX{qy!<}{XO91J
z^Zx?QOzbS|{}RTT2pCzI*_r<J_D87y*GmFMR!#=i|5xJt3@VSKJ`V=9e`VuO*xG>%
zfu$YL0S6b@!R^0uMPeQR3Xia{`Evwpja_wTUOa!RJfo>F$hqcTm0V`|M`%j~OAMkI
z!z6*Max&L3HrxRZ9;hg<r~_73;!IRlUWh9zm#0jz^?r}VmoE6n$fgi*fAP@_;`obB
z>8H2GGq1?w8H2~!*8$er1FW>ZuDZQwXaHVRR(gFigK+*_ELxjZ2JmhI05cAE1~Oa{
z#`t>I+`?uTCg#2;7C;l33jiA*AAVnN1dyniUrY*_nA@*2z5#Rw4QF<66f6&jsSbq8
z^J4_GDlfR9frk$|GBXpqA-)Qkd307gECzAM^wb*A4c-Zes~t!NutyP$0%H~ETOt}W
z94gP!;PjzI5Y+hS!t?+F#1E7$g^Zt@{00%7TPcqi00%dZh!$MF5q$0QH_dYkAn{J_
z9>7Z4>X-BL^r>D9&E*%DwXq>keG_~AeQhH%c$&r<0ANId!^G6pR1^R)_q<++D4Q(7
zjXSX#u@pA3QEi{zLNq`E(O&>!yO8gLW8*z@i|dQQ3uB9WYOxeOo1cI3T4q$>)it2Y
ztn4_STjUlekPN@`tG+uq$Er(y8yDSgA=4C>V$+^E2Io36g@y(vJ3z_EZzEtd!q1d+
z;A}wc%F4>lN*sVZV1N@F!@fIezw%I?8+y`pdPWX#e0t9~o?e(Nu$hrL7>KXHgG1v3
zC=d?rb^!0rFXaz(!n!&@X<C^qz+rwzDel7Utv<uN)oqLoUok0HKR9Z)j}?H*9-ptz
zLhu|0rMT?q5!?G7%?Tn(N;<;gzc+G!zo|q-1g-$+$!ypFkh#&R{bFO`dk2QVZ(eIM
zeW7o94!#qpFg3n}uHZxGTucc7wzoO`BA<JHX@6tFtob&Fz};*^KoQ(<00TDml<1`<
zM=pYVL|%U1!+kg(d~ct8-`#z4eSW9L7e+=;>Upg7z<&8Yh&HFN;eD(7jjRq~c4FNT
z+ytzA>M6nQY`@dMvotx?efDTtm=WJZgsr5ne`twIkxNLq=9MKBHG~gsD?Rk^`faYO
zuK_0W%ck_$-~b?FQ=fm&`ln6}VtG|?_;);}LP2bmeTO8~G6SZ6Y4W<z+4#j56vTrg
zQM>i8w6y~0TJO-Zg0_FNZUCT}a>p^hKmcUr!1ux&ggsQ0Y-j_Z#p~twAhP?3Jh1Np
z)`oqGb^@T?Zim7565Z188Gy<i+75#0yFX*s0jn4K5W@jPKG+UI>c_uE_i7va5;LH;
zZ9`0me6a&LD!lxhLirZy)Tfi|K}?IhvjsW2`;UcxYL)+~ZThG7^QU(9pIVlGPF49L
z;;+n|=nX=~*c^c5&`Ob?Z<Fop5C2@Dx~?EToE70iKArPB#qops+w*5@)^9)kBeJ!3
zN}8W-$KZ{f?3g~h>e3dir#Ncpf!!F~k);WQ3n;rc_0g7?0ddE+XXnHJJA8Z?&IP8C
zmDQpCMF`9kGG~U*1gA&%ii}OP+4lu(x$lA<M2zVj*x@T6#g_wge>*O6AKmFwCi*h9
z_o(G7m2-6HHr8Ap_@0ko<=ggW3>}-6+lm&z<yS4h_*d98)YRq~JOo|pBR+z*eI_44
zE3U6GRCkVskLM8(eiN|mBff{-Ro=F7Z>YexLSSOucNG|S132Hl%Xd1AY_o6ScQ&*v
zgO%q^p}&u72C_DUCw3}d#z%z<T#pze&SW3x0SpUk^Q#;L+|+i&x9IN9yV$)R<9ZF*
z*DMg5orrHJXpaPqEWg(HHyqV6J$BwXe)J9yw>=PF%=aC5>h#zVAMgm+)SavP&3pfz
z^)AGRkxd_|qYLn-J;24lAw0xY&N)89#KdLn7;vu3H*mnF=LcZl;oXM}+{GC3$%||0
z_r?qvUChoE$M<7IK<d8lOU<<;e;)S?h=mWBF%g2b3hIWJ8l~XHa3c>)OZyfQV#Olv
z)S)y_=D7(Wc<z@?zreywW0XFFh($Gtlh<%&+Sh^2piT=9vp%T?)d;3wzNgYd6lKl#
zu}KGK+yaWQG4J0*q`Gi-tdb9?O?hz9P%;6dBgUYTK%*8f^6#=C3$@H^1bt|V&OQP;
zGM6ukgiQws)G`s-wyR%xMUjZnhoy;U`BYVGZy=}^nBLTDI3h3eMoQ#dzxCbQ=o?D7
zgZK9mt<6APH7M8!V@hD$pY*-x32n5CrMr+pnkbf|)g^#NPL;`puXX@71`#7->l}`g
z9aJNzoj_PVT#;qW^MxMU2a%cDLMT9pbbaDTve&VVj?A2vp2*Vna$bi|%8e%o0jH)o
z6*JJ1KMaj%uHhAwT6iv~YZKPwbL-S6Q(X214Nh9xbW;m<dGb7E`U#(<HioJWs|wv}
z;1xU#-EP$lY@3!~t*?m?`oa&%BVpH@y~&DbVBn<N=-@N1hY*<OSCUJ%_UBKM!yu6M
z0WX$^O!*ynsD!u(m&5t%bS-f#L;<5XY$talJZuD^Ia`(VR7n5C?L;PCG~MOkBptNf
z%e+b)=x_>o;u5vF4Xoey7+PsFmUY~J^g7EV<>k85FJ9vh;Tc@SsA;VGzPwiT9jz<N
z3Ev}hCil8`B}=KRT8bvw8;uqnJs(Qd%mZ5SMP+=p+g>~CGB-BQUZSh1Dr{B)mkzjR
z)^;HdpmCyFgz))N3T8@?YKD~J!dH|QaTEF+6>N;IF!7B>mcqXn+qfqinO$P}er*Mn
zt!_Y-eax48a+g3Zb+>;jb5zEDhKm#7CKC$r<IDR^!s;OZF<3sDp}7@H9817)=gZxw
zBf^@{j03c13>agq9oix~yhyH2)3(h`-usY3-<+xq2JbW?5K?Ur+G3h_g_LT{mh8LU
zRs?%Cs}v;_5>~|YS%WW|p3PNl!TyqmM7%>|=bY>3=5P|h4SA~_8=To|X6&gwR*YW%
zT`qLhfR<-X?)$||xJ$7VT%!SyUyGJcc_QWAnk;QUne%vy{suG;;3JD_m8n@0ZjH|;
zJJ{dsDQwu$J045#KUfVMnXvGFCZ*X~Q#m)|`D-(%CX)}p_ASfhg;qy_bDx~Puapb~
z{FhP@bm2+^b)@+y828PF-Ts35VHzSGM5I~&yqbgOuoJ*}we`GQKrjLb(qAE^mhk(-
z%0hVChl#&j4B)Vjk%_O#k;P&2rLl}b)hNmV;RUyUc$MW&Onc@P<xD!?vc|1YgkS$|
zhd4t@*c>=~oHn34nF?Ukv=zq`++*)us@ZmBzv)PnJ4NTo+f0QeG-o*4L|H+y)*mja
zs7~w9Up`zzelXC?EKCi?fTR<tO2!tU1h0?FWm4N^Zx5_S^40j^9X&>aUS7hJOspX7
zF2iwc3x1Pyv9NpoN;*tPwB@T;(Y9NmU3uPbp)H9IATF9&=+ZLY{F(F>y9`94<e4@R
zL%k3C9%lVF<_W!=UYJbN16l)uC74Qg&uHhx%Q{TkC*=pew2AUP9>6UoU1VW1nJ0}s
zLqAv?Z_^J!%@d9I>u`E!X?jw^vUYTtDpST3ln2{bdLEs3NCH_PH`9-oFOZ`MFRY>G
zBY@Uz+}u%7k=;V0FU{efl?N9Tl`Vq{*SChidejb6(WO7LkZF<<Lz1PT4c}ira!^U3
z?AKZT&Pzatq`im~@AUm>4~AQ}l0&M$b4KmgEdK5V!8huEW|07Oy;nlo8im+OwTu@I
zB+7y@bqF1#U;4O^j1BvJ1RD5EJIEp)(mNaxZrx&ZE*Ppo%}Be8@wtX8m0t~?K@|Q_
zJ1)EG6tqO<MPe@k4H(V|)K(8y2wA4)bwp0DeF!1guah|DPdxQ@w7l)MvN?~V5X^qL
z0@`MN{tU}YeAdP|zHq3^Nl4a13fesTp-oht+061DPhj5LFz7*Pm>wf7YG=L?+oYbI
zp(<}C#!=q#lLt_UA{dwPdrgy3;H62_=|{TuPPwJrRtq)6l?(ztwwb(o!8^X<q6W*e
zLVR2dgpq|-JUo2z%MI=~Kl}AzXBLurq<AyZj6i4=SlDJg=)4#Q)6LGUn+CfeKyWcz
zP}GK?aRr9eV<6gFUBh!K*))SH`$x#CBn_OTQU+G;E$@$+i*pOm)8fZuS<Tj>T+hs|
zlox*H2kc_2+VA9prn!>#StRyQIPrBYfv%gu$V#b$d?$Ixe8P%D4g%M^81WFrVW@IM
zj)Oq&^_+ucC{2uBC+I5}zE1g1(xb>fdqLvczz;F&=7<Lj8yvEcpf(}R;nxkswABuS
zm4#}g=+mh!)NNkKE+|FOgL-#)VT~hgr+2yruDi0~Vo)iJ^gp1+!QCqsvG9gr2!P5B
zMnO7!K>4N=qjhJ3U}k3b3W9hjq=hDslZ!jd-orKWGXVn$U(N4)1)sK@7v@9X>V!hs
zZc?hUn(4rI?0iV*&Up}A^E1qX0$t(zX^NF#+WtZ1LIID|_5spAMvNvkF=9^ZaR9GJ
zZjP(@;aYC#DPT@j##w@pJ%&4om9IbN$_FOanP>@Z`7J<`nNqQhPkdE}HO`j_oilK)
zS#cWLO=!E#eFWt#nXZrDpr*xECVp6wsCf}ahwvVNu1fxSUNiuHMBZIsTUnvR(o?%l
zYo_zh=^X1HsbqBFGNWM8zlFTmfe<{G7+}zw8DRA~Ek~`lSy?lO&GlmF!*U@|Jea@3
z_ofK96duvymh34M%vi8NvYb0i)9LSSd7{;w1u>tKOFSQ)Z%9+RMp_T*PnqRpN5$@a
zA_e3~&Wgrx+iK8r11YB%@!JVbG22WS+b<`Lg2_=sUJ4p&&m0+r{izcY#TW1(+37Os
zv}Gj8SXx2eK~G!|iK%6DP2Sijj~>k#6syH+umxFuPuw(93m@Hzk+jmp(!D=J`u;@y
z3o^YJgTH&9&=9x3l}4PM!+^|)b?3e>8WV-8S_Yq2zyrbSL}Rv~T)r^QVohc=8K5ir
zlXA`_M!7Q!Hi`|mT)eQz>sYXY6?O2A2sZ)!VoDIt1qrCJ&2B}5y2(w6(!rH>J8qU}
zKuLlw%zvhl1j^X20QOH`5+KBkdM}!PyI`ZjOYGG?`l>#7`pkzusJ@@*F2k*Q9uV4l
z&C0|6O25v`FxRvD?9L3fxa3xk7EgMGAiz|0ioDx#^9)p-(F*U0P?{zHrqp-eg5mc{
z9l^c3?W>|u!jReF{XQ`IjrssH&(yTiD7us13<oht3E%Cma;zPuJ+Lyz9#)giQk?Z5
z;XydgFkcRbPp?N=vW9$LKWN*A1XV()XMH{OZGSC1=I|I|9q&LXCLc`b4(+znt6A|E
zUfSd^=$mF*P)Vcwy}=u1@uJOnacI>yzV1q4;zUWcN$+RlIaY4C6=c{#Oes7(buTqa
zOI*{!d3@Ow?ev3L1Nrg|<`<OO)c83GMLZxM@Yq1MT~jrLyoY2+m&r%OtmVKt?Vu0;
z-a@%9A$O!1Xc-jWq*NR>)R_iLb$`?Efq{iiIkD;IG_DXBm(VV}!Z4dF)^Xk?O=u6R
zxO;Pfs&#j%Hp}XGRhu}_y2H3}N``Wu&Nu-E70#*6mjqQ(=-aLiy&Z0J9}KJ^J1DpW
z6|8Nt`Q=MT-jjOcr^xj&UUoQ>33}#IQrBRO*}Y>bv&B+jAra2()8QjBkXN00<lnj&
z1qm?xNP9?AyP@vKo51i<eV)0fI>~ggD`34a*ua=3d5E}kPTc`pr0yoCQn5?|6=5!Z
zB97e%5O5*2D{6IwuLlhNp!`9j?;FY!*oxDjr&9P>vj%7<F&ZyCPnfI@*s;^jejb+J
zX4{m99cVqFYp<|l180?AD5%+eu?_&U!0W^2nK%Kl;OB-7hcZbWwdTyO`!v|AA#XKx
z3XKwN&Sk(Pxs=Nq?RF(fLE-sZg|GpNT;<O0On+jWrnfVLc~?Drx|nDhdgz;!qQ`rz
zin2U@gKKnDY*8pD%ptK;$DqP`{ye}Q&h+m|cUQNxmiblCiMnJWts=AVpgoTG)%CvJ
zbW5(ih{-vaZ?Km7z9u^C^07i6q#k2q?YNAbKZyG0E(yAX)5Euf-S?Gkk9FKhCzs`5
zog8sjH>TZ0XZnoT0q$r9U{SlM7#|C_26@A?DS`;#oTZE|7}fJ&5a1qRxipNwjhN6^
zIXhJ3@nOW1!mvW8tl-5!kL*CW(qLPYp_AvFy$mRatQy4#0mbDSDA@OQq!=Uok!+|Z
zM0ih$L%n)Bed8{q(r{2xt*njbJr*Ejc!>tAbd)K0L@icEU;_1Au^4XOHsu$x^n&2l
zelfSnZv>~wq*&LN+AyaAkWP0#Z4?0mR){*&Wi;!z!N+_NfdPS?6%5(uuu6q71A(xO
z#@<xZ(Cc(2`Qg*Myw?p?-t;O`5Qx@igr8s#`BC(0+1m`-{Z2NN2^E;eu<=;;lrFt=
zB7Z1IvOgun9POY};_^;s?y|FGN73?=A+O$$(3Y)<`?Z^HRMBL#zqq&B8gs^0AxX#+
zg|aycvR@}=-fUvrHW4iIN|JJqx!l>K3=)#Y<pJXUOJNU%%5k$Rem-Pf4$_rwSAQwJ
zuypx4Is_8beP(q<^u-cQsz27EY8ovHu6;M|QoC~xcLFj1*FVwbz2?k8)?#PRf?GQ@
zOkifJTiiC%7q70VEcf%JlY@?DB&}D|kJH#;#9G@e_tO2sG!}IQ#LDT$&aV^5p3vpY
zjV!GaeWWGOq4=O7lmy)+0s1$@%anWKCQovrn~hwo={oM=`4o-+X1ZquIp1B}VKI$h
zPcVUz7tRudTq;LKWfT%eMtYk=>f9hZVb)DLLf4Q~8hCmHGV%V`<53Hc^kQIsm8*>2
zt6U`#;l)u-Ci37~KIGxihu|TNQiT;!dUd2~JxIG$`sG;9p-n@{NVaY;^sa1=L95a4
zt5nqooWL{trN3kuGI1Fp220~A14Kkx)3zr`HVzUgurFjk5%JD#L<{}%g={5jk|x<K
zTG&7Nx{gJ^jdnz&@)X?hjMGaPgNQn}`17lpve)PdNvL?M$TxgPQxQkIiOh7f4H2S)
zquXteCD*5=O22N^!)O%gt_!Me!qNDd9xLwl6Xkui%}saW`zzS@bhBNWfDF?rQNkUC
zumoA_hz><)qU*r$$Xs`Ubx0HmLQLLNW7&ULX2d*AGBXE6ds&Fh3Fph~Iv`q%l{;Ra
zrCcS{eM3rxVi~XWti8%>HM-9dWVgujbuM$e!$!ayj6&^M0TMnu=<QMEQS4*CR1)2>
zEwws<#VLddyzAWC1tpiACVln=(bQ()@4B>$KbBdo9w6ZnNEG>+v^WFiNy}ji)W(oC
zNe9nc6przrX+#VvQ5|i~3EwDNPy)L|no9Ew=p4U489Z)#TT2`IHyAfboYE9CEOJOE
z(@;n?rUh&8dU>I}$dGI2Le#Uj(AM)E&^zOgatG|aMdRdTE2DuLowZWCF5P_1j^HN}
z>=mpTMR?}UxPFDMw$|2m*HR|?<<pyeJl(yP6XO7esBj|P!sgn_O1-k)yKmihJPmen
zH1=W3Qc)-pA$;aJp2lF82`CZ&QEY#jg02tMOrnSiwuO8Mz{AwQpg`PJsV*Ur_U);L
zTP56sh-3A{VgzRoluVo`r#b_!Co@5)I%D_b9fW5<bQFBM>w600OEhBxcew=WE?tbU
zCuN=^JiZ_}4@wCScK+xMb0Yp4LjiWvwm=jyYn8V(K-1vRaNvwP?L+dyyR>KvM;=nL
zmm{qAah5Y4-hb~7(%hzXawar%{zDl=48SLC*cSD(6EO04=9n|{ejDUzJ9UaQ1CCtT
z0Y0JM!oBTHDQ28rS<9dHUd^gKq~Y^4X`L-CriTyL0~zX?>Bf^dEIi1<VW<j)1Q)o1
z=B0!5aO&*T%-V?9A{%wkbZZ`o3SD%tmmqGLKpxKN+2S6{&Fu&eQ)>EVA^?YKOcRQU
zpDuybY7{20@r+=`@Q(M%WrGYpiQqvQR8!Cs`Q9WGmP*#@yAo2uiZs%2)+DdpC2o2t
zCzXCQuf2j2p6AiFR-u|l^|kQO-W)}alyHP(A1aY<^JZZG+qpVuveb>SOhJ63E{)a(
zd|Z`GI%f|l;89B-<?^%DjeQ<(9fQ}3`gGQy>F?wfi&r1?X(LK>R+Jx20-7$D=hC@`
z&BQ4b@qE1MtN_bqRUIvBM+Xzf7AO{c+1rgrjg0-hWl{z*?KrU{PLQF3sA*dLP9@k|
zoVrxtT<eg0+cWYxLuZ8#HEY6(w+50pkvmNZ;;BD4s(7gD$*X!(I}d(KAhuyNZ&<~p
zet(wED>ECiHjiM_B6?R`xPvkmBFrBt{!LS-;SGMrb`}6J6UIWd{K(V>{KEyzX&$T^
zv*ck83RwMI?bB(I6fQcs9_7w}u;JWnq6RMev%(YGLaK3oPt?SqOC}_K6*=!1eWp^%
zcLW}xpUxD{=owbU(MS(Owb#Rof#n$v(9dpKRY<D-M%Zvf*qC!FsXoOok0C?VV9is?
zq($)T<^25Sxd1g-vo(hEOV;OsMO@2-ZpY=^aL@dq%1OgJ3m6OoK!O<c(xz#DN#1e@
z`6rLC(SrdjyS;I{#S=1>zlcMlqU=PTBbBz@Mx)G4555qTnuq%i;WTHzs^mmhyKAa2
zfW0rMw-px5p>vfqQhD~S%pLcqOD$P{Q;7OF&AA*R$lDiooxx{%3T+BXq%Zxk&N#h^
z%Uu=p7gc6W^{fZAtfYx4kc#`Gno~d|&l<Lab4Bj`t!W9X2Q6aON#`Y66!dN(@V#H+
zH~`F;OH3AFiOqH&D>P)cx`dN_{}I-?@Vr3cn#)&)JARu(hd;+7Zg&{cqlU!u_viW0
z3G^LWm0JBtWw5m|yxE{cQf|5RVxC>ve89g)Wd#C4Fr)vx&;Gl1gON)z?X`MWj;~#^
z0Oc67O?^?vfMENR<(t+9%;qWY0p|;#r-XlcHcCM}YADm|8`oVFvH*N8!VMuJO{j6^
zVucc{5A3(7-CvEfmS*ztXTsfzEK*mH1$HT++5$GZukbe!4#t;xmjA}sI|ug?v<=!Z
zPVD5wc7CyK+qP}nww;_LC$??dwryw6^S-rP`&I4Mw^dy|-G6k=-CZ+PQ`3E2_qTKL
z0o7`M<VSnd%|&+ME%&X~YBM<VTcjIRjzMz$3bOdgDoX^-wE}gHrW|t2M%2D=Ack)8
zIFBsE4n4eL<*z`*lXbStQ8sK>sf{r}QJ!wry#=0L&RvBA_m~p&cPEMroRQ_$>Sdc4
z9{du#t<us0i!16zx0W8h+etnrYI|LimwrFS9XWx=8~^7bxsejck@6>pc;Zz@pgk2(
zkJ9oMHhQ1;30!X5xcn{KG;`2m3ym*>hb}vMJC+`o_e1k$3Ez_XW~@Q#TjQH6YznV4
zc05il%U|vbmD_mp7P+abtqTN6rJ^paL*QLm!^<!&EjD{yU-eSv5&{?7fq1Q2*9F0y
zrAx<D2qcK^3@3k9e7>}sgeYvZk3~3x?Rz>@GkpF(5+M^@dXaFzKgXdU1rQaPg!>m_
z_f>1EiuqGbveQ(17Mcd+HXlaexxUC;37}c!07)90y08-{K%9+<%1#`snLrp4Qsgj@
zO;ZMDy4BmZW9jzZ<`Ip;^K51t-5vf+SgJEBb+(Aj`izi40shX}@7Dv>bKjlc?Bh-a
zp^-Ti-W1w*8t6>mlOPZb!XkC>hIhPDj<KO)P)+5NGQs&$n&kM#PCc7U_wA3O(dbmT
zt?dRrk&776D*D7zUECNj!@ZWW_le9@(KtONrru2C`zca)n;&O$cc{8f%KP92XS=n)
z$Bos^o-OKHW51?_a6+&YuAKvLwJu=WYlD$<L<knq&6wnQa==?lyGZ=UX*#n$G+eRK
zZ_+#3cw5mYz7HpYZFznv0l$d<d~6foaH#mRXFtDVSDv*l+CRl<9ok+s)#dTgDIFN8
zLY*D*A6duG+QEIG4N0!3Qy*#w(8XNG;cTRpMG~pPOnQ?O1T6WS*2$t&0}6twp~(hK
z1EfMEl~i@*4T7)BVLW%pQv0BQLD{x@e1<f`VzkZpydTN3J!e3u@Lh`3d0U0w34g~d
zb6PKqKD%^SDs=;~mT5NPjbsM;*Hy7daMXuy04&LATwmHt&%ImgylP7X1v-Aq(>VLM
z<^QI~>YJRPw^~%~by@6Ru`h^|&{wYT(@X^Y%4Yx`{~}nk)BQ;uf5Wt|Up069r`h*?
z^p7^a)|WvT!{HUGBVWXO8Xt<xAF{F=-HWmg^2VYM`)i_T6Z7MFg&4_&97mF+$W*>&
z5Ym1arG!JZJSO(;HX)%G!>Iis<=m{9cA)VAM^3)X{zddwlC4-Oy6rGmO#BP4R<Dpq
zY6ExO$DS>Lq87d=9APNDtaVc$JwF)X_d%{u5S5nG=d5yYZMs!-uko5)0DvI1NuK8S
zh@U2#;9_|edtpvi;DHMe>2Ujn!YV_i)8$U~fhwDPM{`H$FI+@v3<9zcaX9Bc<u`_C
zBVo4bj=UZCwyE^;fnjv($dr%Hk-)m9zkHT{77GpFnOoLA0!a?pD&lOCMrZYqHnW#I
zU$1KcqF6CjI}wIYC1o~$TFzI!j@+GkpRiXhXxg+Un6%zJrMw2YH2+3J5z7vyj6_hu
zXi|Lt`wFpFEw6%~-aAYtoqfuMksV1OABbK$iHZL*n25+fG?H-93R4&B;O2dl>5exL
zM&Rzrd@Gqh4wKv~e;Pq>Hw&C&TYm^JMnYLV8sX(Tm@L|<-w~WM&Me*+y(Dqb+-T+5
zJ(At5C4oR%#`B${pPH7E5bpwt(aK*{2!+HTIKi+x!&%qNoSVo!OF7Ls8YT>1D=$1{
zE3*MA9d43TXYHq^<hJd~$>E8lRFcaT-<Y^yY27i6=FHRSbf7LqhF}fphj&oN4W7J-
z3X@0P(d&hh9PPL*n$r$`>rQYweeSE=?sYzxURn8_N46xZEvosgT2FFevJk}9Ep_D3
zltH)a@%s=fK_xJq95Mg*w{v7A5*`53FO(O}qzx?!g_jp5JzQ0XkB~gBpk%zcG~_bT
zrTtIn=FZIzmR6jhS5CRj_k!VtO_ld1*8_rm#GM^8zEQW4cnI&<tfa6zrf5$$WY<Hn
z*+kD;rD?Hr8oiRU^)`{DNJKweSNvAQl}DiGwQOoxHJ^Z0&IA+N()N&6v35-Oo=?<b
zmWY*mU|;h(-LFL$m?rVuf=-ZgGh<-O5&<41*K<Q;ueEIl_Q1ILRNn+b<s6hvbWz?@
zqc;*|LCt_R9h`$`KE^EU#yko;wEk=6fxLJPD#%OI#zH0)uQU>frFWB$aS;CeG_@Uw
z^SL?or*mCtDTdCF`u5RAGkLGzaY)pl?nwoxp77RL$?izbnhKn%p1|M+Txtf~=T!WP
zga|+6m>)_d#jPw{E!IiYDIP-Kles@gP<l}}_Iu?jg``AnmQmtSC;>8<#aOP<bJeWy
zL-``D2A~Z;llMAci@5z5dKv4FH0GV9Vg_0^t)aI`e;j;!w$pzwr&=CZiz4pT9ipjU
zpuiOl(O?F-E{hFT`iG&__d*Se8#-|e+8MC74iF`lw7)PCLqaD`^4&3~ynZ;(bB=Gi
zZ$iNDIC=Ctn4dZy%%^_EX{e%C%u7MPd!x`s4#ial^j3^tZx_6vz*%=#4a7KM-bfQn
zPJDG7@xsFOpDpGrTa_KIamKP);rDk0dOttmr((!mNNj5>UyQ4bcymw^53CDEe&04j
z`P3abwG-45Vq<J%d$#NlGo~JpsKsGKfRj$QsYK>QmS1e0dip#Y{6MH!hQ@3ZI_n7a
zY1(hdwJMI7K&~hnh5h=JLybvauUV9}5k)XUoaI&IxBbO~jmYSWqTq;OKc?lpwHL!g
zXrlXy6SKPi6Mb$3^~mtze`@L~W}rq{m5Ux1i#G2-*f*nYm<Dh|RTiWY^?KRQ?^P5d
zIbH0_|DkynNZp6n<%}cq$1U6l<6Zy@k*5iMA>pJGf}Nr!kra3Vjg?g;sb+a%6%R_x
zz3S6cFiMI(YQH1hqkn@f2HNm(E`)L%VJ8<2?>6*;-@^IwO-As%Qx@*o*9t%<Oh(C|
z68dGL`()in{Uu!N?4`f+*DVZtXn8|x5R5^hIRs8ZBlS0i1e*Y!4g9{_y#74}o_4A3
zq89Vr=op(vRtrMNxLU*_UsB&v4pHOzh>vjJQ_>!n#_3%we%Ocs#_yqQT9L(V&^88N
zlfpMbZlv|Mg@;?~J}(8-ggz0=Glt8MMf%BC1`Ke~<kDhrXwiW-SbmF}n9($)MoO`M
z89GN@DU(-HtCQQ8LIKt1QtU34sC`@l<RoTB_1GBY0D{t*j||G<i5Ct@PvQ&9QsJqr
zjZ6Fx(A*ov7<;0TK(;rW1XBE0Qdb*Q3SW}W)8@{P1TYX2m}@klhnS5;%xmpryj5MT
z_L~`@1EKT@!_1@uJ4bI9vF>cnOriXDL{gK**-+|jD<#jNDH8>(OYbwG<Hrn&)nuIG
zcy(n+HcQ5&F;mm9bTv<AOdrHnHK|*8dLRycqS!NLC}hn+vKMcAiVK5eN6e`bp&&x`
z*spjOos9LePwiMEVZzfu6na#t1ab@e`AZYIM#TkOF(NeV)CaUw!)I&8YE(WBe2{CC
zs`~|Q@3coIFyE5V+u}eTB`RK_f?z;oM=OUV{>4k)ieu}|psT`c_k<a`{YZSJHae}K
zh4=JdLev*u^^4DQza@eFLvM4(c<I`^W~Ps^mMW#_FXJbLtoF(*Tw;x)A{uRQGa3Sw
zdYW6V=Sjgc(y_`qIMM-xsrW^l>L}SHm`^rrD#*y=PCLfOn4yF#oz)g@M|rWQz(gyn
zds~9lNH1TCUq!<4`So4AxGI-D#DQ1a?ky3=2}&ey!x76y;mbs!MFVJBx~UZB$21Z=
zo(yH@y9-?)#>zclrJGvoZaSHIx;5ZqhqGGMa-Cch=A^ue3_g68R#m(~sw!;@MSOB2
zVWzapEz*1CLC@)6`qlp1r9;`fW8&U=_)Rk#xl`<J?2f0M4@YQ9=xseJ(QU04y!{x^
zgeY>Ugx%XqohccEq2q_7wqGvov%p)N%ksDmJrJMAm-H2Dhlq1_CI{wlz)5*?2Zi-|
zibYS#hHNzYgt@$WV3K`472Zq}i%+mOhsh<ooI$vQ114st<Ejl|Gw2S3N7o1M=$T^g
zxo;F^JJwy8c&;A9$0<0U%w%rJ_{#|I1`NGx7i`Z+Dn?9<?z<I32!4_MMVk$$VeMR9
z1y!|+<e!)5*=-dpYN3`qoGT`6v3M~x#^sNlB&E7a5)n9qt1|I5uKNx<uZAheX_nc{
zCD=85-o=hAQMsFOaVmQAL4FuHypane;U8soICd5+YS5fd!2{&#w#*DUQE%BX$@%=q
z(^qFk4o6+Z;D)(<%+r$I6O!WDB_U{D#5ubPL3|K>zwP{i)3qJt3i%(?<Ba?mYa*GA
zV42bS#8mz;Y{47XS{?nV%`7L7GoBncS)G^rkS|j7Wm7}Ii7Fk{totqOl<8P#ZXQjp
z6DKJ_H}m2l(luDzU&FSIlb9#WjouD_6FoTbY$LA{GJ;z=*{$CabwldPBQl)_#AcEU
z8FF7m+rh07vF_&C5cJWP2!|^?kiOje;Kr8ail4BX2!r38E*bbdGr=FzPngh0;p%WV
z)Gx@15;Yr660YuTPq`7Bq|vofLI>&mz50z`e%cky%g{uQXRzJiJvPm#M44E^D{jVp
z5<X>rEzcaJ%8B7HvdS92Qz&x(nYdj!e~Pe)u35@<vc1^-VjL#_5GBO0=RMQb+BO`D
zYNdebPVi7iNbNG8)-Vl04$aj|Ig3hLDa1<YQ(A*;URjqVW^5_i^Xm#_=pyB-WLnLd
z%Y3s(>w!+Pw?fO`dXUqMoRfKeFhDx7xJk6VG%ee_PXu<Qj&9GJMT|@%7xCgMHsK3T
z@WahtW??0TBIJhh_10RA=cG#_Dy~uc#sI5{(ywu(2Og!qh@`XJUC5DqYIDv1>bXw$
z5!FB*&1c#vh48pq<;gwlF3j(|%c}Ei%HJamd!Svp44?ay1qAfe990ja8XN#zM7ebq
z>_;0=IYRmq_(s5$E$&>~C4<j3)DH^a8rI~nM3Dol5h_^omS{v53eocxEH5({m0^|B
znRGAvls*fu7?Gvl4OGs`I%mlmgY-ksqqxyQ*)D^zJ&F&Qio?k%=NYwd+`KK4nNyaw
zL|XE>)IweDV46hrR!5-%$Q@~sbX67DZlP}RvhhP3)?iJ<8Eyv=2ZnaehnuWi*Xpj6
zlmL^TK$L9jhR4+-6@M_hT%fQ=5$$9Pu(2!rOPq%sOgZSR#Js6ox6hpwm4)>U>~v9u
z#mYJ)Icgp(cALfP$`p;il|?1WBkiqCQE9q)%sy#uDfX~{_$3O0W#`$1@8;@lw{4P$
zoh;ASN`+a8u%JFgK~KVDBm#hkWA|0aaOWXkx%84L!^icbmWDqWN$j=9;69H%>sP4A
zXi}VLs%Km4?`x#t8_pISn6WpqIk(IZ+>LT^(&V(FN*?)JaoK!*#YIqA$5&e%`n$()
zXTL>VrTS;#=6SvZ=r#4GK80<AdFW+{&ejS)?LeEXe3FndEMaF4%cQ~}B3gj<mfvZ5
z5Uj|#lPPs1>!~khXr%<R!Y+5tc~Tg&u)nXw;!2DFHGyL9T#Xv!rIW&snC1k%V9RuI
z%zthQwy@m!Lt!HCbCPe)Ek|BinlOS7og@b>wpQAPBSnd>3T6#q{BgHPjqr~$F~_M+
zCXi7cgz3`er^i}9Jjcl{21}KiaOg_bljz&AYv%4zxE@o*UB%UH)vD*<Ow^TbUPqxX
zr@2zLRv3W@yZkgViyRKrj^lnJXAns<vEHiMzyB@WI0w%s+L3-S8c6-wy((GWlJtab
z_!RsR>ddrHDstBEoxc8^T@HYNH1@*y+*VvDH;;Wq<|@znkEdXB(P`zwD(Mfgmc<X_
zI9=OSi;}g^btHJ>VMc-Ca5flK5PYffTn0z!%BtA?#rT}u8(rGtlO9stRRgjUMtA(d
z<%4kQ{o@l7t)c?qJWB<(*(Q$3L9L&1Kb8<CC@ratGN6G36kgSM)UyHLM0fFV2ZT=%
zn_b}^aZRN7Zup|%TiTr3<=S+f>f3*-ldcu&U?rUPK|7Z-VF9FQ3JxTnEp8J_8w!+g
zWp`U2+C1knF>{w%qL20Ue2Vlc#1tkgTaCz|Q-8@|M&snTvqQA46%v^<()ja)YC#?Z
z&GXowoffw)+&`?->*E>q*SwzjiAfnaK`(VpM9987>?rw1d$YUuTkgW?TKP`^0n6Qs
zt&hb_!3JrVZagY9xU4y!mt|aVr?!%0Cm2}9T9CuJP1+!`2Xh$>noJ7YnG^G&a0AaN
znq!7jf^YfHV|#$vO=J;@HJF`-dL1h~TDGrA&g9l3`BG}tq6tRbt>iIEa%!o^6nnj0
zd@vHq)B%G!Un;dIRR5EB&LX&$!sioW@CUn>o+K{YS&cz=ZQim^{>Xwa66XDa-Tf8K
zFv7C>A6g`RC-d!Z4aSN|;~#~0F9&1ec;v85J>;+M8SB<?3P0G*AqWCRp{3f}35Zg<
zK6F_g5bMJ`%rcs{4H%yMu}7JYMX`N?CLdf=^A;`1NXV(fE->FW-4+Y&q`v}((e3xN
z8_&3l<n2sH##sDu^*ukal7&6~I2K*|RG~Vbgq54#6JJJ}x&@h-urymPRx3$ktv@T*
zt;Kn6HR@F{SZXN~@=NWl8JH@Ul;HPO+Hc|HAD+k|4}t;+NKlhUi1dvN@^<+;&74MQ
zm$#O1q2|vUtpzMX``QXx{^Id1eSydw^-IhmgMDO_wtQxy=3go>a%SlBlsypAArGNq
z!ZU)p(lRU1i_E9ziAYq6E0EN}!Y?G}-U~y;{nya#?kjwJ2d(1uZ3o)t$j;)m#_Qa-
zSoI{8o<_G~m0sN3E-g2Z-zmY-I<{JhuIHwcGmkQ1#H|f%csni9DHZWYc!m}<$J%OP
z#QlX9D;fms9+=13P3EIIC4Q?bg2(?Xq}yGvg`@Q|j<3J2t4}=2U}V*^<R;ZQtp`8k
z)izyk(Ej+`;X`gR^gKnmP;RR~H@;))cOQZkW5VUDPMLW4>^}<MIiL70FIVz@72|5u
zp5nXgu@uo{%dXd~18wE9glwd}pXtz1x_;Al{yKQ1X3_l)MQ{fTD8Fjs#;93po{sR^
zwl<9?tM?P2B3GSLB(_kgef8y?!UxMM8oWk~n^Hq@TvPs}K)Ff{6?R5gkaP_HDtdqv
z*6Dg?Y5r>;_d6-7V-JcTMild=sb%W~;|A3ol{=CX*2xKGgxG6yOctfRdqS&Cz6Gtr
zc<K(uRqyr762bkTBr}2#pAvq@zz(xVaY5H_?iW#}<I}Hd6gx2K>h6{KtvgsXAuLob
zymwAHe7fe%JIJK#3g8`;#lz4$diFd*{I*Id7rGr9Gid#Q+-h(3@<VDjd=jTwNH+<~
z0^W`2PM4@ev+YA>LBU$*pco4lo;SkVBY$HVbxSjie4)vx`OUDvM8%uj%#b|1`7A)>
zPsE5L52EcLXb)t~2kcV?dIai=n=eH0eX0&*)p6#vk(nBuJkC+r`jPY)p=bL^tao}@
zkuurd8p|qB0y(uGPJ{N#?Rq}-KuM{}U;Kz+>%@S-Q}=T*EQVx42xwp6yd|u7XH%sl
zs#{xAH>oSTkli;;Gwy^Q7qfKXEHAnb>-Z)I-iiFv-be@B?z(^u-^iEwt-bg$<%bH+
z_TOdpTs^=2^RkMY(&~aUF$?8Helj}4yJs$wADYISM*ZHFxHX$4Kd4<~13TFuO1&od
zt{7GLmc+;-J{V3Y3)q`Qw@#%>p5bc+?V}3ykm{Cyyvc-m`VYVBxF*d8tR7U7`i*vK
z2IwbJ^G`UQHkQe3SYRM|rFUeM)KtEq>Jlc$<Zkwu<s&e@wi05Va3#zcN!a!{)tEw7
z*g^0Vj;HrZX2MKPUEz4q+jHzVvs$@hX|W0Ge;Y$xI1ACvhBaF?uHbJkm%1zZHRz%h
zHhro}Jg3xhCXyLP(VvExsg+CqrdX%mZJJZ2P@4I+nb#0#ETQwA_m!Vj;|!$hshxd;
zLWl_a{elG9xM;QNPyj(o?SlVUwZyxr6R7B^Rsb+PwU9Z*o9D@gQsSVM7gT#S;a9oa
zJHL!ni(c13civ2~zEw#zTbs!o1z-L=Kj3UKpRWer5E&=}JzjS5l57jQ<R4`^*4;`~
z9?;-TP?DzY-srASjA);Uw?`K<0^;S}VZTA}(&(6*sdD0z6x7n{o(cWJK^TPM_7@NN
z%p}4HvdZ(KK<WIcR*Vs9IMVjq3h<p47bYT~!#Jv4GWAEMI~=^g<p42E9@d?dC(OA^
z;wlOidZYC)fx5HmyjPQ<rBA1V#ZU&SHj_>hGWM)wg!t0@V|P62CSFSA!$F}55bUZF
zxj$&cL*`-5k_rRm;zgcOf52!Yxi3yISG6o~Q9In4JF8*QQgLJ?m(I)HyfA6*XYG=n
zNSer+PS`zy6SEsxb$n#i*TX&NH4S9}E(B`%VmY*1Kcs--UP%-<#75$Ul=`MToXQvq
z8!Ac1e6p-;{KP0h0I9<!Kyv+1tG$WF!zj~rJ%;^5G^K1``m5X4YVx8S70#n)$W)^|
zks6EVJ=3**VlR_4gN13{)Rm_-_Z>oSQP?Ja!V`*fJ=a<3=eSqFo)csmQLuH+Kb1R@
z2f1eLYvMB;<G`{a6h!$e&_D*<Pq?j}B2cmwl`3nuDxjZu0J>=FeG>Lid3~y5X=~`_
zH8#0Ha@?j`tdK2aGNgMnL?_O4(D-I4DSH9aVV&w4&A2KpLAdJb_rg>p3cCW)DH(W)
zj*Qz*6VzQu61#ob#{IOf>C@GOh1l=%5$8DEL1T6s7)n3`-td_$?&<}DEE&oc`j6`9
z*d-gG@TYdfMUx3))GHNw;IO)rp04Oo0a(YsT+pE;yU(8s;@9v#cfB7g#%O6ZKDI6B
z;+-4%WQFkguZW*w*AKXyzT2AYqMIEiumI(zgBCH-Fj=&{UA|@?s}ZsDq2+9y;^aQU
z8oEg~vz7{l6HAvb&%gY|mW_Q0TP6j%A-jaIZZH8X8~kE3T^&#---HymHs?>(t8wm!
zGXq2Ezn4<Q+d%#)vp?=(ksQZAE!u69g}A<!Dw&!Rp>6alyWO)F3+zrJq$IMltT{Nh
z<JxcBJsop25cdiB9A!C=iG`);szX`2L_Zcqi)d3o{LXFWJ-cCVHVMm1>(UO&G5na(
z**@DQQ^-XPud;H=wbHhL5J&$;IByM!ztVYwSY*_7doxFSVd}vCaq8QoWKL+#P$=UK
zX5q>RSR_K_%~nu$cV&Q!edp=(;O2U?#qH`<=gWc&h5f1qg$v84Qc#MDTK|YcZOc@U
zo=T>LCFw?A$iac+el)(g<!Y+>Zn4W9_EL6A<0bhZ&7o+BVBuwTgT&D?ZuUcx;fM;)
zJ=m?lc35oeBRdp5$42V*MpEt~kS_D8@TZ;7x*@E#p%r%<ZaRt-u+E{cR=T8d`@?Ob
zr?JKg%g%3Ths4y4d>>v`-Sk_Qz4V6R)l&8MocfEY<h8b@W~lycKaYP<JT$2p*Ispv
z1V{8-9RYaBait0U{)XGq_bHJiB!XE~f3RR;-a=`oQf)HE_fko%k&W(e+)s^5+Za|a
zhtjc|Ff9cL?}Uhbd}JF_1CvyHY_KnRaso3nLhL0hK}w`lAk7jKC}1v%<{7D9FZQ^B
zOa1jB%A&-MI_E)<oKo_Ppt|VoJ6YFI$TQuo50$G@J_(CR*UzMlSE$u$;@Avw^s&+>
z@6N2vGKwYSpiNVSYh|P2CUVIcby)qV6vT(x0?oI;LF`-eZ-Wm=6SG7Ebo0+Zeuwir
zkqD6BiRCxQkZIR-A5BCB85=evOnb5^LhbYXah?*|&UzZ%cW$0Pj@3bj_{x}zwMnSh
zx4sy5s-`q&LRcjjkJ*1cf~GwWp6D}YK-kHukc4<hj{TElmVJH27wih$A5V;JJRh>!
zTBh6|$l~2*b*K<E2=dt0ubV^o{-(vb&h;HnxIelXu(I3iF6xA}ov-;2K{*AfTWdP$
z&pkb>-_B_HZv6hx$4*-3O<bO03bkC88{Ffpe#OZjk>8Vjn00Pr`$9#*@e*+wP4Wn_
z69}IOYQz(Bn|_2SGFi246ge!98Z#L_O>|n3%|}3xISWw?YTyoNYTLE+IN%867)F@6
zW&Ti?RA?tA0eyd5a3oq%OS+SSs=zn-rly)$DwPs6f^y>&I==7?c?)qEVa+pxrZ9$5
z^y&7Ah_J26rx4Eb=@%HZi<Pk3^jJ%~&Pg2CR#0_xCSED`v$Gu&csGKYaI2~@;$x27
zaZ8PpsRlA*$5i2>Iu`#`1PTd72n_wLSw-hIRJ4p#TizlhR|stxj9C@ewOpK=&TVg5
ziRQbWOcn6qVd%W_s(;noxsNhLjlw*ruGNK#9jV-L`*(_3L?6yKV+PgHCif|{9YoGp
zI(h{A+J*wEl>B!vQ3leICp9y)Lzrq!P%jCNj1wEf8L1rOfWq!5zIow0Gl3cH(M|Yy
zM%L?d1<e|fV`So(KBQ?UR$YbH@cJb3k%L#+5JheD@WeS})A$_*vj~F+;XQ+PGdypW
z?<Rze@&$O`dPYs;hZpCLmiKmK?}FH+hhMxZ>)jny5&8J<Nw_Ck^LFYEWNvRAN4on*
zai*AP%H96mQu=nja#sy!_7kf<)jxaQ6MtJM$rolnZT8<c7Bs9I$*&NiM)<#GB>E>j
zZfST7y(IPt^DBtU)h4{lo8pH$GA{{WxNp8oB?kY7mO`v6qddUDd?b$=kp$g?ZesFn
zn<A;q=<;G+A1a}GCNm~^hSlL$-PDEQV&Mi)9CmVS#H6v_Kf^>K%ta12JEKVsK#T0t
zM8=ClfI+IgRuAlg8QtFvUyBl9&B|DN!LBLG7#B<PonMH~CR|vYJfO?WK)nUu9>U4z
zlZU=WNsX9J?RD_$a}59)XX0y4NS%qfCe@`)!N(72lSq-|Tv^@+ZJbf%p>O1lKTZtz
z3*VsGEJ96G&lv#P^7qR$2Z;!Ok#SKr>1mx`;>j{Iy8h**xg+humGFt7y@mc6XJ+nn
zafJ~PStZK=yC`pJZW-CB`h52?l#;!O!GsJ%68Z(LZD%elEY*doCN?3(vxJ+GTv1Jo
zLBuv}ky1X(YFR^UyviK<#~F}9kc9a1hFz{dA39@`L4Hn=(7E+Kz6T+ly=S3H7kQZ1
z40j&QEbs3##9J{E@-#gn>YNa6$D3Xx&NTxw?Y_X=I%lM*51ceEQ<Si!L;j(B3%oHN
zX}B(N5<Ga+!674}bQ?yE@bv3%g!Hn=OoU~Q|GlGh=`bTBEO?)g+MV{Y-HwQPtlZv2
zBYFPTUKi&Do^vv@ICMbnDB4p!ITqDcXGswr9*#B2Iuk%sS(+r<A)^wT>Ff;uWagP|
z)nbX-(eD;jdqd)>T}_e9eMrw+EOlNiwpxzHAg7A3th#QUD*OeHv!Sf&Q(!vEP_vd@
z0AwuTZh?agnndKjInTX}fT(SU#4U_wAW|uT2F)+sDcA7#1<4e6!+kBMz!_x7TKp^U
z+%aQD7<hnS==RIaF=EK=8noJj#k<9#r`z+?mCj(A?{9K){<v{*C0N1C?0&8NEi77%
zU(qA4loj+nyABI7>#$-VYi5Z==6}kCMtILXp4_XCV%ycu^o~0=g7^yG2Jv9YCnGRn
zy1?GJr$er!K?hNu?h<jpwi!r6eN?$Rb8ho~Tc9H5v^J`od6sL#`sT9f(tgdl6$wM%
zDWSbzrB!_!K)}0)X?DS-1eTJsjoQ6vlaa!i`m&>rHLQNi3Ahp95X#a-RY><O6oe|A
zt%Nt-=qfAykqeEs)Qv>Y-FjR;2fM-ZBe$bi=%YB!s~57x5ytw#R48eRi?~F)$<cWK
zR03lRev8deURy{xd5-`0#G*=x6PJscm5%FnymfBpDPTBydhTh{`xh+y9##)_ThW}+
zzly)#fU;QS&yhCQW@>Pfwo|!wl*FL3^Ru~sR9l7%I8ot!;9Ntnjx1ww3q*!N$wC<L
zNc}Cc1SrE!@roHv(^5iOx<02U!{^KV+xk`~iVKY}4QFqdjFgDmXP)Sj-j@5m*&xua
zDuwS;Rlx$|<r;Hs5^Y0dDOG>f%-GJ)?$c}j`2z&PYW?zm!ty!(H!}VI56c%76&4Xv
z{)go&Dg7tc`Tq&bXJz<rFrR~i;~$sL@qg#?|3l*czi|2g{PO>c%V+*KM#$X2QOU%a
zfC3-@5CjMTgaIM|QGggg93TOZ1V{m-0WtttfE+*`pa4(=C;^lKDgaf08bBSO0Wbg<
zI60d*S~yt&3;~9Y21ZsU&ekTT&i}LR_#b5ezzASuXKiQuuePx<02l*|EleCuoGhFG
zCV>BP3II%Oj18R30VcNpeQ`IkHn0Jh0!;r?ECByiTulI`)&NsG7e{~@z#L%iVQ+3?
z3$OrK0;~Ym02_cUz}CXn1YifS`&ZllU*rP+BDFIH*ju|e0UQ7hE_Ti)#)j7a74QG-
z{CB|r)%)+JBftsZWMX6S-!pJBas78!CkuCgll6Zhb_O_`JDQjP{-eov0k{I(0PX+}
zfam`~ng1VO!T-l#@Xz8;Phe+gNx;qhKg|RT1PpBK|I1UrNWjj<^uK!lyTE{hotg3f
zS()DguAFRzO$UoC$(G3IJODIL_|H}V3d_(7MmZ0a2tq|LPmmKhzcsHjPdYCtp5u__
z<@eLG%we^h=~2U#cM*G{Gay}4H9~82V+*6`9M;?R=I(C@!~w^{!|Mx#NYM)lc5lSQ
zbP=>yo!@h`IvGkp0ecCL^pO^VgK7*aHhyTcD=q}*2E~uI_iGmg=oUKS4mt`r0LYg>
z{?SgztqhZYdl<$VVRjd|tX+to-rAJI4MC4$bq+dq^5KQpe<~KYmz;`f@+pjqe;5u7
zd}ECt=r^l9c|0~_QBDjs|I8LVY~aT?IapRO3eGj*@WjQ{KUU`e;=mz{Q?kwpxU<im
z+@B5#BG5D761ax~l-XvJ-%sJ7&lKX6E5N{y>Twfycbion5s1<rd?SE>z8D6?5FrpG
zeht`@>Kup`0r61I=!p-m703?_vUg<QMXvSV(SC3cp>O-<x&|k=huVPGK*Crc$XaTs
z{6A&Wpf3SVz}5uMJ+L;~2G`<#*Z}O>iWu7Y-MFF+AXF1)AXf2Ezq_C{fj&HYOa$1=
z_bB3<8v0R2YW~Zk&8<y-M0VJZmAAt_`FPy=>+ViJmKE^*Yq+}~Yz}|IXZRlTRbp;;
zI0Ah+FmaJ@0Cq9dN6RD<7$g)(06)^M5zr_K(0!fL{#OeB&I;&bSmI3@<``k`&cGJv
zu75G`FCZ@ec)rzddTan95WI*#$mdVhE1>`Q0CY9zY8OZ=o)|Omi;Ghc*6|gE_yYm_
z18CBJ^zHx@$j|57qn=+jy^Cvb>80fjpY}LqK`DVD@#Hh=o=;Lk!wuh`uLY6MKT9tj
zfq;qz1Q8t-80?d6k`Mef>l0tm2K!bo4D=hm48*}6GW>fCSfjJY!~Kf|)7sAlj%&9E
zi+L;I02ZY7E7#X4pI{dLZRq8v6V$Ku^C#xJhT>=C>}Lmsl1N+UXSU(X;`V25qcuRs
z=ck@OmI)+Q8xGFxFp&RyWfAJ}gxAXdvJa}|XQ{l`CZ3uI)WP|yG+sZQXcf}91}=1i
z<BLPY?*$lZ8x0oNqGO=<Kh^>^e8g8i&cpe&HUc{|%z7=qGXBw=cRNY&@SKjXnxg{@
zRFGBv0@m(T{=?uqm|q~DZ}D1|2={MBt-zh!h;Hoj;QqxJK=$@whCk<j;|cuLj?+%%
zNWgYezG8wC8lDl5Awb;G{D^&lYVPeGF?%cC5s(GWo&AV`foit-#8I&pj@gjGZ`Z$q
z@Q8~D>`+>E-u2M-ntm`2L2m=U!4u$Re*(qncKrm9fNCy!2pH2g>_)?)Vc#%>MO}X|
zgcWaJedvufxZ@YjziWza37v#~z!TaG{*6I+?IB2H+1Eq#K)u<C$IqC&L3qtuUMI|#
zKkRxLc_;k%X8S%!1P}1$cU^T_xOUw)m|ilF!AN0T+}_OkcW$AARsou9D<RN6HG{^F
z{OQ9&@*uZ<`Pl?D$ga^WifFhbPtgfS8}=i;r4-W7qbeI(OyN}b-=W^=>mF_!H!z-h
zw^BzOBwv&!S?5>9u+-jl*sW-mG4pp$?N%ZE4sYt467Xgls&+_wP}FRs`JR%{k#Xb3
z%dIHZ0yE7Jg!FWjVI-j`MZ|Y12x7LE!aj$SdKre~wC)R2hrggwQa)!qTXtZwj*Fbs
z!Q({Y6cS&`XG{<%UX)|HB^AEG1+}BUa>vj}h0$B4gp1Oq`DvYHIA9T|`lm`jdN>+9
zw??+GlRXT*PYi;L9>dpBd&;?0f@TJl>04r8n01)XrsgWa!n^dX5w7w5b&$(#D$qFh
zWYY$b;ypp*sxb|XQHEB_!M=p})=;~eNlA$lWZ$x1RjVP%8e6HPvKXZfWN{(3Des%I
zB=}5Z@l_YbAIP>$^}Th?Zw(+-UeD;VX7k7Yh6vpwpekL!nB@N2dJfHD9FQEFaVN|n
zdk}S1KWW!`_U5>NL%NPL#PeOPxHHt)dHy-xnEUsRS1X{NACyp7<_ojzk$=zArWrwN
z{o$RcFjna(I;Y(nQ3;Fx^bQ_nFirI`)RbCP<mM$Cb6NY)ZIEgy_ygqjVGzOB$ppzh
zO5A+CG9g1r5i<3bx{{*@@3$F%|5P{oFQ1({>A2P1ai?<_M&(O(`O&>qn0Bn6<aJgW
zJ$Kv{1#>fR*tGtUic$c9(|McD@M<5Q*fAx~UdtH6hUiu}O8$t<Du$tCPzFc-kBRV0
z^XXQ~g&29LY(_<!6&`TjJjpjg>q{wbAMNf1SYs|0+7S$2b^9$n>dZXOwNsD00rt=2
zfr_3B`?KR8Xo1za6A_*fCm*G8B7!WD#Kap>0cjJ#7b9_(=5_ifpGBsNP-EtTlCY;8
z#vSK>Yckj6nNWcr^-+Sof(hx!?uq@2d$$s2r@Md_BR+0^K~1HoF1+GN7Hdn*NPP73
z{K2NciZ#muuZWjL!IhH;6YY2I5{zhR!9$AKZI`B5kc0`EKTG!SLj4Sw2s=&txFEyp
zJhoiii=l1a*-NXBVP<U4=Qjb~E1TFwy4?=V?xmc&Y8ShSlIf>c<=Wxr_mF%S#7ZOj
z(<~&y>zI1aGP&9!Q^Ti$8zW(YHUIgU%d1O@;ak=RfNIPp04t~4vBWv!dZxSANRo4a
z&!zwTV$JHFfg4o7P^1WQ?L>-vhCE=Ez3h-Vj58Tsdss>hbcNY`Cuw#fR=tQwNj-MO
zo;N9!fS=ss_zgJE#?k}qQg^pKGgF!J(moV(4i8eG9ctb6lG#bc=dVP144y)6>V5VW
z=f6$kmh79V%M-GPV7t_wXW}82t&RgkO`2$XVXQ|eEWp}rxO^Hwj|UqjlL(|-tXIrK
zDtu>0&dR5Ylc4SszCdiI^}s}pWoXdQ)4at@7NPn9Q~`;%(4*P^U-J4hRTmkw&w$LF
zWtFAZaSGB0&YL@L^(iU1^?a(HO>d>RVOF&i(h7gl#tli>EAgP$%Rz5+#hWtJBBZ3%
zxOuKk^Hc+31S|h>;*ZTBFI@a7$mi~57mNjepK412r#fiK7s0BRquM%{`FH69At4$V
zWz~wSiYm%41}9~UlimTy3PH6~+uX$TUdtiV3}DuZk7c40498@W7z;%_Z`AuFTpW+c
z3(!a38(YK>%D=h%sV$c#<@hH!tFSGdHLVk7*J9zZw@=n<JRyF2jlFo$*3BHaskQXk
z>6LGyEEKmc_CtyQE&Ur-i&d-^%G|$g9ii<@(&PH?pzu@O;(-|Dg$J6>2h8PysS{~?
zxXM&?Zj*Cq^nf0Mki8`9eon>tNl<(S(Hv^mghHnXZt1Sp`uO?l&7%ka1#DayOH?Pd
zVIhuq^fie4S?Gjz%?`)Rd>7H+hvW!;p(45}UMkcoV}de2%YyrR<P)kA)k2jmO5~7$
zSZVlMCSh+R0iT{m3_L(n1#v-lM8I3fBi5q8g{a=v)3o@;0%HFl+)kHo7wbb(=x%Zf
z;aP6^q*hFX_aYGFoFu4owSJvX_iX~fn4|m}svVn)X)E5nunS49=z>K0^lITW7b;N|
zo3ji2G23cul6B#l;<EdqZAh|E2+8BK>PFxt`^pDL?M-U&Wrh#GuxFQ1)dXtAb#4~+
zuO(@p8PMX>61_nM8+=k4w1-NRg<s<@tVo;>XF-@rQ_{31!fFHUZ^L)?X1}`!(;|_o
z>~ssR2MojKj{Ovk?v~3-%J`a4k^MC_7%c=j{L^E)VRy&8zOjWZ{_AiSonJkWrj-fO
zv3V?1Mkxr{N}gpMw1jHOM0`6+U=BpND-|P2o;R#?xmEU>oZFF8Q@lr|8CLzc$mKVd
z^}dIUe?FcTEPEbnF$XNdSYw10-@^_{JD|dfXokEFIB+lrgmxb0sY}3HV|Y~M{zg_2
z5rOd7*kcH?=|3H?Yq%>QF%>(!ewZVn*I*GXI#cf;)i~<`wvvqVs0t{*GSuB<JaL<?
zNUw~ThCB%TH3>608sfS?hx*vm2+55<-un(4$bD*dB8rlyy*qC`tC)G-a%N@U5ud4)
z)emyPF1o5;6)n9S4%iNuV1npP?e;6Pg1VAv-W?(f05NTmGb<#?Yo>HtWvRZ`d=oX#
zM<)(Ut%|O>JhYqD*LS2$i|fuKGLO@a1~<Px**){h4F4Wyz8AEXczepJ{NkJfB3qzo
zs(-A?(1A&wtTDs3aasusv$6p-V;AqfEmq&_=ht9~<V+&i4%!!qbGqz%u^8xMxvy$d
z3<zCSAcr}E1gU_HHHfr;ZOmu`U0&^ixYdM!Oew5R7vKm`Ub_J6he<QE{Z-Cm(lbbu
zmh@$GvG7hlu_N)B4l~g)L1*osETm!{<ro{52zYLMzSEhu;ZO}FiJUtf(Ro<4uc;I>
z($h%m9EA+K<tdIW-kyTC#@lNkO)w}pnMXs%Dg_LymvZ<|$sqT(H(rdl;dw~Ac&@Yl
z&?gK?n@mbjoPy^waam{1BmA@LK#GN?=C>z~_~99`y11jvZp)4eUXV(?$<F<=zl%g`
zL06&;6D)xHm1#w!k%Hq90VY*9ya>gH%mgw%nn%q*6-5#dUA#)1+vGnAH;*J568{YM
zwo0c?BNMtx-I6>ris-Th=u&e#y?5G$3?M!hL>vCy#F(&AJS)uTA?JOVN+69q*|Z%3
zZuq$v!<hODl+haIBUahb5gt>yN<BVmBltA@9H2B%v%Nl)8%!%aJp#8&L|OwUfLKWe
ztC>DEZE_+&rsg4ODijiTR}PUwnFV&eu$vpXwV_d{kQQ*NU*_fh>!KvjIIweGnxUK?
zYva6;ygDgJ;#2)fEQ)imWpJVe5&OWS!6t0N416lFOkbT+2T*OE=gm`Sv0gl1XFOKe
z+%=M@=wOMf(lPj_g<ptm{+_}z@qB5%b;kawgSVnC<a9QmoM+Frduy0J%qPv<>R26G
zKq$3<o8enNrrS+TIBUgL7L50wzEU_buWS@0y>4rO{di3O++{*8C#=-0lqi$=Zz4iK
zyBgTs*n1&+f_4ld^1{J{?u{ZL9lUU~`mY3Xpyh_=hw)e5H)9v$Oq?CF*{FMEEpYM%
zw!|~kUyMYd0!O$t*4Hy?+n~A;&BfBN!b9EOrr_b+4mPuqti+5BX<pUj{50}U{3}5%
z16Nzdeu5o?!>9Xd!xpiKW-L_<RtVID3C-Tt!0OjY0f9<hXu?Bfgx|+wtFsCu_dQ_?
z!&O0c@E?!V4_OcM`-A;vwtKlIsKjI#nhD^y=f*E6*3F(ft%eXDWc55lo++y$AWebw
z(0`EIbX+6$Q|}23(-H2tqH?8MI*ubhErJM!qgzeL1Lgp>C#qmY&>_gwc8iipUs9QW
zEx41?#+|+#q>?{1lfbLh(n$;h4Akp^8-pCMR|LDV1}aXWE!)}U2x5IrBh1F8WVE~g
z0&sKpS}InwQXwltD|}W?hF;|fWt`Ag+knSrG*S0c*q^KKU8woP(F$#tVgxMG${-yg
z!tm281xK8CIQwS9VLD+GG1xR9%VaSzB&T{A)=$gC-n)F8l;xzRb;=j%$4Eee&R7$x
zmvZqo^yMiBX}!R;Eh!-vqmFwWj7_u<Fq;q&n7e^;L(YKbF2Ko1xjh_sPE-`GOUzDR
z-iK`4fnN9Put5}2Dqy<pqBNfLXhNg5G&-F!4w_ez*=3$3<Z`qI<xTFM%W6+;A=XGs
zsuXJ`aG<fQr2ELarMkU}qC8Kx95I<)YlSN~SvWZSSXKDe;GL?7GP^_Fpgc;AW+n<4
z&>4+<Vuq^jO@Wkamq3*=t$Im}BM^qm&7RQ$3ui8fjOVBBNTnv<g`iW1RG6(<+~|vD
zd}p0x&Ai>pW(Wmd@CROty^UMTYJN8ymEQ>dVJJ&Y@@*_-NS-eC5=5wCM>E^;5#{z`
z7SN{TZK1sMTNK0o`!wp-XAq_wHG37h))M`P{BH-HZso+0d1g$!pJTm2oIq@Jc{!?C
zu93OK02tfeVU_>TP@MmD@lf+F^?<NNvx{SSH55}y-FO3eDi^$yZ|=Voc2o<8)GHoy
z%FK+va>a$eRzsWVF)+c9NlUIp|FGsKk>5aO9Otmo6-`2TJ<COs2A^=xxRel!N3v6T
z-f^yS=fOL<oPt5_VXyl<;2uSELlJk4Q98x>V@X@l1t-__c|F=I>js}&E($3*t_0rk
z@S<$k*5zBQ!yM=8vX~rU&I7&-brFed-ShHSy7QL?1S@kQCh(kODCdPtXhjZ=GzYVq
zxJA|p-gJyS6n+~HKBBP7?v19?!{g?V@PVTSBS#fO{y8&S#OP0?noHQyu$F=UsBt=i
zZ)UNEw>(Z}&v{ySAyVvFIXyM2x?&RMUKp2u+V@i4^7G*9CS+%(1LSNK^mXHslqN(C
z#l0c16q1hqCXZxYdckpD$g*bO(TWM#I4~<c&Y+Y_PmiI#!BBW+Xx1hawsb##zxZtN
z#v>d{OT=1vbplAA36h+EHzKIV%}cO(t{JUYTPt)kuN`ncIe001d++Qb%3V}k8b@LM
zC1DO`b!%q1Ki0kQ<omfjyZx$I-eV;d@0(sUc8deC^=_IiZOLnOA+q^1wM~6*i7_06
zWeAz3!OM8?FXQhBX__eljfr-~0Q`;&r6GNyES0zH;@;#Qm)yWOM`qDudHwg|j=w>I
z+9v#<5~1Y#vHWdFEBKRsk3{s|5jU<?L|Oz>5sV>A!^i8mN3Nx+{jR_kqGnf*r|ZQo
zGLt0nvV-d%?QANdu><<F{n2JZCB=9w&dJe(?ONqZGYnM!m8m!s)YiY&cuH@};AVUa
z(O^fiuesN0q2J)g2D#OTIYHbNcvdXXf>XQaCm%@}iT1lKX}W84ngd%CvQbyc6VOwh
z{Z`mbb_h~JimWgHw(?Pk!Azjbb<fU8>JsVO;Tj4YMY1K3)n=D@@P;DZlE>e&75G*=
z)*=dS`*1;3LdB2v^*{dNJa62;l@-i)9k0#2RRl2S%z!oE83<#fwOXT0Bup!HAJcr4
zwb5efJsnmSUSH3rnE-7~HuJV)5~_M!Jy5#upXdy)UrnU9h76pz1{n7~snj(GckJGf
z#6v2T(sL}+wiPgWE}t;2`Fd?3dV1!K26nIeXwXL|+IvmWTNHaZ%BA4r%oG<_(-Wgb
z8)4sOI~wT_e=nZY)4fMBSB>a>CiFq$sp@-yd(UqvC)bJHH@o=f_J(lGo<9~p-qlKX
zc$Bx~lq7-$5p-~fp47u2R<LPbrI#RFbF?B~Q@gJ5SpM@k_uvX=>e+iXH)h}xg?T$U
z#MCC+?EY?Nky!ODR{6Z=&YD>Jwy+@T`Ce~67a8qW{y7gKa%?QVCNvZ}dEhM>sM$hv
zi}s{j^&&61{t63}f#X@ER8pU8-8&yyZ++|({f)YCPm|{i<?1sTsKV1cg@P}~7-9<(
z!`X$>xW+-h$cu)ODQLnXD~ju8U7NO57hq32%h8$Ls(OUzYbsF!vL&HjkEfhGPC@l*
z`9QGSaV;k_)X>7!4Kfo6O>=*^9~TU(!7?;9VBwB`>A7|I>a+6Z-5N)9xy{ivZ|k!t
z{I{kKeTmUKJrs*wsDckr0?rk9d+umT=)7tpk#_(27ds+CEnHsGYNnFRUZ5oI<_S0k
z(7i?V(^x&B6MZV#L{2Io<9hp!PEkoPrC!*=<)%NCy!>l<!xj73NZT;NR@H#W7I@0p
z_Si1i6++2=P-)o^Po)(O>(oIs#A%_sTWqSfgPZM_4Sc$C@9j3@H-UUvO8BmE-}l%;
ziVU#ddi(XK!|;^hbOr@&hsUF7ZG)lj1N=GVQ{NoPgv(>q+f2#Z_?CT=TKb=tC>I%T
z$<f`Ku^~xekQ=kdpW+F3jU5M;#h<Alm*JpI7@<bgt5%<Yh%B3Tf;AP_h(6a;Ge%Jw
zW|5k`n6Ul1g4$J`VoP!)HaoaKG;sLl96Kl{R8`x9$+sU|un0}C5Q(Ma0hXI017-~Z
zjjG+3yXRA{nsajIbvu8ymhI=nx7lLa8pz0!IWm-Ov3wxLAIXN3VV31k!SW6rrSvY<
z5bJC&8UkTIXd{H27YSUj<+|~|I*l$!wfJOEQpqb03L7F1wED8?&d~aANhd=}p7J#i
zYE)mQat4BhA=@8UoeF>|D0AMoYSsxE2VYW1c1W!^pwPVJ^95U|2uj1~h*Y5Ipe;M5
z+Bw70w}}i|t!KKV&y_vfAzlyuBIp(S9h5gd?XrAFqfEr0tq%Uq<H+ia<dOc60cLlv
ztJ_m=iMQQ%UF4uV;uZq>q_D3|S*>IH+O%)P6%12U3zf7<0QW+uPT8GayZ%vMeh?~E
zIq))T59*XRJSn`AUy@`zM<IlJMBiQ}{O!j|`0Eoa6ph2<{U0LAXG0TKi<EsbA0`V{
zxdWp@%^ul$NjyXyo#|zNg$qNj#&|h;vuz9dG)r3rMtnRpvTC$Ji<tb6n#&IpI1Qi!
zAzQQ~OP#>GVK86C*Zg^vUvD7qSP#^~o%C{CH_#g<{xJf`4I{Bt0O3E+dX!lOLHgeD
zl)pIK=7tyoZ*jx*9f_oiI-TG4(n2yTO9l{z&}pR?zn%;+e4z`}b=+!Ii@%OWxPP|T
zNPkV)OX(<C7$g&|Jirj0C#}zhH?7P!bT&5VC#SG9F8F|IRQ7qmXL);Rvdpz%I<-j<
z3mqw>$z@mnE*Ep$u`bUAF?8ki#(2QDX7DAHNW|q#Dj}hr2uEj6bn6_1O4^C$Y_{;;
zcrT6)gUc=Miifb92v@YCVuo20)suRPFs(m<A|Ob2;>RsmhnhCWdQI+6ZW0e#&M!g8
z@SkKGDvw6Jh`~4lv~eG6wcN~L9$}CuIO=fATO=mT+b*q1F(zB1v~<b0cy`^q1=FU|
zZ@n0-vtenNK-dBO+vj|YWj>2jJv(%d#uchoR#kWgg9utY{OJGVHGjSV-9^mYKy3R;
zlEM*WD?&GbwenTRbjmS9toMu69~A4Vfl>j7nY`r@)J<L5Ud$*crSQQZaj&IV4C~Sj
zf|#n_lW2`^_AIa!URF1F#4c>CLiByPQUei|<}WdjN^ykJ$&0edAd&d|`-5bT@Mz<T
zn-LsxYGBeea!B^YQt{S%Cy!(L!7+euWLla_7GlWcIVU*Y+LhJL8&O#0xJ1kWzD%(0
zK;G~5@ULnNpN@E3N>$J<qQ4et-^=sBiiyP?;a#^d4Qk)4iG+CN!Iuvq7J7w#1-sV-
ztFpo(Y*qu2*!!cCGB;aM!e(vsVEtVRfB4#~a%O0kp!yAD+eq#kNL{Zj)`ZwlHXC##
zXG^$1oy%L>pEC;#FW{@;28OV_;NV%v3zqM*ixCO~Gu~wgh`{PMSjC`eBB(jB;i~^Z
z+BwFE5-sSqZQHhO+qP}nJZ+sePMfD~+qP}n?tXpmeKWa}%*#yj{?$$@mD*L6RI1k6
z-$w`-95riJf>T)LULOXc1->zip1=6&OtHPkb0b5E)fgG-bbA&%PCwS4g#E4zE<3}o
z88OwdWkNnnS%PVwa?iwrkgJT{oNDURBqk&5C6g^%NTWpTAfYm=o$|~Cv1|E6Hf<*|
zmKWr0)WEo9slsMxXoY|OC*?3IwNsO>q&yZo%`srnme0?QrNS)}C~Wk4wiW6?t`(X_
zk%Gp2;KQzUpEtifB_Drh5gzpb|0lKz?`fNR>y@bF@vL)y|E}sdMm0}wUmVTOka>f5
zJI#e_^mI+<DsI#ESOez0wksBS|651ofx$&`bD*Cp3?w#7IOQA>U5zboN?#*@4kh-<
z^u=k&m(TrcwaBP(V;tByX2v}!hX?P(WR*IjtdFD9lMijnf8-YjU+t`!Z7DohL)06?
zIpra5f!k(}j7mb?zA&>`2v5J)=E&;%ujA7+v)WCfEaM;$;Ax~d;j;k~?UGgz4&v#$
zZ`19WSarrfht0kv81bbEzYS%aa4tSp*dO@VG1MEP={}Gf@XXp_jd)ixu>aUvt0`rM
zZbH6ScTq%@BBEpzq3k(xVK<;r6>3OMbgdPfxg8O@?lfIlF?`B(L~E5#{(dE#ip2jg
zo_XgyL7fRX#1<IW1`lDT!j7)PO!W1zL=|p72naOVd**`n3O`=L38Upg%7AQu$tl6T
z&<gR1UTaBh3~4dl!rrsKIDsTl)4nr}vvhq!>^Ns(bn~cf%G~9Y&Rgf&@rO2-X!%{|
zkvbn%rx{FY^ct~VK|}sowW$IRg;<0Tq^!^m?}1oki@0Vu)GM~ujDKMgzz-eGGiD#P
z^@N>v_(7mqOgCf7kR;{WB%hlw^7|uOq+CgY6B`AO;HP^n<slQtzl`JSHKam)xX{n)
zUB^VEZ^DC}!pKm54W+sqQPplIv!qd%nvcx}HK*@G8@U)fS;H``BQi>7Q272)w{xTZ
z5%v&-DQkq!$rpIps?y07>Jk8mrL0AEOksG7a#8P6rk<xrS~w%NBndWUisrQ=6+4zc
z#|-7S@m0yI55}fSpp&ZXkUx~-K2ILwkT}f;f>|1Rm~!<KzKRh|qt{o>UC97H+vi!T
z5rrKsN6~io<B{7p(`&&j=K{ztWdf$jS(3bFs<D1^rFg;+Pj7n_do&8~RRduZl=<5D
zS7J$w-3le@a?gngglvj%K91<e{o|dA5m>|p-k&&Qi2AzFgRBY^N*>PJuMop+Pmxn>
zTv>QAY0{DhQZ37P-sckypcD;Pd%+%f*Hv)i;vNsyxHWAUrv;5HF5)lEKTiP&xq6hB
z!H)pd5Jo^oliuL$#3`^o@>cZ~3`K7}o_#abfnod_>0`#kK#aurdbt885uSx9M5??w
z(L{UCl{5IGAp1By=QG-jIc|^!gDPfzEnawkzXyVx`k$u&4?CqJuBi{#5#{8g)AHE$
zrdNI1L_nm-MvC9nm)CEyX|mU@tF!o8h79R_`fg(6A|RCvBvemRJ6P3<Q<xX;&{!PP
z$>nahsuZsQo)<@kB!@33(DANZJUm8KWA;4H6t@%A4TsKvMaJT)o`_=BD0B{oLDLzo
z+^5mHu358P@Z*LQso4jYE|_K-AC0Ix{t&kI)>!velfk#z`BlfHSJsen(=FSgVa;vo
zbgm<3(-|+o7OAiLp0Q8Z<+=9cTJ`^^lTU0fwRq0gDO+LI8d_Ii@bL+P;e?USg{h|r
za_TS0bvMJA_F&X}%WQ9*66`)TX?WrB?(oUZ&YvmD%D{xugC;8FA7!QN7JzoZq_>#(
z456Ul&)S+2qCVTsE=*^UD|WEG>l09k<7Eczvo`VCFTEIEkJ)64DO^u32Zxnnx5T~~
z(0mwV3v6}`Ut-O7(&^cU#*0K(Y*7+VGTIA4W695KG?`!9?^JzppDh5oee%i#g7+18
znr$h;oro1A;lp+OGYf}G8lvNRn%stu$Gy;JSW%GK8S=mlG(0F{QuJ}A8l12w+SSy`
ze?BWJ#1NFK_(;dyCAq{E=hC-zf!f?4uVJ~n5f5fap$N1By#QIr!tQS8v<a(1C_l7d
zmZmvO@Z_FZ?<uVk_#mTEsGy>d8<N1w*o_tL63%Qfk=$Qp$=BT9I&=WH?ks#xzivFh
zWEEK&X4RD&o_45=`d&-$<52x)Du#^GycaKq&Q?w_KQ)++)3j>BE2@+8HJFA?zxLgR
zGMa9~_Jde24<c!jqbYX{kALND)IMYlV>Yy3d*+K&@_mjd0cMe?HJPH715{9W`tq51
znY*GiXTd&B>tN}IgO_XKlrBuGY!KgvhZm-wqcgyzuI)z3SelK)J-yw$&6{t+^Q$RO
z*kcGXLRpa>NmY}0XXt4KXgJ)jI7(F2I{aynd!AyH3}P)jtvn!GuMl0Sw1==V&Wt8N
zr0Sh@<6s4QjD=!(DtN2LNj<P=-oRVUj$*lPpw!T5?ixLVboG1vYdlRLx_bx#-ME>r
zpdD_?Y^H(VtJmd*Ix|WxzZLJHYt!`mmCMu(db(l~8*r5J@ExfO71jBo8KdBnP}&Bh
zc>}gmh{pAj^;3I@0#`_xLro;UzkZz{x)~Z8c2C8~6%**O5P8$Zmi3kzte-o#{bZ{i
zY_u>8F73ov$yy(oGz`9J3e8kR18tsTVq{yaMsN2`xNlgbI65qi2k7{=3?*WnYeef{
z8BdhqY*Q}DTo6%=dKKQTw~W!xWy`M#M*f9)IwNd*-4F3lwRi`KOqrc?WLLkmZJ~0I
zFKb`Y!a>>N?-JY`d{!eE28}6?y(Jqq&$88Qp1*GdIcoyMv)|E!vlR%BZt$~+frnoF
zYp%phoYU^;>MHX9_ST~#+Z!PF8WOBy-kQi-R`uo-G(D0CgoYRk1(b?+)@k|`T>BqB
zscd5pG=<s=9Y>=hn*3z-Vb2}*r^J(;!sOuqb|eY?l_azMbNX$sf}kM;fi~6eisPpZ
zMC*9pB#boBi`ipW#v%}Kz<2yk4vLtX5xX4q*EN>G>Ps^CoEtp1r8vv4390i!^)zXH
z*NA<ORJ|5m;=14--Fqy}2_E@p7X<&}C9WC?+P<yYC4zC<!>eT2DCr_hvl<Gc_P?j7
z<<Ib`k%ywI)DQmr_~|5lwfO9Ak$Q}C8x=HXtj0nHGby4qvZh0Bw{HupZ9}s(l;_GA
z+f*tAqapYb21OA&k-j%XA%txf55}AGD!NfxskU5sq#Kw*$~<C`4c?NX{oHRXV6~!n
zUJ<K>MOgG%dk^gA6G+f6H<fwpXvi2zBql8hqwYfp<NaHr66(6<O^i}y8<>pu;!vZ|
z8`M-C8arXXi2)riNg=ajyotS_g7*;D5Km<-QIPC91)1T4Pt}DV@rGKcz4*uz3EMp2
zd%-J^xfpB0)FJS*-7IscVfNN!Xgm%de66cp+hIaIx9dr1XfBRumNc9<L@9D45_XrC
zTJzDnge2W!Uknqpz+36tE~a<d42_@Nx&?{JGrbo(^$qSfEku4hgsKbmzaOPU<k<Fk
zW`euI@zQD`biCPOP3~T&$1U~0Ds-8Bk{Dxro|UddmDhOAB#6lu^*>eqB-Ty_^`wPr
z7R|o!9$FE*QYkvrG#qA+rpEBGjXU238qp@3kItJsUTbS5h{%_)-ndqT<_9Bdzl2W5
zJiRxoE9we$SDb9^u{I{r!>6+mWvlXZ6n1R~X!m13>v{Erz<M$7o~*Z;n+n>~M+X<b
zEv5a&%c?^%&77AWRW1b5xy*U>2VkRTQ}}O;NOs15Gu{3tBT`sdQczvwUq<9ld(VGs
zL^A!uh-4(-Was!XB02v5jYzit!H5+5aUrGt={_3%(`EdZ#c2AYAzJ=l_t5?y%HhB4
z!hZ;ZE<ZA$JG~d>&-vTixrkZ*^dfQm)A#$Y1|ZYFU6=ppFk)n3{h9W^Moa{3%>S@D
z|26t|1CX8h=Wzeq3L|$=6&2fMw#uYm0W4uxI6^{&(rt(!U}gb<hG1r)I6_hiN^OV;
zmLf_?zY~x^3Q15b6{O;kZm@mNe7C;!&)=6ct*2I(W4-mcuNp&B=T~G@P=j28tOyY?
z6eSrUK?OE@CN8)L2mz52fBz=hKV%Iv+?nAwWYUwSfPxh!Liy?!zy$?0c1Q!m2(D)q
zAOhgU=OBOtA~8iJIYc2L0icA0eC>vbrvMcIzJLw}05b}}Ljn{{ye%W{1X5s7XNS2;
z>*fOlG#3<5($kYWXy(p6gB0z}?1kt7h;|J2+LLnu>jz{mWQK`^d{Ku0BxQ#gk`~n2
zn4Xr0xrjg$&eur>6$RkO7=mR8$<L$ErvP~o#b5yO7Wy_I6B~j~Yww-$-!YRwkH*IU
z1$6+xIf4<*b;RR?fItD=fdij#c?2Y;r`Urx{^W-d3E;N|mWY)6!o8Be)(<oY-<~os
zH!mzf$3O%SZULfW;6P!Rn?vt^F%|(}0J<ATfDtEl{4&sIfPr)C!~B@Sfw3sf05HJm
z<v-2KQe>cq(Sm^u{?4cPqK2s&%YX>ynwv+C?8zPVJ1hYd7R+)qg-7@<sK<jS#0PpW
z*#Qz3(D928c=3qPCNS9IQ_zj!mvul`<xitV0UnW%kkCo}4U)SBu&})<`=Rb}4?urn
ze`t>}On`A7@&KgcK$6?bfKG8G8Y&v>n+E`P2n;*)RkF8@lwL>z5Xfl^K>n8+M(|1A
z1qa6URd|RMAM_sFhSZ^+98%)L>+7p67@Hmf2@w8e_`}_QaFVt1v9dAb+u);FP8RkQ
z-aypU6hM&@6H-7)NDb<DVlqh3H}26j(8o^!j}=VlXL{xUKcXulorJ&_JY37(=V1Ty
zbS=V*vnfcRe~v-;z$6n2!i)LCC5g$8h|iz!eGK|?dH4P2U0?C5dG`DISA);a&9+tG
z_S?5O?9;Da;4hY*m^yf{W;8H~4JU%H^#!Q6VJd6^LI(Wl@1--No|Z=i(U!h%I%1eg
z(EBi<XhRJ1W`47iYPy}ienW`Aj7YHWuB!m~2}nraMg6py_25@Rg8N>N+Mt7%^WAX^
zKi`^!-DvVsVrT#b3OfKyI_HiFFagN}wiF;|-`)*Bbsiu{!OnmV4?%$FB#{^D;?>fC
zd&S+k+X*moPJ(Z_UtooYfQ}n6PjP`H1hJ07g5SXLJ%oNE+jree&QF4Q4%F}7ZVr~;
zq~BfmxeES&nOEN_3+s&vy>(5_9V@A#a!$?|Mu+7upA&7oj7=DP=HqJY-csuthdUQ>
z2p6M;F^8BBpNEQrG=90oiho>kIrsAwo}(Dtuz05~K8Zks%XPW739S-Jm@PLuVLqo?
zy5VQkC~~eLt=^Sr$YzO-x&|&;FX81h7cAS4`-AZWl7}s1p3<awEPgyqgVeHR^5ifC
zv$-umHc+g~;5+L^H6Mf2*8C!FT;eEB3@}~)RnZ#~^0l$tiJ4n1+T4~irncQaCW8KV
zIGsOfV_4*yLA@F?X1IjR`LA9!it?=$SIS>2EZ5<Orr)3pc?(wtzIjYu?Id$0v=DKf
z#6jcDF0_xmJ`z*Z!M>&yf6-jGk0Q*zsjaJ0ONIFD&-U|N%EwJFIjC_rVzN|EW#08^
z?GEZ1Y2b#|W8zXy_-M@fsJl(I&_!*JOn;Y9&KK|~*TNLw#pPb?*udY82&$7kDU^J1
zp!mFr4M?^;3NfJe?FaibuU~hhDl8)HzXGmJ-|CbG$95J(B+P?ThoeEHtouD)KE1Xp
z&o)rVHjMXyu2C{l@1DEG-RhNrf00zWmLi_pbgKqt2Sf@@9=ciX=Z|QY+98Y|yxyzs
zJ><r%(qBjOXY08zuG8ns<L~)@R~<xOw@*eu@u>rDT9#IvG3_rGmFR1@PH^6vb`{%T
z^F@p2;b_D)<BF@WrBuiq7}2qVuBw#P+b!rUSAIVs*2P}%5-0aQ{Y3-exu_#b2?#qn
zwv(PmVpJa$!Uo30e|J&C6m67ZY$#9NHpMqNG>jgVI$c)Y9&_x<ewy%L>%Q1T8(O4I
zC8S@p@YY;Q$F^P>5-BHEt{qHKp~xRxfL1@Cp|qv1HOWJ5?2Q%i<I2~zh$UKL@Uj`^
z(K8)nJ$a69PXx)5Ops8HL~leG{L`Y?(Ti)vnOD2;gu3m{AjZ)RS-q_$(8fpNa%1$t
z^`cN3Zg-aDqdC=S)BDhcA7=F-38kmRA6M4A&=p;(rJ;j;xC)z>1)F(&vy~`0{^g~y
z>ld(a8zV&zQ<2pH>|Dt`VoT@D&Y+DiiOj*b3-cGjre2$@?PBq__YKiHTKxEuYZkI)
z-lbbI`?yr(ZP!c^?}mOg+jf|ZOtd;20)nImHJgduY8#RN$bt7civ!zaN2H<tjT&$N
z-(o4s-sH`@daI+`&oqg;j<`em!~}IszNI9;4}h_Z)_m0r*xgMn`y^h``R=E`m1QKC
zG(nCuB!w#$c<C!3w`X=OTID${Gh#5DN$ipW*QzuO3$;>>#GV6dTWZxRHU2%w?Aw?&
z;`}-2w(v8LYy)=)7PhL)Lr)X68PUFIlvq}FM3%*z9(J?&8A38PeehjpnM_HaGEfl?
z=1Ju<YMS-<?RSAPK9Re^sjo4T_hI(1W&_kmW;%M^D>IP?+kX<cq660?a>TBh`$wXG
zZQ^cjFTdL<2EfaI>~(eCpkn2bJHeKCmeA_Ojpbw2Zy>b)=?0?qCFeMv<6(y{sy29!
z*D24ENg-iELAD?Pl()Bf$<`cfx<-kutZq!)<yTVw%LzeIUl2?@z(XJQ#Q!}+CXUM?
zNzwCI&@_St^*q_H{^`ibeQZ_v*O_>KUh1oS6_SgmFc0SXMi##zU?XnlRy%m>e)CG2
z2kj>o^$DkObd`H4rnwAV5UrtJu1X&*f*Y@3HQTzzM#$uewc0LgWrNY8P}e&bLbrug
zX1|CBVvbO4p_P5ilHS*}$QDSG#c-y}lk$8}EdI}&T4FP*uyW++;P~$h(1FbGrGQpD
zO}k9y6Bnt%Mo^@RJAvq<2Lg9bCi7t|`|ia-g*qXRy4k?2(HFZjK;}TrhRbZF>WrjN
zUqo7&@$3)E5UJ>9iv-q)WR55mB`}#?MD2=r*h>6nPuk_j#Lkn;X}frx=)TGQf-^Ls
zM6xs41HUazTHIYDVp(+!n<Y&BdNtCyEWG;pDt@YYVnsfHa=m2MOwaLXnx@B?LIBo9
z&z8|kt#0V}U8h2Z$@n1(1>H3cdrmR-T2EK>4G#Awcf~IMoUMhhucBbRkC|2Ebv3qi
zh^q=}iMHEWDxypGR!uv!(q*z!oC{1!?(&3m=;V!{Es*L2F&*cQ?K!n8y9qQ)Bom5n
zV&~lHL<`)LEXmpu=r!ZPiA`f&+(z@?UH<70vRr1hlc|(?&Rge^hnYG2r2d+=?V?J@
zq*b2!O|$~te~%7rQu1Y!Ta0e5=$b&-?q9;xRp;5gl@K=kbN2SNw6z`yt()|nk6F^Z
zYwo-`%e830$%ew2=xF!2p@p<-Ip9=Rz}wEXFtxJ%y|CP)*w-(Lc_orL^}RN`*=BEK
z(mV8cyfk3nx%Lr?jhi5A(|o>p>@xIsNQ`7@RKBok#9hc_B(t7Ikfki7jvePFi)7{2
zx9Br>4$oZ)s`z)SpF}LwW9t)phyQxv)njEy)ac|Kn$igowVsw-<(PE3Q|JKw<s6ex
z>2Pio4!MQ2NQ1i{1gRZYb<z*q*_L%xt0wn$eLSoc{k37`lDiG^{h@OGxYYA2rYQd$
zOSk^V5QQR5#m=uT>z?wtLz7vYouXhrU3;bpp-uT&ki3?#UP`BR#*2DRE$$BU{G9Zq
z6{6kaboN5`CyO*oS!9AIf{$3}r6Z}gZ__zGDWmL;%yjof3&n85ms?T1rl(hCT1DqZ
z>!??^rXL!f+m)vk!4>t$hjM1TsMLHfUlNDuLnC{bgpOu~<!mm?+MH)xGRa<V6kaE>
zN)-R3Q%>9|xs2hx`?1YKlyimCVfORUOq`h+H>(-$etzg}F)HHxeMcE9%QNgxtRU{`
zl?mO8*DA0t+a8Rg$GoDjYdQWD&E<m<`HK9Q*D?+3CdSgx)J78jx~Da{m4^&B+1NHn
zoNrxH!Lg4VD)0P+dZ<PWr^1vvQf7roC6})pwex{Nq4Cxjg9TcxK$`0pV><nIn6^fM
zdL-Wl;YXTr`**w)d?S|fvqBl5N$!aBEMxXy+8vehZF^12W+k{QUP$u&R=?+Z<B&(}
z>#g>I?kL0{Q9gZWEI0r2*|L>kOrJ>w3&hubT<aA0IB|g)Q6AtEm~rvkbT`KGi{2j1
z2pjMHH_%j|8-DwW!wkROnh(HL;!FWn))(G_&NYe{ZTs=9rw?CtP&IzIsI4Of-8#;8
zi!L<kRQb+P;Dyk29x|%>QSxE*At|O^8Y&;@$m>@Cy|8eEI!#EldVq3wse8)r3f_X>
zN}en<_Fg@<)Ejy1*fMqsYFa*b0QP8jP~?BN&xf^C%%|O={g|}(QA?W7_efB5WXJB-
zy4b(^WV<yC8O9QAl6FeIXOL5IEjldV|8~)t`B+G!e7mqMC|PpI864PpO2K}SZq5b&
z!GEDK%Q&0Sfe%E1jECplhZLC^LJ~~KVJ&d`5dVX{*w9&lLw`0Z|K_tKC<&V#0#`2w
z_gAUIB#x<(Ua5J8GZ~%EieW1?9q4dGcJd|rXuW{yRXvh^d#RRcTQwOS31b1>gmHSu
zpTgFX8cx{eQR$lpqPs!;W9NnAN?2og3e~FUs3jV+geLVejfV}b!^J&5I%X}KLHsQ(
z;tIMqFo9AD^HJ3vf$ixlP4SlpwRbDe>?NA51(kE2Z|Yq8ES&hLM_%1#3DL21d-9tB
zsOPHVoB=Ob3j9Gij}cT9?SZX5Mh@fkY7p)Rhfx{+1L8^)^*}>+80o5Q)j{=3FP}3d
zw-aR7>1bF-dO*c}&QN6Z{d*A%*!f#2=vL1|DXZNv+z3}SYi&or%S_@^b5WN0qsbA0
z6$*<6;PZXU3YO;DwJWn+;)Xn9>iaYrT9~AMM0AIv{Xjh)V9{W#mWjH}tdn^&Xk!k7
zH3UU-%uRLzn;Y;8SSc$a9+%~BbUCW|Qn4JpcQQNuWhiBxSq*r~<S0?DNb71m8<Lh5
zKNVf?MN=ttTpwLM%~u<Mj4ndgCS1JcgXB-wL~M(a28jKelDq6Li&p|fh}*fR#ft^C
z!e~sMxU|khUyECp&EELk#g`5#mcGQd4-MD!O3g*DLc+WU6kaqJr}8w;%&;uRlJSaS
z4!0EHF7paA^7T^^)PuyXI1~Mt0!mc`zrPc`XK5v<ceO+YC(1=C5`f%KXY*!cVYhV;
zg<kNl#`9MV@}YUZ^pnsa(PeRclF2<A{I3-E55wd<^#r|qKR0Ug#w}0HbcQpO&$@W|
z2aFu!xXURd!)Qn-={7Cxd4Te1?Iqm{ul1O?xS*=l+U`S{vdcs2r&7DXf)E&x;>QxZ
z>Skvt%d!x3mYJ~G>xNIW>eBJ1SdmwoeDodto_$0^Pt&!H|9Beb85TOJe|$|Dx-4*a
za%-5(Go;h&gQ??Ad!bJ1#63VevutM}UbIGvMq?lbBU8>;{2^KUdyL>z{e)@Ue!I>c
zI=OnC5HSa)L$SwPR?rZuOV5e-WQrB_+-WP2R!gU=!8X2g-6FF?P2;$saU9@(?q>pk
z!o+IFP|WDJ;~=XWQ9Rj3y5K`!q1~P><F=*0-z2!$gEG$kSx#)#Mq3Hx8bQ}Pqxi8m
zGH@FT#?1?z<ncyLiq=lh+`Je*ftSh|j?k!q7s!Gx?{#D(r~tx|@z`ZdC>?I2WS?9M
zCvjpyPE99}64JVOBYVCmE5*b-HFbI|nF(Tj+{n4jZ5N6VMtcDm(S?Q+m7u5&wp}5W
zeup8ITMbpxg>`7a$6H@Xkei>~$*V1`ut}(z-9hg4E&yiLWvj13_;=G_R*YZXr9ad8
zqz|^b0r-6;+!JnLa6j{AvE=U0_ptWZX8+%#v;v2OL~hn%I&ZL}v#Oi6Na$7pfuX$*
zReC-5`1qk?A2G~6F!Yka+Bd6e=^iprpK~3oZm(%Z{H$}r@15~ECiTGcKP6h*5Ui$%
z<eSYvu63o&iY%Pl`!Ux~f^k0BUYQS7lKOKz3O@XJS9<2@Ev`(<Cnk&^^VIjWmS?L^
z&qj}JA54251?aiv7PH4sg*mRb-IUFVR@+oF*U~P``=<L5RG*oAXz#Ppba5^X;_|Co
zcyg9wQYLzf&7M|MGJ?Iatu#0HdlhmYhUF0JP;sF#)lN54d2x<ul?y&DStzt>af34l
zLeU>S*5Cri71C{$s`?u?V!Mwl_4tqGX-<tHlS}l}P;sk=FuGH6w;r;prE{|J1hC8c
z1nT+^Ssm$H9x%oY6DF7Wf?eNjyUq&jL~+$$CifcYW$CYU`1qc9hmLFaI@ey?!I}Bx
zR#2iH>HO%Z^s>)z@F#ydM%;<lGtDrww7=7&pfl>A>?nQw_|b;sNb3AktpcN|NHakl
zh3S$U44>gDQFM_*Q**>Maa|T*Fa32jIq(ID1SAsqbwgTcgl85RQ+WEWhE@3U^PcCs
z&+p;AW?`a`nnMe9+eDR$$1Yq{feKVHgnt9Or+ZJA7<GfF1E;?wqF(*{6GO^#t>G;m
zzdHOfTWAZjd|M0YoJGteKy3iqw5*riut@%DNq)>fZcEW0xGKQ_v5p`)7xkbPijN?g
zCtPk@@yQYC)=fTN`FTqucDd&T`<4j!t+ZxC+-O9ID>ZcDBZLWMg%>7mh2H9);*HWr
zcfDjgJ4{1PNm{<(vHVGYmV~^cd~;CVuB=}Qn%@lRYU-3Hqv_$DYY+$+6-7L~JgZ|z
z1vtq^xxnE%_Y#+p%*&_Od7<W2&#i*t>4g3g2ot>y?)u%qc1H2GwGllfu-#^2ya12Y
zhhk2?jjxhtMWUii`%xIOur3A)SDD|?N#CbvzwR=jwv^(^dGM0WM<g@Gkgh5Qy}1j(
zXWu5V+7Xs)Pq~sH=9uBp<608+R(<f+TCwKB#{_~n)SgTNo@{x$<!3ieb&6!_eF>HB
zb}Di<7n718bXoFIo>{|8IiHnKAn3EWvC;ucyp~cqkyLp_T*8edren`Pay&v=$blg2
zZtos)QL&`~t6vgLtDJv)U{BMF?vu)AZhHUXAk|}zYt5oa&TII7(MbPZp(GAsI+)tD
zl0%Lf(!bWrRcrfeHS(dkRQVy;SqGQ>Z}@TaTxn4k+?H9Qv+}URKv-HeZ&@xH*|pLc
zzT$kOsahpyBh!0B=x@1ZhRL+92rpcBB}Rj`Ury*unLc}Myn%PrGyUN4aZIlBY!=^{
zUsmn_MHLcy+Pgwc-G#WghUiV(*hKbr5;cFUU42Zx&1!ZI2TI5%m9=P=?!jNL>U6UO
zyXfsqN9JW8J{7v=Z<>;ZcptJ67biR_EP88ZQD3~=!8-9&O8qKyolgwqcdv*s!Re^+
zL(4nO4dWyA8+bx-=Y{VQ@uo*Jn&O(?{xA@?S}=QcBp`<cfq4y31Jx$+3)+&u3zCJK
zTOO3jmdA`?Li!d6gAaUaR#S7QA2-Az-L|f}iY|uV7scJEzLN05H(ULk6oU+jK_fWJ
z=mU61M!2qMcbhJ8+}VZY`QSYM!r@em+>^FVg+GLn!!FQcU;mCa%O<WI^M2W_>Xr=`
zz1%5oC$)7xk68@Oj^X)(at)cFQgI>7uYCQi9l?s~Q(a$&T$P+}Ud9%;;J>5GLeEtN
z-j<pWYqNke(V3s|q%}@U{n`5gAzij5Zm}PQy5+Oq3J<FvzR&QSix<GtlW|2D>8BE?
zn(cHo`K!#BJUgX(ov$9+dj!X)Fb(T=FZ*OezO99wdTkXu;`yOJ`yPEl@D9HuB8PqQ
zVL6gzz596HnC~P`obFk%y8R6^kQi7~hc(O;!x~$~)uu9NbovE~H3(vJg9F`D0FdOJ
zE6!TI>Oh|wblxukCz7l-e^Y+x3uvwuy`@16H?@Y~3IR#brAP&)u!=TE)V`LyjG5Be
zckJp+{FZ$2S<)bg?9B0H$P23Lk8Cp58@t65^6a#(9u&_Ca<Q+2dcn76(3qcf!}pgO
z)GiEb#LSXA0x#=)l43<VP&eH)(r~EGbR%~*4;kk*c|uweFGMmH&|+o?Djq*4eNrhC
z4*6P4al0D@+2pQDk?xqVt-%z~G%6dEhWTvSMksieI5#RqqVL$*C!JAgRMqX7TZt&N
zW!tvUFDT(2CP~{|>Yz$j9*QT-@t1ejHZVhQPEy<J1iI8xH#yp*S)-<_-#~Z+yiM!3
z0};Dukm0b6N~LHP&DFBa4IVqV-ut_>2Ke%gQ88G;@$NZ`kFwFNJ1ysrMTe!1g)rmr
zWW40XzmasG<@Mc8FIYlYnJU(mQhKHfmD1%1cHti?$siC`N=YLDWkdz~^QM8T6&!fB
zVZO%tck6LLk*&u$7dbN`N@k!!t<h8kl{}5&N$k#aJ)bcI^TmqgR8iGNFVqavp<!Vs
zacL82kb%`z+0$c#s0>KUe_)>3;(<3m9AZLkU2SUO!DjNcx2tS2k4}eN*R6TI-u*h-
zMvn9myk7V;8XwB3xmdkca|j=vX}lu;`|I3q$xpD`16a}-_x`(2zq37!+VOE|+_qwk
zDYcc9<~L7%eX`8scn@pt(_l-2!wr4b=W^wtY=b`YXIEE!3DI8C`*t#xt@c$U=J)on
zWz3|zFmUgX*x^hfx$M)%3ng1*X#F=Mj2&B2bC0b2xNy?<b}qenE~WX%f<5uUFRoPM
zZK_d%m|y2pGQo;QP>nqko{Wkr-Bd&qPQsir+f>kKvdTBAPwDBlyMhMT>jf0-Dg{c{
zRb6t7pwnsj0|c)Y0E^8W7mTZ{Q(hu1S|0@p^Nl{P^stI8J>ao&(vgAK(vNFxuQq?T
zXi?KCQA|-a<k9+G`P|8&y#EMB&uSjldeHwY{2~7%Y{$3y7;2k8ZSzu6BK1CDj0f!Q
zNlr*yJ?Tl?+Q^CSp_6k%{*3^+he}cnMPB!Y89ot0#=QlGBR?*|_cBixy@LI-OwML{
z%e1LsW3+pnWBNc+U{hd>|1&E{<3>r>!Br%H#}%Kdz<YZi5_#t~vb|#|6`;E2f!ggT
zdVW8OJadf1<;eE2bO=Y8iGGQTpYg?dvTOdhnm1BXN=}ULwx`U?(FHIv$wM#+)Y6U*
zi!nBa+WtjSqvi_?0FX5Jxgi6nYl3xzlNkLWZ2Oqo={8u)6Q8u)aB(H##<jp^18ITD
z-TKtanT|AqYT6V(Oh>0ZczJ&wuBZjq57p4tkm8!FW|!vV0T2HhJL}e<sM_uhea>hH
z*xJZ-)Z3zXg2C|s%Dok)1pF3YIug7dt$o3#5_#Ypn7N>@UPWvIht@rTm*>?%&|<$s
zq{+9>_7<Y+NP?<I%(y`%BR79`T42+)fy;0D+cenxvYy4zW7Sf8Pnl@4{RxgG?kBEE
z|MZ){|3u2kQGE1iOT)Ks_27{#M#V4==7R!$ZLn>>t&UY3uSvHQgg(bNo+AhszC@xv
z>{&ilf9|eD^w_^Vje@xI!DGpmRg<D1&4+$FHK2#ORm=j(pJT?c_;pm~v(BD&_WWzE
zBri6dXdF_{5}NiIsJJ|;d1G-DBgYX5*{(f|X0oW}T@0FQA|}IncExKjP{4kH^`u?r
z7FfcK2POg8!dM19XUd~fE=vrVmyVxNSKjMMVWo#)g=Dxa|4VCH1(WHt*v>XIvVuzQ
z@*4osh*I~zBg;Qb^glCH{wK1mBB~*yA^uOY{1bum--2b<e?{#uGO#fbu(L7#f0Jbn
z_WvhYR{B?#%|Fs={xd@6zw+XL(Bc2ei&_3{w*SG4nHV{LX7eAsn2~{#ll4D_{|<~9
znV2{j|F^)n=_fMg9E(kccr%8<MZ%qOZgUfcp+`a3-95FCU^C{|94?Sj0ukw^ZqQAP
z_jUba=07QM1?#<@JKr+yG`EXN8CHc68UpigmVw^n<ir?Z2!g!AJVJ<_ogK6NogJZ~
zf_aekwZJdLf;r3JOx+#?@H~DPl1xDX^}jMmCs2*bK^y=Fn->6?8vt^AgmQj_aBu+V
z;9)!dFoaV=0Ft@fD?qseV86pS0(2Bke-NAAlmWH1x<$q}4-m#=IzVPn&<BlwEAYUD
z;FN)LF0cUPi4|;Jm>CQB8Xyb7k-_isW&aQZlv+1eR}>@j=bIY?;I;M|!OTiM###WL
zo~k7P*Wb`1tGFhBKd3ktrUCqOG8#4+F4q#)?Nz%9u-4@ffGZe4ArN;3=U@lu(ZRU}
zfD<@(6cEcP5&%{FcmMWKq(62Iz*i}3Kw`}Ow!^o_Hx!85%Q&Wm9bBzD2$x47ZUsP!
z0XKqxs8;;s;(RhFfJ@DbDMW*FpulI~l|T*d42;#y@@OD{B_k^Ug47MZ7v0g6fw($4
z8+y$qedI-VpFlayp#dRaZ8JD$OIz+2R}RSvrU@KvJNo-}R-<cx2gl0?FikHUT1vNs
zMhj=1HD^H9hJaJTcbb6c-)>Vi0(Ag-aPTkyaCCqJ5CG2%?fP%1>eEArH|fz=X&BR;
z<9!H65Dg*8fRCOOf)n_%IOrK%5C>N`;MdoW(ii(*W22BY0DsznCIr+Bz!$b>^NfBU
zZey(ZnAec^1ORh+X9C3P=lJJD=kA*VI@a93*?y_}ucDeJH5?Y|e@wpUXSHw)An){#
z4Z|87?i~U+I62$`zr1n)`2AqxZJ>{^4}B$7f|sB2&W~7%V*^M)h_6XNt&Z;-hi`Kl
zjBia$vL1hPg-`)=Ob~+)-nUSk4xFPI`l)|#<MG39`zwFZ*Y23FZ#;>$$*C)P)_wiA
z?>H<A_@?`}VF0c%yQ-N+g&={{{#}1rR{{U(I;0Vx3$yQTElza;7v2G!ii<B=;uTS>
z3&=VJ(Dn@NZ(iZ=Qy`WO+!eq=C`Z8WHXDG7OdOo=f?&GLR@w(4$H1pEdQ^xu`R^>H
zv0fabTkG2vk5>QzbMstus)k2}0qC3EUv_^%NA9a<!D{L`1$6T004R9*14sujAJxTQ
zJpeC^x<&qx64O4=9iD^SLwAqu2)JVEPizO!zF|KAo;A7+fhBs%js#Fc{T&6u0dP^)
zjmQ$bW)BimdW(Gvu)^w3Y$227A5VY+__GC8|Fir2#Lv?P9kg|qQq_I9@jdpPJ?f7d
z1mJe>k4Vpw#yJ4D7WXaT;o&EFap_O~u}I+-#ZSOOdIAqonRICX0v+f-{0&NIH|3uu
z3~%-b{;RnDD+^~0-<n?B`gcD*ngZqvJOpjmBtC)`_Bua7Qrsp!g4EI%`@t`yS7N4z
zE~BuIDcHu4uBk5t;pwXvZP-WGUtLN#U*!`#ncouM?IS!h!dybjxOeR~8#6a@b_gd`
zpQ)te<P8A+NnyxiONZ__6x`_Cxlxb!U^6q_n|V=vS7(5)!my7H@4!OmyI<hH@cQ=S
z^aMM3zXKGQ^!x=V{I9!r{4aA`|1c+2pX)c1xCnj+e|sSZ31kyEFAQ>qw>tcte4lw*
z=VD}nU#gszC_m3qjPL@E@2GmX_vBlO9I-bo&Ki)VEsGGJS4AJ|FFEoVO^dT1=7sgH
z3s##man>~~1Y;YJ*SzQ7)P2whq@W=vR73taRmA2Xm$aI$m4QWo!Pq*P;Qe*`>~y_g
zgZ_EpvO0^-1F!-tRaIL$H=?sY;KT*LWAyOXVp&)G0j74}F$<PygXOS+%&%`SVBdaV
z-B|LfgAHbnSv(~*tR5VEu5}t3<<07~tp8Txrt`4WNh~o;0wSUAkiVs8ChA^KI;ne?
zFa0!l6dPACgHPm{xs#;SeqioFiI10!ltwy%tk^0bk;JDPLB0GZP-q){d6w!=sdX@<
zIK=iJPuC9_D0CW=kY2~CFO!E5?_B+F2IF57ow@N~X!v9~wo#fPT@YphK>MDwp&QZ}
zja6{4*seZS{E=^+I}&|k4xx4NzSBptl?tVtqQ8v{;NA8ewf9YA9o8(v>@d+k9ov{p
zKSheooO#>D?0H(VNe~=ywo)_@(a6m%8}tTx$CRV-6NmgWw`<LbYTSdfZazeHt@Chb
zqSnkbC#XE@n6(7-*I42;H1I<GQBwKY$Zkf!;uA8(|N220-Gd3b1wAGWw+{fBzu^cy
zI)hZ#RZ)TPPT{AY+fDPUpVyok3@VCfcN3f5RYsVg>z08NEAqeSsl~8MKt2(I(26WI
z_uoCGw}J>OkDBU0hypLK4g;H-oi35$JRIly{if%lU_|;QCp{OIcM1L~nzOwMBp^?K
z4$J>oV@dcmD}Ylje7;4rny+9c8A|69agZs$I=Ej2{4^R9K?Tyla)5o!v}U@-->_1}
zZEtktGYb<y+7k)606y8P_!`qOz}}8Qw#W5x;MD{u*Xn|V{@Mi9E;Dve)Fj458}uBr
zdci6KW}_tlZU>j0^4gg%IWks8wO^M7+OEbBOZilGr!WqHe9VbE93wVscFWD6VQfAM
zGVE5dN@^2=2{<LRyn$r5&#Y$|0Zq~Qlz*Gy<qI@$8Yq0?{L*+mnVMLVJkjLXY@&*0
zHf%WY#oVW|paz&Kl{25}Y#BPR%m7rCNS*M+Rd*$|@wCU4J%6Q*4o6U$oi)P*EhA-3
z(99J;#lw85fky1n0D=yiwG?#mZu|+C;7O{)Sccw8waMoyoKgHxDW(s^41}*VL$~Y4
zFIP;WYH)aQ*x`}RbPPFnzv8@+V))6#GbP+RnORF*Yt_&0$~Gwt1H!!w$!>L*;C&WX
z5CrUJ^=i$0UR;fhH=50bur-N9^Wfja=~87NYW1o?Da1~g?H|I@XH3W@Y)k+Pr%0qx
z>P$FXsF4!xGr`8hchyylo0cf?Arc<a=b046R4siBZCq%A*zOAO9di{B(Pi`o(h%qH
zr>a#YoW5cdN%teapj)fsNMO;G`(-Axd8SkHH>IE_JQ<^A<m)5RZqtGr&(X5Ng}0q}
zG&J+lmuOmz7jyHc7cfkyh*Vv%wjl3ItV3>1UIamL;J%`nwC)<6kmv|B@8E7pK-t%Z
z5zawy2n1ur`mUv1ufx@$6?cnD<_ZX4#|DH=$xUUeT#B_u8KG?YErfSNd$KX}8ncXB
z?5=Hoj!29#6uA}sgK$-9J7;_#baiQSSE5#C-FV0B38RZw@ZK{>y~5GH{)SWTH4~=>
zSjl_Gx8cn#sJ+Ynopp5Q^L~zj8aBNn(ajZOB9s$of#@#KD8Iq&=>$nVpL_~h<{=v~
z`Ijc2uOvpzGK#p1v;VJs9rxLqn&<-gb<Rbt*3X7RRff7`Cg}VWCmnma(xQRAB)o16
zO4cEpZg^$R2}dO0(wm*`>UGH0)U5OCUUcXTZL!jN-(ccImYOZwn9bEL_p*B?Ah$zg
z+)?7ATTv*UGN{;gx@q#{6Pd4TOs9Drb8vO!*W0<ao5tao%W?TlF+L}*I@idO1kmD*
z$|KxgU(9OP`^#H+;q;dc{^sFAgj7M&4;P`t5=AE+>B19q?={S1Pitm{+)4o5r)6DS
z%VGFMzYFtyZ5P6b1THI?`?aWF9U(773bZm3F3Vnv`jWWa@p@MWH@PS`VeuV*snbQx
zw&D0ne@wgOp5V|}IDZq%g6F)|GroJx<!bK_aV;z`NnfwVf*5j+Pg9ZtbW>l>prcN*
zoE^NY>Sd7ZuHs>ie6Gtu98A!7#7o(GASPkgalER@AY@ao<-InI&G8+~_S{znmg57x
z6<dw2(}Sl3r&XcnQ)P*5xPu=B=_l`nQ@SF0gLZfE>OiuDUZh@CxnzSoEr}lb`z0`=
zv7b<k_eBz(43E-1|4MDTR=iMKeVsEQlxq8)iiWxB;=l-T{mxFlmAY<g!9${4Lm=NS
z7Lh=bM_!gLT~P1Yrevv^9lt}ge_Thi2kkn1f6wyJdClH^*5~nv<w~-SYa5p;^O=l<
z#p$Y-Ue>$gg5)*-LI@-ay-QET^F^1(ExboKoSKF%NkH2l^m>x=U;j&DSDs|@EZIyv
z;uCrjR<d8>w_cq_kAM$J^kD-rm(+GGnkA5LRcx<cnfLN%U(|fQgr%rqP0oi>{pn_0
zQ%VemkSraqmU}RnN;BhEN7q1G#pa-2VjCFt;nxY;y-n#)Gz&U7-7o+A!l1L^W3GYx
zr{N2XdX^?O`@%X;dtPxLtxgd(Yi>YY-=jLebTVmgN3!lc5@(<uJeUd2;K^7V$msl#
zYGao?<x-FHjTj)Vio=nBcv43N{J3#TA|AKZ^b)2-(G=yw2v>GL1B{J%&VfT1HzuX4
z+sNESKS;wS_qJt-1@~YlQU5ellA=l9>^==AHPu1f%yrE(qSM0KDT1UC?F&*YX%3Z}
zM`Fz2v$Dh#*N+!zlF=CePW<}pw+7%C5H|C`PLsET=elRctI@V-70vKt)KH_Ptn6(-
z$h7HSQvB5Q&z{tlS#P>l-RVKJ39E?{8bHyuVT!l2Fdat<5PCY#T-wasg<YBJ+>uzU
zbxuSFjEfsb6C5h>kJi1=F~{o!4B^;@Ugu{M_~|%Y9;S&TN)=036ukzLf|=GcDjV5A
z@?3p=@zRGeb~$D5ze$u$e_AeHcono0qg~}Mh14@=jOY)Q)e{^1%$C>nR)UI`6)jVq
zrqER(JB_LhVMNOlW;Po16x{Hyo2p*Xsw<^U2z}qXdLO%Z&vcZ}d1X<|RvH4f&a&!;
zoXys%pPN54HzsHhY1GN3fES6%8W-&ml#ntT%61pUQ{Gv`X3fLRyD7l%E9wbIoUxmG
z?NRL95M_0JTIt;)UpSDHFE9USmaq*LA*faj9aLu4yc>nO43x7Irf0rRYOfYhj}BkT
ztf{AQo6pq)Ea-iMzTk&87SwUh;dk5Y{nBd-102>j6(JvXO-cW?$^v1#J)-hLrqrST
z@F6FTI=UuHCd6P`@$?Ba=uC;vD34MQ-&VRgrVkt67$xN$sgy9?6)>eqO}nBzzs)CA
zek?-3+gg)mbYZo4@MK0oF3_Jb6Y=LW<Ih=0p~l$sr)32P0Qlq1#3y0iAJ$m`=1m)6
zqhM2cb1d2F6+&vA&?1K4g2CN3^d(?y*wu&5r!@-ewSP1PdFOR%u-2rKXwMthq%{Ka
z)nj3a0G3()!bMXDnS2t{_N!nhFqRl1gN<$~33C?|)$v7<^79bQ3V$v~0N+yw>;cyg
zdc_)5rKR)LX*qH`Gf8H%w<{h)7|T_(($^{O67<aG=Px<phk46}Pw4$_*NfM&Ew>kn
zN+=8`oDY$G)Et))4H2#ZbYk!iE~DN0(@0&BvsvjuVqtG2N+HEonBRTbC4DhmCMOw=
zTh0lK4V0o2Y$WFMYW3dCogMWmDQ)5WY_8L)@knGiJ;;CvxdHez6576ni{c=%F*vxH
zX})Z&yKc!@jpu!<#QjRO-(C1U<rO;CfLB->bVjc;Q6@1JRy%(4+wb0~*~cjdy7q{4
zvq?In5f^sr7u{N5>P;30)J<2Krim$fKn?mEF5zcT^M|;(&T;J~b`*h0m1~EnV|nyQ
zWklEOW`K}(DET|*8!XoT3GWkY%Q_VjHzLFu?o}z0StX!SZHcHfnAW}D>LM3$+?06@
zE>|TqGHj~^limEZ#;w|?lR!7$>JcM2tRxG-Z4$<;AO>b$;T6bY?5V@bbNl7;!=Zz>
zoy#Yi&~=~F;jPcGbCqYUxN20UJ=^^*m_%|On#oD+>@GwlY5U$NE+A%?w`~$Q?n!8^
zy4YX28ZWA;-c8O+=DAU_&d~)PM?c+!Od_5n*SHXp;$`9ZWDcXOVOI{EF`U`mkwB~|
zXREapdc<C>TM^&auQA$fslwgE3t1&4ZO&8F3Oz+#<js<7{neKTuodSCF)C0-MOdj8
zAe6T|IeBAq!54bW;n8IBo(kTWpIh?U+~lagoxiEld-#}@Dqg12iqGhMm4{_3Ti-xg
zDP>J<QvSMF0!((TVO+{(LqRcI&Z)|SLhfx2z_(2o)R9kz#GQZdb|{#p;d>a48%=}W
za%;*H#7l{LIh$ppf%y63ciXbbS(zM|M%;wV1F`KMrXUSorw73v9peBj$^!H47ti95
z$rs2Xnz&{ZN(CAZ^Js?3a}1u=q$nrr6C*vO+d_IJ(c{;4-awx#PjRAB&Sz4_C2^<d
zsYDsr0ad)?MlQRkmukzmJIFb?PJ!#kfTjRUuZ-Dlp!G>Q!Ey4{C&4qpu$Lv;yTHB`
zG=#2HSgww*U*_jF(WRHwH^md1$w$CI%>G{L`|%z(+j=@+y8>GiMRi>n(uK!*(8-Xm
z1p9gK(=7LW<)mJfPPqFg5?hl+%j4l*nM4&L#`@RJiGoCiQt|5AY#|JiwahOkY-=B{
z9vje`@NF8oTWJfBHV}~#^w>$@)0?tUw6A+t*f^)=KzC>TR+vccwI%r}hzjplyu?`H
zqC9>b?W%UnEv6?W<umE8cXxye(zoz*s^PV6xsI+1ry$M6va2wC-Pwp#;^mxQB-$o{
z1WDqdCkPN3FS!?B^)=wT91KOh)c&x|aRdbhrBE`>kz=}dJ=f5gvkqx}?)T@6GHk_*
z4CQzeLhC+!Gw{L7rq#Kvp6;TI^XOa4jM3dq$4?rf<o=q9w~4FSPe<P(7dXtlFVY{?
zq82ep#t%IjgwhoXT$x!-cSs$?cpnDQ@gSWkq3qV<#v?Vye)}@6>S7&rM3Cp<=#MW%
zz3Pz89B9VK$mZ+H6*FOnZ0zi;c6KL<<c-P`m>+F9WWHaEk<&Rn870NUqLL_E%$SGX
zg4;Fu`wH`!x4i=4RBCO%0un-Wwlrw<+`y;7k@edCg;u^{9BC&`K4wKL7Er_B_ehfq
z@(agy+A^B^fOY|W5Lx-!I}h)}u?PhgSpHwZ9fdoKUq>X5T&}9?Pw^^_d!Khp4A;w&
zMa#;#U1jFibln*)@v~$l5^N9gNYbjLec|5{U3)}6r}$M&%z?PIIFiEVc8=%08VZ=~
zYM)O!Ak_m*i2iU$rH>jaL+3mp(C@;X;Z4IIA7a?1LkKM$u?wd8M#En{zw~x%xwEC-
zXG3{qeP~?zz_TM7nTwGKk!ka#<JzP39}=hj3@s_9!TRYeELrk<od;0PF*zH{icB!P
zmNHXsZ#qm{D{|NqNj$LHP_82p*5k**wg#0PTM3#l#+h;U)3ggcKTG(AzMga7D_4HX
z&5C8?5G3l&a+gfcL-%?7wAp0~7FD}xI<LL4cwXn%uC)2!!~PlTV3diNW+S%RFyg<9
z<MSu6fs#rfOni)Ieh%l-Ht>5{+c;01l`6sLN8Fm{=ZN{mYLltxOC_8|&!^KM`C*yg
zBD70HMc7UPVdL*wd9mq{jd`=b;Z^4N6B^c6x>q2c9ldz+CtG(U+PG}M#*_>s);;vF
zpZ3>TvY><dGJ@(==lJ-<RJuq`jC&SddI)JE&?YOd2vwhh2>bNbN>S3^Rk4}?cbqT}
zr<Lpe`cwcE<hjU40UrPiPxmLc`0}@7c~*0E%l+ILFR;Rqb4ll-F;MRmz}JnC)XzPp
zxZ9&hYq!w$`#@-m4Mp4x13aeS1%imU!$gNBTo2)_%)*b7mv%1TVy4voU?w;|-*e57
z0Fs?$?Ad_?ndz~aw@wHC6g^S`{=F)L@-!5@&m{LhxxtRvruTDx<^&4P9@3@-vZjrz
zQOR@QX2;AQtUl{9%#xfUC<M)((Eg~*cu~Etn35cnTTPh@3c8E$2L7;yDreAycKXxW
zNOS9h!By(Bq7PkLv@)!n8`;nAmEoM(o?#$L2`Tty1!cckBiP!JlnyUfj&_!>o>HC3
z`-AzSTJmhztCX=!dnOdnQ~AR-7RY0*))sNh%{%Xxu@q#<@oH4mICdwI;_09*@YL>y
zfkPF|U8VBVAjB7WBBVuS^rIWmB(oUf($D7C{F?cpQjmIBH-4;~*(^|#%n6wkJ~9j2
z(;#<Gkke0j-Tj~S133MDe=Ye<r`qwXiDRWGSinh{%HA+ZDcED@Dfqa%F&6cN+X;}M
zvkKlOUNeOpj}cnGZ(#lpY3~#(S`=;BZriravu)e9ZQHhOo^9K<ZQHhYom-V#sr&Nk
zRVD9de2lM^wUU+0ncjM{`>e-}gNmqc>~D#QTK%}-zonzW8Xm`%LZ&kWq*GV2$2*=>
z#`a_99z={9JB9ifV^PXyx%9te_K}~E^MJF~r*IXoOgJV7a#p~#CO2hDjPDF+Wt5WQ
ziPFJ88$Va=wOFVfiL{hbp-#UmVIoqsm?F<NnqCm8MR2d8e;Ri<ki_L<_P!N9+F+iZ
z0#E3LcR3eMBdt2-81>?t7l6PSpw8AG2=y{T48!4il-&m8k_lx*JJu(f+B&)poeTp7
zmF8-t@kulGC2y$!?3R~Uty)}csaCw}JP6H}{%ri-VSQ$|0JR$w6K{;5qRw!w4inTb
z3YZQmpDtauATVNBPAHkk_N42EZy^0l>d7LKdx&h<6@a-KME@b|C>a<{-#klvm4pW#
zTQfpJrZEi`R5Ol>Qn)rojVI|G+rDkZh#dA#GPADPI026_hU|%}nM5Q*ZinjO$pGF$
zPvEsx&pX+nwJlY_35dzAh;#6L$Sn0y^zQbiLeDAE|2_~HB(gd)%bu#1Op@B%5~_s7
zC79Wy2HdVh{i~_EOm$}l?}xl?WV9;d!s~?%Ol1Px086+qx^;}iR+>@DqG#p}5|U_9
z8TXlcRE}e(pjwMihge8tS87ETFQ|`wr8J*)3g8KY#iAG5N@cehO$MZQhBo0Nnb4~`
z@8@Ay8v0h8uDjl#OsYmw=wy`R9z5aFejRNn_A@fO;yPF+aCgct%=8ldz;vkNk(ZmU
z_(KcP-6<2$-;WK-!~6Jk__ImWg|2vp_N4bT3k)%KOXH#?SQ>##%z(8GJ@9flHF=Ol
z5&Aj9C<8}#E5`9x9X0NXXv=t1v@9TmZ{U1{QJA+sL@o76b1+?&dEKZP6i;i6jEyFt
z$II#Fk33m}7xE0AFH+Udos6OHvW}bFS++pWFx)LKpMAp7rai3bQI7n9A(ay8!z3s~
z;1r3MV?+b_CT*OrNc0;Pxj_DnS=>Lnnync~<+Xb%q4NC2y&A9VBdhX7yR8(K7R5#K
zG7)hloQ7K%6E!#p)pG2IPswOs?VY$@JZl4Kt2z@(CzBP)zvE%&TYq7Xd&%Uw6p&jM
zI8pPxkluPz7Fj)rNN?P_(th2l9kYbHhId;26Cl+LV0d^2pKUpKYX&m;p?}znJriYb
zCNi?PwAJTA)7u2b4IJKz##9-Hi3y9tQT@lPZ$dZbwDvJPDVLU!#6<!Zds29>*5W6W
zkpJ~Vd_1qKUnFNtA}LW}&Uf+6pYR2FcKgIOTPy~Hu11hh?9|z!iAsueLDqO37r#3b
zG6i@t8|W}1mP^=c_1i%WU8Sp0YY9}!sN6x<VMP!GA<tQ!`*cCgY~oqy<}NCpT45jV
zz%3TCSDT<8wHrN#6DZ{r@h1H(kO4ub)GN)NNGo@TQzs+>NW4DIEv}^`z7xA_{-MQ`
z`J1@<rzMyXwMp4FHAbW!cHcQ&N`sEXY4eF{(~bUI!Xld1q{wkg_cx5yqTyx=H<b|E
zX?rHEVhq*b_7SvwSTS$xo4-*mo0Y#GA2CaG%|-jl_??ynEZ0J;u1PimZR3wLVDsxp
zB?Z2v-2j#NOUc)zwHeNVa~P6o_AkwLn;NHU#UXNDyeCaM!D4a<pYr7}VEB2UtxcD*
z%;20;P-ZC`^_0Z~y}C42>}ksf<p!QN4VQM+`onmeQA*2xw;t*#KP~w|c4gwe2*f6`
z(}NHKO-_{6siR4(U7T06qTyC^&-uH3A*1#QfC!|#sf0WVn$6az1*aDa)KG)4@;G!3
zl^QDY>==bf%_2*`4jH{v#>=`Ihjir<GUl0CG83vN+AFp`E<wnT1HUzi7Vy{39woKa
z3B}igynf-Q7Z&n>m$?>a-1qF)|3dQEyZ(w}{!7{Bqewu5?!J6FFnEg3-uv84T-h~B
z<_hVo;xX1ecCZ{X%vIUCMPg3twtSaD#*&flE$KILf2j}cNI3*Cd3`0Cc5@3WqUa^k
zPhx^8IfM3rZb{uG*33RR&>EGjjL{yo<1Q>WUMqZiVg#p^Y|;FOgk5l%`QDSO%yjcx
zd?MA@r;bSmVT$`SY~)QRxz#U@Mi1l5FWkR{wu8Lv21J0fmQ$RIZSv6dENKskgiw5H
zG`c4pc<c=5_|fPXbDm{QN(G!`YlY$?_gvT(-QH}lN2s`s*{mS*_8R)CvLe2V%UNeY
z99Oc{OW-A=S-6;0<WR6Z`-AtjNY*3Q1YmF+7(X0~z1SnwyyTE)$uX2JT^zpd|C}e&
zH6*uewQPFf!waa(7FK~By1sc&a$nYd{@L+rGEmN|XoQ}KrY4zZMdU&J2I+u9xrFFo
z{f4oo$dTL-wl0M##WDUiz%jpXW0oT0)Ngt^bAP2MrueK*OnVuWSta+$Ob)g*?rCks
z<oF?%_Bvj2b7!1w5_r>_(J^4ty1TJ1tNoU}Z0z!!Wk-o6axfHk%yBk(FC>nzWQ1!@
z5%)Z-a7<pJJ4)C+ZOQ9a{wmQ!<D0BQfGmIn{a}b|2x~VskxX$~hoIbw!bR0qemmt3
z9$GOtrvD3T>*fa$dp;hg&x0Q1RV(tC3gvx<9&y+%h=bKGBVsA^QG~blbT6>rPG=b+
zaDCb>W@a&YAHa|$EK$+fKjvZ*C}24dxa?|?6?@&M(C=x4%`-@3WC`(Uy8;P%e=kR*
z7DE8I6hj|n^)#bQ%R8f_*Qb{zWvEP@OxqIb#7<ta9{#ZEo>|xWssRlv*ArV)U3O~H
z`$$eHf*A*5UW1F!|54&ctCQZGl!)K87OqOq3UFr+rmA)I?vv3%sW>SoA!K~|ZV*Z_
z$)Wmy*H1>p=wD19#>dQm^Jq#)V_G@u$nI0xENzwXVEW>Q0_c%Y%Z&uw@$$io&$!!@
z$Qk;Y+m%7oi-Q6?W-Qlz5`++J_>tSnVzy#ZwByy~ZTBE^?0t79uey31kS;xpN?0!u
zcxc-j3D+nA?21|3^Ng4G<YNxRof`C=l^1fgD^i@Ift^J^fBmL=rDl0nn$`tHbm}&m
z?K%W6UedfP0@X<KT~R03G7|sV5tMB)dq*f%-?K&Jp6^0gGc_{N8FFl9U~YXrr@h-D
z!Tx1*CiY?$<Z++44g>$88EPjd!lV{oa_lkjrw4h=^uCRvwwY$}uFK|$0JA2sJ>Z_*
zJF(z^<|L7Q67hB2mSLW889!e<|AKqNmYz>4J))Ol<w8K>{fJ2{`xryQ^j%!w=3$v(
zm>|lfs*xW=58;06X{2mkln%s6q2!cw{QFcb0N0{!Hc}&uO%+y5g_GCBb(HZu267IT
z3&|?#V|)KGG8wMDi7(0wE|lr44whn>*C&g2<_~s?RS0pc%RQyBf(x;rY{6XH{^;!G
zOc78jzk_|HVd&`L)+bx428V^Ib!A9qI+=5^9Bza|u!lR8uo-+UB)u-`ww&YcpME3i
zxi0L7Zm=A}e6?;H&y>_R_ME8D4O^?RuLDfm#+9=PSw2pdobzfRKsMKhKfM=4KraQC
z%k5Ob3QxxzdqkDV6IuO+Ccn^h){Bd8uPYS2Fo&NGb|}p}tuIa+q`l0I(p<1H5VDi0
ztK&(#A@U)InwKM*(`{&o=C=jE9jKZ>E;?h}BwI=z?MQeQB5{tcrp(0H?oh=O2oWX>
zMZFpyl<y899w1L-DuH#J=a`DlgFf<lS0UVM{K;2|ogHdO5-9=TH22hPU<({biKL<i
zsA*6dMwR(4k1ZUYEvOPlo?DOxfXTN-e5qfjixA30_s^ad>#neF(sH@QQy8~a3|x~>
z1d0b0AbjvQNi(dsdtz{1FmgUT<KBM`jT~m0g=+FCWMHbK^)E>gW148gU{RCL7E=0L
zF=#ehQ+!UYx4Al>1gjpwf7oD)fWkxyQx()*_Ndo?g!<P6j7Q$(Dn^gO#`0dXhKp1L
zdEw^qJ2I`3DJF>-v12k}<NM$$dkk@;XFX{Ak|QE_-B+^E;<xR%zw(`VaX)M~Q#9P-
zg6_3UYi8jOhsbBa8tR(pP*PYXyMWKH4pYQ`+0Pxl>9lQ=M5*dH5(ijXRG&*%RW8?8
zd_>D<8?|JPcO?2hJw9RL@SR5E7wWkDXL|E-yDdY~x<wr@?pEYnE!3qQ_ZvHKO8%B^
z|7b2;iv)4L39Yv6%Wm%BvPuOXu*X@(PLg5ra<?vfyN>eq=%D#TlOE4w;z(I}c9)wZ
z7eD`)62=MM-w1~#*U<o*9c!&{Phtc&VDh>p0)S0<Q+qVHtL~SbDOX{rddk2@Dwm%(
zrsYSLfSv$q7z1|drb!%!PfO*1=e>PUe{<!_WU{sb(@tLKI*C39OR!DjoREJ%<qXPQ
zxW9VTA)!Lyvs9q(CaP`B6`b8?sk@wIAg~$Pyhj~kC~pLtP@(j?qtZopH{Y8pU)p%x
zt3PC~=QlPC4Ayyci=<YShh&D9dor}faceFPTw(9nNQH=_|BddKm32i-l9!%C8-d`$
zs+{&jpPh18^cn)oNJ;_@RXXW<V)F*5se3&*Hj(1TEg|Gbf}CW2x5|MB>jZ&~wjcqd
z#vfI|>3pH_yU4luSMBwO-dgTkoz>1_c|%OnlfjH+#%e6+nZ<=Ss?jm_e1NeZr-oZi
zIyX#--CSLSOiMS{T}Vf=h<;#3ER00&1eK{n1A93N41~d!6BN{do?zaoQcGw;6M0-a
z>J3XGkr9+d^pL!rKG(Bc1I>)kB?N<YBD=yoXNuevhNh(1ZVn^eEstR$C)o0;6_`d>
zplmj894sVxPv-^XO)XXDoq;7Qi(HDo<9^i6$<$CWuhS+Y&1K-Yx;FW5-e1;Qyn9la
z`L?eq-`I3y?$?5UPFc%IuMRN?<VvF!TM>^eS)roVHCKI~yCu?~?h?;ciycr|Ijr_s
zHEc1GPQC3?CN8~fWXcmOTHj|UBOzo=`0rPNa&<68AA#G6upCUbn*sNZn0w-BxticW
zrav!a?46}Ol@tbNee-|JG5E@gY^`WW3E4NpFpoCJdU%GH^>~sr6&Xg6FxdZQ8f+x&
zWBdK-@MDvRT!9fQY9r&S&X87cLK4QY7vi4y*}r-4Nw`H#((W}K-t2e<tN|a-AU_Hg
z@^4Bb{kxIFi_CW+h(_GmYFUZBuxD)GccyhW{stbIY+X@PKAD08<9%B_d<=AMa+{l9
zIPqKb&blAHNFhMV1L5pq4>yp9ZNu;Sd7_FhXb4l!fTuQ-cd>hbc<zgfo9Zoi8qJ}6
zyh`;^ylJ2Z?h7UW%CALS52L%?w8-k&@-a1&8R^=!QBkwW_pi~jt%F_6U+aFq+^$-{
zWC$Am<`RdTr(&*6Kt#C2e7RNDii#Y)<8D!~BhrEUk0;job)s)S*WzoFtIAhrbr3;p
z<4pf-UJg;W-^<RYPm+ryV_mmQ`aeo|Ew@LRXlgO~mCyBBI46TAFHRp>K@2*o6cQJD
z!gDP8&$!Oe{CZB%pP`%N4ke!yAM;9iU_6`?<enomtNzjNvr+y>+@&voU)d&INB{+u
zemz-VUV%Kliau({*Qs~vn8U*rM2nfkQTISxTxjFSBYnOwXDnN)B7p6K2dwGKo6}O#
zUeTa)8qM<OhuG1~kmrZN!)%*1kcsD|VCmc_4|*sW8c$SQK9q#UX_@s_#>Lyf;qMv7
zWI2(W*Er)x!BABvZ~vJLL$(sAJz2?Ehu#*7fCUZV?Zu@E3-n^3piuk<gIye^EIp7i
z1F$N^<iR3W!<TNFA5~#NbM|t;%I+HKTj3RL3Z3>v1sinQzxu&w!KOb|-sejyfrbVk
zaau4*E|XN*lw4282FqrIXzW|cOH*`_5BtyqYq`$8%_P-o+Oh8}x#K&<#st8Z(_39q
ze?lbNCIeD&8cVVbfqne>z}o&$!0ys-@Q43XrGb+tp-GR$pa;30L$j%ku52V#n(qOy
z7OqpbGAYWqL}(P<x;u7x)!lr!(+|J%0dKhq;cq50R7n3xP@7D8dV=YS6N=%a4~(8R
zibsqw=Shk;i0eT|Y%;$pv&&M8t?aQX%Td3qDJc3V>X*cwR?Z4Pcv4^Sy&wurRE7ZP
zhr-`3b|9V{jGu~0ABSt@65$JtPCR+bVSeL{qOY^7E?!nXB`iMO>=+y=gc+BA6oDaN
z^i6mHNR<WyJQ4@hX2+1Wgl%P*=RbW+&r&5GA)0Zh&*sA<6`jMsqkq=k^l8D~OR%Fp
zZ>A93PzUXagz<tUTaZCUYr=%lf$Us=z%8N&Qg<d&%Hvr@|KsX%cu9R57Yv`y!AY<F
zj1RgA&?cs@l`dygk8e|Ra^A%a@F;2K$mjc^J-?hSJzA;uFExLMPfTxIWJcF~M2}?9
z)U0@9(lxGh{2)4t#<IRXuu;vb`}bdO1zQdgQ<7X#TU#YIyS5BCujOb%i%0#`(qvlx
zB%Qv2JYVC7gDpXSL$YZ0oZUb{L`m6gpTX;~GHJpEdx4G5k4nOvtKXIxRULom+FWH5
z?j3Y8xvh}LH`i7_L7=5I9%p5Ds$x|HfdT4vUciZ#v<^O6MUWPND0brxkcSajt;mHV
z@YmmEFUJWH%FVQk`O1O4Z}^VULmX+`fPkoX2Mq!`TO_z6u=5BrFbSfXO{fjTcdZa5
z!;js0t56<>jKQef><!^;2<@XC2vsUx{RpNV5bo$9pTGIbM1b1yd?GZy7oV=YNXtGr
zoBkzE-GVJ2kM}f|hfb#j6hP_={r~)5;(u~p<Yx;vdMijL=1V%vV(A-Ien7d5tI|D@
z&Vpvv+8FyZvMoF*vv0=YFEn)5nvjtE%;N5#9__!n-W766GJF)#QaET5Gk<YnY}&mW
zK@9_IiTknNhlr|%>5@%IInZ7SI#I;~ez8Wbw%q8~yeEbyxH>$3&#lRC27to>MzyUP
z<h3}mgB#E=lL2UsCZh1Cw@Qnm835&>jVJP%{GPF!R_+V59!{_<)_TUxdnqMsotX#M
zbrEb1x=uT_oI|B}^v2b3>NJ#*I*?r1?>UUz!c3&9`=h(;BU#K4ojVla^|BLe8O6Pb
zL?dT`?IvAKRMgQV)znI4Z-yIrFB^6lCf<gKj7E^!pwxOHPBID|DK-#vIZ#t}6W4|H
zb*qw!U4dDg8sFLkuoYcPh!z6s+CeKDu}o8+63TlJR+F8WUZ8ihea2ZW9a#ATvH0>?
z(Pb1G*jc+3k+2V)WL)2N+zHy<jYI>jPq8pnE$QC0Xoej}01Tf9dz$|69i)Hni8btv
z!Ah+7bf&UDg1)qVJzVo=RPi=;OxraVOa=gJMieqQhQHo`%zn=;Z16P`5UPPD$hs7P
z0eR_JI$rYeEk1mNQxnx|v0&@#d_0_1Mj(w?`Q*In%;xb)A2G<bh^hPR7apQ?uv&KO
zHq1CFv5x`~+QL#iQiCQz+exfja@P6w5JH?r)DIc2>Unb5S9QD2=Q`)m{xL;+%LR?2
zC)4QKIix7LWZaE7=f4g|qzgoKac5I4L?fgJd0lTP!vNLi`^nh^KibM>nU}ArYxZb5
z9)am+>(s1)SJGx15skRRBKIj~6bGt|3bLWGQUWe9Ca9E$Zj2gjw%L6WnHI0-eJD*6
zN^oGy2|f~fhtBnaH4<ZH+wW_VNsmlEPff4RH87RC5+sy)YLLM!YT4SMor$jA+hCk<
zXAayUn09uS=sB{tH9Y#`UD9@^t3DcJL^J0Vsx|>zh%|+*Nc}>EkBU(|>3|Nw9h+dH
zxZX<^OijDVn@x5xG5r^>J1q>sE4(CaUDc-4?qLxwZW{R*RJrsc-VULv<a5{)WX3+y
zgQOpg%U$N)KM&RYHR|jY&9BCBDLBYm!s&*Bg?+4%oFso03s^#LKth6Z(b5L>9DHvT
zCV|Q*S|V>O3F;z(H1qR*aa0fUvw$-7CNtb1$*7>X84bm~_F_xQy@vT>@&Gk=SyUU;
ztq~*y`hNcO$2@u3&vl=7LdIBk`jfEaXYF3II00>MdeBRyGPz#&(u`d3&%PfK4*3BV
z<it&$VPlN{XK)<w8rKn5Cxg}~&y$%}N&zh-tgb4=gt(LjcDeMocCAeg(Uv%h6ZrvK
z)N%8m9<|1k1A6PfyvIxx>*_N`>n}^TZmt}70fE~)ti?$Z-Cs^FZw)`lW;9uF*5=`z
zqQi0D=^C*(S#du9H}R)mfQ>*pi2pafg5`f94*pMkg|wKAs)Xo&=PNk=H+%)tfASTK
zZ2$O*KmY&v3a0-pUm@_%Wc)uI#{a`#{2%t>|8N(p{c{!@{->V!|EwzhXC40U{^9=*
z@9_VPa`^u#6#h?n@PE|?|5yIwKWLBt%73u_FJI$-@*m7h|K9fh<Ud%L*#9RE@?ZE5
z)_-Q<|6Bf})kHaaa>ZX_j^Ex5E70rk25XD3jrrz=u76u6hzkVb22DG#Qz(9q)2Ajo
z`*Zp$ec8(8RjIlArN*rrr6`gLW`N99lmL=T%M6Twm<gU=Syn_u7<mjv-_+E^*wj=Y
zKVQDtz6I=iT_9f`oP$$iZT;a(NOTF&_zXrDnaLR}YGQpIh__uG*uM&Z031F(DJ%oC
zZ(wTbTTg&X49`EkpfUuKKNj(Bd<N|(n4i$n>~`PW!1M;@<!QGUz|sf{AU-QA=|}GZ
zzyLe`Z%BOsE&t5iI*u{$i@CKqtb9FF3jl}LR~)1zualXX7akcI8ylOb5H`6emlzWu
zmZyJK$iIv}<`IaaGiV0Dk0dsMojLFa7l$Yalz&Bg$YO7+@|ASXHVm!+z+GT-2*cP4
zWYv+O6_6u{|2MF5kqIEw3&Exz^yH70Ki*y51Au>YU+?s{*eAUZTFb8@D>G|LV?A5b
zb6p(^aJr^?5C9ZXL-fqe%oBi+R#GnrY<136-$!UoDB4P>sD6MSs2xN+f+`S3pwpLJ
zsO(5A+RKb=v@F_+pUL=B8m4{0u#JrHwY4>W%Zx09-!Qr9J_I9}+D_2<Epbajvs2^q
z$6yIs3t5R@vEhN?Y>}ze{vJ>w+GiY!JLLXsCgvDs-^|q1)bJD_00#j7&B%$gABVzo
z9r(8u^4lsSJ3KzU7nhemB|B<-TnHt^SJ2+E@fj2lJ6DIlch^t*eQz)T2#_>QbPk{>
z0TZ-VFyBi5B*CH|QyVl6?H1t1)E*fKQ2Ou3*S8${mKm9A%L<;~(67s!^uYntg=D0&
zANludOL%w=S6@^BGK9Xqff*P;MteJ8w)PZ2-f#1h8|+WkZ?7sUEbR~SE+38LX66Q9
z+iwn7oA0m8@H5}Hz<1t#0pM3HxCfWLUJ$^N-w1zLM$S6M7wFe756rLP`)_7IZ`Ic?
zM$j*IQgBrBb1zBRFZIK3;-9Aax|g>&n6^fp?0zL8Gw@v0&0p#Yz>in^s5OnOuU#Gs
zGoiaGh|NrmUmZzka!6>m_=v)g#`^bOt@B=TKvr8D18Bu2r-rx72f#l785zH`c<vw3
z)qUvE$ZuUg&7eT=tG^v6tt>4}?^aM6Ya9Tfv9Y0e5POH<$XtEU5O`|H@t?h%IDJFr
zR;Mtkfc{C3fax0?L|-lyI9C8<`@N|@GB^Na34BA4`bghG+5lwrd_%CdBfSwxn0+Mo
zVg3MQ<9tIv`iUPxRRCn?d>}7$k70c~Ccg-4om==oUTMEV`nOPi$ZgXZDF3lr__tI0
z`M0C;3y1A1`UvZ*FZ(96r@8Qhz=;~c2O2GW|F<&#mf6ym7~}&<W&RM-mpJS_uU#}{
zd<PFa1pAKJ8#MlK`}>UuP-^%A-hWW?+t<rV`%~8V6H@bA_qy7X6LrWp6(BP86R>BP
z{u^WcZ+fj|1>wW{k@Q)i{qoxfr~VrUs{h8x%D-{J`Wud${eeGurLUr8-}-BWpPjv(
zr{6c>`r8OAJMuHjx|YuK#$jd!%lNHC$GYdIiu+p{X}wo=-LDq6oj1`K0BvA*eeeYT
zxrZ9on}B+?(?1O7P5gE10nm^7DaI49_wZHU|AQW&{j0#|TGxjBHt_ow0PZ-B<L-Iv
z$odB2V>BV)(b2W>GYqIrmhRsZMd<tn?ETsPnTOZE_G32!%zAwT52QYOF9p(<$|V@}
z>-@;O99|nn)D}GT;{oY&oArCwlk-Lx!!?esYtlEH!nW4H-}2R?mfjnwW9tWZ%5|+B
zANZqC^t$80?=9*_EkMd;rBR}7SdLFoV75+5e_)^2*z>k&)f!MVA9S4>Abc<vJw#!<
zfBmwpUAT%%xDF%<E!-m-3BfR{HVOzKQYgM;>n?!Y2#5-WVps9elNVu|oL-ESohP_)
z%++Zn)o5G~*}g2-nv$ax6RPsMR_jpi*lJ@K<XY%FpmfYMX8gv9kASpgDbE%r8m2b?
z5#dP$%-arjqZ1#-`8VPb4mq;Ef4Qs{jezge*M(=C|CKRu2!WFUvrPTLeDZ+SUQK^i
z+4fAK+Y^6wZ<&1l^HHLukFJ<|I~z5j=v86f%DcpjX{#gs`AS4}4zeOa?H~-|PJDR{
zd3IWQ#NNKye-#`%lyfRf?{TgnDw9G%tmZf;<P-0my$LCe<UUGWsk?y5R^bCCEQy&s
zSB9-!T<woTXv&{^L+@uw2dv|Y7|5cCJN@JuVdqI_13deF{Vd$(8c${kbW2dhg^A2z
z?&8iUtIA07_&z;PTG1W3vj`2G6}$E4#s{Eh4fh-^1=Y|Eg`4Et=Vtky7c{Rp)JTb=
z8GV2p2>q4?)$N!)BKB-QcDF)s%<GaI+C6ccRA2ZHRs*!nOwtkJH&LKm?%Mj82z%Ee
z<D)Dv89i$ceT1k(5Xm0t(=>}>nh2q(Hy5NLr}SbcR^hg7uDCh`*~lL*iR1hvm{B4K
zde&X5McxaHO~%CZpQt&dFWuBx2{ugPV!1?^hd40P9WuHe40uGD7Du!$VX%Wa&!R-0
zYRsQhk$E>3CHX)s>4dKXdKC*dQ2_;7J{(<3P<n(_ztCN^jh?=0I5^1oEftd}<PAAc
zHOQ+@@e>F99v8_m)B};{K1(NfBZpy@r6VY;?a7N)Xr~sx0UD5IX<7Le^ve;xQ~_QU
z%u8xe?~ww29BS1j@4DW-T)*3}q0A@*U2GLHmiZgXx0G34+C%KkC4$}1o!1@tPMNB{
z@hZy`cvR5N=k9O`Zj1CA%qcVl<8X(+<@@lj_CM7eX{RZt<@T@VDglZpJ?BW!-I`ji
z0dfnF%qZFIy+p1O%-@<ji>j<LZu2$h`R}}#NN`zWQ&pgJ{O8iATsDt#Dy8@_^TM}I
z>gHb<Qw7*Un?-Q;?ypwFBKwlgAp$vX#{LofQcAPZH2!utwdwo!%h%6=bR!>B1-|S_
zyBXsU?Ixm9@@*Ri1XS2B&0l4jJ!>l1gnB}Rq~J-*u>_cS_Esq~Ds_xq(2FFne<k^5
z5(ho^m`q2ON&V#)rmezLjTpA&8?p%0wQ0#`rri@S8azO%CRxu*v_k#i9ls(5QI;3P
zYKOSI*EX$LVXGZEDwyJ8S{*~S$3|_pu>5R&mr&=KozncpV8mM+N8$(PTC=PgaCO48
zWHEV;$wE7>Z4RDEG7jb)vBkm&hS)4(*hth`{2XXD^YReuj2ipzBx|?4Z;;t=9GhB4
zIoF~v4?-#hvO58B_dMv-Y~>u^${WAub_kQWdX&nSu3v?hGqQBh!}*ePh#Hqt+@!f@
z44uj&4V)B{nT~Uvvdh`aU%(5IHij>Hr4+~xxAv0guA}Eey~-w@YJ&t~&QA|sfp<ot
zK;EgqqAZ%aBuMYQQ4a_;`l3`}$QK({$Cnkw?+YMc1Jhu(Jl{VcV5$e)gqg!v#zLgs
zRkM{5#&s(5^g*DA%}p~~?Av7=v~(sCuA{L;Yh2Nr16INhSjDjL0Yj;YRMggTsRN=S
zScYytB^-UnkNROzdEexZK{NKvV(NC1<P1M^=`F6cs$%CxFhY?9z%yAUe3Cz1>p+kS
zUe9dzeJ9`Vgn$MaDT1%nG==r8gPo9`J4h@R?ug`)_1;FUmmUBibXDU9EApMv5B$T>
z3Q~Pm^7pidv)#azpzJV*d{7UQfq2kdBIQ8??ul41aJ^B>N3Qu{^giI>!8&YXO*Su*
zp)E6D?-Qs^9>Vkd*HgxyTk6v+@&2V9$>Y|hK=BkjV^Vy=q*I<escj&8-(ZpFyt<6>
zXknO;#4@0EgfiV_x(6?+PkhK9k=+*C2-|mgoN-RivVd1>mEdY8cDjhr64bSb4``LB
zoU8h}cdh#enM&ez%VV1(o#A)<lFoEfle91*X%w1j)Q^Wp(!++fim*2>L1bFFYX@G)
zbu=K4rhU;rou88`aOf<dCj;oiHtIQ!UK0&3AtijhM_k=n$WRhru(PTofo6BQ0B{Dz
zj8i(NoekDrF|-m#>1TtFN%0Dd`t!|PzG~Uwl<<}4)DiXUYkieUO()ywHIpc<@}qap
z@*D2*!W$*WF$-hE(2UZH*A{zoV+8ECW}tW27}b3f=O!1zF|Xkbv1zH~P@FrtHfn8u
znkxsd5H%KL{y0dVVl2+qMHaGtGl23LbVZ^+2uMp4@Oz)}5`Vtw<ijSa>I<y_87I6+
z@a%h&J+HT}6Y)MduQs{S%CTarJ2I2rJnHtltA*~m1zM18E7GcA<RkETvb&kiXZ%3G
zYYKKS|5oLQruPa6NQrD{ttV!u{&Zw~8)R^TdXV4X$^t2TdF7P92Y9w7zQ}f4NkK35
zP3{iFUz)MbQIU+mK7^uC)`#Tz!sv=xj1th$)!l;JMH}<xdHcffDvfI|_{&nB-E-zu
zab*b{`_H$ier)ez@zCd`X#UemHOsvdT}|vC8r}GY+4Wl4CA9!xgI*f@&}3B>RsdqG
zm6O$fk(`+3x)pmM$s9d*4sH{3^foRS&-O)I6Te8bycJ_Dez+B&m^PTv<%zybuL2k9
zrc<AtS0vQUC0?G7rbu{zfw)gxR4Hq<w`D*9sz8q}lN!EkJ9&NW#RguM&Va|vV>2ds
zvY5TRJM9n1;1^1TVB~D}ARhe{y`{SswJI4iBJCx~awP@vno^#x{ez}968bgRT~^>L
z*3oBUq(NnX14VXn`lbiH!Z+U<J*{s)cfe*^>)t9V_dL%_g^{l!9s?QOag>;2Q^NCV
zXjV5gr0@NEzkLVsR}uD=Cj|u!7VBKbrMa<5e}_foU^`=-{x1lKz9|z?r`s{d#ghsk
z<xmgvpK`zTJJhFh(3j!Lzj;J{P7A%ZniAU5%L%5gP>6ZS!9>9=dF7GMHs_5yZQ?Cf
zKj#YRWr>p3Ze&aku_w3o8O>1Xc_1fbNY};GT|$H1hiOKe=!h@LBYCEY6BFGFj$$7|
z2O1@%jG~7}A{b(fRyU$bTqu6jbwGnj31ls2M2wbWQqk_*!SL)8NEPOdjkC$n?nubg
z3`eTCxNYxM<B1)p?q2vqRz_@R=3$q2weenTEw=;=J1hy)8}B{f<&sGz+BO1-?%UyE
zg&y(kObOr*tB<3(CD7wJy}9jSeV(GI)6b5Lg`5m!rM*dW?EawbrLOQYGDu5fL~+zB
zQ1!BULp}9<kgK`Ycq8khGZJZ@MC9BVnYrgy!E+==709rT`W{lQhFQ^>J*&CJIZ9}f
zkJOF{e?>M^di;M+9&=&aTPrV*FHI~O;B7MfigiPgWF(H0wrH?FI~+ueKR~8l5RJ!E
zd`1eqA9JunZ5k3{-SE6-hm9ZRmG0=Fy!9@nvBL#jLRW@{`*h093lULeecf>~<2c%<
z_*FJj8p7-qG~j9uj%KzvK}^)?6HBZuI;3Bdrd*qye*wiP+dJ49Mens8Cz!aQwTE2H
zr^J-?jcyo%2e3h{^A<BqZM`e>^3)b#m_LocMjlVor<wrHwIVnU*_Vfp;dH3!3`^W%
z7&1@GhndobMLfbpEU^U?t|b(|c$y8;Dys?eFx{wn<6Ede2)z5_44dtOnPmAG*DC%V
zDB4PTC+d?1%0<H|I!wdNnQBQQWx``HF38#WA9m0{+SUlMCw@P<<6sxWE%6t1XoVwo
zmAraU7j0zSDG1;NhVvBnoNk~L3d}-brdXoWPdohO-1LoEeH$!$3QB>)@8*?Svr$H^
z#q6A!QM=v8kL#qTrD5ZHW#@fJH7E14U1UabdY(IvOJw@(3EM&z#06GRZozYPBEi(X
zvQ=M7)i^<$ZGR@&EaT*ZI~6=1t}ibvP4{KFYPXyj0xsiTtHZ_DpqPl3KqMT`<l%d7
zes=6ze1D{4D7DR)6ox7iW<6}N8=X?L1-s(xb?7dZa9~S7M+_jdGvd9jUZ52ES8~B;
zs8TeGoN7-kUtIs`{X0iKnH)gWo7$FZX6Be`Pcn+oZea)Wxwi+)lSQum>a*8346lG{
z_-ua+^+$HfGR94(zfXO7mY4rKu_qOg!Zj;>GB8(9zU<Q?);WQqgoD}((ytb=9_{Yx
zrI0Q9nm3k$n1YJ@3T>;jfW{$es8jhl$fle}Sy|5>!Qu4=P~p<}>VCR#3_L3PBdR@(
zQWh6@r|wk&kyoaD)r8ib@5s|a4(Wp{gb!p++|o3OSg<;=V?W!4TnVXvwPy^;>h$gF
z^02W{qdppDIW9&MNH8CEnM@TR9_>~|D>tK$34XFP{)jcQqjDmjC`tvIo|aZ9hV}d{
z<%3i4;}hnx{%l(;qjC+Z>HW;Tc7OL{t^s586ni6N+wvCV$-vXD5RU(>uOuw`NExeV
zdq)Gw2E)Uf+=ep`u;gJXZgPfkxY5Q3F#GVUn?<xoO?R1}D~ncRtPEc!_^G@X{2`)5
zDw2^L!A;M3&~E7wELNu$ALWJ#bYuAvB&mcy;YN~2w?j7pF92FWauV1z&jGy+M{?nl
zZ5%QDbvcRRQKjW(u2CMO6YE_y%AF!zr&J8qnX<=$SE>^p;o9}W@ptM?`UF|q?b;>G
zR<|;pmFEFFmvARsvM20r<?>wY6>(+qZGhpbHBX5sEVD5+wV-MGBp@a+sKjVlbtGrx
z6pI6+a|+4PwUUh@7=V92^X?f*7i%K-_7S1bv(?aU3)hoQj^vn&uPeo@BvM~XpYheZ
z=|0}5<7-DwnP8RE$Oq}1;$d#V6~CaYe}H8{!&V?J6$?eW>g3Dhs%xcgQ%dTz9>9(}
z(5q&pnBY8j_%ygkv{Zjt75ah!_2+;Km=>cRxdB>N_~YC52>pkRvIQLk&dFF+pWY{#
z()>K9crL0HM~UJp&8r+aNYlAwGBSej7Lsdq<ilhtz3#MhU5Jl+v->kJAa_6s9XA^)
z(Cv$SfhACeBEZr+DmPFPLhn;Fnui1hJwk?>3Vn(fK8EW{QLRpW&cy!VQxL2fIN2YG
zQy%?=F;8}4hHs~tkC`ctQ=G#yqK4Jn_il4@Y`nyek?qg*orQsG#`g^e=F77qUBMVS
z9~dlkAro0?h$O(B56IxyWOn1CK!7b2@HiE3OTi7<QR#{PlcRq@BVL*!W=bf<#Jp_m
za~<<NLac0;I=ZCk!J4PPZ?TvFBTFcKmTR+7>bH1z%}TV!W6O8IHa%#Eb6(nexph>g
zJxY-%4SwypmE?jkANMHnJ5b96w%)aAmBQkE`dHmH>)S4+!?!OAWcr;$Y$cD0FH$@f
z!}*j!B+Aa=n+>;C-P;Q4gjKO;?_qM?+KTidPdqbqch6LOo7^v+$<XVTV1`;}xF)Ot
z3KwKHHYEa%IimZk%Ml2*N#vMKioWm1020~On9*@)l*m37ZbFUqsF^v|@k-38maX0N
zW=5aouYIGm2?QVzD@m5Mlmd?FUeqGvi?|bMEL`SG&o%6!zxdoxku2JjJz3!2*}nVp
z!pv*;iwQ?PX5A$qK(qI?is5fK5f81&<Y?5^9J6`4>>ts)9to4xB`@tvwGb)FoC2fZ
z#sHDxS(D{vxS~_NqANiS7LT|*jL=^28xcsCZKmpJ{MaR&0kC}K!al=}Xz*btd@HpS
zF78|1)8--jL^O6HN;jK7g^zIwQ(nh0A)Fv#j9&;Drln@kXhrcqBet}nn}sZiv6d=^
z-cL9mIl%&Shp-u{=BEjFjek*y!Q#4uT8B=rxPTe4RCfpUI5uL5cN0PVib`vE>r`wW
z+74i^G3@T35yN+Di>fy7#GH&NwNKDxeyF&=wY$=dhmZQEXI+Lf6snIb!4#BWd5}*~
zc6`@FLh1tZo)wIZa~?g`v2;G8@h}lt803hLVg6z6_+nbwcOclMOG;pXDQBp(%1Z$l
zlz~PADFm>z)dI;?%2kW)>m)uuNm}m#4bks3k8&VK<S=ciKD6Gh#yP5LoiOD?a_Wew
zS#v|@x@X@P$m-0KXTE8kd0|suvPj0rcC-%g0x2)tE9_I<H<~e<!o-(}7t2-82>1@(
z&!x^*9HrFObR{It5>U?~Q~3c3Aa#0E!A;Cfbq`xpQo7|^(-id8fr{&P_{D{O2PZ9Q
zZr~SGl%-8xLKrH#af{8x?7{bYpWGA9k779@e%ukdb3X0Z+1nr?)ql%(ANKz6+B+Y1
z(pbk&7J|UJ0fXp6N@Yi6k=7mranvbWrnVT`dG@f$V_lSxEs}7Q|Jnyly3$NL;o-Ah
z6S)zf&5O;kjgo@jbR;n94l-!=48iUcr-4e^Fk?vWfJ7tUJ<vW7bQ`G$)991CmngBe
z)}Y!Z)JA~7HpeM<GL7H0sl^)&gr0fdxOa(8UDd5#F2PpBWK(n05p|c7mrl9-TDoZ!
zr8v0b^^Pwt^L6?dWnKmr-#sIiPtcP+iK?P(;6B47&^3<jvO_5@Xp&~*Xr*F)H%=io
zm@V}WDd~kQoeoyX8QnuGETEUb`3zFvZ8&@H^8y2VQ$32I@4{<s+Jm*fzYe@8+HC=C
zMP4rm@)}EOgp(7TRvM>eJ71teOxE?#V{eFXYWO^6sR#eY^Z74MH8hRlT&+(GGouB(
z?)RJh?Fq{^dnI203;3=GCF+-e|56uis!P4vB-iI`m9!Hj1gUG{@bNf(lYJnrf;cw|
zd7Wiqhm+@XGEdCc5B*b1;%b7ex*NJOlEm3L4!W*mxKJ?&;A*F=NSfg`i1FRw8^dPd
z@(Pj6C7=E#oO^D0Z2%;o_R;kjAtl#)mgW^EX?AL}W*<MO7=awTdyo*sfJM~;MJiad
z=|FBXf6J)%@4gCc=yQT2o)_+>pmxO0ioIu?a}l%m!g^$zyVE@ng_9yKTEvkNftCi_
zmdR4YXA?Q3D6e@rH%hA}x=hd9)IpQ$eB667agFzHL**4fPt0<xi@dkmU%uMYD@sld
z^86W}@*wVZ#meoqZ6;?=rxf2oZ%#X_2lp^hdg*4f#^aG1X3UCxLdfZK*(D&S5Vzxx
zPj78iXs5&n$zely%gaP1ie9-0u`=>(4VPdmn@oo`pH)qaxT8QNV3%hlea*-$RwADz
z&96kQ6~rhhgW1=k<d8rYLWt4q0{GKzpTXUH!Ssc>8C-3ZeEca=$1yu$IzP|DTRtX&
z5EpNGf^l?xB`RnGkG}Tit;AI2n;XHkcauf&H$vG0F^zeH4}w&~x9pMzh7gr1j@3r+
zRKk&NKCe)aAYta-TYWd~H?@GkO}7=6YDvc>nIQh0KaDgu@h#Ew{n_39c>ze==t7FV
zjdkK7WnKJgQ*lZ6Cdzx|()76(v`HVFWI=r;tK21|wO>Y6Ez}mn3UnnRSZrRl4E54K
z8S?d@*KZhVs~1Ura%z;zH^sS#aua{6g2PClp#MVWOiI6y<8qnZ2XM1U<<r_TB$=Vq
z$h$IEEhW^y1V(v1w+J2Ytdn%HJOiZhioahE3sZtZ@eNuJ_|r0q?w36<9>G!8?!@D^
zA@VGM^OTfWN$Py8yVw$%%IDlt--fkKuj>jGct~4FCUn>YPFa}cn?QY1$TNjaYL@ey
zX~e~cgdE0_MAf7F%x8#?n+cl#iCDHEJ<gVZ2=e5S?)V;FMc+T18^p}&^S!d>*1T`C
zFrhi76;ACiog*%cA!p5n7M&*Gxd744nZFAY)4|!E!ZZtfCqg0Bn#DdXG=eKNJ}Y6r
zi1Cz#1lIH`YrzXtwsPdUolQ|G7sP;<zth{?QCFmewI>mG!)P@FV%Kgw7_iBp$fwDz
zzFNx1W+R+LYDr1HCi1`Bjrby;$p$*Z+q5ilJ3pvRUic$;rp~51v#9bKYfj@nXZu1q
z0u(lq!+*`7ML6k%>xI&Q1h$W278kzqY=9tgy@3$SaxU;A7O(^c|H)ltG-T_5*L6!F
zsws14Y-xO(iD{1ou|845T%hFh;Z-l>Sz_x?OAa?7@gS3J-@YBE>>rxTtB9XM#ORVE
zM(epubOF!{4#^^Tla!Ed0yX}88-yNdBdb*8!+}uDsi!$t$_kZ^9*;z;x-_Rh0#fuE
zgf?L5pP4nnVt!t&6wA1$ChIMw$~m?}*M3OJ56a+_AeK~{s5cJR+#XN&*8TV|A}H1R
z9VFc3tVs+X9IO6Vhg_@yReBi7q!QM|G)|p*fcAY8xE-g}GRib`)jHAL-FbOXuM;ZX
zvsaGMVUAaGyQ%cIEv&?{K}B~kY*(80EfRGLlv}H!!J|Ix(GX|nJ5@ez<zZ&e8j)nK
zvij5feLFN9TJC#mt<KZWdQE4wT!nzs=l3ro;(N-eIM{SNQwf-Zqa*U2M$PrM(Pq#K
zkGB%|rxIzk-5`arB~~>!2x%+Whz@oZIe3rdaBJpBT;7W6Y9^jsdFA0<4$I$o4oiw|
z7O@M488}=rLxAS&GHR_8TNa^igu68&qRHAJdql^j7P4nlm`*8(9wFC-J0}(|7`hwb
z1?`1&=lRu3aJpNkuC}0^TCbpvOg=9gue~|Qrq?)&_(LdYTxpNQ(_}X*XLe^Rl`2NL
z5=EF{^R$l^jJw>-FcmB7k4}d5tw9xCwTMs{Ob@`@=_k@lQ0QXTeTzALn$BK6cRZMv
zD05AF0wppx;T7X|l2}vu3uiDI4Dfq-J1Ed8IQ(2A-7MWvHDtJ(G4z_LAZ80>d)~q{
ztt+iw{sd_tG9|-7r!t^Dnm@ggKO=#l#l4xL1Yz@ajcW^f@ox=$vFwIrFi>f=G(#P(
z8%hd3V}9rO^Ql=^+5GFgLU2FR250dmC0AN`R$;Z-O|0Z6x#0%=$FslvfA6|@-jMB%
zUy7F4JV9DVCPI_WA$d2Ea;8RWp&9LSMZlnX{T<kG{vCLE(bo$4`8Ji8#=RuTNKXyK
zb;0#{Fo|zu*t#+9VX%F;Dz%>yDtP<VXnhJlQEay+|J|WTHbmxvmrAf>V#u#pakDaX
zIiU=eyzPtF90QQ*H$GIdO)3axf{X36I{26a_EP*1KYu4{Z;QjDu9^@v<1E#S>qq$r
zbGgY4mC)6uhXp&E7-A9f;=$#a8Ll5R(-`^BYZEZG1O^KSBBG>Gubx-EFo=D&?vXiv
z>hXHczEvmdPJ+sgC)DW3$xTaV>as}j%{_@cj@3O_x4~)i2n;1q$j}zNqBBUO%Y7f>
zboG$utWoQpyqIM?yq=13sd#W3b31f_!ZbOYE%7F(D>>5DlLV8ZLL#fN-ja}?w{m&X
z$Gl}#NJ1i~4uI4OFVwwgx>b2xdNL{4;?bH3a^SzWuNP*eKUS5|W4949_?e5<+;@KK
zb0z)8SB3^==R0XA_901UUK6H`iS|^%cVZ)mbu6{?)OM43*v+JCI??5G#!&UChQRLf
znUZ=j$l*P|AbU8KsCzB-#L8=C(*V0}WT7(6x(NSrfqw7rRCtl9*p1}#e4m*Vop7ls
zY%KzT5++KA0Y+UWMWy{^`6{}SEr9aUWk;mP&U)gw%C=hx$}C&OkvR`tzSi5fzE5MS
z$)g40{Fv^z)J0qLqRbBtI`k`_Kq~oi{y-`4n{_*!!|oB{z;1%;y%M^zsyJ?~812!d
zW>Mp#N4&DMx&oEU9MLycV-J=h;r5*I(oqw!z!DgW^{hokx4ZBOodV>j4O>aFjgG@N
z^D1ku@Oa}5cGSSK2UgUdnV^A>1CCs}Smz&SJQCwvfPwBt3_rPul{i<hh@s7fA)_6c
zPsUp)2`9w^z%|q|$aiE83`NGz=+(KTG-QMuX-+~g`1gPE`)jOp(*@;B?l6{Rpz`8-
z`?^E9*J<$^XR<DBF)y6?{YV}*b%X7()bob6(UQO!dJ$BS+GsWOGjgwe^$<60jLDu{
zsf-qOwe=w`EpDcDA#av}-FKStWRhk(Isah%Eq=BG&!Fy(30>xNu9=iysFMM%h3p>N
zNgRw3ym&HUO1mdz&65AY^KN&9W#s%NN^%L<fEL<VpF0z;3fsD?fc6qtGU8SQUQY&k
zFC>P&UV>MmRn`Jw9$}vv)x@+@#Yy&2qm=xi!R4m@2;GXU0=AF#^;|scF@U!uQK7r0
z*AR3`f4Li5_8GKef&rZVJd{q0UdGSkNkmOFV?T6syipXUw-v+UqkB2Duqn0P;~bs$
z{*9DDQ{%eMTTH^y?5m;MX4;Qz`dr|_(lg+189P$Dl1q`;$(*`Ie4OmuN3Z*$UR7r~
zT&tyMA(a4Ib{^9P#yy~0R}L}2h0sMR4N*GbmAdSjcj;g)V@hcCqQDE22y>XP+S!nn
zRDS~E6nRKAOl%{cD>;wJD~}f!l40QIVOQEaT^IVmAT47}29SpB%4``SeN&l;?pQ3~
zspw)Oya;N*NpPvO>f{-HsfuSAXMaJ1=d>F3>79`t*cEnPc`f4nD4(^0pT2%iF7d8@
zJ2RyQ%hJ-lxcXSmhOh^jfES-urw|vD-oRwgFwB@;=}@ESka1xSW#41}dHQm_hpF4H
zKB&%yajZ=Yw^U3sR8Q_;p;_H)HJXBR0&5W;QJcNG^@_>2TsRy~s)?LMU$qX<JZS|Z
zjj>s){?^`0GwQyF;5Gia{E6n+zVC|Vs|NiN3OOE~z}1P59c$o?q??NwZ|;s4_dx4-
zUjMNmlj?V5oJ2)a9Q2I^{^)dX<cN7(WZAoUAJ1{^_zqtxI8yvLsFL8z!log0^%k{Y
z3~6;OP2w8Cj%;+a$^e@r@ce)htB-7uIfLRIH*rtXbrob;#fD=WKxwQSIbL&1U!^|l
zYcyH+IL)h6Md$Ry3WA(Ss4_PRv_zklFl}XVeu>2M;u`SB|8=o8VPd+_;b#kL%6+Ys
zOG{7Cat)S_;{(k=B`F-MHv}<Zu7dn`1pQ311i3pR2duk(I0Fu9Cl25;vOBiWg2wPR
z8UKUSgk%@FEp*g1!HhBnda+Pw3TL5QfBty2EfM&9n@jT2_jX_+>hDd0g47wy#%oXR
z;Mm~JRp_Qb#Q7Dmu@PW)RL8JFUZjSkZ+hyhNEqb}TbR0cJ+{lvw=AzEH;sH(sg|;G
z?r^v7nh{e%7zxhjIr%B+i5Nu+4WSMtY)i9Y7rT7_sTG77<jHq9A#d;@RivDD=v~5N
zN=0XUj@Iyr2R%v{U$c2r2^=~|d;7xZAY4jZ_j5|mZe+a?(yI?fA9vJk=jj+b*@#U3
zFIfHAmq$t7wnEE<^R%s)T#Xeul^-n7I?YmECM$zLm)LkuB2|^>#S!HD^UN1kBCT!W
zAOAR0szD2>{mX4|6sgmppre9+r6i)M`5X8v77U5V#Qq{{g?jTKmmuqhslsF`Afe5n
zU&U@wv&bM(Y9GD!kXKS|DXpSn1jcbu)t29}Jm}FF$@=!^7AUuaNPaoDy<2~+ajNuK
zSU~~~qF64owpC<j`3D=VBCP2A!PdpSsm{8Smb2xP&B|D(Wg^?X&!E+6iGhU{idyZi
zA+5-9VL0{~C}=p;98(h0;w1|7j@>O+#01Z(ppm0_NuyVfk3R&yJ!b*gmosG9SMSYK
zZ03frNU9|U-0Ox9rFZx0X)GCR+ePnig(!kY)>tfa_T(DLTVk(v&*s%^o*k}hBBa;u
zO{Z!H5Sx0k!+1#=Yq_-Qo2H|yNRrOV5Cdr6F1+X*gNW+qGRbwxjHI_Ro_TG^!)ZRb
zesA$QHx6|yf9Xm!j8Xm@Y4;Q)S`a39zHHmJZQHhO+qP}nuDWI0uDWI0cJ1w+o!#z@
ziQS3V$McZqC0{bm$?x}<lZ7)r87pmaSpttN-_hMkHHP~saY*`nOj6N(=w}pG=C<Yx
zoRN~)Ayiqc^Wv;8cOU8@-wLqGGzLS?7@(<I5i-`O|C-<oAu1>qv?hiUWZUIv-YhL)
zR13WqQ{}7D%1DQB)H$qAU$)ZS0&4$$wbc)t;6LHm%z;|VI-Pt%L}ft<?|mYakOIq>
zOQ+-GvgGwmVjJUYYhjph=GIH#B+pN;BSHTJ9WX3_C;qkd(m;}no|$PnsSysxzQo-Y
zm;2Ed!a`?ny$4tQ791Rqq<f^!Oi<xZI?+)g;sQ{J65BeVTf60Bx%2wNoK{?llaRYU
z+_TPll#vjs=3q!^OLeh5ka?`ujojb+^&@;!G64ADkvjEe!X5`R@oC|;<{)E_#$UkD
zy+u<R-EFfaDF3holSsDo$^NK;{hnw=RaATBz_J7+JW~dZOm;-JIT@rH;)Iq~Ce#J@
z1nWf%+yeMX3>~4EF^A9)|EHiN$51%PYi35wP_t2=ZM1?i46!|8e=xoTBV_9mU+7UB
znAL|!r+(1bdo$%)<xsoXa;vNfm*i%&SAB~gRL=c%t>faT%PYgv4%C6i#JdmA0GHJx
zBD)EvdoSC|sx`VZJtJL2Ih3J(2o%Q=jUuH<;{ejs2z>>a)EZ?0Vc1d&jOBey&$1Ys
zqmGqqgF`c(LF;Y#qqhZdPW!?g2ewkfwUcU$xN3JIRb3Z#B9(^qvmag4adcjd1^rgk
zzGI*G>jmwdK(8@yqy1lOfHJpv3%E@@5QcplzV&mgv&g}|_NA<VL!}#y@09XSkc#G$
zZy*D>!sXPb?DXHi(tV2Neiz2Z>9n44Cfj}pmx&|FV<K7%rbvKpnX^5~>{?KjfYLs7
zK0A?l4JL)yZ)o#AA@k!h*l*nhz28bh1fL6xmFp=OnSuHPEwGSwI6dT|)1=ox`z4Rt
zr=%7=vU$?f6MTrBTK;oASb}<WFHqcZRqlE0(p1enw|u?BzV9)3Sn}^f0J=bAeZ}sM
zl3;kk@GCqUXI2qUUe@fWVTopcq?~)zey1R5xq}3bmcf?=V#5gs1qYSO0Li!|MLruj
zxzDKODJKRaOKE5A(m&}Q4&y0P?Yv-1Ss~X56fxvhXBjIGnBCR*(jh%wYY7$Y6nSRw
z3PSJH+jF2c<XUtu0h199Wdzy$LpL_GBYk02Mr&S0Kk6q57fH*w);|Zkhl}LI3|GZE
zF;l&N{H_ijs_+iNljt2O@Kp)JP5*w{qvXXT0*1PLb~X5^+~;ld`RjuNKJ1(Gx;AE=
zTi|OMxjeAeKdpsS$|?b8sZQ?Dn>R~y@qk#}Cq=;7ve)WNL!SU^c_q!g05VX1N-tqb
z!N{$DVRbK17it3DT~|$yUs2-rqS=>`oi9Y#M50zPt&a=Lvn|;N5v0s|)U&aRv`m+F
z!<9mS5rUAsMQhK81Kru`p=tkOY9We3H%fy(=`L|B|5&|Sc#r-ZGYV7ucnYk>{XC`H
z-W`JKJGvE}E1A(Os2r3ee}%|d<oo4eS3=)z5Ig@J059f#WAt&wqn#pfL5$dQ^05QC
z(6-}&WllOkO-vnKTX@a&0w&=Wql%1u>KJrdUpd<%p7y$s4B*`7na(%arG3A9Y;aW%
zdH`tRA8C=9zKTJdWwWjE93(z><;2GsoldR)USE0PRW_+L3Ze@cB)I`A+nmPaE{o7@
zR^YIZ$cz_bO#9%mbA-Zsm+7N_**~($PyK3GYE77Wbv&dZ#OVN<9j;`lU!<$H>gH|@
z*yA^wJsjFx)R)<WAw)&0<W~y#KvLZK8q`sr{;O8Wg)FqFMmty*uu%SrI_ckf)YVZ{
z{pqRzx*2crIQM5myL}xDyc{wNim{q-;rQ6n&BVgE@C3`a9&tDA<OMS(BjIdcDE<{)
zBy?RL)9%J%Q^<rFvUgbz>`Zv$qGO2}f=J%2;+od-J+Xj$>*3$@N5iS@c%Q?JcxTJi
z1d0pFJmL=7h&ee#*;=LCyErKR&*J$@-U!9~>3gC?&biNx!ske*ev#{nQ*&!YRFfzd
z{CDN#3Zp*#B=!k87-^QL(r3G%13pU1N~c)h82w0D*3*s3sJ*HKsb%laOI0q-IZ<%b
zU9#d~%*O!9#gHDBGJJ)0^5%1?u_@H%ca7DeUJvOLIRZ2XiUW*GO8eq1vUK)@3oHqZ
zBq}Sdoy^g(Z?fGD&NCKM2kQh3Y>uDFq)~$Hn46`Up1+{SsVx^P-tO6G6qPsu-9*So
ze;Q=|K&gkR1S-!&Hi71@?`wDM9hY4u&!nJgIxqQ~CV3v`eJ8$m=cAR^wj4=~rhB?m
z|Bm$HlIo=6(YCAATmZeV$+ToLdjU-LJZ>~xg+wKbe!Z@#ebPxnnqL7UkSKBLCH;(v
z>4$KT^1A`d0^ofqe>(uhHGKM<*#;8jTvJF_-j;Wm3l)c{O3XS|&WTyR=numepK*Mr
z3(#nK{P>b+*-^7`@UXH86cS%F;KdyW`%$Qey!xEVNEcX8*M4u&q-B=MoKz}Lp%*A9
z*E0k-=8?Tc1<zB;Bk89N0)!Y3g?`)#@$MbBQ{y6l;kklde1+|LT|TBbkUP`_VP7YM
zJ@>A!rYreQtx&`7-%wlWl=Qs3BqylA^^@Iil}j2yhli7rgu~brKh0^oGiU0(Q`z|(
zk0?OJ`RkM+Ufbs{Sl4;d&FG1aJi)nBvjUFTWYS%LS)Y}bAR}B?{TyPi7Zr!6LJxax
zWghCz#tpF!x04ai0dncuz6)uBay}kr^$LZ&^AuC>^g2lu=SqM_5BnXwJ_vsujzSXu
zgQ-Dox=_n_y+WNo;qIfwWh!{OkaD|q(dLeYai^Rk28E<M4O!!DmNKb%mU?q=VP!q9
zwKRt@76iEUkxlMScw1}S{-Gmx*etCeR}*z-IB;PpWp~<B!fIu_hNe@tCYSMbLfBMs
ztp*+sApD&(`UPhTY%W?NLavAXl0?O4e90KoV@3YV-T(Y+>)6FSS(;uG*yZxR36lxY
zX8xt_76D{-VMUkI<MSMkNlon^w(P-3#JL1|%&!1sruu~BmJX23C|YdI+6|RFw>QIc
z@kFLwp~~5>lY}c8qdOxVvqq%^7i4Obnzrmz<IUlk2-*JoZJSke+Hk65Rrb~$A@DPB
zeO0%bncX@}4i`8d>iW8-ttUBm%lxSn;bC6?0hvLp5J>$xGL2)EQCPCXwQ|b8T|lSA
zm==oXMnrK*En`iS!$;W`G4YGLsJZJNwvzXqB7Ihwmp)DGd%QoBhJIu)-}jk<kg;*2
zR7cHn!W_AB4L_Ns`AZEE^l-)nz07zktqL?7;B@wv?cL>VyXz`KI#}*++qGrGn4IA0
zPax?mMEgD5=823Pa$FmZ4|d_ZO(RY30XgN^;12jQ`>P@dU-Kj@uPBqDg=D)-gvXnw
z^D!Kv=V&&l3$>H*8)676C)W3>MJ`w)u=va0j?8>MMA6ZK&>k#)>cNZXrS%bX0}U=W
zyjEnU!R>6nAH{OcwYuUCZhh2fepj`eVISvbF@|rh8ZOj#HqWE0UOs1kZ^;SitRJHX
z$eaeCt}UWNfKg~gCIN5?BH%$ApD0hNde_Rrw;-e`OI8PK7A=XWXsttF!ua$Egj+6;
zE|=sJfKE~)-6>3qH4BcJmp}+<9Q{*m&ucGp;nCwO(l1c>!?;o8oMhzEeVCbDY&PSn
zaev%JTvO_e)D$rlw~bd(JU84VW3SZ6oF~t~R+795Z7Z0$XRk?o-SwmAX_QZdBf?4U
z|KV)Lgh87Jm2=Ej(Mo!p*heH7{#99aZz0Y!vo0vgQt8uCCEPtN;79Q5i5M_o77H07
zDPI2BeBW}{CW}@q?t`ab5x)JKnO1;;etG(E(6jA->cMCe7yC=9(ZI4)Q~bn~=F$n+
zPHyi$4C1cdjOmvLa{@T3o-!mamCeh>!~YfoYw60<M6yJDNn{di)*f?H_!x_dmMj#V
z-`}ZBaDlzXlTE4hp(#svHV9sxO+fv$5i9HcN$tcq2+%KCM-Y-Ve}`jDQ2Cn}a_isZ
z{BC^A){YJ(F}cAPS6=g$bVGiE+?gV&X5k|Grjth`EhKc;+-6QQ`p1%?G<7A#(xZL9
znEK1;hSuOnT|;!vdV2_N+e4ScJsgt@LbSc>_SYBWb3jvc^egs!Js)c!fowGTT(X@%
zmk%mEnoK>~TQ;L`8`&j}>s>?2zR^9w&93T$EwrnNP|n<+_>^bDK)NswE`#zx+2&!B
z4(o8oD#$j#Xm)W%b0$w-VAAVucFTy=r}X;yN@B|szI7AO27-z8U^rO#K4{+HNMFRR
ztCrSLZ0^Z_EX9wAHVsL(O<@0{1akNTZK$sNx_5GvNw0<@7`3WvOiw6-sIzl^4$?Nv
zc*JI5>;NsWA05dDN5o8SoQ@%zgVh`d6aRhrz%xsW!+;>Vn%_p`ro1F6HvFat{BJcv
z;VW#p@*S}MU-HuOWUb~XDl*M`(^+F?Vo=%ON2F?V4<Ec~*UWxm5pJ!hByDkZRk?0Y
zP_Rb~M@qKv*R15IARoWgd{#MnHezZy_W3+XJ#hbN`P?K1U&@Q<znray1m=`P7aF*)
zpTkW2+nb2z>T$D^zu1;@SPS?iC@-;XRXBj<DEr@RV%8yR1kVO_i^AC#l@;REf5P1|
zD3_b&t=ayOF@4Q~01-^+uBO%>$ci$0`<)@l)8vn`(>Zn?mSz*eQ2Bbk7qN9DodMG4
zG0Kq29{!~WpqHhp$*wF<q!@ghsHYf?PN{8s<*!9ufL>02v)O_YHwZVu7;PHzhcfqv
z6gynzXSz754lU;fYyOQE47ScJ;f0%owda`I&B$DHAKU?@_jtva)8a)2bCf`ruk@uQ
z2sv%GD|xo{h1W_!nDu{d8KhVsyT~yG-Aak#XJ;&W_`9swFqjWSAw@w64gTW~N4Y1J
zNsnS9Tf{gf_r$HtaM{{MdA7P!_5g_9p$W%k;EL`;v4xrP;PvT^Pu$eujKz)=50%--
zqD?-`d%Ze^$>Ju_%9KcL(w`4}bae`Giv`eQ2*cheClZRoJm^)vlc=a5lov}7Fra8{
z7S)D(CNAIlqa2aq3QF)4_NzaZQ<(e^9OlUxQQOGKz7jI5;PK4Lx<5R5T-hef*oqM;
zW9q#|tV0<cJbC((+qjS+wBy!<EIg>-L>AW}QYsqi?6_kxhxi_b*S<Q<%Qp$1@CiwS
z-O9jDuqZ0pgmeadzUMzeKsy{nmryV_%pA05?EQ{_q6Qz#`#ik+%T3H9ac!t>P(G<}
zygV_i4q6=WEZqaEUB>N+{ua)PmH^Uv#m|AapSGhC0Sk3(-Kc6T_d&$x0nMrPzFx~*
zBHuJ--wcki%C;H!3!&9k43L?`p@1?ZI*lWEB9!_;s%oEFrMJIHY6pq97v8zbY|QmN
zG5)%)|N4F=FUqj{27WWW*(vh2zn8WNce+138sGQ3d2&+dD}4dqhd$gvnuU^#zPmbZ
z1K!*w*xk_gZs46%h)QCYA7Pdzxywojjb;Q4B-qc2@mxzu&{=tcJ3Zgp&#fhrk|<0O
zH?BU?PX(Gx15V;M@(-$cH)^d_X^6c1QQ2?}r9$wOhg)<;Ou?^8`1=dv-hfj|?d%Vm
z=bBoy_l_v&@8qS`>E4t3<7^QR(P~MSOdMJgp<%Z%?tWUdaV%lNS&a$}#j<8-FpuDw
zpHWX0u!ix|un?w!&5zFdG14Wzg4@Noh|tv8k=^}p)V*oC)h|)8(9)>4n-6WvRI+e+
z65(CrDx4-uC=y|A3<gxn{TZ}8-MF!>UH>_RlHDu{I#StIXIjZw191z*8Vc3N+n{OP
z&M6Nj#3X_ANxn1~|3WL=q2{=B?G}T0W53v!P3s)g#>_(6<<CSIjwPS1U2xATIpF;-
z)I)$-Mm*(lqNK`H98B$;RP`+io$fB-EAL!aOBCVvU4bC&)R~pi#0?>fL^o&apk%9h
z%WZq898G(09YYPV$(_B477dZT8L`TeUA!M{=O;7KXnLyLn^hwvA=S#%jUQXbcI~xj
zE{bXxd;YB^BhHegmo3e|<G@)Kpri@4iEv?y)csK@95$Zdx%bK|#3lw4dJgTPMo&e)
zTTDZ3!tYy8$27{zPS@?wX%J>}4TkSL;1W)mx$<Pv*<~Dnzn0dTUl2#xfaF|F2QMW?
z?>B+`L?|SouGgwlu+E#{{Xv=qpJc^O4cpf!5H^!))B+5Y>pd~wvV?<yrTJ0#g(|Nu
z7Q`4HB%{QtSvA|TialPd%p-UAx#Dzqc|zV{f1VpeiHJd%*AAA{o^}IK>DRX4Hs&oY
zO}a#mcRjn?)JGHF+#v&THidQ&v$I#4NQ0Yql{pX^nH8>Gg>9y~y7~DvT8z0dljNoa
z?7tf`Z++YSoPs8N{V8-Ax^Bwg3zADX$+(c1Zyrie3IuXP^6Vz6b`_avFjCY>O)of4
z7V8@6do(2ZAc2s(E*JPpXOHh9th4A_LnP%KkcXlOWb0B%ZVc7o2%eN~i26ZNM+jn;
zYMEbY1XGU?z&cw#7o|o7`-#ndF@x`(MdD{Rt(T~gyx^eGSL1UR(u%MqWghk5<~aK1
zhd!SI$zILJgC^!j)#2*I!rmi+mr+cw0K1#C15VgFXfo`&P5+>Md}GL^NIvG1H*CUM
zcCUpm&q|$=n$_xMho*kFfb_PRWqHIP40a0e#THcJ1w~kMf3J${$%j>aTkT$ydOv4M
zvq4#YNno0J2=v;pp`f!4r`W#vRQm7aOfQ(-0%5U)gU+>%2@KNgLDgu2o-s#DNH@Xx
z42=pT2RcEel|#sdTj$JgAyIswiUCZxj~;3(_L#Ag*O`0;yc?Cm=tB>as{wj;J361I
zn7kT%EBz)m!)3Q8iq^p*;^aqz$9-Jy1E_Ba)^`Ao#Q$6ZmUHQuc%P`6L_wXRuAZKT
z!DB8~F6Lc*R?s_D%+b)fo~~7l+afwt8~Z|*m@6Z9i)mRe?+O)fTp|`)tWI#s^A<J0
zh)<xooIpa-I8*j1O?#cWN38vZGfzP^As}5FPg1be6ds?zsCrR}J8={nTb^l|X=V+i
zsuSgXJ5}-8Vr=tlu>ZYmP|a#m7ZFv*QVDn}^R2j?F!PX#gi=c}#u0Zlhl}z<A;-yw
zJD6|y6KVlwq^6<!@hpb<O$;g5Z5T@ag5@4iQ|!gRlZe;LL8_KWK8hY#epMYWbBI+8
z_|9@|2yM=X#>kd6vBC6$t?UR@(vI}%u<<fteIbRpQL6EFGN`hc+~Q<hD@3?HNO@=_
z##{xR4T8vH&2XZ{XKstM&58M>DmEM+sQleY@49lmdlQCURDRJ+)yqT7iZ9#5`0w8p
z$Vc*<AJw9}E*6%-y?KT>Qy66`7${y;>GF4g`!wyIjvk2!D-0Igpm@f%!dis~zuGk$
ziP@>Wv3rc6`j~TH91032MfLEeDW2$RokZq<?kA<nd|ox(O3!(9r`Ds)7>KMEz%Kf4
z{ScutB16x&#85#_uLHs;S>Ena>IyrmdMW)^i%s&2?!o}M3Vvqm=d$lxsR+KLDC!H<
zQGyifkl|#)8_Za%@jj=^6t$NlUY=|0%U(GC3FW3TucD{g2U&g$e-28@nPm!1echd2
zB1fPeRfZ71$<{lf*ajD&E&XR#L{tpM1XcU`>68jc)**3-xT4bCVhol~k<dSrjw`&4
z6#ZIl9D#7ivvazV9<{{HL?bYirOw<JFMdG$@`(ypC_EQZ?YK3{LR6g~C(evF-`mQ7
z_$GQ^lGI~2tETo3MC>KunY$ga2YSQ9z-_4g4g@Qrc~8xtr6L)nl-&%nxAs{%Z&zL_
zO!LcXytH(Z`l)@}&rB4{{q|-}P%FQ%J3)}4S9YFrcE{Y-sHdYsO8n);1u1#l#f=|o
zY%`{dfD=}jgDUB%rK)xJzJl0q9`Bm?W$`G=Ix(>hG6j*1C(~==4+fGZYur^MHKEL%
zmfSvVJeTsi<Ftnc-jCEexvTIZ@z{V?yBX<s<4V#UGS?bi0V`NGn)qzLF2PbaL7m`G
zF@Hn@(fAcC<}af;Lf35C><=Y+xg{G0<x%yp>&Klvb_8r^m{r^u^KDkV<y<Sc=|0n+
z-szc(2KIg5I15QNbac*OiIEELd)8zxM#M4hdBfnU_2ukF@{F`3a;)7NoMR0k0?uA>
zp-O_<p}br2>RQ<+)YYe94LD3jd`2)9=$%30XcQ|W?$w8O7b5>9{E+33o@N(2gT@nQ
z)o;6C3Ljs`kNc>mz?*y8GAj9TXnZW*=DFCe=I!jN?>&lm{sMg=nJcr*<{6EVbK!%x
znv+Iw^aC|388EICbA{)|3Jv~e@bH8?qdL`L;l$Q)P(VbZSB90|!w^jwAp}WULZtbT
z`I0mSJmk-)Jp=|k8QVBK<T8@$QjW6yOb|^GM@;K@B@*iC((fM9)}IA1vw<Hjf7|DF
zGW!`m%?sV2ZyLT8&E7a;KPMZ^eYoQyWFj0#S|er@9h)+{I&2?5gF<BUXo$!nK*c)n
z+NV;V(%nejmo_}g<T>G&hiR5gqxWs-NKZ!7AbY*6F%mnTqui)~Mf=vN`!y9xUBMs>
z{m~^CplO7jr4lq-ykyFZsY))=!L|#`czXR!E>Gq`UuTNaU;Hg&nCt%)$-(|#5QzUT
zl0#HfUQJQqKS>TD4Mv9lwaUTH_&-$+PELY<O2q$fm4lI)?Z2=btp7X;VGBbiWm6Xd
z3VK0$p?@%k2)!u1*#F6MNYnohphKSiKZp*c|3o@e>DB&09UAnS|A}=N)0@zn(wkbE
zTe$p(#_>Nu4okOxG{^so%3){uudMpNLvuLTxH{82(*Hk&4pTQ%J9=kJ4|<pXyU$AR
zO7BMRPVYhQN$*APW$I-A-@%Um;5q&~*un8%_UiuvI~dp*{|nf`z|P6^KOg@a*uliW
z$oanucD()zw%)D<Krh7xqy+RU1Rc%i3PVV^p8^QOZqqJGNJ>B>Ez(lPvMdr3Y;VtC
z`j`H`ey(zvH=<3qdhfpO%rVa^QdT!cWDCm<EGl@Tqq(US005K}l#xxrfdB;N)CdrO
zgW=|K4UeFG<pTxFAzvNA1a3fo83H*0Ww-a6B$3fENP|}J3e2vc_0K{YoFW>Wq5=Q`
z;s_A@_Jt{s0|t~tVh`gK^g|^CZv^Kuo(U*`%accxrb5#!^x*@-n9&1XeSLLL|3|X*
zx?c`~8QD7^C^8y#9WHHQdKj$$j5Rnwm*<xpBsd{8Iyx8tJG-lkyE-zLnR<Cb>4=oM
zANRlrQ~{JbICrc6BtTyl7)AD~|Mz7yN)TM(PY}m%tzxiNdwVuVARt2^?J$hZEzF~1
zD?2zhpustyRZ~5n6r90lU#$MGZ9mN0sSChW&DC${=h&w@LBzNIESZ^^0yj9aw|qno
zX+Mw=G#G_spPB2ugCSVPdiF09OQUPp;Fs){48cETgCV%jiXBiQx*9;nfc9sppg9wH
zcwi)R1=HxWTCz>w(BPED)E+Kq<8R=#ZVsd03OS@BQ1$@aPV)8cXKQ14=ce~>I2wYt
zbF_YJMi*Cu<+?DA?m$!0FUEifMxSY#0XhIX0>n|UXCN-10X#Exn?B_-FK)j-<zN3u
zL)|woZUI~Xv<9*MUIb`{EBLTK&5_s`dk05QFV8>9|E!LISvZF9j1B;qfiwi~nB4_K
zqlS@P7;HfD2yg*-h-?8jfV;Ih|9lt&VKcbaKhIybKWntjl~L5v6;jOKn~(llkdPJN
z{n?@U=>3BOupkY<o}T~ubzcCxzpzich+msO`bwtA<NU|ph>EK_Ku~+XUC`_I-<y$}
z{6hnGa~ItpU-n?46gFi80Aah-1@K2;Sp$BJzy5k5eL6qCGr#o|zqizP{?bWqt*zfv
z0sHvTzxcq&+!3{>{A3M<Zmxl}5kM$yWG}yKD$rlATGRnEgmrGe>eQD;K-wsd-qPz0
z%?-|9v>_bQ!aV&alL+X{)qUpYyIldK4WaA7sfDu%`tf)f&<Jn-E)B=b&4IjnIfm>n
z=}<76wZF1druJa0zpWuSTRZ^B<mAZmFzW#Q5!ib(aGPq*4P*Mma0bS$;hp_D01aTi
zfM^QjG5W8QL$C(OzoZ}89)LClyTjlH$zQTx0Bn`N5$VYNWS`mX05(p2k>LF=AM680
zR9|EoG9$Yn(Unj5_ke&llYNo6{bdi?90p6j*#>`C{E%zFV}BvqKy1kRAk*fa*asdm
zzs0v0axs_D+@5}=+S~u7TCe;*e>;)c9{CZ}P~Z5ELRJ8|b+nPY_-X#7YQ9GGLEeR-
z9~-`a4IyiP1T@I~v4*r7-+>LEH+}*e%znn9wawW+|1G6>f)82Qzw46Es0F^7F|dbj
zb9kpU{T2UsJ^6VFUAcM#AGC7&05)Xh{taxH`dva_4Z6gKupYRE4{6=|8Pgn?e0cl!
zR*#?1U*-q1I=Kcc-5)QOftP*#pG#5@Fz<jF2D9=4v!H=h)qxpx)B}?nn#&=f@o-A@
z4IG-Ge3%8D&eGACw8Isq#nYqc67#P0QrKE;R8pLJCiHUop0z4UYn~1|4T{n~G?cQ&
z<#_aa<VfB0`O5P9%R|1)Qb`2}6b=_*%4G4ww!-MFf3DPuhwcmOFLERu`~?*#W~g7=
zH1DmoMGZf!i%!3?ys4|`V9R<gF1O6*&YIP(jCN|o&cn&6%ao>|eS1bD-08=3KUR0S
z<JUxY@GvP((OUdOun&UBN!LGhlAv34ocT^Cbkzbk?Jkt<l`(x9bsujIFf-lVL1`_s
zN8qj^%=qt(G+5`duz0XtO#-fiKKamn%c1NaT+fPH%jZW`_`911Jm=#8lQ?gHpRwf>
z$cL_6MI$K{?nuJ6xod)6sfbs*oI}kQQ&Po+b*XS!2B1(+XUv?I=$S8l`}j5(qzm?B
zVCUMtAr{)w=knSV)TZ}~<n%Tnq9M$c-nZT!rL65*zvJS`M3s$0HF<UOdQIERnAa2b
z-#=9*ZGo+fJ%%o*n^6k{EqE}bV!V;A$GR7c*-wy@4pzv|IDQfVfK5vo`D3AcTH0tR
zm&T8`HVm|&#=RbPvUg}?XD906-c3a$PddZGwj2mDc}bfq==bh@Q2KZcf9^ARixE<O
z{ce}!&qWsFr4x*w3*Zn_ipU(7Kw8hdxLJZR`E6*mQ}76F{MN$3KGfxwO62|}G9p8;
zB#{N3AL)6iPfAoUof~E09<^-$Nqp8P=S&@12U|$D%;I;KF{^A5y02&rxmZ;6PePpn
zqgtarRw3i(Lne3Y*<0z|iF(%$9Vb~B!;kyS84;X%++L9B+%OjZZie3uGRk#cJ<u76
zG-8T@IqrUJ4y(dZhh2)1^Fvr8vxh(|cJp-g1LE-JGNZ@*G)Wgsb_OlHBwm1GQU})l
z5P*N02_F7f3@HeoAWs`wXA3?qO73n?V})zLh6w)sXEvv445W)vzu|jHFH{Tl!g8o@
zFqkm7aTbU^QmbEM7xy?EH|U{VOKV=7F>U3ueyccR>3GmJrUAktY?ByYvlG=zVQ9*i
z>(b(*&!?LgAMkDK&2@Shw~EK_o{(K-rNzV4)KkAg*5Qw>k(pforhFx5Py~Qr8Br9S
zg*-Q1YRw2t<X*KgjC|E%UDI%^X==W3-B*edV%BGk8PurHdW$(@ujZx=$9lI!HQeiN
zHAosFMWqz}Nz*G#5LsLEfm|?7H!NM9F_3bFCp!+5-^GqsMc#DXY@FwT32KID8P{(~
zq}?XykwemVqL#9=F!>5Ar<K$|!cYs~q|h)OFYwfEkv;e`KUO8$E|ofER+{AeZO|aV
z5ET;@pD@l8tq&0qALZ^&AMqqF-|CQ888^d_R-^{)6}jq}K|*1}?NRe81VC(Y*1}e?
z)L}-#uP2YJC(b_{KrO9xFqCYP_f~{9CZPN`#iKb3bO@mNOcA_EH=PhoZuJSKwfG*{
zgqb5y=Z9m`GPnmm)438#_H>-ksp<Jp|0MoH;DoD@Td^4EM~%1lLJNvtIB)!&t<#<k
z0(=#;!oasY@^PH*9VI#TO{sgyB1EA!gxR;gJ?kN<f=j}+_HJM!|Koxw=fsk%Ek!{i
zuRTsoRC#7$Gqe8A4u_p<UXhs2{zyD?&`w=;?T^t>iO%9ylBri77&@os=To7GMYC{b
zYOj5oLRe+9>3hx_pP9XHP>n4@i=O-)oPKY-wigy_aw4HC3dTb86fQy4OKsgA+cR#P
zv=er~-rK~NT3D-`q_vs$m>mvSm<~Rp5l3T3DaX#nb=ZS(%hl!zD;*KtTxsNZ9M6%b
zgShpU!_GgCz`H1tmKu{*67*wet@9B0r+N<NmcCaaGM&K(%R?UB3XO&s7YXG>h)prQ
zefbe@|GauaXW4_b7rAtdKFX|~!=qUn_~KkbF-9fr!rG!M{vAI<s;z|@SwXezZ5XP;
zcFSvsv%bGFnZHTyu9eSM%(Ec=ev0yC8Hw{bdZ+7~4li$(@6*Uk8Oa_@Sxfz*G#f|p
z(f0Avm$)e&k8Y4kZGNIP8y?@%s?Tq57^7Mj*f$0<cGE}zk{V_p=rI*5ZSuW9o^0U!
zU|wtfW}Ts?eFjT)dg^@T5shxcms7^3?=eyNE0cYq?gs>ON>&J9*#A+eZ6*1)wrk$-
zV58d1NrreBk=nuv;%g}-LP4yRe#sghO8a(Jdw02Z4s|QJg}2;ZK&FZ&bPFv3+?1R9
zp2OHC+AOOhbDJVqWJ8AH@P=)8*Hrb!v=vBS7p5=sg_hZp?gSnFZSn~!h&76>oaL_`
z()Te&L%TN{$?gl4>f6p|B18X<3TGBO$4)3h#JDA$8_L{#5FPr6qtM3_G&K<Jp?ipp
z_2o*kVQK!hR2kFEf%#L=Lgz(d<)09r+|6Cx!P{#F2)+>O!u^vq#QmDbEl%HS%Wuy|
z9%~t_4*A%9J{L$BAh>koR{!@RW?G7m?*0%G*R?qRaorOQTXBo1G!5|+INh$?FgwV?
z!o!iHUdnj4B{pGFW0TYhNNxl5Vj{))I9It7WnxKG{8-7DgHJ6YAm~j^b2@{qxcdjZ
z0yn)=sZa(}j5t97%Bs?PVhGoBSO2g-4gvmlNQ6d!abe%*ydhwWpAxlitm#z6A&?G%
zU>vC|dP0r7Q0a+-w^mw<`g5I{iuENI6d#8*KLpAnlf2$H-w6q~r_h-N9dHikBaW(^
z#obye>_39|{e0&{f16s(VlpgwN4?p!baD>PqG96WS>m;4&D313!C6!4_Hpv=B|a$6
zi{3@|HYKO(?bj%vq(2q6J8AgEW6M6S>*Drw4h@(>1CL2;5ry+JhBFma6=jzQcT=XS
znR=Ot26_~gbng-ESn+0b6-ssDo9C3wislTDXc!=Hv(7?`#>m*(wy?d+HCnG|w@X2i
z5GLiZK1;+|;v1=C#T=5?^Y6hF>Sirn<odkrrDt7CEhH~ApBkw*9Bv3WZ!aI(HMwnv
ztR|=<RK6WFvmKx3u%1pte0(J*8dlvdtD{j^MDHW(0_`OtGsN}zRx@WGBq`Vpw8@|!
zObK^k#n~<<tjAMrJdlz${Cd6HtULB!5>8jV^$@0dmWzZI+1vy%Lkc!O#_6Ufd${={
zdq?I<NBG<8Ann;Mk0z%RiR>|uDl(&@-Pe!;0IlxD#82{jY`z`23jb1TlGAET%>JD1
zV9ik2F|)Fz+<^%Hfy)S*ztg}JXxGt{H!FsUblJs2zQ2>FD%IDhX5kq(PU?_VJQ;9*
zg7KvDcyMi{eCCp_aMLvAtpfYgaPtUNhJP?k2O0{}Wq{cbKmDCoIpaB02R=L{vT2`2
z+Q2_?{MsbhN|0@qFGv9i-I(7$r1oSL*4|I}-NB4k3wX%<!fEC~w>fOjyZa@QeYM}z
zw|wd1ia>5eXz65cv=^8Aub4sq7P9U8o=26+_2N4`w3Mzk-sw`2GyW|=C2)>p8+h;3
zR><OB*}UCn4;_PJAI>AawrknkSyDWS)~;pnaHR|7o%8o+PUd;ck#+%2SQUh9Hebwx
zTVBHJdaRD3Po)uj(#G=_$9LunO<yU__B)`8MwM#_o}UkfD@wz>*4Cn6<%CXBC>74U
z_C3Nr6xdwFSc71A5j_7i9|N-Q-l*hqWOW|YbKH7?0LbqZw{EP|J+QQM%{UyrQ{%j~
zu9fDX;1mdE89B#AZ7}NTKBdcxz6HrM9gctO@(5-ja)p6s7aP!~isSfJVw`-Z%Sx&k
zNV0aA^C^kqUqDdodc~E<HR;H(#yMw=J+&)3FQY!`@g~K@^2ypFN)gasgD>++If>XQ
zErk4;{WA=ed=~A^C1$H42Z<dQRuPZ!y~28pYaeHyJ2Hra1^bPTOBMVY_vlP;>R6cH
z`inM~UE@<V#8}qFq-l-|2Sj<Yi9L3H^R4A6!y}M>n~-dWb5e|m$r*WQM#kRgAw!+h
zx|c`0M~9>Rdt&+1<TD`dX5AMGMG7<%&KH-h0rrbD{iH)}46vw)4EyvTRph%Cx{GQh
zK6|_~xIE6+jFFVX)e_c;l?aRd86Cqsr?t#2l3nyDmFh6CM8J=TbdXif6CXcBUK;d{
zy1`ws{H~mc3X-1j?a1hAEdx8*t1$jGrCrJwI<Qcz*)9q^`!j-O0IfP`G;f4ubX;o0
z&nJ6ht7DEYK@Rc27tgJ#NwrxPE=}~VKDg`45o`@9{Stfu0H`WgD~KpbhK~=k#_MPa
z&nRuO?OoEbY-$das`0Gr7-M^8JBNgRCvlj<{Vtxhe+v8V%YZ=qZ|w0GS9|{K_lqI%
zySInH6UVI7=QThw2&VtZqX(0VRh?G&&Lm>@sTx3$+P-ss-kWPn&E--MxHvzW{+2IF
zRGOiH#mw>`d|AEKHrzXU72fjk=^p!|-9T7TJ&0zLlsQLs8ju9urnzp_`84K2uTQL>
z($o1ps<T<5ui=2M_nDb2|B~W9FV;M}c47`0hi?=I4>Ln(dA8X};zr6xi$N{d=oBH-
zu7eL_o9C5LI(Ez;Nv=i?&bsP%A}@htp3eO@ypd%(G*3-<>6nR3lVoNfWX+Tc6C0(?
z%Hb2r0)v=l-OJR7Sh;k^xa8V!_J&ezx)^cPPafX{wC3=@4(MWe@qpwLe&lL8U5aH2
z<!oOcZg=LD4#bvV^pU7lU%J8}-^b10z;tp}0wEcGPQvR4<sYgBx5)rQnb)Zm87Mh1
z>de%GSW{sDRy^1qpuz$6nly0JKQn9hA=w^LpV)t13@Od6!)!+f2b5?%PiTk?xMe(M
zTu$nj<3S&+qEI3ia-=;67l4*L)*_cpn{Rh52prI)a6LAR^!#Hb5EzkOQ7wK4&bY+U
zdU3stV@bAWQF5Cm-!nOlJbMRMsQj7+4%<E6vQL%{JvX^pHcro07e}Jm4(onydHG<*
zKLf$V=kVL$8R0G_Aww-M7ElW7XC)TUH6|F$vF47P^wT`NW0Mx|tse_OImyS6aisoe
zq@Q;&iuORCst%CT12B&9%JE{Jec6>*Ed<K3z~AAZ4-JKdoMj0x>$|5hjo#Q`xz71*
zrNbm`Z@_B61QN;a#XadjZ}DGXSwRyzx54*z2s<a$(aEA}Poy*8yf$kE`b;wTeGDa*
zKJ8ciy)6nT{(x;MwfCsA=9?-opzFkVGm)cQ=7y0cyL@=zmE*i;CjNJ?tXgC?p@7t@
z$~!E>KnV7i+vN4&`2jiI5W=}|RnGkegOUmZHAeTsZ;|JH7?C^pO}v&Z>dei9DG33*
zFo&agZ~#;Jk2t1F=1S{Kr}kMhs;{BM^8L)(<+uq47pdMdAsPZWl-_e5QsLGFeEq62
zr!j7UkA9Nt{Pw<>|FmB`owkaF!fQgfKRp!uvXat+RDf>E?$d;j!IZQ1$Qj_pJACC%
zJi!vEZs*Wa6Sd&z3v(YtSR~fwqx#i5TEMZ6tkU_p{k7rJz52J+`z*@UH@2JKCfOY{
zs$?PB7mMxv7PhQmP1&(MpLGTZwo$CW70Phb#~u=59IBQ@ei#ln$JXR@ro?d~>lz7x
zLhAuRN$*R!N~M3(pA+l>m#@EtOyP{o;sRal?}puKb-PB+pobS>Qk{K%^AE*ZhVxOS
z4@ef+3eP?OEhOyYr<Q7lQE{<}E3r?03nD=Bn>Hja&?0sxYwHI0%fUd8x@H*C`gCK|
z6tocrT==1+&IEpz#(#av&lR$elbH5Ro5r1$?XN>tqn52KBvf$1wA|`zzo8gn3zKCF
z+IGSdrK~L{gIwYVjGu>_;W%%XYusaOOW`8*)m6Vu^D5$-%n$ETfIC1KxLs-?yuL9~
z?X;PP;Xhw--_6l}xmWc9{b&{cW!-t9cy75WK*!hH&%)}qu6>S!O2@b|FNCKwYx5>&
zVK18=u$K&}{8+S!0-6#ug==8_;1BRd2@j*sAD*e3k0jBaMDxvic}w^H7H@3{dFN~B
zzVB2QDa-Tr8>uFHfF2LJ^<QqHXBisF?R<{eqjk{z@hfv?wZceY+=D2K5}eb7Pan7w
zbVwhgEo&q)pC!(=<-!JPjkFrHJ+XS0rWxedoHw<1OIGbF&(x%aLC$M?$ArDAsUIyi
z4Yh;jZd_tkgV4yo!H@wKl&vH1bU&wxKn(+fV!M|fTQOhrz$w3l4d=7nedx7`ft$q<
zO;8}1@>3{?j;V*gfwFsOemZm?)?ZpGLp1lhs;nrPt#o_3+T1zSlj6X%=J9wKNff*j
zW>TV=l3az-RN_tVthtClus`!^CLrH>4>;k-@cC0;DI0CIm=Eo6nk;YXjF)D?GFSpD
zBHHIE%;B73spZI;6Ioo>TjbyS8}aaT?kd#&X4C`s`e$fXcyi^7b-2~SOBMI({VFCG
zYKij_Ql!@&KDg7v307M-z-D><7G72UF;rQsKk*DYY?;^w-*FC<t0>sIAIkD$`_?xt
z?zy<{f~3p0_Txp^v(}gWcH(I!epyIX;i|BK_)sloulpcr_?Vg4nNkUT4fXnb0xw8b
zN{kHCvUmUj$+mEUvVBoe4ioYH?o;+L$<trVEJ{)>B|AY;-gB;zM9~z~Ew9`UQObEM
zR-V%}u&4BVRBDUS6>_r;GBJ(G!LwNK8VgTzF2zce5tz-ghDRD5VfR8F!i5@gssrsk
zYHb@fQX+DGIWYTKIWXpLQP!h((^mCRt(ysP8iZrd&ZhFoz7kc(e$J!2$O<{2l_ZNi
zoSN%3eo2?c$|9rus!NP_#nRcdc~K!SdFEan<1dpA9jWPn?|kAYQy5toX%-y#NqY%a
zmK0F@)5w^I#1??nr#VefeZaU{d|?cHaMw1q3i!ZLAy!xLn)Y+qX`}lQ(uQ8Ch+9j~
zIWx3n206G2;|0^4P%Dgn=op`N6y*6C`|k8r=6>rC`kn_jTR4d1AO`&DScMnDy(jYH
z3AyKQv$(Sh3$@N-|0thdf)q+rgo_}dn7hS!q{z*X3Jum@;6W96h{`@YnL;FEqz^Jl
zE1RHY+pLdq(JZu=+D69o=&W?IW89zQTUi`Ee9D{)M^wnIS`lx;o-Ka90+d^H&e*5z
z_h2PemcsYXu;>L{{Hs6Q=J8&`LkKu=2TyvwS7btQg`vLwSxd@U8he#Rl7!i5lv_~J
zn2M;*__{1XGW4g14qm=`@RU(ST_;kQ@GacE(d9jRBi-<8OG3fm4@NU9l=X>ZbcR<o
zq&2?;WFR|TB@lDG$<l8{CBsTAz7fI9OP(npbXmsUBGV6*c7n%5pDL`$HPO?Hn}3hR
zdBrULZQv!h68`czh3HdBVe6502wRaTC?k*SCnJ=Hk3piOJ2U|kv41pVij%FZ=iZv;
zswQYvi*(&cOF>%IuC|`9{_>V!l17O9+>o|~DPO<Cx-qJ*(Um{F-j2Ut$?U_q`QJ?N
z>NyCW`n4CIi;!?VWWc(SF2<!gb)Jy^RVW2c^ypy{W_<4;XY!GP<)k3~DB%Ig{CSaa
z4k72stvZ)0RFdE<5{tmsP2LxlYuDF-Q|IyfCLX%tr7KqN(QaSkYdRyUmoU^B>G%ci
z`6U>#iO_Nr+Q+#qc(oqY86ZlSguO%p!bniW>bR%#-LKF=X`_gW%22b9t7wEcPwu;8
z6AMZaNN8aChdWfJNqZ43TV@-SB_DYoZyVJ2PnMIgH;i}a^zGuQuhLv+&4MXI&?Yf(
zr*e<HX2=SFn`c4;Qn@L-B=B?#W+PVxSlv1VbTxlv%}k<bZ7>sGyJ8>bK^KCd%_K_P
zq~z0zN=%6-uCQ!=f@zCyguipqYw(PKzeP9R#ev&m&4hIdH*<u+v2baR2wr}X&mxJ-
zY_U&3m^JFV3wmABP7|un3}Mg6KBB>5)J__Wp}M7kj~O&tqClg(;&6AXap6oad4w?b
zxyk6_gvA%jeavbKAbgu1iRX61?;j^u2rlo}oeS{cDdi^csOJCX3=GG;&`y;nS49?}
zhCt@sPcf>;jWLjSS!gRIimYxz2TcuS#Kq>+tgWp0@L(0lz*lnji)exZg(+Qoc`ucP
zuA^FdJ9-r&Fg~W*+PF{XR?u*dx-F23vnhs%PPpk<cF}_M#&cwpOJ~|jbbnb;gE_H&
z(8HpdVO65tt4(*WlK5XYFXY4J=+_@#!`Me~{xr&`*Z~yd&pi65`?qK{8m5+<+a`KG
zNO(N!tx};Ne?3_zL{4uT5;0;z=t+yEjYdmVZ{GMTr-7G@E?9S|xeGHG6zRfL;mNJ)
z4xTHNXI*0MCTHnW9jX&`B&4TXkQ4@24Bz9dbmpgA>`->t5_YpJ|0-xn(T<gl1a6d2
z7T<lu!UZy6?M&m)w%8RiJuH_K>!8QCPE7Ni1ma<Q1=bI)W?@);TCq1a28J0aYc|_@
zHG@wzWamt#Y%n;ncsk2at1-#zTAq9G69=gkD!MsIY-9(ymj;tm`a-Wrl_f?xpe0RY
z3PzCC><-}HIiVoxT9Y^|Uykr~q>~`(s86)7FW~Wp+TKH#i41POVsKy$ne~w7)H9(X
zitoGE{gJCB7ft?C9S{AkMeW+i1iwe+Q~*wG-yowm3^o={&O~OZK6|vP*Dg~d928Tj
z{00h9R)?;Uy$#&x@D48@a65QcwOB0<NBO3rni4}ka9NizOyT7hMIjmn(!znk(nQ1|
zKcfdag`&qFZ7aU2SLo803R-K@9Wzxvnuy3<X^P>+I$S2zmw;7&M#n5rH&VTqEsWCW
zjAcgJhdntTU}fQJUsK>~)mAuis;QPs0JB#3lXCT_B+E7UCzI7*R`w%(7z>rINOqK5
z&imWXx8?Tv{8Af|)+(=p>65YCYTS6TupQFJnn@((p868VSB&vXAvix<{SGlZos1%?
zGEoS~w%yEv!3EH`git@}L-8!`Z6_bx^R#7^p{KpDtt5KP1{04Qkp9ic%KG((@vYj(
zZaF9m<8K!kX(C<vd`VE@y)&jgz77pPV(GlP-u<-oL`=D?p}a$u)Kc)eUBeFM;;`4y
zHL%Gv?`5+k+;SzZ#T-(f$4kt9`T3L@UZ}pbA|egMMDN_67If_uV<SEzv&Hp{Nqt(X
zjn+kj)3&<BqeW*;*shOz9x`PLE~Qgu?xK5zT+K?{l%R%7-|Vp+4rfsqXl~A)23OXK
zQsalaXw(QKRjmdTeNFmsjAyXV9u$Hbmxs&4@U#`{8zdNNo+L({=#r~9nop`{s5@ZF
zE%XBhK)6YI<GJ=U{?3n$OZm}xg0Sf|*8`5H>xx%j{Q0;~D*unvvH1wKmh_|59guz7
z;P5>e<xiK!yN2n<@dagiDos?)ATjAw0(kmi^KH+4ar@igieqROZeaiET@$?*DE{}(
z$a<-FzJaqFYG0>c>;%NEC8NsOGDlsU7U*yb8FK~W*|nqew{FfrcS7!<NbnwE2a?nb
zbq5KMFJfB<JrVTC66>W@>+KzGEc+8<Gcfj8+DYl*Qs6a7<UjN)SdDz+7#$7<KxeE$
z%eZ!5F`H8PfAnqr{r&b;=BL_ed+sxzj|3JrJq-!W+__jU9h#NP#hW(C48S5YL>0o4
z21|uC_J6ALpOfLphI3#Xa`ggKFV&}s#f-<+5B^vd)HK=`9XWoSTV2Hrw_6kghkml6
zqS)7-MOnyU!M~NV)y9n5im51sp4avd<#Cl>ryLuHWAAG1@X2m5<c&Te7_Vg$@>MD;
zMt5W%!!!w{Ss!+58yA?6<u>{P@Jhu3pqVD*XwkRw;a|Q%D^t@ET%<S$jb5Vcv6mv?
zvZYkdenr~LHNI1;m+|kx)r#Ny7E}4Saw1=?JV>E)Bw2gE??)UMwV!|2!d^<$T}5<p
z+^lblS)q__twh2Na5FaA&gbByzg$!YUtz`HSFy&Ac<dIVc6`sx)t~rxFVxB;xV%VZ
z8{|eh%ES+<I=Acd=Zv?JKQ;{Z@j2GVR=}KH)gG>kTc>TvK1JW>YuTEzRR6}T7?}qn
zN-x+bbgCjhI(Hz7AT@~u^b?wfF1QJfiH)G&_UCb35cLuH<crg_C5Tyc6HN7#UXx$S
znk=UifUY*C`)EfiU9$b@Jxs^Hk(P*U?!dpt?Q58Z0iG*g0~n@ZxVCn6WRF(8byDXX
zO7vZ|i;h1z#)VM7EfK??rYS<NQaq|n%s&VWzV5O2q9Re~2dKe5EUpDp)Xxhp8JB9a
zs~B-L(rQ~<?7Z?^z|P6qHK=IvAKFRdzFeL5o(d_&uYq;vr-!OllE*GnC>X`?Go^eL
zWB$!l)Z#F$t`r-5U0foRcm8EbkKXCbEUM*b8>dpJSV7N2{^1;LyQ!)B5^twk-EIQs
z)?=DOJek03teF5feN~^Q@bjZ`6347EdbU@BeZggJ8WP}U`S)3PhgEC!yaQ^uS1A>f
zusH08#Qu!S`F5#a^sz$=+1EyrsaY_Lsw72>21uOy70|t~Wywr`QwBybj<cbcC^I$9
zB=(|4?S6`*Wi~#xLm&PiF%(5|2hF0Tg;>nz$K|7}RI?sBn4GE|D^xZcf)BPnqq?r{
z*JY`dQA&$8_3o{ie?L`p5%<YA6pQ6UV@^Q!;4wYzP!SEaB)u}WYw0fzXV*oUQUtd8
zJGuoJl;A-&b!<Jk|68sb0;LUIO!cxg0BCC2Gz@;mStO3lU2OUFY}LoPBiZ&swg+On
zG`O27P*I#d$uoL%G+mw9tSc6359-xqF3KfTk)r=XiWAAVN<gUmEpp>Mug^R)-EQEi
z%_rN_7>fqUp`si`5XazvaPO$;X;M!_uxLiUj1FL?gwh=5q5RI?{vi}AL)^k55B4+D
z$+kah>nyDA@%6wv18Xq^D0#aCWSSfPW;3Tj(73&ECB#O}?kLamt3u2CPq+J4TEV4D
zGVCPRRx?FLk^GZ3>7D-KIkLTjrgC0R1_~%nkH*y8!_4`-a%CSD{>aLPQ?2<PM{9`B
zJ_80iN6g`=oib8wjfAHHi1$ZLikT}I#*!Vh8~())Mu|IVFXTQ$#%0I2mtpr`H`V8$
zD3M1eY4TS#4Ezp;op75US@Y(0u=JO7v^GDiMDj59bqbfDy264nStg=3vwKg@E8)FP
zh-fCIeI<~|rn7kYgCF`$DuvWN8eEPYBVr72QAi8mN)d{H$Xp3p1siDM)FLP!DqzS?
z?6P>q6B7wFxYfZ6qSk<jVP4G#;-Zn@I-;43Ow4b+!yJgBki?7t_h@7{8e$v=XLXRg
z7@s^}&U$BGp|BMrc;y}2u+0L}3-pOkGXL<UPWHDpoPFZaJ*hF4r~NJX_h;2Gu#wk>
z_6m^{Lfqtle8$VWg}+!?wdp9q8+>5gx3qYjpg2hA){B?P18D-L?+{5eUM&J)nLPw;
zN-#j>Cht`^^zeYQYp3Yse3tiEKYsQC_NP>Qutzwl5#TAN(eMm*ScvTe>@!Kd=m|3(
zCE14H{+^BBaosqSmD?Bp2WRgTq*)lPX{IVOD{b4hos~8#ZQHhO`%l}pZQHiZsXjfY
z<D8i8n2EXD-_4G1-|bk>dft~6#QJe>l8wt3r<b3&UIq1^Cc9Z8WT2tcRWq8tE%H)!
zbrjdoP`Bn<bcel3zaJHfa|KB<9w*zL5H}g4n|RJibr-PQLFMWp+=N=8SY0j{ruQo(
zwf9k@s<Dh#1pJE-fS-xX{O!m(hE$$g52&N}MuW<MQ<XY#QDLsB#lcxf=13;;7V&?7
z)usQq#Hvj=zhCSaZufj%b$nBqA}Yy@vIAXflrBlgwU>h1Op(h~#$7_rK|a3SIi3%c
ziC@uNV<Ny3!L=khMN{UOQABuoyij6u8dcxZJ#!aQ@gu`*GV^`_n@hR!XyD;&d^GZE
zUq@3vj|4x#-`JzgSS64=UFB7bVuLaE#uhiT)FP(HPwj?QB8&Q7?@owlg;TM(W1_DO
zuPWidOO@#8DCesL4W$@}Fj5qTt%tRxmCZ`c*P5t60U74tm56-#yu-AkEeEfvmXkiL
zSjK8<MnSlY;()F?5v9r)m7W5<UzXGA-Z+W-=;#;o`@(}XSxlgbAm`IqJyNzVk_LJi
zI{A!{acHf_KhX5ZY-hQ&(>lloqpxkQe|pH(L>leN6&Q!LY*EhgJ}CpMhs-HPqD%yv
z-c{mDjT*vzt;Jhp`dc<^0Z8Sb*1%F%+)i?Y!)dX<ne&p$mjl-L8`oKHiHK=u`{Nc*
zV=+6_Ou~Mn{koeoYV?)q;Nnr`!Chb}hBj{;I2ibPbW?`$+TljjwW$8LP&Q_K*r}wF
zKxPTApZ9*KQ@N}KXLE5!L>~KE%(IsJfAnC&fB>-YDxe;~J8Fp%YYV`K#!sJiy71~2
z`5yojxlX!zDMQ%l$S|616)SBhma6h2<#~10(__sw_6v4i&3!*gjmPzldcp^nDE59h
zNjMTAf6x+h@r(I1Do(P5xwN0nN8%j^PReGo&HpgcaalCS$3**W%%y{gMVEa$58Wwt
zCv*Ce3~Q^8SytfUqT^aRmA8C~IJ5uBW#uWEatkh-B;hFA`=~XY?wv#z9TapacqQc0
zB~JcZ2(@JOxP7E!_@TM;8I_jA-2IJ{HC?7M-A9xqd9UQ9`ir`8wy1{!ChHlD7Uhu`
z4gfY0K!@Mdn=KGe7hsCHpeSDS>W-f8ERn;#7GU!vhEVh(qqFc~WE3xsh)p;QeSOB)
zoH;o&EbS56q2~|6>=e727>|Cr`o#-NtLTanF@)jnNc89u)OQax$n|i=*9a<h4fB=M
z^2zoG@Ck(}V=Vz_-mxWW<%(PgKv~|p-om0bh0V990t7(=3N5Lji5Bgr;+38KoNgP0
zdk>s6Q6k4YY%H8V$drM$XFO@fs%_rA9_W*{Owuvh@ik$xUoH2b+JSR}Jp9ZhFxun9
zqWtZ2l`$Obz6mvDwG3N5s@%W2{P`Md<l9-7ItM&2Kj|bDDjF8Ah@;ba`KYRk|7;Yy
zy%BvXoUAz{u-UzPUKgQas)=m>NhJ_`RZ|4f?5<7T!`V`^Ci<Om%VN~RDKVwQ-P5sV
z@uUt`joRP9#nJDyWjnX}N+#&^V6mH!wxkp=Zc{#4aK=k_<331uN$Y>>e`pIrxi$E4
z2Wt>&zL=sIL)CF*$|_@%+Qh_!lGK#8n(`K3eQAYZ6jD!GJnKNEa9|9+&5XwZOP_uI
zf$dv|C^MFU9y82wdU7qZW9ViOcqad+m~0oP<4(EUmd`2CRq!;BZ*!ReO@f+D%+njj
znxjfx&nQ7#`#`$>_NvqbrM=iEscExTI+O!0hmunH{ym=AyPfOM@{%Mi5`&f@_6LVX
z_Nl+9aAl`=$g09NQ1tkKJ)=)qQ1z%yB~gpKo2_X<m1sGNTB_ID5;Q0>L}CQ`^|+BL
z<O~}q$Dk;A0fTvk^4`Pov|;uf*1aJtm5mVxP=*`VyRQ-D&XcG>h>1F#%-@5wvM(&|
z>gEuaemq1Bx<oq-Y9cByaPKuFLiQN}HL0HnWk|L9LY>h{kuAYIa?;-k;(2RTC<F-%
zzsj`th1+0!J+HI~tqtz=e3@`R2NyJWsLrMY%qU~}^?W#^OTx)td<H1Oc11~CJ`#7%
zK1frZf=G*73khy!8m9X}5&rH>CA#+~|8kIb0G`6I4TRkje*yIzjQtvlT_>c!o>qgG
zv6mtc?QZe=jq@#Lk#hvweR}zahF?PvPdK3x6{LTx{MwVYLD0jQ?#+BZzcw7bh?EPd
zMzzmnb@HY)$?aNI%&}c|aajBhz2m&s2B!pp_OS32>%fBMuv7!b^54<3@bd{??ey-=
z6cQ_2VdKMC_HH4#fQwZ-9lEu-n=SyC#oE!3mrPg2C=dOLgDdwqj}YnKOoyH9QS#&D
zXrr9z1aJg4LXN)e9%vmen-^`+>)i9Uqw{kc$j7`E{n`&|nZ3m@2PdXMN6p!DYtn!;
zp&!=~@@xZI;LPi{6Nq?yZo7!vslOj{QTZ9&GZJ`d{x@UC&rG14Q_Q9hM*`n!up^aH
zjhB@#tNh+dH6$%9A97wI&lC|7N3(B;SS>c;!en_3vpmsRtSLExgQN@PUkjhSKYW!p
z?v43ohJVS;DmZmKzVoFn!97v0iEf6`r<8aQw~or^F0hnR@cs@#tYwFt)2&J&cDBV=
zHN8zE=Jv~qldRI%yGhe`(b1_jSGKpv3qH6M>pn9^`YWDtUz6o2>AngMk+p*~CaL7U
z3no%HyrtyxE^dQimRA7+c=$<93WIM&eZ~h#5r#gh{Qg*}9?n^D>f&o^5xz#}$(H$Y
z&o}@9a>-PE09ax|<rc~Z>cB5a!L&WPUvV;KQy}_o=zq^moge5HhRtTfUlZ568+y$v
zfp7&SgEWoa3Sa!3-@i^cyF^mSp``9oTDN(dQwvq4_azRpMEY}UlO);lFsHF6CYAif
zU&Ny>V;pui7#ceY*MC)-2ABNnQM1udYoY2(Jn;!%kbx^ay_cmP5WEAHSd;cz2iCPq
zc~*)^t}@C!a+~FV{Nxuli_HArc@dodW!L`y&5KYJl>ejnKdVY5{!dy2C;NYD5$ylU
zN?AGo|7sDmjQ{rgm7I*NRR}o#85nBf{~<CEu&}WFcQ4|9S`iZTlK&4M;_v@$K^WRv
z+1k(>+FD!d|4$>r&Cp8U`hOObn$nxmo6}p;ThUw7+x+t$?C9<E9gJ=Mqu}&Eo*e!|
zpRlKQpm+SIH2g1-;lCRU|HD`KZwAA^&QJydTLTLM9-jY{6c`B@S(yF_9{*W6%Erj@
zzfS*EU|?rtWcm+(f#9EZVs7PR>_9*-YNhXFENpCOYh(<?#|P!;<Y26C4du2T?Fzw?
zb+z0~(N`+y#K?s>c}klv94(8$Isn5w4rAyfm_(s~Buqb7@PeitCL6YG`6mc6KhidV
z{f6^|_orw1rApoMc+FGS^X8-dr3ug!5-GDHAOIXuA5xYZhXGtmAdXd9Xsnk@_Y2Hl
z7)+2r9|%2_5aa;zV*`GdDIiZ9k)rU?mmq1D0TTS81HMnsUxR`~eqag_5)C+DsHhKV
zo&Z4}11#bFU$Kf5=#9`OqI_-&fjnq%P#Z{VlKixeSr9Hx;MKzmaokD>JY-5jLhA!J
zZs{(V5U~}3crF?kXeg&X_qqU%KR5+87|g*_EK+B^9n7%0hWO~ffDpp|AywF)jj_8e
zU=IQYyIj6W)DV|YLH`~K&?Z9JTK+{aU@ZY5j{c__Kj3k+17N`cKebTee((^dq2gA;
zJO&no4P3L_>fej@A%s65mfycxK|dWi1!@Uib&tM{eyD;3zGXvJS97zn2*v5a=HbjD
z?Sg=vm05l<=A>~8Kq9?EL)tru=syr%BLoo-VnM%pJ3-2e2*N}5F?|lqEP+Sc4YxwI
z6XE#UqrCqG_naCRFGU(3`yDda3gVM}!v_M}(6`-@It$q2B2d5+2lUWtf+1OaM1n6)
zsh<)e?d^jr_545tIs<=SH3aJW84w_dBXPt1Tf{#$2StBh>YbgyeCCGyJ{op#+dqhM
z0ADvS@692w(qF~*i=`gK1s}}a(v$Q0X1<k!1_B4ogT#iJ#WD#M0QuI&y$D(RHa0wo
z6?Frd5>R{d_s`V@`0}djp@NGL;&y$*{_q+QdJAnqcu;-%Qv6VtmWJJfy+evi0RIXa
z5)fd(Ac0CkfB*)3Hca=}?Aku}bhRT40v7zgwH0v9BS9p;L_j<}eUa^d3ShSREe9Yz
zO(CpNRAv|mZhDSv;gCSr^m=-KPDo#~?|&A)Yp8xIK7T%hscd3mdJZ~$fxiJ9gTMDq
zpCJ5~8nD4Tur(+yLkNDbEnq$@^P2SJgEAL<#g-TmE@LPNu4=i^fuMhmgj>rAc=YA9
z!G^)VQ{%pKz?%JuL4GfYv=RAqIn`1U%=!-1dYT^idDpcXoIG)Z4OB1jXHxx3?VQ#9
zAg3XN3(B2Cpa*jKRFi@^1s$fNLOFiZ+z}uYB!LQ)0XF!}2fUs?>!+(U1oev|==12q
zFu?y$=?}w@|DUv-WB&H_*1WmZs?RFe%lPkR-yRS>2Dvm?Jrg8jVq_~_)J-qNyxv2?
zT1FbFTiUZhVKqIDl?2FzDjt(2B+IxQwvSwK<6`Ja1M{8Ys#AxAIu7UE2AwI515yu?
z-tv0$5sNQ_`}5Ovx2zdP<_{7PP}vmwfPLCC5GVE+x(UZ|mwiw94jWK9DBYZL*9CjL
zO%_{a5|%$1O`03j+lA)yOj4NM+DjuW8ZTHjVYu@TM%7$HU{xX+q2v}Nwfu3`yuSuL
zl9cFP9$%(+P7Ti7zLf`f?F?fdFpop(rRg8M75W_t-mLB;CqZ7WP^j=N|606Ro4MXK
zT0DvfN}Zn=I5lpQ7WB=`2CYQ>R_P`1xaJbWE$_f@iRY*Am|5K3GaXIfjrjmEB>fa9
z727i;q8S$bHi@0*+y~uBbNsv;>D#y~lB~N$;R#jasi%xFNL#{AS#ar2`^)NeAl1T>
zJx7B(nfppfiRf1PMow%!aMc&5L3~n@vZy<EFj>cUi00Gu5#p|Gi^cC4)=Zxm8gnO;
z9W3T;NtPEI24DJn$s$ra*Q#J0?L+x59Y?6=$Wuh~%9A)bYjWPsf+<$by83hxe|BE7
z(iWo{YT$#SkrCEh#euZd=w*ffjW?M<weh=z)*4SM2H77X-kZE8j9J=L!?CJSAmT~#
zMki`-|BF?L2LfO*ar4!i68O}%!F#!Uwq0fR4@sPSdf>9}qtL}Nx15x^zj{qjRp#5<
z1jUcdu9X_|f`*Fd+3my5p{fygI2<lo9(B-+ayTwyH7yz10qHh$a+`txSzlcm4cw<r
zJW{qS<)e>sF@kb<Kmkbi+pym&o%t!b*LG}_ZV1!?itZJU9*#IdB7V;{d4=yqt#NOe
zXN;r%Mq3?U96Hm*=6K-8p1<D%76SW;`?!+MIeY60=!%YZO>WVY_&*V0t@<|NIq+d0
zLW9@tv&muUI4~*s>wroQ!PGXI+y6I%x5%n(P-(Tlch3BGiuy`)T(fIEn3wA&j}i&P
zav2XbdCSU%?UZ%8qnjgMV`yQ~^rf2y_}W^1N33?L%(<*vL!IPX^?BKuihI5?nW0O#
zO|hfWM$-UmFpl%%k>^j<;U1ot;KAM9t_3r-R=7V!b>qL*w_SGwoTNVN&r*{WDSF{P
zE`goOg#5+Gd0oPlmD;Vmw6K!KlmqwWrLacB9OfSFqeeF8TwgynoqOiq)H$=Ul5*Op
zV;XE0kd3vxS8do<aFYl(iw9ofP-HG)kBN_)(RE*{yF#t;`S;<6kdxCJUC9GlVf!=2
zSZv=?g0YR1=6aTQm+6BnH{QLgJLSJ6U!NwS`TQQaL#=$OBG;z_v!PWNT8ybT7~*zD
z$TASW=>i@M&yA%JCLD;zWJq-4k7Mmjr+?`R1yAtwOTNVt+oCJ&Kr--OZZ?5<(a*XU
zKg{=6N~1I;sE^6@nyj(NZYb3=2u<!&PA7=C;FH_5)oLfIDVdAVZXRxTEq_^)R(l?z
zG>9AakD_(p-#zAVkY~y@M~^VjkfVJhqDMpMy^zl3^5MBgb8N!r5@mOk(cD~!Fo|=~
z9~y>dE-NexPc0=y8!b;hQF5F!k@0!^8BT{R+C?fCjM#*r5(N~l7_4_ws%1NI56o)j
zJ1?f>(;O9{s`$*E94s`C-M?A-?1!*l?uMYxD{0TQ;GnVv@X|^dlI?^X-4iTNXKK(%
z5tb0f`FL7K<IcCd?+&(#%!ez$P1`hx87VOFt~Xy?BP+P&Nf2lF3x8Pru?pQ~>jX&X
zxPk`i092Si;;zh<>fA+zfuaPQJ=SG}{hRiFqiHZ86^+M>Ak!{p`RyLtByGU!7Y_2N
z<}BVaBT?F<?7gr!tvB9Y7oSlQNpSlQ0_R0ek!oCQ@M(3m43`z`x)<QF%Gzl-Ow`6s
zZ*@9l$CF!41paGC)eib=D7=5FjW+)qS-9gFMP-MEtzL7n$#OzdWu9E_EzP%Ym%?@W
zvcz>S0Xh!t-G|Wg7SBg`2>%Z%L$zv7siRgARfUJtRzh<C<!u?HLI%f0-jbIx_yBy&
zeQ%RQCko%Mv(l<fFdTV~U*V{k{BZ&8aooC>j=%Z4<KL8T%l}R)uJM}eP00^k@}c<$
z^e=PZm>sWPcQ!56B5mqMSE*M;zDmw(Hl~c!-arkMHZeA_L}rc)o}_Ku$JaSFg#qhU
z=-fZ4Rfihc=d|$qsHSy|t#wirCP0Yd`w?6i2H%$@ZyQQbSymU5qCVMHjBM?Pper}X
zQM81l*2W!ZJN)e`roEkao-%u^#M<ZRChh-&gN}++WZGkFh{nScR+^*QN%^$DSE{N(
zQNC}!{L~86u{Hwt34{ODm>fO8Nz@Xu7{ls?ldV@tok>TfuCkjer63xvRNf(KYQMIH
zs9=1e)&J#W+~+xz{a_oI_Y%wd1vo2Rc(U)#%eqXSKgXZ^4Ts@3In5SC%_nM)B26bm
zG9#p_y>rfJEwjR9)S>RdhOOI#PDDLS`!}=Rr`kU~YN3Diw30#074I!NesGy(!i?Nx
zl5SMEgoT}T`x7y4a^$VnSM~I@T|dumX0Ufaj5SyOSrFdYb3`8i`%E0D%q-#d=iacX
zaefF%Ks4~Qk4sU~ur`%~SH#JjBA}*#OMOr{x2Tw1lh{4qe)Lam=fqJ$kJFY+x#z)>
z<!gV?Mh^2AdgzTPhjV=PeD&cm4pqaq6lb|u&aWc40YNMXTsbW>l^XNEx2Hcm+Lkrt
z6(H^tDceXck7k!;4)&aejqvB=46tI4<R!j}PgPR1^t56fBbX&S;7hNE;v7Y7&)o*t
z62mkP{BO!xybsF^&nYY&Pp-s**(VMiRQY!=SdIWyWLu|St4OFZX@!<KSrQvhK*z)x
zOoDT+sV+msp{28~W142^?kV64y6B}_mk*<5&o#`;cU7&T#p%s1u{=b<k#CEh#;<u<
zdPUew`)-)YLLM1hC5EM(zK#PT7+IU3Yen3e9X>-sUDJu8tJNmT6?M5dO4n&FoGujC
zOXLS+YgwJ9+gK!T0mq~flXK&EFGOZB#a;2H_3=~E<R8Y>>*PjrOVKIB*1EAR$w_~^
z_~CSq#=t~mudP{3?R%lwo0rDjr4y1DZ!xOz#jXJKSok^6RVZQsRtToJh$_cSwCgo4
zOhHZ`S@arivS`u@c=r`cT=Qa_Bnd$C+9WbF^9@D?YBJ=((Z%kAVnN8sy}zr0PW#>@
z!bFPic+JSoSOfujVG=?^#)U&6l6?j)PF+MRjro3dMXF9FQWVmkIbG_R<@&G<;i)v^
zrKoJ-4fb=?WGu^$!rtr58bgFBe5pJXjAbbJgBARH)aijzE6-!0)6OJ3v~=W<JJYT5
zdP9+l{o_(us)3K6`Fc%uhO3Jm6io0<aZ4R({b^k6{^*l7<(+V$iV^BKwKW#6HAeBp
zeSF<mVOr%_+qx)kRzE%;+>-mUj4Kh+wM#?E;-q;}J`Uxb)wA-h3JAZA-u^uJ8PKtO
z5UeGwMC9kX%mj5FY2|;6!Q<Q}xpCMnjpqKmQ~r7?M8c-rDPXnCuMd;7AIff2kou}w
zaJw>&X+9q-ZfIDJPzH4V-Jt6+=VI;nID?pZ=)`bCZ?`vxmU-1cV>|p1nKw!C>wdIr
z-j*(ib8*VjC4}}VFf6f&Z|)iDucG(zu}WuL0CN0HXlJDCpl@Q{ShP7diV$klEmtCx
zl2R<Zu+Hic4K94wzlq1sIaw_B`bDk)RrtNc=Rb+J8!|OC!OpE+j|YTKo;d!vaYxU6
zZ(BMj&z%gT*FF4J#zt|B{zNM0I>KhNf7>GCaamSh`FLAlLJpF`N&&*2(i)_~JOFE(
zdJx4uiq8kI+*iKW@U22x#+v3py6C0x`WQC~rafgmm{4f8gt41G7ul*@3JR-60)xMR
znrAS;Rkf;q+8U5gtpm0AO`T3}AR6ch6{rL%d~E-orLX#=8OVOfN*$?Kk|NndPB+7!
zOa7BKzb$9FTogbB0lJ#9Pj5JOXDCl^!UNZX3-XLPF3n#Ub)}dxnPBxhuB!Bgi**f}
zG+QGC#OK^ny;9A(b)m8L%1ombh7}}2$Ca;gTIa4}ew(t3{QY^6^vqvlD$PB~Ie@AC
zApE{t&%(ENdIjm_X6!k9qRXL6jwk7~Co-##{7=l)HZNQdjja-qTzAm*#JZ;R!P+wu
zzf<NNDW#FGiwGq^WKC`qt+H>eERDmnhAD&9g$oAPRJ{dP!^>1V*Pq>(@SY*JhChUi
z*O6jZSRiz1@ol(lQBvd^nKR)gwG0c<3r<Ud*hh5wXiaB_p*XNNb-j3Uf}SVkplwgY
z4Cq?kHq(=><Gh)3kvEi+`jw$RjaWV^uQAG?aeDDr$N=k(hHRtIbd6PEsoeAcwC*J2
zxxfQfJ6u%B2x^UzOMPOcW3c!U<WOGF*osk!W#T<XqTo6-OG62%M?>tE<=OEsD_0pK
zfREdHUtc#)74Kp4%#412-0kW0M^I{7D4s=n>R4Xm1ecDx%J~L5r`HBJSP8&^RpjXo
z?e(Ob!{v`v+2UBFWw*C>m;E64%TlQh^{UBHttmhM;BC)q9m#T?<sslNicq1GjxGNV
zyO&#d>QBA?#7^`i#EGF=hfjL)EXvD=NkikPaeB&g=Rsg<RNu5ru*RfOWo?F54O5`W
zpsBm_J*X{H>u!4gnCKh#Tf*~zmc+^dCdEZ+i4|-=O%y){)r82UC15^g?R3=)#aGv?
z?QXvnDon^td0cqvU&`Y`MM5OE@A_^rwX#mNfHXs|t?`eH9zi8V<=Gp!&#>6c`)^<i
zaFdQ1Wl_HEHbr&%+sNFyP12Sb@=1HRoJHAac`G={b|z3~_MoHNOgV3?;8;ZE&hqeY
z+7R--n)yOb98~c!TV(D=wkE)?d@qMmA3&zzI5={}yNNj*{rMBbh$+9aEGYebs9NW7
z59yo512tdPc&km_p<|%Ep`L%=`YO|W@{7LX&OL;EcjY>guZ5Ph3X(7yzVTuCAZH*_
ztTYsRU?;TqVMOIws<15C7QY6i(v<JgW7!jX-ogIMVZ(9FVu8~4Lq{PyG|@kvB_GJu
z4N?o?wJ9Fl0Yl?aKM_M<d(zzLiOxUw)_56cTKHnjs)>?xz-=o50WL#_K(5a(KZjq3
z&}wG`c^|)iZzYd*y^cc6nWqQIPXqU8iq%9(Jv+`>dv`*@I5N91wCeAAdTzbz&B*x9
zO;<;W@DSe{<76f#+aKE8DfHj?7|zhT7Z(^R<hpKzIaY(LUf*p4AP-K1Ies(DA0Zvw
zZ@&(kn&`{=+un--6Q%;iRh6QL(9VS)#24jl4sK$<ZK^Jf&GU?R5Tu?#=qxm4Y;&8M
z+Q5OBO71rHJ~=U|r>TY5{Q}V{kVNG%FyUCsEBTW2M#<8|dGfNEbE%ABPK~@O@<KBl
zVDhtovm%hK$8uGkn>)&*Bk^BjkT)qaBGJOyEG%BCVl+QJut$!0ShUUI<8ygqz$Ug}
z{=&y@4@J8A4^ugyV35EDr3b~s6h?=9g&`+R=LPykLO4mBjK3G!mDKJqCDrh5+NlEK
zDtLy?kLfkz!+d!xh%_Lyh~@E!NCfN|L=ILVAn*t3;cJxjzq|C0U{D$$oqbI4ek-_o
zP^uTt3&{)1Pa*d~Ii@ah+pn`nW!|_A{GwC>p@fP!yP=_0?-;+bl=PtUXgD9Ps-xIu
zUBRfy!WxyFDE-8^!v(dXyPH{Fn@Ii;h^IgIVsVkvlt1aCetIbP$E#NMI2hP&2nnQj
zMb#jhFI-r7mrNHsC+JZ8riMT5dR3g3*40YG_T#JY1`|<FXwcG};<l{9DkF|<S~O70
zqF_+T$#7nk0q3oL8SR@(ohIdzrN^(yn2YauPS)XIVR}q!%IP?X2fTKSgf%*3fKE?B
z59Rm`A~;iGFAvucb}<{(=m5GAKOdi@wr9d<kFb5f@+89yr89|uOejv=uH>_5vZ>5v
zq%UDk)}BwykhTw|%h7^A0~^wX+;txb{n{G7sFgJ0#wUaBp?z{qPn4o+<Q&HChq#T!
zMZEe=8lFdnW@j3&2aEwu<fgeWL~V!WLY$fuTTjxyzk_DQ)0NuN-u0RUjsp?t$(m->
z#`ITI-AU8~c1jHlF(bc9bGR8H{^CX7hx-e{i~@F|woPoK*;8EySzKEuoOgO431k_V
z$*7DE&WYK(B&Pl_35f0Wc&+KnO^G{&1eV23(p~tfXeHxg%Zcl%5cU%Mn`kww`4-X{
z&JHo=%{dv#hcEXOlF?)gg=$j8r>I9|*f=>cQrPOLzk)rL`qz+qM1ON8$cLf}ZvRbi
zaXT<>OewyBcxU&Dm=%3vLSrsqjHR6_>Q^tH<6a!ic#*HCFOIaE2+l>caPJA(*m^E!
z#3c05m=z=Yozj*_rW66S6K^#2XyXC#Z-pwGWn19JHuGtw$Hq02d|48&vs)1k-&q}6
zawnAQ0<>F;yreKd=}OssZ);OSv%W;<(#yL1`81$gV06hPOSpGJcUR~x=I|pN4>$7F
zZ2u)zBgdU-V%vGmu^qn(WD@;24;K>8`_|EfiNJJqg(O7eD+2{~p}b$)Xh(8IDwuet
zmq@}4xo3sYA@YxRabi%UWr3WEE*bV33FozKpEvt+{^U<O>E<JCdr92ZG8cZtg}|8I
z1iSIml}ajMs+d^BQ-jB&Yx}RJrGulV+d>A_wnClMkV5SgEEUCjR=mL?ns<}KQ%lkS
zSWuavX`_9ewi{kKM_$?*DQb%mTQW;mv2A0I(FMO5wj~qe_)bviPMt|Pce2kk!b|zO
zwzStw;WDV2s)F6MO$@-z!<>5e(1(#afNX9?rbdK!FB-ZU>&e~d-TVZ3zza@2_AH0*
zO@>Yq5WClQGo5{W+YBIOuqa1uMPzr_s2eOjSy$QQ)Rk-!aCV-MW>nqyU}4JAuCSN{
z(S_-_FKWWVLx0;<XudJAr-ZBd%JR-JL+NlsDdu~huNb+;8q_HAf81VtH`{n%?FO97
zA>$$7iCC&DGVoSQ`=)wW%^E%yaUJ=%o((rUi;{fg%W(F9YV22dfTU4s<J<DHVCQAj
z%G2J1`;ITxXmW3Hz%YDtn-z!`Vk8xpq)6B`qR+v6;efySXqj95fL<0wD~*WiqE+^u
zOkj607LGiotD4UJWcW(63574WxlL&Qa5kyPhGQi4ufyBaK9=m6wtx(r<<Rn5MW}T-
zY5U3r2CR?K*BFZj>lDiEIOiSFGR$Xiy^Sskk8+zZ0&bdInt{<+*9Z(x)B4&RsbshE
zlSR}(XU25P+*0FGXv!<lYJL;?tWAF{qYXZGmE?Zhm#2cq)bEfIZN~bI$v3Ogn%t!%
z>IPved0o7{eUtRxPr0-YtQ@72qywhwULgkF=dJ?iNiOZdyvC^Clg)JaKJx6o`p0E2
zRE%e4#k6CZ9m!CYZSw?tVXN16l}w5Ka#Vw>ZTryP89zdm`fOKwj>mn(S+UTNOXBrC
z>9;4czdeT}S`BTVS!Tx92}ZyqSa5N^mdnA8zq^>;mTrlL16MAQx6!rVY1Z^-DH-?R
zVeRzaC}rK>7c#&B4|k38)G0ahxhH$uN}Wr1#s#{R8fusb2A2X7HWzW?hr?<E_2T7W
zo%Q%&bWYA~=o&bG<dtjQOMwDX50XjhH;JYJiCwh-6YXm*Yud^YbcK8>Ni5Y4iMeN$
z)AM1WGdXieL=hgK7d0r_md<3e+(We-`vk3zs!#L<m7ABXu?)h@rb;^1y;1q6+K%Lu
zzchqMzbaj0yqSwrmXVnz+h+p-D-3%oAwIF#nbsNAlT|j|M7NheIpu0~bCq4OJyf=y
z56++fC?C*KP~N{M5;%&c+gG*jDe|}#mRGv3;u<;5J9JGhlvpbtp^^5dGJFQ;kNLa1
zOrwNe%Fu$p-d*CmV?B93z0(NbwBKiU-PxTLz9P-lBOgq~xJq(9<7HqM@CTuKqIz7%
zzX;d#p19u5Q7_Q!i>h`zUAu#fX<Dkz?hK46IY`2N%{!fe$&Ze#w~aIngBI$JPEcL5
zmSYjg*L^rpW#J*Ev31?O7^vCoRFulamPk=nAubin8?^niQG`7(CC}oDn%;ju8TmWh
z3+}!x9BU$0VTLrjm(v#VN9-tMP7}*)Sbw;zcdOA*Wjvzvtzz8e@=W$09M}%1oi4B7
zKk*lar3WmFwgJewxZ(Kgb`V|iRkUCU_UiKl?aFFnRlnhvX*SOOH&m8`@xSoH|5qxj
zEdOr^^B-w-5}g10th#>*b+k;Z3<Qjf3@ilfZ2!Nr>i#Q~{l{Vd4^Z}hfK;ddMo9mI
zhyHg$%Jg5p@IN!&7+E>~z4bo{DLXSG>wlj9D<S1zW@G>VOh{WHST;4$+31ABPXkVc
z9=APjx?ZL+U<^ekPlcoLx{&`kAqwB*zhDJG&UHWrgx$+HINp40zx?Q4d@NV8K5);_
zo$z?_e@s^D7cR`5BsGPu`6ubggOSst$6^TRDD5PrLi7#{%nS_-giei`0f`C-_#UiA
zi4wSSV1T23(8IgC#?gX=Q?3%l#bhv02uy*3>>fefLWSQ!Mc&;4y}G#x|F{ITMFOK(
z@%B9f$<X;>!9WEVGLH{zdTsW_$U+|S`SOFP`BehxC@AR1?Cb%ltHyC-m_h$CTPMmA
zs2;`jZ$|LrgZWn*<Mo{!AkzQa-wyfU`1$^Rzur06v2PQ%NR1OnM=uCj03HmY%`@Qg
zU+E-hqm{OxZ>haLGYCvp?`;pvI|D>eY85x69~gli0VI@Ih?~Hb;5E?UHIVmTSifd$
z$oJHSVO=;JKmH-$TZg;v#A~_78ezTD8$C?|gyVA)=qF$yTwpjoNXXo(!oGhi6LvsW
zdCxsK)`WH#Oxw!~XfSn6LR_DG?X&{_Vlwq2jb20XaZce}gnHX{^+xGqHhbm?CMqGD
zQbXI?19Blbo@*W`{k8-JjJCX6e^^$2K|KV#e|0$k30>j(%+<cw9k2xj06hGZQNLda
z2rqy1VhF$l;>482#pBR{JfMKwL9<(Zt@U;fz~8MuVTn1xUfcRMf#Afka)D|=A>aJ3
zAX=WlfDx|oa)Ek&*gidjrzW6lAwUs<xZ{X1BEA5Z;uwdwD_Cp3uC?sFSrC-`IzTfu
zEBtaP;=yYJxd?!t)*l<jmh|qdkOagJ{P7=*qeH!IAbwds1)!2r1bDyHR8>d<8X8E*
zSJxb0s$2GtozfWE*O{|#>H?}oU~q)5NU(YjU*x;b0+<wj!vUz*7dQ=u!W==Ml%LU^
z0I@iTFt39zU%Y@F!XMxDpX8$-s-GX;_yh>B`;F=Q)PbK7n+vFr*UvkF^EFWaELa%~
zhd;SL7Zxz@mbt8g&wB7HKTYLCR`JY4(9Xi=S&6ECkz%{_I54Upo`ILny^y&Jx*G2t
zn4`P|dKBUi;1ECUu5k)HJU@m)F?jzH99nwgX+Nw%2+1#YKB)fqGkoY0kCIb>)jGSj
zJoSlE0kt84z1|U-F5w-1Y3>8r&{4qn!vGU-f&<~`U9a|FP0YaWii-Aiep{f=Kchzp
z?~N+@?a_@sb9@Cp+}{GXUTs?Y6F`mv-{~^^;J@yeyZwr1s0Ypc5fCTZ9NpD*{DL1I
zf!=lbbgl^aR|&=&r}#SJ=M+M4_5)7^v>Or_@7>`16a7^|AOP>`YIMCbE~0XA^K=6Z
z1$GY(T!((FCveAb;;L_*x>TQ}la*<RL0y5X@VnSMJ^->fw&pX7g`pv-1Tem~s>+8a
zv64;?4aj&XG2(8=d1-b7Aa`aJCxi+pTxzCrA5Y@H+rMuk3LC-~*kvKe#%hp%W|T@r
z+Zy`pWO1&!UB9>4vHv`8RP|@Vy*8(x^7W$1d&vr<iiW!pW^UBQVi`(~)J9;iH4%gn
zJFw5(GRU)LwF*W9k7$oJ4JG{Cs<X#V;Cz3&GnDkuXa5umbd^Y!GG(QG#@AqJUyIKJ
zaY`7(Xb|HBOUT}8b<?o==X8#;W|?D7iMqX=NDKN}?gRCRbrqy|-3ncq7Qo^)G4yec
zZC)QCQ(Z(`1>$eE#%3nuXK|D-U>;&rTq;b5GIy)o3Eg{KC(Z*gSa9=gnNRmhO43WS
z%A;c&Fam<_HLYd%x*Of&lSd~^)2V(VV!qyM$cjV!Ibhq@>vW@yhaxD}BUx4uS;%+P
zdBSk-?|b@k#&7KpV#D-kUSL^e+r+hpG33Z6ONWzXFrQh!*^;tSM&<btFdHnU2r5<h
zfxKumiJplD{9c@x=&D&*&!e#+Bj)0lz_%TY1=tF_&IuS|lL}q<m#sRk<zj;H7^D_3
zowr2V28rrQgs?~yM}Xb&e6YBC$qBv?<m8%s<=MQzJ5_*jzZlx^(NI5i!ZDL)BGH<#
z1_^+)>hte;h)p|jy{fs{aLQdTGt6eRKBkNO8+8EecUor7dydmbmqjxK^A)*DKv$}C
zV-;I6pGFjnon-JH8<|UNJS!G;vv>H(nz2>Tuyf(51C&_?p_@m2ugSxiSZ-RD%(yN4
z9d7WP%{V^%2P}OE>1q)#O`4Qyo|djm-;_9i0U`1|d^#$8KS@pzkSX+haKwxHL$&UR
zgxOh>UkJS(=W0>N!ojX$BbYyl%m;WohNKmn^{-PKw;Vj+mCp>)vMO&{fOhvnH|8Ud
z#shM=KBaV9Wg3IQy^Zv<S;c0aTWF&Jic~>nY=*)aP?bqpA?%1~X~Q!yXlbVuc$S;D
zRhkd1{FPR8fo1=Owf~H0xkP@fb;E0`2Z9&d;60rjT};b4#S3G~;w%(=6Sk3=+?+2X
zN%a}OS*opg4KNFY{BzE`?jnWprVkY@PQ+e3;J)OZMlcj~?{kwrsDgW^wBX=+ZfTzB
zxlR7UD%e&(aV6lj6ymd3sw$ADZ7cPZju2Y}FQC+*vQ$fWnUT3Nz9?_@hwGE)gBy>a
zmgO)^!@J4pIa^FnJSG~saYU;E(m`f;Sly%7w;JD{YHYoUo-$=9L8&Va)VuDV5Tph>
ztoT9$8qIa>HZmQl7PCek_ekm;rca7RNR`a*u?Er?0*UqxwqO+=(h^YzBZ&y&O5d)+
z{DFY(eK8>mw6&yd!uFCJuXC@tFC6V(<W>%yn3$F^o51t&IG--)MDyq_aB<e^OH#Ph
zi}&l&HNiCd2A)fVsJiHAyT5!(UuM8EPV;s_zx0K0moy;wpYG}zIffjw3K$p4n?HrF
zL1$j>kdNP{_)p9;STLF%jGvkR{uxLNCrNQmJ6P0-^@4ho&N@0ZEiG_mqM1wLtJq4_
zk#RB1ghqZI{17L5lL$q}tF4X#&?uEAlbAVA0<es0)v5Pqk>GM~=V)Uft(Q$;D+as=
zh<O@Vr@~%71=E;h_x>j2+*s}}D1~bGgh^2U`BhTH93BnQGdaoHwiw^l=2ubu9jwIx
z0B!gzo4MC$diqzD)54S@fE9m;MG<_fbS!^g<#`^z0z$W*U=-Sd%H$a0S2#}!Ibw>N
zW-!ULmI8eqD$T~DUs+;y9O;Asfvy^T@W9QCR=bQ)5epM7U6)xxo)Sb9xA+<@>c*_3
zw(5Bu)Wk}?Ov4p8wN0S8skFGnE(V?0R8iP>_5S#_f5%<6f~P|9uCQ7kK%V}`At49=
z#ghkMf?ckoVm`1WTR#QJX(u6SIv7UTgM+_{$Uec39ZnV2jmR7O-(7Dd0b7r3<q83o
zFHE{8-;)n4l7n--oDG70D0X<Kf910HYAsQbz^<)&88)^u@*%97Yo-{xHxu(t_y>Bp
z8HA38l9+NJLXvKqi+Q+crs|zu#VkjVoq|@*I>DVeZPDR%%H?*NxuCb3dD81BTc$KG
zto7`1$`PLj90*~EY1AYf9+1iAF+AGi#$8JLjAk-Zx>TZ@yd66>XV=tsvhW&)@t-=z
zqi@N|tf$RJ#uM>o<AziXo78nY!&eGj*+<A7yGC55?>t($C_1pQ@50>Y*(p5(WAqID
z5~gud9G<3+-)oKGsbT6UKBb-QMNU8E(sTpH;j9Aq@oEToBWG9TDL&?jc%NN15~i{q
za33#dJ;wt4iuuaC4XZ@l*Ixd~7OZ0bmK-^ZEg0Ryq8Mx_(q|mVEXZai5Z;S~f?uEK
z0hZF@*T@Y)86kW(U3PO!)$CVX${DGv@zA9*8_s24i-B$VuA}uF-1<Z8Gvd90RG!|B
zyI9K+NZc_!(08t|!Ba4JNx$PRY?|YgLykT=NuK3mbRP>z-^agc@)%-^*+uH`Fkbey
zKr|9ee!j)7#~m%^y$3%X8!b`gRbB&Pytb-Kmgm^I>g9oC?|`onO?0Y_d2PFC)c>ej
zL3RANuuh0J%w4u5n;p%wBkUnKLr5Nobx&NoFNY@ZC!c+#P5ct=mtg@OqREc+RJ82x
zN%(M}Ut*(qm9>0K3w~o4queYwL*E_zsT(hFJ6%SQ(J%I0a-!m{#kvEhu6MCZ_Ue{!
zAuf9J<_4KpV6JvZw2d)9xP--Dp~>~BMjldmfdZq7r2oe{1@0Wwquit&%lap%qW)sR
z7yB}<J+KD(hkAUowN~ekh+}O^2%{W%w$sk<ob0HR82g@F8pb^lwU%s;I_pt;Cx4oc
zl@@$5xbK_que?>KRC?xjpH^jp$KOx*Z+3;5(uR9u?w$S{@-z=P&8_;pgsU&5d8Iif
z@8!L7*0nhLK(BzpMb$~UaBrWtx$f9`C^ry7Vd*+g&AtZGV>xl;)iyLIx|oDZEIgya
zAe;cxzmV!!<=ENLWajTCI(__0<*IFE;g@=IpbXJuDT3&ZaY~H0s?n#VU_PCm!{=!7
z0mqL)jsDv&E4=@)*yV)F-(xqSf6zoU484~T8RNyd-K85?ppWu&X?1)3K9Bl>IOu}2
zl5jT%jP6}0^T#s%Npnk-;V;2Zh~|pBF3uDK85z&-zq6qeBrJH*79N+h7cI2qAc3Dk
zN&AkTI!~P-sSy!#W+yZLsK3;b<neUt9gbQSD;nhrunu0VC!(BPF}(*&@G812bJ!^h
zDNc{YO5blET{@53`5#EiXL#Nu2%Ss&k%!<3C2VZ|N?M^7?O3KT^YpY^&aB1?1?Ry-
z|1O!s5f)l2r+}9p>~^)1f^9SouZyO&l;_Tk=^{8olrgB6aX>RIc_ubL3U}+}YNN<J
zTve)?ZbrAt-;RDCEA?M<?YneRPVtC;0)9O8+Sk9T6x4*$mow=nkG@0YY5J-m?@)o-
zoyUIIH)L!boG2pewrRJGO@Sq@{%lZ~&(ZrtV1e3jO=F7M_AE!nv9ssBqIXBn4F2R@
zpe?<sSqQ8Hok99i`GbsJTbDi=A~SV;N`^hCFm<Ohzta{V7bovSA<XgdK_ffA(pdif
z=uQ<NP_1oKq$oL_cZ`@5z!Ic0Z6U=%h0dFd4mlDY(1|QZeu`>lIK6arC{7EaLawCU
zkb>rA>fK4sjXSDaUo2I>K`CbF^iSPZQ?APmX6aK$;vQDSlCOK1pMmZg$I^}a$rnEx
zwJ&Z^*)7sKo)&%fw=(}VXYsM%iabL6+DNTEp#<`jHphY({5w&s)7BXW&)4ntILT19
zhhFZ~stY#zJlRt}+<pJ#C5KJdZvEWSr<;KL0_L$>-lTP+0ET)!a~EpV7SER>V_r?N
znPo1IB_$oCvU*GsCf-ZyZQDQQ5|4I6+>;X>{G}OP)HW$c96EBc|BrMEJzZ^?cClEb
z#1bp-%Y!8)a=YM)@@$KKx;-Ri&K>SoV|?fM^pR9NJe^TP_$d-!DyIfsYcpk<r%2J2
zx7N^?t^%Tlvq)DwLbofJ3|)PB3Sy;EM(V3S?E5cG*J5+%S&xV~bPSC2y9VhZD;4`i
z(D2C>0Zf$7rjZrtXrXT;>I3iKGjv=QC6v%5VOLM+qf1JG&hcFE@J~~|$!Z9@aOr_9
zJq32I3%Et3t>&Q{?8zaGwQ*-%)j8(ETlo*kk@hUwi?H01PclV|x9)jps8GQKjntW&
zrVT;+GTqmI0l>`0@%g$4`WxYA7+8*!PkTK<RU*3iG{6C}1<K1E_6Mk)%Ng{v(1CXA
zl*r=GSGsSMWbrWQPlIm1=;LZkkQ^A}Fsf|06$l)ry|pp4v@mFXJy(cd<AdU6+}D&t
zY8>5QmyBgaWqlv|y60SrLeEusBv*DMV><8HG$jRXTs`W1ol#Q-9+u!rDiOU@*_s;v
zPUOn_7;ML9xPj7II`~ZU8X`5}c4Q3DmRp|c>n}hqbpGJ1>0xY$-{xWc&D09PD=02M
zJ=~sUXDE?+*Ot(593#z<#j1*1TwRT-_0FTOUQ@5H^+;VpLXl^=P&Br8tRGP0#tz`L
zn)qAWlkOb*R^EEQgu~c$T3RLh^yh_j0Cj+SObkW$58tu)&Dz8*sQPc3w+YJm+h>z9
z69tSl7Et9<4+<vNNQa@LE8QBCT==$*rbN{TK8NlxC89!t(<m^$P$gnT_~dl@<wM$e
zjZL*1)%qO6&11C%6U2Z2DwFjry2j{B{ELfLebqIv1K#U5V~g^vUW7gMV>Q9^x%%KW
zPfW9Sw^Ag*EltwOy61<(wT|7DG!yJm5;Y3%Hg07?JLP<Mc+)y_Y}V_*NMtF;WqK04
zBOn_}iYDfZ;oR+d5#8hljDL@$!-7{kzkEn+n5F8wNV&s{h*Vylm&(hans99$zBDSv
z{d5ymPyKR|&y>+P@gSb5!HeeM&8eWE$wg7-ZOGI3l+LDNol-E9Ni>DpA>NOO7^cyu
zACi|&oU<$d=1b}9TdKIt+mSE{BV23*Pue*yZna+m24u9P&s?uL(Lnc_>hZ3!(QQ{I
zPt1Wbe%qihp9qsf0+I6s{j9Wn=%N2EoEDnF@G~J0(AADSv(z=+An+tN?8BMCJn=b?
zf1sOWo}>1Sz)XhjD|{-Ed7DK5x@9DT!z&%bb|$DC{@b1>b-KK~)d??KP@lx_LZ3M6
z5$I}j*5Sa~Ea5Oo;_+%AlccY``jp^>Kqe?D*61(ChDU8SkH!cy^`?t3eo>{T3|~;&
zUVRP6)B9u2(AtpKHbSx}!?}-Hw=7d41BidD$93K%b@eqxtsB=<fN1AjV6sn(%BO_{
zKWo)SgZJU<d&g&k#+hu>tjaYBsRyA@wFO?8HOwOOGpX}dIs=}=`(Ank<=EG*?d{gl
zfV^#5#?kqRL%TwaCE?YK7BFL*VAxl@fMEXtNI_G4y~Z<pkVv#GRY!GMaDgT49-+GJ
zKjiA&%pU5Q$OwCHaP+Gtc?)+1hSB3Um>A|h@w{RBXT`w<cR^iFk&g=9P8#}{%+A16
zP5<oUrk1`vnNU(tbq$bRj|_@H53Y%3jOO!Mehe1BPc<(<9Pun-1oeZf`u^2F#~otL
z2wYmK|7{FynbvXC#TMI^^o~a-)eVUD`_2)1syDTl6Rpl&-Yfd??cvthpg|fK>q&eh
z?8DfbJ9x`@<8_d!juYHg5vXCZ`Q$O`>-?Lo{Tx^q3VcP52c_RJq2LxoHa)<`LqxdE
z=2%jr<%)vm$nVlWBKvZlLnvS<UK(MJIne~SOrrE)4fJo3HRG9$@VFO9NO#^dM=kzL
zy4?0s=c!b1bFFf1<Xy6qf}lG0d6)#M<7o%+xxqY+SAxy&fdp6lCcbd9-pg$MKK}hX
zWMT0=Bfh-C*A%v3JvNCZaztZQlxoi9_1@m`##q$5iw2%>XKSA^T*2?!A8bLJixG|l
zB2Or}dkm#TJD7#W2Tcw7H0--I8#3);gPLh6eNyw)gK!;FT(7yNM9KHdmzXh!IeNp9
z4>YHk{prD@N~eRSiG(*_r}{D8!y4Epzvg^Kmg>^ygR8A0bxT7(8*4tj1rr9Rs?{Jc
zyNHBShZi(!i{*d-hoX}mc$h00)s>wBLJqrifrco$X6|b*yM~Q+RsF6Jaz<y)l;s&8
zz%G#BlD3XDU%V2sM0JX<W-+bOXlZ&WR()q=Bt<pgWe^e;(3ER-+^h*W=bU3vB*HvK
z?8G*8RoFUYMoV@Oho-OuLy*>aX3M%GCc>h+*0|4}7>M-zJd-+ao{m8}u6+OxnUpYL
z$<Mx*KadU=w@^RP59p^Op>7$I!vJOcn?NJavF#T70qCpP-3}F@YD1Dd54ylcd^j}>
zl_iDGoC+et=NGd-Qp<6eKVe}bju;WbR`s0RRP$z}QE#t1ENXhIUq(};LmRRyqixM^
zKs;VD<i#u+MV2Y)p2TP6{A1h1`805MRz)Ef9caKG*^1qDcvTB}ORU-)$@FQb;$y_`
z#{2I79Bcc*COn`<Y9oQ^BST$O{>SLv{G9#XY*S~6UK?Lug$DM*SI@2DBhf@s=b&<z
zr7nDtd*GrkHOAolL{esA?NqvT=5&A!C9!C@{|kH2`ugBy&+Xf&vrxN|iGd$%T3^iv
z7C29Ifw}}+k)ubgtQYbjoKMuvV<y6V^@1$HBkfI%-@{Z-4xshD9^?Jd<<fe@Q#k$s
z-4M4j)*d&;?H(Ze%NfgB`u+tGZjB~}NhbwvNHm>z>)6wO=Go9a+2?XzZExb<v)i4a
zwmAEWQ(I)I*L4ONV&lbPA=muPBFJ>_fw+VGWbI|L!P}cG!36U<&~3-dAstn^WhIff
zR{zYC?jV&Q{MwLa$sMPS&^cMH>58~v$#uo5)g$=ngFYA!^$s{!cfBqsHY?i>Gbm<^
zyZZ=3yO?HwU`3fm_T%#du+Gc&Oe5buBHwb#3__pA`!6C|;<S+O?&q+~Q*e3VdEMgK
zdfae%O<J}`^y>B4;;N0B52S1Un#=2^FWZ}DU3`nLl9`@!WNfgh8T%`9dDH4W_9)J(
z<G@k(?l=SHFF+l7DE9P5>^XmPQ%ak~9?O`)s9jK6yXIAinSggd0rD%dN=Nj3S7;g6
zhP}&w;wZS<;eEvG32zlMBA293>UlfZ!YzF5H}kLMf{f#Y8%*8iD<bC|g)Cgv3!$+4
ztdD8n--jsOX*q`!U=xCtGZ<}QOWw4Nct>KP*19#iaa7m(4wZpXuf@MfS;zDSD`PnV
zm;69Qb)kleDPhoGi!%yC+=%(%$QBM<&#_uMp_aPe_>w7li?2Q!f}vNP&S9j>7>wI>
z)H478f*AhQ#7%TENk;E1bypuwx6TnlYDioaZG{Ngqv`l=yy$7Ip{B-Q$0c+7o$;CG
z{C&dF6dUCnYUq162*1)R;kx;!v2>rq&c1cn<29$7SH(P)KLtohVR&94<$ulRD6L6*
zy^<OXRoND~1UH6giutS$j=-}R2xKEmG~bQ9LmjIV1l7GUw?6kBo!s`%Xq$93@Xri-
zt0YS*FhNr8N%h_3+*<PuNg3tXU32RU``~Drk_?Nr)&4wC(QvJG59ydUBSQ|F<kpP3
zo%%{&3KtY#gp-a)>)#<&xm+S1l*+#hbAM-3v8qU~A&AwxUV?9tD;#Nyk)~2u?+wsz
z=25Fjt2k~+Swq4(b9}V8#8jp9Ue*8(#)J_JB7PT2Qc<AB=eGqSu6wjuoO~KDwk2CN
zdD#2Ulr1;g2c8y7*tNoO@^-iWU#z`jkZfVvu35Hi+qP}nwr$%scDZVoZSAsc?y_x8
zb@!a_bkBSz&WVXxzt(y)GcqzWGU8ov-Pdz#M?j6;)N(W9))bt|r&-C<H%bJnDqjjq
zSjkkvr-B_guP{gJyiaIgP@QI1iFl=n9w`8bQBzioakXpODgiY+U(Z{tkZy|)?&GW#
z*m{sgnwt`jqZ~L&5-l(xnKxfkZwffp9A@D)yh*9i`|a{UCx=nukRRo?cZ&FOyiV4o
ziVFJ1zV3&Tl`6vzZCKK(iC0L4Zb+h4UVn`JzAX658+J`H+7luUpg|sWpdT6QgBzUP
z9xS>>hoEY*voo4WsU3mGyq-Wk%tv(n3<5|g5*vDN3=btjY;!&rf7;cfXw%hQ*eHU^
zzBOapJf_&W@itk{S^K@t<p!9d2_7Sj>obn2%wsg9gsQ??qkA7)I_S-M19)ZUK1rSp
z4|;&BJi*G(nA=YUstWZ-rDQ6!{xfZ=6WMnwYu!uyU~CsS<<Zb^YEkN5eXm}e<Qc$u
zBjO#RZ78HfS?O2hxnwj$1y~~bk{xN3*>!iVF+rFJul_r>)#YpyeKL?|4S6=7`#AYA
zx+&R*`fn5JScV~PWzN@zqZ5ogggbb#+764UDy1Pk*ESUX?c0V;kIYR{t2Z0!x7`t3
zYd<JnQiKC%F#cSp4LnaBH+-qH$^)em-blGe2))x6(FLiylH_owQDchg_25Nw_W-FF
zoxR}x8aMS#x)r#G-x58xCHWn}H5yJwF(C!KQUC>6%NO9BN0B4?8Lg4!A&(>7kUjlE
zVq$3BVJ7!JpNk}tW`lO0*up3rI5c~HPnk+|q2*XS2E=QxXUS9wYi$`NC6V%K-%*Ue
z(oLD`2N;_U)LhdQ9M|kR4oaVm7sAB)wp>6fmrTG)$AdW@&*u6urWou0;#}ZU%>C55
z!;&BwKekF5!v=(_IE%9xZ<^W!W<`lFhYD|L;%qncx%ekbtU7GJL}t5?86OIgMH$Tk
zChzFfZlK9mk%^tg0(0gnH_`xroHMFg(6w23FGB%1mD_PBW$IiV>{)>NM>Uhs;6uaX
znEEw!t%=Szq|=A$L(jKuVG<4DK_$(wFvja4y&0T#;gNVcZ8y!jy*sF6DVrc0`-#3X
z4{+FF;QnrKUzlGa<r-^^Z7Fy@{h0sb!)zA6u(lkIy$7leobvv9Ayic$U8Yaei%wQ!
z=q^RWW$T{j#@cP0oyX+Ue)dv)Z;?}<W)I$#Mflu3Eh0~)C$OVP{hq)m=VWh)vT(Vd
zC=8{&A^+jw1{N&Z{Ck2t1CA61n4J-4o;!Fk*bL}){B=N%N(8Wz@dFdHUL8oUCI69M
zLoTlg9L8*RzdX;jxQFaXZ-CsRKAvijLAQ=33zbX0ht8<X_c>SoblTstZ&maVKtsKU
zMxxx;T<1EcjgT&*hWmTgj1Chcn2K8PP4_^O%QCv7chSjYZ@I-dMyo&U2;}Wy*+4Us
zvv(81`m&zOCxvc81KLZgFxIm3SX;vC`b>+Yw8(K%gOy!ebm*8__@rava(8O8ce5&`
z;dM$Ut`w;{OvX?;&^k{d_bL49<b34?b>Jvi^5dB-7p!)(kq$0+luT2vNOH&Okf*-(
z*N)wJa*umGwF`lBJh}FvZnk*+JPTgb&AX>IMmU}nAaA;r(Xy%M!8vAq-w5a;4*dci
z9wlNb8GTL$*N~l&@2Q2?$xVxL=O~0|EDL7(M~92~4V*F?Ye!{za5XbCqp8aNc=w}%
zTyjo>(5-_Ov~wS_v!9MWZe_h(nRSrKa=!fYD^PdJ8q2zjQ=D3Wj|iyN0w|;lFIrHG
z9#=41U?H_E4Z$Dx6LN4?9g@I>Etw>qXe5|YfGyETecqf{@;=h9s-#*$|CDKj&m0At
z%MRmX8nGaf84c^_RjHKy?)uhA>nQq|)Dt##fE?VGyp>PF1(Ki^U&(G}%2O;01urgW
zSIUe0CtaeS(iXRMC)A#&7%}Q@&@Q}$TPsZ|qS!*Vlm*@{=caF9rAINL|1Jpf6MXnD
zk^Fxb1kq9!k`Yt;M-W8we`V)0{RBZc+5Qm(VPqp<XJ`GNc0N1nk7X}x=<t7g_G}!?
zQ1l|E&c>#8E(ElUoIkk?Nf$#KOJhMha~sp2m7j%w+4(I0Zs#-qboBkt)<Q!kCwupQ
z8T5wEKPJ1Sv$d_Eiv_)rlcBNcKOOsjp8e|?My@tCrY`h9>$X4dNpJq+xc|Jw)X~+@
zhTg{1+4)~*&X%^8Hil0BnJ)Q9mgGOvB`p8;wf$GRgoWwf8U)$?S-1GF$NoKC!p_LR
z_J5o%as4R})ZSoI+M}2!D1g~waFKGCa6e&S?n7W6n4I0BB$y|Vl8}UokdO=}pd}1~
zkSKR4+bKWpI_AFqu6_JI*R;+v-)XP))nCDbLjyJjB&C?gD~%H|01W{ZWD-E-HI}9%
zfFPlQ0Ez_Q;E>tXP+!$I0|>{2i47!1WcsES0EGoMc4Sj!KtnGJ7XfJI)d~DlC}0SQ
zX()+_06_r+B>9aSAu<7=+Mnk@p&*Y?9xKwv004(dG7b*w?(%1E{o@nzAnXPpw4{Xe
z)xQilb<@Ct10)9EEU;dVfn7%&VinaQCZx|$Bi}!DH_iEvks_0jke;4ikO6xP0n#PK
z%mDmDka2E+7y}H<C1fy=KOopPaA*G)0WydKfNaig2EUteG4L{UOc<aem^ueYSl=!a
z&(BBztO#p3mgRZSvm%2C{{9YspqD`ZTv!DSg%|$AukkNch`5(rm@t6^I|_zB;BgKh
zTz$HR0=}-gf>rEk_z+?O{-8m+j1oG0tH?0!pd0%@e%ClKD(XxiL5=|5LE(YF{!-A0
zAppC7f-0V<V{TGb68@r+U}whwiU#~x&!@tH4c?905<i(S(M6D;582U&2Il}eyMYE-
z1SU0!3h4CiYf|}!>dyrDUd1ug_{AZiB_t$+1>p?-r;AWdsCPX3<(ENk?w{UC4iG>;
z3KFQ30AN7q0Pa2q`T%^e3rJw2IlQ``V9DjnGZX<0OvoQXjbFTzh;aZP%DCoXoFAI|
zVew(!z}12rSCGJhy?Nh0YkMc4qC`5rUy$Ft`h>Q$xTes$J^T^h`^su!UcjFqB7y>d
zmX40U>grNB6qJ;(FmA4yzBvPW`@dVT0{;5j!ac}~(uM#dQG9{}U)_8S|9wTLZTn7w
zfxp?oy&`nf2?K2ZF6e=kzyaz1g!uM{-gO`Uj(zU^{n1<gy@Rdv?BM!&dGvMp&A(~D
z-c~uI56D5~ZQ$52E4()t_;Rh_yoyhG-v7%1zV-Xb%CMiqlY$V(5d|Fq4H+cz^Ji~x
z=3tMp2oM*_@kuzpQ`Gn95K3$SCIR!Y5*s`L6!<j}u8cbf>RxH!0Q`O#^jGBZy2lFQ
zXL^FPFVP;700S266JUS?337jZpF9S+GR5ROdvBmn5CjQYI?$jY3;-9wT!6mu(3Xu5
z0teO}RPaX@##8!pt1t+~!O`ES&?1LezqPL=)V)BleU{g3oHp9e?k``!K7(xrh#v<Z
zG7;9@4|Cf`r+V;&^yZQG_L#n!Q8I07Zck&n!rV`;L%oq@crz7WfZ7hW7+WS?EKcr?
z`OXdx-gBerIApTSl5c3Q!j+!V9{S)h|Du`u&y_$TOx!@yDWf!sc07#z&#fX^(~*}A
zpC=yS8exZ4yOPITlxB0%7n}<)8g5c!F{2Sa)nSX@NPMG-;dhX)xutNPKxE7G4Z|E0
zZckr)FgXq-`^ZB63ymhZNl$k6`NZj|9QxMO%4t<nc|>tx+0&BQ7_P)sNttI#(N;kJ
zz6u(@3Z<;9QHlQ*+8C!iz}3~yKE%$aGY)j&SjV^R(L%plFgH{oajwJFOGX<2%b<7b
z(@E|-OB$lY;a<!IkL1<GPTD)SGw-F6=$9%{f3LnwEkl8)z2Ibj%xR)eEfglCC0_8w
zNWa*1shfEb8mYKNQz3x1Lgr-jSCl;D8ly~)RIYg5HY@m`$&yN<Ba|2#L-OcvA1Jxu
zQuy79hr3L+<19h1y==Y)aJ8W4r|xroh2l)n)A54MtqeE<y<6*C24Nxm<)Mt}1;4rL
z#z_xq$dEU{JqWgagKRj%^^i*?b=6})u)lQ!dv{L<dCh3k=K4F&C>xux_76GE(|&og
zb-=EfSgSZ~d2X%ZClef%s<`rgzh6wbE_36jKmc=H+^*4;K!Z?vUgKQk^x^%xqMxeY
zdJg>#dlC>#)Z&^P&ob=4JbBjM-}82*MJL)q<B2}bOKXE8xvYJHaJW+_qP2_KQX`$O
z41MwUdVN%UG**GJK;P)Xj;-A|x{ndek2W_?At~XbNTFP%kWSOmlh;xH)FN<Vct4BX
zujP#kOdWr{Pd1C6$22GEN3(<OWIo*^!VyM|5U)1Wqd+o~OS442SnW_sSM#<Y&fFt(
zni0#bzswtP76YOuzGQ-wn@pR)Ug6u-QzvmUi!2w|(YU!Q?;Z9EpBVvG6}W5G`hNtR
zLDuBxEp<8j6$GdOtb>vbm29`x6xv%Bjlw8DPT4(!n6zTpXE1FnhZ0dtzO<K5^?iE&
zs>3Zi_+zK2y+&51RU14ZVs@f6F@W4kbBnRFXMr9}=oBWzm`P9Rf)buIkcb2xL{gwV
zjzr56hCKHme9Rm|=wyDSpRqn>+>R7i-qQx!4da&j0La;};s>3$#Bzzu<9J!Ndnf@M
z8GCCAt?f+dt+^-78MR&4?9Je4Op#0Uf(r~Or5w0LUiH*BOVB2rvUZCTH4XcnXyPC)
zMkIZ0uC}3i8UatGYc?rJm0ul%BoHKbqCKs(ry`+#xLaoF%Oc_@&@eDNqHBk&Iq~)C
zayXZC?t5jxk56IaGdO)JA^!nU5E!^vpNZ0o<Rhbh&}%N6Z^qDy-m-2|TEFNV%EY4o
zPAnRmvtl(0C&U%LqDhY-Y(eXOzg$hHt$o2&8bB~JyFYjx=_asdoYcoNewRs3Lt0sm
zUisI{ba1cqn)l`Qz<4)Ty$V-Y1~nIK$S{a47CJzji`*k@NDS%ZTrJ;dZQ2{W=lv-s
z%jK$ZRP^?3cAZN8xciO|8z5C<(~``=yw%Fre=-MpjXaZ<u7X<TL+Q*@mYG93gB`J#
z7xMFw$citA`T;snH+t}(?zK-ro9Ym(BDQ2Bt|ak~bABle8tgBQ6k__!z3}Oaz|Vu3
zz2LX>Wss~FCSfcX*Fb@dd%BO^7J@D)-M6aqchiJT@BPQy%3?Lb&>mv;km`7op)_Fy
zH|xje76o(DtMT6gQcN|c8T*=8HQ|c)*NwpxG)eLucpZzc=rtC*h*+gh7>l5_{$1r)
zO2#PBc8$^`)|%EhQxV#2?+f|H(^fghf>k`l%%!vZ!rs-P$32zmD4neLE7d%W;$$eQ
zBS@`9xa9z~o!5R|Zy`5129}ZRj<@Hq9KVIsVPQ|0lpu87<ZlOCVr=+j+T+-IJX1^?
zM$nd7L^b{9fwbgsP1+o2sumhQ$N#!-B>rwD8MveT3?F5xJ0TKeOZA*S58R&V{5>O8
z_QI*pRsS#<k~%ynAJ1UZ_T2=$i~xvn?m8`znwEzQDwPE*BjZv=S>}bG7!vo0d{z-9
z5yO;Ai_Y|-Ld>i`aUcd$FEXTD_nQvTv|@AB?YV)g?Iw$9N)^C`jSpjbgP1NDP|Z|O
z3d>vC(Is{|Urs2^pN3wf6dN~;L0#3Z*KOlS#^<$R<jGqF@edEpgBpFY!D~B*`MoUU
za&*3O{4%_185R7DfT;LF5o;Q^Jms2q>AN|#v%9M0VYgY#w6Irjbzz^qQ$??oD{V8j
zfs~o4N8G+<euZRLx`FrQlH%Sfjz`V}9s{*wO{gIaq3d}XlV>IJ_K@6Uau>rg;;^sh
zmQ(hqv}1jv>f%_(1WQR-{UMgN$%fy{-&hiF2{Yh;vQ$xrzi!9_>pwqpL>Rcf(HMd7
z;OD8-J#N7Th~5xXD_f<pMQR2B=tWt{AtVaw8}8s^R3*Y|Jqc@Kc4M;ET1#zAn)k-b
zTN0NmGmc5kA+&J~<5_Fez*Jy|^Ct-UBAk<&31yk)8D&Tv&Xj~#jj-^Y_Vhd(uHKch
zeEzf|4=$$;pG&wWsW?M*0CVeJDyTWdHBJ5YF5{UAx?grD$rI_3rRm`PxJxD+bbNIE
zA^H-vz5e@hWh2uRTm3xWF%~N_>PuJ<HhweYjd`5T-Sy&nSU6LuXz#HeQ40DKOWOeD
zp4KQzXLKRzTw!;$b7I*f>v;*{@&Ll)FTmWw)Q$%ETn;1O7R{Ej^N&Dp+Xe9a)jTk@
zOtV?Wv*m&A#|BDWwB?UMH0spGKStwt&3IdIV!vnK@<R(EiBNNfv~xB^!b%=^#m)-o
zH+ihD%C1vpr=Xv^euGw;_7u0|cjyKG_4foxl!<)EWHb1c#V0imqD|9;ReH*Q*n`aO
zAenaTi*Y=E1!u~V!V~Cm1t5htcbUA(_TDX5lc0@B_Sk#bCibm6Vsrr40D@Z2h^noC
z%K~^im3Lco!9w0+NPEw-Se`+u%I%N87#tr2aV(ri;huSlkWp`voT1`U)fVO|s?kG5
zwDPBnBRbm6_i-wmUF=}}rBd-SLB3AH;@%xHHPQ3xK&rB6fxcsY{~49?gm&vNA9T*!
z9dR3NTvm5Ne&w4aRaw<7JVTYw`qqWNt!r{saJR&d0ey*9j+bk~rs&>X;r@Pn9T;tR
z6Q4LLTqrw*X6_}B_bSka6m1Kr8V^ez3qQ_~pMNXp8sV;Wfb%v1&2^sJ$G|ft_NKs)
zgn^IV@?(2*n7zIxf>47>qS5abBYUg$hM2DX67caiSpdVWLW|W)|4dHQodl}{(S#<K
z=j*XJ5*W&kIo!^|NxkZkB%=u!Ty|^G`TDjJF1OdLS9!T0cQ`e>Y@7JO5|bP#X<9G0
zuI#T|^jzu{(FRRtRwer84St!fPPZYA9vD{K-z_Q_-r+(tulpkRzHEBdYvj?D8(G(+
zN7J~})6p_=(AOX|@pdAwFl3!{421i6<m6+t!>moamb567+fyteaIm7OZ^F`xa@>9n
zL-e;~=^I>p7!V3RW&lJ|Fm%k2onFMvg(BmOr$G8(9tjK$I{dFn0<LmQ-zFElep4}U
zE3#?!$?$H$8*&fxMK)^KP{j%7f8jAh*6FixS>SR?)3l0`t(;(k?~x=Zks&SV_+Qb+
z^WnR&pjK?8r>S_F#6pnx?T8p^FG+K%DIf;RA>vG_)Uxx~R!-U#VEuNzkJ_F&&t}`*
zhUO6DL2L0s>#|}SUUl}gZNmrg?GW%&nQe88v8ypsJB6)G((R+(2RbnVu2mDxEOHjh
zy6&ap&2(Gw6uC#`>Ye`_?{KUHp$<ALH4vPv3e!DJ9@uubXxFaStI;^R9aX*^jmUMT
z$?DaK3lLuD>eS`U2=)^m8o=U9mvmmum7tNdCvlO@LFs#3nCZQo<2%wvM>5D0(&!W`
za$C_~pADdgocRvE7OOdvxJ+IalJZQnq%uTP!z2@13ej_6zeh2jVZ?U56T|4Bu&bH^
z@t+_tZh=c3oP`aT8Id84!ARbx_`a{X7D;abv&a}P;HD$^9tY6Fnp=bCFUhbSw1Dx<
zTv73h%3K#k18V@jBvOMpk=P$G_u?FSP=w4pUf*KO;;h3}p{|+t*4PO8M1D!RzkB{!
z?MEG+QQq1zeQzQgFTceHNJJnR9+6PW(MvUY5D?ctYf3T}o*C{j_p5$2cBl4N8(_4#
z<cbbIhL*|F$3#7f;H0f_8lEpK>rRn3CdH`jOZ+3r>Kjqiqb7+<|5^`m4$e1w>2;QK
zUQPb@E;CelghZ@z!pTI>cuM|Llh^w;hE9ep3^1L4p<<IRyJo&ZU)F#9rKesh`PS!-
zqVtKc<o8+Gx?SiB=8)?Q&st`78brqlTvc>gI=^+CW*o!n3)V*rN(g%hsWW?VUMOFt
zL>2(MxunvHhRe6c2hH2POcU940aD2r$%|QPu6#!f@cc;1%z5qwh31~r+~cU5OR;46
zqor~w#uOszf-e`_%V}s?$(=3q+8F+Gjx%qkuHC3e2F(;4%HGb0BsR6N#lkVabO*}k
z9E(ho6~hjK-%vh_lOQ*jzocwsD_#u$_3BR@9n_I!(XWFSx~A4U)yE^}&;Uc~LTlIX
zyDdvR&u8ImQa$cFeQhh7MP^Y~cm{|?2tS%Q2kHbTY7b9JB3fs#?FZ~yO8E6>tnG%y
z$x6M5a%s3+`3Y$xQ)K&jSaGG!jD}2d7~ygy3oRkkNBsKzqI@L(-=0cK%k+I=>t~oB
zikUUKVz9+Js>XyBy!+W-lxCT?jv3Mdlf!a<$8&_WKDbU3nISidQpnWldl!|04XK<>
zy6xrkF#3@(jsuP0{Q)VhNB)XV!a9u`K7a)PzJXPTE?d4y<<B<~Q@OA`ln-V(SBR=e
zF$4!FSCdzFaL%4Ft#W8E4Dua_8&8E131}Mq(WW}E<30D(bGmc6`PJK$lChROt-FN|
zZw!^ZIhH}pBR7o&cNLAHn+MMmz&<q&ggz}0g&Q}rNw8g$Xc1QU*z$2)R{s}0oDIX7
zv6ErJok^J;8MK@^HHe@-F?Y#wr}u!05hUppfozAEDGc_sR=7MrMC+c)tSUV9o(5#m
zbN$tHiwFe8#l7i}&(Z?`?@^C5IlP~SxaU|T>8;uMB)bBaR+Ox|0PZER2_o_Y@O3$R
z;{Z@3gx=!fX;Yh0!K{`}CbJwcrF#X*>pf;k46NqYPLhouvzQvvW(3O2{MGhxdy-g{
zGYw`sMK$TH^31u{_E4U^5|%$?4Q}Z`N8RN`vhzh`C*`?dMYx%gJaWkdDS^(Z*?9vH
zKQ)<`vOLIclam{6<<r4iR`*hYG>jx@ZMAoJth!VcT2U`E1(DA9FVLAKchrt)%%K}C
z<uWHz9CwMh<O5?0wre$gvrh-@QFA&B9Et6!h!c3E1P?+<_}p5~eVbXpV;4>LbLWU!
zQ7%}~7Fjqf+Z)RXh@|OtJksG?p`~%OLgDwbBvHx`Clws{PYfhfow||3_@ku#mWW=X
zjc0szf0J~N&)ZQeYPojNbtFE<W^x~0_Y<-(XQvO_0nzgYo0OTsP7iIex7M`DM4c59
z_%{%W9k0TNS}eSJAe%ludOkSYhxHm>wHgeWbk+FwU+Q{XRd&A4J0HR}JsG&~&#&4g
zRjk-+sN+-PVl?{*N*yp>5+`RdVebOliROYBTdEt8ed{$NSzW>N0ZnWhdPb=kS7Sbb
z5AyD0Qn6t<)%=@IkdBbWl;)C%Mx<K#5_~;GII%`ui1_~X`1dBAJ2Cs?bVJa6V=Cvq
zzKVN?!ITMPwP(ahLwld@K}aS&HiH=E+liWJS3UIsQ7^k@Y*R<fGa<9QI{_h7MB}$X
zR#W5^X4lX3!Rohi5p`RwJF$1r4L9v_#~86szCch!p)$om3oAu1{(1cezSWvGc>JRp
zNluK4lu~eChUhOkb(QUG*Eb`Ij9PVANJBkoSdGQ0BXjOwCJlqHLKVz>6E57%k=+5J
ziTr&LtRYqNwYG7?@jE5tLaoZh6?W3^vobR}ArTKzPYp_i!QZx-MAdXsrdfj9+6y9B
zxfq2!UQ6|yen4i~0{vSEr<6<Ps>`}650(k)7xtMEFCOww9OQw4dcj>J&!DcnRoiO1
zm<w3PGuX<PCA39juX8xKbnNgD+2b|W1mn0N@tP=yu~j0ft`KW^G%zpnm_}?^9zh9|
zfSD7XZ$9~5He=W)<z_YBDyjU`70Bb@qm*I0+NsY|wiJ}`_7%Og*n<X{(CVpSKoS=3
zJ<X*O=;Jsu$zLJU>8iPVA`6a&E!@o|C!|29C^+7xrN)ydvWAQ%TFU;mAip*q7Amd7
zj-30j86L%!xevl=b7S1)>`&uf<*~_KWP5fJ+N_@S4rRajo~dXG*%$I)v!!nAyt`Eb
zCusRYpu=2Am5?9L>=vdXl2KrT&st8~96U)bW%uakJ+4@!5>hVm&(`%Sh$S2o;ZzJ@
zEshMT(m-@7xKh-BqW{jt;~}HSgoC|o@-p{o_v{LO-=9<7=6+B{B@42REahl&O|!+5
zXCO(_%D5Pb&k8%TwBC|sBMctE)sHI;T~HtW{pti)x>W!Dcr9M0?G+yDJs(+ChrY8q
z1K>d+3Y}X!0iWig9yI5mRhN6!rTW$FOMB<_YSfu`xBCdwV{s>)G@6AC{vq-eN`K%f
zp|&1=F~zmL=({j~c{lq)xR7{3oIy`E1D?@UeD;NqkQ;QY-Md!R>HC~S;<iA2C|1|M
zx%{T;U2@d@W@OYgc^+kmMq4G(o7ItSKjS453p<Hk3ST^x!+kVbjlTGl$z(M~72Ky}
zPb<%j&yUls=OP`IRYxeHZ8uW8|DYCcv7HN=46SyH{}<uz)V9Q41&)2{ytBp##^)|M
zdgJn#LJVp+71wh#Bgr)kMA)=RP23iCpW3wjPmj^C2K4H0YTcG@{n2hOHGb|A|L=Y9
zZO$3>*^by$_v~wL=oMi!Z+7Do&$Up&B#>vIMZ6q}eaZ%DnJoOcUO4~ewuWG}^yW7z
z<6<#I#}Q<z4<den^JYPk$X0*-o_)yWOVbfAWB1y`Q}Tt@M|)mSSIMZ~aTa8EgVibc
zJ5GK4tE9H4@mXSimE@-83z@-s%ERJA<S0wa{80T$&C^>jBRboYuKamH34bJO=V#0h
zs5{5zqvQ!mut#a*us<=4<`Z<Cr8DWu-V=!6Rh_VeSEyUzwc4isxCNhw2e<KI^7J4&
zA=U8xu8_udV6!M16gavy;*J!NTP8EoCV1aW2iHQp<W`eaDi9pozx-r;R_v;9CFhTo
z*BO=YASF(BomgwoeiqobVHJ%1Hirgorr>`!DlTZXSlu|-=9~~t-57bT>|}Slmda@J
z#T8kLFFUskJMt7)dB3(&=V0f<(ckEvPt$If66rA0bye~kFun#={<Wn+I6<YkXO21V
z{p(ZOochkTV8eGs&<j(UjRJ-}@Pmu9`SDLg|1ZZ9uY1G9ux6zOKkdZTZFzgQdyZe*
z4PFQJ`$m~Sp2v$zym_m`O&+y>nSjl#^lA#pk{}7Ri)wHsZ6WopFBnxTvdhzDUP18G
zH);x$W`xC)moH~z_lQ+xSW@$_e1byL?ODcHA(y4cnig~5v7Ai_)OMhpY?j^lH%q0g
zBFx)4lc&+8hcF4VaYdR*c2AKtnpOHcXh<>HJrg!QQXAJnyPO@PJ?7-W)_uLED$c-{
z;zGl#=Fm|quuT4H_s<xD>z`QGbdpJXoN&?ev5#EMg!}O*m-l(ZQcv8$)Y&dTx}-q&
zEh!xTX;qj0olFti8H4sgot_c=nb}}`$C`nR^}eYnr;N9=$Fmj0FWOUdu?BRM%68#X
zK1GX092&w8O=a`>dx(;0tr+AM_xLgzy%d{8mo}BU1s%cf)cjX*E=r?F-SK-|v!jES
z`16zif}!0$A`ix(D{79C2-5nWVP7xp@C^ms)xPHlE#HC6X8v^V8p6?2%gN)?s$uu_
zu}cp>OkJy@;o8&%eDGFR*jR;}0-=zmPxPxC-5|<t*4qy6nBDp@O60KLEP6V%Y_27?
zSMkG2qbqZhw<g8ppp@XK>W`@%9;g6ftT$8ZR7OD8uo%#7Hc!d;9GeeE?@7hNw~`n8
zi{96_e*QD(i{vnD*gg4ta=#)3TV1#&1kah`%}}M%BH<M*-RgBizk#=-r&<3y;L7@M
z<nVt7T%{zXg*0UT30yh;d-0x`;fJ@fvi|T^4%Q#u`ft<F|G)FrpRwWpCEouhZ#A}b
zGPd~{6Yk*Y<U()jYU5(*VB`6psPrFT^gmH4+rNF_|A|W3f2i)iu9yhen3;a&3j8;y
zl=%lD|MU1i7*(zUs)BscNe(JlTx<<NLBZJ3SuDsUGR$Vc&{7`@2Pvq<m3jpSNh%@)
zlA@xhAk@(*cpY>7e)Zh@oZ~$+z4pY{OTN8x|B;s-zA&pHfEm;jZbg8o0ca@r3r0Y7
zW>pCb03-?uD3Hj6hlh-T2Ip-=X4n`^)Yw2GWYXVRK~z{!BS#iCWa#yj5D~!EokKta
zg@A^Tn1&J<1V|)6;ItPf(E>97YZMp^@B|X5DG`DqEJhqP@=k03C%3MR$^R4dCg>I5
z&-{GSNADlN@frmd7#Jad2LTQ8a_Sl)3=nV(;(i8-<#&BbkHefgjg**#gtD?Sf>BiB
z2oY|on_FNWLk;piT?(+MrvM=Uen{X4!J65fNsuuQ05Um-N&8O5dBAf>Az?rcU>vY8
zB8D~ycMy;$u-z!&m}VD1Fbayj*2XJ)aKFC%ZGaHa&~LeC@;7rLj6HP*2pF-BLIDVS
zj6aCG0Kvk5U|1bN8uT>$03iV%E+G9soRnu27$i9O!V#|54i3nI0vk|(Bgp^6u+IVm
zDjIRHZ<n7E>Qg58ynzd-Fv`U>l&B7)A^%x9xS+s5XQMmRPktFR@ImC|i<6@d0U<n}
zgy4#TP#I)$3io%((+H?J(fh~|#1PP6;5-u2o&dliEWjfWH{_>`{a$R)7f`5wmLtrE
zuc8D>AD}qU8N74Ap5LesBLO@GK(t%<+xwUM?hZMVp&<YW*f7vVK^z0f8vmN3S^2;<
zv)zkD26`V>#6lhez{~ypbFVjtr(gm^+cW=F?-8|$VX2*2vG+UselH^`YB74Bx_}54
z0Rb%?C;}Q<GJu4X6wt?a?$I^ahv--O5(3D(Gv_WpW-|huM8JbRT-(v-(7=8?jmLKt
z4F2v8>O&$2tuSEMx5N$@1suqJPssNU^bPfa|H9`C&3F34_q`C-mBR~t#xwdCzd-^u
z%+==zAi1eKjUDYnLffIhM_DE3!!y;nAR&YM%C|^!9l@p-1u>2z(myF_)Mv{80~P~4
z#UMf$DEBAYyna#NQosO$f-ni#)BRAtC}7}s;NKRwiy&`Rh5-J?puM!S9oHp91CoQ|
zr^pCt009e@(QKFq<jjaRjy`pa)>4G>6?$Lb5D-KO8aj}G{S$Zxfx<DrI8_)3gg>H$
zyo65<9E*VO$e*wf-@qSu2l^0ne^A35Mm}u&JPRH6MT8t`fopu*3Vya-uCjI9Sh2wV
z6_6u;Ywv5(fuSCQg2wp#MS@K6xmI{>cxx2RZc#Km+%3IYc!{PP$aQeMjd?~^B-2TR
zofl<?;FQKV60BY-SZof8@F%LL_66y))#Zaqm99*TZx_9fR9Zlt-sPV3IEB^-CCo+$
z#}*?v&#FC$*`&CwiO#blkML)NNx4PSf1^Rutusoi&07Pg3(OIsnnOXeiE&o?#uAJt
zC3~Z7*joytN(Z;dIkW$O(++}&pdq}9CNbt*j~UI8(r5qAx1+K1Je9dEp4=L%$Z-a(
zQft2T98F-zZW)(b`KD|J4|bJ7cvb1-{TgP}j<Br^qNd~=F6;^KEu<alv+dpTS}YMe
zgqxPRZaln9O2U(Pm2ty^Lkpq{oXXycOqH}f#Px3J6_XCMGD3yVB6-)+9Wn(lUpdZv
zgS_FvM&`cUnrz&Gk$ik(gqI3t0fs$;;XsQAhIj4HCWMo=dF9fjTZ1Q)G!=cDPPzix
zvT~yj!PZIKmQo4Ht-|9FNJ<+e+3EbKXSNxTjv8M`^Th~V<xKmi<1I~D<EPuy7HK-=
ze&S9)$$X7pVi`eSx3jzpr(gZ`V%Ln?Rv%~O(|A@Lr!%r0!uL|8roiE@v^Ze)sTFSQ
zLn68hNuPH9wx`YMdx&m+XH#^4p93Csl_C6Or|d$HTqs%diO4wP2$c&B{GA)5J(h4u
z41A8qB{hd0`X*xag289J)^SRow^s7Vd#7Lp7e&ev&I`|ky3Kaiy)LBaE3<u(t9{ZY
zZ}03=#*;kXHmByfqh14g!IL0w45Oqj8wR9&$*Q1SV<I_z>dzmpJliLt#J?MOE%?ZL
zZ&6C~V>_;$T^x8bsXMnbn4LLnD+g;puEo4k0*D&T`E!kXSEetT;Lh{X^Su?m;jGn|
z_SbKiq!&FT#~<KUnqB@N;to@6Lnj#dpNg7d%SLahin_6)=Y=g{FF8)n>DLTBQMO&Q
zV!zGVFUG|4HGC~PG|e#%%Iw3XZ&b|j6Zrd7Ur3v#nqnkM(n8%`WCxHOg!>v<&nk!%
z^b_T=zT;y1YfsIwlKSi85ppYKNuy>uEPLN)ZW`tQz2#$a)kBw3@PmQ#pK4+I!89#u
zQR}CQSoY^Z`5bg4OcMK^eDj6RHmjX9&k1N;X}6m#nR++2+f_OjNFJgWMV@8Fzfo>3
zy>d6b>$3rzCZoC(D1=&X%n%U%jQUNwRc^QKl$P7(Pjo;RCMAoZ*~g4axNuv8L)*&k
zW`v7E)frtH*J1Xkdc%3hRFvstn5$!pA86SdLfbT!*z@%)DrEDGaAF+EQ?gV^z6A}>
zyOA-Y<Ph)#%;9jeNy&u<T)eRmw=bKYjUr!;m24;!PHzKqZYj$dHp$}5!ju;2CsvNP
zo{Xwh(ok27ckb<Jb>sOuY~ar?uMg%O*S;P(=DcTDJU);t>Uzy0GP|F0ZAB>FC^ePQ
ztdCp=)}4#FL~cS2M)$y6F|fKQDWX;rL)YzZsnkgpBP%naN|SkA5>%9z%{Cx)6Or9>
z&0ztF{eZ;v;;ZEqR~qG0=g^^pym))~bs3tc56X6TTNY~ns&(gm|KWF$hVwTt#C?B(
zO$amF79+#+QYFjNEGG-*)peyEB6>`;ZCKp0lH;(u(JOzP<0Av~?tY=PSv-2jdYce_
zRLpAePjprJCY2W`0u0|@()p!47tDZ4h@)#z7Bxo?s_t#kyPe8XIJ(o3rm59;motF}
z&R+Ag%yB-ZSFiX-6B6#V(8wL7kg`BT7f&0+7>@^_vF_%#xP2wPmp{j#twZY%Ipu$X
zNwP1Cj&r6!0gePb^@~c`-o#W%JpvD;?<p#7gc{ql=fJQrR;t!AU#alJF2J|NCBH(8
ziW|1GX5lE&V*x<n2km68u;XiyZYkK;z^M5ri|nr6Z77?^l5{W7<DN^FF`l=#vY(zq
z5>0G+dpqy;SQ}0Ipt7jMqW7>T><oW}A1-<(yu>|6$c|k-#O!#HGpQ~Pw;SheiG2J`
z_7x;-{`l%76+T&Wy?$z8X9NaWSXkMC1Cz~7V$C>%ThU;8W;w4AbvQ!PI<|V~^wR6T
zCTAqYL{ciL`*LH~@V0!F6&MN&6^)>5kAfN*I2OGOJ4%mt#cGzGTo4|*cDS^b<X(Q#
zn-P&r^@)1A)r2Ts!lGK+S+Hnfk9xz{e^^s#rMILy;JIv55ym=e9b_?lNs)yM3_`7Z
zek=|$3$6Y5CF>iBQ{JRFB3EvroK!*4ZFQ{E<#~7t?V44q{=1U&S-<$WxBg)n%-j_v
zoSzhKdPQ$PIUKtzbt4~z5}O=mwq$D+6d7||TW>QkQ<V#+7pE{+GZEfR>{3G8g^V<Q
z`%DQ0kwc)`;tDY)`l)9nDpAeq7ZZ<}XR1f5%trmdGN?Xyqqex4?-U8ZmnS>1@%LQ1
zviMQ<qm!Os=He#Uizgv{b`yD8L-7lN`|z4bDAU-)V6&+**Xd%#Dnsju-pSU8hC+{b
zO;<6k0ks_cuDqI}9Um@HP&4v7#HGx}U6c59-tSUnH5e$%7}#IJcfh6adtQ8D$k5mN
ziiaI+{-2J;_B+}aPaSDRypUW>uU2xW3Zds~%og^@J<cyvdi1|(?kGBtfkKl6v3sPh
zEXh4LH$njT-k)SqV{4>(BV+)}T$a2JJL$^EVQ^->EfGBqs2a6uSk)@@YM#)802yC>
z_d6bIj8by7F78d46wc$8UFcw~6!&4B3a1F`OO>@<B28?pe_?-KxDLnz*biL?<I)#T
zAokHQIaYKm-)3&PgV(%D+FM=EXJzpX#N~-_J&koSuo-ouiaCn!h)N~ph{sUv$2LxO
z0oC<|+>tzbxTkx~^6!ft=m8En8f3EDW2AM>dKEwBwhCYtB%6~|yd%^fCri@h!+-o;
z?Ki>>CC&@JD}d8#%u!j$v`Ak<ij{d8Nf+c!?h<{@-a6pAj(U49XQde1beI3F8I>Px
z!u{9Hzn)(WyH0orF@f?XIJfp{D)_Ok-@Qc-DTa=h+eD4xg?GtxKH#8f$`De%Ue4Y*
zEo7lROd#sjlctcAnitXRrJHR9)h-DZ2o4=(R~R>cu>NFPIFl%sXPS|(l#g>vEFKK2
zOJKS_@NQ&!6{)e^9GCM{+1Y5l*up5!FX(p*%c#LBQ+FCvrj@I#GAJJG!C}}K*#u`|
z`s|8;RCnYhU$gF$M9QyK$7k)JizMm+w9=Z|CVeiGjH%0Ja9)6NHyusdZkfs1up|zM
zZ-c@PH8d|%^^trPz-@MK{@kJUL-8OT-us-5k3Hhtx^`1Ia4Ie|yD?;axbQO31n!kG
znjF-RA!g$d>!)%%s^D|w7CB-ha^_u!&eAZT8gM}gmfn;8sB+1=0K~?vMtfT9M#e+s
z5wqcVG2mv&poUa@<(pq@Z4{DYdcJ(^_KCPv(>#-hV}osEWYLDkSUAeWK2?}#o9K+k
zxi_WXgaf?A6sM4A-_pifCwIWN_EqxMOEu4YJ*om<r-L~YU9x<N)&Z}w+Csp<R%GC;
zJVz=isI@(sI^7b|VqT(&IIzBtOR)q^MZo<@^T=a{t*)${6dTO^jhjXMF2RaP?bh!B
zk&)jMA{DN~TRwBa51AH)1acAD$H>Wz_V-g^PnLAhcs|GQ_oKJ<pHjiphk^GkX5qG<
zXjMV?1YojLOHl1NEMI)>`gu;?qrTa%c*x~uO9wBVsci)%OjA<wMscq|;>T_sREto}
z9me?XHELUo4HngZ%ptmiLmb$PyD!K6Q`AlMLh`ML016rl_hp!^86etB+Lh#`xyKYe
zoVL|O4YegR$ts_NmMGskmI6z^3s+JTRnSxluyn)RI#SR86EzuICkD?j_&8ZL)erk6
z*!SCD97%*L!Bp4oQZ=z*K&CUy`yIJhD__>y(QJJ++F-5gbw)_KyGq)YwlB<Us!jMt
zvhF!+yBOl7nAKdf8v^18>Niq?Rg)X#`-64tLf2Qm9vjcyd^@DoV?FNW+Hxu3-(5nx
z+^&~-ASmk={4o#^hK5Mb3|$ToV%!ZGbXf8H&v(0wC`@NNlNXyr0Vk&E-+(wxXAxHL
zV*}C9T=`(Sfbv>v-^q|?FV#}~#%sY-wBVvVCKVRMk=lwlN<X>0D@j8xxnBq#*FDu6
zp&wx!@b^m?nD*Dmjp#S*MiO3hf1eeVaaXu@2-MTJFL5zdbR?NX&1Tavl|`J!U?V;!
z8c5{vZbEOjVD_&%HSbcNZdEJpuiJ?ak8N?t0L`<=+9hdeI48-fPy3bnN<}&FCiv2y
z;cXi*n4=DIVpoFb#Z@71w!FC2bJaW?B|8UEp$1QkYgC)o@ta@Syx!a}__X`oRMiVi
z=PK$1$%6-$US6klr}*4`J>)KjAJI1r*w7uVTvhmf%kcV`|9#lKbtkl)myR~BcpXh}
zh@rWYN*7=c&AAq89)S?N(L#d~UDaklT05g7)LhslF+s`S?8W7(mRKzTsjZay78?Ae
z&eZWWPb<gTm2gBgIK7(W^m|fB_)2I*ybc8dGjDoIOjD~Er~bwv5-&%lQ4RUaK~cA#
zxG^AFG35SYT*sy9If{m3deTs}8CI;Z+J6*&Eyxx;2*_Msa&ucg3X@}H4t|QD9q$Dv
z2#a<KDu2O%H$A!rkNApNCu<f|<Ph=6D%h$0aex0ZM@2GFv*wk3trDu%KsgH7O*LVJ
zca?ht7{;_>o-M-+GTN6(M$c4@kMFb;=UU3lNJykF>ht~%qV~l8W$f4wlx2}g$2r9V
z*+KU6(Re84VY1Zl6jgr|bK<38^ddWmzI?M$-QJTFCEr({JgKD|4%_HJBflLfEHZ~W
zfhm7J4Z9x=mxgY70K#-_5?CD1JY@{dzI}u<NX!`N>KMw;DH4~2!I4Hn=?d--dWkAm
zUd)<AW=Kr_&WuZD-nqKvuZZXuF*gQDpTfa25ez#M8gK2u&na1|q~9yo0qGB$q<As!
z4MD^7$n{UUbQCUSxLtj{ihgx==hXW&$mV(ZpOY2JVs5sc7e0<xzP@iwzKoE#Nu^ID
zY3<_}U1=e>DIatly(JvSA*DEkA93J46ZN0L8EJm0b`z~%98PJ8U&Dq38s3{S8p5?{
zp&ozb#yu}DeX;xtXVpZ3#>}HmIh`iU?ej4*l1bX}_-qQ_14XCswyUc0(=#Y>uSY%0
zCZ)o#>}j}c5@z>Sy@j?bF{Rnf9JbUy^5XY0E%kD`x8kEG+&%cWIpZ#Se=ysbKVH@=
z2Y1EdFEeJUxVO}p(vy$N&SVQzXXFp>E4pK1L^vYtriZxAVU(U_IpjtByv`Sw2r%jV
z!+}C_sBw8zxK!ko!1aw2dZe*;1pOJbOLZ!HV<?pMAP#qyscZ^;nTZtI4(;mAP4XER
zXz$eE9<Mnj8RK!b;m;*+4)sO>AxSM;x+|sN5Ny>QYb>MM)~h^QJ9(bI^BR2VKc1=V
zlEZ3o@_CdNimY0^@}s?^o=GnVCKkC8PH&$~O|-?#lPtrl6VBF9334M0rf-6tB7LSM
z(ZYsT4y_SF6B55XUdK)#F&eA0D7qnL(Br(HV##lF*xOkpdQ-)AmTkIJAKS;`R;_#N
z1`%z%2PkvglAF{432{%VQF}{+d_|~b2@K6@oO-V85T`d!dM>*Wh{(taZN$Em`bDDN
z=1s@UICJCMHHq|@cSQeM(rrwJWil&`z?C1w_#n+C+OCTJAn7Qqg3)fS+65Z$Hy^Ls
z4tv|t5s$<kA(A`QzqrLg#^_1h5+&2g8oMoR^Cj?(3j0uwokpPpcPvaJ@?Q#NfhxYt
zPp8>LxpPTE^H_#pJ2&}Z_YfDc-uV2trtF;`oZoV!E{u7I8<#JpCzA|5vaFR&tnr55
zVBM5%1Wn;4Rg!Ia4ArSExtd`V8;(u>3W_&Dj?M>p_b~YpMSJ6C&=1QmD)u2N0Bif$
z^4VOWp5lCih|LM^=K;%+>Z3beBo<?p?=0jRxMR}#JG%F6ZYT!UddEFTjH|Zocp^P`
zu*Tj)Nd%@?G=kidj9u%uT<mPUZ}asG@nT!*`ReaMl3+1bC}lD$dG_%<%Jk<sSie>k
z(3mRp;+~qoSCHH03Z-`&=8r$5K}vUvh{7}~aOR+=z~RstP328}72+o{J?6`gw5sTp
z>)8Hy?D%_YtHOKY{h6A7c*}$yQ<8l+3<ei1NC+Q$hWB*U#OO;N#P{c+x!2&Uo?#J3
z;~Oz1aO#-E12kGBZ4z+RmiOZ*SY_w>T_j7+F*@0PuCAAkKHIB4=n12K%J7>Ym~mE8
z*1kC3cUCpqF3uki6%1!*TWQJ;|CV{PMCA4X;F>AziB(S|#Ni~~6q*~b4?NqaO46v>
zTt-INZv3>FguQSyAB(R3w1txG3a9Z@Cw%L}(Dqr6!-bcJmT7RA%dqqGT?9$^BPNIV
z&rgpSx`|<IDaB4(yZY5u_-!n{)TBl2!Y^h$Aw4l>Q7?JlK&p!OMlp-tPKpl>%oQbD
z|I?IPE|2B+I0};e1Jc7Bt*H@{aaA@d=Icc}!7czcbT#18#wNVK;gl^}fV9^Us5o|L
zBIVnv4}VSu=ZReZ>OH;uh}rR-<-ik>iAF9-X-whLUFt=VQr9~bpkf4)p4*|+cR-+g
zI?|}>q<fD_ILKQyIxT{okEwIm5!UwNSv5wYeL&atO%O_4Cy3t(@iUJ+`>|u}Cy1ts
z=i3OHRD~335CvM!fMhF+<x2R~dYs<GONbAyDoh-P<)})&MH+o#Ek31K4Qg$sgxHSS
zYYh}WUAc=3Nbh#gJvN}pe_9bcO-NK|=x?32P4~oZ|ANpefAT<(F&kN-e7Ge0&`39M
z#*=m=89d=yR3~w|_*$T7LXDQQ7k6%~qq0WDJVeCQu+*Q@*jTtT#`{M2Gi>z4LFq78
zvy5TFQ<QkLB4ePnw-8ES)*@$i{Q=YIxQ;0ZMdy&>QM)cBrO4x8^T?deNSrg=fTwAC
ztJ+Dcc)SzP&8x7dm*e|Ohq*Syd%-w{NZ#d5iCThnvtJl>C`jtAuOEbeFnU)}VNBTl
zy(T+6L8n3q1BuWw_t_vCsPi0q>AQo6sf0mJ`5d+*x5K%#v=kzUWZ|O|yD=w5El$WT
zyeJZpC+(p81rmHZHME)Zp?$0qaxbXK;wj~#U*|LjJr^#yJ8prxvf@qWit#%#w=&r`
zKM(wvX4yaavV=c69rNJAGic_VNv^y=s6@o@*vZPz+e>LORpP>89{I^j0j55YoXkwO
z{%)+I>{gF`t8ViPEm3_-D*&lq#h1s(m7C!*ypq6rC}45|Nm9!&SQ;X297_ABzd)#P
zONpqrEMSVb;Ul)p*72Y^$Z|d!gn{I7^6b^+L)#0@i9_Fr_bHt4dLLanKb<x+gj^ER
z1OQcWc#`>`>K=4M-@*uBFHxuICbPAx7<z{yjr;ffC#I4%x0qM1adPBBGqr?w^wT^F
zsWlm}cBXJc4zpl~-vr=TmVK!{p;EMy)pk!3|5~~G4aBg@NDe+9)P8*e+g9OX!Bwc3
z)|007xaM-Ihd=!1um>yfjRa4wWf#Fgia=b>(X0$bl^vxd(E(pp?H+*)x<5w)!j>cO
z2g$CG*0_pUbK=S45GvDF4xe$G%}JN`n3%Bb7F~%2?Yo9;A$X9NW0O{&`cdAJbMi-R
zQ+o7>JpE#=3z8^)xwm-2l4<pHO3(AjhH!KbL}TaO*tKdA4rC?B(j4BV(sQQ}y@M(8
z&hffW`}i}J@WP>>WFv;2-+_`eXxnb}<?}rP<?l}WpSDGNZg*;ID|oA*=@xgbb8J!X
z{2ro0s+O+1q4$gGc&wS>kU;$&?%+Wt(Yr=-X!vcoSLNSU)Ki;G<4BU8O?Bz(RwcKy
z-*e)XujhGKnekek-Ywwjlg#bc56N9`Bu-A>uv=5dVjoia^z})K?adG%@|b@8Ul4Bi
z+{hQx;G;Qz@cWiNf3`b~9Hdoao{d~O+z0?yd#6Ze<dv+rT!wC{WE5#k{Mw~;m6bHu
z+@1DU=tuWg_&1acilJw}Y4|X!76f<>{NET?t8T;q)0=%2gq~mV#))wqHkc1sp3@f3
z5uut3!4fWjWSUanF+C_-6XNn2X7)wy=7oM+9^7%J1~2U;rx~+(t+iF|f<8(a-P7Ys
z19+#RJNxmHQmA$9B6_09&m&jP{oG0u8}F0wUgXklK+KGO4Bt;SDHH(nEC1P-A2*fv
z&<vO15!&78LDyu3hb9^+`DC?3Ka$O}pN8Y3!JY}XY<g8YBUsF_KAm%d(hrIpp;Lrh
zs&Z51fAzwy_UKCJQ6<ku<)~_uXs$RIc3-e3@SGnrr{8g=UE=ONsk&eaJUC*L7NjD^
zXR{MS5UpvXCG|MNs?0>E8VL_v8<^lrkg-R@54A=g%Yig2J+#g8i_bxM&P7UTejrIH
zy-uFYARx!@n9LeXuB)%Qwl4>(Zp~HKf2z~b3OiLasqFSRBUYH41&jKo=c5#j`S0K_
z`@gZ!{~i3*P!m;C{1^QFUrb&9r)<m0@dJNZ{vq44|NkM|{#d#H3;6p_+1AA9N1`=z
zq5mHj|59shE$v*LfBq{@mi8w95PE;KT9g08!vBE4|A~eF(P{nvY_MkHU}XOvAef1O
ziGhjz#{&L;b73Y%4mOtmLoOWi462a3F%KHG2jULhBIMxybF_leY;NfWb&|A!z}oe1
z<?dqP1c5_3t!$g;=jMEO|Mrgl-DiQj)#yD<k8+)|gdsACD-)Om_f!Kz6EoQbzzFK<
zie?7D%#2OU%uG!L8p<`gg!ZD(Nfb&py9Rn-F!LTBn!!9fh)RRZ?g)xa#jy%(vSkBE
zV*`LDr-w$TXJ!UK&CERG4~v`Q3AjggMc^C&L@cn}k9QIzL3p%#atPDV<OF*BdYuno
z#f$|24G)k0xpM`8$o-3XbaD{ym%*MDXcMqG3v)eaIcG*-K#krva!}UJ_|j5hdgR!^
zK=9n)bm(0Fno>d--Y%e1Gf+8@Zou@FfODTdDKLuc{{>1wwZH6u|CGjvNC8l{0=fK0
zq2^%W>S5vx1b_v$ATyx73)scY-W=!*0A~lN$;bng9D(-#F_!<20X^WK<^Wh2S^f?8
zkN01JK=yw-o0yq7*g2Zmdx7jN0Tv)zAV5h{p3&9Ql^$SXZ~m8|iLHwR*x$t61Y~Ps
z3O4wgx(Ps1L<L|1uJE7wT+Ez7j;=0@E+E^#YGnE=47kk__U2*^c6LB}R~N*;_=$s@
zfo9;odolfUxHk3<9`-)}MHV1?bBn*qFn4og(y#|PxdCOw|FHp!5P!=ofvx}!W@ct?
zHdX-82>|povts%yfx4F?@NXr{Ut(|#em;&4jsOdA89+af1rYoT(Z|Ka9SCrBb_4qP
z{Hgd~gvi1IFbA2r0!)FHAbZ5$(ZOP%#eXpP{GCCb06k{#_^|+(|N8m&F9Yy|nLF6q
zdi^&4`-qvu)ivaGbm;ym`Cp}|sDmfKhk+fyz{<u3;9%ndaPn{h{Qft}zxDsO_P?dd
zCZK<~{Si;b-ogRE^PdvHz58z&?*GgM^*>8P1Nh$ziVomu1_G#m55GP$2eTRYAC~{m
zEAY3=|G&xqE6V@t@c&;%l5V!Pf9t9LWAOi@H?afRdi`Soo@O^!@Mb7DfcL}x|C(w6
z|Fg~tKy#3r-T&3fxSD`BM8w|G_TNT=TqHrBKyzh~tC`i`6Z79=07th4*#ngwTtI)_
zHvk3}X6FB+1FxK!4fsxR0Uy@iDj;~H{x_q9y_tjgU#rK;!3i*Nb~f=sWCqU{D+dR_
zhXuT&=0MNCM;pMzXz$<(b^(B^^8;8oI3xabq?{Z8CXv5H|3RDpCeh!B3&14y8*zi@
z?l<BAFiHH2xR?P<lD`oPfJy2%Vg)cs|3+*8CYj%e9l#{}8-cmV{YGFe^1l(7i^6XN
z=A!r;fw?ICi@3pDlz$^I7nR=#%tiG#0&`LOjlf*ge<Ls#jo%2&Me{cTbJ6;Zz+AL{
zBQO`8e-RIui|%g(_A~hxv4Q<e>>R<n^w%}{Unx77p@|E4{6H=?zncxFX5#ug4%pN5
zHv)T@nwZ(R*qXRl{VQc*1s7-PY+?qq1zNcNA?El;{GY}AHwepr#5O?J|FP#``&a&d
z9Kdy&{fjujjLaNt!9)CS9qfNe?CgHm``2_c{VtRZ+){G~TU!(7KP<p3fWL!+Er9=%
zl=CmWlN<P!_%{leA9%WLP3-=N0&a-K?<imk3y}LCVgFJ)xc!+HY-0I430QCW*V7LC
z(*#`3@8+_Dn`q_bXa%(Y!vZV={Q<$`t^a`Fqqq43g8TKm2u^TgZ2wyF-+FL&|ELIj
z$20v-#Rkq~?`CKE*DY-M2Lte9!{qRrBRB;3bL0;-xGu-vYH&zL@E3vo{~R|v%Rkcp
zJ#6qp!UP^v@XP;)Dd*oZki#Fd3+|(%t=k`UfWMlUoPLJ^H_^$>!4+t3`o{_K{3HC|
zlq@V@<3A371zhy+z#L#F7oZ*Je`k&Z?BW7`OaJ8qru~1mf(6X_cM|Y>#N=w_4E$p_
zz-@8$aQMRk9K-GR)Pq_4{n_MV=HUEiZ^7s3{s#op^Y~-uz(GC#fZ(iNe?aiLc>m@M
zj^qt={ttuyx#`W^oWTd<`uAfDeg^)F|Ni0v0zH9dh|7x(W_+R6b)jGHzlaceFzihV
z%u(!VWzaDAEIW6&J)**=($wSxZ#w@JNge4$U)z<Sz7<{}efBwNZGewxOIB(B<@;om
zs5-OPj<__9J2+KzB2qs<j6lSoF1+vi?BuH%WCPs_*(FPn=j6tXs$76|@6j*iSwB#|
zIv&2br?OweDTnk_KFbiR@j*YRbe<yL^y30P8JsHv@f%vq3s1E9+n;loxl^yCvWawv
zey6c)#Xh=QtZ@$u-s_d>tS;RI<OI6-#L%~x<1>^#qK9d+c;9@=N<M4~eEY;q!w9iC
zK0xX_*7fd;S5ICB6OY-}d6`FQoZPRyo+(JX4;dk6PtDkow$AYc^&38&g@=cvRDn`U
zNx$L#a0J3#q%)nlf_pr?qL<J9_A<<^&dIIZ$ywc$p7*Gp#SQ}E4$fM$=(_lGgp(h(
z0vyrJK6kfok#+WG@>-uh>#0-6InRAhwT{6Gi63sLi10<JnP})(MOjT;6eEeZwOH)>
z2sJ$IrVt4FJv4eF1S^sntBg*U`!DQg`Qy47+;YFXJv@jSy=+saN@O@pF`G%P%@WUa
zaVnuuGo$H~xpIpZN6O`DdGFr~%dVPN3(vGmo<dG2L>%1p7?tP$GRJstNmuuHBG7y&
zcoGdWulq5=B%q}^U5hAM@fJH;nnQ<aWtU&KZm{UEC_iE#84{OJVxm}YY?3lBRtBk;
z{cz##C(4KKaA|1xR*p6n&5??FRXG&6qE92ES2dEoLYAtTa-ZuBq9zjf)#j9gTe@G-
z;wy^<Zp(M{#)<lpmkqwW_s<yDo$JwE@*4FVuK7?GQ9Bf*MJcYAIClh*O^MmK*S5e%
z#zWK|rtJ6N*O|E4)i+~uyJ*d5^gJ1xw=yP8+PH4NA8UAgKj;xZGN;fyN9$?VvVgm<
zQ=N`IE_k|z!sxt*Rhi&u;lnK6?aL`#agclTk<Z|msixD)cWRpPzEo8NW@F@w+&r{h
zj_pswHA=0uK)jgsAs>?Ff{SN6$^N(N=M}^s8`8T7R~9ly*b*e={AF;%i@Ahx)wx90
z6n-fwqee6G#jPiNkRGK2g=^P-;`6IZy8nc|Z#|%+eJk<Jt%CGxA;(a=<7$Ga%-53b
z{Ae`*?$hTyE=QG)SaxPJNa@V^1@;R!y;BqqosPb=tWSF%S-<MJPS1{vrmYlyXvh(=
zy2*@gG0oY2p<O11A`e6~A1pSGCI={oA%|s0@dlxj1?OFUriuIPt1)vv`1$061s>c(
z1(tYYk?TGBym@+WX`Vo3b`HnH_16G8!lt4h)?7UlCm`Y1ble@RVGJ4q$eGA7btZ^u
zr^vS&YtxcPwRZ`vEGe!MQ^B*%>jlCnGX;ToEChXpb&`gvLTL;iQ$JqymN?;t!W-ti
z`}F~emc`QJ?tryBpPi{BFSmuQOSoc3{|lG*s)_gU3=7c_O;j`ER_4WZP>h6Uuntw*
zaQWRaN2H`#fPlSqC+kkf0^Fh*_F;kt3v%q!{rDkUplPm*K}1e;k5@|_Z`}BI6pO|D
z&uhmq$$Q6_CBMonEKe!A9P@wigDy#xNT{iNKF2TAjiGu}yr}J~0kf&N#a}PSW(arc
zS*Iqu_x(ujB&z~N7s?#wV%rkWunb{BdeVRBmgK@Aoh-jnr6yE`qe3eA%t&TL^_gsE
zYYhkex}W0XSkEa^wiB5jrpwd(HtM}TT9}k=hD#cOZ_H+>j*6J8?2btJ_RE*up5w|y
zgF98#>~gVUF&M)dj%S|tHzH@-bV5U-C*%ifLo~<2Np-xX)LK;g^w1FyFQNld<;VmK
z&Pn6y`QfPxxH_Of7_?APdYg~XWe1e#0h1LHWZ7<)?aMk_kDrat1u<p}jY%LHs8g%v
zsNn1}ABmCVsL<qg=1`~vb1`xxwv-j=c@++QqSe@>SXEJlIrVe5gcvRs3~{w%sRCCI
zZv!N88J(;$`e5&L2y?G~*#?`GmwAjYD^_bbPLvu~Ro(1lDe%<7eK*3EwVXBec(;>|
zJpR?CKWJ2aHU*+eyc^myc!XEW>779UXfWTSJ=ys6u;?=k{0FMNUgIq*L%3dKb@h0e
z3VZ1&pzZgj0*sD{q^r2QzC2^h^`DEoK^cKRzpVqP%19g|H&LktU@CN2TK9ieMCEC8
z%~X~#ohm)Tsv}YuQ=Ss}gi_<n4e&CGoq{IhcnZ2ar3V<W+;=3N=Gi@sI7lM*;`5f<
zo}#n*>2E%jn(x?hP??UIt~=4EN&jR7C?$V6<Q~{YP*k+P)9shKf`xZ-n;>Z(s$En_
zf$F^JvHW#3Z_We_#R%_k=1<$-k%qb5)`>RS1;lo|A8SNp`BCTRfEFkRaY~(b?j!Pc
z+C^cP*jNaFf&8g)%#Q%w7xO9y69Ti$aCW7X`qv6emwT=Pr1sO2h;_m{O_=Kt8xDcv
zLCS2M{8JFgvd4A!m@i%#{>N~4v6>iVp!cpMOzlSOk+EdRjx4;=yY7k?@4@_`zCsB`
z8ty`o99Lty^ht-~<>Qn$)m12gA>P==`|o^T0;)`Ct0&A4G>{Tt;At3LWr(bt62HAf
zlF%|k=Q;fdY^x2d5j+T0#2L~+7+@PD9gg4PLhBFYQF3k4WAlx@;BaP!zHW6KTzdJn
z#0|fhs+B?HBrZSD@ioq8?aa6iJ9*gLBZbiSTPTW@?5j6ZbIzeORi`d_Xw))9=90o>
z5qE6SR$Tm`F8X+69AwcOey&ZQ8hOVJjJ`Wf8VW4T?L(W<HMWq@(@b@Tq9-wXKX$=p
z-B;(~FFx(gcs&}ruRIdvxr_5s)!S}~>^v)iTgDCqPMs4>!A0BtN2s1VY4K`dzBv{=
zq~9uqy0tGE-G)UyEu>5(o)|r}_<n_DI?#6`;1!F?+t|o8`oQy8ys@-=qKC9S@~xo!
zN*FkckE$?iLR&ab{xgcW%l-XYli(?r#;Ms+z(ijYd<6tmGZc1&eTyR6bmk<>hpdAV
zt&PEo7x=Q}SIsr#&l!qtaCAJcD`@Oo7%^T_Xw);kFPobvWkD#t-O*%K-lIFH4oba7
zDZZ6vx-UfDqDA{5!p*vYBBbk`4_m?}g+R}=Jwj8VgabjcNt{&3*AaM~?y&P8Ig5Bv
zN9rY(q7Ak%Sj(|I8qbo)vNb|vn0k%|y+|vtqmGkSAC`LZCcBf$F2gmTxC5salD+c}
zHag|Qh3W$zp_1{QcuC}9_^NLo^R-?n#<f#IZ(+!nB6#skM|d1}8zc?QvqUFJ7G+hb
zAvyIJ)M?My-{r*4c<DeGQz<k)Q$r(Fy{S1HRlurH7I{Btkr=2>W<m6U3`Piem;Rz%
zN|~hE*)K74f@Dy}Dj#?p-VebEgM2AuMZ--wj>9Bsf^HG=GcEZYUK?{{<Ap1FvjYD>
z474^gq<06)COIcLPP5;yAT9n)7VACG4j1RaGkGU_EfDfcLp|W&v-}>t`((blo64a>
zEART1z@;B?qeR148TGE(=-31^V{%tb9Loj`lKC&(n2`6^qcU(4eC>hC26&hqBaN4>
zxT=s{FR4$5sckaQ^eJS+LfIdagMRLsisnvD4nKY4_FY?*n)<Y{ey-2gCXI|}Ke!EN
z#u~X<<4LML35T7>@=l0%3uhanHEj<a*Y!O~-xjVAMeEF6s%2=jS{G2yJ1ZAB0TJd0
z7~Qm@Nq-bSMTWL!cno_J+JCAP%ISXV?N*c^Y#M?PGk^f{Y%m*KH+Ejdq;&2+AVNWv
zIX#BMj3<a+X*CkWcoT%eqIRri4toQEzo$d~Bg~klsJH5NrXT+sFKjgKkFHb<1EhOZ
zOoM4jnQC<#&x>qpOVdx%$>whusd*85cFrquh!{UiL#0y`IXZQf-w5IcR0<j%d3(~Y
z_!uTlm>D={?{GR)H-|U2-sTT)RJuwhtFMVYPkjTVD)CJ_?F1QcKagX4wKR)HI><Yt
z1o{YAbT?I!QvoFJ2mEMnLnt0-V^A?4%O73MT?%ls#UK^E&6?V?N`D1&Ncm3$EcLd0
z0s@X%V~FR?D~K>5qjwEVUA(EfhI#j<OteHA$}<Jo@#zqsp-MR1HU(r!)UO_^lBFYY
zAi2wQwwrP7Td=05e4c6<g3!L+Kfd|o`a0{U{B)Y)+K*FED(hLF%KdG4PL*hNn-Ndn
zXVlnj{Z$Vql905q^oHW7m8~Z-{wa*BSAW6TJ~&)ambCHsC1&a>^Epql%BHdosUG&K
zQ^~q3z2j~5=E^=@1H2aS?ZULnXN3jL^cb4*`;4^fo-O!t*-IVG%du?khSP4Ao@Y4o
z_p<UVxI$P84Z}iMWCPizrQ<3+s6^+Ttrlj@({s7b4IMQ|{Fb)<vFGWD96B{}QvyM1
zdzS_$=7_?PMS(`}91GGeTfx6>*Kh`pnXY!)V#aMWiPqirEx!k(Jm<$O{y_JmVz*75
zPT3QFsOd6NlG$wKBFLU%@<}j+@%Caccb>raM6n>u?-(b2#N?9QoLRiQ5B)aReb=D&
z@$)WK?ps1{AA;5s)5hXCGaF^oH~xb`5KD;6`L(@$_azhFgu60xRqsMk)I(ZgRLjZd
z=!;2$%TAf{4n-k~PIrPRB{JZ+d+IZXo)i!~H12Xv+0&O+`ss7FKqV16#+$9Qz$F76
zx>%<hg~TG-=8yhugx%h2ZTuTvK^psF)578DI)&kcx#9+GW1DtvAHyd(fB7jL5lAqq
zn>OlVEVTM(VHmMRYZf&8AjpGjAY@lPB8w-+EvlPtJigdQMMe4S5H6b@r`1E*h12S*
zrAz$~dOR&Hk1!&<A2G>guGCk!KN{+!6%%ffctudNR)zP8*9DIP-kpS&Oo4wKjwHkD
z=-ZApkD`n|Y{no*tkuFg8iRgv^uu>~64QB{9{L(fC3`AvoRO#D@~Y~0cyA@<!>Ra)
zU&2^$0TH|5Iimqo1lK0Ig8<tDqO!TlpIi@Q`IDxB>a)UWWGaLy9*&dN**M7yR@y9g
z^?X#fWz(<?h9ypD{8QBD*8z_`98%p#p+IhQqXd&lLnJ~{BLwqwI@=0$G9MBU=NBV)
zXHBJ5+=B2okO>xWX$QH%<yIk^mQ0zR?fG96XU+!aG+2eMEX96cs{~bGzjs9R(~*If
z(+hqWh@|hFyA#PcS5+P-g;%x#@!x5wL+&LPC7^7b7)QEJDn%wK4`$hp$NyCKq19(r
zN17m^7#3aG%Tvrlt`mY~PDytpcs+ZOzi3F&Zm30B*?167F;Rru6Eq3JbpFczEqOth
z;K97JJiRC7z`ce8yB_MeEr$b$(=TqJO4n>ApbUa~b*Wg@PUI$j$(o<Gvx^DLTIG|d
zs*ykLH^`Cptx`+w{Zbpz40wM#+y&oNdT~ES+)%oA@TI$b5Ut$y0QI(x@F8;-Fnm{b
z6{^OGiu(|~e*rA|(bM&g4KnbJsE)cNdIk*K3S6xpu6n+@NycPkaAa=96qE%{_Pk|o
zN&EcEg?5P%oofn9J}ZsLBC5}3+5P#hS;)69d5INe-paHpW1rL2zNzk+(~=(8DxTpj
z#hYSTk3@XNhVtleGa4v`#wTLl#Ahou>ayz-fMz<}3{bhesduxu*(pH0q>@2Ugehp>
z5gNgAvM#^Oq(K~wJIR`>qx;m9FM>HVLp<$Vu+#pTlTvy*MT}?B&KcIXkIT6Z^IGbG
zf;Zwy*7>E0e}lThQp&KmzoFj0##8wS&M;7JXJeiKuV+i=Q724%ESc=SC50SY$fB8x
zBz+}o(+{IHNqg+O!(rqB+fxqJ4xM@!(3k7hKiBh~N3t_%5pFaYkc#bwIDVdy0@{6J
z1%b$;=!!u=oD{|r-0lW4+jp7xE*t29Xr}Ai)PoI6E;g(;LjsEa2D}JCnkBn~_*J*n
zImq{x%~vGRTXI{6{ihcEk{XfpXol=|>8B$a|5t2^oKjQ0E(|51zRLkVoLp6ph|Sg^
zi7JjlA+k?ed%l5u3)M25KVqR$w4tc<&7%+H9&TLxSU(W>sEE*byp=t6h1cGS!vV3m
zeNe?@gHq3S;z%_0CGkIbOyWOc{ngDyU@GDl7L=R$wpd<!UWxPI7ZW-alrb;q)cj=!
zR6|!oVi`#rB+ezb<p&u^@>?nT)YowZV=q6c<B}Y_Q6U<$q&3)$>$I#_u5CG_cif_*
z9dWjhBaCavP!6{wQ@D)_ZjtVYa?PP?LKDoo{N==7uM`YFIY>ZlFP^8M@M}k!n*ild
z4E=|;ngi(&-l2AG@!LtYy2pB1GV=(QHjt699F?5uR{<w0Wtx!HTrfsNWz?HLEX=!K
zVJ-s?8}msKRNHObZOMJBKzFCiPL3BHh$@~CGtLgX(vK>U^_lM8mj~GcAs4{N<xBQ>
z<FcV~;Ahjjj5eVPO{y9b&kGz9<X+nC30#Dse8_hH7==ACv73UZ>XcjNSu*D=LYM4G
zHL>b_D1qKtuTkV*dh@^L4f)?Ol(3M0;gC<+P{6>DVGw-5T}YAn0l^Ys{yqm&mgfg$
zrc69^*!x<*)PQB&W|J9hyC?=EU0D^*nxvM!dANe#l&X$frl~S?b8wY=)`6Ljk)?tS
zhpt1v!#ehNqO#ojkXehoaA<$L%`>T9!TG19#*>Li`dsw9%8Xr0n-K#j32v-rA?S?U
zC1aCFMJ8iMxKgx($(<usRuFXAqw5#~HIrVy@UAbPSji@Yfr&Hc?t=C$N~l%~J=9mR
zm}m$HCETA;DyLC8#$^Y_vPPmcTO?Mc^G=sXSlr0(_(mA7riKW+X#2b5749mB&JVIp
zyHZLN<OtGDd`unWvSs$|XEZ_`S_nNBEQzn!l6n?meG^qXorr}nE-I)VKcnqKsy!|`
z$Si8=_rtFmfELv)hht*(j5c)}jZUq@iK+`<QaFlg%BU2%J<t>)Dd6cOc-=oBMvsz(
zLbuG7pMMV6PBtm=#O0gPL5?%7w?n?Gog4(lUY(zRCNp)}0yzoN_rKM(A0_T9%Vyi}
z{Q#UEr!KCw99(kvRf6*sAV7Yq`q2EE+xW6p`3PH+O9j0n%WGy`&Q=m^!HTGu)X2*w
zCYHdbgg)v{#vgz?TrVQTBhWP;VUje5KkS43_^2?}9^Lel(ocTh-tA*a0Ddw1YObc<
z_!b>wfl-+0aT&Sm&x!<=W5y=Gk;S389<!OJ8lUeK{QTMOr*~^Do4DiNi#HMck4O^`
zmubV(h3gPJ<IZp!_>DniIi|s`O_`$^<X%V9<%{{6rvL-3H2mJh*?Gaj^<8G@PZ+lW
zq{>C2NzGcc#JXZ5gW1tapN`dn0ufG_>yH8#Oycp{f+rWd+u-)pLTfoDQ|}o3xT1pJ
z%n#x&GxTKB;<d<#Rmc0V47Y}hL}zU34%B~#uO@I-N~D&0gY?!u1Cw)?6#Hv@0S{=G
zW78Bv{8{m?j+BJTL%tUU1#*ubv(B0<^$0@Cz1~Y<($vB|yS3<nh?0IGTpGSQJOq8F
z>8q4putnHeKMrN`XXS3smIg{0O#H0O*OQ`nkN`E>SbE-}H*c_>A0UhT_)%~XX2zf)
z|3Jppz1mCpX8Fnd!<-%obg(N%oA->zcfJ`b2(x0Iw3;JrWHs(nOrMX5s0$x;DrMPi
za9mn1sK}k^vR$*DBlfvyLTNVbC}ZTc##QD~cICN}X{zcie+D)Og@3zFoJedUV~p(n
z`f34)M3=~ZrrkkA`+}!1`ZJi9i>T(-`mJ4d1~hW{yGE)Km@aI#R!{7HLVi7Gz>n`0
zFh8R52u|!1)(taV+D3SD4bv5T`gkOg2b!^Gh@i%~XVyh`UHMnE-k;QKxvV0vCHeKD
z2krYCOeu;y&-kCm4gZpC@3wiJnHgtu8i=HrWA*`VxClL7VKnafi>pWyH0QN`GEdh0
z`bFs-_&-Vp6QdsYA?T^J#iR@=_LJSu@$|CVW}n-!;4q(rYNQ?&<rQi5f<mY*kgqAE
z2oGfui;<>#=rm5>wJuXhr1RyAYl)V0N6Mk0Ox$HMFJ^ZdL*7-Zoisx^_i|gF;Lh;n
zIjPTF)MP8p9c#)2$>OP<238z78%v+Rl$))yV#iqAbXXr(R>{p)MQgCuGII?RCyVg8
z_S#htAz&*l7JW2rC~r{%O#=s_-TBn1d>@*+IxgJ#Il{%!mAOL+B9ESHDAQL0;8R*_
zk}L8BS(AVK()z*=$^Yy9TZpV2`a!Y6{xt>Pg|^3KrS+;|?Z`FQkDWmQ2}d8L2A1F$
z*VcUF9@v}=mJh2RcOx_~i=hW;F(JP%yWc#3(j;t_5g=0*J{@|mM}(cJZ>B8o?(T5B
zHU}nPCBH;ezR%Sh=ZzpN1#&hq01lR!!>t$Xs?(=Z0~j)!Dt@s*tEKTiemHAJYd+Mj
zY2OG_t@UT4*Rgunh8|kJYNs4e7OI6@V$Y`evYC;Fdy|I&l>EGG+DnP-ByBFmLL^R0
z?jn;I$$aJcNp#EH&rF0hs-axj9GeQ|`@`<iM|-c5)GHbMOm!sYs~EIc?TdX8hli!G
z>PZCHw0&%;^#?UwfakiPGX5(p<2Z85SX_dl8^@Hd09C%oFTu*sRGF%;E!ZzAjiaB#
zoOTs9D!J>+s>SYtM~1GCE39~U@U=+{gz1((!HlVxod;Y1U!n}qbe8x0hJQxRoj1=L
z5ZYf_yj3Xm34TPO^mhKnaHvk96xF)#(SNPg<MgZFD+Ah3Gr>ZZNZC*V8{tA*&LveK
zzAdK(OA7)TP%qq?;LhsTBS=k6Fhz~Dn*__RNDx-4;lUvhvkC7iGd^@;I|k{zlUe(d
zgTDl=(HretDcOGqPa;IBQgse}AbtcxHQeV{F&5fbLQ^oG6aPFiI=&|;hQBkO5+K%j
zscWLSxA)xDu0(Jj<|{(;WYjjE0Fy($P*sYhp>DBOfXE~(#=0e!+6R@gN@$(F4Ln{u
zg|vwIG0Xt77+IGb?eg*9fs1yF7A{F<;wwGnglh0hnO1<DukZ8}o_UBo>SL4J#u=c^
zq`owL{G76dor`aYQHRS)*INYvhx4_X(?BT3U!lqNbA{_0{m_O!TLU$r()snulif9~
zxM$f`xL4K_6dukfg$_xJ{u=C?gO@MeQhp_RSNd_hP%W|jKJPR17q^wHSWx%NGsu>p
z3>uEe&1l}fx7Bc1XeS})`_@;w&ANhLqcSucgIe4E5>DMw`22n+3XVO+K`h*u+G*(I
zK)l!h==tMr`dI#qk3|Mrq~6A#-d=(!kLh6Vnn%1<QRMwK^P&ItCM;@7t34`=Hk;$}
zI{F>~)AqsOIRXN_LYk3_`G+Z@a5WK$J<`{{Q!_?=EXE7355gOElGU>sIbnFdES}?(
zrVr4cE<@Ga0}V~TOQmG@kswI5$fB5!MF}#Y2n{mqzE2F$1sD%D#Pe}{u(v!^tWGnY
zm7W7pH%X!plzfeTNV%|LiXoJEO1+U5Itv_U3@obQx684PDaOHhj2%h)bZv+<v2Xxn
zuOl=lQTrSP-!H(qwhyPx!{1IPsc7vxqEJ)v^%)m_`hCZ8BTWKFnLkF#Wa?@50Jky&
z{%Mi;`MVhXJ!cly`YGq1I;9Oax!sw)9;Zwd>;<kEh~(RW({bX=7KUp?5J=fy1jZe2
zjJ`+0sFz5bA{3)MBSjMF#>Z%*T<<mSpRY*YhLewYE5Je+qdutf^Cqs<X+@Qvxdi5C
zP-fUod}RWz6n}wEoefy!SXJ=570wHjAEfVMHfHQ-4>D-Q8Tu~khguG2aHm~p<EgKe
zZTFoa@=F&aUj{B#kivt*a@157i$xalk+!pD75l)m0Pk$r2d$*KeKF!$xtFomMmLi^
zSJVX8*w;XQ>*o-kj-R7)zUNGgAK1RvpEje5CWkc21YGCR;#R_h4>2#JC*Lp%cf1gY
z@5Q~%;CslDO<YgPBx~d1jDEN4&4EEEmbf}l$r;U{*#SY1G;$WP=vQsqNeu`aORN$r
z9{t7|8?N3A#J*vuM0!zGNoVX*8X-AN`QeMBbb>ZPU91s#^yJ!o7(U-&NX4)%THKQ+
zR_T!Iz1Xz3^cv8HX?0<%_u55=XvOzBQ%jQk6KyKYNpUFu)G^y~@OWkUp#34eU(_(j
zT&spSqI40c>zg5GQ=f1h4JB4qsDM>-zeX$eMzf=}OohGLE+>cHCffAUMUa?HU&*f~
z%e!B{x_kD?)A}d>dLm1SIhr(g@dXH|*N+U7d^?4s`*E`dx%=4Bw4SVMGcx47IO!%6
zJ)o7bVB?^6M?1>YE2HQXv9#S}Y9i4i@~P?4p`-73KeSUItIga=lr?kkr&NP!7Tcy<
z6VADCKoFuuJTZv+?4Z9rn<!*hxm&L1u4+_(a~dn|**1PQ!SGbaiXp0;ipGB{@cq0V
zZgv=U=I$ZA#p)cexEgj}?T4CP5%muc7M}Gv`>1{4jnMFhZD7FCcu2XtZ_c*Znwk`M
zcq5PjGU%?kYB856#;JL3HQ?YHC3R0BVz}B>a;@Rqw$wYOx0nP*u(-JHVBp6(pOiEO
zx_(dylTQIcUhxuew~$Gy_D4_*{z$MsOrAT{iptc*t{qdd7qPN(4yti{tQ<oi%{PHk
zj(w10U|DmIm-wW*X86T0cP6UEc}3q`sU*(Zl%eyZr)-1fcRs14l`GCay7m)9{XW~B
zJy&Aa*u^__HjS$WH%C#^DYlY+5VEXwcf3xcF#G9dw_y9_U`|rVNX>)uP;-9zhePUD
z5r!v)^Y?Eb#=m85P~%{cuMSWnfO5$o8j7mxJ$1gSK9dw*uNb6o=>5QM=J|;tF;mD^
zVV7@=rOw&B39@=V4Gv`QXkfpHiaEZ{5gZ+$>AL9Cq`?o1xj|MlS7#`?tLcBcZ{AdB
z2ltqj_d>ulR+OlPGHb9<)QXM99u&wlszk(wjG8C9!VpKL$xT9Ie!Z*PKM!9WUtN6h
zsocdcHtXl`$s0XNQ45pKun$;S6OrXvg10+op+zb97>De=@S@-Io#oDCV9z*%jLX9)
zB9EYcew*{awr}0UT)OYCTd+iUOj5JTu3aX$HQ~h_rf8hBy?;o~WDt5cPJ_`;Z91`3
zvN_7<`PFpe`D!q|*YnLY;_<+q;ql=zX3c8<^4549(1WvF_?Vh3M%MlfRr%rmr=<dc
zyHj3!OL4iN$|bI6DK)iM=+p$Zr&$fgv8+3;Cek;-Sg5t+*!8c6@t{0WnN3zDAYM8t
zOfi{AIw-BMPWD`_N%pjbjc(ar;9^p#8*#bZRg)5Z^EuzQa|yLrbyl$%t{LNa4qN(S
zWi3bD`cUXa57<%uIhu7eEREXi%f*OX%3F>eFSt!pWpAizvCo4$JKfbDaxrd*zDPV_
z8||Nr!k3LRV84@!Q{prla1AT^=ZhOmzXtp7IlJCE<~^0bm579;rkp-m_%%QCmNM2J
z+3b@C!m>GpxM&qJ9B#Vdd@NVZXt&cLwN{*LQ{ZpI7_|T1jZNVo*w7Ia#itbex_VAS
znA>{WyKW02d5*STU;i=k&i1ZYk(R&((O62}8U=Be!=3znbaO%ase|VhkE$-j?9NNf
z;RmRLVto}ukWReGQ3wu4dbL%JzznAi=(>8eOuJnr#r($9R9qtVVrfEnx~*gbRpL-<
z!yRDKKGZ}{?7z&ktFPj)p;V9=DKz2FE~duL>+=oQeij-dDIn}&*qxwv71J$K3N1sw
ziw2OU!yrqauH6Z1pTe!A+#tqBvH4x+bHO0;)M!!X_bl_!)ULpzb$K)U7N+&Gn>i?~
zWKZjcUWUa16``LC9k6pzoUo|+?eE?--!cV;(>+Eq<9ZD_=DCOpuh!LmLwvzSLzKpT
z!%;n{H}QIJ#XBz)L&mD1(irDwwy?<`4C&LA(hovCmIwIII?Cgx=GtK@S3y^!42501
zBymQ@Nj>o&Ck4Z)ZH<y!NjSZ|9DMKcmS>TRb$F(TgqpDMX#AwS1E$=<MQumJ!q1hF
zASTqSUi9j7%CH^L^$g_hPGD5bXp*omYqeOMwAnp5?<O+*pt*NJYPwlYE8bq?G?Z)b
zFQSBqF##KPzU)sRk}@%-d;NXCDTu38XV&-2J^aIhN4dBBm)#RkDO$9dtc%_7AWn|k
z>Dd6iOKkzKVK{upJ|Ek%A843{SE8ajk^FFSI{foqQkBxxuH8vkWX26sd$g>z3jUC`
z_}|{QN8f^kb00KM`E%9ZT;q8C=**$?{1!>NmLr;zXZh}%iv5pk@*D-<MCOqBn|@fo
zDVy@?X#GhpWA)ue*gzISKs(U&PL*(jWsmYs9IGwAPb7A##q_PkTu$S)uvL$#t^`(n
zIO>kV3aqnF7PRwE$^hbUn|9}*kN6{~Zz~#}Zyq$Ljr*6%vbc2sW!)Jo#l1a;o;V8A
zj)~^;HpgFVdYI3&s-pBr>wc2nR3|94T{f$8uPI9_Z!3wK<zqt<=3u`mLM()H8NQXs
zPKr79i|`q9VjWVSGOCt+Q*D>-bjE=lBWESVbR%YpSHuLGh*5rPH)NQ2c6h=p*^a)G
zGIIHR7JjkgUElFZY5$U@hUD`m6c$y{y5MrUPe)e{i;VCd+CIAnTDb)+5eqW~NL%%z
zXsVXK_i;Ff-@9=UL9GWpyreOzSevyzs^K-wO9tz-@Qk{ZvgVXsqbC4h=wMdBtOzoF
zmP}((XM4H#4mHqN(~GOB=i&uTB+eD;-NyWmIt(w<mrsbET6#2xNTmrN+xk$LIj!!q
z!e(5mu_%bZpUV_9%E#N^BDkD#cnu8tMpOA3a0tJ<GF|DX@ZfiTy#FDzWR~gL20J(z
z@mjYJj7}3Pe3A&U>c<>i$}dkR+`hP3c{7#MlYN)tK7VLOni|g+*e-eHcfvl_Wl!tD
z#Dg&=WlPSSQ%}@$MTekEyz4ot?rhB!dPijNW0#-V=)oV=vTyC#(0Z(piu%kwsq(p4
zTLWJ%x5-l8TFv%X$v(`D&E8l>2MCou6*cwQ|6HU1A9iK|-BpX^<x6EPZoDtue&>7K
zjQtOj>w-?(KNoiP<a?8+-vWl5Xgi2xkgeioz8OB#+4TBf<SyTyhPJ#MiyFR5yoV7S
zU0xMixZV`Qz8<ujT5XQ4#7PkGYi>g*3#^~?NDdabVWYhoUp2W&+(*167rtynZ|)z#
z?SkorZ^Drvy`4j<V-1Y>SR0ib@jV2i(V{WCHKRmqK!`S<=;#&suF3Q!_Ua?DGgh&}
z<mHuhu%3f>*_T=Bx)1WC^;L=o-D*d&Y<5RU+Zzu=-STaEaR}-qh}C<d73bnY)+|#W
zm5xS~a@q9Tv4^x>8zwhQ3ZP&QD753l@y>2NqrQ;B`;S|v&2`Pwij;O~wtS2*)~8bC
z#-wk3Yu8SX2g^IsJ46(5mF5#DPvyEy4;M`Fz@1#wgV3afiLz=!`JHgGCrE!%^THoZ
z8P1R<Dlydd5!qUN>AllJ@FjUypPLNzb$c`0?Z(Qd_jSt&^9`C<sD)7cG73?gPXG{u
z0XKU)e$$tNz|b9JJNE@AK^(ahqK&#Gygej!6i)mHo~U1b@lGksOc$9@KCQa)8dc`f
zTC6G%sMKHN*R~khP-q)FcW>b)Ifo6(5fu*i-1afO$I2?zlTJ(0<TCs6IC+aliJ{=X
zwNE^yn&s~q(rNL-s^IT@NY8kkR#4t8kt#>h^i7N9F1PUif}<11ON2FtyJIIsSUYDB
zmYS&wQ{l^#M|xteZadw<-NsHYDA$o(3n3wU=@l(C8B5z<atb#MxS4Ih7*^9Lh9Y1`
zw|Q54LbEe?A4RHecJ_F;X}0mkW0`_jdYJNj^OUP&1g{}YNV{JcQ6x~^wN2`i6ghF8
zzrW2^e=&YBgR2?43D1Zzgo=KGAwB;{ejeiYHnbWatE03GtuXx0M`b1o)+)h<=rJnC
zF~N6gd9U{2UG<0XW3Ur@HXCG|IF(pOW^0&6BwKG4>oPM)N;p?)SE>7uEi1Q*ZK0Y%
zG3skw9o|z()>%WV^Be{&=gtf(Zti=3RJ}bUEXjsBx~9{T9<*%FG~5s=vcXlB)Ob(Y
z_T>!?a(0wO3foJt76#I&7J2!1N=D<V$C}P)oQTq|X~Q1g>s8b*fr2iCTn<b%dF>^1
zZN`>}hQe*f=6#=#zK$BP$GCToO}3VPG3S<D#P${5su~}Zc1DT69#Lu5wBVA*y&5o_
zw08r1E>i8{lZby`dq(Z{0`!`_olO^N+l9{9R}|x%K=QVWs2k3-pb7~mpuCu#xE!xo
zD^tD_|73UGV&Emb0?8}DO^+h8>M^tXd^e}^N_+bGf<~@2t>9axf#F2i+K5r^iV(=D
z5FNd-lBHbX09TH@9zn?#ZmpH2`4PLYfI9ybgeN^vwCJeu{5po_puQlC<vd~aHMQov
zNE|*jU?yM7`*!G5MYcZEuNNbNcW!Ee)p!3MQMNH!Yn4bj<UHSq=`yh85{gU7`B6YP
z7a~r?L7Pg`ZD_EPw+PFoYoBlcxHXNF)0Cwdr1W-NHo!;gZm2|(qRg8km<K4}o^BbZ
zV5Ib{9`19X&#$dBCwNRiP+9#;>eDSQXZf=1Hh&O?i=el~ouA$vekg|d#;r6vK-NPB
z9q;>ajd<GLiZ78J$>1;?Git{OY~*7;);*<jEryDaq~DdrttW0JI>hu_OZVye%$ae6
z;z3yPlev~pWY&Q_x@3Zs5BlRrTHB_L$Z+U7_|TFqro-In4z7-;n$`4Gx%d{(YLAq|
z>K~-BZw7VYWry2GH%Pv?E_KI>hbU*hwM&*lyI+$3I#*B@gG;*vVL3YKjn1UDK-x?N
zz@1#PX~#Zo=T<KSR9*`zCtE!DyjCAdxFD{HOznyD*h<OFDU4e+!;*BQJVR~<gp2<O
zHd<k~PkwtY&9N!;jtYfcizt%_(*rgFw1(gd#Z_FFMU^>1VLNI<xaC_%w4>#&xP2nH
z%QI^SB?oIaSlp6^?EaPP$2>g*vAiA*b<T^GFhml4BgC<T(bxzHN6=k=B^sfGrw4(V
zy_$trcK?YWa_4*n1)GgbaIiRXbHa>Ffk?|q|BvqboZI>arJ$aI8GJg5MdPxax4PMB
z9b<7Jy4DP@kH%YVLabYc3_{Lb!Z`^xC=agvWhb=WU!&s*nIh`&g3hx0vqOe7zKSGI
zK>0T^7OV+&y78Z-kbJBX-4Mr$>VH#R;Cq};Hs?-SeNG*lfTg;jOM?&^Cg6!yse4OG
zfKfb552OFWCy*l#r&6p#11vfT(R<Hf&FkM@(qe0-LR627(P2Q|SJ>!95EqS^6M$!R
zw$A|ATb`RVTyQ-iXXpO8agj01dvLx557OHWNBaqfBZA>zq9eEY(%BrEVC_RpK~4#&
zDrY@GzG$t!k?%#2!{d|GPZZ3Fxd)4O<e1m2s!~;O=vYsE5L-ILp6CtH<FLfi8J)qe
z(;Okuk1uM#FHq*0iKdl2Qj#2RZnnLs6GTRm8jyvYHeoXrRLOecQL>y>{FTmqu65$u
zn-KgS_e$^dn=$Waln1+Op-1xby`g8()d7xf)f^3zGaSeAEZgUdiYaLu$(a{pgMKb+
z>Q7&sR+&;0Y55jOjx2=i@=3B!7c}4AMxWx%O+xs*wl1(pcy~HS8yO1+4-jBfmM|qI
zLD`&<z;P<QJtGuir>{31AsjD1JNn^0?PU59hC~vSw~e6C2c`Os(Z&q>q$R(N+6R;B
zA(C4kY1a+NK6Z|IGLE(B4H44RI@4J*F`x55&%i*ySFpE^hg9V`4AY@D@4;7_)2jL=
z6Zbt|W%1#*pq}%+*xhKM!2CzrBA-=@AP4<Z$7PR(ZPfImUl{};xjkpcwGkTgL1`H9
zH^o#R)_(mW$Sw?AXSO8nSsvoGnNXZ7pcdPP$IFN})BRrlK~;c24ajo^O?OU|X-X31
z4<L>=^V~@zhZ1TMucXkiqWLj@bv<1gEBv&$T>mcYz*3y3#(JNY2_i~E)Io@VO_9y(
zNzRCTWrVpL=dF^e?e}z1>2zvz7;~Q}im`M9P`^YM&w&2uT~JHd<kBZpF8m>+5KM%x
z-RWKQhUv3*2)==!cjzv#C3+_Fba4^I=ebJCgw5hnW~p<_D68)`Ek}wmF&%C8-`iK1
z7=KafKCswv9brtKpGD)5=d{BzGMOM&9N@>6_kF|OAU^lfVkeO7$C`_QuY15O84^<U
z98$Te{L>$!93<*lw$un0r^?Gq&{(OKTm)2WlRV@IJdOZ7laub3X(dBH^z~B*yf>Ko
zu|9u!kdB}fRj%DMmT@lyXp(lq$A$>x90&us0PlsX+K&q_EkO6QcHGjXbihRO<|D}r
z{sEa$Tr5bot5u73Cl!qM&Ry47G3y--_Ey8Ha1l%2h~n4K=5$L7M&&e?j$F}paX?vd
z%&JultEyb8a2l9H3Y_CmeHI`BIIaLY`;INE>cgN2j~8kv&V!o6(gm4M9f>vRDxB_$
zJ$>$WoeCJi1HLbOesEq2wUVekA&e<xwwP|Y!60Xv$NA+<hjUEbf;9p(4rM`c=em#S
zDRB*0L(;dey;IVw&recquj>{uo9@Lot`Iw&A(D`(3j}#upIZUKm%q|#p%>WkFb4*;
zKX+s5);yx%`HxWq*zxWCn02Z9VS36poOCW+gCjAFl<t+Fk`#6F{$LK+X0h5lAKv0q
z`mpa(!9|+3Rk$&JfS@;!ln0k@`wDGkZ_}u^ZyoWopK^t@z)S9<;`Z#D*+L0watGO8
zu}n=yT0^Gy-@?9jQ@F}+?Yi}#k-zXEx{ba|P5odd3Y5~u4k4{4D^qj_N<$V-k<G}U
zFc5fj&jkB_w9}-(&CyW(;dJqqKabQxrlQk-cEtr7!{2bJk-ImBhXuF2+m7k_H4v%u
z)zz?}AtXf3Zbdd=n}gz$iN9+1q#85sx-d*)vucTD1i$;@GWj~qtwLb$xaeK~KA}F@
zhvSP`EUvU;m`Y9`Ww|aTUv3wsLJa7hCl`C*OgxBQrk7AX)p~}ME{agkfg(Ngpi^^w
zv3yiw?pPhJ8bkX?HK24uk{b!&NW<zf`9Kb}0u)5+g&r%*_0e1j7fwp}MM}Q{ptps7
zYt^_k+lGb);|RZL-3`j}-W3F_Z~C){A+UI(DLdZK4r87wB;)@Q4Gv4Z5;$Lb<xJ#(
z>&l0U`2_1;AyYg0f_7g?FpEw0p45IcJ_mwe#mf$aE^iWk@bz`~L>}2wFOZQ7ODXny
z93ZK!iYBA#BmoU2byp)Qo;!rQO-HCe`=&jdQ3BF)0r`<<Bu(vmbq+ZtD~rin>kQ&A
zgNw%v`iM5+wF8ta$Xw`TEZ_4^2rX>L4Ewk2p=I$Fc*(Q8yi9r_Kym}pzK8}SwNlXi
z$l+R%S25Qu`1qRrR5c#Z@~jjz+*D#^xx0zivb`mIlx>0j9ARvMwSh^f>TF*a>)ueK
z^wDC!vf0NGNEg0MRxWixCjHs7j5{D}Y867Z4cSvEl4Y9K<MQJIx}$^6GHl+meYK&j
zrlm8h0c&JT$=*lLhR-B`+XL^IM!SlW|K{q{1HqS>gQreoynSpb(x4D`RkKOZI^ZV%
zz9O5ry&A%gMdN7qT+uR;ROj;PPN(G#BB20fkK(aR1md`5Dq4&Z7o#M8tadc<DJ(#N
zno^E>FbE|TaiT0??Jj-uN6@8_)AwHcj}M@fi31s0z&%dC+Z?8J5bIX^(52<fSkYMY
zKwfU~FOAJNaYU>$GrZIYZO4dcQ33^AA9ncXG?s%Nh&`yOg{O*lPo*YgV8a80q52cn
z@dD@~xPFqUz9}0kfvgl$QLl+l?o+0$E#<rS%6m(Qkln9J<3~!v_}(Lx<zw{XzGU)?
z<0ApGinLNvD|w>C007SNLmlx60X;w3g0W@=1|=KptW<orW_#ybHLFNz)I=r|x_JBZ
z0h~Ll2%76$w6JPteP!Q|Hx7?RYMYr_&15mLv`-<0y#yCqUnzR7FTV0`*r%lMye>_2
zVDWKcaN;DvtnI1ucY0DF{6xBsewb%{ls`vI_nt_Z{H#tLDm(xEC0=AAKB`|}^~Fxb
zE@`a_{l2mkD-aFp0|z|`^TtxL-c_0I32$GwCuc2LKB+n`H)qahFO3O3D%4F@R`VBQ
zZ~BqpS1x}Ak^L{+IW|hP-`$a7`7BEz_JlQmsx5rs^EE2E_0J{!kkdr7QC-KR&+rn0
zk-@F!KyRA`df3Pio#my09HY<VVe%|N_cM<nvFVG2dWsx9vsHceSyM8~2YTdRS(;K5
z(C}?xnD@IHNklDu8%DPd6aQSOG-&_|>Em4da-$8GYd*mglm23_rNqQRSyw<ial{25
z`HY(XsIagT&_O$#XPLoIoI=>duOCIsif!;3tRc-BM<7<gVu%tpa-zsjD!^H0G;@G~
zA7WfHELXRt7@ed-<2WUhHwy3jF$l6`FXjt*!*(9B!c^Nxb))wiL;3^$2afwK3bkpA
z@u8@%8_-uTf@a=d3mQn?*=GT$0I;GuDfxqb!vjz3)fSTCs85q4NEp8LZz>c*?Y(E~
zwb<nad<TC<Jk>UP4u?3GFxsAlRtaoH?=PfQh^84}>U!%7x;KH+19Bnw#M`E#mSIua
z;(`P#Elffc%b}N{c>K2E@r55B1kL*p6#)Wv!z-zs*xyr*dw0M3W<tmjsny^^1@hHA
zkgpI?&Qwl4B_nZsIUN_-?{a+?7{Nd5M3*mB%7z`o!Ihu5vOgZb*m5*@T{VeXRe|1Z
zyv!*1jz5RM!0t{VTIK@hyB`{nWQHdm_Q!EIo8|s*8fc?rbf~*^1-T@N_H6E#*O95x
z`Ve1S7)LQsVZGppM70?gp8Fi%P+>%r2&C`aJ9(7tI%X~K%hI>9`;)1Raz_e8+dVD5
z(G5!#mg}KiSd@e)@K$uylW;W=6oB`+Z)R*}s${>szxM<wo;6Z9R8#@+^WSg4W_uu_
zr|vwMorw}BkHjScF!7^rt)M<1>=8O<j9SdEl2ok3sS9s60?YaqcPbTv_f6!iYOflJ
zOT1nqIWbb#Kal*iyv!WF)%fT&aGScq`z;iZ^VA^f`F-b5r()UmD{Ly4o`=mLe~U$r
zTEjI)ziOr0VcP;#-FQ5knCG{Hjp!uD_)VqB=Y_*0)HNaBdRwH@#Mx~8k!jEa$h`QF
zvSJRfmV8u}Yn?)(uOSR!drl<yYq8PtDxj@ir*Q{D&s#9;@Y~bs;Ut%eXzB^fDx)2$
zKQ`0Fs{suqlFLR^2nn?Yn;G27Tz^L`zzM}P4sfA`WpE=t{I!m*t0km$S+OsIUMSW4
z2vayTH&@#BeThBPcduA9uUD0(a<=9XQnX08K>jrHXD@j%lyB&#$2X=FQAZtIioDOk
zy!gQ!UsaQ#MfPt19Zs_I0r?Z&4XaDLv6-AagglZpiXu^=;?<6&J8mMicpGDP6WC|o
zAKv1MwL4X%`O1ZIo%zAeCawF7y=O9V7#1$@0(zZcVWW-o9HAK-Zw`mmZk|F0WJYN4
zIIp)UaTuhMFsRNk7t*fL$Yrb4ObH;Bn`;n{u{N9)4x{wtRV(L_7m^S^N(o*S4perx
zb`J)$$BNG-P9VWp6FpLUZNKM!SnGTF5P;P@|E5~y+FZO*S)<;wc*M=sgTrLWjHTx*
zm++ES;$FRmEJ6`u(4d;;E-GUA4rcMNS=|^yi>_C^g7TZ7BiWF^rGzwP7=mgB(%j&;
zzgBCo#4ToxuuUJY8B4hdyWSjfxv>Me6fbMwtCy4^sB<Rg{qCgpyA??pCCi1#bcQ#U
z+w+M6X-ka+XkSflY>KjVKd2lCRl1%omzhHX0T$Gggzc5Fw^0Z8*nJh2#(UqWj;+4L
z$9A{qzGx#zac@YNu7Q4j_4V+(GtDQSb=WF|x)-m|B9t)aEcSqDGbckFLUzD3_~CV4
zotAH`1Ix$nLd#a=XvrLdX)sSCd%z+LV5I%H(!1fBz|5b}FkoKhXl7ph&K4n(!-!{m
z{!u_i0?o@fQHHW$3j@M%4Z1>(iRk+}p+>>gU^P&v5ug2aqPk-FxnA1rZO#((aYOmo
z@@&W*@Ar>}WAQm8$YI6o*tQ*V$LJ?7^v7-zM0^6`)0Mzxl`pJBH{=%dvqS``;z#~0
z?ApfAq59GE^uCjWku=`#5KtGrR0>oI{JSxo6-DLzXQ}y1TfU`2(4C?4Ov9e#ZV|r!
zWRuf_@a+(}E4Y@&q`hTuB+Z&;DP~rQnOP~R#LUdhtP(R*2}?{RW@hFRLy4K0nVB(1
z_kKOQ-8MZtW43?NJo1f<a1RSh%XrfL9Lu?~`Guh<;BoQY-knqC4Rp0M7@C@Y7+*Ny
z><nD@JS|&;q1z-$gB|e)XKS*lr3(4C+Y(FPB9~2itw#P!Xh16zo>)y2?_*KmVZH(M
zj#bjEFa`dteVu4d50P{bPsJrR+N-bdTsc1ze^4bJl26DcgX7A{SZTg#(d;zXq|lCT
zdmT7$j@LWA$!89TGkVlH^ig9h^K_PeJKvl$s}#I_zx-n|fe`-F8`G_JuKBQYd=e{)
z$<H*&t9wym`l8_cgvY`07e>*!RVjxFyBU25%%l)WBaY<f2~pH<hiqi0#1?<_22Z?(
zqMH`N+g(f?q3B~ZWnPv?QqaBb6S>B<iQVAR=j4fnaNCVh5w=3sM9k<)Q`;9n&Te2)
zL$qRe#)LnU?RXJ_k%n-LSjhdjYkV;pohyJPl99}$A>>tQNCwAIy3rl(j85l)zEVY1
z;l9h`pC1cuWfO;?AY=9&t2u=e{?k*H;#Ju%%qeQ%q$%<>WxqhN6%EeH_@l|SyaKp~
z8jZH!OqOg04Kt2iQta^7Zxh7+*4gs-NtY<yO<b0KcIU=#etFu$=%i)~CE`d{DRG3=
zjP7gbNql{wyo5bGd9oc4h{EBJezXQqzw5iL{q9m-t8W)#{@Vc}r2w~20m08QpR&6-
z!J(IhwmtY3_iY)bEu}RlnU<)8-jyP}X8OIBvd7mu-|SvWY>?~wC|vo=F+Tw2?NHBZ
zf<^VrgFxO5l@At^F+yQ*LSkO=^4{pPe@hJ_vJm8ViPsJl#Eyl2mpSdY{1x=Yj`W%5
z?ZW(8LXO%*WoCfHjlKf}t=lTWoRgvg&ZO1htY0`4@^P770}&WLtUha0IAPl|5St--
z|A-EQ&cXOe?j1++{2-}sSwh&~sH%EX1Ld_LCU`nksesSifb+Fq7P>;*k*Q4fDuHyX
zL$8sy^A=&Va={5WSKxP<-wyiucNrHB*z={>i(Q(2o^%1GGLs8o?wrRw=y^!{&FgPA
zN!MLkDKy*e!>({PV96s8qz#_p<&s;~XJEOcMSAuQb`<*+RcA0lU@7L(6T_tcD8-#y
z{T|N{(>1Y-aq{4U%f5=_TeHIaduzHEa?GBKYKH5%??sf6rAm+09Fy1KLla@BANb9o
z7Yd{ri|lDbM$~EeiCyY0h4rpBquN}NMX9V!SSo>6fkYo$mJttbCZR0inUtzBs<HRs
zd?PZpln0Y&bDqn5+uX)lt*rf-N&w}O+2q3f=d<u8k*fSMpo8#)Frn%*RZYlIyy2?*
z`WI$#?UkRE;QA9U1Kd3n-ne%;NhpqI(MqXrjpSNNbt!?Hqbnc=*@A5<`i904l~-h^
zydL%Gm?50|x)8AKcqwS30}H#NV9wxWlf}ftxyazWZL9EGuHL;_)0Q$QK$$C&f^D5*
z8?bz1H94fWRCADzlhi95sL`<+o&x=8+1VwsRm@jie6k4jbMzPzl8g0`q%q-#&k<mV
z8kO&|?^}Q_E@Xq*k|2IhrBZ6HwFmcWQZi%WP5%COp0yspoL`9=PnW1tUHcFjE;npV
z*FaaBX&OWeGhsIR<LlS61R@F*@a}|TA}4!4i6z9A_)Bn6?J#r9+cdz^m##(Sr}9P`
z(xJN}Lb&;im~8Nlsb24w2s+zQ@)AejymGXT#z<6=&w?lN-RG%Fd+>SP?zXf#q`7>~
zb-(L4D}&SBNuYCZ#SXG^ce|mf?82w{Qnn!U{(Mg*Z}#B1#21pEVT}f*Bea0mU+l3M
zW>-C4V@6HDR0N|^ma3F%C)2&jVna(fMfL~u{xf>ZY75cF+<woM^+>`%pmYZIL)QDx
zDwVVRGTsV7tBoNdzZ~p)Py^i|24EOORKz+ZK|Hu3gM`aDy$8=7vplZVk32Y+;Wmsl
zL9iW}z5O!0yrAaeBhV~<IX`KWU;E#~e__JeZgBe3`fTQJ=ZKQ_hC{-#bc^Uo&}&*n
zE1K8#E&9gy-xsds_t=>dyo=JG@U{gR_vRO;beJQ{2zhXU%M(^yMNF+x_c|g;IzHN9
zjB<+pMQ94z>zAI^Kbl^n3%#skVN8q#oYxuk)<p8?=w12O5!<qtVjbK)B#+nQC-J>*
zhJQoAS6HzBBEv{P?u*c%c|@$DgZY&<x3NYx*r2GtV!}0Q`x4{9n29v&)lULYIqpj`
zvx_^%cHc{A3O1^e(acmpzp291eCa<-V^xN(ppS9E${lzS13u~)OIG(xdM59eYT&e|
zj5;3poH>uN{T}lv9yprsVg)9<A7t1{U?m>AEH0yUoqo&_kJ_T=orcw*=N65_&jfCp
z<8{ZR(f*at_-h-&p$$i30zu0-_iR*hK7}V;=l?0P>Bo2&*)iK2Q6XMJl6+;oht+rC
z%n44z>W^ZiUe2K7*|^w*B{UmZ<IZoaBr52rMK|Z$biFiX2O^KKg4j-ld*|feR><HA
zW`}WQtXe{ZsTiavvX_@cg>9FXujtJ>YU(Nhho`UKlOFUw8Z}stOa)ft+xY2dqniRs
zY4jm4^_LsqmxCqW9lCx$a$;(Y(e_-e2v6<z9vmh2`uj6%G~mTM$p)RFi6x4fpOKeZ
zOmZ0n3Yy;A#kKm_2$W+iG_9<?A^sJ`kJ^i&3(gyM?-!xe3~Qn>n_2AssH;hcIiHDu
zyfr5e&q+11ZEe?2=P~GR`_qf=zd}d;O(DN)4b^vU1`!O{AjIA*j*+(lgB*oM_8lm^
zwbAxK&ue_Fg};DcBl_naEOqH*(|7i`5M<J)@{V}TG}AE6240$B7ka68_m08BXRwc?
zt-KuSixi*&+I%&Rl#Mas4JUv7A577vqQQRK66=8C)!{n-u8}CO8aY8QJ@<X`Mz6G@
zgi@c$N&$UbXF1XSc4OyP5(=gIi~;oE-GtLlL3V%4VV+1=(Y?xm!<}s(G;3#f^E~kY
zh?=GN?jbGdaLtBJr6$IqkBSf<2+R4z^W=9NO~bwHy$uY&1~x|^>_}qlR_q)^<Zqw@
zoQM-ig8vj!<kwhT<|UPq#3<pf=s<rbK+@U|zvDhaKbZG8cos1SQGCh7nnL(!Km5|h
zZl3GpK7riVwHaV|oRV9Brwczudrt^>J^l$Le=^H!hJJ}=idG1wnSsOM*-<agrbL?j
zaC0vFHaIp)mgVNN*1WH*va>97*%tRCS0-_*LHdByW?>V>W6fW;O}3ON96${RCVWh`
z8!(NP2)RJTNN%^KDy>L+=sigy12p7W6$7PntG$-%ZFpiv?sEO(s9-&*Ca=2Qcch<_
zQ3%!SZhysKISnRkKkx;9HGlF$bZc-Ig9W=PFKE(1Qy+^|Pk0OQ=p_U%&V2sBLf!M%
zEZAIytxsS{aWM4N1jNBRQ0zW3v{G?s3`eE4y?Dg58WWwVe=UZKc7ULZnq??A+=b^j
z&a_o3v1TwlDyhn#+rM1THtUFE%M5SF7x}n6uaB5epWI9IK*e11mlKmce~~D-hF~18
z%yXfGkPolpV+#jW(@Z7{_0(yAIIWe>s9NsLU|#3%qg3rEDEZ7Cc+*olbjmB^Y1TxH
zOpA?O7=PxxM0;jH(USx!79jb>u$yYp`M2{Ahg3hTr6lr_c17<5GDe8`LQy2*JOpBr
zG()gqkKcr0%b)r0@SPgGJZB4X#QK&LJ#1?Z?JXuQM;%bFHJO2c_U`SyH%1i-F=b52
z73G<fwX{!M-ibylN0_U?<JqCK_ke!SuJd|LQrk?+F;Zw(X=&D4kV<^$jr`dd9~6${
zZZEQ}#$@bZxov&u46eKs(Lq6?_oZ514q`-NJZ4{O-RWC25hOc7>KqeATO9HhiL%6V
zs-{eiS%{sEvq>XM4GbP`uHHx3^&2kF1L<ZK(Tgm~mOG1hXUUNNL|&$MZ|wTjG$T%R
z`Q$Ul!)2vDFU@)fqh1|NfQmd}P+TBQl8FQIJmxnq6_NmT;U0q`V7G!Ng*;hEo06|>
z?LUT@$HRio)m>k8ECF){{QBC4l>*!-q#H{G=-gY%277+Z#BfcB-CQ|nTDDn<=)*;|
znY?O9eM<S>1;m0|CrA2C?lknwBc<;YCuE<iveQ9l@(i-J1+n8%(EVv$VS4p$uWfh6
z{7RX8{vf9<j<~Q1qMY)pY@O~dS3R`hEBN=v{-u9hckccJKn7Stn04)jl|p$*8fm*}
z^s$r76bFM*8H?_Axa?I7?X@w<HvzMPPmor!U{al#w~7}S#;Tco8u~chFoj?M=NW+M
zK1!WO<8u&i0&Pse4`V*2p(;VmNjZNZHn8h`O$<O;67mVbE1`vT5&w)nkA)RjTgK|j
zvDq2EDRB*(CRS`y)Ptb$CM$s2mK9=lbYHvQ&&#fJaO&^??mEfHK!!ug>ap-9X-*DN
zcyT>5tde)51riT3g!XKJ=As;$%HqG^j8cj^v%0&7k*VCSDo4z_-O<;{-(ds~6=HR$
zGmO`jQxbyzMm6<I-{KW^0F9@Jw%@v-M~Qwxkz9+DkDT{eu*}DboeJGIheFqP#+pC@
z_1oe<0xdv9=#w>b4j%Xhp`Cwdtsm-(crjK6VOVh-`oWpWP-mId|Io=1wGoIN9amZG
z5~_cbe{d!k-0T*OGIw@miK?p)hjIDsK>WB`f|`5pTUta~?H|T8I0D@~iG+G4VI1*e
zSty;#T+Qc@tnRu&xGH=Rc$CRi&QldyR#<@pnc!a5Xw_@UyHJ(Q<Mnohgvu>NjyU%7
zkQ?&m91u&E-|KI0prixBd^=!zsjEl_9j|M(i?WEknF;d&U9H<w!Y>b$Fm={=Q6S`C
z%tm#^tS)$tY}6ooJ{uxj4Lj+CKXo|h>$wAT2iO&?bfoPf5Z~EVFwK{0lsFFtA?#ue
z-jH}qllMLJ)#D$`!%Kf%F1I-nVdSD2I14oD^z9d#&6`0Kpl8a2zu6Ws?JRlHu%K7~
z!>hkYv@|XEMY@cpsbIm8ZWEm~x>AMd8eyGbaD@}bmcj8_DXNiG{8NAGwf|s&mQJXk
z8jL2_`BFSvB?-)y3K>_Sv|7DR3~M36xJCT{ZV?QxGNv_oQ2NYJj5$`ba>)sZsNjg~
z3mJi&88`S&C>CBgSnT3H3$|mgAgNHq^(&c>Kk|U#`00|+h95Rs(Y^M5a-tMefb(bZ
zzTdf~+9oaqv=S1{oQx#=DFx4RZ9&RfsC#ZL{w{2P7RJ%UvbjjGCplK@OJ;KG&VtB-
ztDMxXUchU>>EfcpT&~MSO<*^ed8e}BN45>A4#q@fd^(y?7m<H^^ixlca)jm1v<vf=
zR`P~JMy4EO{nC--kOPcYT8OGxW7@`;q|j$^W9R1^Zs{V}yr!B}!<T7}D!_7KQto|C
zI3HaraAw_ffx2Qo*ufkzhpdMkH!SsSnF36P{58s&jS+1~Yc%B{FzsI0Ox-a;<Bg7|
z|2^DK&*sxjwMM^fqs2m-g%|PQMteQ(+QHI@x|~}QW6+=h6Eui*ev?lcVr@x#RW_#Z
zt@HP?xNp50@5{v<-c-iecY4{BSK8lej$=fCaZ;LNbwB{>YaqJ!I}Ouo|F>g<G%0pg
zHN=BXLjLf36J7mYYYf-U5t;5PU|niFg6WVrHom4NNKX&F8ajNBvTCsBuj(5JMYS8_
zT-Y&f>H)5i&;lttBusbQHcK{hVNGu8Xz8I!iK;gf%7X2+`9rhc@Ln#+?qFd|<Y9mp
z=ll><`v{;CH!a{ylw)%r8(I1y`0C>l6^lNh5?74_{BRE8Vd;4Ue6hLA2H}Dvz8{nG
zbB%+GSa*wLvXu%X#{Hy0BXn-XN;x0H|IdUJ2-9KBc^#lePjc7YRdnkt#!-|+@;atF
zq~YgoU5iW*N6Dw!R_b~eKQ%0T<Z_p42w8cFmn(5~o_uuqT>*-UMRshfr||iHY7Nvl
zfk<lfE;{vs@71ktwmyp=b%{$bccJn(@H%0k_BiAIGT2rs07IjD!Z{@1@sxH+U}_RI
zMss7z6y5dr@p*x?VzUmrB=PK;7odv0BE3!kq4a=Y!AR^l2pz7F+YZDh3;PFju=(e>
za~Ss495x83T`KF#R$zt*i(M!`u(IDux{@3T{+`UGPvDhs*S!JtlUF57=Jw^CR^J9>
z0x(mx>hE<s;^>MQ%Z9}$wB(eDMWAu&3l+;zBk@yjiHQ4wFApTdQKsUTF3qs6v3S{-
zX<-6#$f<xckwy(Py%M%@I`=rsxACd7zPk^|X(6A!UfNYso}MK`epjpA!f<@Q(y<+V
zla>XdG9hy49fV(V?2|j=%+vl#1N>l%)2e8x;#P=4$Mnk`-`z$gdtjLMqZWGa8eAeY
z4UNS6nBWTI^<qeHg)<6q=R=5V%iFW8lK5pXeitIZ%Mq>gSZo`PT`t#3$Qg9*g7cG{
zk&SWe_P3IR?d_R+aKuGEuJ9lleEoH(tpj-l5wDa)ekJ5G@51Xa%`4aWo)|kp`#A!L
zCYpQNZ<o{g+cNSg*9gMQIV_i_&oERD{0^ZM#de@JaW5H#%N0nT#g24#nnAwqMw=}T
zw@;ak$mPRN4qfQy?e?>T=0Nf8syVbs5d7XH>nTw1Hl~im6VKSMstXMp?~Os>N6@NU
zt6=zjW_T%04)MIwOqD9qclr{th8qU#U8ue-Kc%U&=bu!mpl;9nDw7Msj1V`^7_^Zz
zlHPL(w|H$%UiOimbV?~pT#>LOJ3hBwu2gE@k53r$0yvHbSnr>XM^!=;vYf{_p%~;#
z9bHi7*=OLFv938ncLPIlSVo1^=dF$L#Fl$cBV~&{39t);O$3y+&C{B_U;+mht1J44
zdm!M=kbfkBtY@B${&%cfuKxsh{Xem8MOD>gwY0uOr>d&VUt&;k8zXaP;xG2AxV@c=
zu&})cu?_<(F$2r@@5F51If&W0*#BSDEju$a96-d#LDJ09+`@(U-<zUl&i}&Detm+4
z1r8wPVq{}!B4lT7V@CXSa1|FbTlKGF{BJ?GLI7ca2tX7d1`r2G03-oY0BL{>Ko%ee
zkOwFL6ah*AWq=Am6`%%C2WS8^0a^fUfDypR_KSb(Y-DE&FmnC^;95Fc1B_e%#sFg@
z6KiK1BWDYMv6GRB*}v(z|8D(*&iz-@+RWwudHbvVPr(FWVsB$__w{OPYXmR_nA+Re
z7&!sV0RJ8*z|7Ir$Od5MVPa!s3-|%}Vd-WD_+jtr1TY7fJDC}M^#EW2u<&%SFtY<#
z0;~Ym02_dfnX@y%7GMXkbG0@8>a3-?9l#!7Z)XN@05}*qnc4lj7yoYk(}%AS{p&jZ
z2iX_kVDm-!bp$xN+Pj#2fq?&M{IjPMz!~6dW^4IRpPel|0RN2H1>j=gWM&3%akmG!
z0$lA(zfNmn?_>sW1Gob`0G<FZfR~w*{eOi8{|gQLUtqypO#kU#`iBS1Ow7#A@dYdX
zXN!fHm6`qPKKNf*!E9d?;s0xgtP8Yif=pu*u~>nmO8H-jEAl)k6>jAs4k2Q4P+=)y
zG%G<+A(U^Tg<-7la)n`%s5H{3pfo9)VqRC&FwbC%-ax;qC-p_?`l*$p@$<BX(AcqQ
zQCY|UM?ov&X$a<0PEkU|IS#g8(jX{dg`5zipu2nD#ZcGdx_62Boyn16|K=V5jZ1gp
zn80X?k;vl0Ua?to_fDV$DL_l3c<(!SA;2UBiJiK`urqmck?w>^Kztnqp@YPCg?y~A
zD*;Fc`?ADUcV_@%7%Z?{C#NGZ<S|eTcXA}i&^%CI>|UG`d2LiAXK-02hi~!QZ#9^K
z5t9&L>Vyc7kB^w4t$PE^oF$RM`TiUc+X&cXH<FoHXrTC#P>g-+zXgiIgPuWD%)fm~
z{Ds()ybjrk1EJXt2_&2GP!6ZU4d@3gTmVJ4<?8{76a^BWeGrF%2_QrJM~L|SdDHTp
zhWW)$AJV&n?M{zy5h=_A8bi%NZkSs}C4Ws42FAp?>ks4HXCnE)1i&6eJQO6ww<CbE
zD8~k6ivOGQdhLJ@C({$Tr(_3>+Y4<gIaC=SUTU<y@!JB7(4|(u^&1i_RaQIT`RJf6
zpLx?_7qA~DOooqaO3Yhl_?>mM5527FGeHC{;Ilt74nhzYEMWLc+$xC-asuuSMMQ6S
z8YxCdnFnnY9|A&(83c(h#S#ZfEkS+)E!yAL5eEW#6pldrnf1&8CX@#P21x~iBuVuR
z_r8ROCd%bKK<X)0<OO6AW%gMJbh!8Esk@>OR>W}HuFLN@R0DyXrIv}6<=YF*i@UNK
z#vVAzK(2pkaN##%1`^2OVJUD>;N`6j5O5KyB!Yc<v`OsuYFR+Q?7xl-TiEbM(RX&F
z3-ieujdFd3!Du2m7X<+UMCe8o#Ra4C6#`CSa?-y5<vxW{fUeKLb}~5yjz4`J+sJ=`
z38Ae8>pp%gl5^)$#9SgoBDc`5-Ky~W$UJ9csMHEiz#mPpOz|SB)ADxR$Y{ToL|=$e
zL#V;lUmT2o>VcQF<H1F<_HQdeUm+4h{Sy34Ak#v+l=f1&E*#^QC6S7GeTu-f;DmBC
z<b$=u0SW2r^$|ju^^ua40T-nnDS#=Q+OYsPbYu+?qy?2c1p|>lKyq;?VwnZeJRrUY
z9|1Lt-a6)m#d#O6M**>Uh8B`id4`q(GfWWBD+FPRy2cwF0W%EUf{{+QWx54O(z?FF
z#}C{23C&`z0|n!AdQ!V{AwUK{LzG#_MR94qX2n32wGSD@HGW?=h$ANiVv(~Eze7%e
z8%~Kl*Pw@b?>ZC?9Z!ENJbV6yY`{-G3k?hX@MO3RN5UvZV&8>XUh5qx*DNhL!&)h3
zgWr%kZTOJR4yUfNuAkzAwo;-z$A;1R5pJYU`sQ6$L4n@C;5H8wD&$r1yTOx|2z68f
zx`n{=+<EXRgwt%Cp3*(WTLh!eIi)!kJcT%wpfr>YotnDeo>Pc@*1rQhC&%W&DByRg
zMs%)3l^cdfrL)PA%T%SR$2ENOW>?gTCL#?~?DV4eO1RoTa=WyIU!D7UzALfJFM~g}
za^Y59ir=me^ryMA*t9)f2vTkJ^B^YMyF&(XYz9NXa&PM(T>X%&<4Uhinl$&obqIyn
z8O4mGY+QB9k%<BpG#XiTIPi-uk8uA`&U6!0c+mVjo8k<QH*rUV+@jVFw+#McpyE0T
zY)Q)NH~FO@zeK66UIXXQ_;{b*1P><#B^ZQVq=yyVzLU9AO7*19kxYoC9I{2IvvzW~
z<r%CdKht_&Z`u*=mSy_ZG{KggUR-x?PZkJ#_9=F$l@8`)bQah7>G|$)jQSkmup?Y-
zRwsAlR!Zs{j2!vnYz6YCsIIg^Z76q)4(CkI+h1FrfiSSWx#>1^ZMw}X{<1*|+?(Vo
zWO%>6O7~e^uR)IrN}2&3+8+*0JjCwcOEw(3ZgY>GR0djva;;m6>w_~X!Rf`rX?w?|
z3<cKtSXQSKt?cCk3P`n`)Jth=xfO5*{G&w@2K5QIt=D;Gpx&BzW803gwI1dIv2=_$
zQI_n01>U<6L~g=Zi3#o;yV@7g*Z_aLKOEoiErV_0S_u--A&ZmIydHTNF>gz+PlXXj
zVA5J1rcs6pQDV^pI<K#XD9N0@H{NPALl7pLyJnyI$7b{M>lJmgKUmq5jp6^{Vm<M!
z43z`OO>z%^_y1_!cKoE)wanFbnHH~0`MbG1R}YdZiL%NwJP+Zr?>H2X!s)7LozksQ
zpqbz*V55${)00Y>{_IgJ*gI4}V&p7w1p8rz0&?r7SK0y~b+Zg?7rl}FbZZd;^JoXv
z0dnyThunP>O8c%q96&UCPpHvg#Jm^Wdd(^r%quXcms46LmoH3qdiSRoMhSFc>E68t
zUTcWnl6zqD%HmZHQz&N5UfZ<VTPfZA5_j%Jw^6KdT=l)6t=T?KlTNDaz!L)_;_%I&
zzdUom+L3?@F{G%p9A&u*@w`4waBLJLTB6;$7HN8BGvA2zDEjl;^c1VHIx!RqC-#cT
zfgit^IxCV>Iq~K(?NiAM(qW6Y;080l)C&wD!W*YKFoqP*(eCG%%<tFZ<DuhEDzM)(
z(dHk<ShH4imC&Y|<}~!f_L$ULf6kmOZxu2#Qaf~s)U;n!S5ZgZ7*9Owu2Qn_x|ReC
z;;*m1mQoD}@cM)?s6VS#BR0kDKGcrtz$t0<=G``&tGaW}2a7R097{;VG|!vNt%GlL
zT_v5{j%9+bmYE4#FvXQ1infn`cW5OjPYZ`98@F_V8uuZBa#4U8qtuY;T1!#={^!Io
zY2uMd;^>MeyIaNf$q*m!ae6=f_A^^=fY)%ye%8UJEEz}Fr69PRz%rxX*mT(OoFu?@
zQTzP>PUoemb!_qciv4`>a=s~IBmK_+eZ&UgdPHpoG^j+onyCQWFA9ZqQB55pksVGn
z*KAtExSU0u>?1!*TAH=GA;D_BH~RLS<QPK8W2r6lyd@@wXxB>`xT&?JUGe$O+oR%K
zSy>}FXD;E-s}T$~Yo0aKy-Yq4j#Jf-Y??L;W{Z&Qt?tCvETij;Q?2S2=M{tU;nFg>
zWEQ=It@>UtoJi<-+Q<BgDn5ydj=*a9mY#{dflHi?&ZW$BG~Zu{@zkq*%I5mwGKq$;
zyy-DkLOJQ(c_u6G#<lO5T-@sNHZcu#V#jPbqOjdw6#MGzjHiU<Z)C^{lj93k{8s3l
z*V8kITr;RCMe~)=dN700aV>jpE9cndZ4ir4<!wkA6cf(3rKlM+otF^>`E-3;OWrfw
zHC+6wrMTkKK~;}f((OtVLjAdaG*0NT4<okrB0bOS)(8K*8P-ILwlDak#F|+7m=-e#
zvqRZ3kCI2U+$L_$G0#12lPX*f$)D7&&d^s<RJEJ(RYZkZubrlz0T$JQWpk=EOdnR~
z@_qCh*1F66zTIN7`SRu!-s(?#6l-Mq=>$LCdJPyR{=(T}sQ8JapYTT}sjguP6~7z|
zl%SJ4sEXvxxy^Qb4d>5Z#e^PYPW^C1f)YHv?!^6}&*~35)4`RSee&jXwOF2}lj1L0
z$!S?(`@9OrCGQgqe3{JHrfb3(KC|q@F7kOYc?xGxT?XA{`SRyJj9-o~gC1<c5FKxQ
zg!NBexkdxOx-%u}pA)?^q#xk)C2)L|F>Q8!jK8dl_dbJkQrt*g!Z<(}%O9KHuBfsH
zn{igyIJ|-$zGl->hKGCJ2T2zp1|paVi?<dqNCg>b@6o;W+6?Vq)5JOjq#@HXH}=)X
zj@g;kOF>pkAGshE`rhF~UfQV%IPWYVwXb|ZD6`ZVQ84=Azs04Ok+bol+)XE`Asp&!
zytA)>Hm0p+KgOD&Qc5o`veVh68CRB};}lSctG2$2{_1c_Y|qc6Lcm?8U%}UQ^5c-B
zT%vxv+lPEGq@a>!KJLhpXW1$QyhZn~K6M~$;s@yr6-X=&)sHL}b;w&Z=-i8%Qztqu
z9?+e_DTnK-+&z%0<7hJHgz{T9b<(wh6{vdAFF63{B`#ZLq`^x5I%#y8&-7WW$y@yv
zOy13qX@5N>0t2VVZu1^F-s9zGvm&fhD%x_q>}Y>I`0=)ZYEce>&Lx@`IQZ5D1Xt;P
z6X60czx$k_+AV^V2dz760V}IOKhg5ZK}acwlG&S`?Q!>hA1CwR*cv`@{pC3)%QFhc
zT<$8NG)i|m7PDng_5gqU(o=uSHL0TKS6B)-A|O|*FtxY~nd$1xvJBKw#m+M9z|P{4
zNnJ+roEYKoay`Wb7(*b0<x%srx4hp`T@W=5$!xRk(K~lmadXb9y)%CENGocD{&{UD
zjq{R>PnWk_VJBvu);&P&j|6mZQyEtFSW?)_h7a;%sh<t2uj9;(qZL?A$<CKIOxLJM
zoQc<_jnK0GYiR79RpGFKiz9M_v{ty}D-?U{9o(q7r9r8{maOe5b|AJPd-C>)Wnt0Y
z)9yYHG&!~}fZ{K~WFI?J9vK#Jl3g@IGg(hD5Uu*q^!-lWOd6dq&saI0QW#H*HGSrI
zI$v?=9-Hv@lDV9+m)wVLYH8BsN!BuwY1a0d2WZC9RVpU4{i6otO0%v@=8;E1@#;9!
zx6Prog)Ps}2+l0eV%g0!<P-)<Yt~nxZLtV3s@@WdoA%57F+uc<2$(u?3L)f*6$$xM
zBapR`yH8?BfJ@ll1%N4i(Kj(j@7e-S8Vj{u*^P7_@w0F}^4pDmzM=#xmYW2obUi5_
z?ZABfp=sP;&aCe7<8gAmKKJ4Vu{Z8@{m9$hz$iCvmiy!%QiV`KT2WI&xgmeoQRPoz
zA+)Tv#t5v`ankg1usVM=TrM;w^0wmqS<k;`P!PrHF4)9JSAc6UGH!3-Oi?vgvdS`L
zH&4e`#7A-93L&^yxrtv<fAChEncft%UhX?i2Jqv$-rK4VhA$-~_I3$JIm^hq@M-^^
z_de+xg9S$>WsgP&qpNqS?ug){Iw=*09=T=Jy@_Q>V}wmK_N{b}L8E6Qbi9!}RDG#{
z6YMONZtP|&=3aE#-r)^747&>Ktf?AyR;9P^lY$x@&SLbP-xc%aFUoGK8Ird?xug(4
z<>w)rabTP!{&*mV-=L#jv-LQC8oJHm5eL&fAGF$fM$ishD5zAa;ZuAvz(C9~({}x`
zyzs{O<-2Sme6ULrvE}GuPe&mEb<a(_oPW2^F7jJmi-AqLCrMau|ETs@{#_0um_Qrh
z2APQZ`JzKq$J<`0i!&kxB&?d`>TzHE<EX5{fVi357vHT{RQDvkbi5a1Kv#4iV|*U>
z`Kd&Ac1k#;6ZTgnIi9fu!BolSi$&E&F|DyOr6#8yHJUU~L&Q-4{Y&!+s*ifF)B@}_
zL}C7v|DeSbX0@em-skti{eJji)PS?Q&4l;i)85sRL2D)7cZ%k~$TMrKa{c>*?t+)P
zI@uE){qoyjwCI;Ek4^y*nZlf9rc3Ki8WGQ^F8%c*<a>O0?M?Ag&~?1wFC0-t%Y#JM
zM|n<s3(bAba$s9!XL})eetq!aAs!ChbX3O12fnMWElEk)B`?yUeB;>F1MCkDE1K3s
zR25m0M}|M5l%X$($HJaA)zU&G^0?hQnpryL*?odS?W+&i&7b&}X@8|=@-*HO(!RCS
zY{%WZZNbDS*#W&T7@K?{P2|uC%1jS#MWxyitYMQz_g6iUkeEIoF*Z+ziB%MHV}l(8
z3s9?8A3EXDMfXQx7rvCSHKRj#-Sf`Lwsot_?%8{aQsNs?Uz$fhzMr0MLXWtNQ<TLe
z-FFX8AI59W%$2u`It}lWP0lTi$VC$B=W>s~sP|aB*4l*2|DLc{(0Z(vw~vonUfcPo
z&+&c6napM2{MpLZ6Jh`Jr_hU<r>UxW@{}p4CThOpFRMPkI*6Gq+ae>+gOu@(cRn?<
z=QRXnRjfjILz@6=o%(Aj@f%_b94F~ysN7<ZdP72x4srP2UeZS-M}rZGkFCV4upQhB
zZ#v=J<j++*|7H=xGe~g>+vc8V_uVZjw?y~FvG3=Ns4t-_la^XB@$_X4snP1?ZsiMm
zxcY<xx8}CbzALgsku{kLr0{y((L2K|qhHF#wsg~J>}0yYrGijSC{(B^dgbxvzEYjr
z?(F=l{DtaRgFrv#_K>FEYqQmtV?sDOVR{i4#&K%P`N|NGCk(nMTR|P|I2dtz@0luT
zRVx|RmUXcKiyYy0qX}ie*Af`DS6;vdxk^FH=8Km01D%Jl@ujY&%%-==Uv;PN&zrba
zV-@-+Th&d9^#S^&y27(}%lHQ27eChgsu!~CGA_{I!SSs8mexLNjp_Z_P{@hMvDTb-
zX7%~;tN{~oG{~ad5tUC(>ol{aQd7u<gBBb6`(s;K*Ror_%C>4or}t6kG~mr?HOp>(
zC9qqxWh=1;t-9(QMf^Mjq}#?ugKZ0PP|spNE6II|ct2eQ5+9R(ZllGm``^Ut<2#tJ
z=<2!|tbMfnvFg@)U5P4n{Uqrf%wn^eb2^l5W01ICwrd&7;<ZYINt0d<#fmY}PI0-I
zOw@3eAp95-No#Kq8>Ej_qNq6~?2G@2!Tzmy__vy~T~wymwo$rd=U=b0AeBTy*bt}l
zrBqgVYu-d|11%rr7s(@~#{8E}Lf!#)@f70>+IbTq9kN}jaVDyT6OXFlj2Fxw_Zt+=
zksI*MHb}yCR3_gV86&#tAI9{oM$IxzoKunRoRiY3@86)U!9Tqg145PYcP@;*&mBKG
zsJ?5zA>Bo*HYVM2zH26o<Ldp;zSz~KAax}?b$5G6E3i%e$T&_=04${_;;dt3d+DdD
z;1j;&PnGlY<qd=ncD=@Xhgt;ecA`-rW=9>)kX~%2xM+Xlb51OI9x6#y>|Cf~1IsZ%
zaf>(|&J9GsPlS4_6#8MebtV_;7<UW=Q3U4){x_*O7xRBgGXGDhxRQc~qLSi2srXlp
z{okeH%>PQoS&7+LSicf&w*UXBINSe6DlYaP>9@*1!M52~)NJz=C)@ukJ^r`)?JFDp
zR}Sm~aQ!cN?!VL9|C;Br{HLq%Kl5A`cFwP}|Ia*^nT7Sg4|JKobFnl3U&`O2pCDBe
z;n$$x<~AsZnOUU~Hb|o-IcC{dzxPjXY>=Xnq6NBe%!18s%u*oEx(;?ewLAI&yBE14
zn+&UYYTC2S4Re*0%+WbQQv*$j4XN>{ctTMGm6b$9Kze$6CnkG)fA#j{8X-1;e~=9t
zPJ?lA>4@|Q`(aCR2gv3!r6t0C`%wY|3a;P249eLGn&~5)`7Vr)55kCq)C<HB&I|@o
zrmS8(1QB!Vv4+9kkLo3cb-Z?{Z*C0yJNte`=D%`|<FBF^m-7H{_iy1F9$6iSKu}~2
z&<;2^!kLgg2RTLgZjCa(CqR-aR+*~^O+q|9F%iAdznMm`YfAGPg|Ynu@yq=dkSQcz
zP3+ur6$mO#`Iy^HXEJ06Vxg(U17y$$)fC*2%@hPl?U`IffN%@(Z1JBEk_XZJnyHov
zCb$9$@wukCV-K$0BhU%zQ1ASScS&@$6;Rph$MvOt6FMi5?IBz=l=1)7L;#aZU(kc(
z8d?t~+jH2BD0iMo!sLgpfo{!%u9DhoFfIqevRe*<t{DJKJhC}7y#R6j;ojW*rbl}1
zj;)ze)O#3@;_Z!seHwL>k(1bjK8&#FhP=I%&;%*6-lGo$fw!)09k?PL*TeTuH?)?(
zip!iApkzWFEEq?FfV_5caDZ`e1`&(_iEB^8xOVqIM24O=ke=K!)`aBOM?!)2hpdE&
zi*160d4%p8Tj|Gu@aJyz_v!dleaA9#Z~_~zn#cuFi7R5m-ecyLMW}zq>kGLuy+E4Y
z^S>aZ^51)!>drwhJTpTAUGwBk2eKLDCT0fKx1fz}VE`)<;{02{JC;2_ZFJXlLG%gr
zL2`15dycw=J{f?1ML-HQ=DN>N+#wTQu3AV4;%9fr+Qp9=xFx^jz?YmLPpD^mC<+*x
z1R;>ap@F5N?E|KukKK1*$ERQGJ8<oFOX+h969`0$cdTo8lbLu=`2<SX8r$0*-Nb_0
z{MO5@El>>Zkq3YGlWP+4BdRAIUSA#f1X>;BQr7OFK<li2tchEj<5;`LwMMYbkDu1K
zzHFfw&RkUX5@UolJX}#B`bQ@O?E#Msmwx<!_fF^R6Msm7gWxF!{z!z%5E}P0Aix3;
z_4JR7^n(Y1H|y$lbbtWlmp#*i)`6?mAaMi;0e^KtYEOMYFj1(Yw!jbeAVCKFB0t4_
zKyv%ZcA-Hih<=0EoDlvd2BH1{>>h!Dm=U5<fl$ANwSe?!#D?L4BzlYTf^45jK<tqM
z!whx55u!##?ve#MDtU=AF^&RBL9wxtzKZ#;U&Z-<6<@!KGag^X^jMfdu`|@(77GDV
zY^<!Woo){e$YB~-J<oCIgJAEKV4Yb%upFIwms7dZQUlHfF{cIkJAh%pt^&U-E+FPw
zU@ou@Yyb0K4mZR8@lUai+VvkOJj;t83V-|Vbd2qRl21?EU=u+9+Qi%;nA&9j&xC-h
zKbtd<ZoR++hrZz+x2ehg&sgeOR83y-04+_;_D|HuKe)TQ_o3k9{kP1pvi2`;N${_8
z5Q7`1b!eJHmAv?sg4+W0hAd|S#8Dj+L@4Z(YXZcTT(M%;y={h1ZxBQewd$Wrs8r+I
z%oqTAOl-F{zhfic0N(l_E|JfI5Vig5(hx{z|IWf*U>Ys*(U-1mG7(xm1l)$1*iRPr
z5d-r0nG;mg9AA%EF|PH*L@dw;2m}23X?0C>;^*w(^jG+O##&yJipaYc-q{IiMF;4f
z;^DXEuM5Nm0nmqiKPPwsIBTY4UdU!veZnA^Jfvbi=7R`VWeWP)KvjNM<QOJGs>NpX
z6I`Cx1^P4nu=C4A9bfxEhJnAjfexPNx%-VzA5S%Wa-|CNTupD~><t0=ICF*Z{su$_
zKKz1)aDq-V8kZLu4-T@W4axwF4dk@d*&;aJ82a&qzyB)kWRyE>E5fCwadI8aLT4k}
zWUs?<ZIZ1WQ@d6)uMqibWZkl#g-apIqO%cYW7IZy$|roi>$RlS`^#FTt4VDs70<>_
z%1N_k@$6B?=6wZSedx<(9YvspI`}#piB=(^TD;2VMjxD)O0B943Tj6myp4Gim8={N
zcXJb7<8(hsWi0Y0mB_-QQMU1++flJ~2&c7ErJ<bj`5t~j45ZsDIexo9;&<}gL+#=0
z^9p4>k>hpVt~uNW`Jr+Inj{)b2d7>b))QB*c0v8}gVzdC8_BaQn`M2XzRvykNvl$6
zvs3yNCLFy#{i<|NE^0}C1Xp%sASUZjft0OsT{F~Od3*r&LFMvg(p3E>8fCFM;aUgv
z*HU)o25?=4Ssgw6Q(0QAm7gQ7bK<$`1RH<JR8f3X6Lll0^$t_zT^1B%4RrNW<f5hl
zCW1NP^9$zT-n1Q4$a1l-cJ-!2AFEnNe+AmK2M~6|_vLVQ<>rqvsV64|KT>`wZ*$(W
zU@R4fy7w-=EoVkw99Yzg{g5P8bU@{@<^asiC03)fZy((L@i3oz{+LR)2o*Z0u5ujO
zcz$`QR&4+`3?yT|&Yl@Y>MdF1Zr`CDWj46#liI;_AbECB7IY=YNVTE;rc8d9uV9{x
zFu7f<<6-Xpo@5GBtds*Q<rE}u4TqS|FDf_QhdYyx!YrYgcdG>^R)yh`@Z!BVMBb$S
z5?Ps3VBQ&F%c(y-wbVglHC59si`;;Jdnr6Mn&T`Ed$6^%*EDYYq<tR3abB@r?Mld-
zWZVb3yEg9cEQjHH`|#A?5-U>tn3wbd*sGl^TZw+^6lM|SI!wZU!-#-(FyCo^#Fhr%
z%e7uLnvSk!S=@*^$u=1;HUC>t+}SQ?ZXN~q0L?FrqmooeRd2F*FOwwfwLnTg`Fi6M
zLv3EaKe7~apouPtfiT-;%&gR`vY;xF;!CY^E%MZh<+W(P-_3P85F!Vzsta(I&2ku`
z^nvzXRPJVQ_8MfrgCq=(=j+?Bfcq@>Q-LaT4NcXT)GT;G4}3tiJbge)`;}qIb=r*y
z_n?2>{wvDRlap5X{H~DncmkIo^m>26C9`&Rn`m{yYNT|nW#?;w>K_OS(-u60zBfO9
zE(ATqL|6h?%he<1TgatnViS^bGpT-NztH<GOE1gGJy}zsMnCKRHZfgj?ow}=r&c9=
zrHG0j49t^!<BkifG!-y@gXHiFx`PC>LO--W6}5;Wx(LKob4y1&N3b7lu*0v`p66(w
z><ifx247v9QpYuZtX^Z4fq|lj8}QDaSivm3f@#IGgEAA#-9$5pD0fpnguf2TcP%=1
zUS#y;n33u<+;hUJ-oOe?h3q@uCiqcnNfmpmy2kJD>kZk`t<^1!Wmy_{QoU$TY-7qh
zd_pGU=fDxoI6&5WE~7O8bz1P-I_d<`K+O_fl%;>%&-$)}jcJ22PJM=&!K-AnO%+J*
zvMDG3K4BpxwCt*<&q#Tr^YYTNnRx!a+Bx8sxWnu~<C*_UQ5(st51@h!fyFqnz_A(U
z)%S=NdipTuLh*C47Ol+`QKqDx(@`%=-@_aJDl0`d1tZeV{yW-TZ4I>cRs)%^_WL??
zhWpPf*uXA25|t_ZS=?3~N4P4K797{v)bjLi^5>j<k)9aXKGzgn`K9Jj>&$`yi#J2I
zVmsMc-m`{G-gmT;og~@iI@kV`$8YC?*UjzlJiqThFfZ-I2Pl9gSVaTUBt<=407V6w
zoXupTPC>gkO2<`snMb^|9V4=?6Jpcg%d`kJG?H!P&_D5d1ADYlPTe3LTHX3UUD9iv
z-)#1m=NaUw25BR@P{W`Qzvp}c2itWCFTdmD|GjY8ie8?ZPhS2Rrsf%R9s&#f<OQ-f
zNUDd7m?vPx)+F6Rp#(E2O>%TP7nYlMrfMT1G!t4~6Gxlv6y0CFoI|O)Q@Eo&@7Nmw
zbskTvG|54;ps;f<w<PT!&z#i)x3iclcg9g_Af;g6eb1-0x}gTqzxfKnuRB4X;~w+c
zb>a72{TpAMFtwK0j9E?YLaf>M$g>z{c=A@0w5BCzN;$%kE8dZ^;O?`ZrN&2f%%taz
z6NgY-_8`B0D-9PWAklO-WmZ}TZP6;Xz??Wf7FU}<YhxX*M{g1~7eXFwi>{MLWHVag
zV|?FLU9o*-c)F}%_1jioi|ShLKa4n<bDILthuW{fUBBni_k&YC7YsLyc_2%~`l+3?
z4a3L36bFEk9gpZ?*Zg5ufzeS!I5mfjPm5qWSXxV-<fIwvNL8V-AU*aF-7PKou4x-j
zAUh%IxEFRtYkvP6oX?>Xk={Y-qnQVhS~|hNEo2EzKXv`ah2kKDFLuHqc|dcv>^>Zt
z;Z@^kUYat>^g>hp7ynAEFv_7oc(ajHE1it}@Lij1u;JD>jls|b-Q;&##Lq=1$N4Yt
zL?ck7uX{B(+NAZ3o%$IpT`n6T_p^6UK55Ft3cCzP?{QZ5^Yom}64uHDOZ|%S_6?Pl
zuvm4TRq6s-Uys##y4H?|P$7WkaJvU#u4%&aBfQf!fM7=VFl5mUcZ45&?T>dxkFa+k
zUqCq#LWTD2a#na+hWlf<bep2a{;hnG#&r*E)kXRseTG+9lDd+;fSznXKsuh+YdF}*
zM<n<~utgViB4>9q?`bI5oRlwQC#70EfNF@g%us(*b&kLHA)8+(${lC>C&4DycG~Oj
zJYOcR-@6jkVvDZOZ>mxO<RUzBOrLK#T+YXaR{Pen@5&WFNSAJZT0?v(4FO%Dgp9pe
zPcQ6ySd;T9Ws*}FT{?SXouWP*_cG7vktFOxe^B;>tO<1oAx|onpX$cnxfni;+eP7}
z`+lyd6S(gOFKP*YLRN)3a3$V7oavXP_SHzw1QPXokZC|=c~uM@RIs*56rSV`xM*U!
zL@e!JY(#C{t6v%9`b=O&T6YjGH&hYUQ<1{ZCa($UJ}{ipB1pffE^y)?>eVz97(br`
zIDjY&vQv1fMDN}>KMM*B3r?;5#pMn6ELa<WE2F9MTXkIb5VllBp9*h3ugTMQ(P!3V
z5LM%KXte#selfx2?T|5PI>_Y;w2`8oD{ao_j+Mf~8u7gClq@aK*$&eyC-_8C1<Um`
zn0B3j)Z`x-n@Z5{sfLJSYXOl%E!}c;6fL*yLcTsBel_M|pc;HF|E-MXyGyWkN?QSv
zMW2gy6E0D^E+yJVsCL^G#Pdy#57)romli6zTh#~;Yf~Pub?U?W5)EG_a*a62Js`8i
zUg{$_eyA}B*Dr%pQNF+2AJH}P#CAKR+CS|)I!4$AK#-<>SJ}5}@P6rLbyCyFev8>9
z5d9T%W%QSd(w(GqZ-`hd6tdsZMQJ|XOvcV7QoVzL5%1i}O0i7N&8xO(={^(SpHt!6
zZG`!$pLLs;?h~vP-eu8Cl|)Olc>~(8bL_bs?YJHJgkTUj<!EtPZdIN%pa_xK=yt??
zF&lmTYcT5#V~#kSXarMcooRjJO#LnfUCPQlCQS7e+{7*W6`V|#x-}AsfF2)DU?S|0
z?J5&}GQgf~XzV&{HDmXg+kL6l!$wg6S&a7Mm@bBhtpe#UC1+Du$qC95hGgvb9mn@k
z*jesVZ6dG3A2n!UDbv7m=G%uKBns~VG~W@y`&>9VX$Y(8oBF(09J5%jgocTm^+f-e
z*)^1sr+e}Lnr|aQ(!1pv`^ff0*((wzW;}_2qM0LbN61nu-hj>*yAxyAIz7e`{-FxJ
zQgcS5#;w<IHuODBxNoSo6hF0GI2`5|d&4Q$pt9TFJh2TE6y&n5?>~$e{@}{k2P;p9
zxy#Y*VqK|X=nOQoF-CXw5S*uW(7sz>UaxvQ<2xhr2c#z-Pq3LATD_{Bb3J;RPiV!S
zz26?&vApI*h)noDCFyEPjAr-H?+3P0Yo>45`)N|&NIgougho7Pzd036Wp_ZFICsNP
zvuJ;hU*P-TxP_9C6CDri>^8(@Hmhv3`5aZ>7HOVKSQJXf>H9oT=OLHUyQiSvEdD%V
zH-|?{f%Z8+UI%@W;SB)=D7%IX)?BL>=6qYc@k`C_GJ`J2?3X2$AW2vveB<CzkT*Ob
zo8_RahQg55ScrQOTjvjGU*+Jt!#3pttJZES)hF{t`9?0A^^^5@zCW_u``Ss=AriTR
z*tW-`Y|l+GuvyG7vpVwdWi)uac!R&_3Sq0a6DUGd3)Sh6=%kUU*I*PHRv3*)v#(vG
z)(`IaDG69<QcTJBALelg3}{7F#jq|4YdTR06C9OwiQ(gyM{J4EkYV#VyT>tq^<@!v
z*E}nR-nU4A(OI;~0`3HD^g^j*vQ}wLPFNqhe~Q#0;(3d{VAmT=<giqEseb!;rR4ev
zl{Z%V`wTW>osLW0MHsbhmMJ$Xes`jcxju1GGLL?gMT}S=rTaSjx*3hIo%HAxcJe-o
z{O=mGYU7Lw6)gHUUB4h2bD5?Oia~h@f-)A0si>scwh!|^P;N?u^VPo`@sLE#u~R5s
zaJd{rF+8)YXbLIhVy;t`(|7coC!T-y6Tc~rNmreM^PH;sll^4gu|2`?2-&RXQEzxU
zR5wb*6qpvGd&QqAgFmG`-jXrhb;vajW*_NeR*kR|E)zU*`QT%o6TyF~MVM&p<9Tbp
znB|v?V9AQ7gRKr1$ul}|_+8iy2mePLW{T^B8#%qOru2XcKVVAnFayqOCq?2hoD!Rg
ze^1%DUiZOibmsx~iE^b^UXCoPFtz;hedp%eoP!`Fe01VPeXKCe5IM_%Sp`ZF0xK+E
zL5geh8L89Oz*sb_mAr=S9@9e!R5~B34h_WbWL+)vc&eDcS<+0xsCcMqzAIMDWmXs?
z-#R$$d~)P8H@{swml{W4wvMHDtb0EGP7e{V8%|H1pv8<l7r{1Of&POulC7g1Q}~pg
zvY!RN*xJ5P--7_(P5x6TCI2j~cw}W%T^YQtXWb_2&333}^@M7qY;GW}LcyzPtV_GS
z2O6r9M$y4fYgU6-$n)WGILMie&i5(G&5S(APP%UTLSELKu>Wpa+I4K8jba`<T&w#)
z)!pe2C7KolP`sn~qv+BbFa7$`jM8M=U#aqievvY<;jeUuvRT}5QH;wv{C8(vnr0z{
zz-2WRmbi}4xAJvK{<U8^o&al%E%rjI;Bz%*+>z~a4E?Ix%AXEa-veCSlRgID8uY4O
zOFpjGo{_iG@)`NFyOQZhFG?(J7n`C84tS`=NGhIpC}u+i@cv}$Z%wlD3lbEA&Gw~g
zM_r&02o`q$cI8N#;!iB92T8-1%sxfw#N$YWUj4OJ>}mhvS0wzILyh+U3Ig^Fh7Jyn
zo|>;nCj*|TkH{u>d(qPrEzQ2{yRpt_iQARe_uNaD=k7ae>s-b}8sojBt0vw2uH9os
z6dP7OGelR_M=kclIvliidBX&>e2Co9uBJD;?(Z&<iQMc;T{c=ynVS+!a;_8bEaS`J
zd-D_w4f#YbD2tfV1M|SVwcVoGV?>$p^7z29OQmry*tF?J_t!(}S}YbmDdV1Gf~f*t
zu$fi^wvF?tgf*SC_*-A)0Jp{Y9xBnFs7zTaG2^^p3MduFUY;Xm?b`-{Od$ej*nWx6
zOiZBywv3Ixl!gK7fm?zlPi7DonE!*dd+N?C4AgX;R8(;#so1t{+qP}nwr$(?7u&XN
zTV1^l_E>xL+TFWH_b-?y^JKp7yst;lz-1A^zlbcD*`&R3wUWCCn1VQT*nR%sO-2Xd
zWl9;x0DK(v?3DKHMb9Er#W@?Jf=qneDsZXuAc~6G6(PgB<BYv-|MY`tDt4JVOmV@{
zn473dE1yr0lDUPM2-I>v2Q{D4tMK}hPrg15A_OV^6nA6N?6^gfmWE0tlT)0lb9Xy6
z3bBlWMb9RmO&c|?!6uxL&b*FVbvI1k(2#O^SF&`Z9PM4?gkz?PEuT;(dTy=w^>LK4
z?oRijIUa@ldP$~^PH0B&6i%ISE<5Gpx#CuFl>zJ~;{*4%E6ed8K={i+^N`^S>ixCz
z0ev1-U(Z;?bRqG-1@cPQZNetX4;+#Y3YHo)<~Vz9a<dXQI$TvU0-joqyRM6F%bV8~
zIXLa>vJ)(c83qWp+cVXXQ}I?`SdK3&c`IFxDWYAsb^_I+$jgK67_#U`4;rOTbT<v%
zLgj;$2WYGg;Hicu^JnCO!%uJ(r~-f-Q@ACB&=rUdEB303WpQ%qp)5PvyCD?1UT4Ad
zuMy%IdJ~{}=m7XC^I{M2LdmR2)%)uLp+~B%bV2Z6&ofWzJnhuWlZHLfn?=N-mSjH9
z6(HM1)3#4aLB#C(TnOsVN1oDUHT6}-@J{?}pc6vmqKVz+ps7)fzc3;M8Qz`U-zi~N
zHb1aZr-ATqq&raqA&*SA^>OKmlu@9Fpat2n<}TSo*<WT(f{}_=P<14%<JQgcIQugb
z@R1e#Hg8cKH(@rgGZ5)+5l^QfeDmcnS0GN+=^IPB+wY?gh~i7x-Bn})`PirNDY`X5
zYg6=0^^me3zSI{@H|?G+?QB9tH%lRrxVSCn6tnTNT)a;}TX}|EIO<DMt3lFB_-9t6
z4wY2M*EJ%dl$UKkH4Jde%y)V45|+_h2N-!=y$I$YrPdKGgwdY+9>+^*51a-jQo}gL
zLr+>_O)CVWv81ssn=b~FLm}EZ7Jl_iX(7+qsRNF#+~zx6%4&_YqlGWeQs(~P*=1t(
zBw@_@*h(Mk#no92kB~U)5+{c6C}#2}-1BJ0Gmgbc$w?h|;Fdd&(^l+e^aZqIZ2y#P
z8!~y}a-`npQRqV-cX7EtF<u)}#9~xbbApo}1v@$I3(~^|6Ro)2KzUwSVUIX^y>9$6
z#Z9iLWt~0@C>5K{KbpS*Hd>4%MgQ1Q+GzI=xl}wm<W^<vH3dh=zG1V5$O(o(RB-n+
zl!9l}1$DW~(1Yb^Li<^jkkcNR0~~H91r>f0SwwP9)XPbv6|jMw-vEXOvACJ&mZ8Qp
z$S7xgo?0*iAJ~A#)Z(uq!Y@-fl+CLQ8iJiG@G$A|jZc@l1jsSyS&+ZBuaAO>(oem$
zsRq+#8HX*@o~;3cV@Wfr8{j=Qov5HW-fUE=C{7i8A|gMO>-B2*WbjWf)`I1m&p8^Q
zN^iHs3W%NMn`|^v5*=4rFd?R+Pdx5v=V$?yyCjUP!k$pfMA^;L719hrGx=B28N7V<
ziexC!!OqY|5=5o9hl@sDBQ2^#CG+nX2ZP+diN)@<bo4m2rPfJ-FM7e}>)M)@qSR>y
zR|wQNblE@%rn*mv1WsFAY7BTwFX54#J78F>%JZ+SA2RhPeaxh4Q8vOU){3sS1nln!
zhHc(g#S{o;hY0+lCT#VcSul;+CJy-;En!w4eJkEUXw8E-Y5R)FkIBbN->HTxZoa*A
zd7XiJD7VLkx0dUB9!|lKCDpV!NiE)tGP8^*88&%P@#{r9Y`x-Cr&L=r81ttpYPWTZ
zf|?TU993t<V9^MIs6w3D9rQ2uhqSw{g9S5T5QU~X@thVtx3-hKD(1x7Wivvo;!?9o
z>;5fOb$l?nfUTKReFa4Q(eBT6S4U2xEYijsW<{eGsLN*$^k-&`W*sF`slS%~vnT(o
zlfL35pshPepYap-`i40uFQPrzk&IJs+h<^SjFii#j;PHY!gW`VoAGIU$Rq{0n)^!f
zE$|LtdVs-7qxKyLKtbri{Sn!F6<{TeO{Wu-EbYgQUi?jI*{X*|M*(rwIU$QJ;$F&k
zcx4)uUJ@>P8G<CE32+f{g~6_ViR_LVoXuG63;*^&Ra#_DzNU6QDZKw2a9tl481*tN
zLe=yInlicO{DFc@JkT8{ZR?`so3MhqT0(d{5s9d`UUGdnQ~r0EMi0aW=c>QouFLnn
zf*Q9MZO9`i4X|0vb^b0`?|;D`ywhd?k3q`BNTpmG(>hlPzK$#%;jp0A5=IkdAJsfx
zvfqk$#*}aF?+ojPZ=KpE<Y3YnfRN1-MO^D{7-DJGKaU`%Cj*C9T`ck(Hp8ZkqLs7k
zv6^??8zB6k_H9U|<{)SZ#zGUTrV{y}81%AHs2DC*>)191d||xjk;XK?T(!(jgZ!|0
zeq@`YZoQOrEW+_0ofP$Hh>Y|a<QAnBABO5P1Hd&lO{2Kqk1ARa*-0G^WrIO|)Fa=l
z8`T7!b<fpARgP`!OB~6QxBG*uOH*e4L=nS{xYOcFr=M8e3Moimn#|8%moi<CP-eo6
zs3ZM#4VrqT<)y~cgyM9YU{hQEf*#@~$)vGE$V8wAdvVC~V!t(cG#PYbeqpMEV+_vz
z{3H0gX%IBIuIWaDbtQbh7UV-?WV$)L^ulWqJ$#nmPS@k*ItRot1#m{x!BIuJ6Xq_Y
zSpX#FS4Kjd1tSqk^gd2s#t435bB#cGvj}C~WYmNply%GwRw!uc+E!JB)uem2@ro`;
z)}R5W#IDf`$E9~Lhz_Ud!w&r1ISF+SQgb4=r$?@gBTNH+NfY!mRzLMjYXY=F>3JK|
z3*WYTgayfiJem$7O7f3!@OUD=nR%!-JTq3-LEJ9dRaPyea;msLyyYjZb-0XFZp3w&
zl?HT;{t_Ogk-pI5aPG&PwHlACZq<^?(nz-wF;0<AlrCU^rY1zi)g4I}c5mGnMSs20
z6e|LY7IsQ0(qTv^17Q3Kny}t&LtpeFo_Ji|rQ5zQou6mZ)tx}d$z+jqM}2hMv+{5)
zjJriTFqmf5-JC6?JCc%+P+gK2)G0M)jYohaKyxLeOeN#Xp3T`Xpn3(C57}u;(HS8+
zIyJ6IOl?w6fRnyZU0e;8E5;Z!O)L%#f4o=f^0-j8qH@?<XMxiq-2P6RZxL6SOe<hq
z?0=^i`1{rvdKlOw6`?;uH9J}?s<;=}V8d)VNCt9Jwx7wldy7@9;A3iR?VwA`!vcMg
z4AU^4Ki!*zdF!@8#%fW?p%xiT6b9Sf;QM}dh0-S~$O#Bzgp?_d+ZR&NoBXrCG2iVr
zj?28?qXBGnZq#)%)M{RtdwfF~{loBW)STvVmjbQb@WWMR?z*>Vwi0xkygv{!v4~gw
z&cM%ap^K?FRNZYdbBczY?<>kQ2QdApZ5GqpydDuWm#+mXQS|Jk#GUBm#G>bD0u!^5
zLlfQRJHlesHH`;hN{Q8g@g#)dbD}F=V&IDmLvmYxIf_{(P~_NTHuTr$>;w$Hu41cq
z2H#Uelps?J*CofQZsk<b>Zax~D3bGj_646N7Rm66<tY!mq;5z0wfE0}x-*w{O~2(#
z3b~vV#Lv?TlqwHLgwNwu^!cL}yhwRv(hz0E@aDLS+H~(slmlTC1k2+@4=W6Cet&A*
zR{$J};&vLDY@*2-Zy-Y=6Ov4#4Ug+1<$m+C4p|B}X(nDv2f;jPdL<y{m6j*nnTnBL
z{+K(ar{5+*4)ZXrp~4AmDO<G^z9%ngfD6Nc$0&0b-4U%9_9Gb543TRh?0;j>QaJxQ
zrT~nJ4k}lW$d_9$c)9%DuNv3)9$#agacViQ;WIOGQBs&rg6(m+SN056jkgxg{Iqn%
zq(>fubalIw*f5qCj&{1Gj5<SO^*)|;aNSC=7@z`jGpllmHh%@W5gpP`07gIVqpY*J
z?5s1ah?NY`rl62$f;FXZuUz!AAZ25lBUE0u%b2y=pSDB=%hXxtaC_}6%g^oXu(96L
zYRY}Db8zjr<F|!+I5>D!gdSzzrwdX&gNXk1`nk1@|7R0sE4%?s^r!`nPqPIf?(dac
z`ow`l@uLv0J{k<EJKm6j<pP(XLE>z3tn@(Yu#U0aPmLhU0Z+{1IQucjU_a@&-=UWl
z{y}H8U2E@}x(n~2cam~?6lmsC`&SC-jX664e(ocz$O}ee3#pUkV-o%tnMx*`osF{&
z-aRJDIj6}qtpx>9^P~3fg_+Eu2xfD3p%!9d8$S?;m~=zW_@Fb)x3ge0^G!w><FtVT
zE0A&#gLzFJY?*00a)r27Pr^=e1Rxw-*G={-HU|uVo+MaOza+g@PhXAaj1Wq^N2{m*
zwOEgi2yM92r2o4*`Co#457?x6!@e~S{SLp=KAMKim?zClL43v_IN+#$gL!^^hCyfo
z9MXVcVb;n=9t^H{LQmQ4{YD2GRLKar*4M$*Kk?E|%&CB6o9NtBk0-+um8dDqgE9i%
zXH`Z7IO3(?e!1gyqyQpofIx@up@aiC$3YNB4<pMNN!7K=m$8RMvH<u|*#~n-V}6`}
zqeOZNcQH+ZmE)5a(t~)}Q40hggfP^aZ%pBPbF8;{yXQbfQjJ1Tth52i!c&EPJjJ^g
zcEoRK80h-=OIcf&z&KTX!mD?LaQAchTozqQNY){R>xMXzWUu{G&TaSmqo@sVe}MuO
z584tpHxd3$uuhPV*HXDtEx%H~!6W~xJCGi@zBYASZVLXI-NVgJ;STk=Up}LuYbVLo
z$<E{TUM@)KUN7nhmip4vrIsh5r&zy(Wq&I-HPs$Kp%dK{wNxvAFf|9lD@-Q>t5oWD
zfQ-hB%hGXF#QQAzN}SVOhFvyPp&RcnpVi}}`u#yXr8jrKM+zrQY%}I+8#o}OQc8Ek
z{kwr@-`T5prhx5>|J96vM5KUc+0|y}x95!fH6ThYk{<FPxTSFC{aYXCK@4Xahi%f&
zWeJ)5@28JYhkd4syx_D%Hedz4(T2Bealwei-)gNjcb|9}d+Md5eb|!&CgfY;!)z!e
zdL529-gWFsRY^4{*pu)$eGDDhbSVYrP?1wXvSd&9w0Xg$|GFQ>B5;07X{0AI;T%cH
z<|4t>+bpc0Mkpfi(iiS0sc%ognIWjr<51$QFwi}yYxn%`>}w(#IpI%xo=+N4zaMfh
z*LAjL3&@&un6;dweecXAxB&up!SXH7%*3vh_|HP$1G{nOcd5bjfhLl15lGRpXBhfM
zzZSSL`|wRR_GFX55V?#HFaF>0MH-#g1y^DOTRd^;^R@jfWqWTF*+F`3rU)rF;VJr@
z_mJK-Bz}2Kr;J}QS!E~=w-v*<ay-7VO&(spWj`Pr<powQQS6nW6z@Lpwo9UqksDcK
zF!d^y?uoy4AzO_yB3;NHS{;V>8wHH3#0$rrn#OQBuy9UVG{nLHA8@!B@1gU`##1aP
zaVc|Z0^nj;Di3Uu79OpycFj9%GX2_yUz2_F&kp{+nAFBU*{O;`;Y`$yC}Q<GNmKL3
zMsDB3@P<LbWzD*v<i`A`{@PjuIX7Dbi9%s1Q-Q8fEwtp}#Fl=nDTWbkN|Uc(uGHrB
zbw2eEGV7$sJJ0R507g_d%S%%Jzlx-hMcYwT;R358mB%glr6YngdVvKwG+lQ1QH4*X
zx4fAe;m{!|x$aftpp@M-N4_-W?ZOM9o5<3amW<&K&a=J6WBNRvwImnaGF{`<pXe`R
z28f?Aez}np9OCNfuku}sp^^}e)+4*}I@Pda+;3plTK@+6q+yF9$U@Wfn^g9S&5oF}
znE%id@wEUw5!M)-R%O!!J|L-=C(mK(|AY&VKa7dDHoU;{5lC&H4VS$_yPb8Ld!Mr?
zy$s5&U6MJJjPmi$m*R>F??#gJype3Np%E>gE*m;)sr1nJa_gD}cm$auZInWAMfSG>
zFY7Q`Ws11=suQ`6@|Y9d&P=X~6)3+pOFsV9F3$el&oOMID=To2eO;B$)(5rc*+<f3
z+`#mTFcARv1`grAvhaSul2?8^{cp#wQxAT+N7xmpm2aBC({JgT&AP?@SX|;GH09q<
z8<>hP4`mfF_I0%9hL)4w2uoKn*ekgEz^lQ$;)qAaWnZL-D^ZOO31eW{7i}A)Pw*W`
zZ8x0|9p=Wj?bx1hSh*UMB6+OP^U{w^910-{VLJ=*y<4Cep%rzkYq~;cBn_rO*IZT)
z4wZF-(-rCw2UoTk*y>yp#wTB@tKs+4PNz97TQ+#M?ni*m)jJ+dTvIPKqU`0}bL`n;
zvkd&vU+<H*&n5H|cw;0*?9R)3f@5ZjJ?UGLN_*VO+8ocD_p<S1(ZMBD*&YdjoD{_F
z=DL5a_kL^IjVtFgn_nrs^b($HUg@Y9a4Z;+phP`pD)Jr@kS@P9&GWzuT%}pPt}#ZD
zZR#PsD-iZnEivD6XG7?Y#}j)TZe9W!<CGxZknzSJ|EbsE-dxBrbyS*zEaWw(qyR~9
z&0K#f)J`lZ-;r~tr1V(iX$DSbpJWSYl*vAeJz^i(l*`1#wzobsU3bW-C&!XTrRni)
zbfA%zuuw@Gbsl9K@bG6Qs_yXFcK;uojUBo3FKfNUuQqhVE(HP(UIXTg&c|PvADw5!
z;KA}|Fihqa^ItZ_C9+zChNDK$Ik3he5r6)eb_h-Y$xs}4-xcZ_0BbY!vH6#K>W%$^
z?Hs)>Q7iVnbL_o$PtZ9?>$e%viub&Ij=3j3TQSuL&?U8M^KM}El_^#^Cpo9h7=1w;
zxwMs5;DC^tR|VAw54S{4uK`VOr|X~uEg)u7#6vPUkRnatv3}YuR-S&kiB0ZWXl6Qr
z>JP0gsE^^lk};{RH{e`!ch;)iO!PYmJ6Do3I1?JkrX9vl#KHMI9FH~h)CjPDz0N+B
zC#@Wj*_z_FS_<J!-Qi?8q+`(x&C`R75i1VxF6}?59)RY>F5uMpjO!RB@6C1yi^6~B
zH_sm9Bf2k9<dAm{ZnbX_pp6YWOm~=u;I+;~Do@9^DyALb9Gh(U%gdcw0i#9(Mu$kh
zaSdD*x%L5%n7?Z7kMl77Z)Hp=o2+AkmSa%r63V5{yaZ03?YgVSxIa>bo^}=YwGnRk
zXVzxmnb)&;P03HfEnP~x>xi_lTe;7N|AlI-DVeI-g|!!MOXild#OdU(iN=mKcQ-;@
z8wjaTW+~Z*fbAE{$kfKJi6^*&zkiI8XBq?Ue1D_p$7L9ky+_7&2+@+vF?j6#J|S>A
zm{wC5DDYR+!r^4Y!qg_CMi6tUNh!O97)&23Z~&AWCRQDz?;!MfE9weua0H6H>C2R5
zOwU?oNFa!Hx!ZNK|1Bd92&cRuixC4B3RSlipgE@ZP&c_e=722iQ2czu*o5ckwPFo{
zu=I`=y^mg?ov$0AC>vI{hOVW8#%dbunxq(rrZ(7zt41x0HEL#12J7sIeJ;>c0h``R
z1mK85FTR3Ms!IWwK}j*hA;A6<{@T2>=p1VBLAQS(YIbJJYNzY^R{UzUaO8K_c_RuY
zcgdXf)-OrO+3rr*r}DN1M^#yjIuNj<1UHJNLU1UoxJZakJapk%;Gr)}y>dmV&CjTC
zTg`NiK<gnOyM06Qm;54_h<66x3{D<09WOg^g`+x}bjWeuSsUTI4E-XL_+q`gXDJU@
zbkcyA)5!YOdMckkovN0*GeX3MauPksphmgYTpbXhk@jMbCS4mfVH(tLle8A$WUaZ>
zTH|6bm5*jYvOi0B8`Q3@??hF_%QrYJ`2}o!OL%iVHmZLeL5|2s8?gS3vjCb5>H5Ln
z`2xa)9cCQv7li$U#aK9Y8N5U3lEi3VJ@oWhPp$;RGx|GI8RPJfFu=<-HOz*!+zI2{
zJwki)oZS`LTnxYo$*m@!39G(|iC?1SFA?FCfjrH`H*lLKHW=zGv}mLzd~T!sN3BDI
zK_L9v7OrOijYx`&&zk81S!J9G5_{NNLiGOJI{&j{0_nmsHI>~<VaLBnq^nd=m&Lqx
zTc|Gm`9T>FQRQ#fN~b~?i?a)nfU2_t{p>OogjwO8*I%S)vRqY&^@p>%!h$YVWGDb&
zJfB2@L%;?qg_dbz_44mCCB85Nk+v?)Ls9LV{qI;)9^BHcSs^V%kDGuDniqB*{k3r+
z%}!fuUujfX>+}%WV0n#Erytqr%1ir^WV=EZrRYT6LXD@h6@iA>o6V5_)x_dG6#96)
zT|Q*{+fX9bC#9{vLOQt@`Gk)(%dq^kO&JT<j)vT4kYj0~a7-5kLhG-M<6yUchC}vg
zP^e@D+HeC*eR!9+jiTSA_pvASx1VJp1`bz`ZV))44`*~HxzAq^2c5mH%B~tmp<EAF
z%)<#9l#Q)$wsP$gjZw7LFaH|^Ul^1rZkQxI`%=>(IbHE%CE-Uusq)cw!GfG=O*~Lk
zUj9F3>yNj)Vvl~DWte&nS|0aW2@+3F(#V1zgI%Ox@YD=)^kAIq$A0|p#%t6P2Le+t
z-~bU1LI@fIKG&Zu^K|w%n4$~!?$KDLI*z?7C}n4Ub)?KV;=TP?uW+rD>^}6e+#Mx$
z@`hFmJz|}AO)x_U&ZD>2=s-P1pNpXaeM2vCp857tr!J}BfkCQnF-T;XnSo2ftkB>W
zYT6pa_?DPsfALzO8Scf!rvf}QCs&VD;8n|ERy?~g{wBRPWk&5AcE_gm@B@Lb(c;9B
z6zhfgK}_#Y!BXke+Si-9by?Cg!;ml07E!_>s<&`Dy6t9*fzG^1gHa1#n@AR)i5tZG
zqRNoZA#jVo;=kV=cW`DfP%=cyQJ1=JXIMsLQ}b6r__w5)#yT*Txg6mF4W`VpsjQtZ
ztPpcd{|wBSWkYgbBevp0_SR>L8`<mGI;anOeZ_Z)M{r(Idoe8wz%Uba@USw^n;(VV
zXiBNz`d?d6{-tp=JHvVvEO7~;++FGN6BI%z$@aXk>LG3Wk%n3J%qA&q;T%44M*zMt
zV56zf!d@{Oql>v9*Ch>p*<0jKPTfrfm)5oJD#751QA8;23L#@P3aXBRoywJwxPAWY
zC_)K>AAz$+oo*<M@X-^lJ6yG`l0rk71!#8hAvNr%FtbJP!j|_{DmRLr1;S52={<>%
zkRTR2x(I6yKd@!&G|YI(>@`5g=bvsaxO;BbC*Rr-L5s)f3Moshh^&1lzZ~^u)f|FL
zEO(`n7z(LG7lVfQ7E(JOX?j8|&ZB~JXlOsyzO<2}%|E+si^dBqmmhDGO7IpPh7E`g
zohEJnj*mP=fvtYYIYYQyO!B_;OKoIE@AtqF#59uHRT&-;p6V85@!d9J$2nELH}{{E
z*v>R`$nfIr3vq%#rDjro)xQhY<duXh*rE-&voHAkeG5^X(LKdgUKg;kIN^_-N%$Kn
z=}fJW^24*g>P^>{aA|%rH?GZQ#mA+C@qlg|1uRNu-Gmw8qRr=V5D)7ok01f6<8#>N
z=qBepGnAS%tP`l&clNVt-PX{oV-~W(5O)A;8%mi`RVL%l;CFJFavpXKqt{gZi!lt?
z#tHO70<tmET4NE$Q>zYyvZr()`(CXi(_8HOk`<!o+)DNf<fb+)3C5Wx(l~foC0W-x
zdp}0E8V~o69vh_>ueuYRmTA8WYJ^x87uz}}Z|NJBYP~xT*NOS!h&?L%Js{}hP4#=d
zdYIRZBV)Gj2JO&YOSWg<H!Uh<d6@~rO2v=QJck{!1u#_q?4~QM%ZR&Fu1ny?R^?bL
zGDV2=s^6rH#GbECJt{mz?B0P2mH)#u(BY-RAY`1y!AdiSI54EB*+3K$96=&V9d<qf
zaPb#_u63jopeigR*m6su%0DisB$P(P1o`QoD~V?4q+vRrDoqyhp6xxUw^2>75+>S+
zn*5BuE8=o^F<iUm$L!%GiFVol7Cg#;`xM`9FNa_=rBO80(cK7=^6Zp2LzOoMEZBKD
z4c>e(u=2uaQa=x`T+KYUP0HYVT2mpN6${MeMkznEuDfcBw8T8SX<u3U^zmkiTF{c+
z=73?j?;&D@mMPSIzH4#BYgFg9EDF8WpP8;y9%@#Re0=IdpcBv%?5Q?$QSLv(pF$hw
zcL=La(@srA05YcqwEAR=n<-dXCoO7A>$asZ5L2?kC(Pu94SIrVwna*mR=lPax-0WM
z-peb)j#jwv4h}~WN;lh!S{8YZ;{rPa%@Tcn5+y2&QSKq~C*iMBL!p%5ojEUoc@sSj
zHxO1&T%ZCXK=E;GpjoC}y1YVFRfHAR2PB)$9+To=u^rIcsgU1Px@Ssq;6_K2mOApO
zw7+SQp&m6QLamT?ycX=&czyl1kD_KZsdF$FeWk<|^9uEKh3^!5Pn6N}?;Wpxd(NC{
zB$ct??5f5S;7qrmZhmSZtLI)?lF{dkf7r)`LnI&7dw6Ajt=1teBb;|*c1oy=#;Gth
z-6P|>)!!8h?aCm`^9!M@(WpUCGSsz4AEtag4Ck@TyDJ;2B75cu?vTsSAAmH79NxqC
zSS(q=b<rTaRcgP=2;Kd9VcP~vhXw2We*A9bkb;iGi$J^#5?ohs>?o&R$t7+nC7?Ej
zs#MK+M1K}85;dh69jQO1C7VqKZ%9wg7gc>;DHKOSZm*D?vhXz%_L6~}t{1*C1d61Z
zHDY#I9z8N}s!lGMTw`l0#S4M8*c5)m^=$OY(6!=Jg{EvSfM6R$CdU+v)<BnU;G9je
z2j$5~rY^0uhTs|MxLk;fJIauq&i4$_cR*$y{;Q;ZN_@#NRPFD`vE1(gsQw#Q3UTt*
z#Gj;_-%QE6OfHVo@STGw8jr_)XoPI|yL%_Gbjfp7=;w_7zE@?#foszs6i$I=J5F!I
zsi-5gGNjffTC<5k)q2cpdt)tiB}B}2b=ztnI?*>h(wkYvh#2zHa+9YGc9^iLVc_3#
zMLddN4rEAd`Kq}C=>W4-24j?0Zs<S@Jixe)7afX5%Yn{%x{Z=`p(3eZVVz7$&U4>;
zUwUbs;g<&)PIHnebZw2k<mIa`kYRdw^ncX>GXAFm?tj$*3W-a}^2_{32gvqcXMoKA
zJp*LGV`gIhmjSZ>A7_B9EdP@X@P8M_`$yybhlBTjq~HDDVeicUQFs3<@Xp-U=zlQo
z{=>!l5BBcA)b5O3{vmt+U*_Ha1iEAY?-}I(JB(!dPp`%Q6Gk%7vHs^UlAY;a4EjGx
zcl1mQOl<#u!^l=JIjs#8>WI!xP&2H6&dyFjJL}D@Y&?8EJ6GtMzO60dw$9EI`@@_p
z&!1Y4+Kvj(*fYZ#_LUkEQWd3qB!+s%UlAK?91HYwj1C||hf2!rKv7Xtk5N&Rc87-*
zDAUWmJwtYf=X|21(43mrzUX0WzhJX>j0-}hZ(JN)T7b+^ErF1r{lc>|!vizp5qc*_
zN1t#7IYn*!%JM6NA^Bo(@T^UtodO6^>l<I}n&=o^ePkbUMS+s(nf<~eBLkmhUH#%<
zXXpA?XHoD>Pp#mZ+-J<J%wXl}8C!rjJU>-`Q|LIFntIxRgR`=-dI(~YdT@#|`dDcA
zWd=90^P!)DIyr%50Da@Z#IZI3e-B0B#9-xF*z7%~Y7I><u1(D%{B}cC(?kQY&fezM
zvXb_T@q0N0LMB)D3*er8rZrsafiVO2eF5(knt7CM>*@aaZKVHE+L&KopB>$uUmY4;
zjo!Q1kMUdXPinENi)#rO+Iq&PzM0vT(aW{%sfC%owb`T1t?5w+kUc)J-yUaQ&lx*9
zr^YHL=NzZH+7EyHUM$^>HcBHiLVaUB@Hz))-<N!DSuo-3YxflnjBj>Ta&>lQ^~%n_
zhb#|1!XuK<Ji}PDpuo}ak<ax=ZwuS?7@P2!0$~yo5|TnA0i6N?bzo*Pej?~B4dA?%
z-pk%`g5c76a(eomaYE+B29w`@K}=7K%_9G{b8!NGcl*-3`$rFiL+TmmU&7G+%1~eJ
z|BCsh0nhjj-htnl>&MQUdgbwy0vhJ!`QbVP#HNO4S2vH?KJd<@@~g-Q3(F)Q(+>Zf
z1_fVj0_!+k0&$q2pa5Y~GWumlBJDox@PFt3;GX+RAj49B(?s97a*=B)assXB!fb24
z;G)#{!h<dCKnudY-oV_u+$6$)%6;K@VT-Vc9K6t${G8nVEFAsVp8tg1`(l3nP>XF2
zt-s4MFXg`f#IDZ{_pcw=2EOpS*!$>0v3I^K)qUwI!tUxuQb91)+tt76Rwp&?eK`DH
z%TWJW5P>GOhjz^iF9>d|f6G0%&lPlwwXxQRR%&u=cs(cqo~5T}_>R5y-iWH+wnZv^
zDSTH1^U^K(nI<!_v@*RPS*CBS2Z6!HhUMNx+X;<KkA~eHdIaQ<@V)S!VD$3YTAqAf
z|8g@u0ikPl4Ei|WTU!Hy5%3NB7Ptfg-`fcT>HT*tWDkrs_|1<s2?Rd269(3c_k`K4
zN%0xTh7Jr9(Sv~G$A8A^5nJ$y*)1yn7QJmL{{vy)y<+f+Y5xl?>YHC`@+L+kCS;h;
zGOzrhXVA(w@&}{OH!J-cQ4=`(XCf!^2>0!Wt?PmQ8=}4eQ25e$>dNe!g&8>3v+Mb5
z_XhCgkEOK@@zL7HX}#dv_cCbm6L14L>09bas(-C-`|U&j)8dw@{s|3mJ=mGsnP`54
zP2Z0ENWJ;+9@Kl`eDA#axH#86f0@7Zt=#dx+R;UC`B@@UDd{Qx*&<nf0crHTY2La~
zsRJ-Jwz}BAtA4tM5Aenvad!tsW<UI7{XtsrgvR;-+j;wr>3ipal35OsIsRdAll`)N
zafMFZ8F{(>I<cG{Tb;h!QQFx!ey4raiQMQ3Imzz$=-K5ojol&c>h$=9{d0ccdsSE;
zdEjZ7_0vVSllsK_I-^SP=M;`9<b&PTbP~x*9clAjjyUK>q?L1;RS-}aFC~5Qv`{~F
z1PRIE-@(9bw{<5&reB6>Y^)h|u3{H{x^&T24#&~NE>nKA_96U}dB11_y}3`M@+0(J
zFK3Mld<ns!+{b;R3Ss*MM0rax3uk_B1%WC<G^F(i(W(RavJOZ%nwxp(l<-WxWVodz
zUU1S~B5?i{VKU|z#c*sVmX~$*TaTweyNz&WdwVL+So)J7;addc#PL44xL49Lkp0|#
z+Ob(Gd955N6-E_06zCJh+9{%6@)b`W!?Ulljm{z47C$jQ1IK8th4MvURp=%r-!Cm)
z#=w90rDE0$DyR?z?}twx`_bgrKB!CPzte}2WxGCtomVav$b2>HN#_y~%vyIX?9nht
za?J(%R$XqXLcB%e{*9tE{bM_izhYMTiROUooEbu?2Zf*Va0BAV^_dzHFxcz%t}VIq
zd7!2|xzj8k&~C~>nI3pLY&DC7_=10iQ)iwcVjH*p%)J1j?1&-6)Wk4)Z<`v;RICQ<
z?OX|7qvg9M4JB+1wmJ=3yoPpMntj*m$UUmJ9b}trYudmp$)ho~g*&4b61=M<SkYLA
zXVzetI0X+(h`2EIn_6)wQJ|~PLRFT*c~{sc`90T}8?Q+oqpHizdQ-c|?Lio{*nRb5
z&!iU#h<r{4eSeN*4#P!f2tS%bxC3pDd@3VvdNQ%0JgB4&&rsU;*<8S2icSdshQ27n
z7(aT;&L_gT5*2S6YJmpy<I1Fdy#0jnSQ^5Kswe}<-86PuGeO9C6zNE3=E8E^QmBpg
z51Y;2hUKj`IX1%%5n#=>-C6V-NuM9KU4BfBSCkG1d;x+p?4Q$P*-??qaiPQ`a5@p=
zdHHVy>SI+ziei$`CSdPwF!+>0X6JHSI=_BDmb;$U8jGfo+j~{#+sIuJ4z<Z4T&=Mt
zL&nr*EsYC$4jp=cTi21v!f2B<@gnBul+#?qEEuHkG_@kNG`WmZM@tl8tlt2|eXPOt
z&}6_1eh%H37Au{G`5Zp{@AMVsl^jV-lRX;Rb$@3?=)VC<;$hZ1dkK#OfY!v-L(?5P
zs3g!i7wLDn$rBehQbci7+=6OONkHUwN||S%y>YiXJOKi+3R-MRU0E}Kk#-O+$X}f{
znmJSnlYP6yHn5sy<_Pwa=$%Rj(N2!N5dV*EFGQSW8@ua{nzyMk1UdJ=ve;!VS|zZO
zC=%7r&RgD=ia4_@5{)q^bZRN|g!{V_9ERBzvu6ExG{kkAGNB)!`A6h^AUIefob9vK
zbLitqofX!zd(`-edWky2z%geqs3N2YIVCiOIIYo^U$bmZ?+mral7Ms|(yC5dcjIWr
zqt6f}@f^r-T`BHxab0LZE!Ac%jTQc;XR$@k#let<oXv4uoiiIgUccWhul&{^aD8<?
zpG`oQT$mDbL<5w#`B5%PlqBC2Rwr+Z6u<N!TCkCdoaeLDLqJeNnlCunY3grIAE^xa
z9b21b=u_<WZYTQ-E7|~gL5)d-RL?<=hLJuu{gK`<(4ovEqGdK(QtHoTNho_C*O2ai
zeIueds;^+|q{YK=zo&7_04pO3R*CnO0nnY4w*-N*0|2@xlMN4^jtLk1q_82X&a|Y$
zTu(J}pi;>?ojKQx>p@|=5{*YKZHOK5BTuTCOip;MI49vXPy?`HZzsf4WUa=~-c!w{
zoY7h8^`*kU`|u5D;*e8TC+HwtgL-I(g4xDaYfh5)stXN|_I5A+14zhaOsB<SBZ*7N
z&jv8VtT+HE%bbdAoFA)pa&jL|zfpZ|NUSR99}R1#3F&a90Gt=Pk^t9G=<e<1B(8J7
z1hY;@W_;h+UsXm-U+M1wjHvJ?r{4m~H(&>6Q%|L7{gL>;K02v>U)i5rEWtVnjN+$)
z5OCiQ^;3fU8rNhKQSQM4{Y_4*R`AC~5`eWyS|{HcCcK*ePk}79OG}C8iJ57G4;-t#
zl0%vxJ@t;2+|-c`M%#(EZOjk^t0KP8P%NuWcxEp|rjUAYwB}ix7g<CkiLD~Vrzi<%
zR#)qyhCA~q;FDlO_HlXXmoy<nBV+u+TgGWonWKrg>8ihBpE2g#k0-<uSY3e@+i)rI
z-_nnU33S687Al!)(Z!oUb}DsyBH_BBHc5Uo7UqmhIwnJb+Y~#RlFq!O8TwO>qp+C?
zDh+>nZ-2*i`aIpndchCoTr?mdd^9}4W48v}J>A#6HhYc0J-Y{d>(il4Z#R&sA4LL#
zo4~M=4P2S9^0Z0<;>Jj8%hL7PFtS<Hi$LOM*-CmBNv4LMofyZ$^UEK3rj2lMvgzX-
zWC1@(i!J7xt5F@4TfiIZ%YuaE)z75LD!~YRzRUM3O}LUaCLzW2B>K9cP`f4Ap0$!l
z*fi?jnQ?j>aK6{~(;KJ6<|x`%{>7bgB8{!cdG=yZ?zpRwbB7T}{BOr4U{`yhwjlUv
zqY?kNw^JktQ^>4wQ-^dn=-H@ADo=A@%Vpil$NMS@que&hiTFL^@Unkanvtgof)4`t
zz3`@K1)By>A(2*pkB7hG#^?R`udntP>(a7z^Yj=jG`PC3;lV}2#>XEWmNmpofp-e|
z+SR%atfG+$>eMpFbu_vd4NwnFoM-EufiP~m9!?WI&xT&?c`q7tA4I*er-22lI3DA=
zkBEHtNM)={Gg4+=0@fZqcWiQ<umf}?fg#cmC{z8N&d#}N*yMigXdpbb3kyYE2n;>Y
zt-l*_FcQ=w_|Sxipb%AFTi2CPh6HH{8D-gbF|KA=#C~EU?nfqs9ha6!5Y?_7@9%yz
zg|7CytZt1d3=y;l0$c=Hi2g>ksG)EX(;o?&wNx2b*&qJ>fMawkW;e3%8Jv@T(6_v3
zTjpN@#=QzoFR2%MdIoP#P)J%o^?M*m4SrX7W^>7C0A5lYE4&4vmD*`{leYQaXWse*
zrrJ9sl#d8^$f@frv)frSr^oXNG<ksb?!Z4oQ>FI7ce<`6j>!#-vaI=)Wcs9^Df6Vn
z4e5B2<ay}f8>cjWGNd}22~HTm-yc#-ty8qCz0Bbu$5XVsjd8>pyxpCG<wI?4qpOj!
z&=EO@U_gBu*$f(@?r$}&y=%!w7YM*7eIO%8a6MKgKb&jeQLuAE4@w{qv*0Urh|xUz
z>UPOep0ghgWmO_7er)^EKko=%t758ZE|3OSAPbwaDGB#9Gi|9E29ZfqD*|i_)l)(z
zBDQrgNYxk=H?gXoz_|uvql1Hdma2DK?5<RQBlX%gSaS7R_s}OPo#W3)_2V)p$1@tt
zuzO<7hHG?0SDSeAO%tktut+jIs?Cxu0O_s=NCMi8V7`>&cqq$Dl`#`&bSIsh9=*vy
z=Ljc)Sm#!ePf#iD+A%B`B$aeh|4tGCYW+)ag;&wjn11;SmzJgZorzotR|BRqg=FQu
zz!0#p6*{<VzvrLJoplqWiuwGUcJ`f>kA}M%de8xr2v+qn>|oZ=+JZs%R9i6OYW2+)
zplBxke&w*lEQs~IX9jzF>m;W(J4ZQ8IKU_@g{nZv5mJ?mldqOn_Mx8w+X}l5-u%Di
z*~#wZW@kFpN)+!9f|bMYVUEUoGGq)Lxd`zOn4I5Xq-o8O1uaD*Vkm6^H<m31#k3qY
zUBY%+Rg1yL&nblR<Ou;<%o}JC2ek_Oa!9vSjrCLFzOEX%dg~v?RSTO+_TrlLEd3#i
zvOxtzv5aCJYxsG!%SM00%rK#AtRFKzd4SE<mP9OO6xb+q$OR{{B)w91cS>dR5C(%6
zssdKX_Ml$Afb(}B0Yu$_KCGP6>ZMR@IZjmLGouKeyqHbHO^FGBA$ciV`1DA?mZvHt
zd9`Gi*snHcGC~Ykm>wB0Yev>B_q#RY2f+?LDFq>V084f2-~$k^DjmaFMeM*MJtive
zVX5d6Mv;kc4ANP$2^62<XhuTyqMM-9GE)mi#n=pdZje8bU-#7=J<Yz>uVmyqnwo1y
zXad_MD&qZu*iRQ_+Blc!M5y&&FtzDLcko$)=ki%_ZJenF#ZUNC&p-&8Fw*S{#H!dw
zxZbcGY61`WR|@`9q|YCxE5g3nu|Sxqml>B`oMU%y-x)xfn5y~swV7oa===Ff529r6
zC-`NF8$!uW$!mx$?NXK)K1KCqDskr`mPXIx+Rac?#}7A2M?<mi?azmQW>377X*)bN
zRVMv-unOVCQqC|`QR%W<^G;8RDEAj<kUv_`Wm_i;bpt??e*wHf`$Sx`@6(MRjOy3a
zq`D1Gd#G!ws3-cX7ewKDp0C)llIXgD`cl})-%v&{aK$v`zK_Cc<F4NPxlGH&T}7z`
z8TuDp%+_|kBzx{dJ1R@5)WHeHNq`_=y#Mh8#DDI63aZK2N*I{_?$~Mco3etQ<YBJW
zlR!+r<Y440rKVB=-xnmT74zWczAPobSn%s0Y;mfZ{lgh&D186s;AqG<cKKR_$oZGH
z6-%qJe}Oo^xpp=PJ{4BvKqt3yZ*TV+;SBH)3zUeV$_~?|>p1*6!dI2kWX!SEL9S_4
zDfWIfP2>bb?zIql;Lq-5X9ZYN6KA->Iz}r+x*=bq@*<UGL4=X<nCr|nCpmgi4fPms
zxyBaaAagpRoY@`FGZN_=Tr4fIR5zD|Zn4e2cck)C82>d6L1sKTY{`F5E_n3JUp70S
zY&HusV!t*$LB#8im4Dt85!$h7`3T1Sb><d{yylDq5;v2ny)}EGqR$D6kBfCy?n|V$
zvUNlR({Zt)hXfTwo@^;qPZ!Eps-TW)i%W<obSdo)K}Ll(7fyODdp1w%Qzkbp_jf4j
zB)fVW#am_dPq3BL0`gVOX-Y8BexTl}H2UO~P{&mQyC8ZSb?rE7+722bJcHsqZc4yL
ze^8URi(HkJxeeJ;sRWB=Qg3R0-I=wE&EqAH7x@YVb$y^NogVeQ(NyWjIPidxvrWKd
z(w_sBs)LxaWaJ4*olv*a{KSNcPY@VhSMT>jgMJJnCA*YaUv}z4598WSKnzSl+r2^5
z<HR4IR(#{Y6)U|cVd{?BsOF=^nvUDGBlWCOjcnWjQWDVQENlqCFsmaJT<V3w?{Lhg
zeGoMKuQi$!o%Ym`YUv%D?l`6sZo1)F3_3bUsBQngleJ*bT=T3*h==C)99mA<s6N5B
zVcycpm_nr2_&|Q2O|M2nc*{ba7X`!^+K(dX4w8AwAp`VB@U9qG!?+iS84Hz<Ac;iZ
zC};-TKOVQyM86P)<uy;-L5RxSMNbtQ@pD>*d4vqO0*b8J<CT%9bwEC&fIeBzYTt3F
zJj%A>h7I9)w$(zWSH-OBXm#t|`dZ!6v<$%<m%HSKN+u*AMB@+;6zCl~(G-Sc*vvrV
zZeh<&&DlDXAx-cTL;KMj8oeIU3>}C3JL3^x70g$x7AcU+a(xin$3+)`1LqF}!1aSm
zDLJgTItDzS(vQ`%b>C@Z8*faj6VbL1ef#Q1*?P-uxND@mgeh^a=Ey;^&7ZhpBbvYE
z5Gy2Ylz{YqEK=$P2JwLdWw(yIl#9Fbad?Px7XYuvoOyYwXc=9&?2%$X#}<F&-}f!(
zkSzjXR#2bGH@fOynZtJwDmwx>!=S@viW~@O9UAyex>{fPV*bQ%Mc7M<ENY8YW)MOQ
zVbS9k)V&wvfK2X7x}nJdAFMi!L5t1C!in5F$cjp-5peF2EOj)PnPR|E*UN;y@Lj&;
zpHx3)r7IG1BA9IQ`*V_;V@h`a`uB$n*C*SlHQeA?pStknW4n+<qK8>ZW}E*Qw%BZ1
z=WCdhV5zc8myBhD%D#SvqSXOMX2}K2$1n)LpKV<bhMsC`SS2QIKE8U0igQOkB;Y(y
zx}3~*wrZciDneK!)1D`lj?j~}({Zc!<uJrbal$OH7A}yX6FSojJMI^5Nrm1;ROSc(
zPxRiL$n0Z4+fVSK#w>~ape`CI6^P%~q{7GIF(<mZsINsXZ1z{hl#>M`&E}_sdzSvC
zyA8o8|6zJ@`BmBazJr8OteA;LqJEAeh~VM4&y5b|X!6DVc>jAs!2kq3gS#;IS1!Ik
zNi0OoQnafu(P)J_9>Ka_E8NXKOOZh984!srTvH~I{(ZVGFPs!e_<bGocuzu_lTE__
z8NVcY%a*LEm&iF%;m)D8>R6qa@1=x5u4KMQZejUM5H4}<W#BB+u79*lM428TTylM)
zvblIdE@4zRqbk!5Lbuv`{wFn9XbpE;0i^8Fx!sNAtxqJ7Rj=~iwH5&C<lo#AsJ%Gi
z)h#HBmtFZq5Z7}khV`wAU*Df_x~4rHE7lDzc~<kkHQ}0!kNM#3CC+Z0v?wgBg4%e#
z!7B9db}C&Hk~vHpmhX$EYJLL;eK2s`Ygnqq_(Z7s1HI~%aiDvIG|T=8zk{QYk|LjO
ztDFQYSfM_Cr43=E8>X#xy^=CE-aR`*t?coOdza4(oUi*Us~cxc#mX<e!p2E#nF!`m
z@M|e7nUfldON>4l0+0qrV<1iv>pE(fkez-$ug`ZD3YhIAVIW&MSv{V+YRF&lFcD(*
zj#$q-wm+jufZiwCK1HRg+7K`xptm5>7{R&K&#n8zI(`SJd11ynlW+kp>g3W3Js!yh
zlO%L$BRx!O%{PdF$7k{<nMvH5o;a<|56Ti_^i8LCmMufC4{lC1hExT{q2~zh^ZN&@
z?e~oJx%>O1KVAmG4&gqZ96?lhF<kOVL=g8`sRN!|_UfmYZzh|FokS5^lY&pu|Kc+c
zdavQg3O2Y=xESK4@7mKXwGo6%%m<dQnh*&xH$wB&GA*`E+jBSM`=U=E>s%Yrp-evR
zU(?~=p?X;K+Go4XDnBCl175{|t&eyn9TN?YW&ro@7-QaKzoWX`;pA_S9+Iqz(rsie
zLB5?w%Q~cu1=-E=v)VttWKsx**q4e_QNFj8^VS@%i?+RRo_8s&Y`?88!(Pmt8JEJt
zmv(7%Mj!>JUnL$?#ede^>_Q>UC0ZHw8o5p)SUw4Uihz2g7=x8Tg3m7loDbGAOW1;-
zvYpanS762r%VwO(kfZ6yt4-*cwKQD2qG~w%cQ9;zGaJ1oe7Yn7&M+MEZ)Kb+9_EuL
zfTJI~Rz4rFZXY<rJ)THJP|2$18oS{OvtuTye5A?mzVOb41i8DMv;0@VR?Pzc?8-O>
za*QiXG3kVYV)~rR$X>-a)X;*7a7}i-cRsc1OZ!8cDUWV|MwGpI*LZ3OdA+gVX7?Ii
z?U=*#6^^JXIqZxBF;JoKd2}J{^OYFzhQdeIu0wkJ!}96nxyB(NVg{0K8p=kA0H|oO
z*w5+F6cL9|GRV@2^0Hc~jY9oY$b6F1wn;XE5U?Y=@#j@6)=r%ElTnRjSq^HFWXYcY
zIn6khj^Phuj6wcub5@^DR-YnSiD7(BN0U#h{vC(&A~FT53zxr8P2k7Ky8;QdgK^4T
zFq<_k5;JBJe2giR7)0&GEMLsyk?j<P8}X7*+qhiV=(ol@0CSChO*2(=D>|1N7j7#y
zQyS;Ni{Fmd=C5efr;4wc%Rl`o<BcLZP11jlagD|4%s}}BLh0ElJp(F`&xx-vEheU9
zt8M77TN=I$dX8bN%oG<s`dj8>>MV4{V{#d1Ur=M1QI%woUyYfA^p=w27m&Jz5b@8L
zH{sq#wDHy)jI4rHvClYWgftyGlXd0&%#?Qo+R9!fXyKubbJdjjl~zIEPN#_*IeGr|
zaA^mDkt=_rY4yK^*h8C`o*TTc5;qk!`RwbC*0ai`7X<P*s6!kS597(FY$@F{5;CM_
z+wy^Rz&UIwosH2tgz;1vPf7RChg2<}Nb&A(PZI+cT!dB5o3c<fpOYwk=;Q;_p<k-a
z$7ozyxU$Jfezd@nqeBxg#E;VqMc(B@clPrpDz)22|GBm@u!Hr{_#+(p4u;+z5xdu+
zm(Gq2hT8mp1a!ncxmAs@^{RDYf<{d`7T?;q$?$JvFhZW=D+~7LT|1Md#q-__zBqhV
z-aJh~%Iw$ex^4ob50rG`I^|_YK*~((Fz083Xi{$AtN>(hYt|ET{oVFT&UfLiH-#3O
zF+MD-eb7z5-uI8Og)vMPzEUZ){)5h+aI}Fuumd}AXCL@uPTo{hq!ayFK?-MC!$-D0
zWro&A32_V-f#{S^?I%T1*xR<ff?}+o9TM|OUKj?rP>Y6}JjRywd^=IlQc0PGTKU6`
zK(o}xht+vp7E<yDR1uE#h0?lyu#J;ym7=_qahdOiknjOv`5P7CENQleUK&hD9;0Q9
zaV<*Z2MuT#DItrr1C*oOLT_d&*US4Uk&<Z(4pk+bM+%eM>bdhRW8)YmlHSM>_Q69?
zU(kD_JE>L)^Aq1C!BjtgMBH??86jA1AmtU3_s!(qg`&+jC1>qD$PH$mI{k^3?QCDA
z33_IBhXQxv1a17g6eXVMX5~DI9h>2|UWvlWKl_F;!4TF$&}CB6Vi)N>piK$znVs^A
z5U8lB`obOn+FUa~1wMR;8c9PuU46iNIl~k1D);<!$#mM~Xt!qRbxQAz9RkSq@U@Ss
z;glWsgFaFnVzL(3_TZynOa;|(V{l0)WpV^w<cK+Xgd$gprS5;6$~yuC07!@0=MNt}
zFoJzfMYrr$OZg+`_S>Ej?&K(=%$rnJBd-T}OzxoRB`{iv1jf*~wgd5JC&^FsBK3-o
z#~*v^0eIAdIc5T9`m4A9rdppLOjlNOv(7qH6MhvWqjNYnOay@*)>&7$>p5sH!e&d!
z6`mzC70TlD6G5ZNJ_mlMju$}O{f04T>tSIrF>Zm<<EW4azf&T!zYwrk$~JqC<6`Dh
zL>+>uDUF>elta@Li%=X0iYos66Up2IyZ91mT{l2+>$=Dbb;g&;5l3kl)6+yEiih2}
z;scFRE_7LAkZ0m7Mn(CTLil&{FA)5)HeTZ=e4+`<hwB~Dn&1*R|ML>NMZ5Eg%8q#a
zfO<{d4A}w;FOE3(Tl$_pvHO<E-%SyJ9ySDnA=vztvXm-7#uSD7B!~pJ!(<_ML3shQ
zY^a$(a-nZonWd#?>T`L=j_NBJe4M~zp}@3R=8>>^y+|bD)Vi$cgxWiq-&8vB{@LT;
zwu}Yd+fg#^Gz|2)LTJ|~Kj2^gt+cNItE$-=wnb1eu@H|9sKDuSR76TZr9nYNr9%*;
z#KIOaP_eKBTQD&YMeG(6>{kpByBq!2%-)-`_sn_k{k;Eu?(@93&di!MYu2osnLY5A
z>1LbQy#=cdrC1qN-kw<SuzFD^<Bmgg_nvwx-`eh^V}iG{SAOc}4q3}OE^h4daMa#(
zb-b(j{obc7^&iTMEhB>dskY#isdLj$>IOS!Ol-c#cJrkTjSe<Q?eHri(=TW28JDF_
zF2N~=9TzpSyg8@f=j>*>>nCn<4tdq%-tdT+ox5H7+P~}n&m*7JH9L;$JN%`2zmxiR
z9aDyv544PN+UucwdL?`Dg#(4TgK|%_5RKI9_#|m*^(W8v)$pomcBxkMv^DJ;slA@u
z+L=48*^h}^lUi+VUfRRxYEibrt!uz0`S}>rm*>@yH!rRq*52TY-`lsz?snrt<0g-o
z)*@li#fN7*?r-$)g{%EjqaPD|T6D8sw4osErNO@F8}||~My*=t(eF&pZV`SJ14aiW
ztPSn?t1>U)V?)D`$9>JZtQqmoz+R~Zeugm*P5RsYc7E|WbkkCkk@fVOweeGJHtm)p
zu~#$>w3|Qebf{&wxw}(VTnd_5khWn!?#V)v!OFa?hoAJQt!&b)v0a^vkF(r+pN=hF
zG-Q)JbMH8nTz`{Yv)6^K4urR9zQ4=xj=>XtMT~Zi_;~p0(ECnSw`yJN=X>LeqKEsC
z!V2pp`tm8|t*&o~e|n+UvH?5#R$q6~Vf(hd_Sd~%96a|#r_yrgzRQW8YeOn#Zc05e
zeXhmTKbMB|_kO3^)hRB&Q)b4&ukp(~f)kc3Se<-AtTRRa_HOXvruw%=ei)<sAi=V7
z-^0X?Z#T+qW|m!Dqq0A`L-$C7N1sO7|I^~`yY6*c*GUL?*UGTYsw@2$u9Qu#?bk*=
zBX(fOk0RxA;~dMptWMXIxp8st4(5DU8y5R-($D_1@$<(U-#)E!J=vq_hSXWJbUh}n
zjX0;j#vpEe<CS_J&mG@CsE2xPb@7P1-R};aJh{bLpIF<kJ)=6`ExEC@Y?sI0kxwMz
zEitp4v$`)E5<6ph`Kfnv-}HY}f6c8e>d(2qGds=RIA_7f%ilk*wa)dZGx+DlD~=!g
zxSxpYY4>8x$lkApM;vO||ISUD3CV5RTKut_+cMN&>9MobKc{P&r_}6O=d8iQue!4)
zCB3noG-QC&I-}n;_GSC}B;@=qGyk+S<Bsc{k@Z%u9RJI{;6u0E&QpwZZ60?P9l7M_
zY;hynefp|)OLrD0d6r~#?%J#TZp-28#{MzhXyh!KoR;iupHtwvtGA_V*X#>F9wwyD
zd4IateepJR-xdC!=T?vOZeCd<q-60Ax#PiMXIFop794Uf_t~wYcWW}@%Tu~<A0M;l
zOzUk<))Ru8t-t2J!fk2OlP?y3OxzqX+2p$G=y6AMau=4jc>2C|pWgWyL5_8FboPXI
zD%H<?6lqb<Zq$n%H=n=hHD--l4c}qmuclX?E_(f-_)-56pSO$Lr=5$8TxK=;-k2?I
z$BuE27&qc;-nTYuZvHHczfryam!B8cNd3akPn<U7j<;#k1Gx{nMlA2q%Btz1&i$L8
z>y|z2%WYKy<JU*`cIY$JcvoIoe)l8y+Z*S_)}0mPH*=rEZr5tXX;Vroe<Vj_jd1cG
z(R%Rf(u#1al~?Y0uFGjyyS?`WqfeG=hCb<<y1ev>?psMyX;fiq$V%P92Bne4C(Z^u
zYSnPr*YmX&wn(*3JtiyOyQTJsr~{XOjFgPn*E1y9u#rse7QC}yXTXL_t0QMDd_LRw
zaLVyT5sINx`_=1kezlX;A+ONH?d!z9URcgoPF}fX?B$Y2^;+C)f4;%Jxd&^cwq9ZS
zN-w11%h5j>-x3}^*FDlQ{j*-A(XbX?1#1?T%Isd-FMJeOb7@l3c6F><SaluuF05hK
zPoa*j2c2Ac@P7KJh_@b36(y?`?WP@=kU8<m%6sWMrbIrzbZtXm2m8xr8#?Z9KYing
zsBaBzpZQGgIlb?+(}#?oWlU1P?YQBL@wns7@1C4`Xuy?yIt^W<OV{bup5wGSuX(?m
zR(>fW^V09u5w8<QnmzrVwk@w(QAK3PEb|x7$``Kv;5=f=*tC}u<1&k4c6V#?#x=EF
zv$SsibnpwWALhJaNm$#xSKTLduSghZ5H6R*T$`}+UUKBehsQrX=rLKZzwO-K3zq#j
z^sS~{#gX-+8x*&)%qeR=&!FSCJ-RcjCQ0J<dwAtN{JrGR+a#A>I^I8DDm-VEH>efS
z>G9>*WrLh=j;^`zOquI_gV9+dN*dZK+P$?K(zT=G=9b&qjIq^So>(#AXTq2?n;QG%
z<IScfw%>4gp-Y!LVN-)2&p2!J&!B0(8^^8R^Tqc0Xhr=;!?JB=P8|Qwi>Zr@RJQT9
zPjlvP`uZ(S=@O;dpZ4qQ_3?9W&F(Q}oqpN$dTZN!_p!`$tax<n?DDJ|BX2dV9b7Qg
zXQbVd@4xKy#=9)pa4J#1>BY40=oh^w+;ExPYIciz9_cM5A4<hW?#l9G(^EEndSkeC
za@m5)K-uP9onIY${`6Dj`CFy?)_56o&k{AX-e11DsZZ;?uv6zdkI~i1_~hMpL(Dq;
zR}WpIGT)Yk<)6vEW%zaJua?O<>*LLmpNqb4FGyRw_E?mDZ|`9%_ARh$`D^d;ufq@8
zeZKDau=rlfE7wbhM=K0`&m@bE1g#4ynWKAoQ1Qz|WklQ9`?DULesgc$z3!pk0*<z7
zDb9NI*l>K^uK~eVevf;8==d1L!;=nwM(<vG{!#YOtzDx!8QV7-|7B>qvW2$$JTlME
zHtTZv+U?fe?R#vSImW8*PqW5T;&<=#s?kohV({5F4u>Pt)+s&TtE~6j*_u|T&-3@I
z`y1S!o#c|$t4>mbxF>g8)|>yy^F+DdUhfGXUk&}*Kv%ZRF7&`{7iCvF-A6MjGuAkH
zWN+#xGC$F=)Is)U*RAVuPHk>q|21t!T&7t>;1}`tV>3Ia$C|}l&g}50bhpFp(e9m_
zPq^(=xzan!E7bPYrgF<u!wnX_doXiIqK84cPSdk3)(+~kwz>b_mt|2s{ofYV_f2sq
zJMrdyw-2q%cP}}kwl&|k*Ee^cS-^|H{9Q>QIrH-(?H{dLvNFT_=(IM|el&8leLkb~
za<5OjXUyzgcIWjLzfDUP<bRnxrdz|sCu<dVTzPlCwORh~l~cP#p4>CtqC}+1emT9O
ze@AIj;i{|!AB#ffA58!Dcvn=zLw36!OlW-A>tKE5_wMi36%D%fJTLsjiK25a*WBtD
zlctIr>}x1dmPG&PU+;3`jWIPI%rbl*w<~IJa^(FkGsgyaTL;%1_pNsBgbTXEnoj(6
z{p|Cdif+TE4VvKJdVhA`H;(bkKIKcJ-IiuA_k0lgb@DFLdpQTM<czml|D})jyUR;I
z`?UCeHp9$6yYB8g@!QR91|DBC`+9g{TGX$pJ5r}@`srY0vQKn!>!uB6KkY`(u4OuG
z^<}eT$7dyGr<9H}P}JG5*V1n5iijbnKi#-kx;{%*663yYLfX;Gxs$B(e)eoz*xUL=
zuiQJc@|NdqAMO3UGWGL@7yfpAe4O*<I~IK~A2YFqOOv4?r~0g((bUap!u`$NFAkY<
zD7JK(Z*cuSXU&Yl!cLlRH42RTG^+c%ikBz1kDifJ^X!q~I(Z_U8}8@MF36Tg?44vk
zH(;H~ZQt`VyWh*NJ;2qY@19ZVu^$3YF7eqsexJ{`lLOz~%{_LY$l%APy77C8s@po`
z+SrWlp5gal+wc(&K6N>~cvgG=v3K{rp0;4`_#vOY{M~Q3?lhdaC|i{>BBAZAplGuV
z*H2!$aJc{DbA_Lh1GZer-5)bH?cMIzjcRqv(+QS6k6YxMa=exLuFN8QL+4c!XFXcH
zprLdMq!^mG+%I$rmx>~r)^qmgTT*;@ZC-PO`5RhEXUsg8WIt-1UrDFM8DB;5%TwF8
zb5pboPfwl_P#kyc-91B-KVGlawLCLjJTJheIJIGhyz$1z$M-#YF|g%}dZ`M>qp$K>
z-M^7*y-yTy;dn&q_X&x%p%oYFAC9hl&^P+Ax5EU9N||$glKvc(tLYBWl}-H?e9}8q
zql3ln9P348{fGT5l}@{mr}wbLI?2CI%BYPsrsg&cZnC)`y5iEg@=uFrJYQSU`nbG-
z&hr;P9g>&doMA3HlzHm<@Z?HEFAMi`rdOOMb>1(2d$P`xb$y>D-pSUR-Pp-y+)?Z7
zOPgzj4xJRz-T3pnsF3<zb*;K6)^D5E?ZU2JPud>MYI?QfxmMCIA0r#gwjQ%BW8@2;
zO(*jA9yltw{^FU!ckuKU=Ra@yaiq0xM%s;5b))xJe0!N?lGHk`;lYfHb^S&@d61R1
zeG0q{JJh6B(Ga=*z{i&cdkqWx<-5yVC$0Up>UYF->lGw-ODz|Lo=%CGaMAM4j~+%d
zeoQUAF>mU?y#Xa9eJ$!%Sgy`Ka`|qm-R_5L$B%6KJ$K@!yD`1>?z<g!^J_D8$<ON9
zZP)&4+x4t<=M{NBBMaWls&#pOSx~)SKMJ}n%q|(*wsWG)?EQ+D%ZHqE9qQ2jK|o)_
zE`FoWyOy35m-id6(xjd4gPAX{oS*cgV@^!1^9R(aqaV7tbe;QPc#e0U53!d&bhW*h
zWcsD$>Ip+LnwRBGo)%az<z4jKht?t4h2OsvKCI>Pyp7Y{L7|Or9tpbCu(i>lqWLc#
zJ~pfT-1__O(w8w^)4R^>xx9NoU_HN{C!dC<hUSPJ?E08HetGw5ZMFGLJ7sR~l_tGo
zdV4_2bDO=QPIehNvB4hiviufjDu%7Q((cQ|?_YLR_D&da^vtKl3HN&+UF>?LsHMc&
zZ)3!T>}wI@D<{40HsRKR=X>L=m-Zd|*;=uDz^*PSp4XcWof&g(^3!t*6ZLm3eq0#(
zq5N6Esid4+HRtM#_}=ANy*BatBcIi-R+tgJ-?dG*`xlBMUXN*7V(f6q@34Dr?@fi%
z{0-(EOU&!Pen<YH&Z5EBUa#Hu>}yf$oVk5+E-VcyYuSHgUENnXDRuQyb$wJ#ZX~83
z-8i7#+wT_+_~+^**Sp+#T35RZSs9ySA_Awjxw6sfK!?LVJx|S*&l+*ku+n&HuPJBZ
zQ|7FS@G~@2$2A>Ny`x^2og0$3$lg9q?pryrSN^tP7v7j{yz(IR;DhDotA!PM@0+s3
ze*LxMUZc~KL!O?p?9gdS!{-@kHaiwH^j35@A&ooRAxAwwZ(+|`6MvNHt$q73`;(;g
zwb8SF?pd2KJxUZN)*rsx{b~NGHM?Uww}1C2DYW$A&$B-+UTs_1rJ39NE5@(F3ZBXW
z_8)k+_oL}}pUvrKMQfI|y1zZq#Aal1$<vJ3ye+SP+4M4TEbTLH;>GRzl2&v{9QJCh
zSH*+{oqQ8=2ade<C9Q4ikeO$Kx4gO1{CD+`(GuUO`ZYHW9i;Os<)!De869RcT%5FE
zm{pzi9-*~#*EV#q2)1anct&jeo%ToU?Vd@#=}k2Zcq{gv)8=aQmSdk*bu5UTb6B$K
zY=ar_NT-2M$f2W~ZI(XXeJJ_o?L7?_!KbQJt6OBQ-P0@LrP+KxNtsW<0;89!Y+dVa
zU)K4DVV~Li78vVRyidAsklEh1#hh-9Z&`fUykPveUS0G4iFs#Iy5!K4aUaqai;SFS
zH=5+I=Bi{v-i%K(pDne}O}(WaoBqh;eWL|ojrVoBY|?K^hskft`dsmk+Oy2?b@70t
zOU_el+kY3g`_SU4f%?XtA&q+(_K)fosU8_w&$a)!VMpujvC4wi9GYZo{;uD=<;kSh
z6IbNT3He>m|G~6&^Fni+ruobj`?@V!`AxNaoViH8vAek3BWy>VqcK)34TCzpEx8vn
zXKqAub?XPa^KMzrY@Js_@5AK-bv!%nIMphwI8Ir8&Z14<`rJP|`tCjX_l((t>{egR
zKM^v=Zprwup*KpFJT09PmvX4~NUxlym(`o{w`CN|ml(~^J-Apja?HYiJPiuxYz#S3
zQLuYbg6*%X;xBP0uGEWjFnYCOw$7dRjV`>Y_wz@PX`{pIqZ^;u`FMKLxY%FsH|&)-
zJn8jQ-R@MQw<A6KSas@~teX<=U@+!TmqVtHmWfxaZGZhi*Bf&aT3haI{=4*Q{k3=3
zc#WB!ANEj`V3m9;R`UKypToDz`|Szw8|-3qb;Se|WxW|kckXNbyxQ?s#@TC^n97&#
zGG5h4d~QjNk$UA%ni;On$)A(9x4HT1m%g{E9pCo&(BO_Lzx7^^TczpGe7AP%=Ib?G
zb}i5wa={={d2y%on(nX(I{iB@U1WNz-MZbUN?VPIC{#L~a=l#q!$LGKXTSfdj9ow1
zd#`WO_^ofZ;j4d5{B%SVs0`Y*{Mw*pjk@RCXBB6yY*_bv!;3GM7KUF;tu>;>ORp<!
zPrA7;j-T$*VRH9pvz;4S*(CIdYiDCWKgoHGU-i<V>e>rU&Unt8+;4x+I&z;zu2)R<
z%U<7o-a2qsSrc!AiY3dxiF!A^KBa%ZP_u&C2S&ssq!ccb$rc-0>G+kU4Q^_6f7*^A
zzny<RDJXmX@X3a5M>aJY)avN9mPeXyH*I#MLExTr*8v-sH|hJ?`sLuR#!qe6+;7`B
zWrgRK4?epspXyE+|E{*7^Rc$MKE>ze)qlD<ZL;Z&#2%NdEr(4?^BiyqzGo$^qnEUD
zkL|L(x)a9se`q{+*U7MzgWbpa9QOGd<Xkf*#>vy_zAWs<vK2-}z2)7^^Lmb1ak}K-
z@IB^D>W>H$-O@XI?#|&BzDGV9)O*s#W80LE=WAa+wW0SjtA>M5kDuSX`Dg#dC(j-l
z(ze|2N1wOdX8x1lJpKM0rx7QV6K5SUO?UAfaJ8~;_n<Z_bnNRWGhg=i-nS^eM@lWX
zBW68YZ;hF39C!WhFt<G&d|unTbsUv7^2*Lpje=^acZQglpTC%rFw6B?p`P`=)`{7Q
zwx8Z4E;M}kuJq#cxx?q(HtFsDwr5ArxqGih-Y$D%(6YepR~OwHS7taS-aX>A{Js09
zK5@?mHYjXAdAwuu2hE@R2bRo;%8nG99{WeH^v(C28M7ze?5?V{tlb-%<MFWveT<Ie
z#@S>KYc*t@-QMKgh2<*i!6k?N`>n6Nb#^a*uM4+kS<cO}6}x|F(st+9P4*L0^ps(r
zuFCYSCic6Rb^OnOJx0L|H}6Z67ar|8*x4*y-8{Wk`qg;d;vZXlpA_|wbZG105-w>o
zY<BeFHjm_mD;6H~G#)+dRMF{`(<0V;2b(#4+7#C^vc`lK9|jiO`FZYJZrF{k$!|Ia
zwaV~6l6LxgdU{ctc@LWmf26mgT?gNW8^%n?OwHd_XtVK){@kat6D|~pN>YAwD%Ekm
zoWA!;i?%A)Q*XDl7}a5T-Hg-W5gX68*t9OauTPW3x^b7it!-R>*0qlo4>MMUTMe1l
z{?LKL2WRH4&o$gRG0^(gg3hC^y>_|Xdfu+K6(1isx9^f(SyIxeyj8)M*!IaiwqASh
z?Xv9V^0<0+9O~=r-yB%>dql5iLv9v*HQst8c=@ZwYn%&{Yy0c18l@V1VbOrECB>(c
z^sf{x6<Z$pno%&bthY_wl?o&GIbAy4yn5{Vx6|ecT?fqUyluqnlXEZpEOPC9<5eGr
z!}VXjNqK9qs#nmg7H<>Y`uNBCF6x%_-aEVa&#MHV@4bVX)q2qB*qv1IJ=f-!cPIz6
zZndxVwYv)ruU$TCn~lN2rXNb<etc{yyWhJ1H~U_x&Wb1ZTb*x}x&3V7H{W%yb)I*3
zx|Nym=Gpd36=Q!Mzvy*3zBskI<AH@i^Uf5dS=IP*-fhv%^>dvLx4wVg<LQj6mUUbm
z3YM795QPnm`Ppatz@<A3mc(t}Y#JZ!w%Vkt<-7Sl4!$dU@7y+UX|esmdcCr`bxB$6
z74E&btm5OvE=OgL7d_iOY(_w>z8)@9Ui#g4{u4Oh_s}?N+iJP{r^V+PBsF+xmp|3t
z{|-D+_Im8iWaBP9Jr>K1|CshoE!b>SIppxhZq{Xc<X2p_>3!}e$~?8dX33K>uighP
zZFQdWqV`$6<CaEoUw7DVeq!79!=+stQ<Hk<G-%p9rAFZ5xVQ6~7n-}CXcGCd+99JR
zU6wi)>C`N1kg0BWe@n-Ie6B9)pJ}MvvSNB_r?zXP#yP$Lj%OdsXWhKC?7;Gv+N)E-
zt)en|UG_fF$2n$Ou>YMyS34bg>^`9=G<(IRWzTz-Njj8>g8S-(U0zoINzstv3gxD`
z(y6gWryoppnsuypyLk;1y>lkOg&fBlJDjGK9Pj--q<Kb8_=sidKc=2Fujx(+>vHtr
z%)80I-|t+%#qZmVx{+qmspo?>hTXb8**i}K2E4AvpaDI7%KZ-y>s7D%s^lSA^=i)$
zwJy9}Y*1v{uY*zYius;Lu7CXTtlts+n)AK}1?EPKbobdlwL`0qTjvFe9pjBYcHOc}
zKe+L(VW}&|rngyMBP8(LJH1N}=FLfc9+=cgFKE1<QJ$p1-BWqlb*7G<Rp~xGDJQFm
z|G|5^z8}v2XtnX9Ud&6GQ@D=!ZHGLorNJ?$#S5ZqbvIg*dVG3Gc(>4Nx#3v{l+Q2L
z^}O=(iCawHi_ON@`qO0R$46gQZg{i${;8|gRz8{I==ih$qnoLJ?1D0`yMG%Q*yzij
zYI9#zEY@~!RL;8~_}}i04s`bPboRpcM%CKyjVjQ+Q7POTmB77G)&Kv!QQ3dFH)?P0
zXzs**M$*IF(|nM5fO(*K;9p-2jgAg54>pHOp`#N6qRd0#OO)Z};bUXN;T~oL{wU?B
z|I<Cs*r>#K?y@KEW@l7Dd^q}wrFr70;E=fZpqRK2^Qvo{A#pLnuUn!ko5D++3f>xV
zmpH{z3EbVS`nsi5R`sn*xCzYtAd-mTW10WgJDi?+DzBdgR-=2aN_(TWw69njw_^6#
z)?=TI?0qI*=GZ@E<((jfbo~6~mS;L$_&7bm)T6)uQE!h<t86xH=rrBv#A`?6ip8;}
zZ_YlD8d+YcEEs?6dx7PjKSPe~FaC3|IHr~PyHApkk9+!NPFsJ-WVOw{8b-sCj1pR}
zyO!Ck^*n2d;!D5S#=S>%ZMAF0CP^*ZoPxf$2L$MJjcV9vXN^GP*d=qSt*-aHY|y7?
zgJi8oTn%6Pr0>AU+J@^art9}ME^KZa(CYkcqeaG>92Yb?Sfi;!-?ARn69!y-*5yTP
zp}|Ry>2=o^);uv|cjne})9ecMp0u~v`OCESoqn^PH0fsQz241Dv}f*Ar)g2kGON{|
zs#m?m(&N`HGWER{A2)TZKi@rT!Q3ea8!epI^N2<Dos&0Oi0XHkUN=k9!7lwnfQe{(
zP4gCGb`PnMl{Mn4`{a5pM>bsN`MhbxnGN1)jl9~{f1jM`Q)7MqS`BK{u&Xugmf;v(
zqx6;4be`>)yufj0<M(bYr_Wl~x5m1Zp0!l3<kdQ~On<-V{l;0fmbnMday-3odw<*9
zJB^>+3knp^eJsg%F|pySW@U{h4)!=Zv(3~%U0L9-E`8k!`r1!3U*<N^t&>;7eAV0l
zou(E0dgz>~KIhCvzabXY40RvHu350%^ij^@!pN@vAzf5<{TwaoHEJY#<Y-r|>7<Q1
zt(w)TIn*?@W5<Go2@^YBtC&3hU2G$ry6ufC;*(C)*X#H8)UQpBs@r`#K6R)oyA|^|
zccEL$cDD8xief*{+BVYg$i&yHm(Fuv6MOegK&j}&;?p%Yht{{*GEmxOcc+F6hnFvZ
zUZ@(s|5j6*GsC{x_iN!bdCIe_PP5+3Yg(_3XPvQ8(Mu=q>Xv;%$MWRlU%tP8%M9i@
zOnCn1Ym=?EzgK?o_Wix(*RhF>9n6|+t9<HS_|LBeEgPFSG)|9e+`ZD^k>32Bd+J_t
zc=|ZzsF}(xs!p2*O`LP}7mi*KoB1~FQSZ{N$v2ZSZ>Q}FYU^%vW^fPvX(0_o+nRY~
zgip6=d8%c{=ckswFOA)=s=j1upsI0=PI0}P=vbII$(sbGc<sDk_rtV@tJ#t!y|UIA
zy2t<8In1qP!_=`GH#@W}>S?C3+WGW}{edYba<gUzZ1#KKXWQ_5UuQg7wR@*r{6(?$
zN9Nf@Yj^KbCUu$gBk1nPeqS8j&mH#7UEo}FAheU;<w#TG<=u6nly`<Gqfb8?xjcW>
zwTh(IcYjptazTH^=L4DJ9j+bQ8*n~q(bb3Psmph*ib^rLQW-n!+n|x71N7}3vbuEk
z9Z+;)>b9)LgD+g^etG}JAf;?+UiXZn!+k$?Ni-_IwPo(TuPw97o4XdR7-(}>QvYy%
zV&_qn%O4!5J8bF7m-)4CPO0;x#k2Hrw)yA9m7Pald)n7`e~IoED_e8_@niQn{f^z;
zzGtrG?ut{phg~{f-D>cIk2~rQ&dCh9SU+0FH|mSsj<F98Ji57W{>w107B|1GtXLH>
zTGvs3{E?CO62^B=Z}Q>xnqkc{M;n(fuoz#|`N}!{oG!KpmqmoRW%%A*Gqzd!yrBJk
zZtOQvZOeJ{#^Lyp+=)xyeu*7+;J~?BMKiNL%P%kNQSVKR@8vry!5BVkf5rKp-r}@p
z6E}t3-RY%xaCl{8=D|$QiEr%;3xZ?HUl^BfDf<1P!PQfH)FoCw!rC>Kk2-as=EMz_
z-G92SD1lF|AGMv|^Gk52nOSF@txE3dKYRVFGCgHbzX0=*#^v67zJ&C@`Qv-ddTnd^
zxsD%vCuUc;<@+lOZVd}=F(pagG~3wrWww57W#s1jE${xGY#C#jv_0WPhe7Z{w?a|z
zz1`|Dp)*5fyghb!PAk(>E_?gWs5sfi@8kTTOV4%>_%iVGrJK1%zaD(-+HslZ;F#KV
zy}~<LhBvr6Z$gp(1*f4;_I7UbR;uH;t7r0*l1;YGsuJ~w(XAK6#tbx{ziaQd`I9`0
zQ%2{E8Zf)dXS<9=!!AsA-Z%WKRl@iD)n7k9>+^Qi_)T;5TFyBV-{OxVC{NzmTlz+R
z@WaiE2Moo|Plo#KxB32VtpA(EFZJ}h^mKiZb1+2ZqWDp`y3e!WXD^H^&oHgM-)h|E
zo#mZh6_@uoDl^}+aj^LOee*H<wnW@~k`uGIOOkSkt;g_YF;^e0S+yqA;6tZbq4DKE
z2XA^6z9}h7QnA-}<3`(VIt6=j{YN~1t1K^6mz`OXc<b@w#cNs=f8RT6>y^y5C9@q?
zJZ!tlrPbY_gm(wVB<jdN3_W^s#JzzfrIW12z3@E#&hq7{OaJ(1N>5%cPkdi3H|yab
zmu|-nmrjifoi(P1NlD>jpQxL^7GDiNxU;lHvpoTg|Eb(^<8Iw{2k!pb^XbjepQ-h)
z94cDT?w__1`6HiAt^8T{MC9STwu?=##XenL`CiXfI)BYU#}TJ~CO8jxnO`tEVqe&`
ze4TSU7EU<-GsE*<*P~gRG76>~bUhfVj=r$tSNX?g4gqNkk8Li=KY75n@x4i}S6G@G
zmS*hg<acQ7PqXl`y}!TPvF6M^|83=cA}r^neG1R(Z96qyzeV?Zxpy1quT5WYpoO#U
zmp12by`H`EL6_(|BfO58I%Ot)a(*4%R5`xm=)x^i9S;A#H1+zb<5}^`BMiPp7c1hT
z0=FeTF7Oz$ZE$nr;;&P3b>BM3>KlwJJ9DP2*0K6A!@QO!O<84JR$!H~=jG?M@<;CD
zPJcbN#!2N-Gp@d9VP)tI|C6@8Tg)riJHz_2a{Q&dxqkYy)5DsVFMFoG9@(R0_T`>U
zo7G7Tnp?L+hO<?th+S_gU&p3?$(p`If3`*bz`<J@4?2=A3%_1B>}0#`=ad5?Q)5qg
z|Ezp^_hn_T{FKI{b0(<51|}JJ93A!gs@1}G9~)c$Ir;qdqCx7INuP4V+dS{(c6rU8
z-^tm}R?d9&V*mQ={v#`oUz&MohkX3kQR+VX4o&N^>>u~;E8eeepPv$Wy^EFG(gX4D
zavj{SFS+w`z@$EXqe~*bkMzuRk12kT?)Rtqi0>oHw0WvZo)`4rJXLo)ufets-JCsb
zoW<Y`Y5UT-9NsvQNi0P0xpS#VEwRuzt1<<+owfn7jv*0Y;Rzyz3Y=ZLkocgGQ3=Sk
za!Lq@iU_hE6&4kO{^^+z5<N&HgTH*(4l;!l`iBR^d4?p2jLkiS?j3UG`0kxVrPewz
zz?>MEFg7+sWR7Ml)-|^Yh!5!(14Nknc-q_fxPl{Y>uHPU2T(*|TtsX_Oq@t1Lnu99
z-Vt%}35XIAb`JyCe~2Z3ZE!?zLU=s1MPHET6Qqz?{53&?UF_}b`vXBvUS8S=k~4$w
z2vQ(_wTd7yaEv2Jf}XPB6C}0xdk(n_vU70k2Lw4gX{Co5gGY}XdDB(&NMJcRdZgrv
z$^Hj=tnD3Lot-f~@D>hQMgDq$EE4Ob0uiht2mOPiM#41h(?=9CDmW%EQderBePjG5
z`LXr3ak8<&)Sw4|NNNy5f%FKd5et~3A{I+#p*^DfPvm%5+uFLI_lTUGP+UopLkY7`
zcKE_9VcPP!!w;S`YU4kV<L2UNYhw%KcxV$H1trR8?^msrSPXCLaEm1+$&p$7Jvk1p
z)`Of7cRaL6j)D?onLv=m(1)W&Lavq60zp8CM7?|KVl5-W-2YF^8RY8Z>oEjTqeXHQ
zlpGV(@V6BsWtydu{SSgXaDb1iwF9C?i{L0IIToyxf~N!}Bry^RTIRoFO}~D2_O>WJ
z;_0bXa40zz0|gqvk?;gZMkWCyV5$C=9$QZ*>%n#mJ@CR4xv!M8AoG=uSOR@G(Sdis
zggRLwL2pg{9e=#+eFplV#q#vjB0EZ2l3`7&*rVc8Ls<yf-*2md4$gy|kcIH{)FL}d
zN|MDuKovCt=D<T~1arh_ssD~S_V%uW+}$uaHa^<KM@dODAwGNtktR#}KWSw*8xMv-
zT5YJ3mS-W0c>4>x6nvX5xA=Q`_V98W;LZ}HWyw^OJc|(@HESl&h2#d4ivOOVA)Yn^
z`wc>C=BsVRRFp)6m!l!ag6H!{$^9i&{Jm=K?=-;1XCNj=%l=YP5-nIQJ~0d<`I`Cf
ziSZd^KggXSM%!9Z!pvVO8BcN~q$o?UfB1Ks%g)Ptpsz1*M!R>4J#Y`ukX7!oRIpMK
z@KvicA-&oEo;mKWgWd3E2YO%?NX@LKBv}S#Tq8Mr7ehu`E~x~G_FwWx_#h<VfT?LY
z{#W)$seOe_$KR1d)EP)}C_$F;4TYG$T2PA+s%EJg@$2u%A%jVh9!i#xt*TlrzTuK0
z9m7|H5-p4677!K^4*_IMB7}Od?rs5bDDqUPk(_~)4u}Yj7x_uBzVZ-Bm<=M5C@|+m
z5*6MsBB=!yRFM=rG9LcAn(ZvbWC(fie;ZWF4kl$xo=D2*b&*t#`7DyML`apG2*IE#
zdYk+o2W1qyNXp0|gxYv}h-6}BhB7fTV;K`T@&}bk>DB(f4Ju;<StMgzc!;0b!59};
zBvZ2gR^n9=466E{2URg^D8oYPA(FFeD92F){F|Mz9Qzl7HI-uv^1rO9oLy5n<`oPA
zwL6eA_%D`gkzCFYC0AfLf2MNGg#Y89thc~OuUvuuDUvgOmPoF`J3}OAo-h?DuzTPE
z|D^;;p~dt3*EJOks$kitU}vmgB72bn5<qCi3Ra30Oo+#ysp5a0sRA2vK(1uis9<Lb
z<xxDSQjFD^NJ;NgrSvbhj9{ipR(h4}S}NfoS2RN<E5%B7pDG!m_%l`F-S)q%sgl_W
zA|>PZz<(J5QZnXDq*SvrRbyKDgQ~QI#ey{j16-vB1v6DyFl(p6p{$2U#jdf6RSGH`
zHwm^1qe%b545b>St7hb|O6g*C?IB_`DEt>IH4o4d%v5FUl$qT~jEd;~Porw)NQ}yC
zGY}yx`AC$Q9m>cDtSYM@87>M&Wib7RQI+6oF<X+gFJNb=<xyjeE5zcOm;kSpAVY<{
zCJ|$3vDp*B`@%@vI2#7OvBvl?zg6LiNPsM=gjqaYb8GmMweIY4GeZL^x44ib_>~!k
zj}Addba<;V=vY@uD>^Y3<++3;Csa|bK`zFuAdxdR9B36YYPw1^#hl-UX%#c#&YL;|
z5uy{Tv4TUiia9@vrL_tjrjDV7?>jKRljyK$V04^U%c0{Y%TUKiDz|zZvCR5$W3fI+
z4+(z~LhFXtPt0rsW>n$&Rk4^Il@$|qN@SoR#;Svu5@Qq@<igb?kaGbkgPffpZ#D7n
zBy!Daasf8UFBTmsdCcGhI>zt8v~u|j?UE;DXM&M)V$E(PF*gz9LxNbaO&N!<hm5}%
zIEO>hpb-Ja%4!6Z8<klMHb2JW5QY%w82g2%#HIH*4iV@CYs+Sr_{iy<DrW2ugPd4f
zA#ypZ^_j(>DaXH)lxx;j&MGABlym8K7CE)j_;&)iT(fT(x8dKo%#}VE<m^0nY{9>i
z$TdsIm2$LG&PIRcEOKV=@sQ)+3FM3~fO*cQ^*Gxq7V?~Ni*z~U%=++=Ygte>af#F!
zTUh!R<ZNo3MNUPrV*EQvxn@BbgZpnhXVWPha%w?^&N8FJ*fI~IoQ)bO${8+GNqvTG
z1g&gR5TRp>5*#|AN#pb|gPYUQj7=4D^I#MKm#}3OfxxVaaR!^9flc^h8VI$_L=eC~
zWLCvg$&G=K=&H;r6c8B$&q*bRj+q6OO&1Fjq71_s&kW0<Shzhoo-o8Q4sw;HVrgY9
z6+4txONBcUb70&;W>7{fxMd**Whcl?h@Bo!_QmLT61Qg8;tC=1TG-f~n;+&j=N+-Q
zsZA@agD}j}ct~QxLlV->I!LVKV=4tc1OA<$+d{LhOlYm0ZpJU<)B!sQ4NJ86N@C7?
zwiXetl7MK|QDSMPTs-`<kYtV1MEFcXy!<s}sprv4uOR#V4A?J(@5G_SD@OTs_*Wr;
z8i$BWyRwoX-26hT#%W(>^K)jO+56lEWB0y_=LBf+nKUjFmlS7*=LW<(i*++JEYad1
zG0`g8FNAv}#KdYB%pW%G%|nZR74k>pClPLv5G{WJX=>>OWC9~ZE#dhH&~kM_Of9Q%
zHPmYHfix}<;Q<NJvigKU%aG1POB+j#^TXAYiB-awC|?H;DrBdk$}kd+j!@hLHkXrW
zM%uVKGL|-W1v#av;0c=+S4iUt5snZy92Pd#4bm_~i{Ha!C(!Z{UXK8iz~^CIHw{`X
z9*@T1A^aU7TK@7;)M8&q$aIaXLwGtITD*LeXM=wwFjXxd;noP3kF#|cFDH7;@^P*j
zUOvv9)e<mhaeOp>58?K3!{Oy)b(4nA7$BU6#^oVA9u5;Gob_omnDDPcOd7|9J?_F8
zQXxZFtID8d0|bU2oGQiq;Jhf7AN*)mSWCjDmPzp-Atam}femLy(@a8(gTwQ0xUP&h
z!2Jt51ANU%yJ58E<T)@x|3!e6uQ;_hFO)uJvSw(~EE`!(#3IEUX8kbDoY9ZMb;DUk
zgoT}zR-?z?XPjg(tH!!3Y!ZoWr%j5ffi)fs{G{=_s>B6PfSHVjel<?7b2D%Whz^0e
zim_cxP!G?9fVcz`5CEpKVBF8c)y2!r+QA;dFdC5tW@Z6UG_1(jF*GJ7VN^^)h?&>~
ziX&`G9u%6&*$M)`F|?L5J`J2g92gNDkw8!)Y|Kxcss$b$HwoU55)+Y<trhXHNCC`b
z^!a2%V<}^d_)usoWz*F>6fzSi!C=kBZz~5VV}$rq5z@m$A~z9E1}!AHBAU($gq%rb
z^Kd8;6w5825EVe#Dg&>r8aT}+agCke<R-IFSky!;0M<y#cqQ6r07O)^6cW$ht=vj5
zgCKEWi#u#z#A(Ji<gg(wv&B2M4dyUAF$NFfE~8htUWg@J=|CeoRim<)6a<@*T$5~O
z`XFYqRRgv!VkBE3VIjao8DsreD$rzv0AQkwv9??jV1bOWOnkOh4a%UFhzUaZI!3@!
z35+Rc!YpnCV4$26NQ_qk43x9=6&61*P|odYrVTJq&L-B`HoyWoqlzg?(Wq6egp1Fw
z)d(pC2Fe+W$n`~1$kc*a2wHo_kKzDGQ!rMHgCHfr)?FDAq=*HK(WVK7L91E`2}J^>
zb?v2yfdVO&A_fYiR0<4K2v{lw1}X$Bl>!45oSMW;mI5^jG2XZorD#-^Qk8J=@mgx9
z6d0%w>;oWF%BUR{nG6`HU^WH^0R}1rT$cd@6#^#9fEWeCWSUSIlpz#a30Nvf1_;((
z26QPnJAhdsLkwigaV$2(Kq2jjft;RWdLagKI*x%L7BCS6MJXDU#jPeNMTSZ%OMxt@
zU`RP)pg<brh=BrWkRt{Pbe0@3kO?hV66A;lOgu~x3WG9qLL(tZkw(@|C*mIyo^yQ>
z|KOS<w!oM-a>PHj>d4|p{9|j&ECl$cWKR+>5I`ekSWMGdMJS<2#oFu@ra=MhQ?m7Y
z7C*30$z(TJ$`!yqC6j&OAizE)li=YXz&<5g|7Y<4F-j)=LJ^8at!gFY=)W2#1x#GY
zmQUHfz(1uxw<>^tN`Zn>ApQxY7CK3pV3j37iCDnM6ip}$$_xgrgoOU9ZBilzGNCTl
z7cr3YS1{X^h=FX$oyCtB$T<Q`8^k~+nabfoEMSrc6s2fX78AMn|7x6oTT;Q0z$__S
zwKWcijfJE85y@uO;5TwB8x$T8n-CIbiu@4?$Hgk33_&?zVxg!dc26maVRU0nBO!CO
zSt+h9Fq1tQQPmx|i4P%wZ_Bj;c5-@x0|7rdCBT7zp<+(gaxf4OF?ZlV<En7K!su0F
zC<&3!X0n1Jz|FP^iBEuPC5$E&vrZg_+eL(gM@cL!#DTD|jNt+7s7Jq$xVVsjMEFes
zze(BS0_1N>xYsMghl<h6h5gXtGLZ`TlNfj-WsD<#Q3y&2e9%d-MA)p6st==Zj7f|i
z832^4gVC&TjK`9|{UipVPzvTWFeWrGBrXhkp?$!_f@~+Zn1uMCaD-6|J5`Ex88_YF
znB=ixA)~@$qC?;(E>OT^#Ee3Pe|NVY;jthJw5Lqd%FLMGPdH<KB9Aklp%Gz;aUo_3
zEar@pBQj=zN(5B0XD&2AqzpKpGZqff;l0H9DhvS4lW{^t#tZ~7Nfp#S;*Va{P)M{k
zOUj@GI;8Ab7`89oMyz}?ZIGleiDZrt*hDhUIxuZOs%6}wFc8>5GTM!*8kHT5*gb+A
z2ghEf4~Vji$*^!#fTEJI$D$Z)NGUMs7`BN7cqn5sUK|8?C}U5Fv7MlojL~BJjZigc
zRVyJ&wHZ^~M*<9#u?MKwzQ8~kdz6ZWAO>=3i0OqG$mIPwN`ZlL&fqd_fCX~K_)(Oi
zQCZxCXNq}7?okFGFi<X7G(aL@1dK%{1qRAF&lGo(0t4j&CNr)*8~QPA0FRuBv1meJ
zP=-)&riCokW=t7;z(6^ZX65<<1Ld5JXW9S*<?K;07C&MjXF8cSh=ELPfWw1Wpu!mu
zic%C%u$TzXRFDM}NGWuYu;;9puE@2Qt8vPQMTYF7g0qj9EbQ8IP7MPA1}X$AgPn7R
z?G&L^bV4H`N3~f{W&q%yg0q$k1o)>AXe1f%Pa#l1GT@(r$-Z-w0sbkJtUrtK0AeX4
zwKSdZ8^aYMlY#?Its;sYd!_)xbw%uBR*ZunUCN%pWYGirlw459Pz5|vGI2833y4uN
z+Jhn#jmlyoC>5MW7OcD+_^0IZ3e1?mKc!s28{nT(pnT-OKc!&D0UD`*Q0$y@XOuOr
z2r~q<W(b8=f~6Ovyaj|RfPqSZHo~qw7b{`B3dBGzaA6>bf$X7Xc0|NL_V6zY0cuoC
zm_o4>jat=8Na?>CCj~H2#RV7)6~I6hmo{M_z(AE?r$8roZib1la1-R(t2n=kfdDZo
zCXYfB3jG;E!I>5ksx8jL_<$-Ydy19m3Pee{#8iApP(nn^DDakn4jHGjG*F;L#svWy
zC^AY+xld%Qfx@!L{zVjk20=q1>DsK71}&_uj6Ijh!vX6n=Vqm83u`RrgjxdyrpcK)
z6rT#9NzR2`nzq0kIUQ+fen;v|^Djd63W`FUTvxRQu1UG<l?I4ZFqe<eK!NXarg+F3
z4rl5(Uq{mxsbkLH(LjMv1rvGlrAYHTe;$zD6RwChW2fl~yjO7kuLcUdSJ2Y~-Yb|g
z9)BXhdj%I1XfOcp6`Uchg2L>iOsJ}T9zcv{K?tLb9a~a$YcK=r6_m6f7eYa81ms32
zxO#&I17ba=AT?0Ldd{?KponXXdDl7-<jn9ULaKIc#!`bD@tzVFls!~(CQ{QDc(0@c
zP71tNa?x^CTbPKHv!5Cu5UOOL+9v|UXeI(KhETM$MW9u<fk#q`bAV6ExvW(!VQx|`
zJ5~jRsY#jY503(vnT+#P*_LKv6wxrMTI(EmLWSE3rXYCLf!O8H8YG2N`I_NCP&g&8
z0Rnrud^rb{Ai}tCUjsy{gLTt5pcJ)a)VP)@Fm!OsV1^nC=b$Z8)S!m(i0ug|U=GA?
z0U*qQ^BK7AGDH~{=xTs?2QijU0|cD^DoTJI`c%z91_TJ_pv^XGPy=Oz)R@W4U}3p{
zQqvLyjPuzzpd8JCONDBHKo93GYk)8ZrlzND9b||@RdbM2bI@kCHK?JlnDfRpP?&_6
zOLu9YFblY*D`c?T4EBkb6i2xkY!opmk2pafX2eX}Oyq)@pw`0G_zzYbB_BX*hzVT+
zU|^`2^Ypo?D3GLy2@eI@B4&_MsX!|#CdE==hM2*~rM9Us<W#K+YA&3MwiK_1Jj7Ej
zbEknKo|00pFazx)CM<**&bo+6IaFe1kb<bh%wVE%?Nb42s%C;(3up2lEIH7DSXw3J
ziGkc?KtPcE!`*yVoP<B2MdR|D@tU6i9qv`NLxew}UjM#NFfGOeHVH8qewvuMYr~v8
z@PMt#B!$$hrdP9`xtiT4YC6r&JD$K~r6okV2iKN3?|~bR@!i#IPG8M#I5nF?RI?dF
zHIqZ-FBndkOF&4N!AObYn?-gkcH64i#G9He4XWALQO#~(HRFm=W8n%0P9auhYRL>K
z-Z#Vy*<zrY&8w(c8BnufpPH@ttC?&Ue^f2$N*sWYsT9F^HFmD7IH{RqEbxjH%V*Z1
zRI?^g&D0qAqcTGvC5!)p8jN5A;FEB~6xrIZnoWkPS<|X!jyH+aY}!uE<VC5$P~^w$
zI8!sI;|d4lYjk#YtcFvwj<A}&?5k#ze`;3OshPt8`~le=3B!O!;|9gKT;ZU6<;xDr
zhWlzZZK7sWrxxU_cK$D$4?B_sBiVYanytdBS?68N>SHw<bg7x+i2R}Xn@=-Gl9hbr
z#m<VoS*re5<<7B+R6as}Y0)ICr>tgt|9>U+KjY=Q>F_QUoIX-BF(&Wiry6_T94ncZ
zlxzdwEwq?0bY2|ptb;nkhFsU2yBr7*?j#yMU8QF157XW`COE|0J3fSK4BenGW{o3z
zsCMDxCx3h#PI2Q~Cnki)#2GsVjEWcaj|m70kBAnr3d6+Q3mxP$9v$my?P>2Rf+6C%
z#*3T-M#0;@Cg@eQNJt+To0!`|AqbvJGaeY=72+cr6Oj-uf;WFreJE5E8xS-S=41jF
zBw~USgF@nrV}nDXSr4(9%)$&blKFs;v14N5g5!;&VIEPU*tn432%tSFL=*%M1jNNe
z^#UYuFztl6h#(QXD;yjbkN|(h#>IpII67ifST7M%6A=Y49pmA>xcCHU77{kLmk6kf
z>X8r;9U_W}h5tmvL-&|b@R%H?8~(I|h9YCTo>0GmyYOPD2*Cw$Wj70PR||`-&<+^<
z-<o*Ep=x9>Jo^+76(0iY0&nz~^-GM33Je%EDkM&1>=hmnFM|I&M+`u-XP6+8n5oTV
z$cPzp>@eZ2R5CD7*FvO*2bL_V{)f_@ps7MdRWM{A(EmiESeQzJAEV>L;I_0Hc^rHg
z`WtyHBn){30+?Eb&$97hfKP=an-7D(@kJXFhT>)cObz6LRPtefPl>X8d>H&KK?aqC
zA$7usi7lj1loP@Lp8{1Z`0e0tXh_45TIRPy_)sF54@3BnQRl<pZybXXFno%P4+DJo
z8W0}__~gh8@L}*bK4?e4@Z$x1nAk!ErAuL7z=xl2;J1Un<w!M?FmwTc4@3A+l8+Aq
zd^kzShr!<pWS~hH${X-uVwf#50el$XlaOP;@A%R_za4^+as`*3@L4^6JHRJK=8_MC
zzws@05{B$JABOOeJOF$kFv4~SpB9<K&K`kJiQFVUOrcb(kuT4O!QWW-lQ2}1;ltpb
z8;~P}0X`K<{qftucC$df3JF7%WIhaDIF}%AfDc3Xkc-EM0X}>mjf7F_3uSENRq)#Z
zJ_Sm>@L_;YLhl*Sti;^JQaDXU(}!>A6ZoXa=_O&Let|2GQskcU+W~#}Edf3Z=#y!o
zPo@PQu66SJ0zMhNx4>=?yoWJxKAGPa1XoPzFu+IYSm=wN?cn!CFj96Q{>860@Y_Kn
z2}*zPVbE7e?_UT+30?tlQdD2z_eJ<<-3iys>G`V2aUl{{qOgL%XF<v-&<BGE+aVY_
zm&EUj@R2-FKt+hw_vna+urHVt3*_Mm@j<*sjEnHmwg=t{qIfF7NeKZyc=rUw7<?Gu
zQ=t%w4@2e+1<M2spIYL>#Bk~lMb3N};8T)g06r-S68Y^AjM6V~dWEz-fDb=cOZ3I>
zagZ>S#pc7rV0%b80DM|(j|9Jw!|x09$w^rgt0W3i*8x6Uo+A3<GBF85*%CesUR40?
zB!mGzDQWkC7A1-S`F#<Ll4li|1%d~FPlEhvqAz%2G>p<O5Ym%!1^C3M>P6tg!M+d%
zE)6j*yg?yHp(Vc^;IlyC86Sq+Ar!(9Fr0DZ!vLQGg&%wvlrq$$9tV7&0EO)k3>_fl
z_k~?#LGlIgsYtnkz7kSS5sb8}aAHkD>M+0u5+fWJ`btp(flmv9p^5_^2KeABAVL`6
z!v|sc?VvB^mmnA^2k>&3gqCM0!cejSZWFym_;3k-T&V;WlUO&PPmXe4{B}T#wlVN4
zP5gH72$=<ntNAd%ho1%G!vG&X`b)r+<Qf4!{PGjO9pD2&5RL)(a2=oD4qz%$CSf%w
zI}P~ogKcDAa-Rb}ocZ9lL-<JB1Ng+`xTr6U4~j_yuRvHNWM9AsmY<w2Ja|p)RdD&m
zB<}$qev6t<vjW7A_Vqyg2s;D#B&5F!H9XqChqJxpJWyX!cLEH&N8vo+QV&k$6ZGNZ
zBzzd^OYSXz;fJmH?Es&Iw!d(ehZq<2MR^~7Uj!p*hDY;Y7Yf?}{~&A@!l18~7!2-3
z5$!-qWu)H(`0zP8f))kIdl&<s^yjyef@&w%4e$Y&q7T16#_tRG@R1xo46Z+c2QBQ2
z@R4IcUwj&c-xtA9E=dT3SK|o&fdtX<mP&%(=@Rr+NpSK?00YZ|PLK&;U@l2LX`zx(
zej<X=u@Id2BzOf+c~ScuU~-f!=Z_08d?$^7Y0+0wEjku$bRj+q6+GQY?r9Zl8G=4&
zNBg_rf{?lsvMLgk^ybqCFd&A4;TuatJA8+PfT<`yM<rL2HrYZ2>r3u86&&awbr`sb
zRNSM28#NTo3j9W%kQTUILg9m38kB6n8-5i3;5-~fpAuh66OIAfgT{wnuOr&2NgDvs
zKCWvC+kpU5a|6{$tp{u)Vh;j7djEp&h|Vnt$AGdGg%4akiU&}tpvHi#J0+89d{v4V
z1HWn~fI(>q-K8L4_$VR)qkS{j<)qJLp#oh(%>!=ll4C%qM*6^D7$FiMX@U2=NFG38
zit_8AIE6|qLYgHqbWMqX;ZsKh3^G4dJ4hCgFnHIRTsKHFP%;TiMCAaWvP91pOd}~*
zYDgzi<H{g$M$!Uj*C}4XL#w15K#GFezmWQ(=OLC*wg(ChlstomrTrWWT(%PO6!s;>
zdpKT1@fm_9iU*LGqUHu>i{OhGQq81I0T{l%%%=rl3RI*eVED#10aK%bs}KgaiYVTL
zdrH#`i!SU7Tv5^M3r2uwr%=L2SqNUi;W2!&lurx5@ME<?7!-Gixj{Sh-AKVaU<@f0
zGl51V<pmnycjWkEfK!4?ze1P<9||R45_11SAPJjQNDIKI+!Da#RE!OAH)&^39-Wj)
zSYIe?2*<EcgFzzJ4aR`oAZ!OO$wER_2!jL<R68h`l9oS+a)`BrzF;w_zPN<W=M{{r
zAms%fqmq%i2Y^ZFdBDwVil^}8Ff|Xj&PT`wC@}n9EuR)K{Ev*q5r~v=7*s;)cEAT;
zXd>qe2XKk?fOfbvEEv}UY%MhpaEyt)1-t@*5sm>q9F^yXs0?2R5R3sQpU4;j=7w)(
z6YZ$H543}%n{ZrUi;RqC;K-G>x#4^M!ZBdKkunMCadbX~Xa@;i3WjeE5$*8zt_T=@
zM}iMSd?E9WQWb>aq`W|?nap_rs~}jTXu)S@`C~vke1w^R;WNT~80aAoXcC{C&S!&L
zLXQhCTa)_@mXg|UVtDzQlu3Y*J_CGR2tUrsrw?F|9VY36G6aQBiC<L|_J!0nA*WDh
zqGD`-QFSI57k@N^z=w}L6EHdH%L6|A#5d87if5o57+c}oU~l1D#e%+IJxHGmEDAou
zO|%1NPlylNQ8{yfiP5D1L0@#yl;jnJWu(6geN}XvC!^wIka0QP7eYxw--A|z!T~`G
zRHI1W47NFbYEnQmN)40q0DNS=9qg}ylu3Z$BQkt^pu;4jZi4-RPm>GViKQgXXujwS
z3BMf>Oz#<p-l#nT>1;BFMfFBX?jiL`#wn0y)xs;aiab{U?I<4|U{s9{!6@FtxTL=e
z?I?d7V02y{x#OfvLTVV@3=;4P)DXIiN5H7Hgu@JS(q{!2%|CeWf#w0^poskjN&~++
zDI6Cb&!=gjVk<~VQ}GehO{q8l_Oy!f1>jITnInT!w`$V21$<Q90BDxbYXpG`MKicp
z6rZ7RK)0jT2>G34UI3C2GBS4qFnT>8Ge^lYq%o=W08fC7Ljj+J%qPNpRg_Ny(LS}7
z03-c&<ZX~~58A6_?hs&9T!vuuxS$S5eFdI3nU946q?%fD@KnegC?vS?7Z(Nc2W}Ik
zCn1z1eJ<#W?~V%lLVAPH<7gg~{ea2@8IwTGT201&0E1*bu`j_y(e+<QXOnyZuak^T
zQC*y}Gmya}>tImt7Sk}~krT8)YEBD2e4$Uk7r6|7XoP@Kc{p&d@H-#Eb|B;AK9b9*
zTq*2xH6?#?744V61!BrJf(K0b^59C5wI^_|Ad5`$0P5nT9fVXfwO8ReAj<B8l_uj=
zaP`1}A;*wG0*i#f5pJR#*h{L`Ayz<Hm!c2!0nry+LVUGJz<cOR)>y!rLjX+B0#6E2
zGOhq4K<ouEq(vz{!%9+d8NjHx0#c=9P6o~aP&p%@50n#0pGrmTUwG<=@)=+Z%I?F}
zWXk?RI+pT*As<2QL8uN=@eJfL$lNuI0LMBAngK@oO`uDl07te1g+=v+u!rP5_*axp
zgjkZ)#qf>_2nRI=esWMC8*qJ^ifaKz=>vE;gVJjNBjp~l;}AKL<3jvF`Y)(XN6Is3
zI!a$beUP%#P-UWQ1v&;m?k&iGQaKN(hf=Z&Q8%TVPz{J&4><Ke`mAuKgE~(LSuWUd
z1P`DtMCCys1&>Z=3)Tb1p!5*bPN+Sol2i3JaGGd+4{B0NU;Kifa9qf5korIcuRT!n
zfX_Tpe1;kU#VgRKr0l9x+Qx+wyd)1GT};RZ+B1~T0GUn7|AXUU<bH#K15`<gG2mzv
zWmi>j|AiPA`a)qy*cVlRNL_+zpY*uEdy+5Uc2YVAI7Q`Tz>A>ra1bp}dk{{{lk`Cn
zgtkX;)r8XbaNB^UPfo>ofRC!3AQ-i`z;aVORm1HNa(&fskcX5jH5|Jk^%Wd7C4D5M
zVJZJl4K|coH+b%YkZ0%%orP<OVC3FMvPSMzsMt{TP=LwF^Ns+6gd#Zx9NVPF#Sc>P
z_aOA8&PyQ}=`(;uf$Tat1{{W<#sEW2$R8vRs2VungF}2IJ_WwM!>11<2$E?;J4is2
z@dsq@!SEC9z!M;47ur!a8NtXs4KT`901V%K<I{&=q}@VivdFy!Fv<o1jH=DTKBwxE
zzytiuonTz}q9jQ(B)+I~s^D$luLTPGLLfz~C5(YD;_=%d7<rBx#(<M<LOvrHx#j?)
z&gCNd$h`mpN%?R9!=EG<&;l85(ys%2kdP&5fn%G5yZ}D@NlQLH$Rj~Xnd}Sh6|s*1
z8x_02$vi4{hB2u6FSNstLh{E280BvO48L_kw9_(QaI~rU!f^p&4?<-J-@FpygBx<h
zzJzx8K~G^jNM8|p4V^k9_ZEVYwh>@dZXLnMy#O%kP6I^yR6Y@4)cIoQi(e%X&<wkX
z^iNUtfYhe|qw<buT+;V}NC>iXgls^{jr6^tFLjm^`ck=YfJw-B0rt6;vyCWdC&z^n
zDFuU5ZG^6aR3GL0AQ+jufrK=bXMmua$`3(?2Z{+KEpSAH;tM#{q`aVXA`PSR2QUVe
zw?Ht`zX$p($T$UHP(mW<Q$hBhf`PM4!^oNq;G<;&HYRD`fPZitk-&#asifWj6-LI5
zAXU`e7Qjd49f1}o0#Reo`y2$2(x(6;>%TBAy%zvR@c;ys%*#VxS~dVi%N4jAq?|$y
znLH1I{2Yp>kcFdU2*o(048h3&GTuU`i>dX65<O*)z>6i%BtT#MjbMQ;ff_yOV}f@@
z#jDU4lJ(>o!Q+3VO@UdEdmr^BV`n&U0tW#Ie31Sl>+z6OQIqv#=!-vHE0_ly%Oz{V
z&<;*zkz;^}llueQFsdDd#FUJ~kx$aU0{W;kK;YO~P&|;y#T5TwDM?=n^dx@BPdGPV
zGdT}n3uVvY*f7ll&^Z*JwTuBb9?7@@o&BZkAXNRxvnl8dE~UfZOeH0g@bEHad!T$t
z=Cr^Yms9o#)H7v|AcahxlZL|#P|PCM9M1Go`v{IWkU3fyS3={1Yl|fRAhSuH)rG$J
z>kNW*gQ$X(1JDN&%CCc!Bx4*nHw?)UY97?RRA@)V?C8ufC2Lyl)Id8bZ;N1Lz7t@u
zN+d0CFNU;7a1V|0*Pt>>#$`~grQ#!iQ85mR@~Hg=nHw^;hrYm8f<7>f<e601Td>o~
zcHpa!wg(-xBG(*pOVr+iOfID(;OY`ZALNY4nirg-(4vds>^^DFk&901U8qn|_6X&X
z$n}6FqVy@8qNQveynsU4c+i|$+Cg%i;2-2XDE@&9Ozjysx<IZ6lm{V8LE?ja3Asj)
zlcC}ic+P{`zmRJo^JcK-;5d<E&}WLk@Zg8Z1p60UZ)z_fwM*E2i1w+x9>Bo6AjgFS
zDzT?grhw!Ncv94zB!E%pebBh14M2U#+z4zA%I5+Y9LpEd2Ysp9Eb6NTA5}wxcKAIV
zKK~Gm%q<}p>HEN?4(i?_!0;o>LVWNU48k9Wz!_2!<UBw{5HbYqz&oSbLH3J;L0XiM
zNpOa!JdF$;wx`B`<R-CKp=e6&X>@Xstak&9%40%hf;xu>K`j&}37X-o8o7U=9d)M~
zRuX?*Lcmk-7)UvVL&)I7kz>FC1ae#nTq`1E92_M4kgi}1IDbIdL9i60&wyM)YA=Af
zBz;UM!czJL5<FC18G=Tt#sV<vE*?}!soE#Ns2U#x0n~k1fGNnF5hTp%y$UA-$a)17
zQ>Z(N0K?Ct^7#kKg}M(0FshymRt65L3fB!_R9+8NI!K;^p{4Re;GIGOn!pDhD=7zX
z$&fm$3o!gW8o@l^6-=_$3z=~I%8;NP_y^?qFevp>_oyHtNW~fequyPBN-F;DgJ2BE
z7?E)gWV5N9Ae_XZasaTpR4x)=aMMP}E6_MpEfu7II->-)x2W^V;7w9_LIs#X(&qvg
z^{xgS{D4FzK?{@u$#nxqkID^$y`=8Z!+kI+wg(vH9{`N1*?_cw#wTcj<GW<+2kjt2
zqS{gS%Ap;VI|LXNBf?1+s!j(m$Xt+PfSXOj@TY+I@&bLKl1TNX{1*@^I7UFWgRM`)
zD4zlP!qGmeFMW<tg)BAM4k=0sM%|@A6=5=e3NY&25)?kEb4viD?u-JAx~B;+>MR(*
z@W*U~JcVNb6i=c2LBsGz0EK;F*N`#}>0Qzng=8Ha2dF^j5PA|FkI(|6?!`bmNMw?H
zhWsBzGwm~gX2K6v3g>|?@{#y}snl9R3`WTY98aWZfubuV&-C3dNYqpJVbL)UGKK&c
z6?X#+F4~YZ)3qWvB0`-J0T>)gCHjKrNX-Lc5=tMyRX6hP1K^`#L<A#qxd0Qq3jq&&
zMMQ<f>Gtj|GWU!a7lI$kGWUQlN5ZRXps&#PE1gD##sC~v0{_cS<kwp%wGrFP?Zvio
z8@0^F9ug3CNWj!~5<5FP(42il|GkO7s81i=c=T~g{6-hJBXE{qw{vHE*M7SH4>Uh=
A#{d8T

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example1.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example1.cpp
new file mode 100644
index 00000000000..1e4b7d0ec35
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/examples/example1.cpp
@@ -0,0 +1,97 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file examples/example1.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Very simple example for testing qpOASES using the QProblem class.
+ */
+
+
+
+#include <qpOASES.hpp>
+
+
+/** Example for qpOASES main function using the QProblem class. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	/* Setup data of first QP. */
+	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
+	real_t A[1*2] = { 1.0, 1.0 };
+	real_t g[2] = { 1.5, 1.0 };
+	real_t lb[2] = { 0.5, -2.0 };
+	real_t ub[2] = { 5.0, 2.0 };
+	real_t lbA[1] = { -1.0 };
+	real_t ubA[1] = { 2.0 };
+
+	/* Setup data of second QP. */
+	real_t g_new[2] = { 1.0, 1.5 };
+	real_t lb_new[2] = { 0.0, -1.0 };
+	real_t ub_new[2] = { 5.0, -0.5 };
+	real_t lbA_new[1] = { -2.0 };
+	real_t ubA_new[1] = { 1.0 };
+
+
+	/* Setting up QProblem object. */
+	QProblem example( 2,1 );
+
+	Options options;
+	example.setOptions( options );
+
+	/* Solve first QP. */
+	int nWSR = 10;
+	example.init( H,g,A,lb,ub,lbA,ubA, nWSR );
+
+	/* Get and print solution of second QP. */
+	real_t xOpt[2];
+	real_t yOpt[2+1];
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+	printf( "\nxOpt = [ %e, %e ];  yOpt = [ %e, %e, %e ];  objVal = %e\n\n", 
+			xOpt[0],xOpt[1],yOpt[0],yOpt[1],yOpt[2],example.getObjVal() );
+	
+	/* Solve second QP. */
+	nWSR = 10;
+	example.hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR );
+
+	/* Get and print solution of second QP. */
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+	printf( "\nxOpt = [ %e, %e ];  yOpt = [ %e, %e, %e ];  objVal = %e\n\n", 
+			xOpt[0],xOpt[1],yOpt[0],yOpt[1],yOpt[2],example.getObjVal() );
+
+	example.printOptions();
+
+	return 0;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example1a.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example1a.cpp
new file mode 100644
index 00000000000..57a28757cc2
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/examples/example1a.cpp
@@ -0,0 +1,85 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file examples/example1a.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Very simple example for testing qpOASES using the SQProblem class.
+ */
+
+
+
+#include <qpOASES.hpp>
+
+
+/** Example for qpOASES main function using the SQProblem class. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	/* Setup data of first QP. */
+	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
+	real_t A[1*2] = { 1.0, 1.0 };
+	real_t g[2] = { 1.5, 1.0 };
+	real_t lb[2] = { 0.5, -2.0 };
+	real_t ub[2] = { 5.0, 2.0 };
+	real_t lbA[1] = { -1.0 };
+	real_t ubA[1] = { 2.0 };
+
+	/* Setup data of second QP. */
+	real_t H_new[2*2] = { 1.0, 0.5, 0.5, 0.5 };
+	real_t A_new[1*2] = { 1.0, 5.0 };
+	real_t g_new[2] = { 1.0, 1.5 };
+	real_t lb_new[2] = { 0.0, -1.0 };
+	real_t ub_new[2] = { 5.0, -0.5 };
+	real_t lbA_new[1] = { -2.0 };
+	real_t ubA_new[1] = { 1.0 };
+
+
+	/* Setting up SQProblem object. */
+	SQProblem example( 2,1 );
+
+	/* Solve first QP. */
+	int nWSR = 10;
+	example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
+
+	/* Solve second QP. */
+	nWSR = 10;
+	example.hotstart( H_new,g_new,A_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 );
+
+	/* Get and print solution of second QP. */
+	real_t xOpt[2];
+	example.getPrimalSolution( xOpt );
+	printf( "\nxOpt = [ %e, %e ];  objVal = %e\n\n", xOpt[0],xOpt[1],example.getObjVal() );
+
+	return 0;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example1b.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example1b.cpp
new file mode 100644
index 00000000000..102155a666c
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/examples/example1b.cpp
@@ -0,0 +1,86 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file examples/example1b.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Very simple example for testing qpOASES using the QProblemB class.
+ */
+
+
+#include <qpOASES.hpp>
+
+
+/** Example for qpOASES main function using the QProblemB class. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	/* Setup data of first QP. */
+	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
+	real_t g[2] = { 1.5, 1.0 };
+	real_t lb[2] = { 0.5, -2.0 };
+	real_t ub[2] = { 5.0, 2.0 };
+
+	/* Setup data of second QP. */
+	real_t g_new[2] = { 1.0, 1.5 };
+	real_t lb_new[2] = { 0.0, -1.0 };
+	real_t ub_new[2] = { 5.0, -0.5 };
+
+
+	/* Setting up QProblemB object. */
+	QProblemB example( 2 );
+
+	Options options;
+	//options.enableFlippingBounds = BT_FALSE;
+	options.initialStatusBounds = ST_INACTIVE;
+	options.numRefinementSteps = 1;
+	options.enableCholeskyRefactorisation = 1;
+	example.setOptions( options );
+
+	/* Solve first QP. */
+	int nWSR = 10;
+	example.init( H,g,lb,ub, nWSR,0 );
+// 	printf( "\nnWSR = %d\n\n", nWSR );
+	
+	/* Solve second QP. */
+	nWSR = 10;
+	example.hotstart( g_new,lb_new,ub_new, nWSR,0 );
+// 	printf( "\nnWSR = %d\n\n", nWSR );
+
+	/* Get and print solution of second QP. */
+	real_t xOpt[2];
+	example.getPrimalSolution( xOpt );
+	printf( "\nxOpt = [ %e, %e ];  objVal = %e\n\n", xOpt[0],xOpt[1],example.getObjVal() );
+
+	return 0;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example2.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example2.cpp
new file mode 100644
index 00000000000..a770dc70efe
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/examples/example2.cpp
@@ -0,0 +1,124 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file examples/example2.cpp
+ *	\author Hans Joachim Ferreau (thanks to Boris Houska)
+ *	\version 3.0
+ *	\date 2008-2014
+ *
+ *	Very simple example for testing qpOASES in combination
+ *  with the SolutionAnalysis class.
+ */
+
+
+
+#include <qpOASES.hpp>
+
+
+/** Example for qpOASES main function using the SolutionAnalysis class. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	/* Setup data of first QP. */
+	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
+	real_t A[1*2] = { 1.0, 1.0 };
+	real_t g[2] = { 1.5, 1.0 };
+	real_t lb[2] = { 0.5, -2.0 };
+	real_t ub[2] = { 5.0, 2.0 };
+	real_t lbA[1] = { -1.0 };
+	real_t ubA[1] = { 2.0 };
+
+	/* Setup data of second QP. */
+	real_t H_new[2*2] = { 1.0, 0.5, 0.5, 0.5 };
+	real_t A_new[1*2] = { 1.0, 5.0 };
+	real_t g_new[2] = { 1.0, 1.5 };
+	real_t lb_new[2] = { 0.0, -1.0 };
+	real_t ub_new[2] = { 5.0, -0.5 };
+	real_t lbA_new[1] = { -2.0 };
+	real_t ubA_new[1] = { 1.0 };
+
+
+	/* Setting up SQProblem object and solution analyser. */
+	SQProblem example( 2,1 );
+	SolutionAnalysis analyser;
+
+	/* Solve first QP ... */
+	int nWSR = 10;
+	example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
+
+	/* ... and analyse it. */
+	real_t maxKKTviolation;
+    analyser.getMaxKKTviolation( &example, maxKKTviolation );
+    printf( "maxKKTviolation: %e\n", maxKKTviolation );
+
+	/* Solve second QP ... */
+	nWSR = 10;
+	example.hotstart( H_new,g_new,A_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 );
+
+	/* ... and analyse it. */
+	analyser.getMaxKKTviolation( &example, maxKKTviolation );
+    printf( "maxKKTviolation: %e\n", maxKKTviolation );
+
+
+//  ------------ VARIANCE-COVARIANCE EVALUATION --------------------
+
+        real_t *Var              = new real_t[5*5];
+        real_t *Primal_Dual_Var  = new real_t[5*5];
+
+        int run1, run2;
+        for( run1 = 0; run1 < 5*5; run1++ )
+            Var[run1] = 0.0;
+
+        Var[0] = 1.0;
+        Var[6] = 1.0;
+
+//                  (  1   0   0   0   0   )
+//                  (  0   1   0   0   0   )
+//     Var     =    (  0   0   0   0   0   )
+//                  (  0   0   0   0   0   )
+//                  (  0   0   0   0   0   )
+
+
+        analyser.getVarianceCovariance( &example, Var,Primal_Dual_Var );
+
+        printf("\nPrimal_Dual_VAR = \n");
+        for( run1 = 0; run1 < 5; run1++ ){
+          for( run2 = 0; run2 < 5; run2++ ){
+            printf(" %10f", Primal_Dual_Var[run1*5+run2]);
+          }
+          printf("\n");
+        }
+
+        delete[] Primal_Dual_Var;
+        delete[] Var;
+
+	return 0;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example3.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example3.cpp
new file mode 100644
index 00000000000..8fb1cea8336
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/examples/example3.cpp
@@ -0,0 +1,88 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file examples/example3.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2008-2014
+ *
+ *	Example demonstrating usage of qpOASES for solving a QP sequence of the
+ *	Online QP Benchmark Collection. In order to run it, you have to download
+ *	"Example 02" from from http://www.qpOASES.org/onlineQP/ and store it into 
+ *	the directory bin/chain80w/.
+ */
+
+
+
+#include <qpOASES.hpp>
+
+
+/** Example for qpOASES main function using the OQP interface. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	/* 1) Define benchmark arguments. */
+	BooleanType isSparse = BT_FALSE;
+	Options options;
+	options.setToMPC();
+	options.printLevel = PL_NONE;
+
+	int nWSR = 600;
+	real_t maxCPUtime = 10.0; /* seconds */
+	real_t maxStationarity, maxFeasibility, maxComplementarity;
+
+	/* 2) Run benchmark. */
+	if ( runOQPbenchmark(	"./chain80w/",
+							isSparse,
+							options,
+							nWSR,
+							maxCPUtime,
+							maxStationarity,
+							maxFeasibility,
+							maxComplementarity
+							) != SUCCESSFUL_RETURN )
+	{
+		myPrintf( "In order to run this example, you need to download example no. 02\nfrom the Online QP Benchmark Collection website first!\n" );
+		return -1;
+	}
+
+	/* 3) Print results. */
+	printf( "\n\n" );
+	printf( "OQP Benchmark Results:\n" );
+	printf( "======================\n\n" );
+	printf( "maximum violation stationarity:     %.3e\n",maxStationarity );
+	printf( "maximum violation feasibility:      %.3e\n",maxFeasibility );
+	printf( "maximum violation complementarity:  %.3e\n",maxComplementarity );
+	printf( "\n" );
+	printf( "maximum CPU time:             %.3f milliseconds\n\n",1000.0*maxCPUtime );
+
+	return 0;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example3b.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example3b.cpp
new file mode 100644
index 00000000000..58d102250dd
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/examples/example3b.cpp
@@ -0,0 +1,88 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file examples/example3b.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2008-2014
+ *
+ *	Example demonstrating usage of qpOASES for solving a QP sequence of the
+ *	Online QP Benchmark Collection. In order to run it, you have to download
+ *	"Example 02" from http://www.qpOASES.org/onlineQP/ and store it into 
+ *	the directory bin/chain80/.
+ */
+
+
+
+#include <qpOASES.hpp>
+
+
+/** Example for qpOASES main function using the OQP interface. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	/* 1) Define benchmark arguments. */
+	BooleanType isSparse = BT_FALSE;
+	Options options;
+	options.setToMPC();
+	options.printLevel = PL_NONE;
+	
+	int nWSR = 300;
+	real_t maxCPUtime = 10.0; /* seconds */
+	real_t maxStationarity, maxFeasibility, maxComplementarity;
+
+	/* 2) Run benchmark. */
+	if ( runOQPbenchmark(	"./chain80/",
+							isSparse,
+							options,
+							nWSR,
+							maxCPUtime,
+							maxStationarity,
+							maxFeasibility,
+							maxComplementarity
+							) != SUCCESSFUL_RETURN )
+	{
+		myPrintf( "In order to run this example, you need to download example no. 02\nfrom the Online QP Benchmark Collection website first!\n" );
+		return -1;
+	}
+
+	/* 3) Print results. */
+	printf( "\n\n" );
+	printf( "OQP Benchmark Results:\n" );
+	printf( "======================\n\n" );
+	printf( "maximum violation stationarity:     %.3e\n",maxStationarity );
+	printf( "maximum violation feasibility:      %.3e\n",maxFeasibility );
+	printf( "maximum violation complementarity:  %.3e\n",maxComplementarity );
+	printf( "\n" );
+	printf( "maximum CPU time:             %.3f milliseconds\n\n",1000.0*maxCPUtime );
+
+	return 0;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example4.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example4.cpp
new file mode 100644
index 00000000000..ac716ad48dd
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/examples/example4.cpp
@@ -0,0 +1,172 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file examples/example4.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2009-2014
+ *
+ *	Very simple example for testing qpOASES (using the possibility to specify 
+ *	user-defined constraint product function).
+ */
+
+
+
+#include <stdlib.h>
+
+#include <qpOASES.hpp>
+#include "example4CP.cpp"
+
+
+/**	Example for qpOASES main function using the possibility to specify 
+ *	user-defined constraint product function. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	int i,j;
+
+	/* Setup data of first QP... */
+	real_t H[7*7];
+	real_t A[50*7];
+	real_t g[7];
+	real_t lbA[50];
+
+	/*	    ( 1.0 0.5 |                    )
+	 *	    ( 0.5 2.0 |                    )
+	 *	    ( --------+------------------- )
+	 *	H = (         | 1e-6               )
+	 *	    (         |      1e-6          )
+	 *	    (         |           ...      )
+	 *	    (         |               1e-6 ) */
+	for( i=0; i<7*7; ++i )
+		H[i] = 0.0;
+	for( i=2; i<7; ++i )
+		H[i*7+i] = 1.0e-6;
+	H[0] = 1.0;
+	H[1] = 0.5;
+	H[7] = 0.5;
+	H[8] = 2.0;
+
+	/*	    ( x.x x.x | 1.0             )
+	 *	    ( x.x x.x | ...             )
+	 *	    ( x.x x.x | 1.0             )
+	 *	    ( x.x x.x |     1.0         )
+	 *	A = ( x.x x.x |     ...         )
+	 *	    ( x.x x.x |     1.0         )
+	 *	    ( x.x x.x |         ...     )
+	 *	    ( x.x x.x |             1.0 )
+	 *	    ( x.x x.x |             ... )
+	 *	    ( x.x x.x |             1.0 ) */
+	for( i=0; i<50*7; ++i )
+		A[i] = 0.0;
+	for( i=0; i<50; ++i )
+	{
+		for( j=0; j<2; ++j )
+			A[i*7+j] = (real_t)rand() / (real_t)RAND_MAX;
+
+		A[i*7 + (i/10)+2] = 1.0;
+	}
+
+	/*	    ( -1.0 )
+	 *	    ( -0.5 )
+	 *	    ( ---- )
+	 *	g = (      )
+	 *	    (      )
+	 *	    (      )
+	 *	    (      ) */
+	for( i=0; i<7; ++i )
+		g[i] = 0.0;
+	g[0] = -1.0;
+	g[1] = -0.5;
+
+	for( i=0; i<50; ++i )
+		lbA[i] = 1.0;
+
+	/* ... and setting up user-defined constraint product function. */
+	MyConstraintProduct myCP( 7,50,A );
+
+
+	/* Setting up QProblem object and set construct product function. */
+	QProblem exampleCP( 7,50 );
+	exampleCP.setPrintLevel( PL_NONE );
+	
+	exampleCP.setConstraintProduct( &myCP );
+
+
+	/* Solve first QP. */
+	real_t cputime = 1.0;
+	int nWSR = 100;
+	exampleCP.init( H,g,A,0,0,lbA,0, nWSR,&cputime );
+
+
+	/* Solve second QP using a modified gradient. */
+	g[0] = -2.0;
+	g[1] =  0.5;
+
+	cputime = 1.0;
+	nWSR = 100;
+	exampleCP.hotstart( g,0,0,lbA,0, nWSR,&cputime );
+
+	/* Get and print solution of second QP. */
+	real_t xOpt[7];
+	exampleCP.getPrimalSolution( xOpt );
+	printf( "\nxOpt = [ %e, %e, %e ... ];  objVal = %e\n", xOpt[0],xOpt[1],xOpt[2],exampleCP.getObjVal() );
+	printf( "CPU time:  %.3f microseconds\n\n", cputime*1.0e6 ); 
+
+	
+	
+	/* Do the same without specifying constraint product. */
+	QProblem example( 7,50 );
+	example.setPrintLevel( PL_NONE );
+	
+	/* Solve first QP. */
+	g[0] = -1.0;
+	g[1] = -0.5;
+	
+	cputime = 1.0;
+	nWSR = 100;
+	example.init( H,g,A,0,0,lbA,0, nWSR,&cputime );
+	
+	/* Solve second QP using a modified gradient. */
+	g[0] = -2.0;
+	g[1] =  0.5;
+
+	cputime = 1.0;
+	nWSR = 100;
+	example.hotstart( g,0,0,lbA,0, nWSR,&cputime );
+
+	/* Get and print solution of second QP. */
+	example.getPrimalSolution( xOpt );
+	printf( "\nxOpt = [ %e, %e, %e ... ];  objVal = %e\n", xOpt[0],xOpt[1],xOpt[2],example.getObjVal() );
+	printf( "CPU time:  %.3f microseconds\n\n", cputime*1.0e6 ); 
+	
+	return 0;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example4CP.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example4CP.cpp
new file mode 100644
index 00000000000..976e4e99381
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/examples/example4CP.cpp
@@ -0,0 +1,111 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file examples/example4CP.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2009-2014
+ *
+ *	Sample implementation of the ConstraintProduct class tailored for Example4.
+ */
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** 
+ *	\brief Example illustrating the use of the \a ConstraintProduct class.
+ *
+ *	Example illustrating the use of the \a ConstraintProduct class.
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ */
+class MyConstraintProduct : public ConstraintProduct
+{
+	public:
+		/** Default constructor. */
+		MyConstraintProduct( ) {};
+
+		/** Constructor. */
+		MyConstraintProduct(	int _nV,
+								int _nC,
+								real_t* _A
+								)
+		{
+			nV = _nV;
+			nC = _nC;
+			A  = _A;
+		};
+
+		/** Copy constructor (flat copy). */
+		MyConstraintProduct(	const MyConstraintProduct& rhs
+								)
+		{
+			nV = rhs.nV;
+			nC = rhs.nC;
+			A  = rhs.A;
+		};
+
+		/** Destructor. */
+		virtual ~MyConstraintProduct( ) {};
+		
+		/** Assignment operator (flat copy). */
+		MyConstraintProduct& operator=(	const MyConstraintProduct& rhs
+										)
+		{
+			if ( this != &rhs )
+			{
+				nV = rhs.nV;
+				nC = rhs.nC;
+				A  = rhs.A;
+			}
+			else
+				return *this;
+		};
+
+		virtual int operator() (	int constrIndex,
+									const real_t* const x,
+									real_t* const constrValue
+									) const
+		{
+			constrValue[0] = 1.0 * x[(constrIndex/10)+2];
+
+			for( int i=0; i<2; ++i )
+				constrValue[0] += A[constrIndex*nV + i] * x[i];
+
+			return 0;
+		};
+
+	protected:
+		int nV;			/**< Number of variables. */
+		int nC;			/**< Number of constraints. */
+		real_t* A;		/**< Pointer to full constraint matrix (typically not needed!). */
+};
+
+
+END_NAMESPACE_QPOASES
+
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example5.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example5.cpp
new file mode 100644
index 00000000000..f5f611f67c3
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/examples/example5.cpp
@@ -0,0 +1,200 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file examples/example5.cpp
+ *	\author Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2011-2014
+ *
+ *	Very simple example for testing qpOASES (using the possibility to
+ *  compute the local linear feedback law)
+ */
+
+
+
+#include <stdlib.h>
+
+#include <qpOASES.hpp>
+#include "example4CP.cpp"
+
+
+/**	Example for qpOASES main function using the possibility to specify
+ *	user-defined constraint product function. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	int i,j,jj;
+	real_t d = 0.0;
+
+	/* Setup data of first QP... */
+	real_t H[7*7];
+	real_t A[50*7];
+	real_t g[7];
+	real_t lbA[50];
+
+	/*	    ( 1.0 0.5 |                    )
+	 *	    ( 0.5 2.0 |                    )
+	 *	    ( --------+------------------- )
+	 *	H = (         | 1e-6               )
+	 *	    (         |      1e-6          )
+	 *	    (         |           ...      )
+	 *	    (         |               1e-6 ) */
+	for( i=0; i<7*7; ++i )
+		H[i] = 0.0;
+	for( i=2; i<7; ++i )
+		H[i*7+i] = 1.0e-6;
+	H[0] = 1.0;
+	H[1] = 0.5;
+	H[7] = 0.5;
+	H[8] = 2.0;
+
+	/*	    ( x.x x.x | 1.0             )
+	 *	    ( x.x x.x | ...             )
+	 *	    ( x.x x.x | 1.0             )
+	 *	    ( x.x x.x |     1.0         )
+	 *	A = ( x.x x.x |     ...         )
+	 *	    ( x.x x.x |     1.0         )
+	 *	    ( x.x x.x |         ...     )
+	 *	    ( x.x x.x |             1.0 )
+	 *	    ( x.x x.x |             ... )
+	 *	    ( x.x x.x |             1.0 ) */
+	for( i=0; i<50*7; ++i )
+		A[i] = 0.0;
+	for( i=0; i<50; ++i )
+	{
+		for( j=0; j<2; ++j )
+			A[i*7+j] = (real_t)rand() / (real_t)RAND_MAX;
+
+		A[i*7 + (i/10)+2] = 1.0;
+	}
+
+	/*	    ( -1.0 )
+	 *	    ( -0.5 )
+	 *	    ( ---- )
+	 *	g = (      )
+	 *	    (      )
+	 *	    (      )
+	 *	    (      ) */
+	for( i=0; i<7; ++i )
+		g[i] = 0.0;
+	g[0] = -1.0;
+	g[1] = -0.5;
+
+	for( i=0; i<50; ++i )
+		lbA[i] = 1.0;
+
+	/* ... and setting up user-defined constraint product function. */
+	MyConstraintProduct myCP( 7,50,A );
+
+
+	/* Setting up QProblem object and set construct product function. */
+	QProblem example( 7,50 );
+	example.setConstraintProduct( &myCP );
+
+
+	/* Solve first QP. */
+	real_t cputime = 1.0;
+	int nWSR = 100;
+	example.init( H,g,A,0,0,lbA,0, nWSR,&cputime );
+
+	/* Get and print solution of QP. */
+	real_t xOpt[7], yOpt[7+50];
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+
+
+	/* Compute local linear feedback law */
+	const int n_rhs = 7+7+50;
+	real_t g_in[7*n_rhs];
+	real_t b_in[7*n_rhs];
+	real_t bA_in[50*n_rhs];
+	real_t x_out[7*n_rhs];
+	real_t y_out[(7+50)*n_rhs];
+
+	int ii;
+	memset (g_in, 0, sizeof (g_in));
+	memset (b_in, 0, sizeof (b_in));
+	memset (bA_in, 0, sizeof (bA_in));
+
+	for ( ii = 0; ii < 7; ++ii )
+		g_in[ii*7 + ii] = 1.0;
+	for ( ii = 0; ii < 7; ++ii )
+		b_in[(ii+7)*7 + ii] = 1.0;
+	for ( ii = 0; ii < 50; ++ii )
+		bA_in[(ii+14)*50 + ii] = 1.0;
+
+	example.solveCurrentEQP ( n_rhs, g_in, b_in, b_in, bA_in, bA_in, x_out, y_out );
+
+	/* Verify validity of local feedback law by perturbation and hot starts */
+	real_t perturb = 1.0e-6;
+	real_t nrm = 0.0;
+	for ( ii = 0; ii < n_rhs; ++ii )
+	{
+		for ( jj = 0; jj < 7; ++jj )
+			g_in[ii*7 + jj] = g[jj] + g_in[ii*7+jj]*perturb;
+		for ( jj = 0; jj < 50; ++jj )
+			bA_in[ii*50 + jj] = lbA[jj] + bA_in[ii*50+jj]*perturb;
+
+		nWSR = 100;
+		example.hotstart( &g_in[ii*7],0,0,&bA_in[ii*50],0, nWSR, 0 );
+
+		real_t xPer[7], yPer[7+50];
+		example.getPrimalSolution( xPer );
+		example.getDualSolution( yPer );
+
+		for ( jj = 0; jj < 7; ++jj )
+		{
+			d = getAbs (x_out[ii*7+jj]*perturb - (xPer[jj]-xOpt[jj]) );
+			if (nrm < d) nrm=d;
+		}
+		for ( jj = 0; jj < 7+50; ++jj )
+		{
+			d = getAbs (y_out[ii*(7+50)+jj]*perturb - (yPer[jj]-yOpt[jj]) );
+			if (nrm < d) nrm=d;
+		}
+	}
+	printf ("Maximum perturbation over all directions: %e\n", nrm);
+
+	/* // print feedback matrix
+	for (ii = 0; ii < n_rhs; ++ii)
+	{
+		printf ("x: ");
+		for (jj = 0; jj < 7; ++jj )
+			printf ("%8.2e ", x_out[ii*7+jj]);
+		printf (" y: ");
+		for (jj = 0; jj < 7+50; ++jj )
+			printf ("%8.2e ", y_out[ii*(7+50)+jj]);
+		printf("\n");
+	}
+*/
+
+	return 0;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/exampleLP.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/exampleLP.cpp
new file mode 100644
index 00000000000..b7e2f3a3c63
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/examples/exampleLP.cpp
@@ -0,0 +1,87 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file examples/exampleLP.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2008-2014
+ *
+ *	Very simple example for solving a LP sequence using qpOASES.
+ */
+
+
+
+#include <qpOASES.hpp>
+
+
+/** Example for qpOASES main function solving LPs. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	/* Setup data of first LP. */
+	real_t A[1*2] = { 1.0, 1.0 };
+	real_t g[2] = { 1.5, 1.0 };
+	real_t lb[2] = { 0.5, -2.0 };
+	real_t ub[2] = { 5.0, 2.0 };
+	real_t lbA[1] = { -1.0 };
+	real_t ubA[1] = { 2.0 };
+
+	/* Setup data of second LP. */
+	real_t g_new[2] = { 1.0, 1.5 };
+	real_t lb_new[2] = { 0.0, -1.0 };
+	real_t ub_new[2] = { 5.0, -0.5 };
+	real_t lbA_new[1] = { -2.0 };
+	real_t ubA_new[1] = { 1.0 };
+
+
+	/* Setting up QProblem object with zero Hessian matrix. */
+	QProblem example( 2,1,HST_ZERO );
+
+	Options options;
+ 	//options.setToMPC();
+	example.setOptions( options );
+
+	/* Solve first LP. */
+	int nWSR = 10;
+	example.init( 0,g,A,lb,ub,lbA,ubA, nWSR,0 );
+
+	/* Solve second LP. */
+	nWSR = 10;
+	example.hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 );
+
+
+	/* Get and print solution of second LP. */
+	real_t xOpt[2];
+	example.getPrimalSolution( xOpt );
+	printf( "\nxOpt = [ %e, %e ];  objVal = %e\n\n", xOpt[0],xOpt[1],example.getObjVal() );
+
+	return 0;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/index.html b/3rdparty/qpOASES/qpOASES-3.0/examples/index.html
new file mode 100644
index 00000000000..9b354532ede
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/examples/index.html
@@ -0,0 +1,19 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/examples</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/examples</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="Makefile">Makefile</a></li>
+  <li><a href="example1.cpp">example1.cpp</a></li>
+  <li><a href="example1a.cpp">example1a.cpp</a></li>
+  <li><a href="example1b.cpp">example1b.cpp</a></li>
+  <li><a href="example2.cpp">example2.cpp</a></li>
+  <li><a href="example3.cpp">example3.cpp</a></li>
+  <li><a href="example3b.cpp">example3b.cpp</a></li>
+  <li><a href="example4.cpp">example4.cpp</a></li>
+  <li><a href="example4CP.cpp">example4CP.cpp</a></li>
+  <li><a href="example5.cpp">example5.cpp</a></li>
+  <li><a href="exampleLP.cpp">exampleLP.cpp</a></li>
+  <li><a href="qrecipe.cpp">qrecipe.cpp</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/qrecipe.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/qrecipe.cpp
new file mode 100644
index 00000000000..1304ac739ce
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/examples/qrecipe.cpp
@@ -0,0 +1,495 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file examples/qrecipe.cpp
+ *	\author Andreas Potschka
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	QRECIPE example from the CUTEr test set with sparse matrices.
+ */
+
+
+
+#include <qpOASES.hpp>
+
+USING_NAMESPACE_QPOASES
+
+
+/** Infinity constant for qrecipe example. */
+const real_t Inf = INFTY;
+
+/** Sparse Hessian matrix data for qrecipe example. */
+sparse_int_t H_jc[] = { 0,  4,  8, 12, 16, 20, 20, 20, 20, 20, 20,
+	                   24, 28, 32, 36, 40, 40, 40, 40, 40, 40,
+					   44, 48, 52, 56, 60, 60, 60, 60, 60, 60, 60, 60, 60, 60,
+					   64, 68, 72, 76, 80, 80, 80, 80, 80, 80,
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80 };
+
+/** Sparse Hessian matrix data for qrecipe example. */
+sparse_int_t H_ir[] = {
+	0, 10, 20, 34, 1, 11, 21, 35, 2, 12, 22, 36, 3, 13, 23, 37, 4, 14, 24, 38, 
+	0, 10, 20, 34, 1, 11, 21, 35, 2, 12, 22, 36, 3, 13, 23, 37, 4, 14, 24, 38, 
+	0, 10, 20, 34, 1, 11, 21, 35, 2, 12, 22, 36, 3, 13, 23, 37, 4, 14, 24, 38, 
+	0, 10, 20, 34, 1, 11, 21, 35, 2, 12, 22, 36, 3, 13, 23, 37, 4, 14, 24, 38};
+
+/** Sparse Hessian matrix data for qrecipe example. */
+real_t H_val[] = {10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1,
+	1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 1,
+	10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 1, 10, 1,
+	1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10};
+
+/** Sparse constraint matrix data for qrecipe example. */
+sparse_int_t A_jc[] = {0, 10,  20,  30,  40,  50,  60,  70,  80,  90, 100, 110, 120,
+	130, 140, 150, 160, 170, 180, 190, 200, 210, 220, 230, 240, 250, 260, 270,
+	280, 290, 300, 301, 302, 303, 304, 305, 306, 307, 308, 309, 310, 311, 312,
+	313, 314, 318, 319, 320, 321, 322, 323, 324, 325, 326, 327, 328, 329, 331,
+	333, 335, 337, 339, 341, 343, 345, 347, 349, 351, 353, 355, 357, 359, 361,
+	363, 365, 367, 369, 371, 373, 383, 393, 403, 405, 408, 410, 413, 415, 418,
+	420, 422, 424, 426, 428, 430, 432, 434, 436, 438, 440, 442, 444, 446, 448,
+	450, 452, 454, 456, 458, 460, 462, 472, 482, 492, 494, 497, 499, 502, 504,
+	507, 509, 511, 513, 515, 517, 519, 521, 523, 525, 527, 529, 531, 533, 535,
+	537, 539, 541, 543, 545, 547, 549, 551, 561, 571, 581, 583, 586, 588, 591,
+	593, 596, 597, 598, 599, 600, 601, 602, 603, 604, 605, 606, 607, 608, 609,
+	610, 611, 612, 613, 614, 615, 616, 617, 618, 628, 638, 648, 650, 653, 655,
+	658, 660, 663};
+
+/** Sparse constraint matrix data for qrecipe example. */
+sparse_int_t A_ir[] = {0, 14, 35, 36, 71, 72, 85, 86, 87, 88, 1, 14, 35, 36, 71, 72, 85,
+	86, 87, 88, 2, 14, 35, 36, 71, 72, 85, 86, 87, 88, 3, 14, 35, 36, 71, 72,
+	85, 86, 87, 88, 4, 14, 35, 36, 71, 72, 85, 86, 87, 88, 5, 14, 35, 36, 71,
+	72, 85, 86, 87, 88, 6, 14, 35, 36, 71, 72, 85, 86, 87, 88, 7, 14, 35, 36,
+	71, 72, 85, 86, 87, 88, 8, 14, 35, 36, 71, 72, 85, 86, 87, 88, 9, 14, 35,
+	36, 71, 72, 85, 86, 87, 88, 0, 15, 37, 38, 69, 70, 79, 80, 81, 82, 1, 15,
+	37, 38, 69, 70, 79, 80, 81, 82, 2, 15, 37, 38, 69, 70, 79, 80, 81, 82, 3,
+	15, 37, 38, 69, 70, 79, 80, 81, 82, 4, 15, 37, 38, 69, 70, 79, 80, 81, 82,
+	5, 15, 37, 38, 69, 70, 79, 80, 81, 82, 6, 15, 37, 38, 69, 70, 79, 80, 81,
+	82, 7, 15, 37, 38, 69, 70, 79, 80, 81, 82, 8, 15, 37, 38, 69, 70, 79, 80,
+	81, 82, 9, 15, 37, 38, 69, 70, 79, 80, 81, 82, 0, 16, 39, 40, 67, 68, 73,
+	74, 75, 76, 1, 16, 39, 40, 67, 68, 73, 74, 75, 76, 2, 16, 39, 40, 67, 68,
+	73, 74, 75, 76, 3, 16, 39, 40, 67, 68, 73, 74, 75, 76, 4, 16, 39, 40, 67,
+	68, 73, 74, 75, 76, 5, 16, 39, 40, 67, 68, 73, 74, 75, 76, 6, 16, 39, 40,
+	67, 68, 73, 74, 75, 76, 7, 16, 39, 40, 67, 68, 73, 74, 75, 76, 8, 16, 39,
+	40, 67, 68, 73, 74, 75, 76, 9, 16, 39, 40, 67, 68, 73, 74, 75, 76, 10, 11,
+	12, 13, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 47, 48, 49, 50, 51,
+	52, 53, 54, 55, 56, 57, 47, 58, 48, 59, 49, 60, 50, 61, 51, 62, 52, 63, 53,
+	64, 54, 65, 55, 66, 46, 56, 45, 57, 47, 58, 48, 59, 49, 60, 50, 61, 51, 62,
+	52, 63, 53, 64, 54, 65, 55, 66, 46, 56, 45, 57, 10, 14, 71, 72, 85, 86, 87,
+	88, 89, 90, 11, 15, 69, 70, 79, 80, 81, 82, 83, 84, 12, 16, 67, 68, 73, 74,
+	75, 76, 77, 78, 35, 90, 36, 89, 90, 37, 84, 38, 83, 84, 39, 78, 40, 77, 78,
+	44, 58, 43, 59, 42, 60, 41, 61, 34, 62, 33, 63, 32, 64, 31, 65, 30, 66, 29,
+	46, 28, 45, 44, 58, 43, 59, 42, 60, 41, 61, 34, 62, 33, 63, 32, 64, 31, 65,
+	30, 66, 29, 46, 28, 45, 10, 14, 71, 72, 85, 86, 87, 88, 89, 90, 11, 15, 69,
+	70, 79, 80, 81, 82, 83, 84, 12, 16, 67, 68, 73, 74, 75, 76, 77, 78, 35, 90,
+	36, 89, 90, 37, 84, 38, 83, 84, 39, 78, 40, 77, 78, 27, 44, 26, 43, 25, 42,
+	24, 41, 23, 34, 22, 33, 21, 32, 20, 31, 19, 30, 18, 29, 17, 28, 27, 44, 26,
+	43, 25, 42, 24, 41, 23, 34, 22, 33, 21, 32, 20, 31, 19, 30, 18, 29, 17, 28,
+	10, 14, 71, 72, 85, 86, 87, 88, 89, 90, 11, 15, 69, 70, 79, 80, 81, 82, 83,
+	84, 12, 16, 67, 68, 73, 74, 75, 76, 77, 78, 35, 90, 36, 89, 90, 37, 84, 38,
+	83, 84, 39, 78, 40, 77, 78, 27, 26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 27,
+	26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 10, 14, 71, 72, 85, 86, 87, 88, 89,
+	90, 11, 15, 69, 70, 79, 80, 81, 82, 83, 84, 12, 16, 67, 68, 73, 74, 75, 76,
+	77, 78, 35, 90, 36, 89, 90, 37, 84, 38, 83, 84, 39, 78, 40, 77, 78};
+
+/** Sparse constraint matrix data for qrecipe example. */
+real_t A_val[] = {
+-1.0000000000000000e+00,  1.0000000000000000e+00,  8.8678200000000004e+01,
+ 9.3617050000000006e+01,  1.6000000000000000e+01,  8.1999999999999993e+00,
+ 9.9000000000000000e+01,  8.0000000000000000e+01,  1.2000000000000000e+01,
+ 9.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 8.0062830000000005e+01,  9.9224010000000007e+01,  1.0000000000000000e+02,
+ 2.1100000000000001e+01,  1.0000000000000000e+02,  1.0000000000000000e+02,
+ 1.1400000000000000e+02,  1.1680000000000000e+02, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  7.4697360000000003e+01,  8.3801220000000001e+01,
+-8.1999999999999993e+00,  2.0000000000000000e+00,  9.0000000000000000e+01,
+ 2.3999999999999999e+00, -1.2000000000000000e+01, -1.4800000000000001e+01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  7.9194209999999998e+01,
+ 9.0175110000000004e+01,  4.3000000000000000e+01,  8.0000000000000000e+00,
+ 1.0000000000000000e+02,  9.5000000000000000e+01,  9.0000000000000000e+00,
+ 2.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 7.8568219999999997e+01,  8.5996200000000002e+01, -1.2500000000000000e+01,
+ 1.0000000000000000e+00,  9.6500000000000000e+01,  4.0000000000000000e+00,
+-1.8000000000000000e+01, -2.1899999999999999e+01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  8.2922240000000002e+01,  8.6963380000000001e+01,
+ 6.5000000000000000e+01,  1.2500000000000000e+01,  1.0000000000000000e+02,
+ 9.8000000000000000e+01,  4.9000000000000000e+01,  3.7000000000000000e+01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  8.2592740000000006e+01,
+ 9.3147599999999997e+01, -1.2000000000000000e+01,  1.0000000000000000e+00,
+ 9.6500000000000000e+01,  4.0000000000000000e+00, -1.8000000000000000e+01,
+-2.1899999999999999e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 7.6506460000000004e+01,  7.8210250000000002e+01,  7.9000000000000000e+01,
+ 1.2000000000000000e+01,  1.0000000000000000e+02,  9.5000000000000000e+01,
+ 6.8000000000000000e+01,  6.1000000000000000e+01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  8.8357460000000003e+01,  9.4257840000000002e+01,
+ 1.2500000000000000e+02,  6.1299999999999997e+01,  1.0000000000000000e+02,
+ 1.0000000000000000e+02,  1.4500000000000000e+02,  1.4500000000000000e+02,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  9.0590469999999996e+01,
+ 1.0582863000000000e+02,  6.2000000000000002e+00,  6.0000000000000000e+00,
+ 9.7000000000000000e+01,  2.8500000000000000e+01,  4.0000000000000000e+00,
+ 3.6000000000000001e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 8.8678200000000004e+01,  9.3617050000000006e+01,  1.6000000000000000e+01,
+ 8.1999999999999993e+00,  9.9000000000000000e+01,  8.0000000000000000e+01,
+ 1.2000000000000000e+01,  9.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  8.0062830000000005e+01,  9.9224010000000007e+01,
+ 1.0000000000000000e+02,  2.1100000000000001e+01,  1.0000000000000000e+02,
+ 1.0000000000000000e+02,  1.1400000000000000e+02,  1.1680000000000000e+02,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  7.4697360000000003e+01,
+ 8.3801220000000001e+01, -8.1999999999999993e+00,  2.0000000000000000e+00,
+ 9.0000000000000000e+01,  2.3999999999999999e+00, -1.2000000000000000e+01,
+-1.4800000000000001e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 7.9194209999999998e+01,  9.0175110000000004e+01,  4.3000000000000000e+01,
+ 8.0000000000000000e+00,  1.0000000000000000e+02,  9.5000000000000000e+01,
+ 9.0000000000000000e+00,  2.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  7.8568219999999997e+01,  8.5996200000000002e+01,
+-1.2500000000000000e+01,  1.0000000000000000e+00,  9.6500000000000000e+01,
+ 4.0000000000000000e+00, -1.8000000000000000e+01, -2.1899999999999999e+01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  8.2922240000000002e+01,
+ 8.6963380000000001e+01,  6.5000000000000000e+01,  1.2500000000000000e+01,
+ 1.0000000000000000e+02,  9.8000000000000000e+01,  4.9000000000000000e+01,
+ 3.7000000000000000e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 8.2592740000000006e+01,  9.3147599999999997e+01, -1.2000000000000000e+01,
+ 1.0000000000000000e+00,  9.6500000000000000e+01,  4.0000000000000000e+00,
+-1.8000000000000000e+01, -2.1899999999999999e+01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  7.6506460000000004e+01,  7.8210250000000002e+01,
+ 7.9000000000000000e+01,  1.2000000000000000e+01,  1.0000000000000000e+02,
+ 9.5000000000000000e+01,  6.8000000000000000e+01,  6.1000000000000000e+01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  8.8357460000000003e+01,
+ 9.4257840000000002e+01,  1.2500000000000000e+02,  6.1299999999999997e+01,
+ 1.0000000000000000e+02,  1.0000000000000000e+02,  1.4500000000000000e+02,
+ 1.4500000000000000e+02, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 9.0590469999999996e+01,  1.0582863000000000e+02,  6.2000000000000002e+00,
+ 6.0000000000000000e+00,  9.7000000000000000e+01,  2.8500000000000000e+01,
+ 4.0000000000000000e+00,  3.6000000000000001e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  8.8678200000000004e+01,  9.3617050000000006e+01,
+ 1.6000000000000000e+01,  8.1999999999999993e+00,  9.9000000000000000e+01,
+ 8.0000000000000000e+01,  1.2000000000000000e+01,  9.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  8.0062830000000005e+01,
+ 9.9224010000000007e+01,  1.0000000000000000e+02,  2.1100000000000001e+01,
+ 1.0000000000000000e+02,  1.0000000000000000e+02,  1.1400000000000000e+02,
+ 1.1680000000000000e+02, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 7.4697360000000003e+01,  8.3801220000000001e+01, -8.1999999999999993e+00,
+ 2.0000000000000000e+00,  9.0000000000000000e+01,  2.3999999999999999e+00,
+-1.2000000000000000e+01, -1.4800000000000001e+01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  7.9194209999999998e+01,  9.0175110000000004e+01,
+ 4.3000000000000000e+01,  8.0000000000000000e+00,  1.0000000000000000e+02,
+ 9.5000000000000000e+01,  9.0000000000000000e+00,  2.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  7.8568219999999997e+01,
+ 8.5996200000000002e+01, -1.2500000000000000e+01,  1.0000000000000000e+00,
+ 9.6500000000000000e+01,  4.0000000000000000e+00, -1.8000000000000000e+01,
+-2.1899999999999999e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 8.2922240000000002e+01,  8.6963380000000001e+01,  6.5000000000000000e+01,
+ 1.2500000000000000e+01,  1.0000000000000000e+02,  9.8000000000000000e+01,
+ 4.9000000000000000e+01,  3.7000000000000000e+01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  8.2592740000000006e+01,  9.3147599999999997e+01,
+-1.2000000000000000e+01,  1.0000000000000000e+00,  9.6500000000000000e+01,
+ 4.0000000000000000e+00, -1.8000000000000000e+01, -2.1899999999999999e+01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  7.6506460000000004e+01,
+ 7.8210250000000002e+01,  7.9000000000000000e+01,  1.2000000000000000e+01,
+ 1.0000000000000000e+02,  9.5000000000000000e+01,  6.8000000000000000e+01,
+ 6.1000000000000000e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 8.8357460000000003e+01,  9.4257840000000002e+01,  1.2500000000000000e+02,
+ 6.1299999999999997e+01,  1.0000000000000000e+02,  1.0000000000000000e+02,
+ 1.4500000000000000e+02,  1.4500000000000000e+02, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  9.0590469999999996e+01,  1.0582863000000000e+02,
+ 6.2000000000000002e+00,  6.0000000000000000e+00,  9.7000000000000000e+01,
+ 2.8500000000000000e+01,  4.0000000000000000e+00,  3.6000000000000001e+00,
+-1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
+ 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
+ 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
+ 1.0000000000000000e+00,  1.0000000000000000e+00, -1.2000000000000000e-01,
+-3.8000000000000000e-01, -5.0000000000000000e-01,  1.0000000000000000e+00,
+ 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
+ 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
+ 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
+ 1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+-4.7000000000000000e+01, -8.6999999999999993e+00, -9.0000000000000000e+01,
+-5.0000000000000000e+01, -1.0000000000000000e+01, -1.0000000000000000e+01,
+-9.3000000000000000e+01, -8.9000000000000000e+01,  1.0000000000000000e+00,
+-1.0000000000000000e+00, -4.7000000000000000e+01, -8.6999999999999993e+00,
+-9.0000000000000000e+01, -5.0000000000000000e+01, -1.0000000000000000e+01,
+-1.0000000000000000e+01, -8.9000000000000000e+01, -8.5000000000000000e+01,
+ 1.0000000000000000e+00, -1.0000000000000000e+00, -4.7000000000000000e+01,
+-8.6999999999999993e+00, -9.0000000000000000e+01, -5.0000000000000000e+01,
+-1.0000000000000000e+01, -1.0000000000000000e+01, -9.1000000000000000e+01,
+-8.8000000000000000e+01, -1.0000000000000000e+00,  5.0000000000000000e-01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  5.0000000000000000e-01,
+-1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
+ 5.0000000000000000e-01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 5.0000000000000000e-01,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00, -4.7000000000000000e+01,
+-8.6999999999999993e+00, -9.0000000000000000e+01, -5.0000000000000000e+01,
+-1.0000000000000000e+01, -1.0000000000000000e+01, -9.3000000000000000e+01,
+-8.9000000000000000e+01,  1.0000000000000000e+00, -1.0000000000000000e+00,
+-4.7000000000000000e+01, -8.6999999999999993e+00, -9.0000000000000000e+01,
+-5.0000000000000000e+01, -1.0000000000000000e+01, -1.0000000000000000e+01,
+-8.9000000000000000e+01, -8.5000000000000000e+01,  1.0000000000000000e+00,
+-1.0000000000000000e+00, -4.7000000000000000e+01, -8.6999999999999993e+00,
+-9.0000000000000000e+01, -5.0000000000000000e+01, -1.0000000000000000e+01,
+-1.0000000000000000e+01, -9.1000000000000000e+01, -8.8000000000000000e+01,
+-1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
+ 5.0000000000000000e-01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 5.0000000000000000e-01, -1.0000000000000000e+00,  5.0000000000000000e-01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  5.0000000000000000e-01,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00, -4.7000000000000000e+01, -8.6999999999999993e+00,
+-9.0000000000000000e+01, -5.0000000000000000e+01, -1.0000000000000000e+01,
+-1.0000000000000000e+01, -9.3000000000000000e+01, -8.9000000000000000e+01,
+ 1.0000000000000000e+00, -1.0000000000000000e+00, -4.7000000000000000e+01,
+-8.6999999999999993e+00, -9.0000000000000000e+01, -5.0000000000000000e+01,
+-1.0000000000000000e+01, -1.0000000000000000e+01, -8.9000000000000000e+01,
+-8.5000000000000000e+01,  1.0000000000000000e+00, -1.0000000000000000e+00,
+-4.7000000000000000e+01, -8.6999999999999993e+00, -9.0000000000000000e+01,
+-5.0000000000000000e+01, -1.0000000000000000e+01, -1.0000000000000000e+01,
+-9.1000000000000000e+01, -8.8000000000000000e+01, -1.0000000000000000e+00,
+ 5.0000000000000000e-01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 5.0000000000000000e-01, -1.0000000000000000e+00,  5.0000000000000000e-01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  5.0000000000000000e-01,
+-1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
+-1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
+-1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
+-1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
+-1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
+-1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
+-1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
+-1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00, -4.7000000000000000e+01,
+-8.6999999999999993e+00, -9.0000000000000000e+01, -5.0000000000000000e+01,
+-1.0000000000000000e+01, -1.0000000000000000e+01, -9.3000000000000000e+01,
+-8.9000000000000000e+01,  1.0000000000000000e+00, -1.0000000000000000e+00,
+-4.7000000000000000e+01, -8.6999999999999993e+00, -9.0000000000000000e+01,
+-5.0000000000000000e+01, -1.0000000000000000e+01, -1.0000000000000000e+01,
+-8.9000000000000000e+01, -8.5000000000000000e+01,  1.0000000000000000e+00,
+-1.0000000000000000e+00, -4.7000000000000000e+01, -8.6999999999999993e+00,
+-9.0000000000000000e+01, -5.0000000000000000e+01, -1.0000000000000000e+01,
+-1.0000000000000000e+01, -9.1000000000000000e+01, -8.8000000000000000e+01,
+-1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
+ 5.0000000000000000e-01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 5.0000000000000000e-01, -1.0000000000000000e+00,  5.0000000000000000e-01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  5.0000000000000000e-01};
+
+/** Gradient vector for qrecipe example. */
+real_t g[] = {+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
+	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
+	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
+	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
+	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
+	+0e+00, +0e+00, -2e+00, -2e+00, -2e+00, -2e+00, -2e+00, -2e+00, -2e+00,
+	-2e+00, +0e+00, -2e+00, +0e+00, +2e-03, +2e-03, +2e-03, +2e-03, +2e-03,
+	+2e-03, +1e-03, +2e-03, +2e-03, +2e-03, +0e+00, -2e-03, -2e-03, -2e-03,
+	-2e-03, -2e-03, -2e-03, -1e-03, -2e-03, -2e-03, -2e-03, +0e+00, +0e+00,
+	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +2e-03,
+	+2e-03, +2e-03, +2e-03, +2e-03, +2e-03, +1e-03, +2e-03, +2e-03, +2e-03,
+	+0e+00, -2e-03, -2e-03, -2e-03, -2e-03, -2e-03, -2e-03, -1e-03, -2e-03,
+	-2e-03, -2e-03, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
+	+0e+00, +0e+00, +0e+00, +2e-03, +2e-03, +2e-03, +2e-03, +2e-03, +2e-03,
+	+1e-03, +2e-03, +2e-03, +2e-03, +0e+00, -2e-03, -2e-03, -2e-03, -2e-03,
+	-2e-03, -2e-03, -1e-03, -2e-03, -2e-03, -2e-03, +0e+00, +0e+00, +0e+00,
+	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +1e-01, +1e-01,
+	+1e-01, +1e-01, +1e-01, +1e-01, +1e-01, +1e-01, +1e-01, +1e-01, +0e+00,
+	-1e-01, -1e-01, -1e-01, -1e-01, -1e-01, -1e-01, -1e-01, -1e-01, -1e-01,
+	-1e-01, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
+	+0e+00};
+
+/** Lower bound vector for qrecipe example. */
+real_t lb[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, -Inf, 0, -Inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 5,
+	10, 5, 0, 10, 0, 2, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 10, 5, 10, 5, 0, 10, 0, 5, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 5, 10, 5, 0, 10, 0, 5, 0, 10, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+/** Upper bound vector for qrecipe example. */
+real_t ub[] = {Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
+	Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
+	Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
+	Inf, Inf, 0, 92, 39, 87, 29, 0, 20, 0, 28, 20, 71, Inf, 130, 45, 53, 55, 75,
+	112, 0, 73, 480, 154, 121, 50, 30, 77, 20, 0, 18, 0, 5, 20, 71, Inf, Inf,
+	Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, 130, 55, 93, 60, 75, 115, 0, 67,
+	480, 154, 121, 50, 20, 37, 15, 0, 15, 0, 8, 20, 71, Inf, Inf, Inf, Inf, Inf,
+	Inf, Inf, Inf, Inf, Inf, 130, 55, 93, 60, 75, 105, 0, 67, 4980, 154, 110,
+	50, 20, 37, 15, 0, 25, 0, 8, 20, 71, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
+	Inf, Inf, 20, 20, 20, 20, 0, 20, 0, 20, 0, 20, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf};
+
+/** Lower constraint bound vector for qrecipe example. */
+real_t lbA[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -Inf, -Inf,
+	-Inf, -Inf, -Inf, -Inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0};
+
+/** Upper constraint bound vector for qrecipe example. */
+real_t ubA[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
+	Inf, Inf, Inf, Inf};
+
+/** Number of non-zeros in Hessian matrix for qrecipe example. */
+long H_nnz =  sizeof(H_val) / sizeof(real_t);
+
+/** Number of non-zeros in constraint matrix for qrecipe example. */
+long A_nnz =  sizeof(A_val) / sizeof(real_t);
+
+
+/** Example for calling qpOASES with sparse matrices. */
+int main( )
+{
+	long i;
+	int nWSR;
+	real_t err, tic, toc;
+	real_t *x1 = new real_t[180];
+	real_t *y1 = new real_t[271];
+	real_t *x2 = new real_t[180];
+	real_t *y2 = new real_t[271];
+
+	/* create sparse matrices */
+	SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val);
+	SparseMatrix *A = new SparseMatrix(91, 180, A_ir, A_jc, A_val);
+
+	H->createDiagInfo();
+
+	real_t* H_full = H->full();
+	real_t* A_full = A->full();
+
+	SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full);
+	DenseMatrix *Ad = new DenseMatrix(91,180,180,A_full);
+
+	/* solve with dense matrices */
+	nWSR = 1000;
+	QProblem qrecipeD(180, 91);
+	tic = getCPUtime();
+	qrecipeD.init(Hd, g, Ad, lb, ub, lbA, ubA, nWSR, 0);
+	toc = getCPUtime();
+	qrecipeD.getPrimalSolution(x1);
+	qrecipeD.getDualSolution(y1);
+
+	fprintf(stdFile, "Solved dense problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic);
+
+	/* solve with sparse matrices */
+	nWSR = 1000;
+	QProblem qrecipeS(180, 91);
+	tic = getCPUtime();
+	qrecipeS.init(H, g, A, lb, ub, lbA, ubA, nWSR, 0);
+	toc = getCPUtime();
+	qrecipeS.getPrimalSolution(x2);
+	qrecipeS.getDualSolution(y2);
+
+	fprintf(stdFile, "Solved sparse problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic);
+
+	/* check distance of solutions */
+	err = 0.0;
+	for (i = 0; i < 180; i++)
+		if (getAbs(x1[i] - x2[i]) > err)
+			err = getAbs(x1[i] - x2[i]);
+	fprintf(stdFile, "Primal error: %9.2e\n", err);
+	err = 0.0;
+	for (i = 0; i < 271; i++)
+		if (getAbs(y1[i] - y2[i]) > err)
+			err = getAbs(y1[i] - y2[i]);
+	fprintf(stdFile, "Dual error: %9.2e  (might not be unique)\n", err);
+
+	delete H;
+	delete A;
+	delete[] H_full;
+	delete[] A_full;
+	delete Hd;
+	delete Ad;
+
+	delete[] y2;
+	delete[] x2;
+	delete[] y1;
+	delete[] x1;
+
+	return 0;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/index.html b/3rdparty/qpOASES/qpOASES-3.0/include/index.html
new file mode 100644
index 00000000000..77dd423600b
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/index.html
@@ -0,0 +1,9 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/include</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/include</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="qpOASES/">qpOASES/</a></li>
+  <li><a href="qpOASES.hpp">qpOASES.hpp</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES.hpp
new file mode 100644
index 00000000000..4570d6031c1
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES.hpp
@@ -0,0 +1,59 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ */
+
+#ifndef __SINGLE_OBJECT__
+
+#include <qpOASES/QProblemB.hpp>
+#include <qpOASES/QProblem.hpp>
+#include <qpOASES/SQProblem.hpp>
+#include <qpOASES/extras/OQPinterface.hpp>
+#include <qpOASES/extras/SolutionAnalysis.hpp>
+
+#else
+
+#include <MessageHandling.cpp>
+#include <Utils.cpp>
+#include <Indexlist.cpp>
+#include <SubjectTo.cpp>
+#include <Bounds.cpp>
+#include <Constraints.cpp>
+#include <BLASReplacement.cpp>
+#include <LAPACKReplacement.cpp>
+#include <Matrices.cpp>
+#include <Options.cpp>
+#include <QProblemB.cpp>
+#include <Flipper.cpp>
+#include <QProblem.cpp>
+#include <SQProblem.cpp>
+#include <OQPinterface.cpp>
+#include <SolutionAnalysis.cpp>
+
+#endif
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Bounds.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Bounds.hpp
new file mode 100644
index 00000000000..c1ea151e804
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Bounds.hpp
@@ -0,0 +1,256 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/Bounds.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Declaration of the Bounds class designed to manage working sets of
+ *	bounds within a QProblem.
+ */
+
+
+#ifndef QPOASES_BOUNDS_HPP
+#define QPOASES_BOUNDS_HPP
+
+
+#include <qpOASES/SubjectTo.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** 
+ *	\brief Manages working sets of bounds (= box constraints).
+ *
+ *	This class manages working sets of bounds (= box constraints) 
+ *	by storing index sets and other status information.
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ */
+class Bounds : public SubjectTo
+{
+	/*
+	 *	PUBLIC MEMBER FUNCTIONS
+	 */
+	public:
+		/** Default constructor. */
+		Bounds( );
+
+		/** Constructor which takes the number of bounds. */
+		Bounds(	int _n									/**< Number of bounds. */
+				);
+
+		/** Copy constructor (deep copy). */
+		Bounds(	const Bounds& rhs						/**< Rhs object. */
+				);
+
+		/** Destructor. */
+		virtual ~Bounds( );
+
+		/** Assignment operator (deep copy). */
+		Bounds& operator=(	const Bounds& rhs			/**< Rhs object. */
+							);
+
+
+		/** Initialises object with given number of bounds.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_INVALID_ARGUMENTS */
+		returnValue init(	int _n = 0					/**< Number of bounds. */
+							);
+
+
+		/** Initially adds number of a new (i.e. not yet in the list) bound to
+		 *  given index set.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_SETUP_BOUND_FAILED \n
+					RET_INDEX_OUT_OF_BOUNDS \n
+					RET_INVALID_ARGUMENTS */
+		returnValue setupBound(	int number,				/**< Number of new bound. */
+								SubjectToStatus _status	/**< Status of new bound. */
+								);
+
+		/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
+		 *  to the index set of free bounds; the order depends on the SujectToType
+		 *  of each index.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_SETUP_BOUND_FAILED */
+		returnValue setupAllFree( );
+
+		/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
+		 *  to the index set of fixed bounds (on their lower bounds);
+		 *  the order depends on the SujectToType of each index.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_SETUP_BOUND_FAILED */
+		returnValue setupAllLower( );
+
+		/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
+		 *  to the index set of fixed bounds (on their upper bounds);
+		 *  the order depends on the SujectToType of each index.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_SETUP_BOUND_FAILED */
+		returnValue setupAllUpper( );
+
+
+		/** Moves index of a bound from index list of fixed to that of free bounds.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MOVING_BOUND_FAILED \n
+					RET_INDEX_OUT_OF_BOUNDS */
+		returnValue moveFixedToFree(	int number				/**< Number of bound to be freed. */
+										);
+
+		/** Moves index of a bound from index list of free to that of fixed bounds.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MOVING_BOUND_FAILED \n
+					RET_INDEX_OUT_OF_BOUNDS */
+		returnValue moveFreeToFixed(	int number,				/**< Number of bound to be fixed. */
+										SubjectToStatus _status	/**< Status of bound to be fixed. */
+										);
+
+		/** Flip fixed bound.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MOVING_BOUND_FAILED \n
+					RET_INDEX_OUT_OF_BOUNDS */
+		returnValue flipFixed( int number );
+
+		/** Swaps the indices of two free bounds within the index set.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_SWAPINDEX_FAILED */
+		returnValue swapFree(	int number1,					/**< Number of first constraint or bound. */
+								int number2						/**< Number of second constraint or bound. */
+								);
+
+
+		/** Returns number of variables.
+		 *	\return Number of variables. */
+		inline int getNV( ) const;
+
+		/** Returns number of implicitly fixed variables.
+		 *	\return Number of implicitly fixed variables. */
+		inline int getNFV( ) const;
+
+		/** Returns number of bounded (but possibly free) variables.
+		 *	\return Number of bounded (but possibly free) variables. */
+		inline int getNBV( ) const;
+
+		/** Returns number of unbounded variables.
+		 *	\return Number of unbounded variables. */
+		inline int getNUV( ) const;
+
+		/** Returns number of free variables.
+		 *	\return Number of free variables. */
+		inline int getNFR( ) const;
+
+		/** Returns number of fixed variables.
+		 *	\return Number of fixed variables. */
+		inline int getNFX( ) const;
+
+
+		/** Returns a pointer to free variables index list.
+		 *	\return Pointer to free variables index list. */
+		inline Indexlist* getFree( );
+
+		/** Returns a pointer to fixed variables index list.
+		 *	\return Pointer to fixed variables index list. */
+		inline Indexlist* getFixed( );
+
+
+		/** Shifts forward type and status of all bounds by a given
+		 *  offset. This offset has to lie within the range [0,n/2] and has to
+		 *  be an integer divisor of the total number of bounds n.
+		 *  Type and status of the first \<offset\> bounds is thrown away,
+		 *  type and status of the last \<offset\> bounds is doubled,
+		 *  e.g. for offset = 2: \n
+		 *  shift( {b1,b2,b3,b4,b5,b6} ) = {b3,b4,b5,b6,b5,b6}
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_INDEX_OUT_OF_BOUNDS \n
+		 			RET_INVALID_ARGUMENTS \n
+		 			RET_SHIFTING_FAILED */
+		virtual returnValue shift(	int offset		/**< Shift offset within the range [0,n/2] and integer divisor of n. */
+									);
+
+		/** Rotates forward type and status of all bounds by a given
+		 *  offset. This offset has to lie within the range [0,n].
+		 *  Example for offset = 2: \n
+		 *  rotate( {b1,b2,b3,b4,b5,b6} ) = {b3,b4,b5,b6,b1,b2}
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_INDEX_OUT_OF_BOUNDS \n
+		 			RET_ROTATING_FAILED */
+		virtual returnValue rotate(	int offset		/**< Rotation offset within the range [0,n]. */
+									);
+
+
+		/** Prints information on bounds object
+		 *  (in particular, lists of free and fixed bounds.
+		 * \return SUCCESSFUL_RETURN \n
+				   RET_INDEXLIST_CORRUPTED */
+		returnValue print( );
+
+
+	/*
+	 *	PROTECTED MEMBER FUNCTIONS
+	 */
+	protected:
+		/** Frees all allocated memory.
+		 *  \return SUCCESSFUL_RETURN */
+		returnValue clear( );
+		
+		/** Copies all members from given rhs object.
+		 *  \return SUCCESSFUL_RETURN */
+		returnValue copy(	const Bounds& rhs	/**< Rhs object. */
+							);
+
+
+		/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
+		 *  to the index set corresponding to the desired status;
+		 *  the order depends on the SujectToType of each index.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_SETUP_BOUND_FAILED */
+		returnValue setupAll(	SubjectToStatus _status	/**< Desired initial status for all bounds. */
+								);
+
+
+	/*
+	 *	PROTECTED MEMBER VARIABLES
+	 */
+	protected:
+		Indexlist freee;		/**< Index list of free variables. */
+		Indexlist fixed;		/**< Index list of fixed variables. */
+};
+
+
+END_NAMESPACE_QPOASES
+
+#include <qpOASES/Bounds.ipp>
+
+#endif	/* QPOASES_BOUNDS_HPP */
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Bounds.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Bounds.ipp
new file mode 100644
index 00000000000..0efdccf73a9
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Bounds.ipp
@@ -0,0 +1,120 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/Bounds.ipp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of inlined member functions of the Bounds class designed
+ *	to manage working sets of bounds within a QProblem.
+ */
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+/*
+ *	g e t N V
+ */
+inline int Bounds::getNV( ) const
+{
+ 	return n;
+}
+
+
+/*
+ *	g e t N F V
+ */
+inline int Bounds::getNFV( ) const
+{
+ 	return getNumberOfType( ST_EQUALITY );
+}
+
+
+/*
+ *	g e t N B V
+ */
+inline int Bounds::getNBV( ) const
+{
+ 	return getNumberOfType( ST_BOUNDED );
+}
+
+
+/*
+ *	g e t N U V
+ */
+inline int Bounds::getNUV( ) const
+{
+	return getNumberOfType( ST_UNBOUNDED );
+}
+
+
+/*
+ *	g e t N F R
+ */
+inline int Bounds::getNFR( ) const
+{
+ 	return freee.getLength( );
+}
+
+
+/*
+ *	g e t N F X
+ */
+inline int Bounds::getNFX( ) const
+{
+	return fixed.getLength( );
+}
+
+
+/*
+ *	g e t F r e e
+ */
+inline Indexlist* Bounds::getFree( )
+{
+	return &freee;
+}
+
+
+/*
+ *	g e t F i x e d
+ */
+inline Indexlist* Bounds::getFixed( )
+{
+	return &fixed;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constants.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constants.hpp
new file mode 100644
index 00000000000..e2ba4e7edca
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constants.hpp
@@ -0,0 +1,77 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/Constants.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Definition of all global constants.
+ */
+
+
+#ifndef QPOASES_CONSTANTS_HPP
+#define QPOASES_CONSTANTS_HPP
+
+
+#include <qpOASES/Types.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** Numerical value of machine precision (min eps, s.t. 1+eps > 1).
+ *	Note: this value has to be positive! */
+#ifdef __USE_SINGLE_PRECISION__
+const real_t EPS = 1.193e-07;
+#else
+const real_t EPS = 2.221e-16;
+#endif /* __USE_SINGLE_PRECISION__ */
+
+
+/** Numerical value of zero (for situations in which it would be
+ *	unreasonable to compare with 0.0).
+ *	Note: this value has to be positive! */
+const real_t ZERO = 1.0e-25;
+
+/** Numerical value of infinity (e.g. for non-existing bounds).
+	Note: this value has to be positive! */
+const real_t INFTY = 1.0e20;
+
+
+/** Maximum number of characters within a string.
+ *	Note: this value should be at least 41! */
+const unsigned int MAX_STRING_LENGTH = 160;
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_CONSTANTS_HPP */
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/ConstraintProduct.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/ConstraintProduct.hpp
new file mode 100644
index 00000000000..929f6c91556
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/ConstraintProduct.hpp
@@ -0,0 +1,91 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/ConstraintProduct.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2009-2014
+ *
+ *	Declaration of the ConstraintProduct class which allows to specify a
+ *	user-defined function for evaluating the constraint product at the 
+ *	current iterate to speed-up QP solution in case of a specially structured
+ *	constraint matrix.
+ */
+
+
+
+#ifndef QPOASES_CONSTRAINT_PRODUCT_HPP
+#define QPOASES_CONSTRAINT_PRODUCT_HPP
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** 
+ *	\brief Interface for specifying user-defined evaluations of constraint products.
+ *
+ *	A class which allows to specify a user-defined function for evaluating the 
+ *	constraint product at the current iterate to speed-up QP solution in case 
+ *	of a specially structured constraint matrix.
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2009-2014
+ */
+class ConstraintProduct
+{
+	public:
+		/** Default constructor. */
+		ConstraintProduct( ) {};
+
+		/** Copy constructor. */
+		ConstraintProduct(	const ConstraintProduct &toCopy	/**< Rhs object. */
+							) {};
+
+		/** Destructor. */
+		virtual ~ConstraintProduct( ) {};
+		
+		/** Assignment operator. */
+		ConstraintProduct &operator=(	const ConstraintProduct &toCopy	/**< Rhs object. */
+										)
+		{
+			return *this;
+		}
+
+		/** Evaluates the product of a given constraint with the current iterate.
+		 *	This function needs to be implemented in a derived class for the 
+		 *	user-defined constraint product function.
+		 *	\return 0:         successful \n
+					otherwise: not successful */
+		virtual int operator() (	int constrIndex,			/**< Number of constraint to be evaluated. */
+									const real_t* const x,		/**< Array containing current primal iterate. */
+									real_t* const constrValue	/**< Output: Scalar value of the evaluated constraint. */
+									) const = 0;
+};
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_CONSTRAINT_PRODUCT_HPP */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constraints.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constraints.hpp
new file mode 100644
index 00000000000..8d4556ac720
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constraints.hpp
@@ -0,0 +1,246 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/Constraints.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Declaration of the Constraints class designed to manage working sets of
+ *	constraints within a QProblem.
+ */
+
+
+#ifndef QPOASES_CONSTRAINTS_HPP
+#define QPOASES_CONSTRAINTS_HPP
+
+
+#include <qpOASES/SubjectTo.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** 
+ *	\brief Manages working sets of constraints.
+ *
+ *	This class manages working sets of constraints by storing
+ *	index sets and other status information.
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ */
+class Constraints : public SubjectTo
+{
+	/*
+	 *	PUBLIC MEMBER FUNCTIONS
+	 */
+	public:
+		/** Default constructor. */
+		Constraints( );
+
+		/** Constructor which takes the number of constraints. */
+		Constraints(	int _n							/**< Number of constraints. */
+						);
+
+		/** Copy constructor (deep copy). */
+		Constraints(	const Constraints& rhs			/**< Rhs object. */
+						);
+
+		/** Destructor. */
+		virtual ~Constraints( );
+
+		/** Assignment operator (deep copy). */
+		Constraints& operator=(	const Constraints& rhs	/**< Rhs object. */
+								);
+
+
+		/** Initialises object with given number of constraints.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_INVALID_ARGUMENTS */
+		returnValue init(	int _n = 0					/**< Number of constraints. */
+							);
+
+
+		/** Initially adds number of a new (i.e. not yet in the list) constraint to
+		 *  a given index set.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_SETUP_CONSTRAINT_FAILED \n
+					RET_INDEX_OUT_OF_BOUNDS \n
+					RET_INVALID_ARGUMENTS */
+		returnValue setupConstraint(	int number,				/**< Number of new constraint. */
+										SubjectToStatus _status	/**< Status of new constraint. */
+										);
+
+		/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to
+		 *  to the index set of inactive constraints; the order depends on the SujectToType
+		 *  of each index. Only disabled constraints are added to index set of disabled constraints!
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_SETUP_CONSTRAINT_FAILED */
+		returnValue setupAllInactive( );
+
+		/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to
+		 *  to the index set of active constraints (on their lower bounds); the order depends on the SujectToType
+		 *  of each index. Only disabled constraints are added to index set of disabled constraints!
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_SETUP_CONSTRAINT_FAILED */
+		returnValue setupAllLower( );
+
+		/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to
+		 *  to the index set of active constraints (on their upper bounds); the order depends on the SujectToType
+		 *  of each index. Only disabled constraints are added to index set of disabled constraints!
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_SETUP_CONSTRAINT_FAILED */
+		returnValue setupAllUpper( );
+
+
+		/** Moves index of a constraint from index list of active to that of inactive constraints.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MOVING_CONSTRAINT_FAILED */
+		returnValue moveActiveToInactive(	int number				/**< Number of constraint to become inactive. */
+											);
+
+		/** Moves index of a constraint from index list of inactive to that of active constraints.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MOVING_CONSTRAINT_FAILED */
+		returnValue moveInactiveToActive(	int number,				/**< Number of constraint to become active. */
+											SubjectToStatus _status	/**< Status of constraint to become active. */
+											);
+
+		/** Flip fixed constraint.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MOVING_CONSTRAINT_FAILED \n
+					RET_INDEX_OUT_OF_BOUNDS */
+		returnValue flipFixed( int number );
+
+
+		/** Returns the number of constraints.
+		 *	\return Number of constraints. */
+		inline int getNC( ) const;
+
+		/** Returns the number of implicit equality constraints.
+		 *	\return Number of implicit equality constraints. */
+		inline int getNEC( ) const;
+
+		/** Returns the number of "real" inequality constraints.
+		 *	\return Number of "real" inequality constraints. */
+		inline int getNIC( ) const;
+
+		/** Returns the number of unbounded constraints (i.e. without any bounds).
+		 *	\return Number of unbounded constraints (i.e. without any bounds). */
+		inline int getNUC( ) const;
+
+		/** Returns the number of active constraints.
+		 *	\return Number of active constraints. */
+		inline int getNAC( ) const;
+
+		/** Returns the number of inactive constraints.
+		 *	\return Number of inactive constraints. */
+		inline int getNIAC( ) const;
+
+
+		/** Returns a pointer to active constraints index list.
+		 *	\return Pointer to active constraints index list. */
+		inline Indexlist* getActive( );
+
+		/** Returns a pointer to inactive constraints index list.
+		 *	\return Pointer to inactive constraints index list. */
+		inline Indexlist* getInactive( );
+
+
+		/** Shifts forward type and status of all constraints by a given
+		 *  offset. This offset has to lie within the range [0,n/2] and has to
+		 *  be an integer divisor of the total number of constraints n.
+		 *  Type and status of the first \<offset\> constraints  is thrown away,
+		 *  type and status of the last \<offset\> constraints is doubled,
+		 *  e.g. for offset = 2: \n
+		 *  shift( {c1,c2,c3,c4,c5,c6} ) = {c3,c4,c5,c6,c5,c6}
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_INDEX_OUT_OF_BOUNDS \n
+		 			RET_INVALID_ARGUMENTS \n
+		 			RET_SHIFTING_FAILED */
+		virtual returnValue shift(	int offset		/**< Shift offset within the range [0,n/2] and integer divisor of n. */
+									);
+
+		/** Rotates forward type and status of all constraints by a given
+		 *  offset. This offset has to lie within the range [0,n].
+		 *  Example for offset = 2: \n
+		 *  rotate( {c1,c2,c3,c4,c5,c6} ) = {c3,c4,c5,c6,c1,c2}
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_INDEX_OUT_OF_BOUNDS \n
+		 			RET_ROTATING_FAILED */
+		virtual returnValue rotate(	int offset		/**< Rotation offset within the range [0,n]. */
+									);
+
+
+		/** Prints information on constraints object
+		 *  (in particular, lists of inactive and active constraints.
+		 * \return SUCCESSFUL_RETURN \n
+				   RET_INDEXLIST_CORRUPTED */
+		returnValue print( );
+
+
+	/*
+	 *	PROTECTED MEMBER FUNCTIONS
+	 */
+	protected:
+		/** Frees all allocated memory.
+		 *  \return SUCCESSFUL_RETURN */
+		returnValue clear( );
+		
+		/** Copies all members from given rhs object.
+		 *  \return SUCCESSFUL_RETURN */
+		returnValue copy(	const Constraints& rhs	/**< Rhs object. */
+							);
+
+
+		/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
+		 *  to the index set corresponding to the desired status;
+		 *  the order depends on the SujectToType of each index.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_SETUP_CONSTRAINT_FAILED */
+		returnValue setupAll(	SubjectToStatus _status	/**< Desired initial status for all bounds. */
+								);
+
+
+	/*
+	 *	PROTECTED MEMBER VARIABLES
+	 */
+	protected:
+		Indexlist active;		/**< Index list of active constraints. */
+		Indexlist inactive;		/**< Index list of inactive constraints. */
+};
+
+END_NAMESPACE_QPOASES
+
+#include <qpOASES/Constraints.ipp>
+
+#endif	/* QPOASES_CONSTRAINTS_HPP */
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constraints.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constraints.ipp
new file mode 100644
index 00000000000..b58e02b2516
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constraints.ipp
@@ -0,0 +1,122 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/Constraints.ipp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Declaration of inlined member functions of the Constraints class designed
+ *	to manage working sets of constraints within a QProblem.
+ */
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+/*
+ *	g e t N C
+ */
+inline int Constraints::getNC( ) const
+{
+ 	return n;
+}
+
+
+/*
+ *	g e t N E C
+ */
+inline int Constraints::getNEC( ) const
+{
+	return getNumberOfType( ST_EQUALITY );
+}
+
+
+/*
+ *	g e t N I C
+ */
+inline int Constraints::getNIC( ) const
+{
+ 	return getNumberOfType( ST_BOUNDED );
+}
+
+
+/*
+ *	g e t N U C
+ */
+inline int Constraints::getNUC( ) const
+{
+ 	return getNumberOfType( ST_UNBOUNDED );
+}
+
+
+/*
+ *	g e t N A C
+ */
+inline int Constraints::getNAC( ) const
+{
+ 	return active.getLength( );
+}
+
+
+/*
+ *	g e t N I A C
+ */
+inline int Constraints::getNIAC( ) const
+{
+	return inactive.getLength( );
+}
+
+
+
+/*
+ *	g e t A c t i v e
+ */
+inline Indexlist* Constraints::getActive( )
+{
+	return &active;
+}
+
+
+/*
+ *	g e t I n a c t i v e
+ */
+inline Indexlist* Constraints::getInactive( )
+{
+	return &inactive;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Flipper.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Flipper.hpp
new file mode 100644
index 00000000000..4e6f215d681
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Flipper.hpp
@@ -0,0 +1,155 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/Flipper.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Declaration of the Options class designed to manage user-specified
+ *	options for solving a QProblem.
+ */
+
+
+#ifndef QPOASES_FLIPPER_HPP
+#define QPOASES_FLIPPER_HPP
+
+
+#include <qpOASES/Bounds.hpp>
+#include <qpOASES/Constraints.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** 
+ *	\brief Auxiliary class for storing a copy of the current matrix factorisations.
+ *
+ *	This auxiliary class stores a copy of the current matrix factorisations. It
+ *	is used by the classe QProblemB and QProblem in case flipping bounds are enabled.
+ *
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ */
+class Flipper
+{
+	friend class QProblemB;
+	friend class QProblem;
+
+	/*
+	 *	PUBLIC MEMBER FUNCTIONS
+	 */
+	public:
+		/** Default constructor. */
+		Flipper( );
+
+		/** Constructor which takes the number of bounds and constraints. */
+		Flipper(	unsigned int _nV,		/**< Number of bounds. */
+					unsigned int _nC = 0		/**< Number of constraints. */
+					);
+
+		/** Copy constructor (deep copy). */
+		Flipper(	const Flipper& rhs			/**< Rhs object. */
+					);
+
+		/** Destructor. */
+		~Flipper( );
+
+		/** Assignment operator (deep copy). */
+		Flipper& operator=(	const Flipper& rhs	/**< Rhs object. */
+							);
+
+
+		/** Initialises object with given number of bounds and constraints.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_INVALID_ARGUMENTS */
+		returnValue init(	unsigned int _nV = 0,	/**< Number of bounds. */
+							unsigned int _nC = 0		/**< Number of constraints. */
+							);
+
+
+		/** Copies current values to non-null arguments (assumed to be allocated with consistent size).
+		 *	\return SUCCESSFUL_RETURN */
+		returnValue get(	Bounds* const _bounds,					/**< Pointer to new bounds. */
+							real_t* const R,						/**< New matrix R. */
+							Constraints* const _constraints = 0,	/**< Pointer to new constraints. */
+							real_t* const _Q = 0,					/**< New matrix Q. */
+							real_t* const _T = 0					/**< New matrix T. */
+							) const;
+
+		/** Assigns new values to non-null arguments.
+		 *	\return SUCCESSFUL_RETURN */
+		returnValue set(	const Bounds* const _bounds,				/**< Pointer to new bounds. */
+							const real_t* const _R,						/**< New matrix R. */
+							const Constraints* const _constraints = 0,	/**< Pointer to new constraints. */
+							const real_t* const _Q = 0,					/**< New matrix Q. */
+							const real_t* const _T = 0					/**< New matrix T. */
+							);
+
+
+	/*
+	 *	PROTECTED MEMBER FUNCTIONS
+	 */
+	protected:
+		/** Frees all allocated memory.
+		 *  \return SUCCESSFUL_RETURN */
+		returnValue clear( );
+		
+		/** Copies all members from given rhs object.
+		 *  \return SUCCESSFUL_RETURN */
+		returnValue copy(	const Flipper& rhs	/**< Rhs object. */
+							);
+
+		/** Returns dimension of matrix T.
+		 *  \return Dimension of matrix T. */
+		unsigned int getDimT( ) const;
+
+
+	/*
+	 *	PROTECTED MEMBER VARIABLES
+	 */
+	protected:
+		unsigned int nV;				/**< Number of variables. */
+		unsigned int nC;				/**< Number of constraints. */
+
+		Bounds      bounds;				/**< Data structure for problem's bounds. */
+		Constraints constraints;		/**< Data structure for problem's constraints. */
+
+		real_t* R;						/**< Cholesky factor of H (i.e. H = R^T*R). */
+		real_t* Q;						/**< Orthonormal quadratic matrix, A = [0 T]*Q'. */
+		real_t* T;						/**< Reverse triangular matrix, A = [0 T]*Q'. */
+};
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_FLIPPER_HPP */
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Indexlist.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Indexlist.hpp
new file mode 100644
index 00000000000..4a0e40d7fd5
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Indexlist.hpp
@@ -0,0 +1,199 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/Indexlist.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Declaration of the Indexlist class designed to manage index lists of
+ *	constraints and bounds within a SubjectTo object.
+ */
+
+
+#ifndef QPOASES_INDEXLIST_HPP
+#define QPOASES_INDEXLIST_HPP
+
+
+#include <qpOASES/Utils.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** 
+ *	\brief Stores and manages index lists.
+ *
+ *	This class manages index lists of active/inactive bounds/constraints.
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ */
+class Indexlist
+{
+	/*
+	 *  FRIENDS
+	 */
+	friend class DenseMatrix;
+	friend class SymDenseMat;
+	friend class SparseMatrix;
+	friend class SparseMatrixRow;
+	friend class SymSparseMat;
+
+	/*
+	 *	PUBLIC MEMBER FUNCTIONS
+	 */
+	public:
+		/** Default constructor. */
+		Indexlist( );
+
+		/** Constructor which takes the desired physical length of the index list. */
+		Indexlist(	int n	/**< Physical length of index list. */
+					);
+
+		/** Copy constructor (deep copy). */
+		Indexlist(	const Indexlist& rhs	/**< Rhs object. */
+					);
+
+		/** Destructor. */
+		~Indexlist( );
+
+		/** Assingment operator (deep copy). */
+		Indexlist& operator=(	const Indexlist& rhs	/**< Rhs object. */
+								);
+
+
+		/** Initialises index list of desired physical length.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_INVALID_ARGUMENTS */
+		returnValue init(	int n = 0	/**< Physical length of index list. */
+							);
+
+
+		/** Creates an array of all numbers within the index set in correct order.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_INDEXLIST_CORRUPTED */
+		returnValue	getNumberArray(	int** const numberarray	/**< Output: Array of numbers (NULL on error). */
+									) const;
+
+		/** Creates an array of all numbers within the index set in correct order.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INDEXLIST_CORRUPTED */
+		returnValue	getISortArray(	int** const iSortArray	/**< Output: iSort Array. */
+									) const;
+
+
+		/** Determines the index within the index list at which a given number is stored.
+		 *	\return >= 0: Index of given number. \n
+		 			-1: Number not found. */
+		int	getIndex(	int givennumber	/**< Number whose index shall be determined. */
+						) const;
+
+		/** Returns the number stored at a given physical index.
+		 *	\return >= 0: Number stored at given physical index. \n
+		 			-RET_INDEXLIST_OUTOFBOUNDS */
+		int	getNumber(	int physicalindex	/**< Physical index of the number to be returned. */
+						) const;
+
+
+		/** Returns the current length of the index list.
+		 *	\return Current length of the index list. */
+		inline int getLength( ) const;
+
+		/** Returns last number within the index list.
+		 *	\return Last number within the index list. */
+		inline int getLastNumber( ) const;
+
+
+		/** Adds number to index list.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_INDEXLIST_MUST_BE_REORDERD \n
+		 			RET_INDEXLIST_EXCEEDS_MAX_LENGTH */
+		returnValue addNumber(	int addnumber			/**< Number to be added. */
+								);
+
+		/** Removes number from index list.
+		 *	\return SUCCESSFUL_RETURN */
+		returnValue removeNumber(	int removenumber	/**< Number to be removed. */
+									);
+
+		/** Swaps two numbers within index list.
+		 *	\return SUCCESSFUL_RETURN */
+		returnValue swapNumbers(	int number1,		/**< First number for swapping. */
+									int number2			/**< Second number for swapping. */
+									);
+
+		/** Determines if a given number is contained in the index set.
+		 *	\return BT_TRUE iff number is contain in the index set */
+		inline BooleanType isMember(	int _number		/**< Number to be tested for membership. */
+										) const;
+
+
+	/*
+	 *	PROTECTED MEMBER FUNCTIONS
+	 */
+	protected:
+		/** Frees all allocated memory.
+		 *  \return SUCCESSFUL_RETURN */
+		returnValue clear( );
+
+		/** Copies all members from given rhs object.
+		 *  \return SUCCESSFUL_RETURN */
+		returnValue copy(	const Indexlist& rhs	/**< Rhs object. */
+							);
+
+		/** Find first index j between -1 and length in sorted list of indices
+		 *  iSort such that numbers[iSort[j]] <= i < numbers[iSort[j+1]]. Uses
+		 *  bisection.
+		 *  \return j. */
+		int findInsert(	int i
+						) const;
+
+
+	/*
+	 *	PROTECTED MEMBER VARIABLES
+	 */
+	protected:
+		int* number;		/**< Array to store numbers of constraints or bounds. */
+		int* iSort;			/**< Index list to sort vector \a number */
+
+		int	length;			/**< Length of index list. */
+		int	first;			/**< Physical index of first element. */
+		int	last;			/**< Physical index of last element. */
+		int	lastusedindex;	/**< Physical index of last entry in index list. */
+		int	physicallength;	/**< Physical length of index list. */
+};
+
+END_NAMESPACE_QPOASES
+
+#include <qpOASES/Indexlist.ipp>
+
+#endif	/* QPOASES_INDEXLIST_HPP */
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Indexlist.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Indexlist.ipp
new file mode 100644
index 00000000000..338c8a33bf1
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Indexlist.ipp
@@ -0,0 +1,92 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/Indexlist.ipp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of inlined member functions of the Indexlist class designed
+ *	to manage index lists of constraints and bounds within a QProblem_SubjectTo.
+ */
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+/*
+ *	g e t N u m b e r
+ */
+inline int Indexlist::getNumber( int physicalindex ) const
+{
+	/* consistency check */
+	if ( ( physicalindex < 0 ) || ( physicalindex > length ) )
+		return -RET_INDEXLIST_OUTOFBOUNDS;
+
+	return number[physicalindex];
+}
+
+
+/*
+ *	g e t L e n g t h
+ */
+inline int Indexlist::getLength( ) const
+{
+	return length;
+}
+
+
+/*
+ *	g e t L a s t N u m b e r
+ */
+inline int Indexlist::getLastNumber( ) const
+{
+	return number[length-1];
+}
+
+
+/*
+ *	g e t L a s t N u m b e r
+ */
+inline BooleanType Indexlist::isMember( int _number ) const
+{
+	if ( getIndex( _number ) >= 0 )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Matrices.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Matrices.hpp
new file mode 100644
index 00000000000..d03ad5f6a91
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Matrices.hpp
@@ -0,0 +1,867 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/Matrices.hpp
+ *	\author Andreas Potschka, Hans Joachim Ferreau, Christian Kirches
+ *	\version 3.0
+ *	\date 2009-2014
+ *
+ *  Various matrix classes: Abstract base matrix class, dense and sparse matrices,
+ *  including symmetry exploiting specializations.
+ */
+
+
+
+#ifndef QPOASES_MATRICES_HPP
+#define QPOASES_MATRICES_HPP
+
+
+#include <qpOASES/Utils.hpp>
+#include <qpOASES/Indexlist.hpp>
+
+
+#ifdef __USE_SINGLE_PRECISION__
+
+	/** Macro for calling level 3 BLAS operation in single precision. */
+	#define GEMM sgemm_
+	/** Macro for calling level 3 BLAS operation in single precision. */
+	#define SYR ssyr_
+	/** Macro for calling level 3 BLAS operation in single precision. */
+	#define SYR2 ssyr2_
+	/** Macro for calling level 3 BLAS operation in single precision. */
+	#define POTRF spotrf_
+
+#else
+
+	/** Macro for calling level 3 BLAS operation in double precision. */
+	#define GEMM dgemm_
+	/** Macro for calling level 3 BLAS operation in double precision. */
+	#define SYR  dsyr_
+	/** Macro for calling level 3 BLAS operation in double precision. */
+	#define SYR2 dsyr2_
+	/** Macro for calling level 3 BLAS operation in double precision. */
+	#define POTRF dpotrf_
+
+#endif /* __USE_SINGLE_PRECISION__ */
+
+
+extern "C"
+{
+	/** Performs one of the matrix-matrix operation in double precision. */
+	void dgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*,
+			const double*, const double*, const unsigned long*, const double*, const unsigned long*,
+			const double*, double*, const unsigned long* );
+	/** Performs one of the matrix-matrix operation in single precision. */
+	void sgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*,
+			const float*, const float*, const unsigned long*, const float*, const unsigned long*,
+			const float*, float*, const unsigned long* );
+
+	/** Performs a symmetric rank 1 operation in double precision. */
+	void dsyr_ ( const char *, const unsigned long *, const double *, const double *,
+				 const unsigned long *, double *, const unsigned long *);
+	/** Performs a symmetric rank 1 operation in single precision. */
+	void ssyr_ ( const char *, const unsigned long *, const float *, const float *,
+				 const unsigned long *, float *, const unsigned long *);
+
+	/** Performs a symmetric rank 2 operation in double precision. */
+	void dsyr2_ ( const char *, const unsigned long *, const double *, const double *,
+				  const unsigned long *, const double *, const unsigned long *, double *, const unsigned long *);
+	/** Performs a symmetric rank 2 operation in single precision. */
+	void ssyr2_ ( const char *, const unsigned long *, const float *, const float *,
+				  const unsigned long *, const float *, const unsigned long *, float *, const unsigned long *);
+
+	/** Calculates the Cholesky factorization of a real symmetric positive definite matrix in double precision. */
+	void dpotrf_ ( const char *, const unsigned long *, double *, const unsigned long *, long * );
+	/** Calculates the Cholesky factorization of a real symmetric positive definite matrix in single precision. */
+	void spotrf_ ( const char *, const unsigned long *, float *, const unsigned long *, long * );
+}
+
+
+BEGIN_NAMESPACE_QPOASES
+
+/**
+ * Integer type for sparse matrix row/column entries. Make this "int"
+ * for 32 bit entries, and "long" for 64-bit entries on x86_64 platform.
+ *
+ * Most sparse codes still assume 32-bit entries here (HSL, BQPD, ...)
+ */
+typedef int sparse_int_t;
+
+
+/**
+ *	\brief Abstract base class for interfacing tailored matrix-vector operations.
+ *
+ *	Abstract base matrix class. Supplies interface to matrix vector
+ *  products, including products with submatrices given by (ordered) working set
+ *  index lists (see \a SubjectTo).
+ *
+ *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2011-2014
+ */
+class Matrix
+{
+	public:
+		/** Default constructor. */
+		Matrix( ) { doNotFreeMemory(); };
+
+		/** Destructor. */
+		virtual ~Matrix( ) { };
+
+		/** Frees all internal memory. */
+		virtual void free( ) = 0;
+
+		/** Returns a deep-copy of the Matrix object.
+		 *	\return Deep-copy of Matrix object */
+		virtual Matrix* duplicate( ) const = 0;
+
+		/** Returns i-th diagonal entry.
+		 *	\return i-th diagonal entry */
+		virtual real_t diag(	int i			/**< Index. */
+								) const = 0;
+
+		/** Checks whether matrix is square and diagonal.
+		 *	\return BT_TRUE  iff matrix is square and diagonal; \n
+		 *	        BT_FALSE otherwise. */
+		virtual BooleanType isDiag( ) const = 0;
+
+        /** Get the N-norm of the matrix
+         *  \return N-norm of the matrix
+         */
+        virtual real_t getNorm(	int type = 2	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
+								) const = 0;
+
+        /** Get the N-norm of a row
+         *  \return N-norm of row \a rNum
+         */
+        virtual real_t getRowNorm(	int rNum,		/**< Row number. */
+									int type = 2	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
+									) const = 0;
+
+		/** Retrieve indexed entries of matrix row multiplied by alpha.
+		 *	\return SUCCESSFUL_RETURN */
+		virtual returnValue getRow(	int rNum,						/**< Row number. */
+									const Indexlist* const icols,	/**< Index list specifying columns. */
+									real_t alpha,					/**< Scalar factor. */
+									real_t *row						/**< Output row vector. */
+									) const = 0;
+
+		/** Retrieve indexed entries of matrix column multiplied by alpha.
+		 *	\return SUCCESSFUL_RETURN */
+		virtual returnValue getCol(	int cNum,						/**< Column number. */
+									const Indexlist* const irows,	/**< Index list specifying rows. */
+									real_t alpha,					/**< Scalar factor. */
+									real_t *col						/**< Output column vector. */
+									) const = 0;
+
+		/** Evaluate Y=alpha*A*X + beta*Y.
+		 *	\return SUCCESSFUL_RETURN */
+		virtual returnValue times (	int xN,					/**< Number of vectors to multiply. */
+									real_t alpha,			/**< Scalar factor for matrix vector product. */
+									const real_t *x,		/**< Input vector to be multiplied. */
+									int xLD,				/**< Leading dimension of input x. */
+									real_t beta,			/**< Scalar factor for y. */
+									real_t *y,				/**< Output vector of results. */
+									int yLD					/**< Leading dimension of output y. */
+									) const = 0;
+
+		/** Evaluate Y=alpha*A'*X + beta*Y.
+		 *	\return SUCCESSFUL_RETURN */
+		virtual returnValue transTimes (	int xN,				/**< Number of vectors to multiply. */
+											real_t alpha,		/**< Scalar factor for matrix vector product. */
+											const real_t *x,	/**< Input vector to be multiplied. */
+											int xLD,			/**< Leading dimension of input x. */
+											real_t beta,		/**< Scalar factor for y. */
+											real_t *y,			/**< Output vector of results. */
+											int yLD				/**< Leading dimension of output y. */
+											) const = 0;
+
+		/** Evaluate matrix vector product with submatrix given by Indexlist.
+		 *	\return SUCCESSFUL_RETURN */
+		virtual returnValue times (	const Indexlist* const irows,	/**< Index list specifying rows. */
+									const Indexlist* const icols,	/**< Index list specifying columns. */
+									int xN,							/**< Number of vectors to multiply. */
+									real_t alpha,					/**< Scalar factor for matrix vector product. */
+									const real_t *x,				/**< Input vector to be multiplied. */
+									int xLD,						/**< Leading dimension of input x. */
+									real_t beta,					/**< Scalar factor for y. */
+									real_t *y,						/**< Output vector of results. */
+									int yLD,						/**< Leading dimension of output y. */
+									BooleanType yCompr = BT_TRUE	/**< Compressed storage for y. */
+									) const = 0;
+
+		/** Evaluate matrix transpose vector product.
+		 *	\return SUCCESSFUL_RETURN */
+		virtual returnValue transTimes (	const Indexlist* const irows,	/**< Index list specifying rows. */
+											const Indexlist* const icols,	/**< Index list specifying columns. */
+											int xN,							/**< Number of vectors to multiply. */
+											real_t alpha,					/**< Scalar factor for matrix vector product. */
+											const real_t *x,				/**< Input vector to be multiplied. */
+											int xLD,						/**< Leading dimension of input x. */
+											real_t beta,					/**< Scalar factor for y. */
+											real_t *y,						/**< Output vector of results. */
+											int yLD							/**< Leading dimension of output y. */
+											) const = 0;
+
+		/** Adds given offset to diagonal of matrix.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_NO_DIAGONAL_AVAILABLE */
+		virtual returnValue addToDiag(	real_t alpha		/**< Diagonal offset. */
+										) = 0;
+
+		/** Allocates and creates dense matrix array in row major format.
+		 *
+		 *  Note: Calling function has to free allocated memory!
+		 *
+		 *  \return Pointer to matrix array. 
+		 */
+		virtual real_t* full() const = 0;
+
+
+		/** Prints matrix to screen.
+		 *	\return SUCCESSFUL_RETURN */
+		virtual returnValue print(	const char* name = 0	/** Name of matrix. */
+									) const = 0;
+
+
+		/** Returns whether internal memory needs to be de-allocated.
+		 *	\return BT_TRUE  iff internal memory needs to be de-allocated, \n
+		 			BT_FALSE otherwise */
+		BooleanType needToFreeMemory( ) const { return freeMemory; };
+
+		/** Enables de-allocation of internal memory. */
+		void doFreeMemory( ) { freeMemory = BT_TRUE; };
+
+		/** Disables de-allocation of internal memory. */
+		void doNotFreeMemory( ) { freeMemory = BT_FALSE; };
+
+
+	protected:
+			BooleanType freeMemory;				/**< Indicating whether internal memory needs to be de-allocated. */
+
+};
+
+
+/**
+ *	\brief Abstract base class for interfacing matrix-vector operations tailored to symmetric matrices.
+ *
+ *	Abstract base class for symmetric matrices. Extends Matrix interface with
+ *  bilinear form evaluation.
+ *
+ *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2011-2014
+ */
+class SymmetricMatrix : public virtual Matrix
+{
+	public:
+
+		/** Returns a deep-copy of the SymmetricMatrix object.
+		 *	\return Deep-copy of SymmetricMatrix object */
+		virtual SymmetricMatrix* duplicateSym( ) const = 0;
+
+
+		/** Compute bilinear form y = x'*H*x using submatrix given by index list.
+		 *	\return SUCCESSFUL_RETURN */
+		virtual returnValue bilinear(	const Indexlist* const icols,	/**< Index list specifying columns of x. */
+										int xN,							/**< Number of vectors to multiply. */
+										const real_t *x,				/**< Input vector to be multiplied (uncompressed). */
+										int xLD,						/**< Leading dimension of input x. */
+										real_t *y,						/**< Output vector of results (compressed). */
+										int yLD							/**< Leading dimension of output y. */
+										) const = 0;
+
+};
+
+
+/**
+ *	\brief Interfaces matrix-vector operations tailored to general dense matrices.
+ *
+ *	Dense matrix class (row major format).
+ *
+ *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2011-2014
+ */
+class DenseMatrix : public virtual Matrix
+{
+	public:
+		/** Default constructor. */
+		DenseMatrix( ) : nRows(0), nCols(0), leaDim(0), val(0) {};
+
+		/** Constructor from vector of values.
+		 *  Caution: Data pointer must be valid throughout lifetime
+		 */
+		DenseMatrix(	int m,			/**< Number of rows. */
+						int n,			/**< Number of columns. */
+						int lD,			/**< Leading dimension. */
+						real_t *v		/**< Values. */
+						) : nRows(m), nCols(n), leaDim(lD), val(v) {}
+
+
+		/** Destructor. */
+		virtual ~DenseMatrix();
+
+		/** Frees all internal memory. */
+		virtual void free( );
+
+		/** Returns a deep-copy of the Matrix object.
+		 *	\return Deep-copy of Matrix object */
+		virtual Matrix *duplicate( ) const;
+
+		/** Returns i-th diagonal entry.
+		 *	\return i-th diagonal entry */
+		virtual real_t diag(	int i			/**< Index. */
+								) const;
+
+		/** Checks whether matrix is square and diagonal.
+		 *	\return BT_TRUE  iff matrix is square and diagonal; \n
+		 *	        BT_FALSE otherwise. */
+		virtual BooleanType isDiag( ) const;
+
+        /** Get the N-norm of the matrix
+         *  \return N-norm of the matrix
+         */
+        virtual real_t getNorm(	int type = 2	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
+								) const;
+		
+        /** Get the N-norm of a row
+         *  \return N-norm of row \a rNum
+         */
+        virtual real_t getRowNorm(	int rNum,		/**< Row number. */
+									int type = 2	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
+									) const;
+
+        /** Retrieve indexed entries of matrix row multiplied by alpha.
+		 *  \return SUCCESSFUL_RETURN */
+		virtual returnValue getRow(
+				int rNum,						/**< Row number. */
+				const Indexlist* const icols,	/**< Index list specifying columns. */
+				real_t alpha,					/**< Scalar factor. */
+				real_t *row						/**< Output row vector. */
+				) const;
+
+		/** Retrieve indexed entries of matrix column multiplied by alpha.
+		 *  \return SUCCESSFUL_RETURN */
+		virtual returnValue getCol(
+				int cNum,						/**< Column number. */
+				const Indexlist* const irows,	/**< Index list specifying rows. */
+				real_t alpha,					/**< Scalar factor. */
+				real_t *col						/**< Output column vector. */
+				) const;
+
+		/** Evaluate Y=alpha*A*X + beta*Y.
+		 *  \return SUCCESSFUL_RETURN. */
+		returnValue times (	int xN,					/**< Number of vectors to multiply. */
+							real_t alpha,			/**< Scalar factor for matrix vector product. */
+							const real_t *x,		/**< Input vector to be multiplied. */
+							int xLD,				/**< Leading dimension of input x. */
+							real_t beta,			/**< Scalar factor for y. */
+							real_t *y,				/**< Output vector of results. */
+							int yLD					/**< Leading dimension of output y. */
+							) const;
+
+		/** Evaluate Y=alpha*A'*X + beta*Y.
+		 *  \return SUCCESSFUL_RETURN. */
+		returnValue transTimes (	int xN,				/**< Number of vectors to multiply. */
+									real_t alpha,		/**< Scalar factor for matrix vector product. */
+									const real_t *x,	/**< Input vector to be multiplied. */
+									int xLD,			/**< Leading dimension of input x. */
+									real_t beta,		/**< Scalar factor for y. */
+									real_t *y,			/**< Output vector of results. */
+									int yLD				/**< Leading dimension of output y. */
+									) const;
+
+		/** Evaluate matrix vector product with submatrix given by Indexlist.
+		 *	\return SUCCESSFUL_RETURN */
+		virtual returnValue times (	const Indexlist* const irows,	/**< Index list specifying rows. */
+									const Indexlist* const icols,	/**< Index list specifying columns. */
+									int xN,							/**< Number of vectors to multiply. */
+									real_t alpha,					/**< Scalar factor for matrix vector product. */
+									const real_t *x,				/**< Input vector to be multiplied. */
+									int xLD,						/**< Leading dimension of input x. */
+									real_t beta,					/**< Scalar factor for y. */
+									real_t *y,						/**< Output vector of results. */
+									int yLD,						/**< Leading dimension of output y. */
+									BooleanType yCompr = BT_TRUE	/**< Compressed storage for y. */
+									) const;
+
+		/** Evaluate matrix transpose vector product.
+		 *	\return SUCCESSFUL_RETURN */
+		virtual returnValue transTimes (	const Indexlist* const irows,	/**< Index list specifying rows. */
+											const Indexlist* const icols,	/**< Index list specifying columns. */
+											int xN,							/**< Number of vectors to multiply. */
+											real_t alpha,					/**< Scalar factor for matrix vector product. */
+											const real_t *x,				/**< Input vector to be multiplied. */
+											int xLD,						/**< Leading dimension of input x. */
+											real_t beta,					/**< Scalar factor for y. */
+											real_t *y,						/**< Output vector of results. */
+											int yLD							/**< Leading dimension of output y. */
+											) const;
+
+		/** Adds given offset to diagonal of matrix.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_NO_DIAGONAL_AVAILABLE */
+		virtual returnValue addToDiag(	real_t alpha		/**< Diagonal offset. */
+										);
+
+
+		/** Allocates and creates dense matrix array in row major format.
+		 *
+		 *  Note: Calling function has to free allocated memory!
+		 *
+		 *  \return Pointer to matrix array. 
+		 */
+		virtual real_t* full() const;
+
+
+		/** Prints matrix to screen.
+		 *	\return SUCCESSFUL_RETURN */
+		virtual returnValue print( 	const char* name = 0	/** Name of matrix. */
+									) const;
+
+
+	protected:
+		int nRows;			/**< Number of rows. */
+		int nCols;			/**< Number of columns. */
+		int leaDim;			/**< Leading dimension. */
+		real_t *val;		/**< Vector of entries. */
+};
+
+
+/**
+ *	\brief Interfaces matrix-vector operations tailored to symmetric dense matrices.
+ *
+ *	Symmetric dense matrix class.
+ *
+ *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2011-2014
+ */
+class SymDenseMat : public DenseMatrix, public SymmetricMatrix
+{
+	public:
+		/** Default constructor. */
+		SymDenseMat() : DenseMatrix() {}
+
+		/** Constructor from vector of values. */
+		SymDenseMat(	int m,			/**< Number of rows. */
+						int n,			/**< Number of columns. */
+						int lD,			/**< Leading dimension. */
+						real_t *v		/**< Values. */
+						) : DenseMatrix(m, n, lD, v) {}
+
+		/** Returns a deep-copy of the Matrix object.
+		 *	\return Deep-copy of Matrix object */
+		virtual Matrix *duplicate( ) const;
+
+		/** Returns a deep-copy of the SymmetricMatrix object.
+		 *	\return Deep-copy of SymmetricMatrix object */
+		virtual SymmetricMatrix* duplicateSym( ) const;
+
+
+		/** Compute bilinear form y = x'*H*x using submatrix given by index list.
+		 *	\return SUCCESSFUL_RETURN */
+		virtual returnValue bilinear(	const Indexlist* const icols,	/**< Index list specifying columns of x. */
+										int xN,							/**< Number of vectors to multiply. */
+										const real_t *x,				/**< Input vector to be multiplied (uncompressed). */
+										int xLD,						/**< Leading dimension of input x. */
+										real_t *y,						/**< Output vector of results (compressed). */
+										int yLD							/**< Leading dimension of output y. */
+										) const;
+};
+
+
+/**
+ *	\brief Interfaces matrix-vector operations tailored to general sparse matrices.
+ *
+ *	Sparse matrix class (col compressed format).
+ *
+ *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2011-2014
+ */
+class SparseMatrix : public virtual Matrix
+{
+	public:
+		/** Default constructor. */
+		SparseMatrix();
+
+		/** Constructor with arguments. */
+		SparseMatrix(	int nr, 			/**< Number of rows. */
+						int nc, 			/**< Number of columns. */
+						sparse_int_t *r, 	/**< Row indices (length). */
+						sparse_int_t *c, 	/**< Indices to first entry of columns (nCols+1). */
+						real_t *v			/**< Vector of entries (length). */
+						);
+
+		/** Constructor from dense matrix. */
+		SparseMatrix(	int nr, 				/**< Number of rows. */
+						int nc,			 		/**< Number of columns. */
+						int ld,					/**< Leading dimension. */
+						const real_t * const v	/**< Row major stored matrix elements. */
+						);
+
+		/** Destructor. */
+		virtual ~SparseMatrix();
+
+		/** Frees all internal memory. */
+		virtual void free( );
+
+		/** Returns a deep-copy of the Matrix object.
+		 *	\return Deep-copy of Matrix object */
+		virtual Matrix *duplicate( ) const;
+
+		/** Returns i-th diagonal entry.
+		 *	\return i-th diagonal entry (or INFTY if diagonal does not exist)*/
+		virtual real_t diag(	int i			/**< Index. */
+								) const;
+
+		/** Checks whether matrix is square and diagonal.
+		 *	\return BT_TRUE  iff matrix is square and diagonal; \n
+		 *	        BT_FALSE otherwise. */
+		virtual BooleanType isDiag( ) const;
+
+
+        /** Get the N-norm of the matrix
+         *  \return N-norm of the matrix
+         */
+        virtual real_t getNorm(	int type = 2	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
+								) const;
+
+        /** Get the N-norm of a row
+         *  \return N-norm of row \a rNum
+         */
+        virtual real_t getRowNorm(	int rNum,		/**< Row number. */
+									int type = 2	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
+									) const;
+
+		/** Retrieve indexed entries of matrix row multiplied by alpha. */
+		virtual returnValue getRow(	int rNum,						/**< Row number. */
+									const Indexlist* const icols,	/**< Index list specifying columns. */
+									real_t alpha,					/**< Scalar factor. */
+									real_t *row						/**< Output row vector. */
+									) const;
+
+		/** Retrieve indexed entries of matrix column multiplied by alpha. */
+		virtual returnValue getCol(	int cNum,						/**< Column number. */
+									const Indexlist* const irows,	/**< Index list specifying rows. */
+									real_t alpha,					/**< Scalar factor. */
+									real_t *col						/**< Output column vector. */
+									) const;
+
+		/** Evaluate Y=alpha*A*X + beta*Y. */
+		virtual returnValue times (	int xN,					/**< Number of vectors to multiply. */
+									real_t alpha,			/**< Scalar factor for matrix vector product. */
+									const real_t *x,		/**< Input vector to be multiplied. */
+									int xLD,				/**< Leading dimension of input x. */
+									real_t beta,			/**< Scalar factor for y. */
+									real_t *y,				/**< Output vector of results. */
+									int yLD					/**< Leading dimension of output y. */
+									) const;
+
+		/** Evaluate Y=alpha*A'*X + beta*Y. */
+		virtual returnValue transTimes (	int xN,				/**< Number of vectors to multiply. */
+											real_t alpha,		/**< Scalar factor for matrix vector product. */
+											const real_t *x,	/**< Input vector to be multiplied. */
+											int xLD,			/**< Leading dimension of input x. */
+											real_t beta,		/**< Scalar factor for y. */
+											real_t *y,			/**< Output vector of results. */
+											int yLD				/**< Leading dimension of output y. */
+											) const;
+
+		/** Evaluate matrix vector product with submatrix given by Indexlist. */
+		virtual returnValue times (	const Indexlist* const irows,	/**< Index list specifying rows. */
+									const Indexlist* const icols,	/**< Index list specifying columns. */
+									int xN,							/**< Number of vectors to multiply. */
+									real_t alpha,					/**< Scalar factor for matrix vector product. */
+									const real_t *x,				/**< Input vector to be multiplied. */
+									int xLD,						/**< Leading dimension of input x. */
+									real_t beta,					/**< Scalar factor for y. */
+									real_t *y,						/**< Output vector of results. */
+									int yLD,						/**< Leading dimension of output y. */
+									BooleanType yCompr = BT_TRUE	/**< Compressed storage for y. */
+									) const;
+
+		/** Evaluate matrix transpose vector product. */
+		virtual returnValue transTimes (	const Indexlist* const irows,	/**< Index list specifying rows. */
+											const Indexlist* const icols,	/**< Index list specifying columns. */
+											int xN,							/**< Number of vectors to multiply. */
+											real_t alpha,					/**< Scalar factor for matrix vector product. */
+											const real_t *x,				/**< Input vector to be multiplied. */
+											int xLD,						/**< Leading dimension of input x. */
+											real_t beta,					/**< Scalar factor for y. */
+											real_t *y,						/**< Output vector of results. */
+											int yLD							/**< Leading dimension of output y. */
+											) const;
+
+		/** Adds given offset to diagonal of matrix.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_NO_DIAGONAL_AVAILABLE */
+		virtual returnValue addToDiag(	real_t alpha		/**< Diagonal offset. */
+										);
+
+		/** Create jd field from ir and jc.
+		 *  \return Pointer to jd. */
+		sparse_int_t *createDiagInfo();
+
+		/** Allocates and creates dense matrix array in row major format.
+		 *
+		 *  Note: Calling function has to free allocated memory!
+		 *
+		 *  \return Pointer to matrix array. 
+		 */
+		virtual real_t* full() const;
+
+		/** Prints matrix to screen.
+		 *	\return SUCCESSFUL_RETURN */
+		virtual returnValue print( 	const char* name = 0	/** Name of matrix. */
+									) const;
+
+
+	protected:
+		int nRows;			/**< Number of rows. */
+		int nCols;			/**< Number of columns. */
+		sparse_int_t *ir;	/**< Row indices (length). */
+		sparse_int_t *jc;	/**< Indices to first entry of columns (nCols+1). */
+		sparse_int_t *jd;	/**< Indices to first entry of lower triangle (including diagonal) (nCols). */
+		real_t *val;		/**< Vector of entries (length). */
+};
+
+
+/**
+ *	\brief Interfaces matrix-vector operations tailored to general sparse matrices.
+ *
+ *	Sparse matrix class (row compressed format).
+ *
+ *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2011-2014
+ */
+class SparseMatrixRow : public virtual Matrix
+{
+	public:
+		/** Default constructor. */
+		SparseMatrixRow();
+
+		/** Constructor with arguments. */
+		SparseMatrixRow(	int nr, 			/**< Number of rows. */
+							int nc, 			/**< Number of columns. */
+							sparse_int_t *r, 	/**< Indices to first entry of rows (nRows+1). */
+							sparse_int_t *c, 	/**< Column indices (length). */
+							real_t *v			/**< Vector of entries (length). */
+							);
+
+		/** Constructor from dense matrix. */
+		SparseMatrixRow(	int nr, 				/**< Number of rows. */
+							int nc,			 		/**< Number of columns. */
+							int ld,					/**< Leading dimension. */
+							const real_t * const v	/**< Row major stored matrix elements. */
+							);
+
+		/** Destructor. */
+		virtual ~SparseMatrixRow();
+
+		/** Frees all internal memory. */
+		virtual void free( );
+
+		/** Returns a deep-copy of the Matrix object.
+		 *	\return Deep-copy of Matrix object */
+		virtual Matrix *duplicate( ) const;
+
+		/** Returns i-th diagonal entry.
+		 *	\return i-th diagonal entry (or INFTY if diagonal does not exist)*/
+		virtual real_t diag(	int i			/**< Index. */
+								) const;
+
+		/** Checks whether matrix is square and diagonal.
+		 *	\return BT_TRUE  iff matrix is square and diagonal; \n
+		 *	        BT_FALSE otherwise. */
+		virtual BooleanType isDiag( ) const;
+
+		
+        /** Get the N-norm of the matrix
+         *  \return N-norm of the matrix
+         */
+        virtual real_t getNorm(	int type = 2	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
+								) const;
+
+        /** Get the N-norm of a row
+         *  \return N-norm of row \a rNum
+         */
+        virtual real_t getRowNorm(	int rNum,		/**< Row number. */
+									int type = 2	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
+									) const;
+
+		/** Retrieve indexed entries of matrix row multiplied by alpha. */
+		virtual returnValue getRow (
+				int rNum,						/**< Row number. */
+				const Indexlist* const icols,	/**< Index list specifying columns. */
+				real_t alpha,					/**< Scalar factor. */
+				real_t *row						/**< Output row vector. */
+				) const;
+
+		/** Retrieve indexed entries of matrix column multiplied by alpha. */
+		virtual returnValue getCol (
+				int cNum,						/**< Column number. */
+				const Indexlist* const irows,	/**< Index list specifying rows. */
+				real_t alpha,					/**< Scalar factor. */
+				real_t *col						/**< Output column vector. */
+				) const;
+
+		/** Evaluate Y=alpha*A*X + beta*Y. */
+		virtual returnValue times (	int xN,					/**< Number of vectors to multiply. */
+									real_t alpha,			/**< Scalar factor for matrix vector product. */
+									const real_t *x,		/**< Input vector to be multiplied. */
+									int xLD,				/**< Leading dimension of input x. */
+									real_t beta,			/**< Scalar factor for y. */
+									real_t *y,				/**< Output vector of results. */
+									int yLD					/**< Leading dimension of output y. */
+									) const;
+
+		/** Evaluate Y=alpha*A'*X + beta*Y. */
+		virtual returnValue transTimes (	int xN,				/**< Number of vectors to multiply. */
+											real_t alpha,		/**< Scalar factor for matrix vector product. */
+											const real_t *x,	/**< Input vector to be multiplied. */
+											int xLD,			/**< Leading dimension of input x. */
+											real_t beta,		/**< Scalar factor for y. */
+											real_t *y,			/**< Output vector of results. */
+											int yLD				/**< Leading dimension of output y. */
+											) const;
+
+		/** Evaluate matrix vector product with submatrix given by Indexlist. */
+		virtual returnValue times (	const Indexlist* const irows,	/**< Index list specifying rows. */
+									const Indexlist* const icols,	/**< Index list specifying columns. */
+									int xN,							/**< Number of vectors to multiply. */
+									real_t alpha,					/**< Scalar factor for matrix vector product. */
+									const real_t *x,				/**< Input vector to be multiplied. */
+									int xLD,						/**< Leading dimension of input x. */
+									real_t beta,					/**< Scalar factor for y. */
+									real_t *y,						/**< Output vector of results. */
+									int yLD,						/**< Leading dimension of output y. */
+									BooleanType yCompr = BT_TRUE	/**< Compressed storage for y. */
+									) const;
+
+		/** Evaluate matrix transpose vector product. */
+		virtual returnValue transTimes (	const Indexlist* const irows,	/**< Index list specifying rows. */
+											const Indexlist* const icols,	/**< Index list specifying columns. */
+											int xN,							/**< Number of vectors to multiply. */
+											real_t alpha,					/**< Scalar factor for matrix vector product. */
+											const real_t *x,				/**< Input vector to be multiplied. */
+											int xLD,						/**< Leading dimension of input x. */
+											real_t beta,					/**< Scalar factor for y. */
+											real_t *y,						/**< Output vector of results. */
+											int yLD							/**< Leading dimension of output y. */
+											) const;
+
+		/** Adds given offset to diagonal of matrix.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_NO_DIAGONAL_AVAILABLE */
+		virtual returnValue addToDiag(	real_t alpha		/**< Diagonal offset. */
+										);
+
+		/** Create jd field from ir and jc.
+		 *  \return Pointer to jd. */
+		sparse_int_t *createDiagInfo();
+
+		/** Allocates and creates dense matrix array in row major format.
+		 *
+		 *  Note: Calling function has to free allocated memory!
+		 *
+		 *  \return Pointer to matrix array. 
+		 */
+		virtual real_t* full() const;
+
+		/** Prints matrix to screen.
+		 *	\return SUCCESSFUL_RETURN */
+		virtual returnValue print( 	const char* name = 0	/** Name of matrix. */
+									) const;
+
+
+	protected:
+		int nRows;			/**< Number of rows. */
+		int nCols;			/**< Number of columns. */
+		sparse_int_t *jr;	/**< Indices to first entry of row (nRows+1). */
+		sparse_int_t *ic;	/**< Column indices (length). */
+		sparse_int_t *jd;	/**< Indices to first entry of upper triangle (including diagonal) (nRows). */
+		real_t *val;		/**< Vector of entries (length). */
+};
+
+
+/**
+ *	\brief Interfaces matrix-vector operations tailored to symmetric sparse matrices.
+ *
+ *	Symmetric sparse matrix class (column compressed format).
+ *
+ *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2011-2014
+ */
+class SymSparseMat : public SymmetricMatrix, public SparseMatrix
+{
+	public:
+		/** Default constructor. */
+		SymSparseMat() : SparseMatrix() {}
+
+		/** Constructor with arguments. */
+		SymSparseMat(	int nr, 			/**< Number of rows. */
+						int nc, 			/**< Number of columns. */
+						sparse_int_t *r, 	/**< Row indices (length). */
+						sparse_int_t *c, 	/**< Indices to first entry of columns (nCols+1). */
+						real_t *v			/**< Vector of entries (length). */
+						) : SparseMatrix(nr, nc, r, c, v) {}
+
+		/** Constructor from dense matrix. */
+		SymSparseMat(	int nr, 				/**< Number of rows. */
+						int nc,			 		/**< Number of columns. */
+						int ld,					/**< Leading dimension. */
+						const real_t * const v	/**< Row major stored matrix elements. */
+						) : SparseMatrix(nr, nc, ld, v) {}
+
+		/** Returns a deep-copy of the Matrix object.
+		 *	\return Deep-copy of Matrix object */
+		virtual Matrix *duplicate( ) const;
+
+		/** Returns a deep-copy of the SymmetricMatrix object.
+		 *	\return Deep-copy of SymmetricMatrix object */
+		virtual SymmetricMatrix* duplicateSym( ) const;
+
+
+		/** Compute bilinear form y = x'*H*x using submatrix given by index list.
+		*	\return SUCCESSFUL_RETURN */
+		virtual returnValue bilinear(	const Indexlist* const icols,	/**< Index list specifying columns of x. */
+										int xN,							/**< Number of vectors to multiply. */
+										const real_t *x,				/**< Input vector to be multiplied (uncompressed). */
+										int xLD,						/**< Leading dimension of input x. */
+										real_t *y,						/**< Output vector of results (compressed). */
+										int yLD							/**< Leading dimension of output y. */
+										) const;
+};
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_MATRICES_HPP */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/MessageHandling.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/MessageHandling.hpp
new file mode 100644
index 00000000000..fcddeb8fd89
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/MessageHandling.hpp
@@ -0,0 +1,472 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/MessageHandling.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches (thanks to Leonard Wirsching)
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Declaration of the MessageHandling class including global return values.
+ */
+
+
+#ifndef QPOASES_MESSAGEHANDLING_HPP
+#define QPOASES_MESSAGEHANDLING_HPP
+
+
+#include <stdio.h>
+#include <string.h>
+
+#ifdef __DEBUG__
+#include <assert.h>
+#endif
+
+#include <qpOASES/Constants.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** Default file to display messages. */
+static FILE* stdFile = stdout;
+
+
+/**
+ *	\brief Defines all symbols for global return values.
+ *	
+ *  The enumeration returnValueType defines all symbols for global return values.
+ *	Important: All return values are assumed to be nonnegative!
+ *
+ *	\author Hans Joachim Ferreau
+ */
+enum returnValue
+{
+TERMINAL_LIST_ELEMENT = -1,						/**< Terminal list element, internal usage only! */
+/* miscellaneous */
+SUCCESSFUL_RETURN = 0,							/**< Successful return. */
+RET_DIV_BY_ZERO,		   						/**< Division by zero. */
+RET_INDEX_OUT_OF_BOUNDS,						/**< Index out of bounds. */
+RET_INVALID_ARGUMENTS,							/**< At least one of the arguments is invalid. */
+RET_ERROR_UNDEFINED,							/**< Error number undefined. */
+RET_WARNING_UNDEFINED,							/**< Warning number undefined. */
+RET_INFO_UNDEFINED,								/**< Info number undefined. */
+RET_EWI_UNDEFINED,								/**< Error/warning/info number undefined. */
+RET_AVAILABLE_WITH_LINUX_ONLY,					/**< This function is available under Linux only. */
+RET_UNKNOWN_BUG,								/**< The error occurred is not yet known. */
+RET_PRINTLEVEL_CHANGED,							/**< Print level changed. */
+RET_NOT_YET_IMPLEMENTED,						/**< Requested function is not yet implemented in this version of qpOASES. */
+/* Indexlist */
+RET_INDEXLIST_MUST_BE_REORDERD,					/**< Index list has to be reordered. (12) */
+RET_INDEXLIST_EXCEEDS_MAX_LENGTH,				/**< Index list exceeds its maximal physical length. */
+RET_INDEXLIST_CORRUPTED,						/**< Index list corrupted. */
+RET_INDEXLIST_OUTOFBOUNDS,						/**< Physical index is out of bounds. */
+RET_INDEXLIST_ADD_FAILED,						/**< Adding indices from another index set failed. */
+RET_INDEXLIST_INTERSECT_FAILED,					/**< Intersection with another index set failed. */
+/* SubjectTo / Bounds / Constraints */
+RET_INDEX_ALREADY_OF_DESIRED_STATUS,			/**< Index is already of desired status. (18) */
+RET_ADDINDEX_FAILED,							/**< Adding index to index set failed. */
+RET_REMOVEINDEX_FAILED,							/**< Removing index from index set failed. (20) */
+RET_SWAPINDEX_FAILED,							/**< Cannot swap between different indexsets. */
+RET_NOTHING_TO_DO,								/**< Nothing to do. */
+RET_SETUP_BOUND_FAILED,							/**< Setting up bound index failed. */
+RET_SETUP_CONSTRAINT_FAILED,					/**< Setting up constraint index failed. */
+RET_MOVING_BOUND_FAILED,						/**< Moving bound between index sets failed. */
+RET_MOVING_CONSTRAINT_FAILED,					/**< Moving constraint between index sets failed. */
+RET_SHIFTING_FAILED,							/**< Shifting of bounds/constraints failed. */
+RET_ROTATING_FAILED,							/**< Rotating of bounds/constraints failed. */
+/* QProblem */
+RET_QPOBJECT_NOT_SETUP,							/**< The QP object has not been setup correctly, use another constructor. (29) */
+RET_QP_ALREADY_INITIALISED,						/**< QProblem has already been initialised. (30) */
+RET_NO_INIT_WITH_STANDARD_SOLVER,				/**< Initialisation via extern QP solver is not yet implemented. */
+RET_RESET_FAILED,								/**< Reset failed. */
+RET_INIT_FAILED,								/**< Initialisation failed. */
+RET_INIT_FAILED_TQ,								/**< Initialisation failed due to TQ factorisation. */
+RET_INIT_FAILED_CHOLESKY,						/**< Initialisation failed due to Cholesky decomposition. */
+RET_INIT_FAILED_HOTSTART,						/**< Initialisation failed! QP could not be solved! */
+RET_INIT_FAILED_INFEASIBILITY,					/**< Initial QP could not be solved due to infeasibility! */
+RET_INIT_FAILED_UNBOUNDEDNESS,					/**< Initial QP could not be solved due to unboundedness! */
+RET_INIT_FAILED_REGULARISATION,					/**< Initialisation failed as Hessian matrix could not be regularised. */
+RET_INIT_SUCCESSFUL,							/**< Initialisation done. (40) */
+RET_OBTAINING_WORKINGSET_FAILED,				/**< Failed to obtain working set for auxiliary QP. */
+RET_SETUP_WORKINGSET_FAILED,					/**< Failed to setup working set for auxiliary QP. */
+RET_SETUP_AUXILIARYQP_FAILED,					/**< Failed to setup auxiliary QP for initialised homotopy. */
+RET_NO_EXTERN_SOLVER,							/**< No extern QP solver available. */
+RET_QP_UNBOUNDED,								/**< QP is unbounded. */
+RET_QP_INFEASIBLE,								/**< QP is infeasible. */
+RET_QP_NOT_SOLVED,								/**< Problems occurred while solving QP with standard solver. */
+RET_QP_SOLVED,									/**< QP successfully solved. */
+RET_UNABLE_TO_SOLVE_QP,							/**< Problems occurred while solving QP. */
+RET_INITIALISATION_STARTED,						/**< Starting problem initialisation... */
+RET_HOTSTART_FAILED,							/**< Unable to perform homotopy due to internal error. (50) */
+RET_HOTSTART_FAILED_TO_INIT,					/**< Unable to initialise problem. */
+RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED,		/**< Unable to perform homotopy as previous QP is not solved. */
+RET_ITERATION_STARTED,							/**< Iteration... */
+RET_SHIFT_DETERMINATION_FAILED,					/**< Determination of shift of the QP data failed. */
+RET_STEPDIRECTION_DETERMINATION_FAILED,			/**< Determination of step direction failed. */
+RET_STEPLENGTH_DETERMINATION_FAILED,			/**< Determination of step direction failed. */
+RET_OPTIMAL_SOLUTION_FOUND,						/**< Optimal solution of neighbouring QP found. */
+RET_HOMOTOPY_STEP_FAILED,						/**< Unable to perform homotopy step. */
+RET_HOTSTART_STOPPED_INFEASIBILITY,				/**< Premature homotopy termination because QP is infeasible. */
+RET_HOTSTART_STOPPED_UNBOUNDEDNESS,				/**< Premature homotopy termination because QP is unbounded. (60) */
+RET_WORKINGSET_UPDATE_FAILED,					/**< Unable to update working sets according to initial guesses. */
+RET_MAX_NWSR_REACHED,							/**< Maximum number of working set recalculations performed. */
+RET_CONSTRAINTS_NOT_SPECIFIED,					/**< Problem does comprise constraints! You also have to specify new constraints' bounds. */
+RET_INVALID_FACTORISATION_FLAG,					/**< Invalid factorisation flag. */
+RET_UNABLE_TO_SAVE_QPDATA,						/**< Unable to save QP data. */
+RET_STEPDIRECTION_FAILED_TQ,					/**< Abnormal termination due to TQ factorisation. */
+RET_STEPDIRECTION_FAILED_CHOLESKY,				/**< Abnormal termination due to Cholesky factorisation. */
+RET_CYCLING_DETECTED,							/**< Cycling detected. */
+RET_CYCLING_NOT_RESOLVED,						/**< Cycling cannot be resolved, QP probably infeasible. */
+RET_CYCLING_RESOLVED,							/**< Cycling probably resolved. (70) */
+RET_STEPSIZE,									/**< For displaying performed stepsize. */
+RET_STEPSIZE_NONPOSITIVE,						/**< For displaying non-positive stepsize. */
+RET_SETUPSUBJECTTOTYPE_FAILED,					/**< Setup of SubjectToTypes failed. */
+RET_ADDCONSTRAINT_FAILED,						/**< Addition of constraint to working set failed. */
+RET_ADDCONSTRAINT_FAILED_INFEASIBILITY,			/**< Addition of constraint to working set failed (due to QP infeasibility). */
+RET_ADDBOUND_FAILED,							/**< Addition of bound to working set failed. */
+RET_ADDBOUND_FAILED_INFEASIBILITY,				/**< Addition of bound to working set failed (due to QP infeasibility). */
+RET_REMOVECONSTRAINT_FAILED,					/**< Removal of constraint from working set failed. */
+RET_REMOVEBOUND_FAILED,							/**< Removal of bound from working set failed. */
+RET_REMOVE_FROM_ACTIVESET,						/**< Removing from active set... (80) */
+RET_ADD_TO_ACTIVESET,							/**< Adding to active set... */
+RET_REMOVE_FROM_ACTIVESET_FAILED,				/**< Removing from active set failed. */
+RET_ADD_TO_ACTIVESET_FAILED,					/**< Adding to active set failed. */
+RET_CONSTRAINT_ALREADY_ACTIVE,					/**< Constraint is already active. */
+RET_ALL_CONSTRAINTS_ACTIVE,						/**< All constraints are active, no further constraint can be added. */
+RET_LINEARLY_DEPENDENT,							/**< New bound/constraint is linearly dependent. */
+RET_LINEARLY_INDEPENDENT,						/**< New bound/constraint is linearly independent. */
+RET_LI_RESOLVED,								/**< Linear independence of active constraint matrix successfully resolved. */
+RET_ENSURELI_FAILED,							/**< Failed to ensure linear independence of active constraint matrix. */
+RET_ENSURELI_FAILED_TQ,							/**< Abnormal termination due to TQ factorisation. (90) */
+RET_ENSURELI_FAILED_NOINDEX,					/**< QP is infeasible. */
+RET_ENSURELI_FAILED_CYCLING,					/**< QP is infeasible. */
+RET_BOUND_ALREADY_ACTIVE,						/**< Bound is already active. */
+RET_ALL_BOUNDS_ACTIVE,							/**< All bounds are active, no further bound can be added. */
+RET_CONSTRAINT_NOT_ACTIVE,						/**< Constraint is not active. */
+RET_BOUND_NOT_ACTIVE,							/**< Bound is not active. */
+RET_HESSIAN_NOT_SPD,							/**< Projected Hessian matrix not positive definite. */
+RET_HESSIAN_INDEFINITE,							/**< Hessian matrix is indefinite. */
+RET_MATRIX_SHIFT_FAILED,						/**< Unable to update matrices or to transform vectors. */
+RET_MATRIX_FACTORISATION_FAILED,				/**< Unable to calculate new matrix factorisations. (100) */
+RET_PRINT_ITERATION_FAILED,						/**< Unable to print information on current iteration. */
+RET_NO_GLOBAL_MESSAGE_OUTPUTFILE,				/**< No global message output file initialised. */
+RET_DISABLECONSTRAINTS_FAILED,					/**< Unable to disbable constraints. */
+RET_ENABLECONSTRAINTS_FAILED,					/**< Unable to enbable constraints. */
+RET_ALREADY_ENABLED,							/**< Bound or constraint is already enabled. */
+RET_ALREADY_DISABLED,							/**< Bound or constraint is already disabled. */
+RET_NO_HESSIAN_SPECIFIED, 						/**< No Hessian matrix has been specified. */
+RET_USING_REGULARISATION,						/**< Using regularisation as Hessian matrix is not positive definite. */
+RET_EPS_MUST_BE_POSITVE,						/**< Eps for regularisation must be sufficiently positive. */
+RET_REGSTEPS_MUST_BE_POSITVE, 					/**< Maximum number of regularisation steps must be non-negative. (110) */
+RET_HESSIAN_ALREADY_REGULARISED,				/**< Hessian has been already regularised. */
+RET_CANNOT_REGULARISE_IDENTITY,					/**< Identity Hessian matrix cannot be regularised. */
+RET_CANNOT_REGULARISE_SPARSE,					/**< Sparse matrix cannot be regularised as diagonal entry is missing. */
+RET_NO_REGSTEP_NWSR,							/**< No additional regularisation step could be performed due to limits. */
+RET_FEWER_REGSTEPS_NWSR,						/**< Fewer additional regularisation steps have been performed due to limits. */
+RET_CHOLESKY_OF_ZERO_HESSIAN, 					/**< Cholesky decomposition of (unregularised) zero Hessian matrix. */
+RET_CONSTRAINTS_ARE_NOT_SCALED, 				/**< (no longer in use) */
+RET_INITIAL_BOUNDS_STATUS_NYI, 					/**< (no longer in use) */
+RET_ERROR_IN_CONSTRAINTPRODUCT,					/**< Error in user-defined constraint product function. */
+RET_FIX_BOUNDS_FOR_LP,							/**< All initial bounds must be fixed when solving an (unregularised) LP. */
+RET_USE_REGULARISATION_FOR_LP,					/**< Set options.enableRegularisation=BT_TRUE for solving LPs. */
+/* SQProblem */
+RET_UPDATEMATRICES_FAILED,						/**< Unable to update QP matrices. */
+RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED,		/**< Unable to update matrices as previous QP is not solved. */
+/* Utils */
+RET_UNABLE_TO_OPEN_FILE,						/**< Unable to open file. (120) */
+RET_UNABLE_TO_WRITE_FILE,						/**< Unable to write into file. */
+RET_UNABLE_TO_READ_FILE,						/**< Unable to read from file. */
+RET_FILEDATA_INCONSISTENT,						/**< File contains inconsistent data. */
+/* Options */
+RET_OPTIONS_ADJUSTED,							/**< Options needed to be adjusted for consistency reasons. */
+/* SolutionAnalysis */
+RET_UNABLE_TO_ANALYSE_QPROBLEM, 				/**< Unable to analyse (S)QProblem(B) object. */
+/* Benchmark */
+RET_NWSR_SET_TO_ONE,							/**< Maximum number of working set changes was set to 1. */
+RET_UNABLE_TO_READ_BENCHMARK,					/**< Unable to read benchmark data. */
+RET_BENCHMARK_ABORTED,							/**< Benchmark aborted. */
+RET_INITIAL_QP_SOLVED,							/**< Initial QP solved. */
+RET_QP_SOLUTION_STARTED,						/**< Solving QP... */
+RET_BENCHMARK_SUCCESSFUL,						/**< Benchmark terminated successfully. (130) */
+/* Sparse matrices */
+RET_NO_DIAGONAL_AVAILABLE,						/**< Sparse matrix does not have entries on full diagonal. */
+RET_DIAGONAL_NOT_INITIALISED,					/**< Diagonal data of sparse matrix has not been initialised. */
+/* Dropping of infeasible constraints */
+RET_ENSURELI_DROPPED,							/**< Linear independence resolved by dropping blocking constraint. */
+/* Simple exitflags */
+RET_SIMPLE_STATUS_P1,							/**< QP problem could not be solved within given number of iterations. */
+RET_SIMPLE_STATUS_P0,							/**< QP problem solved. */
+RET_SIMPLE_STATUS_M1,							/**< QP problem could not be solved due to an internal error. */
+RET_SIMPLE_STATUS_M2,							/**< QP problem is infeasible (and thus could not be solved). */
+RET_SIMPLE_STATUS_M3 							/**< QP problem is unbounded (and thus could not be solved). */
+};
+
+
+/** 
+ *	\brief Handles all kind of error messages, warnings and other information.
+ *
+ *	This class handles all kinds of messages (errors, warnings, infos) initiated
+ *  by qpOASES modules and stores the corresponding global preferences.
+ *
+ *	\author Hans Joachim Ferreau (thanks to Leonard Wirsching)
+ *	\version 3.0
+ *	\date 2007-2014
+ */
+class MessageHandling
+{
+	/*
+	 *	INTERNAL DATA STRUCTURES
+	 */
+	public:
+		/**
+		*	\brief Data structure for entries in global message list.
+		*	
+		*	Data structure for entries in global message list.
+		*
+		*	\author Hans Joachim Ferreau
+		*/
+		typedef struct {
+			returnValue key;							/**< Global return value. */
+			const char* data;							/**< Corresponding message. */
+			VisibilityStatus globalVisibilityStatus; 	/**< Determines if message can be printed.
+														* 	 If this value is set to VS_HIDDEN, no message is printed! */
+		} ReturnValueList;
+
+
+	/*
+	 *	PUBLIC MEMBER FUNCTIONS
+	 */
+	public:
+		/** Default constructor. */
+		MessageHandling( );
+
+		/** Constructor which takes the desired output file. */
+		MessageHandling(  FILE* _outputFile						/**< Output file. */
+						  );
+
+		/** Constructor which takes the desired visibility states. */
+		MessageHandling(	VisibilityStatus _errorVisibility,	/**< Visibility status for error messages. */
+							VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */
+							VisibilityStatus _infoVisibility	/**< Visibility status for info messages. */
+							);
+
+		/** Constructor which takes the desired output file and desired visibility states. */
+		MessageHandling(	FILE* _outputFile,					/**< Output file. */
+							VisibilityStatus _errorVisibility,	/**< Visibility status for error messages. */
+							VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */
+							VisibilityStatus _infoVisibility	/**< Visibility status for info messages. */
+							);
+
+		/** Copy constructor (deep copy). */
+		MessageHandling(	const MessageHandling& rhs	/**< Rhs object. */
+							);
+
+		/** Destructor. */
+		~MessageHandling( );
+
+		/** Assignment operator (deep copy). */
+		MessageHandling& operator=(	const MessageHandling& rhs	/**< Rhs object. */
+									);
+
+
+		/** Prints an error message(a simplified macro THROWERROR is also provided). \n
+		 *  Errors are definied as abnormal events which cause an immediate termination of the current (sub) function.
+		 *  Errors of a sub function should be commented by the calling function by means of a warning message
+		 *  (if this error does not cause an error of the calling function, either)!
+		 *  \return Error number returned by sub function call
+		 */
+		returnValue throwError(	returnValue Enumber,					/**< Error number returned by sub function call. */
+								const char* additionaltext,				/**< Additional error text (0, if none). */
+								const char* functionname,				/**< Name of function which caused the error. */
+								const char* filename,					/**< Name of file which caused the error. */
+								const unsigned long linenumber,			/**< Number of line which caused the error.incompatible binary file */
+								VisibilityStatus localVisibilityStatus	/**< Determines (locally) if error message can be printed to stdFile.
+																	*   If GLOBAL visibility status of the message is set to VS_HIDDEN,
+																	*   no message is printed, anyway! */
+								);
+
+		/** Prints a warning message (a simplified macro THROWWARNING is also provided).
+		 *  Warnings are definied as abnormal events which does NOT cause an immediate termination of the current (sub) function.
+		 *  \return Warning number returned by sub function call
+		 */
+		returnValue throwWarning(	returnValue Wnumber,	 				/**< Warning number returned by sub function call. */
+									const char* additionaltext,				/**< Additional warning text (0, if none). */
+									const char* functionname,				/**< Name of function which caused the warning. */
+									const char* filename,   				/**< Name of file which caused the warning. */
+									const unsigned long linenumber,	 		/**< Number of line which caused the warning. */
+									VisibilityStatus localVisibilityStatus	/**< Determines (locally) if warning message can be printed to stdFile.
+																			*   If GLOBAL visibility status of the message is set to VS_HIDDEN,
+																			*   no message is printed, anyway! */
+									);
+
+	   /** Prints a info message (a simplified macro THROWINFO is also provided).
+		 *  \return Info number returned by sub function call
+		 */
+		returnValue throwInfo(	returnValue Inumber,	 				/**< Info number returned by sub function call. */
+								const char* additionaltext,	 			/**< Additional warning text (0, if none). */
+								const char* functionname,				/**< Name of function which submitted the info. */
+								const char* filename,   				/**< Name of file which submitted the info. */
+								const unsigned long linenumber,			/**< Number of line which submitted the info. */
+								VisibilityStatus localVisibilityStatus	/**< Determines (locally) if info message can be printed to stdFile.
+																		*   If GLOBAL visibility status of the message is set to VS_HIDDEN,
+																		*   no message is printed, anyway! */
+								);
+
+
+		/** Resets all preferences to default values.
+		 *	\return SUCCESSFUL_RETURN */
+		returnValue reset( );
+
+
+		/** Prints a complete list of all messages to output file.
+		 *	\return SUCCESSFUL_RETURN */
+		returnValue listAllMessages( );
+
+
+		/** Returns visibility status for error messages.
+		 *	\return Visibility status for error messages. */
+		inline VisibilityStatus getErrorVisibilityStatus( ) const;
+
+		/** Returns visibility status for warning messages.
+		 *	\return Visibility status for warning messages. */
+		inline VisibilityStatus getWarningVisibilityStatus( ) const;
+
+		/** Returns visibility status for info messages.
+		 *	\return Visibility status for info messages. */
+		inline VisibilityStatus getInfoVisibilityStatus( ) const;
+
+		/** Returns pointer to output file.
+		 *	\return Pointer to output file. */
+		inline FILE* getOutputFile( ) const;
+
+		/** Returns error count value.
+		 *	\return Error count value. */
+		inline int getErrorCount( ) const;
+
+
+		/** Changes visibility status for error messages. */
+		inline void setErrorVisibilityStatus(	VisibilityStatus _errorVisibility	/**< New visibility status for error messages. */
+												);
+
+		/** Changes visibility status for warning messages. */
+		inline void setWarningVisibilityStatus(	VisibilityStatus _warningVisibility	/**< New visibility status for warning messages. */
+												);
+
+		/** Changes visibility status for info messages. */
+		inline void setInfoVisibilityStatus(	VisibilityStatus _infoVisibility	/**< New visibility status for info messages. */
+												);
+
+		/** Changes output file for messages. */
+		inline void setOutputFile(	FILE* _outputFile	/**< New output file for messages. */
+									);
+
+		/** Changes error count.
+		 * \return SUCCESSFUL_RETURN \n
+		 *		   RET_INVALID_ARGUMENT */
+		inline returnValue setErrorCount(	int _errorCount	/**< New error count value. */
+											);
+
+		/** Provides message text corresponding to given \a returnValue.
+		 * \return String containing message text. */
+		static const char* getErrorCodeMessage(	const returnValue _returnValue
+												);
+
+
+	/*
+	 *	PROTECTED MEMBER FUNCTIONS
+	 */
+	protected:
+		/** Prints a info message to stdFile (auxiliary function).
+		 *  \return Error/warning/info number returned by sub function call
+		 */
+		returnValue throwMessage(
+			returnValue RETnumber,	 				/**< Error/warning/info number returned by sub function call. */
+			const char* additionaltext,				/**< Additional warning text (0, if none). */
+			const char* functionname,				/**< Name of function which caused the error/warning/info. */
+			const char* filename,   				/**< Name of file which caused the error/warning/info. */
+			const unsigned long linenumber,			/**< Number of line which caused the error/warning/info. */
+			VisibilityStatus localVisibilityStatus,	/**< Determines (locally) if info message can be printed to stdFile.
+					  								 *   If GLOBAL visibility status of the message is set to VS_HIDDEN,
+				   									 *   no message is printed, anyway! */
+			const char* RETstring					/**< Leading string of error/warning/info message. */
+			);
+
+
+	/*
+	 *	PROTECTED MEMBER VARIABLES
+	 */
+	protected:
+		VisibilityStatus errorVisibility;		/**< Error messages visible? */
+		VisibilityStatus warningVisibility;		/**< Warning messages visible? */
+		VisibilityStatus infoVisibility;		/**< Info messages visible? */
+
+		FILE* outputFile;						/**< Output file for messages. */
+
+		int errorCount; 						/**< Counts number of errors (for nicer output only). */
+};
+
+
+#ifndef __FUNCTION__
+  /** Ensures that __FUNCTION__ macro is defined. */
+  #define __FUNCTION__ 0
+#endif
+
+#ifndef __FILE__
+  /** Ensures that __FILE__ macro is defined. */
+  #define __FILE__ 0
+#endif
+
+#ifndef __LINE__
+  /** Ensures that __LINE__ macro is defined. */
+  #define __LINE__ 0
+#endif
+
+
+/** Short version of throwError with default values, only returnValue is needed */
+#define THROWERROR(retval) ( getGlobalMessageHandler( )->throwError((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
+
+/** Short version of throwWarning with default values, only returnValue is needed */
+#define THROWWARNING(retval) ( getGlobalMessageHandler( )->throwWarning((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
+
+/** Short version of throwInfo with default values, only returnValue is needed */
+#define THROWINFO(retval) ( getGlobalMessageHandler( )->throwInfo((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
+
+
+/** Returns a pointer to global message handler.
+ *  \return Pointer to global message handler.
+ */
+MessageHandling* getGlobalMessageHandler( );
+
+
+END_NAMESPACE_QPOASES
+
+#include <qpOASES/MessageHandling.ipp>
+
+#endif /* QPOASES_MESSAGEHANDLING_HPP */
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/MessageHandling.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/MessageHandling.ipp
new file mode 100644
index 00000000000..69b17651cb7
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/MessageHandling.ipp
@@ -0,0 +1,144 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/MessageHandling.ipp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of inlined member functions of the MessageHandling class.
+ */
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+/*
+ *	g e t E r r o r V i s i b i l i t y S t a t u s
+ */
+inline VisibilityStatus MessageHandling::getErrorVisibilityStatus( ) const
+{
+ 	return errorVisibility;
+}
+
+
+/*
+ *	g e t W a r n i n g V i s i b i l i t y S t a t u s
+ */
+inline VisibilityStatus MessageHandling::getWarningVisibilityStatus( ) const
+{
+ 	return warningVisibility;
+}
+
+
+/*
+ *	g e t I n f o V i s i b i l i t y S t a t u s
+ */
+inline VisibilityStatus MessageHandling::getInfoVisibilityStatus( ) const
+{
+ 	return infoVisibility;
+}
+
+
+/*
+ *	g e t O u t p u t F i l e
+ */
+inline FILE* MessageHandling::getOutputFile( ) const
+{
+ 	return outputFile;
+}
+
+
+/*
+ *	g e t E r r o r C o u n t
+ */
+inline int MessageHandling::getErrorCount( ) const
+{
+ 	return errorCount;
+}
+
+
+/*
+ *	s e t E r r o r V i s i b i l i t y S t a t u s
+ */
+inline void MessageHandling::setErrorVisibilityStatus( VisibilityStatus _errorVisibility )
+{
+ 	errorVisibility = _errorVisibility;
+}
+
+
+/*
+ *	s e t W a r n i n g V i s i b i l i t y S t a t u s
+ */
+inline void MessageHandling::setWarningVisibilityStatus( VisibilityStatus _warningVisibility )
+{
+ 	warningVisibility = _warningVisibility;
+}
+
+
+/*
+ *	s e t I n f o V i s i b i l i t y S t a t u s
+ */
+inline void MessageHandling::setInfoVisibilityStatus( VisibilityStatus _infoVisibility )
+{
+ 	infoVisibility = _infoVisibility;
+}
+
+
+/*
+ *	s e t O u t p u t F i l e
+ */
+inline void MessageHandling::setOutputFile( FILE* _outputFile )
+{
+ 	outputFile = _outputFile;
+}
+
+
+/*
+ *	s e t E r r o r C o u n t
+ */
+inline returnValue MessageHandling::setErrorCount( int _errorCount )
+{
+	if ( _errorCount >= -1 )
+	{
+		errorCount = _errorCount;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+		return RET_INVALID_ARGUMENTS;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Options.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Options.hpp
new file mode 100644
index 00000000000..83c520cadd0
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Options.hpp
@@ -0,0 +1,170 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/Options.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Declaration of the Options class designed to manage user-specified
+ *	options for solving a QProblem.
+ */
+
+
+#ifndef QPOASES_OPTIONS_HPP
+#define QPOASES_OPTIONS_HPP
+
+
+#include <qpOASES/Utils.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/**
+ *	\brief Manages all user-specified options for solving QPs.
+ *
+ *	This class manages all user-specified options used for solving
+ *	quadratic programs.
+ *
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ */
+class Options
+{
+	/*
+	 *	PUBLIC MEMBER FUNCTIONS
+	 */
+	public:
+		/** Default constructor. */
+		Options( );
+
+		/** Copy constructor (deep copy). */
+		Options(	const Options& rhs			/**< Rhs object. */
+					);
+
+		/** Destructor. */
+		~Options( );
+
+		/** Assignment operator (deep copy). */
+		Options& operator=(	const Options& rhs	/**< Rhs object. */
+							);
+
+
+		/** Sets all options to default values.
+ 		 *	\return SUCCESSFUL_RETURN */
+		returnValue setToDefault( );
+
+		/** Sets all options to values resulting in maximum reliabilty.
+		 *	\return SUCCESSFUL_RETURN */
+		returnValue setToReliable( );
+
+		/** Sets all options to values resulting in minimum solution time.
+ 		 *	\return SUCCESSFUL_RETURN */
+		returnValue setToMPC( );
+
+		/** Same as setToMPC( ), for ensuring backwards compatibility.
+ 		 *	\return SUCCESSFUL_RETURN */
+		returnValue setToFast( );
+
+
+		/** Ensures that all options have consistent values by automatically
+		 *	adjusting inconsistent ones.
+		 *	Note: This routine cannot (and does not try to) ensure that values
+		 *	      are set to reasonable values that make the QP solution work!
+		 *	\return SUCCESSFUL_RETURN \n 
+		 *          RET_OPTIONS_ADJUSTED */
+		returnValue ensureConsistency( );
+
+
+		/** Prints values of all options.
+		 *	\return SUCCESSFUL_RETURN */
+		returnValue print( ) const;
+
+
+	/*
+	 *	PROTECTED MEMBER FUNCTIONS
+	 */
+	protected:
+
+		/** Copies all members from given rhs object.
+		 *  \return SUCCESSFUL_RETURN */
+		returnValue copy(	const Options& rhs	/**< Rhs object. */
+							);
+
+	/*
+	 *	PUBLIC MEMBER VARIABLES
+	 */
+	public:
+		PrintLevel printLevel;					/**< Print level. */
+
+		BooleanType enableRamping;				/**< Specifies whether ramping shall be enabled or not. */
+		BooleanType enableFarBounds;			/**< Specifies whether far bounds shall be used or not. */
+		BooleanType enableFlippingBounds;		/**< Specifies whether flipping bounds shall be used or not. */
+		BooleanType enableRegularisation;		/**< Specifies whether Hessian matrix shall be regularised in case semi-definiteness is detected. */
+		BooleanType enableFullLITests;			/**< Specifies whether condition-hardened LI test shall be used or not. */
+		BooleanType enableNZCTests;				/**< Specifies whether nonzero curvature tests shall be used. */
+		int         enableDriftCorrection;		/**< Specifies the frequency of drift corrections (0 = off). */
+		int enableCholeskyRefactorisation;		/**< Specifies the frequency of full refactorisation of proj. Hessian (otherwise updates). */
+		BooleanType enableEqualities;			/**< Specifies whether equalities shall be always treated as active constraints. */
+
+		real_t terminationTolerance;			/**< Termination tolerance. */
+		real_t boundTolerance;					/**< Lower/upper (constraints') bound tolerance (an inequality constraint whose lower and upper bounds differ by less is regarded to be an equality constraint). */
+		real_t boundRelaxation;					/**< Offset for relaxing (constraints') bounds at beginning of an initial homotopy. It is also as initial value for far bounds. */
+		real_t epsNum;							/**< Numerator tolerance for ratio tests. */
+		real_t epsDen;							/**< Denominator tolerance for ratio tests. */
+		real_t maxPrimalJump;					/**< Maximum allowed jump in primal variables in nonzero curvature tests. */
+		real_t maxDualJump;						/**< Maximum allowed jump in dual variables in linear independence tests. */
+
+		real_t initialRamping;					/**< Start value for Ramping Strategy. */
+		real_t finalRamping;					/**< Final value for Ramping Strategy. */
+		real_t initialFarBounds;				/**< Initial size of Far Bounds. */
+		real_t growFarBounds;					/**< Factor to grow Far Bounds. */
+		SubjectToStatus initialStatusBounds;	/**< Initial status of bounds at first iteration. */
+		real_t epsFlipping;						/**< Tolerance of squared Cholesky diagonal factor which triggers flipping bound. */
+		int numRegularisationSteps;				/**< Maximum number of successive regularisation steps. */
+		real_t epsRegularisation;				/**< Scaling factor of identity matrix used for Hessian regularisation. */
+		int numRefinementSteps;					/**< Maximum number of iterative refinement steps. */
+		real_t epsIterRef;						/**< Early termination tolerance for iterative refinement. */
+		real_t epsLITests;						/**< Tolerance for linear independence tests. */
+		real_t epsNZCTests;						/**< Tolerance for nonzero curvature tests. */
+
+		BooleanType enableDropInfeasibles;		/**< ... */
+		int    dropBoundPriority;				/**< ... */
+        int    dropEqConPriority;				/**< ... */
+        int    dropIneqConPriority;				/**< ... */
+};
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_OPTIONS_HPP */
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblem.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblem.hpp
new file mode 100644
index 00000000000..6e2232fdcf5
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblem.hpp
@@ -0,0 +1,1163 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/QProblem.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Declaration of the QProblem class which is able to use the newly
+ *	developed online active set strategy for parametric quadratic programming.
+ */
+
+
+
+#ifndef QPOASES_QPROBLEM_HPP
+#define QPOASES_QPROBLEM_HPP
+
+
+#include <qpOASES/QProblemB.hpp>
+#include <qpOASES/Constraints.hpp>
+#include <qpOASES/ConstraintProduct.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/**
+ *	\brief Implements the online active set strategy for QPs with general constraints.
+ *
+ *	A class for setting up and solving quadratic programs. The main feature is
+ *	the possibily to use the newly developed online active set strategy for
+ * 	parametric quadratic programming.
+ *
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ */
+class QProblem : public QProblemB
+{
+	/* allow SolutionAnalysis class to access private members */
+	friend class SolutionAnalysis;
+
+	/*
+	 *	PUBLIC MEMBER FUNCTIONS
+	 */
+	public:
+		/** Default constructor. */
+		QProblem( );
+
+		/** Constructor which takes the QP dimension and Hessian type
+		 *  information. If the Hessian is the zero (i.e. HST_ZERO) or the
+		 *  identity matrix (i.e. HST_IDENTITY), respectively, no memory
+		 *  is allocated for it and a NULL pointer can be passed for it
+		 *  to the init() functions. */
+		QProblem(	int _nV,	  							/**< Number of variables. */
+					int _nC,		  						/**< Number of constraints. */
+					HessianType _hessianType = HST_UNKNOWN	/**< Type of Hessian matrix. */
+					);
+
+		/** Copy constructor (deep copy). */
+		QProblem(	const QProblem& rhs		/**< Rhs object. */
+					);
+
+		/** Destructor. */
+		virtual ~QProblem( );
+
+		/** Assignment operator (deep copy). */
+		QProblem& operator=(	const QProblem& rhs	/**< Rhs object. */
+								);
+
+
+		/** Clears all data structures of QProblemB except for QP data.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_RESET_FAILED */
+		virtual returnValue reset( );
+
+
+		/** Initialises a QProblem with given QP data and solves it
+		 *	using an initial homotopy with empty working set (at most nWSR iterations).
+		 *  Note: This function internally calls solveInitialQP for initialisation!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INIT_FAILED \n
+					RET_INIT_FAILED_CHOLESKY \n
+					RET_INIT_FAILED_TQ \n
+					RET_INIT_FAILED_HOTSTART \n
+					RET_INIT_FAILED_INFEASIBILITY \n
+					RET_INIT_FAILED_UNBOUNDEDNESS \n
+					RET_MAX_NWSR_REACHED \n
+					RET_INVALID_ARGUMENTS */
+		returnValue init(	SymmetricMatrix *_H,		/**< Hessian matrix (a shallow copy is made). */
+							const real_t* const _g, 	/**< Gradient vector. */
+							Matrix *_A,  				/**< Constraint matrix (a shallow copy is made). */
+							const real_t* const _lb,	/**< Lower bound vector (on variables). \n
+															 If no lower bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ub,	/**< Upper bound vector (on variables). \n
+															 If no upper bounds exist, a NULL pointer can be passed. */
+							const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
+															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ubA,	/**< Upper constraints' bound vector. \n
+															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							int& nWSR,					/**< Input: Maximum number of working set recalculations when using initial homotopy.
+															 Output: Number of performed working set recalculations. */
+							real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP initialisation. \n
+															 Output: CPU time spend for QP initialisation (if pointer passed). */
+							);
+
+
+		/** Initialises a QProblem with given QP data and solves it
+		 *	using an initial homotopy with empty working set (at most nWSR iterations).
+		 *  Note: This function internally calls solveInitialQP for initialisation!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INIT_FAILED \n
+					RET_INIT_FAILED_CHOLESKY \n
+					RET_INIT_FAILED_TQ \n
+					RET_INIT_FAILED_HOTSTART \n
+					RET_INIT_FAILED_INFEASIBILITY \n
+					RET_INIT_FAILED_UNBOUNDEDNESS \n
+					RET_MAX_NWSR_REACHED \n
+					RET_INVALID_ARGUMENTS */
+		returnValue init(	const real_t* const _H, 	/**< Hessian matrix (a shallow copy is made). \n
+															 If Hessian matrix is trivial, a NULL pointer can be passed. */
+							const real_t* const _g, 	/**< Gradient vector. */
+							const real_t* const _A,  	/**< Constraint matrix (a shallow copy is made). */
+							const real_t* const _lb,	/**< Lower bound vector (on variables). \n
+															 If no lower bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ub,	/**< Upper bound vector (on variables). \n
+															 If no upper bounds exist, a NULL pointer can be passed. */
+							const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
+															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ubA,	/**< Upper constraints' bound vector. \n
+															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							int& nWSR,					/**< Input: Maximum number of working set recalculations when using initial homotopy.
+															 Output: Number of performed working set recalculations. */
+							real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP initialisation. \n
+															 Output: CPU time spend for QP initialisation (if pointer passed). */
+							);
+
+		/** Initialises a QProblem with given QP data to be read from files and solves it
+		 *	using an initial homotopy with empty working set (at most nWSR iterations).
+		 *  Note: This function internally calls solveInitialQP for initialisation!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INIT_FAILED \n
+					RET_INIT_FAILED_CHOLESKY \n
+					RET_INIT_FAILED_TQ \n
+					RET_INIT_FAILED_HOTSTART \n
+					RET_INIT_FAILED_INFEASIBILITY \n
+					RET_INIT_FAILED_UNBOUNDEDNESS \n
+					RET_MAX_NWSR_REACHED \n
+					RET_UNABLE_TO_READ_FILE */
+		returnValue init(	const char* const H_file,	/**< Name of file where Hessian matrix is stored. \n
+															 If Hessian matrix is trivial, a NULL pointer can be passed. */
+							const char* const g_file,	/**< Name of file where gradient vector is stored. */
+							const char* const A_file,	/**< Name of file where constraint matrix is stored. */
+							const char* const lb_file,	/**< Name of file where lower bound vector. \n
+															If no lower bounds exist, a NULL pointer can be passed. */
+							const char* const ub_file,	/**< Name of file where upper bound vector. \n
+															If no upper bounds exist, a NULL pointer can be passed. */
+							const char* const lbA_file,	/**< Name of file where lower constraints' bound vector. \n
+															If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							const char* const ubA_file,	/**< Name of file where upper constraints' bound vector. \n
+															 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+							int& nWSR,					/**< Input: Maximum number of working set recalculations when using initial homotopy.
+															 Output: Number of performed working set recalculations. */
+							real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP initialisation. \n
+															 Output: CPU time spend for QP initialisation (if pointer passed). */
+							);
+
+		/** Initialises a QProblem with given QP data and solves it
+		 *  depending on the parameter constellation: \n
+		 *	1. 0,    0,    0 : start with xOpt = 0, yOpt = 0 and IB empty (or all implicit equality bounds), \n
+		 *	2. xOpt, 0,    0 : start with xOpt, yOpt = 0 and obtain IB by "clipping", \n
+		 *	3. 0,    yOpt, 0 : start with xOpt = 0, yOpt and obtain IB from yOpt != 0, \n
+		 *	4. 0,    0,    IB: start with xOpt = 0, yOpt = 0 and IB, \n
+		 *	5. xOpt, yOpt, 0 : start with xOpt, yOpt and obtain IB from yOpt != 0, \n
+		 *	6. xOpt, 0,    IB: start with xOpt, yOpt = 0 and IB, \n
+		 *	7. xOpt, yOpt, IB: start with xOpt, yOpt and IB (assume them to be consistent!)
+		 *  Note: This function internally calls solveInitialQP for initialisation!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INIT_FAILED \n
+					RET_INIT_FAILED_CHOLESKY \n
+					RET_INIT_FAILED_TQ \n
+					RET_INIT_FAILED_HOTSTART \n
+					RET_INIT_FAILED_INFEASIBILITY \n
+					RET_INIT_FAILED_UNBOUNDEDNESS \n
+					RET_MAX_NWSR_REACHED \n
+					RET_INVALID_ARGUMENTS */
+		returnValue init(	SymmetricMatrix *_H, 						/**< Hessian matrix (a shallow copy is made). */
+							const real_t* const _g, 					/**< Gradient vector. */
+							Matrix *_A, 			 					/**< Constraint matrix (a shallow copy is made). */
+							const real_t* const _lb,					/**< Lower bound vector (on variables). \n
+																			 If no lower bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ub,					/**< Upper bound vector (on variables). \n
+																			 If no upper bounds exist, a NULL pointer can be passed. */
+							const real_t* const _lbA,					/**< Lower constraints' bound vector. \n
+																			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ubA,					/**< Upper constraints' bound vector. \n
+																		 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							int& nWSR,									/**< Input: Maximum number of working set recalculations when using initial homotopy.
+																		 *	 Output: Number of performed working set recalculations. */
+							real_t* const cputime,						/**< Input: Maximum CPU time allowed for QP initialisation. \n
+																			 Output: CPU time spend for QP initialisation. */
+							const real_t* const xOpt,					/**< Optimal primal solution vector. \n
+																			 (If a null pointer is passed, the old primal solution is kept!) */
+							const real_t* const yOpt,					/**< Optimal dual solution vector. \n
+																			 (If a null pointer is passed, the old dual solution is kept!) */
+							const Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
+							const Constraints* const guessedConstraints	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
+							);
+
+		/** Initialises a QProblem with given QP data and solves it
+		 *  depending on the parameter constellation: \n
+		 *	1. 0,    0,    0 : start with xOpt = 0, yOpt = 0 and IB empty (or all implicit equality bounds), \n
+		 *	2. xOpt, 0,    0 : start with xOpt, yOpt = 0 and obtain IB by "clipping", \n
+		 *	3. 0,    yOpt, 0 : start with xOpt = 0, yOpt and obtain IB from yOpt != 0, \n
+		 *	4. 0,    0,    IB: start with xOpt = 0, yOpt = 0 and IB, \n
+		 *	5. xOpt, yOpt, 0 : start with xOpt, yOpt and obtain IB from yOpt != 0, \n
+		 *	6. xOpt, 0,    IB: start with xOpt, yOpt = 0 and IB, \n
+		 *	7. xOpt, yOpt, IB: start with xOpt, yOpt and IB (assume them to be consistent!)
+		 *  Note: This function internally calls solveInitialQP for initialisation!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INIT_FAILED \n
+					RET_INIT_FAILED_CHOLESKY \n
+					RET_INIT_FAILED_TQ \n
+					RET_INIT_FAILED_HOTSTART \n
+					RET_INIT_FAILED_INFEASIBILITY \n
+					RET_INIT_FAILED_UNBOUNDEDNESS \n
+					RET_MAX_NWSR_REACHED \n
+					RET_INVALID_ARGUMENTS */
+		returnValue init(	const real_t* const _H, 					/**< Hessian matrix (a shallow copy is made). \n
+																     		 If Hessian matrix is trivial, a NULL pointer can be passed. */
+							const real_t* const _g, 					/**< Gradient vector. */
+							const real_t* const _A,  					/**< Constraint matrix (a shallow copy is made). */
+							const real_t* const _lb,					/**< Lower bound vector (on variables). \n
+																			 If no lower bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ub,					/**< Upper bound vector (on variables). \n
+																			 If no upper bounds exist, a NULL pointer can be passed. */
+							const real_t* const _lbA,					/**< Lower constraints' bound vector. \n
+																			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ubA,					/**< Upper constraints' bound vector. \n
+																		 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							int& nWSR,									/**< Input: Maximum number of working set recalculations when using initial homotopy.
+																		 *	 Output: Number of performed working set recalculations. */
+							real_t* const cputime,						/**< Input: Maximum CPU time allowed for QP initialisation. \n
+																			 Output: CPU time spend for QP initialisation. */
+							const real_t* const xOpt,					/**< Optimal primal solution vector. \n
+																			 (If a null pointer is passed, the old primal solution is kept!) */
+							const real_t* const yOpt,					/**< Optimal dual solution vector. \n
+																			 (If a null pointer is passed, the old dual solution is kept!) */
+							const Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
+							const Constraints* const guessedConstraints	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
+							);
+
+		/** Initialises a QProblem with given QP data to be read from files and solves it
+		 *  depending on the parameter constellation: \n
+		 *  Note: This function internally calls solveInitialQP for initialisation!
+		 *	1. 0,    0,    0 : start with xOpt = 0, yOpt = 0 and IB empty (or all implicit equality bounds), \n
+		 *	2. xOpt, 0,    0 : start with xOpt, yOpt = 0 and obtain IB by "clipping", \n
+		 *	3. 0,    yOpt, 0 : start with xOpt = 0, yOpt and obtain IB from yOpt != 0, \n
+		 *	4. 0,    0,    IB: start with xOpt = 0, yOpt = 0 and IB, \n
+		 *	5. xOpt, yOpt, 0 : start with xOpt, yOpt and obtain IB from yOpt != 0, \n
+		 *	6. xOpt, 0,    IB: start with xOpt, yOpt = 0 and IB, \n
+		 *	7. xOpt, yOpt, IB: start with xOpt, yOpt and IB (assume them to be consistent!)
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INIT_FAILED \n
+					RET_INIT_FAILED_CHOLESKY \n
+					RET_INIT_FAILED_TQ \n
+					RET_INIT_FAILED_HOTSTART \n
+					RET_INIT_FAILED_INFEASIBILITY \n
+					RET_INIT_FAILED_UNBOUNDEDNESS \n
+					RET_MAX_NWSR_REACHED \n
+					RET_UNABLE_TO_READ_FILE */
+		returnValue init(	const char* const H_file,					/**< Name of file where Hessian matrix is stored. \n
+																     		 If Hessian matrix is trivial, a NULL pointer can be passed. */
+							const char* const g_file,					/**< Name of file where gradient vector is stored. */
+							const char* const A_file,					/**< Name of file where constraint matrix is stored. */
+							const char* const lb_file,					/**< Name of file where lower bound vector. \n
+																			 If no lower bounds exist, a NULL pointer can be passed. */
+							const char* const ub_file,					/**< Name of file where upper bound vector. \n
+																			 If no upper bounds exist, a NULL pointer can be passed. */
+							const char* const lbA_file,					/**< Name of file where lower constraints' bound vector. \n
+																			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							const char* const ubA_file,					/**< Name of file where upper constraints' bound vector. \n
+																			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+							int& nWSR,									/**< Input: Maximum number of working set recalculations when using initial homotopy.
+																			 Output: Number of performed working set recalculations. */
+							real_t* const cputime,						/**< Input: Maximum CPU time allowed for QP initialisation. \n
+																			 Output: CPU time spend for QP initialisation. */
+							const real_t* const xOpt,					/**< Optimal primal solution vector. \n
+																			 (If a null pointer is passed, the old primal solution is kept!) */
+							const real_t* const yOpt,					/**< Optimal dual solution vector. \n
+																			 (If a null pointer is passed, the old dual solution is kept!) */
+							const Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
+							const Constraints* const guessedConstraints	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
+							);
+
+
+		/** Solves QProblem using online active set strategy.
+		 *  Note: This function internally calls solveQP/solveRegularisedQP
+		 *        for solving an initialised QP!
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MAX_NWSR_REACHED \n
+		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+		returnValue hotstart(	const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const lb_new,		/**< Lower bounds of neighbouring QP to be solved. \n
+													 			 	 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
+													 			 	 If no upper bounds exist, a NULL pointer can be passed. */
+								const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+													 			 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
+													 			 	 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int& nWSR,						/**< Input: Maximum number of working set recalculations; \n
+															 		 Output: Number of performed working set recalculations. */
+								real_t* const cputime = 0		/**< Input: Maximum CPU time allowed for QP solution. \n
+																 	 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								);
+
+		/** Solves QProblem using online active set strategy
+		 *  reading QP data from files.
+		 *  Note: This function internally calls solveQP/solveRegularisedQP
+		 *        for solving an initialised QP!
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MAX_NWSR_REACHED \n
+		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+					RET_UNABLE_TO_READ_FILE \n
+					RET_INVALID_ARGUMENTS */
+		returnValue hotstart(	const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+								const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+													 			 If no lower bounds exist, a NULL pointer can be passed. */
+								const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+													 			 If no upper bounds exist, a NULL pointer can be passed. */
+								const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
+													 			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
+													 			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int& nWSR, 					/**< Input: Maximum number of working set recalculations; \n
+																 Output: Number of performed working set recalculations. */
+								real_t* const cputime = 0		/**< Input: Maximum CPU time allowed for QP solution. \n
+																 	 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								);
+
+		/** Solves an initialised QProblem using online active set strategy (using an initialised homotopy).
+		 *  Note: This function internally calls solveQP/solveRegularisedQP
+		 *        for solving an initialised QP!
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MAX_NWSR_REACHED \n
+		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+					RET_SETUP_AUXILIARYQP_FAILED */
+		returnValue hotstart(	const real_t* const g_new,					/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const lb_new,					/**< Lower bounds of neighbouring QP to be solved. \n
+													 						 	 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,					/**< Upper bounds of neighbouring QP to be solved. \n
+													 						 	 If no upper bounds exist, a NULL pointer can be passed. */
+								const real_t* const lbA_new,				/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+													 						 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const real_t* const ubA_new,				/**< Upper constraints' bounds of neighbouring QP to be solved. \n
+													 							 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int& nWSR,									/**< Input: Maximum number of working set recalculations; \n
+																				 Output: Number of performed working set recalculations. */
+								real_t* const cputime,						/**< Input: Maximum CPU time allowed for QP solution. \n
+																			 	 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								const Bounds* const guessedBounds,			/**< Initial guess for working set of bounds.
+																			 *   A null pointer corresponds to an empty working set! */
+								const Constraints* const guessedConstraints	/**< Initial guess for working set of constraints.
+																			 *   A null pointer corresponds to an empty working set! */
+								);
+
+		/** Solves an initialised QProblem using online active set strategy (using an initialised homotopy)
+		 *  reading QP data from files.
+		 *  Note: This function internally calls solveQP/solveRegularisedQP
+		 *        for solving an initialised QP!
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MAX_NWSR_REACHED \n
+		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+					RET_SETUP_AUXILIARYQP_FAILED \n
+					RET_UNABLE_TO_READ_FILE \n
+					RET_INVALID_ARGUMENTS */
+		returnValue hotstart(	const char* const g_file, 					/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+								const char* const lb_file, 					/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+													 							 If no lower bounds exist, a NULL pointer can be passed. */
+								const char* const ub_file, 					/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+													 							 If no upper bounds exist, a NULL pointer can be passed. */
+								const char* const lbA_file, 				/**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
+													 							 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const char* const ubA_file, 				/**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
+													 							 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int& nWSR,									/**< Input: Maximum number of working set recalculations; \n
+																		 		 Output: Number of performed working set recalculations. */
+								real_t* const cputime,						/**< Input: Maximum CPU time allowed for QP solution. \n
+																			 	 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								const Bounds* const guessedBounds,			/**< Initial guess for working set of bounds.
+																			 *   A null pointer corresponds to an empty working set! */
+								const Constraints* const guessedConstraints	/**< Initial guess for working set of constraints.
+																			 *   A null pointer corresponds to an empty working set! */
+								);
+
+        /** Solves using the current working set
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_STEPDIRECTION_FAILED_TQ \n
+		 *			RET_STEPDIRECTION_FAILED_CHOLESKY */
+        returnValue solveCurrentEQP (	const int n_rhs,			/**< Number of consecutive right hand sides */
+										const real_t* g_in,			/**< Gradient of neighbouring QP to be solved. */
+										const real_t* lb_in,		/**< Lower bounds of neighbouring QP to be solved. \n
+																		 If no lower bounds exist, a NULL pointer can be passed. */
+										const real_t* ub_in,		/**< Upper bounds of neighbouring QP to be solved. \n
+																		 If no upper bounds exist, a NULL pointer can be passed. */
+										const real_t* lbA_in,		/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+																		 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+										const real_t* ubA_in,		/**< Upper constraints' bounds of neighbouring QP to be solved. \n */
+										real_t* x_out,				/**< Primal solution */
+										real_t* y_out				/**< Dual solution */
+										);
+
+		/** Writes a vector with the state of the (constraints') bounds
+		 *	\return SUCCESSFUL_RETURN */
+		returnValue getWorkingSet(	real_t* workingSet				/** Output: array containing state of the working set. */
+									);
+
+		/** Returns current constraints object of the QP (deep copy).
+		  *	\return SUCCESSFUL_RETURN \n
+		  			RET_QPOBJECT_NOT_SETUP */
+		inline returnValue getConstraints(	Constraints& _constraints	/** Output: Constraints object. */
+											) const;
+
+
+		/** Returns the number of constraints.
+		 *	\return Number of constraints. */
+		inline int getNC( ) const;
+
+		/** Returns the number of (implicitly defined) equality constraints.
+		 *	\return Number of (implicitly defined) equality constraints. */
+		inline int getNEC( ) const;
+
+		/** Returns the number of active constraints.
+		 *	\return Number of active constraints. */
+		inline int getNAC( ) const;
+
+		/** Returns the number of inactive constraints.
+		 *	\return Number of inactive constraints. */
+		inline int getNIAC( ) const;
+
+		/** Returns the dimension of null space.
+		 *	\return Dimension of null space. */
+		virtual int getNZ( ) const;
+
+
+		/** Returns the dual solution vector (deep copy).
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_QP_NOT_SOLVED */
+		virtual returnValue getDualSolution(	real_t* const yOpt	/**< Output: Dual solution vector (if QP has been solved). */
+												) const;
+
+
+		/** Defines user-defined routine for calculating the constraint product A*x
+		 *	\return  SUCCESSFUL_RETURN \n */
+		returnValue setConstraintProduct(	ConstraintProduct* const _constraintProduct
+											);
+
+
+		/** Prints concise list of properties of the current QP.
+		 *	\return  SUCCESSFUL_RETURN \n */
+		virtual returnValue printProperties( );
+
+
+	/*
+	 *	PROTECTED MEMBER FUNCTIONS
+	 */
+	protected:
+		/** Frees all allocated memory.
+		 *  \return SUCCESSFUL_RETURN */
+		returnValue clear( );
+
+		/** Copies all members from given rhs object.
+		 *  \return SUCCESSFUL_RETURN */
+		returnValue copy(	const QProblem& rhs	/**< Rhs object. */
+							);
+
+		/** Solves a QProblem whose QP data is assumed to be stored in the member variables.
+		 *  A guess for its primal/dual optimal solution vectors and the corresponding
+		 *  working sets of bounds and constraints can be provided.
+		 *  Note: This function is internally called by all init functions!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INIT_FAILED \n
+					RET_INIT_FAILED_CHOLESKY \n
+					RET_INIT_FAILED_TQ \n
+					RET_INIT_FAILED_HOTSTART \n
+					RET_INIT_FAILED_INFEASIBILITY \n
+					RET_INIT_FAILED_UNBOUNDEDNESS \n
+					RET_MAX_NWSR_REACHED */
+		returnValue solveInitialQP(	const real_t* const xOpt,						/**< Optimal primal solution vector.
+																					 *	 A NULL pointer can be passed. */
+									const real_t* const yOpt,						/**< Optimal dual solution vector.
+																					 *	 A NULL pointer can be passed. */
+									const Bounds* const guessedBounds,				/**< Guessed working set of bounds for solution (xOpt,yOpt).
+																		 			 *	 A NULL pointer can be passed. */
+									const Constraints* const guessedConstraints,	/**< Optimal working set of constraints for solution (xOpt,yOpt).
+																		 			 *	 A NULL pointer can be passed. */
+									int& nWSR, 										/**< Input: Maximum number of working set recalculations; \n
+																 					 *	 Output: Number of performed working set recalculations. */
+									real_t* const cputime							/**< Input: Maximum CPU time allowed for QP solution. \n
+																			 		 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+									);
+
+		/** Solves QProblem using online active set strategy.
+		 *  Note: This function is internally called by all hotstart functions!
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MAX_NWSR_REACHED \n
+		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+		returnValue solveQP(	const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
+													 			 	 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
+													 			 	 If no upper bounds exist, a NULL pointer can be passed. */
+								const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+													 			 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
+													 			 	 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int& nWSR,						/**< Input: Maximum number of working set recalculations; \n
+															 		 Output: Number of performed working set recalculations. */
+								real_t* const cputime,			/**< Input: Maximum CPU time allowed for QP solution. \n
+																 	 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								int  nWSRperformed = 0			/**< Number of working set recalculations already performed to solve
+																	 this QP within previous solveQP() calls. This number is
+																	 always zero, except for successive calls from solveRegularisedQP()
+																	 or when using the far bound strategy. */
+								);
+
+
+		/** Solves QProblem using online active set strategy.
+		 *  Note: This function is internally called by all hotstart functions!
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MAX_NWSR_REACHED \n
+		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+		returnValue solveRegularisedQP(	const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
+										const real_t* const lb_new,		/**< Lower bounds of neighbouring QP to be solved. \n
+															 			 	 If no lower bounds exist, a NULL pointer can be passed. */
+										const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
+															 			 	 If no upper bounds exist, a NULL pointer can be passed. */
+										const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+															 			 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+										const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
+															 			 	 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+										int& nWSR,						/**< Input: Maximum number of working set recalculations; \n
+																	 		 Output: Number of performed working set recalculations. */
+										real_t* const cputime,			/**< Input: Maximum CPU time allowed for QP solution. \n
+																		 	 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+										int  nWSRperformed = 0			/**< Number of working set recalculations already performed to solve
+																			 this QP within previous solveRegularisedQP() calls. This number is
+																			 always zero, except for successive calls when using the far bound strategy. */
+										);
+
+
+		/** Determines type of existing constraints and bounds (i.e. implicitly fixed, unbounded etc.).
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_SETUPSUBJECTTOTYPE_FAILED */
+		virtual returnValue setupSubjectToType( );
+
+		/** Determines type of new constraints and bounds (i.e. implicitly fixed, unbounded etc.).
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_SETUPSUBJECTTOTYPE_FAILED */
+		virtual returnValue setupSubjectToType(	const real_t* const lb_new,		/**< New lower bounds. */
+												const real_t* const ub_new,		/**< New upper bounds. */
+												const real_t* const lbA_new,	/**< New lower constraints' bounds. */
+												const real_t* const ubA_new		/**< New upper constraints' bounds. */
+												);
+
+		/** Computes the Cholesky decomposition of the projected Hessian (i.e. R^T*R = Z^T*H*Z).
+		 *  Note: If Hessian turns out not to be positive definite, the Hessian type
+		 *		  is set to HST_SEMIDEF accordingly.
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_HESSIAN_NOT_SPD \n
+		 *			RET_INDEXLIST_CORRUPTED */
+		returnValue computeProjectedCholesky( );
+
+		/** Computes initial Cholesky decomposition of the projected Hessian making
+		 *  use of the function computeCholesky() or computeProjectedCholesky().
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_HESSIAN_NOT_SPD \n
+		 *			RET_INDEXLIST_CORRUPTED */
+		virtual returnValue setupInitialCholesky( );
+
+		/** Initialises TQ factorisation of A (i.e. A*Q = [0 T]) if NO constraint is active.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_INDEXLIST_CORRUPTED */
+		returnValue setupTQfactorisation( );
+
+
+		/** Obtains the desired working set for the auxiliary initial QP in
+		 *  accordance with the user specifications
+		 *  (assumes that member AX has already been initialised!)
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_OBTAINING_WORKINGSET_FAILED \n
+					RET_INVALID_ARGUMENTS */
+		returnValue obtainAuxiliaryWorkingSet(	const real_t* const xOpt,						/**< Optimal primal solution vector.
+																								 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
+												const real_t* const yOpt,						/**< Optimal dual solution vector.
+																								 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
+												const Bounds* const guessedBounds,				/**< Guessed working set of bounds for solution (xOpt,yOpt). */
+												const Constraints* const guessedConstraints,	/**< Guessed working set for solution (xOpt,yOpt). */
+												Bounds* auxiliaryBounds,						/**< Input: Allocated bound object. \n
+																								 *	 Ouput: Working set of constraints for auxiliary QP. */
+												Constraints* auxiliaryConstraints				/**< Input: Allocated bound object. \n
+																								 *	 Ouput: Working set for auxiliary QP. */
+												) const;
+
+		/** Sets up bound and constraints data structures according to auxiliaryBounds/Constraints.
+		 *  (If the working set shall be setup afresh, make sure that
+		 *  bounds and constraints data structure have been resetted
+		 *  and the TQ factorisation has been initialised!)
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_SETUP_WORKINGSET_FAILED \n
+					RET_INVALID_ARGUMENTS \n
+					RET_UNKNOWN_BUG */
+		returnValue setupAuxiliaryWorkingSet(	const Bounds* const auxiliaryBounds,			/**< Working set of bounds for auxiliary QP. */
+												const Constraints* const auxiliaryConstraints,	/**< Working set of constraints for auxiliary QP. */
+												BooleanType setupAfresh							/**< Flag indicating if given working set shall be
+																								 *    setup afresh or by updating the current one. */
+												);
+
+		/** Sets up the optimal primal/dual solution of the auxiliary initial QP.
+		 *	\return SUCCESSFUL_RETURN */
+		returnValue setupAuxiliaryQPsolution(	const real_t* const xOpt,			/**< Optimal primal solution vector.
+																				 	*	 If a NULL pointer is passed, all entries are set to zero. */
+												const real_t* const yOpt			/**< Optimal dual solution vector.
+																					 *	 If a NULL pointer is passed, all entries are set to zero. */
+												);
+
+		/** Sets up gradient of the auxiliary initial QP for given
+		 *  optimal primal/dual solution and given initial working set
+		 *  (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
+		 *	\return SUCCESSFUL_RETURN */
+		returnValue setupAuxiliaryQPgradient( );
+
+		/** Sets up (constraints') bounds of the auxiliary initial QP for given
+		 *  optimal primal/dual solution and given initial working set
+		 *  (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_UNKNOWN_BUG */
+		returnValue setupAuxiliaryQPbounds(	const Bounds* const auxiliaryBounds,			/**< Working set of bounds for auxiliary QP. */
+											const Constraints* const auxiliaryConstraints,	/**< Working set of constraints for auxiliary QP. */
+											BooleanType useRelaxation						/**< Flag indicating if inactive (constraints') bounds shall be relaxed. */
+											);
+
+
+		/** Adds a constraint to active set.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_ADDCONSTRAINT_FAILED \n
+					RET_ADDCONSTRAINT_FAILED_INFEASIBILITY \n
+					RET_ENSURELI_FAILED */
+		returnValue addConstraint(	int number,					/**< Number of constraint to be added to active set. */
+									SubjectToStatus C_status,	/**< Status of new active constraint. */
+									BooleanType updateCholesky,	/**< Flag indicating if Cholesky decomposition shall be updated. */
+									BooleanType ensureLI = BT_TRUE	/**< Ensure linear independence by exchange rules by default. */
+									);
+
+		/** Checks if new active constraint to be added is linearly dependent from
+		 *	from row of the active constraints matrix.
+		 *	\return	 RET_LINEARLY_DEPENDENT \n
+		 			 RET_LINEARLY_INDEPENDENT \n
+					 RET_INDEXLIST_CORRUPTED */
+		returnValue addConstraint_checkLI(	int number			/**< Number of constraint to be added to active set. */
+											);
+
+		/** Ensures linear independence of constraint matrix when a new constraint is added.
+		 * 	To this end a bound or constraint is removed simultaneously if necessary.
+		 *	\return	 SUCCESSFUL_RETURN \n
+		 			 RET_LI_RESOLVED \n
+					 RET_ENSURELI_FAILED \n
+					 RET_ENSURELI_FAILED_TQ \n
+					 RET_ENSURELI_FAILED_NOINDEX \n
+					 RET_REMOVE_FROM_ACTIVESET */
+		returnValue addConstraint_ensureLI(	int number,					/**< Number of constraint to be added to active set. */
+											SubjectToStatus C_status	/**< Status of new active bound. */
+											);
+
+		/** Adds a bound to active set.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_ADDBOUND_FAILED \n
+					RET_ADDBOUND_FAILED_INFEASIBILITY \n
+					RET_ENSURELI_FAILED */
+		returnValue addBound(	int number,					/**< Number of bound to be added to active set. */
+								SubjectToStatus B_status,	/**< Status of new active bound. */
+								BooleanType updateCholesky,	/**< Flag indicating if Cholesky decomposition shall be updated. */
+								BooleanType ensureLI = BT_TRUE	/**< Ensure linear independence by exchange rules by default. */
+								);
+
+		/** Checks if new active bound to be added is linearly dependent from
+		 *	from row of the active constraints matrix.
+		 *	\return	 RET_LINEARLY_DEPENDENT \n
+		 			 RET_LINEARLY_INDEPENDENT */
+		returnValue addBound_checkLI(	int number			/**< Number of bound to be added to active set. */
+										);
+
+		/** Ensures linear independence of constraint matrix when a new bound is added.
+		 *	To this end a bound or constraint is removed simultaneously if necessary.
+		 *	\return	 SUCCESSFUL_RETURN \n
+		 			 RET_LI_RESOLVED \n
+					 RET_ENSURELI_FAILED \n
+					 RET_ENSURELI_FAILED_TQ \n
+					 RET_ENSURELI_FAILED_NOINDEX \n
+					 RET_REMOVE_FROM_ACTIVESET */
+		returnValue addBound_ensureLI(	int number,					/**< Number of bound to be added to active set. */
+										SubjectToStatus B_status	/**< Status of new active bound. */
+										);
+
+		/** Removes a constraint from active set.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_CONSTRAINT_NOT_ACTIVE \n
+					RET_REMOVECONSTRAINT_FAILED \n
+					RET_HESSIAN_NOT_SPD */
+		returnValue removeConstraint(	int number,								/**< Number of constraint to be removed from active set. */
+										BooleanType updateCholesky,				/**< Flag indicating if Cholesky decomposition shall be updated. */
+										BooleanType allowFlipping = BT_FALSE,	/**< Flag indicating if flipping bounds are allowed. */
+										BooleanType ensureNZC = BT_FALSE		/**< Flag indicating if non-zero curvature is ensured by exchange rules. */
+										);
+
+		/** Removes a bounds from active set.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_BOUND_NOT_ACTIVE \n
+					RET_HESSIAN_NOT_SPD \n
+					RET_REMOVEBOUND_FAILED */
+		returnValue removeBound(	int number,								/**< Number of bound to be removed from active set. */
+									BooleanType updateCholesky,				/**< Flag indicating if Cholesky decomposition shall be updated. */
+									BooleanType allowFlipping = BT_FALSE,	/**< Flag indicating if flipping bounds are allowed. */
+									BooleanType ensureNZC = BT_FALSE		/**< Flag indicating if non-zero curvature is ensured by exchange rules. */
+									);
+
+
+		/** Performs robustified ratio test yield the maximum possible step length
+		 *  along the homotopy path.
+		 *	\return  SUCCESSFUL_RETURN */
+		returnValue performPlainRatioTest(	int nIdx, 							/**< Number of ratios to be checked. */
+											const int* const idxList, 			/**< Array containing the indices of all ratios to be checked. */
+											const real_t* const num,	 		/**< Array containing all numerators for performing the ratio test. */
+											const real_t* const den,		 	/**< Array containing all denominators for performing the ratio test. */
+											real_t epsNum,						/**< Numerator tolerance. */
+											real_t epsDen,						/**< Denominator tolerance. */
+											real_t& t,							/**< Output: Maximum possible step length along the homotopy path. */
+											int& BC_idx 						/**< Output: Index of blocking constraint. */
+											) const;
+
+
+		/** Ensure non-zero curvature by primal jump.
+		 *  \return SUCCESSFUL_RETURN \n
+		 *          RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+		returnValue ensureNonzeroCurvature(
+				BooleanType removeBoundNotConstraint,	/**< SubjectTo to be removed is a bound. */
+				int remIdx,								/**< Index of bound/constraint to be removed. */
+				BooleanType &exchangeHappened,			/**< Output: Exchange was necessary to ensure. */
+				BooleanType &addBoundNotConstraint,		/**< SubjectTo to be added is a bound. */
+				int &addIdx,							/**< Index of bound/constraint to be added. */
+				SubjectToStatus &addStatus				/**< Status of bound/constraint to be added. */
+				);
+
+
+		/** Solves the system Ta = b or T^Ta = b where T is a reverse upper triangular matrix.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_DIV_BY_ZERO */
+		returnValue backsolveT(	const real_t* const b,	/**< Right hand side vector. */
+								BooleanType transposed,	/**< Indicates if the transposed system shall be solved. */
+								real_t* const a 		/**< Output: Solution vector */
+								) const;
+
+
+		/** Determines step direction of the shift of the QP data.
+		 *	\return SUCCESSFUL_RETURN */
+		returnValue determineDataShift(	const real_t* const g_new,	/**< New gradient vector. */
+										const real_t* const lbA_new,/**< New lower constraints' bounds. */
+										const real_t* const ubA_new,/**< New upper constraints' bounds. */
+										const real_t* const lb_new,	/**< New lower bounds. */
+										const real_t* const ub_new,	/**< New upper bounds. */
+										real_t* const delta_g,	 	/**< Output: Step direction of gradient vector. */
+										real_t* const delta_lbA,	/**< Output: Step direction of lower constraints' bounds. */
+										real_t* const delta_ubA,	/**< Output: Step direction of upper constraints' bounds. */
+										real_t* const delta_lb,	 	/**< Output: Step direction of lower bounds. */
+										real_t* const delta_ub,	 	/**< Output: Step direction of upper bounds. */
+										BooleanType& Delta_bC_isZero,/**< Output: Indicates if active constraints' bounds are to be shifted. */
+										BooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */
+										);
+
+		/** Determines step direction of the homotopy path.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_STEPDIRECTION_FAILED_TQ \n
+					RET_STEPDIRECTION_FAILED_CHOLESKY */
+		returnValue determineStepDirection(	const real_t* const delta_g,	/**< Step direction of gradient vector. */
+											const real_t* const delta_lbA,	/**< Step direction of lower constraints' bounds. */
+											const real_t* const delta_ubA,	/**< Step direction of upper constraints' bounds. */
+											const real_t* const delta_lb,	/**< Step direction of lower bounds. */
+											const real_t* const delta_ub,	/**< Step direction of upper bounds. */
+											BooleanType Delta_bC_isZero, 	/**< Indicates if active constraints' bounds are to be shifted. */
+											BooleanType Delta_bB_isZero,	/**< Indicates if active bounds are to be shifted. */
+											real_t* const delta_xFX, 		/**< Output: Primal homotopy step direction of fixed variables. */
+											real_t* const delta_xFR,	 	/**< Output: Primal homotopy step direction of free variables. */
+											real_t* const delta_yAC, 		/**< Output: Dual homotopy step direction of active constraints' multiplier. */
+											real_t* const delta_yFX 		/**< Output: Dual homotopy step direction of fixed variables' multiplier. */
+											);
+
+		/** Determines the maximum possible step length along the homotopy path
+		 *  and performs this step (without changing working set).
+		 *	\return SUCCESSFUL_RETURN \n
+		 * 			RET_ERROR_IN_CONSTRAINTPRODUCT \n
+		 * 			RET_QP_INFEASIBLE */
+		returnValue performStep(	const real_t* const delta_g,		/**< Step direction of gradient. */
+									const real_t* const delta_lbA,		/**< Step direction of lower constraints' bounds. */
+									const real_t* const delta_ubA,		/**< Step direction of upper constraints' bounds. */
+									const real_t* const delta_lb,	 	/**< Step direction of lower bounds. */
+									const real_t* const delta_ub,	 	/**< Step direction of upper bounds. */
+									const real_t* const delta_xFX, 		/**< Primal homotopy step direction of fixed variables. */
+									const real_t* const delta_xFR,		/**< Primal homotopy step direction of free variables. */
+									const real_t* const delta_yAC,		/**< Dual homotopy step direction of active constraints' multiplier. */
+									const real_t* const delta_yFX,		/**< Dual homotopy step direction of fixed variables' multiplier. */
+									int& BC_idx, 						/**< Output: Index of blocking constraint. */
+									SubjectToStatus& BC_status,			/**< Output: Status of blocking constraint. */
+									BooleanType& BC_isBound 			/**< Output: Indicates if blocking constraint is a bound. */
+									);
+
+		/** Updates the active set.
+		 *	\return  SUCCESSFUL_RETURN \n
+		 			 RET_REMOVE_FROM_ACTIVESET_FAILED \n
+					 RET_ADD_TO_ACTIVESET_FAILED */
+		returnValue changeActiveSet(	int BC_idx, 						/**< Index of blocking constraint. */
+										SubjectToStatus BC_status,			/**< Status of blocking constraint. */
+										BooleanType BC_isBound 				/**< Indicates if blocking constraint is a bound. */
+										);
+
+
+		/** Compute relative length of homotopy in data space for termination
+		 *  criterion.
+		 *  \return Relative length in data space. */
+		real_t getRelativeHomotopyLength(	const real_t* const g_new,		/**< Final gradient. */
+											const real_t* const lb_new,		/**< Final lower variable bounds. */
+											const real_t* const ub_new,		/**< Final upper variable bounds. */
+											const real_t* const lbA_new,	/**< Final lower constraint bounds. */
+											const real_t* const ubA_new		/**< Final upper constraint bounds. */
+											);
+
+
+		/** Ramping Strategy to avoid ties. Modifies homotopy start without
+		 *  changing current active set.
+		 *  \return SUCCESSFUL_RETURN */
+		virtual returnValue performRamping( );
+
+
+		/** ... */
+		returnValue updateFarBounds(	real_t curFarBound,					/**< ... */
+                                        int nRamp,							/**< ... */
+                                        const real_t* const lb_new,			/**< ... */
+                                        real_t* const lb_new_far,			/**< ... */
+                                        const real_t* const ub_new,			/**< ... */
+                                        real_t* const ub_new_far,			/**< ... */
+                                        const real_t* const lbA_new,		/**< ... */
+                                        real_t* const lbA_new_far,			/**< ... */
+                                        const real_t* const ubA_new,		/**< ... */
+                                        real_t* const ubA_new_far			/**< ... */
+                                        ) const;
+
+
+		/** Drift correction at end of each active set iteration
+		 *  \return SUCCESSFUL_RETURN */
+		virtual returnValue performDriftCorrection( );
+
+
+		/** Updates QP vectors, working sets and internal data structures in order to
+			start from an optimal solution corresponding to initial guesses of the working
+			set for bounds and constraints.
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_SETUP_AUXILIARYQP_FAILED \n
+					RET_INVALID_ARGUMENTS */
+		virtual returnValue setupAuxiliaryQP(	const Bounds* const guessedBounds,			/**< Initial guess for working set of bounds. */
+												const Constraints* const guessedConstraints	/**< Initial guess for working set of constraints. */
+												);
+
+		/** Determines if it is more efficient to refactorise the matrices when
+		 *  hotstarting or not (i.e. better to update the existing factorisations).
+		 *	\return BT_TRUE iff matrices shall be refactorised afresh
+		 */
+		BooleanType shallRefactorise(	const Bounds* const guessedBounds,			/**< Guessed new working set of bounds. */
+										const Constraints* const guessedConstraints	/**< Guessed new working set of constraints. */
+										) const;
+
+		/** Sets up internal QP data.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INVALID_ARGUMENTS \n
+					RET_UNKNONW_BUG */
+		returnValue setupQPdata(	SymmetricMatrix *_H, 		/**< Hessian matrix. \n
+																     If Hessian matrix is trivial,a NULL pointer can be passed. */
+									const real_t* const _g, 	/**< Gradient vector. */
+									Matrix *_A, 			 	/**< Constraint matrix. */
+									const real_t* const _lb,	/**< Lower bound vector (on variables). \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+									const real_t* const _ub,	/**< Upper bound vector (on variables). \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+									const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
+																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+									const real_t* const _ubA	/**< Upper constraints' bound vector. \n
+																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+									);
+
+
+		/** Sets up dense internal QP data. If the current Hessian is trivial
+		 *  (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
+		 *  memory for Hessian is allocated and it is set to the given one.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INVALID_ARGUMENTS \n
+					RET_UNKNONW_BUG */
+		returnValue setupQPdata(	const real_t* const _H, 	/**< Hessian matrix. \n
+																     If Hessian matrix is trivial,a NULL pointer can be passed. */
+									const real_t* const _g, 	/**< Gradient vector. */
+									const real_t* const _A,  	/**< Constraint matrix. */
+									const real_t* const _lb,	/**< Lower bound vector (on variables). \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+									const real_t* const _ub,	/**< Upper bound vector (on variables). \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+									const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
+																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+									const real_t* const _ubA	/**< Upper constraints' bound vector. \n
+																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+									);
+
+		/** Sets up internal QP data by loading it from files. If the current Hessian
+		 *  is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
+		 *  memory for Hessian is allocated and it is set to the given one.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_UNABLE_TO_OPEN_FILE \n
+					RET_UNABLE_TO_READ_FILE \n
+					RET_INVALID_ARGUMENTS \n
+					RET_UNKNONW_BUG */
+		returnValue setupQPdataFromFile(	const char* const H_file, 	/**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n
+																     		 If Hessian matrix is trivial,a NULL pointer can be passed. */
+											const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+											const char* const A_file,	/**< Name of file where constraint matrix, of neighbouring QP to be solved, is stored. */
+											const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+												 			 				 If no lower bounds exist, a NULL pointer can be passed. */
+											const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+												 			 				 If no upper bounds exist, a NULL pointer can be passed. */
+											const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
+												 			 				 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+											const char* const ubA_file	/**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
+												 			 				 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+											);
+
+		/** Loads new QP vectors from files (internal members are not affected!).
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_UNABLE_TO_OPEN_FILE \n
+					RET_UNABLE_TO_READ_FILE \n
+					RET_INVALID_ARGUMENTS */
+		returnValue loadQPvectorsFromFile(	const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+											const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+												 			 				 If no lower bounds exist, a NULL pointer can be passed. */
+											const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+												 			 				 If no upper bounds exist, a NULL pointer can be passed. */
+											const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
+												 			 				 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+											const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
+												 			 				 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+											real_t* const g_new,		/**< Output: Gradient of neighbouring QP to be solved. */
+											real_t* const lb_new,		/**< Output: Lower bounds of neighbouring QP to be solved */
+											real_t* const ub_new,		/**< Output: Upper bounds of neighbouring QP to be solved */
+											real_t* const lbA_new,		/**< Output: Lower constraints' bounds of neighbouring QP to be solved */
+											real_t* const ubA_new		/**< Output: Upper constraints' bounds of neighbouring QP to be solved */
+											) const;
+
+
+		/** Prints concise information on the current iteration.
+		 *	\return  SUCCESSFUL_RETURN \n */
+		returnValue printIteration(	int iter,					/**< Number of current iteration. */
+									int BC_idx, 				/**< Index of blocking constraint. */
+									SubjectToStatus BC_status,	/**< Status of blocking constraint. */
+									BooleanType BC_isBound, 	/**< Indicates if blocking constraint is a bound. */
+									real_t homotopyLength		/**< Current homotopy distance. */
+		 							);
+
+
+		/** Sets constraint matrix of the QP. \n
+			Note: Also internal vector Ax is recomputed!
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_INVALID_ARGUMENTS */
+		inline returnValue setA(	Matrix *A_new	/**< New constraint matrix (a shallow copy is made). */
+									);
+
+		/** Sets dense constraint matrix of the QP. \n
+			Note: Also internal vector Ax is recomputed!
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_INVALID_ARGUMENTS */
+		inline returnValue setA(	const real_t* const A_new	/**< New dense constraint matrix (with correct dimension!), a shallow copy is made. */
+									);
+
+
+		/** Sets constraints' lower bound vector of the QP.
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_QPOBJECT_NOT_SETUP */
+		inline returnValue setLBA(	const real_t* const lbA_new	/**< New constraints' lower bound vector (with correct dimension!). */
+									);
+
+		/** Changes single entry of lower constraints' bound vector of the QP.
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_QPOBJECT_NOT_SETUP \n
+		 *			RET_INDEX_OUT_OF_BOUNDS */
+		inline returnValue setLBA(	int number,		/**< Number of entry to be changed. */
+									real_t value	/**< New value for entry of lower constraints' bound vector (with correct dimension!). */
+									);
+
+		/** Sets constraints' upper bound vector of the QP.
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_QPOBJECT_NOT_SETUP */
+		inline returnValue setUBA(	const real_t* const ubA_new	/**< New constraints' upper bound vector (with correct dimension!). */
+									);
+
+		/** Changes single entry of upper constraints' bound vector of the QP.
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_QPOBJECT_NOT_SETUP \n
+		 *			RET_INDEX_OUT_OF_BOUNDS */
+		inline returnValue setUBA(	int number,		/**< Number of entry to be changed. */
+									real_t value	/**< New value for entry of upper constraints' bound vector (with correct dimension!). */
+									);
+
+
+		/** Drops the blocking bound/constraint that led to infeasibility, or finds another
+		 *  bound/constraint to drop according to drop priorities.
+		 *  \return SUCCESSFUL_RETURN \n
+		 */
+		returnValue dropInfeasibles ( 	int BC_number,				/**< Number of the bound or constraint to be added. */
+										SubjectToStatus BC_status, 	/**< New status of the bound or constraint to be added. */
+										BooleanType BC_isBound,		/**< Whether a bound or a constraint is to be added. */
+										real_t *xiB,				/**< (not yet documented) */
+										real_t *xiC					/**< (not yet documented) */
+										);
+
+		/** Decides if lower bounds are smaller than upper bounds
+		 *
+		 * \return SUCCESSFUL_RETURN \n
+		 * 		   RET_QP_INFEASIBLE */
+
+				returnValue areBoundsConsistent(const real_t* const lb,  /**< Vector of lower bounds*/
+												const real_t* const ub,  /**< Vector of upper bounds*/
+												const real_t* const lbA, /**< Vector of lower constraints*/
+												const real_t* const ubA  /**< Vector of upper constraints*/
+												) const;
+
+
+	public:
+		/** ...
+		 *	\return SUCCESSFUL_RETURN  \n
+					RET_UNABLE_TO_OPEN_FILE */
+		returnValue writeQpDataIntoMatFile(	const char* const filename	/**< Mat file name. */
+											) const;
+
+		/** ...
+		 *	\return SUCCESSFUL_RETURN  \n
+					RET_UNABLE_TO_OPEN_FILE */
+		returnValue writeQpWorkspaceIntoMatFile(	const char* const filename	/**< Mat file name. */
+													);
+
+
+
+	/*
+	 *	PROTECTED MEMBER VARIABLES
+	 */
+	protected:
+		BooleanType freeConstraintMatrix; 		/**< Flag indicating whether the constraint matrix needs to be de-allocated. */
+		Matrix* A;								/**< Constraint matrix. */
+
+		real_t* lbA;							/**< Lower constraints' bound vector. */
+		real_t* ubA;							/**< Upper constraints' bound vector. */
+
+		Constraints constraints;				/**< Data structure for problem's constraints. */
+
+		real_t* T;								/**< Reverse triangular matrix, A = [0 T]*Q'. */
+		real_t* Q;								/**< Orthonormal quadratic matrix, A = [0 T]*Q'. */
+		int sizeT;								/**< Matrix T is stored in a (sizeT x sizeT) array. */
+
+		real_t* Ax;								/**< Stores the current A*x \n
+												 *	 (for increased efficiency only). */
+		real_t* Ax_l;							/**< Stores the current distance to lower constraints' bounds A*x-lbA \n
+												 *	 (for increased efficiency only). */
+		real_t* Ax_u;							/**< Stores the current distance to lower constraints' bounds ubA-A*x \n
+												 *	 (for increased efficiency only). */
+
+		ConstraintProduct* constraintProduct;	/**< Pointer to user-defined constraint product function. */
+
+		real_t* tempA;							/**< Temporary for determineStepDirection. */
+		real_t* tempB;							/**< Temporary for determineStepDirection. */
+		real_t* ZFR_delta_xFRz;					/**< Temporary for determineStepDirection. */
+		real_t* delta_xFRy;						/**< Temporary for determineStepDirection. */
+		real_t* delta_xFRz;						/**< Temporary for determineStepDirection. */
+		real_t* delta_yAC_TMP;					/**< Temporary for determineStepDirection. */
+};
+
+
+END_NAMESPACE_QPOASES
+
+#include <qpOASES/QProblem.ipp>
+
+#endif	/* QPOASES_QPROBLEM_HPP */
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblem.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblem.ipp
new file mode 100644
index 00000000000..84662a069d3
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblem.ipp
@@ -0,0 +1,283 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/QProblem.ipp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of inlined member functions of the QProblem class which
+ *	is able to use the newly developed online active set strategy for
+ *	parametric quadratic programming.
+ */
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+/*
+ *	g e t C o n s t r a i n t s
+ */
+inline returnValue QProblem::getConstraints( Constraints& _constraints ) const
+{
+	int nV = getNV( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	_constraints = constraints;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	g e t N C
+ */
+inline int QProblem::getNC( ) const
+{
+	return constraints.getNC( );
+}
+
+
+/*
+ *	g e t N E C
+ */
+inline int QProblem::getNEC( ) const
+{
+	return constraints.getNEC( );
+}
+
+
+/*
+ *	g e t N A C
+ */
+inline int QProblem::getNAC( ) const
+{
+	return constraints.getNAC( );
+}
+
+
+/*
+ *	g e t N I A C
+ */
+inline int QProblem::getNIAC( ) const
+{
+	return constraints.getNIAC( );
+}
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+
+/*
+ *	s e t A
+ */
+inline returnValue QProblem::setA( Matrix *A_new )
+{
+	int j;
+	int nV = getNV( );
+	int nC = getNC( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( A_new == 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* Set constraint matrix AND update member AX. */
+	if ( ( freeConstraintMatrix == BT_TRUE ) && ( A != 0 ) )
+	{
+		delete A;
+		A = 0;
+	}
+	A = A_new;
+	freeConstraintMatrix = BT_FALSE;
+
+	A->times(1, 1.0, x, nV, 0.0, Ax, nC);
+
+	for( j=0; j<nC; ++j )
+	{
+		Ax_u[j] = ubA[j] - Ax[j];
+		Ax_l[j] = Ax[j] - lbA[j];
+		
+		// (ckirches) disable constraints with empty rows	
+		if ( isZero( A->getRowNorm (j) ) == BT_TRUE )
+			constraints.setType ( j, ST_DISABLED );
+	}
+
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t A
+ */
+inline returnValue QProblem::setA( const real_t* const A_new )
+{
+	int j;
+	int nV = getNV( );
+	int nC = getNC( );
+	DenseMatrix* dA;
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( A_new == 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* Set constraint matrix AND update member AX. */
+	if ( ( freeConstraintMatrix == BT_TRUE ) && ( A != 0 ) )
+	{
+		delete A;
+		A = 0;
+	}
+	A = dA = new DenseMatrix(nC, nV, nV, (real_t*) A_new);
+	freeConstraintMatrix = BT_TRUE;
+
+	A->times(1, 1.0, x, nV, 0.0, Ax, nC);
+
+	for( j=0; j<nC; ++j )
+	{
+		Ax_u[j] = ubA[j] - Ax[j];
+		Ax_l[j] = Ax[j] - lbA[j];
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t L B A
+ */
+inline returnValue QProblem::setLBA( const real_t* const lbA_new )
+{
+	unsigned int i;
+	unsigned int nV = (unsigned int)getNV( );
+	unsigned int nC = (unsigned int)getNC( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( lbA_new != 0 )
+	{	
+		memcpy( lbA,lbA_new,nC*sizeof(real_t) );
+	}
+	else
+	{
+		/* if no lower constraints' bounds are specified, set them to -infinity */
+		for( i=0; i<nC; ++i )
+			lbA[i] = -INFTY;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t L B A
+ */
+inline returnValue QProblem::setLBA( int number, real_t value )
+{
+	int nV = getNV( );
+	int nC = getNC( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ( number >= 0 ) && ( number < nC ) )
+	{
+		lbA[number] = value;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+}
+
+
+/*
+ *	s e t U B A
+ */
+inline returnValue QProblem::setUBA( const real_t* const ubA_new )
+{
+	unsigned int i;
+	unsigned int nV = (unsigned int)getNV( );
+	unsigned int nC = (unsigned int)getNC( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ubA_new != 0 )
+	{
+		memcpy( ubA,ubA_new,nC*sizeof(real_t) );
+	}
+	else
+	{
+		/* if no upper constraints' bounds are specified, set them to infinity */
+		for( i=0; i<nC; ++i )
+			ubA[i] = INFTY;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t U B A
+ */
+inline returnValue QProblem::setUBA( int number, real_t value )
+{
+	int nV = getNV( );
+	int nC = getNC( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ( number >= 0 ) && ( number < nC ) )
+	{
+		ubA[number] = value;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblemB.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblemB.hpp
new file mode 100644
index 00000000000..d62244e130e
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblemB.hpp
@@ -0,0 +1,1091 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/QProblemB.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Declaration of the QProblemB class which is able to use the newly
+ *	developed online active set strategy for parametric quadratic programming
+ *	for problems with (simple) bounds only.
+ */
+
+
+
+#ifndef QPOASES_QPROBLEMB_HPP
+#define QPOASES_QPROBLEMB_HPP
+
+
+#include <qpOASES/Flipper.hpp>
+#include <qpOASES/Options.hpp>
+#include <qpOASES/Matrices.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+class SolutionAnalysis;
+
+/** 
+ *	\brief Implements the online active set strategy for box-constrained QPs.
+ *
+ *	Class for setting up and solving quadratic programs with bounds (= box constraints) only.
+ *	The main feature is the possibily to use the newly developed online active set strategy
+ *	for parametric quadratic programming.
+ *
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ */
+class QProblemB
+{
+	/* allow SolutionAnalysis class to access private members */
+	friend class SolutionAnalysis;
+
+	/*
+	 *	PUBLIC MEMBER FUNCTIONS
+	 */
+	public:
+		/** Default constructor. */
+		QProblemB( );
+
+		/** Constructor which takes the QP dimension and Hessian type
+		 *  information. If the Hessian is the zero (i.e. HST_ZERO) or the
+		 *  identity matrix (i.e. HST_IDENTITY), respectively, no memory
+		 *  is allocated for it and a NULL pointer can be passed for it
+		 *  to the init() functions. */
+		QProblemB(	int _nV,								/**< Number of variables. */
+					HessianType _hessianType = HST_UNKNOWN	/**< Type of Hessian matrix. */
+					);
+
+		/** Copy constructor (deep copy). */
+		QProblemB(	const QProblemB& rhs	/**< Rhs object. */
+					);
+
+		/** Destructor. */
+		virtual ~QProblemB( );
+
+		/** Assignment operator (deep copy). */
+		QProblemB& operator=(	const QProblemB& rhs	/**< Rhs object. */
+								);
+
+
+		/** Clears all data structures of QProblemB except for QP data.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_RESET_FAILED */
+		virtual returnValue reset( );
+
+
+		/** Initialises a QProblemB with given QP data and solves it
+		 *	using an initial homotopy with empty working set (at most nWSR iterations).
+		 *  Note: This function internally calls solveInitialQP for initialisation!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INIT_FAILED \n
+					RET_INIT_FAILED_CHOLESKY \n
+					RET_INIT_FAILED_HOTSTART \n
+					RET_INIT_FAILED_INFEASIBILITY \n
+					RET_INIT_FAILED_UNBOUNDEDNESS \n
+					RET_MAX_NWSR_REACHED \n
+					RET_INVALID_ARGUMENTS */
+		returnValue init(	SymmetricMatrix *_H,	 	/**< Hessian matrix (a shallow copy is made). */
+							const real_t* const _g,		/**< Gradient vector. */
+							const real_t* const _lb,	/**< Lower bounds (on variables). \n
+															 If no lower bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ub,	/**< Upper bounds (on variables). \n
+															 If no upper bounds exist, a NULL pointer can be passed. */
+							int& nWSR, 					/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
+															 Output: Number of performed working set recalculations. */
+				 			real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP initialisation. \n
+															 Output: CPU time spend for QP initialisation (if pointer passed). */
+							);
+
+		/** Initialises a QProblemB with given QP data and solves it
+		 *	using an initial homotopy with empty working set (at most nWSR iterations).
+		 *  Note: This function internally calls solveInitialQP for initialisation!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INIT_FAILED \n
+					RET_INIT_FAILED_CHOLESKY \n
+					RET_INIT_FAILED_HOTSTART \n
+					RET_INIT_FAILED_INFEASIBILITY \n
+					RET_INIT_FAILED_UNBOUNDEDNESS \n
+					RET_MAX_NWSR_REACHED \n
+					RET_INVALID_ARGUMENTS */
+		returnValue init(	const real_t* const _H, 	/**< Hessian matrix (a shallow copy is made). \n
+															 If Hessian matrix is trivial, a NULL pointer can be passed. */
+							const real_t* const _g,		/**< Gradient vector. */
+							const real_t* const _lb,	/**< Lower bounds (on variables). \n
+															 If no lower bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ub,	/**< Upper bounds (on variables). \n
+															 If no upper bounds exist, a NULL pointer can be passed. */
+							int& nWSR, 					/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
+															 Output: Number of performed working set recalculations. */
+				 			real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP initialisation. \n
+															 Output: CPU time spend for QP initialisation (if pointer passed). */
+							);
+
+		/** Initialises a QProblemB with given QP data to be read from files and solves it
+		 *	using an initial homotopy with empty working set (at most nWSR iterations).
+		 *  Note: This function internally calls solveInitialQP for initialisation!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INIT_FAILED \n
+					RET_INIT_FAILED_CHOLESKY \n
+					RET_INIT_FAILED_HOTSTART \n
+					RET_INIT_FAILED_INFEASIBILITY \n
+					RET_INIT_FAILED_UNBOUNDEDNESS \n
+					RET_MAX_NWSR_REACHED \n
+					RET_UNABLE_TO_READ_FILE */
+		returnValue init(	const char* const H_file, 	/**< Name of file where Hessian matrix is stored. \n
+															 If Hessian matrix is trivial, a NULL pointer can be passed. */
+							const char* const g_file,  	/**< Name of file where gradient vector is stored. */
+							const char* const lb_file, 	/**< Name of file where lower bound vector. \n
+															 If no lower bounds exist, a NULL pointer can be passed. */
+							const char* const ub_file, 	/**< Name of file where upper bound vector. \n
+															 If no upper bounds exist, a NULL pointer can be passed. */
+							int& nWSR, 					/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
+															 Output: Number of performed working set recalculations. */
+				 			real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP initialisation. \n
+															 Output: CPU time spend for QP initialisation (if pointer passed). */
+							);
+
+		/** Initialises a QProblemB with given QP data and solves it
+		 *  depending on the parameter constellation: \n
+		 *	1. 0,    0,    0 : start with xOpt = 0, yOpt = 0 and IB empty (or all implicit equality bounds), \n
+		 *	2. xOpt, 0,    0 : start with xOpt, yOpt = 0 and obtain IB by "clipping", \n
+		 *	3. 0,    yOpt, 0 : start with xOpt = 0, yOpt and obtain IB from yOpt != 0, \n
+		 *	4. 0,    0,    IB: start with xOpt = 0, yOpt = 0 and IB, \n
+		 *	5. xOpt, yOpt, 0 : start with xOpt, yOpt and obtain IB from yOpt != 0, \n
+		 *	6. xOpt, 0,    IB: start with xOpt, yOpt = 0 and IB, \n
+		 *	7. xOpt, yOpt, IB: start with xOpt, yOpt and IB (assume them to be consistent!)
+		 *  Note: This function internally calls solveInitialQP for initialisation!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INIT_FAILED \n
+					RET_INIT_FAILED_CHOLESKY \n
+					RET_INIT_FAILED_HOTSTART \n
+					RET_INIT_FAILED_INFEASIBILITY \n
+					RET_INIT_FAILED_UNBOUNDEDNESS \n
+					RET_MAX_NWSR_REACHED \n
+					RET_INVALID_ARGUMENTS */
+		returnValue init(	SymmetricMatrix *_H,				/**< Hessian matrix (a shallow copy is made). */
+							const real_t* const _g,				/**< Gradient vector. */
+							const real_t* const _lb,			/**< Lower bounds (on variables). \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ub,			/**< Upper bounds (on variables). \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+							int& nWSR, 							/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
+																	 Output: Number of performed working set recalculations. */
+				 			real_t* const cputime,				/**< Input: Maximum CPU time allowed for QP initialisation. \n
+															 		 Output: CPU time spend for QP initialisation. */
+							const real_t* const xOpt,			/**< Optimal primal solution vector.
+																	 A NULL pointer can be passed. */
+							const real_t* const yOpt,			/**< Optimal dual solution vector.
+																	 A NULL pointer can be passed. */
+							const Bounds* const guessedBounds	/**< Optimal working set for solution (xOpt,yOpt).
+																	 A NULL pointer can be passed. */
+							);
+
+		/** Initialises a QProblemB with given QP data and solves it
+		 *  depending on the parameter constellation: \n
+		 *	1. 0,    0,    0 : start with xOpt = 0, yOpt = 0 and IB empty (or all implicit equality bounds), \n
+		 *	2. xOpt, 0,    0 : start with xOpt, yOpt = 0 and obtain IB by "clipping", \n
+		 *	3. 0,    yOpt, 0 : start with xOpt = 0, yOpt and obtain IB from yOpt != 0, \n
+		 *	4. 0,    0,    IB: start with xOpt = 0, yOpt = 0 and IB, \n
+		 *	5. xOpt, yOpt, 0 : start with xOpt, yOpt and obtain IB from yOpt != 0, \n
+		 *	6. xOpt, 0,    IB: start with xOpt, yOpt = 0 and IB, \n
+		 *	7. xOpt, yOpt, IB: start with xOpt, yOpt and IB (assume them to be consistent!)
+		 *  Note: This function internally calls solveInitialQP for initialisation!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INIT_FAILED \n
+					RET_INIT_FAILED_CHOLESKY \n
+					RET_INIT_FAILED_HOTSTART \n
+					RET_INIT_FAILED_INFEASIBILITY \n
+					RET_INIT_FAILED_UNBOUNDEDNESS \n
+					RET_MAX_NWSR_REACHED \n
+					RET_INVALID_ARGUMENTS */
+		returnValue init(	const real_t* const _H, 			/**< Hessian matrix (a shallow copy is made). \n
+																     If Hessian matrix is trivial, a NULL pointer can be passed. */
+							const real_t* const _g,				/**< Gradient vector. */
+							const real_t* const _lb,			/**< Lower bounds (on variables). \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ub,			/**< Upper bounds (on variables). \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+							int& nWSR, 							/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
+																	 Output: Number of performed working set recalculations. */
+				 			real_t* const cputime,				/**< Input: Maximum CPU time allowed for QP initialisation. \n
+															 		 Output: CPU time spend for QP initialisation. */
+							const real_t* const xOpt,			/**< Optimal primal solution vector.
+																	 A NULL pointer can be passed. */
+							const real_t* const yOpt,			/**< Optimal dual solution vector.
+																	 A NULL pointer can be passed. */
+							const Bounds* const guessedBounds	/**< Optimal working set for solution (xOpt,yOpt).
+																	 A NULL pointer can be passed. */
+							);
+
+		/** Initialises a QProblemB with given QP data to be read from files and solves it
+		 *  depending on the parameter constellation: \n
+		 *	1. 0,    0,    0 : start with xOpt = 0, yOpt = 0 and IB empty (or all implicit equality bounds), \n
+		 *	2. xOpt, 0,    0 : start with xOpt, yOpt = 0 and obtain IB by "clipping", \n
+		 *	3. 0,    yOpt, 0 : start with xOpt = 0, yOpt and obtain IB from yOpt != 0, \n
+		 *	4. 0,    0,    IB: start with xOpt = 0, yOpt = 0 and IB, \n
+		 *	5. xOpt, yOpt, 0 : start with xOpt, yOpt and obtain IB from yOpt != 0, \n
+		 *	6. xOpt, 0,    IB: start with xOpt, yOpt = 0 and IB, \n
+		 *	7. xOpt, yOpt, IB: start with xOpt, yOpt and IB (assume them to be consistent!)
+		 *  Note: This function internally calls solveInitialQP for initialisation!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INIT_FAILED \n
+					RET_INIT_FAILED_CHOLESKY \n
+					RET_INIT_FAILED_HOTSTART \n
+					RET_INIT_FAILED_INFEASIBILITY \n
+					RET_INIT_FAILED_UNBOUNDEDNESS \n
+					RET_MAX_NWSR_REACHED \n
+					RET_UNABLE_TO_READ_FILE */
+		returnValue init(	const char* const H_file, 			/**< Name of file where Hessian matrix is stored. \n
+																     If Hessian matrix is trivial, a NULL pointer can be passed. */
+							const char* const g_file,  			/**< Name of file where gradient vector is stored. */
+							const char* const lb_file, 			/**< Name of file where lower bound vector. \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+							const char* const ub_file, 			/**< Name of file where upper bound vector. \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+							int& nWSR, 							/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
+																	 Output: Number of performed working set recalculations. */
+				 			real_t* const cputime,				/**< Input: Maximum CPU time allowed for QP initialisation. \n
+															 		 Output: CPU time spend for QP initialisation. */
+							const real_t* const xOpt,			/**< Optimal primal solution vector.
+																	 A NULL pointer can be passed. */
+							const real_t* const yOpt,			/**< Optimal dual solution vector.
+																	 A NULL pointer can be passed. */
+							const Bounds* const guessedBounds	/**< Optimal working set for solution (xOpt,yOpt).
+																	 A NULL pointer can be passed. */
+							);
+
+
+		/** Solves an initialised QProblemB using online active set strategy.
+		 *  Note: This function internally calls solveQP/solveRegularisedQP
+		 *        for solving an initialised QP!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_MAX_NWSR_REACHED \n
+					RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+		returnValue hotstart(	const real_t* const g_new,	/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
+													 			 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,	/**< Upper bounds of neighbouring QP to be solved. \n
+													 			 If no upper bounds exist, a NULL pointer can be passed. */
+								int& nWSR,					/**< Input: Maximum number of working set recalculations; \n
+																 Output: Number of performed working set recalculations. */
+								real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP solution. \n
+																 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								);
+
+		/** Solves an initialised QProblemB online active set strategy
+		 *  reading QP data from files.
+		 *  Note: This function internally calls solveQP/solveRegularisedQP
+		 *        for solving an initialised QP!
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MAX_NWSR_REACHED \n
+		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+					RET_UNABLE_TO_READ_FILE \n
+					RET_INVALID_ARGUMENTS */
+		returnValue hotstart(	const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+								const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+													 			 If no lower bounds exist, a NULL pointer can be passed. */
+								const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+													 			 If no upper bounds exist, a NULL pointer can be passed. */
+								int& nWSR, 					/**< Input: Maximum number of working set recalculations; \n
+																 Output: Number of performed working set recalculations. */
+								real_t* const cputime = 0 	/**< Input: Maximum CPU time allowed for QP solution. \n
+																 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								);
+
+		/** Solves an initialised QProblemB using online active set strategy (using an initialised homotopy).
+		 *  Note: This function internally calls solveQP/solveRegularisedQP
+		 *        for solving an initialised QP!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_MAX_NWSR_REACHED \n
+					RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+					RET_SETUP_AUXILIARYQP_FAILED */
+		returnValue hotstart(	const real_t* const g_new,			/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const lb_new,			/**< Lower bounds of neighbouring QP to be solved. \n
+													 					 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,			/**< Upper bounds of neighbouring QP to be solved. \n
+													 					 If no upper bounds exist, a NULL pointer can be passed. */
+								int& nWSR,							/**< Input: Maximum number of working set recalculations; \n
+																		 Output: Number of performed working set recalculations. */
+								real_t* const cputime,				/**< Input: Maximum CPU time allowed for QP solution. \n
+																 		 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								const Bounds* const guessedBounds	/**< Initial guess for working set of bounds.
+																	 *   A null pointer corresponds to an empty working set! */
+								);
+
+		/** Solves an initialised QProblemB using online active set strategy (using an initialised homotopy)
+		 *  reading QP data from files.
+		 *  Note: This function internally calls solveQP/solveRegularisedQP
+		 *        for solving an initialised QP!
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MAX_NWSR_REACHED \n
+		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+					RET_UNABLE_TO_READ_FILE \n
+					RET_INVALID_ARGUMENTS \n
+					RET_SETUP_AUXILIARYQP_FAILED */
+		returnValue hotstart(	const char* const g_file,			/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+								const char* const lb_file,			/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+													 					 If no lower bounds exist, a NULL pointer can be passed. */
+								const char* const ub_file, 			/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+													 					 If no upper bounds exist, a NULL pointer can be passed. */
+								int& nWSR,							/**< Input: Maximum number of working set recalculations; \n
+																		 Output: Number of performed working set recalculations. */
+								real_t* const cputime,				/**< Input: Maximum CPU time allowed for QP solution. \n
+																 		 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								const Bounds* const guessedBounds	/**< Initial guess for working set of bounds.
+																	 *   A null pointer corresponds to an empty working set! */
+								);
+
+
+		/** Writes a vector with the state of the bounds
+		   *	\return SUCCESSFUL_RETURN */
+		virtual returnValue getWorkingSet(	real_t* workingSet		/** Output: array containing state of the working set. */
+											);
+
+		/** Returns current bounds object of the QP (deep copy).
+		  *	\return SUCCESSFUL_RETURN \n
+		  			RET_QPOBJECT_NOT_SETUP */
+		inline returnValue getBounds(	Bounds& _bounds	/** Output: Bounds object. */
+										) const;
+
+
+		/** Returns the number of variables.
+		 *	\return Number of variables. */
+		inline int getNV( ) const;
+
+		/** Returns the number of free variables.
+		 *	\return Number of free variables. */
+		inline int getNFR( ) const;
+
+		/** Returns the number of fixed variables.
+		 *	\return Number of fixed variables. */
+		inline int getNFX( ) const;
+
+		/** Returns the number of implicitly fixed variables.
+		 *	\return Number of implicitly fixed variables. */
+		inline int getNFV( ) const;
+
+		/** Returns the dimension of null space.
+		 *	\return Dimension of null space. */
+		virtual int getNZ( ) const;
+
+
+		/** Returns the optimal objective function value.
+		 *	\return finite value: Optimal objective function value (QP was solved) \n
+		 			+infinity:	  QP was not yet solved */
+		real_t getObjVal( ) const;
+
+		/** Returns the objective function value at an arbitrary point x.
+		 *	\return Objective function value at point x */
+		real_t getObjVal(	const real_t* const _x	/**< Point at which the objective function shall be evaluated. */
+							) const;
+
+		/** Returns the primal solution vector.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_QP_NOT_SOLVED */
+		returnValue getPrimalSolution(	real_t* const xOpt			/**< Output: Primal solution vector (if QP has been solved). */
+										) const;
+
+		/** Returns the dual solution vector.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_QP_NOT_SOLVED */
+		virtual returnValue getDualSolution(	real_t* const yOpt	/**< Output: Dual solution vector (if QP has been solved). */
+												) const;
+
+
+		/** Returns status of the solution process.
+		 *	\return Status of solution process. */
+		inline QProblemStatus getStatus( ) const;
+
+
+		/** Returns if the QProblem object is initialised.
+		 *	\return BT_TRUE:  QProblemB initialised \n
+		 			BT_FALSE: QProblemB not initialised */
+		inline BooleanType isInitialised( ) const;
+
+		/** Returns if the QP has been solved.
+		 *	\return BT_TRUE:  QProblemB solved \n
+		 			BT_FALSE: QProblemB not solved */
+		inline BooleanType isSolved( ) const;
+
+		/** Returns if the QP is infeasible.
+		 *	\return BT_TRUE:  QP infeasible \n
+		 			BT_FALSE: QP feasible (or not known to be infeasible!) */
+		inline BooleanType isInfeasible( ) const;
+
+		/** Returns if the QP is unbounded.
+		 *	\return BT_TRUE:  QP unbounded \n
+		 			BT_FALSE: QP unbounded (or not known to be unbounded!) */
+		inline BooleanType isUnbounded( ) const;
+
+
+		/** Returns Hessian type flag (type is not determined due to this call!).
+		 *	\return Hessian type. */
+		inline HessianType getHessianType( ) const;
+
+		/** Changes the print level.
+ 		 *	\return SUCCESSFUL_RETURN */
+		inline returnValue setHessianType(	HessianType _hessianType /**< New Hessian type. */
+											);
+
+		/** Returns if the QP has been internally regularised.
+		 *	\return BT_TRUE:  Hessian is internally regularised for QP solution \n
+		 			BT_FALSE: No internal Hessian regularisation is used for QP solution */
+		inline BooleanType usingRegularisation( ) const;
+
+		/** Returns current options struct.
+		 *	\return Current options struct. */
+		inline Options getOptions( ) const;
+
+		/** Overrides current options with given ones.
+ 		 *	\return SUCCESSFUL_RETURN */
+		inline returnValue setOptions(	const Options& _options	/**< New options. */
+										);
+
+		/** Returns the print level.
+		 *	\return Print level. */
+		inline PrintLevel getPrintLevel( ) const;
+
+		/** Changes the print level.
+ 		 *	\return SUCCESSFUL_RETURN */
+		returnValue setPrintLevel(	PrintLevel _printlevel	/**< New print level. */
+									);
+
+
+		/** Prints concise list of properties of the current QP.
+		 *	\return  SUCCESSFUL_RETURN \n */
+		virtual returnValue printProperties( );
+
+		/** Prints a list of all options and their current values.
+		 *	\return  SUCCESSFUL_RETURN \n */
+		returnValue printOptions( ) const;
+
+
+	/*
+	 *	PROTECTED MEMBER FUNCTIONS
+	 */
+	protected:
+		/** Frees all allocated memory.
+		 *  \return SUCCESSFUL_RETURN */
+		returnValue clear( );
+		
+		/** Copies all members from given rhs object.
+		 *  \return SUCCESSFUL_RETURN */
+		returnValue copy(	const QProblemB& rhs	/**< Rhs object. */
+							);
+		
+		/** If Hessian type has been set by the user, nothing is done.
+		 *  Otherwise the Hessian type is set to HST_IDENTITY, HST_ZERO, or
+		 *  HST_POSDEF (default), respectively.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_HESSIAN_INDEFINITE */
+		returnValue determineHessianType( );
+
+		/** Determines type of existing constraints and bounds (i.e. implicitly fixed, unbounded etc.).
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_SETUPSUBJECTTOTYPE_FAILED */
+		virtual returnValue setupSubjectToType( );
+
+		/** Determines type of new constraints and bounds (i.e. implicitly fixed, unbounded etc.).
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_SETUPSUBJECTTOTYPE_FAILED */
+		virtual returnValue setupSubjectToType(	const real_t* const lb_new,	/**< New lower bounds. */
+												const real_t* const ub_new	/**< New upper bounds. */
+												);
+
+		/** Computes the Cholesky decomposition of the (simply projected) Hessian
+		 *  (i.e. R^T*R = Z^T*H*Z). It only works in the case where Z is a simple
+		 *  projection matrix!
+		 *  Note: If Hessian turns out not to be positive definite, the Hessian type
+		 *		  is set to HST_SEMIDEF accordingly.
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_HESSIAN_NOT_SPD \n
+		 *			RET_INDEXLIST_CORRUPTED */
+		returnValue computeCholesky( );
+
+
+		/** Computes initial Cholesky decomposition of the (simply projected) Hessian 
+		 *  making use of the function computeCholesky().
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_HESSIAN_NOT_SPD \n
+		 *			RET_INDEXLIST_CORRUPTED */
+		virtual returnValue setupInitialCholesky( );
+
+		/** Obtains the desired working set for the auxiliary initial QP in
+		 *  accordance with the user specifications
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_OBTAINING_WORKINGSET_FAILED \n
+					RET_INVALID_ARGUMENTS */
+		returnValue obtainAuxiliaryWorkingSet(	const real_t* const xOpt,			/**< Optimal primal solution vector.
+																					 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
+												const real_t* const yOpt,			/**< Optimal dual solution vector.
+																					 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
+												const Bounds* const guessedBounds,	/**< Guessed working set for solution (xOpt,yOpt). */
+												Bounds* auxiliaryBounds				/**< Input: Allocated bound object. \n
+																					 *	 Output: Working set for auxiliary QP. */
+												) const;
+
+		/** Decides if lower bounds are smaller than upper bounds
+		 *
+		 * \return SUCCESSFUL_RETURN \n
+		 * 		   RET_QP_INFEASIBLE */
+
+		returnValue areBoundsConsistent(const real_t* const lb, /**< Vector of lower bounds*/
+										const real_t* const ub  /**< Vector of upper bounds*/
+										) const;
+
+		/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_DIV_BY_ZERO */
+		returnValue backsolveR(	const real_t* const b,	/**< Right hand side vector. */
+								BooleanType transposed,	/**< Indicates if the transposed system shall be solved. */
+								real_t* const a 		/**< Output: Solution vector */
+								) const;
+
+		/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n
+		 *  Special variant for the case that this function is called from within "removeBound()".
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_DIV_BY_ZERO */
+		returnValue backsolveR(	const real_t* const b,		/**< Right hand side vector. */
+								BooleanType transposed,		/**< Indicates if the transposed system shall be solved. */
+								BooleanType removingBound,	/**< Indicates if function is called from "removeBound()". */
+								real_t* const a 			/**< Output: Solution vector */
+								) const;
+
+
+		/** Determines step direction of the shift of the QP data.
+		 *	\return SUCCESSFUL_RETURN */
+		returnValue determineDataShift(	const real_t* const g_new,	/**< New gradient vector. */
+										const real_t* const lb_new,	/**< New lower bounds. */
+										const real_t* const ub_new,	/**< New upper bounds. */
+										real_t* const delta_g,	 	/**< Output: Step direction of gradient vector. */
+										real_t* const delta_lb,	 	/**< Output: Step direction of lower bounds. */
+										real_t* const delta_ub,	 	/**< Output: Step direction of upper bounds. */
+										BooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */
+										);
+
+
+		/** Sets up internal QP data. 
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INVALID_ARGUMENTS */
+		returnValue setupQPdata(	SymmetricMatrix *_H,	 	/**< Hessian matrix.*/
+									const real_t* const _g,		/**< Gradient vector. */
+									const real_t* const _lb,	/**< Lower bounds (on variables). \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+									const real_t* const _ub		/**< Upper bounds (on variables). \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+									);
+
+		/** Sets up internal QP data. If the current Hessian is trivial
+		 *  (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
+		 *  memory for Hessian is allocated and it is set to the given one.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INVALID_ARGUMENTS \n
+					RET_NO_HESSIAN_SPECIFIED */
+		returnValue setupQPdata(	const real_t* const _H, 	/**< Hessian matrix. \n
+																     If Hessian matrix is trivial,a NULL pointer can be passed. */
+									const real_t* const _g,		/**< Gradient vector. */
+									const real_t* const _lb,	/**< Lower bounds (on variables). \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+									const real_t* const _ub		/**< Upper bounds (on variables). \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+									);
+
+		/** Sets up internal QP data by loading it from files. If the current Hessian
+		 *  is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
+		 *  memory for Hessian is allocated and it is set to the given one.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_UNABLE_TO_OPEN_FILE \n
+					RET_UNABLE_TO_READ_FILE \n
+					RET_INVALID_ARGUMENTS \n
+					RET_NO_HESSIAN_SPECIFIED */
+		returnValue setupQPdataFromFile(	const char* const H_file, 	/**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n
+																     		 If Hessian matrix is trivial,a NULL pointer can be passed. */
+											const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+											const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+												 			 				 If no lower bounds exist, a NULL pointer can be passed. */
+											const char* const ub_file 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+												 			 				 If no upper bounds exist, a NULL pointer can be passed. */
+											);
+
+		/** Loads new QP vectors from files (internal members are not affected!).
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_UNABLE_TO_OPEN_FILE \n
+					RET_UNABLE_TO_READ_FILE \n
+					RET_INVALID_ARGUMENTS */
+		returnValue loadQPvectorsFromFile(	const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+											const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+												 			 				 If no lower bounds exist, a NULL pointer can be passed. */
+											const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+												 			 				 If no upper bounds exist, a NULL pointer can be passed. */
+											real_t* const g_new,		/**< Output: Gradient of neighbouring QP to be solved. */
+											real_t* const lb_new,		/**< Output: Lower bounds of neighbouring QP to be solved */
+											real_t* const ub_new		/**< Output: Upper bounds of neighbouring QP to be solved */
+											) const;
+
+
+		/** Sets internal infeasibility flag and throws given error in case the far bound
+		 *	strategy is not enabled (as QP might actually not be infeasible in this case).
+		 *	\return RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_ENSURELI_FAILED_CYCLING \n
+					RET_ENSURELI_FAILED_NOINDEX */
+		returnValue setInfeasibilityFlag(	returnValue returnvalue,			/**< Returnvalue to be tunneled. */
+											BooleanType doThrowError = BT_FALSE	/**< Flag forcing to throw an error. */
+											);
+
+
+		/** Determines if next QP iteration can be performed within given CPU time limit.
+		 *	\return BT_TRUE: CPU time limit is exceeded, stop QP solution. \n
+					BT_FALSE: Sufficient CPU time for next QP iteration. */
+		BooleanType isCPUtimeLimitExceeded(	const real_t* const cputime,	/**< Maximum CPU time allowed for QP solution. */
+											real_t starttime,				/**< Start time of current QP solution. */
+											int nWSR						/**< Number of working set recalculations performed so far. */
+											) const;
+
+
+		/** Regularise Hessian matrix by adding a scaled identity matrix to it.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_HESSIAN_ALREADY_REGULARISED */
+		returnValue regulariseHessian( );
+
+
+		/** Sets Hessian matrix of the QP.
+		 *	\return SUCCESSFUL_RETURN */
+		inline returnValue setH(	SymmetricMatrix* H_new	/**< New Hessian matrix (a shallow copy is made). */
+									);
+									
+		/** Sets dense Hessian matrix of the QP.
+		 *  If a null pointer is passed and
+		 *  a) hessianType is HST_IDENTITY, nothing is done,
+		 *  b) hessianType is not HST_IDENTITY, Hessian matrix is set to zero.
+		 *	\return SUCCESSFUL_RETURN */
+		inline returnValue setH(	const real_t* const H_new	/**< New dense Hessian matrix (with correct dimension!), a shallow copy is made. */
+									);
+
+		/** Changes gradient vector of the QP.
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_INVALID_ARGUMENTS */
+		inline returnValue setG(	const real_t* const g_new	/**< New gradient vector (with correct dimension!). */
+									);
+
+		/** Changes lower bound vector of the QP.
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_QPOBJECT_NOT_SETUP */
+		inline returnValue setLB(	const real_t* const lb_new	/**< New lower bound vector (with correct dimension!). */
+									);
+
+		/** Changes single entry of lower bound vector of the QP.
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_QPOBJECT_NOT_SETUP \n
+		 *			RET_INDEX_OUT_OF_BOUNDS */
+		inline returnValue setLB(	int number,		/**< Number of entry to be changed. */
+									real_t value	/**< New value for entry of lower bound vector. */
+									);
+
+		/** Changes upper bound vector of the QP.
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_QPOBJECT_NOT_SETUP */
+		inline returnValue setUB(	const real_t* const ub_new	/**< New upper bound vector (with correct dimension!). */
+									);
+
+		/** Changes single entry of upper bound vector of the QP.
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_QPOBJECT_NOT_SETUP \n
+		 *			RET_INDEX_OUT_OF_BOUNDS */
+		inline returnValue setUB(	int number,		/**< Number of entry to be changed. */
+									real_t value	/**< New value for entry of upper bound vector. */
+									);
+
+
+		/** Computes parameters for the Givens matrix G for which [x,y]*G = [z,0]
+		 *	\return SUCCESSFUL_RETURN */
+		inline void computeGivens(	real_t xold,	/**< Matrix entry to be normalised. */
+									real_t yold,	/**< Matrix entry to be annihilated. */
+									real_t& xnew,	/**< Output: Normalised matrix entry. */
+									real_t& ynew,	/**< Output: Annihilated matrix entry. */
+									real_t& c,		/**< Output: Cosine entry of Givens matrix. */
+									real_t& s 		/**< Output: Sine entry of Givens matrix. */
+									) const;
+
+		/** Applies Givens matrix determined by c and s (cf. computeGivens).
+		 *	\return SUCCESSFUL_RETURN */
+		inline void applyGivens(	real_t c,		/**< Cosine entry of Givens matrix. */
+									real_t s,		/**< Sine entry of Givens matrix. */
+									real_t nu, 		/**< Further factor: s/(1+c). */
+									real_t xold,	/**< Matrix entry to be transformed corresponding to
+													 *	 the normalised entry of the original matrix. */
+									real_t yold, 	/**< Matrix entry to be transformed corresponding to
+													 *	 the annihilated entry of the original matrix. */
+									real_t& xnew,	/**< Output: Transformed matrix entry corresponding to
+													 *	 the normalised entry of the original matrix. */
+									real_t& ynew	/**< Output: Transformed matrix entry corresponding to
+													 *	 the annihilated entry of the original matrix. */
+									) const;
+
+
+
+		/** Compute relative length of homotopy in data space for termination
+		 *  criterion.
+		 *  \return Relative length in data space. */
+		real_t getRelativeHomotopyLength(	const real_t* const g_new,		/**< Final gradient. */
+											const real_t* const lb_new,		/**< Final lower variable bounds. */
+											const real_t* const ub_new		/**< Final upper variable bounds. */
+											);
+
+		/** Ramping Strategy to avoid ties. Modifies homotopy start without
+		 *  changing current active set.
+		 *  \return SUCCESSFUL_RETURN */
+		virtual returnValue performRamping( );
+
+
+		/** ... */
+		returnValue updateFarBounds(	real_t curFarBound,					/**< ... */
+                                        int nRamp,							/**< ... */
+                                        const real_t* const lb_new,			/**< ... */
+                                        real_t* const lb_new_far,			/**< ... */
+                                        const real_t* const ub_new,			/**< ... */
+                                        real_t* const ub_new_far			/**< ... */
+                                        ) const;
+
+
+		/** Performs robustified ratio test yield the maximum possible step length
+		 *  along the homotopy path.
+		 *	\return  SUCCESSFUL_RETURN */
+		returnValue performRatioTest(	int nIdx, 							/**< Number of ratios to be checked. */
+										const int* const idxList, 			/**< Array containing the indices of all ratios to be checked. */
+										const SubjectTo* const subjectTo,	/**< Bound/Constraint object corresponding to ratios to be checked. */
+										const real_t* const num,	 		/**< Array containing all numerators for performing the ratio test. */
+										const real_t* const den,		 	/**< Array containing all denominators for performing the ratio test. */
+										real_t epsNum,						/**< Numerator tolerance. */
+										real_t epsDen,						/**< Denominator tolerance. */
+										real_t& t,							/**< Output: Maximum possible step length along the homotopy path. */
+										int& BC_idx 						/**< Output: Index of blocking constraint. */
+										) const;
+
+		/** Checks whether given ratio is blocking, i.e. limits the maximum step length
+		 *  along the homotopy path to a value lower than given one.
+		 *	\return  SUCCESSFUL_RETURN */
+		inline BooleanType isBlocking(	real_t num,					 		/**< Numerator for performing the ratio test. */
+										real_t den,		 					/**< Denominator for performing the ratio test. */
+										real_t epsNum,						/**< Numerator tolerance. */
+										real_t epsDen,						/**< Denominator tolerance. */
+										real_t& t							/**< Input: Current maximum step length along the homotopy path,
+																			 *   Output: Updated maximum possible step length along the homotopy path. */
+										) const;
+
+
+		/** Creates a sparse diagonal (square-)matrix which is a given 
+		 *  multiple of the identity matrix.
+		 *  \return Diagonal matrix \n
+		 */
+		SymSparseMat* createDiagSparseMat(	int n,					/**< Row/column dimension of matrix to be created. */
+											real_t diagVal = 1.0	/**< Value of all diagonal entries. */
+											);
+
+
+	/*
+	 *	PRIVATE MEMBER FUNCTIONS
+	 */
+	private:
+		/** Solves a QProblemB whose QP data is assumed to be stored in the member variables.
+		 *  A guess for its primal/dual optimal solution vectors and the corresponding
+		 *  optimal working set can be provided.
+		 *  Note: This function is internally called by all init functions!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_INIT_FAILED \n
+					RET_INIT_FAILED_CHOLESKY \n
+					RET_INIT_FAILED_HOTSTART \n
+					RET_INIT_FAILED_INFEASIBILITY \n
+					RET_INIT_FAILED_UNBOUNDEDNESS \n
+					RET_MAX_NWSR_REACHED */
+		returnValue solveInitialQP(	const real_t* const xOpt,			/**< Optimal primal solution vector.
+																		 *	 A NULL pointer can be passed. */
+									const real_t* const yOpt,			/**< Optimal dual solution vector.
+																		 *	 A NULL pointer can be passed. */
+									const Bounds* const guessedBounds,	/**< Guessed working set for solution (xOpt,yOpt).
+																		 *	 A NULL pointer can be passed. */
+									int& nWSR, 							/**< Input: Maximum number of working set recalculations; \n
+																 		 *	 Output: Number of performed working set recalculations. */
+									real_t* const cputime				/**< Input: Maximum CPU time allowed for QP solution. \n
+																 			 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+									);
+
+		/** Solves an initialised QProblemB using online active set strategy.
+		 *  Note: This function is internally called by all hotstart functions!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_MAX_NWSR_REACHED \n
+					RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+		returnValue solveQP(	const real_t* const g_new,	/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
+													 			 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,	/**< Upper bounds of neighbouring QP to be solved. \n
+													 			 If no upper bounds exist, a NULL pointer can be passed. */
+								int& nWSR,					/**< Input: Maximum number of working set recalculations; \n
+																 Output: Number of performed working set recalculations. */
+								real_t* const cputime,		/**< Input: Maximum CPU time allowed for QP solution. \n
+																 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								int  nWSRperformed = 0		/**< Number of working set recalculations already performed to solve
+																 this QP within previous solveQP() calls. This number is
+																 always zero, except for successive calls from solveRegularisedQP()
+																 or when using the far bound strategy. */
+								);
+
+
+		/** Solves an initialised QProblemB using online active set strategy.
+		 *  Note: This function is internally called by all hotstart functions!
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_MAX_NWSR_REACHED \n
+					RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+		returnValue solveRegularisedQP(	const real_t* const g_new,	/**< Gradient of neighbouring QP to be solved. */
+										const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
+															 			 If no lower bounds exist, a NULL pointer can be passed. */
+										const real_t* const ub_new,	/**< Upper bounds of neighbouring QP to be solved. \n
+															 			 If no upper bounds exist, a NULL pointer can be passed. */
+										int& nWSR,					/**< Input: Maximum number of working set recalculations; \n
+																		 Output: Number of performed working set recalculations. */
+										real_t* const cputime,		/**< Input: Maximum CPU time allowed for QP solution. \n
+																		 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+										int  nWSRperformed = 0		/**< Number of working set recalculations already performed to solve
+																		 this QP within previous solveRegularisedQP() calls. This number is
+																		 always zero, except for successive calls when using the far bound strategy. */
+										);
+
+
+		/** Sets up bound data structure according to auxiliaryBounds.
+		 *  (If the working set shall be setup afresh, make sure that
+		 *  bounds data structure has been resetted!)
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_SETUP_WORKINGSET_FAILED \n
+					RET_INVALID_ARGUMENTS \n
+					RET_UNKNOWN_BUG */
+		returnValue setupAuxiliaryWorkingSet(	const Bounds* const auxiliaryBounds,	/**< Working set for auxiliary QP. */
+												BooleanType setupAfresh					/**< Flag indicating if given working set shall be
+																						 *    setup afresh or by updating the current one. */
+												);
+
+		/** Sets up the optimal primal/dual solution of the auxiliary initial QP.
+		 *	\return SUCCESSFUL_RETURN */
+		returnValue setupAuxiliaryQPsolution(	const real_t* const xOpt,			/**< Optimal primal solution vector.
+																				 	*	 If a NULL pointer is passed, all entries are set to zero. */
+												const real_t* const yOpt			/**< Optimal dual solution vector.
+																					 *	 If a NULL pointer is passed, all entries are set to zero. */
+												);
+
+		/** Sets up gradient of the auxiliary initial QP for given
+		 *  optimal primal/dual solution and given initial working set
+		 *  (assumes that members X, Y and BOUNDS have already been initialised!).
+		 *	\return SUCCESSFUL_RETURN */
+		returnValue setupAuxiliaryQPgradient( );
+
+		/** Sets up bounds of the auxiliary initial QP for given
+		 *  optimal primal/dual solution and given initial working set
+		 *  (assumes that members X, Y and BOUNDS have already been initialised!).
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_UNKNOWN_BUG */
+		returnValue setupAuxiliaryQPbounds( BooleanType useRelaxation	/**< Flag indicating if inactive bounds shall be relaxed. */
+											);
+
+
+		/** Updates QP vectors, working sets and internal data structures in order to
+			start from an optimal solution corresponding to initial guesses of the working
+			set for bounds
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_SETUP_AUXILIARYQP_FAILED */
+		returnValue setupAuxiliaryQP(	const Bounds* const guessedBounds	/**< Initial guess for working set of bounds. */
+										);
+
+		/** Determines step direction of the homotopy path.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_STEPDIRECTION_FAILED_CHOLESKY */
+		returnValue determineStepDirection(	const real_t* const delta_g,	/**< Step direction of gradient vector. */
+											const real_t* const delta_lb,	/**< Step direction of lower bounds. */
+											const real_t* const delta_ub,	/**< Step direction of upper bounds. */
+											BooleanType Delta_bB_isZero,	/**< Indicates if active bounds are to be shifted. */
+											real_t* const delta_xFX, 		/**< Output: Primal homotopy step direction of fixed variables. */
+											real_t* const delta_xFR,	 	/**< Output: Primal homotopy step direction of free variables. */
+											real_t* const delta_yFX 		/**< Output: Dual homotopy step direction of fixed variables' multiplier. */
+											);
+
+		/** Determines the maximum possible step length along the homotopy path
+		 *  and performs this step (without changing working set).
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_QP_INFEASIBLE \n
+		 */
+		returnValue performStep(	const real_t* const delta_g,	/**< Step direction of gradient. */
+									const real_t* const delta_lb,	/**< Step direction of lower bounds. */
+									const real_t* const delta_ub,	/**< Step direction of upper bounds. */
+									const real_t* const delta_xFX, 	/**< Primal homotopy step direction of fixed variables. */
+									const real_t* const delta_xFR,	/**< Primal homotopy step direction of free variables. */
+									const real_t* const delta_yFX,	/**< Dual homotopy step direction of fixed variables' multiplier. */
+									int& BC_idx, 					/**< Output: Index of blocking constraint. */
+									SubjectToStatus& BC_status		/**< Output: Status of blocking constraint. */
+									);
+
+		/** Updates active set.
+		 *	\return  SUCCESSFUL_RETURN \n
+		 			 RET_REMOVE_FROM_ACTIVESET_FAILED \n
+					 RET_ADD_TO_ACTIVESET_FAILED */
+		returnValue changeActiveSet(	int BC_idx, 						/**< Index of blocking constraint. */
+										SubjectToStatus BC_status 			/**< Status of blocking constraint. */
+										);
+
+		/** Drift correction at end of each active set iteration
+		 *  \return SUCCESSFUL_RETURN */
+		virtual returnValue performDriftCorrection( );
+
+		/** Determines if it is more efficient to refactorise the matrices when
+		 *  hotstarting or not (i.e. better to update the existing factorisations).
+		 *	\return BT_TRUE iff matrices shall be refactorised afresh
+		 */
+		BooleanType shallRefactorise(	const Bounds* const guessedBounds	/**< Guessed new working set. */
+										) const;
+
+
+		/** Adds a bound to active set (specialised version for the case where no constraints exist).
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_ADDBOUND_FAILED */
+		returnValue addBound(	int number,					/**< Number of bound to be added to active set. */
+								SubjectToStatus B_status,	/**< Status of new active bound. */
+								BooleanType updateCholesky	/**< Flag indicating if Cholesky decomposition shall be updated. */
+								);
+
+		/** Removes a bounds from active set (specialised version for the case where no constraints exist).
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_HESSIAN_NOT_SPD \n
+					RET_REMOVEBOUND_FAILED */
+		returnValue removeBound(	int number,					/**< Number of bound to be removed from active set. */
+									BooleanType updateCholesky	/**< Flag indicating if Cholesky decomposition shall be updated. */
+									);
+
+
+		/** Prints concise information on the current iteration.
+		 *	\return  SUCCESSFUL_RETURN \n */
+		returnValue printIteration(	int iter,					/**< Number of current iteration. */
+									int BC_idx, 				/**< Index of blocking bound. */
+									SubjectToStatus BC_status,	/**< Status of blocking bound. */
+									real_t homotopyLength		/**< Current homotopy distance. */
+									);
+
+
+	/*
+	 *	PROTECTED MEMBER VARIABLES
+	 */
+	protected:
+		BooleanType freeHessian;	/**< Flag indicating whether the Hessian matrix needs to be de-allocated. */
+		SymmetricMatrix* H;			/**< Hessian matrix. */
+
+		real_t* g;					/**< Gradient. */
+		real_t* lb;					/**< Lower bound vector (on variables). */
+		real_t* ub;					/**< Upper bound vector (on variables). */
+
+		Bounds bounds;				/**< Data structure for problem's bounds. */
+
+		real_t* R;					/**< Cholesky factor of H (i.e. H = R^T*R). */
+		BooleanType haveCholesky;	/**< Flag indicating whether Cholesky decomposition has already been setup. */
+
+		real_t* x;					/**< Primal solution vector. */
+		real_t* y;					/**< Dual solution vector. */
+
+		real_t tau;					/**< Last homotopy step length. */
+
+		QProblemStatus status;		/**< Current status of the solution process. */
+
+		BooleanType infeasible;		/**< QP infeasible? */
+		BooleanType unbounded;		/**< QP unbounded? */
+
+		HessianType hessianType;	/**< Type of Hessian matrix. */
+		real_t regVal;				/**< Holds the offset used to regularise Hessian matrix (zero by default). */
+
+		int count;					/**< Counts the number of hotstart function calls (internal usage only!). */
+
+		real_t *delta_xFR_TMP;		/**< Temporary for determineStepDirection */
+
+		real_t ramp0;				/**< Start value for Ramping Strategy. */
+		real_t ramp1;				/**< Final value for Ramping Strategy. */
+		int rampOffset;				/**< Offset index for Ramping. */
+
+		Options options;			/**< Struct containing all user-defined options for solving QPs. */
+		
+		Flipper flipper;			/**< Struct for making a temporary copy of the matrix factorisations. */
+
+		TabularOutput tabularOutput;	/**< Struct storing information for tabular output (printLevel == PL_TABULAR). */
+};
+
+
+END_NAMESPACE_QPOASES
+
+#include <qpOASES/QProblemB.ipp>
+
+#endif	/* QPOASES_QPROBLEMB_HPP */
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblemB.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblemB.ipp
new file mode 100644
index 00000000000..2a0922cae5e
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblemB.ipp
@@ -0,0 +1,477 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/QProblemB.ipp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of inlined member functions of the QProblemB class which
+ *	is able to use the newly developed online active set strategy for
+ *	parametric quadratic programming.
+ */
+
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+/*
+ *	g e t B o u n d s
+ */
+inline returnValue QProblemB::getBounds( Bounds& _bounds ) const
+{
+	int nV = getNV( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	_bounds = bounds;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t N V
+ */
+inline int QProblemB::getNV( ) const
+{
+	return bounds.getNV( );
+}
+
+
+/*
+ *	g e t N F R
+ */
+inline int QProblemB::getNFR( ) const
+{
+	return bounds.getNFR( );
+}
+
+
+/*
+ *	g e t N F X
+ */
+inline int QProblemB::getNFX( ) const
+{
+	return bounds.getNFX( );
+}
+
+
+/*
+ *	g e t N F V
+ */
+inline int QProblemB::getNFV( ) const
+{
+	return bounds.getNFV( );
+}
+
+
+/*
+ *	g e t S t a t u s
+ */
+inline QProblemStatus QProblemB::getStatus( ) const
+{
+	return status;
+}
+
+
+/*
+ *	i s I n i t i a l i s e d
+ */
+inline BooleanType QProblemB::isInitialised( ) const
+{
+	if ( status == QPS_NOTINITIALISED )
+		return BT_FALSE;
+	else
+		return BT_TRUE;
+}
+
+
+/*
+ *	i s S o l v e d
+ */
+inline BooleanType QProblemB::isSolved( ) const
+{
+	if ( status == QPS_SOLVED )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+/*
+ *	i s I n f e a s i b l e
+ */
+inline BooleanType QProblemB::isInfeasible( ) const
+{
+	return infeasible;
+}
+
+
+/*
+ *	i s U n b o u n d e d
+ */
+inline BooleanType QProblemB::isUnbounded( ) const
+{
+	return unbounded;
+}
+
+
+/*
+ *	g e t H e s s i a n T y p e
+ */
+inline HessianType QProblemB::getHessianType( ) const
+{
+	return hessianType;
+}
+
+
+/*
+ *	s e t H e s s i a n T y p e
+ */
+inline returnValue QProblemB::setHessianType( HessianType _hessianType )
+{
+	hessianType = _hessianType;
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	u s i n g R e g u l a r i s a t i o n
+ */
+inline BooleanType QProblemB::usingRegularisation( ) const
+{
+	if ( regVal > ZERO )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+/*
+ *	g e t O p t i o n s
+ */
+inline Options QProblemB::getOptions( ) const
+{
+	return options;
+}
+
+
+/*
+ *	s e t O p t i o n s
+ */
+inline returnValue QProblemB::setOptions(	const Options& _options
+											)
+{
+	options = _options;
+	options.ensureConsistency( );
+
+	setPrintLevel( options.printLevel );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t P r i n t L e v e l
+ */
+inline PrintLevel QProblemB::getPrintLevel( ) const
+{
+	return options.printLevel;
+}
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+/*
+ *	s e t H
+ */
+inline returnValue QProblemB::setH( SymmetricMatrix* H_new )
+{
+	if ( ( freeHessian == BT_TRUE ) && ( H != 0 ) )
+	{
+		delete H;
+		H = 0;
+	}
+
+	H = H_new;
+	freeHessian = BT_FALSE;
+	
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t H
+ */
+inline returnValue QProblemB::setH( const real_t* const H_new )
+{
+	int nV = getNV();
+	SymDenseMat* dH;
+
+	/* if null pointer is passed, Hessian is set to zero matrix
+	 *                            (or stays identity matrix) */
+	if ( H_new == 0 )
+	{
+		if ( hessianType == HST_IDENTITY )
+			return SUCCESSFUL_RETURN;
+
+		hessianType = HST_ZERO;
+
+		if ( ( freeHessian == BT_TRUE ) && ( H != 0 ) )
+			delete H;
+
+		H = 0;
+		freeHessian = BT_FALSE;
+	}
+	else
+	{
+		if ( ( freeHessian == BT_TRUE ) && ( H != 0 ) )
+			delete H;
+
+		H = dH = new SymDenseMat( nV, nV, nV, (real_t*) H_new );
+		freeHessian = BT_TRUE;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t G
+ */
+inline returnValue QProblemB::setG( const real_t* const g_new )
+{
+	unsigned int nV = getNV( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( g_new == 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	memcpy( g,g_new,nV*sizeof(real_t) );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t L B
+ */
+inline returnValue QProblemB::setLB( const real_t* const lb_new )
+{
+	unsigned int i;
+	unsigned int nV = getNV( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( lb_new != 0 )
+	{
+		memcpy( lb,lb_new,nV*sizeof(real_t) );
+	}
+	else
+	{
+		/* if no lower bounds are specified, set them to -infinity */
+		for( i=0; i<nV; ++i )
+			lb[i] = -INFTY;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t L B
+ */
+inline returnValue QProblemB::setLB( int number, real_t value )
+{
+	int nV = getNV( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ( number >= 0 ) && ( number < nV ) )
+	{
+		lb[number] = value;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+	{
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+	}
+}
+
+
+/*
+ *	s e t U B
+ */
+inline returnValue QProblemB::setUB( const real_t* const ub_new )
+{
+	unsigned int i;
+	unsigned int nV = getNV( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ub_new != 0 )
+	{
+		memcpy( ub,ub_new,nV*sizeof(real_t) );
+	}
+	else
+	{
+		/* if no upper bounds are specified, set them to infinity */
+		for( i=0; i<nV; ++i )
+			ub[i] = INFTY;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t U B
+ */
+inline returnValue QProblemB::setUB( int number, real_t value )
+{
+	int nV = getNV( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ( number >= 0 ) && ( number < nV ) )
+	{
+		ub[number] = value;
+
+		return SUCCESSFUL_RETURN;
+	}
+	else
+	{
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+	}
+}
+
+
+/*
+ *	c o m p u t e G i v e n s
+ */
+inline void QProblemB::computeGivens(	real_t xold, real_t yold, real_t& xnew, real_t& ynew,
+										real_t& c, real_t& s
+										) const
+{
+	real_t t, mu;
+
+	if ( isZero( yold ) == BT_TRUE )
+	{
+		c = 1.0;
+		s = 0.0;
+
+		xnew = xold;
+		ynew = yold;
+	}
+	else
+	{
+		mu = getAbs( xold );
+		if ( getAbs( yold ) > mu )
+			mu = getAbs( yold );
+
+		t = mu * getSqrt( (xold/mu)*(xold/mu) + (yold/mu)*(yold/mu) );
+
+		if ( xold < 0.0 )
+		t = -t;
+
+		c = xold/t;
+		s = yold/t;
+		xnew = t;
+		ynew = 0.0;
+	}
+
+	return;
+}
+
+
+/*
+ *	a p p l y G i v e n s
+ */
+inline void QProblemB::applyGivens(	real_t c, real_t s, real_t nu, real_t xold, real_t yold,
+									real_t& xnew, real_t& ynew
+									) const
+{
+	#ifdef __USE_THREE_MULTS_GIVENS__
+
+	/* Givens plane rotation requiring only three multiplications,
+	 * cf. Hammarling, S.: A note on modifications to the givens plane rotation.
+	 * J. Inst. Maths Applics, 13:215-218, 1974. */
+	xnew = xold*c + yold*s;
+	ynew = (xnew+xold)*nu - yold;
+
+	#else
+
+	/* Usual Givens plane rotation requiring four multiplications. */
+	xnew =  c*xold + s*yold;
+	ynew = -s*xold + c*yold;
+
+	#endif
+
+	return;
+}
+
+
+/*
+ * i s B l o c k i n g
+ */
+inline BooleanType QProblemB::isBlocking(	real_t num,
+											real_t den,
+											real_t epsNum,
+											real_t epsDen,
+											real_t& t
+											) const
+{
+	if ( ( den >= epsDen ) && ( num >= epsNum ) )
+	{
+		if ( num < t*den )
+			return BT_TRUE;
+	}
+
+	return BT_FALSE;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SQProblem.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SQProblem.hpp
new file mode 100644
index 00000000000..aec5adab23f
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SQProblem.hpp
@@ -0,0 +1,394 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/SQProblem.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Declaration of the SQProblem class which is able to use the newly
+ *	developed online active set strategy for parametric quadratic programming
+ *	with varying matrices.
+ */
+
+
+
+#ifndef QPOASES_SQPROBLEM_HPP
+#define QPOASES_SQPROBLEM_HPP
+
+
+#include <qpOASES/QProblem.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** 
+ *	\brief Implements the online active set strategy for QPs with varying matrices.
+ *
+ *	A class for setting up and solving quadratic programs with varying QP matrices.
+ *	The main feature is the possibily to use the newly developed online active set strategy
+ *	for parametric quadratic programming.
+ *
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ */
+class SQProblem : public QProblem
+{
+	/* allow SolutionAnalysis class to access private members */
+	friend class SolutionAnalysis;
+
+	/*
+	 *	PUBLIC MEMBER FUNCTIONS
+	 */
+	public:
+		/** Default constructor. */
+		SQProblem( );
+
+		/** Constructor which takes the QP dimension and Hessian type
+		 *  information. If the Hessian is the zero (i.e. HST_ZERO) or the
+		 *  identity matrix (i.e. HST_IDENTITY), respectively, no memory
+		 *  is allocated for it and a NULL pointer can be passed for it
+		 *  to the init() functions. */
+		SQProblem(	int _nV,	  							/**< Number of variables. */
+					int _nC,  								/**< Number of constraints. */
+					HessianType _hessianType = HST_UNKNOWN	/**< Type of Hessian matrix. */
+					);
+
+		/** Copy constructor (deep copy). */
+		SQProblem(	const SQProblem& rhs	/**< Rhs object. */
+					);
+
+		/** Destructor. */
+		virtual ~SQProblem( );
+
+		/** Assignment operator (deep copy). */
+		SQProblem& operator=(	const SQProblem& rhs	/**< Rhs object. */
+								);
+
+
+		/** Solves an initialised SQProblem using online active set strategy.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MAX_NWSR_REACHED \n
+		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_MATRIX_SHIFT_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+					RET_SETUP_AUXILIARYQP_FAILED */
+		returnValue hotstart(	const real_t* const H_new,	/**< Hessian matrix of neighbouring QP to be solved (a shallow copy is made). \n
+																 If Hessian matrix is trivial, a NULL pointer can be passed. */
+								const real_t* const g_new,	/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const A_new,	/**< Constraint matrix of neighbouring QP to be solved (a shallow copy is made). \n
+																 If QP sequence does not involve constraints, a NULL pointer can be passed. */
+								const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
+													 		 	 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,	/**< Upper bounds of neighbouring QP to be solved. \n
+													 		 	 If no upper bounds exist, a NULL pointer can be passed. */
+								const real_t* const lbA_new,/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+													 		 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const real_t* const ubA_new,/**< Upper constraints' bounds of neighbouring QP to be solved. \n
+												 			 	 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int& nWSR,					/**< Input: Maximum number of working set recalculations; \n
+																 Output: Number of performed working set recalculations. */
+								real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP solution. \n
+																 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								);
+
+		/** Solves an initialised SQProblem using online active set strategy
+		 *  reading QP data from files.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MAX_NWSR_REACHED \n
+		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_MATRIX_SHIFT_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+					RET_SETUP_AUXILIARYQP_FAILED \n
+					RET_UNABLE_TO_READ_FILE \n
+					RET_INVALID_ARGUMENTS */
+		returnValue hotstart(	const char* const H_file,	/**< Name of file where Hessian matrix is stored. \n
+																 If Hessian matrix is trivial, a NULL pointer can be passed. */
+								const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+								const char* const A_file,	/**< Name of file where constraint matrix is stored. \n
+																 If QP sequence does not involve constraints, a NULL pointer can be passed. */
+								const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+													 			 If no lower bounds exist, a NULL pointer can be passed. */
+								const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+													 			 If no upper bounds exist, a NULL pointer can be passed. */
+								const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
+													 			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
+													 			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int& nWSR,					/**< Input: Maximum number of working set recalculations; \n
+																 Output: Number of performed working set recalculations. */
+								real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP solution. \n
+																 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								);
+
+		/** Solves an initialised SQProblem using online active set strategy.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MAX_NWSR_REACHED \n
+		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_MATRIX_SHIFT_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+					RET_SETUP_AUXILIARYQP_FAILED */
+		returnValue hotstart(	SymmetricMatrix *H_new,		/**< Hessian matrix of neighbouring QP to be solved (a shallow copy is made). \n
+																 If Hessian matrix is trivial, a NULL pointer can be passed. */
+								const real_t* const g_new,	/**< Gradient of neighbouring QP to be solved. */
+								Matrix *A_new,				/**< Constraint matrix of neighbouring QP to be solved (a shallow copy is made). \n
+																 If QP sequence does not involve constraints, a NULL pointer can be passed. */
+								const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
+													 		 	 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,	/**< Upper bounds of neighbouring QP to be solved. \n
+													 		 	 If no upper bounds exist, a NULL pointer can be passed. */
+								const real_t* const lbA_new,/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+													 		 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const real_t* const ubA_new,/**< Upper constraints' bounds of neighbouring QP to be solved. \n
+												 			 	 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int& nWSR,					/**< Input: Maximum number of working set recalculations; \n
+																 Output: Number of performed working set recalculations. */
+								real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP solution. \n
+																 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								);
+
+		/** Solves an initialised SQProblem (without matrix shift) using online active set strategy.
+		 *  Note: This functions just forwards to the corresponding
+		 *  	  QProblem::hotstart member function.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MAX_NWSR_REACHED \n
+		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+		returnValue hotstart(	const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const lb_new,		/**< Lower bounds of neighbouring QP to be solved. \n
+													 			 	 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
+													 			 	 If no upper bounds exist, a NULL pointer can be passed. */
+								const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+													 			 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
+													 			 	 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int& nWSR,						/**< Input: Maximum number of working set recalculations; \n
+																	 Output: Number of performed working set recalculations. */
+								real_t* const cputime = 0		/**< Input: Maximum CPU time allowed for QP solution. \n
+																	 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								);
+
+		/** Solves an initialised SQProblem (without matrix shift) using online active set strategy
+		 *  reading QP data from files.
+		 *  Note: This functions just forwards to the corresponding
+		 *  	  QProblem::hotstart member function.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MAX_NWSR_REACHED \n
+		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+					RET_UNABLE_TO_READ_FILE */
+		returnValue hotstart(	const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+								const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+													 			 If no lower bounds exist, a NULL pointer can be passed. */
+								const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+													 			 If no upper bounds exist, a NULL pointer can be passed. */
+								const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
+													 			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
+													 			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int& nWSR, 					/**< Input: Maximum number of working set recalculations; \n
+																 Output: Number of performed working set recalculations. */
+								real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP solution. \n
+																 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								);
+
+		/** Solves an initialised SQProblem (without matrix shift) using online active set strategy
+		 *  (using an initialised homotopy).
+		 *  Note: This functions just forwards to the corresponding
+		 *  	  QProblem::hotstart member function.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MAX_NWSR_REACHED \n
+		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+					RET_WORKINGSET_UPDATE_FAILED */
+		returnValue hotstart(	const real_t* const g_new,					/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const lb_new,					/**< Lower bounds of neighbouring QP to be solved. \n
+													 						 	 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,					/**< Upper bounds of neighbouring QP to be solved. \n
+													 						 	 If no upper bounds exist, a NULL pointer can be passed. */
+								const real_t* const lbA_new,				/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+													 						 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const real_t* const ubA_new,				/**< Upper constraints' bounds of neighbouring QP to be solved. \n
+													 							 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int& nWSR,									/**< Input: Maximum number of working set recalculations; \n
+																				 Output: Number of performed working set recalculations. */
+								real_t* const cputime,						/**< Input: Maximum CPU time allowed for QP solution. \n
+																 	 			 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								const Bounds* const guessedBounds,			/**< Initial guess for working set of bounds.
+																			 *   A null pointer corresponds to an empty working set! */
+								const Constraints* const guessedConstraints	/**< Initial guess for working set of constraints.
+																			 *   A null pointer corresponds to an empty working set! */
+								);
+
+		/** Solves an initialised SQProblem (without matrix shift) using online active set strategy
+		 * (using an initialised homotopy) reading QP data from files.
+		 *  Note: This functions just forwards to the corresponding
+		 *  	  QProblem::hotstart member function.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_MAX_NWSR_REACHED \n
+		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+					RET_HOTSTART_FAILED \n
+					RET_SHIFT_DETERMINATION_FAILED \n
+					RET_STEPDIRECTION_DETERMINATION_FAILED \n
+					RET_STEPLENGTH_DETERMINATION_FAILED \n
+					RET_HOMOTOPY_STEP_FAILED \n
+					RET_HOTSTART_STOPPED_INFEASIBILITY \n
+					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+					RET_WORKINGSET_UPDATE_FAILED \n
+					RET_UNABLE_TO_READ_FILE */
+		returnValue hotstart(	const char* const g_file, 					/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+								const char* const lb_file, 					/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+													 							 If no lower bounds exist, a NULL pointer can be passed. */
+								const char* const ub_file, 					/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+													 							 If no upper bounds exist, a NULL pointer can be passed. */
+								const char* const lbA_file, 				/**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
+													 							 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const char* const ubA_file, 				/**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
+													 							 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int& nWSR,									/**< Input: Maximum number of working set recalculations; \n
+																		 		 Output: Number of performed working set recalculations. */
+								real_t* const cputime,						/**< Input: Maximum CPU time allowed for QP solution. \n
+																 	 			 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
+								const Bounds* const guessedBounds,			/**< Initial guess for working set of bounds.
+																			 *   A null pointer corresponds to an empty working set! */
+								const Constraints* const guessedConstraints	/**< Initial guess for working set of constraints.
+																			 *   A null pointer corresponds to an empty working set! */
+								);
+
+
+
+		#ifdef __MATLAB__
+		/** Sets pointer of Hessian and constraint matrix to zero.
+		 *  QUICK HACK FOR MAKING THE MATLAB INTERFACE RUN! TO BE REMOVED!
+		 *	\return SUCCESSFUL_RETURN */
+		returnValue resetMatrixPointers( );
+		#endif
+
+
+	/*
+	 *	PROTECTED MEMBER FUNCTIONS
+	 */
+	protected:
+		/** Sets new matrices and calculates their factorisations. If the
+		 *  current Hessian is trivial (i.e. HST_ZERO or HST_IDENTITY) but a
+		 *  non-trivial one is given, memory for Hessian is allocated and
+		 *  it is set to the given one. Afterwards, all QP vectors are
+		 *  transformed in order to start from an optimal solution.
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_MATRIX_FACTORISATION_FAILED \n
+		 * 			RET_NO_HESSIAN_SPECIFIED */
+		virtual returnValue setupAuxiliaryQP(	const real_t* const H_new,	/**< New Hessian matrix. \n
+																     		 	 If Hessian matrix is trivial, a NULL pointer can be passed. */
+												const real_t* const A_new,	/**< New constraint matrix. \n
+																     		 	 If QP sequence does not involve constraints, a NULL pointer can be passed. */
+												const real_t *lb_new,		/**< New lower bounds. \n
+													 						 	 If no lower bounds exist, a NULL pointer can be passed. */
+												const real_t *ub_new,		/**< New upper bounds. \n
+													 						 	 If no lower bounds exist, a NULL pointer can be passed. */
+												const real_t *lbA_new,		/**< New lower constraints' bounds. \n
+													 						 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+												const real_t *ubA_new		/**< New lower constraints' bounds. \n
+													 						 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+												);
+
+		/** Sets new matrices and calculates their factorisations. If the
+		 *  current Hessian is trivial (i.e. HST_ZERO or HST_IDENTITY) but a
+		 *  non-trivial one is given, memory for Hessian is allocated and
+		 *  it is set to the given one. Afterwards, all QP vectors are
+		 *  transformed in order to start from an optimal solution.
+		 *	\return SUCCESSFUL_RETURN \n
+		 *			RET_MATRIX_FACTORISATION_FAILED \n
+		 * 			RET_NO_HESSIAN_SPECIFIED */
+		virtual returnValue setupAuxiliaryQP(	SymmetricMatrix *H_new,		/**< New Hessian matrix. \n
+																				 If Hessian matrix is trivial, a NULL pointer can be passed. */
+												Matrix *A_new,				/**< New constraint matrix. \n
+																				 If QP sequence does not involve constraints, a NULL pointer can be passed. */
+												const real_t *lb_new,		/**< New lower bounds. \n
+													 						 	 If no lower bounds exist, a NULL pointer can be passed. */
+												const real_t *ub_new,		/**< New upper bounds. \n
+													 						 	 If no lower bounds exist, a NULL pointer can be passed. */
+												const real_t *lbA_new,		/**< New lower constraints' bounds. \n
+													 						 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+												const real_t *ubA_new		/**< New lower constraints' bounds. \n
+													 						 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+												);
+
+	/*
+	 *	PROTECTED MEMBER VARIABLES
+	 */
+	protected:
+
+};
+
+
+END_NAMESPACE_QPOASES
+
+#include <qpOASES/SQProblem.ipp>
+
+#endif	/* QPOASES_SQPROBLEM_HPP */
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SQProblem.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SQProblem.ipp
new file mode 100644
index 00000000000..61a88faeb32
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SQProblem.ipp
@@ -0,0 +1,51 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/SQProblem.ipp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of inlined member functions of the SQProblem class which
+ *	is able to use the newly developed online active set strategy for
+ *	parametric quadratic programming with varying matrices.
+ */
+
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SubjectTo.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SubjectTo.hpp
new file mode 100644
index 00000000000..9d3d4bea812
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SubjectTo.hpp
@@ -0,0 +1,229 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/SubjectTo.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Declaration of the SubjectTo class designed to manage working sets of
+ *	constraints and bounds within a QProblem.
+ */
+
+
+#ifndef QPOASES_SUBJECTTO_HPP
+#define QPOASES_SUBJECTTO_HPP
+
+
+#include <qpOASES/Indexlist.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** 
+ *	\brief Base class for managing working sets of bounds and constraints.
+ *
+ *	This class manages working sets of bounds and constraints by storing
+ *	index sets and other status information.
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ */
+class SubjectTo
+{
+	/*
+	 *	PUBLIC MEMBER FUNCTIONS
+	 */
+	public:
+		/** Default constructor. */
+		SubjectTo( );
+
+		/** Constructor which takes the number of constraints or bounds. */
+		SubjectTo(	int _n 								/**< Number of constraints or bounds. */
+					);
+
+		/** Copy constructor (deep copy). */
+		SubjectTo(	const SubjectTo& rhs				/**< Rhs object. */
+					);
+
+		/** Destructor. */
+		virtual ~SubjectTo( );
+
+		/** Assignment operator (deep copy). */
+		SubjectTo& operator=(	const SubjectTo& rhs	/**< Rhs object. */
+								);
+
+
+		/** Initialises object with given number of constraints or bounds.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_INVALID_ARGUMENTS */
+		returnValue init(	int _n = 0					/**< Number of constraints or bounds. */
+							);
+
+
+		/** Returns number of constraints/bounds with given SubjectTo type.
+		 *	\return Number of constraints/bounds with given type. */
+		inline int getNumberOfType(	SubjectToType _type	/**< Type of (constraints') bound. */
+									) const;
+
+
+		/** Returns type of (constraints') bound.
+		 *	\return Type of (constraints') bound \n
+		 			RET_INDEX_OUT_OF_BOUNDS */
+		inline SubjectToType getType(	int i			/**< Number of (constraints') bound. */
+										) const;
+
+		/** Returns status of (constraints') bound.
+		 *	\return Status of (constraints') bound \n
+		 			ST_UNDEFINED */
+		inline SubjectToStatus getStatus(	int i		/**< Number of (constraints') bound. */
+											) const;
+
+
+		/** Sets type of (constraints') bound.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_INDEX_OUT_OF_BOUNDS */
+		inline returnValue setType(	int i,				/**< Number of (constraints') bound. */
+									SubjectToType value	/**< Type of (constraints') bound. */
+									);
+
+		/** Sets status of (constraints') bound.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_INDEX_OUT_OF_BOUNDS */
+		inline returnValue setStatus(	int i,					/**< Number of (constraints') bound. */
+										SubjectToStatus value	/**< Status of (constraints') bound. */
+										);
+
+
+		/** Sets status of lower (constraints') bounds. */
+		inline void setNoLower(	BooleanType _status		/**< Status of lower (constraints') bounds. */
+								);
+
+		/** Sets status of upper (constraints') bounds. */
+		inline void setNoUpper(	BooleanType _status		/**< Status of upper (constraints') bounds. */
+								);
+
+
+		/** Returns status of lower (constraints') bounds.
+		 *	\return BT_TRUE if there is no lower (constraints') bound on any variable. */
+		inline BooleanType hasNoLower( ) const;
+
+		/** Returns status of upper bounds.
+		 *	\return BT_TRUE if there is no upper (constraints') bound on any variable. */
+		inline BooleanType hasNoUpper( ) const;
+
+
+		/** Shifts forward type and status of all constraints/bounds by a given
+		 *  offset. This offset has to lie within the range [0,n/2] and has to
+		 *  be an integer divisor of the total number of constraints/bounds n.
+		 *  Type and status of the first \<offset\> constraints/bounds is thrown away,
+		 *  type and status of the last \<offset\> constraints/bounds is doubled,
+		 *  e.g. for offset = 2: \n
+		 *  shift( {c/b1,c/b2,c/b3,c/b4,c/b5,c/b6} ) = {c/b3,c/b4,c/b5,c/b6,c/b5,c/b6}
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_INDEX_OUT_OF_BOUNDS \n
+		 			RET_INVALID_ARGUMENTS \n
+		 			RET_SHIFTING_FAILED */
+		virtual returnValue shift(	int offset		/**< Shift offset within the range [0,n/2] and integer divisor of n. */
+									) = 0;
+
+		/** Rotates forward type and status of all constraints/bounds by a given
+		 *  offset. This offset has to lie within the range [0,n].
+		 *  Example for offset = 2: \n
+		 *  rotate( {c/b1,c/b2,c/b3,c/b4,c/b5,c/b6} ) = {c/b3,c/b4,c/b5,c/b6,c/b1,c/b2}
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_INDEX_OUT_OF_BOUNDS \n
+		 			RET_ROTATING_FAILED */
+		virtual returnValue rotate(	int offset		/**< Rotation offset within the range [0,n]. */
+									) = 0;
+
+
+	/*
+	 *	PROTECTED MEMBER FUNCTIONS
+	 */
+	protected:
+		/** Frees all allocated memory.
+		 *  \return SUCCESSFUL_RETURN */
+		returnValue clear( );
+		
+		/** Copies all members from given rhs object.
+		 *  \return SUCCESSFUL_RETURN */
+		returnValue copy(	const SubjectTo& rhs	/**< Rhs object. */
+							);
+
+
+		/** Adds the index of a new constraint or bound to index set.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_ADDINDEX_FAILED \n
+					RET_INVALID_ARGUMENTS */
+		returnValue addIndex(	Indexlist* const indexlist,	/**< Index list to which the new index shall be added. */
+								int newnumber,				/**< Number of new constraint or bound. */
+								SubjectToStatus newstatus	/**< Status of new constraint or bound. */
+								);
+
+		/** Removes the index of a constraint or bound from index set.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_REMOVEINDEX_FAILED \n
+					RET_INVALID_ARGUMENTS */
+		returnValue removeIndex(	Indexlist* const indexlist,	/**< Index list from which the new index shall be removed. */
+									int removenumber			/**< Number of constraint or bound to be removed. */
+									);
+
+		/** Swaps the indices of two constraints or bounds within the index set.
+		 *	\return SUCCESSFUL_RETURN \n
+		 			RET_SWAPINDEX_FAILED \n
+					RET_INVALID_ARGUMENTS */
+		returnValue swapIndex(	Indexlist* const indexlist,	/**< Index list in which the indices shold be swapped. */
+								int number1,				/**< Number of first constraint or bound. */
+								int number2					/**< Number of second constraint or bound. */
+								);
+
+
+	/*
+	 *	PROTECTED MEMBER VARIABLES
+	 */
+	protected:
+		int n;						/**< Total number of constraints/bounds. */
+
+		SubjectToType* type; 		/**< Type of constraints/bounds. */
+		SubjectToStatus* status;	/**< Status of constraints/bounds. */
+
+		BooleanType noLower;	 	/**< This flag indicates if there is no lower bound on any variable. */
+		BooleanType noUpper;	 	/**< This flag indicates if there is no upper bound on any variable. */
+};
+
+
+END_NAMESPACE_QPOASES
+
+#include <qpOASES/SubjectTo.ipp>
+
+#endif	/* QPOASES_SUBJECTTO_HPP */
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SubjectTo.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SubjectTo.ipp
new file mode 100644
index 00000000000..086f54ca528
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SubjectTo.ipp
@@ -0,0 +1,158 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/SubjectTo.ipp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of the inlined member functions of the SubjectTo class
+ *	designed to manage working sets of constraints and bounds within a QProblem.
+ */
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+/*
+ *	g e t N u m b e r O f T y p e
+ */
+inline int SubjectTo::getNumberOfType( SubjectToType _type ) const
+{
+	int i;
+	int numberOfType = 0;
+
+	if ( type != 0 )
+	{
+		for( i=0; i<n; ++i )
+			if ( type[i] == _type )
+				++numberOfType;
+	}
+
+	return numberOfType;
+}
+
+
+/*
+ *	g e t T y p e
+ */
+inline SubjectToType SubjectTo::getType( int i ) const
+{
+	if ( ( i >= 0 ) && ( i < n ) )
+		return type[i];
+
+	return ST_UNKNOWN;
+}
+
+
+/*
+ *	g e t S t a t u s
+ */
+inline SubjectToStatus SubjectTo::getStatus( int i ) const
+{
+	if ( ( i >= 0 ) && ( i < n ) )
+		return status[i];
+
+	return ST_UNDEFINED;
+}
+
+
+/*
+ *	s e t T y p e
+ */
+inline returnValue SubjectTo::setType( int i, SubjectToType value )
+{
+	if ( ( i >= 0 ) && ( i < n ) )
+	{
+		type[i] = value;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+}
+
+
+/*
+ *	s e t S t a t u s
+ */
+inline returnValue SubjectTo::setStatus( int i, SubjectToStatus value )
+{
+	if ( ( i >= 0 ) && ( i < n ) )
+	{
+		status[i] = value;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+}
+
+
+/*
+ *	s e t N o L o w e r
+ */
+inline void SubjectTo::setNoLower( BooleanType _status )
+{
+	noLower = _status;
+}
+
+
+/*
+ *	s e t N o U p p e r
+ */
+inline void SubjectTo::setNoUpper( BooleanType _status )
+{
+	noUpper = _status;
+}
+
+
+/*
+ *	h a s N o L o w e r
+ */
+inline BooleanType SubjectTo::hasNoLower( ) const
+{
+	return noLower;
+}
+
+
+/*
+ *	h a s N o U p p p e r
+ */
+inline BooleanType SubjectTo::hasNoUpper( ) const
+{
+	return noUpper;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Types.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Types.hpp
new file mode 100644
index 00000000000..fc42d1288c7
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Types.hpp
@@ -0,0 +1,281 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/Types.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Declaration of all non-built-in types (except for classes).
+ */
+
+
+#ifndef QPOASES_TYPES_HPP
+#define QPOASES_TYPES_HPP
+
+
+/* If your compiler does not support the snprintf() function,
+ * uncomment the following line and try to compile again. */
+/* #define __NO_SNPRINTF__ */
+
+
+/* Uncomment the following line for setting the __DSPACE__ flag. */
+/* #define __DSPACE__ */
+
+/* Uncomment the following line for setting the __XPCTARGET__ flag. */
+/* #define __XPCTARGET__ */
+
+
+/* Uncomment the following line for setting the __NO_FMATH__ flag. */
+/* #define __NO_FMATH__ */
+
+/* Uncomment the following line to enable debug information. */
+/* #define __DEBUG__ */
+
+/* Uncomment the following line to enable suppress any kind of console output. */
+/* #define __SUPPRESSANYOUTPUT__ */
+
+
+/** Forces to always include all implicitly fixed bounds and all equality constraints
+ *  into the initial working set when setting up an auxiliary QP. */
+#define __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
+
+
+/* Uncomment the following line to activate the use of an alternative Givens 
+ * plane rotation requiring only three multiplications. */
+/* #define __USE_THREE_MULTS_GIVENS__ */
+
+/* Uncomment the following line to activate the use of single precision arithmetic. */
+/* #define __USE_SINGLE_PRECISION__ */
+
+
+
+/* Work-around for Borland BCC 5.5 compiler. */
+#ifdef __BORLANDC__
+#if __BORLANDC__ < 0x0561
+  #define __STDC__ 1
+#endif
+#endif
+
+
+/* Work-around for Microsoft compilers. */
+#ifdef _MSC_VER
+  #define __NO_SNPRINTF__
+  #pragma warning( disable : 4061 4100 4250 4514 4996 )
+#endif
+
+
+#ifdef __DSPACE__
+
+	#define __NO_SNPRINTF__
+
+	/** Macro for switching on/off the beginning of the qpOASES namespace definition. */
+	#define BEGIN_NAMESPACE_QPOASES
+    
+	/** Macro for switching on/off the end of the qpOASES namespace definition. */
+	#define END_NAMESPACE_QPOASES
+
+	/** Macro for switching on/off the use of the qpOASES namespace. */
+	#define USING_NAMESPACE_QPOASES
+
+	/** Macro for switching on/off references to the qpOASES namespace. */
+	#define REFER_NAMESPACE_QPOASES ::
+
+#else
+
+	/** Macro for switching on/off the beginning of the qpOASES namespace definition. */
+	#define BEGIN_NAMESPACE_QPOASES  namespace qpOASES {
+
+	/** Macro for switching on/off the end of the qpOASES namespace definition. */
+	#define END_NAMESPACE_QPOASES    }
+	
+	/** Macro for switching on/off the use of the qpOASES namespace. */
+	#define USING_NAMESPACE_QPOASES  using namespace qpOASES;
+	
+	/** Macro for switching on/off references to the qpOASES namespace. */
+	#define REFER_NAMESPACE_QPOASES  qpOASES::
+
+#endif
+
+
+/** Macro for accessing the Cholesky factor R. */
+#define RR( I,J )  R[(I)+nV*(J)]
+
+/** Macro for accessing the orthonormal matrix Q of the QT factorisation. */
+#define QQ( I,J )  Q[(I)+nV*(J)]
+
+/** Macro for accessing the triangular matrix T of the QT factorisation. */
+#define TT( I,J )  T[(I)*sizeT+(J)]
+
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** Defines real_t for facilitating switching between double and float. */
+#ifdef __USE_SINGLE_PRECISION__
+typedef float real_t;
+#else
+typedef double real_t;
+#endif /* __USE_SINGLE_PRECISION__ */
+
+
+/** Summarises all possible logical values. */
+enum BooleanType
+{
+	BT_FALSE,					/**< Logical value for "false". */
+	BT_TRUE						/**< Logical value for "true". */
+};
+
+
+/** Summarises all possible print levels. Print levels are used to describe
+ *	the desired amount of output during runtime of qpOASES. */
+enum PrintLevel
+{
+	PL_DEBUG_ITER = -2,			/**< Full tabular debugging output. */
+	PL_TABULAR,					/**< Normal tabular output. */
+	PL_NONE,					/**< No output. */
+	PL_LOW,						/**< Print error messages only. */
+	PL_MEDIUM,					/**< Print error and warning messages as well as concise info messages. */
+	PL_HIGH						/**< Print all messages with full details. */
+};
+
+
+/** Defines visibility status of a message. */
+enum VisibilityStatus
+{
+	VS_HIDDEN,					/**< Message not visible. */
+	VS_VISIBLE					/**< Message visible. */
+};
+
+
+/** Summarises all possible states of the (S)QProblem(B) object during the
+solution process of a QP sequence. */
+enum QProblemStatus
+{
+	QPS_NOTINITIALISED,			/**< QProblem object is freshly instantiated or reset. */
+	QPS_PREPARINGAUXILIARYQP,	/**< An auxiliary problem is currently setup, either at the very beginning
+								 *   via an initial homotopy or after changing the QP matrices. */
+	QPS_AUXILIARYQPSOLVED,		/**< An auxilary problem was solved, either at the very beginning
+								 *   via an initial homotopy or after changing the QP matrices. */
+	QPS_PERFORMINGHOMOTOPY,		/**< A homotopy according to the main idea of the online active
+								 *   set strategy is performed. */
+	QPS_HOMOTOPYQPSOLVED,		/**< An intermediate QP along the homotopy path was solved. */
+	QPS_SOLVED					/**< The solution of the actual QP was found. */
+};
+
+
+/** Summarises all possible types of the QP's Hessian matrix. */
+enum HessianType
+{
+	HST_ZERO,				/**< Hessian is zero matrix (i.e. LP formulation). */
+	HST_IDENTITY,			/**< Hessian is identity matrix. */
+	HST_POSDEF,				/**< Hessian is (strictly) positive definite. */
+	HST_POSDEF_NULLSPACE,	/**< Hessian is positive definite on null space of active bounds/constraints. */
+	HST_SEMIDEF,			/**< Hessian is positive semi-definite. */
+	HST_INDEF,				/**< Hessian is indefinite. */
+	HST_UNKNOWN				/**< Hessian type is unknown. */
+};
+
+
+/** Summarises all possible types of bounds and constraints. */
+enum SubjectToType
+{
+	ST_UNBOUNDED,		/**< Bound/constraint is unbounded. */
+	ST_BOUNDED,			/**< Bound/constraint is bounded but not fixed. */
+	ST_EQUALITY,		/**< Bound/constraint is fixed (implicit equality bound/constraint). */
+	ST_DISABLED,		/**< Bound/constraint is disabled (i.e. ignored when solving QP). */ 
+	ST_UNKNOWN			/**< Type of bound/constraint unknown. */
+};
+
+
+/** Summarises all possible states of bounds and constraints. */
+enum SubjectToStatus
+{
+	ST_LOWER = -1,			/**< Bound/constraint is at its lower bound. */
+	ST_INACTIVE,			/**< Bound/constraint is inactive. */
+	ST_UPPER,				/**< Bound/constraint is at its upper bound. */
+	ST_INFEASIBLE_LOWER,	/**< (to be documented) */
+	ST_INFEASIBLE_UPPER,	/**< (to be documented) */
+	ST_UNDEFINED			/**< Status of bound/constraint undefined. */
+};
+
+/**
+ *	\brief Stores internal information for tabular (debugging) output.
+ *
+ *	Struct storing internal information for tabular (debugging) output 
+ *	when using the (S)QProblem(B) objects.
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2013-2014
+ */
+struct TabularOutput {
+	int idxAddB;		/**< Index of bound that has been added to working set. */
+	int idxRemB;		/**< Index of bound that has been removed from working set. */
+	int idxAddC;		/**< Index of constraint that has been added to working set. */
+	int idxRemC;		/**< Index of constraint that has been removed from working set. */
+	int excAddB;		/**< Flag indicating whether a bound has been added to working set to keep a regular projected Hessian. */
+	int excRemB;		/**< Flag indicating whether a bound has been removed from working set to keep a regular projected Hessian. */
+	int excAddC;		/**< Flag indicating whether a constraint has been added to working set to keep a regular projected Hessian. */
+	int excRemC;		/**< Flag indicating whether a constraint has been removed from working set to keep a regular projected Hessian. */
+};
+
+
+
+/**
+ *	\brief Struct containing the variable header for mat file.
+ *
+ *	Struct storing the header of a variable to be stored in 
+ *	Matlab's binary format (using the outdated Level 4 variant 
+ *  for simplictiy).
+ *
+ *  Note, this code snippet has been inspired from the document
+ *  "Matlab(R) MAT-file Format, R2013b" by MathWorks
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2013-2014
+ */ 
+typedef struct {
+	long numericFormat;		/**< Flag indicating numerical format. */
+	long nRows;				/**< Number of rows. */
+	long nCols;				/**< Number of rows. */
+	long imaginaryPart;		/**< (to be documented) */
+	long nCharName;			/**< Number of character in name. */
+} MatMatrixHeader;
+
+
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_TYPES_HPP */
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/UnitTesting.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/UnitTesting.hpp
new file mode 100644
index 00000000000..96ade4b8bb2
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/UnitTesting.hpp
@@ -0,0 +1,79 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/UnitTesting.hpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2014
+ *
+ *	Definition of auxiliary functions/macros for unit testing.
+ */
+
+
+#ifndef QPOASES_UNIT_TESTING_HPP
+#define QPOASES_UNIT_TESTING_HPP
+
+
+#ifndef TEST_TOL_FACTOR
+#define TEST_TOL_FACTOR 1
+#endif
+
+
+/** Return value for tests that passed. */
+#define TEST_PASSED 0
+
+/** Return value for tests that failed. */
+#define TEST_FAILED 1
+
+/** Return value for tests that could not run due to missing external data. */
+#define TEST_DATA_NOT_FOUND 99
+
+
+/** Macro verifying that two numerical values are equal in order to pass unit test. */
+#define QPOASES_TEST_FOR_EQUAL( x,y ) if ( REFER_NAMESPACE_QPOASES isEqual( (x),(y) ) == BT_FALSE ) { return TEST_FAILED; }
+
+/** Macro verifying that two numerical values are close to each other in order to pass unit test. */
+#define QPOASES_TEST_FOR_NEAR( x,y )  if ( REFER_NAMESPACE_QPOASES getAbs((x)-(y)) / REFER_NAMESPACE_QPOASES getMax( 1.0,REFER_NAMESPACE_QPOASES getAbs(x) ) >= 1e-10 ) { return TEST_FAILED; }
+
+/** Macro verifying that first quantity is lower or equal than second one in order to pass unit test. */
+#define QPOASES_TEST_FOR_TOL( x,tol )  if ( (x) > (tol)*(TEST_TOL_FACTOR) ) { return TEST_FAILED; }
+
+/** Macro verifying that a logical expression holds in order to pass unit test. */
+#define QPOASES_TEST_FOR_TRUE( x )  if ( (x) == false ) { return TEST_FAILED; }
+
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* UNIT_TESTING */
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Utils.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Utils.hpp
new file mode 100644
index 00000000000..ddef94320b6
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Utils.hpp
@@ -0,0 +1,370 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/Utils.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Declaration of some utility functions for working with qpOASES.
+ */
+
+
+#ifndef QPOASES_UTILS_HPP
+#define QPOASES_UTILS_HPP
+
+
+#include <qpOASES/MessageHandling.hpp>
+
+
+#ifdef __NO_SNPRINTF__
+  #if (!defined(_MSC_VER)) || defined(__DSPACE__)
+    /* If snprintf is not available, provide an empty implementation... */
+    int snprintf( char* s, size_t n, const char* format, ... );
+  #else
+	/* ... or substitute snprintf by _snprintf for Microsoft compilers. */
+    #define snprintf _snprintf    
+  #endif
+#endif /* __NO_SNPRINTF__ */
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** Prints a (possibly named) vector.
+ * \return SUCCESSFUL_RETURN */
+returnValue print(	const real_t* const v,	/**< Vector to be printed. */
+					int n,					/**< Length of vector. */
+					const char* name = 0	/**< Name of vector. */
+					);
+
+/** Prints a (possibly named) permuted vector.
+ * \return SUCCESSFUL_RETURN */
+returnValue print(	const real_t* const v,		/**< Vector to be printed. */
+					int n,						/**< Length of vector. */
+					const int* const V_idx,		/**< Pemutation vector. */
+					const char* name = 0		/**< Name of vector. */
+					);
+
+/** Prints a (possibly named) matrix.
+ * \return SUCCESSFUL_RETURN */
+returnValue print(	const real_t* const M,	/**< Matrix to be printed. */
+					int nrow,				/**< Row number of matrix. */
+					int ncol,				/**< Column number of matrix. */
+					const char* name = 0	/**< Name of matrix. */
+					);
+
+/** Prints a (possibly named) permuted matrix.
+ * \return SUCCESSFUL_RETURN */
+returnValue print(	const real_t* const M,		/**< Matrix to be printed. */
+					int nrow,					/**< Row number of matrix. */
+					int ncol	,				/**< Column number of matrix. */
+					const int* const ROW_idx,	/**< Row pemutation vector. */
+					const int* const COL_idx,	/**< Column pemutation vector. */
+					const char* name = 0		/**< Name of matrix. */
+					);
+
+/** Prints a (possibly named) index array.
+ * \return SUCCESSFUL_RETURN */
+returnValue print(	const int* const index,	/**< Index array to be printed. */
+					int n,					/**< Length of index array. */
+					const char* name = 0	/**< Name of index array. */
+					);
+
+
+/** Prints a string to desired output target (useful also for MATLAB output!).
+ * \return SUCCESSFUL_RETURN */
+returnValue myPrintf(	const char* s	/**< String to be written. */
+						);
+
+
+/** Prints qpOASES copyright notice.
+ * \return SUCCESSFUL_RETURN */
+returnValue printCopyrightNotice( );
+
+
+/** Reads a real_t matrix from file.
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_UNABLE_TO_OPEN_FILE \n
+		   RET_UNABLE_TO_READ_FILE */
+returnValue readFromFile(	real_t* data,				/**< Matrix to be read from file. */
+							int nrow,					/**< Row number of matrix. */
+							int ncol,					/**< Column number of matrix. */
+							const char* datafilename	/**< Data file name. */
+							);
+
+/** Reads a real_t vector from file.
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_UNABLE_TO_OPEN_FILE \n
+		   RET_UNABLE_TO_READ_FILE */
+returnValue readFromFile(	real_t* data,				/**< Vector to be read from file. */
+							int n,						/**< Length of vector. */
+							const char* datafilename	/**< Data file name. */
+							);
+
+/** Reads an integer (column) vector from file.
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_UNABLE_TO_OPEN_FILE \n
+		   RET_UNABLE_TO_READ_FILE */
+returnValue readFromFile(	int* data,					/**< Vector to be read from file. */
+							int n,						/**< Length of vector. */
+							const char* datafilename	/**< Data file name. */
+							);
+
+
+/** Writes a real_t matrix into a file.
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_UNABLE_TO_OPEN_FILE  */
+returnValue writeIntoFile(	const real_t* const data,		/**< Matrix to be written into file. */
+							int nrow,						/**< Row number of matrix. */
+							int ncol,						/**< Column number of matrix. */
+							const char* datafilename,		/**< Data file name. */
+							BooleanType append = BT_FALSE	/**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */
+							);
+
+/** Writes a real_t vector into a file.
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_UNABLE_TO_OPEN_FILE  */
+returnValue writeIntoFile(	const real_t* const data,		/**< Vector to be written into file. */
+							int n,							/**< Length of vector. */
+							const char* datafilename,		/**< Data file name. */
+							BooleanType append = BT_FALSE	/**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */
+							);
+
+/** Writes an integer (column) vector into a file.
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_UNABLE_TO_OPEN_FILE */
+returnValue writeIntoFile(	const int* const integer,		/**< Integer vector to be written into file. */
+							int n,							/**< Length of vector. */
+							const char* datafilename,		/**< Data file name. */
+							BooleanType append = BT_FALSE	/**< Indicates if integer shall be appended if the file already exists (otherwise it is overwritten). */
+							);
+
+/** Writes a real_t matrix/vector into a Matlab binary file.
+ * \return SUCCESSFUL_RETURN \n
+		   RET_INVALID_ARGUMENTS
+ 		   RET_UNABLE_TO_WRITE_FILE */
+returnValue writeIntoMatFile(	FILE* const matFile,		/**< Pointer to Matlab binary file. */
+								const real_t* const data,	/**< Data to be written into file. */
+								int nRows,					/**< Row number of matrix. */
+								int nCols, 					/**< Column number of matrix. */
+								const char* name			/**< Matlab name of matrix/vector to be stored. */
+								);
+
+/** Writes in integer matrix/vector into a Matlab binary file.
+ * \return SUCCESSFUL_RETURN \n
+		   RET_INVALID_ARGUMENTS
+ 		   RET_UNABLE_TO_WRITE_FILE */
+returnValue writeIntoMatFile(	FILE* const matFile,		/**< Pointer to Matlab binary file. */
+								const int* const data,		/**< Data to be written into file. */
+								int nRows,					/**< Row number of matrix. */
+								int nCols,					/**< Column number of matrix. */
+								const char* name			/**< Matlab name of matrix/vector to be stored. */
+								);
+
+
+/** Returns the current system time.
+ * \return current system time */
+real_t getCPUtime( );
+
+
+/** Returns the N-norm of a vector.
+ * \return >= 0.0: successful */
+real_t getNorm(	const real_t* const v,	/**< Vector. */
+				int n,					/**< Vector's dimension. */
+				int type = 2			/**< Norm type, 1: one-norm, 2: Euclidean norm. */
+				);
+
+
+/** Tests whether two real_t-valued arguments are (numerically) equal.
+ * \return	BT_TRUE:  arguments differ not more than TOL \n
+		 	BT_FALSE: arguments differ more than TOL */
+inline BooleanType isEqual(	real_t x,			/**< First real number. */
+							real_t y,			/**< Second real number. */
+							real_t TOL = ZERO	/**< Tolerance for comparison. */
+							);
+
+
+/** Tests whether a real-valued argument is (numerically) zero.
+ * \return	BT_TRUE:  argument differs from 0.0 not more than TOL \n
+		 	BT_FALSE: argument differs from 0.0 more than TOL */
+inline BooleanType isZero(	real_t x,			/**< Real number. */
+							real_t TOL = ZERO	/**< Tolerance for comparison. */
+							);
+
+
+/** Returns sign of a real-valued argument.
+ * \return	 1.0: argument is non-negative \n
+		 	-1.0: argument is negative */
+inline real_t getSign(	real_t arg	/**< real-valued argument whose sign is to be determined. */
+						);
+
+
+/** Returns maximum of two integers.
+ * \return	Maximum of two integers */
+inline int getMax(	int x,	/**< First integer. */
+					int y	/**< Second integer. */
+					);
+					
+/** Returns minimum of two integers.
+ * \return	Minimum of two integers */
+inline int getMin(	int x,	/**< First integer. */
+					int y	/**< Second integer. */
+					);
+
+	
+/** Returns maximum of two reals.
+ * \return	Maximum of two reals */
+inline real_t getMax(	real_t x,	/**< First real number. */
+						real_t y	/**< Second real number. */
+						);
+
+/** Returns minimum of two reals.
+ * \return	Minimum of two reals */
+inline real_t getMin(	real_t x,	/**< First real number. */
+						real_t y	/**< Second real number. */
+						);
+
+/** Returns the absolute value of a real number.
+ * \return	Absolute value of a real number */
+inline real_t getAbs(	real_t x	/**< Real number. */
+						);
+
+/** Returns the square-root of a real number.
+ * \return	Square-root of a real number */
+inline real_t getSqrt(	real_t x	/**< Non-negative real number. */
+						);
+
+
+/** Computes "residual" of KKT system.  */
+void getKKTResidual(	int nV,						/**< Number of variables. */
+						int nC,						/**< Number of constraints. */
+						const real_t* const H,		/**< Hessian matrix. */
+						const real_t* const g,		/**< Sequence of gradient vectors. */
+						const real_t* const A,		/**< Constraint matrix. */
+						const real_t* const lb,		/**< Sequence of lower bound vectors (on variables). */
+						const real_t* const ub,		/**< Sequence of upper bound vectors (on variables). */
+						const real_t* const lbA,	/**< Sequence of lower constraints' bound vectors. */
+						const real_t* const ubA,	/**< Sequence of upper constraints' bound vectors. */
+						const real_t* const x,		/**< Sequence of primal trial vectors. */
+						const real_t* const y,		/**< Sequence of dual trial vectors. */
+						real_t& stat,				/**< Maximum value of stationarity condition residual. */
+						real_t& feas,				/**< Maximum value of primal feasibility violation. */
+						real_t& cmpl				/**< Maximum value of complementarity residual. */
+						);
+
+/** Computes "residual" of KKT system (for simply bounded QPs).  */
+void getKKTResidual(	int nV,						/**< Number of variables. */
+						const real_t* const H,		/**< Hessian matrix. */
+						const real_t* const g,		/**< Sequence of gradient vectors. */
+						const real_t* const lb,		/**< Sequence of lower bound vectors (on variables). */
+						const real_t* const ub,		/**< Sequence of upper bound vectors (on variables). */
+						const real_t* const x,		/**< Sequence of primal trial vectors. */
+						const real_t* const y,		/**< Sequence of dual trial vectors. */
+						real_t& stat,				/**< Maximum value of stationarity condition residual. */
+						real_t& feas,				/**< Maximum value of primal feasibility violation. */
+						real_t& cmpl				/**< Maximum value of complementarity residual. */
+						);
+
+
+/** Writes a value of BooleanType into a string.
+ * \return SUCCESSFUL_RETURN */
+returnValue convertBooleanTypeToString(	BooleanType value, 		/**< Value to be written. */
+										char* const string		/**< Input: String of sufficient size, \n
+																	 Output: String containing value. */
+										);
+
+/** Writes a value of SubjectToStatus into a string.
+ * \return SUCCESSFUL_RETURN */
+returnValue convertSubjectToStatusToString(	SubjectToStatus value,	/**< Value to be written. */
+											char* const string		/**< Input: String of sufficient size, \n
+																		 Output: String containing value. */
+											);
+
+/** Writes a value of PrintLevel into a string.
+ * \return SUCCESSFUL_RETURN */
+returnValue convertPrintLevelToString(	PrintLevel value, 		/**< Value to be written. */
+										char* const string		/**< Input: String of sufficient size, \n
+																	 Output: String containing value. */
+										);
+
+
+/** Converts a returnValue from an (S)QProblem(B) object into a more 
+ *	simple status flag.
+ *
+ * \return  0: QP problem solved
+ *          1: QP could not be solved within given number of iterations
+ *         -1: QP could not be solved due to an internal error
+ *         -2: QP is infeasible (and thus could not be solved)
+ *         -3: QP is unbounded (and thus could not be solved)
+ */
+int getSimpleStatus(	returnValue returnvalue, 				/**< ReturnValue to be analysed. */
+						BooleanType doPrintStatus = BT_FALSE	/**< Flag indicating whether simple status shall be printed to screen. */
+						);
+
+
+/** Normalises QP constraints.
+ * \return SUCCESSFUL_RETURN \n
+ *		   RET_INVALID_ARGUMENTS */
+returnValue normaliseConstraints(	int nV,			/**< Number of variables. */
+									int nC, 		/**< Number of constraints. */
+									real_t* A,		/**< Input:  Constraint matrix, \n
+														 Output: Normalised constraint matrix. */
+									real_t* lbA,	/**< Input:  Constraints' lower bound vector, \n
+														 Output: Normalised constraints' lower bound vector. */
+									real_t* ubA,	/**< Input:  Constraints' upper bound vector, \n
+														 Output: Normalised constraints' upper bound vector. */
+									int type = 1	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
+									);
+
+
+#ifdef __DEBUG__
+/** Writes matrix with given dimension into specified file. */
+extern "C" void gdb_printmat(	const char *fname,			/**< File name. */
+								real_t *M,					/**< Matrix to be written. */
+								int n,						/**< Number of rows. */
+								int m,						/**< Number of columns. */
+								int ldim					/**< Leading dimension. */
+								);
+#endif /* __DEBUG__ */
+
+
+#if defined(__DSPACE__) || defined(__XPCTARGET__) 
+extern "C" void __cxa_pure_virtual( void );
+#endif /* __DSPACE__ || __XPCTARGET__*/ 
+
+
+
+END_NAMESPACE_QPOASES
+
+
+#include <qpOASES/Utils.ipp>
+
+#endif	/* QPOASES_UTILS_HPP */
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Utils.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Utils.ipp
new file mode 100644
index 00000000000..3d89822b8b9
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Utils.ipp
@@ -0,0 +1,174 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+
+/**
+ *	\file include/qpOASES/Utils.ipp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of some inlined utilities for working with the different QProblem classes.
+ */
+
+
+#include <math.h>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*
+ *   i s E q u a l
+ */
+inline BooleanType isEqual(	real_t x,
+							real_t y,
+							real_t TOL
+							)
+{
+    if ( getAbs(x-y) <= TOL )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+/*
+ *   i s Z e r o
+ */
+inline BooleanType isZero(	real_t x,
+							real_t TOL
+							)
+{
+    if ( getAbs(x) <= TOL )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+/*
+ *   g e t S i g n
+ */
+inline real_t getSign(	real_t arg
+						)
+{
+	if ( arg >= 0.0 )
+		return 1.0;
+	else
+		return -1.0;
+}
+
+
+
+/*
+ *   g e t M a x
+ */
+inline int getMax(	int x,
+					int y
+					)
+{
+    return (y<x) ? x : y;
+}
+
+
+/*
+ *   g e t M i n
+ */
+inline int getMin(	int x,
+					int y
+					)
+{
+    return (y>x) ? x : y;
+}
+
+
+
+/*
+ *   g e t M a x
+ */
+inline real_t getMax(	real_t x,
+						real_t y
+						)
+{
+	#ifdef __NO_FMATH__
+    return (y<x) ? x : y;
+	#else
+	return (y<x) ? x : y;
+	//return fmax(x,y); /* seems to be slower */
+	#endif
+}
+
+
+/*
+ *   g e t M i n
+ */
+inline real_t getMin(	real_t x,
+						real_t y
+						)
+{
+	#ifdef __NO_FMATH__
+    return (y>x) ? x : y;
+	#else
+	return (y>x) ? x : y;
+	//return fmin(x,y); /* seems to be slower */
+	#endif
+}
+
+
+/*
+ *   g e t A b s
+ */
+inline real_t getAbs(	real_t x
+						)
+{
+	#ifdef __NO_FMATH__
+	return (x>=0.0) ? x : -x;
+	#else
+	return fabs(x);
+	#endif
+}
+
+
+/*
+ *   g e t S q r t
+ */
+inline real_t getSqrt(	real_t x
+						)
+{
+	#ifdef __NO_FMATH__
+	return sqrt(x); /* put your custom sqrt-replacement here */
+	#else
+	return sqrt(x);
+	#endif
+}
+
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/OQPinterface.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/OQPinterface.hpp
new file mode 100644
index 00000000000..4623aa32840
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/OQPinterface.hpp
@@ -0,0 +1,250 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/extras/OQPinterface.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Declaration of an interface comprising several utility functions
+ *	for solving test problems from the Online QP Benchmark Collection
+ *	(This collection is no longer maintained, see 
+ *	http://www.qpOASES.org/onlineQP for a backup).
+ */
+
+
+#ifndef QPOASES_OQPINTERFACE_HPP
+#define QPOASES_OQPINTERFACE_HPP
+
+
+#include <qpOASES/Options.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** Reads dimensions of an Online QP Benchmark problem from file.
+ *
+ * \return SUCCESSFUL_RETURN \n
+		   RET_UNABLE_TO_READ_FILE \n
+		   RET_FILEDATA_INCONSISTENT */
+returnValue readOQPdimensions(	const char* path,	/**< Full path of the data files (without trailing slash!). */
+								int& nQP,			/**< Output: Number of QPs. */
+								int& nV,			/**< Output: Number of variables. */
+								int& nC,			/**< Output: Number of constraints. */
+								int& nEC			/**< Output: Number of equality constraints. */
+								);
+
+/** Reads data of an Online QP Benchmark problem from file.
+ *  This function allocates the required memory for all data; after successfully calling it,
+ *  you have to free this memory yourself!
+ *
+ * \return SUCCESSFUL_RETURN \n
+		   RET_INVALID_ARGUMENTS \n
+		   RET_UNABLE_TO_READ_FILE \n
+		   RET_FILEDATA_INCONSISTENT */
+returnValue readOQPdata(	const char* path,	/**< Full path of the data files (without trailing slash!). */
+							int& nQP,			/**< Output: Number of QPs. */
+							int& nV,			/**< Output: Number of variables. */
+							int& nC,			/**< Output: Number of constraints. */
+							int& nEC,			/**< Output: Number of equality constraints. */
+							real_t** H,		 	/**< Output: Hessian matrix. */
+							real_t** g,		 	/**< Output: Sequence of gradient vectors. */
+							real_t** A,		 	/**< Output: Constraint matrix. */
+							real_t** lb,		/**< Output: Sequence of lower bound vectors (on variables). */
+							real_t** ub,		/**< Output: Sequence of upper bound vectors (on variables). */
+							real_t** lbA,		/**< Output: Sequence of lower constraints' bound vectors. */
+							real_t** ubA,		/**< Output: Sequence of upper constraints' bound vectors. */
+							real_t** xOpt,		/**< Output: Sequence of primal solution vectors
+												 *           (not read if a null pointer is passed). */
+							real_t** yOpt,		/**< Output: Sequence of dual solution vectors
+												 *           (not read if a null pointer is passed). */
+							real_t** objOpt		/**< Output: Sequence of optimal objective function values
+												 *           (not read if a null pointer is passed). */
+							);
+
+
+/** Solves an Online QP Benchmark problem as specified by the arguments.
+ *  The maximum deviations from the given optimal solution as well as the
+ *  maximum CPU time to solve each QP are determined.
+ *
+ *	Note: This variant is outdated and only kept to ensure 
+ *		  backwards-compatibility!
+ *
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_BENCHMARK_ABORTED */
+returnValue solveOQPbenchmark(	int nQP,					/**< Number of QPs. */
+								int nV,						/**< Number of variables. */
+								int nC,						/**< Number of constraints. */
+								int nEC,					/**< Number of equality constraints. */
+								const real_t* const _H,		/**< Hessian matrix. */
+								const real_t* const g,		/**< Sequence of gradient vectors. */
+								const real_t* const _A,		/**< Constraint matrix. */
+								const real_t* const lb,		/**< Sequence of lower bound vectors (on variables). */
+								const real_t* const ub,		/**< Sequence of upper bound vectors (on variables). */
+								const real_t* const lbA,	/**< Sequence of lower constraints' bound vectors. */
+								const real_t* const ubA,	/**< Sequence of upper constraints' bound vectors. */
+								BooleanType isSparse,		/**< Shall convert matrices to sparse format before solution? */
+								const Options& options,		/**< QP solver options to be used while solving benchmark problems. */
+								int& nWSR, 					/**< Input: Maximum number of working set recalculations; \n
+																 Output: Maximum number of performed working set recalculations. */
+								real_t& maxCPUtime,			/**< Output: Maximum CPU time required for solving each QP. */
+								real_t& maxStationarity,	/**< Output: Maximum residual of stationarity condition. */
+								real_t& maxFeasibility,		/**< Output: Maximum residual of primal feasibility condition. */
+								real_t& maxComplementarity	/**< Output: Maximum residual of complementarity condition. */
+								);
+
+/** Solves an Online QP Benchmark problem as specified by the arguments.
+ *  The maximum deviations from the given optimal solution as well as the
+ *  maximum CPU time to solve each QP are determined.
+ *
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_BENCHMARK_ABORTED */
+returnValue solveOQPbenchmark(	int nQP,					/**< Number of QPs. */
+								int nV,						/**< Number of variables. */
+								int nC,						/**< Number of constraints. */
+								int nEC,					/**< Number of equality constraints. */
+								const real_t* const _H,		/**< Hessian matrix. */
+								const real_t* const g,		/**< Sequence of gradient vectors. */
+								const real_t* const _A,		/**< Constraint matrix. */
+								const real_t* const lb,		/**< Sequence of lower bound vectors (on variables). */
+								const real_t* const ub,		/**< Sequence of upper bound vectors (on variables). */
+								const real_t* const lbA,	/**< Sequence of lower constraints' bound vectors. */
+								const real_t* const ubA,	/**< Sequence of upper constraints' bound vectors. */
+								BooleanType isSparse,		/**< Shall convert matrices to sparse format before solution? */
+								BooleanType useHotstarts,	/**< Shall QP solution be hotstarted? */
+								const Options& options,		/**< QP solver options to be used while solving benchmark problems. */
+								int maxAllowedNWSR, 		/**< Maximum number of working set recalculations to be performed. */
+								real_t& maxNWSR,			/**< Output: Maximum number of performed working set recalculations. */
+								real_t& avgNWSR,			/**< Output: Average number of performed working set recalculations. */
+								real_t& maxCPUtime,			/**< Output: Maximum CPU time required for solving each QP. */
+								real_t& avgCPUtime,			/**< Output: Average CPU time required for solving each QP. */
+								real_t& maxStationarity,	/**< Output: Maximum residual of stationarity condition. */
+								real_t& maxFeasibility,		/**< Output: Maximum residual of primal feasibility condition. */
+								real_t& maxComplementarity	/**< Output: Maximum residual of complementarity condition. */
+								);
+
+
+/** Solves an Online QP Benchmark problem (without constraints) as specified
+ *  by the arguments. The maximum deviations from the given optimal solution
+ *  as well as the maximum CPU time to solve each QP are determined.
+ *
+ *	Note: This variant is outdated and only kept to ensure 
+ *		  backwards-compatibility!
+ *
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_BENCHMARK_ABORTED */
+returnValue solveOQPbenchmark(	int nQP,					/**< Number of QPs. */
+								int nV,						/**< Number of variables. */
+								const real_t* const _H,		/**< Hessian matrix. */
+								const real_t* const g,		/**< Sequence of gradient vectors. */
+								const real_t* const lb,		/**< Sequence of lower bound vectors (on variables). */
+								const real_t* const ub,		/**< Sequence of upper bound vectors (on variables). */
+								BooleanType isSparse,		/**< Shall convert matrices to sparse format before solution? */
+								const Options& options,		/**< QP solver options to be used while solving benchmark problems. */
+								int& nWSR, 					/**< Input: Maximum number of working set recalculations; \n
+																 Output: Maximum number of performed working set recalculations. */
+								real_t& maxCPUtime,			/**< Output: Maximum CPU time required for solving each QP. */
+								real_t& maxStationarity,	/**< Output: Maximum residual of stationarity condition. */
+								real_t& maxFeasibility,		/**< Output: Maximum residual of primal feasibility condition. */
+								real_t& maxComplementarity	/**< Output: Maximum residual of complementarity condition. */
+								);
+
+/** Solves an Online QP Benchmark problem (without constraints) as specified
+ *  by the arguments. The maximum deviations from the given optimal solution
+ *  as well as the maximum CPU time to solve each QP are determined.
+ *
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_BENCHMARK_ABORTED */
+returnValue solveOQPbenchmark(	int nQP,					/**< Number of QPs. */
+								int nV,						/**< Number of variables. */
+								const real_t* const _H,		/**< Hessian matrix. */
+								const real_t* const g,		/**< Sequence of gradient vectors. */
+								const real_t* const lb,		/**< Sequence of lower bound vectors (on variables). */
+								const real_t* const ub,		/**< Sequence of upper bound vectors (on variables). */
+								BooleanType isSparse,		/**< Shall convert matrices to sparse format before solution? */
+								BooleanType useHotstarts,	/**< Shall QP solution be hotstarted? */
+								const Options& options,		/**< QP solver options to be used while solving benchmark problems. */
+								int maxAllowedNWSR, 		/**< Maximum number of working set recalculations to be performed. */
+								real_t& maxNWSR,			/**< Output: Maximum number of performed working set recalculations. */
+								real_t& avgNWSR,			/**< Output: Average number of performed working set recalculations. */
+								real_t& maxCPUtime,			/**< Output: Maximum CPU time required for solving each QP. */
+								real_t& avgCPUtime,			/**< Output: Average CPU time required for solving each QP. */
+								real_t& maxStationarity,	/**< Output: Maximum residual of stationarity condition. */
+								real_t& maxFeasibility,		/**< Output: Maximum residual of primal feasibility condition. */
+								real_t& maxComplementarity	/**< Output: Maximum residual of complementarity condition. */
+								);
+
+
+/** Runs an Online QP Benchmark problem and determines the maximum
+ *  violation of the KKT optimality conditions as well as the 
+ *  maximum CPU time to solve each QP.
+ *
+ * \return SUCCESSFUL_RETURN \n
+		   RET_UNABLE_TO_READ_BENCHMARK \n
+ 		   RET_BENCHMARK_ABORTED */
+returnValue runOQPbenchmark(	const char* path,			/**< Full path of the benchmark files (without trailing slash!). */
+								BooleanType isSparse,		/**< Shall convert matrices to sparse format before solution? */
+								const Options& options,		/**< QP solver options to be used while solving benchmark problems. */
+								int& nWSR, 					/**< Input: Maximum number of working set recalculations; \n
+																 Output: Maximum number of performed working set recalculations. */
+								real_t& maxCPUtime,			/**< Output: Maximum CPU time required for solving each QP. */
+								real_t& maxStationarity,	/**< Output: Maximum residual of stationarity condition. */
+								real_t& maxFeasibility,		/**< Output: Maximum residual of primal feasibility condition. */
+								real_t& maxComplementarity	/**< Output: Maximum residual of complementarity condition. */
+								);
+
+
+/** Runs an Online QP Benchmark problem and determines the maximum
+ *  violation of the KKT optimality conditions as well as the 
+ *  maximum and average number of iterations and CPU time to solve 
+ *	each QP.
+ *
+ * \return SUCCESSFUL_RETURN \n
+		   RET_UNABLE_TO_READ_BENCHMARK \n
+ 		   RET_BENCHMARK_ABORTED */
+returnValue runOQPbenchmark(	const char* path,			/**< Full path of the benchmark files (without trailing slash!). */
+								BooleanType isSparse,		/**< Shall convert matrices to sparse format before solution? */
+								BooleanType useHotstarts,	/**< Shall QP solution be hotstarted? */
+								const Options& options,		/**< QP solver options to be used while solving benchmark problems. */
+								int maxAllowedNWSR, 		/**< Maximum number of working set recalculations to be performed. */
+								real_t& maxNWSR,			/**< Output: Maximum number of performed working set recalculations. */
+								real_t& avgNWSR,			/**< Output: Average number of performed working set recalculations. */
+								real_t& maxCPUtime,			/**< Output: Maximum CPU time required for solving each QP. */
+								real_t& avgCPUtime,			/**< Output: Average CPU time required for solving each QP. */
+								real_t& maxStationarity,	/**< Output: Maximum residual of stationarity condition. */
+								real_t& maxFeasibility,		/**< Output: Maximum residual of primal feasibility condition. */
+								real_t& maxComplementarity	/**< Output: Maximum residual of complementarity condition. */
+								);
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_OQPINTERFACE_HPP */
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/SolutionAnalysis.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/SolutionAnalysis.hpp
new file mode 100644
index 00000000000..ad625807495
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/SolutionAnalysis.hpp
@@ -0,0 +1,155 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/extras/SolutionAnalysis.hpp
+ *	\author Hans Joachim Ferreau (thanks to Boris Houska)
+ *	\version 3.0
+ *	\date 2008-2014
+ *
+ *	Declaration of the SolutionAnalysis class designed to perform
+ *	additional analysis after solving a QP with qpOASES.
+ */
+
+
+#ifndef QPOASES_SOLUTIONANALYSIS_HPP
+#define QPOASES_SOLUTIONANALYSIS_HPP
+
+
+#include <qpOASES/SQProblem.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/**
+ *	\brief Provides additional tools for analysing QP solutions.
+ *
+ *	This class is intended to provide additional tools for analysing
+ *  a QP solution obtained with qpOASES.
+ *
+ *	\author Hans Joachim Ferreau (thanks to Boris Houska)
+ *	\version 3.0
+ *	\date 2007-2014
+ */
+class SolutionAnalysis
+{
+	/*
+	 *	PUBLIC MEMBER FUNCTIONS
+	 */
+	public:
+		/** Default constructor. */
+		SolutionAnalysis( );
+
+		/** Copy constructor (deep copy). */
+		SolutionAnalysis(	const SolutionAnalysis& rhs		/**< Rhs object. */
+							);
+
+		/** Destructor. */
+		~SolutionAnalysis( );
+
+		/** Assignment operator (deep copy). */
+		SolutionAnalysis& operator=(	const SolutionAnalysis& rhs		/**< Rhs object. */
+										);
+
+
+		/** Determines the maximum violation of the KKT optimality conditions
+		 *  of the current iterate within the QProblemB object.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_UNABLE_TO_ANALYSE_QPROBLEM */
+		returnValue getMaxKKTviolation(	QProblemB* qp,			/**< QProblemB to be analysed. */
+										real_t& maxKKTviolation	/**< OUTPUT: maximum violation of the KKT conditions. */
+										) const;
+
+		/** Determines the maximum violation of the KKT optimality conditions
+		 *  of the current iterate within the QProblem object.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_UNABLE_TO_ANALYSE_QPROBLEM */
+		returnValue getMaxKKTviolation(	QProblem* qp,			/**< QProblem to be analysed. */
+										real_t& maxKKTviolation	/**< OUTPUT: maximum violation of the KKT conditions. */
+										) const;
+
+		/** Determines the maximum violation of the KKT optimality conditions
+		 *  of the current iterate within the SQProblem object.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_UNABLE_TO_ANALYSE_QPROBLEM */
+		returnValue getMaxKKTviolation(	SQProblem* qp,			/**< SQProblem to be analysed. */
+										real_t& maxKKTviolation	/**< OUTPUT: maximum violation of the KKT conditions. */
+										) const;
+
+
+		/** Computes the variance-covariance matrix of the QP output for uncertain 
+			inputs.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_HOTSTART_FAILED \n
+		 			RET_STEPDIRECTION_FAILED_TQ \n
+					RET_STEPDIRECTION_FAILED_CHOLESKY */
+		returnValue getVarianceCovariance(	QProblemB* qp,			/**< QProblemB to be analysed. */
+											real_t* g_b_bA_VAR,		/**< INPUT : Variance-covariance of g, the bounds lb and ub, and lbA and ubA respectively. Dimension: 2nV x 2nV */
+											real_t* Primal_Dual_VAR	/**< OUTPUT: The result for the variance-covariance of the primal and dual variables. Dimension: 2nV x 2nV */
+											) const;
+
+		/** Computes the variance-covariance matrix of the QP output for uncertain 
+			inputs.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_HOTSTART_FAILED \n
+		 			RET_STEPDIRECTION_FAILED_TQ \n
+					RET_STEPDIRECTION_FAILED_CHOLESKY */
+		returnValue getVarianceCovariance(	QProblem* qp,			/**< QProblem to be analysed. */
+											real_t* g_b_bA_VAR,		/**< INPUT : Variance-covariance of g, the bounds lb and ub, and lbA and ubA respectively. Dimension:  (2nV+nC) x (2nV+nC) */
+											real_t* Primal_Dual_VAR	/**< OUTPUT: The result for the variance-covariance of the primal and dual variables. Dimension:  (2nV+nC) x (2nV+nC) */
+											) const;
+
+		/** Computes the variance-covariance matrix of the QP output for uncertain 
+			inputs.
+		 *	\return SUCCESSFUL_RETURN \n
+					RET_HOTSTART_FAILED \n
+		 			RET_STEPDIRECTION_FAILED_TQ \n
+					RET_STEPDIRECTION_FAILED_CHOLESKY */
+		returnValue getVarianceCovariance(	SQProblem* qp,			/**< SQProblem to be analysed. */
+											real_t* g_b_bA_VAR,		/**< INPUT : Variance-covariance of g, the bounds lb and ub, and lbA and ubA respectively. Dimension:  (2nV+nC) x (2nV+nC) */
+											real_t* Primal_Dual_VAR	/**< OUTPUT: The result for the variance-covariance of the primal and dual variables. Dimension:  (2nV+nC) x (2nV+nC) */
+											) const;
+
+		/** Checks if a direction of negative curvature shows up if we remove all bounds that just recently became active */
+
+
+	/*
+	 *	PROTECTED MEMBER VARIABLES
+	 */
+	protected:
+
+};
+
+
+END_NAMESPACE_QPOASES
+
+#include <qpOASES/extras/SolutionAnalysis.ipp>
+
+#endif	/* QPOASES_SOLUTIONANALYSIS_HPP */
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/SolutionAnalysis.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/SolutionAnalysis.ipp
new file mode 100644
index 00000000000..dc59a7e3d60
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/SolutionAnalysis.ipp
@@ -0,0 +1,51 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES/extras/SolutionAnalysis.ipp
+ *	\author Hans Joachim Ferreau (thanks to Boris Houska)
+ *	\version 3.0
+ *	\date 2008-2014
+ *
+ *	Implementation of inlined member functions of the SolutionAnalysis class
+ *	designed to perform additional analysis after solving a QP with qpOASES.
+ *
+ */
+
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/index.html b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/index.html
new file mode 100644
index 00000000000..6cb3040f5aa
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/index.html
@@ -0,0 +1,10 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/include/qpOASES/extras</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/include/qpOASES/extras</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="OQPinterface.hpp">OQPinterface.hpp</a></li>
+  <li><a href="SolutionAnalysis.hpp">SolutionAnalysis.hpp</a></li>
+  <li><a href="SolutionAnalysis.ipp">SolutionAnalysis.ipp</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/index.html b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/index.html
new file mode 100644
index 00000000000..7bc668d65bf
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/index.html
@@ -0,0 +1,33 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/include/qpOASES</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/include/qpOASES</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="Bounds.hpp">Bounds.hpp</a></li>
+  <li><a href="Bounds.ipp">Bounds.ipp</a></li>
+  <li><a href="Constants.hpp">Constants.hpp</a></li>
+  <li><a href="ConstraintProduct.hpp">ConstraintProduct.hpp</a></li>
+  <li><a href="Constraints.hpp">Constraints.hpp</a></li>
+  <li><a href="Constraints.ipp">Constraints.ipp</a></li>
+  <li><a href="Flipper.hpp">Flipper.hpp</a></li>
+  <li><a href="Indexlist.hpp">Indexlist.hpp</a></li>
+  <li><a href="Indexlist.ipp">Indexlist.ipp</a></li>
+  <li><a href="Matrices.hpp">Matrices.hpp</a></li>
+  <li><a href="MessageHandling.hpp">MessageHandling.hpp</a></li>
+  <li><a href="MessageHandling.ipp">MessageHandling.ipp</a></li>
+  <li><a href="Options.hpp">Options.hpp</a></li>
+  <li><a href="QProblem.hpp">QProblem.hpp</a></li>
+  <li><a href="QProblem.ipp">QProblem.ipp</a></li>
+  <li><a href="QProblemB.hpp">QProblemB.hpp</a></li>
+  <li><a href="QProblemB.ipp">QProblemB.ipp</a></li>
+  <li><a href="SQProblem.hpp">SQProblem.hpp</a></li>
+  <li><a href="SQProblem.ipp">SQProblem.ipp</a></li>
+  <li><a href="SubjectTo.hpp">SubjectTo.hpp</a></li>
+  <li><a href="SubjectTo.ipp">SubjectTo.ipp</a></li>
+  <li><a href="Types.hpp">Types.hpp</a></li>
+  <li><a href="UnitTesting.hpp">UnitTesting.hpp</a></li>
+  <li><a href="Utils.hpp">Utils.hpp</a></li>
+  <li><a href="Utils.ipp">Utils.ipp</a></li>
+  <li><a href="extras/">extras/</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/index.html b/3rdparty/qpOASES/qpOASES-3.0/index.html
new file mode 100644
index 00000000000..f03255769ac
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/index.html
@@ -0,0 +1,31 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="AUTHORS">AUTHORS</a></li>
+  <li><a href="AUTHORS.txt">AUTHORS.txt</a></li>
+  <li><a href="CMakeLists.txt">CMakeLists.txt</a></li>
+  <li><a href="INSTALL">INSTALL</a></li>
+  <li><a href="INSTALL.txt">INSTALL.txt</a></li>
+  <li><a href="LICENSE">LICENSE</a></li>
+  <li><a href="LICENSE.txt">LICENSE.txt</a></li>
+  <li><a href="Makefile">Makefile</a></li>
+  <li><a href="README">README</a></li>
+  <li><a href="README.txt">README.txt</a></li>
+  <li><a href="VERSIONS">VERSIONS</a></li>
+  <li><a href="VERSIONS.txt">VERSIONS.txt</a></li>
+  <li><a href="bin/">bin/</a></li>
+  <li><a href="doc/">doc/</a></li>
+  <li><a href="examples/">examples/</a></li>
+  <li><a href="include/">include/</a></li>
+  <li><a href="interfaces/">interfaces/</a></li>
+  <li><a href="make.mk">make.mk</a></li>
+  <li><a href="make_cygwin.mk">make_cygwin.mk</a></li>
+  <li><a href="make_linux.mk">make_linux.mk</a></li>
+  <li><a href="make_osx.mk">make_osx.mk</a></li>
+  <li><a href="make_windows.mk">make_windows.mk</a></li>
+  <li><a href="src/">src/</a></li>
+  <li><a href="testing/">testing/</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/index.html
new file mode 100644
index 00000000000..70331be2a27
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/index.html
@@ -0,0 +1,12 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/interfaces</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="matlab/">matlab/</a></li>
+  <li><a href="octave/">octave/</a></li>
+  <li><a href="python/">python/</a></li>
+  <li><a href="scilab/">scilab/</a></li>
+  <li><a href="simulink/">simulink/</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1.mat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1.mat
new file mode 100644
index 0000000000000000000000000000000000000000..c0bd0d2ed4364ebaf2b16eeb1791b99510164d60
GIT binary patch
literal 790
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i?A@$QE)CwO)N=GQOM7;
zQV7W?Rq#(PQBW{ZFtoHXwy-iZQZO<wFjpWIFfe-h@-r|n=m2rWoX5!t2^<VbW(v<5
zE-4(~dTQ)ws4P&$km;uomeA11|BWZ&AagZCObxRuNRJv^j{&kCCPU#FX(<IPn`cT$
zG&V7awlb^%sn&(7E<jezXs9e<Y_x<KXonck4*72aznX0sEPR-wL7MdNXfk}FVrWpn
zbd!n4;KKpAQx*&i4u6?#K)Tf7x=<Vjw#smdTH_BNo;41R3^D<X9(eq5$k5n1bCNQ%
z@Pwl*Pna3xb}%;MF&w1YZ<6wal%ya3=QlDksCqM3;5D4vIj*8W0BATX&~U}|jQ2pQ
z4dE{TfEGw1%||Xh{v33=(L`N<+u*<f_G*s5hvgUwC0GSO8WEwp0Yl@HqDhZ{Iwd7$
zNJcd5`Tw8MMu6oVNRu%h+n#t$hG?>A%wc4c5GZW8!2gYzq2v=QA4sbvTq}xwN{M9(
rdcH+H=e>2)6PUUhjxboKF`MIYAV|YGPpF2pgrpzeL4oIZl~o4-!Xx*U

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1a.mat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1a.mat
new file mode 100644
index 0000000000000000000000000000000000000000..48c784fcb7c53eedc3b3124e3577f8d0822e1cb2
GIT binary patch
literal 896
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i?A@$QE)CwO)N=GQOM7;
zQV7W?Rq#(PQBW{ZFtoHXwzM)dRWLFzFjpWIFfe-h@-r|n=m2rWoX5!t2^<VbW(v<5
zE-4(~dTQ)ws4P&$km;uomeA11|BWZ&AagZCObxRuNRJv^j{&kCCPU#FX(<IPn`cT$
zG&V7awlb^%sn&(7E<jezXs9e<Y_x<KXonck4*72aznX0sEPR-wL7MdNXfk}FVrWpn
zbd!n4;KKpAQx*&i4u6?#K)Tf7x=<Vjw#smdTH_BNo;41R3^D<X9(eq5$k5n1bCNQ%
z@Pwl*Pna3xb}%;MF&w1YZ<6wal%ya3=QlDksCqM3;5D4vIj*8W0BATX&~U}|jQ2pQ
z4dE{TfEGw1%||Xh{v33=(L`N<+u*<f_G*s5hvgUwC0GSO8WEwp0Yl@HqDhZ{Iwd7$
zNJcd5`Tw8MMu6oVNRu%h+n#t$hG?>A%wc4c5GZW8!2gYzq2v=QA4sbvTq}xwN{M9(
zdcH+H=e>2)6PUUhjxboKF`MIYAV|YGPpF2pgrpzeL4oIZl~o6%$pCIk2ePk#sWRk@
zvYq}`8INM7jZDluH5=M5@Ec?>oIk~G0n(=h*N5UdV9K1r)@ga{`O<04M`r+2CliB_
HDN`x{YE&BX

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1b.mat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1b.mat
new file mode 100644
index 0000000000000000000000000000000000000000..b45b4f74c6a935c350dac6de0673396f20801338
GIT binary patch
literal 549
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i?A@$QE)CwO)N=GQOM7;
zQV7W?Rq#(PQBW{ZFtoHXwy-iXRWLFzFjpWIFfe-h@-r|n=m2rWoX5!t2^<VbW(v<5
zE-4(~dTQ)ws4P&$km;uomeA11|BWZ&AagZCObxRuNRKXDPXV$XMnh!@W1}U^KwV-$
zUGm=qel^=NSoknUgEZ;k(Pa2U#n7OD=_V78!G{BKrz{v49R4!ffOM(Dbs1n-WjIBx
z@rMu38V5%PnE*x)ka9Kb$`2WGJI7TN2nbI&%KC(vL2*6fJ&<ZcxatpRhKn>Gx%BvR
z(CJ1KbpdXJ0|(fvIsP7&V<?nh6#!{O1i%IijZcauJp$^Kl$aqI(Xi+Le?}VtmUkdc
f#&~Rd;x!qf$)YibkxfFNu;Bv#H)e*CPpo_Z46>$n

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/index.html
new file mode 100644
index 00000000000..2e1a48f8c63
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/index.html
@@ -0,0 +1,20 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces/matlab</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/interfaces/matlab</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="Makefile">Makefile</a></li>
+  <li><a href="example1.mat">example1.mat</a></li>
+  <li><a href="example1a.mat">example1a.mat</a></li>
+  <li><a href="example1b.mat">example1b.mat</a></li>
+  <li><a href="make.m">make.m</a></li>
+  <li><a href="qpOASES.cpp">qpOASES.cpp</a></li>
+  <li><a href="qpOASES.m">qpOASES.m</a></li>
+  <li><a href="qpOASES_auxInput.m">qpOASES_auxInput.m</a></li>
+  <li><a href="qpOASES_matlab_utils.cpp">qpOASES_matlab_utils.cpp</a></li>
+  <li><a href="qpOASES_matlab_utils.hpp">qpOASES_matlab_utils.hpp</a></li>
+  <li><a href="qpOASES_options.m">qpOASES_options.m</a></li>
+  <li><a href="qpOASES_sequence.cpp">qpOASES_sequence.cpp</a></li>
+  <li><a href="qpOASES_sequence.m">qpOASES_sequence.m</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/make.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/make.m
new file mode 100644
index 00000000000..0970f19b2ff
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/make.m
@@ -0,0 +1,238 @@
+function [] = make( varargin )
+%MAKE Compiles the Matlab interface of qpOASES.
+%
+%Type  make            to compile all interfaces that 
+%                      have been modified,
+%type  make clean      to delete all compiled interfaces,
+%type  make clean all  to first delete and then compile 
+%                      all interfaces,
+%type  make 'name'     to compile only the interface with
+%                      the given name (if it has been modified),
+%type  make 'opt'      to compile all interfaces using the 
+%                      given compiler options.
+%
+%Copyright (C) 2013-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%Christian Kirches et al. All rights reserved.
+
+%%
+%%	This file is part of qpOASES.
+%%
+%%	qpOASES -- An Implementation of the Online Active Set Strategy.
+%%	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%%	Christian Kirches et al. All rights reserved.
+%%
+%%	qpOASES 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.
+%%
+%%	qpOASES 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 qpOASES; if not, write to the Free Software
+%%	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+%%
+
+%%
+%%	Filename:  interfaces/matlab/make.m
+%%	Author:    Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+%%	Version:   3.0
+%%	Date:      2007-2014
+%%
+
+       
+    %% consistency check
+    if ( exist( [pwd, '/make.m'],'file' ) == 0 )
+        error( ['ERROR (',mfilename '.m): Run this make script directly within the directory', ...
+                '<qpOASES-inst-dir>/interfaces/matlab, please.'] );
+    end
+
+
+    if ( nargin > 2 )
+        error( ['ERROR (',mfilename '.m): At most two make arguments supported!'] );
+    else
+        [ doClean,fcnNames,userFlags ] = analyseMakeArguments( nargin,varargin );
+    end
+
+    
+    %% define compiler settings
+    QPOASESPATH = '../../';
+    
+    DEBUGFLAGS = ' ';
+    %DEBUGFLAGS = ' -g CXXDEBUGFLAGS=''$CXXDEBUGFLAGS -Wall -pedantic -Wshadow'' ';
+
+    IFLAGS = [ '-I. -I',QPOASESPATH,'include',' -I',QPOASESPATH,'src',' ' ];
+    CPPFLAGS = [ IFLAGS, DEBUGFLAGS, '-largeArrayDims -D__cpluplus -D__MATLAB__ -D__SINGLE_OBJECT__',' ' ];
+    defaultFlags = '-O -D__NO_COPYRIGHT__ '; %% -D__SUPPRESSANYOUTPUT__
+
+    if ( ispc == 0 )
+        CPPFLAGS  = [ CPPFLAGS, '-DLINUX ',' ' ]; 
+    else
+        CPPFLAGS  = [ CPPFLAGS, '-DWIN32 ',' ' ];
+    end
+
+    if ( isempty(userFlags) > 0 )
+        CPPFLAGS = [ CPPFLAGS, defaultFlags,' ' ];
+    else
+        CPPFLAGS = [ CPPFLAGS, userFlags,' ' ];
+    end
+
+    mexExt = eval('mexext');
+    
+    
+    %% ensure copyright notice is displayed
+    if ~isempty( strfind( CPPFLAGS,'-D__NO_COPYRIGHT__' ) )
+        printCopyrightNotice( );
+    end
+    
+    
+    %% clean if desired
+    if ( doClean > 0 )
+        
+        eval( 'delete *.o;' );
+        eval( ['delete *.',mexExt,'*;'] );
+        disp( [ 'INFO (',mfilename '.m): Cleaned all compiled files.'] );
+        pause( 0.2 );
+        
+    end
+    
+    
+    if ( ~isempty(userFlags) )
+        disp( [ 'INFO (',mfilename '.m): Compiling all files with user-defined compiler flags (''',userFlags,''')...'] );
+    end
+    
+    
+    %% call mex compiler
+    for ii=1:length(fcnNames)
+        
+        cmd = [ 'mex -output ', fcnNames{ii}, ' ', CPPFLAGS, [fcnNames{ii},'.cpp'] ];
+        
+        if ( exist( [fcnNames{ii},'.',mexExt],'file' ) == 0 )
+            
+            eval( cmd );
+            disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' successfully created.'] );
+            
+        else
+            
+            % check modification time of source/Make files and compiled mex file
+            cppFile = dir( [pwd,'/',fcnNames{ii},'.cpp'] );
+            cppFileTimestamp = getTimestamp( cppFile );
+            
+            utilsFile = dir( [pwd,'/qpOASES_matlab_utils.cpp'] );
+            utilsFileTimestamp = getTimestamp( utilsFile );
+            
+            makeFile = dir( [pwd,'/make.m'] );
+            makeFileTimestamp = getTimestamp( makeFile );
+            
+            mexFile = dir( [pwd,'/',fcnNames{ii},'.',mexExt] );
+            if ( isempty(mexFile) == 0 )
+                mexFileTimestamp = getTimestamp( mexFile );
+            else
+                mexFileTimestamp = 0;
+            end
+            
+            if ( ( cppFileTimestamp   >= mexFileTimestamp ) || ...
+                 ( utilsFileTimestamp >= mexFileTimestamp ) || ...
+                 ( makeFileTimestamp  >= mexFileTimestamp ) )
+                eval( cmd );
+                disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' successfully created.'] );
+            else            
+                disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' already exists.'] );
+            end
+            
+        end
+        
+    end
+
+    %% add qpOASES directory to path
+    path( path,pwd );
+
+end
+
+
+function [ doClean,fcnNames,userIFlags ] = analyseMakeArguments( nArgs,args )
+
+    doClean = 0;
+    fcnNames = [];
+    userIFlags = [];
+
+    switch ( nArgs )
+        
+        case 1
+            if ( strcmp( args{1},'all' ) > 0 )
+                fcnNames = { 'qpOASES','qpOASES_sequence' };
+            elseif ( strcmp( args{1},'qpOASES' ) > 0 )
+                fcnNames = { 'qpOASES' };
+            elseif ( strcmp( args{1},'qpOASES_sequence' ) > 0 )
+                fcnNames = { 'qpOASES_sequence' };
+            elseif ( strcmp( args{1},'clean' ) > 0 )
+                doClean = 1;
+            elseif ( strcmp( args{1}(1),'-' ) > 0 )
+                % make clean all with user-specified compiler flags
+                userIFlags = args{1};
+                doClean = 1;
+                fcnNames = { 'qpOASES','qpOASES_sequence' };
+            else
+                error( ['ERROR (',mfilename '.m): Invalid first argument (''',args{1},''')!'] );
+            end
+
+        case 2
+            if ( strcmp( args{1},'clean' ) > 0 )
+                doClean = 1;
+            else
+                error( ['ERROR (',mfilename '.m): First argument must be ''clean'' if two arguments are provided!'] );
+            end
+            
+            if ( strcmp( args{2},'all' ) > 0 )
+                fcnNames = { 'qpOASES','qpOASES_sequence' };
+            elseif ( strcmp( args{2},'qpOASES' ) > 0 )
+                fcnNames = { 'qpOASES' };
+            elseif ( strcmp( args{2},'qpOASES_sequence' ) > 0 )
+                fcnNames = { 'qpOASES_sequence' };
+            else
+                error( ['ERROR (',mfilename '.m): Invalid second argument (''',args{2},''')!'] );
+            end
+            
+        otherwise
+            fcnNames = { 'qpOASES','qpOASES_sequence' };
+            
+    end
+    
+end
+
+
+function [ timestamp ] = getTimestamp( dateString )
+
+    try 
+        timestamp = dateString.datenum;
+    catch
+        timestamp = Inf;
+    end
+
+end
+
+
+function [ ] = printCopyrightNotice( )
+
+    disp( ' ' );
+    disp( 'qpOASES -- An Implementation of the Online Active Set Strategy.' );
+    disp( 'Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,' );
+    disp( 'Christian Kirches et al. All rights reserved.' );
+    disp( ' ' );
+    disp( 'qpOASES is distributed under the terms of the' );
+    disp( 'GNU Lesser General Public License 2.1 in the hope that it will be' );
+    disp( 'useful, but WITHOUT ANY WARRANTY; without even the implied warranty' );
+    disp( 'of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.' );
+    disp( 'See the GNU Lesser General Public License for more details.' );
+    disp( ' ' );
+    disp( ' ' );
+
+end
+
+
+%%
+%%	end of file
+%%
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES.cpp
new file mode 100644
index 00000000000..0f1175b754e
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES.cpp
@@ -0,0 +1,557 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file interfaces/matlab/qpOASES.cpp
+ *	\author Hans Joachim Ferreau, Alexander Buchner (thanks to Aude Perrin)
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Interface for Matlab(R) that enables to call qpOASES as a MEX function.
+ *
+ */
+
+
+#include <qpOASES.hpp>
+
+
+USING_NAMESPACE_QPOASES
+
+
+#include "qpOASES_matlab_utils.hpp"
+
+/** initialise handle counter of QPInstance class */
+int QPInstance::s_nexthandle = 1;
+
+/** global pointer to QP objects */
+static std::vector<QPInstance *> g_instances;
+
+#include "qpOASES_matlab_utils.cpp"
+
+
+/*
+ *	q p O A S E S m e x _ c o n s t r a i n t s
+ */
+int qpOASESmex_constraints(		int nV, int nC, int nP,
+								SymmetricMatrix *H, real_t* g, Matrix *A,
+								real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
+								int nWSRin, real_t maxCpuTimeIn,
+								real_t* x0, Options* options,
+								int nOutputs, mxArray* plhs[],
+								double* guessedBounds, double* guessedConstraints
+								)
+{
+	int nWSRout;
+	real_t maxCpuTimeOut;
+	
+	/* 1) Setup initial QP. */
+	QProblem QP( nV,nC );
+	QP.setOptions( *options );
+
+	/* 2) Solve initial QP. */
+	returnValue returnvalue;
+
+	Bounds bounds(nV);
+	Constraints constraints(nC);
+	if (guessedBounds != 0) {
+		for (int i = 0; i < nV; i++) {
+			if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_LOWER);
+			} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_UPPER);
+			} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_INACTIVE);
+			} else {
+				char msg[MAX_STRING_LENGTH];
+				snprintf(msg, MAX_STRING_LENGTH,
+						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
+				myMexErrMsgTxt(msg);
+				return -1;
+			}
+		}
+	}
+
+	if (guessedConstraints != 0) {
+		for (int i = 0; i < nC; i++) {
+			if ( isEqual(guessedConstraints[i],-1.0) == BT_TRUE ) {
+				constraints.setupConstraint(i, ST_LOWER);
+			} else if ( isEqual(guessedConstraints[i],1.0) == BT_TRUE ) {
+				constraints.setupConstraint(i, ST_UPPER);
+			} else if ( isEqual(guessedConstraints[i],0.0) == BT_TRUE ) {
+				constraints.setupConstraint(i, ST_INACTIVE);
+			} else {
+				char msg[MAX_STRING_LENGTH];
+				snprintf(msg, MAX_STRING_LENGTH,
+						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of constraints!");
+				myMexErrMsgTxt(msg);
+				return -1;
+			}
+		}
+	}
+
+	nWSRout = nWSRin;
+	maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+	if (x0 == 0 && guessedBounds == 0 && guessedConstraints == 0)
+		returnvalue = QP.init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut);
+	else
+		returnvalue = QP.init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut, x0, 0,
+				guessedBounds != 0 ? &bounds : 0,
+				guessedConstraints != 0 ? &constraints : 0);
+
+	/* 3) Solve remaining QPs and assign lhs arguments. */
+	/*    Set up pointers to the current QP vectors */
+	real_t* g_current   = g;
+	real_t* lb_current  = lb;
+	real_t* ub_current  = ub;
+	real_t* lbA_current = lbA;
+	real_t* ubA_current = ubA;
+
+	/* Loop through QP sequence. */
+	for ( int k=0; k<nP; ++k )
+	{
+		if ( k > 0 )
+		{
+			/* update pointers to the current QP vectors */
+			g_current = &(g[k*nV]);
+			if ( lb != 0 )
+				lb_current = &(lb[k*nV]);
+			if ( ub != 0 )
+				ub_current = &(ub[k*nV]);
+			if ( lbA != 0 )
+				lbA_current = &(lbA[k*nC]);
+			if ( ubA != 0 )
+				ubA_current = &(ubA[k*nC]);
+
+			nWSRout = nWSRin;
+			maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+			returnvalue = QP.hotstart( g_current,lb_current,ub_current,lbA_current,ubA_current, nWSRout,&maxCpuTimeOut );
+		}
+
+		/* write results into output vectors */
+		obtainOutputs(	k,&QP,returnvalue,nWSRout,maxCpuTimeOut,
+						nOutputs,plhs,nV,nC );
+	}
+
+	//QP.writeQpDataIntoMatFile( "qpDataMat0.mat" );
+
+	return 0;
+}
+
+
+
+/*
+ *	q p O A S E S m e x _ b o u n d s
+ */
+int qpOASESmex_bounds(	int nV, int nP,
+						SymmetricMatrix *H, real_t* g,
+						real_t* lb, real_t* ub,
+						int nWSRin, real_t maxCpuTimeIn,
+						real_t* x0, Options* options,
+						int nOutputs, mxArray* plhs[],
+						double* guessedBounds
+						)
+{
+	int nWSRout;
+	real_t maxCpuTimeOut;
+
+	/* 1) Setup initial QP. */
+	QProblemB QP( nV );
+	QP.setOptions( *options );
+
+	/* 2) Solve initial QP. */
+	returnValue returnvalue;
+
+	Bounds bounds(nV);
+	if (guessedBounds != 0) {
+		for (int i = 0; i < nV; i++) {
+			if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_LOWER);
+			} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_UPPER);
+			} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_INACTIVE);
+			} else {
+				char msg[MAX_STRING_LENGTH];
+				snprintf(msg, MAX_STRING_LENGTH,
+						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
+				myMexErrMsgTxt(msg);
+				return -1;
+			}
+		}
+	}
+
+	nWSRout = nWSRin;
+	maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+	if (x0 == 0 && guessedBounds == 0)
+		returnvalue = QP.init( H,g,lb,ub, nWSRout,&maxCpuTimeOut );
+	else
+		returnvalue = QP.init( H,g,lb,ub, nWSRout,&maxCpuTimeOut, x0, 0,
+				guessedBounds != 0 ? &bounds : 0);
+
+	/* 3) Solve remaining QPs and assign lhs arguments. */
+	/*    Set up pointers to the current QP vectors */
+	real_t* g_current  = g;
+	real_t* lb_current = lb;
+	real_t* ub_current = ub;
+
+	/* Loop through QP sequence. */
+	for ( int k=0; k<nP; ++k )
+	{
+		if ( k > 0 )
+		{
+			/* update pointers to the current QP vectors */
+			g_current = &(g[k*nV]);
+			if ( lb != 0 )
+				lb_current = &(lb[k*nV]);
+			if ( ub != 0 )
+				ub_current = &(ub[k*nV]);
+
+            nWSRout = nWSRin;
+			maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+			returnvalue = QP.hotstart( g_current,lb_current,ub_current, nWSRout,&maxCpuTimeOut );
+		}
+
+		/* write results into output vectors */
+		obtainOutputs(	k,&QP,returnvalue,nWSRout,maxCpuTimeOut,
+						nOutputs,plhs,nV );
+	}
+
+	return 0;
+}
+
+
+
+/*
+ *	m e x F u n c t i o n
+ */
+void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
+{
+	/* inputs */
+	SymmetricMatrix *H=0;
+	Matrix *A=0;
+	real_t *g=0, *lb=0, *ub=0, *lbA=0, *ubA=0, *x0=0;
+	int H_idx, g_idx, A_idx, lb_idx, ub_idx, lbA_idx, ubA_idx, options_idx=-1, x0_idx=-1, auxInput_idx=-1;
+
+	double *guessedBoundsAndConstraints = 0;
+	double *guessedBounds = 0, *guessedConstraints = 0;
+
+    /* Setup default options */
+	Options options;
+	options.printLevel = PL_LOW;
+	#ifdef __DEBUG__
+	options.printLevel = PL_HIGH;
+	#endif
+	#ifdef __SUPPRESSANYOUTPUT__
+	options.printLevel = PL_NONE;
+	#endif
+
+	/* dimensions */
+	unsigned int nV=0, nC=0, nP=0;
+	BooleanType isSimplyBoundedQp = BT_FALSE;
+
+	/* sparse matrix indices and values */
+	sparse_int_t *Hir = 0, *Hjc = 0, *Air = 0, *Ajc = 0;
+	real_t *Hv = 0, *Av = 0;
+
+	/* I) CONSISTENCY CHECKS: */
+	/* 1a) Ensure that qpOASES is called with a feasible number of input arguments. */
+	if ( ( nrhs < 4 ) || ( nrhs > 9 ) )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES' for further information." );
+		return;
+	}
+    
+	/* 2) Check for proper number of output arguments. */
+	if ( nlhs > 6 )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): At most six output arguments are allowed: \n    [x,fval,exitflag,iter,lambda,workingSet]!" );
+		return;
+	}
+	if ( nlhs < 1 )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): At least one output argument is required: [x,...]!" );
+		return;
+	}
+
+
+	/* II) PREPARE RESPECTIVE QPOASES FUNCTION CALL: */
+	/*     Choose between QProblem and QProblemB object and assign the corresponding
+	 *     indices of the input pointer array in to order to access QP data correctly. */
+	H_idx = 0;
+	g_idx = 1;
+	nV = (int)mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
+	nP = (int)mxGetN( prhs[ g_idx ] ); /* number of columns of the gradient matrix (vectors series have to be stored columnwise!) */
+
+	if ( nrhs <= 6 )
+        isSimplyBoundedQp = BT_TRUE;
+	else
+		isSimplyBoundedQp = BT_FALSE;
+
+
+	/* 0) Check whether options are specified .*/
+	if ( isSimplyBoundedQp == BT_TRUE )
+	{
+		if ( ( nrhs >= 5 ) && ( !mxIsEmpty(prhs[4]) ) && ( mxIsStruct(prhs[4]) ) )
+			options_idx = 4;
+	}
+	else
+	{
+		/* Consistency check */
+		if ( ( !mxIsEmpty(prhs[4]) ) && ( mxIsStruct(prhs[4]) ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Fifth input argument must not be a struct when solving QP with general constraints!\nType 'help qpOASES' for further information." );
+			return;
+		}
+
+		if ( ( nrhs >= 8 ) && ( !mxIsEmpty(prhs[7]) ) && ( mxIsStruct(prhs[7]) ) )
+			options_idx = 7;
+	}
+
+	// Is the third argument constraint Matrix A?
+	int numberOfColumns = (int)mxGetN(prhs[2]);
+
+	/* 1) Simply bounded QP. */
+	if ( ( isSimplyBoundedQp == BT_TRUE ) ||
+		 ( ( numberOfColumns == 1 ) && ( nV != 1 ) ) )
+	{
+		lb_idx   = 2;
+		ub_idx   = 3;
+
+		if ( ( nrhs >= 6 ) && ( !mxIsEmpty(prhs[5]) ) )
+		{ 
+			/* auxInput specified */
+			if ( mxIsStruct(prhs[5]) )
+			{
+				auxInput_idx = 5;
+				x0_idx = -1;
+			}
+			else
+			{
+				auxInput_idx = -1;
+				x0_idx = 5;
+			}
+		}
+		else
+		{
+			auxInput_idx = -1;
+			x0_idx = -1;
+		}
+	}
+	else
+	{
+		A_idx = 2;
+
+		/* If constraint matrix is empty, use a QProblemB object! */
+		if ( mxIsEmpty( prhs[ A_idx ] ) )
+		{
+			lb_idx   = 3;
+			ub_idx   = 4;
+
+			nC = 0;
+		}
+		else
+		{
+			lb_idx   = 3;
+			ub_idx   = 4;
+			lbA_idx  = 5;
+			ubA_idx  = 6;
+
+			nC = (int)mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */
+		}
+
+		if ( ( nrhs >= 9 ) && ( !mxIsEmpty(prhs[8]) ) )
+		{ 
+			/* auxInput specified */
+			if ( mxIsStruct(prhs[8]) )
+			{
+				auxInput_idx = 8;
+				x0_idx = -1;
+			}
+			else
+			{
+				auxInput_idx = -1;
+				x0_idx = 8;
+			}
+		}
+		else
+		{
+			auxInput_idx = -1;
+			x0_idx = -1;
+		}
+	}
+
+
+	/* ensure that data is given in real_t precision */
+	if ( ( mxIsDouble( prhs[ H_idx ] ) == 0 ) ||
+		 ( mxIsDouble( prhs[ g_idx ] ) == 0 ) )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
+		return;
+	}
+
+	/* check if supplied data contains 'NaN' or 'Inf' */
+	if (containsNaNorInf(prhs, nV * nV, H_idx, 0) == BT_TRUE) {
+		return;
+	}
+
+	if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
+		return;
+	}
+
+	if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
+		return;
+	}
+	if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
+		return;
+	}
+
+	/* Check inputs dimensions and assign pointers to inputs. */
+	if ( mxGetN( prhs[ H_idx ] ) != nV )
+	{
+		char msg[MAX_STRING_LENGTH]; 
+		snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Hessian matrix input dimension mismatch (%ld != %d)!", 
+				(long int)mxGetN(prhs[H_idx]), nV);
+		myMexErrMsgTxt(msg);
+		return;
+	}
+
+	if ( nC > 0 )
+	{
+		/* ensure that data is given in real_t precision */
+		if ( mxIsDouble( prhs[ A_idx ] ) == 0 )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
+			return;
+		}
+
+		/* Check inputs dimensions and assign pointers to inputs. */
+		if ( mxGetN( prhs[ A_idx ] ) != nV )
+		{
+			char msg[MAX_STRING_LENGTH]; 
+			snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Constraint matrix input dimension mismatch (%ld != %d)!", 
+					(long int)mxGetN(prhs[A_idx]), nV);
+			myMexErrMsgTxt(msg);
+			return;
+		}
+
+		if (containsNaNorInf(prhs, nV * nC, A_idx, 0) == BT_TRUE) {
+			return;
+		}
+		if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
+			return;
+		}
+		if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
+			return;
+		}
+	}
+
+	/* check dimensions and copy auxInputs */
+	if ( smartDimensionCheck( &g,nV,nP, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
+		return;
+
+	if ( smartDimensionCheck( &lb,nV,nP, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
+		return;
+
+	if ( smartDimensionCheck( &ub,nV,nP, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
+		return;
+
+	if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,x0_idx ) != SUCCESSFUL_RETURN )
+		return;
+
+	if ( nC > 0 )
+	{
+		if ( smartDimensionCheck( &lbA,nC,nP, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &ubA,nC,nP, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
+			return;
+	}
+
+	if ( auxInput_idx >= 0 )
+		setupAuxiliaryInputs( prhs[auxInput_idx],nV,nC, &x0,&guessedBoundsAndConstraints,&guessedBounds,&guessedConstraints );
+
+	
+	/* III) ACTUALLY PERFORM QPOASES FUNCTION CALL: */
+	int nWSRin = 5*(nV+nC);
+	real_t maxCpuTimeIn = -1.0;
+
+	if ( options_idx > 0 )
+		setupOptions( &options,prhs[options_idx],nWSRin,maxCpuTimeIn );
+
+	/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
+	setupHessianMatrix(	prhs[H_idx],nV, &H,&Hir,&Hjc,&Hv );
+
+	/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
+	if ( nC > 0 )
+		setupConstraintMatrix( prhs[A_idx],nV,nC, &A,&Air,&Ajc,&Av );
+
+	allocateOutputs( nlhs,plhs,nV,nC,nP );
+
+	if ( nC == 0 )
+	{
+		/* Call qpOASES (using QProblemB class). */
+		qpOASESmex_bounds(	nV,nP,
+							H,g,
+							lb,ub,
+							nWSRin,maxCpuTimeIn,
+							x0,&options,
+							nlhs,plhs,
+							guessedBounds
+							);
+
+		deleteAuxiliaryInputs( &guessedBounds,0 );
+        delete H;
+		if (Hv != 0) delete[] Hv;
+		if (Hjc != 0) delete[] Hjc;
+		if (Hir != 0) delete[] Hir;
+		return;
+	}
+	else
+	{
+		/* Call qpOASES (using QProblem class). */
+		qpOASESmex_constraints(	nV,nC,nP,
+								H,g,A,
+								lb,ub,lbA,ubA,
+								nWSRin,maxCpuTimeIn,
+								x0,&options,
+								nlhs,plhs,
+								guessedBounds, guessedConstraints
+								);
+		
+		deleteAuxiliaryInputs( &guessedBounds,&guessedConstraints );
+		delete A;
+		delete H;
+		if (Av != 0) delete[] Av;
+		if (Ajc != 0) delete[] Ajc;
+		if (Air != 0) delete[] Air;
+		if (Hv != 0) delete[] Hv;
+		if (Hjc != 0) delete[] Hjc;
+		if (Hir != 0) delete[] Hir;
+		return;
+	}
+}
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES.m
new file mode 100644
index 00000000000..e5ab89631e9
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES.m
@@ -0,0 +1,77 @@
+%qpOASES -- An Implementation of the Online Active Set Strategy.
+%Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%Christian Kirches et al. All rights reserved.
+%
+%qpOASES is distributed under the terms of the
+%GNU Lesser General Public License 2.1 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.
+%
+%---------------------------------------------------------------------------------
+%
+%qpOASES solves (a series) of quadratic programming (QP) problems of the
+%following form:
+%
+%                min   1/2*x'Hx + x'g
+%                s.t.  lb  <=  x <= ub
+%                      lbA <= Ax <= ubA  {optional}
+%
+%Call
+%
+%    [x,fval,exitflag,iter,lambda,auxOutput] = 
+%                     qpOASES( H,g,A,lb,ub,lbA,ubA{,options{,auxInput}} )
+%
+%for solving the above-mentioned QP. H must be a symmetric (but possibly 
+%indefinite) matrix and all vectors g, lb, ub, lbA, ubA have to be given
+%as column vectors. Options can be generated using the qpOASES_options command, 
+%otherwise default values are used. Optionally, further auxiliary inputs
+%may be generated using qpOASES_auxInput command and passed to the solver.
+%Both matrices H or A may be passed in sparse matrix format.
+%
+%Call
+%
+%    [x,fval,exitflag,iter,lambda,auxOutput] =
+%                     qpOASES( H,g,lb,ub{,options{,auxInput}} )
+%
+%for solving the above-mentioned QP without general constraints.
+%
+%
+%Optional outputs (only x is mandatory):
+%    x          -  Optimal primal solution vector (if exitflag==0).
+%    fval       -  Optimal objective function value (if exitflag==0).
+%    exitflag   -   0: QP problem solved,
+%                   1: QP could not be solved within given number of iterations,
+%                  -1: QP could not be solved due to an internal error,
+%                  -2: QP is infeasible (and thus could not be solved),
+%                  -3: QP is unbounded (and thus could not be solved).
+%    iter       -  Number of active set iterations actually performed.
+%    lambda     -  Optimal dual solution vector (if status==0).
+%    auxOutput  -  Struct containing auxiliary outputs as described below.
+%
+%The auxOutput struct contains the following entries:
+%    workingSet -  The working set at point x. The working set is a subset
+%                  of the active set (which is the set of all indices
+%                  corresponding to bounds/constraints that hold with 
+%                  equality). The working set corresponds to bound/
+%                  constraint row vectors forming a linear independent set.
+%                  The first nV elements correspond to the bounds, the last
+%                  nC elements to the constraints.
+%                  The working set is encoded as follows:
+%                   1: bound/constraint at its upper bound
+%                   0: bound/constraint not at any bound
+%                  -1: bound/constraint at its lower bound
+%    cpuTime    -  Internally measured CPU time for solving QP.
+%
+%
+%If not a single QP but a sequence of QPs with varying vectors is to be solved,
+%the i-th QP is given by the i-th columns of the QP vectors g, lb, ub, lbA, ubA
+%(i.e. they are matrices in this case). Both matrices H and A remain constant.
+%
+%See also QPOASES_OPTIONS, QPOASES_AUXINPUT, QPOASES_SEQUENCE
+%
+%
+%For additional information see the qpOASES User's Manual or
+%visit http://www.qpOASES.org/.
+%
+%Please send remarks and questions to support@qpOASES.org!
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_auxInput.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_auxInput.m
new file mode 100644
index 00000000000..6d46efe149d
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_auxInput.m
@@ -0,0 +1,103 @@
+%qpOASES -- An Implementation of the Online Active Set Strategy.
+%Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%Christian Kirches et al. All rights reserved.
+%
+%qpOASES is distributed under the terms of the
+%GNU Lesser General Public License 2.1 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.
+%
+%---------------------------------------------------------------------------------
+%
+%Returns a struct containing all possible auxiliary inputs to be passed 
+%to qpOASES.
+%
+%Call
+%    auxInput = qpOASES_auxInput();
+%to obtain a struct with all auxiliary inputs empty.
+%
+%Call
+%    auxInput = qpOASES_auxInput( 'input1',value1,'input2',value2,... )
+%to obtain a struct with 'input1' set to value1 etc. and all remaining
+%auxiliary inputs empty.
+%
+%Call
+%    auxInput = qpOASES_auxInput( oldInputs,'input1',value1,... )
+%to obtain a copy of the options struct oldInputs but with 'input1' set to 
+%value1 etc.
+%
+%
+%qpOASES features the following auxiliary inputs:
+%  x0                   -  Initial guess for optimal primal solution.
+%  guessedWorkingSet    -  Initial guess for working set at optimal
+%                          solution. The first nV elements correspond 
+%                          to the bounds, the last nC elements to the 
+%                          constraints.
+%                          The working set needs to be encoded as follows:
+%                          1: bound/constraint at its upper bound
+%                          0: bound/constraint not at any bound
+%                         -1: bound/constraint at its lower bound
+%
+%
+%See also QPOASES, QPOASES_SEQUENCE, QPOASES_OPTIONS
+%
+%
+%For additional information see the qpOASES User's Manual or
+%visit http://www.qpOASES.org/.
+%
+%Please send remarks and questions to support@qpOASES.org!
+function [ auxInput ] = qpOASES_auxInput( varargin )
+
+	firstIsStruct = 0;
+
+	if ( nargin == 0 ) 
+		auxInput = qpOASES_emptyAuxInput();
+	else
+		if ( isstruct( varargin{1} ) )
+			if ( mod( nargin,2 ) ~= 1 )
+				error('ERROR (qpOASES_auxInput): Auxiliary inputs must be specified in pairs!');
+			end
+			auxInput = varargin{1};
+			firstIsStruct = 1;
+        else
+            if ( mod( nargin,2 ) ~= 0 )
+				error('ERROR (qpOASES_auxInput): Auxiliary inputs must be specified in pairs!');
+            end
+            auxInput = qpOASES_emptyAuxInput();
+		end
+	end
+
+	% set options to user-defined values  
+	for i=(1+firstIsStruct):2:nargin
+
+		argName  = varargin{i};
+		argValue = varargin{i+1};
+
+        if ( ( isempty( argName ) ) || ( ~ischar( argName ) ) )
+			error('ERROR (qpOASES_auxInput): Argmument no. %d has to be a non-empty string!',i );
+        end
+			
+        if ( ( ischar(argValue) ) || ( ~isnumeric( argValue ) ) )
+			error('ERROR (qpOASES_auxInput): Argmument no. %d has to be a numerical constant!',i+1 );
+        end
+
+        if ( ~isfield( auxInput,argName ) )
+			error('ERROR (qpOASES_auxInput): Argmument no. %d is not a valid auxiliary input!',i );
+        end
+
+		eval( ['auxInput.',argName,' = argValue;'] );
+
+	end
+
+end
+
+
+function [ auxInput ] = qpOASES_emptyAuxInput( )
+
+	% setup auxiliary input struct with all entries empty
+	auxInput = struct(	'x0',                 [], ...
+						'guessedWorkingSet',  []  ...
+                        );
+
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_matlab_utils.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_matlab_utils.cpp
new file mode 100644
index 00000000000..7362704f46a
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_matlab_utils.cpp
@@ -0,0 +1,885 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file interfaces/matlab/qpOASES_matlab_utils.cpp
+ *	\author Hans Joachim Ferreau, Alexander Buchner
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Collects utility functions for Interface to Matlab(R) that
+ *	enables to call qpOASES as a MEX function.
+ *
+ */
+
+
+
+QPInstance::QPInstance(	int _nV, int _nC, BooleanType _isSimplyBounded
+						)
+{
+	handle = s_nexthandle++;
+
+	if ( _nC > 0 )
+		isSimplyBounded = BT_FALSE;
+	else
+		isSimplyBounded = _isSimplyBounded;
+	
+	if ( isSimplyBounded == BT_TRUE )
+	{
+		sqp = 0;
+		qpb = new QProblemB( _nV );
+	}
+	else
+	{
+		sqp = new SQProblem( _nV,_nC );
+		qpb = 0;
+	}
+
+	H = 0;
+	A = 0;
+	Hir = 0; 
+	Hjc = 0; 
+	Air = 0; 
+	Ajc = 0;
+	Hv = 0;
+	Av = 0;
+}	
+
+
+QPInstance::~QPInstance( )
+{		
+	deleteQPMatrices();
+
+	if ( sqp != 0 )
+	{
+		delete sqp;
+		sqp = 0;
+	}
+
+	if ( qpb != 0 )
+	{
+		delete qpb;
+		qpb = 0;
+	}
+}
+
+
+returnValue QPInstance::deleteQPMatrices( )
+{
+	if ( H != 0 )
+	{
+		delete H;
+		H = 0;
+	}
+
+	if (Hv != 0)
+	{
+		delete[] Hv;
+		Hv = 0;
+	}
+	
+	if (Hjc != 0)
+	{
+		delete[] Hjc;
+		Hjc = 0;
+	}
+	
+	if (Hir != 0)
+	{
+		delete[] Hir;
+		Hir = 0;
+	}
+	
+	if ( A != 0 )
+	{
+		delete A;
+		A = 0;
+	}
+
+	if (Av != 0)
+	{
+		delete[] Av;
+		Av = 0;
+	}
+	
+	if (Ajc != 0)
+	{
+		delete[] Ajc;
+		Ajc = 0;
+	}
+	
+	if (Air != 0)
+	{
+		delete[] Air;
+		Air = 0;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+int QPInstance::getNV() const
+{
+    if ( sqp != 0 )
+        return sqp->getNV();
+    
+    if ( qpb != 0 )
+        return qpb->getNV();
+    
+    return 0;
+}
+
+
+int QPInstance::getNC() const
+{
+    if ( sqp != 0 )
+        return sqp->getNC();
+   
+    return 0;
+}
+
+
+/*
+ *	a l l o c a t e Q P r o b l e m I n s t a n c e
+ */
+int allocateQPInstance(	int nV, int nC, BooleanType isSimplyBounded, const Options *options
+						)
+{
+	QPInstance* inst = new QPInstance( nV,nC,isSimplyBounded );
+
+	if ( inst->sqp != 0 )
+		inst->sqp->setOptions ( *options );
+	
+	if ( inst->qpb != 0 )
+		inst->qpb->setOptions ( *options );
+
+	g_instances.push_back (inst);
+	return inst->handle;
+}
+
+
+/*
+ *  g e t Q P r o b l e m I n s t a n c e
+ */
+QPInstance* getQPInstance( int handle )
+{
+	unsigned int ii;
+	// TODO: this may become slow ...
+	for (ii = 0; ii < g_instances.size (); ++ii)
+		if (g_instances[ii]->handle == handle)
+			return g_instances[ii];
+	return 0;
+}
+
+
+/*
+ *	d e l e t e Q P r o b l e m I n s t a n c e
+ */
+void deleteQPInstance( int handle )
+{
+	QPInstance *instance = getQPInstance (handle);
+	if (instance != 0) {
+		for (std::vector<QPInstance*>::iterator itor = g_instances.begin ();
+		     itor != g_instances.end (); ++itor)
+		     if ((*itor)->handle == handle) {
+				g_instances.erase (itor);
+				break;
+			}
+		delete instance;
+	}
+}
+
+
+
+/*
+ *	s m a r t D i m e n s i o n C h e c k
+ */
+returnValue smartDimensionCheck(	real_t** input, unsigned int m, unsigned int n, BooleanType emptyAllowed,
+									const mxArray* prhs[], int idx
+									)
+{
+	/* If index is negative, the input does not exist. */
+	if ( idx < 0 )
+	{
+		*input = 0;
+		return SUCCESSFUL_RETURN;
+	}
+
+	/* Otherwise the input has been passed by the user. */
+	if ( mxIsEmpty( prhs[ idx ] ) )
+	{
+		/* input is empty */
+		if ( ( emptyAllowed == BT_TRUE ) || ( idx == 0 ) ) /* idx==0 used for auxInput */
+		{
+			*input = 0;
+			return SUCCESSFUL_RETURN;
+		}
+		else
+		{
+			char msg[MAX_STRING_LENGTH];
+			if ( idx > 0 )
+				snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Empty argument %d not allowed!", idx+1);
+			myMexErrMsgTxt( msg );
+			return RET_INVALID_ARGUMENTS;
+		}
+	}
+	else
+	{
+		/* input is non-empty */
+        if ( mxIsSparse( prhs[ idx ] ) == 0 )
+        {
+            if ( ( mxGetM( prhs[ idx ] ) == m ) && ( mxGetN( prhs[ idx ] ) == n ) )
+            {
+                *input = (real_t*) mxGetPr( prhs[ idx ] );
+                return SUCCESSFUL_RETURN;
+            }
+            else
+            {
+                char msg[MAX_STRING_LENGTH];
+				if ( idx > 0 )
+					snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Input dimension mismatch for argument %d ([%ld,%ld] ~= [%d,%d]).",
+							 idx+1, (long int)mxGetM(prhs[idx]), (long int)mxGetN(prhs[idx]), m, n);
+				else /* idx==0 used for auxInput */
+					snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Input dimension mismatch for some auxInput entry ([%ld,%ld] ~= [%d,%d]).",
+							 (long int)mxGetM(prhs[idx]), (long int)mxGetN(prhs[idx]), m, n);
+                myMexErrMsgTxt( msg );
+                return RET_INVALID_ARGUMENTS;
+            }
+        }
+        else
+        {
+            char msg[MAX_STRING_LENGTH];
+			if ( idx > 0 )
+				snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Vector argument %d must not be in sparse format!", idx+1);
+			else /* idx==0 used for auxInput */
+				snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): auxInput entries must not be in sparse format!" );
+			myMexErrMsgTxt( msg );
+			return RET_INVALID_ARGUMENTS;
+        }
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	c o n t a i n s N a N
+ */
+BooleanType containsNaN( const real_t* const data, int dim )
+{
+	int i;
+
+	if ( data == 0 )
+		return BT_FALSE;
+
+	for ( i = 0; i < dim; ++i )
+		if ( mxIsNaN(data[i]) == 1 )
+			return BT_TRUE;
+
+	return BT_FALSE;
+}
+
+
+/*
+ *	c o n t a i n s I n f
+ */
+BooleanType containsInf( const real_t* const data, int dim )
+{
+	int i;
+
+	if ( data == 0 )
+		return BT_FALSE;
+
+	for ( i = 0; i < dim; ++i )
+		if ( mxIsInf(data[i]) == 1 )
+			return BT_TRUE;
+
+	return BT_FALSE;
+}
+
+
+/*
+ *	c o n t a i n s N a N o r I n f
+ */
+BooleanType containsNaNorInf(const mxArray* prhs[], int dim, int rhs_index,
+		bool mayContainInf) {
+
+	char msg[MAX_STRING_LENGTH];
+
+	// overwrite dim for sparse matrices
+	if (mxIsSparse(prhs[rhs_index]) == 1) {
+		dim = mxGetNzmax(prhs[rhs_index]);
+	}
+
+	if (containsNaN((real_t*) mxGetPr(prhs[rhs_index]), dim) == BT_TRUE) {
+		snprintf(msg, MAX_STRING_LENGTH,
+				"ERROR (qpOASES): Argument %d contains 'NaN' !", rhs_index + 1);
+		myMexErrMsgTxt(msg);
+		return BT_TRUE;
+	}
+
+	if (mayContainInf == 0) {
+		if (containsInf((real_t*) mxGetPr(prhs[rhs_index]), dim) == BT_TRUE) {
+			snprintf(msg, MAX_STRING_LENGTH,
+					"ERROR (qpOASES): Argument %d contains 'Inf' !",
+					rhs_index + 1);
+			myMexErrMsgTxt(msg);
+			return BT_TRUE;
+		}
+	}
+
+	return BT_FALSE;
+}
+
+
+/*
+ *	c o n v e r t F o r t r a n T o C
+ */
+returnValue convertFortranToC( const real_t* const A_for, int nV, int nC, real_t* const A )
+{
+	int i,j;
+
+	for ( i=0; i<nC; ++i )
+		for ( j=0; j<nV; ++j )
+			A[i*nV + j] = A_for[j*nC + i];
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	h a s O p t i o n s V a l u e
+ */
+BooleanType hasOptionsValue( const mxArray* optionsPtr, const char* const optionString, double** optionValue )
+{
+	mxArray* optionName = mxGetField( optionsPtr,0,optionString );
+
+	if ( optionName == 0 )
+	{
+		char msg[MAX_STRING_LENGTH];
+		snprintf(msg, MAX_STRING_LENGTH, "Option struct does not contain entry '%s', using default value instead!", optionString );
+		mexWarnMsgTxt( msg );
+		return BT_FALSE;
+	}
+
+	if ( ( mxIsEmpty(optionName) == false ) && ( mxGetM( optionName ) == 1 ) && ( mxGetN( optionName ) == 1 ) )
+	{
+		*optionValue = mxGetPr( optionName );
+		return BT_TRUE;
+	}
+	else
+	{
+		char msg[MAX_STRING_LENGTH];
+		snprintf(msg, MAX_STRING_LENGTH, "Option '%s' is not a scalar, using default value instead!", optionString );
+		mexWarnMsgTxt( msg );
+		return BT_FALSE;
+	}
+}
+
+
+/*
+ *	s e t u p O p t i o n s
+ */
+returnValue setupOptions( Options* options, const mxArray* optionsPtr, int& nWSRin, real_t& maxCpuTime )
+{
+	double* optionValue;
+	int optionValueInt;
+
+	/* Check for correct number of option entries;
+	 * may occur, e.g., if user types options.<misspelledName> = <someValue>; */
+	if ( mxGetNumberOfFields(optionsPtr) != 31 )
+		mexWarnMsgTxt( "Options might be set incorrectly as struct has wrong number of entries!\n         Type 'help qpOASES_options' for further information." );
+
+
+	if ( hasOptionsValue( optionsPtr,"maxIter",&optionValue ) == BT_TRUE )
+		if ( *optionValue >= 0.0 )
+			nWSRin = (int)*optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"maxCpuTime",&optionValue ) == BT_TRUE )
+		if ( *optionValue >= 0.0 )
+			maxCpuTime = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"printLevel",&optionValue ) == BT_TRUE )
+	{
+        #ifdef __SUPPRESSANYOUTPUT__
+        options->printLevel = PL_NONE;
+        #else
+		optionValueInt = (int)*optionValue;
+		options->printLevel = (REFER_NAMESPACE_QPOASES PrintLevel)optionValueInt;
+        if ( options->printLevel < PL_DEBUG_ITER )
+            options->printLevel = PL_DEBUG_ITER;
+        if ( options->printLevel > PL_HIGH )
+            options->printLevel = PL_HIGH;       
+        #endif
+	}
+
+	if ( hasOptionsValue( optionsPtr,"enableRamping",&optionValue ) == BT_TRUE )
+	{
+		optionValueInt = (int)*optionValue;
+		options->enableRamping = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
+	}
+
+	if ( hasOptionsValue( optionsPtr,"enableFarBounds",&optionValue ) == BT_TRUE )
+	{
+		optionValueInt = (int)*optionValue;
+		options->enableFarBounds = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
+	}
+
+	if ( hasOptionsValue( optionsPtr,"enableFlippingBounds",&optionValue ) == BT_TRUE )
+	{
+		optionValueInt = (int)*optionValue;
+		options->enableFlippingBounds = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
+	}
+
+	if ( hasOptionsValue( optionsPtr,"enableRegularisation",&optionValue ) == BT_TRUE )
+	{
+		optionValueInt = (int)*optionValue;
+		options->enableRegularisation = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
+	}
+
+	if ( hasOptionsValue( optionsPtr,"enableFullLITests",&optionValue ) == BT_TRUE )
+	{
+		optionValueInt = (int)*optionValue;
+		options->enableFullLITests = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
+	}
+
+	if ( hasOptionsValue( optionsPtr,"enableNZCTests",&optionValue ) == BT_TRUE )
+	{
+		optionValueInt = (int)*optionValue;
+		options->enableNZCTests = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
+	}
+
+	if ( hasOptionsValue( optionsPtr,"enableDriftCorrection",&optionValue ) == BT_TRUE )
+		options->enableDriftCorrection = (int)*optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"enableCholeskyRefactorisation",&optionValue ) == BT_TRUE )
+		options->enableCholeskyRefactorisation = (int)*optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"enableEqualities",&optionValue ) == BT_TRUE )
+	{
+		optionValueInt = (int)*optionValue;
+		options->enableEqualities = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
+	}
+
+
+	if ( hasOptionsValue( optionsPtr,"terminationTolerance",&optionValue ) == BT_TRUE )
+		options->terminationTolerance = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"boundTolerance",&optionValue ) == BT_TRUE )
+		options->boundTolerance = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"boundRelaxation",&optionValue ) == BT_TRUE )
+		options->boundRelaxation = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"epsNum",&optionValue ) == BT_TRUE )
+		options->epsNum = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"epsDen",&optionValue ) == BT_TRUE )
+		options->epsDen = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"maxPrimalJump",&optionValue ) == BT_TRUE )
+		options->maxPrimalJump = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"maxDualJump",&optionValue ) == BT_TRUE )
+		options->maxDualJump = *optionValue;
+
+
+	if ( hasOptionsValue( optionsPtr,"initialRamping",&optionValue ) == BT_TRUE )
+		options->initialRamping = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"finalRamping",&optionValue ) == BT_TRUE )
+		options->finalRamping = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"initialFarBounds",&optionValue ) == BT_TRUE )
+		options->initialFarBounds = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"growFarBounds",&optionValue ) == BT_TRUE )
+		options->growFarBounds = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"initialStatusBounds",&optionValue ) == BT_TRUE )
+	{
+		optionValueInt = (int)*optionValue;
+		if ( optionValueInt < -1 ) 
+			optionValueInt = -1;
+		if ( optionValueInt > 1 ) 
+			optionValueInt = 1;
+		options->initialStatusBounds = (REFER_NAMESPACE_QPOASES SubjectToStatus)optionValueInt;
+	}
+
+	if ( hasOptionsValue( optionsPtr,"epsFlipping",&optionValue ) == BT_TRUE )
+		options->epsFlipping = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"numRegularisationSteps",&optionValue ) == BT_TRUE )
+		options->numRegularisationSteps = (int)*optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"epsRegularisation",&optionValue ) == BT_TRUE )
+		options->epsRegularisation = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"numRefinementSteps",&optionValue ) == BT_TRUE )
+		options->numRefinementSteps = (int)*optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"epsIterRef",&optionValue ) == BT_TRUE )
+		options->epsIterRef = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"epsLITests",&optionValue ) == BT_TRUE )
+		options->epsLITests = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"epsNZCTests",&optionValue ) == BT_TRUE )
+		options->epsNZCTests = *optionValue;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	s e t u p A u x i l i a r y I n p u t s
+ */
+returnValue setupAuxiliaryInputs(	const mxArray* auxInput, unsigned int nV, unsigned int nC,
+									double** x0, double** guessedBoundsAndConstraints, double** guessedBounds, double** guessedConstraints
+									)
+{
+	unsigned int i;
+	mxArray* curField = 0;
+
+	/* x0 */
+	curField = mxGetField( auxInput,0,"x0" );
+	if ( curField == NULL )
+		mexWarnMsgTxt( "auxInput struct does not contain entry 'x0'!\n         Type 'help qpOASES_auxInput' for further information." );
+	else
+	{
+		*x0 = mxGetPr(curField);
+		if ( smartDimensionCheck( x0,nV,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
+			return RET_INVALID_ARGUMENTS;
+	}
+
+	/* guessedWorkingSet */
+	curField = mxGetField( auxInput,0,"guessedWorkingSet" );
+	if ( curField == NULL )
+		mexWarnMsgTxt( "auxInput struct does not contain entry 'guessedWorkingSet'!\n         Type 'help qpOASES_auxInput' for further information." );
+	else
+	{
+		*guessedBoundsAndConstraints = mxGetPr(curField);
+		if ( smartDimensionCheck( guessedBoundsAndConstraints,nV+nC,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
+			return RET_INVALID_ARGUMENTS;
+
+		if ( *guessedBoundsAndConstraints != 0 )
+		{
+			*guessedBounds = new double[nV];
+			for (i = 0; i < nV; i++)
+				(*guessedBounds)[i] = (*guessedBoundsAndConstraints)[i];
+
+			if ( nC > 0 )
+			{
+				*guessedConstraints = new double[nC];
+				for (i = 0; i < nC; i++)
+					(*guessedConstraints)[i] = (*guessedBoundsAndConstraints)[i + nV];
+			}
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	d e l e t e A u x i l i a r y I n p u t s
+ */
+returnValue deleteAuxiliaryInputs(	double** guessedBounds, double** guessedConstraints
+									)
+{
+	if ( ( guessedBounds != 0 ) && ( *guessedBounds != 0 ) )
+	{
+		delete[] (*guessedBounds);
+		*guessedBounds = 0;
+	}
+
+	if ( ( guessedConstraints != 0 ) && ( guessedConstraints != 0 ) )
+	{
+		delete[] (*guessedConstraints);
+		*guessedConstraints = 0;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	a l l o c a t e O u t p u t s
+ */
+returnValue allocateOutputs(	int nlhs, mxArray* plhs[], int nV, int nC = 0, int nP = 1, int handle = -1
+								)
+{
+	/* Create output vectors and assign pointers to them. */
+	int curIdx = 0;
+
+	/* handle */
+	if ( handle >= 0 )
+		plhs[curIdx++] = mxCreateDoubleMatrix( 1, 1, mxREAL );
+
+	/* x */
+	plhs[curIdx++] = mxCreateDoubleMatrix( nV, nP, mxREAL );
+
+	if ( nlhs > curIdx )
+	{
+		/* fval */
+		plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
+
+		if ( nlhs > curIdx )
+		{
+			/* exitflag */
+			plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
+
+			if ( nlhs > curIdx )
+			{
+				/* iter */
+				plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
+
+				if ( nlhs > curIdx )
+				{
+					/* lambda */
+					plhs[curIdx++] = mxCreateDoubleMatrix( nV+nC, nP, mxREAL );
+
+					if ( nlhs > curIdx )
+					{
+						/* setup auxiliary output struct */
+						mxArray* auxOutput = mxCreateStructMatrix( 1,1,0,0 );
+						int curFieldNum;
+						
+						/* working set */
+						curFieldNum = mxAddField( auxOutput,"workingSet" );
+						if ( curFieldNum >= 0 )
+							mxSetFieldByNumber( auxOutput,0,curFieldNum,mxCreateDoubleMatrix( nV+nC, nP, mxREAL ) );
+
+						curFieldNum = mxAddField( auxOutput,"cpuTime" );
+						if ( curFieldNum >= 0 )
+							mxSetFieldByNumber( auxOutput,0,curFieldNum,mxCreateDoubleMatrix( 1, nP, mxREAL ) );
+
+						plhs[curIdx] = auxOutput;
+					}
+				}
+			}
+		}
+	}
+	
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	o b t a i n O u t p u t s
+ */
+returnValue obtainOutputs(	int k, QProblemB* qp, returnValue returnvalue, int _nWSRout, real_t _cpuTime,
+							int nlhs, mxArray* plhs[], int nV, int nC = 0, int handle = -1
+							)
+{
+	/* Create output vectors and assign pointers to them. */
+	int curIdx = 0;
+
+	/* handle */
+	if ( handle >= 0 )
+		plhs[curIdx++] = mxCreateDoubleScalar( handle );
+
+	/* x */
+	double* x = mxGetPr( plhs[curIdx++] );
+	qp->getPrimalSolution( &(x[k*nV]) );
+
+	if ( nlhs > curIdx )
+	{
+		/* fval */
+		double* obj = mxGetPr( plhs[curIdx++] );
+		obj[k] = qp->getObjVal( );
+
+		if ( nlhs > curIdx )
+		{
+			/* exitflag */
+			double* status = mxGetPr( plhs[curIdx++] );
+			status[k] = (real_t)getSimpleStatus( returnvalue );
+
+			if ( nlhs > curIdx )
+			{
+				/* iter */
+				double* nWSRout = mxGetPr( plhs[curIdx++] );
+				nWSRout[k] = (real_t) _nWSRout;
+
+				if ( nlhs > curIdx )
+				{
+					/* lambda */
+					double* y = mxGetPr( plhs[curIdx++] );
+					qp->getDualSolution( &(y[k*(nV+nC)]) );
+
+					if ( nlhs > curIdx )
+					{
+						
+						mxArray* auxOutput = plhs[curIdx];
+						mxArray* curField = 0;
+
+						/* working set */
+						curField = mxGetField( auxOutput,0,"workingSet" );
+						double* workingSet = mxGetPr(curField);
+
+						QProblem* problemPointer;
+						problemPointer = dynamic_cast<QProblem*>(qp);
+
+						// cast successful?
+						if (problemPointer != NULL) {
+							problemPointer->getWorkingSet( &(workingSet[k*(nV+nC)]) );
+						} else {
+							qp->getWorkingSet( &(workingSet[k*(nV+nC)]) );
+						}
+
+						/* cpu time */
+						curField = mxGetField( auxOutput,0,"cpuTime" );
+						double* cpuTime = mxGetPr(curField);
+						cpuTime[0] = (real_t) _cpuTime;
+					}
+				}
+			}
+		}
+	}
+	
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	s e t u p H e s s i a n M a t r i x
+ */
+returnValue setupHessianMatrix(	const mxArray* prhsH, int nV,
+								SymmetricMatrix** H, sparse_int_t** Hir, sparse_int_t** Hjc, real_t** Hv
+								)
+{
+	if ( mxIsSparse( prhsH ) != 0 )
+	{
+		mwIndex *mat_ir = mxGetIr( prhsH );
+		mwIndex *mat_jc = mxGetJc( prhsH );
+		double *v = (double*)mxGetPr( prhsH );
+		sparse_int_t nfill = 0;
+		mwIndex i, j;
+		BooleanType needInsertDiag;
+
+		/* copy indices to avoid 64/32-bit integer confusion */
+		/* also add explicit zeros on diagonal for regularization strategy */
+		/* copy values, too */
+		*Hir = new sparse_int_t[mat_jc[nV] + nV];
+		*Hjc = new sparse_int_t[nV+1];
+		*Hv = new real_t[mat_jc[nV] + nV];
+        for (j = 0; j < nV; j++) 
+		{
+            needInsertDiag = BT_TRUE;
+                
+            (*Hjc)[j] = (sparse_int_t)(mat_jc[j]) + nfill;
+            /* fill up to diagonal */
+            for (i = mat_jc[j]; i < mat_jc[j+1]; i++) 
+			{
+                if ( mat_ir[i] == j )
+                    needInsertDiag = BT_FALSE;
+                    
+                /* add zero diagonal element if not present */
+                if ( ( mat_ir[i] > j ) && ( needInsertDiag == BT_TRUE ) )
+                {
+                    (*Hir)[i + nfill] = (sparse_int_t)j;
+                    (*Hv)[i + nfill] = 0.0;
+                    nfill++;
+                    /* only add diag once */
+                    needInsertDiag = BT_FALSE;
+                }
+                        
+				(*Hir)[i + nfill] = (sparse_int_t)(mat_ir[i]);
+				(*Hv)[i + nfill] = (real_t)(v[i]);
+			}
+		}
+		(*Hjc)[nV] = (sparse_int_t)(mat_jc[nV]) + nfill;
+
+		SymSparseMat *sH;
+		*H = sH = new SymSparseMat(nV, nV, *Hir, *Hjc, *Hv);
+		sH->createDiagInfo();
+	}
+	else
+	{
+		/* make a deep-copy in order to avoid modifying input data when regularising */
+		real_t* H_for = (real_t*) mxGetPr( prhsH );
+		real_t* H_mem = new real_t[nV*nV];
+		memcpy( H_mem,H_for, nV*nV*sizeof(real_t) );
+
+		*H = new SymDenseMat( nV,nV,nV, H_mem );
+		(*H)->doFreeMemory( );
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p C o n s t r a i n t M a t r i x
+ */
+returnValue setupConstraintMatrix(	const mxArray* prhsA, int nV, int nC,
+									Matrix** A, sparse_int_t** Air, sparse_int_t** Ajc, real_t** Av
+									)
+{
+	if ( mxIsSparse( prhsA ) != 0 )
+	{
+		mwIndex i;
+		long j;
+
+		mwIndex *mat_ir = mxGetIr( prhsA );
+		mwIndex *mat_jc = mxGetJc( prhsA );
+		double *v = (double*)mxGetPr( prhsA );
+
+		/* copy indices to avoid 64/32-bit integer confusion */
+		*Air = new sparse_int_t[mat_jc[nV]];
+		*Ajc = new sparse_int_t[nV+1];
+		for (i = 0; i < mat_jc[nV]; i++)
+			(*Air)[i] = (sparse_int_t)(mat_ir[i]);
+		for (i = 0; i < nV + 1; i++)
+			(*Ajc)[i] = (sparse_int_t)(mat_jc[i]);
+		
+		/* copy values, too */
+		*Av = new real_t[(*Ajc)[nV]];
+		for (j = 0; j < (*Ajc)[nV]; j++)
+			(*Av)[j] = (real_t)(v[j]);
+
+		*A = new SparseMatrix(nC, nV, *Air, *Ajc, *Av);
+	}
+	else
+	{
+		/* Convert constraint matrix A from FORTRAN to C style
+		* (not necessary for H as it should be symmetric!). */
+		real_t* A_for = (real_t*) mxGetPr( prhsA );
+		real_t* A_mem = new real_t[nC*nV];
+		convertFortranToC( A_for,nV,nC, A_mem );
+		*A = new DenseMatrix(nC, nV, nV, A_mem );
+		(*A)->doFreeMemory();
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_matlab_utils.hpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_matlab_utils.hpp
new file mode 100644
index 00000000000..ad588e8ddf9
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_matlab_utils.hpp
@@ -0,0 +1,92 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file interfaces/matlab/qpOASES_matlab_utils.hpp
+ *	\author Hans Joachim Ferreau, Alexander Buchner
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Collects utility functions for Interface to Matlab(R) that
+ *	enables to call qpOASES as a MEX function.
+ *
+ */
+
+
+
+/* Work-around for settings where mexErrMsgTxt causes unexpected behaviour. */
+#ifdef __AVOID_MEXERRMSGTXT__
+	#define myMexErrMsgTxt( TEXT ) mexPrintf( "%s\n\n",(TEXT) );
+#else
+	#define myMexErrMsgTxt mexErrMsgTxt
+#endif
+
+
+#include "mex.h"
+#include "matrix.h"
+#include "string.h"
+#include <vector>
+
+
+/*
+ * QProblem instance class
+ */
+class QPInstance
+{
+	private:
+		static int s_nexthandle;
+
+	public:
+		QPInstance(	int _nV = 0,
+					int _nC = 0,
+					BooleanType _isSimplyBounded = BT_FALSE
+					);
+
+		~QPInstance( );
+	
+		returnValue deleteQPMatrices();
+		
+		int getNV() const;
+		int getNC() const;
+
+		int handle;
+
+		SQProblem* sqp;
+		QProblemB* qpb;
+		BooleanType isSimplyBounded;
+
+		SymmetricMatrix* H;
+		Matrix* A;
+		sparse_int_t* Hir; 
+		sparse_int_t* Hjc; 
+		sparse_int_t* Air; 
+		sparse_int_t* Ajc;
+		real_t* Hv;
+		real_t* Av;
+};
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_options.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_options.m
new file mode 100644
index 00000000000..01b0e17e7c4
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_options.m
@@ -0,0 +1,251 @@
+%qpOASES -- An Implementation of the Online Active Set Strategy.
+%Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%Christian Kirches et al. All rights reserved.
+%
+%qpOASES is distributed under the terms of the
+%GNU Lesser General Public License 2.1 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.
+%
+%---------------------------------------------------------------------------------
+%
+%Returns a struct containing values for all options to be used within qpOASES.
+%
+%Call
+%    options = qpOASES_options( 'default' );
+%    options = qpOASES_options( 'reliable' );
+%    options = qpOASES_options( 'MPC' );
+%to obtain a set of default options or a pre-defined set of options tuned
+%for reliable or fast QP solution, respectively.
+%
+%Call
+%    options = qpOASES_options( 'option1',value1,'option2',value2,... )
+%to obtain a set of default options but with 'option1' set to value1 etc.
+%
+%Call
+%    options = qpOASES_options( oldOptions,'option1',value1,... )
+%to obtain a copy of the options struct oldOptions but with 'option1' set
+%to value1 etc.
+%
+%Call
+%    options = qpOASES_options( 'default', 'option1',value1,... )
+%    options = qpOASES_options( 'reliable','option1',value1,... )
+%    options = qpOASES_options( 'MPC',     'option1',value1,... )
+%to obtain a set of default options or a pre-defined set of options tuned
+%for reliable or fast QP solution, respectively, but with 'option1' set to 
+%value1 etc.
+%
+%
+%qpOASES features the following options:
+%  maxIter                    -  Maximum number of iterations (if set
+%                                to -1, a value is chosen heuristically)
+%  maxCpuTime                 -  Maximum CPU time in seconds (if set
+%                                to -1, only iteration limit is used)
+%  printLevel                 -  0: no printed output,
+%                                1: only error messages are printed,
+%                                2: iterations and error messages are printed,
+%                                3: all available messages are printed.
+%
+%  enableRamping              -  Enables (1) or disables (0) ramping.
+%  enableFarBounds            -  Enables (1) or disables (0) the use of 
+%                                far bounds.
+%  enableFlippingBounds       -  Enables (1) or disables (0) the use of 
+%                                flipping bounds.
+%  enableRegularisation       -  Enables (1) or disables (0) automatic 
+%                                Hessian regularisation.
+%  enableFullLITests          -  Enables (1) or disables (0) condition-hardened 
+%                                (but more expensive) LI test.
+%  enableNZCTests             -  Enables (1) or disables (0) nonzero curvature 
+%                                tests.
+%  enableDriftCorrection      -  Specifies the frequency of drift corrections:
+%                                0: turns them off, 
+%                                1: uses them at each iteration etc.
+%  enableCholeskyRefactorisation - Specifies the frequency of a full re-
+%                                factorisation of projected Hessian matrix:
+%                                0: turns them off, 
+%                                1: uses them at each iteration etc.
+%  enableEqualities           -  Specifies whether equalities should be treated 
+%                                as always active (1) or not (0)
+%
+%  terminationTolerance       -  Relative termination tolerance to stop homotopy.
+%  boundTolerance             -  If upper and lower bounds differ less than this
+%                                tolerance, they are regarded equal, i.e. as 
+%                                equality constraint.
+%  boundRelaxation            -  Initial relaxation of bounds to start homotopy 
+%                                and initial value for far bounds.
+%  epsNum                     -  Numerator tolerance for ratio tests.
+%  epsDen                     -  Denominator tolerance for ratio tests.
+%  maxPrimalJump              -  Maximum allowed jump in primal variables in 
+%                                nonzero curvature tests.
+%  maxDualJump                -  Maximum allowed jump in dual variables in 
+%                                linear independence tests.
+%
+%  initialRamping             -  Start value for ramping strategy.
+%  finalRamping               -  Final value for ramping strategy.
+%  initialFarBounds           -  Initial size for far bounds.
+%  growFarBounds              -  Factor to grow far bounds.
+%  initialStatusBounds        -  Initial status of bounds at first iteration:
+%                                 0: all bounds inactive,
+%                                -1: all bounds active at their lower bound,
+%                                +1: all bounds active at their upper bound.
+%  epsFlipping                -  Tolerance of squared Cholesky diagonal factor 
+%                                which triggers flipping bound.
+%  numRegularisationSteps     -  Maximum number of successive regularisation steps.
+%  epsRegularisation          -  Scaling factor of identity matrix used for 
+%                                Hessian regularisation.
+%  numRefinementSteps         -  Maximum number of iterative refinement steps.
+%  epsIterRef                 -  Early termination tolerance for iterative 
+%                                refinement.
+%  epsLITests                 -  Tolerance for linear independence tests.
+%  epsNZCTests                -  Tolerance for nonzero curvature tests.
+%
+%
+%See also QPOASES, QPOASES_SEQUENCE
+%
+%
+%For additional information see the qpOASES User's Manual or
+%visit http://www.qpOASES.org/.
+%
+%Please send remarks and questions to support@qpOASES.org!
+function [ options ] = qpOASES_options( varargin )
+
+	firstIsStructOrScheme = 0;
+
+	if ( nargin == 0 ) 
+		options = qpOASES_default_options();
+	else
+		if ( isstruct( varargin{1} ) )
+			if ( mod( nargin,2 ) ~= 1 )
+				error('ERROR (qpOASES_options): Options must be specified in pairs!');
+			end
+			options = varargin{1};
+			firstIsStructOrScheme = 1;
+		else
+			if ( ischar( varargin{1} ) )
+				if ( mod( nargin,2 ) == 0 )
+					options = qpOASES_default_options();
+				else
+					if ( ( nargin > 1 ) && ( ischar( varargin{nargin} ) ) )
+						error('ERROR (qpOASES_options): Options must be specified in pairs!');
+					end
+
+					switch ( varargin{1} )
+						case 'default'
+							options = qpOASES_default_options();
+						case 'reliable'
+							options = qpOASES_reliable_options();
+						case {'MPC','mpc','fast'}
+							options = qpOASES_MPC_options();
+						otherwise
+							error( ['ERROR (qpOASES_options): Only the following option schemes are defined: ''default'', ''reliable'', ''MPC''!'] );
+							
+					end
+					firstIsStructOrScheme = 1;
+				end
+			else
+				error('ERROR (qpOASES_options): First argument needs to be a string or an options struct!');
+			end
+		end
+	end
+
+	% set options to user-defined values  
+	for i=(1+firstIsStructOrScheme):2:nargin
+
+		argName  = varargin{i};
+		argValue = varargin{i+1};
+
+		if ( ( isempty( argName ) ) || ( ~ischar( argName ) ) )
+			error('ERROR (qpOASES_options): Argmument no. %d has to be a non-empty string!',i );
+        end
+			
+		if ( ( ischar(argValue) ) || ( ~isscalar( argValue ) ) )
+			error('ERROR (qpOASES_options): Argmument no. %d has to be a scalar constant!',i+1 );
+        end
+
+		if ( ~isfield( options,argName ) )
+			error('ERROR (qpOASES_options): Argmument no. %d is an invalid option!',i );
+		end
+
+		eval( ['options.',argName,' = ',num2str(argValue),';'] );
+
+	end
+
+end
+
+
+function [ options ] = qpOASES_default_options( )
+
+	% setup options struct with default values
+	options = struct(	'maxIter',                       -1, ...
+						'maxCpuTime',                    -1, ...
+						'printLevel',                     1, ...
+						...
+						'enableRamping',                  1, ...
+						'enableFarBounds',                1, ...
+						'enableFlippingBounds',           1, ...
+						'enableRegularisation',           0, ...
+						'enableFullLITests',              0, ...
+						'enableNZCTests',                 1, ...
+						'enableDriftCorrection',          1, ...
+						'enableCholeskyRefactorisation',  0, ...
+						'enableEqualities',               0, ...
+						...
+						'terminationTolerance',           5.0e6*eps, ...
+						'boundTolerance',                 1.0e6*eps, ...
+						'boundRelaxation',                1.0e4, ...
+						'epsNum',                        -1.0e3*eps, ...
+						'epsDen',                         1.0e3*eps, ...
+						'maxPrimalJump',                  1.0e8, ...
+						'maxDualJump',                    1.0e8, ...
+						...
+    					'initialRamping',                 0.5, ...
+						'finalRamping',                   1.0, ...
+						'initialFarBounds',               1.0e6, ...
+						'growFarBounds',                  1.0e3, ...
+						'initialStatusBounds',            -1, ...
+						'epsFlipping',                    1.0e3*eps, ...
+						'numRegularisationSteps',         0, ...
+						'epsRegularisation',              1.0e3*eps, ...
+						'numRefinementSteps',             1, ...
+						'epsIterRef',                     1.0e2*eps, ...
+						'epsLITests',                     1.0e5*eps, ...
+						'epsNZCTests',                    3.0e3*eps );
+
+end
+
+
+
+function [ options ] = qpOASES_reliable_options( )
+
+	% setup options struct with values for most reliable QP solution
+	options = qpOASES_default_options( );
+
+	options.enableFullLITests             =  1;
+	options.enableCholeskyRefactorisation =  1;
+
+	options.numRefinementSteps            =  2;
+
+end
+
+
+function [ options ] = qpOASES_MPC_options( )
+
+	% setup options struct with values for most reliable QP solution
+	options = qpOASES_default_options( );
+
+	options.enableRamping                 =  0;
+	options.enableFarBounds               =  1;
+	options.enableFlippingBounds          =  0;
+	options.enableRegularisation          =  1;
+	options.enableNZCTests                =  0;
+	options.enableDriftCorrection         =  0;
+	options.enableEqualities              =  1;
+
+	options.terminationTolerance          =  1.0e9*eps;
+	
+	options.initialStatusBounds           =  0;
+	options.numRegularisationSteps        =  1;
+	options.numRefinementSteps            =  0;
+
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_sequence.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_sequence.cpp
new file mode 100644
index 00000000000..943e529e8eb
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_sequence.cpp
@@ -0,0 +1,1061 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file interfaces/matlab/qpOASES_sequence.cpp
+ *	\author Hans Joachim Ferreau, Christian Kirches, Andreas Potschka, Alexander Buchner
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Interface for Matlab(R) that enables to call qpOASES as a MEX function
+ *  (variant for solving QP sequences).
+ *
+ */
+
+
+
+#include <qpOASES.hpp>
+
+
+USING_NAMESPACE_QPOASES
+
+
+#include "qpOASES_matlab_utils.hpp"
+
+/** initialise handle counter of QPInstance class */
+int QPInstance::s_nexthandle = 1;
+
+/** global pointer to QP objects */
+static std::vector<QPInstance *> g_instances;
+
+#include "qpOASES_matlab_utils.cpp"
+
+
+/*
+ *	i n i t S B
+ */
+int initSB(	int handle, 
+			SymmetricMatrix *H, real_t* g,
+			const real_t* const lb, const real_t* const ub,
+			int nWSRin, real_t maxCpuTimeIn,
+			const real_t* const x0, Options* options,
+			int nOutputs, mxArray* plhs[],
+			double* guessedBounds
+			)
+{
+	int nWSRout = nWSRin;
+	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+
+	/* 1) setup initial QP. */
+	QProblemB* globalQPB = getQPInstance(handle)->qpb;
+
+	if ( globalQPB == 0 )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
+		return -1;
+	}
+
+	globalQPB->setOptions( *options );
+	
+	/* 2) Solve initial QP. */
+	returnValue returnvalue;
+	int nV = globalQPB->getNV();
+	
+	/* 3) Fill the working set. */
+	Bounds bounds(nV);
+	if (guessedBounds != 0) {
+		for (int i = 0; i < nV; i++) {
+			if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_LOWER);
+			} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_UPPER);
+			} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_INACTIVE);
+			} else {
+				char msg[MAX_STRING_LENGTH];
+				snprintf(msg, MAX_STRING_LENGTH,
+						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
+				myMexErrMsgTxt(msg);
+				return -1;
+			}
+		}
+	}
+
+	if (x0 == 0 && guessedBounds == 0)
+		returnvalue = globalQPB->init( H,g,lb,ub, nWSRout,&maxCpuTimeOut );
+	else
+		returnvalue = globalQPB->init( H,g,lb,ub, nWSRout,&maxCpuTimeOut, x0, 0,
+				guessedBounds != 0 ? &bounds : 0);
+
+	/* 3) Assign lhs arguments. */
+	obtainOutputs(	0,globalQPB,returnvalue,nWSRout,maxCpuTimeOut,
+					nOutputs,plhs,nV,0,handle );
+
+	return 0;
+}
+
+
+/*
+ *	i n i t
+ */
+int init(	int handle, 
+			SymmetricMatrix *H, real_t* g, Matrix *A,
+			const real_t* const lb, const real_t* const ub, const real_t* const lbA, const real_t* const ubA,
+			int nWSRin, real_t maxCpuTimeIn,
+			const real_t* const x0, Options* options,
+			int nOutputs, mxArray* plhs[],
+			double* guessedBounds, double* guessedConstraints
+			)
+{
+	int nWSRout = nWSRin;
+	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+
+	/* 1) setup initial QP. */
+	SQProblem* globalSQP = getQPInstance(handle)->sqp;
+
+	if ( globalSQP == 0 )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
+		return -1;
+	}
+
+	globalSQP->setOptions( *options );
+	
+	/* 2) Solve initial QP. */
+	returnValue returnvalue;
+	int nV = globalSQP->getNV();
+	int nC = globalSQP->getNC();
+	
+	/* 3) Fill the working set. */
+	Bounds bounds(nV);
+	Constraints constraints(nC);
+	if (guessedBounds != 0) {
+		for (int i = 0; i < nV; i++) {
+			if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_LOWER);
+			} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_UPPER);
+			} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_INACTIVE);
+			} else {
+				char msg[MAX_STRING_LENGTH];
+				snprintf(msg, MAX_STRING_LENGTH,
+						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
+				myMexErrMsgTxt(msg);
+				return -1;
+			}
+		}
+	}
+
+	if (guessedConstraints != 0) {
+		for (int i = 0; i < nC; i++) {
+			if ( isEqual(guessedConstraints[i],-1.0) == BT_TRUE ) {
+				constraints.setupConstraint(i, ST_LOWER);
+			} else if ( isEqual(guessedConstraints[i],1.0) == BT_TRUE ) {
+				constraints.setupConstraint(i, ST_UPPER);
+			} else if ( isEqual(guessedConstraints[i],0.0) == BT_TRUE ) {
+				constraints.setupConstraint(i, ST_INACTIVE);
+			} else {
+				char msg[MAX_STRING_LENGTH];
+				snprintf(msg, MAX_STRING_LENGTH,
+						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of constraints!");
+				myMexErrMsgTxt(msg);
+				return -1;
+			}
+		}
+	}
+
+	if (x0 == 0 && guessedBounds == 0 && guessedConstraints == 0)
+		returnvalue = globalSQP->init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut);
+	else
+		returnvalue = globalSQP->init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut, x0, 0,
+				guessedBounds != 0 ? &bounds : 0,
+				guessedConstraints != 0 ? &constraints : 0);
+
+	/* 3) Assign lhs arguments. */
+	obtainOutputs(	0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
+					nOutputs,plhs,nV,nC,handle );
+
+	return 0;
+}
+
+
+
+/*
+ *	h o t s t a r t S B
+ */
+int hotstartSB(	int handle,
+                const real_t* const g,
+				const real_t* const lb, const real_t* const ub,
+				int nWSRin, real_t maxCpuTimeIn,
+				Options* options,
+				int nOutputs, mxArray* plhs[]
+				)
+{
+	int nWSRout = nWSRin;
+	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+
+	QProblemB* globalQPB = getQPInstance(handle)->qpb;
+
+	if ( globalQPB == 0 )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
+		return -1;
+	}
+
+	/* 1) Solve QP with given options. */
+	globalQPB->setOptions( *options );
+	returnValue returnvalue = globalQPB->hotstart( g,lb,ub, nWSRout,&maxCpuTimeOut );
+
+	/* 2) Assign lhs arguments. */
+	obtainOutputs(	0,globalQPB,returnvalue,nWSRout,maxCpuTimeOut,
+					nOutputs,plhs,0,0 );
+
+	return 0;
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+int hotstart(	int handle,
+                const real_t* const g,
+				const real_t* const lb, const real_t* const ub,
+				const real_t* const lbA, const real_t* const ubA,
+				int nWSRin, real_t maxCpuTimeIn,
+				Options* options,
+				int nOutputs, mxArray* plhs[]
+				)
+{
+	int nWSRout = nWSRin;
+	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+
+	QProblem* globalSQP = getQPInstance(handle)->sqp;
+
+	if ( globalSQP == 0 )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
+		return -1;
+	}
+
+	/* 1) Solve QP with given options. */
+	globalSQP->setOptions( *options );
+	returnValue returnvalue = globalSQP->hotstart( g,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut );
+
+	/* 2) Assign lhs arguments. */
+	obtainOutputs(	0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
+					nOutputs,plhs,0,0 );
+
+	return 0;
+}
+
+
+/*
+ *	h o t s t a r t V M
+ */
+int hotstartVM(	int handle,
+                    SymmetricMatrix *H, real_t* g, Matrix *A,
+					const real_t* const lb, const real_t* const ub, const real_t* const lbA, const real_t* const ubA,
+					int nWSRin, real_t maxCpuTimeIn,
+					Options* options,
+					int nOutputs, mxArray* plhs[]
+					)
+{
+	int nWSRout = nWSRin;
+	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+
+	SQProblem* globalSQP = getQPInstance(handle)->sqp;
+
+	if ( globalSQP == 0 )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
+		return -1;
+	}
+
+	/* 1) Solve QP. */
+	globalSQP->setOptions( *options );
+	returnValue returnvalue = globalSQP->hotstart( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut );
+
+	switch (returnvalue)
+	{
+		case SUCCESSFUL_RETURN:
+		case RET_QP_UNBOUNDED:
+		case RET_QP_INFEASIBLE:
+			break;
+		otherwise:
+			myMexErrMsgTxt( "ERROR (qpOASES): Hotstart failed." );
+			return -1;
+	}
+
+	/* 2) Assign lhs arguments. */
+	obtainOutputs(	0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
+					nOutputs,plhs,0,0 );
+
+	return 0;
+}
+
+
+
+/*
+ *	m e x F u n c t i o n
+ */
+void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
+{
+	/* inputs */
+	char typeString[2];
+	real_t *g=0, *lb=0, *ub=0, *lbA=0, *ubA=0, *x0=0;
+	int H_idx=-1, g_idx=-1, A_idx=-1, lb_idx=-1, ub_idx=-1, lbA_idx=-1, ubA_idx=-1;
+	int x0_idx=-1, auxInput_idx=-1;
+
+	double *guessedBoundsAndConstraints = 0;
+	double *guessedBounds = 0, *guessedConstraints = 0;
+
+	BooleanType isSimplyBoundedQp = BT_FALSE;
+
+	Options options;
+	options.printLevel = PL_LOW;
+	#ifdef __DEBUG__
+	options.printLevel = PL_HIGH;
+	#endif
+	#ifdef __SUPPRESSANYOUTPUT__
+	options.printLevel = PL_NONE;
+	#endif
+
+	/* dimensions */
+	unsigned int nV=0, nC=0, handle=0;
+	int nWSRin;
+	real_t maxCpuTimeIn = -1.0;
+	QPInstance* globalQP = 0;
+
+	/* I) CONSISTENCY CHECKS: */
+	/* 1) Ensure that qpOASES is called with a feasible number of input arguments. */
+	if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
+	{
+		if ( nrhs != 2 )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+	}
+	
+	/* 2) Ensure that first input is a string ... */
+	if ( mxIsChar( prhs[0] ) != 1 )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): First input argument must be a string!" );
+		return;
+	}
+
+	mxGetString( prhs[0], typeString, 2 );
+
+	/*    ... and if so, check if it is an allowed one. */
+	if ( ( strcmp( typeString,"i" ) != 0 ) && ( strcmp( typeString,"I" ) != 0 ) &&
+		 ( strcmp( typeString,"h" ) != 0 ) && ( strcmp( typeString,"H" ) != 0 ) &&
+		 ( strcmp( typeString,"m" ) != 0 ) && ( strcmp( typeString,"M" ) != 0 ) &&
+		 ( strcmp( typeString,"e" ) != 0 ) && ( strcmp( typeString,"E" ) != 0 ) &&
+		 ( strcmp( typeString,"c" ) != 0 ) && ( strcmp( typeString,"C" ) != 0 ) )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): Undefined first input argument!\nType 'help qpOASES_sequence' for further information." );
+		return;
+	}
+
+
+	/* II) SELECT RESPECTIVE QPOASES FUNCTION CALL: */
+	/* 1) Init (without or with initial guess for primal solution). */
+	if ( ( strcmp( typeString,"i" ) == 0 ) || ( strcmp( typeString,"I" ) == 0 ) )
+	{
+		/* consistency checks */
+		if ( ( nlhs < 2 ) || ( nlhs > 7 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		/* ensure that data is given in double precision */
+		if ((mxIsDouble(prhs[1]) == 0) || (mxIsDouble(prhs[2]) == 0)) {
+			myMexErrMsgTxt(
+					"ERROR (qpOASES): All data has to be provided in double precision!");
+			return;
+		}
+
+        nV = (unsigned int)mxGetM( prhs[1] ); /* row number of Hessian matrix */
+
+		/* Check for 'Inf' and 'Nan' in Hessian */
+		H_idx = 1;
+		g_idx = 2;
+		if (containsNaNorInf(prhs, nV * nV, H_idx, 0) == BT_TRUE) {
+			return;
+		}
+
+		/* Check for 'Inf' and 'Nan' in gradient */
+		if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
+			return;
+		}
+
+		/* determine whether is it a simply bounded QP */
+		if ( nrhs <= 7 )
+			isSimplyBoundedQp = BT_TRUE;
+		else
+			isSimplyBoundedQp = BT_FALSE;
+
+		if ( isSimplyBoundedQp == BT_TRUE )
+		{
+			lb_idx = 3;
+			ub_idx = 4;
+
+			if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
+				return;
+			}
+			if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
+				return;
+			}
+
+			/* Check inputs dimensions and assign pointers to inputs. */
+			nC = 0; /* row number of constraint matrix */
+
+			if ( mxGetN( prhs[1] ) != nV )
+			{
+				myMexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );
+				return;
+			}
+
+			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
+				return;
+
+			/* default value for nWSR */
+			nWSRin = 5*nV;
+
+			/* Check whether x0 and options are specified .*/
+			if ( nrhs >= 6 )
+			{
+				if ((!mxIsEmpty(prhs[5])) && (mxIsStruct(prhs[5])))
+					setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );
+
+				if ( ( nrhs >= 7 ) && ( !mxIsEmpty(prhs[6]) ) )
+				{ 
+					/* auxInput specified */
+					if ( mxIsStruct(prhs[6]) )
+					{
+						auxInput_idx = 6;
+						x0_idx = -1;
+					}
+					else
+					{
+						auxInput_idx = -1;
+						x0_idx = 6;
+					}
+				}
+				else
+				{
+					auxInput_idx = -1;
+					x0_idx = -1;
+				}
+			}
+		}
+		else
+		{
+			/* ensure that data is given in double precision */
+			if ( ( mxIsDouble( prhs[1] ) == 0 ) ||
+				 ( mxIsDouble( prhs[2] ) == 0 ) ||
+				 ( mxIsDouble( prhs[3] ) == 0 ) )
+			{
+				myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
+				return;
+			}
+		
+			/* Check inputs dimensions and assign pointers to inputs. */
+			nC = (unsigned int)mxGetM( prhs[3] ); /* row number of constraint matrix */
+
+			A_idx = 3;
+			lb_idx = 4;
+			ub_idx = 5;
+			lbA_idx = 6;
+			ubA_idx = 7;
+
+			if (containsNaNorInf(prhs, nV * nC, A_idx, 0) == BT_TRUE) {
+				return;
+			}
+			if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
+				return;
+			}
+			if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
+				return;
+			}
+			if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
+				return;
+			}
+			if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
+				return;
+			}
+
+			if ( ( mxGetN( prhs[1] ) != nV ) || ( ( mxGetN( prhs[3] ) != 0 ) && ( mxGetN( prhs[3] ) != nV ) ) )
+			{
+				myMexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );
+				return;
+			}
+		
+			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
+				return;
+			
+			if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,7 ) != SUCCESSFUL_RETURN )
+				return;
+
+			/* default value for nWSR */
+			nWSRin = 5*(nV+nC);
+
+			/* Check whether x0 and options are specified .*/
+			if ( nrhs >= 9 )
+			{
+				if ((!mxIsEmpty(prhs[8])) && (mxIsStruct(prhs[8])))
+					setupOptions( &options,prhs[8],nWSRin,maxCpuTimeIn );
+
+				if ( ( nrhs >= 10 ) && ( !mxIsEmpty(prhs[9]) ) )
+				{ 
+					/* auxInput specified */
+					if ( mxIsStruct(prhs[9]) )
+					{
+						auxInput_idx = 9;
+						x0_idx = -1;
+					}
+					else
+					{
+						auxInput_idx = -1;
+						x0_idx = 9;
+					}
+				}
+				else
+				{
+					auxInput_idx = -1;
+					x0_idx = -1;
+				}
+			}
+		}
+
+
+		/* check dimensions and copy auxInputs */
+		if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,x0_idx ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( auxInput_idx >= 0 )
+			setupAuxiliaryInputs( prhs[auxInput_idx],nV,nC, &x0,&guessedBoundsAndConstraints,&guessedBounds,&guessedConstraints );
+
+
+		/* allocate instance */
+		handle = allocateQPInstance( nV,nC,isSimplyBoundedQp, &options );	
+		globalQP = getQPInstance( handle );
+
+		/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
+		setupHessianMatrix(	prhs[1],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );
+
+		/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
+		if ( nC > 0 )
+			setupConstraintMatrix( prhs[3],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );
+
+		/* Create output vectors and assign pointers to them. */
+		allocateOutputs( nlhs,plhs, nV,nC,1,handle );
+
+		/* Call qpOASES. */
+		if ( isSimplyBoundedQp == BT_TRUE )
+		{
+			initSB(	handle,
+					globalQP->H,g,
+					lb,ub,
+					nWSRin,maxCpuTimeIn,
+					x0,&options,
+					nlhs,plhs,
+					guessedBounds
+					);
+
+			deleteAuxiliaryInputs( &guessedBounds,0 );
+		}
+		else
+		{
+			init(	handle,
+					globalQP->H,g,globalQP->A,
+					lb,ub,lbA,ubA,
+					nWSRin,maxCpuTimeIn,
+					x0,&options,
+					nlhs,plhs,
+					guessedBounds, guessedConstraints
+					);
+
+			deleteAuxiliaryInputs( &guessedBounds,&guessedConstraints );
+		}
+
+		return;
+	}
+
+	/* 2) Hotstart. */
+	if ( ( strcmp( typeString,"h" ) == 0 ) || ( strcmp( typeString,"H" ) == 0 ) )
+	{
+		/* consistency checks */
+		if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		if ( ( nrhs < 5 ) || ( nrhs > 8 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		/* determine whether is it a simply bounded QP */
+		if ( nrhs < 7 )
+			isSimplyBoundedQp = BT_TRUE;
+		else
+			isSimplyBoundedQp = BT_FALSE;
+
+
+		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxGetM( prhs[1] ) != 1 ) || ( mxGetN( prhs[1] ) != 1 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		/* get QP instance */
+		handle = (unsigned int)mxGetScalar( prhs[1] );
+		globalQP = getQPInstance ( handle );
+		if ( globalQP == 0 )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
+			return;
+		}
+
+		nV = globalQP->getNV();
+
+		g_idx = 2;
+		lb_idx = 3;
+		ub_idx = 4;
+
+		if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
+			return;
+		}
+
+		if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
+			return;
+		}
+		if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
+			return;
+		}
+
+
+		/* Check inputs dimensions and assign pointers to inputs. */
+		if ( isSimplyBoundedQp == BT_TRUE )
+		{
+			nC = 0;
+
+			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
+				return;
+
+			/* default value for nWSR */
+			nWSRin = 5*nV;
+
+			/* Check whether options are specified .*/
+			if ( nrhs == 6 )
+				if ( ( !mxIsEmpty( prhs[5] ) ) && ( mxIsStruct( prhs[5] ) ) )
+					setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );
+		}
+		else
+		{
+			nC = globalQP->getNC( );
+
+			lbA_idx = 5;
+			ubA_idx = 6;
+
+			if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
+				return;
+			}
+			if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
+				return;
+			}
+
+			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
+				return;
+
+			/* default value for nWSR */
+			nWSRin = 5*(nV+nC);
+
+			/* Check whether options are specified .*/
+			if ( nrhs == 8 )
+				if ( ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
+					setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
+		}
+
+		/* Create output vectors and assign pointers to them. */
+		allocateOutputs( nlhs,plhs, nV,nC );
+
+		/* call qpOASES */
+		if ( isSimplyBoundedQp == BT_TRUE )
+		{
+			hotstartSB(	handle, g,
+						lb,ub,
+						nWSRin,maxCpuTimeIn,
+						&options,
+						nlhs,plhs
+						);
+		}
+		else
+		{
+			hotstart(	handle, g,
+						lb,ub,lbA,ubA,
+						nWSRin,maxCpuTimeIn,
+						&options,
+						nlhs,plhs
+						);
+		}
+
+		return;
+	}
+
+	/* 3) Modify matrices. */
+	if ( ( strcmp( typeString,"m" ) == 0 ) || ( strcmp( typeString,"M" ) == 0 ) )
+	{
+		/* consistency checks */
+		if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		if ( ( nrhs < 9 ) || ( nrhs > 10 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxGetM( prhs[1] ) != 1 ) || ( mxGetN( prhs[1] ) != 1 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		/* ensure that data is given in double precision */
+		if ( ( mxIsDouble( prhs[2] ) == 0 ) ||
+			 ( mxIsDouble( prhs[3] ) == 0 ) ||
+			 ( mxIsDouble( prhs[4] ) == 0 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
+			return;
+		}
+
+		/* get QP instance */
+		handle = (unsigned int)mxGetScalar( prhs[1]);
+		globalQP = getQPInstance ( handle );
+		if ( globalQP == 0 )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
+			return;
+		}
+
+		/* Check inputs dimensions and assign pointers to inputs. */
+		nV = (unsigned int)mxGetM( prhs[2] ); /* row number of Hessian matrix */
+		nC = (unsigned int)mxGetM( prhs[4] ); /* row number of constraint matrix */
+		
+		H_idx = 2;
+		g_idx = 3;
+		A_idx = 4;
+		lb_idx = 5;
+		ub_idx = 6;
+		lbA_idx = 7;
+		ubA_idx = 8;
+
+		/* check if supplied data contains 'NaN' or 'Inf' */
+		if (containsNaNorInf(prhs, nV * nV, H_idx, 0) == BT_TRUE) {
+			return;
+		}
+
+		if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
+			return;
+		}
+
+		if (containsNaNorInf(prhs, nV * nC, A_idx, 0) == BT_TRUE) {
+			return;
+		}
+
+		if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
+			return;
+		}
+		if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
+			return;
+		}
+
+		if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
+			return;
+		}
+		if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
+			return;
+		}
+
+		/* Check that dimensions are consistent with existing QP instance */
+		if (nV != (unsigned int) globalQP->getNV () || nC != (unsigned int) globalQP->getNC ())
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): QP dimensions must be constant during a sequence! Try creating a new QP instance instead." );
+			return;
+		}
+
+		if ( ( mxGetN( prhs[2] ) != nV ) || ( ( mxGetN( prhs[4] ) != 0 ) && ( mxGetN( prhs[4] ) != nV ) ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );
+			return;
+		}
+
+		if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,3 ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,7 ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,8 ) != SUCCESSFUL_RETURN )
+			return;
+
+		/* default value for nWSR */
+		nWSRin = 5*(nV+nC);
+
+		/* Check whether options are specified .*/
+		if ( nrhs > 9 )
+			if ( ( !mxIsEmpty( prhs[9] ) ) && ( mxIsStruct( prhs[9] ) ) )
+				setupOptions( &options,prhs[9],nWSRin,maxCpuTimeIn );
+
+		globalQP->deleteQPMatrices( );
+
+		/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
+		setupHessianMatrix(	prhs[2],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );
+
+		/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
+		if ( nC > 0 )
+			setupConstraintMatrix( prhs[4],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );
+
+		/* Create output vectors and assign pointers to them. */
+		allocateOutputs( nlhs,plhs, nV,nC );
+
+		/* Call qpOASES */
+		hotstartVM(	handle, globalQP->H,g,globalQP->A,
+					lb,ub,lbA,ubA,
+					nWSRin,maxCpuTimeIn,
+					&options,
+					nlhs,plhs
+					);
+
+		return;
+	}
+
+	/* 4) Solve current equality constrained QP. */
+	if ( ( strcmp( typeString,"e" ) == 0 ) || ( strcmp( typeString,"E" ) == 0 ) )
+	{
+		/* consistency checks */
+		if ( ( nlhs < 1 ) || ( nlhs > 3 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		if ( ( nrhs < 7 ) || ( nrhs > 8 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxGetM( prhs[1] ) != 1 ) || ( mxGetN( prhs[1] ) != 1 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		/* get QP instance */
+		handle = (unsigned int)mxGetScalar( prhs[1] );
+		globalQP = getQPInstance ( handle );
+		if ( globalQP == 0 )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
+			return;
+		}
+
+		/* Check inputs dimensions and assign pointers to inputs. */
+		int nRHS = (int)mxGetN(prhs[2]);
+		nV = globalQP->getNV( );
+		nC = globalQP->getNC( );
+		real_t *x_out, *y_out;
+
+		g_idx = 2;
+		lb_idx = 3;
+		ub_idx = 4;
+		lbA_idx = 5;
+		ubA_idx = 6;
+
+		/* check if supplied data contains 'NaN' or 'Inf' */
+		if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
+			return;
+		}
+		if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
+			return;
+		}
+		if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
+			return;
+		}
+
+		if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
+			return;
+		}
+		if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
+			return;
+		}
+
+		if ( smartDimensionCheck( &g,nV,nRHS, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &lb,nV,nRHS, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &ub,nV,nRHS, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &lbA,nC,nRHS, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &ubA,nC,nRHS, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
+			return;
+
+		/* Check whether options are specified .*/
+		if ( ( nrhs == 8 ) && ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
+		{
+			nWSRin = 5*(nV+nC);
+			setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
+			globalQP->sqp->setOptions( options );
+		}
+
+		/* Create output vectors and assign pointers to them. */
+		plhs[0] = mxCreateDoubleMatrix( nV, nRHS, mxREAL );
+		x_out = mxGetPr(plhs[0]);
+		if (nlhs >= 2)
+		{
+			plhs[1] = mxCreateDoubleMatrix( nV+nC, nRHS, mxREAL );
+			y_out = mxGetPr(plhs[1]);
+
+			if (nlhs >= 3) {
+				plhs[2] = mxCreateDoubleMatrix( nV+nC, nRHS, mxREAL );
+				double* workingSet = mxGetPr(plhs[2]);
+
+				globalQP->sqp->getWorkingSet(workingSet);
+			}
+		}
+		else
+			y_out = new real_t[nV+nC];
+
+		/* Solve equality constrained QP */
+		returnValue returnvalue = globalQP->sqp->solveCurrentEQP( nRHS,g,lb,ub,lbA,ubA, x_out,y_out );
+
+		if (nlhs < 2)
+			delete[] y_out;
+
+		if (returnvalue != SUCCESSFUL_RETURN)
+		{
+			char msg[MAX_STRING_LENGTH];
+			snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Couldn't solve current EQP (code %d)!", returnvalue);
+			myMexErrMsgTxt(msg);
+			return;
+		}
+
+		return;
+	}
+
+	/* 5) Cleanup. */
+	if ( ( strcmp( typeString,"c" ) == 0 ) || ( strcmp( typeString,"C" ) == 0 ) )
+	{		
+		/* consistency checks */
+		if ( nlhs != 0 )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		if ( nrhs != 2 )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxGetM( prhs[1] ) != 1 ) || ( mxGetN( prhs[1] ) != 1 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		/* Cleanup SQProblem instance. */
+		handle =(unsigned int)mxGetScalar( prhs[1] );
+		deleteQPInstance ( handle );
+		
+		return;
+	}
+
+}
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_sequence.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_sequence.m
new file mode 100644
index 00000000000..e5d97b6229e
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_sequence.m
@@ -0,0 +1,112 @@
+%qpOASES -- An Implementation of the Online Active Set Strategy.
+%Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%Christian Kirches et al. All rights reserved.
+%
+%qpOASES is distributed under the terms of the
+%GNU Lesser General Public License 2.1 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.
+%
+%---------------------------------------------------------------------------------
+%
+%qpOASES_sequence is intended to solve a sequence of quadratic
+%programming (QP) problems of the following form:
+%
+%                min   1/2*x'Hx + x'g
+%                s.t.  lb  <=  x <= ub
+%                      lbA <= Ax <= ubA  {optional}
+%
+%I) Call
+%
+%    [QP,x,fval,exitflag,iter,lambda,auxOutput] = ...
+%               qpOASES_sequence( 'i',H,g,A,lb,ub,lbA,ubA{,options{,auxInput}} )
+%or 
+%    [QP,x,fval,exitflag,iter,lambda,auxOutput] = ...
+%               qpOASES_sequence( 'i',H,g,lb,ub{,options{,auxInput}} )
+%
+%for initialising and solving the first above-mentioned QP of the sequence
+%starting from an initial guess x0. H must be a symmetric (possibly indefinite) 
+%matrix and all vectors g, lb, ub, lbA, ubA have to be given as column vectors. 
+%Options can be generated using the qpOASES_options command, otherwise default
+%values are used. Optionally, further auxiliary inputs may be generated 
+%using qpOASES_auxInput command and passed to the solver.
+%Both matrices H or A may be passed in sparse matrix format.
+%
+%II) Call
+%
+%     [x,fval,exitflag,iter,lambda,auxOutput] = ...
+%                      qpOASES_sequence( 'h',QP,g,lb,ub,lbA,ubA{,options} )
+%or
+%     [x,fval,exitflag,iter,lambda,auxOutput] = ...
+%                      qpOASES_sequence( 'h',QP,g,lb,ub{,options} )
+%
+%for hotstarting from the previous QP solution to the one of the next QP
+%given by the vectors g, lb, ub, lbA, ubA. Options can be generated using the 
+%qpOASES_options command, otherwise default values are used.
+%
+%III) Call
+%
+%     [x,fval,exitflag,iter,lambda,auxOutput] = ...
+%                     qpOASES_sequence( 'm',QP,H,g,A,lb,ub,lbA,ubA{,options} )
+%
+%for hotstarting from the previous QP solution to the one of the next QP
+%given by the matrices H, A and the vectors g, lb, ub, lbA, ubA. The previous
+%active set serves as a starting guess. If the new projected Hessian matrix
+%turns out to be not positive definite, qpOASES recedes to a safe initial active
+%set guess automatically. This can result in a high number of iterations iter.
+%Options can be generated using the qpOASES_options command, otherwise default
+%values are used.
+%
+%IV) Call
+%
+%     [x,lambda,workingSet] = qpOASES_sequence( 'e',QP,g,lb,ub,lbA,ubA{,options} )
+%
+%for solving the equality constrained QP with constraints determined by the
+%current active set. All inequalities and bounds which were not active in the
+%previous solution might be violated. This command does not alter the internal
+%state of qpOASES. Instead of calling this command multiple times, it is
+%possible to supply several columns simultaneously in g, lb, ub, lbA, and ubA.
+%Options can be generated using the qpOASES_options command, otherwise default
+%values are used.
+%
+%V) Having solved the last QP of your sequence, call
+%
+%     qpOASES_sequence( 'c',QP )
+%
+%in order to cleanup the internal memory.
+%
+%
+%Optional outputs (only x is mandatory):
+%    x          -  Optimal primal solution vector (if exitflag==0).
+%    fval       -  Optimal objective function value (if exitflag==0).
+%    exitflag   -   0: QP solved,
+%                   1: QP could not be solved within given number of iterations,
+%                  -1: QP could not be solved due to an internal error,
+%                  -2: QP is infeasible (and thus could not be solved),
+%                  -3: QP is unbounded (and thus could not be solved).
+%    iter       -  Number of active set iterations actually performed.
+%    lambda     -  Optimal dual solution vector (if status==0).
+%    auxOutput  -  Struct containing auxiliary outputs as described below.
+%
+%The auxOutput struct contains the following entries:
+%    workingSet -  The working set at point x. The working set is a subset
+%                  of the active set (which is the set of all indices
+%                  corresponding to bounds/constraints that hold with 
+%                  equality). The working set corresponds to bound/
+%                  constraint row vectors forming a linear independent set.
+%                  The first nV elements correspond to the bounds, the last
+%                  nC elements to the constraints.
+%                  The working set is encoded as follows:
+%                   1: bound/constraint at its upper bound
+%                   0: bound/constraint not at any bound
+%                  -1: bound/constraint at its lower bound
+%    cpuTime    -  Internally measured CPU time for solving QP.
+%
+%See also QPOASES_OPTIONS, QPOASES_AUXINPUT, QPOASES
+%
+%
+%For additional information see the qpOASES User's Manual or
+%visit http://www.qpOASES.org/.
+%
+%Please send remarks and questions to support@qpOASES.org!
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/clean b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/clean
new file mode 100644
index 00000000000..2957397008a
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/clean
@@ -0,0 +1,3 @@
+rm *.*~
+rm *.o
+rm qpOASES qpOASES_sequence
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/clean.sh b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/clean.sh
new file mode 100644
index 00000000000..2957397008a
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/clean.sh
@@ -0,0 +1,3 @@
+rm *.*~
+rm *.o
+rm qpOASES qpOASES_sequence
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1.mat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1.mat
new file mode 100644
index 0000000000000000000000000000000000000000..c0bd0d2ed4364ebaf2b16eeb1791b99510164d60
GIT binary patch
literal 790
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i?A@$QE)CwO)N=GQOM7;
zQV7W?Rq#(PQBW{ZFtoHXwy-iZQZO<wFjpWIFfe-h@-r|n=m2rWoX5!t2^<VbW(v<5
zE-4(~dTQ)ws4P&$km;uomeA11|BWZ&AagZCObxRuNRJv^j{&kCCPU#FX(<IPn`cT$
zG&V7awlb^%sn&(7E<jezXs9e<Y_x<KXonck4*72aznX0sEPR-wL7MdNXfk}FVrWpn
zbd!n4;KKpAQx*&i4u6?#K)Tf7x=<Vjw#smdTH_BNo;41R3^D<X9(eq5$k5n1bCNQ%
z@Pwl*Pna3xb}%;MF&w1YZ<6wal%ya3=QlDksCqM3;5D4vIj*8W0BATX&~U}|jQ2pQ
z4dE{TfEGw1%||Xh{v33=(L`N<+u*<f_G*s5hvgUwC0GSO8WEwp0Yl@HqDhZ{Iwd7$
zNJcd5`Tw8MMu6oVNRu%h+n#t$hG?>A%wc4c5GZW8!2gYzq2v=QA4sbvTq}xwN{M9(
rdcH+H=e>2)6PUUhjxboKF`MIYAV|YGPpF2pgrpzeL4oIZl~o4-!Xx*U

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1a.mat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1a.mat
new file mode 100644
index 0000000000000000000000000000000000000000..48c784fcb7c53eedc3b3124e3577f8d0822e1cb2
GIT binary patch
literal 896
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i?A@$QE)CwO)N=GQOM7;
zQV7W?Rq#(PQBW{ZFtoHXwzM)dRWLFzFjpWIFfe-h@-r|n=m2rWoX5!t2^<VbW(v<5
zE-4(~dTQ)ws4P&$km;uomeA11|BWZ&AagZCObxRuNRJv^j{&kCCPU#FX(<IPn`cT$
zG&V7awlb^%sn&(7E<jezXs9e<Y_x<KXonck4*72aznX0sEPR-wL7MdNXfk}FVrWpn
zbd!n4;KKpAQx*&i4u6?#K)Tf7x=<Vjw#smdTH_BNo;41R3^D<X9(eq5$k5n1bCNQ%
z@Pwl*Pna3xb}%;MF&w1YZ<6wal%ya3=QlDksCqM3;5D4vIj*8W0BATX&~U}|jQ2pQ
z4dE{TfEGw1%||Xh{v33=(L`N<+u*<f_G*s5hvgUwC0GSO8WEwp0Yl@HqDhZ{Iwd7$
zNJcd5`Tw8MMu6oVNRu%h+n#t$hG?>A%wc4c5GZW8!2gYzq2v=QA4sbvTq}xwN{M9(
zdcH+H=e>2)6PUUhjxboKF`MIYAV|YGPpF2pgrpzeL4oIZl~o6%$pCIk2ePk#sWRk@
zvYq}`8INM7jZDluH5=M5@Ec?>oIk~G0n(=h*N5UdV9K1r)@ga{`O<04M`r+2CliB_
HDN`x{YE&BX

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1b.mat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1b.mat
new file mode 100644
index 0000000000000000000000000000000000000000..b45b4f74c6a935c350dac6de0673396f20801338
GIT binary patch
literal 549
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i?A@$QE)CwO)N=GQOM7;
zQV7W?Rq#(PQBW{ZFtoHXwy-iXRWLFzFjpWIFfe-h@-r|n=m2rWoX5!t2^<VbW(v<5
zE-4(~dTQ)ws4P&$km;uomeA11|BWZ&AagZCObxRuNRKXDPXV$XMnh!@W1}U^KwV-$
zUGm=qel^=NSoknUgEZ;k(Pa2U#n7OD=_V78!G{BKrz{v49R4!ffOM(Dbs1n-WjIBx
z@rMu38V5%PnE*x)ka9Kb$`2WGJI7TN2nbI&%KC(vL2*6fJ&<ZcxatpRhKn>Gx%BvR
z(CJ1KbpdXJ0|(fvIsP7&V<?nh6#!{O1i%IijZcauJp$^Kl$aqI(Xi+Le?}VtmUkdc
f#&~Rd;x!qf$)YibkxfFNu;Bv#H)e*CPpo_Z46>$n

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/index.html
new file mode 100644
index 00000000000..6383f5f1115
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/index.html
@@ -0,0 +1,21 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces/octave</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/interfaces/octave</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="clean">clean</a></li>
+  <li><a href="clean.sh">clean.sh</a></li>
+  <li><a href="example1.mat">example1.mat</a></li>
+  <li><a href="example1a.mat">example1a.mat</a></li>
+  <li><a href="example1b.mat">example1b.mat</a></li>
+  <li><a href="make.m">make.m</a></li>
+  <li><a href="qpOASES.cpp">qpOASES.cpp</a></li>
+  <li><a href="qpOASES.m">qpOASES.m</a></li>
+  <li><a href="qpOASES_auxInput.m">qpOASES_auxInput.m</a></li>
+  <li><a href="qpOASES_octave_utils.cpp">qpOASES_octave_utils.cpp</a></li>
+  <li><a href="qpOASES_octave_utils.hpp">qpOASES_octave_utils.hpp</a></li>
+  <li><a href="qpOASES_options.m">qpOASES_options.m</a></li>
+  <li><a href="qpOASES_sequence.cpp">qpOASES_sequence.cpp</a></li>
+  <li><a href="qpOASES_sequence.m">qpOASES_sequence.m</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/make.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/make.m
new file mode 100644
index 00000000000..28cef5fc907
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/make.m
@@ -0,0 +1,238 @@
+function [] = make( varargin )
+%MAKE Compiles the octave interface of qpOASES.
+%
+%Type  make            to compile all interfaces that 
+%                      have been modified,
+%type  make clean      to delete all compiled interfaces,
+%type  make clean all  to first delete and then compile 
+%                      all interfaces,
+%type  make 'name'     to compile only the interface with
+%                      the given name (if it has been modified),
+%type  make 'opt'      to compile all interfaces using the 
+%                      given compiler options.
+%
+%Copyright (C) 2013-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%Christian Kirches et al. All rights reserved.
+
+%%
+%%	This file is part of qpOASES.
+%%
+%%	qpOASES -- An Implementation of the Online Active Set Strategy.
+%%	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%%	Christian Kirches et al. All rights reserved.
+%%
+%%	qpOASES 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.
+%%
+%%	qpOASES 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 qpOASES; if not, write to the Free Software
+%%	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+%%
+
+%%
+%%	Filename:  interfaces/octave/make.m
+%%	Author:    Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+%%	Version:   3.0
+%%	Date:      2007-2014
+%%
+
+       
+    %% consistency check
+    if ( exist( [pwd, '/make.m'],'file' ) == 0 )
+        error( ['ERROR (',mfilename '.m): Run this make script directly within the directory', ...
+                '<qpOASES-inst-dir>/interfaces/octave, please.'] );
+    end
+
+
+    if ( nargin > 2 )
+        error( ['ERROR (',mfilename '.m): At most two make arguments supported!'] );
+    else
+        [ doClean,fcnNames,userFlags ] = analyseMakeArguments( nargin,varargin );
+    end
+
+    
+    %% define compiler settings
+    QPOASESPATH = '../../';
+    
+    DEBUGFLAGS = ' ';
+    %DEBUGFLAGS = ' -g CXXDEBUGFLAGS=''$CXXDEBUGFLAGS -Wall -pedantic -Wshadow'' ';
+
+    IFLAGS = [ '-I. -I',QPOASESPATH,'include',' -I',QPOASESPATH,'src',' ' ];
+    CPPFLAGS = [ IFLAGS, DEBUGFLAGS, '-D__cpluplus -D__MATLAB__ -D__SINGLE_OBJECT__',' ' ];  %%removed: -largeArrayDims
+    defaultFlags = '-D__NO_COPYRIGHT__ '; %% -D__NO_COPYRIGHT__ -D__SUPPRESSANYOUTPUT__                        %%removed: -O
+
+    if ( ispc == 0 )
+        CPPFLAGS  = [ CPPFLAGS, '-DLINUX ',' ' ]; 
+    else
+        CPPFLAGS  = [ CPPFLAGS, '-DWIN32 ',' ' ];
+    end
+
+    if ( isempty(userFlags) > 0 )
+        CPPFLAGS = [ CPPFLAGS, defaultFlags,' ' ];
+    else
+        CPPFLAGS = [ CPPFLAGS, userFlags,' ' ];
+    end
+
+    mexExt = mexext();
+    
+    
+    %% ensure copyright notice is displayed
+    if ~isempty( strfind( CPPFLAGS,'-D__NO_COPYRIGHT__' ) )
+        printCopyrightNotice( );
+    end
+    
+    
+    %% clean if desired
+    if ( doClean > 0 )
+        
+        eval( 'delete *.o;' );
+        eval( ['delete *.',mexExt,'*;'] );
+        disp( [ 'INFO (',mfilename '.m): Cleaned all compiled files.'] );
+        pause( 0.2 );
+        
+    end
+    
+    
+    if ( ~isempty(userFlags) )
+        disp( [ 'INFO (',mfilename '.m): Compiling all files with user-defined compiler flags (''',userFlags,''')...'] );
+    end
+    
+
+    %% call mex compiler
+    for ii=1:length(fcnNames)
+        
+        cmd = [ 'mkoctfile --mex --output ', fcnNames{ii}, '.', mexext(), ' ', CPPFLAGS, [fcnNames{ii},'.cpp'] ];
+        
+        if ( exist( [fcnNames{ii},'.',mexExt],'file' ) == 0 )
+            
+            eval( cmd );
+            disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' successfully created.'] );
+            
+        else
+            
+            % check modification time of source/Make files and compiled mex file
+            cppFile = dir( [pwd,'/',fcnNames{ii},'.cpp'] );
+            cppFileTimestamp = getTimestamp( cppFile.date ); 
+            
+            utilsFile = dir( [pwd,'/qpOASES_octave_utils.cpp'] );
+            utilsFileTimestamp = getTimestamp( utilsFile.date );
+            
+            makeFile = dir( [pwd,'/make.m'] );
+            makeFileTimestamp = getTimestamp( makeFile.date );
+            
+            mexFile = dir( [pwd,'/',fcnNames{ii},'.',mexExt] );
+            if ( isempty(mexFile) == 0 )
+                mexFileTimestamp = getTimestamp( mexFile.date );
+            else
+                mexFileTimestamp = 0;
+            end
+            
+            if ( ( cppFileTimestamp   >= mexFileTimestamp ) || ...
+                 ( utilsFileTimestamp >= mexFileTimestamp ) || ...
+                 ( makeFileTimestamp  >= mexFileTimestamp ) )
+                eval( cmd );
+                disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' successfully created.'] );
+            else            
+                disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' already exists.'] );
+            end
+            
+        end
+        
+    end
+
+    %% add qpOASES directory to path
+    path( path,pwd );
+
+end
+
+
+function [ doClean,fcnNames,userIFlags ] = analyseMakeArguments( nArgs,args )
+
+    doClean = 0;
+    fcnNames = [];
+    userIFlags = [];
+
+    switch ( nArgs )
+        
+        case 1
+            if ( strcmp( args{1},'all' ) > 0 )
+                fcnNames = { 'qpOASES','qpOASES_sequence' };
+            elseif ( strcmp( args{1},'qpOASES' ) > 0 )
+                fcnNames = { 'qpOASES' };
+            elseif ( strcmp( args{1},'qpOASES_sequence' ) > 0 )
+                fcnNames = { 'qpOASES_sequence' };
+            elseif ( strcmp( args{1},'clean' ) > 0 )
+                doClean = 1;
+            elseif ( strcmp( args{1}(1),'-' ) > 0 )
+                % make clean all with user-specified compiler flags
+                userIFlags = args{1};
+                doClean = 1;
+                fcnNames = { 'qpOASES','qpOASES_sequence' };
+            else
+                error( ['ERROR (',mfilename '.m): Invalid first argument (''',args{1},''')!'] );
+            end
+
+        case 2
+            if ( strcmp( args{1},'clean' ) > 0 )
+                doClean = 1;
+            else
+                error( ['ERROR (',mfilename '.m): First argument must be ''clean'' if two arguments are provided!'] );
+            end
+            
+            if ( strcmp( args{2},'all' ) > 0 )
+                fcnNames = { 'qpOASES','qpOASES_sequence' };
+            elseif ( strcmp( args{2},'qpOASES' ) > 0 )
+                fcnNames = { 'qpOASES' };
+            elseif ( strcmp( args{2},'qpOASES_sequence' ) > 0 )
+                fcnNames = { 'qpOASES_sequence' };
+            else
+                error( ['ERROR (',mfilename '.m): Invalid second argument (''',args{2},''')!'] );
+            end
+            
+        otherwise
+            fcnNames = { 'qpOASES','qpOASES_sequence' };
+            
+    end
+    
+end
+
+
+function [ timestamp ] = getTimestamp( dateString )
+
+    try 
+        timestamp = datenum( dateString );
+    catch
+        timestamp = Inf;
+    end
+
+end
+
+
+function [ ] = printCopyrightNotice( )
+
+    disp( ' ' );
+    disp( 'qpOASES -- An Implementation of the Online Active Set Strategy.' );
+    disp( 'Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,' );
+    disp( 'Christian Kirches et al. All rights reserved.' );
+    disp( ' ' );
+    disp( 'qpOASES is distributed under the terms of the' );
+    disp( 'GNU Lesser General Public License 2.1 in the hope that it will be' );
+    disp( 'useful, but WITHOUT ANY WARRANTY; without even the implied warranty' );
+    disp( 'of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.' );
+    disp( 'See the GNU Lesser General Public License for more details.' );
+    disp( ' ' );
+    disp( ' ' );
+
+end
+
+
+%%
+%%	end of file
+%%
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES.cpp
new file mode 100644
index 00000000000..32aaf7ed9f7
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES.cpp
@@ -0,0 +1,557 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file interfaces/octave/qpOASES.cpp
+ *	\author Hans Joachim Ferreau, Alexander Buchner, Andreas Potschka
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Interface for Matlab(R) that enables to call qpOASES as a MEX function.
+ *
+ */
+
+
+#include <qpOASES.hpp>
+
+
+USING_NAMESPACE_QPOASES
+
+
+#include "qpOASES_octave_utils.hpp"
+
+/** initialise handle counter of QPInstance class */
+int QPInstance::s_nexthandle = 1;
+
+/** global pointer to QP objects */
+static std::vector<QPInstance *> g_instances;
+
+#include "qpOASES_octave_utils.cpp"
+
+
+/*
+ *	q p O A S E S m e x _ c o n s t r a i n t s
+ */
+int qpOASESmex_constraints(		int nV, int nC, int nP,
+								SymmetricMatrix *H, real_t* g, Matrix *A,
+								real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
+								int nWSRin, real_t maxCpuTimeIn,
+								real_t* x0, Options* options,
+								int nOutputs, mxArray* plhs[],
+								double* guessedBounds, double* guessedConstraints
+								)
+{
+	int nWSRout;
+	real_t maxCpuTimeOut;
+	
+	/* 1) Setup initial QP. */
+	QProblem QP( nV,nC );
+	QP.setOptions( *options );
+
+	/* 2) Solve initial QP. */
+	returnValue returnvalue;
+
+	Bounds bounds(nV);
+	Constraints constraints(nC);
+	if (guessedBounds != 0) {
+		for (int i = 0; i < nV; i++) {
+			if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_LOWER);
+			} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_UPPER);
+			} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_INACTIVE);
+			} else {
+				char msg[MAX_STRING_LENGTH];
+				snprintf(msg, MAX_STRING_LENGTH,
+						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
+				myMexErrMsgTxt(msg);
+				return -1;
+			}
+		}
+	}
+
+	if (guessedConstraints != 0) {
+		for (int i = 0; i < nC; i++) {
+			if ( isEqual(guessedConstraints[i],-1.0) == BT_TRUE ) {
+				constraints.setupConstraint(i, ST_LOWER);
+			} else if ( isEqual(guessedConstraints[i],1.0) == BT_TRUE ) {
+				constraints.setupConstraint(i, ST_UPPER);
+			} else if ( isEqual(guessedConstraints[i],0.0) == BT_TRUE ) {
+				constraints.setupConstraint(i, ST_INACTIVE);
+			} else {
+				char msg[MAX_STRING_LENGTH];
+				snprintf(msg, MAX_STRING_LENGTH,
+						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of constraints!");
+				myMexErrMsgTxt(msg);
+				return -1;
+			}
+		}
+	}
+
+	nWSRout = nWSRin;
+	maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+	if (x0 == 0 && guessedBounds == 0 && guessedConstraints == 0)
+		returnvalue = QP.init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut);
+	else
+		returnvalue = QP.init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut, x0, 0,
+				guessedBounds != 0 ? &bounds : 0,
+				guessedConstraints != 0 ? &constraints : 0);
+
+	/* 3) Solve remaining QPs and assign lhs arguments. */
+	/*    Set up pointers to the current QP vectors */
+	real_t* g_current   = g;
+	real_t* lb_current  = lb;
+	real_t* ub_current  = ub;
+	real_t* lbA_current = lbA;
+	real_t* ubA_current = ubA;
+
+	/* Loop through QP sequence. */
+	for ( int k=0; k<nP; ++k )
+	{
+		if ( k > 0 )
+		{
+			/* update pointers to the current QP vectors */
+			g_current = &(g[k*nV]);
+			if ( lb != 0 )
+				lb_current = &(lb[k*nV]);
+			if ( ub != 0 )
+				ub_current = &(ub[k*nV]);
+			if ( lbA != 0 )
+				lbA_current = &(lbA[k*nC]);
+			if ( ubA != 0 )
+				ubA_current = &(ubA[k*nC]);
+
+			nWSRout = nWSRin;
+			maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+			returnvalue = QP.hotstart( g_current,lb_current,ub_current,lbA_current,ubA_current, nWSRout,&maxCpuTimeOut );
+		}
+
+		/* write results into output vectors */
+		obtainOutputs(	k,&QP,returnvalue,nWSRout,maxCpuTimeOut,
+						nOutputs,plhs,nV,nC );
+	}
+
+	//QP.writeQpDataIntoMatFile( "qpDataMat0.mat" );
+
+	return 0;
+}
+
+
+
+/*
+ *	q p O A S E S m e x _ b o u n d s
+ */
+int qpOASESmex_bounds(	int nV, int nP,
+						SymmetricMatrix *H, real_t* g,
+						real_t* lb, real_t* ub,
+						int nWSRin, real_t maxCpuTimeIn,
+						real_t* x0, Options* options,
+						int nOutputs, mxArray* plhs[],
+						double* guessedBounds
+						)
+{
+	int nWSRout;
+	real_t maxCpuTimeOut;
+
+	/* 1) Setup initial QP. */
+	QProblemB QP( nV );
+	QP.setOptions( *options );
+
+	/* 2) Solve initial QP. */
+	returnValue returnvalue;
+
+	Bounds bounds(nV);
+	if (guessedBounds != 0) {
+		for (int i = 0; i < nV; i++) {
+			if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_LOWER);
+			} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_UPPER);
+			} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_INACTIVE);
+			} else {
+				char msg[MAX_STRING_LENGTH];
+				snprintf(msg, MAX_STRING_LENGTH,
+						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
+				myMexErrMsgTxt(msg);
+				return -1;
+			}
+		}
+	}
+
+	nWSRout = nWSRin;
+	maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+	if (x0 == 0 && guessedBounds == 0)
+		returnvalue = QP.init( H,g,lb,ub, nWSRout,&maxCpuTimeOut );
+	else
+		returnvalue = QP.init( H,g,lb,ub, nWSRout,&maxCpuTimeOut, x0, 0,
+				guessedBounds != 0 ? &bounds : 0);
+
+	/* 3) Solve remaining QPs and assign lhs arguments. */
+	/*    Set up pointers to the current QP vectors */
+	real_t* g_current  = g;
+	real_t* lb_current = lb;
+	real_t* ub_current = ub;
+
+	/* Loop through QP sequence. */
+	for ( int k=0; k<nP; ++k )
+	{
+		if ( k > 0 )
+		{
+			/* update pointers to the current QP vectors */
+			g_current = &(g[k*nV]);
+			if ( lb != 0 )
+				lb_current = &(lb[k*nV]);
+			if ( ub != 0 )
+				ub_current = &(ub[k*nV]);
+
+            nWSRout = nWSRin;
+			maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+			returnvalue = QP.hotstart( g_current,lb_current,ub_current, nWSRout,&maxCpuTimeOut );
+		}
+
+		/* write results into output vectors */
+		obtainOutputs(	k,&QP,returnvalue,nWSRout,maxCpuTimeOut,
+						nOutputs,plhs,nV );
+	}
+
+	return 0;
+}
+
+
+
+/*
+ *	m e x F u n c t i o n
+ */
+void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
+{
+	/* inputs */
+	SymmetricMatrix *H=0;
+	Matrix *A=0;
+	real_t *g=0, *lb=0, *ub=0, *lbA=0, *ubA=0, *x0=0;
+	int H_idx, g_idx, A_idx, lb_idx, ub_idx, lbA_idx, ubA_idx, options_idx=-1, x0_idx=-1, auxInput_idx=-1;
+
+	double *guessedBoundsAndConstraints = 0;
+	double *guessedBounds = 0, *guessedConstraints = 0;
+
+    /* Setup default options */
+	Options options;
+	options.printLevel = PL_LOW;
+	#ifdef __DEBUG__
+	options.printLevel = PL_HIGH;
+	#endif
+	#ifdef __SUPPRESSANYOUTPUT__
+	options.printLevel = PL_NONE;
+	#endif
+
+	/* dimensions */
+	unsigned int nV=0, nC=0, nP=0;
+	BooleanType isSimplyBoundedQp = BT_FALSE;
+
+	/* sparse matrix indices and values */
+	sparse_int_t *Hir = 0, *Hjc = 0, *Air = 0, *Ajc = 0;
+	real_t *Hv = 0, *Av = 0;
+
+	/* I) CONSISTENCY CHECKS: */
+	/* 1a) Ensure that qpOASES is called with a feasible number of input arguments. */
+	if ( ( nrhs < 4 ) || ( nrhs > 9 ) )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES' for further information." );
+		return;
+	}
+    
+	/* 2) Check for proper number of output arguments. */
+	if ( nlhs > 6 )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): At most six output arguments are allowed: \n    [x,fval,exitflag,iter,lambda,workingSet]!" );
+		return;
+	}
+	if ( nlhs < 1 )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): At least one output argument is required: [x,...]!" );
+		return;
+	}
+
+
+	/* II) PREPARE RESPECTIVE QPOASES FUNCTION CALL: */
+	/*     Choose between QProblem and QProblemB object and assign the corresponding
+	 *     indices of the input pointer array in to order to access QP data correctly. */
+	H_idx = 0;
+	g_idx = 1;
+	nV = (int)mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
+	nP = (int)mxGetN( prhs[ g_idx ] ); /* number of columns of the gradient matrix (vectors series have to be stored columnwise!) */
+
+	if ( nrhs <= 6 )
+        isSimplyBoundedQp = BT_TRUE;
+	else
+		isSimplyBoundedQp = BT_FALSE;
+
+
+	/* 0) Check whether options are specified .*/
+	if ( isSimplyBoundedQp == BT_TRUE )
+	{
+		if ( ( nrhs >= 5 ) && ( !mxIsEmpty(prhs[4]) ) && ( mxIsStruct(prhs[4]) ) )
+			options_idx = 4;
+	}
+	else
+	{
+		/* Consistency check */
+		if ( ( !mxIsEmpty(prhs[4]) ) && ( mxIsStruct(prhs[4]) ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Fifth input argument must not be a struct when solving QP with general constraints!\nType 'help qpOASES' for further information." );
+			return;
+		}
+
+		if ( ( nrhs >= 8 ) && ( !mxIsEmpty(prhs[7]) ) && ( mxIsStruct(prhs[7]) ) )
+			options_idx = 7;
+	}
+
+	// Is the third argument constraint Matrix A?
+	int numberOfColumns = (int)mxGetN(prhs[2]);
+
+	/* 1) Simply bounded QP. */
+	if ( ( isSimplyBoundedQp == BT_TRUE ) ||
+		 ( ( numberOfColumns == 1 ) && ( nV != 1 ) ) )
+	{
+		lb_idx   = 2;
+		ub_idx   = 3;
+
+		if ( ( nrhs >= 6 ) && ( !mxIsEmpty(prhs[5]) ) )
+		{ 
+			/* auxInput specified */
+			if ( mxIsStruct(prhs[5]) )
+			{
+				auxInput_idx = 5;
+				x0_idx = -1;
+			}
+			else
+			{
+				auxInput_idx = -1;
+				x0_idx = 5;
+			}
+		}
+		else
+		{
+			auxInput_idx = -1;
+			x0_idx = -1;
+		}
+	}
+	else
+	{
+		A_idx = 2;
+
+		/* If constraint matrix is empty, use a QProblemB object! */
+		if ( mxIsEmpty( prhs[ A_idx ] ) )
+		{
+			lb_idx   = 3;
+			ub_idx   = 4;
+
+			nC = 0;
+		}
+		else
+		{
+			lb_idx   = 3;
+			ub_idx   = 4;
+			lbA_idx  = 5;
+			ubA_idx  = 6;
+
+			nC = (int)mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */
+		}
+
+		if ( ( nrhs >= 9 ) && ( !mxIsEmpty(prhs[8]) ) )
+		{ 
+			/* auxInput specified */
+			if ( mxIsStruct(prhs[8]) )
+			{
+				auxInput_idx = 8;
+				x0_idx = -1;
+			}
+			else
+			{
+				auxInput_idx = -1;
+				x0_idx = 8;
+			}
+		}
+		else
+		{
+			auxInput_idx = -1;
+			x0_idx = -1;
+		}
+	}
+
+
+	/* ensure that data is given in real_t precision */
+	if ( ( mxIsDouble( prhs[ H_idx ] ) == 0 ) ||
+		 ( mxIsDouble( prhs[ g_idx ] ) == 0 ) )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
+		return;
+	}
+
+	/* check if supplied data contains 'NaN' or 'Inf' */
+	if (containsNaNorInf(prhs, nV * nV, H_idx, 0) == BT_TRUE) {
+		return;
+	}
+
+	if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
+		return;
+	}
+
+	if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
+		return;
+	}
+	if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
+		return;
+	}
+
+	/* Check inputs dimensions and assign pointers to inputs. */
+	if ( mxGetN( prhs[ H_idx ] ) != nV )
+	{
+		char msg[MAX_STRING_LENGTH]; 
+		snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Hessian matrix input dimension mismatch (%ld != %d)!", 
+				(long int)mxGetN(prhs[H_idx]), nV);
+		myMexErrMsgTxt(msg);
+		return;
+	}
+
+	if ( nC > 0 )
+	{
+		/* ensure that data is given in real_t precision */
+		if ( mxIsDouble( prhs[ A_idx ] ) == 0 )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
+			return;
+		}
+
+		/* Check inputs dimensions and assign pointers to inputs. */
+		if ( mxGetN( prhs[ A_idx ] ) != nV )
+		{
+			char msg[MAX_STRING_LENGTH]; 
+			snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Constraint matrix input dimension mismatch (%ld != %d)!", 
+					(long int)mxGetN(prhs[A_idx]), nV);
+			myMexErrMsgTxt(msg);
+			return;
+		}
+
+		if (containsNaNorInf(prhs, nV * nC, A_idx, 0) == BT_TRUE) {
+			return;
+		}
+		if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
+			return;
+		}
+		if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
+			return;
+		}
+	}
+
+	/* check dimensions and copy auxInputs */
+	if ( smartDimensionCheck( &g,nV,nP, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
+		return;
+
+	if ( smartDimensionCheck( &lb,nV,nP, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
+		return;
+
+	if ( smartDimensionCheck( &ub,nV,nP, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
+		return;
+
+	if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,x0_idx ) != SUCCESSFUL_RETURN )
+		return;
+
+	if ( nC > 0 )
+	{
+		if ( smartDimensionCheck( &lbA,nC,nP, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &ubA,nC,nP, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
+			return;
+	}
+
+	if ( auxInput_idx >= 0 )
+		setupAuxiliaryInputs( prhs[auxInput_idx],nV,nC, &x0,&guessedBoundsAndConstraints,&guessedBounds,&guessedConstraints );
+
+	
+	/* III) ACTUALLY PERFORM QPOASES FUNCTION CALL: */
+	int nWSRin = 5*(nV+nC);
+	real_t maxCpuTimeIn = -1.0;
+
+	if ( options_idx > 0 )
+		setupOptions( &options,prhs[options_idx],nWSRin,maxCpuTimeIn );
+
+	/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
+	setupHessianMatrix(	prhs[H_idx],nV, &H,&Hir,&Hjc,&Hv );
+
+	/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
+	if ( nC > 0 )
+		setupConstraintMatrix( prhs[A_idx],nV,nC, &A,&Air,&Ajc,&Av );
+
+	allocateOutputs( nlhs,plhs,nV,nC,nP );
+
+	if ( nC == 0 )
+	{
+		/* Call qpOASES (using QProblemB class). */
+		qpOASESmex_bounds(	nV,nP,
+							H,g,
+							lb,ub,
+							nWSRin,maxCpuTimeIn,
+							x0,&options,
+							nlhs,plhs,
+							guessedBounds
+							);
+
+		deleteAuxiliaryInputs( &guessedBounds,0 );
+        delete H;
+		if (Hv != 0) delete[] Hv;
+		if (Hjc != 0) delete[] Hjc;
+		if (Hir != 0) delete[] Hir;
+		return;
+	}
+	else
+	{
+		/* Call qpOASES (using QProblem class). */
+		qpOASESmex_constraints(	nV,nC,nP,
+								H,g,A,
+								lb,ub,lbA,ubA,
+								nWSRin,maxCpuTimeIn,
+								x0,&options,
+								nlhs,plhs,
+								guessedBounds, guessedConstraints
+								);
+		
+		deleteAuxiliaryInputs( &guessedBounds,&guessedConstraints );
+		delete A;
+		delete H;
+		if (Av != 0) delete[] Av;
+		if (Ajc != 0) delete[] Ajc;
+		if (Air != 0) delete[] Air;
+		if (Hv != 0) delete[] Hv;
+		if (Hjc != 0) delete[] Hjc;
+		if (Hir != 0) delete[] Hir;
+		return;
+	}
+}
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES.m
new file mode 100644
index 00000000000..970a446414c
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES.m
@@ -0,0 +1,77 @@
+%qpOASES -- An Implementation of the Online Active Set Strategy.
+%Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%Christian Kirches et al. All rights reserved.
+%
+%qpOASES is distributed under the terms of the
+%GNU Lesser General Public License 2.1 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.
+%
+%---------------------------------------------------------------------------------
+%
+%qpOASES solves (a series) of quadratic programming (QP) problems of the
+%following form:
+%
+%                min   1/2*x'Hx + x'g
+%                s.t.  lb  <=  x <= ub
+%                      lbA <= Ax <= ubA  {optional}
+%
+%Call
+%
+%    [x,fval,exitflag,iter,lambda,auxOutput] = 
+%                     qpOASES( H,g,A,lb,ub,lbA,ubA{,options{,auxInput} )
+%
+%for solving the above-mentioned QP. H must be a symmetric (but possibly 
+%indefinite) matrix and all vectors g, lb, ub, lbA, ubA have to be given
+%as column vectors. Options can be generated using the qpOASES_options command, 
+%otherwise default values are used. Optionally, further auxiliary inputs
+%may be generated using qpOASES_auxInput command and passed to the solver.
+%Both matrices H or A may be passed in sparse matrix format.
+%
+%Call
+%
+%    [x,fval,exitflag,iter,lambda,auxOutput] =
+%                     qpOASES( H,g,lb,ub{,options{,auxInput} )
+%
+%for solving the above-mentioned QP without general constraints.
+%
+%
+%Optional outputs (only x is mandatory):
+%    x          -  Optimal primal solution vector (if exitflag==0).
+%    fval       -  Optimal objective function value (if exitflag==0).
+%    exitflag   -   0: QP problem solved,
+%                   1: QP could not be solved within given number of iterations,
+%                  -1: QP could not be solved due to an internal error,
+%                  -2: QP is infeasible (and thus could not be solved),
+%                  -3: QP is unbounded (and thus could not be solved).
+%    iter       -  Number of active set iterations actually performed.
+%    lambda     -  Optimal dual solution vector (if status==0).
+%    auxOutput  -  Struct containing auxiliary outputs as described below.
+%
+%The auxOutput struct contains the following entries:
+%    workingSet -  The working set at point x. The working set is a subset
+%                  of the active set (which is the set of all indices
+%                  corresponding to bounds/constraints that hold with 
+%                  equality). The working set corresponds to bound/
+%                  constraint row vectors forming a linear independent set.
+%                  The first nV elements correspond to the bounds, the last
+%                  nC elements to the constraints.
+%                  The working set is encoded as follows:
+%                   1: bound/constraint at its upper bound
+%                   0: bound/constraint not at any bound
+%                  -1: bound/constraint at its lower bound
+%    cpuTime    -  Internally measured CPU time for solving QP.
+%
+%
+%If not a single QP but a sequence of QPs with varying vectors is to be solved,
+%the i-th QP is given by the i-th columns of the QP vectors g, lb, ub, lbA, ubA
+%(i.e. they are matrices in this case). Both matrices H and A remain constant.
+%
+%See also QPOASES_OPTIONS, QPOASES_AUXINPUT, QPOASES_SEQUENCE
+%
+%
+%For additional information see the qpOASES User's Manual or
+%visit http://www.qpOASES.org/.
+%
+%Please send remarks and questions to support@qpOASES.org!
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_auxInput.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_auxInput.m
new file mode 100644
index 00000000000..cd27e656909
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_auxInput.m
@@ -0,0 +1,103 @@
+%qpOASES -- An Implementation of the Online Active Set Strategy.
+%Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%Christian Kirches et al. All rights reserved.
+%
+%qpOASES is distributed under the terms of the
+%GNU Lesser General Public License 2.1 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.
+%
+%---------------------------------------------------------------------------------
+%
+%Returns a struct containing all possible auxiliary inputs to be passed 
+%to qpOASES.
+%
+%Call
+%    auxInput = qpOASES_auxInput();
+%to obtain a struct with all auxiliary inputs empty.
+%
+%Call
+%    options = qpOASES_auxInput( 'input1',value1,'input2',value2,... )
+%to obtain a struct with 'input1' set to value1 etc. and all remaining
+%auxiliary inputs empty.
+%
+%Call
+%    options = qpOASES_auxInput( oldInputs,'input1',value1,... )
+%to obtain a copy of the options struct oldInputs but with 'input1' set to 
+%value1 etc.
+%
+%
+%qpOASES features the following auxiliary inputs:
+%  x0                   -  Initial guess for optimal primal solution.
+%  guessedWorkingSet    -  Initial guess for working set at optimal
+%                          solution. The first nV elements correspond 
+%                          to the bounds, the last nC elements to the 
+%                          constraints.
+%                          The working set needs to be encoded as follows:
+%                          1: bound/constraint at its upper bound
+%                          0: bound/constraint not at any bound
+%                         -1: bound/constraint at its lower bound
+%
+%
+%See also QPOASES, QPOASES_SEQUENCE, QPOASES_OPTIONS
+%
+%
+%For additional information see the qpOASES User's Manual or
+%visit http://www.qpOASES.org/.
+%
+%Please send remarks and questions to support@qpOASES.org!
+function [ auxInput ] = qpOASES_auxInput( varargin )
+
+	firstIsStruct = 0;
+
+	if ( nargin == 0 ) 
+		auxInput = qpOASES_emptyAuxInput();
+	else
+		if ( isstruct( varargin{1} ) )
+			if ( mod( nargin,2 ) ~= 1 )
+				error('ERROR (qpOASES_auxInput): Auxiliary inputs must be specified in pairs!');
+			end
+			auxInput = varargin{1};
+			firstIsStruct = 1;
+        else
+            if ( mod( nargin,2 ) ~= 0 )
+				error('ERROR (qpOASES_auxInput): Auxiliary inputs must be specified in pairs!');
+            end
+            auxInput = qpOASES_emptyAuxInput();
+		end
+	end
+
+	% set options to user-defined values  
+	for i=(1+firstIsStruct):2:nargin
+
+		argName  = varargin{i};
+		argValue = varargin{i+1};
+
+        if ( ( isempty( argName ) ) || ( ~ischar( argName ) ) )
+			error('ERROR (qpOASES_auxInput): Argmument no. %d has to be a non-empty string!',i );
+        end
+			
+        if ( ( ischar(argValue) ) || ( ~isnumeric( argValue ) ) )
+			error('ERROR (qpOASES_auxInput): Argmument no. %d has to be a numerical constant!',i+1 );
+        end
+
+        if ( ~isfield( auxInput,argName ) )
+			error('ERROR (qpOASES_auxInput): Argmument no. %d is not a valid auxiliary input!',i );
+        end
+
+		eval( ['auxInput.',argName,' = argValue;'] );
+
+	end
+
+end
+
+
+function [ auxInput ] = qpOASES_emptyAuxInput( )
+
+	% setup auxiliary input struct with all entries empty
+	auxInput = struct(	'x0',                 [], ...
+						'guessedWorkingSet',  []  ...
+                        );
+
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_octave_utils.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_octave_utils.cpp
new file mode 100644
index 00000000000..7f75b721c8e
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_octave_utils.cpp
@@ -0,0 +1,884 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file interfaces/octave/qpOASES_octave_utils.cpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Alexander Buchner
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Collects utility functions for Interface to octave that
+ *	enables to call qpOASES as a MEX function.
+ *
+ */
+
+
+
+QPInstance::QPInstance(	int _nV, int _nC, BooleanType _isSimplyBounded
+						)
+{
+	handle = s_nexthandle++;
+
+	if ( _nC > 0 )
+		isSimplyBounded = BT_FALSE;
+	else
+		isSimplyBounded = _isSimplyBounded;
+	
+	if ( isSimplyBounded == BT_TRUE )
+	{
+		sqp = 0;
+		qpb = new QProblemB( _nV );
+	}
+	else
+	{
+		sqp = new SQProblem( _nV,_nC );
+		qpb = 0;
+	}
+
+	H = 0;
+	A = 0;
+	Hir = 0; 
+	Hjc = 0; 
+	Air = 0; 
+	Ajc = 0;
+	Hv = 0;
+	Av = 0;
+}	
+
+
+QPInstance::~QPInstance( )
+{		
+	deleteQPMatrices();
+
+	if ( sqp != 0 )
+	{
+		delete sqp;
+		sqp = 0;
+	}
+
+	if ( qpb != 0 )
+	{
+		delete qpb;
+		qpb = 0;
+	}
+}
+
+
+returnValue QPInstance::deleteQPMatrices( )
+{
+	if ( H != 0 )
+	{
+		delete H;
+		H = 0;
+	}
+
+	if (Hv != 0)
+	{
+		delete[] Hv;
+		Hv = 0;
+	}
+	
+	if (Hjc != 0)
+	{
+		delete[] Hjc;
+		Hjc = 0;
+	}
+	
+	if (Hir != 0)
+	{
+		delete[] Hir;
+		Hir = 0;
+	}
+	
+	if ( A != 0 )
+	{
+		delete A;
+		A = 0;
+	}
+
+	if (Av != 0)
+	{
+		delete[] Av;
+		Av = 0;
+	}
+	
+	if (Ajc != 0)
+	{
+		delete[] Ajc;
+		Ajc = 0;
+	}
+	
+	if (Air != 0)
+	{
+		delete[] Air;
+		Air = 0;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+int QPInstance::getNV() const
+{
+    if ( sqp != 0 )
+        return sqp->getNV();
+    
+    if ( qpb != 0 )
+        return qpb->getNV();
+    
+    return 0;
+}
+
+
+int QPInstance::getNC() const
+{
+    if ( sqp != 0 )
+        return sqp->getNC();
+   
+    return 0;
+}
+
+
+/*
+ *	a l l o c a t e Q P r o b l e m I n s t a n c e
+ */
+int allocateQPInstance(	int nV, int nC, BooleanType isSimplyBounded, const Options *options
+						)
+{
+	QPInstance* inst = new QPInstance( nV,nC,isSimplyBounded );
+
+	if ( inst->sqp != 0 )
+		inst->sqp->setOptions ( *options );
+	
+	if ( inst->qpb != 0 )
+		inst->qpb->setOptions ( *options );
+
+	g_instances.push_back (inst);
+	return inst->handle;
+}
+
+
+/*
+ *  g e t Q P r o b l e m I n s t a n c e
+ */
+QPInstance* getQPInstance( int handle )
+{
+	unsigned int ii;
+	// TODO: this may become slow ...
+	for (ii = 0; ii < g_instances.size (); ++ii)
+		if (g_instances[ii]->handle == handle)
+			return g_instances[ii];
+	return 0;
+}
+
+
+/*
+ *	d e l e t e Q P r o b l e m I n s t a n c e
+ */
+void deleteQPInstance( int handle )
+{
+	QPInstance *instance = getQPInstance (handle);
+	if (instance != 0) {
+		for (std::vector<QPInstance*>::iterator itor = g_instances.begin ();
+		     itor != g_instances.end (); ++itor)
+		     if ((*itor)->handle == handle) {
+				g_instances.erase (itor);
+				break;
+			}
+		delete instance;
+	}
+}
+
+
+
+/*
+ *	s m a r t D i m e n s i o n C h e c k
+ */
+returnValue smartDimensionCheck(	real_t** input, unsigned int m, unsigned int n, BooleanType emptyAllowed,
+									const mxArray* prhs[], int idx
+									)
+{
+	/* If index is negative, the input does not exist. */
+	if ( idx < 0 )
+	{
+		*input = 0;
+		return SUCCESSFUL_RETURN;
+	}
+
+	/* Otherwise the input has been passed by the user. */
+	if ( mxIsEmpty( prhs[ idx ] ) )
+	{
+		/* input is empty */
+		if ( ( emptyAllowed == BT_TRUE ) || ( idx == 0 ) ) /* idx==0 used for auxInput */
+		{
+			*input = 0;
+			return SUCCESSFUL_RETURN;
+		}
+		else
+		{
+			char msg[MAX_STRING_LENGTH];
+			if ( idx > 0 )
+				snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Empty argument %d not allowed!", idx+1);
+			myMexErrMsgTxt( msg );
+			return RET_INVALID_ARGUMENTS;
+		}
+	}
+	else
+	{
+		/* input is non-empty */
+        if ( mxIsSparse( prhs[ idx ] ) == 0 )
+        {
+            if ( ( mxGetM( prhs[ idx ] ) == m ) && ( mxGetN( prhs[ idx ] ) == n ) )
+            {
+                *input = (real_t*) mxGetPr( prhs[ idx ] );
+                return SUCCESSFUL_RETURN;
+            }
+            else
+            {
+                char msg[MAX_STRING_LENGTH];
+				if ( idx > 0 )
+					snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Input dimension mismatch for argument %d ([%ld,%ld] ~= [%d,%d]).",
+							 idx+1, (long int)mxGetM(prhs[idx]), (long int)mxGetN(prhs[idx]), m, n);
+				else /* idx==0 used for auxInput */
+					snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Input dimension mismatch for some auxInput entry ([%ld,%ld] ~= [%d,%d]).",
+							 (long int)mxGetM(prhs[idx]), (long int)mxGetN(prhs[idx]), m, n);
+                myMexErrMsgTxt( msg );
+                return RET_INVALID_ARGUMENTS;
+            }
+        }
+        else
+        {
+            char msg[MAX_STRING_LENGTH];
+			if ( idx > 0 )
+				snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Vector argument %d must not be in sparse format!", idx+1);
+			else /* idx==0 used for auxInput */
+				snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): auxInput entries must not be in sparse format!" );
+			myMexErrMsgTxt( msg );
+			return RET_INVALID_ARGUMENTS;
+        }
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	c o n t a i n s N a N
+ */
+BooleanType containsNaN( const real_t* const data, int dim )
+{
+	int i;
+
+	if ( data == 0 )
+		return BT_FALSE;
+
+	for ( i = 0; i < dim; ++i )
+		if ( mxIsNaN(data[i]) == 1 )
+			return BT_TRUE;
+
+	return BT_FALSE;
+}
+
+
+/*
+ *	c o n t a i n s I n f
+ */
+BooleanType containsInf( const real_t* const data, int dim )
+{
+	int i;
+
+	if ( data == 0 )
+		return BT_FALSE;
+
+	for ( i = 0; i < dim; ++i )
+		if ( mxIsInf(data[i]) == 1 )
+			return BT_TRUE;
+
+	return BT_FALSE;
+}
+
+
+/*
+ *	c o n t a i n s N a N o r I n f
+ */
+BooleanType containsNaNorInf(const mxArray* prhs[], int dim, int rhs_index,
+		bool mayContainInf) {
+
+	char msg[MAX_STRING_LENGTH];
+
+	// overwrite dim for sparse matrices
+	if (mxIsSparse(prhs[rhs_index]) == 1) {
+		dim = mxGetNzmax(prhs[rhs_index]);
+	}
+
+	if (containsNaN((real_t*) mxGetPr(prhs[rhs_index]), dim) == BT_TRUE) {
+		snprintf(msg, MAX_STRING_LENGTH,
+				"ERROR (qpOASES): Argument %d contains 'NaN' !", rhs_index + 1);
+		myMexErrMsgTxt(msg);
+		return BT_TRUE;
+	}
+
+	if (mayContainInf == 0) {
+		if (containsInf((real_t*) mxGetPr(prhs[rhs_index]), dim) == BT_TRUE) {
+			snprintf(msg, MAX_STRING_LENGTH,
+					"ERROR (qpOASES): Argument %d contains 'Inf' !",
+					rhs_index + 1);
+			myMexErrMsgTxt(msg);
+			return BT_TRUE;
+		}
+	}
+
+	return BT_FALSE;
+}
+
+
+/*
+ *	c o n v e r t F o r t r a n T o C
+ */
+returnValue convertFortranToC( const real_t* const A_for, int nV, int nC, real_t* const A )
+{
+	int i,j;
+
+	for ( i=0; i<nC; ++i )
+		for ( j=0; j<nV; ++j )
+			A[i*nV + j] = A_for[j*nC + i];
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	h a s O p t i o n s V a l u e
+ */
+BooleanType hasOptionsValue( const mxArray* optionsPtr, const char* const optionString, double** optionValue )
+{
+	mxArray* optionName = mxGetField( optionsPtr,0,optionString );
+
+	if ( optionName == 0 )
+	{
+		char msg[MAX_STRING_LENGTH];
+		snprintf(msg, MAX_STRING_LENGTH, "Option struct does not contain entry '%s', using default value instead!", optionString );
+		mexWarnMsgTxt( msg );
+		return BT_FALSE;
+	}
+
+	if ( ( mxIsEmpty(optionName) == false ) && ( mxGetM( optionName ) == 1 ) && ( mxGetN( optionName ) == 1 ) )
+	{
+		*optionValue = mxGetPr( optionName );
+		return BT_TRUE;
+	}
+	else
+	{
+		char msg[MAX_STRING_LENGTH];
+		snprintf(msg, MAX_STRING_LENGTH, "Option '%s' is not a scalar, using default value instead!", optionString );
+		mexWarnMsgTxt( msg );
+		return BT_FALSE;
+	}
+}
+
+
+/*
+ *	s e t u p O p t i o n s
+ */
+returnValue setupOptions( Options* options, const mxArray* optionsPtr, int& nWSRin, real_t& maxCpuTime )
+{
+	double* optionValue;
+	int optionValueInt;
+
+	/* Check for correct number of option entries;
+	 * may occur, e.g., if user types options.<misspelledName> = <someValue>; */
+	if ( mxGetNumberOfFields(optionsPtr) != 31 )
+		mexWarnMsgTxt( "Options might be set incorrectly as struct has wrong number of entries!\n         Type 'help qpOASES_options' for further information." );
+
+
+	if ( hasOptionsValue( optionsPtr,"maxIter",&optionValue ) == BT_TRUE )
+		if ( *optionValue >= 0.0 )
+			nWSRin = (int)*optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"maxCpuTime",&optionValue ) == BT_TRUE )
+		if ( *optionValue >= 0.0 )
+			maxCpuTime = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"printLevel",&optionValue ) == BT_TRUE )
+	{
+        #ifdef __SUPPRESSANYOUTPUT__
+        options->printLevel = PL_NONE;
+        #else
+		optionValueInt = (int)*optionValue;
+		options->printLevel = (REFER_NAMESPACE_QPOASES PrintLevel)optionValueInt;
+        if ( options->printLevel < PL_DEBUG_ITER )
+            options->printLevel = PL_DEBUG_ITER;
+        if ( options->printLevel > PL_HIGH )
+            options->printLevel = PL_HIGH;       
+        #endif
+	}
+
+	if ( hasOptionsValue( optionsPtr,"enableRamping",&optionValue ) == BT_TRUE )
+	{
+		optionValueInt = (int)*optionValue;
+		options->enableRamping = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
+	}
+
+	if ( hasOptionsValue( optionsPtr,"enableFarBounds",&optionValue ) == BT_TRUE )
+	{
+		optionValueInt = (int)*optionValue;
+		options->enableFarBounds = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
+	}
+
+	if ( hasOptionsValue( optionsPtr,"enableFlippingBounds",&optionValue ) == BT_TRUE )
+	{
+		optionValueInt = (int)*optionValue;
+		options->enableFlippingBounds = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
+	}
+
+	if ( hasOptionsValue( optionsPtr,"enableRegularisation",&optionValue ) == BT_TRUE )
+	{
+		optionValueInt = (int)*optionValue;
+		options->enableRegularisation = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
+	}
+
+	if ( hasOptionsValue( optionsPtr,"enableFullLITests",&optionValue ) == BT_TRUE )
+	{
+		optionValueInt = (int)*optionValue;
+		options->enableFullLITests = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
+	}
+
+	if ( hasOptionsValue( optionsPtr,"enableNZCTests",&optionValue ) == BT_TRUE )
+	{
+		optionValueInt = (int)*optionValue;
+		options->enableNZCTests = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
+	}
+
+	if ( hasOptionsValue( optionsPtr,"enableDriftCorrection",&optionValue ) == BT_TRUE )
+		options->enableDriftCorrection = (int)*optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"enableCholeskyRefactorisation",&optionValue ) == BT_TRUE )
+		options->enableCholeskyRefactorisation = (int)*optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"enableEqualities",&optionValue ) == BT_TRUE )
+	{
+		optionValueInt = (int)*optionValue;
+		options->enableEqualities = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
+	}
+
+
+	if ( hasOptionsValue( optionsPtr,"terminationTolerance",&optionValue ) == BT_TRUE )
+		options->terminationTolerance = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"boundTolerance",&optionValue ) == BT_TRUE )
+		options->boundTolerance = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"boundRelaxation",&optionValue ) == BT_TRUE )
+		options->boundRelaxation = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"epsNum",&optionValue ) == BT_TRUE )
+		options->epsNum = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"epsDen",&optionValue ) == BT_TRUE )
+		options->epsDen = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"maxPrimalJump",&optionValue ) == BT_TRUE )
+		options->maxPrimalJump = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"maxDualJump",&optionValue ) == BT_TRUE )
+		options->maxDualJump = *optionValue;
+
+
+	if ( hasOptionsValue( optionsPtr,"initialRamping",&optionValue ) == BT_TRUE )
+		options->initialRamping = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"finalRamping",&optionValue ) == BT_TRUE )
+		options->finalRamping = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"initialFarBounds",&optionValue ) == BT_TRUE )
+		options->initialFarBounds = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"growFarBounds",&optionValue ) == BT_TRUE )
+		options->growFarBounds = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"initialStatusBounds",&optionValue ) == BT_TRUE )
+	{
+		optionValueInt = (int)*optionValue;
+		if ( optionValueInt < -1 ) 
+			optionValueInt = -1;
+		if ( optionValueInt > 1 ) 
+			optionValueInt = 1;
+		options->initialStatusBounds = (REFER_NAMESPACE_QPOASES SubjectToStatus)optionValueInt;
+	}
+
+	if ( hasOptionsValue( optionsPtr,"epsFlipping",&optionValue ) == BT_TRUE )
+		options->epsFlipping = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"numRegularisationSteps",&optionValue ) == BT_TRUE )
+		options->numRegularisationSteps = (int)*optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"epsRegularisation",&optionValue ) == BT_TRUE )
+		options->epsRegularisation = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"numRefinementSteps",&optionValue ) == BT_TRUE )
+		options->numRefinementSteps = (int)*optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"epsIterRef",&optionValue ) == BT_TRUE )
+		options->epsIterRef = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"epsLITests",&optionValue ) == BT_TRUE )
+		options->epsLITests = *optionValue;
+
+	if ( hasOptionsValue( optionsPtr,"epsNZCTests",&optionValue ) == BT_TRUE )
+		options->epsNZCTests = *optionValue;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	s e t u p A u x i l i a r y I n p u t s
+ */
+returnValue setupAuxiliaryInputs(	const mxArray* auxInput, unsigned int nV, unsigned int nC,
+									double** x0, double** guessedBoundsAndConstraints, double** guessedBounds, double** guessedConstraints
+									)
+{
+	unsigned int i;
+	mxArray* curField = 0;
+
+	/* x0 */
+	curField = mxGetField( auxInput,0,"x0" );
+	if ( curField == NULL )
+		mexWarnMsgTxt( "auxInput struct does not contain entry 'x0'!\n         Type 'help qpOASES_auxInput' for further information." );
+	else
+	{
+		*x0 = mxGetPr(curField);
+		if ( smartDimensionCheck( x0,nV,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
+			return RET_INVALID_ARGUMENTS;
+	}
+
+	/* guessedWorkingSet */
+	curField = mxGetField( auxInput,0,"guessedWorkingSet" );
+	if ( curField == NULL )
+		mexWarnMsgTxt( "auxInput struct does not contain entry 'guessedWorkingSet'!\n         Type 'help qpOASES_auxInput' for further information." );
+	else
+	{
+		*guessedBoundsAndConstraints = mxGetPr(curField);
+		if ( smartDimensionCheck( guessedBoundsAndConstraints,nV+nC,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
+			return RET_INVALID_ARGUMENTS;
+
+		if ( *guessedBoundsAndConstraints != 0 )
+		{
+			*guessedBounds = new double[nV];
+			for (i = 0; i < nV; i++)
+				(*guessedBounds)[i] = (*guessedBoundsAndConstraints)[i];
+
+			if ( nC > 0 )
+			{
+				*guessedConstraints = new double[nC];
+				for (i = 0; i < nC; i++)
+					(*guessedConstraints)[i] = (*guessedBoundsAndConstraints)[i + nV];
+			}
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	d e l e t e A u x i l i a r y I n p u t s
+ */
+returnValue deleteAuxiliaryInputs(	double** guessedBounds, double** guessedConstraints
+									)
+{
+	if ( ( guessedBounds != 0 ) && ( *guessedBounds != 0 ) )
+	{
+		delete[] (*guessedBounds);
+		*guessedBounds = 0;
+	}
+
+	if ( ( guessedConstraints != 0 ) && ( guessedConstraints != 0 ) )
+	{
+		delete[] (*guessedConstraints);
+		*guessedConstraints = 0;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	a l l o c a t e O u t p u t s
+ */
+returnValue allocateOutputs(	int nlhs, mxArray* plhs[], int nV, int nC = 0, int nP = 1, int handle = -1
+								)
+{
+	/* Create output vectors and assign pointers to them. */
+	int curIdx = 0;
+
+	/* handle */
+	if ( handle >= 0 )
+		plhs[curIdx++] = mxCreateDoubleMatrix( 1, 1, mxREAL );
+
+	/* x */
+	plhs[curIdx++] = mxCreateDoubleMatrix( nV, nP, mxREAL );
+
+	if ( nlhs > curIdx )
+	{
+		/* fval */
+		plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
+
+		if ( nlhs > curIdx )
+		{
+			/* exitflag */
+			plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
+
+			if ( nlhs > curIdx )
+			{
+				/* iter */
+				plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
+
+				if ( nlhs > curIdx )
+				{
+					/* lambda */
+					plhs[curIdx++] = mxCreateDoubleMatrix( nV+nC, nP, mxREAL );
+
+					if ( nlhs > curIdx )
+					{
+						/* setup auxiliary output struct */
+						mxArray* auxOutput = mxCreateStructMatrix( 1,1,0,0 );
+						int curFieldNum;
+						
+						/* working set */
+						curFieldNum = mxAddField( auxOutput,"workingSet" );
+						if ( curFieldNum >= 0 )
+							mxSetFieldByNumber( auxOutput,0,curFieldNum,mxCreateDoubleMatrix( nV+nC, nP, mxREAL ) );
+
+						curFieldNum = mxAddField( auxOutput,"cpuTime" );
+						if ( curFieldNum >= 0 )
+							mxSetFieldByNumber( auxOutput,0,curFieldNum,mxCreateDoubleMatrix( 1, nP, mxREAL ) );
+
+						plhs[curIdx] = auxOutput;
+					}
+				}
+			}
+		}
+	}
+	
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	o b t a i n O u t p u t s
+ */
+returnValue obtainOutputs(	int k, QProblemB* qp, returnValue returnvalue, int _nWSRout, real_t _cpuTime,
+							int nlhs, mxArray* plhs[], int nV, int nC = 0, int handle = -1
+							)
+{
+	/* Create output vectors and assign pointers to them. */
+	int curIdx = 0;
+
+	/* handle */
+	if ( handle >= 0 )
+		plhs[curIdx++] = mxCreateDoubleScalar( handle );
+
+	/* x */
+	double* x = mxGetPr( plhs[curIdx++] );
+	qp->getPrimalSolution( &(x[k*nV]) );
+
+	if ( nlhs > curIdx )
+	{
+		/* fval */
+		double* obj = mxGetPr( plhs[curIdx++] );
+		obj[k] = qp->getObjVal( );
+
+		if ( nlhs > curIdx )
+		{
+			/* exitflag */
+			double* status = mxGetPr( plhs[curIdx++] );
+			status[k] = (real_t)getSimpleStatus( returnvalue );
+
+			if ( nlhs > curIdx )
+			{
+				/* iter */
+				double* nWSRout = mxGetPr( plhs[curIdx++] );
+				nWSRout[k] = (real_t) _nWSRout;
+
+				if ( nlhs > curIdx )
+				{
+					/* lambda */
+					double* y = mxGetPr( plhs[curIdx++] );
+					qp->getDualSolution( &(y[k*(nV+nC)]) );
+
+					if ( nlhs > curIdx )
+					{
+						
+						mxArray* auxOutput = plhs[curIdx];
+						mxArray* curField = 0;
+
+						/* working set */
+						curField = mxGetField( auxOutput,0,"workingSet" );
+						double* workingSet = mxGetPr(curField);
+
+						QProblem* problemPointer;
+						problemPointer = dynamic_cast<QProblem*>(qp);
+
+						// cast successful?
+						if (problemPointer != NULL) {
+							problemPointer->getWorkingSet( &(workingSet[k*(nV+nC)]) );
+						} else {
+							qp->getWorkingSet( &(workingSet[k*(nV+nC)]) );
+						}
+
+						/* cpu time */
+						curField = mxGetField( auxOutput,0,"cpuTime" );
+						double* cpuTime = mxGetPr(curField);
+						cpuTime[0] = (real_t) _cpuTime;
+					}
+				}
+			}
+		}
+	}
+	
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	s e t u p H e s s i a n M a t r i x
+ */
+returnValue setupHessianMatrix(	const mxArray* prhsH, int nV,
+								SymmetricMatrix** H, sparse_int_t** Hir, sparse_int_t** Hjc, real_t** Hv
+								)
+{
+	if ( mxIsSparse( prhsH ) != 0 )
+	{
+		mwIndex *oct_ir = mxGetIr( prhsH );
+		mwIndex *oct_jc = mxGetJc( prhsH );
+		double *v = (double*)mxGetPr( prhsH );
+		long nfill = 0;
+		long i, j;
+		BooleanType needInsertDiag;
+
+		/* copy indices to avoid 64/32-bit integer confusion */
+		/* also add explicit zeros on diagonal for regularization strategy */
+		/* copy values, too */
+		*Hir = new sparse_int_t[oct_jc[nV] + nV];
+		*Hjc = new sparse_int_t[nV+1];
+		*Hv = new real_t[oct_jc[nV] + nV];
+        for (j = 0; j < nV; j++) 
+		{
+            needInsertDiag = BT_TRUE;
+                
+            (*Hjc)[j] = (sparse_int_t)(oct_jc[j]) + nfill;
+            /* fill up to diagonal */
+            for (i = oct_jc[j]; i < oct_jc[j+1]; i++) 
+			{
+                if ( oct_ir[i] == j )
+                    needInsertDiag = BT_FALSE;
+                    
+                /* add zero diagonal element if not present */
+                if ( ( oct_ir[i] > j ) && ( needInsertDiag == BT_TRUE ) )
+                {
+                    (*Hir)[i + nfill] = (sparse_int_t)j;
+                    (*Hv)[i + nfill] = 0.0;
+                    nfill++;
+                    /* only add diag once */
+                    needInsertDiag = BT_FALSE;
+                }
+                        
+				(*Hir)[i + nfill] = (sparse_int_t)(oct_ir[i]);
+				(*Hv)[i + nfill] = (real_t)(v[i]);
+			}
+		}
+		(*Hjc)[nV] = (sparse_int_t)(oct_jc[nV]) + nfill;
+
+		SymSparseMat *sH;
+		*H = sH = new SymSparseMat(nV, nV, *Hir, *Hjc, *Hv);
+		sH->createDiagInfo();
+	}
+	else
+	{
+		/* make a deep-copy in order to avoid modifying input data when regularising */
+		real_t* H_for = (real_t*) mxGetPr( prhsH );
+		real_t* H_mem = new real_t[nV*nV];
+		memcpy( H_mem,H_for, nV*nV*sizeof(real_t) );
+
+		*H = new SymDenseMat( nV,nV,nV, H_mem );
+		(*H)->doFreeMemory( );
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p C o n s t r a i n t M a t r i x
+ */
+returnValue setupConstraintMatrix(	const mxArray* prhsA, int nV, int nC,
+									Matrix** A, sparse_int_t** Air, sparse_int_t** Ajc, real_t** Av
+									)
+{
+	if ( mxIsSparse( prhsA ) != 0 )
+	{
+		long i;
+
+		mwIndex *oct_ir = mxGetIr( prhsA );
+		mwIndex *oct_jc = mxGetJc( prhsA );
+		double *v = (double*)mxGetPr( prhsA );
+
+		/* copy indices to avoid 64/32-bit integer confusion */
+		*Air = new sparse_int_t[oct_jc[nV]];
+		*Ajc = new sparse_int_t[nV+1];
+		for (i = 0; i < oct_jc[nV]; i++)
+			(*Air)[i] = (sparse_int_t)(oct_ir[i]);
+		for (i = 0; i < nV + 1; i++)
+			(*Ajc)[i] = (sparse_int_t)(oct_jc[i]);
+
+		/* copy values, too */
+		*Av = new real_t[(*Ajc)[nV]];
+		for (i = 0; i < (*Ajc)[nV]; i++)
+			(*Av)[i] = (real_t)(v[i]);
+
+		*A = new SparseMatrix(nC, nV, *Air, *Ajc, *Av);
+	}
+	else
+	{
+		/* Convert constraint matrix A from FORTRAN to C style
+		* (not necessary for H as it should be symmetric!). */
+		real_t* A_for = (real_t*) mxGetPr( prhsA );
+		real_t* A_mem = new real_t[nC*nV];
+		convertFortranToC( A_for,nV,nC, A_mem );
+		*A = new DenseMatrix(nC, nV, nV, A_mem );
+		(*A)->doFreeMemory();
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_octave_utils.hpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_octave_utils.hpp
new file mode 100644
index 00000000000..5430fdeac44
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_octave_utils.hpp
@@ -0,0 +1,92 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file interfaces/octave/qpOASES_octave_utils.hpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Alexander Buchner
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Collects utility functions for Interface to octave that
+ *	enables to call qpOASES as a MEX function.
+ *
+ */
+
+
+
+/* Work-around for settings where mexErrMsgTxt causes unexpected behaviour. */
+#ifdef __AVOID_MEXERRMSGTXT__
+	#define myMexErrMsgTxt( TEXT ) mexPrintf( "%s\n\n",(TEXT) );
+#else
+	#define myMexErrMsgTxt mexErrMsgTxt
+#endif
+
+
+#include "mex.h"
+//#include "matrix.h"
+#include "string.h"
+#include <vector>
+
+
+/*
+ * QProblem instance class
+ */
+class QPInstance
+{
+	private:
+		static int s_nexthandle;
+
+	public:
+		QPInstance(	int _nV = 0,
+					int _nC = 0,
+					BooleanType _isSimplyBounded = BT_FALSE
+					);
+
+		~QPInstance( );
+	
+		returnValue deleteQPMatrices();
+		
+		int getNV() const;
+		int getNC() const;
+
+		int handle;
+
+		SQProblem* sqp;
+		QProblemB* qpb;
+		BooleanType isSimplyBounded;
+
+		SymmetricMatrix* H;
+		Matrix* A;
+		sparse_int_t* Hir; 
+		sparse_int_t* Hjc; 
+		sparse_int_t* Air; 
+		sparse_int_t* Ajc;
+		real_t* Hv;
+		real_t* Av;
+};
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_options.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_options.m
new file mode 100644
index 00000000000..f3cd4175129
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_options.m
@@ -0,0 +1,251 @@
+%qpOASES -- An Implementation of the Online Active Set Strategy.
+%Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%Christian Kirches et al. All rights reserved.
+%
+%qpOASES is distributed under the terms of the
+%GNU Lesser General Public License 2.1 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.
+%
+%---------------------------------------------------------------------------------
+%
+%Returns a struct containing values for all options to be used within qpOASES.
+%
+%Call
+%    options = qpOASES_options( 'default' );
+%    options = qpOASES_options( 'reliable' );
+%    options = qpOASES_options( 'MPC' );
+%to obtain a set of default options or a pre-defined set of options tuned
+%for reliable or fast QP solution, respectively.
+%
+%Call
+%    options = qpOASES_options( 'option1',value1,'option2',value2,... )
+%to obtain a set of default options but with 'option1' set to value1 etc.
+%
+%Call
+%    options = qpOASES_options( oldopts,'option1',value1,... )
+%to obtain a copy of the options struct oldopts but with 'option1' set to 
+%value1 etc.
+%
+%Call
+%    options = qpOASES_options( 'default', 'option1',value1,... )
+%    options = qpOASES_options( 'reliable','option1',value1,... )
+%    options = qpOASES_options( 'MPC',     'option1',value1,... )
+%to obtain a set of default options or a pre-defined set of options tuned
+%for reliable or fast QP solution, respectively, but with 'option1' set to 
+%value1 etc.
+%
+%
+%qpOASES features the following options:
+%  maxIter                    -  Maximum number of iterations (if set
+%                                to -1, a value is chosen heuristically)
+%  maxCpuTime                 -  Maximum CPU time in seconds (if set
+%                                to -1, only iteration limit is used)
+%  printLevel                 -  0: no printed output,
+%                                1: only error messages are printed,
+%                                2: iterations and error messages are printed,
+%                                3: all available messages are printed.
+%
+%  enableRamping              -  Enables (1) or disables (0) ramping.
+%  enableFarBounds            -  Enables (1) or disables (0) the use of 
+%                                far bounds.
+%  enableFlippingBounds       -  Enables (1) or disables (0) the use of 
+%                                flipping bounds.
+%  enableRegularisation       -  Enables (1) or disables (0) automatic 
+%                                Hessian regularisation.
+%  enableFullLITests          -  Enables (1) or disables (0) condition-hardened 
+%                                (but more expensive) LI test.
+%  enableNZCTests             -  Enables (1) or disables (0) nonzero curvature 
+%                                tests.
+%  enableDriftCorrection      -  Specifies the frequency of drift corrections:
+%                                0: turns them off, 
+%                                1: uses them at each iteration etc.
+%  enableCholeskyRefactorisation - Specifies the frequency of a full re-
+%                                factorisation of projected Hessian matrix:
+%                                0: turns them off, 
+%                                1: uses them at each iteration etc.
+%  enableEqualities           -  Specifies whether equalities should be treated 
+%                                as always active (1) or not (0)
+%
+%  terminationTolerance       -  Relative termination tolerance to stop homotopy.
+%  boundTolerance             -  If upper and lower bounds differ less than this
+%                                tolerance, they are regarded equal, i.e. as 
+%                                equality constraint.
+%  boundRelaxation            -  Initial relaxation of bounds to start homotopy 
+%                                and initial value for far bounds.
+%  epsNum                     -  Numerator tolerance for ratio tests.
+%  epsDen                     -  Denominator tolerance for ratio tests.
+%  maxPrimalJump              -  Maximum allowed jump in primal variables in 
+%                                nonzero curvature tests.
+%  maxDualJump                -  Maximum allowed jump in dual variables in 
+%                                linear independence tests.
+%
+%  initialRamping             -  Start value for ramping strategy.
+%  finalRamping               -  Final value for ramping strategy.
+%  initialFarBounds           -  Initial size for far bounds.
+%  growFarBounds              -  Factor to grow far bounds.
+%  initialStatusBounds        -  Initial status of bounds at first iteration:
+%                                 0: all bounds inactive,
+%                                -1: all bounds active at their lower bound,
+%                                +1: all bounds active at their upper bound.
+%  epsFlipping                -  Tolerance of squared Cholesky diagonal factor 
+%                                which triggers flipping bound.
+%  numRegularisationSteps     -  Maximum number of successive regularisation steps.
+%  epsRegularisation          -  Scaling factor of identity matrix used for 
+%                                Hessian regularisation.
+%  numRefinementSteps         -  Maximum number of iterative refinement steps.
+%  epsIterRef                 -  Early termination tolerance for iterative 
+%                                refinement.
+%  epsLITests                 -  Tolerance for linear independence tests.
+%  epsNZCTests                -  Tolerance for nonzero curvature tests.
+%
+%
+%See also QPOASES, QPOASES_SEQUENCE
+%
+%
+%For additional information see the qpOASES User's Manual or
+%visit http://www.qpOASES.org/.
+%
+%Please send remarks and questions to support@qpOASES.org!
+function [ options ] = qpOASES_options( varargin )
+
+	firstIsStructOrScheme = 0;
+
+	if ( nargin == 0 ) 
+		options = qpOASES_default_options();
+	else
+		if ( isstruct( varargin{1} ) )
+			if ( mod( nargin,2 ) ~= 1 )
+				error('ERROR (qpOASES_options): Options must be specified in pairs!');
+			end
+			options = varargin{1};
+			firstIsStructOrScheme = 1;
+		else
+			if ( ischar( varargin{1} ) )
+				if ( mod( nargin,2 ) == 0 )
+					options = qpOASES_default_options();
+				else
+					if ( ( nargin > 1 ) && ( ischar( varargin{nargin} ) ) )
+						error('ERROR (qpOASES_options): Options must be specified in pairs!');
+					end
+
+					switch ( varargin{1} )
+						case 'default'
+							options = qpOASES_default_options();
+						case 'reliable'
+							options = qpOASES_reliable_options();
+						case {'MPC','mpc','fast'}
+							options = qpOASES_MPC_options();
+						otherwise
+							error( ['ERROR (qpOASES_options): Only the following option schemes are defined: ''default'', ''reliable'', ''MPC''!'] );
+							
+					end
+					firstIsStructOrScheme = 1;
+				end
+			else
+				error('ERROR (qpOASES_options): First argument needs to be a string or an options struct!');
+			end
+		end
+	end
+
+	% set options to user-defined values  
+	for i=(1+firstIsStructOrScheme):2:nargin
+
+		argName  = varargin{i};
+		argValue = varargin{i+1};
+
+		if ( ( isempty( argName ) ) || ( ~ischar( argName ) ) )
+			error('ERROR (qpOASES_options): Argmument no. %d has to be a non-empty string!',i );
+        end
+			
+		if ( ( ischar(argValue) ) || ( ~isscalar( argValue ) ) )
+			error('ERROR (qpOASES_options): Argmument no. %d has to be a numerical constant!',i+1 );
+        end
+
+		if ( ~isfield( options,argName ) )
+			error('ERROR (qpOASES_options): Argmument no. %d is an invalid option!',i );
+		end
+
+		eval( ['options.',argName,' = ',num2str(argValue),';'] );
+
+	end
+
+end
+
+
+function [ options ] = qpOASES_default_options( )
+
+	% setup options struct with default values
+	options = struct(	'maxIter',                       -1, ...
+						'maxCpuTime',                    -1, ...
+						'printLevel',                     1, ...
+						...
+						'enableRamping',                  1, ...
+						'enableFarBounds',                1, ...
+						'enableFlippingBounds',           1, ...
+						'enableRegularisation',           0, ...
+						'enableFullLITests',              0, ...
+						'enableNZCTests',                 1, ...
+						'enableDriftCorrection',          1, ...
+						'enableCholeskyRefactorisation',  0, ...
+						'enableEqualities',               0, ...
+						...
+						'terminationTolerance',           5.0e6*eps, ...
+						'boundTolerance',                 1.0e6*eps, ...
+						'boundRelaxation',                1.0e4, ...
+						'epsNum',                        -1.0e3*eps, ...
+						'epsDen',                         1.0e3*eps, ...
+						'maxPrimalJump',                  1.0e8, ...
+						'maxDualJump',                    1.0e8, ...
+						...
+    					'initialRamping',                 0.5, ...
+						'finalRamping',                   1.0, ...
+						'initialFarBounds',               1.0e6, ...
+						'growFarBounds',                  1.0e3, ...
+						'initialStatusBounds',            -1, ...
+						'epsFlipping',                    1.0e3*eps, ...
+						'numRegularisationSteps',         0, ...
+						'epsRegularisation',              1.0e3*eps, ...
+						'numRefinementSteps',             1, ...
+						'epsIterRef',                     1.0e2*eps, ...
+						'epsLITests',                     1.0e5*eps, ...
+						'epsNZCTests',                    3.0e3*eps );
+
+end
+
+
+
+function [ options ] = qpOASES_reliable_options( )
+
+	% setup options struct with values for most reliable QP solution
+	options = qpOASES_default_options( );
+
+	options.enableFullLITests             =  1;
+	options.enableCholeskyRefactorisation =  1;
+
+	options.numRefinementSteps            =  2;
+
+end
+
+
+function [ options ] = qpOASES_MPC_options( )
+
+	% setup options struct with values for most reliable QP solution
+	options = qpOASES_default_options( );
+
+	options.enableRamping                 =  0;
+	options.enableFarBounds               =  1;
+	options.enableFlippingBounds          =  0;
+	options.enableRegularisation          =  1;
+	options.enableNZCTests                =  0;
+	options.enableDriftCorrection         =  0;
+	options.enableEqualities              =  1;
+
+	options.terminationTolerance          =  1.0e9*eps;
+	
+	options.initialStatusBounds           =  0;
+	options.numRegularisationSteps        =  1;
+	options.numRefinementSteps            =  0;
+
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_sequence.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_sequence.cpp
new file mode 100644
index 00000000000..35ea59741be
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_sequence.cpp
@@ -0,0 +1,1061 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file interfaces/octave/qpOASES_sequence.cpp
+ *	\author Hans Joachim Ferreau, Christian Kirches, Andreas Potschka, Alexander Buchner
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Interface for octave that enables to call qpOASES as a MEX function
+ *  (variant for solving QP sequences).
+ *
+ */
+
+
+
+#include <qpOASES.hpp>
+
+
+USING_NAMESPACE_QPOASES
+
+
+#include "qpOASES_octave_utils.hpp"
+
+/** initialise handle counter of QPInstance class */
+int QPInstance::s_nexthandle = 1;
+
+/** global pointer to QP objects */
+static std::vector<QPInstance *> g_instances;
+
+#include "qpOASES_octave_utils.cpp"
+
+
+/*
+ *	i n i t S B
+ */
+int initSB(	int handle, 
+			SymmetricMatrix *H, real_t* g,
+			const real_t* const lb, const real_t* const ub,
+			int nWSRin, real_t maxCpuTimeIn,
+			const real_t* const x0, Options* options,
+			int nOutputs, mxArray* plhs[],
+			double* guessedBounds
+			)
+{
+	int nWSRout = nWSRin;
+	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+
+	/* 1) setup initial QP. */
+	QProblemB* globalQPB = getQPInstance(handle)->qpb;
+
+	if ( globalQPB == 0 )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
+		return -1;
+	}
+
+	globalQPB->setOptions( *options );
+	
+	/* 2) Solve initial QP. */
+	returnValue returnvalue;
+	int nV = globalQPB->getNV();
+	
+	/* 3) Fill the working set. */
+	Bounds bounds(nV);
+	if (guessedBounds != 0) {
+		for (int i = 0; i < nV; i++) {
+			if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_LOWER);
+			} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_UPPER);
+			} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_INACTIVE);
+			} else {
+				char msg[MAX_STRING_LENGTH];
+				snprintf(msg, MAX_STRING_LENGTH,
+						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
+				myMexErrMsgTxt(msg);
+				return -1;
+			}
+		}
+	}
+
+	if (x0 == 0 && guessedBounds == 0)
+		returnvalue = globalQPB->init( H,g,lb,ub, nWSRout,&maxCpuTimeOut );
+	else
+		returnvalue = globalQPB->init( H,g,lb,ub, nWSRout,&maxCpuTimeOut, x0, 0,
+				guessedBounds != 0 ? &bounds : 0);
+
+	/* 3) Assign lhs arguments. */
+	obtainOutputs(	0,globalQPB,returnvalue,nWSRout,maxCpuTimeOut,
+					nOutputs,plhs,nV,0,handle );
+
+	return 0;
+}
+
+
+/*
+ *	i n i t
+ */
+int init(	int handle, 
+			SymmetricMatrix *H, real_t* g, Matrix *A,
+			const real_t* const lb, const real_t* const ub, const real_t* const lbA, const real_t* const ubA,
+			int nWSRin, real_t maxCpuTimeIn,
+			const real_t* const x0, Options* options,
+			int nOutputs, mxArray* plhs[],
+			double* guessedBounds, double* guessedConstraints
+			)
+{
+	int nWSRout = nWSRin;
+	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+
+	/* 1) setup initial QP. */
+	SQProblem* globalSQP = getQPInstance(handle)->sqp;
+
+	if ( globalSQP == 0 )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
+		return -1;
+	}
+
+	globalSQP->setOptions( *options );
+	
+	/* 2) Solve initial QP. */
+	returnValue returnvalue;
+	int nV = globalSQP->getNV();
+	int nC = globalSQP->getNC();
+	
+	/* 3) Fill the working set. */
+	Bounds bounds(nV);
+	Constraints constraints(nC);
+	if (guessedBounds != 0) {
+		for (int i = 0; i < nV; i++) {
+			if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_LOWER);
+			} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_UPPER);
+			} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
+				bounds.setupBound(i, ST_INACTIVE);
+			} else {
+				char msg[MAX_STRING_LENGTH];
+				snprintf(msg, MAX_STRING_LENGTH,
+						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
+				myMexErrMsgTxt(msg);
+				return -1;
+			}
+		}
+	}
+
+	if (guessedConstraints != 0) {
+		for (int i = 0; i < nC; i++) {
+			if ( isEqual(guessedConstraints[i],-1.0) == BT_TRUE ) {
+				constraints.setupConstraint(i, ST_LOWER);
+			} else if ( isEqual(guessedConstraints[i],1.0) == BT_TRUE ) {
+				constraints.setupConstraint(i, ST_UPPER);
+			} else if ( isEqual(guessedConstraints[i],0.0) == BT_TRUE ) {
+				constraints.setupConstraint(i, ST_INACTIVE);
+			} else {
+				char msg[MAX_STRING_LENGTH];
+				snprintf(msg, MAX_STRING_LENGTH,
+						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of constraints!");
+				myMexErrMsgTxt(msg);
+				return -1;
+			}
+		}
+	}
+
+	if (x0 == 0 && guessedBounds == 0 && guessedConstraints == 0)
+		returnvalue = globalSQP->init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut);
+	else
+		returnvalue = globalSQP->init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut, x0, 0,
+				guessedBounds != 0 ? &bounds : 0,
+				guessedConstraints != 0 ? &constraints : 0);
+
+	/* 3) Assign lhs arguments. */
+	obtainOutputs(	0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
+					nOutputs,plhs,nV,nC,handle );
+
+	return 0;
+}
+
+
+
+/*
+ *	h o t s t a r t S B
+ */
+int hotstartSB(	int handle,
+                const real_t* const g,
+				const real_t* const lb, const real_t* const ub,
+				int nWSRin, real_t maxCpuTimeIn,
+				Options* options,
+				int nOutputs, mxArray* plhs[]
+				)
+{
+	int nWSRout = nWSRin;
+	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+
+	QProblemB* globalQPB = getQPInstance(handle)->qpb;
+
+	if ( globalQPB == 0 )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
+		return -1;
+	}
+
+	/* 1) Solve QP with given options. */
+	globalQPB->setOptions( *options );
+	returnValue returnvalue = globalQPB->hotstart( g,lb,ub, nWSRout,&maxCpuTimeOut );
+
+	/* 2) Assign lhs arguments. */
+	obtainOutputs(	0,globalQPB,returnvalue,nWSRout,maxCpuTimeOut,
+					nOutputs,plhs,0,0 );
+
+	return 0;
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+int hotstart(	int handle,
+                const real_t* const g,
+				const real_t* const lb, const real_t* const ub,
+				const real_t* const lbA, const real_t* const ubA,
+				int nWSRin, real_t maxCpuTimeIn,
+				Options* options,
+				int nOutputs, mxArray* plhs[]
+				)
+{
+	int nWSRout = nWSRin;
+	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+
+	QProblem* globalSQP = getQPInstance(handle)->sqp;
+
+	if ( globalSQP == 0 )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
+		return -1;
+	}
+
+	/* 1) Solve QP with given options. */
+	globalSQP->setOptions( *options );
+	returnValue returnvalue = globalSQP->hotstart( g,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut );
+
+	/* 2) Assign lhs arguments. */
+	obtainOutputs(	0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
+					nOutputs,plhs,0,0 );
+
+	return 0;
+}
+
+
+/*
+ *	h o t s t a r t V M
+ */
+int hotstartVM(	int handle,
+                    SymmetricMatrix *H, real_t* g, Matrix *A,
+					const real_t* const lb, const real_t* const ub, const real_t* const lbA, const real_t* const ubA,
+					int nWSRin, real_t maxCpuTimeIn,
+					Options* options,
+					int nOutputs, mxArray* plhs[]
+					)
+{
+	int nWSRout = nWSRin;
+	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
+
+	SQProblem* globalSQP = getQPInstance(handle)->sqp;
+
+	if ( globalSQP == 0 )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
+		return -1;
+	}
+
+	/* 1) Solve QP. */
+	globalSQP->setOptions( *options );
+	returnValue returnvalue = globalSQP->hotstart( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut );
+
+	switch (returnvalue)
+	{
+		case SUCCESSFUL_RETURN:
+		case RET_QP_UNBOUNDED:
+		case RET_QP_INFEASIBLE:
+			break;
+		otherwise:
+			myMexErrMsgTxt( "ERROR (qpOASES): Hotstart failed." );
+			return -1;
+	}
+
+	/* 2) Assign lhs arguments. */
+	obtainOutputs(	0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
+					nOutputs,plhs,0,0 );
+
+	return 0;
+}
+
+
+
+/*
+ *	m e x F u n c t i o n
+ */
+void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
+{
+	/* inputs */
+	char typeString[2];
+	real_t *g=0, *lb=0, *ub=0, *lbA=0, *ubA=0, *x0=0;
+	int H_idx=-1, g_idx=-1, A_idx=-1, lb_idx=-1, ub_idx=-1, lbA_idx=-1, ubA_idx=-1;
+	int x0_idx=-1, auxInput_idx=-1;
+
+	double *guessedBoundsAndConstraints = 0;
+	double *guessedBounds = 0, *guessedConstraints = 0;
+
+	BooleanType isSimplyBoundedQp = BT_FALSE;
+
+	Options options;
+	options.printLevel = PL_LOW;
+	#ifdef __DEBUG__
+	options.printLevel = PL_HIGH;
+	#endif
+	#ifdef __SUPPRESSANYOUTPUT__
+	options.printLevel = PL_NONE;
+	#endif
+
+	/* dimensions */
+	unsigned int nV=0, nC=0, handle=0;
+	int nWSRin;
+	real_t maxCpuTimeIn = -1.0;
+	QPInstance* globalQP = 0;
+
+	/* I) CONSISTENCY CHECKS: */
+	/* 1) Ensure that qpOASES is called with a feasible number of input arguments. */
+	if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
+	{
+		if ( nrhs != 2 )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+	}
+	
+	/* 2) Ensure that first input is a string ... */
+	if ( mxIsChar( prhs[0] ) != 1 )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): First input argument must be a string!" );
+		return;
+	}
+
+	mxGetString( prhs[0], typeString, 2 );
+
+	/*    ... and if so, check if it is an allowed one. */
+	if ( ( strcmp( typeString,"i" ) != 0 ) && ( strcmp( typeString,"I" ) != 0 ) &&
+		 ( strcmp( typeString,"h" ) != 0 ) && ( strcmp( typeString,"H" ) != 0 ) &&
+		 ( strcmp( typeString,"m" ) != 0 ) && ( strcmp( typeString,"M" ) != 0 ) &&
+		 ( strcmp( typeString,"e" ) != 0 ) && ( strcmp( typeString,"E" ) != 0 ) &&
+		 ( strcmp( typeString,"c" ) != 0 ) && ( strcmp( typeString,"C" ) != 0 ) )
+	{
+		myMexErrMsgTxt( "ERROR (qpOASES): Undefined first input argument!\nType 'help qpOASES_sequence' for further information." );
+		return;
+	}
+
+
+	/* II) SELECT RESPECTIVE QPOASES FUNCTION CALL: */
+	/* 1) Init (without or with initial guess for primal solution). */
+	if ( ( strcmp( typeString,"i" ) == 0 ) || ( strcmp( typeString,"I" ) == 0 ) )
+	{
+		/* consistency checks */
+		if ( ( nlhs < 2 ) || ( nlhs > 7 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		/* ensure that data is given in double precision */
+		if ((mxIsDouble(prhs[1]) == 0) || (mxIsDouble(prhs[2]) == 0)) {
+			myMexErrMsgTxt(
+					"ERROR (qpOASES): All data has to be provided in double precision!");
+			return;
+		}
+
+        nV = (unsigned int)mxGetM( prhs[1] ); /* row number of Hessian matrix */
+
+		/* Check for 'Inf' and 'Nan' in Hessian */
+		H_idx = 1;
+		g_idx = 2;
+		if (containsNaNorInf(prhs, nV * nV, H_idx, 0) == BT_TRUE) {
+			return;
+		}
+
+		/* Check for 'Inf' and 'Nan' in gradient */
+		if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
+			return;
+		}
+
+		/* determine whether is it a simply bounded QP */
+		if ( nrhs <= 7 )
+			isSimplyBoundedQp = BT_TRUE;
+		else
+			isSimplyBoundedQp = BT_FALSE;
+
+		if ( isSimplyBoundedQp == BT_TRUE )
+		{
+			lb_idx = 3;
+			ub_idx = 4;
+
+			if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
+				return;
+			}
+			if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
+				return;
+			}
+
+			/* Check inputs dimensions and assign pointers to inputs. */
+			nC = 0; /* row number of constraint matrix */
+
+			if ( mxGetN( prhs[1] ) != nV )
+			{
+				myMexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );
+				return;
+			}
+
+			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
+				return;
+
+			/* default value for nWSR */
+			nWSRin = 5*nV;
+
+			/* Check whether x0 and options are specified .*/
+			if ( nrhs >= 6 )
+			{
+				if ((!mxIsEmpty(prhs[5])) && (mxIsStruct(prhs[5])))
+					setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );
+
+				if ( ( nrhs >= 7 ) && ( !mxIsEmpty(prhs[6]) ) )
+				{ 
+					/* auxInput specified */
+					if ( mxIsStruct(prhs[6]) )
+					{
+						auxInput_idx = 6;
+						x0_idx = -1;
+					}
+					else
+					{
+						auxInput_idx = -1;
+						x0_idx = 6;
+					}
+				}
+				else
+				{
+					auxInput_idx = -1;
+					x0_idx = -1;
+				}
+			}
+		}
+		else
+		{
+			/* ensure that data is given in double precision */
+			if ( ( mxIsDouble( prhs[1] ) == 0 ) ||
+				 ( mxIsDouble( prhs[2] ) == 0 ) ||
+				 ( mxIsDouble( prhs[3] ) == 0 ) )
+			{
+				myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
+				return;
+			}
+		
+			/* Check inputs dimensions and assign pointers to inputs. */
+			nC = (unsigned int)mxGetM( prhs[3] ); /* row number of constraint matrix */
+
+			A_idx = 3;
+			lb_idx = 4;
+			ub_idx = 5;
+			lbA_idx = 6;
+			ubA_idx = 7;
+
+			if (containsNaNorInf(prhs, nV * nC, A_idx, 0) == BT_TRUE) {
+				return;
+			}
+			if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
+				return;
+			}
+			if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
+				return;
+			}
+			if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
+				return;
+			}
+			if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
+				return;
+			}
+
+			if ( ( mxGetN( prhs[1] ) != nV ) || ( ( mxGetN( prhs[3] ) != 0 ) && ( mxGetN( prhs[3] ) != nV ) ) )
+			{
+				myMexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );
+				return;
+			}
+		
+			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
+				return;
+			
+			if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,7 ) != SUCCESSFUL_RETURN )
+				return;
+
+			/* default value for nWSR */
+			nWSRin = 5*(nV+nC);
+
+			/* Check whether x0 and options are specified .*/
+			if ( nrhs >= 9 )
+			{
+				if ((!mxIsEmpty(prhs[8])) && (mxIsStruct(prhs[8])))
+					setupOptions( &options,prhs[8],nWSRin,maxCpuTimeIn );
+
+				if ( ( nrhs >= 10 ) && ( !mxIsEmpty(prhs[9]) ) )
+				{ 
+					/* auxInput specified */
+					if ( mxIsStruct(prhs[9]) )
+					{
+						auxInput_idx = 9;
+						x0_idx = -1;
+					}
+					else
+					{
+						auxInput_idx = -1;
+						x0_idx = 9;
+					}
+				}
+				else
+				{
+					auxInput_idx = -1;
+					x0_idx = -1;
+				}
+			}
+		}
+
+
+		/* check dimensions and copy auxInputs */
+		if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,x0_idx ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( auxInput_idx >= 0 )
+			setupAuxiliaryInputs( prhs[auxInput_idx],nV,nC, &x0,&guessedBoundsAndConstraints,&guessedBounds,&guessedConstraints );
+
+
+		/* allocate instance */
+		handle = allocateQPInstance( nV,nC,isSimplyBoundedQp, &options );	
+		globalQP = getQPInstance( handle );
+
+		/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
+		setupHessianMatrix(	prhs[1],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );
+
+		/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
+		if ( nC > 0 )
+			setupConstraintMatrix( prhs[3],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );
+
+		/* Create output vectors and assign pointers to them. */
+		allocateOutputs( nlhs,plhs, nV,nC,1,handle );
+
+		/* Call qpOASES. */
+		if ( isSimplyBoundedQp == BT_TRUE )
+		{
+			initSB(	handle,
+					globalQP->H,g,
+					lb,ub,
+					nWSRin,maxCpuTimeIn,
+					x0,&options,
+					nlhs,plhs,
+					guessedBounds
+					);
+
+			deleteAuxiliaryInputs( &guessedBounds,0 );
+		}
+		else
+		{
+			init(	handle,
+					globalQP->H,g,globalQP->A,
+					lb,ub,lbA,ubA,
+					nWSRin,maxCpuTimeIn,
+					x0,&options,
+					nlhs,plhs,
+					guessedBounds, guessedConstraints
+					);
+
+			deleteAuxiliaryInputs( &guessedBounds,&guessedConstraints );
+		}
+
+		return;
+	}
+
+	/* 2) Hotstart. */
+	if ( ( strcmp( typeString,"h" ) == 0 ) || ( strcmp( typeString,"H" ) == 0 ) )
+	{
+		/* consistency checks */
+		if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		if ( ( nrhs < 5 ) || ( nrhs > 8 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		/* determine whether is it a simply bounded QP */
+		if ( nrhs < 7 )
+			isSimplyBoundedQp = BT_TRUE;
+		else
+			isSimplyBoundedQp = BT_FALSE;
+
+
+		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxGetM( prhs[1] ) != 1 ) || ( mxGetN( prhs[1] ) != 1 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		/* get QP instance */
+		handle = (unsigned int)mxGetScalar( prhs[1] );
+		globalQP = getQPInstance ( handle );
+		if ( globalQP == 0 )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
+			return;
+		}
+
+		nV = globalQP->getNV();
+
+		g_idx = 2;
+		lb_idx = 3;
+		ub_idx = 4;
+
+		if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
+			return;
+		}
+
+		if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
+			return;
+		}
+		if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
+			return;
+		}
+
+
+		/* Check inputs dimensions and assign pointers to inputs. */
+		if ( isSimplyBoundedQp == BT_TRUE )
+		{
+			nC = 0;
+
+			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
+				return;
+
+			/* default value for nWSR */
+			nWSRin = 5*nV;
+
+			/* Check whether options are specified .*/
+			if ( nrhs == 6 )
+				if ( ( !mxIsEmpty( prhs[5] ) ) && ( mxIsStruct( prhs[5] ) ) )
+					setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );
+		}
+		else
+		{
+			nC = globalQP->getNC( );
+
+			lbA_idx = 5;
+			ubA_idx = 6;
+
+			if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
+				return;
+			}
+			if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
+				return;
+			}
+
+			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
+				return;
+
+			if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
+				return;
+
+			/* default value for nWSR */
+			nWSRin = 5*(nV+nC);
+
+			/* Check whether options are specified .*/
+			if ( nrhs == 8 )
+				if ( ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
+					setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
+		}
+
+		/* Create output vectors and assign pointers to them. */
+		allocateOutputs( nlhs,plhs, nV,nC );
+
+		/* call qpOASES */
+		if ( isSimplyBoundedQp == BT_TRUE )
+		{
+			hotstartSB(	handle, g,
+						lb,ub,
+						nWSRin,maxCpuTimeIn,
+						&options,
+						nlhs,plhs
+						);
+		}
+		else
+		{
+			hotstart(	handle, g,
+						lb,ub,lbA,ubA,
+						nWSRin,maxCpuTimeIn,
+						&options,
+						nlhs,plhs
+						);
+		}
+
+		return;
+	}
+
+	/* 3) Modify matrices. */
+	if ( ( strcmp( typeString,"m" ) == 0 ) || ( strcmp( typeString,"M" ) == 0 ) )
+	{
+		/* consistency checks */
+		if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		if ( ( nrhs < 9 ) || ( nrhs > 10 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxGetM( prhs[1] ) != 1 ) || ( mxGetN( prhs[1] ) != 1 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		/* ensure that data is given in double precision */
+		if ( ( mxIsDouble( prhs[2] ) == 0 ) ||
+			 ( mxIsDouble( prhs[3] ) == 0 ) ||
+			 ( mxIsDouble( prhs[4] ) == 0 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
+			return;
+		}
+
+		/* get QP instance */
+		handle = (unsigned int)mxGetScalar( prhs[1]);
+		globalQP = getQPInstance ( handle );
+		if ( globalQP == 0 )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
+			return;
+		}
+
+		/* Check inputs dimensions and assign pointers to inputs. */
+		nV = (unsigned int)mxGetM( prhs[2] ); /* row number of Hessian matrix */
+		nC = (unsigned int)mxGetM( prhs[4] ); /* row number of constraint matrix */
+		
+		H_idx = 2;
+		g_idx = 3;
+		A_idx = 4;
+		lb_idx = 5;
+		ub_idx = 6;
+		lbA_idx = 7;
+		ubA_idx = 8;
+
+		/* check if supplied data contains 'NaN' or 'Inf' */
+		if (containsNaNorInf(prhs, nV * nV, H_idx, 0) == BT_TRUE) {
+			return;
+		}
+
+		if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
+			return;
+		}
+
+		if (containsNaNorInf(prhs, nV * nC, A_idx, 0) == BT_TRUE) {
+			return;
+		}
+
+		if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
+			return;
+		}
+		if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
+			return;
+		}
+
+		if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
+			return;
+		}
+		if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
+			return;
+		}
+
+		/* Check that dimensions are consistent with existing QP instance */
+		if (nV != (unsigned int) globalQP->getNV () || nC != (unsigned int) globalQP->getNC ())
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): QP dimensions must be constant during a sequence! Try creating a new QP instance instead." );
+			return;
+		}
+
+		if ( ( mxGetN( prhs[2] ) != nV ) || ( ( mxGetN( prhs[4] ) != 0 ) && ( mxGetN( prhs[4] ) != nV ) ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );
+			return;
+		}
+
+		if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,3 ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,7 ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,8 ) != SUCCESSFUL_RETURN )
+			return;
+
+		/* default value for nWSR */
+		nWSRin = 5*(nV+nC);
+
+		/* Check whether options are specified .*/
+		if ( nrhs > 9 )
+			if ( ( !mxIsEmpty( prhs[9] ) ) && ( mxIsStruct( prhs[9] ) ) )
+				setupOptions( &options,prhs[9],nWSRin,maxCpuTimeIn );
+
+		globalQP->deleteQPMatrices( );
+
+		/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
+		setupHessianMatrix(	prhs[2],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );
+
+		/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
+		if ( nC > 0 )
+			setupConstraintMatrix( prhs[4],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );
+
+		/* Create output vectors and assign pointers to them. */
+		allocateOutputs( nlhs,plhs, nV,nC );
+
+		/* Call qpOASES */
+		hotstartVM(	handle, globalQP->H,g,globalQP->A,
+					lb,ub,lbA,ubA,
+					nWSRin,maxCpuTimeIn,
+					&options,
+					nlhs,plhs
+					);
+
+		return;
+	}
+
+	/* 4) Solve current equality constrained QP. */
+	if ( ( strcmp( typeString,"e" ) == 0 ) || ( strcmp( typeString,"E" ) == 0 ) )
+	{
+		/* consistency checks */
+		if ( ( nlhs < 1 ) || ( nlhs > 3 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		if ( ( nrhs < 7 ) || ( nrhs > 8 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxGetM( prhs[1] ) != 1 ) || ( mxGetN( prhs[1] ) != 1 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		/* get QP instance */
+		handle = (unsigned int)mxGetScalar( prhs[1] );
+		globalQP = getQPInstance ( handle );
+		if ( globalQP == 0 )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
+			return;
+		}
+
+		/* Check inputs dimensions and assign pointers to inputs. */
+		int nRHS = (int)mxGetN(prhs[2]);
+		nV = globalQP->getNV( );
+		nC = globalQP->getNC( );
+		real_t *x_out, *y_out;
+
+		g_idx = 2;
+		lb_idx = 3;
+		ub_idx = 4;
+		lbA_idx = 5;
+		ubA_idx = 6;
+
+		/* check if supplied data contains 'NaN' or 'Inf' */
+		if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
+			return;
+		}
+		if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
+			return;
+		}
+		if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
+			return;
+		}
+
+		if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
+			return;
+		}
+		if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
+			return;
+		}
+
+		if ( smartDimensionCheck( &g,nV,nRHS, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &lb,nV,nRHS, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &ub,nV,nRHS, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &lbA,nC,nRHS, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
+			return;
+
+		if ( smartDimensionCheck( &ubA,nC,nRHS, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
+			return;
+
+		/* Check whether options are specified .*/
+		if ( ( nrhs == 8 ) && ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
+		{
+			nWSRin = 5*(nV+nC);
+			setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
+			globalQP->sqp->setOptions( options );
+		}
+
+		/* Create output vectors and assign pointers to them. */
+		plhs[0] = mxCreateDoubleMatrix( nV, nRHS, mxREAL );
+		x_out = mxGetPr(plhs[0]);
+		if (nlhs >= 2)
+		{
+			plhs[1] = mxCreateDoubleMatrix( nV+nC, nRHS, mxREAL );
+			y_out = mxGetPr(plhs[1]);
+
+			if (nlhs >= 3) {
+				plhs[2] = mxCreateDoubleMatrix( nV+nC, nRHS, mxREAL );
+				double* workingSet = mxGetPr(plhs[2]);
+
+				globalQP->sqp->getWorkingSet(workingSet);
+			}
+		}
+		else
+			y_out = new real_t[nV+nC];
+
+		/* Solve equality constrained QP */
+		returnValue returnvalue = globalQP->sqp->solveCurrentEQP( nRHS,g,lb,ub,lbA,ubA, x_out,y_out );
+
+		if (nlhs < 2)
+			delete[] y_out;
+
+		if (returnvalue != SUCCESSFUL_RETURN)
+		{
+			char msg[MAX_STRING_LENGTH];
+			snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Couldn't solve current EQP (code %d)!", returnvalue);
+			myMexErrMsgTxt(msg);
+			return;
+		}
+
+		return;
+	}
+
+	/* 5) Cleanup. */
+	if ( ( strcmp( typeString,"c" ) == 0 ) || ( strcmp( typeString,"C" ) == 0 ) )
+	{		
+		/* consistency checks */
+		if ( nlhs != 0 )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		if ( nrhs != 2 )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxGetM( prhs[1] ) != 1 ) || ( mxGetN( prhs[1] ) != 1 ) )
+		{
+			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
+			return;
+		}
+
+		/* Cleanup SQProblem instance. */
+		handle =(unsigned int)mxGetScalar( prhs[1] );
+		deleteQPInstance ( handle );
+		
+		return;
+	}
+
+}
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_sequence.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_sequence.m
new file mode 100644
index 00000000000..6dc57dc767f
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_sequence.m
@@ -0,0 +1,112 @@
+%qpOASES -- An Implementation of the Online Active Set Strategy.
+%Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%Christian Kirches et al. All rights reserved.
+%
+%qpOASES is distributed under the terms of the
+%GNU Lesser General Public License 2.1 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.
+%
+%---------------------------------------------------------------------------------
+%
+%qpOASES_sequence is intended to solve a sequence of quadratic
+%programming (QP) problems of the following form:
+%
+%                min   1/2*x'Hx + x'g
+%                s.t.  lb  <=  x <= ub
+%                      lbA <= Ax <= ubA  {optional}
+%
+%I) Call
+%
+%    [QP,x,fval,exitflag,iter,lambda,auxOutput] = ...
+%               qpOASES_sequence( 'i',H,g,A,lb,ub,lbA,ubA{,options{,auxInput} )
+%or 
+%    [QP,x,fval,exitflag,iter,lambda,auxOutput] = ...
+%               qpOASES_sequence( 'i',H,g,lb,ub{,options{,auxInput} )
+%
+%for initialising and solving the first above-mentioned QP of the sequence
+%starting from an initial guess x0. H must be a symmetric (possibly indefinite) 
+%matrix and all vectors g, lb, ub, lbA, ubA have to be given as column vectors. 
+%Options can be generated using the qpOASES_options command, otherwise default
+%values are used. Optionally, further auxiliary inputs may be generated 
+%using qpOASES_auxInput command and passed to the solver.
+%Both matrices H or A may be passed in sparse matrix format.
+%
+%II) Call
+%
+%     [x,fval,exitflag,iter,lambda,auxOutput] = ...
+%                      qpOASES_sequence( 'h',QP,g,lb,ub,lbA,ubA{,options} )
+%or
+%     [x,fval,exitflag,iter,lambda,auxOutput] = ...
+%                      qpOASES_sequence( 'h',QP,g,lb,ub{,options} )
+%
+%for hotstarting from the previous QP solution to the one of the next QP
+%given by the vectors g, lb, ub, lbA, ubA. Options can be generated using the 
+%qpOASES_options command, otherwise default values are used.
+%
+%III) Call
+%
+%     [x,fval,exitflag,iter,lambda,auxOutput] = ...
+%                     qpOASES_sequence( 'm',QP,H,g,A,lb,ub,lbA,ubA{,options} )
+%
+%for hotstarting from the previous QP solution to the one of the next QP
+%given by the matrices H, A and the vectors g, lb, ub, lbA, ubA. The previous
+%active set serves as a starting guess. If the new projected Hessian matrix
+%turns out to be not positive definite, qpOASES recedes to a safe initial active
+%set guess automatically. This can result in a high number of iterations iter.
+%Options can be generated using the qpOASES_options command, otherwise default
+%values are used.
+%
+%IV) Call
+%
+%     [x,lambda,workingSet] = qpOASES_sequence( 'e',QP,g,lb,ub,lbA,ubA{,options} )
+%
+%for solving the equality constrained QP with constraints determined by the
+%current active set. All inequalities and bounds which were not active in the
+%previous solution might be violated. This command does not alter the internal
+%state of qpOASES. Instead of calling this command multiple times, it is
+%possible to supply several columns simultaneously in g, lb, ub, lbA, and ubA.
+%Options can be generated using the qpOASES_options command, otherwise default
+%values are used.
+%
+%V) Having solved the last QP of your sequence, call
+%
+%     qpOASES_sequence( 'c',QP )
+%
+%in order to cleanup the internal memory.
+%
+%
+%Optional outputs (only x is mandatory):
+%    x          -  Optimal primal solution vector (if exitflag==0).
+%    fval       -  Optimal objective function value (if exitflag==0).
+%    exitflag   -   0: QP solved,
+%                   1: QP could not be solved within given number of iterations,
+%                  -1: QP could not be solved due to an internal error,
+%                  -2: QP is infeasible (and thus could not be solved),
+%                  -3: QP is unbounded (and thus could not be solved).
+%    iter       -  Number of active set iterations actually performed.
+%    lambda     -  Optimal dual solution vector (if status==0).
+%    auxOutput  -  Struct containing auxiliary outputs as described below.
+%
+%The auxOutput struct contains the following entries:
+%    workingSet -  The working set at point x. The working set is a subset
+%                  of the active set (which is the set of all indices
+%                  corresponding to bounds/constraints that hold with 
+%                  equality). The working set corresponds to bound/
+%                  constraint row vectors forming a linear independent set.
+%                  The first nV elements correspond to the bounds, the last
+%                  nC elements to the constraints.
+%                  The working set is encoded as follows:
+%                   1: bound/constraint at its upper bound
+%                   0: bound/constraint not at any bound
+%                  -1: bound/constraint at its lower bound
+%    cpuTime    -  Internally measured CPU time for solving QP.
+%
+%See also QPOASES_OPTIONS, QPOASES_AUXINPUT, QPOASES
+%
+%
+%For additional information see the qpOASES User's Manual or
+%visit http://www.qpOASES.org/.
+%
+%Please send remarks and questions to support@qpOASES.org!
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/README.rst b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/README.rst
new file mode 100644
index 00000000000..64911d9f4cb
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/README.rst
@@ -0,0 +1,68 @@
+pyqpOASES: a Python interface to qpOASES
+
+:Author: Sebastian F. Walter, Manuel Kudruss
+
+
+Installation
+------------
+
+
+Requirements:
+
+  You'll need numpy and cython. Install for instance with::
+
+      sudo pip install cython
+      sudo pip install numpy
+
+Method 1:
+
+  This is a local installation and creates `./interfaces/python/qpoases.so`::
+
+    make python
+
+  Then, you'll have to update your PYTHONPATH, e.g., on LINUX you have to add::
+
+    export PYTHONPATH=$PYTHONPATH:/home/swalter/projects/qpOASES/interfaces/python
+
+  to your ``~/.bashrc``.
+
+Method 2:
+
+  global installation::
+
+    sudo make pythoninstall
+
+Method 3::
+
+   cd ./interfaces/python/
+   python setup.py build_ext --inplace
+   # or python setup.py install
+
+
+Testing your installation
+-------------------------
+
+For a quick test run::
+
+  cd ./interfaces/python
+  python example1.py
+
+
+To run a complete unit test you need ``nose``. Install for instance with::
+
+    sudo pip install nose
+
+Then::
+
+   cd ./interfaces/python/
+   nosestests ./tests
+
+The results of the tests can be found in `./interfaces/python/tests/results`.
+
+Tested setups
+-------------
+
+The Python interface is known to work on
+
+* Windows, Python 3
+* Linux (Ubuntu 12.04) using Python 2.7.3, Python 3.2.3. NumPy 1.8, Cython 0.19
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/example1.pyx b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/example1.pyx
new file mode 100644
index 00000000000..1cdfbc8f5b9
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/example1.pyx
@@ -0,0 +1,73 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+## Example adapted from examples/example1.cpp.
+## author of this file: Sebastian F. Walter
+
+import numpy as np
+from qpoases import PyQProblem as QProblem
+from qpoases import PyPrintLevel as PrintLevel
+from qpoases import PyOptions as Options
+cimport numpy as np
+
+def run():
+
+    #Setup data of QP.
+
+    cdef np.ndarray[np.double_t, ndim=2] H
+    cdef np.ndarray[np.double_t, ndim=2] A
+    cdef np.ndarray[np.double_t, ndim=1] g
+    cdef np.ndarray[np.double_t, ndim=1] lb
+    cdef np.ndarray[np.double_t, ndim=1] ub
+    cdef np.ndarray[np.double_t, ndim=1] lbA
+    cdef np.ndarray[np.double_t, ndim=1] ubA
+
+    H   = np.array([1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
+    A   = np.array([1.0, 1.0 ]).reshape((2,1))
+    g   = np.array([1.5, 1.0 ])
+    lb  = np.array([0.5, -2.0])
+    ub  = np.array([5.0, 2.0 ])
+    lbA = np.array([-1.0 ])
+    ubA = np.array([2.0])
+
+    # Setting up QProblem object.
+
+    cdef example = QProblem(2, 1)
+    cdef options = Options()
+    options.printLevel = PrintLevel.NONE
+    example.setOptions(options)
+
+    # Solve first QP.
+
+    cdef int nWSR = 10
+    example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
+
+    # Solve subsequent QPs
+
+    cdef int i,j
+    for i in range(100000):
+        for j in range(1, 100):
+            g[0] = i%j
+        example.hotstart(g, lb, ub, lbA, ubA, nWSR)
+
+run()
+
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/index.html
new file mode 100644
index 00000000000..8054f6c1c37
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/index.html
@@ -0,0 +1,9 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces/python/examples/cython</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/interfaces/python/examples/cython</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="example1.pyx">example1.pyx</a></li>
+  <li><a href="setup.py">setup.py</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/setup.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/setup.py
new file mode 100644
index 00000000000..d605d2c33a1
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/setup.py
@@ -0,0 +1,16 @@
+from distutils.core import setup
+from distutils.extension import Extension
+from Cython.Distutils import build_ext
+
+ext_module = Extension(
+    "example1",
+    ["example1.pyx"],
+    extra_compile_args=['-fopenmp'],
+    extra_link_args=['-fopenmp'],
+)
+
+setup(
+    name = 'Hello world app',
+    cmdclass = {'build_ext': build_ext},
+    ext_modules = [ext_module],
+)
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example1.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example1.py
new file mode 100644
index 00000000000..b3da19a52fc
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example1.py
@@ -0,0 +1,76 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+## Example adapted from examples/example1.cpp.
+## author of this file: Sebastian F. Walter
+
+import numpy as np
+from qpoases import PyQProblem as QProblem
+from qpoases import PyOptions as Options
+from qpoases import PyPrintLevel as PrintLevel
+
+#Setup data of first QP.
+
+H   = np.array([1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
+A   = np.array([1.0, 1.0 ]).reshape((2,1))
+g   = np.array([1.5, 1.0 ])
+lb  = np.array([0.5, -2.0])
+ub  = np.array([5.0, 2.0 ])
+lbA = np.array([-1.0 ])
+ubA = np.array([2.0])
+
+
+# Setup data of second QP.
+
+g_new   = np.array([1.0, 1.5])
+lb_new  = np.array([0.0, -1.0])
+ub_new  = np.array([5.0, -0.5])
+lbA_new = np.array([-2.0])
+ubA_new = np.array([1.0])
+
+
+# Setting up QProblem object.
+
+example = QProblem(2, 1)
+options = Options()
+options.printLevel = PrintLevel.NONE
+example.setOptions(options)
+
+# Solve first QP.
+nWSR = 10;
+example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
+
+
+# Solve second QP.
+nWSR = 10
+
+for i in range(100000):
+    for j in range(1, 100):
+        g_new[0] = i%j
+    example.hotstart( g_new, lb_new, ub_new, lbA_new, ubA_new, nWSR)
+
+# Get and print solution of second QP.
+
+xOpt = np.zeros(2)
+example.getPrimalSolution(xOpt);
+print("\nxOpt = [ %e, %e ];  objVal = %e\n\n"%(xOpt[0],xOpt[1],example.getObjVal()))
+example.printOptions();
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example1b.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example1b.py
new file mode 100644
index 00000000000..7fb519ad11c
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example1b.py
@@ -0,0 +1,72 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+## Example adapted from examples/example1b.cpp.
+## author of this file: Sebastian F. Walter
+
+import numpy as np
+from qpoases import PyQProblemB as QProblemB
+from qpoases import PyBooleanType as BooleanType
+from qpoases import PySubjectToStatus as SubjectToStatus
+from qpoases import PyOptions as Options
+
+# Example for qpOASES main function using the QProblemB class.
+
+#Setup data of first QP.
+
+H   = np.array([1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
+g   = np.array([1.5, 1.0 ])
+lb  = np.array([0.5, -2.0])
+ub  = np.array([5.0, 2.0 ])
+
+# Setup data of second QP.
+
+g_new   = np.array([1.0, 1.5])
+lb_new  = np.array([0.0, -1.0])
+ub_new  = np.array([5.0, -0.5])
+
+
+# Setting up QProblemB object.
+example = QProblemB(2)
+
+options = Options()
+options.enableFlippingBounds = BooleanType.FALSE
+options.initialStatusBounds  = SubjectToStatus.INACTIVE
+options.numRefinementSteps   = 1
+
+example.setOptions(options)
+
+# Solve first QP.
+nWSR = 10
+example.init(H, g, lb, ub, nWSR);
+print("\nnWSR = %d\n\n"%nWSR)
+
+# Solve second QP.
+nWSR = 10;
+example.hotstart(g_new, lb_new, ub_new, nWSR)
+print("\nnWSR = %d\n\n"% nWSR)
+
+# Get and print solution of second QP.
+xOpt = np.zeros(2)
+example.getPrimalSolution(xOpt)
+print("\nxOpt = [ %e, %e ];  objVal = %e\n\n" %(xOpt[0], xOpt[1],
+	                                            example.getObjVal()))
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example2.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example2.py
new file mode 100644
index 00000000000..d093e2e17fb
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example2.py
@@ -0,0 +1,90 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+## Example adapted from examples/example2.cpp.
+## author of this file: Sebastian F. Walter
+
+import numpy as np
+from qpoases import PySQProblem as SQProblem
+from qpoases import PySolutionAnalysis as SolutionAnalysis
+
+
+#  Setup data of first QP.
+H = np.array([ 1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
+A = np.array([ 1.0, 1.0 ]).reshape((2,1))
+g = np.array([ 1.5, 1.0 ])
+lb = np.array([ 0.5, -2.0 ])
+ub = np.array([ 5.0, 2.0 ])
+lbA = np.array([ -1.0 ])
+ubA = np.array([ 2.0 ])
+
+#  Setup data of second QP.
+H_new = np.array([ 1.0, 0.5, 0.5, 0.5 ]).reshape((2,2))
+A_new = np.array([ 1.0, 5.0 ]).reshape((2,1))
+g_new = np.array([ 1.0, 1.5 ])
+lb_new = np.array([ 0.0, -1.0 ])
+ub_new = np.array([ 5.0, -0.5 ])
+lbA_new = np.array([ -2.0 ])
+ubA_new = np.array([ 1.0 ])
+
+#  Setting up SQProblem object and solution analyser.
+example = SQProblem(2, 1)
+analyser = SolutionAnalysis()
+
+#  Solve first QP ...
+nWSR = 10
+example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
+
+#  ... and analyse it.
+maxKKTviolation = np.zeros(1)
+analyser.getMaxKKTviolation(example, maxKKTviolation)
+print("maxKKTviolation: %e\n"%maxKKTviolation)
+
+#  Solve second QP ...
+nWSR = 10;
+example.hotstart(H_new, g_new, A_new, lb_new, ub_new,
+                 lbA_new, ubA_new, nWSR)
+
+#  ... and analyse it.
+analyser.getMaxKKTviolation(example, maxKKTviolation)
+print("maxKKTviolation: %e\n"%maxKKTviolation)
+
+
+#  ------------ VARIANCE-COVARIANCE EVALUATION --------------------
+
+Var             = np.zeros(5*5)
+Primal_Dual_Var = np.zeros(5*5)
+
+Var.reshape((5,5))[0,0] = 1.
+Var.reshape((5,5))[1,1] = 1.
+
+#                  (  1   0   0   0   0   )
+#                  (  0   1   0   0   0   )
+#     Var     =    (  0   0   0   0   0   )
+#                  (  0   0   0   0   0   )
+#                  (  0   0   0   0   0   )
+
+
+analyser.getVarianceCovariance(example, Var, Primal_Dual_Var)
+print('Primal_Dual_Var=\n', Primal_Dual_Var.reshape((5,5)))
+
+print(maxKKTviolation)
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/index.html
new file mode 100644
index 00000000000..af694d2f6b3
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/index.html
@@ -0,0 +1,11 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces/python/examples</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/interfaces/python/examples</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="cython/">cython/</a></li>
+  <li><a href="example1.py">example1.py</a></li>
+  <li><a href="example1b.py">example1b.py</a></li>
+  <li><a href="example2.py">example2.py</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/index.html
new file mode 100644
index 00000000000..75e2a483c3d
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/index.html
@@ -0,0 +1,13 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces/python</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/interfaces/python</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="README.rst">README.rst</a></li>
+  <li><a href="examples/">examples/</a></li>
+  <li><a href="qpoases.pxd">qpoases.pxd</a></li>
+  <li><a href="qpoases.pyx">qpoases.pyx</a></li>
+  <li><a href="setup.py">setup.py</a></li>
+  <li><a href="tests/">tests/</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/qpoases.pxd b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/qpoases.pxd
new file mode 100644
index 00000000000..60fcf99426a
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/qpoases.pxd
@@ -0,0 +1,476 @@
+##
+##  This file is part of qpOASES.
+##
+##  qpOASES -- An Implementation of the Online Active Set Strategy.
+##  Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##  Christian Kirches et al. All rights reserved.
+##
+##  qpOASES 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.
+##
+##  qpOASES 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 qpOASES; if not, write to the Free Software
+##  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+##
+##    Filename:  qpoases.pxd
+##    Author:    Sebastian F. Walter, Manuel Kudruss
+##    Version:   3.0
+##    Date:      2013-2014
+##
+
+cdef extern from "qpOASES.hpp" namespace "qpOASES":
+
+    ctypedef double real_t
+
+    cdef enum BooleanType:
+
+        BT_FALSE
+        BT_TRUE
+
+    cdef enum PrintLevel:
+
+        PL_DEBUG_ITER = -2
+        PL_TABULAR
+        PL_NONE
+        PL_LOW
+        PL_MEDIUM
+        PL_HIGH
+
+    cdef enum VisibilityStatus:
+
+        VS_HIDDEN
+        VS_VISIBLE
+
+    cdef enum QProblemStatus:
+
+        QPS_NOTINITIALISED
+        QPS_PREPARINGAUXILIARYQP
+
+        QPS_AUXILIARYQPSOLVED
+
+        QPS_PERFORMINGHOMOTOPY
+
+        QPS_HOMOTOPYQPSOLVED
+        QPS_SOLVED
+
+    cdef enum HessianType:
+        HST_ZERO
+        HST_IDENTITY
+        HST_POSDEF
+        HST_POSDEF_NULLSPACE
+        HST_SEMIDEF
+        HST_INDEF
+        HST_UNKNOWN
+
+    cdef enum SubjectToType:
+
+        ST_UNBOUNDED
+        ST_BOUNDED
+        ST_EQUALITY
+        ST_DISABLED
+        ST_UNKNOWN
+
+    cdef enum SubjectToStatus:
+
+        ST_LOWER = -1
+        ST_INACTIVE
+        ST_UPPER
+        ST_INFEASIBLE_LOWER
+        ST_INFEASIBLE_UPPER
+        ST_UNDEFINED
+
+    cdef enum  returnValue:
+        TERMINAL_LIST_ELEMENT = -1
+        SUCCESSFUL_RETURN = 0
+        RET_DIV_BY_ZERO
+        RET_INDEX_OUT_OF_BOUNDS
+        RET_INVALID_ARGUMENTS
+        RET_ERROR_UNDEFINED
+        RET_WARNING_UNDEFINED
+        RET_INFO_UNDEFINED
+        RET_EWI_UNDEFINED
+        RET_AVAILABLE_WITH_LINUX_ONLY
+        RET_UNKNOWN_BUG
+        RET_PRINTLEVEL_CHANGED
+        RET_NOT_YET_IMPLEMENTED
+        RET_INDEXLIST_MUST_BE_REORDERD
+        RET_INDEXLIST_EXCEEDS_MAX_LENGTH
+        RET_INDEXLIST_CORRUPTED
+        RET_INDEXLIST_OUTOFBOUNDS
+        RET_INDEXLIST_ADD_FAILED
+        RET_INDEXLIST_INTERSECT_FAILED
+        RET_INDEX_ALREADY_OF_DESIRED_STATUS
+        RET_ADDINDEX_FAILED
+        RET_REMOVEINDEX_FAILED
+        RET_SWAPINDEX_FAILED
+        RET_NOTHING_TO_DO
+        RET_SETUP_BOUND_FAILED
+        RET_SETUP_CONSTRAINT_FAILED
+        RET_MOVING_BOUND_FAILED
+        RET_MOVING_CONSTRAINT_FAILED
+        RET_SHIFTING_FAILED
+        RET_ROTATING_FAILED
+        RET_QPOBJECT_NOT_SETUP
+        RET_QP_ALREADY_INITIALISED
+        RET_NO_INIT_WITH_STANDARD_SOLVER
+        RET_RESET_FAILED
+        RET_INIT_FAILED
+        RET_INIT_FAILED_TQ
+        RET_INIT_FAILED_CHOLESKY
+        RET_INIT_FAILED_HOTSTART
+        RET_INIT_FAILED_INFEASIBILITY
+        RET_INIT_FAILED_UNBOUNDEDNESS
+        RET_INIT_FAILED_REGULARISATION
+        RET_INIT_SUCCESSFUL
+        RET_OBTAINING_WORKINGSET_FAILED
+        RET_SETUP_WORKINGSET_FAILED
+        RET_SETUP_AUXILIARYQP_FAILED
+        RET_NO_EXTERN_SOLVER
+        RET_QP_UNBOUNDED
+        RET_QP_INFEASIBLE
+        RET_QP_NOT_SOLVED
+        RET_QP_SOLVED
+        RET_UNABLE_TO_SOLVE_QP
+        RET_INITIALISATION_STARTED
+        RET_HOTSTART_FAILED
+        RET_HOTSTART_FAILED_TO_INIT
+        RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED
+        RET_ITERATION_STARTED
+        RET_SHIFT_DETERMINATION_FAILED
+        RET_STEPDIRECTION_DETERMINATION_FAILED
+        RET_STEPLENGTH_DETERMINATION_FAILED
+        RET_OPTIMAL_SOLUTION_FOUND
+        RET_HOMOTOPY_STEP_FAILED
+        RET_HOTSTART_STOPPED_INFEASIBILITY
+        RET_HOTSTART_STOPPED_UNBOUNDEDNESS
+        RET_WORKINGSET_UPDATE_FAILED
+        RET_MAX_NWSR_REACHED
+        RET_CONSTRAINTS_NOT_SPECIFIED
+        RET_INVALID_FACTORISATION_FLAG
+        RET_UNABLE_TO_SAVE_QPDATA
+        RET_STEPDIRECTION_FAILED_TQ
+        RET_STEPDIRECTION_FAILED_CHOLESKY
+        RET_CYCLING_DETECTED
+        RET_CYCLING_NOT_RESOLVED
+        RET_CYCLING_RESOLVED
+        RET_STEPSIZE
+        RET_STEPSIZE_NONPOSITIVE
+        RET_SETUPSUBJECTTOTYPE_FAILED
+        RET_ADDCONSTRAINT_FAILED
+        RET_ADDCONSTRAINT_FAILED_INFEASIBILITY
+        RET_ADDBOUND_FAILED
+        RET_ADDBOUND_FAILED_INFEASIBILITY
+        RET_REMOVECONSTRAINT_FAILED
+        RET_REMOVEBOUND_FAILED
+        RET_REMOVE_FROM_ACTIVESET
+        RET_ADD_TO_ACTIVESET
+        RET_REMOVE_FROM_ACTIVESET_FAILED
+        RET_ADD_TO_ACTIVESET_FAILED
+        RET_CONSTRAINT_ALREADY_ACTIVE
+        RET_ALL_CONSTRAINTS_ACTIVE
+        RET_LINEARLY_DEPENDENT
+        RET_LINEARLY_INDEPENDENT
+        RET_LI_RESOLVED
+        RET_ENSURELI_FAILED
+        RET_ENSURELI_FAILED_TQ
+        RET_ENSURELI_FAILED_NOINDEX
+        RET_ENSURELI_FAILED_CYCLING
+        RET_BOUND_ALREADY_ACTIVE
+        RET_ALL_BOUNDS_ACTIVE
+        RET_CONSTRAINT_NOT_ACTIVE
+        RET_BOUND_NOT_ACTIVE
+        RET_HESSIAN_NOT_SPD
+        RET_HESSIAN_INDEFINITE
+        RET_MATRIX_SHIFT_FAILED
+        RET_MATRIX_FACTORISATION_FAILED
+        RET_PRINT_ITERATION_FAILED
+        RET_NO_GLOBAL_MESSAGE_OUTPUTFILE
+        RET_DISABLECONSTRAINTS_FAILED
+        RET_ENABLECONSTRAINTS_FAILED
+        RET_ALREADY_ENABLED
+        RET_ALREADY_DISABLED
+        RET_NO_HESSIAN_SPECIFIED
+        RET_USING_REGULARISATION
+        RET_EPS_MUST_BE_POSITVE
+        RET_REGSTEPS_MUST_BE_POSITVE
+        RET_HESSIAN_ALREADY_REGULARISED
+        RET_CANNOT_REGULARISE_IDENTITY
+        RET_CANNOT_REGULARISE_SPARSE
+        RET_NO_REGSTEP_NWSR
+        RET_FEWER_REGSTEPS_NWSR
+        RET_CHOLESKY_OF_ZERO_HESSIAN
+        RET_CONSTRAINTS_ARE_NOT_SCALED
+        RET_INITIAL_BOUNDS_STATUS_NYI
+        RET_ERROR_IN_CONSTRAINTPRODUCT
+        RET_FIX_BOUNDS_FOR_LP
+        RET_USE_REGULARISATION_FOR_LP
+        RET_UPDATEMATRICES_FAILED
+        RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED
+        RET_UNABLE_TO_OPEN_FILE
+        RET_UNABLE_TO_WRITE_FILE
+        RET_UNABLE_TO_READ_FILE
+        RET_FILEDATA_INCONSISTENT
+        RET_UNABLE_TO_ANALYSE_QPROBLEM
+        RET_OPTIONS_ADJUSTED
+        RET_NWSR_SET_TO_ONE
+        RET_UNABLE_TO_READ_BENCHMARK
+        RET_BENCHMARK_ABORTED
+        RET_INITIAL_QP_SOLVED
+        RET_QP_SOLUTION_STARTED
+        RET_BENCHMARK_SUCCESSFUL
+        RET_NO_DIAGONAL_AVAILABLE
+        RET_DIAGONAL_NOT_INITIALISED
+        RET_ENSURELI_DROPPED
+        RET_SIMPLE_STATUS_P1
+        RET_SIMPLE_STATUS_P0
+        RET_SIMPLE_STATUS_M1
+        RET_SIMPLE_STATUS_M2
+        RET_SIMPLE_STATUS_M3
+
+
+    cdef cppclass Options:
+
+        Options()
+        Options(const Options&)
+        # Options& operator=( const Options&)  # equality operator cannot be overloaded in Python
+        returnValue setToDefault()
+        returnValue setToReliable()
+        returnValue setToMPC()
+        returnValue setToFast()
+        returnValue ensureConsistency()
+        # returnValue print() # print is a reserved keyword in Python
+        returnValue copy(const Options& )
+
+        PrintLevel printLevel
+
+        BooleanType enableRamping
+        BooleanType enableFarBounds
+        BooleanType enableFlippingBounds
+        BooleanType enableRegularisation
+        BooleanType enableFullLITests
+        BooleanType enableNZCTests
+        int         enableDriftCorrection
+        int         enableCholeskyRefactorisation
+        BooleanType enableEqualities
+
+        real_t terminationTolerance
+        real_t boundTolerance
+        real_t boundRelaxation
+        real_t epsNum
+        real_t epsDen
+        real_t maxPrimalJump
+        real_t maxDualJump
+
+        real_t initialRamping
+        real_t finalRamping
+        real_t initialFarBounds
+        real_t growFarBounds
+        SubjectToStatus initialStatusBounds
+        real_t epsFlipping
+        int numRegularisationSteps
+        real_t epsRegularisation
+        int numRefinementSteps
+        real_t epsIterRef
+        real_t epsLITests
+        real_t epsNZCTests
+
+        BooleanType enableDropInfeasibles
+        int    dropBoundPriority
+        int    dropEqConPriority
+        int    dropIneqConPriority
+
+    cdef cppclass QProblemB:
+        QProblemB()
+        QProblemB(int, HessianType)
+
+        QProblemB(const QProblemB&)
+
+        returnValue init(real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         int&)
+
+        returnValue init(real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         int&,
+                         real_t*)
+
+        returnValue hotstart(real_t*,
+                             real_t*,
+                             real_t*,
+                             int&)
+
+        returnValue hotstart(real_t*,
+                             real_t*,
+                             real_t*,
+                             int&,
+                             real_t*)
+
+
+        returnValue getPrimalSolution(real_t*)
+        returnValue getDualSolution(real_t*)
+        returnValue printOptions()
+        real_t getObjVal()
+
+        Options getOptions()
+        returnValue setOptions(Options&)
+
+    cdef cppclass QProblem:
+        QProblem()
+        QProblem(int, int, HessianType)
+
+        QProblem(const QProblem&)
+
+        returnValue init(real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         int&)
+
+        returnValue init(real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         int&,
+                         real_t*)
+
+        returnValue hotstart(real_t*,
+                             real_t*,
+                             real_t*,
+                             real_t*,
+                             real_t*,
+                             int&)
+
+        returnValue hotstart(real_t*,
+                             real_t*,
+                             real_t*,
+                             real_t*,
+                             real_t*,
+                             int&,
+                             real_t*)
+
+        returnValue getPrimalSolution(real_t*)
+        returnValue getDualSolution(real_t*)
+        returnValue printOptions()
+        real_t getObjVal()
+
+        Options getOptions()
+        returnValue setOptions(Options&)
+
+
+    cdef cppclass SQProblem:
+        SQProblem()
+        SQProblem(int, int, HessianType)
+
+        SQProblem(const QProblem&)
+
+        returnValue init(real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         int&)
+
+        returnValue init(real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         real_t*,
+                         int&,
+                         real_t*)
+
+        returnValue hotstart(real_t*,
+                             real_t*,
+                             real_t*,
+                             real_t*,
+                             real_t*,
+                             real_t*,
+                             real_t*,
+                             int&)
+
+        returnValue hotstart(real_t*,
+                             real_t*,
+                             real_t*,
+                             real_t*,
+                             real_t*,
+                             real_t*,
+                             real_t*,
+                             int&,
+                             real_t*)
+
+        returnValue getPrimalSolution(real_t*)
+        returnValue getDualSolution(real_t*)
+        returnValue printOptions()
+        real_t getObjVal()
+
+        Options getOptions()
+        returnValue setOptions(Options&)
+
+
+    cdef cppclass SolutionAnalysis:
+        SolutionAnalysis()
+        SolutionAnalysis(const SolutionAnalysis&)
+        # ~SolutionAnalysis()
+        # SolutionAnalysis& operator=(const SolutionAnalysis&)
+        returnValue getMaxKKTviolation(QProblem*, real_t& )
+        returnValue getMaxKKTviolation(QProblemB*, real_t&)
+        returnValue getMaxKKTviolation(SQProblem*, real_t&)
+        returnValue getVarianceCovariance(QProblem*, real_t*, real_t*)
+        returnValue getVarianceCovariance(QProblemB*, real_t*, real_t*)
+        returnValue getVarianceCovariance(SQProblem*, real_t*, real_t*)
+
+
+cdef extern from "qpOASES/Utils.hpp" namespace "qpOASES":
+    void getKKTResidual(int nV,                  # Number of variables.
+                        int nC,                  # Number of constraints.
+                        const real_t* const H,   # Hessian matrix.
+                        const real_t* const g,   # Sequence of gradient vectors.
+                        const real_t* const A,   # Constraint matrix.
+                        const real_t* const lb,  # Sequence of lower bound vectors (on variables).
+                        const real_t* const ub,  # Sequence of upper bound vectors (on variables).
+                        const real_t* const lbA, # Sequence of lower constraints' bound vectors.
+                        const real_t* const ubA, # Sequence of upper constraints' bound vectors.
+                        const real_t* const x,   # Sequence of primal trial vectors.
+                        const real_t* const y,   # Sequence of dual trial vectors.
+                        real_t& stat,            # Maximum value of stationarity condition residual.
+                        real_t& feas,            # Maximum value of primal feasibility violation.
+                        real_t& cmpl             # Maximum value of complementarity residual.
+                        )
+
+
+cdef extern from "qpOASES/extras/OQPinterface.hpp" namespace "qpOASES":
+    returnValue runOQPbenchmark(const char* path,           # Full path of the benchmark files (without trailing slash!).
+                                BooleanType isSparse,       # Shall convert matrices to sparse format before solution?
+                                BooleanType useHotstarts,   # Shall QP solution be hotstarted?
+                                const Options& options,     # QP solver options to be used while solving benchmark problems.
+                                int maxAllowedNWSR,         # Maximum number of working set recalculations to be performed.
+                                real_t& maxNWSR,            # Output: Maximum number of performed working set recalculations.
+                                real_t& avgNWSR,            # Output: Average number of performed working set recalculations.
+                                real_t& maxCPUtime,         # Output: Maximum CPU time required for solving each QP.
+                                real_t& avgCPUtime,         # Output: Average CPU time required for solving each QP.
+                                real_t& maxStationarity,    # Output: Maximum residual of stationarity condition.
+                                real_t& maxFeasibility,     # Output: Maximum residual of primal feasibility condition.
+                                real_t& maxComplementarity  # Output: Maximum residual of complementarity condition.
+                                )
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/qpoases.pyx b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/qpoases.pyx
new file mode 100644
index 00000000000..bb011c393a2
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/qpoases.pyx
@@ -0,0 +1,779 @@
+##
+##  This file is part of qpOASES.
+##
+##  qpOASES -- An Implementation of the Online Active Set Strategy.
+##  Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##  Christian Kirches et al. All rights reserved.
+##
+##  qpOASES 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.
+##
+##  qpOASES 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 qpOASES; if not, write to the Free Software
+##  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+## author of this file: Sebastian F. Walter
+
+"""
+Python interface to qpOASES
+using Cython
+:author: Sebastian F. Walter, Manuel Kudruss
+"""
+
+import numpy as np
+cimport numpy as np
+
+from cython.operator cimport dereference as deref
+
+cimport qpoases
+
+cdef class PyBooleanType:
+    FALSE = BT_FALSE
+    TRUE  = BT_TRUE
+
+cdef class PyPrintLevel:
+    DEBUG_ITER = PL_DEBUG_ITER
+    TABULAR    = PL_TABULAR
+    NONE       = PL_NONE
+    LOW        = PL_LOW
+    MEDIUM     = PL_MEDIUM
+    HIGH       = PL_HIGH
+
+cdef class PyHessianType:
+    ZERO               = HST_ZERO
+    IDENTITY           = HST_IDENTITY
+    POSDEF             = HST_POSDEF
+    POSDEF_NULLSPACE   = HST_POSDEF_NULLSPACE
+    SEMIDEF            = HST_SEMIDEF
+    INDEF              = HST_INDEF
+    UNKNOWN            = HST_UNKNOWN
+
+cdef class PySubjectToStatus:
+    LOWER              = ST_LOWER
+    INACTIVE           = ST_INACTIVE
+    UPPER              = ST_UPPER
+    INFEASIBLE_LOWER   = ST_INFEASIBLE_LOWER
+    INFEASIBLE_UPPER   = ST_INFEASIBLE_UPPER
+    UNDEFINED          = ST_UNDEFINED
+
+cdef class PyReturnValue:
+    TERMINAL_LIST_ELEMENT                 = -1
+    SUCCESSFUL_RETURN                     = 0
+    DIV_BY_ZERO                           = RET_DIV_BY_ZERO
+    INDEX_OUT_OF_BOUNDS                   = RET_INDEX_OUT_OF_BOUNDS
+    INVALID_ARGUMENTS                     = RET_INVALID_ARGUMENTS
+    ERROR_UNDEFINED                       = RET_ERROR_UNDEFINED
+    WARNING_UNDEFINED                     = RET_WARNING_UNDEFINED
+    INFO_UNDEFINED                        = RET_INFO_UNDEFINED
+    EWI_UNDEFINED                         = RET_EWI_UNDEFINED
+    AVAILABLE_WITH_LINUX_ONLY             = RET_AVAILABLE_WITH_LINUX_ONLY
+    UNKNOWN_BUG                           = RET_UNKNOWN_BUG
+    PRINTLEVEL_CHANGED                    = RET_PRINTLEVEL_CHANGED
+    NOT_YET_IMPLEMENTED                   = RET_NOT_YET_IMPLEMENTED
+    INDEXLIST_MUST_BE_REORDERD            = RET_INDEXLIST_MUST_BE_REORDERD
+    INDEXLIST_EXCEEDS_MAX_LENGTH          = RET_INDEXLIST_EXCEEDS_MAX_LENGTH
+    INDEXLIST_CORRUPTED                   = RET_INDEXLIST_CORRUPTED
+    INDEXLIST_OUTOFBOUNDS                 = RET_INDEXLIST_OUTOFBOUNDS
+    INDEXLIST_ADD_FAILED                  = RET_INDEXLIST_ADD_FAILED
+    INDEXLIST_INTERSECT_FAILED            = RET_INDEXLIST_INTERSECT_FAILED
+    INDEX_ALREADY_OF_DESIRED_STATUS       = RET_INDEX_ALREADY_OF_DESIRED_STATUS
+    ADDINDEX_FAILED                       = RET_ADDINDEX_FAILED
+    REMOVEINDEX_FAILED                    = RET_REMOVEINDEX_FAILED
+    SWAPINDEX_FAILED                      = RET_SWAPINDEX_FAILED
+    NOTHING_TO_DO                         = RET_NOTHING_TO_DO
+    SETUP_BOUND_FAILED                    = RET_SETUP_BOUND_FAILED
+    SETUP_CONSTRAINT_FAILED               = RET_SETUP_CONSTRAINT_FAILED
+    MOVING_BOUND_FAILED                   = RET_MOVING_BOUND_FAILED
+    MOVING_CONSTRAINT_FAILED              = RET_MOVING_CONSTRAINT_FAILED
+    SHIFTING_FAILED                       = RET_SHIFTING_FAILED
+    ROTATING_FAILED                       = RET_ROTATING_FAILED
+    QPOBJECT_NOT_SETUP                    = RET_QPOBJECT_NOT_SETUP
+    QP_ALREADY_INITIALISED                = RET_QP_ALREADY_INITIALISED
+    NO_INIT_WITH_STANDARD_SOLVER          = RET_NO_INIT_WITH_STANDARD_SOLVER
+    RESET_FAILED                          = RET_RESET_FAILED
+    INIT_FAILED                           = RET_INIT_FAILED
+    INIT_FAILED_TQ                        = RET_INIT_FAILED_TQ
+    INIT_FAILED_CHOLESKY                  = RET_INIT_FAILED_CHOLESKY
+    INIT_FAILED_HOTSTART                  = RET_INIT_FAILED_HOTSTART
+    INIT_FAILED_INFEASIBILITY             = RET_INIT_FAILED_INFEASIBILITY
+    INIT_FAILED_UNBOUNDEDNESS             = RET_INIT_FAILED_UNBOUNDEDNESS
+    INIT_FAILED_REGULARISATION            = RET_INIT_FAILED_REGULARISATION
+    INIT_SUCCESSFUL                       = RET_INIT_SUCCESSFUL
+    OBTAINING_WORKINGSET_FAILED           = RET_OBTAINING_WORKINGSET_FAILED
+    SETUP_WORKINGSET_FAILED               = RET_SETUP_WORKINGSET_FAILED
+    SETUP_AUXILIARYQP_FAILED              = RET_SETUP_AUXILIARYQP_FAILED
+    NO_EXTERN_SOLVER                      = RET_NO_EXTERN_SOLVER
+    QP_UNBOUNDED                          = RET_QP_UNBOUNDED
+    QP_INFEASIBLE                         = RET_QP_INFEASIBLE
+    QP_NOT_SOLVED                         = RET_QP_NOT_SOLVED
+    QP_SOLVED                             = RET_QP_SOLVED
+    UNABLE_TO_SOLVE_QP                    = RET_UNABLE_TO_SOLVE_QP
+    INITIALISATION_STARTED                = RET_INITIALISATION_STARTED
+    HOTSTART_FAILED                       = RET_HOTSTART_FAILED
+    HOTSTART_FAILED_TO_INIT               = RET_HOTSTART_FAILED_TO_INIT
+    HOTSTART_FAILED_AS_QP_NOT_INITIALISED = RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED
+    ITERATION_STARTED                     = RET_ITERATION_STARTED
+    SHIFT_DETERMINATION_FAILED            = RET_SHIFT_DETERMINATION_FAILED
+    STEPDIRECTION_DETERMINATION_FAILED    = RET_STEPDIRECTION_DETERMINATION_FAILED
+    STEPLENGTH_DETERMINATION_FAILED       = RET_STEPLENGTH_DETERMINATION_FAILED
+    OPTIMAL_SOLUTION_FOUND                = RET_OPTIMAL_SOLUTION_FOUND
+    HOMOTOPY_STEP_FAILED                  = RET_HOMOTOPY_STEP_FAILED
+    HOTSTART_STOPPED_INFEASIBILITY        = RET_HOTSTART_STOPPED_INFEASIBILITY
+    HOTSTART_STOPPED_UNBOUNDEDNESS        = RET_HOTSTART_STOPPED_UNBOUNDEDNESS
+    WORKINGSET_UPDATE_FAILED              = RET_WORKINGSET_UPDATE_FAILED
+    MAX_NWSR_REACHED                      = RET_MAX_NWSR_REACHED
+    CONSTRAINTS_NOT_SPECIFIED             = RET_CONSTRAINTS_NOT_SPECIFIED
+    INVALID_FACTORISATION_FLAG            = RET_INVALID_FACTORISATION_FLAG
+    UNABLE_TO_SAVE_QPDATA                 = RET_UNABLE_TO_SAVE_QPDATA
+    STEPDIRECTION_FAILED_TQ               = RET_STEPDIRECTION_FAILED_TQ
+    STEPDIRECTION_FAILED_CHOLESKY         = RET_STEPDIRECTION_FAILED_CHOLESKY
+    CYCLING_DETECTED                      = RET_CYCLING_DETECTED
+    CYCLING_NOT_RESOLVED                  = RET_CYCLING_NOT_RESOLVED
+    CYCLING_RESOLVED                      = RET_CYCLING_RESOLVED
+    STEPSIZE                              = RET_STEPSIZE
+    STEPSIZE_NONPOSITIVE                  = RET_STEPSIZE_NONPOSITIVE
+    SETUPSUBJECTTOTYPE_FAILED             = RET_SETUPSUBJECTTOTYPE_FAILED
+    ADDCONSTRAINT_FAILED                  = RET_ADDCONSTRAINT_FAILED
+    ADDCONSTRAINT_FAILED_INFEASIBILITY    = RET_ADDCONSTRAINT_FAILED_INFEASIBILITY
+    ADDBOUND_FAILED                       = RET_ADDBOUND_FAILED
+    ADDBOUND_FAILED_INFEASIBILITY         = RET_ADDBOUND_FAILED_INFEASIBILITY
+    REMOVECONSTRAINT_FAILED               = RET_REMOVECONSTRAINT_FAILED
+    REMOVEBOUND_FAILED                    = RET_REMOVEBOUND_FAILED
+    REMOVE_FROM_ACTIVESET                 = RET_REMOVE_FROM_ACTIVESET
+    ADD_TO_ACTIVESET                      = RET_ADD_TO_ACTIVESET
+    REMOVE_FROM_ACTIVESET_FAILED          = RET_REMOVE_FROM_ACTIVESET_FAILED
+    ADD_TO_ACTIVESET_FAILED               = RET_ADD_TO_ACTIVESET_FAILED
+    CONSTRAINT_ALREADY_ACTIVE             = RET_CONSTRAINT_ALREADY_ACTIVE
+    ALL_CONSTRAINTS_ACTIVE                = RET_ALL_CONSTRAINTS_ACTIVE
+    LINEARLY_DEPENDENT                    = RET_LINEARLY_DEPENDENT
+    LINEARLY_INDEPENDENT                  = RET_LINEARLY_INDEPENDENT
+    LI_RESOLVED                           = RET_LI_RESOLVED
+    ENSURELI_FAILED                       = RET_ENSURELI_FAILED
+    ENSURELI_FAILED_TQ                    = RET_ENSURELI_FAILED_TQ
+    ENSURELI_FAILED_NOINDEX               = RET_ENSURELI_FAILED_NOINDEX
+    ENSURELI_FAILED_CYCLING               = RET_ENSURELI_FAILED_CYCLING
+    BOUND_ALREADY_ACTIVE                  = RET_BOUND_ALREADY_ACTIVE
+    ALL_BOUNDS_ACTIVE                     = RET_ALL_BOUNDS_ACTIVE
+    CONSTRAINT_NOT_ACTIVE                 = RET_CONSTRAINT_NOT_ACTIVE
+    BOUND_NOT_ACTIVE                      = RET_BOUND_NOT_ACTIVE
+    HESSIAN_NOT_SPD                       = RET_HESSIAN_NOT_SPD
+    HESSIAN_INDEFINITE                    = RET_HESSIAN_INDEFINITE
+    MATRIX_SHIFT_FAILED                   = RET_MATRIX_SHIFT_FAILED
+    MATRIX_FACTORISATION_FAILED           = RET_MATRIX_FACTORISATION_FAILED
+    PRINT_ITERATION_FAILED                = RET_PRINT_ITERATION_FAILED
+    NO_GLOBAL_MESSAGE_OUTPUTFILE          = RET_NO_GLOBAL_MESSAGE_OUTPUTFILE
+    DISABLECONSTRAINTS_FAILED             = RET_DISABLECONSTRAINTS_FAILED
+    ENABLECONSTRAINTS_FAILED              = RET_ENABLECONSTRAINTS_FAILED
+    ALREADY_ENABLED                       = RET_ALREADY_ENABLED
+    ALREADY_DISABLED                      = RET_ALREADY_DISABLED
+    NO_HESSIAN_SPECIFIED                  = RET_NO_HESSIAN_SPECIFIED
+    USING_REGULARISATION                  = RET_USING_REGULARISATION
+    EPS_MUST_BE_POSITVE                   = RET_EPS_MUST_BE_POSITVE
+    REGSTEPS_MUST_BE_POSITVE              = RET_REGSTEPS_MUST_BE_POSITVE
+    HESSIAN_ALREADY_REGULARISED           = RET_HESSIAN_ALREADY_REGULARISED
+    CANNOT_REGULARISE_IDENTITY            = RET_CANNOT_REGULARISE_IDENTITY
+    CANNOT_REGULARISE_SPARSE              = RET_CANNOT_REGULARISE_SPARSE
+    NO_REGSTEP_NWSR                       = RET_NO_REGSTEP_NWSR
+    FEWER_REGSTEPS_NWSR                   = RET_FEWER_REGSTEPS_NWSR
+    CHOLESKY_OF_ZERO_HESSIAN              = RET_CHOLESKY_OF_ZERO_HESSIAN
+    CONSTRAINTS_ARE_NOT_SCALED            = RET_CONSTRAINTS_ARE_NOT_SCALED
+    INITIAL_BOUNDS_STATUS_NYI             = RET_INITIAL_BOUNDS_STATUS_NYI
+    ERROR_IN_CONSTRAINTPRODUCT            = RET_ERROR_IN_CONSTRAINTPRODUCT
+    FIX_BOUNDS_FOR_LP                     = RET_FIX_BOUNDS_FOR_LP
+    USE_REGULARISATION_FOR_LP             = RET_USE_REGULARISATION_FOR_LP
+    UPDATEMATRICES_FAILED                 = RET_UPDATEMATRICES_FAILED
+    UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED= RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED
+    UNABLE_TO_OPEN_FILE                   = RET_UNABLE_TO_OPEN_FILE
+    UNABLE_TO_WRITE_FILE                  = RET_UNABLE_TO_WRITE_FILE
+    UNABLE_TO_READ_FILE                   = RET_UNABLE_TO_READ_FILE
+    FILEDATA_INCONSISTENT                 = RET_FILEDATA_INCONSISTENT
+    UNABLE_TO_ANALYSE_QPROBLEM            = RET_UNABLE_TO_ANALYSE_QPROBLEM
+    OPTIONS_ADJUSTED                      = RET_OPTIONS_ADJUSTED
+    NWSR_SET_TO_ONE                       = RET_NWSR_SET_TO_ONE
+    UNABLE_TO_READ_BENCHMARK              = RET_UNABLE_TO_READ_BENCHMARK
+    BENCHMARK_ABORTED                     = RET_BENCHMARK_ABORTED
+    INITIAL_QP_SOLVED                     = RET_INITIAL_QP_SOLVED
+    QP_SOLUTION_STARTED                   = RET_QP_SOLUTION_STARTED
+    BENCHMARK_SUCCESSFUL                  = RET_BENCHMARK_SUCCESSFUL
+    NO_DIAGONAL_AVAILABLE                 = RET_NO_DIAGONAL_AVAILABLE
+    DIAGONAL_NOT_INITIALISED              = RET_DIAGONAL_NOT_INITIALISED
+    ENSURELI_DROPPED                      = RET_ENSURELI_DROPPED
+    SIMPLE_STATUS_P1                      = RET_SIMPLE_STATUS_P1
+    SIMPLE_STATUS_P0                      = RET_SIMPLE_STATUS_P0
+    SIMPLE_STATUS_M1                      = RET_SIMPLE_STATUS_M1
+    SIMPLE_STATUS_M2                      = RET_SIMPLE_STATUS_M2
+    SIMPLE_STATUS_M3                      = RET_SIMPLE_STATUS_M3
+
+
+
+cdef class PyOptions:
+    cdef Options *thisptr      # hold a C++ instance which we're wrapping
+    def __cinit__(self):
+        # FIXME: add support for the other constructors
+        self.thisptr = new Options()
+
+    def __dealloc__(self):
+        del self.thisptr
+
+    def setToDefault(self):
+        return self.thisptr.setToDefault()
+
+    def setToReliable(self):
+        return self.thisptr.setToReliable()
+
+    def setToMPC(self):
+        return self.thisptr.setToMPC()
+
+    def setToFast(self):
+        return self.thisptr.setToFast()
+
+    def ensureConsistency(self):
+        return self.thisptr.ensureConsistency()
+
+    property printLevel:
+        def __get__(self): return self.thisptr.printLevel
+        def __set__(self, printLevel): self.thisptr.printLevel = printLevel
+
+    property enableRamping:
+        def __get__(self): return self.thisptr.enableRamping
+        def __set__(self, enableRamping): self.thisptr.enableRamping = enableRamping
+
+    property enableFarBounds:
+        def __get__(self): return self.thisptr.enableFarBounds
+        def __set__(self, enableFarBounds): self.thisptr.enableFarBounds = enableFarBounds
+
+    property enableFlippingBounds:
+        def __get__(self): return self.thisptr.enableFlippingBounds
+        def __set__(self, enableFlippingBounds): self.thisptr.enableFlippingBounds = enableFlippingBounds
+
+    property enableRegularisation:
+        def __get__(self): return self.thisptr.enableRegularisation
+        def __set__(self, enableRegularisation): self.thisptr.enableRegularisation = enableRegularisation
+
+    property enableFullLITests:
+        def __get__(self): return self.thisptr.enableFullLITests
+        def __set__(self, enableFullLITests): self.thisptr.enableFullLITests = enableFullLITests
+
+    property enableNZCTests:
+        def __get__(self): return self.thisptr.enableNZCTests
+        def __set__(self, enableNZCTests): self.thisptr.enableNZCTests = enableNZCTests
+
+    property enableDriftCorrection:
+        def __get__(self): return self.thisptr.enableDriftCorrection
+        def __set__(self, enableDriftCorrection): self.thisptr.enableDriftCorrection = enableDriftCorrection
+
+    property enableCholeskyRefactorisation:
+        def __get__(self): return self.thisptr.enableCholeskyRefactorisation
+        def __set__(self, enableCholeskyRefactorisation): self.thisptr.enableCholeskyRefactorisation = enableCholeskyRefactorisation
+
+    property enableEqualities:
+        def __get__(self): return self.thisptr.enableEqualities
+        def __set__(self, enableEqualities): self.thisptr.enableEqualities = enableEqualities
+
+    property terminationTolerance:
+        def __get__(self): return self.thisptr.terminationTolerance
+        def __set__(self, terminationTolerance): self.thisptr.terminationTolerance = terminationTolerance
+
+    property boundTolerance:
+        def __get__(self): return self.thisptr.boundTolerance
+        def __set__(self, boundTolerance): self.thisptr.boundTolerance = boundTolerance
+
+    property boundRelaxation:
+        def __get__(self): return self.thisptr.boundRelaxation
+        def __set__(self, boundRelaxation): self.thisptr.boundRelaxation = boundRelaxation
+
+    property epsNum:
+        def __get__(self): return self.thisptr.epsNum
+        def __set__(self, epsNum): self.thisptr.epsNum = epsNum
+
+    property epsDen:
+        def __get__(self): return self.thisptr.epsDen
+        def __set__(self, epsDen): self.thisptr.epsDen = epsDen
+
+    property maxPrimalJump:
+        def __get__(self): return self.thisptr.maxPrimalJump
+        def __set__(self, maxPrimalJump): self.thisptr.maxPrimalJump = maxPrimalJump
+
+    property maxDualJump:
+        def __get__(self): return self.thisptr.maxDualJump
+        def __set__(self, maxDualJump): self.thisptr.maxDualJump = maxDualJump
+
+    property initialRamping:
+        def __get__(self): return self.thisptr.initialRamping
+        def __set__(self, initialRamping): self.thisptr.initialRamping = initialRamping
+
+    property finalRamping:
+        def __get__(self): return self.thisptr.finalRamping
+        def __set__(self, finalRamping): self.thisptr.finalRamping = finalRamping
+
+    property initialFarBounds:
+        def __get__(self): return self.thisptr.initialFarBounds
+        def __set__(self, initialFarBounds): self.thisptr.initialFarBounds = initialFarBounds
+
+    property growFarBounds:
+        def __get__(self): return self.thisptr.growFarBounds
+        def __set__(self, growFarBounds): self.thisptr.growFarBounds = growFarBounds
+
+    property initialStatusBounds:
+        def __get__(self): return self.thisptr.initialStatusBounds
+        def __set__(self, initialStatusBounds): self.thisptr.initialStatusBounds = initialStatusBounds
+
+    property epsFlipping:
+        def __get__(self): return self.thisptr.epsFlipping
+        def __set__(self, epsFlipping): self.thisptr.epsFlipping = epsFlipping
+
+    property numRegularisationSteps:
+        def __get__(self): return self.thisptr.numRegularisationSteps
+        def __set__(self, numRegularisationSteps): self.thisptr.numRegularisationSteps = numRegularisationSteps
+
+    property epsRegularisation:
+        def __get__(self): return self.thisptr.epsRegularisation
+        def __set__(self, epsRegularisation): self.thisptr.epsRegularisation = epsRegularisation
+
+    property numRefinementSteps:
+        def __get__(self): return self.thisptr.numRefinementSteps
+        def __set__(self, numRefinementSteps): self.thisptr.numRefinementSteps = numRefinementSteps
+
+    property epsIterRef:
+        def __get__(self): return self.thisptr.epsIterRef
+        def __set__(self, epsIterRef): self.thisptr.epsIterRef = epsIterRef
+
+    property epsLITests:
+        def __get__(self): return self.thisptr.epsLITests
+        def __set__(self, epsLITests): self.thisptr.epsLITests = epsLITests
+
+    property epsNZCTests:
+        def __get__(self): return self.thisptr.epsNZCTests
+        def __set__(self, epsNZCTests): self.thisptr.epsNZCTests = epsNZCTests
+
+    property dropBoundPriority:
+        def __get__(self): return self.thisptr.dropBoundPriority
+        def __set__(self, dropBoundPriority): self.thisptr.dropBoundPriority = dropBoundPriority
+
+    property dropEqConPriority:
+        def __get__(self): return self.thisptr.dropEqConPriority
+        def __set__(self, dropEqConPriority): self.thisptr.dropEqConPriority = dropEqConPriority
+
+    property dropIneqConPriority:
+        def __get__(self): return self.thisptr.dropIneqConPriority
+        def __set__(self, dropIneqConPriority): self.thisptr.dropIneqConPriority = dropIneqConPriority
+
+
+
+cdef class PyQProblemB:
+    cdef QProblemB *thisptr      # hold a C++ instance which we're wrapping
+    def __cinit__(self, int nV):
+        # FIXME: allow other HessianTypes!
+        self.thisptr = new QProblemB(nV, HST_UNKNOWN)
+    def __dealloc__(self):
+        del self.thisptr
+
+    def init(self,
+             np.ndarray[np.double_t, ndim=2] H,
+             np.ndarray[np.double_t, ndim=1] g,
+             np.ndarray[np.double_t, ndim=1] lb,
+             np.ndarray[np.double_t, ndim=1] ub,
+             nWSR,
+             double cputime=0):
+
+        # FIXME: add asserts
+
+        if cputime > 1.e-16:
+            return self.thisptr.init(
+                    <double*> H.data,
+                    <double*> g.data,
+                    <double*> lb.data,
+                    <double*> ub.data,
+                    nWSR,
+                    &cputime
+                    )
+
+        return self.thisptr.init(
+                    <double*> H.data,
+                    <double*> g.data,
+                    <double*> lb.data,
+                    <double*> ub.data,
+                    nWSR)
+
+    def hotstart(self,
+             np.ndarray[np.double_t, ndim=1] g,
+             np.ndarray[np.double_t, ndim=1] lb,
+             np.ndarray[np.double_t, ndim=1] ub,
+             nWSR,
+             double cputime=0):
+
+        # FIXME: add asserts
+
+        if cputime > 1.e-16:
+            return self.thisptr.hotstart(
+                    <double*> g.data,
+                    <double*> lb.data,
+                    <double*> ub.data,
+                    nWSR,
+                    &cputime
+                    )
+
+        return self.thisptr.hotstart(
+                    <double*> g.data,
+                    <double*> lb.data,
+                    <double*> ub.data,
+                    nWSR)
+
+    def getPrimalSolution(self, np.ndarray[np.double_t, ndim=1] xOpt):
+        return self.thisptr.getPrimalSolution(<double*> xOpt.data)
+
+    def getDualSolution(self, np.ndarray[np.double_t, ndim=1] yOpt):
+        return self.thisptr.getDualSolution(<double*> yOpt.data)
+
+    def getObjVal(self):
+        return self.thisptr.getObjVal()
+
+    def printOptions(self):
+        return self.thisptr.printOptions()
+
+    def getOptions(self):
+        # FIXME: memory management? who deallocates o
+        cdef Options *o = new Options(self.thisptr.getOptions())
+        retval = PyOptions()
+        retval.thisptr = o
+        return retval
+
+    def setOptions(self, PyOptions options):
+        self.thisptr.setOptions(deref(options.thisptr))
+
+
+cdef class PyQProblem:
+    cdef QProblem *thisptr      # hold a C++ instance which we're wrapping
+    def __cinit__(self, int nV, int nC):
+        self.thisptr = new QProblem(nV, nC, HST_UNKNOWN)
+    def __dealloc__(self):
+        del self.thisptr
+
+    cpdef init(self,
+             np.ndarray[np.double_t, ndim=2] H,
+             np.ndarray[np.double_t, ndim=1] g,
+             np.ndarray[np.double_t, ndim=2] A,
+             np.ndarray[np.double_t, ndim=1] lb,
+             np.ndarray[np.double_t, ndim=1] ub,
+             np.ndarray[np.double_t, ndim=1] lbA,
+             np.ndarray[np.double_t, ndim=1] ubA,
+             nWSR,
+             double cputime=0):
+
+        # FIXME: add asserts
+
+        if cputime > 1.e-16:
+            return self.thisptr.init(
+                    <double*> H.data,
+                    <double*> g.data,
+                    <double*> A.data,
+                    <double*> lb.data,
+                    <double*> ub.data,
+                    <double*> lbA.data,
+                    <double*> ubA.data,
+                    nWSR,
+                    &cputime)
+
+        return self.thisptr.init(
+                    <double*> H.data,
+                    <double*> g.data,
+                    <double*> A.data,
+                    <double*> lb.data,
+                    <double*> ub.data,
+                    <double*> lbA.data,
+                    <double*> ubA.data,
+                    nWSR)
+
+    cpdef hotstart(self,
+             np.ndarray[np.double_t, ndim=1] g,
+             np.ndarray[np.double_t, ndim=1] lb,
+             np.ndarray[np.double_t, ndim=1] ub,
+             np.ndarray[np.double_t, ndim=1] lbA,
+             np.ndarray[np.double_t, ndim=1] ubA,
+             nWSR,
+             double cputime=0):
+
+        # FIXME: add asserts
+
+        if cputime > 1.e-16:
+            return self.thisptr.hotstart(
+                    <double*> g.data,
+                    <double*> lb.data,
+                    <double*> ub.data,
+                    <double*> lbA.data,
+                    <double*> ubA.data,
+                    nWSR,
+                    &cputime
+                    )
+
+        return self.thisptr.hotstart(
+                    <double*> g.data,
+                    <double*> lb.data,
+                    <double*> ub.data,
+                    <double*> lbA.data,
+                    <double*> ubA.data,
+                    nWSR)
+
+    cpdef getPrimalSolution(self, np.ndarray[np.double_t, ndim=1] xOpt):
+        return self.thisptr.getPrimalSolution(<double*> xOpt.data)
+
+    cpdef getDualSolution(self, np.ndarray[np.double_t, ndim=1] yOpt):
+        return self.thisptr.getDualSolution(<double*> yOpt.data)
+
+    cpdef getObjVal(self):
+        return self.thisptr.getObjVal()
+
+    cpdef printOptions(self):
+        return self.thisptr.printOptions()
+
+    cpdef setOptions(self, PyOptions options):
+        self.thisptr.setOptions(deref(options.thisptr))
+
+cdef class PySQProblem:
+    cdef SQProblem *thisptr      # hold a C++ instance which we're wrapping
+    def __cinit__(self, int nV, int nC):
+        self.thisptr = new SQProblem(nV, nC, HST_UNKNOWN)
+    def __dealloc__(self):
+        del self.thisptr
+
+    cpdef init(self,
+             np.ndarray[np.double_t, ndim=2] H,
+             np.ndarray[np.double_t, ndim=1] g,
+             np.ndarray[np.double_t, ndim=2] A,
+             np.ndarray[np.double_t, ndim=1] lb,
+             np.ndarray[np.double_t, ndim=1] ub,
+             np.ndarray[np.double_t, ndim=1] lbA,
+             np.ndarray[np.double_t, ndim=1] ubA,
+             int    nWSR,
+             double cputime=0):
+
+        # FIXME: add asserts
+
+        if cputime > 1.e-16:
+            return self.thisptr.init(
+                        <double*> H.data,
+                        <double*> g.data,
+                        <double*> A.data,
+                        <double*> lb.data,
+                        <double*> ub.data,
+                        <double*> lbA.data,
+                        <double*> ubA.data,
+                        nWSR,
+                        &cputime
+            )
+
+        return self.thisptr.init(
+                    <double*> H.data,
+                    <double*> g.data,
+                    <double*> A.data,
+                    <double*> lb.data,
+                    <double*> ub.data,
+                    <double*> lbA.data,
+                    <double*> ubA.data,
+                    nWSR
+        )
+
+    cpdef hotstart(self,
+             np.ndarray[np.double_t, ndim=2] H,
+             np.ndarray[np.double_t, ndim=1] g,
+             np.ndarray[np.double_t, ndim=2] A,
+             np.ndarray[np.double_t, ndim=1] lb,
+             np.ndarray[np.double_t, ndim=1] ub,
+             np.ndarray[np.double_t, ndim=1] lbA,
+             np.ndarray[np.double_t, ndim=1] ubA,
+             int    nWSR,
+             double cputime=0):
+
+        # FIXME: add asserts
+
+        if cputime > 1.e-16:
+            return self.thisptr.hotstart(
+                    <double*> H.data,
+                    <double*> g.data,
+                    <double*> A.data,
+                    <double*> lb.data,
+                    <double*> ub.data,
+                    <double*> lbA.data,
+                    <double*> ubA.data,
+                    nWSR,
+                    &cputime
+            )
+
+        return self.thisptr.hotstart(
+                    <double*> H.data,
+                    <double*> g.data,
+                    <double*> A.data,
+                    <double*> lb.data,
+                    <double*> ub.data,
+                    <double*> lbA.data,
+                    <double*> ubA.data,
+                    nWSR
+        )
+
+    cpdef getPrimalSolution(self, np.ndarray[np.double_t, ndim=1] xOpt):
+        return self.thisptr.getPrimalSolution(<double*> xOpt.data)
+
+    cpdef getDualSolution(self, np.ndarray[np.double_t, ndim=1] yOpt):
+        return self.thisptr.getDualSolution(<double*> yOpt.data)
+
+    cpdef getObjVal(self):
+        return self.thisptr.getObjVal()
+
+    cpdef printOptions(self):
+        return self.thisptr.printOptions()
+
+    cpdef setOptions(self, PyOptions options):
+        self.thisptr.setOptions(deref(options.thisptr))
+
+
+cdef class PySolutionAnalysis:
+    cdef SolutionAnalysis *thisptr      # hold a C++ instance which we're wrapping
+    def __cinit__(self):
+        self.thisptr = new SolutionAnalysis()
+    def __dealloc__(self):
+        del self.thisptr
+
+    cpdef getMaxKKTviolation(self, qp, np.ndarray[np.double_t, ndim=1] maxKKTviolation):
+
+        if isinstance(qp, PyQProblemB):
+            return self._getMaxKKTviolation_QProblemB(qp, maxKKTviolation)
+
+        elif isinstance(qp, PyQProblem):
+            return self._getMaxKKTviolation_QProblem(qp, maxKKTviolation)
+
+        elif isinstance(qp, PySQProblem):
+            return self._getMaxKKTviolation_SQProblem(qp, maxKKTviolation)
+
+        else:
+            raise ValueError('argument 1 must be QProblemB, QProblem or SQProblem')
+
+
+    cpdef _getMaxKKTviolation_QProblemB(self, PyQProblemB qp, np.ndarray[np.double_t, ndim=1] maxKKTviolation):
+        return self.thisptr.getMaxKKTviolation(qp.thisptr, <double&> maxKKTviolation.data[0])
+
+    cpdef _getMaxKKTviolation_QProblem(self, PyQProblem qp, np.ndarray[np.double_t, ndim=1] maxKKTviolation):
+        return self.thisptr.getMaxKKTviolation(qp.thisptr, <double&> maxKKTviolation.data[0])
+
+    cpdef _getMaxKKTviolation_SQProblem(self, PySQProblem qp, np.ndarray[np.double_t, ndim=1] maxKKTviolation):
+        return self.thisptr.getMaxKKTviolation(qp.thisptr, <double&> maxKKTviolation.data[0])
+
+
+
+    cpdef getVarianceCovariance(self,
+                              qp,
+                              np.ndarray[np.double_t, ndim=1] g_b_bA_VAR,
+                              np.ndarray[np.double_t, ndim=1] Primal_Dual_VAR ):
+
+        if isinstance(qp, PyQProblemB):
+            return self._getVarianceCovariance_QProblemB(qp, g_b_bA_VAR, Primal_Dual_VAR)
+
+        elif isinstance(qp, PyQProblem):
+            return self._getVarianceCovariance_QProblem(qp, g_b_bA_VAR, Primal_Dual_VAR)
+
+        elif isinstance(qp, PySQProblem):
+            return self._getVarianceCovariance_SQProblem(qp, g_b_bA_VAR, Primal_Dual_VAR)
+
+        else:
+            raise ValueError('argument 1 must be QProblemB, QProblem or SQProblem')
+
+    cpdef _getVarianceCovariance_QProblemB(self,
+                              PyQProblemB qp,
+                              np.ndarray[np.double_t, ndim=1] g_b_bA_VAR,
+                              np.ndarray[np.double_t, ndim=1] Primal_Dual_VAR ):
+        return self.thisptr.getVarianceCovariance(qp.thisptr,
+                                                  <double*> g_b_bA_VAR.data,
+                                                  <double*> Primal_Dual_VAR.data)
+
+    cpdef _getVarianceCovariance_QProblem(self,
+                              PyQProblem qp,
+                              np.ndarray[np.double_t, ndim=1] g_b_bA_VAR,
+                              np.ndarray[np.double_t, ndim=1] Primal_Dual_VAR ):
+        return self.thisptr.getVarianceCovariance(qp.thisptr,
+                                                  <double*> g_b_bA_VAR.data,
+                                                  <double*> Primal_Dual_VAR.data)
+
+    cpdef _getVarianceCovariance_SQProblem(self,
+                              PySQProblem qp,
+                              np.ndarray[np.double_t, ndim=1] g_b_bA_VAR,
+                              np.ndarray[np.double_t, ndim=1] Primal_Dual_VAR ):
+        return self.thisptr.getVarianceCovariance(qp.thisptr,
+                                                  <double*> g_b_bA_VAR.data,
+                                                  <double*> Primal_Dual_VAR.data)
+
+# Wrapped some utility functions for unit testing
+cpdef py_runOQPbenchmark(path,               # Full path of the benchmark files (without trailing slash!).
+                       isSparse,           # Shall convert matrices to sparse format before solution?
+                       useHotstarts,       # Shall QP solution be hotstarted?
+                       PyOptions options,  # QP solver options to be used while solving benchmark problems.
+                       int maxAllowedNWSR, # Maximum number of working set recalculations to be performed.
+                       double maxCPUTime,  # Maximum allowed CPU time for qp solving.
+                       ):
+    """run a QP benchmark example"""
+    maxNWSR            = 0.0 # Output: Maximum number of performed working set recalculations.
+    avgNWSR            = 0.0 # Output: Average number of performed working set recalculations.
+    maxCPUtime         = 0.0 # Output: Maximum CPU time required for solving each QP.
+    avgCPUtime         = 0.0 # Output: Average CPU time required for solving each QP.
+    maxStationarity    = 0.0 # Output: Maximum residual of stationarity condition.
+    maxFeasibility     = 0.0 # Output: Maximum residual of primal feasibility condition.
+    maxComplementarity = 0.0 # Output: Maximum residual of complementarity condition.
+
+    maxCPUtime = maxCPUTime
+
+    p = path.encode()
+    returnValue = runOQPbenchmark(p,
+                                  isSparse,
+                                  useHotstarts,
+                                  deref(options.thisptr),
+                                  maxAllowedNWSR,
+                                  maxNWSR,
+                                  avgNWSR,
+                                  maxCPUtime,
+                                  avgCPUtime,
+                                  maxStationarity,
+                                  maxFeasibility,
+                                  maxComplementarity)
+
+    return returnValue, maxNWSR, avgNWSR, maxCPUtime, avgCPUtime, \
+           maxStationarity, maxFeasibility, maxComplementarity
+
+
+def py_getKKTResidual(int nV,                              # Number of variables.
+                      int nC,                              # Number of constraints.
+                      np.ndarray[np.double_t, ndim=2] H,   # Hessian matrix.
+                      np.ndarray[np.double_t, ndim=1] g,   # Sequence of gradient vectors.
+                      np.ndarray[np.double_t, ndim=2] A,   # Constraint matrix.
+                      np.ndarray[np.double_t, ndim=1] lb,  # Sequence of lower bound vectors (on variables).
+                      np.ndarray[np.double_t, ndim=1] ub,  # Sequence of upper bound vectors (on variables).
+                      np.ndarray[np.double_t, ndim=1] lbA, # Sequence of lower constraints' bound vectors.
+                      np.ndarray[np.double_t, ndim=1] ubA, # Sequence of upper constraints' bound vectors.
+                      np.ndarray[np.double_t, ndim=1] x,   # Sequence of primal trial vectors.
+                      np.ndarray[np.double_t, ndim=1] y,   # Sequence of dual trial vectors.
+                      ):
+    stat = 0.0 # Maximum value of stationarity condition residual.
+    feas = 0.0 # Maximum value of primal feasibility violation.
+    cmpl = 0.0 # Maximum value of complementarity residual.
+    getKKTResidual(nV,
+                   nC,
+                   <double*> H.data,
+                   <double*> g.data,
+                   <double*> A.data,
+                   <double*> lb.data,
+                   <double*> ub.data,
+                   <double*> lbA.data,
+                   <double*> ubA.data,
+                   <double*> x.data,
+                   <double*> y.data,
+                   stat,
+                   feas,
+                   cmpl
+                   )
+    return stat, feas, cmpl
+
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/setup.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/setup.py
new file mode 100644
index 00000000000..e338fc993ad
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/setup.py
@@ -0,0 +1,72 @@
+#!/usr/bin/env python
+
+##
+##  This file is part of qpOASES.
+##
+##  qpOASES -- An Implementation of the Online Active Set Strategy.
+##  Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##  Christian Kirches et al. All rights reserved.
+##
+##  qpOASES 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.
+##
+##  qpOASES 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 qpOASES; if not, write to the Free Software
+##  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+##
+##	Filename:  setup.py
+##	Author:    Sebastian F. Walter, Manuel Kudruss
+##	Version:   3.0
+##	Date:      2013-2014
+##
+
+
+from distutils.core import setup
+from distutils.extension import Extension
+from Cython.Distutils import build_ext
+from Cython.Build import cythonize
+
+import os
+import numpy as np
+
+BASEDIR = os.path.dirname(os.path.abspath(__file__))
+BASEDIR = os.path.dirname(BASEDIR)
+BASEDIR = os.path.dirname(BASEDIR)
+print(('BASEDIR=', BASEDIR))
+
+extra_params = {}
+extra_params['include_dirs'] = [
+    '/usr/include',
+    os.path.join(BASEDIR, 'include'),
+    os.path.join(BASEDIR, 'include', 'qpOASES'),
+    np.get_include()]
+extra_params['extra_compile_args'] = ["-O2", "-Wno-unused-variable"]
+extra_params['extra_link_args'] = ["-Wl,-O1", "-Wl,--as-needed"]
+
+extra_params = extra_params.copy()
+extra_params['libraries'] = ['qpOASES']
+
+extra_params['library_dirs'] = ['/usr/lib', os.path.join(BASEDIR, 'bin')]
+extra_params['language'] = 'c++'
+
+if os.name == 'posix':
+	extra_params['runtime_library_dirs'] = extra_params['library_dirs']
+
+ext_modules = [
+    Extension("qpoases",  ["qpoases.pyx", "qpoases.pxd"],   **extra_params),
+]
+
+setup(
+    name='qpOASES interface',
+    cmdclass={'build_ext': build_ext},
+    ext_modules=cythonize(ext_modules),
+)
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/__init__.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/__init__.py
new file mode 100644
index 00000000000..8b137891791
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/__init__.py
@@ -0,0 +1 @@
+
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/index.html
new file mode 100644
index 00000000000..2adccecf074
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/index.html
@@ -0,0 +1,11 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces/python/tests</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/interfaces/python/tests</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="__init__.py">__init__.py</a></li>
+  <li><a href="test_examples.py">test_examples.py</a></li>
+  <li><a href="test_idhessian.py">test_idhessian.py</a></li>
+  <li><a href="test_testbench.py">test_testbench.py</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_examples.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_examples.py
new file mode 100644
index 00000000000..b826ef08868
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_examples.py
@@ -0,0 +1,328 @@
+"""
+This file is part of qpOASES.
+
+qpOASES -- An Implementation of the Online Active Set Strategy.
+Copyright (C) 2007-2009 by Hans Joachim Ferreau et al. All rights reserved.
+
+qpOASES 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.
+
+qpOASES 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 qpOASES; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+
+author Manuel Kudruss
+version 3.0
+date 2013-2014
+"""
+
+import os
+import re
+import numpy as np
+from numpy.testing import *
+from subprocess import Popen, PIPE, STDOUT
+
+from qpoases import PyQProblem as QProblem
+from qpoases import PyQProblemB as QProblemB
+from qpoases import PySQProblem as SQProblem
+from qpoases import PySolutionAnalysis as SolutionAnalysis
+from qpoases import PyBooleanType as BooleanType
+from qpoases import PySubjectToStatus as SubjectToStatus
+from qpoases import PyOptions as Options
+from qpoases import PyPrintLevel as PrintLevel
+
+# get qpOASES path
+qpoases_path = os.path.dirname(os.path.abspath(__file__))
+qpoases_path = os.path.dirname(qpoases_path)
+qpoases_path = os.path.dirname(qpoases_path)
+qpoases_path = os.path.dirname(qpoases_path)
+
+# set qpOASES binary path
+bin_path = os.path.join(qpoases_path, "bin")
+
+class TestExamples(TestCase):
+
+    def test_example1(self):
+        return 0
+        # Example for qpOASES main function using the QProblem class.
+        #Setup data of first QP.
+
+        H   = np.array([1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
+        A   = np.array([1.0, 1.0 ]).reshape((2,1))
+        g   = np.array([1.5, 1.0 ])
+        lb  = np.array([0.5, -2.0])
+        ub  = np.array([5.0, 2.0 ])
+        lbA = np.array([-1.0 ])
+        ubA = np.array([2.0])
+
+        # Setup data of second QP.
+
+        g_new   = np.array([1.0, 1.5])
+        lb_new  = np.array([0.0, -1.0])
+        ub_new  = np.array([5.0, -0.5])
+        lbA_new = np.array([-2.0])
+        ubA_new = np.array([1.0])
+
+        # Setting up QProblemB object.
+        qp = QProblem(2, 1)
+        options = Options()
+        options.printLevel = PrintLevel.NONE
+        qp.setOptions(options)
+
+        # Solve first QP.
+        nWSR = 10
+        qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
+
+        # Solve second QP.
+        nWSR = 10
+        qp.hotstart(g_new, lb_new, ub_new, lbA_new, ubA_new, nWSR)
+
+        # Get and print solution of second QP.
+        xOpt_actual = np.zeros(2)
+        qp.getPrimalSolution(xOpt_actual)
+        xOpt_actual = np.asarray(xOpt_actual, dtype=float)
+        objVal_actual = qp.getObjVal()
+        objVal_actual = np.asarray(objVal_actual, dtype=float)
+
+        cmd = os.path.join(bin_path, "example1")
+        p = Popen(cmd, shell=True, stdout=PIPE)
+        stdout, stderr = p.communicate()
+        stdout = str(stdout).replace('\\n', '\n')
+        stdout = stdout.replace("'", '')
+        print(stdout)
+
+        # get c++ solution from std
+        pattern = re.compile(r'xOpt\s*=\s*\[\s+(?P<xOpt>([0-9., e+-])*)\];')
+        match = pattern.search(stdout)
+        xOpt_expected = match.group('xOpt')
+        xOpt_expected = xOpt_expected.split(",")
+        xOpt_expected = np.asarray(xOpt_expected, dtype=float)
+
+        pattern = re.compile(r'objVal = (?P<objVal>[0-9-+e.]*)')
+        match = pattern.search(stdout)
+        objVal_expected = match.group('objVal')
+        objVal_expected = np.asarray(objVal_expected, dtype=float)
+
+        print("xOpt_actual =", xOpt_actual)
+        print("xOpt_expected =", xOpt_expected)
+        print("objVal_actual = ", objVal_actual)
+        print("objVal_expected = ", objVal_expected)
+
+        assert_almost_equal(xOpt_actual, xOpt_expected, decimal=7)
+        assert_almost_equal(objVal_actual, objVal_expected, decimal=7)
+
+
+    def test_example1b(self):
+        # Example for qpOASES main function using the QProblemB class.
+        #Setup data of first QP.
+
+        H   = np.array([1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
+        g   = np.array([1.5, 1.0 ])
+        lb  = np.array([0.5, -2.0])
+        ub  = np.array([5.0, 2.0 ])
+
+        # Setup data of second QP.
+
+        g_new   = np.array([1.0, 1.5])
+        lb_new  = np.array([0.0, -1.0])
+        ub_new  = np.array([5.0, -0.5])
+
+        # Setting up QProblemB object.
+        qp = QProblemB(2)
+
+        options = Options()
+        options.enableFlippingBounds = BooleanType.FALSE
+        options.initialStatusBounds  = SubjectToStatus.INACTIVE
+        options.numRefinementSteps   = 1
+        options.printLevel = PrintLevel.NONE
+        qp.setOptions(options)
+
+        # Solve first QP.
+        nWSR = 10
+        qp.init(H, g, lb, ub, nWSR)
+
+        # Solve second QP.
+        nWSR = 10;
+        qp.hotstart(g_new, lb_new, ub_new, nWSR)
+
+        # Get and print solution of second QP.
+        xOpt_actual = np.zeros(2)
+        qp.getPrimalSolution(xOpt_actual)
+        xOpt_actual = np.asarray(xOpt_actual, dtype=float)
+        objVal_actual = qp.getObjVal()
+        objVal_actual = np.asarray(objVal_actual, dtype=float)
+
+        cmd = os.path.join(bin_path, "example1b")
+        p = Popen(cmd, shell=True, stdout=PIPE)
+        stdout, stderr = p.communicate()
+        stdout = str(stdout).replace('\\n', '\n')
+        stdout = stdout.replace("'", '')
+        print(stdout)
+
+        # get c++ solution from std
+        pattern = re.compile(r'xOpt\s*=\s*\[\s+(?P<xOpt>([0-9., e+-])*)\];')
+        match = pattern.search(stdout)
+        xOpt_expected = match.group('xOpt')
+        xOpt_expected = xOpt_expected.split(",")
+        xOpt_expected = np.asarray(xOpt_expected, dtype=float)
+
+        pattern = re.compile(r'objVal = (?P<objVal>[0-9-+e.]*)')
+        match = pattern.search(stdout)
+        objVal_expected = match.group('objVal')
+        objVal_expected = np.asarray(objVal_expected, dtype=float)
+
+        print("xOpt_actual =", xOpt_actual)
+        print("xOpt_expected =", xOpt_expected)
+        print("objVal_actual = ", objVal_actual)
+        print("objVal_expected = ", objVal_expected)
+
+        assert_almost_equal(xOpt_actual, xOpt_expected, decimal=7)
+        assert_almost_equal(objVal_actual, objVal_expected, decimal=7)
+
+
+    def test_example2(self):
+        # Example for qpOASES main function using the SQProblem class.
+        #  Setup data of first QP.
+        H = np.array([ 1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
+        A = np.array([ 1.0, 1.0 ]).reshape((2,1))
+        g = np.array([ 1.5, 1.0 ])
+        lb = np.array([ 0.5, -2.0 ])
+        ub = np.array([ 5.0, 2.0 ])
+        lbA = np.array([ -1.0 ])
+        ubA = np.array([ 2.0 ])
+
+        #  Setup data of second QP.
+        H_new = np.array([ 1.0, 0.5, 0.5, 0.5 ]).reshape((2,2))
+        A_new = np.array([ 1.0, 5.0 ]).reshape((2,1))
+        g_new = np.array([ 1.0, 1.5 ])
+        lb_new = np.array([ 0.0, -1.0 ])
+        ub_new = np.array([ 5.0, -0.5 ])
+        lbA_new = np.array([ -2.0 ])
+        ubA_new = np.array([ 1.0 ])
+
+        #  Setting up SQProblem object and solution analyser.
+        qp = SQProblem(2, 1)
+        options = Options()
+        options.printLevel = PrintLevel.NONE
+        qp.setOptions(options)
+
+        analyser = SolutionAnalysis()
+
+        # get c++ solution from std
+        cmd = os.path.join(bin_path, "example2")
+        p = Popen(cmd, shell=True, stdout=PIPE)
+        stdout, stderr = p.communicate()
+        stdout = str(stdout).replace('\\n', '\n')
+        stdout = stdout.replace("'", '')
+        print(stdout)
+
+        #  Solve first QP ...
+        nWSR = 10
+        qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
+
+        #  ... and analyse it.
+        maxKKTviolation = np.zeros(1)
+        analyser.getMaxKKTviolation(qp, maxKKTviolation)
+        print("maxKKTviolation: %e\n"%maxKKTviolation)
+        actual = np.asarray(maxKKTviolation)
+
+        pattern = re.compile(r'maxKKTviolation: (?P<maxKKTviolation>[0-9+-e.]*)')
+
+        match = pattern.findall(stdout)
+        expected = np.asarray(match[0], dtype=float)
+
+        assert_almost_equal(actual, expected, decimal=7)
+
+        #  Solve second QP ...
+        nWSR = 10
+        qp.hotstart(H_new, g_new, A_new,
+                              lb_new, ub_new,
+                              lbA_new, ubA_new, nWSR)
+
+        #  ... and analyse it.
+        analyser.getMaxKKTviolation(qp, maxKKTviolation)
+        print("maxKKTviolation: %e\n"%maxKKTviolation)
+        actual = np.asarray(maxKKTviolation)
+
+        expected = np.asarray(match[1], dtype=float)
+
+        assert_almost_equal(actual, expected, decimal=7)
+
+
+        #  ------------ VARIANCE-COVARIANCE EVALUATION --------------------
+
+        Var             = np.zeros(5*5)
+        Primal_Dual_Var = np.zeros(5*5)
+
+        Var.reshape((5,5))[0,0] = 1.
+        Var.reshape((5,5))[1,1] = 1.
+
+        #                  (  1   0   0   0   0   )
+        #                  (  0   1   0   0   0   )
+        #     Var     =    (  0   0   0   0   0   )
+        #                  (  0   0   0   0   0   )
+        #                  (  0   0   0   0   0   )
+
+
+        analyser.getVarianceCovariance(qp, Var, Primal_Dual_Var)
+        print('Primal_Dual_Var=\n', Primal_Dual_Var.reshape((5,5)))
+        actual = Primal_Dual_Var.reshape((5,5))
+
+        pattern = re.compile(r'Primal_Dual_VAR = (?P<VAR>.*)',
+                             re.DOTALL)
+
+        print(stdout)
+        match = pattern.search(stdout)
+        expected = match.group('VAR').strip().split("\n")
+        expected = [x.strip().split() for x in expected]
+        print(expected)
+        expected = np.asarray(expected, dtype=float)
+
+        assert_almost_equal(actual, expected, decimal=7)
+
+
+    def test_example7(self):
+        H   = np.array([ 0.8514828085899353, -0.15739890933036804, -0.081726007163524628, -0.530426025390625, 0.16773293912410736,
+                        -0.15739890933036804, 1.1552412509918213, 0.57780224084854126, -0.0072606131434440613, 0.010559185408055782,
+                        -0.081726007163524628, 0.57780224084854126, 0.28925251960754395, 5.324830453901086e-006, -3.0256599075073609e-006,
+                        -0.530426025390625, -0.0072606131434440613, 5.324830453901086e-006, 0.35609596967697144, -0.15124998986721039,
+                         0.16773293912410736, 0.010559185408055782, -3.0256599075073609e-006, -0.15124998986721039,
+                         0.15129712224006653], dtype=float).reshape((5, 5))
+        g   = np.array([0.30908384919166565, 0.99325823783874512, 0.49822014570236206, -0.26309865713119507, 0.024296050891280174], dtype=float).reshape((5,))
+        A   = np.array([1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1], dtype=float).reshape((5, 5))
+        lb  = np.array([-0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359938621520996], dtype=float).reshape((5,))
+        ub  = np.array([ 0.052359879016876221, 0.052359879016876221, 0.052359879016876221, 0, 0], dtype=float).reshape((5,))
+        lbA = np.array([-0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359938621520996], dtype=float).reshape((5,))
+        ubA = np.array([0.052359879016876221, 0.052359879016876221, 0.052359879016876221, 0, 0], dtype=float).reshape((5,))
+
+        # Setting up QProblem object.
+        qp = QProblem(5, 5)
+        options = Options()
+        options.printLevel = PrintLevel.NONE
+        qp.setOptions(options)
+
+        # Solve first QP.
+        nWSR = 100
+        qp.init(H, g, A, lb, ub, lbA, ubA,  nWSR)
+
+        result = np.zeros((5,))
+        qp.getPrimalSolution(result)
+
+        # TODO check against what?
+        # Where can I find solution?
+
+if __name__=="__main__":
+    try:
+        import nose
+        nose.runmodule()
+
+    except ImportError:
+        sys.stderr.write("Please install nosestests for python unittesting.\n")
+
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_idhessian.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_idhessian.py
new file mode 100644
index 00000000000..4bddb2dc4b0
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_idhessian.py
@@ -0,0 +1,94 @@
+"""
+This file is part of qpOASES.
+
+qpOASES -- An Implementation of the Online Active Set Strategy.
+Copyright (C) 2007-2009 by Hans Joachim Ferreau et al. All rights reserved.
+
+qpOASES 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.
+
+qpOASES 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 qpOASES; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+"""
+
+#TODO add doxygen support
+# \author Manuel Kudruss
+# \version 3.0
+# \date 2013-2014
+
+import os
+import numpy as np
+from numpy.testing import *
+from qpoases import PyQProblem as QProblem
+from qpoases import PyBooleanType as BooleanType
+from qpoases import PyOptions as Options
+from qpoases import PyPrintLevel as PrintLevel
+
+# get qpOASES path
+qpoases_path = os.path.dirname(os.path.abspath(__file__))
+qpoases_path = os.path.dirname(qpoases_path)
+qpoases_path = os.path.dirname(qpoases_path)
+qpoases_path = os.path.dirname(qpoases_path)
+
+# set qpOASES testing path
+testing_path = os.path.join(qpoases_path, "testing")
+
+class TestIdHessian(TestCase):
+
+    def test_id_hessian(self):
+        """Very simple example for testing qpOASES (using QProblem class)."""
+
+        path = os.path.join(testing_path, "dev_idhessian_data")
+
+        #Setup data for QP.
+        H   = np.loadtxt(os.path.join(path, "H.txt"))
+        g   = np.loadtxt(os.path.join(path, "g.txt"))
+        A   = np.loadtxt(os.path.join(path, "A.txt"))
+        lb  = np.loadtxt(os.path.join(path, "lb.txt"))
+        ub  = np.loadtxt(os.path.join(path, "ub.txt"))
+        lbA = np.loadtxt(os.path.join(path, "lbA.txt"))
+        ubA = np.loadtxt(os.path.join(path, "ubA.txt"))
+
+        #Setting up QProblem object.
+        qp = QProblem(72,144)
+
+        options = Options()
+        options.numRefinementSteps   = 1
+        options.printLevel = PrintLevel.NONE
+
+        #options.setToMPC()
+        #options.setToReliable()
+        #options.enableFlippingBounds = BooleanType.FALSE
+        options.enableRamping = BooleanType.FALSE
+        #options.enableRamping = BooleanType.TRUE
+        #options.enableFarBounds = BooleanType.FALSE
+        #options.enableRamping = BooleanType.FALSE
+        #options.printLevel = PL_LOW
+        #options.enableFullLITests = BooleanType.FALSE
+        #options.boundRelaxation = 1.0e-1
+        qp.setOptions( options )
+
+        #Solve QP.
+        nWSR = 1200
+
+        qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
+
+        # FIXME check against what?
+        # Where can I find solution?
+
+if __name__=="__main__":
+    try:
+        import nose
+        nose.runmodule()
+
+    except ImportError:
+        sys.stderr.write("Please install nosestests for python unittesting.\n")
+
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_testbench.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_testbench.py
new file mode 100644
index 00000000000..0333ff7e42b
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_testbench.py
@@ -0,0 +1,332 @@
+"""
+This file is part of qpOASES.
+
+qpOASES -- An Implementation of the Online Active Set Strategy.
+Copyright (C) 2007-2009 by Hans Joachim Ferreau et al. All rights reserved.
+
+qpOASES 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.
+
+qpOASES 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 qpOASES; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+
+author Manuel Kudruss
+version 3.0
+date 2013-2014
+"""
+
+import os
+import numpy as np
+from numpy.testing import *
+from qpoases import py_runOQPbenchmark as runOQPbenchmark
+from qpoases import PyQProblem as QProblem
+from qpoases import PyBooleanType as BooleanType
+from qpoases import PyReturnValue as ReturnValue
+from qpoases import PyOptions as Options
+from qpoases import PyPrintLevel as PrintLevel
+
+# get qpOASES path
+qpoases_path = os.path.dirname(os.path.abspath(__file__))
+qpoases_path = os.path.dirname(qpoases_path)
+qpoases_path = os.path.dirname(qpoases_path)
+qpoases_path = os.path.dirname(qpoases_path)
+
+# set qpOASES testing path
+testing_path = os.path.join(qpoases_path, 'testing')
+
+
+benchmarks = ('CVXQP1_S',
+              'CVXQP2_S',
+              'CVXQP3_S',
+              'DPKLO1',
+              'DUAL1',
+              'DUAL2',
+              'DUAL3',
+              'DUAL4',
+              'DUALC1',
+              'DUALC2',
+              'DUALC5',
+              'DUALC8',
+              'GENHS28',
+              'HS118',
+              'HS21',
+              'HS268',
+              'HS35',
+              'HS35MOD',
+              'HS51',
+              'HS52',
+              'HS53',
+              'HS76',
+              'LOTSCHD',
+              'PRIMALC1',
+              'PRIMALC2',
+              'PRIMALC5',
+              'QADLITTL',
+              'QAFIRO',
+              'QBEACONF',
+              'QBRANDY',
+              'QE226',
+              'QISRAEL',
+              'QPCBLEND',
+              'QPCBOEI2',
+              'QPTEST',
+              'QRECIPE',
+              'QSC205',
+              'QSCAGR7',
+              'QSHARE1B',
+              'QSHARE2B',
+              'S268',
+              'TAME',
+              'VALUES',
+              'ZECEVIC2',
+              )
+
+def results2str(results):
+    """converts results dictionary to pretty string"""
+    hline = '{0:->11}|{0:-<12}|{0:-<12}|{0:-<12}|{0:-<8}|{0:-<8}\n'.format("")
+
+    npass = 0
+    nfail = 0
+
+    string = ""
+    string += '{:<10} | {: <10} | {: <10} | {: <10} | {: <6} | {:<6}\n'.format('problem', 'stat', 'feas', 'compl', 'nWSR', 'result')
+    string += hline
+    for key in results:
+        line = '{name:<10} | {stat: >10.4e} | {feas: >10.4e} | {comp: >10.4e} | {nwsr: >6d} | {pass!s:<6}\n'.format(**results[key])
+        string += line
+        if results[key]['pass']:
+            npass += 1
+        else:
+            nfail +=1
+
+    string += hline
+    string += '\n'
+    string += 'Testbench results:\n'
+    string += '==================\n'
+    string += 'Pass:  {: >10d}\n'.format(npass)
+    string += 'Fail:  {: >10d}\n'.format(nfail)
+    string += '------------------\n'
+    string += 'Ratio: {: >10.2%}\n'.format(npass/float(len(results)))
+
+    return string
+
+def write_results(name, string):
+    """writes results into results dictionary"""
+    path = os.path.dirname(os.path.abspath(__file__))
+
+    directory = os.path.join(path, 'results')
+
+    if not os.path.exists(directory):
+        os.makedirs(directory)
+
+    with open(os.path.join(directory, name), 'w') as f:
+        f.write(string)
+
+def get_nfail(results):
+    """get nfail from results dictionary"""
+    nfail = 0
+    for key in results:
+        if not results[key]['pass']:
+            nfail +=1
+
+    return nfail
+
+def run_benchmarks(benchmarks, options, isSparse, useHotstarts,
+                   nWSR, cpu_time, TOL):
+    """run all benchmarks and return results as dictionary"""
+    results = {}
+    for item in benchmarks:
+        # NOTE: c/c++ function epects trailing slash in path!
+        path = os.path.join(testing_path, 'problems', item, '')
+
+        # Run QP benchmark
+        returnvalue, maxNWSR, avgNWSR, maxCPUtime, avgCPUtime, \
+        maxStationarity, maxFeasibility, maxComplementarity \
+        = runOQPbenchmark(path, isSparse, useHotstarts,
+                          options, nWSR, cpu_time )
+
+        if (returnvalue == ReturnValue.SUCCESSFUL_RETURN
+            and maxStationarity    < TOL
+            and maxFeasibility     < TOL
+            and maxComplementarity < TOL):
+            ret_val = True
+
+        else:
+            ret_val = False
+
+        tmp_d = {'name': item,
+                 'stat': maxStationarity,
+                 'feas': maxFeasibility,
+                 'comp': maxComplementarity,
+                 'nwsr': int(maxNWSR),
+                 'pass': bool(ret_val),
+                }
+        results[item] = tmp_d
+
+    return results
+
+
+class Testbench(TestCase):
+
+    def setUp(self):
+        # Setup global options for every problem
+        self.TOL = 1e-5
+        self.nWSR = 3500
+        self.cpu_time = 300
+        self.decimal = 7 # number of decimals for assert
+
+    def test_m44_default_dense(self):
+        test_name = 'mm44_default_dense.txt'
+        print("Test: ", test_name)
+
+        # QP Options
+        options = Options()
+        options.setToDefault()
+        options.printLevel = PrintLevel.NONE
+
+        isSparse = False
+        useHotstarts = False
+
+        # run QP benchmarks
+        results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
+                                 self.nWSR, self.cpu_time, self.TOL)
+
+        # print and write results
+        string = results2str(results)
+        print(string)
+        write_results(test_name, string)
+
+        assert get_nfail(results) <= 0, 'One ore more tests failed.'
+
+    def test_m44_default_sparse(self):
+        test_name = 'mm44_default_sparse.txt'
+        print("Test: ", test_name)
+
+        # QP Options
+        options = Options()
+        options.setToDefault()
+        options.printLevel = PrintLevel.NONE
+
+        isSparse = True
+        useHotstarts = False
+
+        # run QP benchmarks
+        results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
+                                 self.nWSR, self.cpu_time, self.TOL)
+
+        # print and write results
+        string = results2str(results)
+        print(string)
+        write_results(test_name, string)
+
+        assert get_nfail(results) <= 0, 'One ore more tests failed.'
+
+    def test_m44_mpc_dense(self):
+        test_name ='mm44_mpc_dense.txt'
+        print("Test: ", test_name)
+
+        # QP Options
+        options = Options()
+        options.setToMPC()
+        options.printLevel = PrintLevel.NONE
+
+        isSparse = False
+        useHotstarts = False
+
+        # run QP benchmarks
+        results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
+                                 self.nWSR, self.cpu_time, self.TOL)
+
+        # print and write results
+        string = results2str(results)
+        print(string)
+        write_results(test_name, string)
+
+        assert get_nfail(results) <= 2, 'One ore more tests failed.'
+
+    def test_m44_mpc_sparse(self):
+        test_name ='mm44_mpc_sparse.txt'
+        print("Test: ", test_name)
+
+        # QP Options
+        options = Options()
+        options.setToMPC()
+        options.printLevel = PrintLevel.NONE
+
+        isSparse = True
+        useHotstarts = False
+
+        # run QP benchmarks
+        results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
+                                 self.nWSR, self.cpu_time, self.TOL)
+
+        # print and write results
+        string = results2str(results)
+        print(string)
+        write_results(test_name, string)
+
+        assert get_nfail(results) <= 19, 'One ore more tests failed.'
+
+    def test_m44_reliable_dense(self):
+        test_name = 'mm44_reliable_dense.txt'
+        print("Test: ", test_name)
+
+        # QP Options
+        options = Options()
+        options.setToReliable()
+        options.printLevel = PrintLevel.NONE
+
+        isSparse = False
+        useHotstarts = False
+
+        # run QP benchmarks
+        results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
+                                 self.nWSR, self.cpu_time, self.TOL)
+
+        # print and write results
+        string = results2str(results)
+        print(string)
+        write_results(test_name, string)
+
+        assert get_nfail(results) <= 0, 'One ore more tests failed.'
+
+    def test_m44_reliable_sparse(self):
+        test_name = 'mm44_reliable_sparse.txt'
+        print(test_name)
+
+        # QP Options
+        options = Options()
+        options.setToReliable()
+        options.printLevel = PrintLevel.NONE
+
+        isSparse = True
+        useHotstarts = False
+
+        # run QP benchmarks
+        results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
+                                 self.nWSR, self.cpu_time, self.TOL)
+
+        # print and write results
+        string = results2str(results)
+        print(string)
+        write_results(test_name, string)
+
+        assert get_nfail(results) <= 0, 'One ore more tests failed.'
+
+
+if __name__=='__main__':
+    try:
+        import nose
+        nose.runmodule(argv=['', '-s', '-v'])
+
+    except ImportError:
+        sys.stderr.write('Please install nosestests for python unittesting.\n')
+
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1.dat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1.dat
new file mode 100644
index 0000000000000000000000000000000000000000..1c9e594b6da775ef2a23e6db6ce75a676f91f100
GIT binary patch
literal 656
zcmb7AK?;O05bJss77vOy5mvCjnEqU!(jPb{@wiiH&_WOz7}7&C$>dl)#m70<<T1)n
z!K>sb9o^zR_9VV><vl9ri$Aot4*n`b%MGC^{UUPjQ<bg{**gp^Pnvu%kDS|7_1}eV
o8NV|*iQj`BF6)5{xef`{^3~|jL)@%~A4`f35<g&l*Zf!c0|-Py!2kdN

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1a.dat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1a.dat
new file mode 100644
index 0000000000000000000000000000000000000000..4e67850dc07986750160308909887ba9fd3a9eef
GIT binary patch
literal 784
zcmb7>O$x*y427N97KSb=-AQ2z^%n8wouhaIvnpLq1Rst}5DbL4@VzfT>F{bij4{uW
zQc5u4)z(O}>f$|oa;<R}AEv9rhqmg#XR?zHgr?<1^!;%|x+N5Lu#?_t<{*FiJl)WL
z6ViKqZOTZPC*Pt5_JFx+o%g;^yBGM}banXX;h(?AUUVQdEe{SX`x(*U4)K&bsGVjG
M5)`R&?~(sZUuq^%egFUf

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1b.dat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1b.dat
new file mode 100644
index 0000000000000000000000000000000000000000..b591a7ddcf2ea8854c8bfdf5ba1193068bc1cce2
GIT binary patch
literal 408
zcmaE_u7MRWGB7YOK{1emf)DmE7ApO~UI42p8X7QjKr{mUfSU8cUX&ZVM(p|?!0bOD
tN0L4f2dLc+0xIHs3RtYfZXe9QKY;GUt`WOFm`^|Kmm^6Z%$*PR0|49CCeQ!?

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/index.html
new file mode 100644
index 00000000000..8b1f81fa0f5
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/index.html
@@ -0,0 +1,14 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces/scilab</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/interfaces/scilab</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="Makefile">Makefile</a></li>
+  <li><a href="example1.dat">example1.dat</a></li>
+  <li><a href="example1a.dat">example1a.dat</a></li>
+  <li><a href="example1b.dat">example1b.dat</a></li>
+  <li><a href="qpOASESinterface.c">qpOASESinterface.c</a></li>
+  <li><a href="qpOASESinterface.sce">qpOASESinterface.sce</a></li>
+  <li><a href="qpOASESroutines.cpp">qpOASESroutines.cpp</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESinterface.c b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESinterface.c
new file mode 100644
index 00000000000..9be0a6f223c
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESinterface.c
@@ -0,0 +1,1061 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file interfaces/scilab/qpOASESinterface.c
+ *	\author Holger Diedam, Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Interface that enables to call qpOASES from scilab.
+ *  (Please excuse a lot of copy and paste...)
+ *
+ */
+
+
+#include <stdio.h>
+#include <string.h>
+
+#include <scilab/stack-c.h>
+#include <scilab/Scierror.h>
+
+
+extern int int_qpOASES( char* fname );
+
+extern int int_init(   char* fname );
+extern int int_initSB( char* fname );
+extern int int_initVM( char* fname );
+
+extern int int_solve(   char* fname );
+extern int int_solveSB( char* fname );
+extern int int_solveVM( char* fname );
+
+extern int int_cleanup(   char* fname );
+extern int int_cleanupSB( char* fname );
+extern int int_cleanupVM( char* fname );
+
+
+typedef int (*gate_function) ( char* );
+extern int sci_gateway( char* name, gate_function f );
+extern int C2F(qpOASESgateway)();
+
+
+/* forward declaration of C++ routines */
+void qpoases(	double* H, double* g, double* A, double* lb, double* ub, double* lbA, double* ubA,
+				int *nV, int* nC, int* nWSR,
+				double* x, double* obj, int* status, int* nWSRout, double* y
+				);
+
+void init(		double* H, double* g, double* A, double* lb, double* ub, double* lbA, double* ubA,
+				int *nV, int* nC, int* nWSR,
+				double* x, double* obj, int* status, int* nWSRout, double* y
+				);
+void initSB(	double* H, double* g, double* lb, double* ub,
+				int *nV, int* nWSR,
+				double* x, double* obj, int* status, int* nWSRout, double* y
+				);
+void initVM(	double* H, double* g, double* A, double* lb, double* ub, double* lbA, double* ubA,
+				int *nV, int* nC, int* nWSR,
+				double* x, double* obj, int* status, int* nWSRout, double* y
+				);
+
+void hotstart(		double* g, double* lb, double* ub, double* lbA, double* ubA,
+					int* nWSR,
+					double* x, double* obj, int* status, int* nWSRout, double* y
+					);
+void hotstartSB(	double* g, double* lb, double* ub,
+					int* nWSR,
+					double* x, double* obj, int* status, int* nWSRout, double* y
+					);
+void hotstartVM(	double* H, double* g, double* A, double* lb, double* ub, double* lbA, double* ubA,
+					int* nWSR,
+					double* x, double* obj, int* status, int* nWSRout, double* y
+					);
+
+void cleanupp( );
+void cleanupSB( );
+void cleanupVM( );
+
+
+/* global variables containing dimensions of matrices
+ * (also used to check whether qpOASES object were initialised) */
+static int qp_rowsH = -1;
+static int qp_rowsA = -1;
+static int qpb_rowsH = -1;
+static int sqp_rowsH = -1;
+static int sqp_rowsA = -1;
+
+
+/*
+ *	i n t _ q p O A S E S
+ */
+int int_qpOASES( char* fname )
+{
+	int H, H_rows, H_cols;
+	int g, g_rows, g_cols;
+	int A, A_rows, A_cols;
+	int lb, lb_rows, lb_cols;
+	int ub, ub_rows, ub_cols;
+	int lbA, lbA_rows, lbA_cols;
+	int ubA, ubA_rows, ubA_cols;
+	int nWSR, nWSR_rows, nWSR_cols;
+
+	int x, obj, status, nWSRout, y;
+
+
+	int minlhs = 1, maxlhs = 5, minrhs = 8, maxrhs = 8, one = 1, y_size;
+
+	CheckRhs( minrhs,maxrhs );
+	CheckLhs( minlhs,maxlhs );
+
+
+	/* check dimensions */
+	GetRhsVar( 1,"d", &H_rows,&H_cols,&H );
+	if ( ( H_rows != H_cols ) || ( H_rows < 1 ) )
+	{
+		Scierror( 111,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 2,"d", &g_rows,&g_cols,&g );
+	if ( !( ( ( g_rows == H_rows ) && ( g_cols == 1 ) ) || ( ( g_rows == 1 ) && ( g_cols == H_rows ) ) ) )
+	{
+		Scierror( 112,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 3,"d", &A_rows,&A_cols,&A );
+	if ( ( A_cols != H_rows ) || ( A_rows < 1 ) )
+	{
+		Scierror( 113,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 4,"d", &lb_rows,&lb_cols,&lb);
+	if ( !( ( ( lb_rows == H_rows ) && ( lb_cols == 1 ) ) || ( ( lb_rows == 0 ) && ( lb_cols == 0 ) ) ) )
+	{
+		Scierror( 114,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 5,"d", &ub_rows,&ub_cols,&ub);
+	if ( !( ( ( ub_rows == H_rows ) && ( ub_cols == 1 ) ) || ( ( ub_rows == 0 ) && ( ub_cols == 0 ) ) ) )
+	{
+		Scierror( 115,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 6,"d", &lbA_rows,&lbA_cols,&lbA);
+	if ( ( lbA_rows != A_rows ) || ( lbA_cols != 1 ) )
+	{
+		Scierror( 116,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 7,"d", &ubA_rows,&ubA_cols,&ubA);
+	if ( ( ubA_rows != A_rows ) || ( ubA_cols != 1 ) )
+	{
+		Scierror( 117,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 8,"i", &nWSR_rows,&nWSR_cols,&nWSR);
+	if ( ( nWSR_rows != nWSR_cols ) || ( nWSR_cols != 1 ) )
+	{
+		Scierror( 118,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+
+	y_size = H_rows + A_rows;
+
+	CreateVar(  9,"d", &H_rows,&one,&x );
+	CreateVar( 10,"d", &one,&one,&obj );
+	CreateVar( 11,"i", &one,&one,&status );
+	CreateVar( 12,"i", &one,&one,&nWSRout );
+	CreateVar( 13,"d", &y_size,&one,&y );
+
+
+	/* call interfaced qpOASES routines with appropriate arguments */
+	if ( ( lb_rows != 0 ) && ( ub_rows != 0 ) )
+	{
+		qpoases(	stk(H),stk(g),stk(A),stk(lb),stk(ub),stk(lbA),stk(ubA),
+					&H_rows,&A_rows,istk(nWSR),
+					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+					);
+	}
+	else if ( ( lb_rows == 0 ) && ( ub_rows != 0 ) )
+	{
+		qpoases(	stk(H),stk(g),stk(A),0,stk(ub),stk(lbA),stk(ubA),
+					&H_rows,&A_rows,istk(nWSR),
+					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+					);
+	}
+	else if ( ( lb_rows != 0 ) && ( ub_rows == 0 ) )
+	{
+		qpoases(	stk(H),stk(g),stk(A),stk(lb),0,stk(lbA),stk(ubA),
+					&H_rows,&A_rows,istk(nWSR),
+					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+					);
+	}
+	else
+	{
+		qpoases(	stk(H),stk(g),stk(A),0,0,stk(lbA),stk(ubA),
+					&H_rows,&A_rows,istk(nWSR),
+					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+					);
+	}
+
+	LhsVar(1) = 9;
+	LhsVar(2) = 10;
+	LhsVar(3) = 11;
+	LhsVar(4) = 12;
+	LhsVar(5) = 13;
+
+	return 0;
+}
+
+
+/*
+ *	i n t _ i n i t
+ */
+int int_init( char* fname )
+{
+	int H, H_rows, H_cols;
+	int g, g_rows, g_cols;
+	int A, A_rows, A_cols;
+	int lb, lb_rows, lb_cols;
+	int ub, ub_rows, ub_cols;
+	int lbA, lbA_rows, lbA_cols;
+	int ubA, ubA_rows, ubA_cols;
+	int nWSR, nWSR_rows, nWSR_cols;
+
+	int x, obj, status, nWSRout, y;
+
+
+	int minlhs = 1, maxlhs = 5, minrhs = 8, maxrhs = 8, one = 1, y_size;
+
+	CheckRhs( minrhs,maxrhs );
+	CheckLhs( minlhs,maxlhs );
+
+
+	/* check dimensions */
+	GetRhsVar( 1,"d", &H_rows,&H_cols,&H );
+	if ( ( H_rows != H_cols ) || ( H_rows < 1 ) )
+	{
+		Scierror( 211,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 2,"d", &g_rows,&g_cols,&g );
+	if ( !( ( ( g_rows == H_rows ) && ( g_cols == 1 ) ) || ( ( g_rows == 1 ) && ( g_cols == H_rows ) ) ) )
+	{
+		Scierror( 212,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 3,"d", &A_rows,&A_cols,&A );
+	if ( ( A_cols != H_rows ) || ( A_rows < 1 ) )
+	{
+		Scierror( 213,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 4,"d", &lb_rows,&lb_cols,&lb);
+	if ( !( ( ( lb_rows == H_rows ) && ( lb_cols == 1 ) ) || ( ( lb_rows == 0 ) && ( lb_cols == 0 ) ) ) )
+	{
+		Scierror( 214,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 5,"d", &ub_rows,&ub_cols,&ub);
+	if ( !( ( ( ub_rows == H_rows ) && ( ub_cols == 1 ) ) || ( ( ub_rows == 0 ) && ( ub_cols == 0 ) ) ) )
+	{
+		Scierror( 215,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 6,"d", &lbA_rows,&lbA_cols,&lbA);
+	if ( ( lbA_rows != A_rows ) || ( lbA_cols != 1 ) )
+	{
+		Scierror( 216,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 7,"d", &ubA_rows,&ubA_cols,&ubA);
+	if ( ( ubA_rows != A_rows ) || ( ubA_cols != 1 ) )
+	{
+		Scierror( 217,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 8,"i", &nWSR_rows,&nWSR_cols,&nWSR);
+	if ( ( nWSR_rows != nWSR_cols ) || ( nWSR_cols != 1 ) )
+	{
+		Scierror( 218,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+
+	y_size = H_rows + A_rows;
+
+	CreateVar(  9,"d", &H_rows,&one,&x );
+	CreateVar( 10,"d", &one,&one,&obj );
+	CreateVar( 11,"i", &one,&one,&status );
+	CreateVar( 12,"i", &one,&one,&nWSRout );
+	CreateVar( 13,"d", &y_size,&one,&y );
+
+
+	qp_rowsH = H_rows;
+	qp_rowsA = A_rows;
+
+
+	/* call interfaced qpOASES routines with appropriate arguments */
+	if ( ( lb_rows != 0 ) && ( ub_rows != 0 ) )
+	{
+		init(	stk(H),stk(g),stk(A),stk(lb),stk(ub),stk(lbA),stk(ubA),
+				&H_rows,&A_rows,istk(nWSR),
+				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+				);
+	}
+	else if ( ( lb_rows == 0 ) && ( ub_rows != 0 ) )
+	{
+		init(	stk(H),stk(g),stk(A),0,stk(ub),stk(lbA),stk(ubA),
+				&H_rows,&A_rows,istk(nWSR),
+				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+				);
+	}
+	else if ( ( lb_rows != 0 ) && ( ub_rows == 0 ) )
+	{
+		init(	stk(H),stk(g),stk(A),stk(lb),0,stk(lbA),stk(ubA),
+				&H_rows,&A_rows,istk(nWSR),
+				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+				);
+	}
+	else
+	{
+		init(	stk(H),stk(g),stk(A),0,0,stk(lbA),stk(ubA),
+				&H_rows,&A_rows,istk(nWSR),
+				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+				);
+	}
+
+	LhsVar(1) = 9;
+	LhsVar(2) = 10;
+	LhsVar(3) = 11;
+	LhsVar(4) = 12;
+	LhsVar(5) = 13;
+
+	return 0;
+}
+
+
+/*
+ *	i n t _ i n i t S B
+ */
+int int_initSB( char* fname )
+{
+	int H, H_rows, H_cols;
+	int g, g_rows, g_cols;
+	int lb, lb_rows, lb_cols;
+	int ub, ub_rows, ub_cols;
+	int nWSR, nWSR_rows, nWSR_cols;
+
+	int x, obj, status, nWSRout, y;
+
+
+	int minlhs = 1, maxlhs = 5, minrhs = 5, maxrhs = 5, one = 1;
+
+	CheckRhs( minrhs,maxrhs );
+	CheckLhs( minlhs,maxlhs );
+
+
+	/* check dimensions */
+	GetRhsVar( 1,"d", &H_rows,&H_cols,&H );
+	if ( ( H_rows != H_cols ) || ( H_rows < 1 ) )
+	{
+		Scierror( 221,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 2,"d", &g_rows,&g_cols,&g );
+	if ( !( ( ( g_rows == H_rows ) && ( g_cols == 1 ) ) || ( ( g_rows == 1 ) && ( g_cols == H_rows ) ) ) )
+	{
+		Scierror( 222,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 3,"d", &lb_rows,&lb_cols,&lb);
+	if ( !( ( ( lb_rows == H_rows ) && ( lb_cols == 1 ) ) || ( ( lb_rows == 0 ) && ( lb_cols == 0 ) ) ) )
+	{
+		Scierror( 223,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 4,"d", &ub_rows,&ub_cols,&ub);
+	if ( !( ( ( ub_rows == H_rows ) && ( ub_cols == 1 ) ) || ( ( ub_rows == 0 ) && ( ub_cols == 0 ) ) ) )
+	{
+		Scierror( 224,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 5,"i", &nWSR_rows,&nWSR_cols,&nWSR);
+	if ( ( nWSR_rows != nWSR_cols ) || ( nWSR_cols != 1 ) )
+	{
+		Scierror( 225,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+
+	CreateVar(  9,"d", &H_rows,&one,&x );
+	CreateVar( 10,"d", &one,&one,&obj );
+	CreateVar( 11,"i", &one,&one,&status );
+	CreateVar( 12,"i", &one,&one,&nWSRout );
+	CreateVar( 13,"d", &H_rows,&one,&y );
+
+
+	qpb_rowsH = H_rows;
+
+
+	/* call interfaced qpOASES routines with appropriate arguments */
+	if ( ( lb_rows != 0 ) && ( ub_rows != 0 ) )
+	{
+		initSB(	stk(H),stk(g),stk(lb),stk(ub),
+				&H_rows,istk(nWSR),
+				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+				);
+	}
+	else if ( ( lb_rows == 0 ) && ( ub_rows != 0 ) )
+	{
+		initSB(	stk(H),stk(g),0,stk(ub),
+				&H_rows,istk(nWSR),
+				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+				);
+	}
+	else if ( ( lb_rows != 0 ) && ( ub_rows == 0 ) )
+	{
+		initSB(	stk(H),stk(g),stk(lb),0,
+				&H_rows,istk(nWSR),
+				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+				);
+	}
+	else
+	{
+		initSB(	stk(H),stk(g),0,0,
+				&H_rows,istk(nWSR),
+				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+				);
+	}
+
+	LhsVar(1) = 9;
+	LhsVar(2) = 10;
+	LhsVar(3) = 11;
+	LhsVar(4) = 12;
+	LhsVar(5) = 13;
+
+	return 0;
+}
+
+
+/*
+ *	i n t _ i n i t V M
+ */
+int int_initVM( char* fname )
+{
+	int H, H_rows, H_cols;
+	int g, g_rows, g_cols;
+	int A, A_rows, A_cols;
+	int lb, lb_rows, lb_cols;
+	int ub, ub_rows, ub_cols;
+	int lbA, lbA_rows, lbA_cols;
+	int ubA, ubA_rows, ubA_cols;
+	int nWSR, nWSR_rows, nWSR_cols;
+
+	int x, obj, status, nWSRout, y;
+
+
+	int minlhs = 1, maxlhs = 5, minrhs = 8, maxrhs = 8, one = 1, y_size;
+
+	CheckRhs( minrhs,maxrhs );
+	CheckLhs( minlhs,maxlhs );
+
+
+	/* check dimensions */
+	GetRhsVar( 1,"d", &H_rows,&H_cols,&H );
+	if ( ( H_rows != H_cols ) || ( H_rows < 1 ) )
+	{
+		Scierror( 231,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 2,"d", &g_rows,&g_cols,&g );
+	if ( !( ( ( g_rows == H_rows ) && ( g_cols == 1 ) ) || ( ( g_rows == 1 ) && ( g_cols == H_rows ) ) ) )
+	{
+		Scierror( 232,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 3,"d", &A_rows,&A_cols,&A );
+	if ( ( A_cols != H_rows ) || ( A_rows < 1 ) )
+	{
+		Scierror( 233,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 4,"d", &lb_rows,&lb_cols,&lb );
+	if ( !( ( ( lb_rows == H_rows ) && ( lb_cols == 1 ) ) || ( ( lb_rows == 0 ) && ( lb_cols == 0 ) ) ) )
+	{
+		Scierror( 234,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 5,"d", &ub_rows,&ub_cols,&ub );
+	if ( !( ( ( ub_rows == H_rows ) && ( ub_cols == 1 ) ) || ( ( ub_rows == 0 ) && ( ub_cols == 0 ) ) ) )
+	{
+		Scierror( 235,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 6,"d", &lbA_rows,&lbA_cols,&lbA );
+	if ( ( lbA_rows != A_rows ) || ( lbA_cols != 1 ) )
+	{
+		Scierror( 236,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 7,"d", &ubA_rows,&ubA_cols,&ubA );
+	if ( ( ubA_rows != A_rows ) || ( ubA_cols != 1 ) )
+	{
+		Scierror( 237,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 8,"i", &nWSR_rows,&nWSR_cols,&nWSR) ;
+	if ( ( nWSR_rows != nWSR_cols ) || ( nWSR_cols != 1 ) )
+	{
+		Scierror( 238,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+
+	y_size = H_rows + A_rows;
+
+	CreateVar(  9,"d", &H_rows,&one,&x );
+	CreateVar( 10,"d", &one,&one,&obj );
+	CreateVar( 11,"i", &one,&one,&status );
+	CreateVar( 12,"i", &one,&one,&nWSRout );
+	CreateVar( 13,"d", &y_size,&one,&y );
+
+
+	sqp_rowsH = H_rows;
+	sqp_rowsA = A_rows;
+
+
+	/* call interfaced qpOASES routines with appropriate arguments */
+	if ( ( lb_rows != 0 ) && ( ub_rows != 0 ) )
+	{
+		initVM(	stk(H),stk(g),stk(A),stk(lb),stk(ub),stk(lbA),stk(ubA),
+				&H_rows,&A_rows,istk(nWSR),
+				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+				);
+	}
+	else if ( ( lb_rows == 0 ) && ( ub_rows != 0 ) )
+	{
+		initVM(	stk(H),stk(g),stk(A),0,stk(ub),stk(lbA),stk(ubA),
+				&H_rows,&A_rows,istk(nWSR),
+				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+				);
+	}
+	else if ( ( lb_rows != 0 ) && ( ub_rows == 0 ) )
+	{
+		initVM(	stk(H),stk(g),stk(A),stk(lb),0,stk(lbA),stk(ubA),
+				&H_rows,&A_rows,istk(nWSR),
+				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+				);
+	}
+	else
+	{
+		initVM(	stk(H),stk(g),stk(A),0,0,stk(lbA),stk(ubA),
+				&H_rows,&A_rows,istk(nWSR),
+				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+				);
+	}
+
+	LhsVar(1) = 9;
+	LhsVar(2) = 10;
+	LhsVar(3) = 11;
+	LhsVar(4) = 12;
+	LhsVar(5) = 13;
+
+	return 0;
+}
+
+
+/*
+ *	i n t _ h o t s t a r t
+ */
+int int_hotstart( char* fname )
+{
+	int g, g_rows, g_cols;
+	int lb, lb_rows, lb_cols;
+	int ub, ub_rows, ub_cols;
+	int lbA, lbA_rows, lbA_cols;
+	int ubA, ubA_rows, ubA_cols;
+	int nWSR, nWSR_rows, nWSR_cols;
+
+	int x, obj, status, nWSRout, y;
+
+
+	int minlhs = 1, maxlhs = 5, minrhs = 6, maxrhs = 6, one = 1, y_size;
+
+	CheckRhs( minrhs,maxrhs );
+	CheckLhs( minlhs,maxlhs );
+
+
+	if ( ( qp_rowsH == -1 ) || ( qp_rowsA == -1 ) )
+	{
+		Scierror( 311,"ERROR (qpOASES): Need to call qpOASES_init first!\n" );
+		return 0;
+	}
+
+	/* check dimensions */
+	GetRhsVar( 1,"d", &g_rows,&g_cols,&g );
+	if ( !( ( ( g_rows == qp_rowsH ) && ( g_cols == 1 ) ) || ( ( g_rows == 1 ) && ( g_cols == qp_rowsH ) ) ) )
+	{
+		Scierror( 312,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 2,"d", &lb_rows,&lb_cols,&lb );
+	if ( !( ( ( lb_rows == qp_rowsH ) && ( lb_cols == 1 ) ) || ( ( lb_rows == 0 ) && ( lb_cols == 0 ) ) ) )
+	{
+		Scierror( 313,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 3,"d", &ub_rows,&ub_cols,&ub );
+	if ( !( ( ( ub_rows == qp_rowsH ) && ( ub_cols == 1 ) ) || ( ( ub_rows == 0 ) && ( ub_cols == 0 ) ) ) )
+	{
+		Scierror( 314,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 4,"d", &lbA_rows,&lbA_cols,&lbA );
+	if ( ( lbA_rows != qp_rowsA ) || ( lbA_cols != 1 ) )
+	{
+		Scierror( 315,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 5,"d", &ubA_rows,&ubA_cols,&ubA );
+	if ( ( ubA_rows != qp_rowsA ) || ( ubA_cols != 1 ) )
+	{
+		Scierror( 316,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 6,"i", &nWSR_rows,&nWSR_cols,&nWSR );
+	if ( ( nWSR_rows != nWSR_cols ) || ( nWSR_cols != 1 ) )
+	{
+		Scierror( 317,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+
+	y_size = qp_rowsH + qp_rowsA;
+
+	CreateVar(  7,"d", &qp_rowsH,&one,&x );
+	CreateVar(  8,"d", &one,&one,&obj );
+	CreateVar(  9,"i", &one,&one,&status );
+	CreateVar( 10,"i", &one,&one,&nWSRout );
+	CreateVar( 11,"d", &y_size,&one,&y );
+
+
+	/* call interfaced qpOASES routines with appropriate arguments */
+	if ( ( lb_rows != 0 ) && ( ub_rows != 0 ) )
+	{
+		hotstart(	stk(g),stk(lb),stk(ub),stk(lbA),stk(ubA),
+					istk(nWSR),
+					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+					);
+	}
+	else if ( ( lb_rows == 0 ) && ( ub_rows != 0 ) )
+	{
+		hotstart(	stk(g),0,stk(ub),stk(lbA),stk(ubA),
+					istk(nWSR),
+					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+					);
+	}
+	else if ( ( lb_rows != 0 ) && ( ub_rows == 0 ) )
+	{
+		hotstart(	stk(g),stk(lb),0,stk(lbA),stk(ubA),
+					istk(nWSR),
+					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+					);
+	}
+	else
+	{
+		hotstart(	stk(g),0,0,stk(lbA),stk(ubA),
+					istk(nWSR),
+					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+					);
+	}
+
+
+	LhsVar(1) = 7;
+	LhsVar(2) = 8;
+	LhsVar(3) = 9;
+	LhsVar(4) = 10;
+	LhsVar(5) = 11;
+
+	return 0;
+}
+
+
+/*
+ *	i n t _ h o t s t a r t S B
+ */
+int int_hotstartSB( char* fname )
+{
+	int g, g_rows, g_cols;
+	int lb, lb_rows, lb_cols;
+	int ub, ub_rows, ub_cols;
+	int nWSR, nWSR_rows, nWSR_cols;
+
+	int x, obj, status, nWSRout, y;
+
+
+	int minlhs = 1, maxlhs = 5, minrhs = 4, maxrhs = 4, one = 1;
+
+	CheckRhs( minrhs,maxrhs );
+	CheckLhs( minlhs,maxlhs );
+
+
+	if ( qpb_rowsH == -1 )
+	{
+		Scierror( 321,"ERROR (qpOASES): Need to call qpOASES_init first!\n" );
+		return 0;
+	}
+
+	/* check dimensions */
+	GetRhsVar( 1,"d", &g_rows,&g_cols,&g );
+	if ( !( ( ( g_rows == qpb_rowsH ) && ( g_cols == 1 ) ) || ( ( g_rows == 1 ) && ( g_cols == qpb_rowsH ) ) ) )
+	{
+		Scierror( 322,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 2,"d", &lb_rows,&lb_cols,&lb );
+	if ( !( ( ( lb_rows == qpb_rowsH ) && ( lb_cols == 1 ) ) || ( ( lb_rows == 0 ) && ( lb_cols == 0 ) ) ) )
+	{
+		Scierror( 323,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 3,"d", &ub_rows,&ub_cols,&ub );
+	if ( !( ( ( ub_rows == qpb_rowsH ) && ( ub_cols == 1 ) ) || ( ( ub_rows == 0 ) && ( ub_cols == 0 ) ) ) )
+	{
+		Scierror( 324,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 4,"i", &nWSR_rows,&nWSR_cols,&nWSR );
+	if ( ( nWSR_rows != nWSR_cols ) || ( nWSR_cols != 1 ) )
+	{
+		Scierror( 325,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+
+	CreateVar( 5,"d", &qpb_rowsH,&one,&x );
+	CreateVar( 6,"d", &one,&one,&obj );
+	CreateVar( 7,"i", &one,&one,&status );
+	CreateVar( 8,"i", &one,&one,&nWSRout );
+	CreateVar( 9,"d", &qpb_rowsH,&one,&y );
+
+
+	/* call interfaced qpOASES routines with appropriate arguments */
+	if ( ( lb_rows != 0 ) && ( ub_rows != 0 ) )
+	{
+		hotstartSB(	stk(g),stk(lb),stk(ub),
+					istk(nWSR),
+					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+					);
+	}
+	else if ( ( lb_rows == 0 ) && ( ub_rows != 0 ) )
+	{
+		hotstartSB(	stk(g),0,stk(ub),
+					istk(nWSR),
+					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+					);
+	}
+	else if ( ( lb_rows != 0 ) && ( ub_rows == 0 ) )
+	{
+		hotstartSB(	stk(g),stk(lb),0,
+					istk(nWSR),
+					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+					);
+	}
+	else
+	{
+		hotstartSB(	stk(g),0,0,
+					istk(nWSR),
+					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+					);
+	}
+
+
+	LhsVar(1) = 5;
+	LhsVar(2) = 6;
+	LhsVar(3) = 7;
+	LhsVar(4) = 8;
+	LhsVar(5) = 9;
+
+	return 0;
+}
+
+
+/*
+ *	i n t _ h o t s t a r t V M
+ */
+int int_hotstartVM( char* fname )
+{
+	int H, H_rows, H_cols;
+	int g, g_rows, g_cols;
+	int A, A_rows, A_cols;
+	int lb, lb_rows, lb_cols;
+	int ub, ub_rows, ub_cols;
+	int lbA, lbA_rows, lbA_cols;
+	int ubA, ubA_rows, ubA_cols;
+	int nWSR, nWSR_rows, nWSR_cols;
+
+	int obj, x, y, status, nWSRout;
+
+
+	int minlhs = 1, maxlhs = 5, minrhs = 8, maxrhs = 8, one = 1, y_size;
+
+	CheckRhs( minrhs,maxrhs );
+	CheckLhs( minlhs,maxlhs );
+
+
+	if ( ( sqp_rowsH == -1 ) || ( sqp_rowsA == -1 ) )
+	{
+		Scierror( 331,"ERROR (qpOASES): Need to call qpOASES_init first!\n" );
+		return 0;
+	}
+
+	/* check dimensions */
+	GetRhsVar( 1,"d", &H_rows,&H_cols,&H );
+	if ( ( H_rows != H_cols ) || ( H_rows < 1 ) )
+	{
+		Scierror( 332,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 2,"d", &g_rows,&g_cols,&g );
+	if ( !( ( ( g_rows == H_rows ) && ( g_cols == 1 ) ) || ( ( g_rows == 1 ) && ( g_cols == H_rows ) ) ) )
+	{
+		Scierror( 333,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 3,"d", &A_rows,&A_cols,&A );
+	if ( ( A_cols != H_rows ) || ( A_rows < 1 ) )
+	{
+		Scierror( 334,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 4,"d", &lb_rows,&lb_cols,&lb);
+	if ( !( ( ( lb_rows == H_rows ) && ( lb_cols == 1 ) ) || ( ( lb_rows == 0 ) && ( lb_cols == 0 ) ) ) )
+	{
+		Scierror( 335,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 5,"d", &ub_rows,&ub_cols,&ub);
+	if ( !( ( ( ub_rows == H_rows ) && ( ub_cols == 1 ) ) || ( ( ub_rows == 0 ) && ( ub_cols == 0 ) ) ) )
+	{
+		Scierror( 399,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 6,"d", &lbA_rows,&lbA_cols,&lbA);
+	if ( ( lbA_rows != A_rows ) || ( lbA_cols != 1 ) )
+	{
+		Scierror( 336,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 7,"d", &ubA_rows,&ubA_cols,&ubA);
+	if ( ( ubA_rows != A_rows ) || ( ubA_cols != 1 ) )
+	{
+		Scierror( 337,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	GetRhsVar( 8,"i", &nWSR_rows,&nWSR_cols,&nWSR);
+	if ( ( nWSR_rows != nWSR_cols ) || ( nWSR_cols != 1 ) )
+	{
+		Scierror( 338,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+	/* have matrices same dimension as last QP? */
+	if ( ( sqp_rowsH != H_rows ) || ( sqp_rowsA != A_rows ) )
+	{
+		Scierror( 339,"ERROR (qpOASES): Dimension mismatch!\n" );
+		return 0;
+	}
+
+
+	y_size = H_rows + A_rows;
+
+	CreateVar(  9,"d", &H_rows,&one,&x );
+	CreateVar( 10,"d", &one,&one,&obj );
+	CreateVar( 11,"i", &one,&one,&status );
+	CreateVar( 12,"i", &one,&one,&nWSRout );
+	CreateVar( 13,"d", &y_size,&one,&y );
+
+
+	/* call interfaced qpOASES routines with appropriate arguments */
+	if ( ( lb_rows != 0 ) && ( ub_rows != 0 ) )
+	{
+		hotstartVM(	stk(H),stk(g),stk(A),stk(lb),stk(ub),stk(lbA),stk(ubA),
+					istk(nWSR),
+					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+					);
+	}
+	else if ( ( lb_rows == 0 ) && ( ub_rows != 0 ) )
+	{
+		hotstartVM(	stk(H),stk(g),stk(A),0,stk(ub),stk(lbA),stk(ubA),
+					istk(nWSR),
+					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+					);
+	}
+	else if ( ( lb_rows != 0 ) && ( ub_rows == 0 ) )
+	{
+		hotstartVM(	stk(H),stk(g),stk(A),stk(lb),0,stk(lbA),stk(ubA),
+					istk(nWSR),
+					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+					);
+	}
+	else
+	{
+		hotstartVM(	stk(H),stk(g),stk(A),0,0,stk(lbA),stk(ubA),
+					istk(nWSR),
+					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
+					);
+	}
+
+
+	LhsVar(1) = 9;
+	LhsVar(2) = 10;
+	LhsVar(3) = 11;
+	LhsVar(4) = 12;
+	LhsVar(5) = 13;
+
+	return 0;
+}
+
+
+/*
+ *	i n t _ c l e a n u p
+ */
+int int_cleanup( char* fname )
+{
+	const int minlhs = 0, maxlhs = 1, minrhs = 0, maxrhs = 0;
+
+	CheckRhs( minrhs,maxrhs );
+	CheckLhs( minlhs,maxlhs );
+
+	cleanupp( );
+	qp_rowsH = -1;
+	qp_rowsA = -1;
+
+	return 0;
+}
+
+
+/*
+ *	i n t _ c l e a n u p S B
+ */
+int int_cleanupSB( char* fname )
+{
+	const int minlhs = 0, maxlhs = 1, minrhs = 0, maxrhs = 0;
+
+	CheckRhs( minrhs,maxrhs );
+	CheckLhs( minlhs,maxlhs );
+
+	cleanupSB( );
+	qpb_rowsH = -1;
+
+	return 0;
+}
+
+
+/*
+ *	i n t _ c l e a n u p V M
+ */
+int int_cleanupVM( char* fname )
+{
+	const int minlhs = 0, maxlhs = 1, minrhs = 0, maxrhs = 0;
+
+	CheckRhs( minrhs,maxrhs );
+	CheckLhs( minlhs,maxlhs );
+
+	cleanupVM( );
+	sqp_rowsH = -1;
+	sqp_rowsA = -1;
+
+	return 0;
+}
+
+
+/*
+ *	q p O A S E S g a t e w a y
+ */
+int C2F(qpOASESgateway)( )
+{
+	gate_function function[] = {	int_qpOASES,
+									int_init, int_initSB, int_initVM,
+									int_hotstart, int_hotstartSB, int_hotstartVM,
+									int_cleanup, int_cleanupSB, int_cleanupVM
+									};
+	char* name[] = {	"qpOASES",
+						"qpOASES_init", "qpOASES_initSB", "qpOASES_initVM",
+						"qpOASES_hotstart", "qpOASES_hotstartSB", "qpOASES_hotstartVM",
+						"qpOASES_cleanup", "qpOASES_cleanupSB", "qpOASES_cleanupVM"
+						};
+
+	Rhs = Max( 0,Rhs );
+	sci_gateway( name[Fin-1],function[Fin-1] );
+
+	return 0;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESinterface.sce b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESinterface.sce
new file mode 100644
index 00000000000..bbe36140e74
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESinterface.sce
@@ -0,0 +1,41 @@
+//
+//	This file is part of qpOASES.
+//
+//	qpOASES -- An Implementation of the Online Active Set Strategy.
+//	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+//	Christian Kirches et al. All rights reserved.
+//
+//	qpOASES 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.
+//
+//	qpOASES 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 qpOASES; if not, write to the Free Software
+//	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+//
+
+
+
+//
+//	Filename:  interfaces/scilab/qpOASESinterface.sci
+//	Author:    Holger Diedam, Hans Joachim Ferreau
+//	Version:   3.0
+//	Date:      2007-2014
+//
+
+
+
+sharedlib = link( './libqpOASESinterface.so' );
+addinter( './libqpOASESinterface.so', 'qpOASESgateway', ["qpOASES","qpOASES_init","qpOASES_initSB","qpOASES_initVM","qpOASES_hotstart","qpOASES_hotstartSB","qpOASES_hotstartVM","qpOASES_cleanup","qpOASES_cleanupSB","qpOASES_cleanupVM"] );
+
+
+
+//
+//	end of file
+//
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESroutines.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESroutines.cpp
new file mode 100644
index 00000000000..d84c2e1dd01
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESroutines.cpp
@@ -0,0 +1,365 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file interfaces/scilab/qpOASESroutines.cpp
+ *	\author Holger Diedam, Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Interface that enables to call qpOASES from scilab
+ *  (C++ file to provide an interface between the files that
+ *  have to be compiled with gcc and the qpOASES library).
+ *
+ */
+
+
+#include <scilab/Scierror.h>
+
+#include <qpOASES.hpp>
+
+
+USING_NAMESPACE_QPOASES
+
+/* global pointers to qpOASES objects */
+static QProblem*  qp  = 0;
+static QProblemB* qpb = 0;
+static SQProblem* sqp = 0;
+
+
+extern "C"
+{
+	void qpoases(	real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
+					int *nV, int* nC, int* nWSR,
+					real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
+					);
+
+	void init(		real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
+					int* nV, int* nC, int* nWSR,
+					real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
+					);
+	void initSB(	real_t* H, real_t* g, real_t* lb, real_t* ub,
+					int* nV, int* nWSR,
+					real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
+					);
+	void initVM(	real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
+					int* nV, int* nC, int* nWSR,
+					real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
+					);
+
+	void hotstart(		real_t* g, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
+						int* nWSR,
+						real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
+						);
+	void hotstartSB(	real_t* g, real_t* lb, real_t* ub,
+						int* nWSR,
+						real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
+						);
+	void hotstartVM(	real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
+						int* nWSR,
+						real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
+						);
+
+	void cleanupp( );
+	void cleanupSB( );
+	void cleanupVM( );
+} /* extern "C" */
+
+
+
+/*
+ *	t r a n s f o r m A
+ */
+void transformA( real_t* A, int nV, int nC )
+{
+	int i, j;
+
+	real_t* A_tmp = new real_t[nC*nV];
+
+	for( i=0; i<nV*nC; ++i )
+		A_tmp[i] = A[i];
+
+	for( i=0; i<nC; ++i )
+		for( j=0; j<nV; ++j )
+			A[i*nV + j] = A_tmp[j*nC + i];
+
+	delete[] A_tmp;
+
+	return;
+}
+
+
+/*
+ *	q p o a s e s
+ */
+void qpoases(	real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
+				int *nV, int* nC, int* nWSR,
+				real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
+				)
+{
+	/* transform A into C style matrix */
+	transformA( A, *nV,*nC );
+
+	/* setup and solve initial QP */
+	QProblem single_qp( *nV,*nC );
+	single_qp.setPrintLevel( PL_LOW );
+	returnValue returnvalue = single_qp.init( H,g,A,lb,ub,lbA,ubA, *nWSR,0 );
+
+	/* assign lhs arguments */
+	single_qp.getPrimalSolution( x );
+	*obj = single_qp.getObjVal( );
+	*status = getSimpleStatus( returnvalue );
+	*nWSRout = *nWSR;
+	single_qp.getDualSolution( y );
+
+	return;
+}
+
+
+/*
+ *	i n i t
+ */
+void init(	real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
+			int* nV, int* nC, int* nWSR,
+			real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
+			)
+{
+	cleanupp( );
+
+	/* transform A into C style matrix */
+	transformA( A, *nV,*nC );
+
+	/* setup and solve initial QP */
+	qp = new QProblem( *nV,*nC );
+	qp->setPrintLevel( PL_LOW );
+	returnValue returnvalue = qp->init( H,g,A,lb,ub,lbA,ubA, *nWSR,0 );
+
+	/* assign lhs arguments */
+	qp->getPrimalSolution( x );
+	*obj = qp->getObjVal( );
+	*status = getSimpleStatus( returnvalue );
+	*nWSRout = *nWSR;
+	qp->getDualSolution( y );
+
+	return;
+}
+
+
+/*
+ *	i n i t S B
+ */
+void initSB(	real_t* H, real_t* g, real_t* lb, real_t* ub,
+				int* nV, int* nWSR,
+				real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
+				)
+{
+	cleanupSB( );
+
+	/* setup and solve initial QP */
+	qpb = new QProblemB( *nV );
+	qpb->setPrintLevel( PL_LOW );
+	returnValue returnvalue = qpb->init( H,g,lb,ub, *nWSR,0 );
+
+	/* assign lhs arguments */
+	qpb->getPrimalSolution( x );
+	*obj = qpb->getObjVal( );
+	*status = getSimpleStatus( returnvalue );
+	*nWSRout = *nWSR;
+	qpb->getDualSolution( y );
+
+	return;
+}
+
+
+/*
+ *	i n i t V M
+ */
+void initVM(	real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
+				int* nV, int* nC, int* nWSR,
+				real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
+				)
+{
+	cleanupVM( );
+
+	/* transform A into C style matrix */
+	transformA( A, *nV,*nC );
+
+	/* setup and solve initial QP */
+	sqp = new SQProblem( *nV,*nC );
+	sqp->setPrintLevel( PL_LOW );
+	returnValue returnvalue = sqp->init( H,g,A,lb,ub,lbA,ubA, *nWSR,0 );
+
+	/* assign lhs arguments */
+	sqp->getPrimalSolution( x );
+	*obj = sqp->getObjVal( );
+	*status = getSimpleStatus( returnvalue );
+	*nWSRout = *nWSR;
+	sqp->getDualSolution( y );
+
+	return;
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+void hotstart(	real_t* g, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
+				int* nWSR,
+				real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
+				)
+{
+	/* has QP been initialised? */
+	if ( qp == 0 )
+	{
+		*status = -1;
+		Scierror( 999,"ERROR (qpOASES): Need to call qpOASES_init first!\n" );
+		return;
+	}
+
+	/* solve QP */
+	returnValue returnvalue = qp->hotstart( g,lb,ub,lbA,ubA, *nWSR,0 );
+
+	/* assign lhs arguments */
+	qp->getPrimalSolution( x );
+	*obj = qp->getObjVal( );
+	*status = getSimpleStatus( returnvalue );
+	*nWSRout = *nWSR;
+	qp->getDualSolution( y );
+
+	return;
+}
+
+
+/*
+ *	h o t s t a r t S B
+ */
+void hotstartSB(	real_t* g, real_t* lb, real_t* ub,
+					int* nWSR,
+					real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
+					)
+{
+	/* has QP been initialised? */
+	if ( qpb == 0 )
+	{
+		*status = -1;
+		Scierror( 999,"ERROR (qpOASES): Need to call qpOASES_init first!\n" );
+		return;
+	}
+
+	/* solve QP */
+	returnValue returnvalue = qpb->hotstart( g,lb,ub, *nWSR,0 );
+
+	/* assign lhs arguments */
+	qpb->getPrimalSolution( x );
+	*obj = qpb->getObjVal( );
+	*status = getSimpleStatus( returnvalue );
+	*nWSRout = *nWSR;
+	qpb->getDualSolution( y );
+
+	return;
+}
+
+
+/*
+ *	h o t s t a r t V M
+ */
+void hotstartVM(	real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
+					int* nWSR,
+					real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
+					)
+{
+	/* has QP been initialised? */
+	if ( sqp == 0 )
+	{
+		*status = -1;
+		Scierror( 999,"ERROR (qpOASES): Need to call qpOASES_init first!\n" );
+		return;
+	}
+
+	/* transform A into C style matrix */
+	transformA( A, sqp->getNV( ),sqp->getNC( ) );
+
+	/* solve QP */
+	returnValue returnvalue = sqp->hotstart( H,g,A,lb,ub,lbA,ubA, *nWSR,0 );
+
+	/* assign lhs arguments */
+	sqp->getPrimalSolution( x );
+	*obj = sqp->getObjVal( );
+	*status = getSimpleStatus( returnvalue );
+	*nWSRout = *nWSR;
+	sqp->getDualSolution( y );
+
+	return;
+}
+
+
+/*
+ *	c l e a n u p p
+ */
+void cleanupp( )
+{
+	/* Remark: A function cleanup already exists! */
+	if ( qp != 0 )
+	{
+		delete qp;
+		qp = 0;
+	}
+
+	return;
+}
+
+
+/*
+ *	c l e a n u p S B
+ */
+void cleanupSB( )
+{
+	if ( qpb != 0 )
+	{
+		delete qpb;
+		qpb = 0;
+	}
+
+	return;
+}
+
+
+/*
+ *	c l e a n u p V M
+ */
+void cleanupVM( )
+{
+	if ( sqp != 0 )
+	{
+		delete sqp;
+		sqp = 0;
+	}
+
+	return;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_QProblem.mdl b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_QProblem.mdl
new file mode 100644
index 00000000000..6b284674b4d
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_QProblem.mdl
@@ -0,0 +1,803 @@
+Model {
+  Name			  "example_QProblem"
+  Version		  7.4
+  MdlSubVersion		  0
+  GraphicalInterface {
+    NumRootInports	    0
+    NumRootOutports	    0
+    ParameterArgumentNames  ""
+    ComputedModelVersion    "1.65"
+    NumModelReferences	    0
+    NumTestPointedSignals   0
+  }
+  SavedCharacterEncoding  "UTF-8"
+  SaveDefaultBlockParams  on
+  ScopeRefreshTime	  0.035000
+  OverrideScopeRefreshTime on
+  DisableAllScopes	  off
+  DataTypeOverride	  "UseLocalSettings"
+  MinMaxOverflowLogging	  "UseLocalSettings"
+  MinMaxOverflowArchiveMode "Overwrite"
+  MaxMDLFileLineLength	  120
+  Created		  "Fri Apr 13 11:08:51 2007"
+  Creator		  "jferreau"
+  UpdateHistory		  "UpdateHistoryNever"
+  ModifiedByFormat	  "%<Auto>"
+  LastModifiedBy	  "chjofer2"
+  ModifiedDateFormat	  "%<Auto>"
+  LastModifiedDate	  "Thu Feb 20 15:11:24 2014"
+  RTWModifiedTimeStamp	  314809883
+  ModelVersionFormat	  "1.%<AutoIncrement:65>"
+  ConfigurationManager	  "None"
+  SampleTimeColors	  off
+  SampleTimeAnnotations	  off
+  LibraryLinkDisplay	  "none"
+  WideLines		  off
+  ShowLineDimensions	  off
+  ShowPortDataTypes	  off
+  ShowLoopsOnError	  on
+  IgnoreBidirectionalLines off
+  ShowStorageClass	  off
+  ShowTestPointIcons	  on
+  ShowSignalResolutionIcons on
+  ShowViewerIcons	  on
+  SortedOrder		  off
+  ExecutionContextIcon	  off
+  ShowLinearizationAnnotations on
+  BlockNameDataTip	  off
+  BlockParametersDataTip  off
+  BlockDescriptionStringDataTip	off
+  ToolBar		  on
+  StatusBar		  on
+  BrowserShowLibraryLinks off
+  BrowserLookUnderMasks	  off
+  SimulationMode	  "normal"
+  LinearizationMsg	  "none"
+  Profile		  off
+  ParamWorkspaceSource	  "MATLABWorkspace"
+  AccelSystemTargetFile	  "accel.tlc"
+  AccelTemplateMakefile	  "accel_default_tmf"
+  AccelMakeCommand	  "make_rtw"
+  TryForcingSFcnDF	  off
+  RecordCoverage	  off
+  CovPath		  "/"
+  CovSaveName		  "covdata"
+  CovMetricSettings	  "dw"
+  CovNameIncrementing	  off
+  CovHtmlReporting	  on
+  CovForceBlockReductionOff on
+  covSaveCumulativeToWorkspaceVar on
+  CovSaveSingleToWorkspaceVar on
+  CovCumulativeVarName	  "covCumulativeData"
+  CovCumulativeReport	  off
+  CovReportOnPause	  on
+  CovModelRefEnable	  "Off"
+  CovExternalEMLEnable	  off
+  ExtModeBatchMode	  off
+  ExtModeEnableFloating	  on
+  ExtModeTrigType	  "manual"
+  ExtModeTrigMode	  "normal"
+  ExtModeTrigPort	  "1"
+  ExtModeTrigElement	  "any"
+  ExtModeTrigDuration	  1000
+  ExtModeTrigDurationFloating "auto"
+  ExtModeTrigHoldOff	  0
+  ExtModeTrigDelay	  0
+  ExtModeTrigDirection	  "rising"
+  ExtModeTrigLevel	  0
+  ExtModeArchiveMode	  "off"
+  ExtModeAutoIncOneShot	  off
+  ExtModeIncDirWhenArm	  off
+  ExtModeAddSuffixToVar	  off
+  ExtModeWriteAllDataToWs off
+  ExtModeArmWhenConnect	  on
+  ExtModeSkipDownloadWhenConnect off
+  ExtModeLogAll		  on
+  ExtModeAutoUpdateStatusClock off
+  BufferReuse		  on
+  ShowModelReferenceBlockVersion off
+  ShowModelReferenceBlockIO off
+  Array {
+    Type		    "Handle"
+    Dimension		    1
+    Simulink.ConfigSet {
+      $ObjectID		      1
+      Version		      "1.6.0"
+      Array {
+	Type			"Handle"
+	Dimension		8
+	Simulink.SolverCC {
+	  $ObjectID		  2
+	  Version		  "1.6.0"
+	  StartTime		  "0.0"
+	  StopTime		  "0.5"
+	  AbsTol		  "auto"
+	  FixedStep		  "auto"
+	  InitialStep		  "auto"
+	  MaxNumMinSteps	  "-1"
+	  MaxOrder		  5
+	  ZcThreshold		  "auto"
+	  ConsecutiveZCsStepRelTol "10*128*eps"
+	  MaxConsecutiveZCs	  "1000"
+	  ExtrapolationOrder	  4
+	  NumberNewtonIterations  1
+	  MaxStep		  "auto"
+	  MinStep		  "auto"
+	  MaxConsecutiveMinStep	  "1"
+	  RelTol		  "1e-3"
+	  SolverMode		  "Auto"
+	  Solver		  "FixedStepDiscrete"
+	  SolverName		  "FixedStepDiscrete"
+	  ShapePreserveControl	  "DisableAll"
+	  ZeroCrossControl	  "UseLocalSettings"
+	  ZeroCrossAlgorithm	  "Nonadaptive"
+	  AlgebraicLoopSolver	  "TrustRegion"
+	  SolverResetMethod	  "Fast"
+	  PositivePriorityOrder	  off
+	  AutoInsertRateTranBlk	  off
+	  SampleTimeConstraint	  "Unconstrained"
+	  InsertRTBMode		  "Whenever possible"
+	}
+	Simulink.DataIOCC {
+	  $ObjectID		  3
+	  Version		  "1.6.0"
+	  Decimation		  "1"
+	  ExternalInput		  "[t, u]"
+	  FinalStateName	  "xFinal"
+	  InitialState		  "xInitial"
+	  LimitDataPoints	  on
+	  MaxDataPoints		  "1000"
+	  LoadExternalInput	  off
+	  LoadInitialState	  off
+	  SaveFinalState	  off
+	  SaveCompleteFinalSimState off
+	  SaveFormat		  "Array"
+	  SaveOutput		  on
+	  SaveState		  off
+	  SignalLogging		  on
+	  InspectSignalLogs	  off
+	  SaveTime		  on
+	  ReturnWorkspaceOutputs  off
+	  StateSaveName		  "xout"
+	  TimeSaveName		  "tout"
+	  OutputSaveName	  "yout"
+	  SignalLoggingName	  "logsout"
+	  OutputOption		  "RefineOutputTimes"
+	  OutputTimes		  "[]"
+	  ReturnWorkspaceOutputsName "out"
+	  Refine		  "1"
+	}
+	Simulink.OptimizationCC {
+	  $ObjectID		  4
+	  Version		  "1.6.0"
+	  Array {
+	    Type		    "Cell"
+	    Dimension		    4
+	    Cell		    "ZeroExternalMemoryAtStartup"
+	    Cell		    "ZeroInternalMemoryAtStartup"
+	    Cell		    "NoFixptDivByZeroProtection"
+	    Cell		    "OptimizeModelRefInitCode"
+	    PropName		    "DisabledProps"
+	  }
+	  BlockReduction	  on
+	  BooleanDataType	  on
+	  ConditionallyExecuteInputs on
+	  InlineParams		  off
+	  UseIntDivNetSlope	  off
+	  InlineInvariantSignals  off
+	  OptimizeBlockIOStorage  on
+	  BufferReuse		  on
+	  EnhancedBackFolding	  off
+	  StrengthReduction	  off
+	  EnforceIntegerDowncast  on
+	  ExpressionFolding	  on
+	  BooleansAsBitfields	  off
+	  EnableMemcpy		  on
+	  MemcpyThreshold	  64
+	  PassReuseOutputArgsAs	  "Structure reference"
+	  ExpressionDepthLimit	  2147483647
+	  FoldNonRolledExpr	  on
+	  LocalBlockOutputs	  on
+	  RollThreshold		  5
+	  SystemCodeInlineAuto	  off
+	  StateBitsets		  off
+	  DataBitsets		  off
+	  UseTempVars		  off
+	  ZeroExternalMemoryAtStartup on
+	  ZeroInternalMemoryAtStartup on
+	  InitFltsAndDblsToZero	  on
+	  NoFixptDivByZeroProtection off
+	  EfficientFloat2IntCast  off
+	  EfficientMapNaN2IntZero on
+	  OptimizeModelRefInitCode off
+	  LifeSpan		  "inf"
+	  BufferReusableBoundary  on
+	  SimCompilerOptimization "Off"
+	  AccelVerboseBuild	  off
+	}
+	Simulink.DebuggingCC {
+	  $ObjectID		  5
+	  Version		  "1.6.0"
+	  RTPrefix		  "error"
+	  ConsistencyChecking	  "none"
+	  ArrayBoundsChecking	  "none"
+	  SignalInfNanChecking	  "none"
+	  SignalRangeChecking	  "none"
+	  ReadBeforeWriteMsg	  "UseLocalSettings"
+	  WriteAfterWriteMsg	  "UseLocalSettings"
+	  WriteAfterReadMsg	  "UseLocalSettings"
+	  AlgebraicLoopMsg	  "warning"
+	  ArtificialAlgebraicLoopMsg "warning"
+	  SaveWithDisabledLinksMsg "warning"
+	  SaveWithParameterizedLinksMsg	"none"
+	  CheckSSInitialOutputMsg on
+	  UnderspecifiedInitializationDetection	"Classic"
+	  MergeDetectMultiDrivingBlocksExec "none"
+	  CheckExecutionContextPreStartOutputMsg off
+	  CheckExecutionContextRuntimeOutputMsg	off
+	  SignalResolutionControl "TryResolveAllWithWarning"
+	  BlockPriorityViolationMsg "warning"
+	  MinStepSizeMsg	  "warning"
+	  TimeAdjustmentMsg	  "none"
+	  MaxConsecutiveZCsMsg	  "error"
+	  SolverPrmCheckMsg	  "warning"
+	  InheritedTsInSrcMsg	  "warning"
+	  DiscreteInheritContinuousMsg "warning"
+	  MultiTaskDSMMsg	  "error"
+	  MultiTaskCondExecSysMsg "error"
+	  MultiTaskRateTransMsg	  "error"
+	  SingleTaskRateTransMsg  "none"
+	  TasksWithSamePriorityMsg "warning"
+	  SigSpecEnsureSampleTimeMsg "warning"
+	  CheckMatrixSingularityMsg "none"
+	  IntegerOverflowMsg	  "warning"
+	  Int32ToFloatConvMsg	  "warning"
+	  ParameterDowncastMsg	  "error"
+	  ParameterOverflowMsg	  "error"
+	  ParameterUnderflowMsg	  "none"
+	  ParameterPrecisionLossMsg "warning"
+	  ParameterTunabilityLossMsg "warning"
+	  FixptConstUnderflowMsg  "none"
+	  FixptConstOverflowMsg	  "none"
+	  FixptConstPrecisionLossMsg "none"
+	  UnderSpecifiedDataTypeMsg "none"
+	  UnnecessaryDatatypeConvMsg "none"
+	  VectorMatrixConversionMsg "none"
+	  InvalidFcnCallConnMsg	  "error"
+	  FcnCallInpInsideContextMsg "Use local settings"
+	  SignalLabelMismatchMsg  "none"
+	  UnconnectedInputMsg	  "warning"
+	  UnconnectedOutputMsg	  "warning"
+	  UnconnectedLineMsg	  "warning"
+	  SFcnCompatibilityMsg	  "none"
+	  UniqueDataStoreMsg	  "none"
+	  BusObjectLabelMismatch  "warning"
+	  RootOutportRequireBusObject "warning"
+	  AssertControl		  "UseLocalSettings"
+	  EnableOverflowDetection off
+	  ModelReferenceIOMsg	  "none"
+	  ModelReferenceVersionMismatchMessage "none"
+	  ModelReferenceIOMismatchMessage "none"
+	  ModelReferenceCSMismatchMessage "none"
+	  UnknownTsInhSupMsg	  "warning"
+	  ModelReferenceDataLoggingMessage "warning"
+	  ModelReferenceSymbolNameMessage "warning"
+	  ModelReferenceExtraNoncontSigs "error"
+	  StateNameClashWarn	  "warning"
+	  SimStateInterfaceChecksumMismatchMsg "warning"
+	  StrictBusMsg		  "Warning"
+	  LoggingUnavailableSignals "error"
+	  BlockIODiagnostic	  "none"
+	}
+	Simulink.HardwareCC {
+	  $ObjectID		  6
+	  Version		  "1.6.0"
+	  ProdBitPerChar	  8
+	  ProdBitPerShort	  16
+	  ProdBitPerInt		  32
+	  ProdBitPerLong	  32
+	  ProdIntDivRoundTo	  "Undefined"
+	  ProdEndianess		  "Unspecified"
+	  ProdWordSize		  32
+	  ProdShiftRightIntArith  on
+	  ProdHWDeviceType	  "32-bit Generic"
+	  TargetBitPerChar	  8
+	  TargetBitPerShort	  16
+	  TargetBitPerInt	  32
+	  TargetBitPerLong	  32
+	  TargetShiftRightIntArith on
+	  TargetIntDivRoundTo	  "Undefined"
+	  TargetEndianess	  "Unspecified"
+	  TargetWordSize	  32
+	  TargetTypeEmulationWarnSuppressLevel 0
+	  TargetPreprocMaxBitsSint 32
+	  TargetPreprocMaxBitsUint 32
+	  TargetHWDeviceType	  "Specified"
+	  TargetUnknown		  off
+	  ProdEqTarget		  on
+	}
+	Simulink.ModelReferenceCC {
+	  $ObjectID		  7
+	  Version		  "1.6.0"
+	  UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange"
+	  CheckModelReferenceTargetMessage "error"
+	  ModelReferenceNumInstancesAllowed "Multi"
+	  ModelReferencePassRootInputsByReference on
+	  ModelReferenceMinAlgLoopOccurrences off
+	}
+	Simulink.SFSimCC {
+	  $ObjectID		  8
+	  Version		  "1.6.0"
+	  SFSimEnableDebug	  on
+	  SFSimOverflowDetection  on
+	  SFSimEcho		  on
+	  SimBlas		  on
+	  SimCtrlC		  on
+	  SimExtrinsic		  on
+	  SimIntegrity		  on
+	  SimUseLocalCustomCode	  off
+	  SimBuildMode		  "sf_incremental_build"
+	}
+	Simulink.RTWCC {
+	  $BackupClass		  "Simulink.RTWCC"
+	  $ObjectID		  9
+	  Version		  "1.6.0"
+	  Array {
+	    Type		    "Cell"
+	    Dimension		    1
+	    Cell		    "IncludeHyperlinkInReport"
+	    PropName		    "DisabledProps"
+	  }
+	  SystemTargetFile	  "grt.tlc"
+	  GenCodeOnly		  off
+	  MakeCommand		  "make_rtw"
+	  GenerateMakefile	  on
+	  TemplateMakefile	  "grt_default_tmf"
+	  GenerateReport	  off
+	  SaveLog		  off
+	  RTWVerbose		  on
+	  RetainRTWFile		  off
+	  ProfileTLC		  off
+	  TLCDebug		  off
+	  TLCCoverage		  off
+	  TLCAssert		  off
+	  ProcessScriptMode	  "Default"
+	  ConfigurationMode	  "Optimized"
+	  ConfigAtBuild		  off
+	  RTWUseLocalCustomCode	  off
+	  RTWUseSimCustomCode	  off
+	  IncludeHyperlinkInReport off
+	  LaunchReport		  off
+	  TargetLang		  "C"
+	  IncludeBusHierarchyInRTWFileBlockHierarchyMap	off
+	  IncludeERTFirstTime	  off
+	  GenerateTraceInfo	  off
+	  GenerateTraceReport	  off
+	  GenerateTraceReportSl	  off
+	  GenerateTraceReportSf	  off
+	  GenerateTraceReportEml  off
+	  GenerateCodeInfo	  off
+	  RTWCompilerOptimization "Off"
+	  CheckMdlBeforeBuild	  "Off"
+	  Array {
+	    Type		    "Handle"
+	    Dimension		    2
+	    Simulink.CodeAppCC {
+	      $ObjectID		      10
+	      Version		      "1.6.0"
+	      Array {
+		Type			"Cell"
+		Dimension		16
+		Cell			"IgnoreCustomStorageClasses"
+		Cell			"InsertBlockDesc"
+		Cell			"SFDataObjDesc"
+		Cell			"SimulinkDataObjDesc"
+		Cell			"DefineNamingRule"
+		Cell			"SignalNamingRule"
+		Cell			"ParamNamingRule"
+		Cell			"InlinedPrmAccess"
+		Cell			"CustomSymbolStr"
+		Cell			"CustomSymbolStrGlobalVar"
+		Cell			"CustomSymbolStrType"
+		Cell			"CustomSymbolStrField"
+		Cell			"CustomSymbolStrFcn"
+		Cell			"CustomSymbolStrBlkIO"
+		Cell			"CustomSymbolStrTmpVar"
+		Cell			"CustomSymbolStrMacro"
+		PropName		"DisabledProps"
+	      }
+	      ForceParamTrailComments off
+	      GenerateComments	      on
+	      IgnoreCustomStorageClasses on
+	      IgnoreTestpoints	      off
+	      IncHierarchyInIds	      off
+	      MaxIdLength	      31
+	      PreserveName	      off
+	      PreserveNameWithParent  off
+	      ShowEliminatedStatement off
+	      IncAutoGenComments      off
+	      SimulinkDataObjDesc     off
+	      SFDataObjDesc	      off
+	      IncDataTypeInIds	      off
+	      MangleLength	      1
+	      CustomSymbolStrGlobalVar "$R$N$M"
+	      CustomSymbolStrType     "$N$R$M"
+	      CustomSymbolStrField    "$N$M"
+	      CustomSymbolStrFcn      "$R$N$M$F"
+	      CustomSymbolStrFcnArg   "rt$I$N$M"
+	      CustomSymbolStrBlkIO    "rtb_$N$M"
+	      CustomSymbolStrTmpVar   "$N$M"
+	      CustomSymbolStrMacro    "$R$N$M"
+	      DefineNamingRule	      "None"
+	      ParamNamingRule	      "None"
+	      SignalNamingRule	      "None"
+	      InsertBlockDesc	      off
+	      SimulinkBlockComments   on
+	      EnableCustomComments    off
+	      InlinedPrmAccess	      "Literals"
+	      ReqsInCode	      off
+	      UseSimReservedNames     off
+	    }
+	    Simulink.GRTTargetCC {
+	      $BackupClass	      "Simulink.TargetCC"
+	      $ObjectID		      11
+	      Version		      "1.6.0"
+	      Array {
+		Type			"Cell"
+		Dimension		13
+		Cell			"IncludeMdlTerminateFcn"
+		Cell			"CombineOutputUpdateFcns"
+		Cell			"SuppressErrorStatus"
+		Cell			"ERTCustomFileBanners"
+		Cell			"GenerateSampleERTMain"
+		Cell			"GenerateTestInterfaces"
+		Cell			"MultiInstanceERTCode"
+		Cell			"PurelyIntegerCode"
+		Cell			"SupportNonFinite"
+		Cell			"SupportComplex"
+		Cell			"SupportAbsoluteTime"
+		Cell			"SupportContinuousTime"
+		Cell			"SupportNonInlinedSFcns"
+		PropName		"DisabledProps"
+	      }
+	      TargetFcnLib	      "ansi_tfl_tmw.mat"
+	      TargetLibSuffix	      ""
+	      TargetPreCompLibLocation ""
+	      TargetFunctionLibrary   "ANSI_C"
+	      UtilityFuncGeneration   "Auto"
+	      ERTMultiwordTypeDef     "System defined"
+	      ERTMultiwordLength      256
+	      MultiwordLength	      2048
+	      GenerateFullHeader      on
+	      GenerateSampleERTMain   off
+	      GenerateTestInterfaces  off
+	      IsPILTarget	      off
+	      ModelReferenceCompliant on
+	      ParMdlRefBuildCompliant on
+	      CompOptLevelCompliant   on
+	      IncludeMdlTerminateFcn  on
+	      GeneratePreprocessorConditionals "Disable all"
+	      CombineOutputUpdateFcns off
+	      SuppressErrorStatus     off
+	      ERTFirstTimeCompliant   off
+	      IncludeFileDelimiter    "Auto"
+	      ERTCustomFileBanners    off
+	      SupportAbsoluteTime     on
+	      LogVarNameModifier      "rt_"
+	      MatFileLogging	      on
+	      MultiInstanceERTCode    off
+	      SupportNonFinite	      on
+	      SupportComplex	      on
+	      PurelyIntegerCode	      off
+	      SupportContinuousTime   on
+	      SupportNonInlinedSFcns  on
+	      SupportVariableSizeSignals off
+	      EnableShiftOperators    on
+	      ParenthesesLevel	      "Nominal"
+	      PortableWordSizes	      off
+	      ModelStepFunctionPrototypeControlCompliant off
+	      CPPClassGenCompliant    off
+	      AutosarCompliant	      off
+	      UseMalloc		      off
+	      ExtMode		      off
+	      ExtModeStaticAlloc      off
+	      ExtModeTesting	      off
+	      ExtModeStaticAllocSize  1000000
+	      ExtModeTransport	      0
+	      ExtModeMexFile	      "ext_comm"
+	      ExtModeIntrfLevel	      "Level1"
+	      RTWCAPISignals	      off
+	      RTWCAPIParams	      off
+	      RTWCAPIStates	      off
+	      GenerateASAP2	      off
+	    }
+	    PropName		    "Components"
+	  }
+	}
+	PropName		"Components"
+      }
+      Name		      "Configuration"
+      CurrentDlgPage	      "Solver"
+      ConfigPrmDlgPosition    " [ 200, 197, 1080, 827 ] "
+    }
+    PropName		    "ConfigurationSets"
+  }
+  Simulink.ConfigSet {
+    $PropName		    "ActiveConfigurationSet"
+    $ObjectID		    1
+  }
+  BlockDefaults {
+    ForegroundColor	    "black"
+    BackgroundColor	    "white"
+    DropShadow		    off
+    NamePlacement	    "normal"
+    FontName		    "Courier"
+    FontSize		    10
+    FontWeight		    "normal"
+    FontAngle		    "normal"
+    ShowName		    on
+    BlockRotation	    0
+    BlockMirror		    off
+  }
+  AnnotationDefaults {
+    HorizontalAlignment	    "center"
+    VerticalAlignment	    "middle"
+    ForegroundColor	    "black"
+    BackgroundColor	    "white"
+    DropShadow		    off
+    FontName		    "Courier"
+    FontSize		    10
+    FontWeight		    "normal"
+    FontAngle		    "normal"
+    UseDisplayTextAsClickCallback off
+  }
+  LineDefaults {
+    FontName		    "Courier"
+    FontSize		    9
+    FontWeight		    "normal"
+    FontAngle		    "normal"
+  }
+  BlockParameterDefaults {
+    Block {
+      BlockType		      FromWorkspace
+      VariableName	      "simulink_input"
+      SampleTime	      "-1"
+      Interpolate	      on
+      ZeroCross		      off
+      OutputAfterFinalValue   "Extrapolation"
+    }
+    Block {
+      BlockType		      "S-Function"
+      FunctionName	      "system"
+      SFunctionModules	      "''"
+      PortCounts	      "[]"
+      SFunctionDeploymentMode off
+    }
+    Block {
+      BlockType		      Scope
+      ModelBased	      off
+      TickLabels	      "OneTimeTick"
+      ZoomMode		      "on"
+      Grid		      "on"
+      TimeRange		      "auto"
+      YMin		      "-5"
+      YMax		      "5"
+      SaveToWorkspace	      off
+      SaveName		      "ScopeData"
+      LimitDataPoints	      on
+      MaxDataPoints	      "5000"
+      Decimation	      "1"
+      SampleInput	      off
+      SampleTime	      "-1"
+    }
+  }
+  System {
+    Name		    "example_QProblem"
+    Location		    [2, 82, 1918, 1147]
+    Open		    on
+    ModelBrowserVisibility  off
+    ModelBrowserWidth	    200
+    ScreenColor		    "white"
+    PaperOrientation	    "landscape"
+    PaperPositionMode	    "auto"
+    PaperType		    "usletter"
+    PaperUnits		    "inches"
+    TiledPaperMargins	    [0.500000, 0.500000, 0.500000, 0.500000]
+    TiledPageScale	    1
+    ShowPageBoundaries	    off
+    ZoomFactor		    "125"
+    ReportName		    "simulink-default.rpt"
+    SIDHighWatermark	    9
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace1"
+      SID		      1
+      Position		      [85, 53, 150, 77]
+      VariableName	      "H"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace2"
+      SID		      2
+      Position		      [85, 113, 150, 137]
+      VariableName	      "g"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace3"
+      SID		      3
+      Position		      [85, 173, 150, 197]
+      VariableName	      "A"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace4"
+      SID		      4
+      Position		      [85, 233, 150, 257]
+      VariableName	      "lb"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace5"
+      SID		      5
+      Position		      [85, 293, 150, 317]
+      VariableName	      "ub"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace6"
+      SID		      6
+      Position		      [85, 353, 150, 377]
+      VariableName	      "lbA"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace7"
+      SID		      7
+      Position		      [85, 418, 150, 442]
+      VariableName	      "ubA"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      Scope
+      Name		      "fval,\nx,\nexitflag,\niter"
+      SID		      8
+      Ports		      [4]
+      Position		      [915, 229, 945, 261]
+      Floating		      off
+      Location		      [6, 78, 1276, 993]
+      Open		      off
+      NumInputPorts	      "4"
+      List {
+	ListType		AxesTitles
+	axes1			"%<SignalLabel>"
+	axes2			"%<SignalLabel>"
+	axes3			"%<SignalLabel>"
+	axes4			"%<SignalLabel>"
+      }
+      YMin		      "-5~-5~-5~-5"
+      YMax		      "5~5~5~5"
+      SaveName		      "ScopeData1"
+      DataFormat	      "StructureWithTime"
+      SampleTime	      "0"
+    }
+    Block {
+      BlockType		      "S-Function"
+      Name		      "qpOASES"
+      SID		      9
+      Ports		      [7, 4]
+      Position		      [595, 226, 790, 264]
+      BackgroundColor	      "[1.000000, 0.915850, 0.439000]"
+      FunctionName	      "qpOASES_QProblem"
+      EnableBusSupport	      off
+      MaskHelp		      "file://../../DOC/manual.pdf"
+      MaskIconFrame	      on
+      MaskIconOpaque	      on
+      MaskIconRotate	      "none"
+      MaskPortRotate	      "default"
+      MaskIconUnits	      "autoscale"
+    }
+    Line {
+      SrcBlock		      "From\nWorkspace1"
+      SrcPort		      1
+      Points		      [180, 0; 0, 165]
+      DstBlock		      "qpOASES"
+      DstPort		      1
+    }
+    Line {
+      SrcBlock		      "From\nWorkspace2"
+      SrcPort		      1
+      Points		      [115, 0; 0, 110]
+      DstBlock		      "qpOASES"
+      DstPort		      2
+    }
+    Line {
+      Labels		      [3, 0]
+      SrcBlock		      "From\nWorkspace7"
+      SrcPort		      1
+      Points		      [180, 0; 0, -170]
+      DstBlock		      "qpOASES"
+      DstPort		      7
+    }
+    Line {
+      SrcBlock		      "From\nWorkspace5"
+      SrcPort		      1
+      Points		      [55, 0; 0, -55]
+      DstBlock		      "qpOASES"
+      DstPort		      5
+    }
+    Line {
+      SrcBlock		      "From\nWorkspace4"
+      SrcPort		      1
+      DstBlock		      "qpOASES"
+      DstPort		      4
+    }
+    Line {
+      SrcBlock		      "From\nWorkspace3"
+      SrcPort		      1
+      Points		      [55, 0; 0, 55]
+      DstBlock		      "qpOASES"
+      DstPort		      3
+    }
+    Line {
+      Labels		      [3, 0]
+      SrcBlock		      "From\nWorkspace6"
+      SrcPort		      1
+      Points		      [115, 0; 0, -110]
+      DstBlock		      "qpOASES"
+      DstPort		      6
+    }
+    Line {
+      SrcBlock		      "qpOASES"
+      SrcPort		      1
+      DstBlock		      "fval,\nx,\nexitflag,\niter"
+      DstPort		      1
+    }
+    Line {
+      SrcBlock		      "qpOASES"
+      SrcPort		      2
+      DstBlock		      "fval,\nx,\nexitflag,\niter"
+      DstPort		      2
+    }
+    Line {
+      SrcBlock		      "qpOASES"
+      SrcPort		      3
+      DstBlock		      "fval,\nx,\nexitflag,\niter"
+      DstPort		      3
+    }
+    Line {
+      SrcBlock		      "qpOASES"
+      SrcPort		      4
+      DstBlock		      "fval,\nx,\nexitflag,\niter"
+      DstPort		      4
+    }
+    Annotation {
+      Position		      [335, 96]
+    }
+    Annotation {
+      Name		      "This file is part of qpOASES.\n\nqpOASES -- An Implementation of the Online Active Set Strategy.\nC"
+      "opyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka, Christian Kirches et al.\nAll rights reserved."
+      Position		      [693, 128]
+    }
+  }
+}
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_QProblemB.mdl b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_QProblemB.mdl
new file mode 100644
index 00000000000..e86ed8e85aa
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_QProblemB.mdl
@@ -0,0 +1,745 @@
+Model {
+  Name			  "example_QProblemB"
+  Version		  7.4
+  MdlSubVersion		  0
+  GraphicalInterface {
+    NumRootInports	    0
+    NumRootOutports	    0
+    ParameterArgumentNames  ""
+    ComputedModelVersion    "1.64"
+    NumModelReferences	    0
+    NumTestPointedSignals   0
+  }
+  SavedCharacterEncoding  "UTF-8"
+  SaveDefaultBlockParams  on
+  ScopeRefreshTime	  0.035000
+  OverrideScopeRefreshTime on
+  DisableAllScopes	  off
+  DataTypeOverride	  "UseLocalSettings"
+  MinMaxOverflowLogging	  "UseLocalSettings"
+  MinMaxOverflowArchiveMode "Overwrite"
+  MaxMDLFileLineLength	  120
+  Created		  "Fri Apr 13 11:08:51 2007"
+  Creator		  "jferreau"
+  UpdateHistory		  "UpdateHistoryNever"
+  ModifiedByFormat	  "%<Auto>"
+  LastModifiedBy	  "chjofer2"
+  ModifiedDateFormat	  "%<Auto>"
+  LastModifiedDate	  "Thu Feb 20 15:12:19 2014"
+  RTWModifiedTimeStamp	  314809937
+  ModelVersionFormat	  "1.%<AutoIncrement:64>"
+  ConfigurationManager	  "None"
+  SampleTimeColors	  off
+  SampleTimeAnnotations	  off
+  LibraryLinkDisplay	  "none"
+  WideLines		  off
+  ShowLineDimensions	  off
+  ShowPortDataTypes	  off
+  ShowLoopsOnError	  on
+  IgnoreBidirectionalLines off
+  ShowStorageClass	  off
+  ShowTestPointIcons	  on
+  ShowSignalResolutionIcons on
+  ShowViewerIcons	  on
+  SortedOrder		  off
+  ExecutionContextIcon	  off
+  ShowLinearizationAnnotations on
+  BlockNameDataTip	  off
+  BlockParametersDataTip  off
+  BlockDescriptionStringDataTip	off
+  ToolBar		  on
+  StatusBar		  on
+  BrowserShowLibraryLinks off
+  BrowserLookUnderMasks	  off
+  SimulationMode	  "normal"
+  LinearizationMsg	  "none"
+  Profile		  off
+  ParamWorkspaceSource	  "MATLABWorkspace"
+  AccelSystemTargetFile	  "accel.tlc"
+  AccelTemplateMakefile	  "accel_default_tmf"
+  AccelMakeCommand	  "make_rtw"
+  TryForcingSFcnDF	  off
+  RecordCoverage	  off
+  CovPath		  "/"
+  CovSaveName		  "covdata"
+  CovMetricSettings	  "dw"
+  CovNameIncrementing	  off
+  CovHtmlReporting	  on
+  CovForceBlockReductionOff on
+  covSaveCumulativeToWorkspaceVar on
+  CovSaveSingleToWorkspaceVar on
+  CovCumulativeVarName	  "covCumulativeData"
+  CovCumulativeReport	  off
+  CovReportOnPause	  on
+  CovModelRefEnable	  "Off"
+  CovExternalEMLEnable	  off
+  ExtModeBatchMode	  off
+  ExtModeEnableFloating	  on
+  ExtModeTrigType	  "manual"
+  ExtModeTrigMode	  "normal"
+  ExtModeTrigPort	  "1"
+  ExtModeTrigElement	  "any"
+  ExtModeTrigDuration	  1000
+  ExtModeTrigDurationFloating "auto"
+  ExtModeTrigHoldOff	  0
+  ExtModeTrigDelay	  0
+  ExtModeTrigDirection	  "rising"
+  ExtModeTrigLevel	  0
+  ExtModeArchiveMode	  "off"
+  ExtModeAutoIncOneShot	  off
+  ExtModeIncDirWhenArm	  off
+  ExtModeAddSuffixToVar	  off
+  ExtModeWriteAllDataToWs off
+  ExtModeArmWhenConnect	  on
+  ExtModeSkipDownloadWhenConnect off
+  ExtModeLogAll		  on
+  ExtModeAutoUpdateStatusClock off
+  BufferReuse		  on
+  ShowModelReferenceBlockVersion off
+  ShowModelReferenceBlockIO off
+  Array {
+    Type		    "Handle"
+    Dimension		    1
+    Simulink.ConfigSet {
+      $ObjectID		      1
+      Version		      "1.6.0"
+      Array {
+	Type			"Handle"
+	Dimension		8
+	Simulink.SolverCC {
+	  $ObjectID		  2
+	  Version		  "1.6.0"
+	  StartTime		  "0.0"
+	  StopTime		  "0.5"
+	  AbsTol		  "auto"
+	  FixedStep		  "auto"
+	  InitialStep		  "auto"
+	  MaxNumMinSteps	  "-1"
+	  MaxOrder		  5
+	  ZcThreshold		  "auto"
+	  ConsecutiveZCsStepRelTol "10*128*eps"
+	  MaxConsecutiveZCs	  "1000"
+	  ExtrapolationOrder	  4
+	  NumberNewtonIterations  1
+	  MaxStep		  "auto"
+	  MinStep		  "auto"
+	  MaxConsecutiveMinStep	  "1"
+	  RelTol		  "1e-3"
+	  SolverMode		  "Auto"
+	  Solver		  "VariableStepDiscrete"
+	  SolverName		  "VariableStepDiscrete"
+	  ShapePreserveControl	  "DisableAll"
+	  ZeroCrossControl	  "UseLocalSettings"
+	  ZeroCrossAlgorithm	  "Nonadaptive"
+	  AlgebraicLoopSolver	  "TrustRegion"
+	  SolverResetMethod	  "Fast"
+	  PositivePriorityOrder	  off
+	  AutoInsertRateTranBlk	  off
+	  SampleTimeConstraint	  "Unconstrained"
+	  InsertRTBMode		  "Whenever possible"
+	}
+	Simulink.DataIOCC {
+	  $ObjectID		  3
+	  Version		  "1.6.0"
+	  Decimation		  "1"
+	  ExternalInput		  "[t, u]"
+	  FinalStateName	  "xFinal"
+	  InitialState		  "xInitial"
+	  LimitDataPoints	  on
+	  MaxDataPoints		  "1000"
+	  LoadExternalInput	  off
+	  LoadInitialState	  off
+	  SaveFinalState	  off
+	  SaveCompleteFinalSimState off
+	  SaveFormat		  "Array"
+	  SaveOutput		  on
+	  SaveState		  off
+	  SignalLogging		  on
+	  InspectSignalLogs	  off
+	  SaveTime		  on
+	  ReturnWorkspaceOutputs  off
+	  StateSaveName		  "xout"
+	  TimeSaveName		  "tout"
+	  OutputSaveName	  "yout"
+	  SignalLoggingName	  "logsout"
+	  OutputOption		  "RefineOutputTimes"
+	  OutputTimes		  "[]"
+	  ReturnWorkspaceOutputsName "out"
+	  Refine		  "1"
+	}
+	Simulink.OptimizationCC {
+	  $ObjectID		  4
+	  Version		  "1.6.0"
+	  Array {
+	    Type		    "Cell"
+	    Dimension		    4
+	    Cell		    "ZeroExternalMemoryAtStartup"
+	    Cell		    "ZeroInternalMemoryAtStartup"
+	    Cell		    "NoFixptDivByZeroProtection"
+	    Cell		    "OptimizeModelRefInitCode"
+	    PropName		    "DisabledProps"
+	  }
+	  BlockReduction	  on
+	  BooleanDataType	  on
+	  ConditionallyExecuteInputs on
+	  InlineParams		  off
+	  UseIntDivNetSlope	  off
+	  InlineInvariantSignals  off
+	  OptimizeBlockIOStorage  on
+	  BufferReuse		  on
+	  EnhancedBackFolding	  off
+	  StrengthReduction	  off
+	  EnforceIntegerDowncast  on
+	  ExpressionFolding	  on
+	  BooleansAsBitfields	  off
+	  EnableMemcpy		  on
+	  MemcpyThreshold	  64
+	  PassReuseOutputArgsAs	  "Structure reference"
+	  ExpressionDepthLimit	  2147483647
+	  FoldNonRolledExpr	  on
+	  LocalBlockOutputs	  on
+	  RollThreshold		  5
+	  SystemCodeInlineAuto	  off
+	  StateBitsets		  off
+	  DataBitsets		  off
+	  UseTempVars		  off
+	  ZeroExternalMemoryAtStartup on
+	  ZeroInternalMemoryAtStartup on
+	  InitFltsAndDblsToZero	  on
+	  NoFixptDivByZeroProtection off
+	  EfficientFloat2IntCast  off
+	  EfficientMapNaN2IntZero on
+	  OptimizeModelRefInitCode off
+	  LifeSpan		  "inf"
+	  BufferReusableBoundary  on
+	  SimCompilerOptimization "Off"
+	  AccelVerboseBuild	  off
+	}
+	Simulink.DebuggingCC {
+	  $ObjectID		  5
+	  Version		  "1.6.0"
+	  RTPrefix		  "error"
+	  ConsistencyChecking	  "none"
+	  ArrayBoundsChecking	  "none"
+	  SignalInfNanChecking	  "none"
+	  SignalRangeChecking	  "none"
+	  ReadBeforeWriteMsg	  "UseLocalSettings"
+	  WriteAfterWriteMsg	  "UseLocalSettings"
+	  WriteAfterReadMsg	  "UseLocalSettings"
+	  AlgebraicLoopMsg	  "warning"
+	  ArtificialAlgebraicLoopMsg "warning"
+	  SaveWithDisabledLinksMsg "warning"
+	  SaveWithParameterizedLinksMsg	"none"
+	  CheckSSInitialOutputMsg on
+	  UnderspecifiedInitializationDetection	"Classic"
+	  MergeDetectMultiDrivingBlocksExec "none"
+	  CheckExecutionContextPreStartOutputMsg off
+	  CheckExecutionContextRuntimeOutputMsg	off
+	  SignalResolutionControl "TryResolveAllWithWarning"
+	  BlockPriorityViolationMsg "warning"
+	  MinStepSizeMsg	  "warning"
+	  TimeAdjustmentMsg	  "none"
+	  MaxConsecutiveZCsMsg	  "error"
+	  SolverPrmCheckMsg	  "warning"
+	  InheritedTsInSrcMsg	  "warning"
+	  DiscreteInheritContinuousMsg "warning"
+	  MultiTaskDSMMsg	  "error"
+	  MultiTaskCondExecSysMsg "error"
+	  MultiTaskRateTransMsg	  "error"
+	  SingleTaskRateTransMsg  "none"
+	  TasksWithSamePriorityMsg "warning"
+	  SigSpecEnsureSampleTimeMsg "warning"
+	  CheckMatrixSingularityMsg "none"
+	  IntegerOverflowMsg	  "warning"
+	  Int32ToFloatConvMsg	  "warning"
+	  ParameterDowncastMsg	  "error"
+	  ParameterOverflowMsg	  "error"
+	  ParameterUnderflowMsg	  "none"
+	  ParameterPrecisionLossMsg "warning"
+	  ParameterTunabilityLossMsg "warning"
+	  FixptConstUnderflowMsg  "none"
+	  FixptConstOverflowMsg	  "none"
+	  FixptConstPrecisionLossMsg "none"
+	  UnderSpecifiedDataTypeMsg "none"
+	  UnnecessaryDatatypeConvMsg "none"
+	  VectorMatrixConversionMsg "none"
+	  InvalidFcnCallConnMsg	  "error"
+	  FcnCallInpInsideContextMsg "Use local settings"
+	  SignalLabelMismatchMsg  "none"
+	  UnconnectedInputMsg	  "warning"
+	  UnconnectedOutputMsg	  "warning"
+	  UnconnectedLineMsg	  "warning"
+	  SFcnCompatibilityMsg	  "none"
+	  UniqueDataStoreMsg	  "none"
+	  BusObjectLabelMismatch  "warning"
+	  RootOutportRequireBusObject "warning"
+	  AssertControl		  "UseLocalSettings"
+	  EnableOverflowDetection off
+	  ModelReferenceIOMsg	  "none"
+	  ModelReferenceVersionMismatchMessage "none"
+	  ModelReferenceIOMismatchMessage "none"
+	  ModelReferenceCSMismatchMessage "none"
+	  UnknownTsInhSupMsg	  "warning"
+	  ModelReferenceDataLoggingMessage "warning"
+	  ModelReferenceSymbolNameMessage "warning"
+	  ModelReferenceExtraNoncontSigs "error"
+	  StateNameClashWarn	  "warning"
+	  SimStateInterfaceChecksumMismatchMsg "warning"
+	  StrictBusMsg		  "Warning"
+	  LoggingUnavailableSignals "error"
+	  BlockIODiagnostic	  "none"
+	}
+	Simulink.HardwareCC {
+	  $ObjectID		  6
+	  Version		  "1.6.0"
+	  ProdBitPerChar	  8
+	  ProdBitPerShort	  16
+	  ProdBitPerInt		  32
+	  ProdBitPerLong	  32
+	  ProdIntDivRoundTo	  "Undefined"
+	  ProdEndianess		  "Unspecified"
+	  ProdWordSize		  32
+	  ProdShiftRightIntArith  on
+	  ProdHWDeviceType	  "32-bit Generic"
+	  TargetBitPerChar	  8
+	  TargetBitPerShort	  16
+	  TargetBitPerInt	  32
+	  TargetBitPerLong	  32
+	  TargetShiftRightIntArith on
+	  TargetIntDivRoundTo	  "Undefined"
+	  TargetEndianess	  "Unspecified"
+	  TargetWordSize	  32
+	  TargetTypeEmulationWarnSuppressLevel 0
+	  TargetPreprocMaxBitsSint 32
+	  TargetPreprocMaxBitsUint 32
+	  TargetHWDeviceType	  "Specified"
+	  TargetUnknown		  off
+	  ProdEqTarget		  on
+	}
+	Simulink.ModelReferenceCC {
+	  $ObjectID		  7
+	  Version		  "1.6.0"
+	  UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange"
+	  CheckModelReferenceTargetMessage "error"
+	  ModelReferenceNumInstancesAllowed "Multi"
+	  ModelReferencePassRootInputsByReference on
+	  ModelReferenceMinAlgLoopOccurrences off
+	}
+	Simulink.SFSimCC {
+	  $ObjectID		  8
+	  Version		  "1.6.0"
+	  SFSimEnableDebug	  on
+	  SFSimOverflowDetection  on
+	  SFSimEcho		  on
+	  SimBlas		  on
+	  SimCtrlC		  on
+	  SimExtrinsic		  on
+	  SimIntegrity		  on
+	  SimUseLocalCustomCode	  off
+	  SimBuildMode		  "sf_incremental_build"
+	}
+	Simulink.RTWCC {
+	  $BackupClass		  "Simulink.RTWCC"
+	  $ObjectID		  9
+	  Version		  "1.6.0"
+	  Array {
+	    Type		    "Cell"
+	    Dimension		    1
+	    Cell		    "IncludeHyperlinkInReport"
+	    PropName		    "DisabledProps"
+	  }
+	  SystemTargetFile	  "grt.tlc"
+	  GenCodeOnly		  off
+	  MakeCommand		  "make_rtw"
+	  GenerateMakefile	  on
+	  TemplateMakefile	  "grt_default_tmf"
+	  GenerateReport	  off
+	  SaveLog		  off
+	  RTWVerbose		  on
+	  RetainRTWFile		  off
+	  ProfileTLC		  off
+	  TLCDebug		  off
+	  TLCCoverage		  off
+	  TLCAssert		  off
+	  ProcessScriptMode	  "Default"
+	  ConfigurationMode	  "Optimized"
+	  ConfigAtBuild		  off
+	  RTWUseLocalCustomCode	  off
+	  RTWUseSimCustomCode	  off
+	  IncludeHyperlinkInReport off
+	  LaunchReport		  off
+	  TargetLang		  "C"
+	  IncludeBusHierarchyInRTWFileBlockHierarchyMap	off
+	  IncludeERTFirstTime	  off
+	  GenerateTraceInfo	  off
+	  GenerateTraceReport	  off
+	  GenerateTraceReportSl	  off
+	  GenerateTraceReportSf	  off
+	  GenerateTraceReportEml  off
+	  GenerateCodeInfo	  off
+	  RTWCompilerOptimization "Off"
+	  CheckMdlBeforeBuild	  "Off"
+	  Array {
+	    Type		    "Handle"
+	    Dimension		    2
+	    Simulink.CodeAppCC {
+	      $ObjectID		      10
+	      Version		      "1.6.0"
+	      Array {
+		Type			"Cell"
+		Dimension		16
+		Cell			"IgnoreCustomStorageClasses"
+		Cell			"InsertBlockDesc"
+		Cell			"SFDataObjDesc"
+		Cell			"SimulinkDataObjDesc"
+		Cell			"DefineNamingRule"
+		Cell			"SignalNamingRule"
+		Cell			"ParamNamingRule"
+		Cell			"InlinedPrmAccess"
+		Cell			"CustomSymbolStr"
+		Cell			"CustomSymbolStrGlobalVar"
+		Cell			"CustomSymbolStrType"
+		Cell			"CustomSymbolStrField"
+		Cell			"CustomSymbolStrFcn"
+		Cell			"CustomSymbolStrBlkIO"
+		Cell			"CustomSymbolStrTmpVar"
+		Cell			"CustomSymbolStrMacro"
+		PropName		"DisabledProps"
+	      }
+	      ForceParamTrailComments off
+	      GenerateComments	      on
+	      IgnoreCustomStorageClasses on
+	      IgnoreTestpoints	      off
+	      IncHierarchyInIds	      off
+	      MaxIdLength	      31
+	      PreserveName	      off
+	      PreserveNameWithParent  off
+	      ShowEliminatedStatement off
+	      IncAutoGenComments      off
+	      SimulinkDataObjDesc     off
+	      SFDataObjDesc	      off
+	      IncDataTypeInIds	      off
+	      MangleLength	      1
+	      CustomSymbolStrGlobalVar "$R$N$M"
+	      CustomSymbolStrType     "$N$R$M"
+	      CustomSymbolStrField    "$N$M"
+	      CustomSymbolStrFcn      "$R$N$M$F"
+	      CustomSymbolStrFcnArg   "rt$I$N$M"
+	      CustomSymbolStrBlkIO    "rtb_$N$M"
+	      CustomSymbolStrTmpVar   "$N$M"
+	      CustomSymbolStrMacro    "$R$N$M"
+	      DefineNamingRule	      "None"
+	      ParamNamingRule	      "None"
+	      SignalNamingRule	      "None"
+	      InsertBlockDesc	      off
+	      SimulinkBlockComments   on
+	      EnableCustomComments    off
+	      InlinedPrmAccess	      "Literals"
+	      ReqsInCode	      off
+	      UseSimReservedNames     off
+	    }
+	    Simulink.GRTTargetCC {
+	      $BackupClass	      "Simulink.TargetCC"
+	      $ObjectID		      11
+	      Version		      "1.6.0"
+	      Array {
+		Type			"Cell"
+		Dimension		13
+		Cell			"IncludeMdlTerminateFcn"
+		Cell			"CombineOutputUpdateFcns"
+		Cell			"SuppressErrorStatus"
+		Cell			"ERTCustomFileBanners"
+		Cell			"GenerateSampleERTMain"
+		Cell			"GenerateTestInterfaces"
+		Cell			"MultiInstanceERTCode"
+		Cell			"PurelyIntegerCode"
+		Cell			"SupportNonFinite"
+		Cell			"SupportComplex"
+		Cell			"SupportAbsoluteTime"
+		Cell			"SupportContinuousTime"
+		Cell			"SupportNonInlinedSFcns"
+		PropName		"DisabledProps"
+	      }
+	      TargetFcnLib	      "ansi_tfl_tmw.mat"
+	      TargetLibSuffix	      ""
+	      TargetPreCompLibLocation ""
+	      TargetFunctionLibrary   "ANSI_C"
+	      UtilityFuncGeneration   "Auto"
+	      ERTMultiwordTypeDef     "System defined"
+	      ERTMultiwordLength      256
+	      MultiwordLength	      2048
+	      GenerateFullHeader      on
+	      GenerateSampleERTMain   off
+	      GenerateTestInterfaces  off
+	      IsPILTarget	      off
+	      ModelReferenceCompliant on
+	      ParMdlRefBuildCompliant on
+	      CompOptLevelCompliant   on
+	      IncludeMdlTerminateFcn  on
+	      GeneratePreprocessorConditionals "Disable all"
+	      CombineOutputUpdateFcns off
+	      SuppressErrorStatus     off
+	      ERTFirstTimeCompliant   off
+	      IncludeFileDelimiter    "Auto"
+	      ERTCustomFileBanners    off
+	      SupportAbsoluteTime     on
+	      LogVarNameModifier      "rt_"
+	      MatFileLogging	      on
+	      MultiInstanceERTCode    off
+	      SupportNonFinite	      on
+	      SupportComplex	      on
+	      PurelyIntegerCode	      off
+	      SupportContinuousTime   on
+	      SupportNonInlinedSFcns  on
+	      SupportVariableSizeSignals off
+	      EnableShiftOperators    on
+	      ParenthesesLevel	      "Nominal"
+	      PortableWordSizes	      off
+	      ModelStepFunctionPrototypeControlCompliant off
+	      CPPClassGenCompliant    off
+	      AutosarCompliant	      off
+	      UseMalloc		      off
+	      ExtMode		      off
+	      ExtModeStaticAlloc      off
+	      ExtModeTesting	      off
+	      ExtModeStaticAllocSize  1000000
+	      ExtModeTransport	      0
+	      ExtModeMexFile	      "ext_comm"
+	      ExtModeIntrfLevel	      "Level1"
+	      RTWCAPISignals	      off
+	      RTWCAPIParams	      off
+	      RTWCAPIStates	      off
+	      GenerateASAP2	      off
+	    }
+	    PropName		    "Components"
+	  }
+	}
+	PropName		"Components"
+      }
+      Name		      "Configuration"
+      CurrentDlgPage	      "Solver"
+      ConfigPrmDlgPosition    " [ 200, 197, 1080, 827 ] "
+    }
+    PropName		    "ConfigurationSets"
+  }
+  Simulink.ConfigSet {
+    $PropName		    "ActiveConfigurationSet"
+    $ObjectID		    1
+  }
+  BlockDefaults {
+    ForegroundColor	    "black"
+    BackgroundColor	    "white"
+    DropShadow		    off
+    NamePlacement	    "normal"
+    FontName		    "Courier"
+    FontSize		    10
+    FontWeight		    "normal"
+    FontAngle		    "normal"
+    ShowName		    on
+    BlockRotation	    0
+    BlockMirror		    off
+  }
+  AnnotationDefaults {
+    HorizontalAlignment	    "center"
+    VerticalAlignment	    "middle"
+    ForegroundColor	    "black"
+    BackgroundColor	    "white"
+    DropShadow		    off
+    FontName		    "Courier"
+    FontSize		    10
+    FontWeight		    "normal"
+    FontAngle		    "normal"
+    UseDisplayTextAsClickCallback off
+  }
+  LineDefaults {
+    FontName		    "Courier"
+    FontSize		    9
+    FontWeight		    "normal"
+    FontAngle		    "normal"
+  }
+  BlockParameterDefaults {
+    Block {
+      BlockType		      FromWorkspace
+      VariableName	      "simulink_input"
+      SampleTime	      "-1"
+      Interpolate	      on
+      ZeroCross		      off
+      OutputAfterFinalValue   "Extrapolation"
+    }
+    Block {
+      BlockType		      "S-Function"
+      FunctionName	      "system"
+      SFunctionModules	      "''"
+      PortCounts	      "[]"
+      SFunctionDeploymentMode off
+    }
+    Block {
+      BlockType		      Scope
+      ModelBased	      off
+      TickLabels	      "OneTimeTick"
+      ZoomMode		      "on"
+      Grid		      "on"
+      TimeRange		      "auto"
+      YMin		      "-5"
+      YMax		      "5"
+      SaveToWorkspace	      off
+      SaveName		      "ScopeData"
+      LimitDataPoints	      on
+      MaxDataPoints	      "5000"
+      Decimation	      "1"
+      SampleInput	      off
+      SampleTime	      "-1"
+    }
+  }
+  System {
+    Name		    "example_QProblemB"
+    Location		    [214, 137, 1484, 1054]
+    Open		    on
+    ModelBrowserVisibility  off
+    ModelBrowserWidth	    200
+    ScreenColor		    "white"
+    PaperOrientation	    "landscape"
+    PaperPositionMode	    "auto"
+    PaperType		    "usletter"
+    PaperUnits		    "inches"
+    TiledPaperMargins	    [0.500000, 0.500000, 0.500000, 0.500000]
+    TiledPageScale	    1
+    ShowPageBoundaries	    off
+    ZoomFactor		    "125"
+    ReportName		    "simulink-default.rpt"
+    SIDHighWatermark	    6
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace1"
+      SID		      1
+      Position		      [85, 93, 150, 117]
+      VariableName	      "H"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace2"
+      SID		      2
+      Position		      [85, 158, 150, 182]
+      VariableName	      "g"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace3"
+      SID		      3
+      Position		      [85, 218, 150, 242]
+      VariableName	      "lb"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace4"
+      SID		      4
+      Position		      [85, 283, 150, 307]
+      VariableName	      "ub"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      Scope
+      Name		      "fval,\nx,\nexitflag,\niter"
+      SID		      5
+      Ports		      [4]
+      Position		      [840, 184, 870, 216]
+      Floating		      off
+      Location		      [6, 78, 1276, 993]
+      Open		      off
+      NumInputPorts	      "4"
+      List {
+	ListType		AxesTitles
+	axes1			"%<SignalLabel>"
+	axes2			"%<SignalLabel>"
+	axes3			"%<SignalLabel>"
+	axes4			"%<SignalLabel>"
+      }
+      YMin		      "-5~-5~-5~-5"
+      YMax		      "5~5~5~5"
+      SaveName		      "ScopeData1"
+      DataFormat	      "StructureWithTime"
+      SampleTime	      "0"
+    }
+    Block {
+      BlockType		      "S-Function"
+      Name		      "qpOASES"
+      SID		      6
+      Ports		      [4, 4]
+      Position		      [510, 181, 705, 219]
+      BackgroundColor	      "yellow"
+      FunctionName	      "qpOASES_QProblemB"
+      EnableBusSupport	      off
+    }
+    Line {
+      SrcBlock		      "From\nWorkspace1"
+      SrcPort		      1
+      Points		      [130, 0; 0, 80]
+      DstBlock		      "qpOASES"
+      DstPort		      1
+    }
+    Line {
+      SrcBlock		      "From\nWorkspace2"
+      SrcPort		      1
+      Points		      [80, 0; 0, 26; 260, 0]
+      DstBlock		      "qpOASES"
+      DstPort		      2
+    }
+    Line {
+      SrcBlock		      "qpOASES"
+      SrcPort		      1
+      DstBlock		      "fval,\nx,\nexitflag,\niter"
+      DstPort		      1
+    }
+    Line {
+      SrcBlock		      "qpOASES"
+      SrcPort		      2
+      DstBlock		      "fval,\nx,\nexitflag,\niter"
+      DstPort		      2
+    }
+    Line {
+      SrcBlock		      "qpOASES"
+      SrcPort		      3
+      DstBlock		      "fval,\nx,\nexitflag,\niter"
+      DstPort		      3
+    }
+    Line {
+      SrcBlock		      "qpOASES"
+      SrcPort		      4
+      DstBlock		      "fval,\nx,\nexitflag,\niter"
+      DstPort		      4
+    }
+    Line {
+      SrcBlock		      "From\nWorkspace3"
+      SrcPort		      1
+      Points		      [80, 0; 0, -25]
+      DstBlock		      "qpOASES"
+      DstPort		      3
+    }
+    Line {
+      SrcBlock		      "From\nWorkspace4"
+      SrcPort		      1
+      Points		      [130, 0; 0, -79; 210, 0]
+      DstBlock		      "qpOASES"
+      DstPort		      4
+    }
+    Annotation {
+      Position		      [335, 96]
+    }
+    Annotation {
+      Name		      "This file is part of qpOASES.\n\nqpOASES -- An Implementation of the Online Active Set Strategy.\nC"
+      "opyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka, Christian Kirches et al.\nAll rights reserved."
+      Position		      [608, 108]
+    }
+  }
+}
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_SQProblem.mdl b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_SQProblem.mdl
new file mode 100644
index 00000000000..106f19a56ed
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_SQProblem.mdl
@@ -0,0 +1,797 @@
+Model {
+  Name			  "example_SQProblem"
+  Version		  7.4
+  MdlSubVersion		  0
+  GraphicalInterface {
+    NumRootInports	    0
+    NumRootOutports	    0
+    ParameterArgumentNames  ""
+    ComputedModelVersion    "1.62"
+    NumModelReferences	    0
+    NumTestPointedSignals   0
+  }
+  SavedCharacterEncoding  "UTF-8"
+  SaveDefaultBlockParams  on
+  ScopeRefreshTime	  0.035000
+  OverrideScopeRefreshTime on
+  DisableAllScopes	  off
+  DataTypeOverride	  "UseLocalSettings"
+  MinMaxOverflowLogging	  "UseLocalSettings"
+  MinMaxOverflowArchiveMode "Overwrite"
+  MaxMDLFileLineLength	  120
+  Created		  "Fri Apr 13 11:08:51 2007"
+  Creator		  "jferreau"
+  UpdateHistory		  "UpdateHistoryNever"
+  ModifiedByFormat	  "%<Auto>"
+  LastModifiedBy	  "chjofer2"
+  ModifiedDateFormat	  "%<Auto>"
+  LastModifiedDate	  "Thu Feb 20 15:12:07 2014"
+  RTWModifiedTimeStamp	  314809925
+  ModelVersionFormat	  "1.%<AutoIncrement:62>"
+  ConfigurationManager	  "None"
+  SampleTimeColors	  off
+  SampleTimeAnnotations	  off
+  LibraryLinkDisplay	  "none"
+  WideLines		  off
+  ShowLineDimensions	  off
+  ShowPortDataTypes	  off
+  ShowLoopsOnError	  on
+  IgnoreBidirectionalLines off
+  ShowStorageClass	  off
+  ShowTestPointIcons	  on
+  ShowSignalResolutionIcons on
+  ShowViewerIcons	  on
+  SortedOrder		  off
+  ExecutionContextIcon	  off
+  ShowLinearizationAnnotations on
+  BlockNameDataTip	  off
+  BlockParametersDataTip  off
+  BlockDescriptionStringDataTip	off
+  ToolBar		  on
+  StatusBar		  on
+  BrowserShowLibraryLinks off
+  BrowserLookUnderMasks	  off
+  SimulationMode	  "normal"
+  LinearizationMsg	  "none"
+  Profile		  off
+  ParamWorkspaceSource	  "MATLABWorkspace"
+  AccelSystemTargetFile	  "accel.tlc"
+  AccelTemplateMakefile	  "accel_default_tmf"
+  AccelMakeCommand	  "make_rtw"
+  TryForcingSFcnDF	  off
+  RecordCoverage	  off
+  CovPath		  "/"
+  CovSaveName		  "covdata"
+  CovMetricSettings	  "dw"
+  CovNameIncrementing	  off
+  CovHtmlReporting	  on
+  CovForceBlockReductionOff on
+  covSaveCumulativeToWorkspaceVar on
+  CovSaveSingleToWorkspaceVar on
+  CovCumulativeVarName	  "covCumulativeData"
+  CovCumulativeReport	  off
+  CovReportOnPause	  on
+  CovModelRefEnable	  "Off"
+  CovExternalEMLEnable	  off
+  ExtModeBatchMode	  off
+  ExtModeEnableFloating	  on
+  ExtModeTrigType	  "manual"
+  ExtModeTrigMode	  "normal"
+  ExtModeTrigPort	  "1"
+  ExtModeTrigElement	  "any"
+  ExtModeTrigDuration	  1000
+  ExtModeTrigDurationFloating "auto"
+  ExtModeTrigHoldOff	  0
+  ExtModeTrigDelay	  0
+  ExtModeTrigDirection	  "rising"
+  ExtModeTrigLevel	  0
+  ExtModeArchiveMode	  "off"
+  ExtModeAutoIncOneShot	  off
+  ExtModeIncDirWhenArm	  off
+  ExtModeAddSuffixToVar	  off
+  ExtModeWriteAllDataToWs off
+  ExtModeArmWhenConnect	  on
+  ExtModeSkipDownloadWhenConnect off
+  ExtModeLogAll		  on
+  ExtModeAutoUpdateStatusClock off
+  BufferReuse		  on
+  ShowModelReferenceBlockVersion off
+  ShowModelReferenceBlockIO off
+  Array {
+    Type		    "Handle"
+    Dimension		    1
+    Simulink.ConfigSet {
+      $ObjectID		      1
+      Version		      "1.6.0"
+      Array {
+	Type			"Handle"
+	Dimension		8
+	Simulink.SolverCC {
+	  $ObjectID		  2
+	  Version		  "1.6.0"
+	  StartTime		  "0.0"
+	  StopTime		  "0.5"
+	  AbsTol		  "auto"
+	  FixedStep		  "auto"
+	  InitialStep		  "auto"
+	  MaxNumMinSteps	  "-1"
+	  MaxOrder		  5
+	  ZcThreshold		  "auto"
+	  ConsecutiveZCsStepRelTol "10*128*eps"
+	  MaxConsecutiveZCs	  "1000"
+	  ExtrapolationOrder	  4
+	  NumberNewtonIterations  1
+	  MaxStep		  "auto"
+	  MinStep		  "auto"
+	  MaxConsecutiveMinStep	  "1"
+	  RelTol		  "1e-3"
+	  SolverMode		  "Auto"
+	  Solver		  "VariableStepDiscrete"
+	  SolverName		  "VariableStepDiscrete"
+	  ShapePreserveControl	  "DisableAll"
+	  ZeroCrossControl	  "UseLocalSettings"
+	  ZeroCrossAlgorithm	  "Nonadaptive"
+	  AlgebraicLoopSolver	  "TrustRegion"
+	  SolverResetMethod	  "Fast"
+	  PositivePriorityOrder	  off
+	  AutoInsertRateTranBlk	  off
+	  SampleTimeConstraint	  "Unconstrained"
+	  InsertRTBMode		  "Whenever possible"
+	}
+	Simulink.DataIOCC {
+	  $ObjectID		  3
+	  Version		  "1.6.0"
+	  Decimation		  "1"
+	  ExternalInput		  "[t, u]"
+	  FinalStateName	  "xFinal"
+	  InitialState		  "xInitial"
+	  LimitDataPoints	  on
+	  MaxDataPoints		  "1000"
+	  LoadExternalInput	  off
+	  LoadInitialState	  off
+	  SaveFinalState	  off
+	  SaveCompleteFinalSimState off
+	  SaveFormat		  "Array"
+	  SaveOutput		  on
+	  SaveState		  off
+	  SignalLogging		  on
+	  InspectSignalLogs	  off
+	  SaveTime		  on
+	  ReturnWorkspaceOutputs  off
+	  StateSaveName		  "xout"
+	  TimeSaveName		  "tout"
+	  OutputSaveName	  "yout"
+	  SignalLoggingName	  "logsout"
+	  OutputOption		  "RefineOutputTimes"
+	  OutputTimes		  "[]"
+	  ReturnWorkspaceOutputsName "out"
+	  Refine		  "1"
+	}
+	Simulink.OptimizationCC {
+	  $ObjectID		  4
+	  Version		  "1.6.0"
+	  Array {
+	    Type		    "Cell"
+	    Dimension		    4
+	    Cell		    "ZeroExternalMemoryAtStartup"
+	    Cell		    "ZeroInternalMemoryAtStartup"
+	    Cell		    "NoFixptDivByZeroProtection"
+	    Cell		    "OptimizeModelRefInitCode"
+	    PropName		    "DisabledProps"
+	  }
+	  BlockReduction	  on
+	  BooleanDataType	  on
+	  ConditionallyExecuteInputs on
+	  InlineParams		  off
+	  UseIntDivNetSlope	  off
+	  InlineInvariantSignals  off
+	  OptimizeBlockIOStorage  on
+	  BufferReuse		  on
+	  EnhancedBackFolding	  off
+	  StrengthReduction	  off
+	  EnforceIntegerDowncast  on
+	  ExpressionFolding	  on
+	  BooleansAsBitfields	  off
+	  EnableMemcpy		  on
+	  MemcpyThreshold	  64
+	  PassReuseOutputArgsAs	  "Structure reference"
+	  ExpressionDepthLimit	  2147483647
+	  FoldNonRolledExpr	  on
+	  LocalBlockOutputs	  on
+	  RollThreshold		  5
+	  SystemCodeInlineAuto	  off
+	  StateBitsets		  off
+	  DataBitsets		  off
+	  UseTempVars		  off
+	  ZeroExternalMemoryAtStartup on
+	  ZeroInternalMemoryAtStartup on
+	  InitFltsAndDblsToZero	  on
+	  NoFixptDivByZeroProtection off
+	  EfficientFloat2IntCast  off
+	  EfficientMapNaN2IntZero on
+	  OptimizeModelRefInitCode off
+	  LifeSpan		  "inf"
+	  BufferReusableBoundary  on
+	  SimCompilerOptimization "Off"
+	  AccelVerboseBuild	  off
+	}
+	Simulink.DebuggingCC {
+	  $ObjectID		  5
+	  Version		  "1.6.0"
+	  RTPrefix		  "error"
+	  ConsistencyChecking	  "none"
+	  ArrayBoundsChecking	  "none"
+	  SignalInfNanChecking	  "none"
+	  SignalRangeChecking	  "none"
+	  ReadBeforeWriteMsg	  "UseLocalSettings"
+	  WriteAfterWriteMsg	  "UseLocalSettings"
+	  WriteAfterReadMsg	  "UseLocalSettings"
+	  AlgebraicLoopMsg	  "warning"
+	  ArtificialAlgebraicLoopMsg "warning"
+	  SaveWithDisabledLinksMsg "warning"
+	  SaveWithParameterizedLinksMsg	"none"
+	  CheckSSInitialOutputMsg on
+	  UnderspecifiedInitializationDetection	"Classic"
+	  MergeDetectMultiDrivingBlocksExec "none"
+	  CheckExecutionContextPreStartOutputMsg off
+	  CheckExecutionContextRuntimeOutputMsg	off
+	  SignalResolutionControl "TryResolveAllWithWarning"
+	  BlockPriorityViolationMsg "warning"
+	  MinStepSizeMsg	  "warning"
+	  TimeAdjustmentMsg	  "none"
+	  MaxConsecutiveZCsMsg	  "error"
+	  SolverPrmCheckMsg	  "warning"
+	  InheritedTsInSrcMsg	  "warning"
+	  DiscreteInheritContinuousMsg "warning"
+	  MultiTaskDSMMsg	  "error"
+	  MultiTaskCondExecSysMsg "error"
+	  MultiTaskRateTransMsg	  "error"
+	  SingleTaskRateTransMsg  "none"
+	  TasksWithSamePriorityMsg "warning"
+	  SigSpecEnsureSampleTimeMsg "warning"
+	  CheckMatrixSingularityMsg "none"
+	  IntegerOverflowMsg	  "warning"
+	  Int32ToFloatConvMsg	  "warning"
+	  ParameterDowncastMsg	  "error"
+	  ParameterOverflowMsg	  "error"
+	  ParameterUnderflowMsg	  "none"
+	  ParameterPrecisionLossMsg "warning"
+	  ParameterTunabilityLossMsg "warning"
+	  FixptConstUnderflowMsg  "none"
+	  FixptConstOverflowMsg	  "none"
+	  FixptConstPrecisionLossMsg "none"
+	  UnderSpecifiedDataTypeMsg "none"
+	  UnnecessaryDatatypeConvMsg "none"
+	  VectorMatrixConversionMsg "none"
+	  InvalidFcnCallConnMsg	  "error"
+	  FcnCallInpInsideContextMsg "Use local settings"
+	  SignalLabelMismatchMsg  "none"
+	  UnconnectedInputMsg	  "warning"
+	  UnconnectedOutputMsg	  "warning"
+	  UnconnectedLineMsg	  "warning"
+	  SFcnCompatibilityMsg	  "none"
+	  UniqueDataStoreMsg	  "none"
+	  BusObjectLabelMismatch  "warning"
+	  RootOutportRequireBusObject "warning"
+	  AssertControl		  "UseLocalSettings"
+	  EnableOverflowDetection off
+	  ModelReferenceIOMsg	  "none"
+	  ModelReferenceVersionMismatchMessage "none"
+	  ModelReferenceIOMismatchMessage "none"
+	  ModelReferenceCSMismatchMessage "none"
+	  UnknownTsInhSupMsg	  "warning"
+	  ModelReferenceDataLoggingMessage "warning"
+	  ModelReferenceSymbolNameMessage "warning"
+	  ModelReferenceExtraNoncontSigs "error"
+	  StateNameClashWarn	  "warning"
+	  SimStateInterfaceChecksumMismatchMsg "warning"
+	  StrictBusMsg		  "Warning"
+	  LoggingUnavailableSignals "error"
+	  BlockIODiagnostic	  "none"
+	}
+	Simulink.HardwareCC {
+	  $ObjectID		  6
+	  Version		  "1.6.0"
+	  ProdBitPerChar	  8
+	  ProdBitPerShort	  16
+	  ProdBitPerInt		  32
+	  ProdBitPerLong	  32
+	  ProdIntDivRoundTo	  "Undefined"
+	  ProdEndianess		  "Unspecified"
+	  ProdWordSize		  32
+	  ProdShiftRightIntArith  on
+	  ProdHWDeviceType	  "32-bit Generic"
+	  TargetBitPerChar	  8
+	  TargetBitPerShort	  16
+	  TargetBitPerInt	  32
+	  TargetBitPerLong	  32
+	  TargetShiftRightIntArith on
+	  TargetIntDivRoundTo	  "Undefined"
+	  TargetEndianess	  "Unspecified"
+	  TargetWordSize	  32
+	  TargetTypeEmulationWarnSuppressLevel 0
+	  TargetPreprocMaxBitsSint 32
+	  TargetPreprocMaxBitsUint 32
+	  TargetHWDeviceType	  "Specified"
+	  TargetUnknown		  off
+	  ProdEqTarget		  on
+	}
+	Simulink.ModelReferenceCC {
+	  $ObjectID		  7
+	  Version		  "1.6.0"
+	  UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange"
+	  CheckModelReferenceTargetMessage "error"
+	  ModelReferenceNumInstancesAllowed "Multi"
+	  ModelReferencePassRootInputsByReference on
+	  ModelReferenceMinAlgLoopOccurrences off
+	}
+	Simulink.SFSimCC {
+	  $ObjectID		  8
+	  Version		  "1.6.0"
+	  SFSimEnableDebug	  on
+	  SFSimOverflowDetection  on
+	  SFSimEcho		  on
+	  SimBlas		  on
+	  SimCtrlC		  on
+	  SimExtrinsic		  on
+	  SimIntegrity		  on
+	  SimUseLocalCustomCode	  off
+	  SimBuildMode		  "sf_incremental_build"
+	}
+	Simulink.RTWCC {
+	  $BackupClass		  "Simulink.RTWCC"
+	  $ObjectID		  9
+	  Version		  "1.6.0"
+	  Array {
+	    Type		    "Cell"
+	    Dimension		    1
+	    Cell		    "IncludeHyperlinkInReport"
+	    PropName		    "DisabledProps"
+	  }
+	  SystemTargetFile	  "grt.tlc"
+	  GenCodeOnly		  off
+	  MakeCommand		  "make_rtw"
+	  GenerateMakefile	  on
+	  TemplateMakefile	  "grt_default_tmf"
+	  GenerateReport	  off
+	  SaveLog		  off
+	  RTWVerbose		  on
+	  RetainRTWFile		  off
+	  ProfileTLC		  off
+	  TLCDebug		  off
+	  TLCCoverage		  off
+	  TLCAssert		  off
+	  ProcessScriptMode	  "Default"
+	  ConfigurationMode	  "Optimized"
+	  ConfigAtBuild		  off
+	  RTWUseLocalCustomCode	  off
+	  RTWUseSimCustomCode	  off
+	  IncludeHyperlinkInReport off
+	  LaunchReport		  off
+	  TargetLang		  "C"
+	  IncludeBusHierarchyInRTWFileBlockHierarchyMap	off
+	  IncludeERTFirstTime	  off
+	  GenerateTraceInfo	  off
+	  GenerateTraceReport	  off
+	  GenerateTraceReportSl	  off
+	  GenerateTraceReportSf	  off
+	  GenerateTraceReportEml  off
+	  GenerateCodeInfo	  off
+	  RTWCompilerOptimization "Off"
+	  CheckMdlBeforeBuild	  "Off"
+	  Array {
+	    Type		    "Handle"
+	    Dimension		    2
+	    Simulink.CodeAppCC {
+	      $ObjectID		      10
+	      Version		      "1.6.0"
+	      Array {
+		Type			"Cell"
+		Dimension		16
+		Cell			"IgnoreCustomStorageClasses"
+		Cell			"InsertBlockDesc"
+		Cell			"SFDataObjDesc"
+		Cell			"SimulinkDataObjDesc"
+		Cell			"DefineNamingRule"
+		Cell			"SignalNamingRule"
+		Cell			"ParamNamingRule"
+		Cell			"InlinedPrmAccess"
+		Cell			"CustomSymbolStr"
+		Cell			"CustomSymbolStrGlobalVar"
+		Cell			"CustomSymbolStrType"
+		Cell			"CustomSymbolStrField"
+		Cell			"CustomSymbolStrFcn"
+		Cell			"CustomSymbolStrBlkIO"
+		Cell			"CustomSymbolStrTmpVar"
+		Cell			"CustomSymbolStrMacro"
+		PropName		"DisabledProps"
+	      }
+	      ForceParamTrailComments off
+	      GenerateComments	      on
+	      IgnoreCustomStorageClasses on
+	      IgnoreTestpoints	      off
+	      IncHierarchyInIds	      off
+	      MaxIdLength	      31
+	      PreserveName	      off
+	      PreserveNameWithParent  off
+	      ShowEliminatedStatement off
+	      IncAutoGenComments      off
+	      SimulinkDataObjDesc     off
+	      SFDataObjDesc	      off
+	      IncDataTypeInIds	      off
+	      MangleLength	      1
+	      CustomSymbolStrGlobalVar "$R$N$M"
+	      CustomSymbolStrType     "$N$R$M"
+	      CustomSymbolStrField    "$N$M"
+	      CustomSymbolStrFcn      "$R$N$M$F"
+	      CustomSymbolStrFcnArg   "rt$I$N$M"
+	      CustomSymbolStrBlkIO    "rtb_$N$M"
+	      CustomSymbolStrTmpVar   "$N$M"
+	      CustomSymbolStrMacro    "$R$N$M"
+	      DefineNamingRule	      "None"
+	      ParamNamingRule	      "None"
+	      SignalNamingRule	      "None"
+	      InsertBlockDesc	      off
+	      SimulinkBlockComments   on
+	      EnableCustomComments    off
+	      InlinedPrmAccess	      "Literals"
+	      ReqsInCode	      off
+	      UseSimReservedNames     off
+	    }
+	    Simulink.GRTTargetCC {
+	      $BackupClass	      "Simulink.TargetCC"
+	      $ObjectID		      11
+	      Version		      "1.6.0"
+	      Array {
+		Type			"Cell"
+		Dimension		13
+		Cell			"IncludeMdlTerminateFcn"
+		Cell			"CombineOutputUpdateFcns"
+		Cell			"SuppressErrorStatus"
+		Cell			"ERTCustomFileBanners"
+		Cell			"GenerateSampleERTMain"
+		Cell			"GenerateTestInterfaces"
+		Cell			"MultiInstanceERTCode"
+		Cell			"PurelyIntegerCode"
+		Cell			"SupportNonFinite"
+		Cell			"SupportComplex"
+		Cell			"SupportAbsoluteTime"
+		Cell			"SupportContinuousTime"
+		Cell			"SupportNonInlinedSFcns"
+		PropName		"DisabledProps"
+	      }
+	      TargetFcnLib	      "ansi_tfl_tmw.mat"
+	      TargetLibSuffix	      ""
+	      TargetPreCompLibLocation ""
+	      TargetFunctionLibrary   "ANSI_C"
+	      UtilityFuncGeneration   "Auto"
+	      ERTMultiwordTypeDef     "System defined"
+	      ERTMultiwordLength      256
+	      MultiwordLength	      2048
+	      GenerateFullHeader      on
+	      GenerateSampleERTMain   off
+	      GenerateTestInterfaces  off
+	      IsPILTarget	      off
+	      ModelReferenceCompliant on
+	      ParMdlRefBuildCompliant on
+	      CompOptLevelCompliant   on
+	      IncludeMdlTerminateFcn  on
+	      GeneratePreprocessorConditionals "Disable all"
+	      CombineOutputUpdateFcns off
+	      SuppressErrorStatus     off
+	      ERTFirstTimeCompliant   off
+	      IncludeFileDelimiter    "Auto"
+	      ERTCustomFileBanners    off
+	      SupportAbsoluteTime     on
+	      LogVarNameModifier      "rt_"
+	      MatFileLogging	      on
+	      MultiInstanceERTCode    off
+	      SupportNonFinite	      on
+	      SupportComplex	      on
+	      PurelyIntegerCode	      off
+	      SupportContinuousTime   on
+	      SupportNonInlinedSFcns  on
+	      SupportVariableSizeSignals off
+	      EnableShiftOperators    on
+	      ParenthesesLevel	      "Nominal"
+	      PortableWordSizes	      off
+	      ModelStepFunctionPrototypeControlCompliant off
+	      CPPClassGenCompliant    off
+	      AutosarCompliant	      off
+	      UseMalloc		      off
+	      ExtMode		      off
+	      ExtModeStaticAlloc      off
+	      ExtModeTesting	      off
+	      ExtModeStaticAllocSize  1000000
+	      ExtModeTransport	      0
+	      ExtModeMexFile	      "ext_comm"
+	      ExtModeIntrfLevel	      "Level1"
+	      RTWCAPISignals	      off
+	      RTWCAPIParams	      off
+	      RTWCAPIStates	      off
+	      GenerateASAP2	      off
+	    }
+	    PropName		    "Components"
+	  }
+	}
+	PropName		"Components"
+      }
+      Name		      "Configuration"
+      CurrentDlgPage	      "Solver"
+      ConfigPrmDlgPosition    " [ 200, 197, 1080, 827 ] "
+    }
+    PropName		    "ConfigurationSets"
+  }
+  Simulink.ConfigSet {
+    $PropName		    "ActiveConfigurationSet"
+    $ObjectID		    1
+  }
+  BlockDefaults {
+    ForegroundColor	    "black"
+    BackgroundColor	    "white"
+    DropShadow		    off
+    NamePlacement	    "normal"
+    FontName		    "Courier"
+    FontSize		    10
+    FontWeight		    "normal"
+    FontAngle		    "normal"
+    ShowName		    on
+    BlockRotation	    0
+    BlockMirror		    off
+  }
+  AnnotationDefaults {
+    HorizontalAlignment	    "center"
+    VerticalAlignment	    "middle"
+    ForegroundColor	    "black"
+    BackgroundColor	    "white"
+    DropShadow		    off
+    FontName		    "Courier"
+    FontSize		    10
+    FontWeight		    "normal"
+    FontAngle		    "normal"
+    UseDisplayTextAsClickCallback off
+  }
+  LineDefaults {
+    FontName		    "Courier"
+    FontSize		    9
+    FontWeight		    "normal"
+    FontAngle		    "normal"
+  }
+  BlockParameterDefaults {
+    Block {
+      BlockType		      FromWorkspace
+      VariableName	      "simulink_input"
+      SampleTime	      "-1"
+      Interpolate	      on
+      ZeroCross		      off
+      OutputAfterFinalValue   "Extrapolation"
+    }
+    Block {
+      BlockType		      "S-Function"
+      FunctionName	      "system"
+      SFunctionModules	      "''"
+      PortCounts	      "[]"
+      SFunctionDeploymentMode off
+    }
+    Block {
+      BlockType		      Scope
+      ModelBased	      off
+      TickLabels	      "OneTimeTick"
+      ZoomMode		      "on"
+      Grid		      "on"
+      TimeRange		      "auto"
+      YMin		      "-5"
+      YMax		      "5"
+      SaveToWorkspace	      off
+      SaveName		      "ScopeData"
+      LimitDataPoints	      on
+      MaxDataPoints	      "5000"
+      Decimation	      "1"
+      SampleInput	      off
+      SampleTime	      "-1"
+    }
+  }
+  System {
+    Name		    "example_SQProblem"
+    Location		    [2, 74, 1918, 1139]
+    Open		    on
+    ModelBrowserVisibility  off
+    ModelBrowserWidth	    200
+    ScreenColor		    "white"
+    PaperOrientation	    "landscape"
+    PaperPositionMode	    "auto"
+    PaperType		    "usletter"
+    PaperUnits		    "inches"
+    TiledPaperMargins	    [0.500000, 0.500000, 0.500000, 0.500000]
+    TiledPageScale	    1
+    ShowPageBoundaries	    off
+    ZoomFactor		    "125"
+    ReportName		    "simulink-default.rpt"
+    SIDHighWatermark	    9
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace1"
+      SID		      1
+      Position		      [85, 53, 150, 77]
+      VariableName	      "H"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace2"
+      SID		      2
+      Position		      [85, 113, 150, 137]
+      VariableName	      "g"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace3"
+      SID		      3
+      Position		      [85, 173, 150, 197]
+      VariableName	      "A"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace4"
+      SID		      4
+      Position		      [85, 233, 150, 257]
+      VariableName	      "lb"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace5"
+      SID		      5
+      Position		      [85, 293, 150, 317]
+      VariableName	      "ub"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace6"
+      SID		      6
+      Position		      [85, 353, 150, 377]
+      VariableName	      "lbA"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      FromWorkspace
+      Name		      "From\nWorkspace7"
+      SID		      7
+      Position		      [85, 418, 150, 442]
+      VariableName	      "ubA"
+      SampleTime	      "0.1"
+      ZeroCross		      on
+      OutputAfterFinalValue   "Holding final value"
+    }
+    Block {
+      BlockType		      Scope
+      Name		      "fval,\nx,\nexitflag,\niter"
+      SID		      8
+      Ports		      [4]
+      Position		      [935, 229, 965, 261]
+      Floating		      off
+      Location		      [6, 78, 1276, 993]
+      Open		      off
+      NumInputPorts	      "4"
+      List {
+	ListType		AxesTitles
+	axes1			"%<SignalLabel>"
+	axes2			"%<SignalLabel>"
+	axes3			"%<SignalLabel>"
+	axes4			"%<SignalLabel>"
+      }
+      YMin		      "-5~-5~-5~-5"
+      YMax		      "5~5~5~5"
+      SaveName		      "ScopeData1"
+      DataFormat	      "StructureWithTime"
+      SampleTime	      "0"
+    }
+    Block {
+      BlockType		      "S-Function"
+      Name		      "qpOASES"
+      SID		      9
+      Ports		      [7, 4]
+      Position		      [600, 226, 795, 264]
+      BackgroundColor	      "yellow"
+      FunctionName	      "qpOASES_SQProblem"
+      EnableBusSupport	      off
+    }
+    Line {
+      SrcBlock		      "From\nWorkspace1"
+      SrcPort		      1
+      Points		      [180, 0; 0, 165]
+      DstBlock		      "qpOASES"
+      DstPort		      1
+    }
+    Line {
+      SrcBlock		      "From\nWorkspace2"
+      SrcPort		      1
+      Points		      [115, 0; 0, 110]
+      DstBlock		      "qpOASES"
+      DstPort		      2
+    }
+    Line {
+      Labels		      [3, 0]
+      SrcBlock		      "From\nWorkspace7"
+      SrcPort		      1
+      Points		      [180, 0; 0, -170]
+      DstBlock		      "qpOASES"
+      DstPort		      7
+    }
+    Line {
+      SrcBlock		      "From\nWorkspace5"
+      SrcPort		      1
+      Points		      [55, 0; 0, -55]
+      DstBlock		      "qpOASES"
+      DstPort		      5
+    }
+    Line {
+      SrcBlock		      "From\nWorkspace4"
+      SrcPort		      1
+      DstBlock		      "qpOASES"
+      DstPort		      4
+    }
+    Line {
+      SrcBlock		      "From\nWorkspace3"
+      SrcPort		      1
+      Points		      [55, 0; 0, 55]
+      DstBlock		      "qpOASES"
+      DstPort		      3
+    }
+    Line {
+      Labels		      [3, 0]
+      SrcBlock		      "From\nWorkspace6"
+      SrcPort		      1
+      Points		      [115, 0; 0, -110]
+      DstBlock		      "qpOASES"
+      DstPort		      6
+    }
+    Line {
+      SrcBlock		      "qpOASES"
+      SrcPort		      1
+      DstBlock		      "fval,\nx,\nexitflag,\niter"
+      DstPort		      1
+    }
+    Line {
+      SrcBlock		      "qpOASES"
+      SrcPort		      2
+      DstBlock		      "fval,\nx,\nexitflag,\niter"
+      DstPort		      2
+    }
+    Line {
+      SrcBlock		      "qpOASES"
+      SrcPort		      3
+      DstBlock		      "fval,\nx,\nexitflag,\niter"
+      DstPort		      3
+    }
+    Line {
+      SrcBlock		      "qpOASES"
+      SrcPort		      4
+      DstBlock		      "fval,\nx,\nexitflag,\niter"
+      DstPort		      4
+    }
+    Annotation {
+      Position		      [335, 96]
+    }
+    Annotation {
+      Name		      "This file is part of qpOASES.\n\nqpOASES -- An Implementation of the Online Active Set Strategy.\nC"
+      "opyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka, Christian Kirches et al.\nAll rights reserved."
+      Position		      [698, 138]
+    }
+  }
+}
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/index.html
new file mode 100644
index 00000000000..e3ce9152edb
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/index.html
@@ -0,0 +1,17 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces/simulink</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/interfaces/simulink</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="example_QProblem.mdl">example_QProblem.mdl</a></li>
+  <li><a href="example_QProblemB.mdl">example_QProblemB.mdl</a></li>
+  <li><a href="example_SQProblem.mdl">example_SQProblem.mdl</a></li>
+  <li><a href="load_example_QProblem.m">load_example_QProblem.m</a></li>
+  <li><a href="load_example_QProblemB.m">load_example_QProblemB.m</a></li>
+  <li><a href="load_example_SQProblem.m">load_example_SQProblem.m</a></li>
+  <li><a href="make.m">make.m</a></li>
+  <li><a href="qpOASES_QProblem.cpp">qpOASES_QProblem.cpp</a></li>
+  <li><a href="qpOASES_QProblemB.cpp">qpOASES_QProblemB.cpp</a></li>
+  <li><a href="qpOASES_SQProblem.cpp">qpOASES_SQProblem.cpp</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_QProblem.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_QProblem.m
new file mode 100644
index 00000000000..af2a81d91c9
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_QProblem.m
@@ -0,0 +1,93 @@
+%%
+%%	This file is part of qpOASES.
+%%
+%%	qpOASES -- An Implementation of the Online Active Set Strategy.
+%%	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%%	Christian Kirches et al. All rights reserved.
+%%
+%%	qpOASES 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.
+%%
+%%	qpOASES 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 qpOASES; if not, write to the Free Software
+%%	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+%%
+
+
+
+%%
+%%	Filename:  interfaces/simulink/load_example_QProblem.m
+%%	Author:    Hans Joachim Ferreau (thanks to Aude Perrin)
+%%	Version:   3.0
+%%	Date:      2007-2014
+%%
+
+
+
+clear all;
+
+
+%% setup QP data
+simulationTime  =  [0;0.1];
+
+H.time = simulationTime;
+data1 = [1.0,0.0,0.0,0.5];
+data2 = [1.0,0.0,0.0,0.5];
+H.signals.values = [data1; data2];
+H.signals.dimensions = length(data1);
+
+g.time = simulationTime;
+data1 = [1.5,1.0];
+data2 = [1.0,1.5];
+g.signals.values = [data1; data2];
+g.signals.dimensions = length(data1);
+
+A.time = simulationTime;
+data1 = [1.0,1.0];
+data2 = [1.0,1.0];
+A.signals.values = [data1; data2];
+A.signals.dimensions = length(data1);
+
+lb.time = simulationTime;
+data1 = [0.5,-2.0];
+data2 = [0.0,-1.0];
+lb.signals.values = [data1; data2];
+lb.signals.dimensions = length(data1);
+
+ub.time = simulationTime;
+data1 = [5.0,2.0];
+data2 = [5.0,-0.5];
+ub.signals.values = [data1; data2];
+ub.signals.dimensions = length(data1);
+
+lbA.time = simulationTime;
+data1 = [-1.0];
+data2 = [-2.0];
+lbA.signals.values = [data1; data2];
+lbA.signals.dimensions = length(data1);
+
+ubA.time = simulationTime;
+data1 = [2.0];
+data2 = [1.0];
+ubA.signals.values = [data1; data2];
+ubA.signals.dimensions = length(data1);
+
+
+clear simulationTime data1 data2
+
+
+%% open corresponding simulink example
+open( 'example_QProblem.mdl' );
+
+
+
+%%
+%%	end of file
+%%
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_QProblemB.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_QProblemB.m
new file mode 100644
index 00000000000..8c3900c9922
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_QProblemB.m
@@ -0,0 +1,75 @@
+%%
+%%	This file is part of qpOASES.
+%%
+%%	qpOASES -- An Implementation of the Online Active Set Strategy.
+%%	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%%	Christian Kirches et al. All rights reserved.
+%%
+%%	qpOASES 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.
+%%
+%%	qpOASES 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 qpOASES; if not, write to the Free Software
+%%	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+%%
+
+
+
+%%
+%%	Filename:  interfaces/simulink/load_example_QProblemB.m
+%%	Author:    Hans Joachim Ferreau (thanks to Aude Perrin)
+%%	Version:   3.0
+%%	Date:      2007-2014
+%%
+
+
+
+clear all;
+
+
+%% setup QP data
+simulationTime  =  [0;0.1];
+
+H.time = simulationTime;
+data1 = [1.0,0.0,0.0,0.5];
+data2 = [1.0,0.0,0.0,0.5];
+H.signals.values = [data1; data2];
+H.signals.dimensions = length(data1);
+
+g.time = simulationTime;
+data1 = [1.5,1.0];
+data2 = [1.0,1.5];
+g.signals.values = [data1; data2];
+g.signals.dimensions = length(data1);
+
+lb.time = simulationTime;
+data1 = [0.5,-2.0];
+data2 = [0.0,-1.0];
+lb.signals.values = [data1; data2];
+lb.signals.dimensions = length(data1);
+
+ub.time = simulationTime;
+data1 = [5.0,2.0];
+data2 = [5.0,-0.5];
+ub.signals.values = [data1; data2];
+ub.signals.dimensions = length(data1);
+
+
+clear simulationTime data1 data2
+
+
+%% open corresponding simulink example
+open( 'example_QProblemB.mdl' );
+
+
+
+%%
+%%	end of file
+%%
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_SQProblem.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_SQProblem.m
new file mode 100644
index 00000000000..afde11103d5
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_SQProblem.m
@@ -0,0 +1,93 @@
+%%
+%%	This file is part of qpOASES.
+%%
+%%	qpOASES -- An Implementation of the Online Active Set Strategy.
+%%	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%%	Christian Kirches et al. All rights reserved.
+%%
+%%	qpOASES 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.
+%%
+%%	qpOASES 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 qpOASES; if not, write to the Free Software
+%%	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+%%
+
+
+
+%%
+%%	Filename:  interfaces/simulink/load_example_SQProblem.m
+%%	Author:    Hans Joachim Ferreau (thanks to Aude Perrin)
+%%	Version:   3.0
+%%	Date:      2007-2014
+%%
+
+
+
+clear all;
+
+
+%% setup QP data
+simulationTime  =  [0;0.1];
+
+H.time = simulationTime;
+data1 = [1.0,0.0,0.0,0.5];
+data2 = [1.0,0.5,0.5,0.5];
+H.signals.values = [data1; data2];
+H.signals.dimensions = length(data1);
+
+g.time = simulationTime;
+data1 = [1.5,1.0];
+data2 = [1.0,1.5];
+g.signals.values = [data1; data2];
+g.signals.dimensions = length(data1);
+
+A.time = simulationTime;
+data1 = [1.0,1.0];
+data2 = [1.0,5.0];
+A.signals.values = [data1; data2];
+A.signals.dimensions = length(data1);
+
+lb.time = simulationTime;
+data1 = [0.5,-2.0];
+data2 = [0.0,-1.0];
+lb.signals.values = [data1; data2];
+lb.signals.dimensions = length(data1);
+
+ub.time = simulationTime;
+data1 = [5.0,2.0];
+data2 = [5.0,-0.5];
+ub.signals.values = [data1; data2];
+ub.signals.dimensions = length(data1);
+
+lbA.time = simulationTime;
+data1 = [-1.0];
+data2 = [-2.0];
+lbA.signals.values = [data1; data2];
+lbA.signals.dimensions = length(data1);
+
+ubA.time = simulationTime;
+data1 = [2.0];
+data2 = [1.0];
+ubA.signals.values = [data1; data2];
+ubA.signals.dimensions = length(data1);
+
+
+clear simulationTime data1 data2
+
+
+%% open corresponding simulink example
+open( 'example_SQProblem.mdl' );
+
+
+
+%%
+%%	end of file
+%%
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/make.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/make.m
new file mode 100644
index 00000000000..96352251986
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/make.m
@@ -0,0 +1,239 @@
+function [] = make( varargin )
+%MAKE Compiles the Simulink interface of qpOASES.
+%
+%Type  make            to compile all interfaces that 
+%                      have been modified,
+%type  make clean      to delete all compiled interfaces,
+%type  make clean all  to first delete and then compile 
+%                      all interfaces,
+%type  make 'name'     to compile only the interface with
+%                      the given name (if it has been modified),
+%type  make 'opt'      to compile all interfaces using the 
+%                      given compiler options.
+%
+%Copyright (C) 2013-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%Christian Kirches et al. All rights reserved.
+
+%%
+%%	This file is part of qpOASES.
+%%
+%%	qpOASES -- An Implementation of the Online Active Set Strategy.
+%%	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+%%	Christian Kirches et al. All rights reserved.
+%%
+%%	qpOASES 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.
+%%
+%%	qpOASES 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 qpOASES; if not, write to the Free Software
+%%	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+%%
+
+%%
+%%	Filename:  interfaces/simulink/make.m
+%%	Author:    Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+%%	Version:   3.0
+%%	Date:      2007-2014
+%%
+
+       
+    %% consistency check
+    if ( exist( [pwd, '/make.m'],'file' ) == 0 )
+        error( ['ERROR (',mfilename '.m): Run this make script directly within the directory', ...
+                '<qpOASES-inst-dir>/interfaces/simulink, please.'] );
+    end
+
+
+    if ( nargin > 2 )
+        error( ['ERROR (',mfilename '.m): At most two make arguments supported!'] );
+    else
+        [ doClean,fcnNames,userFlags ] = analyseMakeArguments( nargin,varargin );
+    end
+
+    
+    %% define compiler settings
+    QPOASESPATH = '../../';
+    
+    DEBUGFLAGS = ' ';
+    %DEBUGFLAGS = ' -g CXXDEBUGFLAGS=''$CXXDEBUGFLAGS -Wall -pedantic -Wshadow'' ';
+
+    IFLAGS = [ '-I. -I',QPOASESPATH,'include',' -I',QPOASESPATH,'src',' ' ];
+    CPPFLAGS = [ IFLAGS, DEBUGFLAGS, '-largeArrayDims -D__cpluplus -D__MATLAB__ -D__SINGLE_OBJECT__',' ' ];
+    defaultFlags = '-O '; %% -D__NO_COPYRIGHT__ -D__SUPPRESSANYOUTPUT__
+
+    if ( ispc == 0 )
+        CPPFLAGS  = [ CPPFLAGS, '-DLINUX ',' ' ]; 
+    else
+        CPPFLAGS  = [ CPPFLAGS, '-DWIN32 ',' ' ];
+    end
+
+    if ( isempty(userFlags) > 0 )
+        CPPFLAGS = [ CPPFLAGS, defaultFlags,' ' ];
+    else
+        CPPFLAGS = [ CPPFLAGS, userFlags,' ' ];
+    end
+
+    mexExt = eval('mexext');
+    
+    
+    %% ensure copyright notice is displayed
+    if ~isempty( strfind( CPPFLAGS,'-D__NO_COPYRIGHT__' ) )
+        printCopyrightNotice( );
+    end
+    
+    
+    %% clean if desired
+    if ( doClean > 0 )
+        
+        eval( 'delete *.o;' );
+        eval( ['delete *.',mexExt,'*;'] );
+        disp( [ 'INFO (',mfilename '.m): Cleaned all compiled files.'] );
+        pause( 0.2 );
+        
+    end
+    
+    
+    if ( ~isempty(userFlags) )
+        disp( [ 'INFO (',mfilename '.m): Compiling all files with user-defined compiler flags (''',userFlags,''')...'] );
+    end
+    
+
+    %% call mex compiler
+    for ii=1:length(fcnNames)
+        
+        cmd = [ 'mex -output ', fcnNames{ii}, ' ', CPPFLAGS, [fcnNames{ii},'.cpp'] ];
+        
+        if ( exist( [fcnNames{ii},'.',mexExt],'file' ) == 0 )
+            
+            eval( cmd );
+            disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' successfully created.'] );
+            
+        else
+            
+            % check modification time of source/Make files and compiled mex file
+            cppFile = dir( [pwd,'/',fcnNames{ii},'.cpp'] );
+            cppFileTimestamp = getTimestamp( cppFile );
+
+            makeFile = dir( [pwd,'/make.m'] );
+            makeFileTimestamp = getTimestamp( makeFile );
+            
+            mexFile = dir( [pwd,'/',fcnNames{ii},'.',mexExt] );
+            if ( isempty(mexFile) == 0 )
+                mexFileTimestamp = getTimestamp( mexFile );
+            else
+                mexFileTimestamp = 0;
+            end
+            
+            if ( ( cppFileTimestamp   >= mexFileTimestamp ) || ...
+                 ( makeFileTimestamp  >= mexFileTimestamp ) )
+                eval( cmd );
+                disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' successfully created.'] );
+            else            
+                disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' already exists.'] );
+            end
+            
+        end
+        
+    end
+
+    %% add qpOASES directory to path
+    path( path,pwd );
+
+end
+
+
+function [ doClean,fcnNames,userIFlags ] = analyseMakeArguments( nArgs,args )
+
+    doClean = 0;
+    fcnNames = [];
+    userIFlags = [];
+
+    switch ( nArgs )
+        
+        case 1
+            if ( strcmp( args{1},'all' ) > 0 )
+                fcnNames = { 'qpOASES_QProblemB','qpOASES_QProblem','qpOASES_SQProblem' };
+            elseif ( strcmp( args{1},'qpOASES_QProblemB' ) > 0 )
+                fcnNames = { 'qpOASES_QProblemB' };
+            elseif ( strcmp( args{1},'qpOASES_QProblem' ) > 0 )
+                fcnNames = { 'qpOASES_QProblem' };
+            elseif ( strcmp( args{1},'qpOASES_SQProblem' ) > 0 )
+                fcnNames = { 'qpOASES_SQProblem' };
+            elseif ( strcmp( args{1},'clean' ) > 0 )
+                doClean = 1;
+            elseif ( strcmp( args{1}(1),'-' ) > 0 )
+                % make clean all with user-specified compiler flags
+                userIFlags = args{1};
+                doClean = 1;
+                fcnNames = { 'qpOASES_QProblemB','qpOASES_QProblem','qpOASES_SQProblem' };
+            else
+                error( ['ERROR (',mfilename '.m): Invalid first argument (''',args{1},''')!'] );
+            end
+
+        case 2
+            if ( strcmp( args{1},'clean' ) > 0 )
+                doClean = 1;
+            else
+                error( ['ERROR (',mfilename '.m): First argument must be ''clean'' if two arguments are provided!'] );
+            end
+            
+            if ( strcmp( args{2},'all' ) > 0 )
+                fcnNames = { 'qpOASES_QProblemB','qpOASES_QProblem','qpOASES_SQProblem' };
+            elseif ( strcmp( args{2},'qpOASES_QProblemB' ) > 0 )
+                fcnNames = { 'qpOASES_QProblemB' };
+            elseif ( strcmp( args{2},'qpOASES_QProblem' ) > 0 )
+                fcnNames = { 'qpOASES_QProblem' };
+            elseif ( strcmp( args{2},'qpOASES_SQProblem' ) > 0 )
+                fcnNames = { 'qpOASES_SQProblem' };
+            else
+                error( ['ERROR (',mfilename '.m): Invalid second argument (''',args{2},''')!'] );
+            end
+            
+        otherwise
+            doClean = 0;
+            fcnNames = { 'qpOASES_QProblemB','qpOASES_QProblem','qpOASES_SQProblem' };
+            userIFlags = [];            
+    end
+    
+end
+
+
+function [ timestamp ] = getTimestamp( dateString )
+
+    try 
+        timestamp = dateString.datenum;
+    catch
+        timestamp = Inf;
+    end
+
+end
+
+
+function [ ] = printCopyrightNotice( )
+
+    disp( ' ' );
+    disp( 'qpOASES -- An Implementation of the Online Active Set Strategy.' );
+    disp( 'Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,' );
+    disp( 'Christian Kirches et al. All rights reserved.' );
+    disp( ' ' );
+    disp( 'qpOASES is distributed under the terms of the' );
+    disp( 'GNU Lesser General Public License 2.1 in the hope that it will be' );
+    disp( 'useful, but WITHOUT ANY WARRANTY; without even the implied warranty' );
+    disp( 'of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.' );
+    disp( 'See the GNU Lesser General Public License for more details.' );
+    disp( ' ' );
+    disp( ' ' );
+
+end
+
+
+%%
+%%	end of file
+%%
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_QProblem.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_QProblem.cpp
new file mode 100644
index 00000000000..e5cba8ffd18
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_QProblem.cpp
@@ -0,0 +1,447 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file interfaces/simulink/qpOASES_QProblem.cpp
+ *	\author Hans Joachim Ferreau (thanks to Aude Perrin)
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Interface for Simulink(R) that enables to call qpOASES as a S function
+ *  (variant for QPs with fixed matrices).
+ *
+ */
+
+
+#include <stdlib.h>
+
+#include <qpOASES.hpp>
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+#define S_FUNCTION_NAME   qpOASES_QProblem		/**< Name of the S function. */
+#define S_FUNCTION_LEVEL  2						/**< S function level. */
+
+#define MDL_START								/**< Activate call to mdlStart. */
+
+#include "simstruc.h"
+
+
+/* SETTINGS: */
+#define SAMPLINGTIME    -1						/**< Sampling time. */
+#define NCONTROLINPUTS  2						/**< Number of control inputs. */
+#define NWSR            100						/**< Maximum number of working set recalculations. */
+
+
+static void mdlInitializeSizes (SimStruct *S)   /* Init sizes array */
+{
+	int nU = NCONTROLINPUTS;
+
+	/* Specify the number of continuous and discrete states */
+	ssSetNumContStates(S, 0);
+	ssSetNumDiscStates(S, 0);
+
+	/* Specify the number of intput ports */
+	if ( !ssSetNumInputPorts(S, 7) )
+		return;
+
+	/* Specify the number of output ports */
+	if ( !ssSetNumOutputPorts(S, 4) )
+		return;
+
+	/* Specify dimension information for the input ports */
+	ssSetInputPortVectorDimension(S, 0, DYNAMICALLY_SIZED);	/* H */
+	ssSetInputPortVectorDimension(S, 1, DYNAMICALLY_SIZED); /* g */
+	ssSetInputPortVectorDimension(S, 2, DYNAMICALLY_SIZED); /* A */
+	ssSetInputPortVectorDimension(S, 3, DYNAMICALLY_SIZED); /* lb */
+	ssSetInputPortVectorDimension(S, 4, DYNAMICALLY_SIZED); /* ub */
+	ssSetInputPortVectorDimension(S, 5, DYNAMICALLY_SIZED); /* lbA */
+	ssSetInputPortVectorDimension(S, 6, DYNAMICALLY_SIZED); /* ubA */
+
+	/* Specify dimension information for the output ports */
+	ssSetOutputPortVectorDimension(S, 0, 1 );   /* fval */
+	ssSetOutputPortVectorDimension(S, 1, nU );  /* uOpt */
+	ssSetOutputPortVectorDimension(S, 2, 1 );   /* exitflag */
+	ssSetOutputPortVectorDimension(S, 3, 1 );   /* iter */
+
+	/* Specify the direct feedthrough status */
+	ssSetInputPortDirectFeedThrough(S, 0, 1);
+	ssSetInputPortDirectFeedThrough(S, 1, 1);
+	ssSetInputPortDirectFeedThrough(S, 2, 1);
+	ssSetInputPortDirectFeedThrough(S, 3, 1);
+	ssSetInputPortDirectFeedThrough(S, 4, 1);
+	ssSetInputPortDirectFeedThrough(S, 5, 1);
+	ssSetInputPortDirectFeedThrough(S, 6, 1);
+
+	/* One sample time */
+	ssSetNumSampleTimes(S, 1);
+
+	/* global variables:
+     * 0: problem
+     * 1: H
+     * 2: g
+     * 3: A
+     * 4: lb
+     * 5: ub
+     * 6: lbA
+     * 7: ubA
+     * 8: count
+     */
+
+	/* Specify the size of the block's pointer work vector */
+    ssSetNumPWork(S, 9);
+}
+
+
+#if defined(MATLAB_MEX_FILE)
+
+#define MDL_SET_INPUT_PORT_DIMENSION_INFO
+#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
+
+static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+	if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
+		return;
+}
+
+static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+	if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
+		return;
+}
+
+#endif
+
+
+static void mdlInitializeSampleTimes(SimStruct *S)
+{
+	ssSetSampleTime(S, 0, SAMPLINGTIME);
+	ssSetOffsetTime(S, 0, 0.0);
+}
+
+
+static void mdlStart(SimStruct *S)
+{
+	USING_NAMESPACE_QPOASES
+
+	int nU = NCONTROLINPUTS;
+	int size_H, size_g, size_A, size_lb, size_ub, size_lbA, size_ubA;
+	int nV, nC;
+
+	QProblem* problem;
+	real_t* count;
+
+
+	/* get block inputs dimensions */
+	size_H   = ssGetInputPortWidth(S, 0);
+	size_g   = ssGetInputPortWidth(S, 1);
+	size_A   = ssGetInputPortWidth(S, 2);
+	size_lb  = ssGetInputPortWidth(S, 3);
+	size_ub  = ssGetInputPortWidth(S, 4);
+	size_lbA = ssGetInputPortWidth(S, 5);
+	size_ubA = ssGetInputPortWidth(S, 6);
+
+
+	/* dimension checks */
+	nV = size_g;
+	nC = (int) ( ((real_t) size_A) / ((real_t) nV) );
+
+
+	if ( NWSR < 0 )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Maximum number of iterations must not be negative!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( nV <= 0 )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( size_H != nV*nV )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( ( nU < 1 ) || ( nU > nV ) )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Invalid number of control inputs!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( size_lb != nV )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( size_ub != nV )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( size_lbA != nC )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( size_ubA != nC )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+
+	/* allocate QProblem object */
+	problem = new QProblem( nV,nC );
+	if ( problem == 0 )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Unable to create QProblem object!" );
+		#endif
+		#endif
+		return;
+	}
+	
+	Options problemOptions;
+	problemOptions.setToMPC();
+	problem->setOptions( problemOptions );
+
+	#ifndef __DEBUG__
+	problem->setPrintLevel( PL_LOW );
+	#endif
+	#ifdef __SUPPRESSANYOUTPUT__
+	problem->setPrintLevel( PL_NONE );
+	#endif
+	#ifdef __DSPACE__
+	problem->setPrintLevel( PL_NONE );
+	#endif
+
+	ssGetPWork(S)[0] = (void *) problem;
+
+	/* allocate memory for QP data ... */
+	ssGetPWork(S)[1] = (void *) calloc( size_H, sizeof(real_t) );	/* H */
+	ssGetPWork(S)[2] = (void *) calloc( size_g, sizeof(real_t) );	/* g */
+	ssGetPWork(S)[3] = (void *) calloc( size_A, sizeof(real_t) );	/* A */
+	ssGetPWork(S)[4] = (void *) calloc( size_lb, sizeof(real_t) );	/* lb */
+	ssGetPWork(S)[5] = (void *) calloc( size_ub, sizeof(real_t) );	/* ub */
+	ssGetPWork(S)[6] = (void *) calloc( size_lbA, sizeof(real_t) );	/* lbA */
+	ssGetPWork(S)[7] = (void *) calloc( size_ubA, sizeof(real_t) );	/* ubA */
+	ssGetPWork(S)[8] = (void *) calloc( 1, sizeof(real_t) ); /* count */
+
+	/* reset counter */
+	count = (real_t *) ssGetPWork(S)[8];
+	count[0] = 0.0;
+}
+
+
+static void mdlOutputs(SimStruct *S, int_T tid)
+{
+	USING_NAMESPACE_QPOASES
+
+	int i;
+	int nV, nC;
+	returnValue status;
+
+	int nWSR = NWSR;
+	int nU   = NCONTROLINPUTS;
+
+	InputRealPtrsType in_H, in_g, in_A, in_lb, in_ub, in_lbA, in_ubA;
+
+	QProblem* problem;
+	real_t *H, *g, *A, *lb, *ub, *lbA, *ubA, *count;
+
+	real_t *xOpt;
+
+	real_T *out_objVal, *out_uOpt, *out_status, *out_nWSR;
+
+
+	/* get pointers to block inputs ... */
+	in_H   = ssGetInputPortRealSignalPtrs(S, 0);
+	in_g   = ssGetInputPortRealSignalPtrs(S, 1);
+	in_A   = ssGetInputPortRealSignalPtrs(S, 2);
+	in_lb  = ssGetInputPortRealSignalPtrs(S, 3);
+	in_ub  = ssGetInputPortRealSignalPtrs(S, 4);
+	in_lbA = ssGetInputPortRealSignalPtrs(S, 5);
+	in_ubA = ssGetInputPortRealSignalPtrs(S, 6);
+
+
+	/* ... and to the QP data */
+	problem = (QProblem *) ssGetPWork(S)[0];
+
+	H   = (real_t *) ssGetPWork(S)[1];
+	g   = (real_t *) ssGetPWork(S)[2];
+	A   = (real_t *) ssGetPWork(S)[3];
+	lb  = (real_t *) ssGetPWork(S)[4];
+	ub  = (real_t *) ssGetPWork(S)[5];
+	lbA = (real_t *) ssGetPWork(S)[6];
+	ubA = (real_t *) ssGetPWork(S)[7];
+
+	count = (real_t *) ssGetPWork(S)[8];
+
+
+	/* setup QP data */
+	nV = ssGetInputPortWidth(S, 1); /* nV = size_g */
+	nC = (int) ( ((real_t) ssGetInputPortWidth(S, 2)) / ((real_t) nV) ); /* nC = size_A / size_g */
+
+	for ( i=0; i<nV*nV; ++i )
+		H[i] = (*in_H)[i];
+
+	for ( i=0; i<nC*nV; ++i )
+		A[i] = (*in_A)[i];
+
+	for ( i=0; i<nV; ++i )
+	{
+		g[i] = (*in_g)[i];
+		lb[i] = (*in_lb)[i];
+		ub[i] = (*in_ub)[i];
+	}
+
+	for ( i=0; i<nC; ++i )
+	{
+		lbA[i] = (*in_lbA)[i];
+		ubA[i] = (*in_ubA)[i];
+	}
+
+	xOpt = new real_t[nV];
+
+	if ( count[0] == 0 )
+	{
+		/* initialise and solve first QP */
+		status = problem->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
+		problem->getPrimalSolution( xOpt );
+	}
+	else
+	{
+		/* solve neighbouring QP using hotstart technique */
+		status = problem->hotstart( g,lb,ub,lbA,ubA, nWSR,0 );
+		if ( ( status != SUCCESSFUL_RETURN ) && ( status != RET_MAX_NWSR_REACHED ) )
+		{
+			/* if an error occurs, reset problem data structures ... */
+			problem->reset( );
+            
+            /* ... and initialise/solve again with remaining number of iterations. */
+            int nWSR_retry = NWSR-nWSR;
+			status = problem->init( H,g,A,lb,ub,lbA,ubA, nWSR_retry,0 );
+			nWSR += nWSR_retry;
+			
+		}
+
+		/* obtain optimal solution */
+		problem->getPrimalSolution( xOpt );
+	}
+
+	/* generate block output: status information ... */
+	out_objVal = ssGetOutputPortRealSignal(S, 0);
+	out_uOpt   = ssGetOutputPortRealSignal(S, 1);
+	out_status = ssGetOutputPortRealSignal(S, 2);
+	out_nWSR   = ssGetOutputPortRealSignal(S, 3);
+
+	out_objVal[0] = ((real_T) problem->getObjVal( ));
+
+	for ( i=0; i<nU; ++i )
+		out_uOpt[i] = ((real_T) xOpt[i]);
+
+	out_status[0] = (real_t)getSimpleStatus( status );
+	out_nWSR[0] = ((real_T) nWSR);
+
+	/* increase counter */
+	count[0] = count[0] + 1;
+
+	delete[] xOpt;
+}
+
+
+static void mdlTerminate(SimStruct *S)
+{
+	USING_NAMESPACE_QPOASES
+
+	/* reset global message handler */
+	getGlobalMessageHandler( )->reset( );
+
+	int i;
+	for ( i=0; i<9; ++i )
+	{
+		if ( ssGetPWork(S)[i] != 0 )
+			free( ssGetPWork(S)[i] );
+	}
+}
+
+
+#ifdef  MATLAB_MEX_FILE
+#include "simulink.c"
+#else
+#include "cg_sfun.h"
+#endif
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_QProblemB.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_QProblemB.cpp
new file mode 100644
index 00000000000..404cb5e47a3
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_QProblemB.cpp
@@ -0,0 +1,392 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file interfaces/simulink/qpOASES_QProblemB.cpp
+ *	\author Hans Joachim Ferreau (thanks to Aude Perrin)
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Interface for Simulink(R) that enables to call qpOASES as a S function
+ *  (variant for simply bounded QPs with fixed matrices).
+ *
+ */
+
+
+#include <stdlib.h>
+
+#include <qpOASES.hpp>
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+#define S_FUNCTION_NAME   qpOASES_QProblemB		/**< Name of the S function. */
+#define S_FUNCTION_LEVEL  2						/**< S function level. */
+
+#define MDL_START								/**< Activate call to mdlStart. */
+
+#include "simstruc.h"
+
+
+/* SETTINGS */
+#define SAMPLINGTIME    -1						/**< Sampling time. */
+#define NCONTROLINPUTS  2						/**< Number of control inputs. */
+#define NWSR            100						/**< Maximum number of working set recalculations. */
+
+
+static void mdlInitializeSizes (SimStruct *S)   /* Init sizes array */
+{
+	int nU = NCONTROLINPUTS;
+
+	/* Specify the number of continuous and discrete states */
+	ssSetNumContStates(S, 0);
+	ssSetNumDiscStates(S, 0);
+
+	/* Specify the number of intput ports */
+	if ( !ssSetNumInputPorts(S, 4) )
+		return;
+
+	/* Specify the number of output ports */
+	if ( !ssSetNumOutputPorts(S, 4) )
+		return;
+
+	/* Specify dimension information for the input ports */
+	ssSetInputPortVectorDimension(S, 0, DYNAMICALLY_SIZED);	/* H */
+	ssSetInputPortVectorDimension(S, 1, DYNAMICALLY_SIZED);	/* g */
+	ssSetInputPortVectorDimension(S, 2, DYNAMICALLY_SIZED);	/* lb */
+	ssSetInputPortVectorDimension(S, 3, DYNAMICALLY_SIZED);	/* ub */
+
+	/* Specify dimension information for the output ports */
+	ssSetOutputPortVectorDimension(S, 0, 1 );   /* fval */
+	ssSetOutputPortVectorDimension(S, 1, nU );  /* uOpt */
+	ssSetOutputPortVectorDimension(S, 2, 1 );   /* exitflag */
+	ssSetOutputPortVectorDimension(S, 3, 1 );   /* iter */
+
+	/* Specify the direct feedthrough status */
+	ssSetInputPortDirectFeedThrough(S, 0, 1);
+	ssSetInputPortDirectFeedThrough(S, 1, 1);
+	ssSetInputPortDirectFeedThrough(S, 2, 1);
+	ssSetInputPortDirectFeedThrough(S, 3, 1);
+
+	/* One sample time */
+	ssSetNumSampleTimes(S, 1);
+
+	/* global variables:
+     * 0: problem
+     * 1: H
+     * 2: g
+     * 3: lb
+     * 4: ub
+     * 5: count
+     */
+
+	/* Specify the size of the block's pointer work vector */
+    ssSetNumPWork(S, 6);
+}
+
+
+#if defined(MATLAB_MEX_FILE)
+
+#define MDL_SET_INPUT_PORT_DIMENSION_INFO
+#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
+
+static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+	if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
+		return;
+}
+
+static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+	if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
+		return;
+}
+
+#endif
+
+
+static void mdlInitializeSampleTimes(SimStruct *S)
+{
+	ssSetSampleTime(S, 0, SAMPLINGTIME);
+	ssSetOffsetTime(S, 0, 0.0);
+}
+
+
+static void mdlStart(SimStruct *S)
+{
+	USING_NAMESPACE_QPOASES
+
+	int nU = NCONTROLINPUTS;
+	int size_H, size_g, size_lb, size_ub;
+	int nV;
+
+	QProblemB* problem;
+	real_t* count;
+
+
+	/* get block inputs dimensions */
+	size_H   = ssGetInputPortWidth(S, 0);
+	size_g   = ssGetInputPortWidth(S, 1);
+	size_lb  = ssGetInputPortWidth(S, 2);
+	size_ub  = ssGetInputPortWidth(S, 3);
+
+
+	/* dimension checks */
+	nV = size_g;
+
+	if ( NWSR < 0 )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Maximum number of iterations must not be negative!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( nV <= 0 )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( size_H != nV*nV )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( ( nU < 1 ) || ( nU > nV ) )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( size_lb != nV )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( size_ub != nV )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+
+	/* allocate QProblemB object */
+	problem = new QProblemB( nV );
+	if ( problem == 0 )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Unable to create QProblemB object!" );
+		#endif
+		#endif
+		return;
+	}
+
+	Options problemOptions;
+	problemOptions.setToMPC();
+	problem->setOptions( problemOptions );
+	
+	#ifndef __DEBUG__
+	problem->setPrintLevel( PL_LOW );
+	#endif
+	#ifdef __SUPPRESSANYOUTPUT__
+	problem->setPrintLevel( PL_NONE );
+	#endif
+	#ifdef __DSPACE__
+	problem->setPrintLevel( PL_NONE );
+	#endif
+
+	ssGetPWork(S)[0] = (void *) problem;
+
+	/* allocate memory for QP data ... */
+	ssGetPWork(S)[1] = (void *) calloc( size_H, sizeof(real_t) );	/* H */
+	ssGetPWork(S)[2] = (void *) calloc( size_g, sizeof(real_t) );	/* g */
+	ssGetPWork(S)[3] = (void *) calloc( size_lb, sizeof(real_t) );	/* lb */
+	ssGetPWork(S)[4] = (void *) calloc( size_ub, sizeof(real_t) );	/* ub */
+	ssGetPWork(S)[5] = (void *) calloc( 1, sizeof(real_t) );		/* count */
+
+	/* reset counter */
+	count = (real_t *) ssGetPWork(S)[5];
+	count[0] = 0.0;
+}
+
+
+static void mdlOutputs(SimStruct *S, int_T tid)
+{
+	USING_NAMESPACE_QPOASES
+
+	int i;
+	int nV;
+	returnValue status;
+
+	int nWSR = NWSR;
+	int nU   = NCONTROLINPUTS;
+
+	InputRealPtrsType in_H, in_g, in_lb, in_ub;
+
+	QProblemB* problem;
+	real_t *H, *g, *lb, *ub, *count;
+
+	real_t *xOpt;
+
+	real_T *out_objVal, *out_uOpt, *out_status, *out_nWSR;
+
+
+	/* get pointers to block inputs ... */
+	in_H  = ssGetInputPortRealSignalPtrs(S, 0);
+	in_g  = ssGetInputPortRealSignalPtrs(S, 1);
+	in_lb = ssGetInputPortRealSignalPtrs(S, 2);
+	in_ub = ssGetInputPortRealSignalPtrs(S, 3);
+
+	/* ... and to the QP data */
+	problem = (QProblemB *) ssGetPWork(S)[0];
+
+	H  = (real_t *) ssGetPWork(S)[1];
+	g  = (real_t *) ssGetPWork(S)[2];
+	lb = (real_t *) ssGetPWork(S)[3];
+	ub = (real_t *) ssGetPWork(S)[4];
+
+	count = (real_t *) ssGetPWork(S)[5];
+
+
+	/* setup QP data */
+	nV = ssGetInputPortWidth(S, 1); /* nV = size_g */
+
+	for ( i=0; i<nV*nV; ++i )
+		H[i] = (*in_H)[i];
+
+	for ( i=0; i<nV; ++i )
+	{
+		g[i] = (*in_g)[i];
+		lb[i] = (*in_lb)[i];
+		ub[i] = (*in_ub)[i];
+	}
+
+	xOpt = new real_t[nV];
+
+	if ( count[0] == 0 )
+	{
+		/* initialise and solve first QP */
+		status = problem->init( H,g,lb,ub, nWSR,0 );
+		problem->getPrimalSolution( xOpt );
+	}
+	else
+	{
+		/* solve neighbouring QP using hotstart technique */
+		status = problem->hotstart( g,lb,ub, nWSR,0 );
+		if ( ( status != SUCCESSFUL_RETURN ) && ( status != RET_MAX_NWSR_REACHED ) )
+		{
+			/* if an error occurs, reset problem data structures ... */
+			problem->reset( );
+            
+            /* ... and initialise/solve again with remaining number of iterations. */
+            int nWSR_retry = NWSR-nWSR;
+			status = problem->init( H,g,lb,ub, nWSR_retry,0 );
+            nWSR += nWSR_retry;
+		}
+
+		/* obtain optimal solution */
+		problem->getPrimalSolution( xOpt );
+	}
+
+	/* generate block output: status information ... */
+	out_objVal = ssGetOutputPortRealSignal(S, 0);
+	out_uOpt   = ssGetOutputPortRealSignal(S, 1);
+	out_status = ssGetOutputPortRealSignal(S, 2);
+	out_nWSR   = ssGetOutputPortRealSignal(S, 3);
+
+	out_objVal[0] = ((real_T) problem->getObjVal( ));
+
+	for ( i=0; i<nU; ++i )
+		out_uOpt[i] = ((real_T) xOpt[i]);
+
+	out_status[0] = (real_t)getSimpleStatus( status );
+	out_nWSR[0] = ((real_T) nWSR);
+
+	/* increase counter */
+	count[0] = count[0] + 1;
+
+	delete[] xOpt;
+}
+
+
+static void mdlTerminate(SimStruct *S)
+{
+	USING_NAMESPACE_QPOASES
+
+	/* reset global message handler */
+	getGlobalMessageHandler( )->reset( );
+
+	int i;
+	for ( i=0; i<6; ++i )
+	{
+		if ( ssGetPWork(S)[i] != 0 )
+			free( ssGetPWork(S)[i] );
+	}
+}
+
+
+#ifdef  MATLAB_MEX_FILE
+#include "simulink.c"
+#else
+#include "cg_sfun.h"
+#endif
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_SQProblem.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_SQProblem.cpp
new file mode 100644
index 00000000000..533d07a4cda
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_SQProblem.cpp
@@ -0,0 +1,460 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file interfaces/simulink/qpOASES_SQProblem.cpp
+ *	\author Hans Joachim Ferreau (thanks to Aude Perrin)
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Interface for Simulink(R) that enables to call qpOASES as a S function
+ *  (variant for QPs with varying matrices).
+ *
+ */
+
+
+#include <stdlib.h>
+
+#include <qpOASES.hpp>
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+#define S_FUNCTION_NAME   qpOASES_SQProblem		/**< Name of the S function. */
+#define S_FUNCTION_LEVEL  2						/**< S function level. */
+
+#define MDL_START								/**< Activate call to mdlStart. */
+
+#include "simstruc.h"
+
+
+/* SETTINGS */
+#define SAMPLINGTIME    -1						/**< Sampling time. */
+#define NCONTROLINPUTS  2						/**< Number of control inputs. */
+#define NWSR            100	    				/**< Maximum number of working set recalculations. */
+
+
+static void mdlInitializeSizes (SimStruct *S)   /* Init sizes array */
+{
+	int nU = NCONTROLINPUTS;
+
+	/* Specify the number of continuous and discrete states */
+	ssSetNumContStates(S, 0);
+	ssSetNumDiscStates(S, 0);
+
+	/* Specify the number of intput ports */
+	if ( !ssSetNumInputPorts(S, 7) )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input ports!" );
+		#endif
+		#endif
+		return;
+	}
+
+	/* Specify the number of output ports */
+	if ( !ssSetNumOutputPorts(S, 4) )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Invalid number of output ports!" );
+		#endif
+		#endif
+		return;
+	}
+
+	/* Specify dimension information for the input ports */
+	ssSetInputPortVectorDimension(S, 0, DYNAMICALLY_SIZED);	/* H */
+	ssSetInputPortVectorDimension(S, 1, DYNAMICALLY_SIZED); /* g */
+	ssSetInputPortVectorDimension(S, 2, DYNAMICALLY_SIZED); /* A */
+	ssSetInputPortVectorDimension(S, 3, DYNAMICALLY_SIZED); /* lb */
+	ssSetInputPortVectorDimension(S, 4, DYNAMICALLY_SIZED); /* ub */
+	ssSetInputPortVectorDimension(S, 5, DYNAMICALLY_SIZED); /* lbA */
+	ssSetInputPortVectorDimension(S, 6, DYNAMICALLY_SIZED); /* ubA */
+
+	/* Specify dimension information for the output ports */
+	ssSetOutputPortVectorDimension(S, 0, 1 );   /* fval */
+	ssSetOutputPortVectorDimension(S, 1, nU );  /* uOpt */
+	ssSetOutputPortVectorDimension(S, 2, 1 );   /* exitflag */
+	ssSetOutputPortVectorDimension(S, 3, 1 );   /* iter */
+
+	/* Specify the direct feedthrough status */
+	ssSetInputPortDirectFeedThrough(S, 0, 1);
+	ssSetInputPortDirectFeedThrough(S, 1, 1);
+	ssSetInputPortDirectFeedThrough(S, 2, 1);
+	ssSetInputPortDirectFeedThrough(S, 3, 1);
+	ssSetInputPortDirectFeedThrough(S, 4, 1);
+	ssSetInputPortDirectFeedThrough(S, 5, 1);
+	ssSetInputPortDirectFeedThrough(S, 6, 1);
+
+	/* One sample time */
+	ssSetNumSampleTimes(S, 1);
+
+	/* global variables:
+     * 0: problem
+     * 1: H
+     * 2: g
+     * 3: A
+     * 4: lb
+     * 5: ub
+     * 6: lbA
+     * 7: ubA
+     * 8: count
+     */
+
+	/* Specify the size of the block's pointer work vector */
+    ssSetNumPWork(S, 9);
+}
+
+
+#if defined(MATLAB_MEX_FILE)
+
+#define MDL_SET_INPUT_PORT_DIMENSION_INFO
+#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
+
+static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+	if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
+		return;
+}
+
+static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+	if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
+		return;
+}
+
+#endif
+
+
+static void mdlInitializeSampleTimes(SimStruct *S)
+{
+	ssSetSampleTime(S, 0, SAMPLINGTIME);
+	ssSetOffsetTime(S, 0, 0.0);
+}
+
+
+static void mdlStart(SimStruct *S)
+{
+	USING_NAMESPACE_QPOASES
+
+	int nU = NCONTROLINPUTS;
+	int size_H, size_g, size_A, size_lb, size_ub, size_lbA, size_ubA;
+	int nV, nC;
+
+	SQProblem* problem;
+	real_t* count;
+
+
+	/* get block inputs dimensions */
+	size_H   = ssGetInputPortWidth(S, 0);
+	size_g   = ssGetInputPortWidth(S, 1);
+	size_A   = ssGetInputPortWidth(S, 2);
+	size_lb  = ssGetInputPortWidth(S, 3);
+	size_ub  = ssGetInputPortWidth(S, 4);
+	size_lbA = ssGetInputPortWidth(S, 5);
+	size_ubA = ssGetInputPortWidth(S, 6);
+
+
+	/* dimension checks */
+	nV = size_g;
+	nC = (int) ( ((real_t) size_A) / ((real_t) nV) );
+
+	if ( NWSR < 0 )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Maximum number of iterations must not be negative!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( nV <= 0 )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( size_H != nV*nV )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( ( nU < 1 ) || ( nU > nV ) )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Invalid number of control inputs!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( size_lb != nV )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( size_ub != nV )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( size_lbA != nC )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+	if ( size_ubA != nC )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
+		#endif
+		#endif
+		return;
+	}
+
+
+	/* allocate QProblem object */
+	problem = new SQProblem( nV,nC );
+	if ( problem == 0 )
+	{
+		#ifndef __DSPACE__
+		#ifndef __XPCTARGET__
+		mexErrMsgTxt( "ERROR (qpOASES): Unable to create QProblem object!" );
+		#endif
+		#endif
+		return;
+	}
+
+	Options problemOptions;
+	problemOptions.setToMPC();
+	problem->setOptions( problemOptions );
+
+	#ifndef __DEBUG__
+	problem->setPrintLevel( PL_LOW );
+	#endif
+	#ifdef __SUPPRESSANYOUTPUT__
+	problem->setPrintLevel( PL_NONE );
+	#endif
+	#ifdef __DSPACE__
+	problem->setPrintLevel( PL_NONE );
+	#endif
+
+	ssGetPWork(S)[0] = (void *) problem;
+
+	/* allocate memory for QP data ... */
+	ssGetPWork(S)[1] = (void *) calloc( size_H, sizeof(real_t) );	/* H */
+	ssGetPWork(S)[2] = (void *) calloc( size_g, sizeof(real_t) );	/* g */
+	ssGetPWork(S)[3] = (void *) calloc( size_A, sizeof(real_t) );	/* A */
+	ssGetPWork(S)[4] = (void *) calloc( size_lb, sizeof(real_t) );	/* lb */
+	ssGetPWork(S)[5] = (void *) calloc( size_ub, sizeof(real_t) );	/* ub */
+	ssGetPWork(S)[6] = (void *) calloc( size_lbA, sizeof(real_t) );	/* lbA */
+	ssGetPWork(S)[7] = (void *) calloc( size_ubA, sizeof(real_t) );	/* ubA */
+	ssGetPWork(S)[8] = (void *) calloc( 1, sizeof(real_t) ); /* count */
+
+	/* reset counter */
+	count = (real_t *) ssGetPWork(S)[8];
+	count[0] = 0.0;
+}
+
+
+
+static void mdlOutputs(SimStruct *S, int_T tid)
+{
+	USING_NAMESPACE_QPOASES
+
+	int i;
+	int nV, nC;
+	returnValue status;
+
+	int nWSR  = NWSR;
+	int nU   = NCONTROLINPUTS;
+
+	InputRealPtrsType in_H, in_g, in_A, in_lb, in_ub, in_lbA, in_ubA;
+
+	SQProblem* problem;
+	real_t *H, *g, *A, *lb, *ub, *lbA, *ubA, *count;
+
+	real_t *xOpt;
+
+	real_T *out_objVal, *out_uOpt, *out_status, *out_nWSR;
+
+
+	/* get pointers to block inputs ... */
+	in_H   = ssGetInputPortRealSignalPtrs(S, 0);
+	in_g   = ssGetInputPortRealSignalPtrs(S, 1);
+	in_A   = ssGetInputPortRealSignalPtrs(S, 2);
+	in_lb  = ssGetInputPortRealSignalPtrs(S, 3);
+	in_ub  = ssGetInputPortRealSignalPtrs(S, 4);
+	in_lbA = ssGetInputPortRealSignalPtrs(S, 5);
+	in_ubA = ssGetInputPortRealSignalPtrs(S, 6);
+
+
+	/* ... and to the QP data */
+	problem = (SQProblem *) ssGetPWork(S)[0];
+
+	H   = (real_t *) ssGetPWork(S)[1];
+	g   = (real_t *) ssGetPWork(S)[2];
+	A   = (real_t *) ssGetPWork(S)[3];
+	lb  = (real_t *) ssGetPWork(S)[4];
+	ub  = (real_t *) ssGetPWork(S)[5];
+	lbA = (real_t *) ssGetPWork(S)[6];
+	ubA = (real_t *) ssGetPWork(S)[7];
+
+	count = (real_t *) ssGetPWork(S)[8];
+
+
+	/* setup QP data */
+	nV = ssGetInputPortWidth(S, 1); /* nV = size_g */
+	nC = (int) ( ((real_t) ssGetInputPortWidth(S, 2)) / ((real_t) nV) ); /* nC = size_A / size_g */
+
+	for ( i=0; i<nV*nV; ++i )
+		H[i] = (*in_H)[i];
+
+	for ( i=0; i<nC*nV; ++i )
+		A[i] = (*in_A)[i];
+
+	for ( i=0; i<nV; ++i )
+	{
+		g[i] = (*in_g)[i];
+		lb[i] = (*in_lb)[i];
+		ub[i] = (*in_ub)[i];
+	}
+
+	for ( i=0; i<nC; ++i )
+	{
+		lbA[i] = (*in_lbA)[i];
+		ubA[i] = (*in_ubA)[i];
+	}
+
+	xOpt = new real_t[nV];
+
+	if ( count[0] == 0 )
+	{
+		/* initialise and solve first QP */
+		status = problem->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
+		problem->getPrimalSolution( xOpt );
+	}
+	else
+	{
+		/* solve neighbouring QP using hotstart technique */
+		status = problem->hotstart( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
+		if ( ( status != SUCCESSFUL_RETURN ) && ( status != RET_MAX_NWSR_REACHED ) )
+		{
+			/* if an error occurs, reset problem data structures ... */
+			problem->reset( );
+            
+            /* ... and initialise/solve again with remaining number of iterations. */
+            int nWSR_retry = NWSR-nWSR;
+			status = problem->init( H,g,A,lb,ub,lbA,ubA, nWSR_retry,0 );
+            nWSR += nWSR_retry;
+		}
+	        
+        /* obtain optimal solution */
+        problem->getPrimalSolution( xOpt );
+	}
+
+	/* generate block output: status information ... */
+	out_objVal = ssGetOutputPortRealSignal(S, 0);
+	out_uOpt   = ssGetOutputPortRealSignal(S, 1);
+	out_status = ssGetOutputPortRealSignal(S, 2);
+	out_nWSR   = ssGetOutputPortRealSignal(S, 3);
+
+	out_objVal[0] = ((real_T) problem->getObjVal( ));
+
+	for ( i=0; i<nU; ++i )
+		out_uOpt[i] = ((real_T) xOpt[i]);
+
+	out_status[0] = (real_t)getSimpleStatus( status );
+	out_nWSR[0] = ((real_T) nWSR);
+
+	/* increase counter */
+	count[0] = count[0] + 1;
+
+	delete[] xOpt;
+}
+
+
+static void mdlTerminate(SimStruct *S)
+{
+	USING_NAMESPACE_QPOASES
+
+	/* reset global message handler */
+	getGlobalMessageHandler( )->reset( );
+
+	int i;
+	for ( i=0; i<9; ++i )
+	{
+		if ( ssGetPWork(S)[i] != 0 )
+			free( ssGetPWork(S)[i] );
+	}
+}
+
+
+#ifdef  MATLAB_MEX_FILE
+#include "simulink.c"
+#else
+#include "cg_sfun.h"
+#endif
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/libs/libqpOASES.so b/3rdparty/qpOASES/qpOASES-3.0/libs/libqpOASES.so
new file mode 120000
index 00000000000..6b42ca6417a
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/libs/libqpOASES.so
@@ -0,0 +1 @@
+libqpOASES.so.3.0
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/libs/libqpOASES.so.3.0 b/3rdparty/qpOASES/qpOASES-3.0/libs/libqpOASES.so.3.0
new file mode 100755
index 0000000000000000000000000000000000000000..363fa77a0367dd7857020474fa0ccfd668b2b2a4
GIT binary patch
literal 1269403
zcmeFad3+O9_dh;`q6HBGqE<wWtOXI1vb7+Z0)?^%TA(V>l(x`fm(T^vB8E*{a0!Zt
zxE0qbqICf)K57KS3Mv5+tEfa=XaO;xXhnqIIrpAP&P*}|pXd9}@Adjzz0ADdbLM@|
zUFXi7ne?v2(ZiZVL?~)#s@$X?GF>J)8yzTpwXJHR*pxWsLio3f(oxtp3mggEnfh*|
zAl0p+m<2}l3m{f~D34ct*je!ER<Q`EcxbDh*M!b9e=E;3e=E;Zw<a`zpl+iiRL~k@
zjm%caR;nn9x<y+I_MeI~aldqoWc1rrt%RAPZq@U~!+Ds$^9?34oIvRrm5aT-G*q{g
z9g(+oaHuK7;bSK%&6|whe|G+-GgptTe#k#6(edP;z2@VY&7pic!qx_{B}7xxrb@J}
z?Yila-AqwcO3MhPh1v9xsX|#_<{o@7s@mJc(yV8sD>d`!+0RFGj8v8`vqU~;Ge>;9
zrR|VtS6h?S8WmwzB8Qrn4z)I^a+#n(Btc0>CVf216mxH^5}6$F;Hq-dP?OT6MU>L#
z*!UKawthclN12<&fCCX`lj5n`(lxT3&9tO#XGgoiF2!b2TCTK3DG^uLE{t|v5b@&R
zb#rY=Wlh{oqm%@j>F$bFwnfbqTg1A>N~CMsMT#xS6k&2X97C08rNservmwixc4^mc
z?zM^|(tK64Vv6Z}fnw?r=`$(oE@>T69%+tJT%DIF0~A}lBSMLCMA}B0hAK-Hm$@<`
zqJ_CRl+VQ}Wr`)cnLpCAc4)^-FK_u$=cOY%46Ru1UcR(V)X;?H9ij)D7P&W?FGx47
zu{|B7nA|OGvCo;KOam&LI!*0NWlcJoZIMZe$KDo-+oUveMYt|*>5fWhGC?ul+rj*3
zl(OUA<LjE3;ploLBFY)LXpyZ-aYZPzBO;dCToI1;$|h6f=nrj!AG!SQWlGZ@ZN*CD
z^-Da1?|u5OsEZZl%D%BtWiE5uHcdK3Emo9!Yb<RSxxO90acPqa+VzO&d`YRx8`Y$&
zqo?0x*E(F$X2llkQXsXCwk~msYmt&V7@Sg~f3!!f>j-V26U5FCF?500RqO@-=?1${
zTS^az*Fn4iq7`Bvh<zdUgV-Np9K-<-2SU6NB8HpfM*Mp-{F?wV5#lh2!y&2x8jUgn
z;z)?2RT8%2Af^zW#Q0R&zZJGqXnz`P(`erT+v&8Q3EM1)GYHS5Z8mLlXgdqGxeyBo
zJ7GJU_KRqXn_}88f$bdHFQqMR=0cnYaRCAfchYttY+ba!6t;KM{ynrUqis2Dm&5j6
zi1$N$5aL4+S3z6@aV^BhAwCK5DToyiF|3mt<!RVH1F;g~dWf$;d=+99#MdCc0g;D|
z3~Yk^w;)zS+zjy@i2s539z+ksEfBXt<Y60t4><w*9~0gQ+fQkK7i_%{cSHP~xV^O9
z2iq?p)<XOm;(mzVK>QZscMuOi{2n5PgAjiZdx~-hwsjEeAs!~~C~S|>K5l-6=!bY5
z;t7bqLsY{bu=6K@zhL_}?VqOY8Qj8P8wv43h!~<^+XCW65HE(<3L*v*Y}?R&Ti9Mg
z`|W6ZDQ(gAa@fBDVta^J65j!~SJOV)T*LOwuzxMYju1OR>;f?cVh@OF=n4D17`C$g
zK5QTD`vUIIxH#C4hj=5zn;_aCVn~4PVA_|Aq6`5%jJPD)j-c%*+N!pEI|h!8g*Xo4
z_%L?K#HGM?BE(6AC&M<C_EGN^*uNFxZNyE1?No?q5FHTHA!b0#gs6rYus;*xEaGm5
zZ7#$-h_fLULM(z<46y_vhB>sw{ZiVWOWXOhT|nDAX^XZCVgD||i(tD1;xdT$KrDk;
z4skiedm-Kr@d1c#h-z2~`>P1AW;`BWL-<kHK2G~j!1gKHuYm13+JBn1&%*Y3h%Z81
z5AkJ)YS;k#uMn<+?duRX5`K%eZ_^gf*$n&d5`GW1??d!J+yZec#O)9<d<5H%A?_sn
z32Z-w_!-1q5O+hwum`rEL;Mor*AVwZ{1)PO5Ha{@D-ra4hJS?ppCKM1uAUt`0{cfH
z{sQqB<5A~Vz<!9wA^ryOcZg~@N&A1m_D_fn5dVgF8sb@qN)z}Wq8ggQelv&{LX3jg
z0^&swFNW9(VjGAUE`jZ35HBYj4cqp#-vPE)LA(Z{nYe3V+Yw?Xh@ByJgNUI!Y<tkY
zWZ>FNxF>9{r~MmX8%z7WY1;?3eIfRP7zZ&P;y{RMxDobmf_O8;1c-TO%I7_kyd$xV
zZ%5*5pKd#R#fF!U<Scl(bKzfK?ETbpV8i~%4JF48=V#aSODo^}{Czt|tlUuIdgv4H
zFP8b;UxD*dET`_+X;~lNrj>JL{RnHum(4zySKjRJX+J-3-IV_K-FjiSX3ZA;vt!qd
zJ(|YvIoUP#wtCRpyx@s>%d)1uxqYmoGB)O`?aBq2Rd3&Syw$Q_pZsIQ!u4||Z~m?4
zfk%c$7W~sADd`>W+ykyDU)}wEuZ?rsPAI&1@sHDN-tAWnzOVb4CciD-Uc2ql$rqhI
zyMNWS|M~Jo-<%G6FS1s4db{812Y+~Oz-`Bmo;f&qN8-#o_bjmgxFfOkGyBUn{89ew
z@!7?erDwl5mH2dOL~Qpxrt!U3=Z)KQY1~!KH<YY+ctgo?IHz^%sHK1J8=Li=Z*14Q
z_NA`9ao4UJ=lt|mP5(Q)cs4xW^6l<mTebPUJv#?I=ejuNfgAEpeD?EQDPy~SGkDjH
z&rJJh*R{{~n|H_(|JCWkdC$*xxXX)%qz)~=EOv6wk})%{&pCP~`@s5PG3&2hy7uJJ
zGslxHA3Zo`f5+zQ#-5ruKJtynT3D8+PknRkRR5C~IahpV%`=@$9suX;-<<wd?N4uv
zi@nOcYS@6~gAW}jTHon|6*v4eclKBPq8H~ZEi(=K?#ZpMT=Hm#2@ASDYI&#Q{MUD!
zcz*8OH_kqDxa`}g7w`S1d3vuqtk?bodHQ_UjeY+(^33H=Wn?Xnc>d=pe=V84_-e~P
zQ?|VE^R9d64BES5QO=JE53jxC%a7h|`^CP`qL#GVKXdMZhu*!c`ML{74!`czo$uaZ
zp8V;;_fyAq?J#`Tjh(hW?0bLmKUcljYje~O<7!<EV>-;-@Ji1=-l;Adz0rH~oB4T1
z&&(`aeg8Lq9X&Jrsyi|(k~$q<^l0mQFWopHHRroco`+|*zi<1A4mX|Hbi>+)Cq9cG
zlK92Zd2r5E@29^tq3X*^PVTd0pMIm2d*WJ8yMbBjA8Pt(O25*6mo%?;f7ATJb(eY{
ztXQ1ft?uQ-l2b>Xxp~j%asFE#*s?LBXXT6cw{g$?=FrOC-`zES{mw~KHV*6NTYJHv
zbs4qY?|Oek?`J-4mi0-+i(_y5@z}IT-?a6<A0FR1efB*^lD>Fm>B1wAr?u+*{q2Pj
zpEaGBy7$R122SWObNsb&DUaN};rb7rXnpT(W3mgMo;Gp8+0!kac>Un<nZHc^vG4vD
z+naA%c6#~)r%v>pacslN_0?a$^Vn_eliOKm^gQxr`o|sbeXYr3i++9WS^sa>d~)dE
z=)&EfeY`#`<@N98+h0EM-cOs}n)1lz$(sg_aveRnY5b}$@4vook#)w=mlK!l%D?NG
zi0=+<I&<WiJEuG`w)@s~w{C>HfJ2YF|M+Fsq$y3C|KnW!_NLoE`rrZc!Pw{j{^6UY
z&rLkKe(9JU_uo5k_KoXbdFwUniXoGx%%9X@=7BMX1|_@_|4j0V3DaJC;H!7`)n*^q
zJ>j0M&yWAPN%YpQmLBR;cTsGUs9%>2dgRF;?_9a9S;6og-=wuq?&N;<NUv#*CD9WP
zuXtmB&+LN<t-st<mR(eE<ifKT|1k5NIR`rKd}z!Mtv0?8H}8>c5l>D@Uw7rtsn>5A
zSiJkaS1h|HWp%sZBikowrc0MS{mS)wCr#<T=_}tq@2%RjI`Ybm$NZI+gzgVddj7c=
zvkP7v)wS8|)W1F%K55FRoR_D&KK*lW^3H^d$Df+~-S<~k?yD?`h<I*bMDLoM$69?j
z>;0D}_V4uEf#U-<ZoKLHqBgc+J0F|BviH{5tnqJ5T6+2HuUo%-{pkhC2S)s!GAG{l
z*8I}l@6GPpXKm5Q6|=p49;=UcRX(+A<;Jh7Hq2hWd+e&OZyVD8*y^k^TSgzB-Q>dy
z>RuiGRFh9Hxp+bShTcE4>Dcq^r-P$*J=P-s{oi_T`|081gEqEY^zPhlmyBrc-L>}D
zZ>}hxo4w=0wXbx`oE15_@3gaZb0Z?gq#XOa`MpPazj1O_tA|$H-)-ehGw=H3=vv>O
zl~d3DShIdy`7cVulvS5E^c~Xmo&P*P^3mp3eSg!RscjSfKJ&`4k1n|5^9gJHdy@bD
z^!?Q2hjNCOp7^2Z$a|h0U%V{$o^IFJnhZM_bM>WfzB1<P=4br3k9cbCnK>_RoAJrk
z3m4_AKhW>xaXpXr%V_!6{dW(teRprs_z`VSmGtks@!2ME=^Ot1A^pz7FFo|hJ-wcu
z_Sxo>3*(+>`FPpNN8_iR9q_`%9j{)p^sAioC~M7^hsR#GFd`x&-t+6_KR@?Uw+T-F
z=@BbqthRf4-TlmV)2N&gbx+@a)AgnMO4~oY<K<HWc5J?(|Awh&YmO@sf17q>48P*G
zt0I5-^P0t<Ww*Yi<9(ACxt{;?<nKGTUOuw4{n3HNpJZfqm_0Z3$-5k{zA*ppH*YFG
zo8=t2|K?|^&p3a|9r18Nmzvl=z8}6UXZ^Rgm$evj;n@{i+gP1n9K7*H|JZ?#<Q!_a
z=#7aZ+76u8zi;$l*R}VL|L|~=xF)^MX18zOYIAwvo4pqIYqobo{GG#=7v^qxC+BnT
zJ-sF`cz$Jd|LCjJb6$LY-iATLbH41;{q^oASBx0D@V-^!ulxN<IIn2vJ-za$H~%2{
z+v6Wse_h)CtFKlr9sfYL$z5V2_TF>VZCA~nvFgruzD=6b_phB>Q^%IJf9l1j4{pfW
z{l>bxkF6VD@n+X|KP}z9f9|6ruU<8$^v9=H-`f43Urvp?<LBziOYVMYZ|=^miynFZ
z?RS27a6z9tSGnH0E3y5>Z@quy^rjzX?4J1H;j!<P7v^1&G4kD!%L|vj-C@Yd9$kjc
z|E*h#O+TKU{mA^O3qGwLn71$X?pnw3C$8!L-h*3Prd`%_S<d(;KHI+|aqVK$>xuTC
ze9JQ4x}o6XZHM<P**jq>=q*^^`JVW02RB^t;@eHWS+%-%^1w}_<LCcyck}x;l;mBw
zIq$%8ul(|l=Y#q8zq@qT<L&>GH}3hOVam?#mp@@jFWUL?YcJpN&(5s}=Fg7!YIbUe
zV`raf_#h&)>hR~kw%YNI`J&^mzc=QE$1mD)>Fk|buN}I#sA<V-H{WEdE*kz=w@m|!
z_Ov(M`r+a7<4MCZ{`hl$L)@7DZ=_bI4a)rD!AY;2dh(xr&5NcU7%^F$ml$oED$Nzg
z1vn`ShQpUBN^r$8$fK1x2m^cR20YGV!|8k%8J^#Fd3Zj}z@M`T;q5yZ_|ssZ^N2yZ
zaRxdQfbR%VAEGaTxvHK7yyepHem)8I;mYUq1>tq_49e{lgL1(8g>d@o`-j)hH7K7f
z1OBC(!s}dT;OCPD<@~ULpQ#sx_y2AK`+p4dd-V*j^RYoWADxQ!7b%WbkP!%3kRFCv
z%_Q&YCW!@-R;omPFH)RcC4W2dXJLU4b-aH`K7sh(NT&)HA0YH2zN<~zd+??O0yG;D
zb`O`lk}7#ztHCe=8tAo3JH=PiSzv6h7}mieBBtx48z<GzDLtgUYp=AgmdPs%dP{pV
z-poP3`!EdaCzH;grUECn78_+VG%8U(^q@e^@5&U(yN*hM#nN1P=xWKk=|;aL@ngXZ
zbxhaFbRjtrru3J*4Q~`7;2Iu=yRVnL|0ikRT++&0#ETnQ;GnsR@QYR2d+=rzf|{=R
z8p*p?X!-ZX6aSG+7w`2j*rrI{yjzyT2)R+3cappvZ~7sq`dv+uw={u<0pSs{Zz|gJ
zMT&2bv{(D)lp87Cb~3>h;w)HOA%AS!CBK^ZRs*EH6K|3ssO8p@%HMQE@-bw;xC8k^
zi!#vsL|8wJ_|ei{9lJh)=7II%rbUZm<o}}XlK0`_0t7X`kX^73y^j1kQrg?+Nc|jf
zmZCfctMEcULF!B=hgMyQ?ek*Aa<wcswf)bzf^_HxekSRBN&ef(|K?IoDW&?YQ0tfU
zAA<&i>6)*R8S+wwqp6+yEs}qf@;m2RY45`U3POa;hSEM+@|G_pLCsMqF=+pfLpq|J
z!}B5x?}&e4-G}<G<73$$)%qGLt{2c)ev|r#$e)WRk^k)^v7B^1r26$!{aT4X9WCu$
zG=Sk6Foq?h<7+Q<)c)QfR@!^1zq~EYmBKEPcYh$uL9O>`?Imx+1waUDdya?l#{BBX
z$xSxNiv=05SM&QQR5#k|$IrW|UsUM&#aZe{j+<q=&^$!An8tw$su0zm$7wwGHk0k@
znhT|Ura`}W;5zA#nHI`u*i({KKPSp`KcjTVQT_U9Je*DZQiE}LB=HX2_=#(HqMhi*
z!`)*gZ>g8<;Vsg?MdepZ{w%fg*WkJ=bm%$))hfcvR9_YO(*G}ruZIB#?L9Ou!L&q#
zmV+tX7^&Y~o~HbkDtTX;Y(M+R{xr4c3Tn>)Mff6K+Uv)wfuki~ai`SZL-roKyiO_K
z$bR7@{s7gBUso?VG@evYM_CdjPxy_>!A|ASjdXsZ_G8nvpJVN$KjxM)U(;lBQHD}K
z^5cRygf!BLx{CbVA^o&d`CI}CiSopSSO{u=`Mj^>Z8RUamvnAc$FWhe9Oe+8bDOmH
zmPvo+(1l?YKvBP4q<$vZKS=fKr26edymyea_u=9xgj=PYvIrVB>Nx(9#1=Kbq+_Oj
zt+t2$7SgGc=?)<Mf7J0gNAkFqjiD#ZuTbAl^Bi?Oeh`(j{}<`c1=McysJ{HTa0@|A
z*9TCPXGbagrZ@`EJCjI%nzY|2H_Bg7;ArnYBZ+=wf4Y_AEq<BbYbd{^lwTW_8w__M
zZ0{lMZ3kq!>U`rV8s99GuZ3ivNA1)8y|mv#{QbSipP#ilZN`y5k4ryaA^(qFD|z)n
z#V=G}3#c91>7h(d(jSr{?frD!UrPK!XsCESwbT0-Q0yY~Cw(tn*VOje7X-1~>_139
zRsIPmK;&(>fDZwNWf9(`@^{aWjt?XM|D<-}#RmrvRGm?9VL}}TJxow}8}xVNt2Rl2
z{*>Q=z41N^(rqGnwfv)QC4cgy;c6P7%MHf8Ck)27&2Rz2bUoilKh^6+bVteiAJh7O
zOS0t6UrB$c8z_I!_~X4@`a|7D$r~x{-6hf<s#^prwFkSdJ$R;2x<5#rXQ>>f7}Ref
z4D9H?8z00#P|I@wv}@!m#!Eg^9EE4sG!A%a9H4He^fF6((@JTtmh(!R<b5=L!n8_+
zx1c=nzQctRMF?*3ucBn%OzD#SmBjx#T=IUpf4Lm|!*Er1$=hi@siwQfVBCJ)VBDTg
z`t}rA&UoL8A+4v>cTjt7O8k|HlK0VgJ6M`4`)!i9-~%HF;GPI~Lw>Oxx~M$Wap|1|
zY44$VDGb-r9t^O2oOJzyZY#pCJ*2&z#uMrWik-$2{kYwo`iqbH(R|Xu`x2~Q{rG$d
zba3RIG_Fn{`xiP%zG|Z^0JWWbN$uZ9?O!ebQ`E10*|HoU`y%{sxzwphlR7(yKSJZD
zj~@2!Cw@E#qCZu=rF{b>5D)VKEFb$$*?vr9zX2Ky+KY$Cz^Une1ml<q-cYJKDecKz
zISjIBuOGJ;P&u2ZoYnmHCw*U6nXj27^aNbUP{)T493gBY{_T#EH&sag2auy@X@2fY
zmi&8Uzn8`V`y|Ozvr|f-AEUm#v*b6D{UK_H78*D&qYk+b`Zcz<stD;1Tt-E34W|6!
z14sz-qr^t}fa=}RL=G6QN?Q3D<{_w~kPc)=gi@+^JJtIn;y(aA^wUB0MeZmACQ4pu
zkm<f6v!RR~PyTF|{A$u^Am012<k!iKG7ntAdbfFHJ*x9VH~DWFFAXoFaqn5uank*`
zIzBhI$aK9l0IT&op2iK|wNgj*e`i-|@7yOHPoM)Gzzxi=Ws~I9_8-wj@~(F!uj<e5
zL;Oj}L$eX#I%+@Oi)6Y(<zZza+1tooy>40(aNP_>g*sUdRnTrQ%%J-e^WU-nW=mT6
z4^$BP>F*$U8eWyZXgs&kL-0guD49_2Xz!)@r%Xjr7SnyMGoA`KQYLU6WDxD0e@gy2
zI$Qu95ZjNF#yz^sD%}T4dpq5~kh$_0WM8CPAPeAk^8aCKZ&kW+W*5yHEi<J(KHtZ1
zu{xgQN*;<)gpb=uefKt*BD@MH!bBQRTzh0r)$!yt7>Cgx^J!^6P|7LSfj+iFGp(CJ
za}wdTWXb#YYjxVveOARv>EJz7ZqFKA@9w02RCT>f@Of%)RWSb-*AaZ-0fO4z)>@^X
ze%-ui8qJ^G^n%ib;w*Rvgv!T7<*Z(hJ{&>$&6D~xO;K*A`J3gm^ha&a8?L2vAJOu;
z)Sk^W9==KGnx{#7Z);fqw5*_<?JarNZ&GK2+$cqtN#2$!32NrbP2e!jODj4^9kpLa
zQM+=smi~W1>9$CfI-b$89&@RJk93#3nO<~(<}Jb-&>pbfeHUo!>k!=UWBGe&UGZwN
zfA1=3@0_bGhwHAByzP6*tMzpUjL)cFLGvycE=2eeGJ?Ekj!btPmBSl7CGWzQZ6I7s
zd?xsh?XZH{t-2mNHbL5pmsY@jwv<zf5+(1W@%C<^2JZ~fe6oV(le>sNP5sVK{Z1{P
z`)Qu(+$+nokUI8fP%r4e{R&wx>UiFa+MDTi={UZZguzYYu!UY4+E0FZq1|Hrny=K>
z?{;d3E^3F+%tWZZjq>$^^rxnk<eSs@Z`vY-X?mdyHW-)MQor+4f03yx%4*QVe0h_k
z<J29MmjEK~>L7WwUuVIE3H^7{4I&IvB8=`R?bVmY&PsQcrZBLhP6fWP3<0t!!eXHV
z<BV>7?-uYM>)p4#v#?u3_Wv0vb$oQckWc($)IKXprJo-VKgBHV%`|V9YKrpgXvzEU
zlK!ldrz!ntymicw#HXY)g2p9Z6KSuu^HwzeRM0pB-9dyr(SO@19`Zk$_;qx>^U(Ef
z38m{8NBMeE>L<yK;+#PI9!b!0h7xmw<X!Yq+D_8Hp2|llmiA?&-<;aNh1x$1J0c`d
zy(`0|q1tYnfWuf{PKV@+s2&Tc+*}K#emd!YOa0eP>xydm>`0b6-tVRT8&F;t7LSm;
z>2}Gh>#Mm`&VDNA1=3uJ1T$<8E-Fv;dQ=7N3frOEEL*Z#&L300e7g4hM~Y0>|EEkB
znwbd8sUPXbja-|w*Wce+rb@ozq%4@7R1R0c{0+-Zyxa=PR^o3QBYE30neLC&!K~Ly
z-lHqG6*NClXk4OVQ+Ckx#ea#+ueu)c9Nl-gr^y6&%WNsn+oisKd{{R`^7cv69~d@8
zD1m>`f6En8XE@dSBJfw_D@O9r{6u&IjwA1$E&0pkMu{bVJhw@Lnvt@E%1xp9hI(Cl
zNlljquu9S&29BY=|842#L@KxaEhS%Zv($&~Cc*<_$)47MRex%3A%3RJ(07!tlhh6s
zr_?{8^8IDIve5XZwyOjhpPi><x^%m!d_ndmvhP6t;D<Ble-*y?3t<TH^)z0&=z6N&
z2Th`U>DTQmVPb<iUK+QhxA6WhbY$f1G=HFKP?}LcvQt08dd2X~Q1WNDR8Ys0Pv98p
zm}wmwE)yc$Naf~n$bxx^%4ZvmWA;B~zUcN(`EsJv@kdF+hvi}A#V(Ss_&^fsdR?zJ
zlrCKt7E-!>Z<4(JKIQs2Sub{7y>x-{!STUF^HrMuC@Tj^9S5!F<dFU+qsgB|vK-Xw
z=EXfFuTX`IBKtE_Byay+=1aYQ*#!w<x!I;lo#!aOkJGqq+N&++0fVKz17EO(@C6JN
z7%HgV_4hRgFO~NG2~zlbX|8mk`G9$m^yg)o_!e{}{e99NWJRc^@!XXu`7^{9Qag0h
zbyFR;M^iiT-X#63paG~I&F5VmC2uAjKebPvu6=$sMEX-f_iMM3{ix28w{4RCSY<Yp
z6Y;VhJ#>Gej`Np7{}S!|jI>wV+d-NSn|1968iTO+(RE*aKKm54AGfZbI>>*I&i~Cc
z?&<F%zE!Uyf5>w9oAUK3IE;Rpj!Hk@kQ*hM+M%bNw4X@)8S1|kG)}7H;bm|mfI1!;
z-_+}Jo8gjojFCDssNDX9ibMM<dOk(Nq;iu%eN{olp}m93X9oHI8r8ddtJHr+Zj|v<
zkN%dC!ZM5eA5ZPaN$m%ks|cs5f0>s^fi|S`6Epx!*Zz=nyq+4U3pyI|&Uw=C57NnM
zCwcELlD~%dmq@2-lhio?<%J=Z`km_&Y2T3wVgl)VN+nOPBPczfz%gAfJqMxgq`XW0
z(M9DC(?SuJ_oe%Nx<6I>S9`S{Q)I!^kbNPwX9vAcqK->(G!Iv<llIR`x0D+4-}Z$h
zXj-6L7$eiQL`Yukr+>vt-b4IMN_QHJD>xoHX#KF7>Z=2_XGfkS)Oo^{RKFE8A5h0P
zh1zWuwOeviISb7|w4WlWvy#&Nk>;iDI?1c|FPpEE_7-|B`8L^qW-xC204Jh;RV$g|
zMzZfSLh^oEcO|!!lf#IwllE7W{j#2tchY^OT3=q6_@TajTyoz+_L;I>&6gXcGqoQR
zwVzi>e*pDIuU)1K%~FKcJ*19{?z`3cN}=*J)AdO0r;82R+hl6zz6H{e8)f%XI>Wpd
z{rAzhr#?44ZkN1?`U^~lMEJFz<UMp>sor<2rgq|@^+vkwS8k$u(T|6pfIX(`*NuCh
zx0bvzLe{&QugiK<yVcz{T}szAmr3RepVwj-09({|b&@*UBn>YpOWsx?4cCHy7*>yy
zyk(s%|84R#<%RB&_tSF|=;k7PMdjl>BJGQbKiNszduT$Zj)%=gOWwIa+EaH`?xA`&
zQ-6VGCc-x~pEv8CTg|0(?e9ySjdG*xru!o|wOh!h2)EI=RMk%AcPka>{xQ-YGp(}~
zk-d+`dHs8F&CQfAw=9@MvhPIWn3w8B?H9YD<A{E-K-&A^J_<vLmCAwE-{wnN*=v@(
zm#!mfJAa7A0SAo(YWd$nyq$P8-3#DEOxH>4>d<UNh=J=1u1EQ)VXFG8Xxy{WxaT1I
z4qeIr52gQa(}4RN^+z*3?@;~u@=9rMqHzplMYx8_!8u>*sD8TPDEetGmONaZM0ktx
z<<jNrJ}7XJuS8kluaW&&I30OArK|Rr_h>%jrwb;?icm-WT|W+=>_z&t4xsw^0u-Pa
z|G(-i&WCA_2>*c#D5hIQ?LoEA0{4WU*UEgU<NtK(7Y@2$4kkxCjhFWNdBPfK&v-v>
zNs<E<baN3dyNT@0vK-WPKU|i_bj1geU<sE|5pFaXC*SHO?M-xFthR?m7f8N>=5H{(
zi?D&_Y5MWdKUmu9*YgWgC2yH3``vNUxq`0aK3b=uW~IDF?XZH{q3X}Vj#5XViK<%8
z+o)bDs9qXmaVx{ROM3^6SF<VI>&Sooc=ZyEKmN(GJXQT2be(d}m;6c6Sq$SPmb3o;
zF_+5OLf4}%WIqIU(B4hYchq&fWzd|EcfBngSJOQ-MDm_NGF^53HgOW!(|dH#JVfZ)
zNAi}FTAehD<ju5Rs_K6T<$&$Ef}R(<NauSfPxQy~o77RSccvuC+i0A>j_kiq!{?+|
zD9%4*zaUz9j@nO^uKoN3%^3BSZc<;Zm&p?)U-g&F?^m+9E4^Av-k&7x)qXvY#veD$
z2UMNz9i+W}Jo!#7e;UWsayUujm44h9Nb^tIB$=*kz3_e!m4k`yPt|hRJC5>26Z}q5
zvLp`B{ZSR&A6-EOw2;cd(N(Ko105X8$3*w1d&u4rL;0e93fUAPWt!w|ZDfI|>sRSC
zjydSM*^%r!Lw%us#bwg|MOoZRHn@Sjtwj2x=IbN!(?@=)<NPNuev0-(_m$D4zmdw#
zdyCZBDmTh+a2)krU1fsM{6sMKlf0SMf7JH&3p8v|4z%tBvLdvm`qhtb_tACSvO?<r
zLJcM55;=Z)=(&$Ne%8Wq)YrdHRYCerDu+3g?l`y~LVIVL%ur9_3))EDMH6Qj7DSjy
z*BviCuT;yeiSVbLQbp^o>bN(9*7wX)rT%m(Hzh&pJ37gHJwf`5&62n4<_T@6J$vqu
z0?(6u5w+W@bgA>LtbWB#?aHPb|9^r8FY2p{)cJyR=282!d@6gykJK>^(fH%10eA%2
z-#bj|ct^=}DOu$+8aMRsR~(~uYoZ4`>iYJFG@hGu^M7m?ZQ(7XI(mLY$to$6WxDP@
z(!p9v_;vMqG)?l)P&t=EMTquB^X;kBf2Z{(|NYW1ffBgHE_us(nXiQ~q+$3O#yv4l
z&^@OZLhZ^%^PD<quCx*31B}D8&IZ*X!h>*qM1S0LU#zYh52SS)|5@peTFxI?rM>t_
z9u(dtoeLnFsN;1?`!|VS0Xo7@UAq5JIk;QM@}X&sGL`1xUb=x+`}<Uy*STn3*IFi{
zoP>6c={o4XNu593F_HYC_tLLX2lTYGnRx~IX+_29g~e%UN?J~SPO*|U12#(9h?FsD
znOTKdGjocHvkFtj49P9X&q_(3o|`3)g+G#(QJRhd>A5-cz;NW8v<ajzBsaaND62?G
zyLD{;+0JnZ$%)DR1{aj%XBPD>%FdZloCs+K55<qj&&(>#g|vrQjgG{pXJ(Eq$(x>4
zsI`dgJu|C#cy7V;^xQF7MMdc|vy#&DGjqXg4o||`mmkf^pJ}xeXBQUSkyuz*P?$J2
zIn8P<%qlJ^%%7B=TauM%AC-~ZI}QHN!$bWh<rL*i&&kaxo|{~pUR(l?Y{Pm#)T&K1
zn6LEY#0jI4El}zc^Y6&X&xC?1D#?Q~OPg6*n&!+ZEGocKES{S-$HKL#(E1e?K)P8X
z=lSV*V3C<_*W`~3hs5eUNG_Q^D=VWorGSp4Oww0~L0+IHGzr89l!ofkP?Of3K(y6T
z1m)@!m7kb{rP(*26s|A>2jif^MY~9}!&$+iJP#2y>AradbFzlzlxAh76bviOlI0i3
ze=tW__h=eExiIx%mC7Iq!l-}#PQ}m2&2iE>VU**F?nrk^U$tE%P%iOk5#CeH8B_>;
zorO91#j-9(seLh6n$|whePq8%C@f5$n`pO#N?K9AIBiB+M)vL8%ecI`cGR1JJ%k^i
zWbC7Ia>NlFW-@Na)6-_8=j3uz)v=7+taM#3r9%cSOy9j!Ym3NTW^ULX(Fcb)?T``7
zeYk^xf(~<#QiQ&wDe7QL(jQSz&<$%u_sJ-5&Q%A7;Bcs(6dQU-lM;<6p(k2HPbAIY
zI21f9s2T1kofs#2aYAmc7%Lm?!gPi;Ry{>DXU$-un)FspMi<<nAF)GhCOTo9gsLto
z&MYW_E7z?l$$`F1#yFOxj0nq48f&$r!3C={eR|FuYb@*+<)mfcWh$+Bt}_d+hBFFu
zrw0p0Ge%nbB^TtD6z3G=C%~9Jw<xEG#@0Z(VJvimV;GAtwH?MPOx1>3>HAL@i*x$J
z+d^R62o<u%!T2#Iy>!&5lsP#Cx#?Iea=;#MFD!uhL*C%T37Oda{~M7wRpfurh)+I$
zkytC_W>R`#PI`Vu){ufZw5QLIJriaI=g3oIQo(Eml+=+qa40r5GYh6-c{%x6$;DaD
zp*e+c85e~+Dl<7YE%3jXx(qHTfWb6B1sZZ9xCW|y(&YdDgO{w}Lfq^(CcU^Yr<58R
zpH#>Lnhv8H>)M=3%i%lFd4kD?st1p05(pHUzTpS$_16w|#4vqpKRG;=l)}_6y>RY$
zJ6!cNg%7m=1)P5e&d69HSgk{|@{6*9MUj#c$ez`jJU350EC!t5j4b3>;H;p#`mrXE
zcl`l&imu?<5x9_vIhi#H=FZUSMGL|<6Ve{>%GS6klZDIOdE15=_jsN{E^?Y=8ac`b
zu#nt@I~Wv>%PuGuw@G2@8>)Olj$Mpss;bojqs9>X#NwR1EbXK@kP$;gy;gW9G6LN_
zu%kcG8e5o^o;j?rAa7VsF5JLm(nO36ya8`wt+@s1nd9wqU`Qz_EDD|kcRg%q5xrjC
z2GMw@<~XdEa&A92FE0y*h74-_&|z@;PJKNwFfPH4J_lh%wf*h<PSIpSe@dVbc$GAI
z#(DZ>ABFjpH@VPm#JD`9Ait=%FdZh$+FQo3PH09Jbo<{ZhIcX~U;jojtdswfV$jKO
zLu>f<!x|3`zWvhV|9p&!S1<5`V=aM;CLho=bI(AwV*BWa_nC$1nK@bN-IChatyWfz
za9bQQ=4cGU+%;&9t2xij9g&|7SM)hXLq@E;aZVXO1I!8vbBg5L>iqIDxuEcNSma2~
zDwa*Lk#;E>DxW~`qGg4ZQIO{>Db9jf9WE$jWe&-PIY-g$a}7qkSepI^FP_uI`p^I7
zkESR?uIYhxVC@eJRwcPGeJjeMh9KPJ_Xh0hti3m|_J{j~FgFJ5ZiAg0WP`0+T-JlP
zA%+J6r3)vBwKQi*VOH9loWkOgbj>Yuz_4#&7BpIeBDdmF7_WKlygMiYW@B%aB7s?X
z;Hb4P93Bb|sCPMVEeYEstn!{88U);z73X9X8M!6z9Rv5(VveC{#=#D34QDGSU15Eo
zPBH@S@pIC1&(S2UeZbKf1%-JN((|0qI}H4djU}_8g|JdNq@Ylpd24(QmY1>1@?sj+
zadj97c8_y*3}puNKh2VAV9_O0F1iG#ZJHyE9Blkh4&F;>*Lh+sVqs_qEL;@k%*-wx
zTTq;nfkW5~Xa)JojG~P6{29uOjNF2vES~Yu>4{EQZxEQi1cu?>cNFFn!x~*NEV<zf
zXOx`cgc-lmX0X*OEH%wil{JhII1@%ESe~Kh2>%*Cr(eKf7%!yHacLvQr41V~I^aF5
zW)$XP!3kSUi8X$Djw~SPt2pf}n30)2m!I7q`#3apnUGR6yAW4Q0@Hf2><9Pkune7(
z34I;r>)6YzX<`4LKvR>(6+#8Vofq3Lqac3{3>fT^lTwfjQ%2~JA?0b;)Ld^1of{ZS
z<O;Ip{E)RNtwwBL(h%#IqqF8@<!Vm11o=emnPOl}&dGD;!X3GI3LxC&j}1cn*OZw4
z5k$Or0x<#?o<fwMLCTr;c|3rKwd6xh!{dV@ShEODIH5{2;gJLUpQB_t3yKS8q$x$T
zWlbTt%GEcxBFk=OgU9i*7HC01I^;4OsGXr9x7LBD5h1sv?1;hIs=;b$(2;O+v*21U
z4luaLAMPO%v$Epyy0j^RUcs{*`V1*n)Zpaw!u&w%<;~$dDv_;W1~VcGI&fuX&fx`q
zL~=o4F<Yx=RU)1W;A1palfeoL91U3|2^<MoL*Pd=Wf64Bs9N}`Ayeky>AcdyW!VDH
z7vVWj5sVme(LIb2u0e|Aw9E4Fq)B@>N-dQ&?hq3!`jk1AwEWmUX@Q~(u~M6Sz!%Mm
zPoK=3^qImHq0}cXjV*rirJ<mqfkD5VqM@jTGY_HFx1W&~H#Jg&VwwPtJYlGZ@g`UW
ziJ4-CClZLW7$(s8*^Q(*qyVljjiqT=K>Z-iM$R@!voBr|i{yNY<r2oz9Jr^>gnu&S
z>J+;K1?t^ujWsHP|5n$)O?U!bn4Vvha&BkU+Q-Kh|1ZRX4V$+{e8QemAle&F_(gND
z#zNf$W?+q2bGO(6XMARfGdCv#pIryqa35_ZjIEhwY_c=GP+d{wO+8GnFtjz)YrJKL
z=@;508b7_UR2ymAGF45_2uZb((~VN4HXQ5-YE8#$Mixy{;m$GA*EH(TRIC;lR)eFa
zCK=;Xjn4RQJEUyd+K#Sq<-Zj+a#WKw{Ya=8m(=Reb#;w*|4NOy)R%W6xDcG9A2nv)
zm}=wQAxv)yNvLs!bEMr!rwG%h!lm7aLL+I1>m?#VZO_st81A%2Qqk1?Is2DBG2^ov
zN%Ft#4B>j2-l6|iyAgLadHiqsn%>8MCCD6$g)3X2$LTMG+4;Jmy%AfSj%qKOjY$VD
z*CEqic{?WRrSa*l?!HYsyPbh=QU$K&y<xf(c7o6+U>E+lIvEd{Z1Y)$I?)cB0^MGQ
z=0~HT&x>X#4LT`QG?_6l4CY2-pk{|X&D9(aOfWSiuQ3V9toEo;?S-=nI;n5Xjpf+b
z)>to0zV$NUv#z%a$$f}{Q2`hjvaZOVxyRv)ocZ|bnSOA<B`{bs!Ta2l?0jqs!I{dx
zR$%99DyYwl5_pL!(0#1EjptoKQzoX*Y|u!Kw^}`8YbMFNiz=6wm6zef6~%C>;ZA_1
zAy!)I3l8v(h<@Y?9nH9$&YEko8wG1@;JyM_S(=U2SQvY*%)AY(n~GL|zV_41Sh=qn
zTLV4uZUD@+#9FLaP7(#$U)#8t1ED7{Wlc-cBrjC8t{NLNMSTM~muRE*jp&AED@-})
z#Y0bGDe#&IokV5M%*xBdmvcli+9_D$XBswS%{<_oQ!k!~@tH>X5zjARfWqZ(kppWV
z!)6$G)+5%+gPl-Q{~DukipJK=M|}mIQ?=0(n6PF-b6z8~=47@w5ZY$KG&D_SL(eeM
z4zAY;P3^*-Ml)#1wDg4$GzueUY#h86Ymz+o@=asrK~a6e=hi*P89`~@C-jTZns?vS
zNk!<~kX-~+BRTK~XNaLD%maSVXg<NO_39k6u|yi3VX8~&ykD}pewZs0e_2q|FT^}b
zGrwd)crchxO;xL(a&Q+|kI`LnWn0v&(P`9l=0<7a>w@+ic$}zxd%&vE0?*FJ3bk-@
z)@T;bWfIchG-azcrjQmF)=J#H1zKD<6J9K=mel5Sj%281HkM4Ijn3L91KtKK&cel9
z@q7{In?aZK9ae3lfgFVx(Bryb3({NYT1A*fbdEC`^)7rP(JLCZrVzuh4JyP68)rzf
z(_4i!E4@M3x?nFNg*;rMZm#)U!J3vfColZ>3c(kU!Yi@keEb!u1m988+~SL4{5&>l
zhMcEA#?I69b^VE<2ickWLe!re_BcCV(<SvMhaP0FXX_VD#L4V<NExzYp;=-F!?_%6
z^!y#eaGywOUVP)91BREN9?hS0@)R&r1}Ews%xluM_7!hT246BbkI&vT0<ji+0-qpW
zdrT<^ya1_rvK;0F)i9+%p89W-HTvdGlN8NmwId}jv~UmAH3}g^0Y6*+E`;Vxs4n9&
zz+ug>z6_qe>(xUZz-tV`y$!E1_!nP)Y0k4Y_JNmyrTUo{-1x8#&5KgspY_KAExZ3X
zCw?+hq_3C&uWRfoFFsv^N1<6W(o1r6Z(FK1`fFx@H`2tSNGX_*1)q|@XHjC!BE(TF
z=ahmm_95X9$BV<m(u)jyvi671PT&<%?4bA|cYX%Fog27rt6qj{?CLRtCZ!&RjEA(Q
zz_BoIN2|xeHZuJyQKF{c1HY`oLQRtovD1}u$g4T}>IpSC4+VSRb-hNNfew1Uma*bZ
zuh8$w<!g86N(J8%`|r+;WgZ%+h|k^6<v}o?|Gi3>_7kplLK<0!(RsC~aBob9C|UdB
zTT}3LEB#1-?p0<hhy;EcU^G~2ub=wHX_c>qk=DO|w+|i&!~5W{0t?+;{{eQW>+$fk
zRQ!s;kb=xC!!+pybGRoc#b143SATd_K0R~Xcv$z&gV6(rOI&xFkRxAShJQ8I@TI)8
zPA^W^+1c4=hx|LA;D?BT`-}cX1-WywAkpbCMrG%v7v7FjZJcX|zJH1h&C1`@0hcD=
z3ry<3l9&VUnX2D~1a`RBfp@jV9fA<#pNDW$_9{9K*z9FcJdM8`%FhZUAiw^=FP=gt
z<B+>^eF_yAr<ohLA`5Q7`<>@{AwT2^%Kuc%+A^Mih1@?){?D^+4VwtBNm1VaTLlJ-
z18eX+EASk3A)BT8d04oTKgWj<L8thqWb~@Es;f5npz-<DQlkZUzE#t=uq0p8$)IEZ
zKd22D4$njP98QKQa4c{2yOhv!3;St^X0{$I<8Xt1|6%aM8}Ma^Y!<U-@kSgx^Bf<w
zFvG?_wh0>I_&jd<Ecjg+7%#P7<b*X6Sqf7+)>!dSfTpzC31Uzv+)YaGsL|ApiYD9z
zHF$F4N3HN)z4$;(`w>o<jKuaWm|hIuL?)=;zVJ_$#P^=!!EE4@HE}s)PZxt8SuNAy
zM@Gc8WC9j$Xxqkb;jyB_xN**twF{Rl)wsY%hC|^hm@IzJ2>Yw}bXNUnc0!0V1`W;%
zmzfeLTz|zC70(J;f<STcLH~J4sGjI9-7EumAE720_TyiJB0J}I!)oTk`zb!wg5}7p
zboJMihULP`&7z>#1WOY9f??o}H9oyCOMY-0xZ}a^9kC`1PKk|$U+a)xYLCvzgCB+|
z&B)5igx~Lhv&4h{z%5y@xX$Mrv!Jy#KDhK6c>5<Y$FLRl3w)CXx2h>wFt>zh5d06N
z!afMVO!ZI8%$$%pL9-SROg9!sPq@2;uSDl$B^Bfq6vHQ4qqFj77H3nD7?+B*Z;2C{
zU)HenLi&CgKPZFOq_IiMU%Ch#k3&D7g^pOlexkyl5uE2&x51n>{@Gs8?)<*n<MyH(
z1co7)jmj@n&$H*A`xiPwgwz>w_@c2|sV(3SegrEGD?$7Zb1+m4`M}Abt-~jV+3C5t
z6JQlXen5=h%z=P9u!eU6e|AeW0)s`ikUWK#QGZg4NogkW{4Dj#j1z9Hi`WR!WzAAi
zVshU!_+OLjfCO{`@oOz=CgCgk(DOrXby=g(HBZg8i51@Y7DKq75MeUB7=li~wKO|D
ze<m%ICPS9QoHuO6hjvWpFZdH(BjDF#<R|0uw>U!1);vxNCNAgm{I^bUE^9FAz@!ZC
z<ltxj=xb~ERf%BBZRA8gWv1U0FrG5gVh&r)4!Wzk6<3=9+$CschAOY0dk1CoXRsr%
zHY;lT+_#M3md2P+n8mT+QT-YvJED8k7d*lXk==KkPcLV`aKeiJJkGTC5j9R<TnB1f
zbEa`k(mffQqMX|{_>&Xow8eTdo;~5|avr7eHhdop@2MP!|HbKA=kYS$4?d~ZjsR?(
zJ6N~6MQ(neNPV{{Fj-OCH=q3nro8GwTu2z2la~^{Yn<cOK);@<iUO~_l?oiN8XX9;
z{>M%VS@u(pQ3qjzKw#BHGX?}$eU}a#5EHuKLoE0KgUpf)dD*b`59N9L2x#z<R)^rg
zL0wq}Y9e6pZ*PG@uYq=$m)W5$g>~+{re>jDH8!7?4IfhrJug<Bx7c&xwr&Dmbl5}|
zyIgSM8hSRn4@<#IhZt-5>|c8y7AQk&qqkwyQSpb`0{@5JlF>9GqyphVCtlWul)!I4
zz|X<JC@0n?)rlC0i#wh0g>Qu!RInAn0&Z5u?W0F%pWy{_(NCRkox?CTRzBMqTaZ65
ztFQoEn*-mV7iNXtE#*wWMO6HmD&y&5XpYr7G?;tJ@@zykv~a_h&iUM?$i%`owsYL8
z;HgmM;$H)Sb4-iH=&YS^hRuof{BGQhmVD1glU*idk-)#<&pYQG{W;D*&naU_-_*<@
zgN?dxCj2OjdZ!P+5@axc4UseMRp+{&2z`JS^roNu@ITlmX+p;bnMTf!jn#H&*$=Tt
zqyHh(>hKlRTccrcYDjq))%U-?Ni!0%_74m{|5dim^F}2kV_^#^^v*-JTAa6O?mvQ6
z+vx3w?(EP@Pi(CC)s^v1T*t#7mI?e$i}puv0_nx}rxWnDu*jL7A%2eLTxY>VBxF_-
zkkr2y5;$-^6YVf#m41#K&<mZ=%JPAK&v{O87JJ_3$iAWd{RO<d8;yqslk+e+4BK|j
zkvr+OX5#&Cl?}=u%#=R7GMnWG>LgLg&6y6bJ7ipUU9X~oUi}c}1@>lUWTX`#vLc@$
z`LrxtD2B(m)8Utk#g9}fX<7N1XhA=;k9NI#S(E|)e+<g-(IW;ANsH|j+pDiKe8`YA
z%*l|{)P%t!(yYCD%fqRuK=%olT7$d6Jv0zO{`H8aN>lj5w)meC__rDCHG}^yfWOq$
z6t?ns6TvoBE>xnF=CB{}fB*eo3H)CP{9g(D4<+!N_**=RVusPDq<v0v1;&;a8~`BT
z`0^?{Ztpr5{?g5b${P-G7>+hkR?~3{P@lhgJN!kLX39Ho9R8GyQr!JP_)9uXmF<8r
zM8KH!F4-eqwQ@E5#hm8K9*4AVmy-j3Y3TyxTf)+xNa2sry8``dq8x(b7^*((EVlIL
zdK7=O`~x)AI^J}d<UKmxLVS&mw-fKx@lN7vb-bH+pN_90zD~z`iTCSxKk*GZ-lYEF
zb^5zPYJM%mN9lMw@g^N#L4351_YiN^@jl{Xbi6|4Y|-&%;^TCD74b>*cTrXU*L)`H
z&#vREc1b=}$H#ptd54a#BR-q{4xp+ZT`TRKI=+VZQXQYVPujb5yz@)R`wq#isvp30
zT7E^>NYO6MI{pdb<8=Hh#M^azO%JJ)t>d?ly-UY`Mf@5ae}ecb9p6CuH9FpWolMuK
z<L$&Z==i8^(%w|xS<@d&$v#HMR}yd2@ioM!>i7<%@6_=PWM8J^qkBq!Ds+4j@zpxM
zlz6X>uOPlo$JY?A9M<O7PkgkFx03%B9e*?NNjg6Idg;GI$J>Z6)$!THyLJ2;;wyE$
zhj@>UuOq%z$6I<yfBZT=mH4P5+H#mm`7-PHS;WWb_%)<s*YP#PXX|+72I-GW$6JVB
zqvNxQuhQ`q#MkI}7y0Sa@pWY1pyQ)0(jU`NZ8;<nAEV<}k-kmGzfOFrj^9GOQ^&hV
zzf8w}PWBZ#zMAZ-b-a&wua1wh%6!%7_`{^H{G!dTjqIa!{Dm=6$D-p)$v#QPR}$|q
z;7fJ9m+aj-UWt|IR_b^&@g5zYN_?%3cN6c|@m}Jij%mvws<%wntmE5IImGGsp2XXA
zd@AW=>-aL_T{?aU>8#Q59<r~}@hN0qqvJ<kCH?mq@C`b?f%HwkYRe(IkJO1V;B7iS
ziR@Dic&Cncl6{$uuOPlc$LCXit95)C@m?M8C7n7Q|0LNfer<j?6CbVP{iI{j@s_@_
ze3Ep$gLsFIcN1T#<7<g`>v*;ORO)zjJn<OtwL1Pw%9mfqoBK)sqmFCK;a9RZ>-ebd
z(mqbdr;?6c$9Ew6Y#m=l_AVXoA%2aH_Y+^G<74_ue`<7mJf-W?@so*f(DA9HV>+QN
z2N&@%I=+&4n~wJqpQ__canetxj!z=KOvjfIU!mixh_BZ1KH|MPJ|<qKTc_il#4EpP
z^IJ`Pw2p5e-eSNf>G+rdGF^v`cMxBy<12}G>-b`-uSy+XNA?~azmn{0b^LS0`*r;L
z#7F(EEeG>J>AzXW+lY_T@ukGub$kW!**d<K(sk+hKZswW<D+^=f2wqRd*W+!d@ZHx
z)A7oUQl~-3#}RKjsVxU5@i98Sig=rj?@s=t>i7n-cj|b{An8w;j?X5(LdUDuHOF@8
zC$0+M-)6?O1q>bozG8oPd*?Ud`D~`sUM@WdeJA7bZ*Hq0rY5Mv`6`w!=gXK*U%7}Z
zq}+^`iZX>YjK750S1^7Q^QV&W*D}6}@$jeF1LbC7{&U{X{O5c%)471@*D(G{=BJnO
zo7F;tZ6zyz&et*fYne_d^ON({27KJd!Fu8Lr3Sp8rTYu>vw`u|j8{Gi>cgKB4}>TK
z-ekZ>8}McWKE{Bz81Qihyv=}5GT`k7e5wKOFyONdc&7nxWA)3+Imv*J-VwfD%m#d|
z0q-;5>kN3m0pDQ2D?7vc8D+qm4ESgR-fY0f81NPYKF)x*8SqI4yt@8}Lk6!$+o$2n
zp}~MxJ_~OjWx$&Z_-F&(Y{17D@D>9;&VaWW@JR-|-GEOu;2j2hwgIoMv+C=`TpPZe
zYYljx0bggp`wjR8177(mygyL}yvcx%HsH+$e2f8aG2r72c$)#AWWblP@vs-W&bk?&
z8@TEy${NNGWPAnVPcq)auD_hGWcKt3L7Y{?>^Wb>>}~9N?Pd0yuV(h|Gk<ECJ?A~l
z{%&UPWA>b{VfHIoxz#ay&U=~tH%#Bo>^bi@;A@%AUZ&H)>^bja_IEIQ<=bF8=X@Qr
z-^}tA#q2rnXZ91AzKPj$zJb|yXZ0A(>^ZNn>+cZm594FG4&#S2e=Lk&%=kFQ-@<qY
z<1YhVo&Q%D@IC|H%;tey$6>%%81UIlKb4h3CF3VCzK-!T8Q;M8{;b@b?0U-ivQL8T
zg!67T&U3zUS9tp<maiLGy441}iOpxYy~lvJu<JOt_ZjdeHvV&aj{zUU{OrieGn?^u
zF}{-V7qR@-Fun`3ui6tVH_q359-g=F3(seN8J_pCbmu~Qj;6L=r{imguR0)g;NiQ1
z_G*Y><$n?LKZ%ty=VMrTa^AvpzEH(sYiBx~w=f;f*D#&k%pdi6FH)RWpX=&*FVlIP
z>G+vHocA(+IB#eD=t<B~%Q;oYTPTf6R?cXzhEis~PbFciJ`YDz&byfXI%Z$N`YGqj
zn0+hOJ}a5MhW7^B73bYdXCq6uis^8E4YR+G<+qyIYj~C}=PQ^_4Ab#29SzTPIA6(h
z;I)%Ls9`!9p6PJDis{_M%G1kqG(6Mcd^OX#mF26J>1cSS!+8(WxtaOnV>%k1>2SV=
z>9k}1)G-|m&vZENWje7;$Io;$Jk#NPEz{Y_bQ+kBhG#mQ_c5K3fm(+hcKy=uyMyhY
z^L0$;W9Cm3)6wuuhx2}>b34;9F&z!hbU5F@bUtG`(M(6fGab&m*>&H~%13?g2BkT_
zhS`6>?5kP2oUdT^-B|ndFni8NvFjt}E1AxVEWb5Ohx1j;zJ%F(nLX#DS-PCBW;!jI
zPA${nyqW26-otbfnU0U?a6X3VaK47=^k(H>$8<PvVLF`mGM#<QA3xLKd>qr^d@a+l
zFn=1D4(Dx5hx0zB^DXm7Vf~u(Nlb_Hbxda$(}`j_oVPO_&ik28Kb9{O)8Tw7)8Tvr
z(>cKWiDo*ScQ75!D{Ox9GV{mGbU2^QbT}WybRJ<kF-(W^PNu_o6Vs_>`LZw_&X+PB
z&fA#%M$l2`QQ11al+tkOcy%5d$L3e4qXsv#@2--twXyM#^L_(<4b$1f^eY(u1mmk1
z-<|Phrf*_=4CCKnekL(Kj@jE8e=*}z8NY|=I2iu~;~N<N3)4|peZ9?i6XQFx@!ZAu
zHej#TW0{Uu=fgHuUuds}YG&_ey!t*SQk?fN`}df=gWWfAzJ}Rf%Ive5J?Epo4$cQS
z?`1lXOvlM|IA6=`-&HdU+fruF`Dm6d=Y33PIZM~YbU1HjI-IX#I`j#O@T!dIa6X3V
zaNf^!mN0+ZOo#Ipro;ILrn8XgtYJEwk7GKVSJ-{@7*@^|Oo#I}ro;Itrt=`vsbo5w
zPhvWpH!+<~EMHYjhx2x(!}(~Y^C|PEn(1&pmFaNa%yhnI{&<)U=N(Lk^D#_k02>Et
zm=5Q&nGWYIOy>cXFE7*Kyp!p0K91=;!}3+jbU0tibU5#2IzyO0>U-ZP&G}kp-;0$)
zDO;c6ypP$x#`Imxp7Un*+??}uOy?BKZyD3!yr0=W#?o~&d(K-}x}0xdI-jz1*DxK<
z$1xqwD|>_E>Q<&x!E`upV>+CVVmfPCzABjx=aZNY=S@s!HPfkLI-IvN9nMEHod}k%
zYNo^aRHnmuGt+sR`Qu?aoOduC&c`sFW6Yl#ro;Jcro(v)(}`#Pc$p68olJ-GaZJa<
zbZVIn=S!Im=WR@<FVpcc9nQO$4(F4Y&UB_z$8<Pf#&kGuXFA_89Y53IyqoE8-obRP
z108i9m968|c~mN!N1?qM+{}KwO2W4Ci(tRw{2FE-*(7*E1>^U!_EX9D=8Ugm{A9*g
zGyW6Cdl>Iwd=2CKFy6~}6Z5l{@$;CykMV<p*;48l{|B@8Gd`2?4UGSs@d_(XAJdOw
z{A9+P82>EOiDvu~W?#kl9gMGL{C$k~F#Z*$U&DC#_%0B<j6WS<6{VK(-!tCF_~#j4
z$9Om6{fz&UrQ5*xgUnuG*XvZqM=|~aR_`Xp7cu>4#?NCqX2!qB_!!0)G2X)XznQ*+
zJ!k(Fc(tFF>3Aod<JR$N-YQuCMIAL%G5Z3QgstCzk7DBqx36aQJ(-S&@mH{PYZyO`
z@m|IcWPB~-4>SD+#`k3Q3Y&NR&FoE#AI9uWEMGeqAI<nj7$3v<LyWIv{2<2HFuomY
zS6;^VVt&>#-puTMjE`e{9phWDbQ>7|1LLFE^Zh?rx-J9W#P~~?PBi1cWjbcYk6?TZ
z<2NuJ3*)b2`HExwCZ=O!{5?!3iSY@{K9%t=GkXW)doVtm@v|6T%J_$weih^AGW%-A
zk7K-r^~YIECyDV*8E<ENB;y^7|AX<_jQ@_M>tuWjW?#zq{fu`p{uRcTG5%u4yBXh#
z@oN}=BlD-4@uwN@VSFLesbTyTjQ29WE7PfE{LjqZ$M|a*U&r|QOsDQpa6Z$4*+=^%
ze<5&hG2YC0E8}Are-q;^jCU|Tj`1UyKQ_j9WA;gm=l#^q_#~#2%J?4{?_hi(<FgrG
z!FVU*<Cs6Cj9<X)U5w}bx{UGHvvI)9cpuYQ!}us>U%~it=1(Q#KV*Cr<DX{w)r|j%
z@gBy%&iER}*D`%C<G*C~wT$n_cpu|kOs9_V+nK$e@vIn>2FCOE!IbZV?RGZPk7E3K
zrf*_=e`X)e_??V5Gd_>$#4w(ZZx+VG4>|-w9OEBgIyS~nWqcCjlL9L69th*{9TYXB
zGXAVe!q&m~u8hxS`~b!~86U&=QpPu772{(331(l$cst|WjK{G?4Qm+x1}ldO#`j?9
zRx*B(DgxUo#y`&ZYQ`rr-otnc)30HC7sh)T{~pt+W&Co+`xqa^_&UZfWW1m8zp-*?
zVElN-D+hz^zZ27mV*D}2n;4(M_-MwbF?}=R-(h?V<L_m>h4HsD{W!+&XZAM6H)r-q
zjQ@o3cE(@E>Lr!&In3U{_*E?3Y{okn?__*GreDhVBTV1L_?FDRjPaSw-p%+&8NY_{
zA2EL_7$3p(D;eK|=~OYkEz_xHd<VvR7=IDdsbTzAjQ2ABYo=4n__rAEWBkWVr;hQL
zvV8d&znIxKFuo<@l^=rbzYo)iVtg*MH!(hs@zIQL#r!ujeiXBhVf=2!TNr;m(}`m|
ze^1QDc>cUBiSgGneLLgp7@x{`K3{b(-p2gNW_&5raWeiOvoB@*!;E(^{!OM+#`tt*
z?`Hhx%zh2ypJjXn<2$hWtz`TSOuvfpTbWKZ<A*YP595Di_BD*>_d#C9KhJb(86VC3
z^fCT&#@8`Ef$@IEH)Hw@jQ27>l^=uc|4zn7G5#LLn;3sT(~oBSbIjh%_$L`3!*~<p
zEsVc~>BljC1+%v?{wKyKG2YE|?2IpC_Nk12joCXGKa%m;jDL#pPR7q>d@18QGv3Ac
zWX6{<{x8P68UF&~*D(GA##b<Y7t3!Y<5QS@72_u{`)bBpS$p#^{wl`TF#a;8?`3=x
z<7*jz4b$;4zL@z_$M_7!`x)P!=`=7tpV=!v1>64@%%3R6zrgHGjBn2LqZ$7y(=jvt
zKTIcv@x7V7h4D+7eH`PrG2X`bOBkQT_yJ7c&iLP$eJbPIvV1uh-;UX5GkzlDos3tX
znc(44#`EVLF2*-uewH!*L&m!q|0_#(4dee|d<Ekh7+=YFKhv*bd<NsI8Q+_gkB9L`
znSKr9k1!oC<5x2KTE^Ei-pBa<Os9_VBbdFP@x2(|!1$XPulyWr|Ccg8it(dax+ccI
z&-iG@&tbfo@t-mM7{)JR_7=uJ$oM$MU(R%FjK7=NCo#S?<L!*!$8=H|U(D<sjK7-I
zdp6^%n2wY21x%-u@q?I-i}Be^r;PE-81H8Mf0+In#xG@j1>>WcP9@_{F}{lN7c)Pr
z882ImJjuiO_DrXS@eeV7yo`T|@wJT4VZ4v=S&Xk^{20dj8UGUVr-AY7n7u;lnYfm4
zC9{uW{0e4oV*FvoM>GC?rekJ&3s#;nj8A6v7RGO3d>rEkG94S^=QH~x#`EXJcE)#O
z{--kjAI3Wv&!1CdGyW=;u9NXynV+SMKg0B0jK7-clrjEYX76VFHOzhu<G*G06^u7C
z`%1=FGW#mVKg8^-8LvKL#KRuO+nG)c<2$l)@G^cPvrk%bG%9IDvu|rVDM`yb#m(xl
zl9QHfi=vy5vwatT)k!(qWhwmI`WhSTAr>P>z3(i9F7pthV!c=3Ld2MIy+`0#h?^p=
z5_me|NW>Ka--@^yVz<B(5aSYIy-VPch_Rd0I|Uw$I0~^t-~ou6Ben}1ix?lR)Y}B^
zff$zv>Ma6yM2xrM^=5(FBgRK4^(KMaBF0<MdPU$Ch+8A}p8=vv6U2DyQ127?6ck-{
z8^m6LPatlK*dy>^#5hH&uM+qmVw}>{R|xzyV!Q;_y9M5Z_%g&Ufp;LrOHaL1;4O%8
zNUe7Wycsb*imA5?{5oPB66<XOzl0cvn0kxAPb0=hDD`H6*CNKQRBsaaLB!Z4>J@>P
zBgQ6Q@Bat$zZkI@u}|Q6h_6NL6}S*_N5me1XCdx{xJuyZh;b;duMqfF#9a`(1)hKy
zmwfA80*^%84Y5<;!H8oJI|LqpxI1FIz_EyNX{p{Oa1X@SA+`wI5phq%W`Wxy#-)mS
zlfZ2e;}oV|5x5278xZ?Xi~2`wLF^Ow)R%y*h`j=zKpczMBk*Cwy%ARld=POT#1#U6
zjkqsjx4?T4_e1Owcn4x!@~?LayajO_Vu!$+5yvC83;a4_TpF*p3H%b`fru>vKaKcC
z#AbolA|8a;B=CcXZ$hjHyc{u3-Rk{+i~2`wL+lfH9^wSVUV#e{4@T?}coyOzh^qvi
zj(8~I3W0A$oQT*h@C3xe5W55(iFi0-r@(^|Cn0tSJOD9HMeFSX$08ny*d}lf#JB`j
zZxOg7;?aoB0=Gvz2C+%twur|fRs?Q=cpPH?DN+B3?TCE>pV|j_JYuiFClF6S>=F1d
zV!YJWR|$L&aSGxJfxkwKw=(r^f%hQBTi|+^z&j96M(h-L3*uD74uLl#z6G&e;MWn~
zir6ObONehnY!Ucr#8VKP1zwAIDq@qs4<eq1SP^(R;xxqmzeN2bb|Cf%JP&a?Vz0o3
zh^Hg=2s{gM2I4A#rz6fpTp{qSh_ev81)hL-24a`MBN5L;>=bw~V!Qy=I|LqpI0vy^
z;8?`75ZeUqf%taB7J)k=&P8k%xIN-L#3q5;BF;yw2;2g30b+lHsDH#x#6E#feF1nj
zVz0m_5Eml$2z(fE5#lO=4<ar`Tp{q+h)WQ=1>S>r4q}(UI}qQ2*eUQ9#HENG0&hk<
z7qMO7*AdS{Y!mn;#5hH(w+Q?+;suD!0<T4UCt{Pp4<cTOSP^(R;=2(0{}lC)*oD|9
z@I1te5PJnKM7$WWN8nkAmmsbZcsk;xh${rX74b5}Zh<Etz8kSi;E{;$LF^QGFyb=A
z4uJ<CE=Oz^I2Q48#5RF@AYOskB5+5<_aZh6+#d0Lh)n{wMSMSEMc@{QA3*H?L)1TF
zH)5Z_r}hF~iP$Uf3B(T~_6U3!@k5BK1U`s(72*nkzefBpVz<D15U)n;5_kvVM-V#&
z-hy}yVu!$+5kHF9F7WG!*CMtF{1W2F5L*O(8u8<Z%>u7Q`~+f?zz-sR60suia>P#|
z_Ma5>kGKM{PvCio*CF-_T!{E-#2$fXA$|sNmB7;xKa03R;9C(thuAIf1jNrHb_qNZ
z@e7EZ0uM%9iP$0V0K_jMwhJ7K_$9<PfqNickJut`N5n59HVfPy@dm^uf!iW}1+gM<
z3&gJ?_Wv&GA8{38pTMU+2mBghufQh|zmC`=@L|MnAg&VlAmTR>R|xzy;*E&i0`Eb*
z39(Dy9f;pT>=bwl;%dYWfj1+58?jyB*AZ_<Y!mn;#P1-s2>dkS{~$ICycY4hh)n`N
zi1<CkionYezmM4eo2Y-p9>hL@=OO+8u~*<i#9I)11fGR>E8;4Frz75mxI*At5r2r-
zE${@y+Y!429*OuP#7=<+Bd$U05O@IMj}hAijzzo!u}$C}h<75k2;33zCy31gw@3Ub
zVw1pa5r2kQ5x527U5NcBMExW7BK8S<Y7gMuh`j=zK)eUBN8rPVKSx|8@Il0T5myNO
zHR3N2y9M5ZcpqYyz&j9siP$Od7R0rP9RhDg{1sxmz^^0z8nI2_mk{qqY!Ucr#NQw`
z3%nNbw}?#wKZy7{#EQVn5g$P8KQ8JYu@A9N;CYC@N9+~25b;689)V{e{sD27z|#@`
zh`2)FTM_?+*e&n`#6Kf;2|N<<A;eCB2P3XS>=1YW;(Ek(fnyOLMr;$f2jU}$EdqB$
zd=#-+;P!}rL2MGZE#hN{6@gnI{uQy`FX|t$AF)s1Q@a5lN9+~&1mY8jJpvy_{2Ssb
zfe#}79dU)gUn4$=*e&oL#D5@m3A_XGpNO4+;a>gYkKtZ@g_2a>?1clJl&v#*wY~-~
zr;4~ghvP2>jz19gczNLX9bu2>1dgYLJw76Ee0bR7-2=yO2z$JF;P~ZXkJs0z`8)ev
zxcq$_IDRPX@fQQfcZE4F*E^P>&3%L31B1;GP;?2^`0v~JPtpoYQhDEPaHKBnS1j;B
zl^>z;rQ^N0>AM_`-~vf~0t^psDDU_{dXI!jZ$+~!0>|$Pd;Bky7xg(a?D4Mx$H#>|
z{&wJa|1ihzYv%kG{T?(OHWQP&Hzbw+l(eLw)g4WyZUyIC;>ob^b7JaQAFl5t-PbH$
z+Q%~c4;q>#m2XM<;PA~!5#FTT4JG2_OAl?wllQ>MN#)zm9>nuqgPOy6N=dh*C4+Xs
zVMwz4r{b1Sifwm6i4KBGN?kT|qp91Q`M&9-MEtgO28KWzhSQQZz-h%@!MSll-&5Rv
zBKRK*G-#dmPOYB|0<EucL1iiRqhLRHx;tTV!lZ<W2`LHXzfVkt#jVn1f3#kTSC18U
zL?xB~v*b)f>-+KInYir7)@Ar0q1?Z_At8QS>j&^H!gBwk34fkmwx{*7;wFg3WE{2b
zAKKkF;)9=>Ec3K}0Eg8Xt*;rXD5LMo?Q9x-UvX#K=<;9dCLKF_c120l=<+}7M&WK^
z6j<M%4jPHpAFcb7RwPCbUJ+>qa_JICW=TUt>!rAgyQJZ4>t&B&!r$S!zrqbC+Ta7+
zx>y)}K8h62Y!lwbS2F5xeI3M>Y%49Vy=`j3l!U1X(-P9Q3V)JTOf;28u1#EawmK4=
zENw9~{?4e@58!nT92*+HZ{eL_5)CHh{)8plBa)VEjlhg8`7m;5yubDRJ)syA%MT@#
zf1ObNT|&eGsnTp{{F%EJz}a^_HMBgXbIZCXU<ew0U$gCJn<xR_rb8OmkF9&&gX}^L
zeN>kOMnhMCd3_#Mj_|K8L!NmroGA<gr_ZL-@yu3W1n$<S2F`<`GeObC)ZYPA>uaE+
zh@y<C?<#TKMN)U~8Rk(NEGX{LCg{lJTkAi71ubefSm3|bErdH<DDY)?xrG9WpBmNr
zew-?fSh6j0+5Xn$L*RtE7vbc9xA=}I6hIavCsMeQ@|+@4z)Bp64uSSy*w&8|DMEXZ
z72HasSG=ljBG-%s%_A>SISqs(kU%8QPdha~O)0gXp^-s;j>7^~(tUTvj4nS`_XG4S
z*td@^|D*0p*thPl&xazaI|8Nw?_)ryeh|<}<y+CmCn2$<@&k2$z?~lW4s6{DppN|D
zR<9{)f0|f+GC66{QM~WM9)+)bt{4tI>ThT~OUD7iHhupv*nz`k+riQJ?KmE^eqb}~
zkIp!TT?msrazqqQQu)x%QFY$|u<pTq(~2QcM>@@b@wB)zEJ!ctY<e5p@kf!XaZ}#%
zk)$PCBI3VT*dyM%5c<<7Q_yfl%a!FVR|6L<_Q>+CeF^2;6S^Nu1b^<w=_K^TBT1k?
z#5B0PT}9%uJ)6~zk_;VX3N&7DLG%TD4+iDHZXJp&`6vP!L<ERocS(#tvv6)g`QgO!
zpP|DfMjVtn%@g9cw7wT#xlD-PzF-Y@mkxFLzlh3w<TPuX)4>-_<Gcclu)B1qpDEh3
z3k<NIFWQEst?BAB++#9+9`V7CO~B8)uC2$rNNUeXE0UwJUk@&C*%4bW7%VIuTK)$X
zWmNa=<^G5|D2b#sD<bb{U3M1EgA<?(p_~VoM|Q-Vm!IgqCE_qRpOmp@MaxGPY^bX`
zD!l*dZ`sxZtq6Op=)=%uF-=UbE*Xqv0p3-Xv;=gO;ub=`?oqf-3OACLorMPs%K@$^
zio&tejH4*D*gxwot(PP;kQTT*L=2!+(424p{RL_*Fo5FJS`47^qSe6wij%$aW7xP}
zfjd?lKs~_J9f!sh@ELzsq#kGCR9<zTV>`UB+38b_Rn0;$s{bAOFm%qPa5AVqCePc(
z&-?$ldl&d9s<VGQ3maH*VHOP<D{5L-8x<(gSXn`vg$>N&2B_Q>FLfzWwQ4Z|l#l?M
zfaACrFI9W7YOAfUwy$`{YBd+aa#I5+a<Kx6GRulwR0Krx|9;PznVsDvfUWKC{rvrW
zBs()_&Ux<7bDr~@GnNUnh++&yBjAvH4GJptT4In_n0x@YgdH0VyOwu$@=bY7_`fth
z#f;OoN6oIl^-g=VQ_%>DaS}SW@!}db(0J#7X^(mXMq1J(J7^(DlfM&S^!+UUCjTNv
z3t9B4663)hHKV!R=WO(8+oPxO1Ot>~0K35mG{_L)abmP&7`?0US+Ga@jL|TE_zmuU
zY7E0~%N||s9O)Kv<s!=-eQERnjI>8nV88%7?a^0g@3co_;MTRT)cEy<Mh;u+xxadi
zfx#{{R_{}_&c<&)mIE7i;|JYjld3hd(`O8JzRNfUorkq0{P_nWr+t*#V%s}}4EeSD
zW{w&-!!!F5gf`W&gYea~AF+evn^JQJrQp{PQc&f}{SK<!j|YM(mX$6+BY5k4c<3A>
zD&Oeid}FTl2CRnQS`X_DTi(?8u)?C{+Fk@$#?3Izln$DOqI%JD9iETUR?Fw3#PjlP
z#+Sg<W>y@nQ$?2iiuQYDpo=a~k*_>sEOEYa4_*OwbG|f%>O>Q~n@B+-ln%HI-T5LR
zOEo*=xv`_Q&rF(nKvRTZmBq$*yo3G=(7z2PGYlCr)yW*}@+(L`H9ljMw$b<pf}+~`
zim)25AXzq&i4)=B5W%iBa-5)$Wf<eteI^bknK(SPUE(0REy*!f;@SqHiSRFX!XJdZ
z2@HIY3JZe^jbGy}@*ln1FLO?xfkBBJ$|r=-veWSyg+c9Ypt2WA0aQi+l~EyWtHdP^
zUjST|8^unrID5xvyT8c7Wiv%RwnPpf?Tqmi?%5zQO_0eClNSRh-{-0NuzZ#Faz5Ir
z(S2$>Z;{@KL1|4T?!d}c{3Ou{>e*_F+mt%vcjzCmOa?6WFa{cDU_Mj=mfuuoTrb-f
zJKJ+ME!ta5Il*pURG(VPxFI2mvMec2%bN+mg5V>tbhf8<%}jTor8;1^<uGr2?f~l>
zYZyztp8lwf{8GQP)f8%>L&;YmOxgUWy_x$7Q$CYb-F-$OeyCcFe;K>42Oy+i3>P8&
z+5!4aSF7-BJ~dulXtYv<VFP(5-`Inzkba3rjZdQI%iRN5Uw#Ddi5ci?`~@?m-Qb5W
z_WO(KjV%xc8m<9=Nw`u>LigjPkoK{W2g}8K`M9?O_h4mRxCMggcM8ZRtLJ$Is1;sB
z5(Hj|p`oKyU7DUn^-Xn7e5G}jljQuDVOWJ#?P|QeO`>94Pa49;8`nr;LZvrfenoQ3
zhw?dWt)W^?e!?F>-~v8`rrt$FwB}Tb|NIj+w<L$g+}MuTf!!o~d?2Xrm=8MaM=^|t
z5dj>vo61jM*sHy7JhTOGBI?D@d-#)ld>=yrqP@@{$ZHB?gV@jSbqL0FpoWk?%TnJ@
z0P!x20;%B`pL0nO`)XfB;?Y8gpU3dfO6$1?UOGJ|qHVYQ&JQqtBz`>{-Y(+={MJ4t
z>hH;(J#xNgxuP?HCkax;_>g|C**G4Ep*Q1L{7r2lf%cF-m_C1@#-|rbr&lcjpna#=
zpllgCc!7LGQ9Yp-@Zue|w*kIZ2urS-3GlBrrvd&seybAVgVG0OL0rC7LcEGGp@jHn
zq%0twh`%PpQ|bt>fR!Q?__F+D<F^7=L5y7_h8*QSa}<^Ft;3(t-U4*c*{6?(dMgXL
zG=kZD(N1%oJH)5EpP<Sy=ZzpMJ{FT%#7ZsIAJ*1aU1zr&?TZb1626;rcB@Te<#x0g
zTC_9mUl9hQT_>f6ai!I+rJC(dE!rvO3m7kEbcQvY{{48V7;NAWPbIP_OME0cB8;>?
zg|rVOf=&Mvmi$fQI#ABGl=~Q3oeh6xxBH!Fr_`JaD{EZrY`Y$9A-M;4WqD!N-EeF%
zvfa;U(N9Go!_MyL%k($)^Yi5M-hAGRW353qHeB2?k-Eot&3W$=+)K_vQ>Q)@?MrTe
zz(IFqJp3A-w<ee3mh?Zb;ia79i}rPPe&<Nns<xjQAv+%w9IKMg_-~7%CK4PiI3|qb
zj;g)G{EkPza)26bg6HBALXH|(rYb`Z8nwiBQy76?qLlY@s{CO+<j*fHS|j1yJ6Q3#
zTlNl7dWPP?bidIR3aqW(5!7q^OpU_@|BBu(fF!^|t_6kl`}|BB71f(`-5XSVq2`>u
z$^>^Ke)q(07y?(IsrsYTh2U?PQ>(p-ulpvj&w4wkZ79<WZNn6$utC+k3R<b1XBzn3
zI%-tyrmCS~{ds>q#=)Dtp23@Gc!yxTcX9ysB=<TK>?z8a#wkj_(wynJdzGH0HX3Wo
zYIl|+8MVte6<(1jbOBcgQ9&5qLE=#caui~iQp|`jAHc7?2K?hq>{TYMK(EWD_dus*
zNOu+TJ{|^@NqO~n(lY%RJQ>R;jV8_wMfF8%oUniKX(x7h#@G0pDg-^)br%3EyC%G4
zAXp=`sooP<TQwEWOEGgE;~!A<c9|aWtEHIC2V)0X%dfz{u8P+qac>tA{TRZAp*J|;
z!~K)^(2H><d0l&zi7TzKX55z^rusK^mZfC&jB%ya?Mdho?~GrSjk@R>J)_e}-mcD`
zmH7iH`$hQ2A+O_A(ii`2QDi2P!U~hNmXV8o<A9KU{{Xn1Nc4*^G9>eR=jP;$h!0}^
zKgX1BnQ3d<2-fWs5=<|J-~tIXO7R$fei7JU>L2KqDS*w@pQNQ&scFEgl;rB#oyx-3
z-I<b1z5~DZ<hZ%|qp<#}KcAp#2g9B36Ji<CRtxBYS`vbcvAZAoq!UsJpo=f}0DcOQ
zZXGp2rvJDqZ|aM!4=d41^pREy_N67IHt1a`aA~Cw9UT=U)6$UK<bb4hr=;9ZI9@#f
zBGlv1fT~=Q#($DcMo3Nrq@eS3H>9+)knrZ}cf--fHkQa?bp%Sn^mN;R>7_|J3B8uE
za4pmJYe`7iatql~Z8b^vVyaUPEP!Z^%v9CSsHX*>Jzp%uUxdX!H1)J+XCdj-Q!mwj
znSk?H!if}A)tS~)pPPDnJem=k#u?4j*VJ;89~1{)8>_I!vI?CY@|8yrz3-wUsl44m
zmSf2!+{QDSg%*s^GTnm*$8!{!tE+CxvI^Zd?~qnuJO0utn0AMDVYTRi6hGe9*LSLZ
zg&{gitFSJ^DkwE419)K>{$Z(bPj-RfH%=#n0ua(d7=}C=hCCXEwbo!W?oAI={eXop
zsIM#>Pr@{O+_`8?QN6>ebwAC7|A&MhE{gEekNmi;$e?deIJVTotl+|~gf&p(r{Y(=
zLEftPrF-YXz9}`op^C!9JwH6|{jp@3&qbC2m-W-Vz@;;OwcyD~e3jyW-|n8Iy!%9;
z)MO6dIK4Jm32Q{TwunNz29(FYE$Ev?81Ro~Esti+ri(Iv7P3sqY4`{B(S+$_j0pM`
z0;cgx&{dNS*Lps&#Fg|WNRt4aG7*i1CN(MZlk9Q}{;^$CY%?!f>8Dza>9~MphGCO&
za(Z23xM>^1T5Iw{WX~<lI;>H;*07DzWc<uDO6Q@fF>SAtmmuVE%!g>DjhEhKWSqNr
zyJOD4%}ip>a4guRBExnK8WHBq`IDV57xBs5PdGK!s|M)v$$M2R_us5)(gh>wU93hE
zTBUyi6W}w^XWDhBfrPT)>aKVt-ur8G!I`14=xv72n>qJ<5Vwp+tm}|}lyR@P2>B5W
zv11m1;6DUSffi-K2-%Yik<inW^qZ<N(R{N+MM!Y78h>8QAN8420oo*eaUHtBudDUN
zQ-K10O;+PGCm5&GKOq@)IlH!A<TO)_FP?)xMklkut}c!S>t%xp=*id$Ls?r7WLS)&
z)b3_SfPIqs3a;@VV2=+nqq_BKq8^gnsw%_w8&9ld0x!YFS4ZOhtR@j<*cMJp=80Cw
zW`_uIyB%F6nluA?IIKzjV!Y<654xHF(-Qz7q)qZqH7-OWKAno3VMyB;(thusgNH%o
z>9uOK5o!LZ9rW(Pi}2P{k;RnLas~z=UA_(@-<x`>CfrT9&A$!eFN4U&Y9#9sw*7<t
zI@%OLFUaNp-v0<2Hf-r!rhORE9}_}AVIYUpsDwFD<G-E#;6F89*_?yiIaoF|qP-jb
za-(`y0`WC_;A@M}rVh(36KeNWMN|PP+&u3R+z2EI5#uN;-ld&eL#qZI@9ACwtslG}
zrpXOIWd3Rpu5xMh+jzH_T>$sm<yFP(s|4fVN2Oo2`cr(JL~reOu{FZ+BVY`s(;tft
z*qzEAZjOL0NHO1lBnnW=k3gw>39Kd&XH01LC!*cwpCkTG^3OCTyhA~YfQK_9@TYvB
z)V>7ThA_&cQoz{9%9;SoNem2f^t`#km?emmYlm1BA-Zd;KMDb?t4lFI<cwm13Rh3D
zM^{(J%X39o0l%Q<EfDW74aMDOyY^yDr7T(xXVKH$B<?o}V!?&7%QZl2a&2qX#h89j
z@KnqKs42W1kFZZW-qZ8u^L=x=;aU-Z8my7MY{T1+;H{Vx0T&??qTzx7xSHXAC>cua
zF*L*4dqJ(q9FAcJY><?%qMeZJrcz4xgAnYdvgn$Sw#k$@N^qt;g&RJAFxfICMxb8=
z=o5g^Wwu=Li7i*p3&13Q;nE%?T&K;!ztW!`2NZjX@THUZRSy~Kq`LMJPbp=I>UEST
zOUm|Doo@<>>=~mMvv;+QZ+6(n2Wc^L%GmQ#v(U-6m$gsUv?FVMVD2btkhFf8l2(@?
zXCS&IY9zqh)my=tt6?feQzA?$n}D1n%&|!?Qy$z)gIbGgMF^}zCqb71X>;x1Jkh;`
z0z$tmMJ^$z@pko6kY_T4xP$-+VS8T4p6GqDL*9fQSn{@4f+&Peg7`jiFKR60pXv$x
zQ>o2`Y^gADkARhsF^V9WD_8#N=rIBND0w;8yc{4er<s?kyo{QcCGv6%F70q(n7G)-
z*x^g#LUCbEc>9WrZG5ubD8PjZJ6iJ)L4ey+a3e{TNc(?t&Si{w18rq~!S=5u&U-LL
z$p^4ZZqnU)uXkn}HTgHRF{Z!e$gSMyf<cPsUborI<D4g;Nf_TpBh&jq5$Obxs5S7%
z5d+m3=Qz8%9$lrLv#1IJBf%jSRnK3SLE1CTOOv!GnwKVNJ?5oJ+5<3;HfcW*7ba;p
ziwl#qYs7^~+GbpUwB^kH8jEpZ#NKqE%=IOpJmY;6<pCzj{m@AJtH3qsfjc+KoNux0
zCAmgOkQ5ssK~k73L?awHEkhTXmnNe9%u5r|)67c~Q6DaCL_3QM6VZJLf$YdMDK1Pz
z-(zrOA^H|BjNgLecHXR&ARh-ztocLotL4t{68Xd2mmjs$=*hkyeMY{W$xdSdr~pDD
z|Gg3~^S^ZdYkXLHH=Rg83Y=N5!jL`#nS`eqRKDIH#El?=geQz(;Q{}6j4#MyttnAb
zh==eexsw1yNV^_O3%7V*Cp+-N7tv;6{XorEbNd4iRU1;EYR4Kc<K3{<m?&Y#e0TRF
zr{{#@(+X%UeKf22$X%=YDAZ}Fs3Ex22y5QsaGkH}B?YD0G!Opw;(zh1OuGj6;T*$S
z=(qy(b`5${<D>G!+Hq>Uj_dMj?lKG%)&}D-vRK-sg{pqM+W9g=)x3Vb3n|;!I%;NF
z?Y8o+iBfTVL!#CP-0?E9NJ|Qx8k0#sLEpqG3QcZ*jx1M+FGtia=5ONp3>a-Z_jUAq
zm{6G9-4C~YA31Jzfx3okR*>ZD*>eW}g5$#4HzF-h&LM;`12#mC`HbS`0EdYFSK*u^
zmc0Z!3-krF_(<L;^I47^+uvEKc>zBl!m73+!qxpZl$v7&=a4t!<f6Yp{}a}`z*8c<
z7t#9o%e1Xw4V2tKPdoqylZ7+=TSwi@;Qi0&&UhKGV9LgHAl!t)U>=OXid7l2JX=&P
z3G(Abv{^>yp)l+MOWb~=iYBbdApK+y+IqI=3ZAGAo*<qwq96}U!l8|rQ`PwAU|=#Y
z__Dngb3QKl9_B&mSCfaJPK=X{)A4@Bixp^(4xd7YVqSI*u2Zie7X^%oxWrl@T7hgU
zk#Yvd+D3sRumg_3l$3!ro`YU8p-=t>+G*#z9rHV(i8EDH)kLpCtXv}!t5n4PRK!B^
zauIrqME@e^ej$GeTh(&}w7QxX6ntJxRcij&2|sJsgtc`_&Hea|3dOLt0`*h)t*sCL
z11<627EmA28vH!@JxauAVgeKi7|uCT%~DDurh^)$P7RzoA*@@H*<2ZRz?j5#VD=~)
z)d3(1-T@2tU_tHsFmw!?Ib7`$iW!=y5mXys67;drNjz3+F9m#&KuTHA9|{?X_xUHm
zWb)a?u2BipUtl;vsqrA><^!WyJ}49qMkzJp@up+$Yr5e;6Y2%xgG7JE^LPizP{_Vg
zlaJ=%=#s;w<3uKI4#&|D_Eh>yP`H5#7Nj%W9}zys{8)|_4!n(b_8SrL4upjjA`qHU
zG<x707>eW_bqLkLlW1%AkyJ2&oC9dPI3I!Sg-&dQ#BzD+GEj^bON1i7h78g5bhmv8
z{UsqQ%qj0~yPg}(oBeajp@(*)G@iF{IYBym)x0#lT)@0Eec;)+G`;}sA?=husb(wi
zE0vG+&`sG-9>WvpQS@C?kE%%?)R<=KVL4tCdicBS_nE%J`=?v)gE`Pc%O9$>%az)T
zz6MCwWWV@(`Qnas=D^~`S~I>f&dYB87c@_f7Ooca3(|e6m_P_$garfgW&#wM(hNH{
znI?*^3JtGTwcdVS`HVXuwcv`@Ecx;^JVPd5QuGzsd|7AS%;L-I!mrqTS!7<Ce0kKo
zH2HFm=mWL8M%uhVHs5MB);Ds<w=$Hab{*1G)=el1Cb*+O08HQZ@Bxr2EalvuVd-|<
zvFMF5D=(;R$M0yAOqg##$05e0Ma!iaqsszzNx7CH&64IUxfUYb73*$v7Z;-EBM_c|
zdf01w#8*H9tvRVP#1kZTpmiL8O!~pzO#e-L```4Re?<g#RQ_|O?LTkDEB~VZym*=L
zpBLCYbnHJ*Zxa6V)wNmvlRHm@|7+iWz6#kAdix=MWcbfRcoBMgJvuzB{{*7{p8uQ(
z>9PH1@)BO9<J)8k2G@E03i5o;>vJ-%ZU4)t3spHP%$p39SbZacU%Ft(am1f517s0F
z73Z1})H(RUb+s=NBD|t?R2K3CG2wgHC^ef!zVa2WzNq?h%kYO;hI^TIS9QGwe~?q?
z?qAahDT@-3iC>DOxX9!%7sj+jPq$ncZLZ$JeFimGpv%}RB4AZ&J_P__9ktN+(vM*!
zEfM1r{)b@};}5FAB6_SIH@Q@VK6CG1ZDz5N$15$`hoz#_5E;K3$HKXTjm0gFT=g~R
zMDP@7K@k(iiy)$Gk2MJ_b$_8O{F4|@muQK>M78j}L{QaVA#7r8V-H;BZXiy2K+1Wu
zr+Y6TC1n5`8h6FLN$x%J3BwhvWevciUMGS`SOdu24&b={Fl}xq@En67Wnl=KuIjN8
zTntXQ7vT?<uO(=A{ahIN8r%{jmz=nl^dKjmBgnRWGbzqhc)$hP``k<AqC75_iX>Ze
z^+u4>#EVcPoo3Ik%oohnKX*N^(bXi$Mv?ssG*@qchgNDPB7TX@@}Q>V8}t&<uJ<K|
zn6_?nBF4G_@HxjwzACNd9yIz=+~zp~GX6mKu`Or>YUCaORg?uEQuHv1j+!?1dc38^
z5i3UGxAc$1U+K?lEn(W_LPH0?)wm2JBic83L`z#>e1`WC6v6x!qkoYm=7RwU)tDt2
z^ExaIYK&Wr@8OrKca9+XoCE2{*mMi<lVcFTz;}9DM_o<Zb_}rPo86$GyuMakw;H#>
z5b^qCTodpWCc5Y0x~RSuwc40xR3D$5M>MhCUXS!pqbJZ@RX$M0yR%bPHVe^RErE><
zn%}A7oq;im#_TsBY!r*wpn$8QUhVv<%uXp>E2}+)gX(%c`&Pzp0vUVvoc=p&kOUXn
za=@=&+wxIV4-G`q`syCZIe5sX*zQv`BY-%!YL5(k<K8qF?+1nqsKR7vbV+PbH&?}5
zY%u`~y**+VwpL)4`$|1}!MJWL&|F3Ox%R{W=wF}_^2zv}V6s@5ePjCF94_Gk`&%(+
zwa)zwV=xgxpM_=(?q=g8LC~PqtkiBYg<%|c1RB?LB^{8<bSbr+F;?x)Iwm@WQ8I0j
zuuW7AD}O5GSExc-0g^?_v0x@-XtS~5Wq5)Z=ay7e#+3!eJAi?|3XRLb?i3a@t$O#T
zz+Tvw&>If>G7qo_`%=jcU|+_e5fDKBiuO2^5#mU@qb6co(3t;GXnHM?2zFv*sN!uq
zqr$_*QnT#wWgTd;6d{9{uY;Ra7h*NsXXW8cex7OZ6G0%jHBawWyS(B9U9UrTCeO8Q
z)f=fExRaCo=#c3bSH9vHJb`CGcKj-bPb7ZR(Fph@PX;4ReDqm9MB_@$7f^0dzmuQb
zfuF`KG_{f8p0d>cn&wnA?}Rl>H0LC)pmmgov#WySP8>9$#%B32N!i0bE<hguqh+Oo
z=8fn|(EJ8L!i4d`-z7{7KpG=gzVVIojcI5o-Z;g4V-em+J_P`bb@+t=ZgMui%NhW}
zB$zta9N<h2@E9&^w6;q!^~dXC08YJ0rjh0wPmrtAoNw$TAyyezI`4IZ_@x#b!=3l`
zu-#+EkDd3pQ7%=7h_{$3F&KR(KLhtTvl9~gKMbECQ$>)eIxM6swF^MRw3XOKo6h2Z
zCCdC?;z^<_^)Y#kXawO$N5(sFkf}w;Qy>jAaiNvw>DmN+%?+oolfQfk0$@82w#Rxg
z7`s>!4SA9B?-?_pV5)vItSR#(wfoACGd_ZdLshw4Eak@-jYeR>p%o4u+p*lWCPBYh
z46w55(dEuY5&Up^mr;`Z1a4p$>{~bseIG#IVH7uO+h2YJ57K4E#(X*vWXRvgUusl1
z2#<q1A4TGmn>=P*(-~d-16^djjQN(>EQ=k49^)d>(JtfH_!|j)Sg{NnH46|?8a22<
z+jOT`RLCqwCfJ+<H5uRhqfNX&HmH5U3co|LXlm{!jELGra=Ab<PawIx9V8zGPl@CK
z{4K5BS@qLkyj0?NL(Fa2H#JH$*DY@kO<?F|O3b}PAZ&~h9quxoz+YrMDo}NJ(Rm1Q
zj0bRo$lhlmn<p!;d%z$Pd^zE3G87Blv<QjRuJ(@Huqv{<b3f^jtrFuO0NAkB9>!0@
zZ4l$GfTt|_MbRb}v5>#tx9ur++wBTzpJKOT#Px-71>OjOZw64AiXhWig&Xbh%?DI0
zzH#4alQ9?~Jw@)f)%FF)mTAqBZLG`GRw6X-fN^ED%gfIM%`abuB8|49nR-p)Fa&^L
z$68gtV0<VPrJO7aA=d9qSGEH+wqTC(bhG+kI6i2J(F+D668NlQ3-EXZah2gV@yKS$
zaTzQTbtcA47*9(ctewM0qhG&CbfjNrnn+aPFX><E&@UoVf~DtUL4zsip{;s>{y@&y
z3TXkei~u^JZmj}tOWp!*g}Bikk67q$@u<o8BS<o(4)y(<P}?Rei1h^Y`WEDlTnke4
zplNeI*$+8IM$wNQ?(27xGe{L=sGWfZ*p#3@2dHZ6VKkK*9w03CSDuO=;rKA5>u&H5
zfRFW}Z4Ukx@GrwqpVU3%W1xx3LhiR^VFvbvV@aVbUYt-n;~9!M8d9eUKEq}scx43@
z6UsyPmjymnY98im@$)ZS3cmvgcF^k2f*4|k`WAk3V?~MaTX2&}yF$?SShQ?I-_5|H
z7H=6XKn0C%ZFZR8B4C09iMH`Ikq3iQyNw9LjuC&`jw!Ww6PCG4NH#}4rw%Kj1+I|m
z2EP{$%a5R>0FiVn8M)cW0b~eWo)ypuU@`hsjNUGYAE33-SAw{1i37v}8^qa!0U*Bn
zEd1F7jAHx^4kLr#s<v9#{p4tPF8Hh9*sy7BK*(LF&ZU|CO4Z(l2rLsAI(E2FgF&nZ
zpD|h9tkksN79?6($RZsf)o>XxcPh`0$C}&_R;A{5c#a|N1?1$F*s1jvL=wEh?uK1N
z-)DR(o#13NLgubrGHk*-#+g76w0f(_L5AyWGFY5yA)MfpG(6V!ms`OlVrCj)3r;B3
z%@`g0fL;L>bpKCu-w_yN2*$%00PR#K7@Px0fw8y|Myq5wxp5k!1@WIl|EPKExE@3w
zr)qCd`PKuz3}qw0MfE|XE})ZMrG|UUK+P~lhVmL+7MUD@67~TAkToyX@~gtprOXP7
z0w1AtLoru$H+%Rtv=I4Un$!!K!^7&%_Tlk91=hR)ey%I_5+xhWOalZ~c)f5e;W1wP
z8zz^5T@@Qt?fuugp|>Db{f!cmqw{3HFEkMHUhx=Zyf*|;+9oMYCKr_22Es6R2{Co(
z3vjv|GYjH$?$b6-wSA~D;7S3gJq!?qI}nc7Fq&};h+&7OaZm#<J>J03lxl2o<vDbp
zob2d#7n*d`?-cg?4BoK&eE|KY{%n-JkfmQiZ5dQd+be1elT`pEi{G}sNk!{W%NV%U
zRFO#W^REGDP)Xyo|20Vh#pCuAx{@EIm5zQyKNd{uM{Mh5O)*r9&cppz@CHs}Gd;+H
z7bR2V9tIdO8J{S%+**;T9vFQE!g`MWD3~rReRIWpOGW-(-?54mLE4Q<fNT@(Ey;|0
zyp+j|Gtnw}gUJl4-Zh8mcLSg|{LX$iNM_W-IA-PpW1mb#FkbOJ8IH%6Om&EwId7}o
zhl2d__q7)7od)Byn)-^h+OnYbmi(P4tF0IBmN!ONVAvRmVquWWJaA_HK`fz0SY5aQ
zYeKlC@|N)g*%Ze@m&ikC8)TqeXto|@JO~6pNUm0RD!MHulap5wc})IBVP#e@4&xa&
z$%f_cA?LP}UE??pu4J&htv9b>YyE}PWKa3eu*c()h>ny!5`w7{?Dz#qv`A;{Aq)mS
zEkB3RYbjthL82W%VT@cHHe7PFidD&902nx~-lCAlheHr5{k<z5E~-zy{7oBq9;da0
zwSCId4K!iugV@VzRl&oUeQlP-j79*M-nN6iT;{$UK}*B<9}yK{IZu$JY6Ld$lxd%p
z!J)_VL?#B(0vks8#-^C>W5vRbUs2L@wp$m4O)J8&2IMkVR;Tbc>{=UkHHHFPE8fsH
zVAB(}om2Av1{{&Q)0(*4v&-l~UmjcHOCPErkDp1$N1V?UVQFu>(dkK~^;P9;lppYq
z+mDLo?z!0F&siK&j1Aa6QX=MmVvBeTw2d9?RB<tKVEd48T2plrpEDOEvxrDbPDC<y
z!s_bOUgf8(+^?U7ByM4&@gR&!YJfQ}9Qc&*G232sxVoQVrYt7f*3|rC!iKzX)juRS
zoK)6OKydm7lU-Fi_-_%jnZ2%i1<6<2|2W`5W_AVfs7^WNrNQxdb6ahzEIEPAg3#U^
z(I<1U4GY8!3wKlMN+3i9e0X&P$t;yY8e&I;U8DFwS?Gp}#)m<ABmVu2cXmgjn~Qd`
za$rEX;ls|BR}z{O(Ke}_Ct>lf7NI>lqsXJ$7CW^H--D14iN}#v#>zNhm_TT*R%*Dp
zWJ;ZwpPhw&C%J!AhN<?ctlq>tf%m;g<WBT9P%8sS%>&aU^t;>-7g!LE=M^Fk%=*A?
z5!XgGl=X37)1-!K1H>VVnx=EpQBSuYTMRvAu8olZ^1xGp8(`%Sz5Pn<t<WBTx<mqz
zoX$&|-aLK8Lo*oNtfBz*=7`ov{E5@*muX8kMmi&91rf)s^Irt7@V77jN_halO3ijM
zwpCfUnJ6oTs~q*9Xw7`}M5`_mXfCVX!3E`$pd`M~8|V)kK!QkE`Yl{Yr+oC&fEJV7
zZ)QK-S$-W+`ySALfBIEcJJHj`CcZ0nRPb>Md$7=j_f64AZbTr}%&jxjPi(F8fWD!?
zo624NK^ccZ7JR~+OK{eNO)p%p5+_2KaS3Gy0tB%sf&qox32_qwiKd^ESaTs8G^G=8
zaY`jFm{hwtHB!m}=p(kSF`+-Pfor|rLpzb5-7sAt`Q&2=$V6<E0oOsRPS{ZgwxdFs
zqN6PQG3+4ctpaoQg!O@JvbExAy0`47%gZ^IObaUurbv-lDtusOye>+FLaf+EQXbR%
zhz8+<{r7?Qu;i=xn|2((_m(oUX30PGkX8#$ioYlD7mBE`ILu!K#?n6tyQenQ0slO8
z@!&C6jk$CzJwq2v5QQD%y`jO#=+dP_J&yZ0`zF?!t1OTKXzuF8pmj8FF^&N<&kI{T
zddyYf5kpIZiJ`txrI+$mb@Ax%;7I9JV=?+r>`TS~DDObKu_HpILlZ+i;Sr^wr9-{;
zFwj@=JTh{8>7|+Di~Hdb!69S#-Z%?n+uq53M`7f4-;;2mS0`MNkxsY<{o=;yKfB=<
zlLg=f_*wDui_uyDKBX>}=p^ueo)nMgCSPiU|ES1SWu>9;*q6%$Q@TviMo66J;2L=0
zxf&j(n#hXlQ0d^Y!+_6K;W4EccJZZXI7J)k1*ydIF~PxOBYzL#_R^u<_~I08oJ~jc
zH)7-nix97m9FLiH7u<|29ohxGRJl3jxAjR~e0cm|;o<ROt!I{>uPo&JI@Wqo`Ps47
zpNNkpmiLIY4pM4<i~E?3B1rhgRemD9h1j)Cxxt-RkdbYzNE&k=!+wXU9^+R0O_b2+
z<ru#dzw`O7@iY8JsMqa9%rN43XoiVry&rt}1~@i4I8J5{K8m*Iqpdd8V-IEXyJ!=W
z?wno4TF+2wsYtItIa`xI!#|wg^87bPz1@J?R=AYB5VXmsEaaD3XpyV%7g<uJrb3wJ
zT~N(a&V9ZU%Y#JR71mZov}ROZnl%=%mZpD&Y4Ks96botabCXbMdGeoW{rlw}3$)M;
zZcU#R(e`8Gu4R8bL35w$UggSVcuU=1iUZX?W^Is<Hro4%cz{}M?<Xn@_i<czjqF5k
zXpYrZ{KUCai0ZJ}Ar!d5pQqeK9|PLJCT81HtQPrP<-dUSI{V|IO$aowp_Tp+AR>q=
zY*6mt#2vrZqWcyaxxCGoFknsW{pw8w3A=uMeb0OW$N-=|0-(9Lb^dw01!&55=7?k6
z((}f%mx@5+qCL-&0$1)w=Lwr$9KkIDAe<~!y(jg1^F3qqAB8j=dd?B&%r(W}H7m~f
z2C_gr9ay4wY(4?$3-NEnUx0rngAHRa-x1`;ZJ)U9!|mkpAXHizEcrm>D&!ua#KjT)
z7!`{#C>Y?Jm|m7=?a_An5D9G>5%2v5&27=naKqNl;rPhbaAFBMMKRhAjN}_0FV4pv
zHBVXmdQ{OM4(qp86d{B)Z0_YIEGyPi?)U;%DrU+!`l{4Lx^#(X4N47bJAqV-svQJ;
zU;(=I?wEOMMB9RETwuot{5vWN48-UAS!%MZ!T_OHsPL~q-@7B?+7S-CRo)ey`c6o_
z3L!#>jyF%3c^V}8SSt=S)YwjR@in?&BsytzMKa=gKjL~T;z|*4h<us$HV{)545fgP
z2IwE_7$6NMFy%kwK?|yeLnxL^aInOF-c?Ni8DN?D9N0B-*9ZbT)Y5VL4ypslK~b`*
zawm^*A0k?=m#B33aPnxwRnq>z0uALd+3Qer$9ymLN&Ecc0-u0gvM=riM1p;tGTHZ;
z;`5MwO23sR2M=*g2{3vD_nPpYlDAyB4F3M7+~K-Lxnn=>O;PgJ*n9*Frzn@L`3&ql
zK^{qA^AY`fpS1Xh3wP`@@Nu5S$Ik>GKQsCG8Tk05#Ygm(<|E$hk>(@&d4haIUq};w
z-730C^AX56*m#k}MszPq75^PJrdf#ICi(jaMmkuy`U#VTl;$ip0&Knk>sgR|m6J-#
zo{H?FUd^@C^6tiskE4sk5E<*}C{7o5C&;_`pgqzYmuH+0f%Un-G<&@++4uLX9&+Tn
z*}w7SAJ8r9>;Lz67&;@~8mmS>6)q5I>D%wZ0BpY<%`j=a-jf)@WNS`hh&YC`d@2eS
z{zfQ_GtdI{@;)^>#AD5L>rb;bkeIVr>oAJ_zlCGOxj)*FeCz-^UDa>#(L2mxPv!xK
z33naE6qORL!0PH9P$01v7Uw6O^N_HUuRiUtllFOYmYrM&wE0fk^J_q#lYRyIB!&s1
zB1lXO6NE%)>r=I^$(4^})LWr_^5%5=?A8y7)(-lL!gj}*FZ#FwH9FLzjW5v0`L$vA
z;n&A`=xXl(epVGTNM)9OnAQ~4%VA~MA>)nTIOu9Nv7SAHXZkEj#?~qb(<QzLP6tKQ
zpjYHu@)Xp)bwSO0x9+`P^P>6$^n6#)6Y-NYS%^j%Z<+*2Be=xF=SaHp44`!Uq~~t9
z$su3hdxm^vpLgRko)jpE^KM$fc(4T^^N<_Vz?F!6fqm@OTO^6LTaE6;e0Wj7qtq}i
z!jw<*l%I;(E~26LAp2TlQGM)S_wpfRv9*4DJRm_d_1NDWa`7JC&d3wICIJvH)!-o{
zcN1>e3AzoqgHEnOC*r;1am^TPzV%+6@f3cd$sp%Agj=*f&TKypjgnWw*wh)boc(jw
zkotpDAH{mPb?lhbw>`wab~&oRr~>6>+B&9#*FG%P3KWU6%@5;bXa8I<2XGJn)r3FK
zgl8#vKw{ziC;UW{N;I+2{v6FR=yZR&{VVusH~a(*4+H-$JPvDnu=S28L|5y1b%DG^
z73Ty4%Vf~ZIb)f&BBU*i0HuNl1xhyv$gDW%66bjQaxQ><>nRhTUM4>K@TQ5+$r7Kx
zqKOSAKXjUb&r-BcJxF;oQut{%JRS{Ge-QWA;y#9&?Ci&D^>czbZjbw!I2LCtTk!tQ
z*&l;^c>=Sge=K)676mStv~V5*4a9!>YvouoontM)5E(EZhei^|40#c!%!<B*-kSab
zWfk-n7eh$kFXVh!AIO_t<1BHLrqS{i4tte67WUMk-wyjfa|msJ3Z}Gnr|E;V>kG<%
zuHxic<`Rp3jlqy!#~G7nik~q3mYuf!nKA1DLDxH<bnxfqY+TPoBfbl(VA9p@iPpsd
zubSveL|~=_k~n_p$$On4ZPxSToqXze-)W!pp*}TsagH_&Zfj_PHmpFYp*M5*FUxUR
zJ22#7N(1f#w9!infI0majBbwYzi4mcx4CF**@E`=0PL-K`~dB|n>Gyypgs}Z1Xhb}
z79RmjeKdN+L9f^e4-4*Lmsrmbb~`>CacZM$;3Wl>Sm-y;HZ$#Z20i%j@}hc@QQEiU
zu@Kn|{$`#><}<np*DdVxX0kEUKI<cW(Ai%^aQIk6SHjUw!9elc>WON!Ash|OI}{vP
zJa_hxSkfg@aPT9ccn}2?^|K4&c^>WEhFzz*4z50^Mh}LggZ_@jFIV=^TJXGb%V*N>
zXsc7Jh$mjdjgp+jq>5v{Zk{y<wec8RXqEKQ9_-rDkJa<$iMFsbfj-YDA<PHpvBYjO
z2fu}WFyS5@<{LPyVD`1Dexg=9&$thL!C}TnVO#htPhfS`1!{CpEZIqI__)(q4QhPM
z1&44-;RV>py|t5i77}{ae7Bn#+u_!R<1n|q6?=K^@`GArfjc(D2M&qz&m>+(3_=2X
za^GJZa{nY4FLb;+B>;Y<A0-K|U2gNsK3&h)OZ~^ez-ko1<MAj3_%-DQ@&Lmc4D`7t
z67MbUlzT6~0$zHO92W;F`mlpP99>;?j;ei$j2J)%U-T0Lt7jh*+u_nqWRoBgd1@@-
zM!V8LL-kf%h62lGpBiy}Y_=cg7EPAAwQ+~k=x84fVMxUXw+0$!pMFU^^gR@9%n|VT
zFzG_<<km**XS4XU*1((9$zURM&mpohcD)a>!AL^-d&WmtBT2J0-#7+{qAC{wX(VbG
zU(pnh$@;OFj6A7EFOJ0BF9-DjSZ?jf{aTnYM16YR-J!q-<pHBN6bEGP`)3E4w*W9f
z<{tP#GAHg4WQKV$kGU#AojF9prwzJW_vj~RJ#b~3F3}<#@8~cN`uKd%LV0?r`XHyD
zjR?4H{~OUy40pb*CH1bke=-C`DXI?*9Pcf6<7_vqFRuxTbz@~=9iYMcy^`43hQHmD
zkKsHsoKRFn*;MtB`9ZA#=L{?>i!Nuz!;Soe$hkMn7$>e}Hdg7^Fk`41FK$L}IT2Sq
zmWUdL$3x3KBjXWQ>+pDFIYe_=`HwIHW@cswy>P|ktp|q&SU0DyheKNAY7)>jg@K_s
zAYGW-Z2XpqohZ~#%w?7#Q-uCb`M$toQukvdKD`2e64mW1Wquy>!`0J*A$>5!v7XWC
zNJQ|;(=&i6&x6Bx5aJXFPSM>sP{};VAQ-5x-bP1h=^MvmCzzghf8tR&farQCyM2e<
zl9rF-k=eOC$11abWG*mib&RnEV+g%qFLz}CXUe|=J}=;#$V`=7J0Xnr`vR5=_3>VW
z^b3bT;3X6|N0!IO7Voz74?waI-vj~5^r<*?5K>dP3~v*?<&QYf8_fPb!_3g?k66Br
z7VY($fgbwaAr>mx!!wQ^>^I3rbm!#?TpBI$-dwcL@V8RF7bV^7$mBoXJ;e`D2((xH
z_4OL$<jr7<?M3h#`4PPv4pFE^g);0=5C=RcPrsYyy*R9+0kVmdTLxXzd|&jigD@k=
zu?^0CNtwsdq=Wu5h{G}7bIKQc)1AGUb}buDFW__#%j&02(&vffHj_TV@c`$HE&}4h
z)xmM3RRL&(JzG$`x6N{+v7?cfR9)s0V!7Wyl4Tonvip^qnTXUNRU4Ig4<VXFs%aZQ
zyo3oHgU9Y{D_V|!ORzwOidt+-$Ku`TVI3vlw%n4II0%L3)`t^rekm!Z+6cl-yEc>t
zHdMR`kou1O14&xA3=3o6;u`>Ml~S`6Ar(g6pv+?}z8qOu*bTqM=tVoR)Uu4-;qPw#
z<?7^mv4*La_>eE$f_JuHfo|bTc*y0w_2NYwSs&DA`}E5(Kb?s%qCYZTEhe2WiMs!S
zqYdZd&1E6iG91_u@wechlwUC0wI0=s`W2oseJqBW?8k!FmHE6s0GcI}tUXfQB;G=g
z@?ykYjjnL*DAoOUfB>bgRe?k}x=NUtmH2m}yM-&C(8Mm)E4ge|AI7H)LV?NtEy`Ut
z(P05M7oaIY(ZGHepduYmx3*iUJr$3o3khBck9!rYR3S{mq+ZHHLO^+@7}!<?1|oBZ
zMPt}}h=rg*BnqZse+oIsWjnFH+3VBaiz6-dnc(CSu7d*ezGb=>ryV=w5DfE*tve9q
z71j|qNU(X|t=__1x{K+qU|?fKgZP9kNkP@T6`hl@f~B#^glY_+f>pQxh)Af&K*3j&
zpIBFvtXu{JwWT58u?$5Q%0f{Y15y}*ttLmwPH+_;>j6i%aA~cD{A81sknW8QiMxvg
zN4Eg4>yM73C!|EZHA$-<o}b>8LM6ddFL{c!`<^|j6B#^RrrdSC#nVt{e4K_%O*8bA
zwhRRWV_~6zlC{OZbFy|mHZ+AGG}~BtG|pt~7&7(%nAGbuG%nglZsHUmuJ*^hqSg%9
zpMnK;B-X(gA7)AQ9<mYutNU>tS;Y<(skl}TEcb7#z&ZrhyMNn`dY)mcC+bmipa$Ex
zbaVaJCNDG8^AOe3QpyOFa=EgQa}WVCl@T0N^$Ccw5FmkDLH$Z(zaZtfSWi38hAm{|
zZXOD?et0zR5!|7Edhc~nL9e!S^UC}%>Q$+uSdpfd0)ZYWB-!T>hVN1@54ES4hp_W1
zn~fylEx!>Yq<+3b%PVv<=J|?;mq880xrPT1nzW-~g-S}JiljG;D!#Wgu&5sA5iM}q
z(|F#y==?Yc9%Gi{sDG%rp<!KJu_E~-`lEEw1g^zMg4!8;IlS3?OmOJ1X0s!EdBxFb
zSC1p;l-stEINO5y^j3yG!A8!q^a+TzV<U^Qc|plL%aTzwT1jO&2qnQuDGl2>f9^$F
zhxxF(k@7-fV?weU8lk!l3XRy5OkyfwS+zes;IL}*c#tmz6#T?ES6Tebpm~NajDaq|
z0&*sHRJw3PJwVNu^il2V0p#Z>ebN!=-I45{19~T~J>Zn;iMaZ<l4sk7Z1WBS5kh`m
z*ml~5OQLzwz6)!h(->gR^5*{|`wn@u?R)$3*v2-<0B4XRz&UHblV0_d!jOJ7rY+m5
zW|}fG!VA+-N*f8A8PR>=Xfw81)DMrk^`P#ahuSF~5(i<vm_e65J)he!^4?{b0&{`z
zH^Uy{7Uu#J<vMTx!H5V*I#qwb3Zc@#s_H)@3aNL&S(Stw9*dh!$?ln7inZOob@#Pr
z;+BB@h%mNsU%_Ha6e3Zv()bOcMtY~M(mUl@-sv<zd#<YO7WH443fFr~3x`R4pb`G5
z0siUDfzY#x)yXJ2wY0I90KtkeR)KVuUs}7*q$)}Vu=tOV(Meg99H6a}y21mYTq2_9
zo75HY{P1Wfj!A<%HaMQ=I}DAF1C6`FjxcTjeXY68M6=kCG^W4pR{g$3<HrS!L#Q^(
zCh{Z_`FOxj!$P~#)q#sJElyObZgQe=G(vKIhLBJ|WISum%d%uc7=~`uA6sxg-H~kE
zOt2??LFO=qr#ZF4(4n}SHVGNfR~!xW#q;80a(Tq(XOQs=Q3s5<Bt!=1I1lCYoA6_%
zo9Z$}%x8}*-~ZDQ4%ZQf2S4Xrvj*_X_<_OfU+;F9{g}WFku(nly$-Df4iL1q%(?B`
zqNhfeRt+*4<j~=7FJF&!gyYYN<MW@<WCI0pu<4(1mBs<;Vj3W2q1ej-(`E$q3?n20
zHw0w1CCby|p<X!%;p*AZGQGIYWET>fyxAVZ%zBDUTa+ztk8z4&o1)M_1W!us)x=@<
zUgX|H{;MwO-Xrq^=KQx#S|;qR5>w#5r%3i;{u>+v@!Wwl{5wd^e#R=3^_cg5&dKi~
zmOl?r=J6X|RJ-4}VImkY(8GU@m|-}oQt;JH5jLD4^E~ax4JfayK(UYSlwaN@0O5Ry
zC*X4~lvH9r?IbgC<I_)Zl(+eG-kF>!h5Vm0isN^5@r68V!V3|@PZm9p(%UTc`V+2I
zd`LpX)uQasu;f>8wYC+XY+O`zGmmkLF6)diJE+gVWccTp+FphH$XMjAitk{}CT6@M
z9>>f9i=5z3IO?roOcd06-w}+D=?;RdaeW5CY~e|#jMjXd2Z=*HCmsVdIkoLny-du(
z<Ovd(C@6Q8gJBTJ{S2WG(W#VRZo8XkVs5)%scjRtq0e3yEELq+fNnQ}KJS5gDJ4dc
zByD@jr+e`9Gd%qqPd`;+tMQbA{kafq9C(<c-{y-%#U!brXdh-poE$l3MyCTA&Wz+d
z9vx@<AA5cj3cO$OX0k7))RYHG$c)VH#kd6g{x~OMZO8hdHI|*fMeFSO82z1?k8!*e
z$rlj3n(i(wLi3t&mlFw~iEy%!Fm)&<=>YW)?Fq^p+?1}+l#?6Ma^jQ`AsD(TCs1yr
zQXl*_aw21U7d&AnAtS4@Wn|_NWTY6+Gx8tApXfJ%7#*W;MFbDEMV{wBz_$J8^#iFp
z<O!f36S|2g;9KhlQzjX>Qv(#eJ?ruUW-G~uF#V%Hm-g}Y<TgxAc&(50z=aOfXLvEk
zEH+mcLh%wTlHiEiFm`z`RK$C(#=!1!e67IOjWZ$`unC-yb*uQ8*~{$w{G8NZ%=HH@
zl=1k5y4y)@)Ts4Pb)VXKu)*aqtVh60f8ksYmGG@kCG7Q!ktn+j4IJsQ)-RSb`G_oT
zw39yE9gN@X3hb%=NR1E10%hvtI{1cAU`2UhBwBJvrzTlVdIa`Ve1KIA-{3&r+;Xlp
zh<Hw}He|!UEVaNKpPCjNOX4<aNOp6jVN&<M;Ae0QZuWZT4(`x|wZSTb56TalvwB)}
zBzhgf^_^9}L{?>aME`-RUk4&wCBJS{xSI4F5uNmFPF{YmM&0*DqM>`SE3<y~naKS6
znCe9VYR}{rkW5_cRpPAFpxs5Q%G8LsTYRI2-^GC(^~@;??5O-3#X*$ESi!IdU=lO}
zrOaD|9wO0e#5@Aohw;l*{p^Sd81dVCyZJdA0+=(B@C}cSc?bqR4WJTd2?Bvk9WU*p
z36m1%7t~G2Q-O6RgpEp#f%^_PKd5}%hLdz~G_CCmnFebM0gbycc^a-vLMjUjZ~>Ut
zv?1n3Gq2M=J%a*NFYqzpoc$vc#$EQnE6dNZ;Pa4<FC?FX^|bt@iO7l+&ax48NfJJ>
zb!Ay<X`RK2WWE!4fZCMoB4CC*Ov3?`UWSI3p<^i$wk#lja_>l!{R)=VP3rct#<%JF
zBRope7c2&CSqUp(S9)U2v%wP%Y^?m><maY?P@}!i@PYc2*stfOMPb@B!ou&GI$)s$
z>=um*S7*ZXI^~8O?@4~y@H~;hZwZh5G&5w<0nf9^$ry<vPLd;nhZgo5r_?#enjH}1
zba#w1`^+@IFviOA-X^}sSopq{bW40!q>w=r!ecTAX|}?K<a>hV_z`t7DA|nLwyX(u
zaj|TQxqgE6B^XV@fj*WA^>3;bYh5Jf6ZlExUk`d=1J*F?i+8^OazIsz7o`>MQQR3G
zy&=aPi4OK*CrIE^AP6PDETRv`wDKw>F+Pjv$CX9BbBF19ca#S5o+}?!HIx)a+u;fK
za`st?as;eX@x||}`ZOe?=kT^@6&*AG5FQ}rWR#Orxx*~AKzkJ-L+VcAGrmx?#WjO7
z_eK_pyO*0a;sQS#$i?i17`|EK5SOcgYxlQtNAM-^sS@Y91OY<t9btm=Qqh;P@J{@j
zjnR^1KcN~zpO{!<pG=IPPbEHpKRWEU{M4#d+5Ig`zAu<8=#+)8ms`g~nZ9><>VDQc
zEp;X7JWV>br4NLm!K9HwFvWj%d}gn&LFh;iF~2zF&oSY6<@(wg^^$sP&B?_FsAcCC
z{xN<-nRg*IJh~6tw3LMl5!iEmzAU=I+;&!kBf|C~f~z{oT0e8nC)B|1O6(p^Aoqcd
zZeLceqi~X4CyL>Pq2H>+W?=}htnV=rD7N5&8r{HFWnL6sN{r4)LP($G4M!W8e8>D@
zH>zm(cYip#3yUmOGZ+l*XRY^cZ1}|F!iebC=ZA4lZJF})dlAgHr_`b9CXPu#dE6b?
zJ^Lcoe4>7(FzrvnQGBQYa{{r&fH9AE1)v8eoHF16LO(f0sCb9v@@gPi`B$!hl?6Um
z?wkSoX9qcb&z&b}jTf0n$9Hl1eW1kpNu<yBnomJvGysKUgIk6J=c>{UsQrCTZRGqO
zAmwqt9@FW0sNtb5?ygKasLsuFq$&PdG`%0<Culkn0NXSbr+?ZsEsJg^O+|7hgQlva
zX)|d0PeD`ZXGmAoq$`%KO}f?rJ~tK+^X3vX($k$mPZ>(%8~|p>0$VE=b7x8!&J(<|
zI-8c36tYGzO&9dv%b0`pVJ!aH(wIpfZjuv=Mc(%5ll(b!Ly)5cbU8dZ%G-!oHaRSN
z=cLE^8T9a`=>b)DNSBZ{E5w%aMVdTs)8w{nnpg_8wA!|RP^ephPx49O5X|`b5!*kQ
z`Y+m>>+5IKgMJjewEi=#0Oh~Hq5OXm%72qrDF0HD8)=rZXfxx+lhg7D{a*@2wft`7
zF8V(#ua*V&Rw9Zs^*;mL8J7M_pgPk3hlKuDd!h8^E~?!y1w#K#S^bq@j#zZ#YL21m
zH&OeW@joQhy|ic_>}fRg0t{N@1$@zd+3X*~trfzg`sixByJfy$p71+j_pa*N5aHYb
zQVR{@9H^&vH_jV@QH}&Yu6)+!%<OE=KrZ_ZsA^C1e{5?0apu!(%~z@ULqhsZILrn}
zfZmzJ?k(~f)O+Tggx-tAZ8)RKq=VWpG}(Ip2TSjN3UI{;q$Q4a`bd~li=1TXt?kn?
z0a|W0X=&-bgPs>kI$Bgs<ziM+h#Z@@Bbp*{51P7nSG;S{^nY<c2rR&t%AFP2G_@>1
zzOVpk`p^b!tCTh%EtEE$ygW~p$!l2z(1uQF{l8G?|6xQ-SvZffO*eBmF=PCmA!kD^
zijY2eyHni}oJ;!+`tXx?K5rALB#b1Cf-QGmz>UPal)JSSjs9rM-PCLvSt|DYET={I
z87;ztun1<n_+avLbAB!JjrWVzR=v$4&=U4QBpdBYo5?%`8_7m&yl{(oBPumU3j2Uf
z@H?yUt-<L?Qz6-itx?hUZTxsX%sJQs7SXYj9xmfu(>|6(-C|*{%+;a>)?bovFQL__
z#55Z_69YRddGu}HUAGAZk_B^Cl}(>x#PG<_@5b;dVpY3hhE_|}2O~g%`@ae$WJuHD
zG>b?SXVx)X&{21dLW|+pgU8WOh>dRzF?|Pqfv0GI3L>mjVO?qwb|uf{^_ED~eRm{!
z!`*IPt&hY9H874U3mCHpqU_lhMn&hjsy+@A=3&0!Q7_VvC*b`#1KoEhH7DdUrp&>=
zTz$YhjE?7UY^D|?Se`X1YcnDSArB~tIdrS!0ip#EdM@6GL=TAVgdajknUXFc#bF64
z{U4(>w^MBiM^T83OaYJ4syYwf!4!0lVBu$EJa37xZ-Zh&-h?ltVnkdCHDCx4J5;$v
zO4iqvuQPtyLMB{?zLV22C@-!MV2RQWM^(TCJdH&62p<{|I&@=N7XB&h1Z~RsN0ft+
zaeN63+TatG4N~u`d|yh$7nT1=?tv6jzR7c#<L>Kc3L41*op~!jBTB;GNS_WQL4{x(
zRuV)$4RSDsa)6m1XkZG%Jj{C@D2v|k0OSC9X2?OKl!K%t2aRbt7!h^fBjw=s!2{j%
zl$xKzdnyY@;$K?mGDM9CZyEo0BnQOgL*UyHCPyLDb3l-PNv_Yc<ofLP<+}XBszOVi
z-)<w%@eFxpU99swhB8bRe!_`t7_3Iw5B9@0;v35Le1+V{w?{cA$215jN0>sGKo86}
zif@fbU`^$F?33ds8NLbyiH(3MuvDjEx>I%j2g;{Lli0&rr9zGI<+v+-_(BJ@W)T{e
z7Hx_|*M?yUa54<y7@-^W!eP+p*3%+K0xgx>!`ReZ<}$*8ROLI=gwbLYvzXViP5i<l
z0D^7$0j4dMZ8{l-w*x*xlaC;greU%KvRnw{Vpt~%<oyuH`z?VihCsqDC1>L^khDvU
zv(HiWp_sX|I+`{K#T4^~1oCcIYAy!3l!b-(*AmB7bi0v(m@`$zlh?sHNSfiJuAs4l
zZW;7C9-lK!xa)<SnRG(DXVdAQ$+gf$OU_Xroe)XfIc!fkIIpc7i1SxEoBT%w3we*p
zVNV>3EO-z5Wb%HN=nL>e4rYj0SM-aGU;G4_lrMaV0P?k>1NrjtP?w3GuxmYxNrO3v
z8dy=eiJg)#$y3p>1wY1l80c8>J;uQY2(58;o8vg(N8dT3ZyN#jz1i;jQbC)vHS1RH
zv}W9bMH{f06$tHHv+r7VBwrvueJuIG*3$Omr~GW3cVn8dx6^WSck)~SYukecSL#{B
z+nM~Z=+vIR9|b?IgfNmS%UN>LK2`AboFizIj9aC@x76yW?7V3g8)w5Vwo8}6RsC(c
z*mSV`e5PHr;y;Q*>W^vrmqo9`$uI1qEx)#b%#>elyO;Ke@`Qa7IMq*CsH6Tx#6~3H
z77;8VCy1Jt^;TqrpvfeoA%Y|j%YZ)|5%n$_5v^S0PQzOkI4~Ra7}DPKvHV3F|MzF%
z{~oaY-+{`N^n8dmJdA?2RQ^8s1&TyySP$MzJ;)9*?uNQsCaD&`J2FWHoE^ch(vIiV
zTVX_AyLP-KIS@U`h+>%7LNH&9ZI<KA<7y-6bvYmt<2mW2c0ez>658y4k6t)1JO|%v
z#lKRZOnULuD2rars7WDo|6pwj9h(!nZ^ca+#~hg5*W#nU13s1)Rh?$Rvo@1|PbSX?
z9LTRxo}W$2N0vMrhq1>;A<yG1d5%2;-Z(k8>PDyaL_$Mw?orgX{Ck|Ed>`4LHVc3H
zqU}!)RKlMkQEap8FUb>FW@9Q~xBn7Y8$l#x;Y$2#ajgwl)u@~y|H-%Hj~J+Mc{T0W
zEQVS3ih3pe^O5}92M8iWug5TqMXv#?SA0oLiUHyk_GiX>ZS8-=Wn|j3*8rgC^AMb8
zM}5u#zM|iD;WYb9`(bYJ5q<VNOrPHp&ZR(^y7_%p_+;zmx3V0Rza#qNLdzc?sDwXu
z%H7k+KZwF2!Qa%gBpFU?l~Id>4JA(e+Uj8&K1>mMDAzBzf3Gh2{4U4(!Wl!amo~gi
zOUNxeSKJ`f6DbA8>*BZ^PGOaKJX|Fb-HKysl!becSP1Et`FO0y0PJO?DP=ZclbFwL
zs>;J|Ok@fg$zKr%2G(<Z0SRN*j!596N>(iOedRhaw3S6PCx;BjML`XB)}Ti^y?7t7
z2T-I0RQ)m!=F(zTHHk?CWDk%kz)bLa{JY<BjADL>$9A6J2`f*40|}WYm@M-I10wNa
zGf%L^njeZh!BFH0Mw;>X?TE*@y06qQCx}I3VLzBB*y6|&ARmH5eztUwCwTi8lCUJ}
zEtMp|-^HD5|M?JHuSMs(@%wOe_Jhv0zT-eCbAbWpH{~^2bl>iv`%3WzF01!*vU&$h
zy@moN2RGrL!HlZj7)KmfB@^^Ku&Uku*CtPgYFcqP<`I9vr5Rsw9$}g*FFtC+__ED>
z@~Gw$qs0(1KV3al5Y?2ObHElMJG0QFW7!b_)8S;tjyoYgR@_<c-;IeGg~*)FO_tL+
z%8!{}*gE@&ai^G%Gp(?-GCd!+<R&xjq@{Dt$7vD7e5ow%HtF|o%$G*ME(lVMf&uK9
zR7X@ZNBqE0>RalYGk?*ZzNPaQTaJQW9p*1Ol(%n{zo5!?ozh7tWO4|6W_#<mw<jMH
z)7=s@Yrnn-`L=(pz9|3O)fb_nNU2d^O&NRv_ERqZxhyW21sqU#Fsb{6xPfX}78>9r
z=5`7VVyyq<>uuNZyWm}OnGzL1T#LsL-95N#*&|jZ4B=Y-WOL0Ob~?K%L8%!k?5X$&
zG4ch@>ICFK=b+nvsXl>YCpb2;s-ljdow3Vrta@9n!a6D<Qg_36)o|FX0t+Qx4vk@#
zzdlTtmsADYbeSB87vUGP>j9kg1I}|+xO9+;AEv(IM%f?RB_BxJ*X;h?C}g$k4Q@)e
z3+cm9_lDiT;b=?M$qxNZxEqMKM4+W|mnAo+V_~1-a;*64>ost4n)TBLAx7~*h)~P*
zEEbiY<zoFANMseNdNqz_g8zWUT)^1TEoOFD^G5#0y}H5>dy|n=k?(VWXRo0=n*CV9
zHSK)#xYp{CLiQ&#GviV8iQXl|y9K(U?`^3+;-x=WFG+ty7?34Qx0iX#p5E0J+mbhd
zrtQ{aEJ{8vQdT8G4`jXGafDOW>wWn%t6uL6Q0IvCde?YR8i$Yts{uUYSk&u{jP|~-
zO*x#X*UL96l>YsCy$Jx*B51qyde<TOC;+3O7>w37fH`LUTSxVJ`Tx)A^|FX*)$6rS
zPuA<@r~WV2>shkktXH$^_3~5Sb-iBMRcTRZQ?Ex@C;=s+Q3v&U`QK%|o{4>X^?LcK
z{{!`UPh{0&oNy({XVqi$m&81(dJI{N^>5Z=EV#nv9?D0odW>7eT@ewtSC3`ZW8i$9
zZ&8ooJGy#|d|_6<>w1hQCP^y8D>y2#IQN2do<Glg+F?Bgtn7cd9z)XdXzMXZ%WqYW
zk^i5q$FS)GYW5m)`FB^3(XohiRF9D*X7KCn*JB7d`>)hv_%Cy+Sav<eBboIWr*sjm
zEqhP^TEfpd?7?B{Gi8aE_$+{35%=xtGiOY60&CT0{zBaC&>s8=Q67rtJm0cDldL*i
zeI^bp`Ons8-ZP;+t6<e<-e5k>whDHAW_#4mtUvts>oX-ykG?)ri2o7lGbLUBbM=`f
zU1$r`@!xHI=3&UuZhdA3In4PV>_NI-mGzlvdi)3KGtV67RH(M~nGa^h17~y*?F(1i
z`O{NCLYRQek}MqL!)>3RD>ayV<1s6RLhxo*CaxRusi<}k325dvnMXz1&5y5CMRe@{
zgoM0-IxJCjR_0n35p`Iidef@I`l#|PFtG2nV}*)j)?ux(>#(L{9B!C+h4Z{<lU$6#
zc|D(VMXH5sr82e3<BkUK-g;H<BGwgAMJiUUm@D89L*!TS7<E^u!x9cx=EuBqkzZYi
z{OUqchn1aQ9c<NM4Tcw*0bX{)Yq#s(xq}1V`AQA9q@t7o|1$s8UVgQugZygw7)f|i
zciL=xz6r%{%ECpUx2VAac~Oi`A&;UVOR!qhXe!JSEIHo?y^B$9V$mCASl*>#y(KzG
z)I`c7(O7TdsKfe(by%z^|3ubdwb=Dn{~RqifvQjEsVrv!9E<6v;CBYo<vbN|{MgBN
z3%*SLqyB0sL$9{=S0ADN>Hv~@Lds(NNFC!<fYIwx0g`z@%)E<G0i#AYsFa7l;lLds
z3?Y34YHlz^$2n(cjT(|Nl;k11k5fmg#20qN`h&Ngbinx8bvI$w-E6AFaK65qE^S8^
zuj(e`A9XiZKo-SWCjCURwxNAtM6UKIgptd*kVPy{V*<nP#28W*i|3>C=Ebt;<QE}}
z5NYIU!Bfd^<Y2j8NO=_XH-lM!19?0buh!|s^M?eAQGc@!nxZV+f`8@0+*Ym^%7kr8
z<?YF7Tro3=I8_i7%ey8KGl^&W^>*uUOuDU0Hh~x<-XE}Y8^R$L`lMVipPzh5BqQbk
z98s5peELY#XBC_I;0AmyE2CZm`E-6LU}6{5^?pWu&T7i7S)UUXeF}X`Zcpclm~TdQ
zXGUQ<-~1-(va|aan}6EY&%EiVpScH}%48Du|BMn^Y9{Q@hICT?&H)G=+M}Zr0m~j8
zr3|nxROIU=|B?G>kN-0N+<v~krShoq^@pV&^3R|j&aZ?jwXJV@(^22Vd_5`U5(QuX
zmgTnms??cpWxtWHm-buGtF8S$T)w_7z1q#!A6A}0FCovYH|!y0I$dwLx=p>ITqC-C
zgj3{BgvbfA^Abv+SVu1weY1Vw(aGGmF*|jXH>?j!c|*NcyYgn&YhisTZD$@xcI&WS
z>pPOKD<NNkERUdIt3$G2Ip`>?v9wnvasK}+Uo_KSUecbIp8^@PyuxMTcSl~~bjSKF
zym^OpP^N!am3#{9foF&R<Y|?(2)lS>PqaxMJb}ZH(y?5&5c4<+o`{qg$e}V+TWT_E
zl2t^Fl7DQ@GmtHhA(D$>3(g0#XJ8%v9eN(sJdZhi*4tbk9l%P{cpof+&Jj=HH{h3s
z;S**?r1P*se5E1k7ANOLG#nNp;s-1;h`Qk}IBUuBOA+obSV6zkB+7C4G1D@9$Mk8|
zNwAcvMY(e<q%`>r%9lv{S;M&&H^P^E09A69f4LDi5q_Kg<p2O8jaG`vtN#-;!pgyN
z)>R*tJ~&>0^nqW1of9;YWv?cUP*<Hvqo=ZIv=MbnM0|~?W%@55KK%&7A?r(z5^qsY
zF7cM-l1GC#E6$Q1ph!xnN9+`+hb0sfRi{w2;)yVIFQ73@U8d$BjKoSbicWC+O^c9c
zmR@2S<kTL+ng;Es;@~L`mV~Z6$SXWU845OR*Fo-|gZ4ZJ?b$NB!meV8_pW8XW$`}i
z!e=dk@=zj5JCX<(QyH%^+W)q4@wkeUa9VKiJHulfe*+b|gk*XJ)lPy0&*H~_jRZG#
zOakWpzH7VD7WYeu`|BvP61cyJA8l|Kli+{FaAL-hw@69@<`2SX>cz28;N#h|aL^XH
zY*jIt{0HyZ9CMOBI3l>@+t`JFNk0Vgjl1o79P2!*;*5H*AD2ppCX2yF4Hb%G9cHz0
z>}YwAGb_U69<zBn?HO&=h5B_!VHjoFK~X`3?Hbrol!NUYy-+?>=d1@Sll5TSe}H<h
z4`n^rQz0>QRy`O!P6*$xz=E~RTZE(Q+t!3#nOPHds#y}olMoy=Va~c9)bq&wV^642
z?|jY<_dD_yI4oGs6**53bzw5@#s0DO^A_Vo-lF6%d5d=H!f@XEP)y(_bzeNhX`r}2
zil=qAbcm|lC?`soIzScnR@x#P&qW>L*#~ptqs4P6*e$avZSynIO=sgN7}p7!*C95J
zV!jTr5&DsZ&v(POf4!d1Jy_}n^DMHSZ%0rFaCXWEDFQp?gWqkHM8?a|lQW<vX?x#J
zJs<T3^?bJ8tW2)N%-QOl$pO1}L=U}g1wghcw%5P8;oh$2Yui7Q0}?)wf5@umgFokd
zjrC~zP|w#U|8Vs4wRGM=jI#j4n(?H$|0sJLYyZxX^BV(2U&wDDe{v!i;ON%@|EGt6
zU&di#zG-hiiUgeV&DTk$!`FjZ=T+f@clG7RWt>$d>=Mqa+JheK{E8ae<w>nbUt8x_
zy=0yrWquDyd?_#2==WpH0un>S2j2LNl@jwjfi3p;I`DnBpLRYS$I9Tukoc&4HJ3$K
zvFbv6)^S*Ys-0MV0$Pkm3zXBqYz`@4Hz9mCY)F56QtCrh?{y&`YB+DB5zj{#7=Oos
zD%tQEsRtbJ85a;Net)lDACo{fei9$UhR?)Dd=D_!xbl3w$nO9q%x?nX15dezD(@Cr
znDVnvxyHHTZfD#qWMHN0B}O9D87a-29DFj+il5|oTWt7%?FLsiJwS{8paqJT3gmaD
z))fo77O=C}tO2faWC(MMI@wg}Pb$sfQ=n-S{s^RcAEZ*)!X{OJ<1QQw)2@6NN!FE-
zU&P;d={%EO-<<vJ=+%#KwxL(PyqiU@PU7yrK(B_JBhu^7L?AJwfE2Om<pP~esmi36
z2jU^<wHZA->2(zz91XpGc&<sWH_ti}z4-l5e4B#brT8K^rc_&klR4jrXv_Hx$imt+
z{6eXx3JID@@j-1Iy=m@G!Z{fY#D-O7_;QFmulqn?UBza6{EeS*#eZ8A^D9?4tgzIF
z^LX<3{T{EZS`X~Thp!G8pA_O7rur2=%$V~y1II^V7<J@p8<eM4Aole+59Hv%;GDl5
z$GM7L<tx*FnNt>BRfYqXS#ysK^I!3FP|oJDsxeHT^f+?$^*En;vn}tJL_@VXwQB~)
zy=W~yS?_(b>WBE$Q!_pp6wzAXAi5x%CT5otv8mG9{J0+nynjez845I2|DCHF$b5T3
zfe))6<f#Ey<I_kyDLubvA401<L`mXkWz=?td0gol9NY6yPSr?4=@Ek=^c8jlD;6h2
zw5=Ff`V)>1nj(KU5!*-cRe=d5`%xNLvL8j36(0nJlRF6Lf`PRaP4syH#S_4g)xT9^
z_#kplc{lES&o|C1f(*o#6k4Bo3|`3(R^U9QT?NUm@GGW08f$&+Y>OgH4UP}Q7v_|w
zhk30pl!0Y=Cd=gK&jLT>;TxBG@olQ2HR6z2)H%Lcb%Kl|6K**_#8jP$i(TPpihGwY
zr5YCP!|X5bB^<x^5@vsmvx_78XzZ4_#BqKRzMpmv=XCDXob03bm7I0D_Cx|WC8(!+
zSq1jfixaFWU*L&`_~2JN{^jp8Ii>kN{;3O&Nh3IRwD@@5SBJ&!@-rDaM7?VCdZ35z
zQYlY=Q7aHc%^Bt++4vE;5kL;>Xk4GfeCOfe(ej7gfxWZ)m1+1;&1lHxkV33g;^>4D
z?A<W&YaxCS*K&MEOd|FX(-+*H{VF%0TlQ1fXNeCO+FB=2;Kw+GehEzAxy+BrwPDr5
zGZ6dmTUxiE)Hk6#J=&`x+hvT0eK7r4y8qN;gioHGB@dCL%ZOEW^|;C>zTO|>8BbHD
z^zH?RegL!7$X@j!UX)BE1`8Jxi3ju5xecOhS(}!x4fbk-eK;wyAfn;mis1#?ZJ4P`
zp4tA69LYqdBU~E&ZE(?=1v<9_ED2x_31IbC0ze2T;8V5h@&O=B22ymlS^#R(JZbn;
z?Lxw50q2Z2q<>s*f8VDg`V1g_?m|oe!f;S5$Yc1#VA3te97)feevp6-&&TL$Tm?^X
za`x>r0h*e-jPPlL3rH_CDKvpOdiwnF^p5B=6=1zC0Q?l5SOO@%ze~c9TWJ8_Iq-uV
ze%$WpfMb4w_Jr$WrajU2;%HNxO$(W;&QJD$b#F&MZ>zWC8+`2*C8**ZETKby2Q?(u
z!@y+5CyVX&YIG!sJ1k$@ivbGt;@gu?qIGt>mF^$y0m?96y8Ug3Z_nLLDBPxziH4pz
zq!&NKXr6GLwKRD#-{pU{{7U&)Y@eU1&i&+eaxyy1gG0VxMQ|K6zNU^33i_w=+t8z!
zASjH)L;kDf(Q|x^58{UW6U5P|Y;(1$jM!a^FOv5R`BmPSif<F*D#AUod*D9&GtmOy
z<-taAd=i{D@F3#9IgDlDctpMfK`Qs(tSTeL5mPf&j5tBX!%=((;oA6oHGcl+3$W-P
zzi0&iXnB5!hH)&8{)7*nyM$;ondj|1Vt@bb=<%EIU&rt2Hu&B0V<PsYh1-u#bK)la
zmE_-pY5cT3V4H5rZ@>2A&A;CaQ=Gz9C7ee%Cair9qrfk`e$_f^raqc}QPrB^)M0Xs
zPY{8@MQBasxVg)C2$%f$F$}!)_tO4~{+>UY{(hg?U)d4+Lwv{J?rX7U5ub?HYy5S>
z)I|KS%Mm}n+ZBO^6k-BKF&v=OT#Iamwwi%hr61p1u)?qeUX$@2!Z4(0@LA@N->ocU
z#SJpSIKDWdPw|Y150cRWGR0!v#ZEn8y|jQoN?3IgvBM)IFCdDO{GOtnjQUDAVnI0S
zu0fJmthEf#%jdiC9@GG8qxX*C`;Q5&7?K+D_b0su_V&*%#{NS0?0#yrneQV_{TaK^
zy$diZqC9*_dYMvlFQCLxN7X1(z&n*V)DmO!H1vFAfZfVnWC8Z#g(ZL};DJN0$N7LF
zjtj?c+aJ;1L|h!w%>DU_&xL3ZE3aH?MJ2&E{C*xkt6Ap4=Q&#{;NO=_$OyiKyursO
zRSad>Hw>;SHB5bD@abW_tiPavDA=H(0CBJj$qTI?ln9?yU#DsZBHH?pw$7Yii7!JY
z<a#;24J`vl+??obh=}l!2b4|3s)-dx2KH93<ca=euFvqj$Y5Yi#a{svsFLJHVgRuR
zRO`<`6$B8cQ1-_zD3mhGe7mZBgIG1BEw{!;td@g-*&Mnn%PJzSZye*TsNNJrgEH4%
zG2ZHmSIzN)7|)mll}t#H6doEx!{<Dgp4%zMI)*2BRq*9%6p@H8dfq`<(-O%qPKC!I
z|1nL;j~#R=HJ$N#>|iIQmb8QlqZDTi&n9OyN{7K~eEu{NALYgZNMl*N_dR@H_l0y7
z!RU(F7oyMwroRyp=$yctRaHTJ)t0lH;90A~u7=bJQeTlIz*+o}x<i3IvoBY*796iv
zk>ZL<Szx1>K1^6$el?~B?$qPP8~_Pq4iq`QP5KygE8{cI=b#aS{@kYiFjX=8d*=9n
z!T-QW7ouA{^)`yWC&DL8SY7eRUgf9Qa@wzc7SG0PG^WDKrc8SxF|P+7pEa?r2iEA~
zlxh7s;al1Q`xas^@#zr%p-001&8pKK_^%LvPG|s}N)2Yx*pdz!JX!}lwh|A41UK8H
z*0tw9lf@7Q!B2e{at!t57ceYPqwAo)h%Cm}4-DPyo_!2@@&$_Ll^=uf*px@vH$TcM
z=O6sOcGpt`;pc#)_NTtFB{oBZjAwgG!e93zlkg6I&|>EwKtvlJNJm?E@{a<Z85jzJ
zup1-U@bu2jg69V&JR89$AqXA7GXwDKE1wO5c7M2!+`IUCUy*dhClvavzGXrLArE2$
z%r>Lry*%7@s2V+i!i~@9)YaBkpnR#iuxNP-6E@68@B!L0F)5pmiAkLrXs9?97M9;Q
zgIN<N&Lb8K;smK>mG5wCTHRs|`h<xmsQl>30;}&iv0C8H!YYDBCN^{MrkQ`6lcb19
zemn4a>l}&CF5m@OIRZY|2=<-gv-*1ipW&dt&81Va43ANXMu*3z3rw`ySv6FxD~}<G
z-ReOuv1fS~e(<^^CwVN`?D!rxD@oa~1Pi1%gHOxrYJS-~G0aPP;X~&>^RM)#iD3oi
zwU~2dfA5;!ULH>{9{H57D88?&cp(z?*BQGoML=khC@B$Lz|MaB1H^x`F>WdT=3cuU
zms;rnB=ZjK<dd88jmr@a;@bb|4|2k0UZ?)F;qgI_vx{(|8GLLNnNw{nUZ}g7lbKlk
z1ayPRmk-AZhm~PptJ>HBTB%PTlBbpC8T(Hl7^Rpz=Fx>R=%>-fp4Q!LY^=v~jPMcf
zp<RKxn7^1$3I~>m7B8X&j$FXQ{`^?_Yy=Uv<QL(C>GFH&4{&ZB4!m8x2OnY2t6e@5
zeiYs4vkKT|4*Sx*4+P~$+Vj{L0k#@gHyhQl+p11OXZ^Vok$GNdf_oh&=wiOkK6k3I
zS%o>}Q;zse_nrV!9}O;)B9T^g5_(0|hy0#9!4ks;R--?BKVD{Mb9pu}P&O>z*Hot_
zN(G-&8*nZnSd{k>S+onC;+@>rKEahpOqUKYw)P@fHyeQ;2;XSg$J}GkC^3xlKm75?
zKkdtwodAc_6|kX5{3eD82vqGv=)Xh0Ftet7<-cBnmO{SnLdu2tO0C(LjGy=-PFs<o
zL`bP2Cxw)FwNkI~VV)2Q$cVRHp=iQ$hfw6V6^cjELJCD8c{tk#iu)`+3c6>=h1Zk|
z$PO}7ZRNt-o?L)~hm(tfobnrwC>IBgr(A$~r3H`+95R245KGA~TR!CQwtRG+Y9vbA
zhzIg{iP8d)KS#<3W*D2$JEh<}=q>cY%3p+R|Jo(29gK7yCGFF6Pgpxw8;>b6W}(_h
zZ$!hUcMYD`7J%`E!Ng!8HNt*D=w43R@}$l1Xqe<`Bk>ZC<+m14(ytHekM0U1lpUc@
z$Tw^6BITS|O2ZKxTc*8%Db%Y(At$H)w6WS7W*QP~=@LSekKQxPBfL>atM+J@AVFyg
zsl$M5flz&etl=)>-s4DzA&4P%;%pUV!Cv$aL-RL|V@&76_@JG<KqH^=bKZ!K_Tt<y
zZ3wKzE%}Tce~-tkgAu_bEWs)L&Oi*MeNm<*i}oSk$Kn^$o@f<0NVI{U<1k6Umr>3%
z?=v$Pi7v(0T@j-X*76p>*aSv<#DVwqRcE177o$f;M)cm(-U<aamajk~yn%Mr?_2!<
zIA(uwY>69d%*Z@%tkzY%b5JY3-&)!+18DOz_afmN(Y`dap73j&1P}2gAsp~Nj{HRs
zU$M>I4!a-J_K_RS!7)K?m6ehsL*5bAU95|NHN*PHOX1H>gzlR*MSW1a#iQXg<9MF1
z;x=0X%dEVlGW4L)8Bl;RINlXJ`TuBp7x*ZP>;FH2M56|GsZe7@O>NM4qoiJ%6wqv0
zU{e!~1qCV!6>G8j{Yo_ftdM}aOL)32#CmUSrLC>nTD4Uxiek71yimj&UMi?mo;84Y
zL%fmS`!n<Gv(E-fzrXMQ_viH@`^?OlnKNh3oH=vm%#5C!<X^{8S%}HoP&-WD#A;q-
zV_=v2eAx6~I19^WLb}Lym}epM0RJSwJ-Y5TikHgYF!=>KOnrCdH+mItp{)AMbe`qM
zV81Qhqu<v0{k9;`Z>#^;{nlaoO`X;`yjuO`OM14)c4#&ZY>!PvRbUjD0g*olayxPL
zhXLd5EWr-eWZN5)?S6K@6H#z}<}SM3v(M>Ti*gBWbnIT|<1vk)-?P8wdL!GD^q{&f
z7V-qva0#rZjJ(|G^QC9c$3Q=8fbNOUo==8_i8=ti^7>fK`!lyYI0Dg4)_ng#>nabw
z|Ht|d++4*J`Txg!hnA%IE}Wb1?vOHQ=evZ2dT_q;COLnCBXO_uT2~)tts2}@M}$Ld
z2g4Fs-O7EAl;Di#b_mpHJ(Vp-<|a5JS9{EFsWUab4GgMAf%)YY^pEWPGHaW3p2IO%
zYi?_ozJseq4}GV+fNQhi*4X6XzPbM2vddwh`TuNwt7l4ET`B=4ou@4GNp>E~&U=_Q
zm&>yDHb9#9hA<!k@+~|6<<^(~pXa~l-tzhDe>eaABQXC(oEIZw8k}d&jXeQ<`PXsO
zf0zIx2;iu5#olK?Zo<|K$d>1O?)3+WBIw|gx7F2b3$^W_boj~6fK4P4$j*Re?qoqv
z9$k{10WS+{29y}pSpa_qJn=|L(XIyPO$pKUIs|3sb?0RIJU9Oxo0ShJSImF1lLaTh
zQf~s3T_Aq~WM^}I(;BJdWT_cl+G_9L7HTqEu}M}`SD1;`tZ%&zc^7e-Zt|38gr1Pa
zgp!r1Y<Qaa@4)Q*SC&CV-pl;gy-D-mORSunk8`t~`i1Rq?q2o<q@4gq?qvduI@ABt
z3|I&rbc|*|e^&Dc5uNlsZw9RMCbs7eF#YGxmYYIA1ZTi3;N1e;qw8*l8fFG`y3|XW
z0<RdL&f8>q&g-8@px-qA<@ej#T))`~(DvKvmj7kHxu)Mtr|oV2+t_3N(}Lzd&3{td
z{?q*T2rGksJOBM>ffg%~Ktufd^Iy2?-_C#j{TzFr|JVW5_H|(XD}Ln-vxs9i5Jk7R
z%Z9?JT6(X~$}jwZ{P9$LPagc@0DR_+{rA2<<#u)E^1uJjqpiPFyu;L)F5dXG2#;Qn
zE>1jSJ}bfG0F|7cTH=;TG~g%Y*EGBm%^}slRuR6wQt5Uu2WIiTC0if+gIzz*?Uzx`
zPxq*QVy^z8T>TDkrv6(QIOKayVP~UV?}9>R$9{_M|EP)2I$%}0HK2iVXXm>tK5YSf
zGJ5AI8%`D1W${V3V(elG<BZ*vXUR~vY<*pok7ctBo5-P%8CuPIQE}*8#it1c>mXKb
zNHb6o={}bj2o}V;QjIPhUG4q~hTUU+%K1CVzb*6QJpAsVYBMM73%~jxe$z#mbg}z_
z@Y`rnyg>k26f+MO8`8BXGTrA%1DJ>AN2u$vS9$cy*+YBaC;uT?oFZlyEWULH4{as1
zQ=~04=gau+LS7*!$ZP7Mc~2stogL5ZiqxzKE%=)W+!p!d3nrRmllGi;G)&%F^;-0}
zO}J<F!n1jSE`YUqdVTtGZG^B#B#mHaZ$@NuiKKg%K1Y<u51I>_@b}q_Q#t!SytrFV
zWFk&A@m*{XylXzXL&;yM;x|;x0vaippAzG)hiEywA2FE(e<&=zVuszai(FvVRA#=5
zrOpjIMZe^UsYOK~Ipe@+&8(8pf@jt8X0-g5T4n)K28gpGq9M=PO$}rD7OmM4nvdgO
z_XuZ8#CbL1yb&qfXiF)M*6ePbN$%D=qU;Noy2C&U%kd@sd*y7B7g2}VLZG?yZ>Q_h
znR<)1)I2h0>E-seIZq_#5&`;6`>B^*IqxGU=w`-&jm-z?!jP`g8zaoEhLn|=V+>Cg
zxPK-K7)8Gpz^G7!V%2LZQn}{=<SfrDHOks-Dl`QD#66O=8U#<iL<|I}xu7I8-#%iv
zq6o(>XE+($Ix_p%9z)So_u^qZ=&KqIC#P!os7N)40p1ltbmm+9X3jP>MzbA!A1fCY
zIybr>1MZi3G-Yy!r0GZZHBzXci3)sxqHKF`TsO7NHMN-sJTRAH&e-n8C6v;$*w5eT
zPUpMowT!Nyb5p^Z^wLje7||%H!tr!)l#+V$!NQp(`EvIJqPpH@FU!iS?07oY`FOOq
z*RBzD0R_nw#Q7ZNN$A}g@$`J%6G-yBRUlfkO5-)O;4!}ICvVpfQkAJfLmq9Imbfhh
zP}=y$&Q{X(4eUp3yJG!eqxVtqac9xZ`f*B)sjTM3mg8&IwJ@Ia99kAIReHTsiqQcN
z7kzn<tuIBKRgvSi^KgsZ5@sZJ#VCJx7_-Mh_8}fq$r>o587LFrNl-9jp^R@82i$K}
z2roIv!H&W-Tt?LFZn-+*ygR~qhXFI9@GV=;0g;+zq3f{66hvxP&Ae5^Wq^Ay!hp{F
z_E!R(c@eedG3TSW-D|iE2+CQ;xpjHS8|s&|H%xw~Zt3P;;Mw~1%#9S~&l{u5G_Jqv
zlpYPT8q_T_qRvjU1AW{oXGft6C1SUxirPZ+y2%&A)y>fPyH3&3YPs{lamxzd1$kcd
z#u`2IFK#1<f&JBeEwB6Rs1chTTB=j^xgXM%mVmAE<oOr&JpaOOjnqN25pMi|k0*#r
zf&^%}1SlN)p8N3b-6oRp^Cx`{Hv;ql*hvBulk{+3fUc)@BS32m*C9aPCC1sVV$NdG
zB>~z&jC(y8`8ZDjIP*Nw9=+VF#E0qP+r#_nnj-<qe8d=LaN6<U4zc+z4(7{>uq;vB
zcM1$KGD~n|&&n~ipkHO?7USD)$WQqAjNW0zE-L&#ap$Ai`0{9FVa;Vhd9*F$2o?8H
z3isQkse)#VK&%?38bA{zk1htt%A+3$VQBMxwdF2TTQp0rBTaqCqaOv!{4dJ%<<Xg>
zP{HPY0XpfqlROsQ*}YrxwT{wb>1&KU`cR@JeT|VvTlmh(qt5~Lv7fA78J3er_feVT
z(S8UpPage>sGvL=V?Ex|msPq81)VDXq0tt&xLOYJz1{C{m$@o-tN75;i{<!A6?zVg
zrIq<TFkqdtLZ7Fran8TGHu>^SJBi7iGa83XFPU{_a(z>)+02(qN&GYQleIgzr_u_j
zvi6VwRc->vP$es+H>pBxp_9+;ZFAfn%rQHg1F2zp?Y?XdpWkBXQH@n=oDX8oDmM%n
z#U1mMt;3YuPI{&DoK4?>sAqQB?A}5#K8<zjYgbj4e+~Qxk3x4UT;)Dtv;5V)IGE*n
zQfb`|#FlB0cKXCrAs;>8-^@KBn6XWDtqj_8&mHZ2=+U$UnxcZuJjMOwOGCZ(pjgyi
zULH^V4j{XF7X4!B>6Ol_?gePNY#G<Z(#;~vtLns05wbM<d3PZ>{ARDTZ8$O8hQquz
zoEa>(w!pnCm}!~tUlYV%dzYpKVYnjOg73Jel1u%MX!L88bjfSMCUIhXMc6Q5R!*m<
zH%6QnXv!~8h-k`LG^H}?ylLBVl5NWx_XYC$B}?t3wn?5KUXpD~Yp?<<XWU-}Grp|$
z<n@Qw7uDi#-V5R?70JuFz{xc*-S2@^ulTO+P+}%ANZIByI&~;a9(cwVq}?4t`QY+G
zQ2uQJIIrdJeC~06yL*s@zcUEGJq!Oe)eB?&-G}FV%_`{)oxQ}OSuW45*UVlrcF#Fm
z<w$dJELAj6yMZw$QL)-&Kr2y;;sm0pA5ThMQ7y+q93ER6-4J6?@<}meZXuzl{7%yo
zG!qs75ELI>NWRBZ^%8v{Hk`{Vm>Vl9syf^qbdCf`j6q|j&RDvC6Pa+zb}Ni5!qJ_$
zONm1x>r0fJ)~+*{T^u{P)S%j2%FHh04O_U9Z-M=HoEq}>LjwN2#WVhqT{7+Oe)Oqn
zN~3#?NJpF8D=iHfVjIuDAak*ouD#E3S&*~!K?6o9k&$zU8se<%>e8Qo8vi9N?A#wS
zRj8QEk-K*H@GlLF=TjKW?&TDfe2r5{sG-gq?q7Ban^}!~{!X7Tjq?Uofl)66#U<_T
z=VXA`Hy#`qIhR5W!#`Qc`9~pWEN6T86UduDx~TJceR?up$y>1ps9RIT<JTU|W*7(W
z>^$%->aR~<X4H#g_;xUKZFC>ECF7_cnGm@kJ2jj}J4KNB(${3!UFm+~GqO8d+{5|p
zdQAj+Uwp}N6ijl{D!m1;MPPr!SIv#|L!<kd6_O?LwOih9ydTJuhwya6EX8*}n}_h6
zk8=n&^BuDo|3tXO<)$9QIDz3=7{4wZ&0_phQ5K9Z<=4abl6K*1aFq*v%_mKVyN{Wi
zh8W!<hOMQ@t0g9VbNQa*Ext9(OVu*LYMNtpUA4_ycMcDyitg5slT{MMEq$&w1q*A|
zwZ6^i*q``tqr>r`e`4J$VU~qiHggLwBdfOM<~!6<;Ch7;((|%#>skeOWYspKzf)h_
z?TvJr_H}LkQQ45;B{Tm8k$7g^jH(>|QRgk&z?11mW{I`#&uG6dS>Iu(cOB3C;=>(e
z;6jOSA@$s|g0QQBg>zp$sK>1MsbzF4=KXS(LtDgqB^1f90g<)u`02m5=>wE5E3o?(
z#rSxw7hb<`*9PNWCoXdhpuzd6YG-Erhr8u_X!X-2q<3dV5@qLuC1fhdoaTo=>~TH?
zGi?7d&0_mY2a!?sD+-O$l2gIr<s}LlTQ<QyMJBb>ruH3M>A3@Sj_L<I8Tw8f+&N0i
zOfn7d${5&*K;&r(ZUdP`Y5bnLk{EBhoZztY_EWIJ77^}$zf#*~6I5#eIN}e)Q239l
z`Ex}rf2L33&!`xG&TiD7ar|kW!k@8I`7?0_f7+xcb+)yuRQ^%Sz)MUq;gh`IQ}x@~
zwvbe>L2bA4-PzW`U;XQBTftv1?XJ5`jHVgIbhbT{OS}I8zNzyO*_M!5zJ8REeh~<3
zEV^T*a}4iiN*S(KN}x_>d?CN;g!6;>4g9gdgMRUoA<!LO1Pni>OFD-e&l$2058L9T
zs|l0gm7T-27yz=uq{L?{#J|qCNMhp_DuREi(~RfLkbHclFK`z<kN@g~_`HE+OHD7=
z8ecbh$`cYdfl-yAxE{6##|0u1QA_b6cqJ77k_IVOWc6aX5xJ9CWg8rd-);fUuJ%%&
z3&nZc#H2Qc;-`C`<3e%tW)nM!jU!X@pixsoZO_`zu~V=73jTrpkAaWOu>}YqMhN%U
zJVViAXG}X#aWhOR5r#QJaV>RH3q+6Pn$WyawzL(YHr-2-x@Do_*N5hPVzFONefFzO
z-93G>%29_;zY1a0)Z%F?hK>-40YF;81BFn_S#K7RV>>*i&!Pe4u3beEsGf9baSr(_
zDl_*h2fbpe0N~UZ0OB!KJ}pelu+wbJlS0D%?FX7aioc=(TV(n?^5}ol4l}dP0YaMA
z>+hM_L~D}SM5%L5+EJ9N_<r?YhCNG9e%R3=NP}{QdCAyeRr{hqz9-egnKh=B@>9!=
zJDcwhxqPS24&-}ep7LGo=fhiLO0KrA2jeH@;&-#qwEQak2M5Z#VqE4Wo0t%|+BYm5
zNN;V-{JBSZ{~f(^?H^m6ncXA()OCUM>4P&3J<<n05J<nv(BUWx0v)LTi1tAG9frP{
zFS7Wf&7ekc=4COgD+^mAJ(j1|#?*x!?(>?f)59h+`_!lJtXN3f+$V`k%|hvU$Gwx8
zg3f<}QrHXKLyg@+2~{cq{lKv$9Y#s+EK@>}O87tuPrCT``zYZ(qb6-J`eSMq=I(dg
z$8D~g_L1vRFIQhv7cO1zxN~i;wteJ^d%0fY7vDMMYP7ko-bbzmFV`PTu71k(HJj_Q
zedIdS%XNjxRi<2<K^AIUu#a4C8bxZ0TSKmBs$l@&n#Za-_N2c1um{`6!fLtkePH*5
zq@NrQR^_K=RS28&EH<O|fz4<y*Pr#99ya6vdiM^#VZafb?<ziduhp5s=^pCky~2a%
zghlZQ`+(mfjR#IT!-MB`oZ@fn1O5>YKIFmI0)O~E;M+X-jr<D#81RSe1AeRrf4>Lc
z2>e0&fIrrQZ}s4H#H05<;6E|i_!jqU<x3CSh_2)g_wr66?|X>hJ-gD<cCnZD5D$I|
z@LTo)pYq_}=2vZ>3jD@>z@P8IKj^{F0RH8Dz@Om3$N5b?F&i-=1{;B8k&LUz`0vEW
z2Y{&4zh^5lRw%M8-bY<edRY!7OEi_%G)s=Z?llLD-|m6C{L=KKL@Dt1>=oWp_X01=
z!~9aiLYw1`z2=~XK_2*AuLcQYYPe;u@V17}QJvN7(Owo!zGS&>A6Zs=S<1aEnw`lq
zhb+dlU|7YoFKn^3+~j51rr-3inSi=)@-6$pjTM>#upjix*$*^JSo^{GJM#3))P7KI
zWTmrzYGjc!s)$?C*tbuOEOkbeazqYq+SJH?&ZvH=k!3i@rbZ5MMh!@fEN8DhHL?QT
zVw4;yMh@23U}qFD;gM3gNAV{-vQ}UG36GTCE#*5rvQc0B36GQpK5ATQ?rd7C@Fa!#
z6CNqGd=!7eBd6+%KjD!x9HZZ-=87p4i^d+FJCA^4G<Kqlik6zY(3Uq>+@M(fI&F$g
z&F$c?@~Iz{W`DzT)kzB5-|$@ZfYa8&-_+a{UYd<lC^dIAf1Nh{P0f{7?-<=)wwTNb
z|H`8;YiOzD+-LF?60}+lES6=bOtqT3QI5qR(eVWKnjzIqII|Vs?tO<czof%*>w})_
zF+elVAI)-%4rWPYmSNuVi**%oXmO*$%l-A<-T-6H^sonvTSW0~l;?cg8C&F>UDBhB
z+5+cL=j@^=JQB<lSg)k^%V#C--IFT_Y$OKaJU|V*hP#@MOnuckG<9APwQy|2IB=f_
z+}LpHyfSBOna-M6eGGgK_|NS#RxqPS8++fJtsOKxPb*UIX>b&ee*EZycy)JELo7Xx
zO{I$~!m>Ef54{<3zv;!pPfn(XY&J%s<z?6nVos3^dxz2`vYcGOZ~U5F-OWqwxKkzh
z<4$<kargFC@kP76uLM^u??@}Q;yGJq(0NhLo5wiZs_|_$IP=h-LTpvX@O1Qy-C9=N
zOfbHy^07*ae?osk^KT$Zik@n;0;8NSVR;WVuS%S#1e$r$_ZJ^sz=;j4woL<THZ=V-
zmToA+7hG<y=$|-Dl@U87RVZ(5fSR{Km$}Fne~qwJxM(kXe^bGGZsj|0cIu%DAxZO3
zF=EHr5K8Lh3XGR(p0!39{{t$^@pHrE*Un5AogbgMGF;gFSp4yd2LLW?egMaW(7Z$V
zu1|BMw&_%mC@tLEWK-`T^y;~^q|BYYUER{{ZqP3WPs<uuNhKD5rK->sw_FC!!C1;#
z%gPy0Lny9PTUipc;hH`^*()r?uP$8+&#oTPG&F)c0hT3P<%S(V5$=DH8tEKi23iJP
zjos<0ZJ8#sc$rPbSjpX(^KRxMVj}SoWrdmN`7-Eum<$PEVvsQCc+%cra?du~tU~I_
z{FIdzm0!nom!{E%JnlPyWYI7!&tQ@5cCUC>Jg0xYeZkuEwS8RPDh>2r)*2C0Z{*Q&
zklS=@5s1gnD0Fsk=v!udv!8P6AE`6y=#)K#WH0Fb%}_?k|EBzwljE&LDg@^t|6C}C
z_Al;H&OO<3jK5C0_%Kl7@U^jXL>`@*MXFfTq@c{BZ__GYKA8PC)1DdBSF)G-zSy0Q
zcTJD_?qSg9>YJCXZ@+APb$RurPVKvh0kogSPeskHCN>8~mT?;KV7tD=np{@%SyNdm
z%$Z@f0Gqet$kYl3%d0qYTnj(&@NIv|8IA+g(bua=*ingvqRXJN#vkWxG4uNJFLGXk
zvx<J2%!X~9^Qq*<vxaMRaQ_9<aWdH&e$jGpov==`L6IjuG*cm>uxC%`qSF{s2-{be
zsGr^MZIa<Jl$3uoY!T)h96$Xc^fbR^5gQ&5alXpZG1=YHFOoW?BDu4rG?Hq-3NoW8
zx&G?o-7#;$r0L?5K+_i#BUP&e#cV!gOz3`7W2xu<Mr~k!LoQ0Roqe|6w!qhe`^ZDH
z1-5zx-baC5e{tKmz{9go`2HuO>z${eM;YBIu`1ry^APwj+*=`Ss2+he$wb0xf{^r*
z;5AZMT&Dnjgoz5tF9ilGFxv}MD<Ec3O05FzULcl7`VwM!>La;^i;a@onu?K0%|CDl
zf;i;b9o*d=pKV=sS7RML6Jakpb$f+ML9`AqqBY8m%gK`tr0O{<)V_dh<$fgglnR=D
zWA>ZsoTuxYPb4QAVHURlk|sMO9~wi+A=t+vse?FYTcJGV>{QnhrSD3<Pw~D7^Id5`
ziz>2y#G67Ya$M;C4s-TVJEsLPotEi9=yb<IqjtG3{!=0tS<4Q4EX`#hH1^MIts2&~
zV$SKT(>{B&qR}uf(`tuHdtt6vYDm44aXxUbV5C}v$2E;4)?N&E7`^Ro-Wafc{}%{Q
z!2?ua_$|vG3jl(Ena6VSIFi~wcOu6$gF`(uO=qyYhO7%Vq|`Lza=cDFdA$vTc8t@D
zxt4h7%V%OYsPVjGMI`lYTEOWA{LPRaUK>KmuhLupw;25Vb&)cF!Bt>T=j^vv1ZL$S
zP?tL0Jt0tkVl9Ls($eucDWsriL(5IbBk|t}xO3hJkXR(m+?u1KJ2{wop;CWFYP)+T
z@$^N7?EYD+;$)KZ1i?<Is*d@(Z}-X}@|_?eCkc^VK>KA)Q&l0Vvhw!-mYPFfv()5L
zE2{|%!lbXk(%6HZb@VYjc_O+^ornZ@qt~_W%Z%P+2My3z8rO7=^aI33wWZ|MaIH_|
z%Qe~fM8433JR+Y%z#R~5+dQ@HWM~pe^+qJm?-IKh_I!*K{&^$s-*W4Puc|iPjN-L!
zJUalUb!N<YT1BC0DvdiE>Y65~0p)=vveRGfzHcT5+r-HpgxAEIVXFK_Cy4PIJ>4W{
zIqOmr@)|vXfO{+~?RTi0(lV!G;8a&?Pmq6JJ866GU|aJ<%$2VQhcUWi#P(z}ru$q^
zdTs4x8ctg$Lv6pUh;`GQ-;SgXQB(KJH8qqt68Wpf0pvCJE(B*j0dDeQa=jiW#wWlu
zlbj{M6ch3Ya6SR|7`V_Uz;9@9X4vOQPSXpWh5{CW-1p#^_l<n6MkebTgFJ$HClL+1
z7`Z0G<zIYPyTNitb(Ugd`|OK5s!YFeqfUS^xYnZD-M|p{MOtrB3#4j6N?pmN1{IOs
zy>mSYM*Dln{TsaQe%azxo5kzt(tY7|8x-_=vQ}f#@6Fz(H>-pA9SDB9*)V1*=d4&L
zq1P7bCI^@;dy2CMy~5ySP%>PFP&)&>>YSpA9-;#7DmGngdCG8HU215Bdq0Dlah&)L
z6>23m5jIm<G!(fZX<=s4usf0j#TgGv!*pK4N!f%*d`LmdY2pgB5RWaxrnn%Z?q<Tv
zXVk4;OpZ}sCB|peXH9aJQJ*v+k5T_dz?}ul_>B6R81+WlvR9#-H&XNF(A6&rt1%Va
z%40|BiwEv&q(17kd7RhUypcKq(UIl7#Cu4Me3E=g*DEGBcDuY7#G6O9SG<@UNjiz~
zN%ELU&XVL_6Y@xM2LbmdaD`8jyG4>Yyi}oXtrm-g-K&8zor!SaKv|DYJKq!-#IVkb
z$zk|yFD8fKKw^9h%S>_>!vjso!|+Qu&RzXd0K><H;l`b;27d_YEcY2S*8^&?HjD=|
z9_TJ1&O@CEs3+<RO^zVy^SziH>NCBV9O^$I#z*}Elbl8UY!mWO|1JTyPZ0H|g?a_3
z2iC9X_s%Dg^LP$2a_$J-3WjssxiNbo!|2#za`2C{)^oO27NhIbaBVL(6+Y4Zn>aA|
zvC?&i-HdwYx(ZFW=&N6{qMeEZ_T|_HK|E1$HyN-8-d-_V09h%VsCXb7zau|h6$HRK
z@`J1MgX{8x8*)MSH*EV#B_0=bc14|wD=v=~ZVFC-8-%Pm9NFYSatq(eoa$=Q-Em68
z-5r1KcUgAnj!FDi-LEsC85)-(kUJwp2UK30uiWHgSp-H==QBU+O7|Uj4BN~+67a~W
z<;UK~c%k9F{&(DcbU|nBU}#cc?y-cSrp8iS!<f1fiB^XN<P^N-V&%@b5@%egGmce(
zT?7IIy}w^x?@Z!XfMZJFrTzy~U@T3VL%yGz_1BzbTExbj#`2gGt8_OT*6Dw)sku@$
zv+|~bt<Ja|_0E)CCik3WgVCU3j&A3@oNSvMT^o_8FpFew9SWVMn&cD@YUqmTLant5
z#03I-RAzwAS{s*6RD?}|95uofjCG%TuNhPiRotpxOfOclI}aw#exJAwqCZqouH_WV
z@00#Pa7;y$9HZtiGf_)BIg=e6jWnzox-n59Ia{?2kA6^<fYFt1myf9GjxF8PTX72&
z2QG#Q3E%34x9XR0yB8MH3P0e5<-?({%#_N%%Y*OWH}gjlW#^$)W(cSdwsz{_n3&e=
z>jVtyjk5HauFym#aQ0edS#wUTaMwNupX@q=I$j4nlq|-=AWj@-<c@ok2t#m^4bDaj
zlCUOtokfzp3xgb0bWniii&#B05=W}4a)FQ2U6IwgcXF^yQHXkG2%$EEqAv%lsiqOa
z3e<4Ntj8VGy<x2?%MR`1ud5KX@Rlf<R>Vk$RdEQV79@Zr3`qqhi3Y$5*(A|0KS|@)
z<Py2_lOz^Q(hgf!pg>6y9TbW6zTNh5?|Um20u>#CoWeDwB#V?}31R;CMZ;>K4&gHo
zP)feBVm2Rj&g*r~w))<8R!jxBo|0zpTgMTB-VasG<}+5ff$TSW48w|>eTZ0ZOdh0R
zXcf6s=t6>Yona$_VyjT1i{;4d#|=-?q5mW#a6>Q4BJYdzum~qJxSqnBVBU;5MHTKh
znfL?jka(Zm(uXoCCH1(+Q~P%}QrzPnQxSsyxRKf}rcN}v;Y_P76}v|ebX^9KKLcM0
zMPL{48yebQFsx~%DQna9fH6$Prf#!D+ng1ZYN5Hl(mM;+u6C<0J_sFOU%~6O7AZ(e
z%!S;a-Pe%Yc0w?2pGZ$Pd=Ao#VeHt688XIRM5H-lW$b+BQY-gJtzjIl0D$>u0NWGy
z{vJ^psU<*jt}bB&%0CrTb<BxWlU_l3EHw^-O~FTbDx?~XNINX%oY<Fcm>O}`InTMH
z7>2+OHn3LGKB*$@>CnD;heIPJG3T6W_eK;w6`&!cZe)INmQ&R2Pf-*{+syrwY?8!!
zk0bGR({-}7my(YsS(_MS@42n?x6!cg_{=K2|4l^Z7X;MnADSi3|ESkbGa94c>&Fsw
zKX^9K>r?3UOfgn~7?&?fH0fXFGMS&OSX%C0i{zEaeb;c1WiK7$NDZd}M?o+hiyihm
z$O(bjqN;W$J&pE*n!ZRm>=}>-qU1LF2ck^Bjg|`=+QmZZ6Ym%qi-FE147X%QCLbCd
zCPw2(V`hfntqhK8e#{EsDEH&0Jt+>@B&A4TD9<>hs3ZmO7q;aqKwn_A*8EmLs8*Ra
zd2rQyZ?N%|iWhy9rCb5SX9VD!gb}hz2`gYEF9CSVn=>?qpGIyP8D>S!iR+w07)T2#
zQWu~U$lhv{F>m9|Wl~fAMkGHf*C_~Lq%_aTNocbMqZO29Dz61ZH@IpecmgfRsaY{n
zh`ilS3b}X{8UP!YADomQoRS}$8VIVT{L_7Gt3qvaNcmoRfSG>9Fo)rl#;@p7>6V%R
zv`cB_<(ntG6(jEq-@idmoc35bzO<Sm7)sR+Cr*q4CKvIAurXRxIrX7M9nX)|qRI@n
z_aA?UuSKypS*}rLdcKDu#cT&fr<6Id5+_#b#OOP#L<JS7XMgh_6ewOia%EUn4fSa|
zC_#AeW72mR*m`P1yyY%t!%9>BV9>2gKWP-4yBLbu?=w4%g0l%=$%A^l6OkjQNWtkq
z>46n0$(gU<{EC9BUgMY^B%Ug!zLy9ECMocM7ciqD%dRt&(qU2(6|?nA*mA5L6Pa_t
zK<7ajPSP7D@!+xWHVx##ZRaf`MB(TI=u$f?-A*~7WTRnO6ba!!lcuTXP)WPp`&Zhw
zi}j^t%rIrzNmmt>tD;H-4jqCDWO`hDj^4^fiY&lR5&l?jxu;2tKnk%uGtG6T!THLE
zxYvv`H7Z|?5GR|Yjy;mZ)h0=F%Fm_2Vv<D9{3MMp?Pm8zPv`F8Q-b=2231bq_>#P8
zseCuaRMaN7F)VL1Jyf5T2w^z$(wX_dDe9Cm+E`5i%xCJ-P~yK3(P$6qa40>IkVr-c
z(bx6P`}NMdcoT`#r@%&mPDxw_s3wiclJU&S>c&_qs~m8JsGf)kF~dvVbN91?1q9uv
zDacn2*gTSQpngU<X#cFxR}Pv0(|N~k_Xo=<WX=i+t~}Mi+s~s#BA3w*PRU{dHHgXS
z2bO5TxE`4P$Y`HnYBYkTHg^mWnOV$N*>hg@QZV=S6=36C5GDbX`5Ch&a@Oh#+nLkq
zQ^VSR#pt$bgXrSx3zu>mUZyUsEATjzV)TWNerffE`qZSLzJN+BePOWG7Y>z>OBENW
zSX6O4WZv6{w(wQWt(x^UH#%m>c+*rWocS#ntLyGI)|r3Pbz@8>zw1U2bi>FFzw0m&
z6=Z6Iy|0}xGk1gh{%WO}wEwK`1B>2fx;H&KZ>gd-x;x+pJ733F%+~PhFidoN#X^2z
zWF&?8D=#BW&(OP3VA+W<rhrC|Qfgyq52jktcYD!;6+Kk|mHL{ZuW~ar8}UiV%yqYV
z;9;_gyvPN2&NF6aHCUKwHNJH=-a+P{zh562+u(5Dl6NVpImot){PpRFDjqVw_j;r8
zPAf#y_Zrlkhbpd9H8<MDA<J#m8f9FXvEXEoWC$h>^PoSeE&9i&My>wwsfOzxpQ=g!
z+$9J^x;FU^ofAtZjMh`1zRsiAM!l|CD?oiz>q7$5(#BIyq_Zdi;b`kO1XEcXh*h9W
zfpuO0p3SK&*aEV10Ob1snaUOJ%R|X_jdClM)HT}b)%5dMAxVKV6xpcx3Eu8d-GmqM
zOHeICKxF5isRE4wio_9B>QP~0Dr1TgYDrLFk^<FUV4MO{!<5p<w+PH<t>Q&=#Y-c*
z)y5Cz({znEhHuvcDOX^V7ce}NB}iB)S%NUhWC;Qd2ATeFR)!KQloPDgE-MW`FoW7#
zRvG!Y$%`>nnwYg-jH%beJnqGq%1z9lycmOliMiE_>G12CON@KYvcT}21@RHNM|^BW
zuFh1k|Iff=MK))}E^_u1wh7{TwqcN0(`u!dx(9eMxi%c=#cc9nb{XyKam!IA*E?QJ
zt_?4EF&#EnC~=9_XiJFAOyCRQZ}D-j0cRRA=A**ge&!?^XZACb%1y(LF%8Qcos;M8
zaGP&HFyE!wd@5V{ws(ji8QgUIc|csGKF%_%IzkL<Q1=*EKAxtd0X9O)u&ia{0OyvA
z^$Z_oIk#FRXE}GU0$I+j<lEz1!+h-qLskaF-^w>7mGEge7sgx3^vU3Z@y_%_w86&^
z`)bm}in&hvu@|GZ*ls-6i^+A!a4)9Ahp8gQ{o=_0Q_Y8dnLlDY$oVr_`Hrhcd@E#Q
zplU@?rdLF;#)GpF?bYUQt;kVnJ|UWhG2!x9S~TPfQ6p+OMHNv@fzESg8^`^E9%8m<
z2!9Z=LV87G(1kNKYD%8}Q)+BAHh(-g7pNiT5L=PV2t`~WXI>iHp1MViuIk{j|E9k2
zcZ>KvtmBh)@lD^<dTLMqPL`=-krZ4ro~o_w-qgdND|_CzjDyR`ZJr!Rw+`k!aTTO<
z-VNRVA_$Q=b+@V3*EjOtKckKDsgh#p#Zu)v;|~C>f3eQEI#U0f?KC95i?7c3>-^PH
zf#gm6=%0#^skSrzf{oxyJ+6OM>y@>@q^kM)*BLicBwy~$8jbN4Jf3#`1yCVMRPQBb
z+zov{Fz%iM){MI&_|W*d?+KfaT;6#!OOH8Q#f}%p0;j!g>r-uEaNwNfvGUlw&Ioz@
zYV1ktQ1U1(1?H?!W`jsA^cZ#+D7Xo96TbUx;Clw&90adMENs|S*wYgP=WjLyKhn|x
zTY}a@@oO4c@BI?yrGz0W?KBfRhS<w(Y_np^Z5<E1%y&OQEEbiELv7OmO5LCU?qSz{
zT8KrsUwf=@9o#^P$9(|&X6lpQRD7|BPr;wlP7<erpxa4ee6jk1gr{u6V&%F|Sf+=a
zt{Q)5fYdp~(~nEtpbu(0F<6`I!|7qye+YDwS6?bFg2p?z1)zBP(YUr9Vlot68!KGa
z{TA!7^sv6n0dCZTC%o<@!lw|<%m~Q6^sw6ymF@uIyI#$WZ{Yu``id04%#>FpxR212
zyZ&avZz_GW!oM@&N7b?y2xlhd<T2%3s`$qhKPoT2uj21iJg-anayUI~&JN<QQ~X_d
zc;2JFYEpc6Ui=!BGePlN^5P3sPE_$)|76Esdf1|!#Gk16QF-y#zejvO#eXv|{--ME
z3)J1rp?UF36~9^WReAB;;Zxu~qxgcn_&2u`|B&JrAWC!iT&?)q75~G$_`_6ALh*MH
z?|u!5nCqX^+10p^)oL`g#<~t~=lYZ8AxMHg55i}R`BE$QmbM&`;_hhtA>Sae_%}Au
z6N*b)-`3j<X543=OYGuGX~}`e*gA9Hwr+PeCMHy1UUuXOi6|1Ub5tYTSI~6M2xQ;<
z=NKlcy80Pa9o~;c@6u4)tA^i4mPFF$!t4FkbscK_Fyne^CS1GA0zE7sUcGTMf8$Gh
zo?PEKYK#gGB^L2xD!88yq5sL#g$0*HLWB07&NI2AawYeH|0tItYj;iWqw;oi^|9^f
z96pA!3XDhc6pE?Z#zjR!Hbq%oBYk~CZwC&zf)X}d`Q22i49Mh~=IUt8<ZA3G6u=Uw
zwzH^GWm)=g(Uk7g9#ONk=_+OJ$9wG2leFM}>Z-q`Za76HtZ$mi{ZveS;ologAT_$2
z?*V#N<-}gRRQ)>8UDN7q-Cp?lX+u@3-Ub|A8=)3zg}zuzO=ZnDc>NXc@kFD?PbM$#
z;Tmkgp&;{p2AA&VWvNxhUpxWaqZSz%AL|Ur_=|uwGTu~%jDPA8GXnz;y5!Ap9Zya_
z>m1%l2I$HGY1>`NAHeNCJ>_nBF{#ZHO%FDZw`yD0*Zp*hp(Y2ltT4hwv-5e5rn}~{
z>ejd27V1XI-?eIA2p$Je*D^XZxvjN4(`Ck**H3q=<$k%WP`z@GvE`n<&vKvpyUGnP
zVsZfmWR6$!bL+!%WssIL@E1&dI76o<R>wbwX=SR!F*r2;*WiE(VIB}N`bcyoOgz45
znNdiVW9wPhnu(y>X@1eV{R}d2J~@<}r-;{D{u{dYi9UxS-(l`op0H9T^THZNY$e(C
z%IJ7o%v<81JIyKvjvDCFuhRxwJ~2;VGoC4HZ~0gFWxSy$fusvquU52y*`UFUw|o!l
zYryIXLyf@zr`x`N@Syw+@btyKG~&F%W(E{m<M95(yXJBOtVkeHd<e9U^`7Axqf%q?
zHR33t<wP}h>^d6DiwYLs=!>G)Qrb42Pra&8^Sq(j+uHcx%;<Zzc6avUhEpjfpAMU-
zDm1r(e$(Tx1Nc=XAN-@RAoDN7t7F$^t}$#=Taaqg#2eq$tEqq7Ez+G8eS~ciQbLMa
zdeU4Okm&~pDZCf>Mc<@^EZuVYsFa$$>7(ZCjL%OxUF==K6gS4+-pD_4I=>E!EtdC+
ze_j-7(}g@J2WK!7qKe#VR1w?H?wJpvuuIV^aBKA`lPBM&nC}vO2Nb>90{8G7eAvR9
zyhZ;GzKia)I|A+|eR}1;V7~V(Kfb$n({T6iq%w}*d&jOueIwIv(D#<$NNtqKb9g{3
zubFdotli^3_bOAb!EK7I*L+W~_3GPiSKfJuIh`V0rXN1Y>aWo{qric%P(m)9x{6*4
zqCRevbnXEzBT}*DHg=GWE%#!LowJWyW@9UD?BctXg+jt^_oFH#t_b~$r53B}Y}(s4
zO%eJ>T8Dm2zD}E_2>l~%hki`jpKY2V^bZZ)<3n?9EtIH%MxJ4oJ0%3-UA39ecY$}G
z4JX!H2XheaL+-*EEHM>L{db8QdVtM>;U#dnGomDvSSanA`xax|CC?)@Wk|6GqDDAd
zxx%owcIH~{C&RhyCGjqved8kWXXXjCfl%US%8jdw)&-|A5-Fja5|)#$ti%1R<$AFM
z15sj8HtQqwP&Vu5i%nMfz^^7+Sxfw^ti!IVRMsz*L$#u6_*pwlR?{Ek{v9c}>TKg8
zOK^YBU3MP@J4w@9sR#9g^;^g~Hk;|0lv#H<$p=UdC0ae)+iX_TnW+c$V{B)g?pZc#
z2Qh$Gd*I_NxME|e2leBD9|d^IEZvYPW?4t`5MIgEz&J@w04cM0Q}QMgr`1HtEaL>H
zIHm0}X&X%1I`gf+skqBu!6{vEk^>6s*GVeXUHliwkbY2uB9?x_{5Z)SN;!w2lzQ04
zV9&kRz-_W{`f<bJdvBF!?lwYCt6F2_4z=~EU1n9z)f{3gta)mBmwD+Zl&nU!#^SbL
zvhNXvl9MEQd4I<A2=y0arV|q=uYcH<*Uv96bENW^dBL3wQy@|jZa_6SA1w3Mrw0R+
zk+%bx?09fbu=(oACs1D$9ZKFMM}US{;wAN<;ooGf(ZyM3k0>`tPO@S5Z9u5MC|Lgq
zJ?ozjP-alDeg+dom8mEVs?19m>CL>-ne0#`|1z(bDlERxXOWFA$oztMWK&qf-3-#q
zdF1x!G1}tOv?V}~uVwLJXqtBIrt>qC$>g8k>UrN(*!`M1-rm8~{jlk9b{TZZ6nSb3
zJX5%4Da1%P{-l_8zi4WDbyGhUA#r<qo_CBWssZ51Dc!aQ;+=-e87R5QzmjX7R&d#A
z<Z;mDB&7}vMxM|&ry|FC@*n={Kkr@C{?MRxp|-!1sAq4a)r=|L05Do}pALQr_gQUz
z2lZL~BnQtzqtHJF6+dpjH|ZMXnh!ZO#vK*hbcV}&RtG5;*slS*@`gn^i)B`$?UDGG
zh0O<Atc6k}{so%ZTvNaF6PrS?AeAl$fNN=zAHk)aNrjZd(=j~$OWmYfmTWHEUSudw
zvefK$Z-DIZ(=!_b{Z|fz;pfRfiZ4$GD07P)1eQNihMw9JXi;=~rkVL5a30v6cirtR
z-r`%UIp<o9(G34WEN!?VzKnJ1YnaO<5UV;EkNI|~NNTQnSo-K(iH2yZqC$6Z&%ca7
zUHU>CByjT@p}pjd?nK5`-Q-TW%E|T@OP{!syli`Hio3j=S&=LvL`qRkf38clz|Us4
z_S3rF^MwUnUHtLl!h*WO_voMRrcOCt*Z6W?&hM&NL(RXMSMs!DMP2G<Wr<;LWdBFo
z#uyRV!5H=x#({Q0%t*6_Mof-yGA2m(5#~L6P%AY5IJF}+SEYEjBNc?&HWG?-nwAS?
z*t&YrMSKKKdU<?_DQl**&AFRM76)Ag7f)k7+}%<?B7H@n{0Q}c29r=%De}^Xt#h{W
z4Cm^G-hZ@J4_XuY`A*7!LF)^*0oJ#)B3ko#Gf#Ai&xC@#EY0(r^6FPbZ%5K+7kZRn
z406J8>6^WC<K~FFF{QF#U?TWhtnjI5&93H5*Zl^OyJ@^e*3CdLa@K*-Q#4}D&l(v|
zq+B4ayYF0qWwyd!@u3x1u=LbzmhkbFT%3naUgFh9JuzJu1;&N%4xy`?w`FR9*Q&zp
zW`^-;UAfvvq3e2gGf5iJnJ=g&kALgz{+oGP%3o#G)vT7x2_<ykzCo*|&s8O(FYzs6
zM0F%e>$ft`5N+hvkclPc;^&zc*t^@W`hzU!%@;#&EvXR-p~M{kM}|zOsGha14sNN9
z53TIgyz(*7@3xsvH<_BAcK>xZs&DpqReubzT_@Ros?*!zx*VRngWZq?zuo<N5d3h!
z8R>7mW@n%!#|d)=_YVQm^crBY*)uCb1x*KyNN{Vij(y9%VXd^9f4$q}XOq9c9ZhaZ
zy^d0S#P+8qBR_okk0q@~6<)uw%$0WNycoJMS`jj;l~%9@fl%T;KF!74!$E@K-F0Sm
zJ{6rR%_G9QU>UtCyE+t?J~skK%IeT{ud6rKOaJP1r`@}}3+|}H^OAeHL-RK(KoRBQ
zg&WNdp9(AeJnHPumO4(QF1_+5(+?_X(eG7K(_`*Ln&UV2I2xOw4yIF$m*i`a(2bp;
z<gK92R=GWgZrXrC&F#irk?i{Ni0aK|UfB?8`wiGeYG8oSg5^Hqb*4I;X~9+3@JDLi
z2+hBULKr#Ge{wUuhW!Mt=$-aAAI1I_#dYrEe+-~lFBA*O34Mi~N8L(d)WwOT^nq48
z|Eqj};<4OUz*bW`sHoi?zq1s)fv}H={6h`TI&aKcZ@4y9bgKr$?`Syd*ybk!{M%wm
zXj<kD45D?l(E5o{I_!lf5MT8*+WHb_fsOkiyzJ9@uyRzBqutc_-T2T&yqw$*I8&LI
zKkVf{Sot3e=C4uyUD89U*4vS_`M0KR39ZuIwjk`4f_=uQN%Sj%iYdgrzVKHs%M=wq
zCYWWkvfL)!$l@0tdMoG+%A#kpU+HB(PTBhhvrka=ACTSM4L|vfK10#N6}=@GeUYM1
zRP?%FbgQEK6YX=N$RJKkwn8{#6ZY<-@SYCRLxxz9bOC5RyR`NqKXwfFk1}bMb~p0M
zqqLEFywOWTH!eGF9lbpNPUs<)Nod6qF;YI<P|z@WT>};+%`8hw_@hVbB<JsYsIZ_R
z{XK?ic;HjmTqPXNOb;!|&Bw@K$^0<psxgea$}UVVEBJmFCU-;Ej-)yqZ=RNduWhx`
zLqdtK6PM&=>2Uq21uaM9>^6D^czn_G#$dyfU=voivfTQhp>SP&;XioArljkJNIF&+
zO;2G?xY6J$6b}N$$fFXu5$7LulRrj9c{cLN#)ionZUg&+bluV|y_w|d(*vH0I!ix!
z#eARgEa#%Souz=Uu9KfWvtZ%-dMpRM<0;-@=T__GC6}~wBtw{fyeoR#Q#?&@T-150
z9v|Pr4>-i}e7(aqS(y1?SxJ4N%xd4=6|4D7s5fuP66Wx|5$4LDh$s=L(opz$L*Ykt
z@prkoVSAxw&jUx)WSZygA-CP%F*<zSS_8Krz1P0z%?aXi%?S+v^$^(ZrNNBy7v#oh
z-+ol*tgc#<xfu{UAIW~tdhf~E6v(q-&RX4I@UblUI-hU!!Lt{ou3+<3p8VEr4y{Qi
z_NGSJ$}M$&&d`YBG*)WVG#-Yw&qmaxrUM0NRfpzR@r*HSPAFQXRu`cZ@b-Dl($;@5
zwS*Gm4WE+t5I)FTu565F=v}l73X_XWaJ|js8biHYLVZ1FP0PSKc4D*ns84w&kXzNQ
z$mV=UTgx+&+pG;NG+zfb6(#F?|NH)Gb|(7V_n-`CVO9RT?ax6=r;k+|ieB);qqJcW
z@6dDaZ}I<?Sh}>m>+q~zWngXc7zW6o<<k%441UuQJ&T?8qCv=WqV1{0i8jg22nMw`
zc_=KL8Ok3u6y*9G44#(0hW++zt!wV81-x9~PqKO9(I*EVBx}Y@kU{&HxzNJgWpQr3
zvr<<jN$|)>7D{Y@bmqOltHPbbB}TAue8Jz;X!h7ci67FF#(;)!(Vb+GKVd(xN!O4h
z<7~v3(oFhx_v^WIW4|Q*Ow!@H?K1wjg`@|EiD&0J6IRLbH_a^GV0y7{(Ngt!%`)r^
z$!*OKYlp?ySNnMBrMdKg(v;oj%C1Wfhum5Q%7N4zec+cydku~`SA|1KUS06*kX^qq
zdRE2yfSxsA{~mhQ%>d<wEe$DfAGG;iV5`?3_2)w-qn~B%4i|d)vYP(#cJ~^a?{@NW
zx6K+qZ=Sv47=SG4lSxfyBW<Q1n@oNdYo73p9Pj_yWXj0Z)3*oY@AuX5uwZ~KP+PR?
z7fKw1Dr9-2o+el?>h;nsQ^O9qGUr9j4b-vJ*{tv6Jn?-u=9Kcy<G|_0@~AU~Yucxj
z@*i(Woukm{vnHKB>(l;I{2<dm#fx`vEdAFRVzW6*SHpN5AuEqLQ{kA=;z$y-@f1p6
z$<H#SR@XVE-0j{ARvuHHMiM>S6wNW^JMMM4^xvumn39_=a!h%-S`}a)Pv@o%!<X&i
z#~AyPl3qD}gC8Hu@uQc%cYZVl=J?yUmr$=~*Uj9{XI9^KFC!#9pP9n%*gg2@s{ri<
zYviqag3VvAndaZ}(@Vd~QZ4zE_-Q9$Dm~O>@~iE^PrtR9GAyn&`6M7GTw)AF2$Rvo
zie)<6WKYn)rt|4MZ0=zNvH~V~ifL?t8@E+HzBbU#Q+kfGK>+2(S#Uk%(Xr`v#x}#c
z>qgH%#o9yI%;p{+EA~87mbSFnyOYyWIF#TT9USEL*zpS8zkJVQ{g>Vfah&&^S9Nnf
zm#TES-@Zf2EKd<N#b~0==5)Jnqi(wQ{nF5J%d>$-o$ED9kN=SOd_ZVYWndMk`IT}~
z4U9bbtHftXI)4v#HY1N&dVo6y-IIfxH~qIi`k1nL+WtyL;LjL!(->Vt2NPzbeZM?+
zc7ao&O00Kg$Z38sf&P6dBbEkG!|!i)_yrDsYWOvTJH5xmImVDZvT05pod|x|*2=JW
z^QORBm`#LO5lU)(t|{R>Qp>Ttllnj|6~<yo!=@oqj9vo+2ooJ<A;261CP&kwNHIpn
zAa$&sqn*8Olkwa==8L=kY{#d!<JyW{S~WHQGNY>>Y<}4T*>DLZ?t%Z{84(q}5fL*t
z8<y<B&%D6ZgP%i*`za3Vu4U*EX2#6C<MH!@aEU*1E}_pEPaB_R5Tb(fAceYq5+I(h
z2>ZUF`MfR%D(5n*<>Z5=`CWf9?J4!!Q`O<Qywn!BXNnuKi&L~;-8CMI83uJf#ekT(
z%`Eq7A+%?ojoNvTHfOY@<d5GcG+be~oB0vr<qV!pSN3RY)tXqU-$39BGMi2Dk~fQ~
z%}3<FKqSMP4LSMX{)%+)eF2XYAM7Fz-z8bQ#Ymz2juun4o0^h#SZ97_ux^0C`(~z6
zr^iFd$N7dT&+*gKpWAol)(Z=E%%kkqh7xl?98*%$OspXFI)C?p^9E9)!CC9>$DkA+
zV^8=MP?{ymjfQ%JSWe8W*Pu4tCMcXTldDWo+nNTLF|mn|AUJEW#a(QHmN(C0lw&*V
zG0xo-+=kt<mV#La{l+rN-`7}1u>>I>?2^lvIhH)0Uh1Bim;W}Kf0~z{g?X3NyWMZ(
z!7Z|IXL)eW8(p{Aap8V`QxKoKE!;6#xb5DF$^!TKJh&%-%M7P;+{JnEe^LAi0sitH
z>%ncQv(0$U(d?!JQ*Flc1~)<X8uG&*53LUH$3G}V<HqvyaX>)JFq;(1+v(A!AD~q?
zotqj}%@qn~F<|;Xd6!Z9akU8_jOT!{QcWM0j_JBqOr>2FdE+bEOP)#f`>}$PkCF#|
zFxq|do4%jaqCp_a`>OJHWNEpTZRDNk0GODY>Z>}E>zx-{s@?mMTqxq}q?R7PQiaMP
zcEWSSJ;j1?H8Cv9$M<;>=ME~8>2+!77p$CQ1Vn3O{ODUulJ&0~4a!DG52tR?4g)Ry
zdg>NqqW_FhDtkMnwr%29W~qPu0khOWqK>wWDG&IW=r>{e^jTp0gSp`v7DVIYGWJR#
zW?zH`#Z{6SY*UA6DTN;;f8BrGV0z_9+#>u@`*j4K(PniWnB>{)>utw?Kb0qlMNPX3
zi^er&k1<UYxZi{avsGs6>iWIG(~a929>;gXuc1%220h@W8U_KBIWkY4oN4nVg87cj
z=DX6%xAPa0CrIXYt#~jb*QbWprdp?H_%p6s>Z&?CJ4V%-NPOt*UZER}a##3N&8pU|
z&aUIu)$C|GAih{VP}uYh?R^YifRE>{i=1UmLzC+leS?acz5ze-B=S1V-vXJ66mga<
zI+irL;VE-|(>nTUun&ex*mr@81>sO6iTc-kW$pR{_m{@J{lHBjE6M6QS3rPn9-8K=
zL;E1N9aNREnw31}{e@x6u)7Qdamh&!FU+bn9Y?_`s5_`MHLF8yeZfMTG~j}{af^FB
z87u&yI!6n1fZd-FU4<tNijGZf`I_C==o|gAAmR*N!v7~@7<g+w$FIp5aGyCk#mD{b
z{ZiL-jHr0TB}a2M<^5Rt+>*`_wW_hf*~NHcTk~aOg}}uCMk#*yN8E+`Du<^~W2T1d
zn2u3Zegrjn5Q;y2P*kR(Yn)i&yy6y04y1}7Ba05DVbW*>=3GG9r;4ZJ9pfk!(RpsQ
z`=YcgZeg`oP86G^1fvAi#?8r9m{xi{jW(Mfx^n^q%4G8Nu;7$eeKCa;H>*;E{)lS#
zRys?awu4H}MHnC$!UKo@wvhJ%Qaqv<<9d#=&JpIVl-5vJvp3FJ%C2gek{Wt#vU|q+
zL#{5FwY5GqK3qTf<>B$6H}q<HgHYM1RKFV{^5PsJTSY%(sT<J2K!5JaCH&oR<szy&
zfgvSxUDvcfA!J?!ElQ>xhdZ+hTJP}YiSw&EE?CZ^noWPFyu#+$DsO-(??lS0#40it
zOUR(-Lf2kohF9gs#C$}|j?lG@#8Af}gkB2G{jLdJh@r6W3Dl6$Lp(Wo%3CDWdWmHI
z+u|w~f7)aN$v)p$$dsn9fd}yAuNR2uw+aNd^8Zzkn$XV-Jd850L}~z^*u^K-#0y{K
zdmTW_TAz`)Kb^8s{InkHC%ffG>8W}bK2SuohH}TeLbeaRB3{ZCF>?(&M@=!-snWGf
zf2bV2iSk9d(Ut=y#+5c&L-D4V&%I(Wb6QMhKE>$)niqTzhIPjJ(eWqyNl=)ja~wJ(
zx3x@H%fER?VL^HXQ^eR!@}-l)sQN2k=sGnkF9|(`RDu!{OPRm=mKHXhNSyZ0E-01N
zjTm~Ex1!cHeWM|{zIm;nLw)-dHgE9ka(#=k5Y2D2n|_C{LE)qH$nMk~VtsA|2Av~j
zv;DzDh1d@dypiw1e_qn=Y@oKMZ~J67Ws}B#QGNo`*Z5ls$ja}6UEt01^|Yoi!mG)_
z6gT#Tru{w`9-7$iYr^QepmdmtxS+I*O%zi4mL66YUtUtv)%;6?mGLi-Ms_yqoEr_h
zv`N<0A!`TA?Dz+dK3ThAz|e@DaT{#V+^6e1o<0vxoPe`l2YF&OFEt&8mvn#2AF?-Q
z&x0&&-OiLQdaP;L8B5(Fa9Pk$<BOK@twD{YyLE?w()_um<?EggU=~iwo)4Fq2KyVU
zUaQ7Id)LBz{S#H8BxRPs^4#tSB^HyJ-Sy^?l>DuFJ@99dW}Z*oqUQVj7&LcwczC&H
zImAQnS$;{bQ5h}Y2()E3h0s5;v-*ZvL$B1RaKu?-oH9x7Gv{`rB^B#8mI}vuf7kU;
zs3+ufM!LV>n61vYH<Ph#6Xhcdif8;9W#cUMln!(4tu0-sKNg<fQu<-MbRg3($sW&Y
z400R1irQ6LnMjJcInXArJ^B5J054(e>7S3^XbB+Zcq4=Q!?0C5t02+X+7an-n%(-R
zeq~yH#P9jd=p3S@cf1?N%XWfV6VBtecU&U{B#&Rto8!CilZ)}rHSIb6DXt=xHV50&
zq6ykJNO?oUIBGF0mWC3qQ3gwjSnnIuspvrE^g0L}NEV3DxAfa~ixTKwq~a7KF8y|M
zD3*$MC`V_}V18h<r3~j)C6SudEwM;y=+B+i_36^IlH%R$a!Kiqaq6>o+}1O?Tsf!+
zlJzb6mOj9}QUM$RO*yTLG+XsTTZ@}U7FvaEw#u}y;R6}Z_!##sMNKs5nEaV5#N&4U
z3RX$!VJI`sv`bYodk-()Gy7n&_vkNwzL38Vm9CK{rXD-HR$@iFwg#{G?q1CUc@Z_%
z+xX+sqm&=s&oB{kGQ6LJxscq?&}&(dgk-d4X;TX~-1g;J`Q|5ph^0>Xsy5H$PefeP
zrSaX}&1>tNW%a$y*aM{8%TLK+3Z_6)s|q*OO;n_fv%<#KV^TvCVTiu_HD8zG$IP!#
zpO7!6j<Myr7I@`fV49HM0t?guj<!_1GAK3k4N=b`Q=UWFL+gSZT0K$qgkonATSu(M
zUPlei;isjH-v@GofwD3601t|#N)L?0KV=D^XG%l!>lhD&3!Bd8Z|~-Th7S;z(Hg|s
zoq)i2kbPN6vg2xB|9Ah~W>yZH*~s<v`rjYqtsD+eF7*2I0Mg25d3&AnTz&5aV&(eO
zL=6hM!ECD)9Wo<CR4wZHLA*Yw`shLKyoV|Vh{SS>4I`{AW(RNIqC-N-ivh{XOCx8v
zLOmNcWqcceWQWQ>Tjd`>`Ku{E50`R_NnX)H(Tg4zs4;jR$wTdIW$#<mH}i}!57mm0
z_;|cQD8H8c79Zrdxak;!X_^dPkyIVZtM#X(wP))SUaD`x9yJ&UlX^%`;BTsge0j?H
zBRy&ydy3jjG-rai+1i|Zn39?}C7QZlGE<w0jr61L;=s}$)mc#8n!?rJ3zbYv%>9<b
zdL-R8+k;0p2Oh(Rdl(5(bZqp0vP*C$cJ$V?%mEMC1P8Yq9^lu&JRPZ7+4>#_CeARI
zVb!G;?4WGBCA9J;jsTeT^yGeX)y$%o5H1==SnWI6xYR#0r!H@OJ(7Cz0oa8DLJ0qP
zSMV9-8ndCg1Ukm>>OaF2X4kW_(=95}K(L1HX{EY$Ml$6ombi7PM}&iMTEGZ=N$9vS
z=>N|9?x`#hk{w7i!lC3JXr2cuUzq@0pFFszfxE>v%gSBwG25==j8?;^*N82VRGS(h
z)%$L>4YZiO+hdbUZTs;n8!PFQJ%`fzFLKC^Afd}L_$;~nC1u}I6E881F*RGEoE<OJ
zubL+qBq$7>xHxtr3WW3~W{inaqF1+W5Y5{bQ5uTE(&>jqIM6g=$X&t^`oU(|@}dxV
zs5vThJ-9?t3yf*wQ@js1gMv6>TrBmF@fNW9*W`Y4Y=5dPMne}HxOK8?dVsj*7}441
z4m@W#l2WH@(#)M{`-9w4&#f@#oKk_|Ei7R5H*;1~MQwNEB<kdhT_~X!4G~IH@@#gh
z2O9K=Qnc+}TY;&wo}Ep5+T|pYy?U~Ba0zHhUCNL&=Jey5ns=!yTinOLz;%_1VY+C=
z;A{hm-yzx-y6?i&JfWU^&0{g6JA1WdJ8(7))r^wm0CioO3j9ucJ2R4=v3w?<&*Vv%
zQ?nS32GTaT2cEe`R|E1t!_#7!c@_(=u*<BaBEtlOjLnw|43X2i=cnt)?EX7=vkdH8
z)G1D*%R}>f36|lVTtDN@$dIwsvtDaRjfx?^+q;|B86J7TWaxU<J!Ve8D1CA%lMC;=
z;do-qS5e5KN$9!RW)hkbFbKHYi1VkdZ<4WVWsuWGI2kd+x38z0{EmYKda#+{tSlc`
zUUNR6yE4C~-AbgnCO?JsKU!4uZWZV{LG4(u2@DxWPfZIE9ADazRkd=o7^8wji#55y
z(b=R0z_=p7AzRn;Mjl2|w-_<8KD6Kq?v2UJR2KyFXYH3^0nZ)&oU>I^X*4w%VgF|%
z>~S=jN0e+blVa(IDrSQnN56*-tmGr+JZ>&LsOnB#gC36bmH`MkUl}7`yUFL>6)=U%
zJ}P4AYsS^n+Sw*0%BwO5qLZoEc`7ZvSf#r=uO$uwbC5CIf6W32<X_MZS!XhRqmlQ$
zVKuOxp{26`A&t*RlqzcQAitcTe}2u{oNC=WW~x|*j=}L|cL8D8uug`b8w~=QTg-_L
zb@Nmz&e{u*tJe}&w3d1$3pK@b!pnA9<N39$Y~2t`n{yvDlg+iJikaj|jiap%4wi{<
zywNn<TzX=rP@EBB&ZjX4PiM!C8n1kNiMN?4kAe(PVWzpx8ij@Pp4{l;aAN_a>7>2b
zThDAnb|oL?lOe2T%;vKN#|wKliK94b!dNtH1P%5_uO0tDrmHa%HAiL}B&$Q#zsGCR
ziq>aUTVu8zej^ZGF&bsiW_Jt2xZt4dMy~-y74AfaA(8(Me|2MO^ccCw>;-@J=<QjY
z9RAh+0smD7f3?DQHTqxTPdmW>4x~XA|Jpt2PhYa3^qTg6H@a_&joCM%Ki_j-Qh=P|
zUymQ|HJ8<-umu!_5+j*vs6Zf<h^Elr_j1d@d(l*hCe#AA1fLIgG`tGvb@(C#yhL^)
zqIB@G4mioQZC4wcuBlT?_LXTJjclbDxT6JkorfxB3pACI%3amb70St5i>TF~#%HJ6
z6$~Qe@Hj0udbPg7XldbgF>me3jm4%EZ_Jph$M!6*js6&q8io&0T^bLO!Vf5q6>DGq
zTk05qtZ?%ydmQRiu*Jizp;uuWkUs{uaaJ+}D#Xl_R2^5IbN3<Zt-PWnFs;KGcF{l;
z@cKA=TL@o)coZC~UwKnyXX^v1%Stw`udt#nlzc>_o3la!GB9aaz0<p2Z0HPnK6}5|
z%FJ!ZT$4`MHPOp3$D<1=#b}I6^kutvAPnP9{h8_Fd;StwI+gX&(n(z$16W=Br|Il%
zT)@!DmV3ZnxyP3KED|Ks!DQhsZ#D0uSsgHnamqXXdLABsEn}<Jk6<BAo4I6Qfb5Dx
zN)HX)w3Mlr1zu6;?`tB3tE0HTl?@qL@~9;3EY8-PRhBC4+x4QsV?-g@qeDhc3f*Ds
zZKnF=6x!u#h2h(4G~Z;;Eb2Wav7tR<t5qpWjC(+~yQw<je4L*81#6&bCrMFF6ft60
zi-r;@ks&>6aMbzB4&soR?%mKFSN|xL9q*pN2!PlZ-y0y}!-j}Fl$2e6zNB8t-a`^_
zwXZF3yGZgifVap<VTV$fQ5_n*KDZg6t|g*V+}l5&I|a1Ci<`x_GYgh!RTOr&(^pZP
zU~v`UYOsam*1|{EWtT;2UJ0e;H5p0Q7Dk;l4U^xHZ|fx^B&#Q{`yh&u_C4lZx%S3V
z58q8*(wE}*+K|3z*IDVIhrz<9J^=*3!~W^OTa$OyO<pSR+4|!)j7VSDE5;|~RlYL9
zS<6Yu4Jxz$HRMp_3SNe*D_mZ8+|zYS-|p?Nk7Mbg)9VX&Q|6KUuSBtV7QK<h#Gp?@
z*G>do?q#U#(w|o&IN!=w-dtUKNi<Z+>doMJX1eI8sPn#>;>HSh82+Fzz1YQ%u7s$3
z3nLnP5XR0uo}JV2|Kh(4%o>W%ARA~^-`KAD!c8@=G%v3&e7T|U<;)~YV!D32`_MG?
zc_>kW2J3zsGW&h@1bt?1{D`I}GrMnY!OwLya!=GxpSc?%cGOMYCED=0BX(<jdO`_K
z#NJaLlXr?7vEx>Ga*S@8bxggpF4jA#E}*0Qby<OFPv4>)vBF(oxvdUUNJGu5&Ch5-
zH6AvO;&_}g+0?W0SWcI&_WU>V&S5QY-I1vz19A+SV64#o%ot?XL>V0+&xP6%L1vA|
zK0jX4Z5=0hw9Vb|vy-6wASgF}_4LD`8T;QCzT(~C%4a5Xo>_dU+r~cAWe)#Gxvfry
zP%f{tO-fYUNf7Sf>dD!B)%(}l*~<l$GC?;#-EQPPwQzkj)gEVV=HpP7zAvt5WVyhK
zO3-M0Kp~#_I{{TtgMrV>QTJX$4n?^cNEmo-;ZZb2VNg%_R(>UUrFqt;yb9r4i<!7g
zm7+qIYw`?>r9n;c27yW;5DJ5zQVbgW1r4BN(F21F_u2*hA$@-lv+W+s3Zqs|(*4*<
zqflZ$7~jgv(A>|!EdHeY5DR1JQQYvjGvd5xTG)JKEOmoOiW<9#^bu8SBvp~#tK1cm
zING(0IuCwi<kn8NkvQv5@{Y<}ypF%#9={F(gc99E*z?4&dh#y@$W$CkE+kN|^7dY3
zy|ddr2aN%zGdZW7#sJ|I!Hff=L$2b`P6#`L+p8%sXf8dN;?l)~aHKGT?l6kiWb>D+
z*q2_}b*e8P+^2~FvIt0j$bHU`Gug88b(+b>VtSALRyReGpRw3h`^X=?Y8$<3PqWo7
zxuo51enKxUG8Hu46HAXarxL5{?6VXxtKL_-9ms@8I$Ggxfb&j;EL3vo%RWA~njP~m
z-J7SXH<Sctmeoy0%gG0527t|UTf5Y6UZv#GHnG93LM<UGISxZN%4oGYXs@d4ELu(@
zjC~|hv!a!|E5gP5$F;0~gR3&kO5aTWD9Vh8s4l{#ba%y-;ssL^If&+1wOEMK&p`q9
z=TB6naQ(_`@eF$oW=vSx%G%*GEcf0l@j`6!di1t++7GkvRAt@Cgi}|1vE`zKxKwK5
zZ$L2AmL%lsvxhK``WKJyN5I}mrn%qk#PkRc6ySW~cjb7x((u4VULRfw5KoYOf?T6T
z@l8b28b13BrnV14%T}}TI%a8T{xCyFPu}i9zyCnD>!>B4ZV9eB1#?d=uV;+;%{YUA
zdmuS{tiJ`;nV*3o8yK;M$uF_x)2^2%kCxJr++eH|>}+skt!SOuDq)|qXkF%Lo5LQA
zQ-VO0aDYj;2AAhoOuwr4513QPyut2c?ksXGJ3vT+yts1H`I#3eK@S(`@Tk|L%-vtm
zOQ;ClXQeB4g?|1aw4`G>LF%{jVn#Ua<iZ3rP95$VO+@BN0_2R}O3zNeXCLw+*SQ95
ziI1;9EKhuV0&^lh41zQElMI<z<=6Z!qt2)BKMA;ZUlE}2nWC_+tVMT^)lFWN>US*;
z4ow5%?-n*MCaxS3p?0I#!=pvZ%=J4qWTQz}^dXFXJ5qir1y3VV>O<yZI{QDc$fcl@
z&m!iTh0IQRw>@Cywio>1CA!mW<o^)1WVJI*+eq=#i<)?kv!w(M@2Kf)t#McUw;2Lo
z;`QM((_7qhk|zeQ00<!%&Mp@4|7l2#EW>0~Qq=UgEU0eNS6Ns7RXZv1p$8W=|Gw)F
zR=>b$Bb1m<w`q0!2)`V`u6m6dkiMSzDIs4T*KBC5XS-b|sB5}H39X|eOpMhv8(KEk
zF-^ueyEmx2`J_nAg(a;Q8ObA8#VzjJ|0Sw~l9MSdo8#{~Pv_++Y`)v%85ziPzsb|I
zz+J_M-|RwmcQU`FV(G(mJTOg_$Fr;YL%OUhogLS-oA}DY=KFK#CeH!Hw%5N9;43z)
z^N%(E49!odK{~PNywTDn&zu-a)USywNDaM}Yo}folIGxVYg1Hr-fnSkWAebj{e)W{
z#PVRUL}t}5^ro8QDMB;c)0g+`BfFld+Qw*FpcfBJSlyU24CnTf>S$N0Uy07k)B($<
zIic_Ok8M}igYFH~<CE-5IK_BYx-*Fet%vax@l)T)@)6Ut!SPstG7lmcI4^o`efo;Q
z@@)UeUd70MUR}+{hGUa}cbfZ+=+gps4!I=8%+W#ZJ7A{S=)Ptqnox2yF!(NQYwgqZ
zE%`%;8&|d*TQj`6<wFfs#wq?5F6u>xun26!V1DPar;YAky^@xaUWIS_Mz<S&XCZb7
z#4=8e<`NOxpY9DDL!qJ1;CJFT^lkdY444yC1)Xs^KwV44M?9{A5L-)Fk776*yBp<A
zk$e_FCH%pN{ZR4)!iGV;)_v*4Z#M;QbkjdJT%Py}edC5H&98f{xK>)Ghu?=kG5DPb
zenKho=q3P`c!+tq+}{9Xo7&GbHI$GKcjhSmczo{u*_N{zUkSgi7bq&Po#z3RX^|f4
zUc;vkxEw%;bQhWP;Ew@FM74rsxjQnLX%(5e*0|pyj=YxzD}JhH#s9pNir=Q<?0$^a
z;^tK9<n5!K=UI@ih9p>bi#+SDy~$Y~uUt4zQv9=`2fouZ0})=IzPtkek_>g;iz3jp
zKOc|q(Zjw$Z*g7c2zbS9r*eSS;D3VK<)fp}8rc<D@_o}+omm<mPnQ`ri31lf!4`Lo
zk>8=@J%VH9Ad^AhPSH;lJ!ggR_Q)gt(HqTri2MXF+VjPE7z(5wfLYwnIbIChDIQX%
zfs~$ZK1ZcGK2kSbvb);~LW9yD$poW+Uk6(|1ld{iE<Xk4Zet@a+~3=BU&rml@7^=B
zdh!_Mf#1&pD5LA}e0kwsX7k+{%=evazR_O3uPWaTy3yT=jLWmH$hu+=fYO;A4D$TW
zw4>g-GS`OYorI^gy|a~LG8~V=x<0A=N0*@GBEYpPtDDk>#CPouCDv27EJP1g$hJcV
zoMKocl)Q^F=m6D~o9MFq#X)%IrHJ#4`eSM+F|ZhM9{&Op=plF~#2$-8qvhW{D_Rg+
z^!*VH<_jG$78-#WAZ%_;($hWKvP&h+iZES#1NS~f*@`G(FQx~p(W_c$#?m%R%qcRc
zqOZb1l21pW)pKf~+6-6CETFLOd+*GRcT+B=xABKv5t^@~^s!U}=E!6RxpnQX*{7ON
zq#dZfMK74s(9gFbH)pZp4fiQ!4v(d763?2;@-{iIptmeV88m)b-u78{Ba7#;o6TPK
zNttQdd8>TE@R9U)d-zrN0gk~IFj4N0>smPnJF7gFN~>g9$SYsB@POmDJHb1wrX5<@
z6E+CGxiWz7OBT~$x@i(fpL)d%MyP*3Uny>wgVs*_nwp9)<OJ=@jc41}Aj8qv%1vIQ
zxwS8R>eKX3)q0<wi8A@$xA}3&+(&*M-hS-IaI;~SPu?ZCbk|W!KkIeevUJfe3Olq5
zm^c}xq%YA5FZN!xY2=&TUN>S@va3+Z_R1TK_$7=MF~U~?ji%BvAc7#7`OB-_{H0JG
zfu6U5Y9u}=Y)>wuq$aPVXZYQ-2+GzXe@`S$*?J0tvj2f9ve&Z9yt1e6wQOojOW0Au
zH<_$V3B<AYVg!^iD$_>2jaJ}3iw>X$52Pr)lD?V8Qc-&mtBjOdcMvloI#q3EAd!ZD
zoE^a)aCYQ%0I&I&dswKt56J&JDv$FjeSlw&%BJ0SLz39!WPxpd7b*{^^`Jm#NwjDZ
z0~Zqa^@_RF79)b1Vss5H#T40M?)*>1JS80h5*$r20oq+dK1;S97>IV%P~cwgWz+#$
zq((2TrP{fb@e8P4pYF5K{1hMDvwmYz0PGdgz0NQ~sdya`O61<8;x*>mH1l|`nrr#Z
zaIW9>Lo~I(wVnJEW~Mz+gg);}DWG9>Dj+@V=fJtc?DrCVSE;}SpQ*rF=ELHX4Z?kn
zH9y5qGr{apEL(&Ch7EGw5EI5+y;NLCB>ZefiH4iv%ZcBPzL9P6JKi@UxcDvaTdjS~
ze4Ew|1!>4`#;!NyC@gb)mjCR1S3nr)4~Hc?tmua)LJ6sLFyGj{l0?%|f%}>?oDi+H
zdL4vGQTv`F(f!KaF^L#Vpx3Z-$mecVaV@CcS^e&5?zIDjB79){dA)@-uF=<ENr=(u
zP6H_O95+_LFT)N}Id@XdUfTW5Y`cMbQ=K+b^_`omkE*KVx!Q+0D?6twn)*MVQ{*5b
zL;Kqqy&2~|@gwa5?Xe$Ir<Yz?Pt4gm4kE@TPpr<<8noe6)nP58as#0Ajh_>ja_Kzn
zB=gD`OO2=ux4?$xJ^>}-yKu36hz}jw8&?oYG$NARYf$g#4L$jxsf&MD>1vjT+Wv-V
zDJI)HEWLArVQul%U{^kEL@H42zMc!zy8o~N&aSvCa`9#EBLtYP>&yWVY<0$7j3V0I
z$Sy7l#dXvgO1=p?G9pikbS7Ugm@r3oCO3GW(IM5L^z%Y0ov2tyWF$SYF!IUEQ9K^z
z5p2*KKr%*?d?R56W+-sG2_!0J^V?PC@r|j7HEncAWhnh)?4<Il5FqRAahPb0D4G)K
zZ2JP)Ltl6ExhwZ6;iuU2W!>q1Wu$59^1w>Pb^If?B=;%t=*3Fl>YsZ*bTKQ!mxz(O
z+f>-A^_P3okgGZ<P6lb^)ru>Q2JEiAz%c**_`gVlUqw5l!Jh_9Kv|x0J{<;OXK07E
z%)IHP>)WuJmeW$TIHo5<MX(b0=^vUETJi=_noPCIgg@b%+7&Q#)oxk6v16MY0>a6J
z?fFd;ZPpUgk(S_gQ`o+%ii2TZR>V^78Nd=#nJK5_B3q7L_movQA5}L)bEJkXsD`ys
zg}4A~Cy=QA>S^@hTMdaUS2w!vLMS<RGb1;KlJ}x8;^j7&tn9P)aopQJ`BZBu-D?I+
z|03KE*_#=3i>CA|55~=~A1N%_d(**WJdseu`6qcf|Hy}z{YONz6fy|dO%rFk*J03+
zPU&`k!BCgc`dWmE+n=7Nug}7x#g|%VWz7tikV8_dgN9~C8`)FfzD<A}1A;l`(cebS
zo$BS750E#W^w^!`1Pg-!W??}0*>Uz2yFQR)$*vC~6LA=xO(DY6NP|ZAt+4^RB{ne;
zYQLmbx8(MLQ_9^rbQ67Fu4wbrxQ+fvAbM~PdbAg<8zC^qO5Z7WPY=M_i-CzQbC0#r
zWnNP<Z$d*4zdHH%A*~ugw7YSf?bZx_E!p|S+xL%-gXgpR{*qcJ=SNh#N9RY>x_xsI
zl3=}ZLGgK6=2R38>~XpJ>3$Xp9tyi{Ke24!Y>&3*GWR0j+jaO14@#t312P>Em3+;q
zYznzWG!fk!u?sXC${uBl-4hspK1GY&0bbBhve@lIP!z<)Ws6(jr5VZ<yYDl}ahOtR
zpSYU|*t)U2huoV0%~cnPn~P|B^VF4C;tc+Wl26lsSo-#gh2nyAaW_u4Bb-n7P)W2<
z-yh`N?~gYOnYu2(!V?V(YrU>A#lzRJ-<j>kOU3zz+iYWk+5Vj6<CDEytDt>0*U99{
z?Y9_8={0ZIu-M&=#0e1VJum1HOY+Dk*7IJPN30bl%_G($CN0}h*)IAaB+Svwy$GD3
z%db*__HM``{%sHOJ^*P#5?M1>qfF%EdzOcU87{@{8D7xC_x~{WF7Q!RSO0ec6O95+
zP}EqlqDC991&fvxuo<1`L?;#%1uH5QYiYHu)dZ;*f+hi{;{dka`e@Zw+uBE;YHPJ9
zUcxniH@u@*!7FD>xp*m}Re8U^wf8wQ6TsR&|Nr~G`6zSFKKruv+H0@1_S$Q&J=kFK
zgS(Ia=HVS+-#onA+P9wY{)yBgc>m1&1>WwT6~Wsb!aK~vdpemSc$e6&0F)Tv_vSN7
z@H^ALdU&Vu8sYas|INdDwte&Pj<Ih&;VmzPx0i!=crm<XAxo_D@cs|v6v4Y2@OmZ-
z|CdfFp7J1B==b}0V2C#+9@w5r9{Gl=z%$$6<k9f19{k!SvLs@#8pvebJRKHUn7|=j
zG}4;_o^5~~HJ*<&E~3Wo_*aju6M2oW)!@H*Y#najaAH3io}uZxFA4cNUaWd!9iHU-
z_>!K4K9QaU_wbFZ;WcVxmH*}&dD_1DAu;_d;!R=KenjPkuE8dxhavnFnvyrEV&%<0
z4aS^oD4vDuN+i7w4ToyD!*tM#OwmyN=p(<5N@q4UWY)2jo3CW_f_|>1ql;_m7uFQY
z!PyK}p55_H!K*wmw!0Q*|7w_hq;EJwrjU7TK<7t?l;R{gjLXpoexyIW54nZ8mXRws
z^xIhOIPY!u4>n~MMS<Hm&BiI6v6%Fskm;)^Ur@IZs?KJ8NX0wo0P$(88Bh}jH+3vg
zG~5)K%TUmV#@nw)e<)IBa=2?siP;u)4Bbgi+<pT(71V+h<j6Bbz{o0o0t>Qu$3)TM
zgoUX`RHZ5DUb=;zGIi^j0E*y(j%Q}F#0jQ#XWK9hNVnk|L_K3ErR@?hDT56|o@wH+
zo_6ff=Gk{(hwV`I7-o~n{QW7HUB!!FwKE5&>KGx1?qprFAaHCyyv*jKizB0Ky^(f9
z)vdnx1)A>oHYM0n3^ln#hVxr1{eG-Kwwap>1~YIWoVKG_)cy6qpl#S>-5hl%KJz5B
z?WW915j$5>tag15)*xM66&9%K4D}pHcO|&zZe~v@Kmpc+M^1tW@T^@hw&Zp`E7G>{
zo0<77Zv;O$(P-|rjv`|JWk_=u0+z(wZ@<Mx$zQ>r`cB7jH7T?NF5GsAb(!9KVC!jp
zEB$S+QujLEq=(G|!|qixMpeP~bdgp-8Y5b*ct;flc;g+q(<!*0p=X|~YcGO%K?rlR
zhxy&(nO0h~6d(m`&|>h49)p|dyQf0bXQ*(sDOzJNvyNSs{~?lj$cp^Kq?N=o<E)bR
zUvK;SM7#*KL>;U(Dd)-P0(OIQSj7<aC6!r9#53a25q~pyhWTgTlktxG`JQN-9sp^%
zP6zRuC2~jAVLxPhR5UhoDkIwg_WHA2qt1HlXiV@#_OI3Y04HtZ)w!QXuIl6_Ac)-2
zryVQ+ysEexc%W`Rq4xPimo@mT$=E6E%4LYR51}Bdm>QyZPa$pRSI$Dr^za9XDL&z{
zNtM`g?KF<<<;`n-Q|`!(Dy)KS6y$f`8^>D(p?#htrWxhuP1*%B0mkVux~*vWq)OiK
zJ{$A6uk8hvZ8fCLC9FeJW|dvOOu!qVL{|+BU@k!=<QTKb9XBVnA@eeU!O3x{j9#&Y
zl|~7+hnqx86$|J--q~qrvtJn?Cb2A3#oJDXdwpJm)xm8LbCS-9E@3Q;UL_+@4h;1f
z^jjIN*aX&Bx{clob`aN`J%B@K2|Bnjvpkv6xg2*(a@dhe7y=4$K8UrLi6f2wbR+=v
zj6F{_XWFYuW_Oa$9hH(Z-aZntCcpT8MZDuypcgV7?$21x(Ry5<Z>gt__>4AV%p3vG
zsmoc6k?K$GT-FT658-#xmpLjXehApHU2;E>mrTX;a25?q{20#vzo`<jU7GvqcfbJB
zmu<1ot%X8Z=pn?9TXf#Kz1+Y%q~9z^p=i+<Z(9d|Tfw@<rP_~mEwfap=Uu4CPh~=e
z^c36){$K@fGGQWF{Ves`x^WL90M37siP4ucY!Yr*d;}+47y8)p5}m=ZR4E}YZ*Hbd
z{z<c%O{Fro`8&j%+^!zwD((IQNR79r;T8;b4LzRuAqDcoNtk8wI3f=YXX7g<(ikQe
z7S@UDw6zrMB5LMA%Nr6uw8V0ssM)Sl*p~QE+U2Cn@3t0o`8aT_E{j=DB#(M)M!b@B
zC7rQ#C8U}t%=zC*Q<*>^D!r+=(sw)_?Vw0kP!-mDpACTF!$@01UJHFQ0K$<1;3_!P
zrr>5UnOT(ohM-?$zB7!!X<1}@L<&uQmAGqR{Zr(x{k)GJTAP|M1_KQu!R`W;HRH<w
zhTjY)M&XO1Z_zJ*tKvj=Pu0zfPrr*C1fO|csZwWbK^bpEnbi3kLdK93Z$Fr_NFU1(
zyqgj;lp!zPE;YyU6lxAv+XM)Oxs0%ygNIc{-IY`c(mGm0$K{-0wxb{ijmQ_sTDt>S
z2z#Lg?2ExZ5VaU)zet_!PfFz26yqg$N`#oJsGL$X(3AZ@KhkPF>k2L=6Et00V(Tai
ze&NbKepyl3Z--^Wd2z&+%1)(XtaUIs9eV)jPn$OLIyzihm<+~(M(a{$e$<`ONaWJ)
z97zh@$t;hzpU(J;)W=cyK$5AJU|1y}r`n#v=&IU(aE$uHThTz-9dMkVmD1BO9g0Ey
z@Wcl8!F+`FV0SbXt!-{4SD_R1fJ6To^&$8rW5y-D#{<+kvx0NgnOw!~mlomaj1W%<
zLsNftA(P#c5yJ3K=C<iu`3tC~n0|GuVG5TS5pOzooA#0wql1_}SML$V^x0NtcFs!D
zFjkIZ5zpz|ovTE!Bp(RHxJx(@Cl$d#4bVXs5Nl%ZNf9_m8}|6JtX;X&<>KguQ7`Ex
zyv3qKM3+x>9W_V;s&bPBOxIDhdM~<HN=3@}RB}nIpHa%!sr>x$xY#|B^%b75H7apc
zlqmV7zrsbaU2T$4`s->7+yhS%o%*A&6kq)XuODS}T<M)EQs}Lq<2=wouYwPx@yP-{
zKiG{^q(a=&eCl37O82`Z{6qIkhHgtlKM$`zV+7&@qY)6QR~<>+xegm5cUZhK5PwbQ
z?sKE@Vel&kCu=!GQ%?c5ob8|EH#k&u%Aov=TGA8V=xV4`H^LBx;SE<eF=RxEs<tia
zq`t=tA$|*cy*e`4<rzdC-Iuc2_p$CfyXl*Mcd_Sp0hTWsUUYk_t>X@W?drHkF!Luc
z-qiqP?0TRfY{s|Shb_Ug*k$Y_gECH4#-8~gw+@jh_YH+Klg!xJ6;f8I1b?&&?b+>8
zg)Nc7j)>|I$-4Ee3Ut|>Q+iT4quGv-mVzOG72Y>xH&BUwu1c_(YIg#B7?g>YBCjTk
z?m8=Yj0}wF%!5p(l?A6U;ot|?&~T(roJWJEPlWtn8#2uSva<X?U<3$k(dFer3#N+7
z;Om;wG=4W~hluy<@FdYGa$<h>+ok(qr$Cn%?Y1|;AT}gUM)$2M;vcCDPEL-6P}`7t
z)w(*^%{Y6+Fg`vGhptjK;3QBm3tA4P0+Y$+(?Ac>?G8l2y+i~|y_fT8v-uP{{fU7s
ztB$5mkfc!XRU@UYVEQ-dpF4vPTw;@IjwV&`RW;VO0C9QA;L}de-wHfF8wX;86N++H
zC?~msOO=Pt#(<AZ-GumG*!@yrev7RJ7pc9>%G2^w7(8|tH``xy{)(FWY%z+`3CSll
z>Ik61bkUf3p2+o0nJ!}1$J>9+cXX@jU=)~4wLMg`iUbiD!VjgaI^KQ*?{Y-a2LX2s
z;z0c1n^cEsDc-&d4{lu%^q~@40B9-Dhun?2vvOmo_RCZtExu2%Bi#oJD;Q0eKNFDN
z8_y;I9FCjqT!?RkaLI5&KCN0h+!-)!Q8@Nh9XtU-M1$4A`^XGnY=4Z(f@!oBQS%>=
zZczB2Tbuzg^f?TMXgpL_s#fqma7p~<<&s*!e=wX}ly0VURO7XwybSe<w*kWfx6NY3
zUEF(NZ#RA1nOfy15OniKeKp1P{U)sMKtp&}aGi?0E=^JTX0>XW56eSB?iu_y-y%rv
zqNI0^^!&?^u^*kuqfxp3iRcHAqN#Zk!?h*)L2!in=P>D?QsK{+63W&f=Qwn`4NJaw
z`#>WBr-|x5-GaUtYNBjtm1v?URI7uIBWONSa0n2s!ySB(3RD$*kFTL7iau_dXjP%+
zEo!1)Q<<%5qE$g}q^8qEX>A60ja0W^N|2RkqSHx<G|`KB2^z$Eauri9WYj?fFcC~P
z9}bhLqHPN>gcL`SeT`W<E9gVXLXpSR&0NLd)Dtbxx);bF82r{sUmJwJDZP_HUg;Dz
zDV<f)u!HzWpg@<dDpEXG(63VHmyi-c|ByZdT8aHDR5q)EPe<r;v9igux(GrE6}{Ve
z-nz2+M;M?OflNe&9td*vAk;Vw_LFD{ty3Q8M)&VCUIn~7<?!<53p`#VX!3t3f@1*B
z`3p_g?7{EsKC4EY9tjPoi#XDmCv4&57$4k@b7gM%6mLJAfeXG5N{qWz7L+qhg{OzR
z&X|L1u@kiTk(jf~uCWiU^&Q`tw{+$`XWk9I<!e}N1>j3A5VfXCb#SN+o=u>S3PnD#
zX&i5VQ?;%nOEi`DXD<g*>BI2@BbB{Wsq`%}g>0%aNYfc`UD-X@^WPhb>!@~hbV0Bs
zbsViaw!moNN2e8+A3%9`?lxk(mH9;s1&ulW5*%2Zzef3I*!b>LoIZ$jH+sgu%yCXA
ze*~35^rLb4&{S%c;&{T2<h^Oa2ZbmUe##%4i~R#M5W?_<B4s^(^u}Q5H)IU&DwGJb
z`jV8uXudJ%Yxx%b%Y1Lv=?Ko7*g)bljuPfe&cqkw0YZ+G?=qHm>Cx=wW=B^TA0_(`
zItyB{hOP|OAU!0NZlua+FSi;jL=(1;y`YfT{wLXkB~VpF6~_vbZ`tZ=Ds_|vcNTv=
zL|?o3s<uvW$Zfxu@e10>1~onWqav|2l-x!ZU!fwA)>p}tzm?*TfPYd}2ESsKB4v(<
zU_B3rB3RF~7Z2-+f_0B#XhsQ|IzhwrF}4n3mX|f}7OW<#$$Tt9+^*zRf+iN-2;4@U
zDseW#&ji9dj0DXS^xo}!zk|GsAo!cTcnFs9(l*Czi1GHH(^T6Wv$fzA#S3oG-)uQX
ztg<RNrwD-l4Q0VOPzBhz<%dPsxgf+&t>L08C?BQ`uT}Y0j9ZXK^Iwe44|GM(E-rd*
zShU6$eStpYkD*-f2~V_iSaJ5V!|eO|?7vs`YGr@IW#6VayS$lf*!J_;Co6lNnPY50
z(*1DyC){a%iCq@xHL@>x<5D%(SL&SFD8*%5NrhP(C@#$+4w*%3pGUYFeD9BF$}GbO
zZ8z`Q-oblD8ZAH4ih{P-R0wsprp!lzFLCCO4f)3z6X8j!LuK$8D5UpaKMzCb16saV
zKfdhx;b}amJ6sG-ercOVRhgZoyRE=)=C~?8IM*ccC=qJn0i74Nsw6iYK*f@H3}yT}
zEx3ed<c=@+Px1rM@2JhnOM&Ztq}ZMj#!vWP<LmXqG?o4pzHUNYDB$bKuN%I$1-!%q
zyN8|Zfv@eTc<cDex+)g@pcr3!g;i}CUtgh2h_B;#F5pXwKKPnMZU49N^&2SXU*qdM
zl&k{2ZiaLK@5S#6UlJ-?qOX^}_Ida^%~f#?RTR2CBCN`5dJudueSOFXg!tN>@`Zv!
z9KMdGw$H~`coQcfIYM0<Q@>{BWQF;=61wcb)`R+>I8Re~H3KXD`Ly6XMOi`H!ft6(
zX1;{#<;o<>{1P5ywn-^#QziQ#ozZ+EHtdMtd%zFOUadXT!Tj@y9%fG?J+#~2)@S5m
zOLg0bU1H4->OdQ|&NtL5F-!<j$>GXbn)k#bK(7tTDZ$`;!`GX5;1n8qqz5sNI21Xa
z)rR?MW<TJI0SwY^F(<$kN3toiTpF^OnYEHQ7yb*Gj48Ht=AG#pwU-<rSK^Fq;>`RA
z)Dq*{7Y;4%^9iocH-E2aobub!TyR`TKE|;uzY>9JmRMcfDreiuvUt0KU<O|<$t<VA
z{GZ9Z1%C4X>3Cnn3@D5@o?4pIzd7e~$9pHr*gEA4k9U7cQ17w6-oyhOL*Hz?(}Vxb
z@xGA)|HgQyQAljO+aRn$pV?7RpZ~}ZMf1{)_h}{huG0{1eY_7T$xL9mFFxLWQ^qFw
zd0eo!B8`D%swR)0?l#%E=ZQ+g`TZVi9h=-O<d4tjB=;8Q^YufD`>NB-^mX3ZpF6%^
zqs->x+kB><%fIjbgW3E2d~Rkw51|D0{(yQD58Oi|TVl579a22Lg~^;B&U~RgG`W@o
zrCshP&1|n(!3xH=FisB%rQfuyp*@2=65|0ZqFxXKxR?=!AWkV_08ISTQg%3*h1uyk
z70ZH4_RjoBR3+<2aA8S1cRC#1jQc@>9$u8<d|XMsPM2>|m{0N`e>eHU^-y9AD(xQE
z)!XL(&i5z44S`*`FC5k^mw#bktQJA0b=3=<>!r4tvk-x*&r_VOvx%@`-T<Q?W3&0`
z3r<UCcQY3pb9%^bPm~iJdlX}Tzoz)zob!OAd-;<tp9dqxH_7(r$};m(bxSXJC($;q
zydkr!sc!iN>;jFe!O~W>?`xRvy5;k<?~6RCK{nhUXZPD0joZ!ysyWUMqdC&XHSt&c
z9#N3WmWwh2?S*fbn0f^M1l(}yv&Gx*Ma6fSr7DOy1~A9}0!>ZX7D?1qJnC@0#{4I;
z8LY>BVF=XEjn_#nik38gE1e0NGVcs|rcs?smoKN2dvX!^LK;VNQ_Gd<_}!cjKWEml
zxxUru@@4dojwrz1sj)5;)jIXX0@GDq@CwHO5}6fEb?;yB>ox%3*u;4?tJ1^I!=Q47
zu6GY51h&Qy(J#l;Q*BrGdl{o?0pSNn2zTpkkMG>*=e5Jsi)Av%=-<oX{q-We&ojK|
z7eXt5_8Lfi13Wz!ze2uuA)d6Ewgo)>bjx@e^TqKL#WVRFd|{Vm!CHc^Uv7r4AC}@P
z1pEQIrQ;tBLpCtRpJ<FlC9Cv(6$Egp5;WM{BR&L~UKP$4S>Z;fGwYi&@B8sil`m)9
zw=muuao2gUzSSZCE_GjSgMMn!pf4*7`iqISg@pO)P7NPdv+4r%;}gccx#pAf@Mews
zgwlRElKn;z$*yMIy7eA4xB4MrBC0!)KBfL(f?D60-IFdE0TT+Kyfd9s6q%5Gw}X7?
zj`(e6uIouEe)nT`Np3L}M;+Skf)~`G2KLy8gVqaf*sMd+KZ*!BmEFxO^Zl8R5V8^Q
z3UQ1A0Xu%~_(e#+p!U>W1p<C8!Q5%cEN_Hy%u9)J<YO#L<@y+CjKw_uYdFWH*0TC6
zCF_SZ;(q$yNSV*U<t3%K^nBp|FdiLs|7$#IPa*$~7SkU1S9uojX~L=2mk{@mPFb8h
zg-b&(^n=(foJ2BywxangrNip~2_<A}@sRa=4WbxbL=@XY0-|v%2VEGMGaDdt3OwO%
zP%fhO<sr3if5FoUV#77yhG@i*^~7<hs2v6zzhX=e;t0nte>g74J?sOF=^iiCIr+7L
z8Sw?>*ZWM$BKZ~nHXV^en-^i|=T3x8b)2=pQTL8Gs;%RU9=np)ioCq|>MmlVPekHt
zZ(wxdtD2|H#n-!(59N}Z-ggv<FM(^~EB<XF%oR(pKSdI3ry_QsEwC+c<{$eZ4<*>=
z%C8Q`0Mzsr<k$PiuNm(nzowh~dLQ{k&QgB7x%|4QbYw#;d-+A3{T@U=D`5Me2-`j6
z*R^G2?uGbzKN4SW6ybgT*2I^*FT<%K`}`J=ba4riT)UF<Ta#Z~z|$7w*Jk({w>iEx
zmtW_V;wyyw|BC$T(T_-ei2%I((s&mJeOY19|9>dIin_CP`Bge9QHTCD-~HdpFGs(i
z_S9Yl3f_YJ>fKX*y-bw3{F?t4<=20PM@QZN8jt@k%deu&^psyl<|Xp0l-iev)c(Ja
zUx!J4Ma%(x%FMLcM>mQq;W}Rx%-J7nUzfRrkp>5zBfGQvc8mL_1piIWH11oQ`=$i_
zxhs6?%Mzm#+|QBuhuR5#T&o>{39-z9E>soq#<JUH$HeNEU9>G>PjwGgW?B61dGiY(
zCQdQGO?E4rG7IIVp$oy?hGpi%=Iv6s3oC=gduoT{&E|M2ckfi%)++bSeQg({c@JVg
zyUuNB?w{QW*PHm=&)9YEJL18keG%nwUGzEDg*nzA2r9_&l~mgt>lb<1BxtOWJ@j6s
zux!D{Oqa(mAg@y=IhlOM{DS|#gP#EQwxDgwEC}Hj@U4|W9|L}afS==l-w=GofH%<Q
zgd_7K0lzi`?PQu$mjON+pYxRC?J&pJHUs`V0e{s#0^YhK;MW@P+m-?^y&^k2e)lE?
zp$)eGG9@z3M=;Z6(Zd+0!uNOfu<wRKzDvNGp1v{|L|Z6)s$`+dB%E0d5q@-4RQ5XS
zWNwD_KYOKerL$A5#@wXJd@TqM*FX4K7%iEPWZj1l?3eaa`?kfhPLJ{*((b*+K=dc?
z%1R^i%sRZI)JCw&?wDerk}vaHOzd6e*GLW=)kwEN*Na6Ddlw4V5QnZ&<xsOwnjxLa
zw2yQK0Cc}mvVU}n;tTnZM_Yj8sxLW6uHkpfkR&tj6@g-|)hmPVdRP*tLu|Ax50{^O
z#liCtaCvy1gphi`<9Gcx2SpeSKHbfMQ?XTWv~^#=jQamm8WGkV`G?TFVZ*XAIJW>z
zeF#m_{#xWO7M!SQ<?mP6#aEfiT|%|_>s6QgBuB7~mh+Ozpw-$~7i?=If%6yq<d}&E
zPM?V4iU~&RiR_qRy{{j}I|o$uO=V|xYB-C}r`RTRxbIvyuXSzqdO1`M>1_MBS9AZi
zcVhhR*7@PGwpCvhf8vJ468&FMGqW=XnWj93gZUR>d!Jrwul3_Mt+M-h|A|xCZ0gKj
zUj=2CM6S5Qnj(<hR}{MHCJT5RoluW=JSycsi~~;d^@{tEVJ|L~c`JBVNXm`8{Og3}
zc#G#`_PAl#7aPWIA#loSPAPv!`62oT_-(sn6vr4>3(nlgJE$<Y%~D6}aHC6oU8y;h
z$&K7YZCuWCGMg1LY38<5eKU{TN*}z3f<mc%tkHoSPgK!tww~<SdsT?lbiDm49`LN2
z(ax`|a!b4&ZpbrjBQEEfTf0*{QU0{<xuumK(uy_kK#x|g@vZz_zu9}$iiZt(5pO??
zcLn2_agenmx6^jOq|GI5Bc>Iu(@n6SqjnCXoy7i67h17K=>{Fz-#2wRzu7jUrOqjb
zw#^wu)u8KTSqyR`Pojq446?M%8SOs2<Uagd9~x9gZshZa@myz_UICbj`1Hrgl>LQX
zh(oh-93wknz^qjHvQ4Y}x;8hmQ3Da9kj6jSum2~cM2)l96be?uXrsORrS8KDm3mU#
zJ~#3VH9OBTiMA^)+CTdXJ<w!(ag*ZPxsiWjb0@fl+$IOws;IB6vsc^3ii`G6<#s;T
z3dF8RmCxJsy6j6MhhTCK&i46vUyp(qjON|P44+C%s`r5tNgqLa_sa$OPWpG0UZeDR
zR$qU;577Ic?cLys>HRnM{*rq3!QRw=rpcPHe$9*E6{R0jl0HP~3zV+2fS%rH_qA`4
zewWgBElDr8{wsZx8LGqje{wweFID<QCFy%9{cNS5S(3hdH0f!j<Fi!M?w^`SAFA}P
zmZV>;dIl<e*OK(tlpa$$8wf@9{6Xn|-;4CJlJs)b|D4i~M0YRr_m^qXA5!{&LOL@r
ze~rER`SDDC4X+TNdk0UABC&Rr+Y1|Cp}vgL&r`a>4Hwe$YJarSzeRc={h%=3i81DX
zX6GT(nT6g<pf;KPt0q=(@Xp$t;H(j$GnpCe8g{kWJcERqPa?)|d`5<RvNNA}fIIrg
z<wo3EK0Qm0#@v|71h>j^mTzA70B09!n<uB4BMIuyy<gwwtWV3|yU#1YgMQc5W)oH|
ze4iGkoK60U0n|>B2EtCi#ur@_9o4*}<_Y!k{m6~+>2vj6_2%{D@!-Jb9ky50Ld$Lb
zyX}XbCJw=lbH|B1sOj$vS)=PMk|bx_#M8QBk`T1EkgE*VPD#|=DKxdd7QDF&f|@XA
z`?(F6$lvKKa=W|LWolbk9%RW#(bk2YpR_HgC2n1j<<q4*B&aPxW+!kOGw<NlIAo1$
zGZVyT{+Z7nei_^Pt~)L_K!-OEUQ}z?%Ezy`i!Ay^m|X3E{EFA?z16S~hOT2KZ*|5&
z<1>#V4Lq7-AwIHlW36(1s@u27t4RH^9VRe@)awf7Kq1-`lDo8a_vvF}+k%)bH=vvG
z8M`x=BZz05%p1qIg-W9P?%lijsl4usE{uOR3%4TlP4gK@5y@?;y|JDoMAsnqI$#&l
zYWiavBT&^LuTL6k{qwI&C++KdG-HUhWy5x4S8R{~F5H6>X6jp-iH6Eb;yng^)Iaz#
zOf?$RRwC3G5{~sqxF7dUCP|*n!Z6N@qZc(yp0fO#d>OI^nrf}hR~mcp^Esj`HSOst
zK0_XIp0@rLTuJSK@TYx>K-g>?zDB0*M@xu1-f=V47?+6j`+|P=Ok9$>;@)5{X49O(
z;8v#>QkmYt4SeZpt0F~zXqVN6M~#6`)#&K2t8I26@s7gdJ%z{4!sFt?<Fdlz^1|b)
z!sGhF<Ho|HMM|Ju01FDKl`#W`PDt}b>*ZFlmBC0xPs&dVOwZOIsmx#17S}hsdB$Db
z>L`1H|8()QS&XTVZV`QiQ@f*1XXmKX?%Uq(n-cU#xjUk7+qiE^@ZW?CSMD8DdO%l#
z|0e9XZ%?{!O7PznN8kSJzA3?fTNZu0&V5sY|F$ancCq`W1pjS)^z97yO$q*+@a$kY
z(tV2%&%zcIyKnosZ_!9(=LAu??cFye_&PN-uH4#c>w7p>&L}a}WqY*RRS|FhDf0t8
ze$%MRBHGJO(A*;ax``@QovGaYgDGJWEBB}#Sut_W`j3?meKLDaWpF-&-!`X;FQ|nR
z_~9fN{}RzGW7HjQ|B)cec38_9BlA?-!-L4fmfw5)Bw0pkxqRIYsG7R3u1z1azZk~9
zTwbFWap&vxDmC{<Fd41j#Tbv&TDI-sTE0=+biRbOrBvH-s!boG+Vmooe7CT+RYkRZ
zAc|^xxKp@_x1XXf**SU>Vk?4cQBjV`4LzuF?%TZ;pXIH7WY5^iKj2h8JO~8UU9O%x
z1XZ?UHa{7gzVY@GdB*6I(ta*~MA`EJJh%<RUv^Ws*aCN4N#%s#5SW=Z)kMX#qNmW>
z(cqf7ha|vV4Y&=NwcJbaj}XA515g<#svi$vAKy_|@>f>!?Xc7izFV$TXQ9;dBD%JR
z7rRnpN=q&8Ysk<?+R~42>lRmPnJcBO#)eGP)^A)XeWa9re5voNRJLO|DUEaA0^OWy
zY|~rtV~V)fReBvCy{^~m3B2NWOJ>a*cWx;>W_Xxo#b!YeM6&J|y#{*Sl~+F5JZdY6
zD?e+<5}l{M29kID4AEGzc#w9#`o|XQrti9?7j;Ku*zr>`hGfL#s-<@Q#XJ7U2V2h;
zj*HsW5@ZrY`r}d1=#RxQHV1m4KR9mT#sa9anoLk-ygi5Fy7@f5Na}24@^tV^VU6JL
zSL@*K8^NFN6@3pbfdmkS&0j7OhU<YMa(fY#??)!PJ#<!ZoXejq&fnQH{{|48{}zC8
ze$%En8OriU2!0&DeW&x=iZ5}7HhiBeWP-2o{Sx<nPxyYAd%q)m?>2lqZ1@p-!wb`R
zPJTMp`&q94J?&|`2gh!w!8d!_Q~jgIYqsYxGOZQx8Tm)s;W@$rF#xz^ZW=WR2Xq%1
zwRguvAd(k9R)n(5tMR+vN7dynB0Ezm3qVqX9a}6U7zsVux*o!9*@<B%6iu<NIxC&~
zp<S-Sw(z>($5JVB#L7tJ9v({@!5JjD2(4;v8O<r2WQXnx7oqMsu$=ZKpRets;}45c
zSqzOisn|rS79Qbc3B}#e!SBq#8bm^4b{i7Y6W$m8ve#N(1uIg!^*GrOts^(G<`^zQ
zo3KFp{KKpl_;S>zIV6}09;*`KhzLk%jl}@uyN%-u8@`9IyttIat`1oMMBnQ0?6Y_%
z>b1=ifF2J)^gzh0Y>3>BKTr=?1JWtzvJYEtCcG~%uiE4NF_0k}6!2lA>SG?Pn6e}5
z>0-P;*1A5C?Rzz-+T(*SVTxcU9aoI<cgtT*)NyAISNZfG_*CnHY{Nv*J7X>6N%)jw
za|7&*Ny8AWyE@c~A$Z=}87vg^a9v2eae0~I+xk+fx1n4kl2bF;ZI8?j{RS?ojcr}A
zDB^f3)mtI**^sYpnQIXZ1viB{R?!5wtGE&m@A!0V*J5+6mGnISZC--KP%u+Akt&+9
zAA~A2hXHIIaUq%PzFXCJ2e{*R=N$ltEqL5WPFP9{OX5Gfl<cVb{BO73Th4qFt#CY|
zJU-*wnsSaR@D%z-CAkxAe=ir|&ci_qdySmSVDvOvzBF04s%33{KPz(?&dP=c=*<(K
zBRkOHC@+7YRx0t@dBHI|G4O7>xP5yEab8Y2J?=-Bdcxt=boQwFZ0p1(&Z6KrHl>kk
zzA%9?&j^d!HezbQ046(IX5;?L>ee@J*ER=6NI<iFq{-gKgm2q;R%TxFh>mAw??VTg
z_c?9;tRc^IFLU&WeIL@!%$u##FnIASJx%secg%=t$_96y#{Q0~+Q_Vo^0hpuqy}ow
z%){T=Zg*fKWGo8>)T!*5tFk{P4r()pAFgJOW?qip{Vb;2L5)PG6G=z@9$m4yGy;v{
zMm?`}J#o5*HsR^5JaQK_uWjJTa@{y|wl7OiWi5ZY{N=iZB4wR5THLxSL73FpltGc8
znZ4AE_-EuF$owd2hxzC4S@HV|*fWabJ5>|ksj-gRY|pM4#clM%s0&_D8E?nyN6rrM
z_Cxhhw*ccuyj{V|*v4`9;xuIzr*MJFyb$aP-<3zYJj-PtX#WupxQgKn#)T+f&x@b=
zd6lTUMQyZ>bhqYQ234!1W+bjijcto!N!`2iTi)wVcNfu2dCV6|WgDuG%nlelWqxCJ
zdOfwUL>}C$<-P|Yt4*%HgRQ>izXZ+On42l4G@T#0213u@&f~1$G(Irdo=Y;>qGKYL
z@!bR@m4bI2cq#G!ulQ`!)JtpPWzALf?U+CMkS*jBWG>ueQ;5SFyib`z0~gZ(t)4-v
zQ9Iq&G2X#rCHel#<vTpgw?FXM3FXkf72H^ozn%QuPf`!FCVow_CN8HilT$p@$FIRI
zm0&jOYx^)B?2_;l=u&8{${{P7azht&Cnzs&GX6-jig4TWb)YRV+(s&X)dRffx|`L(
zQ!sRm)@{_J3xyK8QRoeri!2PWS2J9Pcu2(+yOx(ty2r*uWp1=G&C7z<W5o!048Zfc
zMaJ+NZ*St!fXk}2HQgtB{Bj3-E!bwjm+=FXv3Fo^v4EC{8$3m$JKU%?e$}sead3Xc
z!}$hvIg`Pf_>64;n%bm|P}?AWGHV6*Oj%D#E50BTf&U8R0#kY**rx>iI|Tko^xQTQ
z+`TG)a#?2t8+#Y`^loKk!w2;de&#8+ZNweD;vEXn;)vI7K{?+la2Fp1_m)&LTa~;4
zBX;fCjZ+Dq{Q~L^+Do!OqwIenJMnI9rnHT?f%Z7M;@h(d%6tVf(*{SCRIox7Tqwrx
z$jFb<4y0%Xv{&$8!}SR|8^QVZ3F*@f!0#YZe728|h~#;)d4kHkdfCfGP_;Y};re_l
z)x0$LYp-Giz9(2#Q$cV`N%}^mKc)2PCFzeS{jW;@aY_1XO238lXlm>S{xvl^){>g9
z=8gNHEOtAG<BY*qvTP-AWY$fE_9w+2ve#>KELHbj^FX+`xp~$b0TE7%tJRFdFKk`g
zm^DpSx0G<X3FL+fwZ!hbuQ+B$c8Q=msS-pWMza~(n9VIV%Dhim@jT34D}tXT!nAsE
zJN8emh54;S3xX5hNi}5d<R@MCdh_1p-BdR)k_R?37Zd=S-?|3=RvEIa`Sf6*?~(IY
zExnj(J(b<i$O_qDGY})#LOpm9O-mo1>wo9jujJhP)>VSBV;q?E^I70=?F$JWXtm0`
zpnP$;^(8<SpRZwmoUj+}+GP~?WcK%xCvc_q*Ju~@x#c{yAYjtjV^D#w@8nUtl`}PL
z!lD9Tx|6)IP8+THx?jddi_iW3P=)NB@E8N<#wa%c-O8?*;R0G>R-bk{E)(OteN5*%
zj;WrqPt=a@r)x(-J8Xj6e5F!`SJ$YeZ_<+8V_z|nr;s8djdD~Pfkz2-Ab*<SQ1DM*
z#k)A|u>Nq`;~AkBx75?_zQy_{<@!QGim>MpU?-U);FjnAX&j1kWK%LFwQapW`Nb3o
zLq#Tat}XLVuR=6*tT-k&;J~AD2OV&9F1ZIw2^~VB>5=2&NI)VE5MnNSTzx}sJZ{BR
z`yNiJE|~4t#DgxM?dm?tt@pSusfEb)i&MS*9qD$#AyxH{$$Os}bX2WgDshlyEk@X_
zE;I-(QAXD$oMfy{6!BovxxX+O4)}nvWIj~gxe+WTa<RjCzOjtVzf63%F&8@|S-$?g
z_yN!8jk;6W7$+zWSW2YJRBwx_%1ut(1I)cUXK)x{9HewA*YxPh<*~X)=sD7KosTXM
zex9_ab+hy0=C9Jo0qN|8gMfxyPx1fX(A!rMVe%Dn;^{lC8{qmMv_ri8SmYGhzeEI=
zbngpmX-`hJ<m4J>4d2+ul#`4nkWtIU_C0x!SpJCdU>~NFju|#soOH!tddw~`Bdm11
zW24M`Gv@uMAC#_s<s8R<ESXu^IQO;QZg6g_XIh2ha~&CMSZt8);H|r<Fg^#k@ez+;
z?_v-?z+XH>W=C<O)Ws;dk!m!UUW~Wbxske8BXv*q&PoB*bD(lDp)eXJ#_iDZpfFBt
zDQonArP%@ZaFyD>GeDSAX|T`(&P1ns->FV;Wu(T6c5AmbUO&W4^$z$Y4=Upmsor-=
zSpkC0hAiH{iM+aR#crZ}ln1;h;{H0oW4mHRcXfCpNs<5*jB9X6B*iN~O3>O4;?Mty
zp>*q&I}qJ%=KRA#E?kFu{^Y61lZ9@rhCHi!iG;SH9<EoMkL)YhN9^uZWzd}RrCdHa
z`K!IKmmc)6N<js9T-!Qe`}|Otm7@=zi#FAxLJp~Z;!mI(+<S~O$wfpqXYzMg|I!nV
zfyd7jk8gm+XXy051gRE?#2?9ysb_4S?mjZqKiWR))v_-aM5lUN%+GZ8%t5rhjoOYW
z0_`@qycomS2HT&`z99gZ8grYcB(g(yZ2PRd<zZb=njN}}ROVM94bm{x*~6w5;@!?q
z_rApx0DNbB9n@Y$<^5yCMW$snU`?K>M3?s0P5}39*;noz)5n%89iGhjV^3O9=Njj(
z>K%6Q`g&o6;HTX`u=P3pYjph+s?yn8M5=`@l3;p7SBzx$A(j2BsM5ig{i{d`zM$B4
zx^X?kL8IY;^`6dhg}AtyvMKU=bj%FVv5QB?qhA~nhd2YoVe3pAl->aBh<hA%D-ByG
z?H7hi_o)d_KCnz#eh$t9tXHhMGY1t$+K<8MuGW%~_CTs$jf=C4Y?D`vL+DupHmESH
z9+jsThOXyuX5CnN6q=e{m_M!}jY$DDrJ(r{q8;{W$1PJ=2hf-(bF*=x=Y5OOQVc$U
z1elNiVSBFF=Z*&86ovu@WdxI73erma1?{#==Na)>@FauNw($^7#BgiHX_x|LHJ_Zo
z%HdSBDUEb<=d-r6vnOI@RehY;JY?}jU(b!=QZVfeb${9GEy;>XvZQ+>(>r;Rd?n)!
z#D8vi&G9+xHK(0E`w#{;(#Oz8bnz0mxF&I>a|VqDwg6_lL9SHx%#<CKfPEaD9oQ$)
z_D`5{o-gqkgkZoF_RsR>@A7Z&malld2Q@ZH!0C1w50F^w+jmxS^qo6Z+eyKM+h@b4
zh3G9mgNw7wzlc40TjqzxyZZ^FC$oYv=QCt?;+U`JBkAn-YPsLWCdE<kkl5rnN%cpy
zjp*Ae-u@76w0(q4SZ6ORlUSOA3i6qb&9Lk}73=NPY^*OcR*Cwuw`k(pK~lnWJVZ9r
zv#qYLP#4Vf%KDf%$eC)ZPcI;HxEOwPW&Em}Xx9T+cXTBH4f0f6)MP9^^Xs$`LIu|w
zTLe#jE(AI&{5Z$~nZJufjW0+h)afI<m0)5}DJJ?vm}ox2Apao+6o3e@&#4xQs6;*!
z5iT$Ca0(^cC<CaI>KwvR>*2sXtxzcJoT0yG&W}a$(SA(AG_Kznv2Y((hpgdg=TO<0
z;k+25I(;!n0ibYp^(}AY5Mb|_#^xAiDQ9(S89Y<L9r|c=Tsi;rN7n3xI){6RNhXBQ
zmAlcz(b?EeG+ox*8lS1f6VLQ~6N5ynzDd`LSWm|Wb)r)BZ8v#G7*0KjAY&&8N_S!u
zX1^izbdMiqq#r;E_Er_!6(G+i$aT&vE6|5WLAD=|;y$X{i7HG3958DCJ`D`N022DG
zwG={^?)`g##ursgQ=X7$iks-Jy4x~`Xet1x%pCpg0axD|i@N|F1ljkB=HFw?zx*Xq
zRKodL=s=-PhaN+B@5wv(4jA;!d(pq<3)5MHwSZbd(KnVSA`ZShW;-(M@K67~MI(^M
zi-K($&FpaSHN2bS(xPfY!Fk!^ZC+Y^Vcqd@Gi_e?to{+pJ(W>sV(0H<SF8Y{$FJ(K
zHe)qg>bgF=j_V0Do=cVK?6I0l{h_F|b)d(X71gP1<;}m%pJ13cwz_+5>GV}rgjq3@
z$iGqoCxlW0YM9sPApa5Il*A9^res!%!~~eq;P6C*_PGdxWs?q8TLU|JereKPdftom
z(Hp$JK50i50MBMeTtRC8{)x;tpc7l?FKX??v8AN`WcK=rkY#rIZ2mPtu@*{n5(Kf)
z(FLwq+uy<0)P#*}AEhQNZ=5hMl{@HDE*-&=oSwTekvo#32i&wbcXbSjwZ~+ZlbmCg
z#b)Q`Ub@^erpsP;NaxN#mdz(zX!AEVrn=^-f+1c{RtKc!tzzY1orq6U%n~}X*lx{u
zQGMAi-cEJ8H-X%JrhI8gr`d;4X-<uy$OVL?mu+KlOTMu@Xr!gawtQu({MA(3JC)tP
zN=;an$jnV-I#VCM+_3+$50}%4=e0ac<WAhwG@(<VKMYF|n92CvO9lRY<tD6mNacn;
zo$8t^h?^!XX)2!!sd}8q0pwe`zKap8gc}uPS~f-fm^4m(NFcEq>XxC;q<5%nu1A%3
zUD%(yrV#u4FAe7_jrK~Ff2<yD<9f8%dbGN+Jg*+5=u!87NtQ|lm>*oVwrKj_-e1sH
zY&^(|UvOo6XJ03i6TkC=(~1jROM&huL=$2A+8-vKLn`;^RAI)Qg3&1+=@cobC4lE8
zvyaZ^NB3gb-p^o*nqh%6<rIG<B^BQv!`fYTy-YCaT>xN{Xjo(Jk+G0c@LeTn`-iM)
z_Hb^{FWa^xP32KHdJJ;gkL(TMbcV5B*>BaiOspn&i}DQ}c5A@E_>8}joX8!6No|IB
zEI91<CIczI6pinY&hEX#{t4~L3bEf9Y!$UZ$`vXVktT@L@fsrs+4lI>@@7H)$@(|L
zm>Z4ULL=RKM*J%V^}C|9;Od>%{ADbFrS%p*w!AS*y}AP!nR;Ea!k~%$jDK~aRG}+&
zKIl`|pM%t&2~{M;@M;__U2yJ2C-X`mjOe{VluW}hy74;yzr-w@r7I)rg%fhRZ=(r$
zamf6Oc1_hSZXVRL(n-&UAZhB;XxoBRVO7lrPw9CIPcQDi6s|!Dk4_vjEc@e$X@~*C
z9z%9j%~a|Vh16@Sc`^e!K0AGurH#||i(G?(?M$pF&845Rfskc{*@xws(3m?KHv)Gx
zqP&UtI4Bsj?$b?;xoN6d-)_~f`<4n~?wg2B--KfStu_dl0??ccU<$l6SbOgE+Uk3E
z2RPyAni0C5e#$^S>3(<(OJv@aS=Vh}4o_trJP^bxnvse1(webeZ{}FQGz}IPT=$R7
zZS0B}Ho<>6_YJ*{S}H|#6Zf$D{N$JqJjSQFm%4QwbRoAD8Fp8btucFD;2J416kEnb
zcG^8aD1G)ztWhAG@$9<|w#p_fPsHzDAl?kZ&Q9H++s`#S6S-s!EPEA>XoST|<Qk%v
zi5?z-SHT=_%-%0BB6~WBaQ1H36Z}5bOc_D!Vj&Y%b0ZM&rXsw(!yABaG{C(*q$l{e
z0!-PRO!B6HpHu%l3VS>(XkAD5<ZFT238?k)X(zL$&{dvlFv-9ThL<Kdhav+)`!47|
zf~dNo9ocm;ZxEBA5qG0=?c<yA83%(d`lS{%&(0r$2H-BeWGL?e*d{lkc#CXP8II3{
z*n{wsxtxCs<?!}4#mir2nCmyfS5EEw5%VAR(Z7CUx~{Y36>PEPb)U8_NM+u&Z7VT2
zVocNr-$Q3ptX`hu9g`qB4~;gDTo#H13Ck7lh?5}%uqiuDVEa+A8XB{YtamUA^dX$~
zw|%w=v7dp<r-Vy0sQ^2CP@DmQA{q0tKz0W3MBM^ZOhF!ds`YO?DhfmY*puem^F-^t
zMy!W`wM5$^8jSMzw2v8D4c*C&*(p^(B@hxB0+SIriTeAU&G=YnwR;Mh@%m>Aq4#7q
z8n%Tz#TCjjTy?|9Yx1Nr1F&+xw`tUP->3UU?HZkl3d_MxfEyrY*x7XFnUsbG{Ebg$
z&K65~SGRho-yLgGU8=im%F&FX8_a2LdIG`H?mm7zg@;6Tz!l{mrzYq&ohojjkML0^
zbGFb?-uj|U$K@;2b+5JD4t2<<EgZ`vJ_Gf-UT>5e)vLRUM)g~L1k05D>0inGv8D*6
zI@X6z#p&Nz!fKK2LF85o<}}f+UfAj=h6O0hKVSJZ@83>j@3(o+5sST<sG6`HNV@`8
zGQ@hW6s-qR6U@OPhFQ0;SND#eWMVeUX<uu+j+aDDovQY5CQc+jZTlnvJanQ^LjV<4
zQ@|ouLDnLH?`(iX+=ky7JcfiI62YrzK#hi9GJepeQAi33d0No;M*N0~!YxI_T*V$#
zgh@<W4a-{JfH~XbX6DC1fstu$TTPD{j#-+&)Lh{)MB6$KsD^A5Js9m^y$=Tr$d^#&
zG+1#rZ_ud$m&)|5F^~=ZkaUv*9%}Wqsk;|YNDOI&lo-hjE1>GeTXpJnf%dHeI&5Sv
z;s+5atI(2!k5NN9DGh*Lemguc_CkbwxFSTnE~=U~n>G>b^Df$j_$Y&QbJgI)aFY$N
zypOrO2!YB7&sZjOfVx}xWh{_(Cb$H)4t^lHE~i~2mzE@*MxTpX^S-Al;0Cqd<saC1
ztA|r~496Ey&anQ))umpgGD95(8Rr6_b&P{p4enLI0o)ber%RI#7c|#c;qL9C&#F%-
zm0G;WO@@g0=vC_rlnResDjb^K{b1xUr9z!`3P3~_68Mu-h=!>&-MxE=Uni?QrNn1E
z!&*#YyyKb4uO)_GHw^N=Y&onh#RE=Rv#^E>Ko|$G%<b+H!Zm{~U<T2Yq#k_Q2r=U8
z!Fj}8idg^Ipdi$KJxeE?KJwg(KB5}L_$0Bz`Pay4d@$bkNrHkk<_2`a(-BeiAh5vK
zL6Wf*NX7H9*psa+f^hbIqV<vd&sfEXYa6eI=&7#XeeZd8%?t<!k}5-vt?3GT4yivP
z{wY#O`%R|s=9F*6A(ug#NuSU73$!mz9ZI!oo{RZOf12_9WCtEYFt^B0I@XAbSWWSn
zx3d7xFXNwxo<wdRQ^%wm#NSSVM!;h~qXW&PRUn+aps@$7Eo@zx-;s*q?XLh_RGlAY
z)p|R%#;3h%nv++2is)GMt8I@kYW6}d)su`1rC70;lXw|#!q0?V3iBl6jFe~q=i7PV
zHl!1B5z@N1cXD5J@P*}CvV3)_?xmJH^Lul>sW?(3XE|nQ_qoqP#vg|&G37YVoFQyc
z=kdANQc_HXVx^zT9a(v7ZbWUj8SMmLs{Ex?otUY%+D^rMZHiH^RAyewE7*}SDnHSB
z8(ptgu)0x9n+{IK8AfLa`=GMlsqYGv^0fkp_>7A_RPY^Bf5flOPh)P(jUHC%&}*e=
zPK=(`&Twj&*~;@TZ@^gxg|Pj#e>ln~U%&b59L0~)O10jIsf321+5!cCA@Vhzt$Xb1
z^OJo;z$*b&tQZ~V+jUKbt$cn11;r>ooSF?C6X}#kNB$!cy-F-hw#wk-<Z(d-Q=gw&
z+hd}VSTT>ptqCTkuZikVI@yHE?@9edex1>Jz?LKV17Vw<Sn{_sW<`(`3EU*D76k4Q
zr7*bB5SIxhWHBJ<3&nEx<25}xWu?`#fG>_a{A;;Oeg@8lF#ktmkjA-bUtCz~h}XwV
z|0$lqYw~p%Ty0FA`v9k_k6-yz4we_<fDs)CoU|i#5HkFca6{y`WXd-(>tOGu!mwQV
z+I!xo>;N7@nMJ8iFeoV^GvMUVLef1@+$Xk7%p@+6B|Vh^h|l=@yOG#>B@|oF!Y;hn
zx_VQ~LMy6UW@7Q~`Ji?L#_SpUtDIL0B!nL_iQB!aWi}b-bX6`4_-ws3ak-O>rwqGn
zMN{_Zl#_`-go#3RcXdDI$G(`0ANTi&i>G+}TrM8*@cfQYdvNQDB3sugPzsv+e#zUq
zj$)p%!ViukQ+^$ECndK1Hc}<)P^U&9+CsfVai>#W^gY-uDnSip5*Rc^at|f3QtC@6
z_=vG=#5bi1cU!OK(cgZPs3Vbi*NUezACs+gVp}~JL!@7Yb)_;ouP{EJ0M=0D5#B>h
z$J~_0I_Ed%@+M7ZWxhZ8gKJ+dLdDM=D#~~Bs7QKL)RQUyrZ9I-arP6)ZktoVw~CWz
zlH5HzF@^&NjqYSu=-cFNI&yK!-Z9H6zf|iotR~Jg0nc?=b%rJB=h?Carex_EmB@TX
zOu_TBvnN8m-2F`2*rMZOg;QgwjLqa6pU&QH>-xs{F)zxHXqFe<?L4|>aB%30SbH)H
z8z(F(u4BYP;K3PjF=aLdJ6N7Ya_$|!^<2RVGCopgqpU?Q2Fvvh)Pvo6Gw01rxyfUL
zhnc5Q>aA)iH+w2S!R^H#?r<N*(#;^N4-@EV5}G@ENGI(yYvvA4<c9W%-~9qc^%=X8
z->*YiqtEBf)+aJ7L72^~x;DjU*!Izc`KbwtI6Bz!c!SMk&TTwl37B)H^1wT~Gr!)v
zBYQthjuP|ZXaT2Rbn`^LX1AD)#OFjE;pR}zrMi%ZE0@x6+bSADBh@MP8vXd;B%p6_
z?Mt6>%^Hhq$n#b{sYkAdQ`oMC;8<5)Cr)8T#(Cv+&-2nh_IT@RT0g&I$&|0E<PDUR
z=(fx996t@q#a__ql{&ZRc)T?zZfwe41^a81{~61Vk6HiNW373>8IR5GV6~#rpq24M
zI6=XA0s||?I(}YxvizmG7u~rS3ZZ%tecskesd5hdsO(hA>ZLx_+u5=8l4>mfBvsxF
zKFj}N2&7zd9e38pJFaCPNFh<HxbjmJ6dd#_wnZK|m9IqI(>gfVHA?i?zbdg;l;|&t
zRpOezx#F9zX8ifD;`3WNTcr!ogjV=XlB+Hl%KGe?6YV(HqHM=7O}t#Ql%;ja>~A%9
z0cfl{gv8UZ;1vco+ff5w($eK<@;6TCYJ8ZC{Bg!J=O$RXE=YBCviq?q6+dQU@VjT-
zj2WdvRDK?pWLd_%(zG2PT;j8)Qqc02Ya2@z9qwY*ayV;?4IZXiG{~B#FdObyRSI2W
zGCVvLSoL9i(2wq<n823e51D<m+@)z5rOcY8Z;{3@jZM~VOvalw(&w!-{1B@^xQ-0A
zSP3yWiSj4LXY4{{(#IRS2-~bbw`}ZcUF$t*?a~n$^u}>Xv@ju!<oLI2$<0jMmb+v(
zjjC5|RB3sT8OAx8jZK++kGW{K&vHqCs$$0G)m`X|>+{wrsXloB`A-!dEy1OC)JetE
zsjJud>>;QRW+EG1W6Hw+!^Dl6(Xf%UC>jEB&YOCxecJOU&FO(o{9tq$ji=_BFZ21?
z*0<4krktb_8A=rFHOlXeRBSU86?jYI$5w({r%=#Ui|x^8<F4@9$6DXU1Gh%i9%|KE
z`*z1r9UgsFozKTw>e^1|cJ^4X&w62L63Qfsc=L{F-7H4uf?JSp!UmTy`&)T)q~P)H
zZrYj3i+DiFoss^De;wss&*C*W7%D@{_#K|cUYSc0?EHZZSofsQN#R(!j9M;Vph+0|
z9<oa!YTxXnbV1_TIqC5#>HMy>E37LYUQ^F&--xIL5|KCe0_&oN$PG=st(0O5M09mn
z2Tx__#x84pM=?n*2oYneBrEX}%Q-6EcjfDm+yTqu{lRLXUA%HCjleIj7s_MLiC>n_
zt$Wtkr3IlLySSD8vC%|OKV<1e2Pd?ME8o~q_ioF*iATp!)ZFoxYGXXJI{z@gs#uvP
zJ)3w`gVc@B?-)g+eL8;<oFsbrGb@HrzgE1Nk0ZsS?eCMlhH=b9xij5j91PVWJ5wwx
zky*qvU@KM%M#mDKhsE1><T=F-b%BRTwFsxTS8Y;t0j%;osAQrk4$nWSX}*9_W3$?;
z6hTw`7;cc{KNv-DujMfH!?P&ru(H3jFQx1&{B{}NBzwndzSdJ&a4sp3<V4&g*f8d4
zd};!aASOp4z^d2>(qKpWK%LxG_ub_+2CnJp%_(ZayhO%iX3Ym7A4V4v^}}>9AsU@+
zd;&Tv;$f`v`95VA@h^_RZpOb1QsXr*7IXuyt?I#&8g(tRT}}x%f_C4IIL)ZwyHCr^
z1v8p0%1FkO8$kl92ug`DAcW0~nCCm<3-$D0`<)ddR*9L2MtbmxI=mi=xXEe=_;?lQ
z;|qlAvxxsh{KE4qu_!lh#w~Q{EoCTg0Y;i3_=;-1Z>8gDHqQn+{uA=QkpE!U`1xiG
z2%o>^9gQiEg8_XR%~Jh^0j&_vSsC;--BSvN#bx-^xVGm<>m3VM#&y9P_VT7)RtGQe
z68u`Ul&km>-si=<=y`SrLtb>m<BW9Mg2_dUDAY4KrP^Fm4`HN@H=-eQbWZucaYqqg
zy?XuVt6DAnOa>k?0qY&<ecuK3i*4E?#i@bG^gP3grt}^=%ujR1bhmK+k{#R6M^S-Y
zrPa-A2^@>af1P=6`l90qd3oYRd|-cghV3VJEh2f759DEYylUa3<%iAmWcGiI4>Z>#
zCF>yoq(5q=ChA0}UK&vLQRzNuzww;8rbdB|y4q`I^P|dne5ml)=^rx>IqmV`SrjIV
zrlN9F@p!n%h*a(}X7WR3kqv$g`)-`DF!d;z_>-~}#=#&0`Y|tf6=+m28K+S#H4q7k
zChhZvD;HFycGHY0^I4F#yf7lIV78Roa_vVhr{M!xO6t%6rSLL}3$Lfrf)KM<zgpt7
zJ1>m&Pmzc8W%k{jz<<TkI;0fo)AF!LvSxD7sc+2NiCW4eWf2&~FgP~_rJv6+moBzj
zl%~IU`y{3`AL_jpo|#v-DBko!O2=mT149$$F%-A*9P;B--FoZM=p_#RvFdq!QHhAz
zx_k$b51WaqKxGsMMZ00v?&|5hcpqKsDYM7fDy$wC3*)bmy~@^G?M4}*{s&e7)`Vpp
zm=*iJFnPP-4a4GBJwb=m)lsZ7%A0g;-`QF#U0PGceQ+T^OlMw7XI>We+2LV4{PM||
zW7^^uq%$`e$Uzem6#av&W91y7ZfKeCVekPAGfat?#Yz%kjN{qbo}oMInJLeBElL1a
z0Wh2r-y!%~s82<F(y=MqS8Os`QIWsB9P+g=zuQleUg!B*flm$$cM(cB?^{wxeuM&~
z`f^cy;=5iSDarn})RD#IrlLI^pMDolQcS#(^{J^qkqWk!SJVi{^j@=<O?ufBJjV;T
z9gC(VUQoo~oSgU<Wb=<j5qY&cXOIF-_Mot(WR)ej-snrR$!HA!0>xk0oW5Lrkk~a=
zG42RczEoJRl7DOT*7#B}y;Tmk)!L;;7m*ueRUo(JA~(7YQ-ciO+cLT3uQFY#9vpYb
z;Ah)Z|D%z5mZ{x+8~#QODJ8}`_9dr2JjjQ9H7}wrkyD;27U^S}^*7L{g*m!=p%}t7
zHLH}6y{uM?w(Nbgd5iRa*5_AHtM-@2;YA=LfY;tz-z2?bJUvL~uBqu%6*=d|L3(a1
z>C7>g(3Dw4CzV|cM}C|cwu==AM6zo<j9JCSm_yzfEO_kGO+@WTmP?|I=ejcZ5*NyO
zU@gLi;N&{DL4sBGimk$#J5Gq{H}^&1dR1dC{dgrr)=qIFdMOVi*hM(?8&S7fIFeoH
znKeVt0++kcGbQ}%o~d|A&)$Kv7x(PF3iWKtLAjF*%H2aKM?c3#peAKc_?ET26D@gi
zQUwv=BZ_XDuA3L1z9yz&8ER5p>}cV9jkF6zlUc-U7$JHaFHy}FoLN$@F`>drmL@A&
zUcj=DcE(a~(q(B5#hRlBW=>xjgENAIrSk6^FC34g95_j3wUBo^E{t%cHr=h7um%mJ
z*6-9L@b&{dn8*ktz-vrub!?h)fRDqZFX1{f9Oq(E>}u7XXGDq;@*+i98-auUdwOcL
zs1yhhx+A|0B($iaB1_Sv$Z>L!o)8+~g2N=H&`(5-lRGxFRHr65LDSJWxv!h)@vBZ^
zBn`E6ZaF}}VTc4{0jOLTis-Zl{Q54W84{}*w%oUv3+q$4^pj9Q@{7=^)E{+wJ-^mv
z_1(H{DPN@9p#~Lo`-sKHrG2+Mx7_V6Xf4D)zs|u9?Q(8>nu}Z3ZhA&&jw|RJ6P=oY
z-CaYL!R@!o?Vi{o^APzhYGA^G;$>ZKE-|8o!h+ft-pJ$U6{a?-v%-3$flhvWVa?~y
zD{Qm4q_F08W*f>7x<U8eVd#ZL*@<WYy^7CMYD0O3pN<ydCR352bL%jl^~EW9Y`Jui
z)>qAkP{kIj*a4xQFaI8uGuf?j2>6nMe+#Fgi(YM~uG57R`VeG2h^@Ho8FhVICaXK4
z|3g2yh<#IwV9`#+wVga5AMOBvBL2wHa(I7Xo4bzr=E<7EgRHPGb<QlB3eTfQqQdf{
zUb>Nwc-PYqmsE%G7Ch{)+6H-qr2PNz6!GV-4$UMD7OZ3JNPffny=xcekr4>CsU))Q
z?b7%JuW+WoBLrrmDbRz_d2VK0ZsyaBpSib$%&&;ar87gdpA%vimI?0YUcXZ*WjxNY
zuo7Me%M4kZH569jpv&*XPHRXHA^(V9`5F6$(oS>#;31Vg8G(KU4qK~vqrH)N^m`C3
zVoNi>r8AIeQ0Qo40}cr}FI&gkVn|rzEu)soxuudMw<eeuUj>#Ie0u>nzl65D-M;|W
zRqUG@TpZ2`;X=`9;f<OOJHg;;sx1Izl;0Dszc`4E=l(lz%|j;Sx3iY7b*j%5G=39~
zPf+SIs7?(@hJ@_{)4mVmAn6g}y6W&@44zpNp+n3boZW7)1!L6Ik*;Rn6!@=YuxM)W
zmYX{G#DA};yIfO$pGQ+q(o~5WJ?#N@njfvCGr{f;iGZC>1_kG@XTPsFUPQ_L&X@mP
zns|D`xomc{*Hp2jkbXB<zeuw4wThmc_RCSNoo}TDG54wZr9`#ml=UL>7h{E5GfG^X
zoyv~bw^q4W9nYo>f{Rg3=+0g;QAHJBS66_R*)PP$FYBn9?3`I-lI;xz!@#O_Uj}98
zOazst=ejWNWYE$JQx=8z#B+5jcgfgbS0e2R60!5}N`l^&Hb;AM`XrWMu#qKL-KqR@
z!bPvm5a3>{Z;$C4T)P(X*!gC**^+f@3JY;$J)71hR3L|6s6>M0o>)njoMF%_7dFPz
z3voP&Pp@Z@#2FK_uR&MA`43WcW*vPEzN<$wr4WZ!GnGG<$5O>!#@jNLi#Y>}b7vnM
z9H#v6K0M`7PJ;c4Und5;TE*-i$sAFzI!ES+#bKz<_|YE+?>ykvH=K`}uSE@eW^P+B
z+gwvqq{rh07|D^kMdAmsXIq0Ls2QX+4&v6i&{ND*>UQgdc@L_b)B2-wz%I5u;|#1-
zB3B!a?7f{7V(G-k(6Iru_dDkC$WEQ@dquK5Z$@Zc94IIj7+jrR1J|vYvVwQDKBsZk
z=%jLO8W<n7m<qF6U0}fEvCT9Dm^gMEI61&$KKV$LafgazVl`sb+CaF;>9^o}RPZ)Z
z0vcB3TBM6&QRKMHg>AzOexi_z(@peP@6{A@eY?hp60<yOaO*8aK$m)ZCAFxxr#>b<
z!mlq%_0_F)6$Sdznz3QeqHg7m8m!Tvr`Cx}<%p@JwiZxfcifqqRAvP8Xl2mv-|QH8
z1l@C%GpjK7+zf%EFI3Au@kdhVJjM}cVA63YfPrA$8K6|=Z%H0CFln$YDV@%&XbZ5a
zu=nQT5VFxGR-ySwgu!7Q8&VS%p)TJiQy%fHSkfU~)ixHIMVy>UXLn6(I<ZPVPf2Et
z!Vxyrt!tUnm|2nJ=#ddYrdwPG(L-4Wi4K^k`E1)Ls#0MV;y5slhMPc{YF(zH6B#h^
za;<rU#=#<ePF-m~{roMASNzPmMdZnT%Q$ng_Vk&k>uh~i)Uo-fF^@!hAgekYIad10
z%g7ZGBFCQ2fZ6{`IutW1qC>9P7)>|qe8l~rkgil~Cf+WT_`wl*2nOeWA`HmpF<4ps
zmA&Y|)^S_VbRQP=pOJXh%|EEH90ZIre^}fAY?k6=<j(cRmQC5uJJUWs+~Ol&wjjK1
zr0>-oOIq&tv-su0FgcjA7mmU`sRLV==&MFUB#8qHI)z}e;+-i1%V@nfW{HM~{UCRq
z%<Cs5>-?UBBzST1>|ZoWPi%FR?hyzu$$vFUvth^o&M0Y8Wse%I4fumu5*BYEj0#pf
z?6wd_$u7p$=4#v52^JKR{m#OlNfGE0T%P~kWy|j@{`|3cyH0Gt%#5(=*+)3^A9!Eg
zO4D|p;SoI{oU&z^7&t~0C$p6j<YmE2@Q9%1e%pNb6^`CRmiX<5!~+KoP@#7oX|%#!
z!6Wy6Dpk*#XPHxR9jq`ut(KPY@~n}st?b32z)5E<rV;!fq{{s^X$;W;FE4a+!8N*A
zgs<~5);_%$t6r@y%OH&uS}_H^!<{|y+;vE&Om~AquW4Ifg!N6g`W7k10#sYY=0&)e
z3PH9Ew%N@+Gf`^~i8Z9e5<(uX4oHB7lIyCPsUV;P{;UUouE0;0zf*_#pHnLX2%O|Q
z@x1w`Un<OHFJzh>2MwpPC)KJ)our0jtiIgJyITiDpoZSbxWi*&i;{&hfv$h>N-s%g
zNv2~Yr29TDY`sV5N=pqm-Cv+Uz?xdvg$+tteD8eC`^lkviYg4--$FaqexV&gmcl?S
z>@x$M#(t!!YResJtN6wOVL-c6wiKeLy#7K<OD%3~exrO3d4{zW(4a(U*UI^ZrCnek
zyezU=0S!U9a&yGEAqg!up}bU)J$6Fu7v6J7TFP7~p)=x9zzh$AB^Xk>TAvY5mT#m?
zxEdF6MkfKBG!Qo&FIG9RySa3HSUfllj^Q`5dODSPGS&ND!N)MLv{GOkr5LEavs$4b
zBqIE3AmN@aXGto~mCgn*<xn{4beu4M<~iyW#T+%>alQlr?MwDNAFg}B;b6D-csYuo
z26&=-y}-YYCmOKrO@4<2#k#%86#AlDuHrGUaruJ9AY*eoeD>OUA$`CVxR$j4faO>|
z#Xpg}2-n?b-%aB_fV`=+CzU2l%<qR&v(E3hb^6f4XY}X%9M(TJ-^B2XM8Zj_>?O>g
zdqp-qM8d5kQD3Ruw^SZ<F;e4Vi0=Qh^5704QEny=;#Ur^1!S=X2|^(=(;)Sjf6rFg
zQ+Y%^LHQ`CC!JI1>Aj+vEvYAy3-liLmjiu~egfS;wWXf^@AVV$|1I=1gM~(vR?wA;
zdaB;#zef%mPXyCV-C!ZJu7_-LvZ!e`vgihAfqeaO8jmE=xk#cROC*a@)F;23XYOL<
z+|MjN$QAMrq23;q(u}M3AAcqDu4YbgN+K#`D2H_hqdGWA0+5+rT`Y&IgArk}sfe05
zaS7<^V4va-HpTph?TbIimKBzG_r9nFFVa_%68!fct+)HEDeCQ<ozz?W$tU~Ho=m39
z{CpFh=pqscr~DdoBais5Ibpk1fM~{_q<M&aFwcFxr3;;1CMgYz9Gr$Sy_}6vws)Ap
zEI#BrnSWk<=YP!n4*tT(Yai0b4{fr0Ke;Z7OA1}^(BM7BL%~#U!GZiCfsLoc7{Y#v
zN5kKCJ`zay-S+@~wG3?+1WMsz`P?YQRQNmpV^JH&c@wSp@aq`O;oCF2`Nb}J#uGf_
zrBFNf(xvt78=6nkK%GJZk_>9#wTE2;OZbh}xy*yAz6)Ia3XND)zs}jK{`Y<TnnG58
z>r#9yxI{)L%`BQm(fx>tF;ucVk$KPgFg0dAaj`TAGfmV4Eo9t@QKS+pf&K%*?s*a+
z&3G;(Yx?RiN@cL?pX5Cg@3<NSBsfkzfZ;?gRSm3>DpDD5pTP$l5~uux#DjOEKZ<_9
zsCLT*p^}g0Li0rTSMeI<5Ld?<Kqvegt7n1_G#^MJPGhGl4uTfc+kj4sfv`7Wt6H8=
zc9PQ-Wi2l#eo_B{VTP8Kv(Qo6d>kit%FfFbLGy;f4=~O9D&&<!CD84A8e=W*vjg5c
zIydl}xuG(o9()doiE_n(%B%;N$B8!iF>NcLAp?nyG)_nBPIZK=CueFv@DT)ZEQ5V2
zj%BrY1dUnPT%o@8@jxg0l{F(<Pfzz#lpz7%cL$w}Aoyd;HN=ehCL*yU{)r2uD%W;m
zTWEONf|yPYOuN@myyB!OBZn-(eNMXBnW^sGlfz?u67g$0AwcSlHN@}lY$%_btXtl^
z2XUhkvk{GDm$5KCy1F47tL$DzQR8b;9}}vnQ~HC&rc$6UqBoD|Qy;&UTOzE!`uP2u
z66G5SxkY_PR_Z><vdh>bxVSnyBHsNB#XY{WM-J<K^!SGI#SP`1ry=0uw{!x^{8`4%
zO77z=xY1-DOK0XPYPsEmV{v0ec=bBzl?r;@Bcf~BPbXP!>0~rGRNrnfjKc`(c6q69
zKiKKNIe~zv5+_eQa4!C^5JnYj_ToST9x4X#D3B_Kod44wKHW5nV^|%xQl{;)VaT-K
z+REPCMlkbZP~Zzoe6a)zuD?ObdQdI-*~9=@cAfK|hKUeo*LeH?xB{BuJ{rFYtcDr+
z+>}#+x76grRb=B($URnw*ayF=CZQqoT9@7X1DG^!HtC+5BG7Eb&oQjA9%nbi&icHC
zvl4-y4~q1v4(UBzu^afetVsS^nDAHPGwvh5-#cM%zLQ*RXgkBS-!h<)7!f;Ss9c^Y
zx=@#_h|Q?Xci?0GjEljg`o~^=5G=4GI*boHAB3+Ee5&FTG#^%8%PyEU{8xtDtImSt
zgYVv9LPU{LB9VC(rA>&a8cED5GUbmWJ9HU*jhDjs{)q9-b77I4b86S11C)j-+-C)b
zJ<bOP@if1>Sa1>h&P0nJM=^%Mov#$`+7+>z@*1_G^81c*<!cJ%^9u53^T*=|aT#w{
zqGVROrY{$MusZ3RKRC=6T=IMCX)}0;dU`?F(>;7o_ajq&A9RBt?XowJy=YzAsRGYD
z5@!GKcf|ylzqrUhBIx5Ph?Q0#3q?@{_ftWEAoD+<0z*xL2T+s$4!>^w5Q>e*-Bd+*
zWd5o7>LU4RrZQbIS_T*1Zqs#7FnmiVqR!3MQJRE>a4qv^1?SRqVH|%}n5WOYsV;9}
zo^D5pNMF}{ov6uk$*{7lU1ms>jd1q;-sr&YLz4rjyPWz5@R4Np++i-a7P;-ZZ^`EO
z5w3s+UIpCruA_j<tYTJRgIWx4=DM7hM+bw@;1(?cvO8B@ycfpZ$%FiIPufgZeJvRz
za4O!pkIE+!Wa`-J)aCOAQ#hHK*f*6KHHc*;qoiYpsZ1q_Rmn^~of%(kvptnDP08JB
zN1?LdYFY#O8GnGbXiCa9%UDOW(F)Zq-`w4U-VaL#CGCGov~~i~X8W59TVLuiOa*=s
z%wb#$<CW9AJI;vP=|e*2>qh^6neO?I=3f=FJDPuW3oq)<-%U<OS1L!~NqS%biX%Bf
zxt(X<Z{-Hqyi+-iRmW(`YCyWLQNR2G-sazrt^J<&uboP2@ab(f`!@WgX!do2Oye`*
zbh?8~1q#W(<cQz(FPP(M_)BpOH}$OH95UtmQ*Ks}b@|(i^V{r`dK|WRIGGA0mcN0L
z*2nXC5G47Ts>}3O+b>fPWD}JZOw(pybu#-sA5YVcc4!uk4gU5&HVb#-nh&;NH@ga>
zUKY2$!Rp`8d~;1-q~o~PzzmjMQCQ-EyBS`gMn&Pk;bcslnQy0v;4vwO6^#ct-`hMe
z5@Ng@Gt8_Fe)U_mkDqnC<Ir%DalkkvRQ$xqG}&ib6z>>GrX(92g-s&2zK-9$kmx`~
z8%EkG>b!+T1vfL~5v@SS?qZOGH*|FBDirBQ;85Oq9|ngb+JkaHE_7`<U8BF}t#Uog
zthHNvX9X*MU7YLBF4xgMm+1<5T~xFsI;~=iq3@7RTOFPDM_`fL%}e<c>{03R`C}^!
zTf6hyk;l!0WOhQtnUF*|#=$t#y*xx5hufh%$2s^;<2X0Bd>+S{{HM)1&iE~Hobhm+
z*V38ux5jbyq%{uspZJBijq8v2%NOA{S-VNBt6pk(S^fmb-IWiHyR~>c=D6{=|5Z0?
z9Cpc%SKcW8XRv!o{v|H|v?%|E{NR%O#e74I6uI5w-o{^$T2%iWhz`2>%RNOLWb^q?
zrhKgwo2y**j^gaM&_!c7OcPUVzOL+w4$u9NW7uz!kZsC55k|^&3sf0i#>>`;XD$;|
zHv+Nc!P&V~hMkw&P26S{ww!_0Ii=Hmdn0tunH6vU8I5Q%+La>xm;*LHI_8ti<ZT{k
z<n=e+lmnsnVGu9{$TYv@;>}pbtYFT~*8icDC`_jJL9=><h~11+e47GA_`8JHLc!j<
z+AMtgl_HFE&yjw2Qc1oYTs}p2vUXnWzRuIffhGAqbK`wom~XAWoTn`4U6OAn>g%31
zOZYa8Ci4$N_T_)qwm(Mun0FSy-xGqw4{8Gwn?0uZl{@vtdN{p`)D$;k{TQv^7Anc?
zEfX0|_TO&>m4m)}4x1V<#2kxVu1IJe*0%ApcsqwSs7v+=)r!S(s5KMs*fvgemoHG`
z1s?0-X>8yCO(OGTLk3@uyDT0d_jR(Fm}vVf7H?nUp2{CQ>RZQ)ojuR6>?WaZn+p5(
zFW(M!in#}CN<|`>9F!os{R4s!=Lhll*+(9qEVI3pc*8ubO}5v<x6ypdw#_1A5-)~I
zqy&K!T@jI+GfItzZ&N0i%`a<Lc^P28<lb8LK5>sK_8V9fpLQg%1>^$gjjG{U;Urso
zQ)wr7@0WH8K_@l~+`ow9`*LFZU1aT81HVsV?@{3?*i?_h{P>GbUZgv~hcTWP^c)u&
zV4?02xulA3{m#BiU2~-==k+cv+Rb@%+f3eRT`uqii>i=OVBnZ64XwAeMP^v0D%^<*
zT@mMT+J|>8Bk5QZ!Wf^i9lix9gB$E1W54(A)LZQfo=+Bh85BPW(0vf8x3%-)AvCMW
ztG3=K^v`Tf3(3!Mq1xrG%3muKIDJL=*f!ukmCf^+LJ3{}#cXE6#+9It1cw$Do*W|J
zl^#on)sn_xg-R>-0erXC<c%Es>x!wruAVAaaeT(M%o(~MwE!63rW+BzcJRrpZ4Mb0
z>FrETT-L7A`R7Q7bi1`|d&mXMwhR`~W1mk-@Qr=&F(pi3HY*aK{90-rc^!kGD={iM
z{u>A)+moY@zS!&zd1}&rkOT-elf0m@1uQn2Q|EHe!D!~vnG8s6K&!GrwE#oa$QND^
znOV&TQgcy#A6b2&iT{h#XRGyx0NMpT6h~JRE5L6Ead^*zdNSt^O7FW~L!8uTRr!8!
z#f|MJsm*Or?#J<OQ70DB32yCioxt8zHL`!96X#HAB(JT$k0z-;DJ}oHzIiQ6*z1hX
z*dAt-nqb%Qs{SRMz2LXsb`Ii&m|+z{-fn8SgNj4=d&#z>%M6!0!auzWP&c-`#kKMe
zevfFLA>aG(9a83Q+;i^7z4o7%i~YR3Q}IOFA7uAdirjNDmE?`NmFyU6w&6^8gClK6
zlBC=f68r5nQ{AU=B4UXB{9rmdgc#dOesND&4_l@qeBocqRpkn?FG)&RlP7mlTx;<w
z&tx+h&|Y|UONRX+oxz^Ios0P0VgsL3J8*qqaBa131|2SeJ-Zjba=L@-h^@gj9eu?5
z&+mT#*T(-8uHU<%{2i{nTN=k;9Wq>S9rK^T^~rw<*HaFz9kvG77!TLo{QkT6B7b%<
z1PGnXhJ>x(q@in+P_ZtUD+|{C$|2xr8j2P-r`{y-YwNFHvrXlKI!p|XBpF3<V*aXP
z|KrU19&;+QzGO;qAg*1~9dN#cU+`9DRPbF7$iw{l86pRsEB^+HVF6`KZ)(`MjJM5p
z%Hof-$8M!J=n>3;-F;0Lx|&4OR?|`PG?~yAb2a_(3)J*99Ts)$?`jeiT1~ITtfop=
z)7f93rs=+>mHg5>(T&w~5mf+FUsuzYzCcYQeNET$TQt&Z@wIh>yg8lErnRyl<_nJF
z7u{+xo6W{AwF)>tI-H1ZHaY(w9<?xUC+}0aiZqp{bN#+*KNUZMs7l5_x>+sR_UF6z
zs?j0oT*WsmXOeB)T*cS?Gg@CoB^gx+lT}5zJ=?&Yb)!S~gZ$<>w&URAE}#JTljNTW
z1?;i4DO2N1U#j2S$OiHS=ey_2^?WwZ$?T2NskNsxG2hoPP?pWULiIA4U%{u@CyXj9
zThv~2A{1M8j@M~HtZBbed}Z+eDfKyBc8k>Pde&37ZT~OVyI1%H|EX|4A6WN)68IL7
zo=;n^r_y#=aNE_kE*c0?M0VY;u*xV|=p9d{{3q|1j@O@nkMU}K!sg5+RInLQ=wM;4
zB1cAsG7@JRlu@J~OGy%Yh&Fvq@%3~{@wHeE=u<tPc69|2Y)TnMN*_}6JeX(2@X@n~
z<&($Vv*=_;1H;itAO9>mS>^MHPF}KSqmyr-tZQ2SoL`0}l*9#T8OhHOL@#3jE58p)
zgQNT0$UjkMkfxUWpV>wZ_cd}OZzMg!$>w<I(RW@Y;$!~_G*=olHr~*|K7!{VY<2&3
zug)*{O;FE@$Ec_IKdEO{2|QP;o`a|-pTg6>fS;c#eSp&6#-pQ<K0)c9T>l@ypDMur
zV7g=mUSq4u{7)GmurhKH#GD^${Hb<ren)xCp#SHOV>W3a<d-$1u?nhvHUE8PMzH-g
z#r6W$=b5M6rbD>bz=oL*#g0!_H5<|cvv<#83KEDMHo$uPIet<(xi{y`zh1$<IK&JG
z3vx_AQZ<CiQ5#%JLUzt_O312GDf>!QqVAJRe{9}wx5H!HYAty)27@^MIkxy9jNMVN
z>AE&;VNK~qQ?BPBog2_W#T;s#)~n?ORB)D{QfXOcw2h^ON%O?L2PHC}4B2%2grQaW
z?Wn_#M>6}1R|I`#Q=^M6&`|f>rDqusyGB4f`YV9g!OaH;$CPh+7}iBF3=Cl?Z&3*C
zo-iyj7#cFq84T6Q;kOB0m!2ZKp0bzO+MdDN`2<?y*PPSo2H_<RnLFu#{4D@ow0=_y
zohfrra9QHhrOu=>%LoGf8UbJ8S1#urgVeZW-G#&AGk;EV+Gbv2`c0Be7W};b@rzG4
z&8DB6W+2ZD-QR<+Vd}_C?9aBNKdi(o!n%iH7QXTWu$svZog99z+fMu^m26i1?=oTa
zK3k3poV6rP(WV-~Y&EQ~EoIk4dwDu{RWF@RuT4)F+DpC394=dJw0<4Z$=TO|It5>C
zyYA%K)O@T|p}lEg_;H+q+Rb++H6fpg{rBuF0Ot3n5*x?DfO`sH+~c`^^eX=y{+Xqk
zgumfgbq{-9ZYggb^5Pm_G(_;l?pkb|@PZ-1p*MTbEJJp1^Oc;{#c<!Tj@+hb<lT*v
zlshW5l|rZf+!s16Ds&1@-u`yy&rI$;_sb%=_Y*)gxhGXdJGZBhX$vT39D{RR`D;G6
z{4Tz{A+LxOz+V0v2;l-2Exe}$wxOfGtT-OoNtySFMmCJ$NOGlpk_#Y8EPb_nz^!z;
zcnO@P2F`*P<?p3}z&i$+Wq6RC^5%7DaF%enk$XPRVs$WNMcc-)@%B^ca4Oe;u;%D1
z9XGKWROiHWI;IL4e3<Az)CZe54KI5a`^x&$0h?v<Y2Q>O-mUO&(<tgKU$j~kJgx+u
zY86mmP-9VSk1#G7H`jDRT`myXuYC4uoxF)cmz89>*s{$3f2eyG_^OJl|NBHkjTSx8
zqK#GCV;gE{6`=)96g3ADJVz6ZfF&qZLJ~-7NMbG!S_N!Qg!9A$snw^nwXL>&TAtpZ
zPj6T+MJ@&}RlHPD+d`|=eHtlft0IDVzrUHi_u09C_WAq%Kku6lhuM4fT-K~vv(}n5
zYi4wA;qVD&8yHX~RI1JivHPxJWie=foEW`Xf(5Ede%vR6rL-Az@nJ+Z{Tk~oF6K)5
zY@W2Wn)9wOa^&}KH<ph^u+HlQ`KJ{a6Nq^5T*Si@+zs(YS!zx%+pXX7^jkQhcYr<{
zjPG4f=NPNlLw!e^@q`@F`NB7H@(btY!<#rXdv>;WDm~6QSk~B`y2Ci#WW7<w@xnjR
zZtvU}Kopo!B4wqPo{_&h@+)*N5Qd`nT*d#gtDEB<{<ey4(3v@6UOV&x_<3@c8{s_Y
zz82Wm-@324_VvfSI*V4AL9=J89VfoR`16tI<g=*=5-CZ(;2hU=(A?+UHBLlmd|gGQ
z0bke6!s?A%MAU(C5lfv)(JE%A^NoZkW~YcIH+>I-r7ZQ+N{SOHvYBp=vqXc-_NmvO
zBg(%<0-Z3gp8K0pi^Hk!if!L{M1V6pc0wV~eg);n=8##^6r36w2=P8(^F2%PoV8B(
ztH2H!Z-2EBXz9ovv!CkK0sE=kQ$DsVdm)yB#L7|)*5_MxT(kW%mP!dfymrZ^Edwr%
zrJv5sFG;?n?tB3LPXidkOeJt8_iCimA~K{EGp)1xFzgmCYcAG4Uuv8qg3P0ZkxYf<
z<A$qERb53@a$28bXN-*H653vAOv8rY-_h`<;tt$sY^x2fTs@$Bl(qrnj>L@($L#QI
z_VSeACsxFXJ95w0X<J8N1r-$^f&Vu=fQ=?xgRB!Xd1L>Nrc05`d=L-p!39Q(SW6gG
z)H9~*!-+TO2g+eHDJBTnyN<o`HrWPe-Ot6#u{BhieLpv@mZiR*znrAuL?8b%$r}B@
zkZB8hL73m45A$<wf(A*{ze)5Sy=ygOUY7jpeV3CLN_WZMiG0~CLsw?Rw-EaG8_iJc
zQbjbaQr<~CmX3e1w@Wi|jn47AL6R`ko2?NWO3y$6b#iU3Now^+*e|GBqR9|OuX8Wb
z5WmzrNuFtSrln~u-hzY?6K|!^3sI|S#WtAkpc$HNY6VQg`6eZjXq4O3Wy3dhB8`Hg
z1~qjLBT-!EMw;Mqof&FP7Zp2`ZNYcnA85@7-PYW9CCR{~vl|rBp2x``BCHe<CT05U
zymvc@%4)kDl;U_WhbX54#y);`^d7yWPbZ67dg)hrISzbi`{w{;%M@dg?W6e>CKN_x
zwzT&k{i7XzY~9=Av`~8IzB$q47W@ad(yZGyp1w8xTjNx22tR!x%!s=$Us7L<LYi9t
zhU8vf-Df$D3OSAlpI@Z$gpOmoe<L+0o;+Bz_xh>M*|Zj1uiY3x$zL!+8{OjyS|yY`
z2dXprnpW8;Gsr8ex5v6yND=Iw)YH}9{@<nDDarhJ_q|f;ag{9{^N{f?`8a|e4RDpN
zG1B$Fg12+himc8j@GZpQ_km@vMvK^0kj3|Ccb6y;zXKG##C*84=eT*Lp1CpfRv2Ws
zOAeV)IdpdYj3@9n0sM^<Tb_J_@b~%Trts>|Da-yj8_Fm#oGPM7&Wk*{bN}pcS6uls
z=cPuwOADjjpRc2Ua?S#a_iiDvjt7gxyGvQGVe;_+$v4ojSjuilw)VogkbmJA?cM!?
z<VE$Njsly>CcyB;2b_|n5*oa*+c}^*i18Mq#h5e^(y$pq2SI?f4*|S6pJ)pY24PV!
z#(gL5DK5A=ajjhLh=<U>gae~*ak?aXsH$Pp(bQB(&7QwA{=s;vZA*^M{Gn*BoLIT^
z+<5nGQU+jbn>ZB5=YNz~@_W_Bq5HG#&2^RBhNL@?INGay7kWTe(ZkC&&=hmn=r4hd
z+CgH<Ac;XRH{RW~mZHP!9z`y(Qo-E=sqF1m|K<E7O?Rk)avoi_7kJyu7B3Ydcl3RH
zEVr=5tKUjZ>n~L~Ku$riRN3|%Ot;m#6e*!%INf$sXIa)m@zey4X5D1=hnc;9n*{#{
zVp;E?!b}$YlY##s!4KhWUbao%>L??7oJ$#Y2H!VB<|WC!osKT1$`2<d^Ik@CB3iaO
zEwS2gbu?$r4F*XQtBDxD+bn4<rw7LOU&0=58J!h%;zT7c<-AOtC_|s4fa>)HW0o!*
zPdBr^WY(jmgmce4z$LD^a}IC_QNiz-IX=qO^qijZ<y{)g&l9;vH;6wMUL}_xU0c5!
z;=^0cI#I{Ff2EErO<j{k6|>SMsqDadX0&_cjcRjh)R+>E|KuFg2aRML`6xY)W4883
z<(?od5UDp?D|dedacoJN{V>13fjw43<)wZ6(fp2Bwr)9RrG>&>TY!!Ixmjgdw684d
znX)5M2TO|P<fB4t4X+-}C@P^#D>Qp9W}C%a3DuG<=GaK|R5PMXQK^|na;1PF7^8t6
zpS?ZY^-&rscYkz}m!1D+fKkdg4fc5IUa2n8WS7j<I53^csFw(sDVBd3FA_L+2l!j_
z2VMSlDSadUw*Pu8JEKW%@b8>r^V}ND)93L-mf%gE2bAYkSn>FcH}3ByGMspUH|Nj9
zQ{k*&<{Ym=PsSvb`IjV@VpEt-Qgw#Ef^e-vYi|N8rDi_@YtvVwx9G}?*{I~RH_`#y
zUkhI@gTlt}f80U)d3u{K+rld}{^V$X^|QJNxa>gArM=k)h(o)(Ivrm9DE!o*Srj7@
z5p_V@{(LmrdG8-djf4DH;$RAEc{4O7VtNJl<^7kW%0820dDj?moLqhy&X!ULE$LWt
z5!v`8Gjh9SB!+oxYu{{d+qUzLXn5Ac>B*4E_$ShDU>|dC`I5oq2btXY+Q*AtM|=*b
z)Oi<>C|yVL4BSuAL|T_GDp&<onu#6?7^0fZB5F=~@&ygb(HrTxXz~?&Zl$hGypvZ=
z;k-dRV#Ce0={w{2)Tgf5XGZe1q|(rRWc(XZb3#XH@->cgo|ZbZDB8IN#cslQk@iBf
z;{H*vkr}})EgH9dX7EOfUI`~O`!u+Xr?NkW6P_8bv8-)&Jv^7AHG!*m-+MtV{d;_D
zUo2}!(cjxQ$Fut3bm=;<N#1{pp1!!iqou{C(T?Mf6Dh@eOMz5+!#0tT*=0d1W<ApP
z$I|4}<;g$dY$cS3*FjaWHTK>jlpJ(8Pztv(k}7>3ib^>{o5()V3xeimcLS(Bbw0+y
z0q{<-%;Rs(U(S;@uCFI7;aJI)g!;m6OprE4yYvZa@+;<t$LN4lJ|Ta>0~^u!zf3!&
zNu0<34b1#+k^Tpd+(0q0Znr}Ae5yhT8dD8>t5i+AJ1HpD;T8_{5)~@KR$pN1EwJ^X
z>Qis7sW;cuODgKkrCxI*xEia*IjW1M?6QT^NI|1!eBT^lpr@PjH@b25{772ou1wHC
zd%6QHvS*Z<W;XNx?fJ{eWLjG1tVONUZdOwpE!w3|rYUZ11%7P>er*L_Z3TX9xn6B7
zItVK?NI38uRZRQp(#r?%FV9|NH%3_nw0p&Na=Rv>31WJ#TK5>k(^Km<^Q=u&h(@eN
z>!d+qU`i(JDG%~0jFd1-%P*xt(k$Ysd-5v@Nk6G{46Y6DXm3@4^W#P4=Gf$BT*>}L
zwffVPu0T61$90?_I*p#6$S))>=YLXbwCGRaRg(6}9rrsQORz6%8830ol|3ELLZ$Ly
zIz~rxI=`rSDn&{sC@?B$;$ak5@UCi6gu~j}bt*-%CFLMrN>lCqB}M-X-<%*0JIvAv
zQLioNWzvukgij;%%Ca<7-ERD_296@`4P~A=fda2ZZ&g!-1}gy*`C4a11WLZZ{JQ#7
z=lC}OLI9y5rwFKV8$nz(5;9dZ+X5L6hFX#@!*s&yIK?k88zk*3HtSvj75znuc1~Y%
zus;fxbAX%{h5E+9x18tO4cnfHAoAJv4~}zXt|vclA`zOjJDEVs3d|?09^`zLBz^(+
zQUG$b{}*mKbE!`3|3X_%->-fBj{{!Swiqy#bdThEs|Ukf+SlSt36}R;%Io`CYKm@J
zT)H&*+?!97rb<r4miR=xdRr{(@r*n!B|&+q*aju0nvsZ{B%`iMZl<?h<sRv6+~9!*
zdP!>3(dAhWmyLNep7k)@_sk`!>v!h#Fi5aB9H0Ffo6_-r+TZ>Uu|t$fE_&vYET{e3
zqJ;B&N~qrC5<x6$lj$VtvczEVGK3zu-1VI?JHjhpl-RRp9HD1%hu<DhQJNZgM08AV
zY3j->F@O^vMM+M+jWFE(=JVy%&qq7=WU<Iwko;#n>&dLm+_{+*&w3)fZYNNWzJ;{m
zb@(3VPF&Yz8OX7X#7X#Vk__9gp*n_RM9jR?T+hrAslrWo9S1S<FgZ0cXL70}hfBpD
zl>7NeG<Lk{faGL~%~c`n^D`&TzZ(AX7>G!G_D_nxzqE(}y?<$k_8O|JIDMePRefKq
zdaq=qLruN0`(HXTwubBSD4TO|o-G^0-3H=Sc~*Xo+@?ZbKpnw9s=(X=`uo>Pxk{aw
z?I<U@(!260`e+ow?Rc&e-&<1jTKn(ll|`m5GsXcz6OlOMM9MWK&f1fjbmaJr=+n!s
zK7Df5Tb#r<Se-^o&i#mAuMdf-+X9gR;CSjkX4y}psL@fV<4AfY-r1KG@BBM=i|+|{
zt$~iy2aKkQ#z!YG2>^Za<l82kvF8)D&nk+jfJRZ=fue{4AVu*g1}#q<W;%P@;xkl$
z+DmU-{j6&eR^D`{bU}Z8hH_^;)c1r~nX4Dx1OR2#yEERYalFV@+y?K7-~YEGHS?}(
zf(1Z-$<5B^d6cK=)}@o_Kg)UUF(`z)t<$liU{sj(=5O7jZem{NnVwNVyz`&9N-qxo
z$0mx*#eozeevnHuK<|hhH)2o{2a?4#`Pwj-5xRFV$zPi(#2#mUhnP2KY_i%EUUddN
zT3-ET972-&_J23>F}WY0xvx0h3jq34Cv%`QC)B>_tRRK)pIurPtG1h`p%M-j^=4qU
ziHvd%m)*)d`z0vFj$h`dH!}O>;Qhbb`5w9=WpB!fXYHk9%vG>M=BHbiZR7ktV_e7d
zt2xjJq>mKbBAV^n(R&sLyqY@Ai`(8syf{&I!}uKaqhZ5QfBG|*qh6*a!ckwL!{E&6
zD%qJ*_ViGsXMQYovk@a@gho>EO+sNj%a7R{+xD`RzTdh|a8ZDfis8gRSYc0}NLh%j
zLPXF$-s8w1$)0j|M8_#oBCMQ2ron}6f|JgWYnt!cVt?HVMM=)4e?tMzhiQo<Pl(j&
z_w+9`VW<LhzkRi|FMSFMu(uNQWCKc(w-U5DJx}b?%0ldXlRS9{Rl)xrm+&(ZSgL%s
zTtCGdtm~|Xk4}%7^Z2sA$cJnc6v0P;bHeBz5G0SHG(O1JL?7Nj_}CAymB}uf_LzxA
zf;sNhQjet*Ey0+pi?L-hEuTHoEWwxyKObNAS9SR)7A4%`zvdV3s~l}tgI%MDAON2<
z9&BSze0doIZH3lTU2{#Bj2fU_0~7cz_&^dNSCQpm!&3yPX43X_%9JweiF6@oHT~KC
zO-8S&g#BcV`Wy|9<sxSa4X^kg%8aLOH(fip%ZKf17Pr)?VlyZ=A?JQtWmG;IihAc<
zPOmdwOVOh>+4ocr9zz|+m7af(tJ3{cd9PS#w2()C9MD3hLK4QZc`_+P>md?}*4u&H
z=RY1YF5fg55#pkTFizbb{*NEgM$mF(dNJM^1HgU`LPJ-X5v+Q)SM@x%>Mjzg>iy)_
z1T|k6lG4J#DxLeWcvezAdkw-}<3YAQMl>?SLka1sV~@PSgxP2bx8@g^NC;Ff+ieQ@
zPM9;Svp2z<@}sEIQV`Q=>qW9zQ>z_-qxp#R^AaXwj}Rgdj(2AT;W02W12MWVvTKap
zouQ{&&Af@ZiCxD46|9Ewe}g6n%)RP#)?|!F?0+heyGoo~hKFWard9!}T4|DMWr(?T
z0W4;7GD}$63Z*e*CMsUl%wm?46J(gGnQT!tGn6|1x3w}0t4JRWZ;-Haz5+d&b!`u0
zLZ^$YV}JRj06K+hK)2|05gtINnbP&xaVr0sH+_RUI|;o7a<j9g2l$3rBS%iTv!OgY
z)6wLHI|Y+FQK<@?4PS6q#`lb{8oZXqUxFaC<Ty8`6ER01fj9<H?N=n&cBdt;K+}Av
zqy_qbW19I8bDq?P>Vp}v9V_Y$-=tL%k3;@^fzKhe74W;fvRbY6bx!QiAM5MhS^B-3
zAj^I%z&d5I&|Kih#bNuMg(^d)TiJ)kX{MBGR?bHnA!^b;!<{u8$jB_vxyYm@AwAYv
z2PhJ%(n`3Ug2`4Tzd7Mm^<V*ZlSjd13MY<-J3u4_WIFcX5~FUScWpPrD25u?{Z&a3
z`1dB{H;J%XKV*i2Htc#ESvY3R6R5(w%(4Z}I&h@Rn|Tf=Hqqj{fhlzFRs84QF!gD;
zkd_PG<>*s+^;fmS=EdDJc?2)c@9PXRnEFG@dVKM8GZPSRp*qSI&f570`xDQHyLK}|
zbt2P%Rchu(;jYuc!qA24p0d=WgMh~<?sxXqQW*|V@RbWu;*5UbZ@X#3FH75@X`A|{
zn5`*Np*+0_JdIe>K^4gfSZr%_j2yerS!Gt@W}KroQi+bX5N=qv8RpzhcYE=&7x6h@
zbv9+@&)wrJH!@yO<;m^pX}ieGB?p|20)^zT>rFktOu}|U<+Wn;W^tQESt>M#6)w&9
zp+LVMxZdpTX<>g7)tUuoJSs00T@9|-EL>8lt=;YTml$zo(b-gIR-S)MPh?h}wMFKu
zqAGeGY;;>}0Ef5UY<Axur#VVZr@=vxLzkPMq)|)H_1Bu&$FK9MhO94lXtjE{_2oR|
zf^508iUDHioyTL}S|s*qS-n|XrjPTq>0>!*!--Ep!^op8kl<Z+5M$KH`}Cxx%#F@-
zuo7o9^hcs^e_wRUaUYPU&RQC@!2G-Fp{uStog}e#)Z~S`R9nfB9Dd{Oxf#E^*Z6yD
zAdP3vx%-mdZ0V1o^pCIx&hK%4ETT<~$fst1bgZI!Noo;BuQQtzl9ex#0em6LECR%#
z?@xaaKBRsSUF#cD!aoT4SvU)Jx{Sb>QWC0FxD!8+Y18Kged#^2VVUs)C<S*O0#oZ@
z56;^)2Hv*sSxj9uJf?0#zhnRnSfAeMT!z&5FqQZ#&{+Or)~LT}=XK6EZTX`p-^WM#
zSc?yTnR+UTgc7Y&Tevf;)nW&ocbnDfa00B-kvrWLYqR(?{`q_75bN9m^A)-0r6Z<l
zi=RRKLgVw0{GzT+g%8Is%rHv-i*nsn@Xn1?DO&Is!JqdR!S7^xU|cu^48Vdn4(atS
z+ofNf1%S#@cyg;LH+<V>rar8OZ%)tGEX9t-^mnujer!EU@^6@PvOM`bU_Cq!IyCFy
znpea+j=_%&OC^ipQRm~3F}?pydf!L<mXR*vuQqt>b$(C3TE05uo*Idu?u$7mTZ`a7
z*IczY%0icp1`8*ClB0S#l$B6R-vDRvJ!t*A=t(ZPW06Bv52;74p|p{yyJsksyPUiJ
z9SL`B<jt;s|C%Q-D85~yDdKmH7Gc)EuT~b;zo$`RI2C2Y`0L-7D?ied1zTXHx6R>B
zb+9_hFygzL<F6|sJeK*U7-nKhIxa3<t$SwAfufoDC2=^fk@(GRqXdJ7?xS->@Zqiw
z41clIFZ1O(5KnQmajd$}i88LS+3W_WP{4)Houd`~0X<=#bLuse@VpEN=f}W_3efGl
zBcZFc2Q*JHL;U`~9BJokp1jxi_ACdRm;D_$%b1O`DVx>D@T&2YQl?U*Se|wl?w0V+
z-K^eX_WCAo(cjWdVigmA)glc{Yq`HSQKUbyUg@zyv6hQplb&8M!vtF^>&+L#oL>)_
z1>DzjWR>AaG0(#FF=RLkxahT8^@SDOR$^3w1B4^BJZ~_>tV+!)O<ir5a8r|5!FB1`
zw>OQ!-t-=pZ&{&pmv33m-RCaGaeGf-{WcN`cVz=eoFlc$vS?Fz)?;NyGVrv1Tbw)?
zUj0c(2HMK);yTe*Ak(xgH()YZ^m!dAGYjo)GnNxz_3cN4?^B9ZbHa`A{MlxU;Td2v
zhT*6@b^Tc~x}TMUaV}SWZ+U~T*93^_uwyqcL=6&2blUSx!s*f_LZHT+j4(kYhS%+5
zjq|7g{@nHNqW7@=%@nHOT@d&)p#sHp+@-0K&w}6FoYbuTF>fJ;>^cPy&(_NA!!m2V
zrm4qT5D2xtMp`Yj%N{ygN1kowq4GglBRA4Lq12TABFL7=<Bx%C3&N>aAX`y#YQ{U|
zskXCnahc$2?uq*vUBo=bgD!fm;GFI1y(OuRtTE{%it!UNOOAPMj1$XxRMf^=)T<29
zk@3`|Cxv7#G@p|XBm8|l`DA$AKY;yB)cSE_8qmz?jH~y_404iE2F$h7X@(_cYp@Nk
z`W`V9J5E(_D)+6XAH1nPJWm!|ZHLb*NtI1x$jRR!+CAYGo!6QgIkqJ0QC&yMq2Rcu
zOxVlGtr%T$S%Azv88lhLt-9Tqa>3Vj*NU4lpq?bMUiOpo<r$P>otDyVCm4vt84D?3
z)}FHJP2qLh<Jv-B{gBrsWgJ6~g<51j=DCur{bgCtF@t&<r;z>Dp8OG+>44^F9|#|;
zVeQG&p@!p+F3Y+<CVMi2D@T@H*ifu<PfomZD~8>^EL5eDz?E0jMh~xEh1#w&pv)zj
znM);qgJ|+?mp|f#C43Fq2hmX>2rX~LjAp7yzINL(ecODMVZDhNGL220V`SU!z7ddZ
z^F5`aT>KYi{VF-q`6QQq?|jH+xjC5SQBNFt>p1r*%d42=0)7xO&^=8&vh=cG<~}d;
zm2Q1ED)TQ%4y}z$cbSTz3V*%Yw`(&T4(9-|E?fM2Y#$_n1H`8pBmoDA*KvS=AM1~?
za`pY;)hToagFkT$u~@ikI1w^BP%*O9H+1hNp&<_Pihvvu@GFP{c|e@N1POJeY|MU3
z$IwMCKjQ~sI0g-I{U9vg^&KI^dj~l}Y)aoqU1mIx?ccTQzir!Z?fMs~0NO9BzJ%a$
z@!qMjuOrRW_|KyxnIV{dh9~HnL5<(V!?O9?`i;H+w>V*3XUu*7c5l266YzRN-09dW
zzkt%QqJeHP9YE`I;`UZaKzdd<-IhL|<_9oeAmc1^REe7=GD@)@DfyxTM;}drR#xw+
zw>Gi0rhT~+kGj^VEzF6H6Nkp>pOodqUy~3|k2p>o>d8kC)zNz;(92|X`BU=AX{kwL
zqH^EJdR6?!h@e5@XEcB)^Jc|A7cKgu^=?;`6YE|r7$m-!G#<u}<K0PvjxvrMPSJDi
zo8noo$Ftr@Uyb;6zFcX>)Z70V7{k5Zn7W4AHHOb3QF^W{eDvdk>HnZ#ow&`oDVXsc
zFQYg3|22z@v#~~^4ok+MwXM37F`;gdnx*E+96S<HS7s--%&LN`kwy0}+na7A3l!+N
zoV>Ro{KZds1hbfRK`AD3Ibn~zOET%zB2OChNZ&4zw>&BC;G-_7k%f&e#*<=~iIGNT
zoG6yNA23BIqDEUH<Sar3hynhWaLag!L{tcJ!@0If_8qqZ6fw5~{6JKJNb2VN<vbaO
zei4_gpukkX|5SiIk_xa#QUUfz8LBwWUTLWyol9q`6|%cJ6eK4Ysrt4Rckm>5zn{O+
zeY)A0U=`;b?whf>{?Y(t8q;AKGnfA{dfLVmI^RbBmLN9u$l6Jng2O#=p2qm~(5&^g
z#saUNjDgjwhc;0UZK56-S8baL(>E}F)h24UNqxhxG-HeH0RWmalr5A6&OUT}%JXu%
z)yh*44^YpMW|(>l(qDDSO%x$6C!6+O!lZjWBYo;NOQvkKUU;;(yy?k1c^{1S@X>(3
z+&=uoLoF5^;(arAo)ZULQHZIq+g8X35stxuUk$eFjVcOsXHe@^NE>a1w0gNx=@^V%
z_Q6z`v0&$=KW2o3oiED2rmXsn%%QK=-i*g86WR(UBr{n;k<*F(I*uthe=)b*Ud&hu
z_*1uE;!K=rn90v>2{4nddy5pFbl)`tXL$BR&=DN9ICzhYl^1qay69*4P0LA!`THIY
z!qof`FKQyNejC4ObG|w69oBi8r$`CIF=FPfiQSr@-s}3xJOZ?A0yA?-%t*`@OOn6R
zxdv>;bIRaTW*UdBso5qpUOEwp32W=Fb6)<O?I9Ccy_rk+s1LK9ag6QGUcuX3?0L+y
zX<AJ*xv{JoFAgGc=hz&#lS%F#wsy`HTKQ$JARW3l`%&RF)(bRHTh>Fk4*#j-NOG#1
zPrR&}Nn6*8qHAVP<<%Qsn~GZj=V5DgyewsIWY#pe0AOXXy$CqVtG8U@?HdEGan7eI
z1iW$TX4)&KZi?1qEcpgU^|5py`J+h#Ja0Kr^jAo+t%;^e@{Ji~oYJ56;ymH2E&qC)
zpxG&`CCc2hen;e+EE?us@FyTe6E6U}9g<)V8_A}ew$PH)q@$8=f{u5XNW{k&Gk<X`
z)smImbXjWVR-iA@#Y!mq)J;$RwIp@r5m|eQEfgT{kaOpFnW#8#SF}KU@-59G$_^q%
zo*A=G<~h&F7bC!SfXZX^$nW(JNQ3?1FMnLqJ(LAvACM+9=b)68f`?L3ymq#5Vi9e(
zrE|NX=2Q%5Bysg?G-XyA{a!s9G2S6`P_bmz7#t+G%R3Q^`j~B8n<O3+i)9hL&8*ay
zCAYi#h=3=Tj+egwaUjr6;Mspr4KJc*{)OMOAZzw6=F?B{sq&9N`Vn*E$nh2C%Glp^
zJ>Mb0Q{ZSqY6#Ji-^I9kG8xre$oS2;{@9aKqYg14#Vh1JgCOyF!VN!;G0!MDlJ1Q&
zV~L%AJuW(3t;kU35M|s2f4y~rsA4Fwu2R7w<3~^iySudVs;v}>4@yfEY4FpSxL(89
zY!B$#2sJ$)O^v*W;Q(Nx9twAz%rG=}M$=&>sY|mmwj7|rsL%g+&Bv?nmpQ$_Y|&gp
zQg7MmymD)Mmzb1yy#&iRDBt~V%P?7OLC(=+aL007yCZa4ix4amsFk&nyA)-{@2DG@
z%hCBnWou74_lp@<j?FqB>xNvK!@*pJJFtvqDx(Ym%oST^afFN`7NK7$@5n6!TucO&
zf4`%)%bEKjZ(y%HJNmGZDeL)UWsJ;KM(;2*RVaDpHMc<8)~_I8Y={PgLY<3kz<>)r
zqvf>%=Y%pfn<p`|i0*_B5ko|Wgd~@OA$>s|+ptZ^<1By1{<4W~_1stXs^%CpP@_v#
z5-wqyZJ<m{83WJ@-YNn6O?5VFOe1^E$Y8z>7IH^EWk&{93rN4rTm?1z=(tqp%Gf@@
z#u1rYlS#EHtLVvP_n4aro;JwzHi+6*+7?O|D3ED7;{1cIwFdHt8l~Q}tAn&FoY3)p
z+6C}oOp``|aK0@u1`14SZf@W>Mw~88SvJ89gAzQiE0)ZnPo7idek0!#8WGPpml5N9
z1Q>h+@P~6*srU%Q<=Jax;$@$AiRWARUz+Z|Oh+)?or}rA&v}a{O{;``RP$R%M9<}=
ze@d2EDv>W+wA^YW_T*NRYr=%N4WK0Xnmr6JnQb<Ml}_`;UgAmer|bAn^bK#4k?Jq-
zO`T!4ooBg${gof|Fk9RTdZ>g^UCwE5as$);DW2%8+1nRKZ2IGKN&is1w?vBvV!Kyw
zO5ZA-u19M3lYLwr@7k-Kw=WH#=a)2+hA;RJ*GyjFw)F=jGHtz|NhCaGCOl>g5SKu5
zYAzQHhP(0%9+2~PlR+8I`3wP=xm~p}ds1Vmdv^&<_DIA^=cku3O>rnx(u2wv@*VN&
zXRPs8_z-<&Yx=Pb;u_?z^Q;_po@o0NU2W)(<wiqe8tu-E=^`adbB1YYbe7|16M<k%
za+fbSV@PR+nkCN&=zSu_Ou;hwl%SXHVD&XKUbf)X2D%?|Igtj<Ry!i;tJxzTr-Wyh
z{XwLS&(O=JvWT<ka<>|w@STYm=+1xpAAFVRL7~D@5?8I&)4iv?&8l^H#c>Q7#>Y%%
z(p_6Yq}YXvI+}37Nwu&|mp#hG=L9BzWvTqF6o-g^+4=UxASL-S+;|9cO;<x7f#L$5
z#3E$rfu>+@nTMVqB>-+Ubg%wDomc0NjEdf_9B{^4DE$?tt}eZtDVwu(pk_hkHrn(p
z>sWN2iusj<72a3J^ct}RO*12#qh&|4W5wOlJea0jV*0aF3l80*GcExF)Ms=|S<ZU9
z<W$CkQJ&jTp5Nrz84))leCdk72#>)V2KW=LkGWL;usQ8$IT-=!2F8{sY_Jf3ZbV{`
z>JEtDIsMYVGGhdtRElW&dmzL4G&NhG41$^|I@4rlAyqN}U%Dv(SEUEnJk<~9$@Ad)
zESc41@(FdRfKC!yFf)4jHR<3|;5{;Z6lu}jW0SkH*PEkQV|+0iEp_$<Um~wfW~lAu
zseq^Bv-hT-G6ypiY|%-WBT20puDaEmomriAFHmXjmB`=46O&(A>lI<Ar+T_Qf1?tG
z$Na?1NrA^+-^qjBx4{E$W~7NR(_2nR#LeCm&K{rXd{<q}^vpm4*%?UOX`2)?TR)>l
z^=oZO<>-SC2NU6qiVGLGa2WMi@Y8i3L`>lBurU@QK6#MjT1e8X?Cf~669%O8w=MdF
zG>bkVQs}#l`+VH<bmQ)My5?l^!dTZEnS*k}iAL#LEVVqd^owOkqiAPgcH0NL7v+q9
zV*EySgkIoOP7Sc=6Pe9HAzNTahFHDH-H{}Ub#57WFibHnW;f(@PWo_m+g+XbyE<(j
z<{s3aA;`0%os)92+rQIyr`0RWae3zfiF_KaWYsvvu<5)gyN&h3#U3ahaeiMK03|}*
zX<b@|-Lbs;;yhDi+w)XZQnah>L~43WHL3dc`};V)w?v$jqt{PLAEb}WyV2b^K1gdJ
z(W26vrT>U`U%}b!r+%3uG~53&9!vE2Cx9uu>aWZxIiHxR!s?4m;)`>_tL1vq-I>q-
zETJNy&SYtp!A_?E1!#S+*L<WyMN;Tl;jXci!!0kE3@LWm==AlZ%jie!Y`Xs1C<|wC
z-4;{yQaCZkR8a^zN{aR_-RYuca_6LQe|xO7XfaUqQ+!Exr>21ej`7igVvbWZ*;wwo
z_Kpy&Do9(zbExTt|Lh4Am({+)7PsxN#r-{Mbt;MoC#K7iSXdJ1lxr+~eE-WXeSC_-
z#57)*0<AueA=dV{dpuI-q?3-|$WTZ)S_D2<t>-D;y_gOdbrM(LguA{&1gKO$(y3sB
zM!@WoZN#U)%zy6uD&9RS51bss+W1}iwA*|V{avj19A+UuAgU+*2}>`(16QH+$B43h
z0abiF{Usv3^Ph_LF01zXAzJj0aH12L&^@J4o$%K(PUo3Isr~I!N{ia*fz)A9_QOkT
z4|Lfg4Ej~hcjotaC1thWqLP+fbLf7oGNqWz;Y5`X|2ultR}dRC8Z1Vjb5XI24|#m1
zuQAOQK1w}&e3Yo3zSYp`S4FECO6|p+iS~$M5h!>Vw?M4!IagZKEh&0^=`}hSYU!&S
zQ&dbf>yK#XoZ~66sW5kRvG!*Tfo%|ue@iW%Xk2F~vF&c>13|!-3GfG@4ArYF(!1Q@
zabj9-zl?9yTUzw5@T%|fUQ%QZms<LFTylz`=Sxx}e?@f2Lh=-`;vSQ`C#^@)Z?$q+
z9)cXF&(fGvSS<GsL_0Bvue@;LV~p7%bIa_~liV_UvkfeLH(7dhW<d7vz3VFil5rk&
z8YW0~L*(yJa{6)_Tsr=F=nA5Wq@UnhV0^R48t>ek9W6?S6Tjh8NztW+OEJS3JmwwI
z#w9;f5RwzPN3#8i(xUR5W!IN*@1u2S%hL=o*Lm4CKsr>ev1qohRm$H&UZL>nuYg10
zpKMXn1p_mG%8cJk-f%+uI-OR^^l9k_>XLJqwf7i$=^m*on-j&POE>W!{ZTp|{{}Iz
zg7jT#pr@Zki<)wleWu7+Hq|UhZe`T|Eu3Jfz-2u}&eA{PQK?>|w?1`5+m60R)Zb?O
zc8~fL`t$>Rf%SgBFSD7{w*T0;_Bo3=se%q{zg@Mtt$tNih^?6>+jNgv&X>4O4ZP)|
zKm#|Zfmdif0-ntX;4B)by<6iTX*jk-*?<GcGGzxTdZAkb{tErhxL^%mQ4Rkzl3H@d
z;&D`7dLAst`9Lt`$)tp`T5fQO3Dg*3`Z=vk|2G<(H!jlG(Ifs#9WREvb&o1YAE6em
zCxhRG`IOqX*10_xca>ne2AG_#V04|LV?@)ho&8L$KE>K8=ec0rNwtn!c5+%8n#X5Z
z)KZtOdY-tFqH3(PFKHZ3i7^QO3&=xLyGP!E$7A~+N{ec3=PWx$n_Rwp!NLB%jlx6s
zsGPptJ$78#{hb5!6OL_qBXfC5II)D$73+Ee1tpwV$a7h0@wj*qw}0J1Z}oO=r%rJV
zEStJgh!d8!(J9g_6JH~q!y)_+*C2y5Q}`OC=oE|>mmS}Ct}BlaGRXF&1w~T|m%gU6
z?z$&@nIZTx#JZ8vgDX>{Tec%!{S>Am%qr3+`bs_emp*~8%3+(|c=ss4SMVqkKf*Ex
zS{UEmra&d^%|4#8&3+wD=X*@Re-I5vFlzs*?>y(7hab4X$R2Y);!GCYFa?>(J;%Sp
ztcwt>+o(=}6Ud%rE{nD8gw}QbyuAY0KHYD1pk*(Lm1+OYPG^3RfM`E~QNRgCfz6aX
z&ZUzKAKdcA03RHGl1sXuR$~zMZ=6qGZ_>DqrB;j+Fh!ffT@{c_-%l(Ykd269A*IDq
zol0VyGi8g)an4X|Hy5sPds?B8aT3T-^09Wfbx8q}B|pcx9k!(uQN;lHD+cm#S1Iqt
zLX_$KD#Q4~T^G7Z?}Q-9ns`lPw+)E>v6D-C(rh?I-*c|w1EcdP9K-$Ze?QS}FJPPY
zet<;6RRB}&cyUg#`EL#8|KwnNJ;0%P>El38kJHB^_0<H^pENlAe=0pc(%((s19lL=
z$b1Hwja>5jV?K!n!RLHAm@z~ReOsNzVBC?4`?+&vFzzVDeao3j9QFLJBY>0-4hHg7
zx*~0EYYE<Wc|~+)S@la-#F7VRB%fq=gH<-03b6=h`<Hxd0Iz83oZa&XPo4X-+s}|i
zMA3m;I<$tvI+9ATh~GTF2Q^#=lI3y;nicEpegV=H<l1%=4?p2yklQjAKHQc+0ckmn
zfJ~y77Yl#UyY3w&vitje)2RWncG2wQoQt=t5i^FulFu0*@IB64$hLD|SvYYqfQcyA
zk&m+v<sj+t2jGjP=D6_f;XBnW8444m3{0z`Sm(Jy1Jeu_rZvD6OFlQ)mAdU5%}%>C
z1pt}>w$ak8SIVnjbUs50#`H8S8XKt>zv;gcgwo%G>@xa^b1l{R^t43?NR4=AkxNfA
z$)NFl5{c4ZF!XwUF#YW|{SUqL@<dkpm%v#1Jf)|!ytW}-V4HgfW7+8=DT!)K5z)JD
zro>^I+cnW|Zl<knHxwaUHLgD-nRq+J`OWE}u=E_FGwrqHac!$h9*cMwmOQcr+hrEE
zZS9skeyY|EC6Dxk;mPCY<nG)zt?e8E_csQw_@nbS-QolIO99xe4)N&4oH3Q|N7fr}
z+w|)^Ig25*fc%Un-!bdAGZ;>5A%L>#1%<Kd?`WncJH#%-@66sES?jQnET>z_uym|6
zp*1ha^0Ah8<#0?6yH{$!xLm^r9my$~D+znkv07-|cVsw`%|i@dr#tu(3MX!3Chz2;
zWFTtnsSlZrLUE?hD@sUkN$SkMDLJ-1C-GT0l=k?t_USC<C4R3yxtoOkq^DT#R=Tdg
ztf)_^V(bq(=#-PPyyz80R4mR37WmmrftAiDGcU!?$(ffzXCyCL91?5cPe*=k4zIXL
zt-1dNNg(I%pAlp`SsHS7@@Nj+w5W>q-t?5s#^%ABcDRq-p(32-KvXz&=dnnA`;w~5
zdUHD{zK2vY*30T6sO$8y%e>h2eZ9*Tss0dW>#H?(3Q+6tmd$0DbSqN)#7izlKCwUY
zKb5NGxM=5nvWKGe91ZZ%?p0^<hSt7nGU<~}vhV9}z9okGO(xLlWA!yEdQo0@)kIPd
zV?cw;-oY1yEp^zQqix#gC*1R>t&;Ae_^%#QpYKnND&U(8p6)lD1gJk^b25+e)^}rN
z7S*5g4UlTIwIv3!>&-d!Uuc$t)w6?!BHQD8ZxZb(uX7!vz_~}A$s|sP+mCCp<8n3#
zxbGa21bFt$@k(b=tQFMde5R4wCx8Ll9RgVORw^5&)n;`LO9{3U<OCZjV}YBcxEl5{
z@r_*!EJEukPfx|MB)5LO!_XJwf0UV+V$IcHg&gOjB8$tCZ*j^Zwfgxh^b}FbxqeA3
zgX7ji#k&B7#~dC-5GwydxBUBrs+8R^ttJ$j@~OvGp%OST-ujUE_3{3qTIW?+=~rpM
z$o0X%716B5Q{h?gEH&s`pj2q<p#ZJ(2~8^4#iV)wG=es#4|$+PidBWsAEw}K;m-Ap
zbrQZp9x&|KtPq-@G5lc-56%%|Rh6MSy@$K*%rSI4&d~AFW0N~v3VxLcE&Q1B?C$a8
zpD9?yx%2`01)g&pVnW>ZK5@;jKmt(7F_4y&BcJI$Cvurj4oRv>B=W~v*RABU`f8Fx
z#e;)p^}~FU3R+>0k*%TG#<#@2Ug5sR?CVtbb*_Cqmsj8a_EYXR%(6PI_7wBYJ=XR(
z^WvUUmgOcawh3R8FmfHsv}5o?&K)FRAGsa1^uh5*!4$N3a^!}K3`Ub`BpbQ*3O%V2
zBNq-g$l$`L9;48NoNLAzjP1Y;+{f1IU~Bw?+RUXgg|e@og|a{1(|12@^L#&;=R}i-
zO+ypiEKi>;Y~5~lb_Wj6y`3IMp_2?7ph=gQ-pr87%XCtZR1&+9rhaD}3xQWSPa;<#
z;|jl1Z1g>WXwx~0jyeBjqhm^KI>|<_a--E*4E;SO?UF!R(`iau=*+Xxcv<%=t=20G
zopL|&MMY`{OrcZgMyfJ(X1MFSjOTQp^rLw92OwN7Dof9j?V!i=YN$CAjM=R$NTT$0
z#=9B3c;8Dh-!-h~nu85wH#6lW`<NS=34`1%K;-RaAtb#3>w=|ki#?abfhmbU)EQtk
zt^X*;eCLoF(cW=XP~o%~6%406eX3M^ST}rBIJ1Kv435o*m{aV2$Y9z1aEA9mcxFL;
zj|z%8xn8ttFqk&!3!OJ%gC(ra8C;Vd`ywySu1xfcDuy#86#bwVtzyjJaf?}>`Cx~3
z5>|eD{v9Gx7HUtm*)IVKe^i-T!QYYg+l7!m1L-&T3Ht-&T94Szb%{Mq*O+`I$sOqr
zxO(4Ee16=8W(Yoy4t@yYb1$+_1rNdJGv0@R_}u74560&Y-4BEC*^`M5;&X`?Js6*J
zGan9v&;6u26h8m)X@k!Ncx3zdT<YPo(8cH1N#x`6*Vyokoq@is7(ky-pjRl_5PaSh
z{1C+F9qxxA_*~(A7>LhiFM2ROXSp8+;j=gs9mMAtFM2ROb2A?fgU=t4>QMOn_NNR!
zKU^Qc=Pe#Si(GuhN#x^GJ0OKmSCGP&B%;w%o$0o$hwH5PjNbJf3cBw+TZr3R&TiPn
zp=2|nopL0W7c*?6ReS<{(G#l!$KDCflkj-ISsHP|jw@{6M8@dVaGep3<Gz}{9N|3c
zz8XG0!g+{S^&1{Jdz^dSZ-$$UaDHaKxx)5N^WqBITit}k*w3`#avBWRFz+w%0l89`
zo<t1%c?jKINrGYNE*kt0q`Lz5!w|X)dmjeU-9DtJYB1@8d~v7yVG!MI%R~q1?&n_g
zV7gnA`EVGz`wYPO9c1W7y@bZ0IT6#4^#r_WT9nmxt>u@;6&QM4F+V_$4|w!=z9l8+
zE<;0bZt=^f@y&6bAqF;E5zO{oFB_3Y4f!&O(m#W}I%{q6(}T%3d&w_wlh07{4oZjP
zYqpj?+Z?=NrWaBl(N-oAh+H>7`RQ^2@(!$=8xVu`EcgCzdta?tU+-XX9a>+pi|qC!
z3l?n(<>6Y4K0M-o;A2{tjY)YElFD4m7Q%NUui=}t3K8u-m&1T!So;}oDF1EwGsB5L
zGhU=yf54nvfwh}5SpcG}SV}_+x-6C|j+39cUjUMyl<yAT8E{R6{}Y_1)E&k_h$&OJ
zYc)0LhS<!39PVye|DM;qWM>f&cnjD+yWjcaCo~1#(%mWJJFV0LA<l+>21Ww|z!b3f
zRn95hx|)M2kkv0ROn(N8^aj$&nMql@ck!a;u(4KdOPHlOM?d3HxpmodG-xI+R?*~9
z;e_sQQEIM#(ZnkGl9s}3<K<8_2M1uGIe-J^P8NEh!5jtQ;^JaD)Pvn@0S}<$1R>un
za{fY146oK1vgMpgn(?Nh%}yJXV2d#KHR}=`fqv5X=ljkS|1^NPzyOx%71LYvr0EuC
z(-^#aIE86TM{-9%+cl+h?z@QVo?meOoNow!R&`*!oXu>?7<VyM8NxsL<LKYNarJK>
zDIh3-5a_|sg;tNqCZ0|!e7ral0p<`GN$7fYS`QB74JmgroJMI&#A$-2z<0#t<cz8}
z+D2r+4|hetoO6n8!maZHO}Nr)LYv!!&z;T)dzE@zkTp7z{|NTwP9aaj!GJHoof}~$
zE;UQAgu5OvhO=3?=2mV`kA@SBe?EP?;)aY@bGfuRTxSgsa7<|T?~JvK&Z#iF0p>8*
zpBqkGsd;wt5A0+uPj0~rw>-HGgI~-nGnFSFb?$=-O#17?UAHMMV|Y6yc6K`apT%8;
z8sMO3;`f>Q5Poktgwiea|I*B%Stz4nHQ~f)P{PuG9-rmRn3&JI9M0zq@P4krSfDFu
zKA^hnP<1Yen6A?&iYjoUseu^hEIzV{+H6J$mXym!c`J@naVxHeIck#|u)>=9oi{%Q
z*&$%Y0Wk@21twMl2rgz~P1%o=3c-XyERa^i3D??F0Fb9ST~91sOd-eq*933e+wm^L
zz#>Mcy)xJ=KyBha5yZ6Ulq+j;kDX2y!qdL+8JjgB=oACHAp8A`PQsDuDOegl{$sZ<
zl3)KW-d=DEW5BXR_)}Op1<nPICKY;G5Dp#0HVhaqYN$t*OF16?tOetR@dBeq94K3!
zukTtv17nVMa_9(2aYf*RgtW&K+QNi{*Dz=3lqGdIg(H7d?s{k6tuS0{dasb~>~OvW
zA*(2wX8NR97>)s1O@1N6VVscC>2R>~@z+BE?oLHA%#Be^9baOpY_oe&)X`j)+*dAJ
zCeBb6pR(CXMEj(RK2wvV2QS6OCUVwCYBT1hmF9U)iCHYSMj(<SZ5Wp2$;UnFH~!iC
zHIxfv6RI$+5jeS~)$9O_C9`43nw(9dc@&R%O<SPM*tz>Ds-Z!-oh!vv!<GJpgf;oG
z?u$fh$k*Asgot8Fg-Y3c7YQfEAs#uYgxn)A!SMph9{~!;>FRB|8hCbhF(rmqE#jM$
z9wRXe#><y~#6Qm#TkU}a)y1Pe+y+67oq1wt(MMTNmDobWj5;ZGT^63vCq6)8K0g$`
z_)$iH$&lN2tim}b>cex3J~A{HzSJg%mWOq6>#Z(feo3G-kLUV|6623<iu(}tW857z
z%G9rg++h7vgY~~*GMvyhQsJqL3aS^-gEJkU$>f3X6}feMADobBbQTnDm(v+Ep>StD
zl^V*W4?CdI00GaD3^UY=MKd3da(JTgf)A3fvk}2$s8wB743Z()&dwaF%StDtsP&&_
z#9POlfNa>!XCvND0|=RyencV&?0RVWdW=J2*a2}FWLw3jP+SsYJ#5g_7D>O(bjIuv
zu<P3YfE}u=zW?#)Uk%S;5QRIZLlpl<&3zmpW*ceRYg(Qji42rJz%t<DSPaKMnErso
z9OpkJu9Ex7_3+64@am&@PG7=*@Sbquetl_!YrWAEUfrrs>-djDFq1pVlW%JU!W*@1
zCj`bQ41l3vlz&E7fML^B_<y6h2_~}#(rmMm^ZG|{UN=nLr1`c<1B<72<E##U+G)Nd
z_@YSBYJrSB$C~JcZnSu<uA?)N1*h7ysO;amGSIclz_7Yjz4{b=rJhKS7CNjbGkhZA
zEIBz~>e>{LE$Jmpx$q0H>7Y2as_Af0ohgi%3McNyoaOvTRFE3+iD1niaBDV);`KVS
zyvo-?g&qP8tKys>m})LG#;N|hv_H;}w&8FddlU0=5~8FB(%Ez`z0%t$HVNgD>+SnA
z#h{bYx|NTQW*%YtEj@*(o<r*+ov+ks@vf}esw1%9J19&0;tDinFq{^9hMkq+zaqFQ
z!-;38S$Dq`gu;n#@on)d{&t1q$)|M;M-h^TLv=^<jdP{{rf=>(phD-EJc)7KKCpv^
z6Xr@~QHeGL?fI~g@cL97PCP5F>6h&G8AWT|ZIUU|xsMD=&7>3p&m}{=x;J*0>N9I=
zgJ&1p%$#DSasmE0u(l<f(CXtQsqBNwlhIlH`gGpkK4+#|6Us@iCInEm@B@h!#FLIz
zX^^ulhrA|ma+Lz1V<$y=VUdMS+zA~>+3KYx|Ioc}(6drRj+Gh!n|s+=NsMOTVG+M;
zIcPI+Z`e3vR2<>VRZfz)c8JS)bxwttp~l;0F%E2Rib|q)>9R+|=9a0Iom=V{6dAUb
z{uP~K<bm@%PXluHkdqRpP@>N#`|kDlnHpDVv}iR~5>hycl_%eVo`s7XrKm8?Z-%qd
z?Br%Ov){QBeQsdGS6dBlK*PUe;s!Q+fpTUVUa41_G+<ui&QEYFvt7-89gTc|VRA&T
z;VvolJ_0(>LFqozjkX6(zj>YLHo^AbX&<qcq)XkhhG{wl4{droEQ))%j%E0|_HXJg
zRNZNOE`a{Ae&^LuYQ|%k!O_(R2Zs;@XR*?TyN)5T76*$JXY*$fmGLidPJvJd^xv(v
z|2nU5`;RdhO7ADrz>?QeSKs%0+@nBomx;05mYO)XG8CHO{Q5*dOK#q919>r3Gt|TZ
z*cwvOUCTw9KebE$nGJ#+5X=~Ia+D)}W1J{qHbCukE<|K$+D8As>D&o9#Jbz_NIMs$
z*Z5VKq&uCHNayY$#@VdDLgl>x0ea;j{!w6eI+Cw`d6(PrBBWhM+F*GPn32+6z^AVb
z`t)P-=~odcAGq6QzDmV#Qub?s{W@>Z*Cu@>)y34Daa-Fw9KYpQ3X9M%WpI;m=PQNI
zTy=O5tKDEVs#Q;_^N#)js6C(4Ya+jnXdSw|mXK;oIcSGt&wgUg4Tf8fOg>)9pr|V)
zNe>;R!RI`}*zgW&rl=oKR7o;jVx)|-&iyFOjgQ@YG+47=zv6z~X1_KM{Mzb%-EP0m
z8u;}JzUuteTyt8Qa}rENuX#EadpWQ<*=|fkF_o0zye1AyngWwXF}oDQSk)!&2OwX!
zNB0Zm*}3+csORO(H`wn^#|fhk%(O><iW(mCzU$+|_Txyy1o-&#;K!fwQHd78s4>KL
z4%lAOW9-zQlj}<~`7e2tag(Mz32`h-K17e^IzQ7m=^pVhCjw8|!_1HwNNLD9ieizT
z;Z?_xn&GqZYR07H1L4H=8lTnw?Cr|sqyA#OU3u=KcclB%YHhMhL^}RK!>4iO+-4Hn
z`^)xfK&`i_M6A;8m)pX!H9>*po4u@ilYf?3`kW65&U-iN1G?1+=S28gX==i$@#=SU
zH<?pwi&6qrSPW2^`l4KPwx^`%g%UWtFKffoHoGD-X)ePGuiL0A6Gm;N2;4EP<UP(7
zD*_&nnG?IVxVeXBdKT>I2n4gQGMtcqJNWWYbGPZTE<?iL;3xLg&@8Xtvacf2^6Gzq
z*&RIT&&z(jO|Xk-p>Uyn-Rr(y&8w+eCwhkylAVF7i)~`ZO+4Pd?sH#1ZeMfA>FxqG
zj(g62XbZ~9^`x)eCj@9}g8q0E`0k@X*I#;)WC!cBU8+xiJgP4DQT6Dr?-nb^KXklN
z1a7`8pb+a6NF&3QO<M7N^oJ!pX$ik$UJ`PigA`)jCtmuuNT|2CfEZ|JW2CoO7A9ik
z1;I(Ey~TU=5bG`8XTsDa9I>%Uz7w$|L?1);8!84!QLRbS+g0E`rrVJ}7S3w{fnoUK
zv;dq)j)8EVM55?Df-!wBatfv?AFjv=8WlU8N+8y7OKx_~f@So*(36peSl(jE*Js3%
zZ<nRMH;(F^---siM||nU2tEF?9ivA&H6H{Mow+`!o!$_%#y981V-NPL$EbL#>ij<F
z6%2orzC|&~t<KL+vUS7$Gxd9y)}scE$e~JG-%-wlk>pR^Z6N9(BNQPU%~hXIU!s!j
z7nORIK2qUl-3qVOY$dt1ug~%iwaj_heA{_d`X7|4>NEAbj(TRHfL%@t3cJ@8(nxWm
z#4hmZ^ue+;so5ItlF)E#=1Augpx^GhH;_2IuD`2)>1mkL*Yf7`gNH~?d${-Tv1NNp
zk}ore54h>pA2J=~?-G$PgCULnlH7gtlJlbE0UpgINyiQh_CU)%;aqy0p^q(P0s45B
zY=({_Jcy3ppm&s;W+*p>L_crGmGeQHKM~9?gO9JvD*vyC%WwI;S#QbMZ({(@&EbXJ
zD}+YPl%RHuY})P<(V9yE_u*)zMXrmyV%d62?PUTj>k{*|N5hFdJUH_}JTpsqPVuov
zAEV*hwxA&EzSPphi{Y-F#Q7g}VdzESgwBT}O*DLKuSxSH?;Hnr5z1i&lEkmbT2_dG
zZ~cbAc@1p?n~v*cCSV{dnY=NY0Xs8MsqcA4;4;C*SUP7L@IjSE15V`2lx+0)wU8TQ
z?am<ul3e~Y!qFgI9BC&zIhOo!o)Vk@v{0xx!Ne^O*2X*=*Lite3l)II+1;8e9YE7|
zrb5>+iQiokAOeR*nNIYa&R=B+qho(be9}L^)0F>+v~~QDHd0}UjOlxvd=e`xgNIN}
z*T4AWf?%xeZtZ6YbKArlvbE`wiPS%d0h9@VGNATWTOfET0x#*;sZw&&=_a1^*+I~s
z_dq{QwL*XAkO<H>noP@y43#EW1kMk@jUL1td5?3qt;hKgY)tq$1+bh?66r(uwuSJP
z_#g-$XYxP^KzJ05PS2ol_8asO>u23d*7Qg1B*pfRf#Zjob);(Yi-kNFkD93NJlYsu
z3|ya3JBfSSh3gkI!dVq8{3r$tnAjg11Xqy{R}+M#GTsz?ztFs<N5%&co+VUGF`-pM
zsD@xQ{Fn*l2xW#gN5Zwh<RVYLm~9>G7aB-bZFSB+f!%Mn?&MBy10g*^@dj81p6fGh
zUM+07)Yw+(+!M?ur_=P90raN)g#y14IVC?P-q(N<@7?)Quz;ggfTLa}*>xmS>)!$k
z=?O;68JRZQjsJ_~@=m>!x8&wN+>(tLEaJN)#N{?#Mg7`MdLdDR<iP-b*7-_DS@q(e
z{b#*FB<CWTLt8ZVE7S!Wh~|(1y))%?J%@RjtCwviPYFw6iE<F>&bJe3Ju4%BvJCwm
zn^3&yf%4=d;T4^H!!q=`zWJ7+y(4IdF)OJaj6+YT1kX711Yb2}<x_DuF=0UAzOm*3
zV;nNYI(Mo#Qd{HDKOv&`VjQ}bTud{KU349J-qkX6p2=X1LISp+T&L%NL9M%BF2Xp(
z#K)NSb_$$f?Fw6=imr`N@;K?3ZA)Ye!XV^&iY$g8T}z2OcdjL7u!-n$r84LnU?n<F
z(HUdOr+J0xkAuLg5t$vCd293<_Or-9QfKTrm~hmbVzmW(&R$4N;3FW+VvpO9O2q|i
zJg0&UV#cM<V^~1{PB$KC1{<jnp;Fkl^%e!O@X}e3znG}tfDL*YarBz>XmYvopQizt
zq%y%qet~k}%kYdnio>qc*D;_x`xN_n)8hi~8N10#un1#!S#qTC_SgW@>q_x;pn>Ek
z#0)aMe9v&1<+LT_e2<0?fOCR{^Mf9oyoS;zfR%y8mQzOG579L&zt#T4a_;E9g7Ffa
z8Badtsjuv{kX+RjlkVa%T7x(vS}ZIPoR118zRkOHG|G%oEb^=lAn?Q=t~<htx+)SG
z*5>VY%TYw?OXF;E73oZbn@cR-2GpLKdZ(dLt{f)?e&w!We7>8q)1+i<(%D??WNvL4
z=iDpYrf$yPNTJ3U&YnY->73WIMU*@lGb+^O7Z^*9WHBM_%s9$I@FmK9vr`V~EJ@(H
z`T|dC%|C6ui6?n?K1p%V#CIkK^n)Lba0%oLlcU#p99Vj=-fJ)vI7gCSPve{<%0=G<
z&!6~tQu%V>X3C)4iz(O8+UaglH<3ssSzV_jxi3Syp5FS^U^y!(r|;L&2Yfv!FMYM-
zjdN`<-Pe?Em6>nT#2!q0>V%khoj8kOE5-^U3}m_e(X7uLOFn9d@w&HD>|B2=%I-Fy
zdn|odh}J_hg@HU?64z+%ro4)j@WXda_gST@-lM%Ozcp2AZ_DsS=9nV4ylc8$Y(rwI
z1HT#7wQ=divF=uw&!rLVFr5J|u#M|f#!-e(ps{9i;WU?G=*hbE@UG3LxXG}D&%}!j
zw;Pu|%@tfr3)!QpX(>}0O-aq+>UR8Pa&NJ@1DSi5{Y7|x*5W-8*07PIpCs;6Ibz7J
z0l(M0xGsRdH7_pT{wXg8a%*<^Hm^=S-Rx7`c1v-8xM&c?jUrL{q5!}7Y%tfY<Vw%r
zYmakEFkYK)(&rP;WXiVBc^l+I(rr}WSGta5hQ2>8Qb8a60g24|VUP1LkAA}Msl3lK
z4xL{Hqkp03sYDM$rQHV)K*ViV`k49=1KY>cx6F%+sUGv<Vycsu0H)e_buJ24`m`#2
zoH5jSx7mmkoba@%Ydrqg_A%!Ta+|pi1e7pblg`b|aNIZzFHVt9+m$%8mSaSo5pNNr
zUj9^kt7|u7-*)?-kWPMMwwo8XF`Lbc+n8VT5@^g1dF`}r2jRrm!FlK1JNO?b#s>+a
zo&6WJU!DQ~!_K+GfPZ|*8OQjbEC2g~Kv%vER!mnGxLx`1fdl>Nqv4&<ecO+EoVVXG
znc9Mx{^e!LcQgHfMCr#_%XS{I$xDODcYDc4yU7<T`FBa~iIs}<QaZ+2Z8PTtGwW0s
z1K=m!%;za{t;wvVUZy8?h*Du|obwOijUl&4!v>W%?Vc~>&Bw8#cQ0cwC-Rq5npT^e
zwSL>tMQWp<Sf>%Aw{S)}RMBnr@5i+#3OB1q%9C4}k)sRE(xNoW5~Fl=3(f~R0#P0^
zx?IaS3c3>`mTo5XCr?s~dH+rh4O3gR*smiP2iS|#r@wW8{ihULAV-^sb0Z^x?%I#v
zY4omlD9WJWqbf7A*Y}?N@H}@bI^KrlQ-cc3pfmHy^KB`=q!iS}W|~B4*#xFEm>Gn>
zg&A2;n|cNs@Z{UuJZCb#MS5VGPiZFqX}tH{7qp)ZnlJoc!S~m|+`o_SpT9W}-^bHZ
z+V&r~j7RU%8HmH-yNs$1i|>cuHl>tMN(OBaue9Ouy?{I(zPowOWIU0K!1PAZ|EKW1
z07mlf<2(AsKz!c`>C(23(6;DZ=Xe+zMBjg;lMajTt86Jx6$bG=$t%r_z#;VgCY|Wv
z`xNqL@NI6&NsYLP(hf)8@#I0nQt*f9zQ#3?imQ0D;XKWTkpL*HUC+zP$K+04wmcOp
zoo2Z@r~D2R-flL#$_Tj8yxZIAl{luvnl8%+TP$CZx$GCDeRh{Ktmt|KN;vT_Uv(<&
zX2K}!#+*quzlT%mCpqWR-T`a8Qt3hccg}U2>5^sLKsK;_-vQPFnT&D@mU}?jj<)%%
z3}tt1T=Ijywc;=0SC!&d70%-%r_f8>LZf$8Qp<gV`g0^iz!@v#+tXi>3*_Qi72;Wy
z;Vvy%xc#D4+8H=yUl#42W}4#0)1FhE1=KX4J@wR-{xrGSpSH+?`yBck=edPlaC`gg
zIv8{0b}lx-*Dus|f)LkrF9@TyaMv$+5TiCe+H}|+GvSbm5|N~5O73!e=L~2D#9u4k
zj+=a$w$7RpaC6CJCMSpi*EgnfOI=3UPhap)QC^AWe-QS7Hq5Yn^1*Wgq?cY`rlLyU
zIyilc+I+6WX64{djrw#dpEwWB@Z<lV(d!C`WB_h-P!6QEXTo9WHDYM%J=5#yB&X1C
zxP?aVT172`>2(er_J2&Tzu{Db0qyyYI_Uo%y)J(3e@m~ay&k>(^IwD7khFbr6|NXQ
zs|?WV=E3RLs?CO8*AD(956=PgDoe%z<Mr_W27mpD@#8%le|;KOkm0Xlt6GAIc9*-X
zs5k^4Dsgr~90U6GVT+TC<r0pbci-@QwZkw0JHLwK&|=u@|A=4h2q$i$B9~`<v+r9*
zA9VTab!3BH|Bf4M^sX(`G@O3&6FMy3E$`)*;jbCynm*U?){yh*!7%HjL5lsV{Ko~e
zRJ!!Vq=x%m?r~p#K4|#w%h1z+c5hb)9{IoEzq`X-YpBog-_<+>`R~hz<i9U7a^b({
z1o<ziXUbij+1OsAh7Zes@7;Z%-zgXj{tk<Ro^dV;q&1{V#99x&I;f>jsii0L$vfW!
z?~XX$9a>k#r6b2KmzGsG6*@mhlLLoWT$&p7%J_|ad4})1S1HK4KribUu8Hn3Tz|_P
z!`1m#f7@72;%Vok<lJM^ROzM1VsXhnj^cV4r9H<p#j<jV_jT@wZ6qYY5q)<m!z(#T
zBXuD7cXnOV!rHUzTkGl<w6`>!U0v07ZGGc4i(AjGuB)$UYN<baUVD8*&DnJ=&8<sX
z&;D#nO><RC+mf@dYrZTxqjZKBp7q(W<IZkvsXlvhQ+s1g>)7h%X19{}vHbeR>W223
zTCdKtXSC1zLTz>1%%-vResv)WK%~ihic~i(Y;LJ<t#4cq8C?@uRMk>nHLszzHB!La
z{Fd6<GZj6*zN5Bg%z4Lz6cY)VS06>w&UKr5jxD+L{co4>HlwMby{*2fG1^$wu%xv<
zh(T4j7i)_Nf5({8pC~f;x7>SqAMtJAy1b&L36w4Ly7YZ4e{g^LRRy{;l)-;_Ra;Aa
zhlhW!z5e&28D4&q<k=gZdwu(B_nr4X!Yh1S(N^DJJ7;uj<budCp~&d5<Ikx*ny}{R
zP{HWdF`>~7^CO|rHOGV&hs?J~C~@(@{@eN8!|y46llax}n;ARU-^TA&e&6Sp&##1^
z+Lqe=t2^F&VEu)~H~*^Rv~?$Lxn{yEyWfdkc7Yq-d+PM$nh{y+C%NBC-sg5^##Ux1
z8<Yp|?F~PJ@AtX>cw<d%M?-yUo1wdDm(83Woi;-NywCL*qN=XVj5oi1<0sBovT$K7
zG*=y@yv+CUCAE#MwZU(KFYulGUXl#eX1;%B2CQ^Q0h#X;gWsn&Ew*K3zJG@D{Um+`
z?=ya;G&Q!iwN%wNwz((@L=4Ylt1~$ao2_rGs&1=aWLd3!6tDH7LpCZB5%T!Uv~PT%
zs?ztn-!E$xx6O2W=$KCo{fk5*F7uB>&N?d+xx6BBS;frwWz%NxWoYta1V7@Yv8}9j
zQEdZhhW=d;iHvSl!rDejqUlu&n<bov{0&MswW`GxY=cq`PS*hMq`)DG2c?@{yP&-R
zY1mpNGz7{Vlx}K!Lql17W^HR*YakJigVIgAa>_7?hNdfNsh{6ArKzO_xjmp_l?Ti8
z@G_;Y2{C=`lIgYcp%2s$570sBO0R3LYN&6kN2Cw@^Wkf&ZCMDdskWJv*;3V5T^mTM
zbhWB?o)nkiQrN@;(@n2!sOlJuI+M^(SKHh=t$ksjrh$*abTEm59|LcGx`kC86)p7(
zs~Rq8U)UT-;64V^m4K7sQ`vqq?X7PFZB-42B0#^q`5^C*sfLz!C_?qiThP+9_)v*x
zi=WPgZ$?{HTYKveat_d6bL&)>=!aq@P+nvE!hu9Tqm3@|v#4H6f6c7})A>ovgAbo6
z59HJ?tZi%?Se6n7%Zo#U5c0r++&8~GfEjk64U_JWgUzj%{yoO<BFH-Q<`;gm<ET*|
zx-kF8mH%qF=^qy;F0^uUv4X@iUyCZly~gO5%M)Ymd-3YxvYyY~w<hc43;w=x*O=AU
zZn)s$Gp_s0vg;OH@bRyGdve)hsSCgHb^X115on8?1y4g28r^!<h5UD(WpVC<VRxm|
zr(ZTbG%GrNT725Yq4>0^mxZoqytc7vabu*mrKPC_J-wzjG^4#5N!&WWy&=+4+t%LF
z7%Hh>gz^r9nztnK#oCspkWmXFP3>)wruh*wWLra}rUd$9UTsT+L0qd|sR_-hYH1W}
z9r7XGIKOFVEQ71DEz)2ZcXeG=;{uAXwMh#Bz?PQw<~AayHMP~L+_t7jO;cz_ZCjh(
z+M6vnk$NdEk@;2i4Wyl3+gjV^y~OQ^^H`aiWKe8PQ)4YvL~1)Annp>S)~1F<G^}b7
zMM`9Zh^ud<p{Ds%V&Cf<=hs%X*1HK>eN;9qu_e@iab*|W?%1ktt&KEW?LH(LH4KeL
zFM_2qD;mIf=QmU>2$axTwWwCW)l{`rg{Ca2ZV(P?YTIDzK;%WJaOzw_cbZlRAq$%p
zDee50riBq#^n;`GLeZKU7pw0U>)|t_9ta;*4K1}*HA~zw?KE(h_<@-ko7#qC2*kyp
z#`>zp$U-yoP-*I2$g94sHh_#8x>T4AXg-0jR?r{fV|z0gG?+AXK>mT8P0a{~`4Cbd
zZgC5B0|vb~=!=?f(K_6Rd9{tzbqlLnuC-k@*?+B?*VLjubLr5hxS9}CQqxs*b7Rw3
zk(o;@9zhw-@jmOD<)FN3L2az6u|_oI%{tKwn<5MCx5#`L24k=_$U8z&4c;n7Dg#K&
zp{=eqQq{7cUF?~DQ`Z@~HbC>A;TmTzwoKw|M$a%jL1|~IqWSHOR$t-SABcw6v_#74
z8{0dmwPA@VdM5Q+p4L=d-HxJDqsm28OJD_t=b3cVYp-iZt>SaAa4%i`!e-{&fL>cO
z)+7(|qPnV9jpBK=jH{-Wnp*0>HPP@L!>ejLs%vX&XfX{(ZIIFsX|BVnP|ZU_ZR3Kr
zx=^UXi?9p}7zc7ycRcd79;$T5E8x|^i)@wvYXihMuriymXj)v~Ru`!{Oll8a;nN+s
z^jS@9YrPQGYJ|RRcLomFs^MMYuLJ8=N}sb>;0%8m4$di6jY4wk;wm_HZQJ5n##l}L
z{P|!J{%>+ncx#ZcXU4VLa+h@*0Lv{uGtAg8(61Q8SxFFb(*?@%Y9A&sCC{isQx-?`
z1>M=1yqRJ{)0;rndq^j`WC*)<F_DnaFi*{=y@)SKIJ&Q4$(fOM1cTRmw$gSf;ikQw
zktL!cyuLHVWTXn@=<yRz+AXTD8Y<HQ10z82rkcU|EgB;=?M8sjynLW|8d~4L^xkv=
z3X?^!sTJL~i32|mmG5++s=BGYp~l6M%kob5+jWQp9)O-Su4i(xgk0}6pD)!L;dQ}$
z9v<Cxf%`$iX#lh>-t<nK!TwD(bf^*3G_S3yzA>`6sRc{#0y~u8s8#J9KwH(agr<=t
zlirfYyCffyK3JUL*a7m0U~_J2Yibt9bj^#fACFxwu0!M*gbO@6!f+FKLSs!;3(Udj
zL+>G<Z8sB$7BfCP;pM|&)Sa<oUH+QUj+$#*kT45<ATxa`l?)bY*t;#At+>8cH6$OM
z)Z9|LsJ^MaRn2gz-PMFjP?r3esRlU~`1wYymE<D1R4bo*;QTryJ@Xehmp7BPgb@nK
zupx#|HO8Q<q8n@L7u3O_P;m`H=WDL(HsCPuBb}PTb1kY#drNHwA3?z9)mB%DUD)<{
zWRJ@7Z{_pJK8R0OHwq9Q45u14<td78Pev9XNL$gI%wM?#9usd@?WgsD0uY$Uz-mk%
z8vk^U?P`q@mk!bCk#_E~!2p%+^vLX{cDNpjFN3JHxwg7~{t{5TIH=m4;Y~_H(Rqy!
z5gif4)}h(gVblBUYk*E}+w1At)mE+<rNENdnQo_u@QoH8=pE#XVfvuQxAe|j(kwym
z$}7oTW1=BRM)0WVL410tS%D0sx59Avq()AImVw0@aXoCof%LZ6Lnk!|@%4Z~GYqd`
z(T0XVd!Q&N+s-)7gx8~twHOsA&}1+xU8}05MszZb4mW@V#Ic4p+G=f83v->CTGaL$
zlnL8E@0woXKac};l5&iqVPi2@fh%`X6adSelnj{Z`SYwG|HewK;cEeW&jnlP=~wV&
z+IvV;*=`w#B{QD1IEOYjNKi@buo9S&<j<x8eAFHH-h?P3ZNDjk5xTL}@a07m+tlI?
z_dq^xWY?NO>7r-Ah*6#ZzcVv_Y5t5;OX`ggqq0j8WZ0TW4-d>g7Bn!B8vJPp`f@Xd
z2+g*@s<EaqqxSgfWbs(R9_X@wwrgjFBz~84?%Yylhrx)H0%r9$!;9)|)mNCsPG14A
z4I8R1xWZ^?N0ed|(*$c!2@BhqS}_V*+vm@(udau@A$2dea5R+f4k|&?g!zbDPGeK!
zS&g*|0B=#PL9s_Aeycp)(l5rgFRr15QZ*PGLH_63woL}RLgG!~hfNg$JvH@J3!02@
zZ)`&a0zC`snfx~{2=qT3z_3JUzz53WS8b!Cr7wabU#fVTLqq*S=3=&_skMt`%{gpQ
ztyavK&OKCNzDyXP(u!3Sv^V<Tv94_lXM?Z>C<U!`LH)L^4!dllVSx=z7{A1UG!Gc#
zwclFVY-Gz>u8}QB+=!VPrV0LZ*3BsX;R$M&k9ag-7TP0=>of&%Z4I#6!Qhuk8#VGO
z(&R4MxN9e#l56_lg2^+lnK}K6QbW96ab*=An+S~i!Tja0YFcQjhBsqaI<t%j8qOF5
zHCZ&qFLxmco>Pl_5T`P-;tSBZ=JqZFFgdDa2&-zP0NRt`LsgL75>%*y8DqT3Rl(#j
z?o4f%aTOGk)>7FTSxh6)(^WlgJNnC5csvC}6YGFk&?XBF93Mve5r;N<4vlQl(h3Gx
zjIs^>AW_y5Z@?Hm6|!qJtC`+F8!*8Pa>wE1n<utp=B;UIGAme-c@0g~W`dGopsE)|
z)6?V!OnI!%W9AAF=>phFV?YP=*||>-?^2GdsX>p}+Jb6-ARkO3qf$i*n4e&ELf0P#
zt}#Kl$>%G)@0M+JYfb&a*0D|3HDg(ix#0pgY?$Xhw0jRx|BLt3KF@pVxQ6lLeqR!J
z#w6gsz3=Pmmt9_A5QrSEHO9R6*<R?4n8}`{aOgTNt0IF}aVw@@R#7^AX1sI;ABQaD
znzfV9`MyHZ5RI)w5?y0jrt5jN4NZ&3l5$`=!{LUc5#0_<H>5nwu#0Nnr93~4%JbwU
z#H5uGZ`>8skF6amZILxVBsBC}W@estc#KgE_Rq`j>PZw+z(Rk$3_xRi!S3s4hs)<_
z0e)ifi$?JaSi&K+*H*v4p>X)+wbm}Iw~~h8>tz_4UJA6e$t%sE@8U!4?ov}56CAG?
zjYly}GZl8Zc4jb>YMAetK|7EH3_?lYT3fr&v&<fn5bT1c<TN>~HSRn}s<yR`?n1<1
z=3re`vco80Fm(&^u=(iT0d^L8(N$n6%-C)G_8vCBwfa~oBJ)a(pDi?ezKk)zs6N!|
z>nU#5;q$rdbYU&}mR<L#lDc86^`22E(p-i0*hsVhEW?%uRr^K`nN2L+3$uRTdMydA
z?*)-BRnL{2=-SN<RScR)Th%<dN5~{@RD8OkrgmQYf(0mqZh`*?<(U!=mmk)0X2c9+
zEi1Uo31UOdnzGp5Ib43Bw|${ogk6k~aB>$r>_je5l%e0lm4|O=om(1QC9neG;z!+|
zfz0xk_mR&Z%1>}|(C>4GrWf^JWGdDwm%NF!r$-9vYdTnnA6;{%yP{xTT#fda(Gy8H
z!Tl8x4*YqHa&*zy&(!j9*#Aj%%zF|z(=Hnu8I6x@&2aMO6I!k3LnMx-L2$jE)t)l5
zR--o)MHtsQmHksx<J3yM2v!`FyuVL-51H_=Q8quT_7lgB7l0~x^!Tyo$WO*pj0l-=
zS=r}GONcO0qbJyu7VbmlV;uf(@{Lxf_@H=?$AqTYC4NS)FHFy~DUb2lKD}S>%@D0<
zW3&z8&E7&eU#@_^4W^r+Q6UB-J!5z}ssLTds$6?BEN@Vp)XdPlY8LN8^HfWymBpD*
zTUC1q=BA~$5Z2VmA!dJ*L!&3w;3hhO|12q<L#y?(WQaG##G297k&r=ogp?Ty<XcTd
zhU<dHSu>`)L|vKlPJc1K0)D%WdZ&LS$2K>+&+B<#!*4miUBvGrEaX?oFNa@@UxeR2
zzJ<c?^l#^8-*IlJcRQl(|Gs}-k*|6g-$U2m_P4HI>OD5~zvNf4+&<g?XR=<^5YWu0
zFdcFo3~?OMpXqRswE4?)y6fw;Br+(=q0+}2Sz?uU*q|H((+3JOw!k5pnXl<tcm}6G
zl%OA+Zb-QUw7F1daQXq|2GYKdd{zF;UMG6$^HAa+z(?z?E7l>oD*SK&Up+K`^$1;|
zIy*q$L=DSrgYiCn48wm>=pm!QI{)EWFnO%clqm3j$%hR`j!sbt_=Uem=IbZRu;|Q_
z$~g>QhEWdBUx6gUHZ9#ArkvsV?VvI&3#|4>>lqjyjfvCahqfA*ExvoYnRsP}stv2G
zHmnlPI7OUs${>C?m@j6E_F3`bPlsZmhIyg(dHf$Gh=yi{E)PXJ_;*c%{%a3Soqi4K
zKxk^E3F6TyCIsmJdZ*v{K0J@#27cqXUOJDsW}Ykg74s|Lx03jkgdu)=d0x+N7r$-%
z?%?M(KjXh#=nXBy9^rz!&Rw!OI>k-5JnNJr&%gD!Q0VcjP$=iF1O4lHUbF2$f6j+O
zp$&v<M}<N=2seH>6xv6a%QmdM2M_e`BP=A`@v%^-lCY2?H#!J&*`d6KFpo>1HV`hy
z({G1DIB>y32l~eqGdCh^o)!w#DV&KLAmMtBuU)IKmVDe1x(_&V9%GMnnDU-H(0>Qv
zKEmyUu|FK>59RXxY3e21LAac7%`@az{0`C)I)pjLlaDY$SoiFK{tCjCgw2FG&(S`;
z6FP(&2y;)MUC)z_u!FFJFz-*apRj;%hrSc;C5#b9z*8k*Az>Y11z`ta9pOsC<%EU1
z$WQ3}jq<_gKEeXR!j}&87b|3MoUYi85$+|dBg{FG{DcLB+t}>YLFf>!A<SW)*akxF
z7u!J?Biu)r$Nt5RlgP)m>Rp7DZ1kE-zts_T5Oxsu5UwO#OSpz`2jM=B?%7DaIUEYI
zgRp>bTpr&ED+oI{$6_U69j91qBP=-TV1Mq(<RhF*xGm>k{~E#_gzNRrL3p8$@J^UV
zSjoXem4pQx*|nB1M!1o1Eoa<?P62LCd8;7YN7zGHz`1j4^-j2rQ0I!~o=SOy1%x>q
zj#{jcvo<RU9l~b94IC-DmT(6r(-uIVYdFrjhp>)AoO9Xly_Rqs;Wi!WOz05S={<6=
z{|>@^gc}G8PCwYcoiK-cI3mz@0b!x&oN%tfPk;}?ynNbASUCD%|6anCggFuFB}7#U
z?I0{BbO<X6SAG&a5w0a%Pq>Y68{vjCz{Bah6Xp@_BP=HDC?Fr<2Eyfpbz{g!82c1w
z$Pn%$T+X=5`!wYe77(r{+()>LFz3vJ{k!yj*1`ULdLIiOK0*4k!4F{pVX@x1Z`{#4
zVJM&WOgY%!K^P<KAzVYamT)cMM#3G0y9gb^eT0=Ilsg*u2nz@+WAumK30D$!TmoE#
zD+#yheH!(0Y|2W)N`=$GH{lvWhj7O{>gTj8hp>mRunxR(4or+NLb!pjkZ>De1z}DD
zcp%IpT%-49;L|(dK80<-!+h)iVeehwtEjHN|Cu=#PDpYbG0})o!$k-P5+Gub03it^
zT*RP&QPH9zMx_=NF}7$?6j4#pBBIhtt0G!ku_DHnDpkbvDOTD?5v#3fOD(nfh&P(H
zkn?`mo;Bw(IiP*|KJD}V|IX)=vu6L++H0@9_uAK)Jt>7QffhBB&;Ep=MbKSK$RD&F
zx(Ql;8~XVgXCt%$x&%5Gx)Qnsx)!<(+6vtb-2sgaz|PyrKeQfN1#N=Pgf>H)p*x@}
zp}U~#gujgTCVc2_=t`(N5dF|nXzULB6j}tG30(<YB7Ep-Xe)FRbO&^Ygug+&;|N2m
zplg@o-_RE5LTK|{qzhdFZH4ZD?t<=uT7$^{3i1t&-A%rsMJq`US_)kat%tTi8=>2v
z&Cp%YB~Yu3^q@u17HAoC2ecl#3px{8bPweKErrUN)^_MRXzX6>f$o6rg6@J^gTe1Z
zAG8Qs25p4Sgf>B&p=+UQp)Jr&(3RgJ|3i>lO}fw~=uGG?Xfw1Ox(wR<ZQ?<fKsQ0_
zzeD*!8=<?QMGsJ)L$MQD0&Rrug06*H<)rrz`k}Ek=$G)r=!3RGHwnHLeb9R7Zs?9j
zsE^}G_q&uQv=!RO=M)+r!!GDr=rU+KbS<>y`^1B`LSrYO?{Vya)<Y|xJJz8G+WZ6R
z0on@P4Q+gqdZV2;Lz|#WpsVrE7HEt3E3`uLwH~{n70_mAJ#-ng1=<2_g>HlHfUd>f
z#-|8Fo1nWSA5cr|g_c5N8^|BD2wGZ!+%xEfw*Q!T(8gzxA5K1>qaLA6o3IaB@e}GF
z+74}l?s%U1K9Tx@mO&d|z)om0bRo1I+9Ki2v}@>EXvqlj^CEK4Cg>7q>?P8L7D0Cj
zA8J*S9<&Tv53Pr;Z6zMG1=<YV1zibk{3+=`o1ojEty@VCTJ)d9XFk*PD)FJs(3#Mc
z(3Q{~&~?z*Yxpy?6xs%@hqgnTpzbK@1zHMifmT6xK^q0%PCY<lZx9dK1hqz^7g`Ff
z{}uHMT?=i3wm?@xcfE-`wDphJRfXJc>K)qnA$CC9|3W#9LB1V6bjLp8K^r^BC$tH=
zP597V!iQSb=uO}+(8f?lq6yjpT?pO5`g|L7E$e!>hOn&np`|&@Q=!dWI}*#FOQ35d
zoXh+ax{@_~tQI?=Wzgn>I}**%iUQ`a&?3&S&m4<AM<5SvhPFXh9*O)o(mNV`&?P-d
zrw-bOFtn%?JL-uCErafW)<f(2VJ~!Lf9A;&J{En@_5sL^hYn<JE%Z3%+0arxu(=u<
z8_XOW+B}3gx!~p4J%RL(XMY0P%GV<5p)DiPD|ij_?up3NGG~Xj)KgwUC!+`2G==h+
zM3_$%H$#_91DEhw#5;-nU)YgofHuz|edvx$ncG8G&SS1Wnf%SCJfN-3=sy{KS5Z#T
zcIYx_(O0np+PDCH4cPy6(uY=D-;vk^ZMuQ<r=V{!^3c{B*<*pW+=QM}u>ThHLs#BP
zxtxlg<<u{<=swb)ie1pP(3Q{~&=%-!!B-(a4g8ywAG8&^1lkU5g*JZ+d!}R0{gfAU
zC3FdN7qkW14&4TATunJco1xZeln?8kGHCN7)Su8t$)DiRR%pdz$U}>sKyM@Qp>v_T
z)?*iR<x|wt>EIhES7`H(NDo@{W9sn?^7}0NPSDs3)Q5z(P%hAlm(VZx%gCKcxj{>y
z&96|N(6zrnkA!~@e+K&gi2p#_|4jJ^{eW_X)+ey*EXwJiL}H~-H<8!|ZQ`q`C1=Aw
zB$23swjT-~+T1;nh<york4hxwLR*hcBz8cX1|<@uGf8)7BC!eDI3kg7&%r;&CK5BD
zt*0as>!3|%BofxS;zx-@1GMPML}E3x<?HA@kNArci6zkX#fd~Kw0T7$Q8WvGy*H6)
zg0_4ok!XSLSc84%gRezDwEYp}p=-ZKy0gK*&zJF_OQ6lrwa}H&U5_Uc?a-F>q<;Z+
zwxAE%{2cL`;BP`6+W34T(FSdO0X-MO--7+n(w}0_MWp|;L}DRSt^$u(_E%4_>}J<K
zBCktKKB(RsyqLd+M_J1p#7*2C7cZJ{Q0|qnh1TfqBl-{R-BY59j9m6t8zvbIUGBT#
zmJ;g-Uk}aC=xW)xH&GX|tniPh*y^}@yHg!6x-C>4FS#{b9WPxPsg0K{j*f{}Eb3Al
zUuQe7b&Xfl#LLFSOA$eoDAn=UxLlfpHIKiR=k_LU45RO{{pcGb`f5a9U3`-ry2Q?^
zuqGE>at+{bD}TFR*qgxRt?*r*-lfi%c+uj}n0U#e@SLvkqA|vfF}WgNNja5l-kYd}
zTD9>Kr=e>+R-1bXzY}uL=C?Z6xdJkgM9LAFgIv{%dlT!qw@|cLbT0~xi<f=X(>z9^
zJ5!Q@vAI?)q7M?Um3W&a9tDwZlNY%JMV@8#xz4n%@l?1@Gbr}Hhn|KldlOHI9_+me
z+iHxt)%rU%9uppy)N8TBNkQai2_X3!V=^;Agd{(e@XKD>n|Mw50lOx|ciYa7(=8ce
zEGa;A9`SY%4|hmjV&5e2ZQvJkAMQs7kt(CQc*%tnOKsexj3^dk-$vwJ_DZ^m7GxJW
zlals}eQzoMQst-0`yzCWGrH<>FEK@jT9K0Ji}fn;8qvFe`|vY#Xyoly@z<r|2aCfs
z@zO<+iSaV(wiJM}_q<bb*qb<0$=FKb58~d-L%r1V#CXZ{cu|c>)}&L3+-l^mk#sKX
zoKCH#Gk-tnY$Sdg@rNbTsg-mnt66?g8UNXdoa`wr7e5WuQ(e5wd5~(V@f~Plu5)qc
zYBT-BfH3~U-cc7xPcNy@T5#EG>gt0~!DoP1Wx?lxFL_GqP2-6CQt%SwDgNXo_=Dg@
zS@4bEZWjDi@EABGc}cy#Cjj^wNx;<UsZytS_TsQ9Yx<zjP>0g4NvqXb<F}KDG9bcv
z|E<J_O8ndLxBc{Cb@7H!NhE8RGtTIfd@VrFHva3TsPiwu<?Fny;4BSHySoJ6l=L41
z-wocLL%^gT=tE7PSL^k8tDR6;3<JOdooKZfAnEKy-|9Czr(Y1I{ML7_p8?=yAMimi
z;`*29sRb_uKg0*2g3kbNKz}+e{mMM>M(}LyR`R)A`SLMFFP~$xl(WfaZG2|vAUj(w
zi7J<*w-bH!Z|zMSE`aGLPD}O^!Vlxn2jchs41TThO+M+O7CDoX`WK*omhxXp=Ev0A
z*yPw}>P^SKg|_pjtkqVVORrUdiw3H5e6BNh{{=9M^mmf}O7=+46-PU;-^kb-dV?mA
zHK9;-_ED6MPx3j5E_C(p_+Zw*A)n%JTS<TS?>qbRPVik>@V($|S?~hd*N!as0C4w-
z%yu!Abi?2+$e)G2@H3|#po~o&PlzuJg_cR@(77`G;oA7^D)jE=e>zGX%J}<bhF@Qx
z9B3o_s`vILekpwFb*V%DuXB|N@ol#Abmxw|646hHw~Khc)p)l$v@h)kajr#?3Gp`D
zc`nPSgBiprr=wrVht!_c__xW}cxwoMUMl_QVwuY=iq?sem0gX$GD;$XiOk`=d4RO@
z1?X}AYj2`e_2`_?w2QIHIiZ=und!tmS=)uDPs(E}`j(;Z=c=z$c|6&<Jj8FpbcoV_
zF-#^ev9B1s1pN0&e`XxNP4anb*w~k9w{`J#PUr+_9S7)d=b&#B&l>WRsd)2^>%m*V
zFV%FkKA0~o4x9QoNhVXj3e-nEv1oJrZRW21!@Y?Yl6D6A7wThiRL6y7j`MC-Gre+@
z@+in-&A?{@P5&&3aEadx0AC0`Gt*8peyZPK=Qjd&rs{hR@mKTg<lIa<Q)__q@?33C
z&T6JM0I_Ey`Zn=t&0mOtIM;2?1o1=kEe%f=KV*)!DB2LOa&C{8F=<QpLdoY|;?Lw$
zf&(Nzxj2tryO`SoDySL!KNNSaqv1`+y-GTD<NxqW`R3~7;xMK^HGYvTAE7DqrNJIs
zW&o1@0`$z}Q?e{UCH)5HjCf4&<=|_<ja|ay68tgnP2e2{@qPr~Eb=^K`b`>M&iz~9
zbNQ(C%Yvuo(s;wA%zwR!=-6E6++<)vt|fg<L7ee_ALF*plhWIzS*xY$sWv{>`8D;F
zZK>vL&su>gKU+XLZQsrGGf7{bldk0}@0&i`Pio`K?9hnp?QTpiE1@jy(HJ1<>?Iw`
z;@=f~&UDP^*SUQkFdeC%SxlIU0{Ij_TL9i*?@LHsdGnIXlJf?Mw+jAT_`&(pdhllO
zZ1X44^{VnsyD;@`)&<nNb4RkSS@NYXd?dX*m;TP#mpF!d{}NpO-*^RhFCT<X;;$0C
z0o-5zrLU39d|DP4_XeiZEaB5uO|^c8C7q?{YeQdpetUyI2)+yaSnfSP_SZS5C)YV5
zw-veF$bBMmwCAhMc<C+2<yR)YBKHY$t)YG1IvhD#18a0oB3-WdVA^MRU*a(F59FL$
z1nX5$(%=%iYmsYwB-4)tp8;MK*_Wsh!Kv{Qna2x04_u!0GF&Au@rR}0bHRrQYUZhP
z{o?W3Km29zWg6#S(hqJ1UjlxIfEo483SZm#1TkCo2w3RW=Td&U4i$UCIMOcSA1d+n
z61*5({>PTw`&T(P<>2k$y97+7$GX$`VKTvSxh$?=)_7SE9dn4cE}Ch-)W`MUYrzjx
zzY_m3jX$4z|C0Ed!Hc@^A9w{c_OO~Y>)`YJESvqE_mC??Zmr5Ka>V2dx*8+L(H^95
zgvTZMEXEPnA^)ZTW`1S%f><0b3V+2bP5gIUu5-6ZhPjQ`E~XN%F}5$UM&oI_z=L$}
zppGGlMRvyK!Pwksnd?r8?D^M&=qq}-vwv-rhQc$+ql~2YBjxlexIEKjnwPu;e-C^v
zb`>U3Pk$I@Gk8$G7+mCk5|A$kFU{GPVE9a4qJI)UCE#Yhq#VI#@zdV5v-|?^-Qe<%
zar{f<m-DkLH&gyW@W;U0z;{c(ZN}ZnG9R0gWj+>qhA|;y+@+?*<c40%4*w`Syf8a_
zeQ-=Sy1g8T{ZlF6((ak|&jX(WULtsM9X3VmpPXC=j!n+fLU&|Ox+rVX&hBiPH3o@2
zJ4v_Ys7!kV-wVDDe5{f5egrR|BiIF=?hn#G4FGQgmo`U#=$=k@=x?A(yh-qP2jU4n
zOZCl`czOxG0KAHKL(<c!1z!$c0sdbyzhay2R%e>5PsS(b!_JM~L>?b7MenRm56kwT
z=`Y_xPg~E-e2eX$fbRgeWqvbO_F-p8f8pHb_XHZh_%P;My))y_gI5mT0{(t-{%8C!
zP>*J<7plx&k5$>>SXP)Mz5btc%St=jvl+Y;+$>;H?INo^!?LCjs?QD=WrclvDvR)+
zV>9hp3Uey>TJRGYXVcng%AVBxF?4R$6heEm!p_EQejdtWEgfv@<HZTM#LsqOPveM8
zJEb1?f;WIyXK9b)Wc`_D=g_PMg$878kD=aKrG0zmu@O~Tm1&RIxfHwvJlIYi1aAgU
zZzr-g*a$use6aZ8+2Y6SN8-mXc71^%ZHo*|p%ObDdtr-L4(}nquqHEIX>Z}~=m-C2
z+uJ1cl+<PFmvWv3UIhNH_RqIScQx-qrH@l$&nMt3!T;6mcg_*)cTCQ-N7~Ev;4$!L
zlI<lhE~NS4X<0ohv>@EM2{7Dc_ruWCtX_*Ho}U!Z*;kyMX{VIi0Pr&KG0FC_|9)>&
zRy&=yvYF{@&W0LuO#6M1bYtgb+9Tu9M)?8v$0L?|$^9{DPjA6@;YU4H=2!4fR9|{~
z5<Krn#)}*GC3*(rdxN)wr^`#ymEhaX-ItJiZ@$g~UgL_s8OjgFD+g~S-gIQbPmgBv
zS@w+C-KD`wyyfsEzrlEqflGdw#%g-0=UozStMY^Kc7nH)-f78r45(_T#0xWltO%qh
zW%?HJ%D^ubnIn8Vohq{m5?x#pzn1t*kV|i8QYJIN7lQB4-UZ4J+PfUQnRxrNcRl<T
z`04S)-lgE{z>U3Ej?!}NZ0{$?#b$N3H}7bafbY-V0m@HLcar2Eyq$FaD{~(qwNGgF
z-|D<)+cQI(d0x?ZzXUyUnfgcH%JVzt?=kRY;LnSoS?5gDb&m6xztEBRuOe45n|Za!
z=`{fQ9(Wn}GlH4*4Dofp*m*d$h7q>tDK26D6VM~|S`OX<K9PIR@161K{xlW-F8JyG
zP>$#v@HX&}l#{XFJpYzO*cr*iDbF95`I$jX?0ArPEf;jww-J0Dcz$Pnvy%G8<U02T
z^nF6SB~6*-E9vC*z+b^l{S%f;@ZR9r<SSL4X+-i8`KkMnpQG|jKa-cpU%wyuRr`@&
zzaROnDj)2>c7m@A<X7r(FZeR>SHzk+cG8@e{A%>-A4OFcW|q6DfA9+MpC#kHkP&Yx
z{9S=~lFvEdZQ#N3y*`V)`0FZ_KT*;#{+fEe@s&(>HTG^res>^U(X&(ae_lNWy=cz?
zJ+sg=09^FH>3H_sp7st);CX=Am;5qgV3wB7tgIJZKz-(%)$AV7=1{$ybeiVuOFU^4
zkbS3@Qv;in<9hh9OET+4@U7q$_}3s7&w@(kd;7^dMPC;Az2Gym$QRI=wq}tZ0KN&_
z^dHX6W<yKTtp#uTN@sg!fX@UE%FhFrcgNHF2hp<>+yXxa;$H&I;%CW1-T~!4{N0N5
ze%;$l1~>UfZWD6p{j=aZ!CS!7=K-R3BR}iFFA%+7O6sklw9Oh4*lYh7)Sg9eCHU?v
z_*8Hyk1?V^uUhB;@YUd5@SpJCPcS~M&(6#<3!b4YmNmzsXy`dyC*ygAd1#Lg$<KPx
zf9bx&WYsN1^e+|t;NQb<!rwoj{%eBz)7S9@eW;(yJJ-(ua0|Rr64XoVsRb_uH|<OD
zNzfVK4d9E!kIlN&JjZ7pug}kTK9hV$t08ovG#9c@F;E<V=Y^8LIp|w9k9Te*e{I9G
z{8;novq$sH=Oj~prv~%qJBqZgPtddL3jX~x_p*<%$hp!~IeQ6afB03&&)4@Qu7Dk0
zOgd(M=1fG$S7Yj-vM=KWazQ_y3f>H!?uWRVH2{1r_yW;;Vp8wqpua6gt~HC^$G|rQ
z^h$m<gEx_%rQCbt@kz#1XMwhV_&eaI`=iv`UT}AQXFqxsydL~1Dd%q;e;_}V*XlC-
zG~JJ8lw$9doy%t)cnf&YkCuYBfd~EQLGW1f0sP3!uO@o$y)=Y|f0iGuM_>I_>POqk
z)BWh=<TLEa#*a=3=FfKkDfh~Lw70Kj_aouYfWHm?{_14`{F!%T?(0gsnF_w^>V4k(
zlA}EP(#M*A^nJ+(MIZf?d0%qSe)O3&=0Enn<X-fa+~CdA!$qFn^mh}3?@P|<Pk(^?
z=Y3yt74mZz>vNjaeDO2imwc6Y?Zn$mS$gvZvv1Gi-$mhn<bBEFV`<;4`<@a#45tTr
zUy|HglaP0pq@IVRm&JkJhrFJ6EyN2+I(iL&&H!HrelhpqtwREH=Faa!Zbg1K^4Zoe
zV&8kpm-SKF{F@><@cWQEiC1xx_FFTsgT6fdS>I>94>^m>G!uVNGTrR&Lry|&A#&Fs
z7tZUP&S$+3xs~{vh)>U|`SYI>&PqNflyT$}<aQwU&%O^?Igs_#EsW=qo?Zpeso<sH
z>Cg8Bp95Z!1-~A=`PR&SK;&0}Tgc0F)4v2?58i(BzQi;cSM?HnD|pq?%;#&;5A6i6
z0B3rj<Ac2E;9bHGA4h)(|3ZOiXNw%?0$)J%_J+SQpjXDTO7LaiXNcp{K0?RQTgvk=
zw&Quo2<SaW<Len#>+L*ehys{-ylIcT-?AuN7vJrK`b0aum}Bx!ytcdcB~A|Hb3J&`
z-JSEf72E}v$70^No_Y^Y^t}haY-#6m4&#_|CJ&EGULs!%UV=V`!{jA+Id}zlNfPzu
zWs|_Gz}e1AULrpWydJzTiF)!2z#G6>mM1TfUk=^~&M=j{1b+;CE;!w8@+#+UGx!ql
zZwi_!U(?^2x3HFUDqi#aelPKM6YtW@^-OBtBDL>0KE5vGynTTAnfTG9GRo^-{?DD*
zfj`W}4Qg}GG_EB6G6()l_!lVO+pjwlsj<1|s>6xga^%*2hMbhgM&x!Qcc$pmOYm30
zD>$p@`?#<Ke-FIj=DmrN1z_IijKe-|!#p^avyc76&nUm!oReivIYH(E<7G}hUgiRI
z@r91OQ77}$^f!sTrNShlPC}2hivC|*A3dGM>{NO3dY<F7Wi3x{rZ4ucLf`J2GW~Tb
z_&o6Dhgs*5!SG%>Gn1B?Tc_&I%!%q{PBbOn)-_Z>6U(v|^_LZ9-YZ@1>c><2DT|zm
zaug$x{#;zr|Ah3Ha5i!^?K%9cpMKz-vs7N|;;Un!XV{?5lD_{sqe*`$>2KSxFYyFv
zc>PelY)Y(2PhZM;J#tM?^S(laKq~Lq-oKh4@8!L~c6XMvdE3cbZ-<BCFB|tI<`eeP
zofx0#G?`?j9(u!H2|p}+y#%iWUj|<7gHXYzg13SnoQ9WkKL>mpc#im4YCa=x$-J2M
z-bBWGIh3yTAo136#&fpxv#EIUiYKqQ=t~{CVwLn?MSj;N`ep9@OUms%@G{PT9x8fL
z<z{>)@LpFP?^WL_^XSeSm0rhQiHb_e)aHGOt=evGr9ox9M_S8!4nGaN=b+<)*tvlC
z%U;~q={>jQ;A_FhBI93T|6|~7;AZ}4%E659%wL=t$?_YU%L^Sv{>yFY_YQkT`qqnA
zPkTk*mdy2u^kW4qd3J#BlEl67@Ojfo5tMwF!(aOn?*)lNP>$(yp?dGkMpdX1r^|AV
zK>D|N#9#8W1HbPg{n9GrmLd00vR|T&>HCdluYfkbi1#|HPVL+)d8Z5*d(g8JJ?&eW
z$7bp01NF}Pe4z@dcM3JI4(DVM6Z-u*!~)Wp#Kf!Nm3@g{Yq{Jm?-lDAuhjdz=DE1M
z?Z~sBtjqoB`$>ftn6k*LKS@5flK#5?+?N=aC7-GE^*z{z&@0*fdyKp(l_eo><ySyD
z<!r62d@XZ+C;mAJd>ME_GJSJCB-JOU_la{um+mipRGBkwtI#*|b#K4O^Rv`@+WLN7
zl|v7Z;by$hIX(c@<vOzit<aoO%ZR5V!E(|rU@NSB$G*fH$$X^7mektQFGuI^S?h~~
zY-6%A*x+Z!Ba0s~am@b4s8q4k?_=0i_2$0BN6G#xHRc5BmvbS`?OCgFY%T{zvU1E=
zB<U0kr~UtiGw#~%4ls^O4|i?W+?akS%Nzcaym_(oGYd$^YGZy2@%ou@Z1yrS;hj10
zvsLiR;9nwqz38&6_25gu{rS3{Ym;&0<m7lTF6|{h;>bAi9`QCwdOH3r;;~H$FS5D3
z_D12)^Im`P-kcEmVm4yhe!DO6IQQBQo<%=col9@a#7;Ju{OukypUgvk=I=P0kN)uQ
zdg~Zt+CXY-X4`vlbdt<V#xcgoYZX}vsWz9`)++RE`u)Dd9!-C-bCP+#;TF$O(;%gs
zHY2xk=f1?JBBz(w{}%YdKhV!=eizI62;Pra>cz)HZ_ju?F>eI&@9gu=Q>5wX4PO7p
zeTj2KkIWlSA?Fivo%11hvv<-{U)7X)or?UDUHcN>PL{Jd8eo=0V(&coMej1-5WZfL
z-=&iNpP4sEyJP<B+)M(TA*Jz>oF^#$xgNRZ_xB}sCg(rnljjmr?Nj^N)plrC_W2L*
z2mC&JOQ2+>KP#5O?qlAQNT!o&Z-H^coN4JHXIkjv0t1H`iX`82(6^dzBV5kCe@Qym
zgU9~r?N5305|d8KXH37T{S7A!=<>28lbVOWioSZjy<lFHHS?To`b_ySskcKXODS}o
z=$M0lbFi=XNcuykBk_Ih4{wukO~+ySVcoNrt{}@mT$3BR2mQfSLtXAvz`&sdDp|^7
zIq6kIINPr2b(&Yn`=fVt>f}S?NGIc+QtDBvaHr_wdykvN-%|4xJWS6$j+gT#ErG~h
zdDcq)d6eE7>qzvH1r_nr&-<8p2=ycDqun8yAp~>kd9LJZ4tmzP9f@nimjm?@cz(vy
z$WZ7ESu=F@Co|%UzK!T>JE9|TLUKGvjZ>ZV*`eQLuV>Sbh`zkh%+HVNNK8xmyU`c$
z!&JL^Dztz57NBni`r3MR1m|4=ed^aULw&@&&e=6XbrziY7#&a+MzUXgOY-xz%=M%A
z!+P)--}I3FCp`J+be~{hyI9UPEsC)J9qQlNp`@JTE1&g!I}#7zpW$P@_~~=K)Vp&g
zZ{s)%+f}-t&UvfO75}QOVm{3`L{@MgZa+$W%skg&KwK(kpcY4XH+E6fsq>ta_Kg1|
z50epZ0Dnu-WAP1=#gOn%(UV#$@s7a)hDKiYWULd<E=FcA@-va|#=SSbrQWBJe7>dp
za>?hNo?V^h0%o19b;yCYC$i>qT(0!nbH*?~=9?(r)^^=V7X_3&r!_a!Py7M(ow`$p
z{hTcQm8tisq_doKR`RWtp7?P%aiq4jY;D8XEvNlLJ+eEWt}4Cx*MMsFpN4iMj?sQ3
zyZzd)HaKIkk~vAb`Mf5=fXS32OTN5`k(ASN(y8EEF#pr(>?IwGZ_WHqr^7cctya?c
z!~V+6_>lH1bLGTLmg1qg-`|-rHXiflYf^4oNvDBt=KN2mQ%<AZQNg!iK3{&A#bnP<
zD-2wIg8#gi)=880#*0<tXBFwpJh3D3r0yHv?wpdG=LGuq&|Ua%hO3#*!Stz}VdvRw
z!p=+C(9j`tU7b~keU)RGuT}l;v`^++Z(-l=D>D0k>GwVXUox&EF@t-Z*C$_Glzy~e
z9R3QQ9?$e0dXCndd*RJgdQ}QncwCZBE%GhMA8HWqrxG#)d=vQ70-1RO&wV<b6Lwal
z?Wf8kcymeoRm88V>+sHB?LWRAIQ7N^6qr8C<e&H@d?TvVQ)zyC^ZN;S1^BUor1p^*
zBU0zdBz|!n<LA80@+;T)rNp1defYT|EhF<xgSGymuv71?!qUelT-h3czPZG^UgF*7
z#nZVY@iNA_3Hux8q+I5qN4|Y^o#?s1)1zmGQ{(rPc$sVs0AN^Zu$1~;PyE>U4)gv<
zcm(k?#z}VN)5b|Dhu-x(&q6-<JgXAC1pE|a{7dAgf;WLPGe};N?i}#B;8Dh<v^2Bs
zW6Jou3i$@U?e)jZ`Q85JPv&*2&f}%G#VqMRhQ8fXIud{9K786?={Cqai0ns+Z!<w)
zB$a#=kH>#c$;@{#csY0r`1hs%BgP^p^mV!)v%Z@FG!=gR)Q-d(VFJc)@y+yoJ9931
za=a?EQ~H*`B7g<ojb4wF{*=KC?x++0U620O#*PC%=Ujl?TgbH`cWtu#vaN&696;Vz
zlVxp|Ia*DwvlB+=4Z*Rwld?uIJ4sUBGbT{pXLfw9{R5F-iu~Le9f==tFXgt>p})}g
zQ>j<yp|sD`%tB-%^43}SnfMd(>2uJ)`jq{~(4pDqQ)6>^0+nSt<!vmAANHO|eV@~j
zxJ$>oPTxxq<6ft+rGGYt_fA)3B53mFsq@emJCARnsXl#Afb|%C2aQ&|kbcZKuC(_D
zk#CySkvMyQ{=^{4J7YJq9LabxB?`okcM^X|Q%7RC_<>&K(7oU;-@?0DFjL;_k1Uq&
z;a~?3@3g0{$)yLuq&n%F#h!8srHObiXt|s9pne;P+1?_)|4`*D4|G0eHSO6mkNEAE
zb|g-fboHu*E(Nc;ti!w?XPuV(J{F}L`WAIpRKu~BF&t|#JG4b(k|DlYB}ZJ$CH3_x
zKe2hte;oqGFO1($O1^i7#^krgSpWaU&Q|{^eZ5bT_r>^zo;=V==`;Jf0Y8)1JVIB+
zvbGD;gvGvj=xe&MBXLi%T?Y2Z%sN2l(dfH8l&xK+?AwgKmYexzp!!GldAWHOSjRJ?
z&<)wwtx5X|Cewf2&Nl*+`VO!TUT24n&#o`IPM?Lo_B%TgV-LzGKl2`7s{C{fwap3T
zWncH3_8{fAUi5vtBQeL#&=(jNO!=`d6%I|z%UT~spR|K88?Z$`=tz9Xz0_}d`!@UR
zrhR9QrD8`V@!E*@Kyn^z_Tf@?r2AQe^Yg4$c-@Ryk1j=D72j6eE_R$_-otp%f3tcj
z&|~l|zPb3Q`c0>IM@er{^rUzd4={qGN`3BWfD<x1h=yEi4l2V9yl>Fbk@#8`y~NP{
zEjFEGcc;^6l|^r3Zs<&zj|XJsJ9x85ZyxD2@vX?;B>g`%E=|llcgJ_MWRs)w=G@r*
zehrBPxFnsoNT-c&P<}aCKPf*BtRLig)HPWXs?MDSPRXXPW0~2VOlG*m4{E2-KRn0&
z#X$s8{e$s?KstPj&~))x##wKvy&M(G(O1E@Fe^X1{nhcj?h_f;=rRHZvZK#-WR{{E
zpBtJiQ92hYXW+p*kM#5mlFtE5^xA&ek$9|2=X?g*F*}M*=(X&dKFNM&0s5+b#XeM5
zUmxR7`Zpod`@v0)!;Wc|d>VDezOCq+yR#$lr#xR@%CC%lsrE=cIH3};uXD{N`<Y?}
zDEGsT#J$P<ru-+BU+rhe?^)UNYvw~zesj<_vz>3B#(nz^kYC;p$v(YG<#!|c+M<cX
z+xfmeli$F&r2Xn{$7#x1ekQx7z2wo!%q>VH?qQt&Oh2CM@GK}xb?|&`WA=AVjNvjb
zoku!rixUa?F5zdU(-68XyR%m3hK|ba`D~Tkmen>akon6eq+e8$NQ~F?_4$2ZKfF3H
zf$<)7Nj;WNV?ER>k+>|gy$8x&$M3dKXtR_@wg!qW(RV%i8iynj4}PBVV7x68KkM9j
zP5FwxSJBsUd?K+ZOFee-Tjm2#WPfgB^hrP3dphIo$VB4kWPcQBSLyAP=S}TWEuGsH
zPs}!EHC%>3sh6dsvu;end%uHzwA1+Jy_4P7&K$*v>6PPF;%}->Bz~6c?*n!cKRNH4
z8+ypjRx3uEH%~1-jrGf{%=LdQ{7*=yv^J4=i24fm$T2>gZmW4FWY&1~tnmg?ThceD
zuja<(mZPT<{mbeS-g(6jsVwrGu@)GYoP(RX5Yq<mH;;Hbh<66};g^ZW<UIKdxa2${
z@sKiYpL-|rvHC>TeeM~eqtWA!&~i(Ep9RAgMdW$Y12hL(NOCiQ52vOhY(&ZzR8k@U
zsOBH@;6Lc!Mf%U<KjFb%KC{nJ{W)96oLtW0<GPCQ#`lfroq19sad(z>6Yz7s&t`|L
z?CpjHE^l^cby&~-ousp}0sm(l2>+g+)EIBtycy%o*rNOC3qzsh+1r5E*-|Fbt`?k*
z{ih`o=kL#7)7#bD{k+e>Fn?uM>r?ybJ4vUh@qa#@NjT)()BoqwSx-96e2d$E9-Hn<
zf%#T#e6=md=d#o;166JI@k4f?rGARfq(7PQKcCL^q!as6BJoJ(dNunxg8aOgH9sfi
zhGu2YkIXEke|?X18qP_W=WOD`>EoWZ^D=pYo3W3PY;l&ANZ2E<V9meKjyxQ-Hrm~O
z7`1-mY{YxN7e1N$Ya@c+6hY*hkvPDnNPk9-$dTMWC7XVtpy57xfe*uxKgO)@MY{bh
zX6=ZaYgz9{Pv-U~T?GDV7es&EB@VDKhJu#Zk=%Y7^Axzb&>oR;tAC`<k}r3lwBWEv
zrBxD{ZS{+sW1RptL-9kP{URq@$49QTMw8B2iWflpN2XZAB3D^sBD1V7d3gUDoP}7o
zG8(zrwVsYH3;*1;ws)04Ti4rR{3U)l$i1#79jl0pwx;O)sgX0RIa%(9M#fubDn2yQ
zV4dsTA5;u&i2N#KJz(#o+Q~#*puUlNLssK!9@4ex?3+T?eIbor9+}GL!HjIIJaVRW
zrFXCKz7w)$`tdGGkN5xWDjW4{`)RuW2O>+meA~4)$fuQV%0HOfm-D%OFdo_9TCc~s
z{U9FM<yyBN#O;3^RF1$C?gUeo6~DB<L3w`?YK`2QZ~Y_(fqS}!xP7H-FWxQ6jr=ap
z`c$0m(Y!vOOGNK|@<W}u!{r^Zd!5MN9cznok--k{2XAoXrw-qK`_QpY{=nut<gYtk
zgu{=8j*fgQVoi?R6R|G5HvCDL?>&3W!v4@9kqdFjJ8ZmWfnUXihjnYP$|Bd=Rwdv>
zq_pmPjA$kOCC&-=hpkrUPVRSw;@saAM&@z(!N}qv3oPq7`-EG&S_%7G$ox3;vnYXY
zcJ=sfA@oR`YeM7#-#=a1Q|(L3m`=;NOy!P(_MsJ>61l>6L^mhW&nm>nd&8^s<l<6<
zy&^mv++%x4!I9AN$PHbrD{hFc>|%XGTDTP7!O*_6!@5)(bPtORBla|kP+EwQyX5pm
z)bZP)$&cq)KM0>k3yqw>$8leka%+o3KvzlkvNjsIJBKgKoxq2E-!u20M639hdtLGn
z{<upI;3wt(y={v{xP3P!(jQ5;a;I4QsQmm`ez5HLYHGlG$Bx_^r+82Mdp;i^l>3hy
zIm~)X_>M5p+8EE)+hQvs+}|ZW_Qmx@34BokUzETXCGbTFd{F{ll)(Rm64+hDe1pq9
z)T3oe8<fsfx=iUhrQ4M5R_dCM7;~OLX@k<$N;fHOQ)+3tB}&&R{lAc<FI4>htOPvI
z^)emWl`z*oQUw1@d{%ar{P|b<y<kp>&)GlyXVMp~|4jVPwBPKq?%(zx=N}XgM*o+G
z+dX$l`1D_Hz(1H^$v-InuNH69^89zxe|5e;Z+c&}kI!3D|2?_V|6a6zS*#t}KK|YG
zUzXwjW<o1}-rK}n_lid=UiRoNrTbI+zrFs0cKiK--(Lp#`wRcO{l`y#fA;-v#moM;
z8m4w_)BfS#P5+k{(|*nSqJBLG`Yiw1Zvp<B8DG@j|J(If@o)B@e>C}4?QebhkH-IZ
z$rtVL=0mMY8@GGmxvzV4e|G(|_2akCb}hI4rStE?KdaySzs2|c#n<PDefsap{}<{0
z?=As<uWrScJUjjHLxJ$)f$)ztEU$LU^|prP6=AtP)Nq~`v}Q&<&GPEDT)Per8+y3;
z<vrLJy?){d4KGpJU1`M@k6)}|f1UDaN8&32?N-D6w-R>nvWr|%k63F9yqm5X{&=Q$
z<DF{Y{b<E`@4~y<y!8D2)2hQfSk+kaio9G3^M7-_u#dY>cwyg8ca#_A?OAjA>2)ML
z`y1AIINg}Zk5@Y6OT$~g>+%1p;qQ6DFMdCx1WJDG)nia>h5F+fr5luPQMz5}J4*K`
zjdt-I?NFtClnzrmPU&=|vz5+Qx>)H7rE8RKP`X9wcBStq-J>)b)AW`0Q94ZNIHl8-
z&Q>~K>0+fTl&(>_LFpEy+m*hfbdS=g`Iw_+9jdgC(qT%+DV?r#w$k}Z7b{(%bdAyt
zO1CK8uJj$Hdz41IYW|h>Q94ZNIHl8-&Q>~K>0+fTl&(>_LFpEy+m*hfbdS<#uBNZF
zkJ4dE$0?nzbhgs@N*60#p>&PX4NA8t-LCW<rF)b{^E7>>eUuJUI!@_yrL&dJSGri~
z3Z-k5Zcw^K>2{^>DBYtpny=|A?W1&<(s4?sE1j)$zS6}?S14Vhbc50@O1CS0N9i7=
z(YU6sw2#tZO2;Xku5`B2`AQcnU7>W1(hW+tDBZ5~9i@AeM)jX}9;&pD(qT%+DV?r#
zw$k}Z7b{(%bdAytO1CK8uJj$Hdz409&A-w<N{1;Or*yi~*-GatU95D4(ltsqDBYrT
zyV7@*?ok>&SkqV9N9i!7<CIQUI$P;{rHhrWP`XCx2Blk+Zddw_(mhI}-86lreUuJU
zI!@_yrL&dJSGri~3Z-k5Zcw^K>2{^>DBYtpdWfd4w2#tZO2;Xku5`B2`AQcnU7>W1
z(hW+tDBZ5~9i@AeMhi53rG1nRQ#wxRbfvSE&R4ov=?bN5lx|SEMd@~>?<n1)G+L<X
zEA69nn9^}drz@SUbiUHXN>?adqjZDPElRg5eMjjYrBQvq!ynJv{+NAy_xV1(rH?nG
z@~lriqyA<9_vd@x58(d1u(f}t9xr5B&j;lFdFUGf+@Ftr6u|v?V|zfqKYuJa)=$aH
zkw1@YP~6X#KfjIj&9u{>=QaoA{rUOY0PfGzTLZX1-!Bg6_viiX0eM#|plCp*JqIgZ
zAHcgQ-W<RWQG9g(_vhoS0i4&K&9ys#AFBA^fc^eFeslo$=jlZQGwt`+EmZ;BU#~O;
zaDQF1GJyN*nN0zlH?7UJD}bAC6u~JwF4LZ)RlYHRmneQ#AYVNczbJtBRD5MXPcOw=
z19)%6twEXg9HV$y0PmxCQvmO)`04;&s`$cyo&6Nw6_D?*cu`rVJ;y3uAHd~&p<K%Y
z`UiTj^@9L@oZ`<1@Ii_%4Cwdwr~We_KUn2k0`fx?Zwuf<6^{+h%vZVM?*{ZAuXt5J
zewgB2N;CDGpm=jY-rt{F8^DLFd=NiT@ofS55sF(wGSiiFT5^>Ha6aK_uEqea%V=v^
z03WUL+X8r%;<2Hb>5ftS;6OQ4D?T8A*C^f?&{M1Uk^nwd@tT01af)vW$k!=8H6UNF
z_@pB<?H|7%T(>=}_JID0DqmEdsb|uD@RJm;56Dm64}P-Za|7}XimwjfeDc9umjv?5
zCk@QCB!F}D-(1@QdZsG=t$;j7!p-%=0M1cnbL|f3IZg4h<1@>rQSq4poFk^@S{A@L
zs%fqr0i2_V<_h}H48?=}$yxis&sO~PK)!Ul%_<p|Y5z=>{~#cLj^eR?neyi<-Wbqx
zp5ny;`B{oD3CN$X_@)3pTk+ii`~t;GPsp^VN%7%<bT3r=v;cmQ;*9}47c0InfX`8U
zZ2<qW;@blFC5pQhnRc2@J9za0e6Gqb3E-D1zAk`Yrg&Qbzg+Rs;hE{qQ+#d!ze4er
z06t&w_5gn6e(+|+OHa&9_bSC_2Jo*czBYh=P4Rhw`dXm)u7Ld2ir*2Czee$g0{GVz
ze=dM8RJ>$Frv29{UKPNvQ@lBVFH(GU0KZ=G9f5RjP<&4SU#xg6FwehH@$Lb9iLXD9
z?oEnY=A${O^~}wRR|N1|6rUTwmnyz4fZwWkzkofrDc&BCzg_XNk(uSZO!1}wE|2Sk
z<&SGBX4TcyoLEvi?c6IanRi7=`EeD;l@A<pg$WJ5W^nm&W#z{W?dRd<h__?n#msNn
z;kKbS>)OKArJ}#2@RRVVOg$WH4Kph*{-*w3{7*GP*=NL;uG_K3(35KXkzV}o9pa@h
zcZwH2NE2MBVP#tlpP}Eg@6HE&{i`+p1G-k3`?=!(>J2}B!-bwbH-<cemVK`HZ@lft
zZ=2)Ae<|X{U-!A<|L$*o{B@Uk@i%DvZJ!Z;2I7wO6E=Rl@}1l4#h<Yxd--nD7vKFQ
z(ryjs?5F%K+w-2D$}iKfFJGnMnabx&1m@1?uhVc;`I|KC4`pRerhHW(+@N8<tL6(x
zMpK^ZYYWKpMI*!UyUe+vO!<Wx_U+sK8RhHw*NS`n@?EC!yZh}&kH#Fh{q%X+kH60R
z9~jFzHQ&pCfAr?S@!LQ2<GVe*{1j<{`bT#BsihW6l6dX!##~4JqpZ}6zoM5X@>%WA
zKMJ%z`#k-(&hYhbo9OAU(7x`o^!rDQ_NU(~|2s#9)PC3epKbo*u$G^HH0i+cAF21_
zFWcqW_j9iY=9#9~UiKH?%iqN_{P;89_u?-e;Klo_{P{<v_NV_l^sJsKF?$nL5`H;u
zS&tws-;+qcoQhl?)V?Z9*-)&@dBqSrHj*8Xkke)L(U5SLCId!C8!++#JzzK-x$u&C
zM2HM!kH*<XtM9Vtd{j7_dvbfd+@ftb5hJl^><$Uq(Qznn9z2{bsf>Qr>hcPOup{Tw
zUgSLXBK=~{9y7s?^>exZy9}DK*iodGvsdoj*mtosXP?{`#iC>-r$g>bVqZgVPD1WW
zb0|<N$Fe0*mh(Es<k%)qk#h*i<Txf!m2-zg51T-JPA!c#Ct?B(IZt*W5H*3uoJT3b
zoS1zkQD^47+=D<@`y2vIIR|$okY|g*b8{*s9oGcr<y=9gbGq3ww{6am|2Cg<s9jB9
zVa`j11P-%hRd!uYCH0(BWM6=`ML7i$r`W!dz>=J+Byfx^lfq>=gCx#z_Duv<<_wd-
zAQM=fGg1O&Ca^ZARsw@fU|r5c2@J9O5vL{Rr;LC(<;J#6IX@S3hS^t>&StBtd<rP%
z-Y$nx+(kL7C6lqM>Hl)REw@%|rWEUUB)hR#Un!L}l3Uk`#`=RrV^W8?Qng}s-gVpv
zDz$0QTYdBpQ2FRZzQ{&n-;!I=6yHfkyDq+lA^D)SG`n!uJ70q57RxrF`v`twTL%rs
z{BYNY+l0{_dCR?p+`%}J#YVX6)?Dh_-ELxR#ItR;o({}$XQMRa-qIHt>!6inB;57&
zN)dgNjC|ca<|xrLm|xrdh#uE*Zy<8WeG-@m*Uo@Ms-nocr{_w9pGjMCzu8?PT!%@H
zyT?TM0K3AjeCHzK{uzs-?kZC0;{F0W<{l?r=6)Zft1AO{uKQ3=ips4fBl+&LG@ZEn
z7CH}dLt>kIGx<2!Jp@1N=E}!(4{>{tdVyO9x6sX_DIDs)jh@5YOWD#aa-X72AMWl%
zXLt7y?vHToUVKT&-9g5VbO&L>QSNJ$?9uMK*k9sqL4OZ-8uvZjLTu~hevBo(-M$pT
zG451Ku8-TD-@a}wc`0?njQ#!G`$@OI`v`H5b^nM}1KiWFd7x`!=W*`ql*S;p38c)u
zoklU(<x?Zp5cdph80xOax60ikiFLfYmi!KL$5B=%xXtLSaDT?{aCZ~Me4;BpIl{f2
z)GOT&@xqaASF9T4E+coN-Ph2($#U;P-A^p{14`w2%e{<Be!+5AA-CCbr_zdFwA@B4
z-vUVJCCeQ{4q7eu?-bWhE%)~&e2)bYz@J&}+1R<&a{o-|W!#$U=azdLI$yC|`EtrO
zB9Q4{SniD|`=#YJ(H;J$<u0W}U$xv<F!eRd-2`vD<(^1(U$@*pQ5rie_eW&x4a<F)
zeEiCCTabLyaxX#ITb3)I5&E^|c7y!Ja`#YhZ(HtYa@1zI1r*V5E%#Q;{hj4rLB@V>
zxzCZKotE2%7yrR>|BC8&_|MOPf3(~V8pAHMVA7u~cOJQT*K)r~Vg1>1=Tg$|S?&#F
z?0w6<5+nZ0avvpsACPwv+HJW{lA{kT_m_~5EO!7E@i8SqRz9)ZZ)3!#mU}-b?6KT~
zA%C&lFG1QZ_g=F3SId1Loqx03zfc5!x7=sQ!(PihpG@zgw<gqKxvf~8u-u7&3vIVI
z?sl#1UWPwkXS-V|`9-$7f||eHcBf$c4Yu120~g!wlQ`*(wmXe7TVlH}0p4W06S4DV
z+ue&h-D0~pk-w$3i+@<R!Xu&EZ1)%<+-|$G2`#g&gRdjOlF0j8D3NY|n?Wmm$d*3F
za=P7dHlZ~p6n46uH=NMJCKPkJ)tyD?5nFmD*XdR>iqLmW4_f3LA{rxIZ2LkA>nr>f
zT#uCH!Ch^8A<!*8Tqqip7q{(I;7=rZVn<=}N2LmE`*|eZN=k`_l$5zs(Iag8eZ<<6
zqBonwt`+-^w(T4Ww?O{Hz5+5!=TS$&MS9v}@vCK^_CWX*zD%Kse`p)w$N>9Nh;*3t
z6mG6a$_P{_J$u?uIjTLw4lTlUA}88^#Jf!7+lcss&l~wUoGMah?<ENn`DaA@;PXbF
zPdkcCwZ8{3kz@E|oIkI64m}!^Bd6P2ecgRw4NS2YNbIxiZMdM+h&>M0Bw??RGCj1f
zSaz<RgA*I}`LHfdu@}<yMrPZgp31%+*3BvQ`!uA;MRo@*R%+9J0M;WZb`zc+xx`+B
zi%Koo&%<g>vJaa@{YEacPY~9xfp-Z@>eMUp!_JU8oo`=GXKe3-7eh}vzpxkluiD4b
zZP-V`D-H1fM4ZSq_C3^`JrZ7hl5d>%&}zy1_4c#)j49Z2;Ll0Q7{4ldovcM}vJ2>i
z#5e34;TMXub@(ZC7?I_6VTl*rS(S_`{4_5zPL*^e!(fn<A^6~lwCU2^6YfEM_a3B}
zyIy)R%jy0+tvvTB3B;W4&ybPar^O)G>3*khS|m{5bpIA4_ZbNkIo;nBu^&pH*y;X+
z1b!re66Xj>Cb!O(FX+lgLJQVOA|5)yZUi~cM++r}vL@S?1IvFIGZ6|mm{=1;*D3aG
z!1wqpuMrkYBg~z)p3Z9{qV^N8H~J!8?BZ|HhI40(rl?GjzY6>9q=-qocpcS}dtR~c
z&wqjK;GPA~qqDG8Y$$$$TFJd|px+|8!#*Y;GUX^l=1#$PM5{d#c70OBOK1>%Ztna%
zevQaX*cS&xen}F!3-Ty(BeDqgO}+^K*IcS%;dcDMaK8;l#zkYPReZ@&aBsYxYBpJY
z4$glFR|?-N=Hd%zmAOj~_e+B>q*?C=M0OHC_l^aWtkDv|QwsS@X;D@Cq+0H-q<fOj
zIobn}sSr6+YGR&M9HpV;e!DMCT2wg=u&4W~tm4ZU@p2z(#%RO61kP1H*Q>lE?;u+4
zV`ou6qR`<hZt421{GXALqke;@H|JH%qE*S?(eh*2-QT1p@;(xYQm4B^o#uTkfd=OY
ziC_=0<?G9T5RE^VD)!J5?N320EH<b*r-@KwyD}|IvR;)~B^S~m*^}&V5z8)vH^S$q
zd8j>YJ?vj0Z=VYLY+t0%s4yOC&prWjO@YsceQi?2E69t;lD*(Z)RYmq2lgWwB84({
zw~FN-_}Z)g1DlN8Pmp=Zm$QoVC~*6cD&IfehVzFM*TFUHABe-5xclIQD0r`QihnEC
z|L6l;*Km)3)5qtkLo5{|Ul~h0BQgs1<O~rtvL7yE?>wI#-pI{H=F$u~t9S(-ZvS-@
z(`qAf6YP63L^R2jH15!Gx#UR7$$kQvr$w$!>ZW)Pc<6-rl!@WK0_U|9_f-l!RP_KF
z4EKFFAE&q<q9HVXFePcYxfIU9{Hf+EXdR(bN|7;KS!54Nara5OXMS*$a_iunl;WO6
z#zV7KQcTh=?epPWEL`J-#lID+zB~g*HQYsTZc1`{>_)R4v-A84c3j7J-lK>-<%@Y`
z(_;lLZ;!PbaXnLrjvF$v+mZWirtG2bN!2a0C;NK;2Hyhnj5<`>vHi5&!}e4;hr>EL
z#g>L_Z?Z3B0FacN3RpEh+jH{63n(%B=XQcQh>>Yb@{Q+OJ=aw+R8!%_?UagBJ(gct
zsM<FmxJ-m2XwYiEA3108FEK|V+iPJz84!`mcFvf929x=p!G2RjPVu!Aigv5`1<KpG
z@Uaq=`xu!J+SBB`mUR<7Z~oKYA+|C92xQ8Va>))>J^We9F@Mup@QrXiH;tWzt>R5M
zUw-Qqnz%C;nHxIGX{Kc$%b(g?3()>1f@_n);)d3dG92cg^duR(XDH>eG0DE4AS|i>
zeCZyYo>XwxTOXj1dyQ>4)rt@K5ZPGqDL1W**y}3ue@Uo!*HSvd;fC?X)xY|5+egoy
z*!xq;JU-H3RZE|0KJxLx+GB>(Da5NghVVaP`>bar7CByKJALPoSY)gH72Hs!Sf$U>
zrVl!=wx?GpX9IZ?lYZiS2c7vJWKT+~a3$uOEbF`_8e56&(U?qM`p0DT(cj!h&3z~w
zzLS`tSooVH6pcwiL}Sv?_m9aWr@y(6n)_%>rZ?=G7+jpKzqyab<o;M0irvc49%N+0
zV8FvA-i?(nLs~TUGC~8tEDmzGuzeA^IqpKssWx?xFK#+e95ytXJ8?Io$d{UIq@ppY
zg@NL<q1lLB?~BQeC_6*M22H<RoY**OzGxaKE*!d>yQhK?E=)zZ(0J@X>2g9ZAosE_
zD>qiY^fLpc#|gC&em8?TPzD~m9ju7jGKNc}K{8Xc7nme|_+`2El8_!T)Cb8jUs`Ug
zeCd7%N{1M#A$(E>vup|q!y`pkXd#^AzVxNb3Ex7P+e^6Z$L*k3sH5;mnaB>dhFl{f
z#sdfs{g{fiJIwlV=uYY2ynCx$I!XVznQZaMWv_akII^8`yws)1KJc)w;xP20V#2fh
z$#TFt6{(5~ExW6MZ@|rmUqfX3Xj^<KbVu7Y!`DdXTcY7*Uq5j$8F^73)s>;b?rndC
zInfBI_tP*ubyE)_t+LV=XGk*FNRc%^BShu|WB7@U+?IC71g@F<+1>3zdf*Y_DQgi)
z-i&Mp8Sxc<J_N43P2-#>5xETKWYRKnBPF`|8TAbnJf3<Q-J$_myjIy$?IBh66uYd-
zK1F^Sb3!w_d)8Ljr#rE8tDGw>doapP_6KR)WdQaO_PJuEWa%=Zq;4c@KLQ!E39&$y
z<x{sPy9tpRRkj-dWicv!g!uZ2h$L@DJ_0ggH9zu2<;qsyc+tXTI6s18<VH$#^E2vY
z6*O7?$dC1TvV3VO%eknVqj5{9A1Y>=SSkI%Z^X*vO;3;lhQ%=rh>aB3{EQI%TyQ=`
z|ERjog=na<r#V-HJ*C*yid{pge5jaA^yPmKitIH~Q_kh=>2)Yq@-^z1BRr>(K+OaV
zNWJbh@jBP7m8#oN*Mn`%Y0bGBRg#Re^6|{`le{rKmB-Rj8E-_V+7kmF<fmd)Z&2y8
zR4MtZepazdlL=Whzf?>NxIHQB8}O!bO`bnZ3)FNk_6k7qyi=_ix*u!s=TO&zWS$3V
zR9Rv=bOw^=`K6L16;m@jjoZWHhURZGwLl(5ja9Z-RDG@ns#Oc|Yp+nut2|oWMG6!R
z^^&?AmnfGh+(KpC=(RdJu^Z7du}4}bwh{;OI<c{)x7tj{bizV%J`5@8#Oe(E1cfm6
z69z4svc#9&h;fqv?Ne=8rVejlj$~hy>fn};ynS(I2lqEMXC3P8M!S}ojcM4AqAYbI
zE&LP;Zj6WpTDageVre@E9|@bQO_ZHx-yzDRZ7e|~bt7%$7otqW0&Pcdlt~ZSpvqi2
zR{Jdb1UhxoF?PXck~gUyvH`gA#~NpZXyGzF<oDqOd&uWiP%3ovA2smb=^-06Zk1Sh
zwtc-=DLv#b#LDE2lwTJ{yfJeT3zVO%n#r>CkaJYoYEM~z=A@=$T#QKaCe=f31g>0m
zv{&X8qJ_)!kX;z&gFWP3DrmC&Lq8VoLK`^npb+RG?^Y|fc@=Y|SShvKpF~qPQp=Zv
zjIkKH0<}C8$J8vB9_5)|hLk<aE<8$edLbev%jQM~g&zna1B1*;#w?R@QbN}g3TAbo
zDm7XCo(4Ww?WSc_d~l-1-6YnXXTKoU$uPT-L{m31N=KNv8E=y(ER~IZ@k&0q4Cf%4
z>Ijh?!cE#}eO7wKT<6F9JQWidt)m*(rE9UjViz-SH!e6$5*A-HH{y#=fK>Y8+7Si)
zQVNfr@ipxrj-?|jN251ZST(;(!zZQTFKLlgBR|{2!*^`ECTi7ePvb1~IjwN2|E!$l
zsz-*6J&LVO(>+8pD8t5<BrbjUd^+jTY2{e+QJUz7NwvP{VKTaNNgit|H6RW$pu}qr
zllC*HSk>cHy2?m;!}?^!rYeU0HRmcOwqBN$^$oaKxnjVm8`1(bk7_`yc}cAaPTBkc
z*9>u!lSAB(HL478f6yc^H$$8l^wTtMxQD68(B<9`#|Sp+wKOBDKhnT4o*eTL{)d2U
z4{)giSfl@@Lg_<XG}cD15RJ+DVW7<Mt@Baw75<Wo4Xf@nf`7;BT-A7*(TmTnx)V-e
zPrOxPt)XnHOH}4@07?0BuwjbH+Fmd!zk0M{J3LHsW>uf8;CrdaHK!>iR)<MS71W-q
zxQ4AdSvkp$++1bU+Mb#AstTg*?1h!>h;-*~gw0aoFLIN|O+Jf+6RODHYLO8{wo;AE
zW|=@stS6YTjk-@Y8!z0X0jc+Aja^gi159n4*b09(=l1k0lYNc%yc~r-wx{805lAl_
zU<%=>X2x&Tl%+`*V<7oWmu^T&15dF=4Oe+%<aue4Pdi{_))1rSq({9&1CqZxP1LD&
z-TqqHsE5*`TBBZ0E2z;Qr4<x;>@;+xH4Z0strn71E_<DuRO|Yr!-aZNJNzXVkmob2
ztnYC)&S)NWyiah{8)?SWbfo2DHZecbgy;NvI;59ZPqMFci-{?&OMGvpIYoTCrmqS~
zWv)*bGLu!2@#=9@nF^YEnWh0L+z-^K@%t^@^V6bQ<+9DYP?62af+;?R4d6a$1#<<n
zRIgxOp>(W$5;S`SuSi?JfvMi8`_lB*Y)UifHk5s4Ilbza({D7Ul+zDPOf9Em5m+@J
ztBe%WuhV5xWl1?X3$SW$x>Y@TdvzcMRYdm0s{I$#@hUB4Y?aGqcY`8NB@1dSTHZ@9
zsC=e?UP0}Yf|6(8qwY#mRP)0$8`i1~2k7CQ*e)%ZQ7@*c8U1dWngeCStm{8WwgcIU
zcaQN(VOBC1xqbOREBqzf!OtZZcTvgqUnLi1O!De4qN2UvLj*?+P%Xv-r)WS5_G%P;
zW_5R{MdRmbOxZyVbVg}im+6eGa@p8ks1aUETK`iLJe6&wr1jm8vaA!9g;^hg4&RGr
z`)*q#LU*jYrs9%ur&#tqW(_qPLBfreeXj}MM0mJMBK9g<HWNd4tiERWgN&#4{Tg0-
z%>>!-dPR+ptrEHH@7NEQ(z#y_?=h5&XyRsv+?>VDK5j-^eY{ei#>gY1j5Ot`PHeTb
z)4}4ca?6IyHh<C1Rtz3YImKj%vub{#Y0C4>EhJ`?$oMW+S~b2&!hXkN>i{3RV=erc
zrY~FX9e#Aw7Ar1Ww!J7Lv5I8vDLJ9MBPc_;Mh{985&`MB5a{L$Sfi$?#I<D4d#mj{
zl$m$k?zhV2X~RsFz>UmhueuJ2+xVOKaEZ5nXHT;R$|E>y73kyqC8rGo`qIV7`*2pB
z#E{*DxvKACqfP4Umv9#Kr_d$(Fs6MqOI1c}`ItYi;H~Pr6qo;FCSGUe3*S<(*aMx*
zORY(dDK=OI8B(q476r$p$Nq_eXQX0R|5UL{e2k*}g<}4^N$S?*=_U+rQzkBJE_>A)
zqQ1eO$&<{6tN)}jFQk-?|46aLo^#2NPM0N*BCWoj+g+N}*iO8y$EG!xE+y>ZvNvnb
zJ{zIi3Dp#6EGf5Rk(F}8(3)O~%VYTQK5piim%}|SjoajL{cRN3lSe6A9)c`Uwl`D3
z-l=S}C|`P%(HChzdK5hFz>}ei{QhmAJn^-Dh4TO6FX=tUN$yUR+*x&!^>5(U3pD`-
z33J)24j#bUYW$6m_=ol2=J(tz<)(V23Xg>C`H<HNtm^M8XNq!ASN%i9&R2{HcFktR
zq%IeA(()_i-ks#I#8ONg<#ENSgX;Gce<7v4_EW{gHpx)AJioI_y{aGNWsnB)F;ONC
zOso1+nJ}JwS<~ef5BP*}KjE6gHJ&Uajsr9qHkCZT)EeC<jSI)`W7sZZ?kkVH2g>tx
z>ue%k&R?nyOfofsd2)3P+zz9&EmMuxB<;Kp&e_LN0ixy4*fin-Ze}uXtRAnzF9BrM
z#Fv!wrgEevE>`RV#iS<YD<)N)Pa>IGmMXVLl4I2@S4?VRxZ+Y1_bYyqVrZ{@STWIR
zYC>KaSf}8yWDc(;%Dem}$E?1+X@sVTrU^cmV9gedCav%ZK+jUE<`;^~1HxC5xO(L1
zx0EaS%gCEo?B(+8cDKqb_dEpGh~p(|2B#Uc5etTyDc;X*nd8+m&l<Cdh9g^$+stse
z6=;I&{JH%+J#fuszh>WuKC>Ov!5k-bBl|*ijMtUHD1kj9kx}IpLFI4T?Y{Cm5i`oo
zjVOOpln0{(l#7fipBYsCCwp3%>Kx7}X_T8AQGORlWiU!WxyY#Ug+b*X+duG?f9xwa
zH=?{BwOkpD5>PHOIL41vu`E)zqcOdj8JNZKMr~0UGe7%7T7)jta<&K`rAIhS51C2#
zTWum7V7*o|P^HD5+1b_CoRlVWb9R|gXRD0ZJnFKv06m(#5wlF8E>a3y_No`r^ge&d
zb~KL?8h^LSec&|?84hI(p`^;?g_+gL?&@hlP2Lbx$e%wG`JSkGG^-jXCxIS$wPsx>
zJyX%KAYIQpqUVQM^{_^^8rCwEb^`~X>{ZXAVv0OI?ZoP^Pm#B05{Xl9hS2J!j?LrI
z{#rSEF!gH6CH9EiPWyGYv<Lpv_DINo%0I0ur5p2~@8^%F5T+jmwEDFE{N{vOKc(e!
zHU1s-!=p?9hO#YllR&3Gz|RkWBqUpWh2;V_4$ff}R7|od$#;A3YBYZ`=*pb1@D^^2
zxcBs5BAQnK23u!rXFH_v#6y`k9^$>*;HB2zJVx(tp3GZ!(}Rbtq6ntjn}{1;hm~?l
zrv!8SYXtX>_pU%l4(sH+Dvtsj@34;J9oBKY!#a+4SjVx=VI9XwKdj?;hjl`7Sf}fv
zIj9J2W)SLnz1*VBKdiG-g5F`BkQ~;@EqfHiJFJuUsT}<<hjsGyoDXc7!#a6?zf*3_
zVV%6aa_=41$=fIQ-eH}*4!QRZ>*OWm-aD+5XW0_)4(sIECg2^`$#YD=JFJrzHUaOj
zPF}<Wyu&(qQ4{bE>*U4kbFt7ntdrN(ZX)0v*2&AW#bEESPM&K5-eH}*ZuaGHyu&(q
zhuY#h-eH}*!))2d^A79e71@`d%{#1<S8QKHz&osycZ@Bgymwe9?>KuI0q?L*-XIh3
z4(sHVnSgg#CvUI`c!zcJhDa;c!#a88#y0P;PTny4deYIuI(d3nCvUZ6(j3;w`?lPA
zhjsG4BiZ#1>*TGG+{$5{!C<^aLMT_NR?N;beh(_OY0%|9Du;FMC6F(&`J%%d)5%}_
z4BLF(F`fK7TM@`|Oeg>0-wGq+n9h(Pcy<1BMU1s>R0a`uD!;ZnkZF(OHXnl*x*zpJ
z#ytl-;@-?C7S$s=`LB<cXrKScPORz>I*Tuk?0j)#=Zhmd|8F1JiM{`y%wx=vo!CRR
z^f!8BC$`3f^vF)^VH46LJF!P>>813@PVBp;AJro}F^=rar*l}qU%_)od8cS%9ND=Q
zi2oDq!G)qhc^uh!1pL_~PwXg6{-_j3c3wf^Pf01!kdiWYD$0?azabW3`e|$^e9<I!
zEt8BGM|KVc@8R<br0?~P?8LkyJ91jiJ`w&{U&f1Xj_ky|BRkSz+GoSRE-52Wq4e)*
zKQe*v3^PY|V&0J*6Zt+OKKFSe%aNU!cVx#z-a$nE+qLHIGg6i#J2CIbj)`2zeCr7Q
zyy`JWc4FR<9m6h%bz+JwM|NV~ksYZK`&3w`3;P#Rrsl{_%saAU*q6a-PO;_4PRu*9
zW7v1V;vaPTvE|54%sa9pwP~+|wIRipBRetg$d1&K{R*ttl5BHiC#FYs-Ua?tSmJYD
zk((nsF+H-=6*n*BFP$$(c4B&Dr$4;o1AIBM6VoF*6W~ot@{RMFBRetg$c`zwOW<FX
zlrer~j_ky|BRk?7_Hy`zBJCa7iFrqM1aKZnMwMWi7a6BYx{_fqXOJP-JF*i$VNE^{
z6MHaQj<5fXREu|HC;pTK^vF*9X)#ET?8I9nphtG%&qzRz?8JX40X?!4|B(dr$WFY@
zKABnZH2w;9N+RaSPJDuWKFC}jEtD9_nrtrwz9q?e*Tj+|JMmNO`+(Q@EUyuoBRg?(
zWM?;`_Oq~m>WdVb*ycb)+#K04MgCjZ?<GY{+UCek+#K04B4L&|2k}?12c3m`#RhX^
zCvJ}H7?HlPhXh3A$WGiG*%7Vw1lZG(B3?prWG8Np>==<R!)^|U$dR46IkIC!mcd@(
zi+D$N;^xSX;mXTK>r-4gvJ*E)c8rl*;k+(fDSWS(&5@nBIkIC!K7jqVfQTH~iJK!k
zMx+~_(v!cG7F8wBnd9cj&g0^zCn7Ue<V>k~`IT4w<L1bYsB+GNeV(t%JF*iuM|KSN
zYjEV<BQGISdFIGY+#J~vh0eXgPU*MuchCaNk)1lajbb^#5j6)mOzCVz`j-+z4)0pU
za?B!Xj#(J)yKr`=xN^)QYK~bLZVY{K{xq%}vxu5w7E+dWA2<VqE3cJWNB)+DXVe_C
zFx;vn$2w{#TSpHQE-q&M*kw@L0Nzm$?`Q(MMC3}0ZD)daIkoOs<t~P^G|4@(rXSqd
zgGzZ3KvG{VoVdiZLh|Z;{;lw>{O^*Pd`T-`3gh5jBxoO0QGvh_a(~ch``6&Vm&ko2
zvWHgNAA@xGXyE|E>M0S%*>OgZ!}&82(wf{s<L6?`5JYYH3_ztX;>9kG^(FQcc|v78
zXe#VkB2qzw!cp>5E2(&Lr(f1b<rW~bG$}VBAa_o|F)H^UGV7Ca4Jo+-QaotRxs;_z
zcPlc#P0E>c9TAovtJnx%xxr8TuL#B|E6+%icuM$4BYfQo%Gqd^QuhQ+>M21hUrtFK
z)jprMIglwnd>~$X_e$zj7GU<-V6!CdM=akxe|4(PM8^2U0yx*DxZ<zQ`1z!1xc9<Y
zo#Y-V{_32v(2py}`8}=HQFB<?oq90(#pfbpjw6dk!&}JwPUKFfkFkoy=OU-p^ipo}
zl#byZ{u69=r-Xi~h3*uhu)q{ccPYbcKhg~pnxABWn6mZ?+&cPV>?%2qRAUDZmlHdO
zcVkvnO86Y95c>#W9X!%r1nVYA?NWg~w938*<Ut=Tyi!;_@99s3T3dA4&+xO^=cl>q
z!FAsy3-SuI{TA#$`XZi+UNxfPl&dKyQ$!qrGTO|kokc`4eVvzE<2F{l@OyuRrxkT8
zw2vYSh3q07BR|n@(fZ<OPuuSC9&9gDeMHipj@ZTg72JeMV_d=QqI?xUmM3x*?3+4^
zSbhIWW9oK<eG&B`v2BOX03c^S%FjB9@EuXocejX@*ef8$MIF}|Xa53`Pkph%WZ&s!
zN%HJfk=$9Ozs1?SXkqdbuvRJ$cXPWfkj!?_`}Zg%?B%oD+xStps4dkWbNYy&JqfYX
zM2~!5qVW4-<`Kf^w$Q!+_{%=a>RCYfcUxqOUi&(JZuMDSMfN+0LhQD9J9#7fjuCqV
zk;i>8tN&1nr`ydt_?5(v3K^+aka$g`)=TRB`-`62s_--ECy@#psZWuxXl+Ky8{PUZ
zlJZ=>8gtF)=C~0fT7p;~U)1VviPSyMrxZqw)Ho#MWJ<ciBN6I$|7844)9zwKFGcLy
z3{f>8kGkmg&>?=8<G3*+|7~P{lqqj2Rg`)St^a8f^LwQ!PslMb-z3(j2afqwshy|V
zDMj_6t|n$7d%vUjOJ%3fBxgDS<3q{R)%fp8W;g91v#Kc9C^{P@H~vG4j72{y$3Ik2
zo>BBLO5RQ?G6j%w?E~6oIw-5ZG4PGA;IyhY-{?)Ca|oX8<@6uXYxN%~F8IgOk4dVG
z8<nS^?krJhT7lSR_5aC2M0Yn*L(-|>hX)zas}Wn|r*8F^qio$iy)~uKHBt{C@qJ&)
zo7)$!5XatoDdlavRX!)P)fcgjJwm)R(IJhy4fGx1h&9%Lze%fg?Bk`m{v`=p`O?Ff
z5;<yJ6^EvnbsQ<jB*W&Iq-+`5$AA^gU>Ngi+{%vu<d0-{8Q2YNq1z>=4PeGqtb7>`
zkCW4ZhYWcdPV#i%AUPO#$Z&)Cj|>LM!N5aC8q9xiP$mZh52-!_-Kz)7!NAz@vZfj$
z2Lof?!9cTl5RKhW#-cG<><o>`Ud2#zA2s)mcfc<iJKqS&^l@lR)<Q#LavwGK(U`1)
zqA|%qG$zxkXspiM|G@l;UD4P^^DD#s(3niJhQ{PRYVM;kDTtw}L9!l=$%tGo$x4TL
zJirCeVKIjcP3JBelYwPuOa{fF=00lfqcN$AXzVT{BS&^l7#fq#H>$tl6GLMmW3l{J
zNWU6tMet4jhCf`QH}Xcy{q)XOzEu5*GA1y9g+3&ZPc<4rbK?sRuMp>uiAuf<Joz$W
zRmcz=ItH-9k7I89I99$4+9S-OrZRLAGN<`+a%1I7lVayv>^zvC&|J_(zKGmZUWk`T
zF<JTIT@`ZJJ+y+m)xMP6Sot!iuV|peLr)U^vCotnD_<tn6*Bb?Z6o|EpJ`%N$fQ5C
z3rr?qCff#&#)gSi<EX4qlnIi{pW(`ll`p~-GGh<*A}rr-FiewPh0N(fBf!S`xQR4!
zgk(|XQ<cryBFMZnii*t=<I8Za^kc}4l`p~-G6@Xb%-wRIY1CE7^e^-su*WiSnbd`T
z2)5bBjj{?^z=UK$DW9@3=E@CCRf<Q}FQE?z|22bIA?uD%7o6o_{!ApfvGQfXG0L2s
z98P$&IXgMTk(wH9&Q5yw)(H8WojueVkI<>U2D!2FWz|t7k-vvy`JWXPkCe+^^))z)
z_!}cpd7)^8ELH4E>l*~u@@H%jO_k=og-XG!d|9tlNwi5!bGPY}&y><_aNg!`jA*f^
zjaemRsx?9uJoZ@Y1A-2##-#4yNl&hlXg%mr+d3&dh8P3+8za%>4J}zC*^_sxVAx~`
zWYuJZF5s_5TA`S3PpdV?PZktAjQQ2FezMQ9j-}V*TBFw6t8PW;Qr7fiCA!=kjXC2k
zor|!nW*Q0Ay-QizXU&KCG{O3+2*VF=OngZy=1>{PFPU!HGbMBtWAgZUUncgsGU~`X
zJ`==Wi}k~XNwcrX<tg#RlVP<Z;|aB@P!51x_P6CFO(RrJ4`xj`m-g|#3gsCQgqAH8
zA!*}hA(Xng3Phf;P7202No38>L|;Y?7(UP|ourkkp70H5GX_Xjz7Z6<Yr8M>_2V#V
z@(3;Cc|<9q%ucqa4Y=HjmGq94^cmVaC*j}sTIgSP*Q&PIpx#Fh;1!lb53w5ltQyWV
zv8URDPqxp={iI*a%ku8gC-Z9M<>);(6Kv$*-^rYLy_|_*wHDJY7=ZzDBoIfI#c{r@
zi>qa=Y(KtE^Rk8-mCIhWiX<QJndS~3ktgF0kITYr>LA}89v7)C1nU>7J3Ju{P*JAt
z@B<0W8zP|^%YITqk4nfm$5Y}QM-L@9rrEOJkWdpL<0_XA3RJ>>%4?6t3!Xs6Iz=j>
zt;`cjx=Prd;Y+<QQpQ(8gFUIFuYBAhLQ?;SmZ#r{A2fhW3dRWdipZ#$r>O3vuOxo$
z8*q<E88cU_P*;ief7p8u=qQWzfBc<yH=9j13o9fcAYkYnLVy$?5PB#Hp_fRDAW}t|
zpfp9X3$a}bDp)A?-WAkqZ(sx46??hrMX%`nTHycrJTvp|o27{N`#b-0zUR2-?Cw1C
zl$m+vnexue^A0|$p$cT1!W48t#BiGQ3x*1#rF>LWP0lvin*u7yumqUTYY{UfktHFY
zq%m1&mYbl?x@rxDv*~?KGEw@>M5_PXYKgM*tf<EmiE62xmg-VcjriTz6HUMT!S*`k
zrj2NV2U+npSaDL-V=FHU9@dZ-LyBX}Vfvw^NLyHZ__jb1y^M(44=v*8GE(+{k;1a=
z6G@|TCHwJ~?E??rxW6@Wx}p6XLM?|X5DCY+EAcx{Ys@M8z|;lXx14y?ZM^JjU;Hdd
zVH6mhkwZEuRL~()Ji79mj$;%T&s2Lg*~mlNmycTiHd1Sjfx;U|UV%xB=0MV3aRn=s
zkBOR$B<&UHdIBCh&4@W&_me!;1fAe-<WTYP8D^8^TAV6S^9((kI6rEADMY>MPj(69
zO0ort1K8b6b|N233KaX9uL<9VVa&iy3ZTHHM(mb4=sK@605{rLLEVrQl>=+#4^Fce
zm=oftjX-KwHPpOcU=)`DRWEgyVgHZmo7DQf$lg$la-L?6N1BqwUW%RS<p6aSdr21e
z5tb}!__adxvfH8QaU5s+%f@Dht_0Du4iNArf>USAaa4)G;ckvI{vo8PuoD73-Ai~D
zo`MYgP7&b8<M^_kfCCuH)lkX8Q-o(wYvlqrE(TnQazGW-;+ceBq!xz@{2JlM2#*lB
zLodMPYXOf^eE3~PcmW;})o5A1*-!X2${XXs<HU<$lH@n9H;YFv1`S^$a4(cs!>W(t
zRESr@Ns13r@CR5~Azlp@zH4Pg>yeOe;fGd^FJ*a_NdBgSe|&0Fz)J;wn(!vVrwM#q
zU%)QuPZ#)fz{d}E)N+B;h_YB`-JaMRywQw?BA$F@*}nLH#^Xn{^Tn5q_W?0XDGl?8
z>Av`~YYo7Sq+(4JqT6Ycv1}nIuT%Dfk7TMQNfwe-s7BD-1)2|iH0Szhc87%KA<$%X
zi5KQ=O;b?y*ow=7{;tgfeO=oE#|N0%OIZ(}wC}Vup582*;*0MNQ$VQ;s{ZbY<<!Ur
z`UD>`l{G$TBegV1Wt%Vl2Aj(B)l-Rsf0Rl|zSaz#J<`y8kZ1Azh3b$RI))$|QJ~1-
zK{%}7e{2x$IQSnIthy5a>>6_L8<yV~yiNgRu_t#=&E{;)J+)=9YBxNF%kC?Tr?jMc
zO2MlCQ0D);w03mWX>stl#HNKbU@bn(Z5D$PIc4AZFm%-sO`wcU+3!Bg9*coioU*iT
z@pOK%80g6<tLwwmLNj9vz%W(T+!x>4#IHbnCtrMT6OSovS+OsEyosl+4fDm%H}MM)
zKikB!*`9CWarU)rl`sAVU;ItJ_($ULBOWpF95Bj0Gyn&TpM(fbP@N7lYB(d3x@)Zn
zZyN)!DjJ~w6tzlKboF8KEk>$hpbs<5Vx%g@`Y^LBMyg`E4|BG~NL4KKVXoB}sfsgv
z@%Ni}R>j4>_*YCktKu48{AVVfRk6hv|GSAtRgAdZ#ETkUHvm;wh+&BBD_n3`6@KF9
zO)rXD*<|5G8x<8c@nKH07*S!a4|AEthzfi8Fn3stsIc6JdER0~g%f<3JsKk_oau|_
zf;X+4Dm>K}AHyKbc&hL$Up)7$7*7>e`Qo{;z<8?g3KLHi;vfLG7F8SfE)$ChgM8Hb
zQ^2zuUvC2PcMNAT0cqoMYW6!oJppOsa&*t@XD1*UJ{j=X`;Aymwvd5?r)ua05jh;s
z;Wbg#oP$n)*Mo^q%)|o4L8x4JQd`6yI50UIf2hqVJOLira*~T6Pk=|ZGFUkZ<@6SH
zD`yb0%ej@y3E73*XgNxLLKUJmOuZJra6vRDz^B~^43B%S!#^8c24+S);h&<9duK2n
z%LABi1f7fsWUB3egFAEd(QGG`X#KeY>pT9V+2Q~F(d?A?(d?8zI+~s8JDMGdAI*;V
zj%G)EN3$coquCMP(d>xtXm-SRG&|xynjP^U&5rnwW=H%-vm^ea*%ANI?1=wpcEo=)
zJK{f@9q}K{j`)veNBl>#BmSe=5&zNbi2rDI#D6q9;y;=l@gL2O_>X2s{717R{-fCu
z|IzG7wWHY)b2K{=Kbjr!9nFsTj%G*x-yY3QTN7=70||12LfYYuW~XgF_-J+|7Ab;h
zchw4D6}msy0b-YTMB9tqfnPVa2un4A*m-!&3dZsgljPKtv)pOhIGg`7XSuu1WI8V|
zB-_#4)R4C8W|AKLvG1f}zKH#=W8Z%r`~K_L_y6r<-$~zgz<_ISYT(IS$GfS4Q2eF_
zLh+j#*aOCIYM2H#-%Sm#qjT7eZ+dM|>66$=IQGp0+dtaq3`#IK9Q#hfB(xsB!g9#7
z+(|h0-3An#u4*buI5a7aeUAp&6pys7Bz8u9^r|@aJs<JQElwXr@!!;N9EjBwz;CoP
z8N%<ssev7)dIb1?cr=VU>{(sMe^Y}n{|rVsd`%@~`~kD?rUqf=(d@DKYDN43v+t$`
zVLk<nr&?yme^Y~?*8z1!9PPWQfhM5t1nPdGn^C9!n;HcDDo}66(LDBTZfX$pw?O?n
zj`rQuz}i%4nCjKS*9iCB)IbwcZGh_Np{w20&>zvmh!S-&EvMQ|4JQJ3N&?=0Q^OkI
zsuJ-2n;JF&cejTZ<MrRvAews-__sV7=GgJy)F8*c_X01qUG1g@vb#xWiAPbP_SiRv
zV14X6n(so*a_l>LUoNXfAN!8pPrw}ej{1*%NBzgXqyA&xQU9^;sQ=h^bb#W0V7xvd
zy?}|xvG3?0^(10mwz2xyh(?vE_YwVthw33*JoX(OuDlBrC`&g&IrbfuW8Zxdsk{pm
zNRlDka_l=Q$G%17-USLmA}W_--%&aCEhOFr3WF%696$QlcT|pj3klz6e=)x4J`x`L
zj>eCDZvy&mk3=WLW8cyEvF{gvelvlDyY$icvF~qy{=t&yW8cyEvG3F<I#qln8~Ub(
zs2uy2Otu836S2Oc@}`EU9QziM!9dI12|f}Y`;N-7Zz16Y3M;Bf(KS||$G)TSW8YVR
zW+UlDCAz#k_8paD-xTFO4D>cDN+0`<%CT?3?gHjxi`A7U$G)TSW8Xg!9hdJ!JHu4u
z*!OJMvmE=b5kL0b5^?SEO+QJhkZ(G#IWoVI<4C#s12c@+<5|m+-v%E9K{A~e9SF%Z
zi$)?mzr7Q%oTZJ$&u?FZxJya5&`ac)ksI0@3AY0C0I`+vM83ha{g~;0&o?A5S~Mo}
zjZr&k`Ve<AqN7YO%A(1U*;uenx684)n2kNiZ2sV=ERWK<om6dXYsmTR)0hh#+fB7X
zEbmK5XRH)a|L}Rj5cN@fztlL7XchSz&7{=9>Yqqi%>{0$#rv9_JEA3?NY;Q}RRO);
zl4OX8`tA9MG9?)gFrrFs4SnH}%w>L#{KEqS8KapSE$YGuHe5v10f@<kqI9Xm!{x|7
ziK0G!uY(aa2x69dqIe^X&efG(C?S(^QJGS}+G?pS;T4dx&6AbD7Y7q1G*VU85_W@I
z9rK3;OO((-t;jLu;nk$2o`iU!2_a8H23tmkSgtNl55<=*h?-A?9G<oQ12rE?R^u0;
zV0WV8yHZ4!8>>NEavy=be|Rzza#bC4nuw5vs;8|i9F7zrzj^|s!VXtgXtO;3epk9_
zv%&cJ_g-i_1Mp3MJe&5F1N8-GZG|;6v+g8dW)k}mVlpV+k#VNh_^u#~^hPC}3G_uC
z$uWp>M1Qr0sCSJRwF&swJeul-^Vz<qKPU4%?8Vmlci_TYlaVjMon6~xGjXCAH+eP^
zX(<UmCQGx_^^hyoPzYg_6euN&OGpU$vP2`Vb*>uF-bgqd+;?~q{<u(y23G4fwcX0%
zjd8ztvJ&zT2ff_*WnI?^`FhB0U`poJMlJJYz)~_Na~D)t>pb@r<X);Y7pQ_bdONu{
zxjClcxjdoG_v>oCcx^(T5YGICI#}-Z1(OI{3CgoAwNrO5g>7?p+N@k{QStn0K~9wB
zt33u(hT>fdw`IU%fIptcKy!KwY^BYq7UD7REy!F@q|?}?`%T};3jYUbG)WBT?1M?1
z#`G9a+GBv+I9@C_Jg8nE9zcdqDWowy29)*~5Ho?t05hlfF1ZUWS%x?4YUAaAsSQqs
zPVF%uY}b&bdT#NO8SxlU%UD>BVa@3=pvx&91Il^~v=EPhPeAq^<w)!Pn%NYO0kul)
zM|3dLpqxhZ7*MMf<*K@fZfa3_sA^4*0p)oN2w5>m`dc!*Wuy%##AASo(PKcUP6Wkl
zQtf5xZRjze#AASiGCc-_>OxTb%~I)EL>qbxsA?VqLi!-c9=D`U8+r^V@fa`yLqhcd
zD895*Mj$-~lz0pn(qthGV|JB=ujDX3ygO~^F`&d_z|?10=nFtQygI#9DuwEywhcW7
zlz0pn&J^LC53UQ3%*pflN<0P(XR2`C2ClcNb4qg3b%+glshh&`K)w=>0V60Pg7!g3
zLu`Ba3i}6wBuC;gU<9R!Af7i_><JPD#7+B%B6TxvBN-Hr0V6jma<72Q7yc``P8)g*
zDDfCDB5R1quORM6ij-Esyg6;2Tm@3`7+}9oj{zY~N1LjLud!Bo3@Gs!Fal#j)fE)|
zES1k=K*c=<c!$CXmc(gGj{&7U22Mrna$;miY)6j)WjqEh_M)}R+h{z*)Tf_7U}__D
zkK*$70B9Z~UH5F1qyA@|fxV?7&p0Z)2@H>C8*CShw1M|C+M6zyXRd!I_Ehj@uy4_m
z{XpDjE(1v(i?0@t*$N{-?K<x1U5tdzKy@XW5}dkMU{X>03HLT+f(mkZ&`zV!Q^5ek
z>4hvq9Xts%=O&--sl){hJDFzy#p@H&A7)N;Y_a+qVt6K7Vl%c8)rgL1>3tPtsR!}%
zl*Rid(&^%u_6+FNyFh<tNwkRebWBUH=9nhZ6tb575}0(um-ZynWhY8}lwGkHW(Rs<
zB(J6QbOTR@n2~sM>;fqFVugD1<(%UGay;UuQ}ji6n$;~*Dl#MvAKV~ND}g>gfrR&B
zh2&l=v9}w5zK105S}EFby}{FX`U=UtSVH$~bsCCCu9?k-(@UU!JhA04po*0GfwY_P
z0PPzDr2~oonDTRciSqfV;g9`sB8Wm5P;V=_&LMqI_8$xBup8!`+?`G|iy=U?rd8LG
zzAG8IA3>kqHfJt<S27AE&Te+*?!z8wMv=tXEzeH$UCHSETrcc-cBb!2QWq`^bxA`r
z$b6y}jFPpJ=ey&Ii&Ob!M9<q|Cc^PiS64l!O3<otci|#WL6~c_1!0Cm5>9Y;;bU__
zm}@`<VTMBz4ux}(S}4psgu;9P4Tbse84ABIzbr~9{JH#cDpL^V<hmfN!>oo-m}VD}
z;iixgY&TsIaVP$}iHpm1k_1h(Ak5}f5N0?e;ZS&&U_Zcb_kwVJphNOo5N;%4{uXiY
z4+LRYb@1)6Iokxh^ZHTtPdzW>gaB?W?Laip5=y`l_Mlr!8Bk7?JxY|*sv<Vvz!XF-
zv^)~9JWiBNzPLH^5nv1HwZt?nWR%URh&P!8HUe{><!7MR3@6l^`hbfzz#_J=z;g((
zeMk{3mG*cEh1mv**vbOmfrNK^3K;_wHlD(I^CGX%c(z+7N=Zd*bAjVPQ<TUsBhD~G
znpqLAoec20=24cG0Vm4oZ_y$md71Wni)6rw@=>vfgFxVHz-uj%HQ^L-!WOs|acret
z#XvLfvoN1AZ;t}wWE-pmtY)P)l7Q-i^N~ezzOWBop4vl)X7_bePaRzBsA458J<4>X
z+In(^%-TK{0`jf=gKL{8%>jc$vo5-hMJY$@c+13q6YY-QB92ypIS8GKuarlSaKQqi
zh@(~D0$?^+ECapk<RSWf8KR=(E8@5nxEG<vEuE24pBZSyL2@TB?^!GZPL#x<a3hkm
z#!ui!z=e%WHY89@aXo;d;@<WYw=1r=-Jas!F^by*X`;Ad_Ktx{aMO~zv*q_1Idq7V
zTqMIp-xZjBsI!6YM>ik0skr&{qnnQjJN<uxNxXwWe%>&JjAQ*we!;eW3;6EBcK{nJ
zykmq^b-}CK3@HX3`Ka}8fUHhqoS-G?mVRim6l>{_D^0gr`DvU%ml{DI^}=C!ecS~K
z<7$@E43xYvLF!EcR$)$*%VGVQ+!SPHyE!rw&&*_CPRF-|GCo3PhP+{DyflRS`+WE~
z7!0aWXL0>?VA^T9>tWm}{mRfhidabCsh~kWn7C>=FXMWP(?f}*j`|OJ+zY<7&2XTH
zeI1Lk6jIA+Xjo$*;u*rfVlHFgV+H`esy*OIiW7qgglp$|@a2U6hx8i%3gBU<#9_C}
z8D1X3*(DrhcBhE-Bw-XDfuqcBrGPC7PVMEmQxzNGV1f%u9Cw=9gFMa!IFbj8-RVlk
z`SMY>vr8}~TLsZz%)yge$v(tafxP5v1YTrdj}5$yz_*NHrx6>7g=*<<jF4ne66~s%
zDq)|~9|U7gMZl)w5v&@^z6zgFpJzMHX@>tUW@4IZ0ri!%Zv<Nde@PPr9%i7f4b-#&
zHo*6R8pljnAzm8qs)tw_W~A02laYgnY(_*GXPA6AkfGjH0p^3s7y@=t#@P%|84{o}
z7#RBs0-}*EfN(q-+1hGEm1|LB+aa!mJ>fq{;#71q_#b;=Ttqsw!ElaKajGwVjEygF
zj!)~U$R~7*@yqKU2J`gnmX)EwJU#mm|8aWuF8t&4Y@o`4GMy+*t%w(1I5$ArZTNbP
z&*^^|z%iKB7aQT|73HJWzXr@#_?D1M4aSUZpf3YxEV##sLVpIjadV&0UXy5_-KGJD
zAZ$~Dy~8y8ZnDPRpK^P^@FyRXDlbr2i*E_VmLk;>2hddvL=bqLflLG<Ly(~v)iN|#
zJ3l}%I7&ARn$aifin!G7TnnCi@b!w`i++A?AI3RRu8sF+vVl%8xq~wGE-=9sNSRXl
zBIS}z2&`ik&O1~VFn{6F{04)IcVSH+qyk^B%Xk4YCP+?3FuqnPj``hT*tpoK`z2E{
zlhHD+>2Ehg&uN=LceE1xlUn&?4~DQw(j84<Z`7p>;1X1|E8sSQ(m4ru+)#khH&v2&
zD?#a+1RP(Ex{{vh1Y*$<O_qMiK;g;7?4_nMdABMyc^vw0cj{DuvWBlqMvwkDo_5^1
z9K9mL07@^kf}POm04qBe>teisVXt(&0hYDY<zE3UjMP@mbKGL(^x=ce<0bU4c%1X3
zz4I7o<#eB<iW>~THQ}ArrCrIkVp@<$8A~aUwOnN3NV+tlFwde^ZuJSRe8m8}k=GM~
zy>!NT!k{v00Afn}7$6r~U@fu~hs*V#a||&v3SyoQl``vG0vxN&c^r+EE3Z+`w92@1
zv(B(qCIN?!T7NE5ysU%I5dIDoPdbG)e<9=V7!L4E7`g_383FLwjuw1kB*1J|;6)_<
za1_8S51c+4hVXY1Ura0w;Xz6=kTHa{R|(D-!dhzx4wN*NeZwWwkkNlZ)Kqp4ml8T<
zjJ1l(X%(*!6x-dvl2(odD8{;x;5h`vly4#UCPA^?TL~TyElm;Iy^Y`}1jTl5C%ASz
zKr!As2&Pw{MRxD4Yq3sm9Heb6RZy_NS>QB``*8R`pWnmsL-=r1O!Y}CZDXY^t1it0
z|47<OBkfBV)#+MVu<Be|FRujkY*-A|skqKZz132S718IyM~yMVso3VrDc>hVJ6#TK
z@Ak!4ruB&{r=r*gaQPD)ZI}7{b8XmHs_Oo^6$^b5MraAy%N#neEb}Wff$IUo#hsf6
zG?%p6k<zZvE3BeMFc#wMI{Sr{$Qee$b<&6(Jw(El#5U`gZsu9kpbKx3WiW!5OcL#3
z1h1MT82^DU{&N$*0P#Qg;`f_)D6b-=uhtjkWTDrCicIBX`7pT_BNfrYhZ$lqs5Yk}
z&xbkDVo?1~WjBLiZ&NwS2b^%B0i;v9+=r=r)erpc11d)L)486^Q3xY7O@W;&I$kk0
zI?xyui#cJs!5-4<FtsMbx+{*^IY^dt*G|O=MmlxU+DNBv7y06Qn|SK>d|&){6Hnc4
z^u;gK@kY0I`7jq-jOcco53|K$M7OW{Fi%+wbzAYJ!LUPkS7Z2^_looSYc0_X3(?1U
zS~`*$svM3^e`JGrkM+czMm#IQ6aD8ZKoU#TKgJ8LM#;!g#yxsxfG$UT_V74La~=A1
zZ)S2fIz~Nv_?S=4Cm#<`y4+7V1Na|;(jkA!Wu0*o07?)18L^)blpgqVf|pISvxhIL
z%^tqgkQp?-V)EYX;cGi+Ova$0XAk>0d+0X_ZvvCqLy|nh+z+^Np(&0`AFejQqZsh0
z#6LBC*zA*3@v#9oD#wsHpEPL$M>~D^$|tlkTB21i(}%tWkd|8N!<4Qzz$w0T4?BH$
z&JZ8<^r4KWD|Q&JB-jH#xIi6=t;zKn!G37BTxz|xJkjP)wokC>0?#8X6|Lc|gk`MF
z5d6o4Ww6!ou~YCwD|5w6#e^__7$%d&Or`5PJm{@MmIYIwk<};m=;igiXdM2=4TdB>
z@8CZis0xRyvGe4|o`cHCmnqD}w8b;_k;qrMOo<vP&TB3rER&$d0zXSwW<eT0Kv<?h
zO$1+130NjV8a@TEE@xB4guHS#^~(8~$rVo$z_3sVPWk@Za`Jr2-h;|{36eewGjp$;
zw=kblpB<!rJ|QgaHb>z4CjgfE)bJ?6QlA|Ke=%XHPYpi@SeG+bF(I#<xwf3cb}_n>
z;vyy*_pr8{8Fo@!wsfnq2JJI>uKZtV0ks|rRK-~PS%Io7ul9N7#u=#aLoW>d!TJ|+
zbTwwQo>rWxD3#QH52?*U!czM^1)fh>YG1>*5|-L87W}7#rS>)4V4BrRFU5pBt@Ns{
zmEP60(%aKYlF2iVbo}?WaTu+f*bn6|Kgh%EBV=@RWut>pSW%q51n~zBoHQGM&}Jyi
zU7R8w)d}naTPrMi$#XRWy*VlbXK_@RF>|I9oC9SzGcoHA&efqlPVi)g&_tQ$EFL_H
znF3EhXU`8_O#o5n^zv2GXz=L8wS|fU(vO5?1X`$=Sf}ZL#grGx;#$E;fW?$Gyq&O^
zvWBmmfl3ln)-Wn*_6{UJABm2b;kZj>L2celKzw95O~ABS1o6yrx`4d_>cME43`R=;
zkKDl;e_3&#CQ!9~`N(?g=U!3Vu>rhdnv3~_s~3cy1pTNlK)>CT-$?=PO7&3zmKgRU
z{s0I{sN(Zx<BR~Yb5fz0*8$C`jX?i72=KJcS_J0q!6I$;AXE@O#h?wI_cNA4OB}Sd
z-N2V{%OW<{ANUzDLl9Hh%&>7(nmEV+9F@=~sa*8Dm{;&64(N4L0+Y>^gq;0_D>=F0
z-8BRIG{P#KVa=3GAPeXAXIVxAbtY3TMc_pQ#w<n3Pa0w*BcJ5rlGTVN9uLr22D=&p
zn2U~stG~#HkJEgl@x920gyS%6bxLhM)@x=xJyW?(Wt(!XA1P?L0c=4_6AGGBKPVZK
zg3h-Ebw(}E#&h>zEodIA><*Pth|*nv_-|QEFK`)R@=k`3agz{w&G1<5xDTps<eLNB
zN>a_<0BK)!r3JzSe-tCT0f=#zLN6~5(J9siAwTXubr-oe0;lHr1GgY%IATstWEo3Z
z`6;n9M)*IC@S0jSg-4MO5q^yoE&(#`qVPnP1mUEC@P|R}l<J(PTH!ybnH0{gw&*GT
z0I%2?fS8jLS;mr9eo8D2gdYRbZfAs}*O!m`v$~bSx!<;&!o5I8#N5D`mIyqHfHRr`
zMws1-h!yZZN_f7q%n~4TjroST69TBM9jL7zyPMkDFSRuq7^$ssCR^yj<>MYu6T5+(
zHE=)WcmXOjxBz8NWJypcX^imsMmW-!kL$RPTH&Q26X6n|@N+0UktIPmX-sXMktiJR
z?LZ-=wjQT&R)qvu6F*aUB1=MTk;Vw$XoLqCgO3|<7g^!$78>Capzz^{IXRIfK{#n3
zoHhBh5$>5n(0zcyX#tBU+zYTauVxHQVH+igDZn|K!f^YK{<y9yJRe(T36ME?kzsCw
z0Cw48hO^vo-dkkM;(alTGEhlVXl3ZUcJ_f<1X9uI)s#u)N`U3xhM1ERSrW=m8k7CO
zM!0J-;r>Hg@;(Y?*(Jd2*Fz^MWiLVi*^id#f}`QcXSdCMJ=t2EOsa|pP4~s~JbOXF
z=ONT=#kVG&r$)m)y!g@#)2B4mad-k=F3VOc&>^%lxMGXjpm8f9Z-~LsNnoOpCr@#*
zPBYqIdAfg@!E(8Wx6nGJiw*WpgW(bx*Dxp@YZ&YJk7c4O`9D^6NGjGCmSGV1oR^$a
zafOMeqrit=LQcgd6VHRKKYPhI6<-+ZD?ohHo>mQ#yc8VCOCg72w5<5W5c7O%caPYq
z7-K4eCt0U?@hlxRFiaM<Fb+GVvkc+snn+|jrHf2#m5oKpPUYDqmV;i!Ee7DA_pXrP
zbrH)FM!p^9Jxv(-0;y*zGxSxwXLxyJ>jy7)PUV-r1S+Q)E0omEHvm)XR!pTJwKj)I
z&8fWK$6NV7KA@tZZ>+Aef@U8^kYxKQo9)W+zCpiYrBBd1T9E2=Bspt6IT!ln-0hQ-
z(d!RO_JohO@^wG(xgYq&2b8ujBhw@ik7Y#G=JB&kESjILb!TD+6V<D?H9M@R^F0u$
z@*ap&_#TK<c@M;V9-HzGNX<W=kUbhTpRPF7-vhDW9z<<e_ym4a0=vz7AQsuX`M7hb
zU;40kQ70S)%*C__9R<(g>sLb*$ddw&41s^~jX>GAM{prflPt1tk6_-luv33UXT+NZ
zHR$xs!vN`|)I#Reb_fZ!#8}6>{`R6}+D~C71`~bPpE*?(2qw*#yU+=S`+~;Y%XjEY
z|Llc;{=5HNed%8kyW!Ak_OVI4^e=Q4gZ9$DchUSq7YOo71}|a|XS8m^yeyX7@NGch
zwm>9jUxlC(?uq_2xzSAwI?0*nLc`(wUkP1}d1|;2vvvFpXSgfFL1dmXlJw<;lNsd2
zf~h%mpdo#+U}WFlT+ly*0X6cI7SIaASL9bDYc3Xy{Kl}pSTOQC!}?;u$bN?P#e$Io
z4C{*p@%9P=`eH%6y@G(gSP*ZoAfPW6#M>(f=!*sM_6h>}VnMvUf`GnQ5O1$|4LQ^o
z3*zk+ZvoI33*zk+%&@*#5O1#_pf480+bfm<&=(8h?G?iT=!*sM_6j`cnTrMS_KNo)
zO<ydCw^#fVfWBA|Z?8B9Wcp%3yuIR10QzD<yuE^ezE}`%uOOf=7R1{t2<VFi@%9P@
z9Y2)vLQN>Mv*b-*EQq&Pe2;X@#e(s-SFn(Bv0!8)gZg5@$PFy6zF07_i3N<IPI+;8
zFw8oP&^DRvv`Y|RENfF@6>3ysYa=!pK$Nt>Fn7?SlqA;+rmfkB4BB^CbOWyX^@5M#
z2{M>=&1@pxfc342ZG>&EjQtx;$cZ(D398rv7@QmHh4_F|llO%L(>9+?hHCc_@*z8z
zw(9{Rj^+&$$F#)#l4(dLDTvNv0@6rJR%{<mN{uoAVFAJ^(+2`b5g?W_09nN4lO)SZ
zIR&{(iwKbI)I8&Oq@Ps$8oIK-URd$h3oHJ5Va5N|7gp3d6SINrq;J>afxGrEXw^wK
zEA~&0Tf1r^piKe=-P$LhM@rf(P}r?KU;?16irrnzt=$I$Owz5=EoZsul$eBj3Ue{b
z<Q3H!zk<rq(Rg9SKtzqS(V3KBaCl+GG{m3g;h2w1@5fN#g%uZo;%1ME65>=6GNgE6
z#RDLF(j!%TluSQ^`NIn<UPt@~7MH<J+^I7FGyNoePhm2M)la}HJlIPaGKIg+9>gW-
zdkWc`s(Qfp@Msv7$>GBHGZ~_F#_I4r+DZDJLSY^Q#?z0M+4sT<VZIEE4;?M@Vmu}!
z>3a%=c_$cmS!SosaTw~7^gV@w-V4-EakTG+6|50e6U#>#{6?wMIxSEMN&22bLAM90
za~$n^VTGVeff^A<`(9YV+ElZFnjc5|URc3eQWpTlw@T==v!-GYN;3Bp+80Xb`eIs6
z)_B&bxu@`1u<ax(ZBkcBmj8tnp9A-O0^a|^iV&K~G5BhJG2S}8S$2I-p=ho(@LfC_
zv8%efFi}j>_Y~4LR5|dOq;~2J&j3o_Q^*LH_x)+{1btQ{Ml~5M+S&<X^1&ha*e4)E
zeMI`L6fCHuxTE~B|3U)|TuU4;{L5s(Iac31!tg)5$ok?GHRN0j_LQdZ)RIYG-%J)m
z%Z-vl)6II@%qZGdr%OZDG_j^Te<3>v$QxaqE-qBlZ92oqJNFQDA~aF*Q!lbHL-MWU
z5Nup%g8~#oIjYo@rXhVqR4qj(5%PRW9$qV<OBZL<g#Fbj_IV%-N2?KvdNs{(mEK*`
zv?UE4ntZBi*cTHmylVFrLo=Q<admnLW;SMBYU+E{4Mqv8t5Zw$`v{*<n~YX(BVqNj
z8;N%^{k>`wCUPvtyCjtU@(`JDj@!;@VVdHkvY~J`c54{UbUfT?&UiXr_MHaw%~1s|
zopWd%4V@N%%UlPm5`;_0T|G%ByAyJ+X1Y_Lo9l@eaq=USNBgs-HF&)rBlb1CI}$I-
z@WdWjn8z)0oQC6=vya>_joB!66;ymC;!(Kn$T6Nec0gNHYBtfFp&N966j~h{xXNg*
zC((QqFzA0FUiAyS0MlY&ClXCDrpC@@jV%d$Xe@?rjqqtkj5Yp40Zt?OT19i&RK2By
z#`Lv{7D}95P-skFt7wtLSzoIr^tFoiezY^5!J2AcE1E|GpL3NA-obDhn1f+<HqFAE
zD>RdEP{PgG(+7(KO)wO+!?(reY#nf-937gorw<Mc^hS6{HFR@!#lbm&;}Ls`#W2va
z66y)gnGLl$QRbjIJKf;Qz#1U0sYclwl@eSjnrM{{E_V{cfu|{DEpYB6mZ;@xFnyfl
zhrrfOo68_feF_R(XQ4h~WV`kkvtIrsJ-XA1&hBoJ2&!`o$R<s~@#G>_KZ0`+<m0H8
zS!8712bqC3B*yJE($zX`USM2L6StNv!Ra90B9(~Cp|^;7BfwO0<Wd+Nb~-ZeeH)`n
z%tF#sQ1GMH(~fgr0s6MZ*kmn-fUh8_^De}Ef|xE;&9jCpNi#X>hxb5}_W)42x8kSn
zsj$444AEPt`2rkxs5%KNJcSRL*iWG`r|WJ5wKameF=4mrq=OLMnSS91KIVLPI242r
z=S~G&Wt?8XZOZ~~TNZHJa*x@~E8w=}_XTF#5>*+dffjK@iWwQNoAlNCD5|9ge4B<a
z&>iLI0q>_FRN%P)dag!;RYO=GZvrgtZ1~SL{0Fci4!<x}PKN9y*d6~3>QuvOK^R^!
zzM=~)qR=okvWbo!1h(8%sIV_sau0xJ9s@rja5)0`;|<66T9qh?6nFy*+z|zK!w90p
zg$>Zk@j;1kuw)o&i|H*f+-+=n6YjO?J!I4S)TWoR6zScVkly0Mq*rXxgVR%`VWDBY
z1o5bGWYg($t1tc&6Av@Wf6NyT+LXYqW;*ZC&&LMWpbK&wr9sL9Nn<Ec7D%plibANt
zy@u=~Tdwu*V2;Ado5jj&i;}SNX0a|V0MK)50qSBFiFOf`x}3$jY;qdvvhE0NPwR}h
zmq5q5Jem9_gF@HkJYAO?bX_(yOkQ0s$^@I#<)vVey4=P9>+(|s@_QPNKUbH-O<kT~
zxZBw>FI#ETOFrGC*NFk9Hx_~XbqVPmMO_XsEUe4FLA<TYD}C{Int0abR$qK#UG6kE
zU6&l$i#U_pW1^q1<yhaT4yvmZZ-8LkosOLMJdSPcd}{wWf+<}s*nF9-z4KXnX9Fyb
zjMTL^!wBScc&xpP$-fg6y7tyqtG$?E@@nt0H#4C-w#1y}Mw`boKy99ZKz>KV@uzFA
zPod$YQolheR;eeL_-Qcn6l9t$dpUm5l{xtfeN2g(z0Tl7vqhZtZZI*=drG}6ODlB~
zm3k8jpp_~MlYtdhsR9lqIEyW06I;j~1fMLl;4cKF!EB<+$E}3QUpL9kG|63u#HjLH
zsPaodp;dmXR{1*JV0Ia%hBl9P4FH>{`~k2?gZY>NsyulWRKCw}{5h46L*Mhe;iSqZ
zAr-6gHlwtyutPah;<=Xym5buJ+m_$mkdQS|@h2IaRy?00=9!pRZK2lFnY!mE7@5>l
z)MjfGgniLdRLx=lT9;2zmrnsKzRXBC3wVA?z)xL1ONJU}SY19>Z6LYIFnQ+t@}+f+
zE_1;mx}3-Wb-5aW{6`GOpVOrsNOl`;%)I5J-uTU?x5=jWx=rtArgtDAy~A}rCGmme
zDZ@g|r$Y<2(vn9jNtE$zO*}iFEMI(LP4zH1sj0qvek(RH(7P_e`e2xorlLoIEH}tc
zbV@vx)&M7vf1ZqLtfe?7u6HJ2>6dEBQq)qy(lKfH8^Wg-fxfn4-40%jIn*l5p?X44
zf7E9kVOx#p{xdDK6VQON9$?B+t&pog)wwH5xY4ZY%IeoX;EUfJqcb&L=iB-ISTJ?y
z38IF|Hn4#l?JZ$zL#U2Zw}4h!*i@L8Sj9R9XchM(kiXx^wpOtKO2>R^L*n$JG(Nq6
z;{~=Rox;Faoh?r1@>?0en~^PNo1(NsOvwVN*?64ON6Cs?nH)?ondVlqE?oz&bc19z
zZ3uTjrz%IC1v)9&ceZ5p&Ne0Mi9mkEe_ygZ!`iqDiaFT;Qp`05@QS(eh{f#a6|>Lf
z#=!C+^8m6Adlf}O-6?_c0YP2*KFv%5!$H;>hQlTxb&?Vo?p47s9)HN+;mf5aLvA~n
zde9vdmmrEw44zQrqt<g7L0fizvFu8yYcP7K{$ill0Md3mK<xNUz(Y!nkOjK+@dPt6
z&J)bmQ1qlT7-pwc#1)Nl!*Yt(q84V`bcZYEA$yHZcetc`IUt?x2uU~f9JC(??Xw%$
z*nY+c*$hPM7BW^^$VGsQr;gK!jFl2%4edB<LlcY*ZCD{K#1i89RX%E>dJ)5s*wJs0
zBrT*Rru5Q6N)gDv(8&H1Ed*l;s-ns;is3wH0MBra+&(6GhEx2O(PM?pa<(noNnY8c
zee5=bT>Mir)D6&qlxq!(!v~9dY`J!^Tv&}2iGQ|SeLgnnuys6wL}+`k$HG*9d2<rW
zd*p^P(<^VEbB%a5l;$Wa#dA{RZzlPZjd<q4r0?|EX*k(-PBfe{>ZAn5_}b0_<n>9z
zubT-M*LIkgr#*9B*TkCZa>Y5+FVLDE0G5j-o_QW%82~i=F<}`1R)_(QJRk5tPNP;R
zuJ61LxYH_B&Y2KQgFaKl^teC|2pYN;kT&ntV%|>zF78~RGi3*WFB(}BxZLX^#k_|b
z!j@9ANcjr26|`ao4K6fhFpvQb0CN$@Kh3cG88bj_JAFzGBQ5$JBqHO+VXfLJTxsa#
z>A(1w@w!M|O-|3pf`sn%FQ{H5OXwBpLbVLEQl!ncNN?C8{f9+r{9hKSu!TR11G(C9
zlF24gl#jYzeaO_g+|ltO1i0ie0fGGU4dq;8k<w}N34K^fV1hTr8APFL4e55+g4(E>
zz#K&2Dl70d3fy3*F4h87S1K5`>-4E-Fl-?uVAc&lT1bUWsn30eh%IEYAp&>)cRpqx
zW3QHG1ab;5HCoVZgNw==Obi}Vv=-LaZ3^3#$<e*iHeN%HIfUFRCLO-mwuJk1OCT&Q
z;XY{z8vqZv#|T}REhR8RifBxaEA;6fhP4g+;Ssoq>($#0&tz>pn5FG(4$;dCpvWf_
zXWQREG&AyqWaRv{HY3|4BemAqjBJyPoCUbkUR1-=5W^<<w6J}SXx#{(kw%zWg+}<~
z@j6l42xm957Q4&qhoupIXb3%v-L2Y#RxI{HNRqzzAqLn8KR_Tqd4iVxXBr{Lm_F~C
zwActIB9T98glQ%r-3ZSy+_n+cgcfuoe5HDkETLDVSCs;-6e;r(Q>5Muut?Jp$iLIb
z{<B3Yyw5O77Nrqpn;6WR<fA@Ryv0x2%myaPM))!l?QSS-Bb46XG(tNBKx@j7u>qY2
zNxmT_-hc)gA~v9nhKSRyl|E)4qptyF8zt)o#O)IsQ|Z}l4XCmo56fczgyDZ7`?^7W
zFAb{fQouYrqrMlDRF?ssKG1_F6PAba@5QpW0WQr>!;1nIPnqwiUlliF0&t4%5AOmM
zaXBrUaydsDJbP&HYJdZBI6kGht$@a}$I3oiOx|xnsjq4wstHzoiL2=PJZ~`Dfq7bE
zC|@kDrJV(`gX;6WkMc{O43O$2OZG#PCTd+!9M!mtwyO&Kg0KL{1@ofMQIvIte5W=6
zwETg{v(_N?k)XkyD2~=(6PG<fgDU`w2AfFfXAl+*YWRM_qQNFEpV!pipuuuzuq0Xp
zsJY9Bz5+yw23v}Zxdf>}4c-i}gbu2zy~|CO?*aGOZF2ez3YLOAqHwv(s=r{s2OidW
zzE7V?T0!jLI%zqwQel>eL}Bz2b@GQB%0Ql0C3^;^h<j@D45qE8wHqO+bPr2;A8Wp=
z%O~KjSHcns2={jH2B}*$Ie&#=%``$~-5JXxklYlKHyHGbo@8!w4aNd&naj!2Qix<f
zTqb(^4p6^nJ0zgt6Rtuo_||;(x#dE#8&M@+vq21(Jmp;tHzV$jyk%0VZBrWO79*+L
zM!;<cQQ?<{AP+fp262z1+>8&!NI^|-D@oo3@t=U)Dcoo1WnNy-keYe9>O=!P^hQD;
zb&6}BTCqVTO9v%XvJtTWM#PU=KOFiV3QscZKjt%H)X=%Ok<|br@uns<OO({?8YpSH
zg{}rPv=@#i2oA3SnO$jt#_K`2$OBgsoLvdpIYRrYMtgAGYk|jw;d2E$mC#ZPT?44!
zloOGwlU?RBdL7`G<_SCo@Jhht!=?*-JYct~pmZMeIIKSHV}UChe3LqV^X>j#LyK5_
zi=>P%f{n`>8d`EapaqNYHnhc3nO^}eY26b7G`wpADwj32WQsO&JNZx-xxDC4YU_fH
z(4A1-Vfsyv>Nz(TRC%mZXE^1|Jt0BP7FT0YKpJNJr--}3dky&{@bjbAuYhq5W|h4r
z#km7%vz~T{2EQPLdeTs*8vwCxb_#MOLDr21Um^(Izb?qE8&Rjwy@ud-ST3r6!)1E#
zUoIETzu~SHFs2p%hI_7nS^0ye>SEo2BA_3!b0pX&-+P2-Z#ze#$#UOkI1Lp4WATs6
zRPh^=-z>z91C&iS2>iFC<23$r=t_(K0^>@Xpet^1mzz95;qp=Io3=+RUwnvNY(Chi
zQRSm12t<q;DYKxFhAnE@zHtA8Qsr@hU1mVjpc<JjoQHrjoGUyh86H+EKk74A=tuDB
zHN6|>7HT9a6TO5}#C`KshGD;u1H1m&4lrlQ+xISa&2NKnoxAT{U+y*-M$F~qqauMT
zkoS1*KC`(?xd{YPqUQ}ymgNZqUaZE`7Nk<5a*yZB#1j4L?yttP!1Daf<8h|xN=Gj$
zAN8}Fi&Dk&_K4;AhsTp`cs$J=a8Inpqi!~NYleU>Pl4g_bdVglx*AW3<(W+$siDb+
z2dj$mQ8fdHs-YsDT3BUh|6ys@pU}}(&anZWSy&6Xx^j*U_$$XSW?IHpflfJo1w3yv
z_6;*8m9yROc;?nFFufX2%}quDoe<DGI}A?)n@*R&HPv`VTb|QAp1p<#a|8LPj)B*z
z@my<pp7eNPC+Z6I^42I2fF|M<`l;m!Z-!7QPqyLlM!jYM-Y*#EX#-L*wxQ&a$|*KH
zUgZ=Xx^j4CWU`@sMHd4t=gVAFj!b6y1vr_R54rv3v(xV%U@LzDaOobDq2xeUL=Kdb
z1*uyQnNtIiTZ<4mJ-{;+k07#S6gg)J(FcGB@@U3`CedV@=t+TOq?*eE8YNp1n1;X#
z1e}435qdh2d`2LT<UFNu1IgDS@QjdC`mcr@IgpQ<8<=LLe{H2lZvn3GPD9EmK%vc*
z05(klUh|Rl`N;r0PPrR$F<ngxU$nrff20}1>;O7fx=a_^+)*$-MLe04GhG`-ZCBz@
z35A_8dT<z8q7QB$w}Ux%(BMA^Vw7JR;M}ywt$<_}py3UK7jagg;cmB~0%g*5nh1Lv
zaOq^!l}u=tOI+UV9JVoTYw$9FC2Lr{D*~L~)Vl*SDo(;mPSQEu4t8~I;N?!xJ||!&
zfFQr^42|RjtUyf(Rb9FAr*JDwg>N?H<D~Q7VBtJ}B^3t;H)t&huNAs1h@%}wI8~GH
z8evB@Av?pW7iWHd?S`pXrp`$%txqWQbSQK<&&V8vdLuAu6ygsY7%>ok;2W)-sOZEo
zT02oOIEH7WypvC3ZYE^UJdJsnkUi}b9fwrMzJjO?<KDq9PN5hls_}W4`V`~K*&rW?
z^|4Di4K)7iMsO^~mpDE#%0aHo$$<39ED7-erL|M>HpZS~Z$L{+{g&i>e$_{ukFV&C
z$e65FaQRawKrZL_E<|mWr${~*>vMKm;ISW}uH8|T#bqXqA|(8+o$>XMY5F%Zj*o;o
zOOo;9Z-m?$YsGbj4{`cL=JB6ka87;6<q4+G7%M-7GUZ2DgW-PyHDUaXkV-GHJMqtl
zm(0lsI1{ax7#}?{iF1x^gEEH)Fc&$Dl%HRT35M^+@1%0{HZ|A7%$!XApfgAU&S_Hz
zY-&5;;AtoY{m8P>Q`pa#hA4e?KQek>1>fuq3iy4UP#=FPT!aC_b(1$7hreL*NfPOI
z9}8;dGvh@Tpch%v>ID!Hpch%vSqxe)vao(hxN%Vn(8DW~!y;%8ujG9lVRPc)mHg9e
zL_1-6cqRXO6@%f1C{OZl3~LXs<lh<A9$v}&8P*<N$p;wL9$v|~;sSv7@WQ!y0@}kX
z*%d&0cqIn~&>mjNNdjmOujG&b+QTb3tY#pG+QTb36%P^C!z(#WF~i!!D>){B_V7xs
zrA`Azdw3;hs&W9@!z;Otnh4;`l<qK)<SaZrSP!q{`s!=|+QTcknK~O}+QTckJ)hK#
zhgWh30knr#a*hDn!z;O?0NTSVIakbIdw3;xmb_^XujD*+0n#xZUdiTu;^Yl1q<DBG
zZ)8w=cqQM!;%X1C<V`GX3?6_5hzp4sMy^K`vz@jM0mia6CH5K{TN|+(0YphFZe8Iu
zt<g^9;<2z?JXVwEsoiVu2Gve{V+=UQ@Em4J?q>*ly-}2yT0D%IQYZ-9G1b}!eWnyi
zob8@!^Dt&g?>EpeHl*_~W}pkxMRqt*$#@K+0(}vYxx}RH)RDu4(-4;>hY3$*iL>M^
zp#+_}b9$oZY+i1<?CnUDAMBlD{!nj`AX)G`FiISYgN>>=5nm9R0$?nFY_#usACj^K
zabpRdB;Z7VRRHUAF4;gHCR?*@zks*~c<yY&X4UL202sam7)W520cI0Gk~o72k!WEJ
zW=}<;?FcRd*f0m>deY>49+FT6$8|;2^T1_)GgBAkTc`=2P<R9~6YVWQ<{=cOn7Wgh
z(el_VP5hi!{2^e|#V=>^_d}!<znn$Pzeg9poK*7w=;99}ZVN#v{xBB*BY^ccc*P%?
z!$j{zQBeF*%t0jq6kmWf1W<edwh}<`N0V&KeE_8RV+cMBP>O%H$@yLuzfy{ybU*84
zc0%!)aHCIA+I0sFu$Q6~jcGRrp-ycw4$%h=G%dp)1UBRN8rs4p)Qsb6Xe)!wY2uzm
zH~*TDHRI;{30VV<jzKP3&?Rz1%ZB((34CeB*H-NQQ>FWl>Fz%XbKs!Uvo6?m_dlt(
z?*8wJL5TS@X@q9R4nOcF#$+d;JN$s|@B_NT59kg*prpeOxJmy)7i>EGfOZ>p+2IFQ
zPlE{e?~M^Wi$O>$#7L2Z4!??^3ianO!VW(v=SIZn4nMT73Cs8l$`ksDeWC8~L%&|Y
zpmg}5-x$^%e&~0Gb%!6?&#><BLkAev9exNdEC6(eAA&y<0Nvq-Tmf{49|{VfJN!_R
z0J_5ug#^$YekiOekVD<!hf>uP0NSNDlqQ|G?(jn~0d$8SswF+F?(jpIsyA|=JN!@`
z>HT$wAIehGAx*pVhU%+R0q71t)J&ZU9^K)G+N<*bXqVnl2LW`4AIcFxcle=>0_YAu
zlq<(+bcY}6EP2x%eke~ZM>?j%519@>w1I_`4nMS!LEYhpZeVeBhacL+>S2f98gZd8
z>oCHqWwz7KM1ZlZO^Ln8#<Ih|6hM@;QOb~BKY7iG$5?0H<V&6eL3L+d@y$)%{4NpI
zd~@ZZ_~e(9U@=E^QGD{QJdz%b8?W}eb&L5z^uPRW|MI*2%kTF8s^9G~;&<zQyFC>X
zMEc#jH!Jorj=m_~-6W8?DBj&Hkhv({-Ky9#nTz7xTcz(b7sb1HemDW0!wh`WXM@VA
z&4gU|-JXW1vu$(+B^Vt1Zm&T6tsai~$nbs)75r`=1I61O6(z)}BxFe8cl!m%e(*@=
zOJel9b>Vj#2&0F^SMrxGL#kX9?`pr>B_LKUfT#bk&}0a|TomtWzgu>gYB2C8do+y7
zV4v>$;iN%lOfHIdwco8Up9{v@j+U8zx9$M-8<G&_XTbQ?(K6HT*42Ku!tADCz`|Eo
zk6aY*YQI}SHv+0f98JGl+*q;$vm(}rDh8@Q(JQD^xhNjjjig}WCg@2(O^c)Hck7<0
z0*wuN1yE<j(e%4@XR7^}@~}45wLsk%N7L`tovY|m%UV*80rixJmW$%u`Fiu4j>I1m
zMVr$qmy6<E<9GWra4zKd@$|dJs~oumpy~kEH~~+;TX%)Jsv+?C!1ea<V!U!uysQ0g
zMROIvpXkwuUCBl9uJ*g7ZKyMV&mgtFDBjh6w~TPF@K_1@tVoQ?GbKS_p1>hkUlbqC
zzZ8qEa#4KvzDrmw`l9&o{RGTK@!<!Vp_p5bugeTSL?GR*cOCx14-?38>%E8ah94nN
z->r8yfkz2syK?V)c!262ffDddzmkdQ*g<MIVkX$w400G$nVN&>B_8TJ;o?Q{;o<6B
zL|<xAx)I6+zG1m2{w9#hz4Q-Qk__RNi{is_QM{=9RiNMTNF;5!C_XF~#S6*zK>tRP
zTOc#zF6N>B-JJHzMe#zC4(mM*Uq1;iiVw?0@sz6af$r^*=!7~P2NJm`UP#6RJuQKR
z7sZF=qIe-$4m5A2mxT01@nN|rUa;2yvoVgH+Yr2RQM_dGVPO7FES0a-T%Q-khvlMp
zA$cF@FA_+2QG8e~iWiatK-Yw|#HAQfzq4B8qWCSe(-xq~C7o!E*>mdiqWG{}6i-p^
zAfU^wD1A|UST2ee>`A~ZuvlGra#4I(E{dl>_Z*_*@}1~2XaRERVat?)D}nqL#XIhn
zV)PxXHI+TrNqL=|Flnw@r@Xln@i6H}=Ky$X7XVmsS}y>*Scq)5UVAX7>?Y9EJ@zgD
zDp$Galfw9>e?X}^wuj0>OcNVxtXLQZsZNOQ;h{buSEFBgL)HkzT&fYkO|*EQDXZh}
zMZ$EDREvQ=!;)l(2r*?ft0}s1shO*QzR4re8Xt;OQag-9z6q<xfPRi7|ANd6PW<)N
zkg0MtBw_U-&|iBbTFUjcAVsc*<RgJ{F_=X0)lxJGuZB#Ot05&X%>Z^G2~{M@FsVI_
z2jbKjw`H3=mx5+Obvh=2$9-me$xWyVbqesyJQ`Mo(|C6=&}TdgBDD^vYpSE`uLNJ}
z`5o~qws<elTt}A-JM}*y^Q9^H73@x6c6rz)dtvV>e|rR2DTy0%rH+0Eg$r|vm&j>y
z1$u>)jgyg#Aq@yAUt7?wI;ohxs`}1Ib^8H5*pg_|SDP|X913zz0A{Ymn#w*8y{>xh
zhIB(>-H3H|ycV4y+N=KrhDG(+1XP>IyBD(P*pfi(G}w*_jrx62wxOdJYyyooWGBjk
zxhz=nMHiq-n{ufzl6wq--oi@s2cVmAsW4I~2)oA8j7x=)B8jsrEzP-980mdKy0;B2
zxK!wN;h5Nxe%pB6@yXDvO#VV)oa@2kEeESsVa_#MNjN0oKrna-iV)BX?4dAiF%)Jm
z&??L!t(AoFaEx#$%)zWRN!S~-LBw<TZ!7mzK7ddt%mxq&Q-w%Y*x7-!l5i-@a67g^
z6@X$J<J*2SG{-;(t~}17=$wvNIGZbN!im!Ob7h5d5QsX-3TJ(p>~@kRP91hS(^&SH
z1u+iO{HXOaz*B{97n@2uES%;2VCY_h=JLXGpjmtfIPKWQIBcu;;OphBJ#WOjlC376
z($*EDUo&jA5y_8Q{}wPm;#)xWrAVi+lc8bGyXtcxM#A~9_hN$uQa);Z9SEbNu}HWF
z61ojEG@TI(2|PzuINZd2j10%6?t`A#jZHS#ou1S=;4O#E7f|KLJb*wN0&5WHUJj-0
z1iGjWVro2y9(Ji=XO_3d6YX)0iQk7rpVslg#ZGQhC+~S5#_8$I)!J-rl9xosL$V}#
zDFO$P6f^O>B*VzfzFw^jz{!7CoHy*$mB+oh(n+Z+y{Mlz=cl{UZ~T<5dhZq~w)+7V
z=S)OlPr}!$J|~~$|BX5~f}45Txd50u@hu?Nhsa;yG($s;8vQa4IS|hg%Xu7xJMs0z
zi{<tZd(Ee)kCD(kBczF!(2u}0$v{Hn&w2=fXAu~NK;fl_Op!;2;MUJBZi69s%ahg{
zl6Y~eXV~XMP;XvarZzGDH-LRsfweE%(pKiJ;$dj{-!>&$e$A70U9nCv8gW2u$I;ih
z9s=&eH(o*!Jcx0cF24%Fyke{u8W}JN`Ka~p0~5hKrhrm5Bhi8_2<$<i@KQreHN-V*
z%E`fdgCS_+Nox&BOY!x}S@#iJ&f^(h39!clhK)u$5s{{)^m?*-dT2eUNIJpjLUiW%
zu*1;&=p}hAB)nS3m*gEtqu*{+s(Oy`m5%~^jKk*h)N8B90FEa1dG>lA0PGQ*r!~uo
z<j*=oEG1IM-!Lbyfx*+*d-OH{jlBot|DnOZZwx+flwZtgJ~2y;m_CPTH%{K!e%4!j
ztlJH1@58W4lY7nP;C`PN3~(q;XSz=8FwysUO8o(1U&-<*wd!%J)X%8Y`cDANr&2$s
zQXc`><98#46Ye7LYLkg;F~j(*i59yWfLdH4#3NPT!<50v>*r^k>0{kySdZA_{Ai26
z$S20>%;~{86H`YU0EUHcAh8o#m}kNLw%L;Z&XPCY4)AgisQoN?ttSD_gdu6reF}=$
zVTAIrq7+X})fIt4!m5Tld7t@m(<61h&H?ptuJDXh{SK>-3`4C|>0{ktSdXBOKx!rq
zD9iDgyna48c@um<k24LxwVGxqEv>x&Ve$hT3#6ubLNE8par&qC!V5tU+TU6Pkpo(w
z{U3y?uS$v#f8fBtj`)L8262NcY0DH0=YzPxm9&+?!M~&Sym{E*OqjB_!8N!wAv^7g
z4n#E#>4B&XrGxOB64+}txXSF22oB8|=Yh;AvoGdlC#$1|-iUafryxN_XHq%up_2$Y
zyz$#?gstwQH^R0f0sDzG#k}sXjj+C06!~i-?En9bu)8p~(r>l>QyXFOl32I+CK&u*
zn^k{pR{gbE^?!Y{YATin{`_VYdym6zRwZK?j^dmC0H5Ey&8mi6u&~h?ln~#n>WcWG
z9*)}{@olL1X4Pa+aLY>si-b6p-k{KlVYBK2kZ}i0lRhSi+0ClWh`-0;4&JPK3dHIq
z;ALNoWylbI|7Mje4E_r=wJ`zGX)x-rI}DD0vr3rTg0cK)nSGm8!aM_v7ac9LZ?j66
zH-S;M$)tMxn^l75M%YVMwv(B-S@kJUUlaX2b$ZZdRRF_z3cd!NxLMU0sFrcGZ?lT_
zpn3r{AdXJlteOnei5|MzW)%;JuOUk6iw5LW+pM|<xJ?Oo|7O*rz&)FQ_it8lBj<Au
zFUITNtP;%~06rDXN#}vnf9Ighs%F6Jh2Oe-NvzqdBD>qwV<mW4Q?i>?9D?m;RX&tw
zHmlxewb;!n0{;&;tM(v=e4B3i$4ta-R{ew+h1E%kHOu^iF#`^lDZb&czK8l!xXfl%
zTSVtuly8M!Z&rN=QZ*Fl@s=b*xP6;dqVoAbFZW19<-W}-A-Npr4J7#yGBbW*9(<cs
zLh=yMPbZKhY*xJo^yeOlPAFlsYCq82wABe|l7!8wMnJc)BnNF)^#q1bGa|sLAK$E!
zOilpiBx0$2t!Ce5m5{6g`ho<K_-2)m+z9l&)kut}gw3j#K=U@~L~FV*@y#lVa=!!m
zM=Q#|StZys<gyOFI=fPNiJMhzh>pv5qBCGgW=Z`m;2P@Fw-pF_k8;uOfR59rZv|nO
z)Q{7rZzaw?+hyz1x7WcKH#E{q>TJ1<eM{=n2ihfdHmN3Ij)+ad42L9q=p}XLwMm!_
zuSuBUkc2~FHo2y>S~Xk6un5K_xMrKPb)Y#G#b=8$qpZaivJyTR6fI>XysNB)x0ID|
z9d=sLy46{CnTKXPN6JU7pAD{;@oi1n41D+$mhgvyK%K>Ve6ya$O8yIm?Rq5E?mGi;
zSZmK^*!En8ZO>)c_FRT7X^67{__XIT>^betGHjF;6lJq%#a5&)aTe&M2$WeqYW>eh
zE{4sZ4mK-lR8F8TX_*l$d$yt_ssF?bXRgP|^LFF#l|oQc!<g&FhVCKMTibje;521t
zqDD9~z{3qNPqEFsCH;<+cMQ}}ZWytXV)A)CFi+r{OZID#Qm0XdhPiKVm-IV2$Vg8V
zZ?eHwc@jPX-HljT$YssG4GFn*5m@+lER@}4h+7-tiI(g)V1=y9GbqmehD>VYT>~6d
zjm)z((q)lhW3K74g`9IMq03qm{}S|ev58-dioe<yPg=w|P347NxehnN9>>?yq|=cF
z>;eTk3w1?9ntasyJ-`I9gpo_GGf<pPnTCccGYX_><sbnL6u6}(9rP{m^%7tk(97!`
zDQgB&$~IEcy_C2TvlQQ4vOj{9avLD<ZzkFKkV)!g=REdDDyoVN&6l3QYry^mzMiGE
zeHP=7EQCiD5!V4OU;zuU#IDcP{*I`&=NeH|YTNgG09plmwWb{U)F9_K$T$W|Qt@)?
zRd&W{%8uWFqz2kW&LXYYBrkV`AUX-(TuM3PIRugbJ%T`|E{2%eh-+9Ew-pCab$qcQ
zY2(Q|58S))^-BA1i0UdCI-c>}pGOssHf%Jb#}R4FsNIz6DZ;c+rFxy61<qryu3D&h
zhK$z4k6M2KLMpIz;B`soBAISbXA9}q)X46ZXOU)c3Qt_H&{6FL{U(SDXUE}=F94o1
zW2(h31B_0H<FvYVuSvY45v0~HL&9(R6Tb)aZIWPJ>RR@=`vG?U)v#fwRX#=X#>nf5
zAM_$xinCbP%m<!2Y|))o9GTVT97D0iNKotdA<@fVxt^})KvJ7FOn5T|e+;l7hT7`^
zP29$qzApiQ1Azdi1L!ium+nx=(p7=9rJmc~ZqU+cylw#7X%ro)8^6VN8cr+D9n_<H
z4EK9p{_jV64KNk;@_!w2)J7KX<*ZWemy!Q+lmEw=|8o)3W5!hY+do0@Gk`^VSl(>}
zb6(+C<13I?7_lLz6=x{wo$aS;Rd4b#(gT^Ok5=YoWCSF&k+t-9DEN7TvX(Aj(W}VU
zD&(uEf@!};l6w$WR7voCf?opcv2e~jM}15%@)}gV%cRRHei$iM%UzexeAr#pu*=Z=
z)0gNxOFa}Cz}7>LBh|wmuO51Sj_mEo)Pj%)`Ka~W=qhfBUi2VTSJjK#*ar^u&cz@2
z`ovef`@~ng`wl>}^rlIDClj)BqrRN*RA2G#cP^qf^uG+hDS`i(74HFd#haa-UfwRb
z5gANa-rjjGm$&b31et0((82o$n&s`zQX2WmX@R+%K7|nADtGA3ZixS1`S&HSo(&P=
z-<NzAgFgShD+y|M5Ap8{=iGuA?e3AXkIn<)?ve7-G(>B6kCb08VNl#XQhsAtyL+Vk
z&aig(NZHS@cK1j*z_51rNZ~{Xww|)9F%+l-(C!{7t^nHIBPA$+cK1k05<t6qq=W>}
z?j9*&Rf!yGcaM}*brJyW?vauvx0`BrkCd1I+TA0imRf)uXm^j4Oob{i?j9+1)EEHT
z-6JInFC?+<9x3(J3IN*OBc+*I0W$6Gk<wnR1EAeKQaT8r-91ur1kmmtDIEpS?j9+*
z9OumqZYiB5Z`$1>B~P7&bd0-4igEWy*}y`IyGP1K2DQ6K$_*^8cK1lx#Ok5DM@Phk
z!mPsxtCrbLI}ZWIvNk35G8;>GkE;PhNgJgMara1Fv#p6;<xaiic@P}J-6M7LM?_R}
z_b^wX-5LU79E^8m?4=-t#;(P-kBXg%0n?5BgyB07`xkhFvFp$*lVbnG?pi3eJ^;Im
ziGx+FeJxl=><+}F#-0OxBzALS6eQLgM|7gGC$N`TBlbRI){F%}6N{aXc6UsyHd<D#
zn1g<~cB~OnPmc`%HY1jX{l(1Khmcbzb_$NLWX0}Bk5D)EDP-1*)kgT(m}-JfG`71T
z`t(=_<gH<hPg2LlzDEADW6wf<qu3~f8^<z`w<fWFBPUH`GC_-tL~)zP>fyIVtS`#a
zG8PPD#QkfP`>$2*|J$qFN#D*&HLKjX3XT1Mamh&9B#?2*NZKrramm0{XzV@ADmSh|
zW4B^lGI$kQjbyY;exCuAUZYNeOGYz9<=E&9N-#LMWb{J(Xb;DHWOzS@3N9JbL2-sh
zMG0{#2^mtXa$f?nYdzAJBr&dX^D4AE5dW~nr8D!6xMY~C&~}1Yy$<{bmL@~^WtH1p
zg~pCi{RDhH^aE1P3`S+J-}U`)VAmOwRc><?nlN_)<Cvpm<|=oRcF7Rtd0@QkXqkOi
zp$YTtV7%8dJF?1cu0j*^%Rs#zM{|`sNxNjQM$|un`kv@r)Tyj;o2$?Sotgr}z}KL;
z%AKTLG6dZUs19+o?<zFbrYZrdERObFg~nP^(}9}fp=FiZUWIlhqAwzfHm6lCtK9Y~
zv>SlCJpu2(3hgQ2UP-`nl{?9}WPFV9w;o=MS5~>rRcNBQ5Sr34`06}}UCAo9xeAT8
zq1pm3rlMUk%vES)cYArP1btQ{M&+53ATUUB2-d6Ik^CMQ$z+u~a^I({7QM<Hxu1Yp
z<&Hea3>lY<$U_8-OGe~j0>&jH@(2Ork`Z~7fN{x)3{YPnhu<=nUo#O|<&F$ezau8h
z!I-fb<S?i*m4)ag9%`>}ag{qVTy;Wp4~x=`P+T%1;*#+bNY!Ydr&y8<;g(hIh`3~k
z%9jFtmPaCK%PMz7Trz}YJ<yv;@*A_~HbwSil{+FX8A7rR=ob@6xXK+7mkdf(9|QfZ
zN1_wrDtAO&GK3@vPj4}NbwZkitK1QB$q<s3K=aNKA<?Vc5pl^7Y=2;e#IaoEj)+T!
zWO6DnClec2v#fGQ#3e&W&H?(81QM=tN5myVNNxf8k!mEm#_DsGJ0dO_chXK@2h9hh
z6RqjOaFshEE*TW%?gRQ)D@w0&N5myVu(e^54e{04mCBP<?ufW#P@pR=A949kbRI0J
zVd_A~DN%WW=h1cWdmMipbsYP^u*xP{&4b{xDLjno6yr&+fl$V=tlio|(lwy20L7+*
zsSH6;Ks^W~Um=w!hPe~9WHh2w`Dqas-7lcN2UiC2b$CvZ(T+@wI<-MzM<stT0E<?L
zP;M;P2mC4z0=iR3p^*Jg3y@VLA2SGRteVUbvI!tLAx^fJWJ{AD=<hgEqi#$I*;ydj
zV#zW*wPr96x-cw)uI<Jt)S|FGNS5kqL?!&t5<P;-P}DsY-()4`+@V^blKP>>N;Ulk
z=yGOSWs_1lAj!Ztof-Gl#}?9Pk|scRI+(<1#A2&s)l4YK7MrQ6ksuyVhC3*v5sR&|
z)fM=&#TK&DLBi#0A^W3Zf7A*>gzXNpRL?Dn{aRnFHK<<{i#!P>PAK*ZK<i@Hk~g`Y
z?Xs3JE1FJqt`R8BZ8igMygkRI)x_LpGf!~TxdLPXz?$880%W_*7EhUtONCt4N=rAJ
zNvjg9vQEG^eLf<6dRzj~sZA0$%~d?kyW(IfLr~LId=c402NP&35W`HgYY|=BRWE>x
zz6*&Nh_{HiT{kc8hy1j2MO!L>^`_eRCU80}YQjL=uI^1RYgoQZ)v6p2=2>FXK>E97
zkY^f5K*+{`WCF=nu>6f$Q_@s-4wQW|Du?Yz$mkq;o+Z-;-e@&j$tmvq0XX%}{>+Uf
z2`xRq=$2<h^6Ex+joTfWGa^GmMqjV@EtwJ7pe<-Gc5f;7Xp@EZchE)g$S5M?I(8bR
z!P?wg+)IafCL0z8KKqtbXOQN~9Arp8(`XD?KXjiP;<2U(EB!oHADQ*Ec3=$!7D1V&
zp`;4yO<;SaI;$iY?;pJMO@VJ`f!hX_m3opQBI!Fwp$mtvz<(e~@-#f~b39LJBB=mU
z=6RAt3Gt5Wh%z%+t4<@yV%OkJgS{+9MQ{~_{{6oa>@+H3cYS7XEzHa)u!hL|4D$9;
zCL3o4bLV7!0NuEk1pC9x*rZenVr$|nG)|)@R{?!<@KI2)USWkbMN|$*@<=9z-ICiM
z?)|}?aYKm-*$7Kw=X~t319w20nmY~2%S)P4<t0r~=BHKrcIb1|GIrJ{VF{&aOTebX
z`j9iKkfPecZZ-+k10Z{nNwCpn+=n0MSV62(^!wOlQBLc0W`4DzT=flpezqv-qntKu
z<7&;@XiRM4LRK4V#C7o%;Z9qQf$IF-__Zxus0u;RlT?o|^|ox`>f&P99gD*TE>x31
zG1F3M<7~?Yt}feva@h7Qr00Wdoh5bJvVE(oo{9?$3DsSoc+^rEfo$ID`m)wu+Q~xt
z9?1S#jnvSyb*r0e<JVY8Sm+~|M>WG&N+hIuP-1~}<8I5wt!{6LtQpP};p_*lNk`^n
zJ5u-m2U@gETR2yOYfE)bN$#*syDi&|+P)iwHG(1{=y?eF{!a;#9KF;TwPysSiJ)UJ
zRUC+~*214}*gAvnoD8Eia-$-524t@PujD#y*{;<W<C=LQYlz6lAnrMelvY5!Ic?c)
z)!wnFA=_+)^b3%EYg2dHve~MA7sUm}ger=bmxZq`J0ISh`d84fe>(;G7HiD`I?s|g
z?T)3D9@tN_7=+kzVwfAJ{V!~_uIj>ipW;P34bw|FQTdz8RvyM_`h<x}4WRuKm8Jo9
z7j~l3Odw2DW@4h!JYXg&GcZwUA%K~vv=qQjR9XehM1@JK?~!f@3!&+gkxk!3<wWMX
zHc8y=K~)D7Ee@tK1WyN50gzJ;CeY1>Sw3h2E~1|cs`=o$;)tBWE)($P8R%HuJwZin
zsoTKy*uk7;qVjF<O^omC#f11B5PoQh%|zvw;3Grh-I$O$c+3mon?9T6*Atbjqy<oe
z?Z<?y1xRu&neImPM5S3$+TeIcCbT0!H_p;Jjbx(IHR%GRY($2H>@<*^ZOM#CnW*$h
zx_XF5n=G`OK=(j3TEid{mBC3{hkH#UEDXCq{C#zXLrzrAO8N@<G7U9FSZiSwzAe6@
z5nZ2&tTHKhJn5(SsFy0NrC?iJo%P7QnVtoFlJq9@ZM!p(^fyS^{--2Ko>D?%U<0;4
z6G<OIN?j};>f-$Q{!C}_mC(jAPjFNOcZJY7|CL}fQOQc~7?)W?WL^w;S5YP#r+=c-
zGI<!xOeQM#0RNDsF%uOWH69S}@kG=+Ao-AFV%U13GAMatU#}D~A^Xje*og{bWhU0}
zM?9w%sMpj4tY+|g9DndQ&!@0#l})tbm4qYlO+O16_Vp&`vo+Ngk~fr^28u-oQyGG{
zm0APjEe8{5>yz$4e)J4m9?G9rZBZ0EBdG;m^%8z|Qph?cE>EZKt7<>ml6g8kM#z2u
z$$#Qx@=*SXx-xEowS=rL3R{G)Nc*Fw(|=PJ!Zw5~)s=y#)9H|&qdJQ!S2jMV)1Lyg
zzti6)>+-hK-!4#^TWc1IqMj58kEikw`jnzhvfON@pk6`ZA2S12AktSG>#301LQ<gA
zzd;ee0$2i-A;6X4=|C17OmN8BXuTd?kDDVUQ>9><a4@5uWx9Fo(bcejrX8uJpj&Ba
z^(@mZ8U<mtPKD}vP~1$a8(0LHWxC~Gq1o8pS*V@?#STlQdq$aMx}#Ts)Xp-6bT7z$
zvZQ*J>7KX>ZOit}LY0{f1HxC5H3Fq?p7Uxyk2G0Gdx30NHBv(_UGw~wrCx&z3;ije
zy`VaMLiZ}OOt-2AYTI_q!g(vWo;fn7bb>dwj*kYZ!uc7vYG4C4UL5ILd<#s{P1BT2
z=H=<^?%{pCEJQ?5dk7ixrvyokw$&Nv2}%<|r$We`o}mA>S3Xq@jSoRl5xfIJ`5Gx*
z)ITHG^vZXt_v59mAu^jnUI)r#v(mk?em;Cy&4fu)Z7}PaLR${H(KdZYce_uj)c7D9
z6S4)C#CE&vJfs9scCRvJ?ywQI78uK(?{<(&&FJdsPz$rlz*<(?Q^YXfbhsBoK%gD1
z{UaNxt68IWV?r9}PV}$U&`yVc^#JBtQpG@{rRDIPa$4?bi1ZHNbmS4`v_grq$8b9C
zQ-~{)ID14nmq(P-dOvawPUCdq5#^*V+;-{AnNaj;yk0>X+Zv`7f6iesoz9$?M+YuM
zhG_bC989A1)rE(DlJpBiWUErY3_BF&)=FNOJEM6L4oTS6FAxcZnMf$i#p=8;cV6-&
z9FlM-%srP-m{01VFrS-4;d=PZ&kIk27I1D_eyOHVnB@)0V`Io*S#1TB&yKth$kF)k
zDz6>sC$c!O=7l-s51C)C6y$}Y5~F`<1$kkP5D+fEw3fW^SrX=NUidr-^EWSiv4r{C
zjnDjnSCP_3_;%l%Z2~+)e7%coMcJ<wU5_NOI2`yHK<#FRPy&{)djYhN1(Xxzuo>m*
ze2*8=%z`=V5E&Q>RL}7kZ8Y4o6DC~29IC75qcFf=P7bJ?Ze09R6F|Gtrouq+3@!ss
z13|CeEy3u{768d`Ii(vX;_4F6-DjB?AX8S7k11OKFmY<r=TekD@HXiBE-|g8?@}i)
zj`U6+Hk`oM1}P23>BA-ysMTCEv}1jep}hn~4jtkIIxy(;VJixBG<4nL=psRP<06&n
z3G@_vMd1we8wu554T?0q`XlFO1!prSPLyj;-8f-X3qf~<Wnw@xWhHw|^^y$}=Yf6L
zUITZ6jwO_K>U85{YT$9iJ!b_m;6%9^(~S?Pfp-CaYLPSwryCzn13w_{x9ag+N(!W+
zb*AI%*^bkVvxGon#7(#H5~yY>vw<qc{Y*tNFDd3hXdRl}7wXZ$#f~bLiHZ|t7b-SY
z#9arP;+YU|ot1yEDH5eQx^W5_xEt_KmRthWvhyoY%+A|hc6P<Hv)jwgJ0?4vsY!MQ
zh}%LFba4IXw&329Oatmiw*?b+O16XeolaQy9ELs*9~3}7YJGq3mEk*(4N#48N<PQ`
z?foz-aa<umD>kI3fb1HQvI(kDgDCVLhAL*MGL%RPEDB!_zkg1E=9!b3SkE$o9)XaZ
z_zu~etpjRQDeSGB`G$W&lryHmoE2>_tWRoA9BSSNwn<pC@UfQdMS~p*ei?6iy#`>W
z0j>v7&IY#42+FPnrA#V@fMI(O{WS2-a1#7v2qp`GAkW+(sSX~N?KrEJYQ4p;MqdFL
zS#2C=ge^n%PCQRE!~Dga&vHCLa0%vc?tGH{{dIuSz~-~5Y<vSRo0xy{sldt@JiKfj
z5@sP5l6l^n$T!=*O?MBob!v;&$ae@0;LEoV_#OD1w-NXefh7n?LC!Hb%|;s!<Vr!l
z2hPabYp~_Ox~H9P<=wf<$a{|g%1hquIHRZ-=QLgNAZ6fVWR3zJ(*@>nouLRZ1|N6z
zV~{(B`Sgywj$vlQ8o#%vq~N{-a5x3*FalOFQ)`aExJ?*O2cu-|!$W871q#L|$|=;~
z3$mfb(~Z^ZKbTQ>R7q)DU+pb^7cbubbSN@VH4VIK<jBxq0<1gro~^qo*4>2wOHVj?
zHn67d?n1^{cbAei>3yRYn=14mANTU_A?ygn(|VzJsmZfUuDSegwEhBQ8>M||u-V9w
zd*u<NYNb8)fl=Cj4B!99-g}2<QFMRfJA2>h39L#0BPH}8E%X`^Ak-ivG^r9)Ac!D>
zBB(S00Tl%+Y95IdtPd#IMeGd)yJEv$QPfAVA-~V(%-Ov+H&OY1-}m>&dtJ|7*PYCn
zQ|HW?va>V0arK${zBRIlbL5J1t)uo~V!#W>o8YTg+J^jfALy-%9;J(V?(w5tz>`13
zIue54-A<3(`myi7?eyQnpTK|kz`}OUpv>qo^9C0xVdKw0*qVc@5Z3z~Qsx7on{&Sa
z*ZhDK-;Eo6*<esU#%ZCu)*HgNP_Yu8pvq&vw7q};)xHKWT`|vm3qV?);BNgMpj4)S
zi+=(*4LSw-<7Yy%dntI%?`TCFeQ!|zIOx*cXSQHQThavCOaDZo4ib@rT7^DgF$g0C
z(in-uGiHkUOvR)!2bWvVeC*4*9AqLZ7Z19facm|<3v8_{rq-6GOKVWDk$n#e=?uE!
z>oU?pmLMT5WR$YRRrcrn42?6ELQ9n*u%XDz@&zw-Rq*FbuvaLGfKm8XFSN#B+9Ma<
zEc~3$_avyqp!BHnjX**Sx?0)d47ve$72If1oZ!)>VH+*!&;%8H6%#b*10+On8)b_V
z+$xdMq?;;R94GjNvwgX{K_vz^Oj5b0AR%(kP_8(+_XDq8>Q_p!oz?RCTafuTjnOvo
z$;m4Hr>dpD0K8g#P$`1?`A_v+^~Tf`wff&o&>Q=au+`J#JX=_Q@OGP;GM{f&s`8aH
zLB3~^kkWQkwz$%M4ZI5eb*U8`)Vnc{`GSX~so;f7Q1ER?h+ynkg9Oa6<!{iR{Q|re
zG<k)k2rS-tH2NC}@SD?B?#oP2?g1o3Zf)g?lbfAEVG^h-lp-*=lbP)cUI8+x_j{P2
z;17@x!IxDnII9{3k7Uc)x5^qE2>#%2>fLt2H-lTG|Dn8bX73T{Xl7hrsJPI|PfxHu
zGv(jI?u8vfo$&PP#u+c;)LDkT@Hrk}nR8gU<q^i2z;1Y+%pQ9j3AD^X<{5Ux2LVq&
zM?BB4C!Ti{A$G8qdK2LB(ivOo(`o>Zo^hVcKU-uP=LG4FEj9Y%Ax=%q&Cmm%E!s{5
z#Y9ZREc7`c^uZRYjyHHslup>f*|h;nFKpqoqm468dRleDVu*InHI3V&b+M}rCKl31
z(li+|Z&31GAdlGomN@M|+h1fo>fmV_-dKGmgh_m@hc*d=NS21dR3t=TcV#I6i;lrR
z0;jQ6qF88?3$G7&$rn297!|si3C7m1NJ#CEQMS0+?_VbnJZ-hLG-wQqO*2%ySo<i*
z#M-Z!px|0{Rq%Xeixa#6c$}zX(5{FVyv&HuA@x+~l}u3R`$&kjS5+;vWqoU65tppC
zudzU1b%ZJZ7tDPVKbfcV*2|16mC*aQgr1o+$>-^<JdB*p5;<>+=pwGFXRoz_5ub9I
z@}D6zv!gZwCcJFpoY+d~PV!F;XK*FGh#PcD6*VOg(G^@{@Dtm3Z*N8pS&Ry1w9b*c
z`hk!Mc5blQqwL>Cd<*7*wN2+r?4>gAzfE|}2-^g2bJ-*pLy~QCx4CR{=QltrlE}YJ
zHes!6Xq+M2sT{?>!{xp6;6}FLSm;ASXgC&X*8~lN73rJBhq~@2w^t{Pj{86$(Q%Lo
zR%HLC8q1$)Nh)|NZSM@;0#0N3@=L9!gYN7;_Zna5gUwW^o1;PpAtBLGp=@y>w+VO^
zT>o+_IMMpgai4YHh2X#GEbSn<q&fIbW?8RN3A<R<ms|<hBac;+<}$%@?nXjPx?RcR
zOd8$7+EK(^P<yVhfU)<unBfjVjh@k^Qmu}c`+*OnR*!3mdX%pA1F6^Z2}?kKAocnR
z;1i?=`#|c|3>)GcZEN*IsnwHO*;=*Gn$~_jsv*T7c0YBGf(8HR8Pi5<^iyT70JGNU
zrNC)h-+rYnqu+Miz3$(Lwj<PvTy^4ZWsY;=Uf_8ZaX*-|&H{Ubrg0g}|Dc;TPPVLV
zZ8@l51WJrd2E%FXe6J-&*c$q^Cz@D}K#vN<5y(<M-VxL?0woD`X=jc(j+-RA0?g<M
zGX5lqWPE|t8N&A|vK^&F*p43TYCUeD%evWiWTDmFb%1fT48@3hH!ZTfYn1~GHOqM5
zlUIN|Vh0%Fbbwi)!gf(u?Z_JtCc%1b4-M9XOfXna%2R=>l*KzHsKR%F)3&o-vB3aS
zH_`^~6`>FJRH4y)6*?FRX#pFQEw1ve0bT{qyxLY>Fu+ugtn>vRS)hW)GC^x^KtcpB
zRJJ(5$-O8|2AGSK0)4k!?oko3_HB?zzm(8hg|<aPgs!hz=&8W-D&qb@r`!SugHZbu
z<_^fK)LtdfT1q(V2eWOSEhThb>VwchptY1hGbMJSV`=*r`eD{eXg}?1CpxVp%&zZ8
zh(Xy(B;PaufmV##VxYB=K<hoo2AYMADYAiPq0GU4plJzqfz@pr`L=tNfkxdn(3CtI
ztF93nXvFz}rr2U&-JX&B9vTykj#DqMV1iZmH4-9lm9ng%K->GDJcI&emb_lEL7?@C
ztn-By4ppHSF+riPAz=ee+2Z1%VR0b1>|eIxf<WsS`Q8_N4ag)WK4OA`k1bKb^OP-4
z@EYJX(9TziAkeznK$|p7g|23TLLWdvti8Hwq5lD%R}r_DdKDJv=1Cp~hN%d$^_Zl{
zLLd9IMZ=9#Dnr4rh%683fN!DgBecgGF5&X&C_fN~OK`0@-u8IIB@jD}A;jRa)Sbrq
zp=hbcjc4!?%O#K*6Z|>1rN*4QRMtR7MEpgNU8r3K<|qlwf)i|DT4?M<8<-Xvav~tC
zVOgloWI!@}S$ObN4cjBtAEofKJ2q0$6OqOVc!x50munrnkdOfLDJwu3u@Org0*n*+
z{wgQ9k9{t()0Z`Wn#yW>3g~5evsuY^QSF<N#`zlH4X1;YYl-JG!P@-_38~$Ol_f4@
z&N)?V;5_2#cpDO;<9)#sW~kuDn4sV$Gr=g0qvCpsz+or(3xi&aZ1wr>1(l6qCdgOq
zG&SfBWs5UtJn$;G`wdob&^TU>R4dSurp!{o9hsouNk~XZi<B)+@G{`lecP47gJd@|
zdXg{nYhP&l(^cp>NQlr~RSW$Pc(wMOo2<2g8%~Hk><hkowp#le6STJX92NYzvc*}u
z26(mhxSOru0$N)ZIii=kAnyznx||6LeGv(<cC@m@2|eLVN|H)DQ7M9eyD~D+7hEt`
z1+Qd+f_EWdBk$r36oH95jkzUqozFArEah3v1bH4nLX5diS>lXInny`u%yqX}p^zz;
zdtzk2FL(>cB!c!ZLBZ+sRq$QP7ALq3@M`Umw_CxRXzgc_!RV)CxV;i2BJ_PGD75L>
zDzu@p#0k9!c%5(WxWfwFL!oEcZgs*rD)43|C~z+lQv7$7JWk;7bFGx3S?E?vH(8)}
zf^~o?-v(0$_pLO3+fKDxG~B6LsPO{rRJ%tfL%`Pn?daZJo>pGD&~~cbqnt)|TjY1D
zmb&v|zf-l;e*)$-vU`-Bs=HR#F)T6HF}(V=6U~$7pJy$xov4LgC4}2C3+*}2cA^&g
z7Hea+6SeRi%d`_cQA@uDg5CV+SAD7+`)QZ-q(1o`kVkA!N}R^hrz$KMss~5=6l%5S
zEZ16^#RO~Vek7!p_9|OkEe$+B5d7?BTStN5KGAYtaElcx_;@BLcqtMh_%&sV6TA(0
zwfKFd2%^7#^e12Nz?CZaOeQG!dL%^fw^a*%5_p{h)V;@A9P~)zqZ0isu)HSGe>@Xn
zEfONMxw6Dr`!evninvpD`&J7~kt{3^luz%Aolp45ioq5bi4_Ac+eS$<d8EJa0`e4b
zr)$Bzmc4B-SG^6aS^X=q+ZBT%;WZ;lWznuEK_b7zE|e4{@D9mLIzd(oiW2l$Q!&)?
z;rww4yvLc0B@3G}BV^zlsLD5i%pIOEupeI0$%h0}IA~6xD<s5@Q!=LkXM<DtEL1CN
zrDE`$RW5g2!lj1_5ntY*q%mKH1uePXx&g|qpfeNRJXDZavs_7Y!`8^H|Hq0c1;(A1
z5LXnj;-HcoVg;&$`-s;(VC{%4;B-Y*I0GwJhurEf)9SD{`pcv`wxVKfU$aaq<e>}E
zyU0vwnN-P(7vX-O0_Jhrb-q-}lGV1yuu#p5ZI5B0sh0qnGy4pm_8Y)balV;eLzMJ_
zgd+!NlWx6MV|gwUtiWwZh}*8zk}U>{PU(kU%EHQ+Q~01QchER4O_=TrZF`vtozDb?
zK8A$2vsl?;?v%$?%`OiF?|jG#PPFxkvqlMbLh#?LE!YK}UnoLqwg1Cb!X}pW_5^9O
ztFBO!_A)_}O0HCs#wmH6Njrh(Rm3gdYaX#c>;|zuYIs^vrCOF7#7Cr(pS;Si)<>jT
zuU%(v5Fbg9r;>VuXlZL)?GGT^g!b-h5CRxLwn+fYyp|B>@Y_W4>Hk7N>_N#G5Zfgn
zs;{>JVWBSwVF<C%M-@6qT&g7~Z3L%0o=|;Im4gIzOXd6In?N41g9LFpFUZ(t-LZ$l
zb|#cUn0R^L_3GsjnC^*}??*xe9<40dje}@vdn2VtG__G|FgU!H@PIG$2+Z)LyB)&>
ztL}OvB$_%aTU^zB2fPaYMJa;8;pv1UQEOuDRLtQ-@V}X$;QdI5;N(Yb0pkQuxP{rW
z5>Znra0elm`@9Xb)*DplSSBd+d?Z9@^QwhD20X7K9uTN_%mT4Ruaz+Owd3>#aSP-P
z-pC0E7QH@}fH(lq_8=ci_`G&2(Zn*;j>8<!-U3?cV$7!OqSq%v{q3E0w(^M`0Cdso
z6Onw|T?n;O2w|vwCZRR}!#vlqE%YKGgqnqpxm!c+1}(!Lu)1F)oISY8P@`sv44*s~
zWsBHQBThrD?&H=WrNFvBCA<J(Qh6L{rSeuYK|g<wga~Y|EEN=J*CI~YLMakz`HBrf
z?YD%+aIXj*grQl4UdjZ8?nXi?ufMXzMMRDaQX;t86V}oo)V@hr;0vw*nF#)Z2@1}=
zPX*Ugwm8980<WRgQYnH^+h;><DuyNzdKnWG`X~}&ZGP24e*>OZ5l@g*JY|6gg0+Yr
zVeaw6L!q`DLx3HvQW80N_!oL_`}o1akE839P)tpn46)y%kFlYcn#kG3<7j*g#neQG
zV%c^-6fJdEc+`fXrCtS_+3%$$axQZ1lm2Rir4Cjj(h~jE2u>>)ni+|ltDO3@4NVJO
z^o$Kn3oY3R2&)kmI_nicGDuqZ_SXQ*G|0kNz3vCHdT16V8SW8@J+XgBRwurC6aMDv
z#QAUeF|3rEkQuQtOdLXvs}s8w8{90Oo;bso)%$Ig^&1j)tnP}E@Y+MQixM9vDJNN<
z?FK0)TCLwfg7rNY390WwWr+)-?||2^AEFe24eJtr^9A4et_nWlJr#T=61KVIz>&Oy
z=5l7@@M1M6@qOiM#{>;J0SPgvgR;dLbO-P%_#veTn#=seJAA=oK2X8SnV{g!NQmGU
zsuuhj@G5xFv)1BZW|EnhP@)!h-=l(;GC{#FAR&TBDO;SyM}J6ZGKYFfDYEgr(XF3&
zGKBu)tmJ91i}+nti_iZkuzR;s1U1w>vD~-&0LY~3Cw;7TKZS&?p_iVIEAPg{pM1Vi
zpD5pXOt8FnAt46sR<^kE{sFuS?)74v;K7MQhN&ZO{Zs|N&IARU&s6Yn$`&VhDDW!y
zdNfB~LDe6fxWO0v2gszPIiIWG5lD!|o0Tn2@L9mC;MOn4S-c|AL4PK#=Y7g!>q-4W
z1$RP11b0)mIKj(+*Dkovt8s#J5=Cg+FIDJ#Cg_I8kPx9ml_gGS^{=d?A|Ab)^@as{
zCD0wb6`Tumj~G>HT4_7r7o`J!7~J-5&cb}ufSJHc(*4f*+V6f}lJ2+lH@5qINxI)h
z1>!`~Qup8McfXeUcfjm^Uy|;3(zky1YpJ<#QHr={f4fWi+l=qfYho&3p_>U|Dqx|x
z`v9T8wa~2Z{r=U$$NT_T`d<ql04&=vW~)2)uz|gjc+ZF`Ck`}J#(AGS7dMj;+vyUg
zvG9`$3p%b364OU&EX@B=tL-r+SZyi)(P}%OY;o0gC-B-0>_P9(s}yS8eTkENp__kF
zp_xCc(D_J6EbLR3IH5lPuhy<(?~g06_Km~`eZimZSHVa8qJr~~5WzPoTb$r|z-#2*
zr4&J1+ifE^@~aAM!vtM04ha#uO<CfEE(D%e5nmFtOGXyFjzO{*xw(kiq4>#z3?@DW
z{fTcYU2NwBUkoH0#tk+)EH1G!v4$s?DL)IG58+qB4deBGGtQ{(NceQnZ0VNX9Hw!m
zUZ#RxTY;ya{w@vKY~G|lIP^h0ZksH7`TJn+dpy)|v-$B*GnO0EeL8=pg<be<!bGws
zy_yQE?hDH_V(c17!GVB=imqfy5fA@mW0>WIhA)>XKN-Ro;U_-hHkKD5qpY3MeU7p?
z-Fe>7Sq-9F@slG6haNQ`%I?-5KyhE<mZ4aMpqz4<@|S_hAB`eY*wMeMXJ;#&Um7>}
zAkb2vd7!cGzeI^@sW3OUF$CG2_;a$?J7uetfG#b&MS<UFzZi5S>R`L(@^LFKQ+^_3
zT<sgQ70RYPr!;p#%u--blYH0x)K@gWvQPPHo_&e|gIuQke()VNMt#bICpG>+?ee@w
zS59hhrORJ5-ahWvRR05Tr66+3T&1Ar%W8*#)*-;@&esV!4WJ|szu<&Rk4on)h|)Tt
zIZF$nM1`EL2AJ~6!0g4Zgu))Ap^Wx*N*B~gEBA5|u}K_#0EP<HgGv=tN-Ot4P-(5?
zqE;?Y3KnPD{R+_IAAnAMTlTW%+uEY3C*U?<pXSDD%NvWwZ1{<(qYj|9epQ;@;Asnt
zUt6wB@^IG*wLx1f;`<4*6%(tiDd2kyKWPCx@v{|@rM9LiZBSc3N3W)O)|UB`wZ&N{
zFR85oVSv+6ALDfAI~AOr;QbmusVzBURH?Rz(%Paqk0D^!sE}A~9W@U3CHR$4*tkEn
zww_hGptklzdy|N@^&p6BZGELwL2d1cmVqi%TMsIQ)YcaY(BrE?=hv2zd9LHw+M=nm
zaYvl3xv|>X2A-d21x?)#E2cG9ntjOlASQ8YJI$|LTRaC|tQbrM<TB+o#-op)ptZ%_
zaib1m5+SS0AIlce8vx6U<P+Iy_pN}A`-yA;?SaEQb}hnUR}#jI;}hBP`5GX*<<nyE
z<YdSi46zfZarHsOrK4+|cnX&v1e{KAI@ccre4n5!om$v+0fYB*!5&WttQLBK5I8MV
zD?-ZI*obf8A%uJ3#&xe)b~?aQ3ienp^s>Hij)fa0mO1Wz*>LzWNSrcz6M{He={VC{
z$&G`JPSjp>?fr`q!CO=vH^#+^dt8N?@|h5KpYK_21NaXl4%sE;ciau~sHh-ni(sJ#
z0ZpFkxEtj;lPNA+p+c!0diW-UT)Dfzv&2Bp8_-ez1g*g>(DOaS3~_MUh_31(r|a?(
ztR4$p3y6C1lH?IVaoJ=Qs(N_-*Wt|wE{r>JnR0&f<1Swh4`Y1`F++GVY9eGkB!2Cd
z=sM%-!iJ8@J_~5~VY425C4G=T0R$yokXXw^D<sHLHc-hMv!G?|6jF0|a=uivB0=E3
z2$83kYO!g~RtOqW0ez=_gr@H1^Yq=ZNiIIN-VE>KRsKD+XVC14FlRinR{~#Bg2e4i
z^!F2aNPNVchDanNyUrjUBD_LnBw3F*rk5$<LBbpmjO1u^2*Khh?Amb6$UYX`ODSwi
zMB8@z(C?*SE2giE+@B$YK;ss^Oxx(H+kh8Wz0U+)MULsUm71<%7cv*(rHBm`v6y1|
z)_^i8qDcWai`az;7Lkd8ETS191w0L~GinDc?W7`-Ad{EduWKp9@O0(PBr5w;T_DZD
zSJ+D}NzO$Z@f{t`bc*<@ip45CBdEeP5dQp$p}yxmC>gQ~_Ro|meA)2G;;>Z2)QvEg
z_qn`0EL*wnn)4>Xw6BrB2?Qm)!T6jBzTQg`JCLZChMdhv^g?1RE72E{B%<Jh-KHA_
zu_8|fkyPX%M1Dn{%vjp0b`-!4u#1;hQ4zhooe6qb67=$Z<V@*+gg84Z9e#9X%mVKg
z6n&<|!|T#`@_>@Y(*QT%yee4GH_XShj}fr~1QHPsF~Nu+%k(Zv9*zicSdk)jQO=v@
zROl0jy-#*JEHeWMI!qFD*kI%gq{B>!l+_pEwTzWj*jZNLVp%1@vXWiOI*=^~-&S|V
zGOmWIAxTm!81t676vhl>U`0;P!MvuPQb=I+Rv?I>E(oma)$8F*r--k&s8I6QQ28Z<
zA9GTuZ&(VuMr5>t<|&h5!@~fl-mPpW;fcpd^3iVGoEA06#+{I2YOA@Xyc>j_6Bd|U
z4FY8k_Y4Z`MNs4mxEkQpFAk9pcNFohj*hIG#O@4D`8AOJu`i!v#nX^5UIg0rbshvf
zq!^Vj^MMqUVwLO=E~*YSHC0%pVmL*7DO2ttdfby?0)8?#<5;m1BCMWn@_-=OgPz5K
zo;-O(FanuV`&F%nZ*?kFRKvhuc!Q|3Gpy$=h!`*a>LYrNIKtO!;fa7J#mli}?JG8{
z1Hshu4zU3)b&8(KMi3}!XJEs(&_8raC<y8v>Dn`fH?qn`0kY?7Z{))EeS&n{8@aCi
zod8@|5z@S)Kq}jcg>)`Jo-(|Vz4%=M1f+ie9I^@iJ8dFYac=GoP43!)Ss6p{{}u}k
zm^Eb}pbZu(o^{G#KpQPIde+9ifNr%=>8wUrKXq@jP+9Q<sIix{p!=Z2-S`2DQnCZt
z18TU==}#bGoavKg`3%QXZO5Dv*-$SkZaK+}NQvAW0446)0VkPZu<s-@+Fe_G5<c{f
zIn7D#S;_9&(fH@#^F<~2U)x<<ihtRWwVQ~|K{nC3?%FcQEvnW4y}RmsK@`D^@|p1c
z&Z!~)Plt(e#&a`_c5VV>{lA(1Psk+FaW~Wdc>>U>LV6)U>wh7Al34hEGyVUE00HTs
zn(%)v8oKrWJ=SyT|1B0$|8KRB`u|=FssHb@koy0A3t9i)r3IbE-sIkiD4_WNN+|aI
z-%C~*L;jDsJl_BCdEkH4#sAL#d<RRZ>Wp=Q-@>#8s2^$ToFF{_6UQ&jgV~Q6zVGh`
zz*A={AKy>{TkLnqiul&gHHzsL)Mx>uJ?~3oFV(Y_FL7&>5-$KeWD^`Sksobx?~{4#
z8_1k_&rqnnKl&kv(rdfU><SfrCB*5C(6lWfQ?_XSTpHz`i~j!!#qwUtpZ7RLeCyYq
zLk#^A+;gUdy19|iX^TE6iH2Sec*vf>(7`eh-iu6Y=y6Fjw9(Npbe{^(hdAHRC>Dfy
z(PtAhKW6ArimhxYUyYrsn0FwYmnpv#+#60&L%G6sDs<YrulY%QJeQQk8!YrBXUjVp
zrU^)h&Ey7w82c9M9%Jvm&X(ew49MPnDHeBMUTb?-aIQ#u6BN9Gve3FZfN%q4p{%-q
zu-<E-lL_&QVG8dcEYH_0-mM2%ZqzK?ramV1uVL|;7vF3%H4SUky!f>WbVF|P1sHQp
zE^3#5@b$_~LANv}lDY^~P7z;i>?z+pmK{EO8vc56y84V!crKLLvE^cyJ<IomW$d)@
zlx$RXsg`_=B|#QTZlU8EpyI#ki3nXQM{||~9(yh3CJD0kjKwEfj?2BqJsz4%ZU(Ml
zLlm*L5<I>V*l~ROK_|^w1AXp#cOl3J@~LL>EWGUN%$&rAPT3|c68$p$^~!!R?f=d#
zH@WC_9n%)d2Jqc9e8==2Rd@~!#AV7qhtAcf!psRE=?gPUrXaBpi5c5rRXR{}PDjpt
z$Z;k$*0q3kmfdpg_PLH~gmF2Z<P36`HCIL{#t;P#uX@X3<!}ZpgD}2>a+ZpS6=V}E
z9W?_5;cIdR!;+F~kywtzjD0A`>nzAE$k~P*XVRAVf{^P&pDU{|3KCCp2D!@~2p8o2
z|4~8eBm@=W4B+c?E<uercyPiic8fkAg_t@M1>q}nhQpGQ0Z42`Vn%iwOb3CUGZH!b
zkTZynms3;(CRcK~tD^Fl`nV?G<zwpkNXQctpRyQoBKC<1akwU3ph9pzC6{O~rvz$$
z9f?Co&Y%@#mnfsu>=p$MuVz<7Lk^h&VSMf5E)@}T$OUl5ywl(iz8=VH3X>KfF&l{)
zi&2pMK+m}VId>vQ9P+k`2pm#BNgR^eOdZk*32}%|xhdul;&4qGi)UE80*6rh7)qe_
zOOZH)<P2I-Hc=VHA<GmvJcrax3OR%;`6~utvZ^8weR7%dYvGKC=n$^tzetnTBaxWn
zI%htm#9WAX^787TiSIP-4iJBVpDcbJ`f4Debn%m7f59}ez=#)|Qc=l{!kdK`+Td40
zVfoM{PfUg>T}_{Erzr%{loVJi&sMGjrCkA?r=-<mq^D3G*GUta`&6rxs#c6@4XAXT
z6nB5qd^rq>tw7m<R%WXMc0m@evI-Sr*GXBb8i=j!nv2PfT&Dap2-tDDmZ~?b7)M&4
zZciKW-_2nHDfTNWC{*n3Ac_@x3kx)ki}}AODPRAEx|l#&EOPF#evv&FDO{l<F9lVs
z$kTlpEOJ%F#fO1a#%q}HYy&f>FgqLRlr>X?IHEg0#Ng}({%8FD;=u(`8V&SJ*J89u
zjjCQW6PGEkHyhvm#BU&lnUMp(;lB}1cYeXd?GD88_=y5ZR9b5v50sO^?{mltri%Ez
zkZe37=9Oe!0S${mwdst|Z09^UY(^=(cr9|dT6u!3R_;f(z4x>5jV%DniqOO)>UbOQ
zIGS3hDk>ntotX469xRp6(uOT{wZ|vmbb^=Z7RezIS9=zMNLG6eA@Wyy#+?qbBGuuv
z751TEpK>|!OU}cek08St$LEZE;>|19lAS>^wix^Bt5LSSq~X~#uj=ww<Xr<o8y0iB
ztaqlLsoH*r^BER%lcg2RdmAd`lJ%4~Rav@bx^t2~VzIcM(z6vzn$}C@aJA$&peQSO
zfHUn`&F4BxCpaDXP0w}OHD2JH>oi*IU-+<O2xysOoSH{DWnYANr=_Y16g1xo%03rU
zoH#+L1$&a)G_LQ9_;-kFdVTc<16<0A(oAornG9}b!ggl>gSLGUoU2H*d$~;ct5BVF
zW)QTpPTN_jl9UP`BN@mFkDQsiUMf7E!1xvJ6!CK(i<SRQTUiL)uF$Xw3$1c1#VO}`
zaPo{&RP`CiW#A5w1)2TGwt;Km8(aH<JJ97T@!ke375KaCJwV&=O`PD~2pv*^)!VQF
zuU8{jfe!&C6<FX*yF>HYj$YM#*f8xq&1Zd7N5=o8Vh@CPr`1=}C}@}!bQl%;Oo(fG
z6KtE{RjFcIX=b=$yYu@n&Q;LzIevf9c@b4GbGGAFm#KTf%#(toWHx|f3nWiccotDz
z4%BOh$9h}S4<xF;hnR-8S6=-rv{C#9(f~8E<W~G^2j`dEjl^^$20lW>uz}C=-{^s_
z@ZXq$@8TbxWDVkq{~Q&XqC)Y)-+}Tpb1b5knkQkjU#vv28|hWhx<96QAQhSsR$Ylz
zH_WOVE$Yk|3G5XrD2N(ps}$Jr1ZJ^3TfKo1g?$J7*p8Dh4~vNS{e`!Q5^o<B^R^~W
zJ=t+J2`3-r&H<^s_OYgr_QU;@re@AO+fVC=kiq-a0W5QW)Wvo!$kL#NT?<)1%ts5L
zdC#yCY9Y}y7l~{nCNj|iiET`DLSjD?c%bLBX^XqN3N*zzg8^L(XwE1kzGtEoiJW$5
zbDUGZDqC}K+|sYB25v8E<l)qbAd-3Y1|($1j8C1c)H=5&UKh1%U=I(kpc8KHPNh6{
z!tar6S}WDarnD6(w8<_%nqiGha(3h7?56D+FLXj+sK>3;7i-@+ZBR(sPAe@s7hShg
zHZFvj-dW9}Qg#{(X`^`lW5i;yVk-jAXP%93tK*(aJfG4YexSq4n4rU-VS)}nssj>q
z_-rI*IPEZ#1(!JeW#ov{J9Jd1Pe($WexYnzs^s*0p-`ND1Bk@wUm+n*_o?HYPP{t(
zIh7YQf#&YJlt-s$ccRlD36;MdEQAkQ&J8@Ca?nqQ+?~33duWiFwl^eUgq4s@M}s#!
zELP^4UQ;bozk{RwAsm$kL;F1HLRA(NsD}GIYlH220tzGpRy<KKV9BnLw^eg3)-*1+
zhPw|ULi-T*DTSRnb3@2ehy_2Fx2(KOc>^r8jlxehgggoDCG9~wxHDEdYUoxmu4+x~
zssJ~hz`rsrb@Dc%W#Nl>?o>!vir-(%6^YWmnYtSGMO5mH7UP**ru=rwSO7~XY&i^&
zIr=K4OZDkGMLqyNHb?&ily<6hnUV%mtxk~y%Huq}ZWmC=O#UXNDn&TM^k$$?w_!3S
zDsNW`OoFXxog&4c3eD|?JJWo-*pAKwmhEm%+{~@)DV1a^fkhvSdCWe|1!(1$-vR-f
z(Q2jWV`1tz()x63tfd!JBKQo2Y93)D!FrG-*zX%Dw`@CDVhI-1kC27@QkSymm)c!@
zzZ5Bz*IxXR4~jUy3{#4bU&=sL$uHA=o9LISeB#XY6>+L?Bl!JjDmzu>i?@pSM#};X
zx{JhqhU+dcWGvEY*(CG;r=S2s3Scv*WS7ehiw}^+l|BpM3vW)@JQc*2icygth;fdA
zn9le~%pSUzh$xL&Dl~}MT`Hv)h2S#f6M>nD-#`k(4OA=s8{rHf$2Z5Jf0EsVczLd6
z0LNNj-oj5gWRP#BR6&QZJz9B?k0%H^1FVLlD6^D*YtX~*hlUzU)J=To*A-rvu^4VT
zxY)Yt4{_C9$hNar3y<yY&t4BiOA#Jx0L$$49quB;6_|KL!X!wNPWWjMIn&mv5uClw
z2a1)ST;NPA*L=EZt>$CyQ}(*%vr49Y7Xt3LL{Z+OIAxA1CRy2r5HRhR5b%`>Mhlo;
zmzw}_Rho)6(acIyQGNl)X$UvX#_un}fGC+9Jt3#cWh$ykOhuoF`cqL&VV3lSJOa_s
zVI{N}01>LchnU9Dxdxc;@f%13Fcs~9e_z6rC3#3R?18E103wDB9L|5E2TtI>F$1UK
zUoaI_p}|!2$!KgUO3h76Fcnpz*i^K}dAOUwPc;vuLQF+PU5QmU%&HqL>insw3JO91
z+A2>);|O#rItFFI)t#SHa@IiaulW6iw}}#OKNIt|COH+=B%J(AG?<D$C#0N;YD%V}
zHtn}y7Wb5Xn1!l|u~}$?rC63+hDfGm3t?)>4M@Dl#O+Ab%tPW{B<3>l7!t2Du>*;W
zp6GeDqe;zq6VR!E=6s06PA0xaA~9blM6Y2pBrlAWa=EWXpMh4H5aoeLCPd4TkO`4b
zovhS43ngABL^ZGxkXJAZeK)$7@;D)Sh-A~+s75xL?kF7EB4(i_m1m&=7@8JUn(g?r
z(7_>TBdxUP0%@ERLYV2D)hsGy<3~@8StvbUI2hx-Vq>$=Yawdc`Re)4=?6NzUjY(y
z_@zwH;ombshv)S|AC}z#o(GpWy&O5>^n*;$>79D3(=WoNMP4DN_gJn@p8+Cq`c@>w
z={|Lw(}`E7Kd177=zlkQBIVKPUz2RwqoMNO1QdMGazSVo>Ze2Qt~{Td_FYKAZYv?1
zj!rqa;BRK3wbe59J2*HCt;|uyEc7B(78K~2XfbPpO**HK&O+mff`P}*LRE7x3w<U!
z7b3#5P^GXFAZ~O$XBZ|nnj4#iZUEc6)Xt5rmq2^The*^a<Sf)-9bma&1!tiOa6>Kp
zYiFTF{G?QYVq#N;-yyFHn1W5o#Sk}SG2(|)g=1ye@hN0+s$ii%s1FZ3MIYViIA;wR
z3VUnXSUOS==q$_6hTK6;&Y>v0On+WY<R3hEI=?0YzWII8PLns5LF)VkNl4zx<k|cb
zN$};7vl|ogzd3R?KT(nFe{CeA7+TL6jI4@tN8@iw#QC|g7_y*SO?>tm(<Hhka}Dzr
zw8dEP4ctM8YZiyeu8C+hD3Hxscq2&fMizOvg)Fl>nHTXh7|h}jE|i)4?nUM*A{X*a
zs0T37BmsAsSf0hE@c<Na;4RF#90jv~1dRRJOsxEoytvfE#1et8g4}_B8MG!b`93C_
zOOk(=nBuNz&gD#J@(*6J4s)VTPXNhgJK*RkBXPh2wUjO8Aqmb-kHLW1B@Gf^L)ANx
z<fN0}pNL7$tBZebQbm3IizbyxW+FfM6>$^3Lr6!X3Daht0fvP3wIO0xJ<NWVPKF5g
z$%aTSV-nK3f-xaEF?lUPlh_Xe+?z1eCFj!1Njt}5zL)Y-Be*8{sR*j?=M#|aBscev
z{{2EGlj}iS${$R7$+yGOlmkrHPEJHoQvPH*JNX>QO*zPPPRe-7aSVanls92aiV>ho
z%2800;tEiZvX<;o0S2TLrT|C~pg83o#9>OJ0HafGivmbC<G@;)@?rx3sb&&@vXmom
z)0L8LXz;9*9@OIra7N0RD0Iq^=2T$jr7R<+rs)k}amq890BV_H0OzIjK<K5^Hd7&O
zX-W<9)G=oPSeY`PKofHo$kwE^Ax~?w3c$LQP6XNrP?3^HAXk8mDTM^u3a}}qh(J5T
zK)65UIo#T$bP(INr@TaSI++E~^Q4nHnI>G5R15B|ol?O<CNDsPOu3#(Cz-D-OSyr?
zO-^n`uiV1YdI&#0*hx%g9j38rX?FT~NHCYRDY>it+@p|t8GsDZCMI9UBqde*1O-h!
zKL@R}`bOM1MN=<*1~{({ny%yBia5b1%G<*HXzGSNL}Y>9@h(K^farD#5j)aQ-`*R-
z@o)kV-T<^+*P8;N5%0ogpmD0NLqVdcZ}uSRJ1EE^uSq>hYKy<d`{4*OEC+MMy9e1|
zSTY{}5(*MQ>y1ex!wU!m$NN_n8P0=AuJ^Mrd<VOt-gR(fg7-BnPV~y5CCPgk`N<x?
z8J*(Yg`8CH8PKPBH#dZ<yxu5ChPMq(r<(UJWLEbgw9UH!<v7AS3UPL%=hVO=lh**+
zYk2*E&GgdI6l!`OKu#_1^kYzG-d2p@M|+<@W|nsp(#LqF5l&xuZ=qmyy*99+p7%OD
zTi^Q%_Gf!fLw*BqB+?DNOxV`Q`w^Bj_L{*3O}r8CTvIO#f17!QC`*nPO~#G1w;p<r
z^=<`k3-5DS)zUj2Hn;K&>}>7532(IV%8--mU4ll@*1H-Ow)4iqhW6eT#8wBdF1R{+
z8&Td)UO)J%vo{YiyLd0)Z&z<S+}zD$Om_D!f%YEW_lUwgFBMkx^wyws`QGc0yxsAx
zfVjsT?>l(qamSmDN`Aue%0c&}<BdQoe#-Gi!}6z*3FsNe>xB|L>v+GzUC%k*$JyAi
z1rlVw;CLs%&K-{THJ}#}*0^4Byo(`oC-zhWw+jp?^vjO70>WN#yfSo$uR7jE@aSuf
zw-ctm?s(gQd&BX%p}224-k0#kTaNb-3ih_+Z9qA8JKp`Ee8=%-LfE^G*BheWbG#$*
z?|sMn8IJqF@$ylkJ&socF8a{%R>Rzn9PdmN>|@7!6eaq^@%A8!KXtrcA^J1NTZzoi
z9q&&xhA$umCVlC6XP^{cIo{cD*4K_V3!eVQ@s^`tdmZmA81b#+-G=gghq6P_KF7NU
zCHmg+UctW~9IquR;zxJ_Mfs28T@NFEa=i7>@U!C`fq(lQj~{jZ#qq8|F@JTuy^#5v
z<L!qFes{bFP=-GoZ!!vf0KGM!KOOH`Sbfm(ijcY3c#RQmON=)g@qC`~cEIyXjdvw#
zewpz~VEl699SH-^H{Lx6(iO%V37@Sr-ZRKtWxOKTd4ci%Ksa4!yj3XQMaDyXIIDq!
zqKl2kuQ*>~yeWXz80U!dpfEdO@6+(ek-v>cE4|sUk8#{1*Pa0A7J;JfktcNpv_T+z
zd$IonK(`w9OrCpWpPqnjmmajXdlV%mBpEXe&N>ah8m&R)*z8ne79;CIKRc5W6j#ld
za^!CgaI_;c_@h)cjCmXs?*>$q5TlZmk{)BsUXc9~kaiGdOK4wxV^ZMU8vN0|8YnC}
zkN$96LPOIRan%jErWNpAe40$*uek?sLQ8Wx{;|U}B}kkZ&@d~LJ$vX+2Z*+Y)vS%s
zN$6%iM|27E#bCVqaGCi{w1ocV52z647r^-4;WE!fJ4zT~?!-S~PDV^t!_QVv&H6Ao
zVT{?~%Wei#s~GxRa-U#!Ap}_?rXNs)i7uc|Yc`{0Cz=!lv7jddH9dx2jIK9fiitE-
z^fI6>h@tnQAtjt*{zQvqZJHZ_x;2I_L!>9nG`Ao`Sxe?|pq>rTwN6I;Cd@XSiFyy&
zUl7GQwJxtUmUTMU%t2>t4gi-7IbnQeBjC?AEzoV4y1?bc;lBh=!a{R3YR=>VHz2@E
z@Yd|jvM)2+5HsS~Nx+{P&`4a>eiKDYSY>LU7h-If6~Jeb+Btd{I*f$NO=h-bcgq7-
z;zLm+K^26OATUUB2tJ|+Z8|5d^VJZa)c_jPwv1r4xLJ>*m8WebknCphBtY7IG{|$a
zE+yuE0yW&M>+mn_0Rpw%tanKEAb~n=)@A|^5y*Crp_;V*rZ+mS0r=GzOC>gUkQt4f
zll<IFawuwunS<<w0jgBE22s{9b1|~7_EEMG)?p(|8@UCY7bhuZGtdwFB$>ip=Q_0E
zwDI|HmAL#hpg#ymM0cG{sFt*o>iF@zA7~fhS>sg5%;do6)Y*($Nt@QnZxLBQH;E%D
zsRxo-C5RnLHF-b}2uQ4=Ht2KH=BDGXkdy*FJ&xoRs7O0E9WED=r9iLpNt`-uSQU%k
zKpY76dSEy%ilt7Sne~8Ou?*ELMST>Q=ZU5Bt()siMXOA^=xE;?9|65Lj^q>Yr>#8~
zo)swxh?GqHVp3Gp0anXZJ<vVf#W-yRnl7XhuhBfGP9hpg+V#!Qq$$cR26~h)%BeFO
zBVO9g^I)`KX96?d$J)xPyB2I|cT7P2P@ub-=$L#b;{_C?Ui&t<J4o-+9j%JL`uyXV
ztin;49)3xR95*X>B!I676uZZe!L&0UK-#C2IFD6qbGw<Jki*p+$<^Q#hCasQv@k#|
zBv<w}XfcD$b>K3&z;*ZWp$Ii2KOTl*8T4j2&?opLnIb|W)J(2utqz|H^pb$YI`Z`6
z!Fx{ma3#4K=vyn1WODBA)cK|z=+-}p4o>RsG0;5Y(>Zn0;c&C@9at>b4}keJhIJ7d
zX7hAk#{pm>aJ=<SokvFi`(RhNQFxC5rm2tBfM^L;^V0FY_?|!ysYIeiHb=;qkK00^
zh?)YL(<{+Ab?P*R@-}-~YMieE`Vx{{03SMaPKKgLmtOcw@0bn1+!@2pgWyQ7MffY&
zr+|4OhP{HkgNDGLg8dj6&NEf=IB1Lv@83eP1};g$PqF>L6&e4UFS$N2O=8$S&4HcL
zCI<_7u+roK(}!4zt~y0<OXRdDv<r3|Fp~mogMDao=9f2p?_#fq1k@5xT;x+(FE_jr
zy_4x^M)~e@y@=4>2|Dh=2*qtpeg@VoHV1sUF9ZLkPvg{N^E4aGzqH2OeL(#jL$iIF
zO(p@QW<|NxAuS6(t3hJ-=o&RZv(5aX*0^l~yac4vXj1_md*Jq{&M$C^+q*CR^8V9|
z2jNr_{~I|PK<h#04tWK=IhoBOpf89c*@?Px#}0x7iJcpOzMCX}_);<{-Kp~gx?y+P
z9ob6vJZN@Trn3!Z6*|<6`))ufq=>wKO@b9xUeIr9;BLd@BV+pn;Dxw85^WA8cBDaP
zjA)GMmKfO|G{XWqY0z~!aAh2K4{Tl94n>_3pjQC|idCAyj?8U{3Q1jk9Lm?2-x99Y
z?pn|WD|#8{0&2n~z-nCu;V<sVMyYCflIJh+HRT6|tM&e(E!Oy&*$)bHUXl~~3NR-_
z&9`OZHu|K(hSvUMx|TBv=UB#5^>NQ*L}kG;*CA7?PG=a2$?HIsn9PYri)7v>w2*Y7
zq$Bq0!I9+XzYxNS$#g_wGP~p!$()X~kaVJ?6O%a+!TN^eGk#h~Ix(5)mK-#_9+3*r
z@E)wyhHRU_r)-7oED*IWgt3t)0KegrFwq8Uvc07aG8lQS8H158kzxdiAfIZ8P$ed_
z7Fsh7BOWTPHGV?I1ci+yS?;Jy7?%=z8I;tTQ5oq48h-S+62r6@!!!xP*6b!CGeCEi
zPs@ap!JeZvdzi=tfG@9vY|SCUFmN9a=jU=xXU-8756)oHs$ee`*#*koJ~b0g20PT&
z>;NNs0q?JbY+C{_nnziYV-XT<CQh6K_(H%)&O{PDo|C&1brj8GfV6YkFJ#|-9Edyc
z^RMPRxzp?L3HvOYb~>`#TN7bCVaFCnEnN7hBX~T_eLAr&S^WBaU8nQe2sQHyEqD)L
z7q1g;jxiJKI?XY^G54l^uG8($;UKDR7)g<}dlq(k0sWcr6kfBa`#UIOlc-f#012js
zsZ|pp%vzt@-wh>xK+Y@3$@9r^J0Vw3(#oG6q!~4U(z?4abEOKNMZu}&5ejCUJOh#-
z(X%0PdK`^E-attIR(ZOqbivF7M(BEyUH;_%sDvLNo(pOx*dp*IvzHlR+Tk4mCbt()
zIrZWxr_wBIZ7(y%O+K-gf0{>%e~AjJpp|JR6&=46e=U^85-i$o<n-PJvbZ8QVxQ}D
zZx7pURAEkM5w?lK80lj`5+r&KM^2AH_;U%e^KyLS{UQ^y8yKPMNp|^@-%1Ik$isZD
z!zuFVu_A9pL3XH>xwJCX9H5o#4Gz%CAh82E?Tb+4Qjqmzw*2YA{3Xc87@n$vjzDPl
z!t=%j$jj8cCYrYp4<Ac3j}!7f0WE5Z8;$c%P%O)pzdTgP!rMZHTqk^o*6ptPLET2M
zFrp<bd|X+n^4`D{U*+dvE$NA|==ol?6vai5uf^$8Q>hcs*jQ$iuTMSAJ6iMLhr)K6
zM*}(s`Mv?&6)R;P83GE=R)Dd5k?@T$?f=3W#B(Gy8kBjpvhr5&HdQ${hbn2%)gkP2
z78{wni|vJF$iF#cM4!hM_$n~m>HE6oX`!8h=QYD^XFY+sbUJcQwnq^ww2v_5ZP0!F
zm>ufE?n40elP;{I^n)J{Mjy5Yf%ZPA`lAo)EZMt`bDaVIL0p*LhWt~9VpGNtc*gWG
zzXI;|+(@Ly#yYs}qX3Vq+`)}avh};2;Xc5816!nFKL=s41Y7v&#Rzwj#kFweqlL5W
zoUX$5QCNS|74sfxG8;e=OR$aXqcD=iwH@Yb54lN&Wp{zF<ID#X#xAaD355BHSPyv`
zvh(UD+tPH05MI(lz6eZw57|%&rMu{_!2hO)ykD&>rj<iXOU`lF>F=YJL4y8kIt)c#
z46-=?orZkvA$O>-O;*?|6vi&@W{?DlSP%IQvh(((*dl{PF6ld)4nMSq{7DI=$XTh%
z^}pyL_p6nAXytLHKBmG_xdWjzmS8Pkik#kF*F$P~*odIWE7kV}pu*tv2!*l8D?t)V
za8P)eIqd#(M?z`^=mokFQ2b!9CtSN3zJvaUMdc`cy9(Mx>xP@@^`MlY_YRcC5*(!)
zk3tSd>W7fXyWiL8R|+w$j|T~9OOObS){lfM<|Ci;aIB3Rt*0n&HYNyWqIr_mG6c_{
z;vm6T+=ZMxpZqT-D<ej;k_tO&ZD*n*dlwRQC@bt8%KuL&znL1*67<z@-vIjP+Fc71
zox)BbOpcFv7MMOois@l<WvCpbdBvKy5Vl;Zc^o!o2DI#?=c1EduUH0nAz#tMi=<Zv
zB7D-b6f30Vp|<dvkd82e|6)*a`rM+_Uj#y(K6h!}AAwCy;p3V|1G2Jxzxf8dpjaA^
z|7i#)Oihcm%+rMLABOdw!4OwhSvkb5R+V#tA+C7{dxynF=ER1$j*6y5eTFIUVqmz_
zce3UML)=)+2oG_I$-I|IOy=D~YtHbU4BS!I!cXpH<o;yPK}by>frs`g;CPVn!b@#H
z_y~SI$n_Kgu+L(j1{u^m2i9GwdDArymEUK*<}J2)ESb~i-<o-OjJfbW&7;+K#02zx
zT=S^}0-Zs;Z{o_?E=3d?F%nkp0oz55VC4-U={g3D>vkk+jz!{LBwFIpeV1ai!2zUU
zPMz^e^Dz_T+BLy-@_$#!5{236v3j4<kcIu*sg0X_g+H5{rJP)-xLi18;}HZT;BaQ6
zVuUi^p&>JVr*KV(`pHA75iUM8m`a@dtCe1id?Lhr@Nb#%Mu7YsVb<>z;2_%#j$!!*
z{8cN<Psnh^+tljhw+aa=7#0#lA@TP!{%ln4XBPXD`;J_b%vPyeY)zsnZ-*r}HxA9c
zmYsmxN4Xi6=M(7@QBPe%#uT2Q0LxJgod0OTHou<cC?^++uyt6%c`o@~&O=KW+J!5W
zhIKhUOk-!GB(n9Gzb0(d){s$GsZj&}(z#ECSe=esj@_d<PX>;85>j4j5^~Jt@Pu{D
zODBM;PANcHDR_x({U(6<zlY=&X5;lxyy&M+5cH3Jde!$+PvxYa#tA1|B=}=Ka0-Vj
z5goNWOcXd1e&SnN^2aNs1n`oORom67zjo9WAy%g&7o|68&Q>WKyR5yCa(UB`qgrCO
zj&;<>bky|#^M48nD$J@L^Gq=~|Iss_t7r0?C?|_jFeoI3A_o0MeM>v|HfRT2>z$>n
zuLq^L2X^G;gi5g&W_PJTC&q>8Zht_bSz$<l7cMOv@C)xOR>@sR*Arc*|IA)}9CLxd
z;{kVTJ_7iQ3>V`fYbzFZJC^Vz3U6Fk#08^A)rhvf5to{^&Cp@i&BSIO2<dSo61Ovv
zgv2f+3ic^WemmQG*ckBMmI{`<E$f>}ENSFsM@1+LjTpecWa-ljH>&_X$=eKN=<Ib2
zq?t~kZxivmEZ&t7G_rOh(7!1AcW}oSJ{qky-Lo~yCC?1_h*m+ZDM^XUQ&XIR`XM3`
zFr-1S#3yj_`zpnB6wmJ8Iu63p&jYmH>Bt56Vx>TflMCx#pqPiBeASD?D~{v0=EElE
zeB@q@Uoh~t;`w;){Bj17qdA7%QeUGLhj4xvm>F1~>p}LElU%3pT&1Bc@8Rc<!pk-P
z2b*U{+>13c5h8>z$2qQC^Nz83vMILDM$OFCOc}2?Yi41LyUzofH`>ob=s%`;{>($h
zYnJH-#Mv%IqK@P;<!iwD1b$Mcb5N!}Z!686F`+{~(!5zVxa{8Dj(q60Pcf$l5<ddp
zy~l={%Wg;=inUEMnQi{Wli|1M5jH63w@lFL#G>#h&F2H>_I|$1(l`Lr3t@*@tiKQe
zba87%^ZCJSMPrahJaLG3@;fSCdWf+KunPvo->sCm$<zEEqBWmwI!{9QTlfXh(}tz%
z#?m>1SoC_M(2uABbhC1q^522k)&j03|5tEBj~9`cI2k@)sl?5IwlQQ|fz#(Y#dKE;
z#PzvN^M-037iqR=9_w<(AyS@K?2-WE6uzQ)43w)ipR>jHH2=<+^uC{J9&KZRI`Wa$
zKFw?sl<+;UbZi-_`leH`9ej4#96s|RVm9Eylp}5Bb1GgNnYJxRfpeUKnjtJOeiwuV
zC-fhDK-OB$1q=Y=1pH!kAZq$B&(aO*hU9z0F|$#L3j;fs12do%YJyU>!KNN1NVK1d
zn&_&;n~_nuCWb2JDaEiR#%bQ$n#Y=$qIs<9{f9_7SFveOZ{<6M%QTNQ(Ma=I6PIXy
zXU&83zE^1;d8H=!aJ)h@I|U`23YORK3zq(WL*a&si$*fPPZ=uWUS(q|yjFFvojs=c
ze8BcVkgpM0utTvdUnLJ&w|C6KtYeSTTprvP`~o>^azh4P0t@=dBx#%BEUDHstX%BE
zpvpzO?J``hM^+Kn&YYuc#MqGw{rFR6d23W3*Op&C1=SZMxU!Np9XIgtEOBdTq`}KV
zC_E=#_)hbYFMKk{M7Sg<{4NTQXNeO|8Wmm~FZ^wjkM<%m(@$05lA!Q1<mAP(#0e*j
z3ZE4({9`Pbs_>_M;gX>6KPWt&B~CbL5CC_oVBY3!hs1C-V_;_B<=?9`GIe}C#PI!J
zFnkndNUA|ab_U!MVZko=FYkB?>nb(vDHdwp?Am}-dkXVHG-p&zlRrdhXmkFFApqSL
z_kJwatR%4waaR6sNO}{$pdIy}frBV3l<sZYI5-@#0HEv0ts!d^of?Sw8L0Z0%mrc=
zQ_MA0ig8m8LJl{PR2(9wCuB?s%XxruZmUub-#a&K2_`aLE!zrFXt_-JMxciC@#7(=
z@63n-<R3gZvIG8sZ<IX(!(H~H>=76yk6!^{!2|Q-D+u{d$&cSc$bUYbF%UXOKZ~r2
zF|XloN+em2z>M_|w{Z2$9#k23BZzspWflH&ku0+VEgsu8#h#4e+*T>S!aBo$@Z+G%
zIbr67NC>%odQL`pgx<~>|3+=d@DJQ@Z-AB2LLNi6NamAmhX<;o&N4K7J&%$<&Ynlv
z?yr}F==_OzBF*zC(f{-FC<#j^L!z8VN%*JpD2bu-C`qyNC`qC7C`rH50rosfQs_KN
z(gEh%^C(H7^C(H7^C(H-^C;LlOb&Y<B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!
zB`JI!B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!
zC8^SRlq5Zmk`z0Sk`y|Rk`y|RlJtM$JWA{&O7i)4qff9WQIaox46U&8NtB1DBQBG#
z{eg&AF^(?uuE)y!BJW2Au-7;b&DooS#}cmB3;7W}x{|zsgKy=dD{>ws`G0yIB~tL;
z^C-BP`0sg?|DH$rfBZa3Bz7Jpa<gHFqUTW}w+N)?Q6d`z((@>hTMhdsJ&zKpbRH!V
zK96!gsO%9DZo62Fxt5$qVRj}ZC@y>+MR2quGx(!a;qxeggcy~ilr(%EMM$@avL$uU
z8HUfJ5EnX+60zq|o<wf!JW7mT&Z9)CI*%eWvZrq~Esy<?sfl=1&Z9)CI*)QV%sh`0
zsp>q+;V|<&N~EguC}eizJW8ah^C%Y0^C%H}9)$&ookw|IxOpBWV$Y)pIyjGF(L9e5
zvFA|)9h^t8Xr4!j*z+iihuC=(i{^QhNLA-i1TE)LBJt-@#3SNzIgb*FKaUc^^E^r<
z{ya(u&+{me`12@&m*AE2D3PkpqX><x5y^R!$X}mFiNv2r5!SyACEYD*&!e1l8dmyJ
zx;&GCd*M6I#GLzV&K9tJzsG0*=WYZLHCcC!0kD8TvdMY`v)z=11kz2`qhkRqBH)><
z@#I-dpoYoHCD{@JwM|wQf%90RIwor$$(9nxHd$}st|Mg`$r=Mg;CvdB1JNW~K_J&;
z{hNAL66j*Gz9iWy0tF`PTH1C2fdM8fpTLC#icQu_6mSuN(I#sYF&7gkHCfltoJ$Cl
znXGe(Swmo!$r?^QYYEIVS=)%Yl#`ytChG<Qml3nlWbspQDVMVdYfRRi)N=(f>rB>D
zwCze_DooY}>bZ)TjV9}E0_zBDGFj)5=V}7?o2>a%dM$mh-DC|SrknzvG+D<I_!og^
zP1Z~T*AduZj$y^6<eSUW9p@VSYP`?ZWOI9)+mN%>&&~WqP&^=?($74F>|H)8bVwp)
z;42{bijC+Cpnvp9EO(ucse5QD43$0AX)yc<{H)qKt0{YUYaiPjm^Lx&Zt@<V4I1I?
z56p2fYyz?4F7bIM12a8_twzZwzvg3?0<$WH<%Z~#X$O4l^}uW(b{|VzmmBC)=3ET<
z)ayRv^VuTVt=-A1-vq5a$in^IsqMZ7S#Z0W!5XU1{oScu1>xVh*8d4Nys3GT=ik3(
zbANYg@AGkcSkZv{yCWUh&Kq)ncVve-Ee(|oxDoevrv>}FM<dHN$;SQif#_ha?q)4M
zix@UXxnXr0kLd|?Q}YF;=Pb~bL^EhCx=D{7K({ixx>&SqnztxyBv=QB*qz}`WjA=!
zzu6hioxDwxUxC$4bx%0c&A1)ewcC-Kqtnjec4Rk?H-JdaM6x-O&AC~@mF>vKIxS>7
z@?a3Rl<mlkT?TJU*^X?}PAi6~DRjnz=rsIVZ^+gJpR#a=yOY7tZbK<|qHS%`1q@<%
znewZES&v^XW#F+*TgI*#w_OQGqM|I}cHnp5*Ur}g;#SK@+|GFy`Jdqz=y%$XbP_@<
zyQbBJ3ROWV9?Fbgdvg7NWt9$1l!kV6^j}!gmMz5Go~t-K;8gyB;GK@2Eb*|^EoS0b
zXV=Oy_#E45%VuOAEmhRCKm@ly-W3*c7DY^}N(9@Q*|SSgu~O^=`ww9ePe4S6*OiX;
z#Y(~!X};Q{xUq;nUZ$L1^*jf^pd^1lMwjC5*w0ac$}-<DJ_nqJN12z@wq9(5%Iv&F
z0UpNf$VZP$I&w30M{cI>$S02*^=V#5Za$r9I^qGOd>f%V8^1Zr6l7UxebL0NCI5mI
zuQgOlm?)WzCDS#q<Y@qT^5pd#^ET3vwQCmSm7%Gba~0mSu;<S(>mn7}$jWrgd8R+s
zdvfcbhEgNN4eXNg`urmRSF}&W-%Zfb`QOS`fp!m%!Bi<y1&h=cMZ(imDU&Y%Ws>E1
z(fLxm&Xwk&n9g}s==_5^zduZ!zJN+PqsO>X!l0ZNnKoGT)pAx;p>x0-=xnBJ|5Q$2
zKqZ~s;&rYuH^y{6TZPWcsdK_%>huLv(m5+$=jEmr`Y<i$zAAL?q0Y+=Q>QPWlFnP=
zb*?iP#&p(hcxVi@JOetPJ4~IvfJ!>wjn`Rj4#spARH1VobsjuSoxXrdI+N<y7(&d;
zW!9S+7}nIcWmV{Wk~&)`+dsvSF92I!*)rS5YrDa`6VtY)3T@t*(02S`YV!rCwo{ZX
zXdkzjp*4fD-Cu>a@zl2ZFtzytD*5wzWyVay`tx=<%cB0=Q-#ir)cM?D>huL@315!a
zc9$HgS8d6S4vo_PP}{GEsm&Ll+Du&=D?!;do2zRD{>-gHTftmtYp!hn6f3>})z&s%
z+ZK~lJJ2?|3T+or+nB@D<_l15lj61AXU>diTU>><x2SE^VQTXQsJ6B7+8#6?$Fyy%
zLR;grpzVRf)aDCNZI8ukd&HC+9e8qA720M|+n0x_%@?5B4#aDF-0*EN8k4_Nq3vO6
zYf{e!*FQyxFF>`mQnnx_pE7*AfNINbd}s?uo(F9s4^x{jK($SX*Y>QrHl}Sr71~Bn
z+w#NI<_l15tK+r3U{Wxd*0Rm2LR$s3Z9PnFz5vzsg0cm@<xaC8rfppn+P<N-Zx2(O
zFQ8Iy`Li;&@O#Tw%-1oU+pEyoWj=H^;Hy(`{Zp*?0<?t9l?^-9<T9_DF<1#ye|}Mg
zwq?{-e3;sN0jh1ZvIS*(%RCd)<~2DqGIvqi(!<o|3s7wr#+U6K)3I()wk}m@t8+HA
z-F=wad;zNM!T7ShZ*GifE3HCX8MS?SnA&^+s%>w)whzq_^@6gktU}usYO9@ncv0dD
zP;J@D7S!7(W>HMrrYf{K=RjN0VQTXQV9jfWPX6;TGrA5caMOWD<qd3LI0Z{W=tq3?
zo2uU_@b7ZC?(n*xxyjNgxG%)E*RtVO#wmCrl+RbEh_;wG{|1JO6^#XL#r+y$<9QT&
zXXF(4Hy@-Q5u$ISvr!i6)DXRY<3RdlA^OQ7`tlIHKj|m^mJt25A^II5dVj)B`cFdi
zFNf%P9>urcpKO!9MmF<pdHo47?Qbdhyc2W^{HZJR3q$!6buPvH@u7Tw@=5t;gydfl
zqQ59a?@t^l=XRT)5{XrhKhq=ni4dDVCu9CQp?rV##QYya`TjhI`Dxfo;k(11kubke
zDBqtuFu!{!|9c(fnLj*~?~ne>pKbHQ#qft~qAv)s`2#ESZwTf4Lm=}X4COzf0|)b8
z3FZ5P7}wOeUBs_df1n|~+b~vae@I|{Rw&=^yqVuFl<#-5%pVlW_d7)9pBT#byDsLR
z9m@AR8RlOa${(KXI=q;FTP(lx#8AF8+&BDw!0EPJaXT7=F@sB{@~+D-!Jd8|verA@
zxtMD9n>9)~)KCYq%F{5~H^DEol-5gj4DL6~#unNPt~$5k(zNL=2UUh5KUiJM5#sL5
zvy^R)Me<uL_U>t5OA=eBIt!Rv@N3WGE`!f;o%Vf@SdK)8PnDQ9qj=n0g_@HsyiQxL
za+|^Xlmv?na+&gHz<m(EV9jb9MD-Sf1~WhDTy)ESR+fy#xInO!tWWjP7)cJ<E!7Xc
zEad9wA*jY~-+bgBJec1e|KP}e+%1*3aT1E#kGrK3Z)LLon{aus{M>&ZA%D%ee-tjQ
zyjyBOZDdspY>dAt5l?qZ4GM2~V(tFrhNoklB5rtcF2qDT>g;F-@n$>V=%y2NM-$eR
z{cFj|Xan5Q^xrq^{`U>L;}EO=|G!~3wxcQa{9;6&|8CvKpuO^rCVjW=ZXznZTUXzI
zdlMFl|NH*i|GxkBzwf{OfBpTpH{j{yk+l<kGdm~X&BAEbM@^qR-%0qLP2Wl0Uy65h
z{=p42$;lnZBYi;n3a5s?F_#cNqor@mB@}gQ=o@nhCA&5Bjk)Y^Jhw)lo)F^iP|6$+
zoe2jwL940hzdg4v2Ja&LYK;PiW3#baX*{x~`q{N9O>x+*G!OZ!1040%4*n<=b}L;6
ziu(g9N{CTON=dO>={b<S7Lbk?W!$aAx95I}e7@jL?5l-=9j`Lw`_J_4xhH}+c*CyH
z)E54uLvPQeZ)Hal-=J%0m{ps84*lr>(blk|YvT@5-<~VXr+|@f&^;_>?pD&b=L+)%
zF!Bw$hs7Lvd#*5l0>-a=X6I<G3FzB%1s%a2ofP~;wsUmo?YXQG(-5c}qRZ&hqeE}c
z6?8A4`p3|rx91AF6sRdNbm;B5tWC2BsO2$q{M&P{1L~##ofUq2E?*G)BvFhx>+-Cz
ztW({s^fqvx#Noql&;1p+C>m0jKm7JwzM-^nfS2GsI`sBj@tpsLT`NQ4>X`Vq=bi)^
zwMp&N3B5g+8ScD5Ji$;DNl*o$BnaFjZ~)IBWnFHkz#~*{r(o&z4Si=KS&ut7(%QWP
z$-xvl<6%IugCni0ApF_1?BGbtle|hhIMRA=!kpOd;7I7mvNv?mlb8<WX#CFu)#yPa
z%y5?`HRfiHblJ=i6oWf0)}hFW$)sqK%pR+Wq!T4w`AvLHS;g+S$VE_eJ$}tLWZOi}
zuh8m_V<0&hENN5LxO;NsUZh{BM7ajucTX0|rmS{%M&un3?5jj{Vpk$85$oQa8{xNw
zGVzP)?1!ScbEU$YG638~5x!`*TP332J&BMSZ_ZsI5xXm728fR3u8@fA3PExvlFea$
z4tIq_WLHQFr}-7o5goh=`H}Tt$r(GqhjY3^Xml_o=RDM(Npd?aWgp4oplv1lNG8fY
zl2)>h#HO9r?EOq@<Gc#0&+rS1)GXLY(uNXV8HaAYA$^LwSNRLR@)X#YjbAR=3vdJ2
zwv*EQ!uzA99Vq={Cl@5$@C$@HZOFA66;-VC%>tPdfZ=YYcI3iVlcpRb%s%H#fEVBw
z2+5(-%|3B@8*<zQd5cwEb|CLgU>?S=9R-+??H2Oi2&X9r8FRqd0dPNlF|mvvBjaBV
z1H}Bvy3b)4n1d1qZ741k$}3bE*3jiL<yp9+?2KPKiiNV*`ENwKJ^WW9d!)=Tr!~hg
zlVO|zpga%1KzA!CQifrfT8SOi3{|!gL#o@fSmH_AX+vd$VC8l-1<guLxfz(}@M}jc
zFr|e5Mzov2e<kgv@!zm^XY$`@r!~h*lWm;cfVrd*Skzu_@Ux9<4a?vN*M>p|!$*5m
zC06-VWo-;V?CRT53{-y1e<Pd@@}Agk#%W~ZCnsPS2;!6Q>*zOLGqRH?>}HtF^$@D;
zz){RxVRBV~L^p((@(U<{TW(?kI<xuul>54a>RPi6WbN2$&B$E7!hJqM)#-9B2s^7t
z^f+>vtIQW9X18KScD;iCoG!QEU!GFJQ<id_!6!PLcI;L_`KYC22U8eNd9~?5O7<<F
zl=kOzV+T?cPg!o-vEKkyYy892fn%__*_0~fza$q~<;x**6n5`)_ltfX3e{tw(qTzR
zt^uIP>%cJHC<i|GfvBhK>bcdBo9|QT%x-F(GG_Mx?OmolaJhWb_lXN!yz&Z(i<uaN
z#63vlKd0mwSOO`KgR%wOkW^8?-8%)cd#8Xqk_wte#f}0DJ#!afrR`;9>;`98MjTB%
zpqX7E%`T@+fqq?1>%h^_JZ_xEHhfOE|KNg^AbJ@cY2bo4U|R12mi7t5ab?^>z^Ai4
zKWR8VzDM{*;-3=u#6^G?<pF+L;3o;6!EWUlL%~fK<Gk`B!p};3!>0xN3?ulQkh^P>
z3ya~eCqQbQ{E|3%CD^Q!cdC>3o!c{qYF|-!&BDA-Y1XSk|2gQpa2S}NMZ`l+xy<|K
zCT?rG47kE2@c&9A%9!9+dR8Nmf0nZJtXvV<whj>GtlTb5tj~g$_q(aVZA7f#dnk?-
z`~efJ;OKdfTdFL61usHvRaL=7p$a}1MvK0)mASoNj!bv8udf@Vi@qsLP~U~rcjsa1
z8xzwPUAyPp!~(uuW1+7&rKv4wYhQ7)d(c;Wm#=uYulOJpcUG2vDrj+>Vw@CzOMTCe
zY!n~{7|#pnC)1hjAQyGOKWqla2F)On`i+6n+?dz5ObgA%Fj@tmk@Vdf%AXa=e>IeU
ziOml!_xH`$qEOt17+e0>P(Ex7u{ga>SDZAxj1KTy2Nnl!)?2KY5xy_We}T*;7-53;
z)^I7>OEv?0h;46#rM(T6268!JX>Y{>zerfxTZzDl%K%Gz8z${7AFyq2!~OP_zf;X%
zkHE@KiA>g-fT_H(GLy|7rI{L-dLzu7jon9qsq?7nH_T$p6ft!-VKH^8m@;%ZU@^5!
z;LU`^)M)}Abv|G*^%ODnOv2JNOczt%1l+e)GaG(lE3_jNxT?<Tj;?hsk6Qbu`}X(i
zuk>$7WsNW$|1NC!F`o{+88iz_u`<RAd^yyegzZ5=fq$ogr?RtJC<UId0<aW#kre0^
z!cySH0(V%+0_A~ziNIG9mI9wA1^z|wl($q0Ja!cdyh80@f!jiR)dijt6!<=+_Y2(V
z?+UyU1@6y}k(gX}yE38z<uc`WLtUT6>_l_j4I~;lz$Y53M7B@vAk*H3Fc%+RyaSZk
zIkuMIO1Vt=pd%rtSRYFa4YJ2$5?jQ%_6S9-=>a_x-LAJH&kWv*zXuN<w?vU`KrO8+
zTOTO+V<88`(P-}d)+^=7wy(ychlbV?xGg2(vkk#}&m3ncpK~P3bB>|0Q{h8@gaeXq
zn*_GvKTsFE8z^}OWeRTGO7;-)@77BGL&(2Rlau1Z_*tfk;Wh9V{gRrRmV;@{ZWJK>
zJV;#Y46iR}=L<Xr!8N?KfWCzzZa|nM??t<J*lXHVnvvnhC+tmXUkH3{_9(SUX;U19
zTR5Zm_+(@XvX8=#g`i7nGZCUCt8@HYs2cJwA%*-AGXBA>;tJ%B7Dn7#!EaG#Luc4#
zwgVe&H_1MZ@%oY2UcaKr$m5u#|MLUp$xA0gs2ng){-*=xDWL=Asj&m*si6brsi6br
zsi6brsi6brsi6brsi6brso?|WSVp7o?E&-D@BwqYk(e0!!Nb(>0rS-G0rS-G0rS-G
z0rS-G0rS-G0rS-G0rS-G0rS-G0rS-G0rS-G0rS-G0rS-G0rS-G0rS-G0rS-G0rS-G
z0rS-G0rS*K2h3CTfO%@{fO%@@fO%@@fO+cw-2>)n=TEBZKaNTJs{`g~|I-8JX;mCB
zZ-<-oXxgK_Be-o(%t4iVBk<RFt?;DQ_2xCf%-{Q=IcU5|$WQPtXol9L-$_h+a}L=K
z|2v7%f;@B||9wvM-{(aCeNOcM_H&}>-UFC0$afN>mCmI{E1gS^Ryvm+z16T=({t%j
z|EZ6B5HX5#=@sZ4Zo#j{R8ZNY;!zAy_aTd4I+W~8N>Ci0^t_7vF9IBo6~|7TD;1vf
z{0fSrFpG;xh*5>krAP6kho7}+6OhgjWqi^T#gm=_<QMt4(7E)eJ(oTQ#O7q+PxEOq
zg<sC4NA0<Ec9>=*@OK0>%nBVz=fuapJcp%m<Xn2xo=X?zr@;8-;WG0{Pt=}E7iKpF
zYZdreNBo0kKIw_tbLqmI55~Sevpts{wdc|WeF9J?$IyJz6Ahh9UjWonqUY16@}0zJ
z=v;a^P&dTTe9{vQolAcRsK;VxKIw_tbLor+vm2;AF*Kj_L__D&`8eiafR=OVQ9YMl
z17TbrKkE^3xtvRn>XV)h!13#PVLYGoMD<+y2;e5h;rXN|s^`+r0B%u$m*ACi=}~(w
zT|BoA_!|Qn&S4!nmmUqBOWy{3CaLXr5~KE9Iy2l?16G268A|pLtdqeZ_y{?dp5En8
ztT)TK^z`t#^z`t#^z`t#^z`t#^z`t#^z`t#^z`t#^z`t#^z@}HUba1#o*q7zo_;=!
z$+73s(^nA4wdc~)R}$!A&!wlYB2Zw@rKev&V1PZBo_-;LVtXz<{UQRR?YZ>yiwTt4
zbLr`q5Gb?f($m)vm}SqUr>`Y2&z?(9zm!v=#r9l!`enqdwCB>(!{^e|uOMcfJ(r#y
zK9`<;6)_v_x%BjP1oT{b`qc#Px98H+!{^e|!{^e|!{^e|!{^e|^UVvm>3bQ!8mrlw
z<Xn1sZ}TB?zVUN2FB25crKk5Ze;_*vYK0+mE<Ih&rC-fPR3GSLeG<zp=hD;VT)ISR
zZ=eVGB#xX*PnUD)f-MDRN({?$>FIJVU9by*Sr)_cTza~kOBZZ8FgL`oJeQs>=h6lH
zATZlwSe{EymviZYeG8cPiCxbU%enM)IhRhX`-9JCi)0^Yrma37v>7gLSM4`LGdjPE
zb#nP;XvUVCDaU>@G-E3P{bp!J_?w{_;ctd!gufY@A@AhYZ-!=szZsg*-z-PZb|HQ>
zZl@CYW@yGBb1iai@pJ9>;w);2xfj{|(xEV9ZWb<nGc*HBVaR^hM};aiL%tb$A4tuA
zfIjGxSZ@0PH~D6$xUUuliH7)D1=9V<H$yYzo1sF|4d}ik;fF6WpP&u$&Cm?_W~h+x
zE0$$(B>ZM*M(mrRi-Eo{AhC-0&Crb4H$!g(`mQ(<els*9_RY|zf#%t9vBiEfG$Z!S
z&`*H*Du(4ZLo?)?p<*OI0-1qdxTySQXoh?<R7m(ivfMZlels*fz8NYc1Arb~iA2Bf
z$8Uya$TvebV(d1vKywc1#A~*^{3v0Dd^41y+{=Kz+81TN8JZ#A3>EBVU>^3dw({hg
zp&7AnhVCLdCf~`ZhRV)h2UD+gO9XDUuAhNo0~$>I9KifCxMPkXs#=~o0@=0jtFe#S
zHn+euLryzCH}hwrvadcC426cWOn=~p`S?)ORqKB{ZWsRmy*UMFev(`STM-TVP{gnW
zSSX?=3_b!W($<p9MK#%%1?~iU=5y5PWDwtw?`^JaGR+;JyAjVg8u3r!k%<H9BsMgr
z{x?9|LNz9(`2=LYQe-kjig7inQTQ?V<5&{Du)P+3aU@QYUz<U|G3JyO@EgcsqC>5w
z3y6D?p#g<7*+;T$b0z*sIJlk|XC{JVsZaL*vG?8KSruEqyQI7c2?SC=j2e3Ap?3%n
zAoL`(gc3q3HH6*-B8UY+5-fm54T_2d#g3v!?AXzRf{2Qpqo~+XQSR@zX6Ai&LecZx
z`_FxzFVB-?tyyK(teII;_UzfQ{yK@3B4tBT%B7l0(~p}&iD=(Qk*e0R@>*k}Y+ua6
zu0yN3C=s|SXPu84K{Ui>6Z1KZ$!H$}+vn8N3>L<=rkUppq165v$$uD<l@J5}V`G+j
z?I=9)I3K?vr7E&nEjsbCcUeT#kg#<DOE1G_C3b*&?6n8+mzjaf6|PC($Rk$==APJs
zdah1JJ7Ze}SCDY62FKNgOSvL(H8Z=m66KIy<3t4u>wRE**sxlO7P+=O8&DV`T>HWC
zk>Sz`F94H$%LueLop-3P@&QjRY@myUs<maqR4Xxpb#Z$%{_6Vl34dGg4z9{Cl}e?m
zi6)Mt<c`EZO9&ImX^>idZpo#rwnt9Di#pG6k-QC3uT_<l>;i4bpSv#X4i<I~@4(iI
zYKWp^P!fx08=k`cjUut=)sAQ%T2X{3>I5Zx^;{_u2Lzh-IaSK#DOREw_}*+ZTCF!y
z^sa}_J^v%UR$?zU!7sBAZM4doqVfpTeNC0p3uv2__)IBSk7uHW7*x;>YYA%&^r_nT
z)vc8i3j62v0fkY*)eamz4VRif$6wEeeR2-`Ev<Dl$m0!*)%<+6(o=u2Srj96J}I=u
zYVkXJtz&m$y|3{nTak<b;*pj`shHD&(u#oH%eL<&TM!0FPaI>54YXYyDO>)8jq3=E
zEUgIG@7ng46m5|3_oA)u&c!g)?&2Kf%WhDjRuFLw09E~CwLT6uXXJ*-ih1+7m}A3H
z;Q#86)iQjrW&NAQAYc!0?fZbleAwn(#bD3Cqk<C)aUu^73*ryJ#E_>y)F#?LLBlC(
zNo@;rWJ(<O30P{Ku!~S~X+QNK*9gCg=NZIREAHR<;Camc(eUs?;DgBXRT}ij044LN
zT61#qw3^8vulB29L|36-`mX#DcZzJ23`o^_%03BGbQ=z4$?a`WaDUZ&&TG>Syv^;q
z4e#^*OaiK_>Bom$!R-Xu8}Vp^85H69ho{{Hno%_sXF}P~)lEqBpVG37?mXuotL4o0
z*lob%O&k?f($v_01#H=k-<Z$n%YdV#+n&jQYJQ9Q$xbv*$7sQ#*@yAxOX@uCuOIPS
z)G+%{C+*{t{yfi=iKGQ`vRaD=zi@O5xE2NZzjC1XPV}F_ZMwc5vZHmw{i|wrXEOMI
zSS_k$Y_dN`3unt1jlC34!Ooh~-`KwrhC69aoYs0fwaQ-B|I|!T*v(m@6@F4x;km78
z%af|L+kcD+r2m0uQaB=a?QW809(yM7nz7owB+WehY{zTHYWIsrJF0BYYsP{)aoeW#
zh|bvXTpfcayYyNn_5@?OG*qr*Nv#e%Ivb>JBI6dO5-Hh^u0oMAim>;4ye)Dq-%P=M
za-g2~!EiL?`Od&W_^Ji+x9>w@%!h#B+xti>X2*?Wq=;m*@7SR?j0&-*^t7x7RW+Zf
z9?zOqwMAN;L~UJrXD?&yStW%~P27Jzms?GpecUPT>?0f^aMkm<AwDj`JNv@0Zs{y=
zh`{Cx+FZ|c_Hl9E*+)1;;1C~uAL8R6AL3*Eb)jcCL+*-%FYv#c&uNWUu!W5)iOxQ*
zf;#&MhX@?v<KP?Oi@A`0>*H_t&OXkzL-b!x0y_K3C53-`a3bvv0^@l6_T+&&0sKc`
zBN@Vat>)x9)^<yPtTT)P7{;Dm8{z3WNLD1nUnG|(J-Fs_w*h(#zoJ|KBZGHHQI;N@
zY`Cw0W53~|304nIHQY}C9;=$pWr6!U(m0Xun~Wk}&jQ^-sd}JHGE@Ofo?>S%NWEuq
zMv{$;dgJ>ly%jVw#j^V-aG_-<+cL66G9M{WFRsJrhs8*_*62Uo4<spxKAzLge2F|c
zaM~H5IqlpxaN4=6)lbeYe+TCNJiF{{kwYB)d3I96R-aeW@#He;O61};NWdvV9*9<W
z-wtU6*#P?TKy+|Yb8x{qoQL%|MCWvZ#8Ui<>jbFcoRuKaIq&+Nb0FZHgMR0{r=8OP
z?Bbl1Zpib!4%mE5M+m|X_AKPbu3)peAHM_G&~UI>SZxeqrJ5D%82Pa)-Ub_ol!0V@
z87Ef;QLW)aZ&7@!EiaFidW}}p8Koa=uG8x*pR6CkX=G9&!x?THRHoqs<+}~>=;Nkb
zIJF%^mb2Eeh83r$Sx$RWMhH3<*!#eC2-Y9i5nvHlk#zvk&w<VWI{Y}o)n3Z+7?IQY
zD}em4)reX-K)e4gXxWO9{2*p0ZAu9_yVM0~z|%+$cW>zxBsSiX#%l;F2i}FhuRwYF
zrCMUJDjZ%d9xMkE2lK?~U^#I*m`Cl5g2A{04=*-jPvd$m*$Gj%2Xdlo%F9+j-0lcz
zXtG8f);x^=8Fq6=Y~TZ*)gpkCkEP5g2^sMZ@`>uL3`IVUZ{T2cqHk1dElNG<86G`i
z?8<ZL=&5yBqq4Nz0u<GXWJ9R-N8UK6KO<Iw;})w%HvVy9F1U+^&OSSYv;G{d>mBqQ
z{@~PX8&l^aS?6)<Or58Z(g<n8ic#lj>w(=P7%Dk!Bd|4Bn>tV11hm-(psALV&YZ6V
zGP+13I43*0uOIe}o|)@MqDC(WAoV~A@Fjp0z!JxNn~S!QLFG<)(wM^@tm~MM{g_+6
z#~OYF&1fl1QY~+cYI(Y;ecb}3#7{zRD4MpiF}f9-TJ17?9fxhKoB*5I&j8XBS}JbS
z%$A>}ncc11DXKrHe9!U_oY-Q!eprjAF|)bHOlEhK>&(J8>ddkLq<``cGkfsNnRRZZ
zY%OJG%eZcql5J-mELp@gI<pA?((^R;-<9l%GiP>XYn9nfW_IabliA(OgPHwcGHZ1$
zGJ9Ba>&$}6OIL)PxxYSl=In~vsO*j~yA55M>arC=xny@A0oK}4W>>Dc|316d&Yaz-
zwko@*KFDs%9VWYm*Xiu06JU1R0HiP1+<%|l{xfH{CP`(N%<LYGZKg~2p~<f4^*Xy8
z0O|K@?!V9O=$W&-pq<LDgxNiRsmbmJ=EA!B#AFx03E2(O+<%|lPiM|<r&dtO?Ed|u
z$!;u^OLnUW(3y8KyZ8Tbc2*$ddi&g;?N#aaF}vGxu^f<Y@S4f)gvsuL&B(4<2gUvO
zrTg%-(v7O2V~GqCNgClWaTjvsfYRYy5k5;t_>5958IRC@KqT>%Y6KDXkd@HG?>}Lr
zkzQksG68HUhJ%cDI%G`60CQ4AGo0lZzy|5`axje=rV%l7jz(za0L+8VITBf;OaznM
ze)i-xU1U{mCh`c=0v%Xr(gqLH2AwV@-Q-2?_9FLdgvO3~-HUwgK{CR1L}4-NVVh2h
zQF<<S=b1sdsTLJOI(U(;8j-v&)rb`8WsR^<g_51AjxW6me)1xYjzW@8EiV$I5y>Z4
zBa+Yk8eu+fRLSQRuY$u~<Ud~IS1%Hz1Cv+~rV+_>ghnLO8#Ka9`(x8J(AFOJDtOh4
zeBedC@**d^NU)AoVnMh@#DY?dhy{B!LJMA~VnIC}4uoy0M#y%o3Y&>B!uGv~jo&+D
ztKghab)XDe2MF4fBxL!rUq*dT`Vf^KRDK*|c}52hCErkvf-)-2Lm8FpMdo{v%e=@n
zUSz8mxzCF{q!IDf35`hkQgqCs8-5gXOk>Q`6mj<gFLJpT+2Td+@FEX;k!QTfiyEOh
z8DDxR3$Xsi96G3691`47#Z$4Mt`}+UMS6IVR4+2ti{yEcsTvUruJ%xVH5N!^-Rhy#
z#w0Lc+a3>P)Z1R<GcWRs7jZjz%2V5mT<Ap_X@s^}qgHASXPvQ+eKHrTnYwD>g{8av
z*cIFZAIBwebrKf2nV%s2)Ty+0@egu%IU3XX9$WFp`2k|xu>B;9R{^RlUVDSoyLc|Q
z+Ov^S@8UdsKSY5u^YNfpfmqw8y8Xe5tKy+gmamcA_X+WFBdW(?{Bb9OKF8!lpzk}A
z!SVD$k>lojB)9B3%Z|+u?$jr1$1Fpf=8s(w+0(MJWK|g?3+H|qhO@ZDQ0P)YvBps7
zK|#5&RCuE;;GT?SMzBp|ClbXvLt)nf8$TI~28CkXF@7eN7a_um*Yn0N@L*A(fi+YU
z>w)M&6DE9;2iN-K0jyJ=IH6C02ThvrF3RnM*({7x-gUzFWDb+^a0N0Ufq1xG2V}Q=
zuQlOT)Hm?Dg732?Y*qAl!4Fy!noSXU1Hq426GHNV*RY!ke$<+9FZpZOEd@VrO?Xq`
zNrIoShFi&4YxjUZW>`*5G2=D(V$!Qr6Rpi!w<voz#bRylr^Vd>dNJv7f@2N5XwsBI
z;1_Cq^P)+wmU!^WMU&EtJoxrS*)LMQfz4(z0{XHS<^XRf_)g+qGrvZH*SQh+UgC`f
zpFn&uz1T$XZNMikwwy%SL-~f-63b~WSd$81sPon~_t7R1Lshl0xexa=u<WCBZ<6rV
zya~8PyV?t027L0I63ZE6b1xNdxGu_0+W_(io7=UgK+fUD($vll+ox5T3|c$;iuRZ{
z#wJ+S<u_w?8V=wa0;j;q9MV^6_H7tM5MgA7Hae($^Fqwp(!-Zq5vTA|ze8gzxv9NU
zvmc@M4)*6pdy`wV_BRMnJMB$>qMCMDcDm)3C|~JS-VBz#t+^BWLU|wiF!q}U&PIB0
zC3NNkNF*@c0BHu`rq^(ga>9|XQIZBF_91%#m1H1&EoBD**g>F|0Xk@4N(%r3xwrT>
z=vFTGCEwHbgj(tc>UL2b_a|#@b*wyYYM#baI)(e1I9*x<Wi`9#+ECU^S<76w!#EuB
z@F^`Grg`?U*va;kj@sfiU=fSA6QIQc(BjvbLSugbFpjp4(n|cc_O->%7Po3UPh&84
zqSN)mhEQj8x>S6_ql?Y3nG3O;&j4J|QxzZ?JWf~pPHgNAY=eOHJQxNx0+4VUO))^E
z0T|0?gOo$#jFM6)u?M?<vV?TvXJ8N~HrBcLtvANy(22b?b&nEn3>yNDoVE=H=1zvT
zg}ngb2m7G%gPcZ^brom>xx4+UuyW6Foo1~SU|qnHabwcG9cl`u={(sdWbG1MUbgVs
zAk4lDkZAz*0f&q_TH+b>q^F^blD<?cqr#JZz@#Tzg%9iW2zZ{nvhx>QzD{yUH_8Lq
zaXpl=Ukh!C$N3;FxDx3f>vT?6dg^rY|KLd<t<x_?dZl&%_f(5jIwn2~L#*_NJQQUX
z-1{W_QlVQ|g!Y-!-@Xfy1G&53mp;oO*LKZyydT<2iv6nL*h!Au5ojL!H3vZXaZ>F;
zU7)7`38%m{kc0EkI~*(HsA<y(^XGvD=D73994wLC@~)W9{-o(ViP=6tx6<9N$P7Px
zly1t9(NOnhPCZvE?xriO4viODE38JP=Et7rMOr1=R$R1|G0Y?TmXhtd(z@7c=u2qq
zOK3_Z9`x_DnGj7&&g&{BwqI@qR2lg1^q62wJyW!vl^#6?{yRM;WU}QhQF<I-SCm@&
zm+9puJ;^FGHTa}X(=Au^542p>gfr~UR>mi~CQ2}5#dnjU;{Emo+!B!XhCv2v6XKZq
zALpQxiFD%ZRb1(!+2bEyhR46Cpz@tyau(GKD&K|HnqhZSHO&JNy{qd#u>c%VnuCw<
zbE<QcQpa-5@e2CX=ZeGhzR{*Iuu_IZT1o3N)gXpVQW{G$#af@THcD9wBVhul30Sd-
z*@-B;07*gR2L<RFRKAZ!eBWIagr@>Jbg$+p$r2`rV=`on&SXw-lDNe(Zt*F%6xQvb
zv<)_z;99G&iB6qtQrY1%I_gx0jwOa7j_L2ATw^G7>ZnvrktNJ*jc^O&ZDE7kPO30=
zrbi1_;W908)F?%ZunJ$(sehSNG5U~BjlrxCeOX$KmGK{)N~7BtiWvQuhmvY2G`cWS
zN49K}D*DgYsq;;$IHiS7rT(i8Mf7*`P_`S2IHk9y&?yDQ8WEFOJS)SDPrpFt5hclo
zG#T49E7MQtbj0ZN-JWztv4x0evMh<@6YO*U8N!n+d$a=F$s<2@MgQKIZpfPDi)<wA
zK$MlIB7%J(7*;5PU38~;Dk5kmQLb0M<Z`8)7?vx7yaHr2S1W??Aqf{NN6Ck^iaZ$6
zNR)ao@*hR2ryh)GEMbf+RKD7dmWy@D7+IKnO@sNEBpFi%=j&9#x{le(>G#*efpL*C
zMiv>zG6dagjgd9Rv2lX#v&P60<Jd$&4_ZYD{V~UvWzBb7J~-u>BW}&b|KkeF86iuU
z?-e$6giNu2<P6(ROMj%d>Q<MaXs8iBh~r>{yATk^@Gl)<4F571AAYnm(LeD|wnrsg
zAdI=?J_dv+w>(P1m|I>?0)$(h!2gI3iI`+Ki;}R><1FgxmM4V_b{FlXnfSjC|8wyF
zAUgaWOrqUIN02fV|BoVNucRCYv`?TDfDYpISJP_biQ5^P&JZ3J+NX<FNq+p;6>|oe
z_3cAy;f0!!^{qnlh_XREEDO))h@wR(tkGS-*a{vI;%cG`{OG4Z)w1+ay)3oVveckH
zwJd$irVUPclv<WPCT2M0_0+0#m%UJ^32ITg%U&jEqFR&gvP%U`QcKcZ_8LLEsuk(u
z_FAB_!j;zYxV=GetZ=2dJZ@hr_&#fR4%$l!5Uxm{6bt&_ZC0euD94nqTcTE^&&i5&
zJ?XL{ecr4{m#7u#3uZ;SM6E~-Ub#fANDaPyiCU4qDJ#lvpiiwx_X*zl9^kSfeM|6_
zz`5ewFYCm;#IWLgN3g_u)r#{yS#d4^rq-B;gn9tDtSOJ#+#A7~a*0|~{vd104m&V^
z=c@0kKC0dCfiIl*?Ze5KuEi$cSR5E+O__fm0IvF|G2Kd5>}E~55S@i<%EcHs)5GU^
zmVLQg_SMtuC1`g1alWwM$5Kd^eh*MFmwx*Rj5h$6ev;A&z;7VQLdqzzQqX=6*(FrM
zrC<F0kmb^^4*@Qu1mHr7Kw#;Y4@@onF48uITIvVt9-=y~S}q5dtXfWEGOLzxT(ykS
zDsZesexM?-YPpv=S;LcHX(^m4E3c1a<@F{gvhwPeqP&;`9_NVN0Bo}I3f+l9G(rUG
zMSv?W0l4xKfGe*>Aj!(hDA`GuAWrntWjFu^DZ^-;i@%_z46^c?rK#2c5x*JYfim=e
z00zp+tNj3FYMd$RVE1LR`d40M!pfCbs%F*OXf*0Ou*%A7FS!b~=sY>(agM{K&|<Ul
z5`Zf&0dV;ia?CJ=<UF+-+(b#Pyk60oxL8Wp>0A^Y(CHl9r|EPkN*|+b(+5p3)mw~c
zD9f5FbY&*N5zc??TUi-g_=G$NlVw@+h~_{w$dB`leIcAK%bKC!l4Z>%02wcvwuRy6
zxXRvLsh2fTn2^1r>0H)?K?O$|(5>{(G{G!uR<iJS>b}WEKxf4*M?W6+q-U58Or|x8
zsntCHN+Il&fjujQ@Z=-Dwv8_`D<IRi=;NDoHdrGRnqoh#)3{c~^iS1KI+p;K_l1k_
z!}G`v+{fAxF7YqwSLU;P0Ec0d^I181fLMW9H3l_QLB=+%g0uXFRaIO;`U9HI^UVFK
z(o0GIUDF$2fA2O$HzxYy(wVG)a%Bay0Kn|}r~p`o91#FB{DaIOzP~CX&q9CTH9)kc
zOW87{Y?&CjA<8ncXodZ><c~^9wYiLRoyry(IZ%9tB?NQI@lcu>3Y{>jKvQH+zD^^Y
zlNSmb=j769&UoC%D!g7xELBQTMONW4omOTtVu#51L#NWXyA4H*i|}-;=M9C%Wi-|l
zHo(I=hp<yt;h#GF2g5BnMCgvl93n9GqRHsupYNeGH54g+f~Lsqd#XlMTSG6Iq!T#D
zo~Yy)JJL%$>14)y{GZRUdFt6HctF}XqGUz#U^8#pB-4YihcNl!a{-0+Bg%QxW|<l-
zBZ_&G!X73z6BDH^Li~m(=S>Q2`mmWd-Qb@$$u#*!o0(#=JwfKlH`)UPjZ#yg8|@*2
z)>Ctz3VWoW32HOG0+UzDCaRfF1?H<nlZtZCA}7g2`4*YBU<x|v)?CmkHJUR?rpjA2
zI(3rFm2Z=&118J02C6<WrMC)U|9rA6mx{5!$s`zaWtlT4a1M+qGN-_pAnTcPS2h3L
zgUJhK(3~o}<%tCL0Z0n*SLazLzdTT$q)m+rR<&qXOdf%q^qW}PG-o}^F&VC(Dhre}
zhbxr*#N|<n!Xq95E_IRaP&S3QJWNTKBc2x%7iVTT`Jg`xT+LfEoN2(ZYEqSz;S>w%
zQ}fmgXP%%@s_sTPiv_Kx>TZ;?T+jqnccYwDf+iMKqV6WkI&Z9G@0OofF_~@9peKQ1
zrDxF6m6PSrqrop-$rhUF@VuQ{e#OekXrTrxTgkPa!8Wg)%=T)~%9WGZUJbgPR1fz~
zl0EpYm1@2@!Ql!k`caII&+5A1A(Ey;)o?M-nua4Jc(Ru?O@<5p=YYZg95U=AD`UhU
z)!d>QSyrWA4ucWo&N#@hTxdurUm5>nX||dF(cCKezn)vZo&OUsE9U=1w|p1>Ct*ti
zD>M9c7wwUpGg4K~Tou+E28+QQTz+_kTF3>LA5{=`dXC_<7^Q;Ck1<Uab{Gm(W0f`A
zL70Qq&V`*U5xKIHmhLV(4nUgt2>^)*_{BP3M#)j7USy*e+2%za^&)${$RRKCkw#=J
zu?MU2aV)u?<ug7#<L1t&_L^PHO7S9Ny~tEAveb*L^CGu;kw-KlW*yQ9%^EdandMu9
zsy>rhT#AS1IH(%SZfA}9#;d5#kU;q=jG{}=rD&Q_l<}&TWss|d&c_V=b?@`nJtqvP
zd$cND^=Ma|1WB_(#Y;SsD2F9Kb_Uo;bhR^}NwqVevD&}XS&Dyu^CC5ds)7jR0x#0m
zi*(coz4%{@c6>)BO^b5c%^$nsUn5PR>?VQoI`m-`D7#6Ze1o_I#%>OILmvY!qf~e2
zIpCv+Z_Psl?C$IZehYEg(COj44*VP7D%kaK-UiM<DNFPo&H+JvMMwMtRc}Xc>8OFK
zk4E)Cm8?-cQ1z3TgMsQ{U8rmfe3%1Z%f_>-gCR)vbkfm+)Bpq%H~{%%hX?V30}#W7
z+UZHqJ3R;&zP}%Rrt><)Fu4wK`f@D6!sN40Gx+w9|4pD7zYsI;0L=(TtyR7HaOQ<b
zs+`pVf5Y5Vef42+_c+`+9E}noI>cVd%9`!H%8*Ao&iW;&u2u=JGMs%Iq#i^mD9%PD
zH~m<}?ZR+u3Au57uT;<BOAF-U@<5Ve`IbWLDkNF6`3&|?JSipvUldYmLVSFQD8$DX
zjY52UrXAwjBk&9OJNFM8i#g=2GO+BA@lJ>}8{JIbg1Bj_x&<*|7V}U)@$=C@d_gzW
zz^D}zJgs=X6~s?e)!5$;bjKH(2gQ!<>xEIxaY8GxNOyuldHTq)L;6T!@ORh;aeToY
zQ2d~MC_=>QnUD>7z9GO%2|`UbNCf-Twf>MC2+5#rus<^CZZL$tIT6LI`Ev_&M&An<
z)@uHegk;OtJPPn18wvU@L^d@~5{~lS0~2fhNw}Ubqz>S}2q*ZKL2u1dgcHLiQjcX5
zNeVjvV{n@k5p@}2O-GRAu<|;f;5AB6)Mbb@g9S+odkC$oW{4n}VRr`s@!4}>VNTfV
z7-(yT+lzrr4XYgvBm(0pX0~BHnNO4;^TQUP&^7De@fa?h3A>b(I2(0om4>|(3#6{h
zbytP;M77ndXRm;^)nPG|iMOu?az)q@B2DZSkSPmmOPMxy8Ia9kor$y+q%y1*kt9L3
zhoul{C&<pQp+wr-T_Ce7?B!ZOI*M(3!rq`co$YIp&+}IJW-#z+N)5I*eAS?UCh5x&
zYgV$*@~x_xTM1ge9J={d7T@P<MlapL(np~+a@82(W1Wgl7DIG0l^ICtHj^4nULcXo
zAB~$JHP!qK1+R74U2X7pJC^i8wKjeTTGcOBJvoDf8*8DOqyCNOA1k7^;z`uXs2_XK
zmd3phYNO_LMeB>|i*(njC7-yeb!Q?4s(j)q7dz!awRUEaFt8J)uWLu?`|#I^TF@TS
zQCFfv21Rv2O0ZQ+?o_PxbQuNyi#rv&PiH=_?WWkfh&5K!lbAi*QK{&&PE;Wxj~jJW
zGw|r^7i%3jM%I6F{bJ}%3()>+eVv77>RYf3emh1-TE0l6*ZPKVkZ(EDza{MR#WMXn
z!V$i_C4i3;j`EEl&-a95d^4E-1L1nUu}uGwaJ*GZz6lnd+z8W%|Gn$+fA4zy-@6|F
z|Gw)n{F_fPp2}U1;kVlisFqWQU-$~YL$DyH4!`gfey3nQr_P{B!0xgcpY>gj;rGb6
zQqPgQ9>a0h<7EgA>+l<MKe*IQjkrJ9z6nX&O>!(XXc|W6$C3VqpGG@k{eLtU?t1(P
z9KZRws3E{5kY>eQkKveg#o||Nh<!vdy97f^IPQAnEs30!2`z?!&5Gk!y~5R9kGsKa
z4*`9Q;ZgE(*JHT4>yg3ME(U$0pNC1Y9N#>D`a!iaOzwINS9d*%@I4TG^PdXyuE%h7
z*P{p@g<u3GAF6bJlQZvn3|DtOif|JMwlu=(uE%h7*Q1bAKpGMt^RCBmb=M<n#O8-M
zrjh&%eJXc7hO4_Cg}e%+D*|NR^%$=1dKB_jka$tMmgZfL;p(nO)~3A&q!$BZ-t`!+
z?s{Y`*&l)Qxt}a|J%;PM9)Cx2Fq*RR2=lV!uE%hF*JFLqnpC6nuE%hF*CRidFrXTp
zcRhydyB^1bR^X>g<CVJ}!_{4n;<*)|U+L$Ob|rT`hO4_C**5HLpvRJ1-Srr*?s{Z`
z^SED>sHYW4qw+T;Az-$|Az0mS7uALL$jSY7Q4hbtYEk#wMLj}9-)|T77!8VY>Tkph
zE@~H%7^nWtOnaP2J*WQrC~wpgMD+c3QM-vGIC5QG)F68@)?WqqjoHskRO%3W0a8j$
zYAhu*X@p&d<Qx5@Ln6id?V>X5dyxF7A*oIn&rTR6_uCx-tNjMZ?-`a@kyh7@$o+QW
z^6x?B^@n1S%3JQYi<0~8ge3;ENM3&!^96Lq9-|F%zg?8vZzn8WK<-zKh4<S<$^CXL
zkv$${eqTyykwV6|#gZsW?za<`<se^CjfMByMalhk!cqzH9fn2SZx<!^+X?kwpgbR-
z@_xH0x!+EVJPgX0q|*7is6|m=k^Ak0<rK)_HLJ4le!D2S-%eN>g516ei&n+^?V{v<
zyBF9_hk$1c`NV59&x+^$c2RP_9aTBAK%Qq*sr&7s<bFG$@*YHf5?q-pl_&SxMalhk
zROsAIazMWoc@#b1!oky#i(R|h%_!SN*oCcq`8ek8T!byy+E)+`a_U={x7S`v7Buga
z<v^~=z-q44xhTV`y`B$sPdEwbR_&_^;%=sd(N^sZgnhPiK69wu-)@bd$`4A%oFI=%
z9ccGQ$|#c>%k1QjrVo{GQWg8@e~D-+6^^u5Ao)r|@-*q%8U1I#>b5XsE6Dd6mROON
zCS5x#ay}k}z^(T4An*6H1Po|UR50JFo%3>$<~s(S8gM{Ncw5*Q#Xl@yY@8pGvNxDV
zv1tWZii-M15a~xTmO3_`%nkV`(GL+-m0f$$n3+n<84r;hBc>|5_OkP4Y3gE7RvW4+
z)&&`J;gr&sXXE7-=yNuaOjGet@<Y@ft1EspM&z(IHPA$pkZwN>o)^eRv#j_92nS&q
ziC7#7^#f2o4N(7tRM>>?&`E^)2Pk%Jzw9rhc3B9A3iUitE(%a5H3T((b4_>}Ua-4>
z(u-7j(Te{VYy7ZT6EUI)bqpvI0@RyfbDhy$z$5;g2g(XV)qRO^tj_R<(Hqx5p?y8b
zTm396jxZ12qF5(;H$s<ac^v4=eioV>%eA5vzm9yX2f$L{`xxl=XX0y)Etoo26`)+g
z7Y6Uf@K@Re^`TgBWSxt7LPc0wf!w7k%LO06Uv-LJ!Kf@OLk$^M0(3x0f*QifwbnO>
zw#eNuDUuy9avZoKS)J<U;pnb&WA)9$(M!2@x9s{oGcnh@n1Ln8u76(^kmW>tcKxR&
z09ipK!mj`Hcpxi@MA`KxQl^whj9ov8Y?lzJXV<S!WEBe)Z`VIgw$(%u)Xl@umy)e9
zC}?-lm(jXJs3zOxM3U6a!_ij|>8frXj=qvevbuRVdM%Mub@OoaRYcO%&BM{_h-9jp
zhoi41lA~@Oj^03Is=9eNx{OGPx_LOdoXBEz^KkS=)@rG`c{utSQm#-p4@Y0iB9z(n
zchJ!5NZG7z9*(}AluEn)oh<eyQnss`hod(W*=g5bMVT!`cBz|(qi>`S_Sp4@kWxVf
z&)fA|5V?uSUb}uVk(-IUX3HJJ(S7VbXtx9K8^d=N)Xl@u{p>MF$uX(19fic3hocAD
z{4UipL-IttXt{Z~I~)HsAm40Ql(gJD94$8wOEY^E<R=Y_x_LNSZXOoueozhtsJwYN
zT5cW|>JOmslT^y&c-}l5EjJGfH3}^;4!@epn}?(2=3$|>0i|Pr%8LS{<>q0b4g-ZB
zUX4j+iRI?uXt{ZqRHx9$sUqQ?TX?@QZ>o!OWBM!twO3QjIDbGwI_2#;Z!CkZ+Ci)s
zpVKrI-7{zkC{a$+A}b~)h{3v^(-e;fV`2qKaGEXv5+_KaQ-?!dOuZm>xg@7)h80s^
zDBGPndl95#&KJ!)ou-Sem<xom%W1mAin&nWJx<g4Qx;e;2|;Wnd!0JF5xQd<260Dh
zpHt_vIlvl=hW$>RCDVX45$vE-Ck%$gGz;Pu*<q(nTnVs5(Q?G8Gid>^7C~%9N1Zyu
z3V^ke{Ej<yE?xqxjc}fDnyn(%Rxk|5&Bl{8Ndz$>H{&CZnD#;RVHBxzfp!dHh&clE
z2QqgK;*QvH$2p%)#=Ba_Fk$%tzcEGhuS(6aow|r%_?1)%$Fa0r)3@0TL2K=&&!T8z
z9!lZtv{`9(KhTC7x~D}uPre8r%>%z(0CI_8i4_&ixyEu{#p5g$IB~tIXe$HT^@dg5
zmFN7No2R*W1LE6;8q3C@hGH3y<<AnP$uJ(rVF$O-T7_EpYY5h=X9^k+2|(3_TiPEi
z)xwDo9Of6UwrCZ!y6`}|&3r9f0Kv_E;d7epDUS^4suc6=Q&Y9B2cU;9jH>Di*dBoV
zhMZYfrF{Wfq;%Lnp(p7)qwAc7I!za^VRT-x@6OS>21CzOzpj8a0{Km{;zTaP*@6Zu
zHt^e2m43NuHdL2e=RCAT%RLU65Bzdx%TK0jI@4#JIi@y$fna?!K;^`<WvYZ*3_+I8
zDd%%DAH*HM(o}mu!9c$fX>;*~s3te*=xl9dHptV+vXae`QyVpdbCV~u*7K}OK)&3t
zs2Q9)cp)YX3=#G%pxhpyat7ySlp5KmKzS}e<qXc9kc{>&vhRcPv7g%FPT226yN@r`
zWL!cl#c<9?c*U>SXvGbM?al>mIVR`~e0Bqn_?onr+{tYMp9nb_q@ji^jYQoR>J+&@
z=4vnGgFf5OBaUjp8$X>=_oQh)xVcjGfAHN{l{fwt*x}|k=zze3z+^uJ^5cfZiZ4Yh
za%bIx5XXVSehZWX0qQ&$>MnT^JyEFNf%0>JIv<5~*OZ{rgc^y65shC}*7)&om3wvK
zGEHp>O1l8{Jk)}_X>&^yl2#1{Wn_T*HvH#SUX2JSt-J`7xui1ms`wiJIff_q?g^c>
z`n4eQS*6ZAho(LHBDx7<yuBS1x&M$YN!{z}%Dt|l{zXvVqO7Qw2}k_nSV6gOgcjjz
z0QAd$K>m&_q9E=QINsUf7MVKoVUHskze<BlLR;L%hX0CtT(4+wU2urhQAhA~H@sF{
zS2nCsL4Tqi#F$YaWs$6M;u_eQ67(<}Lb5X#q{RX0xqO~;L(o1g?UaL5VaRH%h-=;+
zJii7l>!ejY0{U*lW3}i6)lSdg3yc9yR4DCq4uj>BYHS07zkpxFPAAGIY%Uf=(fCzG
z_eT;dZUJh~DGzRlnh?udfvLS=v|7|5`GepaH5u*+6Mdt=lI3TME63cx`7?O69)_H`
zAgv==VuC+AwXl@sR9prcq;oKIlaABQ?O=c8bP+4=7nXc|NLNfv#KJeg^l4Q_Hf%j0
z3ip<fZI~Pg_epTZVR7TJM7TX7zoX+g`$J}EpE=yX$)t!hw1Dvfv1h9lwLn*~rN(`a
zT5+O7*Ps_krgI>Dm0!ad#zHO9GIYMNa~s(I<rk@zDN!qKFmvf2x<kik=K$Eh_lr~u
z@I<Y+E79?st3n@HqzwyCz^h*PRo<v3QnhGS+{bkM-q0DvT6hoy$5B}IZ?yARdeMse
z8S0(?gg(Al^>U{KOl!y(sC~v!wG58C9))p&efJ_Vh_h}6=QeUvtrg!1aos%}3pWe(
zNl;!gRGAHFmTzxFuNRh&K>o_lqI`88Vv1YG{x>3~@VHo**Tiq2POZ4Ts4lmOjoUC3
zPczWlpNXfB-NpD|DCiUXJRE{zSu`u|2wIOj#(v%Ofcaor>1R|%3D0ah%rukDpl|o{
z1eDdihe7!=`!nR~JOu`BJt$|2ALUOuycJvZk5zX$%In^1Ux*?(zd-1shNlY#R8-Yt
z#dU^9aVV!ZIx|U5S4a=1hBI^otHsr9j+MR#!A%z<G=xiND2Bv*BW1-Uv2ncQn_~L;
zdXOF<SsI6!ASIV3FHO)7S#hHob_e<9nEj?#Ao!<WSmf?^;D=3ghRDgEKoP}3oMBmU
z(^=gw*%#@7(1~k=yC(7LF(e=-5QVL{*HE)gE8nw-KQ|%483CDW%86G3wbrr#ec7?X
z;!$~Y$U^X3=jRI;C!1@5NngvhtmS24c>74ic0lB<Gl+=-znJPvEuWxlari#%-2Xr>
zvavBOkmVU=_h7E!yc>Q<*IyIJb@I!Kk<z<cUJ1jT^J>i1@pm|QCi(gNc~~uProMhP
z#_Ddl06fbKpVjg>snctu>k;xAP;NF<6-HXVLhs&J<0^z>`tMPYpYgNELZRg|Ex~fE
zMl0Q|-vilca+dP?BYYrSTP-KEGOvkfrspj6A)I<P1^%+BmL<NwjZLEEUGRm|II^y8
zmO0Q+;@2WALbmBz-ORQ>F7l*~Fz6yR*hR{~d%fYeT7J#mx;FA#9X{^^<#AG_gQ#+k
zVjbKOnXKdZTOfa76i8E$eL?NVL1aD_8Hu9OkAH!&W>e!Nu}&(s^%Lyw3u_+N72B{X
zkG~iyz*%j~XIZ#|(Xko6nYSSkXLQVGS(@;(Ea~))og|-SSpc@RRLS8_mdr7C0g8Q;
zJc?yA$lFh6v08XO%R*`RP4-Rfm%;od1)ijm7M{<t=m=!fIQuwRV4<+Z`X42zYLs$G
zoFQbjlh3k<SR;y5wHDop6&BeWq=q|_G5bX+<45+xQ^mHUfl`G%8N}(7l_<nBKvw*T
zXt4AffWO4;RUohTvq;7=;}0J3wvA~XS#Ae;XEl~LYeCC+9#56>?FIQDSu)$fkx~%J
zt$hj&TD+|<)Eoo-S3i$bbo+y_H29Ji!DQEf{c-qJDuf)r5^}+7+u<*9yA8-4$-?4T
z7ko<LYr^nXsHuL6)sWw8U?b=tA8+X8+65#sNP6m9%#=UPOJKG9q5F}E{^AW)EN+Eu
zF@EtzH6ioG8)oTqlH|iSFM{nYX7DUBh}}!$%olGyM)H@2WF_h^-cYZ75`RLQYbmvJ
z)2S8-mzgi#2wNktG&gKk^S~Exm>JriD_n!YF_K(wF!$zxFWzvt09TN3%?8I(!=(as
z^S~Ex#EV!73hNDEt2C@u^S~Exw89YK+5?VP4VP9J_~MOb4Hed}!S+WLR?Q#y;*IVq
zKH)#V1)@HFr9`SpNq>>?PpYYT;EOj}GE5}LKx+26B?Di)(URdJxdBqUs!B?B5^trY
z_;c6B2flct71a<$`~;4T5y4~p-zW-v@kT3(5Je53B+IWz91w^a=TxaeZu7twZ?xV>
z(YpjX@BJU?wVDUMc%xO;6qT<*-2tkUUO?Nd=FgPE5Awwu))?AhEn)o?Y=4^Et>)aw
zR$si)3ZsOJ4T;|gQ%;m+gXfDk(pvdVr-6pWYVY~t&3L5dkV0#$&c9>+VZL}X$DfQ3
zP$D3UN4nHO7x&3ikxihhp`{_ee)Bfa4(2mBbZ5-;_V~l*pvDbp7=b@3gf#qVAT&c~
z2Lk*I&QXd!h_%JviFP>%V|_hYz-CY&rYL*5eAgS4o9Nq~hGKPTTZF`wOdM+xHT%{9
zpv)$vq$(vyeZYZ6cjf0PV%yIIlmAe9H-4TXwwt7xqv_rFd5YLxl4d?n(Vd^Ci0${*
zM~D?Y_<4%(PCRMSli!{QSKprCk*Z$*!blaDm(oy;7Q6^i9SZ5i?^wjiM=4nI7lBtz
zi2~k~YFB(qOS>rr{i^Z_OK)DZ6!)(>sBM<F7Xwz@b5<XP&Zk767gN2}kYBpk0)qET
z7aRoajYy37_6V7JckKl|(}v?OEyT&+TjVbfv-I)tELtBQ;ShnHprDP|<O%UHgAgB&
z2lVmrtV<t(Lj(@-@svx5k7kDW_{^Y>FBfnhfkOli@$s=qh>uSxae7|<((@s{Ir5jA
zQhj`UZrR62I7HxNX3Jr!FA~P#e?R%`#6UnHKDP7_*>dUQ<0Pq%hLx!*g_-v8vElX6
z=5+&Sh;JmC1I)u@AjEgSXnO>I`}gs2a17CZ*=PIsmP-o%rf}SIKLO*<_#JR(f(E#q
z$>Y;jB)jH7PKWqmh8i%W8GeOP0K+(-EBZq#+8N0?P$Va7gLteeWP;ll$zzQafz;n7
zk{}1FBH8!`1)~;1^4%FAmR8{`%O*iM2Q?wnEVm2{w^w1xMVkwmB}{`?p;Rsp9}QWo
zLxnYn2WaZ{bYDj5zl|UPd@&H2rwl^r+%JIF#>7a9Mj$l;T_CtEr$|GGO|ydAQDFEL
z*zgN(PY}|a;-qly<~D`^4|Dsahd`pAu%&S3<_-Xn^R6lkBZMKu$N6CjXK(IIu&gy~
z1gK{`*;2=DK+fZ3#uiCUDV)o>HvxLkE0FrcEbuL6`>;|tm2>xi{Lbkt^5EM_;dIXZ
z6y$FV3kz?ha1Q7G0Wi3|$7(C(&M2fuBkcl{PQZ%fN+X3~-c17D-4NNVtP}=$Hx+3a
zRns{kb90c!Xf3W0{tzF-drBpxR)F%Hp$br?$?XMcgnVm+P8uQK8WBi+YeYc2wU}&6
z#E)b?(l!Tjalt12AyR%f`cH4tBq@m$KBaIYu@J3`Uon<|70HEG3ZLECEr1U;L;`F$
z&g(Qn?)Z6w5G1(tAIVRb1odKydkI*F@Y5why*2^|SPZZWz*K&^B&ZjoXJ0FYyU+Fp
zFyC1vleCj{uoQdSWh!F&X8d^!pu8w?i<QE|FYXISc+*HpjivCBiu(c5zBK7lo)jL3
zaeqeIDU+`F?b=9>#MoFDzfyjc&Sw#B6Qs2@=}HeDK)Ah-HmGVkA1=6KkT$t$Iv*>z
zvyir^YC0bZxNDHMp=$cJ1f<`DG>+F|4_%&G7h{Zjm_Z0pkMn|*_t}nK1XGPpno)o%
z?I;Q)wxf6D+%enH0R^!g9aJFN(R;cbF-vJjDKF8AMwB+n16F*?w4tneS6+u0mY-_5
z-%}RS9O*{#rLEAUGT`nS@TT%hGYU`bkVcS^JazXVf@}{~%2x^Cy#~CGdh<(PoP^{$
zwq%&W3}ftZ<hN3|Z|Ob{=I^Tra8Sker4pECxy^)ML=%lDw<_IJ5NidW`aPV^-jB^n
zzv#GtXr>k&%vqH?6k-g+JvodH89j2C<?BVz8q7J9J6V%uu4JWfFVmfcw6{%eY)9z_
zu_1=tOv<HCED6BF=L$^b^p6V!J^*+`p5<O{A0{-9(8qv=X3V$TwW1<p)&c{f4!A{E
z?N17KOWmI!H5?B2cXO(}S2R$qhKiv}fyzn^i<KG{D|gt`rL5dxXRsmzxu$SO(k(#Z
zT~#tHUNDOrjx6_x`~rN-Xs`0Yf5w6RWd+}u1^5-kgCf3aQ-#u<!joz4lfYjzY6;+L
z#whb(&P?5-z*#*qEyW{TU#V113ZJIBzmU1B#{*UhPqVo-kyg*7i^dc_?Xa67E!(8C
z3yk3V>(Qa?h$Hy&dUPlo%m{f`Jx0Sgi{dEQO5qa^dnPn&J55VV%?>RF#E}-Kl}C$$
zt&}zi(DEQO?DuL(PuqY9xi^&4!}Q%V0lyroM#wef{{+0py!=)QpBcNqBP|GX3x6xH
zQuuJou7k7|CSBruTB|}R{U})GpVnZ`@!dh-pnGL*PxqFHdq>F!sHTIME+3$(T?<_o
zH_Kph<0#k~y&q0H^S7SXL+(2Kj^UgQA0Zmc$JhJx>$9Bv^2e@t7%Y46JB~9tb|bR#
z0rL^f+D{nC`VrV7yH{b&Je&(-N?j(~qqA}b;<IHK$X2<uw8t(CMph)p)3JQQm^6$y
zCQtI08rwlE4ZjnZIo)$PL?-ste3R7sz4(qGOKGD`n9ol|`!&pi^kewVW-0eTdNO$p
zUrocedaIYm$}yj*`sO2SGW)ZMhXp)Dlco&_vz5GEaR}nOdLSDP+`l24DU~Q{FQnvl
zL`w1Fj+NUBz!L!SW@~Yt+KdMaGH~W4O_De9t^E4Kl^1vpms!s{;wsW4z73CK-iew4
zn-20Lc^-%2(<p_^yO8-7wbnv!pgrYhXoPQ&K({?yR8Qh}qbBjYQ6>5#CX?`*wC}A+
z>OFhpHJZ~4KB3Oh3JNi$<&RzQKFV=fG9183Me`@{-uFt)gPFVhoB~JT7ErCJJ2k1c
z7GIpK1+Rc$L3Qdh3gP0071Kum*sEn5E3J5R>u&7{@%f<L1M%!>01gny25=HU{xL0L
zA5p$u#6mm1N|w4EN-ZFYea}GC?DL+~A|JwCd!QZ_5$5jGZn5%=5hQ<P$OoEe@w}fr
z(!VM)n@wK$2vxNdi^CdIv?lj4VhJ8{CqpjtlUsSnqYXKb=|m5Co*|be8|6zp<c)^>
znJIDJMi2P`Lr(2$q;K|+_Ze~^@7Fx!?+iJ!pOHT4A=kv7hLqcvV#pCA18!+;$N}Yz
zJ>+4AysN*F?&%@VFyy2DyoY(nYYlm^UwWd4e3v12^^<3M$a@Vr!LR&M5BW<&KI->c
zg@+u{N0)oMpZu_g+{BQR{5AHPhn!-_C4TA8Jmfq>-tIT;q=&r9kWcs<Q%qXG=UWZ=
zC4X~h<st7e<XwL06c70$L*D7Pe4>Z^mm$~lTRz`Iz5rccn!^dd^g0i@ry)=E=e^BC
zo?yuJ3_1Tv4_WnEqZm=}MNMpjNMMcSp|pKEWjqr2V^`dsjMJj{^^Z9h?RKopt1(B!
zxipSd7{~S@xX-}c*s*0Gvoj=xBU_LpWI_Z#V|gGZDP4Iw&L1m7`{D+M6i*Hxfqf>Y
z(awNiPHlQW37m6djh_UbWnt>h90mn*?7a3Vd{u%Lqzbz0Uye0>Lp#ihbZ(){A3x5p
z;8n;jo2&b8!8Gd*0Jc@kmHs`NHw~Jd@xeRD$0dHRrx8702Cx-?HJz*{s;iw6eAuwR
zXW0KDdvP|{J7=gemr?WV;I;jrnHv%f_dv5BSb>y5Oj!V627rQyQA!TZksqO4O&0P#
z0AMzy<xf#<%`8J&XN{O4t$np2EvLSU7c!*f`iC^J_Fir69$LFF_$6cQJIt8*2|x>e
zX9}&o@EI67okcyWg&S*=oyEbv{<^qh!6ga=pn_#gp@LgP0c$KxTkfy1QNdlPfHn3k
z*;!*>l3k1~)9lA->?OgUo4jf~tMd|oc_lH0d5s28utUpX1sibfTC$LL2LO+2AJoj^
z+5;N-FI`(A-qErNSdBWD1&cG@Vy1Mf0L=XtrqI&*&%x5*(W((tu`~@7v2-2)kEP+7
zSuAa<k^j<CbX2b3)3o$!w(^i&!HYKE#iqK10?vImA@eztaXo7%m;E&UdGyOP6pj6|
zi}d`hNRocpRnRF|8_W%vfx`E4IcMJle9BHpJPNhrQ=w_N%a}6iMa&Be$EbpLJc1oQ
z&=J1=yNey?ZPFU}B!Ck;%x<RC_^G*173g>$bPC*{#VOA`{LKxJ$j)2H**)fAhIm9P
z8U|Bf1}J!_z}=Li+DX=6lxe)1&E1p=7>}Q^JlL8rwrb_s6+gq47KoRACw>n0tR%>%
ziDNDj$K(vtf((qvkr($+Cy%?PX%ZC4k6kes`WE5Wvna{@HWwBZ`Bxgy&sv!#)o|fU
zv}!DmWL9crDgIi%9*Uj=yFcJh2lvc-7C_w=mxnV`_9D1))An;M8=?(ycDkQHIJ+-s
zyTOz_1i*&?tmzW~9oOvsp8bGZ8@)E0d(+O#V4n}52>@$4x2jK8XMfO5COdbj2a|mZ
zfSJNhV`9g#OxT(-#ysZECOeHOBRh?GNZ4sif@b#{v&+5N7;}K^H0F0<r!lRnvp?>>
zZj8C`6>Usk0Ln81svC2cd(s$FKz15)g|O3@wCdXLc3YxTh-dC4JB@i$*lEnf>g@Np
z+};w;{780sChk>Z%v05kdCI+v?DR}`aLuAIwyLAO)m8t?eb87kiM+G~yfkBfb>64l
z4~-e6<fR$5Rb-k8V^Wc61cI0<7Q|GbAf|c*G0qpnxK<G3I6;hC1X-$$Ba(NEM)(H*
zjcD481>LKSh@M5B8-BB^hr}Y|JW*=ABZw(J5o;{ZBp=t=MCc}HSA2rZd_z2){Wka|
z=cMN2+f3-&A?VwU%1~SLd2-pxRPFevUr944C^Jg9>i+>sisCgNLsC(DFOup-rg)L%
zUSx|GdBlTQ^La%2xK637CJLAd-VYnPr+ETH`fCXPbEo3NIyVH7uoHMz8Hzk!?qj?u
zXdKE=>vhK}<`!(fELE^0U~ujUYB&r{umb6sV94ga>sA0$l90)rL$ObA7gD}L%JfkH
za<!~aJH?H+bHS9o1hheKK)x8jH3aeiJPM#_x)#ZSf2^Y5i7IneR(=bOm~pZCdOuSy
z#Ss17@Vd5{9TgnZa@aAEpE0!B9y^Pruuh{EvIIpRcr$h;DJx9I9jdj#DjC=JXN;*1
z_oSEW5|zM3&O<?MhQk4`BmF&=oBPlW--HAAfO;2vn|lZ;Gmw(|8GzdWETAWEs_x0Q
zws;Zml_@U{BS0_C22gan*Nav`sxA%oF62kD-AD-<@(6&_SPE%JA#Erc?)Al7jTm2?
zvl3c*eZlSaUD_@St-#qG)O`ee;ltMQLE6HN^_p+N7s;SL0ypKxf|P=k+=~EQ3_yHw
zu(~fMD_^__25C`s-qyb81fb|hRbT9#pehqvnaUT;Hj@%GWEp^h>MVt{qmVWfy{@ID
zLH?kTf8>h^9$!f32p+Et!F)=7oacfrqd(X=Zl*o#9M2G7wSEC0zpfTx=inrW^BK{V
zTwT}eL2nyfVgJ^;S`nbGbO8Ao)pT8Kbj_}=>z$wrM{2v~8(o`>t|zJMk~8SatF8Uc
z_qKaN0ViM!Wq)v9l9gw|3TKI<H5Df0ncl+mnVxjha+rRpC;cy-3)3rfI^2@iO`F8@
zhdumLJ?XD`(t~wflK(SL`b9dOFW?3tSaMRgu*|_w#P2>?d95{_^SF3L$K2G)PuA(Y
zsz799|C8IG7B-9dKI2%v0U4!3OG~8k$F9hRk{$T<&*MIVhZjn?ev;{l`;lu?Eyh7=
zqt106a&4#6IquNkoFDVHS)Jv4z9o~ad2aVl9p8e6f2Vu0oR2)RO3@2B_xw#>ApHVK
zvLg6h(4=>;BETo6Eawk--L#N+;Zoqy!F-VLEO8zQa$*FJco#zg4=L<3mP>I#PYljL
z*0b*iO>rEl&e0@zF*vfxF&1j9dFN-Vp0f_gEFVf<bdye#j`^%cbjNgjdDS9zm03eV
zuo)_>v3#lLxR!oI<$^l-0U5nL%HyD?U`N&-?VxL9?_$OQIA}UfLkm`PP`m3|L9Y^Y
zV20&fCwSxc80HY}S3>Kes3`d1kzk&%adFe{qN&(ya-IsF36Jo`-$l>N2mY_%euD2@
zIYWkNgYQ}?ehr(Vtc^8(-5Y#ghW6SH<YpD~;p2sxs}xx~dxJk9#~7sl4vwNrH1X0S
z&@uhhE|@;+6g`)qNCXVd{t&b=hY(HP0k9Z=l{aOw%Ku~p4s?UZ-}+c0Yu<~RJVHq6
z(lD#6>8$*)9Hn6kvWIk-`<LFwV_(CC$5`+2zGJNL=oDDUh1?`96mE)rYw+!~kPEsS
zX;=x1y-suOWU+4#mLh)!YEgx^$Y@@C2WzKJ<24QZu`9lZ#$Pja^>YDT|9vd2zRiWQ
zKV_$@*J0p?1NgKLfFFPf&R@Yi6Hxde@PtKz^Ks*}kI;OM!x;82JA_St-4Wej96@J)
zg6S6z(45fL<X}8>>WAzYjl%Kl%&TAs03(oMe+s2!n@zg>$u{ZmRvZ!=<SG}Vj^xLw
z6B0fK3gxs8!~;NUa8E=^L3NSY<dr|!h6inn%BSTi<(0IwbI3TO_)eo(01Ewrlq|AU
zQ^s_ytYN-VwvWoPLbe%Y1)pgv1)#Erk&;EWYRZ_di~WvPR#TN4uXu#mV|B6rGztZv
z!YQAl*wsa<DJKsaeWdp1*4nt102<lox%<G7#E8=o<D7>j^>h{oUmPeJs(Bc@^5%F1
z=2a80ik5m=Z_x<D-2K9e=RwWS66!Fg^~yTBwkZ3nlC`VPYpX=b&=4IZ^Hyk4mO5r!
zpwxV>m$%nL?CK$o#@*7Kvaq~wX&xzW7hO5jmg^A^Z8JQ?%REHwV9_>T^N6-R9)WxO
z0#=?=pxP62dDEZnuPISfL-UBDB#*$UDgsgt+}K^z%gbNl;ZH)W2sDL2xtDtdDm(%?
zRRmPqNwSLW^76mw;V(M_KMXqT<*!*7DDg9=^Gk`(*E}q7QF|{k*o)+N5CloSEn23N
z2C7UDB;&HMosZ`a{tdFHKF2Z>rxLb=aFen97YKnT3(=dmguDQuCx~CS1o#btxBU{_
zQ!MAk5I$tz1}w|M43+`%W{HUXkE-!iVTXwE!NDy;?EN327+xy{T}X5u&Wkv=hH$@f
zKk(w)5oQyLl)X7n?YtPW4>rrZqW@P&7mn7NqfBx9U^v$KIl4_%I`)v`v5**)S#&(>
z=NPCtjw_DNhNH&Ukev6VcHIypvz1`hTXeOSkf!&LM%Xpi2$O7qqC=eiY}+b&$s>x&
z;XA)abdnhq?TUw>CVqmaa{9s)R?d2<oTb22<*b*=c!*R~#??|8$AIO0rFC)~gw@>G
z<hYFQ%%NA~U%IgkFlH5F`I7K)Efb?0hDOo@HcXr7nMid#hMaj;3vUdwnQS61&yW=U
z3vqddr0_Q10Ka|__}>xR))SvG+mF8hJTq*LAIIJ7nQw9$`fdpG#rH_E5=N?K^`119
z#^0i;OhYSE==|?=yE-IPT!n&mH89h1-WR+p@I|~`(D@)lP1km<lxFk$G*!kDD8~if
z&5@Thied<;h7~_uaqLkXLk&mL_ejt0tmQd<qI*}C+A{6gg^7+8$=9M6^N<l=iH@3~
z6osmP@M<}9jm<%hJahCO^kugN@X!wcx&vtbBY*(_;(h{<4#52x{eh<kn`&LhX;i(?
zA7~U$5PkrL0B{FV9{UB^$%ashmPt;7%!Q%5CP9W<DXo8n47XNZ1&~FmYz9p`fxNhd
zF;dHwfP#&nxs>Bt_aQLITK5zIu1_2OhBf76&92tEquEWYqDN+`LbIRbAJT}P<)3!`
zqsq2SU_VIh${6*PEJcIR=dxi9H$w&@Z`mIC48W8lp!Nevc4b!mu6Jdg08qjr&DJ*J
z5<JV*J2LJcx;RM$Se#J+@>f<X4%VpHeEM7$0dqw88N>Y`d(;?OVoy4a?Aet0%Ik?*
zvy@x@kta^ulv$-sOG7D~hM{jyhAX%ivWNxX?uc^|t|$X_I7qT7vmPn3Df0pVam5C$
zWDi}TH)Lx5sa?^709`R3K>jV&T(OWvFIcP#)Ixbe<-D1KwB;=Tr?GhUWtMy0ajQoD
zkvqKmG6l)ijbM)o{|iR2NA)Mb9yN^sd(?UY>`^ZQC>r23VtO?r&N;b?5A`oW7xT~{
z?=eWwm5QaA{MZ#sa<EPSC7Y)LPciES0Ny(Vpp+?=Yh_;rpos%u6M&(vYZb@B@4d9H
z<E-MO(8jq?(hBKKg234bz|dgVnv#RS{l^qkaaWQ;Tr0O9fNKFv--Qs6t(6q%8gfJJ
zFTkB`f$1?YW&aGOLj=Ayz{dvQY2!aZDlS1&H;Q&5KYW64C6(+#dO|2<cL3;5;5Gwn
z(ZK970NFhMUI@B1ooD9@v`wMfCa8Os>Uf6!N^mXV!TZygN@w%zJr;+r<KKL(!on&K
z;<Jw<Et{v{?_(}jzOnY*BWk?%!LeD%Tx8ONZ_!kGFwEzw<U)F{ke<VjM9lYe4y6!s
zI){Em?=xcdfrv!R4=78jM9gJj@Dsz!XIgG2D2SNbD94ET0vIG>)(!{2e$^2`{!g0S
zL`)7HMZvRG^O1<zLL>jEV^oirM*R_%qFd;1d9a3YDw9SqZv6sa3hn^4=7S^=^V%9{
zL)=4t4S<A+pkifC4%ke@jE&I6=|_OYDFBckV_1*S?`i<B3OeZ`RN2bDl7h75UI3@D
zc%o)^uQw+DH{LicYSz`}m7yd~_fQ}EB_q|Fw1rV~d?ehl57gE9I%<B76p5M_)YR^1
ztd&?}0#UONM5*0t2+$p`0m$!I%^fOg?$N1LqULc5(w63U1*o7pizjNnprxg8e5H|p
z)Htd{&F$Keej^y4a*?;hCw@Uf;?sTtj87*BFg~@5a;>5r-a2`^TAiFTYIY1n&H23S
z#VS#a>j(B*{MZ%upjH-6_sln^BgdI@l2B#+LrLh3!<#c#b@1a(9pDM@xsxLFmeIiL
z1@KeAvyxC-T$l_Bg(B<n7*{PtQUz@k>soW=c=ZrDUcD0dlrl&>Iu%kp1D+Pjvl3->
zb!^_Ht+vJ@*KwhH5MCv+)TswAHA2?K0H)-Cx(OtS&95LuVsq|!IyN8HN|1;AAU3a}
zBF5%5V3OFpod9FA0F2E7FgCvqlEh}CXeaVRG(4Y57@Ln%ma#dyJ^;pM0T`PJ1Y&bn
zU@A6$s%;9@HbGrB)iE|t0GGt()0nEn=D&<@umZ0Vn@gFCm2a%A8k@l^9=r`q;=%U;
zWF;fHN^G9q5QdDLquPvrODr?=3mVCnwrZb`O!CHQ8#*4rWk2vQrswVFS@Wz5eF=?y
z2~9gR=C?}y=vhBknm0}B;hUtHN{{S`n7s@x`}sjmJOk03!;m8{8sjPv!MtddPM7X_
zyGC?(h2U8>e&liJ7=KrpQUX8po2J|1L}`oNQP2|36O!jDZ;;2C6e?aMQM@=8z*NH#
zsVlV}G)bF$15`EYWmTFAs2{IIGXO|(l?t%(=4$WrCE6Tl3T((=x}R6(p{}Sx#q2rn
zF%R`-FEt38#UWPFa$OdBE3Z-`^wuHGeDT>lhO-Y|+0c2)DU@x5nBh0_Obc@H_A=w`
zq6J!(MigD=LGqvQXobdeZ@%Rkg;)%3gEeD$QI$1c*>#1j>cXzL0oMII!?Vo44}MqS
z?J5apQ5Tr-rtmeyCA=xT^M$}=IleBG`!N3^F3WL+CtT!OnX(*LIF{F0`%v4m6|+I&
z-*&L5_*X9RFDSva^15qNS<h)OQ0*XO`aQ{g1S>Dc!!=)VNwYRRWFabYjfZEm=3zw|
z9<+?2+cgiJzVtnfu=a{R_fU!=7byQp-E{IIlf1|R4}xcvd>e;%c$Mj7teoY?uBbN?
z_s-)to3Go{Xn>}_8$b$ymjJ8>P_R==HPceU`=VhvWrVsm#K8E7C*%AB8Zq5nZxWha
zmnv^NxZ!UG+RIp89Ct)Z!)|Tt3oz84RrTzdRP;f4iO3$Us##;{S=`g)nD@R^axv^f
zD;$@A>RhP2h)yLxb_G9wR*ql)PT)px&&)!2!L4T8<oS^JQ>Yy0t59ywr8NRBAK?31
zwBG<ci+iGTv$5ad{31-hA#v_w3xNMBczk2HXpYXDFV|y>gzq@<BD6g^d8EH*JPdn&
zLg};lYSR*kW>rGJV0IIXp?sF~sg{C8@`KC%zK57ROUeP0HLw06Rn{PAVxMh~y?dP`
zQKm#AJg_ue@i@L&=Zeb>d}dd@T+61TDET6rPW?N)`rp<F8^I2vf6lq|M;iS_pL-PL
zhc8wIF|Elr_gqRYG)mC?`TEz5v#|@MHQcYAWmnWiDsLjl=G$Oz!(UT)Yxs*uEsRG>
zF<&UVP)j9XTh;kA_yG{J`BK^HrmnS&uh)7R+)3GdyS70y&<<jz<)wvn%l-mMr1^na
z{M?q4Mcui70DK+9+@N-NZlB233b&T(?AcB@sLn%TRvP%l=3KA|mP>mN0Lz=C1^9IQ
zcNnXM^0s>v)hY3(=kNCLn+*8cEpM_&$xMWKy}-R5zXR?}&_I>fZa0E70$1qSz1$=i
zH=;PtvU@8KiG5r;7_VO1$qK?7xP9g1trf|8g5(X{vAl(Ck2YkdzXv=D$?+(#|6IYq
z=B_oTn0K);r<K5FzK%8poPF{q;8V)rngNiRdvrbsZG4=!&S`;u*qnG<AFX+U_!XEk
zIZ1-MEioM)ZaF=Cv}hQxDHe90`_5tfKmchk*{wplRooWxmo8CFF$qi%nmZS@u?p$O
zz>&QeK*Lr5iUDL2$OCXSflLF?qsNg_%>4kPtrXhqe(oE{Rpy(+TI>1dMx9+K%FG|z
z%r}d<7jUnp?o;AFLp-n_uoF70;@?rK-b<Cu7PhmIOMTcp`+_7q6R&|kc(gd3Z|9BH
zOtn--WK0I5yr}3d?0SoJ@kcs0v=o`MpbyG}Lv^k>Q)Y)*90Bp#l{)Lg8EY-A*FR(J
z<=YIsGGqOXCg%O6E5G9reD4V)=ucQ^b!dFCwa_~I{IspMhEiWBDQ>#v4fLe{<w?Ip
zr@ssSI5ZNrcVDWNS3}#k9E~MOrK=fP{!~rTuOrI2l>8E%_!HEaE;IIlhM4(k!Y`Kk
z!TuMtEyoDpA6R;|!F1#$%mQ$!Av|wk)Vd9m1Si!;Hak7D-fUoleZ0ZJ&REVjh@Bxm
z4ncP2a=t_C3{_ZGCf_1<hDp=6r)2UyVrRI*a_7v)Erq^S(6|u!&AbOAoioyRIq?0!
zv-s5nj*J;%Li@I8{%q$8Ee9%(-fz>TiPEKU#{1Tw6Uh_zLtqL3d<IRif;+W{spx9Y
zpc^HjuC3`jB=Cgh^cm$7eV<S{4+3O?EdcO9zy_ogR2QjsXn;IWKCTN2sg>6^%Cmi)
zQ1)!Tq5T4t-wfbu0viF;Y6svl0PP7Z1u%v{F@VJYtP-khwM_M&61dJO@{OUYAUsjJ
zWejy;|C^fZ+%eQ+PZ2Gz8!gAF1)=VKs8BDB^CrQu7qP`?wLfc1vHpjlA&c}Zhi7o@
zF=~3dNb0>)d(@}QdKA71xSIDU+|DSNuXP^RI$6_8rKUdxv%EdEOtrnt@%ff>v9y^x
zI-n8Ye67O1BQ}$*P2qDoB3SWh<Xu{KCEaj|FB?r)9vZy{26YzdS<T@;fV0|H3NCe8
zN^~649Q&wat?yQ&<NQuWNAShU*?t}Ce9u?a(NS|)c>LpB<NLzsm`)uFXz8%(I<E66
z8{ql*^t`6G^ba>p;Ps0aSDjye%TePc)5l`4vIPe1=(nmC_DZchj%+hCmw>w0CvRM5
zs5*IB_uS0dOF?^C(Xz6VU^Q#yHBlAY8N<<DRJ-%KWOOO;@#w$`yOS6n8^0mUUji-W
zi|aeJ{U!9ke|+D=`0QRtAJ7GDw<Ca=0MwbN=QV4%ZV=A5z66w0&O~h>JI_QtD(uu;
zyj*$T|9-<E-$=4k`+G*a)s>FsiK~^G9TPnHaX#{i=0@NU%|plvuk&uvO3nPX;>CW~
zjZl5mcwM*mZ*x(m^J*~a&4RExr=;+X-8d1}{)^|}WtAdHTJ;5j%ClVFPg=S{m1By(
zhC`QNiygoI3B|SDq3yWVmLlRukR<*dpwJ$As-3w~Y4baAXpQkuHHOEw@W#x=eKehu
z>}qfSJYD6DpW}V#jNuJ21<)c}{RVD_#>HqVegorra)O8Fj$+cS(gYSDZEg<FB`EwJ
z;A~9_>(fiMp4+4?9RQ}<mcpOujp8u=rk1MW_~Y+`zFcH1VK=-FYL0<e0LI@Gq!d&a
zsTP09g9+<}mng-oQ!_YPV|h7hrKT+O7k3kKOuN`q+@;KCo93IV`B=6r`3UffAd4f$
z3)(dqeSuAIhn6UXqnsI$f3hGv@K+25X@WNZP~B#?X5C5F!bo0wFAe)Gva;L!Nmg-D
zja8~7{wOphk`J@RGadVC7YRTY6(gmfx=1w_kq64z$wp}9!ARhbQxN&EvE^|pr!60l
zUye22T-}ywk)M*CrHtvPOW75GV*k9l_TtDo@UyVbHtg33JB|6RI{WO%zGP>Qe}(Kc
z=3BBaU87l9y=ca~-#1~k@>6Z~4}E$zgnM9H(BF)k^hYlq%c=VG8azrf2YA*IHfz-F
zOzCJ(AxY|XmZ0@hbVHr5wVNr0Gq1)Mw2kdE4NL)mwQvhk)Y-m&X}JVE3wN%q(Puei
zco4A30Lb_O&RixVrQmhVZYsOl%;j$Iz_&a!*!)sePJho?T%*D$KVp;%K;`uZLV0T~
zau(%j;L%p^)SPLw`tlk-8^xD_MXVNpitnM~r_ZhU4N+XQMwg8X{`|W&qSw)Ow{tC{
zZ>!;d$UF8KRCV$iRrb$xv$ZS6qndZG@>F%uAXK#+cR3~j{9592+(qH>sles9i^9u^
zzu5@I{YIKb_+a4S$jkY*28Ysqz%z$8_v1Vs;}HE$$UGiX3TLq){U9x8DFkFXtI)!s
zK<8S{k3zJDv6b;u^G|}WRya<}{4Druz}4aAUu&q-GCNnQ(=#VDz6;NOC!^(;VvSj!
z^MRfbTzrllj`qrVN6zKSMgI1h9&ssl!ey3p-Uzl%az$OKxRRmDnHX`0;V34@yj!kN
z)RRc&J|Pci^3@4$ot-|mWMWmKULUPC&Y<pmT%9?KqHy@{;%~K9zs4Y(a}GI+&Y<!#
z(z1ffD^^|`bWX)8T7IfaR1-~^KTeH^he~xn-HB33KNW!e^edzke5pmukXkKTCXAGB
z!F{f~)+&dT&|Qln-l1X+y#2|NceQSnJc;xRNEmj4aNZi`%PDLt?@kXx-AgoslpPE>
zuatMyWlBF!19nsl>hLH0miLf{m|lgLWX<cKo%)avMr)U*>-<lvF1!eQuIj?!c^?n0
z`dHbj+U8xay%e>|(-{6ARb5fIcC73Lw$%tj!!VtjFXb%p!7A$Sk-hjPjlkytADp>)
z(Fdk^Nu&0>4vjaZv2$>`7tq0Y&3n>Ss-C{_$Dc#6bNA(*_)~}`q2kXq5u8B2OkCp6
zwSwDez$N}Dyc=<eH`fV$4snS$*9(3Za20Si3C{Q<>*>uA%op(|^P463zd2%$pixsQ
z(MWdyW=Og*f-_WKx(-PS9h<=*geiT6P)`cVm~@li4Ms_fLQJ|@@NvLZOsb4fF{yNg
zib-2Fe#MGKKQR>DE&Oiz$>rl^<KW&1Zhx(Z{0!MT*de$*VbQG^y`AqOc-x>`UbaGJ
zByH9!-<m<nc_m`trRZmz1iT9sG6`@-qr(aNx7FD{h*)6Qhl5Mlmyo@fw+C&wT4_Jd
z9{fYZ`()+KLC=$wHwPUjz?*|&$KdEdbh%=;k4SHo2zRENgRDWuT)Cs@Zk-ECEkDjr
z5d+rfoVr4<<djc<IbF$|4r-CV%W1E<HOG>iHkT;_8=0KyM&567y4U1%(B$+dbK0#%
z{w}9zf0^<g)$N^;?M<jhWOL^o#6QHX(iK+TD<1lHhOW<HUIBW8b;`<)M{wHbG9CE#
zEXSk9b{<OJTN>eJ>qNySyLxKm$p|XnyBxt(2d<#<gTT;BgUX?a!78YHA5`X*>lQ&R
zw^q@@r(OS4OS-NPYWe8zwNx>oXt@?1hVtgE^&mm$k6evlpP=Xioh~uY-Jm?g=N}`4
z3C(OUhg-R>;q-gC-0)u_SbB~Om+Bg&dkb|Ad|P0TPL~PS7aHLg;W_mBQqQTqUZmD;
z6<we^K^v&IBDrT2$*)h0<(DH8G!0&oAG_i+7_jyV9SQl$Pr_J4E_tPAZ6sgm!Sy)_
zIVvozll9X~Fv!Ary`Xm1+>d6WcU~vhM<6cXN-;<0biU5X*&Hcjd5dxAMx2EUz(IXE
zQVOcGR~yvH!=JJIz(u82eYx($cEw#NfV<8U&OZSE%m$nd-yvq*mkIoFJmUhs8>%9+
zfOEs&xigY0i+6!%<>aVh-Yp_)$0MHNH8+KIouGs7Jwkj^PzK+71#dl(4-p{W@hVL4
zx-d@e6+#{e<MZ&PF!`?SbHuP+rm)|L;U-Cqww;7^DR0KyyHPc+J<#X88Trs!grZqU
zF91XKP5?I&xC6lZ1a1Iuel~!s0W2Y~3cvvZ3jqwCjG|Fh?rvar0?T~?z$*Zz^LEdX
z*Qneu8<ih-ot?-W`+%0110|gSP}>sDKn=G~bNiBIib}ekqwcv@>5H0zI=S8+LS1~L
zu>`u*OKL|oM>08Xu;nGS-Js^*ew`A%^a$JhlrH7FR{Y{?UcQHa&#!`9Siap$@*)>D
zq-oQ?fYsY{(XfD#ALrkZ^-w?AyyNrnDJWWPtb-2|NCr?R7eGe<od_fXC?*gOU>AW<
z08So&KOx=(fR(>d*Hphqi4*QZ39^`-AdZ~St(4zk&OojbN93(wkc)z~q(qYz8sQA;
z24Nk2?giE(rneUj@amcF(etj-<M^t-w{G5bvht2;_t3(hPP6bgO{InCc>HWk()Fr-
zP6dlQv1D(EPWM^4r{kr<FRJ4mllFfnap`E^NJl#_AFZ(j70+qTabY=vB$?NIFX*cU
zMA@}|5InaKU27*Krj?mDNTX_+R}#j{>%uWLIbrz>j9jwC3O1pLSd!(`t;vPfHlXtC
zyK;*vgK5Cd`8B^mA!J?9XDTXHULSq=2E}FNqRvOvj9(89zCOAD97SK;sEBN)>fOaG
zosvfOK{cn5oiXknum=DJyM0K>Dg|4$0QV!a#>l)$sb)*q3@sA*)6O=R&a*Zvb`JE9
z3%h3t!xc&k&Cn61x>rjh?0B(l&T}od4D5MW#?_q%ZIx(SanV-ZP>-hJMw8{yq+yTf
zSu{PVHSq<T<w}#9HUyP#=ITpb^2ik{Z#crO47$dKp$vKBJ;<$Em`u}jy7Z^3H9{|b
zEle0cbI;;MImeV$(+ti(X<1r#{sz@;c?1RPlQUWuqg#qt*T91$YhhZ~RHxIrd3S1L
zu`rC5gFjW?5SCdFGPCCG)~xINtX9!UPab)Zy5TYR2Lc)9jnL`)YhgO1(p`NqDc{ml
z7l(3|lEqgUP4nfa;Q!QG$VS_&yoH`T>Y@+II+^C>t@cpbDvH$D6*`^8&97Ub+9(~$
z7<dk6{vY<<JGzRh3mc!An{sbR!VpM83DUbjK#(G61VRx+x-@Bt1PRhYuTjJTqF_Y@
zMMME>EZ7Tn#R4iS))(t*2Rkat_dNUTGj|dYeZSvdzqP)sb#KnDXYYOXsZ*}$Of|I+
zl%bb+XQtl|#nOkGou!>g3hYb;z>L@nwhEo;8sI@)r^8hxAJ=(}c^U-WM^*?!ney|O
z?vvK2qjH{M0#R8FV^G@}`cZ;v-C$4$8E(A`5<x9$rMe_v2Zwt7Dq$Vsaxr<N+wPpA
zGsB`QLZWY~=uyw+{s^Xyn?rgq!1MRta<#4QG);y9K5`j;k@Bk00KXHlHBWfImBQuW
zEt3I%ekPjg?G&!078j#U`~ow;znhYah8Q_T2l)2{Y<V^=E|@7nBrXChPk;~7+Djqr
zeU$PDs+NX-5)API8~*sISTb66yOpbu1Y5wP!*hWuE;EXwm#gBM(^Rn(sQ64^Mv$$7
zGU8R)epR-Y$_}LrwaUt@GAU45qv=e~!<7+_6$)3$k@Y6h=|J$$`y=HZt8gvY#BC{1
zVfz_yyP`-1OUa{_R#)YIp(s9N6>kBHD3$^hC(UG|fvtiv;#FBcRhCC(hf?}op)ouS
zETT*bRJNST$W}oa@#^+&Rh9(=|GY|-wpxX|tU@VJVXYFlT~VZha`LF9->dTJw6s#C
zU##Lnu!yBnpyF$R89}xR%7|BGKdZ7bDod->{YrIvmsKVOD*KVj$W}oa@#yeVG;O5A
zuTg*#)JmwtP<!+R-^VF-erl-(al3P;i1NW&j@9NyN$fAjhge=aB1_R^Wf5zVuQy4^
zZQ#KW{~#I6eWeO(dj)8CM+^(BH`@GfhpP1<*`oVGK;b(fMfCotk3W2Fy~i+heM8*j
zslvU3Vd_@c^42w>VJd4Dh9%j>-dTym)C+(|>_zKv-B~7w_5)_?uynV8cLS6@nRkOx
z3m4_jd#_T@tF_sG2b1KG6xi%_<{&vMid0Ae@?buytcst-mb(xdgV|8-K5C>?E~hW3
zK$QUFMdZrDt7ta_EKb2WiswbqM*}>=if8NBD^c3-DFxVmiEFj}TKrA>$;#*xdo8En
zb(Q67&>M)XeFwxGtZLp<Y6qp1z~QegE(06KK*A}AY%md~t1|<B=90Nn9XVW8S$IF<
zih#u_s;hW<Q`9vC6iiTnuI&{Gq~=jw+Z(txMP(VlKZDvdfXl<R+#0H-ClZcEAH9~{
zww9u8VMXtT6rKA|ikzaaLb64Fg@D2aH=1D5?|-&thqVS>i9cYPtJ2T7CfV`+PZ)Fe
zx=>dditIJ_f;E&4+4F?u@VcR#rF5E$4cY*#0B(YmIU{b8X3mH=n0LTW)=IE$l0UCU
zTG6!_D0z}Qji<QNc&=(rNEa$^UBl{lJ=2~6uble6jhf^vHZl(d$}~lx@H!Q7_nJOs
zUQAN%eetS1y3xd{sa4)St=)C1yf1aqn9Hs5SExK!MXYjJzIx@9W1|v_oYX5pl%mt{
zxX>xupjJu0drkqK7cN1K(oloY?=ZyL<#>fV+k4rCTEW{Y%<*}%2-_i_hmCqCB@Pyg
z+N-Tn<tPPM<@dJAfpLB;{>%S#KqTe(pvuWA%vq)ynw3M9nPsF(R<<f!**ywK5(+=M
z1z(tOq$RA3D}8NUMbCy56@C>~#J)B7Xi7R;B~H;#D%c81;0)vM);c$t#^w*e@y}hi
z3zogQIh5Kp<|CE6&BN3_C!H(R2@7nh&lUK?g*Mgart{da=VHKbP6hv5IaPRj2~xil
zslOdsIkS|CSld#aS<Vr#^*LDZj-Lprb44oaT);b~2k_N|2NVTx{bhjPoC0{69A)GK
zo_i2usJA@b^sCavb9v{ycYZqEiLBYbXwn_(?JV&Xw=rFh&M5EK`b~v4ERjnpmT?!?
z><82LuRpva3f~K=(4pZ@<zncQPPhivNI!Xu1$M%ZQ7Ejj$wd2~`pLZDT~{NR`*PWC
z!xH?Da<l0a)>A$F%>^?-VkO2)LC<Y9739*MAJXr$_9ULC_OxMv_6$S83??6|Oe+nF
z_fvY68`O{aR_<miw*!U3#9RN-4;aLw&|PZjbQ-iK{WvP-LFczpJvq+0+2}GO1Gv_v
zkFZ>=m(xgcZLP?4P5M&HRV-X||B!Muu&!)O-)CLf3@&MsO6O|`Mxszy|CWDrMaF>n
zp`i<>6()53RJg9KZ*%&H4H~+eSPh5DowmN8Sl^U?S>F<^Pixvzv8KDzpSCq+U!XO0
zXMv4735CKJ{_kq~SjE~{pB_%{aHIOP($@5Vt?7N%RCV*ei(%b-JN%DNTC-~r4y@I?
zGgU#*ZZD*7wsjt|brxKx-ra*j;f4RQ&Wi0O-DI;0Z(vrwlAg9v{riJeu+5rWq;>T}
zp>XHFtm~Sf&7_v^RBRV(c{{zt)-sLCS<7{{mhG%1Ys<fjV!Rf)T6&C%1?}~p^#9tL
zKCv}bUZMW9L!t1re_7L(Am=Nz*Em%$%;v~5=~vyPF+81B(Ys}~&MmC-{(o8LvY^fp
zvB;?wzz7(N%!)(N7~Tu6$%K13Vv(XaR6Q0cP)O731+BY@HTS>i&L%bYC2Q`l*4&(x
zFn8a-G&fIET5GDVW9b%K)Ai}^*qTm)UI|Pw3+$$=S<@yJarfG>T-H4sIHluwH~1l+
z1M&A6|9=k*rjoj`$e4K0n#Ll-<4|raGAIaP!EhbJ+rh$GH;a}3<173hQVnB7`mmdL
zao5UWFYo7j{fEnPN?zVyV25^2d2@^r*v#YbUWY5<FA3pih48n8@Vi3z$3ys9+F7aR
ztq{Jg;(2)do#Gdv-qj)gq+4v$IR$Tr@W+JkwwqG_@ge-_nrOuL58)pV;U|XhzlQKj
zLimWL5%sJ$_*mpZo1F!fL*z#-`GtVoN+)zmzh}uCZ3+|&36X!d<PDZwbf%JJNxeb=
zu5!C!;e~8I`oYDL;IhKHP&MO46z~0vTk%YYTx!XI6^qhtGvg;KtZ*mazeq5rsDaAM
z;yA52lnuKQ6H>0CmsB!+8t;Ea-F>%(R>=3FADahB7RGpA)E%n;%Y!6~1a5x`V0lj3
zz)LR$EKj&Dj!{waYQXYD?BW>j%e)lu@MULV)>s_F+6N0KJT+H@_x6ByrmVzE1)sgf
zE};dicNvz%@*wTSF<#DFOjvHWx+2DFe+icZHn(F5d<kK>-}5Sgt6u?FZZBOc@J7ON
zd)GArH@p(C+}?Gqz<UVGyDiqm_{)C1uEG+0Cqm|}kMWc{vgW)cbLD=n8^pfI8r)1J
zd*Tf-ZWJPGE?zQIHVPZX-Z@u8UT&YdIcApdtIcYB@a?8$^AH2?+1OKCb?k1wR!7w5
zSm20y(lr=S&$z>|yXdl99Uoz<D@(d<cEHo!s7$AK-fe_inAUc@F)3|t)%qTUb{WAx
zWr6j@u0?(CsEDoa?{>eBhU!}i^+h*pYVmpHf*lHQlsz3a>bkz*?GS!i2>)pae?bWU
zeF%R;2!Akye;|%8ifM3JZ%JJRzEB1U9WDg%9wpp(xt8}L)wAI}8xP^$W(<`JVR|nU
z)(U$TV{HeC=$ZqIuSh6y>N`afR7%E(3l-qDdy=TYCiZv;{QXY<xtKlN-KN5PgW>(<
z>wi$~6nk-36zt_J!k1x;NLX{B+){<-uDGMZomF4ZoBgVd_tYN@YZEE>OesuH+}K0M
z7NZ0AVM+m7%BZEIUJQEVTwKSxJrt88Xu%Dd)Ix1W3GViS657Nbg8zQPn*G#zx@x_6
zw&R=|)|#;92%b6x=PN%Cd9Mxloq}{e9fv=jSUnivxy-;JOi?YRO3UOaz|q^igN~qO
zCUPx;qmU!~lwu^Mzdd&@IyJ0lW=PQ~R#E$-DY`Id;m1{$=b95lR^E70@E^tZL#i$^
zc#Z!rA<9*jf+*vqyk*9B%9L3O;_ejG4^dvwS}?O0bqe82#wzfoGN9iUEK(eMjC-eP
zY}}ewuquSF5yD>|!nX<G?+W4jgz%4t@Z;n7l8+TXTXC?ZC`ZR08A94CKwsAxCXG*z
z5aoVLk@yS_QFd91#AjTH@`YB(mCv*g-n&cOmtKOvgeWx)h36GTD-};8i#}99(k<a0
z(>lC?Z<a_5NyII-yer-w*&X2sZfuZpnJO>JIxE23eatBHqDQ@Ea$Asc6I7O=d)qoC
zU3*4|^1Y=<=t@JBN@W@d3Eip?rGcSf0Kvuf5WcIGxRA-XHAESz5)9oFikHy6s(^U=
zqXP8yHjzM2?QpaXwkPm5<KB3uc-Gn>-qs6I{$nZPZElG2o27`iT|<;Acbon|Z~KMt
zEeu{0WkiV5T_x!4G{uXzOB4`qZ&HBXULz9lw&T&f{W|dWsZeeAT06ws*Fuz6EJeKC
z8>0NzQpDSDLzF{?B5ivxgs*W=xLd^Ti$}YSp-8u=qj>SQl>*}JU<K&y1tI}&k9x;~
z6Lg6=VMW&|z=?L3Rdm!JO-iwf3La8f&Kd{QY;(g4kGC<)ujVZ86uqr{bfaLu0_;b1
z?$szrtPh1Koh(ISoxUv|pTU+Qv91-O6dQ`fx_Jn{+)7A4Iw3?^uM)_tf}tV&16D%p
zDhyFxvlOwbBt-ezQpB#(5G8V7JmW77;cFYbX8gJkrM*fp<L^<t<iI-$NDlm^0CQla
zNFWDJI9kTHwM|#l;C>TPQPf)jD%x!o9d$}i3lt3sE1DZpR2y3nSsQdcnnkUxqQdnm
z%l9RP*5qA88gaN&>8+4A=6>v9uVE)qrsxjXF7OY}KceC%9}I22x?u@!HeXXyxcO>(
z9bnmfO%wP5!m{}?@JZ_c%jRo(3Kcz1ST<kNQ@Ht>x!!KRrpui;*nEvxjg1GdT$BiA
z=JnY3xW(9u82E9(a&P7=AxCep+oHuO+!mckc=-&lFBSTmgkzHcmkNB!2EePhwK+G1
z+qTI!VjHxN{L2L9V^nkBodS463b%3Y=&5{e>lF>nE=tZzS#>`)DSU40sf~zLLB|J7
z2E4HuFC_)Z>?YV^T+(A%E4OTMiOjnRb%`uLsDY!lgf-3;2~NRM)zNM*-s<;&=s0#x
za5rP@kS#YTwSR6(-ri{IhIQ=ykhu@Wtr%)Fm|NRqIbGh0C1OS5noaUd9GBX>d#*$^
za6CC%HE?d(7}C5}@a45}vtZmPdQMrTGkmQ87aSjn9C}sB(Q^12o5KZlABuZZ7nu`}
zsa%mS7t;#f@CL$RDo$5AsyJLJTA+Bb?*;{E-#Nkr`(_=@zGtj`g-?a-i`#aT_Qsk%
z2$St~xi#>-kSZFQh%Sdecim?&W#D6>UU$z;=r!_cxE(HgU58D8rPsYA@YRH+*BLl=
zGhpd;FS}H<kg)W+mtFR{Zviju!%mU!HG15{d4vO)-EJqshgWFbf__pu3oHiy6R>o<
zT|%C=8Du$)coSPITy`NW-R?u7|F8w{1#=<(AAwhIMK{BP1-OgOWw&GU&-`{G;Ex5K
ze=GRQ<^ldx;AywvGG-nudjAm&67tfCpIr9G%WnsxyrAM|fxSDxxEhT2JPh+^O`C!@
zihHbK1_*L7>y1Z^gHeetYG~y31Fu{d+YBbd6@9D%R}*ih<@$?Ug_D$n?=)e9b9TnO
zyq|adqpK&bIZyhM{drVof<02j+=FK~A2v0xgYST9>fkdrPlfOWiykpdtfVGf|KBSr
z_%@{CjPfcD{5x9<@-<`wF#KN~Qh_^;WhB#+1KDm5moWw-h}?g>M}tHs?@~!zz$-XK
zvy9#G`8d9CYzR+ZB~q<4B7C8r^JqK>+(Dyra#2#WTq}}(e5(SSbbE`yki*_cZyz#{
zOZ{)yAA~D7q@tWECx`3e7JMM@Dww4vN=<7OU`_X%niAK1u|&2|f4gb#iytwR=d4N|
zQ1Kl>P54>6u>Soa{Z-L(gE{1Hm49VjDXjm5$sM+u&6{|OF?8l&C1|k=F&oh^_IG(I
zYuF@=M*~!iu81%m&4wX6wui=}p?6|DkvI1a^7tm-)^`DxH}?(}xJDUZd9|g1cMz8G
zXoyEeJMIQ7<Ixb0<I#e9a4Xy{jz4^u^mLB~HNO`fUB<i#@|)?;5|%NqK;T~Y0hTeZ
zP~e9M%ZN0^<A~I48{Q#zxB9|#Jn?Z8mPN>(ogsWtWP7}07Bo;`A>6uJCCm|{yyR6f
zaFFzU`(wv)z=i2gX3(xG&OE={h%4o^5aq?VDFvfL_>UBi9#b@4@sf7uE5NjC{)9=3
z=+dK2RP$`k6|D~|dMc!7rcreCo-)$vDL9~_&tZ1DQ$+1=+{vUTiwrDr<Z*SJx|MOM
zgJI!8E-X@?G$CeKUNEv67Wo-;G&d=GRV!MkpnXW<fZ{oj@ORMp#}@Usn-=A9(3Og3
zyY*H4VeQ6FRKD4e`vniEC>=YmoM^}~5iRdk(X%SZCMx_&0X7kO?a{BojImMGj#uhV
z#9X*N+uZVo*-QT1byc6l13tYv9ZvG9R6fT&1B63|&K`(=5bW`O7NqJyq6I$ro9p61
zyW@ZvLM7)+L%5zYp^Lw^raveRp2A#6sYB>6B_}FRF%TR>=HQsRr{b0>4$V_CRB@Lp
zju~61ICj#avlL)#ie@N~nt~SW|FkiKzuNe^ikCoTq2fP}S6whgaX;;KoWDZ6a}}Qf
z<7p<UU$j(lgkhr7;|Ip04LC;Q6U5^b`31jm9AbJ$VW~H61ZMZX{jl<xKqLajkfdTQ
zvRXCX<Ov=l;ZjiEc?JQa+V}6rb&1X>40`~Dd=wHN#A2lGS<nog2?K#C1g7XO)g!S;
zdd3rN5{rfkFq=OFyV_pRJB0rwgr5|`r#~I%U!i#32z#vJnY_FO+bOs|BsI%W|F7Pn
zy)&fe;!y1eLikM~{H44ug+F@tXb8Va@yvkTibtXqlqvr9%J^M0#dFlPH<IJl;b)aa
za_mh7r2qY{z`y8!UUZ61$WF<2ZJW_(G0mis-HMxOaApiE`9!hjE9VmAY~|;Shu4O1
zH56A?PP(3ApHWU^Y+*CSF}qne+REvXk&W;Fg6kkY@vN1n3O5mFe!+AI)9?@MTk}-O
zRiN<CUDxng?6&Y3LoFK~LT>IyVe!N0-qV!%6p&dXyZ2nhjZqxC_xXyOsW^_j8x+Ui
zH(jlGJHk38_bGK#Tyw#rilcvz1`<xm^NQaU*S=_{;<N`iJz6pj`@rEn7m7o^ePgRL
zgf9L%AG3x^o=@-b=dNo6iJ$QqL-F_FcfmpB;cvF~1Vy7=<UViOPkLrd0rt$2Oa*2e
zZa!7zl+;t?3X5FrIp?~$irr|iiTrJHyinyNML%GfsiChTU$#hSC>f&2y$0#wH+l<4
zE6(NyTFSV55lYjQ7@sJrKaVjCA8~mAuJ@MASDyC3s=MRPRh+&A6mR|>iqUIB4K%1y
z+L?GDh?5~Vct>c_cNespP3XlQH=(^A1uP4)RRZ5aSQcak9`YDqS@d1vQIYpJU|IBC
zB8$E<z~-s`OFcdhfuo=C12A@;2wqtSUMB0n@lRk~C)e4pk$v40gk_z#P1bo+w)2Kp
z$Q^nCx0m8p89YGmUGH;?xqV5=<u4eAx`K!(perxIa%8<?J0O6khbcw3DuoGAuu}mJ
zDVGZyjtBnk{(Yz917+VD)>yFkMPmUUS$ok?WD$v#Ic+IAU+Ju|<W>c~39DH2sA2;L
zoT6RYUfnQ*75t_Et7!3}?b#SNBWoPgkg0YF?}-X<u`@NyYYvIbC1Kszafd7S0~O_m
zR22QJfW)Y#+Qt|?E&Bdp>vg4F`V@3fQJQk#-<X2MT2a0VvGT&np~^W;{B7gNrf;LI
z*(rQR1#Ry_*60rC19XO6O1&}Y40Dl_{ay)mhK<NNvjcicc07Zh1S~tCrv>i%6kyo_
z8TeVk(ixudsOZ_J0ZV6iMmodnXKZJ9)^>&wWoKzectJWsqi2!$vLSjyHay!2OaI#~
zaJT0GOaFUQ;QI(mNBKZHO4sKx5T3BZ_|$GMo_~4CqzVT7q7fl{(W($oaEAh1SZ`4Y
zJM42aZuXT-mGUcVTyN*f|9KVX`u-^+u9SB|ly~E%6nq}S|Dbp-FTYkiTYf>Emy91w
zyP`%4NJ7p~Kt{ut?byfc2(9MD1X!%w6>L+uU6JEC*{>{DCA4~;QKD8~9ipsO^$1|W
zrVxIM;yG(Rt#}4}yW+WD@m@9#Fsy$l{;;qfdB4(9Md@Kz<dPZPD|o2;71@ndbg!tZ
zYGA-@;*V*7JM<M_KaqG&rPHDR={G`&|2neB{QR;bcU$#(0r2}1FzOn(-HU+bDK|I9
zQN7*{z>@f0jEd4<0xXH|$=z2|0gv|)ggZgx@nfRgz4QvuG6^IJSmR~nvP=Rga`Vyz
zz;oqCotwn$SU+<p_BdW=xmoHj@;8H4?*xG#dIkIwW&-Xausg=|j5Fj`D(_V+(jQRY
zWt7~hz&!(S|KqDR#7@ax#jl9t3jR<WCzFj9k8Nd@S4<1@O}x)pJQ~X;BG-?tl;>kZ
zmnnf9%%~np<By+L+i7hG%kw}-I-X7?164x$!5jtHLk5UA#>aYpb7bI@oTuy)O--ut
z%CN?VLmD^!qsF2)L$XDQubN=cw4&M}prD%qpRU0zpJ6vO7(9b3xLBFm?ZuBh?~+*5
zU4pl&#||wy%3Ia%3rUUq*HZ6?q*niHspEDU2l*i679%BluilF1NU~k=X!(MnA^aZ2
zbG(=k!tYo7QpBuC@e;u0A)w#}1$KAD`}$wAdEyk^9%gz?0cJz7Fd-Y7{7p8<Y}qcj
zqW_7?@=?SKg9?y{Y?+^wNdGn{k^Z^6OgxBxQ1SFHJ%rz-c=}gW@#0@61*9)dRDjEb
z`mbC6*bkWd<&L5;j4$D-6ZbU*b3!$BRykU>EQBAVcv`kPgrBT<TK2HwX-v@$1*D7Z
zQ-Ig|w~GXV+Vp5aooRzwl=xb_LliVufUOh5I7sKB4UeX%I8f9stY}0?(IBg+(a{tY
z28s&8iY^Q(y3Q(Ud^ANxR?(tsRW`m{E!?Q6pO6h^y0X*GRoLd0DYcR7$e+9Jzo_!`
zH$&4-M-0((_ycY~k8kZbZwOQbh#Qgfc9J#)zo`5R!S=fd&bHv^0GGKk1N};s=f4(l
z&U0l3`juzT*mjI<jVrmYsS#NJ$)ESv%-z^ebp`2^*CWV}zm$Q$1-uTV__S-UN^m?z
zCM|axzHS<KAZE`u3@i6q>@spy0bl0sK43?%x5+xatx8F5ovr{cP1b_hl<j}C_&fGG
zK*=~2Z7pJQtWY#Nq<g_u1!VEGEkw~Z@at%ky^4;v$wH{x|5m6?wjwMu*e1SQgzWJK
zK{Sd1e-@CfQbowO>?WDbV&K!?v<rg_mui0nJY1S3L#|PxT_&?#+&4@tgLbLmRsy>;
z%p-5HVOl8%f4sFlNJnXyA<D{znPOP~MZ-)`DQTDs6kx;LCo2A-VQK{pvsy)82#myC
z$syef-cdjr=Ccq*8>Vt~oZ{{_@rXA}wYPE4%AQce)O;JEl^<Gc;qvm#Ho|hb#=sNz
z0G7?lu|m&&2e52b4EzFN=`k%`DxUPNZJL(lnx>7K5;RRa_ZHY9O>^vfY??93!C%hZ
z1=3NP<~(I((<Gx(u>OmtxlE;`X&z93O>>&4_=l!BK4_ZlDq3J_=G0yEW=Qvf*qbKo
z(lnV0uvTrFe${alr|8duaqKRr`|#aR)2u{Tq|fwpIgpKgU;9i?=_AK~K#+Z=m%wQs
z+CF38I{=%8>FrYOssFJJ)7v)8@b4yIOzR`!e5I=_%`NDg^OjRwbKX*Y!~kRm7u~1I
zHNZEaP8Qw$1+0SuE9rvE)vua%XdL2R@{YFiBrutG<!o23U0imLao-~c-#AwFBc6`i
z0$)(k2bd9lF`n`zTE4~*zZrPM{l!pp7yPM!#J%-frV*sIhbX|-?uuai;|Mt{Xzej7
zI#9&qazVjz#c_C_9pE_=n+tz3u-|6|k)+!UlP0OW=~wRB=XI|^ku}xv5Bp1bb1*hw
zygFF)n2OW>qR+xW#@lhvY)Y*~O64j(o>Ja>xTx_#D5ZWt)Fh=!q|u-ISX0V?>p!t6
zHA~2|KD8-j;A)@Ql$z~Q?R|jFFg&|_O3f*sQWn&dnj5549@>>DwLo&H%jcR>3k5tM
zprq7n^{b{$sdL<q;Ft^@JHcd9s!X}IL#MaWZS}sU)PnzNN=bpKQwGckXUbtImHvgK
z)I*{0F8EdfNvS$};(;&dp#W2AH7fhZlv);~RDTuSBw{9|W-E>p(+dH<LP~8?CXRbi
zG(ddZ`&TJ-hl)!|y%`3)cjBJel=>7YRafy}1f$eYRC~dHLMb%@Q8N?B2ALpQehFA6
z5CgwLSSFAgg+BBvz%qdt_}H%j%LKB~rQ(+Xo21%UKB;ahpHvpqq}miDRW4eVNwrzB
zXvjC3R9ghR9-t)End(_pn^bqY$A73v^<OZVq*|?9Wzgx}?=B+;$Dzx<)ufUFlj;Rv
zM(jU4sTzMLNp(#qzzbecK$7ZEh*Hq-UDFOss?Si_KPFXKkW{Ty^fwWcq$(P!I2i-a
zSAg^Du?RHhSG_$>ZZ+ij0M?`TDjRdG*szr!wjyi%ql{Xi8jC&)0h(3kyZNao%J1@>
z?=DV8dFwp*e1W?V<6Jan&_!z#-LfthxR(-qXYso~@A~2K&KI~h5zM=I+q~L$W%fur
z9gTLO`v7noE^?p7zgT3zFAb$7SGb+C@T;C^R(CI6tG$}YmF`#d@QzS?asjm>{xTFY
zyYY)CQ`!VOPIfMK-$LTAbZ-Lwo+R{b{O870c$^h(8#K)~IgT@qI8ndK-7(PcEwQ-F
zyD(vAUIbD6czjU??!p_;$#JT$YX-zMWMaX&#69~Y*g5VAVpw~oly(r5(k_&or0BEw
zmqufg_*_1|k!G-P_HlUKPg_&-cVICVCPimq^LeTJ2fA!0*l%ofs#d~dhR&xw5k_}A
z#1s5Qcy|LVE1L#L|K3}VG>;~%2XCY!1$LhRiWf~TfWw{}?OGfEyyOja@Gl{Gk|2{#
z5@h1OZ15)}o;4Le;uK9h4l#`EItG`ym(keBGxbqCk40$f1i{2;QgkgrH>oESL^joc
zW4X5ile}}Z<LpWPxj6z7#ksYU{0lM`<z_z0f3W4E(PL0s@<EpU=xwkx`A?RsN0Z=D
z@?R|1jV^@V<U=esjg6)r$0d*(dmF|iy8?8KRYf()o&b5VH55+}pm(fC3_zj)17i0g
zRg;qh7#_Pd0YKE90MT)=SCCi9DefcylVVj;0HnG!czUc0>+uDc6`KvGlQUc_7M*#q
z^GL~Z`v547J)a36+htOp8|#8*ORnxtg|=m}YLv-wmjJjhHlIKvcL~^5$68aSmCO8F
z8#|srYXLUIx)R70U`wnAfi?n^#rhIxE5qP}u@@@?XeYL9kG)EBj(5*PJ<m8PXVQdg
zld}=->d71EWOOk)ZSsvQI?-_q>qfd8jW%IaZlSk6(y|S3Nl~_8B{nV1PQ3^PV%eI4
zy~1Lv0(&ihH1ei@3h$!l&qgy<-jaq)ieAP`=zb15v*T|?a=1>UJ}io^Y6eV|40L+$
zQnGx4PljA>ud=5BkX>#C;!lfI%DufnX-P^BY7k`n%c+__V6YQwn+sAf0;N%9wUnk9
zTRI8Bjz!geMtEXf1!o7O%ueVLv2F*^_#3kBOGG-irzddm27G%7375gUY@QWH@FcEN
zeV*FxWYhY{HpO*eoz;h&*3a>}ibvV>BZ7s#h_n>)e8+1Q=W8u|HI|^exqpGL=8v$!
zUCkD$`6>E^DLS<p_C~p}{~q+=?_P`x{BzeeMg!&~EFzH&GXt|#&invezc1p{KGkuX
zknJr3{S}z?J%GLn6#f9I5=u2~1IE*S1np!6vVH>4uLnUYIh9~HrS<wb;vCbj2SL)j
zHPv$}QA;ywSps71?f|*~X#R_6AxO@Kzeb!o)6YgHKg)4j41g!c9Edn|Co53>Hvq*7
zT=09usmG{isPoQLRMD~zTB$y6G>%U)4K@I3d@$lPJU7Ih3vMs3!Np3$YcbhcPpU2q
zaPsH2xe@vr;JC=S4H_EYLWk4hPZXXY28G7d`I<^$^pHQVt>~@IhBJEqqa1dr>$d;R
z>diQ$dPlNAy&F*wy`LOe@3)}iq6+Qx-Zu?Dj=rC8Dw<zxU4SV;Z2gc0>g?tw7@a99
zQNh+{Kv!F<DF+fx{@lLvptlKQ(1iXsp+4hQyAMqD7+nqPfL+#v4W63tG7A(rfP%Qy
zQKc%F&?-ViW4cvoZw0-1;?}v~5V!7RfjZwsL3CDDi3&RX1cOM59*gu!<E)ny?Z6^O
z8#C?J`g0+IH+t7crp|aDnv4jTJ~Wzrs1UkL9};kNVuJLcGnlw95<njkpePvt`j7yp
zM<FGBXbkmri-CqtBS0(#fb^j;RFX#!oknP#D<x<j5?auQ#!|}+5No5;jGYajcB*J0
zNG;iE2~M5W$OrbJ@$5rY(i5Z)2~epr0Q4aN7WfIK552C=*Ttg2Em##jVjrrJkzo3e
z<sJZT?L(w#Qj~sd0vYt7qR*gD8gK%*qyax>fekpJsy5(im8#HyYe84N*DFWRhl;z#
zRByd%s`o4wsP}FZBr!G~S?_MpwGTa`9HsPq#u}@$MyBdq%mQ_Ogo5aN`N%p8vI1M*
zQVuka{JG~m3cW|{LqC2}A=l5Z1}O>GN!e;bF$*-|Y81qTUsb9?xLyHW`_N0DSI|30
zbk?h`I!|SRI_IMxI(Mo>1)X<+&OiK$HVjK!2$7^Hzni{`j<ff$`&AzWS7Xj?hmlq{
z!-RsnRhwg@j(Oi{!97FL7A`qjaLbUimBnK@l?U_nvF!=jN&476gzWU3#_Vo+22dMX
zorQ0_QRkP2Vyp8wHVO$&`##`yw*!?>iV29z;<_^o1HZ4{g#0z@VH`3$5wjkSnDubP
zll5@KtcN46tcN3Bcs(33>tWBVhdDQUW<AUzwAuCW8iHm$?3wlOO~Baoa0B?P>)|m#
z+x75D7Ii($vRx0eY}dmq+x0NZ!FrfLupTB5tcM8%>tO=HdYC}49wrd1heyG}U_Cq@
zK(HRB!NGc%K(HR>mLOOUcLNZthuLVsdbkMMg7xrR0Ks~AF4%(g@I?TE^)P{8Jxm~2
z4-*L1!_5E$>tWg!tcMq)p2OC|bW+#DEZX%j-L>msD&cyVYu=<N+prRwmS(4(ivqE1
zO~J0R*s4w%1$8CdG{)a(#v(l-y7_*v_%(At^jBgH?>d#w84W@tuN1SWKcW(Tj@EyP
zozGv7LC^Kig_!66f|WzW-v`+Q|9bSIME`5<vHW%D0m(iuP(}S#$n=<hCon1g^Wd-K
z-&`L?`rS}jn*TUfmFfQb&{^4!fXDY&U^!mJuUZeQDc`Av^IE?iYOm(^0yWc5#ilIF
z{}1$J`!i}G{rqj|j#I<`6gq4ARZ*_xyA3c|`g@MSeB`%=4afLzV!2<({|ffk^`C?O
zdj3$9>-(9ot%3g|ENSRBse-}X9|Ct9`!%`B@O!|QrhWoujAs4~sJFSl6|yb-|H7(c
z{nKG{OW%c^t^BtUjn@7oU~>ISG3K=KuYrYa{gJTYIR8E$_Z<7RA=TdB0)LP9dm>gH
z{CUvX(SHfwo&4<>b2|IXh7<ftQF|Bvdkjcj{S;VrqQ4s6o#ej>&D$OSN~n9%@%JGr
zPdWZfj4n?*{yOkI<M>0+q|ZA3a9I8vkbs_d{FC9q3y%K>!u6u#e^M6@ynqGBmmGf#
z?0nhrzXtRQ5(<B>I{qclxzq7mfVK+)aQZdJzW~Z!cl=4%hP>hU7bBv(9e*cGeG?Bp
zfcBQ-cZR!fJN}o5#vaFi1dhGq_?zL!yN>@LINx*psZjR5<9CDV4;()O|2}m5Ul6$e
zIQ~iSXs_c}Lx?_d{8cdbW5=Hj$3AiVC*aYij=vWh!_OT5H>m#H@h=4OzmESG67~yd
zfk|IF{w#R$mE$i!u)cQu>4@|<j=vm^ee3vhVZ?Wie=Gdk2j5ZAe#gHT9)0ilujAhj
zj?cx}kB9_Z`N{Ebgb_bG{tc+%7ss!Hf4@5ZSo}NS_}9YC-yHv2==|OBe?<uXaQuhh
z!$HSC6Hfn$aT3s9j{gFzKIHg)fh=|XhDf(_Tz@9=`CQk38IfP+`d6Xl&vX5OFn+n~
zXTZSoUH@Jr=>@Jo6fwKd^`8gwBG>N=J6E{=L8Q}4*S`q<UF`bE4`&r<sOS>cZv=r$
zU4J5=)vi;e&t&u~H`b{DRb}9brq~62*w9|a)b38~LY5P}s!ls}(-mZidR2QY2C(vC
z0Ht2WASbq}3%onvRU7EUE@9Dey=p_8*rhB-T`!Y0#JambAQT7i$?8TPgYD@?5z*@S
z2sXP9DDtKBzHU>{jtl6$MYJ~+p5}H3y1yl5je)3By%!vh4eDMD3RvBVAkVNY*&<#2
zdPF!jB5f9)i9=Z3i$K06V2K-0qv*_8PHfzZMap+Kcy^JGy(oJmzvy80xS%Ay*C?Ub
zS!b50*cT8<MyQN`6UjV<Uo(ReL8p2Wk~TJX=oBO7)qzMOE9O+6iMYiU*1(37HhWz`
z>20aTt(t>of~EAuX}Dqqecou2X)4CN57ByTu9}Qa%BlCED=b60xTWL~Pc8xo#^YM!
zN|9x(bm}LAkSS!`N=9+?N@4WOK}dV3PvaDj#>_~gWE$(q;c-T4+Z`wd%cI&n$w=)a
zIJ-!y&6AAOu7a~mrDJ%Kk=m^j>e*1ooMdn&!i2RJ-w|_?5iv)ENl{jr6r~AuquWug
zE9E39Cq-#hQj`|q2vfdURZ?^(zU%SG)XTkyN^eC4^+)!$aQ)*3uqfC^da>9HT#zdR
zUxS8n)Ug}IMsn1V>N4{h$ytXfJB``k?nkS?KwIH&w0r}z?q^6f$I#Pcb6r!2)JSle
z-mH9s;d9d`!@vh200*#;Zp>_UXYE%4#v%E0*A0N$)nV1AL3J}eKIG2o1D@*)h|Sk*
zf}aCy?tW<uq%$q_k&a`TS8l}hm4?AW{@iu<K>OqPw50JY;C}oxW>{l(ANTTHm3SbK
z_!LazF|3KZlsK+tg41dMh7b30;+Fz!Hg=HXUNI5SOC)wD@ycR=*|kU<=(tyrSRl|4
z$Gw`+U4UB8L)A;Q&b>%`_r_8TsJYhz{Ril|m!mKt2Zak!SdK#TOO$&e0!@FHz*13j
zqZ0h-P_yZIt7Z$OsitymRnr!Q=4BPsz+|K5c_rjg&E|9%D%K38G}T;b)jUi!?^jSW
z2Wr|3SL775LQ5!snYj2dqy?_*mPDO4Q<Quq$o&i%mry{Bl$5$ob87$-!8T|bJ9rw$
zrZo28#vH@kvi+(^H$}+JHBdJYE2JPdKY)>@vC4?=a*Q_C-NOLQ24ouRo>2gI3lWWV
zFNw$1AuAf|K0=oPk|=li%A{K<!}C;2j6xEg??IRF46mzEUWh{TzRGPHi}#^wl*cHc
zjB0kwvTClUG^3nYPt`O<q50Vr)GV@U&Qro(s(JkptEMldsb-~Bb0^hYQ$dY1R;D7^
zSnooCggnaDK$AIbnkku$^_wBHnb?-dyf)SVHdYnHNgJ%`{VH}Hhl965K%4yvP@;QC
z;*SuMlg2qFjgv|m=a|NvMcjk_*CzH&;XeP|b>~3AbNB@9{RaGO#wWMjgT$xSPjK3H
zRI%wnDh`GDcxY%y1Nb>8i3KE{lg6pFF(+fUFi&M#n3_=4H&Ay1)<Z#6!=c+$RY?4e
z0NV^xF%Cv`fz)s`AFudvP-1WlAmcBU9@mtKQ5`nm4-0nrbJuYP((Z1kI*gR+wtpV@
zLx+xk1pgq{;bTl6Zd>Ovh^)h>qaB=`PSi3S`lqW6STm^pCqOyPM+Q^sTiD-uh4Q=)
z1nq={oi9_|e#IgA+B~7S)@VfRVd6fyxh?oEJ*V?d<!G!_XlnDe;_g9GoES(sCw#1U
z*2vWEz!jEzk~0=OOYsSu=^|6XNqxYBBBp^n7M)IX;#WxSS4HO$i$8bWMo@ZUeVt2P
zH#R`Gn1sU6hA5Pva32btt9@hA>NOy&(>aR!OmTF&uj1%*3_9Ztxftbv><^vCDo<^t
z)9oU~(e3tuoO8lV#RqP43F-EChJmLTpSatS(EW)s`O=9ly$730RmHjDE*-ucl%`0b
zTxzM>2!4)5;dvA~-=xeB0b%{TO>wU%j()zaI40}YiZ6kL3}2nURqCO*mNq{tjvi*C
zauXTn1m{~5*}y|Cg56P0NAUE;C-AT%A2Q?~-swc+eWa_evi$?srh@VtKDiWc+!)>M
z80BGiyA(*Rw{vsFZ!$QUSGsmm<Rb<t4-H10&b<}AE5zG*pdvrDN~2Em5sIUe8b0Sl
zbEd{)ixbV68jC0G#&8uZ8c+IykX<gGbdZo;D5f!g^L&`U;p7~A$0FnOOs!iRfV@J^
zsU!O$2BAAwdn|F?E2wNP@3PM&v92r>>&g<bt}GYp$`Y~1O(60LJO!nFu#U##o%X`E
zQSDqX#dQ*#zxu&8cRT0_!@<uJJm*1t^so08tRwZpwz)=~!1KJc-QB?O@2_v`h3y0$
z<fY4n?F1g=r61+Ob^;IbDu*s?`|%6g{x7+-PcCfxp$pr7=)$%iy0GnsE^Pau3)_D9
z!nPm2u<eI0Z2RF0+kW`MwjaK*?ML02NCk6{=cl;5J;NO2`KcHmF&xW<ZQmEb9OU`o
z3)_B{I{<9vAkWX11-d!N^Q+6XY;%z3=eU<ZwK>T1!xy&w@P%zZd|}%UU)c7;7q<Pj
zZU<Cq4)XkVVw*Y0^N)9VSq2Ar+`@E5u@W;X;-vCIx33qv{S9<l4)**TSu_WG{zkfQ
z4)*+8=sgeiP6p0g=oX!HLv#|$3Ixj=It0rL-M0de3*Fc>u}DpF(pTmD>yAT$$TjaO
zmwgCYxoh5)A1y{oSGle-4kY|HI^(jae`6=kEkB+>TN-x7sN|P)L=yWa10T^d!z!Db
zQlQ+K;RJM_geqktNa)#~(pR>j^qu(j{Ml_G?Oz0268sLpBs!Jb0+mqZiPaQHYD$42
z_;&r44Ja_L5e0r|MuG9bC;BUzz<;N5=gAa%`4Nh}l|`{fagyZvz0s9DpMyfgUkTO(
zr}Em`WPSS^vc87_cB$V8J<IXi;M?_os6v6|5RUlw0u2E<_e{;ZAH4tX-1GmPd;Y(3
z&;Q?^d!CC0LfzDF=U^7j_<c0?J2$%=<8bh~W(=TP1WNES3NWgrZWbu&W%N1&&{mg2
zrtf8RKM~MvGSpZ1s#0SrZtJQM#q|Vys@(`K$DmVOHy5ZAEIN}Kl$P$g{eT}I&}c_y
z@TFYUTz4ioc$y`OMMIoRO3GTxb=QM!OTfBWDm#bv)p6a2fPco)s&OK4vUoC>THnpb
zh<68=-48(L;>%Pe@>zQUrylFhz&{R{t{cO-7e0oED02^?+(;;2-JseSmQ@`iYHDZq
zztvUvL<mm!x5C_QrS@`hV~eSIAp~#ux59I9{UCLSdk6lB*$+bSaVzX()xnIHI>LR~
z>V6xf58~uSlpf>m@-au!>0dxPNb<dmX;u?jHo=Wye=OuoY;bDfqvTTT15zitk@`w*
z2U6!a`CBaAQzyHBVTHzga0i1lB2J!!6aLhx?k&KumE4&q%nQibg_wj=XUeL>T?O=7
zl9+QQ<k=(HrgPkqdJw$>v<EBDGaG=uz-@t(XZID*_EeyM2{%)hxYuwl{1&tW0bP<e
zs~dek&wU&@BY~}gu9Sn1u|e{x`rB|V^&+<#jz5?YZU@ja$?en_gbO99SGbvVjkwn@
z5GDO@Q%ML|z;Ft#(ihWv(~J&%5ayco&}}pB+s<b3YCeUDJ!2bzs8{nLIFj)I4f4I3
zmyz-yfofjO>+vt+Ap+ICn(vYAVFEc`O`aWOJVKzZSBuqT^m2cIg$HQmbF9Q*PjRDY
z`>OZ|R_^9iQh&Dr(5(W}%OZ6Obq#V)1iFtUnNFC)PM9(DJ}e4f2dm2?(kYfDQ>1fv
z@gigNNoWZP`FS8;8nDn>C#MX}l2O2W6NH6V*zO_A9_Y;ckT&GpjaJDxtEKG`J3xN3
z0?R<OddBpD$Q^2RzXka~z+x(Djg$I}IjQ&-79VX~3m;RFVR;=@WGqTW$c5!NkULox
zC#N-=qVz4~flvp6a(bLPl@?rZ9-3MFm<GxMQW<;`=A0rQEEm_X(YOrc^%Yn?g?z@E
zMTo3u*#`2n<ychJpKO+^yI^>FmU;RCc)lQ?M9nzEOEeibHbJgYmFJ+4gpbLF9A2Wy
zxOpCo7V0seG_zFGc(vC+EaUbw&^}b?btO5jA9t)Ew~jH_<EnOQ1K;Z4_`d1|r-Rw?
z>P+qKRK1Yp1g|Eq$5mZH7Nj??$5maq7u~Sbt24-{y6RIz=zv#upi}h{7I8hU?hvQy
zr7YumTrJj6wY$3=)$^dQ+E?T;*q-iNz<h48nXFE}l-}3<1vD2Ph*ai&5#{x`s;9YC
zfX=a`P)b*o>v6w;)olmzNtVS(OG;Oj>v7UTr-NJ+u*40h$Ln!b<$9d(Ed|f@<U0t(
znH~a?Ne45dK}meC$Ln!b<$9cm-4BsB1NG@-uEwvLL5ZM~!^W;E*W)PWeG8Evt(YT?
zT~)5f3AGX$A`72TP-^pfTvfRqM<2ZAB-2z}YU1@cHdoGC^yXNrN=VUM;N3ys8BRW$
z<>bt+2I`=uRh2p&lsR$gpOA`;+J}70rR*i3ToI@KLTZOOm_USjJ1F<Wsb^q5iWRO!
zQ?fI;J3!$CSB+70ay~+*kDWaly-KKGfbw0O`aSH4^=X5dE0^^q!H-mY)aI+unX9cn
z5vLy#?S`PVBURdm0@~d!CURLmED*ka07sYS<Mz$TX@(A2ZRMG;SNLXuXK6V;m<xld
zE!u}+NLa20d1D0@j+)hmbbw#N@-WEHmS?H`0YX)+Xb0R9mUk=}&Bd#tJpReaX^76(
zdhfC7x(hFp@u|k#adHX=gWB<Ad`n>JfYK;V9R;a`!M$22wJRvy<J6wWk%ZB^hbVOv
zDC6VQ?#)1**t)4U$b3+ilFC$fa{3Mfby@=ImFBt{l=X4ym9*-V{?IR0Z3Bg;cxp3e
zoJ_gSI{>=eH$eH&%4R-QjF~8B21GN*tb-Z}%r77(Rn^L+F*8j|9Se@kb54gb!gmaK
zS{;dxEt+{z8@MWbJ;5`u9G{c(1c;dnI}KHqvp}9zfrTxaIb=gCWw{vStIM<0{-!Nh
zik|79EVo%Q;=#+;RaPOYPFjDYU>XO~G&XS>yJdqZkn|I>x?GC(=BFXQ9JkSZ7onRM
zCUAoABVOZ^ovbqx>LPB{0Zf?yuqc6JPF;Yr2~HBQX~b*9Av$Yv0=4IYI>gDEBD5_4
z20B@13s45eXP1C+Y69EpLC{W~=VVP6;&u>Mt^#qUfG+@ioZu`0cLCf_(YXnYVbC6c
z?_UUTzEJl9{FvYZ0lxs4dI`XV0`3PmWd*=R0v-V9le$<y%r=c~SPgKAfYAi67Q^GU
zbsco|BzE^>QKMt91KccNT3D>f4?2^{Qy*p8;+_D8o&i%^$~Q^86k6|dUqdUnqd=Qz
z>7kj#d#p2*bp*e=1mue>OQxu3%9XzN%EF7a#Xa8>ZCk;1mt}QizVUu&JWjdz@XPO(
znt390n+bspYf3Ud#J_rwAahF=v(QU-S1ngvRk4WZiI0Tjh{AC-QmUw{x!VyfAsu5Z
z^jsF`itnlZwp3A9Pj}Zjs_QoB;R{BMUH^-ZAtz+6l%dY}y~xd2s`@KsJI)|{{&&BR
zqW>QEGux8I(0xy!|8FA@Cj_xLvi??W&~6{Q@3vR_d6(T!f&L0@6bA#XPL^mm-@D;>
z)sTi!?Rd^r><kt6E=RCSHSF?w*&*ySNS_;Mh*w&n6xfg%Y7eodQDnQ-bshA)8t95w
zU!inFUE45>cw-`0o~NF1$DWD(g7HlHas`v(pe(?oYcxA$KGbZERI*Lz`4Q=i$3xZ0
zfjZ&MnZFVvN$#Rjt#Jg%g=9H}vE;;LrrxBJTQ$~cY!S%kS{5_)CY{m=!!zx3uLXtg
zi4+A+4yWFv!QC4u^+8Y`k5f7ICXId+!=uQ)1<LyYwb|wpxbbXcuqAtbg1GS~SRxp1
zOcl)r6@YDLq$&PM=k@$VVQUDM)|O2Njn_EPeHnQOQ-bjJ27NGjBy~7^JI!vI51yYQ
zt1-f>z9i8%3w#SKuam_^s<%ENQ^z0ga*(cxles1Feov?Z<Eh2F2c*X=*^HT4XPybd
z^u$FN@rCCt(0>nj;!PW5TV{|oPFCxw5Kc%s{d^U!isjSE`252-5NI%q!K{ZxP4a7+
za^6U&xbS~g)2uzVA>NeaTI!1H`N<5Lw-u`Hvg#beHapq5Ac$?Mu<ZiNSC%a_ZUixK
zvVP~d@ltXRO!5+}2-ojiTgnvJEMo@9t)lzS4K&9@b91od25ebxbN(C<eFe%yOCOL<
zi<8%!1@hEr{nbk5Q?3`2EZrfnCf@(e=0_HTSv>LZ@EW=fDtt2pAGW$fB3#2bSr^R%
z_Y1K%&~e0)H^BKxz#UZOWL;0SZc1$(?!1Fw^00e1j%V#BxqHfMmRt*@TuVNz*2mZZ
z_N3gs%=!<`f`D5_7%nL@X`_5OSq@UeTUcqHPB`8ouwPJ4#L0RGVfX&5Gy~(8II{ut
zvT{6TrJ6OEUO$pLeX%mW2&PXCXAIYWIWpXvmey8BGw&eSlaH~9PLrLi50H1>`)PA^
z0{7~Io?DJb>rX|?dRL{7*97$XfT^H7W8k{_--{-nTlqo^l;YbWuwQj_k%^@cneC52
zR7B)1uzx@iNh68!T?}y0%9&gMo3CLUc-c#_!lhR${o8Ehe}Rgc*lU?CE_RBp(G-y{
z>D|26hmgQUZ()@`ZKRKbU~dY?Q&}{~G?4ZCEO3`q*<?qP@!-5Es48w}g%X9$*1tk2
zFv-b!68kRihbpf((qKFawZHwd>Uc9$P;CqzTMVszswV3)%gd<iI79IHhvBb+{Q?F-
zlNdT#ThE5-dDSMMt4Qme4YlV6swLt<A91pdF9P?jYKt(?vk|@P!1-{%eWdz`bDCW;
z8_YjcyGL^bBW8*WSnomhBP;J@<<13LkIdS(!~PCZTWqFHGfLWsMN*MO>Zm2B+4WRd
zni;ceUC*y1^$v#85mao((;cnB_HpJkjW5hg6Sf6lSz+1Y!;6#k^-Ktykd=#W!lv_X
z0@J-@lza=OFemGKIzA!mecM-G2Gh@h#(!QQtA=y3*4hyvy*_Tf!sp+%y_404Hf+fH
z&CbpPAU!|OP+_Td1Q5eF##`^Rtcx+9NRnIz=^fM{?jAwJEKHA(%epjqn0ZtuyOy1Z
zKY=o@fptpSxP0o#aN*?S0-IF4wY}O=6C#ZQF>a)k<%-^=%F-3&-T{kbW6pe3oAl84
z7&FA1aUd5TiDmcww%L|}e0jhkrZF3xoF}m&O8T;??LD`F=l+0CYReLq1h=QI7QCIH
z|0m#y>&hzS+~ZUqvrO$e0H*AQ<$@A)#rQbnWDS}GxmGFpx`y*|A#)1l;)9=bli<tA
z+Oq^A1=U~HD7gh-pBjj;L7f`?;hMLq`b#<$xXVZnb{#mbIxq)4yE%ezLk7-!3+rKc
z<h>Yf85r*;^SxLcI60+Azv$Vwftg*!eHE15aq6sTpe~;d7lir^C_luhBNu>rNz;W&
zO~J70<D;5CLxV?e9fjl+*`}bh3aHt)QTBZI)2VQyJJ3azRB`^1NhtVovKPbD=<V(?
z?Bk0dbn_7e<5QTF;?i=|8qFKk5_2-VcOQfB(^i2wYK``p(@d%Ff$~wD%A?lkpi(RQ
z8z_h3R35cPN99da+3d!6vJ4*+3|Vbv*YiHdRKcvq7#8Jel6N9l`jc%APTAs<up<|N
zvPVCMrQ}{v+=<|s5%7Hul#}%`lx25|JcT*p$J3!}1?blXJfYN`4K3L%z4~ZO=~DNC
z=^@MLWL<^vHT!YzcC?p}Uk7PVoXmx2b{B8iL?wR@Ql%#9S=?kZ0L#&_<b3ui-qq7p
zq#@Yb9WH{E^epHu@QO7o-l<@kL&oyqFtt<|usE+fS-(t%hJjv31VN0y6w;f@SH<+x
zk!?r`xSK>~Oi}K~z_~x*mXOJ6Xa&ZwkZbXL3N48^+h$jeoQUv{?!}tovJE~*vfWIc
z*|j3$ZAdzRa}>GR4NcBV^)d{UpFGVhlRe#Aj-Dn?&Vt}2K~?d2Dl~?0YRg{Y%{o&>
zHiP}iKqO#{J85R3>`T2n5F@GOU9kTYh#X-aF;h?WdT$#hauG>G*J*=~@v4GI`Sx?N
zK4R0|;T2;D5aClGIFiEB_(bWlSK4@_x5@fa0wx~Ynp(scY2#2s{|t+=w|Ng?!Az`o
z3)mkiFCvY~m^*S}h%{6$&^bMJPWL?sd_*A{<YYC&1e=}hEwuC7?;trCVGX%Ck2<nf
zx}RzIpd}(&YFieQgHDT97(sk@5eMgtR(MLZDc}+&vzx)0$4zo+a@1={Mt2<8im5_Q
zBJU($Eg_V;3xPhzlAI>hX!A0cdflt=b)zMjqtjNE(U$J{Zz0|3p|Cv$mZvS7)AYC&
zpsm=0Z&pSRgzFP<d`YhRSbNjv)N@JRu_o`ql^|R(h*jf<TxLt(bQvqVVgvl3OJGeD
z)^=d)Y+0SA4!N#*Dy}d|xJH8GOv|MTYlF$Xp+Aga^Fd*<ur3GNmE~BKKNW4{-dq*m
z%qQ?ig?}4(cb4auE3lFi%u}c7>6E;qDNIwzm`Lt}R18MMz09mAxq!{K?En~6GDRet
zL26KWNvTfqSNeWXyBdE7o!!0%&Z?qHqG%@2*Zoh5#G;qlAwN`6swjE{N`4F!i5tOn
zXfq`Joo%LLHWdTknTDiQy=kJiDgw|G9}}bhMX%GeE5hdfcLe5DRhcd-Cqmr}s+3;9
zK;ap=Qn2nHjuv8zp=~P*>l(0KZEJU$ep3aspI5~d`ohJRwmoIJ%*`Y@S23{<&Op2+
zTi*qFuVrys*J73)`ip7t6R^LNB8%GG-`Q(Dw>>a!CG=>6Mn^|%qt!dHlfNMy9kA)>
zwMeSo)$Iy&cS^{4pTXw2gMk@iv6;)*>6%;vtE%^Ksmm?F*Ahz)T^Fg|>o&M~5xeeH
zAm3<NjEXkhsbbJ#WC61c@$y95V`RJ3_SyJUi_2i#X0`nRQK{}`x_iL46HA5T7{8>9
zq~1A6^<DSaZ$LIqwIrwOgQXEZ)i^DQan-J*@{Ra%3=6NMc0ZiOY4#gKT+5w|9H2Bz
zbXcof2<FKY*g_@EIAgnY-K+3VlSGVjSAvBvyc0IlE#q^llw=BG$!W)d-H1I-k@D8U
zcg7}-wM$O7=Zn5lE@k5V2zImKsb7|d;Tz|7Ij42QjmNEr0F=<u?K=2WlXV1dTy}B}
zKs2#iKE4UN9YF3Du%MtT^~C<KW3J&&!W{|hnH5-eR)&_5r(iG@&liKdiYz{s*)nQm
zn%bVmE;jM3+v_Ud&EVNqo{v>Ht?|+;Cvo1UlTqD^pzjWN*aMu_d-6a&=Q%LBUx4&u
zd2-HD$R)05gKz0Dl@N+5_=ww1?N2Ctc?{n|Z4^+PG{!KEvt#=`4G`BZWEcQ?eDUwJ
z-*+M?>lyJgEybx{)EoER0j$b`+tqcK0=NmEYFBi|7wmIPy0K&eA|)BqjZjC|y$=G<
z94_P>|LSrL*z2SY!y_F=hm6dBHyFZ;)<v*&DY>$+NC<mzV?HR;NXb1CWjL)ng_OWx
z)mA`6VR=f2ls>rb$;YZ|w0#~bgC}G<@v-U}odjo}lj+3As%vx=oPAoRGasw2(XA@h
zGaF9eW7Vncc@tlk$1`!9SJ0!r@%~NLD%;Gie6BL}Wp`R73@2brV^ehLk7TR4ptO_W
zP6OU-c*O;gGc41$pqZJc?aQ;o`A$rws>d_H#}IY7t0TazL(!BJn?W(!X9lGpLdLd}
zg!#U7Wj2hmdauBU#~yA#ER&+VS1>QiTYdASoFrv^EnZTTVt7-Kka;U%o|KcKEGI>I
zq@EO|l}S<FEsz&|4COp2CrKGE9`s4&<^81SQ^LvUCq-YDZy7hE99NQ}99i?CoE7q<
zoD}6ylczQqJoV&Bjd@Xaki01S?8!8f18z5{nUDY7u~Nj7F(~ClIX>rU6)Y!3uLC|Q
zdLzDj<VAN2ir;zBccjeke6ArQFM#Dee0pxKtA*fa>(V&Q_2S5iX&~}FfHbUug;5HY
zv1dng<_wUWG!Ca}9QX4%kwqE-+0n{K!OA#kocely2<4H1;OH|?hgs*8TFAXhT_WRw
zEw-{OU|2p5orZT1R?3f*0>fce%n>qPo`x*VKS-6=gYu%Kvd}jb!Afk+QL^8uXE}*&
z7|#9|V(p&TmPKf%wtNoXk@r#B7nbe=369!e;J?V9AadxH%Bh|V{e>YZ%0Vul19+q%
zSh`s@7Rbio-RV2*Qq(+B2JJLz%I9z%8H!R#MS-*80%ytSozEdXat`>e3iI}T_-yE$
z$CPpMIig24f&9?nEaw;&j_Q%;LFR><z!N8*LwV#sz<pUB-`NNLSKxThvGB{y$7wuR
z&HoZf8>t4uF_y@Jlg2v+^0}Idv;lmAB{Jije6FP;eSjNW9?yMGWIS-i<?*~bE;1ju
zWfm_M<nz9`$Qt0TE04c|_^rTkvmLh>9hQl*7V}W#c~D-nR2G~xGUxNCEb=Kz-<2cg
z^N=iZ5V#}^S)x*e@_9TKsRrCJ<?%ciiyRBw@#XNnk7tmF0>{Mf!r`S4U%Z*pl|`pF
zC&kEFN|wpc$>%9rWG-;;hgF@nX8|UJ^IgiFHhOxzeCkXolliod&Xn>;C=V!bq8GTI
zvsA*;*C_QHILnEy5ETQ@o^6W=1&=){eC6}RF7g+oPRB$LM1X2<7Y$UarK!+mipEMU
zi<MdyEAMcr%UF4bI~wIst@%9Zi%bJ@ez-y>pEn6aE&y(g#k1g~@qjg-cjiSl0lw1`
zB_9V3tOk77T<}Fc0;SK`u_b^n1B~KS6w$O!-<@gX;*{3S$>*I0k$qtP&8m`m@_E(X
zO~y1-2OkLp3;hNkMAhCOk%j<Z4bNufdAxRj<$1gtoP6FH80kiCd_+{N%jcP7WF&Bf
z7H_hPw=+a$0=LNGjS0MmFtP$Tu7cuvlA@fi^QSV2wt(`jrAomL!7^<B0MdZ7xujxD
z226F_6HRGK563;p6zAY^aaUGSX{^VL?>ym;B%XkI2%o_FzJI{oq$`N)cj{y(=_+bH
z756bGU2RI;ousu=Dm>Xqx`yhYkSnY~d`34pcQxS10*LqJ8(9q8_qYKpa!Tv_NG}%I
zNKQVlAVe+%@1x~R_>Cq!WK1~ANqXdT@QMkKni5QSj3sE`)q}_mNdHw%!whQpGSKjK
zpy8W9!?&t|=fsiJE?5BKBVjNt#@lTp4S{QA@hmuLJSffQ&7F~x0r#~;7MwI*#P7{z
zZscsh92sP7*=N3!QYzy@J`bKF3qf3NB~5j_4Z*z}xZ5n=<l?b7z4daVTvb6e7pajK
zz(LirIHl_8Mm28%j=TlpUaLYZ&gZ?Rk?(*zVDVyXK5wFlBz46^h>v(KIoX@*-AGH|
zSc$B2S;>f8T9D5>i6bX~m~Vy52+mtpBZGlE!{SYc;1zVY7`T_PD+sdeG(PNsYv|N@
zT7C)2SEqAMJMG2`P+n`w*Pn~>HB#<35bxt(C+q_!2c@$@$N)Q7euZ*<IwLmtlGRXg
zy{H(79QJNCDyH@v<aoDf`Q+}7ce|EHI^G>p9PC~U$-7Ls!#tGBOnJsql<zU+eV3qo
zuV^1Uc{$4WrE_{8{KbVRZ!>rw@$$B(a}he^5`<u2f#dB>XM+ta2}&4d6^W6&_X5W|
zl+NBd5GAKFOM|n{gBrIo<@yaANtRS044mnBm8Hbi87l8K`VQ5c#&;Wek5!id^W8>T
zcJjIBiW~$>^^>$g4j)rUGNVLqZgC==0W%q7Yr|wHq+Vy(!$|jcVBYkB*s<WRvV5b#
z*PQ{0Gx0f{t5dvaXgKf58mF%jfNApQt~(DbtMM7ZwJWoath_I4y0Q)mSnmSc2jy8u
z%J0qWj1cV(88u)AZbC;&ycVH#*WxV+aDver&YO{Tsm3~iMmG<~rTCn|%GujKhsc<H
z$~RWt+%r_Ybg@J%RWH#7^5?GWo`>65@EJ!h>oiGl#xo+8FWs==Xm>`K$C+@sDr$}0
zT)Y}iPP}1kzZ#H~Xyn~>w?dqQ={U|+!%<DaYPk9ou!U7P{uXQp*v~S&0)|n<oJnE?
zzfw1mQ)sX6(7*UMlbMvz`()_b4g7wvk9!e?oTex|j>5?(6fITu?#4#E4rMIff)U*?
z7VkJ|GZw!+#^Uf;<hGs+Gyf*BmM2uKiNQHuM#4R|rb1T0nr^o>ea@Ob|CcpQ?`><E
z$cg_C70abRnF$@c!5_}?In5HBv#zSZkkoG+mlA_WTaBg+3nz3Q$IZqipv4QDf!P7f
zD5{@)FCedZf;0J16gr_0_<QU=6V<xb-^_%Kw1km84~*jP%PcUmNiE=SKNb0>$Zja_
z?*tVaM}Mm)H0`ebc7aauH_V{^&LoZghH2_AY2vRG=<l7tjG}t+_j5qvZ|!44{$8P0
zG_d|wPq>Vh(BDa56o1QDpub<CQ1rmR^mkX_Z}Dl`wvV>O3znfCog^07b)vR^@u(1a
zmnFZA_q@no@dY9B7lzEY`4q1Y;gfr)*1VIf*u5cgV@qBgkZoqurhH3|SNdH@`V2#s
zN{6WT6q_BwKM}&O3gJHv;cvIq<n~EW-~I@ZU$W#219I|yla8$ID@%?W*(F4d<!b~=
z^Q`g_A#zho{?vBx;+Y|GFH7!yvXx#EB2TpBc-^;$$jdExcsDEkeu%uulA}E=`I`{=
zX-kePPxwCW_s5p}V0SB>6C%4k)$e%$`PdM-t|fO2$X!C@lPvj*!18_}@)%358;}b_
z<i(c!MG&<`A@Vv)-V%_n2$3JL<mmzV_7M49OWqRH{Y;4bn<eK4?frg;Tpi1D>1GFl
zjQ=S_?qJE!2kDUdLp<igEcwAex?zYs+mg$Iy1RtPS6FiOz_B4A^1YUPAdsFCBEN3Q
zlY+X>3z5IK<mv(W(Gc0kv?+c&K_}WBBDc2W;g($VO^9p;CTkWR_x(kQ(|O@}IPY3^
zel%g?tH}B1uA2{?pZ5!`JL<MfaL$$=SD&5CJ<C$U^5g0TevmLXMcy2te@~cO9EAt8
z!oa<Hrdj>WPv+{UYwHAO^i-^D7YO)AZh|w3=TdFd8oLDZ&P(R|TF3D$s!baV+bd95
zhJrJZM^l$8`z|Q=E=%r4R-Q@iB<me0bZ84!(fmb4%`1~<Sj|_1OEf=5_K8^#Z2FT4
zi(dlGk0jSQ1)6!<_7Pcw!tuZ)9|ufJ6k4D#wU^4_a8mw+GM+5tJr9Lxl$Nh4{gj#K
z;zhF+;A;`EmT}v1DU*w(48O3F8He{*@^37D(l<I`EN&`r+W?%zIILCKAZ*tp_ou~-
z#THi1SiFjYGx1f-r1vU&9-Z5id>vVNB>4kbgTiOPv}%W@Bil5h<!hY9Yoy$tO{At%
z*{#Wa`f8-Ugs4PH3iQ5Fdtex;Gf<dXOXaZql0Tu`LKgDAk3uL?b(L8nm9N168mTPk
zeM4m%X{}ya@(OFQbG%wC1zORV7+QP^3eF@dIG_reS_Stczho7ZS_M*|f?KSD9aJEh
zRr-sG*9OLGd-8nrGzpW_!Pd@#)Xsv`j>6QNR1Oz-<xeQ3miHzULh;(7%o49x6!;(G
zRop<k3~x8P0cK(}@k*GReE%e}6aNM}QyPV<I;DJ7Ui6d-SzM;$6tXyyvNSnJ<NW>A
z;MGs4g>O0wHsa1OjzqJTV;`WJ|4{ecb@figaTPw}DD^!2pZ=HfoCvHj)|p=YR}=2R
z1{WzQOLC@vqR65+W6{@&TNLalFhcU-n*B<!oy%Q!9mKZbBilmm%-o?P_bowlei`94
z2zQ7xgHO@mA$c+OPK>qubJy*L(8~Uyp|x?x1ZPTp<y(!Kr<@oDMuveoVc@bbaAz3U
z5e7aF1Kw}O9;xS;Fwi*+3=0D@Lx3}b5C4=Y<{dK}!K+*0(TE`-ua?8BnS3)Mo(>`*
ztrJF(UC8&*d`qPFX9CZ*t^^V==77Hh{t)m)9?UiW-I$p-3lYsoOc;PBUIToS&gc$2
zjhl`_!3Izlo`%i9dSD&`X7Vj4B%Xjm8443nDCv#DFx7@pRsNEm88;Q^?Vyh<Kq0LQ
z3d2#zMPbSq<=({k(V4pUPh*N4CjcSU52geSnuEe&EQPeEkhT=kjwzq13h9X{e;IwY
zC!SiNcjV@npvoA(J?I?HL>@rSQ``14E;SQ#2Ev{<fWL<J@QiUM3I%zyF)bowlm7!u
zVpoLY3ls*j@B<2Wq9AeER530;NnCydUE<Oc+Zl<=Stv}oy?k8CD#nG?UrY%abQcO!
zE3y>Qo<iDENIRx%3&-VM1^!iB_Q&Hgi7ouG+LmD&7<ZH>PGW4>%&*fXHuKLcu$gOO
znN{>t$Q?9+ys3F2b>;t|T?A3|S|)C_db&bN^xVP%^}K|_lu^nq=_1A~Q%qn?`^2BE
zlJBgNDp-+;lH*Y*x-z818Y8+Ms;H}L;>m;5u@O{3#}-*#H&WM2f2V6-MP2!ctE{eP
ztgioBU8z{;7P$vaEa+T0yBaAW@bT2dcdV|~kQBQHvA{shrLGP~&{e!wM-sj;;5R71
zkj>$l6U-cp(#3WlcmW2~7%cRZMNaA={M9_Jp;K%-1nHd)r%!oFTbbis@p}r8d5mFZ
zvrp-$fg!$lQ3zNTXDYrYgugC?|00CHH-tY%-KEl(Lim9p{3nWEjIIB;5dM!4ep(1$
z6$2?JHS(_v;aezv5%AZB@ZID1qOBo3b$}NmAD?O;&gbA)>#&3ci~PCk7DM<ee1fyb
zYtf3cWJrIE_)h>9-=boi3hxF|r`+PZ70>(6cLsQ#&&|O)_fe&C9yAh4F$T@xGm^WM
z=bT_ul0FnW6-*m}L9MZi9JL+Ylt-$s$S&d+phn}M-9WEkgFKqk&8tGJj=lQmsW?F2
zDc~Ih`xN3dHkoRB<R>_@<t%(}GS9*zYt}BE-5j=ipD4Ux>Fh~3$MvenkCR658`g8i
zQe8&08)AlIOv0$m!VfMj7yx&FfZe0zJU?0P!B~Zg=C*K+U+h(j5rpxTerl%k(|*)3
ziI;eLD^I$%iT71<^<n7zywv+FlsJ>Rw#O6v%2myB<wQ>;S1-{1AlF1*0=`ALN@0+f
z6Fr+8V_63_%Tqde#+YxAtInhWxCZl}vg-s-k#!U)0PQA-j2nr<bKGc8|4JLG40JXo
zDkvU!$c!6&anw%5)5{?Ys#826MBZn}kvV&$*W(Amk=sh=QwjVbT%(pbQi2NaR%&OZ
zETLiv;y`3IHcH^){6Oa7pOx9g{Z><rKrHf?o)CyZWlOQaimZ7M|Aw<oo#H&lgo6(}
zoF7#0OqrxX;XGcvN&(JSuL{!;w-Z`3yk9G~(*4P<bmz9vaR0s8IERh${;XlV1h@ZO
z1YSk%(nfaI<qXHxYy>F0BKIX6DkwwbUjnvpJunk_b#JOl?FGea7rh_EalEj1Z7&?U
z@;-(zL-dSsyp5qF&N$^jHcX3kl5<$ng2Fr=$a<rwTTWv?1yN38Yw7yFXqQ(15aOr<
zgEPvXt2WOEn?RFk?{|QT?@)n#=yiu+tO^w$2r0^j&x$Xq#9wT}ANg$s7|-}1wEUP8
zNRJ2u7v85r@OgZK;p2=`V88&F!{K9)bj__mjpj8w0}k)2<Hum3UMnCk%ncFvF~DZT
z7$&!Hd25CjwN{gu`7kN6Xb=7k0R?spKMjWMQieK-#xi%^=P>G#F=`mEFztojv%i~w
z=uVCDu;s>nfZvb9znlvA?RKC~i}KQD-_rme+ge~=(tL|>T{z}VkMdgZz5xl&$fs8c
z%*(&t8fR3g`duQPEEuSc&Ju9#V1ihd&la%mkOXJ`XOu6M+r-WSEQbgK5={K}vaU7J
zy(7`IJS4aQ4DkXF3*G}}6xk{qA`p*c<&97aRaxD6P<BbQ<tQlQJy4y7LRnBqIvzTJ
z8AY~fM9bGWi-$5EZ~UHUEE_;&d!h@djF!DZWkG?K)fvVtptADE8^o)!jY&pXDV1%D
zK5vyx0*fe<0+l^XWn`<MjChQ!w5)H`D8qtB{=9wBhNIQGW~Zw{DNx~=z)Y<uQb9R+
zFd#KcFpBwj)i7vehgd4f#|=9dxE$wxbO&*q`_UcY6c?xj-^VmdB{+b}Fk5_Yh`P1{
zmHJ|boGFhg4@bl)Z-#&cF>L@2k5jrTAZ%xbfh$8maTSd-?TMUWJfJ=N$%5ki5V1*!
zNMo%z{gp=s=!ZfA149C$Eg{(ii&f1I5xG9G+R~Iqw6zWi+<dqI&f%tX4D(M9@xOIA
zzeI0wSm1_`KoUj+HlJ+_h5!}Z9_HU0;_q-czf|yTSfH~e9$g+E5|DUI4-uD#h~enY
zR~~7_HDO?D7<e)SV9evIA@(ck%HR~_UDR>T*wA2n9g^6Lc`w8`<~0}&SjId9F9$rT
z3_Xj3^-D4C#?wY%l)_q1p^-X-y)48l1;h~cihwTxoLd+(jgAcG1>$l`E?zHY(@%)6
z!`KERxQKukd}YPz6eEtGG?PDf-3EBDU|h%xcNDDLuU1A<Xyy4p$r?1lfTMM2O%(7^
zfU?$GTgfQ5Crn;iiU-Z7bFGIv4yIRC`bj|ko&8Lgkp25lQv2sFR^cNEK0(LXgN`J~
z=BtZ45K@h8V>r&>&s}%TINZrm5OVwn_%SMPK29090RBmCmX{gC6bGJx8D!i<44($R
zoABqC2JpE5L))9c`CNVf<FEI77&Bv;k=fcbg;L2{+C!1F+8Z;nGkCp?w-~#bLAFpv
z3yDlZw2=};rCp`cq9_$nsZ`pP%K!O1_uTjU&YMA>-}nE1JdFFi_uO;NJ@?*ox7X{w
zz?v0nQmb}HYHI&Tst??CSX(dnTPmL}xu3~<rXbni);*cr(!%Y4qc+coBroWV<e|NL
z+Vm=YFnV#~e61EM47G!aX+wL$3v<%k9|T^14oQD2MI|XVs1njPlZwO6LQq8m9DcST
zCO<Yy%n(HyFy|9g-KrX^s!^%JK{9_1X|?-;`Dz402$*U9EwSWDX>~9q5RGpaDl(mV
zC{;&Nk?AXzY1MvUDkj*e<fSyh>1o3)!-hgZ6D+P~wA?|8I%(@j!Q$tEA`%$P;^&ei
zT3m_KqE_R@#rQ>vdV1QoR;vfdh(-O5GG#?`ek}PJX(?!IQq*dBq+(GoK9s5%saVvO
zpc1AtNS?P$N$GJS^Tcf4FHxLgG`rLMho(IYJXWBkMy81?z#@OT(oIu3Hr5I9Uc~_<
zOaGoI@Xr2(@H@8%{28zx7gTpDgOt&*<6UXrK#DA;>JEUWTugOD!0X3?EmG3vB%PI(
z+D8jDoustX8j?x_{Y6QE<rkcuHiYD?p&v*d39yFh4n#>}vqWo5=1`<j17B3#6jmpy
zUhwv`QZlE3hk{IWnL~O}d$N*;srJ)pZ(7o~E$Lq*6}1m4X{2ARN~_yf)o$dg+7h7J
zgAkJ+n<YkV(x}?m#~HO5!C!Dq+KpsRwI3jJn(#H!%S!jLSn@m4CX<{i-G52W$~i4$
z$$wMwh*9RIt+wP{Kqchkgq(GmSR=-U_oVH%$~;MODzi1hy^WGbbYGuV18q^1Ngkx!
zn;>9xFNr1JkaoExA85(%j*vecOa4;Y5KI1+CI3mtsry&4<gcWalAMODo3FZGi-0%!
zF0~3f7zT}u+88uaP04R%41R(6ZzFTBJndOr=S)OibEsTDiDUkLVmVYcxY1x>IaF>S
z=>v)7P}$(mfyZ2}a<Bm<zh;oY-g%ykuN}isFR!uE@M?j`yvjYn^0Il=)Ji3LNh=p$
zFaxt+e8(A#1V6L1x*~~7ukVRvY1J}`ORqPE0C&zut810Sr$w9HjB&9PJ%TD-B`Q6M
zC~+8V1U@uW9mdrH#|?wSpyPc@mD$Yh)HdlV42RO2p8!EL!2YqQ0R6*~+xbIG|G0iQ
z>|mBg{nAXucw@kLOww2~=F;bDkVFGqe4IK0nM9ejnx`T2lq7Aj%7j26%1D69TsRWS
zSn^D*lNe<-g9a^_tD$Ym7^`LZ1v@1*9-s>4jZ%dqK!tW7CV!`9ArJmyRzo+8mXgwh
zzbIu7YTZ4lh>YnF9|lP@K&^j8OfJb{G-bS+ATHgQZ5EmJN!nzYT{=dYNr24mK};^m
zVwnAgc&&r5GTT6A`AObDE%Ie!wa5}+k$WS?)Z6L|qrg7qS&`Jm=9z5sNPu}>L`?om
zN?wUJ!Y{^5`^neT!SR`fak9{X`Q1rRS?2Y}sX`JU^J2v0pQ>5PVg5R3)ND<aGHWat
zmnOxbGl}UBk})eo0yNusg)ke*V$8;PW!6@iwI#D9No_4NA0)y|0%Z0inUO4p**}IE
zo8AY?teDK!CoLy4_JbxxT4V{Z$b%7+|HYAuyaY6uuGP^dCu_#56>tr_8|*p#E^mv_
z3oTIr%nBdMyVr_OFg4SGG57`M9~Z)u4F6<yLLd4G<mS8v=D3KB%Z)(CoCk&j4NoCi
zpxxN~f<azzL<;?Dp#9iBee-eYmqFP%-`ohwQ7N2M2YTk*+XZN}LA}NfgL6n(P99wb
zJ&eX49GmhOP--y@yAmd&Ln{)I*B}b3u@!K4<~<(XG&Y1cxb0B=D<O%$!2I{XV*H>o
zPg9TW1xeLKzm7S7V~hZz>xAx@vyZ7Rk50GG-2s_=#C}|L7L$+IYbd{?7c7)W^F(}+
zAsEVXkFU<M$)s}7iN=<w3;6|SSMLc$26M*mjk^?o&*KLT@QYB~U)`jlHh87nK21L@
zQkqgQm^0WJ9_GLlgGNF@Tr$TJ?IM;D%-|Ep1Iq|za2~NdR`s~hKS3-bpuyh(=iH5L
zv*0SR19t9mUZ3W`gq&%U9hj2iL!ICgN}QRKIK+WjIYTi32A`}>or5KHb9f;Y;Cz7%
zbDNJd)w)J3SZe|XChmnAm`lis?OI9wS|DFccv^7MMC^qwt_*(bs&8mwPD9M;$$G_W
zi~7;ZV`~~K7eiQZclB>DR?5Wwaxn0O;y$I=Y$z&2JE5p>3n=`nPd53v!Fo25FrLR1
z76;I`cmU)1xlZ~uHeJsa))skr_i8%t4B}isP7f-S%qqh1L%Ryt|HuFBE8rLc|KmKs
z!+%}kN;2blq7YY-#s3rHN*czap;YBHLGma20kn^-Q;cStt4lLNYu^C*&tUwHtR)^r
z5e7+p{w(!feG;%#sKITBrA7@NPb^jXi_kw!EOlyd{mEz_QmMa6pJDruO0_T{C#g2<
z{F^q2l$`6%bYN!AQl|c{saZKerdn7x*DEv);!mmK2O*66Ee7?Sq0Pg<S7$OYlKR<g
zfOzr4B(A(2h#Lcc38T8R2)U1bK=3ewv%TQof*%shw)KzTKLq2T?O%cCl-O-|?>N)c
zmKB+%7f`%X5AK4BaS#_qQFtBRs1q=E3C7o;Ul<NX(&CEin3~%GqF_Reap*MC;(E`v
zZF1`Ah9ad16g6usv=mE7A#0krx`ravRK6EXsqr_dIL3Vqs%U`YQ>{BNq*T%@DVil_
ztm*<9Fy~3&hi4k*c6Jy1?YPr!26OHXjU`z)Pm3@T(dY{mO4b&d2ECI1w~yWL!nRL#
zg_6xo>mo;IWw(FO#(9nxK1GZD-m!C)J?Ad5duiB<B2SzFVl2X5W;m>z8xD=pcWb%&
z#+9T{-HIif3h%5CyJ1z9`$=lATP=670=oC%t$~fQ>?7)pok7(*q@4G}az4D?<a|`U
zqukdh_sfRGSmp3~A7d@fImfh~xyR1+h}MYK;vVJvdo1U}TZ8%@)fy4*BgQo_djFrb
zXRjYSQ@yhc+ve=cYmf8c6{I6AWJi+jHsWkU5jl7tbX<w0+7hA4JHt@H^~*2VxW*bw
zRryZq7oMqas3Lx$-f^kuTH+4sgDM)JUl@p({O2@FIewv(yp7X2SuGS%H08J<mibGT
zxdh1kA2L5xvshOW<8+$bMdsmKbS#?$Zf2D9{8EkM$h*|UYlp(n%X9Pf4#8`dKw5&1
z&08IUJ(gfCJ3I(5IQv~rGsfaQeHAh7u>&-ZKl~ym@w76D4ZZM?TAEW5;Qx-=<ID3=
zk>2PxsBL&Yfl2EfWdy>3-JrF<*9w$!z{Ql?^bhZ7F{!94lM3V4Jinuk3Y3~S(zOAK
zd3HUcQl2C@#XP*!|LA^hRqpFzxgXyBfcsI67_O!+;&|*1m+8od*Z9A=h)`4QDze_n
zRlxODPb03}^%iG!NyCqWTjw+Mp;_81gwJXP<g>u=2_{^@LpbMIeEV#N7fx+x>f~P~
z<)P~lhZ>nqmSeg|bKs$jjyXM5Krc)sVM6iE?d0zv<N7gFIG>9F=L8)`jf~K}Dnksg
zK>-bLp9)QAS)KtxZ#a2FI~{<96TcC$PowTJcEZGX8<<{($yQ`y+!=0XQ1T==<Zf}R
zvA(*WI7A2CJ&tegZzH~E7H|*27v2rr^>*N%g6|>Taw~8z!G97rnhM-oaF;p2mAF*u
z6UX;$7ZY!!yuN~e0L~3V-Y9uDyMNqX=z76i%u~4n8X&LhP9)?M*TAax=%jX_q&_VW
zb5>vAPXal|4GF${p3HeM#~mbiv|!G0^93&e9wl@2uqgB*u}s#72;NUDv-O(=*S{B7
zrt3om_W?G~q708KJ1HKa*iMQ^DqdUU-`2>~2%k|Lg7r1rZH1Bzg?R3Am6P&>NvT)p
zC1ll3$U41A?IJ7;Ot@CD{RNVm??`A+`;>Zh173O!&)@2BIAAz5F4>2fTa<|d%jdJl
z;gg0HVN}Ps#zuRNM;Dls_>%pF@wKvLJmH9Rl0(wfkW>;9XTf-$L&HTW1Q?p=qS3G1
z#E7~d3%c*D5)dU0Z6p6jhu<MHSF>b{Os>s%KrhpfolgFyn0as}*Pg)<S?tNd5p^q+
z?1AM&=bmds$rGz>9FT(k!toG|)bHlR*VTCZmp9ajCoNP(AMAZH4zD$3D~EUd@cuh8
zEWO3)gY(gZ2A$dsePT>yY+-l@fO9uT`oy@(>=VQ0vrk~_M@W(vuuE8U7ZLl5N&K9U
ztG;5P{qsIT_K5L<J1r!}s&0b7=>pLoCJOvOAp6HHf}55~ZuFl?f+qr-_CC3?Y44>)
zy_*+fQ8y7!bjU`;9h!jEp*+TEF*?GT9Z^b)*A|VECHY;I=@~YGvFV6+3qJmS+Y#po
z?nf*gajxJM#L^M(5&SoB*FFQV`Ls|JJ$F$BZ+Lz|2)mz>9=K^#EBd8JD{~2YBT`2#
zrJs97@G9WmVHBe_{&mIv*Gh@rg6Kl-o_Ob*)=`RNq{%9EKN7J~yctnB_{UtUCD%m{
zXs(8u%Nvhet4*$(kceCj5j9J5JuA5u+FaS1s|RyEYjVA4a?L{2tD5UY$yH&o$|}}e
z8<^`wlj{s5qSc;oo>89erW%sbLK_!FoJ!(B8+M$*IL`srwX|~h)HQr$1IP3GJ1Jq6
zfUAPkZ6zMGVTb8qB}{ktJZJb^fbJP}UWo@Ccr<Pc9By07==+4G(1q<cDRINqH_?B+
z=A>s%5Q{|~l;vjECr<hWHodr#_&?64D>RK-yx64wFBT7XS3ZvmkB$|^bL)KIkjH4~
zpdQr>beL)Ak#t6Uyv9ym_%LvL*?7p7zcM$D@sbtjso@!|pR53>#PF0;i!esLt1{>*
z&run0c%c`x1<E^4?>oTX;ehU%gKqOjH=bi41fy^4LygqC1#^N~_!HbF-FE<*a#z_~
z-Y)Ha6)`?n7c{Wy5<E`*ufxvoaY3{S{xLG^*=E?J_F<c3*^zX_1mythMGP-SJH+xB
zX1(y!>gk(9X=8TJ->q?MXWccPRe)tzX^|I-Qwf~><(;m8pU#d{*x_crGKK!VGHS%U
zhMK(#<4pKMQ(UhCbipq${}sr49(&f2xn6(R$c!_Ct8$!)Uy5<2AzBv4l_3&Uf+!hD
z4BSY_k>qAc-TNRRN0Om}doCm9STRiS2ZFD}SWzJOPr*nYF0jFJ_*|Lhji|~YZ5eQH
z_Kvfmr5OsQ$fYa^4}nnTWK#vVBbGVYG{N(Tm)1txxK;2jVi^ji3vT!@^pc^VL<El^
zmZ4y_q(2TkY9P!Lyjv9ci&zGS1@fuLhL4ax2Zx6QPY3S38H%>RM3!#8RH7e4v<wc#
z7pv{j%#bF7!@EeNn@7|L&9y{w)p%5Mt<+pSm}`m2)f9=yH49OXYOY5m*GQXdv*y~s
zT#uSu_nKV$5VcElt&&`y+Fafxao~%sau#}($>lvpS^jt0a5y+Lnj%g20_+JyU2c)*
zK*!|{Kim*e^&%sP+^#X)WN+W|&u~)iFe#1gY_^=4?O`Qc;qcjM^_^N)-zYqqzMULy
z2g>NX(Mh=i6Nh6MG!Luq#adzX$PXGmddksJo-=JH{@FYPnJr;i+sF81`W@FJ^Vmc@
zGR{pr|8lKfdfw-a3~=&79i4c;w*$Viihz&jhn<vvH9Oq1KhB9yM<1nEgQL&8)rn^u
z-CysvXj&sdBvZ>brQcnS7S@lmr*AYvF`M7pRr`WWI!RwP!4aQmDp;8j#tYr9QQ{a^
zDL6LAxTR{;G4i8W;Y&@6Y?aIoK*#tQi!#S?jNd_xLCLp6x9KT70lj_&a78Y9XUIY8
zWMa9wYi0^F_F4%nH+<ce(g%1RvE2T3d&&Ud_keTc)~{J9`M`Yce*=n6KYNF8OnMw1
zi8FQscdx>vdI&5Zyi=0nFv%&nz{PtD3p$`mBI)}I3szELOaE)3nS0-rVkYJL3(efy
z;$oe9TRgSMc;dTL=!y3~Ne2l(KUc)hSxrbke~;kJg6Zey2|oTQrUTw9@G^mP`||~l
z1Rk}&6SP|(_%UF!WLT)$%_=hP{ecwD{11>`-1}m|E!WWf)<*s%f@czodtWN}72<1Y
zoe$*D{2$^fbk{ot7p{f7ZiqdGH)g=*G7hW~MLtG?83&#aMrS;YB|={xN3j9}-Q<~@
zpPQKm(}TMdPp4c8CbCyN4Ajv8_jMmbOl~R2V)k;YJ%eWL`5|RQUpNU|=s|Gw6ZNcx
zo&z0ERdybdcc;Tgc<dWjhDQ{Z%LBs~)J(pO-IGhAm!!a5pr@4oWw-pjE;r@(Fq8ye
zPq}oamcInlQho`r{A&=C+Z<#u<<D3r<?qnkFrw$X30&wZWG*TPV6cy-(`yxII@)+>
zyc6%g*}^cP5#KOG*v|OR2FUx};Uh-eTDBsh@CZf>qpYoHM9cv`lD5_hMU^?gGF7^i
zR{JPL+}k9vL+{Oq8l$<kORm>!uC1DjU#tvnH@Utwxf&wsam}?`a@AX}vi57P9?Z4d
z<hlWgoH-!sYt8kI<SMbb>Z%1cFxNLG*OMmKK198I7uN5~Fv$u2B>Chu%)w~35scR`
z(~BL9_5BL+vh*v9My-qD<Y&>iMs~ju=i1u3y0*Zr1huNO%YTZ*f_O5oD>%?i&T{Ks
zV=6lhMd2^#Ro@6NC#`P8bK3kQz~(m}G5HNOOI4F2wkbXj8d$%zQspUCJvjH_gY)}(
z!8mzRg{L5Sk0}u!JdCte9^kf%E7@MibrYXiz(E>cS?p4wg!@kzrm+O(sznMg>*|;S
zq*k8>QBr^}Yyl*|0#te)1(>T@$|=BEpg{pvD3vY1_mKi*L<^Aq|5N~Rer>Nb1<0a%
zZCw4L5>@^>s3E100F|GHnB2A?i|PE&8--t1T`&yDZ!S0)i=P)&`1os$wDK$-we?zS
z>t(dHsM@-<$y`iTTUGzks`{H%RRUDC(FUlx@=#Ut5D(inP%W&gSIoAmUKLgKmj6*z
zzxowMO**jd;Ea|P&!*D^Z_z)ZX)OOdlV&_5j|yyilUZVHbMq&+jGAuCf6My5W~JZ#
zUbAw#cR=H*X~jECA3%Kgs#ZpnFL5~gLLJD)D|W08^N0hFLgbL-kaIV%wEYDE9E9wJ
zA4f<YOqi`w-i?*Qsz3BW3X#GWZ}_Irp${dPUU`U=&^(pGGWr*5W2gCR-f0Rf1C(lJ
zYe0IqbonyP&~L0Qkomj)_x$1bHpZ~`N#2gJ&x)Xk?XmGd8@X=@QBQe2%D-Lnvm&2T
z`K-uAPW)yQZ=96BTT@v4J(>gW6NVe9d$|^a0QMgKU#r+`u*X#JaFww#R>uEU!H1KP
zw?uoXbhbdJzy+|zDVG^5a}VZAk&R`B$iyzpENs?Yr3B1$&=M5G&-lZflnd{c22$Jf
zT}<83hs&KR`172c<4w-S7$?fi+oG_1K94wj?lpWWl!$JdHHF1mZ&Rd4qFDP2vG?Q<
zY&QgvJ<MVh+tX<!8T?}zNY@5~sUYaB69zV~3@l^dxm5PePD&%2vVR;bR^+6#F)3Ug
z=FM>8IksZ9?mov?m5;7g?S((t{C4jKt;SG_j2&3B%?7VK7CCFS1@PVGiM}>9_{8TM
zF96FEeOC((z6dN&^cj2+u{_atjild9EKl?q{1C8tX75^)A5Zkjn#95c^UU6Lnwnys
z*|RV+N1oNaz6O;HmSp98k3;F;^)==StedmCSl|MI4SVmvU^EF~t_a)JXoymN0r@=9
zG3csSfw(?2a48{AKn)zUk?R;N*V;?godU5O>mYEz>lN@eAXjf41tW~5Stkjb-MY>a
z-VQpmOuJF=nQz!-T9+DT5xH-iSybjI-apPPDsvSV7n%c|u8OA?7W~>D<?Ju~;^5w~
z+Q5O#z#5$Mm4bg)c@Ekaoct!_Pr{TwI7skA#A}K31)s7B_+Q#>u;3!#f|sTP4-p*5
zEm(x-C4#k6c^f~va-qC|KDM3d?pavn2Upkl`yR|-_;~pyWRC{;c==UtaRvjjm`C&r
z5s&#CU&T4Iy<x@({(`U8IC~x#@>QHCK_;&e?I69pRnfMCA+KSmz2M#&Q%O$wC%vum
zB|!Pvh{^A$S&}tNj4ro<Ms;~xDI>c4U1N>a<#96SD@~u0UM_WhqNCAe7VU6G>dE&)
z4_?Jv_Z^V(s^yLh@G9O>2;_a2Ysimw^|064%9l%>-&QGLBfsF8sZ*?^`Bu_KE9rYm
zs?_P&B&A#(k;K;q7G=lSwp;3MN}_F3HmhwVK-*r6nEa)hr5xK%0FB!Avo40Yl^9G(
ztubHgc?p@*w(pW&n#JomG3487TyDuvdRNJ>LcrX79ah`cvPK%1`UuJ8CTNngI#x2k
z>ezxn-W|tS9W^4=;V-|=C`Et13GCr!<A%yD7;%_Hc>c3a`d*u^XI}e}zRgLmx<K9i
z+HshCVLT|tfbyF|P~Q@4RD!7>XnVcUfCK53hCsF<bDem9x&sDTE-S}k?3m}IOmR~}
z%bgVeLkE0l$m)%A&KhmF{6RITt<jU;ls)-ADXB~%am=%EEN5GI4t*KRm2KwUoRiDM
zb@N!R_!97J0rlPGu<28lO{rzG@~CXWi`8L`6c+8EiRaH&ze5e5FUxwaWqk}9UZA~`
zY~mM|Dd=L$=I9#wldLO2z1CDV!=dGTC%&o1kHrSgV@`aiigtH-z^SnUZspFD9Jn~X
z4Hgr!?6KFa+DE)F&y7nuJC(NQqsudIG}X4#WW*$n#?^<z4!u|4@XHTAe#N45)q&eB
zK35mPInDu71U$mX+Wo;vNh{Uvu!kuJos<SPWd~EbsvBVkXl+t>AA~>1iSO>j&v4>L
zIPq7he__2nr}13npn?9EdTXpsglsC_U#9y1Ln{i+RW?$C&nc*b8oWVxq6V)zVh#GX
z2K}mCO%2l8d8!Z_Qf()`NaN9v{7aqqdrZ96PG=|O37aCdGuTOa%cih)LgSs3&}0Qt
zsD~Bsxw~4Jmht#CVPvFjnM;7nw90igv{jC%c9SWSEStm|mfIrnaMCO`*~7O?Pl`tW
zQl^P|p?WGI46Aw3nX-8SW<nOO&s2h+Jo5|8e*uL!bFnjB-j6}WydbqXmDA;x?*YpT
zPzH}9mKUH5eu`LLpn6y6e<YR{s0_aNeT=VdRPI|Um$TUq<O+%@NR|_VEdrB2z+Pa8
zufTmMH%SC~cE$B?A4#||(z;?}(4hW6{di<FQ+aRT^ksYM+y^jT@|CfdL111iTdWko
z@(X^J`V6Sdt7F3Bd!^V%9$%(zw>)ZWu{<hti<7(&9$%;a8{v_6z0HsQ!$pfZxsP7J
zi-Cgo`hY|B&MZ`eGJ62k#9v_kRf{ppT&mT;w+4Gb@}M7vL7kTJLCF`0<;IPca&5t>
z9|B9Yv`S+&bs^3k1^QNLd>?N%u~gHQlKv)f_0dSbQqn7YgjI}GTWd+@dr3*fNNz33
zxGO40D)A~!P086d1F2VOYG!U09_3^;UL)#U3U;G57J%YfQMUlND_&4EXfBXkJ93>6
zOMPE2!L7_EC;e^H_QJx|@bsJ9Rj@%#)Pqlq4o(}fMCVv7K_GK19|7}@{T3xn5?g|N
zSlUvO^L~KaNY48KRtq_2>JM}`HCfS;_ek4fxo@@H4@S5@spRlt@(cD&tAL6U?x$^2
z?rjk8@<yp5{0=-@0XLm`;vxCp6V4Qm%(HU13bcfF+90?|{7V2&Lw3P`UBEy4)>H9P
zri%PfMF!^QL1Ny6WnRk6k?X}SPUk!Bc%@V>7rP{#Z@35A<<5fD_$p?zbgmf1^vyMh
z)n5w<7Sx;!TeXpJpi99EQ-InTlr3A-XZJ9MN>eq0*QTEY9iqFqa)C&GELluCUftxI
z-eZ(mWWm!hJ%%tpmLw*fTC@t*!UC=US}e!ED<90l?O+$-#;ya{4~X~qp-TDQ*dUf#
zKK7MwLipI%V21G4>k}~W236)EQ$X~;jbIo5sN)7hsL`9dUxlZY)A=>aUj^n7vbUY{
zF;iO*QfNPcA-pErTaq(AVRAtP_Xf^+o4%!wq_eZ#M(bOckh_oFDIb-SCsi;XIpm(@
z?4Cx)wp`8*O&_-$O^??a?+1ZgbX<Qs&M-@n`s*^iXSokz<euf18Q}fKwLis;fxJ2Q
zX63d6^zr77<-d?7cfsEH83Mcub~FNcQ<c)(E89^HG}^fryW(oX&wCr)T8L$#*i__R
zNH)~)MFb8ZiM@Dvx3ZAq`@}wm9|u=-n81I!czL!e+>mZ+So+e3pbzgk{*wCe=HvRG
z^C2Rno{S{9czFb3<l^Oz82A}9Rd=YN9#C$)r6}H9x;zkRVyM>`prO7+An)lIL(%oe
z8Y)W*2EQ&p(I<H)*igg92;|3-#NNbQM_GuWu2yhNhH`FV?nfW;k}90S4U`e-Qyzh#
z3K8Fl`mk#~i9jw>2i^+w5759n5a{{^0t*n>gg_B**{|En)K_!R#!p-DFc|T(32D2)
zC=%cu`Ry2U4g%8<$bVI{HMZ7Z%JU>4?H33*cjUjR#8U2k3T(NL@#_mc`PI$Enm1jg
z#!uZ#h1(!0em7({N8ow{hH_5vh!UeA$}g$8OevU-V~$i!Jrky*pO*>uVfN&mH#SMw
zh5MUftH<S%S#NI(G{#-qZddqNfi39~yTZ*n!^AxJhaRR%axnz0Q}yJa0^^JQ8h{yl
zFzA>|216%ngP@6v12-t5<Ufp#fhOflaJ|q!Y6|9sbNvm@HQdLjFvk1`=UNAj^~c8@
z=Q;#-6z4i#+~H|o62iF}m@5#@)xbLi(z#9$>Q-M_=W1{XuyL*v%Q{yJ6LQ07pyFI7
zi*tPgImEdp^ffx}KzeX?`p1u<LC|%b{xuo|o!M6i^lpe#@^(m)OY9r(K`-FN@v|AA
zbNv|sajvH-w=n3{xo-RhjCzBp&bJ8AXLLm%uc=a6=W2Y$@aV1CaIW?9jBb}p1B7DK
zXFS<Z!^H?3LK5p-n<xwEJJ|}3sYPvwv?z0{eREZK1Bw>BBRy^<^r7qeg!<6AUh^Fc
zm4y@d(JR!sK8qM}t|#wBfX=l80%EB4%B>jm>Rew!niwkUdo|P*2;}9(7>dX0?UKqG
zs#FVB#<}(<8)`Tmf&5sKSm(OLHPjpb#Zazu{R1<RA=XeRTJY}lFk1cKYEYvGzmfVF
zbjjU-lz}GzW&HrV)IuP{z&QwPWZ--RYX1liI}hwil$|#VmU{3#n0@dUi0<(b{<H>B
zkGE{#B?JaChF^M}iNH91P5D91Ur8HLphciZPsFbT!BBpN`4EEQ!Tb_4e!i7`8SZ8N
z7Lw`daM`SXP<g)G2+!l9d>p9{-BU4-jvwLyhoiU!uC?d=7i-UtG)sR$AJaW<gQHt4
zTBsjBf}?vy^B70>haz@}9a@g^9bG{`BTpUO`KRHfIaU2LlLALqZ51}lpK=`C9@u%%
zkcMarrRhuv?*pEIBM!yOiC={w6@NjTg#jpdzt9i-2{<!~p90RI;e(4LofFA5WM^SQ
zZVxmSuFIB)tNRfd<N#=?z#D$n)%Jq|9|kn5ujT1neVy<Ndhup3r~qBNa(?@MIuB$f
z{0fK3=Kb*iQ`Uwhkoj@?t|!qJ*}Ojkfi&^d-@u^<Qge_b&HQS_NHf2i0XFkZ46vC8
zf7fOnH_))1g^ckJKKlf?vYFRGx-|8x7+_PE0Gqnc7&i6W5Rj&Ryz+=`>W_m!n))FG
z(WY+Yv#D=^blcR2C{J2E9R(_{smEOFNkTrFi>B_If{6#7r{I`8aHOf{FVt)ev=84%
zR}$zjl6LnFN$o?|(uZ!P4{hCtb`C9e)p$Kdjo3C{=JO^CBR$URUki@mHMPj`s!Z^c
z^u*Puu0}}v2A1|lkL_=Y%u@OfDEJoBgL6R#1Iqp_-;MSBjZSL&GO2|~^|Y;Vi*%l(
zc4|D<8}8MNzAWUx^7rs-D4(im$S)f{vYN4)u=41w@F@$W<fh~H2&ETlpz*B5(3fff
z0cnn5J-?Qg=LPtr3($iibHgL}q&AvI*mZIQVKKrlk7tj9r`fDx1_g}S1M^3Meb!oM
zzv$FItY`UBJVOq#coKu}AQn$z@FrsMBwq=A#r?qIN(`O}oFn%neVxwzBzhXzZn7{T
zM|PJiOfj2Ddz3iS>>gQ|mAemJTc%*&iRVfC(|Vro#q$gS_N#<U)A<$lo4Q8q2g`Lr
zqdCNYaz(bi`X@o2uH&^8p5H`yu*E%Mc*s7CPPC=nWji#Rl}0WZl9@ce_JE$NG_04=
zOBJw9hgLf&p-){<aZt3kht6>Ts__j}V{?tVJK0uaGgRSsuQ>z8eSe`EE*}YdpOS%L
z!@J}Fuskr3B{=jqusku4m9Y-eKKKWCbus9(GWbCHB?p1G<O}B8i+6Yx@xc9T9QQ&B
zhlMkRP19gSGc24X@Xk0wY(AVVa8?Dv7QpocZv!6Hvmcn&&mhxqMc_BW5&RQzfcqIH
z{W^SgcvRcG2;NEjCTBz@{gO(+RipG{h_{50-XMd+&9A^+Ti{WIh5~;`K=R2Lm4b~#
z{6&etxicbo8F0bh#{oCVFbDkm#+uK7oU3?NVL>0-<h%?TJFaBMSULEP%Qy8^3i||i
zWo&&K(<`2|4(2KAV4kp=1M*5r2Wxu4JsI9J<<M18x*nw4n=uA-a&o&53d+gtNl6Hh
zZwmzSik19m2kCpR;i{9}!M=mXM;PFk6Xf9-*}&d7*c4?4MPHt=fTFnx-UourHu-5o
z3`NSFptv^Uc}uZ0LNQh;nv>$XjGdNZXM|#}QiMryeMa0mm2y&LE2YKFYEn|%kWt4{
zbdFGLPzpY77VMDG+EUCSg*+(`Kh*H(Af*OX&x|#sV%59`Dlz5RRn(ND5y<PJS&nAP
zQLSB5s=WI(bC#9YFXPnpD(@wVVRQJ=%1f;Zc`qEJyh4?yk}e!(N*0k6&X{f`oe#bu
zsW$^`BvUD=i)J}m$zmkE8!Ksa#&1^A!&cHqR?^>;^wa+)X+S|tnQqDG{;XQ>lxkY0
zYZzde!U*I|(=7k7OfP6QOvdCFoSE^ym2`)d^t_d{o05J$MoCkzw(X*4e95x0*s#|c
z%N}k@SIf%0H{*)uv~-n{wRBS$VClX?Aa9IjIa=vr+oHInd79a~3+=u%W4V>pIz?p-
zWq`8gB9QmWG0M6p(hfz+Zq3$^QXa}keqKwo&Pw^(N~uyEQj$j)BOI+nF;ePlwjPx7
zWJZCN(f~ZArROrhvJ_KFm;X)5NX<5jQl88B#7cR<N_odh`IS;0_|H<rCq1m$B5m*0
zj2kwn)iRFL@?60H%hMl$yzR#*X<5W4#k9SYk;d#8x#Sm|n{mPmDs#G(xyH)ePML+8
z<!Ci^$F;V-cS(%9&MJVXClcd^MNvXx+~6omNsPPAX2T1HiE-1SC@V28FG^WAF|M~F
z?MAayuX?s9Z)e<Mi*nEw<?I@2zP1SDtv|-1Y|yT&lK$0fX;#uF8EYX)#&v0QL#>oK
z2$%t<#wcUxHa5?v84X`lLqAJ$4glZSeAQB+U02O=G((pjcI;NU8<fdcR_;w19bQto
zjUYwj-oyYK)db32I{MhUV%vyA`~SIuF%4;$7}qUY`4=R{Wm^<CVgp*#MB7f{Mr;IZ
zA5Gm1I4+vH6>y|NHBs?cQ{EG-iMC}-fr+Bkxx||1bp$+rs?MqS?fQiv<PwYv@wYXe
zi%I`a2mD}&>Wy`t3$x)`#r79-bxwGIPPZDpOc96Q|Khg0v;W0!eb~1hpi#I~SqwWo
zi|4s0joDX-Wl<b9##wcE7JMr}R@=SeOEz;%qXJy#$U%hPQ|B-=)f>XWd>QU_C;kg3
zzNgL)NdK1;|ECjw+$#~2`z1Q|lKw0w{x2u~awoo$4oX~o=V(0l0ctw&H*0(m;wNf6
z`QPgBdCQ4k<iuyFKPLS%PJB-%{yit&x_r`q>%{MN=!36D%R5;;59v>E;$L>+8$0ov
zo%nVpJ~1vu{e|amcap<4nV&n8pWjD)2c=DQxQ=z=AGFy@waa;-nVQUHt-neEm$g3%
zZO3wtdeCa(lFrD`?I8a#^+Ob232Q3tXa28F{OKk>F)kwBzeEp(n5~sfUSzfNZ*h|Q
z9GbkzNuGFUa;o|YGGAnqqq6Hc$uAs|oY&GxMrjh`B5HY|4>ip^S9KQF+1a1h^1K}y
zeTh~|V1DvOJS6>=v!4GEUTM$)E`7aBuIFD*1D5r>m&x_}nd!i?e)lrDexC+>^T+|0
zd%-{^m%=MD1Y;>~aB^lv93fz;!?k>zupEpivs$kxxYhB*e3Cz2@RPu!Hu2!5lHjf<
z0B_}qSAyWR#976_iK4>QCj#%K%1M$w>Le_$<$9(nnH!)IFL5?c`zI^bd!ze|Gw!Iz
z-N>7fdFtyJ%D4t?dU8d0b3U0pPAM`>Bld#nnT4d{liA%t6%Fvo?0XTDADboSsdv)o
z0{0tbK8xb(WR_a-??%K23XOsh@wGGmvEu)-;w3=wSv4U(HcO0n(x~|Bl=%jVZ=89`
z8(R9#B<Y<p2m5(%qH&C);hRa_e}EKuKk*UG+8jhk8^^5wK!eKo0%0UzzTiWWL!OK2
z-?}OG2$jik*0wD&pCY49NNYt#HZV93F@w9|kLSOsnR?g6g&?4g>{y!TJ$SBT9<{|2
zuz7jK#(X3`5+R<lfR;Tr+0wR>(hh_UQrd(f!)60Z+bYwCRiu#j+tM;%N=uSMO52a6
zZJ<oNxnR;dGZDh1fWO<4G7v3kezEF)mRd*3Z_;U(!Tb}Nq(pR%c@HVDyY)wW)?i3v
zo7_K3HM{9jw0x*_BunKrRv{!l5}_xBo}#Q9vd|qeli+Bi(C1}o^-6%%JD)ME;*3+#
zh&QMdTX;$7hm=?YyrP6+fNvH2M*~P>+@YG*vW>A*=Ecxf%xwNRjj)M!T)vKw3n~}_
zuOpbZ32=p0UP~uk()3jXT%QF;Jmq9`)$yiDJyOZY3(TJjKHIlAj(9qXYpyi<M=pIk
za2oIrqhQE?D$^0?*Q)4^547ozh=;1=Pc-~`hhgcr!G_-UpyXQ$I=+2n(5%`(eEZ74
z*9iIcm4Som&=ceBE3XQD{|bS8-6|mXOM~I4gMwR}PI7L$#tFVnFs|RLAh6aM@XOsW
zBMK%|*$e%0fz4$Z=~awhwr^n_uXz8st|JG4-APqAl$0=k`efukS#W$^h?AkFrr`d-
z<{qt61i!&_c`GYR@P%h0T^`juP2}GNT<~f!_|>k$U17fZQ6S?=9nlw8lZ-34astv%
zSG;$e8E4L@!lCZQv+TIiT=m6UVe+0<uPTxEv>wC4XT)Sv8>MXag;kz?2hE;)fz8fF
zBj?g=3Ikjsy?{X8tSN@vF3mWJ#zWere1?VoOoG{oan0<s!z=<Psgx|!gz$mWDram~
zNmc8qqzf6Kq;3f0EzvATlk|9GO%o1lOT8GZ+2M!72^VS{uUqXBiKo&-k-AJ%**iv?
zR9q;)hr{u*4d2#z!6CTU5^PoizOJ)N3ApxoLizL4pualteys_{Y9y-mt|)gw$x7$;
z4BV#|h0e;T=x~=iUFmS^Y`8VHZ`<%nH5)z}pFD@peaa_#6;$Fu8$Jr3@OtI*lJLOG
z5l_my(zp^-EXnUH;=2^u>v^s7J|j$)Pk6~$-ydP~!IeyWeRYfybG4H2o~cZ(N~SvT
zSsG8~bDa2#OuW20gb%bkDIIMJ1bY5zCneveP<rSYO_6E*7Yg{Q%o-s>bMI2lBU>_$
zf4qvF$De*nwA$Y>BE%N0HJ+{R2aPumwU^-it?)=qPJ5pvckL++&%71oh1V%rfgzK}
z;$d?`6Z_BzR=jxa0mU;5)Y!!{HmA(HR`S{dn<5TuCmHSe+R7S{q4&nf%DH{q@ReYu
zi*CajT&M+j&^>wnT@Hr>E{BTJ+GPdJmpX9L0=29HQ6788d7+2Y4M=5dR=~<=^?_I?
zn9vTDVN~KReegYH-VnJxQX5}sW=;n}lG&8UI4;SHiC8F0J)8)<LIH>A)hdt=Bp&uE
zY^nVxth~@&E~9k{sQJNT)4a%X^550WoEx5w0n_+4>vAKS|KJL&G1b%_H6JW_&~9^%
zq<Q6tyylv>(V2J>&yfcXU4?^}cb}5%GiyE5l_NdBV#x+^0cLuuZ}Fjm*}c5bCe14g
zzU+pWVr<r!;YntE2+ThKvBN%drWkWDCYUM4R{2oekaO%5V`~zp7`usE4aFd~O};$W
zvOe%vT$p_<_|yhi+ik`m$JxfG!s1^<akgR5szyMZZ5a4|V-C=mUR0Ht$)F~LoL*ED
zyi0HkjQ`1k>z&JVOeazVE&&`Bm;vd>3BLY3JDo_=TY1+Ob?r%ZnR2b>Ye*kH0@;r@
zm*jTs+S?0WBsXmSgCxF~V^FARMZ9rPV4=+Ucd7N--UE3%lJY*nz{(ZQC!mc6cmqq}
z`7pvPkVWVH^3}_q5Ra8sEA))KX{Oz5AhTUbZ;@FC#9wkjMQ^|?aCHePT-)P5L@_NA
z82KP#u#86jflna#BN7L$N8sWMDFoMwBBk?ItY5C;GWgddPPd*%n*W)K<{Cc*1Z?Kw
z7VYgD?JAc1O4k%>f16QKrqUx6=v4YBtvn%eVQ0Sq7kFNi8HQ8VJm_*o<%Qc&+!2UB
z@gfxI83g(;uo8jw3@k<<^<o6(A~1u283<Il1c71%CL)kaRs&xF`WtBAI|wwsw4&Fa
zk4Z03X*FmF{1RpaQYn$oNzVe2JSR<V@|?7#Mh})>l6g*=>AEbQrA1!}kqLJPnvxec
z1%3rDudoxiu2qU1q?i}z(H(KkF2nR{>ugi-y%=KgBF*`@_Z*v;_-q9Jo>&Id#6>es
zk_&2{M#cK?1Jm@Gh96vp{DN&Ndmlrcf01@+GpvSAyb$PW{sHQ783Gd-XpF#822Mv{
z9|P49xTrY-l@PcI0VANz?M6TYE1+%Vo2-ChP(=kiKtut;{1dVw^Exe*`7j2^d=3Jh
zG=o2sdCI+L2Hh$z{RElW44xq=n?dH~2(TGsGr(prmjO0|w;5nFsBi@WYzD&-$R#Uj
z1|I-PGx!4mX$IYAnW7|XQP2!dq(nA@PA$QtxpI?cV5wu80i|g(*rEJ-KqQ*MrR2qC
zumHSFGe{^g5@(S@o53EW=KY`vk!G-U9P4@y|DIUlf|{6FI7u#Wn!#dK<s4f}y((AQ
z4t3ZJzG~&xQsR{;C~K)618fFk8DLGl$^e@|N^1mIQ%w-?LIHIUGBlsAfZe_=M94Y>
zk294&H1AB(iCMVet;QUEB%j=->37yRPRb@~Jm)1?2=FTDv6{>&{97jZ+~_^VV?7&o
zCd)9Ndo_){zPr@nl>Di<xq7@yrb+Fw9{)cohwu*N&_Z%!f@m%$GT+2F<W^*^g3{FS
za6;Y1$duL58tt`=AENPNLH~iq$HP<2H}P@fINys3v%Ywzl1in-JHGf!qg^oJnj=+8
z*!ts8n&uTo7ded1!8`zxkNho@?`-9U?$f?3gTqS-B7;NcBPp}nGQkMNX=2YQXj^xf
zw(<?y1%HA0bD&S**J#u}NIJ8L4nyFKs}LwgU<v~>5crybxd^mwgXhFHpzHacV?bGg
zm_94`=V|_V8Gj}O-oan|_X+>&^EKmt=AVE0C+RZ$N#mbW`R5$|xsZQa@lQMcxe0$l
zo$fN4O2J1f(1IV?cuT9<Ew<c!P*art21-jYdR?srTfqQ}`ZohCe4A_F=$K<53-uY$
zKo;!PYY|}Kwp@<@3t0UI`alFOVt}Q*i2)XLIRh+gQd<OA;N}dl(4R2Cg4b<_01ID$
zKwlbwUtH=-BgB<FQq-4*;Ldqp8Y8ac$)dhA2s^G!khW+QX3d9pf4bV#9Vj9vNbiDM
zG{6bc)b=P)50J%7kWxC(+4i1=l(k==xcOjz6XFJrMBokvCLr)S0y3O@bGNC94HUS$
z+HX4{ki*GA5Xf+HUPlBtoD?v?;bb)f98PL<LV&}`5C%A$>_Q-ytYkQ8(HYV>tLTG(
z3@2mf7-`O|Vjv}QIC&97GMtc`3@4Tv3laJ0tb*w}oUEN|_@yk6;baDRaX3l3(GDj&
zm7+N*bU5jR)V%OQlQlA&l*-gW<_ss6xS%GUvg2@~NiJ}PligahhN^6EPqoglzYHfG
zvQae7Duy$_nZ+XrL<jzN7|r42_$~-=IO)#-hm+|Dc;h&2>8mBFX9av;O_)DMQZj#+
z0Wz<Yg8;{?vl!rbbsYl~FoyvO*vS9|RLGU~5{7-g{t^{;J6gvXi0eBKfxt}z7vsO>
zz`a+3V$&|@@wg=PeU|@q^E2idJ94y1L^7wsh43F2I`OAE@#{369}c@(<IUEby!~Xh
z=Ez{P64K%KQbH`6{7;?u(GKHpo%q=rKMua;M~xR}-EMxIn2$`?L^J0NbmGlq9xaUT
zE3a5!GVDO<gUynU-;F9|hfM4W`$YomknItZn|0qI9r9kpgBho|8TT1xZ9x^hA^9pY
z<1}|SNJ2k44bt<o?=hq_YY)(=QlZ{XeE53>ZwmqTr!XdC(9r)6`^hO;rX2DuF!1M&
z2=)v3G^t!(!z&j3zf=It^;J|0|NDER+Wo8Xytd?Z$BgM{P1Krowot8@hk4}1CO&TL
zjyO()#_~&DLS#){AKsx$Br|ESp%(f?Sx7Z{YDf+l_X}yq0563p_u~gwChM>f>a1C1
z=o#bip`e6bM`F^&GV#KXsGmNddX7aQt?%A~;rE2Er0{WL<@`dPHox>vV>8a$E;h+p
zQ95t?ND11Z{5RSBI+NKg`&`X*q~wW4v7ylIB<x7?nGP?sz!_Qn7oCxHz0jkGJ<2Jf
z7y91Ko2pez6YsZqZzwOX-$BXw{fLV(<6zB8w`P@KHOvB_qB-J0RSt#RgTm0rdNJPv
zen?r|hep!Vuqe|=;>NDTmgmz-wen<?_8kSRO#HGd)x?-wYV16(lEh&av;fQw{s5ys
zm}Tw>IN>I{L$h73oqwtu@Y93PDYsW$hurPDM-G7|SF5|o@atWQUi%Z+a9idx$v3?R
zHaxfyW8k))+*!d!%;%C?)QgY@@H+&5BA6R7I|Wzo&2;Rfd?D~@Ky$A1m3(8ORUf+%
z^J~=&O*lue=Q@8Vwzup3rMS4zob&u6->eve{N|kJU%@GTAx_SD4hk+JmUA7i8qbA(
z2R7$8!D{9l2j?(yjuTgngIe?)r-EX8j#IH3&vDlDv*$vUs&S)aVIKG`8cDZVt;{*j
z*2Tu4`{)I8s!jR^UXbTFz52tY@*L+?2A)G8Z2$)9hD!~(JzbI4n2&yr^OZ_LmCG+U
zu-Yz2ig~a<FMi3fad*ycrfCEF1MIAWo2zk4Z-Lg|+IaO{{yI%%*FVpsmbqx{11G(+
zN!Q(RxCS2GeD4s9umo7ML4dhwE%|;^3=S8!t9+jA)O6zg4i4C3h??4G+?v$JMrgc6
z?!pcyBM#K1VQ9R=%df6ogeN97zbjjMdJH*kqjx$_2|1qlEgjIx<kCZxc23Go%8!RC
z-JJLWjVIgQ8ZTqRbOqeMeolzs^p1E8GQ+MZ{LPw|X8bTJIDF~@#=K4TvTzt^#r8)$
z!q6w|VxIm(cKADc29W?4>G^Y&IZrP}hyZ<sRko_z#|@R!3q5@5LcYQR)rEkbUnFU3
zCy(`Ic+d%BeC2tAWnslbj|dYUU{uz=AZBc&fM$&UUW%;;7%b9ZFJAK_c5JPgxiURR
zGMggB9AHdx1wN>N1I}<2NKxev>bF^Wq4U&7h~~KpsQHFt(>&L5@`q?<9$<W3zUFti
z0-sSp%^PBXkeS304=}#5pB(Z3)x12w$dSCA64<(uvg1x7AF%+ZN8xr4nl`w`NccZ(
zgL%u9xjiJ&yW{kDv?4Ayl;N8_IeIvUuIGi`*PLN=884fo@(zvJSj~9e!2Fs&;?dFr
z&H(%r9J@IpStri~%pYh6;B{3v04MvvazwITo)H*KEJq~I3N8q7uD#HV#T$ghm_ZoM
za71FzfWbf<nGI|_gkv2>=AVT2Re?Oy_*w8-H#3<d^Dlz$5sZ=fSAiD|wMQhs3x1rq
z#|(^K2LwMh%#PFn9f{W#4aao4f{e3)n|MN!ET30;7nD39F=$4Co{$(Q>y2O3c%>*@
z@TDr#_IYNRF=%*2C&2Z_X(M2R9UzO@0JsbBSZ|zzuGedsX)-CLVD@U2AIa=u#P0x!
zoRGK#iPy-%2-6|~Jt1L!IU$)i5-WP1ki3Hce!ZI`W#muxgybW6Af&@6o{*?$?mjgJ
zL1aE?HsfSYxL;04a#k26*?04b6tF8Dy&HXJ&jG!DVJLO(a>FSMrBF2wKx~g<#IJxt
zk$~Cu_*Y3wNm`@o-k(9rZI51~LCRh9=NRC&NBuDfaNFZP2Dt6fa4Z7c_SnP#w>_>H
zhpHee+4fiuB-<W8BOu!z-5xTEVPY>o?4tLiL~eVuEd&$pqLZ6!dsu3?FZt=V2h(-i
zW0qQ};r+7hF_FBu?XelW%r5%VO3{N9y6tgd5oS|gJZV&kY<m>T-iB;yoQU-!H#!Wc
zX|~oCl>yGS#}+NxS*mRC_^Q|a0(H2HetfaJi+%}^>`HWJfV&bC7~rnNCI+}Gkv<*)
z?n+#U05-9p{nS4&91I&2_JVaycgmH0tAdA3IkFa^0vl9a@~c*0S8`wl{=@()uw()P
ztiVPS5nu(jyafSP;Nz1JU<KBiOn(AaQi0=vqypC<(2u8KR$4JgP<ba%8Y?et3K+2R
z$Z5c(r~*s55|mnTOw)?1`MA+~2l?C`xP*LIac_Z-skqikk@7&a;xebA;-;=X_KGuy
zAzG&{a4IhS5yLN1apzTy|4l0{eVSWw7XnGeb!C7RSHu7-?sW!OaW!s5fECvi0k0qT
zt9q({<|s+<f~vWec^6PbPrw#2BloICPDeDE3*+H4fXG;w-UDR%?eogCm`pFNy7+fc
zu-~o+g*pA2i2$p8Dg&(c{S2_$hunq$tNjZGSnVBeN3|D&l~nsGAgT5}2uQy@`%z;f
zr{5k=iLCl7XMqW;p4_D0TI!g7%XHgsR~mlaVyXU%$&1zhDtKwXRf>kB(0*IJ1l523
zQzmPq-@4n7Wi}{l;#nqVyV3<t^)J<;)wBKfimDaS)1==f&vt95F_6?yCk9wUBN<@z
zzsLY<smdJ)u=*PyU=Bggeat8v*$-`9wU-sp9#qjLP(Z{cAk62I6`6m*0GZdjQ<>j@
zfahPWJ)d**(cs9Dg^PgYDv)anqe44tOk2jRs$W_aZU(QY3U?7jRCtR5sY1qGszN^o
zSiWfpn3njMcGMoIso;%O>;DP;*%BWiDIHMK-3YKH_F{l7@fil#6659|z?RsP0k*{V
z5y&MgX^D;IYD?^hfV9LDR~^z4AJ`9xY>BHtBrTELq$OJFn3l+NZHXP8F#J}UmiP*J
zu_dP80}?roAFC8QoR-)Vsd;I!BZxU)6*qIDp1c}R6Uz&>L``ym(-K2kwEDJ|a;x4>
zV{!!PKF_VC2|&^kmoUJV_!|SPsb=>gz{WC-0oK$b2za4tIt`J9K@$a>Tx|VQCm7w1
zw8D7VPB6lCb;7V$^2rwr{azYZr8;J;e@RQk(u!NKID3@GKl~yk;mW^(A;F#?r#9xS
zcCn_jG50m;S;IX(8d>JkkY&H`=0D{$^>HQ__I0r}uJPwWq2-5D$eDJ1tF&U7WM|s-
zvt-1YIrgr(G7qcuvn!a;$(mOfUG6Zt2s3^Z^vLIVZ(9q89@BOpeBM{Us+wr{1QWU+
zsj6O!F#6hIlvhQYab!Yy#8UlhnP4fy72rzsx}R4yjWaO+8z}Q+#Xxi~@g1}?vy_>T
zOkelJe7kg+nEXl*EBXb%_fLV7xJB?s#P>}Ao+P;YedzfqOUzPcs<1d~Ayx@k${2J;
zDG--327XJ(XUh%DzTfV;O-tsk+Y`VA@@2_eg%>{?Tp(YboG#+xN){It44ee<Gm^Q|
z;udUy`BJ5(KT;$sr3I4xxJ8gA50l*|c&K2mlokrE_5jndQYsZV3(!1Fwn*@&z~<TX
zr;^PcCO-K#hlPJe(*4EyGRSLc=wcWmSdv`)4;%nbSI$|2?i>kl*^|5!_Syilm}O7L
z2P?w*z4syICJ&znlsA=LRjGJ{O{S2qCv&IsR*-T{wmF&moPQEwO=i$F%UB>hwPxS~
zK$&m8zt%X+hD)K~2gxTNgo1SU?}I?x{YlFapt~Q&0Nwp22I%f@dk6u#`@Ia%-CzAM
z3PD!l?w0|{#N$l_#NE$$+DJQ2eIn+Zv;T!ey8AO90TWI<$W7e6rAA+opUyj(uI_%r
zbB13rL}K3gBze)@&jc^y?)NCgN>b>=V=GegK6}YzjZ8e;qtTdi(z)(AsS6x;U;7y~
zq$(TSnp{X@(%pafD2m3T%s&wrEKgu@P;T%TqUrA2GeCDgp8>l2uNa`aPh5eL$m3lP
zs({GM=CkBoRzN*aiGUjzpnypXP{=X{DCA8BC?stq0u<7n0ScLbfaj0bra+$_L{sAg
z#CiPYZcV03uW6FwxzE<5wikXy)2@<uy-im#<DLJt94xt62UnhVP)ott3<V(0>%9Y&
zlUpI+bsgOp7t2WNT>_OxrQZTgz8Aa?xOc+`aYITFbXSe`f`3Sxud@n|Z368V6zJN!
z7&Kb~pt%_v74p5s@g%vqKe|Fi^Zmsf|FQL^vRIZqrYy3F==mF*lpxqZM@^aitjUvu
z9j$XsitJ)x*K{R!taT62>?Ez_^X)Hc4wm&=&B2Y)T~2%-jfWjV2c7s(b#1uJ97<Op
zV@S^PMiILHXCylYwYKGqAzrBJ21724&Ub+4Po}5Dk7oE@72)ps8Ye#7PAzgh43A+C
zpQe}6>i>s7jOFI6^KLkr8joS+!F2h=-EieO!SIYcKBY_#d4P&myU$U5p5!yLl{HTM
zDvf7LJMD#N;n>o^!yJ9bzi1LRprr(xr=-ROj4OT`?L>UC1n83!o`5^^mL1}g+aVsz
zc=Y|7GHVE`VB3@{$c#td^FR_B^0JXcALIGAs6ssD><c<7W%iueX7~+`14_Z;<mrYY
zZn!y4=EkKw@bCFn|E`=3DIeL_l;y(Ns*p65whGu(eiG87oF`wE!kXeYM#gB?1oTUp
z`0)uhFSJCXdF~i`$&r<}$B~5$M>%uOwt~FS0UaYn<E%BtWYqWpBl^e_t$Xa@b-2G~
zT_f4dyGKuIToG*bk%_N>Egw#0X)S-7rqk!`x9QTrF_Fc~d7(CL&&yXp<yv5}B*pAm
zPS2Bf5oTNM{D(C!50b}AUVWE?d(Z87`oU9V9v|Zrd9gV~F4a@yWl-ci?e(-$-V+LV
zq>N^yjYLaknh9olGfu)Y&DsR-fy(>^=D!c&_t$V{dYjQy2W5>w`;AZJZTs0z0>3m9
zxKbkT+s|GN{8eAzghbB#PI?NxqIj8^*;P*D<nYxsoLyiax{AQ}o*~3!s;a;i>)^{L
z%mRaI!eAk>?1HxveC>K*K1&e1R`4F;D<=bAm&lpS{AYDG^_&)F2ZRSx6F-jEp4<01
z?YR<Q&wT|k`Ifwbl>CrheE##&i}^InJIXj^xiCJXk}_Tn5^)I<Amd*VlS{IgX&U3z
zzg+o>X~NCP?3Bb}%dFW3WhMbKyB{(6mb}v8wXE?%gmI2C4jaaAS;mt<B3ep-jNe5}
zF3DoFWIU!5Eb@9~mZV~XXC|JNpv6AvMP(=fGHiyJ{Malp%t-?i@J09?%Dj}CHcA{p
z#(W9BfQ%ynzJ>l8Vsdv-Q%1|59*nQ^Ql!}5D6^D@z^rlN%T~)UNTk>jpq5J!lS{G~
z%^0th)BIJf95TBpv3jBw`6tUv0%SJeWiV@gC^N>Z4Ld6{JDm-7NzAtlUjvzFCjl}X
z^9pPjn<Yk5(x|3`m3f$&c1c`k8Se#&FqQxrU-K$!`%q08ugsoPW~HXs2}xS)J3%7M
zBtT|wkQvEhG-JFn+xVKX;bt-$nApiO^IlVC5+Jhz#N?7Jh8g44hMy@jIAHk&`z5Zj
z4EI=u5+K7#8)3uPEHTVUqniGt%)N)9X}`pSmhpa&NU<eA#+_e>rX-7D#&~6R@kUeZ
zhGaG{@!HC2!-XIbW)dK?Eo4Tr7-oz|vDrhuQD#X}XfPa|xSR~x3o5?>hLHe!Lqo*m
z$7YG?F{DwO{-w;V;{)@5t+xHhoGVWT%ok2#$)j3Jx@z3_byNIItMRRgxm8r->10Zc
zB_RD8G5N7sVl;jYG^+6}%G^4^;H`;k$()L&yr~*XfcCu>G5N7sqFT!fRipuPe#hY%
zWqzI2{O-hoRl%I!aF{^mkpMr?u#7S1Ah3y2wrbu6npaYOBnfGc+XNdjxBLlxs>Cuz
z2j4LE;$#66g}@OnSm>qGF12+d%^PVQrHQXmW16@rWJUwDbr>=Eu~}klO&YcJ<;pzL
zI!Y63S5sSWCUY8H0<`ssZ-IGimKa-;Mmx#%%De|VNonE`%Xkb(gs}w3_;JMKk}QT9
z<CWP+Wj2e<mLxuBnN@#VnMr`mS|cWxWHHPbugttRO?_-2vlWSzleNgREi(y_*$MA}
z8OdUpF<zO)E3<uMwkk2(GV2WzDW(L->=iO2SqwAAYi*yP%rHllAGBLdhOF&so53&=
zU~ONGnEco*F||z^)iht3XFUQ<*CYm0wAjnYm^Cc{YWf*sa!D4WDdV;2e80)qFwH3H
z1ustQZW*S&s|+PThSwq{KQ>DYbJD1$mESVV+fvg@5|@GTjZ`pjll~|DdKoh0k|c%^
z<FO>!sxvuRO#GvOU2qrJu^V1k&U775TfJiBOV&9$VDd?y%<5*bW*1rHfJr-^74rPA
z9J=P^(}ngcoiIOHhb8JYRx)9^bXIwa`Nyj}=c6Wz%a`GIcjz{iPZt`ZbfU|t>a)0@
zt9;xc1;tkzQPPSfbLHD!*;pBd&T_n*zf1uycW4@u-SY=%2j=60H@c}6v3<)o+?Of=
zrB6PTAXfCVN-mlY(3VZnkGpxXs>bIXG?lCEPh(PJ+1{tL!uA&@<4IU2ahdMa{9z2@
zX|G`R&{+=fZ&1LB?jyX)zk$pP6+2l&ce>zV2jtb%e!|lbDu?1@k=Er@OcOL^w`s^&
z>}`P1ewof<?<aJVLGo#}FVeW!TmK%gd|d5|G%og@ejnYd7~P9c1Mik^*gfz8R&&^x
zH>mj*UFv-)i8BOZsrQw@J%F;jctKT3q;mKLPfnkfs;;Nyhw3RLKu<9MG5N7sVmdf!
z)D2a6+Y}J1U-<=3P5;U=Uuc<2Ks+#+$7YFP{)#a7r>fTcRAEw@Tlex3hdl3oztZru
zq{(4v)HSqHEys7q6g@)dg^M(Ps>F4qD>=-DSL_Cw`Qn2AhGy=Shv$DHOzGJ8Ji5Lo
zfLr{POj>l(JH}jWh5L=rV8Yo^a%?%oT-nelUFRcy6V1B_jV(RhSj5~V&po*|nmP=t
z^rj{iyQM|4y@E~{Zm;QAYtEs_(@W!C0mohuj-EeE<L7|aScexj&NQWd7tg!X;j~cW
zSrgAj;<+hfYl}63*F9D!;g20LH;)i{q4gRs9e0ZZ{J#{?WM^ft*yVRz+35K;O)VBm
z+w2S@7bzg0p@z><On<sHeW;boXrRMr{V_30k1#558O?JT?LQhLe8Jes8+y?NAG+XM
z2jnHc8*Oi_jU%_U%2rP}v5r}um!-Me7$$IHli<KQoa{KU1L&>hs{teBtE!)W1T41>
zj!fm&_4cj6tBc`vMy7Jd>gR1ZJlF@P)(Tu?pK7$^d*@^ODF3)r^C*95VQ$u=c<fK^
zfm)Kf7tF?dg54+H(=<^PYYT$lL#byRABcVG-rTfwr_x2%GS8%TD35l5(xTk*3$9OH
z30iq?;9Cfm_Xg6pBk&wFxEO)FXElp`Z-8wbHZHZZ5(F`!qQbS_GbSvi!qZdJPf*6)
z$d4~1+-e!GAmirm8*<Co40>f4i>HXWSgMgq$ucGIf^$+QTS@O(Nhf@&Md*k?UOUZl
zG)ea51oSZ3zL%@2*Q=Tld5@&-vhr@F7?$iQEAJD^+a6Q0!<sDgq8e6w<{kxn*!r9k
z#Q_8p@Kp|eJwyEap-%JJ$k&}cEg$C|xsqA;dgaw2e%M?b6IgIQ)mf=U?pOtsyTr<E
zN4X{wpLdUO)^j6r@6*hjw7%xZwMSE4=oMGiw+iT-;!e^s@+*3Wd)>?n{pRL9K~2iM
z&1~Kd<>qar<Wfdo0n0ee<~?#5XGF?4MKg0U{J3N;V_P#ZhgI)%B!+f6z_0XyY3CH5
z0EZym=g3w2Xhi%enwb;Urjogg_?W6aPpMgK%nb)94D&oF`>zu)LTb#-I+Y2`zZpyy
z)N`EOzi7ecgVhz{uecaTKA!>KQy*QrqToiK1IxE+41StezI+@n=`D5u%eRgVejPZc
z`MJnnDV~D(<(n>8W&xTH;Uwft93YT8`@xcw-YelhZU^P4%SR$2DW37|cIq^+vcOrp
zbjQDnz#6-;gD*a`T0DK|EyT~BgWSi*b4P#Km%!JZ37i$rCl?xg1)NkH_*B796E7t`
zP4G)!0|!Z8OYmEJfbSjwTt{%rZ-D*5z^4mdN1QkW_zc0Xd<*>gG~l{|&;1T~8c!I`
z6#Nu$!O$~+&yMGS5m)l}xPo_TIT&~U$!BWLk@Cfr+)`K|15W*T^Rx-xDU$)Gf##o4
zI7&WK(@4r&XRjS-&W$hg&RFdaO<UYS7o8pd!YS&ae?SItWJ|wS$9e7#aAbo&GUVzw
zE0jz%M_GqD>D8^x_6}VqW?g1vy8mO7{cJ?=7yLE8QI<-~{81&gWPqd100i<jX%_3M
zd0-hc+K5M5A0sc6vc<?c57p~mqJRg@ogvX2ed2V@iT8&(V4)#8eJGw88;+YjoHw<`
zxxn7&G1JMrCMv+6=fuC`#6Rf7f9=FS>BJv&;y-laGfp)HfLTL3op`^g*3L{c)Ep&}
zxw3g5DS&4o<e5Tv$%K;aI8Ghzo#usxX)bY%^A*rHEfp3pc2i`~sy^)dNnPaoP@L!n
zw;H35AA}b|Pcv$&2D9|kei{F7ug@&totL@9=3+3;U@PF`LGu0;hpOO+6h6d(uOZ;A
z9Er{{hxm2PQ6uig2R$Ek=%s@1M;{zlRxtV5>*3^)5C|nRG)xSo5CLAo8$ybqOmIY2
zg_3;|12Ox@g8Tc*edrv|`{loJM}Z9Y1j`-r40p%_cgkaT#{YHWaQPqpZx~lHqtNp!
zZZpPQv<SD~)H<v&#dt7gDUFD7iE&{}JP_jfmuY^kzpshp_x#x!pS2c!EE3Od#TK<t
zQ>8h)tbn8JDj`L4=vU4t3ya1jeQeu8=x@!-=RW+(+Kmae8JTL^6P%P^Yzj=~`DZ#Q
zRclN6#fF~W)JdshQc#zk-^z(^W^<sfJ-@w^a--(Jfv?})i65--Tv+Bi@zX3Pv?Oej
zXbSyG=v@V3tUnagLEqUSd0?z&Y<uP@i^EKvev4SE;wPrEh0|FIX!bMfu;k!$-jU3n
zW*PYdG%wehEhMjbjC-cWOLZ)EKz~DIsw1?{N%7xv!0na{eZ=#>c2XWxDX4+azfQbg
zW4kF6MQ&0e8ojO)zgy#B^l%%E->-3eK%%S0(Q~JqZaM*PL>TMD*Nw)9@74HAG!8sM
zKWLm3z0#*qrFt78Eqa!d;$P^1TP>Lsy{(h7NTsmo1Dtrj*a5F85sN;@iT_;V+1^$<
z@qb!z(%wGQ6lrfiE09)_@tG+JTgeS)Xe&AYNUg-Ttt51r<`t9N=z!tE2+y3+@rA-p
zia*T(3oMzKq|`}SZBp1DLr**L{$>Y!tVA@)*G~M;8c&mWpGPaIN?q;#Vv^G}MND$J
z0x?OR0-B_k<Uz-|;7BIHQ|0D?qtI=RJpVxlOtZqpBpaL*f13kVS~4-oJ||_PNfDDI
z?ueGk&vL-`N<@>K@5EO;(=>XTq^%Qwiiwx;(hK1!Etye?NoFe$ldMrdlN3lEn54Wz
z2yx-FBPRLGWfZs5h~U)VIm_sxBiVMLW#pf!c{${Kr3Es(cwALtUNV)>XnLXRl#a#l
z2Pj}0t$LO=01Ta;KhjA#!=|uy{3%XK3!5UN$=yy$u1%4udca8;YEoo0S>?pfv^iun
zdEQA`qB&S^o1FM(H6HaA`qGIH#ed<nfKwdsla(Ru>;fmnZ|8u_v(*%$e19jUp-GW;
zHr|Q%=Q^O15>ek3PW&K^XFJ>I#80u}r0e~lDbmhP+hs~3?X0x|wzE$q58BzqM`|9g
z+U61J;xa097@dw)pA3FSoO3LSFq-NzdemVw*fP5GNE*Fl8Rc!&ynKD!oN<U6mq>2Z
z=ogwE_OX(#+7heT>=q~D_DYRjYz%|F!xtcUdP8S9y$GusT~1fxa(ebYV6&VSobeN|
zSxyUHNNkqVg3tIF*es_7uK?ENbR`OkE~hJ%T~1fh<#cY=W9D=vNn+0b1xwL@kHwh5
z6M<!mva%$<#bjAYr&i)hI__8CNnF{cOZq(G#Qwk;hTU(#GjTW&JiZcF+FgFf%JWCk
zpCFj8=M;P#0&ZG~EAK$pel;(^)mnOF6oziW<VydX6YzS}L;T}^;LycipsC!1?NHzN
zL$f=`%`mJ@in-;FdGJ*Jk9qJc{*QU^T>Q@`e`OJYD;?JIfksTgQ+W2bTe)&-ahKs*
zZ@BZBDjRmzelhP0E&tNgGxge4MlU{qe9U@b-d+UXwO6^(-yKkH?2Yxmib`qOfTcfX
z&U~yhQs7W~WhDJ|gaXc1X9`#J*P+tB%h_LSP)UbMf1RVzR>^CRDF0Krd4BzqvSOk4
zDJ#0nw69H}Mc0cIP}d95#-%R~JCd$BR@c12nwNDHJxcvg?FQSC5hL2(zAD4>N|p6Q
zxIF#>^H)H*Wlf#le*b>-){*FW!7l>r?cSfj(%W|l?m;ZQ-Qf3#rMK^v^p<}COK&&$
zbzsxmzYI`Nq_=+=(B7^e*%%q@_k*!u?n>?Ndth(u;U7Q;EglAq3|{!R_V;fjxz0bj
zk@l^?Nr0yN{}^ER|BG0<{~wZm#X;aH+z8t*_zU0yyyoKt{|d1Cv&(g;Uo4p2ebo2;
z5I!JjRRZzesFfpu{}FsvFdnBQQ1<xqu-8|8G!jyR%+xaugxRBjs|)^)c-su%;{<oB
z0Gvc2H3UBg+`H`x+)CX9$1TGKhwxx;klLJ6G2ZKUDfSv~`qtQHC5VD;gQ3Q7H+*kr
zI0y{I^6w1AK2o#~-exJ@B1M1l=tX<rr<ISFRfA7js@n0EYECRwm*B^iY7D9JQoc8Q
z&RlGU8l4gC;X6NQ=(r<99(~YNq}jp15HJ31tC_x<y%Ov&>=T?~x;^(73L(KCzQ;(Q
zqw5Hhup96-i#wcn|8)n9QX<R|LO(k3ekZjXb^T6>j;ycI374oHR%7Fs%j)Z&tEOLs
zq4VN%onEVc-du7EZ_m=x+wbB3@b;$kXmcLDeoDoX9fkDs#cE&{zk~AQ?B}3k__dB4
zOh^CEjPKv4y^#KGb5wZfMJGPA-2whD4yb^hfZCFxdU>sXjMk&pq38dsPH06xT*hk1
z<q*JLaYMVCFCChT>>6pQ#j$o)z)4$)WbPO^;)|T$2kaQINV9T=^n@$dJP3djkOWm%
zB-K+uNe7LjiY1#-KS%Q7MM0Go4r}%r@Oj3Id!flL|K$$<u0q7?4Ufb;-ZIbIt9jYr
z40H6mNMn=D8)?4Ta(rfQiqd9=Sw(_0H}QXFD)=h~hlcx1Wu0R7!7(^&f?6kE><kX?
zq5lt>00$SG9PIcM4tOg8XTh}urwDG60Gt`cS0)0#+z9DYCA~`$@EYbf_{z%hIyg)V
zQcz@Ym=@H*A-Ca5%>DSH#Ec*ZgWr)uUX++AuzeLcQW>r0iX*+3SVpUR1XrsHETh#t
z!FLhMXmzjPQ>p>WXf<E(=fpC3To$As9GQ%1&Qod_cE{c4Yx~AT4ZCShN+|eKw6Xga
zIH36jlBZsgGgY(U@~3Jchr@8gA*)DU2ru)2otO#U<C?1rYG<3tWjdEmHTAt2Z5?bW
za6_DY%1$<BbqK&Mmr4NhPBS0PH2yjZ4b<~rRS|R$KdA`DZ*}4mFEkC4v%$Sid@U0%
ze6eBjv#}UCU20Px+Vhj0lpAdd=bWKbO<^A$eT@S4(NHHR#b0R|V1Bbnm5~X!OkL5-
zOc^-^XA$A<IT7q)+&i|wS6WM~r8>`BZfpU{jYWRfiT_*US>!LB`1FgM7UTcvq%^cC
zQsjhRqQ<z|rpPpVsHU(^mz$KM)u~JXQH-NiC|rR5uoVnb{a%(mvzKQoE26BW-ce@k
zE7j1W)$51Sel?|Iwdss>$knxR>5;~_#Hy6P!2A}-HRe)vIwd$&tC|9*QFJMsM(<2I
zoi4z+v)~p+)xHdod71aZp)5h9xQRjXiONp|;vu_yf&T)IYBm$pVfl#Wsnvlu6ax<z
zd?T<wP*v#~3V0tG)#H8%;B>!ZV%*F2gI)eAt1yICyTlaGtn9cwq#yN>#)PL1UW;UV
zUbC^OpRU=sTl}%ZuCv3=ejNypx`EGGb=4>P!QYG>TSELyhu*$H><pfXaqmXNCu=r}
zU*d>A%VD?0VOLiC>k&S!9Qul=Bs=(&rQZ~x@9of^SB72`<+BGU-a8Tc@eY08a_Qf<
z^m)^iekAHlM_Fua<fCDgYRa3DhBXiT-fQlRW;0RireV#LhP4eicXOm+l}N*?bDTD;
zInt7H1)^ci6<8>c_s88M_<n<N)9XCJFAHt~`g;X`4LoXpCp5hIf~(Ys*J}|T)K914
zl7_{Vk!~?AWsnODEXJo+&%{AVBz<j>zd?)2hCAyrF~ngSZtdTrEoZf%5-$S#e`M|H
zw{VEQa)|UH9y4qt#%+sa=<Z}lZ)Pij9l;zSnF{_SLO;%-zp`BV&m#2qI`qTKrQa2y
ze@N;1dg|SVK61@UDL#p@-bsJTrt9Y;@zSXOwv+y$O^<$h(%<2v|8CQZE5S*aYh3=;
z^dab<r=zh&I|%1??X|)0iRhd8hp83PcP>LucVg*(jL@Iw&`)#e?E~jF%O8;}?VT*^
z%VwD`3pJbNcbg?IN3)daRucn_CwX30rs+_}iMyu5PHz$L#=Qww9Get;HuILiP9z*W
zIC2aAL&jF|X~<U_Idbst$_KN|HOy_gbB5QJ6FdabTO@c*LVYy!wq0PMYp=t@)$N%M
zGf`mEDL8&pEteP*LX~ky8Q2O-V@kLZeXGYg2&`k^0tC)ZMc@hqrXtY0`Qv!F7vs*r
z8xZpXW5S53csa8F4Q(f-#e3sAoR6jVUxvSZIx%oTphr){KMR7P7axwGcyJp;<L7^?
zDsnhH8JyW13K5^O-|%Goc~1OA8b7Rt=XKC{dP;NBeT}Bli~G3_i(*5XWWRoh8z%f!
zPI}4}B6{>tPe1x1*ZZ1BK@JyJGa;*XLe}ZLedut>hl|rJ<<DoocJ#?M`ZOL3eH>X1
zET2!sKcl7#8$M#vR40C_6JN`TFLmNuI`M0r_zq6|W+%Rv6TioaFLvTRG$&idw>$Bf
zPW*jNd_yPxDUENI%nvFFpE$kBK=vr%ti905+7@MndbtA5e&<V$BR+B+Oz38p!ksne
zWf(D@R2>rV9l=)|_Ickr?9l>`^db0QLbkH@!n3u>ACG=WTW}n}<^K5F4x>_gFT7j{
z&QSuKyN103rczr(GI@T2#*Ia5@1pUr!H60fPv*$$wK|(VWh5wO;$PIK$OR_L80S@O
zjPs7p^bAeW`?8b)_W0!&n7<k+ZLdVfCzs#QuX~I~U|@Q@7jCM=I}t&NX0A6JI$x_P
zziA35{vnN<em!nN1jDj9#%k&rkrX_tuW>A8GmQuL5w~kRIe@d*>PnWf8z_e0U!;^*
zo`FN0)p!8U>&ybI8IPHd=FE}8(L>*fl$xzkJZ4YGz+88|(yYD?xe;mf^uil8{tXis
zha0E8Nr4YFc8A9P66oZGztqUz9MbUj8kq=b$}{gbjbjNJhte5kf5C*@``c7}bU0~&
zti!CbqoHg#QE9qGnf5wP;~E&IXjzz4Wlw{RvNQ!Vc=-k9-+_G3T;)vswt;KzN;vz;
z0Sqhea9A0U84tu!XfxqK12Gc4OL*1suvsf*ybk+E-$YgMyJ<Xqb^^qpjI<kDJ&NjY
z(q#7Qb8PYs^Ti9$a-xLuRwa^to%D~f3Hx<{<iJ5k7D%~Kb(rVbf(hHS_l4?cR`SWa
z+Tp_ve1v_nV8REs1AEy`Gd;ZMXmof{7aoE6+n~v7*E?1$fl<s!!R8=O3a&p9SoU7t
z75pKw?7bM=?Id8?dwEaNzao~s7lZRpwwpok2Pr7B8T7v049bz$us#U#H7q=4n<H;w
zeGueZSizFa-YKi_b}}-`I`AXeA3CX~ZWnDe+eLupsoKwFd;Mc#+3v}V<91KaQ@9Tg
zHcPL@#bNc}ZGt}p8>R46U_)W}l7q%3d)wHC%sPe(@>^;O-_WUmtlG@Oxf)+v<E-;w
zwPXJv)L+x3+Gi?YwfB$=sP^(+I`YDElzdoJUZ|C4s*C)>a{~t46(tyPLeRw7e%QMd
zg`IN^3VYsU6!y>H4wOI&`<t@g9?6ZuR>aj*{~vqb0UuSd{lE84vJ8Zp1OywQij;(=
zQWA=lY(P;#l7J#536TIX*+2*+flUBA0W8l#K*b&e3zmTOSpqiRQzLfNSf2&ff}Q{O
zoSE5scM}kOzTfZt{vXlYy=Tgqe%hRwO9tv+bR?ZT`X-dXlh?vk{_G?TP<#5L2<ohZ
zv}dSJeljOzGLb)6Hz7-hasP=`ZOJOZ>n24f;0E8Wf#nW{1<fKGyax@A9cM;2O6buo
zRUHs5LLIPF*XIq5jW`&OcK+o$W?!frSJzynBWWtM8LOrO=y%>tI-H04Sq)Ivq;aBy
zcFjL@@~IjhC`SJ4I;_wsV|Kdk02-D{U1?|Q@Vj)FnubcMAnN0H^<v%@7m)vw23~hz
zdrgOZ;gp>Ju@1A)vwChV_)3QnOdKrlQB70W6(jLiwU`K)U95pBn4n5D3a%t<ljJB0
zYhBZhqiWyCY@o1Z+ml!BLY@D%25xtwHB*Sz;Ac7k4@?`^8r;h{4l*#X7m6027^8gN
zEjlkF+TlVp6f_Al<Y4qyw0P6t$%)tfMx8-@_eVN{O#8F<X+*?$uj4upG8k9&nxOD|
zo$#Elu5eYb**gw^BlOp?*QjxWSn)69J^L&dN4N&$Q*96Q5M-(AoB)`&2YNF6A;G*o
zpx~>z0Osw1V>tfyu7G)aK*3jaGaR9ppn^gip_ee8BFqtv6=59VSfM$B-dRo)uL(u=
zCXc#L?<VvW#Mf`{uGvFB2BUg_9TX$NK0(kn?l^*(qYYvBae|qn4P|&xPr%I4hA}*c
zVCHDU8Ge9ZW-lX{y?hRM_D<c8r1}jU&qK_%d3b%gV)8ocDpRuA6J5X+D$noSO<3;2
z@RY)E2)hZ->0D%aKIY7@>BtT&SeMwarR_RWQqw__$%YQX!n+A-?*O|AYJUN{2^X<d
zy`x&SYB%9iT?Fa0BXtpETK~xvK1qj@=G-Pq4JD<&P=%}Ro88kDbE%0z(Vp4IxngcH
zF{qtq_LVw@#KUOJFxPtHZ2>fpD(`seP}N=A1ZtlIO;m<<G*Owuv6D?32YI(>{6rH<
z-qZk5{k#udF|$vLR*j;;K3P|T_Pa0F;gsF1#Yb=mYNrnGtZfq34GWNK$Yj@AoO%q_
z(wEaQm-C}ZI{Ss-w;cGhUVwS^To_HO=e1)2vp{uWG%cT}dIRP|PX*D0_?|vki+t*-
zAX+Ux&y4k0P2CVjC*O;rX+gE?tF5KQ3|`A1r09hV{s2&kUuDtc-7v8qte<?3T6J`Y
z5A!{0>!J-GUN~_c+VD^`t;c7Pe&Mkf0DhR^_(Z_d3jo(Ld@jMyVV2li8NL;8(eNdJ
zw=ry;qgLZb(Io7Bh1k5qysGD!4JB#dJxKV^oo!}=Cx4U<f7c0{Jz0klpWkP~v70ha
zhZ7HOJArYq7BzdBj@Wu0Zo^a&A!U-p8L-Iwn{}GARlGiP0<r5#61(!Z>gahUnom#g
zSMg@Qt>d1}z-7fvqIZ@l&tZjJZTpdf{4W~8!##1mWe5Rex@M|}n5`vgfTnP}iPSpq
z$74|a1E?mV#huYwvB*!+d5__Ia1lW5J^82U&=eC&CqeQix(h9F6}r$A+T{R+dh*t~
z^5#A50%q^l03D0JOO?kAala0yWAVE};b@y408jyS2*|>Tq2bb5tF6!B0X~F_y0cul
zPCn#Zm%DP69CEIQT)8$Ma<1L3Tpt~Bt_96iZ!Dea@%*ZCahG1E!--6Hn532xM9taP
zy22B6IA;FrTV3I&>TuGm?sJ8or^AU2Jn0HAR^jT_*}P|U4A0lkH9+(Aeolt@df=(^
ziz{zl`xef`&pXZq%+AyRVLdEeuY#@zn2;<bZZfMNFUy4}Z?OxQeTxR@qKfkvYe@FP
zngDk9EPOyGrv9C;6I1{G-4%X?4yXRz=?cG9hg1K)?FxTXhg1K4?h4<f!gc@d)iKn+
zdA(y)A2InX)Bp*t!!y*FYvJ`FZU8e+BP)>l!Ji*x60{x(_#0*(kfI8M2#q^iBcjGt
zxx#1baBAFIS9paEr^ems3cp^5Q{x_Xg>O;ex^bWC7*<g{Eu9H4yN3qoFhNYF9`jxY
z8}pw+V}6_)QKklH@=P#@x*rVDHz7nb-H0xBAzEt?^*9)!FASpmYjoawI3KG8c{jR|
zKjT9Fqe0&DV957`knhrY+d?#eK&1%%g7<YOhY*qX<b0-sjAgJeB;;&@A#ab4%?cUv
zrU0RJu1m-nlH~RHr;fqbyGFpk`!Yj0fJoq}>`!r{0VN>rxB&#^P50PeQ{+bs78&rG
zfnLuF>&1&9DSSiGP&8IpSCB<SgK~w%ve36wJWNk3EEa{nRgzH0Y4wnp`nOWkr4w2~
z|LmV*r@-dgfT%Hp(A?R%I>8;cAp}t&VN>~ivGi;l-$<S&zb(%aYn{%P4iT?RWg~MZ
zX_T%TY#@FU2yY(pCdu2RGcgr>MFZrG{4`^jV4Z&8v9#r{hG)O6bG{y`E^ak&p)NVL
zwQ4lY=Dg!HK!xfv20vmT&)31uK=wc{{^LB?K;DK9DxiUE%}&H84ki#Islcxp#84`5
z!*LW#W7&pNPduJr8cYRyPk^Y(gV~njNp0l8G@w)t$?Ytv0~r!|FxzQ|<c_mwG}r4&
zheorb^&V=?!}%Zzz)i?Hg?A&k01zGGaJFfqdW?p1oXYmw!`WG9;^91512mjhF@}Q<
zr)Y7w@qx@Ar?cJ>s;?(6%Z0n-S`F}c-r|bU<9TWaXdgP*8P9KlZo~Pm@$4`J+QWsU
zN%ywM5^xW}>^P|4=|cfOGawWnHw-Xa3i@#RrwC?Cfr5*MV@TOj(3j(BNZC+eKs}`W
zEUHFmNc)8iY1_6D3v{q`|5QDsJ(FojQ+0~3?(uj&L)0M-X}QiuLz;l{k?psKbfwP3
zLwdgkXh>%<hTj;{<3mIGh|aoL6-?4s-m5O$CC%HpmS}qoxR&T@4CyExd^hbR;h%Lw
z9nd{6!!@MK&_ldLPqm2HW~AsPdMdB6_ZY+yJ=G#|n>)fR(bG73FT=D%6})hyS)yaY
zm#6{tc&1uZhtPPYn(@qJ&DN*$CS<Z=YrtM=pCHXrR-yiCpCHXrR-qx2ci*Ru6P?Zu
zu+}K8vYx@<G6u;7fx#C6GCkO#dnk0l=sDKjX?pS;HX4(MKiarQry6xXruO;Pi0Mc{
zH!?nn6wV9XvGzSeCNDv<h<nw}IT5%Nvd3f9z-=aEv#d)EvKx_vk?{*5dkP_w_cTV<
z^&~=;_k(NDOZsc9yuznyfG1(4251s~fx?Kx9E44X7U`i$c!kbtVYFz`K>Ett?#7)O
zr`kfi@AOb2R<YQE1mTt~N@r<5)$k)x;DQwz0j;si6#~7+#Hs`(9g{abT?yYGKe!Tp
z2!0GZyj<s|cIG|e26njs!?c1}5qosFvm)L=sdd?|6)^|B%PV3r&%@Ct>lLBE?F?c?
zDDccv%!(-G)UPv4D?-83PBkl{EPO>6P_Kw`i|P<s5#`}4VoCUlFrZ!$OT$;h($I=X
z*rHZMCC~X!Q5D`pTgKp=)3lLvIfD-X<P}j8uZA?VA}+H8SORm%{Ya!H@p_$V35u|<
zw#HKmS`l}S(UX{8XcF&6$mGY7EMi5BNDZxs{PwEIJ%p^<T5FJ9j3kVeUkF(ZLMHcr
zpb@ebK3)-b=vvWyDA}z6UJ-3{5xgP>Yk*e7eiZhbE8>dKiWs4@#$mL~idg8z{euQb
zd^kUp=y$A$K^;_Q(27{65d4l6k)m_+ikRaDE^+~8MVxgQBqAMtM~Fv!j#A&wb*+er
z=v~G3?`1ZfoCcWLzJi}3nAyI9$BqTeZ2vw^{|dp(_7yy3oMHR-hqHYH>P4}|qH2T|
z1;CM*AgekmHt|6CqA;Ld6c2_kiU&iB;(W{=vTWA!v~NIFcu{O+aMtO1QEX%I9)P?k
zdLI_CD4w)B&eDtGM<h~<VvJ7pIEt`$SXq>U7R5tn=y}X9G>^YT$Yjrhjpp&#@u5XA
zs-r5hKT_E*S@#-b*B}XF<rhL$i;&6rjgfUclNZIAx>md>F46!mipN|rvl}!(iy|J~
z{+o;9snDYMQfKYO*?3zpFIrchCt|7#n7v&CB#i|^Iey0q5uKbB@`OV0J64F)xw&G4
zT!2{=?V%ft*Wp)_)P#T54b}yg=SBRTds~8XZk&PMopVJ22In^36?ziz<n<T@KF4wg
z?>u!mOV6%58Jq-g#!e);7l>#MZsy?25nM{2qFAF#g-Z$lv+of+()*FXd7&X6ezw;$
zH!^tyfu%I~morae_4Ibm^^xZm{#|690*Bf*w-;mWUu2bH+o8=J^nS6G3koWaJnQ1W
zMO&MbFSbeve7*FIZ+8YC9d@y`ioj8qZ7Du#Q~Q%!O#-G%tXmMa{!;56{4^J{zUxl-
zJ(pR-I>4|3qdDE1)xL_tF1J4G0{;U1(bhsfg|Eg-`xEIwLF#P->T#my3acKQlwP?K
z_%5@4g3SObhKI~!3fX1W0L<6VkHm%0p|F+Kjx&)4n4UCY?XMpUycZ+LGfF;>>RfK!
zjQ4v97k+nBIw~KrRMIFpU;vB@<A>tf_ZyByu$6}d^x~<q=70!U=O05MRLA!GHjqGm
z8-h1fxF3F65lS!alb|P#nxoXBLD;iF7g4F_P#!MTD?dcoDr<5wMxd=lCm+a1SK+5Z
zJgcpnQV{nA_YcLR4(+TSt>a=u>myoEJglA7_ApGv4j$2ZSj_O4R5Vd^_r%1EKs=mR
z^eeRa=r#yNedvc$HH))E>zEkIWC4_|LKuc1S#u3Wr^XT3!r;f4T2^Wt<!IT(lNy(V
z$W}e^g3}X2fzY696BUNNZ7*OyvK2NN?6B}ycQFHjD=yI**@@9a4~$9fG?Pn;k46E5
zu{zt+3$EdH63uO$8s{WD%pKtbcXS7<^wfY(je}<*_;7bmP@RqlrHgTJ9_a*z*Wsw9
zB0Qd>orWL9jp}?P+A;(fJ)Q(7%h;HsT<z%+iioB{#>R~6=#K76IFmb3Te=PQ2&pDO
za;GC9!gWW0o)IdbM_2$#?dcA{esH44^gNZ7jPC9gDn3R9QAx)}2I79;wt7zWNwiLj
zqgo`ysFCX%jasO&>ql`WJQ3l#y14t05~;d_G&tx}k$f<>ZU9JuXJ8^)JGfH<PuD>@
z59TMf-w1U(E4JCFjx*z~Igzs*XG*rQJdSr#6L><5HSsWN%Mjgbz(V0eb&n8;5{DgS
z4Tw7nNz-F8qAB_CXwLvnm>h#O7z?VILLnZ{h>HW3ryWvzJR|XnNO~Q02}13Tt4;*2
z(NrYpnu;Z4Cz*r<oU8!2g}SPM^rt!?KJkzUv~ycWG<1y9d(?(hh3TpjJ5d#U)C9_4
znvfRi7F23%C@s}t9HR7?5TesVkpp<Xo}r_4UyWCzp(>;M{7iRg_^2aw&XKr2<7|Gx
zl$)TN0|GR%6V0%RgFUCk(R4kBUyx)Hy-@F`tBEidL$0QHh6$kgq=GWloF(u8bkQM&
z!jLeUF@MoT8u9-cx~N98vkf{xBbg9ME4vU$CTcmj6Vc1A4vh$>m))F*NO}p17{VH@
zC~yy^z#Ydj4eqJ3QUQkT923IUB=yzQk!oOaMNr2`dfhvOkm+e;G^nShz=(FL(Ki%~
zYN%j%9)|+4mQ?p8VsVABfFx(NDHhN_EC3xjz#Tvowh;>$WJvqq_L}V<hqdWBo}>sG
z5)jj#fQPt=`%pt;KteS`h&F_OxDE($h-8QAfSf8cG&e`Msr^X82oAy2epHiWa5U3q
z)a^u5LIj;W$<&I7%gIiu1fCL#Bbw`q19%$6DegpMaf~A5sZM88sGH6*e*&FVtO6g&
zqKYtiC5o)cs-~$f4l!QQTWb56>Xnk8<tC{VcQ)cQtC^sZfr^@5j%e~Zx+R)qPtq;X
zV7kh#IYowUkdDmMGed)uT_E~viYqa|bDQ7==ea8m{loBr^9?VUrcs7?!F0n5W*A;D
z)9`{UHQQ-T8`7U`g8m}@|Hp-5L;OiGh^B?6sB?lNA3I`jOgO!F7(tU1+F=KcqJNVe
zgmWCt3Jjg6h1fLt5Ahh?iZGIYP=eoBymttH7(Z}Gzl$FvI{ct9=Sy-T4YP2>Y^`T0
zW)Q|daLCF~KE*!<g$00r40Z=VZw~PftuKI4go1boh`XQQ)JAC)U>}Cd3^iP47|4oe
zxWi?ViOXpApj3+yM9q|HG1AaK!)3rCNY|iY0$(s(5xhZDKvsZ>Ay9ik;=mBx?1tHa
zi8-}Nb_1>e`i@FM{8ofM)kW~QWM3G4O>>cYNA3n&P^qY@r&FyXNH|H|+)Cmy<8}2l
zQ9jcJq9$i4&U7}$L^q4_7?GRuAkjpsrCX;t$5m~J>XTd`1~{Eshs{A(C~HEQZZ!DB
zgKC6gYEu*dwxBy0y-hVa&m>}QU}hG@fG31N)X~KmD0VvXX^uBTy=s;P_F#sV(q-w%
zsCx$tg49LTpISl5VdjHo&f*s|FgFH9n+qI7*fe&J!qrNd%rgq(oWXju=ejz8M|+;(
z^ZDw91{AL2rd>+iDKIRq(6BhaVR1!neny$+n*fLBEigfc#`7H8Pn0qGM7xzBHU<5!
z8G_<0Ha>wYP-|ly!bxCUq+KXJL^ND>AvOn*U8sX%fnnw!w_PYq(Qzc}CJaDS_~A}j
zM4*hw?J@%yDjHz~Qk?9l#w73PaEzMc>9Jt?<CuWZAu7I$Gb<JO?dnV?6+lKG$Hs%%
z9A@;vCxi`Ig>n;ampO)3oFbmRoW&QWzBtyG-dK|!Hti^qN=6;cD4>YY0wUETWWfQq
zaqKkxHFj>~05>VsOLCy0pM%UA15;5H^*9&R;WY*`4pE~M96~b0MOBA7bYhrUjLKqz
z`X>`o%|XIQ0<jU@7$PD@)6g_)l(U#eJHtT<Pjs40bT!<XlL#9|s1&VUD1Jg&$*AL^
z(5GoknnI6pfk^CTD@=M>)o@dAGU>R`In0F`JWer%(_KZPwr*PvKsR3jc&1YWGR=gk
zAZ}w$69jgmA+WILm>Ho+AKP^ZUAiHC&^VN(C)1EMhtf?}Ye><#DbDIK3-4SK2hKD3
zj7~5$ln}%Bf5#*gUTdO2Z52Ihr$>!jLwy(P^s1r&abW?V0P*es7|b0(2sF}6bojTq
z*Jww*Mp2C0C>|4<!AumLL^OwwaIg`0q?%?&ITfbRquoprn`dUJCpaq=qJ1MS-POJB
zVcIrxsyn}+kuFoVnp`ktD>LFTW^(m1lgnkUKFFMKv&Fx*PaqQ;OA`%!829|I***zZ
z&=LS@GOqSi4nb!O)2bX@k#(0yRgE~S#;;bMYYwV_u<ev1F-tKQ4GAP#)T^;raIE3a
zy^Y$_2jth&*HL>KZJKDNO<N|qI@mHH7EGqRP{_ZtRpP8oC~8gYHw-bWnD{MhTUr<&
z=HkS|RSeA??3AdjmK4L)X|IGgNGN)wVbt)(CU!O2L@HfPH;8D-Ns7Xp>@<@?ji#pA
zHFUK`)Z{c5HG$;M`WkIYq#A1CbH+(glQct3#u{oe&QKGmXk?;wHYLt50g88F^w74y
zc4ze#t|LCvV26q4juif1L0+6`rB-uiXHuvDc)>cWnU*(+=ON*n_9PnF&byiA-91wL
z?m^5{iQhflD;Z`cM|3uVIg8F<V@iToOgXg8+1p^E)zF6kvY;u2yDt`#R-F4e>nhQ;
zP>GCiJxGK#qB`XU7$&TT%5%hZg;B5m!H%4KoGFTi{CHjM=!j~cpj%BcFDgDnEpFO>
zr6sRJhv`JRIgsT;v8bQ32kQ(3;es=>A%q0NHq?kDj0z_Symde%0p=lWSYm8Xa!@Lg
z0J5V54D8S11xX;pE9n)xvf(6Qj0rd>N%(7H^1q!Vge($zyRfmthT9zM$Qx|vjFca=
zr8<1Gu$y}WvU|8d(?bh1SPM#^fevr{Gs2DFVcV3E<vK1Ljj9E0Kh(s=*6Q#w!nP(O
z%LubmH#TEC_UbT!%duF8giW}Dgc^xOugoB{X{qG^MTtq$2@zh>io)&GSes#XYIYBC
z_Eim9$5zc9LlWBYhF+^JO(T;HC7EoPBa`tKrm-~B_NAueY@H_RW#X`fY07nsDJ}y8
zJVYFl#x5d`S%HcENsjjB+OTw_9!})4Ca{Z$!$3fYnvmaE?jZv2-m?VpOQXUf$n;p#
zI!f>M4*__(qS<FS!$ahDyj$g?6%He{vgWWELE9}bM(_#ItOvO!5a2K_Hd0en;I)FJ
zZWA@zfv}MYI!(3&S3XoJ1E5P}WFxgk!={tTuF7%16rEo)9AmK7;CUJyfxtdhw^YX(
zqqUBmW+YiMS3?H$cxJd5?M!#c%2aI_9WZB8d*HAc#!_bi(%d`O%~CILSnX^RjzbX6
zqM%`zW5n*cI$T$Jo@=yFaK0-SzzYrkD^Q4O{9vh8;|EJMRkBD`OY{BtDj`FZdO;K1
zwwSSEhAnhTQ(U#gaMeYIt1dQNwbU?CW7{b+0gjQVTm?A+ODt!_DdMrz74nz*|9^a=
zSUIcy5%0++Rr=rBJSOI-X3>8}`FG~3+6JZd6SiHfCTt(WmB41hjrjeDdMPae%+ztN
z3^xvF5(j?DGJRu?G!QG_GsvxZ40gyOZR$4W3oyhx#G7P!7x89Up4wuhc`d#Vb5SCc
zINVJUDLFW)A~Y$6lEO4k6o))2H6a|O$PHTxT-5bM#fr2Io{(vOO0^+*ipr%td#I+G
z7?R=LP+`!_NW;5RU+~H%S!o)z!OdX8`DKVUQjOg1l#T&j_l@@b36W9S*>%<ItR|=`
zz9!|A%^XzuS}F@Xam`3-dnguoLv1HFrnXbuCi%at9RDYM(Dj68*_!01KgG(?6di-b
z!ze2baqAHo2p@Dq+#Dqal{0SNWC79g9F!G@%z#c3&4N<m2)pcQuGThtM~0J1@;BF{
z5-LU$QZbAlpgzC~vqzJL78pJ>$wMM|pDS85IJCxzCLQOZN#P{Yp<6Cfhl^+qLjq0X
zjE7{T$;53*WqPXx&k=4yK$Hd=hO%OWs3nCOKNy{hJYjU#lDr5_qmh;5WH(|~IuwN=
zg+o_eiE*H#$~XcFs*EG>j#S1G&`p{LGzSc6N&Eknj%betxD(H1p9XSb(0xp2XyQr1
zS2I*;^zEHpC-Q1`Q545GdKk%U<l=x$y728eR$HBfVNMH9u;a84N>8)Faa;%m)6tiv
zTo?X*nqlS(%>p%JjN<|(BPrevwO~Pwg8nSm1!tOrUJ%iM6y*>NmY@vLU>S-4qrV@F
zQee#tP-|9eLdr0zXoKP1`eX{>j8Lqd(<#V7q{47}fG5zpb8qkuaW6y~Jvdma70xGA
zi~z#tQyj5O+eaxq_Fl})qs2}ZQX>YTyaSzBaY&MJo6~X=ds3@jBZ_&l(KWT4rG&0i
z70J`R10yL@RoxHT_XEDvRC(;hL^w=<Y&0TAENq!>!Pyn=189|Q2RcQ6Azqm&XC~Zu
z3Lt=}5PLW%iglKuSZ5oGH6g^Lm`hUk!M!??h@ImmVv|(7L*mlgL;e3tL~;={TKW!A
z`fuI{iZqFK4mZm>ep(T($d3KDk+O-+p%7MWOAfdEs`=x#CP#=$krr2N3l6s?yF{qD
zts`?dcHl<Uqbct4l5p?*JL)Ldgc?7PQAaU$EWaTy;xKEkF}8-;dQBi5PtojA8Fs^u
zr$9UnxAcNVhFN+^X>GLUhNFOv;Hk8CD6sAzx1HCRc!z+Dd4@VB-eKfkbm(?ohko;Z
zlP=9A$QnB@f|Mkt<tNyC-R#IkSlJ`F2{{!T-bzlzDHSDxy7#x)Iyxqd@}|0EQjk?+
z;?)$Gj{|~~qhiDvNPl%nh3q^=(}Uc{bd`zXpW)(ATqGRG6hm^KTv63f+`xTm#Z?Ay
zL0@&FKBo!F@AgRs!H3aT38Kc_tJzYfGYnM8$r`a1c-==<(9&~ti*)RHE)cP9Q!h}R
z?`kjGji*r+;F%oBm}j^GFkK_9zN$xH?^Q3<vpLFiG0Rn1l%E?KwACk~eDY7$-LK@Q
z+03@ELSXh)5;q)^mBc+)&34B_VV((a_*1@{dtIpFo!A1CLGj8a`Iu|>QorJad6Xm1
z{=3J`%^C9;bH==-!Tr~9^Kf2zlo~Bs!%l=bZqCPPXqpih3#lS}<lGei+6{LG#=~hG
zJe1Xa$lM&zR7WXYhjnPT5-i>v(Cn$PV|6K2=NO7tBI6J)&c%<ycN)VuQRB_TFpktX
z+S#XMy~dp6bnz)_oMMh9(&1R4Jn(3a<k(5>y;^;G#?7%|Hz2wn&XVDWWtf5CpVXx=
zwi=FPnCjqAd3r0>;aV8iNN#?-gTUYoJ6B^^tU2G#qAVGF$R?{PiQ>7&Dyvdp9t?M<
zfK|3Ji^V>t(<oEYh)_D7h9qQ<G`5{lhRu!!E%Th{u-TJ{&En*ZW4dO4E~W6omrJP(
zMjbOcMCj-!T)9kRNQGcD)V)5<2$<WZk#igzxy<y3!r-(JoK2_2Oa)(aTILv`SancS
zmv=MGJceVm#>#omb~AD)5l!%JHq8=i0+Ta*IOJG^Jx#~HWcD>Rcgj#qg3e^p0;Nze
z31)EAR|M!oH&D00+d|v}KGH@^nCfOt)Jx}wi3!-gRDOCmg{kBUI%^a1)0=4k>|GkA
zEn95}(ypa8UCnZ~CUY4L@h(@n%saumLaji2o*qQqs5wTGa38SYeDf5~%_n@08i*~+
z5H@r}fgW3ptx#2)n(KG>xjxoY<gOb|+-O`{7+T=Mg^Cu3kEuqn(1il4r$lvz)_NDY
z>Waw4sx}@^sjJe!ToxKH=Jw?+G_9mfWZKefqzW&!xR)Rc1uTUV1yUm`5Mh=C&5BAl
zjFOf)$(3BT+|rFio0Vv>0wGE_zDT{H;urH9&D=}$eySs0Tp9{-B#6r_w}5fEPT|B~
zp+ksiDH(lb2+N@f9Yz|URr}v}mxG&(sP@Gw>m9l4Yc{ONZ<xD25$N{LcpwG(zJoh}
zeJIVzMpy(5GIrZ{%vy&#vmosD8fApiqCXto+*9Q!2OG_tqg^1zy|bb~<OHefPl>v?
z=Qqv2t~@W%4EEWlB;DOfVAgXl7s}Do9RQzcwa6hIG@A6V|0}T{E{{st=;L&an!A0?
z)a_@)VjN6TVsVpFta}#y_j~<^ggLr2Nsoe(uFPpUEMgTvre>3Z#jW<vlyuB69gJ<6
zjm&O8f0rmpgQU*MF~V@!&@no@a$w3i7J?(48KY)A9T5qWK#mRzz>E*ul)_~oMvUm<
zw87!dm~jRjW*x~BFeW=IfgNoP^M=)=$*rBxVK|zwy%Yr}^^SI&T(iBmBDECLCq%K?
zpzJnnh8TKicb@1b?>`Z_Q>;75y(*}O)%FvLC!=zR>kbSNcPdnG(T6SK!LH>4b^x=o
zS|czit2J_hX|s`{hERto32SILN7HnHwxUo2s+IQR=e>E;6R;+SYDH;`F*(FQMw&$k
zmwu(2jy8)B#txDk9p-^;Ckk*l1B5}9!U;OX1RWY@_)8t7>29<F8lnFR|9Vz$LKN)J
zQb)1U-B(6*cdm?Jg;XtkrEt1cQYMKmb7{B+>QYS|WznIUj)KbSVx($z5Cv76PTg)y
zeMv8N^k%J!y7fxOblQX-h#e|N6?3ZSy4=A`1W*=tTB8FS^>biI{nQ5PL2h+d8&$(~
zTbF`rlt8VP(%LnMPcSBfKSLFDHP#fF64E=h@=iAg8LP0PiiWqHw9`?Bt&BF>DOuV)
zCpp^b$;3@SeH`tSZU)k{(<q~%&B8G%AuTv~*gDjbZLUQ54FPtUT%nkfc1jAV<}+cY
zR^=w7%_i!UE+vC*qt57t0Qd))Y2a{1>}WA(<m3}@Ya?<^bn`}Np-m{M&_k7hIh`7$
zm`#YMk;RoefOs0&TXp3KoUBB|NIPpN2bi0_0fLp(sV)`@S6ZcWPIIv{HY{srTCGpb
zFiPr7rF!DV2UXW>7e8fXQM=L-+nnXX$#J=^#1J=NTvdgKylJVY^1$S@RtuFh)J`;F
zuF-#uLhA5Km{OfC(+Xk%a_fO2@uJXG8x-!>v05%Da>k6hbiSS#q|E|cph|)&YPh@c
zsMYFjg!buZ<BMID0HRV?B_K7FxjF=3xeMe$SQ4o<F16e$Y5>}y5;iKJ#Wj-KNe5Lz
ziA(#WE@s_R&wJ{c6%ooR>7PcnQ}VsiLrG1#gf&vqk1pjm>K<sFT6iEyfl4r%Cy{Xz
z2Hlg%gOVnU-s!|wX)Ls46_!;_2>Mxtt#-mNbVN@)^bHYQ*oyxx>!;SO@U(ar7;w_8
zd2_OoatmkW<`fmoOPVpwKRb8s)CGk}GiK$^oL7*WG`%P{Z)Vc0g8agTg-L@7X68>T
z@GnfdF#n9vXP<Jm`Rz9-v42uw!3@uzh%R^TjJ%?mS*8e2Qc+<+k_jvxa{Sce2lva%
zom*7gFK2F1()3(^p-PgMJ3T39#*Cy!Nd_klG5N!g!R|M&Xzt9y#2NYd9`wuEMbj_H
zn&F=?FEJMZ=rkRzp+f)6+<5~NXL;~lzj0ZGh0}7f#!Q<#6IIDcoTV}#$;_;5B*C4~
z{)PEjg(@}Y#49E;Dw#V^y-rXGRL<17GqZ~Gatr+il`dN&cB-ZWPMSBj&_8XiA0;RR
z6aBe)A;7_=>yFEF?f#u;>~R(EMYbh7c<d<48}NS2zg6Dn==V)|U&f>gdb`_O6J>kF
zrm}S0nftQklRIS>F->NR0f|wzSXU;~MS*|hL~%rIB@*0a`4L)ECaWHqC_4H#h&J{x
zOH8nbSfXVR@sC?U(E*S=0?F<!qo9wh0GDj{j`fNQ62;L0Q798$6VqjtIL01eZQUj=
z@QV(BrUyk&nJx!%@_<+3rfRPFV;>IBkOMP-@uyO%Wu={7bRnRxN_}YnSKB^{`-0<E
z+sSzUu{0SGD{TLT;{jEaNuMYPa3i)M{7Wl<&`(QcHZ`JV1A;!a?BSLenMfsyNwOC4
zmzBv_0N+?X27LS;$OQ01X+3}yT+$Dv>9YVVw<*<+Tm|*MqO2O>KUh?q6=m6j0qn8t
z9u~l=vH(MY^Hc-zw5#d0+B>--Y*ZQ;VYSVpf}Ed~$~ts8di#8O_^E$4+ClE^4@wET
z$8Nf}&qV(YiR|rXOJ%x`>tJ_{^{!qbp7+?tT4OoH=ba*AWx(#;LF5EPCwzHF%&A9+
zKS>m1%M`g;=GeUt7jqlLVA)Wqvs8!{lyG77$1=t4-CmT`h}N=UeMD?fc9Urw-5^@Y
z22<YT?K1a*BvIrSF+egMC1;h+PWsVAe~5q&a#BT=CDZF=Qj`!+mez>oG%k<Z$*)I6
zi5q!b-nY@rO4M+$Xo<IYvAQ%^wd)+NLlvI3cMUyHqsK^k^zo*9?ZL4LBSj}!Q?Vd(
z?8eQh8U4h3RS$6rrX<e`udMcNTlccKNZ5&1S}Oj_cuxGhxLPIyi^Z`UUlwt8Uu$b-
z#>UNJk1dloW6*+&B~`LY`0k{HqD8$Ilf^4OS?mXNo$&ptf_*CZoy9c>-Vho5%3_+*
zcMG398lOr@O%f#yGL9CiOcO`T1i4UTSBVy~K`f8~{3eSnrFJ(f?nnG*w*Wf;S!mlq
zlZ9V|6%{QWTx{8~h?#`VQ`_^*{|Bt&ck_kD9K>NVnh*U4`CU?0vA<wiA=uHpydr<@
zyt$+2PRm<Zm>XhKXO1tJH$5+F&WVOiDblYgy~8PVfJtsrhch+iBL&lPiIeCIzs1|Y
z{tRyeoBJcYtx?N}he1>cRn^VpxN>On%%T|~<~VMezaV!8u~)_X8gauW8Q|l+^-LdV
ziRMB2zeUhGY3IP7u^eX-@lHsVvyV+#PRsvmdhDM}YfyIb1_|=6qNmKdiURFH9k8a7
zMNAb)PA5sLFxOXf=~RmIrt@=|SNgm%UZ(8YDoQ}0r9VpSK)RMyIU=tPl<CB(B7h^)
zQ4N{&8t^P^h_cQU1@+>Hba6pY<{%8!@`)p4V9>Rw<mz&n5EY$*OhJzyWeBfq;Gc8p
zc`Q9r=uzyg#gC1zzTmlc9G;duQ#YcB!ekamq&=>+ozlThju%BB4z03Lfs<t-kdN@?
zl!_KUyI<V1DfTh(cDMHHH;R%#rrq;!yBFHC1MR65ZK*wCT{-G3zVc9;YSE^5%L3G>
zoA7ZYkQ<q_9cixSG@p6EZtKP2wQ?WAmT}m{RyFbkk?-lUs&&Y>%_1@tD>Jqt&Cg{r
z8)22`fpZ&V4xw6GPUG|eP;~@#TSciV$~o092#clOK=*rDhQaSLg2m<K5LY?|#ryQZ
ztvHN3*i4)}1|7Pj3?s)Ky5GtM@mpF}hxC^*+y0uRh0ATl3MI2*FHH+;I6b(C3<C;!
z6YB`{N*Wr=Nh&g;yc7%UVE+7ewVkfRJ}V{8EJSmER5N}lHfgT1t7Uunt=;Qz420b?
zPMi+PEe2JIamiwkjLke}ggv06DDj<$Y_{aNNp-Rx5^cE3_N?|^!foSw4)nt_nrdjj
zd<H%3?s4?9ogS}<7@vJ+C%jrBj%IBFg?d-h6TKE?_ov4alEG}d2j`=SW;41;^v301
zPj3V0F>w$*?cVY9vxgoEgYXfvwM3oAUS>i5q!_Q=pD}JFfH8ihD%g{QIb;u2@SMXr
zFI7-2$oZ5*9!*b1e+5@ieoX;fKdx0j0@LV0183ulk5M*<@!Qh`a-C<WI``x{OMEBN
zU=_YZ@q|a#AjI<4d3n&i)xYuUOhPjm!+)#1Vk74F*20R6LK(lcP#jq;9<(5uh-V;2
zi@$q}0Fm$;1(}NcR)Oi>O7V<GCWzV6FIJS-iV90+bCYD%pIAgcSs!mlE779A5iRbC
zvZAD@(4xgUcvst=%KxxvVKhdqvHqDHn-yNtO8+CGg(r-=fn}W?;)|rHqT39tBmkr^
z0nEYyH^ssUQJ8{@07r5F5K>-$AA?6uhuacAMu1Dka3CEQDRCfyJ4mMrwI~8Uky(CN
z9=B{@B0`!m0vyd4fYX;6F=@v6XCj6)W9%|nb{jQ$mxKkzAu!5D+Q(CL+8qdy4GAN}
z8e7(Yg1-;VF)AtzT;)dFKj9M)x$5$O7)hFOcjA##(&<eG@Vb^G#I>w8UtUgnKxUno
z5EOj_VnVIxOIl-33cb5LT|8}3M({A=d|!Jg4q0SoM$wJpX=r{4GV^NvC)!lS4Npkv
zKLtEALB@j1<;Y%g<CGy6*gX#aMlM;sZ&RPiVkT(7k#<i@%=C#Plf}$x2r+iAcF2=$
z_d8q^*NeD%e3wdyJIhH;eF;TIiF?as6@V8#e)+cS_Lx09&h8ffP^Qe4S27J@vhMw!
z-bD$MmGg+aT3)3FG0n`PfD9EZNk*~*-ilRrpSZ1dj}8+?*~8+PUladUG!`!*qN!dC
zv1|OBDKbGeVJQ%CK{3ZC;*!OjYH>!D-9LU6RL3Ms93va-<n}{})?41hKT^hIF;2!_
zlZpnOmzydUW?zH9jN!8DUeP9CkB$?u0WlLFALW=~GVpJWKs-z*@uHRf8xWKuF(P%P
z*%*W^kWZq-^Q8@V-)3h+TTT!Q({GtYJ!&6+82!9PkC%~Zcd751okJH4TOZ{W)9pbP
zf)eb(*6KHg6z8uiT(@cHBT>!7Lzs`RdII9E@&J+@nY{z4x-0cozJgf~Tw<-QgaVc&
zWFI3Uh~_M6GRu{HAT^?v_m)*{pmp%P<sZK81q6HtBZMukvt|1Cs|vps?V${BywUE}
z{@d?>+WO9mspa*Gor`^|kZiLEq7pAH&aPm<58$oE)a+Y?lEFCfzQsHrSBPxUqHgkO
zNc7C&U=C7k6?FifTAWSRpRK|N;2)fQvruysk{6W)N!k|o3N<}B>@$UO6;Q_b?XHI{
z7pIVXVu2Uec)2%h7V2!-Z|t{Nn$_YJPqlcD%t-4kyPqYlDVNoEe~kj~DJ5GG=l#kK
zdKvpSi)Av^?=n%9a|6<>f_MpIm7NHFoQOWUrVO^59SHiTRfFz-rc@?>1L#>>`ZfaI
zTxt&_E74k;rt&TiAkmFSgal5nm=O?1K^@7#-$MK);;%XWCTSxD_u03qfxGP*rg2Qp
z#v2xWGgx%ASfyF?;jH~P`wz3|5Fa(Xx{>KtDc*mcsoE425(*A#TZZM^!`A2D&F2l*
z(UxUalF4+84t02L6Lw@$!J7ysUghG-z~axHlcn(KH>HXz>oe@kMlFX{ZA{L1bxh8{
z7OpfSpb$zm*|JF$pEJ!rYsR!ZGAUD~iA%0G4Dmh7Iz43)R2EvULAGdKW2GmF1vTO=
zn#IuHjzi2GvdVUlB&u7>0cT4JILhv0m7n{VJ-8DrXq4=93O!CIla&ZLg|z8ptSCx%
zLMQ3xbeqa4;VT}Ma<fcXnlmZ$F}rUkS<mrnIDU^u)^UL{Nv=gSicz)M$ENJdET=wu
zV8sT;HI{u0l_YxFy(|$!rh%zqVY2j{DHi(WB0?q)SL6;$ZHH)u<f1Tmm0W<Tt|_Cs
ze!(ny4VS>?Tv%iKfWMN}GhX_sO)JWXpTBESP=ehbrO1Rh5fj`XO8n=El4QReY)WeI
zu9owW<qcLqZ-uHz?^M-0Y*GPR5$<ACmhkQ@CA`%()xD#_n=Gqf<t*`yhfa@Tm}o*1
z^HwWL4&q1_QItm9!|qB{<tavAYX|WD3}dXdt1-ZpgT)lsei8amsU*ww^IQ<53k2N>
z;R8YS9MpjcPJ)#JF`|}6R<x?3!Zr7(EG5eVm-#cqr71u>O_{yFTg>YF6@prz5P!0m
z5&w;Y+7aS(d02+%mM*49rHwOP_OjAzO%g2<ho+2}uqq>C)#Qoeo}T<6TbAvv9l1j{
zdZEsKl(RQyx9kNQj4!EQ%J8&)DNEA|Q`V*xW=t6}ijvt~<Gh60_OcebchVbQ4^N^9
z2nx&OOW<qk<msoLK9v+x>vc-V9CjtWQ964mNci)M$$IjDkcpQ-^kVBbq$zJ8N>B=~
zcz7{PCaM#j6|fsoC<0B#)CsOZ>hYpg&GF+zQQ{?{I9;ABimT2S#o6bNvyX58mE2VE
z7-ie4wm~vfrBQ-cT*}zR_rfRU$;34ui(_Hc=~M%jP$SwWFBh}xk<nVEb#7+kuM#S*
ze(JysnKUaycA1^A@uQ55Xw1$<ByZju<%7KF6GvBJTR~hH6sJST>MCQ$Etlmg-B*i9
zxLt#E7X+6nBuTR~-V-Ot^fT;p;^xGojc=FGB)v1b4)66PWZ%9sTE;G-s255k<$onw
zO~jfKviQ6bjgG>GQb|Aou?$c?#1Px8DXDE76M><6D&fCb;r}%nYuy%s5}L%nazz3q
zv~&?c+d3hDgxmmEA|e&-dj(2p!XpdTM*0Pq;|tS8`<f}DRk|pw6UE8r_pbn}UQt48
z^~Y#G-ml~LPow=0XA~yMt_WGmAr;<aajs10BU;K>%<O>d&|AsT^>&X~uk0vkf5VnH
z(KD4EB>vhfC*w)>EfTlrSNKttZZ)v`&kgGNw6Y{L$$pHcA-84cDLOgLS<Y6-c0*g<
zNbzeiHAD{(CXL~jg$?#Vr32*~>ENHuNIH1GSnUnefg{+SyP*jEU*A^#-HiQE_69s-
zM~@$UQrcNr`FYc3WX;K%>o+^h4(~?~Hl?H^_z$Y#$QjPi5#VOxkPnE7kH;s9>7VFj
zy`C~;!y38%207@`(U{zyduZuZm)DC0pxlA)hGo*^;CZ(;^p-k6EF{K=-P+n=^C{Rd
z0dmr%n=Z_~VAVR&*8l#xB4Dle(DL3~ZjaUFt-fJ}Ja#lm!cTC4kD<WkKCz%yG_QK}
zcbCa#JzgN{ZPSdf()(gh4z@nt=c+Pu`8yA0x_Go)&E*F4lkIurueluVOkwu9Ts9*u
z%l_%pHr#rI;c(1UB=?2|=q;*7Go7aPZu-_SP3={dl|bfSYc)MG*r=dhmy7$#gVrv2
zG`4jQlQnBLo!2|1XnubgXCE6sOp#o+{_-Zt9cO{A&h|lFh0IoiJp^n1vcBaDf3W6P
zlrIz?!j$^m@U)a+BSze99~&p<>-<!ZS~b^;Rz7(|#d2|Rx%6ARL_3*)E#S?_*^u__
z=O+y6|F|egoO%audDq*$JBs{lyZ>Pj7#q^Q9yO%6|J&HiMeqztP?SyKV`cxhuFjO5
zu+<{F0iS<{-M<a=JwL>af52Ud=lNzc&#$*haJ#ll&GRl0@NLiaf6em)%yVZJ{*TS`
zCTH>H0zHc>;T<`7mMBRdYxh1(9uv1fyzi-MG_P68%qSMmm&t^9(J8oU!MS~kM)co0
zJmpHv-hx`u8nah+P7?QH?=ZfoQg*@vmKhoQ$jH-D=Fh@zPAYGe$T*s~Yqud_IMY%V
zaG!-&!9R+Yi(BCK5--{Xc>@34YT#Ak^?Sc2S=jn0bL+Exx6vCo!&1>YU3^qlvoS-i
zl2a?L%pqHzZ4YMrD=Txxp<ddX<L&Gb84HGP!c30k%!Rey2Hs`0yxHEB-%vhPE#A1{
zMM-bga*koal{a!!%-yTUJt4bfWThx6Ny1ks4$2Z1Z*fCPZoj0Y0jWuLGqp%|nnm(3
zFOr{1)gtMH51-qf-T&c5(#Qz$=h-(7)uQr;HZLHuxcA-BOCgxn3<$L&-yl+%69&P6
z0MgSG*WfgN$YBmSo%N~>`QfVxDZ=djZczx;IiS^lCp82A#Fe%MpRlrevD%Uec5_S{
zsIu59C<bPV|A)tlle888vqcKTin3t)w0@#Fo4%oZL&6wB_E}$G3C-9{maQ#uJZa7p
zAbizGay;2JebsjNb^uNw4aKLmMUJ9rE_@BsPeS@{V7UOa7T$A0JXTJm{qL~M3pg$C
ze!>SHiawu09w`S}F60?cwwKHyTCK&le}{A=*icSY-p<({Ecb~IJOSXq&H`uJj!e9V
zZF_l#RIU|qWUaSEe!xpMRNJeJz-vWYg)N|S{*!X56f6wBin3rQMOm+MS$CBuN0jv{
z<zOv^_w1poP(f?FyfbOa?~J0~hJP3E+{MjbU#@%rXdeabcGbNB_<~$TmP=9f7F5O6
zFl^i6W3I%tjT-zhGAV}%*@g1TSEo`f9?*eXRoE2NVkf)({gW$kHfe<YYJvUNGFA@Z
zKPbf6q@MQkt(hnX0Q#xSjKsTe<J0Ea550b_p;GDSl_}X>{g7cz8SEtRB}6#_bBel)
zHhiwuu)%hn4Qxy^y>_=auT1yKyDN%#wCocc3FbXQ&U@Q(#%*`OAW#pikCfH{Yptu(
z_EO!b*I?hu(e?q}IhRabxpph5b}Lc4h`jexO?Y;A<vOxN%hlV{R7H+!vLe@`B5ioX
z>^W4VpgIEIbyOF&RCr~It_$<0{gY6EkH}xchIa{d&pV;+8L2?iJAWq}s&`}sJK=nk
zc|%&-r2f<$c4Cv&qW)Q8xjM)_tG*?KG-#H0qaHl2(@H|f-6Q)^S6O19g!UdjFk{WY
z^f_R`N2(T!m&@u<T`p&pHb*h@d^i=&{R}KPqO*2Vy|fpiYUtgrhITzrd0f+0c6SW7
z%53J)`mvMJQj?hb+Xo)Kk36QE^s@iS>F?LP^<Fu)B1Pfo#XR4!GA*4(gSf0r2GJ0A
z2Uin@05?1|>fCZ3_4gaMe5JDhWEU(fOuJQSJSN($g$ao^%}txcEuIepkr^Br=dP$^
zStqidKa~>2<*XYbTR1i~O*K(D(L`p28T?oxQSMdtkb^^)PDuJG;Y$%zB7PBdmO(Q}
zTU9gDDil2;6ObKVt|c~EY|XlWc*G4%l^VdEgm|_zoi_NebQ*Z+JPR8yOpmsYh$H#?
z>UKV2gB&MNlf7H@1mv~M6L1S`xM&lz3jD$l=<tjWFdoYJ3KT>pdcVo&=blDP>f}*W
zWkrrIzvC__U=1jJIg=xv43S7*ra{)TBF;^h_Va*#>9xD-Rc>P0nThAjJ*70q+-z+Z
z{HZ!n)8q`I2_OAN4JV?8UD-?qkLtRx#;yO2tkk%cx^b|bnrc|$Up7<sxFD}guiZyg
zpL)t7BZftD-dZ{kgruk*19Wtdqr5~9g}0~>;qk=fue4a$Xr+XW`MykfL#6CSd135^
zpS5V2n1!_K%PH|C7R`ngi%4SlA&MyFXN!o(TpLCFs&Ww#)F1eL1&6D5lZMh)E;5vL
ze^mCByGJddYQk-+8&|)UC3aDUFmXCPCWt;Xt?Emv&~*0-SRP${rJO#hfU;YZy^fi_
z_|lcNd%DQr4IF0UmKT1}&+i^}r^+I$t5l8$oE)+{V+*3)L4K<vWB7L^Awlb!=<G+z
zM{@VVT~ZhMAR!n>k8)J?Q<#MzT18W1KPU-=MocaKwb9wj%11uT$Yo7q1Qf9WDY8@w
zzpi6BVOK?vnUz!;;cvK_UPTEBN!D9<+pRd--8@MDETKWVo9nS<5$&<O7!^RBUV^Eb
z!h_ua+`Gt4Iq#2VhGD}PoFVT{I&~C{gC#oozh0TLbfCzsE<=_bFoL5|X2ps}qj}~$
zgrv3?&R^IcK}OM<J08NT4qv4Z?vT5`1j_AY*j_?4UgD}=&U5AKD55S8l_YBF@-@$|
zhampiqAuD;``Qn|h_Cwb`DiU2THXwHk+qe~w{#_v-3pIVJzHEg<+gktU9BgBOoDzW
z6){m1lL&!w_1oKTrHH}Ui;LlFtGNqda!<hbsRoMWnHsQ|_$t+)nrq;u7#~Mh6O|=4
ziDDJmEusRYQ|!Yiwyo;7oH{QKOmKz>>=XvS%Gj`GFs+gb-*CdJI6>!rD_;z?LRH=p
zS48oS+n3z+AD8g*ji#<&9qRgPdDg7vuK!YX{c3DAsICt>U7w@7o*T|xzvRGOzZ!0i
zYE_}@L2bQE)RjN!{!pDiFkSz_!UJ~wMVS6tv@tbcD#Bg={X!aich_I;t^S63P4?qr
zdB`<q4=>l%_nH|*pl{swcCT3WceL%kET2fC4XXFi66$=_4%7L6iy|_5sZC_`aqj!a
zx$pnA&`kTsqkXv(Z$$Jb3k{{bq#1aZnhdXb$#c~9@_=($+>6ebFD2RflVTh!vc-mG
zM1<ZdHj@7nIQ~Jf!TSo~4SG+uySG|L7%aG9G4^!tDtRvjczJDu9QwA10pytW(*hb5
z_d2+NkSIA50<x=iYY7$Cu8`h%MrKj%8j7*e6fU!^caW@e<7lddvNIvsT}(`^fm?V(
zu`goC8wg*m_hk7~<AyyR-mu$57pk-+=0RRY7Q0syol(Mh)N1F>Xjk(T1%074V#$I#
zuVgi=CuO3JRmDvxB^$8}v;+7$Ef#U+y+k)&hQAG(wS@=jxrJun{>kg{xzL38kZ@q&
z<QKYmN<0VMZZvLZbK6da<fn!*tjwh=#Y5V0*R*6K(%Y_vbk2ki+(Da~l_@IA6(u|n
zl77@1s}|Ch?YG>tOBY6t*tgUq8Zpx(<vPtxis=2b;iSfmDLqi{XVG?(^`aWjo`*7?
zORO4li{yE1TG41wv#1$+i_r}3)LSvh=*ZzoTQYd{NY)9t#u0OD0`k344CjOJS$_|;
zIeb`{g!5E*X(6xSEj+`YYc#{RP~klfsc^1r8<`xIeg{H{(XWyhQM@-aw<kc{WF7qA
z-EbdugLuYER)E?@qxlS4=C0l9P-zilWdc|3LaLmrCl8!l{Nz!}^QC6&*~;RYu>CSc
zEb~y4Vk?#HN`U#gtUN3QF=ls)LqI{*e(?ZJjF_&}p*+abbd{NQhgAsF=P7b(vBf8F
zSQO&*VtT)=SuNfxi;cQ<8&8Ewj{CWpk7zj}^<-Qk!4sK~c`GzmW{S&PIY_6A!kOar
zIBF;xnx>EuW28y{{SN5|F8Ibe8r#)`EWB{0BYVlHVf`G^t=73~ws$2Qy0~KwIMX`Q
zf_u{`SGV;pU3dGCmUfpKoRFe=9I%)Wxj=u|eGY|O90K{bpc=t97327H^dV_j7cU~w
z;0h#0qdzDjg~x|J4j}Nl79_cUR76tzx)y;GdH{NAzR`1Gd{JW|YFe;^)*hPASle0_
zr?!iBRV2iV_(X{D9H1rhkf;xoFBXNA_nTS}K~MrcYXTy+TKM5sRYuzAmy97EASF_A
z=Tabe+^Mxg>cmqmXq0Vrl!mPgJToVW7?SDlDI$*dEIa!?Ttu!q&vI*Tn6GUDfK`6B
z9ys||l0IZBE2}FMZGE|f?Yj>|(^}eCL}TzW<FA@e>+0*~)SGv5xi52B8;dAhy_+<&
zyzVSAqSEUvvafn~^b%(7W;$zSwG`D5XVBwpkx0tw5B^AH^}QC=SIKFk8!4+_xU%+4
z7n69J8f7)6{)Kl(-$^AxX{gbP;1(x`?7>k`R*QrD28G2dt}6;DtKYQvO=b16B2rd=
z<jz@M6o{Y^KeqS}Dyz4bu&DD14^ll3z@06K;4LjOqVSzuucbw#0q$?X8sO3*L+~nM
zLK>iv(j7JAD}OE8!eT0Zjjl?)ByNuRU24cb{RdP-uB93%$=B4tQA0lF8o1Sv>X`pc
zHAJ<;s39BC>$X_#r3JE?`+g_){hNN%_dB`o-}F=8-x9-p|E52n3Gf|G-y7|ewJK&d
z+Yj9L+ueQt+uC={f&2b3s)3SOObvMSSp|HeQQtom^S`Hkt5%qve=wT0@0+>nFXpb_
z$6bHF-|+c;Ewl!BkKd4=ds~9Rs^bL}UZUT6JF6-Eh&8~+3t4)6z9o^d`wNXC@o9{L
z4c?ClZymVaF->s2rp16y3L-VYr&?kxRORG$UAxz)j%(@Blm@u1kgdON+f$v>LTE_b
zLdMZl3zY`=OksWFEuRL=FlA`<8exOiB#YEH_?hH<&Yf_j-*n2w8aADB300-uE1D;Z
z5*%&H$QV9}sjzfp=7_F=V+Cq#iF2T4`ROfGxLy{pvcl7wVh$*TK+(FboC)~>ldu}~
zFL;NBYvB71Hi!|z=oV<Zhb#`!#OT+=#At-`5X2!NzG^gau0K#m&*JeX<s_mgbTqE}
zV4?B1l|M6teO*Ji`oNfL;k8&JZc?hj0mpz9`|Ugi?&<pp&AafSXL`i*?r(ltj5PY<
zMKswyIo6w*q2`pG636^rp9XSi1)cX##L~UD5T5N3<6R$#W9Wf!Oxiz@N|h&vmpSAh
z=~3oQg{&=vN%;HFKDxohKBhe_OUpjq;E=q&Tos?RUnzA9Af<<8IbbzA>m0F^<CIwM
zWYamxrjSSt4P~G+pIAs|0>oR*h>bl{NHSb)OA;aW78o{G%LH(50kN@_t(cANEojh#
zv8Gjsfmdi#3{xv+WA7F;(wXjVh5Es8=}?AYiZJX7&8kL(p;ingyIFMssmqVGBJ%r0
zfe$0<77ZQ)Vp*f{cEfZ%6ujM(*=jD#Rt-99k3-BFtE*%t7JkD<`wps4fW{en)Fk1U
zqYPS~(;NKy#DG5Luu=^D4Rvl(CT^2qMq!p$6V|5qgVR8)KiuL_j7@*|153T9Mf?JL
zT6ZDpy9GfKZ9ZEVRPJr7GAa;yxeekjJ}C_bc8r-1_#YmcEccW*&^Fjkk6%2gJYxP;
z8cZ04pjC7X*B0Md8VfcyGE<3`lgGRBHhCD*K2aJ#(kE>?MQ{e?Iy@MVBPcBq8{USq
zmm%&&J~?>5Xjlp0g~iq4aslpw^hX5~L<>4Ukimt%6MYDUUQtYiehUkMB_3WxrEZRD
zy4dR%P}4t_T<moVs>PjAs@VIYOu2tw#05W87L<?no<fx-<EBlt*M2zlCVOD~l1zH%
zRrv7fbm7IgjDBp!VB-q&Xx7d?D1Qc5A{}G(Bcg!4P9}-AXkAO$<<d!V=3I)vU12vO
zdMcYd9*Yiky`j?X9?xkM!(5|wWYn{eB9zM5?<!>5u?V*vVKFri_P?S3uo3bb&ZvZc
zcgFn@`CeH-o=WRVgI)Hg$gMa&u@4s>+2UeBhb#6i4&eP#LB}l?kQRDzX>g5bPUlwk
zSwv+nE~7Ie4_S5U7&h+iqH`$f?yk$rs&Vg0teqGKO!Ek7JAY~ly;DVF3Fps@q>_>R
zY*oQ!+$Wy&P(i>%gKscVDZg4Qs8m?ND^(S4#qAC#7pJ_l#m8mzi=1*{lyyp!JsuvA
zs^qu1JvR$g_S=i8@D1D|_<JOpB3(#OQ-dVRT94GrOM_MtDiE}AYKl&zg@L!(!MLUU
z5xo`;%!t0qtKsE~7+w+`jFU-A2|?u|9@{q-1?Q)Y!Lg(2^9A-rh1kN!!2jXF$x>34
zAHX>~TRi8X_bvQh=gGdH4DS1K!gASXvfVEp$adfqD?AL!B5>WtxZX9mZs+{3Gp^hC
zy*?b*v4rbc3)EkX2wLQBw}3qyevS?n4NpnQO4;Zf(?aA^i>M2pYu;q!iwh|7Eg8g#
zK(a$vymcH}A*aNzf2d-mVh;0Z8iH)hsu*4*S~GQHnu`7CdC9klBWXk460;<)5pg(y
z>X=Ruy%nZDCgy`U7jed$*@OstKpDk-Pme5m@an+1`_zvC!Ev@mr^7vXL~m`;hO#Rg
zC!>|Ass9pLHKJEgr29lKzeukZRr6)d>6Bk~Be#pYAEqpHZy9eeCwnu-%o-zF2k1DO
zxKLU9l=eG@GBL&5gK@kZhtO=X4=2|sc$v3>%o5)(qN#pKRPC#gNC68%AP9jdAQ>B;
z2T_85#dnJ^iRl%2X&GICodzHp5{$bhl^SBf&`j=S)=F4G#@PcqBF#qj68sgD@xW=W
zUPNp9Il;!ZmBm$bTwNbqHwEs1MYT%b)3lc9neNoRS0)V=3$j564@YUc7ZFvtvpHk>
zp`a;DyCG*B0;W%yKHXG;#TP$jM1^=A%TVlWfqRB3$BamuL>Cd>To@2>esN2bU%a`<
zhhLh^cSt_<_!_Qd19Tgp5lH$PQ?c8mFC}M{+_*GNW~xI4*!5<zY8X6xY-oIOeS+*i
zKjg4YI}?L|<+4H|BKum_{vE)ELYnkDS`vwR06OKDIERWw(5eAOHk@cc=y#<&%(h<)
zw@<VxQd3ZE8XY3hYKF-7>6PZIMI6K8!ILBhSWD@l)(o*5YZyJoDrGQmNR=kvEvofR
z%#z=+{UdJ^bLp7f={#$SgK>BdmeSP)xcI`ZQy^9O3fpfFY`<{c!fQ7U&-i4>Co(RT
zYsPMy7H_w5{RGudOr{5MJ<*pQ%jBy`yHe!1b;D1)V0sdkz&DG1eE)$h-V@oA<WaDh
z`bbpR#ja9!*|lO`L&b0bGi6fY3)iNs?Q;5%;fVf<kMUn^`+Hwdwww_24wNd0%T=4q
z!tT*NvI2XPRv)wiEtp%G@@W8HUrfgn?-VlIC%XcjBdu7=c4eh>7H>z@b~|mPh)Eao
z6ZuGsBn}PQ1E-W#LwAbK*_J&%mPY-_V(P+2n;m%6f2EwxpsVp7PJwE?X$RERzcl=U
zhF|F6_lJhxt9IpZb&~8)XJyp+0(NJkk^gc5t%23jKT^!CK^M2GQxo99MXV~UjN&Qq
zXtA0CPc-8x@F>^+$!0VKit{&L7myd6`uk_VHVnW<GXtJjU}nIT&9f){f(ejJ6W|uw
zuMzJwQxia*ZFlc*UBc-@5VMtb<;C`9{@%sEckG`{T7R&jadn!jWE%Qg7tqkZ4MK5%
zp@00~LvP#a;6vkEZQ#`Iu^KzQH^@ZxdPnpq6aAvV^=!{TaH@qjHio*rxWAN`RC}BY
zZ@_yZer0yh_NuZTZ!#a{K{`KvlkpqCZ#Ly?2%7T7qq6K{zs1Y9#Kwwu7vW+o6ch{k
zRn5QmMK|E=a3Z|({RpcQ_u<HJ2*DmV0+m_Z2>Sc(A|imvNKBct-I-%mW-zYi_8xOM
z<c>CU>h;nK)!~ria4mxEX@k1`UqAI~4xk+1x{W`0ID`*i=&L0487k)_pZnlm_;r(^
zyD<)MN(B8KIuiTuU#sk{WTVrCIw1Tc3Ced)k^pcHp6E8wS&}M&82l68<5H*&n4!OY
zxAJ3rUBut5h`W_HMcl1?Jzo;F?SOYHZ+73U{3ze8T>mHDt$Zoh@*7Ltt-N2|tsGEy
zEC0f`Mg;iA<4gD=jD426TN%d~!tPdnhwoPY!F{*#>y7VLzCqosOfJec+?PpLYq0ok
z<%`RdQpdVq8Q-n^9jCg4tEb+t;hRW4<-3)yQFklv)^{tf=ew16Tfcp`@;BV3D?)cG
z|JcOc%4_xA%3t%{$~XR=yOm#yyj%IX|GvAG#Y!uvu2OyrS1B)`VR%SgrF;)xrTovx
ztCVj#<g1iBHg%QqsgYMHe~&{Np{tZHE3Gyg6}V_Q>?-Bm`YPo>DN9OABCk^Zp085A
zB=XMWFZn9vD<gv&_$uXUzDhZUu2NnYewA{G$O^wo`KiWNDIcZ}hkeOcDL<nxQ`!fz
zWqURw?mx~r4tkx@|Hx%Zzxzf7d{HEyt~n0fZyaI>AgN6q8w8(diXk91=fru#1KqQa
zoTe-y^#H?3Jpvq34}d89=6Frn|L(yJJ3(={MK_-xo-Hm6h{OEif&ebO7r6n^(l3hA
zi*R$8n6G|tmtRKWMPhb9bnvTZ<yzb~f9`o_XJ$;!$T+w1))f_^IIvc<46GCLaS0!O
z7FJi_>IHbU)y9kOJ+LWe7U342-Z#CJkV2P?6a|WkM0>x;ODtN1ZIz<M>u$Q~8oJPy
zZcr%p9e?$`pULy&dH2e}_uhB!z4I$3P5CHe@>_3hcx%Ik4U>~_4!wFv(SVCiEt-6H
z#Uva<uPyto|2{fho>vRFL?)sF^<i&wWnu;V>QT?23O~pbH!mgT;Wndy9Go$KNhV16
z<E3<G`IB^MQROzkwG7uW4DbO4aib8T0p82tP6OV};422alfhjEyp_SX2t?Km&>eBP
z60Wo*=thQc;d^(+djlu^$mIN6DcN8DMVA&K?NyxiOAW1NXs?D=GW3mxE@Nn)hAw94
zM-44!=of;hB}=)809{`M=vP=NEwM~PKQV;M;yLbnhAt(DGJd_7*xX8?TD6xWR|{PC
zgt#vmx=O%O3+P`A;f5rR`-GwEb=-#xRqMF-8M;wJ?=rN3pvuQ^IC=o?LvEQU{OSF3
z^uG}q*N<9<e;cObkuhsVW<}PrtgmNJ|N867oEgh<wmu@RTkMM$ZG-Bj$<S4=hV<Qx
zd*t9as!5+8tH}1e@IOKN{=7rfMm#>WOX8fn;r{^Xi==M{xO+fLnjy-FUj5C})rube
z!OxL|(?y_i=pvItd^Vt~ri&YGi%c69%w@6&`!lk>V}4H2g2J>7@i|5F(=HOtt^A~v
zk%eh_9g|YVrpe>u({g52i1<L<@Ra`adI7E&CClFXrIyI@;|MiKo`?rICB64H+jmQi
z94t@8q`H=6Gbndh^zhW?>(RcnYsy87Y+TZDtR-<_mQQ}3x$HzTTu0$640NN-r9xIM
z`-lj2hx##tPQig-F@iQ|H<Z&oYTcCoR=s!qTfkYzH+XHO_4u6VP=$~hSmbfXnSk+O
z`GyKye2yqFo6O=5bIf-7?1PwHCo}NHiv*lcmdRuLW+ur^#pN;=``)pD1}?t??+dGN
zj~Kje+t)V=y3?vZY**=<V+Nf-wS&E|ZMM7`k9b=uO?tdpUGMNMjrFjw;;Nn~Ki$c+
zh%JfLQ8@JlrzyXWS7hgwwC)lyME^;VqtQb7^VZ=1@I?k^oH-t!K*%b{#)VCMSrdBB
zz1kwzslIa{Y!R9(i$k|EHzlR;%Efnt4kD$1Sh}SYfR4(k%XPHah|7<iI_Q)pAne-Y
z1Ky5&z-tj2n~m5~(!ZA`0<g{sNGMpvh4fW`R%FpDq#Ys^#&4w6sW2y&ZM-D`xu&S1
zqL`@&PLT6QVhX|Nz@Yl#O2%55Y*$!HtH#B@Vg>GWV#gfueHq?X*gX!@S?%r}QD%m+
z6dpsRW{}EwEK<qUS}V`Mc|)w!tILU#3NaqH;UY<YxO)JgIu69mz5fDmnu;09F{crp
z;fNu>0ex{jnZPVI#PeGKRhug7@XeK2T!P8T!XsN@>xwR(vWHTMPkQ)>C^i)VJmvB8
zo+$2@M@C!W3%!M*k-3|I<-T(Jc*6362S!o?seUhcYVo}WF4<gD|J&#;K=wPzQDY!@
zO<}u(vAqs#e1X5aQiqHehy1g*ZhhntV82Ya_#1`&GRFQLTs@m(SDO<2T4pCx@d1k)
z^DAr#Ha`1Mi|+;{T6~ehS7l0IT7`>ikl`!d#aN@PkQO_6Srw9Yw=L|I(6+KV2Bq#S
zwwz8l^y;4mvPz$~P#yE0s`@&vI$XJ$=$O!G9da2|P27FMu2Fh_hq=)oscv_LM&$6+
z`H&AuTI#L>b)fr?Abrw<BlF@2zsO1!-4eyQ$>NA?V$nw=h^%S~pOXv?MjYW2S$Nio
zEZ-$#m#i+r{~zL?h-p|S?*q%b1#Ajm+ahE6ued2SS_F82f3=cRIfh1iKSwOZ)c^pN
z;3Cp!e4K+Kme`zXDFXo;pA~_Oyk3l^?TZnR5Hheg4zHh&%Y1mL5?jjxe2cmd_okD@
z{G&2b3Xm$40XI|I-P$c%UWU%!#^YIsZ!G|LNN;A-jnhAs;nvxdZF(Q`QmesB_cDsU
zu!W0Rm=gAhao8D-&HnHcdro_q!#g547sTfyvEAZ&87p(fiZKB(L<TCvX+hDt-afT`
znP^u}e7+SZTOu&23G1z-ImD;EE#-y5gK6$MxBJF&I&n1b(yoKX=6k(-EBme3>eQc;
z%l6WSj7@u8B$?qZce;nKj_h&)n9QGrHT=O6zt|q_!*8;<3g1|{KdM1Ij~&YUqiE}X
z&mvGOe4h$ixC{^tLd$9iE4@eY_D+;`u%XZZrBKO4@45rFMVtx02oM_jjM_3B--1P+
zJrTB84PV2dK>E6*k``SQs93e?Z}y{B+{fZkkb+J=k(0a=(@oCB-Kc@{bBoH?@HG3J
ziZzbK<nF@XXHg$yxj8Bt0XUqq7@7_brVzKoyF!KTqFrx}zn$*^7nqJZIc6Te-6_(Q
zH^-aU^%(6JyU=$Zm(_|1xWeLT&>~!nrIgMuaL)p+K~<-@zks>ZE9y(rohh`*)>EiA
zq*qL#!Ei2BQ)v71cIqKaAzwfoUt1wkgQ8_kZXfyT8#H<3x~Aq1?T{!Ia)7B*fvHm!
zF?B+j!e$Px7~N1VWAS;Cmb53kzLW&@-8M<)H<i^K!Zi9DU4?FmPuNHdpi1q^^ThUw
z9=IHtDy!yvs0z3#BLkT(B@Y@sJKn}d(LZr#$meB2njb!zA2~U5ZGYS&;&uzO2wyLN
z(?tV*>-aKBNDe#gN~`t2-lAppV$7RM><_H?zWryRyn{0|7o`k!q$><u9yj^Px$=R3
z6BEvZ<9h;$;DY#QUuPmJcm3`PqB5JxC3OjOCY)BxnFG5}9zGcK2yen+7USM-qdfdA
zny&x9VUgO-+IlRh2!X#;gadpVG<3IFgd}~U(GQGh?UwgL9|#TK`GO1|sX>rRkKiy3
zLMhogCj1*Jk=a8E$e(t9+NpUcIIF-Y3c7e*PJ}|@G+oDs+Y8Vx$6gSozv#9d(rs)!
zd3UC14P%Yhicb-rd2nJ1PQ)ZFKFQw}Q6I2+^;BGT-iMYedo_=uw+-YeksK$kSipkz
zcV#3GTwx_I-$b$M8i|KRTi*&X*MFmEQ*$hc#Jc<Op|f?Od3Epdn{J$pO@QeEaWq{a
z_*Qg%tsE$pTGco?4b6dk5kD^`H=@5aBSYhj+iu+UC_eMpoj!T`+_vW!=xY6lYabeZ
zfTN+G94mkPQp^d;e3>sMBcKcZa67y>4Wb5x(R*vW&lBnV>;?FF@JW380sdIxNxmH9
z$>`+mnZKk}{0furrQ*@3pqzuaZ*ed#S_~mg;^pRkqD%c(+r!%u(T*0k&nG!*mkOV?
z+V0&Jfw#^lzn~96HO^y}jW4J8w<7$jqH1y~ysSlVhwQWtS1usz<D%d!4)X!{hL3h#
z)}jWd5&-@KO`9d3$KCTM)0n)6@0htt6x|xAGV8rnQPE!UP7#%PQ;P;>v+zPHsydJP
zES#iT(7DTK@p=(?N8J`m@){=*KQvd-p{Br<O8N$!f~H?qLek3j(d?uAm_CL=Zc6qC
z^dK)4JC!aSvgAejLN}7u)Ik@H-7DtMp~iiD3CzvW>~#m9x$s1CI8up@K9MIAAhJzL
zfgcN{(6^DWJFP0KKa~aFLEM|3Y|v7W*c@FiW`I*ot!p=jvQC2KU*NidCp<wD5ED>N
zbYce`yysk-yvh1@1xEksIMK6y({p)Hjc{{6z99ymOk2F-&Bpr{et+Ie0_-FF{;JoH
zo5=y*%HW%v>~4O4nUg&bN|r^*?q~1?PPUETpXOvwnPgn0jJgcyKY6&oDG_<rbKHBJ
z=XQR7o%3uqdFb1$KHS6!E|ro;2v`$$U(35JB6J(U8Jp;Nu-DC$K%7A%Ww8%p4(?a<
zEjw-8X=!6KM+_U0TX{DI;Im?yQdc)iP6P0-V&Aeg_~OkqtU&%E1C-)BcFev+rPzlQ
ztC6CTQ!H!FDXJF+fa85H<5;&)rFexs!!B<g*oEYK=d*ru4L<9zWZNtRd_13uT?d$=
z-(~0yKr`{JAaP-GW$sMa(`mD(Brt2+tctH@XTCB&V;3vC+lqsi>__IxX1@EER9=J{
zUb7G(VnG8G7Nw6~+YCgBESBFElfqQdj6PJ{8<VRTL}LP-t@sXah`E_E@U5-)$%fAm
z{?vR5-i8gMm_*SpQH+Hxq*dK;)|ua?>vcNPrzz6#xwfzP<ZTsB-b%4&K6SvV=7C4V
zoZ3p6v`)m<<g6DHgX>?zml*w7>nhmjA$BXH#%6d{M>Q9*Y}$aId~>mlkM2Izf?ZqH
z0pN#E7l#F(vrp<^AK&iWvGT!HA5GpW&xCj@icq<9(8UA+S4JVOpAX8-WBq;eYJCHx
ztca2bf<{S*`7MCsk0uOl<glfDx>|knFG~!<P5F%H9C#h_I#(a`SxRZyAGZ)~{RFqu
zwq|$rTUt~Dev~QRlP%bB_8oP6RNv<EETUg7s-f&Seujs5y9I1Sd8qU@KT+MMVtk)c
ziC2QQJ>i!L<0c|-2YMd&dqs;a{vh3v{z6RpEPT2+8hQAmM%!_?4Ikq5)5_i+le|ha
zz+fd8iy000#!mv6(h}U|i`xo`FI@+>ZYsd;+Md+B=0xzX@v~Ryk1;j$&6U;k&CWP{
zLzJp%$;sl}D$y78SbT#IZli>?=x|CVyCPLFr0AZ^K;#H0#8u)APqvtgqXyApS78->
z@8yn`>U*W|1IKqpiV$%XI2$7N#-!hk4S*q}IE(XTjm$w@u#l^AWy|#SG6&QzTXe(U
zQTRIrf3M?XE$FIS3fZUo9X|JSQz7-+J1x^s%$+!Sg@~`CE-ti35_h==DWrG_Mnrn2
zdHE9ix*{A~mp**9B|Wzv(|&RUb={7a+K;}8VHd=6xa6VUOG)@G6^P#BqewS#0q<}D
zH*o>)Z~+L<!9Cm6;wXHB#3#BZ-iAAwMP7qAN7h**@L9z=e8YtdpPv<A;xvav1T|dR
z3WpKN2KgDCUJ^I7N}obS;!`8^BPWqZs<m1f{JdlWz=x$^|EJJz^ek7Q(09@^`_7t$
zYOdj`eZ^H<RY;ugtCm#=UsXufm9JXTd|kyZzh6;P2q|DJMztKiYFR&h3MZjXM%G#P
z;si@9K1s{wQJeV^YLUm2%7#+G)#{9^L#ZKkg{-MRf?|Z&UI=Q4U0k>p<NSOf9GIKo
zc3Lnnd^X*OcOtQ%PgY>WGC<4WsDm%Matx7Hft{Tiih!03pav>sGodJIuob*75O$Hj
z;mQ5tSl|6b3!jMbap1#+bvpoV#dqa#uUTPoA%F*4`nE%hq7PVz2mEx$&<^@mIHwQ7
z%F+(NhzbN(v;>(%qGvJT#0%sI2(w3<Y&zF6jMaM_*O&nNk&m8FBaW;}_$(ipl5i^l
zsruC9yST~k6;}OeO}?bHYVsZ|$+JRjeqFa2)kOpAvA>SO-w!qIjl!UAniz|Ay)cFZ
zw+CC2Ec1L}GPU=<mi|?yV;;lU(fZxqvO)e0Nxr#|ra)!u<nw32MR{#XM!yszxm1Y$
zWNj5<PXW|Z)lbQ$pX$jBLj44z8(bWe&CSPsMm9GZOx;oXP|o`;YuCKiFXe(vJVZ?P
zwy*moVZ%<*;W@a7ku6H<Jlg&<ko$5RG~O->WHlO;ORe|_H9m$`1!QSl&cx|O8~)jU
zIzBpuJWC64H{Et}VL+w<l<A9>Sgl1Y_+M9uPo`1Rf5fFOq7|&l()Tu1lY8L#*2z<M
zNqiLumC9Vb9hHkQzlrp{A<<bT?mK1lwPH#wlyq$V<<p}*zk$~tEgSH0J*pkP6toUU
zEw72wBHSnWu$6GNuWgf@W~a70@3M16O!Desvyukn;s(IuQr9iV5fRbGPv6yPlXy$F
zKJ#xvwboxq)Z&NMAo2M%<7MWU%7RMS0iU@C;m38k^Qq)^WHNYs5gMI<91UPK&BX#(
zXNhl|pPad44=`+uQ71hD-Yb_49dLZoq(O+>%3SGQ_?k=-alUib;VO3Dx=G`pNYo{*
z%_;11S5aY(GU=($XsIvFtfXcT>9jKGF@_!^@UZWu2NNllsmsQXJ7+A$>FNA{crx0L
z8sZO@#z$dcd=i$%M{&0zSsFJY?Bzv?_+=08ADe@$SYr8nqU!*v$@uf+g+xQ2Xie6m
zS1(i^5x2G`|A;p(Brje{`%s$((V?z~Xorsv1rlzYFXPd~&*vr6U|-diG?9Jth&5f>
zHZTkQw5OnU8=$II^<q9gGo6?wiqL0OVm54i@F^7HW1JsFf@@n5efzi|Knv>HR`nIw
z%b<SVK988tBW-<8KO*~Q5H-%HI)2%PxYBAGZt+<g-$-{vS}FCFXr2h>Jc6=Zl1~-d
z*T#1pfXnl#xbHasC5#IZ?kts*WhrM_($;qwfJ)A?pR+9GEQm-erSDW6?GpoP&qY0h
zbE%9g+L5e<8$+`{-7<&@n@5!K+y6)1o4{99UG2j+XJdLq>p+;r7H0-YNB{)^HH09!
z8OZ<%sMnC(Bo{J~+%VV{B!HrINI<1(381#MN)YvJH36+-0k!I@1#xO?1+~rwwA#t<
zdDcGD9fG!h-~N7|r8j5TXAf(yy{5g^+9$BHKbC;ELnl<t#m#LEq$}^90Nm=Bd&$I$
zs}cHz)h-_gTIsdFIVJ0YNvq6vR-*JKDgE{dRp*f(-7R;ooY1D4JJczDbyuH`q3FS;
z1svF$1E$olmK}+$_+w>zd<%-J4qmV`%nA?QizBvJw)__?dMSUp^7%kDv|9QPEr&+e
zjnI=jNGJXlBFnpSpdRJ1yCp@dotk_y^m_SvagX&}IR9ID#`wYNQrSa#Z$YcIKJOTS
z$QjzyA;gg`8Rk7|fd0G&AvXGrw7!=b;KNK+qtuoLN>6`0!JiI-(l7fekgQ~LC~$5|
z0~hcgPiW1Zlx=c_*eUrW3R@y5WT)yCzM{<{VIHKD*10AK@o{QE>6#^~Wvemb1ap(R
z<7n($qOJFW#JpRFa_;V~q8(%35MJL}MWlOo$Znh%57}$}?!##RP>==MhWgE196$EM
zO(@r1C8ENR(rmSO9|KoS)mBB^{RC$9#eQ{Gx2mEzwGo0Kp1w82U$zIgw^b2k4i52W
zbFP0<B|`DLs0DXdNtIvX=Ail;&Hz*D<q&JUcc9-KYn(wF!yiMkoL(A;?Xp2?ArDx;
zFT@(3ADArHTT|r5e}B80-KIuWsoDF~Ir~yn9);uQLTzNW4-E7_U>;HfX*RzE1UIPz
z?;eqA9+zq!2}w1N5A<K&ZvILkFPXVYeePrVqm${rdbE1026X&rqU1)n>@8`9hEuf(
zvqrQ`fUGYwtb^ja0(fSaoEt+U!;?T*<Lm(p?qTHGBgSa%L>tDrgQZLsLkS@%$^M&R
zRurhkVGEa^gF3YzpxRYK9qqwE-8hvGPJ_wn!UJj-L|^K`1}rD1a<kYlM_p=UCp`)(
z@84?VCmHzz@M(7q)qEsv8$>CuyM`-u_aJ?yfXd(94o+~HY&EOwHe|H^5)ORp0H0ko
z{!J*^J;>jp&e{zjo~pv_D-KZN#)-d136bhisqIN5^uo=~5RvBDK{TQ#-<Ui4-tjnO
z04zupx}#NhHCX4-oZpAx?szmz>=g9wsuq!C|6m$AaKpKM6DtIPG)M0)GcTOI_NU8N
zwJgBs610Us(IlTppp8yKi<Fq~RN^Igpw8vbeL8EVkw}|>agaae#DFoY|I;lG;3i=k
zVwn=30W7vwv$fw3_E($50ponUzc1qbE%N>y&wDW!z%XNNStYK83Gam};`{a?J#x)O
z@~a^{Givl8R%XuANm4);QrVN2_;Ue&9vshKW8?t-k#h+3;qt88Fo*G1Y>>;(k#cAE
zm*-lX*S~i`J4R?ESon@+G=5VxZDTJFA*1V~epzKq;IzLngxvl^U+W+M`2HXX-g85R
zGF^><nEfjkS7Nrif~6Ofa49`A6taBFZgqUAS~6VqgVHz|-`$GuEM5E$L#J%K2FE?C
zliRXd)TP~A`>DVUvl#tLzyZNy6P`8mGw;AxFI2PXCSxZ1@yK9OyvM4^R~{S89WPV;
zHh4e+=8&G)=$L~SPYy1{#sgxgk<7HZG&?JM&Z?~JEWFq-xK%AU0IhJS-lR4%le|P(
zY-hp*R<6_<l1{4LMT`&9Wnv8uJT}$eVQ+z`XZZ7Qb2T<-arb3H$LySOGjp3B#Iq|A
zsSO++YP1^(U*YBNE1iMgd6NI0$6MY?F`u2&VxImx^^&i{+>EsMfZm4{oc3FW9l$9O
z#<>XU_R;C&<`q~dWAxTJ#K7HzPjJ<|^^2ZCEbW{!b3Gm?d)HBYcc09!D0W>)k|u=3
zIA^eGpgM<ea*<Z=;k6#9&LL?ert{IK^B~1wjbfEZarwG5arGv3(+cccs?=5t=DHy1
z*C#_;*|J_3FkY7$=Mc3bKU{@TZK@P|)Q!W!=4s|im0`BRma9fU-!|0?m0ZZ%AuaNn
zqqnaKzwOxLm+WocN?}?k-+ia~)3;gokL8=cpF|6aw~+2UExlLj);~Wf{EQhyE3Z~c
zwt{O<f<AHgGB|sfOow+sq=cUi5Ds30ia%Yzj<pS-82j-8E{JslG=k#6BMZn@@v<2=
zpDbWk+Xr9^ecuLl<4Qbyd4Y`im-79~3l2~Wyn6s04)#j%tpmt{e!oEaa_;~!U(1W_
z1FFRS^1=d<NAH#j_AL<ZwGBPMM)^{3kimEW+Pa{X6?YGyspY8!VKM1EJRtcXncT%X
zf?5roqWbQJ0?KZSm8Dzi{Otlld-DLQ+^s4Enz|<eGdwQyHGt##5NEh&;FQqqYA_Kh
z!JLIiY=HCg8b437z6RvnJUjohSF=){!nv$t6SkNGJ~x*h!b+z@3IvedssWGHF(BNq
z%Bjn?nis0qz^Y(?HOFrsr3NR1xco~nsdeZ9xh6-sawv{QA3eY%Mf^0V4}ATMq=yev
z52@oiP%ku&fjoQqmj>eJhns1m7C=)%x4SFIIi4IgWi?!O%&HVs$}aQ;XXv9|oKN}W
z(UXl@bgET-wfulFV&EvDj95GH7qv_jYbu4#V9bX4$4M|^$kpmfbk2H)LtIJsjKq79
z{7Np6ka}c4xmtqXgv71#6(aQiSpm+#M{3nD{Y|$lzr@bSa{qA3t+vZ0<`^Req2F67
z#1QxAVIaq=asSf_&4%9@W*#s%;K4fU!KZkD+Z%NGzYXJZ{a;@z++ZnFh)?W|cz;C&
zJWesTwI{Q}!+3IdJ`woZlVKj?O)GA$J2`wcU%e-<Zq~2flUEx~X1<HR$n~a^*~r=a
z?pN~(`qd{Jv&l66F<&t47bjzTYBl$#_RSZ8ZTrc_T{yi6%x*pz$o7+YzW59CIq}<1
zZgZZ$Ka8HNXL6o#*j-pn<TUb3WRy5KUrc<@59?O{^dU-I^}v>ixqqTg*2u&C{N|Wy
zb1lv>Ui-n0`(@;MA&6ct4jKAv14_}J+qt|wk3Vwdz9p5^Ur=7w&K&9l2yTF1QRjAa
z>H0d3eN}%f`9A8O3%PWzO)6C*cEiCACfH>!8EHwz+5PeI>N-jaE3pxWo3CoQ0@oz9
z8l#7RTd97n#<(OnBO4bc!7vXS<p4<Wo$4f6e~HNN^IakR#8#2lA!L2=$+knPdpS>F
zM|>IPVip$I%lp}5k#fX6CP6qt8z1hojx-bUC^S>vC0+})5)re@%>z4{%gUO|<~GeO
zYo1!xthOe=vjUIR*q%AaZ;^sbWU`H>H@8?#LWHY?w5*A5<h#<uHJkpzh1bF<e;X3G
zp*s50?fKc|WOZl2ok>2H74>gbeC3=n^z3cbLdkNVhSC3HErY+6T9S5~{Xq<kGY9G(
z%J@9s!hNJ~OcdY})kpyZt?A{uN1QY38}~|5$a}e;+eT@khR!>0+^#}|PZQ?yRMO7N
z;k4PQ&WCBLFQu0k7Q$ouJs)xRw@GC3-L*8;s+W@XO*`+}g1WcLx~k{pjhwmwo7*^$
z2llTis;*5X30ffl$*UNfUlsghPR`!!pPoP2OoG>4^1P`bRaFI+FYJ}#;`!}!;Rn`6
z=c_4dhS?9nMFY^QY;#vTj+}0s8mbXsLFL_6Hn&q{u*b)myV3r9^{8*R`l^mb*TX-Q
zK3=P)+>X0XBy&ic^51MeOiWuEPF=HZWJO`!`&d_~PKkY6CDPcfCh>>zGfV<KnlvPK
zCwAq`H7DK6r?@e1$X6*)dey9M)gMIDJO@~c+l<!gO`jo&LhTjTE+|i*Q46P@{W&vp
z&R!QR8zU0A*b%M3*BzP_sE?UkhZU=K)hc4QKrDgm04Lm7r9N~@tyitTxa_JcwlN0b
z))m-uP+$5$6s3l{u-(CnL;MjXekyi&hvSSGk;mJy!7^N>ly2H2bqi9-S}#rfOrCTn
zA_WgTe-DnCy2F?_81BEIubpM;RKoOw9|#@2v5$YtmA{eVdRHgW%;Vie{#>ZpSF61C
zP)hE1!2eCqk1vhHOZjUad`4bC-;u%|#b>n~3JDADa5yvqJ&~)%e786)esS3~O8iV+
zeBiaLi5LDdeS^7&vderBVk2cxy4v#txIBmDTV$ntXVW}5g05BNRW0g!*xQ@}!`v!0
zuoVGtt5wogRo}X~bKAB}+cs_9eEa5YWopho%=Y*M4O+=3{+Cy;{@tV&@d}e9#gd0a
zlJr<thiPq9iP{?y8N_(UM|~_Slbjf-%fcjbT#pL3ZfgO4PsG`jn|}7A&0AJ&o?E$i
z^$s<&XWqTWs1sF72i&hIlgj)u0sY%8t2-4=#W)sSYui8X2^k%c#z=gWzt#~c1^PM}
zpfxV+H+-6UO+mCodSr%V80y77Q`VZRF245m8aRxF558@FZ}rl<(sE@N?DDod>&;W}
zztdQUxd=NhJ!&{PZXIaF(YWB2osmyH{qH*K?wX_g?~I<e;J<+$Ctz|_qhFmcMKyM)
z`Bmz4%7P;Y_J$+iGit8c&qa{qTGeG#qelQi?m5=%7JvE&>q+PyKhYnkzW4Ue&7qlh
zJZoNTW&@wjvh=LeA$GvsunkmaIK(L2ZNl~@@G}h65l%92c?_FKuhnC|v;}ZevP-5C
zDJ&plN63wF2K6DGh~#|DjuWBL^)a}5j2-;>6Rf>_<R#uN6QCD=B~W_dAvFX9&`O}<
zzIU%!5@sJD`JE}|8gtDg(jlFdU;Dy(i_UxI*@b0?)k3bf@3yw9WvJUo8Gr}}5ZN^i
z0JxbnxeT-Y8Me@P;2&j}tm>65W7Lu!W^cR7IBnpJ3vX2|aD^50kkYS8>HEH6>EB3c
z9RAvS-6x@L^y2d$36a0Xm|`v%WuA^Fn|PF=df!?l>mY7JF**DrDrq+ueoz#k43Z=s
zHwBv%P<wh163Ki$ei|qa!+{A}1{4R3WD5;$=28>hAS<ViVNms8V3*loW`p2MWY-?u
z_=9faJwIaOKQZ(YwHn_gjqi-p_*2q&w{CowH2%OhZTzRY@dIf5!ck@(M*fsE{uIyd
zI!fU+1^0K*>UTNhX8fH3<2PR2Dy<&WtupGAp0cJNLxwh_$!JGnwfT;D<EA<7bEamg
znhx1VGDheiuU=wo9<|*!+rF33r?0wmzINYyNV1V^gm>NlWrUZ6VSRg1T`>V;|52)u
zlq2LVv0Cu?rsbp;6$Tq?5zti^tPhLRrsJ1LH$?aq#&fE~V<&D4!CK*EjlpIFYaxVR
z;=zs(w}mew-5~LR0aYiwCKo<6k}BZ)zGN|*C4e*TGuO}`<b7X<enk4A>Z`;!mXKI&
zIkO4nqQs$XM>k;0+q_-5M_lcblLTPxHiy#;x73e!t*Gc{5?O>CMOuIvYUfbbxtft6
z%Mu@vK#Y|)J1uPF6@>=lhB5ch6$Y0Hs&uP;@QK|qiaOftNeXOy#g3JtaUOwy07d!g
z`@Ob0=7_Bxty{gz%B+9LK1}IQWT(5>u(a;&^j@r610A;d(AVz6AuAgkkBk(Xzjdlt
z(5dAsURgumi_Kj<^ozGfO^IwKxl>|(zZnK1Sdz_)qlSNcbki??qm~Ie^Cmk}J-geF
zj}l+DyhV)B0zB8C7}@XsFjif>75-qfzD%(3ZMfju7CcIu;=#Amqw!5_*zVv~(#0YV
zDoZTJT>y;Z9!>fb3+3j~emBj=xEFl(=xvnijGVpyXO#Ptee=g4-QJjn?K8Nb@^glJ
zYxrBhy4zRSX!oeoi~n=H!~8@qyr*JX|1YARahzspf3kVk4KP5#(QAd~U7eB@;Hm!x
z@B05`LKNE--^vEy#t49s#N$~ys5mCnAEm#JXW>h`F?#aj*5mh&J7H*6zAmdkB#%QG
z$$#~(#pB*gNYF`~camWB;={XLtz6OCcHz7%xV<3HNn6tTsTZmBD|%8Uo5QP9)|I_2
z#I6(?I(7h7B!l^UBu0{TD;Rk6iI0Lcq<49LgS`J#-rpea4>|9-2YM||<Kd=`m|Lan
zVJUkpS(N&#uN2D06gE7S4bu#1JZxE=Kg9{U<YJI;nN&)yK9y|1>k>*?vX3qa%dAzh
zkJ@0B<TG~mR$Vd?WEUSKsy88mv(7}diHW_eXSy9e)9tVvyLU%IDR<63Fh5Yq5Rwz7
zGM;ga_UFw!&}y#hy!Y&FaDBu!2(0!`&m4ZfS<`v%dD}V_PE_P0bNAD{M-rEPxF4Oc
zZZ=v>c$hyI@h6u*qZ3PwGxUP>4aTCxe>m=t3-*`G^@2S&-v@cA8*}`BDQw0_hu@yy
z`Cm>aV8_SUp+d3@(;z!WEh#G;CMH<O`Q9qvQUFw7zjWe`tR-uejR+!gGY432<B<<c
zxg+vI&b38eH1&1l17Er=SfM~(dV=>JdV;%XdV)vxaO}V%`+l1J!2KpF*+C>FLRAq#
zN8dcg=&}zj;`P*b68g9QU;x$x%xC=F%kz|vjtwmO@pl+lbk#ERz{3@0Di*n4y}umW
zE?6>8N@CKT-<d*X{t1Z?dd!Is!lMV|(Tm?<=|1Wg2t7DTUA}vrc|P+n!|~ftd%q()
z6?ftl8mAZAqVPI1=g)jkZ-xEef#SJ(GlGmBX2cN=8jl}wWHx^E5nKxyTVzESWz&e_
z-04^&?3N(p2#s{=fPoLtI#L+W6!Q#&kKR7&0J`zaA~9+0I4=Apihd@ZlRrL=Hb+Jp
zO+us*>;>4Cl^1Q+i@C-)x&a+rB({WGj-!(7lu&1gxy?K^5gW9KGF`;&oY&#?Xaxt|
zzNmC92YVcMrH<iV?IVjs_1=ix$kJ}yKf6eLl`Q>)hcO())Bjnt&%7BQK6;#y&WNJh
z7qz{N+dm!$uN=#$iW4D5ap%8t(Se=j6+7{E%W>l3{>Mew{Su;_px!)Ac6&F$p9lqT
zXLoGlqBeHyzT<RAk~<(4Z8P_8+Sa*kQ|Go$*f~4pKkE4peFq^hOVsv+wo{sQ5X_PI
zqZq^gYX!D*7NHP`i9!rx)$kZHhu_|q*O=XsGxxcg^J+r-_X7*INw(`30g*(tRdQAD
z1zI2oX-QaOW^`JH1B{44__GwgTMGYP3U^B3y;kAhNPx`;D9rX@`Y(eAc)Jdx`73l;
zgwbG{f_Qk9gslAS=nh<8Dc4^eUCOAMuUcfA_Nu-*$mXwd7M{Any8T$+Zm@3O(YKp%
zyR>m>+2j`dn|t2knjLbbaG+D7`qSc`L-aO(&h-D`ifunrQ+qn^8>QiMi#}vV!v{qV
zFEQH=NlZ=wO=eiOCjP3)J;;^j{+rjM9zggZ0&qIFt2IC=lU`ki#EJ8|<EJd-(9H|2
zSQOcz=*la@!b?_N2)i~1m;<o6Uxicwd*H01&g?cHn%urw!eox_fTn*L5~m-4GfS(Q
zBbyET5rrbq6q|u4a$PxQ#RV(i0Q6ELcYOcn_$fwek}>f_HKPYT`NN`CJlu8MzCAE4
zZCONPB=Mg9$NTW)J@GKT`FK%-U$CP8CBKHT3uuiJ8y0DIfoqS)=!c`m^%z^MGxEQ5
z6*xrVX4e_{UGYL)bpoA{2Y8&3k7qOmol`z-5C<OiXVnQ)wqRs-*e2x`!{o-lVUFgn
z_-9==K^_R{Ms!hMj_`#Bp*|17mc5L4`@apPeN3v251!zE$DCzej87yyr>jAHANRtS
zwTX*eJ=M^2L5afYYTrc=T`T>oo;@MF*i3=nMk|si(E7K`pMjixYEdbV#gwEGYB=OH
z^#}OsL-ItF!O5Vbr)llooU1O`r_LrR`>Y-<ASC3(-EQd3Y`u%JO~QCs=qC-@>EO!~
zbSWhCHj~WD#Z%?x1__z@<b*KeYKDM>Guq}*y+qrrKT)2B(DWH$4$q3^`8`suaNizb
zmi29&X#egL5xa>hu^Gr-hOspfrkQptOzZ&N$F%_1RFrE=s*<fi+hdnoPR8C`Y_kfy
zxA9kuK`+6nd>vcdA^9i_or?c0%O6tJ?6&lOv9NUMNu;oE2&&4@uM7G8-?YWU&~Y(c
z`Q{IPhz>ne3u?m-?MjkyDR})_h8(fTAV~9gKYUhLg9_Uo!SV{8zf+S`1)W^)s%y1q
z{K0-5mLr-eLo#VlNqEqDroU4M2hjEM`PukL-8a$?6nbGcG4i32;YrV61>8D^w&}M=
zz&W8K`)c@~md?ZB+~)Zkc64R0Tzp;*!2Qf@^7UVh4Cg+PlXckS=9YbW@2Wn1rMEwx
zGe!QEBcxa^){m4$_cCwHB&*alb}7^+rEb)v_EfX8IxP`AVCpq=`sqLQz(OwOf6f+<
ziILb_IeKgM^XO^6dB&vguG`SrHFf3cv)R#Y($N=3Qs~gvdq++=mwi58>KCVQBxtg_
z^AQ0d{b&bZ^guqIE&W#IHjuY_2V#Hu;cRQT_CNqxR%8#vpZK;}h8=;v;Wgsjqw@6X
zFh-sz?J*{s7si<<!lxB(449(F;MJN)bJP=~!WixqGTbXhp=0Wk5?aF6joRn+lipK8
zcyy2Qn`^Y6bYJyO39a_GjB@;>`>MA~ICr;>(tgsds$Uy6F;V575^jBTje^^Isk!Ok
zAvO1axfo!(U21)ER2W-An+~GjZ%VlN@z^MTS?9)T+&?T8J~7Iltxj!K4?;cds}7Y2
z?YRzxNQgfzp~uP%C{lyFk4nUiXB~QlyEmkZci`rrI^_T!JY6D-{@3`{%s#XOPkt+(
zV_?~farcCN|FFEr{e9A(=h+{`dZ>46%zhnHxAaLAxZXhb7`f_6YRxvLpBO-dNE@Ru
z`*Ich9hvV0#6Rv&SGrZTgy27uI>s7jQPOLz72eU^zZ*f#@Dclgx@!epxu0Ih@%*|!
zH&C8lNDx2RKl#0OmD8riSE-zRD9!2~U&uAqia2=Oe7dlWaXfAP{d3R5?cc?DWnF&;
zE`GF-%KW<i^v3&Wp$Miw>tAI)u8KguPNtOkvNZThY4Bxf@b3P}_v6JI^5Xjb{!W-`
z5%^dQj$DRAuy(5{-0q)-+<>L+Dji;T>M<$$SpQbgaJG8$LK)n9Qt8bLJID~;>mPn#
zW4pQ(G#@3l>n^|4Kg=F?OS>;gk2@C%6M9Lyj2m*|{rwRPi4A7e2yy=?b+xYkuTuTh
zy86HN4_|J6gacuE5M6r?STiMzix{0s{&3Gi&3d2bXtt||>8|~l4=J_OYsbLRp#RLd
z6e!>rhX9W3$z`_|;=CgIIlpoV#g)5H4KGwUL<OEDIfKl1ff%bUr9A!5Q<#U=UnjAY
zLZ{_tFRHSlCDGxnNQ#h#ENlbu+2Kq0*|n#(B0|#t1GTIwb+UY+gM2^re0S^tHP5fk
zNXCw-F>0_{7)DJ~cz<jM61336ZPO(^psy0Xa_H3XWeP_x@S!lvH%~3Srpz2-TsVl1
zW<8IXGv>`h#Mf|mQHB3(-6X?vb@h3Ol<cb>kZwJHDwo%Nmk{TkKebJ+Nv@wimBPz?
zmk=+XKULE%kaym*!+6F(NtPg~$Cov)L&e#LCGhmcKsdNl^ZA{pF_HpL+VDhTK5sjX
z?r3jZOmcI_X<<A*crlTB%V}_sS!H6LI3<|CRyJdW9L$pI&-hDnZ~VGI9-PA8F0n}A
z1JZJ%>k?x&xtk^Wbmhrg&GC#bHCT3>wfHkn4FhgJg2J?vnM;lIf&5TAR_wiQ76OL(
zPAxV33Gh!|O?AUaBoQ_eWj8@~Rc=THX;jDg*QpWvIICc*Mm>|3^^9p@QR6h-ofW)E
z<<E*0pccm1LA=(1wq{n2+-Me?H4irl@7NB|pUJC}Ib!Z$$tX4EfSLCN(KWKU>v`)u
zQrn)>IfK{F<MKItdg~Qw^3`__Sn_i=>*|_k<bhqPFXtK|;`!P$_{|q{IbYYD(R#%=
z1g0KPpXk(a1_Jh^17mWH8U5AM*eW*F{g;5M@hQF)aoR99A|8s7K?&Gw7|sX;vp`)g
zkrt25_3uH+-<%O{Q9GCKe%J^N5-~>l;v)YI6{<6#4UX@|#RH8C`m0081Mg`H=VhJV
z;q_lJF!~*E)!KtyclHPYB+{cBPPd{(_DXM%@ze2wl{2jj(=!v-V;`==yi^~z60`GR
zPOb>38=z%G^y7)s#5qxWQy5>t^|##!4a7e6T$~?-<Xi_)`VSW)=)~&Kji(DHcd6HL
z9HCK?M3?mjvSwoofA*?r9qsUjg<`=DzYVc25vffPsL3#upIt1#d+oGviwq|Kac)ZD
zW5~R|8gC!JSb|u7a~gtJ4kH8DJR=vGSbeB!gE0%C>PDg44rb_8<fHlx8-jmbmD-O}
z3*qO%bKy+&+Oh-o0=-7^RJ^{d0}oClC0L)xHT;?+tN0mg5l5>J@v;x&@^6!}FF|cU
zG-V>fTEK_<sE?LWAXtZcByV7(OY0%H)<Zp;Gxd4MEUQGkGprrDu3Tg0K%=NXVPWpz
z?_2zlD6`;Lc`8eu(;4xU0PzH{&(UQCwFIlAhLx#QJ&rG(cf}>k=bcxk>RQzht^bwk
zz)h_DL#T)p!2bK0di{9lFULX^@UO+l9~H0j?tzSx+Ak4Y+XhlkdQ8IWo*GDEw>Lyn
zR23<-q;z;-V8=Y<;R8xxnC2n#;GDA656vwzr<$+m6XSJOlwCLrZV*=EWRenb?%Up#
zy%DSN-H?b`14)D532`<5bfBMzn=meMcb~qA>dz95)FB1_EFIi}0x13G5QV9qBQ<Yd
z^{Tx1rSoMszuYU;-6LP>>uo47Vb#+X7=*SE9t5TP*GiP8&ghs2)K~DN#-HY?<}#GB
z$h2%QYq0avRGwdyDn@jiy++%Kk@yUM-{g;w!Ut<{<e0i`MK{9HwB!t*(x%pe5Oc@p
z?N(1HAXK7yDTp;F*k~%aNP&|O{#8WIUnAo0X!RNbcmyHu97^u;dXNyib10l`s*H(A
z$R}X7o2xJdGbyB7M7OKvUb7-(&4v)8U6-hUk(HoMNmhX=>Z~?%j+s3MVEiITW_9&Y
zg7mLa^|eFo$q<nC1UXyR59Mt2NP%_~kegP#xOwPST)!tTZgIZs;ba^PQZ%?tzC0)e
zI#6IjdoPn=XRC{z9OTj&*ve^v1;-pqA$XVcxoaqy`>r6P>$--vnK$5WHv)!uw>1b?
z*D!R09+kUQeRkAt1_>h=*i_pC6oQff=z5g=)o_SYJgY?BkSYMfdCDK&JwLqaZCu|Q
zKvK#+NOy@xzaGvRcq{-7nN;BM;eK-f!sq!91fc9`^JOf8Z5NlUteHEhy2f0>C2|@N
zNhanL{&op7<Z8qvsN9BtNH>2OPT_b%0JY+kQXdW{N~vQ0&<9i@(r-NJoprz%_vhtc
zGsYy^Yx}CMb}p~S0-TpehjT_B4G>wL7;euf8pFiw2ykMak`g-t9P4hBK=B>&>F<Vf
zt{;#WFF2pdlx~wRUz7sdr0%^aFhP9<c^_S3Z$wUT{<@QSrFwq3xfSzhoC#Wq!iJRx
zMvDhpY#~Ul(jfU#KysA>l79`Sy;&9UCm+RD1&pcW@Lvq4zOXhRBKEt({qV%CvfOeF
zW2{I$=AwetX45)zZ>L1w!n|S2ACyMme<{E3@%O8F-jZL+i+242zoZyf$zQFHYS1_H
zi4Zx)vgx=5<GdtZ8~y?KW5+oXH2Q2f+Y{npJb?)d-MeC)`IYV-Xy8%&Hc@q4+KO`(
zi4nMY?-aO4la3gP>-AUsHO?5!KkRnA8h)&6Kvc{r-J0Yfk^y#Ae8W_^NM#l0itH-k
zXROxTB){M1uh70Oz7p!Dg;!Hv$!vC|X_s$<e!NJR%5M@ok*=7ZI20Erd_(`)q?YW%
z(d#>!r)HG_Qbs1Bl)&U#dgy8=`rVqyK&vNC@o!nxvNHR(>n4{?E?X(srK;&EC_qv#
zI3@38$?TOac1d8xvzN-DsBfMUzTFJ2!fBiBczD~Tq<DwjXK+mDFh{6Y`!Gt!R6S*)
z>0N&&-5M`~TVpH8!s@x=P4ka4NTG3qC|FW}LuZgdUN@H%VksZ|9S;)Kp)zVN{h%$Y
zdI}9EaTQ*1W=A{7L#KJ)?E8Ou*Q!;|3Sipg_Yi+&Q4^Mma=n_(A8mNCqG>$gvjPYC
znZshd5;thu5muf~wfu#IF};K$nlV+3<E8lye?3o;5`j;QiF(b*q?+dz8khDr$F|U-
zoN&M8XP#(oVM$rC=EPY>j+B5|iG^N=Z$D&Q0J^=bJ7qF~bml&Yd_C#}>_D@`ew<0(
zFYz<bnC)e>AiQuUqIhQnj7+I!JBz6PJ=hX3rVU2O^!P(KgJdk~d$rV0mw;cK6}}5v
zpIDF(4~*dbC8+9erBvpwJWB@z>#FX9&pB%Y_Bo8-xmXLLdRr+8;lZ=$gnt_x#tik|
zS<sz5@xP=wi2oH^r3<SES?hRx9w*)sdyg0fJb?ueMK0xp+Q=GuQ$L@e<wLC?wMdz}
z0`kRJ@0(v17&DUOafVd1g4cR2%3QrT-bfn+Rs{V$JPl@zWo9X%_RbuE+ASl(Z>s*?
z_y$CpUy2una}|~Lbsj!^X%3>B0FqD63?mnm*(za5zuwWzD&U#`VI82)1?b-v(BJBU
zzDA=9(v=|<GUpoG9(~nDne4aR$wo22{ST#L-@1DQ*R)dm=IGexFU~|7cj>KYwVJ&+
z^LT(GVCN+?D*6gc2cgU7BNIu2QG>laUmF(UC$q)wvVLUM#W)_j9UXdT4p;1+5&mm&
zvttg|=<X4;7dkK8WE^Hs;yQkFuYB{O{^ox9=6~fI=LNEHAn^dU0BAA$q6F3~u~}a=
zD*SUAH2944r)`vf(;5L~fB0h$LXM8U>Z;ikGX73us#+sf<}XK<(ujA{Y<BLWkyeh5
zrBj)s<2Xwv4VVB6cmvXO<W5>;oIYSBzPLrc_+Vt|dHnuPx%=Zt$<r~sRQ(n?vj?Lq
z|0)8tqRn*W*<tqOG3ijVHqL%`IC9!NeZ}G?;KG{X(hDkR^7`9of%-LEQe*YIe->Hs
z=dEYA-SNn<8&xP-ozi0toCu)5QA7~mad!Cj^^C=6grC|8*c+kSH2Tu|DT=&U+&Xh{
z2-|s|B131QYKLFUcaY(oLU23$VR%iEY-hp&=8Jkv3Oy!;ej|k*lS151OjPF|xcY@j
zNXUaQ0Cf6d(D6iUWp7>g>ZqlS?al4?PTB%sJmj1k%+2><Rga>F*Pac777D*sjJ=c=
zq+lvF&q8b6I8ZMU2T-?u#o*P$l3wpQs-Q@bl%8Nr>W@#?k2b3w;aDFjA~AdX>?)4+
zfNBK6^9&W6PKoLPjI}0qtjwbrHCARRj2tTqdB^&$@4$xNVI|&*W6BfZ7!=OM&3@lj
z+}tIOP#Yu-^<S7?I{}VKETx^2=%mDJe@V;cxkhULL*{67{G-u+m>Q|+eNYUOBnQu}
z|6K`fU#Wrv0u;a6j>DV}SF4kH5P*z0(BCtPNrT~jfI%JIjXXratBxXcgLw7k*?8Dd
zmaCHessMWDSlBP@ZL_kro7b61$h>q+mHij;@;yjJm7*ijxQyq*pou^u;H3c@H2-{+
zuW$6u&sS*ea3MOHjj>m)zwz6ENQyut0UN7C-kqb-Z<<~Kj8}^A7vsm~FQPZ_@|m+q
zBk`h8WQJ{Hs$jkNS^Joj)ZDq-yxF91%F<jIE6+rD&*gp|P@@_T5I@XJPev)2GGVl1
zXf<pTNYhh@1;I1v`;jdCX-s$<5P%`pMGT2U{t}SO_X|i&wx$?3JqPi3k1wY^2H{bY
zD1RwYb0HFtqpMN;xzGxv#`0}3CM6l?>b)fd9Tw7xO<5DxjxkUG<m#nD5FYDUYz+1$
zv)y7zjDRvu(=wk(B_;y3Mc3F<NR8~lF|rAYL)B3k&W300kZJc6a`S@MC?Rw{v0E$v
zdz^M38l!oZ^UZGen}_Ugq_#4w?Olb~8A5wqV;H+>$O#B;bGr(;&mzrr*`G>tyPW2_
z$B?x!oZRZ-Z{2UY?Qf*SyhPE6o+#92_=m@kYCnM~gE?@F|B4BioOfqI;=%ovWTOay
z<Vw9Ui+feDU?d~re&P^3dC7XR-FdRddeZGY`Hl7DNj$+PJ7<ZTiuzqLa8GY0LPdVp
z*Q#!D!_J>Y!L74oO4V!0^cXsfCtGF_lm3`2sIz4j1)e`8Gwd8WM&;FC^s5c>>OJe#
zb@J+ves!I^I&8gKC9nReU#*f?|4L?TYwIjbthzFV0n(|h0`kX~GnvO#g%||3Kaycc
z#_e6RME|`$1@1JfcbM<t(O(KE$!$zAE=^EFTjAo+ty<xwgg3Vq>XC0uHr`H1P>?@C
zgAkdCM9Rie1TrFv0Au;^%a}0;>G<ZMSvrSFdolvcF5i7EQm?58alV2&`hb~^we?&9
zS!-tsjrFe;u+w*^Xlo6BRpRCR6?s?bU@$Wm>5QR7UondxUXjuRCUS3q>{h*)f*m;-
z>%R(Mzmcofl#b2w&Kr~a2>pNt3zwaTs3+jcbyC6gDH2$;rJ$7iU4L?F_{6E9P1i7s
z)vd>O2MS1*Hl~!~`mF*Aup3io33*F;zcGb)_*Q`kRU1?G?SdJ7J-WC~zWO9ZBtA1+
zv?rzRE-2kB&9=f97Y!i0Au^w!_?`lqRX$IVW#kmUv!L{8JpMRkt6GHAiOEP+U9|zZ
zCTP6)Nda-|-4uTiH@6g&A{!g>@TGL9ZQPj}OeOu90$6b7%425F_-j6``k$q6%kIYo
zM8?ljrfkB^H7JL>k1;9GBIM1-`ptGQ5xjXng@JU^1(di+`YO+6&Q%v}g~q0E*8Qk%
z^=viLLZ~NeP@R<m=k7r?sr|7=WWg<iXwG@L1`6H}QR^=T`4<%D&Akz6INQv5=4zPM
zDC}zGY7?)F6bj%<>}tyV9j|}PA7q!}pGN+Oxu%F$RJir&^D}w1kU!WlF~$#)V;c;~
zB7>Y!c&^h5t+);eK#kM;t4}S1p=&+S%6qhRr6X~!F-EeQCa8CpTk6#z)R!<BwoKWz
zN0yo%HY}zJWn>JpT`RQv#iC1%sah=&B@w0wq?Fk&BwQT}CgEr2B=><~mN*?1ryE*p
z<*l{#Fv)rS@q~NLcL0^&R@1iBJ-Ah_Y0-RcFbE&Aiad|gC}EfLP1%f(u*?y$BJt)A
zK+izopVSaD9?^xVeC$?F!&Un#eDj)*BxT~hpM0e{aEsilAbZ`Q1ygpaLAz0ea{qNW
zr{k_4a@+Rs0-5P+2M|RMGv^!9#vD-V6JSomlQ%*_er_1R1LHnhfb3vI;td0cc>lYg
zYTe<xaoah-zkcyOJcw%X;q%6-qubOH%JI)GfN{Y>tDOV<({k|aZb>Nf+yMXPrFT67
z*#(v^^JT(?h9{%zQdr2}(Hk;W=J1Wz74nTSQ6`~_(8x(N(x5VtvKur6VE;)9ImV}(
z(9r9(vcT_55;fy@`tE_GedgO({Vy)qEfGE-zyrAe_S7ab5z|9SrAQ;Nsv;GSdXVEa
zdro%Nsw|S7Ym)rqVP4&e6a&fTy(REzP0}<FxeT)JX*merCT>*^rR2LP8T*B3<hq4M
z2F)eR&eT`3Ga*+YvorP8*_ljaXWBP22esdwL}})g+T@&;D{%YsB!4Z(Nu>n_I=p(n
zwoT`Y;$GVNBm(!jTGG{<<iL^p2MF0zKy1B_^cwK8j|p+Owi|lh$b>F@_FU4w{b>3@
zBmt7K!)M0`BOs0~A)^9Nd3fT>$WFzMc(YUe9GMsTs{cWDp(78aV`k2{*|~RLho+uP
z())trI5}PT8cBZ03%)qXcv&TwknXA;zX#QAuIL6<q5h+pS6afK-yaqZ%~`b~XDY&X
zk3s>Gxjzk?G7dMlNg}Ebh9UU)5-4%l4V{9mvQu`$V%l{H*!$Mp6>HR>12}Z)wRg>7
z=GeteSL5^#oSCim@jx_a3$RcfrEaXyJo4RPTlc^eI)$547bfkR^EqsNUDy&sPj&?9
zq_#+lBKw9?@5WYl1-%RISs~s|I-}PsLraYdn9&O<n1iKd$mrFc#h8kFhKA?ips`gY
zM5O0}M5MB;_AFZKwqaX>p>QxCSIE|4eC(jPU9clHCu{B^^K5+i`q1!uAh?sKUNSNr
zLpGapjP#_Z_nV7%<*XTffpK2{HRi@^VRSP$<K!IX&l`>)2@sQW`=5ROUprxsfI?OV
z1*RW!VprkBnqi1p>)W&GhM8k$!Z0$Bb<Ro2N;Oxjp<Ca6btmKsCi+TkGv|I`j6Go|
z=i{dNWO^T*jO@8bsrB<IxPE>zPS-WgBZPi0iFvl3OmzRfl(er&Gq#=#O{a}#D11>)
zT@nh_$-B*bbuz)+S3N#o?9<5J^Djs)xBbY-;o3RKL|K;az!fKj&2i@fVZNwr#n;!J
zRCVEeFe7Y(x4i;qB8MA^flycrQrrLo)kgl1Vk^m8sKoS#eMvr1E#&HAMO3seX?8TL
z2A760X(<1gS$WJJ^`R3s6Uk~^7PK0yM&2R)Di99eTG=p^oT7;&h(;-&!k%7WOxH|B
zY>ejUOEG`QnaWS#9$;tH)5dq<D%y-SngzyLaj)y8s!9C4fInm^_hLlv!HW;uPQzN|
znoQ+xWMKkRF-{wTaG`GXfiGEel_A&zo=&XZ!PD|s#}@uDH{AV|6hd#+A2O{zyg7dh
z4s-R2-E=5J5~PXyUihjEEC%G{DW6fm{E6Mz<xUnZlSI4l=gUiRrZEMXcAW_EW8~N3
z4f}U^aJTvyG*>d2cZ0%YdLRQGt=3jU^kh=9%aC(DhwP<oi2twA+y&{Js^l!RO+%RJ
z>CGxK(oIAB+xi@%{ujAAk!W*Bsa#VQ{MC@Q?2RdA`qSsl#bJme(>Y4468<s2UkE(A
z7S5n3cX%N4zx}g@D;_?zgZ7!{12tQ#_sP0vR#7Gp|FR~Tfy8SEl^Wx$MSgoobBlK2
z7OxJpxJCcn4@*<p5wxoSmAze6YO$V=(R1DlJ=RpkWX1R*LESm1H4*w^;%d!}u8gSv
z^MQUfJsBxdT9M4D8-J70t5u;kCPDiLcB*+}`oX=}%_}4|=E@Mq_tij>w?EfFVV8J<
zdj|m{(TV+4TdfgZfz3#820g<2F~V~!e)t7sM3;=M9^@Zy6epqR;hJuj@LhxSp-3uC
zmm`h>HZZZ^@vrK?HT#BD2PuV(i(-^_S2OK+Z`{Qph3Nm^u-%T_h9NM@>v8+}z*37*
z!p&nM8RczAiYjE7jPfmvwy*lUs&~%i)SZgrTOj$G>Q6PvWo0Xz+czi(lh1+<L2E#m
z+}HIp_l5;!*^8`cyl>Dx0^%?DYB!RsqS}Y7Or_XL;q2@hgw%8WQS~ELSd3ooB53gJ
zAP|E#i!cAFM)-2O&KkLCP^<b77ARBg3vn44>dk>2$VUp=icbx_hb7$iL1e@|a<9p|
zktFgq^g}0!JaDpcMIx8xo^tUYeDHfy*5dAo@~vp+$?tX7EWuH4j6As$Tb8K2lkTkQ
z58vZj{8c%&fE<SYhwu5973I7GBbRvza!X!luIU`36UW-VMcM4`tp)qfgD6^KW;f&L
zn4zfhnF<<iel^Tkn~*RU9eb>t?C2i&gPQ7pjgFbQjd;ChSgTP%G<>U~TVp3TKyDib
zALFz7uWn4kn>Q-Fh}$s?{1=8Lz3tJFcqPHy$)DiO=Dbwco*rJf26<-j1d0K%7~6Hp
zdc&tVH>=(JwAwrj>#4bzULAam<0JT|3yGkOl)=0t&V#(1a2>cYwtjFY8!TimxZq2j
z?m}M<=VLGtfVcZHNfG<#_riPNQ8W%Bc>l9=F!T?~dq-g+q(};B*)wym|0PDhVOS3k
z)k+z&0jHlKUmI*TlFH0rAQF2(ZOyoWKUBSpv2ryYzg7>iMadi3O&aw!gP{g#%^3EB
zK1LdUuAQLE!{OP!xDeTmw!ON8w>(<qG@@ceN~piU4QjhNY(IrFDRt2SbMC8~HXCCZ
zadhKl#IFy(KZWOJth<Z|@WuDLVZnfa0c8kdC{v3LY=UnS4%F?dUb>7cs`L94I9`&{
z_BDxJ=lAI}yYey`oId`(-@KH5+3NMnC{k|zzVUM-0V!91yQs>{00{4c^eNjA&z;Cg
zPpHACkBFn?6DKm==FWO5Tu;GCg2^LqhkXPOZ>STo7A2_CKP<(cJhAlyRzUx)2kO}t
zYQ*(@Qo##S*?sj;O87!@b>0tuX*g4yNO=T4e)RET#MfQ*t<`wG|3v?~RD6GP{Q(?9
zfd~IMu><lmWN3fg+EG6h`nLe@zpRrwuj)tEiJN{5U|0*oD9i|T1jjYlZ9PhTUPpT@
zq^Ex5%ZKF6_5C{Fr^MXHjD5JSPIJExrQ03#9h7L_JkkFo-rZc^!($btW<uo)o!{_U
zr0T`N=2PH(#x_2v6YIo{(iPnFqXu;o{6>ya?@5Cjq$_`^0|CLCTl>*l_icG|TfZuF
z1-A$5Kv(Rq-q+3Cf@UiAt;W{QS^?ErRjS0VM$?1(g}PD#>sQjq%Tm=Z`}uD~?j*l;
z@atr300~?lt&?+Fp6W-h;$2d-t6%s^+&ovu%#h~AsAy+DEt&kh4t5mQ{F>B!pA>&y
zuD3HxLcP?FNY+`0oWGLjbT0;w$^qO{ClUq93dD|5cjyOi$k;d3^$@J@$^6`2N8I{@
z)Ox!f>YM$-Yf#B;bqA!@cluRfTf8=V?*gR1&f5w#052b{lNguX2$CUAo`E~Kd1%yn
z2@lW3gnSpmMS+zSh%lqNoG_D?XJFXP^#Y0aPUMF-)wfAgADk%suAXae-RK^38WzVZ
z#aJBNt@!!qD)aPbZpYDVqWzjbKzIpj`k&%b${x>-W&&1%ayxcNIO{Kr4!4+<`JZV?
z+>kSztx%X*=IL1}vQJF$5<=)PX1Ehpr2(Irr+;qVfq(%>DNjpg?k4PtHL^%Gf08g*
zhF|6!FfRZ<xln!dLoGk-nIjzb!4ZCQg4zSS$0a)C^tL&~*n=Zlp|nG^T`RX=jG*yz
zt!z;KV}v%kw5+=dxySLiON#6oK@qx3;#Xd_ii9fGV~=9DByq*dpUpv!Q28_ZHFF3L
zfmjLyjnwe2H1ZQ^<O92rxjR7IVJvtQ`je@;u%n@Y``cr}MoIrVbHqWIa6pGGpN2!c
znT#K;!jNhz3B=L&?Ywth+hH?@`O$g&kr@|K%R+IpjEc@8Rv3?mh5EkeM?Yfq<d14Q
zM)hU04!PQN>~1DHJu6{9>qJkj<evw)&vwAP$(&?m7ezY6W*Vz+JXS38AIOnBKRFhO
zu!k5`l3lbk2?hWoB-h9U)gQ+YoypUfg#9qV=yAKvrfr9eQ3K2iAUq}`2Y>|_VMpYl
z-fW+^J!1MwDKCsy$gO+TkvMOUIZl#+mSUtYG*NN-ALay5o5i;C7dcNVA2bUgVVbE^
zwu{MT>OPB~*OYs73DRzldzFYV#wqHl<?#5yk$3ocG-0>uTHd3cqb2@5C{i~kwBqj+
z{7qK>ST0hfWvG<>W-$2J{eJ7DZ)r@+18y<TL(k=b#4VeUb#I%Mx73;iEi(MywKM3y
z)XqSJj7sv;<N=%Z=tDN*xq5i<=}2iz3GzrR@;Zqy)o!^=@&DaCh2D5qXHae4txf^Q
z!hzBJ{mj?hiHxMcsAE8%L1S=ki_&>i-;krR?r5fVZ(j?ieec*R=2WdMGtNG-dS31*
zWJisgRCTL2L=j0<bIv=zw#=NIZBDMv>g+wQD&%(hro*f@LlUbFq84~8Oj0pvRi|?V
z$AGM&3l-{u-N;NTgLv(SB$SM9yi-+Of>G^5YE|J%zf}JBjMD~P!9did;WhAxO8p6s
zt~thE_oXpGi}EzY>)qZ2>eXC%+BfkYUQ&Lwven0=Vl~U)uO$Q&629bOrI92-a<S&{
zs@-{S<!YrhM!%=R|M3s8w~1c2D{P1IPHbN{E3@AXFW2l?`93T`!K<5=-&j6jb`FAy
zvTFhNr<uBSHHz>0A<0meqQB`bNhSL;)vDx77egi@K@bp`Ui7xwlTNPIoe>#bIecbx
z)ukSHN<E_dzu>C?oC$a0B5tJ*JH?-fsI)tx(j88zr*xXw%?LJv$!k_Zi~1`F7<Lq_
zRIzQ=nS2gsgH%6%E6;N6#$n<>Xv9+B;MIT#;_W7V8xq<no_2TI&S~tbak@++6lC&1
zOK*_{w@(~EIR(g?19+RKG$Up?SyIcU4u&5D4rAT3O3kBu0jbGU5B>ngN5Q4teZuCe
z6EC*Yh-x*_EMNl&%i^C)wH~M8?qRI@u|62CHE~lr?5V-2)o?$>A$QXdTx(lGR%u*<
z#-}f^W#wGRIIw4ha5jXg)%y`frRtH+p#zC%;YTud;KVd3l5Br`q*~_TBzc(9raWd$
zPg2MYj*YB@-*3pmFfOe@toH_lqLsnzFpOmgCdUU)jc$8p(P?Yu+zgPsm=Av;%6y$;
z!+YTA&t*h4cD1=V@^+KC$=qW;y?VS^_{yTtnmMolJe^NP<*~7}2K+joq~>W{o9d2y
zc)gV2;YqFXo;!`d%Et-cbOx0}MxP#wD9xRyZ;$!xK}i66Dg@t7bBDQe^>}r2K1}}I
z=6yH6zHri}ZRn8pDRh$Aez!;Ln#`3TFL#(BkR@DVZF9b?Y`8`;*|$2IKZ|R`Pq&W*
z7xhVOQO6uOZ=2XBoJT0uXElHLL7$ipuuYzXILYCiojms(zZXJ2MIz6HO+cAlI1kxR
z?dxO7cC-uU`$4%_&SScsOyHJC*K)IGaVIjWA`j+?%t8d@9FY))g#mdocpT2LuzIxZ
zpN*zHX^DiwAT1F+iHBc+gJx-iIlLT4)N;n!5~^<c{1tp*xm;R-<Sl&CHEYqmTaZL|
z3NoW$hq%rUPk&W&069i+SA#Pgz$g^04IeP8u2i@Fs4DY9`fq)BIj3#IQT|%<l<nrD
zU@oAsJuvLSW#G<gbF6v&PUI~O0RUCz<?GB&WAcFYX3A$0I}YLLc#c8$TrEVTF^tIT
z&8deZ#czVRnA>lW!}Ub-!1`4ySM4}tp25$|2L94+n=lgZrl=rn0Z3om*9<?5j04qh
zo<&cN=`rVDt(NV60IQvb$Yh+hNrUc&%kAa;?onLc=Bo=HVO7Y9qW{nYl@R1lnFQur
zR8`icBR0bjBos$kVig5BI1)*!L7Zfu??w#fUt^kZofKZL#6hxpfjf)Uxxm_w+)TjQ
zhi9f1aFLLzK<p<v0cC>t&WojGqblnV7WwI?zi;1IRz0~rSY~FzDR*+aLe_3ZtUbrG
z{e)ZY><duP0Qg)Btq~9R^;d7ZL(&UB1MTTzgk&u-zgl}(h%xmo(1LqbOo4x92czGT
zVapv(b5e)8W(9ueH!`FL7#IHBeBCS|E5zcG&3##PwtKbivsO%myMi1FWJdJ2q7ls;
z+-qHs`i1dPfWIbsU?n4d^D@z5w8bn^&eHO8)H49nJQD_SsN#sM?hi9`nCjvQP=SX7
zBQ<FdSvTs#c+Xf_?V>HUo#rcWWIICY;`kpkopGmG14C6x0^)ut(lqiXX`i}RM6^FZ
zJ^dTb21iz7^OP~>DVU$XN(SK@k$o8cXy&H9R$}A(+{DI1F&@tcugM47zxfBhkq?;P
zxVILDp#^m1GbZ^J?77CMG8c|CW(~sAjq>@2A|CT#R%-q_^Kd)T{SF|@!ZCXTrYJ<g
zBa;yhE@kfH(n{R)NV?y(2DV$7;rM86l9-za2U9p&%FA1@O^&UgABi-&!4Q+0ezVcg
z%M_Ne(_xE6z|dZGX$MZ51T=2NffVH6zj1o}sHey8NNVCcwEMHw<DK$sSDYSqOOKz@
zJ>DsudL(|2ACQ;NOONl@J^r2Uv3~Q?H|X)zPLDtL^!PsM-75wmxb{aN!Ekp*S5G8x
z8}N6EYT&CYCAagJI+yZV$>4lgXK)5b->QzsDn_Z+ABjQa3Ps4S)cyRI?q{ovV3mr2
zY>6@YIQ4r)JwH+1wydWQ4k&^@QKEkyX00F2*hW>Gm-)pPD>3{#GzUK?zY6#4FbrQp
z0`mFxlBapNojPwP43NjFFY<Kiyu!@lK>GCQ)ACF5rUz!EXXRv04`k)d$V@NFnwFK5
zRb1v{iK49Bf}G4iQE_^4Nl~C+tS>M(@7zU=`RPTOMX3{_lD4IdZw`hto0=M$W`~+X
z^Fy_v@Uo(CdAOxHGp{I+dQQw!X`-YkEAOH}Vdh08Iq8L2Md`&^`FXzD^5(Fwp)uH0
z9&TtFULRaCeBLF#qT&ELn>iyZFLS!@qP&vf8KXxJAD=uidHnD(b>WtJh<(BOU{k09
zx8+S0HB*s~dCWO@Sv`DA{y6+UZTOg~P<?HvJ~*bTrM@B@YN&6<qk^mq{5FTnD;ADv
zY-$JxE6}KSr03{laT6+PgXK+`i+zFSnuezE{FW*WqnTMViUZR#i!%#zv+|_>7C?Q0
zto%T2L&ZXf)P!bUeqd&1QBhWUUZAKTGb3w876uXr%&GElPI_S;LF10u1N=!&e!KFb
z%);!Vu>p2C+n$?APU^l%)xoee2_YXQ7!w-^%x?+RhC}s%mii^3`pQ64u(_o!D80p?
z0>S!9Uu~#4oL*aN&2Y1HXjVZWJ*O};eR>%%C960qJtwOO1Mv3BY4%*8LNRhwXNEZS
z<DBR6Olq5%pPOHtUr+|*$}F$}MqrfWrB4IS73Tvf(x(TeW#(nf%uO%Mj^B_oe14if
z8;C4l&&evD8OX`XD=7`+=jCW*&MYj<FZ6(uUt|HHN-+*u40bOfAn5=)bv!_s*HSk>
z*p%MXRK6^;pdh4YZ%%rF)Z=`QX<yP53I_$0_3}xY6m`dzo|m3eR+Jf-RZy6Z;pX~)
znUz`f&A}#(E17e$qPkj8n3Y$YlQ}yxCy+5SJ?|odzN)soy4eeum_?a7A!;g#zeiZa
zlfpGk4NJ1>s~Tj{rZxq`Elu^a%WGSLnFZMuMdJeaS9gT<Ow_Od5h$MJ1I&aZWHhuO
z{f`&2)~Y&(P@OS5vRl@>jI_*<FM!3oFxceaU{QWf35f)$QeiQmP*jqUfn_zLBnKE%
zTvC|l1M;;r0)1yR1%m?j!ptH}wkC7l;k)fDmU5aL+E|{RQJgh9)2bjhe|DyqTFPU<
zsH8A6C(9miMp*`s?jm^y>Y9<CS5#b>j(M{SM3Pvbo&5k9Us6zzSty@6%S%%ed!R^r
zIU`gXBq5ZZSkxmgzc^4vTqdo~&CDZAh}#%)Nr5hD4_%}DY^>brf%L+QN?5wc0as4L
zl3<h50lUjlq&ZNciAJF&8t6;rtP)gcaSZSDm*iR9%)|8fDjFJ>N#&UZMS<KB4k9y9
zkYAKlY{7VDezA-c6PpiEMDwG--1_o5T|pGofo2?-2ikFFump=OawvfrAO|@G@sM%?
zky3E7u|XpqXg9tVT9r4as4xx^rH-@FPT&ZH?u{KXe`!ODtOkc*J7i{3d1a-hGQj=j
znow0Zh*@76tQ7j(T)tRS6%IJRpg1cxJtyF-h#BPOM30>OIY6c6CFPB}s%BjiS0mP%
z{!=PI%b!*Z{3M^5lV6yPzwsA}%TUIL>O*04O)w0kyC`dJCI&<{UtHHHkK<v!7hthO
zYOGKlAr${1+2N!R<H<Mj^D=#Ig?5)B7O#f{S~OsyJ$urD2@c1yn5Qd^R0eB<;UEPP
zviJFr8d@6hr&&`Pivox|kinH0+tPTh1dC}CiU1dTt*H%$ARV^r0p-finT>iHYnC;K
zD#~kXgZ0(n8a=2SumXp~6j?Qh@YK>AtOP>|E+rD?w}cy8!a_ROUP;09^y19i^y0!S
z&`p~(1gbz<>KnZMb%l)7w5DKP!{VTWn4VF&rEJvjytIJ}#1kaOs*oXDa>R59eBgeX
zUb)Osh78uo&NwnnEY^%etuAx1s`dhel+l&60s*3KsJu3Gc`(ojhRa1I0C6OaIfYrp
znSmLQhUG<JCaHb|Z*S(unnk-UL9)X%YHnePfjN*qNE<ap_Chs{-M*-{VSc$SUd=48
zudEGHvV`<m9uE3I9p;11&nOLqDwhhHH3iEnT_lS;gMsuSfK0k#p-dpq*c57jAfoB6
z2fpdEDR}@5(vJ+{G;-%T<xTaW`sx_*9Rw~6BojyxWULPSj9|gg!C9h!n_rNbr(qkY
z4=#-$e-@g!B($Uv!oM}6QB2kgOZ&UAvUo*I*Ai#!`sGB&aF|FA<-yxnWTPN$AzNk8
zl8a+$Rh-V9qbC(4vzK`nFhvsqds$9vXn@vHPq{fx-H!Ad2vpT+tOTPH9xM_t_-Wqs
z^up<65wk7fB2%#4MP7#j5JE*%X2=a<WhHl=yNZx2NZK<ofn<VmS8;=0@S*_3P6uE?
zAu}4O;*9ZJ6lH}LHef}ve-YF?<y+0jo?_(&=P?IpyBG9#xVx5Y^(0raqcTwr*>b^_
zI@Xo(Ee5JdOMVG>`3yazB5OI7TDP{4f{tk7E4qhfZv-I3<A%y0J0mE<IgBI%d;*y?
zGrcf}dfW6&titJBoHoCcrRJMa=+GK1Z~H)UYC-A9<b?V(hT|#48EGC)=ICmH^lAAb
zn|s-Y#&e7EYb=~@Q9Mh+VPCWIw0JrL^fcc#2n1&2q-&|#S;Cgh0*sUBPP}nI9<zAU
zsJhAGjm9?%sXVfermZ&q>mIopjV-Qg6UmVXhO;zX(j)+|EdZPfm@>d`d{IkAic!*>
zo>iC$rUF&R8f?bQ{G809Y^d<=!iyQK5K#|X4EewXpnHKE*}aFdp(R94uGUl?0Y->N
zbbACtNnUnd{+ztPw33Uw?b{rl!lo?K5L7m|d#>P^3SE*T6-hU<04k)rG_V}l6_IYG
zCzvlSc=^7)wJj$IP)4m=RzQeU6@~Gq8ojNoEzoia2M@ajmypdvJ$PjeRcow;KpijA
zxdSD^(zXx6JVL~YJFAxZqJ<5YE_+eXQa$49O`!PGv(hihheg4)fM}$mhyy}QHJAjX
zL_~jwP=Mz|$~KnTtnt*=#w=WLr3(v4V*IgfbW?rJ^@3(qq$|CVO!i?$gzn^`Kx(Q-
zUCHbXi6%ljijP1vNyM@lA}-*QSqiZX76UIAv^4kWg`#ZAnC<Bo=2B#vJ@d-4U^8nh
zuT0f{()6Ek`p<a%XM!flmS7;YX?s&vo<~N9v1fWo257IP*9o_nT@;v|1+y!JPJ0o_
z_bw_xv@guml3hkR#bq0VeSyW{a)^q7hN>#SJrW@ul`@qQXAVrNmiVr5-9k)nD|)JZ
z(`c^9Dbg}<qzO1TEm{%sEqB_HUb{TNwaaL5L}~4L`GS1%POd~?hOH#Cun1(&i~ib@
zkzP^?vYTF5hSh2HG>SZPy*VPCRIC?OU`Annu1CQNHRlFt$McCrR|-OcpGmm_{A7B*
z%k*dq^-!}IVw*;#Kb{~XqG2y4??@^T*K*8(=}~)FlV{K{IXRl4lm)xU$;`V5R?oQg
ziN$(2*#Jw6DKP-nQy5{c<|{2%dGwj8+EAmmKh!qVS4$NUWY3xXi8fR%*~Vscwk^gn
zzECL+&|(m(a5XaAHPi>72n8yGI98@btn-1f8ateh+Lkc()Ww;M{KCSL0#UYEFYN6V
zH46h(<)PXm&RuGn7zr$56)Vesb8}}DrrWwZq3Fz#2a7B|GCj?bK0u%@%E_Mw@|_Dq
zS^7np0Z4=eCB>qLVlfsMmSmECXBA=FLCb#xfN1cgT4c8u1WH9KDH?=%Gx9z5WbX{d
z;Ixnn*JcpS=bQ<ba&Dg-dpMS7<RWyrDRvq_rR+{d3uC@|F_r7>kLg*n1JlX^b2AIE
z99?-LrneKMPIQtq&b5uW*a2AP&(IWIK#>lsiACTomEBXlH8m52KvOCKh60<KC?&W8
zrdJRsiEp}|Alwk$TCTMTX<Dynm*U#9#@V!&3z4R%KwCs?d5_uw0bv({Z0<->Uf@{$
zN=*Zl9Rs)qjXR&ZCzj<7OeIfjurzebUX>5*XlB-Qs0dP!HbyA)FjDzpvc=M+0VGUq
zG;$4QmS$vTPKSV!UK-GZ$S3rorM@{-T_3E}grlLh(q>i5qjnE{RkaP}f*X0WeKgec
z3h`-#I60+YqIdij&<nCdAuL{>wNZ%@mE|4*&w{#R4TxyS6-cP?YlO0p%VD5CXHXS2
z<=S8##U&vrG&}o)5qqe)5E~q!n<|wp(7+zmgiX&VyQ9yh41wmB`GKr}M>P$R8ptfT
zh8@{4bIey3fr!$iWUp7gn~@1Cv2E5a3W#0XOA@upU@a1|R%e64Q7Bz?6427~A;=sl
zJaUj)_VCV@3u>=L)Uz04*1D)RE+%IdOe8F2yaD2sWtf{)kUz~&5kl4ycR?bi*t>dX
zlwxwdOWW-f=id^T#B^f~fC!FF9m)z8o^!yC0L~`80y1C56+-N_?-{SvvFT%p-`rvz
z?jV}Yxiix9G*_{<%QFL6ROap7m)O@u1?do{ykNJ9gtpYU8Um@^5$LSNEqk?meI2*N
zwxPFvS6!OqEae!bFa)7G6tTbQ{mfPok7V%^id$47=un(rTvh<<uDbxhm|T_EC1}nz
zH^}bv%pz#vFrbMGgm(lM96d&F#{l6X(L{S$d<}Hd&H>5pMQ!R$l*AX7jtmY}4nr&v
zcG_%Tm1q^X(PEps-4ChDQqm|OaV$=cW0sm&spAT^STC~e3^m9_mx;DU!cAg!M08G1
z0Sj7_9C4BWsc2~mR5aAK)M+C!mzM0~fQ)6#v~~qtMsDvJ7G~xbPR}f)6p1x|&UhNB
zTI#vufi8g_(1*b>XISRK_&m2TAG7MQb^@aDhU}XO;s-7XgALkT91cSnY^sZZ-$eU(
zKrs%aXJ7?mpD0`&0M)K&h$J!R<d(zmurzgSxT(Cpxj0l8YzEXK?IBn#p~`~nO8itt
zuFG?Z05A&6>GkEc*b-{apcpw_8k;{Y5Ne(qY-$i3fR5A9w9HpmzBD_#cyXwqR%C2!
zF61@H^jPn8KbOuZ^)1&O6FkcBEst&}V!+lQCWODf`_#-+J$7uNMlR-^8*`9iMQtYd
zH%4y3a9MiBw`c}R!J(Te(PNVnF(Vd444hNBmUAP4sVP>GJ^7i15!#{!Bkt%HH%t$e
zS7%mwP$`gCl9L1SoDm5R8?ah_<ZAM)qSRPmId(=*F$Wvd(kii6*;EIYr=oDMaeAl;
z&P~7_;9F%;TEO`iGey9^g36-t0sSvx=AFuAlbo3DX*o3pY(URg|D@Pry+YS%&H}EB
zI5AD7rDBF=mp6sV>nnm84U4TSnIyLwj0>y?CG#4vh-2pz3xveWS{+`Tu%?DfJflHy
zG;+}rW8;(`U)54u>#orZdyR&gi4%emmTfiSYgn_E)zt+-Dk}84oJg0tdb*%RQwy$^
zP!yrCIy;`+0FB$<`YKx*Yq5_U4x)BxTQn?|A}nH|=c{BVy;Gd(F4$%YN$EhT#cYFQ
z(h*1GhBj_rQ%ijWuZW#|2Z!r}w!JbA_+!886=^2fD{`7(&0+}J$x+a>X$$Bc#!%R>
zBm(%Jsm3dkqu8l-hHh206sAo)18VrH1M>s((*v{93$1sxv24KYV|;^h>~N2S-;-P-
zq!E01^msWTMVYynAaF64b6X=#P4lAt*DY!>UD&qY5hLS)E_JL1D_5Ci&UKcq-R3#b
z^Nn_wyVzx&tLuZcz7Bg)(_8T4g0uwyCTrFFtJb=Uv;HC&t%s}X534nQwhS<qniZEl
zI4g(hj3($215foX3|9{D_9$je1MHslsKM@l$;A-yJ)9YAZU$+StpyG{9$}B@kOk&R
zP7_h}TkkC%?tzRub1*P-?ZnQBcT`qA<4GwX*5T_saBOdhe-oG8)vIyY6_iL!QuzZl
z+9SSNJ>brZ#<z&K5fkHbEl6_nYXcZwK<pX#Hhq3Gd+!0XHGWGlLX)<`mepeFgeY+;
z3dHUdIxb}*F-7#xEu<+yv`%WZb&h5jXaPpfrZDSaHFl3&3Xn==0`{MlrC=E4P0MB#
zMDu$veqVhc4CydX<6>1rZO$Cft9z#K&=~BE&?_AdJF%uAEIo{FwGcc?2&ttEXg(G(
zJDEOEO89$wyAn~rqi)#)kK1sCBirEO8#U|!rO>X70-W1jN4{3w0vH7=-9-?klG>n(
z2X{(V(TrSxX-%rY=$t^2{DWPux}_FkOLNe|!3a3O39B3)1`Ty|<;n!V7b{ovTzfxp
zal$hx@w)_h8!zl$Iiepf2tDrVrSoT|w|WQi4g>c9(QR8al$qjS2F@5YyKXU07bB(9
zZ!~@1-afHb5bQ}6&A`TJoC=R^LG6$icc|Vm{QD@#+6xLZVX}cz>e=p+W<B>-56JWA
zwHJ3v(zH)h@vN$H#xsPPMf(FbS2WbY8!!mwx&X=m(7Fa5M$HSC`Jk$DZeXMJV!Fd;
zBB)|KvV&<3iMLOxr)VbhNE?(g4EQ+1?-r2~7ugX2?7qYE9F$5+g*~`(R>5Kk$7puZ
z9!}^U%|#dl1bY3C7~0YpHD>(~?%-MV00l%<G+4hx^mm(txJVdRU2zFC<y~!b(S+Wv
z!0k3{nepL^4{V8v5iM8=GIyMF8xpwIxD~yotDDLzLqY7`QRtD+B1_C^Xj%wYh9bQM
zY<J39%p$BsmY-EHAO852z0ff)5=W?s8t_67>ct5<N@}tvTGotxW7(+K^Z#t=d(qpS
ziPUprh^0%atv#2PW&oworm5Qe!9^3*4;xOjyJ8Ute8GmImzIkp4!Psx!5-^1_*P(X
z^~#xLD!hcl2GX>&vGEpKFGxm~oF2hNxN5CId#ZP^vey}vxdcvI%+5D68a7T965SOc
zRM<#~PI?(r#B%n`*Q7YJfluSEa_^KoJ@5`9f(FPS;te~^+6Z$6A&vH0@Hm1LLKYcg
zva*+z)1hdPxU2C0tpOL&zVVPFWF%V-aVcIT9=Y|)R)Ys}?wej)_Rs+F9LuT?!_yN?
zD13RKvRO-|*vDlY-VCUI*wQP^pCi771>E1Bo>h{I?Y(fY8txbMEP(hV7;EaW=?@!r
zxS939UIh--yab*AzS;7v(-b6eX$^n1P(Bkf5vuYGV%*q@hWf<_q>9>0D{jyZUt&*>
zaTO}4@KsqJOqEbHwNaxOtPw`qil7gs>gna-a%@NhD;Uz}!)Rt^T{P2I4o_t`i8L?5
zW*ma05GiHvbcG`PTVYm$9$W92>|G8@=lV#wj#fX^Uz|RTL6JVvlf~t=>_ZcnKtSkA
zMt+X=*z{CH9iu2z*H{Z{pA&Z(5yBOV*!3{Ld!d(lPBg4N+icuf6`?b{H~@otg%65(
zb*LUXMgcUEf1?^pcEyOq3fCYJ3ef~NZGttC0=S&u;$UsW9FBFLXJYyQvFVxcP87!$
z80xL*l)a89z+e~Kg_6%vp|A@QiB_54w%r*jMQyWfY_<uCvu!O<2|<*JHv5iq=7h*k
zoq@9uJDHwU;Hea|W?>X|`=PT%a_zWXipEJj>s8Ga<@HsNAdj@Xd{q#0>LYN0?zh;3
zK*(5#YYSo`?a!%!xc>?*kpo(86CzR~DvAS#uLVTf*mz`yeW*m(EdGVazH0UA#!9=q
zp+4xVM-U_t3U0Er2yn$UR8^KQ)4sGB2;gZ7RoB4S04Jsh`8#c_5Rs@7puhDQ@D<1_
zht6IJIzm)OI9tT05i<sA1UiGDqQ%YJl8S&r3^-$<=>XA#5ozaG6X)5BiC9k&91jbs
z&Z4zg>|E=SljsWs8S1UQ-^F($z=7Gw5vL9B7(8PY+@wPoW&uJuEO2P70m3Qw^*qEa
zX6^+i<E#OpkNCJs7L70&_PvM{+J?QiX2)v8WrfW)Cp@|eqFe=LyrI&Ev7&=f@0#tw
zx^&2fr?0uOA>33I0IS2~Ra!SN^)-Ck6eTAeTS(c3!N%J1ieMesgQhOEmF3Y3y_b?6
z<;&e&PaRuR-kjH9hvC%gD=*aCvhh_Whxw+8<x4!f@~gyn$vKM-3HQvjTeB9Iwe`Mb
zKGoIYh?U@$bR4!I8C%`<xg&_*ZzqhUzClO6MiEA51U9{PlvKA}3r=>#Clm`<z7Pr-
zzeP6`Ncv{A>f*RtZ>(Tuk3W@qeV*>I_F42SwM^g}g8mrt9lr<E(R+Opv6JWa#p|7;
z9nT=&(R<eI2-lYud-N24+ejWc6@aqUgSc2@K}h3nEPYt+ROYLOF`zz9RgR0JARw&$
zM=Ar5g1QSt%9$5HPeH2w6S48-x`oIze9IOqr_+M2p&|(#jAZrEMI3ihFC3jhao2Tf
zD)yD82a%`%qEfFr3#qAgk!VK;&n|<f(bTjexKv1;_+Waf)>oYM1`Dd^H8$iN_JQ4T
zO&&b&z4#r`k%*0WZ|l+3%anQxq+!rb7o>a3om+I6ExMhJ7)$RUiYPXL5=L{T2Rc|$
za5nG-mgd0G#U&9j#^hLFMZzBS0APAoM3#Op9kqH8z0nC_)a_L4V)FJZVnD=j5%Z!t
z@0llbD}JfiiV8vy6{B|7fis>-`39Tg+!3|WGIGYE=ilD3_l_>wh#yU%BT+bYd<Ddr
zaF9Fvl;5HeI%0_K#*Vi^?=(tdq2|m*ke(rgM2p4I{TH0zu3zLZBLEhOm@$h8ljEzb
z4%XF0c<se3M43Q)cFJoTYan%q2fwv0ZFt8llISkmn}>jai=bXw^9?3A;@jv3dfNpi
zRyb=p5?<aJ_z#yyJ-h)#7;py_G4-Bd+uC3xB1O=wZ<%9=0i`17=%d~~dVq4oJ?hBb
zI-+!hkP%rXBbB<ByP%zMd%+!TD2=Sbc53l1mk6*P`E%(3RZw53sbNXf^hD!L{KXm3
z8tRJ>onltW*HHuS$Rk1Moe~e}j6miilI#fb>upD91guK7!PjlcTk7k4ot(*w2Ad0G
zP^<Fkp@{947yt=<Tb6;H6S3sEeTlC2NM0q8IP!HpciZq%nIq$RG-N;|<GTj+j+oqI
zh@evum=rOuF5iVVN$Ci&C@}sV;d7gBMe?2q6o>|>#@%|1BtmD25c6VKU34W8L@ILZ
z9y+3#7|e}PD=q@6j*mkGkfIxmfYaCFzh|mF{K!Lk;}`G>nH~Ym+g*ncMatQ|6V}L&
zwO`;7N9rL5+PWM${n3Dl;OZ55Es<WzG3)Ngi)0x@PZ$*7B{Dw<%MMI4O_AQH5x8p$
z%`PybJNF;*P_IM~(PA88&?`N1X39I09)U1csHX(v#pUSH2!-?TF?ZbFN&ZhUHDaKg
zre0}HU9heKnYALYDB|l#Ja)SoJ)l?}q$vi0yhDpEAJGSI(Fn9W5>SP4FZZm{$Z<t?
z`G|n=w&Qipb9QfgE09V*V%u2&^jgQ-Y2Sh32E8*B*)MPP>{DJQ{i5|n1QK}*raB43
zY%?1_C=t&&2ah8@m;j4QOJjz!S=L+ROK$_i64k!gR{Xg;l(_B5iq1-h!!olFc^8h`
zh33eJj$R76hA$2=E6O?0J4S&X@=`?#RNfN8MmQz*7M(p)y-$e{Q4Hh=wSHatFKlwz
zz9a5-k+)lEW8?J+*0yh1X!n3#3_VujnXj2j$39vQF!VZ?7NzJN_Eh)mlr0X}F{RFR
zZ&;$CV;8d*(Z7qoj>XpFBy6R&3nDGV*UXT+-fVMiZ@u+eJbNo6luEEU+Sk>K6@tzs
zM7*e_8*QAAo>uJ{onE=1CDN38ZhoWWt(E3sbu0`bsgT(H8Q{?je$9LkVkkhavmhQc
zHj|2LXJK)HaW}QmCu9d<uH7ipx7COsSY=Z~W9B0C54#*XnV4>)u)MC3=~MV#Tu-9j
z&wzA;w2;`tZ<aDy^})zm(6PB0`n?Cp_5i(efH6dQ@rWOXI}pS;(L3DVg6u9bsN8}K
zeK9>)Ro+q?w%}s>x&dlH7RM_NZ<|}1#0vqI^I(02uv42R+C3tPC~iQ_m`yZ@;lg=A
znp|$d3pQU#FlU2C=Ew5r6wvEAvP#V;Z?ar=q~gLLoOPM@4Wk7E<ewSHy&y2w5O+VY
zYS6p<RwZ#@7?0E0qj7jaobM!n93Ji(c9B%m{$5s7af97!6sm4&SmJguPPM{M^iZY)
zX<!!%mLpL#ybrw;$!=hhgPo{b<{NF5FV$XL779kXcExEj_1u`L)x&YbAiFt-_Sqdo
z=FN--I9y1;l!ZSI!hXv{-Mikfp37@(3<45lVd(;2i4XR2ve3c8j@bE+iRKh)foy%)
z#`MhrR&w{oli4>%_KTu)82CJwR|h3GA(pw2f@;ZH7?ZZa1NWGg9W=u(3f4vg#uk?3
zf1c*OvmdX5NPZNZb{s84#DP~VEf2u*x)d1%^hooo=F}h=GjwR!x~y$$cMM<GIBnjs
zd0e#Tf-j&QSHvR;k>1!MCPgfSlxF2j7RY8<5NGt3ec6Cio^B^BA4!PvK>ElYdEms#
zkO(AB>c|KemdJ@qoe<R>IDo)Wt07P$StoI0X9Euebo=5klWBEyi3fBGgUgr>H-G{@
zjMgggHM8%|O)roX-(6o|NpzM;pPi#!{3O2R1s9ckLa^IAkaL?GY8MCd;U^1jP{WLP
zQBEr{TOkZHV33$iPclRB!LS|soeMhfX0%}OwbXknkC<{eY?W8yEqxW>>Qhk|avYuu
z;Im=}WyW3r9kp=at<K|t$(>`1(m)UJ_7y>Xbr7VY$Ly`g@p{w|7z`GGIUzDak&a16
zj3TI=&%s9k+8!+SzmO^fs!i08y`|l;c-<2%e<AS#bw^0{4;EdI93Jo+ql=FXQ`YE^
zf455UQ=k-K2Y(^L!dJey+IEfomN5_$jLrp{AczFN8T!d~TCB^|e~O6_vk)`&+`WtM
zr4`d&Bq|jW@6QeBfAJ?zL<e_n#EZe}%jat>M}}j7UNVx{Gv^>kjIbIF$z?!tU&H(b
z{4+B^<H%B8_A>2M4z(=jop=a%(z^E}X9>hb6;$KJp7=xmn#H5nv|utiU}GLGXNaUE
zM{4-2$2p-ousFE0b4jr@tmn0&2_S$7DSt!FdRq%5$0~sk2xNG#lyM<;5^%w1WIbfd
z@{JcKCPdLFuFIIY|Ati0>T>kr|3*z53B9UWBSR4?@Hb{2y;G3-?^E<6H4GoMnO2k=
zv>Qpjfbicyac+UH33^e!PRA#!Dog7GI-9kvQ+UWuDhO3eqeJf9fQ#M`b}Vy_OVtP!
z%C3x^6UOfQmNCdds!m66cXN#oi2@m0rqkzmLF4wt!cI^0m7t;)L9kcU60{YlUiv2L
zt{mntpMFr$E7gg1unArqGMqM{VSX5%;7;^ngjYSG6B$AX2Zi{9p|PxF8X@GfVluGR
z6|6+gZwV)bB*UEg@mT>ACVB{znDTOUh7~Fy3?iD@P}dNKAE-_d<(prD)fc}b(P09!
z3Otb$xq%SU5bBtbKp5%2d#S{V!o*-2$<(M6XidmFf^$V-3^qw(bYa9=HvlISe(kkP
zWCC!69aoRla6PpSUsHWlZW9+jQpbjxr_uWzQf_7?{zJZts`94bl3;DE4}W!QG-Zey
zE!n(W?Ig}*r>2GI-fF3V(9FJNCL4)x)5MY)2xE{QLgx-IKKg6nD+1}_f1XB1$N1<$
zEY$TNci4P}UIF(Ad1N`SLJ$P*dc}I5?soh(Q^#91SPG+7ZeyuTG!8~ADo_wR7AA(6
zFdi3n5<2<UYI5SZ&(W9Dx7<Rx_}%L@04KH`FCM#7!g#y<`3n$L(c8!zn;@3EC+?7a
zwJ}TB<_sK_<8vt|yvu`)6cSK%{Jz`yiUonSr82@n+WVEg@En7SH7m6Dc+0s1OP%P%
zdrgO&|Nk-fE?{yNRsMg6#1LRrL`0MTF-8PMNy1G;Ny0^fg7kHm877lV0uqo*AY8&F
z89)?V1wo>Kx^xspRK$RwAgmw)in@ZXE~p5oh`5SG0Yw+&cj|r5=Tvo7b<ae1|IeR?
zC-1jToqL_Syl=nV({Mis>#KxsHP+KSb*xO4V-Ju=^Zw&r{~}AV_;oWaohL2p7)LeT
zMPOI?eW_!Q&<E}f`H9IcKk6RLDl%fjlONw%yIK9pDa1VtbRVe8{}Q-=1GVsO_l0h~
zsUIm{q-napQB?fSrM%>Ovfq^4x>`BnAbDT=up{5~uEXWaGE4MNR&)!jSWM-su;PV#
zTH0A;#1KA;>=MRUdvf{i+Sv25R+WwYBzGWVM55GJ_AY)pHG0F?*11`GWQsvvE9?^g
zE}ff+9Qs|sCEtybuQ$0{!)`|yx6>ZDx3eP0qVOTziY1NCAC33*Uwl0|nV{th&A}s&
z%p1<-$%iL@hs#%li=U`*^H}~2K)5G&8LOT38@l_uw||$*J|;ITd>5s%ocFA|WK6!2
zRD8XcgOF5=Em887URx?1QE$fiff6~96L9BuwtZHLC#1nF@;&->Zaqu}*25qvSa(Y!
zuiP}ta?b3#ZdoliX8BoczxvBv@)2FvLv~01)k}9cbQ{}lokz0Dsq$r1m%fbQS-YOJ
zOrC2TZqJGLbKhi=A5L<+V|kLxN$rt2TrfcvyW>Nb(Rx!_EGj1Uocpqx=+Cs3n_HCM
zE^225%e&WCZt#WTV%<)r5nsJ$^vbiB+=F#7Q=Iwy&vTS>A=u;ONuK|_)t8EGLPy%k
z%1)T>J}E0?&uL{Y7r*bH8-`i(WyhmWwRuw<0A=b_R=9f&tvUG}SNYIFo-fNcsckl7
zP3p4Y4(O+3zrXH73Lp1q{S|j6U5d>8^pMUznF7m~%3n?LpZIiq=gOOyJzEEL-xE5X
z{ELFak6Cu&sd|nr7EES#S<5z@s-W%;=58wsGtJa=pZQD41e4B{ziW4Vg?GQkt_$V=
z;uV6yP>*+bmivH&8^`R=BXp5-cL(A5uVZt-&vLdObyVe`g-3O~^y3}S1)<zA(g%V5
zSw@LEg037}eqb%@Ko>7_zhM((t+Mdg@-GQw>QsJ-yy5n%!({9ZT6nT-3{v*2IR1x|
z?A%q6_Z#YmAG5b!J3b%x;m#^w5$HTN>F2uT-8+LI^W6Qh!&SB`+w_yK&&aK<`;5Tt
z1?3;3a<`#YIps&H&CXl(qUOrSnC@!=)x%Hlw}0+wL-!>+_)B9!vAB$9(l%>$Slp63
z$y)IXb=lcF_D>5IH64CAiHFzjJJT#9`Oz<56+CQ3`N34)+nqa7DDGz6bZhATZjII#
z(*2lZ+3nZFNBGSEo%hkUl{e=8Iz@21kZb@+M#>#PM_=kdf0B%Ph?b%*<2L3mrEoVP
zMHfCh+;7E_@_9%$6S5&>e$1u(%%^xENIp_?A0oQ9UW%CAucDP8tFL_i7XB1fx&Ib*
zX%Bn+`L`n9hjAZ3$_diV7kSLE*xfgG%Yi&d$i?PHzWf2zT*2=aZw;L*a~E5|J*lg!
zj@49uzDoZ1zzLPZS1g$)Dbedb?tyHvDiu!%XBPj?vX3Z>o9QB(5^b5rVmA7FlP)F4
zPZPOF%M(a>EDP64d4k{`$921DC_Wu7fBmO$JUf1~b}7zMx@y@tcC6~fo_yr;mObK-
zDSDW^LFyhyXO(u@*E^JQYATN%UB;yNj_IH*0xR_FnciyU&#!sE^x0u~-;}GISwWOy
zRff9QK+5Yr6?sOuOxE?LtkyE@JTi0pOHmQ!wc9?^`9RpM8M)8d%1zasS`Xa2__<#_
z7{<?B_dal5BmQ>q^74y4@~3Iv*_c<^dq4Rzfd9)6-f~vw9ax<|uTmJ@`+wV>K851B
zGNaFF#!k0E6$X8B%*k{V(;|8?s>99_dl6IaKZ@0<<31!X(6Kq$$4KkFUC8_n9W_P8
z&7psTOxBFU<PWHnzuA}mVsLTWU`1_8X8C6r^2Z@EpDT1e!!S9^t*rm8z2%Qs%M+=N
zPYe!U^6mo<$ooB8WW3+<=d#^vSO*@^?fFu%0_{`&3d(+R-|zkwMbmu*=I=C$@BG)6
zxwG8?^2;>xCVug9hWyAUzc8cOshl#he4+bg-Xn`&2xw#-WX;f()$Z_$o4oR-HU2yz
zJ<7{t0r}`aB6XPw>dZ1Ydsb%}mdJ0+NXq0@%hTL%d1pmtm$`c9amP9y$;rvGzMpV-
z`n=@Gx_h|c?smJ}l<E&+6)V5|t&U?x+#ps?bw9!*zf`PS5uZ@V4&`29@cGU;r$0~S
zKC37nZ&#0&Q*Xy@QKtOduUhz8SYAAs=)5N?o`kDM^W4ytUb+OO`ex70mx7e`5<O?l
z%MZt_F>ddz3c95<^VBw%CVr-glV-7}2HSS#t}t5*<vQh`1Xx{-af8<DO^wVgTh5)b
zO7u{ep3lsa*`&0y67*TGd#6R8?H((4b<1Yy`<U{!olJ45$OBzPW>c7c{Lwj;SW&Xl
z<9eoMF^uu(%2kDYqGpe=*n$Fi_ol3W0w~}1F86kxIF>Jgr8BrVzGnlNwR1<b<5eh5
zy5-1EHhH0|T%=D}dfe>1rExwfb`f=aN+S0(%MT0QKr0U{^Yo2iKKWZ-nI}*5^Okpw
zR()4A9qVzA{PcYOSI*<|F67vSCoJvQylvS02N<32M{*-s965BUaZgo>*9^n0M>1WG
zY&JaEJ@bcyF}5C;$JXBSWCfI05JhwI<e3YP-D9_fekJVq>4WsUI!lVjrYj2ZraXf@
zPS*R%iX-<a-eHJ8J}Gtqd6(Qh2=isLy!brrq-Fd0H&b(yG`;f+Gtcwad3OJ3)tFP(
z`*X8WG55RWC*0|3>>jM;br(0IolkheV#`Xo8|BQMa@N3Z<^8fxq26PMPf5zpA}aFC
zQeLO#1@LlX-p8F`$|W)Rgv;wJhozV`&EhUoo(~m&0V!FM+%vWV_m*FhbdN`J8%Xf{
z$nE;&G3LO+4R@G1<#6{SSjG9Nzw@^x{PdAs{hjW{Os0Wb#=D+@#`EEMIlRu*biWV2
z<8z|a={)(y!!gT>oA2VY%lXCc^vNTZT%2P+N-ZtRzj@>SJ753H_fZ|IjQb-Eewd3d
znXb^Ij2z;!k;R|;`{yhAydW>t{aJV}eJoY}lOao2@kF4U;_`k}$5Q|+ky*P-Z4@ia
zm^T19>lCrvth=ME)M$=N{=BSzTHfW?%w-yMI}_!HT~2V(XA6?mF-~xur`#^Smdiwu
zlkO{orZuZ#_U=8lq~ZvCqWj&)c`Ftkl9ez1wqw^13H;FQW<%$1THJ0@Ia<C|<my#l
z5-*Q&<@%Ky9((SZ>x{;(bNAOZL7#)~Til(<$5HWvEOU8`Vs*=X^m%6P;5)y#{Sar<
zg=kif4=dH#@?qRUyA4^`@d}E$I;;Fnihl$s%V;_1GHxeT>=CtJ6PD|Jn`T{qxW32n
zqj=ChC~q#>96c>x5BU#2a--$uqnu3K>j3$C)VzUlJYLKUM6tds)>qQ1d_m2M(vJU7
z<Odslw)`hF;dAUxmR=FMZ4(Q@6A$;vXSY>ooNv-3$7r{Gwrt;(Mi9l4;?+aRx~K7s
zV~MLYtqFOYET6~9lUR8gto0$wO!>6NJ;e9Ym8LrjW{Z1$-_A^*hE(NLR+TAMIizvq
z^5zLgo_wTxabu+XuAA)7^1hiEFUwK>@K<pho%xCh75CqV^*W)_0&e=svTH)654f3;
zTEJH;X;S6M39UA*t+z5+;0z<Rfd6fzKHyVEY5`A~YPt$ngMaY;K=Zx{{HBqHfJEV3
zZMJC<$b`xekSIz<S~YWR2#D7uBdu4N!$Uw~C>etY3V22|2pd#pwr^0{oqBaNFs@Ii
zG;pqNBhvu(D`{e7`Ghu&qm6I)1ceNcXd|v`EY!v~6%w3q&DHo0u{axXM5(8JsqVg1
zcVDWzZ*1M>_c7jY;K@poER42k{2#>E%ZIZ=AJU-5lTgOlk6+(8I(~n%A?<si8uCM$
zWDi9{Ce;9<dtznPXgk&3NVO;9et~Me-D{7BZzt`-nk+AHuU{moqJ^oVg{jjEV~_Sw
zkM=RA$0G%I?>J&xNRe}yPCyhD=}#j`ay<?f1{>9q>vYz4EfFuZ5UCZKqRExf%JRze
zHf>*UNp?q7go2t-S;#muHbRQ?bf&H?_u3lM0{(Gp_JaoSZ$@eXU$jlh<yPEQo7P^_
zOcvNsQprf`YR?F~wu6z@cReHU-VR1uzwwN~zjZLus=Ug41a7FL(#LMO`jO?T2_!)>
z@IVqYq0$7BopN`gHF;b7=mXZ2R5H@~wPysL7DU!;(>lje&;*i3aS#{=+ib_d{#Jwh
zgI?Xf2;Rubpay)Qk|tJ;pU{4ZHg@)Wz}-W>J+*PLUY!l_nI<~~JlAB;n9!ziiSaG`
zH-(IVoHWSsr?pBaREB|HR8p}F(0I`E{aGPn;+z9BB9cwuxi&b%Kn6+t05TawkQu<B
z2`Me%WX<@w4#zL72CG2Ao>;l)+p7p1?FBI&`GOb^{AA4{gD5NK4oYHdgw(GjpZ-Rg
zoCcZru_dRGW}=a1qLF4IS(fpW%9~KB;U`5Vq&PZCIPg0y8}l1yCE#v~0`p|7hj}s>
znE!H8wPrp@TWlUhA5dwjF(A4pRz^qLsrI3CP=;dd|DO1iuw)7#Rh(oEDW>#<${<p!
ztwsiKSLov(j6Gbe9v*G+k4Fk_QY7Pwd2D8e8YvRoE!U~4Bx~bfVPpdz|FH^15jmG!
z1BqmE<>bm4a=f68%_%}3txTUawT<a3G|!VOrxg;kzv1%_4A!9sRuM2miACCeh9yJ%
zc~wmbi~r-0dX+BQZ?%=YajL=_Jn3w$Fx<<Gda9_7qSH-LJy!H`$=s^S^yO1lPntSq
z+KegFwwpcUqI*|Oo*ox4>Svm+VIZy-I}<k9wt^jucnZGQ5#ftX;`UgoVmVyJFG*G-
zO=-FjCbJ(!Z&K;WmF1gGminWOfhkJgWh!=#v@BJDM=R+CmE&DS(j-W9<JOQZFV$}G
zk$*`Y5RZx!<IGt3!=x{+WGL2yrHA909hdp!4;ugI_Nw{9Q#qcX`#DQxA8@jsI44$4
z9&OX`KKz4+oix5)K5Q2Hke(^VbGq78&;Q%ntw9So*GLWEJS9oc{cRfiDzUtUpml_o
zj;n8(c74oPTEIIybkX{rX9WJGgOS#Yr*mV`0&c6M(mPr^c}C#;4n|swJtOe?4#u&A
zYvzJ$w$tF6x!{^@g4@JgX#xpSj(tEPEN`S}z23_LiCbiW1YOF~8uqe4QX#UyFe~Z3
zRXXIgTvlqCtkiN@saaO0n0GZGLAx+&ZCcxVMj&xdsMLY;m8+Dc)#qh_VZ62EUUH?D
zCqYW0fQ)--fx(^PEdX)+|5ssjDQ3c;Fy#>d@WnndU>I4`28hBGSs<hG|4EIeW7cI^
zB3QzaPd0#IWG#!#htUF3S#EG!ZCXqw(FhFdtd-Q+ZJONsEy*q53tnBWg{1{rn<-T+
z01q`%A8@Ecmez@$F<$jjKbrV4(WDwcoN=x-+q4EfBM{3aBQ5Voa^5;qb$rZpwSadk
zsmuqhpL<5&YiF>D^Z{R|q*9jFzMe78N9sotKkhV(4Is{xVba>p)Hi@wE*WWgKgN-d
z8h&8L>8iD9{lvVh0e3JzhJaWuWodaolDojF>R4^MTEJVC<aD*#wC?bXz$ZEwX}#bz
zj8-cigw$vgqez#Dq~&!rV_lo7nb&)Bz&<6F0nl3R8G$EuFph1ZnX}MrW8pZn&;(*h
z)=OY$G~JieuxdG@H8Xm&8La`aB)cduG@5RXXR3}{OjiqdzmXci)^_1K7H4g(3hbm@
z@}weW`EtUNJY&2kV%fMd@!nTXDikG4Zp=HeGG$FyCW4q$xoGXGHZAXLGGUi#93QZF
zTEHuf)Bt|LNG;&iN-C?7*3XPFvG}ldJ!UK|;NwPW0RL#D7Vu9>DlNv@ue`P#zL)A%
zHXh&}M(P8;)krPiUP>y})7sZF0*~)t9NUj3e!R(aHGmkGvj`B&&W~oB)^T1I82XXU
zxHDDY5)*6z?>15c_<bc!s;pc)+NO1nQQeYB>sc=z2cH^kV)Q#E)dxgj87ZwFdPd+c
z13#K=T90`~;9oizY5mPJ0*RqC9fwibzO1E9^(wUle1nl1z@3y-7EU)^Q%tZ4e4~;2
zfT6B*OlPXVd=qQ|Pd8Enc&3udF{O2uQQer*y2gvg0i;Ho7(K(J`hX}b6HDtH&j?%{
z_|a_B`j}?~ey)R&*43U7NDTQgU8@3jnqUj~h>;q=M~##`(@GUJQS@t*>I0%E@6<XK
z*klJgTmV01qz3RxC6$FnYwGKeZ2+e!sbr*ej%SR=IQ656A0IbAhJaWuW1_XKsUHGj
zxn!i}{YbXZ*bn!~AdbAo9O(nT!ALdW&PsBU-~)`U4_Gr&4R}Z(2|nJ~`hcezsRkSk
zB*7mxwmu-~n^36&$({2S{JfV;GnSkv$POCi3395akt#~IXB6$=%>&7!d;&e%{;~E4
zZpPIFf5s=I#m#Z}grbAIPe4Yf@Cj9WSPAwGC_KiKVmDFI+f8($P&`McFXSYf=swAN
z4x~Z~&r|JvsrF>hQPJD;I9{Ya>U{#DXkumAs1>3gtHgl|HLD-B?6!cHD5+d~VjtFM
z@8`TA@bf(d*J|%gUJ&@To`RLv>$y;`?p&zw$~Tl<c~=p9vk5kVgDNOD0l)=;u4bDS
z7BY7WsNc!d*MR+|z7L2(=V~pZJ{h6;s<y6Ix59uAH<N?FBb79<a@vIUs5UMNWWirB
z*(&f$CcA7x`*+%SB9NW3W0~h2^(r4X0%t0zu)DW5mgv>_3~rlj6?lcoirqW4@qqEw
zfDap=yg>7~HukX`*MR#gsd&+bg!>UpUU-<HAs%9?8mUyWGR#$}gY_!A0dU9^El3qD
zh^Nw2jbxfdJOU)U8_Gw@JI`2INj@SaTlcB#hLtmJn=pA<cgBM)H)qUD%5P#th_QQ^
zAp7#DfZUU4A*FF9X?~rW-b1fjR8~~P(?3ty;RL--eR}g9wrSJ<R?FgI;5)_;rJpEY
zS(<p$RE!j;G!w`9Ue$fE*}$!nI^3iG67L?68r!0h7mZD>RTWQ~suu8nW`|1}ttmT~
zR9X9geM-u&eOP$bEHr^nsRdcmfS7T*nr&K}n$ac@qnYI}wOGUAAI)L|h>dbNq=gx&
zE1<WSOvH`eSSID=dUdM}ct@ql<OI%Cl5CL^+6QRk&_EWv+{*$_31x55#y0}lc{`Q9
z4C&SR0zOM=;tTM-N-BJLTpOFrEOj_p@D557S>PL#RLIWOM$Pj9S18G;1K(h5HQ-H3
zDi+P3Y2!)H2mDLOH*uDRt5+GYA%%<dTIgG<jgRZq`2xOPX%ZUnMw68xx?3B&SoKta
zD~(hG-fN_4TsqEZJOh8A^Wsk?)<+Tgic9w7`&Ej<mVFTTQLB)PZW*OAei{<dk9#5v
ze7(*zdFk+$(b&}uR0*Xm`2zmgykCGS?|ri0+@*E)kgrGJ?~T+C{F~`s7<;r_xlb{v
z5g-M=p)8%*jf)C<Ld&V^#swoKRP0f40c1BWB&k>_PU@B76jH28NO7F|Xa&~w+HK>a
zzfbulRF(h_8$*<SqFkSii;1*WZIr2$H>~0~U?k*8l{3~(ZpSmHq;`{KYD)EGCT6Tl
zxMD>w(t5&)Eh}eSHF@&!#W`S9-cxetf&2zNq>@MJwc9RB|FKrNOMv*-O*D4)N>#zC
zvZcK8SGst{8l0Z`^h@Q?5=$=9seG7io-N?vO3LrpsbDiFm_AcX1#3CM^wX7P>dy-A
zD)5S)uBL*`oM7r|Dp<=2j>pw{POzQ~$65{KTRukMRc|f#u>5c!_~KnkE_uEW?Ax;>
z$!dM?DjM%rqVNGRQ1HcdlIpLafA8He*$2c*d0e7}Az8YBkD42Oz~_}z%F^2WEoKDx
zppr^PT2FgMAc2*PV@FmeG8~tTTslev*X2_|AO;G)SpTneC?EGj3dGI^l@s3t#*FB?
zF&Qn#m@#_es%}QnI<a!%jgx5vCRQgV3eJp;)m?26%_fzTDzj#Yoz;`3w;^Klh2r_@
zN!AS+S6<|zb^RBumdo@ubR3DqA(Qx6JWj`5yO!1SfL`4Z3%pKgvIhX4Qc`hbU8v=K
zmR_AK_-ZTtDsZim3fb$m@q<7X{DhYUJ{ig`*uC`mB)vMj;P)y`e7;!WmjgEN&pq3t
z3O9UHsnux%Z=*EP_GN{?4cNe&?ZFhR0=H075$r3qF-Nb?1h~)30tZ9cyS4F9APaun
z%RaC06>s)YfU8QAC<Yat60m_U@N99F#MyO+!r3!Us7xo_7m-AHYFTvLtb}h_qH92M
zUn)7NG97}AM4?1F;$bKyPOMC4TwbSxAsu2?I^xWjqonFN%~L5M5xiQXexpTS1I{*5
zKaf<KClE-ic@k&UJR!|%RJ(Z+C;b_nYfLj&HDIby*22eD(fGRg+s97uHC^)yK34sW
zb-$FionxC@=_a|YZI)Fa3y-LJX4UDs6^+_1RRyy06nwG%?N#rNUOzC<zjoCJRqsV!
zKQPe0Hd%RRtKQwceqf;g(p4E{mnO;<tCDwlWxzn$RjbyjlQ(#0fq}DECHjw6y~lg~
zz(D_+RT*V#5@qvsqA^uvvR|*Y&ZH0$b@4ho3siv|6Qn9Al$TneRDlGPsU5<>cUP~m
z>y8%J#F-lY`z#GD;3t$c*&Wa(zj`TcOh}F0l1tAmNqYWOHC^b908zSOY2?BQ5CSW=
z<*eM6SXr&wFZEV{C{-(uO@<Iyxg%%gj>O7Vx-d-h`!f(_@+D}7+K=l(#?=pE@<d>s
z>U^8m4fMK)Q{BU<?%{Z#4^y2>y>6h_y&%=SAl02b61hNiuJO8o)9g4g1Y{YVP&qVq
za;?_<^;Y{W;0;Q0r{(m$+*Gic6HM==Qo&kIFuj*b1?xG%^j_-BT6swLuwLb1H1J|2
z<zu`}Gw?&79N>TUOwPmF`<NF5K5nE2@Q+4n0so|=ykC{Q%79+0R|yEX{T$9_eZW~t
z>aFX|rmG2jo#`3^hPu+B#V-`d{T*<o=~@U3b)}=7>Z+p)L0Jy38jTA&mFv&Q-8c@p
zfsEYRL~bk1+Z!#wWFeTYEIS%YGM`73<vAOL<Wqw`XxApDCHZ#cUdlp-WmyE0Ggky@
zvJvbN1S~y*faO37q6+L0S1cvB5YyVRocsdKw01nyr>Px^Q$E}QhT*1lieQg$VL8vo
z0_-sWv6RHJeLI$uPn@?;Vma#{jRnCTvA}{Oaxen+h~<Ook!ML>8K0}AINwUL1?)Fc
z12~|h{5rHqdzYGE3%J5a4dAKksjG>uP%tfw)QMV7Fr7fDU_B?8*_$+w^DM>|@KPf+
zfS2_Q`^-dF6J4QT8ul6${GwTG0k1Ps1NdblCC|!JMNJeT<wigC;5OB{^IW#C7Vvf>
z4Fm5~Qdvo~zGsXr;B!VA22Pq+S}0{{%~Y!Uny|vXjU+!CSjwDfq+#IujMM_2tE7CL
zr;#>^lt5%(TRTdFP~`b65D&{#(!xn$1csh;&-p7XNi`tG#gj`jp49LJ0|j5aJg1S?
zGm+ky(O*YDvAIa=ZCZpU#|t2il#FA0(vK&fw%{8;f_J+5+q7_LLZu%_@sy0TPPK|?
z0I3Xd7#KR-pX>%{>IQK57IU}{h>OmTfsDfgIfn<@IDDEp+y}%<d9)1-9Ue#=-d4SO
zt6n#*Tt0<SfHq*Bj{xz!ILR@*SPFHZiz{PwmFcVkZ#Gf`h#A@VP{8#<4St8cZQ%iq
z?hC08_(3JPOt;#!RvTjrc%zZ}fd65n7I0ucvjBWZNu>o^|M@m!1m3Ttl9AS*JR=cu
zCDJ`D(kAd%N|Iq%GfLx~L9ER-t#>K4V5LDKoG;BxTAFcMO2#xZO)~Sx4&$_5s>vyg
z(;!EsPFkC4A_`U-WT|AO^_!5D28k?L$syM?mo;*EvJLYvFicM^lb%{GJ+*e4o*L<S
zzzSy=NZ`)lTALPOWPUj$jiF8q-|_Wu)hG=rz!|9L>fx%<Ts_n?0oQW@*E0dv3785f
zjnkq8WRR~Kr9tQ=E3Loy(z$As2H7cD8C<Sug8E4iBPxc^9Hl`}vO@qF!h(+mF$o_K
zM+!ddb4~pi$oTQhs??8xj33XeO8pqf`0-5g9PTPzD)-rk#i&6|F!1t<I$kgwLShVN
z+v3#NUEzFBVmguw5hMd#?^X}aFb{@O4~9|?hVa1aPQC#071jA2(_JNmt!=hdS*90z
z67au_)Ca_@>^tw*KtE~$SL2mDvpc;-C*nLmA<r8n8(dG)&Q*F9JwR3<sjEvznKdM#
zAfkjTv;97=3`nX)E}d-!k$JV14-&}e7JM}DBojG)up7B2>EI{5Z@}l3B<k1L^xj_G
zaI$H^Rbwx559@K`#ab1P2TVRDR2EPvKeZZIKrTWjz&In4Kqi7S0LDRK1Cp^`zA`@@
z57_(Kz}Fv}uMK>~aNdc=`Z&Q9b*jK;`Kbl`u91fDYl-<agkQa)uIyi~M$7do&q9G4
z%x9q*2EIT^ZpCl4Y5j{awt#On(lBt}4p~~uJR|UIBej6H8EF`Jmyue)Zz-wtjwxx^
zB+@345{Q)l+EE&WA~%FUJah({ZCW@fjKI*7^v;o-4_MA?K#Yqgmu5Vv;Ryx`K8p~L
zL|V^8dSga^9sR^sZtb)PPj1A3I8rj=fL#+$`tjru3%&s)c&DpBQ(gUdNAZ-5v~IGB
zXaK1UaTpjnoZdMyG<Z3oG5~zm9PR^#4i97;9>_U7(8l4fnZtcRyp%gfVCZmq=eSV4
zTB+CYt`FK}=J^N^&!v8@8fAE~6zZ^%RKQq0W;(0D|1nYnh?&d|b*~2hBTGUH_+urx
z;Y~j|O9h)b!F1ZCg0-Ar`gAfCtmg#Nr<18*e@-wh!&Gn}CzzJQ!<xEBd{%+K?wQq8
zu$dD~vziLla)N1AQ^9&pFwJTz*q;+jvziJH<OI{KuG7>#<+BQWx@T5X!Ddb{&1x!G
z%L%4gO$F;Y!8EI>V1G_9&1xz*kP}R^S~;LRUrf@g96f+9pewrqq=L<yV4Brbu$B`{
zvziLlbAoABQ^Ee6V4Brba3Cj`W_43d-AbQT;EnI(K-U1?uB39QrM0zv(%%5?WS{hp
z-g|1B*1J3-@R$z9c>BP+bIiLYaNdD<w*Yub;9av#>l_QC3B++1Mzc-pFUGh4ICQYq
zT){}|e5J~r0`S8fjI>@=H6MZ7DXBC~tK}Ji14e2B&o<Hmpif`!a~{%nzNN1Q{4XO7
z0!f&2t(Hk&jr83S1Y2v<deE{m2>h**<k+xgB@JRO&Cq(C#Wn~e6D1=pk|Ad_V3>q-
zGLnSV7I7U&#Nyw1D`^m=oT-4=F8F9*UHE`FQt)-FWzsDVq=1+fxeGIX^y3GqF8FAW
zY|#fKsRbVm(kgsFLMixY5RC8vgSakSId)tFnYb>^#5ItK>(Z5JTmzZ7E?t?%HIRwx
z(v@jk1DUukP2yUk^>U-tO%-^Hl8X1QpLu&Z$zT4C^6Kuv!Fwr94z$3%l~l+s(#ELg
z1HLEZi*x=)b>L=OvW8g27^}>8Z}(BDn@p-Nl}g^){o6rhTrV`K>Lxm`Uv5*rx{<Eq
zFSI2-`S520Re+)?m5Z)lxq8y8C$!PW0=q$FR`QwGZR$L=CEtq#o~x-U7mBog^6ODr
zKV4RyIQe*jS$TAdP1ZOep`P-~JYd~YE9aGZb(I5tyVB&N1YlE1@=Mwi+6%PtJI@Dv
zBIFy}`JvSLq15^032vH&zEn|Pswk<iIN}W|JGbxA5SUwz7Dp1xO{y%HuPerqC3P%W
z$ufduFY_4=7RkN4@%_olpFVx>pT7R)n<9_HzSugvciY&F#4HzTN&ngwh6YphFE$Sw
zoHIy#rp8i53sOZ3Qbox-bN8ek#ZF(NN3yT$)zvfj4y6^#-uJYzgDx$yJaR-F?!JB?
z^*d7~rwSUotEzImV{Q2=h<y_`u(5ug{WDG=yM7ic8ZKeW4!&k(9CbmA2MNjbv-g^u
zy;yb3lv~E(3Czc88d?IBLN<()X`|<pB_d|ZJXa!tRSnS9EV!w(qGkuQ@lCyoE@mRz
zS4KTjCzz^>u3Aas`$k+ovmGmwcQDg$GsK>WMJ%kJU;nrZNUoo~vous6vg9ERS%`5p
zAh~|_KAE!@tA;619kbjz9z7{jrWRr9VQgIah?#PeEau4+CN=vCJ;J%V2hXtEHu-&x
z9kemupY`G-A$PFJWyTREl#5T}P`$6J?5R3tk~^~4MOb=7!Lf7)-Ctcsz;kTRtpfK^
zQn5J1=GN#E_Ep=>TEIzlZWAtA8jIbc?B6l5<_{G<;7PI6U8z*evZ)$<n_lJF8Sq3S
zHGscVk~>eQ57g1sGQlPg1#<5J40WYX*w9rqT{Yl<kp_UFuJk^Cu?k?>jbEIHJ(LGt
z`CaBcw(jz~%rS5FcbQ|R36(=)oA|R<ZHvvh-E79ZS(`Ca=G$`X)Yj`8WwFV41MfD{
z5b%3SD#x4F4?JTW66)V<EKOj)krn_^<kY7nfUaFlum(hd9BYB0uCxTuHPdv}fqNNg
z02t~@AI0vk0$9$K(>BTjZ?AHSd9y<sGiAyNfBsEv_bR8DH)}Iy%9PW?YHRDJl@oA(
zBlQ6<S5kiYK-Z2Y*aWUN(h%^nKv%O(3k#WN9H@V->8b%gYNQ1~6gpRH8TH9s5bCF!
zt~zk0kp_VH>C~qcwucIkH&<ccLrqx&xI{_CpNFQg)bquYbW`oZOt0dMd9#HQGi8c%
zjoSQ$xzz#=wWysT;1VU3g+gnoXN*Ha{W??M1a4wd!$1@{_365UuHTui8gS5b^#O6N
zEEHN;$V?g3KWgghK>U+s4Tz$Ap`hzQ)71|gHdhw_an9-LZ_~npd}h%tt52Ew0U%ON
zSNhq}b1Fa-xh3*G<$<?X6~?^TDvX&jRk%!(a-}7w1^khb8o<5!%EELprl%cr-C%-E
zAPQs^0*1QMl0(;8(^UiBWTXLLs4Ly-ZdU;;=W>6U^1$0G_c3oa_c2o@_xSTKnpLqm
zzuRdi=FQrSnKE;&s<!&|DyJRbHHWb3EChZ@N#%Ic+PU8t<N1O56{fxk{DF~%fj?69
z^3>v~l{DUH+J}J{Ecm*aTy6Smz|UJm3xLGnBC54%VIVW(i3pu?!vsW1+<GRt7yqQX
zL#(do!0+>MuUm-^-wug!kVrnMT5~1*aqYXt5>W-tvP3k1yBev52aJF$w!ps{SMm`B
zEDVxV??GLwSJl9quU=((P_OcLTP|GHriEzcx$SN$Kw5iw67yy~iJ3As=W3N6Xw}*R
z9;BrFBAE&{bAsuLh~Rv)SOcO!Y7-bbk*<hU)p4ZhY5_lIqz3S?fpFEQb)-@yIlymp
zFvc~E(Gj!I1b)^?LqN>9EH>M;ZZV@nz|d%#HH@BX7HYt%=~@7+1p(CBw6Ks_9agJa
zQk9#R7bwrC%!MlOKy#rFV?-)zH}G8Jssg=B$!A1Jk)K|(A?D4_2ACLSnU`x;wP|4{
zGe>@@0nb$bdwCM`W<80SGB)p3TQ3`Ak7@zmX`~_GLrN;ADy`odV+(k}p{N@IUaX}2
zG{=w4Oji?#8QC_TT}k6n&j-Xn!52>^^uNO7YQP7KGz`Rw^Hsirr3{Z7V-1MS%qM!O
z#X1&Ar_?ME8_r@qW3i4!(kZe)oG4|-j=Uc~LZkf|qx~2SEu=4qZKru57B{KEAGe$>
z0IpHe#LCLiHjS%2UmO6cMwBJDW=zw**O{6s@GC}YJf(1JtIld&;Ypqp+lPg8_nMY5
zZ+2S3bcoFew{g?LOlGqFP5s|R1$%iC^JYDP>3LOL9wBI9CgTZX@gWryPZo4L%*4DI
zb*P~VMBRO_>GJF}=FO;Eqxrek2DJse&T^i+BSY|uCfEd`K;8`lhPu+76kS)Dt{M;p
za{d5@y3+Fpx~?=`bs!4ll`3GUD}6L{o9cMhbhUs}hgh{6z-3A*S8ZA!^o+oVIv8m^
z;u(R{7KEpxc(UQ$U(LHF5SQeh7>Ga4JNXuj8au^|Hi28IQIQ3Pex##^9~IM810H9j
z1wbr2KWgU3Dr2kxF`79Vr55X04As{&>g%!k{7&#f^@r@`7Saus=jG;m75Hu=^}Sc&
z=Z#baGV-!N0XMMZSAjkc$$1PZ#;MmL7xQKpIhZKG@;pWhGnr-NT#fD<dhO*&%$xND
zCgx1(2`$WIJYmG<c~1`Mw%o?NG7#}3Bl~;Wz_fsKjnn|XT}kB<K<n&bTPwy^-$Xsq
z<%Fg6Su@%M{?JH6zy|_9nr&JyH9v-cuT+v81lO&ifxo5Yv0bj^T&}fKmut9;g>fak
zo>O1XsIRB$7mn?He@=aWMtwi($+9Hvu~jrkvP)XOrF=`D-^W(bAo`Lu_VCX-c8rEw
zdcdc9b<ZYWRflPNWpaYxBbAmpd|#+tUon;zaBv}o+Xoy`lJh-%OpC7jOji@Q$aD<@
zLtW{%g03%^t{QO2bPWMRUFo3!UEefab>Mu{)c}UN(nA5d2uw~BKm<3aTr@u()(6$}
zW9C8?IBYH?9||D#tV+2t1@CNP$$J*CY^W?w3_R7u27uF)G_i8(gyh3|vQjg-DsaF^
z4InAVyz8<|xj(K~|AobMw?g}bX&i|YxJJ8vV=OJ;e=DghN?K2OM&N4~g$F8HM=4bv
zSdVSAnKRmKr$(DOqs?||H2H<OG}2nmXf0#3mNQz*7)}0s%5|EpTlDHm27I?=vWoj}
z)0{ar@POqpd7?{5X!K)WsX+U&Sy@6hr>n-0J@z2%!xKNENf}{<jLM&Xi3c4m#N&R}
zd*>PQTjdZ??|#<1=!i8eQ@^(vVF9fHzhb0e;9nQ}s;sqXZLtK|Vc@Tn<mx<K<S=@j
z4MQEcqZ*YR>&8(Un3r!$1F>E3(RhJ5G62Mpg0EZfcbgvrK<vl@ITwr>7u0}7j|Ios
z&A@-E0pY2DUux-H2n<xknkwY%5wB_rc%rFV01Q;wFMXjCRZn?Un}T0ts)m7qs!dVB
z6;%hTm;|^b7`e>bb%-&*A~p?7w&mARfh69sfj73XUI@gXW5YqNSUk@=x;+>(avurA
zim2Kiyp>rT0tObh#}Zd8{?%uBCU{HB@(?gkH4_zFzoIJObxRqZ{>=iK*#_UD(^TdQ
z@HQnC8|TloL1e<Wrv|smf~*41R#Nfe;=gJGcQd=^o7AKIEy!w9;c(zAc$sJ0Nd0=b
zUR|8vH!3Y-kjmM|G>rfcQqq*l(=XZK>CJYS-iC6tX9k`eF;8tnIm0twtC0LpsEm+y
zTn*QGXjhv1i^=|@o&;n-LJbrF*W*>}LcNB(pI6#7dX=y@-$5eZrX3p%r5O8zb!@}F
zqm*{6UPa?(rN+CWN(iO!mFo&kT98M&xUN%m2OP#`(gGf=r2I)9!5vMo2}FTB-UEiZ
z(vv*8wl-ZgAPQtP1BSZNO%7czF<o^a3S`~^LtW`f9$nK+S3eL18&poIfJ0^JlYNv;
zFl7Tk6v(nS9~>%6U+HP8lEd`6L1o|MTpsht57{v4T+dYT_n1#D;Hty@nIpZATcZMZ
z_OKuGWb7mUs~&1%o{SpApY5S0=E<l*{N);YE~zn3Mh)V-^iUJ?WYi$Opof~6C!^*}
z6@QOj<sJq&{H~rutO7Unu)ika$=FBy-X3aVo{SpA*Y!{n^JLT@{z45sH^ebdMh)WA
zdZ>wcGHMXtt%sVJC!=POieIBw8DijuM}$L6>kUfH+|Q=EnmJv~cB(5`UC?!b`Zcj4
z-?yRhF>|&7{DhIJz^lCIb8Q+gF!vikypdxH@YTvySfxSaLi~HOU|y%jCY%1`Y43NG
z<q=pbQ_f#rncAlPw9;jifVenWS!v^^qc0N`tjS!zs;X``mDT@IILRDL?xT@Hd+uvT
z-&8f<_nI-v6&BPKNu-nOINX7yM@naEFy~rOE#US`Ig|_oU$3O{P(temo)P$fky^kP
z9ck)-FHusdj@CY&kyyE=-Zt?T%W}{A=}{WkmVyAHz2HMH*HnKk*6)0+wNnGN*g(mM
zX0EA)Iu>SF&W3<w*y*abX<=M$-GCUEgBp-@ma?=6Uu1EKYwB7*u1z=B`hYm&)c0px
z>q}kh&$-s$PF+hTe(Kr)u3<*Tb4_O42e3)t1s@@EU8B`M+2ULP+*nESj>m*dm<v+x
zlc&VDCE5q~R;`Db!3J=t8C*W0eVaD!GQKMCJH{v9KTN$yeyH{{i5DY`A>PPs-DRrv
zGiGpvByoki&sYw}{OfJ%9u5rhe(5MGxK@?_DwAsgH+^@wcOt+Q!B3lD6Br7nH#-P^
z!vt$U1f@dG?CR-<O|A|^uAFFi$Tc<CpD~#Jg6<kMbdMQa@{Gc%N3oJF-b>-9jkM$r
zg}?J8eBpYSD#8)>$bmkt4=c~(W<U8D0~b+KY*RS-oXHIVUt}Itfm<l4c+5g$OV3Aq
zH6!%_7ntbD@<SimIL`AC2-gvH=n7+52qb&*hvA-)zi~IA4dI)fm$Wk4@}v&UTuJK%
z7Wx2?TG^m-LIv#8CW|{dxY8L6KOXcLSe!dbCyyL4f%hHVYd#^k!~~nbP%xcO2#%Uy
z4TxZNJ|R~#xjGQJ{Cq+XEAsgs5J54R&ZmWH=xj5%_%?-)c+zCe(!2FKp|Y3};W|UP
zv6`DaS18X7ra2j4{6S4IdGP2?lN$p5myxQ#2bEMz9vTmOK7x6L1=9z7jglr-Wb)A1
z&GQi~*H@|5ql{%CkW|^^f$*=M_X7$Uq0HnVtp}O^13(JHO&+jMTW0dm$yiU8=XW#z
zvO~JYsaubkr!C-1k712%05?~XtIPD6%rxztZGtV}&OHU^YVUk62t3G04PebkEnr<q
zdA}BE?+Pyne9lM>;PV~2taU8OXiZW$ORwWK+QjJJ&1fIc%t&IJZEKm)CPrr(sSg<X
zk*-ensIi~u)y-A#AIy;&@J~vbSUEoZpgr|#oXJQ9|M7t4Y6cFoOtgT9D=9xB%e42y
zUJ!WkdI~mkf@$v8sM?3UMc_flQBMtELrJCgw2twNz>|!Wz73$3{$v)Lz;~Kd9}pYP
zVzW)lt4|)NY@s@~*Q+Zg@Ls0B2Haao*<w0VJ3nl4E#SpU%8z3z*vtuLhC|if;4K30
zHc|ulT_YvWIMyVJnkf2?N%a9yRA!m&gzLR3@Vp7OfSWA~&nvX1Dpd|Pz|jsyTIYC1
z;FmfWX<hFbfp>H;;wx8t{*C$E1Wq(v{XiTq<EMr1a(Dsac*#f$--Qu~<0T_4e3vI3
zz%Y!o9tZ<R<>MkCUOEf4HZ2?#Mj&36jI?l67=d_MGSb3PVFZRgr=KpRKG$<T*E2rX
zb3WJGsn7MC&-Hfdb3NyC`YG1ES`*J$ZM1-Mjt@(m*1k%W5(ggF!AR>w&j>uHgOQ5m
ziYvb{SDL_QjZ_EXPg!TQa7i9C0r96~q=idTkigKpOhKv(*q5pRV!SLLTG$sxAjV5Z
zTG$sxVCYI(XBTQ3zw0v${7FZKX+7c@fiFHG99&wPD^&&;*z91WHRKtA@9$uwwc0ZR
zKik2`=yN5w&zsLp;5|kf01{z2xU>jTKFb9XVaZ5qbMtNh*bKaDwrLThXao{rsgc%d
z)7S)t@uqb}ykE57YCwXN)fGsDWd+e9NMQsLVaZ5qbMvkr*bL&WW#aA6#anBq@z!$j
zb}wEcl9>u59OrYrO^b+x5lA>CBP}8lMqn6RJri6#7hF9PTs;?Dx|*)lWPaQ7+yXus
z<T<@PO9h)b!L)ofU0%-PE%hoh7dX#I4dC8Na*0cwsA_N11Y5vXPr*gnd!iQve#S@*
z;AcB@(YoFkTfkp*Fw%O$7#qMpb}+J%a812y;@!EXs}G1jE=A2YEnITv7nOO~%K}3`
z(xFOqb%_+mxu$}>oFGqS0O+vmiDkXKOs`Tez%7)NFOF&2yR#Pr?y{bO&75GGueqxB
z3~v#*(Me?#@(mu~CQ2&3r!~Vf0=G9(a)*Y+bIoECxUos~0kKggnwD3e+@XC))n2Ao
zcZUZ4teLC<zhb1;FBSeikOcqL*lNHH%-Q6*-c0S<#aLRvU6qs{|5UJ<6U+>ws%?3T
zz$1;+0KVHu$(`NYL{Sq(N10R~5JlzaSotehrUD-`!4~jGN-EPp>ru}b>-tEf%MI(N
zm3D`TRe^UKsRqPI<~_<wBjvKWq5FrXs18KY#L9~F3DFJ8k5xHuepgAz2`P?low7V{
z)>^=APqtYIe3g>&HI(XVqHCJz>H~(l(z22#!-cCaF~Ou@W@^_vjHLzql#v?1E0k2a
zPU}9;2)w_8G0s<Nw24u4mL*B+YO~M;h8EJu4_1LiCRhiaqNEKf%QuZ3J5c*RX<|vj
z9@ehMjim*A*(o;Gz^#;2CXCisJtOeu4#wE8)Myi<TbQmsAZE%O(z?+z0z;!|4iTKK
zf^N=&4>4u^g9=v~sR~5DOvpH*KWO^4^itSEq0DsPV&#&LNk)^8048eZ`%P71?KbWD
zFJox|_gZ0t2HaOk<)G18<QakQ>R^mROpP`%I>&VN0Wnh!8m$q}2n>y;gO(btVRWA9
z8UkX*8LhQxE%A)N&}e$U^020L9}`@FuERYk&d;w^YLc-elMO8`V`%~LD6<@|RqlKA
z>IxTp@Tm-UKd`PO#|9qqY_Z!vQmr!rv6$yk<=IwwHmsZ~-)}mhG93cKn^jKQboHba
zm2IX>n=xhDb_EZ8JE-tWW1f??oIIV*Ip)Q(3iv=zThXtWfU?SB_x~J)xM5{Q={~OQ
zu3D!Q9nK#pdz<a!E8w>&P2Nq3-SG-=aKx*2-~1BQv3F-G$DO9O1^kVY%9%@R`e}@L
z3-}Htm5j6&dq&^~Iv8m!Io&jN^X_)@t_j4Sa*EU1){HiR_){{{TId;p_){{{`iy4;
z-V=n;Y}0z$GXh_)k(C;0?QZclfux{h?3M)vMrJ9HxSe3FO>2g^QUelq$w-TV5sg6N
zE*WWE<~0I|yJVzwpJxPy!KKSxT7V;QaFZ)fPL%JjWWpPX!-JIouQPW>fOrZk?Tu|%
zMu3bMth9+p=3qQlUyA0FTL+2fXI8~k;6q9(URl^!oh2wI3!Y<vRDt^_sgNaLhSAA_
zN4+fYJ)!JJwLylQEckXW3%o0o{jD}gnUe)?V;QXirz@%OnRM=}L?;XG^RmFfP&Ur{
z=QNRz=+%7@iNCVLw4j*7zVYZOdDhLz>7lQi9>!iu1&E7O6>1r3VFfa(u+k=yOaYPz
ztZnMP^!C&a`u)#!bEaB3;>Tv?^y=V=eM+^&F6Okv?xtGev|*P3dks~NSBK-)ZhG<h
zpUW|pD_Z<fT(`>W^V%~>ukzj$5EoJ*dPrN6Qc+AF2}-rZp~K0Hme7GLeUuk%Ca|yq
znX9nU?y(g9-)MWNYtHHUmCrelDAH`vLp-^h(?_hSmN@5U>QZ&Htzs?UuaxAr%~qS%
zOV6+~5O9Abm5j7TJR|T!9gJhUQp>qgGgr1U3pF6d%dMUk_T}&Z3|+}=^+{GnVpk|H
z@`49ai9pQ5N*mk48t353$gtaJuza5lQ5E<zB^8_02AU4cJ6Z6amZ~anj*<#lTq8YB
z7W^JB3p_WJjbossR;zPS5z4ww&l-zpF_2(WA$mx7QvafuKEh75&_lpEE%Y%?sg^k8
z*zP}91ZaCEpFmk#>3P;C5J-TL(;`F<;U%F94}D!i7mhgeHChTUS!wG6@Ny#!1J7Jl
zauwg@pmDwu%k_iS7nCX=!vK#R#l|pj_{=~djVqNXqQGfqn<(%#N-E9LdY3V_fIl|U
zF!0*<1ZHV`Q;A|0c<_78EO6fY0#O=M&NV*ZzD8;RFZ_UUbsL5kS<ah4hC?2#0rxSk
zK_F?Dky$%R<2D<ECXgH#d^E1Ggg1fYv*4q##4_0glDvYC#<wk#O(1D0_-M?r95sRD
zsNn0CCGsE}0+3>qUqu9xg>o~Y^^oPH20X;VUjQUCr7W!*jIjpX#!|8XNVZB@T2!O>
z4E(&MX#tQ7ma??&GR7M4&02;MFOXs^WoaGd8G)p~WTe%w!KeXmFwz3xG3S*vT*}fq
zOR3@waK`&XMp_3cRTzPfbuiM}=3kjB3xMBKQfZpjFFhlW8Y&sP711P{M0FsOX+q^t
zAY~)<c(u)|?^q4hfs{zWN8>W9mpYJgDEMeBup+7h8Tx{c#&@j_>Oe-X;G;3m#;pz{
zCk0=(l(5Xp{s$y}4neecK8x`g0TK<Yv{`gTE0BO-r9I~y(+YfrF2S(UrkF)5knmxp
z{UNK)5g_#lD{YEWv;wI}SZV*x3U35Rl40$Z=S{5i27u&MehXtRnDt3s%>q(cjt$IG
zBy2!Z@7TbLtjGp{q}{QB|7tZh03?r&4g3}>u>l~_JGO3-Gp*z{6-cy2(l*<nO`DK~
z6-dOe(k5(S1rjx^v_ENCSPUd`SZNc!Xay2IthCRyY%B(n4OqKn<V{)&lC=t$$tT~|
z0g@HR2IdeYY(O&N*uXO^<b^=8;n={;P0<D<6OIkcIY`)mY&DJzO!&eUNB(P#cr#r^
z-Ob8W#jm!Nwfa>(Ek2@G$M$>0PufYc`aWHVKBrg5cD>@8ZIP<JOv`$%UL6~Fe=GUw
z1-bxzNw1C#{8d|=s+Z`ZbcbFY+xHazr(Wf}_6gm4Z_=w{yF>90ZC|cdbbZ=dua0eo
z;@xcRs(wS^0|6WO51#Ez-A^yqtJC&r#n<Uod^=2+ua$arZ09Kcs4Z>Po%GClkX{`d
zc-YRf$wz=3%U)^KEkD+(__cZ!C#deb12!<_AzzrUYgP2=bz<d=32hdI-zw3027b<$
z5i?^=P)1-%OH6F1TlMSoT9{xlxlM`A1iRf~LA`;gLDBc$+WiN;7Wz2&9j7zW=>wl)
zv$V?es|2+Qre?*&$=ZFIUJDcVXyeC$EcjuiN#cQzn5=x4oh^4+Q2byff!KXiyZ>9S
zg<aNyGlCfbW|oNTWUcy*^;*cXf1j^Jmlp7YHcbYAzcg9-RaTCw!@2@H**g^fE}((;
zwuv$Te4CPrAFJbla9uFTz<=|yz`uvGoEVk`GY|ZAn|TAk+m%$<-BG99!NGI{f6Aug
z0PspB6|x*ZULMR*@KH8L2Y|;asgR8q=XKHZ0GCfDGds2%G>*~<%LI08VD7)<sCnb)
zXLTkrTOA|#)wYTczEvT!)v<xu#|nLC>r`ZFI!5p>wNGAU-AJb$v(>SI+3;k$+IJtF
zb<9l12j+Yr;R2bNjt$JlShz=nY3LZi93sTMm+I7E#yK`H2VCbKnAzp{!0fbz-FN7m
zU@AFAFozUjJa2S6ofFI(#|Y-=A}{`YQrVbFjt$IBU!ji%^TsiPIaC*nZ`5jMk~l^%
zN9uxcV=ZlFgkuDA(^oLkp!^*p*dIRz;|jlDOP6Da3@?x^XY<M#mDw{!E7Mm`D&O#!
zJ!4g6I=$>hTey5qPWis5wafuV3;^%au8e`@mFe`dEp3syanYrtlc&?qp0#Dpk%IGd
zod$kmssY)cHc{b%e~NAgoR-9OARAA{b;%9A>`C2n^BygHzn#>8Y%3iOouk`{-$H6Y
zwvvr}^dHe#z_}|pg0X3AF7Ncro*~KhLwj0fI=#P91;RC&a{W~&1?M$U4`dH1)t?-y
zKfN-Y-goG3QL2x*zNWpL^F%$6Z6@zMy%Tkc<-I>qd(YLYs0XqWxfh;_upxX)d8MPw
zM?)tkr$<o&WJ@w75Wb)?rerJKfaciVR0Fa%nGy)gmDiNS)0)}IorL=h>LQkHbqCts
z&V)4}Gue2VS=?t>#}hhnpV6y4#RD=6jh7k5U5IsjRcGUEdKD!=W~1@$qcib9JG0e*
z%rWErvQDa7^eTFROe*7L_Hegi9rx%=yWehKYCvY1@y5A-NSD-qu@qN<49nK(S|-cJ
z>PZ(}IJ$b$^fqja$hLk``-vI~T&n}|pbbbB$RJKlO%{vX>Pe!g4Hv_=P0nJ#&wb|Q
zHh5Ja11>9(i{^OI4vBHyyzDAB4AnZl{bC*5x7Z+8fh?$F<PA1zV$8g8gL|dbN)^aL
z9E1UhGRlW>v6jr&t#qnD$}P|diK^NxOii)pPH$hT>+l!#DisZ6seUDEm9Gz3$X8F2
zRkaNd%enAomh+PLIbGCWVu$-0kY&3x6))GTCyBi_JS^>{sXXt$tBD_aQ$UvRUQxlr
zGT$qzGj!p;z!v)&@WV!`0$J`~SUK5eb$S~zyFk%RBB!?x)ph-Ny~-c~S^6V|kl7I;
zg|F5%etX*&sz8?i;@}rAKOEmiZ27SR*RFO;Fu(-<t4&}#+q~E2-vIODX`3GdJnVUy
z72^O0jISE03Z(YBjpXT-ZN^=nj+Yb0R#r4sAhlPTN^5Sph{H2heL7xF8$48!%^gVX
z^@<9fvFg)B#Y3u(Sur+&R9!Dq@KBk;OI>ncFBSu5iu1|oR;E=T_1Mc4Jk)G2Q#>qt
zhZS%W$T?=JpI~?@o5o~WxSX$WQSQPW#}#nL^MD&=Zg9hQ+PE?0t}*Q629f)}u#bB<
zZm7b(ZuYq!@9{Vucl<rEJ!8j19-PPCiPqRDSYwAC)rT28kqDjVAv#G5`$!;OhJCn)
zTVWpw#J{kwn{WQ1ihUx2yM%?SJU5XH0P!AHJm-OhumTAN);JL8aS+hs0Al$t*jPu{
zHy%AD&$>A|J@j?c!()#g9}J?F2Nl8!Wax^h<yG*ptVHkW%$b@zc1hmT5uVtAR7>nn
zPD||DB0YvvKVjB+)<(gEeQ||^edDoF@~oSavq4|Z27TRZ@IX<92S{iPPi#_HvBdK^
zAptw4KxS~6XlM-+5znl!FAgB5M}xgJr-#07dYJ9O#K%QuXRKCOyJ@>jk4aK=n*}dy
z%gbT({F@!5mH=@f6`}`^ib{<X$KIt{VrS9z&(>>B59y{-C<`~F^o`}uKUZjYwUgex
zM)O~Wf}Wl2{&q2tl%zrgM7^gR^bm5gtcdA2^rd=+=J9%1`Rsx$Xkp)Y^pNhH9{RfJ
z$s99qky#O|&CCj-J5i%dOOu`zb}z96hzqF@J$RIx6|r}zmN;~FCmx(j6#{vN&dNxe
zC*r~i<SYm4SXQDklKm?-nv{|7#15odVt;a4V&9(EL10A*3y#AiCzG&mJbFr=b#roh
z=<BA350@m(KwKovsO5tmVFmIz53IC_L|9|PXlKs%3O2{vEufe;Gi6}eRX;q7>Q=^{
zo;~bC?Il3`O@-nJ$rnlJ6=9B7BfPbnUXuUMjRTfnpbre9GMDv~o{j90!eSsUq(byi
z3CY|L#q^P&R7)IsyyEmqX^vM@I%h1snW916mioLc9e;YJDu?_9N+8=yDinJHCo5&I
z2y?s|VNx{kaxRym0b+{D+s&nmedOU6<zyh~xx^x048%w}w)9{=S0S-0@tQ1A#EWpZ
zC=h*CpV*`h(NnPpEsKE|NrmWPPa01YYxMET)ApF91zdVQFOV(-9;qbvHgKy=>nq0C
z0#3fbWPvX;QVV#pkro0!W~3JIgC8<o!1I+<`a$cw3yl%@ekGNRw5~SB<j0-J$Fr7?
zCh+_ZqP_tn15SO@@^QVz&;a6M$w=$V7Fh#`izOp1?{M-LF>rVT^R5OQwsGkLh7Q-v
z;d_j+24vLaucrg|w6JPGMoqYYjG8>b2A*wU_5sO&3$xaywaUh&4@d?|Mp`}}$uAL+
zkLPUw>%iAriUxq>L!O1-GD_p?mX11*929&sHnxP+fn=fJqd^|zT@WAv6nyc@LI_lu
ztSmrWWM!dEoe3+Dq{2!Y)56-#@GmWQ{XnF|;WeY(<o;%I13(;fZ{UEbF?k~bNJ<?W
zm?9H4AQ^RRU{We<K+43ifeAp^y17qL$c_ud<symlLN#q15>_Doz)JfxtCGb)e1ets
zN38xA192PHZoW^}+Ln+jU}WV7KVz|*mHlxOo7o0GW7DMiEA{<lI)mM7LEvdNtE)d#
z_(JvHv4OX=n35M;@$+8-C%{{JCxBCxRD4U532{{*3x0#m-(lb^B~7fXn$V``eyT+0
zE~EH1jo8tcNk0f^;F?803~VW>u)%u%eI+`5;DanB!@!!73fXwD9@A+>zML-b0hXuc
z5ei9-V*?Las29!sjAGK@_`u9Pd9MygW*i%s+7`ACYH~=HV*|6~NKwuOlNiSbrv4ot
znBsMOaYCrxZ&~nFAOpQoWtF^sBj4YfJRQ<oG;ZmV4}yv=>iY=|LO!JfGMa%RNQ-n#
z14Yb$-&k2xfedY+2-1*_dY~u{^(hVU#p=B5AwX)oe3j&k%Jivfe0OuA4x~nncXKtp
zlhtA!NKG1V?9<Nb6N_PZUqEFrjA7qJYKx%_`^ICV<XJZ-XM?_+4f?v-V3PFMXE8}-
zk7J)D1Hu|RmDzg9Df2lx-OF<TTws$+Ibt&@N1TD0jwH)NI3i3~;t2c3qeruTYfcY+
z-SlKe1Q&~qP(E*n)n;-^bZCq6AvcTkpgecS6dOvl#8EIW7^z;F%kg&04!o?w|JdOJ
zuezNmuyU%dI1+F42?RYWZB051h-;}3J(O{g>PRttoRd;5^bkZ&OFSIss+(+R;c#FP
zePQ2t^ypC8k`(HpZ!A4BjJQE~LIR?(+i(%xCHdi!g@c}t+i)ELBqdRzgVhE%QVx1>
zFc}Kb5{G`9?f{SI)ot#EFXHw(IVeukt{sh~1>9Fj?*1_St@35s`ystLd*GW*S#l5g
zuy*b8VO**KS1YN!AEotU&j=j8xRfm!X`Q81xjP1KyE<f~HCL&^2)v|&k=BnqBk;8!
z2{qE%OR1s}`27yXc(BqejKo=BF;4R^66XO{GV#1+VgyL+u+k<0vcv<49#-0<Kv;pq
z4=e3YSUyI8<O9}j8To(>cO6Iu+;G?1v~X8WZ9qIN8EN6HFaq(jWTeH=2_q1%OUBse
z{nS?+l_Tz>O4`GQp!ybtjE-=9L`jT-koMJD!)qbkuO$3)T$)(?5>o8cTh)X2>2=e}
z>2g|^BP=hVwtucYZ&WWzj^Mq#cI<Mh<9wC<eus|G%Kh3y1*SUaAq%M=uh9%Jz;2BJ
z`$P@ipll4UV*~r-4FVZl#|HKZO}>cpGEEl~)QN%p$PZqlY)nqa2KGZg2xN(JY+w>G
zp^`Z9V%3hpa5H5_ARhL`)sfRv@~oSa(?ef3JtRUF1t2am(@{$@gcV3KV5LoE2rH1_
zU>(aL*;K<%FJ!}v{Yws{!V~+NYKdLXX^F!~wZu{7w8RnO8<R5}W-OAWux~tiN}hGM
z=Je3lO%E9f#uXPCEYuSAdW<Vkuh9t*<?`04ZF)Yf9CE+|;#w+151u9iAc|wJQ!T_p
z+H+dsw86_dCb<Bzq>(n-tYyLqWG#c0HruVR0$JK%jUB=Y{9)~UM)#X*^cwcv675A{
z-*{}4JnL@F*`P0HgT8JyNYVObEB1nH#U4QKX8jAg7p28Q&zF=#>J*5}sSrJkZ?3}V
zV*pbvad0^;aiBS#IOvD91YYt{4)aamFD~&1<@AT-5j@@mYru<@<c?gmHZ9c23A>vU
zuQOdEz<*Z~gGT%Jo)!3J^%qv!*C<`835YMS#txB!s#$3Qml&x5ByM@$OR%5tT)@kX
z)C68_qy}&sbG8ZG*GLWE2}WuHKVhT>kZ8p(;Psvhc$1Nu!0#EU0VHFht6Q=dGIt&V
zUt|$hNev??2VWo~=p@13U~)cYbTA?V3q&IWOB=1i3Zy(>rA?~-fpU47MY|Y?W6_M3
zg9<&kn9Nq;p%0%^E%e}hP78fRmTIAgr02B6!yfzgf0J6Q{8UpL`7V1rJrDZi14&LQ
zL=UM+0v5&ek(^WuJzdi#TH>@-E-hEps$S(cAc2c7V-0Qqk5!UeN?UDO*L=(vfnQTn
z$w=$=j~gTK`${SqX+7>4yE)u2hnv8aMj8g<qEp{AhreNwHG!L148uUYE@f%`!Wf&t
zEzR3uAc2&!w0u~}s>L|mZ!wGjiGh`Y_KxPs2oNV>rR@!mbj#=4&2SCKsBcg?Whr=x
zrcP*!z=wNUKllhwtAda8wE5s;JZ&zR04G*Xxp9=njmjwdHIQO+{(--wNpb#xul2Nk
z@RvQU3ckV9=7VqYw7FoiTKGrfUS$;jfYiA25ByE773UxLPEYFxf6LRV;JZC-KKMU8
zZ7!H{EBvG3OCUM#q_tP4_D1~htN=42uD0torzk;r)&*otB-RyR#zbh#z>JB@6PPh^
zc>*&gLhA=JCPJ%%84{t*2k)tQ68C@%iLk8zGbBP=24+ZvwiL{e2yGFVArV?Xm?05b
z70ieTZ9dqKPV&c~7#)^0IpqTxiDJ6T3oo?)Vsm9NkUYRjn>9?d0`Un}+CQ`uE(YQ@
ztlbtD%1Pem0a8tpg=Jv;ca=3@(NInz45XTzFqmo*+EOspB(z0fs!3@5V5&)IRWQ{g
zwE1ANCbYR=%1PV<Qcc3P3`{i%Z7G;)651j#)g-ijFx4cqDwt{#+I%o0CA7I<hA8tY
z)eE#tw$STFm1ULkySZ&hZ?H<L0uNA<yxB9_UZAVqv3d<uKw4=FV-@%jB^4@;(Xv{l
z*FXiN^R4o#z|SeEP!X3M2ZBM})7(KGe7x;*RZ8*i0UNk!f!DvG@Y{NI+Q7dtwkow*
z*Wer*c#Y+t&MbQ@U;}UH<D`^El;pI5uQ#@2vDigJAp=efe2GQgx<MfsaBN^Eqx@*y
z=QKJp;Ml-VTI8*bG!!!6*ucy%`Q2kC3>k22V9rf)`~;E##|CCXh_<-C9a~%n@j4kk
zP{Mz_3j1(@xfAw{$41GsZcfeyeK{NSb+f?`$-NDbC9_z8<xLArl7C?Z5-F^)2g2IT
z@cXr1qI(XxC8XzkE3U;r>Ma$*Nlw}+2c!E_E%lUxo(F9*ECw=csZbnWTfL^{!+|C}
zxDxh_M^DMK?$(?h`nu_1bc2D$#d!2^FEe^XhqfpmayLj1%5(RfvHz)-I0|Z#;SDoK
z5u!Eh8;>3`m(xREH$9w%<g5e4MGi2iWnBu6MR*KrH!G`jVU#>YZ_Lnhwk@4YfcTRN
z(SxVO5Jie(uTw3=Ll8MFaX=qbH?iDnsB*kI9CNzqC9)pF(M`0S&#|mW^jzU{48&fP
z9TB33NR#XcPwYzN6Xmmv{q-tOB7rN7Gz?t&ZzXs6Jb>1<N)=h)i!Lu2Csc-kw<xKU
zrS$`2Yy!7zn=J5QC6%(Yo;SwyIfVLtzr`>D#A{X{+B3|P5g<;&O4}PA>6TBd$&DKD
zP)(B@YnOpB?gFc~X<=W4fos(}Ck(#U)0Tq2>}iX@H+Wh<_^X~)1>fvx^TCAd&f?%b
zE$?;UkCf!x1OM35mV)o|v_;^bcv?UBXP#CCKjdli!IXk?59~)LIagBc)U(W4Ah~B<
zr2T{y_hKM<fR*;6nm^GB#3xv3KccY+D-gF~?N(VeE9rjVkgu$z;02zx2)xkK`oSZf
zRs}EdwE5s8JZ&zR92HM!Xnb86<r^SC3RXsGDVTy4+9EIoE3|$v1uL{Fn1U7Bd@uzo
zw7FmgrtpsjLnHnHsaTO+3Z`O(wg^nc3auYZ#R{zorecLQA56sxZ7!H<DEy;A9b~>i
zL;13zxaSaB4J;_4eg&8jaf{19W^ozFEG{cD<FhO?K1(y>vnVq@{h9Hp#^dAi#PIBC
zGh+bAkhs{u42jT|ff*8^Ed?_qLR$o8NQBl8W=Moq1v4T-n-BJ*lYFJ?1zPx9>os@+
z0O^f3H>$vWlvF$cXz2=aoL&PJCo1V|TV$%hOO#YThtT|;tJgrqY9)QjQd|YztfcI7
z2$q?Ctu=R#3O?ReqbjBN_ka!jP7A!rqHw2@oHp=7##W^^->$(qHt?q`2Ti8%;{h9Z
zLmwxlbhwh7Ht=1>mK-ByX((jCiGeS%$OmswNCq4mn8_&j`d4UlWWce3e`k>oPS8-u
zfMWwQbL7u*Fk#4mV*_(;k{9NHWWce3nGoVzT;Gl@-htw+GCWD+KVF4>xWL>A`^ICV
z<XJZ-XM?_+4f?v-V2Ir9sg58^X0Zv&a|le5e_;g@DXg&vnLAk$_<pq}NBHPnJbKQz
z;#v%(-cli)<fNT)FuK3jQcpSPS!a`BF_2+Ph2q4v)oW@#9B9&mD`DSw^ypyTn$tsH
zH$9AQFwnRdj~?!2Mvv&w7Ujb|hlt|OK8Mf*V<^=UM?p<8ykX`jLb4L}jYkie%ju!7
zn;wqCa)t%sB4=3CvMvQDV?2hno0S#1FiM`H#|8A9VN2%{ApWF6^x$c7k1WF&d!1?_
z9)ieei356{x{2jpLzUyz;YglCz)NI3hNGKHm*sOTj|=GeH=ko5_M+^F5IsbiWJh>n
zS2kT!-t=FqS9yXCT=U7Y%N=7|ZCbxpsxSig{8Y&(zZY=TD2>~cD15*ld^%J}>x3&y
zsxSg)e>P;K^{3AnBk=Oihm5o~`+_k7Z@)TZq_z1q#t6J!NoCNq-oDltyCv{tmX{ik
zlsR2BOW?LwpsNPFN=YRnt>1b^AUP`;X<hpn(+J!_lUg#;BERyI0q}w#)wMRQ$CWCK
zz}v4Xjg%T`O|&to0U4W;k=Cv{Qlb&aaFvX-4!6Op0l%-Ll9AR#9a7N<WL(upW!UzM
zmLA|rC6$b{zTz2yM|~-bnpRt>Vj4(=lp4n_k~&3lvDM8WFsz%p)eSWu52t_>fE+i0
zjH_?~|HCHOAP{ebYiw6Xa23DUyJ=I#vUdZiVOVKXpTY{HIANuY`@#yOJYl6xjS4G}
z0)>?}#V4#lDi2oLl%KEysXbU}Q&++Yq^Mw}O=Srykg|f6wjZUDZliRBE!+J-My2@m
z78)$o@^AylLS69DSZ=F!Kak2Q_-IVAC8r<AxD|Xf=34Fb14(beN8>+jdFTfceZki)
zeo9lG$N&l1&BOtlJrt%e0;$Q8krs1H7=Z*`GSZ@cg%L>9B_l1WR~Ug5ZplcC+7(71
zWm_`RVyP8IAnR<&NQ)8|Mj#n48EH}A!U&|cOGa8Owa&Dzui>gY(5A&a5Jn(Vpwvi<
zSs;u+CP2wZi=|c=fz(LJ*sVx7(8vi5NRbp%PF{VY&EybPAX5WY+MH^H6-a5oO8b>I
zTNVQ;4p?bZA)*z?7{N-LvyHF<$p@_6GO|XO4fhK|;7e>H8Um7)f=#>Ib}AVHGIbmq
z_y|4g$!j3MWlC~v;88n23<1wml4AoCmuQPa+D-jp^OO~CFLe=Lq+nj8wiyxOx<?0v
z%nKJ8XPXo*Aahi>Hqnz1vr$Ofsn5(iA<fejjnNX)@p_J7E(z(y(Hs)e>-7}E{1DRZ
zN@6Am=?i)U@tH}Ui?Sh{WrHo>>{QI4%hwi2;J%tj_XG;;lRCh3crIWAPqSU4%C7N#
zB{^;2>pUAnbf6BEV*~qP8)Vp~TItB2yiJC3fJrq?htz2U?_qPV%836wU<3Oq7<{|#
zSyjC{ZQv2x&#Gy$45Y;}NU^*^i^eGf`vMxIfOfIsm+$l@MKwrKy-kbDX#@L08~nL;
z|0-YuPq6cKm3ll`NlqKs*XSTMy1CY>V*|6=%L$aaovFp@*ucJ)2dU+61#Dnn--Fcm
zTCH=Z4eV<^`C4*K>zmqlV&KndpZtK~*R+T!YR3j<eV2!`16r^YwPORb=8Lv7wIV+n
zR4MolrOB<r(+a6prwzQHRq3HrB4y~<z|`D?%Ax0JC0-s>Dfl+4(%Qoc{~WM^U*qrc
z<|`c5t1}8d%d;)hDt=#3&EOl9CXueu%Kb`E&EQ`vP1tVH@}yjyQ7}82M7m7t?tMZ1
zfp1WnXnU33x$mx5rwu$}H~O_x6rLZjfp73^-&OdFfDOEnzx~@*;jTfMf){wUzbo8a
zpCdTmz;Cr@3(1@H_iDv*v~^-&zcdd&sAbB**0F)Fu<d%7b(LeSV*|h2%5<2m*>9D@
zM{9L*&UKPt%2hrYp*neBBQK!<{r0)=YAq*@xK0_^uZRnQ9B>^Q_&rub!<-QO&O7|9
z_HyENl3>3`4$st8kMpi$1N+r)7|3b2@I$N4uYSY-tt8I6jt%TrzhNLJUB?FYtKTq?
zbFO0p`_*qaJ_}JBY~P!eKa6){@pqE+)ocob-8RQVRj}3Oco=^+(9J5N?nn4KY%#2C
zcsU-%l-(|8D~^-Wq~O9wx>Otm5%w|qEG}UmW66WSjy_!kNPXDX%{NwdIh_NUDxA(~
z6Na$H-oqMuF08S`u*T0Up~sHyuZs%{Nf;Cde-a(`;VrI)eIy-s!#<LZ-(ep~$M>+W
zo8R2ZNh*MZLdb-{)-J3-c6nH7`)$4JU0s|2RK&qUj{}Du2Ms+AIMy2`C)OSI#rng(
z@z^MN*3HS;pf6{GzHT;Hi+enR!zAnRI3B@KR>B%PRU|g^XNyQI4#_7f(Z|B{P|wK+
zi}Hy|>}#rpDA-DJTIeH;R7)IHPD`BfyL!xVDt&$D_;_qk0y!J>#nZ@@Cw<*)u))b7
z13Tut3^FF!=Y$o=0f>5_O$LNDb_&+mAyyWw^_p8bUJdltZhBd!)^Ek?rtNCI-HARj
zyme}up0&y$e>(z*3#kx2teMH@jG~x65|nC*osA`Wr8LK@DNR17jd>-d-Aunj57Lpd
zGRf&V*dAdl0d|Q{gkm#rQg2~|JUYnjs^ih4(chZWLti&N%#Zb(DzT+*Q-!GS^~nua
zS*aCzeq_mA48%1Vkw}OhCULSHiemamc&a51BBv!z+t2h~iF@{N7;U4_n~Sh-JbD<a
zoF4kR>EW0u1rEf;Vr=9wcdRxu7l`gR8f7#WB&+oN&h8nP0I{12(St|1H6!*e)e?u!
zZpq#lrkk_mcn<A3J(})0IX(1s^M^Y>H>0A*xlEJzPEc665hNN|@qoKR=(LGYRE*_N
zGy`Ojiv3Gwfbhh=rdndxb6Vmsay)TJsiio=#rkZ5o2jq_SWG$Sg?;1EQ}V2vlhZ?A
zH$B`R%b);pk?}(<4@aQW=Jr`s0C_3`D{UgolmOab>6L^WuLhbNRp7<7{~a$c*QZ0#
zz{|2j&!_Bh<YFK$q(bzNr=;dYF?}Q`)e?_!yioN@X^yvB?Zmv9<r$W1^x3I68@`?=
zla-$9ltYemKyFo1p*TV~Ss23zkJYCk%;&IgJbE<xTXTBo>!ycWDJeD}cTP+R+PE&P
zu_ai$S(&c8cUlSbY^NMDLO}dUh3LVvToKZT*Qu5`h@6%<pzrBZNfz*MsJ75&((E2#
z-+1(tJnL@F>7lQi9yT2r8z3$+HmGGsfKD5?MMXC&4`}phF6nt#Ib;}t_>&6JgQvLx
zh`rA7#1W*H;(!+EM+9@OE#F3yPdk9$R+4)IqSmJMwd;%#xb>GyQprf`bft<$;Nt5;
zMp{=YRTzO6-4HU;`jk?IaqKYagmJS)HV7mZ=Wsm}Sv`(SK7%AIT$K0QfcPg|K<{eu
z7F?R~5nRPD3X(SNNI?Q|2UgmoT3CVP8CKff$B}O7yum&|>IZ(y(zy_bn=YOGmd>rM
zQ2K#`Mp_7bkCFO;&s*Uv1XhB=>2K58$?9PtaAr_A{cT!|ihPg+^uZ^8)G8gIfp~yi
zqyrWyV<PWP02z*wkrpE&j6epXWTZu92_q24OGa8gjN}~+!uYw3;bI^IR#b%CyU}I@
zWX%F%2v*wOaPr7wSSy6tCa=20X%W)pI?9ABp?+US85_bC7baDBn+D}Ry#oKh0`13z
zqtzA12KH_ZP?y_l5jr;T0cl}g7_fmq=h@OA2MF?N4b~X|`@jbY`~gjXV*~qSBqO|9
z112d>4E$?LM&GlV1fq3p;6Zc056H+nHZY;d8wvNS#G^qnz?<64t!=8IY!`$CZYfRH
z@wqx*`}FFJg3qz(T_XvX1#EF?{EtS=Ty?6ze#{pT3UgB4#n@flW==Xu@Lw&Y1uQ#$
z;1&Rxq)rmNgSo#TU56IL-s34%x>;o<e|XIPm;pXfK{+0*Q$9JK*fzDoG7uiuNEagz
z_KinR$+K=wP7i(E^pF$T%YeAZUWQsm6FO~DHjeF(2!(YlhoWPZ+!@9GC5JQNiG9uS
z#D1rih=|h5X`zphQY~?W<P{%#4RelHhcda7hnIyU*jX^e&eF}LAL;TXL4>!Xa(7M7
z{mLP?qd@GXLiAw1$X}#5cI81eFsN%@uk7S_H9N`Y2QjZ?r<>_N<g*}UMbDpn7J&F0
zWkG~uGt?aw(<{OpuSS@RKfF}!KQ|B2wu3IpQC&+((DMdcwig3&Ar+#BX_HJTQA{5R
zO0~o(y;s-&CxSf-ywz9A`<u+2%!N0VU61HhSb+O}t?Y6~z*?KudzC7Tz!%*dGSZr*
zRAB_()4@n<oo58@e@m#5);UTQjllal7-{{%GXhurd#Eu^UYdoGI14QGX&y%6JitmO
zzGMN80Erz|+9W^<7fAH5(k2DM3M775X&<P;3oDR(z}hV%d)jc<fn>ndMcwKGcjZPE
zh^HkZEu0lbAbysNv=};J1mbnc82kLR)-aCBE8!ni_kU|)RDZ9K(GjlwbbuHIAw8ue
zycW`VT2=VxxRiunLW-UGy*jm<noa){)A`!-U<b#_;Ll3ME~h%)qVBHJYszt+_E3Q-
z2R&q=<Ou!vvFc!e-T4#j6E%3aW`*H(Y+#?fK_G+c*uXxa$&1l<Mk|&RW3u{@9~{<f
zGC3U^*bn_6kR{5ofk}isXo*J^gR^YRPK*8&7(IU6Pm{mj8~zzDy!lCoj_{9#(c>$_
z=?FQ}%1HdsF+0^NBj%vT*G}wdT060hw00av=xLn6TcgPu9syWk7=W;EJbFr=b#roh
z=<BA3k(IR)NL8^`qLu*^Rv_gBD{b;4tU!W;bu5RXBY>Q$V*iqZgz&_^rdndxb6Vms
zQY~>*IW2L7_(l<j!;D3;6!wirPsy|H)|?*ty6GV!!MNfggN0hcUXO7l>Z(oq_=OO9
zn#v*PbRe!p10W|xdhj$E0O5(fPPGsZY0qhi(*`d)f#d?nra;<gvl|F2klg@Q+8kJf
z70BiQYwQqK;16r3rAMJ*y@q{9D-Hc&-*{}4JnL@F*`P0HgT8JyNYVObEB1nH#U4QK
zX8lmz&C_C`XQ6UPodR(=6{3gn%~cqE3}C7y4lbu94m8IT2YsQIz<qj^A0!37@ayFr
zgnSnS_>7X=akSZ{HSbm!o50JBGz>iOHh)9XY||Q1s@MfyWu#%?vTua4v_7O%k?j`N
zQj1{(h}V<>?d#3u5g<;&O4}PA8QX9j!@pBs<hSvbfiYg5?`dIQZUlgHH7QOQJkQgX
zg7@*XMd1BBtslI<r&Yo4@U;10LKeGo!QWLraSwQy4ux|Me7L7A1s~yQi@-;ET0i*R
zo>m1P=V|l7l!9{)>_;d0Y>;ZFo@D|9$$c@_@|g|oQ7enZK=J@9?UOApi-GtAEA10C
z7I6TG+pu=4tY6qn?FateSJqPS<DRw%{Di0Vga7DhRq&IZHXr;KPn!!SN5zkE(s-vO
ztRG0h$|x-bQ?No?1g2nx)(@s&g;oVqutJ*;reK9O7tFvE{?TA)<kK%86)Un!!Bni!
z7J;c)q4k5QSfN$HRIJeEgQ-}d%>`2pg?}`tgUqLMlrJlae5MCvK@s&Uz>J7nTn4P5
zSy4n7$e1``Fk>RLWnjicXiLG2iO?2-855!PgBcT{Rly92(B^}`Ym;c;|FQQT;C5Bj
z-uF%+1PDk6AwUR05~PFx5~KyCBe3gXA2=n2UL>>-DLFx@O1*&K1>NxqO0j@~f*?&n
zx-@AQAqXg9i%63q-<WHS-<a#nIrqr{a_{$j@AGo+<ND9B{-e$@=PG-j9UxO8wAo;$
zL}W9;Oo_-20y8Bdn+j%1MAif|B_b<>nGuog0`{{LeO!2vu8<$-vG6mZh;G$W^4bLW
zn2NHW5q(+L&Ifudm>8~gS>EbX2Ck=~%*6XTSVec3f{Ae|nq-?#8MwQOl0Rn3HuH+^
zJnoeNc!<x4Ry;>Vjs||tXmvJ)hUVyK;3I7BDASwI6lmZ{R)acA_#_oMYv7BGR;HES
zDA2$iR*vX4;ttK23OMWF^(>`*c2r0O91YB3l+Qso)l{f}qk&Jb%=bB8Ar){mFe^vC
zX~BY_0*(gezDZu>0I7hZfmsj|TiD-@7G8mt)->5yijND#PgKQoguvP<o*RshoU>rN
zln>6Od~mLv52i>SGk~O)U4-RNl5k1=g$g87s9^*`ZRhxtTFBrwU*2ADW({ku=|K7|
zRw795v||Rdd!n{_%;3yiTMW~IOk1oJ=C`p%P0tr6n(`1z@!VkS<eUY!rR;F7ogHSk
zFwuk<&K}_<W{>Q!7SuytH*f~ysRyty{@6;G1wG017Ar>+Vzqc~Fm|Y1$`0q++2M9r
z?qPw1$UQ7(*_VV0Bx0!Tyo}V{Q0f$Xip-f&wsp<`5>Kqe86wT5C@>sG9a{;LNLdLJ
zT3N%y^FULT;_7rnA9BMbvjL`~T}TtsHJ1H|GaLFE1MwGBN0c~2rcrf-6NWNR_nif{
z|1^L@f0;l04=L4v%c#iR**02?_WYF*f%~W^Co-CTml1(qR8dZ3bhan93;3TFa2>eD
zotW<d5~4F-w}7WvW_93?P1FM<>Rgx6$(C##c%zAWfFzRZGV*CfcRb9)Y|CL9kQ~@D
z89!hlPXiJpRL0)%w08A;)#_ORGV5|5eh_$=Iu+Se@M2!p1TXGoW$+SSwhMS^FPj1;
z!R+yjK~stHnLLnYbMb-q(z$T)fh%6t1lPQ*3~qSYF5pftn*ye)S$qs;DpBGC(&H{Z
z@C@y27aw?kFKdDi^s+MeU@zMRe7KiQ0n=_-d<=XGL|-zGd#_6G4e`@68_bNj-mco3
zq6OvU6p%TQTxWxs6OqjXGbgT2VCKZt3Cx^`tO;gLL{<hfB_i7e{NFY`RUlI$wAo;$
zL}W9;Oo_-20y8Bdn+j%1MAif|B_b<>nGuog0`{{Ly*p!e*wW-W17s$+mNUNC*2;7s
zb%4s4Jxr_ui3uv>qqNn83M6c(?KT%{r&6au-&r$Loz<D>tj<Jd%}jLGL5a?qn&_-%
zqO;10&e|o>SyQMW?IiDYfOM10@Juk>B(j6Rbd$)Yg6SraHNkX~$jV^4No2c#nJJM?
z0W(DsUN}2U#kcgD<DNaiuiJIVmh)>iV-s88De<xzVms2r7PxM$6SX%N?|1b2<AMM$
zXF-&&Q20|l7GAF)deF0he^OEQdWB2<L4`cPr`w7tzpro^O|9Sw(HfR@8MwKMGEc0e
zGgRnu0)NlzF0EBrQ;!8th&Hssm4UmeDEZ3hajha%b)LanSbZ7|g>-<Ufmw|5!=bxs
zDX5&IfiJL%HLg;~>U1<PD@<N>uwbZ;qk*|^l3%n3vX~qV%z_YWVShVX*x=zlS-kZp
zexfR#BLvn?@!Vj1<eUZDrF?KM<%4tWeApC~+TI-ElKRW@G>}ZQ&05~Egb@g}o#P4G
zFY?S${7nh>prU6swC0))q~BsCg5*v+W-z<QYqQ4;&djkDG9AdY#Y$m*+Yz;PaB-q3
z51|y#4aQE+S#Vp*4(Hn0NlY}$GMqiaOUxeGVJ)b~ZKI<#pL%f;#vfY=v*0#_=`B`{
zCd6v-++gfbxs)BwwX?&$klerk36UEZ%(5>PZi|Q*YCA7$YA;Ehg7+Jo8LJHWUO$j{
zVkOQHY4lJp(-=k_TOl8kNLdLJ+Cam^^FULT;_7rnFHYc+*#OhgE~M?!HJ1H|GoSZ0
z2I4QMjwo@4Orz=uCk$nt?mL^@%}a*{aQv@xk$XUDv=|+vQn|zdZ&s1~UjHS143@ml
z3<3{VQErgYL!JoS{=uSWMyow+7J+L#QY13kNTm`YaLdPvL`I!Yn8kK=X<1$BKq`^H
z6k)W%z1XS)uToJ?WJEROYbQYR&WVgRzTYeYmw&*gQExHoRH;}5QnTD5qwi>4g$O+A
zp`t}bKUb*`f#3d3k;v$9o0B?_S;$>8I^X7`4&46HqD4kuRjIfJGF`bvM&H#b6(Vrz
z<3)>%u2QKGfnC2X5*dA0rOu*4IwW`9u1Qw6HBkZ57EZBZi-#J?+Z7-M%887qkr06t
zC?_(aMnVKqpq$8v8VM0dfpQ`vY9vG;1<HwxsFAanC|9NJUc*9?cPv24CZ9M0ixsR|
z1z9qp3uGe^T_DRqSU}c*uz<9-uz>Wnuz<9Hu-eT(S;)-`km$M3Va(njR3IAzRL0~d
zR3KXeRL1NNLItupKxNFr7b=id50x=3DpVl-36(KTDpVjH3Y9V4CsZJ<2bD4XCsZKK
z2bHm(rD^SE>3zG!ZUWb{h1&~cqFk>xt?dpnViS0?iF$#rnWzbza!<a~$S3Z=6HU|v
zGUdVo-eK31Ch#2-^#W-O`L+-6M|Roh1zxM7JSRra+hwB{IKOaxX|@<`r(3^72b@y4
z8Z=vsSTaHcK54p5AUz{2AeU)L1L!BTci|J-$%Ha{aw~F69|LC0ecwrEh#AXx445<F
z0hzOm$KXpgft^5RE#on`)#j@c$h>7d2F#rJ1ya6@$KY|RWhamVW;_NH>{{Ciq?#G8
zT|K$+lRH@;CCrwae65i&brdR)Dney^j#X<qkODzvO!i_GNbXP>a|b9?An8M8Oh*Y7
zNI9UktH^l0`*Z&y4VWEXzSj(-Dj7{XjNM&mK+<$HFnJ0M$foXSV0Lw(0ol+U4a~((
zXh1G@js|Aq7aEW~-_gKa6om$){Ei0Z+8{I_rFS$i*9D;gxg=z<>3tt}F+u}!3*%^D
z?l^=7WKtb1{O?ws$Omjf<=vj*)os`A+)Y<(Q;+U#9GDrCTLWf_+YMQ`KqgFBd+M&+
zrAKEB%!CPTj_$c<>Cw@^Oqe*hL+{;xqen*rGht%wIlXiLiyj>f%!G-xus-ac%NFVb
zrt#gk4x^IU<?s}(qH_qo(kfeKLmjERfun&Z*$z==hq$Of17GK9%#)v!&ii%B9xY5A
z_)jX64C1M4##7hH)cI-ZWSabxG?@}#{!Ys8OWk}yckNg7=rRW{VUJJcGZg-p9+xf6
zl)nf(y6{)6oViXJ%V&)9uPy!?51e~jIh*GH4#d0yNB*gXEBg<N7EUOQZK3?08rl3K
z7+GgX3-u-Ra`cg6`MxUOt;e`5ocXOXQf1-XXx(H!QYa{oQ6nF*EU0hzabmebm2cBy
zTo%qeq>NNqIJcrcN`IuXpuDOY`G{pfecg`}%iXFxUysX{jtLeKXI@ptav9_NDI><(
z@Az<PGktHcnR~4@V&G*XXA36WB0_tzTKUXJu_CJo{T)A6H213Zi+YR;!<oM-BT*Pm
zP1L9Tk5(AdRx2O1Fq^6V_8%*nyHxuxdR#*KLI1rRM4Rbb!jH5%P@b$tKH};?eaDXz
zOL)<LNgs&0_`53;m<Is)NwID9D$b3*qk*}}mrq%rRvK4&M+5WZB-ZxRt2kGBM+4tw
zSNMIovU6E?G%(Li@-Kt|hw4jYM;oK~GX)uVs=aKk&Q<vJ0u6kJ(dxJB-P;p|yEyP-
z_5@vDpm0Rt_6)q4y>_jR(c7}m6mHMJ`>RY|KbG}g;ef(D1NcIf$@{3=6mpB<vIp~2
zBt_z;;J11`cQi29dU=V*jrzKJA<bxt-zdnyTn|N+3&~&gV&hc7TpxtS&bqknF^&di
zzZ4q#%+k6h9SzLxAvD(K(z+%xnqron(88{7>OCwEZ%z&D9|gK!(q81@&C$R#r+j|T
z{iMI|?|wy_hX*yM3BF3ZNHlp3<L@!Mfjql8P4Ls!Yu&t;@wcMg`)Z5vIOjCM{`RyR
z$m5)&fw#22>W<&%bdS^C;R(-af@vIi7TZPpgeN>l1M{>eG$2oSjt1rlPiWnGk>)+2
zqk(xxC~q__>0`iKLPrGihA<;C;F&SAsBOmcqc{$GhKc7{P=B_scw24d=AkUbq1HT+
zr8p$WVp%bD<6zmbR?;leWU1&svDPo{Fjv+BkH@JPKc^2=JLxgM{XC}du)-N%G50vr
z@5#d!g*_DKKdRU-I@+Hn;#4#@Y4iU=k8w1d@pBVL^Mr1{&*(9?a#SHF&iL7jt*|E8
zbJt$zvy-h49#RIO!dhY99fZpDx|V3G<x5Ap2`N7NwSK1iGEJ4@Tp!#pDW~{I8GZ1>
z<)5o{@gw-~>AQH24UTJB@f@2Duk?!N*g{y*#dBf16C3v+@|F<DHqTo^#-t(CFnXv&
z&U*mljLAq$go%hKj34bV_Gk14j>}dtPu@wAU-29X5Pk6+7cw#^o+D~PE}ml^h`o5O
zUEKaA(_T1}3MrEYH=I%(Aore78I!S4NtCO!h=Amdc9=KvFlXdpzR1H|L;Yf1LL0?%
zgRzry7HpTY!?|{L*r*2ha1XOwzz6Z+9<wjhg*le*uLNJhBP7}@`Wlyfe<h45wnDVr
zW2CHbj&NfuVf1?jZ$67t6}D9IT-a8{bA$1ba~5ou^1->356-po!Bt)+3fOPy$rmd_
zmsy949CzH)iU}Y$uuvJZnIzhR1<tA+s6Z*MCVE?#C|q`n54Ul&v$mN&<OW~9x@~le
zGn4I|_Y5E*#7dlD>x#ayC5AaifnqCRu%X64B~5X)q}wgbu9UPL<^FV?WC3&LH@;55
zHW`Xi=w>&4IQxYjiy4NO*y6dt*vUByZcEwWTsu45IY?!Igvj+FG@B?Q*>z}k@r-k3
zA7#i_FoF1ul{iC0sb&kKi>-vIvxV~5B1;^|QyIGfW1h@}3go#ADr4^85*tV8SV9f1
zCt)0^D?u1jY$XgaWhD$Zwi1@@8J#)qDT`gotqB`#@f<rT6IDDn7#}%j!FDMhoJ;xO
zTst2;A<IMoNsT!TT_VRF?<>RvkoOf(8IygYEhrjR2P#mCtBFR}6S!RJ#5Its98oeR
zgaM*_M&A_$4I|q=XP&cHk<)?rjg>e<1W~VwVa^d<Y$cpym{fhBlBT%rN(z_R{&*9Q
zwLR=DK)?M!GUrT18M5^P2_Y^9XK1}tNjXP>Vk==uVe)h^P)So<EopR#4!Mb|3zX~h
zeV+td{DQw+Z8>v;y%C%NB;Hsl%#a&(w&{T~OmW*a4P0*XrD=fN)w2#66GCDgV(qi~
z-ZGv!&TOj;sVES?u@Yy9Ak{RSBf8j1SklkyD;%y_#gc|spyIi~*rD7hJDh80hwH4=
z7f6UqMQAoL6=b)UW*OImGc{$%Q~>cCD{+R1Qd1E|7h4HapQ)cS)Sl$)ops=R6}c~4
z)?19WeaeWydsLJY8I5|{h`>u!loJ_!^%)}qulw&@loJ;obCsAMvo!Vr61cNfO{7r`
z(~vJ8#W_^N9E24oPz@6h*21HoM)ZVAvoR(@X*M7cLS_69E9NvH#e~`}pu4mc<h!dt
zGL(-T=Jhdn)h4zHTwlFrJO*V8r3oaIjMpv>sv+O*0#XP!hn<Nz><r726DbOFCl-Mu
zlM@*+eL@5ha!za)^IxrY(}CE`$}ZnHWlSw((F5@TwVfBH#eFdl{JeF1FOX_E8u-_?
zY<q!J$I-wS+uZg7iQCb@1T5ct4r86A@tmSZ_rVmHB`M|Y(RdEiqoaWdMra#pJX`D0
z(ZB?gm@E;6aZ#_|=uA0F;1_K6nuPB|Z4#;vt4UZs1X~Jk6(R+HGF}OLtH^}`{*tGK
z-Iw?g%xg4g5_6Wo^q~Ay&;uF|OV!c9tW==^X-P)|6O7R2X^1~9gb#kn!mp0h{d-M4
zI&0u5DwB`>rz)IPpn)&<v~Vhy($->kaMr<o-n##-H1-BZ1OL*NLl4u+&fsX^^R;;L
zDK|yf!{X`&`qdD9-g>acaFib7PxLs$8qu$}-B|c$W2>Uk?xV-p3TIeCDJyGhGVALx
zw!#@!SjtK`85G{fAOC}LRNY56C;G^al6&8=ozV9ix&&B;#d}5Oi^(aT8;qTtvtYZF
z9nQ70Lrvs*4P+nYBFvc54C2yGmO>>cF7YA)k}r-H7GrRWC{Imc$kC0aaKi9oD<sD9
zN?G9?nZ;JZL}Nyncgjy#1ER-ou{T1G#dG0;FP<BWkDRmMwv-RfrF?L%oeygLv70D#
z-mZx#-~!v$!dDcWd0iQDw*w^FU@qmo250DtXfB1rIp!|55>_{6glVSygtf*W)(4vV
z6t~?R!sTLHxGlt0yW2ukFSiZ7-wO~a7H6)s>(~q+{-ip^w<e;*8Qf>p2{>VOe#o^&
z?*Rs?PKvA5iCzPRT&Yew*SDstu-)huXYTY>0Isj`22~KH(9Oeo-G9oi{|(^lDsqnq
zjYx5BC4ZLv4?W5s^8+tXQChJyWM~$|+Ikd4;8+!<6_*JVhn0>U*$P#qsfIMx85xB;
z=Gi>l@p=@mz^zPF18%LNbRcCtdz2|QfL}II4fvIQwitca6T>M9Z4E0OJ*E{}%kvx)
z<$1f>y~pe~fDf9e27JUs4dA0HN~gX+&%W&yfjj+<_Z>Ch&MHdV+EUMMqsPTdN0(+E
zG-7NE0cJ+H6Y`aP#+Y5)&9yrbR&c6XoU2EfY~Yn9ssXQ3Q643uM?5h!e~F&LfK1TD
z76bJ235tI#YJn=hZT1_$_f1qY_#8!T3{f~#kLjY0(X->dB5;<8YQW<Lv~{qaJzS4A
zK?pFz1Tn^J+X*^PEncffnIPa(CaM9SR#BcYqan{znp!wP_oxB}vI$~<e!B@8qjtxe
z{RVJD6V-s5n5Y4qq@wf$mG$fiUJ<zJd@||*Zu>$m8rarJW~&Zd&TMr9i?-^~njx;o
z%vJ^XU9}~zx32GFP_!2RlmyleGHX@f=9-fHtpTuTt2&6tnnd<#%d!htwACDhtxjyA
zC|O?F7j_i+oUHk{r)ltiS&sD^G@hj`#~N@s6-h4+iNeCvR|@9A=b8C3@In>IZ}Sal
z-J(A4*P{y!jC1)TgD@i~&lJ=`PH0;GJ`*+4aeKm~aq^F9B3NDXloRrAJIaaB=nESB
zKMJ{m|K)Q9zF*9B3k~)pJ<9td;K?c)R+=5xcZi<+v>siJz^9q5F5p+q)`_vLx}Ka`
zumvWV#BY%hIxUiz5-RheuYy5~==>Y|!ffOZQb?xI#5YDwOyo>a;;yr-vv;I5a09sP
z@7bMdz~xnx-knZY8<*<Q6&-w?*{}Rc;S+jXLcaK{f8mAbZ=MY-sq0~-Bm1J|g!S80
z&v^zfWobk|jy+LN?PZh(@MRO#fPYd^I{E$e?1_4GA%M>}V-?^x3pDUcMvE5j<$CHC
zJr-()=poat0H0P-vSy_h^XVO}N4X>ecQa89xVwtd$xqU=yO?71&j^Q<<x5A8ym0u@
zqb?l2=BN+kztuJ!7foxZGqiN{s^RW3Rb8HFt{cE#nWzT5TSa*qjHbP0oerF#qIAAE
zy++Ns$p&Lo{?ZBf88zl;U>qd=AnGEuin=WKFl#6`Sc+vJT0=`mj%ud{N9KItl;{t(
zLg&(6V_Knmxpai4KcQt<#mZd<PB&2pkOD~_(WHl#2GdNTPNJ%lGzvnHI~L$T(V*6_
zB+9^oS~M9OD;;&I7!b8YbI?C{$coRv>qW(%tKKfNAREBHnWzR_prX9}8GY?#TPMJ?
zRg_*QIDJ9Qxdi~msN@KoqsAN!jDtieeyCO-wD_w)ET)<VwL49%3@oTc(}fzGME}H;
zXzhd)nZEJSzIVe%OAIx#k%j^ibZS#T4NfAlm=YznvCiEV)+1%$StjZLGOMZi2`vp~
zafdpI9Cp_z2tgJ%aG+>Vn_)?mfd#dwTTna1)T+RO+Q@K=oTv03m|6q)V-=-W8j4q#
zVjXy$i7LRNt@z3CVO4n16dS--RFn<^#ivcN4*Z>o%D|$n_Cfs76f3}@V*Dgp`a`}+
zEu%-d#sODWQ961QmovpWa3vE}fJIwz^w=7%ik~bUD|b5iUGAiD$F5rVbqgp2xm@We
zdBEyN;S|1C%qZ@?XVFmDOB2=iwibK?xRZ*~g+Ot8Q>+7bG*Jauv=tWuTbrq(6hdzM
ztPsl<3IT;6wGc%Lr||6yQDlXMn5w={u;3fOFRLhB2ow)B#X9h46%8vL)7N59v=)~F
zYbTqv3b3dccl1nEz|9KTPo!jj2s^VhS|6P_d6JSx3CW*Dio-GTCbgbxt}|rPIM+&9
z1-(XC#CdAB^N-vEG=N8_C|yJpAF{OTz|X0od?jZJct4XJRRR|s#C631>>;ITKosQ_
zPyBR@-Yphb1&F@r&Fy1=F_&gMNmyIT>Qe<472~&EDB?yQ(t#*S7}rJ*_vqbk;WvTk
zi{2&4@Z<MnSX<Y^?*uY4@{R*YV6GIM(e2C28WCY6zC1Wr-E97fbrW!R6{Wig#UEOn
zb>LW4bln7=VzQ%3;G%=Ln{WVIx(SG4vYXJm%mS+b(a*Yx0mjnZgtb4LwJNZv7<Utj
zxRGuGqL}O^^sX_7O(6PNH!;AN3qS5AtSx5K+X-Z5q?>?6TX8qtr4ivd(M?m;&Ee**
z0X#}Y>25;tU{kCEk2FygShN*4&UvcPGQ|e)CKJ_w_o*lyS8S_}t$WQ@7qDn6j_WE_
z_=zc&fxk9UN4VV+=bc6=1MfCb4M+ruhyMA>{<~>4fJ^*|#a{!irlNFNVq0}=ebQ`o
z0gJZcviwWq8D@%Q;3_7H%0isW8>I|f*+exU5hThoQQ1eDRs;CcS1C(3@Maa|WnuK2
zKN}JFn28#|1t#hSF7=mOF<oUsd(c9w199UN>n%ojl7}u}(L-E&JiwM`dtgy9zR03@
zh=o=K5{&Fp=k+nbs%$nu9Avz(>#<M1@^J$YlP>jUBK2nIASbr-aE4{j3B;2;umEu?
z!Ol%2)fxKCc%jRK)YWV~y0<IfxfVtlc#4X$&zicm@s8J{(*<8`ecF6R;Rgj8xNiNo
zPp`t+ddwnGOsa{mJmc^y-XYM(P{!X9-XGA1P{;L|#_9z62%_JWFRABIh5W#jNZu#J
z3p3h4!pU|eV77Nv>oSXH^fA~=g(IEt!r}4*grS#=9!@$D`{-(+o_q~D)Y?#;;at`Y
zTEEv*A_D$VMXQx&=?6HI#+~SYo<|;HOJm{eqxBx$j(h&wb4HAXMWYD2k7vbxmNPai
z=2Suh-Zv<Bu$cLQ-G#+^TNlc3t4tY4)n#gK?4yEs$%LV?Wpzvbksf6e0RFz;Zp~<w
zzq0u^fUBt}-_{xZ(i0c%w4QQWZ^cgQDW~;T>@@o5f=nk^rWN2dg-k0gMua3+2_SA<
zrWJF#j%8W_l6+1K^RKGE<9#T=i%e9ms@dbcdEiARir%+>N)3ELk8Yt8_HL@?WMNE{
z2R-)<L^#N(jQq*~@OSE_NN3g-vnXeT5fte}Ma|^N2ge+-#N<l=oZ>iNosIvS^(Sy&
z73KYj0>@afo>GkOCSF&MtC`1WGB|}nSzW+^gNhXg8PrS-=0*#u0sQlTWr!8)DaE)9
zr>NPh&0|ysPGL~W030}|SaFa+mHwW0!7h3fkHFJZly?K8b374vQ$Hf@Vn;m-Te=pA
zqI~gdn0&80T59X4>KF9rmLqt&%H-A^ILky;;0Y#*YQ2P>BC&zKNgQ$$D-Sc|CWQo&
zW_nr0ii57t!<xdX3v4L^J5-eWWYpt{z%%<1ZPFCr0k$L#M6uoE{ZoS*tq$DefhSoU
zW#CRGsseX6Q8al^t8o$=X!1gCV)8IUZZdg5(o9cYtT^cU%+u_CWmz_Wcd01tGghpp
z6yptOihf=DI6X?1z%xu#1D>g(JP1bfJTdH*s-D88d>edBivd=%uTC?-X#T~|F)hY8
z$;hFb+3E%(LK@#<fQ8&*{K4(*s{EFv(Eu(rH2)U7+;sy#VWI|bSrz3~VYHqRqdWFT
z1CLo%LZ4Zpq1TKV`lVE%=VYngR^M38O2q)n;%0n{0cP9PZ_GdPoo2EgrG<che2q=0
z2K>8<@?tT1-xGoB{WI51H~ey{u!bJpP6FQ8j8%Z!7ii#KPXiuOpn<>QX+KqXdw~Xi
z%+tc|+*%9ss!_Vgn<V9zzeBC>tJW)~)d56Xu5-Y5JS#M`gBq&Z{FH$hl*eEo1`D=*
zkkONIXj|StOm2nNhm~ecZiV(UCeHAXF~j(G{~wH>S$k2-x1>Dj&MJ-AWwiW(1F}Ep
zxe;<E^>++&wNWq8qx3&;#2eQCz-3gFw>6_RJQ28IKVn#vPraU7o}@?D|KOcfCeL2L
z&H@d5oTmZ5T%dt3^|Xf+K2@NB|LJLAte0tFmartd$eScn{jXZ@n&A$#Rx?9ULoTML
zpeEkpPEkt5tki)gs>n@F;##R^yG*eHL~*EmQXZ!NQ>Fi0kFvSOPms6k$u-`jV;aD9
z-$GOaZmgobi5Pv}6T=uVzk!*r1JO$7jN<z~XW&NWt`|ro&Q_(x2oH%LY`{Fh$lD+w
zTFzGdky)(R3>D>-%`MRfu(4hzdh1(0-9R#MF?1&K=??3Bsn+)@J-THH{*~3f0(?+K
zi<f4*uaA_*BKU{rhA#HkQ|If^xd1P1DMVjJ9-LbA;)mdWquz?|9v)K}!4}VjQHOTq
zhC5VG+=H*E`Ha!ylJa*9N9*<+dMGRBdaFSL_{iI~f`Ct}$jxk{#b|*iwljZ=nXd!K
znN|-FgU)<CVLtk;2BJUC%vXSSn5Y9toX&jwfeW@~nqn3BxP{sUEQVT5gc{wLV4n2l
z^A_L}X1*I(G~Y~^kCsqPZA{an>u>OhX1@YFRYi-JzLYN^1Xp=(IG1De)NXooF2Hx0
zTC{`)rxq<Cf<Ij?7njM`R7SAHb79n>9a%4-dSVIPs~%{4x!<4IV!-6&xAyT>iK%8G
zWtT1n7#Ywa9_ZMN9A?JJCI7`bZ2&)Qq8e}m73E8p(Po|qoNS^7aLazW?c!L$6zjmE
zVti*%)@Qw5d**8Zzhj~r@NyO92{5|L6T`U9)KeIc$59~Kxvfy~f<RHKKe-jE=951;
zdegQ*l|Qh+8o-@~*>f>)Hxo60yQ?UVjnQF7jGl`p=qYSUo<MBM%{35%>C`_H*ve85
ztw}#mZiV*R)nkfw#U9oj4d9+8ssZ;>Q63$m`#ll(KtEzQgBR-AJ4`Y95_g}TI#Z8s
zHv?ll@zv^?@8pZ^3_VI^fq!_Hbyx%bO+|S+j8=Njh`@DKl%AEv)yxWdbg_chGp`lk
z<N^&`^E6;jfd)R>(|)M%#sUren5TuMKT8YqC!=(cH%TTQTP{<r|2C}-w3fGOMUO2n
zD;sL+u|>5jW~B~1u~74PbDE-OyG*eHL^1i;qV%8ZG5y%0C)awPMrZ)n{Wqc-aAOta
zy}{`7o)}KS?P`7_GhYXy<!r?{qxf$%AdkYp_03i{5S#L+Y9ql#`|&Ln_F>C&3veS#
zrx!@3F4k&`5gt;HMN!^O%%hclx{35U(Hn1JbpZ*)g(XinO29#)w=dPwU8P62dck*D
zSu4PYRAf&#2>$80;fx=sr!LT=a{(S@DMY_(IXJcG){NjER&T}Iug6tJu*Gv>)S(@@
z4-3^3w`MD9K4bJa;M0wAerh#n03Z3l78>wr73B+!(E?8l6DX_MTg`kO_)8OY12L3d
zd)V6F6f3}|EP@^&!8u!%79%_){uMFI&oT2=;Fe~-1BfAKK7NaUEpnAd8Q|k)s|!eQ
z&XzpNC;<<N^-@#HG(Eaz2A^d1E5OrKWREfkuJPP(;>PN!UG?Z(fPZCb(Rvx2TC`pW
z{tUHTToY%hj9`oB!l*+#vN%HZ#Co|`J<#R(gNF*4j<%07+Age`>~h9nP!{pPgr^^6
ztTNmoH<m2r@Y8yfyISA|DsmmwXffK%6M>UW)BtYTPdBVEL9Sqmbzo63zH&}cg*{BM
z0o>C>HQ-(<$`fF8zb678=tm5r?bEYo>d|#E7|p~j?1f6dLyvNm4=takj{&dQQ%3_h
zb2wAf0X)h?4d7Q)lxN83q{U1Zc#4S{z}-d|3s^T%1NfwgqT52E|Gq_E2d-nGb^*!4
zg<5YhA`#i2fMk&q8C_=ybODJvCo($O0`3BmLrx4Q;n`pk-1M%b@vo~#Cj%3@tQ3O#
zv87uB-l!s3heKKn9xz_dqY9ZIu|{6YSO&Fq6q1~<=Bemso)u;kN7@nF?uc!7#I~bJ
zk8NAr584#ffKRAMs?gVB@IR2>Kr#Hj7(<-&U<%4S`}ym5N#}1LOS<E5g?>t->S5>>
zYlIpQ+rvt8`dYE|T5LUfL-9AYI@V%}DCa5#Z}QnH><^boY^XOj)Efug8#e1VG@|cV
zNd}_@d*D1mUuaQtSx!I<W#x}Ei3&Lg3X_d#sdc>S+qQ_;>P71*EzyWlpM20Tw#C>t
zTvXlXRfUF<$2sPixk)_X&DXtU;^JH)8o<g(L^a?(CTak`s3N!HHd>6nwWR3+f1;wC
z$Y|73Mg%UWqMXR+)1C;N*pJ9)XHNv~(~rn#swV;u>_-fHidt=EwW<RtkW4&~0?GXX
zkm}@DCq@S>K?yp5*QqEcGTPCK+5vo~P}F*h(Mv|G11X~f2rMQW-+%3@N$#mfcU=S@
zYz3$Q4_A@=B*l={kvh1*^MDr@d6(+oM|yNV!1GPF3;0LVl^@z-@Rsp<-&e>C$eji?
z9&R=60WPhgY#U(kdC%)m$eg&~z|4qL6Zl8331pHa4j_v`6j=dGnuy{Sj`oagG{@~N
z?J|(G<&Dfeb4VQPnJ}FBOc-|g0cx2^l%4ZT6)`s=x=}^G{I$3^H7c>WHDoQWL@lmF
zEv`hgEfXi5CpX>1Ns}e+!)f6{+;Q7j+ypBLhh@T0!95ilz&#Z#xL-jxP^M3OCCV1M
z$Izdx54wQZ9#-n>YsJ>PV(ZaIt7}9tMU+GX#D=4)p~aGxzsE)Ebz4CDjMaAWZ4<pX
zxKs0f!15oA7HmpXs5L$lQ;ik{mh}w8P*!U)@us4x4T8eVVp;<FXdQ|nYA&?~62AK?
znfz%~i!nY&ioCBhc9YR9To2QZ7*RSt(_p-|%EdU4MwZ>>**PT4RN|4bUt!S{FR7+!
zVR11Ct&>&jn-*B@X@x6Uht+<sknj?-9viA+=)cW&H8iw}RBldbtowO7{c#QXXW1W?
zzfKDmFy_xQTRlKR&#p}P<gz7iB7x+S@xnk7Z!8HL{dT6>g-Tpfte?asjW2}J>L=$*
zR9k)tc-axse~fW3QstYC#?QT0r84jV6)h&e04X*Yk|eeX>lT~*=nMOVMg65NKqAUY
zOfU<pA1-}!MPIY|5R?G|Ph4?XPyR;Zqh4OMUPLOlC)n<`t9b|T02Rp>XZu<Vd<-A)
zW+%>XpcsxS#t`3A4Cd}?rr!Ve*`q-NIL|~i;I~yINjF;zE>~gx^up-ZUOQY}59_IS
zjM4zEJ1T!N$Zav&K&A3QIdJcOL`Fw=BCxL?k<q!H2)wc%k<p!=2wZXLVvGw<uAWM+
z-ini}r;@9;NbXPzr4A%Xx$OgzVg4Y+=xbgVNZz6gB<Wn2(XCz=NGU`YSgcC?Y!y#=
zB~_J5qAHbCRVr4MgDkoVkfdE2l@_BjJP}CV@)<JlN@eA`jDG5MfyI2|PpN+wwRoF+
zncRcC2Oxp}e=3bO%}g3Jrn~|G9^o?s7Bj2c1ksqH3uIRQS9&y_vo_lj$&!w2OTc1g
z4a>}@(E!p}ZgLwfMl2?=2rTwl{I86R(Y<K09z_wjf4^;t(NUfVyxBw@z+3w1GJ4z-
z!(A_qqfQ(%OsfVY7?*s##pvgr2*h(vWaQ(B?ziTvjd#pe12|!s;<n9b8<k4ofTx?N
z1NhZ`x{NOO#IPQ59ChMY*R*Otg2~flbebmu@thMG`8Wnqj|y?%#@VX07;R}zE5I{N
z)CI(IuFJ^B5j_Q7t2SOYTMgh^pD4}?qj4&gc>(U#kH~0$PYfp^c3Q_N+Ib-vd0Wxv
z?=#iS_q{vdJtnFFcU-oZQyAAgRai?|@^l?iz7tWR_hPpNlqE~UCXN}N8H&D88ghq~
zM$T(XB8lPhGkSAcjC`<BlTFn;POy9$z^|LA2E5cn4d7)ea<kEBF}mM~k;jMi)Kf-j
z0Dotq8t?@ZHGscYQSLFU{sKKaY`J_Ir3rx(Ow<9~&_oU3Mk>n9Guq4(fz$dC7am8Q
zIL4c;8W87l{RHAUZ*xYyUKd!5Bffi^sS2l?Vgq=SiE6-GRW!VG(zSgpMz@*N%`v0D
zc=a&(*l8W7KR2xoAO`bH8QtlLz()&l)LV?6@<iYt`w<zv>WM&d$X$nNEKr5g^43eh
zHB3|kuBD>9aoX8>+k&VA*ECxlz@n{qPRFRiWK(Pa4>eH@I8#OWoH9Dnq;5_beaowd
z3B*q8I6cg?I)E6=3(M#jPXwM)h@;+O^i@v;exo0e(Pf?pB!~2z&QpaOOtArc+(b3t
z6DEq@!^DQ_82YVgbpSDx4r;zCylaXL;D4+@@6~``S5e+*j7F_!MBuV2%887Q@x*YB
z<2dTX@imL13y9}DCq|zz^IbqZ=R`(6j_48^#^FBAB9N6VkPhG)CaM6}QIXRGZ)vm+
z;Pxh}0Cz5Ef~Og+19-5BD!?NOn&2-RtpiB;<lE{%YUiQ_|J`fG6^rgDs16q8t!Heg
z78{B#&lviocMqhF@~LTG>n=TC2s7*^_=%X%mM{kq6Na|&F#(yOEGA4%uomptRbjUm
zg<)c1;4ra?L5UojzLBG9VtaodIgk#?B9E<i#MYxp$Hc&qhxwv)nvV&HA^DksK5Ilj
zS5W|SwW_CCbsNCbRg~{NVGQT#*>8A7;C~LNI8V=B<rRTf52!e1<$QY_r$_g2qHtY3
zrbC^mXD53_VBJJDU}HdAQ}ygYrq}@9W1<@H-T`ertY;rL#Rl-V11iqfv*)bJ1)~AH
zNkw@j7=3m%BZl*ct$(Ubw}F8#UIn%CWeTtLBH#@w8d}<<1zyG?E3cq%tR9^M@Hmrg
z(gJVcRe@U<XyCf1bt>#B(7>}j4S0NkMu8smwBIUxra%L~>S@5&3pDUzpR~Fyt#Elg
zx*~zcdKz$p0u8*4r|qb4w*n2^^fch~0u4OV(}2epXyCIw?LQP=T%dt};AuZr__G2H
z{Gg{js_@AI4g6P6drje61sZs1>*n%G3RlykD=&CMPupA}>%h^%g)l`qdze}Sc+wc=
zvjaF+Mfo~obgCzY=Fd{ICm3aKAlmZCf8I$gM&8!mp{<7&p{?Po=eZHgkkUtw>$8i{
z*33m{>${84*0YPy*5acVUF|nrgtiV@gtorD2yH#G2yG2nW6{-qlSOE2+9I@d{vxz>
z$0D@#{vx!s;hKwX9=jKzt+N-Qt@{_Dtv45;tqE%_y4p7vp{=hiLR&vygtq>=2yKng
z`-nwx-JZX8K94WyQJ$=TFRLj1WECscQ;PBX&jo7seeV%CbZj1sJe~uGnJ9XP7aOW$
z2rU<S9Ko_`b49b=0DewI>6~|8Cy%eGM|lYZ>{QVbrD0Rv92efTW#g$zxyZB|!1q;@
zb~`6fbdOPJ-fG4=fcKcF6ZnXVvhPeWc+z;$s{yP-k&n2he5%byTp>645m!h`eDYQr
zmlyXaJxV76|81gf;IMUbc3v??^Hn;e)D3*sL=E7PpQ6p8Ut%JNBg|tR_zx4+fVh=D
z03K$xYrvT%Dg%84(Mu;HsG7S9uw<$90CDTeP-!tDclmZ9@b{)$0s0`K?+g;eAr?dx
zh(YJE+G0d$<qJJP%;&^tMX2Rj=AjA1rc-PtJTy}t;(yzT955^YU<a`1p)=v3Gvy)v
z7oWD#*e}(iTbJOUSYG=wD<gCY9E}KY<YZxqKBp$S%~TmRrcfFW_@{z(FxG_@8c(S$
zj3X<1OcZ5k$UVu<iCFt;bcgFvHk94lZ73l(xuHPnRfDoy0P&Id6SP^%zQKpl*)EKb
zn+zi)C44TX%YO$wO6uiyK0|KOXGlu;oU6VbvaP29d|X9tb;Vz6Lh&k7tOJXR@ii62
z%T2KYEGoud*hBHZOtA_qD#kx#f#QXx*aQ|8<Bb8ud8XJ2EGow9eZDHJydKw&25_{B
z@~Sf0$rFJ)_alZY4iAf)VjYMlS<67&IH&a%qiww|u;?_NI-FuwzVipfrt?rS4_lgt
z3b5!Qp5xd<HRYjd9=0$KRbbIW+zhdYX39g;JgjFPn!uumxNBk$ohc8U=3#U5&<QMh
zXy0BNtD^k-kYRt6%6jfJGu{B6rJ}qA8U54~fj{d<3|-*iI8&?x@gyw>#7*9UjDF~K
zfkmfr3*r>B(t<#2<}JwRD)UeQ7Cpo*7<;IuJXBkF_?~&F0*fBv7K}YKQy!Y;;bQa9
z1QtESEf{;~OnK-u4_BCnPGHeP+=6&$sbZ=HZ`X6r7-wP&{F*tezN7HH0xj&dxt089
zoc=?PauEk!prX8+L&b^X^BuTPkD>_tiitXbofC@B3hiulnPMHdhAPS*qyWbiY}H$g
z@F2u?=4)oY0$j_?cL6b&cRM3*KKil+mDttHSAlDr`EDQvo%w1a`sjHG^IMzwCU6}y
z-vca~Zzjw~&lxAG+0kaJ2K2VhpEoCAD|&F6sy3Ef->%NUO;sd+CV2Zv40=_VcNn9?
zy*9A()49P+o57b<m}@gyX;Oj8;4T&BR7Stwv_NIhw^=UCsf^B4X%+y3%{MPn!>)^q
zQKuMW>U`8&jQT9^I`DXPEBBW`BFc3c5lZ4lG>)T^ildT<qe2{%T;3S~ske)x(qeS{
zCV5GO2&CFMkrC-Qq9#c$IgydiELwPRX4O<?)kJ1hGF#Di*4#M^?%2RjY_-K``3-Za
z^uXLX3<%E!9bb^*2%D)0n~4aUL|F6?Uyx!Cop{*6`l$x&EiAZBbIJtBJ3k=RcWHID
z7!isP!*2hlTBbeZ5$o!0^mP@@K_QFhs3VSx=R&u|a|`pIJ+_68Ghyg46NavmPkrS0
zwpGzRXERz|J4tTMfNPnk0i-_idW`69GFCT`O7&|sN|XuXfTbzJqTH*NF((E5F_RLO
zp?EGdUOcxjA0M;EVaOk|0=wx-E>z&>RA4t<&(u}2v~!6H*txI*p^vNrq4B|YS!_;b
zHZ~{1FvzS{VRS!;v-~9$2n|RD?A*fa3_hVSg2DIisT<`x?&yj6QQ~`m*tw8Ar5Ewy
zxiI$7P8NG;AQLWJQ|caTCf44O8%MD}K0xetE{q_wGx*qJ&B4b09j*6oecfLncIsVN
zHFhqHJ+za>9vVp2J=T=iW6ea}x6(V1WA!L~@j9_5`rXcjv4?iDFhc{$*kerzGuBMR
zewW5fgGubAZQFe26LOQG#H57R)pZ3?Uh(=I6$`5pa+6+TQo`#Ly?H)WkMjHoym=eD
z(YkO}7y)`~e<ml%U;F@fHc>b5t0w9N7ObB)2<sKBA8)3*fCW=$4Z>6vQ|P5{8siL`
zp`vV|mkh#TXF~7FNN?EWyprG4qx&EPeB@SCx%aKDb6D2dNjz%&MCTH>LrVJrNm(MF
z7e#=#&wn;|A*2Pzo~DB{^(gmMK%&ZcVO*Io?3#p!SC0LLTAt@U01F;oIX2<pm17s?
zK^|<dk1-L2rbM)GL*22V?$}WD;vmdfK8lPD^~8pvR|M;6=dW)opbRWb>)TODD$1{W
zJzzm^ZqyE+-$L6!J5FZwm01_);8H!x$DF`fe!qBS)|EOacz7l9aAxdb!K?>@26v4#
zYnqxX_*f8`V?UVV69qB}GSS1-J{1=cYY8tR!qLgL%(}vq5>0c1dcMg=0Q3=b#}RbL
z5p)v)6_iJnut)!*TSaM0wudsXu!i58^{%e06>S?S16dSO<@aV$k|izaGLQf>UKmGQ
z_zH!mB1@G{DvkO;{#$L+aV><TwxWDeI_tvW+5fC*tc822;{`(!UA!UVjhT>F>OdNB
zDf!_o^NWyalIYHgqvPzx+D`pUASAA|i7>%9&^m!)DD@ipQEfKjPV{9+kvhc}T}Zp@
zs+64`i0P4T3m7qS%;@N2?fDB2BBUgPfQ07{6-1bU{Qb}P7M93i@hpdRMn)+tqj@iH
z`B#DHO33jp_66lbN-Wfnl1x28if?Bi^;nQ)ApRxmsZnyFo{+}0LQN@l*pmb7_v{u~
zQP>~YLNj5wV<yYD#{b}c9NNz6o(T24S>bM*F~eeDZk&cJ^#bl^qAGCV^+`C5+beV`
zGdH;vx*b+JYH}-dterl&6=rNF!T{_<nE5uE9xmjv4_riPa-nV6wWk53jgswimhvHI
z|Ed=QG~4FInz>S!(=yrS;5NDZp8-bBZZ9D$u*)Q6&4e^QCC!9k85gm+$2DrAldHpI
zri0*|ER4;`!qBj_f!<Td%Vi+uC8hbPyYTtPh7K}j=;3yqsq44pZlwWyP(|)ZEdDA^
ztXNMe#vi7~ij|aN{26(ySWPL$pESgZ&6HyN0Ya?UnNo~DsD4;W_g~vtRe{Uxpf~WP
z^DmsuU@aBOgFbNPjs;#gYuNv;*{=gJDYJc^*<Z)(*MSB5=gnTY{Yt|AB?<eLg#Akf
zVZWNNe`UgcHDUkCLD+95>|dL(-%QxQb`bVE6ZUUR*zZi(zcI4^(v*Did$!My?zIE>
zoX_Vn`7r;FI<1qmR}!D^ucjSBc=Fl-Sm=~_iAeWLL^>~ul&F64Iil)%P@xaBD-JYz
zw2uvSKM@(~jtxc6U$LQ{=OaTsv7zYE{zqE6pV%TR0}G`)WA;Ryh0Se|m4Sstc1BdX
zo7C<vynSH7{@mFKV{;>8XK2S1G#AXiLnFG^BC7&v8FxjT-L3so2zNo`;av^$10ODM
zX>C*KKZ2#cBEr=+?Ys1RHSirXR0h&m66kxgKc&6Dovnc~kZ#L(cz?$dF9V4t<AwWP
zoMVL?*Vh6sCI1SAJ(VJ)>h76&wtHrl|MAbv)MU0s)6WD#YM(Z-@IdPXN-UD#C8^$t
zcjHwxHF_wqjf5029$z&=QsT+wQww($Qj)GhQo>ajP4?Uw)^So4PxR^i0E>t=lMv(e
zQrFprG`N)zYGv8ev#ir(NzgpN_dM`I6IFpkm4%+L-##CVW{*4!Fq=K{Fu;f0=r5Q(
zVx7sY(2t!6r{!=B7)$bG1H|ay183i<+y2u4777|Cyo>T7XAdz9&}^Fmi|GpOuqCo0
z9C^@)u`NW`>nVAGBlVdnzdj$<;Ra=}rsWx7SqeZgos4MMM4YUht*dPB>;+z}BKOOF
zUurSJseG6b`V>*<GVzfI(JZqQ_XZ8%+A2yvTf~a>lw$mB5i3?wit)2WtXNGc#?Kb9
zVl$-}KU>6#ohilm*<zxW?hap7V0C8-T?2k^*Ibkrn$fc=mFG#|PP-S0Ve{bhd~;d{
zq9_aU+2~s~=$&JF6<|T{m9~0km|hiF(3{^@?`x*l1QzuE(pHa3%3osvSGHR90#_*%
zwX?+t2Z@!?u}fau!}TbaE8s9Q-+`e`OjHIkadHv)&2G7^Kj=|x0eu7=;RKd-dViuv
zdC~;lJ>bm8iuIIYycSXXk$J2DF(BJIuoy(VJh64P*{TBXGEog!v=y&dY<<rZn?Qn*
zR=%>2LD5>gjInm8S?dI1K&;)Ev=*=J`D$f>9^G>v_#N|52L4+`aw#4XT{|XzA@7qZ
zdXyOlZm**B?)#Cy_Fh)A2YHXcwfD$_kqrm9j*9Xi813zez|MZeu(6L&XD92?l?;5I
z1yTlHsG?-ay7c5&3!;O#nqCw}0EKuZN8rGrPgR>|n(YSgY7^Cf-&avyFGhElt*~KS
zQ0{0?xkqh6k*~L0vs0Td)r8#S*IPo8{AN!W*25ak?<}kaaQ#Z2Ou8>0Q-$aC=w=%H
zni(qt-&9dH)9>iu6Y4=WP2dVD%6O~kfJhS06%&<mhnZ>spRC$s0AEm1o)V*->P8I9
zgZWj=Rvq}9iF$w-bmr?VMmyIq-vd0|M0McPCh7scY@#}F6AQHm=%bH*4x8xbSaB-A
zD@@c2q;B%zFL1u)*b5{pmt)0pBpA8c0)1T3KRUd(IzL5^u6J%!{9HjcQr%N>#{=JI
zp>+`w)s;_okJr*sU8f1Y)XLfcY^g|o@?c2oN*yo_a+L=%4bB|+pWYnst)jV-P8oF(
zb3jVu%!Qq|r`F+F%d?w2QA>P^iWU=?{VU$oWp4}X=!vZ`hUCB29kxbh?rn|i13hM%
z*go(h`yfj9n`>Aj)70;<CBq1-7?M(z{)|?5cReO65t0UDDDf?WQCc379Rwxi>H8Wg
ziy#?tNE%w26m=P#pIeu80ln=L`yxGB@!td0EF`67G2|o)Lak_uWU?VCRkhQ#_^jrn
zp^!9?A+`-!(uqwY?8P4h?nP^1l$3M!9Tsphi;$EmJ)E~RE*DR{SA`)i_Tech=VdkX
znjYnT7`Rb`>tz>k6BXsxZAM=(VgtBBhv@=Go2U`y5(iNyh)<dA4j^vxt2?9pjaUcb
zSqcjDK|~knnOcFldUSIRKG&*KzE<I_1seE1Pb2<!3N-MN7Hauoh1clOIR)Qkw93B~
z5`L;5_ft=wHnkeHK~3(#!_tRYFIoE5NH}DFv86SG?3s%pr5SMw=kG!a&s!`rfPYYt
zyo}og%<PCYAR!myAuf(zR8yy#moo6{CTaq4mbhBKq6so-g+(yd40SC~NQj9oa*X!)
z?!MQ7Ur<rLbVz~Y!FqPC9<%0C{8c^5T{rL?6(yh7=IP1%Os@f4YHyZh2XK^%^0*jn
z<%z(p`w<x(>WRR^`Vkp@$%qZ$izey-zSK{b>TrxRtCJaS@=bx!mFA%ayiP@Oi+SEq
z21U2=JDZoakbk#=M&CVyGD@wdw%P@%{%(6SP`sJ=hV(sEU&3;WqJc7ynUz$1t@dW1
zFxRQ1#;E$I+na$hkeLlsearS{pm;NJZFW@kz1o|Ba+p$5TWML(?c3fAl!452srtd~
z%|IE*%r8{^*X_;Rqo*EgPkCHVz0jVrKu^78lm_r!6%8*PyWpf2qxVdj`Yu$dnQu1B
z=uvi6;4e*71O7@y`98$x6;EUn;J83fy=RmLaF=~;n*;8lqTD>A#k-A&S&o<=-U+1+
z+}X_6fEdiTA4cAMbln}LW?wV&-Dq9dg~#sO6?#L_7d!S)0|%R-`dj2`hU$+heAbId
zlp}@?F+&{~+T64{FyteOZk{nT-3(QL1Ua-cYcj@%oAC<9eU#BFiyk$K#iit5_q8t`
zAjHkm;^xBPcA88%Vhg(bVHl@4#*}EYJwqe9+|sQxle4W+>N{$dU$AT=Ul_W=40XhY
zIxuvc8Hy&Lsc!c-_f_D*Dw0p6<9Fi(@J$P#0wg&%adA);g7Vo#6ZZ`@ip72thY&Xl
znK*1gPfT1)i6)Mq2dE?zpI0<%ztp^*x1!bOX^|hW`nT2kn`w2R#kxp5mR_j_Z!p7E
z;7uw@PAoA#W!@@4!gdo&1U~L)79Up&sP~&igxFljEMg0KVisdcG>i9W;scZoiq9*W
zkHz-M_uN(Vn0QElva(X-dH>4%myp;j<QS3?Z_1~re$`w>-}-}6@vd%+T!o~Bt0PqZ
z<o2$h%=NBrk6eYMgsWxsO1iFHQ5(Q5O;iJJWuj<4V?%Weeb%%(fEdcJyM>9!;1pHp
zGQ|dPKNEET4^>fqi@@j@BQ}69_i%xZw%$vWeVrcN-6i-lrdB&n;jg_YjC~)qaESTu
zAtdj=C)C2%Q>HZyINwCmS68<ioBL@*beR{0w&UD3N#xdzC7)Y#&oVemU9qKTrQD}U
zk>s+aH<p|5`UHK2v0%H=EzXd>FqZGn;LIz&Kw;_&v?^V_%zOj5(^Nz~z+F|8Hxi?h
zjMxC)ZK59Fz5R3<Einxz4d4|fike^<jdCqLx+VmVw+za_4OEo9OgK>e6Qa`v|JcH<
z5!jzp<Y-_blwD$54YE^@js~7#36z0{s7NB|YcZJZc^516xkvZa(=?gW3b}`~xtN|i
zP>=Fq8Spq0m4Ulzit>epOZpi2QuN-d=Y86}K;q3*8TjCP!@$CclL|6#TUw!yD}f0{
z%%5kG-BKWe2{9uwAdaD><H9&fBrx74WX_etn1~Z?LZ$)d7I~qy(Adb57<+T%gip3Y
zi;KC{`^i>l)hmRiH&DOy*^tuoc9e-e>X6c{C8N*fK^5rdxhI~#XyIL~hQ6)GA*E?(
zUE1FLwR-Axqcniuo6frL0{&1%`R2mtK2HQb(vQgKStB-pOU^JSz~xkwJ7Kho5gWi4
zP1FTkd_T-JfTx+L3wXAP8o;v;Kr!0p;}X>=5sA4_>n%p)Cij#;vdD>yHns%nz$GlS
z8jz@ST}D@XB5;lU^Bi&_qjOa%@85wNYO*<z(GFIE8t|xoL`J?=(GD2bszR;k7U#6m
zVsxvuL<f*E=0rwRQofh~q>MR{5#1p~Al)HPH9)#USU@T(EFj$>ETB(5y8nukuaf-f
zmS!)I#9Z{%MDJ8db6M+>ULd*Vx{OFyzQYG3-JHmXjD-j!<DAHdq=g71|C|`^W^oCc
zl;A2WK{t>laJHJ(5)XJHuvn{RqE=06HOgw$4Wv4`6GjwhNT~^=I`a4hq&mU^QW{|a
zDUGmzR4MT&7naD~mQW*lL)I!O6P=@hPqHlQz|&0B3;e2zhL!q;w64*?uZ>p+lC0c`
z0GHCu#W@9U=QkYS=T#(6qeEJC9Z(g?0Z3Jxxo<1&hkA4}@I5LMRp0|E%3M6F1GYBd
z0ms`eR|oE8qF&%ZCaMG36D51#hJGUivLOl!$ZjY>gsb2oy_jBOo!mpKKjcNs@G>?f
zJ#?pUpXf4At1M@RdiPQ2yC=H2i?wR8R+I-?7#>zSG5**C(ux*P4Y;<7Wb*o23^10c
z+re51Iym#k+%Y?uFr1fUWSC=($n_YA2boXo(-6V};!;?lLlK3uK32Pv)se;YH|;p|
zGa)UsuQpmT`p}%zD>Npe&{U$TtgheMQj%E=mr@o2`QEHSXF`Ic-;=@OgXJn=gdRi`
z+80r1JrVzTTBVz9SvG)g9%y%Mz%vi>H$jaSqf1pPADaW8=tm4EGj>|X>0Gnb0mO~F
z#?;N}O`Zt6v*5JeVnis3x2|y<72<f#lJ5Z)<EU61WGug^1;nEZIR0*b?6iv0i_B>^
zu;{d!aN3=68vhXhPOr0RZUX;pPHVu~T1eSiX15p=BWWfgsSyc<&+iEsQIyPP*bu+d
zuvXSs+;wX=#b;Z5Rf<BKasvRIsv(ao%`APXG<wqL7Lud&%;F*+<NoN3i0+P5!m$33
z2&-4anNbK8Jj+6@ETP87gx*6RH>nSLK<;yZzGUa$(iiI8qIz@GfxJ;*hR*h)Fjrxb
zwzqy#{DBdf?`#=OCviU?(@C<+98Qm8m>$P4JxpeTy4ld&O#>DpNZ4*?$OpM!thHaL
z^*N1yC(C0Ru#nmy^1u*lMt*%D){5NESGRvM_YL5yDspQn{!Nltv7S<lzxNU=R#J-b
z=7r+R7HSoU0cl`hG5UB<TA()GGg}SdIfroR=>?vvqP!9|I@E~aq+))JgQ3)cUp4dH
zKr@(KWNc*Sqc`1{-`vbsfWuWwUdqjj)@`g;Mei&Ns|RR-NLbaDjV!F_4mLL5#C##F
zE8B*(lBLrLJk`Rg0WA;-t1}T+bhG;zHCxxC+u*^6n9DNoiz>=qT%V|euN8E`Ew2mw
zPEq$p9sIJO3x3+`0)JQ3{ksmf*Zf_~Bz}EC2L83xsSLbdMOhxd)xnDeUGSS;7x+$5
zH*D^AG`3C6*31%^c;&B~Z|obvs9IoxyS(f~@HCa>+Ki6&%rKPFCK?NUCVxu`EVSHZ
zQOlusn%b5dG|G!!;+}13WpAv<#pP=vn~poWG!_A6_=MaETsVB>m~kV=d@3Vwf{HAc
z)u$hYQxwCDBiK*9CUiMPsZ2L4*yv;iCoU`lAuKEdp`<gA6J$VmVFp4816@|PnTfXD
zG=SeVQ4RQg73CW=qxU=!IP|c5L(Ykej!>yQqX3WXM`U!BCjx)ekH~11!`b+w?TE~F
zu;}VQ;&IW{Ta4B+r*+_#Dso?U2IDeUZdXrS$%{|mRTfAWkP13ml@_B9JP}BYIgt@z
ziWA^bmP8kjqUO4czVC@ZikcG{k#FJ)({YKbBzukJ&<!L#XTEC54zW18fuxrc8Ihs9
zN(CO{b%CUp>oWSGCjv<?Co=NMMmGa-vQ3ixo+aA>EGFBuWZ(BhAnCa{n%0XmE#M9y
zx#UDfS9>CmTyi2KpIP+UeXK^s!k16bfGm7jvA|+x4a<y$FGOH5vxa4Mga!P+um_MC
z6D6;vfW_z*@g9H+R=KPkV6k`~V!<=ptW>cGWPaUjS6YnxY)7BQ5gqgS@u!yP*s8@M
zka%2ljYM?OuD!GN;@-AWD!?8U4J{o#8N1h8H$`7|+bgit@5^qX&85ZW0N*vm)*%*E
zR8}a3C|3<H4!x?*jwtUofW^l65St^VXIB%8KuY9FUu`|3h4PINZE!^Q|3YKL(M64M
zyVmYOn}Ra%5fx>3;9<?D>&fFxuR^V#@uJ^pdH&SCpN<F&_3Nk5E95Vmi>TaC<|swl
zdQ4~rb*?IMuM8xKe3$>N8sVxUL|}2FYqn?@E*3%r78@r1Q4m5|Xm=0_mw~od1VUl2
zXuEBMRhh5J{?&?G2L3}u$yOkR|1`Y{!BQl-lOn7W+qc5GY5+q6wgO}(uI(I#X`=Wo
z^(GB6KI9PWhpr)tO#YzzfZO0BtUPV5<6)>M;@LHB{MIUDxd-xgmbmAWO%GT!9>0yk
zIJZ=iG_a@`H~$!Y%&6PrMgzFj7wriIxTA{liyou1JQ4Wyendw9@I>Id{fLa7Ji=T9
zUr<r*n$f>JF|1{rS)I&2XL0lZ$t9mVMi+P@kX&*iqaPcw4t(20JwVdSbr~IKIrIR}
zHc=h;oQZmX6d^aq=uIQmfxb%78@ISh6{@s@CD03`F3w}6#pohW1X7or$cWO&UIcu{
z>jJ4uuFHth$fv77N+afg^DX&aAa%*jF`_i03-nd$U3f25snWKVKo^j@IFD7U(z%`p
zq%Jv;5v7sGLE!6N7f4-lT}G5fZli&e#(hyotMiOisS8M5a&wF*jpzb>m7*KPxR;t#
zX=_WM16Zt5)2ei~CjzO93$58=^qMCEDMC(U^pqvr0VKVg$jE0FJx6XC?UMg@FDXTP
zNnEA>Q?4aD==@Kyrt7AC&-Wr)^Gz?J37@ql?2g+lx`Dn!-92m<<>=2*dLl|R9Wf<x
z6jLHchb`PuOo<%Dl*mySi`?jrk8Y68h@y#}j!)83U!g~LpB2_Tb?+7G$xVBxCZh1l
z@U#ZR6;duwKsv<jG9Th%d82j@mqW1#q?>Y!jM(TC-#a3@t*j$!z+!X_+kUx!lT3kh
zwR75N5gqq$LIe^|PGsbxi)#0**6uHQlxs2YpDJ>1M6fj6)=)I(F9nwRJ+_247xG2~
zTOU|d(d__~LX^=Pk=Iq7n*zBa1B<2q5cda^o;v`s2(%JqcO2_$p?vA1OEuAL^}pX3
zadc5*lr^eui>?furlRCy@LO8*QKna+)`xh}hFYF2+xOFOWn-vcKZRQW<W<Z?RBk9M
zC?)kqWb<g&qPoPC$P+<umn9E^KuY1(XS21xhQeKz5P`)N-?SCaU6v4m#kPwdEeU0z
zJw_<(I3IuW38!eLCVP@iQ5kr$ijoaQ3LBeVg<vU?Y+5YLUwKv-z&JgHp#d8TG85N!
z4%>OzRK2C%s}L-^cH0d+G>#&ZKj=Q-Ld=uAcm#5R%`X|DM-+M9l`km*i;D5978GHn
zF86SF0FvDlh?<Kke$|2^4---cU{Nt%52cy;OO0*yC?7@vuQ5>v@Ol;HrD1gMk-Qk`
z06wasoXBYMQAPw#Gf@LL>S$vDS2R%r_?n5L2P1OoumtMBFPf+aNF*-QdW#Xk$VXVf
zV=dGkAo1n8jK*4Mb>Mgt^#DmA*A3f@9Co*WE5IvE)CFWFoUKZW(Q!5}UBKlvhn&cW
zq@~?}BrQ7$kfenLB;(v1Ba#+f;Gvd(7w~iy<+_ZX_e5Z^Q1RF2DAX2Kl`8N@Ch7)K
z8t1fXg?i8m)eWR&b0Q<EBd<V#R7c)M0I7qpfYd=)K++OcyUC|(<@<j?+{zc>uI*!h
zW8ndDobedoT6jRB$arBKr2Pl2kK3}q?_15v2DQ1etJW1d_^BS9F8D!}$yaBAj}&#o
znNMw%V{~#^0<wWKxi?#AH8ObCJk}@%nTukWH(SGr)<9*Tch`fvhdc|ouSL+q$~)IY
zW#C07>H>bxL}j4QrVIE>&muvlMM?@}b<27Mj<+(Df%K4E$5_=TdKQq`6kEV+Jqzfo
z+y#8evw%#16rR0s4_ozRAlrn*1w6*HfNUEQ*KK<Ce$N6vW}+V8Urkg7`i6*ZVE5B0
zKFCis&^JUc4dG|Mm%{rh_mVSN$rCc6)Axyo=Ko#W9dX<paoino+|jl8R;?H-LpDHc
zZ)B}e1yUy2jc)0~Asr#AIDF7%ugZiz=|w=w>3*&!T;fx73GL;VQb>7LL-&;EnLDIp
zaWC9X^qT3_g}VwVNmn6Bc7ugQlYFDKLR6h<XlEO(H*M_pXr_lbN$uOAp4_R1Y9b1~
zF$XNI#Y;z(vVRCEB+^0bWiA9Z92zPW7J6Ji<vtjQDG3Wp+=L1Xi19@5q3xliV<(4}
zoD5T3n^S$ehb|g|8f<>(k<_BhuXId)!@j;A#WwIV6}j7}MvKwqo(MeS*xW`=3}+P&
z8=GPsxTT3IK-@T6^@P)U%4z(-hSLqb2jG4t>Hrp<RuWDtIDJ}mU38TeqhXrAv=4A*
zp(K?SBMO!1GXka#vS9)*wt%~V#el1cfUBv1s}``2BffoAXE>MJO<>V!GvTy}Q_Q<6
zC7xO%51n}UhGp6VEIRE>IPJvg)2i#7b|x~7KFwHB70xuLHJts@i!jT?%8k$ZI=7^m
zxCz}(4fL9!GH^c=^#V^ZQJG-Q_aY*A$%}wKs9xYG>%}sVcnhI#=2^goi8_E?CMwgx
z$9NIYmm#{uo~P{VO{)QXUPW%Y;=kmI73(NIXSQlUY$dLkv7riv(8||xxNBY8KJ+IC
z3_Vsfk444!VK`Q_oKZ}4+N&DO%Cqu4ICXt2>$#bFlsz7Jjf(PVV04owhINT;)v<Mz
zY1M!jbmr?VMnCgJp!XR4VqxrY8Xhqye{;|TdrQ+69(x5#S6fIOKn&*n!N{ABJ~KL1
zZCq})W`q$yIa?`9mEKrk!mQF;6W^M!dJEw<On*8Mll}Oi@!M7J4byJ`-!V}Q_^yib
znP+s|alF5)0cWcyCo=lPY$JwoJRZlfD9yjP2+V)Sr}?D90TNnPb6wMNZUDb%q8jiB
z73K9}be1Oq&+bQL^i59$eybmm(QTd>&cOe_qLV*?pV$hkH^N;P)|uD}>z9$k3PKKR
zn32OOLJsSgk;6)^qecF-9^Li|-qt1{dQf8mpcd5{rdA=jV+z*6eV!Hug#~IRmjxge
z^X3cdA;hp6IWeq7;zBcB<2lsgDgzHUQ3a0}l-J0>?-{H73x&RgqYB=xl$TAb0i1dK
zK#v<JzHf?k;5w=(*#S2!*s8Y}ZR?4^qSN?sBX&9sr|W$wk2ZTr%K%5xi^JrX0L}4d
z)Nw{;fNRKMI}A||!}Tb)LBR2rNH;FAmUwl&l^WR58v?FxhI+PA=nX};)_bUdx!w?P
z12fbc8;WiqhbnuVX>|a}Cvnq<`-ZxAs}Fpjh138Zsv`Nf#)h;Q9HGKdrDOXhjg#NW
zxN!J7@$W*MqO3xl<E)}*EVMpcQPKKv6-0}MZ`=FCO~VTj$V|JvBJO=We9bCb1~OOj
zybCN$8y+#Acq$HU%L}i`t<bvs11FPPq5X^*ddQgJ^7+2TjfIaK>{8mKtLt&e(k%JE
z5xb0*fAbsJSUopF&ZPduZ)$OYHGBhjzzG9wdngW3mfRATz)Pr1rWCkzfd<Bb(83xe
ztdGR{D6`%H#G(W-66~!<F9^@oWXaU6SMX(eF8f?+o(|r$>Xc*i(WL}aXv^czcQ9bF
zH)2E4zupxas>X(@v7zYw>-RpA`=3WzgYgw>4JOsc17G=VLf94|%Qap2U&u-Pa&1hB
z9^GO}^lSy?H9{}#2c%yXmo>NPxY?z#2;Nh!oaoC=gwb!H4e|wRl*sQmJ++S><+puT
zp0HMnVULPOneUNuHuN)7S<mZHzPbRMdZJ(V^%kT3R4PQ^{{4vUJd8EP3J{xak}Ku`
zb8-t8+B!y~yjYKJ!GeEg8I&JaNG6U3e#g^*D`{lIO2_oIR?z`5I$iMgmO>fWWx7Z9
zMHx8VFopZn|3enXjHT7aa%O%;U18IULPIyJfjiC6en4^^QaWIW8d%H>?FS^&A*BO%
z(X(IhEFg0<q;yaiK{y-5e@Tv6@#@onv`vv7n!ZAl`iUhs14!QTn@&LT%~#Vg@h@n+
ztY+Ty<_!9jsTGCE%^ew=+fa43G2IzJDk;$c$vZc9RBUdhnwe+jD!|1~%GcnK(mueA
zOjH40YNCC@LY}HNuhgS!$<UIMVd@MuwM6OY(y?a@y>R&G7NT?X)ROs$G4fk4QFrgB
zoKr2L>0v}W=&7<%W&p{<eW%d9nv(r&^~gn^W-{FLJAfqSJ{JR%nA}|fapP!Vbkyli
z^|Dy$cz3_lj`l01$-k=@QaJUvS(=@)1nqZbDY^!IQ8PV8k1nPY;+R5OBBnmI!1OFu
znw_!~(h`@>d1~pu^(dDE;O|wG-?cOPt0w|inp3>MFdD5=u?XC%ACb}So(SB(ACb{f
zo)}INX{=+E3J^E>#fg!(6<wTe)C}*`qgz(s$1M9Y@JSVAH<<s@!3PCh@RAl=8Mus!
zGF=L{t_q!B@I<c*+^nceT6+|9!A-9V>?-Pp<#m3;`p#8_t@_x<8o-@Yl;7bodchMz
zTmPxHeym654}6cx<e?n+fQqsj&DX&{3cBE7=CTZ2Tt%7gbREpqqw{-);(2-$zrf2?
zl<8ipgS+(Tbit2$UEmW%-7x>NHTG>yuNqe^y5(P>?6Grg27u#FMpOe%P*FZ9jL!GO
zFsL`w{2E4S##+r#>o^VOCJU+zyhBBcmyW)tG~!F8v6$P;Jd}Y|6&V-7zMcyt2jhk@
zkqOmtQw846#|b1(`Mex>m$I^dA?$G-yy8`XZ<(k{azrQA!npQT_MxUV9XMA-%jos;
z1pPn<If1P%Uwk>*UChVMF0HuY>T8a67gxxHK<x%3LpneU9C}7hJ_8yHGo>r8uuP{B
z+}1Xs(+B|<@=t)YGerK`8+J^gmwfF`2x!WDgnEf7ny1m7@?{oO19+;6@&(0c<*yhq
zjQw)8b%!2Z8-h1C`&Hm!Dw3Y-YcZJRd9lZ+!SG1P^397_D_X*Ls*m63aiF_-xOZDz
z(}4G>XlUt}uo9&(oq=4yo#|Zwj~T$lQhJVx$*b6jtuXsc7)FOC6CrH@^u}aPLSu>c
z*;9kq+X6j4^j3mXQ;u{!(5uBI{Fc(cXQtb8{>z&NdbK#BmzDlEGu@u^sy7YvYH>vW
z!lW*fp>o3;{s-sZ-ZXHisl}$p=($t;3IzU0Mfp-?w8^P$muhUQp0ZVM#kT5UI^)&(
zR(f>H9*j}B$-vUcQ;4d(ib5Y~^db`D-cS=mUMpJRxSy>dWy=6P0?3462+0M5xfyCs
zvH>1u9^_voW8j0etpDLvfj$mV4dW0OVV&<+6JAdi{Fk(Z7wRz?Kbm+J<3Mjrf(VTz
zf_PAac-H4L2(1Yk9n>t!;o6?eV>0wmQ$h;$5>alb{<hZR;Iw|BY#by53P}m?x2c``
zy!XEcrW0CIv>b=~RSVqf43<nUu&kny?gyerjGQ~Vg=Cwr8X35)iaZ&~xUU&GY&YUL
z#Nw#~_po?+fOvO1P~BqL%3|pO;yEWW@^M5Tj1tG$7DolR(P>2A1-$yqT;$AGT8tL6
zIJ$s%%!%!i$CF(8fV-$u`Mh~<9|IgYYt_hYMol@6|FAL6wuxgR7HgOEEj-}Pgx-0P
z-p{qFck9t@-r%RK;$`5oD$1S^U(mt31zqrHo0>9kj1_L?kk-RGc)|04FBf@l>)-=D
zy7<5=oX+%@fvcz}i*HjM?4n1f3qH*2KCJM0F9N=(qRiaiwCk797IWsnpH!Kw>u)Q(
zqd)^c>S@3yRFt`RUI+Awn7dtD_c?3fGVu2*%2fZXgQaXmM;8$0W(BRKywxpD8Yl0Z
z!-Cq0ur&Dzc@IoN1S}tTycoq^A*S~<HnSUc?>LQ!<&@2o%C4|!>0*;%waH&WQK>Dg
zJYDQ5RWG6_EFn=0L+H^a-pTg18tyhTLp5xBL(%8H`>UZT8ozvL1V}2ff-mdCI>}^0
z2Bam*K+?&0VM3WOOeyUD;zJ1QV3t*PI&ij$rU5T8(e!W?6pgYPK5cD29XQEE(^w$n
zTC@<RFjXBNWC2VAvVvVxZ8}aq>OeryazZ|!One4Hm@HbefPi4zS_s=;{t35<t#AQk
z!Z4vwGre#^tv|+VE2P}3P0Hqw_?x<kx-}ecTSNnR@Of;dHQ>=IlADHmP8u<C;^-EV
zHO@D37zHj4G#7OsTGFcXPim)kgXvX(1-)wrp;t}l-8cxnW<u|kLFjd&H`7Yk1EgT?
zuAnnfzUW?Oi?i~|R`uxiLGW=ZlWPL-1Qlf$fOmDU=GnOorwiUqWugn*Q$?BX%{q9l
zpbK8|9A>)=Tt-Ei?kCU9&u^kf=XZ+YJ@qJldljBspn-4mv<DQvQJ{gRScS{L9aNM>
z5;hf;J5Vd**7+p2AyAgdny#H7=eNTXVM4ivTsq0HTjO<GMs`glw;%Wu;{4ds*f2_#
z;5==8Ss#~2eFy0<D?t~{#rjBG)6n{&X?4e1-C;lzv|JPQD3b?lSub>)qc$%v+tK~T
zMq0(19tYa!;Z9Pnyd(lLODoH(;7wfx<)4RkNUASrbo9Z^-_*nk8tg!B;lAeG0_lOl
zy8W{jVpZK8mn;n{?L0gyZWLn39ab7SZ=s~bcm1x`6rZwGhuyP78=oOJ=`$oHd@|wW
zAbGCPF3*sg^cj*8KF4THKWA5-2JloBxjnYgVswEghV4B`Pwk*bw_kxPrd9^-t0LK9
z`dSQ5@Vs*r;?bG=FfP5R=3q<_=~Q*gH}e@EZIRW1cz_y4he<M)^$x^pmOKOO3J-{j
zj2GIUry;#;p*4V||KQrv0UU0k25?gobpW?kQ9kR84)a9d$tG$5=U-q>fiIb;0bK9v
z#%h<)LzYk-NEYs!;_8{;T&VRHBX2(1f8%;p!g@Jd^4D+_!E&rnGSbh9jL!E&;1-&>
zTy?IT!+^5n+Km2eMXCVFLgNVv6(>?nB_c0DqC~2xM5?Vgkt&H0r3<CnVnm?w{svf#
zIez~#PeY*;@-I;VkFflEfVde}n$y=>>cYHZcGaWX2~JV`oq`N}t;%E_0k2n4c02P6
z9T2emGDuM15^-GMLj$%<R0UqHqU-}V2BaVrBJk-JSSRoV6GflF-5M?LW;~zGc;=eS
z9QBnJoHvJXm@WB4FHHIlbvVhoxfZXg=&_uM{i$h{fvZ}ndVqu>F<slo#4-_?FpTO@
zHG-ymi*B)t@`hMRk0M&Q)&a{#S_dplYeo0<**8|4@cM+2Id#EyW8-h1?G;7X5w}?M
zMe|T=MCr9>UY5K<Rpeo;3H=A!=x6eF`qWoe3$9<YrYx_eMZ4TeT?Q6rZQh)PTd&8~
z>#_BE!uqvstsh_wb^wrchRQQZ?D2ru;{geeS4JLpS5KIimW!?L7hB&iVg1I)`ny{2
zHLS|h!-77bl!t9@rUCtQO^*YZo(SOfD1fk;+^zl7Cq47V!>`@?S@LAQi|4{9;rft1
za!sDqMCa>qS@-A3^8ahf->Av|uQ|5GnU|EYYDt#r8_Q2vT@RiK;}vC&iI}p?onn3v
z+c141GveCL2x~i8{@KDg^6Yf}?e2e9?>AUe?gI*^iUaW%7BaqyL>ov*8Qf-t)+4rb
zqQ}dSvWi@U^M9*xPANZ*-qigSTHBz%Z?<OuZ!*#Tz<X8XR%-YkA>3eU(}2G)(RAQL
z-qP%Lmi*--+VyefDbeL*qTaNgWjC)4-~|`w7bE#gCEzzz<ZhW7Ek+w(X2dW^%wKHg
z>%hIud=GFx6V-wEa31Rkk3H=?o@yQ|z(-8f4a9SPJIV;>@~RbBjH6<4{MZ8S20m0M
zQKiM`pPmS$YPst$jUz6}YdTAh?$!`|hRWo^20Tke*=^u&bnrw$7yM_hyX-e|7o+tk
z=73{Vl$pCu2RG@_g#y0E>jEDr>b`zy?t^4g&$_$ov@j>~9-4_b%gWZv1Wf)<nyvRM
z3h(tI;8QA+H|l+@<u!=4^(YR)xDxNa_lm;mio@!P!|Ece7cHzVB6!v!=!zqVK498R
z=V}W-XTWc$NNy}I>AOfP{&%aq2#MlctAq$~y1cH~Og(J9J=J#Iy9N5<cVnNKP5h<v
zkJZ4<W@s9a7L@?z^)Xo2nrRw6_o^2Gi6K)Zp`~pLng%4Wj2GtkO|`qY%~1_g=q3I@
zlNtS!X-#8BU$&WzzM$Eur=TXkiy4v<k8U&6&cWuY=RX6}39Z$132mjv%sVw?5|bf?
znqoZEOZZz${Y<c>I~c8rI)A-3j}72AOw<MZmWuMr5Tk9r#m>+L{G5t%BBSY^2t2SK
zk<pJl5%?1mHGt2VC|bd!)nPf*f!{Py50FG$wDlIF?ah1-kVJAKqXRq<NFq6r5xL0h
z0m()F4h87bim$O6`=ORY<^SRCO~CG)uKxd%K@t=(wWexBv?v<1q-ttwtf6Oi9-cuG
zMNJJsLz6sFUedRNr08p?o*=C#^`>fwIfj-RDvBCQsG=>J7DWesYv1?!tbJ$ieeyhS
zfA9ag{{Mfji~GCRz2>#{aPBiaK)iMqD;C4co)L)GIU}Q*#ZUpBrKFsZ(fOVcNGv%c
zBf^or97tq2W4JUNsJ>3ttJ|``M_3$X0=?6do>WMPP7-|E#WZK_PK6KXRVLq%_R~7p
z$>OT*r;tQAb(D}a$RZVnpIEo{QOEwvd~M?XXUtxc8XW3Lm?L9}2kj55P68ZS+O~~%
z)WFfgG*?xQX`X#_jsf`|R+^qlelOl%V*&L7sYfQ-$td~<sO{CJz6``n_S~8QHsu8M
zzi;t+bt1FKU<DS5&G+iRovv7wwLSTekb(G=IU44HdGMx1BbD7rn|@cD2}0ac2m2R^
z45lklmae%9ukoZ#M&GtN)_}uI`_z8(!)vdh{YR#~2E?Gu+y1DnlW}5fJu@nCG9U>O
z5^(VOz!wpY*MVr=#yOH8O9c=Yv!ECdvG4&gnDNCXqlc)2jgr)p_rV2(b`mCz#)<~B
zLjHUG6-)iweZ%_ymtu=B_@^~tFDo)jtmG-|`fz`dq@ex7wzSt`MN#ekJ@ZKY&usOp
zXDf&+W{Esrs#;cWv)eX+-!f7SI9Ex-N;ArB2H#bpI~+9HjGmi^!W!^JC1uhKey_w_
zn$hN$m~>dI*yB1LpKTs@198WxubXo(m~-7gEa!}jydTj?b!#>DDZRR#5xld~vZn#N
z>!44svH<`m7x}_K|E{I^w#hY#W~uq#j6IE>4&v1jW=($BF0=*{u8WsAj+cqedc4-@
zT&q{Z;H8wXdx*l}T(eGLw1qJ?fNvTpN;?+kn#DTseIqr2*vPBO=%c2-4)hkI^kMN^
zroIAv&PX*NHk`#uo6#m_u>w3@NjW2<8;r3A#I>9;tlQVrg|F*XJ}UC*P9GHsd6OR%
z37Ha)DToTE#pZ6<#Opt6AjNy3@1dT|pHM;4NhDXZB+C1mZASPd>v*V5NTC;{%koA&
zRIl<oOu)00<Wk>gGrG((0ym#evp0cz7^wmL%;m-f{F0Fxz}^MM1)OH22JkK;HG!`f
zsR8WzvFQRHW~2u2Y$fGkGTQh`lLc<7q?~c!MAk{<8J4mhAlY=f>WSRe<J|U;+k-5(
zbs!0qb_J49d5I1rp~3|`$Ff!jl2G9S{@!x|U$}z&^Z<ujZo7b_U339SyZ8nCr3v-`
zpD|Jwko1eLfpcFW_r*k45{a(FiS8xQ`&*(bK(g!VR%tWZ+xoZyBy~9>qotO}3UD1u
zWH0b$CFQb=R<V@y0?BF4$cVJMw<A=BwC0SA$gCU+fh0L+95~O_RGzDeJXhm9ca!H`
zEYDRSnRR|tEzdNve3S%8Bj=2aW>}u9KuVA^GJ4uG0;z(0+!aU(a#==HK^TFQAZKJm
z6~t{ICCC{WQ3cTmEEXjGFi}ZY7OEg0Y6a5x{6HyV>L9E@>L8E4mrQ5C(jt7ot+mZF
zJ_b|@JB%ri$OEZPCePqHtH4Ac#mV>>yzcpc<TvAEK!(LGkOe&BV?bi@JM8ryZOSQn
zbthc#ENj>b@C+plD;+<ieWMQUFUW$QGTHJ!6t4b5*3$AAh1(dZvY*0}JPCM;k}{)n
zba0*L1KwEVqq9ha3oHzNx2Ugb^v}h%nbm;wk^9Xk+UGv=qDDvTZ>wYtNM|@n8iGCE
z(ZFYE@!cPAp&{7i9Uqv^aC~8$iNjA=r{V{>sn;u3ugO5_CM^cM)xw>;njRC#Z&7KO
zXT;Yt@xy?d8EGPLf{_jb(j4Ln(7STj|HPG8{7a#?VOO}$p)HtxjuxJwQXTe$w$b{U
zUO6dG+pEI|%R}Dep*UnplsjBz#pb9^HbdT|&5$W!lf%&;bWazXyLYk~@+NJDObMH`
zE=!I3F3I-erkBP-cw3Y1zDttb2rGSvb{Y%K2J$01d^5r!`GThVW`ulHXya(@xJYGL
zd~(_RyA%V-zOS+@8IrA2cx}ZVyQiLMiudTapC#^vv4u9WEQiW7VyHD)k5HY|BUC1&
zP*tL8`~N841<ur~#5a@J!x%!|q-P;h!sY_C^}--*hP+9eAydL8whA>KvuB5yAMz$`
zhD-^Y>@(~!#onQxSiy?-=qDz6@m^Sy&_?EKs5~QvT9Z+Q>LjXAnUF$NiP-1qw)?nV
zC58(+^;O85^ekjb*nC-SZLItDifo3wNt+>4!X~x~HD1su&X6~0Gh|BG#1>}~iSzVM
zHbdT|&5$W!6I;AE5u3Yrnm-|L(q_n%u=!=(z3in=Y1liRz6KWZCT)gH37g~f95vsb
z#590&uj0Vm1Kjh!A@u?GH&O$5ijjJNS18H75^J;>9ktNZ0Ut6_5AZc3^#R{EQUf^p
zr=|<I*ENuOfGs2S0ZTv2xpLPSjZ&(71`W7yIYvf{JR|VXYl|8g?ROpC^Z{4B9#RkR
zgGTBDZfvA7@BkzA0k1Gp5AbRwX=K4t8dttf<*BW_lv^}|0o9d9N<hky@iCwv!Ux2W
zj4yDb6#G$0_%SEpM+HCVGSNS01_PSRMPIR&drBKIV`Z?fwo}f^h=4Ly281M!eSpVn
zxidZnyKA*`m5iug#>#*S=B$h;V#dmVMCPmmXR?~e<oStAR>>q`iq-RHFd$HQvI4}B
zjBj8+S_wavB>ZS4{8*Ckqm}SuN%U>&xU;$ve%zbzqbuRZy$L_M5`Nqp`SAr^NGI8X
z+6QD|b$>npJkQu_z$=W@^MXRArF{B_PG=1jQoMFHarL9NP&R?1{i?`9$$BKZfb07e
z21pCbUl$qDW<awhK9_We?lO1UZc`%;rmgHz#6b{3id!*ykL6s|d8J;9PXwT0OxpH|
z8Ke!}8Ib|mDr$j7fSEwp{J&Q7zp)U@z=HL8W}RR~*}NIEbs_qe?Qdluj%9oaPv%8!
z_ndn0JMRgw;K_oNCktj=pdMW8JpmRxSui8v$%4p}SJi`M-V<QKlZ7cy7S8ywdT_P(
z1X%E7Vak*EheqC35B}jj0pf`~r>8ThjgW3s>$iF@fOsKXVK#&m_Q!CYzoTqQmw|X7
zC7x}$_@o-lh-^(bmJ#n$4nJMWKwQiCaBfT8nld6gQ=zA4NA>%F`a0VC48&)7m>y@X
zJM=@i@B)KE3R865Px52YoqClI1uRoox|$PHd4j^GC!MMA5>L8K_xoq<Oj-v1u6Pt+
z-J57jVHwDg(0v_24|{9cp|1?&JSbeiJ3JSV!=P}H`MRA8%Rp9K;R=rf&WYgPK^pCU
zSkx1NS1CzKz1TYHMj!X$88~#39%AGXC$P{bb7tTzu4Nio@o`d!b7nB0z!_)Yvh8(j
zZ1VEzI$qK>wAQp%@m|<t#d~4174K1AoGRWM$hYjJRp?Yk486&Sp&J=7w4V_NHY`7}
z6dM*|Y*>h~VIjtbg%}#XT8{*r10*}QP>q3jlaZkCdR07J8e?Xg5oQuEM;GcDW9u83
z=MCUD7D1{3zo{g59B#B3&G(GJ>y~3=bh~E+zP21Aqp#d(rh)U7l$&Psf@cK2yc}bg
zP2zpq&Q^6G;mAAjz)Q^GI&h(pdVqxIT&uSk;YZ?)eVo|Ql3oE~+0E>J1{a#m3XlY3
zd<?F(Y*c`Fk@0m(2FB&}1`yLCcfR>?p7~J)VmjkvaE19%1s42>e}ps6M=SJ0Hg;U1
z67mxVz-!F0ULa*~MQK@4@KDqNZ!l+DK%7mSCUA8lD_s|Gfsy)vKQYoFK%#Jdb=edm
z5c$~*AX7;;AYd`3E{lmtBaA>|%Z)JlsYTiaTxz5~Ai?LdVapK4ts0qo6Ab*UMOy~G
zprq_WlcBD!>rHezUEo8lq88NWysx3A2X!`IWl~LYML3BUM-$Y3+55eND)8pZDEhhS
zE(1xotgBnAa}9H{3_MgxGDT+f->Cz#o%sCaNOfmRbE^)dc|_IxejKB`8Ii$F=0_b!
zCNjP-gBWGriC@6asvqtpGkB)?Rs~WyQ9rw%!5WsdD)2)}8d~~lY4<4IRiYxtvL=z{
z_q{UUx*DcaHeNA#7fG^lhh?J-yvHQxmcYI!(XX|TpS?Bk;>%QMWJ3N56OcSeQf66F
zK4Ec82JWS#jE}(yo)36Rk&jf}<oQU|M=apUz#Wv7nPA{^H5piF*V+9f@Ie(Wh7Ij$
zy(homu%`bhm6H5`0C#*|qd%F&%-z}=>i^&8k7b&yt#9Il(*RDp6;dDY3?nswi;dI=
zyhBND#cs42?RmSY3nx2HeAJw%1F<Y!GN+#bw&e*k5bYUXC;ekge+7t2S&o6IlqbAE
zVsH^v5)nn8=O!X@DRUo)8F}3c#EftOix#U1i_vT5*kUWTSc@&TQWjeYi_!D8*kV^`
zQK~UJQME2?7OFP8KdjoLTCJD#>b7I>@SAC-GH{fVvaNkh9ehl$P8MACvcP6hcHS+y
z?*G=SlU@C`oc0mDI$G%OHLB#Dg3Xf>y3kv-nNvmWW(D=*b-s`zX^zn1I+_{ME}JBf
zLQ4hCNK~MlsK6Of1-4NA#}z6-l&|_M?x{f@VF8u@pm3!^Kw*&LE%ENFY%-yN2P#Pd
zcxC2f9ZdCnz)OmJ3~o`PD--wulPv?^HB!$On(fcnDpdwj0C}OiOH}kDgA_?nWUEku
zdHu|15+h;F>nA(7oe>#aY0i~_*IQUqOJMJvC=43}?<ktf2iO<zU?80<ek_|=(yH#E
zSNZ#&M@NM|n3@-YeP?DEtTRijbb+IRskxXSdg|fWz=SGn0|i<<7sLS{PQXmhtjQP<
zlKkmfU?I9!qUegY!YGUP;%E;Kqb;(>QAfWdM0x2r`6HPung`nSoK#i#HBSPPnylaq
zNRDU&78>K7L}N^%M@V6&j{!*(eZWFKm(3(+z6&Oi7n)o|fy~*gWO0rrg`KfbC+Hn|
zl?e*O3wc@ntgcr~PvOEX)*j)C%|%c5?^QEwRH6vz&Gn$0mJlwWH)m^-@B5xGI`%3O
zE58#L?;Fw2OWm#Wc!^%ua3|&dMMFl&Z)&>_9Jaaff-<aAnjs%eboYu}p)>C)_ez?~
zNG&%$qzvnnW)4DaS(oIiY-w%)&s@wgpawioN%`TE(LJ6K`0#R!jNbQ*z<ut>jpP~`
z-KbP~tqr_wIYve+-Dy6C2_v$F7FiwmUGt+CNG#5edYjSrjj;|SmYk8%p5|~bkXUj?
zMh#=E1BpfE0Pt?l1@w_d(}GBkv`8z!Z&?6MAh9{WDi-Ozo)JiFIU}Rp%(W(v*m6cj
z-r?u~kHgeMPG3MeO`dN7iw;-KVd^2WKwNZwRNIX9Hi!FwxR^6Cq8_3VNIk?GAoUO~
z;Gfk(u8xt9zi;5yY!N@f$_l)&p8)~NLr@@2XM7BBTlj#uo$)ciap42vZN@jSzg-D`
z7bX1dO8C2|KlZmP;qRjU*x#;%zl-9NnN9?ploy-8`}CH?xgg<hC3dZXYhTqeXQHud
zm9X#`Uzo}p)c&t*u9kuHyS#@$?|$BTx93oWcp<wQ5MM+v^g*~fdGMHe;0}S{KYFi#
ze=|~d=z^0B4LVvUi^Ft{aXjF$w(gXHyC^BkADMWgNW>8WAFLyrs*;_(QS`s<No-B4
zTJp=lEsRtHe$q%~;I2lh0coqe?ezZj##at^xkTMY>sI$^+wvN~+FiB_0$WNNUYap)
zdYe&~QQgBMM%`W<c#JVlUpAx7=mgIQJk1!L9Y$w(M&OytF@`NNNAKQjf(_vHKPR{*
z@D?QvFU=IAjBZ!z%JR9&{t>H;9<osz()PbF9i1%SZkFr7>&$WuMGtxsaJcGrzxcF;
z;;qbH4Y-Yx+$K?PGuqZO0`WCx9N66o?%rm;^#O6)>8d2M5$#Ny-<>CFOTD_81)iWZ
zSxtdED=GW(-u^l`upkR=d0Ak0QT7-e99NJ9PxG?C=|$P!=-`QhEcg{K3w*UG`#$$I
zcP`!`ibq>ME5LP>l*JpCW_PXSQC8Ci@Dd}{fIm`FK0z5>VbtuC+&a3-ivw>lMmIqj
z-RT*D4;!PKpo|{%jKJS4$Ji;j<4mv)TwtVLU@?q(n-LZg@3bye9se+04d9mdFn4Rf
zPbf)#K&{+nFkXqS*Bfm{hkNNTq$zr9nz1y1GmKONo@S&5@asy-Er#mn=-r3BAaM74
z38@L3tfX8QqtlGB0es0wP2ihGY5?#4rCI3Y*N@GwI<VUmbp!Fl`BiT-^6H~U<#E^*
z!mgP59v}*x`iiM1R#|C)#42YTAh8M;@FN;_u8xuSHhPI4ds~gY?M-=GO?X?4z3q*?
zt){%KwqtLj7kaU`t=QYXl((&fx2@RQzS!GV%G*}L+h{L|z3qy<t);x}GH>xm9s~gK
zM?O6PBz3OrUFO;iHV0}zIw5BaX9m*%FXe$b@DDm!<ovd1CQVEJgfDh3`U~K9sEX(G
zT6`P{4KwmNK_K}N%L~o&*K{TobwKl8O$Op!rh{aWi;Nhif@NtBo3dpf4kbQgu|zYz
z!+oqi4d9yhL#hEkq9j+0Mw`(z&j`G3IYvgGc)&CQKdGc#BctOzV;Ck5?_>_wfh(II
zy+B-a>g(q4IAg2>_bfPEZ!@~a7<+-ll51pir!m%n#3EZKaAymv4)l>mO9zp@Vw0i*
zT+7TgfyGEG7U@-<5s1q!fJ&Q@_al1rgCC?>9-{zBvpj487X6SFQtgvwkp+@t=SS5t
zeyus&2gJpkkr8PYjX=^Y-T+CnZ~+f3bX>K~$j2WoigElc;wL<rkPG@55THD&0pfJV
z#{jp54~W|t9|Ig0J|Ny^d;`yru7tk}6aIE3{9V`|``eZ9cVU0*Z&$+Kh5ezwG&N4j
z;Q(0INiK2DOZZ!fU8~^Qi`qz;XzW@gEPTcny7pPM-?GJ~45Z)XbVDaEwBGG`P$6E(
z!vP?^h+ybL;t>KK^r;8#)eiVX?-g*Ck-9?{oMdRw(K^Na?Se({Ri?fSyhlk{{>a3U
z#pL4%fe+S^pQz?vc%$e)z%pHC$2r53fL9u+2Bcvm(?fL~Si=_Oa=0la>M={XuQ#a%
z@XiNqcLP44B)4=m+KkS7*cgGADk*0SOA;HcW3<<d_5lC0V6@(5bdzTU;*T?0H}78b
zjKH^zR0rPhD`MyYp7Kym%GELYjZ&rEfyH3!QUArkR^niLQ^8ge!S<$ttt5i&jf1Ta
zY>x%o3nV-jY^BYJ2xZR)7K4>PZ5aHEgRREF_N9WYCW7rt1zSx7+ZP90CD^kq*ghcP
zxnQe_VEa<R#y=Yt2iqc8{E<~;;Y<d2BpV-)Y-W5xE=#fgF7)4J9oz&KyT8kFgn`6S
z`2|hP-}LI%G4NRH@-pzFN=kmT`f^=9PBy=~<B8ghi^nWsvPG}mQFN;+EWW&e)~`k+
zX$xkCW-=m%$b3d*K<<mGp^>gkAgl-NbBT3S8Q5d_>H|(RQUgfe$$^dzTiH_42VBia
z4d6G8)CWA<NDbh(Z5{3lU1v@ErFR|Zow5bZD~g_0P`Z=Md>QDaqN*-b&+ax48^G0m
zO)u1dn;NMB9H%5VV;gNo7a3y>c*Syzj2`ohVP<gckLFq(xVlL-fmqJ>1V-B$V;y*o
zk(xl9$z>T`Ym9Xu-sX%0J6yrx(Wa{#h>K2F#S({?ax?|vrF?7^h>N)_BfJz@VA0`t
zPlz*I#o@Kg;T~Yo;cCL+YV2@N%He7|cDR~yI9}Idhg&#Y^sbfgu7!6;Xk@aGEb3=~
zM=qhQgnzA+f30Yfjs5GwzjZ93y+Edr>^i_=6kUlZx`=|r%Ki^5#?Y0Bp(_<bR}{lb
zI^CG_@^lS|m+~9Rz@o!``s0$whLdEe=HX4tM+3OaBeviGYes4S>q>H^YP1>MZ;UnI
zlglwOTIo?U(#f?AOjjMatC5;OEIU8y=Ep>1tOM^dQWJ<Xxh$g>jj;~I+njM=hbuU|
zyXoo%79Fl69InI;cc&b#v}1=WDTm_)hvMU<JS+iz&K&Lm79Fl89InO=_oN)ICLFG&
z9F7;9*x?oq7rkpGyldgze`#ddg2R9my27>+{<Tv6#S2dCUl;!EVF~R8GKFNp0T!d^
zN<`5`6eLy_9AGhqu0#x7sTkq~=Y#4fb6yq=AYRIX11vi1r#~(w7M#&K;m6s;Zvc-q
zQVn>zks81=l;ld)XfyhUG1h=9Kb9{oIU}P_DRl*|eZh3qfyWuC3B+=~7BHG?jCJ4}
zMrs0aCYNQjj=5F`;%&|tE;!^8FXf~GoMJ}1fw<^&RV;CMDY8JkloJjR7js!gcqy{L
zqQmim6FXeR;S<c^9$?YoYQo`a>~K%Y;c7c}xSDb}UT|WETR2?wu9fhvg?IO8WZ8nl
z0FPWkTM7SKDgWXHC-$!k|4y`o_5ztgvfu!VQFJAu=pqUdD+>;=7(-VghOSf$@q)9L
zI?9}vnF+*8S#W?whyC=&rNpe>U1!w-n_UgyUyala{FjnkT^emhANdWZly2ZQMrr_m
zWTbB3&y3UnUaO?s0;5|zBXG0F&1g9B<CyBi^s>d&3nU6>v7U&jH;$=JOg}cidV#o{
zt7C-M@&+q#X(7^jn~@JIdgCh&s}hIRlM1Vn2&*R!t3p@{EUX?Nfw-_LZAQc*OB|3u
zaz;iztmsid99EUEaM9_KH_g;5yp;18@OZr?i^#%$1~{2ZGr~`i2I5@KIB++$QvS&Y
z2_pYm3I7%){c9ys+DiEsFLH7Ix<daXE%H85<X#u<k(-Q<0oLWy2|yB+@r8N9=nXo>
z+;c$iy*BO2OgG#~yqQo}?mwGU4XMjC2wBPS6Q>f3+kdG7T$6<o$TX2%9Npfd|LtWg
zo(0jPEi(1E#oUbjir#}pDv{sSEK_CRx=NBqul;QXKI-`M)#}UL=95(ql~O$*iZbnC
zbBx#3`g~ip8^9Ng)D3)1Np3<m+KkqC!d86X#ztxYzh|Uw;AKW?0Ou<yx4`Hs&j?)i
zNi#ZdOm$*<++ykl5{0u^PsG$4$5bb#3(c=yATH<X7~!>SGr%Q<Nb7AzKCEb)iAz<9
z!|F+eRY`=^6NgnHtn)3b9w33Zuqtgv#3I`akU(-qMn0@)n~B4!5*98xUDY-typ)Xs
zI8hzWwiyOEnM*UmPmu=VT+TRfH?>m!wG#fd68<gfPx;qMq_matFWzS2{B?!?W!plW
zzAoG&HyIxTtjjh7BvHw22BVkj6m#1Qc#%!JGSdxr65Grk%Ke;4)sQ;JI<tnKIF;CD
z&Qb-qCff{<X(HPUy1hr;KoTT^K-Ml11hRGs7ZB%MOEnUcJKAQ*)IAn+GxjUmW{^tc
z_g|){3|z@(iEJ|r2sKd$v|sC0;S(xF3W%cQHZw+7>uI)XH-L8<sT=s9lJe{^`jck_
zzGtKc@P|*?z5u+&NDbi6l$2Xwbc<&Mj(XbEg$qL*Q=ORb&#AAs8R4RA0>E9=VzzKG
zz@1#05gxf$Dk_C@Ipe_oRYL#7?D_q%dllTncE-m5>+ZEh;7G<dupd?YxWVez2c+6A
zrfQoJKFT|*Kzx*i9C)?WuMddRxjIJp?OtoB%u>DPjEu;z*c~{m7GdF{)746pzlFn8
zT=KT4p8-zh(v0v^9)bXIE@vFrzpl_fF*`3&{x00ZcE-m5>+a=8T>khsEHHYO*4ph0
z;Pb8i<+#$(vG<e8eY;83kh;JsU&Bwblh_wdR0X&u3pbE9l;s`WR8vSm5+s5^78~IL
zve*b05a(RU8tu5tqJ4o(U2ZWqW51$(0jWfOe`|`$z~_~et=tR<HL)+C{amjKpHL}M
zKolkSh0(ga?rY0z1GvpIth~Lzhm@3Ok5Tit#t3|0NjW2<wVpLb;5|yp85zCk8G+|N
zm&@jijK(iDjlidrlru6K^1LwuuTxUa$mlN57*1$%wUgzl4n)f7ia()-;MYvB0=&aW
zJ-~O3Q~`e4O3?!(`Oadc&FEEQtN<rkb$Wo*BbR0L5leaxkb2~djQ-*ofz%^sWb_lQ
znQT<Rw~W*ST-9n=0q$a?9w3FxEij^%@?k+Bh0GZlQ6O=pQ&~^4yjOvg#OabB5mN@L
zBAXYG{O62}p0#RJfuts9WOTV@r3xfFIU^$yB&LBxm@_i+@kT#-MK?~fcw0as%(g=Y
z#3&mdkN`7227j<<TR`H=_!#`yf^7i_FXLlCfMOR&bQvE5A8M;pX_+*#WdiZqrLD{6
z4>Lp<flQB_kr9nAj6mYc85z;P!U&{!b4Er!jIK^$%-0ONAMONuvFO`K6dG0$8wLKA
z6{`<OMjRXXJzG5bfMme2fq!8uLLZRD!m)w>%PQXoBu$PD%uJRuACOu*Hn5L8IvOS-
zA5G+5tkEgYbiP;{1E%%mx3hpG$+3Z1b%hN`SdI-$Lkk;_upArM`x^aV7a{&gXR(NF
z4*rD|w+X~Q#|HkJ6}JhbppFfEn-#YSq@a!se6|(02_#y_2ENMn*Cvo?9UIt39-XZc
zk&h?xdoA*AAkm8U@!)qX@@^o}IyUfPi@Y01w2lot*COu*60KtcUu}_h1BuqLfqmrB
zS#&>*Zl+${Nghml%LX@9qx+Fw9UJ&ji@Y*HqdQKojtzW)MP3=H(e0vF#|A#iBCphR
z%l;3&IyUe%ww+ft)Ff2(>e#?%S%RWHn&Z+ptlP)8!PH)kye#e{%4rAxx3xx%nfdoZ
z9>E)1g39aw<kM*bKWA(;4ol?Iv4JnQJod0nkWa@3o@ROM;h=PLA>H8TjIGRGLfV}+
z@M2519g0Z1V*{UO>5iUsAFgR8?M@8Lf+$PZ#+pCU?%2S0>pj`37xXjlNW5bNf6_8}
z@Lmc@yki41Co_EvNWEhOGb?3I?eVatpWHh>Fw@fUfl0pO19Kc1QaS|4lI_?&tjVSz
zjs|9HcP7B>XpRre)OUPfmI22X?o$V9d48u?`6@Axy=iFa)KZvrVT%JjwG>7rY_Suk
zhCY<&11!2v`#(A3skfH=^|qa`Ol^zzZ~?Q$d!gOpz0iS_{hVjn?J4`*OWEgM%0Bn<
ztN%at$^8Fa5kkwZe&IhXKCES5{`a|;?KZZ}oyV*(CIN9X7UB+tTuV&L4;8+-#i-l?
zr<jB?X=5#<m1&dG5(b&#34?x4L&3XZOJFnAN`uiud?`KL>!gRQbtpS3Nk^)lgodgx
zsT;Mq^Rm@*B9MH<Iozln8c*?ralNM|sB|&UOk?yG@39tA+v2^!*dWg-8{A9T;9e&i
z>_hU)r>JHt5mLB;2#I}z{Xt0BWjz;CxQq*FAP=OhYZYhHM(xm{m?QKr<_O)5IYRGa
zjxd;*Ba98tvA&{er+Bq$w|COJ;t~xc75YE6Jy0IUMtdt+8%#f^csoT1@BgfqLa8{y
zfod2>xJ1u=E5E=a(FSmtk$Qn=|1Rfp4=WmNM(e(aY%lP3CFP8ae(f27XZ^mYk<omm
z%I8FZt(S_7jFu@?7=ahQTx4W)hf;+Rc+x9HMn<PART#q#BUgX7vvM7Xl#4fh<cr|0
zmeC4umXUgZi;PqO{=-N;K$7n)R;(0PX@TU0A8?E%y$47=a#=>tc}5`h$Qc>^)T+}1
zq#ijVqhqZ)72tJ7>H*$wqzdp~M(P1l$lL-WYAKI=ffO=lWJG}yk9?`l1WR%iNJ*To
zYMT*Nk>{?!Vl}E(jUQMwsz6ecYh-kU#ajiEot%*o2@=ylBFq^X`FNv8zQp?(i?;<N
z!t9YM17eg%uRy}h_!wMa(YAoZm+>(;%7SeH2`}SgK!9QwNOTz=10QPi$d^!=H1gaP
zh}SM{T~;w>h%f?~9yuc;8ebTJ#FsNNqJM=ENb}~5jC>f;BVWQeOfxJm4NG7z7CrJ!
zh>b$*J65bdAQ=(uqrkV>n$QO%1C9-RrmYBlKo$$f20q9t-v=a3jt$IAmPfuoYVFv-
zKJw_1ZzA&1MDE3+N4|*B`C@GhnAVpk9YB)g*ubp1!UiNP#|EaMg$+npjt%U6jUM^p
z>z8yEi`eGiGp)EyApSWv@by;QCXj+UHt<X<ZWBmB9UJ(IR@^3#XdN5)7~5Z)K%#YQ
zU>|w($Tt!Bcp{%|k#_@$R<w@?FS5wHfkf-rz_Tp!ZXnS*HgLrv?*<aBV*?*&k#_@$
z*0F(o<k2JF)ik<Y_39o6foX5q;I`A~4%e$=1D|V=SKd*$jYjF%z+D!3<sOCqDzJgK
zw8$%K>j7&|y*h2+6YTI+d0OFGnheJV-rEusJ@Vzaw7+%x_%@i@%OhVFcM|2agTG>}
zQDbJ_RLCRvx5idx2OytL8@O$GtUafYd^$Gp5tc_gDv?je2A*Jf?BSsFH6=MVaNF3*
z>?Ne#v4Lk<y6sRz+8rCXVd;)ex?5<PNxKsRvmnaSwN%+iyJG{Np?Q?8dO<()j>J1g
z@Eex!gICf_l6c1kW=>}M7?66$2tLdDbnYH=6|+S<J}}c#=H4D)lJEGy97p7lFOVhM
zvHezSLO~o2%+~HqfZ5R;ADF4{_`oa!jxT&6vZ3bvQoYJ!r!d3rxvhGJw?YaPQA>8T
z;wyMGQlH{I@`Z`wy)b~{y}{Ved6wOtvd_JgeeR{~b1%R8{}ca;6Y2SLgwz~Ozq}4F
z{Aon@gp@nyT7f13F%=6DFcTx@2p#!>s!x?Kl_H+V4@OVUvuwAN9`1G0LyU=z!(~{)
zAUA4r=SP;isBQ1oTV$-*VzeuM7w<6>Xp!Q*FqV`J;!fG%UdjgdI@w^&l;?-2X4w-`
zxY!AaHIB7RNZ4iV5mLD12x%Y>q#VKH$<U#gBlIuk2;Gf2LhoaaFqoJlj1AARzM^WU
zc(rQL<4Jf|T%u0iAQk#QwmncD<A---Z7}_u;_Vb6y#KRa3Z>!*2dZHl;T%1qTx4gJ
z2JlZtssUeBl6zs@h<43odbjk4{JEce5E!_Mk!rxzl{8pgb#$$2x|%?A4Rd~ljS;(5
zLDg!esvC%^m7J<ghlk3>tHc2&*#Q35NHyTgN=nCwu9^wffd?6>0W9i@pMatZZh4gg
ze93h60*kuh-+zd8RnfK7baexZy5gT^h;_Bl^@8c@0Ty*7zWSvCD3B*Uz-LWY6Ij$0
z|C~X;3jBv&SC_x+c6@2$54<n`HrRe#8~Qi&{#vD}rE`b7|2z-(zHP>%msObYEYU+G
zSiWOgqE`!-=&j538afO8O1+<QmV5tgW+G=X<5?nSVOeNeqAxh!qqiQ{>#C(GB}*&B
zr%gWc0(n@5m8Q<l*OS6rgy|X66FtRPrng4_(N-hig+{6Y=PN0nsf<Sc3EAi;xlq53
z>8b;fa=PM~8VgnsY@4ngAf7l~@<mi-z=AxLFC-;{vHB|NFEjPMKomOl)r9(}6=U@+
z)L(4syMaaZ@}1Af`{)5otiB8N7n%Ad5QWbBu7vuiCw5jHpVMnNk)ZFT_fr#zdtWer
zq87%?e#R1&2^Oy<+P8XD`Y^qQ&O$#@@28yQ-ZAD$<Sb^6HI}F<uy`$zvuCLExq4l0
z)<ZnsOhjHFe}S<?#e)Sc$%SFG&e$Do<~D#6l;pZ4KD;2fy$RNVD3Ir0z@n~ritnyE
z8m6lO{IZd1z@v>6eFZF5R7cTKCe;L@$ORZXHAMwxnqULC*hn?tT}m2Wnm(()&FF5U
zx)d_H$BP3WH%2GU=(nB`_~LSm(PE}|N4?5A+yL%xq#E#xO3Hm^R5z;gnbARB9C(B=
zI-eOG>luM3F2~r(=e13+4y+id2Z%p;%@|=JwZ3ag&?yH!V9nI`0#WGH%g+}ogI6D|
z@3WNU9MjbRK4YXB@Hr*r8E5o@QC-Fv{oacM-!ev*aYp~}jKHC<<@L|)gc--@OHHs2
zeBMYsK>W!w&Ik*MjE`31AJ(h9tO0Isq#AI7lH@OGO=~mwv=W`oMw?NumkvXkqPNb}
zt9wWZzR<+Vz#k|{zH~SwdRewaZ@zAF4d9=Zl=~GrF{?5pBYjt|5;*W2C8Y(IsoM2l
zw`mL9%1AZf&PvL4G5X72k*xv$rlhpdgkTuZA!-SQtCWUaxmW%YFUnfTJFGNn-hfPr
zpC!LtZFZ^iVspwqYV#XPEBIsgLf=C@nLnX|q?4hfI2p<%{C+*~TfSEuOGT&RP-=K-
z>cSE2P%x*45&k2v_<tiiw6tAh)zLzm!~dLj*_wKF^9;P1(q!8Y#c&nYqS$^W)<xXs
zYn@YXHEgBqh5ndFe;`-r54#t-6za)ZAykm;k5E#YA(Tt>$E3h->5o_{>W@$=uW=}t
z?vGH`)iab#)%a-jbedjW6~NzCTB637gS9BOz{DzeaQO;qe6F$=YP^IRBUh-g-3x0R
z>dDF(DoEBil$3&oa)}z3-pFUiSiQ;y0G$1%EziL3Dk)!{8NK{BV+_NI)z?vvl+zU-
zWn;lgN-%!16$@4ooNEE~0SUo5QEfBAwZv~^#Ohn9-^A26fhct9Tc-YJo)LJ5kq!o~
zr&-Nq8C5OUP2e%hF)}*NGXfu4j**cMHhMX>i#Em4dUf-ot@!5!+6L<Vrh0XJ;2kWo
zCU7Su4J(~Iq`jLCrg*+972d2@rw;s)x9GJpuxz9zu&$)c!m&D->iK{#8L13>!$?iw
zs+P(!kaEb3q6~eW3rH=53ph==vZY4xcTBPg{H~I+04~x26%%zpis#gYb8lm<(Ak#R
zUiuNS#0PY@QK|WQ%@^wDb@bQesG%3>MIDW&S~<71sX!jx-+?BPIOk|#OmmXn;nfhe
zko!Tfmo7&goX^-HZs!YD{x{-4FFMni3%OENW}e|yT2siiB27$BX|u%S3@fs?X16eZ
zJF(zm%6Ph1p{9^4rHPe-bu>>%xCo@vge*&ooEd;DDUy{h{l8&B2~#sfomx$=d4S9*
zmY;Okp{z@5D49xiC@1Mcj3rP=Kzxu4hR$!OPLu3H-dWODAO%ax#Ya>#Yk4{WAy?{*
z2@e~E_y|OK!bfhg(53B%T&blG9=0H{4@7ywKKl*JUfO=hm9h`dcl~aa*qFk~UKa0N
z1F@uxFH<%DGUE;4Z{FraPy;@pBzJUbv>E-$Glnjd-pO^WsaH7#182Q!>Vc;zDOb<v
zYR?#|U#PbpFqQ`JhJTuR;O$Dv)ic`XU&aV5D=BAW^o{q7v6CNnm>+fEBJ-mO#IjRg
zZ!_B0{AdEPoHH`=eniv#akc+BGungtF{ZvJRusL(^0X>C-4u1BXg^cb9V?37{`l<Q
z^Q86YReq8XMQ@ny`g5xDMbjO<dy1l7Q&fu;)ljrX=UO3kpPF}z8~C^8QWf~Tl7^K|
zh#wE)RF^r`021)f($oo<^bt33)JGh>Erjx>DX#!gE^p&38WC%+#M+}bjjE~@m8;5I
z`1V#aMC`2Y?3_E??uY%4GWey+1G7X=hVXg?#e>DCEi!br26m;Tt$wn$)wP}53#pq-
zs)iIc6Tc1!%YUk3*Gu4w%~%z9iIS4Nga_X5D(b)PRRMo&7N<@C`|zT#RiS*og;xP$
zFY6^NdR0;5%u^M}FV{E_Q94lLpbY**<HRgc<B)(AR0<X!SkyQ#Xkc$zj-q#XV0lAX
z<W=P2{5gN2-9sihPZaKEaMyq2t728J;oH;Dn|gm8nSUeYBQ)-R{xdY(Jy^LCUmwKK
zp2{K)FBsX$8?PsF_;i&&&rC(Rf#qCpYGGt5WJ=`bMwP$6vng2a^QIO@rb4EKsXt@F
zmi;IySW31HoP2L&Dr8ES+Dzql>}(2_9lWU}k*Sa=VQP-<eLu84umQZ=NHyRuj1(0;
zR#ZpP&rPZcL{WZN343gb3jDzY8^BM!&$+4z+*L{W0gTbdOT#j?avBGILP^6*GZyr>
z8GX{I?wHDGFE0-K;&P0PDnrdq7{(BFY;C=|1s^=d;-~`0DJfg<&roCE(W{dMpKrcZ
zf!|kBCi}}Fxl6C=)yab2P@1#?@b5~>en^$UNDC{fbg$aNMW-IT&|ItBqwp_!b!=gV
zUe*F{ZP}<%zgs<tn(S%HYh?drPXg|25%vPVYor=+jkoxsw@zMDr{1vcD+9fs-T1k+
zdC-lY`*;$5zGNPB1My^7Y5I^h1K&;2;rKh6khzwaGVmrN^#X4(Qgl?LFuyi8%Rp)+
zEB`BDtxEf742M`O&B+RVf}>X%+`3j--KH{(?0?d#FcCOGNf}@0L#S+&Qzk#^vwl0&
zxRU!O%lhq5s}~4WFH--QJc+Luz;cTvAo@PXq3Zo{7FYv#n~`e3+m)2BT#TOajKJS6
z$H?e4&j@^dIYvfn3}cOon*IM<-pR3Zd^^nE2={zp)A8*ve;IR_L72lFX3Sw0VGi?{
zF^3H_Ra5^py}I=d{2i-Qw3<=?Sc_smG_eZ7-CfWQe#o<hPN9LE$;%QT8uR80^C663
zHgd)=7l{`-*Qh@?n_p$%twyR~5ruLf1-@-u<<+!xeG5k!9IY(tTbDP0_Zg`hc)ybJ
zXu^DbLhpV~udW*4vMDPA>q^Q#4%E`Yu?1Ohzn7h(@QMN(c#&rV-lC-BCyqk(BjqFD
z+l@Q1G!}c`QE9o+pWO%p9;*!ZnB{1R*i3KTYb?<szeI0sKAeT70er|veZWVJ)Bp}&
z3Bf+#%1X)$#b~@|1a7+=Bcq*-u>t(^C^HhKAr7EU0K1vDH6VdF*XnIX#3C<CfjE;h
zGQuBW1kNtRP;WDO&SIzmCyvNt$Tc#WGm^+^z>)@=Gcp=yDX0PGFUQE}7S9NLbU8*w
zKB-aT#7V7?)Lks8y+G3F9F|{xR|YaEs{@cU=8TNUq%Z<WW6sFvcb3#%AZg4Q8IehO
zdlpC<b4EsFQW$}xF=u2%CWR458goWQKB>`b%s8o4l1k2;u4*EK)l>$n?Kp!~GKe3!
z#!i!FV=HD0_=b_1K+HH@t%T7QMtcf#q19$I)ymccQqf!^qs5*PNJVo-MqkybB(D&G
zR5WK~G}7im6Ie`4{Qar8MqQ~I$?vmAUEY<dQCA{eU8Jk%O8oUJT-jGGxeX1Bb=ks!
z6w9$uFe>0^U`imO<o)-WcgF@^-%?xyeppFzh8oh|MhBEc)B#V?TTUH#k*TW#?=Vsm
z_;V%6D~KWOXBpTESs$iwjFFnaj~S^B+|Ni&;E_hE16d%Xmw<GSJ0NKT(LFAv4VAXJ
zUL6g*t2JB=_&FtIX{+mibwx}7Sy#jnAd8A~B>4Asv~QQgM$LYpiuRy6<>>*CF3I>9
z(3`>sq)juvz>N}qlx*(yu;FjzNoYq^;v?j=_9R<>D(5P^!jnStNOdX8@KV2gm=?kB
zEl2Gndz`7S0l%gssad~iBb8E(#ANEE-AgrNspvRHJUAk@fXn&Bz-u3jPk+~nFr)Kq
z-L8{@UeBpv59ZV`%21>D8>$mhs7pwpqDytQvlQg!n77Q#oEpxUv}<T&y^%8;5F;|x
zu)uO4TtJKpS7=d4VF0WE`{@kNoeJlB+Erv(DkMLlU?OG9)Y4~H<_OsUK4+vJ;8G>I
z4O0Hjl#cdY#bkkJ7^w&NA4<w)!^rWY*NoPIm~n#ghqIMoRkKhB7A?ddX2rt#W}yQ7
zt&zHcm~locX7o?SSOI?B{OAT2{fIvsyRqsRr&qV~gSRvNW#A4<%3c8eR0lT}WWo1)
zS>QuOSt5F^APauS%L4ydl%=ebHCX2}c#0*U3_MXu$rr7Msp9^EAK-JnAHegAew0-4
zh6O*s6TBb5omb??2vz*F*=&-cS*9wQp&wB0pP5t*sj5ju3)39E^@_1Hfa|Wx<Y@vo
zP*Ofq7=6(g8^CWHsR<nO0n|kgey>&IOZ4iB2wrNAm4PoRDJvpQY^xTXEco;0MH#ra
zk}}y#b#O~T_FlzH^(rR?;EPJiWS4PY!*X_qC|=t_EdxKKq-5jG(}31NXQ@V5K5J3w
zE?24bO{zPVik{Y^uxUOvfY+=+g=)Z?l#~~o(ah1t7_OE(sM-xyhouL2x$2Tr$D$Fa
zTHRFj0MVZDh5C12E!TU4UZp(1+mtl2H1*!nh?7cVAv~ftR_4YhH^Tg{EaWoqB_){}
z2yc2`ATAqk=<f5HhQm#^0{nrJRxX_=kH)6nJ7UCG)NNu(sQ`CUQh^)N-kuxS9dkn>
zyNS2i_tM&(s#n+2U~(u22q1NJY~+&&ge^=3+Gzv%%~_<pPtniIuBWvdXZ4;8{GyW9
zEzOh^j-6IoXPpf<-stV|Z*TF|hql<T&F!0%Z9VrZ66X$2aKd4MiMot;?6w<)T5r^X
z{@nU|GH`1x=(?qoN+-wuaGScXCv3Tu&<1HCnVmg{V{NT>2h-f92RfUEgNk%e7}j<x
z%`q92UnP4`6Lg$){zT$>+>@AF?|2e1ZE0;ikxt*wlR`%mzk`NT6RdS60qKqPq|#EV
znQ^t?pf@(mIXe_A2>-*@K%zYEAyV_C_0S|BO`?vS8aoCDZM32M^$_Xl4vuMB$>|EC
zfQfGD>;oJWVTC@xL`!v68+H|Iz3-~Ytf#)KCeu}qSPM<2t3GP|GMN^_iy@`Sp}9#~
z#lx*%4*QnEIYyeamYVsXnLBJZg`YRlq)_*Ms^BP7bU1K|l7`Ah7sG#H24qW&*V>HO
zNs|7R1sO;>hm}t3AI!qts}7#5SN9`txw&P#k^Fl!^y0nHCG?QzA*I7YmQ&RPE(!~^
zq&pK*vjeH36QOwVUT7UXR6X%d`v{G511t66z%fc1DsOd#|7cq#ZT>NPLZT<rAHH8*
z^?p>Z#R$-_fi`;aUg!gQXy8O5w^S2;b{)pNVHPK(J`+e4oe0H?_d*BJ!*oidf0o91
z?pmBK8^H6Fl%G@?t+S3XhUUJhx)$iw9b>?otch3^*iw>wkg31TV3OwpeoIMC9r)Yk
zZZ&o+`jqP#>d+~syagoKp{0{1py{J)<7*2@)Q+uF*r<}9P(u@9LJFmRtr0w@S6|1F
zvs8ljHx~6o;8RK(DqB|gFU;wROu*Z1DVhj8)bbyjctbNwt>hOv#<z){<YmOrIV9-@
z`Qs2kuS^;vR3?0(s-4uq&zqxP3t39=?B?SGda*dZS(;w@ZAB@B_Y7Yp;OrGt>3#JG
z6T?d<$rEXEM+fE9Fsx8kk`oFh^3_srCa%CMc)EQsKra@j`D$gqVFjw;xyGvoda+pb
zTgv{PsqW14cdr`g#o{WjtP_4My$&tKs^MAP7Ke#IFBYrbSJ|tkx--uKUNz8*#U9O7
z_8;hVMZF2{dENx@0wtwZkQ?>horB=H)vE@2vDo{Ul>N^uPz}!?y=tHri&c-&Eoxi8
zRRJ$HQaA7iO3Jr6Mpt-7;E$JM45wmjw2slDVEn8)7ObQM<A=+!U=_g&&HEaVz+9Ns
zHls^CBe3Xg{M)0k(H2H8GF?p|W}MNM8NJjq0*glDH`za@0w?O#odm-CFVuVE)@Ac<
z02@lmE6iyAdd3L6K}k6yqp2Gh<G@DiDWml^Mt@=6)q(dHysNhvjrb5o>%fE8&q>a^
zdYjQ?rOL|@;3n#D&d6wQ3!@GsvYc_?;3}!$Di$1Rlm}rza_B5n+KjN9cw!cpv6?bk
zO&G1Fj8+pyqXWd<+B;7c`U{M8c`Qm17g`a^K+5Ji6HMJi5{QRVEOJ2|g#@IIA_$!8
ztK3#dT}2Q`T}2S+Q(X`1ds{SP4kp68Ey9C=MCDGvV8W8WssSWI#}<a@XmN}O6C+VL
zHZWm`1t1YQwlE|LSvLQhgoIe)omfi!t|lZquYN#Ptf$wEh%(Ba5y{3IMNS5re$eLe
zZVJ!SYbMO#UeEW8LQ<OX#d));e@-WM#>aqcNq2-vBHUq?q%x48<vo@DHiN_rxWJ17
zeM&@>ln{b6WE?;_E^_pN-@*DyVem!_0{@?JY^r9!YnK;3RJdNOaF~81@voDsp)%nM
zRgvB=>SmF@NcgIbcCg*dy`W%pv2A;maDL|EOh=ZSoXV<mvu#37>5I`?RH5;XvrWh`
zGV-%9`u-@v9OQRCAQ5HH*BG$Uq}S3g3gIFI+D=F~>)ncG%L?2&OFhlpV&L<eTH^oL
z+<H>`kM$+Fm}JC3EhiZ#i6JW)G3>d}u(V~ohLuC3!u9Vh0M7?;{@<7jLrJXBaVUvh
zIqH#EQ7R{i1cV-aSQieeDlK}ek_sC{tf)SSWk1$kj&-N16zh%_qJLX=n1C4?Ep<q?
zV91nM8Sl`kx%zTz;C8xPk-+prl~7iS5DE$@^!zTh_?Tb51|hY+`p2rV{9?<5%&AOE
z7OS-Pp+<4_-&^=LQP+;K4I`x}Oat4Dm#QQ-iGii+u~d}u!-M3>%ZW8~*CBI48pPHU
zE2_te>an7Z9{K1~io)oZY8>=SUf6J}NS8a5m2!uIiDrF4EiUu5T~p@<P9&seX>z}*
zSNYK%;3Kx%yAKP70V4Qw<&r-cR{|p`C;U!g7;!JIz=+G-2R@+MlOG6Lwo!f#e^0OC
zJ8+GUa76C~uB9aRvNa9|T_620f^{H*^3mrx{b4-F?Q3!s;A-YW4-i%POXq=gRnfJo
z>FNd+b;ZwgV!vAGLQvvaFo<}%5_0n*xr;RZd-dw-di=(DF7MWB>JiU$y?4FwOeukp
z8(x~bsDA{bUl%yR&v@E&@bgN`r5U~9Im7gqHd5c$v}E^%Ji90hIgt(r7Sj6tC_QJZ
z*=zJVQr^bie%#d3SO~<thP*F&?(k7##*G^D(Tsr`guiB%jOAFkMdYR&!i#D(A<HfD
zVzOc6KS)L(pWIk+1i~;-1j3SzKyDBL?u8KuOX%nrt={QY#RhQE$C%u`z?+q{vZQt9
zh*es{Hflp!Hr}`gwo%6hQZl<<nK`grB_TIEYM(f!YC<kvonpBba*M|hT@85XHaW?y
zPOUbhT|Z%rKw``p2M)3eqkmDOlAI+o8DPk1>x#@~tSQ6sm!<xq6(6Zq+<F6E!@?~C
z*EVP5j{$s02iq28!3TO-V6`aQ)WO7pEO?5S1)f-xovnkp1zGR~UKV(9QFinud1cnq
ztL&S=jg*w_n`IreJRh*T$Tv|3U(u_}A9$LV1x_!@jvAZ$vbJ8G?4^pYDbT?GQkuM#
zX>ikAMfM{oC+px!y*hn6kINbM)vKd{Cn`;H0Q{1YvK)L_2VX77g6Dc!;CV&aD|B#A
zK^FY1mj%91lzmSJqc+QZcRqv1C{26@j#E<R^D#O&r63FbhL^oc;cW#r@V%Z5_<)i!
z6H`8#yLE<MoqOQ9N)z|~3bX~ZJVfD2dd<|Gse_9>AMlbQA4}|uN_1wyfAzA!zZGTQ
z*TKg(&s`Vy<|w?+NLApkl$15q4O`?Q59-yKU8?xi0_|PJD{pBgfU7Af3-?<(__1D{
zzH1dfs8?}s<i~S0<Mry;{zLKn0&Rz_bG~=<>iAe3FVwZ!(ZD}cnwS7yp`^^c>viyg
z=K~JaMct_buVxE<8MvmBGIi_gfW=tM0>4>MH$}J2vuyh;1HY-HOkKEbu!8JtD?<ag
zyOQL|!I1XdI#{At7s8{8f3H_@?M7WV9xf~(&nSLNufj&_%-23~+OAT3i?vhvK&@uW
zR~6W;q%5e3I-oP0x{{W-u2=cs5U{1BVWm@sw8v=KH`l9p2i!_YnT^p}&JBE7fg39+
znP9fFy>t(;Uz-RsI<j<veAZq*RU0mHCc#`~F3L;tVVW#IEh}MebG@gO#)fkw*DD5@
zG>>Z<^WVLY8?-zvEe3DWSZQe?(aChGkUktWZS;_u#$)RmDIYn{!VlZU+@ycr*FG{g
zndw_vsmnl0J+gF4t{uX?$}2bIH$EiRbGOQufh+WCH#XOZKc!c@5l*E`wZebVtLsAW
z{koLPxoYuD)?rqejL4+E$tJ&Weov<^N570Sw0npiz$V(!yA0$6=-%N27j!L)j>_n|
z#<qhd-XfQH+i#{?KFxao#HuVJbA~WLm4x#^<O0e_oqS=LM3JM!cbLyo)8F(i0KKB_
zSW)yHq;UH$ein#hz(uydPX=CWq>16~E>SL-$@V^<J-sv;_#-1t1kx_@N$_y@Ec)|`
zE>LV;nWY)pK<o}hfv|N5h;xmbYuk@IzBCpBVY#`M-3T=h7snRU0D+J?Y6w@g;iZ}K
zHdnZ?<<!u6D4SjpL#dJRq8YOMM$aN2w4EpVO_9y?7MqX!KFNAo4~`T<3Kf4<*B#a$
zVPS!xITKPx=ybf?re&l!T&wehh0Aq1jiGFMoex8~E#3-3mT+qrR+>4!9c~U8aUjXT
zZ7Q1#yzj8~Y`!h0%m#3OBb9;Q_+-u{GwYpbr9#!WO|A|s$ej^AS3qu($yE$)Z}#q;
zIYbAm=v5vh0}J}^?daELroU>?>tB-4-%99T(osLb%D3eVdi_r%^mirnKQRdXT?ze5
z6Z$I&{YyLQC%^6?Iv54=3K>YQ9b5R1Qcl?}ubc3U0&i+%>t0ve=<9kNQi}GSgr?E4
zrW%_1O;hyUTQpr~KFPO#)fdFw*#x|)sfyl>{FJKbDyRY<sWdq-1CLQsb`Zw-lp8Pk
zHd#C~^h4&_;;3e;ssu@O0f2Y3L{~nm@F2Z9Ht=ztZJNR}3vA#Yc(%(G-dbP-Kj_)Q
zrr1vBoi%Og5Fp8r7~hOyEUVnXe%=C+AL-Vi5t}X=0imEUe(;IH>r7!Ukfs(#W<?7W
za<i;6qHpxWa;&NrJ@ILBuHHP??3aOOnCj>+BjDmqUJ7`oN%e+FU#i?Mdnw>-lj;kl
zUQ_PBdMV(yteF$-I7d}3GSv;>Cnm75G=US8<es%Q+Ki6;v@wQm#p>&*U$BGi5X)vV
zK&3NKHv@;Lfvh$Ru$r@WGW;{MGU}i<gC$Co84ujig6+eHkDFByWw5iw+6TlT`HO%D
zwHe@D;?LK8T}_><S2wl6mzo>phZX*zzy|)8u~qP5FLlgmJ5VvsI9iyXt5pSN91nOi
z3$hFKs7!oqX{2UzBfUCJ;4OTnfm<snTU(#j!Mg=n@G3jfR^<&8ZlPCa0=$!Ft16u6
zNhc_rrB^2j{!h;qCf2==wcR-Rc>^-NzNSQO$jfZGL2fCuQyk{_zjac*^eG~VnoIhF
z6z^|Y?<3n7Np#Wms(Tx2g&K{EONrn6xkaV!wCP?39&JwcqR5+%R;kBS!P8z5@OMg*
zUtn7_g2A3v^|`bAL;W(Z<2S9IQ#ELi$qKwh3ny}~%tRScXT-1)D0|mR-i_Q*mcVY}
zMOET-^AXL`Sj$!!h;DIUR)44}BZgjmL6scfl>rON-kF(D79ZlN-xRM5h%$LW7OG0T
zMmtXxeBUbqqDZ1SKNaHn(GGmID!Ilh1Af7`MEuqxQ)X9XO3Z+8?I=DUU=6v{>ODD(
zyvQCFhJ|FYiAP^HAFa36F_!3y);PgJmOoOEM|2Qa*78RRawGKPv#Na0&PKvzFgNnG
z(pc6-V#$rjjnK%Es{EMFM$o~+nj4YFD-c*_b0cyiG%`n(pWE3;xGd*J<YyyTr!Q35
z+=$!=jWCt_ZAzDcrx~dVq#+WsoQ;req1Y?X*0$w(5-T`+aFIRqG2H!&yqp5qo8_rl
zxDyoFL)BB%qb2591NcWH9Sr=>o$dU!u%E#lO3V*<jGj`ed_oF1c^8wO-Opf(CoYUu
z0=&b%JOlt@U6z$wlCIR-u`Bh2D|1q=+!DF+w8pc!`s?;?@IFeD=QLq{yLZZCo}yPL
z2A-xg*`<Kfm6RQH?$^OH1zGSPye#mwqU^q($?cx1S7#Ue4W)_Q3lv^nU<2Re*&a~%
zv?u*Z;lK3i41ia$+LyyJJGL-K-_}N>w30?(p~{Q;<9b#S_56OSo{OS-&R3&9wk|IN
zx3t{$0t<e8-~1pp*$;t*ZjL{Zg~^3#Qd&1Y!oYNc<*-KFC{LWxkYeu2I}VFRFd&)3
zN~bOw!Qgi5mbvpRBGQ?OGaxo`H~tVFhUvHL1r3A67DFE~cxCd!hk*~QFYJxiHCKP}
zWdc$YS&L&u(IckO8t>Xx+%nKB>WLLauh40UPk2Q@uPADW#F^;;RZ#befGCobCzq%A
z?87?K>y-iTG14Kx9dy#BORD_Wm|O+8yUqWoP%Kstnp7D`H1g(IoZsl-3O4pMTV)`9
zl{naOi#4ve=Rw=l+HrE|N0B{jK^DKyn)sxU<!tr!9P>Fk*x|%Cl*Nui5Lk*k9)}jI
z<lUW(giCjBM9y!l<;0R3NgV8+SLMI&Yy=%_2Dy<LrLhp$8ge6YBTUz5&Ez_k(I{Qv
zR+1Z$^V=#avw>c7BZ-3@8#bku-7fUw9n~>T_wSMN>nGcfJE1fd0vo*halh@yow{)3
zh_R4%v~$1-CGcnL95Cs#3OO1SG=yHT`rT&I4d4Zz&3AE`IlxPmG|D|F9x-axMr|n9
z?QhJTOk8dz>OiDqDR^^6C%KJxMRf)E&KGi$$UQTY!Mgk9ME8?f;5kam7#aMezz80x
z{%4E~)>ER>2tJ|6$l&w>Be)RTGtmQQ(oR6~rU$SfH*XMft%Th9gOKY=$SsWI#(X}{
z=@xo*Cn)e9N|W9O?xUpa1bK@N9xKR#U-h!UWkuQV?UCF4iC&%EOZLoZH|f>Uzz-=+
zd;vb9q|BE?_sV5w=+()B&sLhq0?$!WCfl=5E_;Groh<lFrHSl?3a==zfp7C{BlgX;
zZLU|R4ZNGuL>ut)O3I_q!B;#VaB7inp|&Hf<ZOhq>j%5#eq5nf#{*uZH1Pv?i;^-w
z7VBV1K^FXkmjymkls#?t-0pn6I=kSXDoyO(q42Q+8~Az8_9nM&ttzjra5KF+1K{mE
z+j|OE|6J~mV*{_JH1Pwtk&?3b$LL^(f-HC+FALniDElWJyi<@3XO!D-wsRi_38#zQ
z2>ae{giUQXzA=cN7@;fe!DLl(0zTR&9mv`*Yv$~k42aL=6MU03r!X?O+lZrfX)~Z>
zGnHY>;vOs4(9+2hzy-l&(N2P3-O69X-REs))-G3vP?dP4gQ8V5QR3#DXw8HP*RsdT
z3{WnT@!Ri6UTl8$;3sm44`8jX&a7qoWCMs*abeC#2B;DjUX4<<pDH=fl$C*(m{)zV
zSJ9FDtEynSR|LGs6m`dnqSsFIRl${B5wH-+J5eM*RsKa@4p@+z6_qbspo*t+7AoIJ
zljw2gbCDg|Xxr^3F%9SIbWHr5A#2(fEKQSv-tJ+s?!(YM*K|(~r_dg{6d!G?bOpG(
zPH<_9B{Lt=)rsgbVyNtRRdTW^D+3G4W~Y?Rj?(^)D)Evhk8j6r2r+a6$zgtl0(xa~
zPDI(G8k?-{z(SbuS?%xY8A?U&-&M6lOZtSBwhTn2WI9gHk?Jx`!%HX2C-uTaz;%KT
z5O}SUMEf%{8QkvqfEdX5Lj6Z6KT0JL|E{VVv{pA;&7v1!%ry=LZYRq=2nm5Bg4{@d
zLh=<={%U6<;gQ0OaK>OVb~LiGmU1;KX%q=MhAK;Lq`x#40!NnIh};M>KS3pT?rbDH
z;^aoAm&S7DVeQC`$c=CXu^XZFzP<A!(g}K%U)2U)X{1TOCyg{2_==Gx0Y7N_?quL*
zMw$ey8EG=`FGiZghRxwY)>7cZx|b(@0DN_QU9hQt$pGBNNOj=eN*Y@FY6*;TnIv;&
zzO9y4)|aF*DS%%$Qt!|7B_~UCCK~2>b7g{AcI|kzgNYZeVlUJ)UiGYiWsbVGp|U1T
z!NiN{gsJmY&k9(|`dW&YVENibj2r7-m0r<8S@8ftUXLf7ss|PaC$+aW=|OtUJ;FZ<
zm-aK16^jV+I3VHYeu2foNiEily<)q31K3x|C#sxWfm4(u-}hZGW5g=s<@>&n9x?7F
z@adQv(pxRl4cxLTC&|;Gg)>Hs8oyB+%HDcM9vuOf9b)ppcZ}2oUeld(72JjNu^v+Z
z+)7E_fp992tdAXla2<%SOvS~~;Ssq7CRYI#<d#kEWDjO}t^+K{y)y{8RzmLlLHOB~
zkee6D?eoPvAE)Tmy;=j$SDLgVaDkGtSHC-zbJ-*G>SV!ZD@|m9=O`(Y-9#IV9(1yQ
zP}+y8xn4&D@1Zo&2i!+VnZ94^;6t^7tm4h}Dzf`4Jj#=R$0;dO_iY{AtXJm>_>W!|
z_<B)xpGNM&(Ry{V(-fbnS8?Tg3U4d0f#3FQ!<)IbkL%TG1MjIc(RPT!|17Y9ANOp)
zr<IfiyHp3G4)VDJ@1`^{0sOp@@<i$27X?}H6DC`EPvMrjc(^FQd)R_f{(-{9o&>yG
zNtxA)bXob8UY$Dd-)+e$1K(3pCVSe!d6d`d)yaZiRGLH!d__r_EKAVsx;i=84R!5d
zEpoJQHF);G+~!~O>Uh8-{45)-aC5ynHt?>VE!56@Jy%)Wi7$I<#ANK(nP)eq>rA^$
zukwioCffEok&Kra#nh4a#mn06U$>c1VRAg7BohxiKffXGZZH+j)n1ny^7%x$!6f*-
zzmb2UF>HHN#XRWJMmAMpC*~iHlsBAc&DXUr^AAVL4VscplxQ#2&ikb`WhLI;>a^c0
z+H1LU^5~A<dPEz^ZiHnF8^Xe)Lss|o%`+ADhDp#Ydxa9A&7E{xV;PpGN<j8|*%t4e
z5eA$Q+0mJeBFv7?M9KIVFjEqbuD+*!uxH5x1n#4AMvf4`nKqy2J}{ku_o4|b?6eO=
zJ1r)^Z##Pfh-vxLDX)xRP*8Snq->rlneUYWkr!oiMlyKcR<pV1PbayfbXAe0oj;wV
zv8-gouzXF;&B2zVGH_E{2D{NursSgv-_yHfO7aaP&cuhyCaalay*c3iwvse~g$A0F
zYM?nYXdv%cGtoeEW{?ZtK+*g4=c!8<c-Mefkqj=KexwF|jHRax#EXnC%tl5G(?4I0
zE$~Kx1*5M_KShmUA(3s-|4P(VSF7Hidi}tH{)N*M&Mu6UvF3Bc&tJeWDimu$RIHqp
z(r`r2sf?(R1hODHWn#Oq-_Vo<B3n?H-qW?G&bHpF4?=1S-M|jeYw<a3XaU}>l~<k;
z0R29Af;?@KKcN-MhWldv=Xt`dG5tq-LfQ158d|LD-g<)Hc0-n5>8&^Px}xCW{k5r^
z2xJ%kp!^`h_U=0ae;c)hra{m~(fQ^seE~;{tWp|Q+Ix8B+<?6DS*>{kGRfi}g<8+g
zR3r76AT~#Tp@Yp|1-wa{AydL;csj_d8y*nSH4kOeCyOx3AH|1{kmXrjU54oaV?{NG
z_XS)2CIUIQ47TRKRCBKl!YsTGDzD5lpg$IMm}f_8Z_#5?=e0ZaR>+&IbI6pa^LRbe
zA7GCf8o)8VdE3b$2Dpim@{b;e<%xCG(Nz?T|CUB9SV;-Se@i14tfmCx-%gALTL^As
zVK;$A@8dsn5$ozg*T$x+8(7p8|GwFQT83$Qb+3!S7g~wRz-yH>Ouq5dzFr4U7G%MH
z_p-os)VEAFEakq+zo7*Xee?G!Wm#ydCw)M<N1Igi$-Q~XJ>R4b1LFUX(%~glFx(U!
z2K=CrjtHGTQ+0mNbWZ|eR{m-m5OeZULf%VB{L0`<s^)KA9T5NAAAwVU@GDm*?=^(p
zjaSBL7C-~IK_4@%8@P#*@|niy5zjcV`iZFjph~$1`d~E5A!cDe1Jpal@L%kH9sP)j
zuK07`v0NpTb1^37c9gGbN-#0EBf(ZmFfoxM!LF2GJZmph!Q1ufW-U4GvE;`OOD@oR
zH(6Pu`ErupI^9?%14;X;@=-neW4~e9zM?LTG5J0`Lq758-cwZSO1;W49;sbSQS>GZ
z;Xk5ME0w0o^O<NO!1kCakD}(*?@iOmDNV4wZknPuA}>>S7wFabBu`$#G=*%5H_@(D
z4HRo7`GdI$O(9$2(+-Od&8PcodX=y209TpFO3(zZsib@*V6>BG1b%TjMn*>+W*Wnw
zV?XNnfs`{E@7J+lB_$ZoX#^j($g9Ai7EldHtj>vQo6)B|Bam2gMn)f*MC3K#TT03q
z84+ya(@!}3s5#sM60yAB0Ys5gA1^QHf?K|(4J-=A%gc*uoV>{M3X-w2C8L|LCwmeB
zj-E^=y2Bo5DL1V0y$QH+D)Ci{SgILIMXzIE8LRHNWfJ@;bFmEkw34#t(OufcIOSwJ
z#d4O$<UTAH+dO%~bE^1v=EunsV*Mvaf2N4ce$!_d_+2GsAADnQqvr$ORpevfbKi}-
zAFx^54O~x2nF$78@O(fb$@mzMw#1{Z6V#>C^}1?ldg%l2%fAh_AJ>LRO68>-kSLeq
z4@-h_m=nDO3wp)xc_TtSl#I9&uViAD-cSo(_=NZDsy%?Tyx75p&sopFikh*aW~?Zh
z5@j_%$y#xI8+?+|Wb*}{qNHp}d}$C<f`~7)R;=RHIBT8g533<{nMrjM-s$F4H;O8z
zs5@-8)GS5mwW)rCxX3J5fQwW7VPQ~ym&r%9g9W|PZwsP+BQ^p`^jpjlnZgUUAK74l
zv|zpykyYP?wXnL0%?b7B5+`Ft%~(;?h3n|HxQSk~^Q+<=^eXm&dnies^ABkspabtj
z^hyywpRl4-@$(XErz-GDCCRT_?GL`zv&G>=?TbC{QaAQ|g1ciy(dI}#2t0odd{Yo{
zP7OOTlpR{yz8y+B+CXxW_fRa+3cm<^8U)LVj8jzifbGc{PrbF|uea?qKQK|e7X|_E
zwJMW1PEXNQ<NLOHG=Mi4sRq1VNp89{+Kis}jKH^-V`S8OI0v8_@NgyN8X5iR2xAOm
ziXE=wFj7ued|HkLD=ESFw2a_%3$qHm$w=Kml9I0|j9&DNK$4O(GMZ>9>IRaOoRJa1
zCXNy~+;2u(Kq8jYG7v>heSBI*7u<4M1{MY5)ABiLe4#nfBpEMw5@E0PB?{goz}c3G
zXrUaX+&h_64-jeh!+mJDQ#Co7Ve7D-p$fOrt78Lqdp6*c1vc<Yo()KLoVM^E{xb<=
znE>Lyt45<u1Sm_aCj|7Q1v$PAe$7%?2L44!+0g~}QJ*>|;OnO}*F#HFC&V_VP9S63
zS$d{Ui1km6PV-cr0?O_Q++9t|2DfMggCnfwP2llGJ_dajdy_0uK`{Y5LcPsQFu2dE
z*#r_v#>arPB~B%qs7u@GbyayvVW)YBq*PAxK%!iZKWrbAlP5VW1CdX49b(v$SmF~t
zWh57gj)R39ig_SXDf77YB`ZuXkfs;0Q51_Flb0%QsRV|Fgdtyo=v4;xm{|i(GH<aK
zTdc(vqXQPMU!wlH=>)#cqAmk(R8lq-<7Fd$VNFE)+7#$%YpXtLEgPMFk*b=MO$Z-x
zlZc0#q9(eRcv9H;sRaW&lTxh@@o;md0z5XwAGSWqndONWvaq06+B|+?L2Lw+X!DpQ
zYB9VRu*Jx#?_ldE%!mhJ4<wv=Uz4;|p~b*o^eqOg6<aLKCEiaqxt`e19{S$<*%SL2
zH66MOt8lEl8S8Gwx})9yMa{u`)<e-F@VR=c5Ly^LECmlcu?)h{_tl`x<uPprUak?U
zUZ!(u?2(*%8o=$0R0Hm;r2J68=s3>^oW2|*qhB43k!Z<^{ix#yQqE|6nvMl4DZ%(O
zjo?3RRjC4Zw19em#Oj==wi!+Jj6h<|85t3SY*C#m@V@!c0uqQEl7T34>f=K)F~Kc|
zWMEM+J|v&^<-9WI=~bRF0WVNewmU4;!L6PTct?>h403f%)W<Dh-Q@6iPa<27n)dFe
zj>;X|!CEJJRlZK7yNT|@O?MOBA2Qv|FDksxlVWqxtlu;;SH|2vX08mx19>m>mVSJ=
z)blM+c!MWBsPGX_!s#u{(=w1C<m?Bmd#-5;sjt+PXy5f*ME#;BTeyHDEbo25b&OO7
z`s(xoYo6<Hg(rGaTj6D%6t#@{x0d%4NT9Om5)B~;2@mM_#Mb=3B~^=UMJwC!n$!#R
z>W(Vl+kL`;cPS~GZedQC7(UwW*n?<-Jf#`*Qq5Q@>hq>LGR6BCx-(2~-D51#$s}fp
z+>2QvUtk%8FRQCB*#^kK%S9U?zCUJiQ_mm4z-KJl*_UYNu6hi6S_8Ptu{p`D(ea<1
zjRor|!FZ>O1uH4Rc&Cd6t0}?wlM@KOYqPWkJkG-I1AawG`PRhf63+<y(Q=H8$Xep#
z2l(-(`OyVD%GCD&vF!ZlYBRdnGXk-kGcxjiL|gSA)qc0>s-b?4C!y#uPx`w;uRB^M
zMk~v9CRGORXrw0aAS0E5-l-<=FwcdbCwdalyWYh0YdsgxN7Dp8<GFwYDz6xT!*w>u
zrUKl|NKN2YMk)h+vZD21cjf+>Ni~4aC@G(Oj7~d+Sy}^r%Sa924NA&o89hARWP!g?
zQqCAIi?MfgynEan?g8SW(^YRXde<21z_X9Tj~?Lnj8q4H>}2Btwv1E<j+|y(z|R}0
z4*aH(dVtp$sSYHea<&DMQ2A&(kd!4(z;V7R<coB;<Wv&Lsl>_YBRLORaw<SFlB;91
zqvfXp{L@$S{N#*`NR>P%1Cp1Vk<m{qOBLYJ)^2@3vX#p+B4J_#cuJw&Ds4s=T3_}7
zNph}n;J&P;@?1^ixf<uWH_mf4mFH@kJkxygat26dox{~OqpdA{RUnzo85u3`j6jkr
zr#>K=&1D&pT=51-W^+bHBv&*7i+PT>&A6XisXVt5d2WSycCNJAajILXRJRhTZlzM)
zvQ%$o8EgSbZf=^<Wu6guvyl!4zNRGiU6|<%$at=h5lNRn-~=qDHGXj$r*&eO))A#Q
zhP}1s7Ky}83=<1$oZ5+`c70tJvS1j{%*Zn)&GObJkao`G8O*SxO$3tSjE?~s6@5VZ
z8Pkl(XeQ5qoQgb<hR*mH(9$Sn%vP5%GoU~)hbzo`y5z3=m3;YhuXVtiD^1P;z^#;&
zz1EqhgF6ed;9q;$Hx!PT!VXp*qi`Efnx*i1y*fw04|%p%6u#|A!;jDPeoC)S@=nG7
zDA3M2A?LeUuZ|CVkJ2Pm;FC&{Q2W~qp7wlU+MT*EyQk@yfUcL9{Opf!*m0o=q~YEB
zWHkAo%wQ8ZVrm|T)Oq1_nt3<d5t~4o+Nq<L*#yNA;EvkfjxB6uN8?bn_o)T5VepCf
zv1UiHcN04uFK<AjoqkQcCw+FL=nxc<u_2{HNEl0jkbrw=14_acTf)eW@R1y5w~)d_
z2q~;>*!{(?GBFF5+ANq1WH!hw2kvCOJo!dFA&`!u($E3KN1OOzz|D*_5jep}hXK9a
ziJ|UrHWvM0Rz77KzXW{UNE3lWbp{S8eF?~fl@u)0c!|4cC-ez1=6R798uMPhfpWR3
z#zc`lRE-$RMUfX8%g2yX^tBaM1!m^x(wke1!p2+cZLwjSHvgecqL_m_zf+F2i#p~$
zF<rjPxM8%ugj2(<LClSZb!B%n@S}FXE(2LW<V8G?>FFeK9JhoG_^x{D*uX>l2^YAn
zk{lbDtxU85Sy3Dtm|2i`z_6moQ~smMsi!hrsVtj8R{dE^cRb~;&wd^n8^Dm#kwB_0
zZH&Y8Tu}p7xF)UX&7>X#gX<w$OBO!)uHh^Db@)zc$eaAGVaSwN(^=Qp-QABkY(GwZ
z`whaMb-~NOLzVbZxX<fy@MXQOUz$2*gnY$B+I#AvA&NG7YppFdj>>e7irnZ$Hy@<v
zs=AAAqu2G^=ND9Vru$xs`=)Tx=piaG(@QQGoFtudpGvyl;|4#hG-;ins-?Q_?o380
z>nVJNG@NC{d*Re7-W$k9)()Zaj2LQ7HcsfWG)|~YNTI4kdtIWjzpGb?VJ5MMF@(HH
z&qAhz%}<}0A8Y37bwxHq-lWZtDPi+rwY9}bx$PC%40)3_L#BkyFRQIP24OSgP1+2Z
z5;n0_=;hg+zV;sSCT)gH37eejIMfyALHP2kcrV=5iub~;tavZ%%g}xnM`$=Bh88no
zXeyb8(1xTTv>>EVzmP)hiTs_TFC-q+tHgRir#Tq%CY=qL5;lLUwmziK@K$6q<W1TP
znG!ZXrnV*y!e+>uv>7rbY#y(+t{jBTkT+>FWJ=h?R$)FY=v2>;H)%6uO4#J3C9k5S
zp6Be};b;=_CT)gH37g;7m$5v;5}S)Vtu-NU(q_n%u=$?aTw9+Mr#_G$@(g#szZ#uw
z(7-en@8@+o%!Ry3??a}9_q?EIoe-PlP8kb%lQu)9gw6Q0AAf%$<Vk02slKcjHzVIe
z<tw<rKg@(w1HPdo_dv7JW^`b`F#=mk${88`be1s!Z&p&y$mmVa2z+ljMn)T*W*WmC
zgvdT@k=21X?jG^=GZ=1B)PZQv_&Vv|&h%G+s1&)Y68b9%{a5u5tiOu>XU$|E@MR-4
zfPc06^#O6rWxHzi!yS1;0C<okqYp?Cb6G|=S*7}b6ftLH^e@i{q=-2qqajw(29P==
zUcgdiQX>!5fdng6I6qN^7O|o|;~TgNT?zg368gIm`r|)i*QIqkPOt8THTXoOWlxD`
z=-_91l}D`9{*8hx_@71DA=(K$T1S-kRd}*qon3I-vr*>5tRKqMiL|921^i1_mDf#E
z57nrdrXG}EZ>`Zox4i2~p}oZWsH<wQ_nWyU=C(9*P0U^GNjEDjnYrd_3ODzpJr(wN
z5`MnrNwJ^NyZ76upV#YEp3>f-kVz{foVw5a>_+#xHaDUV6QTPSRXD7)^80NDSXoJ4
zq|0CKWr)#?_seSTWWBB=FSO+)edtZb8~T$G!>NNu%okZ+fcPa78i-G>oxn7>umN|r
z5;t~Nc$FtH<u<d_HK_Wro&+S)p`~rxVVGi){6FU<X^zdT><v=ES|uAO&{wMu$fOaj
zPU%WC$4(YucN}y#L08Pr?l|b~FlhNu{-eW)+K1Rph%~)4Bq4g|qwc#<yN=Z<f1-6t
z{QDxTZIn3w;k3|?^v|gw%ep8_U}9Aa^SZGnkHxQ}Yb>3lIPC;n|Fk(=PAspX69=dh
zEU`mN(@Nlr^j_}Rw9;4ztiz6(in0(JbJ$sqIsA9MT3~$~TKZ(0#q3mT<!HHLV!f#1
z(lX=Q^aXMmF*LYDC6<#6nby^QWr=oPvFc(aarUL%Icnx*Vnm)sx)bWA@`qBweoj~Q
z6Y2|U22dJ?GFQdv<18smDm=)DY#bbGGa>^fn!L*_&c{#K{||L<0;lD;|BpW<l|4z8
zl1{elSx=5FBq7K1JoC()8D`PU6j6jYNwXa5IEqOj>oXCjtb=l*kR@_#W6vJqWS<Bj
z{66=6z2Bev?z*nqq~rVgzy7_vyx-U7{#>8!TJGz)pJnLgeqHHlQhM$rUvmq82%6qI
z>-eTg8IbBSs3#)bwlkEYGkwrtcuuA<CKC#6bE~D)8+W_Y^sZM(deZqk_ZN|V29f+h
zfEM|S1oHPODd~QJQ4Y?Tg?Y9*U#uobkAf&NLCL$3GU+FdG9`yLsAbY)Ac`zga#o~F
zdY+?9$&5&ube5w`$y1Rs>9Y`-O4x(q);q^~U!p@$K+0@od}r0Lv8nnjAf?YxuWgqZ
zx{UR6|3<@H4C(X0;M73A1TwijkS>KNJW20#p&6cmz@RVr-yoB|q{l-P_PrC0r(=D|
z---JE0=n5n##P{gd^gC%1xIQ=surl(75ec!V`4o_??9&;_|CctoKI=n#R%>f;l5yJ
zsu%1u%mw)&Smw17Ny!EIEpoYFXQVxt;t@zyd)Ud@i5LrVY+z>lB12=rbDqdxVs4^?
zUd{#13FP@21bdyT1uSE)zo?<IDy;C)y7<dD&$rZh?tE7IWJEMdmrR<JQ(OMZd1E^^
z7^bX`<{G;|G!_L~%0TOM9+h&JiUKWVpo2BgQtmj%MI!Di(<2e*B)1IDCrQphW;IcU
zq_Je4aFVHo{#8*whWUk-*qO;+LHnDi7b!P*DP2>|ZWy?|bO6(&^b+cE=$K$AwCnx@
zoc$6xC5<_qQ{(Y4r^e%9j#G@jSl2rPWji2^d6?nzFvI7;{RQqLVR=a8f%F}n_XW(L
zhmJr2rxVL2pJ}&K@_aG&Ot`6y+>tUNc~M1ok?jiCc7;70w9RD@Y=Y#G@lqORE`b{P
z;Zc61%u}W~DZ>hSGOj(aI06|t*OENIEzPsR85Jnn8B9i17<Hh-aA(ABCgi-)1g}lW
zuhSKuW4zp=<n!jF+ynv-<QHl3Ic9ZSEGupUT>C*+a^9Q71L<$&Nsn*9qdW~inD3L&
zCEdlZ+4ZvRgip$C1K!8yc25~y_8SpxbEJ&L|8)F9RGgDYBEBc-9T;2J8yuBZ>QI)F
z(uY5i7KFZ1Qz#@o^b&JIFo!y}j)hd7G?3ml(J0CjG}84hi}z(~aHA@cj-C`(Wa}JA
z#UknOE8>c54VtVL?U80?-)d$#Qf4T7BPr99UAwi0jUk$Anv@Phy(E`3D&IIDrAg_x
zT<ssO_A{h0`|e+_XG$sUmnEfr^URyAcOpenj)hTVs~g5ko~4nFj}2iq&(=keBIy;E
zM<bKXvvmWcVv&@&8P`e0V@C4Cos{VW9gFj9F(XkVWjaQYEoLN&q)f*svc-%<k(B8e
zMYfocD3ZpLb3bZ-^FK|E?g9ooA^9B0WHyq{gD8A$uo#WhrN&0kmwW(ZQf1PC5UrIu
zqg}ocji+LL$x{=!cG9FBAPW7?MPn&2@JqfT>iY#|#JU*pK&=<}fSBwxaIXA7k36j#
zLn-MXYyoSf#@FSKMB_MM(3iYY_lh*>aP2#~UEbbPg1+{q5<0ObWGB>Yvsmrw!5e^5
zchU(Eg>`3*-;MR{g_`{{R!{OFx`kGdj#km}_GFav&RA6J%BXUrc@<?yr$ZF}6g?Y@
zBfSAA7>h3VHg|`0PEU4$WCF8RoP?d^7o7LpR-QdlFc<}q3-V%5F4&baFV*DFXDvsW
ze$lCxC1r^cU%l;!C9`X^v`BY{sC}xYKRGuF*_;>G*u@To3o&`Q%0r<&Vd(W79k~MA
zn4E9j!a84Fr6Jx;#oG#BTq|lA7Z*~fY0vmjXuA!Cb_RQ)IDd)iM#`BguX;&2L&a2M
zEgOtko=7dJnbKjKc6qx7p*jO+Sdls<u~IkI+JR;eAiasrrdp4qrdm3RRj8@fQ=z6>
zkK-oC@da>vaa1AFV|AiBPrSL6vPkl04c^4Ic^+n#h`6Ssg;~8F7Uq7s$yIZ$a!(BY
zM1u9tbvK;5bmVTh>u&hoj=SNmyWy6%)N43ltpIjN)yM;Kws;fol$uoBF?e!r-@o+&
zmmzP`dcnRw>jnFMtQV3(<Oxt-b&&Fe_D3HEnSabaGv{GaT$09Sd#m%B{zM$(INMEu
z*<#uBtqBS^<&8R9ocpRy0dI;%oh_~kRi}Uv8+E(jqY?6WU~tzZzgSPDY0^s}3J)H(
zEvC=@aa%8`tqjxV^pVrIJ#j?DDRWKiXhP1joT%9`d<Z|w)L)u(Bi#xz2V+qmqNn6E
zX+}kk?HjOhM7u+oy_LxeA-nFL&nwu5I;OrJxtDyf&$dRnZIW_3d5_)l1iPW!V>gs1
z*-hhy**R4w-&c@7?UJX2C6O!gH)Mh<b}msL+iP^CEx+s1?>3zs&vMOpmTR`}ct6{F
z{92O#`}K}7ob*wMIx!^rd{XDnaJqUOFNx*bRHR&#azC8m+`Bl8ygkUBgOqO9q1$ab
zyN;_}$JHLk^Bu>}Bld4Jhf3auChG0a`5PtIL%T_P4JD0=QR&81>4~Y?iHXJF3=6&!
z#9v}&>f9^h*XWOxj$gCKnRcRZ*SLgtpa{*tE~#%f*-`%MCEw1L|9bVw^U5q}Y@YhI
z+m7<3nI3>hlNxiN+1t_VZaNwe+tiE_sbU>i>~^UtQkJH_e4(>ezPdB1Irx8%RT{ly
ze#S1v8{*F{WOr;3JotO|K3B!rXI&NNm(8iNgC60lNOxD!Fw*@Y3cfpJ|LcltTWhzW
zq&usqk~Fe(hHWTCOHuO93CvY-&V)x|p>g4PN=TJ6bxo#8*-P%vW1Z1qzl`>)A}v?Z
zFw)Z?YA^2`+W%<jIPEr+v{FTtq_h<LDdxSl_52|unq&0AA3n4GxhhWOD)2g)=Vkc~
zK%UUM>ao0p=cO+q5h<67Y|1N~eTde1oaa55_Sl(nEB=(He|WF;^!MI_?fx<^?5>`v
z!d{1Iw&iZG@}yYL)F%(aNm*QZS)OvMJDkQ@QW;577D%ew;+&irC(X+(&$V9dTCet4
zU*uTd64@W5Rd8-%pCi>)O?fD3G<hoB04hBJEOi24dYnA*n-OiYKJjbz!2E$VS&2kb
zsEK={;*}22NWM`~qcb$yzEM##s%;hPz=Fz+ij<|18`Ywk;BK@u_<xU88ogvz#V*Ag
zo*R|>Y^}tuHC0~J8R5J&ZL91ie=(?Y?`=Ex-hO;Lc|tX%Lod_n6E?PKH@+PNJ;QC#
z*`R6f4s}^Y`hbdtkv<KPZ-b_#+1hO==|d{2Bz-=z<a~ILmN@l1d$X(J?9FMs|8u*(
z4^&9{mn->ryOi`G6%~>mqoPvMoQeubFI7<~=@k$KZwwUX*_w5oIwhT>qC(RBnpDf4
z9d1H7CdBw;qgztL1|yUQ8l<!y>a}e@<FUWoV?X1u@4iLl#-H`rU*WNz_1ItG?0ByI
zV%pz%im9sP-CDy2>tx=)VT-Qh{Tot_Al~S~=&l5YHrZf-jjCPYE1fC%dm>~Q^z}bE
z_LrV6n1;A;>+ELXpC=w;?xb*y`6so<*b-+3xW?9e$<ra%s=V((%C#!*bCa@QqGb>I
zub3xST*Z~0&R=cldq8=pM7ji`@a-5jwwTJU8PWj|1rNN)2di3ot7}b<t$~3W`Hqmu
zBSz9)Aqt<dos7o0@xIp^dwT+<ubt;utg#z_flKlkkV!YvJ9J6P8*#l+n+mN_@p+)P
z0Rwa7b5t!&IuD{Sg9T_j1?(uVC(cau%(LA^>z*bZsG<z%G>C%dExl{x`4R=M>V?zI
z1qvo(0JH}MSK~rwIl|VZdhXdXPst9~n8Iq8lOSjS26>ZDhfK;yIs>9G?}IR69KoP3
zc@wTlU()Fig?&e3WUl}QeaY{NM*X`RO}roHt+?&!Fc2vpqmwdA%Bq<6=;Vz)yDwwr
z3^Z^2c|M9+j2q}ldLwnF<vh%@_hQrXKTv-k807j95@UkFr=aY(ZpHpRQ;!sBQr>CW
zr_Sn6&RdG?q78N2Pq<Tr!XyXmDc?n~`+2^ez_6IAyeQLNI%CTA16@x?y|!JxCn~_L
zO}eruD{*P9l<4h#gBkArz);9iaV~gX3@$*WUWqD3${eK-D{C}f9DhQgU7Z%k*l{%0
zPj?eJsMA>NM(MsjdK1RJjUpqx2gc+bS(;j|J5GUHGUrX&k6@E?u0v|PytzeNthJGH
zE<TCM8Yx?yF)Jk$((@rQifnN#M3HnkmZ(u=i}Ozu?fNq8DE|u5*kCQu`o5(_sUT%>
zWc-%Yu)&1ovk9b(BGj|vScdYg6%Fc=nx}9ZeG4f27-?*r-7~>8*sN#kJ~VV=tc32m
zIGo?|aeIw)dyTXMw|jL<ecLm51gpO()_QOs_ou+dBSPK^<P5<fj8>L&tgw5@L!h+}
zls9zju9Vxcdit+Cz~4WVC-rB}@^KfI_$w`SmeHJsxhPts%uS|1gY)Q%(M8tNuJyEQ
zJ?*hRHPQMMRK8tmBq<%Xmrtj-9!I(!M|wQEA4+iZsdBAXxz?*Z*4<AYe2;=}rxV(F
z*z^Qc7V6jxC5?uw(hZ=}6M*~n5!2%ojz3VMO^$c`nmwuhz?!^@MBf0)!zOvpqg(7<
z5ng!*8uGA7-Zu%}6)_OKD?*L;p(ziWEKOPFK}Q4f@Q0dFZL3)a7F3=$lCm`Nu&F70
z>mz*6qsJ<ZUh+Yj;9(Ok#T%Z7P3|iPX9KZSR&TRnuWl%XY{T{-QsoO>UDc#Nsc01G
z1~BA%q(n>KX}3z!UsY5^N=v~9%e>G0X^yX<WZ5Uh9;rCDhG7Vpr&!NRK3iq=N^AG>
zeAK{Iao(n)%HMT>s!8irG>UYhR^NS_YACLqs@*C{&s9+sX=KTLn~IjAk~xKURh+_?
zV|T06T`o<!QeWH@e2-%*kFPR8Nr&Tz6+2Ar8=bEX2jz*%b3ob}uY=P1dZ>!@ZxG4d
zY-tT&OD&A_NMDHS*|X1c>SHwAJtV5tILWz&DqKSquAvG$qJ=2gYg)QBG{iz5parku
zuJ@&$4XSMS1MqU7L7$2il5TU8c|{@xA)O4-I>9shj_Ve$)iY1gBe-N9$+C6oEoy`G
zZir$wC_4XE)h3+?QB0en<&ie&&nhY;?ffreGiHFIakwN2l3wg;Q*?i%P5PLtP0{m_
zHtEX{nF_J>W~A8m*yK1iTTEl6Do9y!Ss$}%*jT7zSV7A2g?el}riH5@WniIR+o5F`
zT3@)7elu#=U_3$YnLJyIrkgB6RW>*oj4E5}BEV3U4MuNN+4?%J$_B^WsIqkeiXU2K
zg9C0<+2SaMs%&r&jVfEQ%DG?QTa1xgrX!dp{T8C|Y0;l<HnrIs7(78C9|D=|yrgM}
z!oE|{m>ug&{#4YL^qF|y9WW*b0t3I~$3P~2M}gMH)W|Q3)LOCFf7X2<tyG6e9#3X4
z5FCTRtG$#0nLQ8fikRnEr>FYZ>FNbL7jr?r_{`)^LG*ILd?VUUOM5WSf7w0kD9^&?
z%GDr`akvCZv@m6y>YQAnWzQ(j^B>xtqbt3XwmA<y=ckLDh>F;2v5sC5BjS3KCu3Y>
zKj?;0WG^Ps;-a}rH@6~tG)2qJ&iU;2WW;&3-k#E=>qq;{9Nq8UiVGK@whUjE?wlGX
zz-=xRGKFoU{lNWl?^ML_U=$0DP0?yNXLWARTSe=g^dcRy^4CBazpOCQTAd-~91psc
zuLCfRXwsCsWpzFa@+SndZ4?wK@8|L;i4jEZ^yUz`t8m>pXPip785X%Cr8^mhrW)H}
zD6}&Sg|^dBXfM0Ta5pz{OWH+Ce;g?nr<j_fku3Mkrx$fZ(xjYX?NY~aF7?;BpC(PK
z=r~etc%fH2foTYwp#?i2Yt#qJG?0;V?+%`<ldsg}oa6N2x{$Z6Ng01&jeL8Z)H#j;
zqYc!!#<{Qq8F@54a`s3CYO9kcbLIq*{er1;MFghEnVYDQa@vN0Ar~G80vS06Ed1Ix
z8_Xb3T%8nndSN{AuR*h*3@Cn$6=gN!*X%OcKEf{}+r~qoZPoj=Wt!uLZ@#0<R;5nz
zB2wmN6zRA@r%4eh6E%tq=2V^iMWoEyDBAfhMu&A~@UUA*dK5&#!K*OORxVN`ee^E&
zEhX)Cw-L#=o|<YqcJ9|RPstbes50rydu?S(Hoi}lNza~TD^oJ#epOD&vb_eMBW1R7
zHy~xIvR^E(VI!km)1-75>Lt0PQF#tRN|VxWh1x$x?Po}1_TA4faNsu3epyo54+bu)
z1NTm(NXjuWifr}7*vV6F(hDFmifm1c6iGM3=o&?~dg+*FNjbtsk*zy16T~$si(nL!
zs?5~o^Dd<EDinLFQ0%KhvCdPbE*43dcMwW(o-L*>ilofjD6+-WMUj+w8%4I5x+s#y
ztKfe5dge?s^Irf48!Gv`kjeZe{Q#oybxh4|rtfXQpfC9x$fR%PcB57V4AjU^f=tv%
zM?w_3Sb)Z-vA*OhqP|DnX<Up01}?}OAd^+}3g~va>!e#T5!Y`v7J{BzgC81`eF6S+
zOm+?CW+zO&pfCAGIvq<%IY!|Zs<|h1!!{KP**FNoz##b`-OtjbM`>SqiB^CHw>0n9
zwm-)3{WUgX<m+oW%Sm@q(ea}}Z@t6RNj9L?8a8^Pt|DAn!^Q{{c`f;pP@av`wel6D
z*Q?&7g*99Tm!iyageGO>LwjtjtJ_04Dc4M>$JO#r=!IeT$JmrbV?=tQiZY}lAqqc`
z#0JMwCe<Qb|3LMmGWPJwYdj9jQL{}2B{kB=APS>ngX=cT!`{4RVE`Y}WtJu7;F%Z5
z^5K%@7`k6{G_$0KVl<7ee7@uYjO1myXmX@nJo1eKdEqU;XluvR6_blM1IcEd4rnwg
zBIKyBilHX!809URNj`X3kY|IOGV~xK+Fz!#qkz^qFrvi)p+gbb?*4?7wgbvGMY;n-
zvd)%vWP@QzLi2s8&)3wsPlTFwaG}r+FBIAV?t&=xjshf&4Z})bEGspZ*f6ZDVJxhQ
zxLxTC!+~&jC~zJ5e3@iCr9;m=MMH6E{nYq5ou+iaRZ4qeeytz;s(@bQP;$wydevU@
zt6t@pbEU6(m05B4SG~$obLp*m)voy<WX4=PZ&UHk7;T`{ypwTzly^UQ*WwN+e^Ti!
z+>TfQ(X#J6qE(N1&0IyRKXxCErHGg-?<yjiikrkty_q<l8fWO|>&`NqG|FZKvl)&J
zq+MzRvq|gLF`P6?a3t@yQGz4f1V_3Fj&Ktk;U+j@x|84td-#^(Wy{())XD<xmuG_~
zz>cX&>5lSiusjL!z(+Y37teQ;+b289xjAu`QqJMxVU6;np!w26o(+)lrGe~Cq>NhD
z?wY>}VM%CI?9(n^#fXSBwq3ZNajrzEqwP^j&IU^<!8X;h!Pj@;;IP4Bg_>+|5`>y;
za6yNfY(yc7ce}u+AjH1|gQFMu#gIw5q?bYz9#U;vOqc!Rwq8<O8Rp74B>RXxeMDr>
zoWK}!=dAJUKp(=-GWC}x<$YRu9f9rUNPRh$CM{8s^IcKy{v1L1eBGjyGoJE$6Y~0f
zo4$GO>deHbb0u(ajg&2Bs7CA!+^BQBIkc*74@o(8vuFz|WmwTpRBGR0yr(wjb0oHH
z`M}v_!4KBi4k)+1P|kVEQz7MT<sy3Z3I0O4$6qM-_zUGp{<`4&*f~`zA1?_$4^9cI
zDp%z90D>!a?ol5*n&^rhPjscNh}LC9@;d}}OmfY{B-iYy;{EKn;@9lR;@6U58>{!v
z5v072c4D}U=P6xlBk1aVJgbr$D=FuNY-;YOc{vGr`<9zMDc!C^x7&1f9ap=Kt38hE
zosSZ}fY`s&94cALwe|Szd>Ze1Xg6uEp`=kUD&3eWJu$f-He@k4FXNxMW9nS?@oNm6
zFD2vG7z;-se$Ac(^bw`9#^t>e1-b|5er1$bW<w+SE2GqS0GjSsMz=7Z;xizhs-tF<
zNEPeAfsn6^lCm`NDZcvfqk7?I^m?q)=w-e~YCg-?W0i*IQ+%&t5%D#x=dDs##kpU(
zD$a+-sPeI{+A7koRCE&QPZ0Tj+u%i9dsn*+CH+)Im86j+_d{c}6eaJRWnC5LJbyiQ
z?dWw!>CQNzN5}F~t_H3EDKr-WZ`&f`Jv(~cQOf%=pMHoIrvY!uo<U<!Gx6+)w>mov
z%I#Pvw*#Wwj*4<SG|KG-?1?YDHG1L;<(~LLxhK9*?ujpyd*Tb_N%6g>FTW~CH^ks>
z96X?#(!qQg+T4_RiQ~S>sw%QhJNC)jDSDOce7#C`^759oowtaXGGBX(h?En^YdxtX
zd}S(!b<#zsQt;)ovr~QKAV3K#VtJ4gj41IU%eASeT%u+dEJ4jKS%MmiR;c&RH0eJf
zilt#adW^2Hsu%0S#xyCb8y+LrV5t+LN-Fk4`bcpEDU04-zUSvAGr|pQgeR~?&J!oD
zNSc@PjcdKywO;M9?jB;woChkEoT~4j#_dwhA!aA^+D*r5C@IHGdbu%GdSY@9G0d2I
zWc<Kk&$;+Dd!og!v1FV|@oP+&J9+#XbKo?IU$f^1eMBW#<6+q!g*_PP9)5V`FlZzn
zeyDK-G}}J>P%}!Tign-s%HfBUE9tinKjx=2a4w2BJcpm|*tYnP-g6&yRh+|*tKuAf
zsIr6JO{z%yspurq10eDpe!Ak?*4k|->CP&uB#kV&haXysl6MY2u8MQ`se{wl;pZmY
zANh8MRCRBhLii??r_eO?Z_Oe@`W8g%rp5$6&^M!#+(apRAJ=-M&Z-L^gZDZk)n|{M
zyeS)HDz!soTdQ*TZ2YWL&pxYmpdufl?_i$2@6pq^h&@raI@(K>S>2#nrAg;O)H!vo
zylm{#zh|D3jS;5YGlM7o3{8HOSd@4%cF&9}$$7tHpl0u1a@+B#*;`qH8kZh-rv$a6
z?4!w1)wYT=#n$*RZAa~yb+&CDSMBmqZr6))+ZE+@x*mVw@zdiklzaSza*sdnF_!*D
z>07|HWAkiq3duLEM>`Wngk$rz`%q~64uwgH(wDp!OB!=K)^RI(wo?(=E=82&@Ng_1
z`Q{ZV-y~@-DY4K`>D%8UX?$P(h|)+>&UZP<Ep!e{tS!yUt;)4t<yx=uSa%O6FCf5g
zG|);GW>ef?JqM=iq1~jthLT1ZSGqA(dSY_FgUO6Jg7HHNQ|Ig3_%#O37rOCljD@9+
zU$e)UKB83CxV(3wK=%OM?{4zSY-l8Zcas_qK(p=dZc;N!q?&c$K*;f%l%<h_Py9QX
zt2BDa1dV-nGv4qVd~S7b(RQs}73Wmrs^pTa%+Xa@P5P*cMv*=bQSi-Z`yWpgcWbvw
z(s?SXB8@DKu??kYDLS7xUlnv!oUaP5#*v09)!2OHgL8H;R5?&ps!5Ml(J0a~h~!}C
z{Gu5x4bpCvq{phLiZrt1R+^Tg!<thcSH-DM28)4f)^qcv%6O>C0|ip{l6U{yU;Jdh
z675$-dZLO>A{`5n@7uq$RH@yDlAfZXN>W-%{KZe2WAyUYf<54_igVuSf!D+R^>uL}
zKYDhNeyX6b8MK?ejZsL-2gq{QYH{98pmo0g$CC=*=;LP@D5q;0rQA-;V>djJc<hF9
zk6rHx#<nX@uWXYdvaN|IDZr?#S@w#`nsv*X^_0~;3^2^SbkIhS(qZtt#`QSD^*F-g
zalUi>=<ZCDYS((TYrWcI-Tmp(%P_s}(i!JGoT<R2DpeUu8WptC4WQB!fcrR#1?4=A
zp9|~>5WmJ6aK6T`*;9hLQMhYduhA&N7@+$gg;&NxBl$rJHO51;?FT8;j1sAG>t1Eo
zUCtBc2SvljBt2GX^pcm0!Fd9g;tkJv!u`;u^M~N6a;+{9=bK`-$|^rRI!|5+>kp3x
zjkbSyl=jA`%PP|IRCE&Q6%hIE6SP#L-G-80q@qgFt0PO!ZJL%itv$C0SH-zSG~=bp
zkNT3OkaUlK^TV%2q<?`Z_#k^>o~;5^EF|swu=XV#tfE5Fx<^%u^n4W+k~Tf2TBOre
zR7iUH6RKq|PB+IKbKF)#FDGSIL5?}i@kCY3k)H4XJ(iO)SJRg*rYj!=Bb}$kC?{nC
zrf*UXpXfMcNa;py(u-@@__I35kkUb@mt>!XkrO5<3nKlN)Ud%Kh#o0jhk9%b);O}H
zu{hjcz;ef`*p4G8P_buViW$g1F;)`Dj2bo=km=19W0LP+kp3MJ8%4J6fi$$q1~W0L
zc1eFg7(M5hc@lgYjC=>k<e)km(`d0yr!?s=5QTq=t>i&tW;`%3N8SLLn0t1fQF{{@
zsF5#)Ow>p}hba7TT<?cW-<^R$-<C&=>_edRy#lL`t1(cslYbrw{x!;ql#4}5*;4c9
zTx0%iU|@@UIb`CJbOl7Kgj<Sm<wJ6$CqN`$1ZbB(Y`*bP3=Djb*Fq*f>=f*=(l4uX
z@b*ED<`enyz4r?+443P0WJzCzNI$)Ad$VgFyfg^^u$&#K31dvE4@18JDHzjo!8Xk`
z#T_olpX->*Bam@XaC!$aa_&-|It|4<AEsMmnv^Rt_@PO1dXeE@Uc=?Zl@to?`00Q>
z<-`B%*P^$J0@4!9PBB$q%f^O}n<*e&Nz)L8dTgAecaZ|pSZMB>kqm8`PSsL|cBn3x
z(kjq7k*F&I-%=AOWvpCQ@;cWI%lSm(>8SeOqw15gUXo!$EyIeX<NlIeJ&YWo%S`^}
zI(sr9G1pvUrxXgA!O77OG}qX>MW|^f77Fcg=LWh~v{{mlhe&>kMm`C#=eDx=D5}S$
z^>ArYb6v+i6vj%@TEjWL1BOL{XszKGG3-#tMl>{O(pWv+KX~?zb-I#2Y~L@>`x%$x
zTB)<6^~eKMsL4hYwP><2Sno(;lMNoDLYr*xAQjqVgYk!&Z16A@YO)b!DBkTcVunmc
zo~M&?iwW+I(Lsv4zbKOaioH=vJ)xEjrfGV!#UYd4q_NSPP|F7As_2n&#u|fcakxd1
zl!I*)*<y)Ak(86zD6++UBg}>k*3hWhMSKs_geN-j!GyW^L}d4B!bHC>S{|fNs3=Ry
zy2|gK*p(BJ9lYn)QEtSY{9a)2a@d5_dN5_b=#8&(eJrY8y5~7B`;#rEQm)0JTR0Y-
zM}>AT`7Jy7vr=8#_)XIZ$S5jNkz1l7w?xiQtk{O+30I%vI;2vzHOg(1l-ncivD+=d
zZYcNI4dqF8S+*_p?{loPqSyF3zFpi$zCf5~4&Pzh`7K@ge+ReN-_oTHS4z)>7$!B!
zHtuXctPC@d<B1)pi0pVoWV@Bs>b`U4TCaAkSG(4$J=WbHc0JQtQ1T|6YrWF7Ug@#k
z<UF1I1>>}xuD>dK_@9MJ3sAnTM#@o^cOu;YsyqR>Z}G8sTvhSMXUvn^b^IFraYDqe
z*%{DI6z&?=>unTaF;E`6ZMviU7yYm*uPlLvJa+3E{OP(LfJl=XOQ9)`-7HP{b=RPy
z0eK8Y%_xy!tOJYPE;WplrIE*Ozwr~SQ{z9u8gF<WyKRe&n=dluw_og9+bXNKx3br<
zdS`Gx^+c6@Rb?3I!73U>Is_u$N1A%#+OFEIiu5lkswRyrjdmWc(Na_o=U(HgI5)U|
z!6{$-d8%rwtX@^^UY;+Nxhl?EhE#b#Ycq`WF%^v>eE}k0;cv#ZX6;r*`jCpMNh3?{
zTZXh0mCPx;tKt;?5WIfj>loR__}XNgK02%-<q=Qr0nN^vBQ$ZLzQU^}{a5rVX?_i#
zldP`~6sk#kK_vF)*YIHkU$x7-^Q6-wz2=&S(D;+SK&&P`2%^v)8$8#Gy~()#W7Ksb
zvSzBH#ZxSg`68MHt;e)R)yqJ;=?my;?mnaSwX*Y6`C3%wel1#>lm}ZmVe-JuR7KPm
zAu}t_<AynSV)aP$FpG4eKK1bo<d(>U6Cr%zvlY_fW*Z96Mq)g)heCThw9Ux@-8Lp7
z+mz=R-xG0duSH)&LwvCD`-Z9!$-bbYSxDM`fj%@LT_2+0bxdKNtrwnAMbbAQGKy^N
z_?#-*^<*5+XdF4xpH);wO3&u?6kBUQMe}8(eIPQ5Y<>10RU~DIMv<-Uo>fKC9aWSg
zJyk_zq~}6p%&`^aUzU{rbDC_1lu655(WJ~cFrU$JSft}nLCQpoB3sNwERr%8xs{OS
zVmV~;Y(+UZZ;W#cU)LP6r15~W8gQk~uq^3Wu^E=lv$gM&9EU6^$1=z)n`i5EodQ|X
zTVhinn`i6ZNRjm3g~o}o$kuC+$~zsTteH_{i)9l<Qr65UvNcqvOO})q#3-^=7b%iH
z6dRLlo~<_`MN$rvv1re>c^DyG>hh?U{AC@zG)HYu9jEjmpvM7&hs5ONks1f?x=2KN
zd#oq<EXcy=`t#6uG3rbDZmci)H<8-*m?oSfL2%^U5o8N?CrjF%ETY9}@hE0WV2qrT
zL)2cv{NUsW)ZT(@2~a-R+AjYI8V5Yhxm-?K2vN|NycDvq?=Unv>Bdz~+8v^x@3!D4
zsagr?G>GKT&{X#V8XrY^q~Ajnm?Ll2eWi#yNLQV4<)mB(p$ATE?uSw+QqFEU4POo`
zoX4W|G(?=oK^){ua7}(cqFsJD8e8c-B}dv@MWv)%4)PwXy%ui6eCOUOadF;9bEb%M
z<_j&E)`E#iN9Go}b6!W?19xldv@NB(g<9uQ*7;dvDQ^alvKnip&YaWHPKMT*Oz``q
zG|xp8$jGC`Q^*Xt6yz@k+M8w2lf%s2ERZp6j%Og_m@-^3X9wF6%avsgy3spb2Qsci
zuDVc$)x_l&%o8rBfx4Xb(AyRVDd@{E8DbzyijCWcoJ2`kD4FQ<>g;$!Ap_=shC&W0
z3m6JnMXncdL&~CtdUnX6(2lvcwf)Hh9Z$^7zL=Zq7&D739-8Y|-B=>cb)H0;>)b?!
zdJ<`_^CZ$-XD9Nob$bfmk8tl#Wt`c6(zR4Z%3GGK68BvT+kP;4T=RPS+e*!+N$Eyz
zSao?e=+pD6<$w3M9fAlwvhf=p-F()#T&}6#Dh*=_1GPtX5VGKrp+o9S`D=4*MF)rq
z9w08&+E<YBAYuBp<sleb*M6GzFH-wyQu+xKV1rIQV{7O1`#oMBA6qtl!=szen%B#<
z^;@-JP~O449cl39;&~T|Dm?H>IX%Zcdra-4o*8-W!-wesKbT9^XmMlq&l20B#6fd^
z(^m#*QrZl&W`hnqGvfDIry*}mKS52S8+FBgR)(IBD7(GV?8TfUY|`HhfOn9?adR>C
zybZ0pSCRf3t%p~Uvc=oH#5(~g@GkF3fqsvKEjb&K7C|IO<GMT}iL95q)}1#`X?@kK
zX!x{M;%AGVBv{flOrHghrO*4~t$N^dUS2b1mLaJk%pz71*I~Kq&?(YtCr<M$yi~|F
z@8m=CETZJ&8d{w=*KC5hy;f)9@D<>5n8D%sVn-ZCchlE<a_HC_js4?7HqM7ow#e&2
zdGPY=nsoZ2ZcXK+?`j;f78pllz1+24&T|=$#Njfsw_LhhB~LnArcNbKHt1a9Ctdw|
z#FFO<;OYE-s0ZVXtoxI9T{>%Fb+csYS$B!vweCFqqjeTcmV#@j+%@Ecxn}dr?d6*{
z_7r&w6PGW$yhm*+@ll9m_KDR`M7HT4Efue6J}Kdo5OZhZs}nw!@ZbO0mrq$F2G27V
z+m<KN3#=zIzb6R0ub5vAj}Ll1+7-05e6MkB9Z!0TEN82s;`79ZRD8-5jweSbdO_@Y
zskR56x9~YuyVPnw(sJ#~qc6Kohr<3}fnj89FkD&0wlUAHL{tA)p{`j}?Dc;Y)mRjc
z&lFd$R_t{Pddskq(vUoWXsUC|USf|RpHg<zr)L$U>q2A}<=$uwiWEstT}6?t=(%SF
z@0ENg<2kT04my>`f%F&|Dsl}uPYC_d|B<2E9wbzn|9zn{XZA9MwsZ7v!R)(%4o4B`
zmJrE1uXTAg=tF|F9f-)D6>BzzZm-qx#YfTW;kk3Thp-Vn3U==Ld^F1E+VYT>ruZmZ
zo>!CdQMPE2Mz)=&+H}v}Yiy_=k~&`>suz;J`aIvsC?I_sA~{p0^K85aVenp5VV<pD
zU(nv9>$F7gz!m1%%0nt2-Y1<2k$nDpPF=@!`^jHVrKsDBYSx|;Or>5^$&tRPsg#j2
z&LHZXM!mj9T}IkRMLE*vRa8d$ii&ci9W=2rQs!)&vNb(YBxSlrk*z3sXR<N*8cjY!
z`mQElK^jj!<4L~4O+Mo$U*RU7Ve-r^NFbwQJwr3CAZ2bwk*z3G=XK>wWOy$ycpgVS
z4>Eby(k}lf8jFGQNQ87rT<;S!RsaKo<Q;Uvi^28L*i{Sae5<Gqm#zf{w#a8i-ALy^
z6vk`M=c5tNBJEeoaci%2EM=i@ibQS^o$r-oVEZ+-T}0a{HB>~~O_9j8?d;7Q4A#|q
zx~CFHOa_PJ#li7BdE>Agh_L;)9%^y$#L#LCU2p6xCAz~DlAfcYBGOujf@xcrXX}1d
zEF|6hC2m+nq(`Z!kaX*pRf}{#6%~@+s-hy&S5;I<I^q?zWlt$L#~gEH3TDS)>po2=
zN6IYZMn!srCX^$+N=2olOw4#-YhO(uN6Os91L;eV7HPGnSxU;xjfb|A&-jwh=xE-h
z$!AEJvD~ak57kUFr1wJ<#F5Ff^_40Xkg^Cyk*z&6*$gS`U=-PUE>a|ADI`GBcvUj_
zwyTo$RVAxcnW<IDlCl&*blE&x2Wq}qQf6or+1f?Z$dWQcqsZ36NRc$2T-Hf$2x4VM
z^6G^%yFt?^Bju6}zRyTL4{GgGXZL4f98j^wfn<atBIR-ntdYNk1OjXJK%`KvO|)!j
zI(i}!ZH-yKx6c0b5uk;U=s3`dNMyUa0f|iq%HPvW2Y<Uj{yAl|b-jNj(^isF*giSn
zEYWZM4NqIw>(f?}S#SH?1GT#eC_e9Do*cdQ7?$|8w(NxQ*ao~o+7U>Qc0W(t^AOYH
zQEdAikuW~Ct~U%@$umBqF<mx#m3NRr(jVTWsGPLJTND+Nrd3o<S^!b7k_+=}HNDNg
z<)rtjsF3u74^_(^4mZ@C9jZKGXsmIA%`sRy4)xfeYk5IM$|yp;w&Tcn;+X1*Bjbr<
zYK<F5#uLZXnzrM}dg5sE#F6#H(d3CE>xrXjHR34t#4*DYN3kc48J;+bJ#oxfjW|j?
zaWs44DDlM6?1`hq6Nmc+gZ8hPDO><-pPJN#{94Fl^^@KLQTS+tjX9Ct_n=?A!Ie?K
zC3Uc_p90dqLL?{fy8J{mrbc?CoOGdAruEc2>VouDh=QNKA>a00qZamMW3%_PFX?s=
z1%1iKKo<68<HW_<m-H-%g1+QWKo<68<FNO&FX<47g1+QmLKgOA<Gl~GFX<N$1%1hL
z*ptG(Y@DwfRRQU}aXmI3hA@}~<nP24?Pa|LJM!DQBbUCl$i&$JQ$T*Lv_E&bopk>#
z<=79`;VwM|lrurj*Bp5cx`>iOcwR#&utSi?L328~F%&R{$YFu&(79u`M}ivx<>gx?
z=oyjdWl)wuv<^Z=N_BChN1z}Rv@mJXNf61l-a7tOl;#VqR+{u1h(bM1@B=aH#VhGy
zx)8)3C-X#I2x-#EI_X7^lW%REd}-44APW83gR&CyVhapP^mXl2iZTv}7-*^CF2VYT
zLN+-1aVwNX(^IV2VGM-(!8$k6q%rUH>YX!Cyw}$>IMXrBiNe4_Jx?U{P9!HIf-|B>
zNMn&K9nU)4AC3Of@!T(C;Vm8SiDl_{#?k{jp#->eJSW8|(F|CsGhl8s+)KxEpL$z2
zAnDFIuvawTOUK(gOX$enMna)ICg-986QW9x#wyXOm1u}6(dw&2tFIERT8U45%VWQ&
z646??B&tNKuM*K}XctwY)mI7ej!l!Bs8phLyj_VaQGsisN|44Xu|g{m8~zo(q*rLt
zu|=`MaeEWo-5R+ijk#UybGz8*cCpXxV#n>>aCcwimNe#ek-GgM8uvxMabM(%Ws$~G
z9j$;xx&mfJ<Gx5&KnJ~3Pg>*~_eHt_#z*76$T#ld-5vKuP9^@03OpKBf|Qkz7eCwy
zIO-y@m%NC$;5p)+S53S(FOJ-iM(#@Kj`u{l)AH6FMc{0JqUA<VYESef@celc0qNQL
zx~;;Epu$bK!gc37fnOW<*8}xFJCyW9h-5v^(OthfnrU-txCOqW$GoAWv0bsDhPxs+
z!q6fcJY<Qmp@t34jZl*t1IH~C+I8bWoCj=q{Xd(pu}^;89J6@Rj$gAY8NX(iG=7bz
z3`QEiW(OR<)>ho%>wP=mP-sUQ3hfX>p&eT&w1WzTZAT-6;zlDvHyRPT(TI@Iyb<jk
z4K-|VYIF#G)g#Xq=SDau*x=kSs%)|Tp{kps__uSMhGoPBEu%A!Go>wEZ_#LQCJdVZ
z*`gcIJKwF?0WVp?mn(&&?LVfdfV2Zd=IIMt{USxuLsn5_>*z?4^qf@`*;@C%>YDUm
zh>UBtE{_yRr>vrASCjc3t@-9inU3tqq`PXsIno1ER7T45g3xk#wit(Ji)RqmYY-Vy
zdX~9VU&lsI^_d}M0YW`CinJOTQbrN#B^86t<;$p~bREoTjbk&7BTGuxp&lCpHI6K4
zERKe{w&N)F#L=j6bk#VDN$FbRXslx+qj40I#^PvPjW|j?aZJ@XHqbaqNa<SQn5uCc
zsd1E$#^RXj#L@L5GyS&%25<Y44}wfyDv}-rk@s!i|6-2OW?+l_4YgIymG_RO<b2Gs
z6D;*tl_GbpJ8$3KfL^n8uBY#Y+sJ*IhB9h9P0GmRvDQLdZH=@@e}YKr-dcMa>OE2G
z?!2>pJi<9u!^)AK0a5V7>EyBHFujh!#>RTOj`a}rnIkQOC@@GK8~^Ff_%pci8eE$6
zRt;`a7xE8PElbJ~mU679Wusn)J4-r4r_Q9swGTMsl4YtK4A~(#9+PwsvZN12gRr>v
z88kRT(v<}}Rcn+c9iugx)P+16KIhkHSfM+Rdi+rXWvqq#o(m}}5zZ1eSOd``eIzPN
zYb_g}Y5A&1zmDs%R)e%KRjd^YBQ8i^jB3?d%SKeID$>}*UQx>ejzhKLX;apKYx0{^
z8%Muomy%y)<CUncRrXHIHO@1Dd=HnDX8>7;q+AvM|2+;gp;!+AC129vU+5WxbTmXJ
zU$(YsRYkj;H2+^UpCjD`wqz>S*Rj!0edb7MJ=9CGzeMe4NNG~ee55ofzuG}MI+jr;
z&lY`p&Wp_GUG<nHr5ic$kkXB;eNy@eYMAwSEKADqef3yOx=ck`QhJtGx}@|hKif?j
z52Dx;g!7V^Y155-0-2O<<iqZybR$}%agQY)kIvn8y-&=n+6oxlx5@W`OwItL`#}_*
z0S-l@IM$c^yr?hfMe)9q(YPhnmwaB-mvnx-@6%|!7wfD1Q?8P9d(e)+Fy@;-GuIyh
z2KnytxsmM;3}jn>X=K|21KIB217flx!H<v0?5Q=*<CFfE!KX~!8CWFUVy1jyeDjqF
zt&465SyHa3AX+<$g_y;hP%^5doKUj(CwMlAG-nshhkCaCXJP+ST`h&A^ds-+&FRQS
z%xR;?X`#oddzAhH&JI{=Y7!*AH_E+{?sDnLpe%8qM*d5rW>;HE#Bn_VgCZi*8&s4d
zy#pdCP)qIiXmrAO$(!?}`#==x4MpQDU=SSnh1xewdasHqI1|28Q96Z5!5JYh;}6WU
zv5C$Y=b*YT3dllAo}{sXno`FNh82|zrqr>N%p?@DQ5^+GdKyHcm+HtyY;sL;j;aiA
z2G(*ofZ$^=-ePTWEgN@1D0f=YSRRXO?L6W$;!~Xwg`}Jrp$|5=H;4~XPM5$O`IXV6
zSX|2nXG~ZEP6v)>(3_lN7kXxcwGz+vGPEldKfAGd+>pd(OD!AGav4q<t5}Ouu~iJC
zV$oU}PRe41zSv;7+NFk*vRZMol(AT%ZkH;q&XPsd8J<)f&TaVtQ{Dk<^{z9FcdTC`
z5pO0rOgiaE%Bo6+OKaKKOXuG(QWjeDR(PsD%u{u-=TvqWE6e*t=$DPCRKvIcxxgf3
zQtlz)ptxllW|!?B*d49_%I-iKn<I;A8xSpb2hn2!+zrL$qFRn3Lkxv>9HG#T<SImP
zT@(rFLKPiPx&$IIwOE}F(hcc&(pVgeYi*aW!^oFvKTSGBj}|iY_DmM1OKUT5Mh{XE
z+y1w(%VU?Qklv!Rt%8&jOtk2b=MfQ+a^-lw62=n{r<?r5v%4%SIT+k=Jb#V#WlZt+
zfE%Uc_k+)z)2`#`sXp?@wR+}xVHp&<$%6@VEv{vkV;!7`HU&Cnd0`tUbW4q04YLK%
zu2{wF{7xDzFPyGs&!yRzmK|*aw5KGQ<%L_-?6}o1+Yfi@JM@lSNP6Jc=BOhtkV%h*
z$h=@=Yj&hay2Ur9uTf;{PDsTf>AkBcvUTiob#2#y0ngTebEJ$%)<Z)b8#xUwNBSgu
zhI(u~ud(GwJ0glukBx2`T8@;_hI&cy&eV7_r2A-S<)n-{s9Pq_76X?Tx};-cBaz9o
z)m@V)CuQx8MYfo=yvQSE*5ZVeIg1u4a~3Vq{ZSF)fh`ur^POpT2(uhQ)<Ir#ld=eb
z`K+fTSzk%AT9R&Bk}^^j!B}LAbr9F2tb@E}B4r&!i<ET`Em9W2cwj3kiStmvElIIk
zk|JM8iajMM_LZbqOVUM4Qbfuk1OXT4*<yzB+=P@F%EJdzrf2%H6(#FDLU)rbVX_=-
zSxSwbC0xS9I6|Qw8yq8fOhL-9Lp?Saxag5G+ECA4!iQt&9<q$PQYqu)!js1{EZR0&
z-BNcua~{MDf$cGByTY|yLE8+|bGLmMhFWx=Nt1HgNOCjk7%__$3g1G^tdQK`-Enlz
z6Lk3oTn4Wu$Ulj~ApI61@z_+y(l8A1Xa^CPv!e)P_V6%_PAFlZLmo?gs-{lg63A31
zkf8Vf8VM&#V2k_+9pE%6gOka_QRx~@9*#NZQ(%mo83dnyK@S%4kBFa>F|b8`P82`s
zlPaoU{Ez4@P3P<%5!gNOv_L(k>G4AKn4S&#a3mt#Nb6ca%KFO8I}=yyRV!_uPejX}
zcWK=B%Jf7~NILOb?i59&Qy>a<$wFri(^j?G%8`yxQ6XvER?e|C3LVbSj%m^f8kh62
z;`5<>sj4`8)ju5-=RP(KD)&UbNtda}c{Z$<)OWfaXcUgqs|hN1X|F=k>hD?qQqof(
z^4FiXW~;3n=_4vCB#qm0>;E`9EYyx^(xDocQ~%U=Cgf68af;j7QE_gqn>Z>J_O$Kc
zs5q6S%GOW~H_9YC+o;m`#!hT73+Z`%o!xUZQk<lz7Lv~Ufdf}WIv=7AshXB^@@ze>
z(qK8UwdqgVy=|vCI(=B3meSDNA8DwR^jnC6U-%(k5ve8lyF`=BkoH?a;{}_*0G$Lo
zE_r0!nJjJvvrL912<Ex_jn8F><{Mz(k36MeWl1|g6#fm?&Cu8a81yCYt9>)1yJ%ne
zE2NcZRLA;~kJi3f(lOfijCRicR)m_Jq{UB@vUKte%n5Dw6Q$mHyQgQL?~O^dk4~;a
z(orfZARP@+htznPu56vI(qOu>H7V+DPaoH5j!sY2UPYud7z}z&&5wx`NiT@Sk;}97
z&q$G!p&5&IbWP|mS34Gxwy3Cp^i>rVlD-C!*^SxyQ5BsTpIT|wOlP1xkS5(gMFpfA
ztEiB)D@4YloltLF9T;^aeMLnDq;IZbi><Cd>#`>80g-XaRyI;3J$@BMJGyfC8v_i_
zW#IKddG$$p0Yu@dZ9wCCV9=L*M%0(|j(A@?jy-Vkb?sNm<TurvN||P1B(e?t83tZe
zLq#;SiS{a@p@K+c8#)1(9#WM;(s94&aFE^tkr@uQHvUx=NxMU26xq5WQY5{86-Bn5
zjTA{=Tt$(sy;J5Z%kn^s^dN|gYqp+^6iMG*MUk!U?bIS^4~Q@c(fS}#Bwe|RB3r*k
ziliI0H%^R2wz@(p$&mJ0MX~L{%W?2HHgbMwtYPEdI-)sJRy@>WW4e|<N6J7#y|&}X
zc;cAqi6i5QV`_~XN5&I}`z2*}GG{$;G<o94dg5sE#F6#H;eJWkjicBT#|%###hy53
zc;YDb#NmEP*^Q%waZJ*wP)^EO5=_dHJX;J%p7oG&c;v|>DZ>#hQZ7+rjxEk3&r{8@
z*n*lgv@!+}^>WVi)0_<~%^cZV=Ik6#t<!csp`M#jni(;QP|sci_aTlafbq9GDPyI@
zK(iB2yn`pQ<{G<%p%ps}?$otf#55_r%l2aT>W6LSN!@`8Ne6b&{gCvp5SiVJt>+>|
z(zjMoWb3K5)nZZ{FK8S&(pT3qy(H`UIyPuBh$NS1Yl%jZBRvQ{!`^HR(m-;g|A_0c
zF(J|;We%aeq%0oSEHb1FQTnZ@WrIP=TLPpEG1Oy&L5dzJLk#uUV349m8jE*@^R_8R
zpQV<Q5b2Rx$}&=pcTn?eo~;FuA}P}|ifl1MaYEV|^)-rYF>86FfRtHFUsC2QTBOWb
zv`Ft;#RFR`iu6qyLe@c^){`EtnHG_<2tfkHo{|*#N>c2Zuf@KS6z5qIW+-nbkTOFF
zkd*0}XxNI9b)I>+<6pvLkI-aGN#n_uc#<vkC0pW2w#1igi6+ZImq)au9CW$Mkg^V<
zManve7AcDml%yojR#Xz_N!{UCy2aRa<aWgg%)t=R=9pI7=mt^V7c{c%JRBMV+n1>A
z3fkU84OP%~WXO3C^%e|$q5Djllw%<K!qhrO%rb_;BDiAJ<aN7S7w3`fcW~8FV^5QI
zjW+1UI+lSh#d@jbFnAqhdkkduQrs4HV<|OgO7tTsx!)q<;2o`#xSW%h#$*s#9Twbk
zGT}%aqlPM6L(cF#gTP;pYDLQ45_WwZ8-;2;Jr(qZNJPrW!>)F%T2SNe9eIl_ByH$K
zQ32@{5CyBEFwfS@>#HK^rw|!Mw!VlINxxb}vF+$`jBa<0u88!=Sai8OTQ!j)DFY9p
z%V`>SM2e)3s3=FeerG0IM7nGpBQoaLx@=ulB#kHQKE!s1BI71o>Pt4`Nw(CNY{rvp
zshezu$<EbeOG%ktkZdN;7BiGhl{B8L`y+pDvROCTGGDS;PqJmcWV4=R%iLtMOm?y+
zTSm(Cf@HIvWXpWXx{rX}WQ*No%YDffdy*~pC0p!Cw%kp&n8}``$(EDGlP&fnTkcD?
z*x7yEWJ{PV<B^@HsfG<kBsX7DPMuKC-V;7V<ZEHo1$O}Q9U+sO2kEX5h2Id^6OAKc
zeaVMMeMwJ__qF3#A3G+K6Q86^P2!*8$usTAb4pE;eNGGc0D`-vRd6diXFcvV6_;U4
zy<T@)=SIWWw?Yo_SCW*)l9=iw5%1v1#(mH-0Hc=Gd6y=AMXM|?x;e+5LGV(-IZkOf
zW5-3tNNG&&$+juaZJO)+mdFrkWJnh|WAdE-xjQ}=86u4g>DCw-a*mN4$MbYN)1(|%
znFt)($dEI{t-P^rz==`V0d$gz3P^8MQ6Xs_qG0bU%(K;FLz*if-4P<A$W~#bXwPhh
zcBDGZkzT8!3Q~Fw%;z+YyHqhpx`}$MAZ0M7FI#)7Vvdy28pXB)&M@FBG>#(Dc)%G?
zz!^8-B45Cnyc=-F7qEMGbt{}@z*98fQqp+9Sx>-OH{eoVz*$egSzo~J9nuZBm;qm<
z0hf`+11|OiT<iv1<_oyk6L7IFVD~QP23*2`r)t3Er15}DJOP)u0hjv%F7X6h;tROM
zxh)@#sJP<gnI0)uyu86j8V@*H{0zzSJn#gpsWG~?3YBi8TShz34OLV~x)DTX7h&sC
zRV*OAW)(%Y7DS5n>SbsTXlOaob+uOoDLtE=fURDtm?J%3MHQqB#`I-tx+>;K8LfZ2
zaRbgU;Pz^(h?F4)wlaCP7^OVXBW0BGEQpjLn!ao?O6f})57^yq+<>zTxPt~<O3Dxe
zr&&+HSvTNPU%=VC8*tVau)E#30T(mijv8<oX*}R!Pr$`)z-7LGi#-7s`vP{i8#mw*
z2HaT#E+>r#T;d71#0|LI7jTIu;1XZJ?shYqi%p{|_}z=2A$hi&7MNY4<_k$r@1olc
zX|0M1N$VgACShTot)En}fOMUWjX9&p);^H_Mrb>#tsLn%6;+VZb6`HFaa^W~Inr-b
zR6)vMOkcLTX=pi8Mr#!9{gJg~l(Ote&rqjDr15|=o`5rMz(u}*<xj96P(~?!N#g;#
z+l?D=mI05^fJ;f^0cSk{XWf8HeF0}Z0cU*yyW5Q$a4`cuTLUg5jR#!p3Aoq|xXc%D
zu_xeSU%>8m;|5&9fX8aU<)ra|OFRLWxB-{@0xt0cT;dDZ-EOu+R9x}0E=jrK<#a<D
z4>(%<49T;m2Vkncqid^>bce3oZpujeL1eZJwuVHCq~$6qBptVjnj<|=MTMm2Lu5R#
zH6c<Y{Zd8F8&+;kIp)Op<i+=jS~eJ=eAS(ljzc{*=vwqh8AYgPZ$EAv8BZMbb#5FP
zPaO4iZX6j;9QAc=$C35K(cp<A>xrYm6Gzq)hx;CcTaRK-9F3khial{Odg3Vd#Nqz@
zf*VJPCyuF}I7&QmO!dT3;)%okB{Mqx5^FDb-bcPd*JIjUX6ZYzydD4sJ;@j9(o4Sw
z`covb=SB{G`lw0){hg&Ntbj9*)7`U?90x<&sG%YnI$I4DxrUr~VE+R{td8t%q+6=(
zGTP?aktbKA93$yS$_+=fNV(yN7Ab=>d(VA{I?CDEXgFBwYFwpmT+V|q_VNtzL24*X
zdMHHll(#O=22+(Mv-W_}`f_&y(WLYurJ>NeZHhB0nm*3^w^Z>=x-X(!NSDQPVI#8c
z<U;FDM5Z{UX;N|_r6F%Ftr*RT-FO!&Bz;{)Wu%KCGBbg#m60OphMTH6(w(|fR7Sd|
ziV8{hg2;Ga>wrj+^hOmq)6dN*$D9~ncwb_J5z76Gl#W9^Ht1UPNEwCqKIg`f@x)PI
z=f;uo#8F@8#*y*F;oj%mII^BN8a#1iJ#jR6;>dd9aPM<&9L1hE8a;6od*W#H#8K>t
z!@bYBag=!CnCgk6#1qF<PaGwlINbXjo!*G`6Wp!HXXu(ryUQ$HhUGN^81y6`t4l9^
zIq2<?$ll93z|TwSuYmrx(-l_0naAnwxzDW&123zgA{yFW4HdbDocr837+`heMnw7_
zwOvNrTsv|HBjp%LM^bJ$qD9ILN3=*8oY{M-5OtKZv(a#{9@Mx>-MF0l9D8|&_)9gE
zCjA<saQ9||se0~nv_3sD#h7SPdXdtQ_ujx66-^)KK1UVLq`M)SbfnWD3caxrS$FPp
zv_2*>#VJjbk_#yfd2?yPXzslk??Q#7H7Y74ZGgzk1h(#u6iFXeQ6cHiDk>%Iu(>)V
zU7?~<()L@Z7U^0LnK;-wQWc%K<))cqnhZI-Rk6XK<(5XuphG=27`Et<G79gl&y6GF
ziKD*GjU(fUqrT3KBjbs~z4f_qWIb^-c;d)<;%M;1k@dvk-um1)ial{Odg3Vd#L?)9
zqu3LNd+T%KDDlKG)e}dFCyuF}I7&QmxVJt!9f-ve+`!1wx`5K|%1iHzh4W`%(35<y
zE~<0|=(&-|zV*$*mM~8J70}<e+N;1_bj~N^AB3TE)ld-)eW!+sTtm*SZzmXFb>!Ye
zdWPCArERVq`2-dz$4EMoa^;8?OUDgIM5Nqsq$4TgUG2Sx<5#Jnmbsxi_fYoojQR~~
zC{20`MByIK2DA6v4QYKyWQsA-WN-(;RT}c%K{&~x8Rgs!sp6T3qjVyqNl%9;^u|VH
z-MJgm`oPE(XE{wuE~GT%%_WUH$f<fKDI|SCMFpg<LS$wFTOUV?_R4b2=V<;V?Nv%j
zgMs;6o~_8dbCVkm8!c+9$nE7Uu9KntiuNjVdpXndG-#g-47MBci`7z^^iqhV>h1EA
z(U=?QxgMQiruBvBW&Xan-O>M7$Jv)*$b49{AP(80T{x&+w=~!n`1hWsZIAc@=syh{
z*Td+Y2ekERZ+F<e0Json>r?MAT<-$Ay%X&32ffwRr(V{MBf<Vi=rQgT^ca^Nf8FoY
zpNu;HxRt5nX1xq{`jfx@KSKXk;Ew3m4`|1;G2~kSw*hXSpr3}k7<eCWMS?!{M&kOy
ztxepEfOb5U&|6)7>eZuPBka<TZU1WMF>c1OG$Eebey4t4jL$B>e!%{~(ZDl-=K!04
zGl0!NJN|Qjr~c8<|0}QnSO%;F4hI&a4y>c?zay@%OYCjNvn$ZnufaT>&-vQN=x+(^
z4V(mQ0?q>3_NHxT`riS(54ijGMsDl>)Yr(b+tFYiI1Shg+-xUvy*Kazplz=o_O%Cb
zy-Rn~uMg1HZ@;s-z6o$!V0u?0KLL0v(C&XP<nw@wfS>MZ`lt3XSOm2DzYF>Mz>k0<
z_cr}b2c83*0{nP?*gwGFj04fO9Lz@@X5)H4)Zx&Cdh|KS*x3QNFR;(Se)|XEdJb3u
zJOgNjlNty41mHEm1;GCRzXW~_{0?Z_`v>F=K-!y$_C3IdfR6&{&$icAKgRb0?AZO=
z4KQ`O8+Z@!e&9mjm%y)q-vWOHjyc5GIS+U-(DpYC^69{(z_SOM{^Ni(z>9&;A8xL{
z2mE@FY1{S>Il{=l0xko72kbo9T;C12CvYF&!N6I-$AC`(ZGVRyY3$wkSAz?IqcWy#
z>(7UN@1xE2Cyq7v32^(gX%_<R{=JVf*K@!ZfL{S^{YLn?1vnLcnt`_dT|<oh0mm5Z
zhdjy?@}XV}^i#0gCBgoDtZUY1H0;&^>w%MjX{`Iog!MlidUJsDfX@N#{0fUqUd6!E
zfn$M<z>EH7^qPUY6`QtgZx>wO1Gq2nc%ZF+9PA7Q4hN0^o)7F-f;fTmfVRCw(0>Q`
z0dVaE{huLkcbu`mHt>Gnd%(+%_vd>#u3rtj4tNXDj_(uLIl9!?8x8DH=C{8G<og5D
z!1K!e`d2_c1$Z^^THuYqCg9D$+ktlj?*~2rd>r@`@Oj|Nz-)y-o-vS523`%kE<t}f
z<b6&s_WA*b0quIGAuj<Q53ESgKLhfyz<S_?z-xdv0sjTO6L>GMrP9Q=82Dk8KmXyw
zjeIh&33xZ~Uf?glE+-oOU4eGKx3$sV);|Px4g(GWZh4aN(+k)e*bmr#gt@*k@Gzil
ze=zimfNkkFVIQKs(cO)o^MJPfa`>$Vo(jD3WYcdta5nG>V3$+Ob=%&f=>Gz+E&X1w
zvn_BpVChI>{}kYO;BCOyfwun((SI_qE&Vj&q`gMe>86BwjTmL(`VROba3#==r}<Q)
zKj<`r#lVw*w*F$|^{+FGely~DEFqpDV~k$=vkZ0tZVj~U_dna{T?fnqrvYaGUjo|w
zKY_g4-;JH#z*#_B|0T%x8*B6j151EqzzU$<zZrEHj_WPmjhzpGwtf@z?f^Cehn{2l
z+4_&*`cuG<fvvzD(Ql80{@+0_h3kI?4oc9khkOaJ6*zdDi7yRo05$@v&h^Jndl$gY
z#lV|^Gl6#l?fj{KIj-k{>~HHc-sR9+0bCbx+WIY!zXE(6_(_6(J@Q-(TnZdG9`ylE
z0NVa8fc^yF^}r^etxx-(<9aKweS-a)AioRP44ea;3w#8)5cnK$F|fYY<TDYNsW<I%
z;O6I<_IAMO=bQFDz`ZXr?L&Z#mzuVn-|vl={!c<2ZP~N)we^p#F>xIW%mR-CGXA5H
zhu!~E#PKC?8Sn?-wwD<Dy8-R~H=!<f0q+Ao0(>0!6z~IJ=|p391hD>c)1C+%-)P#C
zfOi1@DE~_lM_cyhLXY{_`g=_<ab|#LUuN19fNjOUKl&XEtVO@`fY%`J8xrzAVUqDT
z5;z+8AaFkLNuVA7V#t>OKL-8){0X?$6~@onz)OKsfO}0g*9QQfztXf<*WZ`$vuri}
zuLHju0XGHO{+JK#Gk*5B^{<{{;=T@eGjKN0)?bV~mjFLv-d7nr?SPj7rvl%<I-x!Z
z_P&6;)v~`a?Ck`!<6}JRPyJ03^xIE0acv0f0_+Lw4cr#E8*u0~rr)W+JFhkE2Z4`X
zXWGvLzX00tuBrdM5l370HisVbvGr?kJrDc_`K*IH+KT@Y^qUC$3H^TszKOiwP0Igz
z)F0Rd*b~?rxGivJ;PE$@ekTF%zR|Sj0bkCW_F~|#Ks*2Tu(u&_b^Y%F{kH6F4n5{$
z>(}6V9{3sZX@@-8ivJSyn+W^{{l5o(j=WnF^6%PY>eLI^8@LN_PvD<{hX5x}GyR%?
z*WGN|Gk{goO?x!(+?l4`2t2CUwC#Gd70-*X_ZG0N{(TWI^Re~s!S&g|Il%eAuMzJ`
zpxuA|O(u^gfD3`|0&V?cZZY~9U=COceC!T${aK*h|FVCX>sJ7;0^R|%^}F6~^t%JM
z1oi{k`t;iZ|E~Z)O^Bxw<9I6YOkf@GAHWNNmjUPAW8zu}{N_H>PR%m7&1{4HfHwi{
zc-Peb6vWY%y>p?*d~E&si0fHk3vkQ(P5ixq+XL<X10XL0mIFrsPX_MxZ)2}NaPb_|
z{t$Tj1Ey`;KN9_I`@<n02W$Xd0{fQ&ZF|Q+uh8mWi?*$QA>x||ya6~3XzMed|Kj?W
z!0&*+0@q2%w-NFwz^i~Wfwu$i1I`BS|DdVgK;VbKR$%Qz{`w~QyAgJ$0o(Hb0Q4RK
zE&$r~VLr6a_}SmqKVqJV`)FVRa2U|mzXEyQ2y6o00c-}&0?q|~_OS7@0(kIz(;fn>
zdepQ>11|^uQT`uA9BtW~0X^np>u-SgcL$#Ph>3d=u&wyFK)<bk=cC`n!12htJ|X{`
zAioRP44em?4}1dnJh0v4Ce96k$2?)$#{q{fFzr);6M=UAYwG_M#L<?$InZN1w*FR+
znK%yyPDVa6fo;XVBl_(Mybk^Hz;VdCHYtC|?*iTjd<3`v_#faaz#dPTxcdTce#W$$
zfiFL6+KYjEy<pk{fF-Y)wq1{#U_U9I0SisPqkwJop9_1;$JXEPNn`Io-~ixY;Fc}s
z`nEv3{{qDEH1JvAE5Q9;HvJC++WpT!9cqE~zzM)c;1u93z?WY)b{7NZE;8+fz)RmS
z?Ww?f-ZJe!%Kz<%qb++4&|^NfKI314JeC4~0owYb-ZOce1{?$Y2e1LS>-+wEGmsw-
ztOBn0fnR@5$PWM>1Uv*d5V!<r$J_2hW49}CQ{a(6TYn$O4*(8qLw^LWj|7eeJ_2k3
zmM-z<_YYie0A2~a2RIu@dv?6EpMm{CV3NOfxZWAK32<|uZNCrXI|BCv?h8B+xVru?
zf&K(wBk&VoEAXO^{PnsE*WU+z1Z00Z{@J)b7x)<PX`ro7|F!Uc9xy4MKjC^`;9kJ~
zK->P&kRJ;y0<NyVr2eI_Qw1CjwEd?(Hg)X`To>3KXzTZbyg%?Sz(EQ6|APE8;4<J3
z!1n(&{)Yi=f6qhjMc`Y&#lVk%v~Tyn7<Qilz6g8+_yO>cr3v#3*Y5$&1<nU90Mfqg
z|7zI17I*{j@dSVWh5S?C7r;$E@yAyL`E8$?>+^x{eP-Gp0zU;V2d?+I-(EM!-vKTK
zKK_MYe_zNC1<nCJl%T&D@=jkGdtHFNfp+~yLw+@|3D}&VpZdz!tpU~pF9O>3*MWRv
z;1<BG67*XjKc?0ANdt?3w*5Dt|32^&;1@t!e}236q39{#Gr(Vg?c1B{>jJv~cL3V<
zR<zOI*6+K_<h2WMci@L#8+j{mhi^>V?mrOv<AGNK?*QHpd=U6E>~(;>PQWb_>>rJO
zSzr-x7|@O<$^Lo?_8TCd0K6Qy_O~YAF2G%Zw!fz!e;)WM@I7F=?~I)ez)gU?zc=#T
zfrkNY`@<n04LlEcA@B;|6yUYMn}D|gTUPkTZ#w4D?Z7#}1qt)374q+aKLPu#W#;K#
zz{7z@14jbw{C0%g?!Rw>KJ8_3y#knI|BxR{-iHGR1CIe73;Ykzj&JObrvG?g9q^-{
zjJy@N-AaFbpMv~(;2XdXf$e`b{W=0S1@`{M$ae=G2DIZF19>g*eBedE$-pat*8`^m
z?*O*^>W}}Blo{8<frEj^0FMR!2WZDPdaSYgEUw$1Uu}W>OJFPT2Vi~1^ylYWQ`q17
z0Q&*=0}cQ-SvV8zX5jrm?!Vkm`(Zxs2OI!A5|{>ZeYBwcHgGZU6Cl?M=jUk5*YUu5
z;3dG;?k3;%oB79M8uZTSV8*2Z_<Bdvw(GwguI~tZb8Vyd0npZ`z00A$1lTu0pL#b#
zZ#4Ak6ZEfv-s<X8?_Mj86B6t{3q8h7J;r6{``+)={|0qFuCuA*-+?Et=db^9os7H!
zcnPo(XvcFp<mUj-2VRt*-wgR2;3kNt7--w4-m}o}yRM0^KhV~H40@}pPrb$H*Se0e
zPd~Q(@2$8Q$KZr`)<s^-vn~Aw=uZGP0;d97fG-2z0HzRkXJ8kg9sgUuQ~!48-vzu6
z_z>_h;FG{P@XI>d{)glG$wbucjD-5VgL&F}eKRg60LKCAfy;oY4UAqlpzVLsM&|nU
zz?*?vcQJBX|Ff<}e(5F#F9%KnHUT?zGuOKS#{g}6jo1%wqdxY{J_-BhlJ3UthMODQ
z95@R&ANZFo&2`(~Ajppao&>Du?bmNcTx<0)*ZU!^Llffbw~f*JGcXG*0^0FRg?ujX
zVc^pV`de&k>~94;5V#1q)^_Il$n6c<{&I->D&V!id_q25kayp{#$LJ&{e7T!0B|7i
zNZ?<A1;D=nOM%tE5y0`lb~_sXeStZko!|eJ{|gX@?eBZUvDZ!}z7v4s_BQQ0;6D49
zw%z|T=(hsD0ruY6uRja&1;D3)A0+7SyNj`N1n?+e70{07l3k7dOyHfsSwLI=#NCYk
zdB6*SlYq8<!R|)?RN(2r8lbIzVLzka1iTq|H_+DKY7e8|AGkm82%xRs4EdwL1;Dow
z^nc#d*x!6FgFS#d0B!q6VBC)a4gnquECiMU%YY{WPX(R>tN~sKyaIR?@J8Ud`<wV0
zfpZTuZ9Be`_BHZRz|p`nf#ZO6K)Zhv<THS`1MdRP20jS1`@aPFV&F%>BmV4<m-=s5
z`ahukGq5B2cL#2nU~j_&y=@Zohd_QDupIc01pUqjm^{7*rZEmffoB7g#%C+^8w;!f
zUJYdb>k`J7{db1`p1^&9`vDIG+T%m{5L{0Kk5AB_fjTq;X9MRY)aOyey#V+u@D<?O
zz$Xs!&xcnae;v34_&IQ`gH6AUfZc#s9cJXsz|n{M?SBgWuYk*d%Yi=uQv>|{H-~&{
z;GV#O1pWR4jh+7j_e5MLB*b?p^o|7PfHQ!%0~Y{kFa1019gqH%z~R6Vz)?VZ{!l(1
z*Xx0oCg?YyE>nOrfXxZ@IvshO1w04%58x%h9)tYja|Glk0xtxvb)?bn4IFf|zkdeu
zLSQLyeu92KoFksZb^CdW{k)_J=dF7a&SxLOPWvGyzRtj&z}~<<Kzn>jA>Z;C)4vz6
zH_+C9Ic@Yd%NXnp+zDvw9|8GMz`p`JXVD+n0$d6llrz_Dd$*(iy};uNjNVY--srb~
zLjQeXKMh<T{n9|&-yP7qANU|}0npZOgq=Ka2Jnpp`%5AJp4Sog9trVhAuj?}1K$NM
z1#Vwx>e3(hF|ZZbv&diHX3O3RwB_f_!c_YJlkD5|U_7)#|G&VFt-n3u+6y=UI3yw7
zBFIMr&jenXpueKn<gpT%DlzR2z|Oz{#~XPX*s094?RXD`{^7u5fF}b}rKaCbz(v46
z%AOrh4gA&v8-TX}na?QXH5l=n5By*4+5P9h?_A*Hz{S8O#KAhPY5#?^V;MLW<5mPb
z4p<Hx4g9FwKfawSjC?)dhQNJJF!KF?2LaQ-9Iy;H2RIM-Ft7!9%TT}nW7_by4*Ff4
z(7zYteSu@!(4T;K8i7{>r?HLxN%1~~>;C~h56oAZ{O$tI2R;FO61WK1zRKut4xBjL
zU!N7QzZUwP0IUX%0FDG&(<3zk_QwFz!;GIopsnAh+Faii*b2RG6ZG#v+_QiW0LPwW
z?2HH20xtpH2Yd|p8qkiX_ld^Nw!j^My8>-}%J;zaeSrHV=<f%=2LcZT4oZlJ@}qG5
zufS}A{<n~?1a3aU<g*oU4`A)dMt(W4!zre1=l3J@+e2>yV0YkNz&``4fun%7y`GS7
z4;&0EO3>dParFc41w07Yd8CPZJ78bnUO?ObK=i8sHbd{t1pQ;7R{;E*)!(*HyXQe~
z7VuRd{cJYMU+>lJ|4~A`r@_yez`p|-j~y@NdFV0DZ_yqLKQ|@#YpcKQ?{e6^3V1D$
z{%!pOQKtdGfxs4E)2XJ8^MRvJGwq4MMqnPe6!-)1kMdWOV6Pl;4g-z^jscDZ)&m=W
zQ-OATN&POc`b|sd&p6k#K5xM8a@t4UJ0;}51bRz>pWFSd1Zj6OjOP&GFd+SKoUQJu
z)$PAeLcAT|r!#PUAmg$9QNB0y80Up(b6$Rm_V4X)>u(D?I|FwI(!Z_0<!G}WwgPSk
zyahN1I2ZUZ@bS}4zeT{W&oph@{?5>E%ii&@v-ugu&H!LP*c}ag1o&QpzodAN{r}kh
zfeG=xjrx2D{1o^Lup{E$2)G%r2hgs!-EWhGe!Ua=GtSkmH^*r##;XhR<ha@K9Rt19
z)!%T8nGYKSw*X!WYyv(Gd<ys<;48p}v;6b(3&;;X+gu+EJO*gTUjlh4uo5^OI16|`
z(C+^V<lpl8-;LkVz%jtHf%U+R#`@#$1$iNGH1N_k^aon{V-xggcNDHS0<Q-)0UrU{
z`922uv%r^tZvfu}egym+_zmy}V2^Po|AT;|fwsRv=NP@CfX4t&1KRo*K>jiCGhj!{
z-w?Eq1y&{ar@c#I=L+C8!0Ulcz!|_hfOdS8--qk7fh`I8tdAXUE+O92A+G^806U*+
z>bL=LIM9x77s&So9t12((4PtUoxpzs=O*ZX4tXo^XJEVW{`Igo<o$sIfrAtD&w_kB
zu#S2)#(rnuRX{ucX6VlX{u}rZ?N}X}VCQDwt-!9e#^2_^{=fmiGk})4)CG|L6F3pr
z0$dC{xX#!w2F?IB1K$MN_E$i@-9JqKe!xjUTmMeT?*Tq`p6T}_(AJ*|y@kM$&>Nkg
z-}iiD?>1mFu<Hdz-W#~bg{E!$dl>rjfzJS61lsz|uy-%;A>iXcTmMk_9RwT(90j}p
zcyNP>?=axuz%+38f12wD0p|ms25x()Kflz)Mt(7n_WEC9<kNt+0qyv^K)waA2XLDN
z{h0~zy#TpwzXEZe2y6mQ2i^l5+-Tw*3*3B?Ki|tQGx9vJwGI8=u(LgIC*VZjWx&Zm
zJN`!@-{}hDcK~pQtNiiR;QHdp=K6Y53?}I>g#M<`?+F|LJQjEgaDE&1c>M|JEd)Lb
zwDVgE`Paa$ul((Jk44@St~Pp2z$>pY?Hho*UvJvBzk8wI0{k$cfB$Pu|MP)K_MXP|
z=YWfVZv#IBegb^|I%9V^(2lPJ@)5vmfzuN7tA3~cmdJNUAoDyJ@!9cg{X6v!Mcoev
z4hBvFHUam!(bQouuozehEC-GTz6-SD%fem}@Lu49KwG~G@)LoKXL^EuE9A?8f4RZL
zeK^pzzd7W+fc3x&67+{ao&`<?UZ0@94DxS*KLS?*S+6k(^|JfbL9a1E{{zTB1}+DF
z5Bw2$Vcyi`65yr4slYdz%=M3eeW#hWUB6<;-vf5O$>@CrwDq^U*<9ZNxGV4g;K9HX
zfQ`UjxBLCyiR%l2Ex<)U+kcYY7SsLqpPXsrF9Kf%z71@-&0K#QX!~n{-8X@60~aUw
z``5pWz4^d}z!!kF{VyT!G{f}U2-qEH>yJYnYk(I5&$-+5ZwAi1-?Z)i1$P+v$-q&-
zGk`nYX|5jyECiMTZF|qb&#S<Xfz#(0JF|iReZU`2Qvc-%@o)H`vC{?E4S3f>M!pbu
z_#=LQHPElO^jpzh`(e|s3fK>JlI&eP*XU0MUNX<LX}_)hXTbhN^G(00z{h}#fcrga
zt`7#T{g`Rn<MH9+M&7)@;Cx_*C;j@jJ!RyXg$7RoKJm0){~x&C3LFi+B>nGk{RiML
zz;&K6_BQ}_0d5JL4x9}<{aJrJO_1LVyaV{)b4I@fc+-pi{-?ZZ<fDLf!2be21AYly
z0le-t(=QLa33wauPT<|Z?Oyl$pAUOa08`NK0Net&6Y&4l_9gI9RoDLyQA9-zsEB)T
zS1itiy>$soSY#WLsJJm1G7~b8WQNHiRFqh3-D^~=Xtl=HU8`|lt5H#?RU1)J>lUpG
zYSmbGMfjil&iA}`@16GuvA_R(lrQsr=bn4+z2~00&ckl+?$X=xC&7+WfEnN`z}JCq
z0oMV~f0gaO2zUwbO5oMNYk<4F7F^yo=(`8l1^f!Q4btxpwC(RMy)8c(b{qqY1Mdeu
z415&08o2ylZ2uL&KLc+B-U7TG*!T6|^0tURU<dFe;QPQIfVTbJrMKlfrm`JkPj~m>
z+HntVO~`$?0VvnLz$w5Pz}di+Z*ac<1b+1<f8Pjv<SqWb3V7k${Cx%RmUsC3y}&1d
zcKK|3B>yVd5hlO8Wp71@{1&i(2cX#d0_?W+uYsNK0=IdW%drD+5b&&jGk+2Ap7;3s
zW56Zv^Y^oX(?8(vvw`#1@%KjHGGKT1?1B6b0fx!{3-*irej)NJ!FK?~-iu+kt-k>E
zEcSN7&;A?YZ%aSoa{dN*F7Wq2TmQA-uLIr+YzvWJ1^#(p2k=GUE5J8^c78{E%=R4(
zoCur>wB_f3PXMcdwIT9nfj=L3A+QCw9C!uL&aVZ2G3QgZ(;39Sn;ihO<+G6cGw>$h
zBfxgx6F@utdhlC(#`*06+zn{U7lZ!{_#?3Vb51`O_{D!=zolRj<c|TK1Y8QV#2wLp
z81%M4Z)=Et$$vTIMXoJGe(x{Y&gQQeR{(DZ-VOZlo8a<Xiu6|lzXomuu0gstLed`$
z{l5er4QvG3^&$4G#`DzIY}c=WwtO$}Cj)20j+zjAn!#TJ{3Gz6A@UD>&-p(Nd<yt1
zumjk(p5@wsBY)!Wqk-drG2poy__<x4Lw{iY7r^1bUjoMh=Kv+Y_mOWW^cVbp(r??l
z2zHzX{0;CN;6=dOfX%Q^?EJsd*9N_|zLFogK4$`t1D*u5<=;WQy$Addc=$%vQw%J0
zdb>&Y2Oa_(4J-!6fGdHk<asYnKN&b{3;u4~GpC#Mw)_pycMmWEy<z(6@cepUJMeX&
ztzYDqTk@+z<VEk_@VpcFA7B@7$KJu^-v#_`zypBoA@c8m{}A{s@XIY(|F^&&fLm_G
zd?9c&@bkXG_Qa6x1mJIh7XX(7uLj!ncN+Mxbhi8tuxIN&Y)|{vjH`gQ{GTCr2QUV?
z<3r?~ZCGD#U_aoCz)s+zZG-FMa`0CJZv^hN9m^L2TYxKs?-#6ZB=|da<man^XA}g>
zPlJ3puylKtn*+4#=N<500ypfy@>?LEKENG-cKYiee=G0~;9WplemU%30bDbH^IHqF
z<^K%1HsI|3EPpc4mj5027GN{<o)w~h<<6|{m|YlW1M7F^@3#KuApZ(*XUG=<ZTTwj
z)uI>J7$W}w_zvLGUD=MafwulHA-5j5Vj#<31+?XRgYN^}3%Fm1d=`8Tcsg)ti2S{<
z^M2stz-L12A32EaJsSAlZv1^6(6&DYxiVlCum)($zXX0QuoL)Eh<r2J{Vd?Qz>W}q
z_z?Uz!1cgC?8)W05_lEx8sOi79l#HOw*B9N-vAuE7wbI`SOV;|5A$~VC3`b}7VskA
z3gCUfhk)+^KLy(Q{sjIe;A-I75cxA;*PMgcj(NbFfe!<pJA|LF1@3t$e?JUZH-x{R
z3havTciX<3_h<gz0~tmCkc0XAD?mHHfyigvFIcY6P{yMUXFMNh=hu7yKR*|EKJZeY
zEq@N|x)`_|cooo=m-5emeS=_MA<&lp8uhXsI3>#VO#{Y&DPSXTG4M>_slz#)t^X?U
zt-zarw*v12J_39K_zduE;DMuA|7c+KnBe^W3;FLy@$-`)7bfpOZd+hK;2@w~o-p}>
z5cvrBUjmN=PU)fi*bw>I;1j?kFxx}<>Ja&6@aF+90Iukvd`pPDw9^JWm-amX?KT7`
z?P#~h<#>KI@D|{0z&7ArN3cJv24;^8_MiJ8|Epi|^CN-9z$w7<pr;ag!t^ym{#@Ys
zz{`PldBWr`3XyLE{}AvI;OZXAw};4gfPWqMCh(&k%D)pL@1P#H1@;3D0^0QxCSMRD
zFYP@U?KB%G?Plx$2ztH%_JaJjzzFbHKs$W{_|t&b0B;072mB29Be2g=>~A{&FBr?;
zZvx&sE!bZt9L;=jF=NL##!rB@egB30SHSOpy{+^wg{1$Xhw@_Anb0rg5j$=B<|5xD
zuo{>HJ^*|SXs4gwL;0Iv*PXz-fsX-g{bBO=hseJQ{vF_Zz^)$3uM3g?4*VAIldXXL
zfwui&^4o;S9}K<-I2>5qL;2Am^3Hhn*K^_De*}j4du$@hR{-yx%HJOX+T|_pq5Pyt
ztnYZ>-mvF@kp4Hdhw}SOXZ;5OTY(#AGaoqt&re~r%d=z_^A`a{eiYK#@@GKqOyIH}
zroX$K^LrAw`Xv7TBhb$8LfCyV@G4+yNO?X5{{?Wp$ju2Z|H<GJzzlFeoaGM&z6iAK
zYlr+w;40v=KwG{OdOiStA$lt~zh1!WfY$?W2etw41KRpu0RIy3Bj6SZ);|b11b8@b
zG_V*rKN(zKv+?{AU_m9z9R;iaRs-J#7XBaUv&)-<9gBfWfJ=ePfMTz0pRG^io<zE^
za-Cz9E3CeHs_z8UQ<%QLz^+?>e+9My?*WSacKL+=ZZ5Z*ltbFdmOmJB6Mzo^CA}@b
z1okflt^l?H+kr0vUjcRk*8z{OV*9edPv!;tW8W0>#lUMIe-m&u@N3{u=*j*c>9fl}
z7<L>CJQVl~U=$c;udPqy79w2>%F$gp`=WfUz_9xJU-iG%js8;DeFE?#;2hvwpp?%p
zzwqJ@QZBn4rJQ#9i=lr7@J8V6!25vD0@niH1a<=71KRr6qx>6zdsnkR4gpR8RsrV&
zYk{W%JJOu)U%=-V1p9kf`ex`k7Z`({rNGVAFZr*UkA4L^!t4o?zb3@~-q5=(a5rEf
z@L=Fkz{$Xwz*69GK)XFYM!w$zd)II|<^Z$67T|K=<-lu!N7e?{|2*(%U<P;w@FL*d
zz)s)`OM><Fu48^{;7-6@fO`S=1s)1K95@<yG|;XO(c8~TH_S>m0pF(prvb}>Cjo8!
zqIYxcmVCwT&CRztB>&YJuD><Fmw>MVZF~1!$nwL0Cj%2eTYd@nbAY!2?+lUu5BTqa
zgL9nUfk0dTSnx66LSR#fd^`A+z~_K3gvejn#QEP1d=&T;(6;A2@IL|jp33QW1lsaL
z!A}B~0#68$Uj+VQ;8noCgvft|e)A1*yVKd8_s(Ga4mfEkf1e7R4om`XY3Ap*1KWV@
zz>k5S0lx%p0PcAvr?<<Wggp(wD}mPmZTXMDuLtf8`ws@%^3%bW0*?pAf%AY1fX%?)
z18se`fqxe`8RcsRhRNS&$<KuRiNI4r()UIEOZyZ+&dyib$If5U?*cuNes}0O0C;dn
z{)b!nOF6^xI~(%n1OEWL6KMO3ZSQ>e$tTO$Uk0AVxayo>|2g${%r6CI&*twfKwJO6
zq33<zr@+X$oNgSj4443}0A2&!@jRB_6?g#f5a7g1`T6GNzaDl-{?m|;o&IykZLa)0
z=(z}Z$wh3}6+m16^H%we{yobt0owAzF5u_WfpdXb;MKr3;9bCnfi2Mc5^yc>Q{XwU
zQ|zv~g!2olU(vT5=~n=+0^0UJ0RCxUcly7DJ-=*WJL14az)s-oi~0Ftpy;*p7y0*)
zZXNJHKwG{q^bOcd{Y~)03xToA*sjxow*DI|xfPHb)5_^=`IjO8D)4<^@8vAtf%LD3
z<o87n<<~>cL07Q;Z~nyi!8O75ee?(Br(Mpt1b7ATffdYeu0D}}1nJs=D}i?XyaWDo
zV0ZS8M7h>q$#%W-N5=Q9a)z}>U!;@tJ6^@<vq0OvJFe#EcLARUt_3bbzAZpI{pudd
z?+QJ80EYk%2aW_D0hD(ADx`fI;g^emzXM+SXD(Me@cA40`-{MT0qyd<dL7Gk0t;^7
z?-8Ia|0VQ%4g3i>`X)|y3~)AZF7R^THNXKkv;6MBy?}#(H{Hk2H#h%npjYyrh3|Iy
zjj(rf<;DJspl{bZ*{;2Tw*Kd>@=dyf<<A1z^21TyF~B2%6M?q;2JqY6!g_}Sj{wdA
zE(F#CPY1TZj+cS20K3FKl&b*PbPwlq%)N}ZJ)&<p(ystsCHkxc4}xC>>`wp4TRGox
zx4{nJalqNYn{VgmEp4#VDu>-}=i&LQcd`7tKq+UK|24v%-vX}!J_QuN=uZC=A@v!i
z{{iT?s?!nq(=B_h50Mx9_C@&*1Re^s^-ltSEKuqv%>D%0V?J;x@Jisdz#D*eevg4K
z{42N15a3b3V&E}AY44ZO?ymqp1&SX_`-|OS?JN40BmD~CRY2R`2f?obcBlV#)Z_iY
zH-O&&_kbUc2A18;^&tJk&R?D%54n?oaiA@~1pM!SZNM=7s}P5u0j>dl1pFMh|NU%d
zF>o?41}p<6fETS~dD}iazc;P?B9C&qQ9wKW+dY(@{{ZXT_z+_|^hO@z@3AKtt3&L4
z5$R8QkmW7_-V5xhK9OGuJ5~dq4Jq$B@ZSKtvv;?L*-oj)aVTdc@Uf6`hqc$t&D8g|
zcFuRe<BUfE?e_cqBb#Sm1@zhJ`#izvw*l@BJP^1S+H)|_PQPal<-dmf_rTuJyESkJ
z;Lbqtqoxplx(@t}z&n9I0Slhu@~!>{<Fml$fv*7%d5)jk<$Dx*o&XMpzC(bv{68W0
zqLu&7A@UQ!PY0e4yfj4qMDTNfi-1c)<U7HC1Uz^Zm-BF761WIxm#4e*w!Gx?CGr~s
zy^})p7eCGRlmKr5-Ve0(XTUE8irq^>^gjx@)xZ|WEf0}j4gNXcA*<QG;XvD-FCh0L
z@Epip5F-C8@MD3k!0STfXFS9CmjgS1F9B_P7J)w-IOthUe*n;yci{hhfTMv&0Bw1Z
z+ZNAbz~e*Y>%gB5tb-k=h1jzl`r$xeAuz0ePQdd?z*)fOpXYL~0loz61pe?(eqPYQ
zcqQ;=;La~FZ<l`>_!EH5z~oE8^76d@8h%~~43l35edh!3ev#8X1hnnh5Bi3Pe#jMv
z$bSqw4}G2W&IZ2u?_m4)U(0+Hc=Rj$Jr0}?%m8iswtkQ0cLeSYJP<ez82NzZ#sZgr
z$lrgizRA#Qr+)%^R|1~}p7<%}mj&+E6`cRHb<7`T$zO`^ZNP6oW4RHZGbVuF0K@d3
z@-fTT0<QtK0WbW7pZ^Wm4lG2zqJO6l`@7TsF7$m2wCnF?@OK061&Td`{=@d|3AEEM
zfL(RKdf<rva=J0VBY+cueZS=Ag}|9#@poJQDd3a96mTi<JmC3X2dBRs{Jp@3fiD1G
z1Fi#h0e=Mc`-b)I0Ne+7^0&eH-3<P|@A>&^;CDX+%O3|m39JS-0T%(U0NV94YCY?H
z7T5{g_9y0d0QLtK0*?XC0L}v1`Y!>08Sq-*#T!`vO~8G(*wRkwem`U*%e4ZF9hR#A
zw)EofD}V!_CrsZvmYzbSive4KHvu;SMXzmS8g_I7MUU9khV-uh->~%B`WC?cI^c!C
zBe&#o#(+-&{|<Z>xE8o-tKj<l0etU1{JbCV^sP~kz|Z>yr(f8g`Fh|=;O+yM-y1j>
zI0Ts8iJzYV{0wN9zYF@l0!EO21aKnoL|{+T+w$$uvl7?|d=L0F&<U}(yYzN`gLme7
z*bmqMd<OVD@CD%Oz`}u?ZYc0iKwEz+<gWvE0^bLI1Kf5O*4NYYwtN<P8h|ap<-qHK
zZNQ$Ux8+|zxnBhCzbls`2W$c^1}+134Pv<sz$v>0mwz(krvY1l%Yioo9|GF;b(h|j
zFNQr6fEB<buo2h{>}h&iem=^*8u%*kgOK{j^iaM6@0$^Q-M!zc5bwPy3VHvHGqiqj
zZ9`%%VMC6wTK-m-X-Ey7SJyanZezM8`HOVYalch3>Z_fh$;EY)lCdGjPnuG>`gEqw
z_!6gQxl~O;CBWaZH4V<tbX}VMY)CDlKda~qJ<B8$4GCvxsybekOVp;~)k#V#-$-d?
zWjwX0GL>zJlc6;!))7zSa*4&PL;gO$k`gLqDreH`TA8R#SCWzp<wt)Eb&cQ*=hoLd
zLn|}2wW&IKm`u%WoJX1xb@Nih^Vf7;mD0kG=FZKfn&ewex{fU2Csc9r*P0Av#w;}1
zp7Jxk??Gv{rvK&DF01+7MvmbAmQK414yEVcY}}ZU^j%0Vc+UV<z*7Rp>1*j5P2abp
z|0VrRJF|dbWF&uA`6?MnKZt3km{94K^sgSn4+Oug;P3J$%#*LqWcu4i&HiU^<wt^N
zp}=Cl<Rf@2iEq%-$J#i(U^{<xry5F?*bmxC(qE)(iYvI2pLlo_f0z6vy_CO>9_#4~
zKWBNt;$8jp+tOd{^oxj7b<&5$oQghNLxQ`57f^ZWpL+VDr*B4j!Im9$X(}JS3Z7x5
zFBr)A3$E=gCFg{aPPCq7rC*2OC%BCW0&V-xLwadXXLm>d7Y#EN*!f>#rC&6i(+e)&
z!Ax)G-)f~VIGH5`4?EdRFYPDnEmry?9^mwXgSQe%My_3-dxZX~Oq5z~duo68VcS!z
z=Nw<u{hOWtYGPFrOZw4IbNbOwb9!6e{{9lBRW?cbVx%wL%=E8W=_`=F0_ocal7RLp
z)c&si`T_l~d|&b}e3R`je3R`jfMf7CzyF9IsP3lQA9<V8N8aWX?etv#Ncjc7qKA5V
zr<2n=ot*wv6`Zw?q!av!p6Tg3KjieCA9DJR?X|@8*Fy8F)0+}2T~hw`uQ`3&*POo1
zlDE_MrSvNQh$ScKhgiIlRUg6rNYAYQ=j+J7wHyB}3`u|5k8J-ps~JVJ<SX9~3rW9l
zB}a}wr~t2h?DW43Nx${(D%v{EvTwD-^jACmM5MRt@4WtMRCJubwz%N^A?XEYA-yMn
zzl{1br$4JqB0E2jbRvIpNcxAr?(d4k9uUdk^h!#6N0;30b{_cWTNzqk-TLriucqLE
z2RQw!M>DCsGEM)B-^=)?S}7?1mhU+Ic2%5S{<QNKJ_vi0butgqpZBKOhRknFzPBUu
z7L(u7k@<+pZ{^55!{qxoGJi1nt-bky$@g_+yf^u6yz$)Rw{>KEHu>!w8Fx*-pCjX`
z$y0EV@lt!xn?@8F|1_G~X-7xKHIpxJWV|x@{*H`CCO^QDamM6#a%B84`JEjZ7fgN^
zN8-K74|F7sn><aXBtB~odOL$0iMtw2<=xGZcxv+YtcsuZcGPS<|I^#qGoK`;x3gCW
zzjp|~PY7Qa!Vk{lRsY%2`C_B}pdS%i&`e(ZhkKvn97udGs)yAWSET<Q5u#@n^h7WZ
zwDojYdKyFYEQ6j{h@Mw1J*^>n)ci`7D;uJx)6(-~h#oahQF>ZJ^sKY=YzWcQj~YPf
zX$#TQW$8JD<b&F46!dh2==sjlQy!ux0X<zIdYo@m<o4tC=^=V9gq{Mt#Mo~4zLuWb
zLi9WUJ&_PS1(u!{L-dH>7F+TklDzWgLhyxd>1Yj0gP3ZU81kJ7ekk!l^?x+vTSM%e
zY~>pd(NhaO^3rj;-eQ)X^Fs9e33}vZ(zc%2mY&B$^gIVW-b<2E4^p2MmLBzP<)Hde
z@5WX2DVt5UJ=K<;{xk^)(z7S=y`21U;Sl1L{$gtr>nQM9i=PF)&En^|`u#Xh?%{r#
zTt0uCx*B@AEIsOuYcD5%ybCLrx@*vj#?#SQ?;%@ft;^?^S3M7^pLbpPLC!j?8zerf
zGoD_~&Q9@K?!OP9V29Imx|fqb4wkt5Ag2v+L+qIa-X0(4xctse^Yi<=qN4vo@P*GY
zA49&6gLhsvd9_UF<>Zg!8;Bo3?Hk#T9Em=MxPLm%ezcxYe8FH=d_4GH62C9;UE~$&
z!?S<Ksf2v<K`g%n^1lY(x`g>Itn>d4zT;5lS)1d04nFoK^P+!HAGW9SK<35I4<~*w
z)qm^jTwa;)PKA6i?7Tqi0q;y=J*ya;e}Zp5jfow>Z$;}$w;uLo3yYxVK=2Og?O^az
z!Dm&VqmR^EHTVu3Qtd<E)#m~51y8Vi6Y^andOl;~G!}Qh1>X*Pb^(7RIi#}Fxr*g+
z7U*Qacj6ym`5olX1>d@e<sS!s8~Bzv%)bEsP2mgK&YAq!Sr5JtKUTX9<tnCzQ|;2+
z!R>-(a84#(+1Yv=>zM#Omw+$CWsoV<uhgfLc;#=AEm)qux}U9R*{Jl$%M-7F{8;dj
z^{iOUx74Q|d~5^rt;qKZ@NMuz`s#lE2EG&jsKq$&>%hxDE^s&bjrx2JK8yY$<vM_x
zMA?~r&$OoueDOQXOSvu+{y64QEaySu2Rqw4ZTnD%rO$5epN_K@@?CSde(q&(HiB;(
z#(KU(22-d)TtE4MdD-@<17A_k{Ke3JzQ{M2^4Ej!+Q|GA$UhCfa{wE%oF6+IM9+83
z-wu8cS{f;P3ZWm{+Rg~@*$Nh{M@5_pzH=b!IT3toh@OXtcjLnv_CtxEuYhmEK3D<l
z{}g(%JF@<l!S6tEOW7H_nC+4FIs$y#&TPn&6u;Exk0QU0<*E4Y=VjuRJza=blJ94(
zJcKmZhBghAe8o#_zsMg#yjySRM~5NJH1LsGZ2v&i&*|XhABkN9`Fp@;lPq`|<o^S{
z1^-CXa8~CWNCv2UI}c=H9`q~&-+m<X+oOHgfiFIV`Rl<Cqzw^OuGl}!aupF@2%S2N
z4dIU^Ud94lzS`v}ZXC=Rs{M2ISq8rOKWxu$n0BrKU-%mHXg239@Xk@JCkp#_qfFg;
zfIr|4loJErT)-8%9QC=BcvaqJw4?apHIR>8%7P!kQQj6k8P0Gg*!h#IXM3jwaX?nQ
zdy#<apR-whA?!Q~eCK6M^y1<<jUs;#6Vi^)gKtHA_$TuH-sSN{gYBuqx_+o8*z`g5
zIY)wTL;d4;%9#y5g3BM5qke87Ue!<GYW9Z+@^}*R1#htZ((nEY-kFW|f_woDh{~Qe
z{NoL;ZOIQJ;Ip5wd;{bsgYT?i{lmdmfp2eNz6AXF;0qS9o;$!lC_K(#9tQsb__iZi
z55j{JrNHa@&tTRg{B)N`Vh!d&z5s{NGw^IF@oIeRzyK>d8m*A;`i}FJ`248L=kHUk
zgnY+j)6Q?f7h^nC`w{9>NK;!kE}^{+X4+BbXsTTHyspZX-`<Jr%?(1;;(nGwz5<u0
zo<MuO06vECNb2Vk@NM^Ty^VtZ>`xOvm2Vc~-7x6SfR8n?d>Z_9;M?xy@=AREFZd3O
zFOu&78gP~V80K9ELC-MottYV_>Ynar9`UMOVwkryg8wb#TZXgYucEwngD*as_0U)M
z^Bnm0Q<=X3{FmShYni9+>3+7SEi=_F?HJdnIPT{_;+6d^_(v~Bf*<el_@cpakZ;Dk
zMdEog_>R{(-~Ey1M(_m(FrNhfH28M(U#XvegU_DE?eaC`_ol5q*RNh<&7;Bpig;yz
z*Mls7EE=X7yuI#ND!f&nSA&n?zzXi;JW9N3uTE>8`3~f}8o69i*wdQ=uWJwD$ywm{
z1mA{#^bghHoCLo4Yu58i$e$_ve$1}~{|NYOe=hG5@UMydo-BX%mi*vr@Ljmvmg2`w
z32l8UJF|<JkbYMMzO|0|m!bb`@C7e%zGDB~;2m6|ly-a;d>8CI9D2G$|M{%HAKGg;
z1t>`iQ5R;oJo)EXmY)mxv%wc)oMo?|^B&?|KlwW|5`Uf(`7c<{)?5tdJMfX`xW(np
zbTMu9s`@Fm#+PdFo!helX_wo<N9M8qouK~>@LiTad=1_~9Fz6MK82ibGv-m{sEl8O
z?|PT@(=^Qe+yp-I2hMOK>gO%+?HG^RIGNLLFzYWonB{4i=6;5OkL<>Rlh6TXfG@^8
zd>!O7;Im^O2!C4vzWFWIBjfT)@D*66OaFQsd}klFNBp+ezHDa~`r|_6J6QNXvVMur
z#}Z!%866fvzNKmlm!8Pr+z!5NXC^>9PlIp8W#)fyHqLtR74WwzWKy^v+u8oPY0pgX
zG2Ey41LT{)cVN6GH*i1Kf|q|Rz8?Im;0vt!+(5iq_r-8uf~Gz0XCS$~@>>V{EmCh0
z;#C}L?#(5Yb}56r9Z#~NXMdJo$l$1DyVBozV{bRV)wDiQpWO~%J~E93X?pB_N?aaa
zG?)$fc8rf$&N&x=FMgW!NWI+x-X8Z?f^T`;tp6X0S8<>i`wTK(?0O*E+4T+U{}DDt
zz!xL#QF8Zl4Dm`&^C~JheOlZ<X<rNS9Vc+UPr^@DfVaoryIdZLHFz5Gkz3iGGpOm*
z=S$*Md$spsUgqHiG{GozCCon#C0?~lJLZ!z?oS|I+24tHNXstwlZ1Q(aX_7OsLv(f
zTX8@u^>7FH!hN{iJJ7(}QGj**VKCdd6{~R$0v|z#8;A!>!M9@lG6eEh3y*me4X^I!
zHSmRq!!(?^pYOpJpg)Q~A9M)(d=?i->ftEjm7QJCb3F9K!OP3=OAuHe1>d=b%O&I8
z$D&7VjME3^Iuj3N{S}B;3!uLed^^@tCxO2aygjdboOsn<h1U4>7V-NctqwmxkG*ak
zNEPe03;NOTkSGGatCH&{4!#<EGYV7){u1ylPjWr{g~9oo@Q5ceUc3dq1M^92|2aF-
zMuf7bVlC$@aeD#ySUD?x7Jk?w{5woM0ekKO-)W7@uYhl_;C8u%!5Q)khfW3ZKc^6{
z>?t0>`JUF7Wu61y0{`!aq#MC^EMUc$UOUGOWj*$OLJE94=5@<Zu5-XQf62sO!QTPC
z3;T^*BHx$6+xx(ugYUSE6^NZfX`<!EE9`4Yf1gUcDsL+;k>8AVydHdZ6%#TCdrb6T
zy&&uEmm%M2tq1#(fZM)XaQ)l?`$vFpeTwaT5C)wCzH2GxdoAjr4ZOWi{+#HEv*0t(
z{~__JyscOlU|H*I8Abc{=bF47<^47I3Tys<1NdfibeZ=)E_!~;3f@B;8&JghJJ8>y
z-VPUjJC>LI-YobC){hcbF9Y8Sdn8`n4Zi(eE^ige`!Dc?Z!<pt{EwpNN3Q=9!S6hb
z?QGkD?YS5H81M!7M`dX_?|x>BJoY!ludWAQ_ygxV1M*LU@4|eh59;SL@EzFq*a!R!
z+DK7;((yj)k#*>);EV5LK8pT(r|{Un+8+6?Azrn61@;La;AGBckdN)pgp_yt5y<x(
zR@}5LD>@l`7sjuhpnnngPU{@%O7MltxIVGGasCCq<!_v?oU?303u;wfdw*sy_?AZ2
zGZXn{!57!EoqyPoA3Wyr5Yk``<lAzb?|3}>348=UF2uH=bNDE>rx<bR2*@7`KKpC7
z=Ns_XfUl@x{m+4a8+^-y%*#IKZZy$S<t?<<DHFgqTm5b>_z2`5g#NRMSMBRO!}G2f
zXXD%mdFS6uOojYv@CB{R|FVD|3>w4p0r^n|s?#}~cx7k%zAS$a^c)MmMb@X_{|dfy
z3Af7;5};|puJkxq9}R^3?%+E<W&P5Rrh#w%kokW={&euM<GB7yz~2JCU<%8hf^n+X
zuQ=akY)Fg#q2MbpK)nDxQ^9wYvYvOqSA%cGxeU3D`#BeU$KTnWYZ;s;!Dq3KTLAfg
ziGIWrT+49wBnMOVBmd}J8RSoNd3@1eG4X0$6M@4H!n4bu$Bx^NgYT+l1F@WQc07{v
zt-yZuzR+_7`0NsvAHvn)<iK}b&V<zGo#10QcZ1tGFN(anz(Ak37@VDs;(Q~ouzrL&
zrxbk0JIu@Z;0fSkU$FuiFU}`k)kD@g*SiVwUDo*fBltq>!~6>68gw+<(*i@bK|M?c
z-?=;IEBhg5g728hc8Z&~f{)B$`M*H_JK$reXVKrknDfmxvS1VP9SOb#4TUi5REYdb
ztUrg2dI|XUY0TH4U;hBU;xn_o_8-gj7PI`Pn0V!ft&g)jek<-Yf^UXBTRZ&uGx!MN
zr1--N!egA0@;c*Kf1y<mzW`s+#`%uJIG6+9`XQHV8T$2g;IqfF{PVEqQSg!Tw{V$H
zp{L(?&R71?kF}6L7JR`1)+6^BP66MJeOf8k?IMr;D4I^VpJ%~$;pGU|^=5wC37oHk
zd3Yblj|5+UbG;$p%fT07{sZ%!8t_@HyZ3|q??eygIb!D>#H)VMQn;lnDe>VS#23)%
zw|#%+73hheT_iql1Rr~h>r>9h52L`V*4@Run6H36Q;6p{&-^)Dt<JgNJFx%rAmr}?
zA4{`fJLDe{Ju8?HKm36B{z$9C9uwJ~R@fu{R!qFIr?ZLmypBXw;G5SoPs4}%xya@5
zMT1)*-)_Z+SHL?6kT-C#ojoUUzFk(on*zRI0Sihzxd432N_Lzp;OC!$FGhS2KiO`w
z(x3l1z~$-O1OZt5VJ7%?Yo4|cd<Diw(Rms8<`mb@Tgd94;5*etKKe{XdAFLv<!!<K
z+d{}62)+>ez%qY0UgX!Yf|DU%558hQ=C20-7x10fw-G!42EGIPBws;(i(}ZHj_=w2
zvEcUuFF!_=xH^`2)nD58<o>b?<WGcr3+ioa@TY^%p2qs6o*x!@E53DsZ@rkyCHuFd
zrm~$CZ?Yb#w`0M#EHU+83f|to`YZV2ms!7zryId{^<}@hjMX_Krg6Rz_|;Y@R~h(<
z9k?FQ{Lb0nJFwm#3;FB8cVa(eAMnqB&z7(~cY@!4I``8o_CxxCKZbbKPqW{$o!2us
zjo>SWvEgFpL*P5CdEGkj&X=4oh7D(LT8JtAkyBXl7sz)G_*U4z2ly*R{wmfZ>*9~V
z7w^t|NgqzohZZJEfAbm4Cs5C$!N;)QyAAl`z}x#^_29d{Wj))%p8LTUe8T*};MWTO
zA69%f^gA(SPyT0T;@y3KH}-cQHbdkvSAILkIhYk34*nR(7vP+_0DL9nyR3M)9K3x`
z;5P7W)_&9r;5&}ua<!s9N6re4^EJe)dTvF3l=^7_-&w}?KMFf<1>cPLAnVq@gSYq3
zKP28=PoaLY(BqVHzJ(vLe(~EV_?AV?Zv*)g!N<O39?LD~bmG<co5j6iQU4p_)p**5
z@e9@9Tn{~M_p#r;y*;Psba_eb<5!Tcn9lWCg?_XbEhtpIb)vt>cvk|x9qUb*|C}Lw
z6YH7D#c*yWUd72)#K{53`Z37I(7wlkf5VlheJAUl!*`H(togv+<!onjk~7=@J#&dy
z_2Zy^_65HT@?CfdhxF4o!B=1$><#;SAIti)aV8Ffp1p*(=2xS@cj2DQt{fJf+2AV<
z;re-*OjDmffp0~>kotK7ynX-S-{9@|*_SF%wO7lw+<@3-a>js<VV*30cnSCj)|L0O
zInEm5-Tsb!a$5eopU)v**pKzAd(!GN@OaL*3vpmM>TNRk;#QWw5(DAc;M=h-z;xWX
z6@15z<~Z`U%X41(pI<`W!TMd|{KVO8kA08&6!2}oU;}SJf@{GyW4}e}=ij0S>-sgw
z{3qi3>xSU({ZHV0?eXz&@b<cXqRZzinhW{nXW38og`KU$D}V0Vg83y#`Y7bvF&^y#
ze#;Zto>pt$axd@|STCd@e+2QWpGGEg{fnMsA#blQYK6!C#u?D_d+;p_xt?Xeq8)rI
z&X1Cie+|5SUi<~|s=N{OUx|~upQOr_{~7A?<o{N@DkEO`b0_xOS0HH`@`aZD=ZT&t
zxt{;R;M@zowVcbf8hX}(x91_fXoBjF@7T8#zZy?`0sSt|UQZnZ`7HK(OQF93yaPk_
z>&p-B24A={_pkFH{|R_|-oE`QY)>clb7UNx1imB7<+=xYP6J<z`S1ncZ*+Nl(O@;?
zn-{Ylgi&V$czeIMa1Ps3i1|NtXZJG!d?)sA>%gB1z6<kS@qx>TSM8g{_&W&le}#NI
z)@#eyEayeYcVc`Q4f)R@ANi66Y1-y~wvV%&9WizUa!dC!!sYQrgUOI@OR)T#h?AFq
zFUEXq2-4gRK8E!;mVeI2B9Hw>DesmQY)=I9ir3+vV_jY|1b;n2<gaDHm8gfy!FOUG
z^)$#o2)@HQ|9u~PGvZ7k<abPP+$h|G3o3RVK)mw*Ec`+IKj!jCtifF3Revu=e3SnE
zTksv=MbE9!-)il5y$n8!^OSpgaV8sySAHVDw-rCxWiIT*zOm?^4Bo*+O!#Hs3s4V5
zD8qH&Tb|+!MgCov*9wZiehv99tY`NI-@lUWvF}R^0pE4}7Ote|F9qL;ej)rK@G;Db
z4uk%Cz!zd%SP1?F(f<wW{}KGhqGu}eP2fjS09Wm0@BhyrUbVZee*y9RDegJz_IEY3
zVP1MJ<Xg{#ezezv;3L?Fxe58c4!-ad7Mu(Deksn^$+8~2+tHaoe9(A38}h~Icj9kn
zy8JeIzfV7#c;!Fs_#Ht6_WZ@=w|5G#FS;lAC!s&PmJJ-c6&{SsG}a~ShgN2@PJGg|
znd8Pzi^uC5=f<bR^(QCJ7_~4vb8J~j*|bsh4ate=npBBXmZ@oMpqEaKtxMD_u1^Ol
z36v?D70YGlHBz-f*_scJZAjMyD)t{t8yg!terjndTa&0vsTWBFX$=-CYn(ejRoPIU
z2})u<oKlxeEviY^2WmGTPER!C(v_(|vyErtGj;Udszkc3AyC$SJTt3mJ5W@AG(A;c
zpO}}LoTy9E8?xqc(X<9Eip;CY%uUqja;aR2-TeAvQ8RN^jEqg?s;H)esx0rhH1xP2
zY4hQ@X=BT}X~ZCri8bkLHkAv~uRoH!g5->6mAQuchQ_L@p_NWNe$4pN__Qfy<?*<a
zOyyGZsQFX5ctdTxvL;hUuP6(k;_+lA&J`R_He_=3@kHYyhhA}(tw}Ycl0!$188wVF
z2MER0yV&?WYH{-7++v5{#g=TWtzAq>v~O|s3O7SE9-mk`c6v#?WX1$CmM!PMPdJ`l
z#a1;m9-p^pQJiY3K11C#-LN>`6mjAwmXG&(cC@H$acx;Pk*iOUmzIo<&nSx%Cqyp2
z$oG+%6DO9Hl*h}*j+<7ZvMZlH-pe{!pNUs1=Sx}oqojXAs*cr1T=h};&e{!g4Dn$_
zuKWm6QJYfT%k4<YA|s7i1kNKM&4d$XjGaDZJSFx^Kf-eO5xG=-s-dJw7aGYiaeUOJ
z%bOzcx}xE6O^0RP1QzrJBz;jyTB)5XK{cw^!-$4-ZK}Q`HkImsj2Br-%7(?$$=K8+
z{gF(oriqTrWNK20y7I+Y3dVX-M%qz#d{Ig1)UpV*)QPUqsy4m6jV2yml}n`p3q2vi
zg`VQ4jA{|ev|xnpHl>+Uqr)q6sYF9+LOL;T3cb+J%wdcd^G8=_sQV>y4J9#E#Jr}X
z@~a0U(fVb><KBOz=~yznQb*29HH^>Hl%(xyrP?&=&UOR438S_^qRgw?;F^KAcn?QQ
zyonC4&(t)frchr_Cu(NJsL7LMMRC_ccnW(#)Tn4Fs5Iah(Kn8o*>zXnuk3aho}>m~
z=ab}l1tNQ6qvKO%#wSjhR-#&A<Z<cx^xU)x8f6WMhQ@m3i}asa+lCuOBQ&nn)t9@T
z?7OXA)4thK#lG2kcNm$jS6*X!OmyU_xpV_nT|<Uk!JUryHpix>)6~us!2Rw$3M1t3
z%1m}~euH^!QdE>kCVjW2KwRILOHG?n;&!<a-kbmxL0Oa^=e2Cq2w8geeqp1-D5}JA
zG>lG5HKl6Y3XW*s^;Di!(GistQRk(`RyL%YQe_mDg3SpE3;9t>w-UYDD}t7#>zv~o
zb2%D}O2|8@N|k$h|HYE*18xSxDc1V!XS>~)WMj4_T}c7JYDQa-e9vt=qfeqQxHZ?9
z9a~d#tQzqxSJ!HI1oia^jfonGf+f_&lk-xwwXVmC-;34=wMS#t4eTMcsv22^db5nW
zk6Ps`udY)O)~^q}#z(83RF_KR@*=#dU<zXH&>&V8MY76Q#*^3P((|etW@H*@YU!4}
zyv%DgYv|T}NUs+)3u$afjjyJ0qkh3+s|KTa!$wh*YVx>5E}f{WOpVVp;csqi_8f(3
zchj+86e*dJ$<>-+#~;~-tI15*LpylVkf2RSXk?jym?srx#LCfb+mgq)-7Kj0dR6F^
zXGA3ps_-sR>Km&ZRIlFXa4Ox@spX}q`gD?fj@(yulW08fzfyD{U4ulX=qQdgD)^{*
zW?dy3D8LAwKj@Lih`#Qu$RDR=wA6Dos%eDlW9(D`j;>AHEJx3;y`mzn^2SBv<%!(l
zS+Tk90+XG66Qf#a=!!;YhNX2SZN?Zr%9Q)1_d5VpOT5Z0YE`_ldO>mVq-j&ejgJ=%
z9X510)lGT1SDZXAb7wc65x&YX)cP~$&OeSC-k2!*5>fU+Z{ToeYaB`S_V5!F4YzBM
z0<pW!#{57}=6P*2>ZY0%Q}0!Vpckq6FM2vZJ8i}#;ayEIj7f-|GH<#NEo!LFWllXl
zk*lL+qTA7;ISQ${I;xe%6xmYA(}mjLxrCv7lso9qGIB2Ye|2plw?K^t!CflPV@lPG
zS-Mk6X_|W5s5t+pXX2N=D3YW#S+17C@nl*wQ<!sGN42Ex40@zjJ~Xd3Qx|t<scNZp
zOjEoRE5q?MiTZk47RFDkOVq->X3vhZdwI@<PN2=ny+@<4WDR=eo6P}J)vJug++xYH
zk>=#lD781oj*he)EWlq>QqSMbRiK(mQ{e4(D((4ZQ4tN-RWt%j$fc`jD5Dt#&0{ik
z9B{PhlO5aLUMJztQx_eXOVwtYXh4$P6CSsX^$>?&vjSwZhhn(58q&tlP39ggqV-F1
zVlGq5lTB}exrs{pS?i54Zf|neNUnqTv@7}c=x&QOr7;sv*2J1Zoz(ZX@W`UqE}S}O
zG2}Iz9%_8wB`3}cIBxVYhA8C<+ITm{i>K~`{6Wju2nZXs{6Yo|`kvh(YX+^enz7%r
zEl8`e)E`5Ojq;4UyfJ3HzF|c?J;fT)oVn%o3<#IcqWwAAdP>(Pyl$p-<{nM+{H3`+
z&y##5)Oyj10)yJS$W$d0i~Z5HXy^#tQ>5Bt)hF8W7W;Vu#vF&|eeOSBpe^%!b9HAk
zmM!-3Id80|;!y8SQv`9h2DGU}o{2_)%AY?62O86Nz1Z)$+a@hH#^vB{;%W3}t@b3@
z_b-~Bs67Z8!5V95dS9PPdixOQnAT2J&%DRmn()2H3sAlhMMdfQ@iE%buT4!$*V1Bu
z{;7#nGL=;0lDiyomtWB$V~uHegE13ytHm$8xpQW?A~pra(12K;nV6`zw)^ttI^-Hg
zSC0<!wjp>}Pz$)Jm3cE0JyUb7W;?m(1w~PFBcif8Rk=XzL+HAEcegF`i%;?U)I@eh
zW9?kp`z6md)619$57C_6*KK&YXP_7At1{VCU0!$hn%USCi57V~0ko>%%Jd~SD`;b+
z8Ldh+QEgV5MKa>6y}O~goZ2KPrGFenZKzDn)MgrJg*`1*H?N`EpAAt}>GMFtJfl*4
zy#bxq4mdLZvsRLReS7g#3v9+<sEsSM)j%8dUJNx%6yNeH*msK2RC;%7+4Lug%@nxi
z&o>;!>&TdVDi;sho#pLVyU6AQQmvu20P9(giiwU;vpdza)F;!6C=3K|cIoyR)uRHq
z{t$Xc<}$Q<N&ccg^W3?ca{atr=YZ{2Kl+DED|}xHp63TzZ#%kCA67=5I~ZN1ClNGW
zxhN5*s$7(A$m{pwioQGRUBa05@$yA{MKddW!$FHrdE>U*BYiDKR|twbVyWi>feRyY
zbodX&1vjw!>yCPrr8_na(+^biiFd$xiqSSj5#{Vt>S)47fv7ygJG`6W@kNn3T2awS
zUcI&uQt_R(<05hT&v0|EV;7-gd-deUgy=986v{HWhOz43bt;L)3>A5UA$g2i_?P5z
zncUA>_VcJdY^1)uV=uynS2fb^oiXV89VAK{4XXOI&0Av;sCN}D)foRQ%G*pVNgEUJ
zB2^cYf{w`zQ{WjR>Z@sCYX%m-UajsI;C3|9yG5W~DSwd9_pAK62|KIP1_&>@8g3UD
z3k?YlKFR}JL&g}bWX!z+)|OMsr^o#Rb2=PW$K~bjzZ|CTD`;&JnVYCwz*A8*{^re~
zXbZ_asFTHqJ+<|EbI~YwZZ<1>d?vS`K1(+ijME}M9`d?@IYQ}YPVA*c<|wP}L5Hp3
zJVV@mrHr8Mu`)WkG0WkL1#J|%-sTyp)z`@S>O@UVX-ZalYM)IDRnlz6>9o;ma-8dL
zfOwV|QyaLr<_?>haqt;&u+OPsU+<Cm0fl!GaSq?}kb+}~ybd;6`d3I;&3CpU<D^Kd
z_TV<EqEnU1(2+2Ik>G9~<>ji^jh|)EWYCW-nBoO=Q9D=!^ipGTs~=Gr4&b>(-V(#x
z=fD$Ve?}&ZhOYdoNNz@5c=@=cr5jT<0qYHEb-(6#FF9~IE5gRwgnNVvU2?6*4qTGV
zpW8_N8%GEES5*8CF-mPC(*Aam*1B}gLtB{CaVtyHJQt!rbuTa`XyJjb$lJv(Ye;1$
zq-A@RPZ>PTc@F2sRPX+Tp0Na1Z3DwE!iZdBov}k7wBn^)&Fy%#8$U8`{io;YPmH~l
zrM~{LuU(MOsR9MuO>ocNbj?OJ`V+E%z^>2S!s0qk)ADBAMs)Ra@#@X%M@2<^&^IGf
z_v=(HLnrJ_bg3ej;((`L>PeTXZEUqaTB^#r$@5F!?`7HG?|R7;Tm08EB|6%3oe6Yq
zROU`&EVprwcliW|{&Wp7CRt^4^;~Z#{jQ4bv-u%k9iX{0$nlwEDm3!z8Tg%#yi<*j
z`kKIK1PVRwN`*EFX`cM+RA_|uX2<@)fY)q(z2+4r@90<6oO$vabl{s;2~-aM@`_PZ
z38Ly|58wIH&#81#W9}9-ANAV2h~}g2H2}JUt1iE$lE$tw_ll@-eIaBl+2lT3UbIox
z2Fzr%6I7}`L&%(y`6~g}L}O;7_glMTdIh%*GPp-sv6@T>Tj`T&G{8k8%BL13=BAsX
z(NXkwb>tUspeuNECzI~A6&H`6J$vl9De>q~y5{NMKZ^cOC!3ocO#@<@-FS9f^V-C#
zsG?|1CPDkaP2?SPfz?0s(oP)7A~P`Pg~gmPaP%;GsB&vH0oYW#haKMJZ?^}H8-Dkd
zXra!EMu+btcXPUF?gwhi0ArrxRi(t@kbMsoFsw7%;7x|0aD@Aq{`slwQ|L|MN6XM{
zn9$K-Q)f?}*W_)(V4~Uc;2@)u-(=C@I5;j%)MnM?0%P%Qggi_^y?XL49Z?m#8^qe=
z)ZBsJWW)6|NT|Bx8I`dd>#k9~vw62exS=yF(94#Ce`UujvtMyV-A)Pf!n4?|JC9Mr
zc3d?cMn|}J3jD3HkfoNcT_%}PbtAGoL!0qwwL@SwvhTlo?+*7qedX>|OrDS0W%2r}
zY?+HM8igEFMZYz`j<hO>XNAlkl+#T}*Ov3NL<Jh_%t{}pda~iFjU#qAM_BX9>J=r*
zx4LMQr%u$6rW@tvW`ZYJG$J#%fqsNCR_<)+dll}TpzeG*rdARZ$c#!cX5G;URSjJl
z;d_AYHh@=qd9Bk;2=<C<-^p;#-pB^MtLrsx^mOziyG^Cxz1oQmGtS{c{5r2Y(fAnT
z>$($5R`~;&?S*>BcKeyvRNnQvJOj+Ou)QW|H^8e`ac6tNtC^v0CD0v}R6;I>PpqLU
z+O84$g_S_7q}+ZL4|jvv6uOGV7Z%dN2R?b)l8)MWsn46&d#>egl(_da`4BX4ciS^&
zvxiA@cIpOh|I&aT8~jR(j`T(|x|UI%qSI!Y`p|z=E`RZ>c0cLPS~6LhEVZ`Fs0)>)
z)$fRK`F!)L+pCPgezu7_N<5bw7QXT8&&WO3@x!$~`3pC}SD|;>yn{v4ne{&HEAXq_
zY?=RgJ|YW#>+Bx^1gDkioRW?!-ScP_g52P5uA+3a|7W8-e~pevQiPYjNcXxoH$u77
zcRfb=oduIAw-BR@m7p<u-(*nNOKS{u#vL@j(X@V|hS69qO@~`$-pyCzbjJ6xpd&lg
zE%H1_U)Axfo42C8wi+IK%NVz#X_w2WEpJ!S-x~M2ftN%LN%WtQ(wI{O4G4Z!1Oy{B
z!!*WY-6;5^9k?Z+tBIC*PC!)=FgK+w9qj6OJ=gQMq9~$RtcK<cr7B=+p-fGFC3Lxa
zFIn2yiZ`Wm4Rkl&S{v)XU(mh97^eNErhvlZLNsh@?AP0-_qAl<n!lH&>e4H6UcB6t
zziWAqXw1?j?9{|W&OOwtFHufFKQp5#0nL-d;YQPelAo2)uQvO)YSx`p-b>g0!E3ds
zy5iKB<?G>L3wmXsIc@PvB@0<|Q;}1c?pW{r(#9*EXGT#G&g9f?WBzfR?h!_fgt&y>
zJ$%E+sJu=R;N*ezG$QBy=yQ`l;t9G>kYC)Wf2vuo#`U}j3?Bu03H|C;x07a^7&c>%
zZra6EtI!Yh40ju8gg1LS=&hE`nYlkAL?g6`nw&zv-KC#qn$r?}zt>&Lxj)gGmN`{^
zqLnw_%RiW5KT@q`==S_3DnUmbaJVxh*Ht;0{%6=ud0<y}$G73({m{R?qdR8aL7CTl
z0Rf}iUqKp!fsAG~HF6=uUrCxjJ~MiUwL|6>YlJavv)fx=CA;FLdl=e7e8Ze%86&*g
z*?iv#zklhq?<PC4-ch}Q)jc-UpZI;JJ7)>=5v9#*OV2cFlTDtq2St|pY^EVs<%N1{
z6TohapI3K&{RUn3@Xg=k{a&-*@m)f%3aEv(_W@W==1q0{dJme)$R3ZqOX06~)WV5s
z-t|IbPv5R4IcoJUEEs)<hKTrF+KP97mt!<#Ubi$oTEdfG5ZxQGajQa;?rrA5F@ELm
z)ts-Y+pTn&ix|r7q8nTGNzb`_Z#6q((Bj@Yx}dAn@%nUkrx*E+q6fdcT?+4A1!`kP
z-OJ+jwl!YZg>&b1-pX0EU*!Vz^SpRfB2809`Hi0Y3q7@}GDDR<aWvc2YmNN<m8iP@
zIz6$7ez)0_&Zs-U^kXEoLhu`|q*S%HTLI<>VVmy{5&>OeGmo5j$5?j(um9S=1~m5O
z$YRX+c~)<Bh`iZ`o`D}e{B9esP1VvgG4QvM?t0eT3(%`xTZT)2%CB|_0)ETrD!kBc
zES8E!RA%a$Xx2dsLi}XDJX1z50KrcU^`FkijFf&`%CIW?o9>kmt?u-C$qN>?mm77=
zw>SCDW_iXNw2euOEWU!mbly>LbcA}v5dB~oN8#ACl{)oTL-r1~A5hh8C%)S4dbHOm
zL-qwWS$J#&_<_drOKpOxO^$r~j2XLZmEgC*H8t)&iF<ah{K`Cy(|w|wonW_nni~{$
z_Y7FGd6ns%uJ`zYm{DoQY)M8bw_L{5(k|B~cHaC*;vEdxOM2hCu`*M=J%87O_7dG!
zq^R*L;9N>Chu5@r$e9(Rdzp<KBi-WwJ(V9v$XDc7m7e{c&iv)0H*3!uT8;V?50pmB
z>oGyuMgtGq)wYk^&d7_6=4B=Ox5N1Zp)ohIJEK=@GA?A#!jFL?Ws^%@qZU3odETIn
zPEm8?%BZ~Hkm^~tiJzu(>*x*br7GH-U(V#G&u_TsW?$g)o!$1$u;DPTeN|DsI+s}~
z>c==$4$o^=Y+f#rOslt{grv}R54<j5j2HG`;#aCVQ1MSs>Ci6OSc#V)jmqcshLQ2b
zx@PVb)|?yW$HQbI|7XD3X1r%&P((NL)GlKA!;wlB5U2HMm~WF;dr~+|9rF93uE(z3
zpAiPgZ_P*$c5Ts~Pe<eNrrK^N)Lyw{L{vMnVf!Us@6F%W^Q$)~{P`|S)h01nYf}HY
z*Em)bu}4wQ6g9(g8^N3u>#YzlAvRsmo$#wIQHS2H<j%7Bbva&$-_#<*bI0y}`@x~!
zsiHUa)ra5w^Q)q}<MO<Tp;0GZ!}>EIeHCeJPI7Fthds>_x+5&V#>G}QZv*)&-{3d8
zXm3W5X^8(C6yrGF=*wp77;8-mn$jx6BAAGK=N^0tY0Sp+Zm615^1KO3=)l>{e1WUb
z*mn-vhtXrcQU0*a3^x+tm&jI-_lm9DSC7Ab!<o_0t2yPVeCL~A&7$A-j>)u)Cui25
zDjPqJ>z&M8>Pj00jj_g16(k@5!n4=wV)kHb4S;SOh?-}d=Jey|zR1ukvxI;3Yb<wx
z?k#uSYnsN`>c5mu8;8iZwLL7u?Y&}d`j#1T{5fxuijI;Wo@~Zz<h<&a$w^eLmgwaW
z^b?=P8f(ho*)IJg<bI)3RbQE?tMYeItamX4M=L{_ic_1@(ZmN)Uovm^nlgVoAatJV
zl}?-HnrlRReClSM=+&(kv-zqJy~#M0%Z1mDw|3UXQBM~N%8@o+U+3Oo34F1Xm&6Yh
z(am`Eqdh{HzHEoi{H|T3-6~h};eek?c&~Ak*7K^~UT=z5s(w(OnI0Q&4@rL6j9w<`
z#2+Lhqw5A$uRp~ZqkichyrW?R;?cE>-A$q@+6*&xg2V<H6xG_?+G;khfxC5E<sMb$
zIfw7iW~_6YA31Att>BT`pV8NMJAvEuy?@Q4)l~y^5zj2V*P`yU%Uu^)=jU!5H;<!(
z`d9dVMM}MM%owY@OrvVCXupe5n^pK;Ls3}m@O66$%-x3f8o|$KOd`j>1Flz<>%Krw
z-F5J1Uiz4wAB@C5J<XbB{wZ#$(wMilG>-;+$CW8+u<si77`_zrvfO~TH(S>!LS_n^
z8yfSg#0~*@GhppUPC?bSi53YjJHnbagnLbF#Fac_r~$p75@@N7gBnwVQRmb<jG2<w
z{A%){?=vV5tUERT{cZtu(9^56d_Tk+FJ$`TZ_yj`LG3jOJ$tFB@1in_c($8^%;u;)
zw(it)U6S5`SKnBx7Fy%S(+ls^tHr%yXmz=n@3B>>b(6H;S@_FKH*T5d+(wh?huoem
zn;s@9Uu`#ACYy>?V}_ZRJ{|Y7s$ZDPdnX2NlK4ZP)^OtQd1I0>an0M=2)=X?^j_S&
zj$*BSw7z1O*Zj@DUf#eHMpXpsWt~QQ80XyPEkLho>|c2z2JuqEd=aQUMun-TL#@iJ
z_ByhtI!W~WLAn@Eq^+0U=xaVVI(R;*xpa{`ZsAQ*JSz%lOwEJs#w;3%m9_TMPh(YW
z{M^!9QG3BLXbPo2G=f`nvwp&BZiLxe;$98P^;j!_`k4ou3y=Z+8$zeiU~NQZzj8PE
z=19*1{|rKHUIZRGZ~ArN0UK=I*--a4=Nwu_WE%5(eV!NC9?N}Sz5Q5482@)%#@txd
ej24rqJDYP=x8^**%<Jt&uW;9Ker;J7bN(N5U8tV`

literal 0
HcmV?d00001

diff --git a/3rdparty/qpOASES/qpOASES-3.0/make.mk b/3rdparty/qpOASES/qpOASES-3.0/make.mk
new file mode 100644
index 00000000000..ae907b4a1ea
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/make.mk
@@ -0,0 +1,38 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+##
+##	Filename:  make.mk
+##	Author:    Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+##	Version:   3.0
+##	Date:      2007-2014
+##
+
+
+TOP = $(realpath $(dir $(lastword $(MAKEFILE_LIST))))
+
+include ${TOP}/make_linux.mk
+#include ${TOP}/make_cygwin.mk
+#include ${TOP}/make_windows.mk
+#include ${TOP}/make_osx.mk
diff --git a/3rdparty/qpOASES/qpOASES-3.0/make_cygwin.mk b/3rdparty/qpOASES/qpOASES-3.0/make_cygwin.mk
new file mode 100644
index 00000000000..2ffdd18b57c
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/make_cygwin.mk
@@ -0,0 +1,106 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+##
+##	Filename:  make_cygwin.mk
+##	Author:    Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+##	Version:   3.0
+##	Date:      2007-2014
+##
+
+################################################################################
+# user configuration
+
+# include directories, relative
+IDIR =   ${TOP}/include
+SRCDIR = ${TOP}/src
+BINDIR = ${TOP}/bin
+
+# Matlab include directory (ADAPT TO YOUR LOCAL SETTINGS!)
+MATLAB_IDIR   = /cygdrive/c/progra~2/matlab/r2009b/extern/include/
+MATLAB_LIBDIR = /cygdrive/c/progra~2/matlab/r2009b/extern/lib/win32/microsoft
+
+
+# system or replacement BLAS/LAPACK
+REPLACE_LINALG = 1
+
+ifeq ($(REPLACE_LINALG), 1)
+	LIB_BLAS =   ${SRCDIR}/BLASReplacement.o
+	LIB_LAPACK = ${SRCDIR}/LAPACKReplacement.o
+else	
+	LIB_BLAS =   /usr/lib/lapack/cygblas-0.dll
+	LIB_LAPACK = /usr/lib/lapack/cyglapack-0.dll
+endif	
+
+################################################################################
+# do not touch this
+
+CPP = g++
+AR  = ar
+RM  = rm
+F77 = gfortran
+ECHO = echo
+CD = cd
+CP = cp
+
+# file extensions
+OBJEXT = o
+LIBEXT = a
+DLLEXT = so
+EXE = .exe
+MEXOCTEXT = mex
+DEF_TARGET = -o $@
+SHARED = -shared
+
+# 32 or 64 depending on target platform
+BITS = $(shell getconf LONG_BIT)
+
+# decide on MEX interface extension
+ifeq ($(BITS), 32)
+	MEXEXT = mexw32
+else
+	MEXEXT = mexw64
+endif
+
+CPPFLAGS = -Wall -pedantic -Wshadow -Wfloat-equal -Wconversion -Wsign-conversion -O3 -finline-functions -DWIN32 
+#          -g -D__DEBUG__ -D__NO_COPYRIGHT__ -D__SUPPRESSANYOUTPUT__ -D__USE_SINGLE_PRECISION__ 
+
+FFLAGS = -Wall -O3 -fPIC -DLINUX -Wno-uninitialized
+#        -g 
+
+# libraries to link against when building qpOASES .so files
+LINK_LIBRARIES = ${LIB_LAPACK} ${LIB_BLAS} -lm 
+LINK_LIBRARIES_AW = ${LIB_LAPACK} ${LIB_BLAS} -lm -lgfortran 
+
+# how to link against the qpOASES shared library
+QPOASES_LINK = -L${BINDIR} -Wl,-rpath=${BINDIR} -lqpOASES 
+QPOASES_AW_LINK = -L${BINDIR} -Wl,-rpath=${BINDIR} -lqpOASES_aw
+
+# link dependencies when creating executables
+LINK_DEPENDS = ${LIB_LAPACK} ${LIB_BLAS} ${BINDIR}/libqpOASES.${LIBEXT} ${BINDIR}/libqpOASES.${DLLEXT}
+
+
+##
+##	end of file
+##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/make_linux.mk b/3rdparty/qpOASES/qpOASES-3.0/make_linux.mk
new file mode 100644
index 00000000000..b36f90b8791
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/make_linux.mk
@@ -0,0 +1,107 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+##
+##	Filename:  make_linux.mk
+##	Author:    Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+##	Version:   3.0
+##	Date:      2007-2014
+##
+
+################################################################################
+# user configuration
+
+# include directories, relative
+IDIR =   ${TOP}/include
+SRCDIR = ${TOP}/src
+BINDIR = ${TOP}/bin
+
+# Matlab include directory (ADAPT TO YOUR LOCAL SETTINGS!)
+#MATLAB_IDIR   = ${HOME}/Programs/matlab/extern/include/
+MATLAB_IDIR   = /usr/local/matlab/extern/include/
+MATLAB_LIBDIR = /usr/local/matlab/bin/glnxa64/
+
+
+# system or replacement BLAS/LAPACK
+REPLACE_LINALG = 1
+
+ifeq ($(REPLACE_LINALG), 1)
+	LIB_BLAS =   ${SRCDIR}/BLASReplacement.o
+	LIB_LAPACK = ${SRCDIR}/LAPACKReplacement.o
+else
+	LIB_BLAS =   /usr/lib/libblas.so
+	LIB_LAPACK = /usr/lib/liblapack.so
+endif
+
+################################################################################
+# do not touch this
+
+CPP = g++
+AR  = ar
+RM  = rm
+F77 = gfortran
+ECHO = echo
+CD = cd
+CP = cp
+
+# file extensions
+OBJEXT = o
+LIBEXT = a
+DLLEXT = so
+EXE = 
+MEXOCTEXT = mex
+DEF_TARGET = -o $@
+SHARED = -shared
+
+# 32 or 64 depending on target platform
+BITS = $(shell getconf LONG_BIT)
+
+# decide on MEX interface extension
+ifeq ($(BITS), 32)
+	MEXEXT = mexglx
+else
+	MEXEXT = mexa64
+endif
+
+CPPFLAGS = -Wall -pedantic -Wshadow -Wfloat-equal -O3 -finline-functions -fPIC -DLINUX -D__NO_COPYRIGHT__
+#          -g -D__DEBUG__ -D__NO_COPYRIGHT__ -D__SUPPRESSANYOUTPUT__ -D__USE_SINGLE_PRECISION__ 
+
+FFLAGS = -Wall -O3 -fPIC -DLINUX -Wno-uninitialized
+#        -g
+
+# libraries to link against when building qpOASES .so files
+LINK_LIBRARIES = ${LIB_LAPACK} ${LIB_BLAS} -lm
+LINK_LIBRARIES_AW = ${LIB_LAPACK} ${LIB_BLAS} -lm -lgfortran -lhsl_ma57 -lfakemetis
+
+# how to link against the qpOASES shared library
+QPOASES_LINK = -L${BINDIR} -Wl,-rpath=${BINDIR} -lqpOASES
+QPOASES_AW_LINK = -L${BINDIR} -Wl,-rpath=${BINDIR} -lqpOASES_aw
+
+# link dependencies when creating executables
+LINK_DEPENDS = ${LIB_LAPACK} ${LIB_BLAS} ${BINDIR}/libqpOASES.${LIBEXT} ${BINDIR}/libqpOASES.${DLLEXT}
+
+
+##
+##	end of file
+##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/make_osx.mk b/3rdparty/qpOASES/qpOASES-3.0/make_osx.mk
new file mode 100644
index 00000000000..71982da7f29
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/make_osx.mk
@@ -0,0 +1,99 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+##
+##	Filename:  make_osx.mk
+##	Author:    Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+##	Version:   3.0
+##	Date:      2007-2014
+##
+
+################################################################################
+# user configuration
+
+# include directories, relative
+IDIR =   ${TOP}/include
+SRCDIR = ${TOP}/src
+BINDIR = ${TOP}/bin
+
+# Matlab include directory (ADAPT TO YOUR LOCAL SETTINGS!)
+MATLAB_IDIR   = /Applications/MATLAB_R2011a.app/extern/include/
+MATLAB_LIBDIR = 
+
+## system BLAS or qpOASES replacement BLAS
+#LIB_BLAS = "-framework Accelerate"
+
+## system LAPACK or qpOASES replacement LAPACK
+#LIB_LAPACK = "-framework Accelerate"
+
+################################################################################
+# do not touch this
+
+CPP = g++
+AR  = ar
+RM  = rm
+F77 = gfortran
+ECHO = echo
+CD = cd
+
+# file extensions
+OBJEXT = o
+LIBEXT = a
+DLLEXT = dylib
+EXE = 
+MEXOCTEXT = mex
+DEF_TARGET = -o $@
+SHARED = -shared
+
+# 32 or 64 depending on target platform
+BITS = $(shell getconf LONG_BIT)
+
+# decide on MEX interface extension
+ifeq ($(BITS), 32)
+	MEXEXT = mexglx
+else
+	MEXEXT = mexa64
+endif
+
+CPPFLAGS = -Wall -pedantic -Wshadow -Wfloat-equal -Wconversion -Wsign-conversion -O3 -finline-functions -fPIC -DLINUX
+#          -g -D__DEBUG__ -D__NO_COPYRIGHT__ -D__SUPPRESSANYOUTPUT__ -D__USE_SINGLE_PRECISION__
+
+FFLAGS = -Wall -O3 -fPIC -DLINUX -Wno-uninitialized
+#        -g 
+
+# libraries to link against when building qpOASES .so files
+LINK_LIBRARIES = -framework Accelerate -lm
+LINK_LIBRARIES_AW = -framework Accelerate ${LIB_LAPACK} ${LIB_BLAS} -lm -lgfortran -lhsl_ma57 -lfakemetis
+
+# how to link against the qpOASES shared library
+QPOASES_LINK = -L${BINDIR}  -lqpOASES 
+QPOASES_AW_LINK = -L${BINDIR}  -lqpOASES_aw
+
+# link dependencies when creating executables
+LINK_DEPENDS = ${LIB_LAPACK} ${LIB_BLAS} ${BINDIR}/libqpOASES.${LIBEXT} ${BINDIR}/libqpOASES.${DLLEXT}
+
+
+##
+##	end of file
+##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/make_windows.mk b/3rdparty/qpOASES/qpOASES-3.0/make_windows.mk
new file mode 100644
index 00000000000..95ae0786831
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/make_windows.mk
@@ -0,0 +1,114 @@
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+##
+##	Filename:  make_windows.mk
+##	Author:    Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+##	Version:   3.0
+##	Date:      2007-2014
+##
+
+
+##
+##	definitions for compiling with Visual Studio under Windows
+##
+
+
+################################################################################
+# user configuration
+
+# include directories, relative
+IDIR =   ${TOP}/include
+SRCDIR = ${TOP}/src
+BINDIR = ${TOP}/bin
+
+# Matlab include directory (ADAPT TO YOUR LOCAL SETTINGS!)
+#MATLAB_IDIR   = ${HOME}/Programs/matlab/extern/include/
+MATLAB_IDIR = /usr/local/matlab/extern/include/
+MATLAB_LIBDIR = /usr/local/matlab/bin/glnxa64/
+
+
+# system or replacement BLAS/LAPACK
+REPLACE_LINALG = 1
+
+ifeq ($(REPLACE_LINALG), 1)
+	LIB_BLAS =   ${SRCDIR}/BLASReplacement.o
+	LIB_LAPACK = ${SRCDIR}/LAPACKReplacement.o
+else
+	LIB_BLAS =   /usr/lib/libblas.so
+	LIB_LAPACK = /usr/lib/liblapack.so
+endif
+
+
+################################################################################
+# do not touch this
+
+CPP = cl
+AR  = ar
+RM  = rm
+F77 = gfortran
+ECHO = echo
+CD = cd
+CP = copy
+
+# file extensions
+OBJEXT = obj
+LIBEXT = lib
+DLLEXT = so
+EXE = .exe
+MEXOCTEXT = mex
+DEF_TARGET = 
+SHARED = /LD
+
+# 32 or 64 depending on target platform
+BITS = $(shell getconf LONG_BIT)
+
+# decide on MEX interface extension
+ifeq ($(BITS), 32)
+	MEXEXT = mexglx
+else
+	MEXEXT = mexa64
+endif
+
+CPPFLAGS = -nologo -EHsc -DWIN32 -Dsnprintf=_snprintf
+#-g -D__DEBUG__ -D__NO_COPYRIGHT__ -D__SUPPRESSANYOUTPUT__
+
+FFLAGS = -Wall -O3 -fPIC -DWIN32 -Wno-uninitialized
+#        -g
+
+# libraries to link against when building qpOASES .so files
+LINK_LIBRARIES = ${LIB_LAPACK} ${LIB_BLAS}
+LINK_LIBRARIES_AW = ${LIB_LAPACK} ${LIB_BLAS} -lm -lgfortran -lhsl_ma57 -lfakemetis
+
+# how to link against the qpOASES shared library
+QPOASES_LINK = /I${BINDIR} /WL /link ${BINDIR}/libqpOASES.lib
+QPOASES_AW_LINK = /I${BINDIR} /WL /link ${BINDIR}/libqpOASES_aw.lib
+
+# link dependencies when creating executables
+LINK_DEPENDS = ${LIB_LAPACK} ${LIB_BLAS} ${BINDIR}/libqpOASES.${LIBEXT} ${BINDIR}/libqpOASES.${DLLEXT}
+
+
+##
+##	end of file
+##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/BLASReplacement.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/BLASReplacement.cpp
new file mode 100644
index 00000000000..d882eba8766
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/BLASReplacement.cpp
@@ -0,0 +1,145 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file src/BLASReplacement.cpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	BLAS Level 3 replacement routines.
+ */
+
+
+#include <qpOASES/Utils.hpp>
+
+
+extern "C" void dgemm_ ( const char *TRANSA, const char *TRANSB,
+		const unsigned long *M, const unsigned long *N, const unsigned long *K,
+		const double *ALPHA, const double *A, const unsigned long *LDA, const double *B, const unsigned long *LDB,
+		const double *BETA, double *C, const unsigned long *LDC)
+{
+	unsigned int i, j, k;
+
+	if ( REFER_NAMESPACE_QPOASES isZero(*BETA) == REFER_NAMESPACE_QPOASES BT_TRUE )
+		for (k = 0; k < *N; k++)
+			for (j = 0; j < *M; j++)
+				C[j+(*LDC)*k] = 0.0;
+	else if ( REFER_NAMESPACE_QPOASES isEqual(*BETA,-1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
+		for (k = 0; k < *N; k++)
+			for (j = 0; j < *M; j++)
+				C[j+(*LDC)*k] = -C[j+(*LDC)*k];
+	else if ( REFER_NAMESPACE_QPOASES isEqual(*BETA,1.0) == REFER_NAMESPACE_QPOASES BT_FALSE )
+		for (k = 0; k < *N; k++)
+			for (j = 0; j < *M; j++)
+				C[j+(*LDC)*k] *= *BETA;
+
+	if (TRANSA[0] == 'N')
+		if ( REFER_NAMESPACE_QPOASES isEqual(*ALPHA,1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
+			for (k = 0; k < *N; k++)
+				for (j = 0; j < *M; j++)
+					for (i = 0; i < *K; i++)
+						C[j+(*LDC)*k] += A[j+(*LDA)*i] * B[i+(*LDB)*k];
+		else if ( REFER_NAMESPACE_QPOASES isEqual(*ALPHA,-1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
+			for (k = 0; k < *N; k++)
+				for (j = 0; j < *M; j++)
+					for (i = 0; i < *K; i++)
+						C[j+(*LDC)*k] -= A[j+(*LDA)*i] * B[i+(*LDB)*k];
+		else
+			for (k = 0; k < *N; k++)
+				for (j = 0; j < *M; j++)
+					for (i = 0; i < *K; i++)
+						C[j+(*LDC)*k] += *ALPHA * A[j+(*LDA)*i] * B[i+(*LDB)*k];
+	else
+		if ( REFER_NAMESPACE_QPOASES isEqual(*ALPHA,1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
+			for (k = 0; k < *N; k++)
+				for (j = 0; j < *M; j++)
+					for (i = 0; i < *K; i++)
+						C[j+(*LDC)*k] += A[i+(*LDA)*j] * B[i+(*LDB)*k];
+		else if ( REFER_NAMESPACE_QPOASES isEqual(*ALPHA,-1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
+			for (k = 0; k < *N; k++)
+				for (j = 0; j < *M; j++)
+					for (i = 0; i < *K; i++)
+						C[j+(*LDC)*k] -= A[i+(*LDA)*j] * B[i+(*LDB)*k];
+		else
+			for (k = 0; k < *N; k++)
+				for (j = 0; j < *M; j++)
+					for (i = 0; i < *K; i++)
+						C[j+(*LDC)*k] += *ALPHA * A[i+(*LDA)*j] * B[i+(*LDB)*k];
+}
+
+extern "C" void sgemm_ ( const char *TRANSA, const char *TRANSB,
+		const unsigned long *M, const unsigned long *N, const unsigned long *K,
+		const float *ALPHA, const float *A, const unsigned long *LDA, const float *B, const unsigned long *LDB,
+		const float *BETA, float *C, const unsigned long *LDC)
+{
+	unsigned int i, j, k;
+
+	if ( REFER_NAMESPACE_QPOASES isZero(*BETA) == REFER_NAMESPACE_QPOASES BT_TRUE )
+		for (k = 0; k < *N; k++)
+			for (j = 0; j < *M; j++)
+				C[j+(*LDC)*k] = 0.0;
+	else if ( REFER_NAMESPACE_QPOASES isEqual(*BETA,-1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
+		for (k = 0; k < *N; k++)
+			for (j = 0; j < *M; j++)
+				C[j+(*LDC)*k] = -C[j+(*LDC)*k];
+	else if ( REFER_NAMESPACE_QPOASES isEqual(*BETA,1.0) == REFER_NAMESPACE_QPOASES BT_FALSE )
+		for (k = 0; k < *N; k++)
+			for (j = 0; j < *M; j++)
+				C[j+(*LDC)*k] *= *BETA;
+
+	if (TRANSA[0] == 'N')
+		if ( REFER_NAMESPACE_QPOASES isEqual(*ALPHA,1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
+			for (k = 0; k < *N; k++)
+				for (j = 0; j < *M; j++)
+					for (i = 0; i < *K; i++)
+						C[j+(*LDC)*k] += A[j+(*LDA)*i] * B[i+(*LDB)*k];
+		else if ( REFER_NAMESPACE_QPOASES isEqual(*ALPHA,-1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
+			for (k = 0; k < *N; k++)
+				for (j = 0; j < *M; j++)
+					for (i = 0; i < *K; i++)
+						C[j+(*LDC)*k] -= A[j+(*LDA)*i] * B[i+(*LDB)*k];
+		else
+			for (k = 0; k < *N; k++)
+				for (j = 0; j < *M; j++)
+					for (i = 0; i < *K; i++)
+						C[j+(*LDC)*k] += *ALPHA * A[j+(*LDA)*i] * B[i+(*LDB)*k];
+	else
+		if ( REFER_NAMESPACE_QPOASES isEqual(*ALPHA,1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
+			for (k = 0; k < *N; k++)
+				for (j = 0; j < *M; j++)
+					for (i = 0; i < *K; i++)
+						C[j+(*LDC)*k] += A[i+(*LDA)*j] * B[i+(*LDB)*k];
+		else if ( REFER_NAMESPACE_QPOASES isEqual(*ALPHA,-1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
+			for (k = 0; k < *N; k++)
+				for (j = 0; j < *M; j++)
+					for (i = 0; i < *K; i++)
+						C[j+(*LDC)*k] -= A[i+(*LDA)*j] * B[i+(*LDB)*k];
+		else
+			for (k = 0; k < *N; k++)
+				for (j = 0; j < *M; j++)
+					for (i = 0; i < *K; i++)
+						C[j+(*LDC)*k] += *ALPHA * A[i+(*LDA)*j] * B[i+(*LDB)*k];
+}
+
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/Bounds.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/Bounds.cpp
new file mode 100644
index 00000000000..59a5a945f4b
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/Bounds.cpp
@@ -0,0 +1,515 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file src/Bounds.cpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of the Bounds class designed to manage working sets of
+ *	bounds within a QProblem.
+ */
+
+
+#include <qpOASES/Bounds.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+/*
+ *	B o u n d s
+ */
+Bounds::Bounds( ) : SubjectTo( )
+{
+}
+
+
+/*
+ *	B o u n d s
+ */
+Bounds::Bounds( int _n ) : SubjectTo( _n )
+{
+	init( _n );
+}
+
+
+/*
+ *	B o u n d s
+ */
+Bounds::Bounds( const Bounds& rhs ) : SubjectTo( rhs )
+{
+	copy( rhs );
+}
+
+
+/*
+ *	~ B o u n d s
+ */
+Bounds::~Bounds( )
+{
+	clear( );
+}
+
+
+/*
+ *	o p e r a t o r =
+ */
+Bounds& Bounds::operator=( const Bounds& rhs )
+{
+	if ( this != &rhs )
+	{
+		clear( );
+		SubjectTo::operator=( rhs );
+		copy( rhs );
+	}
+
+	return *this;
+}
+
+
+
+/*
+ *	i n i t
+ */
+returnValue Bounds::init(	int _n
+							)
+{
+	if ( _n < 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	clear( );
+
+	if ( _n > 0 )
+	{
+		freee.init( _n );
+		fixed.init( _n );
+	}
+
+	return SubjectTo::init( _n );
+}
+
+
+
+/*
+ *	s e t u p B o u n d
+ */
+returnValue Bounds::setupBound(	int number, SubjectToStatus _status
+								)
+{
+	/* consistency check */
+	if ( ( number < 0 ) || ( number >= n ) )
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+
+	/* Add bound index to respective index list. */
+	switch ( _status )
+	{
+		case ST_INACTIVE:
+			if ( this->addIndex( this->getFree( ),number,_status ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_SETUP_BOUND_FAILED );
+			break;
+
+		case ST_LOWER:
+			if ( this->addIndex( this->getFixed( ),number,_status ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_SETUP_BOUND_FAILED );
+			break;
+
+		case ST_UPPER:
+			if ( this->addIndex( this->getFixed( ),number,_status ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_SETUP_BOUND_FAILED );
+			break;
+
+		default:
+			return THROWERROR( RET_INVALID_ARGUMENTS );
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p A l l F r e e
+ */
+returnValue Bounds::setupAllFree( )
+{
+	return setupAll( ST_INACTIVE );
+}
+
+
+/*
+ *	s e t u p A l l L o w e r
+ */
+returnValue Bounds::setupAllLower( )
+{
+	return setupAll( ST_LOWER );
+}
+
+
+/*
+ *	s e t u p A l l U p p e r
+ */
+returnValue Bounds::setupAllUpper( )
+{
+	return setupAll( ST_UPPER );
+}
+
+
+/*
+ *	m o v e F i x e d T o F r e e
+ */
+returnValue Bounds::moveFixedToFree( int number )
+{
+	/* consistency check */
+	if ( ( number < 0 ) || ( number >= n ) )
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+
+	/* Move index from indexlist of fixed variables to that of free ones. */
+	if ( this->removeIndex( this->getFixed( ),number ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_MOVING_BOUND_FAILED );
+
+	if ( this->addIndex( this->getFree( ),number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_MOVING_BOUND_FAILED );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	m o v e F r e e T o F i x e d
+ */
+returnValue Bounds::moveFreeToFixed(	int number, SubjectToStatus _status
+										)
+{
+	/* consistency check */
+	if ( ( number < 0 ) || ( number >= n ) )
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+
+	/* Move index from indexlist of free variables to that of fixed ones. */
+	if ( this->removeIndex( this->getFree( ),number ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_MOVING_BOUND_FAILED );
+
+	if ( this->addIndex( this->getFixed( ),number,_status ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_MOVING_BOUND_FAILED );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	f l i p F i x e d
+ */
+returnValue Bounds::flipFixed( int number )
+{
+	/* consistency check */
+	if ( ( number < 0 ) || ( number >= n ) )
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+
+	if ( status != 0 )
+		switch (status[number])
+		{
+			case ST_LOWER: status[number] = ST_UPPER; break;
+			case ST_UPPER: status[number] = ST_LOWER; break;
+			default: return THROWERROR( RET_MOVING_BOUND_FAILED );
+		}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s w a p F r e e
+ */
+returnValue Bounds::swapFree(	int number1, int number2
+								)
+{
+	/* consistency check */
+	if ( ( number1 < 0 ) || ( number1 >= n ) || ( number2 < 0 ) || ( number2 >= n ) )
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+
+	/* Swap index within indexlist of free variables. */
+	return this->swapIndex( this->getFree( ),number1,number2 );
+}
+
+
+/*
+ *	s h i f t
+ */
+returnValue Bounds::shift(	int offset )
+{
+	int i;
+
+	/* consistency check */
+	if ( ( offset == 0 ) || ( n <= 1 ) )
+		return SUCCESSFUL_RETURN;
+
+	if ( ( offset < 0 ) || ( offset > n/2 ) )
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+
+	if ( ( n % offset ) != 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+
+	/* 1) Shift types and status. */
+	for( i=0; i<n-offset; ++i )
+	{
+		setType( i,getType( i+offset ) );
+		setStatus( i,getStatus( i+offset ) );
+	}
+
+	/* 2) Construct shifted index lists of free and fixed variables. */
+	Indexlist shiftedFreee( n );
+	Indexlist shiftedFixed( n );
+
+	for( i=0; i<n; ++i )
+	{
+		switch ( getStatus( i ) )
+		{
+			case ST_INACTIVE:
+				if ( shiftedFreee.addNumber( i ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_SHIFTING_FAILED );
+				break;
+
+			case ST_LOWER:
+				if ( shiftedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_SHIFTING_FAILED );
+				break;
+
+			case ST_UPPER:
+				if ( shiftedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_SHIFTING_FAILED );
+				break;
+
+			default:
+				return THROWERROR( RET_SHIFTING_FAILED );
+		}
+	}
+
+	/* 3) Assign shifted index list. */
+	freee = shiftedFreee;
+	fixed = shiftedFixed;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	r o t a t e
+ */
+returnValue Bounds::rotate( int offset )
+{
+	int i;
+
+	/* consistency check */
+	if ( ( offset == 0 ) || ( offset == n ) || ( n <= 1 ) )
+		return SUCCESSFUL_RETURN;
+
+	if ( ( offset < 0 ) || ( offset > n ) )
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+
+
+	/* 1) Rotate types and status. */
+	SubjectToType*   typeTmp   = new SubjectToType[offset];
+	SubjectToStatus* statusTmp = new SubjectToStatus[offset];
+
+	for( i=0; i<offset; ++i )
+	{
+		typeTmp[i] = getType( i );
+		statusTmp[i] = getStatus( i );
+	}
+
+	for( i=0; i<n-offset; ++i )
+	{
+		setType( i,getType( i+offset ) );
+		setStatus( i,getStatus( i+offset ) );
+	}
+
+	for( i=n-offset; i<n; ++i )
+	{
+		setType( i,typeTmp[i-n+offset] );
+		setStatus( i,statusTmp[i-n+offset] );
+	}
+
+	delete[] statusTmp; delete[] typeTmp;
+
+	/* 2) Construct shifted index lists of free and fixed variables. */
+	Indexlist rotatedFreee( n );
+	Indexlist rotatedFixed( n );
+
+	for( i=0; i<n; ++i )
+	{
+		switch ( getStatus( i ) )
+		{
+			case ST_INACTIVE:
+				if ( rotatedFreee.addNumber( i ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_ROTATING_FAILED );
+				break;
+
+			case ST_LOWER:
+				if ( rotatedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_ROTATING_FAILED );
+				break;
+
+			case ST_UPPER:
+				if ( rotatedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_ROTATING_FAILED );
+				break;
+
+			default:
+				return THROWERROR( RET_ROTATING_FAILED );
+		}
+	}
+
+	/* 3) Assign shifted index list. */
+	freee = rotatedFreee;
+	fixed = rotatedFixed;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	p r i n t
+ */
+returnValue Bounds::print( )
+{
+	if ( n == 0 )
+		return SUCCESSFUL_RETURN;
+
+	#ifndef __XPCTARGET__
+	#ifndef __DSPACE__
+	char myPrintfString[MAX_STRING_LENGTH];
+
+	int nFR = getNFR( );
+	int nFX = getNFX( );
+
+	int* FR_idx;
+	getFree( )->getNumberArray( &FR_idx );
+
+	int* FX_idx;
+	getFixed( )->getNumberArray( &FX_idx );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"Bounds object comprising %d variables (%d free, %d fixed):\n",n,nFR,nFX );
+	myPrintf( myPrintfString );
+
+	REFER_NAMESPACE_QPOASES print( FR_idx,nFR,"free " );
+	REFER_NAMESPACE_QPOASES print( FX_idx,nFX,"fixed" );
+
+	#endif
+	#endif
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+/*
+ *	c l e a r
+ */
+returnValue Bounds::clear( )
+{
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	c o p y
+ */
+returnValue Bounds::copy(	const Bounds& rhs
+							)
+{
+	freee = rhs.freee;
+	fixed = rhs.fixed;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	s e t u p A l l
+ */
+returnValue Bounds::setupAll( SubjectToStatus _status )
+{
+	int i;
+
+	/* 1) Place unbounded variables at the beginning of the index list of free variables. */
+	for( i=0; i<n; ++i )
+	{
+		if ( getType( i ) == ST_UNBOUNDED )
+		{
+			if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_SETUP_BOUND_FAILED );
+		}
+	}
+
+	/* 2) Add remaining (i.e. bounded but possibly free) variables to the index list of free variables. */
+	for( i=0; i<n; ++i )
+	{
+		if ( getType( i ) == ST_BOUNDED )
+		{
+			if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_SETUP_BOUND_FAILED );
+		}
+	}
+
+	/* 3) Place implicitly fixed variables at the end of the index list of free variables. */
+	for( i=0; i<n; ++i )
+	{
+		if ( getType( i ) == ST_EQUALITY )
+		{
+			if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_SETUP_BOUND_FAILED );
+		}
+	}
+
+	/* 4) Moreover, add all bounds of unknown type. */
+	for( i=0; i<n; ++i )
+	{
+		if ( getType( i ) == ST_UNKNOWN || getType( i ) == ST_DISABLED )
+		{
+			if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_SETUP_BOUND_FAILED );
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/Constraints.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/Constraints.cpp
new file mode 100644
index 00000000000..c0ec04590d2
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/Constraints.cpp
@@ -0,0 +1,500 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file src/Constraints.cpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of the Constraints class designed to manage working sets of
+ *	constraints within a QProblem.
+ */
+
+
+#include <qpOASES/Constraints.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+/*
+ *	C o n s t r a i n t s
+ */
+Constraints::Constraints( ) : SubjectTo( )
+{
+}
+
+
+/*
+ *	C o n s t r a i n t s
+ */
+Constraints::Constraints( int _n ) : SubjectTo( _n )
+{
+	init( _n );
+}
+
+
+/*
+ *	C o n s t r a i n t s
+ */
+Constraints::Constraints( const Constraints& rhs ) : SubjectTo( rhs )
+{
+	copy( rhs );
+}
+
+
+/*
+ *	~ C o n s t r a i n t s
+ */
+Constraints::~Constraints( )
+{
+	clear( );
+}
+
+
+/*
+ *	o p e r a t o r =
+ */
+Constraints& Constraints::operator=( const Constraints& rhs )
+{
+	if ( this != &rhs )
+	{
+		clear( );
+		SubjectTo::operator=( rhs );
+		copy( rhs );
+	}
+
+	return *this;
+}
+
+
+/*
+ *	i n i t
+ */
+returnValue Constraints::init(	int _n
+								)
+{
+	if ( _n < 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	clear( );
+
+	if ( _n > 0 )
+	{
+		active.init(   _n );
+		inactive.init( _n );
+	}
+
+	return SubjectTo::init( _n );
+}
+
+
+
+/*
+ *	s e t u p C o n s t r a i n t
+ */
+returnValue Constraints::setupConstraint(	int number, SubjectToStatus _status
+											)
+{
+	/* consistency check */
+	if ( ( number < 0 ) || ( number >= n ) )
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+
+	/* Add constraint index to respective index list. */
+	switch ( _status )
+	{
+		case ST_INACTIVE:
+			if ( this->addIndex( this->getInactive( ),number,_status ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
+			break;
+
+		case ST_LOWER:
+			if ( this->addIndex( this->getActive( ),number,_status ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
+			break;
+
+		case ST_UPPER:
+			if ( this->addIndex( this->getActive( ),number,_status ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
+			break;
+
+		default:
+			return THROWERROR( RET_INVALID_ARGUMENTS );
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p A l l I n a c t i v e
+ */
+returnValue Constraints::setupAllInactive( )
+{
+	return setupAll( ST_INACTIVE );
+}
+
+
+/*
+ *	s e t u p A l l L o w e r
+ */
+returnValue Constraints::setupAllLower( )
+{
+	return setupAll( ST_LOWER );
+}
+
+
+/*
+ *	s e t u p A l l U p p e r
+ */
+returnValue Constraints::setupAllUpper( )
+{
+	return setupAll( ST_UPPER );
+}
+
+
+/*
+ *	m o v e A c t i v e T o I n a c t i v e
+ */
+returnValue Constraints::moveActiveToInactive( int number )
+{
+	/* consistency check */
+	if ( ( number < 0 ) || ( number >= n ) )
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+
+	/* Move index from indexlist of active constraints to that of inactive ones. */
+	if ( this->removeIndex( this->getActive( ),number ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_MOVING_BOUND_FAILED );
+
+	if ( this->addIndex( this->getInactive( ),number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_MOVING_BOUND_FAILED );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	m o v e I n a c t i v e T o A c t i v e
+ */
+returnValue Constraints::moveInactiveToActive(	int number, SubjectToStatus _status
+												)
+{
+	/* consistency check */
+	if ( ( number < 0 ) || ( number >= n ) )
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+
+	/* Move index from indexlist of inactive constraints to that of active ones. */
+	if ( this->removeIndex( this->getInactive( ),number ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_MOVING_BOUND_FAILED );
+
+	if ( this->addIndex( this->getActive( ),number,_status ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_MOVING_BOUND_FAILED );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	f l i p F i x e d
+ */
+returnValue Constraints::flipFixed( int number )
+{
+	/* consistency check */
+	if ( ( number < 0 ) || ( number >= n ) )
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+
+	if ( status != 0 )
+		switch (status[number])
+		{
+			case ST_LOWER: status[number] = ST_UPPER; break;
+			case ST_UPPER: status[number] = ST_LOWER; break;
+			default: return THROWERROR( RET_MOVING_CONSTRAINT_FAILED );
+		}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s h i f t
+ */
+returnValue Constraints::shift( int offset )
+{
+	int i;
+
+	/* consistency check */
+	if ( ( offset == 0 ) || ( n <= 1 ) )
+		return SUCCESSFUL_RETURN;
+
+	if ( ( offset < 0 ) || ( offset > n/2 ) )
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+
+	if ( ( n % offset ) != 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+
+	/* 1) Shift types and status. */
+	for( i=0; i<n-offset; ++i )
+	{
+		setType( i,getType( i+offset ) );
+		setStatus( i,getStatus( i+offset ) );
+	}
+
+	/* 2) Construct shifted index lists of free and fixed variables. */
+	Indexlist shiftedActive( n );
+	Indexlist shiftedInactive( n );
+
+	for( i=0; i<n; ++i )
+	{
+		switch ( getStatus( i ) )
+		{
+			case ST_INACTIVE:
+				if ( shiftedInactive.addNumber( i ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_SHIFTING_FAILED );
+				break;
+
+			case ST_LOWER:
+				if ( shiftedActive.addNumber( i ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_SHIFTING_FAILED );
+				break;
+
+			case ST_UPPER:
+				if ( shiftedActive.addNumber( i ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_SHIFTING_FAILED );
+				break;
+
+			default:
+				return THROWERROR( RET_SHIFTING_FAILED );
+		}
+	}
+
+	/* 3) Assign shifted index list. */
+	active = shiftedActive;
+	inactive = shiftedInactive;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	r o t a t e
+ */
+returnValue Constraints::rotate( int offset )
+{
+	int i;
+
+	/* consistency check */
+	if ( ( offset == 0 ) || ( offset == n ) || ( n <= 1 ) )
+		return SUCCESSFUL_RETURN;
+
+	if ( ( offset < 0 ) || ( offset > n ) )
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+
+
+	/* 1) Rotate types and status. */
+	SubjectToType*   typeTmp   = new SubjectToType[offset];
+	SubjectToStatus* statusTmp = new SubjectToStatus[offset];
+
+	for( i=0; i<offset; ++i )
+	{
+		typeTmp[i] = getType( i );
+		statusTmp[i] = getStatus( i );
+	}
+
+	for( i=0; i<n-offset; ++i )
+	{
+		setType( i,getType( i+offset ) );
+		setStatus( i,getStatus( i+offset ) );
+	}
+
+	for( i=n-offset; i<n; ++i )
+	{
+		setType( i,typeTmp[i-n+offset] );
+		setStatus( i,statusTmp[i-n+offset] );
+	}
+
+	delete[] statusTmp; delete[] typeTmp;
+
+	/* 2) Construct shifted index lists of free and fixed variables. */
+	Indexlist rotatedActive( n );
+	Indexlist rotatedInactive( n );
+
+	for( i=0; i<n; ++i )
+	{
+		switch ( getStatus( i ) )
+		{
+			case ST_INACTIVE:
+				if ( rotatedInactive.addNumber( i ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_ROTATING_FAILED );
+				break;
+
+			case ST_LOWER:
+				if ( rotatedActive.addNumber( i ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_ROTATING_FAILED );
+				break;
+
+			case ST_UPPER:
+				if ( rotatedActive.addNumber( i ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_ROTATING_FAILED );
+				break;
+
+			default:
+				return THROWERROR( RET_ROTATING_FAILED );
+		}
+	}
+
+	/* 3) Assign shifted index list. */
+	active = rotatedActive;
+	inactive = rotatedInactive;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	p r i n t
+ */
+returnValue Constraints::print( )
+{
+	if ( n == 0 )
+		return SUCCESSFUL_RETURN;
+
+	#ifndef __XPCTARGET__
+	#ifndef __DSPACE__
+	char myPrintfString[MAX_STRING_LENGTH];
+
+	int nIAC = getNIAC( );
+	int nAC  = getNAC( );
+
+	int* IAC_idx;
+	getInactive( )->getNumberArray( &IAC_idx );
+
+	int* AC_idx;
+	getActive( )->getNumberArray( &AC_idx );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"Constraints object comprising %d constraints (%d inactive, %d active):\n",n,nIAC,nAC );
+	myPrintf( myPrintfString );
+
+	REFER_NAMESPACE_QPOASES print( IAC_idx,nIAC,"inactive" );
+	REFER_NAMESPACE_QPOASES print( AC_idx, nAC, "active  " );
+
+	#endif
+	#endif
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+/*
+ *	c l e a r
+ */
+returnValue Constraints::clear( )
+{
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	c o p y
+ */
+returnValue Constraints::copy(	const Constraints& rhs
+								)
+{
+	active   = rhs.active;
+	inactive = rhs.inactive;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	s e t u p A l l
+ */
+returnValue Constraints::setupAll( SubjectToStatus _status )
+{
+	int i;
+
+	/* 1) Place unbounded constraints at the beginning of the index list of inactive constraints. */
+	for( i=0; i<n; ++i )
+	{
+		if ( getType( i ) == ST_UNBOUNDED )
+		{
+			if ( setupConstraint( i,_status ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
+		}
+	}
+
+	/* 2) Add remaining (i.e. "real" inequality) constraints to the index list of inactive constraints. */
+	for( i=0; i<n; ++i )
+	{
+		if ( getType( i ) == ST_BOUNDED )
+		{
+			if ( setupConstraint( i,_status ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
+		}
+	}
+
+	/* 3) Place implicit equality constraints at the end of the index list of inactive constraints. */
+	for( i=0; i<n; ++i )
+	{
+		if ( getType( i ) == ST_EQUALITY )
+		{
+			if ( setupConstraint( i,_status ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
+		}
+	}
+
+	/* 4) Moreover, add all constraints of unknown type. */
+	for( i=0; i<n; ++i )
+	{
+		if ( getType( i ) == ST_UNKNOWN || getType( i ) == ST_DISABLED )
+		{
+			if ( setupConstraint( i,_status ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
+		}
+	}
+
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/Flipper.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/Flipper.cpp
new file mode 100644
index 00000000000..879d4b054d8
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/Flipper.cpp
@@ -0,0 +1,273 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file src/Flipper.cpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of the Flipper class designed to manage working sets of
+ *	constraints and bounds within a QProblem.
+ */
+
+
+#include <qpOASES/Flipper.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+/*
+ *	F l i p p e r
+ */
+Flipper::Flipper( )
+{
+	R = 0;
+	Q = 0;
+	T = 0;
+	
+	init( );
+}
+
+
+/*
+ *	F l i p p e r
+ */
+Flipper::Flipper(	unsigned int _nV,
+					unsigned int _nC
+					)
+{
+	R = 0;
+	Q = 0;
+	T = 0;
+	
+	init( _nV,_nC );
+}
+
+
+/*
+ *	F l i p p e r
+ */
+Flipper::Flipper( const Flipper& rhs )
+{
+	R = 0;
+	Q = 0;
+	T = 0;
+
+	copy( rhs );
+}
+
+
+/*
+ *	~ F l i p p e r
+ */
+Flipper::~Flipper( )
+{
+	clear( );
+}
+
+
+/*
+ *	o p e r a t o r =
+ */
+Flipper& Flipper::operator=( const Flipper& rhs )
+{
+	if ( this != &rhs )
+	{
+		clear( );
+		copy( rhs );
+	}
+
+	return *this;
+}
+
+
+
+/*
+ *	i n i t
+ */
+returnValue Flipper::init(	unsigned int _nV,
+							unsigned int _nC
+							)
+{
+	if ( ( _nV < 0 ) || ( _nC < 0 ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	clear( );
+
+	nV = _nV;
+	nC = _nC;
+
+	/*if ( nV > 0 )
+	{
+		R = new real_t[nV*nV];
+	
+		if ( nC > 0 )
+		{
+			Q = new real_t[nV*nV];
+			T = new real_t[getDimT()];
+		}
+	}*/
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	g e t
+ */
+returnValue Flipper::get(	Bounds* const _bounds,
+							real_t* const _R,
+							Constraints* const _constraints,
+							real_t* const _Q,
+							real_t* const _T 
+							) const
+{
+	if ( _bounds != 0 )
+		*_bounds = bounds;
+
+	if ( _constraints != 0 )
+		*_constraints = constraints;
+
+	if ( ( _R != 0 ) && ( R != 0 ) )
+		memcpy( _R,R, nV*nV*sizeof(real_t) );
+
+	if ( ( _Q != 0 ) && ( Q != 0 ) )
+		memcpy( _Q,Q, nV*nV*sizeof(real_t) );
+
+	if ( ( _T != 0 ) && ( T != 0 ) )
+		memcpy( _T,T, getDimT()*sizeof(real_t) );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t
+ */
+returnValue Flipper::set(	const Bounds* const _bounds,
+							const real_t* const _R,
+							const Constraints* const _constraints,
+							const real_t* const _Q,
+							const real_t* const _T
+							)
+{
+	if ( _bounds != 0 )
+		bounds = *_bounds;
+
+	if ( _constraints != 0 )
+		constraints = *_constraints;
+
+	if ( _R != 0 )
+	{
+		if ( R == 0 )
+			R = new real_t[nV*nV];
+
+		memcpy( R,_R, nV*nV*sizeof(real_t) );
+	}
+
+	if ( _Q != 0 )
+	{
+		if ( Q == 0 )
+			Q = new real_t[nV*nV];
+
+		memcpy( Q,_Q, nV*nV*sizeof(real_t) );
+	}
+
+	if ( _T != 0 )
+	{
+		if ( T == 0 )
+			T = new real_t[getDimT()];
+
+		memcpy( T,_T, getDimT()*sizeof(real_t) );
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+/*
+ *	c l e a r
+ */
+returnValue Flipper::clear( )
+{
+	if ( R != 0 )
+	{
+		delete[] R;
+		R = 0;
+	}
+	
+	if ( Q != 0 )
+	{
+		delete[] Q;
+		Q = 0;
+	}
+	
+	if ( T != 0 )
+	{
+		delete[] T;
+		T = 0;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	c o p y
+ */
+returnValue Flipper::copy(	const Flipper& rhs
+							)
+{
+	return set( &(rhs.bounds),rhs.R, &(rhs.constraints),rhs.Q,rhs.T );
+}
+
+
+unsigned int Flipper::getDimT( ) const
+{
+	if ( nV > nC )
+		return nC*nC;
+	else
+		return nV*nV;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/Indexlist.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/Indexlist.cpp
new file mode 100644
index 00000000000..dc2bac3179b
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/Indexlist.cpp
@@ -0,0 +1,319 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file src/Indexlist.cpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of the Indexlist class designed to manage index lists of
+ *	constraints and bounds within a QProblem_SubjectTo.
+ */
+
+
+#include <qpOASES/Indexlist.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+/*
+ *	I n d e x l i s t
+ */
+Indexlist::Indexlist( )
+{
+	number = 0;
+	iSort  = 0;
+
+	init( );
+}
+
+
+/*
+ *	I n d e x l i s t
+ */
+Indexlist::Indexlist( int n )
+{
+	number = 0;
+	iSort  = 0;
+
+	init( n );
+}
+
+
+/*
+ *	I n d e x l i s t
+ */
+Indexlist::Indexlist( const Indexlist& rhs )
+{
+	copy( rhs );
+}
+
+
+/*
+ *	~ I n d e x l i s t
+ */
+Indexlist::~Indexlist( )
+{
+	clear( );
+}
+
+
+/*
+ *	o p e r a t o r =
+ */
+Indexlist& Indexlist::operator=( const Indexlist& rhs )
+{
+	if ( this != &rhs )
+	{
+		clear( );
+		copy( rhs );
+	}
+
+	return *this;
+}
+
+
+
+/*
+ *	i n i t
+ */
+returnValue Indexlist::init(	int n
+								)
+{
+	if ( n < 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	clear( );
+
+	length = 0;
+	physicallength = n;
+
+	if ( n > 0 )
+	{
+		number = new int[n];
+		iSort  = new int[n];
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t N u m b e r A r r a y
+ */
+returnValue Indexlist::getNumberArray( int** const numberarray ) const
+{
+	if (numberarray == 0)
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	*numberarray = number;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t I S o r t A r r a y
+ */
+returnValue Indexlist::getISortArray( int** const iSortArray ) const
+{
+	*iSortArray = iSort;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t I n d e x
+ */
+int Indexlist::getIndex( int givennumber ) const
+{
+	int index = findInsert(givennumber);
+	return number[iSort[index]] == givennumber ? iSort[index] : -1;
+}
+
+
+/*
+ *	a d d N u m b e r
+ */
+returnValue Indexlist::addNumber( int addnumber )
+{
+	if ( length >= physicallength )
+		return THROWERROR( RET_INDEXLIST_EXCEEDS_MAX_LENGTH );
+
+	int i, j;
+	number[length] = addnumber;
+	j = findInsert(addnumber);
+	for (i = length; i > j+1; i--)
+		iSort[i] = iSort[i-1];
+	iSort[j+1] = length;
+	++length;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	r e m o v e N u m b e r
+ */
+returnValue Indexlist::removeNumber( int removenumber )
+{
+	int i;
+	int idx = findInsert( removenumber );
+	int iSidx = iSort[idx];
+
+	/* nothing to be done if number is not contained in index set */
+	if ( number[iSidx] != removenumber )
+		return SUCCESSFUL_RETURN;
+
+	/* update sorted indices iSort first */
+	for (i = 0; i < length; i++)
+		if (iSort[i] > iSidx) iSort[i]--;
+	for (i = idx+1; i < length; i++)
+		iSort[i-1] = iSort[i];
+
+	/* remove from numbers list */
+	for( i=iSidx; i<length-1; ++i )
+		number[i] = number[i+1];
+	number[length-1] = -1;
+
+	--length;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s w a p N u m b e r s
+ */
+returnValue Indexlist::swapNumbers( int number1, int number2 )
+{
+	int index1 = findInsert( number1 );
+	int index2 = findInsert( number2 );
+
+	/* consistency check */
+	if ( ( number[iSort[index1]] != number1 ) || ( number[iSort[index2]] != number2 ) )
+		return THROWERROR( RET_INDEXLIST_CORRUPTED );
+
+	int tmp;
+	/* swap numbers */
+	tmp = number[iSort[index1]];
+	number[iSort[index1]] = number[iSort[index2]];
+	number[iSort[index2]] = tmp;
+	/* swap sorting indices */
+	tmp = iSort[index1];
+	iSort[index1] = iSort[index2];
+	iSort[index2] = tmp;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+/*
+ *	c l e a r
+ */
+returnValue Indexlist::clear( )
+{
+	if ( iSort != 0 )
+	{
+		delete[] iSort;
+		iSort = 0;
+	}
+
+	if ( number != 0 )
+	{
+		delete[] number;
+		number = 0;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	c o p y
+ */
+returnValue Indexlist::copy(	const Indexlist& rhs
+								)
+{
+	int i;
+
+	length = rhs.length;
+	physicallength = rhs.physicallength;
+
+	if ( rhs.number != 0 )
+	{
+		number = new int[physicallength];
+		for( i=0; i<physicallength; ++i )
+			number[i] = rhs.number[i];
+		iSort = new int[physicallength];
+		for( i=0; i<physicallength; ++i )
+			iSort[i] = rhs.iSort[i];
+	}
+	else
+	{
+		number = 0;
+		iSort = 0;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+int Indexlist::findInsert(int i) const
+{
+	/* quick check if index can be appended */
+	if (length == 0 || i < number[iSort[0]]) return -1;
+	if (i >= number[iSort[length-1]]) return length-1;
+
+	/* otherwise, perform bisection search */
+	int fst = 0, lst = length-1, mid;
+
+	while (fst < lst - 1)
+	{
+		mid = (fst + lst) / 2;
+		if (i >= number[iSort[mid]]) fst = mid;
+		else lst = mid;
+	}
+
+	return fst;
+}
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/LAPACKReplacement.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/LAPACKReplacement.cpp
new file mode 100644
index 00000000000..e7bac2a37e3
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/LAPACKReplacement.cpp
@@ -0,0 +1,120 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file src/LAPACKReplacement.cpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *  LAPACK replacement routines.
+ */
+
+
+#include <qpOASES/Utils.hpp>
+
+
+extern "C" void dpotrf_(	const char *uplo, const unsigned long *_n, double *a,
+							const unsigned long *_lda, long *info
+							)
+{
+	double sum;
+	long i, j, k;
+	long n = (long)(*_n);
+	long lda = (long)(*_lda);
+
+	for( i=0; i<n; ++i )
+	{
+		/* j == i */
+		sum = a[i + lda*i];
+
+		for( k=(i-1); k>=0; --k )
+			sum -= a[k+lda*i] * a[k+lda*i];
+
+		if ( sum > 0.0 )
+			a[i+lda*i] = REFER_NAMESPACE_QPOASES getSqrt( sum );
+		else
+		{
+			a[0] = sum; /* tunnel negative diagonal element to caller */
+			if (info != 0)
+				*info = (long)i+1;
+			return;
+		}
+
+		for( j=(i+1); j<n; ++j )
+		{
+			sum = a[j*lda + i];
+
+			for( k=(i-1); k>=0; --k )
+				sum -= a[k+lda*i] * a[k+lda*j];
+
+			a[i+lda*j] = sum / a[i+lda*i];
+		}
+	}
+	if (info != 0)
+		*info = 0;
+}
+
+
+extern "C" void spotrf_(	const char *uplo, const unsigned long *_n, float *a,
+							const unsigned long *_lda, long *info
+							)
+{
+	float sum;
+	long i, j, k;
+	long n = (long)(*_n);
+	long lda = (long)(*_lda);
+
+	for( i=0; i<n; ++i )
+	{
+		/* j == i */
+		sum = a[i + lda*i];
+
+		for( k=(i-1); k>=0; --k )
+			sum -= a[k+lda*i] * a[k+lda*i];
+
+		if ( sum > 0.0 )
+			a[i+lda*i] = (float)(REFER_NAMESPACE_QPOASES getSqrt( sum ));
+		else
+		{
+			a[0] = sum; /* tunnel negative diagonal element to caller */
+			if (info != 0)
+				*info = (long)i+1;
+			return;
+		}
+
+		for( j=(i+1); j<n; ++j )
+		{
+			sum = a[j*lda + i];
+
+			for( k=(i-1); k>=0; --k )
+				sum -= a[k+lda*i] * a[k+lda*j];
+
+			a[i+lda*j] = sum / a[i+lda*i];
+		}
+	}
+	if (info != 0)
+		*info = 0;
+}
+
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/Matrices.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/Matrices.cpp
new file mode 100644
index 00000000000..e962e383da1
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/Matrices.cpp
@@ -0,0 +1,1967 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file src/Matrices.cpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of the matrix classes.
+ */
+
+
+#include <qpOASES/Matrices.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+
+DenseMatrix::~DenseMatrix()
+{
+	if ( needToFreeMemory( ) == BT_TRUE )
+		free( );
+}
+
+void DenseMatrix::free( )
+{
+	if (val != 0)
+		delete[] val;
+	val = 0;
+}
+
+Matrix *DenseMatrix::duplicate( ) const
+{
+	DenseMatrix *dupl = 0;
+
+	if ( needToFreeMemory( ) == BT_TRUE )
+	{
+		real_t* val_new = new real_t[nRows*nCols];
+		memcpy( val_new,val, ((unsigned int)(nRows*nCols))*sizeof(real_t) );
+		dupl = new DenseMatrix(nRows, nCols, nCols, val_new);
+		dupl->doFreeMemory( );
+	}
+	else
+	{
+		dupl = new DenseMatrix(nRows, nCols, nCols, val);
+	}
+
+	return dupl;
+}
+
+real_t DenseMatrix::diag(	int i
+							) const
+{
+	return val[i*(leaDim+1)];
+}
+
+BooleanType DenseMatrix::isDiag( ) const
+{
+	int i, j;
+
+	if (nRows != nCols)
+		return BT_FALSE;
+
+	for ( i=0; i<nRows; ++i )
+		for ( j=0; j<i; ++j )
+			if ( ( getAbs( val[i*leaDim+j] ) > EPS ) || ( getAbs( val[j*leaDim+i] ) > EPS ) )
+				return BT_FALSE;
+
+	return BT_TRUE;
+}
+
+
+real_t DenseMatrix::getNorm(	int type
+								) const
+{
+	return REFER_NAMESPACE_QPOASES getNorm( val,nCols*nRows,type ); 
+}
+
+
+real_t DenseMatrix::getRowNorm( int rNum, int type ) const
+{
+	return REFER_NAMESPACE_QPOASES getNorm( &(val[rNum*leaDim]),nCols,type ); 
+}
+
+returnValue DenseMatrix::getRow(int rNum, const Indexlist* const icols, real_t alpha, real_t *row) const
+{
+	int i;
+    if (icols != 0)
+    {
+	    if ( isEqual(alpha,1.0) == BT_TRUE )
+		    for (i = 0; i < icols->length; i++)
+			    row[i] = val[rNum*leaDim+icols->number[i]];
+	    else if ( isEqual(alpha,-1.0) == BT_TRUE )
+		    for (i = 0; i < icols->length; i++)
+			    row[i] = -val[rNum*leaDim+icols->number[i]];
+	    else
+		    for (i = 0; i < icols->length; i++)
+			    row[i] = alpha*val[rNum*leaDim+icols->number[i]];
+    }
+    else
+    {
+	    if ( isEqual(alpha,1.0) == BT_TRUE )
+		    for (i = 0; i < nCols; i++)
+			    row[i] = val[rNum*leaDim+i];
+	    else if ( isEqual(alpha,-1.0) == BT_TRUE )
+		    for (i = 0; i < nCols; i++)
+			    row[i] = -val[rNum*leaDim+i];
+	    else
+		    for (i = 0; i < nCols; i++)
+			    row[i] = alpha*val[rNum*leaDim+i];
+    }
+	return SUCCESSFUL_RETURN;
+}
+
+
+returnValue DenseMatrix::getCol(int cNum, const Indexlist* const irows, real_t alpha, real_t *col) const
+{
+	int i;
+
+	if ( isEqual(alpha,1.0) == BT_TRUE )
+		for (i = 0; i < irows->length; i++)
+			col[i] = val[irows->number[i]*leaDim+cNum];
+	else if ( isEqual(alpha,-1.0) == BT_TRUE )
+		for (i = 0; i < irows->length; i++)
+			col[i] = -val[irows->number[i]*leaDim+cNum];
+	else
+		for (i = 0; i < irows->length; i++)
+			col[i] = alpha*val[irows->number[i]*leaDim+cNum];
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+returnValue DenseMatrix::times(	int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
+{
+	unsigned long _xN     = (unsigned long)xN;
+	unsigned long _nRows  = (unsigned long)nRows;
+	unsigned long _nCols  = (unsigned long)nCols;
+	unsigned long _leaDim = (unsigned long)getMax(1,nCols);
+	unsigned long _xLD    = (unsigned long)getMax(1,xLD);
+	unsigned long _yLD    = (unsigned long)getMax(1,yLD);
+
+	/* Call BLAS. Mind row major format! */
+	GEMM("TRANS", "NOTRANS", &_nRows, &_xN, &_nCols, &alpha, val, &_leaDim, x, &_xLD, &beta, y, &_yLD);
+	return SUCCESSFUL_RETURN;
+}
+
+returnValue DenseMatrix::transTimes( int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
+{
+	unsigned long _xN     = (unsigned long)xN;
+	unsigned long _nRows  = (unsigned long)nRows;
+	unsigned long _nCols  = (unsigned long)nCols;
+	unsigned long _leaDim = (unsigned long)getMax(1,nCols);
+	unsigned long _xLD    = (unsigned long)getMax(1,xLD);
+	unsigned long _yLD    = (unsigned long)getMax(1,yLD);
+
+	/* Call BLAS. Mind row major format! */
+	GEMM("NOTRANS", "NOTRANS", &_nCols, &_xN, &_nRows, &alpha, val, &_leaDim, x, &_xLD, &beta, y, &_yLD);
+	return SUCCESSFUL_RETURN;
+}
+
+returnValue DenseMatrix::times(	const Indexlist* const irows, const Indexlist* const icols,
+								int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD,
+								BooleanType yCompr) const
+{
+	int i, j, k, row, col, iy, irA;
+
+	if (yCompr == BT_TRUE)
+	{
+		if ( isZero(beta) == BT_TRUE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[j+k*yLD] = 0.0;
+		else if ( isEqual(beta,-1.0) == BT_TRUE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[j+k*yLD] = -y[j+k*yLD];
+		else if ( isEqual(beta,1.0) == BT_FALSE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[j+k*yLD] *= beta;
+
+		if (icols == 0)
+			if ( isEqual(alpha,1.0) == BT_TRUE )
+				for (k = 0; k < xN; k++)
+					for (j = 0; j < irows->length; j++)
+					{
+						row = irows->iSort[j];
+						iy = row + k * yLD;
+						irA = irows->number[row] * leaDim;
+						for (i = 0; i < nCols; i++)
+							y[iy] += val[irA+i] * x[k*xLD+i];
+					}
+			else if ( isEqual(alpha,-1.0) == BT_TRUE )
+				for (k = 0; k < xN; k++)
+					for (j = 0; j < irows->length; j++)
+					{
+						row = irows->iSort[j];
+						iy = row + k * yLD;
+						irA = irows->number[row] * leaDim;
+						for (i = 0; i < nCols; i++)
+							y[iy] -= val[irA+i] * x[k*xLD+i];
+					}
+			else
+				for (k = 0; k < xN; k++)
+					for (j = 0; j < irows->length; j++)
+					{
+						row = irows->iSort[j];
+						iy = row + k * yLD;
+						irA = irows->number[row] * leaDim;
+						for (i = 0; i < nCols; i++)
+							y[iy] += alpha * val[irA+i] * x[k*xLD+i];
+					}
+		else /* icols != 0 */
+			if ( isEqual(alpha,1.0) == BT_TRUE )
+				for (k = 0; k < xN; k++)
+					for (j = 0; j < irows->length; j++)
+					{
+						row = irows->iSort[j];
+						iy = row + k * yLD;
+						irA = irows->number[row] * leaDim;
+						for (i = 0; i < icols->length; i++)
+						{
+							col = icols->iSort[i];
+							y[iy] += val[irA+icols->number[col]] * x[k*xLD+col];
+						}
+					}
+			else if ( isEqual(alpha,-1.0) == BT_TRUE )
+				for (k = 0; k < xN; k++)
+					for (j = 0; j < irows->length; j++)
+					{
+						row = irows->iSort[j];
+						iy = row + k * yLD;
+						irA = irows->number[row] * leaDim;
+						for (i = 0; i < icols->length; i++)
+						{
+							col = icols->iSort[i];
+							y[iy] -= val[irA+icols->number[col]] * x[k*xLD+col];
+						}
+					}
+			else
+				for (k = 0; k < xN; k++)
+					for (j = 0; j < irows->length; j++)
+					{
+						row = irows->iSort[j];
+						iy = row + k * yLD;
+						irA = irows->number[row] * leaDim;
+						for (i = 0; i < icols->length; i++)
+						{
+							col = icols->iSort[i];
+							y[iy] += alpha * val[irA+icols->number[col]] * x[k*xLD+col];
+						}
+					}
+	}
+	else /* y not compressed */
+	{
+		if ( isZero(beta) == BT_TRUE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[irows->number[j]+k*yLD] = 0.0;
+		else if ( isEqual(beta,-1.0) == BT_TRUE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[irows->number[j]+k*yLD] = -y[j+k*yLD];
+		else if ( isEqual(beta,1.0) == BT_FALSE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[irows->number[j]+k*yLD] *= beta;
+
+		if (icols == 0)
+			if ( isEqual(alpha,1.0) == BT_TRUE )
+				for (k = 0; k < xN; k++)
+					for (j = 0; j < irows->length; j++)
+					{
+						row = irows->number[irows->iSort[j]];
+						iy = row + k * yLD;
+						irA = row * leaDim;
+						for (i = 0; i < nCols; i++)
+							y[iy] += val[irA+i] * x[k*xLD+i];
+					}
+			else if ( isEqual(alpha,-1.0) == BT_TRUE )
+				for (k = 0; k < xN; k++)
+					for (j = 0; j < irows->length; j++)
+					{
+						row = irows->number[irows->iSort[j]];
+						iy = row + k * yLD;
+						irA = row * leaDim;
+						for (i = 0; i < nCols; i++)
+							y[iy] -= val[irA+i] * x[k*xLD+i];
+					}
+			else
+				for (k = 0; k < xN; k++)
+					for (j = 0; j < irows->length; j++)
+					{
+						row = irows->number[irows->iSort[j]];
+						iy = row + k * yLD;
+						irA = row * leaDim;
+						for (i = 0; i < nCols; i++)
+							y[iy] += alpha * val[irA+i] * x[k*xLD+i];
+					}
+		else /* icols != 0 */
+			if ( isEqual(alpha,1.0) == BT_TRUE )
+				for (k = 0; k < xN; k++)
+					for (j = 0; j < irows->length; j++)
+					{
+						row = irows->number[irows->iSort[j]];
+						iy = row + k * yLD;
+						irA = row * leaDim;
+						for (i = 0; i < icols->length; i++)
+						{
+							col = icols->iSort[i];
+							y[iy] += val[irA+icols->number[col]] * x[k*xLD+col];
+						}
+					}
+			else if ( isEqual(alpha,-1.0) == BT_TRUE )
+				for (k = 0; k < xN; k++)
+					for (j = 0; j < irows->length; j++)
+					{
+						row = irows->number[irows->iSort[j]];
+						iy = row + k * yLD;
+						irA = row * leaDim;
+						for (i = 0; i < icols->length; i++)
+						{
+							col = icols->iSort[i];
+							y[iy] -= val[irA+icols->number[col]] * x[k*xLD+col];
+						}
+					}
+			else
+				for (k = 0; k < xN; k++)
+					for (j = 0; j < irows->length; j++)
+					{
+						row = irows->number[irows->iSort[j]];
+						iy = row + k * yLD;
+						irA = row * leaDim;
+						for (i = 0; i < icols->length; i++)
+						{
+							col = icols->iSort[i];
+							y[iy] += alpha * val[irA+icols->number[col]] * x[k*xLD+col];
+						}
+					}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+returnValue DenseMatrix::transTimes(	const Indexlist* const irows, const Indexlist* const icols,
+										int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
+{
+	int i, j, k, row, col;
+
+	if ( isZero(beta) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < icols->length; j++)
+				y[j+k*yLD] = 0.0;
+	else if ( isEqual(beta,-1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < icols->length; j++)
+				y[j+k*yLD] = -y[j+k*yLD];
+	else if ( isEqual(beta,1.0) == BT_FALSE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < icols->length; j++)
+				y[j+k*yLD] *= beta;
+
+	if ( isEqual(alpha,1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < irows->length; j++)
+			{
+				row = irows->iSort[j];
+				for (i = 0; i < icols->length; i++)
+				{
+					col = icols->iSort[i];
+					y[col+k*yLD] += val[irows->number[row]*leaDim+icols->number[col]] * x[row+k*xLD];
+				}
+			}
+	else if ( isEqual(alpha,-1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < irows->length; j++)
+			{
+				row = irows->iSort[j];
+				for (i = 0; i < icols->length; i++)
+				{
+					col = icols->iSort[i];
+					y[col+k*yLD] -= val[irows->number[row]*leaDim+icols->number[col]] * x[row+k*xLD];
+				}
+			}
+	else
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < irows->length; j++)
+			{
+				row = irows->iSort[j];
+				for (i = 0; i < icols->length; i++)
+				{
+					col = icols->iSort[i];
+					y[col+k*yLD] += alpha * val[irows->number[row]*leaDim+icols->number[col]] * x[row+k*xLD];
+				}
+			}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+returnValue DenseMatrix::addToDiag( real_t alpha )
+{
+	int i;
+	for (i = 0; i < nRows && i < nCols; i++)
+		val[i*(leaDim+1)] += alpha;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+real_t* DenseMatrix::full() const
+{
+	real_t* v = new real_t[nRows*nCols];
+	memcpy( v,val, ((unsigned int)(nRows*nCols))*sizeof(real_t) );
+	return v;
+}
+
+
+returnValue DenseMatrix::print( const char* name ) const
+{
+	return REFER_NAMESPACE_QPOASES print( val,nRows,nCols,name );
+}
+
+
+
+Matrix *SymDenseMat::duplicate( ) const
+{
+	return duplicateSym();
+}
+
+
+SymmetricMatrix *SymDenseMat::duplicateSym( ) const
+{
+	/* "same" as duplicate() in DenseMatrix */
+	SymDenseMat *dupl = 0;
+
+	if ( needToFreeMemory( ) == BT_TRUE )
+	{
+		real_t* val_new = new real_t[nRows*nCols];
+		memcpy( val_new,val, ((unsigned int)(nRows*nCols))*sizeof(real_t) );
+		dupl = new SymDenseMat(nRows, nCols, nCols, val_new);
+		dupl->doFreeMemory( );
+	}
+	else
+	{
+		dupl = new SymDenseMat(nRows, nCols, nCols, val);
+	}
+
+	return dupl;
+}
+
+
+returnValue SymDenseMat::bilinear(	const Indexlist* const icols,
+									int xN, const real_t *x, int xLD, real_t *y, int yLD) const
+{
+	int ii, jj, kk, col;
+	int i,j,k,irA;
+
+	for (ii = 0; ii < xN; ii++)
+		for (jj = 0; jj < xN; jj++)
+			y[ii*yLD+jj] = 0.0;
+
+	real_t *Ax = new real_t[icols->length * xN];
+
+	for (i=0;i<icols->length * xN;++i)
+		Ax[i]=0.0;
+
+	/* exploit symmetry of A ! */
+	for (j = 0; j < icols->length; j++) {
+		irA = icols->number[j] * leaDim;
+		for (i = 0; i < icols->length; i++)
+		{
+			real_t h = val[irA+icols->number[i]];
+			for (k = 0; k < xN; k++)
+				Ax[j + k * icols->length] += h * x[k*xLD+icols->number[i]];
+		}
+	}
+
+	for (ii = 0; ii < icols->length; ++ii) {
+		col = icols->number[ii];
+		for (jj = 0; jj < xN; ++jj) {
+			for (kk = 0; kk < xN; ++kk) {
+				y[kk + jj*yLD] += x[col + jj*xLD] * Ax[ii + kk*icols->length];
+			}
+		}
+	}
+	delete[] Ax;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+SparseMatrix::SparseMatrix() : nRows(0), nCols(0), ir(0), jc(0), jd(0), val(0) {}
+
+
+SparseMatrix::SparseMatrix( int nr, int nc, sparse_int_t *r, sparse_int_t *c, real_t *v )
+								: nRows(nr), nCols(nc), ir(r), jc(c), jd(0), val(v) { doNotFreeMemory(); }
+
+
+SparseMatrix::SparseMatrix( int nr, int nc, int ld, const real_t * const v ) 
+								: nRows(nr), nCols(nc), jd(0)
+{
+	int i, j, nnz;
+
+	jc = new sparse_int_t[nc+1];
+	ir = new sparse_int_t[nr*nc];
+	val = new real_t[nr*nc];
+
+	nnz = 0;
+	for (j = 0; j < nCols; j++)
+	{
+		jc[j] = nnz;
+		for (i = 0; i < nRows; i++)
+			if ( ( isZero( v[i*ld+j],0.0 ) == BT_FALSE ) || ( i == j ) ) /* also include zero diagonal elemets! */
+			{
+				ir[nnz] = i;
+				val[nnz++] = v[i*ld+j];
+			}
+	}
+	jc[nCols] = nnz;
+
+	doFreeMemory( );
+}
+
+
+SparseMatrix::~SparseMatrix()
+{
+	if (jd != 0)
+	{
+		delete[] jd;
+		jd = 0;
+	}
+
+	if ( needToFreeMemory() == BT_TRUE )
+		free( );
+}
+
+
+void SparseMatrix::free( )
+{
+	if (ir != 0) delete[] ir;
+	ir = 0;
+	if (jc != 0) delete[] jc;
+	jc = 0;
+	if (val != 0) delete[] val;
+	val = 0;
+
+	doNotFreeMemory( );
+}
+
+Matrix *SparseMatrix::duplicate() const
+{
+	long i, length = jc[nCols];
+	SparseMatrix *dupl = new SparseMatrix;
+
+	dupl->nRows = nRows;
+	dupl->nCols = nCols;
+	dupl->ir = new sparse_int_t[length];
+	dupl->jc = new sparse_int_t[nCols+1];
+	dupl->val = new real_t[length];
+
+	for (i = 0; i < length; i++) dupl->ir[i] = ir[i];
+	for (i = 0; i <= nCols; i++) dupl->jc[i] = jc[i];
+	for (i = 0; i < length; i++) dupl->val[i] = val[i];
+
+	if ( jd != 0 )
+	{
+		dupl->jd = new sparse_int_t[nCols];
+		for (i = 0; i < nCols; i++) dupl->jd[i] = jd[i];
+	}
+	else
+		dupl->jd = 0;
+	
+	dupl->doFreeMemory( );
+
+	return dupl;
+}
+
+
+
+real_t SparseMatrix::diag(int i) const
+{
+	if ( jd == 0 )
+	{
+		THROWERROR( RET_DIAGONAL_NOT_INITIALISED );
+		return INFTY;
+	}
+
+	int entry = jd[i];
+	return (entry < jc[i+1] && ir[entry] == i) ? val[entry] : 0.0;
+}
+
+
+BooleanType SparseMatrix::isDiag() const
+{
+	int j;
+	
+	if ( nCols != nRows )
+		return BT_FALSE;
+	
+	for (j = 0; j < nCols; ++j)
+	{
+		if ( jc[j+1] > jc[j]+1 )
+			return BT_FALSE;
+		
+		if ( ( jc[j+1] == jc[j]+1 ) && ( ir[jc[j]] != j ) )
+			return BT_FALSE;
+	}
+	
+	return BT_TRUE;
+}
+
+
+
+real_t SparseMatrix::getNorm(	int type
+								) const
+{
+	long length = jc[nCols];
+	return REFER_NAMESPACE_QPOASES getNorm( val,length,type );
+}
+
+
+real_t SparseMatrix::getRowNorm( int rNum, int type ) const
+{
+    int i,j;
+    real_t norm = 0.0;
+
+	switch( type )
+	{
+		case 2:
+			for ( j=0; j < nCols; ++j ) {
+				for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++) {};
+				norm += (i < jc[j+1] && ir[i] == rNum) ? val[i]*val[i] : 0.0;
+			}
+			return getSqrt(norm);
+
+		case 1:
+			for ( j=0; j < nCols; ++j ) {
+				for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++) {};
+				norm += (i < jc[j+1] && ir[i] == rNum) ? REFER_NAMESPACE_QPOASES getAbs( val[i] ) : 0.0;
+			}
+			return norm;
+
+		default:
+			THROWERROR( RET_INVALID_ARGUMENTS );
+			return -INFTY;
+	}
+}
+
+
+returnValue SparseMatrix::getRow(int rNum, const Indexlist* const icols, real_t alpha, real_t *row) const
+{
+	long i, j, k;
+
+    if (icols != 0)
+    {
+	    if ( isEqual(alpha,1.0) == BT_TRUE )
+		    for (k = 0; k < icols->length; k++)
+		    {
+			    j = icols->number[icols->iSort[k]];
+			    for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
+			    row[icols->iSort[k]] = (i < jc[j+1] && ir[i] == rNum) ? val[i] : 0.0;
+		    }
+	    else if ( isEqual(alpha,-1.0) == BT_TRUE )
+		    for (k = 0; k < icols->length; k++)
+		    {
+			    j = icols->number[icols->iSort[k]];
+			    for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
+			    row[icols->iSort[k]] = (i < jc[j+1] && ir[i] == rNum) ? -val[i] : 0.0;
+		    }
+	    else
+		    for (k = 0; k < icols->length; k++)
+		    {
+			    j = icols->number[icols->iSort[k]];
+			    for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
+			    row[icols->iSort[k]] = (i < jc[j+1] && ir[i] == rNum) ? alpha*val[i] : 0.0;
+		    }
+    }
+    else
+    {
+	    if ( isEqual(alpha,1.0) == BT_TRUE )
+		    for (j = 0; j < nCols; j++)
+		    {
+			    for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
+			    row[j] = (i < jc[j+1] && ir[i] == rNum) ? val[i] : 0.0;
+		    }
+	    else if ( isEqual(alpha,-1.0) == BT_TRUE )
+		    for (j = 0; j < icols->length; j++)
+		    {
+			    for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
+			    row[j] = (i < jc[j+1] && ir[i] == rNum) ? -val[i] : 0.0;
+		    }
+	    else
+		    for (j = 0; j < icols->length; j++)
+		    {
+			    for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
+			    row[j] = (i < jc[j+1] && ir[i] == rNum) ? alpha*val[i] : 0.0;
+		    }
+    }
+	return SUCCESSFUL_RETURN;
+}
+
+
+returnValue SparseMatrix::getCol(int cNum, const Indexlist* const irows, real_t alpha, real_t *col) const
+{
+	long i, j;
+
+	i = jc[cNum];
+	j = 0;
+	if ( isEqual(alpha,1.0) == BT_TRUE )
+		while (i < jc[cNum+1] && j < irows->length)
+			if (ir[i] == irows->number[irows->iSort[j]])
+				col[irows->iSort[j++]] = val[i++];
+			else if (ir[i] > irows->number[irows->iSort[j]])
+				col[irows->iSort[j++]] = 0.0;
+			else
+				i++;
+	else if ( isEqual(alpha,-1.0) == BT_TRUE )
+		while (i < jc[cNum+1] && j < irows->length)
+			if (ir[i] == irows->number[irows->iSort[j]])
+				col[irows->iSort[j++]] = -val[i++];
+			else if (ir[i] > irows->number[irows->iSort[j]])
+				col[irows->iSort[j++]] = 0.0;
+			else
+				i++;
+	else
+		while (i < jc[cNum+1] && j < irows->length)
+			if (ir[i] == irows->number[irows->iSort[j]])
+				col[irows->iSort[j++]] = alpha * val[i++];
+			else if (ir[i] > irows->number[irows->iSort[j]])
+				col[irows->iSort[j++]] = 0.0;
+			else
+				i++;
+
+	/* fill in remaining zeros */
+	while (j < irows->length)
+		col[irows->iSort[j++]] = 0.0;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+returnValue SparseMatrix::times(int xN, real_t alpha, const real_t *x, int xLD,
+		real_t beta, real_t *y, int yLD) const
+{
+	long i, j, k;
+
+	if ( isZero(beta) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nRows; j++)
+				y[j+k*yLD] = 0.0;
+	else if ( isEqual(beta,-1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nRows; j++)
+				y[j+k*yLD] = -y[j+k*yLD];
+	else if ( isEqual(beta,1.0) == BT_FALSE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nRows; j++)
+				y[j+k*yLD] *= beta;
+
+	if ( isEqual(alpha,1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nCols; j++)
+				for (i = jc[j]; i < jc[j+1]; i++)
+					y[ir[i]+k*yLD] += val[i] * x[j+k*xLD];
+	else if ( isEqual(alpha,-1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nCols; j++)
+				for (i = jc[j]; i < jc[j+1]; i++)
+					y[ir[i]+k*yLD] -= val[i] * x[j+k*xLD];
+	else
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nCols; j++)
+				for (i = jc[j]; i < jc[j+1]; i++)
+					y[ir[i]+k*yLD] += alpha * val[i] * x[j+k*xLD];
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+returnValue SparseMatrix::transTimes(int xN, real_t alpha, const real_t *x, int xLD,
+		real_t beta, real_t *y, int yLD) const
+{
+	long i, j, k;
+
+	if ( isZero(beta) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nCols; j++)
+				y[j+k*yLD] = 0.0;
+	else if ( isEqual(beta,-1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nCols; j++)
+				y[j+k*yLD] = -y[j+k*yLD];
+	else if ( isEqual(beta,1.0) == BT_FALSE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nCols; j++)
+				y[j+k*yLD] *= beta;
+
+	if ( isEqual(alpha,1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nCols; j++)
+				for (i = jc[j]; i < jc[j+1]; i++)
+					y[j+k*yLD] += val[i] * x[ir[i]+k*xLD];
+	else if ( isEqual(alpha,-1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nCols; j++)
+				for (i = jc[j]; i < jc[j+1]; i++)
+					y[j+k*yLD] -= val[i] * x[ir[i]+k*xLD];
+	else
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nCols; j++)
+				for (i = jc[j]; i < jc[j+1]; i++)
+					y[j+k*yLD] += alpha * val[i] * x[ir[i]+k*xLD];
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+returnValue SparseMatrix::times(const Indexlist* const irows, const Indexlist* const icols,
+		int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD,
+		BooleanType yCompr) const
+{
+	long i, j, k, l, col;
+
+	if (yCompr == BT_TRUE)
+	{
+		if ( isZero(beta) == BT_TRUE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[j+k*yLD] = 0.0;
+		else if ( isEqual(beta,-1.0) == BT_TRUE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[j+k*yLD] = -y[j+k*yLD];
+		else if ( isEqual(beta,1.0) == BT_FALSE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[j+k*yLD] *= beta;
+
+		if (icols == 0)
+			if ( isEqual(alpha,1.0) == BT_TRUE )
+				for (l = 0; l < nCols; l++)
+				{
+					i = jc[l];
+					j = 0;
+					while (i < jc[l+1] && j < irows->length)
+						if (ir[i] == irows->number[irows->iSort[j]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+irows->iSort[j]] += val[i] * x[k*xLD+l];
+							i++, j++;
+						}
+						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
+						else i++;
+				}
+			else if ( isEqual(alpha,-1.0) == BT_TRUE )
+				for (l = 0; l < nCols; l++)
+				{
+					i = jc[l];
+					j = 0;
+					while (i < jc[l+1] && j < irows->length)
+						if (ir[i] == irows->number[irows->iSort[j]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+irows->iSort[j]] -= val[i] * x[k*xLD+l];
+							i++, j++;
+						}
+						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
+						else i++;
+				}
+			else
+				for (l = 0; l < nCols; l++)
+				{
+					i = jc[l];
+					j = 0;
+					while (i < jc[l+1] && j < irows->length)
+						if (ir[i] == irows->number[irows->iSort[j]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+irows->iSort[j]] += alpha * val[i] * x[k*xLD+l];
+							i++, j++;
+						}
+						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
+						else i++;
+				}
+		else /* icols != 0 */
+			if ( isEqual(alpha,1.0) == BT_TRUE )
+				for (l = 0; l < icols->length; l++)
+				{
+					col = icols->iSort[l];
+					i = jc[icols->number[col]];
+					j = 0;
+					while (i < jc[icols->number[col]+1] && j < irows->length)
+						if (ir[i] == irows->number[irows->iSort[j]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+irows->iSort[j]] += val[i] * x[k*xLD+col];
+							i++, j++;
+						}
+						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
+						else i++;
+				}
+			else if ( isEqual(alpha,-1.0) == BT_TRUE )
+				for (l = 0; l < icols->length; l++)
+				{
+					col = icols->iSort[l];
+					i = jc[icols->number[col]];
+					j = 0;
+					while (i < jc[icols->number[col]+1] && j < irows->length)
+						if (ir[i] == irows->number[irows->iSort[j]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+irows->iSort[j]] -= val[i] * x[k*xLD+col];
+							i++, j++;
+						}
+						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
+						else i++;
+				}
+			else
+				for (l = 0; l < icols->length; l++)
+				{
+					col = icols->iSort[l];
+					i = jc[icols->number[col]];
+					j = 0;
+					while (i < jc[icols->number[col]+1] && j < irows->length)
+						if (ir[i] == irows->number[irows->iSort[j]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+irows->iSort[j]] += alpha * val[i] * x[k*xLD+col];
+							i++, j++;
+						}
+						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
+						else i++;
+				}
+	}
+	else /* y not compressed */
+	{
+		if ( isZero(beta) == BT_TRUE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[irows->number[j]+k*yLD] = 0.0;
+		else if ( isEqual(beta,-1.0) == BT_TRUE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[irows->number[j]+k*yLD] = -y[j+k*yLD];
+		else if ( isEqual(beta,1.0) == BT_FALSE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[irows->number[j]+k*yLD] *= beta;
+
+		if (icols == 0)
+			if ( isEqual(alpha,1.0) == BT_TRUE )
+				for (l = 0; l < nCols; l++)
+				{
+					i = jc[l];
+					j = 0;
+					while (i < jc[l+1] && j < irows->length)
+						if (ir[i] == irows->number[irows->iSort[j]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+irows->number[irows->iSort[j]]] += val[i] * x[k*xLD+l];
+							i++, j++;
+						}
+						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
+						else i++;
+				}
+			else if ( isEqual(alpha,-1.0) == BT_TRUE )
+				for (l = 0; l < nCols; l++)
+				{
+					i = jc[l];
+					j = 0;
+					while (i < jc[l+1] && j < irows->length)
+						if (ir[i] == irows->number[irows->iSort[j]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+irows->number[irows->iSort[j]]] -= val[i] * x[k*xLD+l];
+							i++, j++;
+						}
+						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
+						else i++;
+				}
+			else
+				for (l = 0; l < nCols; l++)
+				{
+					i = jc[l];
+					j = 0;
+					while (i < jc[l+1] && j < irows->length)
+						if (ir[i] == irows->number[irows->iSort[j]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+irows->number[irows->iSort[j]]] += alpha * val[i] * x[k*xLD+l];
+							i++, j++;
+						}
+						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
+						else i++;
+				}
+		else /* icols != 0 */
+			if ( isEqual(alpha,1.0) == BT_TRUE )
+				for (l = 0; l < icols->length; l++)
+				{
+					col = icols->iSort[l];
+					i = jc[icols->number[col]];
+					j = 0;
+					while (i < jc[icols->number[col]+1] && j < irows->length)
+						if (ir[i] == irows->number[irows->iSort[j]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+irows->number[irows->iSort[j]]] += val[i] * x[k*xLD+col];
+							i++, j++;
+						}
+						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
+						else i++;
+				}
+			else if ( isEqual(alpha,-1.0) == BT_TRUE )
+				for (l = 0; l < icols->length; l++)
+				{
+					col = icols->iSort[l];
+					i = jc[icols->number[col]];
+					j = 0;
+					while (i < jc[icols->number[col]+1] && j < irows->length)
+						if (ir[i] == irows->number[irows->iSort[j]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+irows->number[irows->iSort[j]]] -= val[i] * x[k*xLD+col];
+							i++, j++;
+						}
+						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
+						else i++;
+				}
+			else
+				for (l = 0; l < icols->length; l++)
+				{
+					col = icols->iSort[l];
+					i = jc[icols->number[col]];
+					j = 0;
+					while (i < jc[icols->number[col]+1] && j < irows->length)
+						if (ir[i] == irows->number[irows->iSort[j]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+irows->number[irows->iSort[j]]] += alpha * val[i] * x[k*xLD+col];
+							i++, j++;
+						}
+						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
+						else i++;
+				}
+	}
+	return SUCCESSFUL_RETURN;
+}
+
+
+returnValue SparseMatrix::transTimes(const Indexlist* const irows, const Indexlist* const icols,
+		int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
+{
+	long i, j, k, l, col;
+
+	if ( isZero(beta) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < icols->length; j++)
+				y[j+k*yLD] = 0.0;
+	else if ( isEqual(beta,-1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < icols->length; j++)
+				y[j+k*yLD] = -y[j+k*yLD];
+	else if ( isEqual(beta,1.0) == BT_FALSE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < icols->length; j++)
+				y[j+k*yLD] *= beta;
+
+	if ( isEqual(alpha,1.0) == BT_TRUE )
+		for (l = 0; l < icols->length; l++)
+		{
+			col = icols->iSort[l];
+			i = jc[icols->number[col]];
+			j = 0;
+			while (i < jc[icols->number[col]+1] && j < irows->length)
+				if (ir[i] == irows->number[irows->iSort[j]])
+				{
+					for (k = 0; k < xN; k++)
+						y[k*yLD+col] += val[i] * x[k*xLD+irows->iSort[j]];
+					i++, j++;
+				}
+				else if (ir[i] > irows->number[irows->iSort[j]]) j++;
+				else i++;
+		}
+	else if ( isEqual(alpha,-1.0) == BT_TRUE )
+		for (l = 0; l < icols->length; l++)
+		{
+			col = icols->iSort[l];
+			i = jc[icols->number[col]];
+			j = 0;
+			while (i < jc[icols->number[col]+1] && j < irows->length)
+				if (ir[i] == irows->number[irows->iSort[j]])
+				{
+					for (k = 0; k < xN; k++)
+						y[k*yLD+col] -= val[i] * x[k*xLD+irows->iSort[j]];
+					i++, j++;
+				}
+				else if (ir[i] > irows->number[irows->iSort[j]]) j++;
+				else i++;
+		}
+	else
+		for (l = 0; l < icols->length; l++)
+		{
+			col = icols->iSort[l];
+			i = jc[icols->number[col]];
+			j = 0;
+			while (i < jc[icols->number[col]+1] && j < irows->length)
+				if (ir[i] == irows->number[irows->iSort[j]])
+				{
+					for (k = 0; k < xN; k++)
+						y[k*yLD+col] += alpha * val[i] * x[k*xLD+irows->iSort[j]];
+					i++, j++;
+				}
+				else if (ir[i] > irows->number[irows->iSort[j]]) j++;
+				else i++;
+		}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+returnValue SparseMatrix::addToDiag(real_t alpha)
+{
+	long i;
+	
+	if ( jd == 0 )
+		return THROWERROR( RET_DIAGONAL_NOT_INITIALISED );
+
+	if ( isZero( alpha ) == BT_FALSE )
+		for (i = 0; i < nRows && i < nCols; i++)
+			if (ir[jd[i]] == i) val[jd[i]] += alpha;
+			else return RET_NO_DIAGONAL_AVAILABLE;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+sparse_int_t *SparseMatrix::createDiagInfo()
+{
+	long i, j;
+
+	if (jd == 0) {
+		jd = new sparse_int_t[nCols];
+
+		for (j = 0; j < nCols; j++)
+		{
+			for (i = jc[j]; i < jc[j+1] && ir[i] < j; i++);
+			jd[j] = i;
+		}
+	}
+
+	return jd;
+}
+
+
+
+real_t *SparseMatrix::full() const
+{
+	long i, j;
+	real_t *v = new real_t[nRows*nCols];
+
+	for (i = 0; i < nCols*nRows; i++)
+		v[i] = 0.0;
+
+	for (j = 0; j < nCols; j++)
+		for (i = jc[j]; i < jc[j+1]; i++)
+			v[ir[i] * nCols + j] = val[i];
+
+	return v;
+}
+
+
+returnValue SparseMatrix::print( const char* name ) const
+{
+	real_t* tmp = this->full();
+	returnValue retVal = REFER_NAMESPACE_QPOASES print( tmp,nRows,nCols,name );
+	delete[] tmp;
+
+	return retVal;
+}
+
+
+
+SparseMatrixRow::SparseMatrixRow() : nRows(0), nCols(0), jr(0), ic(0), jd(0), val(0) {}
+
+
+SparseMatrixRow::SparseMatrixRow(int nr, int nc, sparse_int_t *r, sparse_int_t *c, real_t *v)
+	: nRows(nr), nCols(nc), jr(r), ic(c), jd(0), val(v) { doNotFreeMemory(); }
+
+
+SparseMatrixRow::SparseMatrixRow(int nr, int nc, int ld, const real_t * const v) : nRows(nr), nCols(nc), jd(0)
+{
+	int i, j, nnz;
+
+	jr = new sparse_int_t[nr+1];
+	ic = new sparse_int_t[nr*nc];
+	val = new real_t[nr*nc];
+
+	nnz = 0;
+	for (j = 0; j < nRows; j++)
+	{
+		jr[j] = nnz;
+		for (i = 0; i < nCols; i++)
+			if ( ( isZero( v[j*ld+i],0.0 ) == BT_FALSE ) || ( j == i ) )
+			{
+				ic[nnz] = i;
+				val[nnz++] = v[j*ld+i];
+			}
+	}
+	jr[nRows] = nnz;
+
+	doFreeMemory( );
+}
+
+
+SparseMatrixRow::~SparseMatrixRow()
+{
+	if (jd != 0)
+	{
+		delete[] jd;
+		jd = 0;
+	}
+
+	if ( needToFreeMemory() == BT_TRUE )
+		free( );
+}
+
+
+void SparseMatrixRow::free( )
+{
+	if (jr != 0) delete[] jr;
+	jr = 0;
+	if (ic != 0) delete[] ic;
+	ic = 0;
+	if (val != 0) delete[] val;
+	val = 0;
+
+	doNotFreeMemory( );
+}
+
+
+Matrix *SparseMatrixRow::duplicate() const
+{
+	long i, length = jr[nRows];
+	SparseMatrixRow *dupl = new SparseMatrixRow;
+
+	dupl->nRows = nRows;
+	dupl->nCols = nCols;
+	dupl->jr = new sparse_int_t[nRows+1];
+	dupl->ic = new sparse_int_t[length];
+	dupl->val = new real_t[length];
+
+	for (i = 0; i < length; i++) dupl->jr[i] = jr[i];
+	for (i = 0; i <= nCols; i++) dupl->ic[i] = ic[i];
+	for (i = 0; i < length; i++) dupl->val[i] = val[i];
+
+	if ( jd != 0 )
+	{
+		dupl->jd = new sparse_int_t[nRows];
+		for (i = 0; i < nCols; i++) dupl->jd[i] = jd[i];
+	}
+	else
+		dupl->jd = 0;
+
+	dupl->doFreeMemory( );
+
+	return dupl;
+}
+
+
+
+real_t SparseMatrixRow::diag(int i) const
+{
+	if ( jd == 0 )
+	{
+		THROWERROR( RET_DIAGONAL_NOT_INITIALISED );
+		return INFTY;
+	}
+
+	int entry = jd[i];
+	return (entry < jr[i+1] && ic[entry] == i) ? val[entry] : 0.0;
+}
+
+
+BooleanType SparseMatrixRow::isDiag() const
+{
+	int i;
+	
+	if ( nCols != nRows )
+		return BT_FALSE;
+	
+	for (i = 0; i < nRows; ++i)
+	{
+		if ( jr[i+1] > jr[i]+1 )
+			return BT_FALSE;
+		
+		if ( ( jr[i+1] == jr[i]+1 ) && ( ic[jr[i]] != i ) )
+			return BT_FALSE;
+	}
+	
+	return BT_TRUE;
+}
+
+
+
+real_t SparseMatrixRow::getNorm(	int type
+									) const
+{
+	long length = jr[nRows];
+	return REFER_NAMESPACE_QPOASES getNorm( val,length,type );
+
+}
+
+
+real_t SparseMatrixRow::getRowNorm( int rNum, int type ) const
+{
+	long length = jr[rNum+1] - jr[rNum];
+	return REFER_NAMESPACE_QPOASES getNorm( &(val[jr[rNum]]),length,type );
+}
+
+
+
+returnValue SparseMatrixRow::getRow(int rNum, const Indexlist* const icols, real_t alpha, real_t *row) const
+{
+	long i, j;
+
+	if (icols != 0)
+	{
+		j = jr[rNum];
+		i = 0;
+		if ( isEqual(alpha,1.0) == BT_TRUE )
+			while (j < jr[rNum+1] && i < icols->length)
+				if (ic[j] == icols->number[icols->iSort[i]])
+					row[icols->iSort[i++]] = val[j++];
+				else if (ic[j] > icols->number[icols->iSort[i]])
+					row[icols->iSort[i++]] = 0.0;
+				else
+					j++;
+		else if ( isEqual(alpha,-1.0) == BT_TRUE )
+			while (j < jr[rNum+1] && i < icols->length)
+				if (ic[j] == icols->number[icols->iSort[i]])
+					row[icols->iSort[i++]] = -val[j++];
+				else if (ic[j] > icols->number[icols->iSort[i]])
+					row[icols->iSort[i++]] = 0.0;
+				else
+					j++;
+		else
+			while (j < jr[rNum+1] && i < icols->length)
+				if (ic[j] == icols->number[icols->iSort[i]])
+					row[icols->iSort[i++]] = alpha * val[j++];
+				else if (ic[j] > icols->number[icols->iSort[i]])
+					row[icols->iSort[i++]] = 0.0;
+				else
+					j++;
+
+		/* fill in remaining zeros */
+		while (i < icols->length)
+			row[icols->iSort[i++]] = 0.0;
+	}
+	else
+	{
+		for (i = 0; i < nCols; i++)
+			row[i] = 0;
+
+		if ( isEqual(alpha,1.0) == BT_TRUE )
+			for (j = jr[rNum]; j < jr[rNum+1]; j++)
+				row[ic[j]] = val[j];
+		else if ( isEqual(alpha,-1.0) == BT_TRUE )
+			for (j = jr[rNum]; j < jr[rNum+1]; j++)
+				row[ic[j]] = -val[j];
+		else
+			for (j = jr[rNum]; j < jr[rNum+1]; j++)
+				row[ic[j]] = alpha * val[j];
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+returnValue SparseMatrixRow::getCol(int cNum, const Indexlist* const irows, real_t alpha, real_t *col) const
+{
+	long i, j, k, srt;
+
+    if (irows != 0)
+    {
+	    if ( isEqual(alpha,1.0) == BT_TRUE )
+		    for (k = 0; k < irows->length; k++)
+		    {
+				srt = irows->iSort[k];
+			    j = irows->number[srt];
+			    for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
+			    col[srt] = (i < jr[j+1] && ic[i] == cNum) ? val[i] : 0.0;
+		    }
+	    else if ( isEqual(alpha,-1.0) == BT_TRUE )
+		    for (k = 0; k < irows->length; k++)
+		    {
+				srt = irows->iSort[k];
+			    j = irows->number[srt];
+			    for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
+			    col[srt] = (i < jr[j+1] && ic[i] == cNum) ? -val[i] : 0.0;
+		    }
+	    else
+		    for (k = 0; k < irows->length; k++)
+		    {
+				srt = irows->iSort[k];
+			    j = irows->number[srt];
+			    for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
+			    col[srt] = (i < jr[j+1] && ic[i] == cNum) ? alpha*val[i] : 0.0;
+		    }
+    }
+    else
+    {
+	    if ( isEqual(alpha,1.0) == BT_TRUE )
+		    for (j = 0; j < nCols; j++)
+		    {
+			    for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
+			    col[j] = (i < jr[j+1] && ic[i] == cNum) ? val[i] : 0.0;
+		    }
+	    else if ( isEqual(alpha,-1.0) == BT_TRUE )
+		    for (j = 0; j < irows->length; j++)
+		    {
+			    for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
+			    col[j] = (i < jr[j+1] && ic[i] == cNum) ? -val[i] : 0.0;
+		    }
+	    else
+		    for (j = 0; j < irows->length; j++)
+		    {
+			    for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
+			    col[j] = (i < jr[j+1] && ic[i] == cNum) ? alpha*val[i] : 0.0;
+		    }
+    }
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+returnValue SparseMatrixRow::times(int xN, real_t alpha, const real_t *x, int xLD,
+		real_t beta, real_t *y, int yLD) const
+{
+	long i, j, k;
+
+	if ( isZero(beta) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nRows; j++)
+				y[j+k*yLD] = 0.0;
+	else if ( isEqual(beta,-1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nRows; j++)
+				y[j+k*yLD] = -y[j+k*yLD];
+	else if ( isEqual(beta,1.0) == BT_FALSE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nRows; j++)
+				y[j+k*yLD] *= beta;
+
+	if ( isEqual(alpha,1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nRows; j++)
+				for (i = jr[j]; i < jr[j+1]; i++)
+					y[j+k*yLD] += val[i] * x[ic[i]+k*xLD];
+	else if ( isEqual(alpha,-1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nRows; j++)
+				for (i = jr[j]; i < jr[j+1]; i++)
+					y[j+k*yLD] -= val[i] * x[ic[i]+k*xLD];
+	else
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nRows; j++)
+				for (i = jr[j]; i < jr[j+1]; i++)
+					y[j+k*yLD] += alpha * val[i] * x[ic[i]+k*xLD];
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+returnValue SparseMatrixRow::transTimes(int xN, real_t alpha, const real_t *x, int xLD,
+		real_t beta, real_t *y, int yLD) const
+{
+	long i, j, k;
+
+	if ( isZero(beta) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nCols; j++)
+				y[j+k*yLD] = 0.0;
+	else if ( isEqual(beta,-1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nCols; j++)
+				y[j+k*yLD] = -y[j+k*yLD];
+	else if ( isEqual(beta,1.0) == BT_FALSE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < nCols; j++)
+				y[j+k*yLD] *= beta;
+
+	if ( isEqual(alpha,1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (i = 0; i < nRows; i++)
+				for (j = jr[i]; j < jr[i+1]; j++)
+					y[ic[j]+k*yLD] += val[j] * x[i+k*xLD];
+	else if ( isEqual(alpha,-1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (i = 0; i < nRows; i++)
+				for (j = jr[i]; j < jr[i+1]; j++)
+					y[ic[j]+k*yLD] -= val[j] * x[i+k*xLD];
+	else
+		for (k = 0; k < xN; k++)
+			for (i = 0; i < nRows; i++)
+				for (j = jr[i]; j < jr[i+1]; j++)
+					y[ic[j]+k*yLD] += alpha * val[j] * x[i+k*xLD];
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+returnValue SparseMatrixRow::times(const Indexlist* const irows, const Indexlist* const icols,
+		int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD,
+		BooleanType yCompr) const
+{
+	long i, j, k, l, srt, row;
+
+	if (yCompr == BT_TRUE)
+	{
+		if ( isZero(beta) == BT_TRUE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[j+k*yLD] = 0.0;
+		else if ( isEqual(beta,-1.0) == BT_TRUE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[j+k*yLD] = -y[j+k*yLD];
+		else if ( isEqual(beta,1.0) == BT_FALSE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[j+k*yLD] *= beta;
+
+		if (icols == 0)
+			if ( isEqual(alpha,1.0) == BT_TRUE )
+				for (l = 0; l < irows->length; l++)
+				{
+					srt = irows->iSort[l];
+					row = irows->number[srt];
+					for (j = jr[row]; j < jr[row+1]; j++)
+						for (k = 0; k < xN; k++)
+							y[k*yLD+srt] += val[j] * x[k*xLD+ic[j]];
+				}
+			else if ( isEqual(alpha,-1.0) == BT_TRUE )
+				for (l = 0; l < irows->length; l++)
+				{
+					srt = irows->iSort[l];
+					row = irows->number[srt];
+					for (j = jr[row]; j < jr[row+1]; j++)
+						for (k = 0; k < xN; k++)
+							y[k*yLD+srt] -= val[j] * x[k*xLD+ic[j]];
+				}
+			else
+				for (l = 0; l < irows->length; l++)
+				{
+					srt = irows->iSort[l];
+					row = irows->number[srt];
+					for (j = jr[row]; j < jr[row+1]; j++)
+						for (k = 0; k < xN; k++)
+							y[k*yLD+srt] += alpha * val[j] * x[k*xLD+ic[j]];
+				}
+		else /* icols != 0 */
+			if ( isEqual(alpha,1.0) == BT_TRUE )
+				for (l = 0; l < irows->length; l++)
+				{
+					srt = irows->iSort[l];
+					row = irows->number[srt];
+					j = jr[row];
+					i = 0;
+					while (j < jr[row+1] && i < icols->length)
+						if (ic[j] == icols->number[icols->iSort[i]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+srt] += val[j] * x[k*xLD+icols->iSort[i]];
+							j++, i++;
+						}
+						else if (ic[j] > icols->number[icols->iSort[i]]) i++;
+						else j++;
+				}
+			else if ( isEqual(alpha,-1.0) == BT_TRUE )
+				for (l = 0; l < irows->length; l++)
+				{
+					srt = irows->iSort[l];
+					row = irows->number[srt];
+					j = jr[row];
+					i = 0;
+					while (j < jr[row+1] && i < icols->length)
+						if (ic[j] == icols->number[icols->iSort[i]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+srt] -= val[j] * x[k*xLD+icols->iSort[i]];
+							j++, i++;
+						}
+						else if (ic[j] > icols->number[icols->iSort[i]]) i++;
+						else j++;
+				}
+			else
+				for (l = 0; l < irows->length; l++)
+				{
+					srt = irows->iSort[l];
+					row = irows->number[srt];
+					j = jr[row];
+					i = 0;
+					while (j < jr[row+1] && i < icols->length)
+						if (ic[j] == icols->number[icols->iSort[i]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+srt] += alpha * val[j] * x[k*xLD+icols->iSort[i]];
+							j++, i++;
+						}
+						else if (ic[j] > icols->number[icols->iSort[i]]) i++;
+						else j++;
+				}
+	}
+	else /* y not compressed */
+	{
+		if ( isZero(beta) == BT_TRUE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[irows->number[j]+k*yLD] = 0.0;
+		else if ( isEqual(beta,-1.0) == BT_TRUE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[irows->number[j]+k*yLD] = -y[j+k*yLD];
+		else if ( isEqual(beta,1.0) == BT_FALSE )
+			for (k = 0; k < xN; k++)
+				for (j = 0; j < irows->length; j++)
+					y[irows->number[j]+k*yLD] *= beta;
+
+		if (icols == 0)
+			if ( isEqual(alpha,1.0) == BT_TRUE )
+				for (l = 0; l < irows->length; l++)
+				{
+					row = irows->number[irows->iSort[l]];
+					for (j = jr[row]; j < jr[row+1]; j++)
+						for (k = 0; k < xN; k++)
+							y[k*yLD+row] += val[j] * x[k*xLD+ic[j]];
+				}
+			else if ( isEqual(alpha,-1.0) == BT_TRUE )
+				for (l = 0; l < irows->length; l++)
+				{
+					row = irows->number[irows->iSort[l]];
+					for (j = jr[row]; j < jr[row+1]; j++)
+						for (k = 0; k < xN; k++)
+							y[k*yLD+row] -= val[j] * x[k*xLD+ic[j]];
+				}
+			else
+				for (l = 0; l < irows->length; l++)
+				{
+					row = irows->number[irows->iSort[l]];
+					for (j = jr[row]; j < jr[row+1]; j++)
+						for (k = 0; k < xN; k++)
+							y[k*yLD+row] += alpha * val[j] * x[k*xLD+ic[j]];
+				}
+		else /* icols != 0 */
+			if ( isEqual(alpha,1.0) == BT_TRUE )
+				for (l = 0; l < irows->length; l++)
+				{
+					row = irows->iSort[l];
+					j = jr[irows->number[row]];
+					i = 0;
+					while (j < jr[irows->number[row]+1] && i < icols->length)
+						if (ic[j] == icols->number[icols->iSort[i]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+row] += val[j] * x[k*xLD+icols->iSort[i]];
+							j++, i++;
+						}
+						else if (ic[j] > icols->number[icols->iSort[i]]) i++;
+						else j++;
+				}
+			else if ( isEqual(alpha,-1.0) == BT_TRUE )
+				for (l = 0; l < irows->length; l++)
+				{
+					row = irows->iSort[l];
+					j = jr[irows->number[row]];
+					i = 0;
+					while (j < jr[irows->number[row]+1] && i < icols->length)
+						if (ic[j] == icols->number[icols->iSort[i]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+row] -= val[j] * x[k*xLD+icols->iSort[i]];
+							j++, i++;
+						}
+						else if (ic[j] > icols->number[icols->iSort[i]]) i++;
+						else j++;
+				}
+			else
+				for (l = 0; l < irows->length; l++)
+				{
+					row = irows->iSort[l];
+					j = jr[irows->number[row]];
+					i = 0;
+					while (j < jr[irows->number[row]+1] && i < icols->length)
+						if (ic[j] == icols->number[icols->iSort[i]])
+						{
+							for (k = 0; k < xN; k++)
+								y[k*yLD+row] += alpha * val[j] * x[k*xLD+icols->iSort[i]];
+							j++, i++;
+						}
+						else if (ic[j] > icols->number[icols->iSort[i]]) i++;
+						else j++;
+				}
+	}
+	return SUCCESSFUL_RETURN;
+}
+
+
+returnValue SparseMatrixRow::transTimes(const Indexlist* const irows, const Indexlist* const icols,
+		int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
+{
+	long i, j, k, l, row, srt;
+
+	if ( isZero(beta) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < icols->length; j++)
+				y[j+k*yLD] = 0.0;
+	else if ( isEqual(beta,-1.0) == BT_TRUE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < icols->length; j++)
+				y[j+k*yLD] = -y[j+k*yLD];
+	else if ( isEqual(beta,1.0) == BT_FALSE )
+		for (k = 0; k < xN; k++)
+			for (j = 0; j < icols->length; j++)
+				y[j+k*yLD] *= beta;
+
+	if ( isEqual(alpha,1.0) == BT_TRUE )
+		for (l = 0; l < irows->length; l++)
+		{
+			srt = irows->iSort[l];
+			row = irows->number[srt];
+			j = jr[row];
+			i = 0;
+			while (j < jr[row+1] && i < icols->length)
+				if (ic[j] == icols->number[icols->iSort[i]])
+				{
+					for (k = 0; k < xN; k++)
+						y[k*yLD+icols->iSort[i]] += val[j] * x[k*xLD+srt];
+					j++, i++;
+				}
+				else if (ic[j] > icols->number[icols->iSort[i]]) i++;
+				else j++;
+		}
+	else if ( isEqual(alpha,-1.0) == BT_TRUE )
+		for (l = 0; l < irows->length; l++)
+		{
+			srt = irows->iSort[l];
+			row = irows->number[srt];
+			j = jr[row];
+			i = 0;
+			while (j < jr[row+1] && i < icols->length)
+				if (ic[j] == icols->number[icols->iSort[i]])
+				{
+					for (k = 0; k < xN; k++)
+						y[k*yLD+icols->iSort[i]] -= val[j] * x[k*xLD+srt];
+					j++, i++;
+				}
+				else if (ic[j] > icols->number[icols->iSort[i]]) i++;
+				else j++;
+		}
+	else
+		for (l = 0; l < irows->length; l++)
+		{
+			srt = irows->iSort[l];
+			row = irows->number[srt];
+			j = jr[row];
+			i = 0;
+			while (j < jr[row+1] && i < icols->length)
+				if (ic[j] == icols->number[icols->iSort[i]])
+				{
+					for (k = 0; k < xN; k++)
+						y[k*yLD+icols->iSort[i]] += alpha * val[j] * x[k*xLD+srt];
+					j++, i++;
+				}
+				else if (ic[j] > icols->number[icols->iSort[i]]) i++;
+				else j++;
+		}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+returnValue SparseMatrixRow::addToDiag(real_t alpha)
+{
+	long i;
+	
+	if ( jd == 0 )
+		return THROWERROR( RET_DIAGONAL_NOT_INITIALISED );
+
+	if ( isZero(alpha) == BT_FALSE )
+		for (i = 0; i < nRows && i < nCols; i++)
+			if (ic[jd[i]] == i) val[jd[i]] += alpha;
+			else return RET_NO_DIAGONAL_AVAILABLE;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+sparse_int_t *SparseMatrixRow::createDiagInfo()
+{
+	long i, j;
+
+	if (jd == 0) {
+		jd = new sparse_int_t[nRows];
+
+		for (i = 0; i < nRows; i++)
+		{
+			for (j = jr[i]; j < jr[i+1] && ic[j] < i; j++);
+			jd[i] = j;
+		}
+	}
+
+	return jd;
+}
+
+
+real_t *SparseMatrixRow::full() const
+{
+	long i, j;
+	real_t *v = new real_t[nRows*nCols];
+
+	for (i = 0; i < nCols*nRows; i++)
+		v[i] = 0.0;
+
+	for (i = 0; i < nRows; i++)
+		for (j = jr[i]; j < jr[i+1]; j++)
+			v[ic[j] + i * nCols] = val[j];
+
+	return v;
+}
+
+
+returnValue SparseMatrixRow::print( const char* name ) const
+{
+	real_t* tmp = this->full();
+	returnValue retVal = REFER_NAMESPACE_QPOASES print( tmp,nRows,nCols,name );
+	delete[] tmp;
+
+	return retVal;
+}
+
+
+
+Matrix *SymSparseMat::duplicate() const
+{
+	return duplicateSym();
+}
+
+
+SymmetricMatrix *SymSparseMat::duplicateSym() const
+{
+	/* "same" as duplicate() in SparseMatrix */
+	long i, length = jc[nCols];
+	SymSparseMat *dupl = new SymSparseMat;
+
+	dupl->nRows = nRows;
+	dupl->nCols = nCols;
+	dupl->ir = new sparse_int_t[length];
+	dupl->jc = new sparse_int_t[nCols+1];
+	dupl->val = new real_t[length];
+
+	for (i = 0; i < length; i++) dupl->ir[i] = ir[i];
+	for (i = 0; i <= nCols; i++) dupl->jc[i] = jc[i];
+	for (i = 0; i < length; i++) dupl->val[i] = val[i];
+
+	if ( jd != 0 )
+	{
+		dupl->jd = new sparse_int_t[nCols];
+		for (i = 0; i < nCols; i++) dupl->jd[i] = jd[i];
+	}
+	else
+		dupl->jd = 0;
+
+	dupl->doFreeMemory( );
+
+	return dupl;
+}
+
+
+returnValue SymSparseMat::bilinear(const Indexlist* const icols,
+		int xN, const real_t *x, int xLD, real_t *y, int yLD) const
+{
+	int i, j, k, l, idx, row, col;
+
+	if ( jd == 0 )
+		return THROWERROR( RET_DIAGONAL_NOT_INITIALISED );
+
+	/* clear output */
+	for (i = 0; i < xN*xN; i++)
+		y[i] = 0.0;
+
+	/* compute lower triangle */
+	for (l = 0; l < icols->length; l++)
+	{
+		col = icols->number[icols->iSort[l]];
+		idx = jd[col];
+		k = 0;
+		while (idx < jc[col+1] && k < icols->length)
+		{
+			row = icols->number[icols->iSort[k]];
+			if (ir[idx] == row)
+			{
+				/* TODO: It is possible to formulate this as DSYR and DSYR2
+				 * operations. */
+				if (row == col) /* diagonal element */
+					for (i = 0; i < xN; i++)
+						for (j = i; j < xN; j++)
+							y[i*yLD+j] += val[idx] * x[i*xLD+col] * x[j*xLD+col];
+				else /* subdiagonal elements */
+					for (i = 0; i < xN; i++)
+						for (j = i; j < xN; j++)
+							y[i*yLD+j] += val[idx] * (x[i*xLD+col] * x[j*xLD+row] + x[i*xLD+row] * x[j*xLD+col]);
+				idx++, k++;
+			}
+			else if (ir[idx] > row) k++;
+			else idx++;
+		}
+	}
+
+	/* fill upper triangle */
+	for (i = 0; i < xN; i++)
+		for (j = i; j < xN; j++)
+			y[j*yLD+i] = y[i*yLD+j];
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/MessageHandling.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/MessageHandling.cpp
new file mode 100644
index 00000000000..897b2d7f80c
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/MessageHandling.cpp
@@ -0,0 +1,612 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file src/MessageHandling.cpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of the MessageHandling class including global return values.
+ *
+ */
+
+
+#include <stdio.h>
+
+#ifdef __MATLAB__
+  #include "mex.h"
+#endif
+
+#include <qpOASES/MessageHandling.hpp>
+#include <qpOASES/Utils.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+
+#ifndef __XPCTARGET__
+/** Defines pairs of global return values and messages. */
+MessageHandling::ReturnValueList returnValueList[] =
+{
+/* miscellaneous */
+{ SUCCESSFUL_RETURN, "Successful return", VS_VISIBLE },
+{ RET_DIV_BY_ZERO, "Division by zero", VS_VISIBLE },
+{ RET_INDEX_OUT_OF_BOUNDS, "Index out of bounds", VS_VISIBLE },
+{ RET_INVALID_ARGUMENTS, "At least one of the arguments is invalid", VS_VISIBLE },
+{ RET_ERROR_UNDEFINED, "Error number undefined", VS_VISIBLE },
+{ RET_WARNING_UNDEFINED, "Warning number undefined", VS_VISIBLE },
+{ RET_INFO_UNDEFINED, "Info number undefined", VS_VISIBLE },
+{ RET_EWI_UNDEFINED, "Error/warning/info number undefined", VS_VISIBLE },
+{ RET_AVAILABLE_WITH_LINUX_ONLY, "This function is available under Linux only", VS_HIDDEN },
+{ RET_UNKNOWN_BUG, "The error occurred is not yet known", VS_VISIBLE },
+{ RET_PRINTLEVEL_CHANGED, "Print level changed", VS_VISIBLE },
+{ RET_NOT_YET_IMPLEMENTED, "Requested function is not yet implemented.", VS_VISIBLE },
+/* Indexlist */
+{ RET_INDEXLIST_MUST_BE_REORDERD, "Index list has to be reordered", VS_VISIBLE },
+{ RET_INDEXLIST_EXCEEDS_MAX_LENGTH, "Index list exceeds its maximal physical length", VS_VISIBLE },
+{ RET_INDEXLIST_CORRUPTED, "Index list corrupted", VS_VISIBLE },
+{ RET_INDEXLIST_OUTOFBOUNDS, "Physical index is out of bounds", VS_VISIBLE },
+{ RET_INDEXLIST_ADD_FAILED, "Adding indices from another index set failed", VS_VISIBLE },
+{ RET_INDEXLIST_INTERSECT_FAILED, "Intersection with another index set failed", VS_VISIBLE },
+/* SubjectTo / Bounds / Constraints */
+{ RET_INDEX_ALREADY_OF_DESIRED_STATUS, "Index is already of desired status", VS_VISIBLE },
+{ RET_ADDINDEX_FAILED, "Adding index to index set failed", VS_VISIBLE },
+{ RET_REMOVEINDEX_FAILED, "Removing index from index set failed", VS_VISIBLE },
+{ RET_SWAPINDEX_FAILED, "Cannot swap between different indexsets", VS_VISIBLE },
+{ RET_NOTHING_TO_DO, "Nothing to do", VS_VISIBLE },
+{ RET_SETUP_BOUND_FAILED, "Setting up bound index failed", VS_VISIBLE },
+{ RET_SETUP_CONSTRAINT_FAILED, "Setting up constraint index failed", VS_VISIBLE },
+{ RET_MOVING_BOUND_FAILED, "Moving bound between index sets failed", VS_VISIBLE },
+{ RET_MOVING_CONSTRAINT_FAILED, "Moving constraint between index sets failed", VS_VISIBLE },
+{ RET_SHIFTING_FAILED, "Shifting of bounds/constraints failed", VS_VISIBLE },
+{ RET_ROTATING_FAILED, "Rotating of bounds/constraints failed", VS_VISIBLE },
+/* QProblem */
+{ RET_QPOBJECT_NOT_SETUP, "The QP object has not been setup correctly, use another constructor", VS_VISIBLE },
+{ RET_QP_ALREADY_INITIALISED, "QProblem has already been initialised", VS_VISIBLE },
+{ RET_NO_INIT_WITH_STANDARD_SOLVER, "Initialisation via extern QP solver is not yet implemented", VS_VISIBLE },
+{ RET_RESET_FAILED, "Reset failed", VS_VISIBLE },
+{ RET_INIT_FAILED, "Initialisation failed", VS_VISIBLE },
+{ RET_INIT_FAILED_TQ, "Initialisation failed due to TQ factorisation", VS_VISIBLE },
+{ RET_INIT_FAILED_CHOLESKY, "Initialisation failed due to Cholesky decomposition", VS_VISIBLE },
+{ RET_INIT_FAILED_HOTSTART, "Initialisation failed! QP could not be solved!", VS_VISIBLE },
+{ RET_INIT_FAILED_INFEASIBILITY, "Initial QP could not be solved due to infeasibility!", VS_VISIBLE },
+{ RET_INIT_FAILED_UNBOUNDEDNESS, "Initial QP could not be solved due to unboundedness!", VS_VISIBLE },
+{ RET_INIT_FAILED_REGULARISATION, "Initialisation failed as Hessian matrix could not be regularised", VS_VISIBLE },
+{ RET_INIT_SUCCESSFUL, "Initialisation done", VS_VISIBLE },
+{ RET_OBTAINING_WORKINGSET_FAILED, "Failed to obtain working set for auxiliary QP", VS_VISIBLE },
+{ RET_SETUP_WORKINGSET_FAILED, "Failed to setup working set for auxiliary QP", VS_VISIBLE },
+{ RET_SETUP_AUXILIARYQP_FAILED, "Failed to setup auxiliary QP for initialised homotopy", VS_VISIBLE },
+{ RET_NO_EXTERN_SOLVER, "No extern QP solver available", VS_VISIBLE },
+{ RET_QP_UNBOUNDED, "QP is unbounded", VS_VISIBLE },
+{ RET_QP_INFEASIBLE, "QP is infeasible", VS_VISIBLE },
+{ RET_QP_NOT_SOLVED, "Problems occurred while solving QP with standard solver", VS_VISIBLE },
+{ RET_QP_SOLVED, "QP successfully solved", VS_VISIBLE },
+{ RET_UNABLE_TO_SOLVE_QP, "Problems occurred while solving QP", VS_VISIBLE },
+{ RET_INITIALISATION_STARTED, "Starting problem initialisation...", VS_VISIBLE },
+{ RET_HOTSTART_FAILED, "Unable to perform homotopy due to internal error", VS_VISIBLE },
+{ RET_HOTSTART_FAILED_TO_INIT, "Unable to initialise problem", VS_VISIBLE },
+{ RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED, "Unable to perform homotopy as previous QP is not solved", VS_VISIBLE },
+{ RET_ITERATION_STARTED, "Iteration", VS_VISIBLE },
+{ RET_SHIFT_DETERMINATION_FAILED, "Determination of shift of the QP data failed", VS_VISIBLE },
+{ RET_STEPDIRECTION_DETERMINATION_FAILED, "Determination of step direction failed", VS_VISIBLE },
+{ RET_STEPLENGTH_DETERMINATION_FAILED, "Determination of step direction failed", VS_VISIBLE },
+{ RET_OPTIMAL_SOLUTION_FOUND, "Optimal solution of neighbouring QP found", VS_VISIBLE },
+{ RET_HOMOTOPY_STEP_FAILED, "Unable to perform homotopy step", VS_VISIBLE },
+{ RET_HOTSTART_STOPPED_INFEASIBILITY, "Premature homotopy termination because QP is infeasible", VS_VISIBLE },
+{ RET_HOTSTART_STOPPED_UNBOUNDEDNESS, "Premature homotopy termination because QP is unbounded", VS_VISIBLE },
+{ RET_WORKINGSET_UPDATE_FAILED, "Unable to update working sets according to initial guesses", VS_VISIBLE },
+{ RET_MAX_NWSR_REACHED, "Maximum number of working set recalculations performed", VS_VISIBLE },
+{ RET_CONSTRAINTS_NOT_SPECIFIED, "Problem does comprise constraints! You have to specify new constraints' bounds", VS_VISIBLE },
+{ RET_INVALID_FACTORISATION_FLAG, "Invalid factorisation flag", VS_VISIBLE },
+{ RET_UNABLE_TO_SAVE_QPDATA, "Unable to save QP data", VS_VISIBLE },
+{ RET_STEPDIRECTION_FAILED_TQ, "Abnormal termination due to TQ factorisation", VS_VISIBLE },
+{ RET_STEPDIRECTION_FAILED_CHOLESKY, "Abnormal termination due to Cholesky factorisation", VS_VISIBLE },
+{ RET_CYCLING_DETECTED, "Cycling detected", VS_VISIBLE },
+{ RET_CYCLING_NOT_RESOLVED, "Cycling cannot be resolved, QP is probably infeasible", VS_VISIBLE },
+{ RET_CYCLING_RESOLVED, "Cycling probably resolved", VS_VISIBLE },
+{ RET_STEPSIZE, "", VS_VISIBLE },
+{ RET_STEPSIZE_NONPOSITIVE, "", VS_VISIBLE },
+{ RET_SETUPSUBJECTTOTYPE_FAILED, "Setup of SubjectToTypes failed", VS_VISIBLE },
+{ RET_ADDCONSTRAINT_FAILED, "Addition of constraint to working set failed", VS_VISIBLE },
+{ RET_ADDCONSTRAINT_FAILED_INFEASIBILITY, "Addition of constraint to working set failed", VS_VISIBLE },
+{ RET_ADDBOUND_FAILED, "Addition of bound to working set failed", VS_VISIBLE },
+{ RET_ADDBOUND_FAILED_INFEASIBILITY, "Addition of bound to working set failed", VS_VISIBLE },
+{ RET_REMOVECONSTRAINT_FAILED, "Removal of constraint from working set failed", VS_VISIBLE },
+{ RET_REMOVEBOUND_FAILED, "Removal of bound from working set failed", VS_VISIBLE },
+{ RET_REMOVE_FROM_ACTIVESET, "Removing from active set:", VS_VISIBLE },
+{ RET_ADD_TO_ACTIVESET, "Adding to active set:", VS_VISIBLE },
+{ RET_REMOVE_FROM_ACTIVESET_FAILED, "Removing from active set failed", VS_VISIBLE },
+{ RET_ADD_TO_ACTIVESET_FAILED, "Adding to active set failed", VS_VISIBLE },
+{ RET_CONSTRAINT_ALREADY_ACTIVE, "Constraint is already active", VS_VISIBLE },
+{ RET_ALL_CONSTRAINTS_ACTIVE, "All constraints are active, no further constraint can be added", VS_VISIBLE },
+{ RET_LINEARLY_DEPENDENT, "New bound/constraint is linearly dependent", VS_VISIBLE },
+{ RET_LINEARLY_INDEPENDENT, "New bound/constraint is linearly independent", VS_VISIBLE },
+{ RET_LI_RESOLVED, "Linear independence of active constraint matrix successfully resolved", VS_VISIBLE },
+{ RET_ENSURELI_FAILED, "Failed to ensure linear independence of active constraint matrix", VS_VISIBLE },
+{ RET_ENSURELI_FAILED_TQ, "Abnormal termination due to TQ factorisation", VS_VISIBLE },
+{ RET_ENSURELI_FAILED_NOINDEX, "QP is infeasible", VS_VISIBLE },
+{ RET_ENSURELI_FAILED_CYCLING, "QP is infeasible", VS_VISIBLE },
+{ RET_BOUND_ALREADY_ACTIVE, "Bound is already active", VS_VISIBLE },
+{ RET_ALL_BOUNDS_ACTIVE, "All bounds are active, no further bound can be added", VS_VISIBLE },
+{ RET_CONSTRAINT_NOT_ACTIVE, "Constraint is not active", VS_VISIBLE },
+{ RET_BOUND_NOT_ACTIVE, "Bound is not active", VS_VISIBLE },
+{ RET_HESSIAN_NOT_SPD, "Projected Hessian matrix not positive definite", VS_VISIBLE },
+{ RET_HESSIAN_INDEFINITE, "Hessian matrix is indefinite", VS_VISIBLE },
+{ RET_MATRIX_SHIFT_FAILED, "Unable to update matrices or to transform vectors", VS_VISIBLE },
+{ RET_MATRIX_FACTORISATION_FAILED, "Unable to calculate new matrix factorisations", VS_VISIBLE },
+{ RET_PRINT_ITERATION_FAILED, "Unable to print information on current iteration", VS_VISIBLE },
+{ RET_NO_GLOBAL_MESSAGE_OUTPUTFILE, "No global message output file initialised", VS_VISIBLE },
+{ RET_DISABLECONSTRAINTS_FAILED, "Unable to disable constraints", VS_VISIBLE },
+{ RET_ENABLECONSTRAINTS_FAILED, "Unable to enable constraints", VS_VISIBLE },
+{ RET_ALREADY_ENABLED, "Bound or constraint is already enabled", VS_VISIBLE },
+{ RET_ALREADY_DISABLED, "Bound or constraint is already disabled", VS_VISIBLE },
+{ RET_NO_HESSIAN_SPECIFIED, "No Hessian matrix has been specified", VS_VISIBLE },
+{ RET_USING_REGULARISATION, "Using regularisation as Hessian matrix is not positive definite", VS_VISIBLE },
+{ RET_EPS_MUST_BE_POSITVE, "Eps for regularisation must be sufficiently positive", VS_VISIBLE },
+{ RET_REGSTEPS_MUST_BE_POSITVE, "Maximum number of regularisation steps must be non-negative", VS_VISIBLE },
+{ RET_HESSIAN_ALREADY_REGULARISED, "Hessian has been already regularised", VS_VISIBLE },
+{ RET_CANNOT_REGULARISE_IDENTITY, "Identity Hessian matrix cannot be regularised", VS_VISIBLE },
+{ RET_CANNOT_REGULARISE_SPARSE, "Sparse matrix cannot be regularised as diagonal entry is missing", VS_VISIBLE },
+{ RET_NO_REGSTEP_NWSR, "No additional regularisation step could be performed due to limits", VS_VISIBLE },
+{ RET_FEWER_REGSTEPS_NWSR, "Fewer additional regularisation steps have been performed due to limits", VS_VISIBLE },
+{ RET_CHOLESKY_OF_ZERO_HESSIAN, "Cholesky decomposition of (unregularised) zero Hessian matrix", VS_VISIBLE },
+{ RET_CONSTRAINTS_ARE_NOT_SCALED, "(should not be thrown, no longer in use)", VS_VISIBLE },
+{ RET_INITIAL_BOUNDS_STATUS_NYI, "(should not be thrown, no longer in use)", VS_VISIBLE },
+{ RET_ERROR_IN_CONSTRAINTPRODUCT, "Error in user-defined constraint product function", VS_VISIBLE },
+{ RET_FIX_BOUNDS_FOR_LP, "All initial bounds must be fixed when solving an (unregularised) LP", VS_VISIBLE },
+{ RET_USE_REGULARISATION_FOR_LP, "Set options.enableRegularisation=BT_TRUE for solving LPs", VS_VISIBLE },
+/* SQProblem */
+{ RET_UPDATEMATRICES_FAILED, "Unable to update QP matrices", VS_VISIBLE },
+{ RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED, "Unable to update matrices as previous QP is not solved", VS_VISIBLE },
+/* Utils */
+{ RET_UNABLE_TO_OPEN_FILE, "Unable to open file", VS_VISIBLE },
+{ RET_UNABLE_TO_WRITE_FILE, "Unable to write into file", VS_VISIBLE },
+{ RET_UNABLE_TO_READ_FILE, "Unable to read from file", VS_VISIBLE },
+{ RET_FILEDATA_INCONSISTENT, "File contains inconsistent data", VS_VISIBLE },
+/* Options */
+{ RET_OPTIONS_ADJUSTED,	"Options needed to be adjusted for consistency reasons", VS_VISIBLE },
+/* SolutionAnalysis */
+{ RET_UNABLE_TO_ANALYSE_QPROBLEM, "Unable to analyse (S)QProblem(B) object", VS_VISIBLE },
+/* Benchmark */
+{ RET_NWSR_SET_TO_ONE, "Maximum number of working set changes was set to 1", VS_VISIBLE },
+{ RET_UNABLE_TO_READ_BENCHMARK, "Unable to read benchmark data", VS_VISIBLE },
+{ RET_BENCHMARK_ABORTED, "Benchmark aborted", VS_VISIBLE },
+{ RET_INITIAL_QP_SOLVED, "Initial QP solved", VS_VISIBLE },
+{ RET_QP_SOLUTION_STARTED, "Solving QP no.", VS_VISIBLE },
+{ RET_BENCHMARK_SUCCESSFUL, "Benchmark terminated successfully", VS_VISIBLE },
+/* Sparse matrices */
+{ RET_NO_DIAGONAL_AVAILABLE, "Sparse matrix does not have entries on full diagonal", VS_VISIBLE },
+{ RET_DIAGONAL_NOT_INITIALISED, "Diagonal data of sparse matrix has not been initialised", VS_VISIBLE },
+/* Dropping of infeasible constraints */
+{ RET_ENSURELI_DROPPED, "Linear independence resolved by dropping blocking constraint", VS_VISIBLE },
+/* Simple exitflags */
+{ RET_SIMPLE_STATUS_P1, "QP problem could not be solved within given number of iterations", VS_VISIBLE },
+{ RET_SIMPLE_STATUS_P0, "QP problem solved", VS_VISIBLE },
+{ RET_SIMPLE_STATUS_M1, "QP problem could not be solved due to an internal error", VS_VISIBLE },
+{ RET_SIMPLE_STATUS_M2, "QP problem is infeasible (and thus could not be solved)", VS_VISIBLE },
+{ RET_SIMPLE_STATUS_M3, "QP problem is unbounded (and thus could not be solved)", VS_VISIBLE },
+/* IMPORTANT: Terminal list element! */
+{ TERMINAL_LIST_ELEMENT, "", VS_HIDDEN }
+};
+#else
+MessageHandling::ReturnValueList returnValueList[1]; /* Do not use messages for embedded platforms! */
+#endif
+
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+/*
+ *	M e s s a g e H a n d l i n g
+ */
+MessageHandling::MessageHandling( )
+{
+	errorVisibility   = VS_VISIBLE;
+	warningVisibility = VS_VISIBLE;
+	infoVisibility    = VS_VISIBLE;
+
+	outputFile = stdFile;
+	errorCount = 0;
+}
+
+/*
+ *	M e s s a g e H a n d l i n g
+ */
+MessageHandling::MessageHandling( FILE* _outputFile )
+{
+	errorVisibility   = VS_VISIBLE;
+	warningVisibility = VS_VISIBLE;
+	infoVisibility    = VS_VISIBLE;
+
+	outputFile = _outputFile;
+	errorCount = 0;
+}
+
+/*
+ *	M e s s a g e H a n d l i n g
+ */
+MessageHandling::MessageHandling(	VisibilityStatus _errorVisibility,
+									VisibilityStatus _warningVisibility,
+		 							VisibilityStatus _infoVisibility
+									)
+{
+	errorVisibility   = _errorVisibility;
+	warningVisibility = _warningVisibility;
+	infoVisibility    = _infoVisibility;
+
+	outputFile = stdFile;
+	errorCount = 0;
+}
+
+/*
+ *	M e s s a g e H a n d l i n g
+ */
+MessageHandling::MessageHandling( 	FILE* _outputFile,
+									VisibilityStatus _errorVisibility,
+									VisibilityStatus _warningVisibility,
+		 							VisibilityStatus _infoVisibility
+									)
+{
+	errorVisibility   = _errorVisibility;
+	warningVisibility = _warningVisibility;
+	infoVisibility    = _infoVisibility;
+
+	outputFile = _outputFile;
+	errorCount = 0;
+}
+
+
+
+/*
+ *	M e s s a g e H a n d l i n g
+ */
+MessageHandling::MessageHandling( const MessageHandling& rhs )
+{
+	errorVisibility   = rhs.errorVisibility;
+	warningVisibility = rhs.warningVisibility;
+	infoVisibility    = rhs.infoVisibility;
+
+	outputFile = rhs.outputFile;
+	errorCount = rhs.errorCount;
+}
+
+
+/*
+ *	~ M e s s a g e H a n d l i n g
+ */
+MessageHandling::~MessageHandling( )
+{
+	#ifndef __XPCTARGET__
+	if ( ( outputFile != 0 ) && ( outputFile != stdout ) && ( outputFile != stderr ) )
+		fclose( outputFile );
+	#endif /* __XPCTARGET__ */
+}
+
+
+/*
+ *	o p e r a t o r =
+ */
+MessageHandling& MessageHandling::operator=( const MessageHandling& rhs )
+{
+	if ( this != &rhs )
+	{
+		errorVisibility   = rhs.errorVisibility;
+		warningVisibility = rhs.warningVisibility;
+		infoVisibility    = rhs.infoVisibility;
+
+		outputFile = rhs.outputFile;
+		errorCount = rhs.errorCount;
+	}
+
+	return *this;
+}
+
+
+/*
+ *	t h r o w E r r o r
+ */
+returnValue MessageHandling::throwError(
+	returnValue Enumber,
+	const char* additionaltext,
+	const char* functionname,
+	const char* filename,
+	const unsigned long linenumber,
+	VisibilityStatus localVisibilityStatus
+	)
+{
+	/* consistency check */
+	if ( Enumber <= SUCCESSFUL_RETURN )
+		return throwError( RET_ERROR_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+
+	/* Call to common throwMessage function if error shall be displayed. */
+	if ( errorVisibility == VS_VISIBLE )
+		return throwMessage( Enumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"ERROR" );
+	else
+		return Enumber;
+}
+
+
+/*
+ *	t h r o w W a r n i n g
+ */
+returnValue MessageHandling::throwWarning(
+	returnValue Wnumber,
+	const char* additionaltext,
+	const char* functionname,
+	const char* filename,
+	const unsigned long linenumber,
+	VisibilityStatus localVisibilityStatus
+  	)
+{
+	/* consistency check */
+  	if ( Wnumber <= SUCCESSFUL_RETURN )
+		return throwError( RET_WARNING_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+
+	/* Call to common throwMessage function if warning shall be displayed. */
+	if ( warningVisibility == VS_VISIBLE )
+		return throwMessage( Wnumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"WARNING" );
+  	else
+  		return Wnumber;
+}
+
+
+/*
+ *	t h r o w I n f o
+ */
+returnValue MessageHandling::throwInfo(
+  	returnValue Inumber,
+	const char* additionaltext,
+  	const char* functionname,
+	const char* filename,
+	const unsigned long linenumber,
+	VisibilityStatus localVisibilityStatus
+ 	)
+{
+	/* consistency check */
+	if ( Inumber < SUCCESSFUL_RETURN )
+		return throwError( RET_INFO_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+
+	/* Call to common throwMessage function if info shall be displayed. */
+	if ( infoVisibility == VS_VISIBLE )
+		return throwMessage( Inumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"INFO" );
+	else
+		return Inumber;
+}
+
+
+/*
+ *	r e s e t
+ */
+returnValue MessageHandling::reset( )
+{
+	setErrorVisibilityStatus(   VS_VISIBLE );
+	setWarningVisibilityStatus( VS_VISIBLE );
+	setInfoVisibilityStatus(    VS_VISIBLE );
+
+	setOutputFile( stdFile );
+	setErrorCount( 0 );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	l i s t A l l M e s s a g e s
+ */
+returnValue MessageHandling::listAllMessages( )
+{
+	#ifndef __XPCTARGET__
+	int keypos = 0;
+	char myPrintfString[MAX_STRING_LENGTH];
+
+	/* Run through whole returnValueList and print each item. */
+	while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
+	{
+		snprintf( myPrintfString,MAX_STRING_LENGTH," %d - %s \n",keypos,returnValueList[keypos].data );
+		myPrintf( myPrintfString );
+
+		++keypos;
+	}
+	#endif
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+
+/*
+ *	t h r o w M e s s a g e
+ */
+returnValue MessageHandling::throwMessage(
+	returnValue RETnumber,
+	const char* additionaltext,
+	const char* functionname,
+	const char* filename,
+	const unsigned long linenumber,
+	VisibilityStatus localVisibilityStatus,
+	const char* RETstring
+ 	)
+{
+	#ifndef __SUPPRESSANYOUTPUT__
+	#ifndef __XPCTARGET__
+	int keypos = 0;
+	char myPrintfString[MAX_STRING_LENGTH];
+
+	/* 1) Determine number of whitespace for output. */
+	char whitespaces[MAX_STRING_LENGTH];
+	int numberOfWhitespaces = (errorCount-1)*2;
+
+	if ( numberOfWhitespaces < 0 )
+		numberOfWhitespaces = 0;
+
+	if ( numberOfWhitespaces > 40 )
+		numberOfWhitespaces = 40;
+
+	if ( numberOfWhitespaces >= (int)MAX_STRING_LENGTH )
+		numberOfWhitespaces = (int)MAX_STRING_LENGTH-1;
+
+	memset( whitespaces, ' ', (size_t) numberOfWhitespaces );
+	whitespaces[numberOfWhitespaces] = '\0';
+
+	/* 2) Find error/warning/info in list. */
+	while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
+	{
+		if ( returnValueList[keypos].key == RETnumber )
+			break;
+		else
+			++keypos;
+	}
+
+	if ( returnValueList[keypos].key == TERMINAL_LIST_ELEMENT )
+	{
+		throwError( RET_EWI_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+		return RETnumber;
+	}
+
+	/* 3) Print error/warning/info. */
+	if ( ( returnValueList[keypos].globalVisibilityStatus == VS_VISIBLE ) && ( localVisibilityStatus == VS_VISIBLE ) )
+	{
+		if ( errorCount < 0 )
+		{
+			myPrintf( "\n" );
+			errorCount = 0;
+		}
+
+		if ( errorCount > 0 )
+		{
+			snprintf( myPrintfString,MAX_STRING_LENGTH,"%s->", whitespaces );
+			myPrintf( myPrintfString );
+		}
+
+		if ( additionaltext == 0 )
+		{
+			#ifdef __DEBUG__
+			snprintf(	myPrintfString,MAX_STRING_LENGTH,"%s (%s, %s:%d): \t%s\n",
+						RETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data
+						);
+			#else
+			snprintf(	myPrintfString,MAX_STRING_LENGTH,"%s:  %s\n",
+						RETstring,returnValueList[keypos].data
+						);
+			#endif
+			myPrintf( myPrintfString );
+		}
+		else
+		{
+			#ifdef __DEBUG__
+			snprintf(	myPrintfString,MAX_STRING_LENGTH,"%s (%s, %s:%d): \t%s %s\n",
+						RETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data,additionaltext
+						);
+			#else
+			snprintf(	myPrintfString,MAX_STRING_LENGTH,"%s:  %s %s\n",
+						RETstring,returnValueList[keypos].data,additionaltext
+						);
+			#endif
+			myPrintf( myPrintfString );
+		}
+
+		/* take care of proper indention for subsequent error messages */
+		if ( RETstring[0] == 'E' )
+		{
+			++errorCount;
+		}
+		else
+		{
+			if ( errorCount > 0 )
+				myPrintf( "\n" );
+			errorCount = 0;
+		}
+	}
+	#endif /* __XPCTARGET__ */
+	#endif /* __SUPPRESSANYOUTPUT__ */
+
+	return RETnumber;
+}
+
+/****************************************************************************/
+/* S T A T I C  P U B L I C */
+/****************************************************************************/
+
+const char* MessageHandling::getErrorCodeMessage(	const returnValue _returnValue
+													)
+{
+	#ifndef __XPCTARGET__
+	int keypos = 0;
+	
+	/* 2) Find error/warning/info in list. */
+	while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
+	{
+		if ( returnValueList[keypos].key == _returnValue )
+			break;
+		else
+			++keypos;
+	}
+
+	if ( returnValueList[keypos].key == TERMINAL_LIST_ELEMENT )
+	{
+		return "Unknown error code";
+	}
+
+	return (returnValueList[keypos].data != 0) ? returnValueList[keypos].data : "No message for this error code";
+	#else /* __XPCTARGET__ */
+
+	return "No message for this error code";
+
+	#endif /* __XPCTARGET__ */
+}
+
+
+/*****************************************************************************
+ *  G L O B A L  M E S S A G E  H A N D L E R                                *
+ *****************************************************************************/
+
+
+/** Global message handler for all qpOASES modules.*/
+static MessageHandling globalMessageHandler( stdFile,VS_VISIBLE,VS_VISIBLE,VS_VISIBLE );
+
+
+/*
+ *	g e t G l o b a l M e s s a g e H a n d l e r
+ */
+MessageHandling* getGlobalMessageHandler( )
+{
+	return &globalMessageHandler;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/OQPinterface.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/OQPinterface.cpp
new file mode 100644
index 00000000000..e172d6da99f
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/OQPinterface.cpp
@@ -0,0 +1,670 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file src/OQPinterface.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2008-2014
+ *
+ *	Implementation of an interface comprising several utility functions
+ *	for solving test problems from the Online QP Benchmark Collection
+ *	(This collection is no longer maintained, see 
+ *	http://www.qpOASES.org/onlineQP for a backup).
+ *
+ */
+
+
+#include <qpOASES/extras/OQPinterface.hpp>
+#include <qpOASES/QProblem.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*
+ *	r e a d O Q P d i m e n s i o n s
+ */
+returnValue readOQPdimensions(	const char* path,
+								int& nQP, int& nV, int& nC, int& nEC
+								)
+{
+	/* 1) Setup file name where dimensions are stored. */
+	char filename[MAX_STRING_LENGTH];
+	snprintf( filename,MAX_STRING_LENGTH,"%sdims.oqp",path );
+
+	/* 2) Load dimensions from file. */
+	int dims[4];
+	if ( readFromFile( dims,4,filename ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_UNABLE_TO_READ_FILE );
+
+	nQP = dims[0];
+	nV  = dims[1];
+	nC  = dims[2];
+	nEC = dims[3];
+
+
+	/* consistency check */
+	if ( ( nQP <= 0 ) || ( nV <= 0 ) || ( nC < 0 ) || ( nEC < 0 ) )
+		return THROWERROR( RET_FILEDATA_INCONSISTENT );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	r e a d O Q P d a t a
+ */
+returnValue readOQPdata(	const char* path,
+							int& nQP, int& nV, int& nC, int& nEC,
+							real_t** H, real_t** g, real_t** A, real_t** lb, real_t** ub, real_t** lbA, real_t** ubA,
+							real_t** xOpt, real_t** yOpt, real_t** objOpt
+							)
+{
+	char filename[MAX_STRING_LENGTH];
+
+	/* consistency check */
+	if ( ( H == 0 ) || ( g == 0 ) || ( lb == 0 ) || ( ub == 0 ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+
+	/* 1) Obtain OQP dimensions. */
+	if ( readOQPdimensions( path, nQP,nV,nC,nEC ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_UNABLE_TO_READ_FILE );
+
+
+	/* another consistency check */
+	if ( ( nC > 0 ) && ( ( A == 0 ) || ( lbA == 0 ) || ( ubA == 0 ) ) )
+		return THROWERROR( RET_FILEDATA_INCONSISTENT );
+
+
+	/* 2) Allocate memory and load OQP data: */
+	/* Hessian matrix */
+	*H  = new real_t[nV*nV];
+	snprintf( filename,MAX_STRING_LENGTH,"%sH.oqp",path );
+	if ( readFromFile( *H,nV,nV,filename ) != SUCCESSFUL_RETURN )
+	{
+		delete[] *H;
+		return THROWERROR( RET_UNABLE_TO_READ_FILE );
+	}
+
+	/* gradient vector sequence */
+	*g  = new real_t[nQP*nV];
+	snprintf( filename,MAX_STRING_LENGTH,"%sg.oqp",path );
+	if ( readFromFile( *g,nQP,nV,filename ) != SUCCESSFUL_RETURN )
+	{
+		delete[] *g; delete[] *H;
+		return THROWERROR( RET_UNABLE_TO_READ_FILE );
+	}
+
+	/* lower bound vector sequence */
+	*lb  = new real_t[nQP*nV];
+	snprintf( filename,MAX_STRING_LENGTH,"%slb.oqp",path );
+	if ( readFromFile( *lb,nQP,nV,filename ) != SUCCESSFUL_RETURN )
+	{
+		delete[] *lb; delete[] *g; delete[] *H;
+		return THROWERROR( RET_UNABLE_TO_READ_FILE );
+	}
+
+	/* upper bound vector sequence */
+	*ub  = new real_t[nQP*nV];
+	snprintf( filename,MAX_STRING_LENGTH,"%sub.oqp",path );
+	if ( readFromFile( *ub,nQP,nV,filename ) != SUCCESSFUL_RETURN )
+	{
+		delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
+		return THROWERROR( RET_UNABLE_TO_READ_FILE );
+	}
+
+	if ( nC > 0 )
+	{
+		/* Constraint matrix */
+		*A   = new real_t[nC*nV];
+		snprintf( filename,MAX_STRING_LENGTH,"%sA.oqp",path );
+		if ( readFromFile( *A,nC,nV,filename ) != SUCCESSFUL_RETURN )
+		{
+			delete[] *A;
+			delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
+			return THROWERROR( RET_UNABLE_TO_READ_FILE );
+		}
+
+		/* lower constraints' bound vector sequence */
+		*lbA = new real_t[nQP*nC];
+		snprintf( filename,MAX_STRING_LENGTH,"%slbA.oqp",path );
+		if ( readFromFile( *lbA,nQP,nC,filename ) != SUCCESSFUL_RETURN )
+		{
+			delete[] *lbA; delete[] *A;
+			delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
+			return THROWERROR( RET_UNABLE_TO_READ_FILE );
+		}
+
+		/* upper constraints' bound vector sequence */
+		*ubA = new real_t[nQP*nC];
+		snprintf( filename,MAX_STRING_LENGTH,"%subA.oqp",path );
+		if ( readFromFile( *ubA,nQP,nC,filename ) != SUCCESSFUL_RETURN )
+		{
+			delete[] *ubA; delete[] *lbA; delete[] *A;
+			delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
+			return THROWERROR( RET_UNABLE_TO_READ_FILE );
+		}
+	}
+	else
+	{
+		*A = 0;
+		*lbA = 0;
+		*ubA = 0;
+	}
+
+	if ( xOpt != 0 )
+	{
+		/* primal solution vector sequence */
+		*xOpt = new real_t[nQP*nV];
+		snprintf( filename,MAX_STRING_LENGTH,"%sx_opt.oqp",path );
+		if ( readFromFile( *xOpt,nQP,nV,filename ) != SUCCESSFUL_RETURN )
+		{
+			delete[] xOpt;
+			if ( nC > 0 ) { delete[] *ubA; delete[] *lbA; delete[] *A; };
+			delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
+			return THROWERROR( RET_UNABLE_TO_READ_FILE );
+		}
+	}
+
+	if ( yOpt != 0 )
+	{
+		/* dual solution vector sequence */
+		*yOpt = new real_t[nQP*(nV+nC)];
+		snprintf( filename,MAX_STRING_LENGTH,"%sy_opt.oqp",path );
+		if ( readFromFile( *yOpt,nQP,nV+nC,filename ) != SUCCESSFUL_RETURN )
+		{
+			delete[] yOpt;
+			if ( xOpt != 0 ) { delete[] xOpt; };
+			if ( nC > 0 ) { delete[] *ubA; delete[] *lbA; delete[] *A; };
+			delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
+			return THROWERROR( RET_UNABLE_TO_READ_FILE );
+		}
+	}
+
+	if ( objOpt != 0 )
+	{
+		/* dual solution vector sequence */
+		*objOpt = new real_t[nQP];
+		snprintf( filename,MAX_STRING_LENGTH,"%sobj_opt.oqp",path );
+		if ( readFromFile( *objOpt,nQP,1,filename ) != SUCCESSFUL_RETURN )
+		{
+			delete[] objOpt;
+			if ( yOpt != 0 ) { delete[] yOpt; };
+			if ( xOpt != 0 ) { delete[] xOpt; };
+			if ( nC > 0 ) { delete[] *ubA; delete[] *lbA; delete[] *A; };
+			delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
+			return THROWERROR( RET_UNABLE_TO_READ_FILE );
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s o l v e O Q P b e n c h m a r k
+ */
+returnValue solveOQPbenchmark(	int nQP, int nV, int nC, int nEC,
+								const real_t* const _H, const real_t* const g, const real_t* const _A,
+								const real_t* const lb, const real_t* const ub,
+								const real_t* const lbA, const real_t* const ubA,
+								BooleanType isSparse, 
+								const Options& options, int& nWSR, real_t& maxCPUtime,
+								real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
+								)
+{
+	real_t maxNWSR = 0.0;
+	real_t avgNWSR = 0.0;
+	real_t avgCPUtime = 0.0;
+
+	returnValue returnvalue = solveOQPbenchmark(	nQP,nV,nC,nEC,
+													_H,g,_A,lb,ub,lbA,ubA,
+													isSparse,BT_TRUE,
+													options,nWSR,
+													maxNWSR,avgNWSR,maxCPUtime,avgCPUtime, 
+													maxStationarity,maxFeasibility,maxComplementarity
+													);
+	nWSR = (int)maxNWSR;
+
+	return returnvalue;
+}
+
+
+
+/*
+ *	s o l v e O Q P b e n c h m a r k
+ */
+returnValue solveOQPbenchmark(	int nQP, int nV, int nC, int nEC,
+								const real_t* const _H, const real_t* const g, const real_t* const _A,
+								const real_t* const lb, const real_t* const ub,
+								const real_t* const lbA, const real_t* const ubA,
+								BooleanType isSparse, BooleanType useHotstarts, 
+								const Options& options, int maxAllowedNWSR,
+								real_t& maxNWSR, real_t& avgNWSR, real_t& maxCPUtime, real_t& avgCPUtime,
+								real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
+								)
+{
+	int k;
+
+	/* I) SETUP AUXILIARY VARIABLES: */
+	/* 1) Keep nWSR and store current and maximum number of
+	 *    working set recalculations in temporary variables */
+	int nWSRcur;
+
+	real_t CPUtimeLimit = maxCPUtime;
+	real_t CPUtimeCur = CPUtimeLimit;
+	maxNWSR = 0.0;
+	avgNWSR = 0.0;
+	maxCPUtime = 0.0;
+	avgCPUtime = 0.0;
+	maxStationarity    = 0.0;
+	maxFeasibility     = 0.0;
+	maxComplementarity = 0.0;
+	real_t stat, feas, cmpl;
+
+	/* 2) Pointers to data of current QP ... */
+	const real_t* gCur;
+	const real_t* lbCur;
+	const real_t* ubCur;
+	const real_t* lbACur;
+	const real_t* ubACur;
+
+	/* 3) Vectors for solution obtained by qpOASES. */
+	real_t* x = new real_t[nV];
+	real_t* y = new real_t[nV+nC];
+	//real_t obj;
+
+	/* 4) Prepare matrix objects */
+	SymmetricMatrix *H; 
+	Matrix *A;
+
+	real_t* H_cpy = new real_t[nV*nV];
+	memcpy( H_cpy,_H, ((unsigned int)(nV*nV))*sizeof(real_t) );
+	real_t* A_cpy = new real_t[nC*nV];
+	memcpy( A_cpy,_A, ((unsigned int)(nC*nV))*sizeof(real_t) );
+
+	if ( isSparse == BT_TRUE )
+	{
+		SymSparseMat *Hs;
+		H = Hs = new SymSparseMat(nV, nV, nV, H_cpy);
+		A = new SparseMatrixRow(nC, nV, nV, A_cpy);
+		Hs->createDiagInfo();
+		delete[] A_cpy; delete[] H_cpy;
+	}
+	else
+	{
+		H = new SymDenseMat(nV, nV, nV, const_cast<real_t *>(H_cpy));
+		A = new DenseMatrix(nC, nV, nV, const_cast<real_t *>(A_cpy));
+	}
+
+	H->doFreeMemory( );
+	A->doFreeMemory( );
+
+	/* II) SETUP QPROBLEM OBJECT */
+	QProblem qp( nV,nC );
+	qp.setOptions( options );
+	//qp.setPrintLevel( PL_LOW );
+
+	//qp.printOptions();
+
+	/* III) RUN BENCHMARK SEQUENCE: */
+	returnValue returnvalue;
+
+	for( k=0; k<nQP; ++k )
+	{
+		//if ( k%50 == 0 )
+		//	printf( "%d\n",k );
+
+		/* 1) Update pointers to current QP data. */
+		gCur   = &( g[k*nV] );
+		lbCur  = &( lb[k*nV] );
+		ubCur  = &( ub[k*nV] );
+		lbACur = &( lbA[k*nC] );
+		ubACur = &( ubA[k*nC] );
+
+		/* 2) Set nWSR and maximum CPU time. */
+		nWSRcur = maxAllowedNWSR;
+		CPUtimeCur = CPUtimeLimit;
+
+		/* 3) Solve current QP. */
+		if ( ( k == 0 ) || ( useHotstarts == BT_FALSE ) )
+		{
+			/* initialise */
+			returnvalue = qp.init( H,gCur,A,lbCur,ubCur,lbACur,ubACur, nWSRcur,&CPUtimeCur );
+			if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
+			{
+				delete A; delete H; delete[] y; delete[] x;
+				return THROWERROR( returnvalue );
+			}
+		}
+		else
+		{
+			/* hotstart */
+			returnvalue = qp.hotstart( gCur,lbCur,ubCur,lbACur,ubACur, nWSRcur,&CPUtimeCur );
+			if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
+			{
+				delete A; delete H; delete[] y; delete[] x;
+				return THROWERROR( returnvalue );
+			}
+		}
+
+		/* 4) Obtain solution vectors and objective function value */
+		qp.getPrimalSolution( x );
+		qp.getDualSolution( y );
+		//obj = qp.getObjVal( );
+
+		/* 5) Compute KKT residuals */
+		getKKTResidual( nV, nC, _H,gCur,_A,lbCur,ubCur,lbACur,ubACur, x, y, stat, feas, cmpl );
+		
+		/* 6) Update maximum and average values. */
+		if ( ((double)nWSRcur) > maxNWSR )
+			maxNWSR = ((double)nWSRcur);
+		if (stat > maxStationarity) maxStationarity = stat;
+		if (feas > maxFeasibility) maxFeasibility = feas;
+		if (cmpl > maxComplementarity) maxComplementarity = cmpl;
+
+		if ( CPUtimeCur > maxCPUtime )
+			maxCPUtime = CPUtimeCur;
+
+		avgNWSR += ((double)nWSRcur);
+		avgCPUtime += CPUtimeCur;
+	}
+	avgNWSR /= ((double)nQP);
+	avgCPUtime /= ((double)nQP);
+
+	delete A; delete H; delete[] y; delete[] x;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s o l v e O Q P b e n c h m a r k
+ */
+returnValue solveOQPbenchmark(	int nQP, int nV,
+								const real_t* const _H, const real_t* const g,
+								const real_t* const lb, const real_t* const ub,
+								BooleanType isSparse, 
+								const Options& options, int& nWSR, real_t& maxCPUtime,
+								real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
+								)
+{
+	real_t maxNWSR = 0.0;
+	real_t avgNWSR = 0.0;
+	real_t avgCPUtime = 0.0;
+
+	returnValue returnvalue = solveOQPbenchmark(	nQP,nV,
+													_H,g,lb,ub,
+													isSparse,BT_TRUE,
+													options,nWSR,
+													maxNWSR,avgNWSR,maxCPUtime,avgCPUtime, 
+													maxStationarity,maxFeasibility,maxComplementarity
+													);
+	nWSR = (int)maxNWSR;
+
+	return returnvalue;
+}
+
+
+/*
+ *	s o l v e O Q P b e n c h m a r k
+ */
+returnValue solveOQPbenchmark(	int nQP, int nV,
+								const real_t* const _H, const real_t* const g,
+								const real_t* const lb, const real_t* const ub,
+								BooleanType isSparse, BooleanType useHotstarts, 
+								const Options& options, int maxAllowedNWSR,
+								real_t& maxNWSR, real_t& avgNWSR, real_t& maxCPUtime, real_t& avgCPUtime,
+								real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
+								)
+{
+	int k;
+
+	/* I) SETUP AUXILIARY VARIABLES: */
+	/* 1) Keep nWSR and store current and maximum number of
+	 *    working set recalculations in temporary variables */
+	int nWSRcur;
+
+	real_t CPUtimeLimit = maxCPUtime;
+	real_t CPUtimeCur = CPUtimeLimit;
+	real_t stat, feas, cmpl;
+	maxNWSR = 0;
+	avgNWSR = 0;
+	maxCPUtime = 0.0;
+	avgCPUtime = 0.0;
+	maxStationarity = 0.0;
+	maxFeasibility = 0.0;
+	maxComplementarity = 0.0;
+
+	/* 2) Pointers to data of current QP ... */
+	const real_t* gCur;
+	const real_t* lbCur;
+	const real_t* ubCur;
+
+	/* 3) Vectors for solution obtained by qpOASES. */
+	real_t* x = new real_t[nV];
+	real_t* y = new real_t[nV];
+	//real_t  obj;
+
+	/* 4) Prepare matrix objects */
+	SymmetricMatrix *H; 
+	real_t* H_cpy = new real_t[nV*nV];
+	memcpy( H_cpy,_H, ((unsigned int)(nV*nV))*sizeof(real_t) );
+
+	if ( isSparse == BT_TRUE )
+	{
+		SymSparseMat *Hs;
+		H = Hs = new SymSparseMat(nV, nV, nV, H_cpy);
+		Hs->createDiagInfo();
+		delete[] H_cpy;
+	}
+	else
+	{
+		H = new SymDenseMat(nV, nV, nV, const_cast<real_t *>(H_cpy));
+	}
+	
+	H->doFreeMemory( );
+
+	/* II) SETUP QPROBLEM OBJECT */
+	QProblemB qp( nV );
+	qp.setOptions( options );
+	//qp.setPrintLevel( PL_LOW );
+
+
+	/* III) RUN BENCHMARK SEQUENCE: */
+	returnValue returnvalue;
+
+	for( k=0; k<nQP; ++k )
+	{
+		//if ( k%50 == 0 )
+		//	printf( "%d\n",k );
+
+		/* 1) Update pointers to current QP data. */
+		gCur   = &( g[k*nV] );
+		lbCur  = &( lb[k*nV] );
+		ubCur  = &( ub[k*nV] );
+
+		/* 2) Set nWSR and maximum CPU time. */
+		nWSRcur = maxAllowedNWSR;
+		CPUtimeCur = CPUtimeLimit;
+
+		/* 3) Solve current QP. */
+		if ( ( k == 0 ) || ( useHotstarts == BT_FALSE ) )
+		{
+			/* initialise */
+			returnvalue = qp.init( H,gCur,lbCur,ubCur, nWSRcur,&CPUtimeCur );
+			if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
+			{
+				delete H; delete[] y; delete[] x;
+				return THROWERROR( returnvalue );
+			}
+		}
+		else
+		{
+			/* hotstart */
+			returnvalue = qp.hotstart( gCur,lbCur,ubCur, nWSRcur,&CPUtimeCur );
+			if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
+			{
+				delete H; delete[] y; delete[] x;
+				return THROWERROR( returnvalue );
+			}
+		}
+
+		/* 4) Obtain solution vectors and objective function value ... */
+		qp.getPrimalSolution( x );
+		qp.getDualSolution( y );
+		//obj = qp.getObjVal( );
+
+		/* 5) Compute KKT residuals */
+		getKKTResidual( nV, _H,gCur,lbCur,ubCur, x,y, stat,feas,cmpl );
+
+		/* 6) update maximum values. */
+		if ( nWSRcur > maxNWSR )
+			maxNWSR = nWSRcur;
+		if (stat > maxStationarity) maxStationarity = stat;
+		if (feas > maxFeasibility) maxFeasibility = feas;
+		if (cmpl > maxComplementarity) maxComplementarity = cmpl;
+
+		if ( CPUtimeCur > maxCPUtime )
+			maxCPUtime = CPUtimeCur;
+
+		avgNWSR += nWSRcur;
+		avgCPUtime += CPUtimeCur;
+	}
+	avgNWSR /= nQP;
+	avgCPUtime /= ((double)nQP);
+
+	delete H; delete[] y; delete[] x;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	r u n O Q P b e n c h m a r k
+ */
+returnValue runOQPbenchmark(	const char* path, BooleanType isSparse, const Options& options,
+								int& nWSR, real_t& maxCPUtime,
+								real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
+								)
+{
+	real_t maxNWSR = 0.0;
+	real_t avgNWSR = 0.0;
+	real_t avgCPUtime = 0.0;
+
+	returnValue returnvalue = runOQPbenchmark(	path,isSparse,BT_TRUE,
+												options,nWSR,
+												maxNWSR,avgNWSR,maxCPUtime,avgCPUtime, 
+												maxStationarity,maxFeasibility,maxComplementarity
+												);
+	nWSR = (int)maxNWSR;
+
+	return returnvalue;
+}
+
+
+/*
+ *	r u n O Q P b e n c h m a r k
+ */
+returnValue runOQPbenchmark(	const char* path, BooleanType isSparse, BooleanType useHotstarts, 
+								const Options& options, int maxAllowedNWSR,
+								real_t& maxNWSR, real_t& avgNWSR, real_t& maxCPUtime, real_t& avgCPUtime,
+								real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
+								)
+{
+	int nQP=0, nV=0, nC=0, nEC=0;
+
+	real_t *H, *g, *A, *lb, *ub, *lbA, *ubA;
+
+
+	returnValue returnvalue;
+
+	/* I) SETUP BENCHMARK: */
+	/* 1) Obtain QP sequence dimensions. */
+	if ( readOQPdimensions( path, nQP,nV,nC,nEC ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_BENCHMARK_ABORTED );
+
+	/* 2) Read OQP benchmark data. */
+	if ( readOQPdata(	path,
+						nQP,nV,nC,nEC,
+						&H,&g,&A,&lb,&ub,&lbA,&ubA,
+						0,0,0
+						) != SUCCESSFUL_RETURN )
+	{
+		return THROWERROR( RET_UNABLE_TO_READ_BENCHMARK );
+	}
+
+	// normaliseConstraints( nV,nC,A,lbA,ubA ); //only works when nP==1
+
+	/* II) SOLVE BENCHMARK */
+	if ( nC > 0 )
+	{
+		returnvalue = solveOQPbenchmark(	nQP,nV,nC,nEC,
+											H,g,A,lb,ub,lbA,ubA,
+											isSparse,useHotstarts,
+											options,maxAllowedNWSR,
+											maxNWSR,avgNWSR,maxCPUtime,avgCPUtime,
+											maxStationarity,maxFeasibility,maxComplementarity
+											);
+
+		if ( returnvalue != SUCCESSFUL_RETURN )
+		{
+			delete[] H; delete[] A;
+			delete[] ubA; delete[] lbA; delete[] ub; delete[] lb; delete[] g;
+			return THROWERROR( returnvalue );
+		}
+	}
+	else
+	{
+		returnvalue = solveOQPbenchmark(	nQP,nV,
+											H,g,lb,ub,
+											isSparse,useHotstarts,
+											options,maxAllowedNWSR,
+											maxNWSR,avgNWSR,maxCPUtime,avgCPUtime,
+											maxStationarity,maxFeasibility,maxComplementarity
+											);
+
+		if ( returnvalue != SUCCESSFUL_RETURN )
+		{
+			delete[] H; delete[] A;
+			delete[] ub; delete[] lb; delete[] g;
+			return THROWERROR( returnvalue );
+		}
+	}
+
+	delete[] H; delete[] A;
+	delete[] ubA; delete[] lbA; delete[] ub; delete[] lb; delete[] g;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/Options.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/Options.cpp
new file mode 100644
index 00000000000..14bc5bf6cf3
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/Options.cpp
@@ -0,0 +1,553 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file src/Options.cpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of the Options class designed to manage working sets of
+ *	constraints and bounds within a QProblem.
+ */
+
+
+#include <qpOASES/Options.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+/*
+ *	O p t i o n s
+ */
+Options::Options( )
+{
+	setToDefault( );
+}
+
+
+/*
+ *	O p t i o n s
+ */
+Options::Options( const Options& rhs )
+{
+	copy( rhs );
+}
+
+
+/*
+ *	~ O p t i o n s
+ */
+Options::~Options( )
+{
+}
+
+
+/*
+ *	o p e r a t o r =
+ */
+Options& Options::operator=( const Options& rhs )
+{
+	if ( this != &rhs )
+	{
+		copy( rhs );
+	}
+
+	return *this;
+}
+
+
+
+/*
+ *	s e t T o D e f a u l t
+ */
+returnValue Options::setToDefault( )
+{
+	printLevel = PL_MEDIUM;
+	#ifdef __DEBUG__
+	printLevel = PL_HIGH;
+	#endif
+	#ifdef __XPCTARGET__
+	printLevel = PL_NONE;
+	#endif
+	#ifdef __SUPPRESSANYOUTPUT__
+	printLevel = PL_NONE;
+	#endif
+
+	enableRamping                 =  BT_TRUE;
+	enableFarBounds               =  BT_TRUE;
+	enableFlippingBounds          =  BT_TRUE;
+	enableRegularisation          =  BT_FALSE;
+	enableFullLITests             =  BT_FALSE;
+	enableNZCTests                =  BT_TRUE;
+	enableDriftCorrection         =  1;
+	enableCholeskyRefactorisation =  0;
+	enableEqualities              =  BT_FALSE;
+
+	#ifdef __USE_SINGLE_PRECISION__
+	terminationTolerance          =  1.0e2 * EPS;
+	boundTolerance                =  1.0e2 * EPS;
+	#else
+	terminationTolerance          =  5.0e6 * EPS;
+	boundTolerance                =  1.0e6 * EPS;
+	#endif
+	boundRelaxation               =  1.0e4;
+	#ifdef __USE_SINGLE_PRECISION__
+	epsNum                        = -1.0e2 * EPS;
+	epsDen                        =  1.0e2 * EPS;
+	#else
+	epsNum                        = -1.0e3 * EPS;
+	epsDen                        =  1.0e3 * EPS;
+	#endif
+	maxPrimalJump                 =  1.0e8;
+	maxDualJump                   =  1.0e8;
+
+	initialRamping                =  0.5;
+	finalRamping                  =  1.0;
+	initialFarBounds              =  1.0e6;
+	growFarBounds                 =  1.0e3;
+ 	initialStatusBounds           =  ST_LOWER;
+	#ifdef __USE_SINGLE_PRECISION__
+	epsFlipping                   =  5.0e1 * EPS;
+	#else
+	epsFlipping                   =  1.0e3 * EPS;
+	#endif
+	numRegularisationSteps        =  0;
+	#ifdef __USE_SINGLE_PRECISION__
+	epsRegularisation             =  2.0e1 * EPS;
+	numRefinementSteps            =  2;
+	#else
+	epsRegularisation             =  1.0e3 * EPS;
+	numRefinementSteps            =  1;
+	#endif
+	epsIterRef                    =  1.0e2 * EPS;
+	#ifdef __USE_SINGLE_PRECISION__
+	epsLITests                    =  5.0e1 * EPS;
+	epsNZCTests                   =  1.0e2 * EPS;
+	#else
+	epsLITests                    =  1.0e5 * EPS;
+	epsNZCTests                   =  3.0e3 * EPS;
+	#endif
+
+	enableDropInfeasibles         =  BT_FALSE;
+    dropBoundPriority             =  1;
+    dropEqConPriority             =  1;
+    dropIneqConPriority           =  1;
+    
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t T o R e l i a b l e
+ */
+returnValue Options::setToReliable( )
+{
+	setToDefault( );
+
+	enableFullLITests             =  BT_TRUE;
+	enableCholeskyRefactorisation =  1;
+
+	#ifdef __USE_SINGLE_PRECISION__
+	numRefinementSteps            =  3;
+	#else
+	numRefinementSteps            =  2;
+	#endif
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t T o M P C
+ */
+returnValue Options::setToMPC( )
+{
+	setToDefault( );
+
+	enableRamping                 =  BT_FALSE;
+	enableFarBounds               =  BT_TRUE;
+	enableFlippingBounds          =  BT_FALSE;
+	enableRegularisation          =  BT_TRUE;
+	enableNZCTests                =  BT_FALSE;
+	enableDriftCorrection         =  0;
+	enableEqualities              =  BT_TRUE;
+
+	#ifdef __USE_SINGLE_PRECISION__
+	terminationTolerance          =  1.0e3 * EPS;
+	#else
+	terminationTolerance          =  1.0e9 * EPS;
+	#endif
+
+	initialStatusBounds           =  ST_INACTIVE;
+	numRegularisationSteps        =  1;
+	#ifdef __USE_SINGLE_PRECISION__
+	numRefinementSteps            =  2;
+	#else
+	numRefinementSteps            =  0;
+	#endif
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t T o F a s t
+ */
+returnValue Options::setToFast( )
+{
+	return setToMPC( );
+}
+
+
+
+/*
+ *	e n s u r e C o n s i s t e n c y
+ */
+returnValue Options::ensureConsistency( )
+{
+	BooleanType needToAdjust = BT_FALSE;
+
+	/* flipping bounds require far bounds */
+    /* (ckirches) Removed this as per filter's trust region
+	if( enableFlippingBounds == BT_TRUE )
+		enableFarBounds = BT_TRUE;
+    */
+	
+	if( enableDriftCorrection < 0 )
+	{
+		enableDriftCorrection = 0;
+		needToAdjust = BT_TRUE;
+	}
+	
+	if( enableCholeskyRefactorisation < 0 )
+	{
+		enableCholeskyRefactorisation = 0;
+		needToAdjust = BT_TRUE;
+	}
+
+	if ( terminationTolerance <= 0.0 )
+	{
+		terminationTolerance = EPS;
+		needToAdjust = BT_TRUE;
+	}
+
+	if ( epsIterRef <= 0.0 )
+	{
+		epsIterRef = EPS;
+		needToAdjust = BT_TRUE;
+	}
+
+	if ( epsRegularisation <= 0.0 )
+	{
+		epsRegularisation = EPS;
+		needToAdjust = BT_TRUE;
+	}
+
+	if ( boundTolerance <= 0.0 )
+	{
+		boundTolerance = EPS;
+		needToAdjust = BT_TRUE;
+	}
+
+	if ( boundRelaxation <= 0.0 )
+	{
+		boundRelaxation = EPS;
+		needToAdjust = BT_TRUE;
+	}
+	
+	if ( maxPrimalJump <= 0.0 )
+	{
+		maxPrimalJump = EPS;
+		needToAdjust = BT_TRUE;
+	}
+
+	if ( maxDualJump <= 0.0 )
+	{
+		maxDualJump = EPS;
+		needToAdjust = BT_TRUE;
+	}
+
+
+	if ( initialRamping < 0.0 )
+	{
+		initialRamping = 0.0;
+		needToAdjust = BT_TRUE;
+	}
+
+	if ( finalRamping < 0.0 )
+	{
+		finalRamping = 0.0;
+		needToAdjust = BT_TRUE;
+	}
+
+	if ( initialFarBounds <= boundRelaxation )
+	{
+		initialFarBounds = boundRelaxation+EPS;
+		needToAdjust = BT_TRUE;
+	}
+	
+	if ( growFarBounds < 1.1 )
+	{
+		growFarBounds = 1.1;
+		needToAdjust = BT_TRUE;
+	}
+
+	if ( epsFlipping <= 0.0 )
+	{
+		epsFlipping = EPS;
+		needToAdjust = BT_TRUE;
+	}
+
+	if ( numRegularisationSteps < 0 )
+	{
+		numRegularisationSteps = 0;
+		needToAdjust = BT_TRUE;
+	}
+
+	if ( epsRegularisation < 0.0 )
+	{
+		epsRegularisation = EPS;
+		needToAdjust = BT_TRUE;
+	}
+
+	if ( numRefinementSteps < 0 )
+	{
+		numRefinementSteps = 0;
+		needToAdjust = BT_TRUE;
+	}
+
+	if ( epsIterRef < 0.0 )
+	{
+		epsIterRef = EPS;
+		needToAdjust = BT_TRUE;
+	}
+
+	if ( epsLITests < 0.0 )
+	{
+		epsLITests = EPS;
+		needToAdjust = BT_TRUE;
+	}
+
+	if ( epsNZCTests < 0.0 )
+	{
+		epsNZCTests = EPS;
+		needToAdjust = BT_TRUE;
+	}
+
+	if ( needToAdjust == BT_TRUE)
+		return THROWWARNING( RET_OPTIONS_ADJUSTED );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	p r i n t
+ */
+returnValue Options::print( ) const
+{
+	#ifndef __XPCTARGET__
+	#ifndef __DSPACE__
+	char myPrintfString[MAX_STRING_LENGTH];
+	char info[MAX_STRING_LENGTH];
+
+	myPrintf( "\n###################   qpOASES  --  QP OPTIONS   ##################\n" );
+	myPrintf( "\n" );
+
+	convertPrintLevelToString( printLevel,info );
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"printLevel                     =  %s\n",info );
+	myPrintf( myPrintfString );
+
+	myPrintf( "\n" );
+
+	convertBooleanTypeToString( enableRamping,info );
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableRamping                  =  %s\n",info );
+	myPrintf( myPrintfString );
+
+	convertBooleanTypeToString( enableFarBounds,info );
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableFarBounds                =  %s\n",info );
+	myPrintf( myPrintfString );
+
+	convertBooleanTypeToString( enableFlippingBounds,info );
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableFlippingBounds           =  %s\n",info );
+	myPrintf( myPrintfString );
+
+	convertBooleanTypeToString( enableRegularisation,info );
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableRegularisation           =  %s\n",info );
+	myPrintf( myPrintfString );
+
+	convertBooleanTypeToString( enableFullLITests,info );
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableFullLITests              =  %s\n",info );
+	myPrintf( myPrintfString );
+
+	convertBooleanTypeToString( enableNZCTests,info );
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableNZCTests                 =  %s\n",info );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableDriftCorrection          =  %d\n",enableDriftCorrection );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableCholeskyRefactorisation  =  %d\n",enableCholeskyRefactorisation );
+	myPrintf( myPrintfString );
+
+	convertBooleanTypeToString( enableEqualities,info );
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableEqualities               =  %s\n",info );
+	myPrintf( myPrintfString );
+
+	myPrintf( "\n" );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"terminationTolerance           =  %e\n",terminationTolerance );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"boundTolerance                 =  %e\n",boundTolerance );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"boundRelaxation                =  %e\n",boundRelaxation );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"epsNum                         =  %e\n",epsNum );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"epsDen                         =  %e\n",epsDen );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"maxPrimalJump                  =  %e\n",maxPrimalJump );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"maxDualJump                    =  %e\n",maxDualJump );
+	myPrintf( myPrintfString );
+
+	myPrintf( "\n" );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"initialRamping                 =  %e\n",initialRamping );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"finalRamping                   =  %e\n",finalRamping );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"initialFarBounds               =  %e\n",initialFarBounds );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"growFarBounds                  =  %e\n",growFarBounds );
+	myPrintf( myPrintfString );
+
+	convertSubjectToStatusToString( initialStatusBounds,info );
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"initialStatusBounds            =  %s\n",info );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"epsFlipping                    =  %e\n",epsFlipping );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"numRegularisationSteps         =  %d\n",numRegularisationSteps );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"epsRegularisation              =  %e\n",epsRegularisation );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"numRefinementSteps             =  %d\n",numRefinementSteps );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"epsIterRef                     =  %e\n",epsIterRef );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"epsLITests                     =  %e\n",epsLITests );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,"epsNZCTests                    =  %e\n",epsNZCTests );
+	myPrintf( myPrintfString );
+
+	myPrintf( "\n\n" );
+	#endif
+	#endif
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+/*
+ *	c o p y
+ */
+returnValue Options::copy(	const Options& rhs
+							)
+{
+	printLevel             = rhs.printLevel;
+
+	enableRamping                 =  rhs.enableRamping;
+	enableFarBounds               =  rhs.enableFarBounds;
+	enableFlippingBounds          =  rhs.enableFlippingBounds;
+	enableRegularisation          =  rhs.enableRegularisation;
+	enableFullLITests             =  rhs.enableFullLITests;
+	enableNZCTests                =  rhs.enableNZCTests;
+	enableDriftCorrection         =  rhs.enableDriftCorrection;
+	enableCholeskyRefactorisation =  rhs.enableCholeskyRefactorisation;
+	enableEqualities              =  rhs.enableEqualities;
+
+	terminationTolerance          =  rhs.terminationTolerance;
+	boundTolerance                =  rhs.boundTolerance;
+	boundRelaxation               =  rhs.boundRelaxation;
+	epsNum                        =  rhs.epsNum;
+	epsDen                        =  rhs.epsDen;
+	maxPrimalJump                 =  rhs.maxPrimalJump;
+	maxDualJump                   =  rhs.maxDualJump;
+
+	initialRamping                =  rhs.initialRamping;
+	finalRamping                  =  rhs.finalRamping;
+	initialFarBounds              =  rhs.initialFarBounds;
+	growFarBounds                 =  rhs.growFarBounds;
+ 	initialStatusBounds           =  rhs.initialStatusBounds;
+	epsFlipping                   =  rhs.epsFlipping;
+	numRegularisationSteps        =  rhs.numRegularisationSteps;
+	epsRegularisation             =  rhs.epsRegularisation;
+	numRefinementSteps            =  rhs.numRefinementSteps;
+	epsIterRef                    =  rhs.epsIterRef;
+	epsLITests                    =  rhs.epsLITests;
+	epsNZCTests                   =  rhs.epsNZCTests;
+
+	enableDropInfeasibles         =  rhs.enableDropInfeasibles;
+    dropBoundPriority             =  rhs.dropBoundPriority;
+    dropEqConPriority             =  rhs.dropEqConPriority;
+    dropIneqConPriority           =  rhs.dropIneqConPriority;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/QProblem.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/QProblem.cpp
new file mode 100644
index 00000000000..14dc07cb962
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/QProblem.cpp
@@ -0,0 +1,6338 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file src/QProblem.cpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of the QProblem class which is able to use the newly
+ *	developed online active set strategy for parametric quadratic programming.
+ */
+
+
+#include <qpOASES/QProblem.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+/*
+ *	Q P r o b l e m
+ */
+QProblem::QProblem( ) : QProblemB( )
+{
+	freeConstraintMatrix = BT_FALSE;
+	A = 0;
+
+	lbA = 0;
+	ubA = 0;
+
+	sizeT = 0;
+	T = 0;
+	Q = 0;
+
+	Ax = 0;
+	Ax_l = 0;
+	Ax_u = 0;
+
+	constraintProduct = 0;
+
+	tempA = 0;
+	ZFR_delta_xFRz = 0;
+	delta_xFRy = 0;
+	delta_xFRz = 0;
+	tempB = 0;
+	delta_yAC_TMP = 0;
+}
+
+
+/*
+ *	Q P r o b l e m
+ */
+QProblem::QProblem( int _nV, int _nC, HessianType _hessianType ) : QProblemB( _nV,_hessianType )
+{
+	int i;
+
+	/* consistency checks */
+	if ( _nV <= 0 )
+	{
+		_nV = 1;
+		THROWERROR( RET_INVALID_ARGUMENTS );
+	}
+
+	if ( _nC < 0 )
+	{
+		_nC = 0;
+		THROWERROR( RET_INVALID_ARGUMENTS );
+	}
+
+	if ( _nC > 0 )
+	{
+		freeConstraintMatrix = BT_FALSE;
+		A = 0;
+
+		lbA = new real_t[_nC];
+		for( i=0; i<_nC; ++i ) lbA[i] = 0.0;
+
+		ubA = new real_t[_nC];
+		for( i=0; i<_nC; ++i ) ubA[i] = 0.0;
+	}
+	else
+	{
+		/* prevent segmentation faults in case nC == 0
+		 * (avoiding checks for A!=0 around all calls to A->... */
+		freeConstraintMatrix = BT_TRUE;
+		A = new DenseMatrix( );
+
+		lbA = 0;
+		ubA = 0;
+	}
+
+	constraints.init( _nC );
+
+	delete[] y; /* y of no constraints version too short! */
+	y = new real_t[_nV+_nC];
+	for( i=0; i<_nV+_nC; ++i ) y[i] = 0.0;
+
+	sizeT = getMin( _nV,_nC );
+	T = new real_t[sizeT*sizeT];
+	Q = new real_t[_nV*_nV];
+
+	if ( _nC > 0 )
+	{
+		Ax = new real_t[_nC];
+		Ax_l = new real_t[_nC];
+		Ax_u = new real_t[_nC];
+	}
+	else
+	{
+		Ax = 0;
+		Ax_l = 0;
+		Ax_u = 0;
+	}
+
+	constraintProduct = 0;
+
+	tempA = new real_t[_nV];			/* nFR */
+	ZFR_delta_xFRz = new real_t[_nV];	/* nFR */
+	delta_xFRz = new real_t[_nV];		/* nZ */
+
+	if ( _nC > 0 )
+	{
+		tempB = new real_t[_nC];			/* nAC */
+		delta_xFRy = new real_t[_nC];		/* nAC */
+		delta_yAC_TMP = new real_t[_nC];   /* nAC */
+	}
+	else
+	{
+		tempB = 0;
+		delta_xFRy = 0;
+		delta_yAC_TMP = 0;
+	}
+
+	flipper.init( (unsigned int)_nV,(unsigned int)_nC );
+}
+
+
+/*
+ *	Q P r o b l e m
+ */
+QProblem::QProblem( const QProblem& rhs ) : QProblemB( rhs )
+{
+	freeConstraintMatrix = BT_FALSE;
+	A = 0;
+
+	copy( rhs );
+}
+
+
+/*
+ *	~ Q P r o b l e m
+ */
+QProblem::~QProblem( )
+{
+	clear( );
+}
+
+
+/*
+ *	o p e r a t o r =
+ */
+QProblem& QProblem::operator=( const QProblem& rhs )
+{
+	if ( this != &rhs )
+	{
+		clear( );
+		QProblemB::operator=( rhs );
+		copy( rhs );
+	}
+
+	return *this;
+}
+
+
+/*
+ *	r e s e t
+ */
+returnValue QProblem::reset( )
+{
+	int i;
+	int nV = getNV( );
+	int nC = getNC( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+
+	/* 1) Reset bounds, Cholesky decomposition and status flags. */
+	if ( QProblemB::reset( ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_RESET_FAILED );
+
+	/* 2) Reset constraints. */
+	constraints.init( nC );
+
+	/* 3) Reset TQ factorisation. */
+	for( i=0; i<sizeT*sizeT; ++i )
+		T[i] = 0.0;
+
+	for( i=0; i<nV*nV; ++i )
+		Q[i] = 0.0;
+
+	/* 4) Reset constraint product pointer. */
+	constraintProduct = 0;
+
+	/* 5) Reset flipper object */
+	flipper.init( (unsigned int)nV,(unsigned int)nC );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	i n i t
+ */
+returnValue QProblem::init(	SymmetricMatrix *_H, const real_t* const _g, Matrix *_A,
+							const real_t* const _lb, const real_t* const _ub,
+							const real_t* const _lbA, const real_t* const _ubA,
+							int& nWSR, real_t* const cputime
+							)
+{
+	if ( getNV( ) == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* 1) Consistency check. */
+	if ( isInitialised( ) == BT_TRUE )
+	{
+		THROWWARNING( RET_QP_ALREADY_INITIALISED );
+		reset( );
+	}
+
+	/* 2) Setup QP data. */
+	if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* 3) Call to main initialisation routine (without any additional information). */
+	return solveInitialQP( 0,0,0,0, nWSR,cputime );
+}
+
+
+/*
+ *	i n i t
+ */
+returnValue QProblem::init(	const real_t* const _H, const real_t* const _g, const real_t* const _A,
+							const real_t* const _lb, const real_t* const _ub,
+							const real_t* const _lbA, const real_t* const _ubA,
+							int& nWSR, real_t* const cputime
+							)
+{
+	if ( getNV( ) == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* 1) Consistency check. */
+	if ( isInitialised( ) == BT_TRUE )
+	{
+		THROWWARNING( RET_QP_ALREADY_INITIALISED );
+		reset( );
+	}
+
+	/* 2) Setup QP data. */
+	if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* 3) Call to main initialisation routine (without any additional information). */
+	return solveInitialQP( 0,0,0,0, nWSR,cputime );
+}
+
+
+/*
+ *	i n i t
+ */
+returnValue QProblem::init(	const char* const H_file, const char* const g_file, const char* const A_file,
+							const char* const lb_file, const char* const ub_file,
+							const char* const lbA_file, const char* const ubA_file,
+							int& nWSR, real_t* const cputime
+							)
+{
+	if ( getNV( ) == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* 1) Consistency check. */
+	if ( isInitialised( ) == BT_TRUE )
+	{
+		THROWWARNING( RET_QP_ALREADY_INITIALISED );
+		reset( );
+	}
+
+	/* 2) Setup QP data from files. */
+	if ( setupQPdataFromFile( H_file,g_file,A_file,lb_file,ub_file,lbA_file,ubA_file ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_UNABLE_TO_READ_FILE );
+
+	/* 3) Call to main initialisation routine (without any additional information). */
+	return solveInitialQP( 0,0,0,0, nWSR,cputime );
+}
+
+
+/*
+ *	i n i t
+ */
+returnValue QProblem::init(	SymmetricMatrix *_H, const real_t* const _g, Matrix *_A,
+							const real_t* const _lb, const real_t* const _ub,
+							const real_t* const _lbA, const real_t* const _ubA,
+							int& nWSR, real_t* const cputime,
+							const real_t* const xOpt, const real_t* const yOpt,
+							const Bounds* const guessedBounds, const Constraints* const guessedConstraints
+							)
+{
+	int i;
+	int nV = getNV( );
+	int nC = getNC( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* 1) Consistency checks. */
+	if ( isInitialised( ) == BT_TRUE )
+	{
+		THROWWARNING( RET_QP_ALREADY_INITIALISED );
+		reset( );
+	}
+
+	if ( guessedBounds != 0 )
+	{
+		for( i=0; i<nV; ++i )
+		{
+			if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
+				return THROWERROR( RET_INVALID_ARGUMENTS );
+		}
+	}
+
+	if ( guessedConstraints != 0 )
+	{
+		for( i=0; i<nC; ++i )
+			if ( guessedConstraints->getStatus( i ) == ST_UNDEFINED )
+				return THROWERROR( RET_INVALID_ARGUMENTS );
+	}
+
+	/* exclude these possibilities in order to avoid inconsistencies */
+	if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* 2) Setup QP data. */
+	if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* 3) Call to main initialisation routine. */
+	return solveInitialQP( xOpt,yOpt,guessedBounds,guessedConstraints, nWSR,cputime );
+}
+
+
+/*
+ *	i n i t
+ */
+returnValue QProblem::init(	const real_t* const _H, const real_t* const _g, const real_t* const _A,
+							const real_t* const _lb, const real_t* const _ub,
+							const real_t* const _lbA, const real_t* const _ubA,
+							int& nWSR, real_t* const cputime,
+							const real_t* const xOpt, const real_t* const yOpt,
+							const Bounds* const guessedBounds, const Constraints* const guessedConstraints
+							)
+{
+	int i;
+	int nV = getNV( );
+	int nC = getNC( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* 1) Consistency checks. */
+	if ( isInitialised( ) == BT_TRUE )
+	{
+		THROWWARNING( RET_QP_ALREADY_INITIALISED );
+		reset( );
+	}
+
+	if ( guessedBounds != 0 )
+	{
+		for( i=0; i<nV; ++i )
+		{
+			if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
+				return THROWERROR( RET_INVALID_ARGUMENTS );
+		}
+	}
+
+	if ( guessedConstraints != 0 )
+	{
+		for( i=0; i<nC; ++i )
+			if ( guessedConstraints->getStatus( i ) == ST_UNDEFINED )
+				return THROWERROR( RET_INVALID_ARGUMENTS );
+	}
+
+	/* exclude these possibilities in order to avoid inconsistencies */
+	if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* 2) Setup QP data. */
+	if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* 3) Call to main initialisation routine. */
+	return solveInitialQP( xOpt,yOpt,guessedBounds,guessedConstraints, nWSR,cputime );
+}
+
+
+/*
+ *	i n i t
+ */
+returnValue QProblem::init(	const char* const H_file, const char* const g_file, const char* const A_file,
+							const char* const lb_file, const char* const ub_file,
+							const char* const lbA_file, const char* const ubA_file,
+							int& nWSR, real_t* const cputime,
+							const real_t* const xOpt, const real_t* const yOpt,
+							const Bounds* const guessedBounds, const Constraints* const guessedConstraints
+							)
+{
+	int i;
+	int nV = getNV( );
+	int nC = getNC( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* 1) Consistency checks. */
+	if ( isInitialised( ) == BT_TRUE )
+	{
+		THROWWARNING( RET_QP_ALREADY_INITIALISED );
+		reset( );
+	}
+
+	for( i=0; i<nV; ++i )
+	{
+		if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
+			return THROWERROR( RET_INVALID_ARGUMENTS );
+	}
+
+	for( i=0; i<nC; ++i )
+		if ( guessedConstraints->getStatus( i ) == ST_UNDEFINED )
+			return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* exclude these possibilities in order to avoid inconsistencies */
+	if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* 2) Setup QP data from files. */
+	if ( setupQPdataFromFile( H_file,g_file,A_file,lb_file,ub_file,lbA_file,ubA_file ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_UNABLE_TO_READ_FILE );
+
+	/* 3) Call to main initialisation routine. */
+	return solveInitialQP( xOpt,yOpt,guessedBounds,guessedConstraints, nWSR,cputime );
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+returnValue QProblem::hotstart(	const real_t* const g_new,
+								const real_t* const lb_new, const real_t* const ub_new,
+								const real_t* const lbA_new, const real_t* const ubA_new,
+								int& nWSR, real_t* const cputime
+								)
+{
+	int i, nActiveFar;
+	int nV = getNV ();
+	int nC = getNC ();
+
+	if (nV == 0)
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	returnValue returnvalue = SUCCESSFUL_RETURN;
+
+	/* Simple check for consistency of bounds and constraints. */
+	if ( areBoundsConsistent(lb_new, ub_new, lbA_new, ubA_new) != SUCCESSFUL_RETURN )
+		return setInfeasibilityFlag(returnvalue,BT_TRUE);
+
+	++count;
+
+	int nWSR_max = nWSR;
+	int nWSR_performed = 0;
+
+	real_t cputime_remaining = INFTY, *pcputime_rem;
+	real_t cputime_needed = 0.0;
+
+	real_t farbound = options.initialFarBounds;
+	
+	/* writeQpDataIntoMatFile( "qpData.mat" ); */
+	/* writeQpWorkspaceIntoMatFile( "qpWorkspace.mat" ); */
+	
+	if ( haveCholesky == BT_FALSE )
+	{
+		returnvalue = setupInitialCholesky( );
+		if (returnvalue != SUCCESSFUL_RETURN)
+			return THROWERROR(returnvalue);
+	}
+
+	if ( options.enableFarBounds == BT_FALSE )
+	{
+		/* Automatically call standard solveQP if regularisation is not active. */
+		returnvalue = solveRegularisedQP( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime,0 );
+	}
+	else
+	{
+		real_t *ub_new_far = new real_t[nV];
+		real_t *lb_new_far = new real_t[nV];
+		real_t *ubA_new_far = new real_t[nC];
+		real_t *lbA_new_far = new real_t[nC];
+
+		/* possibly extend initial far bounds to largest bound/constraint data */
+		if (ub_new)
+			for (i = 0; i < nV; i++)
+				if ((ub_new[i] < INFTY) && (ub_new[i] > farbound)) farbound = ub_new[i];
+		if (lb_new)
+			for (i = 0; i < nV; i++)
+				if ((lb_new[i] > -INFTY) && (lb_new[i] < -farbound)) farbound = -lb_new[i];
+		if (ubA_new)
+			for (i = 0; i < nC; i++)
+				if ((ubA_new[i] < INFTY) && (ubA_new[i] > farbound)) farbound = ubA_new[i];
+		if (lbA_new)
+			for (i = 0; i < nC; i++)
+				if ((lbA_new[i] > -INFTY) && (lbA_new[i] < -farbound)) farbound = -lbA_new[i];
+
+		updateFarBounds(	farbound,nV+nC,
+							lb_new,lb_new_far, ub_new,ub_new_far,
+							lbA_new,lbA_new_far, ubA_new,ubA_new_far
+							);
+
+		for ( ;; )
+		{
+			nWSR = nWSR_max;
+			if ( cputime != 0 )
+			{
+				cputime_remaining = *cputime - cputime_needed;
+				pcputime_rem = &cputime_remaining;
+			}
+			else
+				pcputime_rem = 0;
+
+			/* Automatically call standard solveQP if regularisation is not active. */
+			returnvalue = solveRegularisedQP( g_new,lb_new_far,ub_new_far,lbA_new_far,ubA_new_far, nWSR,pcputime_rem,nWSR_performed );
+
+			nWSR_performed  = nWSR;
+			cputime_needed += cputime_remaining;
+
+			/* Check for active far-bounds and move them away */
+			nActiveFar = 0;
+			farbound *= options.growFarBounds;
+
+			if ( infeasible == BT_TRUE )
+			{
+				if ( farbound >= INFTY )
+				{
+					returnvalue = RET_HOTSTART_STOPPED_INFEASIBILITY;
+					break; // goto farewell;   
+				}
+
+				updateFarBounds(	farbound,nV+nC,
+									lb_new,lb_new_far, ub_new,ub_new_far,
+									lbA_new,lbA_new_far, ubA_new,ubA_new_far
+									);
+			}
+			else if ( status == QPS_SOLVED )
+			{
+				real_t tol = farbound/options.growFarBounds * options.boundTolerance;
+
+				for ( i=0; i<nV; ++i )
+				{
+					if ( ( ( lb_new == 0 ) || ( lb_new_far[i] > lb_new[i] ) ) && ( getAbs ( lb_new_far[i] - x[i] ) < tol ) )
+						++nActiveFar;
+					if ( ( ( ub_new == 0 ) || ( ub_new_far[i] < ub_new[i] ) ) && ( getAbs ( ub_new_far[i] - x[i] ) < tol ) )
+						++nActiveFar;
+				}
+				for ( i=0; i<nC; ++i )
+				{
+					if ( ( ( lbA_new == 0 ) || ( lbA_new_far[i] > lbA_new[i] ) ) && ( getAbs ( lbA_new_far[i] - Ax[i] ) < tol ) )
+						++nActiveFar;
+					if ( ( ( ubA_new == 0 ) || ( ubA_new_far[i] < ubA_new[i] ) ) && ( getAbs ( ubA_new_far[i] - Ax[i] ) < tol ) )
+						++nActiveFar;
+				}
+
+				if ( nActiveFar == 0 )
+					break;
+
+				status = QPS_HOMOTOPYQPSOLVED;
+
+				if ( farbound >= INFTY )
+				{
+					unbounded = BT_TRUE;
+					returnvalue = RET_HOTSTART_STOPPED_UNBOUNDEDNESS;
+					goto farewell;
+				}
+
+				updateFarBounds(	farbound,nV+nC,
+									lb_new,lb_new_far, ub_new,ub_new_far,
+									lbA_new,lbA_new_far, ubA_new,ubA_new_far
+									);
+			}
+			else
+			{
+				/* some other error when solving QP */
+				break;
+			}
+
+			/* advance ramp offset to avoid Ramping cycles */
+			rampOffset++;
+		}
+
+		farewell:
+			if ( cputime != 0 )
+				*cputime = cputime_needed;
+			delete[] lbA_new_far; delete[] ubA_new_far;
+			delete[] lb_new_far; delete[] ub_new_far;
+	}
+
+	return ( returnvalue != SUCCESSFUL_RETURN ) ? THROWERROR( returnvalue ) : returnvalue;
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+returnValue QProblem::hotstart(	const char* const g_file,
+								const char* const lb_file, const char* const ub_file,
+								const char* const lbA_file, const char* const ubA_file,
+								int& nWSR, real_t* const cputime
+								)
+{
+	int nV = getNV( );
+	int nC = getNC( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* consistency check */
+	if ( g_file == 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+
+	/* 1) Allocate memory (if bounds exist). */
+	real_t* g_new  = new real_t[nV];
+	real_t* lb_new = 0;
+	real_t* ub_new = 0;
+	real_t* lbA_new = 0;
+	real_t* ubA_new = 0;
+
+	if ( lb_file != 0 )
+		lb_new = new real_t[nV];
+	if ( ub_file != 0 )
+		ub_new = new real_t[nV];
+	if ( lbA_file != 0 )
+		lbA_new = new real_t[nC];
+	if ( ubA_file != 0 )
+		ubA_new = new real_t[nC];
+
+	/* 2) Load new QP vectors from file. */
+	returnValue returnvalue;
+	returnvalue = loadQPvectorsFromFile(	g_file,lb_file,ub_file,lbA_file,ubA_file,
+											g_new,lb_new,ub_new,lbA_new,ubA_new
+											);
+	if ( returnvalue != SUCCESSFUL_RETURN )
+	{
+		if ( ubA_file != 0 )
+			delete[] ubA_new;
+		if ( lbA_file != 0 )
+			delete[] lbA_new;
+		if ( ub_file != 0 )
+			delete[] ub_new;
+		if ( lb_file != 0 )
+			delete[] lb_new;
+		delete[] g_new;
+
+		return THROWERROR( RET_UNABLE_TO_READ_FILE );
+	}
+
+	/* 3) Actually perform hotstart. */
+	returnvalue = hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
+
+	/* 4) Free memory. */
+	if ( ubA_file != 0 )
+		delete[] ubA_new;
+	if ( lbA_file != 0 )
+		delete[] lbA_new;
+	if ( ub_file != 0 )
+		delete[] ub_new;
+	if ( lb_file != 0 )
+		delete[] lb_new;
+	delete[] g_new;
+
+	return returnvalue;
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+returnValue QProblem::hotstart(	const real_t* const g_new,
+								const real_t* const lb_new, const real_t* const ub_new,
+								const real_t* const lbA_new, const real_t* const ubA_new,
+								int& nWSR, real_t* const cputime,
+								const Bounds* const guessedBounds, const Constraints* const guessedConstraints
+								)
+{
+	int nV = getNV( );
+	int nC = getNC( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+
+	/* start runtime measurement */
+	real_t starttime = 0.0;
+	if ( cputime != 0 )
+		starttime = getCPUtime( );
+
+
+	/* 1) Update working sets according to guesses for working sets of bounds and constraints. */
+	if ( ( guessedBounds != 0 ) && ( guessedConstraints != 0 ) )
+	{
+		if ( setupAuxiliaryQP( guessedBounds,guessedConstraints ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+	}
+
+	if ( ( guessedBounds == 0 ) && ( guessedConstraints != 0 ) )
+	{
+		/* create empty bounds for setting up auxiliary QP */
+		Bounds emptyBounds( nV );
+		if ( emptyBounds.setupAllFree( ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+		if ( setupAuxiliaryQP( &emptyBounds,guessedConstraints ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+	}
+
+	if ( ( guessedBounds != 0 ) && ( guessedConstraints == 0 ) )
+	{
+		/* create empty constraints for setting up auxiliary QP */
+		Constraints emptyConstraints( nC );
+		if ( emptyConstraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+		if ( setupAuxiliaryQP( guessedBounds,&emptyConstraints ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+	}
+
+	if ( ( guessedBounds == 0 ) && ( guessedConstraints == 0 ) )
+	{
+		/* create empty bounds and constraints for setting up auxiliary QP */
+		Bounds emptyBounds( nV );
+		Constraints emptyConstraints( nC );
+		if ( emptyBounds.setupAllFree( ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+		if ( emptyConstraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+		if ( setupAuxiliaryQP( &emptyBounds,&emptyConstraints ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+	}
+
+	status = QPS_AUXILIARYQPSOLVED;
+
+
+	/* 2) Perform usual homotopy. */
+
+	/* Allow only remaining CPU time for usual hotstart. */
+	if ( cputime != 0 )
+		*cputime -= getCPUtime( ) - starttime;
+
+	returnValue returnvalue = hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
+
+	/* stop runtime measurement */
+	if ( cputime != 0 )
+		*cputime = getCPUtime( ) - starttime;
+
+	return returnvalue;
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+returnValue QProblem::hotstart(	const char* const g_file,
+								const char* const lb_file, const char* const ub_file,
+								const char* const lbA_file, const char* const ubA_file,
+								int& nWSR, real_t* const cputime,
+								const Bounds* const guessedBounds, const Constraints* const guessedConstraints
+								)
+{
+	int nV = getNV( );
+	int nC = getNC( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* consistency check */
+	if ( g_file == 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* 1) Allocate memory (if bounds exist). */
+	real_t* g_new   = new real_t[nV];
+	real_t* lb_new  = ( lb_file != 0 ) ? new real_t[nV] : 0;
+	real_t* ub_new  = ( ub_file != 0 ) ? new real_t[nV] : 0;
+	real_t* lbA_new = ( lbA_file != 0 ) ? new real_t[nC] : 0;
+	real_t* ubA_new = ( ubA_file != 0 ) ? new real_t[nC] : 0;
+
+	/* 2) Load new QP vectors from file. */
+	returnValue returnvalue;
+	returnvalue = loadQPvectorsFromFile(	g_file,lb_file,ub_file,lbA_file,ubA_file,
+											g_new,lb_new,ub_new,lbA_new,ubA_new
+											);
+	if (returnvalue != SUCCESSFUL_RETURN) {
+		returnvalue = RET_UNABLE_TO_READ_FILE;
+		goto farewell;
+	}
+
+	/* 3) Actually perform hotstart using initialised homotopy. */
+	returnvalue = hotstart(	g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime,
+							guessedBounds,guessedConstraints
+							);
+farewell:
+	/* 4) Free memory. */
+	if ( ubA_file != 0 )
+		delete[] ubA_new;
+	if ( lbA_file != 0 )
+		delete[] lbA_new;
+	if ( ub_file != 0 )
+		delete[] ub_new;
+	if ( lb_file != 0 )
+		delete[] lb_new;
+	delete[] g_new;
+
+	return ( returnvalue != SUCCESSFUL_RETURN ) ? THROWERROR ( returnvalue ) : returnvalue;
+}
+
+
+/*
+ *
+ */
+returnValue QProblem::solveCurrentEQP(	const int n_rhs,
+										const real_t* g_in,
+										const real_t* lb_in,
+										const real_t* ub_in,
+										const real_t* lbA_in,
+										const real_t* ubA_in,
+										real_t* x_out,
+										real_t* y_out
+										)
+{
+	returnValue returnvalue = SUCCESSFUL_RETURN;
+	int ii, jj;
+	int nV  = getNV( );
+	int nC  = getNC( );
+	int nFR = getNFR( );
+	int nFX = getNFX( );
+	int nAC = getNAC( );
+
+	real_t *delta_xFX = new real_t[nFX];
+	real_t *delta_xFR = new real_t[nFR];
+	real_t *delta_yAC = new real_t[nAC];
+	real_t *delta_yFX = new real_t[nFX];
+
+	/* 1) Determine index arrays. */
+	int* FR_idx;
+	int* FX_idx;
+	int* AC_idx;
+
+	bounds.getFree( )->getNumberArray( &FR_idx );
+	bounds.getFixed( )->getNumberArray( &FX_idx );
+	constraints.getActive( )->getNumberArray( &AC_idx );
+
+	for ( ii = 0 ; ii < (nV+nC)*n_rhs; ++ii )
+		y_out[ii] = 0.0;
+	
+	for ( ii = 0 ; ii < n_rhs; ++ii )
+	{
+		returnvalue = determineStepDirection(
+			g_in, lbA_in, ubA_in, lb_in, ub_in, BT_FALSE, BT_FALSE,
+			delta_xFX, delta_xFR, delta_yAC, delta_yFX );
+
+		for ( jj = 0; jj < nFX; ++jj )
+			x_out[FX_idx[jj]] = delta_xFX[jj];
+		for ( jj = 0; jj < nFR; ++jj )
+			x_out[FR_idx[jj]] = delta_xFR[jj];
+		for ( jj = 0; jj < nFX; ++jj )
+			y_out[FX_idx[jj]] = delta_yFX[jj];
+		for ( jj = 0; jj < nAC; ++jj )
+			y_out[nV+AC_idx[jj]] = delta_yAC[jj];
+
+		g_in += nV;
+		lb_in += nV;
+		ub_in += nV;
+		lbA_in += nC;
+		ubA_in += nC;
+		x_out += nV;
+		y_out += nV+nC;
+	}
+
+
+	delete[] delta_yFX;
+	delete[] delta_yAC;
+	delete[] delta_xFR;
+	delete[] delta_xFX;
+
+	return returnvalue;
+}
+
+
+
+/*
+ *	g e t W o r k i n g S e t
+ */
+returnValue QProblem::getWorkingSet( real_t* workingSet )
+{
+	int nC = this->getNC();
+	int nV = this->getNV();
+
+	/* At which limit are the bounds active? */
+	QProblemB::getWorkingSet( workingSet );
+
+	/* At which limit are the contraints active? */
+	for (int i = 0; i < nC; i++) {
+		switch (constraints.getStatus(i)) {
+		case ST_LOWER:
+			workingSet[i + nV] = -1.0;
+			break;
+		case ST_UPPER:
+			workingSet[i + nV] = +1.0;
+			break;
+		default:
+			workingSet[i + nV] =  0.0;
+			break;
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t N Z
+ */
+int QProblem::getNZ( ) const
+{
+	/* nZ = nFR - nAC */
+	return getNFR( ) - getNAC( );
+}
+
+
+/*
+ *	g e t D u a l S o l u t i o n
+ */
+returnValue QProblem::getDualSolution( real_t* const yOpt ) const
+{
+	int i;
+
+	for( i=0; i<getNV( )+getNC( ); ++i )
+		yOpt[i] = y[i];
+
+	/* return optimal dual solution vector
+	 * only if current QP has been solved */
+	if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
+		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
+		 ( getStatus( ) == QPS_SOLVED ) )
+	{
+		return SUCCESSFUL_RETURN;
+	}
+	else
+	{
+		return RET_QP_NOT_SOLVED;
+	}
+}
+
+
+
+/*
+ *	s e t C o n s t r a i n t P r o d u c t
+ */
+returnValue QProblem::setConstraintProduct( ConstraintProduct* const _constraintProduct )
+{
+	constraintProduct = _constraintProduct;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	p r i n t P r o p e r t i e s
+ */
+returnValue QProblem::printProperties( )
+{
+	#ifndef __XPCTARGET__
+	/* Do not print properties if print level is set to none! */
+	if ( options.printLevel == PL_NONE )
+		return SUCCESSFUL_RETURN;
+
+	char myPrintfString[MAX_STRING_LENGTH];
+
+	myPrintf( "\n#################   qpOASES  --  QP PROPERTIES   #################\n" );
+	myPrintf( "\n" );
+
+	/* 1) Variables properties. */
+	snprintf( myPrintfString,MAX_STRING_LENGTH,  "Number of Variables: %4.1d\n",getNV( ) );
+	myPrintf( myPrintfString );
+
+	if ( bounds.hasNoLower( ) == BT_TRUE )
+			myPrintf( "Variables are not bounded from below.\n" );
+		else
+			myPrintf( "Variables are bounded from below.\n" );
+
+	if ( bounds.hasNoUpper( ) == BT_TRUE )
+			myPrintf( "Variables are not bounded from above.\n" );
+		else
+			myPrintf( "Variables are bounded from above.\n" );
+
+	myPrintf( "\n" );
+
+
+	/* 2) Constraints properties. */
+	snprintf( myPrintfString,MAX_STRING_LENGTH,  "Total number of Constraints:      %4.1d\n",getNC( ) );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,  "Number of Equality Constraints:   %4.1d\n",getNEC( ) );
+	myPrintf( myPrintfString );
+
+	snprintf( myPrintfString,MAX_STRING_LENGTH,  "Number of Inequality Constraints: %4.1d\n",getNC( )-getNEC( ) );
+	myPrintf( myPrintfString );
+
+	if ( getNC( ) > 0 )
+	{
+		if ( constraints.hasNoLower( ) == BT_TRUE )
+				myPrintf( "Constraints are not bounded from below.\n" );
+			else
+				myPrintf( "Constraints are bounded from below.\n" );
+
+		if ( constraints.hasNoUpper( ) == BT_TRUE )
+				myPrintf( "Constraints are not bounded from above.\n" );
+			else
+				myPrintf( "Constraints are bounded from above.\n" );
+	}
+
+	myPrintf( "\n" );
+
+
+	/* 3) Further properties. */
+	switch ( hessianType )
+	{
+		case HST_ZERO:
+			myPrintf( "Hessian is zero matrix (i.e. actually an LP is solved).\n" );
+			break;
+
+		case HST_IDENTITY:
+			myPrintf( "Hessian is identity matrix.\n" );
+			break;
+
+		case HST_POSDEF:
+			myPrintf( "Hessian matrix is (strictly) positive definite.\n" );
+			break;
+
+		case HST_POSDEF_NULLSPACE:
+			myPrintf( "Hessian matrix is positive definite on null space of active constraints.\n" );
+			break;
+
+		case HST_SEMIDEF:
+			myPrintf( "Hessian matrix is positive semi-definite.\n" );
+			break;
+
+		case HST_INDEF:
+			myPrintf( "Hessian matrix is indefinite.\n" );
+			break;
+
+		default:
+			myPrintf( "Hessian matrix has unknown type.\n" );
+			break;
+	}
+
+	if ( infeasible == BT_TRUE )
+		myPrintf( "QP was found to be infeasible.\n" );
+	else
+		myPrintf( "QP seems to be feasible.\n" );
+
+	if ( unbounded == BT_TRUE )
+		myPrintf( "QP was found to be unbounded from below.\n" );
+	else
+		myPrintf( "QP seems to be bounded from below.\n" );
+
+	myPrintf( "\n" );
+
+
+	/* 4) QP object properties. */
+	switch ( status )
+	{
+		case QPS_NOTINITIALISED:
+			myPrintf( "Status of QP object: freshly instantiated or reset.\n" );
+			break;
+
+		case QPS_PREPARINGAUXILIARYQP:
+			myPrintf( "Status of QP object: an auxiliary QP is currently setup.\n" );
+			break;
+
+		case QPS_AUXILIARYQPSOLVED:
+			myPrintf( "Status of QP object: an auxilary QP was solved.\n" );
+			break;
+
+		case QPS_PERFORMINGHOMOTOPY:
+			myPrintf( "Status of QP object: a homotopy step is performed.\n" );
+			break;
+
+		case QPS_HOMOTOPYQPSOLVED:
+			myPrintf( "Status of QP object: an intermediate QP along the homotopy path was solved.\n" );
+			break;
+
+		case QPS_SOLVED:
+			myPrintf( "Status of QP object: solution of the actual QP was found.\n" );
+			break;
+	}
+
+	switch ( options.printLevel )
+	{
+		case PL_DEBUG_ITER:
+			myPrintf( "Print level of QP object is set to display a tabular output for debugging.\n" );
+			break;
+
+		case PL_TABULAR:
+			myPrintf( "Print level of QP object is set to display a tabular output.\n" );
+			break;
+
+		case PL_LOW:
+			myPrintf( "Print level of QP object is low, i.e. only error are printed.\n" );
+			break;
+
+		case PL_MEDIUM:
+			myPrintf( "Print level of QP object is medium, i.e. error and warnings are printed.\n" );
+			break;
+
+		case PL_HIGH:
+			myPrintf( "Print level of QP object is high, i.e. all available output is printed.\n" );
+			break;
+
+		default:
+			break;
+	}
+
+	myPrintf( "\n" );
+	#endif
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+/*
+ *	c l e a r
+ */
+returnValue QProblem::clear( )
+{
+	if ( ( freeConstraintMatrix == BT_TRUE ) && ( A != 0 ) )
+	{
+		delete A;
+		A = 0;
+	}
+
+	if ( lbA != 0 )
+	{
+		delete[] lbA;
+		lbA = 0;
+	}
+
+	if ( ubA != 0 )
+	{
+		delete[] ubA;
+		ubA = 0;
+	}
+
+	if ( T != 0 )
+	{
+		delete[] T;
+		T = 0;
+	}
+
+	if ( Q != 0 )
+	{
+		delete[] Q;
+		Q = 0;
+	}
+
+	if ( Ax != 0 )
+	{
+		delete[] Ax;
+		Ax = 0;
+	}
+
+	if ( Ax_l != 0 )
+	{
+		delete[] Ax_l;
+		Ax_l = 0;
+	}
+
+	if ( Ax_u != 0 )
+	{
+		delete[] Ax_u;
+		Ax_u = 0;
+	}
+
+	if ( tempA != 0 )
+	{
+		delete[] tempA;
+		tempA = 0;
+	}
+
+	if ( ZFR_delta_xFRz != 0 )
+	{
+		delete[] ZFR_delta_xFRz;
+		ZFR_delta_xFRz = 0;
+	}
+
+	if ( delta_xFRy != 0 )
+	{
+		delete[] delta_xFRy;
+		delta_xFRy = 0;
+	}
+
+	if ( delta_xFRz != 0 )
+	{
+		delete[] delta_xFRz;
+		delta_xFRz = 0;
+	}
+
+	if ( tempB != 0 )
+	{
+		delete[] tempB;
+		tempB = 0;
+	}
+
+	if ( delta_yAC_TMP != 0 )
+	{
+		delete[] delta_yAC_TMP;
+		delta_yAC_TMP = 0;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	c o p y
+ */
+returnValue QProblem::copy(	const QProblem& rhs
+							)
+{
+	unsigned int _nV = (unsigned int)rhs.getNV( );
+	unsigned int _nC = (unsigned int)rhs.getNC( );
+
+	constraints = rhs.constraints;
+
+	if ( ( freeConstraintMatrix == BT_TRUE ) && ( A != 0 ) )
+	{
+		delete A;
+		A = 0;
+	}
+
+	freeConstraintMatrix = rhs.freeConstraintMatrix;
+
+	if ( freeConstraintMatrix == BT_TRUE )
+		A = rhs.A->duplicate();
+	else
+		A = rhs.A;
+
+	if ( rhs.lbA != 0 )
+	{
+		lbA = new real_t[_nC];
+		setLBA( rhs.lbA );
+	}
+	else
+		lbA = 0;
+
+	if ( rhs.ubA != 0 )
+	{
+		ubA = new real_t[_nC];
+		setUBA( rhs.ubA );
+	}
+	else
+		ubA = 0;
+
+	if ( rhs.y != 0 )
+	{
+		delete[] y; /* y of no constraints version too short! */
+		y = new real_t[_nV+_nC];
+		memcpy( y,rhs.y,(_nV+_nC)*sizeof(real_t) );
+	}
+	else
+		y = 0;
+
+	sizeT = rhs.sizeT;
+
+	if ( rhs.T != 0 )
+	{
+		T = new real_t[sizeT*sizeT];
+		memcpy( T,rhs.T,((unsigned int)(sizeT*sizeT))*sizeof(real_t) );
+	}
+	else
+		T = 0;
+
+	if ( rhs.Q != 0 )
+	{
+		Q = new real_t[_nV*_nV];
+		memcpy( Q,rhs.Q,_nV*_nV*sizeof(real_t) );
+	}
+	else
+		Q = 0;
+
+	if ( rhs.Ax != 0 )
+	{
+		Ax = new real_t[_nC];
+		memcpy( Ax,rhs.Ax,_nC*sizeof(real_t) );
+	}
+	else
+		Ax = 0;
+
+	if ( rhs.Ax_l != 0 )
+	{
+		Ax_l = new real_t[_nC];
+		memcpy( Ax_l,rhs.Ax_l,_nC*sizeof(real_t) );
+	}
+	else
+		Ax_l = 0;
+
+	if ( rhs.Ax_u != 0 )
+	{
+		Ax_u = new real_t[_nC];
+		memcpy( Ax_u,rhs.Ax_u,_nC*sizeof(real_t) );
+	}
+	else
+		Ax_u = 0;
+
+	if ( rhs.constraintProduct != 0 )
+		constraintProduct = rhs.constraintProduct;
+	else
+		constraintProduct = 0;
+
+	tempA = new real_t[_nV];			/* nFR */
+	ZFR_delta_xFRz = new real_t[_nV];	/* nFR */
+	delta_xFRz = new real_t[_nV];		/* nZ */
+
+	if ( _nC > 0 )
+	{
+		delta_xFRy = new real_t[_nC];		/* nAC */
+		tempB = new real_t[_nC];			/* nAC */
+		delta_yAC_TMP = new real_t[_nC];   /* nAC */
+	}
+	else
+	{
+		delta_xFRy = 0;
+		tempB = 0;
+		delta_yAC_TMP = 0;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	s o l v e I n i t i a l Q P
+ */
+returnValue QProblem::solveInitialQP(	const real_t* const xOpt, const real_t* const yOpt,
+										const Bounds* const guessedBounds, const Constraints* const guessedConstraints,
+										int& nWSR, real_t* const cputime
+										)
+{
+	int i;
+
+	/* some definitions */
+	int nV = getNV( );
+	int nC = getNC( );
+
+	//writeQpDataIntoMatFile( "qpData.mat" );
+
+	/* start runtime measurement */
+	real_t starttime = 0.0;
+	if ( cputime != 0 )
+		starttime = getCPUtime( );
+
+	status = QPS_NOTINITIALISED;
+
+
+	/* I) ANALYSE QP DATA: */
+	/* 1) Check if Hessian happens to be the identity matrix. */
+	if ( determineHessianType( ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INIT_FAILED );
+
+	/* 2) Setup type of bounds and constraints (i.e. unbounded, implicitly fixed etc.). */
+	if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INIT_FAILED );
+
+	status = QPS_PREPARINGAUXILIARYQP;
+
+
+	/* II) SETUP AUXILIARY QP WITH GIVEN OPTIMAL SOLUTION: */
+	/* 1) Setup bounds and constraints data structure. */
+	if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INIT_FAILED );
+
+	if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INIT_FAILED );
+
+	/* 2) Setup optimal primal/dual solution for auxiliary QP. */
+	if ( setupAuxiliaryQPsolution( xOpt,yOpt ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INIT_FAILED );
+
+	/* 3) Obtain linear independent working set for auxiliary QP. */
+	Bounds auxiliaryBounds( nV );
+	Constraints auxiliaryConstraints( nC );
+
+	if ( obtainAuxiliaryWorkingSet(	xOpt,yOpt,guessedBounds,guessedConstraints,
+									&auxiliaryBounds,&auxiliaryConstraints ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INIT_FAILED );
+
+	/* 4) Setup working set of auxiliary QP and setup matrix factorisations. */
+	/* a) Regularise Hessian if necessary. */
+	if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_SEMIDEF ) )
+	{
+		if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_INIT_FAILED_REGULARISATION );
+	}
+
+	/* b) TQ factorisation. */
+	if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INIT_FAILED_TQ );
+
+	/* c) Working set of auxiliary QP. */
+	if ( setupAuxiliaryWorkingSet( &auxiliaryBounds,&auxiliaryConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INIT_FAILED );
+
+	haveCholesky = BT_FALSE;
+
+	/* 5) Store original QP formulation... */
+	real_t* g_original = new real_t[nV];
+	real_t* lb_original = new real_t[nV];
+	real_t* ub_original = new real_t[nV];
+	real_t* lbA_original = new real_t[nC];
+	real_t* ubA_original = new real_t[nC];
+
+	for( i=0; i<nV; ++i )
+	{
+		g_original[i] = g[i];
+		lb_original[i] = lb[i];
+		ub_original[i] = ub[i];
+	}
+
+	for( i=0; i<nC; ++i )
+	{
+		lbA_original[i] = lbA[i];
+		ubA_original[i] = ubA[i];
+	}
+
+	/* ... and setup QP data of an auxiliary QP having an optimal solution
+	 * as specified by the user (or xOpt = yOpt = 0, by default). */
+	if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
+	{
+		delete[] ubA_original; delete[] lbA_original; delete[] ub_original; delete[] lb_original; delete[] g_original;
+		return THROWERROR( RET_INIT_FAILED );
+	}
+
+	if ( setupAuxiliaryQPbounds( &auxiliaryBounds,&auxiliaryConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
+	{
+		delete[] ubA_original; delete[] lbA_original; delete[] ub_original; delete[] lb_original; delete[] g_original;
+		return THROWERROR( RET_INIT_FAILED );
+	}
+
+	status = QPS_AUXILIARYQPSOLVED;
+
+
+	if ( options.enableRamping == BT_TRUE )
+		performRamping( );
+
+
+	/* III) SOLVE ACTUAL INITIAL QP: */
+	/* Allow only remaining CPU time for usual hotstart. */
+	if ( cputime != 0 )
+		*cputime -= getCPUtime( ) - starttime;
+
+	/* Use hotstart method to find the solution of the original initial QP,... */
+	returnValue returnvalue = hotstart( g_original,lb_original,ub_original,lbA_original,ubA_original, nWSR,cputime );
+
+	/* ... deallocate memory,... */
+	delete[] ubA_original; delete[] lbA_original; delete[] ub_original; delete[] lb_original; delete[] g_original;
+
+	/* ... check for infeasibility and unboundedness... */
+	if ( isInfeasible( ) == BT_TRUE )
+		return THROWERROR( RET_INIT_FAILED_INFEASIBILITY );
+
+	if ( isUnbounded( ) == BT_TRUE )
+		return THROWERROR( RET_INIT_FAILED_UNBOUNDEDNESS );
+
+	/* ... and internal errors. */
+	if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
+		return THROWERROR( RET_INIT_FAILED_HOTSTART );
+
+
+	/* stop runtime measurement */
+	if ( cputime != 0 )
+		*cputime = getCPUtime( ) - starttime;
+
+	THROWINFO( RET_INIT_SUCCESSFUL );
+
+	return returnvalue;
+}
+
+
+/*
+ *	s o l v e Q P
+ */
+returnValue QProblem::solveQP(	const real_t* const g_new,
+								const real_t* const lb_new, const real_t* const ub_new,
+								const real_t* const lbA_new, const real_t* const ubA_new,
+								int& nWSR, real_t* const cputime, int nWSRperformed
+								)
+{
+	int iter;
+	int nV  = getNV( );
+	int nC  = getNC( );
+
+	returnValue returnvalue;
+
+	/* consistency check */
+	if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
+		 ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
+		 ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
+	{
+		return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
+	}
+
+	/* start runtime measurement */
+	real_t starttime = 0.0;
+	if ( cputime != 0 )
+		starttime = getCPUtime( );
+
+	/* I) PREPARATIONS */
+	/* 1) Allocate delta vectors of gradient and (constraints') bounds,
+	 *    index arrays and step direction arrays. */
+	real_t* delta_xFR = new real_t[nV];
+	real_t* delta_xFX = new real_t[nV];
+	real_t* delta_yAC = new real_t[nC];
+	real_t* delta_yFX = new real_t[nV];
+
+	real_t* delta_g   = new real_t[nV];
+	real_t* delta_lb  = new real_t[nV];
+	real_t* delta_ub  = new real_t[nV];
+	real_t* delta_lbA = new real_t[nC];
+	real_t* delta_ubA = new real_t[nC];
+
+	BooleanType Delta_bC_isZero, Delta_bB_isZero;
+
+	int BC_idx;
+	SubjectToStatus BC_status;
+	BooleanType BC_isBound;
+
+	real_t homotopyLength;
+
+	#ifndef __XPCTARGET__
+	char messageString[MAX_STRING_LENGTH];
+	#endif
+
+
+	/* 2) Update type of bounds and constraints, e.g.
+	 *    a former equality constraint might have become a normal one etc. */
+
+  // (ckirches) disabled this, as inactive but tight bounds may become inactive equalities
+    //            which would then never become active again!
+/*
+	if ( setupSubjectToType( lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_HOTSTART_FAILED );
+*/
+
+	/* 3) Reset status flags. */
+	infeasible = BT_FALSE;
+	unbounded  = BT_FALSE;
+
+
+	/* II) MAIN HOMOTOPY LOOP */
+	for( iter=nWSRperformed; iter<nWSR; ++iter )
+	{
+		tabularOutput.idxAddB = tabularOutput.idxRemB = tabularOutput.idxAddC = tabularOutput.idxRemC = -1;
+		tabularOutput.excAddB = tabularOutput.excRemB = tabularOutput.excAddC = tabularOutput.excRemC = 0;
+
+		if ( isCPUtimeLimitExceeded( cputime,starttime,iter-nWSRperformed ) == BT_TRUE )
+		{
+			/* If CPU time limit is exceeded, stop homotopy loop immediately!
+			* Assign number of working set recalculations (runtime measurement is stopped later). */
+			nWSR = iter;
+			break;
+		}
+
+		status = QPS_PERFORMINGHOMOTOPY;
+
+		#ifndef __XPCTARGET__
+		snprintf( messageString,MAX_STRING_LENGTH,"%d ...",iter );
+		getGlobalMessageHandler( )->throwInfo( RET_ITERATION_STARTED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+		#endif
+
+		/* 2) Determination of shift direction of the gradient and the (constraints') bounds. */
+		returnvalue = determineDataShift(	g_new,lbA_new,ubA_new,lb_new,ub_new,
+											delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,
+											Delta_bC_isZero, Delta_bB_isZero
+											);
+		if ( returnvalue != SUCCESSFUL_RETURN )
+		{
+			delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
+			delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
+
+			/* Assign number of working set recalculations and stop runtime measurement. */
+			nWSR = iter;
+			if ( cputime != 0 )
+				*cputime = getCPUtime( ) - starttime;
+
+			THROWERROR( RET_SHIFT_DETERMINATION_FAILED );
+			return returnvalue;
+		}
+
+		/* 3) Determination of step direction of X and Y. */
+		returnvalue = determineStepDirection(	delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,
+												Delta_bC_isZero, Delta_bB_isZero,
+												delta_xFX,delta_xFR,delta_yAC,delta_yFX
+												);
+		if ( returnvalue != SUCCESSFUL_RETURN )
+		{
+			delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
+			delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
+
+			/* Assign number of working set recalculations and stop runtime measurement. */
+			nWSR = iter;
+			if ( cputime != 0 )
+				*cputime = getCPUtime( ) - starttime;
+
+			THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
+			return returnvalue;
+		}
+
+		/* 4) Determination of step length TAU.
+		 *    This step along the homotopy path is also taken (without changing working set). */
+		returnvalue = performStep(	delta_g, delta_lbA,delta_ubA,delta_lb,delta_ub,
+									delta_xFX,delta_xFR,delta_yAC,delta_yFX,
+									BC_idx,BC_status,BC_isBound
+									);
+		if ( returnvalue != SUCCESSFUL_RETURN )
+		{
+			delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
+			delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
+
+			/* Assign number of working set recalculations and stop runtime measurement. */
+			nWSR = iter;
+			if ( cputime != 0 )
+				*cputime = getCPUtime( ) - starttime;
+
+			THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );
+			return returnvalue;
+		}
+
+		/* 5) Termination criterion. */
+		nV = getNV( );
+		nC = getNC( );
+
+		homotopyLength = getRelativeHomotopyLength( g_new,lb_new,ub_new,lbA_new,ubA_new );
+		if ( homotopyLength <= options.terminationTolerance )
+		{
+			status = QPS_SOLVED;
+
+			THROWINFO( RET_OPTIMAL_SOLUTION_FOUND );
+
+			if ( printIteration( iter,BC_idx,BC_status,BC_isBound,homotopyLength ) != SUCCESSFUL_RETURN )
+				THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
+
+			nWSR = iter;
+			if ( cputime != 0 )
+				*cputime = getCPUtime( ) - starttime;
+
+			delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
+			delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
+
+			return SUCCESSFUL_RETURN;
+		}
+
+		/* 6) Change active set. */
+		returnvalue = changeActiveSet( BC_idx,BC_status,BC_isBound );
+		if ( returnvalue != SUCCESSFUL_RETURN )
+		{
+			delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
+			delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
+
+			/* Assign number of working set recalculations and stop runtime measurement. */
+			nWSR = iter;
+			if ( cputime != 0 )
+				*cputime = getCPUtime( ) - starttime;
+
+			/* Checks for infeasibility... */
+			if ( isInfeasible( ) == BT_TRUE )
+			{
+				status = QPS_HOMOTOPYQPSOLVED;
+				return setInfeasibilityFlag( RET_HOTSTART_STOPPED_INFEASIBILITY );
+			}
+
+			/* ...unboundedness... */
+			if ( unbounded == BT_TRUE ) /* not necessary since objective function convex! */
+				return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );
+
+			/* ... and throw unspecific error otherwise */
+			THROWERROR( RET_HOMOTOPY_STEP_FAILED );
+			return returnvalue;
+		}
+
+		/* 6a) Possibly refactorise projected Hessian from scratch. */
+		if ( ( options.enableCholeskyRefactorisation > 0 ) && ( (iter % options.enableCholeskyRefactorisation) == 0 ) )
+		{
+			returnvalue = computeProjectedCholesky( );
+			if (returnvalue != SUCCESSFUL_RETURN)
+			{
+				delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
+				delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
+				return returnvalue;
+			}
+		}
+
+		/* 7) Output information of successful QP iteration. */
+		status = QPS_HOMOTOPYQPSOLVED;
+
+		if ( printIteration( iter,BC_idx,BC_status,BC_isBound,homotopyLength ) != SUCCESSFUL_RETURN )
+			THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
+
+		/* 8) Perform Ramping Strategy on zero homotopy step or drift correction (if desired). */
+		if (BC_status != ST_UNDEFINED)
+		{
+			if ( ( tau <= EPS ) && ( options.enableRamping == BT_TRUE ) )
+				performRamping( );
+			else
+			if ( (options.enableDriftCorrection > 0) && ((iter+1) % options.enableDriftCorrection == 0) )
+				performDriftCorrection( );  /* always returns SUCCESSFUL_RETURN */
+		}
+	}
+
+	delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
+	delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
+
+	/* stop runtime measurement */
+	if ( cputime != 0 )
+		*cputime = getCPUtime( ) - starttime;
+
+
+	/* if program gets to here, output information that QP could not be solved
+	 * within the given maximum numbers of working set changes */
+	if ( options.printLevel == PL_HIGH )
+	{
+		#ifndef __XPCTARGET__
+		snprintf( messageString,MAX_STRING_LENGTH,"(nWSR = %d)",iter );
+		return getGlobalMessageHandler( )->throwWarning( RET_MAX_NWSR_REACHED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+		#else
+		return RET_MAX_NWSR_REACHED;
+		#endif
+	}
+	else
+	{
+		return RET_MAX_NWSR_REACHED;
+	}
+}
+
+
+/*
+ *	s o l v e R e g u l a r i s e d Q P
+ */
+returnValue QProblem::solveRegularisedQP(	const real_t* const g_new,
+											const real_t* const lb_new, const real_t* const ub_new,
+											const real_t* const lbA_new, const real_t* const ubA_new,
+											int& nWSR, real_t* const cputime, int nWSRperformed
+											)
+{
+	int i, step;
+	int nV = getNV( );
+
+
+	/* Perform normal QP solution if QP has not been regularised. */
+	if ( usingRegularisation( ) == BT_FALSE )
+		return solveQP( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime,nWSRperformed );
+
+
+	/* I) SOLVE USUAL REGULARISED QP */
+	returnValue returnvalue;
+
+	int nWSR_max   = nWSR;
+	int nWSR_total = nWSRperformed;
+
+	real_t cputime_total = 0.0;
+	real_t cputime_cur   = 0.0;
+
+	if ( cputime == 0 )
+	{
+		returnvalue = solveQP( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0,nWSRperformed );
+	}
+	else
+	{
+		cputime_cur = *cputime;
+		returnvalue = solveQP( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,&cputime_cur,nWSRperformed );
+	}
+	nWSR_total     = nWSR;
+	cputime_total += cputime_cur;
+
+	/* Only continue if QP solution has been successful. */
+	if ( returnvalue != SUCCESSFUL_RETURN )
+	{
+		if ( cputime != 0 )
+			*cputime = cputime_total;
+
+		if ( returnvalue == RET_MAX_NWSR_REACHED )
+			THROWWARNING( RET_NO_REGSTEP_NWSR );
+
+		return returnvalue;
+	}
+
+
+	/* II) PERFORM SUCCESSIVE REGULARISATION STEPS */
+	real_t* gMod = new real_t[nV];
+
+	for( step=0; step<options.numRegularisationSteps; ++step )
+	{
+		/* 1) Modify gradient: gMod = g - eps*xOpt
+		 *    (assuming regularisation matrix to be regVal*Id). */
+		for( i=0; i<nV; ++i )
+			gMod[i] = g_new[i] - regVal*x[i];
+
+		/* 2) Solve regularised QP with modified gradient allowing
+		 *    only as many working set recalculations and CPU time
+		 *    as have been left from previous QP solutions. */
+		nWSR = nWSR_max;
+
+		if ( cputime == 0 )
+		{
+			returnvalue = solveQP( gMod,lb_new,ub_new,lbA_new,ubA_new, nWSR,0,nWSR_total );
+		}
+		else
+		{
+			cputime_cur = *cputime - cputime_total;
+			returnvalue = solveQP( gMod,lb_new,ub_new,lbA_new,ubA_new, nWSR,&cputime_cur,nWSR_total );
+		}
+
+		nWSR_total     = nWSR;
+		cputime_total += cputime_cur;
+
+		/* Only continue if QP solution has been successful. */
+		if ( returnvalue != SUCCESSFUL_RETURN )
+		{
+			delete[] gMod;
+
+			if ( cputime != 0 )
+				*cputime = cputime_total;
+
+			if ( returnvalue == RET_MAX_NWSR_REACHED )
+				THROWWARNING( RET_FEWER_REGSTEPS_NWSR );
+
+			return returnvalue;
+		}
+	}
+
+	delete[] gMod;
+
+	if ( cputime != 0 )
+		*cputime = cputime_total;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p S u b j e c t T o T y p e
+ */
+returnValue QProblem::setupSubjectToType( )
+{
+	return setupSubjectToType( lb,ub,lbA,ubA );
+}
+
+
+/*
+ *	s e t u p S u b j e c t T o T y p e
+ */
+returnValue QProblem::setupSubjectToType(	const real_t* const lb_new, const real_t* const ub_new,
+											const real_t* const lbA_new, const real_t* const ubA_new
+											)
+{
+	int i;
+	int nC = getNC( );
+
+
+	/* I) SETUP SUBJECTTOTYPE FOR BOUNDS */
+	if ( QProblemB::setupSubjectToType( lb_new,ub_new ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_SETUPSUBJECTTOTYPE_FAILED );
+
+
+	/* II) SETUP SUBJECTTOTYPE FOR CONSTRAINTS */
+	/* 1) Check if lower constraints' bounds are present. */
+	constraints.setNoLower( BT_TRUE );
+	if ( lbA_new != 0 )
+	{
+		for( i=0; i<nC; ++i )
+		{
+			if ( lbA_new[i] > -INFTY )
+			{
+				constraints.setNoLower( BT_FALSE );
+				break;
+			}
+		}
+	}
+
+	/* 2) Check if upper constraints' bounds are present. */
+	constraints.setNoUpper( BT_TRUE );
+	if ( ubA_new != 0 )
+	{
+		for( i=0; i<nC; ++i )
+		{
+			if ( ubA_new[i] < INFTY )
+			{
+				constraints.setNoUpper( BT_FALSE );
+				break;
+			}
+		}
+	}
+
+	/* 3) Determine implicit equality constraints and unbounded constraints. */
+	if ( ( lbA_new != 0 ) && ( ubA_new != 0 ) )
+	{
+		for( i=0; i<nC; ++i )
+		{
+			if (constraints.getType (i) == ST_DISABLED)
+				continue;
+
+			if ( ( lbA_new[i] <= -INFTY ) && ( ubA_new[i] >= INFTY )
+					&& (options.enableFarBounds == BT_FALSE))
+			{
+				constraints.setType( i,ST_UNBOUNDED );
+			}
+			else
+			{
+				if ( options.enableEqualities && lbA[i] > ubA[i] - options.boundTolerance 
+				                              && lbA_new[i] > ubA_new[i] - options.boundTolerance)
+					constraints.setType( i,ST_EQUALITY );
+				else
+					constraints.setType( i,ST_BOUNDED );
+			}
+		}
+	}
+	else
+	{
+		if ( ( lbA_new == 0 ) && ( ubA_new == 0 ) )
+		{
+			for( i=0; i<nC; ++i )
+				constraints.setType( i,ST_UNBOUNDED );
+		}
+		else
+		{
+			for( i=0; i<nC; ++i )
+				constraints.setType( i,ST_BOUNDED );
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	c o m p u t e P r o j e c t e d C h o l e s k y
+ */
+returnValue QProblem::computeProjectedCholesky( )
+{
+	int i, j;
+	int nV  = getNV( );
+	int nZ  = getNZ( );
+
+	SymSparseMat* Id;
+
+	/* Revert to unprotected Cholesky decomposition */
+	if ( getNFX() + getNAC() == 0 )
+		return QProblemB::computeCholesky( );
+
+	/* 1) Initialises R with all zeros. */
+	for( i=0; i<nV*nV; ++i )
+		R[i] = 0.0;
+
+	/* Do not do anything for empty null spaces (important for LP case, HST_ZERO !)*/
+	if ( nZ == 0 ) // nZ == nV - getNFX() - getNAC()
+		return SUCCESSFUL_RETURN;
+	
+	/* 2) Calculate Cholesky decomposition of projected Hessian Z'*H*Z. */
+	int* FR_idx;
+	bounds.getFree( )->getNumberArray( &FR_idx );
+
+	int* AC_idx;
+	constraints.getActive( )->getNumberArray( &AC_idx );
+
+	/* calculate Z'*H*Z */
+	switch ( hessianType )
+	{
+		case HST_ZERO:
+			if ( usingRegularisation() == BT_TRUE )
+			{
+				Id = createDiagSparseMat( nV, regVal );
+				Id->bilinear(bounds.getFree(), nZ, Q, nV, R, nV);
+				delete Id;
+			}
+			else
+			{
+				/* Code should not get here, as  nZ == 0  always holds for an LP (without regularisation)! */
+				if ( nZ > 0 )
+					return THROWERROR( RET_UNKNOWN_BUG );
+			}
+			break;
+			
+		case HST_IDENTITY:
+			Id = createDiagSparseMat( nV, 1.0 );
+			Id->bilinear(bounds.getFree(), nZ, Q, nV, R, nV);
+			delete Id;
+			break;
+
+		default:
+			if ( getNAC() == 0 ) {
+				/* make Z trivial */
+				for ( j=0; j < nZ; ++j ) {
+					for ( i=0; i < nV; ++i )
+						QQ(i,j) = 0.0;
+					QQ(FR_idx[j],j) = 1.0;
+				}
+				/* now Z is trivial, and so is Z'HZ */
+				int nFR = getNFR ();
+				for ( j=0; j < nFR; ++j )
+					H->getCol (FR_idx[j], bounds.getFree (), 1.0, &R[j*nV]);
+			} else {
+				/* this is expensive if Z is large! */
+				H->bilinear(bounds.getFree(), nZ, Q, nV, R, nV);
+			}
+	}
+
+	/* R'*R = Z'*H*Z */
+	long info = 0;
+	unsigned long _nZ = (unsigned long)nZ, _nV = (unsigned long)nV;
+
+	POTRF( "U", &_nZ, R, &_nV, &info );
+
+	/* <0 = invalid call, =0 ok, >0 not spd */
+	if (info > 0) {
+		if ( R[0] < 0.0 )
+		{
+			/* Cholesky decomposition has tunneled a negative
+			 * diagonal element. */ 
+			options.epsRegularisation = getMin( -R[0]+options.epsRegularisation,getSqrt(getAbs(options.epsRegularisation)) );
+		}
+
+		hessianType = HST_SEMIDEF;
+		return RET_HESSIAN_NOT_SPD;
+	}
+
+	/* zero first subdiagonal to make givens updates work */
+	for (i=0;i<nZ-1;++i)
+		RR(i+1,i) = 0.0;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p I n i t i a l C h o l e s k y
+ */
+returnValue QProblem::setupInitialCholesky( )
+{
+	returnValue returnvalueCholesky;
+	
+	/* If regularisation shall be used, always regularise at beginning 
+	 * if initial working set is not empty. */
+	if ( ( getNV() != getNFR()-getNFV() ) && ( options.enableRegularisation == BT_TRUE ) )
+		if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
+			return RET_INIT_FAILED_REGULARISATION;
+
+	/* Factorise projected Hessian 
+	 * now handles all special cases (no active bounds/constraints, no nullspace) */
+	returnvalueCholesky = computeProjectedCholesky( );
+
+	/* If Hessian is not positive definite, regularise and try again. */
+	if ( returnvalueCholesky == RET_HESSIAN_NOT_SPD )
+	{
+		if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
+			return RET_INIT_FAILED_REGULARISATION;
+
+		returnvalueCholesky = computeProjectedCholesky( );
+	}
+
+	if ( returnvalueCholesky != SUCCESSFUL_RETURN )
+		return RET_INIT_FAILED_CHOLESKY;
+
+	haveCholesky = BT_TRUE;
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p T Q f a c t o r i s a t i o n
+ */
+returnValue QProblem::setupTQfactorisation( )
+{
+	int i, ii;
+	int nV  = getNV( );
+	int nFR = getNFR( );
+
+	int* FR_idx;
+	bounds.getFree( )->getNumberArray( &FR_idx );
+
+	/* 1) Set Q to unity matrix. */
+	for( i=0; i<nV*nV; ++i )
+		Q[i] = 0.0;
+
+	for( i=0; i<nFR; ++i )
+	{
+		ii = FR_idx[i];
+		QQ(ii,i) = 1.0;
+	}
+
+ 	/* 2) Set T to zero matrix. */
+	for( i=0; i<sizeT*sizeT; ++i )
+		T[i] = 0.0;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	o b t a i n A u x i l i a r y W o r k i n g S e t
+ */
+returnValue QProblem::obtainAuxiliaryWorkingSet(	const real_t* const xOpt, const real_t* const yOpt,
+													const Bounds* const guessedBounds, const Constraints* const guessedConstraints,
+													Bounds* auxiliaryBounds, Constraints* auxiliaryConstraints
+													) const
+{
+	int i = 0;
+	int nV = getNV( );
+	int nC = getNC( );
+
+
+	/* 1) Ensure that desiredBounds is allocated (and different from guessedBounds). */
+	if ( ( auxiliaryBounds == 0 ) || ( auxiliaryBounds == guessedBounds ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	if ( ( auxiliaryConstraints == 0 ) || ( auxiliaryConstraints == guessedConstraints ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+
+	SubjectToStatus guessedStatus;
+
+	/* 2) Setup working set of bounds for auxiliary initial QP. */
+	if ( QProblemB::obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds, auxiliaryBounds ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+
+	/* 3) Setup working set of constraints for auxiliary initial QP. */
+	if ( guessedConstraints != 0 )
+	{
+		/* If an initial working set is specific, use it!
+		 * Moreover, add all equality constraints if specified. */
+		for( i=0; i<nC; ++i )
+		{
+			/* Add constraint only if it is not (going to be) disabled! */
+			guessedStatus = guessedConstraints->getStatus( i );
+
+			#ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
+			if ( constraints.getType( i ) == ST_EQUALITY )
+			{
+				if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+			}
+			else
+			#endif
+			{
+				if ( auxiliaryConstraints->setupConstraint( i,guessedStatus ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+			}
+		}
+	}
+	else	/* No initial working set specified. */
+	{
+		/* Obtain initial working set by "clipping". */
+		if ( ( xOpt != 0 ) && ( yOpt == 0 ) )
+		{
+			for( i=0; i<nC; ++i )
+			{
+				if ( Ax[i] - lbA[i] <= options.boundTolerance )
+				{
+					if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+					continue;
+				}
+
+				if ( ubA[i] - Ax_u[i] <= options.boundTolerance )
+				{
+					if ( auxiliaryConstraints->setupConstraint( i,ST_UPPER ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+					continue;
+				}
+
+				/* Moreover, add all equality constraints if specified. */
+				#ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
+				if ( constraints.getType( i ) == ST_EQUALITY )
+				{
+					if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+				}
+				else
+				#endif
+				{
+					if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+				}
+			}
+		}
+
+		/* Obtain initial working set in accordance to sign of dual solution vector. */
+		if ( ( xOpt == 0 ) && ( yOpt != 0 ) )
+		{
+			for( i=0; i<nC; ++i )
+			{
+				if ( yOpt[nV+i] > EPS )
+				{
+					if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+					continue;
+				}
+
+				if ( yOpt[nV+i] < -EPS )
+				{
+					if ( auxiliaryConstraints->setupConstraint( i,ST_UPPER ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+					continue;
+				}
+
+				/* Moreover, add all equality constraints if specified. */
+				#ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
+				if ( constraints.getType( i ) == ST_EQUALITY )
+				{
+					if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+				}
+				else
+				#endif
+				{
+					if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+				}
+			}
+		}
+
+		/* If xOpt and yOpt are null pointer and no initial working is specified,
+		 * start with empty working set (or implicitly fixed bounds and equality constraints only)
+		 * for auxiliary QP. */
+		if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
+		{
+			for( i=0; i<nC; ++i )
+			{
+				/* Only add all equality constraints if specified. */
+				#ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
+				if ( constraints.getType( i ) == ST_EQUALITY )
+				{
+					if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+				}
+				else
+				#endif
+				{
+					if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+				}
+			}
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	s e t u p A u x i l i a r y W o r k i n g S e t
+ */
+returnValue QProblem::setupAuxiliaryWorkingSet(	const Bounds* const auxiliaryBounds,
+												const Constraints* const auxiliaryConstraints,
+												BooleanType setupAfresh
+												)
+{
+	int i;
+	int nV = getNV( );
+	int nC = getNC( );
+	BooleanType WSisTrivial = BT_TRUE;
+
+	/* consistency checks */
+	if ( auxiliaryBounds != 0 )
+	{
+		for( i=0; i<nV; ++i )
+			if ( ( bounds.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryBounds->getStatus( i ) == ST_UNDEFINED ) )
+				return THROWERROR( RET_UNKNOWN_BUG );
+	}
+	else
+	{
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+	}
+
+	if ( auxiliaryConstraints != 0 )
+	{
+		for( i=0; i<nC; ++i )
+			if ( ( constraints.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryConstraints->getStatus( i ) == ST_UNDEFINED ) )
+				return THROWERROR( RET_UNKNOWN_BUG );
+	}
+	else
+	{
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+	}
+
+	/* Check for trivial working set (all and only bounds active) */
+	for (i = 0; i < nV; i++)
+		if (auxiliaryBounds->getStatus(i) == ST_INACTIVE)
+		{
+			WSisTrivial = BT_FALSE;
+			break;
+		}
+	for (i = 0; i < nC; i++)
+		// (ckirches) here we chose to ignore an invalid ST_INACTIVE on
+		//            constraints that are ST_EQUALITies or may just have become equalities
+		if ( (constraints.getType(i) == ST_EQUALITY) // NOT auxiliaryConstraints here
+			|| (auxiliaryConstraints->getStatus(i) != ST_INACTIVE) )
+		{
+			WSisTrivial = BT_FALSE;
+			break;
+		}
+
+	if (WSisTrivial == BT_TRUE)
+	{
+		for (i = 0; i < nV; i++)
+			if (bounds.getStatus(i) == ST_INACTIVE)
+				bounds.moveFreeToFixed(i, auxiliaryBounds->getStatus(i));
+
+		return SUCCESSFUL_RETURN;
+	}
+
+
+	/* I) SETUP CHOLESKY FLAG:
+	 *    Cholesky decomposition shall only be updated if working set
+	 *    shall be updated (i.e. NOT setup afresh!) */
+	BooleanType updateCholesky;
+	if ( setupAfresh == BT_TRUE )
+		updateCholesky = BT_FALSE;
+	else
+		updateCholesky = BT_TRUE;
+
+
+	BooleanType was_fulli = options.enableFullLITests;
+	real_t backupEpsLITests = options.epsLITests;
+
+	options.enableFullLITests = BT_FALSE;
+	/* options.epsLITests = 1e-1; */
+
+	/* II) REMOVE FORMERLY ACTIVE (CONSTRAINTS') BOUNDS (IF NECESSARY): */
+	if ( setupAfresh == BT_FALSE )
+	{
+		/* 1) Remove all active constraints that shall be inactive or disabled AND
+		*    all active constraints that are active at the wrong bound. */
+		for( i=0; i<nC; ++i )
+		{
+			if ( ( constraints.getStatus( i ) == ST_LOWER ) && ( auxiliaryConstraints->getStatus( i ) != ST_LOWER ) )
+				if ( removeConstraint( i,updateCholesky,BT_FALSE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
+
+			if ( ( constraints.getStatus( i ) == ST_UPPER ) && ( auxiliaryConstraints->getStatus( i ) != ST_UPPER ) )
+				if ( removeConstraint( i,updateCholesky,BT_FALSE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
+		}
+
+		/* 2) Remove all active bounds that shall be inactive AND
+		*    all active bounds that are active at the wrong bound. */
+		for( i=0; i<nV; ++i )
+		{
+			if ( ( bounds.getStatus( i ) == ST_LOWER ) && ( auxiliaryBounds->getStatus( i ) != ST_LOWER ) )
+				if ( removeBound( i,updateCholesky,BT_FALSE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
+
+			if ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) )
+				if ( removeBound( i,updateCholesky,BT_FALSE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
+		}
+	}
+
+
+	/* III) ADD NEWLY ACTIVE (CONSTRAINTS') BOUNDS: */
+
+	/* 1) Add all equality bounds. */
+	for( i=0; i<nV; ++i )
+	{
+		//if ( ( bounds.getType( i ) == ST_EQUALITY ) && ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) ) )
+
+		// (ckirches) force equalities active
+
+		if ( ( bounds.getType( i ) == ST_EQUALITY ) && ( bounds.getStatus( i ) == ST_INACTIVE ) )
+		{
+            // assert ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE );
+			/* No check for linear independence necessary. */
+			if ( addBound( i,ST_LOWER,updateCholesky ) != SUCCESSFUL_RETURN ) // was auxiliaryBounds->getStatus( i )
+				return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
+		}
+	}
+
+	/* 2) Add all equality constraints. */
+	for( i=0; i<nC; ++i )
+	{
+        //if ( ( constraints.getType( i ) == ST_EQUALITY ) && ( ( constraints.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryConstraints->getStatus( i ) != ST_INACTIVE ) ) )
+
+		// (ckirches) force equalities active
+
+		if ( ( constraints.getType( i ) == ST_EQUALITY ) && ( constraints.getStatus( i ) == ST_INACTIVE ) )
+		{
+            // assert ( auxiliaryConstraints->getStatus( i ) != ST_INACTIVE );
+			/* Add constraint only if it is linearly independent from the current working set. */
+			if ( addConstraint_checkLI( i ) == RET_LINEARLY_INDEPENDENT )
+			{
+				if ( addConstraint( i,ST_LOWER,updateCholesky ) != SUCCESSFUL_RETURN )  // was auxiliaryConstraints->getStatus( i )
+					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
+			}
+			else
+			{
+				/* Equalities are not linearly independent! */
+				constraints.setType(i, ST_BOUNDED);
+			}
+		}
+	}
+
+
+	/* 3) Add all inactive bounds that shall be active AND
+	 *    all formerly active bounds that have been active at the wrong bound. */
+	for( i=0; i<nV; ++i )
+	{
+		if ( ( bounds.getType( i ) != ST_EQUALITY ) && ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) ) )
+		{
+			/* Add bound only if it is linearly independent from the current working set. */
+			if ( addBound_checkLI( i ) == RET_LINEARLY_INDEPENDENT )
+			{
+				if ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
+			}
+		}
+	}
+
+	/* 4) Add all inactive constraints that shall be active AND
+	 *    all formerly active constraints that have been active at the wrong bound. */
+	for( i=0; i<nC; ++i )
+	{
+		if ( ( constraints.getType( i ) != ST_EQUALITY ) && ( auxiliaryConstraints->getStatus( i ) != ST_INACTIVE ) )
+		{
+			/* formerly inactive */
+			if ( constraints.getStatus( i ) == ST_INACTIVE )
+			{
+				/* Add constraint only if it is linearly independent from the current working set. */
+				if ( addConstraint_checkLI( i ) == RET_LINEARLY_INDEPENDENT )
+				{
+					if ( addConstraint( i,auxiliaryConstraints->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
+				}
+			}
+		}
+	}
+
+	options.enableFullLITests = was_fulli;
+	options.epsLITests = backupEpsLITests;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p A u x i l i a r y Q P s o l u t i o n
+ */
+returnValue QProblem::setupAuxiliaryQPsolution(	const real_t* const xOpt, const real_t* const yOpt
+												)
+{
+	int i, j;
+	int nV = getNV( );
+	int nC = getNC( );
+
+
+	/* Setup primal/dual solution vector for auxiliary initial QP:
+	 * if a null pointer is passed, a zero vector is assigned;
+	 *  old solution vector is kept if pointer to internal solution vevtor is passed. */
+	if ( xOpt != 0 )
+	{
+		if ( xOpt != x )
+			for( i=0; i<nV; ++i )
+				x[i] = xOpt[i];
+
+		A->times(1, 1.0, x, nV, 0.0, Ax, nC);
+
+		for ( j=0; j<nC; ++j )
+		{
+			Ax_l[j] = Ax[j];
+			Ax_u[j] = Ax[j];
+		}
+	}
+	else
+	{
+		for( i=0; i<nV; ++i )
+			x[i] = 0.0;
+
+		for ( j=0; j<nC; ++j )
+		{
+			Ax[j] = 0.0;
+			Ax_l[j] = 0.0;
+			Ax_u[j] = 0.0;
+		}
+	}
+
+	if ( yOpt != 0 )
+	{
+		if ( yOpt != y )
+			for( i=0; i<nV+nC; ++i )
+				y[i] = yOpt[i];
+	}
+	else
+	{
+		for( i=0; i<nV+nC; ++i )
+			y[i] = 0.0;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p A u x i l i a r y Q P g r a d i e n t
+ */
+returnValue QProblem::setupAuxiliaryQPgradient( )
+{
+	int i;
+	int nV = getNV( );
+	int nC = getNC( );
+
+
+	/* Setup gradient vector: g = -H*x + [Id A]'*[yB yC]
+	 *                          = yB - H*x + A'*yC. */
+	switch ( hessianType )
+	{
+		case HST_ZERO:
+			if ( usingRegularisation( ) == BT_FALSE )
+				for ( i=0; i<nV; ++i )
+					g[i] = y[i];
+			else
+				for ( i=0; i<nV; ++i )
+					g[i] = y[i] - regVal*x[i];
+			break;
+
+		case HST_IDENTITY:
+			for ( i=0; i<nV; ++i )
+				g[i] = y[i] - x[i];
+			break;
+
+		default:
+			/* y'*Id */
+			for ( i=0; i<nV; ++i )
+				g[i] = y[i];
+
+			/* - H*x */
+			H->times(1, -1.0, x, nV, 1.0, g, nV);
+			break;
+	}
+
+	/* + A'*yC */
+	A->transTimes(1, 1.0, y + nV, nC, 1.0, g, nV);
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	a r e B o u n d s C o n s i s t e n t
+ */
+returnValue QProblem::areBoundsConsistent(	const real_t* const lb_new, const real_t* const ub_new,
+											const real_t* const lbA_new, const real_t* const ubA_new) const
+{
+	if (QProblemB::areBoundsConsistent(lb_new, ub_new) == RET_QP_INFEASIBLE)
+		return RET_QP_INFEASIBLE;
+
+	if (lbA_new && ubA_new) {
+		for (int i = 0; i < getNC(); ++i) {
+			if (lbA_new[i] > ubA_new[i]+EPS) {
+				return RET_QP_INFEASIBLE;
+			}
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p A u x i l i a r y Q P b o u n d s
+ */
+returnValue QProblem::setupAuxiliaryQPbounds(	const Bounds* const auxiliaryBounds,
+												const Constraints* const auxiliaryConstraints,
+												BooleanType useRelaxation
+												)
+{
+	int i;
+	int nV = getNV( );
+	int nC = getNC( );
+
+
+	/* 1) Setup bound vectors. */
+	for ( i=0; i<nV; ++i )
+	{
+		switch ( bounds.getStatus( i ) )
+		{
+			case ST_INACTIVE:
+				if ( useRelaxation == BT_TRUE )
+				{
+					if ( bounds.getType( i ) == ST_EQUALITY )
+					{
+						lb[i] = x[i];
+						ub[i] = x[i];
+					}
+					else
+					{
+						/* If a bound is inactive although it was supposed to be
+						* active by the auxiliaryBounds, it could not be added
+						* due to linear dependence. Thus set it "strongly inactive". */
+						if ( auxiliaryBounds->getStatus( i ) == ST_LOWER )
+							lb[i] = x[i];
+						else
+							lb[i] = x[i] - options.boundRelaxation;
+
+						if ( auxiliaryBounds->getStatus( i ) == ST_UPPER )
+							ub[i] = x[i];
+						else
+							ub[i] = x[i] + options.boundRelaxation;
+					}
+				}
+				break;
+
+			case ST_LOWER:
+				lb[i] = x[i];
+				if ( bounds.getType( i ) == ST_EQUALITY )
+				{
+					ub[i] = x[i];
+				}
+				else
+				{
+					if ( useRelaxation == BT_TRUE )
+						ub[i] = x[i] + options.boundRelaxation;
+				}
+				break;
+
+			case ST_UPPER:
+				ub[i] = x[i];
+				if ( bounds.getType( i ) == ST_EQUALITY )
+				{
+					lb[i] = x[i];
+				}
+				else
+				{
+					if ( useRelaxation == BT_TRUE )
+						lb[i] = x[i] - options.boundRelaxation;
+				}
+				break;
+
+            case ST_DISABLED:
+                break;
+
+			default:
+				return THROWERROR( RET_UNKNOWN_BUG );
+		}
+	}
+
+	/* 2) Setup constraints vectors. */
+	for ( i=0; i<nC; ++i )
+	{
+		switch ( constraints.getStatus( i ) )
+		{
+			case ST_INACTIVE:
+				if ( useRelaxation == BT_TRUE )
+				{
+					if ( constraints.getType( i ) == ST_EQUALITY )
+					{
+						lbA[i] = Ax_l[i];
+						ubA[i] = Ax_u[i];
+					}
+					else
+					{
+						/* If a constraint is inactive although it was supposed to be
+						* active by the auxiliaryConstraints, it could not be added
+						* due to linear dependence. Thus set it "strongly inactive". */
+						if ( auxiliaryConstraints->getStatus( i ) == ST_LOWER )
+							lbA[i] = Ax_l[i];
+						else
+							lbA[i] = Ax_l[i] - options.boundRelaxation;
+
+						if ( auxiliaryConstraints->getStatus( i ) == ST_UPPER )
+							ubA[i] = Ax_u[i];
+						else
+							ubA[i] = Ax_u[i] + options.boundRelaxation;
+					}
+				}
+				break;
+
+			case ST_LOWER:
+				lbA[i] = Ax_l[i];
+				if ( constraints.getType( i ) == ST_EQUALITY )
+				{
+					ubA[i] = Ax_l[i];
+				}
+				else
+				{
+					if ( useRelaxation == BT_TRUE )
+						ubA[i] = Ax_l[i] + options.boundRelaxation;
+				}
+				break;
+
+			case ST_UPPER:
+				ubA[i] = Ax_u[i];
+				if ( constraints.getType( i ) == ST_EQUALITY )
+				{
+					lbA[i] = Ax_u[i];
+				}
+				else
+				{
+					if ( useRelaxation == BT_TRUE )
+						lbA[i] = Ax_u[i] - options.boundRelaxation;
+				}
+				break;
+
+            case ST_DISABLED:
+                break;
+
+			default:
+				return THROWERROR( RET_UNKNOWN_BUG );
+		}
+		Ax_l[i] = Ax_l[i] - lbA[i];
+		Ax_u[i] = ubA[i] - Ax_u[i];
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	a d d C o n s t r a i n t
+ */
+returnValue QProblem::addConstraint(	int number, SubjectToStatus C_status,
+										BooleanType updateCholesky,
+										BooleanType ensureLI
+										)
+{
+	int i, j, ii;
+
+	/* consistency checks */
+	if ( constraints.getStatus( number ) != ST_INACTIVE )
+		return THROWERROR( RET_CONSTRAINT_ALREADY_ACTIVE );
+
+	if ( ( constraints.getNC( ) - getNAC( ) ) == constraints.getNUC( ) )
+		return THROWERROR( RET_ALL_CONSTRAINTS_ACTIVE );
+
+	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
+		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
+		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
+		 ( getStatus( ) == QPS_SOLVED )            )
+	{
+		return THROWERROR( RET_UNKNOWN_BUG );
+	}
+
+
+	/* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,
+	 *    i.e. remove a constraint or bound if linear dependence occurs. */
+	/* check for LI only if Cholesky decomposition shall be updated! */
+	if ( updateCholesky == BT_TRUE && ensureLI == BT_TRUE )
+	{
+		returnValue ensureLIreturnvalue = addConstraint_ensureLI( number,C_status );
+
+		switch ( ensureLIreturnvalue )
+		{
+			case SUCCESSFUL_RETURN:
+				break;
+
+			case RET_LI_RESOLVED:
+				break;
+
+			case RET_ENSURELI_FAILED_NOINDEX:
+				return RET_ADDCONSTRAINT_FAILED_INFEASIBILITY;
+
+			case RET_ENSURELI_FAILED_CYCLING:
+				return RET_ADDCONSTRAINT_FAILED_INFEASIBILITY;
+
+			case RET_ENSURELI_DROPPED:
+				return SUCCESSFUL_RETURN;
+
+			default:
+				return THROWERROR( RET_ENSURELI_FAILED );
+		}
+	}
+
+	/* some definitions */
+	int nV  = getNV( );
+	int nFR = getNFR( );
+	int nAC = getNAC( );
+	int nZ  = getNZ( );
+
+	int tcol = sizeT - nAC;
+
+
+	int* FR_idx;
+	bounds.getFree( )->getNumberArray( &FR_idx );
+
+	real_t* aFR = new real_t[nFR];
+	real_t* wZ = new real_t[nZ];
+	for( i=0; i<nZ; ++i )
+		wZ[i] = 0.0;
+
+
+	/* II) ADD NEW ACTIVE CONSTRAINT TO MATRIX T: */
+	/* 1) Add row [wZ wY] = aFR'*[Z Y] to the end of T: assign aFR. */
+	A->getRow(number, bounds.getFree(), 1.0, aFR);
+
+	/* calculate wZ */
+	for( i=0; i<nFR; ++i )
+	{
+		ii = FR_idx[i];
+		for( j=0; j<nZ; ++j )
+			wZ[j] += aFR[i] * QQ(ii,j);
+	}
+
+	/* 2) Calculate wY and store it directly into T. */
+	if ( nAC > 0 )
+	{
+		for( j=0; j<nAC; ++j )
+			TT(nAC,tcol+j) = 0.0;
+		for( i=0; i<nFR; ++i )
+		{
+			ii = FR_idx[i];
+			for( j=0; j<nAC; ++j )
+				TT(nAC,tcol+j) += aFR[i] * QQ(ii,nZ+j);
+		}
+	}
+
+	delete[] aFR;
+
+
+	real_t c, s, nu;
+
+	if ( nZ > 0 )
+	{
+		/* II) RESTORE TRIANGULAR FORM OF T: */
+		/*     Use column-wise Givens rotations to restore reverse triangular form
+		*      of T, simultanenous change of Q (i.e. Z) and R. */
+		for( j=0; j<nZ-1; ++j )
+		{
+			computeGivens( wZ[j+1],wZ[j], wZ[j+1],wZ[j],c,s );
+			nu = s/(1.0+c);
+
+			for( i=0; i<nFR; ++i )
+			{
+				ii = FR_idx[i];
+				applyGivens( c,s,nu,QQ(ii,1+j),QQ(ii,j), QQ(ii,1+j),QQ(ii,j) );
+			}
+
+			if ( ( updateCholesky == BT_TRUE ) &&
+				 ( hessianType != HST_ZERO )   && ( hessianType != HST_IDENTITY ) )
+			{
+				for( i=0; i<=j+1; ++i )
+					applyGivens( c,s,nu,RR(i,1+j),RR(i,j), RR(i,1+j),RR(i,j) );
+			}
+		}
+
+		TT(nAC,tcol-1) = wZ[nZ-1];
+
+
+		if ( ( updateCholesky == BT_TRUE ) &&
+			 ( hessianType != HST_ZERO )   && ( hessianType != HST_IDENTITY ) )
+		{
+			/* III) RESTORE TRIANGULAR FORM OF R:
+			 *      Use row-wise Givens rotations to restore upper triangular form of R. */
+			for( i=0; i<nZ-1; ++i )
+			{
+				computeGivens( RR(i,i),RR(1+i,i), RR(i,i),RR(1+i,i),c,s );
+				nu = s/(1.0+c);
+
+				for( j=(1+i); j<(nZ-1); ++j ) /* last column of R is thrown away */
+					applyGivens( c,s,nu,RR(i,j),RR(1+i,j), RR(i,j),RR(1+i,j) );
+			}
+			/* last column of R is thrown away */
+			for( i=0; i<nZ; ++i )
+				RR(i,nZ-1) = 0.0;
+		}
+	}
+
+	delete[] wZ;
+
+
+	/* IV) UPDATE INDICES */
+	tabularOutput.idxAddC = number;
+	if ( constraints.moveInactiveToActive( number,C_status ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_ADDCONSTRAINT_FAILED );
+
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	a d d C o n s t r a i n t _ c h e c k L I
+ */
+returnValue QProblem::addConstraint_checkLI( int number )
+{
+	returnValue returnvalue = RET_LINEARLY_DEPENDENT;
+
+	int i, j, ii;
+	int nV  = getNV( );
+	int nFR = getNFR( );
+	int nZ  = getNZ( );
+	int nC  = getNC( );
+	int nAC = getNAC();
+	int nFX = getNFX();
+	int *FR_idx;
+
+	bounds.getFree( )->getNumberArray( &FR_idx );
+
+
+	if (options.enableFullLITests)
+	{
+		/*
+		 * expensive LI test. Backsolve with refinement using special right
+		 * hand side. This gives an estimate for what should be considered
+		 * "zero". We then check linear independence relative to this estimate.
+		 */
+
+		int *FX_idx, *AC_idx, *IAC_idx;
+
+		real_t *delta_g   = new real_t[nV];
+		real_t *delta_xFX = new real_t[nFX];
+		real_t *delta_xFR = new real_t[nFR];
+		real_t *delta_yAC = new real_t[nAC];
+		real_t *delta_yFX = new real_t[nFX];
+
+		bounds.getFixed( )->getNumberArray( &FX_idx );
+		constraints.getActive( )->getNumberArray( &AC_idx );
+		constraints.getInactive( )->getNumberArray( &IAC_idx );
+
+		int dim = (nC>nV)?nC:nV;
+		real_t *nul = new real_t[dim];
+		for (ii = 0; ii < dim; ++ii)
+			nul[ii]=0.0;
+
+		A->getRow (number, 0, 1.0, delta_g);
+
+		returnvalue = determineStepDirection ( delta_g,
+											  nul, nul, nul, nul,
+											  BT_FALSE, BT_FALSE,
+											  delta_xFX, delta_xFR, delta_yAC, delta_yFX);
+		delete[] nul;
+
+		/* compute the weight in inf-norm */
+		real_t weight = 0.0;
+		for (ii = 0; ii < nAC; ++ii)
+		{
+			real_t a = getAbs (delta_yAC[ii]);
+			if (weight < a) weight = a;
+		}
+		for (ii = 0; ii < nFX; ++ii)
+		{
+			real_t a = getAbs (delta_yFX[ii]);
+			if (weight < a) weight = a;
+		}
+
+		/* look at the "zero" in a relative inf-norm */
+		real_t zero = 0.0;
+		for (ii = 0; ii < nFX; ++ii)
+		{
+			real_t a = getAbs (delta_xFX[ii]);
+			if (zero < a) zero = a;
+		}
+		for (ii = 0; ii < nFR; ++ii)
+		{
+			real_t a = getAbs (delta_xFR[ii]);
+			if (zero < a) zero = a;
+		}
+
+		/* relative test against zero in inf-norm */
+		if (zero > options.epsLITests * weight)
+			returnvalue = RET_LINEARLY_INDEPENDENT;
+
+		delete[] delta_yFX;
+		delete[] delta_yAC;
+		delete[] delta_xFR;
+		delete[] delta_xFX;
+		delete[] delta_g;
+
+	}
+	else
+	{
+		/*
+		 * cheap LI test for constraint. Check if constraint <number> is
+		 * linearly independent from the the active ones (<=> is element of null
+		 * space of Afr).
+		 */
+
+		real_t *Arow = new real_t[nFR];
+		A->getRow(number, bounds.getFree(), 1.0, Arow);
+
+		real_t sum, l2;
+
+		l2  = 0.0;
+		for (i = 0; i < nFR; i++)
+			l2  += Arow[i]*Arow[i];
+
+		for( j=0; j<nZ; ++j )
+		{
+			sum = 0.0;
+			for( i=0; i<nFR; ++i )
+			{
+				ii = FR_idx[i];
+				sum += Arow[i] * QQ(ii,j);
+			}
+
+			if ( getAbs( sum ) > options.epsLITests*l2 )
+			{
+				/*fprintf(stdFile, "LI test: |sum| = %9.2e, l2 = %9.2e, var = %d\n", getAbs(sum), l2, jj+1); */
+				returnvalue = RET_LINEARLY_INDEPENDENT;
+				break;
+			}
+		}
+
+		delete[] Arow;
+	}
+
+	return THROWINFO( returnvalue );
+}
+
+
+/*
+ *	a d d C o n s t r a i n t _ e n s u r e L I
+ */
+returnValue QProblem::addConstraint_ensureLI( int number, SubjectToStatus C_status )
+{
+	int i, j, ii, jj;
+	int nV  = getNV( );
+	int nFR = getNFR( );
+	int nFX = getNFX( );
+	int nAC = getNAC( );
+	int nZ  = getNZ( );
+
+
+	/* I) Check if new constraint is linearly independent from the active ones. */
+	returnValue returnvalueCheckLI = addConstraint_checkLI( number );
+
+	if ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED )
+		return THROWERROR( RET_ENSURELI_FAILED );
+
+	if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
+		return SUCCESSFUL_RETURN;
+
+
+ 	/* II) NEW CONSTRAINT IS LINEARLY DEPENDENT: */
+	/* 1) Determine coefficients of linear combination,
+	 *    cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter:
+	 *    An Algorithm for the Solution of the Parametric Quadratic Programming
+	 *    Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */
+	int* FR_idx;
+	bounds.getFree( )->getNumberArray( &FR_idx );
+
+	int* FX_idx;
+	bounds.getFixed( )->getNumberArray( &FX_idx );
+
+	real_t* xiC = new real_t[nAC];
+	real_t* xiC_TMP = new real_t[nAC];
+	real_t* xiB = new real_t[nFX];
+	real_t* Arow = new real_t[nFR];
+	real_t* num = new real_t[nV];
+
+	returnValue returnvalue = SUCCESSFUL_RETURN;
+
+	real_t y_min = options.maxDualJump;
+	int y_min_number = -1;
+	int y_min_number_bound = -1;
+	BooleanType y_min_isBound = BT_FALSE;
+
+	A->getRow(number, bounds.getFree(), C_status == ST_LOWER ? 1.0 : -1.0, Arow);
+
+	/* 2) Calculate xiC */
+	if ( nAC > 0 )
+	{
+		for( i=0; i<nAC; ++i )
+		{
+			xiC_TMP[i] = 0.0;
+			for( j=0; j<nFR; ++j )
+			{
+				jj = FR_idx[j];
+				xiC_TMP[i] += QQ(jj,nZ+i) * Arow[j];
+			}
+		}
+
+		if ( backsolveT( xiC_TMP, BT_TRUE, xiC ) != SUCCESSFUL_RETURN )
+		{
+			returnvalue = RET_ENSURELI_FAILED_TQ;
+			goto farewell;
+		}
+	}
+
+	/* 3) Calculate xiB. */
+	int* AC_idx;
+	constraints.getActive( )->getNumberArray( &AC_idx );
+
+	A->getRow(number, bounds.getFixed(), C_status == ST_LOWER ? 1.0 : -1.0, xiB);
+	A->transTimes(constraints.getActive(), bounds.getFixed(), 1, -1.0, xiC, nAC, 1.0, xiB, nFX);
+
+	/* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */
+
+	/* 1) Constraints. */
+	for( i=0; i<nAC; ++i )
+	{
+		ii = AC_idx[i];
+		num[i] = y[nV+ii];
+	}
+
+	performRatioTest (nAC, AC_idx, &constraints, num, xiC, options.epsNum, options.epsDen, y_min, y_min_number);
+
+	/* 2) Bounds. */
+	for( i=0; i<nFX; ++i )
+	{
+		ii = FX_idx[i];
+		num[i] = y[ii];
+	}
+
+	performRatioTest (nFX, FX_idx, &bounds, num, xiB, options.epsNum, options.epsDen, y_min, y_min_number_bound);
+
+	if ( y_min_number_bound >= 0 )
+	{
+		y_min_number = y_min_number_bound;
+		y_min_isBound = BT_TRUE;
+	}
+
+	#ifndef __XPCTARGET__
+	/* setup output preferences */
+	char messageString[MAX_STRING_LENGTH];
+	#endif
+
+	/* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
+	if ( y_min_number >= 0 )
+	{
+		/* Update Lagrange multiplier... */
+		for( i=0; i<nAC; ++i )
+		{
+			ii = AC_idx[i];
+			y[nV+ii] -= y_min * xiC[i];
+		}
+		for( i=0; i<nFX; ++i )
+		{
+			ii = FX_idx[i];
+			y[ii] -= y_min * xiB[i];
+		}
+
+		/* ... also for newly active constraint... */
+		if ( C_status == ST_LOWER )
+			y[nV+number] = y_min;
+		else
+			y[nV+number] = -y_min;
+
+		/* ... and for constraint to be removed. */
+		if ( y_min_isBound == BT_TRUE )
+		{
+			#ifndef __XPCTARGET__
+			snprintf( messageString,MAX_STRING_LENGTH,"bound no. %d.",y_min_number );
+			getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+			#endif
+
+			if ( removeBound( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
+			{
+				returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
+				goto farewell;
+			}
+			tabularOutput.excRemB = 1;
+
+			y[y_min_number] = 0.0;
+		}
+		else
+		{
+			#ifndef __XPCTARGET__
+			snprintf( messageString,MAX_STRING_LENGTH,"constraint no. %d.",y_min_number );
+			getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+			#endif
+
+			if ( removeConstraint( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
+			{
+				returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
+				goto farewell;
+			}
+			tabularOutput.excRemC = 1;
+
+			y[nV+y_min_number] = 0.0;
+		}
+	}
+	else
+	{
+		if (options.enableDropInfeasibles == BT_TRUE) {
+			/* dropping of infeasible constraints according to drop priorities */
+			returnvalue = dropInfeasibles (number, C_status, BT_FALSE, xiB, xiC);
+		}
+		else
+		{
+			/* no constraint/bound can be removed => QP is infeasible! */
+			returnvalue = RET_ENSURELI_FAILED_NOINDEX;
+			setInfeasibilityFlag( returnvalue );
+		}
+	}
+
+farewell:
+	delete[] num;
+	delete[] Arow;
+	delete[] xiB;
+	delete[] xiC_TMP;
+	delete[] xiC;
+
+	getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+
+	return ( (returnvalue != SUCCESSFUL_RETURN) && (returnvalue != RET_ENSURELI_FAILED_NOINDEX ) ) ? THROWERROR (returnvalue) : returnvalue;
+}
+
+
+
+/*
+ *	a d d B o u n d
+ */
+returnValue QProblem::addBound(	int number, SubjectToStatus B_status,
+								BooleanType updateCholesky,
+								BooleanType ensureLI
+								)
+{
+	int i, j, ii;
+
+	/* consistency checks */
+	if ( bounds.getStatus( number ) != ST_INACTIVE )
+		return THROWERROR( RET_BOUND_ALREADY_ACTIVE );
+
+	if ( getNFR( ) == bounds.getNUV( ) )
+		return THROWERROR( RET_ALL_BOUNDS_ACTIVE );
+
+	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
+		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
+		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
+ 		 ( getStatus( ) == QPS_SOLVED )            )
+	{
+		return THROWERROR( RET_UNKNOWN_BUG );
+	}
+
+
+	/* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,
+	 *    i.e. remove a constraint or bound if linear dependence occurs. */
+	/* check for LI only if Cholesky decomposition shall be updated! */
+	if ( updateCholesky == BT_TRUE && ensureLI == BT_TRUE )
+	{
+		returnValue ensureLIreturnvalue = addBound_ensureLI( number,B_status );
+
+		switch ( ensureLIreturnvalue )
+		{
+			case SUCCESSFUL_RETURN:
+				break;
+
+			case RET_LI_RESOLVED:
+				break;
+
+			case RET_ENSURELI_FAILED_NOINDEX:
+				return RET_ADDBOUND_FAILED_INFEASIBILITY;
+
+			case RET_ENSURELI_FAILED_CYCLING:
+				return RET_ADDBOUND_FAILED_INFEASIBILITY;
+
+			case RET_ENSURELI_DROPPED:
+				return SUCCESSFUL_RETURN;
+
+			default:
+				return THROWERROR( RET_ENSURELI_FAILED );
+		}
+	}
+
+
+	/* some definitions */
+	int nV  = getNV( );
+	int nFR = getNFR( );
+	int nAC = getNAC( );
+	int nZ  = getNZ( );
+
+	int tcol = sizeT - nAC;
+
+
+	/* II) SWAP INDEXLIST OF FREE VARIABLES:
+	 *     move the variable to be fixed to the end of the list of free variables. */
+	int lastfreenumber = bounds.getFree( )->getLastNumber( );
+	if ( lastfreenumber != number )
+		if ( bounds.swapFree( number,lastfreenumber ) != SUCCESSFUL_RETURN )
+			THROWERROR( RET_ADDBOUND_FAILED );
+
+
+	int* FR_idx;
+	bounds.getFree( )->getNumberArray( &FR_idx );
+
+	real_t* w = new real_t[nFR];
+
+
+	/* III) ADD NEW ACTIVE BOUND TO TOP OF MATRIX T: */
+	/* 1) add row [wZ wY] = [Z Y](number) at the top of T: assign w */
+	for( i=0; i<nFR; ++i )
+		w[i] = QQ(FR_idx[nFR-1],i);
+
+
+	/* 2) Use column-wise Givens rotations to restore reverse triangular form
+	 *    of the first row of T, simultanenous change of Q (i.e. Z) and R. */
+	real_t c, s, nu;
+
+	for( j=0; j<nZ-1; ++j )
+	{
+		computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );
+		nu = s/(1.0+c);
+
+		for( i=0; i<nFR; ++i )
+		{
+			ii = FR_idx[i];
+			applyGivens( c,s,nu,QQ(ii,1+j),QQ(ii,j), QQ(ii,1+j),QQ(ii,j) );
+		}
+
+		if ( ( updateCholesky == BT_TRUE ) &&
+			 ( hessianType != HST_ZERO )   && ( hessianType != HST_IDENTITY ) )
+		{
+			for( i=0; i<=j+1; ++i )
+				applyGivens( c,s,nu,RR(i,1+j),RR(i,j), RR(i,1+j),RR(i,j) );
+		}
+	}
+
+
+	if ( nAC > 0 )	  /* ( nAC == 0 ) <=> ( nZ == nFR ) <=> Y and T are empty => nothing to do */
+	{
+		/* store new column a in a temporary vector instead of shifting T one column to the left */
+		real_t* tmp = new real_t[nAC];
+		for( i=0; i<nAC; ++i )
+			tmp[i] = 0.0;
+
+		{
+			j = nZ-1;
+
+			computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );
+			nu = s/(1.0+c);
+
+			for( i=0; i<nFR; ++i )
+			{
+				ii = FR_idx[i];
+				applyGivens( c,s,nu,QQ(ii,1+j),QQ(ii,j), QQ(ii,1+j),QQ(ii,j) );
+			}
+
+			applyGivens( c,s,nu,TT(nAC-1,tcol),tmp[nAC-1], tmp[nAC-1],TT(nAC-1,tcol) );
+		}
+
+		for( j=nZ; j<nFR-1; ++j )
+		{
+			computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );
+			nu = s/(1.0+c);
+
+			for( i=0; i<nFR; ++i )
+			{
+				ii = FR_idx[i];
+				applyGivens( c,s,nu,QQ(ii,1+j),QQ(ii,j), QQ(ii,1+j),QQ(ii,j) );
+			}
+
+			for( i=(nFR-2-j); i<nAC; ++i )
+				applyGivens( c,s,nu,TT(i,1+tcol-nZ+j),tmp[i], tmp[i],TT(i,1+tcol-nZ+j) );
+		}
+
+		delete[] tmp;
+	}
+
+	delete[] w;
+
+
+	if ( ( updateCholesky == BT_TRUE ) &&
+		 ( hessianType != HST_ZERO )   && ( hessianType != HST_IDENTITY ) )
+	{
+		/* IV) RESTORE TRIANGULAR FORM OF R:
+		 *     use row-wise Givens rotations to restore upper triangular form of R */
+		for( i=0; i<nZ-1; ++i )
+		{
+			computeGivens( RR(i,i),RR(1+i,i), RR(i,i),RR(1+i,i),c,s );
+			nu = s/(1.0+c);
+
+			for( j=(1+i); j<nZ-1; ++j ) /* last column of R is thrown away */
+				applyGivens( c,s,nu,RR(i,j),RR(1+i,j), RR(i,j),RR(1+i,j) );
+		}
+		/* last column of R is thrown away */
+		for( i=0; i<nZ; ++i )
+			RR(i,nZ-1) = 0.0;
+	}
+
+
+	/* V) UPDATE INDICES */
+	tabularOutput.idxAddB = number;
+	if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_ADDBOUND_FAILED );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	a d d B o u n d _ c h e c k L I
+ */
+returnValue QProblem::addBound_checkLI( int number )
+{
+	int i, ii;
+	int nV  = getNV( );  /* for QQ() macro */
+	int nFR = getNFR( );
+	int nAC = getNAC();
+	int nFX = getNFX();
+	int nC  = getNC( );
+	returnValue returnvalue = RET_LINEARLY_DEPENDENT;
+
+	if (options.enableFullLITests)
+	{
+		/*
+		 * expensive LI test. Backsolve with refinement using special right
+		 * hand side. This gives an estimate for what should be considered
+		 * "zero". We then check linear independence relative to this estimate.
+		 */
+
+		/*
+		 * expensive LI test. Backsolve with refinement using special right
+		 * hand side. This gives an estimate for what should be considered
+		 * "zero". We then check linear independence relative to this estimate.
+		 */
+
+		real_t *delta_g   = new real_t[nV];
+		real_t *delta_xFX = new real_t[nFX];
+		real_t *delta_xFR = new real_t[nFR];
+		real_t *delta_yAC = new real_t[nAC];
+		real_t *delta_yFX = new real_t[nFX];
+
+		for (ii = 0; ii < nV; ++ii)
+			delta_g[ii] = 0.0;
+		delta_g[number] = 1.0;	/* sign doesn't matter here */
+
+		int dim = (nC>nV)?nC:nV;
+		real_t *nul = new real_t[dim];
+		for (ii = 0; ii < dim; ++ii)
+			nul[ii]=0.0;
+
+		returnValue dsdReturnValue = determineStepDirection (
+				delta_g, nul, nul, nul, nul, BT_FALSE, BT_FALSE,
+				delta_xFX, delta_xFR, delta_yAC, delta_yFX);
+		if (dsdReturnValue != SUCCESSFUL_RETURN)
+			returnvalue = dsdReturnValue;
+
+		/* compute the weight in inf-norm */
+		real_t weight = 0.0;
+		for (ii = 0; ii < nAC; ++ii)
+		{
+			real_t a = getAbs (delta_yAC[ii]);
+			if (weight < a) weight = a;
+		}
+		for (ii = 0; ii < nFX; ++ii)
+		{
+			real_t a = getAbs (delta_yFX[ii]);
+			if (weight < a) weight = a;
+		}
+
+		/* look at the "zero" in a relative inf-norm */
+		real_t zero = 0.0;
+		for (ii = 0; ii < nFX; ++ii)
+		{
+			real_t a = getAbs (delta_xFX[ii]);
+			if (zero < a) zero = a;
+		}
+		for (ii = 0; ii < nFR; ++ii)
+		{
+			real_t a = getAbs (delta_xFR[ii]);
+			if (zero < a) zero = a;
+		}
+
+		/* relative test against zero in inf-norm */
+		if (zero > options.epsLITests * weight)
+			returnvalue = RET_LINEARLY_INDEPENDENT;
+
+		delete[] nul;
+		delete[] delta_yFX;
+		delete[] delta_yAC;
+		delete[] delta_xFR;
+		delete[] delta_xFX;
+		delete[] delta_g;
+
+	}
+	else
+	{
+		/*
+		 * cheap LI test for simple bound. Check if constraint <number> is
+		 * linearly independent from the the active ones (<=> is element of null
+		 * space of Afr).
+		 */
+
+		/* some definitions */
+		int nZ  = getNZ( );
+
+		for( i=0; i<nZ; ++i )
+			if ( getAbs( QQ(number,i) ) > options.epsLITests )
+			{
+				returnvalue = RET_LINEARLY_INDEPENDENT;
+				break;
+			}
+	}
+
+	return THROWINFO( returnvalue );
+}
+
+
+/*
+ *	a d d B o u n d _ e n s u r e L I
+ */
+returnValue QProblem::addBound_ensureLI( int number, SubjectToStatus B_status )
+{
+	int i, ii;
+	int nV  = getNV( );
+	int nFX = getNFX( );
+	int nAC = getNAC( );
+	int nZ  = getNZ( );
+
+
+	/* I) Check if new constraint is linearly independent from the active ones. */
+	returnValue returnvalueCheckLI = addBound_checkLI( number );
+
+	if ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED )
+		return THROWERROR( RET_ENSURELI_FAILED );
+
+	if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
+		return SUCCESSFUL_RETURN;
+
+
+ 	/* II) NEW BOUND IS LINEARLY DEPENDENT: */
+	/* 1) Determine coefficients of linear combination,
+	 *    cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter:
+	 *    An Algorithm for the Solution of the Parametric Quadratic Programming
+	 *    Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */
+	int* FR_idx;
+	bounds.getFree( )->getNumberArray( &FR_idx );
+
+	int* FX_idx;
+	bounds.getFixed( )->getNumberArray( &FX_idx );
+
+	int* AC_idx;
+	constraints.getActive( )->getNumberArray( &AC_idx );
+
+	real_t* xiC = new real_t[nAC];
+	real_t* xiC_TMP = new real_t[nAC];
+	real_t* xiB = new real_t[nFX];
+	real_t* num = new real_t[nV];
+
+	real_t y_min = options.maxDualJump;
+	int y_min_number = -1;
+	int y_min_number_bound = -1;
+	BooleanType y_min_isBound = BT_FALSE;
+
+	returnValue returnvalue = SUCCESSFUL_RETURN;
+
+
+	/* 2) Calculate xiC. */
+	if ( nAC > 0 )
+	{
+		if ( B_status == ST_LOWER )
+		{
+			for( i=0; i<nAC; ++i )
+				xiC_TMP[i] = QQ(number,nZ+i);
+		}
+		else
+		{
+			for( i=0; i<nAC; ++i )
+				xiC_TMP[i] = -QQ(number,nZ+i);
+		}
+
+		if ( backsolveT( xiC_TMP, BT_TRUE, xiC ) != SUCCESSFUL_RETURN )
+		{
+			returnvalue = RET_ENSURELI_FAILED_TQ;
+			goto farewell;
+		}
+	}
+
+	/* 3) Calculate xiB. */
+	A->transTimes(constraints.getActive(), bounds.getFixed(), 1, -1.0, xiC, nAC, 0.0, xiB, nFX);
+
+
+	/* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */
+
+	/* 1) Constraints. */
+	for( i=0; i<nAC; ++i )
+	{
+		ii = AC_idx[i];
+		num[i] = y[nV+ii];
+	}
+
+	performRatioTest( nAC,AC_idx,&constraints, num,xiC, options.epsNum,options.epsDen, y_min,y_min_number );
+
+	/* 2) Bounds. */
+	for( i=0; i<nFX; ++i )
+	{
+		ii = FX_idx[i];
+		num[i] = y[ii];
+	}
+
+	performRatioTest( nFX,FX_idx,&bounds, num,xiB, options.epsNum,options.epsDen, y_min,y_min_number_bound );
+
+	if ( y_min_number_bound >= 0 )
+	{
+		y_min_number = y_min_number_bound;
+		y_min_isBound = BT_TRUE;
+	}
+
+	/* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
+	#ifndef __XPCTARGET__
+	char messageString[MAX_STRING_LENGTH];
+	#endif
+
+	if ( y_min_number >= 0 )
+	{
+		/* Update Lagrange multiplier... */
+		for( i=0; i<nAC; ++i )
+		{
+			ii = AC_idx[i];
+			y[nV+ii] -= y_min * xiC[i];
+		}
+		for( i=0; i<nFX; ++i )
+		{
+			ii = FX_idx[i];
+			y[ii] -= y_min * xiB[i];
+		}
+
+		/* ... also for newly active bound ... */
+		if ( B_status == ST_LOWER )
+			y[number] = y_min;
+		else
+			y[number] = -y_min;
+
+		/* ... and for bound to be removed. */
+		if ( y_min_isBound == BT_TRUE )
+		{
+			#ifndef __XPCTARGET__
+			snprintf( messageString,MAX_STRING_LENGTH,"bound no. %d.",y_min_number );
+			getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+			#endif
+
+			if ( removeBound( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
+			{
+				returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
+				goto farewell;
+			}
+			tabularOutput.excRemB = 1;
+
+			y[y_min_number] = 0.0;
+		}
+		else
+		{
+			#ifndef __XPCTARGET__
+			snprintf( messageString,MAX_STRING_LENGTH,"constraint no. %d.",y_min_number );
+			getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+			#endif
+
+			if ( removeConstraint( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
+			{
+				returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
+				goto farewell;
+			}
+			tabularOutput.excRemC = 1;
+
+			y[nV+y_min_number] = 0.0;
+		}
+	}
+	else
+	{
+		if (options.enableDropInfeasibles == BT_TRUE) {
+			/* dropping of infeasible constraints according to drop priorities */
+			returnvalue = dropInfeasibles (number, B_status, BT_TRUE, xiB, xiC);
+		}
+		else
+		{
+			/* no constraint/bound can be removed => QP is infeasible! */
+			returnvalue = RET_ENSURELI_FAILED_NOINDEX;
+			setInfeasibilityFlag( returnvalue );
+		}
+	}
+
+farewell:
+	delete[] num;
+	delete[] xiB;
+	delete[] xiC_TMP;
+	delete[] xiC;
+
+	getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+
+	return ( (returnvalue != SUCCESSFUL_RETURN) && (returnvalue != RET_ENSURELI_FAILED_NOINDEX ) ) ? THROWERROR (returnvalue) : returnvalue;
+}
+
+
+
+/*
+ *	r e m o v e C o n s t r a i n t
+ */
+returnValue QProblem::removeConstraint(	int number,
+										BooleanType updateCholesky,
+										BooleanType allowFlipping,
+										BooleanType ensureNZC
+										)
+{
+	int i, j, ii, jj;
+	returnValue returnvalue = SUCCESSFUL_RETURN;
+	BooleanType hasFlipped = BT_FALSE;
+
+	/* consistency check */
+	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
+		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
+		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
+ 		 ( getStatus( ) == QPS_SOLVED )            )
+	{
+		return THROWERROR( RET_UNKNOWN_BUG );
+	}
+
+	/* some definitions */
+	int nV  = getNV( );
+	int nFR = getNFR( );
+	int nAC = getNAC( );
+	int nZ  = getNZ( );
+
+	int tcol = sizeT - nAC;
+	int number_idx = constraints.getActive( )->getIndex( number );
+
+	int addIdx;
+	BooleanType addBoundNotConstraint;
+	SubjectToStatus addStatus;
+	BooleanType exchangeHappened = BT_FALSE;
+
+
+	/* consistency checks */
+	if ( constraints.getStatus( number ) == ST_INACTIVE )
+		return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );
+
+	if ( ( number_idx < 0 ) || ( number_idx >= nAC ) )
+		return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );
+
+
+	int* FR_idx;
+	bounds.getFree( )->getNumberArray( &FR_idx );
+
+	/* N) PERFORM ZERO CURVATURE TEST. */
+	if (ensureNZC == BT_TRUE)
+	{
+		returnvalue = ensureNonzeroCurvature(BT_FALSE, number, exchangeHappened, addBoundNotConstraint, addIdx, addStatus);
+
+		if (returnvalue != SUCCESSFUL_RETURN)
+			return returnvalue;
+	}
+
+	/* save index sets and decompositions for flipping bounds strategy */
+	if ( ( exchangeHappened == BT_FALSE ) && ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) )
+		flipper.set( &bounds,R,&constraints,Q,T );
+
+	/* I) REMOVE <number>th ROW FROM T,
+	 *    i.e. shift rows number+1 through nAC  upwards (instead of the actual
+	 *    constraint number its corresponding index within matrix A is used). */
+	if ( number_idx < nAC-1 )
+	{
+		for( i=(number_idx+1); i<nAC; ++i )
+			for( j=(nAC-i-1); j<nAC; ++j )
+				TT(i-1,tcol+j) = TT(i,tcol+j);
+		/* gimmick: write zeros into the last row of T */
+		for( j=0; j<nAC; ++j )
+			TT(nAC-1,tcol+j) = 0.0;
+
+
+		/* II) RESTORE TRIANGULAR FORM OF T,
+		 *     use column-wise Givens rotations to restore reverse triangular form
+		 *     of T simultanenous change of Q (i.e. Y). */
+		real_t c, s, nu;
+
+		for( j=(nAC-2-number_idx); j>=0; --j )
+		{
+			computeGivens( TT(nAC-2-j,tcol+1+j),TT(nAC-2-j,tcol+j), TT(nAC-2-j,tcol+1+j),TT(nAC-2-j,tcol+j),c,s );
+			nu = s/(1.0+c);
+
+			for( i=(nAC-j-1); i<(nAC-1); ++i )
+				applyGivens( c,s,nu,TT(i,tcol+1+j),TT(i,tcol+j), TT(i,tcol+1+j),TT(i,tcol+j) );
+
+			for( i=0; i<nFR; ++i )
+			{
+				ii = FR_idx[i];
+				applyGivens( c,s,nu,QQ(ii,nZ+1+j),QQ(ii,nZ+j), QQ(ii,nZ+1+j),QQ(ii,nZ+j) );
+			}
+		}
+	}
+	else
+	{
+		/* gimmick: write zeros into the last row of T */
+		for( j=0; j<nAC; ++j )
+			TT(nAC-1,tcol+j) = 0.0;
+	}
+
+
+	if ( ( updateCholesky == BT_TRUE ) &&
+		 ( hessianType != HST_ZERO )   && ( hessianType != HST_IDENTITY ) )
+	{
+		/* III) UPDATE CHOLESKY DECOMPOSITION,
+		 *      calculate new additional column (i.e. [r sqrt(rho2)]')
+		 *      of the Cholesky factor R. */
+		real_t* Hz = new real_t[nFR];
+		real_t* z = new real_t[nFR];
+		real_t rho2 = 0.0;
+
+		/* 1) Calculate Hz = H*z, where z is the new rightmost column of Z
+		 *    (i.e. the old leftmost column of Y).  */
+		for( j=0; j<nFR; ++j )
+			z[j] = QQ(FR_idx[j],nZ);
+		H->times(bounds.getFree(), bounds.getFree(), 1, 1.0, z, nFR, 0.0, Hz, nFR);
+		delete[] z;
+
+		if ( nZ > 0 )
+		{
+			real_t* ZHz = new real_t[nZ];
+			for ( i=0; i<nZ; ++i )
+				ZHz[i] = 0.0;
+			real_t* r = new real_t[nZ];
+
+			/* 2) Calculate ZHz = Z'*Hz (old Z). */
+			for( j=0; j<nFR; ++j )
+			{
+				jj = FR_idx[j];
+
+				for( i=0; i<nZ; ++i )
+					ZHz[i] += QQ(jj,i) * Hz[j];
+			}
+
+			/* 3) Calculate r = R^-T * ZHz. */
+			if ( backsolveR( ZHz,BT_TRUE,r ) != SUCCESSFUL_RETURN )
+			{
+				delete[] Hz; delete[] r; delete[] ZHz;
+				return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
+			}
+
+			/* 4) Calculate rho2 = rho^2 = z'*Hz - r'*r
+			 *    and store r into R. */
+			for( i=0; i<nZ; ++i )
+			{
+				rho2 -= r[i]*r[i];
+				RR(i,nZ) = r[i];
+			}
+
+			delete[] r; delete[] ZHz;
+		}
+
+		/* 5) Store rho into R. */
+		for( j=0; j<nFR; ++j )
+			rho2 += QQ(FR_idx[j],nZ) * Hz[j];
+
+		delete[] Hz;
+
+		if ( ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
+		{
+			if ( rho2 > options.epsFlipping )
+				RR(nZ,nZ) = getSqrt( rho2 );
+			else
+			{
+				hessianType = HST_SEMIDEF;
+
+				flipper.get( &bounds,R,&constraints,Q,T );
+				constraints.flipFixed(number);
+				tabularOutput.idxAddC = number;
+				tabularOutput.excAddC = 2;
+
+				switch (constraints.getStatus(number))
+				{
+					case ST_LOWER:
+						lbA[number] = ubA[number]; Ax_l[number] = -Ax_u[number]; break;
+					case ST_UPPER:
+						ubA[number] = lbA[number]; Ax_u[number] = -Ax_l[number]; break;
+					default:
+						return THROWERROR( RET_MOVING_BOUND_FAILED );
+				}
+
+				hasFlipped = BT_TRUE;
+			}
+		}
+		else if ( exchangeHappened == BT_FALSE )
+		{
+			if ( rho2 > ZERO )
+				RR(nZ,nZ) = getSqrt( rho2 );
+			else
+			{
+				if ( allowFlipping == BT_FALSE )
+				{
+					RR(nZ,nZ) = 100.0*EPS;
+				}
+				else
+				{
+					hessianType = HST_SEMIDEF;
+					return THROWERROR( RET_HESSIAN_NOT_SPD );
+				}
+			}
+		}
+	}
+
+
+	/* IV) UPDATE INDICES */
+	tabularOutput.idxRemC = number;
+	if ( hasFlipped == BT_FALSE )
+	{
+		if ( constraints.moveActiveToInactive( number ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
+	}
+
+	if (exchangeHappened == BT_TRUE)
+	{
+		/* add bound or constraint */
+
+		/* hessianType = HST_SEMIDEF; */
+		RR(nZ,nZ) = 0.0;
+
+		if ( addBoundNotConstraint )
+		{
+			addBound(addIdx, addStatus, BT_TRUE, BT_FALSE);
+			tabularOutput.excAddB = 1;
+		}
+		else
+		{
+			addConstraint(addIdx, addStatus, BT_TRUE, BT_FALSE);
+			tabularOutput.excAddC = 1;
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	r e m o v e B o u n d
+ */
+returnValue QProblem::removeBound(	int number,
+									BooleanType updateCholesky,
+									BooleanType allowFlipping,
+									BooleanType ensureNZC
+									)
+{
+	int i, j, ii, jj;
+	returnValue returnvalue = SUCCESSFUL_RETURN;
+	int addIdx;
+	BooleanType addBoundNotConstraint;
+	SubjectToStatus addStatus;
+	BooleanType exchangeHappened = BT_FALSE;
+
+
+	/* consistency checks */
+	if ( bounds.getStatus( number ) == ST_INACTIVE )
+		return THROWERROR( RET_BOUND_NOT_ACTIVE );
+
+	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
+		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
+		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
+ 		 ( getStatus( ) == QPS_SOLVED )            )
+	{
+		return THROWERROR( RET_UNKNOWN_BUG );
+	}
+
+	/* some definitions */
+	int nV  = getNV( );
+	int nFR = getNFR( );
+	int nAC = getNAC( );
+	int nZ  = getNZ( );
+
+	int tcol = sizeT - nAC;
+
+	/* 0) PERFORM ZERO CURVATURE TEST. */
+	if (ensureNZC == BT_TRUE)
+	{
+		returnvalue = ensureNonzeroCurvature(BT_TRUE, number, exchangeHappened, addBoundNotConstraint, addIdx, addStatus);
+
+		if (returnvalue != SUCCESSFUL_RETURN)
+			return returnvalue;
+	}
+
+	/* save index sets and decompositions for flipping bounds strategy */
+	if ( ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
+		flipper.set( &bounds,R,&constraints,Q,T );
+
+	/* I) UPDATE INDICES */
+	tabularOutput.idxRemB = number;
+	if ( bounds.moveFixedToFree( number ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_REMOVEBOUND_FAILED );
+
+	int* FR_idx;
+	bounds.getFree( )->getNumberArray( &FR_idx );
+
+	/* I) APPEND <nFR+1>th UNITY VECTOR TO Q. */
+	int nnFRp1 = FR_idx[nFR];
+	for( i=0; i<nFR; ++i )
+	{
+		ii = FR_idx[i];
+		QQ(ii,nFR) = 0.0;
+		QQ(nnFRp1,i) = 0.0;
+	}
+	QQ(nnFRp1,nFR) = 1.0;
+
+	if ( nAC > 0 )
+	{
+		/* store new column a in a temporary vector instead of shifting T one column to the left and appending a */
+		int* AC_idx;
+		constraints.getActive( )->getNumberArray( &AC_idx );
+
+		real_t* tmp = new real_t[nAC];
+		A->getCol(number, constraints.getActive(), 1.0, tmp);
+
+
+		/* II) RESTORE TRIANGULAR FORM OF T,
+		 *     use column-wise Givens rotations to restore reverse triangular form
+		 *     of T = [T A(:,number)], simultanenous change of Q (i.e. Y and Z). */
+		real_t c, s, nu;
+
+		for( j=(nAC-1); j>=0; --j )
+		{
+			computeGivens( tmp[nAC-1-j],TT(nAC-1-j,tcol+j),TT(nAC-1-j,tcol+j),tmp[nAC-1-j],c,s );
+			nu = s/(1.0+c);
+
+			for( i=(nAC-j); i<nAC; ++i )
+				applyGivens( c,s,nu,tmp[i],TT(i,tcol+j),TT(i,tcol+j),tmp[i] );
+
+			for( i=0; i<=nFR; ++i )
+			{
+				ii = FR_idx[i];
+				/* nZ+1+nAC = nFR+1  /  nZ+(1) = nZ+1 */
+				applyGivens( c,s,nu,QQ(ii,nZ+1+j),QQ(ii,nZ+j),QQ(ii,nZ+1+j),QQ(ii,nZ+j) );
+			}
+		}
+
+		delete[] tmp;
+	}
+
+
+	if ( ( updateCholesky == BT_TRUE ) &&
+		 /*( hessianType != HST_ZERO )   &&*/ ( hessianType != HST_IDENTITY ) )
+	{
+		/* III) UPDATE CHOLESKY DECOMPOSITION,
+		 *      calculate new additional column (i.e. [r sqrt(rho2)]')
+		 *      of the Cholesky factor R: */
+		real_t z2 = QQ(nnFRp1,nZ);
+		real_t rho2;
+
+		if ( hessianType != HST_ZERO )
+			rho2 = H->diag(nnFRp1)*z2*z2; /* rho2 = h2*z2*z2 */
+		else
+			rho2 = 0.0;
+
+		if ( nFR > 0 )
+		{
+			/* Attention: Index list of free variables has already grown by one! */
+			real_t* Hz = new real_t[nFR+1];
+			real_t* z = new real_t[nFR+1];
+			/* 1) Calculate R'*r = Zfr'*Hfr*z1 + z2*Zfr'*h1 =: Zfr'*Hz + z2*Zfr'*h1 =: rhs and
+			 *    rho2 = z1'*Hfr*z1 + 2*z2*h1'*z1 + h2*z2^2 - r'*r =: z1'*Hz + 2*z2*h1'*z1 + h2*z2^2 - r'r */
+			for( j=0; j<nFR; ++j )
+				z[j] = QQ(FR_idx[j],nZ);
+			z[nFR] = 0.0;
+			H->times(bounds.getFree(), bounds.getFree(), 1, 1.0, z, nFR+1, 0.0, Hz, nFR+1);
+
+			H->getCol(nnFRp1, bounds.getFree(), 1.0, z);
+
+			if ( nZ > 0 )
+			{
+				real_t* r = new real_t[nZ];
+				real_t* rhs = new real_t[nZ];
+				for( i=0; i<nZ; ++i )
+					rhs[i] = 0.0;
+
+				/* 2) Calculate rhs. */
+				for( j=0; j<nFR; ++j )
+				{
+					jj = FR_idx[j];
+					for( i=0; i<nZ; ++i )
+										/* Zfr' * ( Hz + z2*h1 ) */
+						rhs[i] += QQ(jj,i) * ( Hz[j] + z2 * z[j] );
+				}
+
+				/* 3) Calculate r = R^-T * rhs. */
+				if ( backsolveR( rhs,BT_TRUE,BT_TRUE,r ) != SUCCESSFUL_RETURN )
+				{
+					delete[] z;
+					delete[] Hz; delete[] r; delete[] rhs;
+					return THROWERROR( RET_REMOVEBOUND_FAILED );
+				}
+
+
+				/* 4) Calculate rho2 = rho^2 = z'*Hz - r'*r
+				 *    and store r into R. */
+				for( i=0; i<nZ; ++i )
+				{
+					rho2 -= r[i]*r[i];
+					RR(i,nZ) = r[i];
+				}
+
+				delete[] rhs; delete[] r;
+			}
+
+			for( j=0; j<nFR; ++j )
+			{
+				jj = FR_idx[j];
+							/* z1' * ( Hz + 2*z2*h1 ) */
+				rho2 += QQ(jj,nZ) * ( Hz[j] + 2.0*z2*z[j] );
+			}
+
+			delete[] z;
+			delete[] Hz;
+		}
+
+		/* 5) Store rho into R. */
+		if ( ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
+		{
+			if ( rho2 > options.epsFlipping )
+				RR(nZ,nZ) = getSqrt( rho2 );
+			else
+			{
+				if ( hessianType != HST_ZERO )
+					hessianType = HST_SEMIDEF;
+
+				flipper.get( &bounds,R,&constraints,Q,T );
+				bounds.flipFixed(number);
+				tabularOutput.idxAddB = number;
+				tabularOutput.excAddB = 2;
+
+				switch (bounds.getStatus(number))
+				{
+					case ST_LOWER: 
+						lb[number] = ub[number]; 
+						break;
+					case ST_UPPER: 
+						ub[number] = lb[number]; 
+						break;
+					default: return THROWERROR( RET_MOVING_BOUND_FAILED );
+				}
+
+			}
+		}
+		else if ( exchangeHappened == BT_FALSE )
+		{
+			if ( rho2 > ZERO )
+				RR(nZ,nZ) = getSqrt( rho2 );
+			else
+			{
+				if ( allowFlipping == BT_FALSE )
+					RR(nZ,nZ) = 100.0*EPS;
+				else
+				{
+					hessianType = HST_SEMIDEF;
+					return THROWERROR( RET_HESSIAN_NOT_SPD );
+				}
+			}
+		}
+		else
+		{
+			/* add bound or constraint */
+
+			/* hessianType = HST_SEMIDEF; */
+			RR(nZ,nZ) = 0.0;
+
+			if ( addBoundNotConstraint )
+			{
+				addBound(addIdx, addStatus, BT_TRUE, BT_FALSE);
+				tabularOutput.excAddB = 1;
+			}
+			else
+			{
+				addConstraint(addIdx, addStatus, BT_TRUE, BT_FALSE);
+				tabularOutput.excAddC = 1;
+			}
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+returnValue QProblem::performPlainRatioTest(	int nIdx,
+												const int* const idxList,
+												const real_t* const num,
+												const real_t* const den,
+												real_t epsNum,
+												real_t epsDen,
+												real_t& t,
+												int& BC_idx
+												) const
+{
+	int i;
+	for (i = 0; i < nIdx; i++)
+		if ( (num[i] > epsNum) && (den[i] > epsDen) && (t * den[i] > num[i]) )
+		{
+			t = num[i] / den[i];
+			BC_idx = idxList[i];
+		}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+returnValue QProblem::ensureNonzeroCurvature(	BooleanType removeBoundNotConstraint,
+												int remIdx,
+												BooleanType &exchangeHappened,
+												BooleanType &addBoundNotConstraint,
+												int &addIdx,
+												SubjectToStatus &addStatus
+												)
+{
+	int i, ii;
+	int addLBndIdx = -1, addLCnstrIdx = -1, addUBndIdx = -1, addUCnstrIdx = -1; /* exchange indices */
+	int *FX_idx, *AC_idx, *IAC_idx;
+	returnValue returnvalue = SUCCESSFUL_RETURN;
+
+	int nV  = getNV( );
+	int nFR = getNFR( );
+	int nAC = getNAC( );
+	int nC  = getNC( );
+	int nFX = getNFX( );
+	int nIAC = getNIAC( );
+
+	int* FR_idx;
+	bounds.getFree( )->getNumberArray( &FR_idx );
+
+// 	real_t *delta_g   = new real_t[nV];
+	real_t *delta_xFX = new real_t[nFX];
+	real_t *delta_xFR = new real_t[nFR];
+	real_t *delta_yAC = new real_t[nAC];
+	real_t *delta_yFX = new real_t[nFX];
+
+	bounds.getFixed( )->getNumberArray( &FX_idx );
+	constraints.getActive( )->getNumberArray( &AC_idx );
+	constraints.getInactive( )->getNumberArray( &IAC_idx );
+
+	addBoundNotConstraint = BT_TRUE;
+	addStatus = ST_INACTIVE;
+	exchangeHappened = BT_FALSE;
+
+	if (removeBoundNotConstraint)
+	{
+		int dim = nV < nC ? nC : nV;
+		real_t *nul = new real_t[dim];
+		real_t *ek = new real_t[nV]; /* minus e_k (bound k is removed) */
+		for (ii = 0; ii < dim; ++ii)
+			nul[ii]=0.0;
+		for (ii = 0; ii < nV; ++ii)
+			ek[ii]=0.0;
+		ek[remIdx] = bounds.getStatus(remIdx) == ST_LOWER ? 1.0 : -1.0;
+
+		returnvalue = determineStepDirection (nul, nul, nul, ek, ek,
+											  BT_FALSE, BT_FALSE,
+											  delta_xFX, delta_xFR, delta_yAC, delta_yFX);
+		delete[] ek;
+		delete[] nul;
+	}
+	else
+	{
+		real_t *nul = new real_t[nV];
+		real_t *ek = new real_t[nC]; /* minus e_k (constraint k is removed) */
+		for (ii = 0; ii < nV; ++ii)
+			nul[ii]=0.0;
+		for (ii = 0; ii < nC; ++ii)
+			ek[ii]=0.0;
+		ek[remIdx] = constraints.getStatus(remIdx) == ST_LOWER ? 1.0 : -1.0;
+
+		returnvalue = determineStepDirection (nul,
+											  ek, ek, nul, nul,
+											  BT_FALSE, BT_TRUE,
+											  delta_xFX, delta_xFR, delta_yAC, delta_yFX);
+		delete[] ek;
+		delete[] nul;
+	}
+
+	/* compute the weight in inf-norm */
+	real_t normXi = 0.0;
+	for (ii = 0; ii < nAC; ++ii)
+	{
+		real_t a = getAbs (delta_yAC[ii]);
+		if (normXi < a) normXi = a;
+	}
+	for (ii = 0; ii < nFX; ++ii)
+	{
+		real_t a = getAbs (delta_yFX[ii]);
+		if (normXi < a) normXi = a;
+	}
+
+	/* look at the "zero" in a relative inf-norm */
+	real_t normS = 0.0;
+	for (ii = 0; ii < nFX; ++ii)
+	{
+		real_t a = getAbs (delta_xFX[ii]);
+		if (normS < a) normS = a;
+	}
+	for (ii = 0; ii < nFR; ++ii)
+	{
+		real_t a = getAbs (delta_xFR[ii]);
+		if (normS < a) normS = a;
+	}
+
+	/* relative test against zero in inf-norm */
+	if (normXi < options.epsNZCTests * normS)
+	{
+		/* determine jump in x via ratio tests */
+		real_t sigmaLBnd, sigmaLCnstr, sigmaUBnd, sigmaUCnstr, sigma;
+
+		/* bounds */
+
+		/* compress x-u */
+		real_t *x_W = new real_t[getMax(1,nFR)];
+		for (i = 0; i < nFR; i++)
+		{
+			ii = FR_idx[i];
+			x_W[i] = ub[ii] - x[ii];
+		}
+		/* performRatioTest( nFR,FR_idx,&bounds, x_W,delta_xFR, options.epsNum,options.epsDen, sigmaUBnd,addUBndIdx ); */
+		sigmaUBnd = options.maxPrimalJump;
+		addUBndIdx = -1;
+		performPlainRatioTest(nFR, FR_idx, x_W, delta_xFR, options.epsNum, options.epsDen, sigmaUBnd, addUBndIdx);
+		if (removeBoundNotConstraint == BT_TRUE && bounds.getStatus(remIdx) == ST_LOWER)
+		{
+			/* also consider bound which is to be removed */
+			real_t one = 1.0;
+			x_W[0] = ub[remIdx] - x[remIdx];
+			performPlainRatioTest(1, &remIdx, x_W, &one, options.epsNum, options.epsDen, sigmaUBnd, addUBndIdx);
+		}
+
+		/* compress x-l */
+		for (i = 0; i < nFR; i++)
+		{
+			ii = FR_idx[i];
+			x_W[i] = x[ii] - lb[ii];
+		}
+		for (i = 0; i < nFR; i++)
+			delta_xFR[i] = -delta_xFR[i];
+		/* performRatioTest( nFR,FR_idx,&bounds, x_W,delta_xFR, options.epsNum,options.epsDen, sigmaLBnd,addLBndIdx ); */
+		sigmaLBnd = options.maxPrimalJump;
+		addLBndIdx = -1;
+		performPlainRatioTest(nFR, FR_idx, x_W, delta_xFR, options.epsNum, options.epsDen, sigmaLBnd, addLBndIdx);
+		if (removeBoundNotConstraint == BT_TRUE && bounds.getStatus(remIdx) == ST_UPPER)
+		{
+			/* also consider bound which is to be removed */
+			real_t one = 1.0;
+			x_W[0] = x[remIdx] - lb[remIdx];
+			performPlainRatioTest(1, &remIdx, x_W, &one, options.epsNum, options.epsDen, sigmaLBnd, addLBndIdx);
+		}
+		for (i = 0; i < nFR; i++)
+			delta_xFR[i] = -delta_xFR[i];
+
+		delete[] x_W;
+
+		/* constraints */
+
+		/* compute As (compressed to inactive constraints) */
+		real_t *As = new real_t[nIAC];
+		A->times(constraints.getInactive(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 0.0, As, nIAC);
+		A->times(constraints.getInactive(), bounds.getFree(), 1, 1.0, delta_xFR, nFR, 1.0, As, nIAC);
+
+		/* compress Ax_u */
+		real_t *Ax_W = new real_t[nIAC];
+		for (i = 0; i < nIAC; i++)
+		{
+			ii = IAC_idx[i];
+			Ax_W[i] = Ax_u[ii];
+		}
+		/* performRatioTest( nIAC,IAC_idx,&constraints, Ax_W,As, options.epsNum,options.epsDen, sigmaUCnstr,addUCnstrIdx ); */
+		sigmaUCnstr = options.maxPrimalJump;
+		addUCnstrIdx = -1;
+		performPlainRatioTest(nIAC, IAC_idx, Ax_W, As, options.epsNum, options.epsDen, sigmaUCnstr, addUCnstrIdx);
+		if (removeBoundNotConstraint == BT_FALSE && constraints.getStatus(remIdx) == ST_LOWER)
+		{
+			/* also consider constraint which is to be removed */
+			real_t one = 1.0;
+			performPlainRatioTest(1, &remIdx, &Ax_u[remIdx], &one, options.epsNum, options.epsDen, sigmaUCnstr, addUCnstrIdx);
+		}
+
+		/* compress Ax_l */
+		for (i = 0; i < nIAC; i++)
+		{
+			ii = IAC_idx[i];
+			Ax_W[i] = Ax_l[ii];
+		}
+		for (i = 0; i < nIAC; i++)
+			As[i] = -As[i];
+		/* performRatioTest( nIAC,IAC_idx,&constraints, Ax_W,As, options.epsNum,options.epsDen, sigmaLCnstr,addLCnstrIdx ); */
+		sigmaLCnstr = options.maxPrimalJump;
+		addLCnstrIdx = -1;
+		performPlainRatioTest(nIAC, IAC_idx, Ax_W, As, options.epsNum, options.epsDen, sigmaLCnstr, addLCnstrIdx);
+		if (removeBoundNotConstraint == BT_FALSE && constraints.getStatus(remIdx) == ST_UPPER)
+		{
+			/* also consider constraint which is to be removed */
+			real_t one = 1.0;
+			performPlainRatioTest(1, &remIdx, &Ax_l[remIdx], &one, options.epsNum, options.epsDen, sigmaLCnstr, addLCnstrIdx);
+		}
+
+		/* perform primal jump */
+		sigma = options.maxPrimalJump;
+		if (sigmaUCnstr < sigma) { sigma = sigmaUCnstr; addStatus = ST_UPPER; addBoundNotConstraint = BT_FALSE; addIdx = addUCnstrIdx; }
+		if (sigmaLCnstr < sigma) { sigma = sigmaLCnstr; addStatus = ST_LOWER; addBoundNotConstraint = BT_FALSE; addIdx = addLCnstrIdx; }
+		if (sigmaUBnd < sigma) { sigma = sigmaUBnd; addStatus = ST_UPPER; addBoundNotConstraint = BT_TRUE; addIdx = addUBndIdx; }
+		if (sigmaLBnd < sigma) { sigma = sigmaLBnd; addStatus = ST_LOWER; addBoundNotConstraint = BT_TRUE; addIdx = addLBndIdx; }
+
+		if (sigma >= options.maxPrimalJump)
+		{
+			unbounded = BT_TRUE;
+			returnvalue = RET_HOTSTART_STOPPED_UNBOUNDEDNESS;
+		}
+		else
+		{
+			for (i = 0; i < nFR; i++)
+				x[FR_idx[i]] += sigma * delta_xFR[i];
+
+			for (i = 0; i < nFX; i++)
+				x[FX_idx[i]] += sigma * delta_xFX[i];
+
+			/* update Ax, Ax_u, and Ax_l */
+			A->times(1, 1.0, x, nV, 0.0, Ax, nC);
+			for (i = 0; i < nC; i++) Ax_u[i] = ubA[i] - Ax[i];
+			for (i = 0; i < nC; i++) Ax_l[i] = Ax[i] - lbA[i];
+
+			/* change working set later */
+			exchangeHappened = BT_TRUE;
+		}
+
+		delete[] Ax_W;
+		delete[] As;
+	}
+
+	delete[] delta_yFX;
+	delete[] delta_yAC;
+	delete[] delta_xFR;
+	delete[] delta_xFX;
+// 	delete[] delta_g;
+
+	return returnvalue;
+}
+
+
+
+/*
+ *	b a c k s o l v e T
+ */
+returnValue QProblem::backsolveT( const real_t* const b, BooleanType transposed, real_t* const a ) const
+{
+	int i, j;
+	int nT = getNAC( );
+	int tcol = sizeT - nT;
+
+	real_t sum;
+
+	/* nothing to do */
+	if ( nT <= 0 )
+		return SUCCESSFUL_RETURN;
+
+
+	/* Solve Ta = b, where T might be transposed. */
+	if ( transposed == BT_FALSE )
+	{
+		/* solve Ta = b */
+		for( i=0; i<nT; ++i )
+		{
+			sum = b[i];
+			for( j=0; j<i; ++j )
+				sum -= TT(i,sizeT-1-j) * a[nT-1-j];
+
+			if ( getAbs( TT(i,sizeT-1-i) ) > EPS )
+				a[nT-1-i] = sum / TT(i,sizeT-1-i);
+			else
+				return THROWERROR( RET_DIV_BY_ZERO );
+		}
+	}
+	else
+	{
+		/* solve T^T*a = b */
+		for( i=0; i<nT; ++i )
+		{
+			sum = b[i];
+			for( j=0; j<i; ++j )
+				sum -= TT(nT-1-j,tcol+i) * a[nT-1-j];
+
+			if ( getAbs( TT(nT-1-i,tcol+i) ) > EPS )
+				a[nT-1-i] = sum / TT(nT-1-i,tcol+i);
+			else
+				return THROWERROR( RET_DIV_BY_ZERO );
+		}
+	}
+
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	d e t e r m i n e D a t a S h i f t
+ */
+returnValue QProblem::determineDataShift(	const real_t* const g_new, const real_t* const lbA_new, const real_t* const ubA_new,
+											const real_t* const lb_new, const real_t* const ub_new,
+											real_t* const delta_g, real_t* const delta_lbA, real_t* const delta_ubA,
+											real_t* const delta_lb, real_t* const delta_ub,
+											BooleanType& Delta_bC_isZero, BooleanType& Delta_bB_isZero
+											)
+{
+	int i, ii;
+	int nC  = getNC( );
+	int nAC = getNAC( );
+
+	int* FX_idx;
+	int* AC_idx;
+
+	bounds.getFixed( )->getNumberArray( &FX_idx );
+	constraints.getActive( )->getNumberArray( &AC_idx );
+
+
+
+	/* I) DETERMINE DATA SHIFT FOR BOUNDS */
+	QProblemB::determineDataShift(	g_new,lb_new,ub_new,
+									delta_g,delta_lb,delta_ub,
+									Delta_bB_isZero );
+
+
+	/* II) DETERMINE DATA SHIFT FOR CONSTRAINTS */
+	/* 1) Calculate shift directions. */
+	for( i=0; i<nC; ++i )
+	{
+		/* if lower constraints' bounds are to be disabled or do not exist, shift them to -infinity */
+		if ( lbA_new != 0 )
+			delta_lbA[i] = lbA_new[i] - lbA[i];
+		else
+			delta_lbA[i] = -INFTY - lbA[i];
+	}
+
+	for( i=0; i<nC; ++i )
+	{
+		/* if upper constraints' bounds are to be disabled or do not exist, shift them to infinity */
+		if ( ubA_new != 0 )
+			delta_ubA[i] = ubA_new[i] - ubA[i];
+		else
+			delta_ubA[i] = INFTY - ubA[i];
+	}
+
+	/* 2) Determine if active constraints' bounds are to be shifted. */
+	Delta_bC_isZero = BT_TRUE;
+
+	for ( i=0; i<nAC; ++i )
+	{
+		ii = AC_idx[i];
+
+		if ( ( getAbs( delta_lbA[ii] ) > EPS ) || ( getAbs( delta_ubA[ii] ) > EPS ) )
+		{
+			Delta_bC_isZero = BT_FALSE;
+			break;
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	d e t e r m i n e S t e p D i r e c t i o n
+ */
+returnValue QProblem::determineStepDirection(	const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA,
+												const real_t* const delta_lb, const real_t* const delta_ub,
+												BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero,
+												real_t* const delta_xFX, real_t* const delta_xFR,
+												real_t* const delta_yAC, real_t* const delta_yFX
+												)
+{
+	int i, j, ii, jj, r;
+	int nV  = getNV( );
+	int nFR = getNFR( );
+	int nFX = getNFX( );
+	int nAC = getNAC( );
+	int nZ  = getNZ( );
+	
+	int* FR_idx;
+	int* FX_idx;
+	int* AC_idx;
+
+	bounds.getFree( )->getNumberArray( &FR_idx );
+	bounds.getFixed( )->getNumberArray( &FX_idx );
+	constraints.getActive( )->getNumberArray( &AC_idx );
+
+
+	/* I) DETERMINE delta_xFX (this is exact, does not need refinement) */
+	if ( Delta_bB_isZero == BT_FALSE )
+	{
+		for( i=0; i<nFX; ++i )
+		{
+			ii = FX_idx[i];
+
+			if ( bounds.getStatus( ii ) == ST_LOWER )
+				delta_xFX[i] = delta_lb[ii];
+			else
+				delta_xFX[i] = delta_ub[ii];
+		}
+	}
+	else
+	{
+		for( i=0; i<nFX; ++i )
+			delta_xFX[i] = 0.0;
+	}
+
+
+	/* tempA and tempB hold the residuals in gFR and bA (= lbA or ubA)
+	 * delta_xFR, delta_yAC hold the steps that get refined */
+	for ( i=0; i<nFR; ++i )
+	{
+		ii = FR_idx[i];
+		tempA[i] = delta_g[ii];
+		delta_xFR[i] = 0.0;
+	}
+	for ( i=0; i<nAC; ++i )
+		delta_yAC[i] = 0.0;
+	if ( Delta_bC_isZero == BT_FALSE )
+	{
+		for ( i=0; i<nAC; ++i )
+		{
+			ii = AC_idx[i];
+			if ( constraints.getStatus( ii ) == ST_LOWER )
+				tempB[i] = delta_lbA[ii];
+			else
+				tempB[i] = delta_ubA[ii];
+		}
+	}
+	else
+	{
+		for ( i=0; i<nAC; ++i )
+			tempB[i] = 0.0;
+	}
+
+	/* Iterative refinement loop for delta_xFRz, delta_xFRy, delta_yAC */
+	for ( r=0; r<=options.numRefinementSteps; ++r )
+	{
+		/* II) DETERMINE delta_xFR */
+		if ( nFR > 0 )
+		{
+			for( i=0; i<nFR; ++i )
+				delta_xFR_TMP[i] = 0.0;
+
+			/* 1) Determine delta_xFRy. */
+			if ( nAC > 0 )
+			{
+				if ( ( Delta_bC_isZero == BT_TRUE ) && ( Delta_bB_isZero == BT_TRUE ) )
+				{
+					for( i=0; i<nAC; ++i )
+						delta_xFRy[i] = 0.0;
+				}
+				else
+				{
+					/* compute bA - A * delta_xFX. tempB already holds bA->
+					 * in refinements r>=1, delta_xFX is exactly zero */
+					if ( ( Delta_bB_isZero == BT_FALSE ) && ( r == 0 ) )
+						A->times(constraints.getActive(), bounds.getFixed(), 1, -1.0, delta_xFX, nFX, 1.0, tempB, nAC);
+
+					if ( backsolveT( tempB, BT_FALSE, delta_xFRy ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_STEPDIRECTION_FAILED_TQ );
+
+					for( i=0; i<nFR; ++i )
+					{
+						ii = FR_idx[i];
+						for( j=0; j<nAC; ++j )
+							delta_xFR_TMP[i] += QQ(ii,nZ+j) * delta_xFRy[j];
+					}
+				}
+			}
+
+
+			/* 2) Determine delta_xFRz. */
+			for( i=0; i<nZ; ++i )
+				delta_xFRz[i] = 0.0;
+
+			if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_IDENTITY ) )
+			{
+				/* compute Z*delta_gFR [/eps] (delta_gFR is stored in tempA) */
+				for( j=0; j<nFR; ++j )
+				{
+					jj = FR_idx[j];
+					for( i=0; i<nZ; ++i )
+						delta_xFRz[i] -= QQ(jj,i) * tempA[j];
+				}
+
+				if ( hessianType == HST_ZERO )
+				{
+					if ( usingRegularisation( ) == BT_TRUE )
+					{
+						for( i=0; i<nZ; ++i )
+							delta_xFRz[i] /= regVal;
+					}
+					else
+					{
+						/* When solving LPs without regularisation, iterates must always ne at a vertex. */
+						if ( nZ > 0 )
+							return THROWERROR( RET_UNKNOWN_BUG );
+					}
+				}
+			}
+			else
+			{
+				/* compute HMX*delta_xFX. DESTROY delta_gFR that was in tempA */
+				if ( ( Delta_bB_isZero == BT_FALSE ) && ( r == 0 ) )
+					H->times(bounds.getFree(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, tempA, nFR);
+
+				/* compute HFR*delta_xFRy */
+				if ( ( nAC > 0 ) && ( ( Delta_bC_isZero == BT_FALSE ) || ( Delta_bB_isZero == BT_FALSE ) ) )
+					H->times(bounds.getFree(), bounds.getFree(), 1, 1.0, delta_xFR_TMP, nFR, 1.0, tempA, nFR);
+
+				/* compute ZFR_delta_xFRz = (Z'*HFR*Z) \ Z * (HFR*delta_xFR + HMX*delta_xFX + delta_gFR) */
+				if ( nZ > 0 )
+				{
+					for( j=0; j<nFR; ++j )
+					{
+						jj = FR_idx[j];
+						for( i=0; i<nZ; ++i )
+							delta_xFRz[i] -= QQ(jj,i) * tempA[j];
+					}
+
+					if ( backsolveR( delta_xFRz,BT_TRUE,delta_xFRz ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
+
+					if ( backsolveR( delta_xFRz,BT_FALSE,delta_xFRz ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
+				}
+			}
+
+			/* compute Z * ZFR_delta_xFRz */
+			if ( nZ > 0 )
+			{
+				for( i=0; i<nFR; ++i )
+				{
+					ZFR_delta_xFRz[i] = 0.0;
+
+					ii = FR_idx[i];
+					for( j=0; j<nZ; ++j )
+						ZFR_delta_xFRz[i] += QQ(ii,j) * delta_xFRz[j];
+
+					delta_xFR_TMP[i] += ZFR_delta_xFRz[i];
+				}
+			}
+		}
+
+		/* III) DETERMINE delta_yAC */
+		if ( nAC > 0 ) /* => ( nFR = nZ + nAC > 0 ) */
+		{
+			if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_IDENTITY ) )
+			{
+				/* if zero:     delta_yAC = (T')^-1 * ( Yfr*delta_gFR + eps*delta_xFRy ),
+				 * if identity: delta_yAC = (T')^-1 * ( Yfr*delta_gFR +     delta_xFRy )
+				 *
+				 * DESTROY residual_bA that was stored in tempB
+				 * If we come here, residual_gFR in tempA is STILL VALID
+				 */
+				if ( hessianType == HST_IDENTITY )
+				{
+					for( j=0; j<nAC; ++j )
+						tempB[j] = delta_xFRy[j];
+				}
+				else /* hessianType == HST_ZERO */
+				{
+					if ( usingRegularisation( ) == BT_TRUE )
+					{
+						for( j=0; j<nAC; ++j )
+							tempB[j] = regVal*delta_xFRy[j];
+					}
+					else
+					{
+						for( j=0; j<nAC; ++j )
+							tempB[j] = 0.0;
+					}
+				}
+
+				for( j=0; j<nAC; ++j )
+				{
+					for( i=0; i<nFR; ++i )
+					{
+						ii = FR_idx[i];
+						tempB[j] += QQ(ii,nZ+j) * tempA[i];
+					}
+				}
+			}
+			else
+			{
+				/* Compute HFR * delta_xFR + HMX*delta_xFX
+				 * Here, tempA holds (HFR*delta_xFRy + HMX*delta_xFX) */
+				if ( nZ > 0 )
+					H->times(bounds.getFree(), bounds.getFree(), 1, 1.0, ZFR_delta_xFRz, nFR, 1.0, tempA, nFR);
+
+				for( i=0; i<nAC; ++i)
+				{
+					tempB[i] = 0.0;
+					for( j=0; j<nFR; ++j )
+					{
+						jj = FR_idx[j];
+						tempB[i] += QQ(jj,nZ+i) * tempA[j];
+					}
+				}
+			}
+
+			if ( backsolveT( tempB,BT_TRUE,delta_yAC_TMP ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_STEPDIRECTION_FAILED_TQ );
+		}
+
+		/* refine the solution found so far */
+		for ( i=0; i<nFR; ++i )
+			delta_xFR[i] += delta_xFR_TMP[i];
+		for ( i=0; i<nAC; ++i )
+			delta_yAC[i] += delta_yAC_TMP[i];
+
+		if ( options.numRefinementSteps > 0 )
+		{
+			/* compute residuals in tempA and tempB, and max-norm */
+			for ( i=0; i<nFR; ++i )
+			{
+				ii = FR_idx[i];
+				tempA[i] = delta_g[ii];
+			}
+
+			switch ( hessianType )
+			{
+				case HST_ZERO:
+					if ( usingRegularisation( ) == BT_TRUE )
+						for ( i=0; i<nFR; ++i )
+							tempA[i] += regVal*delta_xFR[i];
+					break;
+
+				case HST_IDENTITY:
+					for ( i=0; i<nFR; ++i )
+						tempA[i] += delta_xFR[i];
+					break;
+
+				default:
+					H->times(bounds.getFree(), bounds.getFree(),  1, 1.0, delta_xFR, nFR, 1.0, tempA, nFR);
+					H->times(bounds.getFree(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, tempA, nFR);
+					break;
+			}
+
+			A->transTimes(constraints.getActive(), bounds.getFree(), 1, -1.0, delta_yAC, nAC, 1.0, tempA, nFR);
+			real_t rnrm = 0.0;
+			for ( i=0; i<nFR; ++i )
+				if (rnrm < getAbs (tempA[i]))
+					rnrm = getAbs (tempA[i]);
+
+			if (!Delta_bC_isZero)
+			{
+				for ( i=0; i<nAC; ++i )
+				{
+					ii = AC_idx[i];
+					if ( constraints.getStatus( ii ) == ST_LOWER )
+						tempB[i] = delta_lbA[ii];
+					else
+						tempB[i] = delta_ubA[ii];
+				}
+			}
+			else
+			{
+				for ( i=0; i<nAC; ++i )
+					tempB[i] = 0.0;
+			}
+			A->times(constraints.getActive(), bounds.getFree(), 1, -1.0, delta_xFR, nFR, 1.0, tempB, nAC);
+			A->times(constraints.getActive(), bounds.getFixed(), 1, -1.0, delta_xFX, nFX, 1.0, tempB, nAC);
+			for ( i=0; i<nAC; ++i )
+				if (rnrm < getAbs (tempB[i]))
+					rnrm = getAbs (tempB[i]);
+
+			/* early termination of residual norm small enough */
+			if ( rnrm < options.epsIterRef )
+				break;
+		}
+	} /* end of refinement loop for delta_xFRz, delta_xFRy, delta_yAC */
+
+
+	/* IV) DETERMINE delta_yFX */
+	if ( nFX > 0 )
+	{
+		for( i=0; i<nFX; ++i )
+			delta_yFX[i] = delta_g[FX_idx[i]];
+
+		A->transTimes(constraints.getActive(), bounds.getFixed(), 1, -1.0, delta_yAC, nAC, 1.0, delta_yFX, nFX);
+
+		if ( hessianType == HST_ZERO )
+		{
+			if ( usingRegularisation( ) == BT_TRUE )
+				for( i=0; i<nFX; ++i )
+					delta_yFX[i] += regVal*delta_xFX[i];
+		}
+		else if ( hessianType == HST_IDENTITY )
+		{
+			for( i=0; i<nFX; ++i )
+				delta_yFX[i] += delta_xFX[i];
+		}
+		else
+		{
+			H->times(bounds.getFixed(), bounds.getFree(), 1, 1.0, delta_xFR, nFR, 1.0, delta_yFX, nFX);
+			H->times(bounds.getFixed(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, delta_yFX, nFX);
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	p e r f o r m S t e p
+ */
+returnValue QProblem::performStep(	const real_t* const delta_g,
+									const real_t* const delta_lbA, const real_t* const delta_ubA,
+									const real_t* const delta_lb, const real_t* const delta_ub,
+									const real_t* const delta_xFX, const real_t* const delta_xFR,
+									const real_t* const delta_yAC, const real_t* const delta_yFX,
+									int& BC_idx, SubjectToStatus& BC_status, BooleanType& BC_isBound
+									)
+{
+	int i, j, ii, jj;
+	int nV  = getNV( );
+	int nC  = getNC( );
+	int nFR = getNFR( );
+	int nFX = getNFX( );
+	int nAC = getNAC( );
+	int nIAC = getNIAC( );
+
+	int* FR_idx;
+	int* FX_idx;
+	int* AC_idx;
+	int* IAC_idx;
+
+	bounds.getFree( )->getNumberArray( &FR_idx );
+	bounds.getFixed( )->getNumberArray( &FX_idx );
+	constraints.getActive( )->getNumberArray( &AC_idx );
+	constraints.getInactive( )->getNumberArray( &IAC_idx );
+
+	/* initialise maximum steplength array */
+	tau = 1.0;
+	BC_idx = -1;
+	BC_status = ST_UNDEFINED;
+
+	int BC_idx_tmp = -1;
+
+	real_t* num = new real_t[ getMax( nV,nC ) ];
+	real_t* den = new real_t[ getMax( nV,nC ) ];
+
+	real_t* delta_Ax_l = new real_t[nC];
+	real_t* delta_Ax_u = new real_t[nC];
+	real_t* delta_Ax   = new real_t[nC];
+
+	real_t* delta_x = new real_t[nV];
+	for( j=0; j<nFR; ++j )
+	{
+		jj = FR_idx[j];
+		delta_x[jj] = delta_xFR[j];
+	}
+	for( j=0; j<nFX; ++j )
+	{
+		jj = FX_idx[j];
+		delta_x[jj] = delta_xFX[j];
+	}
+
+
+	/* I) DETERMINE MAXIMUM DUAL STEPLENGTH: */
+	/* 1) Ensure that active dual constraints' bounds remain valid
+	 *    (ignoring inequality constraints).  */
+	for( i=0; i<nAC; ++i )
+	{
+		ii = AC_idx[i];
+
+		num[i] = y[nV+ii];
+		den[i] = -delta_yAC[i];
+	}
+
+	performRatioTest( nAC,AC_idx,&constraints, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
+
+	if ( BC_idx_tmp >= 0 )
+	{
+		BC_idx = BC_idx_tmp;
+		BC_status = ST_INACTIVE;
+		BC_isBound = BT_FALSE;
+	}
+
+
+	/* 2) Ensure that active dual bounds remain valid
+	 *    (ignoring implicitly fixed variables). */
+	for( i=0; i<nFX; ++i )
+	{
+		ii = FX_idx[i];
+		num[i] = y[ii];
+		den[i] = -delta_yFX[i];
+	}
+
+	performRatioTest( nFX,FX_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
+
+	if ( BC_idx_tmp >= 0 )
+	{
+		BC_idx = BC_idx_tmp;
+		BC_status = ST_INACTIVE;
+		BC_isBound = BT_TRUE;
+	}
+
+
+ 	/* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH */
+	/* 1) Ensure that inactive constraints' bounds remain valid
+	 *    (ignoring unbounded constraints). */
+
+	/* calculate product A*x */
+	if ( constraintProduct == 0 )
+	{
+		A->times(constraints.getInactive(), 0, 1, 1.0, delta_x, nV, 0.0, delta_Ax, nC, BT_FALSE);
+	}
+	else
+	{
+		for( i=0; i<nIAC; ++i )
+		{
+			ii = IAC_idx[i];
+
+			if ( constraints.getType( ii ) != ST_UNBOUNDED )
+			{
+				if ( (*constraintProduct)( ii,delta_x, &(delta_Ax[ii]) ) != 0 )
+				{
+					delete[] den; delete[] num;
+					delete[] delta_Ax; delete[] delta_Ax_u; delete[] delta_Ax_l; delete[] delta_x;
+					return THROWERROR( RET_ERROR_IN_CONSTRAINTPRODUCT );
+				}
+			}
+		}
+	}
+
+	if ( constraints.hasNoLower( ) == BT_FALSE )
+	{
+		for( i=0; i<nIAC; ++i )
+		{
+			ii = IAC_idx[i];
+			num[i] = getMax( Ax_l[ii],0.0 );
+			den[i] = delta_lbA[ii] - delta_Ax[ii];
+		}
+
+		performRatioTest( nIAC,IAC_idx,&constraints, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
+
+		if ( BC_idx_tmp >= 0 )
+		{
+			BC_idx = BC_idx_tmp;
+			BC_status = ST_LOWER;
+			BC_isBound = BT_FALSE;
+		}
+	}
+
+	if ( constraints.hasNoUpper( ) == BT_FALSE )
+	{
+		for( i=0; i<nIAC; ++i )
+		{
+			ii = IAC_idx[i];
+			num[i] = getMax( Ax_u[ii],0.0 );
+			den[i] = delta_Ax[ii] - delta_ubA[ii];
+		}
+
+		performRatioTest( nIAC,IAC_idx,&constraints, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
+
+		if ( BC_idx_tmp >= 0 )
+		{
+			BC_idx = BC_idx_tmp;
+			BC_status = ST_UPPER;
+			BC_isBound = BT_FALSE;
+		}
+	}
+
+
+	for( i=0; i<nIAC; ++i )
+	{
+		ii = IAC_idx[i];
+
+		if ( constraints.getType( ii ) != ST_UNBOUNDED )
+		{
+			delta_Ax_l[ii] = delta_Ax[ii] - delta_lbA[ii];
+			delta_Ax_u[ii] = delta_ubA[ii] - delta_Ax[ii];
+		}
+	}
+
+
+	/* 2) Ensure that inactive bounds remain valid
+	 *    (ignoring unbounded variables). */
+	/* inactive lower bounds */
+	if ( bounds.hasNoLower( ) == BT_FALSE )
+	{
+		for( i=0; i<nFR; ++i )
+		{
+			ii = FR_idx[i];
+			num[i] = getMax( x[ii] - lb[ii],0.0 );
+			den[i] = delta_lb[ii] - delta_xFR[i];
+		}
+
+		performRatioTest( nFR,FR_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
+
+		if ( BC_idx_tmp >= 0 )
+		{
+			BC_idx = BC_idx_tmp;
+			BC_status = ST_LOWER;
+			BC_isBound = BT_TRUE;
+		}
+	}
+
+	/* inactive upper bounds */
+	if ( bounds.hasNoUpper( ) == BT_FALSE )
+	{
+		for( i=0; i<nFR; ++i )
+		{
+			ii = FR_idx[i];
+			num[i] = getMax( ub[ii] - x[ii],0.0 );
+			den[i] = delta_xFR[i] - delta_ub[ii];
+		}
+
+		performRatioTest( nFR,FR_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
+
+		if ( BC_idx_tmp >= 0 )
+		{
+			BC_idx = BC_idx_tmp;
+			BC_status = ST_UPPER;
+			BC_isBound = BT_TRUE;
+		}
+	}
+
+	delete[] den;
+	delete[] num;
+	delete[] delta_x;
+
+
+	#ifndef __XPCTARGET__
+	char messageString[MAX_STRING_LENGTH];
+
+	if ( BC_status == ST_UNDEFINED )
+		snprintf( messageString,MAX_STRING_LENGTH,"Stepsize is %.15e!",tau );
+	else
+		snprintf( messageString,MAX_STRING_LENGTH,"Stepsize is %.15e! (idx = %d, isBound = %d, status = %d)",tau,BC_idx,BC_isBound,BC_status );
+
+	getGlobalMessageHandler( )->throwInfo( RET_STEPSIZE_NONPOSITIVE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+	#endif
+
+
+	/* III) PERFORM STEP ALONG HOMOTOPY PATH */
+	if ( tau > ZERO )
+	{
+		/* 1) Perform step in primal and dual space... */
+		for( i=0; i<nFR; ++i )
+		{
+			ii = FR_idx[i];
+			x[ii] += tau*delta_xFR[i];
+		}
+
+		for( i=0; i<nFX; ++i )
+		{
+			ii = FX_idx[i];
+			x[ii] += tau*delta_xFX[i];
+			y[ii] += tau*delta_yFX[i];
+		}
+
+		for( i=0; i<nAC; ++i )
+		{
+			ii = AC_idx[i];
+			y[nV+ii] += tau*delta_yAC[i];
+		}
+
+		/* 2) Shift QP data. */
+		for( i=0; i<nV; ++i )
+		{
+			g[i]  += tau*delta_g[i];
+			lb[i] += tau*delta_lb[i];
+			ub[i] += tau*delta_ub[i];
+		}
+
+		for( i=0; i<nC; ++i )
+		{
+			lbA[i] += tau*delta_lbA[i];
+			ubA[i] += tau*delta_ubA[i];
+		}
+
+		A->times( constraints.getActive(),0, 1, 1.0, x, nV, 0.0, Ax, nC, BT_FALSE );
+		for( i=0; i<nAC; ++i )
+		{
+			ii = AC_idx[i];
+			Ax_u[ii] = ubA[ii] - Ax[ii];
+			Ax_l[ii] = Ax[ii] - lbA[ii];
+		}
+		for( i=0; i<nIAC; ++i )
+		{
+			ii = IAC_idx[i];
+			if ( constraints.getType( ii ) != ST_UNBOUNDED )
+			{
+				Ax[ii]   += tau*delta_Ax[ii];
+				Ax_l[ii] += tau*delta_Ax_l[ii];
+				Ax_u[ii] += tau*delta_Ax_u[ii];
+			}
+		}
+	}
+	else
+	{
+		/* print a stepsize warning if stepsize is zero */
+		#ifndef __XPCTARGET__
+		snprintf( messageString,MAX_STRING_LENGTH,"Stepsize is %.15e",tau );
+		getGlobalMessageHandler( )->throwWarning( RET_STEPSIZE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+		#endif
+	}
+
+	delete[] delta_Ax; delete[] delta_Ax_u; delete[] delta_Ax_l;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	c h a n g e A c t i v e S e t
+ */
+returnValue QProblem::changeActiveSet( int BC_idx, SubjectToStatus BC_status, BooleanType BC_isBound )
+{
+	int nV = getNV( );
+
+	#ifndef __XPCTARGET__
+	char messageString[MAX_STRING_LENGTH];
+	#endif
+
+	switch ( BC_status )
+	{
+		/* Optimal solution found as no working set change detected. */
+		case ST_UNDEFINED:
+			return SUCCESSFUL_RETURN;
+
+		/* Remove one variable from active set. */
+		case ST_INACTIVE:
+			if ( BC_isBound == BT_TRUE )
+			{
+				#ifndef __XPCTARGET__
+				snprintf( messageString,MAX_STRING_LENGTH,"bound no. %d.", BC_idx );
+				getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+				#endif
+
+				if ( removeBound( BC_idx,BT_TRUE,BT_TRUE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
+
+				y[BC_idx] = 0.0;
+			}
+			else
+			{
+				#ifndef __XPCTARGET__
+				snprintf( messageString,MAX_STRING_LENGTH,"constraint no. %d.", BC_idx );
+				getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+				#endif
+
+				if ( removeConstraint( BC_idx,BT_TRUE,BT_TRUE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
+
+				y[nV+BC_idx] = 0.0;
+			}
+			break;
+
+
+		/* Add one variable to active set. */
+		default:
+			returnValue returnvalue;
+			if ( BC_isBound == BT_TRUE )
+			{
+				#ifndef __XPCTARGET__
+				if ( BC_status == ST_LOWER )
+					snprintf( messageString,MAX_STRING_LENGTH,"lower bound no. %d.", BC_idx );
+				else
+					snprintf( messageString,MAX_STRING_LENGTH,"upper bound no. %d.", BC_idx );
+				getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+				#endif
+
+				returnvalue = addBound( BC_idx,BC_status,BT_TRUE );
+				if ( returnvalue == RET_ADDBOUND_FAILED_INFEASIBILITY )
+					return returnvalue;
+				if ( returnvalue != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
+			}
+			else
+			{
+				#ifndef __XPCTARGET__
+				if ( BC_status == ST_LOWER )
+					snprintf( messageString,MAX_STRING_LENGTH,"lower constraint's bound no. %d.", BC_idx );
+				else
+					snprintf( messageString,MAX_STRING_LENGTH,"upper constraint's bound no. %d.", BC_idx );
+				getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+				#endif
+
+				returnvalue = addConstraint( BC_idx,BC_status,BT_TRUE );
+				if ( returnvalue == RET_ADDCONSTRAINT_FAILED_INFEASIBILITY )
+					return returnvalue;
+				if ( returnvalue != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
+			}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ * g e t R e l a t i v e H o m o t o p y L e n g t h
+ */
+real_t QProblem::getRelativeHomotopyLength(	const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
+											const real_t* const lbA_new, const real_t* const ubA_new
+											)
+{
+	int i;
+	int nC = getNC( );
+	real_t len = QProblemB::getRelativeHomotopyLength( g_new,lb_new,ub_new );
+	real_t d, s;
+
+	/* lower constraint bounds */
+	for (i = 0; i < nC && lbA_new; i++)
+	{
+		s = getAbs(lbA_new[i]);
+		if (s < 1.0) s = 1.0;
+		d = getAbs(lbA_new[i] - lbA[i]) / s;
+		if (d > len) len = d;
+	}
+
+	/* upper constraint bounds */
+	for (i = 0; i < nC && ubA_new; i++)
+	{
+		s = getAbs(ubA_new[i]);
+		if (s < 1.0) s = 1.0;
+		d = getAbs(ubA_new[i] - ubA[i]) / s;
+		if (d > len) len = d;
+	}
+
+	return len;
+}
+
+
+/*
+ * p e r f o r m R a m p i n g
+ */
+returnValue QProblem::performRamping( )
+{
+	int nV = getNV( ), nC = getNC( ), bstat, cstat, i, nRamp;
+	real_t tP, rampValP, tD, rampValD, sca;
+
+	/* compute number of values in ramp */
+	nRamp = nV + nC + nC + nV;
+
+	/* ramp inactive variable bounds and active dual bound variables */
+	for (i = 0; i < nV; i++)
+	{
+		switch (bounds.getType(i))
+		{
+			case ST_EQUALITY:
+				lb[i] = x[i]; ub[i] = x[i];  /* reestablish exact feasibility */
+				continue;
+
+			case ST_BOUNDED:
+				tP = static_cast<real_t>((i+rampOffset) % nRamp) / static_cast<real_t>(nRamp-1);
+				rampValP = (1.0-tP) * ramp0 + tP * ramp1;
+				tD = static_cast<real_t>((nV+nC+nC+i+rampOffset) % nRamp) / static_cast<real_t>(nRamp-1);
+				rampValD = (1.0-tD) * ramp0 + tD * ramp1;
+				bstat = bounds.getStatus(i);
+				if (bstat != ST_LOWER) { sca = getMax(getAbs(x[i]), 1.0); lb[i] = x[i] - sca * rampValP; }
+				if (bstat != ST_UPPER) { sca = getMax(getAbs(x[i]), 1.0); ub[i] = x[i] + sca * rampValP; }
+				if (bstat == ST_LOWER) { lb[i] = x[i]; y[i] = +rampValD; }
+				if (bstat == ST_UPPER) { ub[i] = x[i]; y[i] = -rampValD; }
+				if (bstat == ST_INACTIVE) y[i] = 0.0; /* reestablish exact complementarity */
+				break;
+
+			case ST_UNBOUNDED:
+			case ST_DISABLED:
+			default:
+				 continue;
+		}
+	}
+
+	/* ramp inactive constraints and active dual constraint variables */
+	for (i = 0; i < nC; i++)
+	{
+		switch (constraints.getType(i))
+		{
+			case ST_EQUALITY:
+				lbA[i] = Ax[i]; ubA[i] = Ax[i];  /* reestablish exact feasibility */
+				continue;
+
+			case ST_BOUNDED:
+				tP = static_cast<real_t>((nV+i+rampOffset) % nRamp) / static_cast<real_t>(nRamp-1);
+				rampValP = (1.0-tP) * ramp0 + tP * ramp1;
+				tD = static_cast<real_t>((nV+nC+i+rampOffset) % nRamp) / static_cast<real_t>(nRamp-1);
+				rampValD = (1.0-tD) * ramp0 + tD * ramp1;
+				cstat = constraints.getStatus(i);
+				if (cstat != ST_LOWER) { sca = getMax(getAbs(Ax[i]), 1.0); lbA[i] = Ax[i] - sca * rampValP; }
+				if (cstat != ST_UPPER) { sca = getMax(getAbs(Ax[i]), 1.0); ubA[i] = Ax[i] + sca * rampValP; }
+				if (cstat == ST_LOWER) { lbA[i] = Ax[i]; y[nV+i] = +rampValD; }
+				if (cstat == ST_UPPER) { ubA[i] = Ax[i]; y[nV+i] = -rampValD; }
+				if (cstat == ST_INACTIVE) y[nV+i] = 0.0; /* reestablish exact complementarity */
+
+				Ax_l[i] = Ax[i] - lbA[i];
+				Ax_u[i] = ubA[i] - Ax[i];
+				break;
+
+			case ST_UNBOUNDED:
+			case ST_DISABLED:
+			default:
+				continue;
+		}
+	}
+
+	/* reestablish exact stationarity */
+	setupAuxiliaryQPgradient( );
+
+	/* advance ramp offset to avoid Ramping cycles */
+	rampOffset++;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ * u p d a t e F a r B o u n d s
+ */
+returnValue QProblem::updateFarBounds(	real_t curFarBound, int nRamp,
+										const real_t* const lb_new, real_t* const lb_new_far,
+										const real_t* const ub_new, real_t* const ub_new_far,
+										const real_t* const lbA_new, real_t* const lbA_new_far,
+										const real_t* const ubA_new, real_t* const ubA_new_far
+										) const
+{
+	int i;
+	real_t rampVal, t;
+	int nV = getNV( );
+	int nC = getNC( );
+
+    returnValue returnvalue = QProblemB::updateFarBounds(	curFarBound,nRamp,
+															lb_new,lb_new_far, ub_new,ub_new_far
+															);
+    if ( returnvalue != SUCCESSFUL_RETURN )
+        return returnvalue;
+
+	if ( options.enableRamping == BT_TRUE )
+	{
+		for ( i=0; i<nC; ++i )
+		{
+			t = static_cast<real_t>((nV+i + rampOffset) % nRamp) / static_cast<real_t>(nRamp-1);
+			rampVal = curFarBound * (1.0 + (1.0-t)*ramp0 + t*ramp1);
+
+			if ( lbA_new == 0 )
+				lbA_new_far[i] = -rampVal;
+			else
+				lbA_new_far[i] = getMax( -rampVal,lbA_new[i] );
+
+			if ( ubA_new == 0 )
+				ubA_new_far[i] = rampVal;
+			else
+				ubA_new_far[i] = getMin( rampVal,ubA_new[i] );
+		}
+	}
+	else
+	{
+		for ( i=0; i<nC; ++i )
+		{
+			if ( lbA_new == 0 )
+				lbA_new_far[i] = -curFarBound;
+			else
+				lbA_new_far[i] = getMax( -curFarBound,lbA_new[i] );
+
+			if ( ubA_new == 0 )
+				ubA_new_far[i] = curFarBound;
+			else
+				ubA_new_far[i] = getMin( curFarBound,ubA_new[i] );
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ * p e r f o r m D r i f t C o r r e c t i o n
+ */
+returnValue QProblem::performDriftCorrection( )
+{
+	int i;
+	int nV = getNV ();
+	int nC = getNC ();
+
+	for ( i=0; i<nV; ++i )
+	{
+		switch ( bounds.getType ( i ) )
+		{
+			case ST_BOUNDED:
+				switch ( bounds.getStatus ( i ) )
+				{
+					case ST_LOWER:
+						lb[i] = x[i];
+						ub[i] = getMax (ub[i], x[i]);
+						y[i] = getMax (y[i], 0.0);
+						break;
+					case ST_UPPER:
+						lb[i] = getMin (lb[i], x[i]);
+						ub[i] = x[i];
+						y[i] = getMin (y[i], 0.0);
+						break;
+					case ST_INACTIVE:
+						lb[i] = getMin (lb[i], x[i]);
+						ub[i] = getMax (ub[i], x[i]);
+						y[i] = 0.0;
+						break;
+					case ST_UNDEFINED:
+					case ST_INFEASIBLE_LOWER:
+					case ST_INFEASIBLE_UPPER:
+						break;
+				}
+				break;
+			case ST_EQUALITY:
+				lb[i] = x[i];
+				ub[i] = x[i];
+				break;
+			case ST_UNBOUNDED:
+			case ST_UNKNOWN:
+            case ST_DISABLED:
+				break;
+		}
+	}
+
+	for ( i=0; i<nC; ++i )
+	{
+		switch ( constraints.getType ( i ) )
+		{
+			case ST_BOUNDED:
+				switch ( constraints.getStatus ( i ) )
+				{
+					case ST_LOWER:
+						lbA[i] = Ax[i];
+						Ax_l[i] = 0.0;
+						ubA[i] = getMax (ubA[i], Ax[i]);
+						Ax_u[i] = ubA[i] - Ax[i];
+						y[i+nV] = getMax (y[i+nV], 0.0);
+						break;
+					case ST_UPPER:
+						lbA[i] = getMin (lbA[i], Ax[i]);
+						Ax_l[i] = Ax[i] - lbA[i];
+						ubA[i] = Ax[i];
+						Ax_u[i] = 0.0;
+						y[i+nV] = getMin (y[i+nV], 0.0);
+						break;
+					case ST_INACTIVE:
+						lbA[i] = getMin (lbA[i], Ax[i]);
+						Ax_l[i] = Ax[i] - lbA[i];
+						ubA[i] = getMax (ubA[i], Ax[i]);
+						Ax_u[i] = ubA[i] - Ax[i];
+						y[i+nV] = 0.0;
+						break;
+					case ST_UNDEFINED:
+					case ST_INFEASIBLE_LOWER:
+					case ST_INFEASIBLE_UPPER:
+						break;
+				}
+				break;
+			case ST_EQUALITY:
+				lbA[i] = Ax[i];
+				Ax_l[i] = 0.0;
+				ubA[i] = Ax[i];
+				Ax_u[i] = 0.0;
+				break;
+			case ST_UNBOUNDED:
+			case ST_UNKNOWN:
+            case ST_DISABLED:
+				break;
+		}
+	}
+
+	return setupAuxiliaryQPgradient( );
+}
+
+/*
+ *	s e t u p A u x i l i a r y Q P
+ */
+returnValue QProblem::setupAuxiliaryQP( const Bounds* const guessedBounds, const Constraints* const guessedConstraints )
+{
+	int i, j;
+	int nV = getNV( );
+	int nC = getNC( );
+
+	/* consistency check */
+	if ( ( guessedBounds == 0 ) || ( guessedConstraints == 0 ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* nothing to do */
+	if ( ( guessedBounds == &bounds ) && ( guessedConstraints == &constraints ) )
+		return SUCCESSFUL_RETURN;
+
+	status = QPS_PREPARINGAUXILIARYQP;
+
+
+	/* I) SETUP WORKING SET ... */
+	if ( shallRefactorise( guessedBounds,guessedConstraints ) == BT_TRUE )
+	{
+		/* ... WITH REFACTORISATION: */
+		/* 1) Reset bounds/constraints ... */
+		bounds.init( nV );
+		constraints.init( nC );
+
+		/*    ... and set them up afresh. */
+		if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+		if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+		if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+		/* 2) Setup TQ factorisation. */
+		if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+		/* 3) Setup guessed working sets afresh. */
+		if ( setupAuxiliaryWorkingSet( guessedBounds,guessedConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
+			THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+		/* 4) Computes Cholesky decomposition of projected Hessian
+		 *    This now handles all special cases (no active bounds/constraints, no nullspace) */
+		if ( computeProjectedCholesky( ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+	}
+	else
+	{
+		/* ... WITHOUT REFACTORISATION: */
+		if ( setupAuxiliaryWorkingSet( guessedBounds,guessedConstraints,BT_FALSE ) != SUCCESSFUL_RETURN )
+			THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+	}
+
+
+	/* II) SETUP AUXILIARY QP DATA: */
+	/* 1) Ensure that dual variable is zero for free bounds and inactive constraints. */
+	for ( i=0; i<nV; ++i )
+		if ( bounds.getStatus( i ) == ST_INACTIVE )
+			y[i] = 0.0;
+
+	for ( i=0; i<nC; ++i )
+		if ( constraints.getStatus( i ) == ST_INACTIVE )
+			y[nV+i] = 0.0;
+
+	/* 2) Setup gradient and (constraints') bound vectors. */
+	if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
+		THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+	A->times(1, 1.0, x, nV, 0.0, Ax, nC);
+	for ( j=0; j<nC; ++j )
+	{
+		Ax_l[j] = Ax[j];
+		Ax_u[j] = Ax[j];
+	}
+
+	/* (also sets Ax_l and Ax_u) */
+	if ( setupAuxiliaryQPbounds( 0,0,BT_FALSE ) != SUCCESSFUL_RETURN )
+		THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s h a l l R e f a c t o r i s e
+ */
+
+BooleanType QProblem::shallRefactorise(	const Bounds* const guessedBounds,
+										const Constraints* const guessedConstraints
+										) const
+{
+	int i;
+	int nV = getNV( );
+	int nC = getNC( );
+
+	/* always refactorise if Hessian is not known to be positive definite */
+	if ( ( hessianType == HST_SEMIDEF ) || ( hessianType == HST_INDEF ) )
+		return BT_TRUE;
+
+	/* 1) Determine number of bounds that have same status
+	 *    in guessed AND current bounds.*/
+	int differenceNumberBounds = 0;
+
+	for( i=0; i<nV; ++i )
+		if ( guessedBounds->getStatus( i ) != bounds.getStatus( i ) )
+			++differenceNumberBounds;
+
+	/* 2) Determine number of constraints that have same status
+	 *    in guessed AND current constraints.*/
+	int differenceNumberConstraints = 0;
+
+	for( i=0; i<nC; ++i )
+		if ( guessedConstraints->getStatus( i ) != constraints.getStatus( i ) )
+			++differenceNumberConstraints;
+
+	/* 3) Decide wheter to refactorise or not. */
+	if ( 2*(differenceNumberBounds+differenceNumberConstraints) > guessedConstraints->getNAC( )+guessedBounds->getNFX( ) )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+/*
+ *	s e t u p Q P d a t a
+ */
+returnValue QProblem::setupQPdata(	SymmetricMatrix *_H, const real_t* const _g, Matrix *_A,
+									const real_t* const _lb, const real_t* const _ub,
+									const real_t* const _lbA, const real_t* const _ubA
+									)
+{
+	int nC = getNC( );
+
+	/* 1) Load Hessian matrix as well as lower and upper bounds vectors. */
+	if ( QProblemB::setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	if ( ( nC > 0 ) && ( _A == 0 ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	if ( nC > 0 )
+	{
+		/* 2) Setup lower/upper constraints' bounds vector. */
+		setLBA( _lbA );
+		setUBA( _ubA );
+
+		/* 3) Only load constraint matrix after setting up vectors! */
+		setA( _A );
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p Q P d a t a
+ */
+returnValue QProblem::setupQPdata(	const real_t* const _H, const real_t* const _g, const real_t* const _A,
+									const real_t* const _lb, const real_t* const _ub,
+									const real_t* const _lbA, const real_t* const _ubA
+									)
+{
+	int nC = getNC( );
+
+	/* 1) Load Hessian matrix as well as lower and upper bounds vectors. */
+	if ( QProblemB::setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	if ( ( nC > 0 ) && ( _A == 0 ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	if ( nC > 0 )
+	{
+		/* 2) Setup lower/upper constraints' bounds vector. */
+		setLBA( _lbA );
+		setUBA( _ubA );
+
+		/* 3) Only load constraint matrix after setting up vectors! */
+		setA( _A );
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p Q P d a t a F r o m F i l e
+ */
+returnValue QProblem::setupQPdataFromFile(	const char* const H_file, const char* const g_file, const char* const A_file,
+											const char* const lb_file, const char* const ub_file,
+											const char* const lbA_file, const char* const ubA_file
+											)
+{
+	int i;
+	int nV = getNV( );
+	int nC = getNC( );
+
+	returnValue returnvalue;
+
+
+	/* 1) Load Hessian matrix as well as lower and upper bounds vectors from files. */
+	returnvalue = QProblemB::setupQPdataFromFile( H_file,g_file,lb_file,ub_file );
+	if ( returnvalue != SUCCESSFUL_RETURN )
+		return THROWERROR( returnvalue );
+
+	if ( ( nC > 0 ) && ( A_file == 0 ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	if ( nC > 0 )
+	{
+		/* 2) Load lower constraints' bounds vector from file. */
+		if ( lbA_file != 0 )
+		{
+			returnvalue = readFromFile( lbA, nC, lbA_file );
+			if ( returnvalue != SUCCESSFUL_RETURN )
+				return THROWERROR( returnvalue );
+		}
+		else
+		{
+			/* if no lower constraints' bounds are specified, set them to -infinity */
+			for( i=0; i<nC; ++i )
+				lbA[i] = -INFTY;
+		}
+
+		/* 3) Load upper constraints' bounds vector from file. */
+		if ( ubA_file != 0 )
+		{
+			returnvalue = readFromFile( ubA, nC, ubA_file );
+			if ( returnvalue != SUCCESSFUL_RETURN )
+				return THROWERROR( returnvalue );
+		}
+		else
+		{
+			/* if no upper constraints' bounds are specified, set them to infinity */
+			for( i=0; i<nC; ++i )
+				ubA[i] = INFTY;
+		}
+
+		/* 4) Only load constraint matrix from file after setting up vectors! */
+		real_t* _A = new real_t[nC * nV];
+		returnvalue = readFromFile( _A, nC,nV, A_file );
+		if ( returnvalue != SUCCESSFUL_RETURN )
+		{
+			delete[] _A;
+			return THROWERROR( returnvalue );
+		}
+		setA( _A );
+		A->doFreeMemory( );
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	l o a d Q P v e c t o r s F r o m F i l e
+ */
+returnValue QProblem::loadQPvectorsFromFile(	const char* const g_file, const char* const lb_file, const char* const ub_file,
+												const char* const lbA_file, const char* const ubA_file,
+												real_t* const g_new, real_t* const lb_new, real_t* const ub_new,
+												real_t* const lbA_new, real_t* const ubA_new
+												) const
+{
+	int nC = getNC( );
+
+	returnValue returnvalue;
+
+
+	/* 1) Load gradient vector as well as lower and upper bounds vectors from files. */
+	returnvalue = QProblemB::loadQPvectorsFromFile( g_file,lb_file,ub_file, g_new,lb_new,ub_new );
+	if ( returnvalue != SUCCESSFUL_RETURN )
+		return THROWERROR( returnvalue );
+
+	if ( nC > 0 )
+	{
+		/* 2) Load lower constraints' bounds vector from file. */
+		if ( lbA_file != 0 )
+		{
+			if ( lbA_new != 0 )
+			{
+				returnvalue = readFromFile( lbA_new, nC, lbA_file );
+				if ( returnvalue != SUCCESSFUL_RETURN )
+					return THROWERROR( returnvalue );
+			}
+			else
+			{
+				/* If filename is given, storage must be provided! */
+				return THROWERROR( RET_INVALID_ARGUMENTS );
+			}
+		}
+
+		/* 3) Load upper constraints' bounds vector from file. */
+		if ( ubA_file != 0 )
+		{
+			if ( ubA_new != 0 )
+			{
+				returnvalue = readFromFile( ubA_new, nC, ubA_file );
+				if ( returnvalue != SUCCESSFUL_RETURN )
+					return THROWERROR( returnvalue );
+			}
+			else
+			{
+				/* If filename is given, storage must be provided! */
+				return THROWERROR( RET_INVALID_ARGUMENTS );
+			}
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	p r i n t I t e r a t i o n
+ */
+returnValue QProblem::printIteration( 	int iter,
+										int BC_idx,	SubjectToStatus BC_status, BooleanType BC_isBound, real_t homotopyLength
+		  								)
+{
+	#ifndef __XPCTARGET__
+
+	/* consistency check */
+	if ( iter < 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	int i;
+	int nV = getNV();
+	int nC = getNC();
+	int nAC = getNAC();
+
+	real_t stat, bfeas, cfeas, bcmpl, ccmpl, Tmaxomin;
+	real_t *grad = 0;
+	real_t *AX = 0;
+	real_t Tmin, Tmax;
+
+	char myPrintfString[MAX_STRING_LENGTH];
+	char info[MAX_STRING_LENGTH];
+	const char excStr[] = " ef";
+
+	switch ( options.printLevel )
+	{
+		case PL_DEBUG_ITER:
+			grad = new real_t[nV];
+			AX = new real_t[nC];
+			stat = bfeas = cfeas = bcmpl = ccmpl = Tmaxomin = 0.0;
+
+			/* stationarity */
+			for (i = 0; i < nV; i++) grad[i] = g[i] - y[i];
+			H->times(1, 1.0, x, nV, 1.0, grad, nV);
+			A->transTimes(1, -1.0, y+nV, nC, 1.0, grad, nV);
+			for (i = 0; i < nV; i++) if (getAbs(grad[i]) > stat) stat = getAbs(grad[i]);
+
+			/* feasibility */
+			for (i = 0; i < nV; i++) if (lb[i] - x[i] > bfeas) bfeas = lb[i] - x[i];
+			for (i = 0; i < nV; i++) if (x[i] - ub[i] > bfeas) bfeas = x[i] - ub[i];
+			A->times(1, 1.0, x, nV, 0.0, AX, nC);
+			for (i = 0; i < nC; i++) if (lbA[i] - AX[i] > cfeas) cfeas = lbA[i] - AX[i];
+			for (i = 0; i < nC; i++) if (AX[i] - ubA[i] > cfeas) cfeas = AX[i] - ubA[i];
+
+			/* complementarity */
+			for (i = 0; i < nV; i++) if (y[i] > +EPS && getAbs((lb[i] - x[i])*y[i]) > bcmpl) bcmpl = getAbs((lb[i] - x[i])*y[i]);
+			for (i = 0; i < nV; i++) if (y[i] < -EPS && getAbs((ub[i] - x[i])*y[i]) > bcmpl) bcmpl = getAbs((ub[i] - x[i])*y[i]);
+			for (i = 0; i < nC; i++) if (y[nV+i] > +EPS && getAbs((lbA[i]-AX[i])*y[nV+i]) > ccmpl) ccmpl = getAbs((lbA[i]-AX[i])*y[nV+i]);
+			for (i = 0; i < nC; i++) if (y[nV+i] < -EPS && getAbs((ubA[i]-AX[i])*y[nV+i]) > ccmpl) ccmpl = getAbs((ubA[i]-AX[i])*y[nV+i]);
+
+			Tmin = 1.0e16; Tmax = 0.0;
+			for (i = 0; i < nAC; i++)
+				if (getAbs(TT(i,sizeT-i-1)) < Tmin)
+					Tmin = getAbs(TT(i,sizeT-i-1));
+				else if (getAbs(TT(i,sizeT-i-1)) > Tmax)
+					Tmax = getAbs(TT(i,sizeT-i-1));
+			Tmaxomin = Tmax/Tmin;
+
+			if (iter % 10 == 0)
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH, "\n%5s %4s %4s %4s %4s %9s %9s %9s %9s %9s %9s %9s %9s\n",
+						"iter", "addB", "remB", "addC", "remC", "hom len", "tau", "stat",
+						"bfeas", "cfeas", "bcmpl", "ccmpl", "Tmin");
+				myPrintf( myPrintfString );
+			}
+
+			snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d ", iter);
+			myPrintf( myPrintfString );
+
+			if (tabularOutput.idxAddB >= 0)
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH, "%4d ", tabularOutput.idxAddB);
+				myPrintf( myPrintfString );
+			}
+			else
+			{
+				myPrintf( "     " );
+			}
+
+			if (tabularOutput.idxRemB >= 0)
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH, "%4d ", tabularOutput.idxRemB);
+				myPrintf( myPrintfString );
+			}
+			else
+			{
+				myPrintf( "     " );
+			}
+
+			if (tabularOutput.idxAddC >= 0)
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH, "%4d ", tabularOutput.idxAddC);
+				myPrintf( myPrintfString );
+			}
+			else
+			{
+				myPrintf( "     " );
+			}
+
+			if (tabularOutput.idxRemC >= 0)
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH, "%4d ", tabularOutput.idxRemC);
+				myPrintf( myPrintfString );
+			}
+			else
+			{
+				myPrintf( "     " );
+			}
+
+			snprintf( myPrintfString,MAX_STRING_LENGTH, "%9.2e %9.2e %9.2e %9.2e %9.2e %9.2e %9.2e %9.2e\n",
+					homotopyLength, tau, stat, bfeas, cfeas, bcmpl, ccmpl, Tmin);
+			myPrintf( myPrintfString );
+
+			delete[] AX;
+			delete[] grad;
+			break;
+
+		case PL_TABULAR:
+			if (iter % 10 == 0)
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH, "\n%5s %6s %6s %6s %6s %9s %9s\n",
+						"iter", "addB", "remB", "addC", "remC", "hom len", "tau" );
+				myPrintf( myPrintfString );
+			}
+			snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d ",iter);
+			myPrintf( myPrintfString );
+
+			if (tabularOutput.idxAddB >= 0)
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d%c ", tabularOutput.idxAddB, excStr[tabularOutput.excAddB]);
+				myPrintf( myPrintfString );
+			}
+			else
+			{
+				myPrintf( "       " );
+			}
+
+			if (tabularOutput.idxRemB >= 0)
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d%c ", tabularOutput.idxRemB, excStr[tabularOutput.excRemB]);
+				myPrintf( myPrintfString );
+			}
+			else 
+			{
+				myPrintf( "       " );
+			}
+
+			if (tabularOutput.idxAddC >= 0) 
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d%c ", tabularOutput.idxAddC, excStr[tabularOutput.excAddC]);
+				myPrintf( myPrintfString );
+			}
+			else 
+			{
+				myPrintf( "       " );
+			}
+
+			if (tabularOutput.idxRemC >= 0) 
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d%c ", tabularOutput.idxRemC, excStr[tabularOutput.excRemC]);
+				myPrintf( myPrintfString );
+			}
+			else 
+			{
+				myPrintf( "       " );
+			}
+
+			snprintf( myPrintfString,MAX_STRING_LENGTH, "%9.2e %9.2e\n", homotopyLength, tau);
+			myPrintf( myPrintfString );
+			break;
+
+		case PL_MEDIUM:
+			/* 1) Print header at first iteration. */
+ 			if ( iter == 0 )
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH,"\n\n####################   qpOASES  --  QP NO. %3.0d   #####################\n\n", count );
+				myPrintf( myPrintfString );
+
+				myPrintf( "    Iter   |    StepLength    |       Info       |   nFX   |   nAC    \n" );
+				myPrintf( " ----------+------------------+------------------+---------+--------- \n" );
+			}
+
+			/* 2) Print iteration line. */
+			if ( BC_status == ST_UNDEFINED )
+			{
+				if ( hessianType == HST_ZERO )
+					snprintf( info,3,"LP" );
+				else
+					snprintf( info,3,"QP" );
+
+				snprintf( myPrintfString,MAX_STRING_LENGTH,"   %5.1d   |   %1.6e   |    %s SOLVED     |  %4.1d   |  %4.1d   \n", iter,tau,info,getNFX( ),getNAC( ) );
+				myPrintf( myPrintfString );
+			}
+			else
+			{
+				if ( BC_status == ST_INACTIVE )
+					snprintf( info,5,"REM " );
+				else
+					snprintf( info,5,"ADD " );
+
+				if ( BC_isBound == BT_TRUE )
+					snprintf( &(info[4]),4,"BND" );
+				else
+					snprintf( &(info[4]),4,"CON" );
+
+				snprintf( myPrintfString,MAX_STRING_LENGTH,"   %5.1d   |   %1.6e   |   %s %4.1d   |  %4.1d   |  %4.1d   \n", iter,tau,info,BC_idx,getNFX( ),getNAC( ) );
+				myPrintf( myPrintfString );
+			}
+			break;
+
+		default:
+			/* nothing to display */
+			break;
+	}
+
+	#endif /* __XPCTARGET__ */
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ * d r o p I n f e a s i b l e s
+ */
+returnValue QProblem::dropInfeasibles( int BC_number, SubjectToStatus BC_status, BooleanType BC_isBound,
+										real_t *xiB, real_t *xiC )
+{
+	int i;
+	
+	int nAC                   = getNAC ();
+	int nFX                   = getNFX ();	
+	int blockingPriority      = (BC_isBound) ? options.dropBoundPriority : options.dropIneqConPriority;
+	int y_min_number          = -1;
+	BooleanType y_min_isBound = BC_isBound;
+	int y_min_priority        = blockingPriority;
+
+	int* AC_idx;
+	constraints.getActive( )->getNumberArray( &AC_idx );
+
+	int* FX_idx;
+	bounds.getFixed( )->getNumberArray( &FX_idx );
+	
+	if (options.dropEqConPriority <= y_min_priority) 
+	{
+		// look for an equality constraint we can drop according to priorities
+		for ( i = 0; i < nAC; ++i )
+			if ( (constraints.getType (i) == ST_EQUALITY)
+				&& (getAbs (xiC[i]) > options.epsDen) ) 
+			{
+				y_min_number = AC_idx[i];
+				y_min_isBound = BT_FALSE;
+				y_min_priority = options.dropEqConPriority;
+				break;
+			}
+	}
+	
+	if (options.dropIneqConPriority <= y_min_priority) 
+	{
+		// look for an inequality constraint we can drop according to priorities
+		for ( i = 0; i < nAC; ++i )
+			if ( (constraints.getType (i) == ST_BOUNDED)
+				&& (getAbs (xiC[i]) > options.epsDen) ) 
+			{
+				y_min_number = AC_idx[i];
+				y_min_isBound = BT_FALSE;
+				y_min_priority = options.dropIneqConPriority;
+				break;
+			}
+	}
+	
+	if (options.dropBoundPriority <= y_min_priority) 
+	{
+		// look for a simple bound we can drop according to priorities
+		for ( i = 0; i < nFX; ++i )
+			if (getAbs (xiB[i]) > options.epsDen) 
+			{
+				y_min_number = FX_idx[i];
+				y_min_isBound = BT_TRUE;
+				y_min_priority = options.dropBoundPriority;
+				break;
+			}
+	}
+
+	if (y_min_number >= 0) {
+
+		// drop active equality or active bound we have found
+		if (y_min_isBound) {
+			SubjectToStatus status_ = bounds.getStatus (y_min_number);
+			removeBound (y_min_number, BT_TRUE, BT_FALSE, BT_FALSE);
+			bounds.setStatus (y_min_number, (status_ == ST_LOWER) ? ST_INFEASIBLE_LOWER : ST_INFEASIBLE_UPPER);
+			// TODO: fix duals y[]
+			/* fprintf (stdFile, "Dropping bounds %d for %s %d\n", y_min_number, BC_isBound?"bound":"constraint", BC_number); */
+		} else {
+			SubjectToStatus status_ = constraints.getStatus (y_min_number);
+			removeConstraint (y_min_number, BT_TRUE, BT_FALSE, BT_FALSE);
+			constraints.setStatus (y_min_number, (status_ == ST_LOWER) ? ST_INFEASIBLE_LOWER : ST_INFEASIBLE_UPPER);
+			// TODO: fix duals y[]
+			/* fprintf (stdFile, "Dropping constraint %d for %s %d\n", y_min_number, BC_isBound?"bound":"constraint", BC_number); */
+		}
+
+		// ... now return, add the blocking constraint, and continue solving QP with dropped bound/constraint
+		return SUCCESSFUL_RETURN;
+
+	} else {
+
+		// nothing found, then drop the blocking (still inactive) constraint
+		if (BC_isBound)
+			bounds.setStatus (BC_number, (BC_status == ST_LOWER) ? ST_INFEASIBLE_LOWER : ST_INFEASIBLE_UPPER);
+		else
+			constraints.setStatus (BC_number, (BC_status == ST_LOWER) ? ST_INFEASIBLE_LOWER : ST_INFEASIBLE_UPPER);
+
+		/* fprintf (stdFile, "Dropping %s %d itself\n", BC_isBound?"bound":"constraint", BC_number); */
+
+		// ... now return, and continue solving QP with dropped bound/constraint
+		return RET_ENSURELI_DROPPED;
+	}	
+}
+
+
+
+/*
+ *  w r i t e Q p D a t a I n t o M a t F i l e
+ */
+returnValue QProblem::writeQpDataIntoMatFile(	const char* const filename
+												) const
+{
+	#ifndef __XPCTARGET__
+	FILE* matFile;
+	matFile = fopen( filename,"w+" );
+
+	if ( matFile == 0 )
+		return RET_UNABLE_TO_OPEN_FILE;
+
+	int nV = getNV();
+	int nC = getNC();
+
+	real_t* Hfull = H->full();
+	writeIntoMatFile( matFile, Hfull, nV,nV, "H"   );
+	delete[] Hfull;
+
+	writeIntoMatFile( matFile, g,     nV,1,  "g"   );
+	
+	real_t* Afull = A->full();
+	writeIntoMatFile( matFile, Afull, nC,nV, "A"   );
+	delete[] Afull;
+	
+	writeIntoMatFile( matFile, lb,    nV,1,  "lb"  );
+	writeIntoMatFile( matFile, ub,    nV,1,  "ub"  );
+	writeIntoMatFile( matFile, lbA,   nC,1,  "lbA" );
+	writeIntoMatFile( matFile, ubA,   nC,1,  "ubA" );
+
+	fclose( matFile );
+
+	return SUCCESSFUL_RETURN;
+	#else /* __XPCTARGET__ */
+
+	return RET_NOT_YET_IMPLEMENTED;
+
+	#endif /* __XPCTARGET__ */
+}
+
+
+/*
+ *  w r i t e Q p W o r k s p a c e I n t o M a t F i l e
+ */
+returnValue QProblem::writeQpWorkspaceIntoMatFile(	const char* const filename
+													)
+{
+	#ifndef __XPCTARGET__
+	FILE* matFile;
+	matFile = fopen( filename,"w+" );
+
+	if ( matFile == 0 )
+		return RET_UNABLE_TO_OPEN_FILE;
+
+	int nV = getNV();
+	int nC = getNC();
+	int nFR  = getNFR();
+	int nFX  = getNFX();
+	int nAC  = getNAC();
+	int nIAC = getNIAC();
+	
+
+	writeIntoMatFile( matFile, T, sizeT,sizeT, "T" );
+	writeIntoMatFile( matFile, Q, nV,nV, "Q" );
+
+	writeIntoMatFile( matFile, Ax, nC,1, "Ax" );
+	writeIntoMatFile( matFile, Ax_l, nC,1, "Ax_l" );
+	writeIntoMatFile( matFile, Ax_u, nC,1, "Ax_u" );
+
+	
+	int *FR_idx, *FX_idx, *AC_idx, *IAC_idx;
+	bounds.getFree( )->getNumberArray( &FR_idx );
+	bounds.getFixed( )->getNumberArray( &FX_idx );
+	constraints.getActive( )->getNumberArray( &AC_idx );
+	constraints.getInactive( )->getNumberArray( &IAC_idx );
+
+	writeIntoMatFile( matFile, FR_idx,  nFR, 1, "FR_idx"  );
+	writeIntoMatFile( matFile, FX_idx,  nFX, 1, "FX_idx"  );
+	writeIntoMatFile( matFile, AC_idx,  nAC, 1, "AC_idx"  );
+	writeIntoMatFile( matFile, IAC_idx, nIAC,1, "IAC_idx" );
+
+	fclose( matFile );
+
+	return SUCCESSFUL_RETURN;
+	#else /* __XPCTARGET__ */
+
+	return RET_NOT_YET_IMPLEMENTED;
+
+	#endif /* __XPCTARGET__ */
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/QProblemB.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/QProblemB.cpp
new file mode 100644
index 00000000000..6366a304189
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/QProblemB.cpp
@@ -0,0 +1,3862 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file src/QProblemB.cpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of the QProblemB class which is able to use the newly
+ *	developed online active set strategy for parametric quadratic programming.
+ */
+
+
+#include <qpOASES/QProblemB.hpp>
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+/*
+ *	Q P r o b l e m B
+ */
+QProblemB::QProblemB( )
+{
+	/* print copyright notice */
+	if (options.printLevel != PL_NONE)
+		printCopyrightNotice( );
+
+	/* reset global message handler */
+	getGlobalMessageHandler( )->reset( );
+
+	freeHessian = BT_FALSE;
+	H = 0;
+
+	g = 0;
+	lb = 0;
+	ub = 0;
+
+	R = 0;
+	haveCholesky = BT_FALSE;
+
+	x = 0;
+	y = 0;
+
+	tau = 0.0;
+
+	hessianType = HST_UNKNOWN;
+	regVal = 0.0;
+
+	infeasible  = BT_FALSE;
+	unbounded   = BT_FALSE;
+
+	status = QPS_NOTINITIALISED;
+
+	count = 0;
+
+	ramp0 = options.initialRamping;
+	ramp1 = options.finalRamping;
+	rampOffset = 0;
+
+	delta_xFR_TMP = 0;
+
+	setPrintLevel( options.printLevel );
+}
+
+
+/*
+ *	Q P r o b l e m B
+ */
+QProblemB::QProblemB( int _nV, HessianType _hessianType )
+{
+	int i;
+
+	/* print copyright notice */
+	if (options.printLevel != PL_NONE)
+		printCopyrightNotice( );
+
+	/* consistency check */
+	if ( _nV <= 0 )
+	{
+		_nV = 1;
+		THROWERROR( RET_INVALID_ARGUMENTS );
+	}
+
+	/* reset global message handler */
+	getGlobalMessageHandler( )->reset( );
+
+	freeHessian = BT_FALSE;
+	H = 0;
+
+	g = new real_t[_nV];
+	for( i=0; i<_nV; ++i ) g[i] = 0.0;
+
+	lb = new real_t[_nV];
+	for( i=0; i<_nV; ++i ) lb[i] = 0.0;
+
+	ub = new real_t[_nV];
+	for( i=0; i<_nV; ++i ) ub[i] = 0.0;
+
+	bounds.init( _nV );
+
+	R = new real_t[_nV*_nV];
+	haveCholesky = BT_FALSE;
+
+	x = new real_t[_nV];
+	for( i=0; i<_nV; ++i ) x[i] = 0.0;
+
+	y = new real_t[_nV];
+	for( i=0; i<_nV; ++i ) y[i] = 0.0;
+
+	tau = 0.0;
+
+	hessianType = _hessianType;
+	regVal = 0.0;
+
+	infeasible  = BT_FALSE;
+	unbounded   = BT_FALSE;
+
+	status = QPS_NOTINITIALISED;
+
+	count = 0;
+
+	ramp0 = options.initialRamping;
+	ramp1 = options.finalRamping;
+	rampOffset = 0;
+
+	delta_xFR_TMP = new real_t[_nV];
+
+	setPrintLevel( options.printLevel );
+
+	flipper.init( (unsigned int)_nV );
+}
+
+
+/*
+ *	Q P r o b l e m B
+ */
+QProblemB::QProblemB( const QProblemB& rhs )
+{
+	freeHessian = BT_FALSE;
+	H = 0;
+
+	copy( rhs );
+}
+
+
+/*
+ *	~ Q P r o b l e m B
+ */
+QProblemB::~QProblemB( )
+{
+	clear( );
+
+	/* reset global message handler */
+	getGlobalMessageHandler( )->reset( );
+}
+
+
+/*
+ *	o p e r a t o r =
+ */
+QProblemB& QProblemB::operator=( const QProblemB& rhs )
+{
+	if ( this != &rhs )
+	{
+		clear( );
+		copy( rhs );
+	}
+
+	return *this;
+}
+
+
+/*
+ *	r e s e t
+ */
+returnValue QProblemB::reset( )
+{
+	int i;
+	int nV = getNV( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* 1) Reset bounds. */
+	bounds.init( nV );
+
+	/* 2) Reset Cholesky decomposition. */
+	for( i=0; i<nV*nV; ++i )
+		R[i] = 0.0;
+	
+	haveCholesky = BT_FALSE;
+
+	/* 3) Reset steplength and status flags. */
+	tau = 0.0;
+
+	hessianType = HST_UNKNOWN;
+	regVal = 0.0;
+
+	infeasible  = BT_FALSE;
+	unbounded   = BT_FALSE;
+
+	status = QPS_NOTINITIALISED;
+
+	ramp0 = options.initialRamping;
+	ramp1 = options.finalRamping;
+	rampOffset = 0;
+
+	/* 4) Reset flipper object */
+	flipper.init( (unsigned int)nV );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	i n i t
+ */
+returnValue QProblemB::init(	SymmetricMatrix *_H, const real_t* const _g,
+								const real_t* const _lb, const real_t* const _ub,
+								int& nWSR, real_t* const cputime
+								)
+{
+	if ( getNV( ) == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* 1) Consistency check. */
+	if ( isInitialised( ) == BT_TRUE )
+	{
+		THROWWARNING( RET_QP_ALREADY_INITIALISED );
+		reset( );
+	}
+
+	/* 2) Setup QP data. */
+	if ( setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* 3) Call to main initialisation routine (without any additional information). */
+	return solveInitialQP( 0,0,0, nWSR,cputime );
+}
+
+
+/*
+ *	i n i t
+ */
+returnValue QProblemB::init(	const real_t* const _H, const real_t* const _g,
+								const real_t* const _lb, const real_t* const _ub,
+								int& nWSR, real_t* const cputime
+								)
+{
+	if ( getNV( ) == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* 1) Consistency check. */
+	if ( isInitialised( ) == BT_TRUE )
+	{
+		THROWWARNING( RET_QP_ALREADY_INITIALISED );
+		reset( );
+	}
+
+	/* 2) Setup QP data. */
+	if ( setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* 3) Call to main initialisation routine (without any additional information). */
+	return solveInitialQP( 0,0,0, nWSR,cputime );
+}
+
+
+/*
+ *	i n i t
+ */
+returnValue QProblemB::init(	const char* const H_file, const char* const g_file,
+								const char* const lb_file, const char* const ub_file,
+								int& nWSR, real_t* const cputime
+								)
+{
+	if ( getNV( ) == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* 1) Consistency check. */
+	if ( isInitialised( ) == BT_TRUE )
+	{
+		THROWWARNING( RET_QP_ALREADY_INITIALISED );
+		reset( );
+	}
+
+	/* 2) Setup QP data from files. */
+	if ( setupQPdataFromFile( H_file,g_file,lb_file,ub_file ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_UNABLE_TO_READ_FILE );
+
+	/* 3) Call to main initialisation routine (without any additional information). */
+	return solveInitialQP( 0,0,0, nWSR,cputime );
+}
+
+
+/*
+ *	i n i t
+ */
+returnValue QProblemB::init( 	SymmetricMatrix *_H, const real_t* const _g,
+								const real_t* const _lb, const real_t* const _ub,
+								int& nWSR, real_t* const cputime,
+								const real_t* const xOpt, const real_t* const yOpt,
+								const Bounds* const guessedBounds
+								)
+{
+	int i;
+	int nV = getNV( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* 1) Consistency checks. */
+	if ( isInitialised( ) == BT_TRUE )
+	{
+		THROWWARNING( RET_QP_ALREADY_INITIALISED );
+		reset( );
+	}
+
+	if ( guessedBounds != 0 )
+	{
+		for( i=0; i<nV; ++i )
+		{
+			if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
+				return THROWERROR( RET_INVALID_ARGUMENTS );
+		}
+	}
+
+	/* exclude this possibility in order to avoid inconsistencies */
+	if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( guessedBounds != 0 ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* 2) Setup QP data. */
+	if ( setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* 3) Call to main initialisation routine. */
+	return solveInitialQP( xOpt,yOpt,guessedBounds, nWSR,cputime );
+}
+
+
+/*
+ *	i n i t
+ */
+returnValue QProblemB::init( 	const real_t* const _H, const real_t* const _g,
+								const real_t* const _lb, const real_t* const _ub,
+								int& nWSR, real_t* const cputime,
+								const real_t* const xOpt, const real_t* const yOpt,
+								const Bounds* const guessedBounds
+								)
+{
+	int i;
+	int nV = getNV( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* 1) Consistency checks. */
+	if ( isInitialised( ) == BT_TRUE )
+	{
+		THROWWARNING( RET_QP_ALREADY_INITIALISED );
+		reset( );
+	}
+
+	if ( guessedBounds != 0 )
+	{
+		for( i=0; i<nV; ++i )
+		{
+			if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
+				return THROWERROR( RET_INVALID_ARGUMENTS );
+		}
+	}
+
+	/* exclude this possibility in order to avoid inconsistencies */
+	if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( guessedBounds != 0 ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* 2) Setup QP data. */
+	if ( setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* 3) Call to main initialisation routine. */
+	return solveInitialQP( xOpt,yOpt,guessedBounds, nWSR,cputime );
+}
+
+
+/*
+ *	i n i t
+ */
+returnValue QProblemB::init( 	const char* const H_file, const char* const g_file,
+								const char* const lb_file, const char* const ub_file,
+								int& nWSR, real_t* const cputime,
+								const real_t* const xOpt, const real_t* const yOpt,
+								const Bounds* const guessedBounds
+								)
+{
+	int i;
+	int nV = getNV( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* 1) Consistency checks. */
+	if ( isInitialised( ) == BT_TRUE )
+	{
+		THROWWARNING( RET_QP_ALREADY_INITIALISED );
+		reset( );
+	}
+
+	for( i=0; i<nV; ++i )
+	{
+		if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
+			return THROWERROR( RET_INVALID_ARGUMENTS );
+	}
+
+	/* exclude this possibility in order to avoid inconsistencies */
+	if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( guessedBounds != 0 ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	/* 2) Setup QP data from files. */
+	if ( setupQPdataFromFile( H_file,g_file,lb_file,ub_file ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_UNABLE_TO_READ_FILE );
+
+	/* 3) Call to main initialisation routine. */
+	return solveInitialQP( xOpt,yOpt,guessedBounds, nWSR,cputime );
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+returnValue QProblemB::hotstart(	const real_t* const g_new,
+									const real_t* const lb_new, const real_t* const ub_new,
+									int& nWSR, real_t* const cputime
+									)
+{
+	int i, nActiveFar;
+	int nV = getNV ();
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	returnValue returnvalue = SUCCESSFUL_RETURN;
+
+	/* Simple check for consistency of bounds */
+	if ( areBoundsConsistent(lb_new, ub_new) != SUCCESSFUL_RETURN )
+		return setInfeasibilityFlag(returnvalue,BT_TRUE);
+
+	++count;
+
+
+	int nWSR_max = nWSR;
+	int nWSR_performed = 0;
+
+	real_t cputime_remaining = INFTY;
+	real_t cputime_needed = 0.0;
+
+	real_t farbound = options.initialFarBounds;
+
+	if ( haveCholesky == BT_FALSE )
+	{
+		returnvalue = setupInitialCholesky( );
+		if (returnvalue != SUCCESSFUL_RETURN)
+			return THROWERROR(returnvalue);
+	}
+
+	if ( options.enableFarBounds == BT_FALSE )
+	{
+		/* Automatically call standard solveQP if regularisation is not active. */
+		returnvalue = solveRegularisedQP( g_new,lb_new,ub_new, nWSR,cputime,0 );
+	}
+	else
+	{
+		real_t *ub_new_far = new real_t[nV];
+		real_t *lb_new_far = new real_t[nV];
+
+		/* possibly extend initial far bounds to largest bound/constraint data */
+		if (ub_new)
+			for (i = 0; i < nV; i++)
+				if ((ub_new[i] < INFTY) && (ub_new[i] > farbound)) farbound = ub_new[i];
+		if (lb_new)
+			for (i = 0; i < nV; i++)
+				if ((lb_new[i] > -INFTY) && (lb_new[i] < -farbound)) farbound = -lb_new[i];
+
+		updateFarBounds(	farbound,nV,
+							lb_new,lb_new_far, ub_new,ub_new_far
+							);
+
+		for ( ;; )
+		{
+			nWSR = nWSR_max;
+			if ( cputime != 0 )
+				cputime_remaining = *cputime - cputime_needed;
+
+			/* Automatically call standard solveQP if regularisation is not active. */
+			returnvalue = solveRegularisedQP( g_new,lb_new_far,ub_new_far, nWSR,&cputime_remaining,nWSR_performed );
+
+			nWSR_performed  = nWSR;
+			cputime_needed += cputime_remaining;
+
+			/* Check for active far-bounds and move them away */
+			nActiveFar = 0;
+			farbound *= options.growFarBounds;
+
+			if ( infeasible == BT_TRUE )
+			{
+				if ( farbound >= INFTY )
+				{
+					returnvalue = RET_HOTSTART_STOPPED_INFEASIBILITY;
+					goto farewell;
+				}
+
+				updateFarBounds(	farbound,nV,
+									lb_new,lb_new_far, ub_new,ub_new_far
+									);
+			}
+			else if ( status == QPS_SOLVED )
+			{
+				real_t tol = farbound/options.growFarBounds * options.boundTolerance;
+				nActiveFar = 0;
+				for ( i=0; i<nV; ++i )
+				{
+					if ( ( ( lb_new == 0 ) || ( lb_new_far[i] > lb_new[i] ) ) && ( getAbs ( lb_new_far[i] - x[i] ) < tol ) )
+						++nActiveFar;
+					if ( ( ( ub_new == 0 ) || ( ub_new_far[i] < ub_new[i] ) ) && ( getAbs ( ub_new_far[i] - x[i] ) < tol ) )
+						++nActiveFar;
+				}
+
+				if ( nActiveFar == 0 )
+					break;
+
+				status = QPS_HOMOTOPYQPSOLVED;
+
+				if ( farbound >= INFTY )
+				{
+					unbounded = BT_TRUE;
+					returnvalue = RET_HOTSTART_STOPPED_UNBOUNDEDNESS;
+					goto farewell;
+				}
+
+				updateFarBounds(	farbound,nV,
+									lb_new,lb_new_far, ub_new,ub_new_far
+									);
+			}
+			else
+			{
+				/* some other error when solving QP */
+				break;
+			}
+
+			/* advance ramp offset to avoid Ramping cycles */
+			rampOffset++;
+		}
+
+		farewell:
+			if ( cputime != 0 )
+				*cputime = cputime_needed;
+			delete[] lb_new_far; delete[] ub_new_far;
+	}
+
+	return ( returnvalue != SUCCESSFUL_RETURN ) ? THROWERROR( returnvalue ) : returnvalue;
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+returnValue QProblemB::hotstart(	const char* const g_file,
+									const char* const lb_file, const char* const ub_file,
+									int& nWSR, real_t* const cputime
+									)
+{
+	int nV  = getNV( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* consistency check */
+	if ( g_file == 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+
+	/* 1) Allocate memory (if bounds exist). */
+	real_t* g_new  = new real_t[nV];
+	real_t* lb_new = 0;
+	real_t* ub_new = 0;
+
+	if ( lb_file != 0 )
+		lb_new = new real_t[nV];
+	if ( ub_file != 0 )
+		ub_new = new real_t[nV];
+
+	/* 2) Load new QP vectors from file. */
+	returnValue returnvalue;
+	returnvalue = loadQPvectorsFromFile(	g_file,lb_file,ub_file,
+											g_new,lb_new,ub_new
+											);
+	if ( returnvalue != SUCCESSFUL_RETURN )
+	{
+		if ( ub_file != 0 )
+			delete[] ub_new;
+		if ( lb_file != 0 )
+			delete[] lb_new;
+		delete[] g_new;
+
+		return THROWERROR( RET_UNABLE_TO_READ_FILE );
+	}
+
+	/* 3) Actually perform hotstart. */
+	returnvalue = hotstart( g_new,lb_new,ub_new, nWSR,cputime );
+
+	/* 4) Free memory. */
+	if ( ub_file != 0 )
+		delete[] ub_new;
+	if ( lb_file != 0 )
+		delete[] lb_new;
+	delete[] g_new;
+
+	return returnvalue;
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+returnValue QProblemB::hotstart(	const real_t* const g_new,
+									const real_t* const lb_new, const real_t* const ub_new,
+									int& nWSR, real_t* const cputime,
+									const Bounds* const guessedBounds
+									)
+{
+	int nV = getNV( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+
+	/* start runtime measurement */
+	real_t starttime = 0.0;
+	if ( cputime != 0 )
+		starttime = getCPUtime( );
+
+
+	/* 1) Update working set according to guess for working set of bounds. */
+	if ( guessedBounds != 0 )
+	{
+		if ( setupAuxiliaryQP( guessedBounds ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+	}
+	else
+	{
+		/* create empty bounds for setting up auxiliary QP */
+		Bounds emptyBounds( nV );
+		if ( emptyBounds.setupAllFree( ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+		if ( setupAuxiliaryQP( &emptyBounds ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+	}
+
+	/* 2) Perform usual homotopy. */
+
+	/* Allow only remaining CPU time for usual hotstart. */
+	if ( cputime != 0 )
+		*cputime -= getCPUtime( ) - starttime;
+
+	returnValue returnvalue = hotstart( g_new,lb_new,ub_new, nWSR,cputime );
+
+	/* stop runtime measurement */
+	if ( cputime != 0 )
+		*cputime = getCPUtime( ) - starttime;
+
+	return returnvalue;
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+returnValue QProblemB::hotstart(	const char* const g_file,
+									const char* const lb_file, const char* const ub_file,
+									int& nWSR, real_t* const cputime,
+									const Bounds* const guessedBounds
+									)
+{
+	int nV = getNV( );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	/* consistency check */
+	if ( g_file == 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+
+	/* 1) Allocate memory (if bounds exist). */
+	real_t* g_new  = new real_t[nV];
+	real_t* lb_new = 0;
+	real_t* ub_new = 0;
+
+	if ( lb_file != 0 )
+		lb_new = new real_t[nV];
+	if ( ub_file != 0 )
+		ub_new = new real_t[nV];
+
+	/* 2) Load new QP vectors from file. */
+	returnValue returnvalue;
+	returnvalue = loadQPvectorsFromFile(	g_file,lb_file,ub_file,
+											g_new,lb_new,ub_new
+											);
+	if ( returnvalue != SUCCESSFUL_RETURN )
+	{
+		if ( ub_file != 0 )
+			delete[] ub_new;
+		if ( lb_file != 0 )
+			delete[] lb_new;
+		delete[] g_new;
+
+		return THROWERROR( RET_UNABLE_TO_READ_FILE );
+	}
+
+	/* 3) Actually perform hotstart using initialised homotopy. */
+	returnvalue = hotstart(	g_new,lb_new,ub_new, nWSR,cputime,
+							guessedBounds
+							);
+
+	/* 4) Free memory. */
+	if ( ub_file != 0 )
+		delete[] ub_new;
+	if ( lb_file != 0 )
+		delete[] lb_new;
+	delete[] g_new;
+
+	return returnvalue;
+}
+
+
+/*
+ *	g e t W o r k i n g S e t
+ */
+returnValue QProblemB::getWorkingSet( real_t* workingSet )
+{
+	int nV = this->getNV();
+
+	// At which limit is the bound active?
+	for (int i = 0; i < nV; i++) {
+		switch (bounds.getStatus(i)) {
+			case ST_LOWER: workingSet[i] = -1.0; break;
+			case ST_UPPER: workingSet[i] = +1.0; break;
+			default: workingSet[i] = 0.0; break;
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t N Z
+ */
+int QProblemB::getNZ( ) const
+{
+	/* if no constraints are present: nZ=nFR */
+	return getNFR( );
+}
+
+
+/*
+ *	g e t O b j V a l
+ */
+real_t QProblemB::getObjVal( ) const
+{
+	real_t objVal;
+
+	/* calculated optimal objective function value
+	 * only if current QP has been solved */
+	if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
+		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
+		 ( getStatus( ) == QPS_SOLVED ) )
+	{
+		objVal = getObjVal( x );
+	}
+	else
+	{
+		objVal = INFTY;
+	}
+
+	return objVal;
+}
+
+
+/*
+ *	g e t O b j V a l
+ */
+real_t QProblemB::getObjVal( const real_t* const _x ) const
+{
+	int i;
+	int nV = getNV( );
+
+	if ( nV == 0 )
+		return 0.0;
+
+	real_t objVal = 0.0;
+
+	for( i=0; i<nV; ++i )
+		objVal += _x[i]*g[i];
+
+	switch ( hessianType )
+	{
+		case HST_ZERO:
+			break;
+
+		case HST_IDENTITY:
+			for( i=0; i<nV; ++i )
+				objVal += 0.5*_x[i]*_x[i];
+			break;
+
+		default:
+			real_t *Hx = new real_t[nV];
+			H->times(1, 1.0, _x, nV, 0.0, Hx, nV);
+			for( i=0; i<nV; ++i )
+				objVal += 0.5*_x[i]*Hx[i];
+			delete[] Hx;
+			break;
+	}
+
+	/* When using regularisation, the objective function value
+	 * needs to be modified as follows:
+	 * objVal = objVal - 0.5*_x*(Hmod-H)*_x - _x'*(gMod-g)
+	 *        = objVal - 0.5*_x*eps*_x * - _x'*(-eps*_x)
+	 *        = objVal + 0.5*_x*eps*_x */
+	if ( usingRegularisation( ) == BT_TRUE )
+	{
+		for( i=0; i<nV; ++i )
+			objVal += 0.5*_x[i]*regVal*_x[i];
+	}
+
+	return objVal;
+}
+
+
+/*
+ *	g e t P r i m a l S o l u t i o n
+ */
+returnValue QProblemB::getPrimalSolution( real_t* const xOpt ) const
+{
+	int i;
+
+	/* return optimal primal solution vector
+	 * only if current QP has been solved */
+	if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
+		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
+		 ( getStatus( ) == QPS_SOLVED ) )
+	{
+		for( i=0; i<getNV( ); ++i )
+			xOpt[i] = x[i];
+
+		return SUCCESSFUL_RETURN;
+	}
+	else
+	{
+		return RET_QP_NOT_SOLVED;
+	}
+}
+
+
+/*
+ *	g e t D u a l S o l u t i o n
+ */
+returnValue QProblemB::getDualSolution( real_t* const yOpt ) const
+{
+	int i;
+
+	for( i=0; i<getNV( ); ++i )
+		yOpt[i] = y[i];
+
+	/* return optimal dual solution vector
+	 * only if current QP has been solved */
+	if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
+		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
+		 ( getStatus( ) == QPS_SOLVED ) )
+	{
+		return SUCCESSFUL_RETURN;
+	}
+	else
+	{
+		return RET_QP_NOT_SOLVED;
+	}
+}
+
+
+/*
+ *	s e t P r i n t L e v e l
+ */
+returnValue QProblemB::setPrintLevel( PrintLevel _printLevel )
+{
+	#ifndef __XPCTARGET__
+		#ifndef __MATLAB__
+			if ( ( options.printLevel == PL_HIGH ) && ( options.printLevel != _printLevel ) )
+				THROWINFO( RET_PRINTLEVEL_CHANGED );
+		#endif /* __MATLAB__ */
+		options.printLevel = _printLevel;
+	#else
+	options.printLevel = PL_NONE;
+	#endif /* __XPCTARGET__ */
+
+	/* update message handler preferences */
+ 	switch ( options.printLevel )
+ 	{
+ 		case PL_NONE:
+ 			getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_HIDDEN );
+			getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_HIDDEN );
+			getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );
+			break;
+
+		case PL_TABULAR:
+		case PL_LOW:
+			getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );
+			getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_HIDDEN );
+			getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );
+			break;
+
+		case PL_DEBUG_ITER:
+		case PL_MEDIUM:
+			getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );
+			getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_VISIBLE );
+			getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );
+			break;
+
+		default: /* PL_HIGH */
+			getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );
+			getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_VISIBLE );
+			getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_VISIBLE );
+			break;
+ 	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	p r i n t P r o p e r t i e s
+ */
+returnValue QProblemB::printProperties( )
+{
+	#ifndef __XPCTARGET__
+	#ifndef __DSPACE__
+	/* Do not print properties if print level is set to none! */
+	if ( options.printLevel == PL_NONE )
+		return SUCCESSFUL_RETURN;
+
+	char myPrintfString[MAX_STRING_LENGTH];
+
+	myPrintf( "\n#################   qpOASES  --  QP PROPERTIES   #################\n" );
+	myPrintf( "\n" );
+
+	/* 1) Variables properties. */
+	snprintf( myPrintfString,MAX_STRING_LENGTH,  "Number of Variables: %4.1d\n",getNV( ) );
+	myPrintf( myPrintfString );
+
+	if ( bounds.hasNoLower( ) == BT_TRUE )
+			myPrintf( "Variables are not bounded from below.\n" );
+		else
+			myPrintf( "Variables are bounded from below.\n" );
+
+	if ( bounds.hasNoUpper( ) == BT_TRUE )
+			myPrintf( "Variables are not bounded from above.\n" );
+		else
+			myPrintf( "Variables are bounded from above.\n" );
+
+	myPrintf( "\n" );
+
+
+	/* 2) Further properties. */
+	switch ( hessianType )
+	{
+		case HST_ZERO:
+			myPrintf( "Hessian is zero matrix (i.e. actually an LP is solved).\n" );
+			break;
+
+		case HST_IDENTITY:
+			myPrintf( "Hessian is identity matrix.\n" );
+			break;
+
+		case HST_POSDEF:
+			myPrintf( "Hessian matrix is (strictly) positive definite.\n" );
+			break;
+
+		case HST_POSDEF_NULLSPACE:
+			myPrintf( "Hessian matrix is positive definite on null space of active constraints.\n" );
+			break;
+
+		case HST_SEMIDEF:
+			myPrintf( "Hessian matrix is positive semi-definite.\n" );
+			break;
+
+		case HST_INDEF:
+			myPrintf( "Hessian matrix is indefinite.\n" );
+			break;
+
+		default:
+			myPrintf( "Hessian matrix has unknown type.\n" );
+			break;
+	}
+
+	if ( infeasible == BT_TRUE )
+		myPrintf( "QP was found to be infeasible.\n" );
+	else
+		myPrintf( "QP seems to be feasible.\n" );
+
+	if ( unbounded == BT_TRUE )
+		myPrintf( "QP was found to be unbounded from below.\n" );
+	else
+		myPrintf( "QP seems to be bounded from below.\n" );
+
+	myPrintf( "\n" );
+
+
+	/* 3) QP object properties. */
+	switch ( status )
+	{
+		case QPS_NOTINITIALISED:
+			myPrintf( "Status of QP object: freshly instantiated or reset.\n" );
+			break;
+
+		case QPS_PREPARINGAUXILIARYQP:
+			myPrintf( "Status of QP object: an auxiliary QP is currently setup.\n" );
+			break;
+
+		case QPS_AUXILIARYQPSOLVED:
+			myPrintf( "Status of QP object: an auxilary QP was solved.\n" );
+			break;
+
+		case QPS_PERFORMINGHOMOTOPY:
+			myPrintf( "Status of QP object: a homotopy step is performed.\n" );
+			break;
+
+		case QPS_HOMOTOPYQPSOLVED:
+			myPrintf( "Status of QP object: an intermediate QP along the homotopy path was solved.\n" );
+			break;
+
+		case QPS_SOLVED:
+			myPrintf( "Status of QP object: solution of the actual QP was found.\n" );
+			break;
+	}
+
+	switch ( options.printLevel )
+	{
+		case PL_DEBUG_ITER:
+			myPrintf( "Print level of QP object is set to display a tabular output for debugging.\n" );
+			break;
+
+		case PL_TABULAR:
+			myPrintf( "Print level of QP object is set to display a tabular output.\n" );
+			break;
+
+		case PL_LOW:
+					myPrintf( "Print level of QP object is low, i.e. only error are printed.\n" );
+			break;
+
+		case PL_MEDIUM:
+			myPrintf( "Print level of QP object is medium, i.e. error and warnings are printed.\n" );
+			break;
+
+		case PL_HIGH:
+			myPrintf( "Print level of QP object is high, i.e. all available output is printed.\n" );
+			break;
+
+		default:
+			break;
+	}
+
+	myPrintf( "\n" );
+	#endif
+	#endif
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+returnValue QProblemB::printOptions( ) const
+{
+	return options.print( );
+}
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+/*
+ *	c l e a r
+ */
+returnValue QProblemB::clear( )
+{
+	if ( ( freeHessian == BT_TRUE ) && ( H != 0 ) )
+	{
+		delete H;
+		H = 0;
+	}
+
+	if ( g != 0 )
+	{
+		delete[] g;
+		g = 0;
+	}
+
+	if ( lb != 0 )
+	{
+		delete[] lb;
+		lb = 0;
+	}
+
+	if ( ub != 0 )
+	{
+		delete[] ub;
+		ub = 0;
+	}
+
+	if ( R != 0 )
+	{
+		delete[] R;
+		R = 0;
+	}
+
+	if ( x != 0 )
+	{
+		delete[] x;
+		x = 0;
+	}
+
+	if ( y != 0 )
+	{
+		delete[] y;
+		y = 0;
+	}
+
+	if ( delta_xFR_TMP != 0 )
+	{
+		delete[] delta_xFR_TMP;
+		delta_xFR_TMP = 0;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	c o p y
+ */
+returnValue QProblemB::copy(	const QProblemB& rhs
+								)
+{
+	unsigned int _nV = (unsigned int)rhs.getNV( );
+
+	bounds = rhs.bounds;
+
+	freeHessian = rhs.freeHessian;
+	
+	if ( freeHessian == BT_TRUE )
+		H = (SymmetricMatrix *)(rhs.H->duplicateSym());
+	else
+		H = rhs.H;
+
+	if ( rhs.g != 0 )
+	{
+		g = new real_t[_nV];
+		setG( rhs.g );
+	}
+	else
+		g = 0;
+
+	if ( rhs.lb != 0 )
+	{
+		lb = new real_t[_nV];
+		setLB( rhs.lb );
+	}
+	else
+		lb = 0;
+
+	if ( rhs.ub != 0 )
+	{
+		ub = new real_t[_nV];
+		setUB( rhs.ub );
+	}
+	else
+		ub = 0;
+
+	if ( rhs.R != 0 )
+	{
+		R = new real_t[_nV*_nV];
+		memcpy( R,rhs.R,_nV*_nV*sizeof(real_t) );
+	}
+	else
+		R = 0;
+	
+	haveCholesky = rhs.haveCholesky;
+
+	if ( rhs.x != 0 )
+	{
+		x = new real_t[_nV];
+		memcpy( x,rhs.x,_nV*sizeof(real_t) );
+	}
+	else
+		x = 0;
+
+	if ( rhs.y != 0 )
+	{
+		y = new real_t[_nV];
+		memcpy( y,rhs.y,_nV*sizeof(real_t) );
+	}
+	else
+		y = 0;
+
+	tau = rhs.tau;
+
+	hessianType = rhs.hessianType;
+	regVal = rhs.regVal;
+
+	infeasible = rhs.infeasible;
+	unbounded = rhs.unbounded;
+
+	status = rhs.status;
+
+	count = rhs.count;
+
+	ramp0 = rhs.ramp0;
+	ramp1 = rhs.ramp1;
+
+	delta_xFR_TMP = new real_t[_nV];	/* nFR */
+
+	options = rhs.options;
+	setPrintLevel( options.printLevel );
+
+	flipper = rhs.flipper;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	d e t e r m i n e H e s s i a n T y p e
+ */
+returnValue QProblemB::determineHessianType( )
+{
+	int i;
+	int nV = getNV( );
+	real_t curDiag;
+
+	/* if Hessian type has been set by user, do NOT change it! */
+	if ( hessianType != HST_UNKNOWN )
+		return SUCCESSFUL_RETURN;
+
+	/* if Hessian has not been allocated, assume it to be all zeros! */
+	if ( H == 0 )
+	{
+		hessianType = HST_ZERO;
+
+		if ( options.enableRegularisation == BT_FALSE )
+			options.enableRegularisation = BT_TRUE;
+
+		return SUCCESSFUL_RETURN;
+	}
+
+	/* 1) If Hessian has outer-diagonal elements,
+	 *    Hessian is assumed to be positive definite. */
+	hessianType = HST_POSDEF;
+	if (H->isDiag() == BT_FALSE)
+		return SUCCESSFUL_RETURN;
+
+	/* 2) Otherwise it is diagonal and test for identity or zero matrix is performed. */
+	BooleanType isIdentity = BT_TRUE;
+	BooleanType isZero = BT_TRUE;
+
+	for ( i=0; i<nV; ++i )
+	{
+        curDiag = H->diag(i);
+        if ( curDiag >= INFTY )
+            return RET_DIAGONAL_NOT_INITIALISED;
+        
+		if ( curDiag < -ZERO )
+		{
+			hessianType = HST_INDEF;
+			if ( options.enableFlippingBounds == BT_FALSE )
+				return THROWERROR( RET_HESSIAN_INDEFINITE );
+			else
+				return SUCCESSFUL_RETURN;
+		}
+
+		if ( getAbs( curDiag - 1.0 ) > EPS )
+			isIdentity = BT_FALSE;
+
+		if ( getAbs( curDiag ) > EPS )
+			isZero = BT_FALSE;
+	}
+
+	if ( isIdentity == BT_TRUE )
+		hessianType = HST_IDENTITY;
+
+	if ( isZero == BT_TRUE )
+		hessianType = HST_ZERO;
+
+	if ( ( hessianType == HST_ZERO ) && ( options.enableRegularisation == BT_FALSE ) )
+		options.enableRegularisation = BT_TRUE;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p S u b j e c t T o T y p e
+ */
+returnValue QProblemB::setupSubjectToType( )
+{
+	return setupSubjectToType( lb,ub );
+}
+
+
+/*
+ *	s e t u p S u b j e c t T o T y p e
+ */
+returnValue QProblemB::setupSubjectToType( const real_t* const lb_new, const real_t* const ub_new )
+{
+	int i;
+	int nV = getNV( );
+
+
+	/* 1) Check if lower bounds are present. */
+	bounds.setNoLower( BT_TRUE );
+	if ( lb_new != 0 )
+	{
+		for( i=0; i<nV; ++i )
+		{
+			if ( lb_new[i] > -INFTY )
+			{
+				bounds.setNoLower( BT_FALSE );
+				break;
+			}
+		}
+	}
+
+	/* 2) Check if upper bounds are present. */
+	bounds.setNoUpper( BT_TRUE );
+	if ( ub_new != 0 )
+	{
+		for( i=0; i<nV; ++i )
+		{
+			if ( ub_new[i] < INFTY )
+			{
+				bounds.setNoUpper( BT_FALSE );
+				break;
+			}
+		}
+	}
+
+	/* 3) Determine implicitly fixed and unbounded variables. */
+	if ( ( lb_new != 0 ) && ( ub_new != 0 ) )
+	{
+		for( i=0; i<nV; ++i )
+		{
+			if ( ( lb_new[i] <= -INFTY ) && ( ub_new[i] >= INFTY )
+					&& (options.enableFarBounds == BT_FALSE))
+			{
+				bounds.setType( i,ST_UNBOUNDED );
+			}
+			else
+			{
+				if (options.enableEqualities
+						&& lb[i] > ub[i] - options.boundTolerance
+						&& lb_new[i] > ub_new[i] - options.boundTolerance)
+					bounds.setType( i,ST_EQUALITY );
+				else
+					bounds.setType( i,ST_BOUNDED );
+			}
+		}
+	}
+	else
+	{
+		if ( ( lb_new == 0 ) && ( ub_new == 0 ) )
+		{
+			for( i=0; i<nV; ++i )
+				bounds.setType( i,ST_UNBOUNDED );
+		}
+		else
+		{
+			for( i=0; i<nV; ++i )
+				bounds.setType( i,ST_BOUNDED );
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	c o m p u t e C h o l e s k y
+ */
+returnValue QProblemB::computeCholesky( )
+{
+	int i, j;
+	int nV  = getNV( );
+	int nFR = getNFR( );
+	
+	/* 1) Initialises R with all zeros. */
+	for( i=0; i<nV*nV; ++i )
+		R[i] = 0.0;
+
+	/* 2) Calculate Cholesky decomposition of H (projected to free variables). */
+	if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_IDENTITY ) )
+	{
+		if ( hessianType == HST_ZERO )
+		{
+			/* if Hessian is zero matrix and it has been regularised,
+			 * its Cholesky factor is the identity matrix scaled by sqrt(eps). */
+			if ( usingRegularisation( ) == BT_TRUE )
+			{
+				for( i=0; i<nV; ++i )
+					RR(i,i) = getSqrt( regVal );
+			}
+			/*else
+				return THROWERROR( RET_CHOLESKY_OF_ZERO_HESSIAN );*/
+		}
+		else
+		{
+			/* if Hessian is identity, so is its Cholesky factor. */
+			for( i=0; i<nV; ++i )
+				RR(i,i) = 1.0;
+		}
+	}
+	else
+	{
+		if ( nFR > 0 )
+		{
+			int* FR_idx;
+			bounds.getFree( )->getNumberArray( &FR_idx );
+
+			/* get H */
+			for ( j=0; j < nFR; ++j )
+				H->getCol (FR_idx[j], bounds.getFree (), 1.0, &(R[j*nV]) );
+
+			/* R'*R = H */
+			long info = 0;
+			unsigned long _nFR = (unsigned long)nFR, _nV = (unsigned long)nV;
+
+			POTRF( "U", &_nFR, R, &_nV, &info );
+
+			/* <0 = invalid call, =0 ok, >0 not spd */
+			if (info > 0) {
+				if ( R[0] < 0.0 )
+				{
+					/* Cholesky decomposition has tunneled a negative
+					 * diagonal element. */ 
+					options.epsRegularisation = getMin( -R[0]+options.epsRegularisation,getSqrt(getAbs(options.epsRegularisation)) );
+				}
+
+				hessianType = HST_SEMIDEF;
+				return RET_HESSIAN_NOT_SPD;
+			}
+
+			/* zero first subdiagonal to make givens updates work */
+			for (i=0;i<nFR-1;++i)
+				RR(i+1,i) = 0.0;
+
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p I n i t i a l C h o l e s k y
+ */
+returnValue QProblemB::setupInitialCholesky( )
+{
+	returnValue returnvalueCholesky;
+
+	/* If regularisation shall be used, always regularise at beginning 
+	 * if initial working set is not empty. */
+	if ( ( getNV() != getNFR()-getNFV() ) && ( options.enableRegularisation == BT_TRUE ) )
+		if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
+			return RET_INIT_FAILED_REGULARISATION;
+	
+	returnvalueCholesky = computeCholesky( );
+
+	/* If Hessian is not positive definite, regularise and try again. */
+	if ( returnvalueCholesky == RET_HESSIAN_NOT_SPD )
+	{
+		if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
+			return RET_INIT_FAILED_REGULARISATION;
+
+		returnvalueCholesky = computeCholesky( );
+	}
+
+	if ( returnvalueCholesky != SUCCESSFUL_RETURN )
+		return RET_INIT_FAILED_CHOLESKY;
+
+	haveCholesky = BT_TRUE;
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	o b t a i n A u x i l i a r y W o r k i n g S e t
+ */
+returnValue QProblemB::obtainAuxiliaryWorkingSet(	const real_t* const xOpt, const real_t* const yOpt,
+													const Bounds* const guessedBounds, Bounds* auxiliaryBounds
+													) const
+{
+	int i = 0;
+	int nV = getNV( );
+
+
+	/* 1) Ensure that desiredBounds is allocated (and different from guessedBounds). */
+	if ( ( auxiliaryBounds == 0 ) || ( auxiliaryBounds == guessedBounds ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+
+	/* 2) Setup working set for auxiliary initial QP. */
+	if ( guessedBounds != 0 )
+	{
+		/* If an initial working set is specific, use it!
+		 * Moreover, add all implictly fixed variables if specified. */
+		for( i=0; i<nV; ++i )
+		{
+			#ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
+			if ( bounds.getType( i ) == ST_EQUALITY )
+			{
+				if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+			}
+			else
+			#endif
+			{
+				if ( auxiliaryBounds->setupBound( i,guessedBounds->getStatus( i ) ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+			}
+		}
+	}
+	else	/* No initial working set specified. */
+	{
+		if ( ( xOpt != 0 ) && ( yOpt == 0 ) )
+		{
+			/* Obtain initial working set by "clipping". */
+			for( i=0; i<nV; ++i )
+			{
+				if ( xOpt[i] <= lb[i] + options.boundTolerance )
+				{
+					if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+					continue;
+				}
+
+				if ( xOpt[i] >= ub[i] - options.boundTolerance )
+				{
+					if ( auxiliaryBounds->setupBound( i,ST_UPPER ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+					continue;
+				}
+
+				/* Moreover, add all implictly fixed variables if specified. */
+				#ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
+				if ( bounds.getType( i ) == ST_EQUALITY )
+				{
+					if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+				}
+				else
+				#endif
+				{
+					if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+				}
+			}
+		}
+
+		if ( ( xOpt == 0 ) && ( yOpt != 0 ) )
+		{
+			/* Obtain initial working set in accordance to sign of dual solution vector. */
+			for( i=0; i<nV; ++i )
+			{
+				if ( yOpt[i] > EPS )
+				{
+					if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+					continue;
+				}
+
+				if ( yOpt[i] < -EPS )
+				{
+					if ( auxiliaryBounds->setupBound( i,ST_UPPER ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+					continue;
+				}
+
+				/* Moreover, add all implictly fixed variables if specified. */
+				#ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
+				if ( bounds.getType( i ) == ST_EQUALITY )
+				{
+					if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+				}
+				else
+				#endif
+				{
+					if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
+						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+				}
+			}
+		}
+
+		/* If xOpt and yOpt are null pointer and no initial working is specified,
+		 * start with empty working set (or implicitly fixed bounds only)
+		 * for auxiliary QP. */
+		if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
+		{
+			for( i=0; i<nV; ++i )
+			{
+				switch( bounds.getType( i ) )
+				{
+					case ST_UNBOUNDED:
+						if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
+							return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+						break;
+
+					/* Only add all implictly fixed variables if specified. */
+					#ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
+					case ST_EQUALITY:
+						if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
+							return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+						break;
+					#endif
+
+					default:
+						if ( auxiliaryBounds->setupBound( i,options.initialStatusBounds ) != SUCCESSFUL_RETURN )
+							return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
+						break;
+				}
+			}
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	b a c k s o l v e R
+ */
+returnValue QProblemB::backsolveR(	const real_t* const b, BooleanType transposed,
+									real_t* const a
+									) const
+{
+	/* Call standard backsolve procedure (i.e. removingBound == BT_FALSE). */
+	return backsolveR( b,transposed,BT_FALSE,a );
+}
+
+
+/*
+ *	b a c k s o l v e R
+ */
+returnValue QProblemB::backsolveR(	const real_t* const b, BooleanType transposed,
+									BooleanType removingBound,
+									real_t* const a
+									) const
+{
+	int i, j;
+	int nV = getNV( );
+	int nR = getNZ( );
+
+	real_t sum;
+
+	/* if backsolve is called while removing a bound, reduce nZ by one. */
+	if ( removingBound == BT_TRUE )
+		--nR;
+
+	/* nothing to do */
+	if ( nR <= 0 )
+		return SUCCESSFUL_RETURN;
+
+
+	/* Solve Ra = b, where R might be transposed. */
+	if ( transposed == BT_FALSE )
+	{
+		/* solve Ra = b */
+		for( i=(nR-1); i>=0; --i )
+		{
+			sum = b[i];
+			for( j=(i+1); j<nR; ++j )
+				sum -= RR(i,j) * a[j];
+
+			if ( getAbs( RR(i,i) ) >= ZERO*getAbs( sum ) )
+				a[i] = sum / RR(i,i);
+			else
+				return THROWERROR( RET_DIV_BY_ZERO );
+		}
+	}
+	else
+	{
+		/* solve R^T*a = b */
+		for( i=0; i<nR; ++i )
+		{
+			sum = b[i];
+			for( j=0; j<i; ++j )
+				sum -= RR(j,i) * a[j];
+
+			if ( getAbs( RR(i,i) ) >= ZERO*getAbs( sum ) )
+				a[i] = sum / RR(i,i);
+			else
+				return THROWERROR( RET_DIV_BY_ZERO );
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	d e t e r m i n e D a t a S h i f t
+ */
+returnValue QProblemB::determineDataShift(	const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
+											real_t* const delta_g, real_t* const delta_lb, real_t* const delta_ub,
+											BooleanType& Delta_bB_isZero
+											)
+{
+	int i, ii;
+	int nV  = getNV( );
+	int nFX = getNFX( );
+
+	int* FX_idx;
+	bounds.getFixed( )->getNumberArray( &FX_idx );
+
+
+	/* 1) Calculate shift directions. */
+	for( i=0; i<nV; ++i )
+		delta_g[i]  = g_new[i]  - g[i];
+
+	if ( lb_new != 0 )
+	{
+		for( i=0; i<nV; ++i )
+			delta_lb[i] = lb_new[i] - lb[i];
+	}
+	else
+	{
+		/* if no lower bounds exist, assume the new lower bounds to be -infinity */
+		for( i=0; i<nV; ++i )
+			delta_lb[i] = -INFTY - lb[i];
+	}
+
+	if ( ub_new != 0 )
+	{
+		for( i=0; i<nV; ++i )
+			delta_ub[i] = ub_new[i] - ub[i];
+	}
+	else
+	{
+		/* if no upper bounds exist, assume the new upper bounds to be infinity */
+		for( i=0; i<nV; ++i )
+			delta_ub[i] = INFTY - ub[i];
+	}
+
+	/* 2) Determine if active bounds are to be shifted. */
+	Delta_bB_isZero = BT_TRUE;
+
+	for ( i=0; i<nFX; ++i )
+	{
+		ii = FX_idx[i];
+
+		if ( ( getAbs( delta_lb[ii] ) > EPS ) || ( getAbs( delta_ub[ii] ) > EPS ) )
+		{
+			Delta_bB_isZero = BT_FALSE;
+			break;
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	s e t u p Q P d a t a
+ */
+returnValue QProblemB::setupQPdata(	SymmetricMatrix *_H, const real_t* const _g,
+									const real_t* const _lb, const real_t* const _ub
+									)
+{
+	/* 1) Setup Hessian matrix. */
+	setH( _H );
+
+	/* 2) Setup gradient vector. */
+	if ( _g == 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+	else
+		setG( _g );
+
+	/* 3) Setup lower/upper bounds vector. */
+	setLB( _lb );
+	setUB( _ub );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p Q P d a t a
+ */
+returnValue QProblemB::setupQPdata(	const real_t* const _H, const real_t* const _g,
+									const real_t* const _lb, const real_t* const _ub
+									)
+{
+	/* 1) Setup Hessian matrix. */
+	setH( _H );
+
+	/* 2) Setup gradient vector. */
+	if ( _g == 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+	else
+		setG( _g );
+
+	/* 3) Setup lower/upper bounds vector. */
+	setLB( _lb );
+	setUB( _ub );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p Q P d a t a F r o m F i l e
+ */
+returnValue QProblemB::setupQPdataFromFile(	const char* const H_file, const char* const g_file,
+											const char* const lb_file, const char* const ub_file
+											)
+{
+	int i;
+	int nV = getNV( );
+
+	returnValue returnvalue;
+
+
+	/* 1) Load Hessian matrix from file. */
+	if ( H_file != 0 )
+	{
+		real_t* _H = new real_t[nV * nV];
+		returnvalue = readFromFile( _H, nV,nV, H_file );
+		if ( returnvalue != SUCCESSFUL_RETURN )
+		{
+			delete[] _H;
+			return THROWERROR( returnvalue );
+		}
+		setH( _H );
+		H->doFreeMemory( );
+	}
+	else
+	{
+		real_t* _H = 0;
+		setH( _H );
+	}
+
+	/* 2) Load gradient vector from file. */
+	if ( g_file == 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	returnvalue = readFromFile( g, nV, g_file );
+	if ( returnvalue != SUCCESSFUL_RETURN )
+		return THROWERROR( returnvalue );
+
+	/* 3) Load lower bounds vector from file. */
+	if ( lb_file != 0 )
+	{
+		returnvalue = readFromFile( lb, nV, lb_file );
+		if ( returnvalue != SUCCESSFUL_RETURN )
+			return THROWERROR( returnvalue );
+	}
+	else
+	{
+		/* if no lower bounds are specified, set them to -infinity */
+		for( i=0; i<nV; ++i )
+			lb[i] = -INFTY;
+	}
+
+	/* 4) Load upper bounds vector from file. */
+	if ( ub_file != 0 )
+	{
+		returnvalue = readFromFile( ub, nV, ub_file );
+		if ( returnvalue != SUCCESSFUL_RETURN )
+			return THROWERROR( returnvalue );
+	}
+	else
+	{
+		/* if no upper bounds are specified, set them to infinity */
+		for( i=0; i<nV; ++i )
+			ub[i] = INFTY;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	l o a d Q P v e c t o r s F r o m F i l e
+ */
+returnValue QProblemB::loadQPvectorsFromFile(	const char* const g_file, const char* const lb_file, const char* const ub_file,
+												real_t* const g_new, real_t* const lb_new, real_t* const ub_new
+												) const
+{
+	int nV = getNV( );
+
+	returnValue returnvalue;
+
+
+	/* 1) Load gradient vector from file. */
+	if ( ( g_file != 0 ) && ( g_new != 0 ) )
+	{
+		returnvalue = readFromFile( g_new, nV, g_file );
+		if ( returnvalue != SUCCESSFUL_RETURN )
+			return THROWERROR( returnvalue );
+	}
+	else
+	{
+		/* At least gradient vector needs to be specified! */
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+	}
+
+	/* 2) Load lower bounds vector from file. */
+	if ( lb_file != 0 )
+	{
+		if ( lb_new != 0 )
+		{
+			returnvalue = readFromFile( lb_new, nV, lb_file );
+			if ( returnvalue != SUCCESSFUL_RETURN )
+				return THROWERROR( returnvalue );
+		}
+		else
+		{
+			/* If filename is given, storage must be provided! */
+			return THROWERROR( RET_INVALID_ARGUMENTS );
+		}
+	}
+
+	/* 3) Load upper bounds vector from file. */
+	if ( ub_file != 0 )
+	{
+		if ( ub_new != 0 )
+		{
+			returnvalue = readFromFile( ub_new, nV, ub_file );
+			if ( returnvalue != SUCCESSFUL_RETURN )
+				return THROWERROR( returnvalue );
+		}
+		else
+		{
+			/* If filename is given, storage must be provided! */
+			return THROWERROR( RET_INVALID_ARGUMENTS );
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t I n f e a s i b i l i t y F l a g
+ */
+returnValue QProblemB::setInfeasibilityFlag(	returnValue returnvalue,
+												BooleanType doThrowError
+												)
+{
+	infeasible = BT_TRUE;
+
+	if ( ( doThrowError == BT_TRUE ) || ( options.enableFarBounds == BT_FALSE ) )
+		THROWERROR( returnvalue );
+
+	return returnvalue;
+}
+
+
+/*
+ *	a r e B o u n d s C o n s i s t e n t
+ */
+returnValue QProblemB::areBoundsConsistent(	const real_t* const lb_new, const real_t* const ub_new ) const
+{
+	if (lb_new && ub_new) {
+		for (int i = 0; i < getNV(); ++i) {
+			if (lb_new[i] > ub_new[i]+EPS) {
+				return RET_QP_INFEASIBLE;
+			}
+		}
+	}
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	i s C P U t i m e L i m i t E x c e e d e d
+ */
+BooleanType QProblemB::isCPUtimeLimitExceeded(	const real_t* const cputime,
+												real_t starttime,
+												int nWSR
+												) const
+{
+	/* Always perform next QP iteration if no CPU time limit is given. */
+	if ( cputime == 0 )
+		return BT_FALSE;
+
+	/* Always perform first QP iteration. */
+	if ( nWSR <= 0 )
+		return BT_FALSE;
+
+	real_t elapsedTime = getCPUtime( ) - starttime;
+	real_t timePerIteration = elapsedTime / ((real_t) nWSR);
+
+	/* Determine if next QP iteration exceed CPU time limit
+	 * considering the (current) average CPU time per iteration. */
+	if ( ( elapsedTime + timePerIteration*1.25 ) <= ( *cputime ) )
+		return BT_FALSE;
+	else
+		return BT_TRUE;
+}
+
+
+/*
+ *	r e g u l a r i s e H e s s i a n
+ */
+returnValue QProblemB::regulariseHessian( )
+{
+	/* Do nothing if Hessian regularisation is disbaled! */
+	if ( options.enableRegularisation == BT_FALSE )
+		return SUCCESSFUL_RETURN;
+
+	/* Regularisation of identity Hessian not possible. */
+	if ( hessianType == HST_IDENTITY )
+		return THROWERROR( RET_CANNOT_REGULARISE_IDENTITY );
+
+	/* Determine regularisation parameter. */
+	if ( usingRegularisation( ) == BT_TRUE )
+		return SUCCESSFUL_RETURN; /*THROWERROR( RET_HESSIAN_ALREADY_REGULARISED );*/
+	else
+	{
+		/* Regularisation of zero Hessian is done implicitly. */
+		if ( hessianType == HST_ZERO )
+		{
+			regVal = getNorm( g,getNV() ) * options.epsRegularisation;
+		}
+		else
+		{
+			regVal = H->getNorm() * options.epsRegularisation;
+
+			if ( H->addToDiag( regVal ) == RET_NO_DIAGONAL_AVAILABLE )
+				return THROWERROR( RET_CANNOT_REGULARISE_SPARSE );
+		}
+
+		THROWINFO( RET_USING_REGULARISATION );
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *  c r e a t e D i a g S p a r s e M a t
+ */
+SymSparseMat* QProblemB::createDiagSparseMat( int n, real_t diagVal )
+{
+	real_t* M_val = new real_t[n];
+	sparse_int_t* M_jc = new sparse_int_t[n+1];
+	sparse_int_t* M_ir = new sparse_int_t[n+1];
+
+	for( int ii=0; ii<n; ++ii )
+	{
+		M_val[ii] = diagVal;
+		M_jc[ii] = (sparse_int_t)ii;
+		M_ir[ii] = (sparse_int_t)ii;
+	}
+	M_jc[n] = (sparse_int_t)n;
+	M_ir[n] = (sparse_int_t)n;
+
+	SymSparseMat* M = new SymSparseMat( n,n, M_ir,M_jc,M_val );
+	M->createDiagInfo( );
+	M->doFreeMemory( );
+
+	return M;
+}
+
+
+
+/*
+ *	p e r f o r m R a t i o T e s t
+ */
+returnValue QProblemB::performRatioTest(	int nIdx,
+											const int* const idxList,
+											const SubjectTo* const subjectTo,
+											const real_t* const num,
+											const real_t* const den,
+											real_t epsNum,
+											real_t epsDen,
+											real_t& t,
+											int& BC_idx
+											) const
+{
+	int i, ii;
+
+	BC_idx = -1;
+
+	for( i=0; i<nIdx; ++i )
+	{
+		ii = idxList[i];
+
+		if ( subjectTo->getType( ii ) != ST_EQUALITY )
+		{
+			if ( ( subjectTo->getStatus( ii ) == ST_LOWER ) || ( subjectTo->getStatus( ii ) == ST_INACTIVE ) )
+			{
+				if ( isBlocking( num[i],den[i],epsNum,epsDen,t ) == BT_TRUE )
+				{
+					t = num[i] / den[i];
+					BC_idx = ii;
+				}
+			}
+			else
+			if ( subjectTo->getStatus( ii ) == ST_UPPER )
+			{
+				if ( isBlocking( -num[i],-den[i],epsNum,epsDen,t ) == BT_TRUE )
+				{
+					t = num[i] / den[i];
+					BC_idx = ii;
+				}
+			}
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ * g e t R e l a t i v e H o m o t o p y L e n g t h
+ */
+real_t QProblemB::getRelativeHomotopyLength(	const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new
+												)
+{
+	int i;
+	int nV = getNV( );
+	real_t d, s, len = 0.0;
+
+	/* gradient */
+	for (i = 0; i < nV; i++)
+	{
+		s = getAbs(g_new[i]);
+		if (s < 1.0) s = 1.0;
+		d = getAbs(g_new[i] - g[i]) / s;
+		if (d > len) len = d;
+	}
+
+	/* lower bounds */
+	for (i = 0; i < nV && lb_new; i++)
+	{
+		s = getAbs(lb_new[i]);
+		if (s < 1.0) s = 1.0;
+		d = getAbs(lb_new[i] - lb[i]) / s;
+		if (d > len) len = d;
+	}
+
+	/* upper bounds */
+	for (i = 0; i < nV && ub_new; i++)
+	{
+		s = getAbs(ub_new[i]);
+		if (s < 1.0) s = 1.0;
+		d = getAbs(ub_new[i] - ub[i]) / s;
+		if (d > len) len = d;
+	}
+
+	return len;
+}
+
+
+/*
+ * p e r f o r m R a m p i n g
+ */
+returnValue QProblemB::performRamping( )
+{
+	int nV = getNV( ), bstat, i;
+	real_t t, rampVal;
+
+	/* ramp inactive bounds and active dual variables */
+	for (i = 0; i < nV; i++)
+	{
+		switch (bounds.getType(i))
+		{
+			case ST_EQUALITY: lb[i] = x[i]; ub[i] = x[i]; continue; /* reestablish exact feasibility */
+			case ST_UNBOUNDED: continue;
+			case ST_DISABLED: continue;
+			default: break;
+		}
+
+		t = static_cast<real_t>((i + rampOffset) % nV) / static_cast<real_t>(nV-1);
+		rampVal = (1.0-t) * ramp0 + t * ramp1;
+		bstat = bounds.getStatus(i);
+		if (bstat != ST_LOWER) { lb[i] = x[i] - rampVal; }
+		if (bstat != ST_UPPER) { ub[i] = x[i] + rampVal; }
+		if (bstat == ST_LOWER) { lb[i] = x[i]; y[i] = +rampVal; }
+		if (bstat == ST_UPPER) { ub[i] = x[i]; y[i] = -rampVal; }
+		if (bstat == ST_INACTIVE) y[i] = 0.0; /* reestablish exact complementarity */
+	}
+
+	/* reestablish exact stationarity */
+	setupAuxiliaryQPgradient( );
+
+	/* advance ramp offset to avoid Ramping cycles */
+	rampOffset++;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ * u p d a t e F a r B o u n d s
+ */
+returnValue QProblemB::updateFarBounds(	real_t curFarBound, int nRamp,
+											const real_t* const lb_new, real_t* const lb_new_far,
+											const real_t* const ub_new, real_t* const ub_new_far
+											) const
+{
+	int i;
+	real_t rampVal, t;
+	int nV = getNV( );
+
+	if ( options.enableRamping == BT_TRUE )
+	{
+		for ( i=0; i<nV; ++i )
+		{
+			t = static_cast<real_t>((i + rampOffset) % nRamp) / static_cast<real_t>(nRamp-1);
+			rampVal = curFarBound * (1.0 + (1.0-t)*ramp0 + t*ramp1);
+
+			if ( lb_new == 0 )
+				lb_new_far[i] = -rampVal;
+			else
+				lb_new_far[i] = getMax( -rampVal,lb_new[i] );
+
+			if ( ub_new == 0 )
+				ub_new_far[i] = rampVal;
+			else
+				ub_new_far[i] = getMin( rampVal,ub_new[i] );
+		}
+	}
+	else
+	{
+		for ( i=0; i<nV; ++i )
+		{
+			if ( lb_new == 0 )
+				lb_new_far[i] = -curFarBound;
+			else
+				lb_new_far[i] = getMax( -curFarBound,lb_new[i] );
+
+			if ( ub_new == 0 )
+				ub_new_far[i] = curFarBound;
+			else
+				ub_new_far[i] = getMin( curFarBound,ub_new[i] );
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+
+
+/*****************************************************************************
+ *  P R I V A T E                                                            *
+ *****************************************************************************/
+
+/*
+ *	s o l v e I n i t i a l Q P
+ */
+returnValue QProblemB::solveInitialQP(	const real_t* const xOpt, const real_t* const yOpt,
+										const Bounds* const guessedBounds,
+										int& nWSR, real_t* const cputime
+										)
+{
+	int i;
+	int nV = getNV( );
+
+
+	/* start runtime measurement */
+	real_t starttime = 0.0;
+	if ( cputime != 0 )
+		starttime = getCPUtime( );
+
+
+	status = QPS_NOTINITIALISED;
+
+	/* I) ANALYSE QP DATA: */
+	/* 1) Check if Hessian happens to be the identity matrix. */
+	if ( determineHessianType( ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INIT_FAILED );
+
+	/* 2) Setup type of bounds (i.e. unbounded, implicitly fixed etc.). */
+	if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INIT_FAILED );
+
+	status = QPS_PREPARINGAUXILIARYQP;
+
+
+	/* II) SETUP AUXILIARY QP WITH GIVEN OPTIMAL SOLUTION: */
+	/* 1) Setup bounds data structure. */
+	if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INIT_FAILED );
+
+	/* 2) Setup optimal primal/dual solution for auxiliary QP. */
+	if ( setupAuxiliaryQPsolution( xOpt,yOpt ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INIT_FAILED );
+
+	/* 3) Obtain linear independent working set for auxiliary QP. */
+	Bounds auxiliaryBounds( nV );
+	if ( obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds, &auxiliaryBounds ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INIT_FAILED );
+
+	/* 4) Setup working set of auxiliary QP and setup cholesky decomposition. */
+	/* a) Working set of auxiliary QP. */
+	if ( setupAuxiliaryWorkingSet( &auxiliaryBounds,BT_TRUE ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_INIT_FAILED );
+
+	/* b) Regularise Hessian if necessary. */
+	if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_SEMIDEF ) )
+	{
+		if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_INIT_FAILED_REGULARISATION );
+	}
+
+	haveCholesky = BT_FALSE;
+
+	/* 5) Store original QP formulation... */
+	real_t* g_original = new real_t[nV];
+	real_t* lb_original = new real_t[nV];
+	real_t* ub_original = new real_t[nV];
+
+	for( i=0; i<nV; ++i )
+	{
+		g_original[i]  = g[i];
+		lb_original[i] = lb[i];
+		ub_original[i] = ub[i];
+	}
+
+	/* ... and setup QP data of an auxiliary QP having an optimal solution
+	 * as specified by the user (or xOpt = yOpt = 0, by default). */
+	if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
+	{
+		delete[] ub_original; delete[] lb_original; delete[] g_original;
+		return THROWERROR( RET_INIT_FAILED );
+	}
+
+	if ( setupAuxiliaryQPbounds( BT_TRUE ) != SUCCESSFUL_RETURN )
+	{
+ 		delete[] ub_original; delete[] lb_original; delete[] g_original;
+		return THROWERROR( RET_INIT_FAILED );
+	}
+
+	status = QPS_AUXILIARYQPSOLVED;
+
+
+	/* III) SOLVE ACTUAL INITIAL QP: */
+
+	/* Allow only remaining CPU time for usual hotstart. */
+	if ( cputime != 0 )
+		*cputime -= getCPUtime( ) - starttime;
+
+	/* Use hotstart method to find the solution of the original initial QP,... */
+	returnValue returnvalue = hotstart( g_original,lb_original,ub_original, nWSR,cputime );
+
+	/* ... deallocate memory,... */
+	delete[] ub_original; delete[] lb_original; delete[] g_original;
+
+	/* ... check for infeasibility and unboundedness... */
+	if ( isInfeasible( ) == BT_TRUE )
+		return THROWERROR( RET_INIT_FAILED_INFEASIBILITY );
+
+	if ( isUnbounded( ) == BT_TRUE )
+		return THROWERROR( RET_INIT_FAILED_UNBOUNDEDNESS );
+
+	/* ... and internal errors. */
+	if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
+		return THROWERROR( RET_INIT_FAILED_HOTSTART );
+
+
+	/* stop runtime measurement */
+	if ( cputime != 0 )
+		*cputime = getCPUtime( ) - starttime;
+
+	THROWINFO( RET_INIT_SUCCESSFUL );
+
+	return returnvalue;
+}
+
+
+/*
+ *	s o l v e Q P
+ */
+returnValue QProblemB::solveQP(	const real_t* const g_new,
+								const real_t* const lb_new, const real_t* const ub_new,
+								int& nWSR, real_t* const cputime, int nWSRperformed
+								)
+{
+	int iter;
+	int nV  = getNV( );
+
+	/* consistency check */
+	if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
+		 ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
+		 ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
+	{
+		return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
+	}
+
+	/* start runtime measurement */
+	real_t starttime = 0.0;
+	if ( cputime != 0 )
+		starttime = getCPUtime( );
+
+
+	/* I) PREPARATIONS */
+	/* 1) Allocate delta vectors of gradient and bounds,
+	 *    index arrays and step direction arrays. */
+	real_t* delta_xFR = new real_t[nV];
+	real_t* delta_xFX = new real_t[nV];
+	real_t* delta_yFX = new real_t[nV];
+
+	real_t* delta_g  = new real_t[nV];
+	real_t* delta_lb = new real_t[nV];
+	real_t* delta_ub = new real_t[nV];
+
+	returnValue returnvalue;
+	BooleanType Delta_bB_isZero;
+
+	int BC_idx;
+	SubjectToStatus BC_status;
+
+	real_t homotopyLength;
+
+	#ifndef __XPCTARGET__
+	char messageString[MAX_STRING_LENGTH];
+	#endif
+
+	/* 2) Update type of bounds, e.g. a formerly implicitly fixed
+	 *    variable might have become a normal one etc. */
+	if ( setupSubjectToType( lb_new,ub_new ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_HOTSTART_FAILED );
+
+	/* 3) Reset status flags. */
+	infeasible = BT_FALSE;
+	unbounded  = BT_FALSE;
+
+
+	/* II) MAIN HOMOTOPY LOOP */
+	for( iter=nWSRperformed; iter<nWSR; ++iter )
+	{
+		tabularOutput.idxAddB = tabularOutput.idxRemB = tabularOutput.idxAddC = tabularOutput.idxRemC = -1;
+		tabularOutput.excAddB = tabularOutput.excRemB = tabularOutput.excAddC = tabularOutput.excRemC = 0;
+
+		if ( isCPUtimeLimitExceeded( cputime,starttime,iter-nWSRperformed ) == BT_TRUE )
+		{
+			/* Assign number of working set recalculations and stop runtime measurement. */
+			nWSR = iter;
+			if ( cputime != 0 )
+				*cputime = getCPUtime( ) - starttime;
+
+			break;
+		}
+
+		status = QPS_PERFORMINGHOMOTOPY;
+
+		#ifndef __XPCTARGET__
+		snprintf( messageString,MAX_STRING_LENGTH,"%d ...",iter );
+		getGlobalMessageHandler( )->throwInfo( RET_ITERATION_STARTED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+		#endif
+
+		/* 2) Initialise shift direction of the gradient and the bounds. */
+		returnvalue = determineDataShift(	g_new,lb_new,ub_new,
+											delta_g,delta_lb,delta_ub,
+											Delta_bB_isZero
+											);
+		if ( returnvalue != SUCCESSFUL_RETURN )
+		{
+			delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
+			delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
+
+			/* Assign number of working set recalculations and stop runtime measurement. */
+			nWSR = iter;
+			if ( cputime != 0 )
+				*cputime = getCPUtime( ) - starttime;
+
+			THROWERROR( RET_SHIFT_DETERMINATION_FAILED );
+			return returnvalue;
+		}
+
+		/* 3) Determination of step direction of X and Y. */
+		returnvalue = determineStepDirection(	delta_g,delta_lb,delta_ub,
+												Delta_bB_isZero,
+												delta_xFX,delta_xFR,delta_yFX
+												);
+		if ( returnvalue != SUCCESSFUL_RETURN )
+		{
+			delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
+			delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
+
+			/* Assign number of working set recalculations and stop runtime measurement. */
+			nWSR = iter;
+			if ( cputime != 0 )
+				*cputime = getCPUtime( ) - starttime;
+
+			THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
+			return returnvalue;
+		}
+
+
+		/* 4) Determination of step length TAU.
+		 *    This step along the homotopy path is also taken (without changing working set). */
+		returnvalue = performStep(	delta_g,delta_lb,delta_ub,
+									delta_xFX,delta_xFR,delta_yFX,
+									BC_idx,BC_status
+									);
+		if ( returnvalue != SUCCESSFUL_RETURN )
+		{
+			delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
+			delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
+
+			/* Assign number of working set recalculations and stop runtime measurement. */
+			nWSR = iter;
+			if ( cputime != 0 )
+				*cputime = getCPUtime( ) - starttime;
+
+			THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );
+			return returnvalue;
+		}
+
+		/* 5) Termination criterion. */
+		homotopyLength = getRelativeHomotopyLength(g_new, lb_new, ub_new);
+		if ( homotopyLength <= options.terminationTolerance )
+		{
+			status = QPS_SOLVED;
+
+			THROWINFO( RET_OPTIMAL_SOLUTION_FOUND );
+
+			if ( printIteration( iter,BC_idx,BC_status,homotopyLength ) != SUCCESSFUL_RETURN )
+				THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
+
+			nWSR = iter;
+			if ( cputime != 0 )
+				*cputime = getCPUtime( ) - starttime;
+
+			delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
+			delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
+
+			return SUCCESSFUL_RETURN;
+		}
+
+
+		/* 6) Change active set. */
+		returnvalue = changeActiveSet( BC_idx,BC_status );
+		if ( returnvalue != SUCCESSFUL_RETURN )
+		{
+			delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
+			delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
+
+			/* Assign number of working set recalculations and stop runtime measurement. */
+			nWSR = iter;
+			if ( cputime != 0 )
+				*cputime = getCPUtime( ) - starttime;
+
+			/* checks for infeasibility... */
+			if ( infeasible == BT_TRUE )
+			{
+				status = QPS_HOMOTOPYQPSOLVED;
+				return setInfeasibilityFlag( RET_HOTSTART_STOPPED_INFEASIBILITY );
+			}
+
+			/* ...unboundedness... */
+			if ( unbounded == BT_TRUE ) /* not necessary since objective function convex! */
+				return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );
+
+			/* ... and throw unspecific error otherwise */
+			THROWERROR( RET_HOMOTOPY_STEP_FAILED );
+			return returnvalue;
+		}
+
+		/* 6a) Possibly refactorise projected Hessian from scratch. */
+		if ( ( options.enableCholeskyRefactorisation > 0 ) && ( (iter % options.enableCholeskyRefactorisation) == 0 ) )
+		{
+			returnvalue = computeCholesky( );
+			if (returnvalue != SUCCESSFUL_RETURN)
+			{
+				delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
+				delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
+				return returnvalue;
+			}
+		}
+
+
+		/* 7) Perform Ramping Strategy on zero homotopy step or drift correction (if desired). */
+		 if ( ( tau <= EPS ) && ( options.enableRamping == BT_TRUE ) )
+			performRamping( );
+		else
+		if ( (options.enableDriftCorrection > 0) && ((iter+1) % options.enableDriftCorrection == 0) )
+			performDriftCorrection( );  /* always returns SUCCESSFUL_RETURN */
+
+		/* 8) Output information of successful QP iteration. */
+		status = QPS_HOMOTOPYQPSOLVED;
+
+		if ( printIteration( iter,BC_idx,BC_status,homotopyLength ) != SUCCESSFUL_RETURN )
+			THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
+	}
+
+	delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
+	delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
+
+	/* stop runtime measurement */
+	if ( cputime != 0 )
+		*cputime = getCPUtime( ) - starttime;
+
+
+	/* if programm gets to here, output information that QP could not be solved
+	 * within the given maximum numbers of working set changes */
+	if ( options.printLevel == PL_HIGH )
+	{
+		#ifndef __XPCTARGET__
+		snprintf( messageString,MAX_STRING_LENGTH,"(nWSR = %d)",iter );
+		return getGlobalMessageHandler( )->throwWarning( RET_MAX_NWSR_REACHED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+		#else
+		return RET_MAX_NWSR_REACHED;
+		#endif
+	}
+	else
+	{
+		return RET_MAX_NWSR_REACHED;
+	}
+}
+
+
+/*
+ *	s o l v e R e g u l a r i s e d Q P
+ */
+returnValue QProblemB::solveRegularisedQP(	const real_t* const g_new,
+											const real_t* const lb_new, const real_t* const ub_new,
+											int& nWSR, real_t* const cputime, int nWSRperformed
+											)
+{
+	int i, step;
+	int nV = getNV( );
+
+
+	/* Perform normal QP solution if QP has not been regularised. */
+	if ( usingRegularisation( ) == BT_FALSE )
+		return solveQP( g_new,lb_new,ub_new, nWSR,cputime,nWSRperformed );
+
+
+	/* I) SOLVE USUAL REGULARISED QP */
+	returnValue returnvalue;
+
+	int nWSR_max   = nWSR;
+	int nWSR_total = nWSRperformed;
+
+	real_t cputime_total = 0.0;
+	real_t cputime_cur   = 0.0;
+
+	if ( cputime == 0 )
+	{
+		returnvalue = solveQP( g_new,lb_new,ub_new, nWSR,0,nWSRperformed );
+	}
+	else
+	{
+		cputime_cur = *cputime;
+		returnvalue = solveQP( g_new,lb_new,ub_new, nWSR,&cputime_cur,nWSRperformed );
+	}
+	nWSR_total     = nWSR;
+	cputime_total += cputime_cur;
+
+
+	/* Only continue if QP solution has been successful. */
+	if ( returnvalue != SUCCESSFUL_RETURN )
+	{
+		if ( cputime != 0 )
+			*cputime = cputime_total;
+
+		if ( returnvalue == RET_MAX_NWSR_REACHED )
+			THROWWARNING( RET_NO_REGSTEP_NWSR );
+
+		return returnvalue;
+	}
+
+
+	/* II) PERFORM SUCCESSIVE REGULARISATION STEPS */
+	real_t* gMod = new real_t[nV];
+
+	for( step=0; step<options.numRegularisationSteps; ++step )
+	{
+		/* 1) Modify gradient: gMod = g - eps*xOpt
+		 *    (assuming regularisation matrix to be regVal*Id). */
+		for( i=0; i<nV; ++i )
+			gMod[i] = g_new[i] - regVal*x[i];
+
+		/* 2) Solve regularised QP with modified gradient allowing
+		 *    only as many working set recalculations and CPU time
+		 *    as have been left from previous QP solutions. */
+		if ( cputime == 0 )
+		{
+			nWSR = nWSR_max;
+			returnvalue = solveQP( gMod,lb_new,ub_new, nWSR,0,nWSR_total );
+		}
+		else
+		{
+			nWSR = nWSR_max;
+			cputime_cur = *cputime - cputime_total;
+			returnvalue = solveQP( gMod,lb_new,ub_new, nWSR,&cputime_cur,nWSR_total );
+		}
+
+		nWSR_total     = nWSR;
+		cputime_total += cputime_cur;
+
+		/* Only continue if QP solution has been successful. */
+		if ( returnvalue != SUCCESSFUL_RETURN )
+		{
+			delete[] gMod;
+
+			if ( cputime != 0 )
+				*cputime = cputime_total;
+
+			if ( returnvalue == RET_MAX_NWSR_REACHED )
+				THROWWARNING( RET_FEWER_REGSTEPS_NWSR );
+
+			return returnvalue;
+		}
+	}
+
+	delete[] gMod;
+
+	if ( cputime != 0 )
+		*cputime = cputime_total;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p A u x i l i a r y W o r k i n g S e t
+ */
+returnValue QProblemB::setupAuxiliaryWorkingSet( 	const Bounds* const auxiliaryBounds,
+													BooleanType setupAfresh
+													)
+{
+	int i;
+	int nV = getNV( );
+
+	/* consistency checks */
+	if ( auxiliaryBounds != 0 )
+	{
+		for( i=0; i<nV; ++i )
+			if ( ( bounds.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryBounds->getStatus( i ) == ST_UNDEFINED ) )
+				return THROWERROR( RET_UNKNOWN_BUG );
+	}
+	else
+	{
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+	}
+
+
+	/* I) SETUP CHOLESKY FLAG:
+	 *    Cholesky decomposition shall only be updated if working set
+	 *    shall be updated (i.e. NOT setup afresh!) */
+	BooleanType updateCholesky;
+	if ( setupAfresh == BT_TRUE )
+		updateCholesky = BT_FALSE;
+	else
+		updateCholesky = BT_TRUE;
+
+
+	/* II) REMOVE FORMERLY ACTIVE BOUNDS (IF NECESSARY): */
+	if ( setupAfresh == BT_FALSE )
+	{
+		/* Remove all active bounds that shall be inactive AND
+		*  all active bounds that are active at the wrong bound. */
+		for( i=0; i<nV; ++i )
+		{
+			if ( ( bounds.getStatus( i ) == ST_LOWER ) && ( auxiliaryBounds->getStatus( i ) != ST_LOWER ) )
+				if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
+
+			if ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) )
+				if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
+					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
+		}
+	}
+
+
+	/* III) ADD NEWLY ACTIVE BOUNDS: */
+	/*      Add all inactive bounds that shall be active AND
+	 *      all formerly active bounds that have been active at the wrong bound. */
+	for( i=0; i<nV; ++i )
+	{
+		if ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) )
+		{
+			if ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p A u x i l i a r y Q P s o l u t i o n
+ */
+returnValue QProblemB::setupAuxiliaryQPsolution(	const real_t* const xOpt, const real_t* const yOpt
+													)
+{
+	int i;
+	int nV = getNV( );
+
+
+	/* Setup primal/dual solution vectors for auxiliary initial QP:
+	 * if a null pointer is passed, a zero vector is assigned;
+	 * old solution vector is kept if pointer to internal solution vector is passed. */
+	if ( xOpt != 0 )
+	{
+		if ( xOpt != x )
+			for( i=0; i<nV; ++i )
+				x[i] = xOpt[i];
+	}
+	else
+	{
+		for( i=0; i<nV; ++i )
+			x[i] = 0.0;
+	}
+
+	if ( yOpt != 0 )
+	{
+		if ( yOpt != y )
+			for( i=0; i<nV; ++i )
+				y[i] = yOpt[i];
+	}
+	else
+	{
+		for( i=0; i<nV; ++i )
+			y[i] = 0.0;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p A u x i l i a r y Q P g r a d i e n t
+ */
+returnValue QProblemB::setupAuxiliaryQPgradient( )
+{
+	int i;
+	int nV = getNV( );
+
+	/* Setup gradient vector: g = -H*x + y'*Id. */
+	switch ( hessianType )
+	{
+		case HST_ZERO:
+			if ( usingRegularisation( ) == BT_FALSE )
+				for ( i=0; i<nV; ++i )
+					g[i] = y[i];
+			else
+				for ( i=0; i<nV; ++i )
+					g[i] = y[i] - regVal*x[i];
+			break;
+
+		case HST_IDENTITY:
+			for ( i=0; i<nV; ++i )
+				g[i] = y[i] - x[i];
+			break;
+
+		default:
+			/* y'*Id */
+			for ( i=0; i<nV; ++i )
+				g[i] = y[i];
+
+			/* -H*x */
+			H->times(1, -1.0, x, nV, 1.0, g, nV);
+
+			break;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p A u x i l i a r y Q P b o u n d s
+ */
+returnValue QProblemB::setupAuxiliaryQPbounds( BooleanType useRelaxation )
+{
+	int i;
+	int nV = getNV( );
+
+
+	/* Setup bound vectors. */
+	for ( i=0; i<nV; ++i )
+	{
+		switch ( bounds.getStatus( i ) )
+		{
+			case ST_INACTIVE:
+				if ( useRelaxation == BT_TRUE )
+				{
+					if ( bounds.getType( i ) == ST_EQUALITY )
+					{
+						lb[i] = x[i];
+						ub[i] = x[i];
+					}
+					else
+					{
+						lb[i] = x[i] - options.boundRelaxation;
+						ub[i] = x[i] + options.boundRelaxation;
+					}
+				}
+				break;
+
+			case ST_LOWER:
+				lb[i] = x[i];
+				if ( bounds.getType( i ) == ST_EQUALITY )
+				{
+					ub[i] = x[i];
+				}
+				else
+				{
+					if ( useRelaxation == BT_TRUE )
+						ub[i] = x[i] + options.boundRelaxation;
+				}
+				break;
+
+			case ST_UPPER:
+				ub[i] = x[i];
+				if ( bounds.getType( i ) == ST_EQUALITY )
+				{
+					lb[i] = x[i];
+				}
+				else
+				{
+					if ( useRelaxation == BT_TRUE )
+						lb[i] = x[i] - options.boundRelaxation;
+				}
+				break;
+
+            case ST_DISABLED:
+                break;
+                
+			default:
+				return THROWERROR( RET_UNKNOWN_BUG );
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t u p A u x i l i a r y Q P
+ */
+returnValue QProblemB::setupAuxiliaryQP( const Bounds* const guessedBounds )
+{
+	int i;
+	int nV = getNV( );
+
+	/* nothing to do */
+	if ( guessedBounds == &bounds )
+		return SUCCESSFUL_RETURN;
+
+	status = QPS_PREPARINGAUXILIARYQP;
+
+
+	/* I) SETUP WORKING SET ... */
+	if ( shallRefactorise( guessedBounds ) == BT_TRUE )
+	{
+		/* ... WITH REFACTORISATION: */
+		/* 1) Reset bounds ... */
+		bounds.init( nV );
+
+		/*    ... and set them up afresh. */
+		if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+		if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+		/* 2) Setup guessed working set afresh. */
+		if ( setupAuxiliaryWorkingSet( guessedBounds,BT_TRUE ) != SUCCESSFUL_RETURN )
+			THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+		/* 3) Calculate Cholesky decomposition. */
+		if ( computeCholesky( ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+	}
+	else
+	{
+		/* ... WITHOUT REFACTORISATION: */
+		if ( setupAuxiliaryWorkingSet( guessedBounds,BT_FALSE ) != SUCCESSFUL_RETURN )
+			THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+	}
+
+
+	/* II) SETUP AUXILIARY QP DATA: */
+	/* 1) Ensure that dual variable is zero for free bounds. */
+	for ( i=0; i<nV; ++i )
+		if ( bounds.getStatus( i ) == ST_INACTIVE )
+			y[i] = 0.0;
+
+	/* 2) Setup gradient and bound vectors. */
+	if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
+		THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+	if ( setupAuxiliaryQPbounds( BT_FALSE ) != SUCCESSFUL_RETURN )
+		THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	d e t e r m i n e S t e p D i r e c t i o n
+ */
+returnValue QProblemB::determineStepDirection(	const real_t* const delta_g, const real_t* const delta_lb, const real_t* const delta_ub,
+												BooleanType Delta_bB_isZero,
+												real_t* const delta_xFX, real_t* const delta_xFR,
+												real_t* const delta_yFX
+												)
+{
+	int i, ii;
+	int r;
+	int nFR = getNFR( );
+	int nFX = getNFX( );
+	
+	int* FR_idx;
+	int* FX_idx;
+
+	bounds.getFree( )->getNumberArray( &FR_idx );
+	bounds.getFixed( )->getNumberArray( &FX_idx );
+
+
+	/* This routine computes
+	 * delta_xFX := delta_b
+	 * delta_xFR := R \ R' \ -( delta_g + HMX*delta_xFX )
+	 * delta_yFX := HMX'*delta_xFR + HFX*delta_xFX  { + eps*delta_xFX }
+	 */
+
+	/* I) DETERMINE delta_xFX := delta_{l|u}b */
+	if ( Delta_bB_isZero == BT_FALSE )
+	{
+		for( i=0; i<nFX; ++i )
+		{
+			ii = FX_idx[i];
+
+			if ( bounds.getStatus( ii ) == ST_LOWER )
+				delta_xFX[i] = delta_lb[ii];
+			else
+				delta_xFX[i] = delta_ub[ii];
+		}
+	}
+	else
+	{
+		for( i=0; i<nFX; ++i )
+			delta_xFX[i] = 0.0;
+	}
+
+
+	/* delta_xFR_TMP holds the residual, initialized with right hand side
+	 * delta_xFR holds the step that gets refined incrementally */
+	for ( i=0; i<nFR; ++i )
+	{
+		ii = FR_idx[i];
+		delta_xFR_TMP[i] = - delta_g[ii];
+		delta_xFR[i] = 0.0;
+	}
+
+
+	/* Iterative refinement loop for delta_xFR */
+	for ( r=0; r<=options.numRefinementSteps; ++r )
+	{
+		/* II) DETERMINE delta_xFR */
+		if ( nFR > 0 )
+		{
+			/* Add - HMX*delta_xFX
+			 * This is skipped if delta_b=0 or mixed part HM=0 (H=0 or H=Id) */
+			if ( ( hessianType != HST_ZERO ) && ( hessianType != HST_IDENTITY ) && ( Delta_bB_isZero == BT_FALSE ) && ( r == 0 ) )
+				H->times(bounds.getFree(), bounds.getFixed(), 1, -1.0, delta_xFX, nFX, 1.0, delta_xFR_TMP, nFR);
+
+			/* Determine R' \ ( - HMX*delta_xFX - delta_gFR ) where R'R = HFR */
+			if ( backsolveR( delta_xFR_TMP,BT_TRUE,delta_xFR_TMP ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
+
+			/* Determine HFR \ ( - HMX*delta_xFX - delta_gFR ) */
+			if ( backsolveR( delta_xFR_TMP,BT_FALSE,delta_xFR_TMP ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
+		}
+
+		/* refine solution found for delta_xFR so far */
+		for ( i=0; i<nFR; ++i )
+			delta_xFR[i] += delta_xFR_TMP[i];
+
+		if ( options.numRefinementSteps > 0 )
+		{
+			real_t rnrm = 0.0;
+			/* compute new residual in delta_xFR_TMP:
+			 * residual := - HFR*delta_xFR - HMX*delta_xFX - delta_gFR
+			 * set to -delta_gFR */
+			for ( i=0; i<nFR; ++i )
+			{
+				ii = FR_idx[i];
+				delta_xFR_TMP[i] = -delta_g[ii];
+			}
+			/* add - HFR*delta_xFR */
+			switch ( hessianType )
+			{
+				case HST_ZERO:
+					break;
+
+				case HST_IDENTITY:
+					for ( i=0; i<nFR; ++i )
+					{
+						delta_xFR_TMP[i] -= delta_xFR[i];
+
+						/* compute max norm */
+						if (rnrm < getAbs (delta_xFR_TMP[i]))
+							rnrm = getAbs (delta_xFR_TMP[i]);
+					}
+					break;
+
+				default:
+					H->times(bounds.getFree(), bounds.getFree(),  1, -1.0, delta_xFR, nFR, 1.0, delta_xFR_TMP, nFR);
+					H->times(bounds.getFree(), bounds.getFixed(), 1, -1.0, delta_xFX, nFX, 1.0, delta_xFR_TMP, nFR);
+
+					/* compute max norm */
+					for ( i=0; i<nFR; ++i )
+						if (rnrm < getAbs (delta_xFR_TMP[i]))
+							rnrm = getAbs (delta_xFR_TMP[i]);
+
+					break;
+			}
+			
+			/* early termination of residual norm small enough */
+			if ( rnrm < options.epsIterRef )
+				break;
+		}
+
+	} /* end of refinement loop for delta_xFR */
+
+	/* III) DETERMINE delta_yFX */
+	if ( nFX > 0 )
+	{
+		if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_IDENTITY ) )
+		{
+			for( i=0; i<nFX; ++i )
+			{
+				/* set to delta_g */
+				ii = FX_idx[i];
+				delta_yFX[i] = delta_g[ii];
+
+				/* add HFX*delta_xFX = {0|I}*delta_xFX */
+				if ( hessianType == HST_ZERO )
+				{
+					if ( usingRegularisation( ) == BT_TRUE )
+						delta_yFX[i] += regVal*delta_xFX[i];
+				}
+				else
+					delta_yFX[i] += delta_xFX[i];
+			}
+		}
+		else
+		{
+			for( i=0; i<nFX; ++i )
+			{
+				/* set to delta_g */
+				ii = FX_idx[i];
+				delta_yFX[i] = delta_g[ii];
+			}
+			H->times(bounds.getFixed(), bounds.getFree(), 1, 1.0, delta_xFR, nFR, 1.0, delta_yFX, nFX);
+			if (Delta_bB_isZero == BT_FALSE)
+				H->times(bounds.getFixed(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, delta_yFX, nFX);
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	p e r f o r m S t e p
+ */
+returnValue QProblemB::performStep(	const real_t* const delta_g,
+									const real_t* const delta_lb, const real_t* const delta_ub,
+									const real_t* const delta_xFX,
+									const real_t* const delta_xFR,
+									const real_t* const delta_yFX,
+									int& BC_idx, SubjectToStatus& BC_status
+									)
+{
+	int i, ii;
+	int nV = getNV( );
+	int nFR = getNFR( );
+	int nFX = getNFX( );
+
+	int* FR_idx;
+	int* FX_idx;
+
+	bounds.getFree( )->getNumberArray( &FR_idx );
+	bounds.getFixed( )->getNumberArray( &FX_idx );
+
+	tau = 1.0;
+	BC_idx = -1;
+	BC_status = ST_UNDEFINED;
+
+	int BC_idx_tmp = -1;
+
+	real_t* num = new real_t[nV];
+	real_t* den = new real_t[nV];
+
+
+	/* I) DETERMINE MAXIMUM DUAL STEPLENGTH, i.e. ensure that
+	 *    active dual bounds remain valid (ignoring implicitly fixed variables): */
+	for( i=0; i<nFX; ++i )
+	{
+		ii = FX_idx[i];
+		num[i] = y[ii];
+		den[i] = -delta_yFX[i];
+	}
+
+	performRatioTest( nFX,FX_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
+
+	if ( BC_idx_tmp >= 0 )
+	{
+		BC_idx = BC_idx_tmp;
+		BC_status = ST_INACTIVE;
+	}
+
+
+	/* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH, i.e. ensure that
+	 *     inactive bounds remain valid (ignoring unbounded variables). */
+	/* 1) Inactive lower bounds. */
+	if ( bounds.hasNoLower( ) == BT_FALSE )
+	{
+		for( i=0; i<nFR; ++i )
+		{
+			ii = FR_idx[i];
+			num[i] = getMax( x[ii] - lb[ii],0.0 );
+			den[i] = delta_lb[ii] - delta_xFR[i];
+		}
+
+		performRatioTest( nFR,FR_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
+
+		if ( BC_idx_tmp >= 0 )
+		{
+			BC_idx = BC_idx_tmp;
+			BC_status = ST_LOWER;
+		}
+	}
+
+	/* 2) Inactive upper bounds. */
+	if ( bounds.hasNoUpper( ) == BT_FALSE )
+	{
+		for( i=0; i<nFR; ++i )
+		{
+			ii = FR_idx[i];
+			num[i] = getMax( ub[ii] - x[ii],0.0 );
+			den[i] = delta_xFR[i] - delta_ub[ii];
+		}
+
+		performRatioTest( nFR,FR_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
+
+		if ( BC_idx_tmp >= 0 )
+		{
+			BC_idx = BC_idx_tmp;
+			BC_status = ST_UPPER;
+		}
+	}
+
+	delete[] den;
+	delete[] num;
+
+
+	#ifndef __XPCTARGET__
+	char messageString[MAX_STRING_LENGTH];
+
+	if ( BC_status == ST_UNDEFINED )
+		snprintf( messageString,MAX_STRING_LENGTH,"Stepsize is %.15e!",tau );
+	else
+		snprintf( messageString,MAX_STRING_LENGTH,"Stepsize is %.15e! (idx = %d, status = %d)",tau,BC_idx,BC_status );
+
+	getGlobalMessageHandler( )->throwInfo( RET_STEPSIZE_NONPOSITIVE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+	#endif
+
+
+	/* III) PERFORM STEP ALONG HOMOTOPY PATH */
+	if ( tau > ZERO )
+	{
+		/* 1) Perform step in primal und dual space. */
+		for( i=0; i<nFR; ++i )
+		{
+			ii = FR_idx[i];
+			x[ii] += tau*delta_xFR[i];
+		}
+
+		for( i=0; i<nFX; ++i )
+		{
+			ii = FX_idx[i];
+			x[ii] += tau*delta_xFX[i];
+			y[ii] += tau*delta_yFX[i];
+		}
+
+		/* 2) Shift QP data. */
+		for( i=0; i<nV; ++i )
+		{
+			g[i]  += tau*delta_g[i];
+			lb[i] += tau*delta_lb[i];
+			ub[i] += tau*delta_ub[i];
+		}
+	}
+	else
+	{
+		/* print a warning if stepsize is zero */
+		#ifndef __XPCTARGET__
+		snprintf( messageString,MAX_STRING_LENGTH,"Stepsize is %.15e",tau );
+		getGlobalMessageHandler( )->throwWarning( RET_STEPSIZE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+		#endif
+	}
+
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	c h a n g e A c t i v e S e t
+ */
+returnValue QProblemB::changeActiveSet( int BC_idx, SubjectToStatus BC_status )
+{
+	#ifndef __XPCTARGET__
+	char messageString[MAX_STRING_LENGTH];
+	#endif
+
+	/* IV) UPDATE ACTIVE SET */
+	switch ( BC_status )
+	{
+		/* Optimal solution found as no working set change detected. */
+		case ST_UNDEFINED:
+			return RET_OPTIMAL_SOLUTION_FOUND;
+
+
+		/* Remove one variable from active set. */
+		case ST_INACTIVE:
+			#ifndef __XPCTARGET__
+			snprintf( messageString,MAX_STRING_LENGTH,"bound no. %d.", BC_idx );
+			getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+			#endif
+
+			if ( removeBound( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
+
+			y[BC_idx] = 0.0;
+			break;
+
+
+		/* Add one variable to active set. */
+		default:
+			#ifndef __XPCTARGET__
+			if ( BC_status == ST_LOWER )
+				snprintf( messageString,MAX_STRING_LENGTH,"lower bound no. %d.", BC_idx );
+			else
+				snprintf( messageString,MAX_STRING_LENGTH,"upper bound no. %d.", BC_idx );
+				getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+			#endif
+
+			if ( addBound( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
+			break;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ * p e r f o r m D r i f t C o r r e c t i o n
+ */
+returnValue QProblemB::performDriftCorrection( )
+{
+	int i;
+	int nV = getNV ();
+
+	for ( i=0; i<nV; ++i )
+	{
+		switch ( bounds.getType ( i ) )
+		{
+			case ST_BOUNDED:
+				switch ( bounds.getStatus ( i ) )
+				{
+					case ST_LOWER:
+						lb[i] = x[i];
+						ub[i] = getMax (ub[i], x[i]);
+						y[i] = getMax (y[i], 0.0);
+						break;
+					case ST_UPPER:
+						lb[i] = getMin (lb[i], x[i]);
+						ub[i] = x[i];
+						y[i] = getMin (y[i], 0.0);
+						break;
+					case ST_INACTIVE:
+						lb[i] = getMin (lb[i], x[i]);
+						ub[i] = getMax (ub[i], x[i]);
+						y[i] = 0.0;
+						break;
+					case ST_UNDEFINED:
+					case ST_INFEASIBLE_LOWER:
+					case ST_INFEASIBLE_UPPER:
+						break;
+				}
+				break;
+			case ST_EQUALITY:
+				lb[i] = x[i];
+				ub[i] = x[i];
+				break;
+			case ST_UNBOUNDED:
+			case ST_UNKNOWN:
+            case ST_DISABLED:
+				break;
+		}
+	}
+
+	return setupAuxiliaryQPgradient( );
+}
+
+
+/*
+ *	s h a l l R e f a c t o r i s e
+ */
+BooleanType QProblemB::shallRefactorise( const Bounds* const guessedBounds ) const
+{
+	int i;
+	int nV = getNV( );
+
+	/* always refactorise if Hessian is not known to be positive definite */
+	if ( ( hessianType == HST_SEMIDEF ) || ( hessianType == HST_INDEF ) )
+		return BT_TRUE;
+
+	/* 1) Determine number of bounds that have same status
+	 *    in guessed AND current bounds.*/
+	int differenceNumber = 0;
+
+	for( i=0; i<nV; ++i )
+		if ( guessedBounds->getStatus( i ) != bounds.getStatus( i ) )
+			++differenceNumber;
+
+	/* 2) Decide wheter to refactorise or not. */
+	if ( 2*differenceNumber > guessedBounds->getNFX( ) )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+/*
+ *	a d d B o u n d
+ */
+returnValue QProblemB::addBound(	int number, SubjectToStatus B_status,
+									BooleanType updateCholesky
+									)
+{
+	int i, j;
+	int nV  = getNV( );
+	int nFR = getNFR( );
+
+
+	/* consistency check */
+	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
+		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
+		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
+		 ( getStatus( ) == QPS_SOLVED )            )
+	{
+		return THROWERROR( RET_UNKNOWN_BUG );
+	}
+
+	/* Perform cholesky updates only if QProblemB has been initialised! */
+	if ( getStatus( ) == QPS_PREPARINGAUXILIARYQP )
+	{
+		/* UPDATE INDICES */
+		if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_ADDBOUND_FAILED );
+
+		return SUCCESSFUL_RETURN;
+	}
+
+
+	/* I) PERFORM CHOLESKY UPDATE: */
+	if ( ( updateCholesky == BT_TRUE ) &&
+		 ( hessianType != HST_ZERO )   && ( hessianType != HST_IDENTITY ) )
+	{
+		/* 1) Index of variable to be added within the list of free variables. */
+		int number_idx = bounds.getFree( )->getIndex( number );
+
+		real_t c, s, nu;
+
+		/* 2) Use row-wise Givens rotations to restore upper triangular form of R. */
+		for( i=number_idx+1; i<nFR; ++i )
+		{
+			computeGivens( RR(i-1,i),RR(i,i), RR(i-1,i),RR(i,i),c,s );
+			nu = s/(1.0+c);
+
+			for( j=(1+i); j<nFR; ++j ) /* last column of R is thrown away */
+				applyGivens( c,s,nu,RR(i-1,j),RR(i,j), RR(i-1,j),RR(i,j) );
+		}
+
+		/* 3) Delete <number_idx>th column and ... */
+		for( i=0; i<nFR-1; ++i )
+			for( j=number_idx+1; j<nFR; ++j )
+				RR(i,j-1) = RR(i,j);
+		/* ... last column of R. */
+		for( i=0; i<nFR; ++i )
+			RR(i,nFR-1) = 0.0;
+	}
+
+	/* II) UPDATE INDICES */
+	tabularOutput.idxAddB = number;
+	if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_ADDBOUND_FAILED );
+
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	r e m o v e B o u n d
+ */
+returnValue QProblemB::removeBound(	int number,
+									BooleanType updateCholesky
+									)
+{
+	int i;
+	int nV  = getNV( );
+	int nFR = getNFR( );
+
+
+	/* consistency check */
+	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
+		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
+		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
+		 ( getStatus( ) == QPS_SOLVED )            )
+	{
+		return THROWERROR( RET_UNKNOWN_BUG );
+	}
+
+	/* save index sets and decompositions for flipping bounds strategy */
+	if ( options.enableFlippingBounds == BT_TRUE )
+		flipper.set( &bounds,R );
+
+	/* I) UPDATE INDICES */
+	tabularOutput.idxRemB = number;
+	if ( bounds.moveFixedToFree( number ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_REMOVEBOUND_FAILED );
+
+	/* Perform cholesky updates only if QProblemB has been initialised! */
+	if ( getStatus( ) == QPS_PREPARINGAUXILIARYQP )
+		return SUCCESSFUL_RETURN;
+
+
+	/* II) PERFORM CHOLESKY UPDATE */
+	if ( ( updateCholesky == BT_TRUE ) &&
+		 ( hessianType != HST_ZERO )   && ( hessianType != HST_IDENTITY ) )
+	{
+		int* FR_idx;
+		bounds.getFree( )->getNumberArray( &FR_idx );
+
+		/* 1) Calculate new column of cholesky decomposition. */
+		real_t* rhs = new real_t[nFR+1];
+		real_t* r   = new real_t[nFR];
+
+		real_t r0;
+		switch ( hessianType )
+		{
+			case HST_ZERO: /* TODO: Code can/should? never get here!! */
+				if ( usingRegularisation( ) == BT_FALSE )
+					r0 = 0.0;
+				else
+					r0 = regVal;
+				for( i=0; i<nFR; ++i )
+					rhs[i] = 0.0;
+				break;
+
+			case HST_IDENTITY:
+				r0 = 1.0;
+				for( i=0; i<nFR; ++i )
+					rhs[i] = 0.0;
+				break;
+
+			default:
+				H->getRow(number, bounds.getFree(), 1.0, rhs);
+				r0 = H->diag(number);
+				break;
+		}
+
+		if ( backsolveR( rhs,BT_TRUE,BT_TRUE,r ) != SUCCESSFUL_RETURN )
+		{
+			delete[] rhs; delete[] r;
+			return THROWERROR( RET_REMOVEBOUND_FAILED );
+		}
+
+		for( i=0; i<nFR; ++i )
+			r0 -= r[i]*r[i];
+
+		/* 2) Store new column into R. */
+		for( i=0; i<nFR; ++i )
+			RR(i,nFR) = r[i];
+
+		if ( options.enableFlippingBounds == BT_TRUE )
+		{
+			if ( r0 > options.epsFlipping )
+				RR(nFR,nFR) = getSqrt( r0 );
+			else
+			{
+				hessianType = HST_SEMIDEF;
+
+				flipper.get( &bounds,R );
+				bounds.flipFixed(number);
+
+				switch (bounds.getStatus(number))
+				{
+					case ST_LOWER: lb[number] = ub[number]; break;
+					case ST_UPPER: ub[number] = lb[number]; break;
+					default: delete[] rhs; delete[] r; return THROWERROR( RET_MOVING_BOUND_FAILED );
+				}
+
+			}
+		}
+		else
+		{
+			if ( r0 > ZERO )
+				RR(nFR,nFR) = getSqrt( r0 );
+			else
+			{
+				delete[] rhs; delete[] r;
+
+				hessianType = HST_SEMIDEF;
+				return THROWERROR( RET_HESSIAN_NOT_SPD );
+			}
+		}
+
+		delete[] rhs; delete[] r;
+	}
+
+	if ( ( hessianType == HST_ZERO ) && ( options.enableFlippingBounds == BT_TRUE ) )
+	{
+		flipper.get( &bounds,R );
+		bounds.flipFixed(number);
+
+		switch (bounds.getStatus(number))
+		{
+			case ST_LOWER: lb[number] = ub[number]; break;
+			case ST_UPPER: ub[number] = lb[number]; break;
+			default: return THROWERROR( RET_MOVING_BOUND_FAILED );
+		}
+
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	p r i n t I t e r a t i o n
+ */
+returnValue QProblemB::printIteration( 	int iter,
+											int BC_idx,	SubjectToStatus BC_status, real_t homotopyLength
+											)
+{
+	#ifndef __XPCTARGET__
+
+	/* consistency check */
+	if ( iter < 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	int i;
+	int nV = getNV();
+	real_t stat, bfeas, bcmpl;
+	real_t *grad = 0;
+		
+	char myPrintfString[MAX_STRING_LENGTH];
+	const char excStr[] = " ef";
+
+	switch ( options.printLevel )
+	{
+		case PL_DEBUG_ITER:
+			grad = new real_t[nV];
+			stat = bfeas = bcmpl = 0.0;
+
+			/* stationarity */
+			for (i = 0; i < nV; i++) grad[i] = g[i] - y[i];
+			H->times(1, 1.0, x, nV, 1.0, grad, nV);
+			for (i = 0; i < nV; i++) if (getAbs(grad[i]) > stat) stat = getAbs(grad[i]);
+
+			/* feasibility */
+			for (i = 0; i < nV; i++) if (lb[i] - x[i] > bfeas) bfeas = lb[i] - x[i];
+			for (i = 0; i < nV; i++) if (x[i] - ub[i] > bfeas) bfeas = x[i] - ub[i];
+
+			/* complementarity */
+			for (i = 0; i < nV; i++) if (y[i] > +EPS && getAbs((lb[i] - x[i])*y[i]) > bcmpl) bcmpl = getAbs((lb[i] - x[i])*y[i]);
+			for (i = 0; i < nV; i++) if (y[i] < -EPS && getAbs((ub[i] - x[i])*y[i]) > bcmpl) bcmpl = getAbs((ub[i] - x[i])*y[i]);
+
+			if (iter % 10 == 0)
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH, "\n%5s %4s %4s %9s %9s %9s %9s %9s\n",
+						"iter", "addB", "remB", "hom len", "tau", "stat", "bfeas", "bcmpl");
+			}
+			myPrintf( myPrintfString );
+
+			snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d ", iter);
+			myPrintf( myPrintfString );
+		
+			if (tabularOutput.idxAddB >= 0)
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH, "%4d ", tabularOutput.idxAddB);
+				myPrintf( myPrintfString );
+			}
+			else
+			{
+				myPrintf( "     " );
+			}
+
+			if (tabularOutput.idxRemB >= 0)
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH, "%4d ", tabularOutput.idxRemB);
+				myPrintf( myPrintfString );
+			}
+			else 
+			{
+				myPrintf( "     " );
+			}
+
+			snprintf( myPrintfString,MAX_STRING_LENGTH, "%9.2e %9.2e %9.2e %9.2e %9.2e\n",
+					homotopyLength, tau, stat, bfeas, bcmpl);
+			myPrintf( myPrintfString );
+
+			delete[] grad;
+			break;
+
+		case PL_TABULAR:
+			if (iter % 10 == 0)
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH, "\n%5s %6s %6s %9s %9s\n",
+						"iter", "addB", "remB", "hom len", "tau");
+				myPrintf( myPrintfString );
+			}
+
+			snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d ", iter);
+			myPrintf( myPrintfString );
+
+			if (tabularOutput.idxAddB >= 0)
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d%c ", tabularOutput.idxAddB, excStr[tabularOutput.excAddB]);
+				myPrintf( myPrintfString );
+			}
+			else
+			{
+				myPrintf( "       " );
+			}
+
+			if (tabularOutput.idxRemB >= 0)
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d%c ", tabularOutput.idxRemB, excStr[tabularOutput.excRemB]);
+				myPrintf( myPrintfString );
+			}
+			else 
+			{
+				myPrintf( "       " );
+			}
+					
+			snprintf( myPrintfString,MAX_STRING_LENGTH, "%9.2e %9.2e\n", homotopyLength, tau);
+			myPrintf( myPrintfString );
+			break;
+
+		case PL_MEDIUM:
+			/* 1) Print header at first iteration. */
+ 			if ( iter == 0 )
+			{
+				snprintf( myPrintfString,MAX_STRING_LENGTH,"\n\n#################   qpOASES  --  QP NO. %3.0d   ##################\n\n", count );
+				myPrintf( myPrintfString );
+
+				myPrintf( "    Iter   |    StepLength    |       Info       |   nFX    \n" );
+				myPrintf( " ----------+------------------+------------------+--------- \n" );
+			}
+
+			/* 2) Print iteration line. */
+			if ( BC_status == ST_UNDEFINED )
+			{
+				if ( hessianType == HST_ZERO )
+					snprintf( myPrintfString,MAX_STRING_LENGTH,"   %5.1d   |   %1.6e   |    LP SOLVED     |  %4.1d   \n", iter,tau,getNFX( ) );
+				else
+					snprintf( myPrintfString,MAX_STRING_LENGTH,"   %5.1d   |   %1.6e   |    QP SOLVED     |  %4.1d   \n", iter,tau,getNFX( ) );
+				myPrintf( myPrintfString );
+			}
+			else
+			{
+				char info[MAX_STRING_LENGTH];
+
+				if ( BC_status == ST_INACTIVE )
+					snprintf( info,8,"REM BND" );
+				else
+					snprintf( info,8,"ADD BND" );
+
+				snprintf( myPrintfString,MAX_STRING_LENGTH,"   %5.1d   |   %1.6e   |   %s %4.1d   |  %4.1d   \n", iter,tau,info,BC_idx,getNFX( ) );
+				myPrintf( myPrintfString );
+			}
+			break;
+
+		default:
+			/* nothing to display */
+			break;
+	}
+
+	#endif /* __XPCTARGET__ */
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/SQProblem.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/SQProblem.cpp
new file mode 100644
index 00000000000..8860f7dda34
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/SQProblem.cpp
@@ -0,0 +1,553 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file src/SQProblem.cpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of the SQProblem class which is able to use the newly
+ *	developed online active set strategy for parametric quadratic programming
+ *	with varying matrices.
+ */
+
+
+#include <qpOASES/SQProblem.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+/*
+ *	S Q P r o b l e m
+ */
+SQProblem::SQProblem( ) : QProblem( )
+{
+}
+
+
+/*
+ *	S Q P r o b l e m
+ */
+SQProblem::SQProblem( int _nV, int _nC, HessianType _hessianType ) : QProblem( _nV,_nC,_hessianType )
+{
+}
+
+
+/*
+ *	S Q P r o b l e m
+ */
+SQProblem::SQProblem( const SQProblem& rhs ) : QProblem( rhs )
+{
+}
+
+
+/*
+ *	~ S Q P r o b l e m
+ */
+SQProblem::~SQProblem( )
+{
+}
+
+
+/*
+ *	o p e r a t o r =
+ */
+SQProblem& SQProblem::operator=( const SQProblem& rhs )
+{
+	if ( this != &rhs )
+	{
+		QProblem::operator=( rhs );
+	}
+
+	return *this;
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+returnValue SQProblem::hotstart(	const real_t* const H_new, const real_t* const g_new, const real_t* const A_new,
+									const real_t* const lb_new, const real_t* const ub_new,
+									const real_t* const lbA_new, const real_t* const ubA_new,
+									int& nWSR, real_t* const cputime )
+{
+	if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
+		 ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
+		 ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
+	{
+		return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
+	}
+
+	/* start runtime measurement */
+	real_t starttime = 0.0;
+    if ( cputime != 0 )
+        starttime = getCPUtime( );
+
+
+	/* I) UPDATE QP MATRICES AND VECTORS */
+	if ( setupAuxiliaryQP( H_new,A_new,lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+
+	/* II) PERFORM USUAL HOMOTOPY */
+
+	/* Allow only remaining CPU time for usual hotstart. */
+	if ( cputime != 0 )
+		*cputime -= getCPUtime( ) - starttime;
+
+	returnValue returnvalue = QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
+
+
+	/* stop runtime measurement */
+	if ( cputime != 0 )
+		*cputime = getCPUtime( ) - starttime;
+
+	return returnvalue;
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+returnValue SQProblem::hotstart(	const char* const H_file, const char* const g_file, const char* const A_file,
+									const char* const lb_file, const char* const ub_file,
+									const char* const lbA_file, const char* const ubA_file,
+									int& nWSR, real_t* const cputime
+									)
+{
+	int nV = getNV( );
+	int nC = getNC( );
+
+	returnValue returnvalue;
+
+	/* consistency checks */
+	if ( ( H_file == 0 ) || ( g_file == 0 ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	if ( ( nC > 0 ) && ( A_file == 0 ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+
+	/* 1) Load new QP matrices from files. */
+	real_t* H_new  = new real_t[nV*nV];
+	real_t* A_new  = new real_t[nC*nV];
+
+	if ( readFromFile( H_new, nV,nV, H_file ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_UNABLE_TO_READ_FILE );
+
+	if ( readFromFile( A_new, nC,nV, A_file ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_UNABLE_TO_READ_FILE );
+
+	/* 2) Load new QP vectors from files. */
+	real_t* g_new  = new real_t[nV];
+	real_t* lb_new = 0;
+	real_t* ub_new = 0;
+	real_t* lbA_new = 0;
+	real_t* ubA_new = 0;
+
+	if ( lb_file != 0 )
+		lb_new = new real_t[nV];
+	if ( ub_file != 0 )
+		ub_new = new real_t[nV];
+	if ( lbA_file != 0 )
+		lbA_new = new real_t[nC];
+	if ( ubA_file != 0 )
+		ubA_new = new real_t[nC];
+
+	returnvalue = loadQPvectorsFromFile(	g_file,lb_file,ub_file,lbA_file,ubA_file,
+											g_new,lb_new,ub_new,lbA_new,ubA_new
+											);
+	if ( returnvalue != SUCCESSFUL_RETURN )
+	{
+		if ( ubA_file != 0 )
+			delete[] ubA_new;
+		if ( lbA_file != 0 )
+			delete[] lbA_new;
+		if ( ub_file != 0 )
+			delete[] ub_new;
+		if ( lb_file != 0 )
+			delete[] lb_new;
+		delete[] g_new;
+		delete[] A_new;
+		delete[] H_new;
+
+		return THROWERROR( RET_UNABLE_TO_READ_FILE );
+	}
+
+	/* 3) Actually perform hotstart. */
+	returnvalue = hotstart(	H_new,g_new,A_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
+
+	if ( ubA_file != 0 )
+		delete[] ubA_new;
+	if ( lbA_file != 0 )
+		delete[] lbA_new;
+	if ( ub_file != 0 )
+		delete[] ub_new;
+	if ( lb_file != 0 )
+		delete[] lb_new;
+	delete[] g_new;
+	delete[] A_new;
+	delete[] H_new;
+
+	return returnvalue;
+}
+
+/*
+ * h o t s t a r t
+ */
+returnValue SQProblem::hotstart(	SymmetricMatrix *H_new,
+									const real_t* const g_new,
+									Matrix *A_new,
+									const real_t* const lb_new,
+									const real_t* const ub_new,
+									const real_t* const lbA_new,
+									const real_t* const ubA_new,
+									int& nWSR,
+									real_t* const cputime
+									)
+{
+	if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
+		 ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
+		 ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
+	{
+		return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
+	}
+
+	/* start runtime measurement */
+	real_t starttime = 0.0;
+    if ( cputime != 0 )
+        starttime = getCPUtime( );
+
+
+	/* I) UPDATE QP MATRICES AND VECTORS */
+	if ( setupAuxiliaryQP( H_new,A_new,lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+
+	/* II) PERFORM USUAL HOMOTOPY */
+
+	/* Allow only remaining CPU time for usual hotstart. */
+	if ( cputime != 0 )
+		*cputime -= getCPUtime( ) - starttime;
+
+	returnValue returnvalue = QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
+
+
+	/* stop runtime measurement */
+	if ( cputime != 0 )
+		*cputime = getCPUtime( ) - starttime;
+
+	return returnvalue;
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+returnValue SQProblem::hotstart(	const real_t* const g_new,
+									const real_t* const lb_new, const real_t* const ub_new,
+									const real_t* const lbA_new, const real_t* const ubA_new,
+									int& nWSR, real_t* const cputime
+									)
+{
+	/* Call to hotstart function for fixed QP matrices. */
+	return QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+returnValue SQProblem::hotstart(	const char* const g_file,
+									const char* const lb_file, const char* const ub_file,
+									const char* const lbA_file, const char* const ubA_file,
+									int& nWSR, real_t* const cputime
+									)
+{
+	/* Call to hotstart function for fixed QP matrices. */
+	return QProblem::hotstart( g_file,lb_file,ub_file,lbA_file,ubA_file, nWSR,cputime );
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+returnValue SQProblem::hotstart(	const real_t* const g_new,
+									const real_t* const lb_new, const real_t* const ub_new,
+									const real_t* const lbA_new, const real_t* const ubA_new,
+									int& nWSR, real_t* const cputime,
+									const Bounds* const guessedBounds, const Constraints* const guessedConstraints
+									)
+{
+	/* Call to hotstart function for fixed QP matrices. */
+	return QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime, guessedBounds,guessedConstraints );
+}
+
+
+/*
+ *	h o t s t a r t
+ */
+returnValue SQProblem::hotstart(	const char* const g_file,
+									const char* const lb_file, const char* const ub_file,
+									const char* const lbA_file, const char* const ubA_file,
+									int& nWSR, real_t* const cputime,
+									const Bounds* const guessedBounds, const Constraints* const guessedConstraints
+									)
+{
+	/* Call to hotstart function for fixed QP matrices. */
+	return QProblem::hotstart( g_file,lb_file,ub_file,lbA_file,ubA_file, nWSR,cputime, guessedBounds,guessedConstraints );
+}
+
+
+
+#ifdef __MATLAB__
+returnValue SQProblem::resetMatrixPointers( )
+{
+	H = 0;
+	A = 0;
+
+	return SUCCESSFUL_RETURN;
+}
+#endif
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+/*
+ *	s e t u p A u x i l i a r y Q P
+ */
+returnValue SQProblem::setupAuxiliaryQP( const real_t* const H_new, const real_t* const A_new,
+    const real_t *lb_new, const real_t *ub_new, const real_t *lbA_new, const real_t *ubA_new )
+{
+	int nV = getNV( );
+	int nC = getNC( );
+
+	DenseMatrix *dA = new DenseMatrix(nC, nV, nV, (real_t*) A_new);
+	SymDenseMat *sH = new SymDenseMat(nV, nV, nV, (real_t*) H_new);
+
+	returnValue returnvalue = setupAuxiliaryQP ( sH, dA, lb_new, ub_new, lbA_new, ubA_new );
+
+	if ( H_new != 0 )
+		freeHessian = BT_TRUE;
+	freeConstraintMatrix = BT_TRUE;
+
+	return returnvalue;
+}
+
+
+/*
+ *	s e t u p A u x i l i a r y Q P
+ */
+returnValue SQProblem::setupAuxiliaryQP ( SymmetricMatrix *H_new, Matrix *A_new,
+    const real_t *lb_new, const real_t *ub_new, const real_t *lbA_new, const real_t *ubA_new
+)
+{
+
+	int i;
+	int nV = getNV( );
+	int nC = getNC( );
+	returnValue returnvalue;
+
+	if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
+		 ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
+		 ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
+	{
+		return THROWERROR( RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED );
+	}
+
+	status = QPS_PREPARINGAUXILIARYQP;
+
+
+	/* I) SETUP NEW QP MATRICES AND VECTORS: */
+	/* 1) Shift constraints' bounds vectors by (A_new - A)'*x_opt to ensure
+	 *    that old optimal solution remains feasible for new QP data. */
+	/*    Firstly, shift by -A'*x_opt and ... */
+	if ( nC > 0 )
+	{
+		if ( A_new == 0 )
+			return THROWERROR( RET_INVALID_ARGUMENTS );
+
+		for ( i=0; i<nC; ++i )
+		{
+			lbA[i] = -Ax_l[i];
+			ubA[i] =  Ax_u[i];
+		}
+
+		/* Set constraint matrix as well as ... */
+		setA( A_new );
+
+		/* ... secondly, shift by +A_new'*x_opt. */
+		for ( i=0; i<nC; ++i )
+		{
+			lbA[i] += Ax[i];
+			ubA[i] += Ax[i];
+		}
+
+		/* update constraint products. */
+		for ( i=0; i<nC; ++i )
+		{
+			Ax_u[i] = ubA[i] - Ax[i];
+			Ax_l[i] = Ax[i] - lbA[i];
+		}
+	}
+
+	/* 2) Set new Hessian matrix, determine Hessian type and
+	 *    regularise new Hessian matrix if necessary. */
+	/* a) Setup new Hessian matrix and determine its type. */
+	if ( H_new != 0 )
+	{
+		setH( H_new );
+
+		hessianType = HST_UNKNOWN;
+		if ( determineHessianType( ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+		/* b) Regularise new Hessian if necessary. */
+		if ( ( hessianType == HST_ZERO ) ||
+			 ( hessianType == HST_SEMIDEF ) ||
+			 ( usingRegularisation( ) == BT_TRUE ) )
+		{
+			regVal = 0.0; /* reset previous regularisation */
+
+			if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
+				return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+		}
+	}
+	else
+	{
+		if ( H != 0 )
+			return THROWERROR( RET_NO_HESSIAN_SPECIFIED );
+	}
+
+	/* 3) Setup QP gradient. */
+	if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
+		return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+
+	/* II) SETUP WORKING SETS AND MATRIX FACTORISATIONS: */
+	/* 1) Make a copy of current bounds/constraints ... */
+	Bounds      oldBounds      = bounds;
+	Constraints oldConstraints = constraints;
+
+    /* we're trying to find an active set with positive definite null
+     * space Hessian twice:
+     * - first for the current active set including all equalities
+     * - second after moving all inactive variables to a bound
+     *   (depending on Options). This creates an empty null space and
+     *   is guaranteed to succeed. Thus this loop will exit after n_try=1.
+     */
+    int n_try;
+    for (n_try = 0; n_try < 2; ++n_try) {
+
+        if (n_try > 0) {
+            // the current active set leaves an indefinite null space Hessian
+            // move all inactive variables to a bound, creating an empty null space
+            for (int ii = 0; ii < nV; ++ii)
+                if (oldBounds.getStatus (ii) == ST_INACTIVE)
+                    oldBounds.setStatus (ii, options.initialStatusBounds);
+        }
+
+        /*    ... reset them ... */
+        bounds.init( nV );
+        constraints.init( nC );
+
+        /*    ... and set them up afresh. */
+        if ( setupSubjectToType(lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
+            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+        if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
+            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+        if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
+            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+        /* 2) Setup TQ factorisation. */
+        if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
+            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+		// check for equalities that have become bounds ...
+		for (int ii = 0; ii < nC; ++ii) {
+			if (oldConstraints.getType (ii) == ST_EQUALITY && constraints.getType (ii) == ST_BOUNDED) {
+				if (oldConstraints.getStatus (ii) == ST_LOWER && y[nV+ii] < 0.0)
+					oldConstraints.setStatus (ii, ST_UPPER);
+				else if (oldConstraints.getStatus (ii) == ST_UPPER && y[nV+ii] > 0.0)
+					oldConstraints.setStatus (ii, ST_LOWER);
+			}
+		}
+
+		// ... and do the same also for the bounds!
+		for (int ii = 0; ii < nV; ++ii) {
+			if (oldBounds.getType(ii) == ST_EQUALITY
+					&& bounds.getType(ii) == ST_BOUNDED) {
+				if (oldBounds.getStatus(ii) == ST_LOWER && y[ii] < 0.0)
+					oldBounds.setStatus(ii, ST_UPPER);
+				else if (oldBounds.getStatus(ii) == ST_UPPER && y[ii] > 0.0)
+					oldBounds.setStatus(ii, ST_LOWER);
+			}
+		}
+
+        /* 3) Setup old working sets afresh (updating TQ factorisation). */
+        if ( setupAuxiliaryWorkingSet( &oldBounds,&oldConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
+            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
+
+        /* Factorise projected Hessian
+        * this now handles all special cases (no active bounds/constraints, no nullspace) */
+        returnvalue = computeProjectedCholesky( );
+
+        /* leave the loop if decomposition was successful, i.e. we have
+         * found an active set with positive definite null space Hessian */
+        if ( returnvalue == SUCCESSFUL_RETURN )
+            break;
+    }
+
+    /* adjust lb/ub if we changed the old active set in the second try
+     */
+    if (n_try > 0) {
+		// as per setupAuxiliaryQPbounds assumptions ... oh the troubles
+		for (int ii = 0; ii < nC; ++ii)
+			Ax_l[ii] = Ax_u[ii] = Ax[ii];
+        setupAuxiliaryQPbounds (&bounds, &constraints, BT_FALSE);
+	}
+
+	status = QPS_AUXILIARYQPSOLVED;
+
+	return SUCCESSFUL_RETURN;
+}
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/SolutionAnalysis.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/SolutionAnalysis.cpp
new file mode 100644
index 00000000000..d6b766c3db6
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/SolutionAnalysis.cpp
@@ -0,0 +1,567 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file src/SolutionAnalysis.cpp
+ *	\author Hans Joachim Ferreau (thanks to Boris Houska)
+ *	\version 3.0
+ *	\date 2008-2014
+ *
+ *	Implementation of the SolutionAnalysis class designed to perform
+ *	additional analysis after solving a QP with qpOASES.
+ *
+ */
+
+
+#include <qpOASES/extras/SolutionAnalysis.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+/*
+ *	S o l u t i o n A n a l y s i s
+ */
+SolutionAnalysis::SolutionAnalysis( )
+{
+
+}
+
+
+/*
+ *	S o l u t i o n A n a l y s i s
+ */
+SolutionAnalysis::SolutionAnalysis( const SolutionAnalysis& rhs )
+{
+
+}
+
+
+/*
+ *	~ S o l u t i o n A n a l y s i s
+ */
+SolutionAnalysis::~SolutionAnalysis( )
+{
+
+}
+
+
+/*
+ *	o p e r a t o r =
+ */
+SolutionAnalysis& SolutionAnalysis::operator=( const SolutionAnalysis& rhs )
+{
+	if ( this != &rhs )
+	{
+
+	}
+
+	return *this;
+}
+
+
+
+/*
+ *	g e t M a x K K T v i o l a t i o n
+ */
+returnValue SolutionAnalysis::getMaxKKTviolation( QProblemB* qp, real_t& maxKKTviolation ) const
+{
+	int i;
+	int nV = qp->getNV( );
+
+	real_t *tmp = new real_t[nV];
+	maxKKTviolation = 0.0;
+
+
+	/* 1) Check for Hx + g - y*A' = 0  (here: A = Id). */
+	for( i=0; i<nV; ++i )
+		tmp[i] = qp->g[i];
+
+	switch ( qp->getHessianType( ) )
+	{
+		case HST_ZERO:
+			/*tmp += qp->eps * qp->x[i]; */
+			break;
+
+		case HST_IDENTITY:
+			for( i=0; i<nV; ++i )
+				tmp[i] += qp->x[i];
+			break;
+
+		default:
+			qp->H->times(1, 1.0, qp->x, nV, 1.0, tmp, nV);
+			break;
+	}
+
+	for( i=0; i<nV; ++i )
+	{
+		tmp[i] -= qp->y[i];
+
+		if ( getAbs( tmp[i] ) > maxKKTviolation )
+			maxKKTviolation = getAbs( tmp[i] );
+	}
+	delete[] tmp;
+
+	/* 2) Check for lb <= x <= ub. */
+	for( i=0; i<nV; ++i )
+	{
+		if ( qp->lb[i] - qp->x[i] > maxKKTviolation )
+			maxKKTviolation = qp->lb[i] - qp->x[i];
+
+		if ( qp->x[i] - qp->ub[i] > maxKKTviolation )
+			maxKKTviolation = qp->x[i] - qp->ub[i];
+	}
+
+	/* 3) Check for correct sign of y and for complementary slackness. */
+	for( i=0; i<nV; ++i )
+	{
+		switch ( qp->bounds.getStatus( i ) )
+		{
+			case ST_LOWER:
+				if ( -qp->y[i] > maxKKTviolation )
+					maxKKTviolation = -qp->y[i];
+				if ( getAbs( ( qp->x[i] - qp->lb[i] ) * qp->y[i] ) > maxKKTviolation )
+					maxKKTviolation = getAbs( ( qp->x[i] - qp->lb[i] ) * qp->y[i] );
+				break;
+
+			case ST_UPPER:
+				if ( qp->y[i] > maxKKTviolation )
+					maxKKTviolation = qp->y[i];
+				if ( getAbs( ( qp->ub[i] - qp->x[i] ) * qp->y[i] ) > maxKKTviolation )
+					maxKKTviolation = getAbs( ( qp->ub[i] - qp->x[i] ) * qp->y[i] );
+				break;
+
+			default: /* inactive */
+			if ( getAbs( qp->y[i] ) > maxKKTviolation )
+					maxKKTviolation = getAbs( qp->y[i] );
+				break;
+		}
+	}
+
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t M a x K K T v i o l a t i o n
+ */
+returnValue SolutionAnalysis::getMaxKKTviolation( QProblem* qp, real_t& maxKKTviolation ) const
+{
+	int i;
+	int nV = qp->getNV( );
+	int nC = qp->getNC( );
+
+	real_t *tmp = new real_t[nV];
+	maxKKTviolation = 0.0;
+
+
+	/* 1) check for Hx + g - [yFX yAC]*[Id A]' = 0. */
+	for( i=0; i<nV; ++i )
+		tmp[i] = qp->g[i];
+
+	switch ( qp->getHessianType( ) )
+	{
+		case HST_ZERO:
+				/*tmp += qp->eps * qp->x[i]; */
+			break;
+
+		case HST_IDENTITY:
+			for( i=0; i<nV; ++i )
+				tmp[i] += qp->x[i];
+			break;
+
+		default:
+			qp->H->times(1, 1.0, qp->x, nV, 1.0, tmp, nV);
+			break;
+	}
+
+	qp->A->transTimes(1, -1.0, qp->y + nV, nC, 1.0, tmp, nV);
+
+	for( i=0; i<nV; ++i )
+	{
+		tmp[i] -= qp->y[i];
+
+		if ( getAbs( tmp[i] ) > maxKKTviolation )
+			maxKKTviolation = getAbs( tmp[i] );
+	}
+
+	/* 2) Check for [lb lbA] <= [Id A]*x <= [ub ubA]. */
+	/* lbA <= Ax <= ubA */
+	real_t* Ax = new real_t[nC];
+	qp->A->times(1, 1.0, qp->x, nV, 0.0, Ax, nC);
+
+	for( i=0; i<nC; ++i )
+	{
+		if ( qp->lbA[i] - Ax[i] > maxKKTviolation )
+			maxKKTviolation = qp->lbA[i] - Ax[i];
+
+		if ( Ax[i] - qp->ubA[i] > maxKKTviolation )
+			maxKKTviolation = Ax[i] - qp->ubA[i];
+	}
+
+	/* lb <= x <= ub */
+	for( i=0; i<nV; ++i )
+	{
+		if ( qp->lb[i] - qp->x[i] > maxKKTviolation )
+			maxKKTviolation = qp->lb[i] - qp->x[i];
+
+		if ( qp->x[i] - qp->ub[i] > maxKKTviolation )
+			maxKKTviolation = qp->x[i] - qp->ub[i];
+	}
+
+	/* 3) Check for correct sign of y and for complementary slackness. */
+	/* bounds */
+	for( i=0; i<nV; ++i )
+	{
+		switch ( qp->bounds.getStatus( i ) )
+		{
+			case ST_LOWER:
+				if ( -qp->y[i] > maxKKTviolation )
+					maxKKTviolation = -qp->y[i];
+				if ( getAbs( ( qp->x[i] - qp->lb[i] ) * qp->y[i] ) > maxKKTviolation )
+					maxKKTviolation = getAbs( ( qp->x[i] - qp->lb[i] ) * qp->y[i] );
+				break;
+
+			case ST_UPPER:
+				if ( qp->y[i] > maxKKTviolation )
+					maxKKTviolation = qp->y[i];
+				if ( getAbs( ( qp->ub[i] - qp->x[i] ) * qp->y[i] ) > maxKKTviolation )
+					maxKKTviolation = getAbs( ( qp->ub[i] - qp->x[i] ) * qp->y[i] );
+				break;
+
+			default: /* inactive */
+			if ( getAbs( qp->y[i] ) > maxKKTviolation )
+					maxKKTviolation = getAbs( qp->y[i] );
+				break;
+		}
+	}
+
+	/* constraints */
+	for( i=0; i<nC; ++i )
+	{
+		switch ( qp->constraints.getStatus( i ) )
+		{
+			case ST_LOWER:
+				if ( -qp->y[nV+i] > maxKKTviolation )
+					maxKKTviolation = -qp->y[nV+i];
+				if ( getAbs( ( Ax[i] - qp->lbA[i] ) * qp->y[nV+i] ) > maxKKTviolation )
+					maxKKTviolation = getAbs( ( Ax[i] - qp->lbA[i] ) * qp->y[nV+i] );
+				break;
+
+			case ST_UPPER:
+				if ( qp->y[nV+i] > maxKKTviolation )
+					maxKKTviolation = qp->y[nV+i];
+				if ( getAbs( ( qp->ubA[i] - Ax[i] ) * qp->y[nV+i] ) > maxKKTviolation )
+					maxKKTviolation = getAbs( ( qp->ubA[i] - Ax[i] ) * qp->y[nV+i] );
+				break;
+
+			default: /* inactive */
+			if ( getAbs( qp->y[nV+i] ) > maxKKTviolation )
+					maxKKTviolation = getAbs( qp->y[nV+i] );
+				break;
+		}
+	}
+
+	delete[] tmp;
+	delete[] Ax;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t M a x K K T v i o l a t i o n
+ */
+returnValue SolutionAnalysis::getMaxKKTviolation( SQProblem* qp, real_t& maxKKTviolation ) const
+{
+	return getMaxKKTviolation( (QProblem*)qp,maxKKTviolation );
+}
+
+
+
+/*
+ *	g e t V a r i a n c e C o v a r i a n c e
+ */
+returnValue SolutionAnalysis::getVarianceCovariance( QProblemB* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR ) const
+{
+	return THROWERROR( RET_NOT_YET_IMPLEMENTED );
+}
+
+
+/*
+ *	g e t V a r i a n c e C o v a r i a n c e
+ */
+returnValue SolutionAnalysis::getVarianceCovariance( QProblem* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR ) const
+{
+
+  /* DEFINITION OF THE DIMENSIONS nV AND nC:
+   * --------------------------------------- */
+  int nV  = qp->getNV( );                      /* dimension of x / the bounds */
+  int nC  = qp->getNC( );                      /* dimension of the constraints */
+  int dim = 2*nV+nC;                           /* dimension of input and output */
+                                               /* variance-covariance matrix */
+  int run1, run2, run3;                        /* simple run variables (for loops). */
+
+
+  /* ALLOCATION OF MEMORY:
+   * --------------------- */
+  real_t* delta_g_cov    = new real_t[nV];     /* a covariance-vector of g */
+  real_t* delta_lb_cov   = new real_t[nV];     /* a covariance-vector of lb */
+  real_t* delta_ub_cov   = new real_t[nV];     /* a covariance-vector of ub */
+  real_t* delta_lbA_cov  = new real_t[nC];     /* a covariance-vector of lbA */
+  real_t* delta_ubA_cov  = new real_t[nC];     /* a covariance-vector of ubA */
+
+  returnValue returnvalue;                     /* the return value */
+  BooleanType Delta_bC_isZero = BT_FALSE;      /* (just use FALSE here) */
+  BooleanType Delta_bB_isZero = BT_FALSE;      /* (just use FALSE here) */
+
+
+
+  /* ASK FOR THE NUMBER OF FREE AND FIXED VARIABLES:
+   * (ASSUMES THAT ACTIVE SET IS CONSTANT FOR THE
+   *  VARIANCE-COVARIANCE EVALUATION)
+   * ----------------------------------------------- */
+  int nFR, nFX, nAC;
+
+  nFR = qp->getNFR( );
+  nFX = qp->getNFX( );
+  nAC = qp->getNAC( );
+
+
+  /* ASK FOR THE CORRESPONDING INDEX ARRAYS:
+   * --------------------------------------- */
+  int *FR_idx, *FX_idx, *AC_idx;
+
+  if ( qp->bounds.getFree( )->getNumberArray( &FR_idx ) != SUCCESSFUL_RETURN )
+       return THROWERROR( RET_HOTSTART_FAILED );
+
+  if ( qp->bounds.getFixed( )->getNumberArray( &FX_idx ) != SUCCESSFUL_RETURN )
+       return THROWERROR( RET_HOTSTART_FAILED );
+
+  if ( qp->constraints.getActive( )->getNumberArray( &AC_idx ) != SUCCESSFUL_RETURN )
+       return THROWERROR( RET_HOTSTART_FAILED );
+
+
+
+  /* INTRODUCE VARIABLES TO MEASURE THE REACTION OF THE QP-SOLUTION TO
+   * THE VARIANCE-COVARIANCE DISTURBANCE:
+   * ----------------------------------------------------------------- */
+  real_t *delta_xFR = new real_t[nFR];
+  real_t *delta_xFX = new real_t[nFX];
+  real_t *delta_yAC = new real_t[nAC];
+  real_t *delta_yFX = new real_t[nFX];
+
+  real_t* K             = new real_t[dim*dim];  /* matrix to store */
+                                                /* an intermediate */
+                                                /* result. */
+
+  /* SOME INITIALIZATIONS:
+   * --------------------- */
+  for( run1 = 0; run1 < dim*dim; run1++ ){
+    K              [run1] = 0.0;
+    Primal_Dual_VAR[run1] = 0.0;
+  }
+
+
+  /* ================================================================= */
+
+  /* FIRST MATRIX MULTIPLICATION (OBTAINS THE INTERMEDIATE RESULT
+   *  K := [ ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * g_b_bA_VAR ]^T )
+   * THE EVALUATION OF THE INVERSE OF THE KKT-MATRIX OF THE QP
+   * WITH RESPECT TO THE CURRENT ACTIVE SET
+   * USES THE EXISTING CHOLESKY AND TQ-DECOMPOSITIONS. FOR DETAILS
+   * cf. THE (protected) FUNCTION determineStepDirection. */
+
+  for( run3 = 0; run3 < dim; run3++ ){
+
+
+    for( run1 = 0; run1 < nV; run1++ ){
+      delta_g_cov  [run1]   = g_b_bA_VAR[run3*dim+run1];
+      delta_lb_cov [run1]   = g_b_bA_VAR[run3*dim+nV+run1];         /*  LINE-WISE LOADING OF THE INPUT */
+      delta_ub_cov [run1]   = g_b_bA_VAR[run3*dim+nV+run1];         /*  VARIANCE-COVARIANCE            */
+    }
+    for( run1 = 0; run1 < nC; run1++ ){
+      delta_lbA_cov [run1]  = g_b_bA_VAR[run3*dim+2*nV+run1];
+      delta_ubA_cov [run1]  = g_b_bA_VAR[run3*dim+2*nV+run1];
+    }
+
+
+    /* EVALUATION OF THE STEP:
+     * ------------------------------------------------------------------------------ */
+
+    returnvalue = qp->determineStepDirection( delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
+                                              Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
+                                              delta_yAC,delta_yFX );
+
+    /* ------------------------------------------------------------------------------ */
+
+
+    /* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
+     * ------------------------------------------------------ */
+    if ( returnvalue != SUCCESSFUL_RETURN ){
+
+      delete[] delta_g_cov;
+      delete[] delta_lb_cov;
+      delete[] delta_ub_cov;
+      delete[] delta_lbA_cov;
+      delete[] delta_ubA_cov;
+      delete[] delta_xFR;
+      delete[] delta_xFX;
+      delete[] delta_yAC;
+      delete[] delta_yFX;
+      delete[] K;
+
+      THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
+      return returnvalue;
+    }
+
+
+
+    for( run1=0; run1<nFR; run1++ ){
+      run2                  = FR_idx[run1];
+      K[run3*dim+run2]      = delta_xFR[run1];
+    }                                                               /*  LINE WISE                  */
+    for( run1=0; run1<nFX; run1++ ){                                /*  STORAGE OF THE QP-REACTION */
+      run2                  = FX_idx[run1];                         /*  (uses the index list)      */
+      K[run3*dim+run2]      = delta_xFX[run1];
+      K[run3*dim+nV+run2]   = delta_yFX[run1];
+    }
+    for( run1=0; run1<nAC; run1++ ){
+      run2                  = AC_idx[run1];
+      K[run3*dim+2*nV+run2] = delta_yAC[run1];
+    }
+
+  }
+
+
+  /* ================================================================= */
+
+  /* SECOND MATRIX MULTIPLICATION (OBTAINS THE FINAL RESULT
+   * Primal_Dual_VAR := ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * K )
+   * THE APPLICATION OF THE KKT-INVERSE IS AGAIN REALIZED
+   * BY USING THE PROTECTED FUNCTION
+   * determineStepDirection */
+
+  for( run3 = 0; run3 < dim; run3++ ){
+
+    for( run1 = 0; run1 < nV; run1++ ){
+      delta_g_cov  [run1]   = K[run3+     run1*dim];
+      delta_lb_cov [run1]   = K[run3+(nV+run1)*dim];                /*  ROW WISE LOADING OF THE */
+      delta_ub_cov [run1]   = K[run3+(nV+run1)*dim];                /*  INTERMEDIATE RESULT K   */
+    }
+    for( run1 = 0; run1 < nC; run1++ ){
+      delta_lbA_cov [run1]  = K[run3+(2*nV+run1)*dim];
+      delta_ubA_cov [run1]  = K[run3+(2*nV+run1)*dim];
+    }
+
+
+    /* EVALUATION OF THE STEP:
+     * ------------------------------------------------------------------------------ */
+
+    returnvalue = qp->determineStepDirection( delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
+                                              Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
+                                              delta_yAC,delta_yFX);
+
+
+    /* ------------------------------------------------------------------------------ */
+
+
+    /* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
+     * ------------------------------------------------------ */
+    if ( returnvalue != SUCCESSFUL_RETURN ){
+
+      delete[] delta_g_cov;
+      delete[] delta_lb_cov;
+      delete[] delta_ub_cov;
+      delete[] delta_lbA_cov;
+      delete[] delta_ubA_cov;
+      delete[] delta_xFR;
+      delete[] delta_xFX;
+      delete[] delta_yAC;
+      delete[] delta_yFX;
+      delete[] K;
+
+      THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
+      return returnvalue;
+    }
+
+
+
+    for( run1=0; run1<nFR; run1++ ){
+      run2                                = FR_idx[run1];
+      Primal_Dual_VAR[run3+run2*dim]      = delta_xFR[run1];
+    }
+    for( run1=0; run1<nFX; run1++ ){                                 /*  ROW-WISE STORAGE */
+      run2                  = FX_idx[run1];                          /*  OF THE RESULT.   */
+      Primal_Dual_VAR[run3+run2*dim     ]   = delta_xFX[run1];
+      Primal_Dual_VAR[run3+(nV+run2)*dim]   = delta_yFX[run1];
+    }
+    for( run1=0; run1<nAC; run1++ ){
+      run2                                  = AC_idx[run1];
+      Primal_Dual_VAR[run3+(2*nV+run2)*dim] = delta_yAC[run1];
+    }
+
+  }
+
+
+  /* DEALOCATE MEMORY:
+   * ----------------- */
+
+  delete[] delta_g_cov;
+  delete[] delta_lb_cov;
+  delete[] delta_ub_cov;
+  delete[] delta_lbA_cov;
+  delete[] delta_ubA_cov;
+  delete[] delta_xFR;
+  delete[] delta_xFX;
+  delete[] delta_yAC;
+  delete[] delta_yFX;
+  delete[] K;
+
+  return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t V a r i a n c e C o v a r i a n c e
+ */
+returnValue SolutionAnalysis::getVarianceCovariance( SQProblem* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR ) const
+{
+	/* Call QProblem variant. */
+	return getVarianceCovariance( (QProblem*)qp,g_b_bA_VAR,Primal_Dual_VAR );
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/SubjectTo.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/SubjectTo.cpp
new file mode 100644
index 00000000000..f135769655b
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/SubjectTo.cpp
@@ -0,0 +1,289 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file src/SubjectTo.cpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of the SubjectTo class designed to manage working sets of
+ *	constraints and bounds within a QProblem.
+ */
+
+
+#include <qpOASES/SubjectTo.hpp>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*****************************************************************************
+ *  P U B L I C                                                              *
+ *****************************************************************************/
+
+
+/*
+ *	S u b j e c t T o
+ */
+SubjectTo::SubjectTo( )
+{
+	type   = 0;
+	status = 0;
+
+	init( );
+}
+
+
+/*
+ *	S u b j e c t T o
+ */
+SubjectTo::SubjectTo( int _n )
+{
+	type   = 0;
+	status = 0;
+
+	init( _n );
+}
+
+
+/*
+ *	S u b j e c t T o
+ */
+SubjectTo::SubjectTo( const SubjectTo& rhs )
+{
+	copy( rhs );
+}
+
+
+/*
+ *	~ S u b j e c t T o
+ */
+SubjectTo::~SubjectTo( )
+{
+	clear( );
+}
+
+
+/*
+ *	o p e r a t o r =
+ */
+SubjectTo& SubjectTo::operator=( const SubjectTo& rhs )
+{
+	if ( this != &rhs )
+	{
+		clear( );
+		copy( rhs );
+	}
+
+	return *this;
+}
+
+
+/*
+ *	i n i t
+ */
+returnValue SubjectTo::init(	int _n
+								)
+{
+	int i;
+
+	if ( _n < 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	clear( );
+
+	n = _n;
+	noLower = BT_TRUE;
+	noUpper = BT_TRUE;
+
+	if ( n > 0 )
+	{
+		type   = new SubjectToType[n];
+		status = new SubjectToStatus[n];
+
+		for( i=0; i<n; ++i )
+		{
+			type[i]   = ST_UNKNOWN;
+			status[i] = ST_UNDEFINED;
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+/*
+ *	c l e a r
+ */
+returnValue SubjectTo::clear( )
+{
+	if ( type != 0 )
+	{
+		delete[] type;
+		type = 0;
+	}
+
+	if ( status != 0 )
+	{
+		delete[] status;
+		status = 0;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	c o p y
+ */
+returnValue SubjectTo::copy(	const SubjectTo& rhs
+								)
+{
+	int i;
+
+	n = rhs.n;
+	noLower = rhs.noLower;
+	noUpper = rhs.noUpper;
+
+	if ( rhs.n != 0 )
+	{
+		type   = new SubjectToType[n];
+		status = new SubjectToStatus[n];
+
+		for( i=0; i<n; ++i )
+		{
+			type[i]   = rhs.type[i];
+			status[i] = rhs.status[i];
+		}
+	}
+	else
+	{
+		type   = 0;
+		status = 0;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	a d d I n d e x
+ */
+returnValue SubjectTo::addIndex(	Indexlist* const indexlist,
+									int newnumber, SubjectToStatus newstatus
+									)
+{
+	if ( status != 0 )
+	{
+		/* consistency check */
+		if ( status[newnumber] == newstatus )
+			return THROWERROR( RET_INDEX_ALREADY_OF_DESIRED_STATUS );
+
+		status[newnumber] = newstatus;
+	}
+	else
+		return THROWERROR( RET_ADDINDEX_FAILED );
+
+	if ( indexlist != 0 )
+	{
+		if ( indexlist->addNumber( newnumber ) == RET_INDEXLIST_EXCEEDS_MAX_LENGTH )
+			return THROWERROR( RET_ADDINDEX_FAILED );
+	}
+	else
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	r e m o v e I n d e x
+ */
+returnValue SubjectTo::removeIndex(	Indexlist* const indexlist,
+									int removenumber
+									)
+{
+	if ( status != 0 )
+		status[removenumber] = ST_UNDEFINED;
+	else
+		return THROWERROR( RET_REMOVEINDEX_FAILED );
+
+	if ( indexlist != 0 )
+	{
+		if ( indexlist->removeNumber( removenumber ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_REMOVEINDEX_FAILED );
+	}
+	else
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s w a p I n d e x
+ */
+returnValue SubjectTo::swapIndex(	Indexlist* const indexlist,
+									int number1, int number2
+									)
+{
+	/* consistency checks */
+	if ( status != 0 )
+	{
+		if ( status[number1] != status[number2] )
+			return THROWERROR( RET_SWAPINDEX_FAILED );
+	}
+	else
+		return THROWERROR( RET_SWAPINDEX_FAILED );
+
+	if ( number1 == number2 )
+	{
+		THROWWARNING( RET_NOTHING_TO_DO );
+		return SUCCESSFUL_RETURN;
+	}
+
+	if ( indexlist != 0 )
+	{
+		if ( indexlist->swapNumbers( number1,number2 ) != SUCCESSFUL_RETURN )
+			return THROWERROR( RET_SWAPINDEX_FAILED );
+	}
+	else
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/Utils.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/Utils.cpp
new file mode 100644
index 00000000000..a76bc7eaa34
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/Utils.cpp
@@ -0,0 +1,984 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file src/Utils.cpp
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches (thanks to Eckhard Arnold)
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Implementation of some utility functions for working with qpOASES.
+ */
+
+
+#include <math.h>
+
+#if defined(__WIN32__) || defined(WIN32)
+  #include <windows.h>
+#elif defined(LINUX) || defined(__LINUX__)
+  #include <sys/stat.h>
+  #include <sys/time.h>
+#endif
+
+#ifdef __MATLAB__
+  #include "mex.h"
+#endif
+
+#ifdef __SCILAB__
+  #include <scilab/sciprint.h>
+#endif
+
+
+#include <qpOASES/Utils.hpp>
+
+
+#ifdef __NO_SNPRINTF__
+#if (!defined(_MSC_VER)) || defined(__DSPACE__)
+/* If snprintf is not available, provide an empty implementation. */
+int snprintf( char* s, size_t n, const char* format, ... )
+{
+	if ( n > 0 )
+		s[0] = '\0';
+
+	return 0;
+}
+#endif
+#endif /* __NO_SNPRINTF__ */
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/*
+ *	p r i n t
+ */
+returnValue print( const real_t* const v, int n, const char* name )
+{
+	#ifndef __SUPPRESSANYOUTPUT__
+	#ifndef __XPCTARGET__
+	int i;
+	char myPrintfString[MAX_STRING_LENGTH];
+
+	/* Print vector name. */
+	if ( name != 0 )
+	{
+		snprintf( myPrintfString,MAX_STRING_LENGTH,"%s = \n", name );
+		myPrintf( myPrintfString );
+	}
+
+	/* Print vector data. */
+	for( i=0; i<n; ++i )
+	{
+		snprintf( myPrintfString,MAX_STRING_LENGTH," %.16e\t", v[i] );
+		myPrintf( myPrintfString );
+	}
+	myPrintf( "\n" );
+	#endif /* __XPCTARGET__ */
+	#endif /* __SUPPRESSANYOUTPUT__ */
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	p r i n t
+ */
+returnValue print(	const real_t* const v, int n, const int* const V_idx, const char* name )
+{
+	#ifndef __SUPPRESSANYOUTPUT__
+	#ifndef __XPCTARGET__
+	int i;
+	char myPrintfString[MAX_STRING_LENGTH];
+
+	/* Print vector name. */
+	if ( name != 0 )
+	{
+		snprintf( myPrintfString,MAX_STRING_LENGTH,"%s = \n", name );
+		myPrintf( myPrintfString );
+	}
+
+	/* Print a permuted vector data. */
+	for( i=0; i<n; ++i )
+	{
+		snprintf( myPrintfString,MAX_STRING_LENGTH," %.16e\t", v[ V_idx[i] ] );
+		myPrintf( myPrintfString );
+	}
+	myPrintf( "\n" );
+	#endif /* __XPCTARGET__ */
+	#endif /* __SUPPRESSANYOUTPUT__ */
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	p r i n t
+ */
+returnValue print( const real_t* const M, int nrow, int ncol, const char* name )
+{
+	#ifndef __SUPPRESSANYOUTPUT__
+	#ifndef __XPCTARGET__
+	int i;
+	char myPrintfString[MAX_STRING_LENGTH];
+
+	/* Print matrix name. */
+	if ( name != 0 )
+	{
+		snprintf( myPrintfString,MAX_STRING_LENGTH,"%s = \n", name );
+		myPrintf( myPrintfString );
+	}
+
+	/* Print a matrix data as a collection of row vectors. */
+	for( i=0; i<nrow; ++i )
+		print( &(M[i*ncol]), ncol );
+	myPrintf( "\n" );
+	#endif /* __XPCTARGET__ */
+	#endif /* __SUPPRESSANYOUTPUT__ */
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	p r i n t
+ */
+returnValue print(	const real_t* const M, int nrow, int ncol, const int* const ROW_idx, const int* const COL_idx, const char* name )
+{
+	#ifndef __SUPPRESSANYOUTPUT__
+	#ifndef __XPCTARGET__
+	int i;
+	char myPrintfString[MAX_STRING_LENGTH];
+
+	/* Print matrix name. */
+	if ( name != 0 )
+	{
+		snprintf( myPrintfString,MAX_STRING_LENGTH,"%s = \n", name );
+		myPrintf( myPrintfString );
+	}
+
+	/* Print a permuted matrix data as a collection of permuted row vectors. */
+	for( i=0; i<nrow; ++i )
+		print( &( M[ ROW_idx[i]*ncol ] ), ncol, COL_idx );
+	myPrintf( "\n" );
+	#endif /* __XPCTARGET__ */
+	#endif /* __SUPPRESSANYOUTPUT__ */
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	p r i n t
+ */
+returnValue print( const int* const index, int n, const char* name )
+{
+	#ifndef __SUPPRESSANYOUTPUT__
+	#ifndef __XPCTARGET__
+	int i;
+	char myPrintfString[MAX_STRING_LENGTH];
+
+	/* Print indexlist name. */
+	if ( name != 0 )
+	{
+		snprintf( myPrintfString,MAX_STRING_LENGTH,"%s = \n", name );
+		myPrintf( myPrintfString );
+	}
+
+	/* Print a indexlist data. */
+	for( i=0; i<n; ++i )
+	{
+		snprintf( myPrintfString,MAX_STRING_LENGTH," %d\t", index[i] );
+		myPrintf( myPrintfString );
+	}
+	myPrintf( "\n" );
+	#endif /* __XPCTARGET__ */
+	#endif /* __SUPPRESSANYOUTPUT__ */
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	m y P r i n t f
+ */
+returnValue myPrintf( const char* s )
+{
+	#ifndef __SUPPRESSANYOUTPUT__
+	#ifndef __XPCTARGET__
+	
+		if ( s == 0 )
+			return RET_INVALID_ARGUMENTS;
+		
+		#ifdef __MATLAB__
+			mexPrintf( s );
+		#else
+			#ifdef __SCILAB__
+				sciprint( s );
+			#else
+				FILE* outputfile = getGlobalMessageHandler( )->getOutputFile( );
+				if ( outputfile == 0 )
+					return THROWERROR( RET_NO_GLOBAL_MESSAGE_OUTPUTFILE );
+				fprintf( outputfile, "%s", s );
+			#endif /* __SCILAB__ */
+		#endif /* __MATLAB__ */
+
+	#endif /* __XPCTARGET__ */
+	#endif /* __SUPPRESSANYOUTPUT__ */
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	p r i n t C o p y r i g h t N o t i c e
+ */
+returnValue printCopyrightNotice( )
+{
+	#ifndef __SUPPRESSANYOUTPUT__
+		#ifndef __XPCTARGET__
+		#ifndef __DSPACE__
+		#ifndef __NO_COPYRIGHT__
+		myPrintf( "\nqpOASES -- An Implementation of the Online Active Set Strategy.\nCopyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,\nChristian Kirches et al. All rights reserved.\n\nqpOASES is distributed under the terms of the \nGNU Lesser General Public License 2.1 in the hope that it will be \nuseful, but WITHOUT ANY WARRANTY; without even the implied warranty \nof MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. \nSee the GNU Lesser General Public License for more details.\n\n" );
+		#endif /* __NO_COPYRIGHT__ */
+		#endif /* __DSPACE__ */
+		#endif /* __XPCTARGET__ */
+	#endif /* __SUPPRESSANYOUTPUT__ */
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	r e a d F r o m F i l e
+ */
+returnValue readFromFile(	real_t* data, int nrow, int ncol,
+							const char* datafilename
+							)
+{
+	#ifndef __XPCTARGET__
+	int i, j;
+	double float_data;
+	FILE* datafile;
+
+	/* 1) Open file. */
+	if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
+	{
+		char errstr[MAX_STRING_LENGTH];
+		snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
+		return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+	}
+
+	/* 2) Read data from file. */
+	for( i=0; i<nrow; ++i )
+	{
+		for( j=0; j<ncol; ++j )
+		{
+			//#ifdef __USE_SINGLE_PRECISION__
+			//if ( fscanf( datafile, "%f ", &float_data ) == 0 )
+			//#else
+			if ( fscanf( datafile, "%lf ", &float_data ) == 0 )
+			//#endif /* __USE_SINGLE_PRECISION__ */
+			{
+				fclose( datafile );
+				char errstr[MAX_STRING_LENGTH];
+				snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
+				return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+			}
+			data[i*ncol + j] = ( (real_t) float_data );
+		}
+	}
+
+	/* 3) Close file. */
+	fclose( datafile );
+
+	return SUCCESSFUL_RETURN;
+	#else /* __XPCTARGET__ */
+
+	return RET_NOT_YET_IMPLEMENTED;
+
+	#endif /* __XPCTARGET__ */
+}
+
+
+/*
+ *	r e a d F r o m F i l e
+ */
+returnValue readFromFile(	real_t* data, int n,
+							const char* datafilename
+							)
+{
+	return readFromFile( data, n, 1, datafilename );
+}
+
+
+
+/*
+ *	r e a d F r o m F i l e
+ */
+returnValue readFromFile(	int* data, int n,
+							const char* datafilename
+							)
+{
+	#ifndef __XPCTARGET__
+	int i;
+	FILE* datafile;
+
+	/* 1) Open file. */
+	if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
+	{
+		char errstr[MAX_STRING_LENGTH];
+		snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
+		return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+	}
+
+	/* 2) Read data from file. */
+	for( i=0; i<n; ++i )
+	{
+		if ( fscanf( datafile, "%d\n", &(data[i]) ) == 0 )
+		{
+			fclose( datafile );
+			char errstr[MAX_STRING_LENGTH];
+			snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
+			return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+		}
+	}
+
+	/* 3) Close file. */
+	fclose( datafile );
+
+	return SUCCESSFUL_RETURN;
+	#else /* __XPCTARGET__ */
+
+	return RET_NOT_YET_IMPLEMENTED;
+
+	#endif /* __XPCTARGET__ */
+}
+
+
+/*
+ *	w r i t e I n t o F i l e
+ */
+returnValue writeIntoFile(	const real_t* const data, int nrow, int ncol,
+							const char* datafilename, BooleanType append
+							)
+{
+	#ifndef __XPCTARGET__
+	int i, j;
+	FILE* datafile;
+
+	/* 1) Open file. */
+	if ( append == BT_TRUE )
+	{
+		/* append data */
+		if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
+		{
+			char errstr[MAX_STRING_LENGTH];
+			snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
+			return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+		}
+	}
+	else
+	{
+		/* do not append data */
+		if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
+		{
+			char errstr[MAX_STRING_LENGTH];
+			snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
+			return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+		}
+	}
+
+	/* 2) Write data into file. */
+	for( i=0; i<nrow; ++i )
+	{
+		for( j=0; j<ncol; ++j )
+		 	fprintf( datafile, "%.16e ", data[i*ncol+j] );
+
+		fprintf( datafile, "\n" );
+	}
+
+	/* 3) Close file. */
+	fclose( datafile );
+
+	return SUCCESSFUL_RETURN;
+	#else /* __XPCTARGET__ */
+
+	return RET_NOT_YET_IMPLEMENTED;
+
+	#endif /* __XPCTARGET__ */
+}
+
+
+/*
+ *	w r i t e I n t o F i l e
+ */
+returnValue writeIntoFile(	const real_t* const data, int n,
+							const char* datafilename, BooleanType append
+							)
+{
+	return writeIntoFile( data,1,n,datafilename,append );
+}
+
+
+/*
+ *	w r i t e I n t o F i l e
+ */
+returnValue writeIntoFile(	const int* const integer, int n,
+							const char* datafilename, BooleanType append
+							)
+{
+	#ifndef __XPCTARGET__
+	int i;
+
+	FILE* datafile;
+
+	/* 1) Open file. */
+	if ( append == BT_TRUE )
+	{
+		/* append data */
+		if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
+		{
+			char errstr[MAX_STRING_LENGTH];
+			snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
+			return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+		}
+	}
+	else
+	{
+		/* do not append data */
+		if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
+		{
+			char errstr[MAX_STRING_LENGTH];
+			snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
+			return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
+		}
+	}
+
+	/* 2) Write data into file. */
+	for( i=0; i<n; ++i )
+		fprintf( datafile, "%d\n", integer[i] );
+
+	/* 3) Close file. */
+	fclose( datafile );
+
+	return SUCCESSFUL_RETURN;
+	#else /* __XPCTARGET__ */
+
+	return RET_NOT_YET_IMPLEMENTED;
+
+	#endif /* __XPCTARGET__ */
+}
+
+
+/*
+ *	w r i t e I n t o M a t F i l e
+ */
+returnValue writeIntoMatFile(	FILE* const matFile,
+								const real_t* const data, int nRows, int nCols, const char* name
+								)
+{
+	/*  Note, this code snippet has been inspired from the document
+	 *  "Matlab(R) MAT-file Format, R2013b" by MathWorks */
+
+	#ifndef __XPCTARGET__
+	if ( ( matFile == 0 ) || ( data == 0 ) || ( nRows < 0 ) || ( nCols < 0 ) || ( name == 0 ) )
+		return RET_INVALID_ARGUMENTS;
+
+	MatMatrixHeader var;
+
+	// setup variable header
+	var.numericFormat = 0000;  /* IEEE Little Endian - reserved - double precision (64 bits) - numeric full matrix */
+	var.nRows         = nRows; /* number of rows */
+	var.nCols         = nCols; /* number of columns */
+	var.imaginaryPart = 0;     /* no imaginary part */
+	var.nCharName     = (long)(strlen(name))+1; /* matrix name length */
+	
+	/* write variable header to mat file */
+	if ( fwrite( &var, sizeof(MatMatrixHeader),1,  matFile ) < 1 )
+		return RET_UNABLE_TO_WRITE_FILE;
+
+	if ( fwrite( name, sizeof(char),(unsigned long)(var.nCharName), matFile ) < 1 )
+		return RET_UNABLE_TO_WRITE_FILE;
+
+	int ii, jj;
+	double curData;
+
+	for ( ii=0; ii<nCols; ++ii )
+		for ( jj=0; jj<nRows; ++jj )
+		{
+			curData = (real_t)data[jj*nCols+ii];
+			if ( fwrite( &curData, sizeof(double),1, matFile ) < 1 )
+				return RET_UNABLE_TO_WRITE_FILE;
+		}
+
+	return SUCCESSFUL_RETURN;
+	#else /* __XPCTARGET__ */
+
+	return RET_NOT_YET_IMPLEMENTED;
+
+	#endif /* __XPCTARGET__ */
+}
+
+
+/*
+ *	w r i t e I n t o M a t F i l e
+ */
+returnValue writeIntoMatFile(	FILE* const matFile,
+								const int* const data, int nRows, int nCols, const char* name
+								)
+{
+	real_t* realData = new real_t[nRows*nCols];
+
+	int ii, jj;
+
+	for ( ii=0; ii<nRows; ++ii )
+		for ( jj=0; jj<nCols; ++jj )
+			realData[ ii*nCols+jj ] = (real_t) data[ ii*nCols+jj ];
+
+	returnValue returnvalue = writeIntoMatFile( matFile,realData,nRows,nCols,name );
+	delete[] realData;
+	
+	return returnvalue;
+}
+
+
+/*
+ *	g e t C P U t i m e
+ */
+real_t getCPUtime( )
+{
+	real_t current_time = -1.0;
+
+	#if defined(__WIN32__) || defined(WIN32)
+	LARGE_INTEGER counter, frequency;
+	QueryPerformanceFrequency(&frequency);
+	QueryPerformanceCounter(&counter);
+	current_time = ((real_t) counter.QuadPart) / ((real_t) frequency.QuadPart);
+	#elif defined(LINUX) || defined(__LINUX__)
+	struct timeval theclock;
+	gettimeofday( &theclock,0 );
+	current_time = 1.0*theclock.tv_sec + 1.0e-6*theclock.tv_usec;
+	#endif
+
+	return current_time;
+}
+
+
+/*
+ *	g e t N o r m
+ */
+real_t getNorm( const real_t* const v, int n, int type )
+{
+	int i;
+
+	real_t norm = 0.0;
+
+	switch ( type )
+	{
+		case 2:
+			for( i=0; i<n; ++i )
+				norm += v[i]*v[i];
+			return getSqrt( norm );
+
+		case 1:
+			for( i=0; i<n; ++i )
+				norm += getAbs( v[i] );
+			return norm;
+
+		default:
+			THROWERROR( RET_INVALID_ARGUMENTS );
+			return -INFTY;
+	}
+}
+
+
+/*
+ *	g e t K K T R e s i d u a l
+ */
+void getKKTResidual(	int nV, int nC,
+						const real_t* const H, const real_t* const g,
+						const real_t* const A, const real_t* const lb, const real_t* const ub,
+						const real_t* const lbA, const real_t* const ubA,
+						const real_t* const x, const real_t* const y,
+						real_t& stat, real_t& feas, real_t& cmpl
+						)
+{
+	/* Tolerance for dual variables considered zero. */
+	const real_t dualActiveTolerance = 1.0e3 * EPS;
+
+	int i, j;
+	real_t sum, prod;
+
+	/* Initialize residuals */
+	stat = feas = cmpl = 0.0;
+
+	/* check stationarity */
+	for (i = 0; i < nV; i++)
+	{
+		/* g term and variable bounds dual term */
+		if ( g != 0 )
+			sum = g[i] - y[i];
+		else
+			sum = 0.0 - y[i];
+
+		/* H*x term */
+		if ( H != 0 )
+			for (j = 0; j < nV; j++) sum += H[i*nV+j] * x[j];
+
+		/* A'*y term */
+		if ( A != 0 )
+			for (j = 0; j < nC; j++) sum -= A[j*nV+i] * y[nV+j];
+		
+		/* update stat */
+		if (getAbs(sum) > stat) stat = getAbs(sum);
+	}
+
+	/* check primal feasibility and complementarity */
+	/* variable bounds */
+	for (i = 0; i < nV; i++)
+	{
+		/* feasibility */
+		if ( lb != 0 )
+			if (lb[i] - x[i] > feas) 
+				feas = lb[i] - x[i];
+
+		if ( ub != 0 )
+			if (x[i] - ub[i] > feas) 
+				feas = x[i] - ub[i];
+
+		/* complementarity */
+		prod = 0.0;
+
+		if ( lb != 0 )
+			if (y[i] > dualActiveTolerance) /* lower bound */
+				prod = (x[i] - lb[i]) * y[i];
+
+		if ( ub != 0 )
+			if (y[i] < -dualActiveTolerance) /* upper bound */
+				prod = (x[i] - ub[i]) * y[i];
+
+		if (getAbs(prod) > cmpl) cmpl = getAbs(prod);
+	}
+	/* A*x bounds */
+	for (i = 0; i < nC; i++)
+	{
+		/* compute sum = (A*x)_i */
+		sum = 0.0;
+		if ( A != 0 )
+			for (j = 0; j < nV; j++) 
+				sum += A[i*nV+j] * x[j];
+
+		/* feasibility */
+		if ( lbA != 0 )
+			if (lbA[i] - sum > feas) 
+				feas = lbA[i] - sum;
+
+		if ( ubA != 0 )
+			if (sum - ubA[i] > feas) 
+				feas = sum - ubA[i];
+
+		/* complementarity */
+		prod = 0.0;
+
+		if ( lbA != 0 )
+			if (y[nV+i] > dualActiveTolerance) /* lower bound */
+				prod = (sum - lbA[i]) * y[nV+i];
+		
+		if ( ubA != 0 )
+			if (y[nV+i] < -dualActiveTolerance) /* upper bound */
+				prod = (sum - ubA[i]) * y[nV+i];
+
+		if (getAbs(prod) > cmpl) cmpl = getAbs(prod);
+	}
+}
+
+
+/*
+ *	g e t K K T R e s i d u a l
+ */
+void getKKTResidual(	int nV,
+						const real_t* const H, const real_t* const g,
+						const real_t* const lb, const real_t* const ub,
+						const real_t* const x, const real_t* const y,
+						real_t& stat, real_t& feas, real_t& cmpl
+						)
+{
+	getKKTResidual(	nV,0,
+					H,g,0,lb,ub,0,0,
+					x,y,
+					stat,feas,cmpl
+					);
+}
+
+
+/*
+ *	c o n v e r t B o o l e a n T y p e T o S t r i n g
+ */
+returnValue convertBooleanTypeToString( BooleanType value, char* const string )
+{
+	#ifndef __XPCTARGET__
+	if ( value == BT_FALSE )
+		snprintf( string,20,"BT_FALSE" );
+	else
+		snprintf( string,20,"BT_TRUE" );
+	#endif /* __XPCTARGET__ */
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	c o n v e r t S u b j e c t T o S t a t u s T o S t r i n g
+ */
+returnValue convertSubjectToStatusToString( SubjectToStatus value, char* const string )
+{
+	#ifndef __XPCTARGET__
+	switch( value )
+	{
+		case ST_INACTIVE:
+			snprintf( string,20,"ST_INACTIVE" );
+			break;
+
+		case ST_LOWER:
+			snprintf( string,20,"ST_LOWER" );
+			break;
+
+		case ST_UPPER:
+			snprintf( string,20,"ST_UPPER" );
+			break;
+
+		case ST_UNDEFINED:
+			snprintf( string,20,"ST_UNDEFINED" );
+			break;
+			
+		case ST_INFEASIBLE_LOWER:
+			snprintf( string,20,"ST_INFEASIBLE_LOWER" );
+			break;
+
+		case ST_INFEASIBLE_UPPER:
+			snprintf( string,20,"ST_INFEASIBLE_UPPER" );
+			break;
+
+		default:
+			snprintf( string,20,"<invalid value>" );
+			break;
+	}
+	#endif /* __XPCTARGET__ */
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	c o n v e r t P r i n t L e v e l T o S t r i n g
+ */
+returnValue convertPrintLevelToString( PrintLevel value, char* const string )
+{
+	#ifndef __XPCTARGET__
+	switch( value )
+	{
+		case PL_NONE:
+			snprintf( string,20,"PL_NONE" );
+			break;
+
+		case PL_LOW:
+			snprintf( string,20,"PL_LOW" );
+			break;
+
+		case PL_MEDIUM:
+			snprintf( string,20,"PL_MEDIUM" );
+			break;
+
+		case PL_HIGH:
+			snprintf( string,20,"PL_HIGH" );
+			break;
+			
+		case PL_TABULAR:
+			snprintf( string,20,"PL_TABULAR" );
+			break;
+
+		case PL_DEBUG_ITER:
+			snprintf( string,20,"PL_DEBUG_ITER" );
+			break;
+
+		default:
+			snprintf( string,20,"<invalid value>" );
+			break;
+	}
+	#endif /* __XPCTARGET__ */
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t S i m p l e S t a t u s
+ */
+int getSimpleStatus(	returnValue returnvalue,
+						BooleanType doPrintStatus
+						)
+{
+	int simpleStatus = -1;
+
+	/* determine simple status from returnvalue */
+	switch ( returnvalue )
+	{
+		case SUCCESSFUL_RETURN:
+			simpleStatus = 0;
+			break;
+
+		case RET_MAX_NWSR_REACHED:
+			simpleStatus = 1;
+			break;
+
+		case RET_INIT_FAILED_INFEASIBILITY:
+		case RET_HOTSTART_STOPPED_INFEASIBILITY:
+			simpleStatus = -2;
+			break;
+
+		case RET_INIT_FAILED_UNBOUNDEDNESS:
+		case RET_HOTSTART_STOPPED_UNBOUNDEDNESS:
+			simpleStatus = -3;
+			break;
+
+		default:
+			simpleStatus = -1;
+			break;
+	}
+
+	if ( doPrintStatus == BT_TRUE )
+	{
+		VisibilityStatus vsInfo = getGlobalMessageHandler( )->getInfoVisibilityStatus( );
+		getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_VISIBLE );
+		getGlobalMessageHandler( )->setErrorCount( -1 );
+		
+		int retValNumber = (int)RET_SIMPLE_STATUS_P0 - simpleStatus;
+		THROWINFO( (returnValue)retValNumber );
+
+		getGlobalMessageHandler( )->setInfoVisibilityStatus( vsInfo );
+	}
+
+	return simpleStatus;
+}
+
+
+/*
+ *	n o r m a l i s e C o n s t r a i n t s
+ */
+returnValue normaliseConstraints(	int nV, int nC,
+									real_t* A, real_t* lbA, real_t* ubA,
+									int type
+									)
+{
+	int ii, jj;
+	real_t curNorm;
+
+	if ( ( nV <= 0 ) || ( nC <= 0 ) || ( A == 0 ) )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	for( ii=0; ii<nC; ++ii )
+	{
+		/* get row norm */
+		curNorm = getNorm( &(A[ii*nV]),nV,type );
+
+		if ( curNorm > EPS )
+		{
+			/* normalise if norm is positive */
+			for( jj=0; jj<nV; ++jj )
+				A[ii*nV + jj] /= curNorm;
+
+			if ( lbA != 0 ) lbA[ii] /= curNorm;
+			if ( ubA != 0 ) ubA[ii] /= curNorm;
+		}
+		else
+		{
+			/* if row norm is (close to) zero, kind of erase constraint */
+			if ( type == 1 )
+			{
+				for( jj=0; jj<nV; ++jj )
+					A[ii*nV + jj] = 1.0 / ((real_t)nV);
+			}
+			else
+			{
+				/* assume type == 2 */
+				for( jj=0; jj<nV; ++jj )
+					A[ii*nV + jj] = 1.0 / getSqrt((real_t)nV);
+			}
+
+			if ( lbA != 0 ) lbA[ii] = -INFTY;
+			if ( ubA != 0 ) ubA[ii] =  INFTY;
+		}
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+#ifdef __DEBUG__
+/*
+ *	g d b _ p r i n t m at
+ */
+extern "C" void gdb_printmat(const char *fname, real_t *M, int n, int m, int ldim)
+{
+	#ifndef __XPCTARGET__
+	int i, j;
+	FILE *fid;
+
+	fid = fopen(fname, "wt");
+	if (!fid) 
+	{
+		perror("Error opening file: ");
+		return;
+	}
+
+	for (i = 0; i < n; i++)
+	{
+		for (j = 0; j < m; j++)
+			fprintf(fid, " %23.16e", M[j*ldim+i]);
+		fprintf(fid, "\n");
+	}
+	fclose(fid);
+	#endif /* __XPCTARGET__ */
+}
+#endif /* __DEBUG__ */
+
+
+
+#if defined(__DSPACE__) || defined(__XPCTARGET__) 
+/*
+ *	_ _ c x a _ p u r e _ v i r t u a l
+ */
+void __cxa_pure_virtual( void )
+{
+	/* put your customized implementation here! */
+}
+#endif /* __DSPACE__ || __XPCTARGET__*/ 
+
+
+
+END_NAMESPACE_QPOASES
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/index.html b/3rdparty/qpOASES/qpOASES-3.0/src/index.html
new file mode 100644
index 00000000000..22da2b9c4c4
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/src/index.html
@@ -0,0 +1,24 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/src</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/src</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="BLASReplacement.cpp">BLASReplacement.cpp</a></li>
+  <li><a href="Bounds.cpp">Bounds.cpp</a></li>
+  <li><a href="Constraints.cpp">Constraints.cpp</a></li>
+  <li><a href="Flipper.cpp">Flipper.cpp</a></li>
+  <li><a href="Indexlist.cpp">Indexlist.cpp</a></li>
+  <li><a href="LAPACKReplacement.cpp">LAPACKReplacement.cpp</a></li>
+  <li><a href="Makefile">Makefile</a></li>
+  <li><a href="Matrices.cpp">Matrices.cpp</a></li>
+  <li><a href="MessageHandling.cpp">MessageHandling.cpp</a></li>
+  <li><a href="OQPinterface.cpp">OQPinterface.cpp</a></li>
+  <li><a href="Options.cpp">Options.cpp</a></li>
+  <li><a href="QProblem.cpp">QProblem.cpp</a></li>
+  <li><a href="QProblemB.cpp">QProblemB.cpp</a></li>
+  <li><a href="SQProblem.cpp">SQProblem.cpp</a></li>
+  <li><a href="SolutionAnalysis.cpp">SolutionAnalysis.cpp</a></li>
+  <li><a href="SubjectTo.cpp">SubjectTo.cpp</a></li>
+  <li><a href="Utils.cpp">Utils.cpp</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/checkForMemoryLeaks b/3rdparty/qpOASES/qpOASES-3.0/testing/checkForMemoryLeaks
new file mode 100644
index 00000000000..44abb2c6ee7
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/checkForMemoryLeaks
@@ -0,0 +1,106 @@
+#!/usr/bin/env bash
+
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+##
+##	Filename:  testing/checkForMemoryLeaks
+##	Author:    Hans Joachim Ferreau
+##	Version:   3.0
+##	Date:      2014
+
+# defining colors for output
+red='\e[0;31m'
+green='\e[0;32m'
+NC='\e[0m' # No Color
+
+# runs valgrind on a number of examples to detect possible memory leaks
+
+function callValgrind {
+	echo -n "Checking for memory leaks in $2 $3... "
+
+	valgrind --tool=memcheck --leak-check=yes --error-exitcode=42 -q $2 $3 > dummy.txt;
+	retVal=$?;
+	rm -rf dummy.txt;
+
+	if [ $retVal == 42 ]; then
+		echo -e "${red}failed!${NC}"
+		counter=$[counter+1];
+	else
+		if [ $retVal == 99 ]; then
+			echo "problem data missing!"
+		else
+			echo -e "${green}passed!${NC}"
+		fi
+	fi
+}
+
+
+cd ..;
+make examples testing;
+cd testing;
+
+counter=0;
+
+# run examples in /examples
+callValgrind $counter ../bin/example1;
+callValgrind $counter ../bin/example1a;
+callValgrind $counter ../bin/example1b;
+callValgrind $counter ../bin/example2;
+callValgrind $counter ../bin/example3;
+callValgrind $counter ../bin/example3b;
+callValgrind $counter ../bin/example4;
+callValgrind $counter ../bin/example5;
+callValgrind $counter ../bin/exampleLP;
+callValgrind $counter ../bin/qrecipe;
+
+# run examples in /testing ...
+callValgrind $counter ../bin/test_matrices;
+callValgrind $counter ../bin/test_matrices2;
+callValgrind $counter ../bin/test_indexlist;
+callValgrind $counter ../bin/test_example6;
+callValgrind $counter ../bin/test_example7;
+callValgrind $counter ../bin/test_sebastien1;
+callValgrind $counter ../bin/test_vanBarelsUnboundedQP;
+callValgrind $counter ../bin/test_janick1;
+callValgrind $counter ../bin/test_janick2;
+callValgrind $counter ../bin/test_constraintProduct1;
+callValgrind $counter ../bin/test_constraintProduct2;
+callValgrind $counter ../bin/test_guessedWS1;
+callValgrind $counter ../bin/test_runAllOqpExamples;
+
+
+# ... including testbench with different settings
+callValgrind $counter ../bin/test_bench Odd; # default dense
+callValgrind $counter ../bin/test_bench Ods; # default sparse
+callValgrind $counter ../bin/test_bench Omd; # MPC dense
+callValgrind $counter ../bin/test_bench Ors; # reliable dense
+
+
+if [ $counter == 0 ]; then
+	echo -e "${green}All available tests passed successfully!${NC}"
+else
+	echo -e "${red}$counter test(s) failed!${NC}"
+	exit 1
+fi
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/data/fetch_cpp_data b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/data/fetch_cpp_data
new file mode 100644
index 00000000000..4d841d6ac71
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/data/fetch_cpp_data
@@ -0,0 +1,34 @@
+#!/usr/bin/env bash
+
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+##
+##	Filename:  testing/cpp/data/fetch_cpp_data
+##	Author:    Hans Joachim Ferreau
+##	Version:   3.0
+##	Date:      2014
+
+
+svn export https://projects.coin-or.org/svn/qpOASES/misc/testingdata/cpp . --force
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/data/index.html b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/data/index.html
new file mode 100644
index 00000000000..b6c3242cfb8
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/data/index.html
@@ -0,0 +1,8 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/testing/cpp/data</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/testing/cpp/data</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="fetch_cpp_data">fetch_cpp_data</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/index.html b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/index.html
new file mode 100644
index 00000000000..bb4321bcdc7
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/index.html
@@ -0,0 +1,33 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/testing/cpp</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/testing/cpp</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="data/">data/</a></li>
+  <li><a href="test_bench.cpp">test_bench.cpp</a></li>
+  <li><a href="test_constraintProduct1.cpp">test_constraintProduct1.cpp</a></li>
+  <li><a href="test_constraintProduct2.cpp">test_constraintProduct2.cpp</a></li>
+  <li><a href="test_example1.cpp">test_example1.cpp</a></li>
+  <li><a href="test_example1a.cpp">test_example1a.cpp</a></li>
+  <li><a href="test_example1b.cpp">test_example1b.cpp</a></li>
+  <li><a href="test_example2.cpp">test_example2.cpp</a></li>
+  <li><a href="test_example4.cpp">test_example4.cpp</a></li>
+  <li><a href="test_example5.cpp">test_example5.cpp</a></li>
+  <li><a href="test_example6.cpp">test_example6.cpp</a></li>
+  <li><a href="test_example7.cpp">test_example7.cpp</a></li>
+  <li><a href="test_exampleLP.cpp">test_exampleLP.cpp</a></li>
+  <li><a href="test_gradientShift.cpp">test_gradientShift.cpp</a></li>
+  <li><a href="test_guessedWS1.cpp">test_guessedWS1.cpp</a></li>
+  <li><a href="test_indexlist.cpp">test_indexlist.cpp</a></li>
+  <li><a href="test_janick1.cpp">test_janick1.cpp</a></li>
+  <li><a href="test_janick2.cpp">test_janick2.cpp</a></li>
+  <li><a href="test_matrices.cpp">test_matrices.cpp</a></li>
+  <li><a href="test_matrices2.cpp">test_matrices2.cpp</a></li>
+  <li><a href="test_matrices3.cpp">test_matrices3.cpp</a></li>
+  <li><a href="test_qrecipe.cpp">test_qrecipe.cpp</a></li>
+  <li><a href="test_qrecipe_data.hpp">test_qrecipe_data.hpp</a></li>
+  <li><a href="test_runAllOqpExamples.cpp">test_runAllOqpExamples.cpp</a></li>
+  <li><a href="test_sebastien1.cpp">test_sebastien1.cpp</a></li>
+  <li><a href="test_vanBarelsUnboundedQP.cpp">test_vanBarelsUnboundedQP.cpp</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_bench.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_bench.cpp
new file mode 100644
index 00000000000..762d99859c9
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_bench.cpp
@@ -0,0 +1,309 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_bench.cpp
+ *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2010-2014
+ *
+ *	Unit test running all benchmark examples stored in problems directory.
+ */
+
+
+
+#include <dirent.h>
+#include <cstring>
+#include <cstdlib>
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+/** Run benchmark examples. */
+int main( int argc, char *argv[] )
+{
+	USING_NAMESPACE_QPOASES
+	#ifdef __USE_SINGLE_PRECISION__
+	const real_t TOL = 5e-2;
+	#else
+	const real_t TOL = 1e-5;
+	#endif
+
+	/* 1) Define benchmark arguments. */
+	BooleanType isSparse = BT_FALSE;
+	//BooleanType isSparse = BT_TRUE;
+	Options options;
+ 	options.setToDefault();
+	//options.setToMPC();
+	//options.setToReliable();
+	options.printLevel = PL_LOW;
+	//options.printLevel = PL_MEDIUM;
+	//options.printLevel = PL_TABULAR;
+	//options.enableFarBounds = BT_FALSE;
+
+// 	options.initialStatusBounds = ST_LOWER;
+	//options.numRegularisationSteps = 1;
+	//options.epsRegularisation = 1.0e3 * EPS;
+ 	
+	//options.enableFlippingBounds = BT_FALSE;
+	//options.enableFlippingBounds = BT_FALSE;
+	//options.enableRamping = BT_TRUE;
+	//options.enableFarBounds = BT_FALSE;
+	//options.enableNZCTests = BT_FALSE;
+	//options.epsNZCTests = 1.0e4 * EPS;
+	//options.epsFlipping = 1.0e5 * EPS;
+	//options.enableFullLITests = BT_TRUE;
+ 	//options.enableDriftCorrection = 1;
+ 	//options.enableEqualities = BT_TRUE;
+	//options.enableEqualities = BT_FALSE;
+ 	//options.epsNum = -1.0e3 * EPS;
+ 	//options.epsDen =  1.0e3 * EPS;
+
+
+	int nWSR;
+	real_t maxCPUtime; /* seconds */
+	real_t maxStationarity = 0.0, maxFeasibility = 0.0, maxComplementarity = 0.0;
+	real_t avgStationarity = 0.0, avgFeasibility = 0.0, avgComplementarity = 0.0;
+
+	int scannedDir = 0;
+	int nfail = 0, npass = 0;
+	int nproblems, i;
+	struct dirent **namelist;
+	char resstr[MAX_STRING_LENGTH], OQPproblem[MAX_STRING_LENGTH];
+	char *problem;
+	returnValue returnvalue;
+
+	int expectedNumSolvedProblems = 44;
+	real_t expectedAvgStationarity = TOL;
+	real_t expectedAvgavgFeasibility = TOL;
+	real_t expectedAvgFeasibility = TOL;
+
+	
+	if ( argv[argc-1][0] == 'O' )
+	{
+		if ( strlen(argv[argc-1]) != 3 )
+		{
+			fprintf( stdout,"ERROR (testbench): Invalid options passed!\n" );
+			return TEST_DATA_NOT_FOUND;
+		}
+		
+		fprintf( stdout,"Analysing passed options:  " );
+		switch ( argv[argc-1][1] )
+		{
+			case 'd':
+				fprintf( stdout,"default options, " );
+				options.setToDefault();
+				if ( argv[argc-1][2] == 's' )
+				{
+					expectedNumSolvedProblems = 44;
+					expectedAvgStationarity   = 1e-7;
+					expectedAvgavgFeasibility = 1e-10;
+					expectedAvgFeasibility    = 5e-7;
+				}
+				else
+				{
+					expectedNumSolvedProblems = 44;
+					expectedAvgStationarity   = 1e-7;
+					expectedAvgavgFeasibility = 5e-10;
+					expectedAvgFeasibility    = 5e-8;
+				}
+				break;
+				
+			case 'r':
+				fprintf( stdout,"reliable options, " );
+				options.setToReliable();
+				if ( argv[argc-1][2] == 's' )
+				{
+					expectedNumSolvedProblems = 44;
+					expectedAvgStationarity   = 1e-9;
+					expectedAvgavgFeasibility = 2e-11;
+					expectedAvgFeasibility    = 2e-9;
+				}
+				else
+				{
+					expectedNumSolvedProblems = 44;
+					expectedAvgStationarity   = 2e-9;
+					expectedAvgavgFeasibility = 1e-9;
+					expectedAvgFeasibility    = 2e-7;
+				}
+				break;
+				
+			case 'm':
+				fprintf( stdout,"MPC options, " );
+				options.setToMPC();
+				if ( argv[argc-1][2] == 's' )
+				{
+					expectedNumSolvedProblems = 42;
+					expectedAvgStationarity   = 2e-8;
+					expectedAvgavgFeasibility = 1e-8;
+					expectedAvgFeasibility    = 1e-7;
+				}
+				else
+				{
+					expectedNumSolvedProblems = 42;
+					expectedAvgStationarity   = 2e-8;
+					expectedAvgavgFeasibility = 1e-8;
+					expectedAvgFeasibility    = 5e-8;
+				}
+				break;
+				
+			default:
+				fprintf( stdout,"ERROR (testbench): Invalid options passed!\n" );
+				return TEST_DATA_NOT_FOUND;
+		}
+		
+		switch ( argv[argc-1][2] )
+		{
+			case 's':
+				fprintf( stdout,"sparse QP data\n" );
+				isSparse = BT_TRUE;
+				break;
+				
+			case 'd':
+				fprintf( stdout,"dense QP data\n" );
+				isSparse = BT_FALSE;
+				break;
+				
+			default:
+				fprintf( stdout,"ERROR (testbench): Invalid options passed!\n" );
+				return TEST_DATA_NOT_FOUND;
+		}
+		options.printLevel = PL_NONE;
+		
+		nproblems = argc-2;
+	}
+	else
+	{
+		nproblems = argc-1;
+	}
+
+	
+	if (nproblems == 0)
+	{
+		/* 2a) Scan problem directory */
+		nproblems = scandir("../testing/cpp/data/problems", &namelist, NULL, alphasort);
+		if (nproblems <= 0)
+		{
+			myPrintf( "No test problems found!\n" );
+			return TEST_DATA_NOT_FOUND;
+		}
+		scannedDir = 1;
+	}
+	else
+	{
+		/* 2b) Use problem list given by arguments */
+		scannedDir = 0;
+	}
+
+	/* 3) Run benchmark. */
+	printf("%10s %9s %9s %9s %6s  %-12s\n", "problem", "stat",
+			"feas", "compl", "nWSR", "result");
+	for (i = 0; i < nproblems; i++)
+	{
+		if (scannedDir)
+		{
+			/* skip special directories and zip file cuter.*bz2 */
+			if (namelist[i]->d_name[0] == '.' || namelist[i]->d_name[0] == 'c')
+			{
+				free(namelist[i]);
+				continue;
+			}
+			problem = namelist[i]->d_name;
+		}
+		else
+		{
+			problem = argv[i+1];
+		}
+
+		fprintf(stdFile, "%-10s ", problem);
+		fflush(stdFile);
+
+		snprintf(OQPproblem, MAX_STRING_LENGTH, "../testing/cpp/data/problems/%s/", problem);
+		maxCPUtime = 300.0;
+		nWSR = 2500;
+
+		returnvalue = runOQPbenchmark(	OQPproblem, isSparse, options,
+										nWSR, maxCPUtime, maxStationarity, maxFeasibility, maxComplementarity 
+										);
+		if (returnvalue	== SUCCESSFUL_RETURN
+				&& maxStationarity < TOL
+				&& maxFeasibility < TOL
+				&& maxComplementarity < TOL)
+		{
+			npass++;
+			
+			avgStationarity    += maxStationarity;
+			avgFeasibility     += maxFeasibility;
+			avgComplementarity += maxComplementarity;
+
+			strncpy(resstr, "pass", MAX_STRING_LENGTH);
+		}
+		else
+		{
+			if ( returnvalue == RET_BENCHMARK_ABORTED )
+				return TEST_DATA_NOT_FOUND;
+
+			nfail++;
+			snprintf (resstr, MAX_STRING_LENGTH, "fail (%d)", returnvalue);
+		}
+		fprintf(stdFile, "%9.2e %9.2e %9.2e %6d  %-12s\n", maxStationarity,
+				maxFeasibility, maxComplementarity, nWSR, resstr);
+
+		if (scannedDir) free(namelist[i]);
+	}
+	if (scannedDir) free(namelist);
+
+	avgStationarity    /= npass;
+	avgFeasibility     /= npass;
+	avgComplementarity /= npass;
+
+
+	/* 4) Print results. */
+	printf("\n\n" );
+	printf("Testbench results:\n" );
+	printf("======================\n\n" );
+	printf("Pass:  %3d\n", npass);
+	printf("Fail:  %3d\n", nfail);
+	printf("Ratio: %5.1f%%\n", 100.0 * (real_t)npass / (real_t)(npass+nfail));
+	printf("\n" );
+
+	QPOASES_TEST_FOR_TRUE( npass >= expectedNumSolvedProblems );
+
+
+	printf("avg. stat:  %e\n", avgStationarity);
+	printf("avg. feas:  %e\n", avgFeasibility);
+	printf("avg. cmpl:  %e\n", avgFeasibility);
+
+	QPOASES_TEST_FOR_TOL( avgStationarity,    expectedAvgStationarity );
+	QPOASES_TEST_FOR_TOL( avgFeasibility,     expectedAvgavgFeasibility );
+	QPOASES_TEST_FOR_TOL( avgComplementarity, expectedAvgFeasibility );
+
+
+	return 0;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_constraintProduct1.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_constraintProduct1.cpp
new file mode 100644
index 00000000000..ad8aedec49e
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_constraintProduct1.cpp
@@ -0,0 +1,197 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_constraintProduct1.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2014
+ *
+ *	Another example for testing qpOASES using the possibility to specify 
+ *	user-defined constraint product function.
+ */
+
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+
+USING_NAMESPACE_QPOASES
+
+
+/** 
+ *	\brief Example illustrating the use of the \a ConstraintProduct class.
+ *
+ *	Example illustrating the use of the \a ConstraintProduct class.
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ */
+class MpcConstraintProduct : public ConstraintProduct
+{
+	public:
+		/** Default constructor. */
+		MpcConstraintProduct( ) {};
+
+		/** Constructor. */
+		MpcConstraintProduct(	int _nV,
+								int _nC,
+								int _diagOffset,
+								real_t* _A
+								)
+		{
+			nV = _nV;
+			nC = _nC;
+			diagOffset = _diagOffset;
+			A  = _A;
+		};
+
+		/** Copy constructor (flat copy). */
+		MpcConstraintProduct(	const MpcConstraintProduct& rhs
+								)
+		{
+			nV = rhs.nV;
+			nC = rhs.nC;
+			diagOffset = rhs.diagOffset;
+			A  = rhs.A;
+		};
+
+		/** Destructor. */
+		virtual ~MpcConstraintProduct( ) {};
+		
+		/** Assignment operator (flat copy). */
+		MpcConstraintProduct& operator=(	const MpcConstraintProduct& rhs
+										)
+		{
+			if ( this != &rhs )
+			{
+				nV = rhs.nV;
+				nC = rhs.nC;
+				diagOffset = rhs.diagOffset;
+				A  = rhs.A;
+			}
+			else
+				return *this;
+		};
+
+		virtual int operator() (	int constrIndex,
+									const real_t* const x,
+									real_t* const constrValue
+									) const
+		{
+			int maxI = (int)(((real_t)constrIndex) * ((real_t)nV) / ((real_t)nC)) + diagOffset;
+			maxI = getMin( maxI,nV );
+
+			constrValue[0] = 0.0;
+
+			for( int i=0; i<maxI; ++i )
+				constrValue[0] += A[constrIndex*nV + i] * x[i];
+
+			return 0;
+		};
+
+	protected:
+		int nV;			/**< Number of variables. */
+		int nC;			/**< Number of constraints. */
+		int diagOffset;	/**< ... */
+		real_t* A;		/**< Pointer to full constraint matrix (typically not needed!). */
+		
+};
+
+
+
+/**	Example for qpOASES main function using the possibility to specify 
+ *	user-defined constraint product function. */
+int main( )
+{
+	int nQP, nV, nC, nEC;
+	real_t *H, *g, *A, *lb, *ub, *lbA, *ubA;
+	real_t cputime;
+
+	real_t xOpt[1000];
+	real_t yOpt[1000];
+	real_t xOptCP[1000+1000];
+	real_t yOptCP[1000+1000];
+	
+	const char* path = "./cpp/data/oqp/chain80w/";
+	int k = 10; //th problem
+	
+		
+	if ( readOQPdimensions(	path, nQP,nV,nC,nEC ) != SUCCESSFUL_RETURN )
+		return TEST_DATA_NOT_FOUND;
+	
+	readOQPdata(	path, nQP,nV,nC,nEC,
+					&H,&g,&A,&lb,&ub,&lbA,&ubA,
+					0,0,0
+					);
+	
+	Options myOptions;
+	//myOptions.setToMPC();
+	myOptions.printLevel = PL_LOW;
+
+	int nWSR = 500;
+	cputime = 20.0;
+	QProblem qp( nV,nC );
+	qp.setOptions( myOptions );
+	qp.init( H,&(g[k*nV]),A,&(lb[k*nV]),&(ub[k*nV]),&(lbA[k*nC]),&(ubA[k*nC]),nWSR,&cputime );
+	qp.getPrimalSolution( xOpt );
+	qp.getDualSolution( yOpt );
+	printf( "cputime without constraintProduct: %.3ems\n", cputime*1000.0 );
+	
+
+	nWSR = 500;
+	cputime = 20.0;
+	MpcConstraintProduct myCP( nV,nC,2,A );
+	QProblem qpCP( nV,nC );
+	qpCP.setOptions( myOptions );
+	qpCP.setConstraintProduct( &myCP );
+	qpCP.init( H,&(g[k*nV]),A,&(lb[k*nV]),&(ub[k*nV]),&(lbA[k*nC]),&(ubA[k*nC]),nWSR,&cputime );
+	qpCP.getPrimalSolution( xOptCP );
+	qpCP.getDualSolution( yOptCP );
+	printf( "cputime with    constraintProduct: %.3ems\n", cputime*1000.0 );
+	
+	delete[] ubA;
+	delete[] lbA;
+	delete[] ub;
+	delete[] lb;
+	delete[] A;
+	delete[] g;
+	delete[] H;
+
+	for( int ii=0; ii<nV; ++ii )
+		QPOASES_TEST_FOR_NEAR( xOptCP[ii],xOpt[ii] );
+
+	for( int ii=0; ii<nV+nC; ++ii )
+		QPOASES_TEST_FOR_NEAR( yOptCP[ii],yOpt[ii] );
+
+	return 0;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_constraintProduct2.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_constraintProduct2.cpp
new file mode 100644
index 00000000000..67fcfa48e20
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_constraintProduct2.cpp
@@ -0,0 +1,196 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_constraintProduct2.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2014
+ *
+ *	Another example for testing qpOASES using the possibility to specify 
+ *	user-defined constraint product function.
+ */
+
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+
+USING_NAMESPACE_QPOASES
+
+
+/** 
+ *	\brief Example illustrating the use of the \a ConstraintProduct class.
+ *
+ *	Example illustrating the use of the \a ConstraintProduct class.
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ */
+class MpcConstraintProduct : public ConstraintProduct
+{
+	public:
+		/** Default constructor. */
+		MpcConstraintProduct( ) {};
+
+		/** Constructor. */
+		MpcConstraintProduct(	int _nV,
+								int _nC,
+								int _diagOffset,
+								real_t* _A
+								)
+		{
+			nV = _nV;
+			nC = _nC;
+			diagOffset = _diagOffset;
+			A  = _A;
+		};
+
+		/** Copy constructor (flat copy). */
+		MpcConstraintProduct(	const MpcConstraintProduct& rhs
+								)
+		{
+			nV = rhs.nV;
+			nC = rhs.nC;
+			diagOffset = rhs.diagOffset;
+			A  = rhs.A;
+		};
+
+		/** Destructor. */
+		virtual ~MpcConstraintProduct( ) {};
+		
+		/** Assignment operator (flat copy). */
+		MpcConstraintProduct& operator=(	const MpcConstraintProduct& rhs
+										)
+		{
+			if ( this != &rhs )
+			{
+				nV = rhs.nV;
+				nC = rhs.nC;
+				diagOffset = rhs.diagOffset;
+				A  = rhs.A;
+			}
+			else
+				return *this;
+		};
+
+		virtual int operator() (	int constrIndex,
+									const real_t* const x,
+									real_t* const constrValue
+									) const
+		{
+			int maxI = (int)(((real_t)constrIndex) * ((real_t)nV) / ((real_t)nC)) + diagOffset;
+			maxI = getMin( maxI,nV );
+
+			constrValue[0] = 0.0;
+
+			for( int i=0; i<maxI; ++i )
+				constrValue[0] += A[constrIndex*nV + i] * x[i];
+
+			return 0;
+		};
+
+	protected:
+		int nV;			/**< Number of variables. */
+		int nC;			/**< Number of constraints. */
+		int diagOffset;	/**< ... */
+		real_t* A;		/**< Pointer to full constraint matrix (typically not needed!). */
+		
+};
+
+
+/**	Example for qpOASES main function using the possibility to specify 
+ *	user-defined constraint product function. */
+int main( )
+{
+	int nQP, nV, nC, nEC;
+	real_t *H, *g, *A, *lb, *ub, *lbA, *ubA;
+	real_t cputime;
+	
+	real_t xOpt[1000];
+	real_t yOpt[1000];
+	real_t xOptCP[1000+1000];
+	real_t yOptCP[1000+1000];
+		
+	const char* path = "./cpp/data/oqp/diesel/";
+	int k = 200; //th problem
+	
+	
+	if ( readOQPdimensions(	path, nQP,nV,nC,nEC ) != SUCCESSFUL_RETURN )
+		return TEST_DATA_NOT_FOUND;
+
+	readOQPdata(	path, nQP,nV,nC,nEC,
+					&H,&g,&A,&lb,&ub,&lbA,&ubA,
+					0,0,0
+					);
+	
+	Options myOptions;
+	myOptions.setToMPC();
+	myOptions.printLevel = PL_LOW;
+	
+	int nWSR = 500;
+	cputime = 10.0;
+	QProblem qp( nV,nC );
+	qp.setOptions( myOptions );
+	qp.init( H,&(g[k*nV]),A,&(lb[k*nV]),&(ub[k*nV]),&(lbA[k*nC]),&(ubA[k*nC]),nWSR,&cputime );
+	qp.getPrimalSolution( xOpt );
+	qp.getDualSolution( yOpt );
+	printf( "cputime without constraintProduct: %.3ems\n", cputime*1000.0 );
+	
+	
+	nWSR = 500;
+	cputime = 10.0;
+	MpcConstraintProduct myCP( nV,nC,1,A );
+	QProblem qpCP( nV,nC );
+	qpCP.setOptions( myOptions );
+	qpCP.setConstraintProduct( &myCP );
+	qpCP.init( H,&(g[k*nV]),A,&(lb[k*nV]),&(ub[k*nV]),&(lbA[k*nC]),&(ubA[k*nC]),nWSR,&cputime );
+	qpCP.getPrimalSolution( xOptCP );
+	qpCP.getDualSolution( yOptCP );
+	printf( "cputime without constraintProduct: %.3ems\n", cputime*1000.0 );
+	
+	delete[] ubA;
+	delete[] lbA;
+	delete[] ub;
+	delete[] lb;
+	delete[] A;
+	delete[] g;
+	delete[] H;
+	
+	for( int ii=0; ii<nV; ++ii )
+		QPOASES_TEST_FOR_NEAR( xOptCP[ii],xOpt[ii] );
+
+	for( int ii=0; ii<nV+nC; ++ii )
+		QPOASES_TEST_FOR_NEAR( yOptCP[ii],yOpt[ii] );
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1.cpp
new file mode 100644
index 00000000000..2a7ba863e1c
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1.cpp
@@ -0,0 +1,123 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_example1.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Very simple example for testing qpOASES (using QProblem class).
+ */
+
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+/** Example for qpOASES main function using the QProblem class. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	/* Setup data of first QP. */
+	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
+	real_t A[1*2] = { 1.0, 1.0 };
+	real_t g[2] = { 1.5, 1.0 };
+	real_t lb[2] = { 0.5, -2.0 };
+	real_t ub[2] = { 5.0, 2.0 };
+	real_t lbA[1] = { -1.0 };
+	real_t ubA[1] = { 2.0 };
+
+	/* Setup data of second QP. */
+	real_t g_new[2] = { 1.0, 1.5 };
+	real_t lb_new[2] = { 0.0, -1.0 };
+	real_t ub_new[2] = { 5.0, -0.5 };
+	real_t lbA_new[1] = { -2.0 };
+	real_t ubA_new[1] = { 1.0 };
+
+
+	/* Setting up QProblem object. */
+	QProblem example( 2,1 );
+
+	Options options;
+	example.setOptions( options );
+
+	/* Solve first QP. */
+	int nWSR = 10;
+	example.init( H,g,A,lb,ub,lbA,ubA, nWSR );
+
+	/* Get and print solution of second QP. */
+	real_t xOpt[2];
+	real_t yOpt[2+1];
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+	printf( "\nxOpt = [ %e, %e ];  yOpt = [ %e, %e, %e ];  objVal = %e\n\n", 
+			xOpt[0],xOpt[1],yOpt[0],yOpt[1],yOpt[2],example.getObjVal() );
+	
+	/* Compute KKT tolerances */
+	real_t stat, feas, cmpl;
+
+	getKKTResidual(	2,1,
+					H,g,A,lb,ub,lbA,ubA,
+					xOpt,yOpt,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-15 );
+	QPOASES_TEST_FOR_TOL( feas,1e-15 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
+
+
+	/* Solve second QP. */
+	nWSR = 10;
+	example.hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR );
+
+	/* Get and print solution of second QP. */
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+	printf( "\nxOpt = [ %e, %e ];  yOpt = [ %e, %e, %e ];  objVal = %e\n\n", 
+			xOpt[0],xOpt[1],yOpt[0],yOpt[1],yOpt[2],example.getObjVal() );
+	
+	/* Compute KKT tolerances */
+	getKKTResidual(	2,1,
+					H,g_new,A,lb_new,ub_new,lbA_new,ubA_new,
+					xOpt,yOpt,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-15 );
+	QPOASES_TEST_FOR_TOL( feas,1e-15 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1a.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1a.cpp
new file mode 100644
index 00000000000..de43caa7957
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1a.cpp
@@ -0,0 +1,118 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_example1a.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Very simple example for testing qpOASES using the SQProblem class.
+ */
+
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+/** Example for qpOASES main function using the SQProblem class. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	/* Setup data of first QP. */
+	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
+	real_t A[1*2] = { 1.0, 1.0 };
+	real_t g[2] = { 1.5, 1.0 };
+	real_t lb[2] = { 0.5, -2.0 };
+	real_t ub[2] = { 5.0, 2.0 };
+	real_t lbA[1] = { -1.0 };
+	real_t ubA[1] = { 2.0 };
+
+	/* Setup data of second QP. */
+	real_t H_new[2*2] = { 1.0, 0.5, 0.5, 0.5 };
+	real_t A_new[1*2] = { 1.0, 5.0 };
+	real_t g_new[2] = { 1.0, 1.5 };
+	real_t lb_new[2] = { 0.0, -1.0 };
+	real_t ub_new[2] = { 5.0, -0.5 };
+	real_t lbA_new[1] = { -2.0 };
+	real_t ubA_new[1] = { 1.0 };
+
+
+	/* Setting up SQProblem object. */
+	SQProblem example( 2,1 );
+
+	/* Solve first QP. */
+	int nWSR = 10;
+	example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
+
+	real_t xOpt[2];
+	real_t yOpt[2+1];
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+
+	/* Compute KKT tolerances */
+	real_t stat, feas, cmpl;
+
+	getKKTResidual(	2,1,
+					H,g,A,lb,ub,lbA,ubA,
+					xOpt,yOpt,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-15 );
+	QPOASES_TEST_FOR_TOL( feas,1e-15 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
+
+
+	/* Solve second QP. */
+	nWSR = 10;
+	example.hotstart( H_new,g_new,A_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 );
+
+	/* Get and print solution of second QP. */
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+	printf( "\nxOpt = [ %e, %e ];  objVal = %e\n\n", xOpt[0],xOpt[1],example.getObjVal() );
+
+	/* Compute KKT tolerances */
+	getKKTResidual(	2,1,
+					H_new,g_new,A_new,lb_new,ub_new,lbA_new,ubA_new,
+					xOpt,yOpt,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-15 );
+	QPOASES_TEST_FOR_TOL( feas,1e-15 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1b.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1b.cpp
new file mode 100644
index 00000000000..bb812576e80
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1b.cpp
@@ -0,0 +1,119 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_example1b.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Very simple example for testing qpOASES using the QProblemB class.
+ */
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+/** Example for qpOASES main function using the QProblemB class. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	/* Setup data of first QP. */
+	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
+	real_t g[2] = { 1.5, 1.0 };
+	real_t lb[2] = { 0.5, -2.0 };
+	real_t ub[2] = { 5.0, 2.0 };
+
+	/* Setup data of second QP. */
+	real_t g_new[2] = { 1.0, 1.5 };
+	real_t lb_new[2] = { 0.0, -1.0 };
+	real_t ub_new[2] = { 5.0, -0.5 };
+
+
+	/* Setting up QProblemB object. */
+	QProblemB example( 2 );
+
+	Options options;
+	//options.enableFlippingBounds = BT_FALSE;
+	options.initialStatusBounds = ST_INACTIVE;
+	options.numRefinementSteps = 1;
+	options.enableCholeskyRefactorisation = 1;
+	example.setOptions( options );
+
+	/* Solve first QP. */
+	int nWSR = 10;
+	example.init( H,g,lb,ub, nWSR,0 );
+// 	printf( "\nnWSR = %d\n\n", nWSR );
+
+	real_t xOpt[2];
+	real_t yOpt[2];
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+
+	/* Compute KKT tolerances */
+	real_t stat, feas, cmpl;
+
+	getKKTResidual(	2,
+					H,g,lb,ub,
+					xOpt,yOpt,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-15 );
+	QPOASES_TEST_FOR_TOL( feas,1e-15 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
+
+	
+	/* Solve second QP. */
+	nWSR = 10;
+	example.hotstart( g_new,lb_new,ub_new, nWSR,0 );
+// 	printf( "\nnWSR = %d\n\n", nWSR );
+
+	/* Get and print solution of second QP. */
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+	printf( "\nxOpt = [ %e, %e ];  objVal = %e\n\n", xOpt[0],xOpt[1],example.getObjVal() );
+
+	/* Compute KKT tolerances */
+	getKKTResidual(	2,
+					H,g_new,lb_new,ub_new,
+					xOpt,yOpt,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-15 );
+	QPOASES_TEST_FOR_TOL( feas,1e-15 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example2.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example2.cpp
new file mode 100644
index 00000000000..dd73899dd4e
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example2.cpp
@@ -0,0 +1,132 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_example2.cpp
+ *	\author Hans Joachim Ferreau (thanks to Boris Houska)
+ *	\version 3.0
+ *	\date 2008-2014
+ *
+ *	Very simple example for testing qpOASES in combination
+ *  with the SolutionAnalysis class.
+ */
+
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+/** Example for qpOASES main function using the SolutionAnalysis class. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	/* Setup data of first QP. */
+	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
+	real_t A[1*2] = { 1.0, 1.0 };
+	real_t g[2] = { 1.5, 1.0 };
+	real_t lb[2] = { 0.5, -2.0 };
+	real_t ub[2] = { 5.0, 2.0 };
+	real_t lbA[1] = { -1.0 };
+	real_t ubA[1] = { 2.0 };
+
+	/* Setup data of second QP. */
+	real_t H_new[2*2] = { 1.0, 0.5, 0.5, 0.5 };
+	real_t A_new[1*2] = { 1.0, 5.0 };
+	real_t g_new[2] = { 1.0, 1.5 };
+	real_t lb_new[2] = { 0.0, -1.0 };
+	real_t ub_new[2] = { 5.0, -0.5 };
+	real_t lbA_new[1] = { -2.0 };
+	real_t ubA_new[1] = { 1.0 };
+
+
+	/* Setting up SQProblem object and solution analyser. */
+	SQProblem example( 2,1 );
+	SolutionAnalysis analyser;
+
+	/* Solve first QP ... */
+	int nWSR = 10;
+	example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
+
+	/* ... and analyse it. */
+	real_t maxKKTviolation;
+    analyser.getMaxKKTviolation( &example, maxKKTviolation );
+    printf( "maxKKTviolation: %e\n", maxKKTviolation );
+
+	QPOASES_TEST_FOR_TOL( maxKKTviolation,1e-15 );
+
+
+	/* Solve second QP ... */
+	nWSR = 10;
+	example.hotstart( H_new,g_new,A_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 );
+
+	/* ... and analyse it. */
+	analyser.getMaxKKTviolation( &example, maxKKTviolation );
+    printf( "maxKKTviolation: %e\n", maxKKTviolation );
+
+	QPOASES_TEST_FOR_TOL( maxKKTviolation,1e-15 );
+
+
+//  ------------ VARIANCE-COVARIANCE EVALUATION --------------------
+
+        real_t *Var              = new real_t[5*5];
+        real_t *Primal_Dual_Var  = new real_t[5*5];
+
+        int run1, run2;
+        for( run1 = 0; run1 < 5*5; run1++ )
+            Var[run1] = 0.0;
+
+        Var[0] = 1.0;
+        Var[6] = 1.0;
+
+//                  (  1   0   0   0   0   )
+//                  (  0   1   0   0   0   )
+//     Var     =    (  0   0   0   0   0   )
+//                  (  0   0   0   0   0   )
+//                  (  0   0   0   0   0   )
+
+
+        analyser.getVarianceCovariance( &example, Var,Primal_Dual_Var );
+
+        printf("\nPrimal_Dual_VAR = \n");
+        for( run1 = 0; run1 < 5; run1++ ){
+          for( run2 = 0; run2 < 5; run2++ ){
+            printf(" %10f", Primal_Dual_Var[run1*5+run2]);
+          }
+          printf("\n");
+        }
+
+        delete[] Primal_Dual_Var;
+        delete[] Var;
+
+		QPOASES_TEST_FOR_NEAR( Primal_Dual_Var[3*5+3], 26.0 );
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example4.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example4.cpp
new file mode 100644
index 00000000000..a4cf32983a0
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example4.cpp
@@ -0,0 +1,211 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_example4.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2009-2014
+ *
+ *	Very simple example for testing qpOASES (using the possibility to specify 
+ *	user-defined constraint product function).
+ */
+
+
+
+#include <stdlib.h>
+
+#include <qpOASES.hpp>
+#include "../../examples/example4CP.cpp"
+#include <qpOASES/UnitTesting.hpp>
+
+
+/**	Example for qpOASES main function using the possibility to specify 
+ *	user-defined constraint product function. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	int i,j;
+
+	/* Setup data of first QP... */
+	real_t H[7*7];
+	real_t A[50*7];
+	real_t g[7];
+	real_t lbA[50];
+
+	/*	    ( 1.0 0.5 |                    )
+	 *	    ( 0.5 2.0 |                    )
+	 *	    ( --------+------------------- )
+	 *	H = (         | 1e-6               )
+	 *	    (         |      1e-6          )
+	 *	    (         |           ...      )
+	 *	    (         |               1e-6 ) */
+	for( i=0; i<7*7; ++i )
+		H[i] = 0.0;
+	for( i=2; i<7; ++i )
+		H[i*7+i] = 1.0e-6;
+	H[0] = 1.0;
+	H[1] = 0.5;
+	H[7] = 0.5;
+	H[8] = 2.0;
+
+	/*	    ( x.x x.x | 1.0             )
+	 *	    ( x.x x.x | ...             )
+	 *	    ( x.x x.x | 1.0             )
+	 *	    ( x.x x.x |     1.0         )
+	 *	A = ( x.x x.x |     ...         )
+	 *	    ( x.x x.x |     1.0         )
+	 *	    ( x.x x.x |         ...     )
+	 *	    ( x.x x.x |             1.0 )
+	 *	    ( x.x x.x |             ... )
+	 *	    ( x.x x.x |             1.0 ) */
+	for( i=0; i<50*7; ++i )
+		A[i] = 0.0;
+	for( i=0; i<50; ++i )
+	{
+		for( j=0; j<2; ++j )
+			A[i*7+j] = (real_t)rand() / (real_t)RAND_MAX;
+
+		A[i*7 + (i/10)+2] = 1.0;
+	}
+
+	/*	    ( -1.0 )
+	 *	    ( -0.5 )
+	 *	    ( ---- )
+	 *	g = (      )
+	 *	    (      )
+	 *	    (      )
+	 *	    (      ) */
+	for( i=0; i<7; ++i )
+		g[i] = 0.0;
+	g[0] = -1.0;
+	g[1] = -0.5;
+
+	for( i=0; i<50; ++i )
+		lbA[i] = 1.0;
+
+	/* ... and setting up user-defined constraint product function. */
+	MyConstraintProduct myCP( 7,50,A );
+
+
+	/* Setting up QProblem object and set construct product function. */
+	QProblem exampleCP( 7,50 );
+	exampleCP.setPrintLevel( PL_NONE );
+	
+	exampleCP.setConstraintProduct( &myCP );
+
+
+	/* Solve first QP. */
+	real_t cputime = 1.0;
+	int nWSR = 100;
+	exampleCP.init( H,g,A,0,0,lbA,0, nWSR,&cputime );
+
+
+	/* Solve second QP using a modified gradient. */
+	g[0] = -2.0;
+	g[1] =  0.5;
+
+	cputime = 1.0;
+	nWSR = 100;
+	exampleCP.hotstart( g,0,0,lbA,0, nWSR,&cputime );
+
+	/* Get and print solution of second QP. */
+	real_t xOptCP[7];
+	real_t yOptCP[7+50];
+	exampleCP.getPrimalSolution( xOptCP );
+	exampleCP.getDualSolution( yOptCP );
+	printf( "\nxOpt = [ %e, %e, %e ... ];  objVal = %e\n", xOptCP[0],xOptCP[1],xOptCP[2],exampleCP.getObjVal() );
+	printf( "CPU time:  %.3f microseconds\n\n", cputime*1.0e6 ); 
+
+	/* Compute KKT tolerances */
+	real_t stat, feas, cmpl;
+
+	getKKTResidual(	7,50,
+					H,g,A,0,0,lbA,0,
+					xOptCP,yOptCP,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-15 );
+	QPOASES_TEST_FOR_TOL( feas,1e-15 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
+	
+	
+	/* Do the same without specifying constraint product. */
+	QProblem example( 7,50 );
+	example.setPrintLevel( PL_NONE );
+	
+	/* Solve first QP. */
+	g[0] = -1.0;
+	g[1] = -0.5;
+	
+	cputime = 1.0;
+	nWSR = 100;
+	example.init( H,g,A,0,0,lbA,0, nWSR,&cputime );
+	
+	/* Solve second QP using a modified gradient. */
+	g[0] = -2.0;
+	g[1] =  0.5;
+
+	cputime = 1.0;
+	nWSR = 100;
+	example.hotstart( g,0,0,lbA,0, nWSR,&cputime );
+
+	/* Get and print solution of second QP. */
+	real_t xOpt[7];
+	real_t yOpt[7+50];
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+	printf( "\nxOpt = [ %e, %e, %e ... ];  objVal = %e\n", xOpt[0],xOpt[1],xOpt[2],example.getObjVal() );
+	printf( "CPU time:  %.3f microseconds\n\n", cputime*1.0e6 ); 
+
+	/* Compute KKT tolerances */
+	getKKTResidual(	7,50,
+					H,g,A,0,0,lbA,0,
+					xOpt,yOpt,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-15 );
+	QPOASES_TEST_FOR_TOL( feas,1e-15 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
+
+	for( int ii=0; ii<7; ++ii )
+		QPOASES_TEST_FOR_NEAR( xOptCP[ii],xOpt[ii] );
+
+	for( int ii=0; ii<7+50; ++ii )
+		QPOASES_TEST_FOR_NEAR( yOptCP[ii],yOpt[ii] );
+
+	QPOASES_TEST_FOR_NEAR( exampleCP.getObjVal(),example.getObjVal() );
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example5.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example5.cpp
new file mode 100644
index 00000000000..c7f919b8277
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example5.cpp
@@ -0,0 +1,204 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_example5.cpp
+ *	\author Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2011-2014
+ *
+ *	Very simple example for testing qpOASES (using the possibility to
+ *  compute the local linear feedback law)
+ */
+
+
+
+#include <stdlib.h>
+
+#include <qpOASES.hpp>
+#include "../../examples/example4CP.cpp"
+#include <qpOASES/UnitTesting.hpp>
+
+
+/**	Example for qpOASES main function using the possibility to specify
+ *	user-defined constraint product function. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	int i,j,jj;
+	real_t d = 0.0;
+
+	/* Setup data of first QP... */
+	real_t H[7*7];
+	real_t A[50*7];
+	real_t g[7];
+	real_t lbA[50];
+
+	/*	    ( 1.0 0.5 |                    )
+	 *	    ( 0.5 2.0 |                    )
+	 *	    ( --------+------------------- )
+	 *	H = (         | 1e-6               )
+	 *	    (         |      1e-6          )
+	 *	    (         |           ...      )
+	 *	    (         |               1e-6 ) */
+	for( i=0; i<7*7; ++i )
+		H[i] = 0.0;
+	for( i=2; i<7; ++i )
+		H[i*7+i] = 1.0e-6;
+	H[0] = 1.0;
+	H[1] = 0.5;
+	H[7] = 0.5;
+	H[8] = 2.0;
+
+	/*	    ( x.x x.x | 1.0             )
+	 *	    ( x.x x.x | ...             )
+	 *	    ( x.x x.x | 1.0             )
+	 *	    ( x.x x.x |     1.0         )
+	 *	A = ( x.x x.x |     ...         )
+	 *	    ( x.x x.x |     1.0         )
+	 *	    ( x.x x.x |         ...     )
+	 *	    ( x.x x.x |             1.0 )
+	 *	    ( x.x x.x |             ... )
+	 *	    ( x.x x.x |             1.0 ) */
+	for( i=0; i<50*7; ++i )
+		A[i] = 0.0;
+	for( i=0; i<50; ++i )
+	{
+		for( j=0; j<2; ++j )
+			A[i*7+j] = (real_t)rand() / (real_t)RAND_MAX;
+
+		A[i*7 + (i/10)+2] = 1.0;
+	}
+
+	/*	    ( -1.0 )
+	 *	    ( -0.5 )
+	 *	    ( ---- )
+	 *	g = (      )
+	 *	    (      )
+	 *	    (      )
+	 *	    (      ) */
+	for( i=0; i<7; ++i )
+		g[i] = 0.0;
+	g[0] = -1.0;
+	g[1] = -0.5;
+
+	for( i=0; i<50; ++i )
+		lbA[i] = 1.0;
+
+	/* ... and setting up user-defined constraint product function. */
+	MyConstraintProduct myCP( 7,50,A );
+
+
+	/* Setting up QProblem object and set construct product function. */
+	QProblem example( 7,50 );
+	example.setConstraintProduct( &myCP );
+
+
+	/* Solve first QP. */
+	real_t cputime = 1.0;
+	int nWSR = 100;
+	example.init( H,g,A,0,0,lbA,0, nWSR,&cputime );
+
+	/* Get and print solution of QP. */
+	real_t xOpt[7], yOpt[7+50];
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+
+
+	/* Compute local linear feedback law */
+	const int n_rhs = 7+7+50;
+	real_t g_in[7*n_rhs];
+	real_t b_in[7*n_rhs];
+	real_t bA_in[50*n_rhs];
+	real_t x_out[7*n_rhs];
+	real_t y_out[(7+50)*n_rhs];
+
+	int ii;
+	memset (g_in, 0, sizeof (g_in));
+	memset (b_in, 0, sizeof (b_in));
+	memset (bA_in, 0, sizeof (bA_in));
+
+	for ( ii = 0; ii < 7; ++ii )
+		g_in[ii*7 + ii] = 1.0;
+	for ( ii = 0; ii < 7; ++ii )
+		b_in[(ii+7)*7 + ii] = 1.0;
+	for ( ii = 0; ii < 50; ++ii )
+		bA_in[(ii+14)*50 + ii] = 1.0;
+
+	example.solveCurrentEQP ( n_rhs, g_in, b_in, b_in, bA_in, bA_in, x_out, y_out );
+
+	/* Verify validity of local feedback law by perturbation and hot starts */
+	real_t perturb = 1.0e-6;
+	real_t nrm = 0.0;
+	for ( ii = 0; ii < n_rhs; ++ii )
+	{
+		for ( jj = 0; jj < 7; ++jj )
+			g_in[ii*7 + jj] = g[jj] + g_in[ii*7+jj]*perturb;
+		for ( jj = 0; jj < 50; ++jj )
+			bA_in[ii*50 + jj] = lbA[jj] + bA_in[ii*50+jj]*perturb;
+
+		nWSR = 100;
+		example.hotstart( &g_in[ii*7],0,0,&bA_in[ii*50],0, nWSR, 0 );
+
+		real_t xPer[7], yPer[7+50];
+		example.getPrimalSolution( xPer );
+		example.getDualSolution( yPer );
+
+		for ( jj = 0; jj < 7; ++jj )
+		{
+			d = getAbs (x_out[ii*7+jj]*perturb - (xPer[jj]-xOpt[jj]) );
+			if (nrm < d) nrm=d;
+		}
+		for ( jj = 0; jj < 7+50; ++jj )
+		{
+			d = getAbs (y_out[ii*(7+50)+jj]*perturb - (yPer[jj]-yOpt[jj]) );
+			if (nrm < d) nrm=d;
+		}
+	}
+	printf ("Maximum perturbation over all directions: %e\n", nrm);
+
+	QPOASES_TEST_FOR_TOL( nrm,1e-15 );
+
+
+	/* // print feedback matrix
+	for (ii = 0; ii < n_rhs; ++ii)
+	{
+		printf ("x: ");
+		for (jj = 0; jj < 7; ++jj )
+			printf ("%8.2e ", x_out[ii*7+jj]);
+		printf (" y: ");
+		for (jj = 0; jj < 7+50; ++jj )
+			printf ("%8.2e ", y_out[ii*(7+50)+jj]);
+		printf("\n");
+	}
+*/
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example6.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example6.cpp
new file mode 100644
index 00000000000..577aec123de
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example6.cpp
@@ -0,0 +1,112 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_example6.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Example that caused troubles in an earlier release.
+ */
+
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+/** Example for qpOASES main function using the QProblem class. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	/* Setup data of first QP. */
+	real_t H[5*5] = {	1.224642131370767e+01, 2.908638763113702e+00, 0.0, 0.0, 0.0,
+						2.908638763113702e+00, 2.497106275003180e+00, 0.0, 0.0, 0.0,
+						0.0, 0.0, 1.0, 0.0, 0.0,
+						0.0, 0.0, 0.0, 5.158460640334052e-02, 4.723556059962540e-02,
+						0.0, 0.0, 0.0, 4.723556059962540e-02, 4.325317843302175e-02 };
+	real_t A[2*5] = { 	-1.404358970692652e+00, -2.556613491156063e+00, 3.202524559238066e+00, -1.0, 0.0,
+						6.587910295430314e-01, -5.349454475937998e-01, 4.391976356955536e-01, 0.0, -1.0 };
+	real_t g[5] = { 	2.474135331302147e+01,
+						5.857286430296258e+00,
+						2.359382646348721e-01,
+						1.721047069188781e-01,
+						1.575947337774199e-01 };
+	real_t lb[5] = { -10.0, -10.0, -10.0, -10.0, -10.0 };
+	real_t ub[5] = {  10.0,  10.0,  10.0,  10.0,  10.0 };
+	real_t lbA[2] = { 1.643135416077167e+00, 1.056813028189597e+00 };
+	real_t ubA[2] = { 1.643135416077167e+00, 1.056813028189597e+00 };
+
+	/* Setting up QProblem object. */
+	QProblem example( 5,2 );
+
+	Options options;
+ 	//options.enableFlippingBounds = BT_FALSE;
+	//options.enableEqualities = BT_TRUE;
+	//options.initialStatusBounds = ST_INACTIVE;
+	example.setOptions( options );
+	example.setPrintLevel( PL_NONE );
+	
+	/* Solve first QP. */
+	returnValue retVal;
+	int simpleStatus = -1;
+
+	int nWSR = 10;
+	retVal = example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
+	simpleStatus = getSimpleStatus( retVal,BT_TRUE );
+
+	QPOASES_TEST_FOR_TRUE( simpleStatus == 0 );
+
+
+	/* Get and print solution of second QP. */
+	real_t xOpt[5];
+	real_t yOpt[5+2];
+	
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+	printf( "\nxOpt = [ %e, %e, ... ];  objVal = %e\n\n", xOpt[0],xOpt[1],example.getObjVal() );
+	
+	/* Compute KKT tolerances */
+	real_t stat, feas, cmpl;
+
+	getKKTResidual(	5,2,
+					H,g,A,lb,ub,lbA,ubA,
+					xOpt,yOpt,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-14 );
+	QPOASES_TEST_FOR_TOL( feas,1e-14 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example7.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example7.cpp
new file mode 100644
index 00000000000..a9a7bfc3d4f
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example7.cpp
@@ -0,0 +1,90 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_example7.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Example that caused troubles in an earlier release.
+ */
+
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	real_t H[5*5] = {0.8514828085899353, -0.15739890933036804, -0.081726007163524628, -0.530426025390625, 0.16773293912410736, -0.15739890933036804, 1.1552412509918213, 0.57780224084854126, -0.0072606131434440613, 0.010559185408055782, -0.081726007163524628, 0.57780224084854126, 0.28925251960754395, 5.324830453901086e-006, -3.0256599075073609e-006, -0.530426025390625, -0.0072606131434440613, 5.324830453901086e-006, 0.35609596967697144, -0.15124998986721039, 0.16773293912410736, 0.010559185408055782, -3.0256599075073609e-006, -0.15124998986721039, 0.15129712224006653};
+	real_t g[5] = {0.30908384919166565, 0.99325823783874512, 0.49822014570236206, -0.26309865713119507, 0.024296050891280174};
+	real_t A[5*5] =   {1,0,0,0,0,
+					   0,1,0,0,0,
+					   0,0,1,0,0,              
+					   0,0,0,1,0,
+					   0,0,0,0,1};
+	real_t lb[5] = {-0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359938621520996};
+	real_t ub[5] = { 0.052359879016876221, 0.052359879016876221, 0.052359879016876221, 0, 0};
+	real_t lbA[5] = {-0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359938621520996};
+	real_t ubA[5] = {0.052359879016876221, 0.052359879016876221, 0.052359879016876221, 0, 0};
+
+	/* Setting up QProblem object. */
+	QProblem example( 5,5 );
+
+	/* Solve first QP. */
+	int nWSR = 100;
+	returnValue retVal = example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
+	printf( "nWSR = %d,  retVal = %d (%s)\n", nWSR,retVal,getGlobalMessageHandler( )->getErrorCodeMessage(retVal) );
+	
+	
+	real_t sol[5]    = {0};
+	real_t yOpt[5+5] = {0};
+	example.getPrimalSolution(sol);
+	example.getDualSolution(yOpt);
+	
+	printf("l1 = %f, l2 = %f, l3 = %f, l4 = %f, l5 = %f\n",(float) lb[0], (float) lb[1], (float) lb[2], (float) lb[3], (float) lb[4]);
+	printf("x1 = %f, x2 = %f, x3 = %f, x4 = %f, x5 = %f\n",(float) sol[0], (float) sol[1], (float) sol[2], (float) sol[3], (float) sol[4]);
+	printf("u1 = %f, u2 = %f, u3 = %f, u4 = %f, u5 = %f\n",(float) ub[0], (float) ub[1], (float) ub[2], (float) ub[3], (float) ub[4]);
+
+	/* Compute KKT tolerances */
+	real_t stat, feas, cmpl;
+
+	getKKTResidual(	5,2,
+					H,g,A,lb,ub,lbA,ubA,
+					sol,yOpt,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-15 );
+	QPOASES_TEST_FOR_TOL( feas,1e-15 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
+
+	return TEST_PASSED;
+}
+
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_exampleLP.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_exampleLP.cpp
new file mode 100644
index 00000000000..e4a1681e4b4
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_exampleLP.cpp
@@ -0,0 +1,121 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_exampleLP.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2008-2014
+ *
+ *	Very simple example for solving a LP sequence using qpOASES.
+ */
+
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+/** Example for qpOASES main function solving LPs. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	real_t tol = EPS;
+
+	/* Setup data of first LP. */
+	real_t A[1*2] = { 1.0, 1.0 };
+	real_t g[2] = { 1.5, 1.0 };
+	real_t lb[2] = { 0.5, -2.0 };
+	real_t ub[2] = { 5.0, 2.0 };
+	real_t lbA[1] = { -1.0 };
+	real_t ubA[1] = { 2.0 };
+
+	/* Setup data of second LP. */
+	real_t g_new[2] = { 1.0, 1.5 };
+	real_t lb_new[2] = { 0.0, -1.0 };
+	real_t ub_new[2] = { 5.0, -0.5 };
+	real_t lbA_new[1] = { -2.0 };
+	real_t ubA_new[1] = { 1.0 };
+
+
+	/* Setting up QProblem object with zero Hessian matrix. */
+	QProblem example( 2,1,HST_ZERO );
+
+	Options options;
+ 	//options.setToMPC(); tol = 1e-12;
+	example.setOptions( options );
+
+	/* Solve first LP. */
+	int nWSR = 10;
+	example.init( 0,g,A,lb,ub,lbA,ubA, nWSR,0 );
+
+	real_t xOpt[2];
+	real_t yOpt[2+1];
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+
+	/* Compute KKT tolerances */
+	real_t stat, feas, cmpl;
+
+	getKKTResidual(	2,1,
+					0,g,A,lb,ub,lbA,ubA,
+					xOpt,yOpt,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,tol );
+	QPOASES_TEST_FOR_TOL( feas,tol );
+	QPOASES_TEST_FOR_TOL( cmpl,tol );
+
+
+	/* Solve second LP. */
+	nWSR = 10;
+	example.hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 );
+
+
+	/* Get and print solution of second LP. */
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+	printf( "\nxOpt = [ %e, %e ];  objVal = %e\n\n", xOpt[0],xOpt[1],example.getObjVal() );
+
+	getKKTResidual(	2,1,
+					0,g_new,A,lb_new,ub_new,lbA_new,ubA_new,
+					xOpt,yOpt,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,tol );
+	QPOASES_TEST_FOR_TOL( feas,tol );
+	QPOASES_TEST_FOR_TOL( cmpl,tol );
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_gradientShift.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_gradientShift.cpp
new file mode 100644
index 00000000000..d4d8cc81897
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_gradientShift.cpp
@@ -0,0 +1,125 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2009 by Hans Joachim Ferreau et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_gradientShift.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Simple test case which caused troubles in version 2.0.
+ */
+
+ 
+
+#include <math.h>
+#include <iostream>
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+/** Running simple test case which caused troubles in version 2.0. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	
+	double H[2*2] 	= { 0.055944055944055944,0, 0, 0 };
+	double A[1*2] 	= { 0.70514808036997589, -1 };
+	//double g[2]	= { -15.830543073928741, 0};
+	double g[2]		= { 0, 0 };
+	double lb[2]	= { 137.00242299940646, 154.0 };
+	double ub[2]	= { 282.19008595111382, 198.98579740786641};
+	double lbA[1]	= {0.0};
+	double ubA[1]	= {0.0};
+	
+	double gStart=-16.4;
+ 
+	/* Setting up QProblem object. */
+	QProblem example( 2,1 );
+	Options options;
+	options.setToMPC();
+	options.printLevel = REFER_NAMESPACE_QPOASES PL_NONE;
+	example.setOptions( options );
+	
+	returnValue ret;
+	int nWSR = 10;
+	double Xopt[2]={0.0,0.0};
+	//fprintf(stdFile, "g[0]\t,\tReturn code\n");
+	int errorCount=0;
+	int i=0;
+	double granularity=0.00001;
+	g[0]=gStart;
+	for( i=0; i< 70000; i++)
+	{
+
+		g[0]=g[0]+granularity;
+		/* Solve first QP. */
+		nWSR = 10;
+		ret= example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
+		if (ret != SUCCESSFUL_RETURN)
+		{
+			//fprintf(stdFile, "%f\t,\t%d\n",g[0],ret);
+			errorCount++;
+		}
+		//fprintf(stdFile, "%f\t,\t%d\n",g[0],ret);
+	}
+	example.printProperties();
+	fprintf(stdFile, "#Number of optimizer runs: %d\n",i);
+	fprintf(stdFile, "#g[0] test interval: %f < g[0] < %f\n",gStart,g[0]);
+	fprintf(stdFile, "#Granularity: %f\n",granularity);
+	double errorPercent = double(errorCount)/double(i)*100.0;
+	fprintf(stdFile, "#Number of errors (error): %d (%f)\n",errorCount,errorPercent);
+
+	example.getPrimalSolution(Xopt);
+	fprintf(stdFile,"#Optimization primary result : LD=%f BD=%f\n",Xopt[0], Xopt[1]);
+
+	double Yopt[3]={0.0,0.0,0.0};
+	example.getDualSolution(Yopt);
+	fprintf(stdFile,"#Optimization dual result : %f %f %f8\n",Yopt[0], Yopt[1], Yopt[2]);
+
+	int Nc=0;
+	Nc=example.getNC();
+	fprintf(stdFile,"#Number of constraints : %d\n",Nc);
+
+	int Nec=0;
+	Nec=example.getNEC();
+	fprintf(stdFile,"#Number of equality constraints : %d\n",Nec);
+
+	int Nac=0;
+	Nac=example.getNAC();
+	fprintf(stdFile,"#Number of active constraints : %d\n",Nac);
+
+	int Niac=0;
+	Niac=example.getNIAC();
+	fprintf(stdFile,"#Number of inactive constraints : %d\n",Niac);
+
+	QPOASES_TEST_FOR_TRUE( errorCount == 0 )
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_guessedWS1.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_guessedWS1.cpp
new file mode 100644
index 00000000000..04de2a710eb
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_guessedWS1.cpp
@@ -0,0 +1,164 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_guessedWS1.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2014
+ *
+ *	Very simple example for testing qpOASES (using QProblem class).
+ */
+
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+/** Example for qpOASES main function using the QProblem class. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	/* Setup data of first QP. */
+	real_t H[4*4] = { 1.0, 0.0, 0.0, 0.5, 
+	                  0.0, 1.0, 0.0, 0.0,
+	                  0.0, 0.0, 1.0, 0.0,
+	                  0.5, 0.0, 0.0, 1.0 };
+	real_t A[3*4] = { 1.0, 1.0, 0.0, 0.0,
+	                  1.0, 1.0, 1.0, 0.0,
+	                  1.0, 1.0, 1.0, 1.0 };
+	real_t g[4] = { 1.5, 1.0, -1.0, -1.0 };
+	real_t lb[4] = { 0.5, -2.0, 0.0, 0.0 };
+	real_t ub[4] = { 1.0, 2.0, 1.0, 0.5 };
+	real_t lbA[3] = { -1.0, -1.0, -1.0 };
+	real_t ubA[3] = { 0.0, 0.25, 1.0 };
+
+	
+
+	/* Setting up QProblem object. */
+	QProblem example( 4,3 );
+
+	Options options;
+	example.setOptions( options );
+
+	/* Solve first QP. */
+	int nWSR = 10;
+	example.init( H,g,A,lb,ub,lbA,ubA, nWSR );
+
+	/* Get and print solution of second QP. */
+	real_t xOpt[4];
+	real_t yOpt[4+3];
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+	print( xOpt,4,"xOpt" );
+	print( yOpt,4+3,"yOpt" );
+	printf( "objVal = %e\n\n", example.getObjVal() );
+	
+	/* Compute KKT tolerances */
+	real_t stat, feas, cmpl;
+
+	getKKTResidual(	4,3,
+					H,g,A,lb,ub,lbA,ubA,
+					xOpt,yOpt,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-15 );
+	QPOASES_TEST_FOR_TOL( feas,1e-15 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
+
+
+	/* Solve first QP again (with optimal guess for working set). */
+	Bounds prevBounds;
+	Constraints prevConstraints;
+	
+	example.getBounds( prevBounds );
+	example.getConstraints( prevConstraints );
+
+	nWSR = 10;
+	example.hotstart( g,lb,ub,lbA,ubA, nWSR,0,&prevBounds,&prevConstraints );
+
+	/* Get and print solution of second QP. */
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+	print( xOpt,4,"xOpt" );
+	print( yOpt,4+3,"yOpt" );
+	printf( "objVal = %e\n\n", example.getObjVal() );
+	
+	/* Compute KKT tolerances */
+	getKKTResidual(	4,3,
+					H,g,A,lb,ub,lbA,ubA,
+					xOpt,yOpt,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-15 );
+	QPOASES_TEST_FOR_TOL( feas,1e-15 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
+
+
+	/* Solve first QP again (with inaccurate guess for working set). */
+	prevBounds.print();
+	prevBounds.rotate(1);
+	//prevBounds.moveFixedToFree(0);
+	prevBounds.print();
+
+	prevConstraints.print();
+	//prevConstraints.moveInactiveToActive(0,ST_LOWER);
+	prevConstraints.moveActiveToInactive(1);
+	prevConstraints.print();
+
+	nWSR = 10;
+	example.hotstart( g,lb,ub,lbA,ubA, nWSR,0,&prevBounds,&prevConstraints );
+
+	/* Get and print solution of second QP. */
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+	print( xOpt,4,"xOpt" );
+	print( yOpt,4+3,"yOpt" );
+	printf( "objVal = %e\n\n", example.getObjVal() );
+	
+	/* Compute KKT tolerances */
+	getKKTResidual(	4,3,
+					H,g,A,lb,ub,lbA,ubA,
+					xOpt,yOpt,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-15 );
+	QPOASES_TEST_FOR_TOL( feas,1e-15 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_indexlist.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_indexlist.cpp
new file mode 100644
index 00000000000..552dfc2a115
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_indexlist.cpp
@@ -0,0 +1,96 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_indexlist.cpp
+ *	\author Andreas Potschka, Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2010-2014
+ *
+ *	Unit test for Indexlist class.
+ */
+
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+/** Test Indexlist sorting */
+int main()
+{
+	USING_NAMESPACE_QPOASES
+
+	Indexlist il(10);
+	int i, *numbers;
+
+	il.addNumber(1);
+	il.addNumber(3);
+	il.addNumber(5);
+	il.addNumber(2);
+	il.addNumber(4);
+	il.addNumber(0);
+	il.addNumber(7);
+	il.addNumber(6);
+	il.addNumber(8);
+	il.addNumber(9);
+
+	il.getNumberArray(&numbers);
+	fprintf(stdFile, "Unsorted numbers: ");
+	for (i = 0; i < 10; i++)
+		fprintf(stdFile, " %2d", numbers[i]);
+	fprintf(stdFile, "\n");
+
+	fprintf(stdFile, "Unsorted index of number 0: %3d\n", il.getIndex(0));
+
+	QPOASES_TEST_FOR_TRUE( il.getIndex(0) == 5 )
+
+	il.removeNumber(5);
+	fprintf(stdFile, "Unsorted index of (removed) number 5: %3d\n", il.getIndex(5));
+
+	QPOASES_TEST_FOR_TRUE( il.getIndex(5) == -1 )
+
+	il.getNumberArray(&numbers);
+	fprintf(stdFile, "Unsorted numbers: ");
+	for (i = 0; i < 9; i++)
+		fprintf(stdFile, " %2d", numbers[i]);
+	fprintf(stdFile, "\n");
+
+	il.swapNumbers(2, 7);
+
+	il.getNumberArray(&numbers);
+	fprintf(stdFile, "Unsorted numbers: ");
+	for (i = 0; i < 9; i++)
+		fprintf(stdFile, " %2d", numbers[i]);
+	fprintf(stdFile, "\n");
+
+	QPOASES_TEST_FOR_TRUE( numbers[2] == 7 )
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_janick1.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_janick1.cpp
new file mode 100644
index 00000000000..39cc744baeb
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_janick1.cpp
@@ -0,0 +1,197 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file debugging/cpp/debug_janick1.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Example that causes troubles when hotstarting.
+ */
+
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+#include <stdio.h>
+
+
+//#define __MAKE_POS_DEF__
+#undef __MAKE_POS_DEF__
+
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	int nWSR = 100;
+	/* Setting up QProblem object. */
+	SQProblem example( 11,3 );
+
+	Options options;
+	options.setToFast();
+	//options.setToDefault();
+	//options.initialStatusBounds = qpOASES::ST_INACTIVE;
+	example.setOptions( options );
+
+
+	/* Setup data of first QP. */
+	real_t H[11*11] = {
+	6.20100988531485e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-3.84861756786704e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-7.43268431723266e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	0.00000000000000e+00,	 1.00000000000000e-01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	-3.84861756786704e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 5.41188294952735e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 4.61304826562310e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.10000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 1.00000000000000e-01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.01000000000000e+01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.10000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	-7.43268431723266e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 4.61304826562310e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.73544778892019e+01,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+00,	 0.00000000000000e+00,
+	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+01
+	};
+	real_t g[11] =	{
+	-1.28196485091041e-07,	 1.13322235104306e-08,	 4.52417455660281e-06,	 1.95881304363781e-07,	 1.61991589634459e-06,	 2.69786724710014e-09,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.18185813270785e+02,	 9.58903285459640e-06,	 3.82276791847511e-06
+	};
+	real_t zLow[11] =	{
+	-1.07876236159966e+01,	-1.00000000002784e+12,	-1.00000000000000e+12,	 0.00000000000000e+00,	-7.00000000000000e+00,	-2.60479553037772e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-4.50000000000000e+01,	-1.00000000000000e+12
+	};
+	real_t zUpp[11] =	{
+	9.99999999989212e+11,	 9.99999999972157e+11,	 1.00000000000000e+12,	 4.68471854329546e+01,	 7.00000000000000e+00,	 9.99999999997395e+11,	 1.00000000000000e+12,	 1.00000000000000e+12,	 0.00000000000000e+00,	 4.50000000000000e+01,	 1.00000000000000e+12
+	};
+	real_t D[11*3] =	{
+	1.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.00000000000000e-02,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	-1.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-1.00000000000000e-02,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,
+	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 1.00000000000000e+00,	 0.00000000000000e+00,	-1.00000000000000e-02,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00
+	};
+	real_t dLow[3] =	{
+	-1.00000000000000e+12,	-1.00000000000000e+12,	-1.00000000000000e+12
+	};
+	real_t dUpp[3] =	{
+	2.12376384003361e-01,	 4.78762361599664e+00,	 8.95204469622285e-01
+	};
+
+	#ifdef __MAKE_POS_DEF__
+	H[9*11+9] += 30;
+	#endif
+	returnValue status = example.init( H,g,D,zLow,zUpp,dLow,dUpp, nWSR );
+	printf("\nqpOASES_status = %d\n", (int)status );
+
+	/* Get and print solution of first QP. */
+	real_t xOpt[11];
+	real_t yOpt[11+3];
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+	printf("first QP:\n");
+	for (int ii =0; ii<11; ++ii )	{
+		printf("x[%d] = %.3e\n", ii, xOpt[ii]);
+	}
+
+	/* Compute KKT tolerances */
+	real_t stat, feas, cmpl;
+
+	getKKTResidual(	11,3,
+					H,g,D,zLow,zUpp,dLow,dUpp,
+					xOpt,yOpt,
+					stat,feas,cmpl
+					);
+	printf( "\nstat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-9 );
+	QPOASES_TEST_FOR_TOL( feas,1e-7 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
+
+
+
+	nWSR = 100;
+
+	/* Setup data of second QP. */
+	real_t H2[11*11] = {
+	6.20100988531485e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-3.84861756786704e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-7.43268433147671e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	0.00000000000000e+00,	 1.00000000000000e-01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	-3.84861756786704e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 5.41188294952735e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 4.61304827446359e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.10000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 1.00000000000000e-01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.01000000000000e+01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.10000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	-7.43268433147671e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 4.61304827446359e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.73544778550554e+01,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+00,	 0.00000000000000e+00,
+	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+01
+	};
+	real_t g2[11] =	{
+	-6.57752219998813e-04,	 3.61759517784935e-04,	-7.51224940044046e-05,	 6.01232615232452e-04,	 1.23562395380546e-04,	 4.59040118744990e-05,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.18185813270785e+02,	 3.38813178901720e-21,	 0.00000000000000e+00
+	};
+	real_t zLow2[11] =	{
+	-2.41857058824735e-08,	-2.77410094895458e-10,	-9.90563843681406e-09,	-3.85694249871449e-08,	 4.50633213817586e-14,	-2.22044604925031e-15,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-4.49999952054836e+01,	-1.00000000000000e+12
+	};
+	real_t zUpp2[11] =	{
+	-2.41857058824735e-08,	-2.77410094895458e-10,	-9.90563843681406e-09,	-3.85694249871449e-08,	 4.50633213817586e-14,	-2.22044604925031e-15,	 1.00000000000000e+12,	 1.00000000000000e+12,	 0.00000000000000e+00,	 4.50000047945164e+01,	 1.00000000000000e+12,
+	};
+	real_t D2[11*3] =	{
+	1.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.00000000000000e-02,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	-1.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-1.00000000000000e-02,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,
+	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 1.00000000000000e+00,	 0.00000000000000e+00,	-1.00000000000000e-02,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00
+	};
+	real_t dLow2[3] =	{
+	-1.00000000000000e+12,	-1.00000000000000e+12,	-1.00000000000000e+12
+	};
+	real_t dUpp2[3] =	{
+	2.12376363329877e-01,	 4.78762363667012e+00,	 8.95204496600957e-01
+	};
+	#ifdef __MAKE_POS_DEF__
+	H2[9*11+9] += 30;
+	#endif
+
+
+	status = example.hotstart( H2,g2,D2,zLow2,zUpp2,dLow2,dUpp2, nWSR );
+	printf("qpOASES_status = %d\n", (int)status );
+
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+	printf("second QP:\n");
+	for (int ii =0; ii<11; ++ii )	{
+		printf("x[%d] = %.3e\n", ii, xOpt[ii]);
+	}
+
+	/* Compute KKT tolerances */
+	getKKTResidual(	11,3,
+					H2,g2,D2,zLow2,zUpp2,dLow2,dUpp2,
+					xOpt,yOpt,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-9 );
+	QPOASES_TEST_FOR_TOL( feas,1e-7 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-12 );
+
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_janick2.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_janick2.cpp
new file mode 100644
index 00000000000..e25a032c4a8
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_janick2.cpp
@@ -0,0 +1,251 @@
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+#include <stdio.h>
+
+#define __MAKE_POS_DEF__
+// #undef __MAKE_POS_DEF__
+
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	int nWSR = 100;
+	/* Setting up QProblem object. */
+	SQProblem example( 11,3 );
+
+	Options options;
+	options.setToFast();
+// 	options.setToDefault();
+	options.initialStatusBounds = REFER_NAMESPACE_QPOASES ST_INACTIVE;
+
+	//options.terminationTolerance = 1.e-12;
+	options.initialStatusBounds = REFER_NAMESPACE_QPOASES ST_INACTIVE;
+	//options.enableFarBounds = REFER_NAMESPACE_QPOASES BT_FALSE;
+	//options.enableRegularisation = REFER_NAMESPACE_QPOASES BT_FALSE;
+
+	example.setOptions( options );
+
+
+	/* Setup data of first QP. */
+	real_t H[11*11] = {
+	 6.20100988531485e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-3.84861756786704e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-7.43268431723266e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	 0.00000000000000e+00,	 1.00000000000000e-01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	-3.84861756786704e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 5.41188294952735e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 4.61304826562310e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.10000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 1.00000000000000e-01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.01000000000000e+01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.10000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	-7.43268431723266e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 4.61304826562310e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.73544778892019e+01,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+00,	 0.00000000000000e+00,
+	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+01,
+	};
+	real_t g[11] =	{
+//	 9.13378607947379e-07,
+//	 0.00000000000000e+00,
+//	 0.00000000000000e+00,
+//	-1.12448469735682e-06,
+//	 0.00000000000000e+00,
+//	 0.00000000000000e+00,
+//	 0.00000000000000e+00,
+//	 0.00000000000000e+00,
+//	-1.18185650936822e+02,
+//	 0.00000000000000e+00,
+//	 0.00000000000000e+00,
+	 -6.93766478421491e-04,
+	  3.84943289898669e-04,
+	 -3.63779116055460e-05,
+	  6.38114176725135e-04,
+	  1.85797765355698e-04,
+	  6.21922122437904e-05,
+	  0.00000000000000e+00,
+	  0.00000000000000e+00,
+	 -1.18185758699839e+02,
+	  1.54357580390960e-05,
+	  5.39852809009711e-06,
+	};
+	real_t zLow[11] =	{
+	 0.00000000000000e+00,
+	 0.00000000000000e+00,
+	 0.00000000000000e+00,
+	 0.00000000000000e+00,
+	 0.00000000000000e+00,
+	 0.00000000000000e+00,
+	 0.00000000000000e+00,
+	 0.00000000000000e+00,
+	 0.00000000000000e+00,
+	-4.50000000000000e+01,
+	-1.00000000000000e+12,
+	};
+	real_t zUpp[11] =	{
+	 0.00000000000000e+00,
+	 0.00000000000000e+00,
+	 0.00000000000000e+00,
+	 0.00000000000000e+00,
+	 0.00000000000000e+00,
+	 0.00000000000000e+00,
+	 1.00000000000000e+12,
+	 1.00000000000000e+12,
+	 0.00000000000000e+00,
+	 4.50000000000000e+01,
+	 1.00000000000000e+12,
+	};
+	real_t D[11*3] =	{
+	 1.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.00000000000000e-02,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	-1.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-1.00000000000000e-02,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,
+	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 1.00000000000000e+00,	 0.00000000000000e+00,	-1.00000000000000e-02,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	};
+	real_t dLow[3] =	{
+	-1.00000000000000e+12,
+	-1.00000000000000e+12,
+	-1.00000000000000e+12,
+	};
+	real_t dUpp[3] =	{
+	 2.12376384003361e-01,
+	 4.78762361599664e+00,
+	 8.95204469622285e-01,
+	};
+
+
+	#ifdef __MAKE_POS_DEF__
+// 	H[9*11+9] += 30;
+	H[8*11+8] += 30;
+	#endif
+	returnValue status = example.init( H,g,D,zLow,zUpp,dLow,dUpp, nWSR );
+	printf("qpOASES_status = %d\n", (int)status );
+
+	/* Get and print solution of second QP. */
+	real_t xOpt[11];
+	real_t yOpt[11+3];
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+	printf("first QP:\n");
+	for (int ii =0; ii<11; ++ii )	{
+		printf("x[%d] = %.3e\n", ii, xOpt[ii]);
+	}
+
+	/* Compute KKT tolerances */
+	real_t stat, feas, cmpl;
+
+	getKKTResidual( 11,3,
+			H,g,D,zLow,zUpp,dLow,dUpp,
+			xOpt,yOpt,
+			stat,feas,cmpl
+	);
+	printf( "\nstat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-9 );
+	QPOASES_TEST_FOR_TOL( feas,1e-7 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
+
+	nWSR = 100;
+
+	/* Setup data of second QP. */
+	real_t H2[11*11] = {
+	 6.20101055067033e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-3.84861780549400e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-7.43268533746787e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	 0.00000000000000e+00,	 1.00000000000000e-01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	-3.84861780549400e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 5.41188396792859e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 4.61304896387257e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.10000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 1.00000000000000e-01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.01000000000000e+01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.10000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	-7.43268533746787e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 4.61304896387257e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.73544780086860e+01,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+00,	 0.00000000000000e+00,
+	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+01,
+	};
+	real_t g2[11] =	{
+	-8.92227256391600e-08,
+	 6.89531726031141e-08,
+	-1.91970120006650e-07,
+	 1.77206607789402e-07,
+	-3.83145267945144e-09,
+	-1.88284265021358e-08,
+	 0.00000000000000e+00,
+	 0.00000000000000e+00,
+	-1.18185657392775e+02,
+	 1.45337027424899e-17,
+	-6.04156175796480e-20,
+	};
+	real_t zLow2[11] =	{
+	-1.07876236566374e+01,
+	-1.00000000002784e+12,
+	-1.00000000000000e+12,
+	-8.30554585107279e-08,
+	-7.00000003695781e+00,
+	-2.60479531522807e+00,
+	 0.00000000000000e+00,
+	 0.00000000000000e+00,
+	 0.00000000000000e+00,
+	-4.50000000018062e+01,
+	-1.00000000000000e+12,
+	};
+	real_t zUpp2[11] =	{
+	 9.99999999989212e+11,
+	 9.99999999972157e+11,
+	 1.00000000000000e+12,
+	 4.68471853498991e+01,
+	 6.99999996304219e+00,
+	 9.99999999997395e+11,
+	 1.00000000000000e+12,
+	 1.00000000000000e+12,
+	 0.00000000000000e+00,
+	 4.49999999981938e+01,
+	 1.00000000000000e+12,
+	};
+	real_t D2[11*3] =	{
+	 1.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.00000000000000e-02,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	-1.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-1.00000000000000e-02,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,
+	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 1.00000000000000e+00,	 0.00000000000000e+00,	-1.00000000000000e-02,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
+	};
+	real_t dLow2[3] =	{
+	-1.00000000000000e+12,
+	-1.00000000000000e+12,
+	-1.00000000000000e+12,
+	};
+	real_t dUpp2[3] =	{
+	 2.12376343362616e-01,
+	 4.78762365663739e+00,
+	 8.95204684771929e-01,
+	};
+	#ifdef __MAKE_POS_DEF__
+	H2[8*11+8] += 30;
+// 	H2[9*11+9] += 30;
+	#endif
+
+
+	status = example.hotstart( H2,g2,D2,zLow2,zUpp2,dLow2,dUpp2, nWSR );
+	printf("qpOASES_status = %d\n", (int)status );
+
+	example.getPrimalSolution( xOpt );
+	example.getDualSolution( yOpt );
+	printf("second QP:\n");
+	for (int ii =0; ii<11; ++ii )	{
+		printf("x[%d] = %.3e\n", ii, xOpt[ii]);
+	}
+	
+	printf( "\nQP objective value: %.3e\n", example.getObjVal() );
+
+	/* Compute KKT tolerances */
+
+	getKKTResidual( 11,3,
+			H2,g2,D2,zLow2,zUpp2,dLow2,dUpp2,
+			xOpt,yOpt,
+			stat,feas,cmpl
+	);
+	printf( "\nstat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
+
+	QPOASES_TEST_FOR_TOL( stat,1e-9 );
+	QPOASES_TEST_FOR_TOL( feas,1e-7 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
+
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices.cpp
new file mode 100644
index 00000000000..1d5e0cb5607
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices.cpp
@@ -0,0 +1,903 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_matrices.cpp
+ *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2010-2014
+ *
+ *	Unit test for Matrix classes.
+ */
+
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+USING_NAMESPACE_QPOASES
+
+
+/** Compare deviations when computing dot product. */
+int sumOfSquares()
+{
+	int i;
+
+	/* sum of first n squares */
+	const int N = 100;
+	real_t *av = new real_t[N];
+	real_t *aTv = new real_t[N];
+	real_t *bv = new real_t[N];
+	real_t c;
+
+	for (i = 0; i < N; i++) av[i] = i+1.0;
+	for (i = 0; i < N; i++) aTv[i] = i+1.0;
+	for (i = 0; i < N; i++) bv[i] = i+1.0;
+
+	DenseMatrix a(1, N, N, av);
+	DenseMatrix aT(N, 1, 1, aTv);
+
+	a.times(1, 1.0, bv, N, 0.0, &c, 1);
+	real_t err = c - (1.0/6.0)*N*(N+1)*(2*N+1);
+	fprintf(stdFile, "Dot product; Error in sum of first %d squares: %9.2e\n", N, err );
+	
+	aT.transTimes(1, 1.0, bv, N, 0.0, &c, 1);
+	real_t errT = c - (1.0/6.0)*N*(N+1)*(2*N+1);
+	fprintf(stdFile, "Transpose; Error in sum of first %d squares: %9.2e\n", N, errT);
+
+	delete[] bv;
+	aT.free ();	// or delete[] aTv;
+	a.free (); // or delete[] av;
+
+	QPOASES_TEST_FOR_TOL( err ,1e-10 )
+	QPOASES_TEST_FOR_TOL( errT,1e-10 )
+	
+	return TEST_PASSED;
+}
+
+
+/** Compare deviations when multiplying Hilbert matrix with its inverse. */
+int hilbert()
+{
+	int i, j;
+	real_t d, err;
+
+	/* permuted 4x4 Hilbert matrix, row major format */
+	real_t _Av[] = {1.0/3.0, 1.0, 0.5, 0.25,
+		0.25, 0.5, 1.0/3.0, 0.2,
+		0.2, 1.0/3.0, 0.25, 1.0/6.0,
+		1.0/6.0, 0.25, 0.2, 1.0/7.0};
+	/* and its inverse, column major format */
+	real_t Bv[] = {240, 16, -120, -140,
+		-2700, -120, 1200, 1680,
+		6480, 240, -2700, -4200,
+		-4200, -140, 1680, 2800};
+
+	/* result */
+	real_t *Av = new real_t[4*4];
+	real_t *Cv = new real_t[4*4];
+
+	DenseMatrix A(4, 4, 4, Av);
+
+	for (i = 0; i < 16; i++) Av[i] = _Av[i];
+
+	A.times(4, 1.0, Bv, 4, 0.0, Cv, 4);
+
+	err = 0.0;
+	for (j = 0; j < 4; j++)
+	{
+		for (i = 0; i < 4; i++)
+		{
+			d = getAbs(Cv[j*4+i] - static_cast<real_t>(i == j));
+			if (d > err) err = d;
+		}
+	}
+	fprintf(stdFile, "Hilbert; Deviation from identity: %9.2e\n", err);
+
+	delete[] Cv;
+	A.free ();	// or delete[] Av;
+
+	QPOASES_TEST_FOR_TOL( err,1e-12 )
+	
+	return TEST_PASSED;
+}
+
+
+/** Compare deviations when multiplying sub-matrices. */
+int submatrix()
+{
+	int i, j;
+	real_t d, err;
+
+	/* 2x3 transposed submatrix */
+	real_t _Asubv[] = {1.0/3.0, 0.25,
+		1.0, 0.5,
+		0.5, 1.0/3.0,
+		0.25, 0.2};
+	real_t Bsubv[] = {240, 16, -120, -140,
+		-2700, -120, 1200, 1680};
+	real_t Csubv[2*2];
+
+	real_t *Asubv = new real_t[2*4];
+
+	for (i = 0; i < 8; i++) Asubv[i] = _Asubv[i];
+
+	DenseMatrix Asub(4, 2, 2, Asubv);
+
+	Asub.transTimes(2, 1.0, Bsubv, 4, 0.0, Csubv, 2);
+
+	err = 0.0;
+	for (j = 0; j < 2; j++)
+	{
+		for (i = 0; i < 2; i++)
+		{
+			d = getAbs(Csubv[j*2+i] - static_cast<real_t>(i == j));
+			if (d > err) err = d;
+		}
+	}
+	fprintf(stdFile, "Submatrix transpose; Deviation from identity: %9.2e\n", err);
+	
+	Asub.free ();	// or delete[] Asubv;
+
+	QPOASES_TEST_FOR_TOL( err,1e-13 )
+
+	return TEST_PASSED;
+}
+
+
+/** Compare deviations when multiplying dense sub-matrices via index lists. */
+int indexDenseSubmatrix()
+{
+	/* dense submatrices via index lists */
+	const int M = 20, N = 15, K = 5;
+	int i, j, k, m, n;
+	Indexlist rows(N), cols(N);
+	int *rNum, *cNum;
+	real_t err=0.0, errT=0.0;
+	real_t *Av, *X, *Y, *x, *y, *xc, *yc;
+
+	// prepare index lists
+	m = (M-3)/4;
+	n = (N-3)/4;
+	for (i = 0; i < m; i++) { rows.addNumber(M-1 - 2*i); rows.addNumber(2*i); }
+	for (i = 0; i < n; i++) { cols.addNumber(N-1 - 2*i); cols.addNumber(2*i); }
+	m *= 2;
+	n *= 2;
+
+	rows.getNumberArray(&rNum);
+	fprintf(stdFile, "Rows: ");
+	for (i = 0; i < m; i++) fprintf(stdFile, " %2d", rNum[i]);
+	fprintf(stdFile, "\n");
+
+	cols.getNumberArray(&cNum);
+	fprintf(stdFile, "Cols: ");
+	for (i = 0; i < n; i++) fprintf(stdFile, " %2d", cNum[i]);
+	fprintf(stdFile, "\n");
+
+	// prepare input matrices
+	Av = new real_t[M*N];
+	X = new real_t[n*K];
+	Y = new real_t[m*K];
+	x = new real_t[n*K];
+	y = new real_t[m*K];
+	xc = new real_t[n*K];
+	yc = new real_t[m*K];
+
+	DenseMatrix A(M, N, N, Av);
+	for (i = 0; i < M*N; i++) Av[i] = -0.5*N*M + (real_t)i;
+	for (i = 0; i < n*K; i++) X[i] = 1.0 / (real_t)(i+1);
+	for (i = 0; i < m*K; i++) Y[i] = 1.0 / (real_t)(i+1);
+
+	// multiply
+	A.times(&rows, &cols, K, 1.0, X, n, 0.0, y, m);
+
+	// check result
+	for (j = 0; j < m; j++)
+	{
+		for (k = 0; k < K; k++)
+		{
+			yc[j+k*m] = -y[j+k*m];
+			for (i = 0; i < n; i++)
+				yc[j+k*m] += Av[cNum[i]+rNum[j]*N] * X[i+k*n];
+			if (getAbs(yc[j+k*m]) > err) err = getAbs(yc[j+k*m]);
+		}
+	}
+	fprintf(stdFile, "Indexlist submatrix; error: %9.2e\n", err);
+
+	// transpose multiply
+	A.transTimes(&rows, &cols, K, 1.0, Y, m, 0.0, x, n);
+
+	// check result
+	errT = 0.0;
+	for (j = 0; j < n; j++)
+	{
+		for (k = 0; k < K; k++)
+		{
+			xc[j+k*n] = -x[j+k*n];
+			for (i = 0; i < m; i++)
+				xc[j+k*n] += Av[cNum[j]+rNum[i]*N] * Y[i+k*m];
+			if (getAbs(xc[j+k*n]) > errT) errT = getAbs(xc[j+k*n]);
+		}
+	}
+	fprintf(stdFile, "Indexlist transpose submatrix; error: %9.2e\n", errT);
+
+	// check result
+
+	// clean up
+	delete[] yc;
+	delete[] xc;
+	delete[] y;
+	delete[] x;
+	delete[] Y;
+	delete[] X;
+	A.free ();	// or delete[] Av;
+
+	QPOASES_TEST_FOR_TOL( err ,1e-13 )
+	QPOASES_TEST_FOR_TOL( errT,1e-14 )
+
+	return TEST_PASSED;
+}
+
+
+/** Obtain column of sparse matrix. */
+int spGetCol()
+{
+	long j, i;
+	sparse_int_t *ir = new sparse_int_t[10];
+	sparse_int_t *jc = new sparse_int_t[4];
+	real_t *val = new real_t[10];
+	real_t *col = new real_t[4];
+
+	/* Test matrix:
+	 *
+	 *  [  0  3  6 ]
+	 *  [  1  0  7 ]
+	 *  [  2  0  8 ]
+	 *  [  0  4  9 ]
+	 *  [  0  5 10 ]
+	 */
+
+	jc[0] = 0; jc[1] = 2; jc[2] = 5; jc[3] = 10;
+	ir[0] = 1; ir[1] = 2;
+	ir[2] = 0; ir[3] = 3; ir[4] = 4;
+	ir[5] = 0; ir[6] = 1; ir[7] = 2; ir[8] = 3; ir[9] = 4;
+	for (i = 0; i < 10; i++) val[i] = 1.0+i;
+
+	SparseMatrix A(5, 3, ir, jc, val);
+
+	Indexlist rows(4);
+
+	rows.addNumber(2);
+	rows.addNumber(4);
+	rows.addNumber(3);
+	rows.addNumber(0);
+
+	/* Indexed matrix:
+	 *
+	 *  [  2  0  8 ]
+	 *  [  0  5 10 ]
+	 *  [  0  4  9 ]
+	 *  [  0  3  6 ]
+	 */
+
+	for (j = 0; j < 3; j++)
+	{
+		fprintf(stdFile, "Column %ld:\n", j);
+		A.getCol(j, &rows, 1.0, col);
+		for (i = 0; i < 4; i++)
+			fprintf(stdFile, " %3.0f\n", col[i]);
+	}
+				
+	delete[] col;
+	A.free ();	// or delete[] val,jc,ir;
+
+	return TEST_PASSED;
+}
+
+
+/** Obtain row of sparse matrix. */
+int spGetRow()
+{
+	long j, i;
+	sparse_int_t *ir = new sparse_int_t[10];
+	sparse_int_t *jc = new sparse_int_t[6];
+	real_t *val = new real_t[10];
+	real_t *row = new real_t[4];
+
+	/* Test matrix:
+	 *
+	 *  [  0  3  4  6  0 ]
+	 *  [  1  0  0  7  9 ]
+	 *  [  2  0  5  8 10 ]
+	 */
+
+	jc[0] = 0; jc[1] = 2; jc[2] = 3; jc[3] = 5; jc[4] = 8; jc[5] = 10;
+	ir[0] = 1; ir[1] = 2;
+	ir[2] = 0; 
+	ir[3] = 0; ir[4] = 2;
+	ir[5] = 0; ir[6] = 1; ir[7] = 2; 
+	ir[8] = 1; ir[9] = 2;
+	for (i = 0; i < 10; i++) val[i] = 1.0+i;
+
+	SparseMatrix A(3, 4, ir, jc, val);
+
+	Indexlist cols(4);
+
+	cols.addNumber(2);
+	cols.addNumber(4);
+	cols.addNumber(3);
+	cols.addNumber(1);
+
+	/* Indexed matrix:
+	 *
+	 * [  4  0  6  3 ]
+	 * [  0  9  7  0 ]
+	 * [  5 10  8  0 ]
+	 */
+
+	for (j = 0; j < 3; j++)
+	{
+		A.getRow(j, &cols, 1.0, row);
+		for (i = 0; i < 4; i++)
+			fprintf(stdFile, " %3.0f", row[i]);
+		fprintf(stdFile, "\n");
+	}
+				
+	delete[] row;
+	A.free ();
+
+	return TEST_PASSED;
+}
+
+
+/** Compare deviations when multiplying sparse matrix. */
+int spTimes()
+{
+	long i;
+	sparse_int_t *ir = new sparse_int_t[10];
+	sparse_int_t *jc = new sparse_int_t[6];
+	real_t *val = new real_t[10];
+
+	real_t *x = new real_t[5*2];
+	real_t *y = new real_t[3*2];
+
+	real_t Ax[] = {-23, -11, -26, 42, 74, 99};
+	real_t ATy[] = {-63, -69, -222, -423, -359, 272, 126, 663, 1562, 1656};
+	real_t err=0.0, errT=0.0;
+
+	for (i = 0; i < 10; i++) x[i] = -4.0+i;
+
+	/* Test matrix:
+	 *
+	 *  [  0  3  4  6  0 ]
+	 *  [  1  0  0  7  9 ]
+	 *  [  2  0  5  8 10 ]
+	 */
+
+	jc[0] = 0; jc[1] = 2; jc[2] = 3; jc[3] = 5; jc[4] = 8; jc[5] = 10;
+	ir[0] = 1; ir[1] = 2;
+	ir[2] = 0; 
+	ir[3] = 0; ir[4] = 2;
+	ir[5] = 0; ir[6] = 1; ir[7] = 2; 
+	ir[8] = 1; ir[9] = 2;
+	for (i = 0; i < 10; i++) val[i] = 1.0+i;
+
+	SparseMatrix A(3, 5, ir, jc, val);	// reference to ir, jc, val
+
+	A.times(2, 1.0, x, 5, 0.0, y, 3);
+	
+	for (i = 0; i < 6; i++)
+		if (getAbs(y[i] - Ax[i]) > err) err = getAbs(y[i] - Ax[i]);
+	fprintf(stdFile, "Error in sparse A*x: %9.2e\n", err);
+
+	A.transTimes(2, 1.0, y, 3, 0.0, x, 5);
+
+	errT = 0.0;
+	for (i = 0; i < 10; i++)
+		if (getAbs(x[i] - ATy[i]) > errT) errT = getAbs(x[i] - ATy[i]);
+	fprintf(stdFile, "Error in sparse A'*x: %9.2e\n", errT);
+
+	A.free ();	// or delete[] val,ir,jc
+	delete[] y;
+	delete[] x;
+
+	QPOASES_TEST_FOR_TOL( err ,1e-15 )
+	QPOASES_TEST_FOR_TOL( errT,1e-15 )
+
+	return TEST_PASSED;
+}
+
+
+/** Compare deviations when multiplying sparse matrix via index lists. */
+int spIndTimes()
+{
+	const long N = 4;
+	long i, j;
+	long nRows = 2 * N + 1;
+	long nCols = N;
+	long nnz = 3 * N;
+	sparse_int_t *ir = new sparse_int_t[nnz];
+	sparse_int_t *jc = new sparse_int_t[nCols+1];
+	real_t *val = new real_t[nnz];
+	real_t *xc = new real_t[3*2];
+	real_t *yc = new real_t[4*2];
+	real_t Ax[] = {0.31, 0.05, 0.06, 0.30, 0.76, 0.20, 0.24, 0.60};
+	real_t ATy[] = {0.278, 0.000, 0.548, 0.776, 0.000, 1.208};
+	real_t err=0.0, errT=0.0;
+
+	Indexlist rows(4), cols(3), allcols(nCols);
+
+	rows.addNumber(2);
+	rows.addNumber(4);
+	rows.addNumber(3);
+	rows.addNumber(0);
+
+	cols.addNumber(1);
+	cols.addNumber(3);
+	cols.addNumber(0);
+
+	for (i = 0; i < nCols; i++) 
+		allcols.addNumber(i);
+
+	// build test matrix
+	for (i = 0; i <= N; i++) jc[i] = 3*i;
+	for (j = 0; j < N; j++) 
+		for (i = 0; i < 3; i++)
+		{
+			ir[j*3+i] = 2*j + i;
+			val[j*3+i] = 1.0 - 0.1 * (j*3+i);
+		}
+	SparseMatrix A(nRows, nCols, ir, jc, val);
+
+	fprintf(stdFile, "Test matrix A =\n");
+	for (j = 0; j < nRows; j++)
+	{
+		A.getRow(j, &allcols, 1.0, xc);
+		for (i = 0; i < nCols; i++)
+			fprintf(stdFile, "%6.2f", xc[i]);
+		fprintf(stdFile, "\n");
+	}
+
+	for (i = 0; i < 6; i++)
+		xc[i] = (1.0 + i) * 0.1;
+
+	A.times(&rows, &cols, 2, 1.0, xc, 3, 0.0, yc, 4, BT_TRUE);
+
+	for (i = 0; i < 8; i++)
+		if (getAbs(yc[i] - Ax[i]) > err)
+			err = getAbs(yc[i] - Ax[i]);
+	fprintf(stdFile, "Error in sparse indexed A*x: %9.2e\n", err);
+
+	A.transTimes(&rows, &cols, 2, 1.0, yc, 4, 0.0, xc, 3);
+	errT = 0.0;
+	for (i = 0; i < 6; i++)
+		if (getAbs(xc[i] - ATy[i]) > errT)
+			errT = getAbs(xc[i] - ATy[i]);
+	fprintf(stdFile, "Error in sparse indexed A'*y: %9.2e\n", errT);
+
+	delete[] xc;
+	delete[] yc;
+	A.free ();
+
+	QPOASES_TEST_FOR_TOL( err ,1e-15 )
+	QPOASES_TEST_FOR_TOL( errT,1e-15 )
+
+	return TEST_PASSED;
+}
+
+
+/** Obtain column of sparse row matrix. */
+int sprGetCol()
+{
+	long j, i;
+	sparse_int_t *ir = new sparse_int_t[10];
+	sparse_int_t *jc = new sparse_int_t[4];
+	real_t *val = new real_t[10];
+	real_t *col = new real_t[4];
+
+	/* Test matrix:
+	 *
+	 *  [  0  3  6 ]
+	 *  [  1  0  7 ]
+	 *  [  2  0  8 ]
+	 *  [  0  4  9 ]
+	 *  [  0  5 10 ]
+	 */
+
+	jc[0] = 0; jc[1] = 2; jc[2] = 5; jc[3] = 10;
+	ir[0] = 1; ir[1] = 2;
+	ir[2] = 0; ir[3] = 3; ir[4] = 4;
+	ir[5] = 0; ir[6] = 1; ir[7] = 2; ir[8] = 3; ir[9] = 4;
+	for (i = 0; i < 10; i++) val[i] = 1.0+i;
+
+	SparseMatrix Ac(5, 3, ir, jc, val);
+	real_t *Acv = Ac.full(); // row major format
+	SparseMatrixRow A(5, 3, 3, Acv);
+	delete[] Acv;
+	Ac.free ();	// or delete[] val,jc,ir;
+
+	Indexlist rows(4);
+
+	rows.addNumber(2);
+	rows.addNumber(4);
+	rows.addNumber(3);
+	rows.addNumber(0);
+
+	/* Indexed matrix:
+	 *
+	 *  [  2  0  8 ]
+	 *  [  0  5 10 ]
+	 *  [  0  4  9 ]
+	 *  [  0  3  6 ]
+	 */
+
+	for (j = 0; j < 3; j++)
+	{
+		fprintf(stdFile, "Column %ld:\n", j);
+		A.getCol(j, &rows, 1.0, col);
+		for (i = 0; i < 4; i++)
+			fprintf(stdFile, " %3.0f\n", col[i]);
+	}
+				
+	delete[] col;
+	A.free ();	// or delete[] val,jc,ir;
+
+	return TEST_PASSED;
+}
+
+
+/** Obtain row of sparse row matrix. */
+int sprGetRow()
+{
+	long j, i;
+	sparse_int_t *ir = new sparse_int_t[10];
+	sparse_int_t *jc = new sparse_int_t[6];
+	real_t *val = new real_t[10];
+	real_t *row = new real_t[4];
+
+	/* Test matrix:
+	 *
+	 *  [  0  3  4  6  0 ]
+	 *  [  1  0  0  7  9 ]
+	 *  [  2  0  5  8 10 ]
+	 */
+
+	jc[0] = 0; jc[1] = 2; jc[2] = 3; jc[3] = 5; jc[4] = 8; jc[5] = 10;
+	ir[0] = 1; ir[1] = 2;
+	ir[2] = 0; 
+	ir[3] = 0; ir[4] = 2;
+	ir[5] = 0; ir[6] = 1; ir[7] = 2; 
+	ir[8] = 1; ir[9] = 2;
+	for (i = 0; i < 10; i++) val[i] = 1.0+i;
+
+	SparseMatrix Ac(3, 5, ir, jc, val);
+	real_t *Acv = Ac.full(); // row major format
+	SparseMatrixRow A(3, 5, 5, Acv);
+	delete[] Acv;
+	Ac.free ();	// or delete[] val,jc,ir;
+
+	Indexlist cols(4);
+
+	cols.addNumber(2);
+	cols.addNumber(4);
+	cols.addNumber(3);
+	cols.addNumber(1);
+
+	/* Indexed matrix:
+	 *
+	 * [  4  0  6  3 ]
+	 * [  0  9  7  0 ]
+	 * [  5 10  8  0 ]
+	 */
+
+	for (j = 0; j < 3; j++)
+	{
+		A.getRow(j, &cols, 1.0, row);
+		for (i = 0; i < 4; i++)
+			fprintf(stdFile, " %3.0f", row[i]);
+		fprintf(stdFile, "\n");
+	}
+				
+	delete[] row;
+	A.free ();
+
+	return TEST_PASSED;
+}
+
+
+/** Compare deviations when multiplying sparse row matrix. */
+int sprTimes()
+{
+	long i;
+	sparse_int_t *ir = new sparse_int_t[10];
+	sparse_int_t *jc = new sparse_int_t[6];
+	real_t *val = new real_t[10];
+
+	real_t *x = new real_t[5*2];
+	real_t *y = new real_t[3*2];
+
+	real_t Ax[] = {-23, -11, -26, 42, 74, 99};
+	real_t ATy[] = {-63, -69, -222, -423, -359, 272, 126, 663, 1562, 1656};
+	real_t err=0.0, errT=0.0;
+
+	for (i = 0; i < 10; i++) x[i] = -4.0+i;
+
+	/* Test matrix:
+	 *
+	 *  [  0  3  4  6  0 ]
+	 *  [  1  0  0  7  9 ]
+	 *  [  2  0  5  8 10 ]
+	 */
+
+	jc[0] = 0; jc[1] = 2; jc[2] = 3; jc[3] = 5; jc[4] = 8; jc[5] = 10;
+	ir[0] = 1; ir[1] = 2;
+	ir[2] = 0; 
+	ir[3] = 0; ir[4] = 2;
+	ir[5] = 0; ir[6] = 1; ir[7] = 2; 
+	ir[8] = 1; ir[9] = 2;
+	for (i = 0; i < 10; i++) val[i] = 1.0+i;
+
+	SparseMatrix Ac(3, 5, ir, jc, val);	// reference to ir, jc, val
+	real_t *Acv = Ac.full(); // row major format
+	SparseMatrixRow A(3, 5, 5, Acv);
+	delete[] Acv;
+	Ac.free ();	// or delete[] val,jc,ir;
+
+	A.times(2, 1.0, x, 5, 0.0, y, 3);
+	
+	for (i = 0; i < 6; i++)
+		if (getAbs(y[i] - Ax[i]) > err) err = getAbs(y[i] - Ax[i]);
+	fprintf(stdFile, "Error in sparse A*x: %9.2e\n", err);
+
+	A.transTimes(2, 1.0, y, 3, 0.0, x, 5);
+
+	errT = 0.0;
+	for (i = 0; i < 10; i++)
+		if (getAbs(x[i] - ATy[i]) > errT) errT = getAbs(x[i] - ATy[i]);
+	fprintf(stdFile, "Error in sparse A'*x: %9.2e\n", errT);
+
+	A.free ();	// or delete[] val,ir,jc
+	delete[] y;
+	delete[] x;
+
+	QPOASES_TEST_FOR_TOL( err ,1e-15 )
+	QPOASES_TEST_FOR_TOL( errT,1e-15 )
+
+	return TEST_PASSED;
+}
+
+
+/** Compare deviations when multiplying sparse row matrix via index lists. */
+int sprIndTimes()
+{
+	const long N = 4;
+	long i, j;
+	long nRows = 2 * N + 1;
+	long nCols = N;
+	long nnz = 3 * N;
+	sparse_int_t *ir = new sparse_int_t[nnz];
+	sparse_int_t *jc = new sparse_int_t[nCols+1];
+	real_t *val = new real_t[nnz];
+	real_t *xc = new real_t[3*2];
+	real_t *yc = new real_t[4*2];
+	real_t Ax[] = {0.31, 0.05, 0.06, 0.30, 0.76, 0.20, 0.24, 0.60};
+	real_t ATy[] = {0.278, 0.000, 0.548, 0.776, 0.000, 1.208};
+	real_t err=0.0, errT=0.0;
+
+	Indexlist rows(4), cols(3), allcols(nCols);
+
+	rows.addNumber(2);
+	rows.addNumber(4);
+	rows.addNumber(3);
+	rows.addNumber(0);
+
+	cols.addNumber(1);
+	cols.addNumber(3);
+	cols.addNumber(0);
+
+	for (i = 0; i < nCols; i++) 
+		allcols.addNumber(i);
+
+	// build test matrix
+	for (i = 0; i <= N; i++) jc[i] = 3*i;
+	for (j = 0; j < N; j++) 
+		for (i = 0; i < 3; i++)
+		{
+			ir[j*3+i] = 2*j + i;
+			val[j*3+i] = 1.0 - 0.1 * (j*3+i);
+		}
+	SparseMatrix Ac(nRows, nCols, ir, jc, val);
+	real_t *Acv = Ac.full(); // row major format
+	SparseMatrixRow A(nRows, nCols, nCols, Acv);
+	delete[] Acv;
+	Ac.free ();	// or delete[] val,jc,ir;
+
+	fprintf(stdFile, "Test matrix A =\n");
+	for (j = 0; j < nRows; j++)
+	{
+		A.getRow(j, &allcols, 1.0, xc);
+		for (i = 0; i < nCols; i++)
+			fprintf(stdFile, "%6.2f", xc[i]);
+		fprintf(stdFile, "\n");
+	}
+
+	for (i = 0; i < 6; i++)
+		xc[i] = (1.0 + i) * 0.1;
+
+	A.times(&rows, &cols, 2, 1.0, xc, 3, 0.0, yc, 4, BT_TRUE);
+
+	for (i = 0; i < 8; i++)
+		if (getAbs(yc[i] - Ax[i]) > err)
+			err = getAbs(yc[i] - Ax[i]);
+	fprintf(stdFile, "Error in sparse indexed A*x: %9.2e\n", err);
+
+	A.transTimes(&rows, &cols, 2, 1.0, yc, 4, 0.0, xc, 3);
+	for (i = 0; i < 3; i++)
+	{
+		for (j = 0; j < 2; j++)
+			fprintf(stdFile, "%6.2f", ATy[i + j*3]);
+		fprintf(stdFile, "\n");
+	}
+	for (i = 0; i < 3; i++)
+	{
+		for (j = 0; j < 2; j++)
+			fprintf(stdFile, "%6.2f", xc[i + j*3]);
+		fprintf(stdFile, "\n");
+	}
+	for (i = 0; i < 6; i++)
+		if (getAbs(xc[i] - ATy[i]) > errT)
+			errT = getAbs(xc[i] - ATy[i]);
+	fprintf(stdFile, "Error in sparse indexed A'*y: %9.2e\n", errT);
+
+	delete[] xc;
+	delete[] yc;
+	A.free ();
+
+	QPOASES_TEST_FOR_TOL( err ,1e-15 )
+	QPOASES_TEST_FOR_TOL( errT,1e-15 )
+
+	return TEST_PASSED;
+}
+
+
+/** Compare deviations when using bilinear multiplication with dense matrix. */
+int symmetry()
+{
+	int i,j;
+	real_t *Hv = new real_t[6*6];
+	real_t *Z = new real_t[6*3];
+	real_t *ZHZd = new real_t[3*3];
+	real_t *ZHZs = new real_t[3*3];
+	real_t ZHZv[] = {0.144, 0.426, 0.708, 0.426, 1.500, 2.574, 0.708, 2.574, 4.440};
+	real_t err=0.0, errS=0.0;
+	SymDenseMat *Hd;
+	SymSparseMat *Hs;
+	Indexlist *cols = new Indexlist(6);
+
+	for (i = 0; i < 36; i++) Hv[i] = 0.0;
+	for (i = 0; i < 6; i++) Hv[i*7] = 1.0 - 0.1 * i;
+	for (i = 0; i < 5; i++) Hv[i*7+1] = Hv[i*7+6] = -0.1 * (i+1);
+
+	Hd = new SymDenseMat(6, 6, 6, Hv);	// deep-copy from Hv
+	Hs = new SymSparseMat(6, 6, 6, Hv);	// deep-copy from Hv
+	Hs->createDiagInfo();
+
+	for (i = 0; i < 6; ++i)
+	{
+		for (j = 0; j < 6; ++j)
+			fprintf (stdFile, "%3.3f ", Hv[i*6+j]);
+		fprintf (stdFile, "\n");
+	}
+	fprintf (stdFile, "\n");
+	
+	cols->addNumber(3);
+	cols->addNumber(0);
+	cols->addNumber(4);
+	cols->addNumber(1);
+	for (i = 0; i < 18; i++) Z[i] = 0.1 * (i+1);
+
+	fprintf (stdFile, "\n");
+	for (i = 0; i < 6; ++i)
+	{
+		for (j = 0; j < 3; ++j)
+			fprintf (stdFile, "%3.3f ", Z[i+j*6]);
+		fprintf (stdFile, "\n");
+	}
+	fprintf (stdFile, "\n");
+	
+	Hd->bilinear(cols, 3, Z, 6, ZHZd, 3);
+
+	for (i = 0; i < 9; i++)
+		if (getAbs(ZHZd[i] - ZHZv[i]) > err)
+			err = getAbs(ZHZd[i] - ZHZv[i]);
+	fprintf(stdFile, "Error in indexed dense bilinear form: %9.2e\n", err);
+
+	Hs->bilinear(cols, 3, Z, 6, ZHZs, 3);
+	
+	for (i = 0; i < 3; ++i)
+	{
+		for (j = 0; j < 3; ++j)
+			fprintf (stdFile, "%3.3f ", ZHZd[i*3+j]);
+		fprintf (stdFile, "\n");
+	}
+	fprintf (stdFile, "\n");
+
+	for (i = 0; i < 3; ++i)
+	{
+		for (j = 0; j < 3; ++j)
+			fprintf (stdFile, "%3.3f ", ZHZv[i*3+j]);
+		fprintf (stdFile, "\n");
+	}
+	fprintf (stdFile, "\n");
+
+	for (i = 0; i < 9; i++)
+		if (getAbs(ZHZs[i] - ZHZv[i]) > errS)
+			errS = getAbs(ZHZs[i] - ZHZv[i]);
+	fprintf(stdFile, "Error in indexed sparse bilinear form: %9.2e\n", errS);
+
+	delete cols;
+	delete Hs;
+	delete Hd;
+	delete[] ZHZs;
+	delete[] ZHZd;
+	delete[] Z;
+	delete[] Hv;
+
+	QPOASES_TEST_FOR_TOL( err ,1e-15 )
+	QPOASES_TEST_FOR_TOL( errS,1e-15 )
+
+	return TEST_PASSED;
+}
+
+
+/** Run various tests on matrix classes. */
+int main()
+{
+	int errorCount = TEST_PASSED;
+	
+	errorCount += sumOfSquares();
+	errorCount += hilbert();
+	errorCount += submatrix();
+	errorCount += indexDenseSubmatrix();
+
+	errorCount += spGetCol();
+	errorCount += spGetRow();
+	errorCount += spTimes();
+	errorCount += spIndTimes();
+
+	errorCount += sprGetCol();
+	errorCount += sprGetRow();
+	errorCount += sprTimes();
+	errorCount += sprIndTimes();
+
+	errorCount += symmetry();
+	
+	return errorCount;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices2.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices2.cpp
new file mode 100644
index 00000000000..7deed6925e3
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices2.cpp
@@ -0,0 +1,113 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_matrices2.cpp
+ *	\author Hans Joachim Ferreau,Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2014
+ *
+ *	Unit test for Matrix classes.
+ */
+
+
+#include <stdlib.h>
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+#include "test_qrecipe_data.hpp"
+
+
+/** Compare deviations when performing matrix operations. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+	
+	int i;
+
+	real_t errH=0.0, errA=0.0;
+	real_t v[180];
+	real_t resHs[180];
+	real_t resHd[180];
+	real_t resAs[91];
+	real_t resAd[91];
+	
+	/* create sparse matrices */
+	SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val);
+	SparseMatrix *A = new SparseMatrix(91, 180, A_ir, A_jc, A_val);
+
+	H->createDiagInfo();
+
+	real_t* H_full = H->full();
+	real_t* A_full = A->full();
+
+	//print( A_full,91,180 );
+
+
+	SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full);
+	DenseMatrix *Ad = new DenseMatrix(91,180,180,A_full);
+
+	for( i=0; i<180; ++i )
+		v[i] = 2.0 * ((real_t)rand()) / ((real_t)RAND_MAX) - 1.0;
+	
+	H ->times(1, 1.0, v, 180, 0.0, resHs, 180);
+	Hd->times(1, 1.0, v, 180, 0.0, resHd, 180);
+	
+	A ->times(1, 1.0, v, 180, 0.0, resAs, 91);
+	Ad->times(1, 1.0, v, 180, 0.0, resAd, 91);
+
+	
+	for ( i=0; i<180; ++i )
+		if ( getAbs(resHs[i] - resHd[i]) > errH)
+			errH = getAbs(resHs[i] - resHd[i]);
+	
+	fprintf(stdFile, "maximum difference in H*v: %9.2e\n", errH);
+	
+	
+	for ( i=0; i<91; ++i )
+		if ( getAbs(resAs[i] - resAd[i]) > errA)
+			errA = getAbs(resAs[i] - resAd[i]);
+	
+	fprintf(stdFile, "maximum difference in A*v: %9.2e\n", errA);
+
+	delete H;
+	delete A;
+	delete[] H_full;
+	delete[] A_full;
+	delete Hd;
+	delete Ad;
+
+	
+	QPOASES_TEST_FOR_TOL( errH,1e-13 )
+	QPOASES_TEST_FOR_TOL( errA,1e-13 )
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices3.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices3.cpp
new file mode 100644
index 00000000000..88822d6e607
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices3.cpp
@@ -0,0 +1,90 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_matrices3.cpp
+ *	\author Hans Joachim Ferreau,Andreas Potschka, Christian Kirches
+ *	\version 3.0
+ *	\date 2014
+ *
+ *	Unit test for Matrix classes.
+ */
+
+
+#include <stdlib.h>
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+#include "test_qrecipe_data.hpp"
+
+
+/** Compare deviations when performing matrix operations. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+	
+	int i;
+
+	real_t errH=0.0;
+	real_t v[180];
+	real_t resHn[180];
+	real_t resHt[180];
+	
+	/* create sparse matrices */
+	SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val);
+	
+	H->createDiagInfo();
+
+	real_t* H_full = H->full();
+	
+	SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full);
+
+	for( i=0; i<180; ++i )
+		v[i] = 2.0 * ((real_t)rand()) / ((real_t)RAND_MAX) - 1.0;
+	
+	Hd->times(     1, 1.0, v, 180, 0.0, resHn, 180);
+	Hd->transTimes(1, 1.0, v, 180, 0.0, resHt, 180);
+	
+	for ( i=0; i<180; ++i )
+		if ( getAbs(resHn[i] - resHt[i]) > errH)
+			errH = getAbs(resHn[i] - resHt[i]);
+	
+	fprintf(stdFile, "maximum difference in H*v vs. H'*v: %9.2e\n", errH);
+	
+	delete H;
+	delete[] H_full;
+	delete Hd;
+	
+	
+	QPOASES_TEST_FOR_TOL( errH,1e-15 )
+	
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_qrecipe.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_qrecipe.cpp
new file mode 100644
index 00000000000..b96406ac4bc
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_qrecipe.cpp
@@ -0,0 +1,149 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_qrecipe.cpp
+ *	\author Andreas Potschka
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	QRECIPE example from the CUTEr test set with sparse matrices.
+ */
+
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+#include "test_qrecipe_data.hpp"
+
+
+
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	long i;
+	int nWSR;
+	real_t tic, toc;
+	real_t errP=0.0, errD=0.0;
+	real_t *x1 = new real_t[180];
+	real_t *y1 = new real_t[271];
+	real_t *x2 = new real_t[180];
+	real_t *y2 = new real_t[271];
+
+	/* create sparse matrices */
+	SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val);
+	SparseMatrix *A = new SparseMatrix(91, 180, A_ir, A_jc, A_val);
+
+	H->createDiagInfo();
+
+	real_t* H_full = H->full();
+	real_t* A_full = A->full();
+
+	SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full);
+	DenseMatrix *Ad = new DenseMatrix(91,180,180,A_full);
+
+	/* solve with dense matrices */
+	nWSR = 1000;
+	QProblem qrecipeD(180, 91);
+	tic = getCPUtime();
+	qrecipeD.init(Hd, g, Ad, lb, ub, lbA, ubA, nWSR, 0);
+	toc = getCPUtime();
+	qrecipeD.getPrimalSolution(x1);
+	qrecipeD.getDualSolution(y1);
+
+	fprintf(stdFile, "Solved dense problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic);
+
+	/* Compute KKT tolerances */
+	real_t stat, feas, cmpl;
+
+	getKKTResidual(	180,91,
+					H_full,g,A_full,lb,ub,lbA,ubA,
+					x1,y1,
+					stat,feas,cmpl
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n\n", stat,feas,cmpl );
+
+
+	/* solve with sparse matrices */
+	nWSR = 1000;
+	QProblem qrecipeS(180, 91);
+	tic = getCPUtime();
+	qrecipeS.init(H, g, A, lb, ub, lbA, ubA, nWSR, 0);
+	toc = getCPUtime();
+	qrecipeS.getPrimalSolution(x2);
+	qrecipeS.getDualSolution(y2);
+
+	fprintf(stdFile, "Solved sparse problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic);
+	
+	/* Compute KKT tolerances */
+	real_t stat2, feas2, cmpl2;
+	getKKTResidual(	180,91,
+					H_full,g,A_full,lb,ub,lbA,ubA,
+					x2,y2,
+					stat2,feas2,cmpl2
+					);
+	printf( "stat = %e\nfeas = %e\ncmpl = %e\n\n", stat2,feas2,cmpl2 );
+
+	/* check distance of solutions */
+	for (i = 0; i < 180; i++)
+		if (getAbs(x1[i] - x2[i]) > errP)
+			errP = getAbs(x1[i] - x2[i]);
+	fprintf(stdFile, "Primal error: %9.2e\n", errP);
+
+	for (i = 0; i < 271; i++)
+		if (getAbs(y1[i] - y2[i]) > errD)
+			errD = getAbs(y1[i] - y2[i]);
+	fprintf(stdFile, "Dual error: %9.2e (might not be unique)\n", errD);
+
+	delete H;
+	delete A;
+	delete[] H_full;
+	delete[] A_full;
+	delete Hd;
+	delete Ad;
+
+	delete[] y2;
+	delete[] x2;
+	delete[] y1;
+	delete[] x1;
+
+	QPOASES_TEST_FOR_TOL( stat,1e-14 );
+	QPOASES_TEST_FOR_TOL( feas,1e-14 );
+	QPOASES_TEST_FOR_TOL( cmpl,1e-13 );
+	
+	QPOASES_TEST_FOR_TOL( stat2,1e-14 );
+	QPOASES_TEST_FOR_TOL( feas2,1e-14 );
+	QPOASES_TEST_FOR_TOL( cmpl2,1e-13 );
+
+	QPOASES_TEST_FOR_TOL( errP,1e-13 );
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_qrecipe_data.hpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_qrecipe_data.hpp
new file mode 100644
index 00000000000..d4c6342dcdf
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_qrecipe_data.hpp
@@ -0,0 +1,401 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_qrecipe.cpp
+ *	\author Andreas Potschka
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	QRECIPE example from the CUTEr test set with sparse matrices.
+ */
+
+
+USING_NAMESPACE_QPOASES
+
+
+
+const real_t Inf = INFTY;
+
+sparse_int_t H_jc[] = { 0,  4,  8, 12, 16, 20, 20, 20, 20, 20, 20,
+	                   24, 28, 32, 36, 40, 40, 40, 40, 40, 40,
+					   44, 48, 52, 56, 60, 60, 60, 60, 60, 60, 60, 60, 60, 60,
+					   64, 68, 72, 76, 80, 80, 80, 80, 80, 80,
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
+					   80, 80, 80, 80, 80, 80 };
+
+sparse_int_t H_ir[] = {
+	0, 10, 20, 34, 1, 11, 21, 35, 2, 12, 22, 36, 3, 13, 23, 37, 4, 14, 24, 38, 
+	0, 10, 20, 34, 1, 11, 21, 35, 2, 12, 22, 36, 3, 13, 23, 37, 4, 14, 24, 38, 
+	0, 10, 20, 34, 1, 11, 21, 35, 2, 12, 22, 36, 3, 13, 23, 37, 4, 14, 24, 38, 
+	0, 10, 20, 34, 1, 11, 21, 35, 2, 12, 22, 36, 3, 13, 23, 37, 4, 14, 24, 38};
+
+real_t H_val[] = {10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1,
+	1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 1,
+	10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 1, 10, 1,
+	1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10};
+
+sparse_int_t A_jc[] = {
+	  0,  10,  20,  30,  40,  50,  60,  70,  80,  90, 100, 110, 120,
+	130, 140, 150, 160, 170, 180, 190, 200, 210, 220, 230, 240, 250, 260, 270,
+	280, 290, 300, 301, 302, 303, 304, 305, 306, 307, 308, 309, 310, 311, 312,
+	313, 314, 318, 319, 320, 321, 322, 323, 324, 325, 326, 327, 328, 329, 331,
+	333, 335, 337, 339, 341, 343, 345, 347, 349, 351, 353, 355, 357, 359, 361,
+	363, 365, 367, 369, 371, 373, 383, 393, 403, 405, 408, 410, 413, 415, 418,
+	420, 422, 424, 426, 428, 430, 432, 434, 436, 438, 440, 442, 444, 446, 448,
+	450, 452, 454, 456, 458, 460, 462, 472, 482, 492, 494, 497, 499, 502, 504,
+	507, 509, 511, 513, 515, 517, 519, 521, 523, 525, 527, 529, 531, 533, 535,
+	537, 539, 541, 543, 545, 547, 549, 551, 561, 571, 581, 583, 586, 588, 591,
+	593, 596, 597, 598, 599, 600, 601, 602, 603, 604, 605, 606, 607, 608, 609,
+	610, 611, 612, 613, 614, 615, 616, 617, 618, 628, 638, 648, 650, 653, 655,
+	658, 660, 663};
+
+sparse_int_t A_ir[] = {0, 14, 35, 36, 71, 72, 85, 86, 87, 88, 1, 14, 35, 36, 71, 72, 85,
+	86, 87, 88, 2, 14, 35, 36, 71, 72, 85, 86, 87, 88, 3, 14, 35, 36, 71, 72,
+	85, 86, 87, 88, 4, 14, 35, 36, 71, 72, 85, 86, 87, 88, 5, 14, 35, 36, 71,
+	72, 85, 86, 87, 88, 6, 14, 35, 36, 71, 72, 85, 86, 87, 88, 7, 14, 35, 36,
+	71, 72, 85, 86, 87, 88, 8, 14, 35, 36, 71, 72, 85, 86, 87, 88, 9, 14, 35,
+	36, 71, 72, 85, 86, 87, 88, 0, 15, 37, 38, 69, 70, 79, 80, 81, 82, 1, 15,
+	37, 38, 69, 70, 79, 80, 81, 82, 2, 15, 37, 38, 69, 70, 79, 80, 81, 82, 3,
+	15, 37, 38, 69, 70, 79, 80, 81, 82, 4, 15, 37, 38, 69, 70, 79, 80, 81, 82,
+	5, 15, 37, 38, 69, 70, 79, 80, 81, 82, 6, 15, 37, 38, 69, 70, 79, 80, 81,
+	82, 7, 15, 37, 38, 69, 70, 79, 80, 81, 82, 8, 15, 37, 38, 69, 70, 79, 80,
+	81, 82, 9, 15, 37, 38, 69, 70, 79, 80, 81, 82, 0, 16, 39, 40, 67, 68, 73,
+	74, 75, 76, 1, 16, 39, 40, 67, 68, 73, 74, 75, 76, 2, 16, 39, 40, 67, 68,
+	73, 74, 75, 76, 3, 16, 39, 40, 67, 68, 73, 74, 75, 76, 4, 16, 39, 40, 67,
+	68, 73, 74, 75, 76, 5, 16, 39, 40, 67, 68, 73, 74, 75, 76, 6, 16, 39, 40,
+	67, 68, 73, 74, 75, 76, 7, 16, 39, 40, 67, 68, 73, 74, 75, 76, 8, 16, 39,
+	40, 67, 68, 73, 74, 75, 76, 9, 16, 39, 40, 67, 68, 73, 74, 75, 76, 10, 11,
+	12, 13, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 47, 48, 49, 50, 51,
+	52, 53, 54, 55, 56, 57, 47, 58, 48, 59, 49, 60, 50, 61, 51, 62, 52, 63, 53,
+	64, 54, 65, 55, 66, 46, 56, 45, 57, 47, 58, 48, 59, 49, 60, 50, 61, 51, 62,
+	52, 63, 53, 64, 54, 65, 55, 66, 46, 56, 45, 57, 10, 14, 71, 72, 85, 86, 87,
+	88, 89, 90, 11, 15, 69, 70, 79, 80, 81, 82, 83, 84, 12, 16, 67, 68, 73, 74,
+	75, 76, 77, 78, 35, 90, 36, 89, 90, 37, 84, 38, 83, 84, 39, 78, 40, 77, 78,
+	44, 58, 43, 59, 42, 60, 41, 61, 34, 62, 33, 63, 32, 64, 31, 65, 30, 66, 29,
+	46, 28, 45, 44, 58, 43, 59, 42, 60, 41, 61, 34, 62, 33, 63, 32, 64, 31, 65,
+	30, 66, 29, 46, 28, 45, 10, 14, 71, 72, 85, 86, 87, 88, 89, 90, 11, 15, 69,
+	70, 79, 80, 81, 82, 83, 84, 12, 16, 67, 68, 73, 74, 75, 76, 77, 78, 35, 90,
+	36, 89, 90, 37, 84, 38, 83, 84, 39, 78, 40, 77, 78, 27, 44, 26, 43, 25, 42,
+	24, 41, 23, 34, 22, 33, 21, 32, 20, 31, 19, 30, 18, 29, 17, 28, 27, 44, 26,
+	43, 25, 42, 24, 41, 23, 34, 22, 33, 21, 32, 20, 31, 19, 30, 18, 29, 17, 28,
+	10, 14, 71, 72, 85, 86, 87, 88, 89, 90, 11, 15, 69, 70, 79, 80, 81, 82, 83,
+	84, 12, 16, 67, 68, 73, 74, 75, 76, 77, 78, 35, 90, 36, 89, 90, 37, 84, 38,
+	83, 84, 39, 78, 40, 77, 78, 27, 26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 27,
+	26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 10, 14, 71, 72, 85, 86, 87, 88, 89,
+	90, 11, 15, 69, 70, 79, 80, 81, 82, 83, 84, 12, 16, 67, 68, 73, 74, 75, 76,
+	77, 78, 35, 90, 36, 89, 90, 37, 84, 38, 83, 84, 39, 78, 40, 77, 78};
+
+real_t A_val[] = {
+-1.0000000000000000e+00,  1.0000000000000000e+00,  8.8678200000000004e+01,
+ 9.3617050000000006e+01,  1.6000000000000000e+01,  8.1999999999999993e+00,
+ 9.9000000000000000e+01,  8.0000000000000000e+01,  1.2000000000000000e+01,
+ 9.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 8.0062830000000005e+01,  9.9224010000000007e+01,  1.0000000000000000e+02,
+ 2.1100000000000001e+01,  1.0000000000000000e+02,  1.0000000000000000e+02,
+ 1.1400000000000000e+02,  1.1680000000000000e+02, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  7.4697360000000003e+01,  8.3801220000000001e+01,
+-8.1999999999999993e+00,  2.0000000000000000e+00,  9.0000000000000000e+01,
+ 2.3999999999999999e+00, -1.2000000000000000e+01, -1.4800000000000001e+01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  7.9194209999999998e+01,
+ 9.0175110000000004e+01,  4.3000000000000000e+01,  8.0000000000000000e+00,
+ 1.0000000000000000e+02,  9.5000000000000000e+01,  9.0000000000000000e+00,
+ 2.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 7.8568219999999997e+01,  8.5996200000000002e+01, -1.2500000000000000e+01,
+ 1.0000000000000000e+00,  9.6500000000000000e+01,  4.0000000000000000e+00,
+-1.8000000000000000e+01, -2.1899999999999999e+01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  8.2922240000000002e+01,  8.6963380000000001e+01,
+ 6.5000000000000000e+01,  1.2500000000000000e+01,  1.0000000000000000e+02,
+ 9.8000000000000000e+01,  4.9000000000000000e+01,  3.7000000000000000e+01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  8.2592740000000006e+01,
+ 9.3147599999999997e+01, -1.2000000000000000e+01,  1.0000000000000000e+00,
+ 9.6500000000000000e+01,  4.0000000000000000e+00, -1.8000000000000000e+01,
+-2.1899999999999999e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 7.6506460000000004e+01,  7.8210250000000002e+01,  7.9000000000000000e+01,
+ 1.2000000000000000e+01,  1.0000000000000000e+02,  9.5000000000000000e+01,
+ 6.8000000000000000e+01,  6.1000000000000000e+01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  8.8357460000000003e+01,  9.4257840000000002e+01,
+ 1.2500000000000000e+02,  6.1299999999999997e+01,  1.0000000000000000e+02,
+ 1.0000000000000000e+02,  1.4500000000000000e+02,  1.4500000000000000e+02,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  9.0590469999999996e+01,
+ 1.0582863000000000e+02,  6.2000000000000002e+00,  6.0000000000000000e+00,
+ 9.7000000000000000e+01,  2.8500000000000000e+01,  4.0000000000000000e+00,
+ 3.6000000000000001e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 8.8678200000000004e+01,  9.3617050000000006e+01,  1.6000000000000000e+01,
+ 8.1999999999999993e+00,  9.9000000000000000e+01,  8.0000000000000000e+01,
+ 1.2000000000000000e+01,  9.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  8.0062830000000005e+01,  9.9224010000000007e+01,
+ 1.0000000000000000e+02,  2.1100000000000001e+01,  1.0000000000000000e+02,
+ 1.0000000000000000e+02,  1.1400000000000000e+02,  1.1680000000000000e+02,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  7.4697360000000003e+01,
+ 8.3801220000000001e+01, -8.1999999999999993e+00,  2.0000000000000000e+00,
+ 9.0000000000000000e+01,  2.3999999999999999e+00, -1.2000000000000000e+01,
+-1.4800000000000001e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 7.9194209999999998e+01,  9.0175110000000004e+01,  4.3000000000000000e+01,
+ 8.0000000000000000e+00,  1.0000000000000000e+02,  9.5000000000000000e+01,
+ 9.0000000000000000e+00,  2.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  7.8568219999999997e+01,  8.5996200000000002e+01,
+-1.2500000000000000e+01,  1.0000000000000000e+00,  9.6500000000000000e+01,
+ 4.0000000000000000e+00, -1.8000000000000000e+01, -2.1899999999999999e+01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  8.2922240000000002e+01,
+ 8.6963380000000001e+01,  6.5000000000000000e+01,  1.2500000000000000e+01,
+ 1.0000000000000000e+02,  9.8000000000000000e+01,  4.9000000000000000e+01,
+ 3.7000000000000000e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 8.2592740000000006e+01,  9.3147599999999997e+01, -1.2000000000000000e+01,
+ 1.0000000000000000e+00,  9.6500000000000000e+01,  4.0000000000000000e+00,
+-1.8000000000000000e+01, -2.1899999999999999e+01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  7.6506460000000004e+01,  7.8210250000000002e+01,
+ 7.9000000000000000e+01,  1.2000000000000000e+01,  1.0000000000000000e+02,
+ 9.5000000000000000e+01,  6.8000000000000000e+01,  6.1000000000000000e+01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  8.8357460000000003e+01,
+ 9.4257840000000002e+01,  1.2500000000000000e+02,  6.1299999999999997e+01,
+ 1.0000000000000000e+02,  1.0000000000000000e+02,  1.4500000000000000e+02,
+ 1.4500000000000000e+02, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 9.0590469999999996e+01,  1.0582863000000000e+02,  6.2000000000000002e+00,
+ 6.0000000000000000e+00,  9.7000000000000000e+01,  2.8500000000000000e+01,
+ 4.0000000000000000e+00,  3.6000000000000001e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  8.8678200000000004e+01,  9.3617050000000006e+01,
+ 1.6000000000000000e+01,  8.1999999999999993e+00,  9.9000000000000000e+01,
+ 8.0000000000000000e+01,  1.2000000000000000e+01,  9.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  8.0062830000000005e+01,
+ 9.9224010000000007e+01,  1.0000000000000000e+02,  2.1100000000000001e+01,
+ 1.0000000000000000e+02,  1.0000000000000000e+02,  1.1400000000000000e+02,
+ 1.1680000000000000e+02, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 7.4697360000000003e+01,  8.3801220000000001e+01, -8.1999999999999993e+00,
+ 2.0000000000000000e+00,  9.0000000000000000e+01,  2.3999999999999999e+00,
+-1.2000000000000000e+01, -1.4800000000000001e+01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  7.9194209999999998e+01,  9.0175110000000004e+01,
+ 4.3000000000000000e+01,  8.0000000000000000e+00,  1.0000000000000000e+02,
+ 9.5000000000000000e+01,  9.0000000000000000e+00,  2.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  7.8568219999999997e+01,
+ 8.5996200000000002e+01, -1.2500000000000000e+01,  1.0000000000000000e+00,
+ 9.6500000000000000e+01,  4.0000000000000000e+00, -1.8000000000000000e+01,
+-2.1899999999999999e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 8.2922240000000002e+01,  8.6963380000000001e+01,  6.5000000000000000e+01,
+ 1.2500000000000000e+01,  1.0000000000000000e+02,  9.8000000000000000e+01,
+ 4.9000000000000000e+01,  3.7000000000000000e+01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  8.2592740000000006e+01,  9.3147599999999997e+01,
+-1.2000000000000000e+01,  1.0000000000000000e+00,  9.6500000000000000e+01,
+ 4.0000000000000000e+00, -1.8000000000000000e+01, -2.1899999999999999e+01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  7.6506460000000004e+01,
+ 7.8210250000000002e+01,  7.9000000000000000e+01,  1.2000000000000000e+01,
+ 1.0000000000000000e+02,  9.5000000000000000e+01,  6.8000000000000000e+01,
+ 6.1000000000000000e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 8.8357460000000003e+01,  9.4257840000000002e+01,  1.2500000000000000e+02,
+ 6.1299999999999997e+01,  1.0000000000000000e+02,  1.0000000000000000e+02,
+ 1.4500000000000000e+02,  1.4500000000000000e+02, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  9.0590469999999996e+01,  1.0582863000000000e+02,
+ 6.2000000000000002e+00,  6.0000000000000000e+00,  9.7000000000000000e+01,
+ 2.8500000000000000e+01,  4.0000000000000000e+00,  3.6000000000000001e+00,
+-1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
+ 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
+ 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
+ 1.0000000000000000e+00,  1.0000000000000000e+00, -1.2000000000000000e-01,
+-3.8000000000000000e-01, -5.0000000000000000e-01,  1.0000000000000000e+00,
+ 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
+ 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
+ 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
+ 1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+-4.7000000000000000e+01, -8.6999999999999993e+00, -9.0000000000000000e+01,
+-5.0000000000000000e+01, -1.0000000000000000e+01, -1.0000000000000000e+01,
+-9.3000000000000000e+01, -8.9000000000000000e+01,  1.0000000000000000e+00,
+-1.0000000000000000e+00, -4.7000000000000000e+01, -8.6999999999999993e+00,
+-9.0000000000000000e+01, -5.0000000000000000e+01, -1.0000000000000000e+01,
+-1.0000000000000000e+01, -8.9000000000000000e+01, -8.5000000000000000e+01,
+ 1.0000000000000000e+00, -1.0000000000000000e+00, -4.7000000000000000e+01,
+-8.6999999999999993e+00, -9.0000000000000000e+01, -5.0000000000000000e+01,
+-1.0000000000000000e+01, -1.0000000000000000e+01, -9.1000000000000000e+01,
+-8.8000000000000000e+01, -1.0000000000000000e+00,  5.0000000000000000e-01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  5.0000000000000000e-01,
+-1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
+ 5.0000000000000000e-01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 5.0000000000000000e-01,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00, -4.7000000000000000e+01,
+-8.6999999999999993e+00, -9.0000000000000000e+01, -5.0000000000000000e+01,
+-1.0000000000000000e+01, -1.0000000000000000e+01, -9.3000000000000000e+01,
+-8.9000000000000000e+01,  1.0000000000000000e+00, -1.0000000000000000e+00,
+-4.7000000000000000e+01, -8.6999999999999993e+00, -9.0000000000000000e+01,
+-5.0000000000000000e+01, -1.0000000000000000e+01, -1.0000000000000000e+01,
+-8.9000000000000000e+01, -8.5000000000000000e+01,  1.0000000000000000e+00,
+-1.0000000000000000e+00, -4.7000000000000000e+01, -8.6999999999999993e+00,
+-9.0000000000000000e+01, -5.0000000000000000e+01, -1.0000000000000000e+01,
+-1.0000000000000000e+01, -9.1000000000000000e+01, -8.8000000000000000e+01,
+-1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
+ 5.0000000000000000e-01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 5.0000000000000000e-01, -1.0000000000000000e+00,  5.0000000000000000e-01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  5.0000000000000000e-01,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
+-1.0000000000000000e+00, -4.7000000000000000e+01, -8.6999999999999993e+00,
+-9.0000000000000000e+01, -5.0000000000000000e+01, -1.0000000000000000e+01,
+-1.0000000000000000e+01, -9.3000000000000000e+01, -8.9000000000000000e+01,
+ 1.0000000000000000e+00, -1.0000000000000000e+00, -4.7000000000000000e+01,
+-8.6999999999999993e+00, -9.0000000000000000e+01, -5.0000000000000000e+01,
+-1.0000000000000000e+01, -1.0000000000000000e+01, -8.9000000000000000e+01,
+-8.5000000000000000e+01,  1.0000000000000000e+00, -1.0000000000000000e+00,
+-4.7000000000000000e+01, -8.6999999999999993e+00, -9.0000000000000000e+01,
+-5.0000000000000000e+01, -1.0000000000000000e+01, -1.0000000000000000e+01,
+-9.1000000000000000e+01, -8.8000000000000000e+01, -1.0000000000000000e+00,
+ 5.0000000000000000e-01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 5.0000000000000000e-01, -1.0000000000000000e+00,  5.0000000000000000e-01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  5.0000000000000000e-01,
+-1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
+-1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
+-1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
+-1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
+-1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
+-1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
+-1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
+-1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
+ 1.0000000000000000e+00, -1.0000000000000000e+00, -4.7000000000000000e+01,
+-8.6999999999999993e+00, -9.0000000000000000e+01, -5.0000000000000000e+01,
+-1.0000000000000000e+01, -1.0000000000000000e+01, -9.3000000000000000e+01,
+-8.9000000000000000e+01,  1.0000000000000000e+00, -1.0000000000000000e+00,
+-4.7000000000000000e+01, -8.6999999999999993e+00, -9.0000000000000000e+01,
+-5.0000000000000000e+01, -1.0000000000000000e+01, -1.0000000000000000e+01,
+-8.9000000000000000e+01, -8.5000000000000000e+01,  1.0000000000000000e+00,
+-1.0000000000000000e+00, -4.7000000000000000e+01, -8.6999999999999993e+00,
+-9.0000000000000000e+01, -5.0000000000000000e+01, -1.0000000000000000e+01,
+-1.0000000000000000e+01, -9.1000000000000000e+01, -8.8000000000000000e+01,
+-1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
+ 1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
+ 5.0000000000000000e-01, -1.0000000000000000e+00,  1.0000000000000000e+00,
+ 5.0000000000000000e-01, -1.0000000000000000e+00,  5.0000000000000000e-01,
+-1.0000000000000000e+00,  1.0000000000000000e+00,  5.0000000000000000e-01};
+
+real_t g[] = {+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
+	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
+	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
+	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
+	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
+	+0e+00, +0e+00, -2e+00, -2e+00, -2e+00, -2e+00, -2e+00, -2e+00, -2e+00,
+	-2e+00, +0e+00, -2e+00, +0e+00, +2e-03, +2e-03, +2e-03, +2e-03, +2e-03,
+	+2e-03, +1e-03, +2e-03, +2e-03, +2e-03, +0e+00, -2e-03, -2e-03, -2e-03,
+	-2e-03, -2e-03, -2e-03, -1e-03, -2e-03, -2e-03, -2e-03, +0e+00, +0e+00,
+	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +2e-03,
+	+2e-03, +2e-03, +2e-03, +2e-03, +2e-03, +1e-03, +2e-03, +2e-03, +2e-03,
+	+0e+00, -2e-03, -2e-03, -2e-03, -2e-03, -2e-03, -2e-03, -1e-03, -2e-03,
+	-2e-03, -2e-03, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
+	+0e+00, +0e+00, +0e+00, +2e-03, +2e-03, +2e-03, +2e-03, +2e-03, +2e-03,
+	+1e-03, +2e-03, +2e-03, +2e-03, +0e+00, -2e-03, -2e-03, -2e-03, -2e-03,
+	-2e-03, -2e-03, -1e-03, -2e-03, -2e-03, -2e-03, +0e+00, +0e+00, +0e+00,
+	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +1e-01, +1e-01,
+	+1e-01, +1e-01, +1e-01, +1e-01, +1e-01, +1e-01, +1e-01, +1e-01, +0e+00,
+	-1e-01, -1e-01, -1e-01, -1e-01, -1e-01, -1e-01, -1e-01, -1e-01, -1e-01,
+	-1e-01, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
+	+0e+00};
+
+real_t lb[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, -Inf, 0, -Inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 5,
+	10, 5, 0, 10, 0, 2, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 10, 5, 10, 5, 0, 10, 0, 5, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 5, 10, 5, 0, 10, 0, 5, 0, 10, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+real_t ub[] = {Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
+	Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
+	Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
+	Inf, Inf, 0, 92, 39, 87, 29, 0, 20, 0, 28, 20, 71, Inf, 130, 45, 53, 55, 75,
+	112, 0, 73, 480, 154, 121, 50, 30, 77, 20, 0, 18, 0, 5, 20, 71, Inf, Inf,
+	Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, 130, 55, 93, 60, 75, 115, 0, 67,
+	480, 154, 121, 50, 20, 37, 15, 0, 15, 0, 8, 20, 71, Inf, Inf, Inf, Inf, Inf,
+	Inf, Inf, Inf, Inf, Inf, 130, 55, 93, 60, 75, 105, 0, 67, 4980, 154, 110,
+	50, 20, 37, 15, 0, 25, 0, 8, 20, 71, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
+	Inf, Inf, 20, 20, 20, 20, 0, 20, 0, 20, 0, 20, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf};
+
+real_t lbA[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -Inf, -Inf,
+	-Inf, -Inf, -Inf, -Inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0};
+
+real_t ubA[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
+	Inf, Inf, Inf, Inf};
+
+long H_nnz =  sizeof(H_val) / sizeof(real_t);
+long A_nnz =  sizeof(A_val) / sizeof(real_t);
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_runAllOqpExamples.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_runAllOqpExamples.cpp
new file mode 100644
index 00000000000..44e4a6ca77b
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_runAllOqpExamples.cpp
@@ -0,0 +1,177 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_runAllOqpExamples.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2013-2014
+ *
+ *	Use qpOASES for solving all QP sequences of the Online QP Benchmark 
+ *	Collection. In order to run it, you have to download all examples
+ *	from http://www.qpOASES.org/onlineQP/.
+ */
+
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+/** Example for qpOASES main function using the OQP interface. */
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	/* 1) Define benchmark arguments. */
+	BooleanType isSparse = BT_FALSE;
+	BooleanType useHotstarts;
+
+	Options options;
+	options.setToMPC();
+	options.printLevel = PL_LOW;
+
+	int maxAllowedNWSR;
+	real_t maxNWSR, avgNWSR, maxCPUtime, avgCPUtime;
+	real_t maxStationarity, maxFeasibility, maxComplementarity;
+
+	const int numBenchmarks = 4; //5
+	const char *benchmarkPath[numBenchmarks];
+	benchmarkPath[0] = "../testing/cpp/data/oqp/chain80/";
+	benchmarkPath[1] = "../testing/cpp/data/oqp/chain80w/";
+	benchmarkPath[2] = "../testing/cpp/data/oqp/diesel/";
+	benchmarkPath[3] = "../testing/cpp/data/oqp/crane/";
+	//benchmarkPath[4] = "./cpp/data/oqp/CDU/";
+
+
+	/* 2) Run all benchmarks in a loop */
+	for ( int ii=0; ii<2*numBenchmarks; ++ii )
+	{
+		if ( ii%2 == 0 )
+			useHotstarts = BT_FALSE;
+		else
+			useHotstarts = BT_TRUE;
+		
+		maxAllowedNWSR = 1000;
+		maxNWSR = 0.0;
+		avgNWSR = 0.0;
+		maxCPUtime = 1000.0; /* seconds */
+		avgCPUtime =    0.0; /* seconds */
+		maxStationarity    = 0.0;
+		maxFeasibility     = 0.0;
+		maxComplementarity = 0.0;
+
+		if ( runOQPbenchmark(	benchmarkPath[ii/2],
+								isSparse,useHotstarts,
+								options,maxAllowedNWSR,
+								maxNWSR,avgNWSR,maxCPUtime,avgCPUtime,
+								maxStationarity,maxFeasibility,maxComplementarity
+								) != SUCCESSFUL_RETURN )
+		{
+			myPrintf( "In order to run this example, you need to download example no. 02\nfrom the Online QP Benchmark Collection website first!\n" );
+			return TEST_DATA_NOT_FOUND;
+		}
+
+		/* 3) Print results. */
+		printf( "\n\n" );
+		if ( useHotstarts == BT_FALSE )
+			printf( "OQP Benchmark Results for %s (cold-starts):\n", benchmarkPath[ii/2] );
+		else
+			printf( "OQP Benchmark Results for %s (hot-starts):\n", benchmarkPath[ii/2] );
+
+		printf( "=======================================================\n\n" );
+		//printf( "maximum CPU time:             %.2f milliseconds\n",1000.0*maxCPUtime );
+		//printf( "average CPU time:             %.2f milliseconds\n",1000.0*avgCPUtime );
+		//printf( "\n" );
+		printf( "maximum iterations:    %.1f\n",maxNWSR );
+		printf( "average iterations:    %.1f\n",avgNWSR );
+		printf( "\n" );
+		printf( "maximum violation stationarity:     %.3e\n",maxStationarity );
+		printf( "maximum violation feasibility:      %.3e\n",maxFeasibility );
+		printf( "maximum violation complementarity:  %.3e\n",maxComplementarity );
+		printf( "\n" );
+
+		QPOASES_TEST_FOR_TOL( maxStationarity,    1e-9  );
+		QPOASES_TEST_FOR_TOL( maxFeasibility,     1e-11 );
+		QPOASES_TEST_FOR_TOL( maxComplementarity, 1e-10 );
+		
+		switch( ii )
+		{
+			case 0:
+				/* chain80 (cold) */
+				QPOASES_TEST_FOR_TRUE( maxNWSR <= 62.5 );
+				QPOASES_TEST_FOR_TRUE( avgNWSR <=  7.5 );
+				break;
+
+			case 1:
+				/* chain80 (hot) */
+				QPOASES_TEST_FOR_TRUE( maxNWSR <= 19.5 );
+				QPOASES_TEST_FOR_TRUE( avgNWSR <=  2.4 );
+				break;
+
+			case 2:
+				/* chain80w (cold) */
+				QPOASES_TEST_FOR_TRUE( maxNWSR <= 84.5 );
+				QPOASES_TEST_FOR_TRUE( avgNWSR <= 10.1 );
+				break;
+
+			case 3:
+				/* chain80w (hot) */
+				QPOASES_TEST_FOR_TRUE( maxNWSR <= 16.5 );
+				QPOASES_TEST_FOR_TRUE( avgNWSR <=  2.7 );
+				break;
+
+			case 4:
+				/* diesel (cold) */
+				QPOASES_TEST_FOR_TRUE( maxNWSR <= 26.5 );
+				QPOASES_TEST_FOR_TRUE( avgNWSR <=  0.5 );
+				break;
+
+			case 5:
+				/* diesel (hot) */
+				QPOASES_TEST_FOR_TRUE( maxNWSR <= 22.5 );
+				QPOASES_TEST_FOR_TRUE( avgNWSR <=  0.3 );
+				break;
+
+			case 6:
+				/* crane (cold) */
+				QPOASES_TEST_FOR_TRUE( maxNWSR <= 64.5 );
+				QPOASES_TEST_FOR_TRUE( avgNWSR <= 44.0 );
+				break;
+
+			case 7:
+				/* crane (hot) */
+				QPOASES_TEST_FOR_TRUE( maxNWSR <= 42.5 );
+				QPOASES_TEST_FOR_TRUE( avgNWSR <=  0.4 );
+				break;
+		}
+	}
+	
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_sebastien1.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_sebastien1.cpp
new file mode 100644
index 00000000000..56b79cea5f7
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_sebastien1.cpp
@@ -0,0 +1,74 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/test_sebastien1.cpp
+ *	\author Sebastien B.
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Example that caused troubles in an earlier release.
+ */
+
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+
+/** Example for qpOASES main function using the OQP interface. */
+int main( )
+{
+	REFER_NAMESPACE_QPOASES real_t solution[2]       = {0.0f, 0.0f};
+	REFER_NAMESPACE_QPOASES real_t expectedFirst[2]  = {0.5f, -1.5f};
+  
+	REFER_NAMESPACE_QPOASES real_t H[2*2] = {1.0f, 0.0f, 0.0f, 0.5f};
+	REFER_NAMESPACE_QPOASES real_t g[2]   = {1.5f, 1.0f};
+
+	REFER_NAMESPACE_QPOASES real_t A[1*2] = {1.0f, 1.0f};
+	REFER_NAMESPACE_QPOASES real_t lbA[1] = {-1.0f};
+	REFER_NAMESPACE_QPOASES real_t ubA[1] = {2.0f};
+
+	REFER_NAMESPACE_QPOASES real_t lb[2]  = {0.5f, -2.0f};
+	REFER_NAMESPACE_QPOASES real_t ub[2]  = {5.0f, 2.0f};
+
+	REFER_NAMESPACE_QPOASES QProblem example(2, 1);
+	REFER_NAMESPACE_QPOASES Options options = example.getOptions();
+	//options.enableFarBounds = REFER_NAMESPACE_QPOASES BT_FALSE;
+	example.setOptions(options);
+	example.setPrintLevel(REFER_NAMESPACE_QPOASES PL_NONE);
+
+	// Solve first QP.
+	int nWSR = 10;
+	QPOASES_TEST_FOR_TRUE( example.init(H, g, A, lb, ub, lbA, ubA, nWSR, NULL) == REFER_NAMESPACE_QPOASES SUCCESSFUL_RETURN );
+	QPOASES_TEST_FOR_TRUE( example.isSolved() == REFER_NAMESPACE_QPOASES BT_TRUE );
+	example.getPrimalSolution(solution);
+
+	printf( "\nxOpt = [ %e, %e ];\n\n", solution[0],solution[1] );
+
+	for (unsigned int i=0; i<2; i++)
+		QPOASES_TEST_FOR_NEAR( solution[i],expectedFirst[i] );
+
+	return TEST_PASSED;
+}
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_vanBarelsUnboundedQP.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_vanBarelsUnboundedQP.cpp
new file mode 100644
index 00000000000..bc345e388fe
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_vanBarelsUnboundedQP.cpp
@@ -0,0 +1,69 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES 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.
+ *
+ *	qpOASES 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 qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file testing/cpp/debug_vanBarelsUnboundedQP.cpp
+ *	\author Hans Joachim Ferreau
+ *	\version 3.0
+ *	\date 2007-2014
+ *
+ *	Example that causes troubles when hotstarting.
+ */
+
+
+
+#include <qpOASES.hpp>
+#include <qpOASES/UnitTesting.hpp>
+
+#include <stdio.h>
+
+
+
+int main( )
+{
+	USING_NAMESPACE_QPOASES
+
+	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.0 };
+	real_t g[2] = { 1.5, 1.0 };
+		
+	Options options;
+	//options.enableFarBounds = BT_FALSE;
+
+	QProblemB qp(2);
+	qp.setOptions( options );
+
+	int iter = 10;
+	qp.init( H,g,0,0,iter );
+
+	real_t xOpt[2];
+	qp.getPrimalSolution( xOpt );
+	print( xOpt,2 );
+
+	return TEST_PASSED;
+}
+
+
+/*
+ *	end of file
+ */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/index.html b/3rdparty/qpOASES/qpOASES-3.0/testing/index.html
new file mode 100644
index 00000000000..03648241cd0
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/index.html
@@ -0,0 +1,12 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/testing</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/testing</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="Makefile">Makefile</a></li>
+  <li><a href="checkForMemoryLeaks">checkForMemoryLeaks</a></li>
+  <li><a href="cpp/">cpp/</a></li>
+  <li><a href="matlab/">matlab/</a></li>
+  <li><a href="runUnitTests">runUnitTests</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/generateExample.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/generateExample.m
new file mode 100644
index 00000000000..9476d7eca91
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/generateExample.m
@@ -0,0 +1,25 @@
+function [ qpData ] = generateExample( nV,nC, isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC, seed, givenH,givenA )
+
+    if ( nargin < 11 )
+        givenA = [];
+        if ( nargin < 10 )
+            givenH = [];
+        end
+    end
+
+    qpFeatures = setupQpFeaturesStruct( );
+    
+    qpFeatures.nV = nV;
+    qpFeatures.nC = nC;
+    
+    qpFeatures.isSparseH = isSparseH;
+    qpFeatures.isSparseA = isSparseA;
+    
+    qpFeatures.hasLowerB = hasLowerB;
+    qpFeatures.hasUpperB = hasUpperB;
+    qpFeatures.hasLowerC = hasLowerC;
+    qpFeatures.hasUpperC = hasUpperC;
+    
+    qpData = generateRandomQp( qpFeatures,seed, givenH,givenA );
+
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/generateRandomQp.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/generateRandomQp.m
new file mode 100644
index 00000000000..3fd4fd39787
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/generateRandomQp.m
@@ -0,0 +1,110 @@
+function [ qpData ] = generateRandomQp( qpFeatures,randSeed, givenH,givenA )
+
+    if ( nargin < 4 )
+        givenA = [];
+        if ( nargin < 3 )
+            givenH = [];
+        end
+    end
+    
+    if ( isoctave == 0 )
+        if ( nargin < 2 )
+            s = RandStream( 'mt19937ar', 'Seed','shuffle' );
+            RandStream.setGlobalStream(s);
+        else
+            s = RandStream( 'mt19937ar', 'Seed',randSeed );
+            RandStream.setGlobalStream(s);
+        end
+    end
+    
+
+    qpData = setupQpDataStruct( );
+    qpData.nV = qpFeatures.nV;
+    qpData.nC = qpFeatures.nC;
+    
+    
+    % generate random optimal primal solution
+    xFeas = rand( qpFeatures.nV,1 );
+    
+    % generate random optimal dual solution
+    %yOpt = zeros( qpFeatures.nV+qpFeatures.nC,1 );
+    
+    if ( isempty(givenH) > 0 )
+        
+        switch ( qpFeatures.hessianType )
+
+            case 0
+                qpData.H = 100 * rand( qpData.nV,qpData.nV ) - 50;
+                qpData.H = qpData.H' * qpData.H / 2;
+
+            case 1
+                qpData.H = 100 * rand( qpData.nV,round(qpData.nV/2) ) - 50;
+                qpData.H = qpData.H' * qpData.H / 2;
+
+            case 2
+                qpData.H = eye( qpData.nV );
+
+            case 3
+                qpData.H = zeros( qpData.nV,qpData.nV );
+
+        end
+        
+    else
+        qpData.H = givenH;
+    end
+    
+    qpData.g = 1000 * rand( qpFeatures.nV,1 ) - 500;
+    
+    if ( isempty(givenA) > 0 )
+        
+        if ( qpFeatures.nC > 0 )
+            qpData.Ain = 100 * rand( qpData.nC,qpData.nV ) - 50;
+        else
+            qpData.Ain = [];
+        end
+        
+    else
+        qpData.Ain = givenA;
+    end
+    
+    if ( qpFeatures.makeInfeas > 0 )
+        alpha = -0.1;
+        beta  = -0.001;
+    else
+        alpha = 1;
+        beta  = 1;
+    end
+        
+    if ( qpFeatures.hasLowerB > 0 )
+        qpData.lb = xFeas - 3*rand( qpData.nV,1 );
+    else
+        qpData.lb = [];
+    end
+    
+    if ( qpFeatures.hasUpperB > 0 )
+        qpData.ub = xFeas + alpha*3*rand( qpData.nV,1 );
+    else
+        qpData.ub = [];
+    end
+    
+    if ( ( qpFeatures.hasLowerC > 0 ) && ( qpData.nC > 0 ) )
+        qpData.lbA = qpData.Ain*xFeas - 100*rand( qpData.nC,1 );
+    else
+        qpData.lbA = [];
+    end
+    
+    if ( ( qpFeatures.hasUpperC > 0 ) && ( qpData.nC > 0 ) )
+        qpData.ubA = qpData.Ain*xFeas + beta*100*rand( qpData.nC,1 );
+    else
+        qpData.ubA = [];
+    end
+    
+    if ( qpFeatures.isSparseH > 0 )
+        qpData.H = sparse( qpData.H );
+    end
+    
+    if ( qpFeatures.isSparseA > 0 )
+        qpData.Ain = sparse( qpData.Ain );
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/getKktResidual.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/getKktResidual.m
new file mode 100644
index 00000000000..55b17a08da0
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/getKktResidual.m
@@ -0,0 +1,134 @@
+function [kkt,stat,feas,cmpl] = getKktResidual( H,g,A,lb,ub,lbA,ubA, x,y )
+
+    % Tolerance for dual variables considered zero.
+	dualActiveTolerance = 1.0e3 * eps;
+
+	% Initialize residuals
+    stat = 0.0;
+    feas = 0.0;
+    cmpl = 0.0;
+    
+    if ( isempty(H) == 0 )
+        nV = size( H,1 );
+    else
+        nV = size( g,2 );
+    end
+
+    if ( isempty(A) == 0 )
+        nC = size( A,1 );
+    else
+        nC = 0;
+    end
+
+   
+	%% check stationarity
+	for i=1:nV
+        
+		% g term and variable bounds dual term
+        if ( isempty(g) == 0 )
+            sum = g(i) - y(i);
+        else
+            sum = 0 - y(i);
+        end
+
+		% H*x term
+        if ( isempty(H) == 0 )
+            sum = sum + H(i,:) * x;
+        end
+
+		% A'*y term
+        if ( isempty(A) == 0 )
+            sum = sum - A(:,i)' * y(nV+1:nV+nC);
+        end
+		
+		% update stat
+        if (abs(sum) > stat)
+            stat = abs(sum);
+        end
+        
+	end
+
+    %% check primal feasibility and complementarity
+	% variable bounds
+	for i=1:nV
+
+		% feasibility
+        if ( isempty(lb) == 0 )
+            if (lb(i) - x(i) > feas) 
+				feas = lb(i) - x(i);
+            end
+        end
+
+        if ( isempty(ub) == 0 )
+            if (x(i) - ub(i) > feas) 
+				feas = x(i) - ub(i);
+            end
+        end
+
+		% complementarity
+		prod = 0.0;
+
+        if ( isempty(lb) == 0 )
+             if (y(i) > dualActiveTolerance) % lower bound
+				prod = (x(i) - lb(i)) * y(i);
+            end
+        end
+
+        if ( isempty(ub) == 0 )
+            if (y(i) < -dualActiveTolerance) % upper bound
+				prod = (x(i) - ub(i)) * y(i);
+            end
+        end
+
+        if (abs(prod) > cmpl)
+            cmpl = abs(prod);
+        end
+        
+	end
+    
+	% A*x bounds
+    for i=1:nC
+
+        % compute sum = (A*x)_i
+		sum = 0.0;
+        if ( isempty(A) == 0 )
+            sum = sum + A(i,:) * x;
+        end
+
+		% feasibility
+        if ( isempty(lbA) == 0 )
+            if (lbA(i) - sum > feas) 
+				feas = lbA(i) - sum;
+            end
+        end
+
+        if ( isempty(ubA) == 0 )
+            if (sum - ubA(i) > feas) 
+				feas = sum - ubA(i);
+            end
+        end
+
+		% complementarity
+		prod = 0.0;
+
+        if ( isempty(lbA) == 0 )
+            if (y(nV+i) > dualActiveTolerance) % lower bound
+				prod = (sum - lbA(i)) * y(nV+i);
+            end
+        end
+		
+        if ( isempty(ubA) == 0 )
+            if (y(nV+i) < -dualActiveTolerance) % upper bound
+				prod = (sum - ubA(i)) * y(nV+i);
+            end
+        end
+
+        if (abs(prod) > cmpl)
+            cmpl = abs(prod);
+        end
+        
+    end
+    
+    kkt = max( [stat,feas,cmpl] );
+
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/index.html b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/index.html
new file mode 100644
index 00000000000..80bcff84a22
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/index.html
@@ -0,0 +1,13 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/testing/matlab/auxFiles</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/testing/matlab/auxFiles</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="generateExample.m">generateExample.m</a></li>
+  <li><a href="generateRandomQp.m">generateRandomQp.m</a></li>
+  <li><a href="getKktResidual.m">getKktResidual.m</a></li>
+  <li><a href="isoctave.m">isoctave.m</a></li>
+  <li><a href="setupQpDataStruct.m">setupQpDataStruct.m</a></li>
+  <li><a href="setupQpFeaturesStruct.m">setupQpFeaturesStruct.m</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/isoctave.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/isoctave.m
new file mode 100644
index 00000000000..e4726d45db6
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/isoctave.m
@@ -0,0 +1,21 @@
+
+% ISOCTAVE  True if the operating environment is octave.
+%    Usage: t=isoctave();
+% 
+%    Returns 1 if the operating environment is octave, otherwise
+%    0 (Matlab)
+% 
+% ---------------------------------------------------------------
+function t=isoctave()
+%ISOCTAVE  True if the operating environment is octave.
+%   Usage: t=isoctave();
+%
+%   Returns 1 if the operating environment is octave, otherwise
+%   0 (Matlab)
+
+if exist('OCTAVE_VERSION')
+  % Only Octave has this variable.
+  t=1;
+else
+  t=0;
+end;
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/setupQpDataStruct.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/setupQpDataStruct.m
new file mode 100644
index 00000000000..93d9e3c8172
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/setupQpDataStruct.m
@@ -0,0 +1,18 @@
+function [ qpData ] = setupQpDataStruct( )
+
+    qpData = struct(    'H', [], ...        % Hessian matrix
+                        'g', [], ...        % gradient vector
+                        'Aeq', [], ...      % equality constraints matrix
+                        'beq', [], ...      % equality constraints vector
+                        'lb', [], ...       % lower bound vector
+                        'ub', [], ...       % upper bound vector
+                        'Ain', [], ...      % inequality constraints matrix
+                        'lbA', [], ...      % lower constraints vector
+                        'ubA', [], ...      % upper constraints vector
+                        'x0', [], ...       % primal initial guess
+                        'options', [], ...  % QP solver options
+                        'nV', 0, ...        % number of QP variables
+                        'nC', 0 ...         % number of constraints
+                        );
+
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/setupQpFeaturesStruct.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/setupQpFeaturesStruct.m
new file mode 100644
index 00000000000..903a891a715
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/setupQpFeaturesStruct.m
@@ -0,0 +1,21 @@
+function [ qpFeatures ] = setupQpFeaturesStruct( )
+
+    qpFeatures = struct(    'nV',  0, ...
+                            'nIB', 0, ...
+                            'nEB', 0, ...
+                            'nC',  0, ...
+                            'nIC', 0, ...
+                            'nEC', 0, ...
+                            'nActB', 0, ...
+                            'nActC', 0, ...
+                            'hasLowerB', 0, ...
+                            'hasUpperB', 0, ...
+                            'hasLowerC', 0, ...
+                            'hasUpperC', 0, ...
+                            'isSparseH', 0, ...
+                            'isSparseA', 0, ...
+                            'makeInfeas', 0, ...
+                            'hessianType', 0 ... % 0 = pos def; 1 = pos sem def; 2 = id; 3 = zero
+                            );
+
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/data/fetch_matlab_data b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/data/fetch_matlab_data
new file mode 100644
index 00000000000..44cd54eb588
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/data/fetch_matlab_data
@@ -0,0 +1,34 @@
+#!/usr/bin/env bash
+
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+##
+##	Filename:  testing/matlab/data/fetch_matlab_data
+##	Author:    Hans Joachim Ferreau
+##	Version:   3.0
+##	Date:      2014
+
+
+svn export https://projects.coin-or.org/svn/qpOASES/misc/testingdata/matlab . --force
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/data/index.html b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/data/index.html
new file mode 100644
index 00000000000..b2561df2fd3
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/data/index.html
@@ -0,0 +1,8 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/testing/matlab/data</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/testing/matlab/data</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="fetch_matlab_data">fetch_matlab_data</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/index.html b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/index.html
new file mode 100644
index 00000000000..14c3e5dab82
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/index.html
@@ -0,0 +1,11 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/testing/matlab</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/testing/matlab</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="auxFiles/">auxFiles/</a></li>
+  <li><a href="data/">data/</a></li>
+  <li><a href="runAllTests.m">runAllTests.m</a></li>
+  <li><a href="tests/">tests/</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/runAllTests.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/runAllTests.m
new file mode 100644
index 00000000000..c1e671a6f4e
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/runAllTests.m
@@ -0,0 +1,153 @@
+function [ successFlag ] = runAllTests( doPrint )
+    
+    if ( nargin < 1 )
+        doPrint = 0;
+    end
+    
+    
+    %% add sub-folders to Matlab path
+    addpath('../../interfaces/matlab/');
+    addpath(genpath(pwd));
+    addpath(genpath([pwd 'auxFiles']));
+    addpath(genpath([pwd 'data']));
+    addpath(genpath([pwd 'tests']));
+    
+    
+    successFlag = 1;
+    
+    curWarnLevel = warning;
+    warning('off');
+    
+    clc;
+
+    %% run interface tests
+    fprintf( 'Running qpOASES interface tests... ' )
+    successFlag = updateSuccessFlag( successFlag, runInterfaceTest( 10,20, doPrint,42 ) );
+    
+    fprintf( 'Running qpOASES_sequence interface tests... ' )
+    successFlag = updateSuccessFlag( successFlag, runInterfaceSeqTest( 8,5, doPrint,42 ) );
+    
+    
+    %% run functional tests
+    fprintf( 'Running tests with random QPs having identity Hessian... ' )
+    successFlag = updateSuccessFlag( successFlag, runRandomIdHessian( 12,12, doPrint,4242 ) );
+    
+    %fprintf( 'Running tests with random QPs having zero Hessian... ' )
+    %successFlag = updateSuccessFlag( successFlag, runRandomZeroHessian( 11,41, doPrint,4242 ) );
+    
+    fprintf( 'Running alternativeX0 test... ' )
+    successFlag = updateSuccessFlag( successFlag, runAlternativeX0Test( 50,300,doPrint,4242 ) );
+    
+    fprintf( 'Running testAPrioriKnownSeq1... ' )
+    successFlag = updateSuccessFlag( successFlag, runTestAPrioriKnownSeq1( doPrint ) );
+    
+    fprintf( 'Running testSeq... ' )
+    successFlag = updateSuccessFlag( successFlag, runTestSeq( doPrint ) );
+    
+    fprintf( 'Running testSparse... ' )
+    successFlag = updateSuccessFlag( successFlag, runTestSparse( doPrint ) );
+    
+    fprintf( 'Running testSparse2... ' )
+    successFlag = updateSuccessFlag( successFlag, runTestSparse2( doPrint ) );
+    
+    fprintf( 'Running testSparse3... ' )
+    successFlag = updateSuccessFlag( successFlag, runTestSparse3( doPrint ) );
+    
+    fprintf( 'Running testSparse4... ' )
+    successFlag = updateSuccessFlag( successFlag, runTestSparse4( doPrint ) );
+    
+    fprintf( 'Running simpleSpringExample... ' )
+    successFlag = updateSuccessFlag( successFlag, runSimpleSpringExample( doPrint ) );
+    
+    fprintf( 'Running vanBarelsUnboundedQP... ' )
+    successFlag = updateSuccessFlag( successFlag, runVanBarelsUnboundedQP( doPrint ) );
+    
+    fprintf( 'Running alexInfeas1... ' )
+    successFlag = updateSuccessFlag( successFlag, runAlexInfeas1( doPrint ) );
+    
+    %fprintf( 'Running alexInfeas2... ' )
+    %successFlag = updateSuccessFlag( successFlag, runAlexInfeas2( doPrint ) );
+
+    %fprintf( 'Running QAP8... ' )
+    %successFlag = updateSuccessFlag( successFlag, runQAP( doPrint ) );
+    
+    fprintf( 'Running testWorkingSetLI... ' )
+    successFlag = updateSuccessFlag( successFlag, runTestWorkingSetLI( doPrint ) );
+    
+
+    fprintf( 'Running EXAMPEL1... ' );
+    successFlag = updateSuccessFlag( successFlag, runBenchmarkEXAMPLE1( 10 ) );
+	
+    fprintf( 'Running EXAMPLE1A... ' );
+    successFlag = updateSuccessFlag( successFlag, runBenchmarkEXAMPLE1A( 10 ) );
+	
+    fprintf( 'Running EXAMPLE1B... ' );
+    successFlag = updateSuccessFlag( successFlag, runBenchmarkEXAMPLE1B( 10 ) );
+
+	fprintf( 'Running CHAIN1... ' );
+    successFlag = updateSuccessFlag( successFlag, runBenchmarkCHAIN1( 20 ) );
+	
+    fprintf( 'Running CHAIN1A... ' );
+    successFlag = updateSuccessFlag( successFlag, runBenchmarkCHAIN1A( 20 ) );
+
+	fprintf( 'Running CRANE1... ' );
+    successFlag = updateSuccessFlag( successFlag, runBenchmarkCRANE1( 100 ) );
+
+    fprintf( 'Running CRANE2... ' );
+    successFlag = updateSuccessFlag( successFlag, runBenchmarkCRANE2( 100 ) );
+
+    fprintf( 'Running CRANE3... ' );
+    successFlag = updateSuccessFlag( successFlag, runBenchmarkCRANE3( 100 ) );
+	
+	fprintf( 'Running EQUALITY1... ' );
+    successFlag = updateSuccessFlag( successFlag, runBenchmarkEQUALITY1( 100 ) );
+
+    fprintf( 'Running EQUALITY2... ' );
+    successFlag = updateSuccessFlag( successFlag, runBenchmarkEQUALITY2( 3200 ) );
+
+    %fprintf( 'Running IDHESSIAN1... ' );
+	%successFlag = updateSuccessFlag( successFlag, runBenchmarkIDHESSIAN1( 1200 ) );
+
+    fprintf( 'Running DIESEL... ' );
+    successFlag = updateSuccessFlag( successFlag, runBenchmarkDIESEL( 230 ) );
+    
+    fprintf( 'Running QSHARE1B... ' )
+    successFlag = updateSuccessFlag( successFlag, runQSHARE1B( doPrint ) );
+
+    
+    
+    %% display results
+    disp( ' ' );
+    
+    if ( successFlag == 0 )
+        disp( 'At least one test failed!' );
+    else
+        disp( 'All available tests passed successfully!' );
+    end
+
+    warning( curWarnLevel );
+    
+end
+
+
+function [ newSuccessFlag ] = updateSuccessFlag( curSuccessFlag,curResult )
+
+    switch ( curResult )
+        
+        case 0
+            newSuccessFlag = 0;
+            fprintf( 'failed!\n' );
+        
+        case 1          
+            newSuccessFlag = curSuccessFlag;
+            fprintf( 'passed!\n' );
+            
+        case -1
+            newSuccessFlag = curSuccessFlag;
+            fprintf( 'problem data missing!\n' );
+            
+        otherwise
+            error( 'Unknown success flag!' );
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/index.html b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/index.html
new file mode 100644
index 00000000000..366b7aa9077
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/index.html
@@ -0,0 +1,37 @@
+<html><head><title>qpOASES - Revision 198: /stable/3.0/testing/matlab/tests</title></head>
+<body>
+ <h2>qpOASES - Revision 198: /stable/3.0/testing/matlab/tests</h2>
+ <ul>
+  <li><a href="../">..</a></li>
+  <li><a href="runAlexInfeas1.m">runAlexInfeas1.m</a></li>
+  <li><a href="runAlexInfeas2.m">runAlexInfeas2.m</a></li>
+  <li><a href="runAlternativeX0Test.m">runAlternativeX0Test.m</a></li>
+  <li><a href="runBenchmarkCHAIN1.m">runBenchmarkCHAIN1.m</a></li>
+  <li><a href="runBenchmarkCHAIN1A.m">runBenchmarkCHAIN1A.m</a></li>
+  <li><a href="runBenchmarkCRANE1.m">runBenchmarkCRANE1.m</a></li>
+  <li><a href="runBenchmarkCRANE2.m">runBenchmarkCRANE2.m</a></li>
+  <li><a href="runBenchmarkCRANE3.m">runBenchmarkCRANE3.m</a></li>
+  <li><a href="runBenchmarkDIESEL.m">runBenchmarkDIESEL.m</a></li>
+  <li><a href="runBenchmarkEQUALITY1.m">runBenchmarkEQUALITY1.m</a></li>
+  <li><a href="runBenchmarkEQUALITY2.m">runBenchmarkEQUALITY2.m</a></li>
+  <li><a href="runBenchmarkEXAMPLE1.m">runBenchmarkEXAMPLE1.m</a></li>
+  <li><a href="runBenchmarkEXAMPLE1A.m">runBenchmarkEXAMPLE1A.m</a></li>
+  <li><a href="runBenchmarkEXAMPLE1B.m">runBenchmarkEXAMPLE1B.m</a></li>
+  <li><a href="runBenchmarkIDHESSIAN1.m">runBenchmarkIDHESSIAN1.m</a></li>
+  <li><a href="runInterfaceSeqTest.m">runInterfaceSeqTest.m</a></li>
+  <li><a href="runInterfaceTest.m">runInterfaceTest.m</a></li>
+  <li><a href="runQAP8.m">runQAP8.m</a></li>
+  <li><a href="runQSHARE1B.m">runQSHARE1B.m</a></li>
+  <li><a href="runRandomIdHessian.m">runRandomIdHessian.m</a></li>
+  <li><a href="runRandomZeroHessian.m">runRandomZeroHessian.m</a></li>
+  <li><a href="runSimpleSpringExample.m">runSimpleSpringExample.m</a></li>
+  <li><a href="runTestAPrioriKnownSeq1.m">runTestAPrioriKnownSeq1.m</a></li>
+  <li><a href="runTestSeq.m">runTestSeq.m</a></li>
+  <li><a href="runTestSparse.m">runTestSparse.m</a></li>
+  <li><a href="runTestSparse2.m">runTestSparse2.m</a></li>
+  <li><a href="runTestSparse3.m">runTestSparse3.m</a></li>
+  <li><a href="runTestSparse4.m">runTestSparse4.m</a></li>
+  <li><a href="runTestWorkingSetLI.m">runTestWorkingSetLI.m</a></li>
+  <li><a href="runVanBarelsUnboundedQP.m">runVanBarelsUnboundedQP.m</a></li>
+ </ul>
+</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlexInfeas1.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlexInfeas1.m
new file mode 100644
index 00000000000..d1977ed3fe4
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlexInfeas1.m
@@ -0,0 +1,30 @@
+function [ successFlag ] = runAlexInfeas1( doPrint )
+    
+    if ( nargin < 1 )
+        doPrint = 0;
+    end
+
+    successFlag = 0;
+    
+    try
+        data = load( 'alexInfeas1.mat' );
+    catch
+        successFlag = -1;
+        return;
+    end
+    
+    options1 = qpOASES_options( 'default', 'printLevel',-2*doPrint );
+    
+    [x1,dummy,exitflag1] = qpOASES( data.H,data.g,data.A, ...
+                data.lb,data.ub,data.lbA,data.ubA, options1 ); %#ok<*NASGU>
+
+    % should return "QP infeasible"
+    if ( exitflag1 == -2 )
+        successFlag = 1;
+    else
+        if ( doPrint > 0 )
+            disp( [data.lbA data.A*x1 data.ubA] );
+        end
+    end
+   
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlexInfeas2.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlexInfeas2.m
new file mode 100644
index 00000000000..730a01c4871
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlexInfeas2.m
@@ -0,0 +1,35 @@
+function [ successFlag ] = runAlexInfeas2( doPrint )
+    
+    if ( nargin < 1 )
+        doPrint = 0;
+    end
+
+    successFlag = 0;
+    
+    try
+        data = load( 'alexInfeas2.mat' );
+    catch
+        successFlag = -1;
+        return;
+    end
+    
+    options1 = qpOASES_options( 'default', 'printLevel',-2*doPrint,... 
+                                           'maxIter',7000, 'enableFlippingBounds',0,...
+                                           'terminationTolerance',1e7*eps);
+                                       
+    options2 = qpOASES_options( 'MPC', 'printLevel',-2*doPrint,... 
+                                       'maxIter',3000, 'terminationTolerance',1e8*eps );
+    
+    [x1,dummy,exitflag1] = qpOASES( data.H,data.g,data.A, ...
+                data.lb,data.ub,data.lbA,data.ubA, options1 ); %#ok<*NASGU>
+
+    % should return "QP infeasible"
+    if ( exitflag1 == -2 )
+        successFlag = 1;
+    else
+        if ( doPrint > 0 )
+            %disp( [data.lbA data.A*x1 data.ubA] );
+        end
+    end
+   
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlternativeX0Test.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlternativeX0Test.m
new file mode 100644
index 00000000000..3178ed221e5
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlternativeX0Test.m
@@ -0,0 +1,84 @@
+function [ successFlag ] = runAlternativeX0Test( nV,nC, doPrint,seed )
+    
+    if ( nargin < 4 )
+        seed = 4242;
+        if ( nargin < 3 )
+            doPrint = 0;
+            if ( nargin < 2 )
+                nC = 300;
+                if ( nargin < 1 )
+                    nV = 50;
+                end
+            end
+        end
+    end
+
+    successFlag = 1;
+    TOL = eps;
+    
+    qpData = generateExample( nV,nC, 0,0, 0,1,1,1, seed );
+    
+    H = qpData.H;
+	g = qpData.g;
+    A = [qpData.Aeq;qpData.Ain];
+	lb = qpData.lb;
+    ub = qpData.ub;
+    lbA = [qpData.beq;qpData.lbA];
+    ubA = [qpData.beq;qpData.ubA];
+    
+    x0 = nV*rand( nV,1 );
+    
+    options = qpOASES_options( 'default', 'printLevel',2*doPrint );
+    auxInput = qpOASES_auxInput( 'x0',x0 );
+    
+    [ x1,f1,e1,i1,l1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],x0 );
+    [ x2,f2,e2,i2,l2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+    [ x3,f3,e3,i3,l3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,x0 );
+    [ x4,f4,e4,i4,l4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+    
+    if ( ( norm(x1-x2) > TOL ) || ...
+         ( norm(x1-x3) > TOL ) || ...
+         ( norm(x1-x4) > TOL ) )
+        if ( doPrint > 0 )
+            disp('diff in x')
+        end
+        successFlag = 0;
+    end
+    
+    if ( ( norm(f1-f2) > TOL ) || ...
+         ( norm(f1-f3) > TOL ) || ...
+         ( norm(f1-f4) > TOL ) )
+        if ( doPrint > 0 )
+            disp('diff in fval')
+        end
+        successFlag = 0;
+    end
+    
+    if ( ( norm(e1-e2) > TOL ) || ...
+         ( norm(e1-e3) > TOL ) || ...
+         ( norm(e1-e4) > TOL ) )
+        if ( doPrint > 0 )
+            disp('diff in exitflag')
+        end
+        successFlag = 0;
+    end
+    
+    if ( ( norm(i1-i2) > TOL ) || ...
+         ( norm(i1-i3) > TOL ) || ...
+         ( norm(i1-i4) > TOL ) )
+        if ( doPrint > 0 )
+            disp('diff in iter')
+        end
+        successFlag = 0;
+    end
+    
+    if ( ( norm(l1-l2) > TOL ) || ...
+         ( norm(l1-l3) > TOL ) || ...
+         ( norm(l1-l4) > TOL ) )
+        if ( doPrint > 0 )
+            disp('diff in lambda')
+        end
+        successFlag = 0;
+    end
+		
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCHAIN1.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCHAIN1.m
new file mode 100644
index 00000000000..70d8a33c480
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCHAIN1.m
@@ -0,0 +1,56 @@
+function [ successFlag ] = runBenchmarkCHAIN1( nWSR )
+
+    successFlag = 0;
+	maxViolation = 0;
+
+    clear H g A lb ub lbA ubA;
+    
+    try
+        load 'benchmarkCHAIN1.mat';
+    catch
+        successFlag = -1;
+        return;
+    end
+	
+    if ( exist( 'A','var' ) )
+        [nC,nV] = size(A);
+    else
+        nC = 0;
+    end
+    [nV,nP] = size(g);
+
+    xOpt = zeros(nV,nP);
+    yOpt = zeros(nV+nC,nP);
+    objOpt = zeros(1,nP);
+    iter = zeros(1,nP);
+
+	options = qpOASES_options( 'fast','maxIter',nWSR );
+    %options = qpOASES_options( 'maxIter',nWSR );
+
+	for i=1:nP
+		%disp(i);
+
+		if ( i == 1 )
+			[QP,x,obj,status,nWSRout,lambda,ws] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+            %disp(ws')
+		else
+			[x,obj,status,nWSRout,lambda,ws] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+            %disp(ws')
+        end
+
+		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
+		maxViolation = max( [maxViolation,maxViolationTMP] );
+        
+        xOpt(:,i) = x;
+        yOpt(:,i) = lambda;
+        objOpt(:,i) = obj;
+        iter(:,i) = nWSRout;
+	end
+	
+	qpOASES_sequence( 'c',QP );
+    
+    if ( ( maxViolation < 9e-13 ) && ( status == 0 ) )
+        successFlag = 1;
+    end
+	
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCHAIN1A.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCHAIN1A.m
new file mode 100644
index 00000000000..ed5272c60c2
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCHAIN1A.m
@@ -0,0 +1,57 @@
+function [ successFlag ] = runBenchmarkCHAIN1A( nWSR )
+
+    successFlag = 0;
+	maxViolation = 0;
+
+    clear H g A lb ub lbA ubA;
+    
+    try
+        load 'benchmarkCHAIN1A.mat';
+    catch
+        successFlag = -1;
+        return;
+    end
+	
+    if ( exist( 'A','var' ) )
+        [nC,nV] = size(A);
+    else
+        nC = 0;
+    end
+    [nV,nP] = size(g);
+
+    xOpt = zeros(nV,nP);
+    yOpt = zeros(nV+nC,nP);
+    objOpt = zeros(1,nP);
+    
+	options = qpOASES_options( 'fast','maxIter',nWSR );
+    %options = qpOASES_options( 'reliable', 'maxIter',500,'printLevel',1 );
+% ,'enableRamping',0,'enableFlippingBounds',0,'enableFullLITests',0,'enableDriftCorrection',0
+  
+  
+	for jj=1:1
+  
+	for i=1:nP
+		%disp(i);
+		if ( i == 1 )
+            [QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),lb(:,i),ub(:,i),options );
+		else
+            [x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),options );
+        end
+
+		[ maxViolationTMP ] = getKktResidual( H,g(:,i),[],lb(:,i),ub(:,i),[],[], x,lambda );
+		maxViolation = max( [maxViolation,maxViolationTMP] );
+
+        xOpt(:,i) = x;
+        yOpt(:,i) = lambda;
+        objOpt(:,i) = obj;
+	end
+	
+	qpOASES_sequence( 'c',QP );
+
+    end
+    
+    if ( ( maxViolation < 1e-14 ) && ( status == 0 ) )
+        successFlag = 1;
+    end   
+	
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE1.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE1.m
new file mode 100644
index 00000000000..4016aa86255
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE1.m
@@ -0,0 +1,51 @@
+function [ successFlag ] = runBenchmarkCRANE1( nWSR )
+
+    successFlag = 0;
+	maxViolation = 0;
+
+    clear H g A lb ub lbA ubA;
+	
+    try
+        load 'benchmarkCRANE1.mat';
+    catch
+        successFlag = -1;
+        return;
+    end
+
+    if ( exist( 'A','var' ) )
+        [nC,nV] = size(A);
+    else
+        nC = 0;
+    end
+    [nV,nP] = size(g);
+
+    xOpt = zeros(nV,nP);
+    yOpt = zeros(nV+nC,nP);
+    objOpt = zeros(1,nP);
+
+	options = qpOASES_options( 'fast','maxIter',nWSR );
+
+    for i=1:nP
+		%disp(i);
+
+		if ( i == 1 )
+			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		else
+			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		end
+
+		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
+		maxViolation = max( [maxViolation,maxViolationTMP] );
+
+        xOpt(:,i) = x;
+        yOpt(:,i) = lambda;
+        objOpt(:,i) = obj;
+	end
+	
+	qpOASES_sequence( 'c',QP );
+
+    if ( ( maxViolation < 3e-9 ) && ( status == 0 ) )
+        successFlag = 1;
+    end
+	
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE2.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE2.m
new file mode 100644
index 00000000000..384a4195dd2
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE2.m
@@ -0,0 +1,52 @@
+function [ successFlag ] = runBenchmarkCRANE2( nWSR )
+
+    successFlag = 0;
+	maxViolation = 0;
+
+    clear H g A lb ub lbA ubA;
+	
+    try
+        load 'benchmarkCRANE2.mat';
+	catch
+        successFlag = -1;
+        return;
+    end
+    
+    if ( exist( 'A','var' ) )
+        [nC,nV] = size(A);
+    else
+        nC = 0;
+    end
+    [nV,nP] = size(g);
+    
+    xOpt = zeros(nV,nP);
+    yOpt = zeros(nV+nC,nP);
+    objOpt = zeros(1,nP);
+    
+	%options = qpOASES_options( 'maxIter',nWSR );
+    options = qpOASES_options( 'fast','maxIter',nWSR );
+
+	for i=1:nP
+		%disp(i);
+
+		if ( i == 1 )
+			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		else
+			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'm',QP,H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		end
+
+		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
+		maxViolation = max( [maxViolation,maxViolationTMP] );
+
+        xOpt(:,i) = x;
+        yOpt(:,i) = lambda;
+        objOpt(:,i) = obj;
+	end
+
+	qpOASES_sequence( 'c',QP );
+
+    if ( ( maxViolation < 1e-11 ) && ( status == 0 ) )
+        successFlag = 1;
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE3.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE3.m
new file mode 100644
index 00000000000..f62ac4ca36a
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE3.m
@@ -0,0 +1,51 @@
+function [ successFlag ] = runBenchmarkCRANE3( nWSR )
+
+    successFlag = 0;
+	maxViolation = 0;
+
+    clear H g A lb ub lbA ubA;
+    try
+        load 'benchmarkCRANE3.mat';
+    catch
+        successFlag = -1;
+        return;
+    end
+
+    if ( exist( 'A','var' ) )
+        [nC,nV] = size(A);
+    else
+        nC = 0;
+    end
+    [nV,nP] = size(g);
+
+    xOpt = zeros(nV,nP);
+    yOpt = zeros(nV+nC,nP);
+    objOpt = zeros(1,nP);
+
+	%options = qpOASES_options( 'maxIter',nWSR );
+    options = qpOASES_options( 'fast','maxIter',nWSR );
+
+	for i=1:nP
+		%disp(i);
+
+		if ( i == 1 )
+			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		else
+			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		end
+
+		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
+		maxViolation = max( [maxViolation,maxViolationTMP] );
+
+        xOpt(:,i) = x;
+        yOpt(:,i) = lambda;
+        objOpt(:,i) = obj;
+	end
+	
+	qpOASES_sequence( 'c',QP );
+
+    if ( ( maxViolation < 5e-11 ) && ( status == 0 ) )
+        successFlag = 1;
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkDIESEL.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkDIESEL.m
new file mode 100644
index 00000000000..36ea449fcc0
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkDIESEL.m
@@ -0,0 +1,55 @@
+function [ successFlag ] = runBenchmarkDIESEL( nWSR )
+
+    successFlag = 0;
+	maxViolation = 0;
+
+    clear H g A lb ub lbA ubA;
+    try
+        load 'benchmarkDIESEL.mat';
+    catch
+        successFlag = -1;
+        return;
+    end
+	
+    if ( exist( 'A','var' ) )
+        [nC,nV] = size(A);
+    else
+        nC = 0;
+		nEC = 0;
+    end
+    [nV,nP] = size(g);
+
+    xOpt = zeros(nV,nP);
+    yOpt = zeros(nV+nC,nP);
+    objOpt = zeros(1,nP);
+    iter = zeros(1,nP);
+
+    options = qpOASES_options( 'fast','maxIter',nWSR );
+    %options = qpOASES_options( 'maxIter',nWSR );
+    %options = qpOASES_options( 'maxIter',nWSR, 'initialStatusBounds',0 );
+
+	for i=1:nP
+		%disp(i);
+
+		if ( i == 1 )
+			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		else
+			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+        end
+
+		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
+		maxViolation = max( [maxViolation,maxViolationTMP] );
+        
+        xOpt(:,i) = x;
+        yOpt(:,i) = lambda;
+        objOpt(:,i) = obj;
+        iter(:,i) = nWSRout;
+	end
+	
+	qpOASES_sequence( 'c',QP );
+    
+    if ( ( maxViolation < 6e-13 ) && ( status == 0 ) )
+        successFlag = 1;
+    end	
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEQUALITY1.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEQUALITY1.m
new file mode 100644
index 00000000000..e21fd27b3c7
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEQUALITY1.m
@@ -0,0 +1,52 @@
+function [ successFlag ] = runBenchmarkEQUALITY1( nWSR )
+
+    successFlag = 0;
+	maxViolation = 0;
+
+    clear H g A lb ub lbA ubA;
+	
+    try
+        load 'benchmarkEQUALITY1.mat';
+    catch
+        successFlag = -1;
+        return;
+    end
+
+   	if ( exist( 'A','var' ) )
+        [nC,nV] = size(A);
+    else
+        nC = 0;
+    end
+    [nV,nP] = size(g);
+
+    xOpt = zeros(nV,nP);
+    yOpt = zeros(nV+nC,nP);
+    objOpt = zeros(1,nP);
+    
+	%options = qpOASES_options( 'maxIter',nWSR );
+    options = qpOASES_options( 'fast','maxIter',nWSR );
+
+	for i=1:nP
+		%disp(i);
+
+		if ( i == 1 )
+			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		else
+			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		end
+
+		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
+		maxViolation = max( [maxViolation,maxViolationTMP] );
+
+        xOpt(:,i) = x;
+        yOpt(:,i) = lambda;
+        objOpt(:,i) = obj;
+	end
+	
+	qpOASES_sequence( 'c',QP );
+
+    if ( ( maxViolation < 4e-15 ) && ( status == 0 ) )
+        successFlag = 1;
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEQUALITY2.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEQUALITY2.m
new file mode 100644
index 00000000000..c743047c4ae
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEQUALITY2.m
@@ -0,0 +1,56 @@
+function [ successFlag ] = runBenchmarkEQUALITY2( nWSR )
+
+    successFlag = 0;
+	maxViolation = 0;
+
+    clear H g A lb ub lbA ubA;
+	
+    try
+        load 'benchmarkEQUALITY2.mat';
+    catch
+        successFlag = -1;
+        return;
+    end
+
+   	if ( exist( 'A','var' ) )
+        [nC,nV] = size(A);
+    else
+        nC = 0;
+    end
+    [nV,nP] = size(g);
+
+    xOpt = zeros(nV,nP);
+    yOpt = zeros(nV+nC,nP);
+    objOpt = zeros(1,nP);
+   
+    %H = H;
+    %lbA(65:320) = lbA(65:320) - 0.000000000;
+    %ubA(65:320) = ubA(65:320) + 0.000000000;
+
+	options = qpOASES_options( 'maxIter',nWSR, 'enableEqualities',1, 'printLevel',0 );
+    %options = qpOASES_options( 'fast','maxIter',nWSR, 'printLevel',-2 );
+
+	for i=1:nP
+		%disp(i);
+
+		if ( i == 1 )
+			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		else
+			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		end
+
+		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
+		maxViolation = max( [maxViolation,maxViolationTMP] );
+
+        xOpt(:,i) = x;
+        yOpt(:,i) = lambda;
+        objOpt(:,i) = obj;
+	end
+	
+	qpOASES_sequence( 'c',QP );
+
+    if ( ( maxViolation < 3e-7 ) && ( status == 0 ) )
+        successFlag = 1;
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1.m
new file mode 100644
index 00000000000..338e95232ac
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1.m
@@ -0,0 +1,52 @@
+function [ successFlag ] = runBenchmarkEXAMPLE1( nWSR )
+
+    successFlag = 0;
+	maxViolation = 0;
+
+    clear H g A lb ub lbA ubA;
+	
+    try
+        load 'benchmarkEXAMPLE1.mat';
+    catch
+        successFlag = -1;
+        return;
+    end
+	
+    if ( exist( 'A','var' ) )
+        [nC,nV] = size(A);
+    else
+        nC = 0;
+    end
+    [nV,nP] = size(g);
+
+    xOpt = zeros(nV,nP);
+    yOpt = zeros(nV+nC,nP);
+    objOpt = zeros(1,nP);
+    
+	%options = qpOASES_options( 'maxIter',nWSR );
+    options = qpOASES_options( 'fast','maxIter',nWSR );
+
+	for i=1:nP
+		%disp(i);
+
+		if ( i == 1 )
+			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		else
+			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		end
+
+		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
+		maxViolation = max( [maxViolation,maxViolationTMP] );
+
+        xOpt(:,i) = x;
+        yOpt(:,i) = lambda;
+        objOpt(:,i) = obj;
+	end
+	
+	qpOASES_sequence( 'c',QP );
+	
+    if ( ( maxViolation < 1e-13 ) && ( status == 0 ) )
+        successFlag = 1;
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1A.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1A.m
new file mode 100644
index 00000000000..22811e5dc80
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1A.m
@@ -0,0 +1,52 @@
+function [ successFlag ] = runBenchmarkEXAMPLE1A( nWSR )
+
+    successFlag = 0;
+    maxViolation = 0;
+
+    clear H g A lb ub lbA ubA;
+	
+    try
+        load 'benchmarkEXAMPLE1A.mat';
+    catch
+        successFlag = -1;
+        return;
+    end
+
+    if ( exist( 'A','var' ) )
+        [nC,nV] = size(A);
+    else
+        nC = 0;
+    end
+    [nV,nP] = size(g);
+
+    xOpt = zeros(nV,nP);
+    yOpt = zeros(nV+nC,nP);
+    objOpt = zeros(1,nP);
+    
+	%options = qpOASES_options( 'maxIter',nWSR );
+    options = qpOASES_options( 'fast','maxIter',nWSR );
+
+	for i=1:2
+		%disp(i);
+
+		if ( i == 1 )
+			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H(:,:,i),g(:,i),A(:,:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		else
+			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'm',QP,H(:,:,i),g(:,i),A(:,:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		end
+
+		[ maxViolationTMP ] = getKktResidual( H(:,:,i),g(:,i),A(:,:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
+		maxViolation = max( [maxViolation,maxViolationTMP] );
+
+        xOpt(:,i) = x;
+        yOpt(:,i) = lambda;
+        objOpt(:,i) = obj;
+	end
+	
+	qpOASES_sequence( 'c',QP );
+
+    if ( ( maxViolation < 3e-13 ) && ( status == 0 ) )
+        successFlag = 1;
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1B.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1B.m
new file mode 100644
index 00000000000..74fc36f5317
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1B.m
@@ -0,0 +1,52 @@
+function [ successFlag ] = runBenchmarkEXAMPLE1B( nWSR )
+
+    successFlag = 0;
+	maxViolation = 0;
+
+    clear H g A lb ub lbA ubA;
+	
+    try
+        load 'benchmarkEXAMPLE1B.mat';
+    catch
+        successFlag = -1;
+        return;
+    end
+	
+    if ( exist( 'A','var' ) )
+        [nC,nV] = size(A);
+    else
+        nC = 0;
+    end
+    [nV,nP] = size(g);
+
+    xOpt = zeros(nV,nP);
+    yOpt = zeros(nV+nC,nP);
+    objOpt = zeros(1,nP);
+    
+	%options = qpOASES_options( 'maxIter',nWSR );
+    options = qpOASES_options( 'fast','maxIter',nWSR );
+
+	for i=1:nP
+		%disp(i);
+
+		if ( i == 1 )
+			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),lb(:,i),ub(:,i),options );
+		else
+			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),options );
+		end
+
+		[ maxViolationTMP ] = getKktResidual( H,g(:,i),[],lb(:,i),ub(:,i),[],[], x,lambda );
+		maxViolation = max( [maxViolation,maxViolationTMP] );
+
+        xOpt(:,i) = x;
+        yOpt(:,i) = lambda;
+        objOpt(:,i) = obj;
+	end
+	
+	qpOASES_sequence( 'c',QP );
+
+    if ( ( maxViolation < 3e-12 ) && ( status == 0 ) )
+        successFlag = 1;
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkIDHESSIAN1.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkIDHESSIAN1.m
new file mode 100644
index 00000000000..39920038a40
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkIDHESSIAN1.m
@@ -0,0 +1,55 @@
+function [ successFlag ] = runBenchmarkIDHESSIAN1( nWSR )
+
+    successFlag = 0;
+	maxViolation = 0;
+
+    clear H g A lb ub lbA ubA;
+	
+    try
+        load 'benchmarkIDHESSIAN1.mat';
+    catch
+        successFlag = -1;
+        return;
+    end
+	
+    if ( exist( 'A','var' ) )
+        [nC,nV] = size(A);
+    else
+        nC = 0;
+    end
+    [nV,nP] = size(g);
+
+    xOpt = zeros(nV,nP);
+    yOpt = zeros(nV+nC,nP);
+    objOpt = zeros(1,nP);
+       
+	options = qpOASES_options( 'maxIter',nWSR, 'printLevel',-2 );
+    %options = qpOASES_options( 'fast','maxIter',nWSR, 'printLevel',2 );
+    
+
+	for i=1:nP
+		%disp(i);
+
+		if ( i == 1 )
+			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i)-1e-11,ubA(:,i)+1e-11,options );
+			%disp(status);
+		else
+			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i)-1e-11,ubA(:,i)+1e-11,options );
+			%disp(status);
+		end
+
+		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
+		maxViolation = max( [maxViolation,maxViolationTMP] );
+
+        xOpt(:,i) = x;
+        yOpt(:,i) = lambda;
+        objOpt(:,i) = obj;
+	end
+	
+	qpOASES_sequence( 'c',QP );
+
+    if ( ( maxViolation < 8e-6 ) && ( status == 0 ) )
+        successFlag = 1;
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runInterfaceSeqTest.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runInterfaceSeqTest.m
new file mode 100644
index 00000000000..a7427c9f4c6
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runInterfaceSeqTest.m
@@ -0,0 +1,428 @@
+function [ successFlag ] = runInterfaceSeqTest( nV,nC, doPrint,seed )
+
+    if ( nargin < 4 )
+        seed = 42;
+        if ( nargin < 3 )
+            doPrint = 1;
+            if ( nargin < 2 )
+                nC = 10;
+                if ( nargin < 1 )
+                    nV = 5;
+                end
+            end
+        end
+    end
+    
+    successFlag = 1;
+
+    for isSparseH=0:1
+        for isSparseA=0:1
+            successFlag = runSeveralInterfaceSeqTests( successFlag, nV,nC,isSparseH,isSparseA, doPrint,seed );
+        end
+    end
+    
+end
+
+
+
+function [ successFlag ] = runSeveralInterfaceSeqTests( successFlag, nV,nC, isSparseH,isSparseA, doPrint,seed )
+
+    %% test without or empty A matrix
+    for hasA=0:1
+        for changeMat=0:hasA % cannot change matrices if QProblemB object is instantiated
+            for hasLowerB=0:1
+                for hasUpperB=0:1
+                    for hasOptions=0:2
+                        for hasX0=0:1
+                            for hasWS=0:2
+                                curSuccessFLAG = runSingleInterfaceSeqTest( nV,0,hasA,isSparseH,isSparseA, hasLowerB,hasUpperB,0,0,hasOptions,hasX0,hasWS,changeMat, doPrint,seed );
+                                successFlag = min( successFlag,curSuccessFLAG );
+                            end
+                        end
+                    end
+                end
+            end
+        end
+    end
+    
+    %% test with non-empty A matrix
+    for hasLowerB=0:1
+        for hasUpperB=0:1
+            for hasLowerC=0:1
+                for hasUpperC=0:1
+                    for hasOptions=0:2
+                        for hasX0=0:1
+                            for hasWS=0:2
+                                for changeMat=0:1
+                                    curSuccessFLAG = runSingleInterfaceSeqTest( nV,nC,1,isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC,hasOptions,hasX0,hasWS,changeMat, doPrint,seed );
+                                    successFlag = min( successFlag,curSuccessFLAG );
+                                end
+                            end
+                        end
+                    end
+                end
+            end
+        end
+    end
+    
+end
+
+
+function [ successFlag ] = runSingleInterfaceSeqTest( nV,nC,hasA,isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC,hasOptions,hasX0,hasWS,changeMat, doPrint,seed )
+
+    successFlag = 0;
+
+    qpData1 = generateExample( nV,nC, isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC, seed );
+    if ( changeMat > 0 )
+        qpData2 = generateExample( nV,nC, isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC, seed+1 );
+    else
+        qpData2 = generateExample( nV,nC, isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC, seed+1,qpData1.H,qpData1.Ain );
+    end
+
+    if ( changeMat == 0 )
+        string = 'Testing qpOASES_sequence( ''i/h/c'',H';
+    else
+        string = 'Testing qpOASES_sequence( ''i/m/c'',H';
+    end
+    
+    if ( isSparseH > 0 )
+        string = [string,'s,g'];
+    else
+        string = [string,'d,g'];
+    end
+    
+    if ( nC > 0 )
+        if ( isSparseA > 0 )
+            string = [string,',As'];
+        else
+            string = [string,',Ad'];
+        end
+    else
+        if ( hasA > 0 )
+            string = [string,',[]'];
+        end
+    end
+    
+    if ( hasLowerB > 0 )
+        string = [string,',lb'];
+    else
+        string = [string,',[]'];
+    end
+    
+    if ( hasUpperB > 0 )
+        string = [string,',ub'];
+    else
+        string = [string,',[]'];
+    end
+    
+    if ( hasLowerC > 0 )
+        string = [string,',lbA'];
+    else
+        if ( hasA > 0 )
+            string = [string,',[] '];
+        end
+    end
+    
+    if ( hasUpperC > 0 )
+        string = [string,',ubA'];
+    else
+        if ( hasA > 0 )
+            string = [string,',[] '];
+        end
+    end
+    
+    switch ( hasOptions )
+        case 1
+            string = [string,',opt'];
+        
+        case 2
+            string = [string,',[] '];
+            
+        case 0
+            if ( ( hasX0 > 0 ) || ( hasWS > 0 ) )
+                string = [string,',[] '];
+            end
+    end
+
+    switch ( hasX0 )
+        case 1
+            string = [string,',{x0'];
+        
+        case 2
+            string = [string,',{[]'];
+            
+        case 0
+            if ( hasWS > 0 )
+                string = [string,',{[]'];
+            end
+    end
+    
+    switch ( hasWS )
+        case 1
+            string = [string,',WS}'];
+        
+        case 2
+            string = [string,',[]}'];
+    end
+
+    string = [string,' )... '];
+    if ( doPrint > 0 )
+        %disp( string );
+    end
+    
+    curSuccessFlag = callQpOasesSeq( qpData1,qpData2,hasA,hasOptions,hasX0,hasWS,changeMat );
+    if ( curSuccessFlag > 0 )
+        string = [string,'pass!'];
+        successFlag = 1;
+    else
+        string = [string,'fail!'];
+    end
+    
+    if ( doPrint > 0 )
+        disp( string );
+        if ( curSuccessFlag == 0 )
+            pause(0.1);
+        end
+    end
+    
+end
+
+function [ successFlag ] = callQpOasesSeq( qpData1,qpData2,hasA,hasOptions,hasX0,hasWS,changeMat, doPrint )
+
+    if ( nargin < 8 )
+        doPrint = 1;
+    end
+
+    %TOL = 1e-15;
+    KKTTOL = 1e-6;
+
+    successFlag = 0;
+    
+    H1 = qpData1.H;
+	g1 = qpData1.g;
+    A1 = [qpData1.Aeq;qpData1.Ain];
+	lb1 = qpData1.lb;
+    ub1 = qpData1.ub;
+    lbA1 = [qpData1.beq;qpData1.lbA];
+    ubA1 = [qpData1.beq;qpData1.ubA];
+    
+    if ( changeMat > 0 )
+        H2 = qpData2.H;
+    else
+        H2 = H1;
+    end
+	g2 = qpData2.g;
+    if ( changeMat > 0 )
+        A2 = [qpData2.Aeq;qpData2.Ain];
+    else
+        A2 = A1;
+    end
+	lb2 = qpData2.lb;
+    ub2 = qpData2.ub;
+    lbA2 = [qpData2.beq;qpData2.lbA];
+    ubA2 = [qpData2.beq;qpData2.ubA];
+
+    [nV,dummy] = size(H1);
+    [nC,dummy] = size(A1);
+    
+    if ( hasWS > 0 )
+        if ( hasWS == 1 )
+            WS = 0 * ones( nV+nC,1 );
+        else
+            WS = [];
+        end
+        
+        if ( hasX0 > 0 )
+            if ( hasX0 == 1 )
+                x0 = -1e-3 * ones( nV,1 );
+            else
+                x0 = [];
+            end
+            
+            auxInput = qpOASES_auxInput( 'x0',x0,'guessedWorkingSet',WS );
+
+            if ( hasOptions > 0 )
+                if ( hasOptions == 1 )
+                    options = qpOASES_options();
+                else
+                    options = [];
+                end
+
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
+                    qpOASES_sequence( 'c',QP );
+                end
+            else
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
+                    qpOASES_sequence( 'c',QP );
+                end
+            end
+
+        else % hasX0 == 0
+
+            auxInput = qpOASES_auxInput( 'guessedWorkingSet',WS );
+            
+            if ( hasOptions > 0 )
+                if ( hasOptions == 1 )
+                    options = qpOASES_options();
+                else
+                    options = [];
+                end
+
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
+                    qpOASES_sequence( 'c',QP );
+                end
+            else
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,[] );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,[] );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
+                    qpOASES_sequence( 'c',QP );
+                end
+            end
+
+        end % hasX0
+        
+    else % hasWS == 0
+        
+        if ( hasX0 > 0 )
+            if ( hasX0 == 1 )
+                x0 = -1e-3 * ones( nV,1 );
+            else
+                x0 = [];
+            end
+            
+            auxInput = qpOASES_auxInput( 'x0',x0 );
+
+            if ( hasOptions > 0 )
+                if ( hasOptions == 1 )
+                    options = qpOASES_options();
+                else
+                    options = [];
+                end
+
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
+                    qpOASES_sequence( 'c',QP );
+                end
+                    
+            else
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
+                    qpOASES_sequence( 'c',QP );
+                end
+            end
+
+        else % hasX0 == 0
+
+            if ( hasOptions > 0 )
+                if ( hasOptions == 1 )
+                    options = qpOASES_options();
+                else
+                    options = [];
+                end
+
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
+                    qpOASES_sequence( 'c',QP );
+                end
+            else
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1 );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1 );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
+                    qpOASES_sequence( 'c',QP );
+                end
+            end
+
+        end % hasX0
+
+    end % hasWS
+
+
+    kktTol1 = getKktResidual( H1,g1,A1,lb1,ub1,lbA1,ubA1, x1,l1 );
+    kktTol2 = getKktResidual( H2,g2,A2,lb2,ub2,lbA2,ubA2, x2,l2 );
+    
+    if ( ( kktTol1 <= KKTTOL ) && ( e1 >= 0 ) && ( kktTol2 <= KKTTOL ) && ( e2 >= 0 ) )
+        successFlag = 1;
+    else
+        if ( doPrint > 0 )
+            if ( ( kktTol1 > KKTTOL ) || ( kktTol2 > KKTTOL ) )
+                disp('kkt error')
+            else
+                disp('exitflag<0')
+            end
+        end
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runInterfaceTest.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runInterfaceTest.m
new file mode 100644
index 00000000000..2f97eda6b2f
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runInterfaceTest.m
@@ -0,0 +1,459 @@
+function [ successFlag ] = runInterfaceTest( nV,nC, doPrint,seed )
+
+    if ( nargin < 4 )
+        seed = 42;
+        if ( nargin < 3 )
+            doPrint = 1;
+            if ( nargin < 2 )
+                nC = 10;
+                if ( nargin < 1 )
+                    nV = 5;
+                end
+            end
+        end
+    end
+    
+    successFlag = 1;
+
+    for isSparseH=0:1
+        for isSparseA=0:1
+            successFlag = runSeveralInterfaceTests( successFlag, nV,nC,isSparseH,isSparseA, doPrint,seed );
+        end
+    end
+    
+end
+
+
+
+function [ successFLAG ] = runSeveralInterfaceTests( successFLAG, nV,nC, isSparseH,isSparseA, doPrint,seed )
+
+    %% test without or empty A matrix
+    for hasA=0:1
+        for hasLowerB=0:1
+            for hasUpperB=0:1
+                for hasOptions=0:2
+                    for hasX0=0:1
+                        for hasWS=0:2
+                            if ( ( hasWS ~= 2 ) || ( hasA ~= 0 ) || ( hasOptions == 1 ) || ( hasX0 ~= 0 ) )
+                                curSuccessFLAG = runSingleInterfaceTest( nV,0,hasA,isSparseH,isSparseA, hasLowerB,hasUpperB,0,0,hasOptions,hasX0,hasWS, doPrint,seed );
+                                successFLAG = min( successFLAG,curSuccessFLAG );
+                            end
+                        end
+                    end
+                end
+            end
+        end
+    end
+    
+    %% test with non-empty A matrix
+    for hasLowerB=0:1
+        for hasUpperB=0:1
+            for hasLowerC=0:1
+                for hasUpperC=0:1
+                    for hasOptions=0:2
+                        for hasX0=0:1
+                            for hasWS=0:2
+                                curSuccessFLAG = runSingleInterfaceTest( nV,nC,1,isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC,hasOptions,hasX0,hasWS, doPrint,seed );
+                                successFLAG = min( successFLAG,curSuccessFLAG );
+                            end
+                        end
+                    end
+                end
+            end
+        end
+    end
+    
+end
+
+
+function [ successFLAG ] = runSingleInterfaceTest( nV,nC,hasA,isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC,hasOptions,hasX0,hasWS, doPrint,seed )
+
+    successFLAG = 0;
+
+    qpData = generateExample( nV,nC, isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC, seed );
+
+    string = 'Testing qpOASES( H';
+    
+    if ( isSparseH > 0 )
+        string = [string,'s,g'];
+    else
+        string = [string,'d,g'];
+    end
+    
+    if ( nC > 0 )
+        if ( isSparseA > 0 )
+            string = [string,',As'];
+        else
+            string = [string,',Ad'];
+        end
+    else
+        if ( hasA > 0 )
+            string = [string,',[]'];
+        end
+    end
+    
+    if ( hasLowerB > 0 )
+        string = [string,',lb'];
+    else
+        string = [string,',[]'];
+    end
+    
+    if ( hasUpperB > 0 )
+        string = [string,',ub'];
+    else
+        string = [string,',[]'];
+    end
+    
+    if ( hasLowerC > 0 )
+        string = [string,',lbA'];
+    else
+        if ( hasA > 0 )
+            string = [string,',[] '];
+        end
+    end
+    
+    if ( hasUpperC > 0 )
+        string = [string,',ubA'];
+    else
+        if ( hasA > 0 )
+            string = [string,',[] '];
+        end
+    end
+    
+    switch ( hasOptions )
+        case 1
+            string = [string,',opt'];
+        
+        case 2
+            string = [string,',[] '];
+            
+        case 0
+            if ( ( hasX0 > 0 ) || ( hasWS > 0 ) )
+                string = [string,',[] '];
+            end
+    end
+
+    switch ( hasX0 )
+        case 1
+            string = [string,',{x0'];
+        
+        case 2
+            string = [string,',{[]'];
+            
+        case 0
+            if ( hasWS > 0 )
+                string = [string,',{[]'];
+            end
+    end
+    
+    switch ( hasWS )
+        case 1
+            string = [string,',WS}'];
+        
+        case 2
+            string = [string,',[]}'];
+    end
+
+    string = [string,' )... '];
+    if ( doPrint > 0 )
+        %disp( string );
+    end
+    
+    curSuccessFlag = callQpOases( qpData,hasA,hasOptions,hasX0,hasWS, 1 );
+    if ( curSuccessFlag > 0 )
+        string = [string,'pass!'];
+        successFLAG = 1;
+    else
+        string = [string,'fail!'];
+    end
+    
+    if ( doPrint > 0 )
+        disp( string );
+        if ( curSuccessFlag == 0 )
+            pause;
+        end
+    end
+    
+end
+
+function [ successFLAG ] = callQpOases( qpData,hasA,hasOptions,hasX0,hasWS, doPrint )
+
+    if ( nargin < 6 )
+        doPrint = 1;
+    end
+
+    TOL = 1e-15;
+    KKTTOL = 1e-6;
+
+    successFLAG = 0;
+    
+    H = qpData.H;
+	g = qpData.g;
+    A = [qpData.Aeq;qpData.Ain];
+	lb = qpData.lb;
+    ub = qpData.ub;
+    lbA = [qpData.beq;qpData.lbA];
+    ubA = [qpData.beq;qpData.ubA];
+
+    [nV,dummy] = size(H);
+    [nC,dummy] = size(A);
+    
+    if ( hasWS > 0 )
+        if ( hasWS == 1 )
+            WS = 0 * ones( nV+nC,1 );
+        else
+            WS = [];
+        end
+        
+        if ( hasX0 > 0 )
+            if ( hasX0 == 1 )
+                x0 = -1e-3 * ones( nV,1 );
+            else
+                x0 = [];
+            end
+            
+            auxInput = qpOASES_auxInput( 'x0',x0,'guessedWorkingSet',WS );
+
+            if ( hasOptions > 0 )
+                if ( hasOptions == 1 )
+                    options = qpOASES_options();
+                else
+                    options = [];
+                end
+
+                if ( hasA > 0 )
+                    [ x1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                    [ x2,f2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                    [ x3,f3,e3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                    [ x4,f4,e4,i4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                else                        
+                    [ x1 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                    [ x2,f2 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                    [ x3,f3,e3 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                    [ x4,f4,e4,i4 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                end
+            else
+                if ( hasA > 0 )
+                    [ x1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                    [ x2,f2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                    [ x3,f3,e3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                    [ x4,f4,e4,i4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                else
+                    [ x1 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                    [ x2,f2 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                    [ x3,f3,e3 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                    [ x4,f4,e4,i4 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                end
+            end
+
+        else % hasX0 == 0
+
+            auxInput = qpOASES_auxInput( 'guessedWorkingSet',WS );
+            
+            if ( hasOptions > 0 )
+                if ( hasOptions == 1 )
+                    options = qpOASES_options();
+                else
+                    options = [];
+                end
+                
+                if ( hasA > 0 )
+                    [ x1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                    [ x2,f2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                    [ x3,f3,e3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                    [ x4,f4,e4,i4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                else
+                    [ x1 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                    [ x2,f2 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                    [ x3,f3,e3 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                    [ x4,f4,e4,i4 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                end
+            else
+                if ( hasA > 0 )
+                    [ x1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                    [ x2,f2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                    [ x3,f3,e3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                    [ x4,f4,e4,i4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                else
+                    [ x1 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                    [ x2,f2 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                    [ x3,f3,e3 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                    [ x4,f4,e4,i4 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                end
+            end
+
+        end % hasX0
+        
+    else % hasWS == 0
+        
+        if ( hasX0 > 0 )
+            if ( hasX0 == 1 )
+                x0 = -1e-3 * ones( nV,1 );
+            else
+                x0 = [];
+            end
+            
+            auxInput = qpOASES_auxInput( 'x0',x0 );
+
+            if ( hasOptions > 0 )
+                if ( hasOptions == 1 )
+                    options = qpOASES_options();
+                else
+                    options = [];
+                end
+
+                if ( hasA > 0 )
+                    [ x1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                    [ x2,f2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                    [ x3,f3,e3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                    [ x4,f4,e4,i4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
+                else
+                    [ x1 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                    [ x2,f2 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                    [ x3,f3,e3 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                    [ x4,f4,e4,i4 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,lb,ub,options,auxInput );
+                end
+                    
+            else
+                if ( hasA > 0 )
+                    [ x1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                    [ x2,f2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                    [ x3,f3,e3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                    [ x4,f4,e4,i4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
+                else
+                    [ x1 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                    [ x2,f2 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                    [ x3,f3,e3 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                    [ x4,f4,e4,i4 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,lb,ub,[],auxInput );
+                end
+            end
+
+        else % hasX0 == 0
+            
+            if ( hasOptions > 0 )
+                if ( hasOptions == 1 )
+                    options = qpOASES_options();
+                else
+                    options = [];
+                end
+
+                if ( hasA > 0 )
+                    [ x1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
+                    [ x2,f2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
+                    [ x3,f3,e3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
+                    [ x4,f4,e4,i4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
+                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
+                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
+                else
+                    [ x1 ] = qpOASES( H,g,lb,ub,options );
+                    [ x2,f2 ] = qpOASES( H,g,lb,ub,options );
+                    [ x3,f3,e3 ] = qpOASES( H,g,lb,ub,options );
+                    [ x4,f4,e4,i4 ] = qpOASES( H,g,lb,ub,options );
+                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,lb,ub,options );
+                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,lb,ub,options );
+                end
+            else
+                if ( hasA > 0 )
+                    [ x1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA );
+                    [ x2,f2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA );
+                    [ x3,f3,e3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA );
+                    [ x4,f4,e4,i4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA );
+                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,A,lb,ub,lbA,ubA );
+                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,A,lb,ub,lbA,ubA );
+                else
+                    [ x1 ] = qpOASES( H,g,lb,ub );
+                    [ x2,f2 ] = qpOASES( H,g,lb,ub );
+                    [ x3,f3,e3 ] = qpOASES( H,g,lb,ub );
+                    [ x4,f4,e4,i4 ] = qpOASES( H,g,lb,ub );
+                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,lb,ub );
+                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,lb,ub );
+                end
+            end
+
+        end % hasX0
+
+    end % hasWS
+
+    % check whether all calls lead to same optimal solution
+    % independent from output arguments
+    if ( ( norm(x1-x2) > TOL ) || ...
+         ( norm(x1-x3) > TOL ) || ...
+         ( norm(x1-x4) > TOL ) || ...
+         ( norm(x1-x5) > TOL ) || ...
+         ( norm(x1-x6) > TOL ) )
+        if ( doPrint > 0 )
+            disp('diff in x')
+        end
+        return;
+    end
+    
+    if ( ( norm(f2-f3) > TOL ) || ...
+         ( norm(f2-f4) > TOL ) || ...
+         ( norm(f2-f5) > TOL ) || ...
+         ( norm(f2-f6) > TOL ) )
+        if ( doPrint > 0 )
+            disp('diff in fval')
+        end
+        return;
+    end
+
+    if ( ( norm(e3-e4) > TOL ) || ...
+         ( norm(e3-e5) > TOL ) || ...
+         ( norm(e3-e6) > TOL ) )
+        if ( doPrint > 0 )
+            disp('diff in exitflag')
+        end
+        return;
+    end
+
+    if ( ( norm(i4-i5) > TOL ) || ...
+         ( norm(i4-i6) > TOL ) )
+        if ( doPrint > 0 )
+            disp('diff in iter')
+        end
+        return;
+    end
+    
+    if ( norm(l5-l6) > TOL )
+        if ( doPrint > 0 )
+            disp('diff in lambda')
+        end
+        return;
+    end
+    
+    
+    kktTol = getKktResidual( H,g,A,lb,ub,lbA,ubA, x6,l6 );
+    
+    if ( ( kktTol <= KKTTOL ) && ( e6 >= 0 ) )
+        successFLAG = 1;
+    else
+        if ( doPrint > 0 )
+            disp('error')
+        end
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runQAP8.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runQAP8.m
new file mode 100644
index 00000000000..0b3646f7b49
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runQAP8.m
@@ -0,0 +1,36 @@
+function [ successFlag ] = runQAP8( doPrint )
+    
+    if ( nargin < 1 )
+        doPrint = 0;
+    end
+
+    successFlag = 0;
+
+    try
+        load 'QAP8.mat';
+    catch
+        successFlag = -1;
+        return;
+    end
+
+    options = qpOASES_options('default', 'maxIter',30000, 'printLevel',-2*doPrint );
+    tic
+    [x, fval, status, nWSRout, y] = qpOASES(sparse(QP.H), QP.f, ...
+        sparse(QP.C), QP.lb, QP.ub, QP.cl, QP.cu, options);
+    t = toc;
+    
+    if ( doPrint > 0 )
+        disp( ['solution time: ',num2str(t),' seconds'] );
+    end
+    
+    if ( ( status == 0 ) && ( nWSRout < 23200 ) )
+        successFlag = 1;
+    end
+    
+    return;    
+    % check error and print
+    [stat, feas, cmpl] = qpresidual(S.B, S.b1, S.C, S.cl1, S.cu1, x, -y);
+    fprintf('%d iters in %.3fs to tolerance %.2e\n', nWSRout, t, stat + feas + cmpl)
+    fprintf('Status: %d\n', status)
+
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runQSHARE1B.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runQSHARE1B.m
new file mode 100644
index 00000000000..b915683666d
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runQSHARE1B.m
@@ -0,0 +1,43 @@
+function [ successFlag ] = runQSHARE1B( doPrint )
+    
+    if ( nargin < 1 )
+        doPrint = 0;
+    end
+
+    successFlag = 0;
+
+    try
+        load 'QSHARE1B.mat';
+    catch
+        successFlag = -1;
+        return;
+    end
+    
+    options = qpOASES_options('default', 'maxIter',550, 'maxCpuTime',2.0, 'printLevel',0 );
+    tic
+    [xD,fvalD,exitflagD,iterD,lambdaD] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
+    tD = toc;
+    kktD = getKktResidual( H,g,A,lb,ub,lbA,ubA, xD,lambdaD );
+    
+    if ( doPrint > 0 )
+        disp( ['dense kkt tol:        ',num2str(kktD, '%.3e')] );
+        disp( ['dense solution time:  ',num2str(tD),' seconds'] );
+		disp( ['dense #iterations:    ',num2str(iterD),] );
+    end
+    
+    tic
+    [xS,fvalS,exitflagS,iterS,lambdaS] = qpOASES( sparse(H),g,sparse(A),lb,ub,lbA,ubA,options );
+    tS = toc;
+    kktS = getKktResidual( H,g,A,lb,ub,lbA,ubA, xS,lambdaS );
+    
+    if ( doPrint > 0 )
+        disp( ['sparse kkt tol:       ',num2str(kktS, '%.3e')] );
+        disp( ['sparse solution time: ',num2str(tS),' seconds'] );
+		disp( ['sparse #iterations:   ',num2str(iterS),] );
+    end
+    
+    if ( ( exitflagD == 0 ) && ( kktD < 1e-9 ) && ( exitflagS == 0 ) && ( kktS < 1e-9 ) )
+        successFlag = 1;
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runRandomIdHessian.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runRandomIdHessian.m
new file mode 100644
index 00000000000..48f9c2c0c31
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runRandomIdHessian.m
@@ -0,0 +1,428 @@
+function [ successFlag ] = runRandomIdHessian( nV,nC, doPrint,seed )
+
+    if ( nargin < 4 )
+        seed = 42;
+        if ( nargin < 3 )
+            doPrint = 1;
+            if ( nargin < 2 )
+                nC = 10;
+                if ( nargin < 1 )
+                    nV = 5;
+                end
+            end
+        end
+    end
+    
+    successFlag = 1;
+
+    for isSparseH=1:1
+        for isSparseA=1:1
+            successFlag = runSeveralIdSeqTests( successFlag, nV,nC,isSparseH,isSparseA, doPrint,seed );
+        end
+    end
+    
+end
+
+
+function [ successFlag ] = runSeveralIdSeqTests( successFlag, nV,nC, isSparseH,isSparseA, doPrint,seed )
+
+    %% test without or empty A matrix
+    for hasA=0:1
+        for changeMat=0:hasA % cannot change matrices if QProblemB object is instantiated
+            for hasLowerB=0:1
+                for hasUpperB=0:1
+                    for hasOptions=0:1
+                        for hasX0=0:1
+                            for hasWS=0:2
+                                curSuccessFLAG = runSingleIdSeqTest( nV,0,hasA,isSparseH,isSparseA, hasLowerB,hasUpperB,0,0,hasOptions,hasX0,hasWS,changeMat, doPrint,seed );
+                                successFlag = min( successFlag,curSuccessFLAG );
+                            end
+                        end
+                    end
+                end
+            end
+        end
+    end
+    
+    %% test with non-empty A matrix
+    for hasLowerB=0:1
+        for hasUpperB=0:1
+            for hasLowerC=0:1
+                for hasUpperC=0:1
+                    for hasOptions=0:1
+                        for hasX0=0:1
+                            for hasWS=0:2
+                                for changeMat=0:1
+                                    curSuccessFLAG = runSingleIdSeqTest( nV,nC,1,isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC,hasOptions,hasX0,hasWS,changeMat, doPrint,seed );
+                                    successFlag = min( successFlag,curSuccessFLAG );
+                                end
+                            end
+                        end
+                    end
+                end
+            end
+        end
+    end
+    
+end
+
+
+function [ successFlag ] = runSingleIdSeqTest( nV,nC,hasA,isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC,hasOptions,hasX0,hasWS,changeMat, doPrint,seed )
+
+    successFlag = 0;
+
+    qpFeatures = setupQpFeaturesStruct( );
+    
+    qpFeatures.nV = nV;
+    qpFeatures.nC = nC;
+    
+    qpFeatures.isSparseH = isSparseH;
+    qpFeatures.isSparseA = isSparseA;
+    
+    qpFeatures.hasLowerB = hasLowerB;
+    qpFeatures.hasUpperB = hasUpperB;
+    qpFeatures.hasLowerC = hasLowerC;
+    qpFeatures.hasUpperC = hasUpperC;
+    
+    qpFeatures.hessianType = 2;
+    
+    qpData1 = generateRandomQp( qpFeatures,seed );
+    qpData2 = generateRandomQp( qpFeatures,seed );
+
+    if ( changeMat == 0 )
+        string = 'Testing qpOASES_sequence( ''i/h/c'',ID';
+    else
+        string = 'Testing qpOASES_sequence( ''i/m/c'',ID';
+    end
+    
+    if ( isSparseH > 0 )
+        string = [string,'s,g'];
+    else
+        string = [string,'d,g'];
+    end
+    
+    if ( nC > 0 )
+        if ( isSparseA > 0 )
+            string = [string,',As'];
+        else
+            string = [string,',Ad'];
+        end
+    else
+        if ( hasA > 0 )
+            string = [string,',[]'];
+        end
+    end
+    
+    if ( hasLowerB > 0 )
+        string = [string,',lb'];
+    else
+        string = [string,',[]'];
+    end
+    
+    if ( hasUpperB > 0 )
+        string = [string,',ub'];
+    else
+        string = [string,',[]'];
+    end
+    
+    if ( hasLowerC > 0 )
+        string = [string,',lbA'];
+    else
+        if ( hasA > 0 )
+            string = [string,',[] '];
+        end
+    end
+    
+    if ( hasUpperC > 0 )
+        string = [string,',ubA'];
+    else
+        if ( hasA > 0 )
+            string = [string,',[] '];
+        end
+    end
+    
+    switch ( hasOptions )
+        case 1
+            string = [string,',opt'];
+        
+        case 2
+            string = [string,',[] '];
+            
+        case 0
+            if ( ( hasX0 > 0 ) || ( hasWS > 0 ) )
+                string = [string,',[] '];
+            end
+    end
+
+    switch ( hasX0 )
+        case 1
+            string = [string,',{x0'];
+        
+        case 2
+            string = [string,',{[]'];
+            
+        case 0
+            if ( hasWS > 0 )
+                string = [string,',{[]'];
+            end
+    end
+    
+    switch ( hasWS )
+        case 1
+            string = [string,',WS}'];
+        
+        case 2
+            string = [string,',[]}'];
+    end
+
+    string = [string,' )... '];
+    if ( doPrint > 0 )
+        %disp( string );
+    end
+    
+    curSuccessFlag = callQpOasesSeq( qpData1,qpData2,hasA,hasOptions,hasX0,hasWS,changeMat,doPrint );
+    if ( curSuccessFlag > 0 )
+        string = [string,'pass!'];
+        successFlag = 1;
+    else
+        string = [string,'fail!'];
+    end
+    
+    if ( doPrint > 0 )
+        disp( string );
+        if ( curSuccessFlag == 0 )
+            pause;
+        end
+    end
+    
+end
+
+
+
+function [ successFlag ] = callQpOasesSeq( qpData1,qpData2,hasA,hasOptions,hasX0,hasWS,changeMat, doPrint )
+
+    if ( nargin < 8 )
+        doPrint = 1;
+    end
+
+    TOL = 1e-15;
+    KKTTOL = 1e-6;
+
+    successFlag = 0;
+    
+    H1 = qpData1.H;
+	g1 = qpData1.g;
+    A1 = [qpData1.Aeq;qpData1.Ain];
+	lb1 = qpData1.lb;
+    ub1 = qpData1.ub;
+    lbA1 = [qpData1.beq;qpData1.lbA];
+    ubA1 = [qpData1.beq;qpData1.ubA];
+    
+    H2 = qpData2.H;
+	g2 = qpData2.g;
+    A2 = [qpData2.Aeq;qpData2.Ain];
+	lb2 = qpData2.lb;
+    ub2 = qpData2.ub;
+    lbA2 = [qpData2.beq;qpData2.lbA];
+    ubA2 = [qpData2.beq;qpData2.ubA];
+
+    [nV,dummy] = size(H1);
+    [nC,dummy] = size(A1);
+    
+    if ( hasWS > 0 )
+        if ( hasWS == 1 )
+            WS = 0 * ones( nV+nC,1 );
+        else
+            WS = [];
+        end
+        
+        if ( hasX0 > 0 )
+            if ( hasX0 == 1 )
+                x0 = -1e-3 * ones( nV,1 );
+            else
+                x0 = [];
+            end
+            
+            auxInput = qpOASES_auxInput( 'x0',x0,'guessedWorkingSet',WS );
+
+            if ( hasOptions > 0 )
+                if ( hasOptions == 1 )
+                    options = qpOASES_options();
+                else
+                    options = [];
+                end
+
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else                        
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
+                    qpOASES_sequence( 'c',QP );
+                end
+            else
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
+                    qpOASES_sequence( 'c',QP );
+                end
+            end
+
+        else % hasX0 == 0
+            
+            auxInput = qpOASES_auxInput( 'guessedWorkingSet',WS );
+
+            if ( hasOptions > 0 )
+                if ( hasOptions == 1 )
+                    options = qpOASES_options();
+                else
+                    options = [];
+                end
+
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
+                    qpOASES_sequence( 'c',QP );
+                end
+            else
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,[] );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,[] );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
+                    qpOASES_sequence( 'c',QP );
+                end
+            end
+
+        end % hasX0
+        
+    else % hasWS == 0
+        
+        if ( hasX0 > 0 )
+            if ( hasX0 == 1 )
+                x0 = -1e-3 * ones( nV,1 );
+            else
+                x0 = [];
+            end
+            
+            auxInput = qpOASES_auxInput( 'x0',x0 );
+
+            if ( hasOptions > 0 )
+                if ( hasOptions == 1 )
+                    options = qpOASES_options();
+                else
+                    options = [];
+                end
+
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
+                    qpOASES_sequence( 'c',QP );
+                end
+                    
+            else
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
+                    qpOASES_sequence( 'c',QP );
+                end
+            end
+
+        else % hasX0 == 0
+
+            if ( hasOptions > 0 )
+                if ( hasOptions == 1 )
+                    options = qpOASES_options();
+                else
+                    options = [];
+                end
+
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
+                    qpOASES_sequence( 'c',QP );
+                end
+            else
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1 );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1 );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
+                    qpOASES_sequence( 'c',QP );
+                end
+            end
+
+        end % hasX0
+
+    end % hasWS
+
+
+    kktTol1 = getKktResidual( H1,g1,A1,lb1,ub1,lbA1,ubA1, x1,l1 );
+    kktTol2 = getKktResidual( H1,g1,A1,lb1,ub1,lbA1,ubA1, x2,l2 );
+    
+    if ( ( kktTol1 <= KKTTOL ) && ( e1 >= 0 ) && ( kktTol2 <= KKTTOL ) && ( e2 >= 0 ) )
+        successFlag = 1;
+    else
+        if ( doPrint > 1 )
+            disp('error')
+        end
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runRandomZeroHessian.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runRandomZeroHessian.m
new file mode 100644
index 00000000000..ae2a0444d60
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runRandomZeroHessian.m
@@ -0,0 +1,426 @@
+function [ successFlag ] = runRandomZeroHessian( nV,nC, doPrint,seed )
+
+    if ( nargin < 4 )
+        seed = 42;
+        if ( nargin < 3 )
+            doPrint = 1;
+            if ( nargin < 2 )
+                nC = 10;
+                if ( nargin < 1 )
+                    nV = 5;
+                end
+            end
+        end
+    end
+    
+    successFlag = 1;
+
+    for isSparseH=0:1
+        for isSparseA=0:1
+            successFlag = runSeveralIdSeqTests( successFlag, nV,nC,isSparseH,isSparseA, doPrint,seed );
+        end
+    end
+    
+end
+
+
+function [ successFlag ] = runSeveralIdSeqTests( successFlag, nV,nC, isSparseH,isSparseA, doPrint,seed )
+
+    %% test without or empty A matrix
+    % {
+    for hasA=0:1
+        for changeMat=0:hasA % cannot change matrices if QProblemB object is instantiated
+            for hasOptions=1:1
+                for hasX0=[0 2]
+                    for hasWS=0:2
+                        curSuccessFLAG = runSingleIdSeqTest( nV,0,hasA,isSparseH,isSparseA, 1,1,0,0,hasOptions,hasX0,hasWS,changeMat, doPrint,seed );
+                        successFlag = min( successFlag,curSuccessFLAG );
+                    end
+                end
+            end
+        end
+    end
+    %}
+    
+    %% test with non-empty A matrix
+    for hasLowerC=0:1
+        for hasUpperC=0:1
+            for hasOptions=1:1
+                for hasX0=[0 2]
+                    for hasWS=0:2
+                        for changeMat=0:1
+                            curSuccessFLAG = runSingleIdSeqTest( nV,nC,1,isSparseH,isSparseA, 1,1,hasLowerC,hasUpperC,hasOptions,hasX0,hasWS,changeMat, doPrint,seed );
+                            successFlag = min( successFlag,curSuccessFLAG );
+                        end
+                    end
+                end
+            end
+        end
+    end
+    
+end
+
+
+function [ successFlag ] = runSingleIdSeqTest( nV,nC,hasA,isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC,hasOptions,hasX0,hasWS,changeMat, doPrint,seed )
+
+    successFlag = 0;
+
+    qpFeatures = setupQpFeaturesStruct( );
+    
+    qpFeatures.nV = nV;
+    qpFeatures.nC = nC;
+    
+    qpFeatures.isSparseH = isSparseH;
+    qpFeatures.isSparseA = isSparseA;
+    
+    qpFeatures.hasLowerB = hasLowerB;
+    qpFeatures.hasUpperB = hasUpperB;
+    qpFeatures.hasLowerC = hasLowerC;
+    qpFeatures.hasUpperC = hasUpperC;
+    
+    qpFeatures.hessianType = 3;
+    
+    qpData1 = generateRandomQp( qpFeatures,seed );
+    qpData2 = generateRandomQp( qpFeatures,seed );
+
+    if ( changeMat == 0 )
+        string = 'Testing qpOASES_sequence( ''i/h/c'',0';
+    else
+        string = 'Testing qpOASES_sequence( ''i/m/c'',0';
+    end
+    
+    if ( isSparseH > 0 )
+        string = [string,'s,g'];
+    else
+        string = [string,'d,g'];
+    end
+    
+    if ( nC > 0 )
+        if ( isSparseA > 0 )
+            string = [string,',As'];
+        else
+            string = [string,',Ad'];
+        end
+    else
+        if ( hasA > 0 )
+            string = [string,',[]'];
+        end
+    end
+    
+    if ( hasLowerB > 0 )
+        string = [string,',lb'];
+    else
+        string = [string,',[]'];
+    end
+    
+    if ( hasUpperB > 0 )
+        string = [string,',ub'];
+    else
+        string = [string,',[]'];
+    end
+    
+    if ( hasLowerC > 0 )
+        string = [string,',lbA'];
+    else
+        if ( hasA > 0 )
+            string = [string,',[] '];
+        end
+    end
+    
+    if ( hasUpperC > 0 )
+        string = [string,',ubA'];
+    else
+        if ( hasA > 0 )
+            string = [string,',[] '];
+        end
+    end
+    
+    switch ( hasOptions )
+        case 1
+            string = [string,',opt'];
+        
+        case 2
+            string = [string,',[] '];
+            
+        case 0
+            if ( ( hasX0 > 0 ) || ( hasWS > 0 ) )
+                string = [string,',[] '];
+            end
+    end
+
+    switch ( hasX0 )
+        case 1
+            string = [string,',{x0'];
+        
+        case 2
+            string = [string,',{[]'];
+            
+        case 0
+            if ( hasWS > 0 )
+                string = [string,',{[]'];
+            end
+    end
+    
+    switch ( hasWS )
+        case 1
+            string = [string,',WS}'];
+        
+        case 2
+            string = [string,',[]}'];
+    end
+
+    string = [string,' )... '];
+    if ( doPrint > 0 )
+        %disp( string );
+    end
+    
+	%try
+    curSuccessFlag = callQpOasesSeq( qpData1,qpData2,hasA,hasOptions,hasX0,hasWS,changeMat,doPrint );
+	%catch
+	%curSuccessFlag = 0;
+	%end
+    if ( curSuccessFlag > 0 )
+        string = [string,'pass!'];
+        successFlag = 1;
+    else
+        string = [string,'fail!'];
+    end
+    
+    if ( doPrint > 0 )
+        disp( string );
+        if ( curSuccessFlag == 0 )
+            %pause;
+        end
+    end
+    
+end
+
+
+
+function [ successFlag ] = callQpOasesSeq( qpData1,qpData2,hasA,hasOptions,hasX0,hasWS,changeMat, doPrint )
+
+    if ( nargin < 8 )
+        doPrint = 1;
+    end
+
+    TOL = 1e-15;
+    KKTTOL = 1e-6;
+
+    successFlag = 0;
+    
+    H1 = qpData1.H;
+	g1 = qpData1.g;
+    A1 = [qpData1.Aeq;qpData1.Ain];
+	lb1 = qpData1.lb;
+    ub1 = qpData1.ub;
+    lbA1 = [qpData1.beq;qpData1.lbA];
+    ubA1 = [qpData1.beq;qpData1.ubA];
+    
+    H2 = qpData2.H;
+	g2 = qpData2.g;
+    A2 = [qpData2.Aeq;qpData2.Ain];
+	lb2 = qpData2.lb;
+    ub2 = qpData2.ub;
+    lbA2 = [qpData2.beq;qpData2.lbA];
+    ubA2 = [qpData2.beq;qpData2.ubA];
+
+    [nV,dummy] = size(H1);
+    [nC,dummy] = size(A1);
+    
+    if ( hasWS > 0 )
+        if ( hasWS == 1 )
+            WS = 0 * ones( nV+nC,1 );
+        else
+            WS = [];
+        end
+        
+        if ( hasX0 > 0 )
+            if ( hasX0 == 1 )
+                x0 = -1e-3 * ones( nV,1 );
+            else
+                x0 = [];
+            end
+            
+            auxInput = qpOASES_auxInput( 'x0',x0,'guessedWorkingSet',WS );
+
+            if ( hasOptions > 0 )
+                if ( hasOptions == 1 )
+                    options = qpOASES_options( 'fast' );
+                else
+                    options = [];
+                end
+
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else                        
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
+                    qpOASES_sequence( 'c',QP );
+                end
+            else
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
+                    qpOASES_sequence( 'c',QP );
+                end
+            end
+
+        else % hasX0 == 0
+
+            auxInput = qpOASES_auxInput( 'guessedWorkingSet',WS );
+            
+            if ( hasOptions > 0 )
+                if ( hasOptions == 1 )
+                    options = qpOASES_options( 'fast' );
+                else
+                    options = [];
+                end
+
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
+                    qpOASES_sequence( 'c',QP );
+                end
+            else
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,[] );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,[] );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
+                    qpOASES_sequence( 'c',QP );
+                end
+            end
+
+        end % hasX0
+        
+    else % hasWS == 0
+        
+        if ( hasX0 > 0 )
+            if ( hasX0 == 1 )
+                x0 = -1e-3 * ones( nV,1 );
+            else
+                x0 = [];
+            end
+            
+            auxInput = qpOASES_auxInput( 'x0',x0 );
+
+            if ( hasOptions > 0 )
+                if ( hasOptions == 1 )
+                    options = qpOASES_options( 'fast' );
+                else
+                    options = [];
+                end
+
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
+                    qpOASES_sequence( 'c',QP );
+                end
+                    
+            else
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
+                    qpOASES_sequence( 'c',QP );
+                end
+            end
+
+        else % hasX0 == 0
+
+            if ( hasOptions > 0 )
+                if ( hasOptions == 1 )
+                    options = qpOASES_options( 'fast' );
+                else
+                    options = [];
+                end
+
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
+                    qpOASES_sequence( 'c',QP );
+                end
+            else
+                if ( hasA > 0 )
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1 );
+                    if ( changeMat > 0 )
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
+                    else
+                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
+                    end
+                    qpOASES_sequence( 'c',QP );
+                else
+                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1 );
+                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
+                    qpOASES_sequence( 'c',QP );
+                end
+            end
+
+        end % hasX0
+
+    end % hasWS
+
+
+    kktTol1 = getKktResidual( H1,g1,A1,lb1,ub1,lbA1,ubA1, x1,l1 );
+    kktTol2 = getKktResidual( H1,g1,A1,lb1,ub1,lbA1,ubA1, x2,l2 );
+    
+    if ( ( kktTol1 <= KKTTOL ) && ( e1 >= 0 ) && ( kktTol2 <= KKTTOL ) && ( e2 >= 0 ) )
+        successFlag = 1;
+    else
+        if ( doPrint > 1 )
+            disp('error')
+        end
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runSimpleSpringExample.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runSimpleSpringExample.m
new file mode 100644
index 00000000000..2f494a6b136
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runSimpleSpringExample.m
@@ -0,0 +1,34 @@
+function [ successFlag ] = runSimpleSpringExample( doPrint )
+    
+    if ( nargin < 1 )
+        doPrint = 0;
+    end
+
+    successFlag = 0;
+    
+    % parameter
+    k1 = 100;
+    k2 = 100;
+
+    m = 1;
+    g = 9.81;
+
+    % QP data
+    H = [ k1,0; 0,k2 ];
+    g = [ m*g; 0 ];
+
+    lb = [ 0; 0 ];
+    ub = [ 1; 1 ];
+
+    A = [ 1,1 ];
+    lbA = 1;
+    ubA = 1;
+
+    options = qpOASES_options( 'default', 'printLevel',2*doPrint );
+    [x,fval,exitflag,iter] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
+
+    if ( ( exitflag == 0 ) && ( iter < 5 ) )
+        successFlag = 1;
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestAPrioriKnownSeq1.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestAPrioriKnownSeq1.m
new file mode 100644
index 00000000000..057196db750
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestAPrioriKnownSeq1.m
@@ -0,0 +1,87 @@
+function [ successFlag ] = runTestAPrioriKnownSeq1( doPrint )
+
+    if ( nargin < 1 )
+        doPrint = 0;
+    end
+
+    successFlag = 1;
+    TOL = eps;
+
+    clear H g A lb ub lbA ubA;
+	
+    try
+        load 'benchmarkCRANE1.mat';
+    catch
+        successFlag = -1;
+        return;
+    end
+
+    [nC,nV] = size(A);
+    [nV,nP] = size(g);
+
+    xOptSeq = zeros(nV,nP);
+    objOptSeq = zeros(1,nP);
+    statusSeq = zeros(1,nP);
+    iterSeq = zeros(1,nP);
+    yOptSeq = zeros(nV+nC,nP);
+
+	options = qpOASES_options( 'fast','maxIter',100, 'printLevel',2*doPrint );
+
+    for i=1:nP
+		%disp(i);
+
+		if ( i == 1 )
+			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		else
+			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
+		end
+
+        xOptSeq(:,i) = x;
+        objOptSeq(:,i) = obj;
+        statusSeq(:,i) = status;
+        iterSeq(:,i) = nWSRout;
+        yOptSeq(:,i) = lambda;
+        
+    end
+	
+	qpOASES_sequence( 'c',QP );
+    
+    
+    [xOptAPKSeq,objOptAPKSeq,statusAPKSeq,iterAPKSeq,yOptAPKSeq] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
+    
+    if ( norm( xOptSeq-xOptAPKSeq ) > TOL )
+        successFlag = 0;
+        if ( doPrint > 0 )
+            disp( 'xOpt error' )
+        end
+    end
+    
+    if ( norm( objOptSeq-objOptAPKSeq ) > TOL )
+        successFlag = 0;
+        if ( doPrint > 0 )
+            disp( 'objOpt error' )
+        end
+    end
+    
+    if ( sum( statusSeq~=statusAPKSeq ) > 0 )
+        successFlag = 0;
+        if ( doPrint > 0 )
+            disp( 'status error' )
+        end
+    end
+    
+    if ( sum( iterSeq~=iterAPKSeq ) > 0 )
+        successFlag = 0;
+        if ( doPrint > 0 )
+            disp( 'iter error' )
+        end
+    end
+    
+    if ( norm( yOptSeq-yOptAPKSeq ) > TOL )
+        successFlag = 0;
+        if ( doPrint > 0 )
+            disp( 'yOpt error' )
+        end
+    end
+	
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSeq.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSeq.m
new file mode 100644
index 00000000000..00c4fe8af4a
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSeq.m
@@ -0,0 +1,104 @@
+function [ successFlag ] = runTestSeq( doPrint )
+    
+    if ( nargin < 1 )
+        doPrint = 0;
+    end
+
+    successFlag = 1;
+    
+
+    % test case constants
+    m = 50;
+    n = 100;
+    nMajSeq = 8;
+    nMinSeq = 4;
+    nSeq = nMajSeq * nMinSeq;
+    p = 2; % interpolation monomial power
+    fldim = 5; % feedback law dimension
+
+    % generate start and end problem
+    Ls = sprand(n, n, 0.03);
+    Hs = Ls' * Ls + 1e-8 * eye(n);
+    As = sprand(m, n, 0.05);
+
+    % negative (!) definite Hessian
+    Le = sprand(n, n, 0.03);
+    He = -Le' * Le;
+    Ae = sprand(m, n, 0.05);
+
+    lbAs = -rand(m,1);
+    ubAs = rand(m,1); 
+    ubs = ones(n,1);
+    lbs = -ones(n,1);
+    gs = 10*rand(n,1);
+
+    lbAe = -rand(m,1);
+    ubAe = rand(m,1); 
+    ube = ones(n,1);
+    lbe = -ones(n,1);
+    ge = 10*rand(n,1);
+
+    % monomial interpolation
+    tmaj = 1 - (1:1/(1-nMajSeq):0).^p;
+    for i = 1:nMajSeq
+        tau = tmaj(i);
+        H{i} = (1-tau) * Hs + tau * He;
+        A{i} = (1-tau) * As + tau * Ae;
+    end
+    t = 1 - (1:1/(1-nSeq):0).^p;
+    for i = 1:nSeq
+        tau = t(i);
+        lbA{i} = (1-tau) * lbAs + tau * lbAe;
+        ubA{i} = (1-tau) * ubAs + tau * ubAe;
+        lb{i} = (1-tau) * lbs + tau * lbe;
+        ub{i} = (1-tau) * ubs + tau * ube;
+        g{i} = (1-tau) * gs + tau * ge;
+    end
+
+    x = zeros(n,1);
+
+    if ~exist('cumIters', 'var')
+        cumIters = zeros(nSeq, 1);
+    end
+
+    % solve sequence of QPs
+    if ( doPrint > 0 )
+        fprintf('%3s %5s\n', 'seq', 'iters')
+    end
+    
+    for i = 1:nSeq
+        if i == 1
+            [QP, x, fval, exitflag, iter, lambda] = qpOASES_sequence('i', ...
+                H{i}, g{i}, A{i}, lb{i}, ub{i}, lbA{i}, ubA{i}, x);
+        else
+            if mod(i-1, nMinSeq) == 0
+                j = (i-1) / nMinSeq + 1;
+                [x, fval, exitflag, iter, lambda] = qpOASES_sequence('m', QP, ...
+                    H{j}, g{i}, A{j}, lb{i}, ub{i}, lbA{i}, ubA{i});
+            else
+                [x, fval, exitflag, iter, lambda] = qpOASES_sequence('h', QP, ...
+                    g{i}, lb{i}, ub{i}, lbA{i}, ubA{i});
+            end
+        end
+        
+        if ( doPrint > 0 )
+            fprintf('%3d %5d\n', i, iter)
+        end
+        
+        cumIters(i) = cumIters(i) + iter;
+        
+        if ( exitflag ~= 0 )
+            successFlag = 0;
+        end
+    end
+
+    % solve EQP
+    V0 = zeros(n, fldim);
+    Lambda = eye(m, fldim);
+    [X, Y] = qpOASES_sequence('e', QP, V0, V0, V0, Lambda, Lambda);
+    FL = X(1:fldim,:);
+
+    % clear
+    qpOASES_sequence('c', QP)
+
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse.m
new file mode 100644
index 00000000000..e145a68d494
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse.m
@@ -0,0 +1,33 @@
+function [ successFlag ] = runTestSparse( doPrint )
+    
+    if ( nargin < 1 )
+        doPrint = 0;
+    end
+
+    successFlag = 0;
+
+
+    m = 50;
+    n = 100;
+
+    L = sprand(n, n, 0.03);
+    H = L' * L;
+    A = sprand(m, n, 0.05);
+
+    lbA = -rand(m,1);
+    ubA = rand(m,1); 
+    ub = ones(n,1);
+    lb = -ones(n,1);
+    g = 10*rand(n,1);
+    
+    options = qpOASES_options( 'default', 'printLevel',2*doPrint );
+
+    [x1,~,exitflag1,iter1] = qpOASES(full(H), g, full(A), lb, ub, lbA, ubA, options);
+    [x2,~,exitflag2,iter2] = qpOASES(H, g, A, lb, ub, lbA, ubA, options);
+    
+    if ( ( exitflag1 == 0 ) && ( exitflag2 == 0 ) && ( iter1 == iter2 ) ...
+            && ( norm(x1-x2) < 1e-10 ) )
+        successFlag = 1;
+    end
+
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse2.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse2.m
new file mode 100644
index 00000000000..72789059bff
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse2.m
@@ -0,0 +1,33 @@
+function [ successFlag ] = runTestSparse2( doPrint )
+    
+    if ( nargin < 1 )
+        doPrint = 0;
+    end
+
+    successFlag = 0;
+    
+
+    m = 50;
+    n = 100;
+
+    L = sprand(n, n, 0.03);
+    H = L' * L;
+    A = sprand(m, n, 0.05);
+
+    lbA = -rand(m,1);
+    ubA = rand(m,1); 
+    ub = ones(n,1);
+    lb = -ones(n,1);
+    g = 10*rand(n,1);
+
+    options = qpOASES_options( 'default', 'printLevel',2*doPrint );
+    
+    [QP,~,~,exitflag1] = qpOASES_sequence( 'i', H, g, A, lb, ub, lbA, ubA, options );
+    [~,~,exitflag2] = qpOASES_sequence( 'h',QP, g*2, lb, ub, lbA, ubA );
+    qpOASES_sequence( 'c',QP );
+    
+    if ( ( exitflag1 == 0 ) && ( exitflag2 == 0 ) )
+        successFlag = 1;
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse3.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse3.m
new file mode 100644
index 00000000000..c62d3aba028
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse3.m
@@ -0,0 +1,39 @@
+function [ successFlag ] = runTestSparse3( doPrint )
+    
+    if ( nargin < 1 )
+        doPrint = 0;
+    end
+
+    successFlag = 0;
+
+    
+    m = 50;
+    n = 100;
+
+    L = sprand(n, n, 0.03);
+    H = L' * L;
+    A = sprand(m, n, 0.05);
+
+    lbA = -rand(m,1);
+    ubA = rand(m,1); 
+    ub = ones(n,1);
+    lb = -ones(n,1);
+    g = 10*rand(n,1);
+
+    options = qpOASES_options( 'default', 'printLevel',2*doPrint );
+    
+    
+    [QP,~,~,exitflag1] = qpOASES_sequence( 'i', H, g, A, lb, ub, lbA, ubA, options );
+
+    L = sprand(n, n, 0.03);
+    H = L' * L;
+    A = sprand(m, n, 0.05);
+
+    [~,~,exitflag2] = qpOASES_sequence( 'm',QP, H, g*2, A, lb, ub, lbA, ubA );
+    qpOASES_sequence( 'c',QP );
+    
+    if ( ( exitflag1 == 0 ) && ( exitflag2 == 0 ) )
+        successFlag = 1;
+    end
+    
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse4.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse4.m
new file mode 100644
index 00000000000..0841ef3c78b
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse4.m
@@ -0,0 +1,29 @@
+function [ successFlag ] = runTestSparse4( doPrint )
+    
+    if ( nargin < 1 )
+        doPrint = 0;
+    end
+
+    successFlag = 0;
+    
+    
+    n = 100;
+
+    L = sprand(n, n, 0.03);
+    H = L' * L;
+
+    ub = ones(n,1);
+    lb = -ones(n,1);
+    g = 10*rand(n,1);
+    
+    options = qpOASES_options( 'default', 'printLevel',2*doPrint );
+
+    [QP,~,~,exitflag1] = qpOASES_sequence( 'i', H, g, lb, ub, options );
+    [~,~,exitflag2] = qpOASES_sequence( 'h',QP, g*2, lb, ub );
+    qpOASES_sequence( 'c',QP );
+
+    if ( ( exitflag1 == 0 ) && ( exitflag2 == 0 ) )
+        successFlag = 1;
+    end
+
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestWorkingSetLI.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestWorkingSetLI.m
new file mode 100644
index 00000000000..ac0dfe5f5f1
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestWorkingSetLI.m
@@ -0,0 +1,50 @@
+function [ successFlag ] = runTestWorkingSetLI( doPrint )
+    
+    if ( nargin < 1 )
+        doPrint = 0;
+    end
+
+    successFlag = 1;
+    
+    qpFeatures = setupQpFeaturesStruct( );
+    
+    qpFeatures.nV = 20;
+    qpFeatures.nC = 100;
+    
+    qpFeatures.isSparseH = 0;
+    qpFeatures.isSparseA = 0;
+    
+    qpFeatures.hasLowerB = 1;
+    qpFeatures.hasUpperB = 1;
+    qpFeatures.hasLowerC = 1;
+    qpFeatures.hasUpperC = 1;
+    
+    qpFeatures.makeInfeas = 1;
+    
+    options = qpOASES_options( 'default', 'printLevel',0, 'initialStatusBounds',0 );
+    
+    exitflag = 0;
+    counter = 0;
+    
+    while ( ( exitflag ~= -42 ) && ( counter < 100 ) )
+        
+        counter = counter+1;
+        
+        qpData = generateRandomQp( qpFeatures );
+        B = [ eye( qpFeatures.nV ); qpData.Ain ];
+    
+        [x,dummy1,exitflag,dummy2,dummy3,auxOutput] = qpOASES( qpData.H,qpData.g,qpData.Ain, ...
+                qpData.lb,qpData.ub,qpData.lbA,qpData.ubA, options ); %#ok<*NASGU>
+            
+        WS = auxOutput.workingSet;
+        nAct = sum( WS~=0 );
+        Bact = B( WS~=0,: );
+
+        if ( nAct ~= rank(Bact) )
+            successFlag = 0;
+            return;
+        end
+        
+    end
+   
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runVanBarelsUnboundedQP.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runVanBarelsUnboundedQP.m
new file mode 100644
index 00000000000..e6293f30143
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runVanBarelsUnboundedQP.m
@@ -0,0 +1,25 @@
+function [ successFlag ] = runVanBarelsUnboundedQP( doPrint )
+    
+    if ( nargin < 1 )
+        doPrint = 0;
+    end
+
+    successFlag = 0;
+    
+    try
+        data = load( 'vanBarelsUnboundedQP.mat' );
+    catch
+        successFlag = -1;
+        return;
+    end
+    
+    options1 = qpOASES_options( 'default', 'printLevel',-1*doPrint );
+    
+    [~,~,exitflag1,iter1] = qpOASES( data.H,data.g,data.lb,data.ub,options1 ); %#ok<*NASGU>
+
+    % should return "QP unbounded"
+    if ( exitflag1 == -3 )
+        successFlag = 1;
+    end
+   
+end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/runUnitTests b/3rdparty/qpOASES/qpOASES-3.0/testing/runUnitTests
new file mode 100644
index 00000000000..e50cc44b2a3
--- /dev/null
+++ b/3rdparty/qpOASES/qpOASES-3.0/testing/runUnitTests
@@ -0,0 +1,106 @@
+#!/usr/bin/env bash
+
+##
+##	This file is part of qpOASES.
+##
+##	qpOASES -- An Implementation of the Online Active Set Strategy.
+##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
+##	Christian Kirches et al. All rights reserved.
+##
+##	qpOASES 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.
+##
+##	qpOASES 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 qpOASES; if not, write to the Free Software
+##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+##
+
+
+
+##
+##	Filename:  testing/runUnitTests
+##	Author:    Hans Joachim Ferreau
+##	Version:   3.0
+##	Date:      2014
+
+# defining colors for output
+red='\e[0;31m'
+green='\e[0;32m'
+NC='\e[0m' # No Color
+
+# runs a number of examples to detect malfunctioning of the code
+
+function runTest {
+	echo -n "Running unit test $2 $3... "
+
+	$2 $3 > dummy.txt;
+	retVal=$?;
+	rm -rf dummy.txt;
+
+	if [ $retVal == 0 ]; then
+		echo -e "${green}passed!${NC}"
+	else
+		if [ $retVal == 99 ]; then
+			echo "problem data missing!"
+		else
+			echo -e "${red}failed!${NC}"
+			counter=$[counter+1];
+		fi
+	fi
+}
+
+
+cd ..;
+make testing;
+cd testing;
+
+counter=0;
+
+# run unit tests
+runTest $counter ../bin/test_matrices;
+runTest $counter ../bin/test_matrices2;
+runTest $counter ../bin/test_matrices3;
+runTest $counter ../bin/test_indexlist;
+
+runTest $counter ../bin/test_example1;
+runTest $counter ../bin/test_example1a;
+runTest $counter ../bin/test_example1b;
+runTest $counter ../bin/test_example2;
+runTest $counter ../bin/test_example4;
+runTest $counter ../bin/test_example5;
+runTest $counter ../bin/test_exampleLP;
+runTest $counter ../bin/test_qrecipe;
+
+runTest $counter ../bin/test_example6;
+runTest $counter ../bin/test_example7;
+runTest $counter ../bin/test_sebastien1;
+runTest $counter ../bin/test_vanBarelsUnboundedQP;
+runTest $counter ../bin/test_janick1;
+runTest $counter ../bin/test_janick2;
+runTest $counter ../bin/test_constraintProduct1;
+runTest $counter ../bin/test_constraintProduct2;
+runTest $counter ../bin/test_guessedWS1;
+runTest $counter ../bin/test_gradientShift;
+runTest $counter ../bin/test_runAllOqpExamples;
+
+runTest $counter ../bin/test_bench Odd;
+runTest $counter ../bin/test_bench Ods;
+#runTest $counter ../bin/test_bench Ord;
+#runTest $counter ../bin/test_bench Ors;
+runTest $counter ../bin/test_bench Omd;
+#runTest $counter ../bin/test_bench Oms;
+
+
+if [ $counter == 0 ]; then
+	echo -e "${green}All available tests passed successfully!${NC}"
+else
+	echo -e "${red}$counter test(s) failed!${NC}"
+	exit 1
+fi
diff --git a/CMakeLists.txt b/CMakeLists.txt
index cea6963dac9..891ca89e828 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -196,7 +196,7 @@ add_subdirectory(idl)
 add_subdirectory(lib)
 add_subdirectory(ec)
 # option for 3rdparty
-option(USE_QPOASES "Build qpOASES" OFF)
+option(USE_QPOASES "Build qpOASES" ON)
 add_subdirectory(3rdparty)
 add_subdirectory(rtc)
 option(ENABLE_DOXYGEN "Use Doxygen" ON)
diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl
index a278d03bc69..071bc5d3f62 100644
--- a/idl/AutoBalancerService.idl
+++ b/idl/AutoBalancerService.idl
@@ -420,5 +420,6 @@ module OpenHRP
      * @return true if set successfully, false otherwise
      */
     boolean startHumanSyncAfter5sec();
+    boolean stopHumanSync();
   };
 };
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index deba4531776..116a67ba7c8 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -39,6 +39,7 @@ humanpose_t humanpose;
 humanpose_t filt_hp_data,hp_data_old;
 bool startCountdownForHumanSync = false;
 bool HumanSyncOn = false;
+#define USEX true
 
 
 typedef coil::Guard<coil::Mutex> Guard;
@@ -519,9 +520,9 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     //for human tracker
     if (m_htzmpIn.isNew()){
     	m_htzmpIn.read();
-//    	humanpose.zmp[0] = m_htzmp.data.x;//zmpはこっちで計算
-//    	humanpose.zmp[1] = m_htzmp.data.y;
-//    	humanpose.zmp[2] = m_htzmp.data.z;
+    	humanpose.zmp[0] = m_htzmp.data.x;//zmpは外部から読まずに両足wrenchからこのソースの中で計算
+    	humanpose.zmp[1] = m_htzmp.data.y;
+    	humanpose.zmp[2] = m_htzmp.data.z;
     }
     if (m_htrfwIn.isNew()){
     	m_htrfwIn.read();
@@ -686,11 +687,19 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
 //					humanpose.zmp[0],humanpose.zmp[1]);
 	   if(HumanSyncOn){
 //ZMP上書き
-//	      m_zmp.data.x = humanpose.zmp[0]*0.5 - filt_hp_data.com[0];//
-		  m_zmp.data.y = filt_hp_data.zmp[1] - filt_hp_data.com[1];//test
+			if(USEX)m_zmp.data.x = filt_hp_data.zmp[0]*0.5 - filt_hp_data.com[0];//
+			m_zmp.data.y = filt_hp_data.zmp[1]*0.5 - filt_hp_data.com[1];//
+//			if(USEX)m_zmp.data.x = filt_hp_data.zmp[0];//
+//			m_zmp.data.y = filt_hp_data.zmp[1];//
+
+//			std::cout<<"debug0"<<m_zmp.data.x<<" , "<<m_zmp.data.y<<" , "<<m_zmp.data.z<<std::endl;
+
+
+//		m_zmp.data.x = 0;//
+//		m_zmp.data.y = 0;//
 // COM上書き
-//	  m_cog.data.x = 0.0558423 + filt_hp_data.com[0];
-	  m_cog.data.y = filt_hp_data.com[1];
+//		if(USEX)m_cog.data.x = filt_hp_data.com[0]*0.7;//ここの.comは今はhumanのbaseposなので適当調整
+//		m_cog.data.y = filt_hp_data.com[1]*0.7;
 	  }
 
       // sbpCogOffset
@@ -739,6 +748,11 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
         m_limbCOPOffsetOut[i]->write();
     }
 
+//ishiguro
+//    std::cout<<"out "<<m_controlSwingSupportTime.data[contact_states_index_map["rleg"]]<<" "<<gg->get_current_swing_time(0)<<" "<<m_controlSwingSupportTime.data[contact_states_index_map["lleg"]]<<" "<<gg->get_current_swing_time(1)<<std::endl;
+//    std::cout<<"out2"<<m_controlSwingSupportTime.data[contact_states_index_map[2]]<<" , "<<gg->get_current_swing_time(2)<<std::endl;
+//    std::cout<<"out3"<<m_controlSwingSupportTime.data[contact_states_index_map[3]]<<" , "<<gg->get_current_swing_time(3)<<std::endl;
+
     return RTC::RTC_OK;
 }
 
@@ -1044,6 +1058,12 @@ void AutoBalancer::getTargetParameters()
       ref_zmp(1) = ref_cog(1);
       ref_zmp(2) = tmp_foot_mid_pos(2);
     }
+    //ishiguro
+    if (HumanSyncOn) {
+		if(USEX)ref_zmp(0) = humanpose.zmp[0];
+		ref_zmp(1) = humanpose.zmp[1];
+		ref_zmp(2) = tmp_foot_mid_pos(2);
+    }
   }
 };
 
@@ -1081,24 +1101,28 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri
 static bool moveleft = false;
 static bool ht_first_call = true;
 hrp::Vector3 rfinitpos,lfinitpos;
+static int rfuplevel,lfuplevel;
+#define LEVELNUM 200
+#define FUPHIGHT 0.05
 bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 {
-	static int rfuplevel,lfuplevel;
-	if(HumanSyncOn){
 		if(param.target_link->name == "RLEG_JOINT5"){
-			if(humanpose.rfwrench[2] < 20.0 && humanpose.lfwrench[2] > 20.0){
+			if(humanpose.rfwrench[2] < 20.0 && humanpose.lfwrench[2] > 20.0 && HumanSyncOn){
 //			if(humanpose.rfwrench[2] < 20.0){
-				if(rfuplevel < 100){
+				if(rfuplevel < LEVELNUM){
 					rfuplevel++;
-					filt_hp_data.rfpos[2] = 0.025 - 0.025*cos(M_PI*rfuplevel/100);
+					filt_hp_data.rfpos[2] = FUPHIGHT/2*(1-cos(M_PI*rfuplevel/LEVELNUM));
 					//filt_hp_data.rfpos[1] -= 0.001;//歩行遊び
 				}
-				cout<<"RUP"<<endl;
+				if(loop%100==0)cout<<"RUP"<<endl;
+		        m_contactStates.data[contact_states_index_map["rleg"]] = false;
 			}else{
 				if(rfuplevel > 0){
 					rfuplevel--;
-					filt_hp_data.rfpos[2] = 0.025 - 0.025*cos(M_PI*rfuplevel/100);
+					filt_hp_data.rfpos[2] = FUPHIGHT/2*(1-cos(M_PI*rfuplevel/LEVELNUM));
 					//if(filt_hp_data.rfpos[2] == 0.0)moveleft = true;//歩行遊び
+				}else{
+			        m_contactStates.data[contact_states_index_map["rleg"]] = true;
 				}
 			}
 			param.target_p0(0) = rfinitpos(0) + filt_hp_data.rfpos[0];
@@ -1106,18 +1130,18 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 			param.target_p0(2) = rfinitpos(2) + filt_hp_data.rfpos[2];
 		}
 		if(param.target_link->name == "LLEG_JOINT5"){
-			if(humanpose.lfwrench[2] < 20.0 && humanpose.rfwrench[2] > 20.0){
+			if(humanpose.lfwrench[2] < 20.0 && humanpose.rfwrench[2] > 20.0 && HumanSyncOn){
 //			if(humanpose.lfwrench[2] < 20.0){
-				if(lfuplevel < 100){
+				if(lfuplevel < LEVELNUM){
 					lfuplevel++;
-					filt_hp_data.lfpos[2] = 0.025 - 0.025*cos(M_PI*lfuplevel/100);
+					filt_hp_data.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
 					//filt_hp_data.lfpos[1] -= 0.001;//歩行遊び
 				}
-				cout<<"LUP"<<endl;
+				if(loop%100==0)cout<<"LUP"<<endl;
 			}else{
 				if(lfuplevel > 0){
 					lfuplevel--;
-					filt_hp_data.lfpos[2] = 0.025 - 0.025*cos(M_PI*lfuplevel/100);
+					filt_hp_data.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
 					//if(filt_hp_data.lfpos[2] == 0.0)moveleft = true;//歩行遊び
 				}
 			}
@@ -1125,17 +1149,16 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 			param.target_p0(1) = lfinitpos(1) + filt_hp_data.lfpos[1];
 			param.target_p0(2) = lfinitpos(2) + filt_hp_data.lfpos[2];
 		}
-		if(param.target_link->name == "RARM_JOINT7"){
+		if(param.target_link->name == "RARM_JOINT7" && HumanSyncOn){
 			param.target_p0(0) = 0.151 + filt_hp_data.rhpos[0];
 			param.target_p0(1) = -0.374 + filt_hp_data.rhpos[1];
 			param.target_p0(2) = 0.973 + filt_hp_data.rhpos[2];
 		}
-		if(param.target_link->name == "LARM_JOINT7"){
+		if(param.target_link->name == "LARM_JOINT7" && HumanSyncOn){
 			param.target_p0(0) = 0.151 + filt_hp_data.lhpos[0];
 			param.target_p0(1) = 0.374 + filt_hp_data.lhpos[1];
 			param.target_p0(2) = 0.973 + filt_hp_data.lhpos[2];
 		}
-	}
 
 
   param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
@@ -1186,7 +1209,8 @@ void AutoBalancer::solveLimbIK ()
   m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog;
   m_robot->rootLink()->R = target_root_R;
 //mytest
-  #define MAXVEL 0.004
+#define MAXVEL 0.004
+#define MAXPOS 0.2
 
   static humanpose_t init_humanpose;
   static int HumanSyncCountdownNum = 5 * (1/m_dt);
@@ -1195,15 +1219,21 @@ void AutoBalancer::solveLimbIK ()
 	  if(HumanSyncCountdownNum == 0){
 		  HumanSyncOn = true;
 		  startCountdownForHumanSync = false;
+		  HumanSyncCountdownNum = 5 * (1/m_dt);
 	  }else{
 		  HumanSyncCountdownNum--;
 	  }
   }
+  if(ht_first_call){
+	  rfinitpos = ikp["rleg"].target_p0;
+	  lfinitpos = ikp["lleg"].target_p0;
+	  ht_first_call=false;
+  }
 
   if(HumanSyncOn){
 	  for(int i=0;i<3;i++){
 		  filt_hp_data.zmp[i] = 0.99*filt_hp_data.zmp[i] + 0.01*(humanpose.zmp[i]);
-		  filt_hp_data.com[i] = 0.99*filt_hp_data.com[i] + 0.01*(humanpose.com[i] - init_humanpose.com[i]);
+		  filt_hp_data.com[i] = 0.99*filt_hp_data.com[i] + 0.01*(humanpose.com[i] - init_humanpose.com[i] + init_humanpose.zmp[i]);//Sync開始時のzmpのオフセットはつまり重心のオフセット
 		  filt_hp_data.rfwrench[i] = 0.99*filt_hp_data.rfwrench[i] + 0.01*(humanpose.rfwrench[i]);
 		  filt_hp_data.lfwrench[i] = 0.99*filt_hp_data.lfwrench[i] + 0.01*(humanpose.lfwrench[i]);
 	//	  filt_hp_data.rfpos[i] = 0.99*filt_hp_data.rfpos[i] + 0.01*(humanpose.rfpos[i] - init_humanpose.rfpos[i]);//足ロック
@@ -1213,31 +1243,29 @@ void AutoBalancer::solveLimbIK ()
 
 		  if(filt_hp_data.com[i] - hp_data_old.com[i] > MAXVEL)filt_hp_data.com[i] = hp_data_old.com[i] + MAXVEL;
 		  if(filt_hp_data.com[i] - hp_data_old.com[i] < -MAXVEL)filt_hp_data.com[i] = hp_data_old.com[i] - MAXVEL;
-		  hp_data_old.com[i] = filt_hp_data.com[i];
+		  if(filt_hp_data.com[i] > MAXPOS)filt_hp_data.com[i] = MAXPOS;
+		  if(filt_hp_data.com[i] < -MAXPOS)filt_hp_data.com[i] = -MAXPOS;
 	  }
 
 	  if(loop%100==0)printf("[COM] %+06.3f,%+06.3f,%+06.3f [ZMP] %+06.3f,%+06.3f\n", filt_hp_data.com[0],filt_hp_data.com[1],filt_hp_data.com[2],filt_hp_data.zmp[0],filt_hp_data.zmp[1],filt_hp_data.zmp[2]);
-	  if(ht_first_call){ht_first_call=false;
-		  rfinitpos = ikp["rleg"].target_p0;
-		  lfinitpos = ikp["lleg"].target_p0;
-	  }
   }else{
 	  for(int i=0;i<3;i++)init_humanpose.com[i] = humanpose.com[i];
+	  for(int i=0;i<3;i++)init_humanpose.zmp[i] = humanpose.zmp[i];
 	  for(int i=0;i<3;i++)init_humanpose.rfpos[i] = humanpose.rfpos[i];
 	  for(int i=0;i<3;i++)init_humanpose.lfpos[i] = humanpose.lfpos[i];
 	  for(int i=0;i<3;i++)init_humanpose.rhpos[i] = humanpose.rhpos[i];
 	  for(int i=0;i<3;i++)init_humanpose.lhpos[i] = humanpose.lhpos[i];
+	  for(int i=0;i<3;i++){
+		  if(filt_hp_data.com[i]>0)filt_hp_data.com[i] -= 0.0001;
+		  if(filt_hp_data.com[i]<0)filt_hp_data.com[i] += 0.0001;
+	  }
 	  if(loop%100==0)printf("[pre-COM] %+06.3f,%+06.3f,%+06.3f [pre-ZMP] %+06.3f,%+06.3f\n", humanpose.com[0],humanpose.com[1],humanpose.com[2],humanpose.zmp[0],humanpose.zmp[1],humanpose.zmp[2]);
 
   }
-  filt_hp_data.com[0] = 0.0;//重心前方向ロック
-
-//  m_robot->rootLink()->p(0) = 0.0558423 + filt_hp_data.com[0];
-//  m_robot->rootLink()->p(1) = 0+ filt_hp_data.com[1];//safetest
-//  m_robot->rootLink()->p(2) = 0.976372 + filt_hp_data.com[2];
-  m_robot->rootLink()->p(0) += filt_hp_data.com[0];
-  m_robot->rootLink()->p(1) += filt_hp_data.com[1];//safetest
+  if(USEX)m_robot->rootLink()->p(0) += filt_hp_data.com[0];
+  m_robot->rootLink()->p(1) += filt_hp_data.com[1];
   m_robot->rootLink()->p(2) += filt_hp_data.com[2];
+  for(int i=0;i<3;i++)hp_data_old.com[i] = filt_hp_data.com[i];
 
 static int movelcnt;
 	if(moveleft){
@@ -1304,7 +1332,6 @@ static int movelcnt;
   }
 */
 
-
 bool AutoBalancer::startHumanSyncAfter5sec()
 {
 	std::cerr << "[" << m_profile.instance_name << "] start HumanSync after 5 sec" << std::endl;
diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp
index b5175201764..27a979ff67f 100644
--- a/rtc/Stabilizer/Stabilizer.cpp
+++ b/rtc/Stabilizer/Stabilizer.cpp
@@ -1045,6 +1045,7 @@ void Stabilizer::getTargetParameters ()
   m_robot->rootLink()->R = target_root_R;
   m_robot->calcForwardKinematics();
   ref_zmp = m_robot->rootLink()->R * hrp::Vector3(m_zmpRef.data.x, m_zmpRef.data.y, m_zmpRef.data.z) + m_robot->rootLink()->p; // base frame -> world frame
+ std::cout<<"debug"<<m_zmpRef.data.x<<" , "<<m_zmpRef.data.y<<" , "<<m_zmpRef.data.z<<std::endl;
   if (st_algorithm == OpenHRP::StabilizerService::EEFM || st_algorithm == OpenHRP::StabilizerService::EEFMQP || st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP) {
     // apply inverse system
     hrp::Vector3 tmp_ref_zmp = ref_zmp + eefm_zmp_delay_time_const[0] * (ref_zmp - prev_ref_zmp) / dt;

From 5ee603149fca056a60987d1251ed9e6d0adb280e Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 5 Jul 2016 09:14:05 +0900
Subject: [PATCH 007/217] tune COM ratio as 0.55

---
 python/hrpsys_config.py           |  2 +
 rtc/AutoBalancer/AutoBalancer.cpp | 89 ++++++++++++++++++++-----------
 rtc/AutoBalancer/AutoBalancer.h   |  2 +
 rtc/Stabilizer/Stabilizer.cpp     |  1 -
 4 files changed, 63 insertions(+), 31 deletions(-)

diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index 0f121447aad..22b0b4b0808 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -386,6 +386,8 @@ def connectComps(self):
             connectPorts(self.seq.port("qRef"), self.st.port("qRefSeq"))
             connectPorts(self.abc.port("walkingStates"), self.st.port("walkingStates"))
             connectPorts(self.abc.port("sbpCogOffset"), self.st.port("sbpCogOffset"))
+            # for human tracker calib
+            connectPorts(self.st.port("zmp"), self.abc.port("actzmpIn"))
             if self.es:
                 connectPorts(self.st.port("emergencySignal"), self.es.port("emergencySignal"))
             connectPorts(self.st.port("emergencySignal"), self.abc.port("emergencySignal"))
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 116a67ba7c8..444c7939285 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -83,6 +83,7 @@ AutoBalancer::AutoBalancer(RTC::Manager* manager)
       m_htlfIn("htlfIn", m_htlf),
       m_htrhIn("htrhIn", m_htrh),
       m_htlhIn("htlhIn", m_htlh),
+      m_actzmpIn("actzmpIn", m_actzmp),
 
       m_qOut("q", m_qRef),
       m_zmpOut("zmpOut", m_zmp),
@@ -134,6 +135,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     addInPort("htlfIn", m_htlfIn);
     addInPort("htrhIn", m_htrhIn);
     addInPort("htlhIn", m_htlhIn);
+    addInPort("actzmpIn", m_actzmpIn);
 
     // Set OutPort buffer
     addOutPort("q", m_qOut);
@@ -566,6 +568,9 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     	humanpose.lhpos[1] = m_htlh.data.y;
     	humanpose.lhpos[2] = m_htlh.data.z;
     }
+    if (m_actzmpIn.isNew()){
+    	m_actzmpIn.read();
+    }
 
     Guard guard(m_mutex);
     hrp::Vector3 ref_basePos;
@@ -1108,11 +1113,9 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 {
 		if(param.target_link->name == "RLEG_JOINT5"){
 			if(humanpose.rfwrench[2] < 20.0 && humanpose.lfwrench[2] > 20.0 && HumanSyncOn){
-//			if(humanpose.rfwrench[2] < 20.0){
 				if(rfuplevel < LEVELNUM){
 					rfuplevel++;
 					filt_hp_data.rfpos[2] = FUPHIGHT/2*(1-cos(M_PI*rfuplevel/LEVELNUM));
-					//filt_hp_data.rfpos[1] -= 0.001;//歩行遊び
 				}
 				if(loop%100==0)cout<<"RUP"<<endl;
 		        m_contactStates.data[contact_states_index_map["rleg"]] = false;
@@ -1120,7 +1123,6 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 				if(rfuplevel > 0){
 					rfuplevel--;
 					filt_hp_data.rfpos[2] = FUPHIGHT/2*(1-cos(M_PI*rfuplevel/LEVELNUM));
-					//if(filt_hp_data.rfpos[2] == 0.0)moveleft = true;//歩行遊び
 				}else{
 			        m_contactStates.data[contact_states_index_map["rleg"]] = true;
 				}
@@ -1131,18 +1133,15 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 		}
 		if(param.target_link->name == "LLEG_JOINT5"){
 			if(humanpose.lfwrench[2] < 20.0 && humanpose.rfwrench[2] > 20.0 && HumanSyncOn){
-//			if(humanpose.lfwrench[2] < 20.0){
 				if(lfuplevel < LEVELNUM){
 					lfuplevel++;
 					filt_hp_data.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
-					//filt_hp_data.lfpos[1] -= 0.001;//歩行遊び
 				}
 				if(loop%100==0)cout<<"LUP"<<endl;
 			}else{
 				if(lfuplevel > 0){
 					lfuplevel--;
 					filt_hp_data.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
-					//if(filt_hp_data.lfpos[2] == 0.0)moveleft = true;//歩行遊び
 				}
 			}
 			param.target_p0(0) = lfinitpos(0) + filt_hp_data.lfpos[0];
@@ -1211,6 +1210,10 @@ void AutoBalancer::solveLimbIK ()
 //mytest
 #define MAXVEL 0.004
 #define MAXPOS 0.2
+#define FNUM 0.01
+
+  static bool isCalibState=false;
+  static double h2r_ratio = 0.55;//human 1.1 vs chidori 0.69
 
   static humanpose_t init_humanpose;
   static int HumanSyncCountdownNum = 5 * (1/m_dt);
@@ -1218,6 +1221,7 @@ void AutoBalancer::solveLimbIK ()
 	  printf("Start Count Down for HumanSync [%d]\r", HumanSyncCountdownNum);
 	  if(HumanSyncCountdownNum == 0){
 		  HumanSyncOn = true;
+		  isCalibState = true;
 		  startCountdownForHumanSync = false;
 		  HumanSyncCountdownNum = 5 * (1/m_dt);
 	  }else{
@@ -1232,14 +1236,14 @@ void AutoBalancer::solveLimbIK ()
 
   if(HumanSyncOn){
 	  for(int i=0;i<3;i++){
-		  filt_hp_data.zmp[i] = 0.99*filt_hp_data.zmp[i] + 0.01*(humanpose.zmp[i]);
-		  filt_hp_data.com[i] = 0.99*filt_hp_data.com[i] + 0.01*(humanpose.com[i] - init_humanpose.com[i] + init_humanpose.zmp[i]);//Sync開始時のzmpのオフセットはつまり重心のオフセット
-		  filt_hp_data.rfwrench[i] = 0.99*filt_hp_data.rfwrench[i] + 0.01*(humanpose.rfwrench[i]);
-		  filt_hp_data.lfwrench[i] = 0.99*filt_hp_data.lfwrench[i] + 0.01*(humanpose.lfwrench[i]);
-	//	  filt_hp_data.rfpos[i] = 0.99*filt_hp_data.rfpos[i] + 0.01*(humanpose.rfpos[i] - init_humanpose.rfpos[i]);//足ロック
-	//	  filt_hp_data.lfpos[i] = 0.99*filt_hp_data.lfpos[i] + 0.01*(humanpose.lfpos[i] - init_humanpose.lfpos[i]);//足ロック
-		  filt_hp_data.rhpos[i] = 0.99*filt_hp_data.rhpos[i] + 0.01*(humanpose.rhpos[i] - init_humanpose.rhpos[i]);
-		  filt_hp_data.lhpos[i] = 0.99*filt_hp_data.lhpos[i] + 0.01*(humanpose.lhpos[i] - init_humanpose.lhpos[i]);
+		  filt_hp_data.zmp[i] = (1-FNUM)*filt_hp_data.zmp[i] + FNUM*(humanpose.zmp[i]);
+		  filt_hp_data.com[i] = (1-FNUM)*filt_hp_data.com[i] + FNUM*(humanpose.com[i] - init_humanpose.com[i] + init_humanpose.zmp[i]);//Sync開始時のzmpのオフセットはつまり重心のオフセット
+		  filt_hp_data.rfwrench[i] = (1-FNUM)*filt_hp_data.rfwrench[i] + FNUM*(humanpose.rfwrench[i]);
+		  filt_hp_data.lfwrench[i] = (1-FNUM)*filt_hp_data.lfwrench[i] + FNUM*(humanpose.lfwrench[i]);
+	//	  filt_hp_data.rfpos[i] = (1-FNUM)*filt_hp_data.rfpos[i] + FNUM*(humanpose.rfpos[i] - init_humanpose.rfpos[i]);//足ロック
+	//	  filt_hp_data.lfpos[i] = (1-FNUM)*filt_hp_data.lfpos[i] + FNUM*(humanpose.lfpos[i] - init_humanpose.lfpos[i]);//足ロック
+		  filt_hp_data.rhpos[i] = (1-FNUM)*filt_hp_data.rhpos[i] + FNUM*(humanpose.rhpos[i] - init_humanpose.rhpos[i]);
+		  filt_hp_data.lhpos[i] = (1-FNUM)*filt_hp_data.lhpos[i] + FNUM*(humanpose.lhpos[i] - init_humanpose.lhpos[i]);
 
 		  if(filt_hp_data.com[i] - hp_data_old.com[i] > MAXVEL)filt_hp_data.com[i] = hp_data_old.com[i] + MAXVEL;
 		  if(filt_hp_data.com[i] - hp_data_old.com[i] < -MAXVEL)filt_hp_data.com[i] = hp_data_old.com[i] - MAXVEL;
@@ -1247,7 +1251,7 @@ void AutoBalancer::solveLimbIK ()
 		  if(filt_hp_data.com[i] < -MAXPOS)filt_hp_data.com[i] = -MAXPOS;
 	  }
 
-	  if(loop%100==0)printf("[COM] %+06.3f,%+06.3f,%+06.3f [ZMP] %+06.3f,%+06.3f\n", filt_hp_data.com[0],filt_hp_data.com[1],filt_hp_data.com[2],filt_hp_data.zmp[0],filt_hp_data.zmp[1],filt_hp_data.zmp[2]);
+//	  if(loop%100==0)printf("[COM] %+06.3f,%+06.3f,%+06.3f [ZMP] %+06.3f,%+06.3f\n", filt_hp_data.com[0],filt_hp_data.com[1],filt_hp_data.com[2],filt_hp_data.zmp[0],filt_hp_data.zmp[1]);
   }else{
 	  for(int i=0;i<3;i++)init_humanpose.com[i] = humanpose.com[i];
 	  for(int i=0;i<3;i++)init_humanpose.zmp[i] = humanpose.zmp[i];
@@ -1259,24 +1263,49 @@ void AutoBalancer::solveLimbIK ()
 		  if(filt_hp_data.com[i]>0)filt_hp_data.com[i] -= 0.0001;
 		  if(filt_hp_data.com[i]<0)filt_hp_data.com[i] += 0.0001;
 	  }
-	  if(loop%100==0)printf("[pre-COM] %+06.3f,%+06.3f,%+06.3f [pre-ZMP] %+06.3f,%+06.3f\n", humanpose.com[0],humanpose.com[1],humanpose.com[2],humanpose.zmp[0],humanpose.zmp[1],humanpose.zmp[2]);
+//	  if(loop%100==0)printf("[pre-COM] %+06.3f,%+06.3f,%+06.3f [pre-ZMP] %+06.3f,%+06.3f\n", humanpose.com[0],humanpose.com[1],humanpose.com[2],humanpose.zmp[0],humanpose.zmp[1]);
+  }
+
+  static double filt_actzmp[3];
+  if(isCalibState){
+	  filt_actzmp[0] = (1-FNUM)*filt_actzmp[0] + FNUM*m_actzmp.data.x;
+	  filt_actzmp[1] = (1-FNUM)*filt_actzmp[1] + FNUM*m_actzmp.data.y;
+	  filt_actzmp[2] = (1-FNUM)*filt_actzmp[2] + FNUM*m_actzmp.data.z;
+	    const int errnum = 500*5;
+	    const int errdelay = 500;
+	    double zmp_robot[3],zmp_human[3];
+	    static double zmp_human_old[3][errdelay];
+	    static double err_sum[3] = {0};
+	    for(int i=0;i<3;i++){
+		    for(int j=errdelay-1;j>0;j--){
+		    	zmp_human_old[i][j] = zmp_human_old[i][j-1];
+		    }
+		    zmp_human_old[i][0] = filt_hp_data.zmp[i] - current_root_p(i);
+		    zmp_human[i] = filt_hp_data.zmp[i] - current_root_p(i);
+//		    zmp_human[i] = zmp_human_old[i][errdelay-1];//位相遅れ見越して
+			zmp_robot[i] = filt_actzmp[i];
+			err_sum[i] += fabs(zmp_human[i]) - fabs(zmp_robot[i]);
+	    }
+
+//	    if(loop%errnum==0){
+//			if(err_sum[1]/errnum>0.01 ){h2r_ratio -= 0.01;std::cout<<"h2r_ratio UP:"<<h2r_ratio<<" (EP):"<<err_sum[1]<<std::endl;}
+//			else if(err_sum[1]/errnum<-0.01 ){h2r_ratio += 0.01;std::cout<<"h2r_ratio DOWN:"<<h2r_ratio<<" (EP):"<<err_sum[1]<<std::endl;}
+//			else{std::cout<<"h2r_ratio KEEP:"<<h2r_ratio<<" (EP):"<<err_sum[1]<<std::endl;}
+//	    }
 
+	    //debug
+//	    static FILE *fp;
+//	    static bool fp_isOpen=false;
+//		if(!fp_isOpen){fp = fopen("/home/ishiguro/hcflog/nilog.log","w+");fp_isOpen=true;}
+//		if(fp!=NULL)fprintf(fp,"zmp: %f %f %f %f %f\n",zmp_human[0],zmp_human[1],zmp_robot[0],zmp_robot[1],h2r_ratio);
+//		fflush(fp);
   }
-  if(USEX)m_robot->rootLink()->p(0) += filt_hp_data.com[0];
-  m_robot->rootLink()->p(1) += filt_hp_data.com[1];
-  m_robot->rootLink()->p(2) += filt_hp_data.com[2];
-  for(int i=0;i<3;i++)hp_data_old.com[i] = filt_hp_data.com[i];
 
-static int movelcnt;
-	if(moveleft){
-		if(movelcnt<5000){
-			m_robot->rootLink()->p(1) += 0.00001;//遊び
-			movelcnt++;
-		}else{
-			movelcnt = 0;
-			moveleft = false;
-		}
-	}
+
+  if(USEX)m_robot->rootLink()->p(0) += h2r_ratio*filt_hp_data.com[0];
+  m_robot->rootLink()->p(1) += h2r_ratio*filt_hp_data.com[1];
+  m_robot->rootLink()->p(2) += h2r_ratio*filt_hp_data.com[2];
+  for(int i=0;i<3;i++)hp_data_old.com[i] = filt_hp_data.com[i];
 
   // Fix for toe joint
   for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index ff748dfff0e..16d0ec812ed 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -154,6 +154,8 @@ class AutoBalancer
   InPort<TimedPoint3D> m_htrhIn;
   TimedPoint3D m_htlh;
   InPort<TimedPoint3D> m_htlhIn;
+  TimedPoint3D m_actzmp;
+  InPort<TimedPoint3D> m_actzmpIn;
   
   // </rtc-template>
 
diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp
index 27a979ff67f..b5175201764 100644
--- a/rtc/Stabilizer/Stabilizer.cpp
+++ b/rtc/Stabilizer/Stabilizer.cpp
@@ -1045,7 +1045,6 @@ void Stabilizer::getTargetParameters ()
   m_robot->rootLink()->R = target_root_R;
   m_robot->calcForwardKinematics();
   ref_zmp = m_robot->rootLink()->R * hrp::Vector3(m_zmpRef.data.x, m_zmpRef.data.y, m_zmpRef.data.z) + m_robot->rootLink()->p; // base frame -> world frame
- std::cout<<"debug"<<m_zmpRef.data.x<<" , "<<m_zmpRef.data.y<<" , "<<m_zmpRef.data.z<<std::endl;
   if (st_algorithm == OpenHRP::StabilizerService::EEFM || st_algorithm == OpenHRP::StabilizerService::EEFMQP || st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP) {
     // apply inverse system
     hrp::Vector3 tmp_ref_zmp = ref_zmp + eefm_zmp_delay_time_const[0] * (ref_zmp - prev_ref_zmp) / dt;

From ce537255737e79c9922e9db7de742f563486c3bf Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 5 Jul 2016 18:31:02 +0900
Subject: [PATCH 008/217] fix wrong ratio between com and ZMP

---
 rtc/AutoBalancer/AutoBalancer.cpp | 89 ++++++++++---------------------
 1 file changed, 29 insertions(+), 60 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 444c7939285..528e3662620 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -388,30 +388,6 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     rot_ik_thre = (1e-2)*M_PI/180.0; // [rad]
     ik_error_debug_print_freq = static_cast<int>(0.2/m_dt); // once per 0.2 [s]
 
-
-//    //SHM my2
-//	if((key = ftok("/tmp/shm.dat", 'R')) == -1){
-//		printf("WARN: ftok() failed retry\n");
-//		system("touch /tmp/shm.dat");
-//		if((key = ftok("/tmp/shm.dat", 'R')) == -1){
-//	    	perror("ERROR: ftok() failed exit\n");
-//	        return RTC::RTC_ERROR;
-//		}
-//	}
-//	if((shmid = shmget(key, sizeof(humanpose_t), 0666)) < 0){
-//		printf("WARN: shmget() failed retry\n");
-//		shmid = shmget(key, sizeof(humanpose_t), S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH | IPC_CREAT);
-//	    if (shmid == -1)  {
-//	    	perror("ERROR: shmget() failed exit\n");
-//	        return RTC::RTC_ERROR;
-//	    }
-//	}
-//    humanpose = (humanpose_t *)shmat(shmid, (void *)0, 0);
-//    if (humanpose == NULL) {
-//        perror("shmat");
-//        return RTC::RTC_ERROR;
-//    }
-
     return RTC::RTC_OK;
 }
 
@@ -685,27 +661,14 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
 			humanpose.zmp[0] = 0;	humanpose.zmp[1] = 0;	humanpose.zmp[2] = 0;
 		}
 
-      //debug表示
-//			printf("[L] %+06.1f,%+06.1f,%+06.1f,%+06.1f,%+06.1f,%+06.1f [R] %+06.1f,%+06.1f,%+06.1f,%+06.1f,%+06.1f,%+06.1f [ZMP] %+06.3f,%+06.3f\n",
-//					humanpose.lfwrench[0],humanpose.lfwrench[1],humanpose.lfwrench[2],humanpose.lfwrench[3],humanpose.lfwrench[4],humanpose.lfwrench[5],
-//					humanpose.rfwrench[0],humanpose.rfwrench[1],humanpose.rfwrench[2],humanpose.rfwrench[3],humanpose.rfwrench[4],humanpose.rfwrench[5],
-//					humanpose.zmp[0],humanpose.zmp[1]);
-	   if(HumanSyncOn){
-//ZMP上書き
-			if(USEX)m_zmp.data.x = filt_hp_data.zmp[0]*0.5 - filt_hp_data.com[0];//
-			m_zmp.data.y = filt_hp_data.zmp[1]*0.5 - filt_hp_data.com[1];//
-//			if(USEX)m_zmp.data.x = filt_hp_data.zmp[0];//
-//			m_zmp.data.y = filt_hp_data.zmp[1];//
-
-//			std::cout<<"debug0"<<m_zmp.data.x<<" , "<<m_zmp.data.y<<" , "<<m_zmp.data.z<<std::endl;
-
-
-//		m_zmp.data.x = 0;//
-//		m_zmp.data.y = 0;//
-// COM上書き
-//		if(USEX)m_cog.data.x = filt_hp_data.com[0]*0.7;//ここの.comは今はhumanのbaseposなので適当調整
-//		m_cog.data.y = filt_hp_data.com[1]*0.7;
-	  }
+		if(HumanSyncOn){
+		//ZMP上書き
+			if(USEX)m_zmp.data.x = filt_hp_data.zmp[0] - m_robot->rootLink()->p(0);//
+			m_zmp.data.y = filt_hp_data.zmp[1] - m_robot->rootLink()->p(1);//
+		// COM上書き
+		//		if(USEX)m_cog.data.x = filt_hp_data.com[0]*0.7;//ここの.comは今はhumanのbaseposなので適当調整
+		//		m_cog.data.y = filt_hp_data.com[1]*0.7;
+		}
 
       // sbpCogOffset
       m_sbpCogOffset.data.x = sbp_cog_offset(0);
@@ -1065,6 +1028,7 @@ void AutoBalancer::getTargetParameters()
     }
     //ishiguro
     if (HumanSyncOn) {
+        ref_cog = tmp_ref_cog;
 		if(USEX)ref_zmp(0) = humanpose.zmp[0];
 		ref_zmp(1) = humanpose.zmp[1];
 		ref_zmp(2) = tmp_foot_mid_pos(2);
@@ -1103,14 +1067,14 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri
   m_robot->calcForwardKinematics();
 }
 
-static bool moveleft = false;
 static bool ht_first_call = true;
-hrp::Vector3 rfinitpos,lfinitpos;
+hrp::Vector3 rfinitpos,lfinitpos,baseinitpos;
 static int rfuplevel,lfuplevel;
 #define LEVELNUM 200
 #define FUPHIGHT 0.05
 bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 {
+	  if(HumanSyncOn){
 		if(param.target_link->name == "RLEG_JOINT5"){
 			if(humanpose.rfwrench[2] < 20.0 && humanpose.lfwrench[2] > 20.0 && HumanSyncOn){
 				if(rfuplevel < LEVELNUM){
@@ -1158,6 +1122,7 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 			param.target_p0(1) = 0.374 + filt_hp_data.lhpos[1];
 			param.target_p0(2) = 0.973 + filt_hp_data.lhpos[2];
 		}
+	  }
 
 
   param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
@@ -1209,12 +1174,11 @@ void AutoBalancer::solveLimbIK ()
   m_robot->rootLink()->R = target_root_R;
 //mytest
 #define MAXVEL 0.004
-#define MAXPOS 0.2
+#define MAXPOS 0.25
 #define FNUM 0.01
 
   static bool isCalibState=false;
-  static double h2r_ratio = 0.55;//human 1.1 vs chidori 0.69
-
+  static double h2r_ratio = 0.62;//human 1.1 vs chidori 0.69
   static humanpose_t init_humanpose;
   static int HumanSyncCountdownNum = 5 * (1/m_dt);
   if(startCountdownForHumanSync){
@@ -1228,13 +1192,14 @@ void AutoBalancer::solveLimbIK ()
 		  HumanSyncCountdownNum--;
 	  }
   }
-  if(ht_first_call){
-	  rfinitpos = ikp["rleg"].target_p0;
-	  lfinitpos = ikp["lleg"].target_p0;
-	  ht_first_call=false;
-  }
 
   if(HumanSyncOn){
+	  if(ht_first_call){
+		  rfinitpos = ikp["rleg"].target_p0;
+		  lfinitpos = ikp["lleg"].target_p0;
+		  baseinitpos = m_robot->rootLink()->p;
+		  ht_first_call=false;
+	  }
 	  for(int i=0;i<3;i++){
 		  filt_hp_data.zmp[i] = (1-FNUM)*filt_hp_data.zmp[i] + FNUM*(humanpose.zmp[i]);
 		  filt_hp_data.com[i] = (1-FNUM)*filt_hp_data.com[i] + FNUM*(humanpose.com[i] - init_humanpose.com[i] + init_humanpose.zmp[i]);//Sync開始時のzmpのオフセットはつまり重心のオフセット
@@ -1250,7 +1215,6 @@ void AutoBalancer::solveLimbIK ()
 		  if(filt_hp_data.com[i] > MAXPOS)filt_hp_data.com[i] = MAXPOS;
 		  if(filt_hp_data.com[i] < -MAXPOS)filt_hp_data.com[i] = -MAXPOS;
 	  }
-
 //	  if(loop%100==0)printf("[COM] %+06.3f,%+06.3f,%+06.3f [ZMP] %+06.3f,%+06.3f\n", filt_hp_data.com[0],filt_hp_data.com[1],filt_hp_data.com[2],filt_hp_data.zmp[0],filt_hp_data.zmp[1]);
   }else{
 	  for(int i=0;i<3;i++)init_humanpose.com[i] = humanpose.com[i];
@@ -1301,11 +1265,16 @@ void AutoBalancer::solveLimbIK ()
 //		fflush(fp);
   }
 
+  const double c2b_ratio = 1.38;
 
-  if(USEX)m_robot->rootLink()->p(0) += h2r_ratio*filt_hp_data.com[0];
-  m_robot->rootLink()->p(1) += h2r_ratio*filt_hp_data.com[1];
-  m_robot->rootLink()->p(2) += h2r_ratio*filt_hp_data.com[2];
-  for(int i=0;i<3;i++)hp_data_old.com[i] = filt_hp_data.com[i];
+  if(HumanSyncOn){
+	  if(USEX)m_robot->rootLink()->p(0) = baseinitpos(0) + h2r_ratio * c2b_ratio * filt_hp_data.com[0];
+	  m_robot->rootLink()->p(1) = baseinitpos(1) + h2r_ratio * c2b_ratio * filt_hp_data.com[1];
+	  m_robot->rootLink()->p(2) = baseinitpos(2) + h2r_ratio * c2b_ratio * filt_hp_data.com[2];
+	  std::cout<<"filt_hp_data.com[1]:"<<filt_hp_data.com[1]<<std::endl;
+	  std::cout<<"m_robot->rootLink()->p(1):"<<m_robot->rootLink()->p(1)<<std::endl;
+	  for(int i=0;i<3;i++)hp_data_old.com[i] = filt_hp_data.com[i];
+  }
 
   // Fix for toe joint
   for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {

From 89d980a8093eead154342172316a270b5c1ba4f6 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 12 Jul 2016 23:27:02 +0900
Subject: [PATCH 009/217] fix left foot contact state

---
 rtc/AutoBalancer/AutoBalancer.cpp | 35 ++++++++++++++++++-------------
 1 file changed, 20 insertions(+), 15 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 528e3662620..34a4338eabf 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -439,6 +439,10 @@ RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
   return RTC::RTC_OK;
 }
 
+
+static double h2r_ratio = 0.62;//human 1.1 vs chidori 0.69
+
+
 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
 //#define DEBUGP2 ((loop%200==0))
 #define DEBUGP2 (false)
@@ -641,7 +645,7 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
 
       double rfzmp[2],lfzmp[2];
       const double F_H_OFFSET = 0.03;
-      const double rfpos[3] = {0,-0.1,0},lfpos[3] = {0,0.1,0};
+      const double rfpos[3] = {0,-0.1/h2r_ratio,0},lfpos[3] = {0,0.1/h2r_ratio,0};
 
 		if( humanpose.rfwrench[2] > 1.0e-6 ){
 			rfzmp[0] = ( -humanpose.rfwrench[4] - humanpose.rfwrench[0] * F_H_OFFSET + humanpose.rfwrench[2] * 0 ) / humanpose.rfwrench[2] + rfpos[0];
@@ -663,11 +667,11 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
 
 		if(HumanSyncOn){
 		//ZMP上書き
-			if(USEX)m_zmp.data.x = filt_hp_data.zmp[0] - m_robot->rootLink()->p(0);//
-			m_zmp.data.y = filt_hp_data.zmp[1] - m_robot->rootLink()->p(1);//
-		// COM上書き
-		//		if(USEX)m_cog.data.x = filt_hp_data.com[0]*0.7;//ここの.comは今はhumanのbaseposなので適当調整
-		//		m_cog.data.y = filt_hp_data.com[1]*0.7;
+			if(USEX)m_zmp.data.x = h2r_ratio*filt_hp_data.zmp[0] - m_robot->rootLink()->p(0);
+			m_zmp.data.y = h2r_ratio*filt_hp_data.zmp[1] - m_robot->rootLink()->p(1);//
+//		// COM上書き
+//			if(USEX)m_cog.data.x = filt_hp_data.com[0]*0.7;//ここの.comは今はhumanのbaseposなので適当調整
+//			m_cog.data.y = filt_hp_data.com[1]*0.7;
 		}
 
       // sbpCogOffset
@@ -716,11 +720,6 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
         m_limbCOPOffsetOut[i]->write();
     }
 
-//ishiguro
-//    std::cout<<"out "<<m_controlSwingSupportTime.data[contact_states_index_map["rleg"]]<<" "<<gg->get_current_swing_time(0)<<" "<<m_controlSwingSupportTime.data[contact_states_index_map["lleg"]]<<" "<<gg->get_current_swing_time(1)<<std::endl;
-//    std::cout<<"out2"<<m_controlSwingSupportTime.data[contact_states_index_map[2]]<<" , "<<gg->get_current_swing_time(2)<<std::endl;
-//    std::cout<<"out3"<<m_controlSwingSupportTime.data[contact_states_index_map[3]]<<" , "<<gg->get_current_swing_time(3)<<std::endl;
-
     return RTC::RTC_OK;
 }
 
@@ -1028,7 +1027,7 @@ void AutoBalancer::getTargetParameters()
     }
     //ishiguro
     if (HumanSyncOn) {
-        ref_cog = tmp_ref_cog;
+//        ref_cog = tmp_ref_cog;
 		if(USEX)ref_zmp(0) = humanpose.zmp[0];
 		ref_zmp(1) = humanpose.zmp[1];
 		ref_zmp(2) = tmp_foot_mid_pos(2);
@@ -1102,10 +1101,13 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 					filt_hp_data.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
 				}
 				if(loop%100==0)cout<<"LUP"<<endl;
+		        m_contactStates.data[contact_states_index_map["lleg"]] = false;
 			}else{
 				if(lfuplevel > 0){
 					lfuplevel--;
 					filt_hp_data.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
+				}else{
+			        m_contactStates.data[contact_states_index_map["lleg"]] = true;
 				}
 			}
 			param.target_p0(0) = lfinitpos(0) + filt_hp_data.lfpos[0];
@@ -1178,7 +1180,6 @@ void AutoBalancer::solveLimbIK ()
 #define FNUM 0.01
 
   static bool isCalibState=false;
-  static double h2r_ratio = 0.62;//human 1.1 vs chidori 0.69
   static humanpose_t init_humanpose;
   static int HumanSyncCountdownNum = 5 * (1/m_dt);
   if(startCountdownForHumanSync){
@@ -1271,8 +1272,12 @@ void AutoBalancer::solveLimbIK ()
 	  if(USEX)m_robot->rootLink()->p(0) = baseinitpos(0) + h2r_ratio * c2b_ratio * filt_hp_data.com[0];
 	  m_robot->rootLink()->p(1) = baseinitpos(1) + h2r_ratio * c2b_ratio * filt_hp_data.com[1];
 	  m_robot->rootLink()->p(2) = baseinitpos(2) + h2r_ratio * c2b_ratio * filt_hp_data.com[2];
-	  std::cout<<"filt_hp_data.com[1]:"<<filt_hp_data.com[1]<<std::endl;
-	  std::cout<<"m_robot->rootLink()->p(1):"<<m_robot->rootLink()->p(1)<<std::endl;
+	// COM上書き
+		if(USEX)ref_cog(0) = baseinitpos(0) + h2r_ratio * filt_hp_data.com[0];
+		ref_cog(1) = baseinitpos(1) + h2r_ratio * filt_hp_data.com[1];
+		ref_cog(2) = baseinitpos(2) + h2r_ratio * filt_hp_data.com[2];
+//	  std::cout<<"filt_hp_data.com[1]:"<<filt_hp_data.com[1]<<std::endl;
+//	  std::cout<<"m_robot->rootLink()->p(1):"<<m_robot->rootLink()->p(1)<<std::endl;
 	  for(int i=0;i<3;i++)hp_data_old.com[i] = filt_hp_data.com[i];
   }
 

From ebb3343dd16f48a795a0c9e790f0c1951f189ea8 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 20 Jul 2016 20:52:04 +0900
Subject: [PATCH 010/217] enable XY foot move

---
 rtc/AutoBalancer/AutoBalancer.cpp | 44 ++++++++++++++++++++++++-------
 1 file changed, 34 insertions(+), 10 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 34a4338eabf..96f44f58e71 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -441,7 +441,7 @@ RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
 
 
 static double h2r_ratio = 0.62;//human 1.1 vs chidori 0.69
-
+hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
 
 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
 //#define DEBUGP2 ((loop%200==0))
@@ -645,7 +645,12 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
 
       double rfzmp[2],lfzmp[2];
       const double F_H_OFFSET = 0.03;
-      const double rfpos[3] = {0,-0.1/h2r_ratio,0},lfpos[3] = {0,0.1/h2r_ratio,0};
+      double rfpos[3] = {0,-0.1/h2r_ratio,0},lfpos[3] = {0,0.1/h2r_ratio,0};
+
+      rfpos[0] = pre_cont_rfpos(0);
+      rfpos[1] = -0.1/h2r_ratio + pre_cont_rfpos(1);
+      lfpos[0] = pre_cont_lfpos(0);
+      lfpos[1] = 0.1/h2r_ratio + pre_cont_lfpos(1);
 
 		if( humanpose.rfwrench[2] > 1.0e-6 ){
 			rfzmp[0] = ( -humanpose.rfwrench[4] - humanpose.rfwrench[0] * F_H_OFFSET + humanpose.rfwrench[2] * 0 ) / humanpose.rfwrench[2] + rfpos[0];
@@ -1069,6 +1074,7 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri
 static bool ht_first_call = true;
 hrp::Vector3 rfinitpos,lfinitpos,baseinitpos;
 static int rfuplevel,lfuplevel;
+static humanpose_t init_humanpose;
 #define LEVELNUM 200
 #define FUPHIGHT 0.05
 bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
@@ -1079,6 +1085,8 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 				if(rfuplevel < LEVELNUM){
 					rfuplevel++;
 					filt_hp_data.rfpos[2] = FUPHIGHT/2*(1-cos(M_PI*rfuplevel/LEVELNUM));
+				}else{
+					filt_hp_data.rfpos[2] = FUPHIGHT;
 				}
 				if(loop%100==0)cout<<"RUP"<<endl;
 		        m_contactStates.data[contact_states_index_map["rleg"]] = false;
@@ -1086,12 +1094,19 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 				if(rfuplevel > 0){
 					rfuplevel--;
 					filt_hp_data.rfpos[2] = FUPHIGHT/2*(1-cos(M_PI*rfuplevel/LEVELNUM));
+					if(rfuplevel==0){
+						pre_cont_rfpos(0) = filt_hp_data.rfpos[0];
+						pre_cont_rfpos(1) = filt_hp_data.rfpos[1];
+					}
 				}else{
 			        m_contactStates.data[contact_states_index_map["rleg"]] = true;
+					filt_hp_data.rfpos[0] = pre_cont_rfpos(0);
+					filt_hp_data.rfpos[1] = pre_cont_rfpos(1);
+					filt_hp_data.rfpos[2] = 0;
 				}
 			}
-			param.target_p0(0) = rfinitpos(0) + filt_hp_data.rfpos[0];
-			param.target_p0(1) = rfinitpos(1) + filt_hp_data.rfpos[1];
+			param.target_p0(0) = rfinitpos(0) + h2r_ratio * filt_hp_data.rfpos[0];
+			param.target_p0(1) = rfinitpos(1) + h2r_ratio * filt_hp_data.rfpos[1];
 			param.target_p0(2) = rfinitpos(2) + filt_hp_data.rfpos[2];
 		}
 		if(param.target_link->name == "LLEG_JOINT5"){
@@ -1099,6 +1114,8 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 				if(lfuplevel < LEVELNUM){
 					lfuplevel++;
 					filt_hp_data.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
+				}else{
+					filt_hp_data.lfpos[2] = FUPHIGHT;
 				}
 				if(loop%100==0)cout<<"LUP"<<endl;
 		        m_contactStates.data[contact_states_index_map["lleg"]] = false;
@@ -1106,12 +1123,19 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 				if(lfuplevel > 0){
 					lfuplevel--;
 					filt_hp_data.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
+					if(lfuplevel==0){
+						pre_cont_lfpos(0) = filt_hp_data.lfpos[0];
+						pre_cont_lfpos(1) = filt_hp_data.lfpos[1];
+					}
 				}else{
 			        m_contactStates.data[contact_states_index_map["lleg"]] = true;
+			        filt_hp_data.lfpos[0] = pre_cont_lfpos(0);
+			        filt_hp_data.lfpos[1] = pre_cont_lfpos(1);
+			        filt_hp_data.lfpos[2] = 0;
 				}
 			}
-			param.target_p0(0) = lfinitpos(0) + filt_hp_data.lfpos[0];
-			param.target_p0(1) = lfinitpos(1) + filt_hp_data.lfpos[1];
+			param.target_p0(0) = lfinitpos(0) + h2r_ratio * filt_hp_data.lfpos[0];
+			param.target_p0(1) = lfinitpos(1) + h2r_ratio * filt_hp_data.lfpos[1];
 			param.target_p0(2) = lfinitpos(2) + filt_hp_data.lfpos[2];
 		}
 		if(param.target_link->name == "RARM_JOINT7" && HumanSyncOn){
@@ -1176,11 +1200,11 @@ void AutoBalancer::solveLimbIK ()
   m_robot->rootLink()->R = target_root_R;
 //mytest
 #define MAXVEL 0.004
-#define MAXPOS 0.25
+//#define MAXPOS 0.25
+#define MAXPOS 1000//ほぼ無制限
 #define FNUM 0.01
 
   static bool isCalibState=false;
-  static humanpose_t init_humanpose;
   static int HumanSyncCountdownNum = 5 * (1/m_dt);
   if(startCountdownForHumanSync){
 	  printf("Start Count Down for HumanSync [%d]\r", HumanSyncCountdownNum);
@@ -1206,8 +1230,8 @@ void AutoBalancer::solveLimbIK ()
 		  filt_hp_data.com[i] = (1-FNUM)*filt_hp_data.com[i] + FNUM*(humanpose.com[i] - init_humanpose.com[i] + init_humanpose.zmp[i]);//Sync開始時のzmpのオフセットはつまり重心のオフセット
 		  filt_hp_data.rfwrench[i] = (1-FNUM)*filt_hp_data.rfwrench[i] + FNUM*(humanpose.rfwrench[i]);
 		  filt_hp_data.lfwrench[i] = (1-FNUM)*filt_hp_data.lfwrench[i] + FNUM*(humanpose.lfwrench[i]);
-	//	  filt_hp_data.rfpos[i] = (1-FNUM)*filt_hp_data.rfpos[i] + FNUM*(humanpose.rfpos[i] - init_humanpose.rfpos[i]);//足ロック
-	//	  filt_hp_data.lfpos[i] = (1-FNUM)*filt_hp_data.lfpos[i] + FNUM*(humanpose.lfpos[i] - init_humanpose.lfpos[i]);//足ロック
+		  filt_hp_data.rfpos[i] = (1-FNUM)*filt_hp_data.rfpos[i] + FNUM*(humanpose.rfpos[i] - init_humanpose.rfpos[i]);//足ロック
+		  filt_hp_data.lfpos[i] = (1-FNUM)*filt_hp_data.lfpos[i] + FNUM*(humanpose.lfpos[i] - init_humanpose.lfpos[i]);//足ロック
 		  filt_hp_data.rhpos[i] = (1-FNUM)*filt_hp_data.rhpos[i] + FNUM*(humanpose.rhpos[i] - init_humanpose.rhpos[i]);
 		  filt_hp_data.lhpos[i] = (1-FNUM)*filt_hp_data.lhpos[i] + FNUM*(humanpose.lhpos[i] - init_humanpose.lhpos[i]);
 

From 2e1377eba428b4298192d556039d68f346daaceb Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 25 Jul 2016 23:14:46 +0900
Subject: [PATCH 011/217] enable COM IK

---
 rtc/AutoBalancer/AutoBalancer.cpp | 341 +++++++++++++++++-------------
 rtc/AutoBalancer/AutoBalancer.h   |   3 +
 2 files changed, 193 insertions(+), 151 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 96f44f58e71..b1320c19982 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -24,19 +24,30 @@
 //#include <sys/shm.h>
 //int     shmid;
 //key_t   key;
+//typedef struct{
+//	double com[3];
+//	double rfpos[3];
+//	double lfpos[3];
+//	double rhpos[3];
+//	double lhpos[3];
+//	double rfwrench[6];
+//	double lfwrench[6];
+//	double zmp[3];
+//	bool startsync;
+//}humanpose_t;
 typedef struct{
-	double com[3];
-	double rfpos[3];
-	double lfpos[3];
-	double rhpos[3];
-	double lhpos[3];
+	hrp::Vector3 com;
+	hrp::Vector3 zmp;
+	hrp::Vector3 rfpos;
+	hrp::Vector3 lfpos;
+	hrp::Vector3 rhpos;
+	hrp::Vector3 lhpos;
 	double rfwrench[6];
 	double lfwrench[6];
-	double zmp[3];
 	bool startsync;
 }humanpose_t;
 humanpose_t humanpose;
-humanpose_t filt_hp_data,hp_data_old;
+humanpose_t robot_ref_pose,hp_data_old;
 bool startCountdownForHumanSync = false;
 bool HumanSyncOn = false;
 #define USEX true
@@ -672,11 +683,11 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
 
 		if(HumanSyncOn){
 		//ZMP上書き
-			if(USEX)m_zmp.data.x = h2r_ratio*filt_hp_data.zmp[0] - m_robot->rootLink()->p(0);
-			m_zmp.data.y = h2r_ratio*filt_hp_data.zmp[1] - m_robot->rootLink()->p(1);//
+			if(USEX)m_zmp.data.x = robot_ref_pose.zmp[0] - m_robot->rootLink()->p(0);
+			m_zmp.data.y = robot_ref_pose.zmp[1] - m_robot->rootLink()->p(1);//
 //		// COM上書き
-//			if(USEX)m_cog.data.x = filt_hp_data.com[0]*0.7;//ここの.comは今はhumanのbaseposなので適当調整
-//			m_cog.data.y = filt_hp_data.com[1]*0.7;
+//			if(USEX)m_cog.data.x = robot_ref_pose.com(0)*0.7;//ここの.comは今はhumanのbaseposなので適当調整
+//			m_cog.data.y = robot_ref_pose.com(1)*0.7;
 		}
 
       // sbpCogOffset
@@ -1079,78 +1090,6 @@ static humanpose_t init_humanpose;
 #define FUPHIGHT 0.05
 bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 {
-	  if(HumanSyncOn){
-		if(param.target_link->name == "RLEG_JOINT5"){
-			if(humanpose.rfwrench[2] < 20.0 && humanpose.lfwrench[2] > 20.0 && HumanSyncOn){
-				if(rfuplevel < LEVELNUM){
-					rfuplevel++;
-					filt_hp_data.rfpos[2] = FUPHIGHT/2*(1-cos(M_PI*rfuplevel/LEVELNUM));
-				}else{
-					filt_hp_data.rfpos[2] = FUPHIGHT;
-				}
-				if(loop%100==0)cout<<"RUP"<<endl;
-		        m_contactStates.data[contact_states_index_map["rleg"]] = false;
-			}else{
-				if(rfuplevel > 0){
-					rfuplevel--;
-					filt_hp_data.rfpos[2] = FUPHIGHT/2*(1-cos(M_PI*rfuplevel/LEVELNUM));
-					if(rfuplevel==0){
-						pre_cont_rfpos(0) = filt_hp_data.rfpos[0];
-						pre_cont_rfpos(1) = filt_hp_data.rfpos[1];
-					}
-				}else{
-			        m_contactStates.data[contact_states_index_map["rleg"]] = true;
-					filt_hp_data.rfpos[0] = pre_cont_rfpos(0);
-					filt_hp_data.rfpos[1] = pre_cont_rfpos(1);
-					filt_hp_data.rfpos[2] = 0;
-				}
-			}
-			param.target_p0(0) = rfinitpos(0) + h2r_ratio * filt_hp_data.rfpos[0];
-			param.target_p0(1) = rfinitpos(1) + h2r_ratio * filt_hp_data.rfpos[1];
-			param.target_p0(2) = rfinitpos(2) + filt_hp_data.rfpos[2];
-		}
-		if(param.target_link->name == "LLEG_JOINT5"){
-			if(humanpose.lfwrench[2] < 20.0 && humanpose.rfwrench[2] > 20.0 && HumanSyncOn){
-				if(lfuplevel < LEVELNUM){
-					lfuplevel++;
-					filt_hp_data.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
-				}else{
-					filt_hp_data.lfpos[2] = FUPHIGHT;
-				}
-				if(loop%100==0)cout<<"LUP"<<endl;
-		        m_contactStates.data[contact_states_index_map["lleg"]] = false;
-			}else{
-				if(lfuplevel > 0){
-					lfuplevel--;
-					filt_hp_data.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
-					if(lfuplevel==0){
-						pre_cont_lfpos(0) = filt_hp_data.lfpos[0];
-						pre_cont_lfpos(1) = filt_hp_data.lfpos[1];
-					}
-				}else{
-			        m_contactStates.data[contact_states_index_map["lleg"]] = true;
-			        filt_hp_data.lfpos[0] = pre_cont_lfpos(0);
-			        filt_hp_data.lfpos[1] = pre_cont_lfpos(1);
-			        filt_hp_data.lfpos[2] = 0;
-				}
-			}
-			param.target_p0(0) = lfinitpos(0) + h2r_ratio * filt_hp_data.lfpos[0];
-			param.target_p0(1) = lfinitpos(1) + h2r_ratio * filt_hp_data.lfpos[1];
-			param.target_p0(2) = lfinitpos(2) + filt_hp_data.lfpos[2];
-		}
-		if(param.target_link->name == "RARM_JOINT7" && HumanSyncOn){
-			param.target_p0(0) = 0.151 + filt_hp_data.rhpos[0];
-			param.target_p0(1) = -0.374 + filt_hp_data.rhpos[1];
-			param.target_p0(2) = 0.973 + filt_hp_data.rhpos[2];
-		}
-		if(param.target_link->name == "LARM_JOINT7" && HumanSyncOn){
-			param.target_p0(0) = 0.151 + filt_hp_data.lhpos[0];
-			param.target_p0(1) = 0.374 + filt_hp_data.lhpos[1];
-			param.target_p0(2) = 0.973 + filt_hp_data.lhpos[2];
-		}
-	  }
-
-
   param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
   // IK check
   hrp::Vector3 vel_p, vel_r;
@@ -1178,6 +1117,7 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 }
 
 
+
 void AutoBalancer::solveLimbIK ()
 {
   for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
@@ -1220,91 +1160,163 @@ void AutoBalancer::solveLimbIK ()
 
   if(HumanSyncOn){
 	  if(ht_first_call){
-		  rfinitpos = ikp["rleg"].target_p0;
-		  lfinitpos = ikp["lleg"].target_p0;
+		  rfinitpos = ikp["rleg"].target_link->p;
+		  lfinitpos = ikp["lleg"].target_link->p;
 		  baseinitpos = m_robot->rootLink()->p;
 		  ht_first_call=false;
 	  }
+	  ///////入力データによる更新/////
+	  robot_ref_pose.zmp = (1-FNUM)*robot_ref_pose.zmp + FNUM*(h2r_ratio*humanpose.zmp);
+	  robot_ref_pose.com = (1-FNUM)*robot_ref_pose.com + FNUM*(h2r_ratio*(humanpose.com - init_humanpose.com + init_humanpose.zmp));//Sync開始時のzmpのオフセットはつまり重心のオフセット
+	  robot_ref_pose.rfpos = (1-FNUM)*robot_ref_pose.rfpos + FNUM*(humanpose.rfpos - init_humanpose.rfpos);//足ロック
+	  robot_ref_pose.lfpos = (1-FNUM)*robot_ref_pose.lfpos + FNUM*(humanpose.lfpos - init_humanpose.lfpos);//足ロック
+	  robot_ref_pose.rhpos = (1-FNUM)*robot_ref_pose.rhpos + FNUM*(humanpose.rhpos - init_humanpose.rhpos);
+	  robot_ref_pose.lhpos = (1-FNUM)*robot_ref_pose.lhpos + FNUM*(humanpose.lhpos - init_humanpose.lhpos);
+	  for(int i=0;i<6;i++){
+		  robot_ref_pose.rfwrench[i] = (1-FNUM)*robot_ref_pose.rfwrench[i] + FNUM*(humanpose.rfwrench[i]);
+		  robot_ref_pose.lfwrench[i] = (1-FNUM)*robot_ref_pose.lfwrench[i] + FNUM*(humanpose.lfwrench[i]);
+	  }
+	  ///////指令値範囲の制限/////
 	  for(int i=0;i<3;i++){
-		  filt_hp_data.zmp[i] = (1-FNUM)*filt_hp_data.zmp[i] + FNUM*(humanpose.zmp[i]);
-		  filt_hp_data.com[i] = (1-FNUM)*filt_hp_data.com[i] + FNUM*(humanpose.com[i] - init_humanpose.com[i] + init_humanpose.zmp[i]);//Sync開始時のzmpのオフセットはつまり重心のオフセット
-		  filt_hp_data.rfwrench[i] = (1-FNUM)*filt_hp_data.rfwrench[i] + FNUM*(humanpose.rfwrench[i]);
-		  filt_hp_data.lfwrench[i] = (1-FNUM)*filt_hp_data.lfwrench[i] + FNUM*(humanpose.lfwrench[i]);
-		  filt_hp_data.rfpos[i] = (1-FNUM)*filt_hp_data.rfpos[i] + FNUM*(humanpose.rfpos[i] - init_humanpose.rfpos[i]);//足ロック
-		  filt_hp_data.lfpos[i] = (1-FNUM)*filt_hp_data.lfpos[i] + FNUM*(humanpose.lfpos[i] - init_humanpose.lfpos[i]);//足ロック
-		  filt_hp_data.rhpos[i] = (1-FNUM)*filt_hp_data.rhpos[i] + FNUM*(humanpose.rhpos[i] - init_humanpose.rhpos[i]);
-		  filt_hp_data.lhpos[i] = (1-FNUM)*filt_hp_data.lhpos[i] + FNUM*(humanpose.lhpos[i] - init_humanpose.lhpos[i]);
-
-		  if(filt_hp_data.com[i] - hp_data_old.com[i] > MAXVEL)filt_hp_data.com[i] = hp_data_old.com[i] + MAXVEL;
-		  if(filt_hp_data.com[i] - hp_data_old.com[i] < -MAXVEL)filt_hp_data.com[i] = hp_data_old.com[i] - MAXVEL;
-		  if(filt_hp_data.com[i] > MAXPOS)filt_hp_data.com[i] = MAXPOS;
-		  if(filt_hp_data.com[i] < -MAXPOS)filt_hp_data.com[i] = -MAXPOS;
+		  if(robot_ref_pose.com[i] - hp_data_old.com[i] > MAXVEL)robot_ref_pose.com[i] = hp_data_old.com[i] + MAXVEL;
+		  if(robot_ref_pose.com[i] - hp_data_old.com[i] < -MAXVEL)robot_ref_pose.com[i] = hp_data_old.com[i] - MAXVEL;
+		  if(robot_ref_pose.com[i] > MAXPOS)robot_ref_pose.com[i] = MAXPOS;
+		  if(robot_ref_pose.com[i] < -MAXPOS)robot_ref_pose.com[i] = -MAXPOS;
 	  }
-//	  if(loop%100==0)printf("[COM] %+06.3f,%+06.3f,%+06.3f [ZMP] %+06.3f,%+06.3f\n", filt_hp_data.com[0],filt_hp_data.com[1],filt_hp_data.com[2],filt_hp_data.zmp[0],filt_hp_data.zmp[1]);
+//	  if(robot_ref_pose.rfpos(1) > -0.07)robot_ref_pose.rfpos(1) = -0.07;
+//	  if(robot_ref_pose.lfpos(1) <  0.07)robot_ref_pose.lfpos(1) =  0.07;
+
+
+
+//	  if(loop%100==0)printf("[COM] %+06.3f,%+06.3f,%+06.3f [ZMP] %+06.3f,%+06.3f\n", robot_ref_pose.com(0),robot_ref_pose.com(1),robot_ref_pose.com(2),robot_ref_pose.zmp[0],robot_ref_pose.zmp[1]);
   }else{
-	  for(int i=0;i<3;i++)init_humanpose.com[i] = humanpose.com[i];
-	  for(int i=0;i<3;i++)init_humanpose.zmp[i] = humanpose.zmp[i];
-	  for(int i=0;i<3;i++)init_humanpose.rfpos[i] = humanpose.rfpos[i];
-	  for(int i=0;i<3;i++)init_humanpose.lfpos[i] = humanpose.lfpos[i];
-	  for(int i=0;i<3;i++)init_humanpose.rhpos[i] = humanpose.rhpos[i];
-	  for(int i=0;i<3;i++)init_humanpose.lhpos[i] = humanpose.lhpos[i];
+	  init_humanpose.com = humanpose.com;
+	  init_humanpose.zmp = humanpose.zmp;
+	  init_humanpose.rfpos = humanpose.rfpos;
+	  init_humanpose.lfpos = humanpose.lfpos;
+	  init_humanpose.rhpos = humanpose.rhpos;
+	  init_humanpose.lhpos = humanpose.lhpos;
 	  for(int i=0;i<3;i++){
-		  if(filt_hp_data.com[i]>0)filt_hp_data.com[i] -= 0.0001;
-		  if(filt_hp_data.com[i]<0)filt_hp_data.com[i] += 0.0001;
+		  if(robot_ref_pose.com[i]>0)robot_ref_pose.com[i] -= 0.0001;
+		  if(robot_ref_pose.com[i]<0)robot_ref_pose.com[i] += 0.0001;
 	  }
 //	  if(loop%100==0)printf("[pre-COM] %+06.3f,%+06.3f,%+06.3f [pre-ZMP] %+06.3f,%+06.3f\n", humanpose.com[0],humanpose.com[1],humanpose.com[2],humanpose.zmp[0],humanpose.zmp[1]);
   }
 
-  static double filt_actzmp[3];
-  if(isCalibState){
-	  filt_actzmp[0] = (1-FNUM)*filt_actzmp[0] + FNUM*m_actzmp.data.x;
-	  filt_actzmp[1] = (1-FNUM)*filt_actzmp[1] + FNUM*m_actzmp.data.y;
-	  filt_actzmp[2] = (1-FNUM)*filt_actzmp[2] + FNUM*m_actzmp.data.z;
-	    const int errnum = 500*5;
-	    const int errdelay = 500;
-	    double zmp_robot[3],zmp_human[3];
-	    static double zmp_human_old[3][errdelay];
-	    static double err_sum[3] = {0};
-	    for(int i=0;i<3;i++){
-		    for(int j=errdelay-1;j>0;j--){
-		    	zmp_human_old[i][j] = zmp_human_old[i][j-1];
-		    }
-		    zmp_human_old[i][0] = filt_hp_data.zmp[i] - current_root_p(i);
-		    zmp_human[i] = filt_hp_data.zmp[i] - current_root_p(i);
-//		    zmp_human[i] = zmp_human_old[i][errdelay-1];//位相遅れ見越して
-			zmp_robot[i] = filt_actzmp[i];
-			err_sum[i] += fabs(zmp_human[i]) - fabs(zmp_robot[i]);
-	    }
-
-//	    if(loop%errnum==0){
-//			if(err_sum[1]/errnum>0.01 ){h2r_ratio -= 0.01;std::cout<<"h2r_ratio UP:"<<h2r_ratio<<" (EP):"<<err_sum[1]<<std::endl;}
-//			else if(err_sum[1]/errnum<-0.01 ){h2r_ratio += 0.01;std::cout<<"h2r_ratio DOWN:"<<h2r_ratio<<" (EP):"<<err_sum[1]<<std::endl;}
-//			else{std::cout<<"h2r_ratio KEEP:"<<h2r_ratio<<" (EP):"<<err_sum[1]<<std::endl;}
+//  static double filt_actzmp[3];
+//  if(isCalibState){
+//	  filt_actzmp[0] = (1-FNUM)*filt_actzmp[0] + FNUM*m_actzmp.data.x;
+//	  filt_actzmp[1] = (1-FNUM)*filt_actzmp[1] + FNUM*m_actzmp.data.y;
+//	  filt_actzmp[2] = (1-FNUM)*filt_actzmp[2] + FNUM*m_actzmp.data.z;
+//	    const int errnum = 500*5;
+//	    const int errdelay = 500;
+//	    double zmp_robot[3],zmp_human[3];
+//	    static double zmp_human_old[3][errdelay];
+//	    static double err_sum[3] = {0};
+//	    for(int i=0;i<3;i++){
+//		    for(int j=errdelay-1;j>0;j--){
+//		    	zmp_human_old[i][j] = zmp_human_old[i][j-1];
+//		    }
+//		    zmp_human_old[i][0] = robot_ref_pose.zmp[i] - current_root_p(i);
+//		    zmp_human[i] = robot_ref_pose.zmp[i] - current_root_p(i);
+////		    zmp_human[i] = zmp_human_old[i][errdelay-1];//位相遅れ見越して
+//			zmp_robot[i] = filt_actzmp[i];
+//			err_sum[i] += fabs(zmp_human[i]) - fabs(zmp_robot[i]);
 //	    }
+//
+////	    if(loop%errnum==0){
+////			if(err_sum[1]/errnum>0.01 ){h2r_ratio -= 0.01;std::cout<<"h2r_ratio UP:"<<h2r_ratio<<" (EP):"<<err_sum[1]<<std::endl;}
+////			else if(err_sum[1]/errnum<-0.01 ){h2r_ratio += 0.01;std::cout<<"h2r_ratio DOWN:"<<h2r_ratio<<" (EP):"<<err_sum[1]<<std::endl;}
+////			else{std::cout<<"h2r_ratio KEEP:"<<h2r_ratio<<" (EP):"<<err_sum[1]<<std::endl;}
+////	    }
+//
+//	    //debug
+////	    static FILE *fp;
+////	    static bool fp_isOpen=false;
+////		if(!fp_isOpen){fp = fopen("/home/ishiguro/hcflog/nilog.log","w+");fp_isOpen=true;}
+////		if(fp!=NULL)fprintf(fp,"zmp: %f %f %f %f %f\n",zmp_human[0],zmp_human[1],zmp_robot[0],zmp_robot[1],h2r_ratio);
+////		fflush(fp);
+//  }
 
-	    //debug
-//	    static FILE *fp;
-//	    static bool fp_isOpen=false;
-//		if(!fp_isOpen){fp = fopen("/home/ishiguro/hcflog/nilog.log","w+");fp_isOpen=true;}
-//		if(fp!=NULL)fprintf(fp,"zmp: %f %f %f %f %f\n",zmp_human[0],zmp_human[1],zmp_robot[0],zmp_robot[1],h2r_ratio);
-//		fflush(fp);
-  }
 
-  const double c2b_ratio = 1.38;
 
   if(HumanSyncOn){
-	  if(USEX)m_robot->rootLink()->p(0) = baseinitpos(0) + h2r_ratio * c2b_ratio * filt_hp_data.com[0];
-	  m_robot->rootLink()->p(1) = baseinitpos(1) + h2r_ratio * c2b_ratio * filt_hp_data.com[1];
-	  m_robot->rootLink()->p(2) = baseinitpos(2) + h2r_ratio * c2b_ratio * filt_hp_data.com[2];
+	  ////////////////////// 右足拘束位置設定 /////////////////////////
+	if(humanpose.rfwrench[2] < 20.0 && humanpose.lfwrench[2] > 20.0 && HumanSyncOn){
+		if(rfuplevel < LEVELNUM){
+			rfuplevel++;
+			robot_ref_pose.rfpos[2] = FUPHIGHT/2*(1-cos(M_PI*rfuplevel/LEVELNUM));
+		}else{
+			robot_ref_pose.rfpos[2] = FUPHIGHT;
+		}
+		if(loop%100==0)cout<<"RUP"<<endl;
+		m_contactStates.data[contact_states_index_map["rleg"]] = false;
+	}else{
+		if(rfuplevel > 0){
+			rfuplevel--;
+			robot_ref_pose.rfpos[2] = FUPHIGHT/2*(1-cos(M_PI*rfuplevel/LEVELNUM));
+			if(rfuplevel==0){
+				pre_cont_rfpos(0) = robot_ref_pose.rfpos[0];
+				pre_cont_rfpos(1) = robot_ref_pose.rfpos[1];
+			}
+		}else{
+			m_contactStates.data[contact_states_index_map["rleg"]] = true;
+			robot_ref_pose.rfpos[0] = pre_cont_rfpos(0);
+			robot_ref_pose.rfpos[1] = pre_cont_rfpos(1);
+			robot_ref_pose.rfpos[2] = 0;
+		}
+	}
+	ikp["rleg"].target_p0 = rfinitpos + robot_ref_pose.rfpos;
+
+	  ////////////////////// 左足拘束位置設定 /////////////////////////
+	if(humanpose.lfwrench[2] < 20.0 && humanpose.rfwrench[2] > 20.0 && HumanSyncOn){
+		if(lfuplevel < LEVELNUM){
+			lfuplevel++;
+			robot_ref_pose.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
+		}else{
+			robot_ref_pose.lfpos[2] = FUPHIGHT;
+		}
+		if(loop%100==0)cout<<"LUP"<<endl;
+		m_contactStates.data[contact_states_index_map["lleg"]] = false;
+	}else{
+		if(lfuplevel > 0){
+			lfuplevel--;
+			robot_ref_pose.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
+			if(lfuplevel==0){
+				pre_cont_lfpos(0) = robot_ref_pose.lfpos[0];
+				pre_cont_lfpos(1) = robot_ref_pose.lfpos[1];
+			}
+		}else{
+			m_contactStates.data[contact_states_index_map["lleg"]] = true;
+			robot_ref_pose.lfpos[0] = pre_cont_lfpos(0);
+			robot_ref_pose.lfpos[1] = pre_cont_lfpos(1);
+			robot_ref_pose.lfpos[2] = 0;
+		}
+	}
+	ikp["lleg"].target_p0 = lfinitpos + robot_ref_pose.lfpos;
+
+	  ////////////////////// 右足拘束位置設定 /////////////////////////
+//	if(param.target_link->name == "RARM_JOINT7" && HumanSyncOn){
+//		param.target_p0(0) = 0.151 + robot_ref_pose.rhpos[0];
+//		param.target_p0(1) = -0.374 + robot_ref_pose.rhpos[1];
+//		param.target_p0(2) = 0.973 + robot_ref_pose.rhpos[2];
+//	}
+	  ////////////////////// 右足拘束位置設定 /////////////////////////
+//	if(param.target_link->name == "LARM_JOINT7" && HumanSyncOn){
+//		param.target_p0(0) = 0.151 + robot_ref_pose.lhpos[0];
+//		param.target_p0(1) = 0.374 + robot_ref_pose.lhpos[1];
+//		param.target_p0(2) = 0.973 + robot_ref_pose.lhpos[2];
+//	}
+
+	  ////////////////////// 重心拘束位置設定 /////////////////////////
+	  m_robot->rootLink()->p = cominitpos + robot_ref_pose.com;//まずは重心目標分，体幹を動かす
+	  m_robot->rootLink()->p(2) = baseinitpos(2)+ robot_ref_pose.com(2);//高さだけはベース基準
 	// COM上書き
-		if(USEX)ref_cog(0) = baseinitpos(0) + h2r_ratio * filt_hp_data.com[0];
-		ref_cog(1) = baseinitpos(1) + h2r_ratio * filt_hp_data.com[1];
-		ref_cog(2) = baseinitpos(2) + h2r_ratio * filt_hp_data.com[2];
-//	  std::cout<<"filt_hp_data.com[1]:"<<filt_hp_data.com[1]<<std::endl;
-//	  std::cout<<"m_robot->rootLink()->p(1):"<<m_robot->rootLink()->p(1)<<std::endl;
-	  for(int i=0;i<3;i++)hp_data_old.com[i] = filt_hp_data.com[i];
+	  ref_cog = cominitpos + robot_ref_pose.com;
+	  hp_data_old.com = robot_ref_pose.com;
   }
 
+
   // Fix for toe joint
   for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
       if (it->second.is_active && it->second.has_toe_joint && gg->get_use_toe_joint()) {
@@ -1318,9 +1330,36 @@ void AutoBalancer::solveLimbIK ()
   }
   m_robot->calcForwardKinematics();
 
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-    if (it->second.is_active) solveLimbIKforLimb(it->second);
+
+
+  // additional COM fitting IK ishiguro
+  hrp::Vector3 tmp_com_err = robot_ref_pose.com + cominitpos - m_robot->calcCM();
+  int com_ik_loop=0;
+  const int COM_IK_MAX_LOOP = 10;
+  const double COM_IK_MAX_ERROR = 0.0001;//1mm
+
+  if(HumanSyncOn){
+	  while(fabs(tmp_com_err(0))>COM_IK_MAX_ERROR || fabs(tmp_com_err(1))>COM_IK_MAX_ERROR){//Z方向のCOM合わせはなんか調子悪い
+
+		  m_robot->rootLink()->p(0) += tmp_com_err(0);
+		  m_robot->rootLink()->p(1) += tmp_com_err(1);//Z方向のCOM合わせはなんか調子悪い
+
+		  m_robot->calcForwardKinematics();
+
+		  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+			if (it->second.is_active) solveLimbIKforLimb(it->second);
+		  }
+		  m_robot->calcForwardKinematics();
+		  tmp_com_err = (robot_ref_pose.com + cominitpos - m_robot->calcCM());
+		  if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "[" << m_profile.instance_name << "] COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
+	  }
+	  if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
+  }else{
+	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
+		if (it->second.is_active) solveLimbIKforLimb(it->second);
+	  }
   }
+
   if (gg_is_walking && !gg_solved) stopWalking ();
 }
 
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 16d0ec812ed..634ab4c104f 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -280,6 +280,9 @@ class AutoBalancer
   hrp::Vector3 graspless_manip_p_gain;
   rats::coordinates graspless_manip_reference_trans_coords;
   double pos_ik_thre, rot_ik_thre;
+
+  //ishiguro
+  hrp::Vector3 cominitpos;
 };
 
 

From a03aa9cf40dbeb6e4750baebf31b1a4f2a7cd1b9 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 26 Jul 2016 17:58:52 +0900
Subject: [PATCH 012/217] compatible to JAXON

---
 rtc/AutoBalancer/AutoBalancer.cpp | 29 ++++++++++++++++++-----------
 rtc/AutoBalancer/AutoBalancer.h   |  4 ++++
 2 files changed, 22 insertions(+), 11 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index b1320c19982..75c90fd62fa 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -451,7 +451,7 @@ RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
 }
 
 
-static double h2r_ratio = 0.62;//human 1.1 vs chidori 0.69
+
 hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
 
 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
@@ -1083,7 +1083,7 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri
 }
 
 static bool ht_first_call = true;
-hrp::Vector3 rfinitpos,lfinitpos,baseinitpos;
+hrp::Vector3 rfinitpos,lfinitpos,rhinitpos,lhinitpos,baseinitpos;
 static int rfuplevel,lfuplevel;
 static humanpose_t init_humanpose;
 #define LEVELNUM 200
@@ -1162,16 +1162,18 @@ void AutoBalancer::solveLimbIK ()
 	  if(ht_first_call){
 		  rfinitpos = ikp["rleg"].target_link->p;
 		  lfinitpos = ikp["lleg"].target_link->p;
+		  rhinitpos = ikp["rarm"].target_link->p;
+		  lhinitpos = ikp["larm"].target_link->p;
 		  baseinitpos = m_robot->rootLink()->p;
 		  ht_first_call=false;
 	  }
 	  ///////入力データによる更新/////
-	  robot_ref_pose.zmp = (1-FNUM)*robot_ref_pose.zmp + FNUM*(h2r_ratio*humanpose.zmp);
-	  robot_ref_pose.com = (1-FNUM)*robot_ref_pose.com + FNUM*(h2r_ratio*(humanpose.com - init_humanpose.com + init_humanpose.zmp));//Sync開始時のzmpのオフセットはつまり重心のオフセット
-	  robot_ref_pose.rfpos = (1-FNUM)*robot_ref_pose.rfpos + FNUM*(humanpose.rfpos - init_humanpose.rfpos);//足ロック
-	  robot_ref_pose.lfpos = (1-FNUM)*robot_ref_pose.lfpos + FNUM*(humanpose.lfpos - init_humanpose.lfpos);//足ロック
-	  robot_ref_pose.rhpos = (1-FNUM)*robot_ref_pose.rhpos + FNUM*(humanpose.rhpos - init_humanpose.rhpos);
-	  robot_ref_pose.lhpos = (1-FNUM)*robot_ref_pose.lhpos + FNUM*(humanpose.lhpos - init_humanpose.lhpos);
+	  robot_ref_pose.zmp =   (1-FNUM)*robot_ref_pose.zmp   + FNUM * h2r_ratio *  humanpose.zmp;
+	  robot_ref_pose.com =   (1-FNUM)*robot_ref_pose.com   + FNUM * h2r_ratio * (humanpose.com - init_humanpose.com + init_humanpose.zmp);//Sync開始時のzmpのオフセットはつまり重心のオフセット
+	  robot_ref_pose.rfpos = (1-FNUM)*robot_ref_pose.rfpos + FNUM * h2r_ratio * (humanpose.rfpos - init_humanpose.rfpos);//足ロック
+	  robot_ref_pose.lfpos = (1-FNUM)*robot_ref_pose.lfpos + FNUM * h2r_ratio * (humanpose.lfpos - init_humanpose.lfpos);//足ロック
+	  robot_ref_pose.rhpos = (1-FNUM)*robot_ref_pose.rhpos + FNUM * h2r_ratio * (humanpose.rhpos - init_humanpose.rhpos);
+	  robot_ref_pose.lhpos = (1-FNUM)*robot_ref_pose.lhpos + FNUM * h2r_ratio * (humanpose.lhpos - init_humanpose.lhpos);
 	  for(int i=0;i<6;i++){
 		  robot_ref_pose.rfwrench[i] = (1-FNUM)*robot_ref_pose.rfwrench[i] + FNUM*(humanpose.rfwrench[i]);
 		  robot_ref_pose.lfwrench[i] = (1-FNUM)*robot_ref_pose.lfwrench[i] + FNUM*(humanpose.lfwrench[i]);
@@ -1183,6 +1185,8 @@ void AutoBalancer::solveLimbIK ()
 		  if(robot_ref_pose.com[i] > MAXPOS)robot_ref_pose.com[i] = MAXPOS;
 		  if(robot_ref_pose.com[i] < -MAXPOS)robot_ref_pose.com[i] = -MAXPOS;
 	  }
+	  if(robot_ref_pose.com(1) < robot_ref_pose.rfpos(1)-0.1-0.03)robot_ref_pose.com(1) = robot_ref_pose.rfpos(1)-0.1-0.03;
+	  if(robot_ref_pose.com(1) > robot_ref_pose.lfpos(1)+0.1+0.03)robot_ref_pose.com(1) = robot_ref_pose.lfpos(1)+0.1+0.03;
 //	  if(robot_ref_pose.rfpos(1) > -0.07)robot_ref_pose.rfpos(1) = -0.07;
 //	  if(robot_ref_pose.lfpos(1) <  0.07)robot_ref_pose.lfpos(1) =  0.07;
 
@@ -1295,13 +1299,16 @@ void AutoBalancer::solveLimbIK ()
 	}
 	ikp["lleg"].target_p0 = lfinitpos + robot_ref_pose.lfpos;
 
-	  ////////////////////// 右足拘束位置設定 /////////////////////////
+	  ////////////////////// 右手拘束位置設定 /////////////////////////
+	ikp["rarm"].target_p0(1) = rhinitpos(1) + current_root_p(1);
+
 //	if(param.target_link->name == "RARM_JOINT7" && HumanSyncOn){
 //		param.target_p0(0) = 0.151 + robot_ref_pose.rhpos[0];
 //		param.target_p0(1) = -0.374 + robot_ref_pose.rhpos[1];
 //		param.target_p0(2) = 0.973 + robot_ref_pose.rhpos[2];
 //	}
-	  ////////////////////// 右足拘束位置設定 /////////////////////////
+	  ////////////////////// 左手拘束位置設定 /////////////////////////
+	ikp["larm"].target_p0(1) = lhinitpos(1) + current_root_p(1);
 //	if(param.target_link->name == "LARM_JOINT7" && HumanSyncOn){
 //		param.target_p0(0) = 0.151 + robot_ref_pose.lhpos[0];
 //		param.target_p0(1) = 0.374 + robot_ref_pose.lhpos[1];
@@ -1353,7 +1360,7 @@ void AutoBalancer::solveLimbIK ()
 		  tmp_com_err = (robot_ref_pose.com + cominitpos - m_robot->calcCM());
 		  if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "[" << m_profile.instance_name << "] COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
 	  }
-	  if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
+	  if(loop%100==0)if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
   }else{
 	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
 		if (it->second.is_active) solveLimbIKforLimb(it->second);
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 634ab4c104f..ef97a672fae 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -283,6 +283,10 @@ class AutoBalancer
 
   //ishiguro
   hrp::Vector3 cominitpos;
+  //double h2r_ratio = 0.62;//human 1.1 vs chidori 0.69
+//  double h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
+  double h2r_ratio = 0.96;//human 1.1 vs jaxon 1.06
+//    double h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
 };
 
 

From e20fdd858c2803817212c2248bd449b5680deae9 Mon Sep 17 00:00:00 2001
From: ishiguroJSK <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 4 Aug 2016 00:39:21 +0900
Subject: [PATCH 013/217] start constructing HumanSynchronizer class

---
 rtc/AutoBalancer/AutoBalancer.cpp | 126 +++++++++---------------------
 rtc/AutoBalancer/AutoBalancer.h   |  74 ++++++++++++++++--
 2 files changed, 106 insertions(+), 94 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 75c90fd62fa..b96d641e2d1 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -16,42 +16,23 @@
 #include <hrpUtil/MatrixSolvers.h>
 #include "hrpsys/util/Hrpsys.h"
 
-//SHM関連
-//#include <errno.h>
-//#include <sys/stat.h>
-//#include <sys/types.h>
-//#include <sys/ipc.h>
-//#include <sys/shm.h>
-//int     shmid;
-//key_t   key;
-//typedef struct{
-//	double com[3];
-//	double rfpos[3];
-//	double lfpos[3];
-//	double rhpos[3];
-//	double lhpos[3];
-//	double rfwrench[6];
-//	double lfwrench[6];
-//	double zmp[3];
-//	bool startsync;
-//}humanpose_t;
-typedef struct{
-	hrp::Vector3 com;
-	hrp::Vector3 zmp;
-	hrp::Vector3 rfpos;
-	hrp::Vector3 lfpos;
-	hrp::Vector3 rhpos;
-	hrp::Vector3 lhpos;
-	double rfwrench[6];
-	double lfwrench[6];
-	bool startsync;
-}humanpose_t;
+
 humanpose_t humanpose;
 humanpose_t robot_ref_pose,hp_data_old;
 bool startCountdownForHumanSync = false;
 bool HumanSyncOn = false;
 #define USEX true
-
+hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
+static bool ht_first_call = true;
+hrp::Vector3 rfinitpos,lfinitpos,rhinitpos,lhinitpos,baseinitpos;
+static int rfuplevel,lfuplevel;
+static humanpose_t init_humanpose;
+#define LEVELNUM 200
+#define FUPHIGHT 0.05
+#define MAXVEL 0.004
+//#define MAXPOS 0.25
+#define MAXPOS 1000//ほぼ無制限
+#define FNUM 0.01
 
 typedef coil::Guard<coil::Mutex> Guard;
 using namespace rats;
@@ -227,7 +208,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
         coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
         ABCIKparam tp;
         for (size_t j = 0; j < 3; j++) {
-          coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
+            coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
         }
         double tmpv[4];
         for (int j = 0; j < 4; j++ ) {
@@ -399,17 +380,15 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     rot_ik_thre = (1e-2)*M_PI/180.0; // [rad]
     ik_error_debug_print_freq = static_cast<int>(0.2/m_dt); // once per 0.2 [s]
 
+    //ishiguro
+    hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer());
+
     return RTC::RTC_OK;
 }
 
 
-
 RTC::ReturnCode_t AutoBalancer::onFinalize()
 {
-	//SHM
-//    if (shmdt(humanpose) == -1){       perror("shmdt");    }
-//    if (shmctl(shmid, IPC_RMID, 0) == -1){        perror("shmctl");    }
-
   delete zmp_offset_interpolator;
   delete transition_interpolator;
   delete adjust_footstep_interpolator;
@@ -452,7 +431,6 @@ RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
 
 
 
-hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
 
 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
 //#define DEBUGP2 ((loop%200==0))
@@ -654,32 +632,17 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
 
       // my test zmp SHM
 
-      double rfzmp[2],lfzmp[2];
+      hrp::Vector3 rfzmp,lfzmp;
       const double F_H_OFFSET = 0.03;
-      double rfpos[3] = {0,-0.1/h2r_ratio,0},lfpos[3] = {0,0.1/h2r_ratio,0};
+//      hrp::Vector3 rfpos = {0,-0.1/hsp->h2r_ratio,0},lfpos = {0,0.1/hsp->h2r_ratio,0};
+      hrp::Vector3 rfpos,lfpos;
 
       rfpos[0] = pre_cont_rfpos(0);
-      rfpos[1] = -0.1/h2r_ratio + pre_cont_rfpos(1);
+      rfpos[1] = -0.1/hsp->h2r_ratio + pre_cont_rfpos(1);
       lfpos[0] = pre_cont_lfpos(0);
-      lfpos[1] = 0.1/h2r_ratio + pre_cont_lfpos(1);
-
-		if( humanpose.rfwrench[2] > 1.0e-6 ){
-			rfzmp[0] = ( -humanpose.rfwrench[4] - humanpose.rfwrench[0] * F_H_OFFSET + humanpose.rfwrench[2] * 0 ) / humanpose.rfwrench[2] + rfpos[0];
-			rfzmp[1] = ( humanpose.rfwrench[3] - humanpose.rfwrench[1] * F_H_OFFSET + humanpose.rfwrench[2] * 0 ) / humanpose.rfwrench[2] + rfpos[1];
-		}
-		if( humanpose.lfwrench[2] > 1.0e-6 ){
-			lfzmp[0] = ( -humanpose.lfwrench[4] - humanpose.lfwrench[0] * F_H_OFFSET + humanpose.lfwrench[2] * 0 ) / humanpose.lfwrench[2] + lfpos[0];
-			lfzmp[1] = ( humanpose.lfwrench[3] - humanpose.lfwrench[1] * F_H_OFFSET + humanpose.lfwrench[2] * 0 ) / humanpose.lfwrench[2] + lfpos[1];
-		}
-		//humanpose.zmpをrfw,lfwから計算して上書き
+      lfpos[1] = 0.1/hsp->h2r_ratio + pre_cont_lfpos(1);
 
-		if( humanpose.rfwrench[2] > 1.0e-6 || humanpose.lfwrench[2] > 1.0e-6 ){
-			humanpose.zmp[0] = ( rfzmp[0]*humanpose.rfwrench[2] + lfzmp[0]*humanpose.lfwrench[2] ) / ( humanpose.rfwrench[2] + humanpose.lfwrench[2]);
-			humanpose.zmp[1] = ( rfzmp[1]*humanpose.rfwrench[2] + lfzmp[1]*humanpose.lfwrench[2] ) / ( humanpose.rfwrench[2] + humanpose.lfwrench[2]);
-			humanpose.zmp[2] = 0;
-		}else{
-			humanpose.zmp[0] = 0;	humanpose.zmp[1] = 0;	humanpose.zmp[2] = 0;
-		}
+      humanpose.zmp = hsp->CalcWorldZMP(rfpos,lfpos,humanpose.rfwrench,humanpose.lfwrench);
 
 		if(HumanSyncOn){
 		//ZMP上書き
@@ -1017,7 +980,6 @@ void AutoBalancer::getTargetParameters()
         if ( gg_is_walking && gg->get_lcg_count() == gg->get_overwrite_check_timing()+2 ) {
             hrp::Vector3 vel_htc(calc_vel_from_hand_error(tmp_fix_coords));
             gg->set_offset_velocity_param(vel_htc(0), vel_htc(1) ,vel_htc(2));
-            gg->set_offset_velocity_param(1,2,3);
         }//  else {
         //     if ( gg_is_walking && gg->get_lcg_count() == static_cast<size_t>(gg->get_default_step_time()/(2*m_dt))-1) {
         //         gg->set_offset_velocity_param(0,0,0);
@@ -1082,12 +1044,7 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri
   m_robot->calcForwardKinematics();
 }
 
-static bool ht_first_call = true;
-hrp::Vector3 rfinitpos,lfinitpos,rhinitpos,lhinitpos,baseinitpos;
-static int rfuplevel,lfuplevel;
-static humanpose_t init_humanpose;
-#define LEVELNUM 200
-#define FUPHIGHT 0.05
+
 bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
 {
   param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
@@ -1138,11 +1095,7 @@ void AutoBalancer::solveLimbIK ()
   dif_cog(2) = m_robot->rootLink()->p(2) - target_root_p(2);
   m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog;
   m_robot->rootLink()->R = target_root_R;
-//mytest
-#define MAXVEL 0.004
-//#define MAXPOS 0.25
-#define MAXPOS 1000//ほぼ無制限
-#define FNUM 0.01
+
 
   static bool isCalibState=false;
   static int HumanSyncCountdownNum = 5 * (1/m_dt);
@@ -1162,18 +1115,18 @@ void AutoBalancer::solveLimbIK ()
 	  if(ht_first_call){
 		  rfinitpos = ikp["rleg"].target_link->p;
 		  lfinitpos = ikp["lleg"].target_link->p;
-		  rhinitpos = ikp["rarm"].target_link->p;
-		  lhinitpos = ikp["larm"].target_link->p;
+		  if(ikp.count("rarm"))rhinitpos = ikp["rarm"].target_link->p;
+		  if(ikp.count("larm"))lhinitpos = ikp["larm"].target_link->p;
 		  baseinitpos = m_robot->rootLink()->p;
 		  ht_first_call=false;
 	  }
 	  ///////入力データによる更新/////
-	  robot_ref_pose.zmp =   (1-FNUM)*robot_ref_pose.zmp   + FNUM * h2r_ratio *  humanpose.zmp;
-	  robot_ref_pose.com =   (1-FNUM)*robot_ref_pose.com   + FNUM * h2r_ratio * (humanpose.com - init_humanpose.com + init_humanpose.zmp);//Sync開始時のzmpのオフセットはつまり重心のオフセット
-	  robot_ref_pose.rfpos = (1-FNUM)*robot_ref_pose.rfpos + FNUM * h2r_ratio * (humanpose.rfpos - init_humanpose.rfpos);//足ロック
-	  robot_ref_pose.lfpos = (1-FNUM)*robot_ref_pose.lfpos + FNUM * h2r_ratio * (humanpose.lfpos - init_humanpose.lfpos);//足ロック
-	  robot_ref_pose.rhpos = (1-FNUM)*robot_ref_pose.rhpos + FNUM * h2r_ratio * (humanpose.rhpos - init_humanpose.rhpos);
-	  robot_ref_pose.lhpos = (1-FNUM)*robot_ref_pose.lhpos + FNUM * h2r_ratio * (humanpose.lhpos - init_humanpose.lhpos);
+	  robot_ref_pose.zmp =   (1-FNUM)*robot_ref_pose.zmp   + FNUM * (hsp->h2r_ratio) *  humanpose.zmp;
+	  robot_ref_pose.com =   (1-FNUM)*robot_ref_pose.com   + FNUM * (hsp->h2r_ratio) * (humanpose.com - init_humanpose.com + init_humanpose.zmp);//Sync開始時のzmpのオフセットはつまり重心のオフセット
+	  robot_ref_pose.rfpos = (1-FNUM)*robot_ref_pose.rfpos + FNUM * (hsp->h2r_ratio) * (humanpose.rfpos - init_humanpose.rfpos);//足ロック
+	  robot_ref_pose.lfpos = (1-FNUM)*robot_ref_pose.lfpos + FNUM * (hsp->h2r_ratio) * (humanpose.lfpos - init_humanpose.lfpos);//足ロック
+	  robot_ref_pose.rhpos = (1-FNUM)*robot_ref_pose.rhpos + FNUM * (hsp->h2r_ratio) * (humanpose.rhpos - init_humanpose.rhpos);
+	  robot_ref_pose.lhpos = (1-FNUM)*robot_ref_pose.lhpos + FNUM * (hsp->h2r_ratio) * (humanpose.lhpos - init_humanpose.lhpos);
 	  for(int i=0;i<6;i++){
 		  robot_ref_pose.rfwrench[i] = (1-FNUM)*robot_ref_pose.rfwrench[i] + FNUM*(humanpose.rfwrench[i]);
 		  robot_ref_pose.lfwrench[i] = (1-FNUM)*robot_ref_pose.lfwrench[i] + FNUM*(humanpose.lfwrench[i]);
@@ -1190,8 +1143,6 @@ void AutoBalancer::solveLimbIK ()
 //	  if(robot_ref_pose.rfpos(1) > -0.07)robot_ref_pose.rfpos(1) = -0.07;
 //	  if(robot_ref_pose.lfpos(1) <  0.07)robot_ref_pose.lfpos(1) =  0.07;
 
-
-
 //	  if(loop%100==0)printf("[COM] %+06.3f,%+06.3f,%+06.3f [ZMP] %+06.3f,%+06.3f\n", robot_ref_pose.com(0),robot_ref_pose.com(1),robot_ref_pose.com(2),robot_ref_pose.zmp[0],robot_ref_pose.zmp[1]);
   }else{
 	  init_humanpose.com = humanpose.com;
@@ -1300,7 +1251,7 @@ void AutoBalancer::solveLimbIK ()
 	ikp["lleg"].target_p0 = lfinitpos + robot_ref_pose.lfpos;
 
 	  ////////////////////// 右手拘束位置設定 /////////////////////////
-	ikp["rarm"].target_p0(1) = rhinitpos(1) + current_root_p(1);
+	if(ikp.count("rarm"))ikp["rarm"].target_p0(1) = rhinitpos(1) + current_root_p(1);
 
 //	if(param.target_link->name == "RARM_JOINT7" && HumanSyncOn){
 //		param.target_p0(0) = 0.151 + robot_ref_pose.rhpos[0];
@@ -1308,7 +1259,7 @@ void AutoBalancer::solveLimbIK ()
 //		param.target_p0(2) = 0.973 + robot_ref_pose.rhpos[2];
 //	}
 	  ////////////////////// 左手拘束位置設定 /////////////////////////
-	ikp["larm"].target_p0(1) = lhinitpos(1) + current_root_p(1);
+	if(ikp.count("larm"))ikp["larm"].target_p0(1) = lhinitpos(1) + current_root_p(1);
 //	if(param.target_link->name == "LARM_JOINT7" && HumanSyncOn){
 //		param.target_p0(0) = 0.151 + robot_ref_pose.lhpos[0];
 //		param.target_p0(1) = 0.374 + robot_ref_pose.lhpos[1];
@@ -1316,10 +1267,11 @@ void AutoBalancer::solveLimbIK ()
 //	}
 
 	  ////////////////////// 重心拘束位置設定 /////////////////////////
-	  m_robot->rootLink()->p = cominitpos + robot_ref_pose.com;//まずは重心目標分，体幹を動かす
+	  m_robot->rootLink()->p = hsp->cominitpos + robot_ref_pose.com;//まずは重心目標分，体幹を動かす
 	  m_robot->rootLink()->p(2) = baseinitpos(2)+ robot_ref_pose.com(2);//高さだけはベース基準
 	// COM上書き
-	  ref_cog = cominitpos + robot_ref_pose.com;
+
+	  ref_cog = hsp->cominitpos + robot_ref_pose.com;
 	  hp_data_old.com = robot_ref_pose.com;
   }
 
@@ -1340,7 +1292,7 @@ void AutoBalancer::solveLimbIK ()
 
 
   // additional COM fitting IK ishiguro
-  hrp::Vector3 tmp_com_err = robot_ref_pose.com + cominitpos - m_robot->calcCM();
+  hrp::Vector3 tmp_com_err = robot_ref_pose.com + hsp->cominitpos - m_robot->calcCM();
   int com_ik_loop=0;
   const int COM_IK_MAX_LOOP = 10;
   const double COM_IK_MAX_ERROR = 0.0001;//1mm
@@ -1357,7 +1309,7 @@ void AutoBalancer::solveLimbIK ()
 			if (it->second.is_active) solveLimbIKforLimb(it->second);
 		  }
 		  m_robot->calcForwardKinematics();
-		  tmp_com_err = (robot_ref_pose.com + cominitpos - m_robot->calcCM());
+		  tmp_com_err = (robot_ref_pose.com + hsp->cominitpos - m_robot->calcCM());
 		  if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "[" << m_profile.instance_name << "] COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
 	  }
 	  if(loop%100==0)if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index ef97a672fae..3813a243170 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -31,6 +31,71 @@
 // Service Consumer stub headers
 // <rtc-template block="consumer_stub_h">
 
+
+  //ishiguro
+typedef struct{
+	hrp::Vector3 com;
+	hrp::Vector3 rfpos;
+	hrp::Vector3 lfpos;
+	hrp::Vector3 rhpos;
+	hrp::Vector3 lhpos;
+	hrp::Vector3 zmp;
+	double rfwrench[6];
+	double lfwrench[6];
+}humanpose_t;
+
+class HumanSynchronizer
+{
+
+
+	public:
+		HumanSynchronizer(){
+			 h2r_ratio = 0.96;//human 1.1 vs jaxon 1.06
+			//h2r_ratio = 0.62;//human 1.1 vs chidori 0.69
+			//h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
+			//h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
+		}
+		~HumanSynchronizer(){}
+
+		void UpdateInputData(){
+
+		}
+		hrp::Vector3 CalcWorldZMP(hrp::Vector3 rfpos, hrp::Vector3 lfpos, double * rfwin, double * lfwin){
+			hrp::Vector3 rfzmp,lfzmp,zmp_ans;
+			const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
+			if( rfwin[2] > 1.0e-6 ){
+				rfzmp(0) = ( - rfwin[4] - rfwin[0] * F_H_OFFSET + rfwin[2] * 0 ) / rfwin[2] + rfpos(0);
+				rfzmp(1) = (   rfwin[3] - rfwin[1] * F_H_OFFSET + rfwin[2] * 0 ) / rfwin[2] + rfpos(1);
+			}
+			if( lfwin[2] > 1.0e-6 ){
+				lfzmp(0) = ( - lfwin[4] - lfwin[0] * F_H_OFFSET + lfwin[2] * 0 ) / lfwin[2] + lfpos(0);
+				lfzmp(1) = (   lfwin[3] - lfwin[1] * F_H_OFFSET + lfwin[2] * 0 ) / lfwin[2] + lfpos(1);
+			}
+			if( rfwin[2] > 1.0e-6 || lfwin[2] > 1.0e-6 ){
+				zmp_ans(0) = ( rfzmp(0)*rfwin[2] + lfzmp(0)*lfwin[2] ) / ( rfwin[2] + lfwin[2]);
+				zmp_ans(1) = ( rfzmp(1)*rfwin[2] + lfzmp(1)*lfwin[2] ) / ( rfwin[2] + lfwin[2]);
+			}else{
+				zmp_ans(0) = 0;	zmp_ans(1) = 0;
+			}
+			zmp_ans(2) = 0;
+			return zmp_ans;
+		}
+
+		hrp::Vector3 cominitpos;
+		double h2r_ratio;
+
+		humanpose_t raw_in;
+
+	 private:
+
+		humanpose_t ref_out;
+
+
+
+};
+
+
+
 // </rtc-template>
 
 using namespace RTC;
@@ -281,14 +346,9 @@ class AutoBalancer
   rats::coordinates graspless_manip_reference_trans_coords;
   double pos_ik_thre, rot_ik_thre;
 
-  //ishiguro
-  hrp::Vector3 cominitpos;
-  //double h2r_ratio = 0.62;//human 1.1 vs chidori 0.69
-//  double h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
-  double h2r_ratio = 0.96;//human 1.1 vs jaxon 1.06
-//    double h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
-};
+  boost::shared_ptr<HumanSynchronizer> hsp;
 
+};
 
 extern "C"
 {

From 2ff1512ece2417f32d58bd686ee0d32259a23386 Mon Sep 17 00:00:00 2001
From: ishiguroJSK <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 4 Aug 2016 22:13:27 +0900
Subject: [PATCH 014/217] move to use HumanSynchronizer class

---
 rtc/AutoBalancer/AutoBalancer.cpp | 116 +++++++------
 rtc/AutoBalancer/AutoBalancer.h   | 265 +++++++++++++++++++++++-------
 2 files changed, 268 insertions(+), 113 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index b96d641e2d1..f9bd9f4c969 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -489,53 +489,36 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
         // }
     }
     //for human tracker
-    if (m_htzmpIn.isNew()){
-    	m_htzmpIn.read();
-    	humanpose.zmp[0] = m_htzmp.data.x;//zmpは外部から読まずに両足wrenchからこのソースの中で計算
-    	humanpose.zmp[1] = m_htzmp.data.y;
-    	humanpose.zmp[2] = m_htzmp.data.z;
-    }
-    if (m_htrfwIn.isNew()){
-    	m_htrfwIn.read();
-    	for(int i=0;i<6;i++){
-    		humanpose.rfwrench[i] = m_htrfw.data[i];
-    	}
-    }
-    if (m_htlfwIn.isNew()){
-    	m_htlfwIn.read();
-    	for(int i=0;i<6;i++){
-    		humanpose.lfwrench[i] = m_htlfw.data[i];
-    	}
-    }
-    if (m_htcomIn.isNew()){
-    	m_htcomIn.read();
-    	humanpose.com[0] = m_htcom.data.x;
-    	humanpose.com[1] = m_htcom.data.y;
-    	humanpose.com[2] = m_htcom.data.z;
-    }
-    if (m_htrfIn.isNew()){
-    	m_htrfIn.read();
-    	humanpose.rfpos[0] = m_htrf.data.x;
-    	humanpose.rfpos[1] = m_htrf.data.y;
-    	humanpose.rfpos[2] = m_htrf.data.z;
-    }
-    if (m_htlfIn.isNew()){
-    	m_htlfIn.read();
-    	humanpose.lfpos[0] = m_htlf.data.x;
-    	humanpose.lfpos[1] = m_htlf.data.y;
-    	humanpose.lfpos[2] = m_htlf.data.z;
-    }
-    if (m_htrhIn.isNew()){
-    	m_htrhIn.read();
-    	humanpose.rhpos[0] = m_htrh.data.x;
-    	humanpose.rhpos[1] = m_htrh.data.y;
-    	humanpose.rhpos[2] = m_htrh.data.z;
-    }
-    if (m_htlhIn.isNew()){
-    	m_htlhIn.read();
-    	humanpose.lhpos[0] = m_htlh.data.x;
-    	humanpose.lhpos[1] = m_htlh.data.y;
-    	humanpose.lhpos[2] = m_htlh.data.z;
+    if (m_htzmpIn.isNew()){    	m_htzmpIn.read();
+    	humanpose.zmp[0] = m_htzmp.data.x;    	humanpose.zmp[1] = m_htzmp.data.y;    	humanpose.zmp[2] = m_htzmp.data.z;//zmpは外部から読まずに両足wrenchからこのソースの中で計算
+    }
+    if (m_htrfwIn.isNew()){    	m_htrfwIn.read();
+      for(int i=0;i<6;i++)humanpose.rfw[i] = m_htrfw.data[i];
+      for(int i=0;i<6;i++)hsp->hp_wld_in.rfw[i] = m_htrfw.data[i];
+    }
+    if (m_htlfwIn.isNew()){    	m_htlfwIn.read();
+      for(int i=0;i<6;i++)humanpose.lfw[i] = m_htlfw.data[i];
+      for(int i=0;i<6;i++)hsp->hp_wld_in.lfw[i] = m_htlfw.data[i];
+    }
+    if (m_htcomIn.isNew()){    	m_htcomIn.read();
+    	humanpose.com[0] = m_htcom.data.x;    	humanpose.com[1] = m_htcom.data.y;    	humanpose.com[2] = m_htcom.data.z;
+      hsp->hp_wld_in.com = HumanSynchronizer::Point3DToVector3(m_htcom.data);
+    }
+    if (m_htrfIn.isNew()){    	m_htrfIn.read();
+    	humanpose.rfpos[0] = m_htrf.data.x;    	humanpose.rfpos[1] = m_htrf.data.y;    	humanpose.rfpos[2] = m_htrf.data.z;
+      hsp->hp_wld_in.rfpos = HumanSynchronizer::Point3DToVector3(m_htrf.data);
+    }
+    if (m_htlfIn.isNew()){    	m_htlfIn.read();
+    	humanpose.lfpos[0] = m_htlf.data.x;    	humanpose.lfpos[1] = m_htlf.data.y;    	humanpose.lfpos[2] = m_htlf.data.z;
+      hsp->hp_wld_in.lfpos = HumanSynchronizer::Point3DToVector3(m_htlf.data);
+    }
+    if (m_htrhIn.isNew()){    	m_htrhIn.read();
+    	humanpose.rhpos[0] = m_htrh.data.x;    	humanpose.rhpos[1] = m_htrh.data.y;    	humanpose.rhpos[2] = m_htrh.data.z;
+      hsp->hp_wld_in.lfpos = HumanSynchronizer::Point3DToVector3(m_htrh.data);
+    }
+    if (m_htlhIn.isNew()){    	m_htlhIn.read();
+    	humanpose.lhpos[0] = m_htlh.data.x;    	humanpose.lhpos[1] = m_htlh.data.y;    	humanpose.lhpos[2] = m_htlh.data.z;
+      hsp->hp_wld_in.lhpos = HumanSynchronizer::Point3DToVector3(m_htlh.data);
     }
     if (m_actzmpIn.isNew()){
     	m_actzmpIn.read();
@@ -642,7 +625,7 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
       lfpos[0] = pre_cont_lfpos(0);
       lfpos[1] = 0.1/hsp->h2r_ratio + pre_cont_lfpos(1);
 
-      humanpose.zmp = hsp->CalcWorldZMP(rfpos,lfpos,humanpose.rfwrench,humanpose.lfwrench);
+      hsp->calcWorldZMP(rfpos,lfpos,humanpose.rfw,humanpose.lfw,humanpose.zmp);
 
 		if(HumanSyncOn){
 		//ZMP上書き
@@ -1111,6 +1094,7 @@ void AutoBalancer::solveLimbIK ()
 	  }
   }
 
+
   if(HumanSyncOn){
 	  if(ht_first_call){
 		  rfinitpos = ikp["rleg"].target_link->p;
@@ -1119,6 +1103,10 @@ void AutoBalancer::solveLimbIK ()
 		  if(ikp.count("larm"))lhinitpos = ikp["larm"].target_link->p;
 		  baseinitpos = m_robot->rootLink()->p;
 		  ht_first_call=false;
+		  hsp->setInitOffsetPose(hsp->hp_wld_in);
+
+		  hsp->cominitpos = m_robot->calcCM();
+
 	  }
 	  ///////入力データによる更新/////
 	  robot_ref_pose.zmp =   (1-FNUM)*robot_ref_pose.zmp   + FNUM * (hsp->h2r_ratio) *  humanpose.zmp;
@@ -1128,8 +1116,8 @@ void AutoBalancer::solveLimbIK ()
 	  robot_ref_pose.rhpos = (1-FNUM)*robot_ref_pose.rhpos + FNUM * (hsp->h2r_ratio) * (humanpose.rhpos - init_humanpose.rhpos);
 	  robot_ref_pose.lhpos = (1-FNUM)*robot_ref_pose.lhpos + FNUM * (hsp->h2r_ratio) * (humanpose.lhpos - init_humanpose.lhpos);
 	  for(int i=0;i<6;i++){
-		  robot_ref_pose.rfwrench[i] = (1-FNUM)*robot_ref_pose.rfwrench[i] + FNUM*(humanpose.rfwrench[i]);
-		  robot_ref_pose.lfwrench[i] = (1-FNUM)*robot_ref_pose.lfwrench[i] + FNUM*(humanpose.lfwrench[i]);
+		  robot_ref_pose.rfw[i] = (1-FNUM)*robot_ref_pose.rfw[i] + FNUM*(humanpose.rfw[i]);
+		  robot_ref_pose.lfw[i] = (1-FNUM)*robot_ref_pose.lfw[i] + FNUM*(humanpose.lfw[i]);
 	  }
 	  ///////指令値範囲の制限/////
 	  for(int i=0;i<3;i++){
@@ -1155,9 +1143,15 @@ void AutoBalancer::solveLimbIK ()
 		  if(robot_ref_pose.com[i]>0)robot_ref_pose.com[i] -= 0.0001;
 		  if(robot_ref_pose.com[i]<0)robot_ref_pose.com[i] += 0.0001;
 	  }
+    hsp->setInitOffsetPose(hsp->hp_wld_in);
 //	  if(loop%100==0)printf("[pre-COM] %+06.3f,%+06.3f,%+06.3f [pre-ZMP] %+06.3f,%+06.3f\n", humanpose.com[0],humanpose.com[1],humanpose.com[2],humanpose.zmp[0],humanpose.zmp[1]);
   }
 
+
+
+  hsp->updateInputData();
+
+
 //  static double filt_actzmp[3];
 //  if(isCalibState){
 //	  filt_actzmp[0] = (1-FNUM)*filt_actzmp[0] + FNUM*m_actzmp.data.x;
@@ -1197,7 +1191,7 @@ void AutoBalancer::solveLimbIK ()
 
   if(HumanSyncOn){
 	  ////////////////////// 右足拘束位置設定 /////////////////////////
-	if(humanpose.rfwrench[2] < 20.0 && humanpose.lfwrench[2] > 20.0 && HumanSyncOn){
+	if(humanpose.rfw[2] < 20.0 && humanpose.lfw[2] > 20.0 && HumanSyncOn){
 		if(rfuplevel < LEVELNUM){
 			rfuplevel++;
 			robot_ref_pose.rfpos[2] = FUPHIGHT/2*(1-cos(M_PI*rfuplevel/LEVELNUM));
@@ -1224,7 +1218,7 @@ void AutoBalancer::solveLimbIK ()
 	ikp["rleg"].target_p0 = rfinitpos + robot_ref_pose.rfpos;
 
 	  ////////////////////// 左足拘束位置設定 /////////////////////////
-	if(humanpose.lfwrench[2] < 20.0 && humanpose.rfwrench[2] > 20.0 && HumanSyncOn){
+	if(humanpose.lfw[2] < 20.0 && humanpose.rfw[2] > 20.0 && HumanSyncOn){
 		if(lfuplevel < LEVELNUM){
 			lfuplevel++;
 			robot_ref_pose.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
@@ -1291,13 +1285,26 @@ void AutoBalancer::solveLimbIK ()
 
 
 
+
+
+
   // additional COM fitting IK ishiguro
-  hrp::Vector3 tmp_com_err = robot_ref_pose.com + hsp->cominitpos - m_robot->calcCM();
+//  hrp::Vector3 tmp_com_err = robot_ref_pose.com + hsp->cominitpos - m_robot->calcCM();
+  hrp::Vector3 tmp_com_err = hsp->rp_ref_out.com + hsp->cominitpos - m_robot->calcCM();
   int com_ik_loop=0;
   const int COM_IK_MAX_LOOP = 10;
   const double COM_IK_MAX_ERROR = 0.0001;//1mm
 
+
   if(HumanSyncOn){
+
+    ikp["rleg"].target_p0 = rfinitpos + hsp->rp_ref_out.rfpos;
+    ikp["lleg"].target_p0 = lfinitpos + hsp->rp_ref_out.lfpos;
+//
+//    cout<<"com\n"<<hsp->rp_ref_out.com<<endl;
+//    cout<<"rfpos\n"<<hsp->rp_ref_out.rfpos<<endl;
+//    cout<<"lfpos\n"<<hsp->rp_ref_out.lfpos<<endl;
+
 	  while(fabs(tmp_com_err(0))>COM_IK_MAX_ERROR || fabs(tmp_com_err(1))>COM_IK_MAX_ERROR){//Z方向のCOM合わせはなんか調子悪い
 
 		  m_robot->rootLink()->p(0) += tmp_com_err(0);
@@ -1309,7 +1316,8 @@ void AutoBalancer::solveLimbIK ()
 			if (it->second.is_active) solveLimbIKforLimb(it->second);
 		  }
 		  m_robot->calcForwardKinematics();
-		  tmp_com_err = (robot_ref_pose.com + hsp->cominitpos - m_robot->calcCM());
+//      tmp_com_err = (robot_ref_pose.com + hsp->cominitpos - m_robot->calcCM());
+      tmp_com_err = (hsp->rp_ref_out.com + hsp->cominitpos - m_robot->calcCM());
 		  if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "[" << m_profile.instance_name << "] COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
 	  }
 	  if(loop%100==0)if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 3813a243170..e113edf04b3 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -31,67 +31,214 @@
 // Service Consumer stub headers
 // <rtc-template block="consumer_stub_h">
 
-
-  //ishiguro
-typedef struct{
-	hrp::Vector3 com;
-	hrp::Vector3 rfpos;
-	hrp::Vector3 lfpos;
-	hrp::Vector3 rhpos;
-	hrp::Vector3 lhpos;
-	hrp::Vector3 zmp;
-	double rfwrench[6];
-	double lfwrench[6];
-}humanpose_t;
-
-class HumanSynchronizer
-{
-
-
-	public:
-		HumanSynchronizer(){
-			 h2r_ratio = 0.96;//human 1.1 vs jaxon 1.06
-			//h2r_ratio = 0.62;//human 1.1 vs chidori 0.69
-			//h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
-			//h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
-		}
-		~HumanSynchronizer(){}
-
-		void UpdateInputData(){
-
-		}
-		hrp::Vector3 CalcWorldZMP(hrp::Vector3 rfpos, hrp::Vector3 lfpos, double * rfwin, double * lfwin){
-			hrp::Vector3 rfzmp,lfzmp,zmp_ans;
-			const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
-			if( rfwin[2] > 1.0e-6 ){
-				rfzmp(0) = ( - rfwin[4] - rfwin[0] * F_H_OFFSET + rfwin[2] * 0 ) / rfwin[2] + rfpos(0);
-				rfzmp(1) = (   rfwin[3] - rfwin[1] * F_H_OFFSET + rfwin[2] * 0 ) / rfwin[2] + rfpos(1);
-			}
-			if( lfwin[2] > 1.0e-6 ){
-				lfzmp(0) = ( - lfwin[4] - lfwin[0] * F_H_OFFSET + lfwin[2] * 0 ) / lfwin[2] + lfpos(0);
-				lfzmp(1) = (   lfwin[3] - lfwin[1] * F_H_OFFSET + lfwin[2] * 0 ) / lfwin[2] + lfpos(1);
-			}
-			if( rfwin[2] > 1.0e-6 || lfwin[2] > 1.0e-6 ){
-				zmp_ans(0) = ( rfzmp(0)*rfwin[2] + lfzmp(0)*lfwin[2] ) / ( rfwin[2] + lfwin[2]);
-				zmp_ans(1) = ( rfzmp(1)*rfwin[2] + lfzmp(1)*lfwin[2] ) / ( rfwin[2] + lfwin[2]);
-			}else{
-				zmp_ans(0) = 0;	zmp_ans(1) = 0;
-			}
-			zmp_ans(2) = 0;
-			return zmp_ans;
-		}
-
-		hrp::Vector3 cominitpos;
-		double h2r_ratio;
-
-		humanpose_t raw_in;
-
-	 private:
-
-		humanpose_t ref_out;
-
+class humanpose_t{
+  public:
+    hrp::Vector3 com;
+    hrp::Vector3 rfpos;
+    hrp::Vector3 lfpos;
+    hrp::Vector3 rhpos;
+    hrp::Vector3 lhpos;
+    hrp::Vector3 zmp;
+    Eigen::VectorXd rfw;
+    Eigen::VectorXd lfw;
+    humanpose_t(){ clear(); }
+    ~humanpose_t(){cout<<"humanpose_t destructed"<<endl;}
+    void clear(){
+      com = hrp::Vector3::Zero();
+      rfpos = hrp::Vector3::Zero();
+      lfpos = hrp::Vector3::Zero();
+      rhpos = hrp::Vector3::Zero();
+      lhpos = hrp::Vector3::Zero();
+      zmp = hrp::Vector3::Zero();
+      rfw.resize(6); rfw = Eigen::VectorXd::Zero(6);
+      lfw.resize(6); lfw = Eigen::VectorXd::Zero(6);
+    }
+};
 
 
+class HumanSynchronizer{
+  private:
+
+    double FNUM;
+    int FUPLEVELNUM;
+    int cur_rfup_level,cur_lfup_level;
+    double FUPHIGHT;
+    double CNT_F_TH;
+    bool HumanSyncOn;
+    bool is_rf_contact,is_lf_contact;
+    hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
+    double MAXVEL,MAXACC;
+    humanpose_t hp_wld_initpos;
+    humanpose_t hp_rel_raw;
+    humanpose_t hp_filtered;
+    humanpose_t rp_ref_out_old;
+
+  public:
+
+    hrp::Vector3 cominitpos;
+    double h2r_ratio;
+
+    humanpose_t hp_wld_in;
+    humanpose_t rp_ref_out;
+
+    HumanSynchronizer(){
+//      h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
+      h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
+      //h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
+      //h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
+      cur_rfup_level = 0;       cur_lfup_level = 0;
+      is_rf_contact = true;     is_lf_contact = true;
+      pre_cont_rfpos.Zero();    pre_cont_lfpos.Zero();
+      FNUM = 0.01;
+      FUPLEVELNUM = 200;
+      FUPHIGHT = 0.05;
+      CNT_F_TH = 20.0;
+      MAXVEL = 0.004;
+      HumanSyncOn = false;
+    }
+
+    ~HumanSynchronizer(){cout<<"HumanSynchronizer destructed"<<endl;}
+
+    void setInitOffsetPose(const humanpose_t& abs_in){ hp_wld_initpos = abs_in; }
+
+    void startHumanSync(){
+      setInitOffsetPose(hp_wld_in);
+      HumanSyncOn = true;
+    }
+
+    void updateInputData(){
+      removeInitOffsetPose(hp_wld_in, hp_wld_initpos, hp_rel_raw);
+      applyLPFilter(hp_rel_raw, hp_filtered);
+      calcWorldZMP(hp_filtered.rfpos, hp_filtered.lfpos, hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
+      convertHumanToRobot(hp_filtered,rp_ref_out);
+      judgeFootContactStates(rp_ref_out.rfw, rp_ref_out.lfw, is_rf_contact, is_lf_contact);
+      overwriteFootZFromContactStates(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);
+      lockFootXYOnContact(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);
+      applyWorkspaceLimit(rp_ref_out, rp_ref_out);
+      applyCOMToSupportRegionLimit(rp_ref_out, rp_ref_out);
+      applyVelLimit(rp_ref_out, rp_ref_out_old, rp_ref_out);
+      rp_ref_out_old = rp_ref_out;
+    }
+
+    void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos,
+        const Eigen::VectorXd& rfwin, const Eigen::VectorXd& lfwin, hrp::Vector3& zmp_ans){
+      hrp::Vector3 rfzmp,lfzmp;
+      const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
+      if( rfwin[2] > 1.0e-6 ){
+        rfzmp(0) = ( - rfwin[4] - rfwin[0] * F_H_OFFSET + rfwin[2] * 0 ) / rfwin[2] + rfpos(0);
+        rfzmp(1) = (   rfwin[3] - rfwin[1] * F_H_OFFSET + rfwin[2] * 0 ) / rfwin[2] + rfpos(1);
+      }
+      if( lfwin[2] > 1.0e-6 ){
+        lfzmp(0) = ( - lfwin[4] - lfwin[0] * F_H_OFFSET + lfwin[2] * 0 ) / lfwin[2] + lfpos(0);
+        lfzmp(1) = (   lfwin[3] - lfwin[1] * F_H_OFFSET + lfwin[2] * 0 ) / lfwin[2] + lfpos(1);
+      }
+      if( rfwin[2] > 1.0e-6 || lfwin[2] > 1.0e-6 ){
+        zmp_ans(0) = ( rfzmp(0)*rfwin[2] + lfzmp(0)*lfwin[2] ) / ( rfwin[2] + lfwin[2]);
+        zmp_ans(1) = ( rfzmp(1)*rfwin[2] + lfzmp(1)*lfwin[2] ) / ( rfwin[2] + lfwin[2]);
+      }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
+      zmp_ans(2) = 0;
+    }
+    static hrp::Vector3 Point3DToVector3(const RTC::Point3D& in){
+      hrp::Vector3 out(in.x,in.y,in.z); return out;
+    }
+
+  private:
+
+    void removeInitOffsetPose(const humanpose_t& abs_in, const humanpose_t& init_offset, humanpose_t& rel_out){
+      rel_out.com   = abs_in.com   - init_offset.com;
+      rel_out.rfpos = abs_in.rfpos - init_offset.rfpos;
+      rel_out.lfpos = abs_in.lfpos - init_offset.lfpos;
+      rel_out.rhpos = abs_in.rhpos - init_offset.rhpos;
+      rel_out.lhpos = abs_in.lhpos - init_offset.lhpos;
+      rel_out.rfw = abs_in.rfw;
+      rel_out.lfw = abs_in.lfw;
+    }
+    void applyLPFilter(const humanpose_t& raw_in, humanpose_t& filt_out){
+      filt_out.com   = (1-FNUM) * filt_out.com   + FNUM * raw_in.com;
+      filt_out.rfpos = (1-FNUM) * filt_out.rfpos + FNUM * raw_in.rfpos;
+      filt_out.lfpos = (1-FNUM) * filt_out.lfpos + FNUM * raw_in.lfpos;
+      filt_out.rhpos = (1-FNUM) * filt_out.rhpos + FNUM * raw_in.rhpos;
+      filt_out.lhpos = (1-FNUM) * filt_out.lhpos + FNUM * raw_in.lhpos;
+      filt_out.zmp   = (1-FNUM) * filt_out.zmp   + FNUM * raw_in.zmp;
+      filt_out.rfw = (1-FNUM) * filt_out.rfw + FNUM * raw_in.rfw;
+      filt_out.lfw = (1-FNUM) * filt_out.lfw + FNUM * raw_in.lfw;
+    }
+    void convertHumanToRobot(const humanpose_t& hp_in, humanpose_t& rp_out){
+      rp_out.com   = h2r_ratio * hp_in.com;
+      rp_out.rfpos = h2r_ratio * hp_in.rfpos;
+      rp_out.lfpos = h2r_ratio * hp_in.lfpos;
+      rp_out.rhpos = h2r_ratio * hp_in.rhpos;
+      rp_out.lhpos = h2r_ratio * hp_in.lhpos;
+      rp_out.zmp   = h2r_ratio * hp_in.zmp;
+      rp_out.rfw = hp_in.rfw;
+      rp_out.lfw = hp_in.lfw;
+    }
+    void judgeFootContactStates(const Eigen::VectorXd& rfw_in, const Eigen::VectorXd& lfw_in, bool& rfcs_ans, bool& lfcs_ans){
+      //他のプログラムは足に荷重時に力センサの値がZ軸マイナスに出るっぽい？(のでTODO FIX)
+      if      (rfw_in(2)<CNT_F_TH && lfw_in(2)>CNT_F_TH){rfcs_ans = false;  lfcs_ans = true;}//右足浮遊かつ左足接地
+      else if (rfw_in(2)>CNT_F_TH && lfw_in(2)<CNT_F_TH){rfcs_ans = true;   lfcs_ans = false;}//右足接地かつ左足浮遊
+      else                                              {rfcs_ans = true;   lfcs_ans = true;}//両足接地または両足浮遊
+    }
+
+    void overwriteFootZFromContactStates(const humanpose_t& in, const bool& rfcs_in, const bool& lfcs_in, humanpose_t& out){
+      out = in;
+      if(!rfcs_in){//右足浮遊時
+        if(cur_rfup_level < FUPLEVELNUM){
+          cur_rfup_level++;
+          out.rfpos(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
+        }else{
+          out.rfpos(2) = FUPHIGHT;
+        }
+      }else{
+        if(cur_rfup_level > 0){
+          cur_rfup_level--;
+          out.rfpos(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
+        }else{
+          out.rfpos(2) = 0;
+        }
+      }
+      if(!lfcs_in){//左足浮遊時
+        if(cur_lfup_level < FUPLEVELNUM){
+          cur_lfup_level++;
+          out.lfpos(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
+        }else{
+          out.lfpos(2) = FUPHIGHT;
+        }
+      }else{
+        if(cur_lfup_level > 0){
+          cur_lfup_level--;
+          out.lfpos(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
+        }else{
+          out.lfpos(2) = 0;
+        }
+      }
+    }
+    void lockFootXYOnContact(const humanpose_t& in, const bool& rfcs_in, const bool& lfcs_in, humanpose_t& out){
+      out = in;
+      if(rfcs_in){
+        out.rfpos(0) = pre_cont_rfpos(0);
+        out.rfpos(1) = pre_cont_rfpos(1);
+      }else{
+        pre_cont_rfpos(0) = in.rfpos(0);
+        pre_cont_rfpos(1) = in.rfpos(1);
+      }
+      if(lfcs_in){
+        out.lfpos(0) = pre_cont_lfpos(0);
+        out.lfpos(1) = pre_cont_lfpos(1);
+      }else{
+        pre_cont_lfpos(0) = in.lfpos(0);
+        pre_cont_lfpos(1) = in.lfpos(1);
+      }
+    }
+    void applyWorkspaceLimit(const humanpose_t& in, humanpose_t& out){}
+    void applyCOMToSupportRegionLimit(const humanpose_t& in, humanpose_t& out){}
+    void applyVelLimit(const humanpose_t& in, const humanpose_t& in_old, humanpose_t& out){
+      for(int i=0;i<3;i++){if(in.com(i) - in_old.com(i) > MAXVEL){out.com(i) = in_old.com(i) + MAXVEL;}else if(in.com(i) - in_old.com(i) < -MAXVEL){out.com(i) = in_old.com(i) - MAXVEL;}}
+      for(int i=0;i<3;i++){if(in.rfpos(i) - in_old.rfpos(i) > MAXVEL){out.rfpos(i) = in_old.rfpos(i) + MAXVEL;}else if(in.rfpos(i) - in_old.rfpos(i) < -MAXVEL){out.rfpos(i) = in_old.rfpos(i) - MAXVEL;}}
+      for(int i=0;i<3;i++){if(in.lfpos(i) - in_old.lfpos(i) > MAXVEL){out.lfpos(i) = in_old.lfpos(i) + MAXVEL;}else if(in.lfpos(i) - in_old.lfpos(i) < -MAXVEL){out.lfpos(i) = in_old.lfpos(i) - MAXVEL;}}
+      for(int i=0;i<3;i++){if(in.rhpos(i) - in_old.rhpos(i) > MAXVEL){out.rhpos(i) = in_old.rhpos(i) + MAXVEL;}else if(in.rhpos(i) - in_old.rhpos(i) < -MAXVEL){out.rhpos(i) = in_old.rhpos(i) - MAXVEL;}}
+      for(int i=0;i<3;i++){if(in.lhpos(i) - in_old.lhpos(i) > MAXVEL){out.lhpos(i) = in_old.lhpos(i) + MAXVEL;}else if(in.lhpos(i) - in_old.lhpos(i) < -MAXVEL){out.lhpos(i) = in_old.lhpos(i) - MAXVEL;}}
+    }
 };
 
 

From 8dc6ef58dc54af1b7666531394fc2770f26bcada Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 6 Aug 2016 03:23:21 +0900
Subject: [PATCH 015/217] Almostly complete moving to HumanSync and check with
 Xtion

---
 rtc/AutoBalancer/AutoBalancer.cpp | 362 ++++++------------------------
 rtc/AutoBalancer/AutoBalancer.h   | 235 ++++++++++++-------
 2 files changed, 225 insertions(+), 372 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index f9bd9f4c969..02e4228293c 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -17,23 +17,6 @@
 #include "hrpsys/util/Hrpsys.h"
 
 
-humanpose_t humanpose;
-humanpose_t robot_ref_pose,hp_data_old;
-bool startCountdownForHumanSync = false;
-bool HumanSyncOn = false;
-#define USEX true
-hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
-static bool ht_first_call = true;
-hrp::Vector3 rfinitpos,lfinitpos,rhinitpos,lhinitpos,baseinitpos;
-static int rfuplevel,lfuplevel;
-static humanpose_t init_humanpose;
-#define LEVELNUM 200
-#define FUPHIGHT 0.05
-#define MAXVEL 0.004
-//#define MAXPOS 0.25
-#define MAXPOS 1000//ほぼ無制限
-#define FNUM 0.01
-
 typedef coil::Guard<coil::Mutex> Guard;
 using namespace rats;
 
@@ -380,7 +363,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     rot_ik_thre = (1e-2)*M_PI/180.0; // [rad]
     ik_error_debug_print_freq = static_cast<int>(0.2/m_dt); // once per 0.2 [s]
 
-    //ishiguro
+    //for HumanSynchronizer
     hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer());
 
     return RTC::RTC_OK;
@@ -488,41 +471,17 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
         //     gg->emergency_stop();
         // }
     }
-    //for human tracker
-    if (m_htzmpIn.isNew()){    	m_htzmpIn.read();
-    	humanpose.zmp[0] = m_htzmp.data.x;    	humanpose.zmp[1] = m_htzmp.data.y;    	humanpose.zmp[2] = m_htzmp.data.z;//zmpは外部から読まずに両足wrenchからこのソースの中で計算
-    }
-    if (m_htrfwIn.isNew()){    	m_htrfwIn.read();
-      for(int i=0;i<6;i++)humanpose.rfw[i] = m_htrfw.data[i];
-      for(int i=0;i<6;i++)hsp->hp_wld_in.rfw[i] = m_htrfw.data[i];
-    }
-    if (m_htlfwIn.isNew()){    	m_htlfwIn.read();
-      for(int i=0;i<6;i++)humanpose.lfw[i] = m_htlfw.data[i];
-      for(int i=0;i<6;i++)hsp->hp_wld_in.lfw[i] = m_htlfw.data[i];
-    }
-    if (m_htcomIn.isNew()){    	m_htcomIn.read();
-    	humanpose.com[0] = m_htcom.data.x;    	humanpose.com[1] = m_htcom.data.y;    	humanpose.com[2] = m_htcom.data.z;
-      hsp->hp_wld_in.com = HumanSynchronizer::Point3DToVector3(m_htcom.data);
-    }
-    if (m_htrfIn.isNew()){    	m_htrfIn.read();
-    	humanpose.rfpos[0] = m_htrf.data.x;    	humanpose.rfpos[1] = m_htrf.data.y;    	humanpose.rfpos[2] = m_htrf.data.z;
-      hsp->hp_wld_in.rfpos = HumanSynchronizer::Point3DToVector3(m_htrf.data);
-    }
-    if (m_htlfIn.isNew()){    	m_htlfIn.read();
-    	humanpose.lfpos[0] = m_htlf.data.x;    	humanpose.lfpos[1] = m_htlf.data.y;    	humanpose.lfpos[2] = m_htlf.data.z;
-      hsp->hp_wld_in.lfpos = HumanSynchronizer::Point3DToVector3(m_htlf.data);
-    }
-    if (m_htrhIn.isNew()){    	m_htrhIn.read();
-    	humanpose.rhpos[0] = m_htrh.data.x;    	humanpose.rhpos[1] = m_htrh.data.y;    	humanpose.rhpos[2] = m_htrh.data.z;
-      hsp->hp_wld_in.lfpos = HumanSynchronizer::Point3DToVector3(m_htrh.data);
-    }
-    if (m_htlhIn.isNew()){    	m_htlhIn.read();
-    	humanpose.lhpos[0] = m_htlh.data.x;    	humanpose.lhpos[1] = m_htlh.data.y;    	humanpose.lhpos[2] = m_htlh.data.z;
-      hsp->hp_wld_in.lhpos = HumanSynchronizer::Point3DToVector3(m_htlh.data);
-    }
-    if (m_actzmpIn.isNew()){
-    	m_actzmpIn.read();
-    }
+    //for HumanSynchronizer
+    if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
+    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); for(int i=0;i<6;i++)hp_raw_data.rfw[i] = m_htrfw.data[i]; }
+    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); for(int i=0;i<6;i++)hp_raw_data.lfw[i] = m_htlfw.data[i]; }
+    if (m_htcomIn.isNew()){ m_htcomIn.read(); hp_raw_data.com   = HumanSynchronizer::Point3DToVector3(m_htcom.data); }
+    if (m_htrfIn.isNew()) { m_htrfIn.read();  hp_raw_data.rf = HumanSynchronizer::Point3DToVector3(m_htrf.data); }
+    if (m_htlfIn.isNew()) { m_htlfIn.read();  hp_raw_data.lf = HumanSynchronizer::Point3DToVector3(m_htlf.data); }
+    if (m_htrhIn.isNew()){  m_htrhIn.read();  hp_raw_data.rh = HumanSynchronizer::Point3DToVector3(m_htrh.data); }
+    if (m_htlhIn.isNew()){  m_htlhIn.read();  hp_raw_data.lh = HumanSynchronizer::Point3DToVector3(m_htlh.data); }
+    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
+    hsp->readInput(hp_raw_data);
 
     Guard guard(m_mutex);
     hrp::Vector3 ref_basePos;
@@ -611,31 +570,6 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
       m_cog.data.y = ref_cog(1);
       m_cog.data.z = ref_cog(2);
       m_cog.tm = m_qRef.tm;
-
-
-      // my test zmp SHM
-
-      hrp::Vector3 rfzmp,lfzmp;
-      const double F_H_OFFSET = 0.03;
-//      hrp::Vector3 rfpos = {0,-0.1/hsp->h2r_ratio,0},lfpos = {0,0.1/hsp->h2r_ratio,0};
-      hrp::Vector3 rfpos,lfpos;
-
-      rfpos[0] = pre_cont_rfpos(0);
-      rfpos[1] = -0.1/hsp->h2r_ratio + pre_cont_rfpos(1);
-      lfpos[0] = pre_cont_lfpos(0);
-      lfpos[1] = 0.1/hsp->h2r_ratio + pre_cont_lfpos(1);
-
-      hsp->calcWorldZMP(rfpos,lfpos,humanpose.rfw,humanpose.lfw,humanpose.zmp);
-
-		if(HumanSyncOn){
-		//ZMP上書き
-			if(USEX)m_zmp.data.x = robot_ref_pose.zmp[0] - m_robot->rootLink()->p(0);
-			m_zmp.data.y = robot_ref_pose.zmp[1] - m_robot->rootLink()->p(1);//
-//		// COM上書き
-//			if(USEX)m_cog.data.x = robot_ref_pose.com(0)*0.7;//ここの.comは今はhumanのbaseposなので適当調整
-//			m_cog.data.y = robot_ref_pose.com(1)*0.7;
-		}
-
       // sbpCogOffset
       m_sbpCogOffset.data.x = sbp_cog_offset(0);
       m_sbpCogOffset.data.y = sbp_cog_offset(1);
@@ -970,7 +904,6 @@ void AutoBalancer::getTargetParameters()
         // }
     }
 
-    //
 
     hrp::Vector3 tmp_ref_cog(m_robot->calcCM());
     if (gg_is_walking) {
@@ -986,13 +919,6 @@ void AutoBalancer::getTargetParameters()
       ref_zmp(1) = ref_cog(1);
       ref_zmp(2) = tmp_foot_mid_pos(2);
     }
-    //ishiguro
-    if (HumanSyncOn) {
-//        ref_cog = tmp_ref_cog;
-		if(USEX)ref_zmp(0) = humanpose.zmp[0];
-		ref_zmp(1) = humanpose.zmp[1];
-		ref_zmp(2) = tmp_foot_mid_pos(2);
-    }
   }
 };
 
@@ -1080,196 +1006,41 @@ void AutoBalancer::solveLimbIK ()
   m_robot->rootLink()->R = target_root_R;
 
 
-  static bool isCalibState=false;
-  static int HumanSyncCountdownNum = 5 * (1/m_dt);
-  if(startCountdownForHumanSync){
-	  printf("Start Count Down for HumanSync [%d]\r", HumanSyncCountdownNum);
-	  if(HumanSyncCountdownNum == 0){
-		  HumanSyncOn = true;
-		  isCalibState = true;
-		  startCountdownForHumanSync = false;
-		  HumanSyncCountdownNum = 5 * (1/m_dt);
-	  }else{
-		  HumanSyncCountdownNum--;
-	  }
+  //for HumanSynchronizer
+//  static int HumanSyncCountdownNum = 5 * (1/m_dt);
+  if(hsp->startCountdownForHumanSync){
+	  printf("Start Count Down for HumanSync [%d]\r", hsp->getRemainingCountDown());
+	  hsp->updateCountDown();
   }
-
-
-  if(HumanSyncOn){
-	  if(ht_first_call){
-		  rfinitpos = ikp["rleg"].target_link->p;
-		  lfinitpos = ikp["lleg"].target_link->p;
-		  if(ikp.count("rarm"))rhinitpos = ikp["rarm"].target_link->p;
-		  if(ikp.count("larm"))lhinitpos = ikp["larm"].target_link->p;
-		  baseinitpos = m_robot->rootLink()->p;
-		  ht_first_call=false;
-		  hsp->setInitOffsetPose(hsp->hp_wld_in);
-
-		  hsp->cominitpos = m_robot->calcCM();
+  if(hsp->isHumanSyncOn()){
+	  if(hsp->ht_first_call){
+		  hsp->init_wld_rp_basepos = m_robot->rootLink()->p;
+      hsp->setInitOffsetPose();
+      hsp->rp_wld_initpos.com = m_robot->calcCM();
+      hsp->rp_wld_initpos.rf = ikp["rleg"].target_link->p;
+      hsp->rp_wld_initpos.lf = ikp["lleg"].target_link->p;
+      if(ikp.count("rarm"))hsp->rp_wld_initpos.rh = ikp["rarm"].target_link->p;
+      if(ikp.count("larm"))hsp->rp_wld_initpos.lh = ikp["larm"].target_link->p;
+      hsp->ht_first_call = false;
+
+      hsp->calibInitHumanComPos();
 
 	  }
-	  ///////入力データによる更新/////
-	  robot_ref_pose.zmp =   (1-FNUM)*robot_ref_pose.zmp   + FNUM * (hsp->h2r_ratio) *  humanpose.zmp;
-	  robot_ref_pose.com =   (1-FNUM)*robot_ref_pose.com   + FNUM * (hsp->h2r_ratio) * (humanpose.com - init_humanpose.com + init_humanpose.zmp);//Sync開始時のzmpのオフセットはつまり重心のオフセット
-	  robot_ref_pose.rfpos = (1-FNUM)*robot_ref_pose.rfpos + FNUM * (hsp->h2r_ratio) * (humanpose.rfpos - init_humanpose.rfpos);//足ロック
-	  robot_ref_pose.lfpos = (1-FNUM)*robot_ref_pose.lfpos + FNUM * (hsp->h2r_ratio) * (humanpose.lfpos - init_humanpose.lfpos);//足ロック
-	  robot_ref_pose.rhpos = (1-FNUM)*robot_ref_pose.rhpos + FNUM * (hsp->h2r_ratio) * (humanpose.rhpos - init_humanpose.rhpos);
-	  robot_ref_pose.lhpos = (1-FNUM)*robot_ref_pose.lhpos + FNUM * (hsp->h2r_ratio) * (humanpose.lhpos - init_humanpose.lhpos);
-	  for(int i=0;i<6;i++){
-		  robot_ref_pose.rfw[i] = (1-FNUM)*robot_ref_pose.rfw[i] + FNUM*(humanpose.rfw[i]);
-		  robot_ref_pose.lfw[i] = (1-FNUM)*robot_ref_pose.lfw[i] + FNUM*(humanpose.lfw[i]);
-	  }
-	  ///////指令値範囲の制限/////
-	  for(int i=0;i<3;i++){
-		  if(robot_ref_pose.com[i] - hp_data_old.com[i] > MAXVEL)robot_ref_pose.com[i] = hp_data_old.com[i] + MAXVEL;
-		  if(robot_ref_pose.com[i] - hp_data_old.com[i] < -MAXVEL)robot_ref_pose.com[i] = hp_data_old.com[i] - MAXVEL;
-		  if(robot_ref_pose.com[i] > MAXPOS)robot_ref_pose.com[i] = MAXPOS;
-		  if(robot_ref_pose.com[i] < -MAXPOS)robot_ref_pose.com[i] = -MAXPOS;
-	  }
-	  if(robot_ref_pose.com(1) < robot_ref_pose.rfpos(1)-0.1-0.03)robot_ref_pose.com(1) = robot_ref_pose.rfpos(1)-0.1-0.03;
-	  if(robot_ref_pose.com(1) > robot_ref_pose.lfpos(1)+0.1+0.03)robot_ref_pose.com(1) = robot_ref_pose.lfpos(1)+0.1+0.03;
-//	  if(robot_ref_pose.rfpos(1) > -0.07)robot_ref_pose.rfpos(1) = -0.07;
-//	  if(robot_ref_pose.lfpos(1) <  0.07)robot_ref_pose.lfpos(1) =  0.07;
-
-//	  if(loop%100==0)printf("[COM] %+06.3f,%+06.3f,%+06.3f [ZMP] %+06.3f,%+06.3f\n", robot_ref_pose.com(0),robot_ref_pose.com(1),robot_ref_pose.com(2),robot_ref_pose.zmp[0],robot_ref_pose.zmp[1]);
   }else{
-	  init_humanpose.com = humanpose.com;
-	  init_humanpose.zmp = humanpose.zmp;
-	  init_humanpose.rfpos = humanpose.rfpos;
-	  init_humanpose.lfpos = humanpose.lfpos;
-	  init_humanpose.rhpos = humanpose.rhpos;
-	  init_humanpose.lhpos = humanpose.lhpos;
-	  for(int i=0;i<3;i++){
-		  if(robot_ref_pose.com[i]>0)robot_ref_pose.com[i] -= 0.0001;
-		  if(robot_ref_pose.com[i]<0)robot_ref_pose.com[i] += 0.0001;
-	  }
-    hsp->setInitOffsetPose(hsp->hp_wld_in);
-//	  if(loop%100==0)printf("[pre-COM] %+06.3f,%+06.3f,%+06.3f [pre-ZMP] %+06.3f,%+06.3f\n", humanpose.com[0],humanpose.com[1],humanpose.com[2],humanpose.zmp[0],humanpose.zmp[1]);
+    hsp->setInitOffsetPose();
   }
 
+  hsp->update();//////HumanSynchronizerの主要処理
+  hsp->rp_ref_out.print();
 
-
-  hsp->updateInputData();
-
-
-//  static double filt_actzmp[3];
-//  if(isCalibState){
-//	  filt_actzmp[0] = (1-FNUM)*filt_actzmp[0] + FNUM*m_actzmp.data.x;
-//	  filt_actzmp[1] = (1-FNUM)*filt_actzmp[1] + FNUM*m_actzmp.data.y;
-//	  filt_actzmp[2] = (1-FNUM)*filt_actzmp[2] + FNUM*m_actzmp.data.z;
-//	    const int errnum = 500*5;
-//	    const int errdelay = 500;
-//	    double zmp_robot[3],zmp_human[3];
-//	    static double zmp_human_old[3][errdelay];
-//	    static double err_sum[3] = {0};
-//	    for(int i=0;i<3;i++){
-//		    for(int j=errdelay-1;j>0;j--){
-//		    	zmp_human_old[i][j] = zmp_human_old[i][j-1];
-//		    }
-//		    zmp_human_old[i][0] = robot_ref_pose.zmp[i] - current_root_p(i);
-//		    zmp_human[i] = robot_ref_pose.zmp[i] - current_root_p(i);
-////		    zmp_human[i] = zmp_human_old[i][errdelay-1];//位相遅れ見越して
-//			zmp_robot[i] = filt_actzmp[i];
-//			err_sum[i] += fabs(zmp_human[i]) - fabs(zmp_robot[i]);
-//	    }
-//
-////	    if(loop%errnum==0){
-////			if(err_sum[1]/errnum>0.01 ){h2r_ratio -= 0.01;std::cout<<"h2r_ratio UP:"<<h2r_ratio<<" (EP):"<<err_sum[1]<<std::endl;}
-////			else if(err_sum[1]/errnum<-0.01 ){h2r_ratio += 0.01;std::cout<<"h2r_ratio DOWN:"<<h2r_ratio<<" (EP):"<<err_sum[1]<<std::endl;}
-////			else{std::cout<<"h2r_ratio KEEP:"<<h2r_ratio<<" (EP):"<<err_sum[1]<<std::endl;}
-////	    }
-//
-//	    //debug
-////	    static FILE *fp;
-////	    static bool fp_isOpen=false;
-////		if(!fp_isOpen){fp = fopen("/home/ishiguro/hcflog/nilog.log","w+");fp_isOpen=true;}
-////		if(fp!=NULL)fprintf(fp,"zmp: %f %f %f %f %f\n",zmp_human[0],zmp_human[1],zmp_robot[0],zmp_robot[1],h2r_ratio);
-////		fflush(fp);
-//  }
-
-
-
-  if(HumanSyncOn){
-	  ////////////////////// 右足拘束位置設定 /////////////////////////
-	if(humanpose.rfw[2] < 20.0 && humanpose.lfw[2] > 20.0 && HumanSyncOn){
-		if(rfuplevel < LEVELNUM){
-			rfuplevel++;
-			robot_ref_pose.rfpos[2] = FUPHIGHT/2*(1-cos(M_PI*rfuplevel/LEVELNUM));
-		}else{
-			robot_ref_pose.rfpos[2] = FUPHIGHT;
-		}
-		if(loop%100==0)cout<<"RUP"<<endl;
-		m_contactStates.data[contact_states_index_map["rleg"]] = false;
-	}else{
-		if(rfuplevel > 0){
-			rfuplevel--;
-			robot_ref_pose.rfpos[2] = FUPHIGHT/2*(1-cos(M_PI*rfuplevel/LEVELNUM));
-			if(rfuplevel==0){
-				pre_cont_rfpos(0) = robot_ref_pose.rfpos[0];
-				pre_cont_rfpos(1) = robot_ref_pose.rfpos[1];
-			}
-		}else{
-			m_contactStates.data[contact_states_index_map["rleg"]] = true;
-			robot_ref_pose.rfpos[0] = pre_cont_rfpos(0);
-			robot_ref_pose.rfpos[1] = pre_cont_rfpos(1);
-			robot_ref_pose.rfpos[2] = 0;
-		}
-	}
-	ikp["rleg"].target_p0 = rfinitpos + robot_ref_pose.rfpos;
-
-	  ////////////////////// 左足拘束位置設定 /////////////////////////
-	if(humanpose.lfw[2] < 20.0 && humanpose.rfw[2] > 20.0 && HumanSyncOn){
-		if(lfuplevel < LEVELNUM){
-			lfuplevel++;
-			robot_ref_pose.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
-		}else{
-			robot_ref_pose.lfpos[2] = FUPHIGHT;
-		}
-		if(loop%100==0)cout<<"LUP"<<endl;
-		m_contactStates.data[contact_states_index_map["lleg"]] = false;
-	}else{
-		if(lfuplevel > 0){
-			lfuplevel--;
-			robot_ref_pose.lfpos[2] = FUPHIGHT/2*(1-cos(M_PI*lfuplevel/LEVELNUM));
-			if(lfuplevel==0){
-				pre_cont_lfpos(0) = robot_ref_pose.lfpos[0];
-				pre_cont_lfpos(1) = robot_ref_pose.lfpos[1];
-			}
-		}else{
-			m_contactStates.data[contact_states_index_map["lleg"]] = true;
-			robot_ref_pose.lfpos[0] = pre_cont_lfpos(0);
-			robot_ref_pose.lfpos[1] = pre_cont_lfpos(1);
-			robot_ref_pose.lfpos[2] = 0;
-		}
-	}
-	ikp["lleg"].target_p0 = lfinitpos + robot_ref_pose.lfpos;
-
-	  ////////////////////// 右手拘束位置設定 /////////////////////////
-	if(ikp.count("rarm"))ikp["rarm"].target_p0(1) = rhinitpos(1) + current_root_p(1);
-
-//	if(param.target_link->name == "RARM_JOINT7" && HumanSyncOn){
-//		param.target_p0(0) = 0.151 + robot_ref_pose.rhpos[0];
-//		param.target_p0(1) = -0.374 + robot_ref_pose.rhpos[1];
-//		param.target_p0(2) = 0.973 + robot_ref_pose.rhpos[2];
-//	}
-	  ////////////////////// 左手拘束位置設定 /////////////////////////
-	if(ikp.count("larm"))ikp["larm"].target_p0(1) = lhinitpos(1) + current_root_p(1);
-//	if(param.target_link->name == "LARM_JOINT7" && HumanSyncOn){
-//		param.target_p0(0) = 0.151 + robot_ref_pose.lhpos[0];
-//		param.target_p0(1) = 0.374 + robot_ref_pose.lhpos[1];
-//		param.target_p0(2) = 0.973 + robot_ref_pose.lhpos[2];
-//	}
-
+  if(hsp->isHumanSyncOn()){
 	  ////////////////////// 重心拘束位置設定 /////////////////////////
-	  m_robot->rootLink()->p = hsp->cominitpos + robot_ref_pose.com;//まずは重心目標分，体幹を動かす
-	  m_robot->rootLink()->p(2) = baseinitpos(2)+ robot_ref_pose.com(2);//高さだけはベース基準
-	// COM上書き
-
-	  ref_cog = hsp->cominitpos + robot_ref_pose.com;
-	  hp_data_old.com = robot_ref_pose.com;
+    m_robot->rootLink()->p = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;//まずは重心目標分，体幹を動かす
+    m_robot->rootLink()->p(2) = hsp->init_wld_rp_basepos(2)+ hsp->rp_ref_out.com(2);//高さだけはベース基準
+    m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
+    m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
   }
 
-
   // Fix for toe joint
   for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
       if (it->second.is_active && it->second.has_toe_joint && gg->get_use_toe_joint()) {
@@ -1284,49 +1055,49 @@ void AutoBalancer::solveLimbIK ()
   m_robot->calcForwardKinematics();
 
 
-
-
-
-
-  // additional COM fitting IK ishiguro
-//  hrp::Vector3 tmp_com_err = robot_ref_pose.com + hsp->cominitpos - m_robot->calcCM();
-  hrp::Vector3 tmp_com_err = hsp->rp_ref_out.com + hsp->cominitpos - m_robot->calcCM();
+  // additional COM fitting IK for HumanSynchronizer
+  hrp::Vector3 tmp_com_err = hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM();
   int com_ik_loop=0;
   const int COM_IK_MAX_LOOP = 10;
-  const double COM_IK_MAX_ERROR = 0.0001;//1mm
-
-
-  if(HumanSyncOn){
-
-    ikp["rleg"].target_p0 = rfinitpos + hsp->rp_ref_out.rfpos;
-    ikp["lleg"].target_p0 = lfinitpos + hsp->rp_ref_out.lfpos;
-//
-//    cout<<"com\n"<<hsp->rp_ref_out.com<<endl;
-//    cout<<"rfpos\n"<<hsp->rp_ref_out.rfpos<<endl;
-//    cout<<"lfpos\n"<<hsp->rp_ref_out.lfpos<<endl;
-
-	  while(fabs(tmp_com_err(0))>COM_IK_MAX_ERROR || fabs(tmp_com_err(1))>COM_IK_MAX_ERROR){//Z方向のCOM合わせはなんか調子悪い
-
+  const double COM_IK_MAX_ERROR = 0.0001;//0.001だとたまにカクッとなる
+  if(hsp->isHumanSyncOn()){
+    ikp["rleg"].target_p0 = hsp->rp_wld_initpos.rf + hsp->rp_ref_out.rf;
+    ikp["lleg"].target_p0 = hsp->rp_wld_initpos.lf + hsp->rp_ref_out.lf;
+    while(fabs(tmp_com_err(0))>COM_IK_MAX_ERROR || fabs(tmp_com_err(1))>COM_IK_MAX_ERROR || fabs(tmp_com_err(2))>COM_IK_MAX_ERROR){//Z方向のCOM合わせは要注意(たまに怪しい)
 		  m_robot->rootLink()->p(0) += tmp_com_err(0);
-		  m_robot->rootLink()->p(1) += tmp_com_err(1);//Z方向のCOM合わせはなんか調子悪い
-
+      m_robot->rootLink()->p(1) += tmp_com_err(1);
+      m_robot->rootLink()->p(2) += tmp_com_err(2);
 		  m_robot->calcForwardKinematics();
-
 		  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-			if (it->second.is_active) solveLimbIKforLimb(it->second);
+		    if (it->second.is_active) solveLimbIKforLimb(it->second);
 		  }
 		  m_robot->calcForwardKinematics();
-//      tmp_com_err = (robot_ref_pose.com + hsp->cominitpos - m_robot->calcCM());
-      tmp_com_err = (hsp->rp_ref_out.com + hsp->cominitpos - m_robot->calcCM());
+      tmp_com_err = (hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM());
 		  if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "[" << m_profile.instance_name << "] COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
 	  }
+
+//    static hrp::Vector3 pre_ik_basepos;
+//    for(int i=0;i<3;i++){//重心拘束で解いた結果の体幹軌道の滑らかさは保証されていないので体幹位置に速度制限＆再度IK
+//      if(m_robot->rootLink()->p(i) - pre_ik_basepos(i) > 0.0002){ m_robot->rootLink()->p(i) =  pre_ik_basepos(i) + 0.0002; }
+//      else if(m_robot->rootLink()->p(i) - pre_ik_basepos(i) < -0.0002){ m_robot->rootLink()->p(i) =  pre_ik_basepos(i) - 0.0002; }
+//    }
+//    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+//      if (it->second.is_active) solveLimbIKforLimb(it->second);
+//    }
+//    pre_ik_basepos = m_robot->rootLink()->p;
+
+    //out用のデータ上書き
+    ref_zmp(0) = hsp->rp_ref_out.zmp(0);
+    ref_zmp(1) = hsp->rp_ref_out.zmp(1);
+    ref_zmp(2) = 0;
+    ref_cog = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;
+
 	  if(loop%100==0)if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
   }else{
 	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
 		if (it->second.is_active) solveLimbIKforLimb(it->second);
 	  }
   }
-
   if (gg_is_walking && !gg_solved) stopWalking ();
 }
 
@@ -1368,14 +1139,13 @@ void AutoBalancer::solveLimbIK ()
 bool AutoBalancer::startHumanSyncAfter5sec()
 {
 	std::cerr << "[" << m_profile.instance_name << "] start HumanSync after 5 sec" << std::endl;
-	startCountdownForHumanSync = true;
+	hsp->startCountdownForHumanSync = true;
 	return true;
 }
 
 bool AutoBalancer::stopHumanSync()
 {
 	std::cerr << "[" << m_profile.instance_name << "] stop HumanSync Now" << std::endl;
-	HumanSyncOn = false;
 	return true;
 }
 
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index e113edf04b3..14afa3dd699 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -33,26 +33,33 @@
 
 class humanpose_t{
   public:
-    hrp::Vector3 com;
-    hrp::Vector3 rfpos;
-    hrp::Vector3 lfpos;
-    hrp::Vector3 rhpos;
-    hrp::Vector3 lhpos;
+    hrp::Vector3 com,rf,lf,rh,lh;
     hrp::Vector3 zmp;
-    Eigen::VectorXd rfw;
-    Eigen::VectorXd lfw;
+    Eigen::VectorXd rfw,lfw;
     humanpose_t(){ clear(); }
     ~humanpose_t(){cout<<"humanpose_t destructed"<<endl;}
     void clear(){
-      com = hrp::Vector3::Zero();
-      rfpos = hrp::Vector3::Zero();
-      lfpos = hrp::Vector3::Zero();
-      rhpos = hrp::Vector3::Zero();
-      lhpos = hrp::Vector3::Zero();
-      zmp = hrp::Vector3::Zero();
+      com =   hrp::Vector3::Zero();
+      rf = hrp::Vector3::Zero();
+      lf = hrp::Vector3::Zero();
+      rh = hrp::Vector3::Zero();
+      lh = hrp::Vector3::Zero();
+      zmp =   hrp::Vector3::Zero();
       rfw.resize(6); rfw = Eigen::VectorXd::Zero(6);
       lfw.resize(6); lfw = Eigen::VectorXd::Zero(6);
     }
+    static void hp_printf(const humanpose_t& in){
+      printf("\x1b[31mcom:\x1b[39m %+05.2f %+05.2f %+05.2f ",in.com(0),in.com(1),in.com(2));
+      printf("\x1b[31mrf:\x1b[39m %+05.2f %+05.2f %+05.2f ",in.rf(0),in.rf(1),in.rf(2));
+      printf("\x1b[31mlf:\x1b[39m %+05.2f %+05.2f %+05.2f ",in.lf(0),in.lf(1),in.lf(2));
+      printf("\x1b[31mrh:\x1b[39m %+05.2f %+05.2f %+05.2f ",in.rh(0),in.rh(1),in.rh(2));
+      printf("\x1b[31mlh:\x1b[39m %+05.2f %+05.2f %+05.2f ",in.lh(0),in.lh(1),in.lh(2));
+      printf("\x1b[31mzmp:\x1b[39m %+05.2f %+05.2f %+05.2f ",in.zmp(0),in.zmp(1),in.zmp(2));
+      printf("\x1b[31mrfw:\x1b[39m %+04.0f %+04.0f %+05.0f %+04.0f %+04.0f %+04.0f ",in.rfw(0),in.rfw(1),in.rfw(2),in.rfw(3),in.rfw(4),in.rfw(5));
+      printf("\x1b[31mlfw:\x1b[39m %+04.0f %+04.0f %+05.0f %+04.0f %+04.0f %+04.0f ",in.lfw(0),in.lfw(1),in.lfw(2),in.lfw(3),in.lfw(4),in.lfw(5));
+      printf("\n");
+    }
+    void print(){ hp_printf(*this); }
 };
 
 
@@ -64,53 +71,92 @@ class HumanSynchronizer{
     int cur_rfup_level,cur_lfup_level;
     double FUPHIGHT;
     double CNT_F_TH;
-    bool HumanSyncOn;
-    bool is_rf_contact,is_lf_contact;
     hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
+    hrp::Vector3 init_wld_rp_compos;
+    hrp::Vector3 init_wld_hp_rfpos,init_wld_hp_lfpos;
+    hrp::Vector3 init_wld_rp_rfpos,init_wld_rp_lfpos;
+    hrp::Vector3 init_hp_calibcom;
     double MAXVEL,MAXACC;
+    humanpose_t hp_wld_raw;
     humanpose_t hp_wld_initpos;
     humanpose_t hp_rel_raw;
     humanpose_t hp_filtered;
     humanpose_t rp_ref_out_old;
+    int countdown_num;
+    bool countdown_flag;
+    bool HumanSyncOn;
 
   public:
+    bool startCountdownForHumanSync;
+    bool ht_first_call;
+    bool is_rf_contact,is_lf_contact;
 
-    hrp::Vector3 cominitpos;
+
+    humanpose_t rp_wld_initpos;
     double h2r_ratio;
+    hrp::Vector3 init_wld_rp_basepos;
 
-    humanpose_t hp_wld_in;
     humanpose_t rp_ref_out;
 
     HumanSynchronizer(){
 //      h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
-      h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
-      //h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
+//      h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
+      h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
       //h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
       cur_rfup_level = 0;       cur_lfup_level = 0;
       is_rf_contact = true;     is_lf_contact = true;
       pre_cont_rfpos.Zero();    pre_cont_lfpos.Zero();
+      init_wld_hp_rfpos = hrp::Vector3(0,-0.1/h2r_ratio,0);
+      init_wld_hp_lfpos = hrp::Vector3(0, 0.1/h2r_ratio,0);
+      init_wld_rp_rfpos = hrp::Vector3(0,-0.1,0);
+      init_wld_rp_lfpos = hrp::Vector3(0, 0.1,0);
+      init_hp_calibcom.Zero();
       FNUM = 0.01;
       FUPLEVELNUM = 200;
       FUPHIGHT = 0.05;
       CNT_F_TH = 20.0;
-      MAXVEL = 0.004;
+//      MAXVEL = 0.004;
+      MAXVEL = 0.001;
       HumanSyncOn = false;
+      ht_first_call = true;
+      countdown_flag = false;
+      countdown_num = 5*500;
     }
 
     ~HumanSynchronizer(){cout<<"HumanSynchronizer destructed"<<endl;}
 
-    void setInitOffsetPose(const humanpose_t& abs_in){ hp_wld_initpos = abs_in; }
 
-    void startHumanSync(){
-      setInitOffsetPose(hp_wld_in);
-      HumanSyncOn = true;
+    void readInput(const humanpose_t& raw_in){ hp_wld_raw = raw_in; }
+
+    int getRemainingCountDown(){ return countdown_num; }
+    bool isHumanSyncOn(){ return HumanSyncOn; }
+
+    void setInitOffsetPose(){ hp_wld_initpos = hp_wld_raw; }
+
+
+
+//    void startCountDown(int sec_x_dt){
+//      countdown_num = sec_x_dt;
+//    }
+
+    void updateCountDown(){
+      if(countdown_num>0){
+        countdown_num--;
+      }else if(countdown_num==0){
+        HumanSyncOn = true;
+        startCountdownForHumanSync = false;
+      }
     }
 
-    void updateInputData(){
-      removeInitOffsetPose(hp_wld_in, hp_wld_initpos, hp_rel_raw);
-      applyLPFilter(hp_rel_raw, hp_filtered);
-      calcWorldZMP(hp_filtered.rfpos, hp_filtered.lfpos, hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
-      convertHumanToRobot(hp_filtered,rp_ref_out);
+    void update(){
+      removeInitOffsetPose(hp_wld_raw, hp_wld_initpos, hp_rel_raw);
+
+      hp_rel_raw.com += init_hp_calibcom;
+
+      applyLPFilter(hp_rel_raw, hp_filtered, hp_filtered);
+      calcWorldZMP((hp_filtered.rf+init_wld_hp_rfpos), (hp_filtered.lf+init_wld_hp_lfpos), hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
+
+      convertRelHumanPoseToWldRobotPose(hp_filtered,rp_ref_out);
       judgeFootContactStates(rp_ref_out.rfw, rp_ref_out.lfw, is_rf_contact, is_lf_contact);
       overwriteFootZFromContactStates(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);
       lockFootXYOnContact(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);
@@ -120,6 +166,12 @@ class HumanSynchronizer{
       rp_ref_out_old = rp_ref_out;
     }
 
+    void calibInitHumanComPos(){
+      init_hp_calibcom(0) = hp_filtered.zmp(0);
+      init_hp_calibcom(1) = hp_filtered.zmp(1);
+    }
+
+
     void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos,
         const Eigen::VectorXd& rfwin, const Eigen::VectorXd& lfwin, hrp::Vector3& zmp_ans){
       hrp::Vector3 rfzmp,lfzmp;
@@ -138,40 +190,45 @@ class HumanSynchronizer{
       }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
       zmp_ans(2) = 0;
     }
-    static hrp::Vector3 Point3DToVector3(const RTC::Point3D& in){
-      hrp::Vector3 out(in.x,in.y,in.z); return out;
-    }
+    static hrp::Vector3 Point3DToVector3(const RTC::Point3D& in){ hrp::Vector3 out(in.x,in.y,in.z); return out; }
+
 
   private:
 
+    void startHumanSync(){
+      countdown_flag = true;
+      setInitOffsetPose();
+      HumanSyncOn = true;
+    }
+
     void removeInitOffsetPose(const humanpose_t& abs_in, const humanpose_t& init_offset, humanpose_t& rel_out){
-      rel_out.com   = abs_in.com   - init_offset.com;
-      rel_out.rfpos = abs_in.rfpos - init_offset.rfpos;
-      rel_out.lfpos = abs_in.lfpos - init_offset.lfpos;
-      rel_out.rhpos = abs_in.rhpos - init_offset.rhpos;
-      rel_out.lhpos = abs_in.lhpos - init_offset.lhpos;
+      rel_out.com = abs_in.com - init_offset.com;
+      rel_out.rf  = abs_in.rf  - init_offset.rf;
+      rel_out.lf  = abs_in.lf  - init_offset.lf;
+      rel_out.rh  = abs_in.rh  - init_offset.rh;
+      rel_out.lh  = abs_in.lh  - init_offset.lh;
       rel_out.rfw = abs_in.rfw;
       rel_out.lfw = abs_in.lfw;
     }
-    void applyLPFilter(const humanpose_t& raw_in, humanpose_t& filt_out){
-      filt_out.com   = (1-FNUM) * filt_out.com   + FNUM * raw_in.com;
-      filt_out.rfpos = (1-FNUM) * filt_out.rfpos + FNUM * raw_in.rfpos;
-      filt_out.lfpos = (1-FNUM) * filt_out.lfpos + FNUM * raw_in.lfpos;
-      filt_out.rhpos = (1-FNUM) * filt_out.rhpos + FNUM * raw_in.rhpos;
-      filt_out.lhpos = (1-FNUM) * filt_out.lhpos + FNUM * raw_in.lhpos;
-      filt_out.zmp   = (1-FNUM) * filt_out.zmp   + FNUM * raw_in.zmp;
-      filt_out.rfw = (1-FNUM) * filt_out.rfw + FNUM * raw_in.rfw;
-      filt_out.lfw = (1-FNUM) * filt_out.lfw + FNUM * raw_in.lfw;
+    void applyLPFilter(const humanpose_t& raw_in, const humanpose_t& out_old, humanpose_t& filt_out){
+      filt_out.com = (1-FNUM) * out_old.com + FNUM * raw_in.com;
+      filt_out.rf  = (1-FNUM) * out_old.rf  + FNUM * raw_in.rf;
+      filt_out.lf  = (1-FNUM) * out_old.lf  + FNUM * raw_in.lf;
+      filt_out.rh  = (1-FNUM) * out_old.rh  + FNUM * raw_in.rh;
+      filt_out.lh  = (1-FNUM) * out_old.lh  + FNUM * raw_in.lh;
+      filt_out.zmp = (1-FNUM) * out_old.zmp + FNUM * raw_in.zmp;
+      filt_out.rfw = (1-FNUM) * out_old.rfw + FNUM * raw_in.rfw;
+      filt_out.lfw = (1-FNUM) * out_old.lfw + FNUM * raw_in.lfw;
     }
-    void convertHumanToRobot(const humanpose_t& hp_in, humanpose_t& rp_out){
-      rp_out.com   = h2r_ratio * hp_in.com;
-      rp_out.rfpos = h2r_ratio * hp_in.rfpos;
-      rp_out.lfpos = h2r_ratio * hp_in.lfpos;
-      rp_out.rhpos = h2r_ratio * hp_in.rhpos;
-      rp_out.lhpos = h2r_ratio * hp_in.lhpos;
-      rp_out.zmp   = h2r_ratio * hp_in.zmp;
-      rp_out.rfw = hp_in.rfw;
-      rp_out.lfw = hp_in.lfw;
+    void convertRelHumanPoseToWldRobotPose(const humanpose_t& hp_in, humanpose_t& rp_out){//Wldと言いつつまだRel
+      rp_out.com  = h2r_ratio * hp_in.com;//+ init_wld_rp_compos;
+      rp_out.rf   = h2r_ratio * hp_in.rf;//+ init_wld_rp_rfpos;
+      rp_out.lf   = h2r_ratio * hp_in.lf;//+ init_wld_rp_lfpos;
+      rp_out.rh   = h2r_ratio * hp_in.rh;
+      rp_out.lh   = h2r_ratio * hp_in.lh;
+      rp_out.zmp  = h2r_ratio * hp_in.zmp;
+      rp_out.rfw  = hp_in.rfw;
+      rp_out.lfw  = hp_in.lfw;
     }
     void judgeFootContactStates(const Eigen::VectorXd& rfw_in, const Eigen::VectorXd& lfw_in, bool& rfcs_ans, bool& lfcs_ans){
       //他のプログラムは足に荷重時に力センサの値がZ軸マイナスに出るっぽい？(のでTODO FIX)
@@ -185,59 +242,83 @@ class HumanSynchronizer{
       if(!rfcs_in){//右足浮遊時
         if(cur_rfup_level < FUPLEVELNUM){
           cur_rfup_level++;
-          out.rfpos(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
+          out.rf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
         }else{
-          out.rfpos(2) = FUPHIGHT;
+          out.rf(2) = FUPHIGHT;
         }
       }else{
         if(cur_rfup_level > 0){
           cur_rfup_level--;
-          out.rfpos(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
+          out.rf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
         }else{
-          out.rfpos(2) = 0;
+          out.rf(2) = 0;
         }
       }
       if(!lfcs_in){//左足浮遊時
         if(cur_lfup_level < FUPLEVELNUM){
           cur_lfup_level++;
-          out.lfpos(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
+          out.lf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
         }else{
-          out.lfpos(2) = FUPHIGHT;
+          out.lf(2) = FUPHIGHT;
         }
       }else{
         if(cur_lfup_level > 0){
           cur_lfup_level--;
-          out.lfpos(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
+          out.lf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
         }else{
-          out.lfpos(2) = 0;
+          out.lf(2) = 0;
         }
       }
     }
     void lockFootXYOnContact(const humanpose_t& in, const bool& rfcs_in, const bool& lfcs_in, humanpose_t& out){
       out = in;
       if(rfcs_in){
-        out.rfpos(0) = pre_cont_rfpos(0);
-        out.rfpos(1) = pre_cont_rfpos(1);
+        out.rf(0) = pre_cont_rfpos(0);
+        out.rf(1) = pre_cont_rfpos(1);
       }else{
-        pre_cont_rfpos(0) = in.rfpos(0);
-        pre_cont_rfpos(1) = in.rfpos(1);
+        pre_cont_rfpos(0) = in.rf(0);
+        pre_cont_rfpos(1) = in.rf(1);
       }
       if(lfcs_in){
-        out.lfpos(0) = pre_cont_lfpos(0);
-        out.lfpos(1) = pre_cont_lfpos(1);
+        out.lf(0) = pre_cont_lfpos(0);
+        out.lf(1) = pre_cont_lfpos(1);
       }else{
-        pre_cont_lfpos(0) = in.lfpos(0);
-        pre_cont_lfpos(1) = in.lfpos(1);
+        pre_cont_lfpos(0) = in.lf(0);
+        pre_cont_lfpos(1) = in.lf(1);
       }
+//      out.rf(0) = 0;//足固定テスト
+//      out.rf(1) = 0;
+//      out.lf(0) = 0;
+//      out.lf(1) = 0;
     }
     void applyWorkspaceLimit(const humanpose_t& in, humanpose_t& out){}
-    void applyCOMToSupportRegionLimit(const humanpose_t& in, humanpose_t& out){}
+
+    void applyCOMToSupportRegionLimit(const humanpose_t& in, humanpose_t& out){
+      out = in;
+      double x_upper_region,x_lower_region,y_upper_region,y_lower_region;
+      if(rp_wld_initpos.rf(0) + in.rf(0) > rp_wld_initpos.lf(0) + in.lf(0)){
+        x_upper_region = rp_wld_initpos.rf(0) + in.rf(0) + 0.13;
+        x_lower_region = rp_wld_initpos.lf(0) + in.lf(0) - 0.08;
+      }else{
+        x_upper_region = rp_wld_initpos.lf(0) + in.lf(0) + 0.13;
+        x_lower_region = rp_wld_initpos.rf(0) + in.rf(0) - 0.08;
+      }
+      y_upper_region = rp_wld_initpos.lf(1) + in.lf(1) + 0.03;
+      y_lower_region = rp_wld_initpos.rf(1) + in.rf(1) - 0.03;
+      if(in.com(0) < x_lower_region){ out.com(0) = x_lower_region; }
+      else if(in.com(0) > x_upper_region){ out.com(0) = x_upper_region; }
+      if(in.com(1) < y_lower_region){ out.com(1) = y_lower_region; }
+      else if(in.com(1) > y_upper_region){ out.com(1) = y_upper_region; }
+    }
+
     void applyVelLimit(const humanpose_t& in, const humanpose_t& in_old, humanpose_t& out){
       for(int i=0;i<3;i++){if(in.com(i) - in_old.com(i) > MAXVEL){out.com(i) = in_old.com(i) + MAXVEL;}else if(in.com(i) - in_old.com(i) < -MAXVEL){out.com(i) = in_old.com(i) - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.rfpos(i) - in_old.rfpos(i) > MAXVEL){out.rfpos(i) = in_old.rfpos(i) + MAXVEL;}else if(in.rfpos(i) - in_old.rfpos(i) < -MAXVEL){out.rfpos(i) = in_old.rfpos(i) - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.lfpos(i) - in_old.lfpos(i) > MAXVEL){out.lfpos(i) = in_old.lfpos(i) + MAXVEL;}else if(in.lfpos(i) - in_old.lfpos(i) < -MAXVEL){out.lfpos(i) = in_old.lfpos(i) - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.rhpos(i) - in_old.rhpos(i) > MAXVEL){out.rhpos(i) = in_old.rhpos(i) + MAXVEL;}else if(in.rhpos(i) - in_old.rhpos(i) < -MAXVEL){out.rhpos(i) = in_old.rhpos(i) - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.lhpos(i) - in_old.lhpos(i) > MAXVEL){out.lhpos(i) = in_old.lhpos(i) + MAXVEL;}else if(in.lhpos(i) - in_old.lhpos(i) < -MAXVEL){out.lhpos(i) = in_old.lhpos(i) - MAXVEL;}}
+//      for(int i=0;i<3;i++){if(in.rf(i)  - in_old.rf(i)  > MAXVEL){out.rf(i)  = in_old.rf(i)  + MAXVEL;}else if(in.rf(i)  - in_old.rf(i)  < -MAXVEL){out.rf(i)  = in_old.rf(i)  - MAXVEL;}}
+//      for(int i=0;i<3;i++){if(in.lf(i)  - in_old.lf(i)  > MAXVEL){out.lf(i)  = in_old.lf(i)  + MAXVEL;}else if(in.lf(i)  - in_old.lf(i)  < -MAXVEL){out.lf(i)  = in_old.lf(i)  - MAXVEL;}}
+      for(int i=0;i<3;i++){if(in.rf(i)  - in_old.rf(i)  > MAXVEL/2){out.rf(i)  = in_old.rf(i)  + MAXVEL/2;}else if(in.rf(i)  - in_old.rf(i)  < -MAXVEL/2){out.rf(i)  = in_old.rf(i)  - MAXVEL/2;}}
+      for(int i=0;i<3;i++){if(in.lf(i)  - in_old.lf(i)  > MAXVEL/2){out.lf(i)  = in_old.lf(i)  + MAXVEL/2;}else if(in.lf(i)  - in_old.lf(i)  < -MAXVEL/2){out.lf(i)  = in_old.lf(i)  - MAXVEL/2;}}
+      for(int i=0;i<3;i++){if(in.rh(i)  - in_old.rh(i)  > MAXVEL){out.rh(i)  = in_old.rh(i)  + MAXVEL;}else if(in.rh(i)  - in_old.rh(i)  < -MAXVEL){out.rh(i)  = in_old.rh(i)  - MAXVEL;}}
+      for(int i=0;i<3;i++){if(in.lh(i)  - in_old.lh(i)  > MAXVEL){out.lh(i)  = in_old.lh(i)  + MAXVEL;}else if(in.lh(i)  - in_old.lh(i)  < -MAXVEL){out.lh(i)  = in_old.lh(i)  - MAXVEL;}}
     }
 };
 
@@ -493,7 +574,9 @@ class AutoBalancer
   rats::coordinates graspless_manip_reference_trans_coords;
   double pos_ik_thre, rot_ik_thre;
 
+  //for HumanSynchronizer
   boost::shared_ptr<HumanSynchronizer> hsp;
+  humanpose_t hp_raw_data;
 
 };
 

From 73de3d68c6ce8f6a7ea44b03998d57a097cb522b Mon Sep 17 00:00:00 2001
From: ishiguroJSK <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 7 Aug 2016 09:52:48 +0900
Subject: [PATCH 016/217] make smart access to HumanPose

---
 rtc/AutoBalancer/AutoBalancer.cpp |  25 ++--
 rtc/AutoBalancer/AutoBalancer.h   | 220 +++++++++++++++---------------
 2 files changed, 125 insertions(+), 120 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 02e4228293c..c31216090eb 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -473,13 +473,13 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     }
     //for HumanSynchronizer
     if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
-    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); for(int i=0;i<6;i++)hp_raw_data.rfw[i] = m_htrfw.data[i]; }
-    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); for(int i=0;i<6;i++)hp_raw_data.lfw[i] = m_htlfw.data[i]; }
-    if (m_htcomIn.isNew()){ m_htcomIn.read(); hp_raw_data.com   = HumanSynchronizer::Point3DToVector3(m_htcom.data); }
-    if (m_htrfIn.isNew()) { m_htrfIn.read();  hp_raw_data.rf = HumanSynchronizer::Point3DToVector3(m_htrf.data); }
-    if (m_htlfIn.isNew()) { m_htlfIn.read();  hp_raw_data.lf = HumanSynchronizer::Point3DToVector3(m_htlf.data); }
-    if (m_htrhIn.isNew()){  m_htrhIn.read();  hp_raw_data.rh = HumanSynchronizer::Point3DToVector3(m_htrh.data); }
-    if (m_htlhIn.isNew()){  m_htlhIn.read();  hp_raw_data.lh = HumanSynchronizer::Point3DToVector3(m_htlh.data); }
+    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); hp_raw_data.rfw = HumanSynchronizer::DoubleSeqToWrench6(m_htrfw); }
+    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); hp_raw_data.lfw = HumanSynchronizer::DoubleSeqToWrench6(m_htlfw); }
+    if (m_htcomIn.isNew()){ m_htcomIn.read(); hp_raw_data.com = HumanSynchronizer::Point3DToVector3(m_htcom); }
+    if (m_htrfIn.isNew()) { m_htrfIn.read();  hp_raw_data.rf  = HumanSynchronizer::Point3DToVector3(m_htrf); }
+    if (m_htlfIn.isNew()) { m_htlfIn.read();  hp_raw_data.lf  = HumanSynchronizer::Point3DToVector3(m_htlf); }
+    if (m_htrhIn.isNew()) { m_htrhIn.read();  hp_raw_data.rh  = HumanSynchronizer::Point3DToVector3(m_htrh); }
+    if (m_htlhIn.isNew()) { m_htlhIn.read();  hp_raw_data.lh  = HumanSynchronizer::Point3DToVector3(m_htlh); }
     if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
     hsp->readInput(hp_raw_data);
 
@@ -1009,7 +1009,7 @@ void AutoBalancer::solveLimbIK ()
   //for HumanSynchronizer
 //  static int HumanSyncCountdownNum = 5 * (1/m_dt);
   if(hsp->startCountdownForHumanSync){
-	  printf("Start Count Down for HumanSync [%d]\r", hsp->getRemainingCountDown());
+    std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";//ややCOMのIKに手間取った時プリント
 	  hsp->updateCountDown();
   }
   if(hsp->isHumanSyncOn()){
@@ -1023,7 +1023,9 @@ void AutoBalancer::solveLimbIK ()
       if(ikp.count("larm"))hsp->rp_wld_initpos.lh = ikp["larm"].target_link->p;
       hsp->ht_first_call = false;
 
-      hsp->calibInitHumanComPos();
+      hsp->calibInitHumanCOMFromZMP();
+
+      std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;//ややCOMのIKに手間取った時プリント
 
 	  }
   }else{
@@ -1032,6 +1034,7 @@ void AutoBalancer::solveLimbIK ()
 
   hsp->update();//////HumanSynchronizerの主要処理
   hsp->rp_ref_out.print();
+  cerr<<"getUpdateTime:"<<hsp->getUpdateTime()*1000<<"[ms]"<<endl;
 
   if(hsp->isHumanSyncOn()){
 	  ////////////////////// 重心拘束位置設定 /////////////////////////
@@ -1063,6 +1066,8 @@ void AutoBalancer::solveLimbIK ()
   if(hsp->isHumanSyncOn()){
     ikp["rleg"].target_p0 = hsp->rp_wld_initpos.rf + hsp->rp_ref_out.rf;
     ikp["lleg"].target_p0 = hsp->rp_wld_initpos.lf + hsp->rp_ref_out.lf;
+    if(ikp.count("rarm"))ikp["rarm"].target_p0 = hsp->rp_wld_initpos.rh + hsp->rp_ref_out.rh;
+    if(ikp.count("larm"))ikp["larm"].target_p0 = hsp->rp_wld_initpos.lh + hsp->rp_ref_out.lh;
     while(fabs(tmp_com_err(0))>COM_IK_MAX_ERROR || fabs(tmp_com_err(1))>COM_IK_MAX_ERROR || fabs(tmp_com_err(2))>COM_IK_MAX_ERROR){//Z方向のCOM合わせは要注意(たまに怪しい)
 		  m_robot->rootLink()->p(0) += tmp_com_err(0);
       m_robot->rootLink()->p(1) += tmp_com_err(1);
@@ -1086,7 +1091,7 @@ void AutoBalancer::solveLimbIK ()
 //    }
 //    pre_ik_basepos = m_robot->rootLink()->p;
 
-    //out用のデータ上書き
+    //outport用のデータ上書き
     ref_zmp(0) = hsp->rp_ref_out.zmp(0);
     ref_zmp(1) = hsp->rp_ref_out.zmp(1);
     ref_zmp(2) = 0;
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 14afa3dd699..674d3b2505a 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -31,32 +31,56 @@
 // Service Consumer stub headers
 // <rtc-template block="consumer_stub_h">
 
-class humanpose_t{
+
+class HumanPose{
+  private: hrp::Vector3 zero;
   public:
     hrp::Vector3 com,rf,lf,rh,lh;
     hrp::Vector3 zmp;
-    Eigen::VectorXd rfw,lfw;
-    humanpose_t(){ clear(); }
-    ~humanpose_t(){cout<<"humanpose_t destructed"<<endl;}
+    int idsize;
+    typedef struct{ hrp::Vector3 f; hrp::Vector3 t; }Wrench6;
+    Wrench6 rfw,lfw;
+    HumanPose() : idsize(10){
+      clear();
+    }
+    ~HumanPose(){cerr<<"HumanPose destructed"<<endl;}
     void clear(){
-      com =   hrp::Vector3::Zero();
-      rf = hrp::Vector3::Zero();
-      lf = hrp::Vector3::Zero();
-      rh = hrp::Vector3::Zero();
-      lh = hrp::Vector3::Zero();
-      zmp =   hrp::Vector3::Zero();
-      rfw.resize(6); rfw = Eigen::VectorXd::Zero(6);
-      lfw.resize(6); lfw = Eigen::VectorXd::Zero(6);
+      for(int i=0;i<idsize;i++){ Seq(i) = hrp::Vector3::Zero(); }
+    }
+    hrp::Vector3& Seq(const int i){
+      switch(i){
+        case 0: return com;
+        case 1: return rf;
+        case 2: return lf;
+        case 3: return rh;
+        case 4: return lh;
+        case 5: return zmp;
+        case 6: return rfw.f;
+        case 7: return rfw.t;
+        case 8: return lfw.f;
+        case 9: return lfw.t;
+        default: std::cerr <<"[WARN] HumanPose::getSeq(int) invalid index"<<std::endl;return zero;
+      }
+    }
+    const hrp::Vector3& Seq(const int i) const{//const指定オブジェクトからオーバーロードする時のためのオーバーライド
+//      return Seq(i);//呼べない・・・うーん
+      switch(i){
+        case 0: return com;
+        case 1: return rf;
+        case 2: return lf;
+        case 3: return rh;
+        case 4: return lh;
+        case 5: return zmp;
+        case 6: return rfw.f;
+        case 7: return rfw.t;
+        case 8: return lfw.f;
+        case 9: return lfw.t;
+        default: std::cerr <<"[WARN] HumanPose::getSeq(int) invalid index"<<std::endl;return zero;
+      }
     }
-    static void hp_printf(const humanpose_t& in){
-      printf("\x1b[31mcom:\x1b[39m %+05.2f %+05.2f %+05.2f ",in.com(0),in.com(1),in.com(2));
-      printf("\x1b[31mrf:\x1b[39m %+05.2f %+05.2f %+05.2f ",in.rf(0),in.rf(1),in.rf(2));
-      printf("\x1b[31mlf:\x1b[39m %+05.2f %+05.2f %+05.2f ",in.lf(0),in.lf(1),in.lf(2));
-      printf("\x1b[31mrh:\x1b[39m %+05.2f %+05.2f %+05.2f ",in.rh(0),in.rh(1),in.rh(2));
-      printf("\x1b[31mlh:\x1b[39m %+05.2f %+05.2f %+05.2f ",in.lh(0),in.lh(1),in.lh(2));
-      printf("\x1b[31mzmp:\x1b[39m %+05.2f %+05.2f %+05.2f ",in.zmp(0),in.zmp(1),in.zmp(2));
-      printf("\x1b[31mrfw:\x1b[39m %+04.0f %+04.0f %+05.0f %+04.0f %+04.0f %+04.0f ",in.rfw(0),in.rfw(1),in.rfw(2),in.rfw(3),in.rfw(4),in.rfw(5));
-      printf("\x1b[31mlfw:\x1b[39m %+04.0f %+04.0f %+05.0f %+04.0f %+04.0f %+04.0f ",in.lfw(0),in.lfw(1),in.lfw(2),in.lfw(3),in.lfw(4),in.lfw(5));
+    static void hp_printf(HumanPose& in){
+      const std::string cap[] = {"com:","rf:","lf:","rh:","lh:","zmp:","rfw:","","lfw:",""};
+      for(int i=0;i<10;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.Seq(i)(0),in.Seq(i)(1),in.Seq(i)(2)); }
       printf("\n");
     }
     void print(){ hp_printf(*this); }
@@ -77,35 +101,34 @@ class HumanSynchronizer{
     hrp::Vector3 init_wld_rp_rfpos,init_wld_rp_lfpos;
     hrp::Vector3 init_hp_calibcom;
     double MAXVEL,MAXACC;
-    humanpose_t hp_wld_raw;
-    humanpose_t hp_wld_initpos;
-    humanpose_t hp_rel_raw;
-    humanpose_t hp_filtered;
-    humanpose_t rp_ref_out_old;
+    HumanPose hp_wld_raw;
+    HumanPose hp_wld_initpos;
+    HumanPose hp_rel_raw;
+    HumanPose hp_filtered;
+    HumanPose rp_ref_out_old;
     int countdown_num;
     bool countdown_flag;
     bool HumanSyncOn;
+    struct timeval t_calc_start, t_calc_end;
 
   public:
+
     bool startCountdownForHumanSync;
     bool ht_first_call;
     bool is_rf_contact,is_lf_contact;
-
-
-    humanpose_t rp_wld_initpos;
+    HumanPose rp_wld_initpos;
     double h2r_ratio;
     hrp::Vector3 init_wld_rp_basepos;
-
-    humanpose_t rp_ref_out;
+    HumanPose rp_ref_out;
 
     HumanSynchronizer(){
 //      h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
-//      h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
-      h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
+      h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
+//      h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
       //h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
       cur_rfup_level = 0;       cur_lfup_level = 0;
       is_rf_contact = true;     is_lf_contact = true;
-      pre_cont_rfpos.Zero();    pre_cont_lfpos.Zero();
+      pre_cont_rfpos = hrp::Vector3::Zero();    pre_cont_lfpos = hrp::Vector3::Zero();
       init_wld_hp_rfpos = hrp::Vector3(0,-0.1/h2r_ratio,0);
       init_wld_hp_lfpos = hrp::Vector3(0, 0.1/h2r_ratio,0);
       init_wld_rp_rfpos = hrp::Vector3(0,-0.1,0);
@@ -122,23 +145,13 @@ class HumanSynchronizer{
       countdown_flag = false;
       countdown_num = 5*500;
     }
-
     ~HumanSynchronizer(){cout<<"HumanSynchronizer destructed"<<endl;}
 
-
-    void readInput(const humanpose_t& raw_in){ hp_wld_raw = raw_in; }
-
-    int getRemainingCountDown(){ return countdown_num; }
-    bool isHumanSyncOn(){ return HumanSyncOn; }
-
+    void readInput(const HumanPose& raw_in){ hp_wld_raw = raw_in; }
     void setInitOffsetPose(){ hp_wld_initpos = hp_wld_raw; }
-
-
-
-//    void startCountDown(int sec_x_dt){
-//      countdown_num = sec_x_dt;
-//    }
-
+    const int getRemainingCountDown() const { return countdown_num; }
+    const bool isHumanSyncOn() const { return HumanSyncOn; }
+    const double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0; }
     void updateCountDown(){
       if(countdown_num>0){
         countdown_num--;
@@ -147,16 +160,13 @@ class HumanSynchronizer{
         startCountdownForHumanSync = false;
       }
     }
-
     void update(){
+      gettimeofday(&t_calc_start, NULL);
       removeInitOffsetPose(hp_wld_raw, hp_wld_initpos, hp_rel_raw);
-
-      hp_rel_raw.com += init_hp_calibcom;
-
+      applyInitHumanCOMOffset(hp_wld_raw, init_hp_calibcom, hp_wld_raw);
       applyLPFilter(hp_rel_raw, hp_filtered, hp_filtered);
       calcWorldZMP((hp_filtered.rf+init_wld_hp_rfpos), (hp_filtered.lf+init_wld_hp_lfpos), hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
-
-      convertRelHumanPoseToWldRobotPose(hp_filtered,rp_ref_out);
+      convertRelHumanPoseToRelRobotPose(hp_filtered,rp_ref_out);
       judgeFootContactStates(rp_ref_out.rfw, rp_ref_out.lfw, is_rf_contact, is_lf_contact);
       overwriteFootZFromContactStates(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);
       lockFootXYOnContact(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);
@@ -164,80 +174,74 @@ class HumanSynchronizer{
       applyCOMToSupportRegionLimit(rp_ref_out, rp_ref_out);
       applyVelLimit(rp_ref_out, rp_ref_out_old, rp_ref_out);
       rp_ref_out_old = rp_ref_out;
+      gettimeofday(&t_calc_end, NULL);
     }
-
-    void calibInitHumanComPos(){
+    void calibInitHumanCOMFromZMP(){
       init_hp_calibcom(0) = hp_filtered.zmp(0);
       init_hp_calibcom(1) = hp_filtered.zmp(1);
     }
-
-
-    void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos,
-        const Eigen::VectorXd& rfwin, const Eigen::VectorXd& lfwin, hrp::Vector3& zmp_ans){
+    void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const HumanPose::Wrench6& rfwin, const HumanPose::Wrench6& lfwin, hrp::Vector3& zmp_ans){
       hrp::Vector3 rfzmp,lfzmp;
       const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
-      if( rfwin[2] > 1.0e-6 ){
-        rfzmp(0) = ( - rfwin[4] - rfwin[0] * F_H_OFFSET + rfwin[2] * 0 ) / rfwin[2] + rfpos(0);
-        rfzmp(1) = (   rfwin[3] - rfwin[1] * F_H_OFFSET + rfwin[2] * 0 ) / rfwin[2] + rfpos(1);
+      if( rfwin.f(2) > 1.0e-6 ){
+        rfzmp(0) = ( - rfwin.t(1) - rfwin.f(0) * F_H_OFFSET + rfwin.f(2) * 0 ) / rfwin.f(2) + rfpos(0);
+        rfzmp(1) = (   rfwin.t(0) - rfwin.f(1) * F_H_OFFSET + rfwin.f(2) * 0 ) / rfwin.f(2) + rfpos(1);
       }
-      if( lfwin[2] > 1.0e-6 ){
-        lfzmp(0) = ( - lfwin[4] - lfwin[0] * F_H_OFFSET + lfwin[2] * 0 ) / lfwin[2] + lfpos(0);
-        lfzmp(1) = (   lfwin[3] - lfwin[1] * F_H_OFFSET + lfwin[2] * 0 ) / lfwin[2] + lfpos(1);
+      if( lfwin.f(2) > 1.0e-6 ){
+        lfzmp(0) = ( - lfwin.t(1) - lfwin.f(0) * F_H_OFFSET + lfwin.f(2) * 0 ) / lfwin.f(2) + lfpos(0);
+        lfzmp(1) = (   lfwin.t(0) - lfwin.f(1) * F_H_OFFSET + lfwin.f(2) * 0 ) / lfwin.f(2) + lfpos(1);
       }
-      if( rfwin[2] > 1.0e-6 || lfwin[2] > 1.0e-6 ){
-        zmp_ans(0) = ( rfzmp(0)*rfwin[2] + lfzmp(0)*lfwin[2] ) / ( rfwin[2] + lfwin[2]);
-        zmp_ans(1) = ( rfzmp(1)*rfwin[2] + lfzmp(1)*lfwin[2] ) / ( rfwin[2] + lfwin[2]);
+      if( rfwin.f(2) > 1.0e-6 || lfwin.f(2) > 1.0e-6 ){
+        zmp_ans(0) = ( rfzmp(0)*rfwin.f(2) + lfzmp(0)*lfwin.f(2) ) / ( rfwin.f(2) + lfwin.f(2));
+        zmp_ans(1) = ( rfzmp(1)*rfwin.f(2) + lfzmp(1)*lfwin.f(2) ) / ( rfwin.f(2) + lfwin.f(2));
       }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
       zmp_ans(2) = 0;
     }
-    static hrp::Vector3 Point3DToVector3(const RTC::Point3D& in){ hrp::Vector3 out(in.x,in.y,in.z); return out; }
-
+    static hrp::Vector3 Point3DToVector3(const RTC::TimedPoint3D& in){ hrp::Vector3 out(in.data.x,in.data.y,in.data.z); return out; }
+    static HumanPose::Wrench6 DoubleSeqToWrench6(const RTC::TimedDoubleSeq& in){
+      HumanPose::Wrench6 out;
+      if(in.data.length() == 6){out.f(0)=in.data[0]; out.f(1)=in.data[1]; out.f(2)=in.data[2]; out.t(0)=in.data[3]; out.t(1)=in.data[4]; out.t(2)=in.data[5];
+      }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; out.f.Zero(); out.t.Zero(); }
+      return out;
+    }
 
   private:
 
+    void applyInitHumanCOMOffset(const HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
+      out = in;
+      out.com = in.com + com_offs;
+    }
     void startHumanSync(){
       countdown_flag = true;
       setInitOffsetPose();
       HumanSyncOn = true;
     }
-
-    void removeInitOffsetPose(const humanpose_t& abs_in, const humanpose_t& init_offset, humanpose_t& rel_out){
-      rel_out.com = abs_in.com - init_offset.com;
-      rel_out.rf  = abs_in.rf  - init_offset.rf;
-      rel_out.lf  = abs_in.lf  - init_offset.lf;
-      rel_out.rh  = abs_in.rh  - init_offset.rh;
-      rel_out.lh  = abs_in.lh  - init_offset.lh;
+    void removeInitOffsetPose(const HumanPose& abs_in, const HumanPose& init_offset, HumanPose& rel_out){
+      for(int i=0;i<rel_out.idsize-5;i++){
+        rel_out.Seq(i) =  abs_in.Seq(i) - init_offset.Seq(i);
+      }
       rel_out.rfw = abs_in.rfw;
       rel_out.lfw = abs_in.lfw;
     }
-    void applyLPFilter(const humanpose_t& raw_in, const humanpose_t& out_old, humanpose_t& filt_out){
-      filt_out.com = (1-FNUM) * out_old.com + FNUM * raw_in.com;
-      filt_out.rf  = (1-FNUM) * out_old.rf  + FNUM * raw_in.rf;
-      filt_out.lf  = (1-FNUM) * out_old.lf  + FNUM * raw_in.lf;
-      filt_out.rh  = (1-FNUM) * out_old.rh  + FNUM * raw_in.rh;
-      filt_out.lh  = (1-FNUM) * out_old.lh  + FNUM * raw_in.lh;
-      filt_out.zmp = (1-FNUM) * out_old.zmp + FNUM * raw_in.zmp;
-      filt_out.rfw = (1-FNUM) * out_old.rfw + FNUM * raw_in.rfw;
-      filt_out.lfw = (1-FNUM) * out_old.lfw + FNUM * raw_in.lfw;
+    void applyLPFilter(const HumanPose& raw_in, const HumanPose& out_old, HumanPose& filt_out){
+      for(int i=0;i<filt_out.idsize;i++){
+        filt_out.Seq(i) = (1-FNUM) * out_old.Seq(i) + FNUM * raw_in.Seq(i);
+      }
     }
-    void convertRelHumanPoseToWldRobotPose(const humanpose_t& hp_in, humanpose_t& rp_out){//Wldと言いつつまだRel
-      rp_out.com  = h2r_ratio * hp_in.com;//+ init_wld_rp_compos;
-      rp_out.rf   = h2r_ratio * hp_in.rf;//+ init_wld_rp_rfpos;
-      rp_out.lf   = h2r_ratio * hp_in.lf;//+ init_wld_rp_lfpos;
-      rp_out.rh   = h2r_ratio * hp_in.rh;
-      rp_out.lh   = h2r_ratio * hp_in.lh;
-      rp_out.zmp  = h2r_ratio * hp_in.zmp;
+    void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//Wldと言いつつまだRel
+      for(int i=0;i<rp_out.idsize-4;i++){
+        rp_out.Seq(i) =  h2r_ratio * hp_in.Seq(i);
+      }
       rp_out.rfw  = hp_in.rfw;
       rp_out.lfw  = hp_in.lfw;
     }
-    void judgeFootContactStates(const Eigen::VectorXd& rfw_in, const Eigen::VectorXd& lfw_in, bool& rfcs_ans, bool& lfcs_ans){
+    void judgeFootContactStates(const HumanPose::Wrench6& rfw_in, const HumanPose::Wrench6& lfw_in, bool& rfcs_ans, bool& lfcs_ans){
       //他のプログラムは足に荷重時に力センサの値がZ軸マイナスに出るっぽい？(のでTODO FIX)
-      if      (rfw_in(2)<CNT_F_TH && lfw_in(2)>CNT_F_TH){rfcs_ans = false;  lfcs_ans = true;}//右足浮遊かつ左足接地
-      else if (rfw_in(2)>CNT_F_TH && lfw_in(2)<CNT_F_TH){rfcs_ans = true;   lfcs_ans = false;}//右足接地かつ左足浮遊
+      if      (rfw_in.f(2)<CNT_F_TH && lfw_in.f(2)>CNT_F_TH){rfcs_ans = false;  lfcs_ans = true;}//右足浮遊かつ左足接地
+      else if (rfw_in.f(2)>CNT_F_TH && lfw_in.f(2)<CNT_F_TH){rfcs_ans = true;   lfcs_ans = false;}//右足接地かつ左足浮遊
       else                                              {rfcs_ans = true;   lfcs_ans = true;}//両足接地または両足浮遊
     }
-
-    void overwriteFootZFromContactStates(const humanpose_t& in, const bool& rfcs_in, const bool& lfcs_in, humanpose_t& out){
+    void overwriteFootZFromContactStates(const HumanPose& in, const bool& rfcs_in, const bool& lfcs_in, HumanPose& out){
       out = in;
       if(!rfcs_in){//右足浮遊時
         if(cur_rfup_level < FUPLEVELNUM){
@@ -270,7 +274,7 @@ class HumanSynchronizer{
         }
       }
     }
-    void lockFootXYOnContact(const humanpose_t& in, const bool& rfcs_in, const bool& lfcs_in, humanpose_t& out){
+    void lockFootXYOnContact(const HumanPose& in, const bool& rfcs_in, const bool& lfcs_in, HumanPose& out){
       out = in;
       if(rfcs_in){
         out.rf(0) = pre_cont_rfpos(0);
@@ -286,14 +290,11 @@ class HumanSynchronizer{
         pre_cont_lfpos(0) = in.lf(0);
         pre_cont_lfpos(1) = in.lf(1);
       }
-//      out.rf(0) = 0;//足固定テスト
-//      out.rf(1) = 0;
-//      out.lf(0) = 0;
-//      out.lf(1) = 0;
+//      out.rf(0) = 0; out.rf(1) = 0; out.lf(0) = 0; out.lf(1) = 0;//足固定テスト
     }
-    void applyWorkspaceLimit(const humanpose_t& in, humanpose_t& out){}
-
-    void applyCOMToSupportRegionLimit(const humanpose_t& in, humanpose_t& out){
+    void applyWorkspaceLimit(const HumanPose& in, HumanPose& out){
+    }
+    void applyCOMToSupportRegionLimit(const HumanPose& in, HumanPose& out){
       out = in;
       double x_upper_region,x_lower_region,y_upper_region,y_lower_region;
       if(rp_wld_initpos.rf(0) + in.rf(0) > rp_wld_initpos.lf(0) + in.lf(0)){
@@ -310,8 +311,7 @@ class HumanSynchronizer{
       if(in.com(1) < y_lower_region){ out.com(1) = y_lower_region; }
       else if(in.com(1) > y_upper_region){ out.com(1) = y_upper_region; }
     }
-
-    void applyVelLimit(const humanpose_t& in, const humanpose_t& in_old, humanpose_t& out){
+    void applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out){
       for(int i=0;i<3;i++){if(in.com(i) - in_old.com(i) > MAXVEL){out.com(i) = in_old.com(i) + MAXVEL;}else if(in.com(i) - in_old.com(i) < -MAXVEL){out.com(i) = in_old.com(i) - MAXVEL;}}
 //      for(int i=0;i<3;i++){if(in.rf(i)  - in_old.rf(i)  > MAXVEL){out.rf(i)  = in_old.rf(i)  + MAXVEL;}else if(in.rf(i)  - in_old.rf(i)  < -MAXVEL){out.rf(i)  = in_old.rf(i)  - MAXVEL;}}
 //      for(int i=0;i<3;i++){if(in.lf(i)  - in_old.lf(i)  > MAXVEL){out.lf(i)  = in_old.lf(i)  + MAXVEL;}else if(in.lf(i)  - in_old.lf(i)  < -MAXVEL){out.lf(i)  = in_old.lf(i)  - MAXVEL;}}
@@ -576,7 +576,7 @@ class AutoBalancer
 
   //for HumanSynchronizer
   boost::shared_ptr<HumanSynchronizer> hsp;
-  humanpose_t hp_raw_data;
+  HumanPose hp_raw_data;
 
 };
 

From 4ab63ffea8cc29e127ae33ce77f9421813343c22 Mon Sep 17 00:00:00 2001
From: ishiguroJSK <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 7 Aug 2016 11:47:06 +0900
Subject: [PATCH 017/217] fix countdown bug

---
 rtc/AutoBalancer/AutoBalancer.cpp |  6 +++---
 rtc/AutoBalancer/AutoBalancer.h   | 21 ++++++++++++---------
 2 files changed, 15 insertions(+), 12 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index c31216090eb..bfbd3621cc9 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1033,8 +1033,8 @@ void AutoBalancer::solveLimbIK ()
   }
 
   hsp->update();//////HumanSynchronizerの主要処理
-  hsp->rp_ref_out.print();
-  cerr<<"getUpdateTime:"<<hsp->getUpdateTime()*1000<<"[ms]"<<endl;
+  if(loop%100==0)hsp->rp_ref_out.print();
+  if(loop%100==0)cerr<<"getUpdateTime:"<<hsp->getUpdateTime()*1000<<"[ms]"<<endl;
 
   if(hsp->isHumanSyncOn()){
 	  ////////////////////// 重心拘束位置設定 /////////////////////////
@@ -1100,7 +1100,7 @@ void AutoBalancer::solveLimbIK ()
 	  if(loop%100==0)if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
   }else{
 	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
-		if (it->second.is_active) solveLimbIKforLimb(it->second);
+	    if (it->second.is_active) solveLimbIKforLimb(it->second);
 	  }
   }
   if (gg_is_walking && !gg_solved) stopWalking ();
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 674d3b2505a..4b1d04b5af5 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -107,7 +107,6 @@ class HumanSynchronizer{
     HumanPose hp_filtered;
     HumanPose rp_ref_out_old;
     int countdown_num;
-    bool countdown_flag;
     bool HumanSyncOn;
     struct timeval t_calc_start, t_calc_end;
 
@@ -122,8 +121,8 @@ class HumanSynchronizer{
     HumanPose rp_ref_out;
 
     HumanSynchronizer(){
-//      h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
-      h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
+      h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
+//      h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
 //      h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
       //h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
       cur_rfup_level = 0;       cur_lfup_level = 0;
@@ -142,7 +141,7 @@ class HumanSynchronizer{
       MAXVEL = 0.001;
       HumanSyncOn = false;
       ht_first_call = true;
-      countdown_flag = false;
+      startCountdownForHumanSync = false;
       countdown_num = 5*500;
     }
     ~HumanSynchronizer(){cout<<"HumanSynchronizer destructed"<<endl;}
@@ -211,11 +210,11 @@ class HumanSynchronizer{
       out = in;
       out.com = in.com + com_offs;
     }
-    void startHumanSync(){
-      countdown_flag = true;
-      setInitOffsetPose();
-      HumanSyncOn = true;
-    }
+//    void startHumanSync(){
+//      countdown_flag = true;
+//      setInitOffsetPose();
+//      HumanSyncOn = true;
+//    }
     void removeInitOffsetPose(const HumanPose& abs_in, const HumanPose& init_offset, HumanPose& rel_out){
       for(int i=0;i<rel_out.idsize-5;i++){
         rel_out.Seq(i) =  abs_in.Seq(i) - init_offset.Seq(i);
@@ -229,6 +228,10 @@ class HumanSynchronizer{
       }
     }
     void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//Wldと言いつつまだRel
+
+      if(rp_wld_initpos.com(2) > 0.6 && rp_wld_initpos.com(2) < 0.7)h2r_ratio = 0.62;//CHIDORIと自動判定
+      else if(rp_wld_initpos.com(2) > 1.0 && rp_wld_initpos.com(2) < 1.2)h2r_ratio = 0.96;//JAXONと自動判定
+
       for(int i=0;i<rp_out.idsize-4;i++){
         rp_out.Seq(i) =  h2r_ratio * hp_in.Seq(i);
       }

From 4faeddba8930060b8db17d2b9bf0e8518e05f003 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 10 Aug 2016 16:34:34 +0900
Subject: [PATCH 018/217] apply COM safe limit

---
 rtc/AutoBalancer/AutoBalancer.h | 63 ++++++++++++++++++---------------
 1 file changed, 34 insertions(+), 29 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 4b1d04b5af5..2dcf27c6e17 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -121,9 +121,9 @@ class HumanSynchronizer{
     HumanPose rp_ref_out;
 
     HumanSynchronizer(){
-      h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
+//      h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
 //      h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
-//      h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
+      h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
       //h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
       cur_rfup_level = 0;       cur_lfup_level = 0;
       is_rf_contact = true;     is_lf_contact = true;
@@ -164,14 +164,16 @@ class HumanSynchronizer{
       removeInitOffsetPose(hp_wld_raw, hp_wld_initpos, hp_rel_raw);
       applyInitHumanCOMOffset(hp_wld_raw, init_hp_calibcom, hp_wld_raw);
       applyLPFilter(hp_rel_raw, hp_filtered, hp_filtered);
+      lockFootXYOnContact(hp_filtered, is_rf_contact, is_lf_contact, hp_filtered);//根本から改変すべき
       calcWorldZMP((hp_filtered.rf+init_wld_hp_rfpos), (hp_filtered.lf+init_wld_hp_lfpos), hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
       convertRelHumanPoseToRelRobotPose(hp_filtered,rp_ref_out);
       judgeFootContactStates(rp_ref_out.rfw, rp_ref_out.lfw, is_rf_contact, is_lf_contact);
       overwriteFootZFromContactStates(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);
-      lockFootXYOnContact(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);
+//      lockFootXYOnContact(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);//ここで改変するとLPFかかってないからジャンプする
       applyWorkspaceLimit(rp_ref_out, rp_ref_out);
       applyCOMToSupportRegionLimit(rp_ref_out, rp_ref_out);
       applyVelLimit(rp_ref_out, rp_ref_out_old, rp_ref_out);
+//      rp_ref_out.com(2) = 0;//Z方向固定
       rp_ref_out_old = rp_ref_out;
       gettimeofday(&t_calc_end, NULL);
     }
@@ -206,6 +208,13 @@ class HumanSynchronizer{
 
   private:
 
+    void removeInitOffsetPose(const HumanPose& abs_in, const HumanPose& init_offset, HumanPose& rel_out){
+      for(int i=0;i<rel_out.idsize-5;i++){
+        rel_out.Seq(i) =  abs_in.Seq(i) - init_offset.Seq(i);
+      }
+      rel_out.rfw = abs_in.rfw;
+      rel_out.lfw = abs_in.lfw;
+    }
     void applyInitHumanCOMOffset(const HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
       out = in;
       out.com = in.com + com_offs;
@@ -215,23 +224,12 @@ class HumanSynchronizer{
 //      setInitOffsetPose();
 //      HumanSyncOn = true;
 //    }
-    void removeInitOffsetPose(const HumanPose& abs_in, const HumanPose& init_offset, HumanPose& rel_out){
-      for(int i=0;i<rel_out.idsize-5;i++){
-        rel_out.Seq(i) =  abs_in.Seq(i) - init_offset.Seq(i);
-      }
-      rel_out.rfw = abs_in.rfw;
-      rel_out.lfw = abs_in.lfw;
-    }
     void applyLPFilter(const HumanPose& raw_in, const HumanPose& out_old, HumanPose& filt_out){
       for(int i=0;i<filt_out.idsize;i++){
         filt_out.Seq(i) = (1-FNUM) * out_old.Seq(i) + FNUM * raw_in.Seq(i);
       }
     }
-    void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//Wldと言いつつまだRel
-
-      if(rp_wld_initpos.com(2) > 0.6 && rp_wld_initpos.com(2) < 0.7)h2r_ratio = 0.62;//CHIDORIと自動判定
-      else if(rp_wld_initpos.com(2) > 1.0 && rp_wld_initpos.com(2) < 1.2)h2r_ratio = 0.96;//JAXONと自動判定
-
+    void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
       for(int i=0;i<rp_out.idsize-4;i++){
         rp_out.Seq(i) =  h2r_ratio * hp_in.Seq(i);
       }
@@ -296,30 +294,37 @@ class HumanSynchronizer{
 //      out.rf(0) = 0; out.rf(1) = 0; out.lf(0) = 0; out.lf(1) = 0;//足固定テスト
     }
     void applyWorkspaceLimit(const HumanPose& in, HumanPose& out){
+      if(!is_rf_contact && is_lf_contact){//右足浮遊時
+        if(in.rf(1) + rp_wld_initpos.rf(1) > in.lf(1) + rp_wld_initpos.lf(1) - 0.15)out.rf(1) = in.lf(1) + rp_wld_initpos.lf(1) - rp_wld_initpos.rf(1) - 0.15;
+      }
+      else if(is_rf_contact && !is_lf_contact){//左足浮遊時
+        if(in.lf(1) + rp_wld_initpos.lf(1) < in.rf(1) + rp_wld_initpos.rf(1) + 0.15)out.lf(1) = in.rf(1) + rp_wld_initpos.rf(1) - rp_wld_initpos.lf(1) + 0.15;
+      }
     }
     void applyCOMToSupportRegionLimit(const HumanPose& in, HumanPose& out){
       out = in;
+      const double XUMARGIN = 0.08;
+      const double XLMARGIN = 0.04;
+      const double YLRMARGIN = 0.03;
       double x_upper_region,x_lower_region,y_upper_region,y_lower_region;
       if(rp_wld_initpos.rf(0) + in.rf(0) > rp_wld_initpos.lf(0) + in.lf(0)){
-        x_upper_region = rp_wld_initpos.rf(0) + in.rf(0) + 0.13;
-        x_lower_region = rp_wld_initpos.lf(0) + in.lf(0) - 0.08;
+        x_upper_region = rp_wld_initpos.rf(0) + in.rf(0) + XUMARGIN;
+        x_lower_region = rp_wld_initpos.lf(0) + in.lf(0) - XLMARGIN;
       }else{
-        x_upper_region = rp_wld_initpos.lf(0) + in.lf(0) + 0.13;
-        x_lower_region = rp_wld_initpos.rf(0) + in.rf(0) - 0.08;
+        x_upper_region = rp_wld_initpos.lf(0) + in.lf(0) + XUMARGIN;
+        x_lower_region = rp_wld_initpos.rf(0) + in.rf(0) - XLMARGIN;
       }
-      y_upper_region = rp_wld_initpos.lf(1) + in.lf(1) + 0.03;
-      y_lower_region = rp_wld_initpos.rf(1) + in.rf(1) - 0.03;
-      if(in.com(0) < x_lower_region){ out.com(0) = x_lower_region; }
-      else if(in.com(0) > x_upper_region){ out.com(0) = x_upper_region; }
-      if(in.com(1) < y_lower_region){ out.com(1) = y_lower_region; }
-      else if(in.com(1) > y_upper_region){ out.com(1) = y_upper_region; }
+      y_upper_region = rp_wld_initpos.lf(1) + in.lf(1) + YLRMARGIN;
+      y_lower_region = rp_wld_initpos.rf(1) + in.rf(1) - YLRMARGIN;
+      if(in.com(0) < x_lower_region){ out.com(0) = x_lower_region; cerr<<"[WARN] COM X Lower Limit!"<<endl;}
+      else if(in.com(0) > x_upper_region){ out.com(0) = x_upper_region; cerr<<"[WARN] COM X Upper Limit!"<<endl;}
+      if(in.com(1) < y_lower_region){ out.com(1) = y_lower_region; cerr<<"[WARN] COM Y Lower Limit!"<<endl;}
+      else if(in.com(1) > y_upper_region){ out.com(1) = y_upper_region; cerr<<"[WARN] COM Y Upper Limit!"<<endl;}
     }
     void applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out){
       for(int i=0;i<3;i++){if(in.com(i) - in_old.com(i) > MAXVEL){out.com(i) = in_old.com(i) + MAXVEL;}else if(in.com(i) - in_old.com(i) < -MAXVEL){out.com(i) = in_old.com(i) - MAXVEL;}}
-//      for(int i=0;i<3;i++){if(in.rf(i)  - in_old.rf(i)  > MAXVEL){out.rf(i)  = in_old.rf(i)  + MAXVEL;}else if(in.rf(i)  - in_old.rf(i)  < -MAXVEL){out.rf(i)  = in_old.rf(i)  - MAXVEL;}}
-//      for(int i=0;i<3;i++){if(in.lf(i)  - in_old.lf(i)  > MAXVEL){out.lf(i)  = in_old.lf(i)  + MAXVEL;}else if(in.lf(i)  - in_old.lf(i)  < -MAXVEL){out.lf(i)  = in_old.lf(i)  - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.rf(i)  - in_old.rf(i)  > MAXVEL/2){out.rf(i)  = in_old.rf(i)  + MAXVEL/2;}else if(in.rf(i)  - in_old.rf(i)  < -MAXVEL/2){out.rf(i)  = in_old.rf(i)  - MAXVEL/2;}}
-      for(int i=0;i<3;i++){if(in.lf(i)  - in_old.lf(i)  > MAXVEL/2){out.lf(i)  = in_old.lf(i)  + MAXVEL/2;}else if(in.lf(i)  - in_old.lf(i)  < -MAXVEL/2){out.lf(i)  = in_old.lf(i)  - MAXVEL/2;}}
+      for(int i=0;i<3;i++){if(in.rf(i)  - in_old.rf(i)  > MAXVEL){out.rf(i)  = in_old.rf(i)  + MAXVEL;}else if(in.rf(i)  - in_old.rf(i)  < -MAXVEL){out.rf(i)  = in_old.rf(i)  - MAXVEL;}}
+      for(int i=0;i<3;i++){if(in.lf(i)  - in_old.lf(i)  > MAXVEL){out.lf(i)  = in_old.lf(i)  + MAXVEL;}else if(in.lf(i)  - in_old.lf(i)  < -MAXVEL){out.lf(i)  = in_old.lf(i)  - MAXVEL;}}
       for(int i=0;i<3;i++){if(in.rh(i)  - in_old.rh(i)  > MAXVEL){out.rh(i)  = in_old.rh(i)  + MAXVEL;}else if(in.rh(i)  - in_old.rh(i)  < -MAXVEL){out.rh(i)  = in_old.rh(i)  - MAXVEL;}}
       for(int i=0;i<3;i++){if(in.lh(i)  - in_old.lh(i)  > MAXVEL){out.lh(i)  = in_old.lh(i)  + MAXVEL;}else if(in.lh(i)  - in_old.lh(i)  < -MAXVEL){out.lh(i)  = in_old.lh(i)  - MAXVEL;}}
     }

From 3a502bee3b46df2d03baf0d066b3b5b3963bc493 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 11 Aug 2016 04:45:50 +0900
Subject: [PATCH 019/217] fix leptrino filter

---
 3rdparty/qpOASES/CMakeLists.txt               |    5 -
 3rdparty/qpOASES/qpOASES-3.0/AUTHORS          |   74 -
 3rdparty/qpOASES/qpOASES-3.0/AUTHORS.txt      |   74 -
 3rdparty/qpOASES/qpOASES-3.0/CMakeLists.txt   |  150 -
 .../qpOASES/qpOASES-3.0/CTestTestfile.cmake   |    6 -
 3rdparty/qpOASES/qpOASES-3.0/INSTALL          |   79 -
 3rdparty/qpOASES/qpOASES-3.0/INSTALL.txt      |   79 -
 3rdparty/qpOASES/qpOASES-3.0/LICENSE          |  503 --
 3rdparty/qpOASES/qpOASES-3.0/LICENSE.txt      |  503 --
 3rdparty/qpOASES/qpOASES-3.0/README           |   84 -
 3rdparty/qpOASES/qpOASES-3.0/README.txt       |   84 -
 3rdparty/qpOASES/qpOASES-3.0/VERSIONS         |  106 -
 3rdparty/qpOASES/qpOASES-3.0/VERSIONS.txt     |  106 -
 3rdparty/qpOASES/qpOASES-3.0/bin/example1     |  Bin 32801 -> 0 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/example1a    |  Bin 28799 -> 0 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/example1b    |  Bin 31765 -> 0 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/example2     |  Bin 31208 -> 0 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/example3     |  Bin 30847 -> 0 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/example3b    |  Bin 30848 -> 0 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/example4     |  Bin 34008 -> 0 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/example5     |  Bin 39753 -> 0 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/exampleLP    |  Bin 31905 -> 0 bytes
 3rdparty/qpOASES/qpOASES-3.0/bin/index.html   |    7 -
 3rdparty/qpOASES/qpOASES-3.0/bin/qrecipe      |  Bin 57832 -> 0 bytes
 .../qpOASES/qpOASES-3.0/doc/DoxygenLayout.xml |  189 -
 .../qpOASES/qpOASES-3.0/doc/doxygen.config    | 1123 ---
 3rdparty/qpOASES/qpOASES-3.0/doc/index.html   |   12 -
 3rdparty/qpOASES/qpOASES-3.0/doc/mainpage.dox |  120 -
 3rdparty/qpOASES/qpOASES-3.0/doc/manual.pdf   |  Bin 710503 -> 0 bytes
 .../qpOASES/qpOASES-3.0/examples/example1.cpp |   97 -
 .../qpOASES-3.0/examples/example1a.cpp        |   85 -
 .../qpOASES-3.0/examples/example1b.cpp        |   86 -
 .../qpOASES/qpOASES-3.0/examples/example2.cpp |  124 -
 .../qpOASES/qpOASES-3.0/examples/example3.cpp |   88 -
 .../qpOASES-3.0/examples/example3b.cpp        |   88 -
 .../qpOASES/qpOASES-3.0/examples/example4.cpp |  172 -
 .../qpOASES-3.0/examples/example4CP.cpp       |  111 -
 .../qpOASES/qpOASES-3.0/examples/example5.cpp |  200 -
 .../qpOASES-3.0/examples/exampleLP.cpp        |   87 -
 .../qpOASES/qpOASES-3.0/examples/index.html   |   19 -
 .../qpOASES/qpOASES-3.0/examples/qrecipe.cpp  |  495 --
 .../qpOASES/qpOASES-3.0/include/index.html    |    9 -
 .../qpOASES/qpOASES-3.0/include/qpOASES.hpp   |   59 -
 .../qpOASES-3.0/include/qpOASES/Bounds.hpp    |  256 -
 .../qpOASES-3.0/include/qpOASES/Bounds.ipp    |  120 -
 .../qpOASES-3.0/include/qpOASES/Constants.hpp |   77 -
 .../include/qpOASES/ConstraintProduct.hpp     |   91 -
 .../include/qpOASES/Constraints.hpp           |  246 -
 .../include/qpOASES/Constraints.ipp           |  122 -
 .../qpOASES-3.0/include/qpOASES/Flipper.hpp   |  155 -
 .../qpOASES-3.0/include/qpOASES/Indexlist.hpp |  199 -
 .../qpOASES-3.0/include/qpOASES/Indexlist.ipp |   92 -
 .../qpOASES-3.0/include/qpOASES/Matrices.hpp  |  867 ---
 .../include/qpOASES/MessageHandling.hpp       |  472 --
 .../include/qpOASES/MessageHandling.ipp       |  144 -
 .../qpOASES-3.0/include/qpOASES/Options.hpp   |  170 -
 .../qpOASES-3.0/include/qpOASES/QProblem.hpp  | 1163 ---
 .../qpOASES-3.0/include/qpOASES/QProblem.ipp  |  283 -
 .../qpOASES-3.0/include/qpOASES/QProblemB.hpp | 1091 ---
 .../qpOASES-3.0/include/qpOASES/QProblemB.ipp |  477 --
 .../qpOASES-3.0/include/qpOASES/SQProblem.hpp |  394 -
 .../qpOASES-3.0/include/qpOASES/SQProblem.ipp |   51 -
 .../qpOASES-3.0/include/qpOASES/SubjectTo.hpp |  229 -
 .../qpOASES-3.0/include/qpOASES/SubjectTo.ipp |  158 -
 .../qpOASES-3.0/include/qpOASES/Types.hpp     |  281 -
 .../include/qpOASES/UnitTesting.hpp           |   79 -
 .../qpOASES-3.0/include/qpOASES/Utils.hpp     |  370 -
 .../qpOASES-3.0/include/qpOASES/Utils.ipp     |  174 -
 .../include/qpOASES/extras/OQPinterface.hpp   |  250 -
 .../qpOASES/extras/SolutionAnalysis.hpp       |  155 -
 .../qpOASES/extras/SolutionAnalysis.ipp       |   51 -
 .../include/qpOASES/extras/index.html         |   10 -
 .../qpOASES-3.0/include/qpOASES/index.html    |   33 -
 3rdparty/qpOASES/qpOASES-3.0/index.html       |   31 -
 .../qpOASES/qpOASES-3.0/interfaces/index.html |   12 -
 .../interfaces/matlab/example1.mat            |  Bin 790 -> 0 bytes
 .../interfaces/matlab/example1a.mat           |  Bin 896 -> 0 bytes
 .../interfaces/matlab/example1b.mat           |  Bin 549 -> 0 bytes
 .../qpOASES-3.0/interfaces/matlab/index.html  |   20 -
 .../qpOASES-3.0/interfaces/matlab/make.m      |  238 -
 .../qpOASES-3.0/interfaces/matlab/qpOASES.cpp |  557 --
 .../qpOASES-3.0/interfaces/matlab/qpOASES.m   |   77 -
 .../interfaces/matlab/qpOASES_auxInput.m      |  103 -
 .../matlab/qpOASES_matlab_utils.cpp           |  885 ---
 .../matlab/qpOASES_matlab_utils.hpp           |   92 -
 .../interfaces/matlab/qpOASES_options.m       |  251 -
 .../interfaces/matlab/qpOASES_sequence.cpp    | 1061 ---
 .../interfaces/matlab/qpOASES_sequence.m      |  112 -
 .../qpOASES-3.0/interfaces/octave/clean       |    3 -
 .../qpOASES-3.0/interfaces/octave/clean.sh    |    3 -
 .../interfaces/octave/example1.mat            |  Bin 790 -> 0 bytes
 .../interfaces/octave/example1a.mat           |  Bin 896 -> 0 bytes
 .../interfaces/octave/example1b.mat           |  Bin 549 -> 0 bytes
 .../qpOASES-3.0/interfaces/octave/index.html  |   21 -
 .../qpOASES-3.0/interfaces/octave/make.m      |  238 -
 .../qpOASES-3.0/interfaces/octave/qpOASES.cpp |  557 --
 .../qpOASES-3.0/interfaces/octave/qpOASES.m   |   77 -
 .../interfaces/octave/qpOASES_auxInput.m      |  103 -
 .../octave/qpOASES_octave_utils.cpp           |  884 ---
 .../octave/qpOASES_octave_utils.hpp           |   92 -
 .../interfaces/octave/qpOASES_options.m       |  251 -
 .../interfaces/octave/qpOASES_sequence.cpp    | 1061 ---
 .../interfaces/octave/qpOASES_sequence.m      |  112 -
 .../qpOASES-3.0/interfaces/python/README.rst  |   68 -
 .../python/examples/cython/example1.pyx       |   73 -
 .../python/examples/cython/index.html         |    9 -
 .../python/examples/cython/setup.py           |   16 -
 .../interfaces/python/examples/example1.py    |   76 -
 .../interfaces/python/examples/example1b.py   |   72 -
 .../interfaces/python/examples/example2.py    |   90 -
 .../interfaces/python/examples/index.html     |   11 -
 .../qpOASES-3.0/interfaces/python/index.html  |   13 -
 .../qpOASES-3.0/interfaces/python/qpoases.pxd |  476 --
 .../qpOASES-3.0/interfaces/python/qpoases.pyx |  779 --
 .../qpOASES-3.0/interfaces/python/setup.py    |   72 -
 .../interfaces/python/tests/__init__.py       |    1 -
 .../interfaces/python/tests/index.html        |   11 -
 .../interfaces/python/tests/test_examples.py  |  328 -
 .../interfaces/python/tests/test_idhessian.py |   94 -
 .../interfaces/python/tests/test_testbench.py |  332 -
 .../interfaces/scilab/example1.dat            |  Bin 656 -> 0 bytes
 .../interfaces/scilab/example1a.dat           |  Bin 784 -> 0 bytes
 .../interfaces/scilab/example1b.dat           |  Bin 408 -> 0 bytes
 .../qpOASES-3.0/interfaces/scilab/index.html  |   14 -
 .../interfaces/scilab/qpOASESinterface.c      | 1061 ---
 .../interfaces/scilab/qpOASESinterface.sce    |   41 -
 .../interfaces/scilab/qpOASESroutines.cpp     |  365 -
 .../interfaces/simulink/example_QProblem.mdl  |  803 ---
 .../interfaces/simulink/example_QProblemB.mdl |  745 --
 .../interfaces/simulink/example_SQProblem.mdl |  797 ---
 .../interfaces/simulink/index.html            |   17 -
 .../simulink/load_example_QProblem.m          |   93 -
 .../simulink/load_example_QProblemB.m         |   75 -
 .../simulink/load_example_SQProblem.m         |   93 -
 .../qpOASES-3.0/interfaces/simulink/make.m    |  239 -
 .../interfaces/simulink/qpOASES_QProblem.cpp  |  447 --
 .../interfaces/simulink/qpOASES_QProblemB.cpp |  392 -
 .../interfaces/simulink/qpOASES_SQProblem.cpp |  460 --
 .../qpOASES/qpOASES-3.0/libs/libqpOASES.so    |    1 -
 .../qpOASES-3.0/libs/libqpOASES.so.3.0        |  Bin 1269403 -> 0 bytes
 3rdparty/qpOASES/qpOASES-3.0/make.mk          |   38 -
 3rdparty/qpOASES/qpOASES-3.0/make_cygwin.mk   |  106 -
 3rdparty/qpOASES/qpOASES-3.0/make_linux.mk    |  107 -
 3rdparty/qpOASES/qpOASES-3.0/make_osx.mk      |   99 -
 3rdparty/qpOASES/qpOASES-3.0/make_windows.mk  |  114 -
 .../qpOASES-3.0/src/BLASReplacement.cpp       |  145 -
 3rdparty/qpOASES/qpOASES-3.0/src/Bounds.cpp   |  515 --
 .../qpOASES/qpOASES-3.0/src/Constraints.cpp   |  500 --
 3rdparty/qpOASES/qpOASES-3.0/src/Flipper.cpp  |  273 -
 .../qpOASES/qpOASES-3.0/src/Indexlist.cpp     |  319 -
 .../qpOASES-3.0/src/LAPACKReplacement.cpp     |  120 -
 3rdparty/qpOASES/qpOASES-3.0/src/Matrices.cpp | 1967 -----
 .../qpOASES-3.0/src/MessageHandling.cpp       |  612 --
 .../qpOASES/qpOASES-3.0/src/OQPinterface.cpp  |  670 --
 3rdparty/qpOASES/qpOASES-3.0/src/Options.cpp  |  553 --
 3rdparty/qpOASES/qpOASES-3.0/src/QProblem.cpp | 6338 -----------------
 .../qpOASES/qpOASES-3.0/src/QProblemB.cpp     | 3862 ----------
 .../qpOASES/qpOASES-3.0/src/SQProblem.cpp     |  553 --
 .../qpOASES-3.0/src/SolutionAnalysis.cpp      |  567 --
 .../qpOASES/qpOASES-3.0/src/SubjectTo.cpp     |  289 -
 3rdparty/qpOASES/qpOASES-3.0/src/Utils.cpp    |  984 ---
 3rdparty/qpOASES/qpOASES-3.0/src/index.html   |   24 -
 .../qpOASES-3.0/testing/checkForMemoryLeaks   |  106 -
 .../testing/cpp/data/fetch_cpp_data           |   34 -
 .../qpOASES-3.0/testing/cpp/data/index.html   |    8 -
 .../qpOASES-3.0/testing/cpp/index.html        |   33 -
 .../qpOASES-3.0/testing/cpp/test_bench.cpp    |  309 -
 .../testing/cpp/test_constraintProduct1.cpp   |  197 -
 .../testing/cpp/test_constraintProduct2.cpp   |  196 -
 .../qpOASES-3.0/testing/cpp/test_example1.cpp |  123 -
 .../testing/cpp/test_example1a.cpp            |  118 -
 .../testing/cpp/test_example1b.cpp            |  119 -
 .../qpOASES-3.0/testing/cpp/test_example2.cpp |  132 -
 .../qpOASES-3.0/testing/cpp/test_example4.cpp |  211 -
 .../qpOASES-3.0/testing/cpp/test_example5.cpp |  204 -
 .../qpOASES-3.0/testing/cpp/test_example6.cpp |  112 -
 .../qpOASES-3.0/testing/cpp/test_example7.cpp |   90 -
 .../testing/cpp/test_exampleLP.cpp            |  121 -
 .../testing/cpp/test_gradientShift.cpp        |  125 -
 .../testing/cpp/test_guessedWS1.cpp           |  164 -
 .../testing/cpp/test_indexlist.cpp            |   96 -
 .../qpOASES-3.0/testing/cpp/test_janick1.cpp  |  197 -
 .../qpOASES-3.0/testing/cpp/test_janick2.cpp  |  251 -
 .../qpOASES-3.0/testing/cpp/test_matrices.cpp |  903 ---
 .../testing/cpp/test_matrices2.cpp            |  113 -
 .../testing/cpp/test_matrices3.cpp            |   90 -
 .../qpOASES-3.0/testing/cpp/test_qrecipe.cpp  |  149 -
 .../testing/cpp/test_qrecipe_data.hpp         |  401 --
 .../testing/cpp/test_runAllOqpExamples.cpp    |  177 -
 .../testing/cpp/test_sebastien1.cpp           |   74 -
 .../testing/cpp/test_vanBarelsUnboundedQP.cpp |   69 -
 .../qpOASES/qpOASES-3.0/testing/index.html    |   12 -
 .../testing/matlab/auxFiles/generateExample.m |   25 -
 .../matlab/auxFiles/generateRandomQp.m        |  110 -
 .../testing/matlab/auxFiles/getKktResidual.m  |  134 -
 .../testing/matlab/auxFiles/index.html        |   13 -
 .../testing/matlab/auxFiles/isoctave.m        |   21 -
 .../matlab/auxFiles/setupQpDataStruct.m       |   18 -
 .../matlab/auxFiles/setupQpFeaturesStruct.m   |   21 -
 .../testing/matlab/data/fetch_matlab_data     |   34 -
 .../testing/matlab/data/index.html            |    8 -
 .../qpOASES-3.0/testing/matlab/index.html     |   11 -
 .../qpOASES-3.0/testing/matlab/runAllTests.m  |  153 -
 .../testing/matlab/tests/index.html           |   37 -
 .../testing/matlab/tests/runAlexInfeas1.m     |   30 -
 .../testing/matlab/tests/runAlexInfeas2.m     |   35 -
 .../matlab/tests/runAlternativeX0Test.m       |   84 -
 .../testing/matlab/tests/runBenchmarkCHAIN1.m |   56 -
 .../matlab/tests/runBenchmarkCHAIN1A.m        |   57 -
 .../testing/matlab/tests/runBenchmarkCRANE1.m |   51 -
 .../testing/matlab/tests/runBenchmarkCRANE2.m |   52 -
 .../testing/matlab/tests/runBenchmarkCRANE3.m |   51 -
 .../testing/matlab/tests/runBenchmarkDIESEL.m |   55 -
 .../matlab/tests/runBenchmarkEQUALITY1.m      |   52 -
 .../matlab/tests/runBenchmarkEQUALITY2.m      |   56 -
 .../matlab/tests/runBenchmarkEXAMPLE1.m       |   52 -
 .../matlab/tests/runBenchmarkEXAMPLE1A.m      |   52 -
 .../matlab/tests/runBenchmarkEXAMPLE1B.m      |   52 -
 .../matlab/tests/runBenchmarkIDHESSIAN1.m     |   55 -
 .../matlab/tests/runInterfaceSeqTest.m        |  428 --
 .../testing/matlab/tests/runInterfaceTest.m   |  459 --
 .../testing/matlab/tests/runQAP8.m            |   36 -
 .../testing/matlab/tests/runQSHARE1B.m        |   43 -
 .../testing/matlab/tests/runRandomIdHessian.m |  428 --
 .../matlab/tests/runRandomZeroHessian.m       |  426 --
 .../matlab/tests/runSimpleSpringExample.m     |   34 -
 .../matlab/tests/runTestAPrioriKnownSeq1.m    |   87 -
 .../testing/matlab/tests/runTestSeq.m         |  104 -
 .../testing/matlab/tests/runTestSparse.m      |   33 -
 .../testing/matlab/tests/runTestSparse2.m     |   33 -
 .../testing/matlab/tests/runTestSparse3.m     |   39 -
 .../testing/matlab/tests/runTestSparse4.m     |   29 -
 .../matlab/tests/runTestWorkingSetLI.m        |   50 -
 .../matlab/tests/runVanBarelsUnboundedQP.m    |   25 -
 .../qpOASES/qpOASES-3.0/testing/runUnitTests  |  106 -
 CMakeLists.txt                                |    2 +-
 rtc/AutoBalancer/AutoBalancer.cpp             |   10 +-
 rtc/AutoBalancer/AutoBalancer.h               |   26 +-
 rtc/AutoBalancer/bak/AutoBalancer.cpp         | 2177 ++++++
 rtc/AutoBalancer/bak/AutoBalancer.h           |  596 ++
 240 files changed, 2800 insertions(+), 55724 deletions(-)
 delete mode 100644 3rdparty/qpOASES/CMakeLists.txt
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/AUTHORS
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/AUTHORS.txt
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/CMakeLists.txt
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/CTestTestfile.cmake
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/INSTALL
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/INSTALL.txt
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/LICENSE
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/LICENSE.txt
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/README
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/README.txt
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/VERSIONS
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/VERSIONS.txt
 delete mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/example1
 delete mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/example1a
 delete mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/example1b
 delete mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/example2
 delete mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/example3
 delete mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/example3b
 delete mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/example4
 delete mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/example5
 delete mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/exampleLP
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/bin/index.html
 delete mode 100755 3rdparty/qpOASES/qpOASES-3.0/bin/qrecipe
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/doc/DoxygenLayout.xml
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/doc/doxygen.config
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/doc/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/doc/mainpage.dox
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/doc/manual.pdf
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example1.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example1a.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example1b.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example2.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example3.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example3b.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example4.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example4CP.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/example5.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/exampleLP.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/examples/qrecipe.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Bounds.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Bounds.ipp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constants.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/ConstraintProduct.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constraints.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constraints.ipp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Flipper.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Indexlist.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Indexlist.ipp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Matrices.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/MessageHandling.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/MessageHandling.ipp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Options.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblem.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblem.ipp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblemB.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblemB.ipp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SQProblem.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SQProblem.ipp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SubjectTo.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SubjectTo.ipp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Types.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/UnitTesting.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Utils.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Utils.ipp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/OQPinterface.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/SolutionAnalysis.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/SolutionAnalysis.ipp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1.mat
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1a.mat
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1b.mat
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/make.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_auxInput.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_matlab_utils.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_matlab_utils.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_options.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_sequence.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_sequence.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/clean
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/clean.sh
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1.mat
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1a.mat
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1b.mat
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/make.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_auxInput.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_octave_utils.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_octave_utils.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_options.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_sequence.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_sequence.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/README.rst
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/example1.pyx
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/setup.py
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example1.py
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example1b.py
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example2.py
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/qpoases.pxd
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/qpoases.pyx
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/setup.py
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/__init__.py
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_examples.py
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_idhessian.py
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_testbench.py
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1.dat
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1a.dat
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1b.dat
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESinterface.c
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESinterface.sce
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESroutines.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_QProblem.mdl
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_QProblemB.mdl
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_SQProblem.mdl
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_QProblem.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_QProblemB.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_SQProblem.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/make.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_QProblem.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_QProblemB.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_SQProblem.cpp
 delete mode 120000 3rdparty/qpOASES/qpOASES-3.0/libs/libqpOASES.so
 delete mode 100755 3rdparty/qpOASES/qpOASES-3.0/libs/libqpOASES.so.3.0
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/make.mk
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/make_cygwin.mk
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/make_linux.mk
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/make_osx.mk
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/make_windows.mk
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/BLASReplacement.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/Bounds.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/Constraints.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/Flipper.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/Indexlist.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/LAPACKReplacement.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/Matrices.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/MessageHandling.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/OQPinterface.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/Options.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/QProblem.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/QProblemB.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/SQProblem.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/SolutionAnalysis.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/SubjectTo.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/Utils.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/src/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/checkForMemoryLeaks
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/data/fetch_cpp_data
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/data/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_bench.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_constraintProduct1.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_constraintProduct2.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1a.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1b.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example2.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example4.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example5.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example6.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example7.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_exampleLP.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_gradientShift.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_guessedWS1.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_indexlist.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_janick1.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_janick2.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices2.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices3.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_qrecipe.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_qrecipe_data.hpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_runAllOqpExamples.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_sebastien1.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_vanBarelsUnboundedQP.cpp
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/generateExample.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/generateRandomQp.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/getKktResidual.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/isoctave.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/setupQpDataStruct.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/setupQpFeaturesStruct.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/data/fetch_matlab_data
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/data/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/runAllTests.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/index.html
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlexInfeas1.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlexInfeas2.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlternativeX0Test.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCHAIN1.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCHAIN1A.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE1.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE2.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE3.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkDIESEL.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEQUALITY1.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEQUALITY2.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1A.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1B.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkIDHESSIAN1.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runInterfaceSeqTest.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runInterfaceTest.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runQAP8.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runQSHARE1B.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runRandomIdHessian.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runRandomZeroHessian.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runSimpleSpringExample.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestAPrioriKnownSeq1.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSeq.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse2.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse3.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse4.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestWorkingSetLI.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runVanBarelsUnboundedQP.m
 delete mode 100644 3rdparty/qpOASES/qpOASES-3.0/testing/runUnitTests
 create mode 100644 rtc/AutoBalancer/bak/AutoBalancer.cpp
 create mode 100644 rtc/AutoBalancer/bak/AutoBalancer.h

diff --git a/3rdparty/qpOASES/CMakeLists.txt b/3rdparty/qpOASES/CMakeLists.txt
deleted file mode 100644
index 39f6061d219..00000000000
--- a/3rdparty/qpOASES/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-if (NOT EXISTS ${CMAKE_CURRENT_BINARY_DIR}/qpOASES-3.0)
-  execute_process(COMMAND svn co https://projects.coin-or.org/svn/qpOASES/stable/3.0 ${CMAKE_CURRENT_BINARY_DIR}/qpOASES-3.0)
-  execute_process(COMMAND sed -i -e "s/qpOASES\ STATIC/qpOASES\ SHARED/g" ${CMAKE_CURRENT_BINARY_DIR}/qpOASES-3.0/CMakeLists.txt)
-  execute_process(COMMAND cmake -E chdir ${CMAKE_CURRENT_BINARY_DIR}/qpOASES-3.0 make)
-endif()
diff --git a/3rdparty/qpOASES/qpOASES-3.0/AUTHORS b/3rdparty/qpOASES/qpOASES-3.0/AUTHORS
deleted file mode 100644
index 49b20f8dc7d..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/AUTHORS
+++ /dev/null
@@ -1,74 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-MAIN AUTHORS
-============
-
-qpOASES's core functionality and software design have been developed by the 
-following main developers (in alphabetical order):
-
-    Hans Joachim Ferreau
-    Christian Kirches
-    Andreas Potschka
-
-
-
-FURTHER AUTHORS
-===============
-
-Moreover, the following developers have contributed code to qpOASES's 
-third-party interfaces (in alphabetical order):
-
-    Alexander Buchner
-    Holger Diedam
-    Manuel Kudruss
-    Sebastian F. Walter
-
-
-
-CONTRIBUTORS
-============
-
-Finally, the following people have not contributed to the source code,
-but have helped making qpOASES even more useful by testing, reporting 
-bugs or proposing algorithmic improvements (in alphabetical order):
-
-    Eckhard Arnold
-    Boris Houska
-    Aude Perrin
-    Milan Vukov
-    Thomas Wiese
-    Leonard Wirsching
-
-
-
-All users are invited to further improve qpOASES by providing comments, 
-code enhancements, bug reports, additional documentation or whatever you 
-feel is missing.
-
-
-
-##
-##	end of file
-##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/AUTHORS.txt b/3rdparty/qpOASES/qpOASES-3.0/AUTHORS.txt
deleted file mode 100644
index 5132d8c105e..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/AUTHORS.txt
+++ /dev/null
@@ -1,74 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-MAIN AUTHORS
-============
-
-qpOASES's core functionality and software design have been developed by the 
-following main developers (in alphabetical order):
-
-    Hans Joachim Ferreau
-    Christian Kirches
-    Andreas Potschka
-
-
-
-FURTHER AUTHORS
-===============
-
-Moreover, the following developers have contributed code to qpOASES's 
-third-party interfaces (in alphabetical order):
-
-    Alexander Buchner
-    Holger Diedam
-    Manuel Kudruss
-    Sebastian F. Walter
-
-
-
-CONTRIBUTORS
-============
-
-Finally, the following people have not contributed to the source code,
-but have helped making qpOASES even more useful by testing, reporting 
-bugs or proposing algorithmic improvements (in alphabetical order):
-
-    Eckhard Arnold
-    Boris Houska
-    Aude Perrin
-    Milan Vukov
-    Thomas Wiese
-    Leonard Wirsching
-
-
-
-All users are invited to further improve qpOASES by providing comments, 
-code enhancements, bug reports, additional documentation or whatever you 
-feel is missing.
-
-
-
-##
-##	end of file
-##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/CMakeLists.txt b/3rdparty/qpOASES/qpOASES-3.0/CMakeLists.txt
deleted file mode 100644
index fa0c2a3d123..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/CMakeLists.txt
+++ /dev/null
@@ -1,150 +0,0 @@
-##
-##     This file is part of qpOASES.
-##
-##     qpOASES -- An Implementation of the Online Active Set Strategy.
-##     Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##     Christian Kirches et al. All rights reserved.
-##
-##     qpOASES 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.
-##
-##     qpOASES 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 qpOASES; if not, write to the Free Software
-##     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-##
-##     Filename:  CMakeLists.txt
-##     Author:    Hans Joachim Ferreau (thanks to Milan Vukov)
-##     Version:   3.0
-##     Date:      2007-2014
-##
-
-cmake_minimum_required(VERSION 2.6)
-
-PROJECT(qpOASES CXX)
-SET(PACKAGE_NAME "qpOASES")
-SET(PACKAGE_VERSION "3.0.1")
-SET(PACKAGE_SO_VERSION "3.0")
-SET(PACKAGE_DESCRIPTION "QP solver using the online active set strategy")
-SET(PACKAGE_AUTHOR "Hans Joachim Ferreau")
-SET(PACKAGE_MAINTAINER "Hans Joachim Ferreau")
-SET(PACKAGE_URL "https://projects.coin-or.org/qpOASES")
-
-SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/bin)
-SET(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/libs)
-
-IF( NOT CMAKE_VERBOSE_MAKEFILE )
-    SET( CMAKE_VERBOSE_MAKEFILE OFF )
-ENDIF( NOT CMAKE_VERBOSE_MAKEFILE )
-
-IF( NOT CMAKE_BUILD_TYPE )
-    SET(CMAKE_BUILD_TYPE Release CACHE STRING
-        "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel."
-        FORCE
-        )
-ENDIF( NOT CMAKE_BUILD_TYPE )
-
-
-############################################################
-#################### compiler flags ########################
-############################################################
-SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D__NO_COPYRIGHT__")
-IF ( UNIX )
-    SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -pedantic -Wfloat-equal -Wshadow -DLINUX")
-    SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_DEBUG} -O3 -finline-functions")
-ELSEIF( WINDOWS )
-    SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -nologo -EHsc -DWIN32")
-ENDIF()
-
-SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D__DEBUG__")
-
-############################################################
-######################## rpath #############################
-############################################################
-# use, i.e. don't skip the full RPATH for the build tree
-set(CMAKE_SKIP_BUILD_RPATH  FALSE)
-
-# when building, don't use the install RPATH already
-# (but later on when installing)
-set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
-
-set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib:${CMAKE_INSTALL_PREFIX}/lib/casadi")
-
-# add the automatically determined parts of the RPATH
-# which point to directories outside the build tree to the install RPATH
-set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
-
-# the RPATH to be used when installing, but only if it's not a system directory
-list(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir)
-if("${isSystemDir}" STREQUAL "-1")
-   set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib:${CMAKE_INSTALL_PREFIX}/lib/casadi")
-endif("${isSystemDir}" STREQUAL "-1")
-
-
-
-############################################################
-#################### build and install #####################
-############################################################
-INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/include)
-
-# compile qpOASES libraries
-FILE(GLOB SRC src/*.cpp)
-
-# library
-#ADD_LIBRARY(qpOASES STATIC ${SRC})
-ADD_LIBRARY(qpOASES SHARED ${SRC})
-INSTALL(TARGETS qpOASES
-  LIBRARY DESTINATION lib
-  ARCHIVE DESTINATION lib
-  RUNTIME DESTINATION lib
-)
-SET_TARGET_PROPERTIES(
-    qpOASES
-    PROPERTIES
-    SOVERSION ${PACKAGE_SO_VERSION}
-    )
-
-# headers
-INSTALL(FILES include/qpOASES.hpp
-  DESTINATION include)
-INSTALL(DIRECTORY include/qpOASES
-  DESTINATION include
-  FILES_MATCHING PATTERN "*.hpp"
-  PATTERN ".svn" EXCLUDE)
-
-############################################################
-######################### examples #########################
-############################################################
-# compile qpOASES examples
-SET(EXAMPLE_NAMES
-    example1
-    example1a
-    example1b
-    example2
-    example3
-    example3b
-    example4
-    example5
-    exampleLP
-    qrecipe
-)
-
-FOREACH(ELEMENT ${EXAMPLE_NAMES})
-    ADD_EXECUTABLE(${ELEMENT} examples/${ELEMENT}.cpp)
-    TARGET_LINK_LIBRARIES(${ELEMENT} qpOASES)
-ENDFOREACH(ELEMENT ${EXAMPLE_NAMES})
-
-
-##
-##   end of file
-##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/CTestTestfile.cmake b/3rdparty/qpOASES/qpOASES-3.0/CTestTestfile.cmake
deleted file mode 100644
index 321a6fdcdf6..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/CTestTestfile.cmake
+++ /dev/null
@@ -1,6 +0,0 @@
-# CMake generated Testfile for 
-# Source directory: /home/ishiguro/catkin_ws/chidori/build/hrpsys/3rdparty/qpOASES/qpOASES-3.0
-# Build directory: /home/ishiguro/catkin_ws/chidori/build/hrpsys/3rdparty/qpOASES/qpOASES-3.0
-# 
-# This file includes the relevant testing commands required for 
-# testing this directory and lists subdirectories to be tested as well.
diff --git a/3rdparty/qpOASES/qpOASES-3.0/INSTALL b/3rdparty/qpOASES/qpOASES-3.0/INSTALL
deleted file mode 100644
index 69852192cec..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/INSTALL
+++ /dev/null
@@ -1,79 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-INSTALLATION UNDER LINUX
-========================
-
-0. Obtain qpOASES from COIN-OR:
-
-Download a zipped archive containg the latest stable release and unpack it 
-into <install-dir>. Alternatively, you check out the latest stable branch, 
-e.g. by running
-
-svn co https://projects.coin-or.org/svn/qpOASES/stable/3.0 <install-dir>
-
-from your shell. 
-
-
-1. Compilation of the qpOASES library libqpOASES.a and test examples:
-
-cd <install-dir>
-make
-
-The library  libqpOASES.a  provides the complete functionality of the qpOASES 
-software package. It can be used by, e.g., linking it against a main function 
-from the examples folder. The make also compiles a couple of test examples; 
-executables are stored within the directory <install-dir>/bin.
-
-
-2. Running a simple test example:
-
-Among others, an executable called  example1  should have been created; run 
-it in order to test your installation:
-
-cd <install-dir>/bin
-./example1
-
-If it terminates after successfully solving two QP problems, qpOASES has been 
-successfully installed!
-
-
-3. Optional, create source code documentation (using doxygen):
-
-cd <install-dir>/doc
-doxygen doxygen.config
-
-Afterwards, you can open the file <install-dir>/doc/html/index.html with
-your favorite browser in order to view qpOASES's source code documentation.
-
-
-NOTE: More detailed installation instructions, including information on how
-      to run unit tests can be found in the qpOASES User's Manual located 
-      at <install-dir>/doc/manual.pdf!
-
-
-
-##
-##	end of file
-##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/INSTALL.txt b/3rdparty/qpOASES/qpOASES-3.0/INSTALL.txt
deleted file mode 100644
index 89eb3abb47d..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/INSTALL.txt
+++ /dev/null
@@ -1,79 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-INSTALLATION UNDER LINUX
-========================
-
-0. Obtain qpOASES from COIN-OR:
-
-Download a zipped archive containg the latest stable release and unpack it 
-into <install-dir>. Alternatively, you check out the latest stable branch, 
-e.g. by running
-
-svn co https://projects.coin-or.org/svn/qpOASES/stable/3.0 <install-dir>
-
-from your shell. 
-
-
-1. Compilation of the qpOASES library libqpOASES.a and test examples:
-
-cd <install-dir>
-make
-
-The library  libqpOASES.a  provides the complete functionality of the qpOASES 
-software package. It can be used by, e.g., linking it against a main function 
-from the examples folder. The make also compiles a couple of test examples; 
-executables are stored within the directory <install-dir>/bin.
-
-
-2. Running a simple test example:
-
-Among others, an executable called  example1  should have been created; run 
-it in order to test your installation:
-
-cd <install-dir>/bin
-./example1
-
-If it terminates after successfully solving two QP problems, qpOASES has been 
-successfully installed!
-
-
-3. Optional, create source code documentation (using doxygen):
-
-cd <install-dir>/doc
-doxygen doxygen.config
-
-Afterwards, you can open the file <install-dir>/doc/html/index.html with
-your favorite browser in order to view qpOASES's source code documentation.
-
-
-NOTE: More detailed installation instructions, including information on how
-      to run unit tests can be found in the qpOASES User's Manual located 
-      at <install-dir>/doc/manual.pdf!
-
-
-
-##
-##	end of file
-##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/LICENSE b/3rdparty/qpOASES/qpOASES-3.0/LICENSE
deleted file mode 100644
index 9ef3d701d14..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/LICENSE
+++ /dev/null
@@ -1,503 +0,0 @@
-		  GNU LESSER GENERAL PUBLIC LICENSE
-		       Version 2.1, February 1999
-
- Copyright (C) 1991, 1999 Free Software Foundation, Inc.
- 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- Everyone is permitted to copy and distribute verbatim copies
- of this license document, but changing it is not allowed.
-
-[This is the first released version of the Lesser GPL.  It also counts
- as the successor of the GNU Library Public License, version 2, hence
- the version number 2.1.]
-
-			    Preamble
-
-  The licenses for most software are designed to take away your
-freedom to share and change it.  By contrast, the GNU General Public
-Licenses are intended to guarantee your freedom to share and change
-free software--to make sure the software is free for all its users.
-
-  This license, the Lesser General Public License, applies to some
-specially designated software packages--typically libraries--of the
-Free Software Foundation and other authors who decide to use it.  You
-can use it too, but we suggest you first think carefully about whether
-this license or the ordinary General Public License is the better
-strategy to use in any particular case, based on the explanations below.
-
-  When we speak of free software, we are referring to freedom of use,
-not price.  Our General Public Licenses are designed to make sure that
-you have the freedom to distribute copies of free software (and charge
-for this service if you wish); that you receive source code or can get
-it if you want it; that you can change the software and use pieces of
-it in new free programs; and that you are informed that you can do
-these things.
-
-  To protect your rights, we need to make restrictions that forbid
-distributors to deny you these rights or to ask you to surrender these
-rights.  These restrictions translate to certain responsibilities for
-you if you distribute copies of the library or if you modify it.
-
-  For example, if you distribute copies of the library, whether gratis
-or for a fee, you must give the recipients all the rights that we gave
-you.  You must make sure that they, too, receive or can get the source
-code.  If you link other code with the library, you must provide
-complete object files to the recipients, so that they can relink them
-with the library after making changes to the library and recompiling
-it.  And you must show them these terms so they know their rights.
-
-  We protect your rights with a two-step method: (1) we copyright the
-library, and (2) we offer you this license, which gives you legal
-permission to copy, distribute and/or modify the library.
-
-  To protect each distributor, we want to make it very clear that
-there is no warranty for the free library.  Also, if the library is
-modified by someone else and passed on, the recipients should know
-that what they have is not the original version, so that the original
-author's reputation will not be affected by problems that might be
-introduced by others.
-
-  Finally, software patents pose a constant threat to the existence of
-any free program.  We wish to make sure that a company cannot
-effectively restrict the users of a free program by obtaining a
-restrictive license from a patent holder.  Therefore, we insist that
-any patent license obtained for a version of the library must be
-consistent with the full freedom of use specified in this license.
-
-  Most GNU software, including some libraries, is covered by the
-ordinary GNU General Public License.  This license, the GNU Lesser
-General Public License, applies to certain designated libraries, and
-is quite different from the ordinary General Public License.  We use
-this license for certain libraries in order to permit linking those
-libraries into non-free programs.
-
-  When a program is linked with a library, whether statically or using
-a shared library, the combination of the two is legally speaking a
-combined work, a derivative of the original library.  The ordinary
-General Public License therefore permits such linking only if the
-entire combination fits its criteria of freedom.  The Lesser General
-Public License permits more lax criteria for linking other code with
-the library.
-
-  We call this license the "Lesser" General Public License because it
-does Less to protect the user's freedom than the ordinary General
-Public License.  It also provides other free software developers Less
-of an advantage over competing non-free programs.  These disadvantages
-are the reason we use the ordinary General Public License for many
-libraries.  However, the Lesser license provides advantages in certain
-special circumstances.
-
-  For example, on rare occasions, there may be a special need to
-encourage the widest possible use of a certain library, so that it becomes
-a de-facto standard.  To achieve this, non-free programs must be
-allowed to use the library.  A more frequent case is that a free
-library does the same job as widely used non-free libraries.  In this
-case, there is little to gain by limiting the free library to free
-software only, so we use the Lesser General Public License.
-
-  In other cases, permission to use a particular library in non-free
-programs enables a greater number of people to use a large body of
-free software.  For example, permission to use the GNU C Library in
-non-free programs enables many more people to use the whole GNU
-operating system, as well as its variant, the GNU/Linux operating
-system.
-
-  Although the Lesser General Public License is Less protective of the
-users' freedom, it does ensure that the user of a program that is
-linked with the Library has the freedom and the wherewithal to run
-that program using a modified version of the Library.
-
-  The precise terms and conditions for copying, distribution and
-modification follow.  Pay close attention to the difference between a
-"work based on the library" and a "work that uses the library".  The
-former contains code derived from the library, whereas the latter must
-be combined with the library in order to run.
-
-		  GNU LESSER GENERAL PUBLIC LICENSE
-   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
-
-  0. This License Agreement applies to any software library or other
-program which contains a notice placed by the copyright holder or
-other authorized party saying it may be distributed under the terms of
-this Lesser General Public License (also called "this License").
-Each licensee is addressed as "you".
-
-  A "library" means a collection of software functions and/or data
-prepared so as to be conveniently linked with application programs
-(which use some of those functions and data) to form executables.
-
-  The "Library", below, refers to any such software library or work
-which has been distributed under these terms.  A "work based on the
-Library" means either the Library or any derivative work under
-copyright law: that is to say, a work containing the Library or a
-portion of it, either verbatim or with modifications and/or translated
-straightforwardly into another language.  (Hereinafter, translation is
-included without limitation in the term "modification".)
-
-  "Source code" for a work means the preferred form of the work for
-making modifications to it.  For a library, complete source code means
-all the source code for all modules it contains, plus any associated
-interface definition files, plus the scripts used to control compilation
-and installation of the library.
-
-  Activities other than copying, distribution and modification are not
-covered by this License; they are outside its scope.  The act of
-running a program using the Library is not restricted, and output from
-such a program is covered only if its contents constitute a work based
-on the Library (independent of the use of the Library in a tool for
-writing it).  Whether that is true depends on what the Library does
-and what the program that uses the Library does.
-  
-  1. You may copy and distribute verbatim copies of the Library's
-complete source code as you receive it, in any medium, provided that
-you conspicuously and appropriately publish on each copy an
-appropriate copyright notice and disclaimer of warranty; keep intact
-all the notices that refer to this License and to the absence of any
-warranty; and distribute a copy of this License along with the
-Library.
-
-  You may charge a fee for the physical act of transferring a copy,
-and you may at your option offer warranty protection in exchange for a
-fee.
-
-  2. You may modify your copy or copies of the Library or any portion
-of it, thus forming a work based on the Library, and copy and
-distribute such modifications or work under the terms of Section 1
-above, provided that you also meet all of these conditions:
-
-    a) The modified work must itself be a software library.
-
-    b) You must cause the files modified to carry prominent notices
-    stating that you changed the files and the date of any change.
-
-    c) You must cause the whole of the work to be licensed at no
-    charge to all third parties under the terms of this License.
-
-    d) If a facility in the modified Library refers to a function or a
-    table of data to be supplied by an application program that uses
-    the facility, other than as an argument passed when the facility
-    is invoked, then you must make a good faith effort to ensure that,
-    in the event an application does not supply such function or
-    table, the facility still operates, and performs whatever part of
-    its purpose remains meaningful.
-
-    (For example, a function in a library to compute square roots has
-    a purpose that is entirely well-defined independent of the
-    application.  Therefore, Subsection 2d requires that any
-    application-supplied function or table used by this function must
-    be optional: if the application does not supply it, the square
-    root function must still compute square roots.)
-
-These requirements apply to the modified work as a whole.  If
-identifiable sections of that work are not derived from the Library,
-and can be reasonably considered independent and separate works in
-themselves, then this License, and its terms, do not apply to those
-sections when you distribute them as separate works.  But when you
-distribute the same sections as part of a whole which is a work based
-on the Library, the distribution of the whole must be on the terms of
-this License, whose permissions for other licensees extend to the
-entire whole, and thus to each and every part regardless of who wrote
-it.
-
-Thus, it is not the intent of this section to claim rights or contest
-your rights to work written entirely by you; rather, the intent is to
-exercise the right to control the distribution of derivative or
-collective works based on the Library.
-
-In addition, mere aggregation of another work not based on the Library
-with the Library (or with a work based on the Library) on a volume of
-a storage or distribution medium does not bring the other work under
-the scope of this License.
-
-  3. You may opt to apply the terms of the ordinary GNU General Public
-License instead of this License to a given copy of the Library.  To do
-this, you must alter all the notices that refer to this License, so
-that they refer to the ordinary GNU General Public License, version 2,
-instead of to this License.  (If a newer version than version 2 of the
-ordinary GNU General Public License has appeared, then you can specify
-that version instead if you wish.)  Do not make any other change in
-these notices.
-
-  Once this change is made in a given copy, it is irreversible for
-that copy, so the ordinary GNU General Public License applies to all
-subsequent copies and derivative works made from that copy.
-
-  This option is useful when you wish to copy part of the code of
-the Library into a program that is not a library.
-
-  4. You may copy and distribute the Library (or a portion or
-derivative of it, under Section 2) in object code or executable form
-under the terms of Sections 1 and 2 above provided that you accompany
-it with the complete corresponding machine-readable source code, which
-must be distributed under the terms of Sections 1 and 2 above on a
-medium customarily used for software interchange.
-
-  If distribution of object code is made by offering access to copy
-from a designated place, then offering equivalent access to copy the
-source code from the same place satisfies the requirement to
-distribute the source code, even though third parties are not
-compelled to copy the source along with the object code.
-
-  5. A program that contains no derivative of any portion of the
-Library, but is designed to work with the Library by being compiled or
-linked with it, is called a "work that uses the Library".  Such a
-work, in isolation, is not a derivative work of the Library, and
-therefore falls outside the scope of this License.
-
-  However, linking a "work that uses the Library" with the Library
-creates an executable that is a derivative of the Library (because it
-contains portions of the Library), rather than a "work that uses the
-library".  The executable is therefore covered by this License.
-Section 6 states terms for distribution of such executables.
-
-  When a "work that uses the Library" uses material from a header file
-that is part of the Library, the object code for the work may be a
-derivative work of the Library even though the source code is not.
-Whether this is true is especially significant if the work can be
-linked without the Library, or if the work is itself a library.  The
-threshold for this to be true is not precisely defined by law.
-
-  If such an object file uses only numerical parameters, data
-structure layouts and accessors, and small macros and small inline
-functions (ten lines or less in length), then the use of the object
-file is unrestricted, regardless of whether it is legally a derivative
-work.  (Executables containing this object code plus portions of the
-Library will still fall under Section 6.)
-
-  Otherwise, if the work is a derivative of the Library, you may
-distribute the object code for the work under the terms of Section 6.
-Any executables containing that work also fall under Section 6,
-whether or not they are linked directly with the Library itself.
-
-  6. As an exception to the Sections above, you may also combine or
-link a "work that uses the Library" with the Library to produce a
-work containing portions of the Library, and distribute that work
-under terms of your choice, provided that the terms permit
-modification of the work for the customer's own use and reverse
-engineering for debugging such modifications.
-
-  You must give prominent notice with each copy of the work that the
-Library is used in it and that the Library and its use are covered by
-this License.  You must supply a copy of this License.  If the work
-during execution displays copyright notices, you must include the
-copyright notice for the Library among them, as well as a reference
-directing the user to the copy of this License.  Also, you must do one
-of these things:
-
-    a) Accompany the work with the complete corresponding
-    machine-readable source code for the Library including whatever
-    changes were used in the work (which must be distributed under
-    Sections 1 and 2 above); and, if the work is an executable linked
-    with the Library, with the complete machine-readable "work that
-    uses the Library", as object code and/or source code, so that the
-    user can modify the Library and then relink to produce a modified
-    executable containing the modified Library.  (It is understood
-    that the user who changes the contents of definitions files in the
-    Library will not necessarily be able to recompile the application
-    to use the modified definitions.)
-
-    b) Use a suitable shared library mechanism for linking with the
-    Library.  A suitable mechanism is one that (1) uses at run time a
-    copy of the library already present on the user's computer system,
-    rather than copying library functions into the executable, and (2)
-    will operate properly with a modified version of the library, if
-    the user installs one, as long as the modified version is
-    interface-compatible with the version that the work was made with.
-
-    c) Accompany the work with a written offer, valid for at
-    least three years, to give the same user the materials
-    specified in Subsection 6a, above, for a charge no more
-    than the cost of performing this distribution.
-
-    d) If distribution of the work is made by offering access to copy
-    from a designated place, offer equivalent access to copy the above
-    specified materials from the same place.
-
-    e) Verify that the user has already received a copy of these
-    materials or that you have already sent this user a copy.
-
-  For an executable, the required form of the "work that uses the
-Library" must include any data and utility programs needed for
-reproducing the executable from it.  However, as a special exception,
-the materials to be distributed need not include anything that is
-normally distributed (in either source or binary form) with the major
-components (compiler, kernel, and so on) of the operating system on
-which the executable runs, unless that component itself accompanies
-the executable.
-
-  It may happen that this requirement contradicts the license
-restrictions of other proprietary libraries that do not normally
-accompany the operating system.  Such a contradiction means you cannot
-use both them and the Library together in an executable that you
-distribute.
-
-  7. You may place library facilities that are a work based on the
-Library side-by-side in a single library together with other library
-facilities not covered by this License, and distribute such a combined
-library, provided that the separate distribution of the work based on
-the Library and of the other library facilities is otherwise
-permitted, and provided that you do these two things:
-
-    a) Accompany the combined library with a copy of the same work
-    based on the Library, uncombined with any other library
-    facilities.  This must be distributed under the terms of the
-    Sections above.
-
-    b) Give prominent notice with the combined library of the fact
-    that part of it is a work based on the Library, and explaining
-    where to find the accompanying uncombined form of the same work.
-
-  8. You may not copy, modify, sublicense, link with, or distribute
-the Library except as expressly provided under this License.  Any
-attempt otherwise to copy, modify, sublicense, link with, or
-distribute the Library is void, and will automatically terminate your
-rights under this License.  However, parties who have received copies,
-or rights, from you under this License will not have their licenses
-terminated so long as such parties remain in full compliance.
-
-  9. You are not required to accept this License, since you have not
-signed it.  However, nothing else grants you permission to modify or
-distribute the Library or its derivative works.  These actions are
-prohibited by law if you do not accept this License.  Therefore, by
-modifying or distributing the Library (or any work based on the
-Library), you indicate your acceptance of this License to do so, and
-all its terms and conditions for copying, distributing or modifying
-the Library or works based on it.
-
-  10. Each time you redistribute the Library (or any work based on the
-Library), the recipient automatically receives a license from the
-original licensor to copy, distribute, link with or modify the Library
-subject to these terms and conditions.  You may not impose any further
-restrictions on the recipients' exercise of the rights granted herein.
-You are not responsible for enforcing compliance by third parties with
-this License.
-
-  11. If, as a consequence of a court judgment or allegation of patent
-infringement or for any other reason (not limited to patent issues),
-conditions are imposed on you (whether by court order, agreement or
-otherwise) that contradict the conditions of this License, they do not
-excuse you from the conditions of this License.  If you cannot
-distribute so as to satisfy simultaneously your obligations under this
-License and any other pertinent obligations, then as a consequence you
-may not distribute the Library at all.  For example, if a patent
-license would not permit royalty-free redistribution of the Library by
-all those who receive copies directly or indirectly through you, then
-the only way you could satisfy both it and this License would be to
-refrain entirely from distribution of the Library.
-
-If any portion of this section is held invalid or unenforceable under any
-particular circumstance, the balance of the section is intended to apply,
-and the section as a whole is intended to apply in other circumstances.
-
-It is not the purpose of this section to induce you to infringe any
-patents or other property right claims or to contest validity of any
-such claims; this section has the sole purpose of protecting the
-integrity of the free software distribution system which is
-implemented by public license practices.  Many people have made
-generous contributions to the wide range of software distributed
-through that system in reliance on consistent application of that
-system; it is up to the author/donor to decide if he or she is willing
-to distribute software through any other system and a licensee cannot
-impose that choice.
-
-This section is intended to make thoroughly clear what is believed to
-be a consequence of the rest of this License.
-
-  12. If the distribution and/or use of the Library is restricted in
-certain countries either by patents or by copyrighted interfaces, the
-original copyright holder who places the Library under this License may add
-an explicit geographical distribution limitation excluding those countries,
-so that distribution is permitted only in or among countries not thus
-excluded.  In such case, this License incorporates the limitation as if
-written in the body of this License.
-
-  13. The Free Software Foundation may publish revised and/or new
-versions of the Lesser General Public License from time to time.
-Such new versions will be similar in spirit to the present version,
-but may differ in detail to address new problems or concerns.
-
-Each version is given a distinguishing version number.  If the Library
-specifies a version number of this License which applies to it and
-"any later version", you have the option of following the terms and
-conditions either of that version or of any later version published by
-the Free Software Foundation.  If the Library does not specify a
-license version number, you may choose any version ever published by
-the Free Software Foundation.
-
-  14. If you wish to incorporate parts of the Library into other free
-programs whose distribution conditions are incompatible with these,
-write to the author to ask for permission.  For software which is
-copyrighted by the Free Software Foundation, write to the Free
-Software Foundation; we sometimes make exceptions for this.  Our
-decision will be guided by the two goals of preserving the free status
-of all derivatives of our free software and of promoting the sharing
-and reuse of software generally.
-
-			    NO WARRANTY
-
-  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
-WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
-EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
-OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
-KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
-IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
-PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
-LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
-THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
-
-  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
-WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
-AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
-FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
-CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
-LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
-RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
-FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
-SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
-DAMAGES.
-
-		     END OF TERMS AND CONDITIONS
-
-           How to Apply These Terms to Your New Libraries
-
-  If you develop a new library, and you want it to be of the greatest
-possible use to the public, we recommend making it free software that
-everyone can redistribute and change.  You can do so by permitting
-redistribution under these terms (or, alternatively, under the terms of the
-ordinary General Public License).
-
-  To apply these terms, attach the following notices to the library.  It is
-safest to attach them to the start of each source file to most effectively
-convey the exclusion of warranty; and each file should have at least the
-"copyright" line and a pointer to where the full notice is found.
-
-    <one line to give the library's name and a brief idea of what it does.>
-    Copyright (C) <year>  <name of author>
-
-    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 Street, Fifth Floor, Boston, MA  02110-1301  USA
-
-Also add information on how to contact you by electronic and paper mail.
-
-You should also get your employer (if you work as a programmer) or your
-school, if any, to sign a "copyright disclaimer" for the library, if
-necessary.  Here is a sample; alter the names:
-
-  Yoyodyne, Inc., hereby disclaims all copyright interest in the
-  library `Frob' (a library for tweaking knobs) written by James Random Hacker.
-
-  <signature of Ty Coon>, 1 April 1990
-  Ty Coon, President of Vice
-
-That's all there is to it!
-
diff --git a/3rdparty/qpOASES/qpOASES-3.0/LICENSE.txt b/3rdparty/qpOASES/qpOASES-3.0/LICENSE.txt
deleted file mode 100644
index 2d59e84cd83..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/LICENSE.txt
+++ /dev/null
@@ -1,503 +0,0 @@
-		  GNU LESSER GENERAL PUBLIC LICENSE
-		       Version 2.1, February 1999
-
- Copyright (C) 1991, 1999 Free Software Foundation, Inc.
- 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- Everyone is permitted to copy and distribute verbatim copies
- of this license document, but changing it is not allowed.
-
-[This is the first released version of the Lesser GPL.  It also counts
- as the successor of the GNU Library Public License, version 2, hence
- the version number 2.1.]
-
-			    Preamble
-
-  The licenses for most software are designed to take away your
-freedom to share and change it.  By contrast, the GNU General Public
-Licenses are intended to guarantee your freedom to share and change
-free software--to make sure the software is free for all its users.
-
-  This license, the Lesser General Public License, applies to some
-specially designated software packages--typically libraries--of the
-Free Software Foundation and other authors who decide to use it.  You
-can use it too, but we suggest you first think carefully about whether
-this license or the ordinary General Public License is the better
-strategy to use in any particular case, based on the explanations below.
-
-  When we speak of free software, we are referring to freedom of use,
-not price.  Our General Public Licenses are designed to make sure that
-you have the freedom to distribute copies of free software (and charge
-for this service if you wish); that you receive source code or can get
-it if you want it; that you can change the software and use pieces of
-it in new free programs; and that you are informed that you can do
-these things.
-
-  To protect your rights, we need to make restrictions that forbid
-distributors to deny you these rights or to ask you to surrender these
-rights.  These restrictions translate to certain responsibilities for
-you if you distribute copies of the library or if you modify it.
-
-  For example, if you distribute copies of the library, whether gratis
-or for a fee, you must give the recipients all the rights that we gave
-you.  You must make sure that they, too, receive or can get the source
-code.  If you link other code with the library, you must provide
-complete object files to the recipients, so that they can relink them
-with the library after making changes to the library and recompiling
-it.  And you must show them these terms so they know their rights.
-
-  We protect your rights with a two-step method: (1) we copyright the
-library, and (2) we offer you this license, which gives you legal
-permission to copy, distribute and/or modify the library.
-
-  To protect each distributor, we want to make it very clear that
-there is no warranty for the free library.  Also, if the library is
-modified by someone else and passed on, the recipients should know
-that what they have is not the original version, so that the original
-author's reputation will not be affected by problems that might be
-introduced by others.
-
-  Finally, software patents pose a constant threat to the existence of
-any free program.  We wish to make sure that a company cannot
-effectively restrict the users of a free program by obtaining a
-restrictive license from a patent holder.  Therefore, we insist that
-any patent license obtained for a version of the library must be
-consistent with the full freedom of use specified in this license.
-
-  Most GNU software, including some libraries, is covered by the
-ordinary GNU General Public License.  This license, the GNU Lesser
-General Public License, applies to certain designated libraries, and
-is quite different from the ordinary General Public License.  We use
-this license for certain libraries in order to permit linking those
-libraries into non-free programs.
-
-  When a program is linked with a library, whether statically or using
-a shared library, the combination of the two is legally speaking a
-combined work, a derivative of the original library.  The ordinary
-General Public License therefore permits such linking only if the
-entire combination fits its criteria of freedom.  The Lesser General
-Public License permits more lax criteria for linking other code with
-the library.
-
-  We call this license the "Lesser" General Public License because it
-does Less to protect the user's freedom than the ordinary General
-Public License.  It also provides other free software developers Less
-of an advantage over competing non-free programs.  These disadvantages
-are the reason we use the ordinary General Public License for many
-libraries.  However, the Lesser license provides advantages in certain
-special circumstances.
-
-  For example, on rare occasions, there may be a special need to
-encourage the widest possible use of a certain library, so that it becomes
-a de-facto standard.  To achieve this, non-free programs must be
-allowed to use the library.  A more frequent case is that a free
-library does the same job as widely used non-free libraries.  In this
-case, there is little to gain by limiting the free library to free
-software only, so we use the Lesser General Public License.
-
-  In other cases, permission to use a particular library in non-free
-programs enables a greater number of people to use a large body of
-free software.  For example, permission to use the GNU C Library in
-non-free programs enables many more people to use the whole GNU
-operating system, as well as its variant, the GNU/Linux operating
-system.
-
-  Although the Lesser General Public License is Less protective of the
-users' freedom, it does ensure that the user of a program that is
-linked with the Library has the freedom and the wherewithal to run
-that program using a modified version of the Library.
-
-  The precise terms and conditions for copying, distribution and
-modification follow.  Pay close attention to the difference between a
-"work based on the library" and a "work that uses the library".  The
-former contains code derived from the library, whereas the latter must
-be combined with the library in order to run.
-
-		  GNU LESSER GENERAL PUBLIC LICENSE
-   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
-
-  0. This License Agreement applies to any software library or other
-program which contains a notice placed by the copyright holder or
-other authorized party saying it may be distributed under the terms of
-this Lesser General Public License (also called "this License").
-Each licensee is addressed as "you".
-
-  A "library" means a collection of software functions and/or data
-prepared so as to be conveniently linked with application programs
-(which use some of those functions and data) to form executables.
-
-  The "Library", below, refers to any such software library or work
-which has been distributed under these terms.  A "work based on the
-Library" means either the Library or any derivative work under
-copyright law: that is to say, a work containing the Library or a
-portion of it, either verbatim or with modifications and/or translated
-straightforwardly into another language.  (Hereinafter, translation is
-included without limitation in the term "modification".)
-
-  "Source code" for a work means the preferred form of the work for
-making modifications to it.  For a library, complete source code means
-all the source code for all modules it contains, plus any associated
-interface definition files, plus the scripts used to control compilation
-and installation of the library.
-
-  Activities other than copying, distribution and modification are not
-covered by this License; they are outside its scope.  The act of
-running a program using the Library is not restricted, and output from
-such a program is covered only if its contents constitute a work based
-on the Library (independent of the use of the Library in a tool for
-writing it).  Whether that is true depends on what the Library does
-and what the program that uses the Library does.
-  
-  1. You may copy and distribute verbatim copies of the Library's
-complete source code as you receive it, in any medium, provided that
-you conspicuously and appropriately publish on each copy an
-appropriate copyright notice and disclaimer of warranty; keep intact
-all the notices that refer to this License and to the absence of any
-warranty; and distribute a copy of this License along with the
-Library.
-
-  You may charge a fee for the physical act of transferring a copy,
-and you may at your option offer warranty protection in exchange for a
-fee.
-
-  2. You may modify your copy or copies of the Library or any portion
-of it, thus forming a work based on the Library, and copy and
-distribute such modifications or work under the terms of Section 1
-above, provided that you also meet all of these conditions:
-
-    a) The modified work must itself be a software library.
-
-    b) You must cause the files modified to carry prominent notices
-    stating that you changed the files and the date of any change.
-
-    c) You must cause the whole of the work to be licensed at no
-    charge to all third parties under the terms of this License.
-
-    d) If a facility in the modified Library refers to a function or a
-    table of data to be supplied by an application program that uses
-    the facility, other than as an argument passed when the facility
-    is invoked, then you must make a good faith effort to ensure that,
-    in the event an application does not supply such function or
-    table, the facility still operates, and performs whatever part of
-    its purpose remains meaningful.
-
-    (For example, a function in a library to compute square roots has
-    a purpose that is entirely well-defined independent of the
-    application.  Therefore, Subsection 2d requires that any
-    application-supplied function or table used by this function must
-    be optional: if the application does not supply it, the square
-    root function must still compute square roots.)
-
-These requirements apply to the modified work as a whole.  If
-identifiable sections of that work are not derived from the Library,
-and can be reasonably considered independent and separate works in
-themselves, then this License, and its terms, do not apply to those
-sections when you distribute them as separate works.  But when you
-distribute the same sections as part of a whole which is a work based
-on the Library, the distribution of the whole must be on the terms of
-this License, whose permissions for other licensees extend to the
-entire whole, and thus to each and every part regardless of who wrote
-it.
-
-Thus, it is not the intent of this section to claim rights or contest
-your rights to work written entirely by you; rather, the intent is to
-exercise the right to control the distribution of derivative or
-collective works based on the Library.
-
-In addition, mere aggregation of another work not based on the Library
-with the Library (or with a work based on the Library) on a volume of
-a storage or distribution medium does not bring the other work under
-the scope of this License.
-
-  3. You may opt to apply the terms of the ordinary GNU General Public
-License instead of this License to a given copy of the Library.  To do
-this, you must alter all the notices that refer to this License, so
-that they refer to the ordinary GNU General Public License, version 2,
-instead of to this License.  (If a newer version than version 2 of the
-ordinary GNU General Public License has appeared, then you can specify
-that version instead if you wish.)  Do not make any other change in
-these notices.
-
-  Once this change is made in a given copy, it is irreversible for
-that copy, so the ordinary GNU General Public License applies to all
-subsequent copies and derivative works made from that copy.
-
-  This option is useful when you wish to copy part of the code of
-the Library into a program that is not a library.
-
-  4. You may copy and distribute the Library (or a portion or
-derivative of it, under Section 2) in object code or executable form
-under the terms of Sections 1 and 2 above provided that you accompany
-it with the complete corresponding machine-readable source code, which
-must be distributed under the terms of Sections 1 and 2 above on a
-medium customarily used for software interchange.
-
-  If distribution of object code is made by offering access to copy
-from a designated place, then offering equivalent access to copy the
-source code from the same place satisfies the requirement to
-distribute the source code, even though third parties are not
-compelled to copy the source along with the object code.
-
-  5. A program that contains no derivative of any portion of the
-Library, but is designed to work with the Library by being compiled or
-linked with it, is called a "work that uses the Library".  Such a
-work, in isolation, is not a derivative work of the Library, and
-therefore falls outside the scope of this License.
-
-  However, linking a "work that uses the Library" with the Library
-creates an executable that is a derivative of the Library (because it
-contains portions of the Library), rather than a "work that uses the
-library".  The executable is therefore covered by this License.
-Section 6 states terms for distribution of such executables.
-
-  When a "work that uses the Library" uses material from a header file
-that is part of the Library, the object code for the work may be a
-derivative work of the Library even though the source code is not.
-Whether this is true is especially significant if the work can be
-linked without the Library, or if the work is itself a library.  The
-threshold for this to be true is not precisely defined by law.
-
-  If such an object file uses only numerical parameters, data
-structure layouts and accessors, and small macros and small inline
-functions (ten lines or less in length), then the use of the object
-file is unrestricted, regardless of whether it is legally a derivative
-work.  (Executables containing this object code plus portions of the
-Library will still fall under Section 6.)
-
-  Otherwise, if the work is a derivative of the Library, you may
-distribute the object code for the work under the terms of Section 6.
-Any executables containing that work also fall under Section 6,
-whether or not they are linked directly with the Library itself.
-
-  6. As an exception to the Sections above, you may also combine or
-link a "work that uses the Library" with the Library to produce a
-work containing portions of the Library, and distribute that work
-under terms of your choice, provided that the terms permit
-modification of the work for the customer's own use and reverse
-engineering for debugging such modifications.
-
-  You must give prominent notice with each copy of the work that the
-Library is used in it and that the Library and its use are covered by
-this License.  You must supply a copy of this License.  If the work
-during execution displays copyright notices, you must include the
-copyright notice for the Library among them, as well as a reference
-directing the user to the copy of this License.  Also, you must do one
-of these things:
-
-    a) Accompany the work with the complete corresponding
-    machine-readable source code for the Library including whatever
-    changes were used in the work (which must be distributed under
-    Sections 1 and 2 above); and, if the work is an executable linked
-    with the Library, with the complete machine-readable "work that
-    uses the Library", as object code and/or source code, so that the
-    user can modify the Library and then relink to produce a modified
-    executable containing the modified Library.  (It is understood
-    that the user who changes the contents of definitions files in the
-    Library will not necessarily be able to recompile the application
-    to use the modified definitions.)
-
-    b) Use a suitable shared library mechanism for linking with the
-    Library.  A suitable mechanism is one that (1) uses at run time a
-    copy of the library already present on the user's computer system,
-    rather than copying library functions into the executable, and (2)
-    will operate properly with a modified version of the library, if
-    the user installs one, as long as the modified version is
-    interface-compatible with the version that the work was made with.
-
-    c) Accompany the work with a written offer, valid for at
-    least three years, to give the same user the materials
-    specified in Subsection 6a, above, for a charge no more
-    than the cost of performing this distribution.
-
-    d) If distribution of the work is made by offering access to copy
-    from a designated place, offer equivalent access to copy the above
-    specified materials from the same place.
-
-    e) Verify that the user has already received a copy of these
-    materials or that you have already sent this user a copy.
-
-  For an executable, the required form of the "work that uses the
-Library" must include any data and utility programs needed for
-reproducing the executable from it.  However, as a special exception,
-the materials to be distributed need not include anything that is
-normally distributed (in either source or binary form) with the major
-components (compiler, kernel, and so on) of the operating system on
-which the executable runs, unless that component itself accompanies
-the executable.
-
-  It may happen that this requirement contradicts the license
-restrictions of other proprietary libraries that do not normally
-accompany the operating system.  Such a contradiction means you cannot
-use both them and the Library together in an executable that you
-distribute.
-
-  7. You may place library facilities that are a work based on the
-Library side-by-side in a single library together with other library
-facilities not covered by this License, and distribute such a combined
-library, provided that the separate distribution of the work based on
-the Library and of the other library facilities is otherwise
-permitted, and provided that you do these two things:
-
-    a) Accompany the combined library with a copy of the same work
-    based on the Library, uncombined with any other library
-    facilities.  This must be distributed under the terms of the
-    Sections above.
-
-    b) Give prominent notice with the combined library of the fact
-    that part of it is a work based on the Library, and explaining
-    where to find the accompanying uncombined form of the same work.
-
-  8. You may not copy, modify, sublicense, link with, or distribute
-the Library except as expressly provided under this License.  Any
-attempt otherwise to copy, modify, sublicense, link with, or
-distribute the Library is void, and will automatically terminate your
-rights under this License.  However, parties who have received copies,
-or rights, from you under this License will not have their licenses
-terminated so long as such parties remain in full compliance.
-
-  9. You are not required to accept this License, since you have not
-signed it.  However, nothing else grants you permission to modify or
-distribute the Library or its derivative works.  These actions are
-prohibited by law if you do not accept this License.  Therefore, by
-modifying or distributing the Library (or any work based on the
-Library), you indicate your acceptance of this License to do so, and
-all its terms and conditions for copying, distributing or modifying
-the Library or works based on it.
-
-  10. Each time you redistribute the Library (or any work based on the
-Library), the recipient automatically receives a license from the
-original licensor to copy, distribute, link with or modify the Library
-subject to these terms and conditions.  You may not impose any further
-restrictions on the recipients' exercise of the rights granted herein.
-You are not responsible for enforcing compliance by third parties with
-this License.
-
-  11. If, as a consequence of a court judgment or allegation of patent
-infringement or for any other reason (not limited to patent issues),
-conditions are imposed on you (whether by court order, agreement or
-otherwise) that contradict the conditions of this License, they do not
-excuse you from the conditions of this License.  If you cannot
-distribute so as to satisfy simultaneously your obligations under this
-License and any other pertinent obligations, then as a consequence you
-may not distribute the Library at all.  For example, if a patent
-license would not permit royalty-free redistribution of the Library by
-all those who receive copies directly or indirectly through you, then
-the only way you could satisfy both it and this License would be to
-refrain entirely from distribution of the Library.
-
-If any portion of this section is held invalid or unenforceable under any
-particular circumstance, the balance of the section is intended to apply,
-and the section as a whole is intended to apply in other circumstances.
-
-It is not the purpose of this section to induce you to infringe any
-patents or other property right claims or to contest validity of any
-such claims; this section has the sole purpose of protecting the
-integrity of the free software distribution system which is
-implemented by public license practices.  Many people have made
-generous contributions to the wide range of software distributed
-through that system in reliance on consistent application of that
-system; it is up to the author/donor to decide if he or she is willing
-to distribute software through any other system and a licensee cannot
-impose that choice.
-
-This section is intended to make thoroughly clear what is believed to
-be a consequence of the rest of this License.
-
-  12. If the distribution and/or use of the Library is restricted in
-certain countries either by patents or by copyrighted interfaces, the
-original copyright holder who places the Library under this License may add
-an explicit geographical distribution limitation excluding those countries,
-so that distribution is permitted only in or among countries not thus
-excluded.  In such case, this License incorporates the limitation as if
-written in the body of this License.
-
-  13. The Free Software Foundation may publish revised and/or new
-versions of the Lesser General Public License from time to time.
-Such new versions will be similar in spirit to the present version,
-but may differ in detail to address new problems or concerns.
-
-Each version is given a distinguishing version number.  If the Library
-specifies a version number of this License which applies to it and
-"any later version", you have the option of following the terms and
-conditions either of that version or of any later version published by
-the Free Software Foundation.  If the Library does not specify a
-license version number, you may choose any version ever published by
-the Free Software Foundation.
-
-  14. If you wish to incorporate parts of the Library into other free
-programs whose distribution conditions are incompatible with these,
-write to the author to ask for permission.  For software which is
-copyrighted by the Free Software Foundation, write to the Free
-Software Foundation; we sometimes make exceptions for this.  Our
-decision will be guided by the two goals of preserving the free status
-of all derivatives of our free software and of promoting the sharing
-and reuse of software generally.
-
-			    NO WARRANTY
-
-  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
-WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
-EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
-OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
-KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
-IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
-PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
-LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
-THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
-
-  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
-WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
-AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
-FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
-CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
-LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
-RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
-FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
-SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
-DAMAGES.
-
-		     END OF TERMS AND CONDITIONS
-
-           How to Apply These Terms to Your New Libraries
-
-  If you develop a new library, and you want it to be of the greatest
-possible use to the public, we recommend making it free software that
-everyone can redistribute and change.  You can do so by permitting
-redistribution under these terms (or, alternatively, under the terms of the
-ordinary General Public License).
-
-  To apply these terms, attach the following notices to the library.  It is
-safest to attach them to the start of each source file to most effectively
-convey the exclusion of warranty; and each file should have at least the
-"copyright" line and a pointer to where the full notice is found.
-
-    <one line to give the library's name and a brief idea of what it does.>
-    Copyright (C) <year>  <name of author>
-
-    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 Street, Fifth Floor, Boston, MA  02110-1301  USA
-
-Also add information on how to contact you by electronic and paper mail.
-
-You should also get your employer (if you work as a programmer) or your
-school, if any, to sign a "copyright disclaimer" for the library, if
-necessary.  Here is a sample; alter the names:
-
-  Yoyodyne, Inc., hereby disclaims all copyright interest in the
-  library `Frob' (a library for tweaking knobs) written by James Random Hacker.
-
-  <signature of Ty Coon>, 1 April 1990
-  Ty Coon, President of Vice
-
-That's all there is to it!
-
diff --git a/3rdparty/qpOASES/qpOASES-3.0/README b/3rdparty/qpOASES/qpOASES-3.0/README
deleted file mode 100644
index 2147a72fb67..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/README
+++ /dev/null
@@ -1,84 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-INTRODUCTION
-============
-
-qpOASES is an open-source C++ implementation of the recently proposed 
-online active set strategy, which was inspired by important observations 
-from the field of parametric quadratic programming (QP). It has several 
-theoretical features that make it particularly suited for model predictive 
-control (MPC) applications. Further numerical modifications have made 
-qpOASES a reliable QP solver, even when tackling semi-definite, ill-posed or 
-degenerated QP problems. Moreover, several interfaces to third-party software 
-like ​Matlab or ​Simulink are provided that make qpOASES easy-to-use even for 
-users without knowledge of C/C++.
-
-
-
-GETTING STARTED
-===============
-
-1. For installation, usage and additional information on this software package 
-   see the qpOASES User's Manual located at doc/manual.pdf or check its
-   source code documentation!
-
-
-2. The file LICENSE.txt contains a copy of the GNU Lesser General Public 
-   License (v2.1). Please read it carefully before using qpOASES!
-
-
-3. The whole software package can be obtained from 
-
-       http://www.qpOASES.org/ or
-	   https://projects.coin-or.org/qpOASES/
-
-   On this webpage you will also find further support such as a list of 
-   questions posed by other users.
-
-
-
-CONTACT THE AUTHORS
-===================
-
-If you have got questions, remarks or comments on qpOASES, it is strongly 
-encouraged to report them by creating a new ticket at the qpOASES webpage.
-In case you do not want to disclose your feedback to the public, you may
-send an e-mail to
-
-        support@qpOASES.org
-
-Finally, you may contact one of the main authors directly:
-
-        Hans Joachim Ferreau, joachim.ferreau@ch.abb.com
-        Andreas Potschka,     potschka@iwr.uni-heidelberg.de
-        Christian Kirches,    christian.kirches@iwr.uni-heidelberg.de
-
-Also bug reports, source code enhancements or success stories are most welcome!
-
-
-
-##
-##	end of file
-##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/README.txt b/3rdparty/qpOASES/qpOASES-3.0/README.txt
deleted file mode 100644
index 561939fa534..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/README.txt
+++ /dev/null
@@ -1,84 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-INTRODUCTION
-============
-
-qpOASES is an open-source C++ implementation of the recently proposed 
-online active set strategy, which was inspired by important observations 
-from the field of parametric quadratic programming (QP). It has several 
-theoretical features that make it particularly suited for model predictive 
-control (MPC) applications. Further numerical modifications have made 
-qpOASES a reliable QP solver, even when tackling semi-definite, ill-posed or 
-degenerated QP problems. Moreover, several interfaces to third-party software 
-like ​Matlab or ​Simulink are provided that make qpOASES easy-to-use even for 
-users without knowledge of C/C++.
-
-
-
-GETTING STARTED
-===============
-
-1. For installation, usage and additional information on this software package 
-   see the qpOASES User's Manual located at doc/manual.pdf or check its
-   source code documentation!
-
-
-2. The file LICENSE.txt contains a copy of the GNU Lesser General Public 
-   License (v2.1). Please read it carefully before using qpOASES!
-
-
-3. The whole software package can be obtained from 
-
-       http://www.qpOASES.org/ or
-	   https://projects.coin-or.org/qpOASES/
-
-   On this webpage you will also find further support such as a list of 
-   questions posed by other users.
-
-
-
-CONTACT THE AUTHORS
-===================
-
-If you have got questions, remarks or comments on qpOASES, it is strongly 
-encouraged to report them by creating a new ticket at the qpOASES webpage.
-In case you do not want to disclose your feedback to the public, you may
-send an e-mail to
-
-        support@qpOASES.org
-
-Finally, you may contact one of the main authors directly:
-
-        Hans Joachim Ferreau, joachim.ferreau@ch.abb.com
-        Andreas Potschka,     potschka@iwr.uni-heidelberg.de
-        Christian Kirches,    christian.kirches@iwr.uni-heidelberg.de
-
-Also bug reports, source code enhancements or success stories are most welcome!
-
-
-
-##
-##	end of file
-##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/VERSIONS b/3rdparty/qpOASES/qpOASES-3.0/VERSIONS
deleted file mode 100644
index 4681efc920e..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/VERSIONS
+++ /dev/null
@@ -1,106 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-VERSION HISTORY
-===============
-
-
-3.0 (released on 29th July 2014, last updated on 17th December 2014):
----------------------------------------------------------------------
-
-+ Addition of unit testing
-+ Several bugfixes
-
-
-3.0beta (released on 16th August 2011, last updated on 4th April 2014):
------------------------------------------------------------------------
-
-+ Improved ratio tests and termination check for increased reliabilty
-+ Introduction of iterative refinement in step determination and 
-  drift correction to handle ill-conditioned QPs
-+ Introduction of ramping strategy to handle degenerated QPs
-+ Addition of far bounds and flipping bounds strategy to handle 
-  semi-definite and unbounded QPs more reliably
-+ Limited support of sparse QP matrices (also in Matlab interface)
-+ Optional linking of LAPACK/BLAS for linear algebra operations
-+ Addition of a number of algorithmic options, summarised in an option struct
-+ Improved Matlab interface
-+ Python interface added
-+ Several bugfixes
-
-
-2.0 (released on 10th February 2009, last updated on 7th December 2009):
-------------------------------------------------------------------------
-
-+ Implementation of regularisation scheme for treating QPs with 
-  semi-definite Hessians
-+ Addition of convenience functionality for Bounds and Constraints 
-  objects for specifying guessed active sets
-+ Allows to specify a CPU time in addition to an iteration limit
-+ Improved efficiency for QPs comprising many constraints
-+ Source code cleanup and bugfixing
-
-
-1.3 (released on 2nd June 2008, last updated on 13th August 2008):
-------------------------------------------------------------------
-
-+ Implementation of "initialised homotopy" concept
-+ Addition of the SolutionAnalysis class
-+ Utility functions for solving test problems in OQP format added
-+ Flexibility of Matlab(R) interface enhanced
-+ Major source code cleanup
-  (Attention: a few class names and calling interfaces have changed!)
-
-  
-1.2 (released on 9th October 2007):
------------------------------------
-
-+ Special treatment of diagonal Hessians
-+ Improved infeasibility detection
-+ Further improved Matlab(R) interface
-+ Extended Simulink(R) interface
-+ scilab interface added
-+ Code cleanup and several bugfixes
-
-
-1.1 (released on 8th July 2007):
---------------------------------
-
-+ Implementation of the QProblemB class
-+ Basic implementation of the SQProblem class
-+ Improved Matlab(R) interface
-+ Enabling/Disabling of constraints introduced
-+ Several bugfixes
-
-
-1.0 (released on 17th April 2007):
-----------------------------------
-
-Initial release.
-
-
-
-##
-##	end of file
-##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/VERSIONS.txt b/3rdparty/qpOASES/qpOASES-3.0/VERSIONS.txt
deleted file mode 100644
index 8824a9dd5ef..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/VERSIONS.txt
+++ /dev/null
@@ -1,106 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-VERSION HISTORY
-===============
-
-
-3.0 (released on 29th July 2014, last updated on 17th December 2014):
----------------------------------------------------------------------
-
-+ Addition of unit testing
-+ Several bugfixes
-
-
-3.0beta (released on 16th August 2011, last updated on 4th April 2014):
------------------------------------------------------------------------
-
-+ Improved ratio tests and termination check for increased reliabilty
-+ Introduction of iterative refinement in step determination and 
-  drift correction to handle ill-conditioned QPs
-+ Introduction of ramping strategy to handle degenerated QPs
-+ Addition of far bounds and flipping bounds strategy to handle 
-  semi-definite and unbounded QPs more reliably
-+ Limited support of sparse QP matrices (also in Matlab interface)
-+ Optional linking of LAPACK/BLAS for linear algebra operations
-+ Addition of a number of algorithmic options, summarised in an option struct
-+ Improved Matlab interface
-+ Python interface added
-+ Several bugfixes
-
-
-2.0 (released on 10th February 2009, last updated on 7th December 2009):
-------------------------------------------------------------------------
-
-+ Implementation of regularisation scheme for treating QPs with 
-  semi-definite Hessians
-+ Addition of convenience functionality for Bounds and Constraints 
-  objects for specifying guessed active sets
-+ Allows to specify a CPU time in addition to an iteration limit
-+ Improved efficiency for QPs comprising many constraints
-+ Source code cleanup and bugfixing
-
-
-1.3 (released on 2nd June 2008, last updated on 13th August 2008):
-------------------------------------------------------------------
-
-+ Implementation of "initialised homotopy" concept
-+ Addition of the SolutionAnalysis class
-+ Utility functions for solving test problems in OQP format added
-+ Flexibility of Matlab(R) interface enhanced
-+ Major source code cleanup
-  (Attention: a few class names and calling interfaces have changed!)
-
-  
-1.2 (released on 9th October 2007):
------------------------------------
-
-+ Special treatment of diagonal Hessians
-+ Improved infeasibility detection
-+ Further improved Matlab(R) interface
-+ Extended Simulink(R) interface
-+ scilab interface added
-+ Code cleanup and several bugfixes
-
-
-1.1 (released on 8th July 2007):
---------------------------------
-
-+ Implementation of the QProblemB class
-+ Basic implementation of the SQProblem class
-+ Improved Matlab(R) interface
-+ Enabling/Disabling of constraints introduced
-+ Several bugfixes
-
-
-1.0 (released on 17th April 2007):
-----------------------------------
-
-Initial release.
-
-
-
-##
-##	end of file
-##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/example1 b/3rdparty/qpOASES/qpOASES-3.0/bin/example1
deleted file mode 100755
index 6a0a92bb236a494c7bbbafe9dd0767a1d92c0e03..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 32801
zcmeHwdwg5PmH%AHc4RBIujM2p<bePu8y<efNt}=n$(9_6$PXkr4+7lSmST(8lCh*b
zC=Ds$WlCs5Db2QQ30>M|yJed$v;;~^c(l-N>84O<fOeY&N)rmdh7{WL0r`E;JftgG
zF5Ta+pZ$xQSTpC$nKNh3oS8W@_ujeJc?0bhO=Ie^u!|W%4_3K2O8hc57~`PCwXjm=
zV)NL!tOS^1{JD4vg(9ZHQ<Aj`ZIaH4un2!G7J+E7xOhZ`vjm!{P)SH3m6yF-GAhDL
zrV3Dnj5#@i@{z31%;q4%4LqX46H*SH1ml;J{}P(=9h3Qv$$To*RX!Q2`cd8JFD&t4
ziwh9JE((~3|7Js?rK|f_sR+gQS91`di$_$rOrV(xRlZA+kL3JYFI^G_WqA|nCEGqC
zgKCHRp;+Id=K7&X?NBV9-del0Wl`;-=DK8}u8G&1_!I5Q&K{mo>2W$Bl6eaLNY#`c
zJ>!K}Cmm`HpFH>6SN`MMQ{Fmo<R>0Vqo$;Mlkm4&-(h2CPbs$*6_wvo#N3oN1Ak8Z
zT~*rl;v=u!ac!&jg}2U|ecP<bOFwIG>U-twudRCR(96%h_Jt!?ubp+(b)UQT;a%@k
zOrG-U7v6mSw-wv`o4$pt6J9q%ct?TM=YT7Oe|rLYYDQ-PKIcq;zaJGY#Q*UL@QWtk
z6Py4apFqw(gT7F??g{AkO+f#`1oT4_(0^wFdg@n&<T+yk`a36}zi0yd128Zh7PQ~F
z0g`2}H$bnmGue?m{PP??gB{GnKLUIe=nqM}>i^%gQoE1m|1j_lR-PGORmt}OU&Sh!
zQ}%b&uMbK3eGV5Vjmiu!OF8r9`7!Xs-&G;#Q^-%(4>=z@J386LVfAyT<gd$ek)2;(
z;P?tQMdB6x)to;d$zS0a#)GRzy4)dehz5ZLbq&lg28I)HBbn+QO&JC=F4=4ZqXV&I
zDmvOW)SFC3lg#jkJB&zlRHDNjZ9|E8G~C-a6cuTOQ!eXVEX!)Sw0krG>fz>CJeKly
zuZo14jJ*HBSa*aO1@g2ECQ@9w{CxR27I%%LVu^UNad9-BOpiv}5T|;?`?q;Fn^`Nu
z-jFxAD%5~VU)H&5oUDxt2cjuYx_2m)7)ldkuMjXpxkS~fwQ(^>yGLWg<A}#`^)%+n
zVq&r&$(h7cf#~Mwkhe2rG&X>#f(z9kpPQ;#n=GY(tV<wjSKn2udxvrb$+Qvj#$r@>
zqc56F#(Lx7Z6i@{j*d)1H;s^lQuE*l152hN{TEz7_O*ztYwK2HBs!W*#CwNgscpvQ
z2BI10?>CY}(#VXS_?B2a0;@}=hoh9(ukv!<8;$<KO$<yE=@j@;y0q5e-dLO&(MWHq
z7YTjIq`(6ekFff5a+IdO`rZ`GxVEo185Pl`^@EAwXnia>7#m2BChGfpQ=4LOV@tBW
ze=rtFjK=Ew(y^gP{ov?Ga$B-~!DwUzU1M9lv}G06qVo{{<kI@YNUDA`k*r58Vgm_6
zes~!XYGWc*Rq9Wsjg9CY=uus$M+73JlTmaDgX*VhCaO=g)RWlCwl;%0aogIp?pD9i
zShs+!4ES5yjHbG#x`mneg1Y9+2*4v2KC9>A1Vmn&#{XGmC?N5x#yf&;;$s`b_z8b#
z-ZndVe23*YM(KT~^yk0635<)_kxaUU{j*G`a?@wVO3BlB4e8{Qq-GFoVZW91r_S0y
zWZcJ7G7zlBbCQ~T7IjT`Zs1Y!NiY><E~g2P;xpHL6J8d{(;7^8bGa=hyxE^~nebG$
zx_l;lnL;A$HsPVb%(d2ppPa!mw!wriH{l0O_zDw##DuRj;kTOb%EzSS9VUF0iT)N7
z-aOy$HsQ}P(eE|krHjl{_nGkITdK>q^DUe2lf~uLPUgGuP^$RYDx~;!K4p7SP@G<9
zLDK2DMfj~e%Y_)hgOqsu=xJPY|KtEnO?~_@hffkrO?><ihu<XFO7H;=ze+GQ>G6FW
zewko0qvN|d{2am5w8wXF_=f~jlO7-8@Dl`+8*zLChaVvrHp`B8bNJs0rlvmb;_z1q
zrY1h#z~L_vOig>-$>GltOig-R=kOf_Q&S#i9KMxcYQp0uJ^*6w^#oJX9Y4z9YY3($
zJAN22bU6luZ>*U3Jh?mcFjeId($9BEDZWp<r2})aQeR$$O8a&_T0=g(@97(Arpx|<
zwAt@NL4<Q=aSLCPxI<t=7wD$G4cy7fsz+<)gAkFsYj*GyV0H_V+0StjefhtOuv~;y
z57y`+OR8oB8B;Y|@%v!SEh_t7k$qvm%)UvKbZ5<eL2-Fa12TkbT)fn01ohNmK|R|m
zsBf#;C#WB+IUpjB)*KX(+iDJr$elGuMC9_CZZ61%V*{i}Xr=)Y9y?zGwDR=rdQc#L
z^}e7W<gu*)K8gBp0-?`iVNT$wart&#UUOk!myjf|>jthD->yK-h;P@OH9JtWZ_HC;
z^X>A~G!T2zO14!zHGFXMja^=&`^JiDp7zvSNVawCKF<Hanxn)(HsUcuB2So+U*$+C
z)-V2Y`t-)i`NzIHAATbm@V*)vB_G2dL3h`jLgXM>44^w}hz}yS)f^L%8+dWY4uVMl
zHP{e<(A_@NjZ{h1ImAPuY$cTsGTVShJ!fU*e+oRPx<`2qDcwC1nIr$394RSg1Ni@z
z|J0vF<B{PX`?h4y+xkj#S?>_pC!V72(s*e7lfJPhRh5ptNJ(SAJQg=)M*6WifPeE+
z<J<nRpIqr1d)2q|^%LD;|I>%ad-py4I4u%<Pd`n*|FNB5@$Sam2Oo28U_HLtg9v;(
zPgVHFeznDN#gk;Dd_a8nipMDF^wG+*b`b6$L5lwIQ&!OX#-8v!{^kmwcG&lmQ)y&=
zga~~%K9_p;3g6iGMgCW1{vFG&u5e=7t?EI}W7{#HT=D(lD}mB}_au$4zMabl?I1|a
z$%rte=dq*MRRR6!!&e;t7_wBJ#f~RM=w9O<JMCWWAN#$#$3OOhI~>?GR|kV0->%x}
z2t$GMe%1Jj@A3DoG}0d{{}U8<<J+k@m1o^RQnJ9<n}M<4d%)xLl%M-{KB@VZ{3iV-
zO#th!a9{4e!hNONcyi;$Iz|7xCs8$;GvpMZ@f`@p%C^E2buM?Vcg~Goh(G5Q7df5V
za&Z}af_uTBo*ONr-jumctPs(oGCU<ic1Yl-xJ1Zgc;YbuAAL-lX}yT{e5miN{p0D=
zw;;R)l|6uPFGA`;lQEL4MR*D!4Yq?&;Rfn@xYV4eT@z&5R$ZGpd6I3nHc2P^Ec|^9
zz4&}0lviPT#%hpG^v|3+eVR;~Rp{*%(=VyCZ?WxQE6%*=`~@}V5DUuh!QWxXYW5E(
zFJ0O--2pDiBr7c?twJ8gE<_rYu^;(p?OIeb)y3Qu`cBL0ihYx<_13Zqy{*FLRz3}t
z@iqJ%f&5W~thGY_l*L;y{dTLj!nv!+TQUE(VqZnWO(njHmYtJ2DqKSqE$)g2cg6hH
z3TJD@bkMg}*top!Qhw+T-8SbseOf!`K?`QH$1TM_EMYHN{?qzu3A?|9fJaJz{{{Vg
zbbX|Ok2LU+20qfjM;iD@10QMNe^&z|aQ*3OfHOr`w}d%>AL#~;3u!NnE|pGuZ)TYG
z(&$n?+;1d2S)eob6V#rOx?e!|=IB!QS7!>4@4->@ObH*6_fwT$$X9Ymd+&qO2_o5R
zaZ!-D=j$sGF!{m!vbuPn-0VNYXQWH{htmbf?<LA84<s35&J7%v)nW&vB9#BD_S(*n
znH%gb9#eQGVI_Biq@xWRx>WgaD|vl(m$^6|t~rO59492~k?EZ1f8j^n$IdTmTDgm(
z=E-o84BKV6T80~CxLJlb$?z^2ep!a!lHvDd_@WHokm2uTSR(CrnhfX3Fke3GPcL=O
z@99g&Q)y>&T}xeaZBv><joTZW>l&Kt7Mv&W{8qVWUeQRZi&RRU`M1jQi@FWATf~tz
z&4Z1zqVFK*lR9IrJW9(Ew0iMRn}N34CXtC-HQPGmwpu2=j^_ugMU#p-vSd9+7Kf&R
zTTwA?b09<Uen6IGpexx)D;djqPDF2@D7Za?4pnR`vF#yHE9nCR%bhUFNe%xD%%sb}
z_lWJib5T#*BG{De_g4bVY~Mq_wEg*$6t(>T_sng7p}21QEc%P>eTq-F%|TUcA5h$B
z`!)F5PE&k->Gy~k(+FrN^*aG*9B3(h5S_$k;ef036O_G(1HRG^&@XJo9Oy260G-rU
z!hyA=>qsh_wjNA3ly;m2piJupFj#s7&0?FZk-#IRmr;t&fmG=%)XL`2hLEzg^e#%7
zrnLgtQF^Qzz!_RMfa^=|!LMz)HUMrrOYeuGY_qg20B$LD5`($g77*+%4N{&uZ4AKP
z(scyXb6{WT1_By5u)lPGfJP1+D2)@)q@9O62TNCyGMl+<he};e0E@JbqnxK$*?)n+
z_PI$9fMU9BA63$}){dWhDavejlUlz>)wS8ylUBY=wbfaP?R?-$Y}AH!YFZL|^7V)i
zmRgfzcPVT&u%8A{PPFC3qvAWLV(CpcV7#h4dos+a^bUGdKwnM+nEsEj5RFyNL8}y%
ze*PpS)I-GW`m@mab$T0`j_Fsx?lk=v+SH<NfN)k;8NsksRQlL?Xfypv)Pw1Vz+2N7
zBWls@SnXQ%w@^Znz8vg|bp~@PVU;x)#fwURb{S=R1)Q$azewY{{sa7K`ZYjX^iQBG
zS@lgo7qQCkK)pp4R}JLTzlip|PQM*QOdmn{ntl>xTJ(Pb!m3{i*^BfV2wAK@jarrH
z<)ECTF96=AuR=vi^|OE}(|?Wv?D~7)Hd+5AjG<g#0Qw63R`9IUqof1<S;$_cKRgHb
z%Jo$a#;Wzl(RHWj2aq~de+V6An*J{MoS{z#hw1u$NHRlT?ZmvLZ$^Bko<w|>?m=&z
ztv?H?=IAZJpQXP98Jzk|a5!6Ej`%tHBN#Hy)hl5EHTrvq&(&{+{^scy<M$u*Rgi7I
zegeumPq(5W&euNx<^ugFq`FY|LgrfiE0D8JzY3bJ*H;76ptnNWM*VA$ut|R(%3Yw(
z2j^zpK(2-QY;ahlKL))m)_)F3TJ(BwUZNjDDNFVDA?-!_uTc6j{SLJ4a{UPMUaTL5
z##ZPla6ZKJQlK7Z`s--dCzyV)3IiU~gBbUoVmfw9*!P%jpp@@3-2>=prY}VkKEw1o
zp{^e=eGC%+km&<J{s+_jkn=|{AwbUx2jIs{|0#I>C)1}P?J(0@A?8nDoM84G)1QP|
zpJ#e8H2PDfp9QgBVESI9y~y<EP~D#~{aR?_2+~lomzX|;dc2Hb4V14i{TpcApEJD{
ztbf7u_t6Uf#q?89+%K8_2I_Z|=@I1nZ>Bdx+*g@C7ZrPr>0d{UUT6AMDB)L3zXvLK
zgXvp={51*!&tptq2a%35{XLZaCQ1Oa-!T36(DYlV1A@1i{wy^6f0#ZAP4Qc%KaYY=
zF#Qjx(L1Ou^m3Bvbo=LbOurQcy~}hTgm@2~1byN6OfN(52d1YHoC1V`{)pa;41Z$!
z15m-Anf`BRmA^3Eg-XBA^m`%12WT8HJI(ZYK<?1=2f*_anoduxT(9Xnp!uDeJ`K<f
zn*JPk-l*ww(EvATdLPttv!?Gw{ch3p8c02+>0gGpw`%%(fIg||m9UuGH2oaZZ<nUu
z3`@RU)7z2ZQ=0xoK)W?obvwZMWs6Tj7{@zMb=iZ&ky#u8$W``D0&Et?9#o+0p(~M~
zTO4yJ<sbo5EROXE${r?Qy2bGtF?fW4Sr*3_<@q)NPD?cvQ?^q3JSucA{-%6^3K7^#
zv~K|OsKQRA95Sg>`w`GT%Ou^$bL}EtLG9N-pHxXxc&=$<sCZe=Qq=paAkxYp!c_e6
z;->Q4)AnH8ExWt~^`+rXt3`54hD302LeXXYH$v;2q#McWiR1uyPW=|iFzsGgS6OT(
z>dQ%zNZy`Da-7sOG6TJnxN3JH`Q8kPC}=yHt8DXn{Bn|qk^EF1$qrKKwe!(hoaE<7
zepQjMX{&%Q+wlk398didQt*VQkd#fUBK9}$femsYrz7QDN+snBHBUQ6RlV&f@^O-j
zklda}@;dUD?Rfy2<s?@kIi5ozSq)(*DEsU;&~NrbbJ|Uy*+X<(YofksBDwcg^hIK2
z`4=RAU9n=*o<RpJ`_fDZ%~PL7%5y4JG~O8&WGnmHvtU3BEi@>Y__K1FkSyOtol1A@
zK_$!o0D`J&AP?JHtUx<HjcVAhp_!Ixj&(?|UrW*!X^zVwoBiVi*fhr!CxGn)Ox7IF
zLCN;(2+%dhzfqnY1WeH!ug?MS2?C~TjvZ)e`}HK(EY0y56lUK^fKzjfU_i6qKx8$V
z<7onJBEjZsjt?***l#AFL32Do<=jF*3m6dD7y&NLaTDrdzm))==BPmbw||m=Zp~4S
zVZ(kK0c$k}-R-sCPQV5LB+jP@7}Okh!b0r32^i5FKfxGd-$TGw&G9j+_8sIf?9dz+
z5b$YAxkYoFAZ~Y36?SWmO(f`NC}pqacoWq2yC`L!=15aHpQV)jn&Slm_7ZSFbDRsS
zvi~Ci2Q|m{(CzG>CmkHp9Cwov?<NLMX^xcy{1XAsXpa9T;0px&NUNrXv#-z&qmD1)
zZ^|>&^#rz6djptv6n5&5dD1Krxn0v>ACvLNGnhsOyMG(3^QR!yYLHy7NJQ>whcIl|
zyIw}m=7zo!$sLM>O}iG|z#fW1cRV$Ulug;x??V#%n)PsJc<N51jAc`=fMwg)51@T{
z>b*$$N;b6`vzR^do=SZTDc{SczDR8n>wrD+yuU!oYn1vj)%XnRGxlTvtwX7nKd5}7
zO00YmQl@W*S{?#}X*v{YeQZA{Ij{48YsTM{qu?cS%$Nz1$)~jcM14376}&b>!#mE5
zi_prGAJQ%YTTXK|(rI3wl0qG(K1zbkpn+!c{o0oxA6bA#9pdwfhMl><iDZlAE?5O;
zaUhe#rZ1So*k(-Pe5fwJ{ZmS*M?hl-e##pp?h_yW6VHe<^L})I@^`iQ2zYrGy_htj
zT?l@QiRU#L+B4sS3d#f8d?j1p!^_N;#AYqUxNoaiLN!>7mYv-U_z$ELt(uZ5mTUJS
z=_|x^655sttV??cm?so=YB?p%`3&r}!lMx{?PvITRi&G(x?<&4;sSc@Pe?9CnVhj;
zan@oaR0KCbr_`%7i*DiE&L*-MNaLfo&?av=R!-^8R-{j_oT@EDxBxA1HvN=1N_?|Q
z&e62lgcg-LhbXlae#Y=O<t(t|!cCb=%$xAT1j%QS{FU(}Y~~EuTIEcwANr=;5Rnmk
z?HT<1m@>2wlbK(FDOWnR$<o$A#)Z><3zBLG$H}H<W*bo@sx!ApF-e{KO_U|FokLl2
zrNzj7ALa+W@>;6wEVS9|F8~(xtT~C{p|V7yA<SkuhX}Q+K$a#x7gJq5BxW@wRPNAj
z1p1RIiOt!EK3BO@Bfi=`{QRp*5)C<bfOK%v$5A(GAx`!*NS;+>Y~HOHc`9!mK!2jb
zKxO4rZ-e4@M8#j6m{*Gdpz`*ABa@?EXVHr|RW-Wm9Q<)Ap^ABrfS~fugXqgtC6E?#
z(u+anQKW3%o#+ge|9A@~Wy!FFQ$;|LP*jrPL+C=4_pFE7B<Unh`YDjzlS3-$w*XoB
zrCabTyQGcNKL*50x%9kIiK!S|=DkEZzJHzaS}b}g&-q8>szy?_mckrNIr~uE%5T1+
zy0%3x<2e@~*X6l6c_u!{a2v|-kiwsYiYpHtR1P2(*PPL{VDhE^5hE_qe^CFCTA0ik
zeGg3D&oJT&$eQ+Fm2E0DC!5Cwd}%F=R`M<9d}m_>?8cwa(f@=mo9BSqDu2BULy2Tr
z!C8)g-N%R}?*$|qn|B_W&53o0)1U(5s^p}f2id)<bT*IXw#xUMuxZIq=Twh_;zx>V
zDs7^$X-%;4%D;RH8Mw8+j^sBL3HyhiQgNqGQBfZNTZ9OghRvtFlS+&BI|!{!&7fI1
zjWApz=XtwBOmDKF>)B=n$MzO$2bP`Rhwi8^;kcJR{_=Z=Pkwjl`FBB;`f`qQ-S*9`
zv%fyobHNUxYh41v-*@4^qO~jwX+x*nhN-2T<`-=H?8UFOs3;7>v)YP&0Xa%+WKktH
za#m|?wDPLu@e&?K4zMV(kseEIR9uOTX7O4ZIk2@nUc%!gHY%sYMg`RoOB$%^fp{Nb
zgY9m3khIrRVxs}Q)<%<2EsvM*c!`a4P+}veuf*1f-^N-S4SFT=m)fG%b~eY*ZxfAY
z)+<4HHU1W?M^$*F0wdF2YNT>9faYpysi~UvW&r=J2zf*iF4zF#{YYZvGys>=OwmMB
zpY_{_{TP3oIghA3aAkmB_&ZF9+PjD?x&!*rE~jcQdWnWj9%YNKLFq*s32m(c?V=_O
zh(#O8hU!_%edw<FyU5zzLC`IE9z4e-UB2`f(5Fvd^a})N{-!FZ+PVSsY5Z{^DYB>=
zqr3I7YY=7S<V%&4JG$%vl%~xmQSx`HwYwiiS6Yq~b{`@aYnT5M1YF@`S}i45_nb(t
za5Js$2OKz@S~0-1dI20wtr+TKT7!U2q*kN>H411)8fnPa^b*gvn-T#o5a7O)s}2^Y
zEfU1L)2{6(R9h^-eQDP(Nuw<SJb+ZxL0cm79!R;qgnp$h72u(i>ta~Db}^Zrb<fc)
zu1lc-%|&)<-LreMtDRCkycPD`mU7jiQ8b_6d?@XD9!;(JkBiEs-K77UDqW-vwut{C
z*?lgw*BT?0;%X(GKZHoz=OrQe_wT}0_h>y(*EWw9rh-}9ewj}Gbvanbbmo0jCXmC+
zkw>>M<|b2Gqr0dK=H|aByJL{bE<%ytf`+_%WqJtR967Z{6jo}b2PMVDnVeB2zzmZ$
z_bVo^5|gzueIh39+oXIyQ9>Wxf^xJbk9H}Q7}OTAcBQ2^6b;pQmK5(0Dz*Y$QC5y?
z<U|_T(LgCMu<kjGU@f$TV4Zc(j<m<Jg_<jc&}QAUk3!v&WC*5fc!Qu(GA8%m$YXpc
z&Dz>!I{D<+<)t4+dPt_z(qL~kowfa3CXnO(-`NDwhC<?f2q5tR1d#ZEkoXWu$b^J8
zlJFgqgc{7rTy>JLL@J#mJUcI)B&?R{B;nF*I%|u{1X{6#ObP9yGGS05(E{z)N#w^B
z5~kJuIf=Z(gk1UW63GQcR_6Vp#Ox5ph00-(Lwgt0TP9x*$SPp!c^uXROx=gWlKzhh
zwlH5a`uU{&blbvrE0(R3{)M9cdElCv;`~bqQj?p%lOQ#%mHScbC4E5cTNTG|sr9gP
z15kJ>^EiHr)=<ahOCE~w3A!FL1py_tZOBk!qrs!&ZVZ_W^_n_{5dEr3n_E$Bw^q!o
z=sBZG)6sh^)0nklsr@3mzZw;-VRrk)NJkMe3J(wK89Qd#5(<cFQnhBSu-hq<1`t!A
z;?M%JQVACOs|hSq{GlXG^H)=j$!9Zvbv+Wx=OKc{(GoCJ>?(_Lxa;%5eHM(M%94Y3
zm`iN}uj)E&#SFow$-ZnxgqJj>7zNN)^;C+$iOCYhEBw`2!ZQjbVEsjfE}KC@&&W^|
z=Vm>#Py)1`iw@X3rq4DNI4fJ7*<94wR3Xw?^9-uaoJ@9mz;4cXRyKj;F!RNtF~hZ3
za6Nk#>fJIuU?-mEh^$nAQqZ{qMMWqy_Q=dlNEzpv%QTlrikW9l%oT`||G}J`EqHzg
z4Rs1MZsat>LlefhY{o21W#<DSty3T_$U#7~3(W|4OS&}Dx~bCG>M>HY2C`jJFI=p7
zfSc;HmDNtWFp>quT=Qfm&4h;l#VT!K9Sop(hLc<RA|e#xVP_#z0dxu{T8%2(BFL*W
z$0XPE&$e>hlG$2gHDT7;*FoT=#jLS<798%2cm$YbW==U-m)o_TGl=iSOBt0r9nlq8
z9a&H{SGN8Hy3K`A&Q>bLf{xxccD;uF-^h*4bL;hZJbU_{R%G$21Z+HuDCJuzq8Puj
z1+18{3_9{Rvz67<IG6AUDzJ(oRC>T%6VYut1S$?CQfBr5#z704ieF9pb459R`4I{j
zr?kK0JAl!xy~86zcun^2$TJr2A4*4}>Rm(eGNVG`C4OU3vwWj}AfB%8i=|MydfySR
zR@RRr!Lt{5J&`lbVaAvgzp<`=WQ4rl4!rc)I}r8t#v?d0V4!XgXlO9=u4`RPAm|mv
zL{r@$W-NL+GZNj1I2Iu(ye*o{pkoO{VSsqiQAO!>QV<9({&*z1bqH_iDujHKRDs!I
z^W;zj^pbCC6fc6}{o6z&-JfC9(VH5L^=C`sguU@prf5tb7PSVjzQVL3i*=ob2_bdT
zI@M|2^!iq7)%DiI^^fkbmf4nBuX$wq9eeKBv2**LZ|}M5?L7lm?b&JFdi`f?=h)63
zxazVZtMz*A8QZEKBL9x-Y|AX6s-0KZR@nkYMb?=%2e2`VTeB>#+PPk0t2p**V66ks
zSZ6--gBPq(+a(qUj5O#C8*Y!MwX3JoW3;>d0k6mKceZ=oA%Cks;1923L=p0LbO*df
zDC`dRgpBS6<_m?5?yiu>+s@N_LjKN`M$o&mC*ThHL+-G@tCQhPSbB$cb1G%vrPg7F
zEGWd=?(g(^%<mUBE*_`X?90;0!Nh2)FTIf$<L_MU4){HWJGio^!`m4S3E_PH_ORjc
zhP}ZKe<v?n)`Y*_I+W<&WRM`H7ksHson3~{8w&Z|okpnJ+vacgL#zUoZRE%`?qDa?
zv0x3BWGpKyxalqLMch2LHa5U^;>mDqIGV)k<#_8ko6j3sV8kMLR~<i*7()v{^LWh|
zF9)aS<y`UB@~C)gnO@q3O8A?{c#S)p@I*KEriW5o&`Y}wcOd9>d)A>H{b4^87DC0k
z14f6}<L~KU8%Lwjj_7b=bQ`Dlb#-)wySmq*alPG2Z={BvPIoKXJlutLb$g6fZ)cmY
z!yR13bGcW;RCxLtf7oXP{GB~(jjqmsFc)ty*cCLXirD$l^lKptH#b$zP@fTxZpnS!
zJEP0SX1qP#nI7(oj=D!jd$)PJyJNiY&>D9)XQ$YZLbi;?Qc<pwT;biF?!dZ`*SNGh
z*oFFZut=gak!sH>0q;Ra{PARTROrvU#-As7chKJ%4tQ6414f(A-MNyicjHj+K#~hX
zN|tgM;Y*o;_p|9-0A>vLj&8!M=$r_R66y-{P`98?6byq0G@&{g)Ewwq;|;P<Pg@&o
zvb`q&rH6Zhoou_X9dmy%y%67k=9hyQP8Wbhsdw_dyx5>O1kH<H&Eo0dU=)>&4oBmu
zP%1hiL=!z<nT)$F>|d=icXX}xng<5X6GsK~1ib;js&m`AHnjdqo`!*>t*bK>4!WT+
zMW8AvMw4iqPIB;GjiU}?sTd}*Ts8D`cS8-34eidyj=mvdBsFT#K+lWF*tF>9N|`u7
zVmKD>CDn%$Ls9gzenH>a6*kt9`OyGIV}|PFP;a-9Q@iY#W_0!h0-<hqTkc5&vL7$x
z0||O?e?}rz*Sryscg}V`42~ZTmPFDcLopl(5XDIbqK9NHsi%{R=Jj;K9CG_dW|qo1
zT%kWPBKkA-9F2}1XtmW#BeXv(o72}7=E{QobwSC6M)~}vg*lU4rY0?5COzm|xz#q>
z(fI@2%os@|Y25MoSNb?3jJTlW<4ot8P>^x;1fxSSez=D^bRl~xM|OBz(i@40k-#g@
zEa=^w?d)B6;jzOV$ab-In$5_vM6XJ2=^YWdg~@ldhGC#Iny={!u0m+)CD{%|;&o?E
z#9$lKLqlwU4_Exa1YRSmOUQqjmqkVsBi^eqk0A$6bx3Vv!q9_o*WlQ2;zGSc=mp9!
zNCu3xU7cP=!(IXtCNXVIq~no1rtJ^~Q*$&4PmCHUZ~n#zXBys!i9a6Akxd<P!so}n
z-u_ML5rfXG;M`<~<`Zfgbr<T5m$l^-N0N%60%GUn!jRJySWQFZc#B_zinS$%ys`Yr
z4|5(H8QhjcZx|Yi#s^Y^qFw=vxS8=JBunH;J>4F6*xTU_2mKf@)er|`3%gs%2f+v(
z1E)mJxS5?<nnp1JZjNS6s*ur%^J8Mv9O4WUDU_I8J#cd{p9q&BJ6H1BW3nk!|7Nlw
z+1btYqA@D#f*Aa?jTpwqYof-8sBLB{UK8|(y+%9SG!{#GV!gt$f?gV`bEd>3<xa7*
zm|*j|F%>9BdK&b4I#+det?A4Wxp84=+wgFd+*&aMwe-c{qDOm2$GI+O&Ir!aLC4H$
zQO!fmG%m@5Z(v6_>?b|cI~q&!k=mR=j;+n)oj^r2qmM>=Bf>0I(Gxel;SNFR@adpQ
zX@BJJiLOYsH?@#^HF*QDA2;ix-j$s&hI_Zt89bMyheudubcP;yxukT;gjt&!51tHW
zlf35X>h^Yu*1<rrHO1ZJ%e=uZ?sRe5yfKz_xYuGthKI4z2zWbJhQ&xZZm`as54PvE
zjTlKgy4%beHMt2NcG!;`L~?qy>Qk)=%%|d99u^^kGmMSH!d1d}&b=S*0%CIM^tgi_
zq5q()EOimHsY_pk3Jj6kPW6^Jll5c={)PQ0d{p)s7&+08o3#BNv;-|x%wtZLM`J_Q
z#l!SLWs9!T)e|<l+6BK5$G7{}N>EK=xh^P8XC7~B&q|z91wVy453y>8x0i_Ed@9qJ
zr+R?&7Q_^vYoa6xd?&9v7$9fe<AwE5Rpi{!HVDlpH*JI0*4v+gDXU(Cz9F1ThAuKh
zYa7*D;41NYWSn)w-P*;yP4j@qNz_#C<ztH&#?<<%9fL!cTE`jf0k?4PGFB<qiI@ba
z{pr9yXWIoAh<7WTwZo}&Ji2@!8joV|0N6X)Ke${R2I#CEaMpG$z+aQI7Og@fO6|sU
zyr28bK;i7I{!O(bqlr|Ms>~(j{x?^6&e~42W7cJA^K}Khp;hY`y@Jm(nr3$~Uno<{
z&zkR|<{jw8nW?oRZ*QtLe8bJqcU^v8B<AE7c>F;xylHs+Qe7DV&1Rx}%G8Ii3i&Zv
zrtawddu{@$jivgWEt>4Yin|AN-~o$AqZl$x9tQeEC%G+DaXFP3=pyE^NOgu5?($)v
zLXXDq98YcJwwokxj+bB>1TtrYeb_l}f4{9Th6w<^7EV^<Q^0n)wBj|RCdXo$W*IfZ
zn0mWuuq}K%8=atqIbHdK7*>LbE!mB(oS{S71i4EfD(o&zXl2rA7cN0|=vAg4r&XYN
z63(s!rEgw%`GB$B<9Dy@!fM*cEdJGg9*dJZPa?q;6=SPupwAyRvQxFxDc_(-;M}%e
zbou4NF!-@_vWLkfFx>8TICW;bG7a*LCNCdn%8}OwUy}<3*}ZEqUSsV|zJZ#X<T}<9
z<m;iFM&?sYh?eTK-_^O!pVK}W%Pf3qA9lMUIBAZ8CJ`(UL9+-J@}k5ZiNDk2;bMov
z)6*7a(pK7gN4fW(gcs-L3lG%Ed`2LK|5SQ3j??wh(segG%R#SjhuYlarD7Y7R#`@3
z<3{wsoa&gAxBw*tuwIppv6!@^r<=8<jKAGVi~K-Hbp6~t!VXm3)IX$2H#P{HrsWMa
z5}qmG6@zQ&iN@LXjEBxul%2)0`yxVjd3z9@UEJto6RLeC>_K>gA&kMw_G!8fc{|`P
zV8YJ&s!Fyd^SVj89Au#?JA`Z>!iF@h=ajY1KaUqGdYjQ6?CLPBi^LKz+l;X!_T8|X
z#kf<WnmLr}O{J5vr3>k7QT~W4WrMTpBYz0fu%|2Q1JHSdd~D^C<<3L}H%a+W!nZ?A
z6Q-&EVy`E&s#C*`@Kp<My7DbsCkA)&maqcnatNo*wD2%BfSMgLo2T2yZ3r_Q0y@BX
znl+TQ+!m!<>ENl^T6sB{9SZ6SbiQFK)}LKx@zscdn-Dm76dU~5*vCGS(ID&zMwGkP
zL^|O1#&<K@x`M%;Zndq%?Iq7z)q<KTpQA{OODh9itr$@|u*>6K={4YycK3w&@(z6~
z9PHtn0sav7!1)#^mx!-=v1;Jk-llef7t>|hx-6jJ#wI@h%I#ulMp(gPC(PZm78`W#
z;5y82l4oH}H_{l9l_;|*i+q_y3KugwQJRkS<S8EiYNK_X!52m};AB_)S+znFUF~8R
zW=cy&63baXd|x_=v(VsT+Fv(qrqg{0I^7ccywR<+4N6^*PgdApkR!cZ4b$2iCxDKM
zWwp>ucHC2np}fk#_=>YzxX;SLqeTT(P7#?08)fT_v?dED)b1DBBD3J+vZ<XVa;^9_
z9&O7SbT5M^)0P(od(5|&V6P8GP3wKWNjfl^*peM`a&6VSw$1DHV1mb@Q?537ze?lm
z+JSg9!aEqY+HgmKuk;c_5z~Y*?s(Y`IV=~85^l-nrW&`c(AbozHymlGfgd$$&SG+{
z^3Q*jr=-gDl85hfZH)FNW0>i%13%tG7791pWE^=1!U}z5#(YEhE;-Dy!wzl13iE~J
z6wl0^EThx@!QRnA7O;dH0PPN|j+k?ktphGZQr-B<n_Cx9S#S;cE+OB;fF1R26`Lw@
z<1xp{<FkXfLqbbLXht<=#vTivW4tjoihFpWbl+82j)fCqs*+yW_%S+0;}kc~>~0Ic
zcf`$rR4a$By!9o&>&Wx9dvO;;Z4$%rBDa#Su~q*Z-=xj7DS=U+_ObZQ11cx8s?M&d
zp)u}Vu<Rhgi&QPFI(KHL*{r2(i<>vJ4C)+Mep88;c6YU2LcRtc5NJsKD<+fKcZ5(P
zo<l4h5?V+IM~Rn^alfeqv*%337j!^WmBfUcF-V%3DNmcbQ%p(nuC3R|Y@`%U#vKv(
zQ<+7JSwT`{+NT$Thd07fgR$IBSl~u)Ki>_e{a-4Q54fQouE=m#cwINPI<v-s2}Lc`
zvHeN(VyU6#c#OIpZwRYy>_KtI9UI|kj0AFc$n0U8tY5i)3-fLYqDPuXk8Ja|C^Le^
zAu1M&>dbD2se|UGjYElExY+z2VH4v+4#Q?gcJ&8O(vLe<*x1PoU9pW=u8fYNo5A$}
zqx7iJkE6hc;|%@>-PVRHMK@2q;x1N>c(N9Y#Evjz2Y>VZ0UGY9E5S<KZ9chWot<jc
z-J+nkE9mhCvEk3}(J5b7d8TCSi<)8X+__XsW89jixfR|%hI_gxow<)7ck2pIpTRD)
zkZJQzJ$`yur3=r};3++ZM{e&?(B~vpV__8?t}+uU=xT}8SdWU{nnS3biTr8~Rzcs$
zW>-=5(GwvMm(BE}#C|9%_R%f(DCpZ6v3peX{TxCC&6E$DLTnYiP@+&SI4Y<iOQ@pm
z96|+ovxF+znM0_cV3trtpU5Fp&?{L&6@4p*P(dfMgqdho+o+5tq2jEzRkR|9P(h9?
zp^9$EAyiN#OQ@pP<Pa)odzMf|O?Cnqm7SQHA6Qe%^Qd~fTjecdnP<1h(W<Ao{}x(z
zg^Tm=mf;#1s;7Z>N%56*`EsczjaJBf>Lr1R=viHI+c5SA5T-^&IH+QnEuy0%tS*i%
z**cv2UYo)_00H4~usVKxeQhkl>IfR_O%AfU$hJ86i4fifPuYw|A!uYX0|wGY;kZx+
z85_Y<EOp!ktV_`ilsb%?F{CB<x%+j|L1W`+?{L%@j3BRqz_h>Lh;HqVj-(8}%^`Rg
zz4X|K;GyDI^&=z7>m82uBVPhV;HQq4E6Tvp{;aM)ftz2LC+i~7zVrYx_r?d{Qc5ru
z-<VMG-oC!k=w=1chPH}BgDMKYT}36Q{3iQ<fbsY<Jwrzi%<-$m;*uc`GT%uOkf2p@
z3eUwKp61Tv4_jP-=zBywU``Qae(}6r&eaIWoIho8@${6;U&Ep7Pkw$6l75dfu9W<H
zEiO*XU-&^R!XZ4SEkiu9opW^p!b6%F`I*%v((#ZWT|OD&F?Fsdl^@S@@@qAIiA0sJ
z&Jpub23+dgFaeF%A3c;#XLhOl>YOeQ9wDWxT?85V6`cw<A)U?uQ?NP*Y`4rmBJ-0t
zB&U*JqS-ZwkWN*8b?z8f3h)h*FkgSF{x{_1SLc{DkWz7}bI&OMWc;b}RsP$7Bm0x;
z%AEUll!$TRt%012WHn#(=)O5WD|d<HauM*eghiD9;!U5N@J>MH{A$!|z*GKoy_#2`
z$}dtG`y3+X{OX*#Db<3aR1wNBj~&|wJmpvVSKsMz%KW#;d@4}-Q}BL7%=!1r*M8J@
zzLqIM8RnP&t-SolWPUmbi>_tF+;ruae+X&j^66M*x@z#^5?%8|konDH$DTt9sakfk
z%(;H*yJ7kI&M*JJkQc8BX7a0Z|Bm2|V7l_@^Wir^Lo%uI>BMllj{c4Le+ydGw<w=F
z_fCD+O?}5r*|#cR)$0`Un9En+4LODPG3iqD0?NV)|2`7T`PI3D%qep0R~cof@~N-{
zna%n4J2!C3{Z5fV$tj?0`3jzl%*0FS?~u%Y&?zWXKUehm{HkR64T?a9D*pv}c#@Sa
z6`m>c3vBlHHK&NXvM3%?`4sxR3G$zNwxHNe6Qt?N&%bbj{13>Ew==K9<>zmkApat_
zDD)vUFi08l^LNSo`TBdiO=P&)DKqBfS9~s$`E#l8P<%vSDpbSf<yZ943G!b*ASgx@
z;RN}sfX^#}Q=XLhlS<GD@>6o2ia6!5QIX*mY3OqSF2SF&du5k2J{Kx~<YOZLH$J5O
z1XbZ*`W{h>#WgO`3W=4UWKOg~66OEo;)^opmFD6xt6E)V6pvdmA}K$_oM?rorTmLr
zyp1WpAr}wr*j;86k6UpppPJ{*iFgw*Gk@peF`27**_2?#_vF-kYeGW{<G&9_={P_z
z^BucfdOh<!%UpaFQ}bCa-huhaWk&J1l~rfvA9JD=r}(RRB^N&x^TY--ipQ;NT4o+F
zCtBGVOpWikcvz!zgBiu+)&k%8qXa9wA~nvMGg;|n#sZghoEI;MtV~XFd3mgCP98?l
z%SkCO)ykZC7)5{f1o(5t;i(@Nv3E}ACy;MoG1&>t^>h&j4SG;-eyV=m0(>DpZJd65
zdvya({Ey1jFhG7q@k@B!;Svb>yH(=z&p{S`?3wHt*^X2ny8a1&S^@ic3V7nL&Q~T{
zx(?${D`1Bc)r<TO9FxbKdG;*)g7NjGG79OZ6as5>s&Jkip2hLwzfXCA#2?6`Z{ZcM
z9@k&|60go}COPQ3obwsqUua%&7O?*j;2k1s&hM8w9_=OlS2ZVmOXBl)EhhRt=o=I8
zF9kzqPKg4TJ^}th;0wvw&hg{Nx4;DSn<n6YGw_AJ>v`7%^mGDgq3@D@2lzsAzBmD%
z&MTqsfbNt1g8B(vbn;yxKJ^pedndqe1D?K<N}mfdUpG!b|FEP#l1KmD3Fu#(0Iy-5
zC;jA?YXiPey_ytW?v5#0Lkh1DGNjLd72<OT@P)qf`ejZ({(Gzc%JBu}+1y9&<Rbxj
zH@nsSy1-)j7?hXwkK$9MX?&=tACEL#(uQ{~=mvqovL8=1Qo}|+eLgjbM}@>^R!t~;
z>@i^s3?=&T3~2;UUL}p*^j3zQFT7xyibm=dEm=$tE)^i+-;NYJYIx%+g*VB>u3RKN
zJiHBAOb~y_B8Sw#V>0dzuYqS>=@YPAav7IZ?`m(yogjLYA1_9M0^fkmP)CvpV~}1K
z#dBb}yd${?JmZ0zk%nhor@O=7#?0Tf%-;Pp-CO>!XQbuFK(k-Ylm%p&{q<X^Iroza
zzr*vld>z;H7Ng6hKEFwy+?3z*%>QI*mWaO5`PV<LoBJtH`VOzMn%uAO=6obHTmQ^s
zaT9$gHuHRsNtb`^c7Nd~DNVQUG9`Y<lL%QERU3(K)|x-#nx)Bb{jdj5`P+sjaTN3X
z`eztU^tD_0Raw)`?M#*l9x)vMp<9>B^j+56ry5`b^3$z_jj%EQxeD<rR<7xYr*3$2
F{a-yIDrEow

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/example1a b/3rdparty/qpOASES/qpOASES-3.0/bin/example1a
deleted file mode 100755
index b28dd75d6ef4dfdeaee86c16ddc039298e38467e..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 28799
zcmeHwd3;;Nwf|hnixkJojh)3I2`Iq{0pc}|<Aj7rw&X~}wuB^SfrJ~!l5GXsaxFOv
zk4H@sAf^c<UDG}uHLvg<{gf8kQXX4NC|g^)1lm$4l%^~tp@pWD(zGmj-!qGJCEG2(
z_vz>N&*LW6%$zyr%$YN1&Mf!Ny&w0sG+Q){smsDHWRzOvaB-0MWo*l(JSkxf%+6eF
zAv>ED0FsYC7tf(k#8h}nvR0u@;#m>q;jh6W01XxwkEn2t05cUT2`QxVGM7t6MR?Ct
zkyIgLP7WYClJ$FYc@p6k9#P@FQjViCRCJUiy5o}WxTI5|F7Z{U>PK~>zlh9_SX@X^
z+C>5L@ZU^GxO4?yO+_j0)Oix2i$_$rQh=EX72U<4BRN0pCE2r(EN>#cWZFk?R_##L
z8{b-ASJfM>=#BRe?X1|@P+w7BSD8#y*7AB2f5KhU*3Jo)9?Os-nWy28Y=H6~{hRT-
zchCO8=0BWx@YPLs-f+{}BR9N4c@F#$T_OGs>cciRZ(6Y}FRyrC9&=M(3I1l`Z=1d8
znfqV<<du!ypa1^cx%=l#Uh%o2+O5yOamU)1kNomSFW>y^$2QE__VF)VdEbFQIwnth
z>E_pe`Ul4@|K<09bi%6z!V~}8s}uldE)ePY6Tt7EfX|O7fY(pJCo}<k6!7unynh1t
zYZJhGCxDkuAm=R;z>lE8bhOHVa|<|Tu0H`^XJ@d(Iq+i~K8p?Kz@GqoD)1Ft^{elK
zzjGY^j{{!9iqn2emHc(Ur?M&Rt|=}KQvKye62Dr)W0K%CDW@t|@p&F_;y+v@_<S4m
zbp3+!DPsF3xp-Rr+#>mpO8yd^eTBmv>{ba^_*ZfM=>L+xf;059j|~LeVQ-i^*OJO=
zW*FUliGCxQ>KIHJ1~V?&ZiHgp@nkAC*wovROvaMT@JCvWXlzh|BdtxniT+rmV{30r
z<c*(mN!v16R>Q@?!341T>f-(JlsC9G8m={R{)ghhC^IrN%fpJuQ{&xEIcpWW<tnGK
zW?6SE6&#HBb@YZ4y+f&ZqTf3ao2D9XJl+;IYHED3WHR2-AK5h!^NwF+Lr)^bHS+gm
z9Y><&5Gb&9+q#Zk2n}r~Q_;@z&nIQqld5;_GzMaW$wYrgZ#=cj*j`OI-JP9Al2B@x
z(cZr!-XBF1B!~K9l-a3h2L|K)sV<|l=W+(7iJ=tuQNC=tzK(c5Gh)$>R0lG)CX)gV
zP=AzF4J8N3$5wTu(54kzJCZRGT~XDO=!;dwlRfe7p}|B|XGiMtc)zhDS=HGSk0u7=
zRa=MRz0s<k!GYwiWYv<v=m6|wSCwp76;{BKh<|cLRbn7jHJC_Np%(G(1OfkX8PVAG
zSTCWfDs?7@j4s#;%qxI;L?O~pG6wrFsD7$uqWXkOX2sStH5p{lO&d128~sL2<r220
z#oyRu)K=D3E=|XmRMw@tgN2V8=_Dfg7-#68RfZxZe)Bl&Q@l?z<^*<d;jSX0`IYRD
zgqJYo|MbqwF}=uRucq@Y>@Jy4r4F4Dx6{aV68SX7rDjpu!rqhkr_S0!Xnf8fK|HPe
z3C&&P6otdfq~D`44#lJ|rwMN6v&aNjzKn>fO>p!4sKEqRK9ulXCO9gPzI-P5q%;(I
zGr^(2^tHhRpPYs=w#5XWZG!ih;Ke4mZ_n>-zE9-GraPJMnj@+F<4cg~+w-XH5fc)k
zr<aC+a(Y1ozf;a~Ax7yQLOOBmG_C~~QJPeF;wVoyQkqnF;s{T#ptP0Jcky&BrAd`1
zMtS-?N|Opt9OUWwl%}cGiD8~TgVNMoCkA-Bgwmwi6I*zC5~WF{CxSd}p){%TgbQiZ
z(GGxbB%kOWIT*f=Sko+leTRhfUH$tY3V&;gKGzMQd<PyZcOldF#NKk7v=8JhsGvN+
z^bBn2a*n^Ld@&GKO+%t<%AzL`uP8$tVLnZ~-kLJ?rgE}0L~boF6Om2j14wQv--+J`
z%ZDKbN85)8c)URTXhOzG4vO5n%WvlqeZe<GPO97myrp+2yu$+Tj`C4Nz~uy}uPm>|
z@BVV9DE6lEMJiIQA`K$4zx*yie|P!4BC@~yh=}Yh{~jV#`J)KwLb=5rUame@h70Bh
zIrf)7%MqsDU4BeNZYn=6BGK|7gy}8ca=h#Vw95fcxvS;C{_;`ewj9`7PWAL1Xel4?
z9r$$lFi+gg73UlAl-mfsI@36wa+=qoRX3IEzLC80Cp_hgsmYK3=zYO@3$Z>12(c!)
zeMnOkiTe>A-p$pL<{tbI+&|8_-(7x^GZ2<?d<6%}#IN2veHzL?`ki|*-+-R>m!AUe
zLr^lM_HxR5p(=L#r@&N29{(*Ru6~p(vF6C7k0`ir<dNe~Qr5_?kDrZVYTodVJaw6G
z<R#ypS55{a{wI#mbjSC^!!)b%J@EuhmX2Q!g15V-oV6FFFm~^C&MmCnS22vhx960@
zH}dKZ%jQR@k$J~|d-Fq-b^6$pvxW(FFQpXz!>6pk_l<nl_wehhecDmqQ>TVNOmkfj
zKb?Aevv1^aLI0AZA6|9KR43f_)OJuFzY0BY^W*t90Hpot5%M3tJ*%E|03mf|T7+rs
z4;{lQ5BR5#Za(oZAenL&JCPKjd%b()w0oU@<Q;drf8^)xNXvl*!(h<vJ5aF?VYubo
zS8Ja4J^a3vJlgRE??PMGypcLn)xTxr^_G!$Jm7J9+6%rtk7&N-zZ-g;9Nndx-J9H-
z-IuwIN4mNy75<+dLDk4NN>`}y6%k{Tb_NDg&Q;D!oeN@%@#oxpfzz4b(-=S(#3q@Y
zH#@nSRK(aZ8J?0MJHo+inoERCh9@5q>0=LRv#l4<T9drg!M~k8O+F!rN|EhuK}elx
zF8a>~ggR729)<km7BVqh(?r0uD?&`$scW+*7upVLg*w6K;P2Zoqw^#V1>@s0R*pQv
zXOQ6tg$}*hQFhUkq8+wjw)%_<&RbGGpTI=#!QWxvO(B51I4$DZ6~PuRNuiad+EyVC
zV~deTWek9h=Jt6vO?6?Fsqe9@a~zvwz1}j(p*J~fZZ-B%8F%3CB;=1FWQ`8}MvK=`
zc7xUHa309>Iu`BE_c^MsE$}%S_7t`{T)mD4x1-waSk&lnHag0H-{`P$dEW*(`5+XV
zb)7z~&Ddi_AAHezr*>OD`%FIK_tMWt*GC%oNCO{f;3Exuq=An#@R0^S(!fU=_(%i)
zr!{~TH(di*)X;UzAwr(Ok6ez832E<>E|pJvrRFs4ebS}YQtCeLWC2d!_b8TWbzg(-
z%h9Fouc|#jU4U_CM_kX!`>ASuW|Qzge{ecM(Zd!O1?hXfqxm9D>rsALT|7|BU7J;a
z)jF+AB-wNssz9PKhV2Eq)Y?mJvZ-|_ZS>JKQv__-?&2{856ZNXyISJW1|VIkd@S#H
zeU8~&9F8S2Pb)c2%Jl6rpCkQ$_)+(<bBlU>l8b|$k>PJ;_>K$<q@B%>;X)bK%dlC7
z>txs^!`yOk3t)wFQTx`R{?w4OuCk%BuA+8`r)sXMsjIB6t6Xxffb(14qM2-*i!ya(
znz^^;a*KKtHju}GHmwQvo2TK%X$fv5yK*osx*fScs{`0-E8IH+fo6-r0<4z89WZQb
zUf~D_7Ibr9em(9A=jG+&ZVw3Z_aJ5Y2^v0s5A6(CE`;sp@1-cXT>`><TY>EmCAETn
zFt8+GYz5U{0;KR8$bPo)&u62awi4Jz;X9WB%xsUso(tdo1VwFkK<vWzD6ZQsgQSJ;
zQ@qT!8-1|w1ByFsUjyI5(-dE1r(2PQOru1#{bC4HsPRODoo?b5T6n@`KT70zJmIsq
z!oCahc_L_EiOyD7z!MwnF_OxrT?(dK?DwD>7f#YTkm#}NXqLjs8VNjL|2F05Jdv`G
zqE>|^S}$^T+MlGH8CoL}!}c5D4+>{$K_qtDzmDI+GOZij_Sjbw%^Yn968r48QeuI&
z0|*E0XAn)LHiE=q`#efi@x-Wo5hbd5;&yu#C2Dx$E_*#CYPEAgbFcmCA|&d#Y)9-q
zQb@h_uPEnHHt8u6;R}V|07hBiC{@yy2iH<~3q_f2AF1^}sJb@W->?r;_@7i;ofX*5
z1FXPCZCFH2OJYynjR--hH97Qvf|dgMDI|&sx0rZ1-b58`*M1&yPKl$$JliK927>+y
z08CH8LNqpIHd(Okp;MH>H~4fL1l9Cqz_IAJ!MLsZ6;pwu?*KesZ-8|auqjdS$g{Y<
zi)Pe+0quWWzZQ+i^mD*Y(^ml1qCWtDRj&Z&Jbfn2B47UxvP%5~AcguA{Mz(8VIg+?
zUBoBp+fi7NeioEBS?`4C#kvFd4&4o&Q*<|Qbp29@GF2}}i6weFdTFWtSD45&eLr%i
z>tBEw&(LoGpPBkkKvkxnM7?L}mx1RQ`gahYt^W(+bM#-JFU{4z38~K1hXFrJABFr*
z{mbA#PoIVOeEqYK?QDHE+NWH90G)e*z7XxRP=6Nu&(Rk`wnh4ODBxWEw<z~K{ab*X
zuRjf`7VGaJUZIyj&Px3{XroFmhCJ2!6_B<@UkMJi`a#IBL_Y_d>-0FNmg=X_&Fl5c
zp|@rFL*Ut<mjJR{_oI{*`g4%>0(~!JSgD@}nOEsIgZ4sw5^A(szXP0)p!)*!Fw?(+
zX8JDEe*;(a2-9bxCXX_GGkV}-OkV-n9%uS>NIk*yTfqB!OkW6vf1l}Z!x#O4>0blz
zNv7vR&L1-UY+(L~-<o7UX8QZ!`4gr;4ILe2`lC?TQ%oNO%F|3=2lAgXy%o~^jOpKj
zs6S`=^T>OK=`mFI7fk;=wDBy{EvVRYO#d#l`%9+(7cie^`f4zHf$1l}`d3W955bE}
zzY~i4HPg>TjgB$>QqcW|>AwT;C8ocQ+P=*6>rtaum_8XLyvlSJP=3qwFo3Ty{c%|8
zaa0o`ok0Ci`s<Jd%K9A&f~J4Z^hN}4FufN-yvg)gXo^2T1>kZLtWl#sGX2X4-okD)
zQ2xa9#VF`)q#(qfnLdCL-eLNCp!*BcS0QzZ=^+&KSEj!Jg1<5S9jM@4rVpVG?=k)N
z2;OJ<a}eSKhy)d!hCTrt)^so0;c8946ZX7Y)1N>)?9ueM(DHjV{VDLgM$<Q=0j|~b
zeyHa<O<xF|?bGynls=;A%OU6Wn*JJ6pU`v*==N*+eAMrNrYBL*4VwN21iVqxZ$Rpx
z#-`2z{UZCagDAe_c2vlI_g8_yEG2IO!Tv2u*eoSKLj~;r%{`N)<Uz{0mlD$~C6^+w
z-$#iuOUW0A!Tpq&V=4I|(L6v2r=^sNv9HmV1GXA})4oZC2<Syx0FX-*bUIPUtTt^M
zz(eV*`#9BX;uX@a0r;TGn#QSSd<qq}w^yLv4*^xX9ogShDAPIhj1$nXeba?dJ~=(@
zNn}5hMiE?kplEw1`DBjrCbIuZD35{X^e0J%8CSx(?D41Z%TcC+-5mU7p}<P)1H0h^
zh^tnI?A2)$QP5O0mwo%2K;$Uvk-aqsWf`gT$~vf=qmTo^V{4)yHe)?C#qiA#iRT_h
z&dr(JTS<cJ&VUhdA@4!X!<0+P7iymI8dY_F8K4}6Z1R;Hlx?85AG!^i<tXnVdorvg
z!$q>{gvYXfZa@0X?WEXp;8YPF*P5trFQMG>H2Na3vaCUNt764wbfdG_Z~GdA=D9KC
z45(bucr#xFnf;FG5SkcT_EL6+KP#pI$?|Pv+YbCEDp~v)Ae8+VI?S7h%(@kAG3mdx
zB+|SpPXI{$ZrV4X<mpdQ!=7;|syFGYng_BFvG$tE2cskKqpKnOOtPV(8(u_`^C<#s
z+OJ5dzxc;LccjZ=We<VFL1O5Ec}?E77+sn(+(Zmt{>Qv{aVCbdUw}Q>ikB15i=l-%
z&mjFa)k`a-tm0ML1IT)q*yo`aF#&aHPXY3Rf=-`AS#t-_lEog4cxivc&wDE0)YppF
zTn|(o_}Vm-cNYFQW5MFg`;bu_dJ<O7&C{Y=IJXN3Z3gnV<p`y5?XzOaKdTUGDRWHM
z)&Z{-F5XE$1tkT(IR*1IZH|U)QK@-^soemyFA+;}GF-T6^ND#CewaY{I<g-gi^AqV
zhURt5)*e8E618U08I`KNgrDO?u#}k0?<F*+_Oj|q7M%;H*&$d3{y5t7v?YovQJuL}
zi4Jl$sa24zB$8}tF*j+4dFqZUsj@V=oD)D=)U%wP?Q|4q)Uj-qd4#C#1=@|o=K`>u
z?j|uyDZ??WeG%Z>R2G|i7-r?zqY+>2e*8S9vP45JD22`)*Luj>(Lx;UH$Zt!p|OQ?
zpdQEd1?Ur07_h7yi?m;iziDfz^o2vj=LQe@FYh@PJ&$8m0HZ-+2~{k73%$(o={F!Q
z_Z1dBpQCO7+GPrrEqos8c6@#|%ttaT;8<4yV?<#|hF^kpJHGfL<dCR^9QDgUyDtk>
z;*Y`yIBxs8@&p#$#_^v6?w_*pd7~0j;UE@12}kJo>P_gdysKhm$0<v}ssevp7rYQ*
z4kpUSsCB;e4s<CgCvnPFP;Jkq<V3u4a~n$YkiuUB6+4b3;7BB+BF^ZuVDixa6C*Cs
zlgm+I$!IcX^lLDgiE4_va0O&c`|rv&otl#^<N`jo7Dg-i7IVJKz;oOGiZ5IESu(-b
z)?-F0Svok&>%s0b#FF;{k`1V2HYX3k#K@_@TBmT-ZvpKARXSUEAI2ibpYP8w)H&Ak
z!1%4gnoi3rHsd+;1jl=WAmG;e0kZSJS(yDf%U}wQ)7MhRC<F9N${}gkq923WVbL~G
zccrzX0JCE9FkB<&dV^s$rFIlz+vbEu4(Drktvt_xZZu^%hduYNzx?yjx4yIDJOeF0
zWfh0H_J3>V+&iax&Mzap#vQPN(Z&A`kt|DTsaJd(>7c-N7{AuMJbHrPYRmIMgaR8G
zQ-O`f$O;=x_$qk3fX54LG}SAxkqQfJR8oPB2F(H+jd+zrO1`KHfWINEwk?L{PH+<P
z*%dY#Z@?Du0v<20Q8f!}i=c`ETP1#LDs1G}3gj=TyTWz>htO{=xh(6YK<vlglE=~Q
zdE_zF@G!MQF=@4~f*NDGX5EX#s6ylsg}7u3kZ(g4D<&UXOoKr!jc(R^5qlDUoH>su
z8dgl>;nE*t%+vmeSp5%RK3b5fT~EpoQMT;+=quJP0$Uxhhx#jsZ5NqI6>GQ@k`5HP
zkcpPzzaYFR0KDZ#!E;PjaK)<tpFVxTFA<=#tlS0$+9KjUEZL}s!nK<Uk++JNzJ*EW
zg<A06QRZqN(<&&#dT4aVYB$p=XY$0s?W?<)RwWXn+gJB)Wm>gJ-L-x75K=WFb%gRj
zS4#_b>!G7V$V769NFEz<eHCJ9^`eL)J6xnBZJD4wy503}FeR-)B#&)(T>_D`<sx}<
zyK5%tZG}h<?{Ha&_5#8A<dAC<np|5clEd3wU&kP&xyaV6hmP%V1))E!iR<~$4~JZb
zpmuGoV0CbZn_B3#Vi#!xM~1|uwLXZ(Yor}Gx|)g|E*?3Ant8N#t;VB8>{fbcO<Z1^
zWN-{*K%ioQqSbn|iwQZT)wAYKiSrj!WjXYo!6S_W6Tx=iS`QsWu)%uhE(9B`hmLIb
zSaxuQBD8_^5JKIOWC+T%YS#R)Wb8wQKbgb$h+uq7Fg_|6pA?LTcL>HOi6IydQ$c3N
z&q&5!gT&v+VSI20Ykpbg)4cb|Og{6TA@z041YEOJ0%g6SR&|Q&UZGZ02~%O3Yg6M)
z-i?%1q^V_jS`+DIDvhVl?tn8dxSbYQzEYS_!IvrOcY#aV4f-RLQV!7HLn-C@iv1|&
zqALL!UAqszcI!ds7J%?9<O}$*t|z<U(_)HnCtd|tLkVn>fuMB+X8jj9u<0X!!L+7o
z3mm0IR>y2d`^>4Dj>&{&27364q6>=rFb``vD=NAW`EdL)3ipsd1yHc0fUpWnHLIhj
zh=?>KZN+I04M4NYWWK+Yl9LpFs87@Ur9?4#9`l!0A+vZPB5-rd!BDZAl9$C@Uj*)R
z&@HA~vfw4=QftAhv{G9=OR%XeS~)AqOPZFC0_xylr&9z&Tn4G%;V;b~&KxfTvvVqR
z<t!3<RvIHeoAiwFGBC}}$UB$If=XCXW<yckERuI_nyjd$Xs$_=Gcy@fP)kv3naakR
zA<Z>kaCOc>#Tv?5iiqbtK}rQE4a`q-<6@h+QLWAv*(8CY!akt635oB5G?Su!M=pzn
z=FDt)sf2UP*%`SOrNNL(fU(6`he2Ez<H}ic;4;nwKw75&oSy}NXp7AN479p5(#q-5
z*s3t4VAW*1q<xIe<Vb5PwKb*AB4H#;@_ECMnbZ*+0_3Z_rIj#%x>-(c>Ggyt#KYEQ
zx&r7FPBbc2wn3mz)k+FoWuI&0u;p{Lno@#nDB1{tSLCyr(mAxm;1NJpnmJ{Wt}4>n
zXA<8FS1>BK4AIpY9a&H{SEl}ybejt!%0?>1f{xxavRgy{uK~<+{cb!ATXv`s1YQ9q
z)MkZ2i8Uet&y^`)^{kc9k-v_uDW%4_h(}O?wG^S!Tg){P-KJH5`k_Q3W_#cPEx2_8
zXQaLBD8?^8LQBK%Kk;}@Y-dN`KrfDS{S$e{`#XDwqA_(0NSrfLKpf&U>g(i*&hGx9
zs;%)9N>@jbaO$aQ3<~bq;p7l!n#GJUDSk~yW#_;E&6rwoimRhL=IiK>;?QV!We>nm
zA)OXV)K>OjETVWc)`d9SeF~?&l4)=}fhY_PXO2{qj)MU~aPjv?V>^3s3QhsiN6vBq
zDxE&ArT|Te{$y$pr?PNBEfF2+OtWh3NDan2Gi7naj{a1-a10n0HDR>=glTyeYfQrs
zkb28H(`k*rveR0++uFC=cAUb?H#|6E9olW1_ke9)%kZHaui0kZwfoR#z1uA1rM7wa
zw`$MuA=|v}ZJR7@Q}=ALT#O&v+B~auwygv_yDbZ~?|%h`s%ar_#Bh5&je+(ykJ0S*
zw|G5<zpdHp4*MJZE&j+xMi^m#Yp}&@gd^@qd)Nq8GhaAj1Os7@x0&a+hy879jF5Lt
zdy6~d54$7&KpVrMGdixeJ(V(WeymS0^7)%1hQ}N6hFbk?98Z>NK9g6oY>XZ|!rsu@
za5c^<CVLWtsjWj@oS(mKox8>FG2Edw?XBLnNSGfU>rHfCZcy2#<8xGvwt(UDhQoe$
zn-LCroBYjwuV=ja)o|c?cc_i31<iPy{cS)spHxg6OGYx&X~lZp-aBimVGRArNW3qW
z#Hm6YL|mSx^M;og@hFZ(;wOqjZw4K@OVOb>aT;(?oCc)BaHuUm(dRSzV>`HP7Y7Y@
zOUUc?Y(#7MBYr3$3<0|aW3kp)Ut(|<$MOYQ1Cc;*BO1;dRC*>ww70n%(Vme2+RW`S
z8og~zzE*c=EvIs?gBkGr_5O&@Xz{nTZ!iLFEy5JMp->=X(h0HiLqi)N0XHR8PVZJO
zZPppibmeR6aAdM=sBdd*&^<WVvCA6_#(Cl4_3j{Nr`S;acMQf;F|L7Z;oWWSmW^Sr
zad9vdKz&+SG|`qwHD{E7<AhOve=;_h;>^A4{W+2cL;ki%i+7#3#c1-m+tyGcclCC3
zCpkW;R?1;SF3xKHbkF}#r$+la1~116P|g%h7Y?+vQx~BQ6^fwDaC=h|470ht1q~8u
z54EwYgaL7ikT(n^i_WQBBF_-LRvCi3DdJz}l@zUkbzXC)=3sAIxIN@;@vAyDZEQm8
zuHkub0!@Loa3ti0MifG}C!oH(wbvL(4I1QFIa%8JL|;}FGW8VM6_$5IqmhIs-qG!i
za>}+q#Mns2L#~DTF7!ani@LT4jV#$S-o|KaZ)ph!-A()qZ~7>%EMqC}@91cqY1vdw
za(q+Hkz2EoDtV(r1HEw^jg8?<uQ2kAF|@aF`MjPsv>Mq(T9V8$<*Y-&orwX_C9wfy
zw6>$CG<wN_`XjQ%e1Qnp9ZVt+436uBb6XD<NphK5Z5&HzNB79CyU~pP+Y)5PKtdQK
zydp5Ti)dRP4y6won~r6t`z^Ji=(b*YPPb!wre_9lfUDKrlCke*8dg|WZ=xf`k~=yE
z1gR)C&=`TKk%L_y2(3kEGV-pW-d@(7_I4y&*nf$z?ePqiD}sb4>mj!mX!A1iFbRx1
z=H{tc8s62{7o)LQbg+i4aZDs)9fM=sbDUf=d1g{_&JYPlWAZZU+&CY5wsv%0J~Uv^
zX=_d?`xqbdnv{)$9Z4yAEJV)2M0XFgtRqikvMXto^{7-+qSp)IvIi54A#9*$R}vQ6
z+Z*fePW1@6Tj1Ez-8w8w<XP=Ok2~URbw@&e_#Wk6Z~`0Ti7a22ZvC3tK@2Y2V;Kt=
z&ql;~|2XMi9Ez8ch~dhE=>dGT=<Atrh*ueoe7s86kvYg-Xs!%-kjyx4@B`s-oUy+m
zW(<fbriZ!pA%DbcG-G<h;z@EsJU`?mH<>luB#Ao3hQ#2N)4!-d>1D|!wYRNp3#@N5
z>q<6L9Sy{gm<F%GSVvTtgEF;=)o-}N&;jNtsF<_?n#15oeoD9xlfvBVXy8K|x>?#)
z!MHhdswm}PWG01SWQKpuQAr@^Z4(+nKiiq&Q@TsMp#YymuvYg5xFU>>Ym63e+nR`v
zQ;+dd*^W^5lq}O$hSiuz^v1;d7g&@Uz%aV{#IOtp#79~_-GCQt^SDDEp@0xC*2~+`
z-1KrVqyob<r6f5e4811(&WN<-F6~6&gVKi3=!8FJi1B-%C7SY@J6>kOR#TmsPQd$7
z*}`-J?GYo;Eck^vyxG4&CY86%p6+mE6LB*$`Vvv$DY6@$*kf?^x<f5A$Mbm6TqGa2
zDKVd5Fs)vi=PE11$jpmR&*uzxV}MUE%>9<5sG&lrR`f<SnQn%S2h=p)Xl`+f8A7`8
z<uU;djGB$!zvFB=|9tU?rL&?hHPjzl)g9}P!O0@oG1%F&O1#_0S<&sR2rR*0t+N8k
zB+pyXHPqip@9;<hgx7C$US2UUm`KGsQwdQS)94^vmTG9x0GUBKd$Q6Wi($TD^3sH^
zX8&CEBs``O+o83}oKb<Rh(;ZcKjg)<3R5*%=_X&G#T#CmZ6j*>k^0B;eEu#HQ(u4p
z=?N|$Gw4t*FP?Xa+)}9$((M?H?Q2+i8p^~{hjUBPn9P}(`of2P9m_v&ryyMoEKkU-
zDVJs%)-uzj3Et|0eiKUUVCj{3R^K!Yia;8FP$Z3w+mdqnB*uV@Z!ue!oED8gV}sZ7
z_}yy)Sc4epB@^qX85EU?AzpX~>6W<psY#w}){M8O##W2C1YQ=?0qb8yGqGk2$L0aX
z<c?}gGb+60hQ?b=4S3;@&AS0>Y^*S77*SqS&i*|iKE=#x9_|dov>>4se%nSrt)4rg
zcsUIPwIWy|f;tf_<qX7n(BEd7W?(huX>W>P5Own@++Zv<G}w=~;|$3uk$FNA@`~ZE
z$xV|2tT5;eLq?*jD;a}lGn-1<nb#~qRa>yTAm<@sel8~#nU+k?X&Pxg(-Icb$Ly_)
z)>?-9VO+|qa(%#6=>mC0Gfp+LS`!+`Syi<Kc*9Hes+BQTRo+k-&O{j)%^<?wRt!2A
z1TtfmwCS2!^AvOJ5`-x$S#&NhNSLwF9166Wrj=&>DaO^JOVQMrtmdEB-K*4Qql(CF
zyLb{?5?JUnJ_cvCsgkKa*IUa9V<P0Ep&CPtCy*KE=<P<_-|?zv_wI4`mbeq(>q^sT
zV(OS!5~pX~%3+9U&-i;)d=JWrT|O=V-$cOsvb>?J4J~3M<`|}V<5gpJwiw>4)y;XC
zTPxGg8<hU!oI{OaY6MTO?x_3FTar@o&dgL8DZJ<l+ogDy7j`DGBT4CMVVr4+)v}#L
z=Elo6bD9F7P<v3VS@?LAWBO`wLlw+YARN$|mOvw1V=Gq3?loQmlcZpKgwMdyA0wf5
zzCiPbv6;trfVf0_HG+vJ-z7A)4~F(Y&d!b4CUCcJz%Hdbv=PIL#2;Ua+-GD;OYb*<
zE<K0kB4oxX(;LHhj>o^wXxwP<IWjq*%qlCRGSo8AEPN?b8ZeSrP52G*t}c^9Q^s%J
zBQ$LY()KC6|0+(q(yX8Fm2o-TSkp;&Ef*uS^uasA2F0pIXeQ$am13xImGSTuMo>)q
z)Rcziuv9sPlv!5rVyT<aA{_3zjqhC0W|Ki%tvs97{_yJ<_b$Tr25ga5Ql@Pp<p;eR
zn!H{QhEyyt<jR6~fT8|myt_XZ<vk(2SHz4&Z&Zy2{GDLj3ve?sH}sfwJ^ZKIzky$e
zJK=8&lW#B^&j&3PlQ&ZXh|Z9iU#j_U7Imhd(teMy36z#KK(Tb)I(s_Aq&8<1!Z4du
zTiQ8UONbX{H)1OzJy)cueph@DJM1K8&eo8e)Q)vEaq4cIBD0X>JHT9_#7XwRoFxFi
zFTm-Vz3aUpwO@pZ0!??!4Iz!Xj+S_ESm?O&7D9S0lUd22Vtj~YnN4zDhElXL-1MFk
zl{9v3S4W#`>h1v93YJ7vm5YOc#*1jc<;G6lKTFc=mP>DGKqwK<LYCE{HxG(|lb4X5
z|4|9%DI66)?ue|aBnFVQU8s?y$=xPgsl1WmHPUNRVmUS&TcMa#uvAYRZ)`OAmT~T&
zbn;yz+9x7b+)0Gnxq>2r$i^UcM>4GpC#Y6a*fAk|F_%?i4m^d&8^+oQ%XB_t#D>@q
ze={UsP077$Q)?+-w=`#m9$m#OcBV}llySg_5DjL%?vcUS@i!mxt@>KVyDor3gGMLb
z2-(-qFtXA8r4}rqG0pOd4d$$(q(t>%J4ntq$SLt{M0AQKpWLL%cz3mN8}bH19&d=o
zTbNsY9j(EK`pH>iqKqy|_iNG{fO5BCe19AYKz*jo3-yrG0{I-yBs{|=pJ4hTZW?D@
zR~NZ>`k)LyCqwmMkK&=8;K?FJLfvirxTI5$piP9&DwDhVFg-_@8WdrViot+mg9EIx
zA1n7tyoj+Pg&iJ|!cE{x{yxcyc$8IAs;48_!z!b@`oT|xaArJbJ8qI-ev*|ikT)3X
z?H~df8|Y23O774rQ?zwe$zK~;i94?JBFxHIkI^;Q(HAp%qM%hNFzxI#Vmmuy11W>A
zG6WB!gKj_x9xA@A6ND(Qqc7eGx&(^APbDu`l!13`vdYc`_L4CPtc=FC4t0aLqrV&X
z+GR4{-<44Dj;&h<W7}1V_B~V_8dOn?r79{p<u=*>Dvje}bnBe%lk=;=;*uc`((lI>
zDS@lv6rPPgJo1{Rk62tt(R-+Qz?>pT|Kh%T)>VU)nLcH4@%)seFXySuPcFR&S?}O%
zvy%U?#l?~NGopw^c?yqb$q-L!WnFDZ;o*$5{LJbS`FL1>u2IPt52kTFDSAA|!LN1r
zB~(@ZXugZXM`eNPyf_EqhjeYe=rJ@ptF7qO`Rv<eK@AdF$*=HKcscUv?6*p*^WVoL
z{eYw=`AJSCzXY=@5Fwo^dUf7BXcrjO5>b_}=vDpq=FqG2>%)>>op&et$@t5qzX34P
z3CTy7de8AbJUT-c9#zh|NLKSTf`pm=u1PMQeV3#^!c&<aJnzRZ6@D5iGrjVA17sAq
zYI6!ybRw6rFCb#3SMMjdb&9}{YEbB0*XaZzqxd6wrGNDv>ANKT0P@V2(w~Cej)<Aw
zsk?ZVQwq9LA<8hPI_w_6&GcI&{T4+}nWihZ{3FOS)6;A1=(-6H<<gbgemU%LP83fz
z;!k$B^!pst`{Hx;om>8kpe0o)diB1CXXgTlD;GaE{W@?+CPhzgW~A%bhot`la8=(T
zI`zH-^*(>~-hO4@s(e+iQ=l=Iuio!_>MY_mvHXh9`@k{NtM^SXCsAV@7eV@0(W$Ti
z#Af;d=N67T;1ncEPLUcb&two2FQq?vohDtwP9CJ+Pa#q`(xf6XoGQz&&VeiX^K;-p
z=a&l4kn}ld>?=+IaphzvIt4y=0{Y)M1;+RAptAYOrC&M${Wt3c)t;OVmrLI?0e$@i
zg8n+?7^D!n^Z`kqtG{pa(*ZaXMgzR*QuHdkMADm3coYxx@N02ShN4s8*aY-DCH;Ux
zlwmGCxd~-EX4K=7KAA(1ljOFWqax1cSX6L3Mw1I%3-Grbf6DIF_(lGCyz-CqA_Lb$
zIhncT3#{>f>5btH7T1_eE4;m$Cz&&?kVMUYvf+8@_ta#=F{)Z!W)P2C;gQrl#GGkm
zg-p#avf(zS<_+0!J5%$4Z1|+~dup=bW4}{C`BselYFy67FHWyBvf*RDUm*BozO#^x
zuj6eEE;ES7t#}i<8qds`R#t-XX*~SW^!SsFKaHtzB^y2+<HQy-h{vsLMtU4DXIk-=
zALZY(;jl*OH?!emzjHzPR?~MYvhi~|yUetXnZ=6?E0cqqnF-=nc4iJl;mbiOC)diH
zIS_?EZvy!IF>vZvdF+p;b2Dgcx3G+z$Y~K@cWBUqI_shOHTCiF_;>)<@Rp|B_PSW{
zmuqSy`PGGADt9yozDvS$-+y%@;3UsJ*^X2nx^BjwhPUwM+RtNv6MvRIf<)N#`V7J!
zYljon>m<jY$`0q)^9LM0w!Q$5tDi{_SfkN8#}1twKKA=5izR$#4*qgp@zOE%y-32<
z`?^RDx;Aq@WBW@N;LdTr!}2k}={qT-Qg6!lUoGLe`xE;(T$E-0{R-etGeE}aP5I-&
zC;Fbwap0F=2CL@%ishReu2Pw>7y^&S|6IVw`;JV*1o$DqOEPTJv0lK(`##Ptj$e#3
z9xjd>lN2Kp@VRFK_)DCB=DRtn;5R40pKm33#(tOQ9Kgque+$PSYj52WKUgH>SLN=T
zfX^u4<9*-fZjL|pJ3I7#nG);-sP(7fuO6wDw<{;OZKL*`Tb9YY@?O?Ci0{P=;Z5tE
z*a*C+2~SnhW~0F}U(GR6eMTpJHz$c_b;S2|Odx#NBw=*-Cbr@!o+xhkCykDwoeY~w
zII)q6MJwx<FQYr|;~??xq=?NbJQkh8<KAM!D>~HIw+keu6u%Xig=*j~vAflan+6{G
z_79g_#^pV`Kyx$phm8n-2pkyr_)i);noJlybo>I3C1mrCW&`-AbeQ4U*ye8aH?i@*
z;lelW)Tdoc``4N0Lb4vC{V?AxGClO~a;a~@(Dz^D$7OQApOZnN561lc&mLuemxjKO
zq?$GR`$(qS6zL~)#21bJ-nVSBcEZz7yiD+5jA?T{^VOt@K6hmPG*Cvi5B*3GfAoIb
q0zcH#@9O(RV?TuCa+$u{ll@FKnnixv2mK8nKFV$;aWj=~0sjxaxXAJV

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/example1b b/3rdparty/qpOASES/qpOASES-3.0/bin/example1b
deleted file mode 100755
index f1a19d4c515cc9a87fab518fe5968868794398b2..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 31765
zcmeHwdwg5PmH%AHc4W(zZ{#F6kbnqIngD(!j^hN<NVa85iETNOoCh@A$d+u2*p`vx
zJSYtz0ZM>CLfJM@mIli1LbtRjrIhy9E#XlfeFi9nzO$ut6WVT5Ud^^Ze%~_>=}MN{
ze)iMf=kxo6n^-gF%sDe>&YYP!GxyHDS9`lUESkpDWnmv?1l?EZ<|y&Y*zp(#C9Z{)
zGB;bm&SNFO6ywj$Qz#TM6`qn=tI#IttO$$n*J2Tf7K@ulR5(YVnF^JJ6jFKF%PpfK
zoL4146*A`H2;w7IpPkD=gqwLpg~y~Ej0EGCi~kau_>N1y<C0H>y5v)#svp&j{=zan
zY;gl3*i8Xz=fByIXzA*GEghx!uIU^^=;jd>UM$c|g^F)2_(;yb_tGU{P?k59Ub5{Y
z7*spdkHq_%o9aiRwIlJdiS4!9TbgT|o9a@Dy2ZTSlt0mS_VjT^rN?SOB=ZdXk*X<u
z;{Hee^84v;K6=F+$E&th?PAv~XK6~KrX;>;_}j1Vvaxe#RM?7&DsCuZ9!fg{e`n+G
z^3t{+ef`zju4?tZ^t(lKZ=O@W;#2m;{XhNv=hwV?<R{;I_3l@$*f8hvtM9z(Yy1A_
zD4+4GyI+6t4~`xFEsub8%Iix=aTZ9u3Y98^ADM#wS5wfF{H_A|&zl0@3q=$vH#!Br
zW{Pr~r^r7z1^zq07b^FkrojJc3jDXHkmsQ(@N1{QFPoyi>tHZCo6QcoHba8!wHNd{
zJCi+|hu_Nav)TQ5_)Wl12mN7*SN(Z|mD+1^{vQF}$tp7Ar7F1-`01>Y>9YT-emWrO
z_v>y>s)p5%NjdZ7xdM2~e{+SPZwE}*D9^{vs>|IRRzJ&R{!ErD$(e`a9jr*=75xmJ
zKVuCte}!l0-&c$WJRxt0I`xve24)yTqlqyim5wCShQW;0Ta93BD4t5kl5HcAR4SHY
zhCkeGL}N*b4tKYWB*tRlNdHJoq!muNq-U8dYgu4C9Z!s<8kfb!QWMEo8{$;L*uW0&
zR%T2ljD)<wHK7KGlTBN`wl|qTk)y5cjk#Rel&1JtJnijW6Adjk#Q$KtH%hszQL<!-
z8<!2m(!I&}Xk;Xm7?~gmy@Gf$ZOd>X%~h1mT;)8OxUJC}?+F==jlNha6_1RCcZ|op
zg{#uIB*kSIOLxV##zwrv)&Q9lZ01s0%&)$}qgbxY0##j(ng;qWUl$nx32Y#hjt*RK
z0hvHE8Nl}K#&|55N{mHD;^`g6)&`;(8W=EAMAFENzOilbu_z29H8C2a!~w-SPO=Uf
z1H)SwGEGdRksqZ?gBp#*$Cwd|M$!=^^run+57bzc)la07G~?7q(y+AJ{zxh&qATi$
z6Qi;EcxpI4G?7fy4@A;i;$z0PRQ<qkJeo+x>-#6-BhmWd<alaFs(wi_Iu7gIQ7_F+
zg|)D6%0IQDJ~5uIPbO0Js6~7zLCC*dMwD8FNL7^vQWM4?EDS~;Ks}-mX(APac^Onc
zRWnh2q9tQwoo#If^|Q7O8$7Lkqp@xY>+JHkwi%1-7S}D!#Fx}HWjd*dSh7xwDWw?m
zFa5L1P(b3hh~pNa@6$Z(!e8%#9d`1pQWz)bBD!Kz`io!O0=KJ(eKeD9VI4A^=qJvM
zm(q-WJ<=)9^lXAH>^4b%>YU9)mhl%kfFCu^l212FArV%)HgkyPeoO_K%Vol&_{_D?
zgqKC~v<4Gijlq=AV#1sKNw*14Wvk0)!mBxf(t1sJR5){OFyYHHSjIM+@D(QfunF%l
z;m1w*N)vv&39o!PO5SC{OBa%J-C)AII7a;JH{s3myl>C%Y`&X{U#fC3-*rdQ#m5Ig
z@7wc~?MV|eqNkTW0g}`6pTckDIc~%V9wwp_$4=v#|8;_?2~Qm5@IwSs)15fN;m;9l
zCHOFh?<JU;>ck-q-$^hv(TV*WzKvjVbx-W#@QnmhQ=J&+@NR;siB4?h@RbBp)12t#
zaFXCMg54axoM6~KJJG=5euAmVPPjOH3BlA<Cv*-638p4G!8p8{U}~BZC*MDP8amqu
zgl}&#<@4nJ(AP=SZ&H1I*Zgij>h(sYKKE(V*SGH*HM^h}-!s?N97Hl*NSpr<r2%({
zV;CE(T=?c2mDBI4*)Iqx?%@ROHJgb5DfiXfFH&l+MhXxgsac5MXiWnZMI9DUS4|6L
zJ|UoVO)sF82@dH?$yothF(gt)YFs=QH_yd~-*n9e{9aWv%<<#+?W);+{20|2wK$C8
zkAGPL`%zWjzK1yywP=u?7tvls2}1NJ0*W5rWa2u0?7h>cZ+f^!<RFxD{2Rb^?dz(U
z)3vX?#_dC_!M875<Dw!+Az3XJ)c|DAwAaj`#y$QfX9Pq%<HWPsw-5DJ4Bp1)GPGX>
zEoHb1j4J*33mhrM=mUU;IQq?M_)k#P%{4cG_A#mhplfS(A@V4+$BwU&Y+NZ{Fz4}~
z@1m4zo+2A)JaXxizP(Ro%6giT_Wts?q9<mgA0Gq%Z-3nQd;i|&FZ1pFm2c0lPxglW
z&m1A|&iBmY<V5<Od4_z$<F6u%w+8n=`bF1f*5|9eAAxVrDTi<GYuhZFo_rJ;@{au0
zrpGAh^s&lwb`kDTf)xGZr>vm&?R~=c`0J~D+EL&0rzXJsbt3d#_d@!uO}@QP3;th8
z{#`4t&|Mh*r}u&L_)heQO-~nJ29);ZlQbUq_N?4q0fO||84+gmJ$4MMXwaWNy6MD6
z!BTk+JCPEhXT4|dY0o<U-gi8G{=F}G!d?63yO2SjZ(r>~grTlQuQmSE_xO8O8ljH&
z{RN7<?)T}lE6=%>q-0%tU+>!cPCN2AJ>zG-Jx^-B<-eVHod&K;H+eqd+2py*V>~%H
zSf}XUd=gcou}%(c8eefTR<<3UsB5L`QrG<0h4^!Ay2#~9aK{MP`LQxGvdndIm53gb
z;VBuiBLY9eEkY*4laC4b*kjsR){AH@Pou<<Kc7BLW_1*mA`3W%@HoO6bdn7SY5b{%
z$?Zb8nM@BCt)uDEt_(75yRMy8KFzjYo2C<f4*s5k1)Wa>@+wS^GLTO64Uj>nyd8Rn
zqk46veVc6;TXp6|=P#+5M_CYmJN~L6n|VGWUb?g^d%L(K)2y_lunKt?yAWwq#!2K)
zYnY;Uoo?oF=zA<{9fwM-MV2y$-sZ4*XdtGG%J@9~PD1_|AzB^!troAN`WCC#;o4W^
zbu7HO*ym`tzQpHf*)y%%;U00ccpMEL$HG>JtJP5r`c{XH%lj7CtD!>_n{%B$t<C$X
zhN<PaR(xj>yW8@z_D~Uf&q~0JMZiBuKObBlXy5}4e4v33H1L52KG47i8u&m1A86qJ
zKMicgQjo4*INfx)CCmZ*$mPPMkoNfKQt7lOXohKzk1plw-y-31fzI52P<vVG{=#gT
zuI{g%C1G8LXUgyuc|TRHA8Zo;_WP$36g_BhQ;@mmTU{bxS}*a->gIu3j@@k)Xtj>0
z79g7`LlsCe#<2XQOIC~Rl8R94B((=f_ZsOsSmx%T!VgPW$=$$H8SamZOO=o12d~em
zQa4Y)5{|=4j*}98M5asj|L1|akDXu8I(c7dP=;G&c)biiDZ_u2;UhA9T82N8;csO4
zjtomG+`On+GF%|TW*K(KFkc>Qj<0Yn?CYNxOHa6(>RRfWY8Ov%sBvdwQ(Z$--I7HD
z&u@*3<`j*hx<sYSGyhgveo>!-?G|yQO?%CO_-r)hFJQRid6brJSY7cCn}D|3raccf
zt7dx{mTk36>z|9DXxfiBvgF4cSzLo1?xLb%+y?<e@eV+iufy<5_RtE~a`!oiUQ1Er
zb_W=XZ6&q?1ZpM6kbxxzBb?UoSzxA}M)E7Rx6ea8ZOyPD+dG#5&1{2^*Y=m2C~CVC
zHg9{E;<~Nej`({NueOb%Dz^72?y|+uFKwqOzOeLLlo`_qXej*>gt2KHXeoUdox*0}
zfV=c4u@`Z`S2`UkwiR=rxAZgUl(rHMY$)AGQrWaik?H2rCvcnIR;EP&443|vc*->r
zc)aunl%jJWUHW^}%I4HYkg~nhP7=)0S^?}TeY^_58CoxZ-KC$wudP}eLT-CXcM{JW
zZ5x0aO8-c}d~F*D_LsI1Po1_Gz`@eh1k`ijQ0ZC%8aQx&=>`HCIdHhtAYie!2t1FL
zeglJ=t%=Kar1X9gr&*)>v`?|JUyulQP76cb)wV-aN!$1A__>Fo%yuTJ^<Jv3&DKd;
z`B$o~&Pr_O16N|BHndaIlGx?D5g{zKCdckm*eYN@0ic3tD<}`gMX-d@>t|wos{AAd
z;iA&p9tVLwiv}$Ha#)DQDwkn&Dk}Z-8<cP#DzQ^P2AyB6--f1RdL8Ue(~qM~EqWJ(
zv$D#i7^aF!A6tYr(_chAn7$Njn!XHCi~c9{2CM!bD4|Gy2>4=s0)}70D(^)#i%MU<
zgxH=zPFL$L8pric{A&6}lxERKIrA2viy$%7TV!$9KtBBpDD-OmO7Jq>gYq?f2zgud
z2Y|5ZYax4)PERou>jzP*61@VH)AS|4+w?n;b*X+1FlG9)D8Q~?j@-)i!)X5sy$JLU
z{ZiyvsmDkM`rVLyx?TrcbLxXmJXxYQqXskd!$_T}-++!XOFxEu&d^VQt6HB6NoMQc
zM4o5rzlE01(tnEh9Q_GYY_9%2NOiXUAn@nt-+~M-{ddU!Tzw_t^YlwG&YY+3Lo?Or
z2N0jHr?88(KtCV9AJXSQwuO2r6tGDDA<8{p{{b);=zoD!7wWT6daXVOa@Of~*jBy%
zAz&KxuVMsh)DJ?!#rj7f!xH^Dq&DeSf@`V%-)PWg{W0imnf@U1Y|-gf+H(E#Xp$BB
zdyw`b{dtsru|5qluhi3!>ce^;YP3o}ft-&p{d36fai(uX+dlyln9kUfO#g4tJ;n5M
zVLRVqdIRKqn(6BSJ;U^F)ZkgB$Dr`%nEpOSyKgi7N+7=j+ku?lMJ|B8$8QF*?=$@m
z$nytGw;=5()2krn^Gt6=W-p-m!TuuHA>9v|Zim<}G5z0>_9LeEqPj0LeJ%uih3O~3
z_hY92C*=GI)89g+e#-RS(EiVuJ{wv8oaraf3cq0bb5PtbnSLqS`4~ikLjIHK#Sr&b
zOn)2|dzI<CP@`Wn{Wz@nHKx0P{0-CnK>iogPr*`;GyVGz=>*e1iPB$Z`n^!rZ<+oM
zH2pgWkKp%A{~Cn&Z>G;fQ~Uwl4F#QK`UGn9N2cG7;0>mSQKL683Zp7-K~4zqHZnsA
z?=XEnf<G~R5Wy*?Uk$_iAEqw?!=IV{G*s{xrk{t}zRUElAb1Zw9W{EN=|>^;X{LV~
z$X%L#E%LlZ(_e%=@7DBBK=XSveHNf=H9ZC4uhVol8sK_O?}vImuIX{;>;_F=1*!LH
zdJW{fQPXt@ag(N3!eVaL^m(Y?K20x1LAPl7IbgU|(_Mh}Yizm!aADc9H=q&c6Hs;8
z!;64n7U#D?Q1%r9Y!>Hzs6g3Q`PgT1-bE>o5-`K!yc9v%*9fS#I3K1AzD~d#kP*-S
zA;4v+qGHNAwNIl$_uy|vJryFbtF<oy^9_ZaNgOh%NBb_&FK3dLa<0oLub}o{K)<1q
zW^k@q_dyM1eJiM^fJiHY2s81=i<`;0XEmckmwn`;s4oq7S}l@WG9)6Ge?sGx4O|DU
zbCO;pUrHn%<T<mGWSEtNb(O_`hhI*TLh{Z$k|9`0+4wDIGe)^;pG5LK84^*@yF{|}
z$N1$WUqkX!c_eE|rC0qcgytkaL-MZ_37hpCRc6;wG&fKEGg8Q7<&v^lw^H^W{}^nL
z3t5en^C*>+FVsA1FKnvp<~PB|NiITiM;^&h@RuF<J~YcoE<^HI4vEZaCAxIkr*@;?
z+z-uZ*MsH&(Q&Pb`gRb>J*UwZDJ#nZNdBVAip_c$9kA?Q?tsud^%<nRpi)KSonZl6
z+2>o)K9r&5H<X;spH<L=WHC^u(tYz#$%;RLVETnX9<{ev!R7n{s$sv9W?H5>N04B@
zili;loVAe6{xJe<nsYD4RQpZ>$~9*%?A?Af0lMaVoOpH-Fhg^0JR86@1XOFzw+Yxy
za?R14&5*&qhX9x6{0Ig#`?W+?qdC7!!1W~9Ld}^%ueX1kfCkO!rgCl|pha_DLu7ji
zaBI%>sEhqZ0(_ct5&FOVCIWgj=Wfd2W&$>7&OMZJ3jv!o=g&!;TL~D}oY%oZ?E48A
z*PQoajIkdeV7un*qH5np4#O_Z`7V|72}-#^bJ|H&w^J4NYtCaBtnB|pDF-#@k3enz
zB&8hEoUc<kpQ4oeHRo#5+d%>jLkF-b`yB*4syW+WsrFBk4vuKfFOm}Pq70tWoR<*r
z&jdWHIo~4SZUVlmRZ+v)S7}F4#~<Nu#zu(81h!TC4KROH*qJoGu&QOGfDTQAftBNr
zGnhsOyMHyTGX_$v2Fdk`L~zeKf?>lR_;=N(JCWS2NZ2fTRKXr<hVFPbh#_T5HuWMz
z?dv}WcZR3#0k}7t>W5|9FMSOv;Hmc@<=?WYU&MrEkEYQ2JoPc8d@Gy!Z`45XCt$oh
z_2)==l~T7*jnAMyV^2Mf)}d6(pA?^{604Yol<GU6mU0;6tVK|$^|9MQ$@5x-Oq=jG
z;|k;@IA%A2r2LfDj*K`>H_|s`Xn4orLumO8zXX<(j3aV;h9pFJ<^5a@t<CHvfoI!b
z6y=MwpQ%CVK`?%IDrOVkVVGI@SG9Gh3NQ3E@SG&R>oR$tc@U;n{-AaMIw7mmicx2t
z4#)3NsMl7poYe3={LOg?F!>uytD>ZemD)0(R}#l<gcex0)(y-Cg`IhjlI9M=iYnSQ
z%1ax?&vun=vbBoN?I0z8Rojo`Pb(6U#o0ErYDMrROxI-Vnnkzp+`dX=_koFzsX~oh
zZ>)mS&-n#RrP?u5djY_6h`Z>g#988-Q!-D}=DY$(RO(!!)XMO44*q6*4q0;HW*nx>
zX$imtNiC9BP9|aVZb9F2oTb@dw8RY&8I`JC2I43&JV}|%JCn#<+BMPwLB@sCt^>&f
zifm?Pj22a*I&*svQ^$Gd!`cMfqr{RcEgJ7J3`M%*Dyr;57|G^*4X~(Z%^T>njuMRq
z7MtZ<BGkxLwc~Hbb5z$KkeF4J;Mk>|1@yTpiOszkJ;AX@qkOeypgk%{H01oBQRS|G
z19hVo;$)jZGN8!Vf=gk2jvGU;I4TTOR!+4C6nly4B`STvy{Mn#mJ2bnal5qWMV#sZ
zP&}-tgen%)qcF$qwHQaKO31L7ll~B7KT)J?!M~xGI_@Ze`N#}QIMts)QH&nKi<KEJ
zg>^eV(*!vr=`>C{A7o2&NG1JkKsx^Av-sux%%a;keIF2sTzcN9l&R?23*I6fKRBX%
z35#CJId28m1Am?KWpr-GS2n8g(V~}e&ga1OMlL5Wj*l+fhBBo~;m4t3$C178nWXS`
zo{<xhU4*}^US|G{jJQPK(V<V7Q8~}(Qe^UpOh#M*S=0WjS<a;9WDB@}Ki&YNmHAfi
ze7}r5fAhb}mo0deOz^+H1Y3|<I(U{P=qS_iCwc+N#ul`a*_`}7;xwqhxGFhmBgmE!
zsjya>aU5@#!=`11I;UC>ihe~kleRY4tZSek$Ge-sz^(O4B=1rr>_g<CJ5KYCaXYYg
zQVKzAA?;{5EZQ*IUHkV8npMyU!!@$V+Z$q)i!JC4wmHGQ2aC1C7oYz!x?|;Xj{EV)
ze)9IwH@>;z{7*ual`A>Uee+kg&;8=e_6y!7y4Kau%b^ROhE^?0XQI>!8>S2F!yLq~
zwW#Phlx?*Y1re0k$e2oO<UQ8fXem_7<0U*^Vp|065*w+o#6~5R*l1d<wUI|z%i|?H
zUSgvn>L@cBo$5h&1HuLy4!W>yJQEtlYi%?G)!Hat!s8`2s(6X*5l-_B{5ICwXrwEV
zztjM=wy$yw{Vt|K%z6TZ<gzU}ipJ-WU&8VaQp;43o|_&38)gfu6Ae^{KTgOaig3wh
z5HCRztDsT0f~JSXG~ZdtSKp|3ctr8A3YsC8K80COyBZYDqz!E?RlB(dsuxkV>^aC-
zG)QRcT|`DMPthP5PCaXxfTZ~wy4qbo2i@|^@G~iC?TQr8r%zuri~w!Ecsf*V{Vf36
zBjFXJNOLcSbn9cqh_VWDYbwYyy!cv_rhSM+$=@{A?s^?*E0MxpLgd5R+JA<Ct9(qm
zkP@s1PHtN@#I)K!b70rDRU`dOs}s=vZL1~#)eGp*Hg_isNLwtx!`s|Dk(0JWfJe5u
z!=$Dr0Uq7v-U!RoTF7Fo2afG<uZ7^+3R0!@!2a#-2&J}i<sEo;tNSYGQtJ}D$F_M$
zPk2;{q-ON1tqQ_zZGDT>fvc7Dat|VHdn6IL!{>pdUF$=YKH9E@sT9_>U#3&rT%1j3
z-uq+%`G=h8*bXcWsk!Sb-Bbon4T?+a`4p9XE0SJB!+85-dKCQ+oLVCaE49+|mg4Gk
z%d8(!<sI7r5^Zt2wpMFqovL2nQOP9d&m|AJqLpY0DH5;8$!*}Y9@vE-h*}}oU_EeX
zn@`(lh3d2w?Urp!!{hj+QZVvW*W~4KSmbd;<Z%?t$b&O$8;SR9$xE*4mOS2L+gRHL
zGM!vfGB%~M&VHG2heDz@oe7Ejf<i*Moi|G4lP0A9UWw%Sft7i0lbD-?dMznv)jkhK
zPR3Eb8<17NWgy_N##a9i;?wS@eeBg+;dxJcfTC-@fsrILx~`#J1e+RH*Zi6wHKJDd
zk!=@kr5)-nLQ$pGBd*OrG1hYeKSk@wPPwyA5k5E6W4t1u#8wH0z+JG6otWQe?gb9^
zI$fLZsIprfXF2-Ln6Bw)YRfEUb*!*oWcQ;OYYns8Ka6x_E~9WOiYtLdmJ|@xv?|T&
zu-l1ABXKJ-IkbSRRD#9+Dgw(?{!o{u`KySd{9NX*sz+kQ0z@!YE=PtcyUL=R-1UXX
zeNF`?RZ9-uX)bj!@~W!SR?QaKEVf@fJIYI%QH%nb&<Zmtf-x&gH0JPEWeLwHlmMrn
z3cYwX2|YVQQJl+qW}yUl&sllra#>IbE6QxKH_aw_=VsXKUG}*qQO?dLP(fYx?rN2c
zWkn{}Vv#HE7$DE;E<5FUu3)7Clm_Nya^qr~bE8_FCz445#f9w>a}!d&^D~*)#~k@t
zEHEeL%1b4D$ef&&YheZrxda+}`c3c+gfU(`o1Q~I9|&ok0&zhO0-{}LMqswqrIFUn
zl*U$%{>d81c1in~;%NX~T&Hzbx$MG7mK5`bAv0+rJOn6KX-n&108O)9+|rwgP>2W5
zEmHw>3KtrcD%&E+r)$n>?&?pqa@_K{T4NPqHrVO@z=~qlSTzS;-bFkD%*E!Ma#&Z|
zwZ1bb-w&@~RBkn*tFk(>pla@H{R#9?VL@<FVZ_-=rC89>+xG6((El5`Ew<md8&7Ii
zA7}-OS0!MgF^fvvog$5*z6{H4#N`($7}>G{teSl>bhxI8byiX1b@2!)*i8|VuE$(c
z(X9dkH3lUUGrJWdq=l8>SJVFDsK75jU5{~0`v;y?i*1jLj*sBP<v%db_}IY6L^P((
z#EC<93W@XGMst%q>^(F#QQsd=qjYsH4@Us&Cz0SbJ&w`wOmi~BUYZcUvA=F$e4N~}
zZk&IN48?qru_(@y57i9=4Hae%a@NHKg3i(<7S|19vZ7Oj(byp3@RHLwIGM_z;|WAz
zp*WGJqI9wn1R@vzSTweM1m{W>LY_KRV76GvK*vziNt{r`DbPf8Vj#oV9Z4tS1KFH8
zVPq_wDGKw6MLDI`qf9HZSfd(dh}0Wa-DMrWd%LxKw>7=n_L~2;k6U-_Kk)L<HAB}8
zU30B<$L`_(7#jNKB}G>24cfEd`18&;9()kcwc6)5ejD)nJ8c(RE}Xt+lkFw^ttl$9
zo@sLeAG9pcth1hd?j`Hxw$&CV3^3>o8=m&|)<9oRyV2qCcX`_le@}<k6Y{tEyZqse
zj3`3>?%po15ej?4eIcW_f%!sVqc;$0_jd5~zL3AC(+GMy`?@?qf5;Q|2YMJzW70X!
zt?9IZgO#HUEGWd=;qURbn@^-RE}Nvn?8)8Ka3Y!RpBUuD_<PoQy8P{iC)nB7?d=JN
zgm6B8N7!ihhP}aVe-AHP)`TCi97zmpF-Q>8AycYTPr&ebLm|JX#|ZU$+x#7Vh*hAn
zjU2h&6YQZn7OcVYjL~ETHysvk=9aL%u>oc>mI}v5V<{W}#v$D-pEtC`h(~ec7(dZC
zLkmFjIO>W6s%bhPDo)NO#mQMZI0}{U)2}%A8&0&x1|t(AX)fs6Uc=KB^m^Jiq8<HV
zKNJ>1#d^DpZg0E4ubT}fW3leoXd=0T)B6J5fpDOABO2G+tMo={=<D&cqRqnrw5zAx
zX!Z8A`MN#9HJr<{4yMA>*Zadhqs!mZx4{VXbP03u27`g1NmZ1cA5FgjvT$=#<&5+j
zW3g?yC#y5MY;3}L<DQAp{#eqJOh$Hidwb)&@X&fsFV9Y8LkiiJjHhE<CAq?TdOTel
zLtbNTZ!m!RbhBupCz0;RDgkG4qyDi}EGhKoUGLA6yf^6Y33qwddAp1@pQopjtaorE
zGL+)NkdmbwMtChVa7LI8sWW3VlH7vh(3}X35(;$nQMaH@6bvJeP+wac?60G*3knYR
z1$)>|VJGIkVLE5tfTosX5stOPden(+FE1kK4MEGI2eYw>(O?V}i;c#{(xG&0T!<lh
zxiSw=TiCx&F?R>ndCh|W&l3m9`-0vszp7u`#x}HcCr`tm(H7_lg@YbwM-k+X9I-Jv
z-0!_&f)0eoF(IMx6$LD-ueaA5WRMMQ%*ToT5o0`^G-$l$MP%$(^l7Cyyi;H_J{BQ0
zhZ7?)^r-<s-xCNM8_CpYu%hunb!e!!*T|_|cKkAW`ntM8y`DCHwmx&}UG~|fe9%B|
z9mq(e>Y6v&@s8NRhrG#Me|dCbd?bzo=rNp}7rh~CHGMr?G;ezk%ptcgWG0)OqxS=e
zanX0Np=EUUL94A^8jby7*_^&Wm@5mm7J!lqjp+GJ3)3aJOifzAO#0A|a;t50prd#7
zGGjcEqA|wj@AUDEFuH=0k10LtLqW#X6O4_-`THK!A%D|*88Y2|c_bPYz1}O&$Vawj
zJ9YqP9J@VT+3wUqGZ$G_S71HNDYY#!F1XPJP1$EOIn_HDsP=)@Fszw|`SpR|8ib}U
zmF<00m+Z?G*x<y-2pi(#80A4#4*4(fvS>0f?!5w28))&ef%Fb0`a=-j8Qd7Ir9z!Z
zs1G_bsB{d!fgUfTkuZTdlrn8iOpHbIGVOp6n5|<eI9?=0-jt3(&otsuCjPNlj%@19
z6nx6;j|^;?7&quu3OqO2@A(|tM!kwU?ImqF#gU|9M1j~jTo{=GUF&GnoZK;*QL(nf
zh&P@;A7UD0<HI{r=rkiEv9Y1_u&7rT2I0)G6OtwJq`uyEPuSb-2?zZcSJg-en+$te
z$t}U49Y;=y9J9z+UE|^;=ESYBjOG0UooD5c*XO#nX7kL*A>KU_rv?|V+K|;2GkQCG
zD@<a-PsvWuT)&v%3Te2B>{E_9X3f!Hne}@N{+fw62I?zg#<<XDX8vCv^oPAh2Yf>o
zPqoJ*!ghmRnlp0d;S_PF*@T$Y^Trb@P)<2C`u6p#=?Sdw$q~76X=umjXpG!uF%`D-
z$KmqFBFRbq5)3Aam$G1l$aZoyAvVzfDPR2pOT~+5CPpI3c#2OMW(GOPH<3#O6_Fc}
zj76ft@Kw=M8<*h;LFsTLp-E}s<YbD@PPI3+kY^paAh1vm>!<#kow-IM+uJciuAUej
zXPIdNdf?@f(uoQ4CN&=1CroR3%@gSL_K4QOAhbQree+Aa!2tL4IBniw&$>MuFiyd(
z=`^~$J)L1OWKWtkawn#pd2J(x^zPm^vqnw+%HMh?jU1+O#tqeLTN9Yr#Vc=Elnl-=
z21kW|hKYr{T-@)(EZ5WS3APLU2W4fci<nJaIz?1qh}H^JZ;3NmPu3Y<I)K8Hvd_TC
ziGGsz;BQAu(E7+cs%7(NY{>eRn8&DW(NzL{m}fdfej$$U@NbZynoV>4UYb|iy{&zn
zc!LsVIO;r<RR>(mL=^8%GL5RL2S~Ri=dDQDrkUdXMkHgdITHpBpVt%YqSZvZ7nVlV
zmNQw~Fj^_KWd~f%$UququX-5zk??*Qy3+_PzEsbGf5+>bSxp$8)&O^h&BHGzQT~UQ
zk3(X}QwzWj3?u=y&@?)_Ji;f<Sg>5kVoD-gp%=-y+Ag?2oB?pvj;1HZVk?JYV=)Xg
z03*qP;g#Ygb*|bWS8ZSk{uaAx(SkJk)DBLJ4REI(D7?ODU`y?IGLeo^mARyRfxsIl
zXYnc8JnQGR`2t<u&>FEsGC3Gx;!+lw&!rq@u^6B9<|{6MTE}v;pc=!M-3nFL<#$=i
zoE(&Pf6xo>AFC0msf=u9Yf?UN`fsmT>5Ws3re5pK^Yw*umR+ZD*MeT7z{X-R42CA}
z1lHa|E)U5nCkX?_ZysfonYVDS5<?LBDh6J>LW)~jiX28>!ar^jXBej8bKxyte4ySb
zS7KaB(m2-H)OplAVCs|EemBLd!srE0n0b|7h|V8OY|CzM<#ce_B;+=MsIc2KWg|-;
zUrgND0a97s7%dvjQ)PDHC|&u&s|}3E?S4;Z01Is+voD~w@z@x7|0EJzQZWLWhU5GW
zo1IXlPWj$N0<RT|ptG$Mjm+P1CJkJ!n6aMdfOBWI5z|oIXj&9_<J`A`&&h>??A{F+
zRk19m8C=aEa@pG+<jbd=M&=Vi2)&4Jh4pOo)6P;Pir1S_uvi33M9?IHr91<%DdO)j
z`LEcLXzyzaGid=GktFx+Qy4bW6Ui~WXl+6|;AWRM=oKzUn}<9>?61+1%18_jqBrCi
zi%A7bU@%=+oJ#jrEJCF>nYDzB*W5}g`mT`Zgt?o4-Ke6;x}`xjHVBKPMGC1HZdZG3
zjP1<$wOj?+Q9rx+A#{_s(a;m%rXm%uHjc2-;0=Z_a4CzWsW9a2##D^SFzW{@nHHN@
zJ#soHyHe~BvSS3B$Fv$#HnwmPFI4mqqazsTHmz#J0xsK3@f0@Fusy}N!=Rcml#Zk)
zQnDor>8d$@%#*Ugk@S(TfoZEfkexH>C3k#u<C5hL-vxI)`2fK8JWR8Usn=mcC$lC~
zgN5(|3-6-xJy;ipSMo$Kl5;tPLu7L5O%0&N=ge;C&PhAG3@?8i;(3}il(pCvrCaGd
zsVPo*1(|IK>H_rAymWjZyN2POzJVJCc;O^A=dme|%_5^g*b|H>cSDJE!0nB1Pqqbu
z!M<L#cf{=_&sx=Tm@1#6NQ@?(U4d4NgWcH5@pO6(czeBlVZL-ip9%;2_)^{<!lpLg
z)8rEIH7h)FzK?BcCrteT(_Ul&1vf6{Goal6m1czXH@3SxeH-91c!C=-E6F?yYr2uf
zhO9)HT~_dA764q#?DS@OBPCC1_pdWrHyV7=K?6;8y`EJoG!f_!gC|p3GE!J7`r#|m
z>m3UXDy9u{)9yIkSfE!X#_3iK?R8QY<WmfG2juuHSDLi0#cL&#V#z8rlO6ArVu)86
z7+=lx3U^a^MdYwk<rI;5uu-<o-@zG9s4XtEMP@O_WmEe?<lOMRI@)_R=;j1ZrhP38
z@|Y!8V?z%{O>1es6FQVkY|9Qgxwh)v(B}2FV_L?7POjv5znZ|CQHRE2QQpC@pN3lm
zd=-@#iK>Z$?v_F8aLl=Nn$<sP8Q!=!Qx|xYs2YDKrF>7OTnx1HO{KwDBo)VW#5dn2
zZKeuG($tc9uLKp9%M8$lau0HhBRfXWrl_zrh(z(s1jjN87#NNu3pLkr-f?KZR`r^k
zTU_0k&r|BQQ{Du)gvx^V#W&>mVjhhi*)Db$<Q`v+AH}sS?oZJAr%*pG_)6QscoKKe
zLKFR$W33TRh<QaiK9dL3I1MV?ma^Lp{LT$;UsA0cXYv+L{GK1@>+oU+Rc+gb48Cv6
z7n`axP41#iv^vKSO&dCVJD<wQEE%&4Mre$?0xa93d6BAxRZq<9{hGCuZE^F?jzN};
zwI!9fwl~nanj8b(dujapD<+fK;)75ko<l6X2YRiq@Md@k84s6AFgvGId_iwPRY}aA
z8H1#0lz7@aJ!1Zl_e{M;W=ElLGVW2p`7!H7N=h5%qJw**EIl00?Ry38t`6}1R@z~u
zlK7w)>f`zg2f`bBvDKHg2@Lvb1&ZxIq8CoQnujqmwtGWZPGS#+yTsUPp1=SgM}^ET
zvB~I_d$u(1P8>R-c@)aFdW*6i7#gBt8Kz#g%rLyu{4qF^h`{~jH|7>IKGHBO;Ir#0
zczb@_I>Kf}X2gmQV)HqfMDK!ofQ%-R#sJ>oJUYhUBhbBMxI1*m(kpH!<%lP1(JXdE
zAv^Ln@4(ZLOg#w3;c4^9E!*sLsBYT?y@6o6H;65DezQ#Zqsn_E8*gr+9h8YNn)2X$
zW5lMLznRT>xj|KURtpBuD5m|co${H--7?fzSVcF<hX9Khp5pwBf+7;Du|5^OGKWw-
zJ9uXfRzde<*;Q11JwlX&r($w0CE#bIfS@|I<1-5SY)0%p6@4~`P(eS-5~}F`<Pa*z
zmK9J%XJ>1Iq7^hZOQ@m?atIZ4ewI*0FV7)V&{&pGMNO(j(Mq)@jRBjZE&%G;S;bq%
zG7rX1qK#CzIr|nFCS|A|9{rdUNtKvimU@nMlhmbZE%jt?KD~On#gOHwC#9##pVg&y
zjAA{DFr5_Pu!>>ej3vie-59ps>hKcm+BEis1%zjg>gZ94Xl*>o>IfQ+q=s2tbjKL-
z6CoyEp0XA9<f*rt0Rw4C%;Cf!W8-+7qK>;cb!pmEuftdpM_PivguE^`Yz!tNqcLMR
z3SI>v(}4jawtXNro;LWhT;yRy==lYahl*c507jG-8I2EsFM%TPQ^(5{W#B#MtZpEI
z8zPu8>Y}m!i6JmY#)jZsNiaS(m{9RZe}6KzRYA0Isp8O}io)MiQJGVIll?DXoCc!j
zwCJHMezjQKGUNe&cc_SoU!pZ%=K;V|?iv2D#SJL?eH0gmvOjpHE9YtiWadv>+&n!k
z1*+jt_9vge9ZB!t9Xm??gBCX@=BFMIi*g9h3d<0W4d+}vfbb+>Mt){>i*!7QMpv~A
z@su>zlj6rigZx^DUm{WEtM?dH69X>w-XZ~sJXD-QdM29Q8Kn5tdx9Rp=@PnHMUatS
z(W!6?(&-&U3RdqSIwkqXB|nKnaw_>Hnq7$q=~VHn_a5;#X#?LN3G?-*>VIt>zj}|-
zK|E(k*Dh%=#9xj-RlefC1vny+>dL%#s+x##;TiFqi)1xl^!&J)-(BwJ$!^Kd-+60d
zz_|&2sql6{W_~s5^;QTvdTY1&QhW+`Cn9Ek^`5LEoKvGqHiH20n=k(%;D}%8U!U#<
zgyW5pf1E?vAEiHqxgQZT|1Qa|zB6>OB9vi%`H$rBACde=Wce3U=B6vZ{3A#+mrt+%
zrR!asSD|Zx2r|EU*|8Upf~WeqZ4!}subBF7QNF(O%l`#<Doy<Ay<@N72ozoU^!f1X
zpdp!5Gt(Qa={ojz^8W#}s&5gWdheF{u9PktR@t{IU)Adrc+BOi?<t(ZiBGx|y@0Z?
z!oP<EGrxN88FLAagNjjxicf_lU^ep~bZzF82VH_e$tj?0`3f!vGv%f9cUbb@?-CTM
zpDX(O{HDwD8x(;I75@c!c#@Sa6`m>i1vdNpqD#cxSrm^cK80R11%K7Kg5uNk?6>L4
z=U+Mn{})yWrqAYexP1P$Dfri}68!X*DD#!iACUa{`unv<aQ#8rk;+5ytMC%ZuhLcc
zVnoDFD?*u&2eQ}{{4Wd$hC`B11^N6m%qZJYG!!PI0!rmE<bm7{@>IkbhZBN-gEaK{
z0GH!W*}bw$8lMZ5e`K5B@B16=C#VYl(id7<Ebd8(R*d$_PckQ3A&K&Ta`8o(_a)}y
zF{@hLW)zQGF(N5H#GGh_r=|RhT)d4bzabZ2iZ5!o%_ttXva-zk63vM?XqK73bMfU&
z&C9v?ip;^zT>RwkSy4J(XqNfzRxZ7s`JP@bemYb0SuWm*`N?fY@wk;$W#%7qq7`qO
zSMy3PekSIL&1MvjTXFJK%>(8{E52-}#`j!2tWl1ex%kQ7C8Kn!={sh*^yJM?zO0kH
zctK-ja+1qqv9h!CFp55JFp*SN=9+^3+$r$$CgG`H7qLH{&QGAZ-Gc9=DZh+hy3WI&
z20f^QhpJywA1{<oJEx!AUTcA;{Ey1jGeCX~;+ODvf0#hX-yIU4{~p#`fhTz$mF-CN
zq3dq^X$9=(TfkHPoAdI2p3_gZ!>Q_ZlG9ITr}FIieU6`8U!V)>rwjsXY_c7?IDYc?
z^e&Y6LwR<)oL9VRQh!-3@yfp=Iq2HN^O@XV27xd1UA`-TcM8^=-<zhuKhE)JN9ot9
zx#D?=&)>~>Wr}<>FcvDe26$JFBm$uK85Y8KPLU5Cz;(hCR_h0qCA|!@P`TGnf&Vh_
zh06T_@P)ox_=_p%PXS-(yNkuv!tyjuf$!${3Vi3&&8rlX727lg{f$%L?**Q|_eh_$
zF<)Ph_-FI*Uk1LA-o7&h{vDprWIKOP(l=DN&Bd~_;TIIjzZv*K-<e#^>CxY1KI(gq
z0gf*)Z{|I3EbmR{-E~!89_(5spQ7=yfh0atIDs$W4B+1S>NY&#PrKj-%YMDlNRJu=
z^!-M9Rz-a0(S*XM>=MS%NTMGP|3&fOPRfW(Y-iZY!m-43ELzvRd>K6<Q-Fwn8Bc7H
z;Ve%YXOzS?TXbS{bO%^W5Wm}>Lu%l84Nte%z(b1k4N5M#jLWJAIy!K>gPsV-`4dp!
zBa|8HXewb0(-}iNMwFX(G#7z~4RE8)Xy4f5>GroV^H=+_&!A+V`pW;%Vm9S(f3`9E
z6+piIs6GyuEhcmKx!?`NoZBgx6A(FPN&cP>cbZPlx!vlsgY>CE`5nXjPc>$V=-Y&*
zJ3rZ~Pjw38uYV*o_j`%-JyB(RxnC2_`I2H*S=r*JcmmcWs#0v`5f=GD%FGuT3+r}@
zNBjz8EWWdu|1rw~gj3tk6kiBc-^rZ(K~A^Z^!>=(d+%s7`H4w!HYc|Y#S<EQJN*9w
D<qyHa

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/example2 b/3rdparty/qpOASES/qpOASES-3.0/bin/example2
deleted file mode 100755
index 51984ae9bcd9619f0e177a2dc552a9b7653c4f90..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 31208
zcmeHwd3;;dmH&N`7b%YAXFDrNKuH~%ka&%gI3bH=OO8Zj%aQFY5c;B6iY?-;S`vq$
z5E4Q$C6IKOPKKc+bf!#ZKGPvHr5#$@5=aa5SK6U;ffh<L6quN$4Pj|0OXhp-^7JIz
zl$lR|zu!N+#P8m7?m6e4d+xpGE_(02>wLi$o2D^!+1MIJ&}T~<IZE6a>+R;C#5FJ{
zYh)L*Pp|@D^6}TmQz#TM6`qr#Rp^j(c7%EOYp@AKgRPNAR5)9pnF^JL6jFYft5HTp
zxN@ohRmhl|BZ!Z5eQXW~5pLrV6`q!QFj9<PZhj{;@tu}@rzM{Xb;+kfRX!>k{dLOp
zPFo`&f*UDdUVhJnL`zrui>WBZhs!yLu#rbpxJsaz3Kic5@R6P$_0lC_Smrm8T{86}
z7*st}_r<p_tFP{hR`$gQ(!-U*4a+K*)mJ44tLk{UNj}l8YwO~S%8oMtk<L@_N2aFq
z>A&ee`)2IE#|9EVIQ!zvFTQZ<#M%F#G@>ECLi`=l_d3|zDaDSwyyBbkn1|9x)<yWc
z%GvyjZ~gZ3*EIQl{_4Uxx6Gcr@~?~Pwm<XQKd=AoiC_Kbw_ket)0<~sb=@7;eDmPz
zuE|rL|I*7p{k>~XVCOf$I^jj;caM`gZvy;gC|WiSy=?;g`U&vIV6^e%TsZ-K-vsz&
z6X+S90Dlzt@$y|Xfj;Lapl_XkzHb8hZ%u%AOi-@D3FOyJK>sN;B%RG<N8Q_yQ|3Ah
zdYxUs9?QXB%keYW@f>^%_)^d-yz1xMAm2TX{3!5ctl0FUD(4XJrL2T8*{@aq-6`q!
zmNjzHm}EF0^;G#PIeUO7`6r46{iEQgE6wE;v7yP099FlrQvMk!AME@JaJ-AXDe;QF
zgv&?2lkydwq2GUcDCFtzbx>ztQdPqYqo;pxz(}SdiIib5<FZ{wIMx$SrecZazDP0|
zOEM$S*=j^%35o7(ZSET!h;>G`_r*lo_$imSEth#UY-mpmg1WyxJ`hj&+Sf-r>WrNK
z;dpzL85y1x9g4|Y>)S;s>y^0LWgUZk=~R4hz%vl(+mnnZYa4oEsn*Ew`t_Z=;)8t=
zO7pe1br=~bzVHO%n`?dXI7n-2{jp>+9vSG|GZgb#RT-O~QfXq1AeAevC^CvP^bV%D
zwLjbrA5z-7T9kHUBmrH!W6guRWGuJ9?a_`U2Gw{n742TMh$?&;iZu}F$2}Tphki!S
z@USryOC$$j$ardxv8#p{db+!fB+=J0qibMyd?1Q8kxci;D6v}#GKLcIfz%G8yLTrW
zN~e-^>+X#t7_u8ory!h&Wy9)^#0QuWi$+orBy3M61s<q@D638<6Eq@KM^b2UmD?l9
zn24^d?j7upRmYRP@t$;Iu(~^v+8G}(b|<U5d*ji;M7(-?I^Gwp?oA9O_av*AB%(uT
z#e1q{vr}Ou+BwNjuB;v$N>wKYlhr6vyl0S*e>#t7Y*(z0NL7)#lWAiIS~41c2<3=E
zrF1fe=4DX%RLMl~iIy5YTi4udP=9OQyxG$fFlwuouyw&e6F{KrMck-Em(0ZLs+P)*
zYr|+uzQ)E!RI@kp2)T{|jMnteCPM*<JI%K!R>6G{gTKQ(<>Ea>#Pt`%jEgwmfN!R8
zzXpFly>}<r^4Wf*6E3yz^KhAVcB_TnBjq8PrDn#RG&8tE(x)#V*v9UW^ye<xMr3?0
zhHT7B`9;!kx{1Zyz`cz_G<UGlyDfMrn$s<?;FV9Ngc=K;@=#ZU1#g{qHCphLx4Qfm
zyc#<wt=)o$LCtHk1wYxuGPccvFSg)&EqIp&KV-pU)G@DN3!dsnU3)F~QiVi#lLc>`
zCmgci=UV8GSnzTx%TxXPUv>CDm;ae^H}fAjk;*?k3uyoT?>SBi=JQJvNIE}1g?q_G
zjffH4ONpbW&f}VY6~R=cqfc;nJHa%Zjh^7}<pkRaKF;AV!BnNAM>%{M!Bmx_hdA6s
zFje8`UJkD$n5u4ch{JURQ<aTw<M1T}qiwR$b`H-YcoM;l9KL{Hs>;zC4wn&3RXFPA
z@Faq%>PB@A+X$vA8)Y2+V;Nwos?jqa05Sh8!Bj<~rvSrt?*QQ+$tS*(hddj<NxFZF
z4DwyFf~U*V+125{{?$g5@ob4+97gCr`0a`skoX_Hq2e5f=tA1mHIxRN>LS3+C;A;F
z3tq?H9|6x<DdbdMz{Ap8D$a1`(%UN>ka)JFG*~gj@58v?QgM??JtXodei^o>yhK<U
zt*D_~p5@rmf3DaIGGz5T&U}RPK=e@&xwGP!h-|HB1i^h3y2$d|6*E-Cts)CV<i3jI
zg5~ck?ni`5_XI+^xP%itqOaa5)LPms)$11FoJ)mb_f<S5IKEx+w21tD#VHZFz2dZp
zY^`XA$i9kgr(b#>HFNNqibbbi<uPwXqyOO6ip9Z$w^SSjaq!>`6=V<pK^V(_@b-$m
z1dMnq9R7peiW>jH`?ysz)#R<10Sb_8t<e1=c@>X(D;855JAEgY-&xU1JiJPIwtodQ
z%<8S^{cav+mJh-f?0&kPe&sz<&0DdJbU6hK=|UFq113|EIwyE2%xkLE{t?t!J-#{x
z6RNWn>O4rMJ>8^YfsyZ>UJ4*E@<{F5fk*S|kqVdah{J#V2Sk~E1tqKe92$34nS&#*
z2S=Vi>nSN+TtWA~ikj0dk~tFwIQ_GC&z}#Dyyf{0S}hz2t9^#dGlzY*GTS_lgU-MI
zq{F}e-394qALKTEn&ZB|z_b4~E&WLAfwxnR;K(0NdmubG@?WP{AQYAT8zPil>-56}
zT>m}lcC{z2xYD!LbEW4~C;cNQO^TyLG4h+!3zPsPp8i`PUwO9nwZO<vKII>I-oO8a
zGwq#$M^Dg1-T&xAv>xz3`Y28FPq%~g%^g)G7u`S>LaDr{<bQW>V_p8r;|Tow&$;{~
zFYdNod6Jqwcd&0>`5+~oKUH$kUc%i^kfML+oE`N3k%#>cy}Z`1J>mb!xipw*O$O#C
zQ*U1BA9+OZKQH<BuD;&oMxB&)f%EjWa8Or1l0N{H_Qpx_*Z%#hZz=*o>Oxb6DP0eq
z!mbJE&p&bH=wE;Z^)Z?hp=Xn4<h*BNVB~F2S77Alp3dOG`85#G<v&>2fUqOD@Wt9^
z{13fvr;+3Iciw>s54@JTP#GaO@^WzGZ7*b;pYkj!QuD8PCH*oDD5?rP#>pKSqrGtw
zMWZoD4mmV$>Cc|_uXN9kO=6SU6Y>5?pW#j8w$T%IuXZ!{{MwowWE^#!Su0|vWOz=7
z?1aEiX%r!o;h6^oeCk1Mmi<!N!y)HA1BJQ?VZ$HKpXWUiA-R|m`1{QW9WYe`I`c5X
zz0|33(f$Wr+SOsE4eQ#h$%T$XTA@z(+4#E`J@67Dkk|P1j8!0==x;iA{ye!-=F(eS
zGcGGB+U?lO)?RSwB}*#ikqF{Pb7Om<D-BNSB3`<*tJ{NIlR`T!8SO$JcpIcq9-F~O
zYt+0cQyW>6OTX2&!8PNcy~*XiCC}qpaAUs5RkOcfo$Jgb`)>OrS532Pfyd=;a?Jp3
zlgr^zb0gB>ar`}o^2HF6tlMlp*Nj{3K9~Dop3k-5mVCdf=EefQt6_g(tE;ik)d1PZ
z19_S9czLFvyk{T>o+ax#e_oq+z>a==n?3(%A^W|3)OLR%d#`|ikwW0VMz@czk1g=A
z1wOXG#}@e50v}u8V+;KMwLm-833MID<cqEv33C89Ic*#h(w-e%DxLQDtT64-(WT~m
zUyv|uXwap8XHa`l>UV^hBH8?%m9}~4(nWw@QpEMN{5@68BOMa|)(7VYDSE`#NP+pA
zZ%u)KY2M5)dm|4f35fk0=9qM;`Qi)#GWoNxgv4);9B^;rftmvzk%mz7WwnQ=_7<O>
z)X3=-{+NW7-Zeax;rDTIsr)gO=jB--=`oe(uvC|wk?`}*M#=gACQ!d)=jL{g{Qc+~
zGJHgazmVZeGJIQx1;s+|X)?T6hRbBwBEyX`+#$nVGQ3fSUy$KFGR)Nro8T+m3%a(a
z2U2NweN{tMePvymL$%k|)>qZkS1nm6@cbuoQLUnKRM#kxGIM`A%gyQ#+Hf96I<za%
zzVqHiW!(er#vDr9&uHuVXZ1ka9fhx=sn|8go2Yrat?&*se0yHu>l|6|Hb>@v39TzH
zFCRaCfFb`CK(^P=A@cXripFN(QG@&&C<<v0gE8Mx;5ba6R`5Oq*q%aTE~xoCU<xk)
z{B+@4pFlYsWl*p1?N0&C9M7SD7QXvAiaMr1e&Kr**B#r@PYU0s_zcG^6s7P3in|@F
zQP#rq6kp)H8^Q{iMnH`-0c8p`4m3FLM&~HBaiGx|CH6cH_?=}iabZ3O+MPc^=PWGX
zz-DKRv~p-yK<GB-Q|RV}le7qcUgyt=XR<~L4>_Ns6rBSprvs%bEYtdsGVFYnQl@E5
z0QNdvWdNpY?Ev;U{}uPb8Cnmd?RWkI3{^N=+YR6*=VTHvU)v3WL(b2Z0;tkP0331N
zN<cLSjynH}fEo@QbKXfnEeDP}|Biq<Z6SE>cm6FIv!3gA!Wk!nEYm)Ne7?sf{e)Dw
zqwsN1%qTocg>*QIfV!Ka%rTwJ`eiDv!*Lr~<sK@n&I%lt09W9kIxISj8>u~cA0mXM
z+T_@S3R@2BUjis5+G3L7+JPcEZhRbimh4B4d5+IN1OmMq24ecNXdxOBrcuI!=P1Di
zBBsxSqME)OQJdb1MXX)-Vs}1IKMZ`n{vO(K0W0|}gyz{AABMvEIavQX{YF$G({F<~
zP45J$O@9CgyIu*&dHQrTi+p_&QVaA^U<!5m_1K{|qlGy2cM+eYUx>_#^hOwOvi?I<
zRk6McI=b|K1Xqdf0gbNDhbpD|CgfPAKa7E*T>lOfnxfx=)Tz1+&3Kx=405LH%OPQg
zJ_(x4)USig3-qT^yjl94h|kvFLSLGre+^n)sNVtnMfy?b@79x$KUbfL_&og*==KTy
zpJ2oay~M@XeEr9ee6hY4_fP7#LbnBa5uE%&y$$(ZqR)Yzi}bay%wj!?c%|-xo>ltY
z(4ksS15=}~fwr~!3-B3r`c=?jiSB{hsMmjivMklNLFO`jC+xOd|0HBK=vU*uLVpfs
zUa9{Q+Fq(V5nrWOLFd)_S@5pW7okLJ^&v<;f!YJ=A*SDpPW&*_+aUHN(~m>@?;#Dn
z>HAFoGIDu@=|Mn`GJP3J@EFr?g}Hvf^iM+L|HJfVAb-g8Vd(id(|-==M`Bq1G1ISt
z%>Q8e3$W1>;Df<_!t~`3_9WByz?J=!={?ZyXH4&asy}D?QKbEX>2IQ;KgIN~!5U9P
z3JUg1rf&f9S4>|8%4eAFgs^9sz7^U2n(5y}4$m=tIyCzY)1OEAPBHx?`2G{3fqb6n
z`%$ppGW{${^a9iW7Q$X+`Z18a#PkP%{4b{a(ArM}f=Z)k;xNz4OkW6LuP}WSmVTA#
zO$c6N`s=9I-!c70l<)UUp9c0bO#doM^g7cYM{t(uw?of2APgD3iDE*Dx0wEW)cD&>
zk0ba4)Bg#<Ii~lanC~$C(_r``7+`{TVII`VdrUur;C-fl5lVc3GC<gQrvC!Sy_)`2
z$h=<D??rpwr|I{=^7}RYENcD+O@9S44`}-9XqPu?`T)%HSxrBK^4+BAe+R8cG<_@d
zycr!3N_<Y!ZQ#2_)90al2LU0YTQ$8847X|e96*OOR{9da1<vJ%piG&L%$@h00yDFf
zeFg;1uM^;~mHiD0;QV*)nQUe2Ddm0wrr64^K;Zl)0W)l6HWKhH0%qIF4inD<1h{SG
zl#O$pwgLrOi@zx^Q6>U=nHB=(a)q5r95Sg*y9(&Ene-~>dXZ#>wF5vOQb|)d*EBph
z!<=1}DEC>!v|~vAnj)FXxu<PFhjwnAjPjDx(|(BLUzj99%03v{*-bu~ll%_J?-0p3
z$ej8C=`igDG!JKdB|0@HDMeXl<IhS`g#PUu`Z{_PN!99+yw)TU8P%h@oV!YK=Omku
zygi5HE@a`n<|LHnBv&IDkGYAA*tD0ZFni~~AUyR5QofW)y_Pii?7e7%T*+@B<snKX
z;|nuS)6gNDxBMDdPV!4cUdSOi4}Rz2GVpMc_mE81%Se%;eu&=d{OdQ-`;L)eD?n3C
zblhs9ygwt7yI)0LBvH0?NN!c4*tA#C0i9pD2}<+S7*fcqiHvyVO@9t-&VQ~({g6Q0
z4V0Xb&x&b4vOR@l$H8Ah;o`pnLFrG?Q?yA9vyp8%#V4)R9s>SRV%A|giEYxJ0*3qy
z$4;Hblb$4o7VRw1@2ezBW1AFs92T4nQf(@Fup56C?r9IB*-YB725ySmt^vt@BAEw<
zsSD^<MY#yNE&nKsu5AO&kV#jcLl?OV4wB2e2{cDcx|JEa4JhlRz9~xHanO9<q~rPK
z5i#A^G)}yGD^%q|e+|O3CNa-ELp+@m?;8OlCoVunqBK1B3?VE23d}WQ3jET{PI#={
zAe>HpujtkUK+fDu%shihq|JxlI^lJM!fa*-Bs@j}ozxhwT8uj40&gdQT#*l##ZxT;
zFW3p;j^Y(0a|EqtRt)fZGLBYGNyV$RH<0uWiNAu-0^6uLK|F>22s<@GNwc3u4=DC(
zBuiU}w0f0pX*R{{ZU$)&=(Qk{Hz^V!V$Oc_lj86P$}iY-8<&<OvVO{mTEDOtuR>N#
z=@%YFJDcH}s@)2@189~P(XF7Yz(2cSo~F&Z1IeOLZlcs40ogMoG6|7fxhZ={_>H(R
zK{ATucgB*ixj#Syam~^`feIyV&89QTRl5K=y74#V77{X-8nMf*IS}v)vguqojb=SD
zMK;y!!J<gKaJ(gPuQBgulmVBXB9?4zF|)e~E>?G4Lxmkcr=PVCuqfvzevFa8RiKeW
za@gENsC^q`kCL3vL+sQeq-HrKxb|vK0sU*0#AY8svvTd%NUlaxt@l-usK|=xu(|6-
z>dz>?jg!rUT`$5P7tZG2fbr0E^S{8aQD&gBbE>tV@DbIQDEIldk(^t9igrfs3RHQV
zDh7%nMJ0Uf{BMB3b-N2QKJH^}dOjyT1hOwEQa1lv=*q6YS%c;y1r~6sZ-C+<MI{B+
zAy?O3$I#v+X(1>5PmsNyMJnlw(Vtykxe#|*pAJr6iVnF5f4oGZb@ECjp`z2yZ$Q<!
zz8XP?m7Gq_xgK1x59fS|YUk?*(9|U7B+hvNT>p~I$+P2~o3|k|cd5Jqf9N{#lp3^c
zdJz}&V+i@f|40zm=!Z|jKvJX0T+nowC4@g=1#W;$)BaH1rc!mX`CP$Yu1BMla*Mg#
zLCE~_|4J^K{{c0@|2nTm44dxaB5CpTBNEAb0qMr(FQR61rVDZEd}xOyob)x2y`gev
z^XJ07x!&52HZ673IaN8T_d@)MveUAZO{;`~T<`q~jf=O|Wk_D7NZ7@rl->Duq-yQJ
zZln~_hJEtK;C9)x+o-$tm}pi^K7d<ffv>%TmDC-Ds*c&=kt6xq@l^}U(2Yt~aNI9H
z^Q*U>IQ!j|3x9{^U9y_v8gKdf@SJ<6dM}}QKuOc<Xii5LO@egWQd%No`FsR-dtTl<
zm_yhdd4GZ23mnvr3LG@8SnQxV@?st@;PC<n&5jEkWW53hWm4dvL2t2xvRKUH1w3Bh
zn9DJO@?yjb95f(Sl6Z1eRX}V&SnU`^uWdxEz(Fp3v4e)A#XMfX;{^_~LV@EtPICbF
zn#B%sdj;}NwXxWd<QTfwl1H=u8wkITzq)rIpGPXuFpf|)6_e4I#6X3i!~P6_KPW;T
zQG|8dK>Qw(STVWaVj3%IY4Ed`pbqEZj|=A!#lwneV60z;*|O#b#nM}0A8j`kd+Fbk
z<K$7c>`HV$`wl|eV?;&+xqSz<qAIrhBsy&F&YX7VJ)mp&ByMApcCT0k^!f8EYZ0I?
zUHT*hXbVYt?uMRr=k;g@tB}I_5LvD5{vz^R<7ZkGCD;$|-Mz-cwCckG*tMpIX*B{k
zv};Y@cBa(|=;*FBX+U)XI!<ZeTXHw&JCQ~rz<L2bkzV^%sHH6zSsdNHc0Y`yH3;4l
zyVlmAXJ{(~_{6TYmqR6Or2tRuTKf!{@KOPu*|qk1;$0;qpGvQN234-D7T}rm+JD6u
zrg^BX*$<!Cz1B{=P1H#3haX6<{SLad=Hq5R{O$DG#c1YQfEua&@VRs&p8jUqW--CQ
z+GPM`(YA2c@F~w^G$ZW_;#`J54-KdS{2ah0s_Gw>G$M-OnctDTek)o*^JXO73zYXo
znO=l0>eITkTA$YGw9}($;^IOs*9B+_>9|`7c?M+OluZ9nAx}yov;ux<JAVx!Eu@L}
zS($dR(86h_N5jO$6?j>uUnr5<z&wcIr6@!ldh<#JdA0)d1ev(F5SJu^JLH!(v=Kst
z0yAV9ZLg9UR^o-H%cN<p1{H8rb1;rp=hHTjnZnvKwysms(2g6Yc_7XF_eo?m>ie`x
z;LOyL!EQf%Xcq(kLa^C>_$Y!c_QS__`E0wXg`^QW?1!JAP`4!+f*D#3^Q+3F`CJJc
zhs+UL3FRR89P)3L*4m5wdli|?|ES3S36cK^k^d=?{~4M8DVhJ7-27ELp!{!Bf*(ld
z=6|is|8f}f9|W1r@fh=e&Vql?f>$krmOHPEC)RciiW;_Ei)CB2tf)^zwjEsoCgAwx
z7W^!UzXp})k@y;cqyC0h*(B1ZkSBJtbwd()xdj<GpeU*S!OFbD5_5xSDX1MRrf8K*
zu3+*$Kz0FB=jX5{;D1#xW0y@qyx<sZZCrK?y}jTcDY||bQp|yL{VfEkA$0vwg4Ezy
z96&a~`+z#ydIEQ+{e*iPP>i)bi<^BDc?9Z9Vh$)Ac<BCO>_*~`5~-a}<<o=E4<O*D
z_#-r5j8Oz*7X_h>P_z*4X#@b7d}=)<w!aP6crB912vaGIZ5Tm6ybIG20Wz&ro9`+w
zvb$!vx~7+Ew5Mg8#_X<@MVA%@%AsQgD=J!pbWC$(6m!!25@2Bu3W%z(T(i51iik-A
z=qNTNG=R)0!Tdluf!Nm;@=+n07APl<$#Yqtocdw$#fV^RT>*hgTuEM*bbSH%XTw0H
zwk&*^HP<@GDzDPk&J<$midM~x@|>pRBZGRf2SqUL$`B2>0_7RP>Ek8Pz8!L31^$_o
z`>L6w{RJ6C=I~5rkt$T$`kAE2?2M>j(Hu)2b215(d9bK;hDyf#(3F}lq+T=|8aK=c
z7LiQ1V5JOH(dL@c@BnGHG}3aONG1&w7q;xJbwhG1Od&-BE~`Q$V!kzTYz`M&lQVjK
z(nLcqfyORsJ*JJKm93gN8y;mL5VF|`#3fk>sJ6(8K-1S{>#Lk9TTvC7I;*BeN!G_)
z?_$8URoc38caf<5x_n+SRR2o|4+Zj7T74BNw0@?WH{qp3DAdECZ59Bn-Hl2mTPzpk
zrCM2G<BY#<;<$!6T5UODHWzJy!YlGwZTW0;Hazl=2r!pgC1tU$D$=^9libxS8Ra_z
z(KQ(x*-*5#ner3ZXh}u4^;l8?Hd$36xy_^lZ8>{K_G#$$G~M&vybsUo&N$ozaV?Y%
zPb^Z2!><Sl2nYb4Fq6;PnbZ;1<MpU=s=*+Spp>l?Ar;%K#T9)eBv8~q>%mT<+Z9S7
zHdqm;LNBIeHa18TJMCTkD52p3nr4N6#@Eqe!;$`>KD=D{XOtNq=<Z8LW9qdx@m`%m
z;uUXWS-pJUyJsL>y*-Y1_RLrB@XBBH7!v$Kk2mGG&@5s97nQD#4W|;3BsLy|f$FNd
zhla@bt#~;z(i8JX2BLUpy{D=dXw(tCem7WG)r$d^;?dX+#Id4I;U&(biH;8<imri|
z_*9hM@&tj95*UcahWqf+t3u?yV9N0rl+`<_CS5L~d2k?!0^s2Qyz&}NcbgJhBdJ8Z
zJCh|Rj0~hq9WZp+)OyMOHq-KK_U~S&VKhpgwNJ0I$6t8dUcS%Xzt1sx-TL)=_d4c2
zaM)44f2VzTpWS)LK783>$Go_$qTDg>n}@HzVgGf94?E`eT(#xPwvN*MTl4Jp3mrQk
ztH*K3wxM+Y6+lemh#|+JyEd(6wmhw1Gw!y6t#tq9U)m?_v+r_L_1K-MJo~0RbX;(M
z+OZjoB~^Kv<41Y+NS^kqe!*F=-`=**UjL;ZI0E*d<1$+bdQI5ZX?VQerchU#*J$ws
zf<CViXlwC#Is#3BV4!mgBZ`hdYkSaVbac{tPewcDvmKpAd#J<fYvJi#9f7uWM%cHm
zE9eOaIy{|$P#eQrqV#_0u2jmv3!nW$kU!AUX?T5|zHn=xjnm0ovtH$0KBj^q9lr4T
zjvBoBoa`M;q_(GbaCw2Yjh<k@Yk0!zx>|j0ogMsb&%VL#od)G?dHI!!(H1iNzK)K7
zr_JbS_caGv0zU6}<*Vh$O`dQY6$_T}wFKHgYJKt9Y>XMrEH6JV;|*!Jwg#<iAlVu3
zk0tRUF}><+^7-)IZ#;^(tZ|FtEmecwl1<TDs^VSVgm{;i-d;s%`75}7V<5Jh>$aiY
z@C3s?k9P}dE6^E$5jvpYjzlch8tWfS?BP`YP;00&)V>83=WADXCPQ?!d74n4ogvhj
z$7?kC+M4~Xp746k<=Kd4z|%JcI{ij4(AKrt2(<-8Q}Bhup|Hg!BrX68ZH5NCDXDz=
zwsUQ*uY_j`Ut5otqubK`+hYk&A`#i+Yj2P9%sV!D+POF-hRVM?5l_Xq1+ta*w0VMC
zI(){4_HYR0X=TyDw!u_O#t3--HyRj7#u6zm+_x!^qj`Hc(AF9BZS)0=X1}Lx9aZv<
zzDQ4!)01hX9!BSetm-%4d;dr;iuXqnJMsQD7mBLu2nD;Si%^FOcOuV@uI6Sm%$BYo
zDx|Y3+{UgI4Ty7ueH}2e=$y*q@&wUqRYUMJcLp~4*tNM{QOalyZS+~Y`JZ!mP~#-V
z1wLSk@qjAJ*VfS$_5}mVjLln`(V*6Go#1zxLv0<MVGk-n5o9|bYwH>Ng8xT6P(U<M
z*-M1aQHHnlXxU{~M558oL2o?L<BM`F+d`ej7HZ_=0?EgqzDOeKq^sS?vS`LD8f{&{
zU`M;BnIBPLo)RGQSjznp96~pjWNl_SJ~ZdWeycf@zR~nhUmWk;$8eN^DycGNR~y&Q
z=WRplV%>v7!Y^R+*J$lRcWv^K7Y}rz*$}Hg)X5Elb{T4KA2Sl<yu-y=a_bk}gd|z&
zHK%z}Y8U!!c0r96IF(>KGlm95FM+ED1@~KRn>xbgaRrv6CrnRDRVI9iPrmIR*_Ck@
zA-w9>>Ir5#T?>tMY)9W<B*l`uBSV5!WE*PgL}MhczbO=6kI>TdcBK3ISdZy~Nw<!`
z<-&5~HD&S_X3dXnydQQxgn1N)PaF?HMk6y&qa&{!YV$F2po17z$DXOdYanM(hikO>
z#X+1V5lIau+|gKHEERKKaV1mdUGM>Pd!&13ddQ$N9z=DDUdspWX4L_Z1L-2%4iw9x
zgwF~EH<CNG7(q7KWl*_!u+JCI9T71QvY}oa71AB)>x&Kaq<TeBg7D6!qv()1@}#bI
zucy=3>gfyz;6Rm+#JLY(AIx&tW^1XfOJIcD6*F5*gda~5ON>u04nq-xlWNLdOmQ&Y
z2tSY+OL>Vg7>!rvdTKqghg!=*-X${^G59GIah&pTb<7wNB{YZXP2oVN&uGCEjm4AX
zpm=)NM~*#f$V?J<ilxN>oYNgCLpf5AyXtCN-xk``mZfs-(vCg-{V|#ei&3aydmOX*
zSR^rK4udL;;z$^DyG+NFwOmh=5jtK419&19iHg>wBu!j<hNlBI#9R%Akxhu^aX3dL
zCA^o#;GT^%_@fDUnCS+gI~Pudq7=+d$l#crz;}ymO_PS&eQm;`=+?t2KCQjn7Y^}>
z6l?Wth9|@fVVx25wXN&)bM7%tGTX_bZDZR%=g=3<nA=}eYi$}F?2Cy5ZCI45)i8GS
ziwO(7ARip~L=3LE&Fcw!g;~N}rjHlJ+Mwh#foi>jCflUF#98VnGZS0djm#6WWuhq&
z{g^>5;DzmIF<|Y@85wwE*PLj<1ykOlL58|IjZlk_*TL~Efz1+BE}#5ZW+KR|fS6mD
z(LWd!?kBrBka)}?e4cQSX1`t^Dv|W#Eltd{7#dt3tpHTp8%ia3_U262@HB<^)XVA&
zIEm`y!nC4~t7S$DdQM0!aEz9qM@(eQCLtG4@QPGzbnu$HdC?;Ath~FjKb0Pct?r2p
z#L%+<MiSk<tHq&i?#do_WoQZh>fDtuCUv*U9qEB?IwL6w6wV^*-dQ=67)-^w;kYf!
z1%9eeB0W%N?So^78*`aL=CcfL!h@<NBC0Mk>&>2x4a8!Y8(GE?YjYTz<;2r7wH|Mx
z^PVVAwJhT`PqxFnX%{N3D#sIXVj6k9fv^wrUCg1S&71w9ps!<nb`w?et<;}hfbp}X
zEL{-_m@5x+9ORubH$&22x>2L4YaKI}223nJxu7IXNw^$KFEoewKXu~PpKtq#8<2{D
zB_p|6<zo4PQ|OA}!QJNeNmEvLw+!AO8h`L6Q;b_Xa#|<`tBkv{Haj`p8-HdBN9YZB
z)`hU(GtBiV8=#pX<q6j+9FFvKyh*9atgNrhup*1kSIlZAw3=p^Etob~dy&P%)mmnX
zIEjpAU6{h~%%PiaGd47^oTKqdIcB-;@rL<SI?L|dzjn~-kG50VwghOaA`-<pUKF4s
zQ?Nt?^&(ix1&GCWpv^KJ!qV5<)ePrn?jNWVzVM4VpR6WgsdQpsW27%FCv(=RRM;m*
z+GY>UXRujA=fxR=J9Z>vaG#dBo5dDOQ1~D=cjOF8EIZ_!C{wrQY^;gacEJuYQO@2;
zYOQ1VnSaba!B|)FVUy*|p>1-ud~L4NxYnHfaVus9fy}yFm^Wv!+7{xiO4gm)<G^Co
z7w&-fQLUHeQ60Wk43ZeM#>|6i>#S4nv5PdOn&5(kJR{LZjFxby)iSrXR)y?|wKbx@
z(kg(OWITzjA8e>FKB8w;p5j=R>%pZum~8oJ;K#t_4P{16I`)nGFs@j3#~ybNkN1DR
zF1Cy?mOhDn0CQoWe1(`=jlU<z_xjw}rsWFo%^DnY=nH3U9Fib0W3kL_uN||4$Z#BD
z50_=NWu{p-DErA7j2e>Ee860=Qa_>N<5KbN%ygX}W@upB8%Ho=YZ+V07`TlZ(R56W
z)wTnv%$pY9G;0op!(HuaS;fbgoQ5fPO?X$$G9Y}<x?rdYUa=J`e9t<cfhkveS0`U3
zpyPFhyZA~m(1A@;zSG7v;tM8BZuxGkrG7BnhjMnv#5RYgYcqC(J>e}FG9>-@R;(rG
zkjc&5*8`t9Pv$CQ#vaRojy%O1*l09uG59>3JWXaHm@yej8EO%3lqm}sNvwDRhB(B~
z;<;4gx9-JSHeP9)nhplU3^q2*2KZEz>*2w&Q+jH-V4~$2PEt&Wm6xzg#^EW$Q01z|
z!&fowViKulC^RFc!YQK6(uHSB-Hi4XP}yyKbA~qc4BFD>$+VpT2aX}{GVI`>Ez)X_
zen;V{84LL~H~V~EjHXz`$khw)0O^5byk{U5<vqdN>te=WUsSn$epn*+0=yYnD|*b*
z5Z+DgKf&w5o$zB6l}mGDznV4|<WY~T-Fn^+9*$BcT#l`+rDDovsVC8AGE-AE>CSR=
zneM#8Y&uYi;^u(GOcU^fFiCFCsDyzyseWO|Nm@d(F#q9OFPRx4tuJ=O6ZpY{#N}*?
z$*JsEj})g~$vHA>TD~FA?M{+pN6lGx@SkZoUyE;(FRXUTFqNPQk+r^L+pni}CHxxt
zxBNxNTtH=(Pbe54C$j|n4~OE+#9nM%Wq$tP>Z&?ZUB%qEqdd(+Sk2M8o^~wN?>?3`
zGFGVxmm202o1ZL8EgZ{2vh0TTP}5~Ji1RK=V-~|A3N>%ZC%ZM78%8jch-VQ?Q!l4f
zJ|gpM%vmdCW1V=C4&x5>$~I!`Gux^f1e-l=Bq}o+8Rp`gM2=0yE;ME<7_xSyw)Yum
zs0Q~Isoprw47L=ucI=OC-F%OZcI8Mk_jes#+-jYn&MocOjLNh`_;a;b#0C}7b6;fs
zOa-Us^>tv?hW!gZ@5PQ}nx9R~*Lm{i4ofpoZhL9Yk2>hiR<$#AM5hb$rbD~H=-PCa
zE&na2j&WxKWIADV<51%M0fr%&et-;OCjqlJpV-;Y5-jsyCidEpE0u}7Dc^`iH*NOI
z9l(s!Rh#NzUnuPLg=wOI7PqXPwj$C4G|FIdk7XtOpl2>B<)+K{14%dpV_EiaZj=w=
z-7G_mwX5hArHyz3f}Vlc%0tWTw4_$gO6IerpJB7_QlMLM=CfslR^XK;np0ck@u(82
zp76A>n!9po)MJm^q^L=FKD@k<Q;nfjk0^c=T9v1TRy{I0M()GKBEz4^@HrW(M+v*7
zZt8Okxn)p4_dh84)FaRn(X*=Lo_?&O5T+6$>{T)J;#gvcRSjStrwS(wSEjJvCm=lS
zQN<5Ru8c=n6+yj`WG}0V?iqkQ5n`0#DZB7vBIcJ_fPu6`tS>?gGB(tgVpV)Vu1eAN
zW)(jVxeAXw&<W2~v0h_GBGMl-dZXY~5QKJj8?oW;*ig#g%RwQ-h|mwULWYW8)eT1E
z7wM08gKrR7;8w-+6?xzgX;#%ei0x!d;;W*u?dcvcM+SQEP>lrR13Ly)JhFXzBDPCG
zv<s%<u%L=!E~KJTQf`&~?_fNUP0z5<11tP$ur<n%2j+J&1SDuxoWf7w4^LN`{GGN&
zKzKA$3UqTQbHf8LSywF}D}Tz?$kS7jzk)-VTQ0vBNpItf6Q%zVTO%jtFDW1v<q#g#
zlp&tl%(~hD;ZZG9e`aqK>3C{~t{NHQ2^(%F#gC`D__YytB2oFPb7X3W0hc;AMnGfj
zM~|)2nJtQ6ozp^Zl+s1zqf6<p=v259X><mRg4H=N3-BNsU3>W*&!{3KJ(d0v&8|iy
z4}XeZojY?(^4Ca0mA~Rw<-Z|^U!7y~wB%Rk-Vi_D`Y`iX{I>!}?N6G^Jojgah;gZB
z_yr_%RdEXGX<jS;p~;PeW``udn-VP-9uDT03U3Ew<yVgHI3BE}t4S4HfaW<v5@dHE
zV&zxo6uns{C}cG%a&BuAK;$U?h+o;C$&rGU1Bq)`1m<1YUm=bmV&xx}{KJx;zonvx
z5mH@SuWtab^4~A{@0a{rDbaG}=6?cdR(?8$lddUvS%R*MMPS}@#IYxlf`_Vk+ax0O
z-5mAZ7*$V{MpthB&jDA0KiN&0I``>myzNL=E`2WiGH6IA#ZM>7(sk-1^8X&Rs&5gW
zI`>F@*GGN7N40O2zbe-`@L2O#=h2_Lh@?%dzmoGlXsrCqy^YuybMrfIUm_;%icf_F
zV7BrfmHbB~ztU65P<#rW3}(V9`_VDLbRBo|fPYIz#Kb*UUa8E##!7^^;$M`5Ctc}M
z;RTXkU^Dk;+#=qXLGhU4Q|N^g@XwknC~n7-z1Az2f9VAL_pTINU&-lkx%|x&@OQ2i
z{C;IvsYEV+Nb={}@6IN{a9p-y$!^+B$+>)j{Da#B#}Y*-1>}G%HUa;25y3Dd`Bae0
zUy6jBB68B-Oa5dILk`HxWBN{&3BuET0vM7Fc0Rxr_*3myjX&g{$IE{+UY?=r(4VM3
zK{fu}4(%Fjjbjq+@b+q+WKFa~6E*+I#^;&m)n(%`IN2MmC?2=NBdK|aHPMclmYQE=
z;~h-R8?y0Ersf0L_(@ES^V#^Z-v^>}J4SvrE@#sho9|&}<HvqCNXUVwm*Z15y>5Qb
zEE`|S)OePSFT?oMXhre39fPhKf2@gioa(H`m2CV}j1$|eC?2=7Y34X!O|;`nBFevK
z<Ix(W-^|93{k{~X+c7Irem0w)X7gh&`<Pk0ps}+Y=O9__?831^fU)zhqK!?mGxt~|
zr=L3ke%=^7_3J$L`uW@hnj6^gJt{RXqp_YYlAys3C#2m}zit42JUPvrer$cU15ff#
z$OSP#e#LMn{Fxm5u*Bz{gDK{*7qI)KK2#pMzKB0<oc8lQ;7PtZUz2F*dIEpiIPGwv
za?$()2Qo1|r#*{#!PxRr9^=`^357Kpt#jJpMI1l&yJhtKHVxk=JC;LF=gFg!j_EG}
ziO=m{G{4lw=`S>{xX1ba*%0vbeX=uUjg%2=D6YK{pSwZvSuRHsS%NQ2fd3~h$3iHh
z-<|;f9~0n5C&1T3;WEs4)jC1tw36c$lnFx<;0J(rXUNRh&A^ZM9k{=lfc|R}$o~=W
z<9*-mR};{`$MMB%udEm9KXgsDQ+dZ;4ZzcP@^bwcz2h_<{f-In^j)X%=zlW-{te*A
z`##^f3FvDv{*xVY^=ts1@;y}Cm{qtf5}$<x@M+-3`_A71PCxd0dpC3ZIOA#7)8-TW
z45)rp2ricoc=%X%0-ttE<AW95*q^<u84n)Q9<aeOAI~#V{YE!^J}-$U$;4;&EGT?I
zY|!ZG8{Cd(%%XT0D``a1!+5%yUKLBlqE*XQET@Mz#v$S#Bomuwc%31I*EGZiTr}O^
zzXvQ9i2t;lMQY%g08gtAzb<*{6M$TE8JE8Shgw>&NpE!Wmp4FxZvdLq(d3}fORr_&
z372fy(QE|&>K!w@TiQIWfo3-T$I|#lqWKNAKlyD!dPc3Wk$*`IUr(birtvS_NtFD!
zn)P*^3^9Fb&AQ(|_P3w^(X%1qlW_78nM^KRrQA>9<s^UT_Z720F-N4T;beb^(eg8w
z`J9;ePGQz(=`v(v%PYz8mRz@Cez?x^!%60On2A1`X#KR`hvk&}c|`Nav_JK{+J{%z
zgg@40G#B52q|YDbv=MqFW!$oUr1x;trwzw`P_ePm^2xvKw=ST8{3;+k2)>z^-7Li8
J27HtKzX5|-s!;#{

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/example3 b/3rdparty/qpOASES/qpOASES-3.0/bin/example3
deleted file mode 100755
index b10c54a32773d7cbd8045c64e10d34c3cae0bb92..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 30847
zcmeHwd3;;dmH&N`7s*>*>?9;10YL%;l6Xm+#My{sOOBMtl9A-Zfx?Sy$+m(mxmpst
z10f_plakP+8v#m5*J)?kfthZUwh%~Lx=zzVTe?CQXhLDyLQ69R>fiU=MS7C$c0TiC
z{^NaY-Fwe=&pr3t^}ToBO&))zMbntNEbK}~sqfY}IY|66HrmgV64u7-%*ifgi&zC9
z<@k4U422@5!gErz3T=|kim(j-Z59D&vp9J~g$o3jsZeQ1A>~)NoH8oHT{A^eg^W2k
zfcQw)uP@|Dggbdeg(syR&&W{mQIh!1NWL?YPldXqSD`8&m5u%*5+AWRk)pJd0_Ntw
zg^+0J>V7sGqxe%ZcoLzLM^v~$fSC#v-&NovJ^!bdE(t?2zv=8!s2{<g>Y-^k(YLy_
zX*jlgIFZbaEgx%Jy?k|RV<y$Of|r}*6Yb_e4`)<%oQo9cJPZHi1BCy1?aa;|%Yi$;
z{K;#Ye)j!i_nq1FdVp|b0^+O0|51IPja@XW)>c+ld$5eT2-krB`S{;u@A%<Ezkb&Z
z?VcaJaQVW+3u@NgS-qm~r!RhT>#tA#<U7Cqz*BGSU9jt>yKnfykw4be%=*;_o`2%^
zb>qJ4KL^%nFOuz;0(<c^@NpEjehT{GY2bUOfghO$zIvK`L({<R)5!Tg=%>>23)8^=
zI1M~Jjr`$h=x;(r>TDhxbL<2~;rb8A(NT*ZFM;33;q%zR68LR^&jkHZ30LjyX2^F;
zA^%pu>sf8SKT|o60zMNS=B#sakZMPVCH=`-0gp?Dk4ZgC^|>2xl0RB4=r<!x*Za7f
zYPPS+$<yj*r<C6;^F@+hyEwd#wMw|6Z|3qDb4vLN&d~neIvR9^Jz;8eD;t}cVGNF>
zl13&QO=k^*8C!N6q4;1TlZ~f4hNGEGJi`oMq|1oK(-Iu%>KINX<B@3Ja9rS~##|j(
zBlBummChxDS9SNrll?;@(e(A6?ydcSu+h@eo=OeJqshqlXxtOp3ecM1Xf}~bW;~%p
zC>CC0h(Bf&gtd)~cc&A{>;MTdBNS^}CZ3I?y1F|&yQf0CTc(D05FGZ&WMlnHmy&r_
zlX1qzjL~>HlS)R16WMWNcQerp_V*hZB57epPjXKp8H3R>xsf;_`xWmfSJUVpx}J^Z
zvKjj6ABv_Ka!BQ}5KhF>@*~kik{R(>G#dq?FOv~)fRZuRl*^>4>uic<Q6<a!qM5je
zu4@`fjl`Q0nW4mBE}d%Xk7ln=B#k|prv9NsER{|)_2m-7v8JK)Xl6Xqv@#tVMKzB%
z$$C-YaySCX&#Y@ojb@wDsZ0}!lo(7A@E_+9i|>vP6R9dve<o)PB$5euM-b(RL8V+K
z9s`=nr%EP@PqZM-vdtYG203I$Z?CJ}XS6h~WSjlI_6}nO{IoS6U(vWqd9wv$fra<=
z3L-{%4P%G~xCqU^s=X4-!`mif4ls9LI$lk@@0SgW@E4izPuzb!rVnN8BYC`q^+`PC
zot>Ak)5vr`@HB?y=2P0jz9{L>UAU9T_?!T2`AhXjG&eE#uXANTMPrzm-eH1U^HLaF
zVuGtO61v$0H_w0COmOq~?=-<tfc)h(!K?C6*v$lo0rOX{30{+jGPctMS93H%51HV#
zCi+nmyv_vo9(cj#y}f*7wu5<ZJ((>(b3TyX17ES75=0ZL`hc3aB!=G^7djE6^bjG>
zJ~M&qlB+3AragO_r$dyc+B$oZr?*hrO6do9x}DNw+OsEkdL5<7q-T%v^a@IoDbMcX
z=_@Er)pvH3r!S^74LfId^7MR4lZnrE^K?C>$+TykJY7X;GU?f7q{H5uU(lhg_s9*4
zooDs}B4W4jn0s;COEdJ^CaC8>;$OVPf5g4m;YDnL_mF$B&5LBS_ef;1v*ndX+!!ey
zcQ1BAGV>lPTa5UEUX=T#88iKh9cK<eAPjo(Bl|#iW))Hgz9sW&qr7hZ3aN4EscTL>
zawAcE`fn2xk91S)6BHYy*nNmWANS%V4>Qa|A0aD0NIz%Zt1!(EHy306NnzR^-dT)^
zl3X&(nL$BQ9z?Y~e6)z_r5QU=<TI;4(el%?er^?5%6sV4nVSLg9(wxBT?hweTzCs4
zq2#xhf1|+x1C)Os!Ks!PeTTkxo%hhMya#^shwg~)@sl*rdLMt3#&hrEkJB)H=G^NO
z6Mvd<;XdMj@SSkX9`AA**S!bM)p-v+yT@|vsRs#14*92RA0gDlGm4&4ivH1aR?vG7
zJ?4G%`AuH!wD)`Ga$tT)Fn=G+-a}s#{J)a?`!-$#Jy~vM4>)=KU;DN4I)Jp7PmzCn
z4{Y@6Ajn>jS7BDqBhO&f0s4v4*Pi_+m}JI<>}*DauI;Wv6RvH(L$A1ce20GEiujLQ
z@`o7?sCZ=gIfP;V<<GYK)cfe`R%&Tyj=TmN-uhzp0%bq{q38XFUU5Ul#H^or51i7x
zYk!w}o*Lja*Sg-~y4H1_%Q!VK(5UENJ_Wl_o0UzJdgn%rQ5f{vn)fs@%ncmvYI5M9
zp>#N#S;sa`dBGWcNk=M;8HppCa$suV$POhkj`&z~WOO*b%rTzIIg;^sjIgoPp5$;U
z8dF$DGS%p4UQsoWPK|&q?g%Ea6p1^E6zE6|56Amy9^%*&@5>~zamPR+oylIL`o59q
zSYjkM;@F)?4M#a0=1&wx)0iZzbI^53<I4E-^aJr|CefFmIfWqq2lV|ZD%(griFumf
znM!^~caI~R7>P@fTo10vfMX;vJe<hH`%}qSrb_8WF2|VY4Vd$tPVIc_dRik=NA~r<
zPD~s`c>L9giPH$rzzNPF{0qWfN~1kcXV8m=)l1DA*DMh*?QJ2Zjp^F?HI=rbTBS~K
z>hJFa?-i1Uf~o1D5pYD`1|2AGR;PE?&D}DidXH@%+cbau6)P8COkg#JXm9KG^BgO!
zH08C5e1#m6brN!Dj$P)T>13`t{eWe2-3g23?NxPpN1e@8O?n*0{~45()`;qIBk~da
zpN8IX0=C!b@3eU8=H6lT)H#lndFqxNF89_o-&WzRYdcWcRp%V8Yjf2#yXuy-*E!nj
z=7PSx&c@660@%+%j+_#ZG=5D?X!C0^lAIU&4+}e>A$DAQw}$v*78+mPyxz3Hn-+M}
z0&iO2O$)qffj2GirUl-#z?&9$(*kdZ1&+%56({9=+fkWT!sW6qzXz$(7s-f9(_SiF
zDouN%bg8w}=ViJ^fb;h^YGqp8znCY}>i#Ni>(NDfs&vtPJG!2d_fysSk2com`t#o=
zQWPGyI4Q{A^L?;fq-kBtFRPOWY90KzT$HIj%(;S!&6c4GtSW--9J<t6&LPXE*0Z$n
zN7s2GU_0$j9#imUnO1r?OFFgZs`AIunwRIKP4Hv)hNqPte~{^Y5--L4p9ktbc4<bR
zs}g#CO@=>|;d3&4MTQkvq|-G=hL_54wG2CDxJ`xwGUQCAAKWuo=UCFymrG`Ij@HJu
z#@6L4ay-?tx23hQxwUcS<pR!ceT(X{aRH^b-c%D%E^Eq#M;qXqWjton;_%zDBr0hL
z)H_QkEsO2IKG_Pe)mHg1o-D9xwo~A?S}Grc!&=KKPjO(yS2?i!UU*GeSvhWLfuZ~*
zq%8HID?dOx4weTlK=c-hLfUHNS#GPa9iyaH@ofmOJO?MOX#OxDmA?b}smebuLOE?$
zz(*=yxej1vdk*cc^3~fZYHNV}%GW5a+n$4_m9JBLuI)AKuUGz!;ttzYXlIoZ6klTh
z0)$mEjS|iFQ&6T-<B2x=`{4AI7M^h0uZATm%Xq?TkHF6>%Xy;Pz6Oo1vVtdi?QYV_
zrd<P}JMB;7>4?fIEsDgDy%E(?S)-A{qxKgFqw_@8{$rG?vR)eoX3V~lG?=5cBeBnZ
zK|K=ZY28Tdx4#F!m2<U0NIPIZpLiB%dyqJ2|2ZWt(e{AgsQn(;zOqp}gv4?ChbYm+
z6DRDSphPoIeBAy3C0cmm0sH4Eu|m5XJP+E3$e68Mx0CjVNS)Q%jmYOKtm-?Y!rhfm
zf?{sv2`Z%RpV7rsevqQfRzYU{5Ea*Edm4K?l^>(h>a4<c1z;66s>AAI_#w4x_9H@2
zs!a|(qM!|cz8i^JqOBzvb(g^tY`0yC=MQG=sD?Y)-t{O5^b8Ed^xwflG&bX}Flw3Y
zo|g!5HB{WIFM`c)(if1+>;DYD(DZMjPA&Rxpq!P>xEK1C*?zPLb*3Ar#hdi=sYBM=
z@vG@);4l{bGPDV+z7yawHsfX3sm$tp5wyAozIBrx2QSlqFozfxB2SC{@0`I8CCc;)
z<W{cF1GYlf$TRdtz-{_sXk^zL0IAaVgT7k70d}s@yHJs}`mHEVo&I}p&CqvJVf0r~
z%$fSNC||w)Fvf}oy%xF8(&={HY+Xai=ID1r&UyN6=#l5@&%i|U^wW?zU;j76&(~i>
ze1Se6&2FK7KeW0)9|inE{U+$((BrV{MS2V37wgZX?Jv^DVeiGd117jcZ-lij)t`m@
z%k+Dp+Y&ts16;0mA>S+X`QTluhhdpz`iG(Oa(yfGY}9XuHJbFt0cqCnKs~hRAxK!E
z-v;BZ)P2aMRsTN9vPw@u=4$-|FytEjQpjx6$ML&XcO#c|`qR*My`F#$8}z@x**5A{
zXmzDNh!SnmPeSrZre6VZk1~BF@Q)#Y*i%fu5p-W+dJK*At4#lQ<nlG9-;LDcOuq}V
zzs__E4E_zKFNen8Wct?t{1(%H4?BFD=@)|XJ7Q%1Po@VU^Sey{5^$%Pei()R9@B3H
z$@d`)W_^O`8)4BWnf_s@`U9r_7jQph`ZXx-kC=Whtnn1n6DZh^nf^WK`4dP1<xiRZ
zG-~%}On(BRe-3s8zhL^csK}?8z7+NR4AcJ&e7|J+hXDK))4NfyUo%~UZGXe`U&D)^
zW%_qvg6Ek2O%(IDOz(lrGfY1QmCiDK4sg#ieH_O69n+tJrC(tB8U!yg{VFK&UrgVR
z^8KFa3&H*erhf+|`Xkd{K=2X*ki5)vD~j?brhg7f{F&(!sPR`|WCVX<`lnE$b4*`_
zV*WSN9|gl-nQljZuQL60l;Jg|zY`>{GyO#X|HkxhLF);o=K$QN=@+3MZq{^~rtH`B
zr%?|FG@TxfyG7GqhRj<v{YPjsw`qD8%=30l{~F47P}A>&)`v9x2I%<?O&@|Tw`=+z
zQLw|Beldb0NWqiu00|i0sp&Py_Nc~YehJAX_BCIGYW3StAp2+5p-M3@eH8@u&r-r>
zssAnN(Ej=BfY2@VcM|47O3bp<cO$TWff92q^<EP25G58^>YpZ_FH*u`X`pQEn>BL5
zUHG5nq)Y^Ki}p4^$b&d^HgQN)KzlF1C-SIH&h<r-71AC6_=^fPi*wC+4Q8<SEP%cI
zAl04#`WK31Hs_vmG0b3p%UfW4>h82xfYy*O<vnLB3~lcpM;&mIc|b2Fk`QFh?j;@O
zybSlSC&uA;oMZ#gn@dR2@Dlszm(ij~H_ZTgSDr+u8$os1cemh|lk5ljND0Za$ijZZ
zyPz~D`6$r;sz}(J&r)IbEr&rk_7pJRC}8g<4c<N*PQ;b`B`_}#mW(gVJjad(VL!YY
zP)<??H<*EcGYQ@7wja9`F2zYM1A0vni4^rYNVVVjDjMj=VL2@bnyZP9TTPVr5hD5E
zV`z&c%8~|pOo?K1{AeupkA4J7bL>%I?on7#dFQ<X$@Wj4g8&j}xu4Jl`K*=(B+JvN
zKHHI%$gB1w2xdNp26MWq?b`s?SD+YGZ=)fTY4s0+y6OhfwoI%480f2Rq=ZeYzvluZ
z_EMrotKR^Bueym6x>jEcf2rC>iCJ3xK~!$l&6Jp{)h{HT{iN3dt^RY!tm*(I99sPx
z^!HV_5ZPj_euNUYkzz}<`YyEks<%_3S*u@0`5dG~n^wP`$PQ7$snvf56<qZWN_e&U
zPop?hw^O29tA7g#I82FNt^P&A+(C(*T75aG^G-?(Y4yLNVjrc%s8+uTeN5FcN{ngs
zU!`Kdi>3<uwE7$+-c6W;TK)BCOI7coA{^D~9|ElEy@WZg)gMLWRlSceC$#!{%I8kP
zd>raf;y5K9(CWv@RCiJ0L9M<Co?3Me+2EvB|0)^&{UqQkFgzvhrNq~@dMELGfD+%<
z8mQu`HfeXEy6?mPtX)u#323`^KOhe&=<HFBdXyMCwQmCa1BEj63{}2U@Xp;J)m{Xe
z+=$B*+;cvJZlfx=OtonpRX7L#BHKB1tF$Wo1bR5$3>txHEnt^IR@L@a<j=8wU_u3K
z89ck{n(x5`9J>pc(E|2LjAB(W1GUew2Z6bxfOSCSszee1j{O)SpCs%JRO0if%~WNm
znGn`;Qt^o*vD(wX%>6V(+p5=o56E|+z0dm;((fjAt$|S08@1PfvH&&f!`zM>>V&gn
zmZYGwKS8MZPeYw*w??wG6~Jv&c$1y0H;;hye$Z<@K<`u}Ld5wu15q97hMlRNHH&WH
z(rzHK&jQE$li4vI!w;G<;TMd+Omk~yYj=X~4#XGIPepx&cR|I)nl^6`DN(2kh*G0z
zz;8(87a@ZyH|tRn`JW_Gkh}!61+*n3>_U1zr{;X^PiQE_4HX&Xsx1QX<;3tL3Ayl3
zFm8=QyGB|cWL!CI3rLcRY<9kXl0^dN?CVGqDm`<|hvx`3nx>Vi!secbrmNT7K!x3d
zZeZR_q(wO|dJ+y>Q=w6#vRN)5LhY*{J56$aLODD`YBms}W}o&9z|ScZo4*JCQgc8f
zx!SAvsetctE>V#ef17M@+qY3Rsv%BxF-VpuGPdX(+Ct4c>fnKt8K|tB$_t7BQ9Vbw
zFN&jlHFsQwUY&XZP?d42Bq+uel`zGk3bN;WHeDb`5Q|>UN$&#LeTtMVdKnF|=C127
zXi9+<oazx!d{a?LffvEMYwkM+vq{oQPWn8^{#ryT>0bk|=A%}0e7vn$bQ`Ci4?rva
zd5Jiws8kZltCB79lZ`)hKYB08Y3G~~a3%jf=btc!)O^;X`UZ<$#X0W)*XN5ld3K^%
zz_7FPxl85$2_~*Nxlnab7QLDa`VoZSQD(F9|3DDe=v()}CuCV_xF83F^yUR|uP(Uu
z-xWEVJd`cs3jTN<oL0)M<#NX$^I!j0a@nHqqJ`G{)(#U&k#$_;w;=9^B$BrR(v2;8
z54p`BX#PZv4}Lg<lfDA7*HrFo5%sn;e|{W;uN0_rss(VN%kVEefY#^O(IRJg?Z*&C
z#nUzc?NKCb@k+Qt&BPkYY6qYOVMrTx$#bBtv1oTfX>BwQX0_DAaEn~(=?=5n72kr|
zwgsU>$IG<`HeB`$y35+N9QNZIfAZ(kFFm~OvN4#lb|Z&54}W%S;r+ASOP)out!@7R
z>~iAr3s5fWs!NgU6<<cqb*H~7=T<sEv#AukX82e!TZ0GA-cN|)AGWj=EqF%%Y><;p
zI3I0S9EV72S=n<aht*bgGlB{mb!ingnuRU1(UM{rk5}+`g>4<ASJ<dJDr}^6g^h-%
zWj30bE#vVD87G?MB#e5xMiAbGu*uerdO662(1@_iMx)s>8^tSlyuwCCtgt=KX}*Ks
z=4CeOH7n#VRpv4qErBZdZwqyZ*1v+V78So@2@1g@8MLC~C@`y~I$N0p6-FNGA`op-
zggl}MSM-3`4HT=T5u=udnid*6t=j?IrFeKm@vvGNZCk_eBg+64)_NFpt9HTMmI11r
zMz-36b%ypS0{;S@tA&Yd)e_XPh_W?vp{cf%z@__~+WQy63)+BTHbmBH;d>!--HpW1
z%v<F8jU>H=D#3c}#NPE^Kv$uy;+h<L?Z)+2LpyCXRfYA~zP;<W!g;hcB6)D{`h)0O
zRr2WG^`C)%s^sy#8&Xi&TH~b58MNh>=G%w-Hr@o(m9-N0AT-;g_1LZSkg2#hufpef
zw|ptjdtNCoJ(g<XRroyb)^mB@i%NOvK~@v5!lQ8f()=4yFem9u*OlufKXp9NTWi^*
zMeNu?b8<AcW~ktbC@-XIzY?;Hg!D`)gdUU?7Z;+4@<IZ%g~T=KB_XLPh3qUAqKNXQ
z2u>&=w~&x=Ddc%soOO)3s024lcr%E!ZYNxZXZ!&1+%sjy^q8)=+V@wBXb;$6BXTX)
zaaaOr&YUzOel8JDqES935Oym)yi6CYt2J-Zt|A$3Eo6mTTB~|3o3vhX$xYf0>#>7-
zH(B<;LyuBuvmQH+%zh_jQa|@gC6gX>=2!bUiGIbThv$A-ETu8K^79f>iJlQqO9tGX
zcJP$mLD1fR9qhN6TJI!O*!w|%CnkLFLxBEz%d;S@_&DtiZ>d8+S@8*qZoL_$&v*7)
z-%Bag&2Rk#rBnxB>w^~lF9UR<>l^sBTTeK4@?r%})?x;Pe`>(wpQ5J@(xk|;oq$x!
zV)`O_m%wQZ5I?~`A$k8#5p<9gbki$nyJ%22L_ns^)Gn!OsJ7OfU)OWqOihO!EOVH(
zZe8{IY9H)uUCgShuLK^eX&FV2RXzhyl#&9Xs%+4#b=B3xq#<dm%}Zzl8FnMMz;a&$
zC99NttQR!R*FYRK7cpN$6Ogr+B7#9}Ed(lYGs=pj>q{VR0W31pQUu4kM(N#Fy#lft
z8nsRH1ZYL|hIuib)2wo2&`JiN2nPHDQL@h0P#`>SDg^C^<uhH#1<s>vsbsD5DAV}`
zHh=X(lf3f_2+GJ`-8EOCX=6=EEf-Q3E`WAzbN##^7YJ6$Ko#S{ytMLCX=G1_K$8ZF
z3tQmkdLUgd&I_qd)|nL|5sS>oVtom{*o-ddbx9r!y#yG$>#dke2#?w@ZvlF(%K(rr
zCICx{08s66GXOK4yjW=JYuGT4R9IRHgp=#iBbU#Xp4Euf!J5cXsg^KxxD@G@Ms0J0
zqgr^xigI3o<P$3i4n@lquC<Vb4-Bn5-&MpQii4f~M$}yEJO_9DHD>aeT79K+?w#$N
zr>$D+IggZHyN(g-TtwFu%D4bUUSCMkzWs)UT1x}T=&jztg>NipE#SKH(0&cAmZF>9
zu^&(3&pp--awlOeXudAu3n4ef*De?+^A5#J33s0RT_nIGaOez0iHg+UY*q&QRg~ER
zED?P?0_C?9l;1RO15CLUuY5I7UHf?iBwZAtk_Cj7s3KcIOS%Njh7+J}MMX5IvnSD^
zTG#-7HLPrF@vEULqs~`*1K*;PFCk>aJ9~*_|8OoAZ^C=d#_Cr2((_<4*VLE5`+#p?
zwkjj}Vqf8nLA<FrId|MC#|wU3wIV(L4>f9vk7d)*3}ezjD;xVqN6Gl~dSGfr;}GhG
zb;UE8=wRF%O~&w2#$e+RNU*j5Ni04963p|ncvCbZs5vL1a3j2Xm`I5zy-ErK!RbrJ
z;$wL2A*%rL?NXKOz?-H}3%B08)2UdlKTp>c&88Fm1wouJn#|@U<Bb(?#nS`P{y3M0
zLAxiL7|s)5*s`d(w{@xZ_WP{*2dvp%Yt4S^TP>Gp)*1V)nJovbH3QbMUTf8s`<Mlz
zRP6=|?klrcXMYmOA<J$J@wxjc{u_!7AhxU5a!Es()!L_7X26R>o`~UcyW4|30k_fV
z^7%b(!x!lExWc}6pWheR!H6R4>+1G<jBvyi=?NR%%^cVh_60T@A<yO>zboVmyCS||
zfZ+{OdN*}<Hf!Kz(GeDo7?AAg^aVU_^ZT?dYbIGQ8pcb!;by!=oEb``vwgV%k+;{^
z88O_Rh$qzL3-DZ|qO9b-VE)4BaH{`$Bg;7hLBs0_hkdSq5$^VM_&R;iVT$6kaNu@V
zC_n|8Qr)$AUnppKbzrM{MR_%^@v)X>xI;1%!8<e=ykCqLk_&vE@Jb^Q>)sl}Pb|S;
zW7rn&>f#OCEWP0>-UCjH_kiimSCpT>zRTZ7cE<;zc&mvkdR4dK@`pSw_YTyQFXD6g
zePI-=+i!Gv+`gVJHjs|TyW%6M^f;&Y2D^fhVD}DGpQl@yj?B;#aJ8dWBSBQ6%WbrK
z0v+BiS7@ubo{Ha>=H9XmUcwo-`yyV$?+f(w8o_{Hc#0<!3WiKJBn}@OpchJVPg4ru
zo$S2%3RY@q9gJrKxskqj+Lca6$35NM2~lO+UEN$xo*yRKgO{1&+(^a6aRprd9bu1g
zRd*<e5_YjzDv-){7R-Xzi(|fICY}}+^=$W*sN5a$1tNaWHjm%v@VWw<$$1Bcqk|c)
z44GZ(VMMNC2ECMyx4c#DgoFMbY7Ep~LJ_crdpbJch@CxtSUS=Z3b4JxVK_&~6NZsQ
z+hfVxNGJ~V_{&=1Y<yHm741`1yQ?GO+vbtXUBPW0bMxh3PaxbA^7ws9%Z?o#sHn{x
zhvwc941^;g7s{vziraiVNw4R7-kPJ=))VmzHDg5qkLl^|_JkO8Lrw9<-ZyNFX43|B
zuRM#qUyA0c?0_C`B$4E=x<&BjKH5sZpbrEi#t!ln>Snq;0Wum@DGbxoZA>biFR%?x
z?>1ba%{@dNCXdZ~lWdHu_zU#>HGkoRg*q$gvv>>Z<Q>;!F>7PF(cuJMjgR96|58Cc
z0j`_J9YFQ6{?w>waoB4!x_V%<c8}2=4ErLovb@0vHw7Fc2op}#)0S2o+>0_bRoE0x
z&;wsDE|bxTR_O0$#%L--J%iV`*~<l?PXs0J2?E>0A;!xdiVr9Fc?=j7`Pl-6^AZY8
zb8R#h6D`vtU(Jv1E;PO%UVQCx`Hj5qchZPLe&Y{rhqGk%L`Ma;aOhxr1nx)u?e<`3
zD?(En7|0C|i|R**74}{2VX<^-)bm!17&yBD=MrScnefmM-r2-Z&xxmUYf)sBlRSw!
zx?sS=sE0{mXd<EQsa!HvBD53PW8{lxFv%j@mkf{SOiVoq3GpT4MY{3cJKBGJZq%T|
z7Pt`E0Qor9LGDeh=jx84Tu4FDw4rSg7rKa`e;ai|lO1$5%7a%<J&Do*2jdJI9U9NT
zdGSJOaxgn2isMJ0mT&)InIlK_bh}*<PnRna@}a*{T@u_U;%cWs9(`Z}l2YaYqTnDc
zE7BNrcE<}&GL;*((s2#&AZ}SXjJYv;KtC<od|_1LWkx4HRjFIaNo0#Omxg+h!eDRk
z!yyvr#oiV-MnxI(gWmR#FXAyeG38;2j5`q(ZWQuRhgvk$WQbdg6osq|%1L&_a%$qo
zU<ilzh%T&1>y}mF@sW`@O$$ZO-$rlmCgaicq<I4BF_xeyg6Y%}bvQ{Ybun_51P8)V
zE4g93(VO92s+mFdfvq$ZfR)H2)A49b_@l~t`Z_mUVc4A)Q8}K0?o>2AsyS07xwg?j
z3&(M>K59*co*nNwyD@@n$&HM%d@l++@O;Vc#DsB$j0(e{BQL3;V7DhAD#%D;1;DXl
zg5e98d>Es=yQd?<=RJa^c=Tgku3mJx7}7TzeotU?M0ACddgJ2Wd~ZpOiLMd{!!UJ(
z49^R_Ls7d`t;TdV4U3U;8OFefC<6LmK7-)%CkzMyw=3kPo^u;IO0uqp`)I+}@=cMD
zVVVz9QRIxl<R*o-xT+rq<D_N#fI|~~(J*T6ND6E%&4rnfY@?zj1bZSzuv3T#a~(T<
zy)voBn&O!ojXG{md(UQ^|AS%~BdHk9`!RK2%BSTxEe6+Or=#Lx+TwAA{502fd*Bal
zaud<}cMM??k-2^x6Q*c?7S5^K0lZyI$rzk|7)>|t|Cpt4?elY8!_^+-b2M|`#YxmS
zD=I_uO={xNiH09kYaXN1?-Dbsyhq7}6#8?jXgYAr(Xn)?c$VC;d?cGo#y1YelW}wh
zNJi8BLmS0mW{%~9j^)9X_+Q~zj=G>eV);NW$&VAr0ECkQ`mbL;noebL#6T)-T0!tb
zg2-Zg?#iuRv{)h(cRQ(Ehd1c=gtv-myJ-*>gMjj`QZ6+M6l=qHGLuWkX(dNTJ|+9*
zf<rF4sK&7HL;W*jH!R;++M-A}O-0<kkOxy*%$Q4cGdql$*U=FL|6qAF>2#=)39Oit
zkXMK$1Rtj9(1(b5Y=;L2Xu?2h_5`*Dg4+WI9NOH^DknCd8pY?G1LE`|nj&%GZ`^JT
zL$`_<%ZTh)_sUHPc!J27pBSa84U0qS$Ym!^LoWtHHETkH4W;%JHA-n?no|HZ^_*hb
zT9b2CF@hF)FXc)}TH2Tgtitj}PD!VpHKV(9`&^rYSkxIVwfJO7n!S=L=zxnxYl4+F
z#KLe!?x<GGDICoeMI+kCxaItG5S8>)xje&sxDzuTbB&ui%NEn5){{uk#1a!Rt`v0k
z^rGLx8qnOW%SD+x#Hat9QYxhSQVa*Q#S+-zqY912a6}jdD@3qT1g#=i#c-0^pd*LY
zH<-kMC5a?$y{H`pbzYdP^v-CSPj)lt6SKK=a$9sbC#R<7$zaGMM*R*K&Bw5hL+b(~
zH86mMLXoGK?649A_hb1aCw`(Qmvg#;8{`MEc3O1$!=hak?<{qpoTec{dSgqoa7UW8
zlbtd1amSNvZ+=gP8?w-)7na)`QM#lG1i8OR>#MB^tc5+HFuEb-uH4P}`qS0Zi|)=9
z+JTlddC*l#t}w3)F!G|A;y@MZtn&4&xKV)4;lwCkx+o`HaycgxjmPK=1-ndZ0Wsbc
zsxXnk{tos?7$4@Dtl}_Ek;!FbEl*{~)ummWoFHH->!le2#w&NQFj&&zalB=4&5Aqf
zDfi<FCNqsE(qe?eenfsH!)ej?iwW7(`+IyJkh?XyV)WTu4>64~&Aw4mwGXGd<NPMw
z-buTRqRsH8V)C?tH@7L<%K4AlK~&R${5l)W1IOLbX@h8V`~aW+!fJz0$PMg7<K#o^
z5o2!{gofBRrTI|tz6jZZ`y^jKbOb}8o^G`T!iVe<k5-FGGD49-(d}&Z2iwt6c3}*5
zZT1+LxOMkL_#y}GE)weD>tA0O8?1bTjhmFO_%KE0JFTYr!eAdXZH!G}=#~}c?MYf`
zn2b8b9&bNir_##QOjxjf3-u)h?eg1yXnFbBId56T2O9BG6vjESr`xy9Xy0L6?Fj{`
zWA+yozy(XgIKfWtPuM|{hBGp`KEsFkBb}&dDvWC8<}F^+elG2M(<z16Sd5R+E*`Z`
z(!+)2w#Y)Q7HLt6K{`E%WoLLanwC2RVy~;vU#iZ6geWJZ3J-USxu}}I(Y%~WsfbAH
z()Q4@vS^<N^^{+aaV4p3(?$!;cld@CZ7&+MmCn($xr0FhL+uvqYQgboMa(z*2GglM
zg>I?Xk3GE|9*-MiI#yqD+4aWFf^bKOpVyU95phchdsLAWU&|Y*;g}k?=w1(M6VpB3
z5X{v&X<^>7B5xW@S79mYT$#EvCOTp_-}DiOaA5^9m7lB<vs9CZlpOUbY*pBkR8y8B
zVTFdVHt)kwiQ@V3mgUEPoQQ@6!Q|Rs%PXHYIKfN#mJGU>V`TDoTZ-=VbgiVkFcah(
zW_)>%y~602*l&<~a7A-EZe=m4rnS}-+O({hv}%A_7%e*shng{dU>XbO`gUQN6G@4Y
zP0r0Gcj*c0&UncR`x5+?2Cr@^vFzbW)@}TD7w7BrV3ShqjAHsllS{tmEq8d#E+cz_
zR$A3#kN}z34@VJ5Pj!wPmz!U`7S^sXD4)2n!o4C+svJPI`}}sR*@y*)FmDB!to#NX
zmP@ThZg*YP9c<r1b0XgH&}8WExMzOf4<!`wB4TN_HH768IbQ1g%${;EPtK{<rko5Z
z`-sUw-uY=nC!P*hKn#oWwxq}K(UyU_lQ|h3?sj3`XEw6bls3FYXX1%5IwO$F=iPL;
zhnpo5jO^&f7GlAVFb1g=GPV^-AF(!3vj_CmZciA?UF`SpnLc)tb9552XiNDWVN;E&
zfoE08O)NATvsV^8qfL1joD<^=4P*m2#1PYWeg|s><6RTO8ZwDtk=8ueXeg`-F*)_&
zMh!Mqq*Cs5VgL)9bXpv02${LG(VrU5jZ8U0t`?iAP!E#K`{*<dP{TsCx;ngax3@4D
zs~aXEPcY>6gs?--Z$hbwvYOVCcdl-wJ)K;V23)Kd(67_Y()?b#-0zxtC=CT+chmNn
zdN}h#^1-|+dUx50M?3I-0RJ`psAn)uwCVxNNwn%wOB1bnj&c&MdZN-qs~%mQM5~^z
zG|{TZBq!01$!8MPvx(~AS@jU2sc1Z^p3pohZSg)Cs;4tc>D3boN}hU9@w<|*l%6$a
z#z(NCM3_yBa7e}AjPdj+YfNHis}V;yFVAAnRHX38OCzmnW6KjU)<~(LXl95t#>SJ7
zCqj%h9J3quVbKE@B@E!w7`BK(#zt{3y^)XXjak}RZbTDE0GHxNXg9`(jDd7?ByJ4F
zz^hUa+TU-)$NJ-=S%WW(g$yG~x2lB<72nklM&uVAN%Vs+g)H#X$nzC>;FNCGh=V!l
zR8Ek_`*MR|jwS~&p^(W$av-JR(Z0TPe78!`zMzW3f+~s`n~F+Fld9l<0l+ib^sEg%
zW5cgDi&KU?$e*JrQi4{+DO`kqyt$C)k64^Y(RV3$z#JmT|KdTNqN@cdGk@0N<oK-Q
zU(8d5pHhA|P_N*u6Q%!gi<1-chw%}M@f4mbl_4H6ExH0o;R&+5{><tWcsyN1*Mm|p
z9^&D4Qv7(>j9=UEOQb4)b)L+FGDCHKjG&%uKYDVO&Tdis>bw?woq}I@z?xr5e?_Ol
z>w%@SU{v~~&B^KM^?tfWMZidZ(o^X#!R&2_kWCf8I)A3wE-2`MZn{+dieHug7U0bM
zJMB&)#~B_xz}u`MBz`=}ozGwK-vJnrkbZRSt8&Va2l;b-L`vkP;uO-e#%BH#lK+H!
zg8j6a7;#;uRrsC~e%0;KArEvtrVJyJd48E>cOzoXU!7OfS|=!^85B9URX%~p3H%ek
z%El=lI(JI`<AgL_%Ki%UaYW4g>boxLdoLRlp$w_6%-83TF!RsVog6(^8mRRj<bSe+
z-#=6E)BA07T_A$|Z;3b@0)a=e@h_X3={q`tMb(oMP+I<90D<S#^Ze@kr>F2{4qc`6
zrRnEELprJa>11rWp7|g0{~oleZ4sY3|44n`M}5af`M1hnmFpaM%=xSD0i3&#q)o5C
zlJh!f%>2x;lh_z@@Lx72g8Z-IQz0F+V&+%hQyG)|N>3#~@u_qTm<gurcU0zoQ1YvG
zuINkU&6N2!D*_oR{-q^w(v>b1&X@cGTKN66L&Tj05RWN71-^V5{+Ats;yijl*mRZh
zubPIxN_O1;Qqtf``8%fJ|IC#l)2CF&Aay9^4@&-0`yF-(22J{rDuUuy;nk8~;Z=BK
zr-=Pb0c5E76gWN&e<CU<jw-@w_^F#vzGEi+Z^@r2$*?5J{h-7|T+APag|u@txxjS^
z{@3DP`LFUz>Yt~||4bGLTz8frOY;|0Q~z3_U7N)@2}$23QS&4-(h5z~{HGXRmOr<y
z7>-fZ>NJCR+=?Da%|pycD`r}1eo+j!F*R=}hTCznm(vX5aVx9JpIc`}S}~K_S2Y!V
z4LdO<yzrf)VmXt)TSRy)-tym9DyG---!&_S&%}{VPBVzdt@xsf8mG)iD{IJ)KgIA_
z`Evz};j=ML>@<UT+{)(U#{o0aic|ko|6UA-H_Cpq7(V&?R)n{jzE@RDPqX>Smvz!C
zUeH*X9OOz^tdmA<MPJgHNIolblt@tY7fl1dcoLl2RT=x^L@9#i1{PL3={-4;purC6
zWj58W+oqAz!RaU0SGSbUYMqD`t~jEUZ*vKJOu|dg!@L7<(&xCWM=B3p_u^l}DgSC-
z!h!ht3jRs{(GvNmIsIfmoUU9n|G=qC%q!<zNb*a}3nrJBkW<;2zVD{7$$oethfn@)
z+ENMMSK_yAyx<L!%IlMGb$+MNlCigNIg{H9jVn`qe{B?S`aT+chJ`NG_wSSN((Q@2
zb2*%}=!cFdbQD2E;$OI&$%rpa1OFk1PwuaNHVyqBr-9QvaVmY50Y24t<<?I_-vjtm
z-^J?#oa}Z|`WyK-U1<&%ar5sxxSaWuzL!Vep_}UadiP8t=b>rfPfP><9hX1Z-{`=h
zdMN0ucAC{>bQB=LORuHVz{dce>U)2OIelR~qHluGb&SKO7}tv4R+{Dpqq<e+Un3tj
z@v#1M7B>^|0UUgw&e+m{2f697QiByfqGx1BjDGrz9zD$=KDTEA;X`RDV{kathiBhn
zc)lfLL~~<!I-R}&la0q3SFc?|50Xql#J{^Hwy^M8MHVlRh;6V~Ze(N}ET$B{*Ih(v
z;Hd>ymj_QTy6IDZTyq(h_f~_Qo%}g${`Lqc@GZbRbu5!IhUnETJZ)1f8~2LM0NiTF
z%^t(OBjD=tb+D<w3CFkd@}G+Phu<iqXXl(w^}#s$NSyg;sse^SB`3a3C!ek<pac_M
zB{P4MuLS*fzl2!)tve!BRa^YY#J~3yJkxCh^(X}YFyW+c0!~3_euT}0E`9;XgislX
rFCwaM4^IB#q0?#lCSUPQP?$%4w$CK@|Njn_c#EOf_Tqj%-|_u_*z%6W

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/example3b b/3rdparty/qpOASES/qpOASES-3.0/bin/example3b
deleted file mode 100755
index 25466df8c7532a88f8cec57d92eec167ff812b3b..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 30848
zcmeHwd3;;dmH&N`7s*>*>^LMLfFKD3NW8^KoQ+7f<VcAu8A;AU3oo)I$4YF;NOEE`
z5JJL|v;+ceA#{OsI_-~c1GBWHWd=e+p?gCME!}BLX+oi;P?-D*#q&LPk)C9^{O0rP
z{Kxy)y7!*#o_p@O>wE9M>plK%i>5JkS=eQaQs1j_a*+6CY;=GpC9H$lnUh_}7O)CH
z%JJ{y7z#y9g=eH_71|`76=50vJ1hdwVR7<^3g-(jQ=!t3Ldq|HIb~FYJ7<cd3K?^7
z0P&HoUq6Q@5pL%Z6&{y*JSRiNM@iy4CHYQCJ{9VcUWKZBR5tpLNPNWNM2gZ*3YeS!
z=0l>TtM|9r7{#BR!IKD`Jfgz20?bsX_$~(@>G}V9>5?!c^PA2t`T7wIsveq$6a6dN
zn}=gdhZD)M@ulM(E0(TkZ_1>a+IYE1KGALn^l?UI$2mxm&a?1OK0x>*&YM2%+4;=#
z!)L$erp$*HTz9pl<1>UK6A)h|{*UPUZ0!75wYIXd+Jj}xMYsn1&%^&td)E)Y@|!!a
z?eskJ(#7Z8I=^Q1-PLXVKYjUgn|^crC*S$az0Y2~b^gxlKYZ<%5C5sIX4bFoeenl>
ztef!d`XX4Ty-2pB1onby;1eipeF^&EY2bUOfghd*zG9kuL({<R)5!Tg=u7GO<!Ru5
zng$-8M*i?L^w*;zbvBocJGO%&fBh@u=%~f}i{STh_*{0d2!1o*GeLhu!c}{_0rDLs
z<lh8%J*&<2XDa7Wz-Pk4oOMnPQtjwgNq@Xnz~hqPQ&P`jeLf60$ses2^c#?->n<**
zn(eD{^0fNdF6Fn#e39hWP7beQ?Gmo&Tey72oKn7mGqk^}M}w}gCrpiQSyKx$j2$DX
zq>;%+(^<n{#>U-7D83_+$;Q)N!_iD8o?(VB(qqKpX$g+>bPcDH@kq3PI4*FdF;@gu
z%Dh^ar^k}P%X|Cd$$_DfXnL2Ychf*1Y_zs^rc%T4XfiS}8ux@Y0kkqWnoXpV8BZt?
ziiKAi;*S}5VI3nAz3D_UJ4iyz2*uiwiDx6Jp589c?ow!XYiW2F!C{|FHa2kSrDUEJ
zWSsGFV>F)5q>|C$M0Ud1-9j`w1_q1_k+d?SFS#d?jKOG`v5`0-2NdrpSJN06+Qml4
zvKjgr7>cGDa!8G3A)JV%<wv54Bs1c%Xf_H&e<mZ~03~Crc`TEruCqCsMU^b=k7nW`
zy1IENH4<-5WQGzu#?q<gfoOJDB5CZ&G!G0VVySeZxqmD%9BUp*k7gz^&CAlUQB?Cp
zv#b{tE`=kI{LJd+)M&Ojoys($NQoUO0={z|vH0%zFp;Vv4P?fQ!9+3v?+BtCF{m__
ziN}DZ@~M)E;u9@Mvus0Gmq8BMwRNkj(`U3cEn^$}zRoVA4Sw35i?=l`SKe&FSYY9O
zy@H5QUc(ro0WL!GuWGLZ^YFIGm;=nc7fw_Y??+|BBK-L#{0|=3h3P{X`==b<!ulni
z^3Kjp*lA>X0C*b1#^zDl!XB3NXU^MBWPDBlw%n!qBbu9-``0<LpQ16$Oz$wktvM--
zEjGbb842BDf}7_*9VWPW{CAq*C_wJ=n&4GADC}l}!+^PKs|j9{gEF?=1Xpu3LJyhX
zwI=#e6THp@_a1o3=6!$pNTY*!Z#teWKXo>c-UE-@P6(pO<^4cSE{frI#(7S}C_O~T
z)6Y%fT66`a$+V|W@^pyOR9mNy^YlhaTPgh@Pj^z9OndqmPp_slne_A#o^GQwney~L
zp1y?ARDGvMdHMoM)39@TJ5SG}G@1BxFHhG~noN7z$<tMoCX=3SK|1Wc;Uyi~dJkW_
z(0OVvAR=}nkGU6iygEa#ZH9XO!~TVf{fFHP9bUxddk?u6+Pp}%cn?PwI$Pg()Qyqi
zN%ul0Bs1@!vW1AxUjoUm&Y0<6=s0x%0%6b#9^D7JQ_GP$@GY5F2jz9c<D|x+XWw(;
z(QAp~-`<{_d^AY0Pg87=V)r2iecTHdKf*8%eUz;H5dEC`kixV)(o%@|2ZiZ)WP2ed
zN^;3Cr*;UM@*t|^ks}3Eug=(pBA;3giq@Z=_H(PiQr<%+PTc^Q_s}m--Ggv&#(6hF
z5=#F5@^3UaV1V-PBRJ9evhUDSS9uTp+I!&nS9&A9Cy&!W>wWSu8qd8?K1sv$sWX3_
zoc!~Q^Y#({L$|{*`@BnOT=yP0Q|CSO+dY;mPdr38a>zel`6!_#pHuXdQuL3Vv4Y-v
z=&RnxUR>|hPI{j@GY00b2<GpD*?Z^-!T)Q?zi-|7(36eL>;osS|0|y;uLDSX?F9L^
z_rN-@4ub5tITdF0J^CD09iX2)dFAPUfJtVY$4+NN=-T2sH0j#xJM@OD&v)n<SHypK
z(JM0?Q1S56GYG@}i+|htQ}1JcwNgtvb@(mV@TQlu=PLX8554F=^oAQUCTIQ3d*FoT
zUG@917pVch=StUmU01rUav3KE2b&cAYbRhAYO}J5Qt#Y^F$#lTM@uuq)WFfH<^~QL
zNr$tU)ofkKi+sVCbfnUlkvOs`2c{N|>`)@(h>u4{Mu+1|922QAM=~Cd5jK|ElN?S(
zV+!j?rkWfrZB>Km)Ckz(j$jf?k+`EkgRa!@aD0H~A&x!q{!AhpcMK-dne6$h?;DAZ
zCq~9b9J>>#;V7rW{E5P78k2<84!RaKEsIZ2KNyc@68#C9QwZ{RpdUz4*+$|?%+mx<
zDfwN!eU5BmBrZjAJ-8-=j*-Oha3T{QNF`&LDy0{>9Alz4V9wi}+S%4Mv__<k?CXD-
zoIHr|=$n(1ClQ{46P!W#UkJBS8ts8PgRN*-Td8^Dnk53JT@zy3xUQXDQ)xS*Rq6z%
z{{C+8ULt8IC`}KIfFt@2=s<b1I=#DY&c+$ldu;pI`gv<ES+?*30;@4ZyQbIAbF8$|
zl-DZq6>><{amb-LcA0;sley~j1C|YS$1Ijxs_OKvI-9GS^tcuOr%+Z}BdW`d$Uou#
zB=n9Gu(M9T-Qua6bDPyu=Qv#Esat$&xwo$6<_d3J$AQY8I_Gd*hpVo|Rkyga&e2&n
z2lSnFHeSw`z<vgD<dlG<@oREYn^%jG<gD1gTG#;%v7_1tG{nDZq4DM2>s<@HYk_wy
z@U8{kwZOX;c-I2&THsv^yla7XE%0}-z!7=B;<&tTJ1WykxLnrd_8?XId>K(`+DoNN
zrD<=JF142Wl1$eKaPIy_txT)?7jtD=-Cw0`J-TR5l`gt(N7u9ReyUpk(Z(8GufIK+
zqVTB2NkQ(O??dGxP3u~IS)Dvk>)<ElqD<{!&Jk3sQHCn8stC4o=u&Grhb*63&(g*p
zU1y1aZMQplOu<`ZTIt;)>C~R9${$N>UY_GN!H?Y=o>qFiBGdaMUW)l&57d3^;*7pn
zCG>nkhCh_y3o?8|h80+((=}U$7s_yj47+8xS%!l$<V>a?+%s71Slrh?mduVh+M7C>
z+LyMC@l@;H*7l~B_NHYQ3pl^^Evn1L1r*<UQ%yj*tSJ{BZGdl<@t95PhToR8Vnh0~
zpx$0YX<29o_UU$jt+vWXP?c8A_Bgn$mdc0Wu-3B56C7Ca1P7MC46i9GE5|J@FqC(~
z=PV7ND?dOx4wip87ttFj3TZckvD{W+yMvNi#djdUvSK#&Yg#@5NaagFKU?|w0+iEs
z34EmTjjI4=wkOf<D&KrRMQsDft@163>$Vr5Y2{xjKF78mMX7w7;tpE~?W}T=;*0HH
zhOkPeQKH5EIFzZ>c%s987o5J*!V^yW$B4a*C%pCu{JgT9CwlEG(da5Gcw(#FO<LKs
z_dw`&`*V0YqOwYhA~9rdLbX)ZXr%C{{bj=FJdw5k1f{C1*M@-^x3`l9v$akn_Suv5
zNSvkhBC+58LHt(E(RM)E0sAiEnXm0Z;-LK(lvt$g0l^XbFYu^GWs`OYiKF)4QlgnB
zj@e(LL<>*cZ-0#vtvvCd{V$Yg(=G<jLv|k-vz_a9-2O1BvqHNL`8>|5zC$W}xbh32
zm{WO-3TgX4=wd2APEls7B(we_71w6_CH8hIKSib0S%vKqz$$E1ht+rBht#gwj|f4j
zHaYaLf;Is90VHaPww7enEruu9ZoUxDAIw-?4R^BL`4|ZF6EG0d*T6$GHsehgwaoUB
zR|&BOD(=-6z~<NM2D!ZcU+@b}|0e3xqCXGitZc@;(6`L?qXnol-9Rl~ug|9rS?|KH
zre6v+i+&N>gjL@Ta2cD?h3v|#&X+-}d*EBw>v8ZheG&RWO@AATS@eJ541Oq4rdJ@h
zay>x~tZU>MdK2I_eFsF_^#(wy^!=c()~|z|YxIwyB5U<$&_e3;^^iG3-%f?mH=*1!
z^{Y_6dR;?ZHR!d-eU?tQ>l*dTQL@?khau-I{fFS1qdx{s=IT#D<~;p&#Lw3MfcSj<
zY&5%b^ar5Tx%%aRpQm3B9US@>Apd;574Zx7Z=vlk(652L7wQ_ifJJ%}taYLOTgbmi
ze;K+h*0V6c#rnsP?<M+9@LsCl1+A9ouc4$%bwBiM(uYw4&H7o;r$xUF_0Xz^AfZja
z1;$;bUxHlP^+s?l*B^z<75cp}<Vt-vWOnG^!0#&Eja*jizl64H^aONRt3L~8Tc<w{
z-plkMlxV$v9FmVSy&9m$n7$17uQGi%hSL*FKMw65XL<~c^=nMmpxYBn|1eTdGW{ON
z{yNh`F!(o^-UN-m$@C`x{1(&y2s?b6>F0s+J7Q%1F4OOX%<nP%KY%;Q^q-;7Pci)_
zkbEC9Vb&io{aje|X{LVysy@T?KLGbbroR`({Snjm!Wz#q{mUrWkD30j(DNruzZH}}
zWqLD&{fy~9fasrt9l<Y{{&^VgmrTDD_52*u-v-~WnEon&zh?ReP_W-H{c@D(d8R)P
zFa9mlAA|{BVEQ}&f5-Gs!c$K%Jq?vk!}pQ<i%g$@v3}3=XJP4=n7$Ih%a8&k{=oFF
zp?rU2`UvXc6{dd|CHfQ7Uq<jM(_aF~YfLXgQU1*I3fSy*rU#M38!$4e@V{VPq|PvX
zHZuCZOn(dve_^^E`Mt^XzoHCpG5vNF`mc});M+|9Hng5(`WS%wH2rGGyg}1xnzCQh
ze~EfHpy~8*+>M(4G-TeS>GPoA&6?f=^W388vthG?n*M754{7>!(DQwo{#oeqeog-q
z3U;feUx47SroReLzD?7=4Fzx4^jc(lL}N2+z`xkO@?oe}|0!72{_m>+VV3$QKw$p@
zC2W@Z-=PleU%Cni-BN#)Fb`2;mZiQIf&I&rm;($6_zETFTk3yFJP%XCVQHXj>>D(4
z!JYV@<)lmmbfb0+Aml+D+DIG{70^Be@Ua}Kn{!o@tdRB~zz-|bEY3B1Jqm8`n-6>W
zL8Scv=wB$3M$SEZ0nA{3?|Wf<>h82RfYy*O<vn`|3~e8nKpk+Bxj-)@k`QDzZY3RN
zcfq^tiErSSldJ`LLlH>^USc181TBhm(+r?@=17FPeW)(`?l$~#lKns*E+Sb@CcX9p
zP@0o`66nt=5;psbRG59&p-MUS1Tf#oV?RO~+|mdq;!6Gsn3o7k#usLu{UR0h))jzq
zk}|l#4E&o(=w`S5jsz^rNiG6<WdVs4Re`2yzk3tf&Hb>P76i=|M8~Zr$~%KdKK==`
zMG|F613j)pvDtn!7W*eZ38gvq2rwT}SW$Ury#dMg&;1wzNTB5bLg(ePS{jfn$5AHR
z;bkaf?QsyyEJxcoS=I4vfa`rIM%6VmWHPP(Ay8LcOWKxc^>2W_>N-l;wEB;pi^N_^
z)M)kV;O|w}Qv$8K7XDJTj}o)A`a`JPsv9UVN2@=Vc=nTC^R@cBky+INN;tIo+34@9
zZX~jWTKxzmZYISRYxO;7^;Ne}qD8A;O8Fe5M2A+thR6<4!l~7lk#6s!gcpip%&dAp
zC3>~`_mY5HDX~?nZ$Uw;ZllC@t-gZPxt$V2T74Tlr0NJIMz#9e(Z^KXL5Xp#z7bVY
zbtg>~_G$HBqkKL<n1fpVKa;c%QW1`5^<M$3>O+J%s?{Gs<yGB9m}6Rf1Lbo!VeZ%J
zucX9LN<665PmrnZp~OSb4W3%{5wgK?t^Q3i{6|T^<68ZvDDg2$d|j(|63@Mq__o$S
z6<4)hy9d>MAO2_22+IVtQ+oiAuPA8aC`XkML$~%#fS*w)Q_oQ4Yk+s|2C4Qk&}S5h
z;GX@D=r*c?*Qz$HqY9}T7un9HTcuUu1+X1&22H@U=dm@ARkdXgs)l3zz=ZPHGI(~?
zd%h17aO_TCM)TP7Fp5>hwxjkr_8>5~<*}nwL5Uy$9Q!FmK1bMVsl;bdo2klBGa)Q@
zeXg%4601E4%$$FRXj}EFr@(e62FJPojPwVHU27mz^*Ze>pe#Vm`UJNlhdSZxm?bG_
z<EIHVuNfAsc55U{YXfeb!kg?|y<r5T4}f0l1A4n65hBih3W(~^G1!^vS+nRCF6~+(
zqX`)APa0!9h95Lz!q0yYW|~vesND^^+Ymp8ek$rKyz?tA(6qUqK}r<rT%y#z2Qr!<
z&3YIzxN@@|Baz=Fk%Ht^pe>*+B4Ovz^EowVYp<iSi5n_1%2itc;){vlX%ceYpJCh@
zhxQ(6eUNeGw2dH1Dze60|0Ih9&c>@q6DmD(yotO7+jYcJs4Yr-1$r#K=2|N39<<`Q
z5u`;q&wm;YTT`J?qq14fB|`0MAUjENenL4sM`|_@qGq4=9KbIq6q~mP{!(*5Be~j}
z_^E*JaxPJk7kr*<aPzlOHmV^`b^%BhD>Amg3-i>xuMQqanSsj6sl1>F5Y-Em`+{z?
zjGEgPqgSV%090k1DhY}SMI}tJpn~lALFc)01hMGlob(=$-KR*|g4fUxYwj^HXi9+<
zoa#|fd{a?LffvBLYwr6M%qB@IIq8ca`%3|-q+gHzqvn&{_~mWQqT4t<&A8g}&r8Hf
zMWvEZUX^UY?PTN6KY-p#a@sj(1YF6#&G|aUkeV-atG>aaS8>kU!1bj<PM)1;7BFmM
zE_bQ?wJ>qb@e#}yqzcts(2pPlk20H;e+NNaqi_8hJ|P9wa6t|T*_so?y*lsOe^X>5
zc_>@J75wpPIIWah%jJ$kCQVp{b^j-F*@Bbgg1@VR38lz7F7jIt_d^oNTLJ0D7JQK0
z<`tShQR9Oj&fuhPfb1=mJ6k}#ZO!XXV(^s$bxt)OE_4z8g^sj7XS2!2Yu>sT3{*UA
zJ<uLS!WJ%rE7VNhLRoDC)F2FL!xp^&>Kco76iREOIWViG9)?@wLQij))wX>LYTM?A
z4jnDm9$b4-3+lCY6^H%!x}Us$^3_LHUo;L=)~@3)=dE8DKj(o)_u?neY->C3g<Xza
z97VaT%P&N(m;5Jkt~>dJoLlJtHBu>f&G50JQG*9I?k7ay54*GxEqKO2Bgn}noR79E
zjzXlhtn7Cvht*cLA3=qUy0i)#&BB)0Xi2ez$18Ze!nPXHD{NF96*khk!bZc>5*y9T
zmhgCmj1$dL5=K2;6A14?*lg=Wy&U91Xhc|IqtR@Mjp7wNUST66R@k28G~dB*%Mu&)
znicYwDszeLS`MM#R_YL~e*s}F>ap!&6oN-iz}iPqU{*_Ywk!!Mj6Bu_AX={oc|;Mm
z^?}$86sx5kyq1QVRvJ64TL9dtcz8tduv!{z+wX!OSq7=F)>}chd=t!V8Kl~2Vk<s`
z;a2-Rfqwzd)xt!!d@&6fJjzxMK~wEg0vGRdY9BoZUeEyyvmvrd3x5nUSAT~XT6l|G
zbC{&JQYBdLIJS4qTj(mZ<y@0H-nwqhXP}+7f~vxL$G*L5{BRy^rAQv!yXFx3R+T)m
zcTE|ImDr<u*QTJdwZ=)AGib{%&9@Kvt-Btm%W5U=A!xQ<>$6+wAyaX2UWL!`Zv2lN
z?^(sX^jNBiSK)KKo6h8T&oAbs2U$(L3Xj6^OY^V8JjF>m({<Sz$^Si}x7NB|i`cP)
z=HzH>%}~J=QBFwDekEiH3F#{-gdUU?7Z;+4azX-6Dj{1*NUEfe?S(=VQO*>>NhRb)
z5;7r$yeNyanlTrZ;06hA0g=}0MBQ=|JV-nrDVZ@nrYo+_{naAc2R76#xfbiXRRU?w
zoHQeTE)h?oIet|j>{fbsnJ!pYYgw;dPBPqD$O^Z#RxQ!iYg@@B*K6CXcO2Zi-m(WC
zdW1ro^^T*+?DtY8^>e>cGU-8Qes!La=r>Gycpi|&QW~?%z9b=)=otaEWWe3M4Lrvs
z0u7g6+DET~{WehRoq`H`KMwHZr0*^S=&v`v0Md&4X>WLA9s0?NPg8W$4JduCv)}X~
zN~vys)2At=I`~>2vh@EaK*xH%fnU4zm}5KFBJlBA45av{22B1bdg@>;TD4^h0jZY7
z^hNY80S5$Jf`3Bt{+}Y~ASvjjSI~CRpm2zQOq;1Ks%xmW)}39~ch*czhaD`lnYC_p
z^_prQ>}*}gs;e&p9;;~?MUPcJ15lKb0-~yH(5!XU)x@MBX{*gi=m42rCd+*dl&n(n
zv0l(LUjuQ}oX>m>%|O;(hzJI?RS>Ae%_u97t}ll4`LM`LO933~8l`tfbsJ<gG->PS
z3Q$}1+PN{F)2wo2&`t)R2nPH-QL@h0kS9E=6aq8%e5U7cfpaNaDp~tn%5+|y&0l?v
zN#5Cc1ZCu}?wO;|w6Ug&QZA&PGauS@%<=ProGVx<167Rka?;9+rI9@y0!<nyE^L9D
z>w$E=ASa|cS!Y&=L@Y2P3-u-RLNhwA*P<L4dI>Og*V{3d5FWL5?tJuG7Xct$OaK-a
z0HE5%W&qqdCl;Fe8rIGw6)r6X!pU{%kxLt;XEmX9ux4^pswGSvE=0PuN!!ris1_d4
zR?aJsd}0~Fp=i0nwdb?&fuWt}yPOzAaj>)Bgqmxg>)?*R(o8;6tFLs<xx16|bX047
zXOYsYRx@IqgXrpf8Rw(OYw}48lwNy|*4jWawpMTB!q=6vR&ZT*XupP5OVRc3+m9#l
z=iJc=awlOeXudAu3n4ef*C`k%^Df0p33rbBP7>e|ICO@hL<Q<^Fe`)oD#~m=mWVzc
zf$|&k%CDch7N*>USH2pkuKhd$k{*gs$pXSkRFUnVC0&AM!wFEYq9Pj9*)!-+EsSmt
zYFOFU;#WggMxC$rcYKRZzJ!nw@9ZU#1H)sncr)H}HdeIDm!5Yd$C~>ScpvcZn61i4
zzSx(4V-RmDPR$*6%JG69SFJ$L|3!_O<Kx+MG{cxQ(6auffzeU2KD{59YHJ!o<*=T3
zCKKHe_ePU3yp^$|X$T}(T!17N9|Q>|`dPdxni15T6H)jPUOr5uM3mko1%cr7C1de%
zy!Vh*0Qq{UN_OE@Q|N_z@4e|%Y-}J$*AvaA69aicoG_Zq<|N~t6>!DVgVBLFmxfWh
zFPj+75n$Z1sL8kWQtg)etopsy>{e^de(TkiMVfWSersmq0c*{mb$qL}YU6#(f?=w5
zEd}?LS*(qpLvqNnTSI)#eu_U&u|dRkZnZ3GD6?AoHOmaRamW)fTyA$~urJ^?x?MiM
z$8Gom-5yui*Xi^7BHI{Ignd1|evc83xFUUFqql_v`@+7!1|#Iz(C2rBd|_9_7Ys1G
zVoEQk?#^Znye&Gy!Vv?KJ>9;5$8CP0wsqwc>qWzOi#ObY*N8JisdTn~Y*6Iw^>s%K
zw<qEW_4ooj7pW*KdNG*4F*=+Y*kxonXCP>JJ>js=6)?iRo-SXv4?2`6PAdm)afJd@
zppxpY%K1WG%PRt#)H}*6c#V&@w!j^dnFwB@$>0TJypf#e^MscfiCFKZ7=B_21{=e+
zcv%;(*k<V!SMefnTD%BMufC%E{QX`2Lb5wP7{z-{T+z#W4VORUak;mlrhE~f%kK-L
zV7-2$$K&?(^{~NoJl+!@Nu?(^y*Jnsj0AhPq53?%%5-FgzJRL}wHgVc5?yYi(-Y|O
z_P9cu%=J|G!Zi1m&F~V=xWyOo8h&4(Z>tdu_=Tr<LZM*DWJBWc!2z~HN$zP%0lbu*
zGhg0Ht?fJF*}&LHe?09<r=t^|-rj_$vMsJ&E+@wi6Yas<OmS|c!s56BF8{W$$GE&V
z6hsMoSS%GtWxMlc!TZHAUosO<3yXTT_=;5S4fz5Qzh|?@Z*+NGfeqxmgTv7s8LkYO
zUFu;(E@uY4m5$fERqcd>{yu69)Ludnu!j4(y5NZ2eSTOv(iaM_y~1HQN5~U~kwn{L
z$+3}89P076wZhr>sE{h!r>b^WSH!p3Bbj@Gn?2^{%fX&NxG&`K`;?Yl+qzIu8#oTl
zy(<_9M?x-?Q4tij`FN7v&-YwCM(?dB;u&hjiUJ<f*W2p}G3bVx;*Gt3*ci>G4eDNb
z7CFBZ%~ja}J>Ezn$=`L0;MIM!l>tE?2u6%;<SEq6^mqbfG^$b<rmxqSQaWE?Go0RS
zxI!EHh&oIjoAV~w7?<-m==pp8!U^+rR@7(l7S_!>uBl>H#l}X56L>d1jyL>^1@#5E
zZXS04)yoD_qoT!Ov(4z~gUvcUMsG0ei^$6I1|!@QaEu^KSgNNjt~j_CWooLh5>C(u
zUoR|^(T!H<?`6hlDnmVk*SEpT1))y_CGQCWTf!m6%N~jkC-{L37!>*80{H_I@=bG9
zG!_#r(<9%_kM7Plz98Ow?Q!{yobPwjh(dnj4{m|8WcEZy1-EeMU}psGNB!-VU}zIU
zQyUl@8y*(bj}9yByTZd_>C~v_YK#~-ya5LjWG9&L&=6kQ#8A(POS!cuGRjGwL>*l)
z;9=Cmq%bs*(9YCYGFBwC8`)#zi)S#)BHI@YkLXNHJqZc%CF2FU@!mT+uxo78pwkw(
z5ZM6vIMzk(O|9pOu7X@hLD96KZ2=d$h@gKnbwX1ebOp+TcTPQt;sFQa3>zJq$iR8=
zMrv|Lc1RS*k3KEe{=+gyj_T`myCR+*S0v;^f2F!4xKG5@NrOE4zyu_v%mYN;L0a3=
z7<6{W^G;IAjoRtF26zy+tQ^MNm_4AM7HvL1D)BO-6E9Wjc5)KgBF&|to+Llm8~k*L
z1bVS+;>M^bV{Xvf67oenMmMHBERk_1qQZ?r9_mmFhMEj<i;*Iql|eblj#y4j{1^=3
z@E*~H6=>bMJUlTn5~pdQ==nS7_1$DVnw~OGKt0A1G(|8STcS=UX{Rnm&XV9jIBR8W
z7_anZc$aEskbPh~O$A^j^2l^N8WaAgvYx)q4ObX;=S5VGXP`S3O^<5MR7tMQG|<9v
zT&$m3Q@&@%i_UJ0AREU<Mp>>Gg&lalWOri1xI#vS;n0y6)ljh46A%?-B(VhG6B|xa
z)Jw7+*H-kY7_&DReotUSMD%r2y4%9;dT&vch`ta<zc6)349^SAzo22NW??#ghQ-Kx
z3}bLacnLaLK1JXYB=q<Jw=3kPu5mMZLNcF+J7M0La&3)}VVdw#QRI}s<Oli2wtN6b
z-=t*&fX5Pj!8mE|De`QsE%~XBY=pwqgMASr*eyhaxsKhwtum<wlftPP4J&R>XWs^#
z@PlF*BdHip^fC2M%4y|5EJn{lucG2&UgB|u{4|+$dr;wS@&nPmcMV~dkl8hX8Buf~
z3-42o4tExFE(Wh2M%&CeIHn<7``l#KaCHXxB+T47aS}C<ipmh3kD6I@qh$xxqQ>a<
zyTsHf=TUN{gbtf3nvN86bX|I>cv{@CbR;{LjIY}fPsY*Sk&LDXhSrJW${b5~IF<&N
z;lItX6m>zJz|z67BtI}90}##v7}&LRG@Z)g5P?+Mv_#;?0FlM`<dj>zU^PT2?sihS
zE^pB932zefa?>a)dU@qt#awEdCl-J4WM(WKr==Pl>XaOq_<OGM)piG#_x$h$xvVFR
z)Vhs)0&7Qj7&CUGZkvkRED_Ue#_bDvFe}ESySVgbmsC?jI_BUVR%laBm@1kBi)jt{
zomhqNk)95Yh?u7bXd?ldbC8;SflYznmVkjKWA3U|Gclh$#iy==;shj`WpUwuza1Zj
zo*9##5!pBImHQg-HIXqtbxJt~)|WIE$Z>#1Z;YpE;)O0Dl-g6!?4^xqS^`w*Y00#;
zCa1e%5YG3H%2|`NIx>%W`4y6!)s~)kqeFK4TpNN|7aA_Leq~9T0Fx@{;tK|Rf)zLQ
z{D?^Is21HR9PbrHGhN5H<@|J*mGo4(!o!rh8<Qq;jhnjTR@1!LlSt6q6mvPQ6m<4%
zMbC<5q<P?w>pFLcuOK+3R7myK7;tEpCa}#%6&j7<m@x|4M6gT*?IKvt1&BqvFJPJ%
zV&&}a>xzI%?NF!#$fO5!N7H=roIw|y9ZM%SM~BDcRM$MY40*&XqRU0|JM0V5I>Sf}
z4r1X^;5H_sEQ1yNSdPhwq!>8l95L?`xiPbo7P0=YXk>*uRXr%QX+)B)+1es}lU4y_
zbj+mO@g&=u+vDLz$&VoU<-4#}(b_r?<jy0jLTz<mZS4t#(I+Y2<v!2Xv#!3a=v`f*
zZD>|g$9Sb=n|YmqfgWuZC$vz{C3DKHXQ=?)!--M86jC0!_+m~bT9VNn3ig=R7GikK
zS79Q9{UYqSFg~kbvWmkvmS!v?Yq^viR}}Ymaz28ov6m(!7#`ih{MbyV(eY-&H7o43
zOYZOGO=cRtq{Rq_{f*qph|^*q5c9v%`-Oa8(UC}IY3_m!oa-TGO{UpAN~-qtRNtK2
z^xHdS*OK8l!yR0v*+O}5?ohUs(=9c!so6(v-Hzsg^YiHVLNq#lBG5p7^};9k26nb_
zlp^+=vDXYjL+r!S^s8{6g>1onlCMX)f}v1fui8@KGlU|KR*O?ILV-ci2W{{NJJD<Q
zV1vW8!DC>a*xMK3izc+YNT`pmr+r~;*zyfKZc@I2#EhKp+?whObBmy9qpgIYTieXr
zowRZ>HQy3@#shraODkP7Vcz<!)R*M7%WWs3<>eLwyk!-hki<)oAN0tcZr^63bDMF6
zClsV!+Mi!g=PeE61iQIEVMj_D&d7}Q8$L`|>4-*CVN^3WZyB5RhiM<3jy=RiWqh1=
z1*vtC9xg23MHXuHNsC;J*y$Zu#)e0uX}RMd_RjMCrRr)(h;l-z@Nln~`l{(7Ed!{O
ziior>ZV#=?3igdqPr3CSSCZN`ZQRi0hi`z<cBMgE^Bhf^MHnP7?ry{`862Nh*nIPF
zM>@49-z^pTv1e<S$K%ETkHwx`<?*&RhRNxUBu?z2hK)_CNQy7!jnr^V4K#GK2j<6Q
zjeD-S9;U3!TibGF!DJIfqu!9Idt;*Kbn{&waSRt$D5WZPxtL>`s<h~^Pf@sBC!U*d
z6bQ?E@~WKEK_!ak21u4O0M10if?;ZPt>U#sJDlL9e2Yd&%n&lU+bspRdU}>oUYN)6
z9W%bZ$7W%4Tx>YVO}K&?8Mm^S64P3&gf^`!r>rWV7KXR({IO<?ADPC&WBog^42q=0
zP$XwoQ~Tfq^*_91`HcyFPlH!Cl~{I3Me90#zl-yAd$3EXwni~|qNyR@^p;yZW|xso
zx1Cn~=+!|cHpEc`(o>z}#^vT#xB0a#49aI3EPu0zlNQZ5$(6+n$ZW*CLzwr1OjiCo
z9G6S2M(%fA-W%-PNRt@eQ_$??Z@8zNukl%IUi0F8`IRWYn0fJ1=jP><gL!IAwN`TW
zqB=4$n&+IKT0il0xdLJwllLV(hL5%s)UC{^=y0zK6E(AuWd+lYx9ChfF-E5Za{0Vp
z4fk=gM1qlRz1Ty{`w<2WwSvaJBIzSm7HYUh=j!%^u~@}^5TB=GJ9&)GLKbZ)w<T<<
zQ8iL5FS?6`Mq~EMyk~SM4})`JV4#s_5XTr|zRho8wK3i`F{}WS7{_SEf{lp$nhvu`
zAMVs((?u%fPA3MjEJ>%uv4)U2mNo`b!($^QhqM)9Hx=qZl6fPYI&NxMs8&~(S8n&_
zM^AOfB;*N(+@27&$oX9;HM3RoQ1Z?d?X>4JmZT99Q+)L6bhk9O*)I3PN{@Y^Anb11
zUQ>@}{-b;XuZrGYcH-d<ydl7UO+V@>OcSko#BvI)df3uLtDdBsLaUysG|{SuSEtab
z=POOL>OsjVwBz!rMD=u{dVE$rhG;4pkE&-hk4RhGB}4UmW-+~bL_x_@k1Bpo@)gsw
zrp&|$mVgMeX%P;o7@RSl9%W5Q?0hxh5a*>??5T<r9(rk_WoT?^BF361H5APZv8LEW
z67ocdv4&%I<3=oc;DUq!TpGg`G04~`Zl*W!k-aHPTgy#o0tw(!{1EM?_>eJ}j*i5Q
zp%{2o3PJ}4jQIFKd^BtDm8_6qMCo3&kfGu`2f&E@q9cg`@THIiewui`A`hI?&6;pD
zC!Na)(s=*a4lqZPJ20V;$wYE6rQ*^4{&ak|O3}Wdio=2`iYb<gN=Z|y;C~FjGurg1
z4LxGRuMUe-hCIlfqbX8?R>dh?fPcKXkmHY7oJi4kDR{sfBFO#XIh}&56)7`+*5c&&
ztmI$FQ~94_em78W;H(p+|51yR6Z41h5sUE@o-36h9x*Mr0!ZNrvYh_R>J)f9TtwGH
zQZSz3;dWB|c-V|zoAFDeDt~pJ%tJCmdNJF4k*&=aJvmEfw<t1oUJJfX!7n^u%`c_D
zqEq26VCgIvl|F8Ba(a5bpRQ36Fw&p&RQgLWy9N=msp41X&$QSD1wGJBm&#x9tMcCn
zoS9#pXR}@MJ7j^0A5U`U@>l$~0Y)UGA6@&ZoHFD=?pz;{5_zdOh4iekng5vNKPI1G
zKWQdL9M3lMONAdq%FM4iy-{)sTwhg&5y>3COtKFnV&+%p6}8t13TXyK&TW-TAaV@<
z#ILe(%7@OKlK&_nO_#F20^N^@nO}X^MSbsOts;~m)s^}BA`)i)Il7ah=STy!zeE1V
zi}?LB1-~D!647<82y(wg;&2EA9?8bPY;LCS=m-{7Pf9>>`F{ZfJyx&y)%j1);>{eo
zis_5fFM@`2Qv7r>HeGZA4PC|b#pyqSMzt-XSLYw8@B66l_$dEY{Hk1Mz+=u|eGlNw
zc_eN6{FR))g2v3x9NUSFF$e!;<08oYDn1p`K`Ul{^*xnw$*=TO0u-N0*MON|%6>;=
z{s$$$YUhf+Sl&#Te~TiJq2j-^2u`}vrNViVUqJJ}|K<>JXCB03icf(ro`(N5hoCr%
z9uPKN#r(^s;jfY%_h*V4Trq#wH2nX5naK3Vs$-Bk6!Qlqf3f`zy99$p`jIMv;#c7n
zl3(Ffc>8t{`<VjBQ1K~nd>Z~;Q9*G;5l+KT-GuTTGwGX>KU0)pQIh*Xk%_pNSB8bO
zGc>utwFv*K@UQ$=`6czwrSd<O1p?R6B4ly?f~xed720)JoKukWZ4xz4G9#_fM9qH+
z;bpmV>k8o*Rjp1lh{vtyk<>iIjI?5=rREofa2r$ehC;X<Cwn=~ARf1}s@%DCW~3D}
zseM(Y=xf-qlJNX@jtb>W{caKAv2@FQU#XB@&wbad5Iz$}IyucC9=GC)Dr%fEBdx3<
zH~tjDXXVZnEQB{=oY-y#@wk=E&W!_Rq!p+Bss6nX4sVqGW+8m)_pJzTHGQwDke+7q
zQ!ne3S-haJGC9Z<u~?^!+KRrYGm(5&<|vY&=+B=9e!&ztwW~7rr^#XjjqMgzJLNsO
z1%PX?gL;`wwQFkQrR2Cd{nYxpT*_y)PQ>!pAfl9SOA&lR!i&$tyd7}T=cuelDi2-v
z;$Ool|7u>sf%y3v{z?9kBKc2o`l)_6UAbQ2^fQ@P&byH0*W38ja7tw<|14#nDk!Y6
zseb6-@TuQTqc6i~_&(acBEMb53*IoLyc;E4o!=?dWLI)IQ`^fR;HAF5b~WJieYE25
zo!ube#hVicxg3>SKD>Jx_-DDCJe`XD=QQviaro5!>gUtYzd8+kCI&Hw%FGll1-#UE
z<<?F^zZvjS-^CjMoa}a7`deJ)k>PMthBEqoE@$49@8#Vs;rAESJAEIkeySM$ovTvt
zr>CKRiOZkrZ?8#uXSK5+ms!@*`7W6TJ^^^C@BJO-^!f2feRuCp4lgmT6+YH9%?&|y
zYt6q>J}Kg11L-Vo{^1Ka_(Gkru?r7!(_^Iu%YQ}B$c`8T^c_8V`a*nf&jiAk(o)8b
z;Z#2!jf>&ol#CG_8^_b>^bweBJl3>g)k=B_qy!QF@S51c!fO>-yg(v$!D3@0BNJdT
zrTD$+0#XA{8Mt~pc(~C`-vZ>C%ecH38tm@o&tdboM?is(0p_S<nUpa^uWsQ{nL^pP
zw`&IA);R9<818KWSC6lYmHsFk-_6T?EAAaXQb>>1Ii2c@arBiq^P^CC41G&Ze4I``
z4U<O+CcH{!{wQA&`fq*;vG8MeM5?N`@SBN$>oa(!y9Vmf1^#8iDIWzaL1=#d%!Dp{
x0my_<8Hi6Js*ev&{p6w3Y5FK%;XP28M}D`@B=`US29|h>q0si?{yX3B{coQ|g$Muu

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/example4 b/3rdparty/qpOASES/qpOASES-3.0/bin/example4
deleted file mode 100755
index 564bfb13c1d076357ad3aac2d760341c21d17216..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 34008
zcmeHwd3;;Nwf|hni)<ybZ=A&mSrl++0>oRK;DiJu+j68twiH=T?6Am=CC4JR<yvwU
z9tA@J#gOol0&UucCf)Pe*I)DcSkjgjLx2EnA1N&mN=s8#hc)bZY<}M}cjoF!mMQJ$
z{eFM_ZsI#LXU;iu=FFKhi|(C!waZ&?QWPdH6T6U6>W--n4$|(7ja<%?0#?oPn1h|o
z&SE)$WaH1lb12keGCVC<%g`e5%m}maS8dV&)g}jz$Z)0xW-^o#Qb^?`FNcU~;eQGO
z88T+)`9w#uK0J#j5w`J&3{MC-j*(#evhzEEiEc#DjR-m!7798U%KA~==r1VpgC+-3
zly*?S>i9hw5-wf-7ZYKM`)BYZLI;n?Fsy-@3?<zf(2<;<_bL>K0a4y~dP%mAMj+dv
ztUJ14NmW^Qcwu+6cVO$nt<_5wE~zSw$4V=Cy@@~Ju54`Pgi?>iNRiBw@kgqr{GJQ@
zcf_9j_BPj_Iv?A4Vn=Szmb-3Lkv9o{giAV|TR5pLN6E^nHeJKmPRBL&1+2WdZO|I1
zuq<X-IUKhLc+>DV9e-!wkDAPmzccYyg1@uzw*Y_V;E%3_TEOpRxZi%mv$wtShc|71
z-udip`?l<Pe$&<8xN`QMXK(xYuD5KgEBgmef3aoezps9N_?ORr<x0D2sP*!^+9w~m
zCjW8&tN-W0ZJtect=;@YN3H9L*CtGU?pqJ;`~FbnhG$-{JLk<U&;0KBZ(UdV=9US|
zzGgifesJp1$~U+49y>bq_KsgYxM0>!Y2pTHlF>_`MAAe0^_6kpqyu|8GSZ}jOz=)L
zOeX%fjsyS6IPfLo@DGdwuN_DJspG&8fKMhlyT^e)J`ViOarhq`r(XXu4%`DLbS9g~
z?8Oe0#MlIWXY588^(1CvcL=xuvljs;`~%Y*93=h39vJvsR=^HS*WmL7!7q5Z6WK9U
zgTFu>0oU20+;l!SA%7CHvfYIajwf$#iuyVP9|?a5{0rF(cF^7?K|FjCa;lJLSjaDm
zWseK|y>nCXcLSf~9~AQ21vd)_3(*tzrtvp{&s>x{EZ{8y|0Cd&eujm9LIOS?_|)zP
z(&Yb7;8VGNQ7-uvy6)w2P9IaRLC84?{11tG0gYcz2|g#%^lt;4@Z~rt(AhE}XL(w?
zUnB4x0v-_rQw08@w06%2+@3){A;76#<wBm>f={<-_jEpY2>sgyzVz!Wgr4nl+c*cQ
z=Whe9LjQvTF71%PpMSB>=WKE{krQ83TFyeDjXkm6P(0Dzp9qCmXw~LWAhIzUPel4_
zyW8XONSuW{!G=&c(l5ZlhT85}ZzR~hp*y1GWzJdOc)lpBdX2w72JD`yXm2#(@~;jz
zRff|32crHk3uU0G?usS2bm?^CQdE}5BZ=BrZ#>c89_>w_?C?MbifwEPRh&N-PNlAv
zqAr!9)>NFoF#_6XPkVP$ta~64jrF?55wWhqwb>}Ufyp9>0w%o)Z)9_%+eIRj8`9&H
zy%a_JHe9~8y*rh$uE^R7S2Rk@D%_EHJlftH+}0OyQS-(V;f{09rPf`7w(Z#39_kzD
zkAyZy`x680-6$`(wy~m|a<;Z_h;FW^I3ICp=uiSILebvNn2U^J>()?Tq(2_(ZSRgI
zwuLssQqoB)!6qKfK)pWP?cYqoZ0zU=#YvV57HaO@673DcxZ(pn5z6e4LiUk<Izt^@
zn;4{z4J259doPS3L`cFid(e_B6bZK{+L5sV;&Ph+2$*F9@qU`Bl(i>NlZ6}F;}I>o
ztgI{66Df<vyP_Kh`eS7s?TJm%-q4nKSw~ki9P5vkZ5W7lhs(P9`{LW;WsCa5edq?;
z%7op@a3MMh@sBSni}fYS`eX4jR3*AGM!>&cMwptOP-T@m;sc>hbOlmrlio`zmNsn5
zXq`b%5+|emdVJ7E7!@0cM^ZXb26NYJHAIS(Dol2|j3T64cV?t=ZmGuVWE53vQ&IrA
z0?npm-^(B^OmV%j`s1o-ZBj+!;UqhG#u{{VyQ1(+sa-sSNOhH|eKHe@ri{-PiSt=m
zTN@%TywdBbsSQ<@R+cW-<BLkGSZzy-v&IvuC|#t1TUvl#B}NMhOqgF|FiE?zu$oa)
z{$(3d6gQ^RF|#;s55^E0XYBa%pS{gWE2KRbpXgFh?u{b99O;1>(L9>Pe;xTmlbB9v
zGyA>){{-eJa@_jEPd6baoBfBxU+@(SQ6_eez&|~w4P3M}8HPUnlJf~do1;S^qZsE1
zUTT2L@sHp%_m@|(n3v=N0I~GTZh)hD`ZeDGH_B6PfD6$%O|=1TT(3C{a4K6~ZUa1D
z0ulNRa5R8^wHV;E4w6@!0WRmNl-Ff|Pn5_A`wVcK0lw7$FEGFd4RG0(lzoE%KG%T1
z+W@Dvp1j=KU$eMx%D%L~&fM1xC$dK_K#qI+1C}Ei#^~YzvPS0xaW9zTK#bB|lzH;R
zD6V;{C{0aw@)%FoP@0<T<S<V!qqLdQhj_Y@($rB-9^mP7C{0avayL((Noi`TlY=}x
zgVNMQC;NDM5~XPxb+V18^C=A*XD9tUZK5<a-AM;ee`rIRn(SmbPrprRYLb(7o_>YW
z)D$NRdHOj@+bGR=`cITDp!BIvM@ONnpQ8ZxQ1<-@j_h`>bv8SL%}wrWUZb_Jd)JPV
zW59Lq+I<6b(z)Ax6e-f^u*JRf*ubf`3knz00^7UmdnJXC0_lmAcJI2igc!VCpx#m9
z2dQ`H1ds1VoJWqy2yK06XgpH5;xM#Be200c-oqsT#?F%CD4ItsIx!;Dj_GoFhhB75
zyk7B)^Vb_Yag`R#*}(-Lc?Ok!yWpH*_x1;?-!8Z;yF)9)_3+4jB4_Bh^Mf~rj&DDr
z+;@5o<d`=fDEED6T5!K}SKSnobNhq2?!81xa7g1iw*8|@gx`CB6fkhRdskhF-@VIU
zok=eDu6-qKqMoN<6{IgMRIeneTS!0dq2u7^Eotf8jr@fNfkz4ltm`ys{T27FdrD5h
z27e6+*w4wheo={=`)FNB`6<lg5mr+c!sV3V9SWB?Mvi@g_7;>C?~f=vnJ6iju#x?M
zcz1P|%y;8f;@%Z3@uU1H^Qrt@cL+n!O7;$QmlV5kE2Kind?EGB5?93|u8Lvr(BB|y
zuw*{6O2Wt*Sq#o#HESYgmEe3AA4e~dWg~ONFw(@=;vPDWIC+MSS3FYjI9Odm1ZtwB
zMKVO{%98mb|MfA|=#G-zRJ^F!$k#bg6LaJ|BsvS`AOH2kSXiNuTT2cA_g*NOQaelb
zB665|FH$==pOFP1K`J{6aejRoWE}gjM$%e`Vsk|aWavrl^Trt2WXOY(_kF~fP0ZqT
z6$%|ifl{F;Y1UDmzNdtDBwex4K$>oaV!b7PgC2q<EnGwMM~-P_Ek4NEX-ZxZH}FPT
z6E8+OvywRh)M`|+7qyLotx?G&<wz9=Y9fyu{qP@F@&h0tC1*jL%u4<aFsMy=ad(OY
z8U4sXN?h{*xtEIJi!XIv;=I&(*%9~95#0o?Cyb%Lj@S%RB7dY8@K>L%c-=Gf`^(%z
z&$+k1aLOO_937@rz5D2WwD@)(JxY5fBa49gZs+d9Kee~9X7|E_2;AFG+uTDhZZTbY
z<S;CT4>0dudM{;-o+y|zNU+0{lKA(XHUr;1biez)S1xiZ$K1a^Jpkfg5Tg6K#}n^f
z>K=MXqkm4&53Z=lx5JC5&7d5)3Lg8?hq7k_q`Y&4`;+Y}x^jS!n5~O2x%u7`*i{Dp
z=&?&r{tHM7=CG4-Ep#8vJp_jU+8tCXs+XD`I^{e}9ugXHelRk03NEkUr&}!C-MMdC
zS?KM!|Lxj>LhpTrFtR*%$CKM1QrsQyqv^7az3nV$Ro*UWxer+g3n!ea7$%qpo~q?x
z!asuXf|qylfzu?GLzaK?4v}`Qa}JF<*LsHDcQ$*5o^S@eyXN&}+o8f;3%4R{@-BF>
z;u-gSpO|T6ADIO*_jRu)X3P3`hhFgxy<Z0&qm%#ahR=2{eHH!@1HLp2XXuE*|G#qt
zI-og%m=e%@ldk-&zP^Ngh5cgtyvTXt&t6(uYQOXXyFJDi%*dS=k#ne8f3rOi?TIY2
z+vk-o>a_PnJFs|*bi{hY@qCE{zQ<q=bjkgg<jg(OA_jPKHGF+P42}0$SCU`m#fhYq
z`#W;q&kj?kD}7&&12f$GLJqlqB=@i6e%r3s?>z9qLx(QZNKaj)g(pPF9E=UaAphzV
z`S*%^5wc-Q5AG;+DrcB42qR7toVNEzM-L(F`(SjGrVxV&X%-TOiyZ{qk3ofI1Z@aU
z5gtOC7|=LpM|c8ZHNsN}k0Wfu0B_ftoBw_Q5~f@cV9M4)<%|irmfcEjA^omE8|r_C
z=QxKD#6@trQt`Fl1qlD(d!wU;D1+Gw>utrW3anc!gY2Ri7o4-G<V>O?`a1meL3Tb`
z5)c=`FTr0|26zJSHo!v&<wf{A@plk#%r^9k;NQdFfei3_0566vY0@Vzl9?{$3cr_Y
zAlFP=q-ISI7$T5Yg1=L!7wsKq?VRagHMYW=O*OXSUFI5_eP@=_Hvjr;r>%T@&Pv;%
zQuBeVd|P>~ZNAfH2M%y+Y!)ZABVAPYI_Rem@<j-M{C_pMY{fU5T{io!ESGKm&TO}>
z{Q4ZXt$KTIgU!)xs|IhBfwFXdTwgoEry6`jQxZ3RjgBg3J!8f|`-=G)(=YPaV>$Pl
z9?xT6&C5pOyLrg?9o_!_l|ci2Vt;v9?34RNTB4F^)0G~c5C93MwY?FK*79^o+j~l+
zrR~b!8|2zi{_Y{yp7QtAGc-2(I)8>pAIHc?m$YZux8D17G)C~fCI<!B5zsD+#la&q
zXQt~0vxA4yWj`zsrQH_uT*jt|PzGih5&m7;;-F9j^+&pB7ETxau1Hs5frEz<e#oY!
zrQB^im*Mwu?ULogb@BR?3w$_Jo|bZ)66s?T9fI=zO(1{APA}~t@jKF!B78}N?~5=;
z*x6JOo-M*9BCHqTS`l`NaI*-n7vWb$_yZB%CBk%hupPF{KEHXxKyPBeUR7FMTD7op
zfTt?1s;DY0uPR-%K!bDlsx_z9D00Y@O(cgvnU+JKbklNZl_oBSN|}~jtU$<^{u5og
z=y|Yhcwk%>MS&e=U4+i@4wAzJGh8NG2Ic&*3U{+5_pe0=6w8-j>1I>zv*<wPtlSqk
zFz0Fx%=W>$va+(VM-GDQHP|FDRl#s`w@1Lj^xjlNcTg1EzJ?&%l4IFJNhRlcVqnSk
zf+Y7cfO0H9!abMgT5|t{QgSRb>{v_}!$K|P4<nve0ixr1@42De{O=Y5@IH~Ud>f2C
z@1vUtJ--hfJMUwP7v_hdvAj<xUY!4R5aoSJaeICZ6p}Ye@%h$2LV`S|P@>$r5h~47
zc%s_ci7uCC;t7W}0V(pbc*1R6i;CrC^Mv0zix}tdM2ocuZXnO13<B0>rKL$;zH&Vh
zUDi)Yt_cbW+-F@$f)(;a!umX1SKcJ08#!C8XAsR)r3Q&X>qN9u-ZaIJ#I@E=DzsSH
z32xi1W)f$nvJZ(HtS=LTdCC@`?Y1r^y_70LNbI#Pr$iY~9I)0>qMRoVT31n`f+r4H
z*HEHTSpb^D*5j0@;<61}e@TKaQNDw69$*v7V6l1M$Qyx-#d!y)lKF3vRNtm3%Womd
z?@)Cu`5|hOA5d)z(ULyEaxBz_6RCwsY}>Vn5R_VzLw_crMXX=~+*|(jXHEjS0wrYS
zf8|N!sw?S<3Uw)3NnwS1QCwF3H*=v1btd$FmAVf)zFOT%wyrLKA68T!;3oA&@G-MO
zdSoFh|K1CUU>!hARUoCJu0TVZR3EBpR$qjPWvM2_v(;XhMh+{?L3UP_!vc}i-@)Lo
zR$ZLfigFaShnJ8Kk<98el1Obw$=T{u+;h}Fqr2s*TS013?*Ubw+6RT@tB*obtGWYy
zYJz$LtZ|}x40oHl9KEMNJp$T7^&aq7)nSM@N$o+!i`28w(390mfH_6I9VJdx4?-)`
z)VI)Giq&s|YPvcZWzA5}fmCOx%g|mk)h_~nmU;zbo2{M${5fhjDrr{_TJcM-Y6qV)
z)oVd}mf8vY67?adX`cEJ<UCuw9Gt(Pwn1a_)!zbhf%-I5bB;O)+0Iof5kF5|4^1vq
zPoSAfRR!gisilCFt8W8Rp}vK(D%Gz;m5bE-QQs;x8$1`QO^|bmdKc<@zB&>2YIPrK
zx>S7>+Fqu1U`zG_^$lomxjKwmuTXyu$c1VhxLu?^i@FRmH4jR>kEw@IkNcV0il#Zj
z)Q>>_08^g?`h)nP8P`KhU5?-=Q=fs<4>R>A5b_bGmO+k3nfe`6;x|nFPo#d!)axMV
z@0dCRlKvM{AB935V`?c{?if=qhk(Cl>SZYXai&fI<{y~)9EA8IQ{MsWCz$#%@}6XB
z6$*ZeslNr?aj=GRpJwW6kpGFPFF?J|F!e2z@MosBK={AF9Kq#TrcMLhUzz$F)ba#V
ze~gO%jj4XL{c}tm0NwN80+bh+dJ|AyWa>*0@Fk{Rga&$<sTTregsImeb&{#&DE1Ym
zz6RyK%G4`C_ZoDDyw{ofDpGGSwE>lWlc{&0Elx2t8wI_^)SsfA-)8DT2>%X*hxNS6
z)H6Z%9#g*yso!U+8%q9wsXHOyY19Z6`){Ux6<j`KY8y~KV(M0qe~c7@Pnb$i5q-+k
z*O3}URnSy}idqK&uTj)5q3+izYAaf0yP`gVf_5nCizwkbMO^|VU$3Z*sOFaym7Yzw
zK~WDOHKeG^QP7QwIsyT2QdD{lVW*<*N3cs#*P_xlD=I9I{i~vGLJHfWlU{^P&$pic
zB;+j`McjJF5(vahMa$6&);lR-F%`WAko9ivx=clnqd~2QDKXhp^jnB%{W&FyO+`<D
zsr46>m}x3{foOh73A<@B6=PkgTn&9(hrcPysSpjiO1TY?0}?uwC`4AHatPpG>RC0M
z>ILEzP<{jOlQL@xr<%45CSYxT2^Dq&Re1y1|1D9ba_VW%L4DRs{s<^JJw=7$X5x=a
zq?J7_icD+A)qrx8Dnu?Klt%EJ8YCH}T}Rc9UIiU-ly%77kcOh7Ev$VigeBROE0BGi
zj-rWcC0REA5RJ`IZb$aPG?e?%->p}czzjLcFtQ(#C~Vq9G_-Z_M`#e9`z&%^NaiM}
z3BUXi?2HTfF>-Rz&c>>~59epyxeP|jQRX1~oHUd#le+gf(E~V24YC6%C_-zyiPhIC
zpu1lorjTbCAUv)$t-cSD*1r8sbTndR+J)>}BrEuA*uM4q@4@%*+#e(7E}5$}UU3fT
z^xyvsL}F-ql(LijSuPDorX%Pl`Mb){PPxwjVfuwAwFQ4OZiAjCoK^;5rMxN=0W8Mf
zlul|`8co@ZGoaB4cPqaKnB!C+vYc=}K+#jzQ{$h}2)$1DiDE^93Q*R_d=wT&OBEo0
z#zHWf_@QzD1f0<oKq9M|(oKbZpBQmloOtu6fO3@E5jm!#keitL**9{SLZ;_Ysfk`?
z7TgHu_2S>>lcZ)dAB5J6O=Xi2(BCY&F=ck|T4w4ZRa;E6duy2~O7RJ%*-mD<oODrW
znoN|YMM^8`9Kzp}eyW)UU82MQ*&(6aPP7}>qlV;EZUw@A9f_PIoAUs1U8RsFl{;{|
zSLPc!x~buAIJ2vOuRMk9=OoHh&cc2-dX8!RBp3q;t(Xc;oZF{_wgW_(BwWE<95!<e
zEWn~HrMf?XzqzZCzKz6DCR3KOLb(WF7yhR3_S2var5TVm37z_5UV&D4BBf3tUdjM&
zgEHSx0cB+b9q&J=0(+7DU5TQx;1y5;33NPJ0nY7SLc2Rt1#+o^xrB7)f1vnc+Z5%m
zNIZ!`&Z1k+q#XCmoHG?=?vIhJ)vSatm1)R54}VjB1-@LuDMzS;M{r{r3i*gtV^P?=
zsQ}u}P;P;nAZjR@QK`x%z`sZYj}eo3YjCsKm437bH&9a{7f$&KQ0|gwQ}xcQSw2;q
z8@T3U&VH1dfpdS5NK&PRBYzLjLfe&8*?Zw-=DvxDR?jccLx{E<g?y96R6>Z#M7ZB#
z{7rd+>iP_cIhitSgUWn>OJx?Da{z{B+pZ8_r54~unWeSn{HxIsZP(v|K1X)O(Jlu{
zpG0E|D$z8y8}9&s3ImpzW9<gutAzD8Dt*Ck;52ME&&Pnk9gC?ji(}mdjQb>(riulv
zROGF3REN7;Q(-no{Tt9;mZ)sOC&cjU)o2^RFo$F1!l)+Uj~6Q#K0^xH|06gMftt%v
zD}h#%f-3MALczB0p8*Fb+S9`EHvsp_RD9m3#8mTD3*zv@wx1|4JVBYqDffZuuFt0Y
z5r#0^ov#5&Q08;WCjp?H*kpC4^5S^s<~F34E`;BLA;dP^4?7i%tenvtNan`h=V!zv
z`b{r<jbJo^GwK4913DwFfTU@ErfgHGIoSd(;L~r(X^N?EBIi2{p6~oqeA$Bgpm5vE
zw`0O6M7432MQF#__!GSV%>+~!jP2CF!WPIi!deSB>V-h7BUH^=Z$s4f-t%(EF%=ea
ztT4bai8b|m;K!!@4Bg)L@wYG);nw<PWbcwF?3`K{ylr$QI+^lqKz~R%)J@pAFOgB2
zlnLZe4(ni+OFn?x=XoxF6Dz2E31VAj28Q-#D~FaZyco0vOF8W6Fa7DgV{iX@*+Las
zD_Fr{j-7XIo%Pcxb)}b}*$QfIM^8FXW(G~xVp?A3zDhdCvHS{mb5_<O7?#<R^%Hce
z919s!j)kTf<rbPVm-BcIkLOruwwz<3=FPECNjVl`m1CjVM~;QObGd~^mU14?;qeM$
zNj|C)DBnc5$kKsI-^f{!FE6+JiQg%n!{a#?s(y~;E>M<R$kFA9JL$jN@&Jd>y^4I8
z`91Km&<gW*knzaB!7c0uHOnP6FS(1H-W0_=8%WC}B9BPK#aDpQMaW{g<aBdsbf}^+
z&l~_SB58O;(y&|_C97y?H*W>TO%jVol2|ljs462~-vY)#iAB8F`9ruXkKk7QF@!Y#
zkhE3(II-kWRz;Hz^HYR{84#}_v0oaZsml_=Mieub^R*}@F4cUrU*<-eWOowOJR6kN
z^aDtCCs}JHyPys%$cv_LQ!4wM!1ds7In`TfW*5+Qz+WIk`VOYDuN!E$5Gqw(X<k80
z)3HpeBqMeqO<>aZJ(Ybw1BVascM&xnMU-Yo4KPjXNp`xNTbb$NQRFN|qQ;M=HC>Vg
z%T@xllwB8Prgp-OQ3|_{)PCd{b4d}rs>|UM(sx*uedmEkAO2ia)n9_No@yZR&;|IV
z)U7w-u$AW(J9x(3=vHo}+58T)q^W5W;OQH;%D!7bewtX2n%)IUJ*iaU$zn{ao-^Wb
zWsb$+q7o0h#I!t<9G>-P)%49`W#3ZxyA@=4=~?C<uKE?gqob>Th5#nwjZ%+VNcv3Q
zV^;RfM3o!Lk(G-`qxpxleFycMc^_X0>blECVj6OMZf33`67!w|O+F_xFFubaPDFhh
znR&?yEhpgX-hjOG8+h{2TK^)*YVILtV%{?t@Sh96X8t0181tUtR(~r7HuF|;LFPTD
z*Z8j>s%_lL_q@Bt|6R1W`Ff4>iGaTf?QXtVqdnGMPu*-+kpuE$MBsO1_cX$FyWU3@
zxuw;wB2u~f1n#*m>UBQ_kkwP)J2=_FY04z!4Yv@bimXq8wB{+1|0qO)T__c9C75TX
zM~Jj5KurEBklFw<5B*UxsK;_~&3|WPKo1jX7nfq<R4u7X@!jbdpSTn^6O-j4|1+hi
z6G*$k*!;cqlEE*C!DSg4B&BG&bCM>(i>PPQV}?ZVkkIYd!H{|e+6y{R&UY(oOqNQw
z5>S?~=6eOwyJ*6@QxVHq^Y2A&JKFoHlw2|f)_kte5v?>AU_4D0Lq>*z5Cl+?=}Kg^
zDCKUY6*tosW{NWe7M>_HZKg!A=}U|?Un@A$!n8Y`BUr5=CIPy&5D%RnXJO$iz9v}E
zT6<?2ixUv~dm^9K+22p*v*45|nkpZaz+>%W6;>jUNMR$Xh)~#WkxL3$HbW?cv-xKg
z^0MGa3ZZ>Eu42jY^C{$G!GaWWdm0N;NS?4UQpjD&d=}g!GJazOZWX{25;)w-g5MCp
z_YA<bKNCP9GOYPK0`iI0Dba%1Mxd<|az*_2h;l7WK4aar$h}n20OXH2LXMWcRHiFf
zOBhXIIY_^Hw0ua>f1_yYBPybDj@J4YO3B$->syqP^RiqI%2}TcDGpq$;+|*TJ+}=Y
zJk{}A3<7>siMsF9Ap%|c1vs(nb>!lx$MN|+pLABT%i1tyDBR3{w<#A>x0&D<+5w>s
z!c;U4*aTGO*@~=Y+ZndzX{u5<4I}7OX0|P}USRdW$C*o*)p{ZFF+&tltV6O30L9dc
z0>a8Yn^9mn8<OM|DQ271N(>ah@+a!tszG8E$!t#%B`3(Tpn65|6cL4OE|`@evtTJW
zNj`;HDQwmGU^^3aoMcLY7a5DAh3Di_<)Y~tn@a2Q>0w^Plx!4HMOvT;=IBYJUYn;V
ziCCN|16y5G=<?|#?F=0wJC*c|Oc}I0tjjx#%Q9;wM5wlstg|7!Mbo?nn4|Nsdabhz
za@ms^RI1n7P%N{t^d}t<&#LLf^-R6QY^}tzc!|YcUS^5LiBP3>=IP9Nm5j`(#%F8U
zB#@-SuR_LlrBdhXOsu`ObQTMYnW^Ga3FjEIlX9J_gCUm&MrW<kb+&vu{qDCA0Aaow
z0B@lf01&Or2*7l!P*`!r6k&mt7+o0FK33R0nMYNrva-l-)of>RHrFm$&k};8y|QKA
z`K4&ws_Axa`PGD|wI=4LNQkC|GuA7D-eX5IQKc^^RVs>lZK^UU*HIiU`dST#F3)Ba
zMJC9zVivL~v&BmLjTchUX}A|j$q3((EZu}o?M#+W$r?j0kZY|<GoW=zT@sRumyJlh
zv22oc<<PYXJjZP{(<_j2-*_z^do12l15O@I1Da%_4y%aIOia#J^9ZE(8WoU&+dz^~
z5sk|j>8==2pFu1X;ZHUnRdkIe3keb+IRjuoIwB@b$ivUNlo!;{2-++uD6v*cz{7L5
z11VdQ{4bim9L?NX#a0%PX|3lG$bT_KsPId)iu9t<HFt2Sv1S_RvQj2I5S$+I3mJ)E
zX^&xUWnx7bCluu)+eF+I%$I4#t^6;(6&UU9=pG11<eP%*f01=$YkN;$cO<U8-Y5&g
z5e%UvRpLB`jlBb98=?t;f;SAy@Wy4?7!*87gBKRLpef8?qUmqRmUi{^k&Z;+Rki-o
z4nUwq36bdrym;EaG2+Innm8O{qa>sk5o495U3eCN-sTKPIuXZ!N^h6Ob#OF>D69e^
z%P76U3IwfqPj5J~wHt3COMw1bZaP4w8`=~7(T*esUbFVzgw7Jepfs&mjHf2K5;q@#
zX_zdyWtjk$>uK{NOu^uIg&|8WrOi)cfBQ>L(^*q2FQ5Cg39rv7Ae75kjo$>xPcA8S
zep#j~OnCcF$ufgTt`5yV03geJ^VI;Gx4q3wStfIl<pm0VjGJ;X-QK0!DZ1TCw`#hX
zN{T4Bj&4`eZBNS0WS;mi0Hz=3wB!t(GMzO!=Ctg4(0LQV%!L~NN4H<|4yB2QX|xR)
z9!uw*?b{(qRZHtjmb0JRVJa`&xc!nZ2d^L8VOg*-dQI0QmepC=<~GYDAk8(EDM*B_
zrT7aJ>qPAA7Rygw<`ky+6EDBItLtI|`Nf>v#mSdz<aI>8oXG1ma&yPS<|PlCS6Nn>
zXIWO6CP8HZS1?rDTI=;Rt_(H$f}wz`$>&|`s$-PV<Z1AGU7@BRy%-g0sN}#k{*VDh
z(&}^@qA=Y^d%whOZmjV&H`alp;Z5|6Z;RH^E1vP_6_HSa2V~wxBK=M6fAnSazOHTY
zXh(Z@ccgb?qU(RrpYS@?d77LxURSNJu_+jEdK!aGp?assD^%FrNSUFaFSIV;3A#e{
z$l+veS5uS6*%<ObH}#%I<l$B3&YlD_ynRl!*I%Pw!n@<viY2{~NH`d)?~g<pB0aJG
zZO|Gu7V!`zBVO-vHhF41UQe)<=i)W_=77uVkr=@>M%6CWox{@haJX@xXG5glg?Fjx
zE$H^m5&lLvmC@+aidp9gy2&gW>zsi)%^U(Ow7D<QUpmJ9p<0_08@fZt<nP00s??Hb
zye`_l5jBa%Q7M$p%ihu-O+-TU?m4eRlPlQl4{>vnD(=*^z^-cA*3%OqkEJP!;f4P|
zY)fOTzlVimjj=>hW2_&F+Z@sA$wJ++j!nFP<|gU}0oTfAuQPyV4SIZyJfqg_^SYW=
zw}yQ6q4lnSFC<!zXF@3}y}lZ!H`IVOb*^-Ue9b|Bb5L(;-ZD+iwYA8uZ}ukDs@KfL
ztz;Y%O6`UIa!nU0UDIEo$*&t*z~!t9)wmjK-3`vb>dZ~u-sB3bZYswC6&+o46aY1F
zoo8*Rrj^$b;DP>72hM5e!7%|L!-)c7*fMe*?~QHZsf!UZ=)muVF*RykL2FlMa|_JX
z8EA!r5go|jF@*J#c?M(LR>E9h>OL)APZQaH&=qKM)yhtwsVRt`9z5&Ow_wUW?fsiz
zStL_Z)gh;sl+_w?)&@OmU92P4w@q-a_l3xFp?9J)Ltw9O9nX;zTB~WU(udcn-~mF;
zx>e0hLEv}v;d~36%0Q*9a|Rm8vXb0zj7D!fi2S%HE7^CYt?`To-&!I}v3cT%j;woJ
zN+;FetgADqPxl_l9$rz7a}vUy-gu-xL4ACzi&=u6Qyb&&?xRLYN*8~VAByCb9m81`
z?TJ|b3QbUFOQ>;OQvhN)Yu%{-Rbw3i2@0uk7wtRH8;@@6jfCwmK&@RGoWX#n1^Kl>
zpX`AWTk|jJy<9g4E{369X*3gU>V|nVxf(oZOI_U_C#;6|aB^e;YBF&|g;DVaeo33m
zpww#Vx1dqp9<aq!=Fq9;Z=C)8?V5$*m1B>W4-ed*VhnC>3Hchmn#XQ#a;3SVP`xh@
z^7?rJa`=$8s5>px!_aM#ji;Rxld-)k%9G}Z6Jb)@ylGJ=8pa_GxP`S5f!rmAXx^GW
zOby6D4I`d5KWL`zplen$(WH&LeL=4D6zfaMT2Ym%EtlUPWz?Ir-U5eP=W(v|HNxfU
zI@IKGp~D!4VxwIC06YZF-H{C=L?+vSVKfG5q$DrVA88NMV3i`9a6i;DFs9I{7O7gI
zo<WsN8)jLkFBYdcyKt`oE?=lV;A;@JiQWL`N=K$7=&%v(K$m{)Ko?>NUxF!E<4TOT
zbuP%>;AzzC%Aj%H`E@^rA*#;R<O#SiF!PBU7pxw=ihB{vPBbfmRCF#36@n(C-Xf-T
z!cWG@<<+@rnpbkAal|IKr{37!`VxX^!vm2txavI34IG_21nTdY{zzx935yYu#!^ip
z8mT=^vHpbCH4Nq})LuoyyqM;YLdc`j@ZtA0c{HcpO*2j$R71y~B&SS<LxVp5$wPnd
zK&9|NUpHoxi3p35%os4}R3tuuqKar|4Dsrx8H1c>GkuDcDy%*St5}qx5N`cxJ&5y9
z&MljqYh7r?I%m+S4b?b@C$Wv#>j%MPdP|a@m%=*$4Rc0xIoVvCHggh<fF9^-fi<Fs
z(`o^8zs~OVjqyrG%+M;}7HVwvdYk;tT5as*y^7Cwj144<F*+cFw@GwGwH}W_ErTZ>
z=%vGXD%n*T4*Ai{l}{Mg`2wrqqYUGYK`%+CQ$-`cm=Wo1jM0X3(mJkYyUBq-DmTfb
zqS83E#9*KuyJTpGv2VZX^Dyo}0s(YSPorUm;t%-hnrnkx4s9Y}7}|(NdQ<dwpWlyu
zudlfpG2>*RSa&!S>+Hm_Sz?YZCdeUvU{DmNQDM#1N5(5=#KbhCt1|jin$=<uCFda=
z2aYh?Xy-S2u|`gcL-BzPArG7id?1-ebK~kp-?~P=EJ%cKIFzPgH1U{Oj$SdLayTr0
zK$@Y83kzDRw-_vxytuY>pp&e`p-9P|Ft!DFKhS#ET1*S-Le9X-<_3%jO<d{5l@-=a
zo!zna1gn8p3pB&HlWl7-gNpc;_C8IUaU3hgMcOdXsjX#<Js`uxIeBUliG;Rx(+CR}
zTIb{9)H)l<0Fonq2;B@bU!Li!31SUO1R2d<pSlRWE#{-D%h2brbd*a*Zi>k{tC;rJ
z`pLz#hjIQG1(jN`NDHd8U@;5DQC%((O-LcUHWj1c4t9pwCC`KH$O()vCr=Y5FewMT
z3E%QrzS0ZFuPGa?-x=+vVW1}#)+XOc$I+yXa(-8>r{05NNWD(4*U;54f7ez4GD{4Z
z0t(47`g1x=)?Chjw-qC#-v#g4sP#`Vf92z(v9q{+4L)?vRz98^GvG<(RIW)Ej1Q)n
zEo*TH4Ryi|8=AP8oIkb~k~<G_XNIN?wKTn@I$`V#HDG~)MH%^NPY^2}Oa$tqbnc*`
zTO~)%lymclh2|Ji`f74rhumOuppkcUS}5~EljD>Rx=L<=Xfkt^VQDO64>_CC<|A}K
z0p|l<!RToMz?T|=H}@tXOjCGTZx`~_vSlzUaeG#}(}dw`WPOpI=4%XM7%h_&02^-h
zH#OI+f?9*VV5>hx?Lxw_Y9>;nC(-*apU<J+;Pkyjv?J-0k-{mG*bBx%k(goNyhTcD
z%l}LRBDfoOeUjc-UuKb8)@xued!yNqdh0NIL_fd;KI5D-M5BpV0q`M_ufH*+^VMtp
zMJps_hoV8-p-A1!Xn@x+xKDjHS@HLdc;uC!xoBnqX(C8d1FUl+TbVrUl1nAsjo7uT
zqjQw)wdbCzy#i}r*pnFOjjY%h>5X6`3CZ^Uj;<Bj8O`>E8|@2yi||)zUx+m{?Q<^d
z9O&ik5(fwk*s*D0Uw<qS>A=F6HDFDQJwbBD+7v#ei=}$t^aVxAvI}dCii(<83|(KG
zN+}oL^B0gi^=ccN5@-)cx+9pMl9pWSJcextg6M`WXA7xjli~&RDQPuI;R#G>dbZIJ
zU~m?qCN#yS-8R}(zzDnn${6SqBVN1<Q)M|K@RgX?wbljK>vlG-B#)mQE>l=nEKXbR
zS5)dFJ#xt14|Ho|iLq~`PJph`tHDCOk*ydU$bgqr!GwGHRwwoY69fIdI2&{z)eUCc
zr%sN~$=NQ~wQv-2ou#e5LXE8+efX#Oh*5K6s#h(y&&RC8=^#a%=Ne_{+rJRLy<7A~
z+J$PsoD35(Z2?|Sd+%Jh<Sv}E#>clicqdBf?b_&pM$vXVNmMz-)i>Wi`|McW&UW3;
zWa5q)6qQVlm)0xrS~Nmq2am>3qjH2<RZ$DmtZuBuRg`NZvSxKrCf3K;siO@+m)Ave
zxJ-nL`3MUgcQ8Y@!t6LfC-CZ%<z)4REU>=RjracxzfG2<{3>{roU5mv^=fFDj5eCG
zrob#q_Hps!LvlpsQ+Euw!hp5uZ`$^o*SRv2605?rWgo@nx?yKMq#aaCEv!uuq{9_c
zD1C>8x&mP(MR8(3rxlxCYp~$KZzc`2nGkf=&|Z8>@yTg6%}vC-Puq5*d2uV4@}Y+s
zU(WK<=Ad%G!;!K0Q7DEvqudcCcgo!b7cOm4qN_3ayNbRMMMWED3feCkS{3=+ENz~{
zda+x|Xk$aDUhZSd=~=+#3)Hy+TJJSZVx0ln{J=b<)+v8w(tWz<!I*E8EKKW;W5)mV
zjfqAdAIAAmqEDqM<xe@QndaqVcXgAiE!s5Gc0)8z${ox)r$1%cOu#jMAD=yG)u8EA
znhsnowJuj3=1>?x<<N<V5q_xX-^fwxUF%%5*UV>wnAejo$sE)8)cOK}X1}xoz1npF
zzD}kZ@}GxDrm?H4<gTE>^Q0P|Ht@nZVKrFq^oli@oY3<|^4Bprui?VMq*C-De*u4L
zngfqg(8Cl#8J$zCfwI^;N|3(^z%j&468@J?xJE`Lp~6~?6cm3$VCM<mCRV;*#%JrW
ziv2P!S(#Yn7zyOh^XCdW1&_9hsC>ShL~kBLD1X4eAW0~r@`3Qogz{<bH&h3&7)j5>
z)4K~qNRrC<VjZ?<zl>j~!>abn_(~nNc)yJMb=Z>qGQLBHoxlH(i2p@|3Tu+l6FSH*
zquJt_U^9zfy<a9~3Lr>%bqqJj{ZYNd^8GR{$xW<6XC;r@&(uzvMKRZj=SUTHiHzPT
zKv`_hiRgZrlFWc;5Q#@6uLlHA5Do_R%hWNwLO~goyfnmgcJg_2S#mzpA2#1FAA6M?
zjCtvN<a6r=KJsbrOnmgbbUyOQbORsx@cS4mUM_TJz#W&b>~=O`@r5Fk-7H;>Q^LUI
zW5c^dSyC72`0|nbH^fug?}|`9j-QS%AD(X#d|O2*ADmCemrwQ_6z)Sl(sNj(r8AQ5
zLb?X&2V{?z-CI7S^O&fo?AYVgkCn!^^<Z*_Fww7tT{4E*W~9H5mG)xYRf?m%7bY+c
zXem6*RZ73DgcnA`tdvq+?eQ*F8s63mep-k@i|1^{(fn9orzAqi>yLD|6M=~JbthOU
z-%~72&|0FDpE_TP4<gVx`lXSsP-lO8PbAb82CYniX-7vWvb7`9mk9AWrp6=G&VLN#
zJY@Xx4iKWecATsTx)_SUt(2Fmm4T!BS!qWMPdi{eUmA{V7}yBn_TG*7eM}_jn*%c5
zzF|XuWV1}ss#V6JK^eu5dNL|FrJKM%na1PZ^za)!?#8cblS70&(9bW^QW~y|Q+O8s
z==~x|A2d0T!ef#GVCSji4bR1;Top(e=@TXg&rb;Y5}r!l(&_7vg(C!Gvi!X!2S?s3
z0I%^Bo-`LB9y(9C8j-?-n7aJ(+_f9<FdtomqCmX&$@L`Z@ysT_*5Xd6vV3`tn?)eX
zbK5lRvHGKD?&-`lNiWYyJBCNn>1yNmr2G<3hMSO2XRyh%Jcq3u58Bf;$nSW>9wEso
z<riRf1tL^MNiWZRJ0$4a1Y)}WWc_!f(aUq>3h~A#U4vv?xQHGv-|6K``kMhG6q1iF
zdG6lsLM=T*3-r5a4n0lpFBs_$6*_p{Awkd2zSObtDuam2@K&Ua^wRSUs~Y0lQm`~S
zk!0UUqnGCd-XSW8w<J=obomb;m&%v=9~L9eu%O5JGAWlmC52ZokT%lG@2UBP;=*b8
z>E+**Mt^vQgHs$9^nXk%Kiz(Y)9C3qPP*jRzrMgz$y*vboOpy6HSj08o8kL)8jWlx
z$sxV`XOTgsCh6t5i^p+>2wmy;>FHO1Lo!KvI<c6p6Q7g*P2kGDMRfAqLHS)h`Taa;
zMzVZauhXD0mM^~pae5AM8()6O=M&%<>6yKa$QZNpJKL)T`d!k=FbBj&`n`gFub`K5
zN(PcnrYC@yU{Zg>g8rbOKQ0kPD8M>Y73G)f5G^9<&rO4qtaQn6hM?D=$@?GdT6}jB
z#AA|9f)|WK-!WIipwoejS33RTap?P2XjD6-;}lHN>FIlM=`cclw?-rQgH-G|^gcnK
zuD_%88o@Qfjs>}1mMnAqIOV&A<Dhe(jF+U7u*f*{c<P*Ajx>U_BtJ?mtq4wsPX+NS
zo<@+C<n@uhQ=`!CT?s9ubMEMxhrgxxllEIoKwOlasr>m@@oaWTc3dr~>rW&<GcW_Y
z+T<96V}`ev^CV-Y8Is8PPbxf1KQA^Fo~@q~n+nfiavqWj&t-CckqWmkId4dX=P@}S
zNQLL?=f$SN$9|`e^353e<+z-RkMH7%6-Fw2>~{t=KFRO*rQ#Rr-xE!Rt4xk(sqje{
zpBzRIkDFPMKK>Xp%{bjzjw`9~DHtc(j36F2v#I(xV9Ye*v|Z`nQ{k{idz%r&<K_(C
z(W4AAOMYk2NMvR-n;(0b$IRk295ertXeRvGV}XE}Q@%iyL@_gaGGU0B@x{MPmzkY8
z22A}ri@i0Po<Va16TVj{?L<r~IR^!LkZ-lheqB8dpIX2bHnzR|g8zxB4#bjI1W_ut
zP^_S3&Q<~Ur@^&(><o6p+%}FX{nYJ9DH-hN0l}v)jn6TTKh_S%s~62b@M;>nA<dq(
zdBNEF0zIRi^B}Oo#@gW=4j=p7$#Vt#K$_jsx%M~)aZG>l2)H~qo3QAj6C9Nc{e{LA
zdj|XO101g(kGX!p;bXspd6$5v?_B%}@J!$L{M$JE@un2Bk0}*39S8mez%$8t!8q_{
z4j=3HE*=N}3cxeT^Hsp9Uh=yNg+k`TfM@#t=~2KleZTdUaromMF07%(e*aVjocPF{
zC)ppD0G{c)sPv^O!k0Twv^Jz`7vP!n_5;pm?Ds<-=J*qtL#&&q-_mt_96s4t01<yj
z+PG5!IO*-cbO({M`TUMoRt3ID>H(bwO!)Hq6X-eo+6Q>1@6Xb=Z%7aJxosS;MMGzQ
z8HYa|<d{hh&43gC^zYTK2b|<NktY9DfRmo(4q!->`)>j+cMfHH{TA>{<$f>@JP-3D
zvIqLSjPWW2ocJ6_(=(m5P4d`9d&zbW3Vb6Eiqo0GnZD~f$nnR1NBBApA3G1BgKaa}
zXX;CynI4KX{FduIUp$P3_fY!riPiypz@-C^E3B%;ga7oKc8DcE8XHRV;6tJKY-}8l
z0BE0=HGo2)a4fX3JGKE22ZZr7N<7p)uoXYg(fhB7NVs&#((~z=y$neFJDS>VBwqYV
z^l!tjl(eH69_Z=8GsR+mlc)H@N-3xzJlyVVaN$AtI{Nf2mt4ffOH#i2dj3c_fAIkr
z`1Y-i9gfFBUG(-V9>YlGjprte0P&^Ly4FT#gQu2d{`6+@A-2E!K#}+`YVs?hqJYo!
zG0{v9oTofC_jx`D%zrm`IQUm9#V09^FI|Zoe9w|TRw+Jpn*KG{Bock}@-x3Nobn0K
zu}{R}xmG;F!(UwZ{9iz&ZyHNWO#PzqKk;qU&+$!Nhr{qKPeU)t_(TMMyVQWBKhPq+
z#rltbW;Ii@kN1V+F<%W%lO45{PugU7A{ecad=y0&{Ij3L`CK1)HGcK?bAF0AO>kXo
zU97+N{zRs#{G)I5@)uJ1$CGufWqQAX>rH>;Iip!5ALcU1m7xjvBjN@Wy}ILkHkzLM
vPwS5IOUq-w13jKjGs(ffgPZa&5Q-6>ywzUmNiA0U%JZ0q;4`U<*YSS<@Uk@{

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/example5 b/3rdparty/qpOASES/qpOASES-3.0/bin/example5
deleted file mode 100755
index 67c0493f8c2aa81031ac0ac699c9d76e239fb470..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 39753
zcmeHwdwf*I+5eo~To#rjd$@+HD+UW{E*laED!Q9w!!G2;CV_zB$!$Ycb8B`3p;$#x
zYgAgWTB)@X`&MhKt@N#}ms&+Z(e~{tRco=@O1)IDUMOB_>xJL<nakO;n~iPX&-?rR
z@%wOK&&)jYJkLDy%rno-nKNf{slU3)rfE!FHg*o9)XUSn93<|Hxi8~M2`gjcn3tW(
zPGK2<q~qu1ITVVR3O|ypRp^j-c7$p8mDvQK%;x1070wc1ra~nlg;ZYr^2(?P|9iGb
zst}TM0MU`GPv-L^!WJG;;bAGq0TPT~Zhj{)(H)j_hb5f~U6M|Ps(w^A`Zdb@Mw=HY
zN_#0_mHZwL374+=*JB+N@1Mz&2)#U_!VUpuDpYhUK}T}_tCve6hGcnT=_TGif<U!H
zVRxi$QAuHUM}BvtcW@|wsBBUGqLPAWUqLaiH}NOjWwlM5Q0XxjDUx|Iexz#3Z!0Uf
z_wpaVcJEE+zrL(9M=P9v*Kg<Q$eV;8;gU{Ic1>)_(9+V%Y?m>1o%b^L0#=mUvc(zl
zI7(Ss2FE3y#D4~UWH)p0I~l)I@SBI<7w|g`zti!ft3U+&UWEI%um0nWZ$A5O_Dk3O
z@y1&=?s)a0OK-k-&W=Cc_`tTmX0xvJJ6`x=^Q^m9yt?P-uU>z#+dsVK!f_SPKXuvo
zXX^j*qsKP|FZ$W44Zm%#@c;I$36o#B_3>M7A1-ct@$Jgf-re}(uV20O%7S+{PB`->
z=lvazPu*Yq?#A8&`=@@p{TGifn0=ixaf>wZ=#QYdiSeWv8$m1;oOI^KFZr4afIAt8
zbaV{<w~YaRbPV{SG5CkZfPY_0E&s)1z+VENRQ2s1170=;pU1|4KQu<Yvd6%`dJOz^
za87es7OS7`g=CCP;CIGug|Sa$+3Yn5mtgjHzzP3Au9t(9|F|87e=^Hqdu9mud`a-%
zyxc5SG*Q6sk<Rc;S#C0)UEnj3IoUy%z?W>;C$e7K^+fpZ!QX`*w7aN9q4Mxe$f-jf
zCgq`iN!Onx{!UK|PpjK^flu;}NPM@<m<2)?o6bg(_~(Gn$tZV^gf~n499gfUQa^@-
zmvTKIA@v}^Y`@f7zbrRbW<1N~oIa{vH}H1JVuxhA0F7UNk$jl+ze@je0iTHWQsbjU
zXSIM+JGLaX`;8L6U&6zZV1dLxl+^BX0C%U*PZV&{XT7W!`8K+?Nc|-9c~a`%EAh!6
z)3sdc*)7|vS%BG30oS4bEfTKmkipM?vA@pO;BO%3zObN(8OHjazFs35YaNIg1~Zm#
zFhb$=k!UPDP|@8QjfSJl2sYLj9pM29Zmg;3?&}RVwzhSLMPBNhwY7_7S!FBh2l{~B
zQxfTo#QgOuIvR?Nr2nBveFrmA(0EFtecc<v6@vo<;og{kWxYR=#Gt;TVId?>L0i_<
z7vnWcE>o4ZVw8xU()Hn3<zQ=fLtpn`EYjEOA49I<qG&i)(bpS|4YWpjW2j-rU^^;T
z+hBMWkA{<cW@$^5LdrdhQC$5%r01U(Sn2U^NLkAYk3SNjVm*OyG#Y8`ZQRr!_ETF&
zV;$|MpH5A^2o2pn)N1q(4up*jk%1U$2xg6|YCT1iGt}A^+2HXkMx0vRh=GL>>Fw<E
zlZ6Zo8U5jbXkTw@cO<sS*Z{LhCM^b=Xe0&o+K%q}4J6F^_I4vmvUr%$)Vnd#+W||9
z4)%m8vt0?<KLBNR8tq*dv7T@bRLoF`zQGt9Xzhi87=$aWwFi1<M!2Ij){2ZaNX^?C
zAYc{_Mh9qGQ`j0qh4R~4qhS#}v#_hLCtMhbc16|?4)hhax5h4t^cowZh3#FDj=q6N
zVcTG&yQ8pcpg+1PTDWkaqaR&kQ=znF73QP!5dY|zg?;_8!hybMA!-y^-$%fIUPecF
zL%5qzRh8PKgGML12Whs!>^Yt#HJeh}Qe!ZVlhWFPb<jo!DmEAmCv>b7#wjezAVn$_
z#ye9=5t3~&HBu2bSZj4siYnTa6hQ7ln3n37DWru(uC-QwOckw)t7t5oc$Z9BgZ6Ij
zY!W>{3Xv*{6T4<A6rqgI7m4#(R#9P)XRc^&_LT<>Pr*XAtU6eZB)}yiZWP0L$K%BX
zrSSY}w6kNLt>SV(khvL{#asWSTT&Farmc9CLmUbAB}8caapPBi>Lw>WQQeMli!KdV
zZ(}Y(`9=6Wd(TD4NoPOAonWyA*JCKMv->UhH%WfTX0hp!arBh1PvQ^GptOx0koX@J
zwh$VBhQp9(UTU5}Xl@gVxE%ioUTA@<agpF8r@C_Gd?gbARKvX77PwWj^DS_zJVh3`
z6rJ-bv%sy-cU}ve%2rpv0w1q{2<t6yG=O<ETj2EEq^=eVT+LxAuge0@QpgDVE%0m$
ze8>XF?9sfoSm4yo>blwjr)NNQZMVSZDj>q07WhO9Jh1gGN8sA@DJQvE;L1I*^ura%
z3~b%!*lR&T^mwTaD97ie;huAn7cokA5z>)E$8pX3#~h@oX^$M>>0^|pCOxu;r{APB
zHRX}rJpBr#F<WCtMtJ)7l%^&<vYn@&p)@t+ku5y^OG;A{9_i=l$0$wHuOlry{UD`b
z)9gq+PyZ*SscDaRdHM&GrY1d7#M9rQG&R`~H&5S8X=<t?E}p)g($qvp7*AhAX=<7y
zM?V8#-sP00COL8lY3Ohb>2Nr`4Z+^+zSX`}z9wH|Q$yggw+>I$SYX=~d2XmDux<O*
z6zm5jX?l+%u;jqt(PKHT{5_~mVB6h!M<FHBOYcYeSdM;o-Uz3=@hEB&7(UEFhXCRc
zH&o0cEh;i1A`kK6i0=^R<60_0@SCrQn?>ZIycR@qPO2O&S8(_(zvpewi@t}~cj79@
zIq3>6_~Bon(#LX6+Y{Wnuk2XP1?i_yA-){FGVjU5_sG2AgTBALGkkFCUhUD3PJ$#;
z{*3l}^dGhbkNLJ$PO<s6KAsuaNyG#f#0R#1T#V}6u#c25_|3q!%Dk4qw)(PUeN}J!
zZr*NQQCI$MqRw%Zics&)YYA?<A#XdXR6TqM%wl<6+mSz;tcMg1Q1eHm^*00C9?Cle
z=)?H22gukSET&f3Uzz81;fV%efU*!)^Niu{y!yj?K1F+Pi{*J0`QckRq%seha4prS
zm~D+D_r0>oMJa*d%Dh~vQ3(|QeUX59d4A7Re$O7tbOnZwK)8$Z%8*~uf&9ZIob$bT
zBS@IVz559XaBrR$-Nytj2WB;Zho&;-+4I02R6nromb^oGD21x&1^C12;lDjdYhv#O
z?g|XQ+zBkis)vssJ_9Tve10XA5Zv~aLsKZYZ+lo87cqrocjvjM$w-TejHt*V6>;NX
zk|VdMNY|+na;J(MR+0H%kXc<Sa;J(MMnu^gWSe&<yh9*;Cn^T+BVE+%mK+^?oqE=x
z@5VJzJ$!av&G6rY!v_PyF9@{~2HLNB_}Ifx13D4Ze>nK@@#CF2tB-r$<`&KsKVOyS
z+0)7UgKt}7I*N0ppF)MVKXWW+%@xlHZ8TTzYk@gwfvq29f>?kD|GQ`p`A0?e7B!I~
zL)ozM=Cpm1j4T~w01rMb>_Xb3X&<nSn&FQ;PX&h4@;uK}4}WMHh7;104IHBSNV3Dv
z|3d&>Xhjp~0j38+Z2=}z^|@}}A*85c%aw%ZU$<`rFp^Ew5*ibAJO9znoW}GuKs=UX
zROZcx*tie=q==Ms1!x%CHrWnl*X^T{iR=6D^t|5S%FWc|`d!X-$*~;4)t=1tr(5?f
zA<q!p`b1gqvi;PJw?1Bc*~eb^;+!ixxNC8t_=g>MWPb#??0&i({=wg=?xLq1{skp2
z+eePqv*)~ZzVm(Sd>8nPy$T^Py!Y@o31|4_!yYKj^LB9fHx~qkUkPk|?Pz^taQ_}!
z=m_k8l-45x`}fl_(&1(hyf5{5|9u!xn*#Z}5kN`Vf#KIT+Scv8pL9V3>HF&*p{(PF
z6h5UC{-Yn+fgc!tEb!=?=LWO`f!};I2;v6?@iQO}3_l_0Uy<}%mTJ@8uoJxrl!q_D
z0JQFj^mlUIOnYxH*;`=i(y}Q)h|MuYnB4Tpp?pX;{QB_&>yB&&NzO^^NK}M@{h0@#
zKhTQyOHL0T_1#Y%A8PUa?eOqXIPjc%HafWT4qUs;Ro(vBv5Fj5^`kBr!?-~E^IM<L
z0_`87>C+Ct#@E1vnjb|LLf3?&o;?H$!t*viN%(uW%kqxA_BRp>@(X#6JSWo_KZlR|
zRt1MY^fd*Cf9q?k-ZsxW*$owL%MT!Is9x~8=f%LIpW0~#bGQ^_fh*sR%~AEK9)1(O
zr4l@jPkt$Y5h$?aFYx!6*jTK=GM>G+7wtxKq0v(xY86KFZ@Ow)hax?LJ#IX-#|8)5
zT4^1_-M1k;;BM{ic6USu!tDf#p6Q+!w$3XLp!w-i>r(Njl~fOIm^*s}zFHSl>r<Rb
zid&pt>IX7KqWGxwPqqG<yuPXMlf!-(4P9y-O|9dpb;oUQ|8(SUPwYO2BeA3BitvyO
znU}FWFu<36GXD{o{|JC|T`JPh_e}d)9Z1lGZTknukMBmf`@`eM>(STuAf!1|AI7XL
zfVW`eJBY9!;ZcNJ5IT^52%#I{QG~S4&EPu@A?!yuUo<Cwzk(d5eJRAWA(u9DLZ)N8
zmg%B*B=o}J4LHx!2ti&1rz;Vku{^*C|KR(gOqT7c%FbP$<J{=j!p@y>)@cj#<`Nyz
zSK_w?vhx`L0dW!heEfz|z+-^-18yKx7vW!r-~E7FpF0TtUHo>ZfIkFyGjwTvz95<D
z(!Nw*%{7o|r=?=M&;#DXA&=@-1UgznPrG-vmz8I`zGf@W&i$&rJlnl3tvq}Fb?LtB
zqN_4|*=1WZmt~LqD9yepeSCIVMRt)ddp?kW45IREhmRT*7wMoHZIKH(G09+pr=NKG
ziKm}<28d^Xc-CZlyR*wsMiM)&w;RC63qG<*i5tI;AJ?Y7Xot{m+lMo6a<b3HBmM&?
zZYQo28aSbW6B;<7ffE`yp@9<`IH7?P8aSbW6B;<7ffE`yp@ILOH83Lgk-4$RLDv?U
z<_X+rnTW3Bklwvp;q<QFnx=Q{bg8wKzsR&&n^F4;)w^J|zj3-CGxu4~lxdd?)w||{
zSjnYJt=FivqYpki-be7AHZKL(5g;zN!^<OBj_23ab}tXrn&e9oQ7*@@T!F^$-Y4P0
zGQPOv`lVW9p|>z}(RveInId3#2gEOLj+ciDekfa{mE0{nmtlXrxK#OAn&b6pk@#2>
z<Y^_xQJMZE%PT4W-vnwOdva-0bs@*8GF&9XDjBYlVW$i?$nYu|enW<L$na+}d_soL
z%ka-K{7{A&(tfAPFj+2abve^Lzo~7oH#X=lDJUx_$uAz{DbFRIl7ga=f`tnNoG&Aa
zW)+R2mWEYRsij_(R!hBVIZ`blD^piX!YZx0T8@-4c}s4x=)T!vJ3BVCYv;or)4E|~
z?*YrDI;EtrInri;seL=rj_1z8-R8*50kTbVTn)pw*)l&xf3l@zPUgUjYdA3d7cjQ8
zv~=u-13~)3c&}vd0K&Mf-C$vFhY63nf}-Gd6M}R{hGQoswTvCaz>)p{NHW6!WjKC}
zdnV6yWR3@u48C!~VZR6lJHF^?<c?bmqJz#4>Y(HCpP^qnKO|B%{uJ20^W*Cgb&S6T
z{oMHp#a-hcf#RH>QapG3_d(?RjN<O`t?2vC;}oCo^r7BPrct8E`Fl!eJW=L+4&pm)
zJmGa-0V$kmJP~mI0`G90={!;IoK1`~c%s=kA6~)f(7pm#i*s@s663XPNOU=~pg-pX
zjRfv@euo5e@kGp71symiYEk42IZr2=sag<;EzZsGY0hcdY9uaq{+bHS)xHjHTb+|g
zoLSm`AaS*GIWd^0T>`Z2&Ra<@1sb*FPUr2EDCCI|=iQVj;)y$*KcR$&Cw4m@q(re+
z1e*Ju3$Y2tS;A%8<D3dF>s+Lf^7gR_l`vfA&Er0RjJeJcs^s`}q@>#@%EmuRY`#O)
zb&QWuliWeIb)h9!0hZyQHq4?HCb6@J5g{nGCWk(tpp#h6-_X(%t}34h@;)dbZNl|`
zK(5{mgJ$|2XeEugu7?q)O?YH7RH1(jm`tCFLN)y^h-A}$02SNy1K^XUuR}ate-UcR
zV6GUl)6%?uM2>zIcwef2nG?@Mtu+0=(1135BTBIA*PuRW`ucPtrkB<k`q^lMOnn<%
zu|t0!RO9sD0Ww~Hfs9W74>a)v{d+L3Ed5>Fv-MxYj&t;<K<m=?fxoW*79vj6Z$dpL
z>6bvcll9HOoT9r>)>Qp|RC1brGN^L(+dws4KMpZx=-VLGOuY{3pQUdH{%rkK;Lp)d
z1O7?+M&P)0J1Tjyz73Mk)vpHaDf(vM=jl&CP4n~;)cREY=1KS_Mem2k=IiHz!veho
zGMuJg4cSiDw}HbM`dVl*U;hU*Q=m^pxrKTGAVvBF)X}5AgtChD43x4^{}bw4qL+ea
zseTT0vPjqAWESg7A%B_vW7KqsUIq_wrv5N&>MXq$x;k4w1<kNjpA7Atqt}7kx%v|1
z?qT{VDBw}1Pl6PWF?~M>_cA?#U?0;xKz|(D#c@5s^s^D{XZov<`bnlwz{vX)(+$Y+
zOQwGc@?RkjssCd7F%<S|rZ0e`|IPH5p^&GU-U((0n0_e){0$0%9-d+Pmx1{#)Bg$~
zo<l)U;BT3}8YMmt>q785rl*1DL8d=|ycd|>0`lK8{WYlfMW(yZDlakpCy?b2O#dRd
z{E_JcpnI9=KSM1KF+Ce4{E6v<X!}<XfbLb;GEiP)dJh!+I@3RffPZFs9UABjrk?|p
z!%V*(sUu8Zgks-h`ZB2aFHHX(=-y)bbmYCw^eWWo9n=8=zRUEd&=yCTei{n;E7Rwq
ze#e-82q^C{{TaylKGTaq_W{!%gI+#_fH01~G5uZG!beR1J1X{frr!lF|A3Z&@-fqQ
zgZvYw??mt^)6a)wpD}$l$~z8Sqm8y``f><(nWo=_x?is8_oGHzHT?w?bcLoj0Od+e
zzZELDO4Hj>&97*BAKLF~O@ADzVNG9wg09i@EJ%H=rtbsYb(($;f^C}q1}y5Unhu*}
zU(@tYkiuq$i472Ye%9i<A@8If#Ix?LgLup~>04-pte;ZCVVm?8Kw0;Nad+7!<wI>*
z_fulBZPMw4_5dYvZIg<i*Q^IAG0QgTH3V5dr-a)!nTp9;rZqty=ixV{i3$<W<=RDn
zY*5gtL?N?kwW|Q$VP>7jsY1jnq}>Ma-70Ger<&%6m1H%2hzhp?RojQ`rxnUnPCcy-
z-7f3=BY=|A)80Vjn292|T!M5~`wdVjM;VXE6#A_P&#65m!?f4XGFg!qP<f7$kL<D}
z6gS!;tA7anmAGp4$Ue_R5e4<4DYG{G42{iEqR74^31u$mvn~#zK{?7dkbR3nVbjil
zC1!0&M}zR(dy(^CJojm8o3DHfJL5t=gPcE5E-7EAd0GZ^kagXS<U~muA0V41!B&)a
zNP-<dg4#GrF0$t*ph#A<(J$+!S<u}+#56B(RuCT7nyBwQ66m%MAuh49wIiGM{R(TC
zHYXjqx7SNqY?mSDI+ZILFLw&*^zK)HNDOTwlpW{KGHF1vd(clNY+C??O!A-8zlXN^
z3nDYlgtoHp(~4B9y-egbsCH_U8kJ4|D&kX+JM$bEa!$2&EjaN?XMx2c{H9z=s@pWy
z?GqFrc+KjAq&e?vjZg>YU~{FD4BBdNX(LV!v)q~0Aj|oOHUbei?q=j)WtMh7$;Qn#
z=d0_K=(i&KHzvxM0>v}pqRm)==E>Qsc|gvK`?JZ1lsb0pNJ&ytv)yQ=TwCD`3>5U6
zLpP?K)Vqq=x`>Ozc2aKzvqdOA!FH05*)Ak<mu)gp+7@aCY7)k8%9T_-0bQgG0&<Ol
za=Q{Y-aZ1!r`-&M+fAfviQat@ab2!a<7p4#_PENoI0jqIZ{YlP0AKq9vJWeishq{h
z6Jh7JwOim^NNCOGvT<%{sM(a8KqMsL8WrX;yA_IbXiKQ>FXA`%Jf!a=akR;lr7hL`
z0Iwhhyb%S|tDOf(r-Dv>m{)*41J)`v;-ziI?JAXTsercZNmO$mRp3@+e_x>p7Q6yl
z=nZsKRRPZJaYFlLstROM1v3fhlv_}Iu4{_+Iub9TkUY9&Ow0((%9yKZbH9OXQL}l3
zsm(@iF@94HfG?MD%JWphOSmzC;zc&~<s=j~>x%%oW@;J~P1H~{qf#}3ZzO_Oh{>##
zxVhXKZR3<3hzqCf1j<7SZK~P%h1pWYxrqv&bSixpDad|BB#F{u3fK$>>~dX9m3<su
zX>JH<QO_?t4N_NzM!w8pn@5P+B;4lUH{~$Z^&Jv(GUu>GD*(7eWij`?Ff`XzjreLo
zfE!hoXwCVz!aKOGI);u%cE-{AfwDoNu?0KOG_Gq-Mq^T8z_N3!Zvf*K!lIcoTks}4
zhwH257(mF=0V|DTJp_#X3QMSBK^GNy!&X#>yJz?#j`{}B-d3n=!CZJH*Ee^eD@cYJ
z94iZQ<l@JRl?-W3$+asNHI=BD9JLH+fdo{Ee>D{By1fW;%Jy_{eA>i*c_Kb<RAMT;
z&4QOO`*Qs-6NV=#$8pN<fa;;or+ftCnd_$p=q2TNPWck3(gBQDXDTm_cW!P&X6aJ+
zFQ<dio?pXGB_k(iG!IN_|1CybqF+6Q@kKJ4z!~*|$sHymu7J2{f39p(sX5sKF5n9p
zsITOk#rZw~o`3%z@ns9{hQeKM&?n83Wj1Fy9qpKhpXdc78(UC_Ug|pfDTYgOjj+QU
zj#>${<%BA%^(dmQ4_3lrBtsX+x)9(&g*Ejq@MF{5VCDMcR!m*EwQfW9*A)soZ5<5W
zb^IDI(!K}i4=IPb2|N8mGD@4ao*W82FbOcrBoD*w^NfGMwlls)V!wub-tH{lU*CZF
z{6hdbW`%}#rfa*;F8Dea>PtB6g)jg9g9FDNKC|Eos8V0bVczS0IyC#9DV2ryp)K|D
z|3pt5DY^hOX{GeYo%tx3Wzvw3$Mb!-+tSiTV0IY}vabvW%~U)Nns$45JcGyaBoA7w
z;{cxFpmH)C#3RE&uG-_EVaCJb89ZK0%*bIa1j>&PmN@8WfNygq<k&rqL;Ozh3?9#L
zkS;PDzW}AjL0&II-l;J>j$d*J-Al=#*&TR3nTg+`k3hyFPok=KfSP5J&KExfGK?H{
z50GjUB9AD<MdV~c$YPn~fir2SD5U|=9tLno(eQ|(VVN{!meM$HzZMv`Dl8s}W6|`X
zw3K-L7#KfSSj3B!J%GFR5^hTjs>)u{){=Ze=22EkGZ6cmK%gZ#UPEGkra|+VMTCtg
zrZI~}6tkEzKJ=gUMHo?f^>!fGt3bKrhcHEvUUDx=O0VW6Wx|=JR~G?`o&7WvYF`W7
zGl!)4v9o>*|DV}OsP>J-<vN&qW+!!kg>30+2+|0HNnZcdZn+t_-@)%3(w^4DmeM0@
z8ETchG^*Y5IMCiAR8pbVbS^PX#<DF}jF^|^M9B-P+AU64cmsYuG69NcP37MMrfn_B
zPFKYcvz^zBoFzz9_Q61G=dUBgke{-vBFxrGxP6pjnKYfsBxjpR>f}|ehMP%Vy47yE
z5<JLPE~Bdc9HarNfx^SwnqOMwPpmjBc>Z)R&-gD0vRrGjzXvS|HC+UF@`|u_%Ud8{
zi6&h}YI+|i0aB^LQ^nYppJm12${dZuMI|2iWTiWm94lyKPF{u9Zn+-3&n5d!&a&^V
zJ^=9X<28>XfQi&TPsW^2`b=KD)^2$R!+af?+zW`*+xOCfAk=U75PCkSt1gs@X_RxJ
zO0`p}gV~G8_0X{kD+A14O5D<RY<X_;N*}W?`i!Rz9ND~bJ+m*)fpXJ!?0IJM%I-Gg
zmYu}Y+kdw?bTg`A?<J3wwqxX(&7n%Pt$l#}81VnJIb@`h#Zy6PJ9Z-fo8W67A_3EO
z+<^3pFbey{g8!ChH;0ZA{l$fx|H$t*H{6DPW51kS5N)yz&8FhEo-4#TzB%NDYqeh?
z)8D!{^bk6y{mOcg|Lnz~=g_O{SIPYQE)LxWLG53m^=ydutxKvRP}eX?lZS`Y#-GEz
z*>{q#RlNf}%>GS^lC=iNvAjLW3q)~f4TbhzTBTVw4t<W+Dr-IiPI1k9xMwaSKl2%Y
ztckqM9h1F8z*_LLUvm{voe6LqbhE4wkwJ>&A@WT`1j$b&$q12L`+y>OQ<2cuE}WzU
z5kc|`NpgTluA2Z&@KSmSlS;Wkk=&2Ss@Ej;%g_b-jqOt08?~b4+DiL)`c7;0r&GLO
zM2k{9U@i>Ap&>HI?@Dt(a}C4o*}p5A>*yag2gpMnpX%ilw7x)>{Rf>SOFgoVk+2-8
zX_}Yv_{l*O+V8nmA+4oO*nFep@;;j2X=Fi0Zc&iZb_r4|N!AeEnz`(25VdU-R)vTI
ztMko2kUV|~9)C^b0bb8;W~&~N81%qzDIskKDg#N$iC-rrMfLe*|49p?%PvILV~}*!
z9Wp-~ssIbk0}033=@TKkmTRq<JFb3a^M{TFjIEg>O=tmd*LW<6wJz1(;B65^Ta238
z9K}@CMXY(Q#G+M&#}YAl_hZc$OAK05n2Ek-;l(>Q*rDprYz9Ny2IMxA2Bq$77vt~|
z(j4wC-aQe?<^7g5?~sg1mrF-6hAx|xm6I-wWDN9W*8DqZQnZ?}I}yX?@KQ~wWka?o
z3C8bhsBO`#B5*+T`z{vJq4%=2KaskB!orN5_d5wybw?wq!(d89BY`&-m(U`N{2YI^
zQY6x3+UE}=W1S`dCjr0_GDMneou@q_O}>Sv%S4)d4`+Z6#CQqhY&hW32?8*n0Ow}2
zb(^5h@psZYpmjOW%J}b7^a7f$nzPdjKBAPGmtHUd9@C*_q?z>))o=qe(sBmw<Lui#
zEdXIr^o*&DT?iYY4nB1|f!b&)LovQUL=pbPGnJ}~*<cv=498+{$#9&F`x%Sjz%o}8
znCw;H-i6dc*1i{>a^eR5(yQZ}sKR6DU7`@C>Ds*PNlttA%<QIVy5_>;f-R3Zo#z0A
zem<4iv(I#%<qX0@rb#$FMS2cUP%;ISm3b<oz;P<d8#hU_XFHulr2#fR%Vbdo5~ob2
z2PaW-f+_$FtZBhXM3H?mm=z*3X9+kdKCZL`w)%XqokjJsCBP?Hiz^0`$pzZE(*>Ji
z=h@Rcco9?5QA7!8fg*USi6ix92Pef5b5mttJ(3DNdpb!w!^B9BlcG1L0u%nM#EfcZ
zP3~+<fwSY)naxF=GYb-yIjJgGO0zpeYY2eb<mIe(TDhDY&mf_!g)lakR75;WrW4mw
z%o5W@iFv%l+-hFtJi&=jmGVv%ASzLT@!rkauEch}RRpU5q=W_5%tZMp`!s8Iyx`MK
zFjOtTuzM2-OB79#Ut;R%?CG=6PYVE$W-I`O2>?i2WCfsmxujtiPmxBs5Mw$kA$up?
z;mM{zTQ<q<6y~%jookc~YB9mlM(HZAtN^A`GTqHP!xBOiEr{n0BxG~q{h_rQZZr+)
z;cQU|MVA(6o=Lsgx;8P>o7*wzrgDyOPCD~UvO%<SXCs?3o1JS=9WN0k;rf)2gkK&P
z#D*?k5f_D$m6lu}`<+@7pjC0T64EkWHX;FQ*(7Ul_;L+?<;L>q^yInxn#(n2%iU29
zPAh~mgk-V~)x>8OW=b_Y0_kh5x=6sSBT1-;`ZF1+F&EL57O_x7DBhA((FRKrvNpz(
zn1_+zA#fz65!Y2haFTkp6E&EaNzi5mrNkN}m<~F|)(SX5&l6D$9o%=JGV9_6ojd((
z7{&!SD`pZIqroGnW(!3~##T|^UYLn6;5Gpu7bO7g1xz|I_)X&0VI?7_3+HcR`528f
z?c?k$+%=jA;-~#D9<~(eZSNlJ2&)5_*#9Ey@K9?{e|I=4PK{Cp;S@|`QHeYfbA9h%
zVOu07QE+fmA<nZZ9EE~SOgIOM3!1<TR-SxRSV32RKj}yoUQ$tC&<+T+C?TaPU4vu2
zTGxjIH~|aibFNo}IC%`Ig#}#<4<t>oNO!a#B3RPNPkqHA1M!Y<C*o)%I@K#`f+Ky1
z!dg%n6{VBIfFQU8dpp8I-8c$P0nAg&k^w4R(+ZK=;~aRMTYF<BO9+F~M6s|Nn_9)N
z?}gFW9Jr;~09I=e_NSPJk@riMEVT?_e}QS!zU;G|GR5)6=`YxDGLZ&CwFDseO;G&Q
z<G9H$&Gsc*I_26yq@Jwpj{%ToKXwVg_D#pIG-tC<cDzR6UASrI(d|9DT}HPL=~hNJ
zTizrJcF^q#x@|F&1hd(*o~PihjOL8tqqcdI`+SaD{^YxsXSwd?S-;$R8CKMQg!yZl
z?RX1_0*=ldTem`#(&jaeQ&(MKJJYp(>-mlqY3cS=j^%)#WjjZ+HBBZ0+cF+FR&yEc
zk6dbNg4+X=r?86yp<_SYSJ3Ti83hz%+?EV4wtYe0dY)s2?Ubpu(_GbQ8TN~)l&P2B
z_jj4@r1Z?o@3Z$fI@2=kwGSdW-m%^O8}_9Ao=Y8P!<}eY+G#W@)>KpnYnK_db&W>I
z-%wY*%3sMSqaj#RU+p&<8tKd#qo$YxSJoRA6h&*&wMAg`;no3#+f-X#*Hl{xj+P_;
zQXbz`NoQ_FBVP&|I7JQtnZK3DJeTm_J<G4ZYg07Rj&JG1z3XFL|BIf8*Re9#;4828
zSJc%uG=_Y^+QtT>$``DbDr~ByOrx>RSRD#B`i&~&aI%2Ep&{t2HG<GhRj?L$I9sl>
zC&nzN0#fbGQwSIF?mFaI)Ef?WH1<^ugu^xAp1y%i&>A%s@sK4WUgh^S1j~cf!NxT_
z*I(Pv6!KRG6-MJqt7?~+&SObyM@Q{oPg{7vkHhWgSi05?VSYp(l~G$Kidh|O43JsW
zR{BDf!W=>@>EK6ZZ0L^-6pV7DsOE-PTepEser{o^N-l~-D<iGzQL{)Cm4q~0mW>0E
zSlFP$0ePJo{EbcZ1~)II@=jAL?2?8}Jw0Laa6)kmM>mH0HrDoG{ima^wl5afARB-p
zH-tsKnbF<Xei1LAse!sh$iJ+q+807IHwNo!c}7K`uG-(QVvSK(Wvumw>I~VMJQGS<
zR$W)_t2S!T#=d2Kqpqp3zNyh{dfrkEO%)Z$u4<}|tJ$oXpIglsCY9JL>x+ag61r-=
zN62p)U&!yPG|K(86@eOGXhrI#Z*B00Rx}jhY|i#BI;)WyyfV1TC||?t2=L&5(T;OA
zdvK1UVLACx4uDp!V;!kSJWVkSiw-D5w5PAB8D{GXt%0MF9m(Q3WG^5yZS3PV(!m9#
zZq!^IY@mM7=npmcD^zC?>S@Hwq2O9S+f>(&GYetYJ*@*5!K|p<xSkDPHEC*%;j3s2
zuJW_?zWz;;eN~-7ZVX*j_H2bJ+DbUgIvn!?KVbMOmp3&u0<paxr;_5#Oe$uzFH}qR
z6la68NPDA=$dAfG;yqYd7|*DwTSbHkmQEbeolU<>>9`Vnm6aCdnLZ@m&pkyrzOp0O
z8^xI;)YC^hnnma@6@C0jMbs!x>F=Ne7NJ3I&uusblJ()_lGa$?z)~Ttui2<w-4KFo
zzKQ^paLH&NK+-~V++`CF_C_P?d&3=W*d97NcOA(c*lcrNjHT6R;F2vCfCV)8Yl3Jg
zQ>Q^641;%G@>wBj7kNCWRl62`akERIxiaOFpf;{_m|G%qt1`_&E9FVGP_b}2zJY;O
zVQDzxEm+OR4DMtxFgG<Db+y&PjW;#;ll+lURTna<>$xauOi`w3dM?!1(ud-0DNguJ
z+5DcOxOL(@-b53?3BHjIoa>8QhZrWvXJXvu?OKQV0@<r&7!;F+Ch8q#8wiVyn|z?I
zkvC6*F~()}lq71)Utb?#)VW0Gf!D1J`j*wz!ta_o6!Q4de=Os(Rj&FF+yzcdR!t~H
zCM$vo)i%=bNwz)^Ztb8ED?v8ti>RYuV4<@|6SYJgg({ge_A;ZtFG}a+N*^2I@)=d3
zx*BQ4=oj#?bg*rVj)WDb?+%F5cZnff3+83D%P<I6`XPHwuvS>0MdRETnEnjoRHeTm
z81iGZ=Ce62SXHoDI2BA;gcU(5I=z<)L6cFxl5;)jFLAtd{H$4vZ6}TWV;uTSY-@at
zArxDM{#vKTRBMYg1cFso>*+NHx}wZ{mSJMt;SzEuLp}tvtGZe-trFG7fJ;L&cbE+{
z@CO_E24bR<St=kKu!Lp}au!9ZrGWx<udT0Z2#R5#n<lh4n3#^cjnBybN9RKSvqQBz
z2K&1)S&fBRgk)x$3~G>e{y55KX{k&~P2rh2drK75oZfjBCMbjVA!-%N<dO4_Pg@&&
ztNgHqN?)T-jQTipIkw4a2B|0WbjB|4Ml@(z(bYV*G-(nh8vy;--wd0EpP&bk5HmWv
zTh~X6IWx2fxEZxg)zuC4z6wm4Ro~=OCu`%VVyte=;<I915#cv5Ql{|ygS~VRb1}Ol
z#evRTL1M15x-PT=p3X9kS*$DWx=LssAtzU6`(kwCoJ98*b{ijf)QQS*CZ1yJEER)~
zJT6QgwH@!4Ddt}0)Q;NP9}1zf2Wu_!p88N-Wm82Xmq*MpEJG^MDBle4QCD9NFJeB{
z)nXFMjK1y;qp!1*4geQpx14hu{E+Af4kE{6T|f0iImsTXmy@rQ-p}d~$TTs6lRr|v
z2oqF>L+#hQ`HjB0QZsHNI@o3e;jG|W$@-dVSJc+6uI0}hF#|_~3!S6UM@^>8D#}KH
zBjiWsTROb7sU>C`!e+?=#d8UrV=j-xj-Q-1`Xn}fYP9Hct1vOCG<>0DO*I&68o2r`
zJ<-5JT4#4(YmAk{$Ay|;?(tT(7=|ahv9(|5HHx$4*${UbBv{*Er+8el_KXzv5kEPb
z1Vh^gX#9p-t*qlhRrqSjAmZ~01KkgkY@S(H-iXIuB1qXC%qf)AUMU~5{g%Fnr&aZo
z%1tvqLB?Z7YX=UXr=VB_3q?>Of>LHgQAsWb&3PfRm<ZAk3Twjzmgm8E)TBt-Q?P+1
zMclW1p0BKi(-*2mTX#kVXb9-(>ku>gxZ`LLV`aU+B3Km!V^X89y4upk@K7M0DO8pi
zoD>wYVh+LSFy-_6Le*<9V%PiOJ!?h3lmjOpAFcf(P*+ojPPvB93alCMq-sLgU<$^E
z&(wBQ+D^lqbi<aWY$E56m5cbIhFbKY$wviEr>RaDH;o!R_2Bu=;3FrVpD>-Mis0`~
zP`>Es3EeLK{D<7ercf>Kv-CjCDdK}r9TX8?IuQbKec%yV3TF5klBO&}^r%NALlLlC
z8rk`CiR8^4gHhhJjHgYdlOwjOJuU=)hBhDKliY=+c&mX?@qDOgXewV0T{YG<uBlJZ
zrIc*6?ugXt49w2Qr+P4d{P~1fq&@Cn`MHq>R-N(JKd{)0KW~9UBMq@UkZP0zck3!q
z+@YG!V&qxP8dz-EY73;+N=z(a+?d;^oQ@haG~nUj|H3PP!i2?lp)$1)fJtComFQ)n
z770rvHDZY*aWSQ4p}?d$XzIGqB#@>Ccz6yEG4%&aIF)iRazU_?{=SyG;`GzSC(-Ww
zp4eb-c<K6ZZy0M*NVX2NcP$lvyUU%w-ko2!5Wiw~J|4_z2{pfSuveV>2@u-4{i6K-
zfxZ~d0__`MHF(Cxaw9oi@gR`k!xBAh@?%ND!yBGhJf8BtK6HKghe*`J{QL!MJw4pR
zGgXU)Q<dZH2zQ4uK_zwjR|hQ%8U!&rhMFoQnwk7FA=UvYK9rTAFg{ns^rmN1MSZGT
z(Vt+UMLb%ypdk*58SIy%T(k>QW;Gu0XS-_uDnHzCz*oBre#$aXQoVd_{CUP4p**EY
zOAel5bEroSjR1q)VnDI>%*46SCFW$C8NJ~lj2C47i%QS|dilaCRvU55YwxPo?m@o3
z9WOrBF{|;RndUQM0?aip-HdvG6OVO9?V6xa3r2pLkXZFRs(NK=v3}G8KK;=i{E43k
zOI~b-@U7kQL7!H;YA`dyv`jqNSJ6s87cRcW=PR${D=yrRB=`z3prCEUTB>zWGM_9z
z|4#z(=60K2DiwE3p{QhX+w=$npGSi>)(L4?wJJyUnG$MYn%0e;_IZk6GI9l(rdjgA
zyBf=IXc>RCpC)ms2ut~(3mvyJOHac@IYxh!$(%RG=XO#SSYP5Yzrj5_9!luN-4JW@
zQU6T(>pT!#Jv*d)FiUt8z_dv9PWf6QKHTxiHilGbY+|mO^w?hQTb8QAB^|VQAHg!a
zWsThse_DuILremczm)SL^HGk>mayWY_^_z61}k4H@tlIUDK!aY#=XZ9@(k+T1#zG`
z>l%dI@l)%lnOV|IQH^K#lRkJy)Q5SVS{fxU!TU3pDCx1m)Lr~Fi@5|vMO!BV;)MpW
zv;MUzQBOX<N}A5FUaX)pT9}afSEHJmVuk#5p-O*9bT{in)EA<~3rssIeCj2X>5^ra
z!z`I(W1|0z8t#+UtH6yvo$`^uoDNf}KH)E6u~dIy%kq3g%V%O8L%2R_p@!AI`h-Vc
z0<NsD<C7&(4SLCwqyv9*h2LL^DHBFPHT<C~;IVLEJx8tbulCdWGoSQfZce%+n@r+U
zQ5Om|)st1=FI9!b8xOPcl_CD5OZDXMj7YN4&q483LW{#lG)Xaf!l&RVuF6*}pPJOv
zoHtc{CEG*~#A=y~%L!{sP>p(*o;lr%?HaUUqftfcBq)u&r#0^4xt5zE{4CeYp&DDM
zqKZ&sYb*pjI!E$cB10Q1+NI*PCd{)-#T6?XD;_0*deMKLq|;cFimF}yBzn^*LbVm(
zqBx<7s%`nH3DutZ`iY{>Bz=pFUuMFJcd7UdCT!s@75}ygE7_&ucbKr!T`GQ$30t&F
z#a}gHi+8!?cHnbmsIdkWEjK~+DjJa>JBwbrOC`3Lcn!(zq`03UQK+UWuE=f7W3o~k
zW>dA(;VD8}$7D#nRD7n~P_5zb8OdlU4ti2X--(0Va<guw3_+MIyJ{s>?s#^q+FPEC
zYHqdPFWK3#S9X-WsJ-uYO9fPXf(i5NQt>G!taz7-&oN;Ocd7UnOjyY-6)!SjrMpyo
zi3wY@OU27gSkW#OUtz*LyHvc<gca{n@pUF_;Vu>LG+`yXRJ_lGmF`mUArrP}l%ABr
zCrM?dG1|(mQmW-N)~ceB1Voj(-E7)c6;;h6I3~B^^U_HFAZ63oDiu|Bna+0L<CIYn
zE6OQyw>r@9Rmv`vJXwO_6jjv1kT8=)xkFOK1s>0`|0x#rh_N{RC@$Kb<C}M>q|&v<
zE>KZbCMrRg$qb+Qmqbg(N+p`YdBxOsgNj;Z_}0avWOyls3?2UzE0qk2ld8PN3>CG?
z@a$+AlATHw`!efia~<wHhx^opc{|wL@!r>F;RB5K7dX@(iI~&Rc+Lbb{!R<!sq_|^
zo;7P!EtHWfPgTXDqskc=s~ne}Sk9q|BAucf^}&g%_IPaSo#ExJKT0Mw{GKC6KCcYb
zaGPv1YM@gePN*_eFG$8$=C3|S+ArJtmoj`>hRTAH=@p&&Hc8>B)=tJ(cw?2L=#&Rf
zrdN9p56Vf5+H`nWrqv`zO+(a_L(MPLOhQc?)HcFGIZ&&iXe@oPg6O6mJg^~*4T!Ky
z#qb0g9_VKUy;z<pz+ZvPk724QQrL)EKreDT@*^FrfKpwp(Jofdv8fmQM2LAY&%s}O
ziDJnvAz>hIAl%(b1Txm&9b*OjMMFW1))xx!xDr8LAOF*;1>r8EbD*^+Y;<*iR;9qS
zz1;{8wTJs-27m4pJd9Sp^MLbE@eA8Qi1J!{BJH5-LlL+Y@Nz{N`0K5#puG?KtFS^?
z&=GDMTo2;b-t~AHFO&4wXjQzmt!*H@L8WMMOU0o<6~zt$6_uQlP2k^6&jbPOY{nj4
zewEq0GUS1I?xjcxTotGA6#VFmHAUZO^CE?f;S%8HsrU_Bb`vfSQdaty&CBy+l0J{8
z;<se_N@U^B()6kFciOxhd8Y)t#Z&mgMuy2#r&_?Vcifa;o!5CaHXzfrQx-^{?vb7p
zJ+_(iYZdN<s>)aAf4U{2I`30pkJjG>$e^<}6}>uN^AmiELsy#!O!*a_3NJ!Fo!zO@
z>io_Yd~HP67JkR4MF>ewCBFo-FCjuzRP=|ky*%rXq;KJ=(fU*MzXJJIdUgJ&7oVQe
zrOqoQ{o|WzvwTJWRlrEEBp+P|U0xaTfXxtrd8e|h7k#~DrDwWVWa}buQ>Nu|$+!w{
zK-x;LTpoQ?Mi+hLXuTAjg58XWm0q1M`x-tfpbKAzC0xn!k06)mmHycbFH*Q<#ru=w
zck|5n?M_6k^!+ovJhPuVHm(j4nD^xJe})_@{Xt28P}0AYRDQDk>`9`hKT|~4z4&I1
zuH^PhV#l5V4w;(lZsvKsPv#Rf&RY<Hc~36?kH{fYQ}pV*-h+}p89zDwCTK|}MNfaZ
zjjlugBK^C-Reg)-)OodsmHe_{m3^!7RlPm}jkSFB_kcetByREkB2w|R;`1potn{p?
zMW7amU_=DwUD2s91H@MP5lKHH>6M&{fud9C2_Pnz(%&9QzgyBDRERQkC#5kl<mF3r
zGgr}{ZbC$aWTi`mGbFu$#_ummhTG#H9#eD*ykHFai)diN<(GCWQt@(Cx^xWs-L(Q^
zzZ!TXL2~&OW6*DG6a-mG9X6T1PSPjq?}}zYf7clDD?V$-D8F!npnoUH|0z0I9Tpyg
zem0#iiwh?&CR|BLoShlB9V_AwFBPe15<yau+kxpjEeuAk5s8D+0nbBn34Y3cb0sci
zE8CSI9t_dIfa^D^<BFtdcQX08fgRXoHt#4LJG^~kO1PcHQo_^BbH=UA?3fj(`AQ-@
zgQ<B)B0Q6+`9&h!!PLAV5k8Ko`9LCkJX7O*BHU??--&RH{AygbWY}4jc?O6DjP}T8
zY8*|3!_&|3T0uN+$0(u3J!_^N)}_X?MEFFEPhKmC$L(yAIsRBP?Ko{+jVp=pDHtbO
ztRNn@v#I7dV9m6%X-xU|L^!Nb`E5&volVC{ODz_Nh}to#DnDz@v@<zqTY-q%N6q3D
zpCsoX>FjJy5(kBU(im{}81R$FfX^KTr+%Hr{(3w)gXRV{i=EI|PZx2}pa=EAzv|az
zfT!Y9!SP48S3Tgw-<9h{EPjO%CAc>UJ|y8=lHgwjoaDLM(}GxhocK16kG7wEfD`|b
zBt8c?{%AWKt6nt!zz4K!calAS%HgBy3vf#Pj6(xxY_uJo#NpG~?j$=rUBd57g7d%k
ziGGyaUxL)YDf$<k4XmZ;FEp;WQ`kSvhvn3q7@F4=W5D-tcosV}-OF*+SaCqYlb14n
z2l&aDQI5<I_-;;xLyZAfaS7tVcrZ)_p96R*Ig7`DS9ADizZV(<|BHa9Dwht>Ao;yg
zewP&X9>8^)DNDGTRy+*&T-c#nI#T`UPh;@^7;smLc5z@3B|e9wJu5!>fS-(hq5M^|
zl*f?xYRQS74e6qj^Hb^V+ni61`FDJkhkAtLXR&_SzSM8&dU^~#pGbJ}-`%D2+(~bH
zq<-cLHf$;2spO&4s0m;FUEW-Y|8>A!sBdzA*~Rf^Ft?|LV=6s7HwJ$P=8dV!#n=6u
z|4zwA7Ry!wPVzAM{HElm!<DFA)Y6|J2}UG5x!v~yo~qn;#(>kq57|R<ed!F#RD3)N
zKGCa0=V1tNl2a{7Dtj1I_zBoJ<{NMwdfp=SpzQ57j-NH^ys$eseDwT;j($vKw~1e@
zr`kGg*+f#kSZ?CP*Kh+dY<|NZxNgV$^5qry?1(nL7_1{a5MGZD&%^kjv&U%1MnWtD
zrT}64N}sX5yRQv9h&r%~Eo!t54q;0Geas&VcN8pIvY2-Fra;2RjNXV?p2V)E*uW<2
zjG!gXj=`QDe2X9#Jb4QH03xvjR0E&Z_-g$445t#p3dv<$ey3SiRYhC)8~JBfz-X+g
zFtIzLeMT33%8$KTiM+A7$_l_1J$$ZVRIaJ@)dVXTEtJw0O~AyKH?MbDb+Ej`C@v^2
zz_#W1HXU=blc)4w`-}#g6(VizMtgLS>jhh&MbT>;8$N&U4>ph+>wB;%loOCLmTxdZ
zvh6CcwK0l@6Z>?ew9U=NLY#<V{oFHN51zvppQS{CEp5J!Bk{@x8*8w2Ntp!HOE%I-
zyPr@TzV<P9EREhS51FyckAE#?(8f!v=2Glruk>J}1wQDi!6#e7CKF3DEbU@xgOppP
z1h2J?t7<((hS5GW)Y=x=;PEU*oOH#%UWNs9_W8Zuisoi4{2QJESa7^)Q?sS-+$~6E
z`>XOSt(a0D@#g1m0l>=KLio==NkTnZjsC%bu(2V6KR4gnog_PI`}wUAubnCQ=kL->
zY~fUU9LX;6N+E8?i5f44_M?%1S?Q)&|Ljw%RBiC@o-H6ZSz5I=Mhz{kl+rBX8$B&@
zrLu7PvdV&DR=2`~U5R`@ujL2<KmR-^sXK=GTO6z1m`qZBrV6hyG-Tj=qiA0*KIMsR
vGBy+?na$@J4L{f+ahoCps;Gc#DWXpV@PS*Rfc*0v?pv)DO{q2hHO>D4xFENg

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/exampleLP b/3rdparty/qpOASES/qpOASES-3.0/bin/exampleLP
deleted file mode 100755
index 87ee8a1ed82c272ebc3adca06654f738e83a0d3d..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 31905
zcmeHwdwf*Ywf{adNth%gISH?zprZr|YF-c^D9B7Q6J}tNi8B)(iamtPgp4FJaUKw?
zVhYrXih}i4+S-b}y)CxhTie=dZLeUlmEPOiVp|`*ww7zF1*?@>we+F+eb;`>oS95&
z`}y?q`^y8Feb(A*t-bczYp?w{uN&RI4vVHSby(QtjG(hs4vrGPj1Ba2P~uuyDRZ!S
zYz`{{rWk(?o<pfPQ|VbLTBSBgXGL0szgCMtv|1eeM5VI?nyFN2NGX+<Jsk2>q`RgF
zP^FC7IfD2|*W<G}h;$P_QRzvk2P4J!VduYuCcaaW@08?IsV@0cs_I8|qrZ^M4_O?5
z2zF4yT>Lki5-lCwFQvkCezKZ_NFDq{rOO4HsZ{Y@0Y1|6?>%%$7?9;nWS4CF2nN*-
zjf2t6OIjKS!}WvFSbC&>q;*OCl9q;KykQ})H_0d3Rb4%tQQ5H;5a~P>e`IRP|Kg<k
z7y0I0<5>F8pZ%*}->_(4)f3YwkD8MBCgE?tK5AnZOs%jL6;<3^#GI5j1Aph?@0!x~
zXCD34o!7UypMG`0?AvBdzVz<$g`1y$?SVDFI{Lr=^Q+H1ckTLF*WCEY>%Y10_mz{U
zzWA9}e)4+d4)4})fOW#-YA9b*Aos_>6~Z5zfS&ZU7m#z|1o(}pa3T4_6X2IjASW;Z
z-Zg=qe*t}=a%W6He`o^wFHJx{I05~~CZK<L0(|=fcGwJu(b-IP(7p*JWsiRWz0S^I
z$Mf*JIDRHOoQIDBUk&;PC0_OCu$9_zy!-*+Ygk1lUaFFVz*n;>HZ1$E>Zfr@e@1t3
z(uic(EA`CR=LX<O{&0n$zZozcJGh*3Hh;2%!|JD9%HJf*mE^3G<11NM;uXD}%V*3X
z<tscx|GswE?+m(w)TtLWG%>^2G8B&)$y9G5Wf;s@z0C+jwnUSuNTPkPH<^qinc)p}
z8sSJnqC=hSgYj4-)Vp~wBJv97T-mi$meqPicOnkzp_XVYnsRrq2?rM%dH(~^?l3b7
z@U#xZQ(U`zzI=|Q{^3+K9!oYajl`1aM5G<(RF7ES4)->*Xhql?bO+W1n^5U1yVfXO
zmnmJ_nwM^gq`DK)q29q@d@xP2-4p2OYR)aeBxF&N3yGzCk!_JdcURD8Zh}w+7t$+V
zn$ovjmQq01Wl+_>`I@!8gSm=iEDzpjlnQV5M3Tv9Z!EN9IN~OMNT$Ml7hg>7u!Njp
zWW*SbB$Dx1?_e~w!`RkDG+X-mj3kjXGovTAJsJzcNs{TI2xayu-eIa%ztK0al|g7c
zoq{~dm(DfR8;vm|67Eg)B4cwhDeyqW!mKf!Owj1l*qega)o<=iM#Sl*jRWzaNMkfP
z5Z#ha#2fp1Q(L1kV|%i(Zy*|uC!&p;)6v0j<3M6Kxg*)QC=nipC+}#KuBXy^_&Ui?
zUfLKRPBkXt$wt&7x+PA?_bww$jYp)aN`1++(GPD%2k@gFVW^Z&M&Q8))lbz-RG(<c
z@!6{Oc7ys{`}+0HHm}j#u!ybldE45Jg$)ZE7H7^EHMC?pES*@g)0Z54F~(y0XO*dd
z#IJ}<biNp|fW~b*{<`PwD5sg$jffj`P=2*3|0iGGifLF8`)DTL!U8g%=+oy!OKF6^
z6Zs@FHIrZq`;4SNd;TUO<8vz&8wpl%m*#lW2_cVK`zC%ya{&wunZs_vqxj4*--MS%
z^1LP!-dt|032&Z9IZSveTOA$~zDyyJcAM}pVCGnF!cWd%8QWySSD5evCVZs{KWxHR
zneZbfyqeol_NWOjr<9!QW)t4dG2&;x32&Yccy_;P^W0ke-6?kF`N+{!@u?u_J-eT<
z9WycGbZqfgK{7V?A^cXI@4y+s14MNC<QR^*UnH2C@bn1|f0kfsy3<EF{7HhX1V6~(
zI|-(yI(>-4w-8M3ae6<8cM(iYc6yY<*AYxjb$Xb?34&=DKfQ^=*ANU}WT(41yqREX
z!qW~8Ur8`E-RUL{2MDGnJ8kFiYJ#b$PU{?QBbb`#G~@841XI(TKJ)I_7;NSR!n3!S
z<Q&@{Z2tbERQboK#-0zqdIB|hqe`DW6*cti`*z(?WO|<5Q+E{EbRci89eGIW&%!E5
zSBSKFW8Lr@Rrnpj?>%)l3*y<o<wVsF)QxgP^|$Nxi=6uZlkE44wBnnB@}9awg7R~9
zhs8;_&Vgcz_p1C8LcrXc1>@{PBICBYgMxmrt_do9yY4{&?x}l3oIFr>T%6of_nbHh
z*L8#P`npZ09wOVIzGskeYP$qZz$l)54+-Q#6Y|t|-x(Xb^`SaJAgpxiKHxn2Ty+l5
zzKwO4`1ak#E8^R?hu7G%&sR6>*>_jnC`$0`b=BF3wkd04R~@ZIkhQT+Z+_dex2Wz(
zSKTG#2&euC$v*IJ@`0t>1Ic6p5_XiQ5Amv+MUj6&)U2$Gk8$lDsyoR!q{=allz;}H
zziR%8$3`$GgTZgBI|bS!<Q;(aaLz|yYUuM<ld-Nb%Xo)!KKulEQS;HOj(PSTQ(B#R
zk+Syw@|2<{OC$f(Md1JK&zfKJ?tSWmp1m)6cK`ZJcgXwXQJOJ&o_w4prk*FCr1|El
zJ0RlC{)Vda_kfr2{`|<7?VDJSr~WV!&+fC8p1m(^w_JUUTAT-lH?KZISz{-w&L1V*
zBLpe>$In_p@7enu&*QJG^k^qMPn}JJ`B5VDeB{TeH?Q{W{hr`|QSy(jsMhTmPO5vr
zd1@zm*wx=Fez(fbv_Bl9_~O~UqP7AAsq-=_Ozk;x603C3kDa*s^oPJwbv`?t6sdEa
zbMKgQt#|L+&K~dHr=20+zPU#y+o9sV`r}B0z6CEeKks?`9V<n=Q^kLQ5kK-;>O5rw
z-`-bzd*60J#@N)Kdv+hwJj;HYeuV<zRaZMVI<Izq&}kg&&)Vz{$51tjoibEv7z61j
z8^IjZzQTT$eQx9u{MoO*%x;hKc^R;CBW316g`HW+nb=vGo|Gv&D)3VsB4si?b40)=
zk7(ywFQdHz3XsEp9vizE=_o2ieRV%la=BXcwDm|Sp4GzrMv-nJH^f2fbUL)_0!$mx
zwR0y=vhCL<>4cw!zenIr7ZHIx3ez)IhkT+x`KPflon%+)9hJ4KtID_AM%l`9F1u(^
z-GwBA_+9ur4qeUT8}ZViUDxg7noP3NlEo_YVeAs*Q5lD*jxcf2#wiZwtkic~f|V!B
ztsR!KO1-_(=2T-cmGJ=n&OrYNQr1?f-(hiA*4}P)SK9X#xhv=2R_v*4x~ard*}8jD
zXQgAXvejAH<gA?ER%vgmtOb2rrH$+RCfLb_qGd^#IL5}b3qNW>*SyP8{6Z1C-?HEG
zb`g8Lh=3m!0snjYdH;Cd0`FVkeG9yAf%h%&z6IX5!21?>-va-*1&(6bM#m;his?8c
zVGiI&PUpv^w5Lah%BMX*GfaDWbf~%bCnY>tpflGWXhVb!b$wx`05jKF&y}z)({p6{
zoV=c@=J$Ldjl8$s9g7pmL5qWu%r)QI5&>uS=&S-(%cU<`iJCDr&#x6An<i70NHWH-
zJf%Zci;YS{sCA6m^P_8xbQ~^o@KoVPB&_sq;<*gh$Hk$_$MS&J$0q5q#Nx2h<BWvA
zQ|geM|9=v79Xr3YetBJKn@n$#>BnXId6|Agrr(q4GctWyrf<u%q(andx=iQEbcswm
zWV%+S`TAf({8Ib;p3Ui4Ds69RXl-bzUzp}l^Umg$hNhN=MGFL;Um6$9DH=s}i7F{G
z|593hQ5V2Bi#XDz&8fuszruOD;kb@GO3TA&<>H^V0ByBR8iC7OHQN`#ZM96g5%&vN
zizeN`ktGjsWbxPFg+)chxb^{tVjK3iEziR7OLo%=)bhmnINd|1kmdkmv8}{*fIzL}
zYY<?W3@4n_^sm57`Z2Pfv%NJ3^|UR3582-SAkfVAIrKx@Uv8yS+X9qk`zxL6wr%Ji
zws+{f*7hYB!uBqm+ik6=nr)2E=a)W0!k9)tQ|Yr%#-?$gwe$<<6gCS79Ho`8gsq4J
zp3+~SAJ~dH&|P{bI;E|I1M5qDq?Jv(3PLxPp2B5%Tbb4iV4(D2G>dJrMhXv?zC}4Y
z2U4YXp;opUZ4fylrOl+lbgd1*XsKrkfEijhfL*2c;@4KIZGp7irE`gAmbM+h&7}tk
zn5%6E!T!?k)BtGE_5wIq`h5Z#IdG`-1OZJPI9&QH0nHqEu=E827HSK?^GNADGG+_c
z?P%!_Nu4Fy4JhXcR`vp^@X1Mcf}+-Th$?AYT8^Ll=#<%BLvz_aOVzd6E+wmco@%SJ
z65B<<mDs2a%c*He?a8}vLRe}|j@_rQQ-J*#fC{3mAQ_dF5M6px3*u81Ll7=1z4LJp
z=%0pxm|hJJ(OA_xD6**ZQ*Tg4B~;w0KLDHGsGp!Np#LZQLerl|n_BdhptQ28pM$Tc
z^ay#9z5?}N`n`~?=}U2H(O*Gtu<AY(S)|W`xMKa6aQqThm4^2el|FkVu{{b&H|i@W
zj_YCkYWm$M&7yDO%v*sjVpX%DWs${E2d~jrz@RtkL*QllDbibi3T0aK`+>0PF6dsQ
z)7=Nf`by-M=+Ki*(&qth)1QOrQoRP4GW}a9pj_V%X_NJJX#WcROXygYx&@)PN?%Vl
z(CeXlwH|`6)#xu(<IWQO5Na@0|5xNr(`UdS)AgS~&J2A&xN7x}LX(;LuOagsy$9##
z>Y5$<SNbnevDx}jXmy@G1NifG+Mu=TAA<Y~^d_8NsDA<>V~)NF%~YrV2<LP4e(dth
z(`V!N1Nt%OHede{jI}`TL%A2}+fciU_3Pkxm+1eA^Lo7vdN$}@Sh`XFAuvt)1<<xx
zzZDuT)DJ?3MfxO2ZqfgQx-8ajfXpTO5!h|1z8o@J^*ivpOurYUT&kaewwLKop!DVX
zwP@QF`d7hwx&9&4ZlxZC<fCW+6!tjNk0DHahv@-`J;wA)VC5&6{u)~AyG(x;rF@U+
zbKpr&GJPhH$C*AE2LC?O>!9&}F}(xG|7QAp==lS<5TO6z7ysA~nI418A2Izc*yse)
zKMR9B1?PmYA2a=f2$nx#`mdqgPnrHMRDGK1+mZJS(}z&qXPJH#6nu{9H=<%cWBM9U
z{x8#i1Ip*&^$_-R=z?N@!StV_gcq1TjE4Lr)4z-Qon-nG;QK$OKLF&5*yn@+e#P_;
zz_!0;dNE3PiRn9me3|KIP|e?<Ab9F2rnjPzPc!|eDE$?b0Aasn`e|7DRi?Kgc?~k5
z#P67XAL{oy(-(pL4AbvKjegJcuONAY>8sEXe_;At6!a$3k3xyJn7#ldyv_8_q2d3?
z^p}vFg=m!bC#KH=!=E7qCin}}_n}q(%JlP5>35hu3MJlU`o~bv7?cNcRMRg;JA7Ev
zzXyNbrRfGNzgyEw0qxQBuR!^aX!;Sv?VB{c2j=;xrk_RWH*5N1(0Z??JE7+-ntlhA
zxK-0F@R-{){X*1lpQb+tPrhB#KM#gGH2n{N_Ty6U?*Ps(Tlxl+sfnUOWe<7K3d~aT
zX~cxGuM=Ri)C{8nWe-!_(k(S{%6Wu<sg|0nkd%FsfLZ_~;86l*S!(E7UD>w?uv?~3
zF=eZ?PoYBh;csde6(X>!wXXp4ZH1ji95Sm*`vK6;X0k$@>ot-U(0&8-8!Brm=bAnT
z6))?lN4>8Esa6IRrs0nlH;r>oFGa=6HogQbg*&YtC#@M0A!Q7WSJpQIt8<cWWM4%j
zn;>&qKj|=iCS0y8`Z4@+k|eTs=8^1ymy``ZbRJ_QRr@%y@5_*gf@Y$*%C@}?LQe8c
zWIvHda+V6Zeg~B1BtJ*?i;9FzzlSO_x(){6xqn6u%~-jnZ2AMF!AFPTgIvj4<jkR5
zGQKeL^e5p{Ww#vxA1ApC*&TT#$G~59pc9tmBp*a}EQdsj`T?Yt-CYfLJ`BrgH-Y8=
z(Q&JZ`aVr0_eEh|5@op`*<V(o*z_-;1D5^UH=s1neG)l8R=J|_X1oQ-We+?60VL4!
zGG%AwvkDrJET2Z5O7}TYR>dDdQ2hofaH71`3Up09s!@I&jkHXwX$5up^`vc)R`YGp
zm)}5uO{@7bVruzL0w!xUbKvjgHxi(0HT#HXlz^#P&5h>)_%H#rTFtq{vy1earPZWR
zSov-O>{^W<0j+!wk=1E6pC{lZQf$6f^9w|T@{bbGq}5EJa&9J|6#|HCF98m%=4CW+
z`7H!^w3<($|CirNK(|)&Dharafc0991wo+vb^<nOHC9sR4gv<Wnj#XgpMYVl#)}wJ
zet>`xt>$^E_MJ3g7}aWSCE#O}bF)@+3rV|+s<2<HSxx2q3*{WtYQ7KZ@{d!_A+1KI
za_**_!&=QS0S5_q5bD6I%0EHCBU;ULcxw5l$OcEXnh%i??;!zCXf^W)_%s2>wVI;@
ze1?D@Xj7=+%2#S95E`Gs-_#MP#{{-bdl{JDE9|tLJnMckO^2qzKPKakGngVnx%X!n
zKK6iAt3!69A`#rvzlN|;?hmLwy$ac#iiAzSij)cd0uhdPg9viAW^?}ynv}0Qg@Kmm
z?nchuZ0>S+cKKD0zyv(^KID8MoBJ9DtnzRNTA$}0LC$xxxl5pOdGr$a6VLqxa(+d*
zw@{5|P@gGJUX0eET+1I7pQsY6n1r0#TD0Ur2$=pl>R>%WxB75d3xI3E-_-q(B{*jO
z86=a>YX5=yaGFl!ug}o%jx+O_^FZ^k_B5=`X|6^7^+Z#J8crJ|#m=#y*(d+Iwg>u=
z2WSUWKJ@t^!rsNGG>^p<x6e{?z6qLFiSFa1PvX6QD3;6eehSB_@M(wO+gv9N^4~*F
zmYbb>E39v;Tt-^GioaPO0sIB3h&F|?DpzQ0f!;_$A11WGI<!Gxwkzzk|D>$hZ=uC1
zT^h;KZo|*rD&ORam8(WTdJOd1my!LbA`v3ayBismfxo~`)D<*~ZsF3NA+qlykB3}g
zA#QF~LHXy;g8$W4P1Al4fPB$TKP5FKo>?UqYTB&nfJCJ(AWCfkelEw~)E_}4S8ggr
z=xHzFhY1oNvNw(=VHci-V^y82Jr8>mH&kR)s&)g2cN4=Y5^~`lBC~71g$wXzvFKbm
z?Q<YGuE?fkLb#|B)tNV|7+L1r1!oj&&k{?nwwTmkj-akrT~C#L3W07`D_~L2x|h(W
zt4cJAJ2uM&M5vt)ce@CGQ-4QweUsFjLK#(~+ND4{R2G{}_smu8)<~|_1N0`9B^q*W
z2lT4CX(OsgEyT&L2gxo)#^zaIo~m0q&<Ut8P+2+EXF+k8sQyf)&!bpXb-Nb>AMYv_
zy@*pC2gOeml`zG;Pr#5>cLfn<sY(!7%t`+MvOg(OHZO_JQ1yv9a33kKgj3B#9H_$|
zFIEb)!Mm&O{T9q7Nhfhq7sx_6q>{cG$f|$45WliLZJa&@#O=BCyirN0=%MqHWaEFQ
zxt8QC<(&TkuH%23^Bu&Ss;^(7!ly+q<D93!RSlup>P+Ls@xZ}-C{wyrejiL+b+k#v
zaEo5f1vNv+mH&_+uF-#Y!7*fACUZfw`1@)`5H~>9wg0AW)2KPwJg(r+*3kSFzK7<m
z;BtQqnZ;-#QPY1!E}M4?x!`Y}g)c~vm0aXph-)H|ycdveY@UtW=1eQjDX5_IDo)xB
zvbCyoHjf6Rs<*~4Kam1;PPGjbA68V;Xy=1XKLP_){q=PS<KFrSWZ$bu*ayBw#f`0}
zqP_;~Hz|j-Ve?-Cca=rE9qq3DR|d^0D8g`yEO2)RnZD41u4kJS*n6;8dvN(hbWe!B
zjN^WG!~ec@;*G~Hy=X5?sjuKT$8BF9nf>Kyu8R$#Yx^nea_AB_ENfXzyD$|tj6AHw
zb`ZbTqN3MPw$)ZNf~3So&QxNf$z{EbR!a5!yo8^Z*l3MZVj~ll*r=ou8;!d4HkxqO
z^YapZUSgvnPy&7%NF>FmMj-wjX_M_ku;@Kp6vgs-8;wKtHaaii=Os3(eu<4{Y$dk;
z!*6rFjpAL2{H127w>`r#^t+HknY9wlbpiesy$u)OCpW?W4^sP7khNQmQzK2&tV=-@
zRD}FQ5iZ&U;`PX46%>0bXpmS)qn>pukUJF*KT$lag2u_kK`aNf&w*mewXlyCplUC9
zgd!$CWlJYP$D)2hTOTK~YQ(~#esZ5i);bHC=I{Dy_gElv*=`Vz%L-h25a_Y7%l08b
z%P*{ksjWKb=i-lBfKHas!xh#e7vYpu(A=kjrjN@nMrqo7QYC+9S-U3%Z&`sHX5i#<
zE%0e5xYEhAddjdK*q>gxg=q}}IFw#FxS45<0(vmLG7YFnKu0MLe9e0}--#450WK8a
z$&{l4PNppu#3#~@Yf+B2M1UvLj>pJ|O9gl)?RWvLthEZ>GbzV30+$JJH0^i}@ljhr
z-ex^;a=XJt5oIO0CiLCrpa;`57dQNYAEX?Q!V9!cA$fG0lWe@V(m^$5OZYFcoeyJf
z+%}WUhNF$_`Y=w~KO+fg9J?7LF0IFU;LHw}7NU|^`<G-s&3W(3<}>#ZnL(3K&UA7+
zV@`6H-Bk`M1CJ((L-YR?jBO7g>ogkHeZS2A9n1($tr>-tTImK%adccSMcdJ+QZz`k
zg)Z$1A`WOxtV30Bv7#YmXG+QUL8seP#>wr(+yrLpfkSDqQ<J;2_0|IqBH3U)a5U|*
zY=>b_q>$QRZ%TDbk|C)@S?yO#u{6JZI!7#+AOnI=3c+WD;4>*Mc$5nkf@~xxAqCN-
z{H;7eqtO3GnNJh)N?{#pe%A2~nK4HpQQMAV64|PdMEQb5cAAi@-j+yiOt3Qd%MvpU
z12eGD5o-}zc9REA-UUb#@LdYV`gIEYd(vUrQTIf#)R=TXoqC^#=Vk)A_jQ6)2=`hc
z(WZiTg%@I1&j9MsnmPC_wH~!^0*W!;S1FRLBZuY_dphC6P$PmO0VTEuFm&Dl_ql@B
z4SRuuiq+cO$|>d6%5y7wW>jlBybQi?t-Q4SvT`r<u-38i^2?DA;qnwSnBpp6A(9fJ
znlweTR+g6&lSax{WF)kLtW<)ygPg!JB_HO}H18DRn0x{APH9AD#XOv#<(5I95?57}
zBVC^l>9l`eZOOscm`hy<SyLLcl{1Byh2_g<hIvU-i%~!eT4WlXV35ud#VWm1vV=1V
zWng_lg)W~-O3%zt6z8&@Qz!!y^{l?Lxh|-L6=k-Tx6CAcXJ^>TedV)Fs+^b2pn`nm
zowX_(tDcP1Vj<N&3l(dv^_7#%3j`|_pe%4<MjBV!ERAY4M`V)*iVM?ba}$!>xfvnl
zvC4cA^URsK`cerWFlT4=nx8>KFM-C@g%(UhgflLmISax2A|Rx93dF@Z2&i_68G$K*
zE}gVtnsl~Cgbmh2zDw4}Jc6R^!Uk>C6nnXFl10V5VaQEd2oD8{Ro>zTI6%uxJNNV@
zL@3n5Rf|jo&?)R_RH|&NAg|VHCOK;FZsWLRv$f_agjrv{0SaGQ%$ldn!XoK1ege#L
zv!oo>73Esb43c~KrHsn0#p%kdjp$;LBU^t0o#w)bvyDozprg0%-KC-bH*;Tf-Leb!
zbk`ne1B+W_U^i5p65lFuipZ5MVCBr^u#vZgt(roOvzniv0&D1mO81#-BDzhdK*eB0
zVrHL01hlZ}_|>$(;4+_vgCZ0D{}Z2ti;VOR4G-ek&VQoJXsmBA9ge6c?!+T~3W+Dl
zjU_GevGOgkbmQh|3Z<*3{_ucd<2VxB5P*mDxX>J7j7jx<-3@)i!(^LIJk;2`CF1Fg
zh4JM2mWBbKVZzL_oDETdpeOa>3mXO)raw3jNBVJ&sdfraLnbrmXdI{TKs@iKPU&$;
z5C|#WSU55=h-XU`LOyw_z-+O3dME;VG&PmL^N@JHG#*a(WdwEhrV`P<Y)PE3H<rp2
zjq$^xX1dm=m{w%5_GuUqQg2wN+O30+j#%|w)*ZX7*F8GAd-Pj(jvl!4ChK*(cH1tD
z?m6&b>&UJ<ud!Wl;J}t^t}L=zcWcLOYyJz&qc_@?TRN+EZ?r6}-hH)gO;M3`rmY4U
z>n!s$Yt8ZRKW&ZKR$FS|i~)DZaJpP={+=$E(c$#^+%Ci0)!}vqy=`8fH?)BfMbO*X
z?Q<K!kTcX1G`gFZCm1ri{Xv(zgXi}Iy<Mw}fO}Ps&l&IroguHki{ZIXdSY~2DrMji
z&LIXC6yomicDY^V$7GwAjx%5ON#5i@JdxU*?&rmLyVg2=UYFqvtm^4>cZGsNIghs^
zWVqZRcc9bT#mkm8;SX~T#{0G!DNfntH$3iO(Ch3ng5B<RZ-*C}6sTV_N3L@Qx~N(O
zm0p(dnXIm+r;eBKh97Bef>*?nq3BQ~iN~Gsyl$4y9b9BY!+3TYKjA2Y(P3;nM~kOl
zQ}pzycq}&|9?PYtQeg%DXe}N<4#i!O{@(PUsMZzThSL{tJ6#*lbl#8`HVUF*-9Dq!
z?eg|?vi?LQ(is_wCwB0PdHkLJkiUBa+SA>wOhsns>2kKAnL~awrPF1!xx3muozB1-
z&gEPS@8J3CydjU_^LF*DH~d{b;UVroz#lM~io|)*+UubU_b^q?;ASHh*`E7|cE*;?
zEqLy^D?PM1l5i#xy*u39-BHm%>zv(OoDxF@*`A1|BHSdo$~(K9z70XQaYc8)kNR}7
zaJ(y?>c|=a5Bi3^v1BA6?B`zR&C|R);Oz?e+-u!Fqut}|T15`qKiIn^$(11^OFfLx
z70kc`$n=~#GlqH-Tk)(lCqknH{k|UR3Dj=_A;<{!w70|mI(mFCaHuEH#dZomF?R>k
zv*t}`Y8i^~WIMb^Jvr{?MFiYISXp#h7E2EWBB)qoC=yErQ;}hzhUnPJJ)G?!?^?y&
z>0j$M_j@iA5486L+&-_WU;Bo3wDc;Thd9yh?+S(jPFP0~<OYjKjNT&PzBWxyn@2Go
zq45<3JgcX>+Z|xg4Q<Rr!{$L_IF&FcT=OC_{wun%G8|rBFcgjTl9@yC!3g?PpP=vZ
zhl~y6Y80O+B&ZGz^L86KwabPqqpQc~3wAr(-8pV3`|M)=*gJY_Uq&NU*SrA7J7Ncq
za^t)HvT%BMFp8(!BY1*e^oFe0^mK96+^#OTLyi*J*3WqbKwo@V^j&O?8J#__YMYw^
zus0-|)8h|uW5L(_Fmj<Fp5L@^UDC_cqy^lh2mL6w+C~RDy04oV!|^1=7>{?AhYLb<
z1tpIuUF(7Y#?2Fm3`Y5jCDa=PvTuaQhQejN;jrlSZutOy@3w5m_T$0FPNy&1ojPc+
zBG2;q*TJ2V+k1xvw{UrXTL=zHk$as#um-8Amt;E>sh53k1M5!@4zevgH1VhZd5x$p
zLGP7r7EZ*6-PdB60ta4EklMk7qX#hS!t{g_7wR3tE-;2cIv}q4yWEUITpZ&e32lp~
zW8plZ9Z&@$ZzPEc6*W-a@Qi3?3S}h38;j)Vre3hYhri9eeOuGR2EBlROOqX%kC*M#
zU8px+*`8AzX(~bm)Xw2T$npEuQaBv%@k>y#_V}PXnm^ECd}G4{JCf)PgM*RSmehc#
zmk-f36HkJ&M4r{t?Q(|Poz76ei-4&@9GorWY@;a#qHz?G;yH0MJBln!V6fX3$+}b_
zrxWikiBfZjH*H9z#6ar8ECyqTm>y)uMP7RhEQRXdLOvurySZKzqq5TtgTFc=iuib4
z#26N}%?!Ki0^X3@=)f$DMU$>*ukfsZn?iNY@Rua+6ibW2G_M;|filul(Cg`1)8${+
zm7{X=;^2;<p$JV*#R$~8If{vPq&G2cnu6vG<6SQ3m{}{Taj1pjl6>h3{0J|aNe}iW
zqDda9%?vWOw$Mz3-e`i+I}zy(3%687Pu%o|GYF#_+>+A&41{UX6{+^77ILnoksto!
zWSgmXWe1F*-VqlDrq$`8VU~%`umdlbj804#uBh=aYr$xe*F64icb8}#1d5Rqp9Np(
z4*2;5i__-CSk~!WkBE%f#45w*?phTRk#bzH&K(bS=CzH8q@CUEW{YMAistuvC5;Rs
zIlWr-skS)AQ}Ol~7AA)?jQ$}pEkZo!GaWwN!{E~8at2%!h1VkJl1w-ETeGXnxd>$j
zX;x14mN=95WCQ==J`|pieFjcW^y3C?uL~_fD-m<d$;xPM%1-And{Eh<tN43DhQC9|
z3vzsicfADFAeKAbq~XlvZtGcvHzc8s)Oko$2WEEhFy4)1ig~IB$k~Dz;&WY;G{M}-
z?F{&6V(xOod#EaM>}Vf=<&#@?VAj^#mx3#+UW2|NCXx(YWDvbP(_1iA;`PW(%nfIo
zpU*7K0gscYp<J|)2xDsb)PdmOSIab`!{-#!yNp-LWgrFtYJYn9o4x(wi^YBW_WGez
zIu=>6B@&AucmV88^bM>KFP5{{Z?V_=7vXQAy&kPX5v9IA9qZ%sW1#Tfr@pQA!-;q*
zLRIFP^0_uQc+S#Iv}1P4)b8>7+`%<swP70A#ZaNVET2owJ;hRX+<T<B0%}Rfo0@8j
zY2h}Qx*@+Cl5m<5xV!;3CeoPLOG{;RGkb`dPNv>_@yFleWa^3TylJzT=VX^Wd@!e1
z6R=n$f&gcl55R-FXp%v?%D`{Hq0Ny)xo#_;pdqZF{~?&-RZra0k~H_?C78Nw=Dkzz
z^~SG%-u@FqxW>!~Z|mX#d#79oAwUUb`Fp8Udt=Q@14Mzzi78GNe*YJ}$qB=;n#v&}
z1mfGXJ54#gUbZRCbU;*irZb_J$oaMy&9hNcxo(UWaOUwdyNHw1>cTUBglU)8xyp}4
zuaQ}!s~t5KqnS9Vgju8ri>4Tz-{Z5REOi65j}XWE!+OzMR|qfR?>&<}M6O;jWA4Cw
z%<N>Q_}pxopyQ=wG~vQDkt+q=-Rlu*u|zfxD{=|y3h<>2r<4k*@QvY!cA~mAc*!4n
z!+4JwB@0EeNF*&HS<D594GM3UY08Q10as6Zh)ECV=uPnHcoNepCtnF9BB^vDhL^CV
z<=ojkTMf9ye5Bn;(?o1@(K5$~_xGb)<v5MW6pK)I9~O*qW-3MvIn~U1L}tR<MyqUJ
zQ1s^9J-AL()ifMQ$82sAo=NKqGBvJ7^NL_@=8DAF&dd~<n<P7oWH%;+t@1Vjy8PUA
zWJ9P;9c%))13^SW<-s&w2Hl+)uQ6g~CyPqAh318e9P_DJ6+4vd7{tCVEvS^w&0oL^
z6@A9&2>3fq3lp&h%QjmyiA^qS9Wg$uP|X@l^`_EE*|LRfwj@7@O5HHq^w5L^<Dko*
z9faxac02-c&2q<sf}4;$An^SN)39agh1hJ#EUQ%T5R=8io0@z})Q)&f(-jO2Tn{m8
zGR^!=4WI&kW~XxJxD8i^H$ZOTGR+psdTy(-t(;-0F;h)5GFt@H73ghwsc2tzp~II0
z2KMst)=BJ>V}BeQIYyK4Cpb~=W)ImQJD=w}k$go&c<zUMOR?P_2=sKT{T}W|d0wnm
zx@43b10(9J^7-2kFFUdQ;#}o6FjegC3Gq_U=R$!VzDoB7vG>gPA-Se}Ig05#-)=Uw
zALbZ-C|qD?TN%E2As=exo~?8*EUmFc<?LCHxrsBd0i&0cS-1t7DRyKv%Iu7SFSB~!
zYG#K))0-iAj?25&Xxm`$6$=HT>{2{yR#?K{!JP^FQqoFB66;JaCZ+Vg$3j8Jw8?GS
z(Wd=>dWRx5TO%X1KS^Da8qtY`zbHa2Q)y9*@jM~cyTUTrc&H3Ryvp(TQm|XhYt{6N
zX8Tk*MPv>_nWmt{QYfyrrO+0cl_1wmZTZkFitn${eyKs%5O_B2MPU%YXu2AkaBy~7
zi1XdcEs6N{Y$(e0RrmUKx7&r`8>>ON4CMVSjTf43iABP^<6#dB+xmP779R|o1`_oq
z)65*~J>H;-^#k{0b5o65?>8^Z)Em=Wn1R30QodwUu25WjZ>c}hn~Y*;!~XX8wqGn}
zx29Ild!MK<L?*}^YRZu#EE{KN(^L2*RHF0DV97GZ>Ko`y6l&UKyk%)WR`sx)t5%(u
zBP7+8q`bj&5tW4*9p9JZy9RKO-Vw2lA-DQ+=4O025Z51QQ3lJXR?0Y9!ME1-M-#Xx
z7EEux1`D82T#QX}GBrLZM=1dPGcW0?%fyYj7hI{4WOU108}bW+-19r!*lSfAyqI~>
zyppev<xW!p=V+k~1q>Lp2gNu3shrG;IJ+K(?fJxnW#b4hQu&JN|CwE4v%RyvW8RE1
zsQ<{Tepht++g8&wg~tqvx_`s@GJA$lN}T5q%ef4#B*YwxE1H>qQwip&GgY|Y+(lJM
zjO7{Era_%}+MQiu)RGre-9}~;q;NK_W?(9oS+$rABvqz8b`f9PVU`+*=62@-*I@hj
zPA=`;Qjt6+2Ya|7L;lc)ZfsL#odaWzT4`hJlIX=+Lro75m|gB5md)5d;xl(_Vy6*P
zWT46HKb!iInl&!YyLgAbZFaS6^S3HDg2&-hEFIO0o*70L8ov4m<Gq-j^J{qv84q3z
z8x`5*A10w*Ty4UpOeP3M`>|e0B+y|oYk{D2!sx^MqlaP)Qy048j9D06<aCQGP&w+!
zS}YN}xzHW*&3pSaB2ZU?mpI!!a=SS@8mlWk0k=QkatE+k&#$kkX|kHslCv*qp`D_1
zj7D6n4KN7M_2SI7zua6aJg5fzXcW_4ow{}M6Y@68B8EFX?^V#JC01i0b@~-~v!EbU
z(AOnaV?FBhhdG4m9>d?|U={RMmR+5m%^}2XG&zS-`_Zi0C%50LpvN+5_o&meIfM$T
z&6cZ9+q30DqJlPN3DxOP4xxgs%@V59>v9Mcv@=VnP9Mu5RM2;_gzD5}TIi@uYqA)y
zIpzYOZp2l*Wh`@-?Ks+ND;%8t2AS@Wsk+JZDXEewF~2NzmrGposoSt8qGt`s9Ya`D
zBTXelI-t%FCnJer))2!+Vgp{iU7y0vwSaIhQUia3czra?8VDNbO%AYz@QxVdi4-F?
z&)J5H_0*@$fPuUO27h9ZXT!M9qJhtk8d9|F-++h_MP8i0`n(}BVDu+?ha$#67`zIC
z(7rw+GSU|rP8ob}K*%t9>Ba^jL!DpK2S$|FI~45$UmQi?r-7F%%D~&zSwmkOmuxUX
zG=w9Y(_6sY8{2|unFOP;{<u2t-Ml#w*`^@chE?aVpgP5*R-H;o`Azl@!+6|??%twX
zwfNC$ambV>neW61NYJWtO6TAYw-9IeLly@h`W_2Um|Y~9U)<@Hb2I}o^QSBho}ZHZ
zbsWn6<nz0b^)}w|qx3&$ad2Y(Kn2di9Ks#KGR4ipIY$>D+#i_HpIIFuAGfE`Q7cp2
z8_n&c_;D*CKi1-xNL2aiJxaC2fJ41ENkHT6M|VQgJBt**dQZ`J@MHlUZ6e9&ujo{|
z75VfIBn7MYAlY#DBOSvc;qR#>J(d0v&91`<*;MhX_b!b{{w7J7Z$DN4J$d}<Jx+%u
z|0p>Z4&tATKUKcsza2Otk><+0_i8>7<G`KaIS1)#KIpD-Gk^DF2hZ-7{QRA`CI;M;
z&JUH|1<1^=V&0|-L1)P;Q1OXe#y*+Huin!Y#^W4x$YvA(e)Gh!L%>n_%KqxRCc~0{
z7<uMH*-zmP<HXFbzGtMqbF^F$$~3?HZ{+dYYaE=?jt8;mSWd!CM}GN7k!LQSUJp#i
zM%<uF2i?kSKJvujEhV_Q9)Gf%W!`J1zMGV9@BH##0B@CvU%mJ2IXqoOM?QT%{0eAD
zCsjVZ;hK(<e<%Ozpe5f`{OY}5>bq9z`&G)nRr#u3XTfVOUwyyfEFLMPL(vN;3oHCP
z$T0J(_pUL!;5eulWvcj8S^{P>|3UjEPI=HS7?hp@%9gL-$zUc~s{Rj3{=;@bq58R^
z&zDy%%WqNyGFAK+=iy0LI#ha&<QLfN?@#RF+>u4`GsUOS3nt*7a)F?jn-_rc`4>;X
z-+Gzg-<{Xt^7-2*;Q!W2!GEs`3{v5Ie!t|;x8Id*f?=9nGUo9sIaf|letfg!e~<Db
z6Y%c|3;yBU;xp~0&=lsB?<gFlZ%Y28QdDN<fm~jmik$LPTrjMcjy@OQGW;pOSAI$H
zxls9z5y9X49_=Tn3jfkqVOlMYahX;`do@ooXIi0&n*ZeDi!$$P%*A6=wK~iwer`oX
zQu7dVrWG?SHNVKk+wc}ShZ)7str)%3e88M(Wo4Q7HRj^Se>aQrtr+>$xSUI0k$Gk`
z7eD@cT0&0tyI;BVdggn6x%g_P#<N^}4aO&j8O6`7_%hyc8UM|hRyLKXaU~Z&4dcWn
zGm4*E@r4~V4wy5oYz9;DJr@selyNf`KmNODly5bCM=h70X7l3@>$q9Gps_MJ$mOwE
z*?D;wMK1@Xyj&}@=V2861ry*e9EYcVUBrGrmY+dmy9M8AQ}Z%{>6n8*4R$yx?WX!Q
z_3=V-T%3M<dtCuM$#2pfILjXWIOXN$;de-U{(E5W0G{-DMCwEJq2n|7(+c>{cY!DQ
zNAu)A#p%cU;Y9U1!|AKp`aFMrm*dCR7wCfaDTBfq8}Em9jvxO$ze^<kP@dl|;}xGW
zuD`67`27Aw--FZ&^p}3%3w;;xTHtG_5_t!`N|V0lm;Zgfhb2CL&*HHO<osp=d>IBA
zdtR9Y&YS>W4}2j#S8@FK_|`cA{on-hZw9{5cM$KMfc`7M7y6DPeJ`nyp3hEz{{zQY
zu>9{VTCIiU(7PK8$*1p17Q$Z#JbhR4uzgdGwQrh${!vMPJdgg!1oY2NfTwjH*(bl;
zQs7BXM};FtpNk|u2MM4J_(I>gT+8Xle{b?CjxR87=3eia;KHuDgymZ*Z|HHez68G2
zn8ufK`fyo)bvthSr=4ztWxpP2q=t+>T+PHrl=^I>35Cz+#f>e4@y)oAF^qeNl16WO
zgkd8K4=bi3;f5v4meS2S1&H|9_{0Vo9`Q-x(Iv6T7ETWh?Es4j;x`)PkQ%sE#M$Y_
z9jY$+1}4{Bp37_6{*De@(xCgv@mLBd@Da=mbvPL}2I$E{+|iUPJDiKaeGa%jXSg<W
zIXk`W%>1Rm>;*y74QTImqptkCV)k2vvVg3xzkPKv=fX(lp~ajBN&cSCgPLxzb2!uo
z4e29>@(YRiAA!sg(N_=u_SZ#oKdwk0B2`|K`x(-l&n{-`pScD;(Kj_S_qdpB$+vC`
z+vXqmCg^)yb<gUk+DLrf)BIt~EKQ~!?{z;Vf4t3fSxgDfuYX411Rn;?e5Z5#2SFVU
a)AuWLFW;kO<tHx1<2$)6Ep7<m*8P8{MImGW

diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/index.html b/3rdparty/qpOASES/qpOASES-3.0/bin/index.html
deleted file mode 100644
index eb319306ec7..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/bin/index.html
+++ /dev/null
@@ -1,7 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/bin</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/bin</h2>
- <ul>
-  <li><a href="../">..</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/bin/qrecipe b/3rdparty/qpOASES/qpOASES-3.0/bin/qrecipe
deleted file mode 100755
index 49be1d1d5539fadb962463eb7fdf0024e54f7b21..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 57832
zcmeHw3t&{$we~(UNth4_ho|AC0|J6dUIYjVGLvMI8Oe*xB!rhIlVp;NB$GHZfnX6M
zP-{e6wXM?DE3~)vTC1(S*ILw8;}dIbi?+3DU({aPV68>1iq^{it^GK2W|CCAw)ghl
zp9Aacz1LoA?X}ll`#I;FT<)nXw`m$vmyKP(D79_6i=)JyG4COsl(=G+$y{s!JCkJq
zlMZn4912BDg~z366*?rH9bp=v*d`FgHW!bmaF#$b6)FuWr1BD%OGZWb+UX*xLdKjN
zL42g^6N`BgVH1z2@S4>EU+fd1;v@BlZ%FbDNj??oQhyby`cd5o0x~~fb0I}(7X_@0
z-xDFx(pCFXyp`gIWyKHAauFJ75mxdPV=7d9Yr#i)p6sPd!a7;rXm&}ok6=*kkk=V*
zURIRX*}Awh9O>;_+*iD8@v@@aSa)tAuQ$mj+KTFW&Zz8|gB0mJ1wcMP`JcPN|MRLp
zE`4F}`Ay#`2)_2&OTV*w#eC#V1rROSbo{I-UrN&gX<yw{rn4zyzseRm@^hSr&#=vL
zr1wv;*@%{|X#gsNuIVCRvk^LFd=A1h0rLP00A~TzwMa(JL3l18S7ji~0~7!X0ZRaM
zEd><wn7FM#a6VwA#C@z`R(t1|`?|*8V`G<n^Qx@#p1d#b<sbhk`*_u?_U~OW_L;lC
zY%6}@2fw{#>E5fdYEG;ffBf7lcWlWlefDEZelTv_c{gQEf8_Z+i)LRlYuuC<Z}B(m
z+Hr7s&gtLYbklwJ|I)j)&$s2CEth|O$N6=$+F3{XSDwG5an}96y<x|lYkoUe*!=T9
z+JC)feBUpAfBh5wAB_7w<d%AV`icuOKHayTeRunMcJH;XWlJ?j3~ol_Od26)GH}ij
z@N-AOJ4cZd83q5%QSfg<{z&@#22M5-e%UDc)Qy6FViY+uN5Ow#6#Rlw^#36`z({&t
zIST$8qtv%^6nw`h<?bE@AA$aJ*?877!v%?=k5M=Tede$U?8r<PC7Bv0{)~<|5k1Pe
zx{2ct$U*p+#Ipi{e-8X~RdG2pSW{OMPcw<X9`ut~HcON8MJ8kW5Fvf~r9R_i#$xC{
zhs|I|x|=wrQQ`}vd{<Q9528MF9pv)Iv+JaskU+Daay@4-XS|7Hl%D@7^&e~#_<pJ9
zO%mUKvB3XC;=cyG4m%9y3p^Kw+gp<Uh{UUT;7;J3BhY^w`jVZUa$KkS({(%QI}!C|
zQcor46Oc0r_F>X)QXTdKRc=cYu`{J-SlXva+O3%9GS(=|JtpNSpZunzcl9)JN;O~m
zrT-k5?2-~h@Bqh;XZceEzC!wg8}di;tMO8fD@D$e!0V`QK>3L*<ynbm+nOXs1UK{g
z!fsN3B>DACX&<$2k!bd^w9imVyF3p9@~Z<W_IX~?50*7?5eI3a!u4y3cZHjH+J$=3
z^$6E<`Y`>Uko5iQn>gL3u{>bUDEtb6SN&*@#2;CbjDHyTNi2)?OmYbU>W=yMlAap3
z-{YrAcS&wOGmQ4G?uZeK2cvPrV8(@8jk-{KI2I2@OFM(HSSZE}U!clp4Min7P*vL5
z9SH@3&7C2UH*(H~>gBSmf<k{!Fd7S01>@0hUqMkzG!%@7%EG~RU!<+uvy~YM+GT>M
zEMK74C>h0TYopyLv8!NddnjJk8|?IVclO4^-4Rc1D+B}riQ)?U+q=p_5niCjDm+<{
z(gII7TpRWm7+gYvKS|xv!jY+zq6J*hfZt@78A$~z;zB{(Fe0&8*A=R+B?Ze-+1hBh
zE48|&0MjmILXqJaMd3&|?y0Tz8wE>Yn63~Sp+#8PQ@f^>GL*{xBE$S&7Z!F+=vq*q
zjEYvI>IJNMDK@vZM<Mb|4@k0$sXcECQnA1rip9dgNML(UD7B2y<rL(@PNlW=@o-m2
znAm3;LRF5z`D5|cmUGS_-(LpLZ|MsfJ-yM8u{9ix!=q58NU#g{)?h6;CF)m@Ph@?;
z=J3{ng5`))pD^N3&Ipt5Qvd1eGkQYNSa&4Y8IEr^wxUyQh-~9>Iw1$ewzsqxF)Foy
z8TFBE;YcewS**7!M42s0owlB6I1+C&S~|8cDB9f{hjb#4y|XJAjxZzC8jJ^#(F{F!
zuLUa7%JO<+QCeQ-1>;b0adR*h64CSWI=Z_;dEr<`xV<;po!1hKZwW_?ZLz$Tj&N&t
zG@RGm8}4k)>xlNmw#V|8L|f5U<J<FO-&Nsa3>G9mc3xh0PdqQ$9m_*C!tLFJ{NOTL
zLt8_gM9O-bRjpcLy+#|HlX;DcTQHK;pgyfotv41zk2a{Ds)oW0#6Zr>+(tNx8@M`~
z@!yU3Ry3cN&|VphL@q^HZq|-WidU4D8Z_XQHa5CTd`3a;5>`>^D?t+IA`v$Va|?5q
znsLJ860@cqI}Z%2Nf(B3dD*ew)8s$e!)ScJsW2)`<2hoFlP;-L9!(92IK@P|m(LZ9
zIgzeiusw@*mhVYLH0bs|)T8xQKH$gS*n$}~ou%PUxcH(QuoSSf338q#`WvJ?WV7BG
z;Y`{s%~bT$DQ#nCN&4e?O+?1`vcd{HX|B(RY`Q@4yIMC8UhRu0L(P|jr~Qw*j>vhG
z@M_Oad1^gDc-mK~ORd`oPx~|TlItGA)Ba3dIdc9U1B9G<d6v_Hx7KT+1#fNZd<&k+
zP*<@9Z{0_`EO;tgU0w@bt<fm2)`CX^n^&U+KhDH5)?~qBb!T237Ch}e)zxFcTQ_ih
z7W@Q7jIiH=CqGixRTlh2g+zFr1+UhIl)cY_Ki5Kkz=EG_!QW=V%Ow=g_3nPf;oX~l
z*=#5Ceta;VK15F8-TkQJ5y5<7X%tx}=ErfLaE1#pN_SA^(IY2t&A*t^<N`+z^K>(%
zsp*dn^7IBuQ+Gak7f;twnq1)M0iM2)($w@vujA<wN>h^`?dR$9C{0a$w1=k)DNRj$
zw27zBrZoB%J6g-rb16-$yQ40io<V79`lI<gJ&DrP<VT%6J(kkc)JJumwo#gz@+jl!
zcV{6@O?dR!`#{Wpozm2FN00FI%ao=jJ9?O>U!*iO)zLwuVcSJbPWJi)J^gto{rZH7
z>5I;Stq0P_wh($^`q@ZQcwY0#hmW*PIAafrV9dSlL0Yo98{GBoK)v64#Vc2#n4w-2
z{a_zr-hoGkIw(O@-hs!4HmV%oz@tOel<-1~4><*>UFF_YFK!_99`YV?73X6!bjanR
za=d$&K8=jZYcFwl2g>F--S^T?%zcp7-<1Q!zP%Mg_uUP;%7M4N14WwmP})4&!re=_
z2l_A=4V9oQ?_RJz=puQCSYuBU0$1@}m+;WB59y&hK;^!dR{QRQM_M3&7VKQ|0~?8L
z;1CtRaOly$LfL`0hc+pxRPbJx<3w$rcW+?cLZ2Wk_U^TL2Yv?ZgS6u!-uxjiczgqo
zA9B&aj+&+CQI+>j%|}pmZG-bc!b8|l<v?Jb*EcXF12i)Q%`BoB*qP%#RPLa<Asj~`
zkt42C+&2)&aQY5q^kEO>J5;{#xd;!P3lR=|ABubT?wFTP7KoyP<m!i93vnlFID2BK
z??#Suh=9v~fEO~*m*c&5$GoOPE?Ll_LuK<e4WV}pErHyj({c0d9YZvmDhJ;2?v2m$
zy6>eujQe0eb~yLZaRHpIY@W+&_YQdncFfbcKriIv2o=iaIq2|!T97v9Ll?gX!H?l4
z2nT+}`MpEC5~x>sK71VJ+FLnqlXvf`bK&&&EySkq0U8=!pRg$~&&AIK+9oV~c<7$L
zkht&6>%lm{4Zv&6>u6S}naZuALa05dDo({>t&*$_HS^QnYXc~pS1d43=au~%6}uiv
z483B}q;lYQvTk2V!oE86JET#)O=k6~3{<blQoT)y>OC^_vv*INXq&KZuwd{yaE*K9
zb@zl#;~w?y9@M<6s2$rT1jhduReybg@dWJgQ1A1w*t-`&<P%CHx7E-mO+l)^Ltf=$
zj07SO5+}gIB=HKX#GgUpZ6tADvc#2;NbcwK?j3WYHix3HEuxqOcFZgGe*6UHfJ2ff
zUnCDA>D!xu9tJYFSLMJ7M5;di$GD?%;7>!JLw422|C18GAG(HEDaVVP%X#AGl=$^f
zloGPdhvxGHJb7p)#jbdiMy-Oujho$@+?(AOxs69egAX8_+SuS^G{962T;QeQ1^w|*
zIUk&c{^cETL-@xn!fD8RjPTS`hVGXXEkrT!$RCc1DuX5{b9~4t_@i&&>5IGrFM4<X
z=ds#=@6aF}?0647MCVrCLx<>CWyl4FKey$orkHR>8P2y1bn<%d;zk7C-N&=N121i}
zZGPk+?@O3a|GfD@$~tjm!WsR9J4h)-|Il$e=)D6EdmnmvwO2drefoGWnC}zJ&w$xG
z@VMZAQS$e%`ptSLmX{Oj!8x=OL-yv!(|-t*_QoSLKYMqt+BX>l@zYHerqn-pWHGcG
zc<IF9%||~5mI-IDqcIVB4~?Nw9=v;~RMeHHkCq)LG8g6-_rM8vgKyw1cfD`mr|v-I
zwey`Qpx%4!;)MwPm5W{~___C?zuIXfF?8qKuriwebS1BH;N{AJx5^;n#FSrncR!+e
zSNx&(Wm;fZ+PW=a)Hfc14zwmoEJ3(l>PG8qx^P6XHPq^CrLzcUk2t?@h9l1Tt<EqG
zSb}tX5j)p8KX*x+GZt#;j<m*dX;C2gV*J$PLom{@h%*$8c1O>1&R>yR7;-unc7@wJ
z;?78S+}Rv*_C~^&^oAD6V<jd|7Lqu4)-g<lbgA>c;d^PG{eib5a8J6_d9gaDRp)vC
z?cp`w>YM$IsgJ^|=(?OMhnqIjetsoFbZ<8J=7|&gC<9J%3_%mZg&4g44TF3K(g!em
z4I;b?;d2NF5%wcLAM-W+9zfR=5isr2I;QpM+RSld9M@@Mbi&U9EQhSKB@G25(~IAm
zh~5dETqHYNFVD`oa6;BLM?YIV<NULi%$uwD(aqU&kV|z?7s=fOI1Kzag+xgBIKc7N
zi4*HAcq)r7?b6yxuK5@{9pT!A{`egddGi4I;G?tKw7XAtv65{4Q?`oioKM<IvYpqa
zm1HlxCf%K#e`SU{yLk7QitNFO_IuLCW*3)c=ex5Pf~F)p2RtR&4!7FtknU-yUorFy
zAueQkNT!EmdPt_1WO_+vRko`$yBKAp$m8{@gq$YG5j~KEiR;7(ZN;<|jD1y0e?F7_
zNQ3%sYw7<!mYvW(3EFGZp4RRh%YK!9vwiPac28zH5`&p*7`tuk8A`;d@RS8kS>Tif
zPFdiT1x{Julm$*%;FJYUS>TifPFdiT1^!oBz%|3g^*)SEK3z_k<_X;7amKKaesiWv
z<<oD})-?UbOqcpi@#iwF&LGrtN$R&t^<3X{!Dc>ZJyWK28P1U5b2uraOPyD!Gmkgl
zKhaJ2eKr>b410Wj9n5s`kbcGBSDK3l>Ktr|Bvj`SIf9BmODv;2kk!BeNE1)1GltZ2
zAN8B>X_EOi-Nj=HKQuw4mEMCqm*M$%ajEigq{QpfA?a~u#nVcUV>10pwo7vU_XO%W
z_SDk8GD+xhj|?A|;j=RQoebZSVTSa(X);_O!(}oomtlhp+hn*^hF8k)Mj3uZhWE(u
zaT%uS`9bNMWW&~e5aFr(Qx^CL7I2C4=(2y7P*vrhMW6rcdMJB~aGi^X{iPxttQR3$
zB}1NzTTx1SrA$lw;98fm8|QE}iTwU@8D1#EH6k3W6Cv9qLnbbl!tWG#SBnfU5+U=+
z@B)eV%XF%Kg%Z!?g<mQ8HC$h%_eTx6c(dNJo-S!$RxQd`^&U*A_h7BaXCG8=segZo
zq|cQhlUHimDf9nH>h0Q)tgotf{67R<wRitUNvGQTgX+B^SuXo;s<+uMq<+KFOkOJg
zWYdc7qlX{WuND7C*W{Fh|N9o0=Mr_CBE!_QvVg*?G?VlyJuF{k{F8=C-w#`FMX#!*
z<f!yNd#LnP^+-)C{S{uN6}?KQ<}3WE@RSAqsVy)lwfnDzs&a~-z4z7Eo~deXTK(a|
z+EM9M0aD(BA1<sNm0rpHdxamazREtQ>YZvg)gB+NottDk|M#``=RZf+y{rFTp{lo(
z_u$_vRC515;jssWoMWpyGv7JTeolkysmr|&Ue>PmiTSQJe#!mIV53XW2V}TVhI3_@
zkhkUsr+1{M1zewaKQp)Chl+2#i`!xPQc1r)Iqcu$TEef*!{qffKfCvfchtG=SmDaA
zcq5?n8f4-U^!*Kz-!9T@;h?bl;z2={&~M+r9{N+)Q-0UmPu;+l?ro6#9?7TnYf`^K
zWskWsU#$-l@(!NA<=dOT?05ak#9P1n(B<mfV$mSu*Rj<+a4I{<!*EtO$j$q^rHamM
zkIYB^@NWA30oOh4H>Tb8T`A9{_T`IQTppA9^-H<^UV-l~k@*Sz{`$~h>`%7_Tz@L|
zR78EMJ_`pKzdpQvzsN26S=i}!y}R?`@5HZ_=N^OV{6qOgs{Pgqz53<(154;v)G@UG
z$g~F6CBIu2>%UIw=Q2b)e?`%!_=~ey$Wx(3-ioKcUh?Fv4KCaMGs+#k8(cqlimn_%
z&t&LQc2nm^x}+N?=@R;#>K_a*#hvOOumY|R?!Tw{2ifV={6SV=r{<4AZU@Ww{Qp`1
zQ1g0fDAol`&c7caJb3!Y|FHJp0}s~m$aAusBdWZ=TRE~FQ{|<W^FN(d`i-W?X!sA}
z`$y$}IJx~5V%^v_NA#=y2HC%SGMtwjy4H!bOVOGBChYK`<w-fNdRfj#lsBmKQlVOR
z9T~+gsrb~i(kB(4njS_sN_k2jMSm*I`1KL(@uB_hlphPbpYmhre`5Sf%$xr+=~I53
zGH!mLf68_`SvqsPRqgxF8YbJ}lwTy<gPm-9e-NGM{n-f41kn4q6#y@w9?$?-2WSM)
z^SLKOXFEwcdhYaO<fWY?9X)n?GV(G`l5WgN($RCbCsQsxk9#t7Amh(E&qufb-~p5a
zY5=u>wSYPRwZ{a&2hmY`oD3bc$H~x9dz=g%wa3ZOQG1*W9ks{F&{2Dw3>~${$<Tp}
zfB)<(gbM+sfHHs&a3Np~pb|jsp#wgMj@sj7=%_tThK}0fWay|pPKJ)!<7DWlJx+#>
z+T&#Cs69@GZY=1s0OJ7anSZi9*_CWY_Mo~_JxE6?pJb=<<pGxuC;$`!mH>(XO99IO
z%K^mzyynlpy?q`+qCX!%JSzdjw+cYK7XXN#25OSw0+1XxfMk^basecBG2kC5m+DGm
zO$ERUNFB@91GfP{cBDSw0gN=p_<^VIG?1;SZASC`D&$oI&IFL$c>q<$e8fq{0)Uc7
zeT3+#KPldG5hoj;10Wlp4ImpY0+5a80?5X50Ayn)zy&x1KsG)dKsKHYAUn?jke&Zg
za{o8lc&4<+41{Fo=>W2G4uI@@8i4FP4M29D3UC6Z0Ladh0c7V%0J8H$0NME;CHH@;
zjmf@bTe2(Jl<Y~iBs-D~$$n%zvfIhh{%^Des>d(Vo$5}$Lw%mcP3qe;=F|c<09pYN
zKp$Wi;2OY<fCGTr0e1r)06YPB2Jj2O?*PXD?*izXFWG<`z+Aw&fW?4ZKpr3;Pyi?d
zECCb&mI9UmmII0bD*)#KE&!|sxB&D$@e)8OpbX#vlmjXNUVsm<22cg40jvf10rh}&
zfc1clfXx5{5CpUULV$Ka7_bG<1?UD`0*C>60ha>?0G|eY7O)TSdBDE__5;2I_%h%t
zfZG7K1MUEP9dH-m-vM_6?g7yE)$Rj)2SA^w8w5NIcogtBfZjWM67Uq@hk)k*F9Kcx
zyb5>?@CM*5z;VD|0DlFX0BAPMy?}JU7(gZ<3osrq0Wc9T888)a8elqLCSW$;48R<~
znSl9#vjFsM{qq4HKn37JKqa6Cuoggn)1)5I09XfT1gr;a0Bi(o0&E6c1TX+ifFPh5
z&;n=$gaB=Tb^v|7lfGAfF(3}u3AhSyHQ-vnrvNtqJ_AtibHU{-*F2X<nCEyjMd4EE
z|LOdH7Ck=54yW||ulS7`U+01u-I1^BXuhHK7+jXG-)Ih^bs4RZ#{+0yqO}>Vm$Lyh
zS1kb0_5YhZvVjAj*10rSjRa&PTJJakWJ{XMMgm%M(q5qmPz<2C?qt9Px-x(lPzj*9
z@f4i0!H4w$e4qY2=fe8t-blRHS(ICxTeP^am!}GL78K>?7v(NlB=G!`el$dg4v_e~
zB6SD4`e(R!NL_<2^?Lt0ksS8tS0vgDc4ADT?gjbU4<RJ&77R9iz!6cZNoaGV^<q72
zuf%3O<0t0>Z*z=k#3*Ic98KtWHrtq8SS#Do#%$)ujAo8ZpM;K@mX?mcb^wNS11b9p
zSbLAz-3bx)E$FUe_D~eka>1DH$Z+hVq?Qqe0Q(w@la72^V2-&G+0SLZc{UVt&{qjE
z-wFZE9Lu0~=3j20sN*Uu-!tE%xbFBjXqx#~isv}SqehwUQ{3tJD&%IKp!mYfPmmm@
zQ6fKcCzQ$5c%nG-GZ?5dZ9L)1^bmU*Pk1vwfn$rzbe^cqbY>ut!4r*{g&4du9U3iT
znlgV2Yh{krXzkLG*?{)S9H)`OJ((|4j?NSD%s6V5IZ2Blr!Vt3<xJCjNc3l3gDptr
zY1%p@c4dAO_skqE0%^N5Clk*sjW2^T`zSGA+X;f}GA9sEu67*~`!c6dB9A8yWX`5U
zK2O}1Igb(rJaJd%Ig}{W^1*X3^Y=J&$SmTz4QBR|L6&KIQ0Sv<?9-&e=f}*0xpOiP
zP$eBzn29pKNKxk4O=kTPRoCITJ00<_P;GUV;h^PWhJ)HLi<*|y9yfppVW~Aamey_@
zI~mx|BQc(6$CHfgqp0H8E4M(;3HPJKw6Qll0RsIuKrp=)9-^@c?;%JV`yhP_Qul+1
z=}oALrY}d-re6Xdx9k5p2{ihTfKS)2AU|dkf{>ABcYP1dsHdU2m+SNgjo>|)k2U=U
zklOV7fw1c{AURDR3%5wuccQQi{inc;(cc5cp)Vy5(SHxjSiJ;=W$BfWHcnp-)yL~E
z!|k*6xsW+Q&m|4@Jy2z$z8390NuR7^`K>R7LR0i8a;NHB;l|VS4Ulu1elxgo^b%+?
zUEd3tGjtvCnfh&r&(dE+Uz)8SfDWhY&!F08=v~m?sXqhxbM!M1pR3P;ZfEM#&_46@
za;z=p>+eGH0{zRlpQYPSmxcN_VSq*Y!zlM`{d!=|(XWG6=jzWPzF1!kJ#+PUP;Q?7
zDlqx_J#gp(U4w)|{R_}xiT>~Kiz59ja4pq;g8DAgx4>@8^)EnXvHmLVEA+)E<vg9X
zGw16|;Ug>c7<68xFM?JV=*v-~)%p-54>JAJ5cd$%{|u`<%=G;@2783*Ujp5uOuq>|
z@G+)OgC>tN{UM|dG5u-CeuC-m!d%~D`W$HdB-4Kd<oB6Qe?;mj_ys6`z)#B951D>H
zWc~=kkaw8rZ@^$rGyM*bJj3)LhO!@{SZMbXruRYBpE7+aG<%lmVN~~LOwWRX&!GfV
z?0Kd?4(q-EK2ZLg=^G*J7fk;;ME{cMI}!Yf={um=ubJMC8XaN!a+v5hOg{+Zi%joD
z#eU25Hq_`p;T<U9C8o~;$?uq+2juUW{s=sE2zo-Lqo73TFEjm%Q0Wg${{<}l3e!sv
z{E_Lip~S09{~7A{C#GKwDaV-pS=8t?rr(X=b*A47k~f&1kAnWp^rxW2n@sOQ32!m|
z5Ss99rWYc0oawit(cfYEjbM0}=_^s*Uzq*?>hK=ZmqM4n!Y)X?&-CLk!3n0%1hQY#
zr^E5D(Dcv3pLc2c+h~W~n*M9F{2om&h4LTQ^r=wrN=-i>=J|xCm%wIMY5Jdm9MJSK
z=y|oK-wIv!YWka~*fp9y7s0if9zj8$)by93;HNZw0IBOVHnAA|3$vEr1Z5`mqC#1B
z_kn=fCcO@VtZz}mVVjhV3S@mdgbdv_X^?UbQeuj2(nbVX_fjIqHmRKi+((I7wn-Vp
zb3Y}Vw#igXR)yw*pOgcp?4&{j_Cl=zn4rQ=B@UTYtz8224l`>H=UPs(>a=TsCYR>2
zrf{xlYf<s6dME64HAuB@Ap1T=GL>^r`z|b;wP_EOqd`x52HC$fNraR^G+tIq1+qEG
zo5-ex^pxu$bLx$x!?Z?tS62AzxO0*mWS^Nr@(FlJR?j!kqe!ZDKC;VA5>e1BG*{Nv
zcR<KVE<$!Vg(Qbex+4!J=OkAk`xA<UO{=4(=>Gy#;<@{gb88~^8>GP}-bt5Aeiu1U
zP%ar?n0eZI_*B+4R4}i{5k&rwLUIe7FzfmjSeBF6;VF{=2`N(4YINzWn>x^MzCngP
z2Q<rwj$4h?W7E7u^2K>DFNv~MBRil(v1vI_H0#!Tp)}9!M9wyqD;n>#C&89=M;8Q;
zK-*r*PRM8DX+pB+p-yA3T?B&hH-jK2gud}CBGaEnTa5d*b`H|KDo+x-e=dx1Gc_v9
zc^d6Ovmkep@jYHh<AiU6@ND3x+(wogqT83~HU^$3Vmz6+RhdF(tOeotKWi^3``Pq#
zQbwyFdFxF&HuDJD(viJ_%AbM?&UyjqK`=9IGG%42(sm+i4`m-Fw7|Nw&j3RsF2_!N
zma=A7q7|~sG?Jy=joX7N-_lRAE3O9VFF>#T4B0O#5+UOBA=E9q?l>$$rq*n_jZ1r<
z$X)~!Z*@_5VZHH`f5r_Ml5@1F+H_PwM?*O2mN6;AJ1b+Zrp>wr*`iW&h*Db%vNpgJ
zGAdVY%Bv*&FSs#55=Zu);UsKsHp<gxYM(-fCT^(6s8sD%+`dK(8mhqNev7Q_)Fz`B
z^TxF4TsiG2kQ`HFQ%x@tRiZj`uMp0C<^qTkY$u2%SzD|tcEPoDZ3k8M9k|!59Y~9M
z&buF@k(QxRr*PQj5TRB9vO2&N?m**7&B;8YUuy=M{;UnpVzWaS<uyzJz-X7@c9qH!
z4LP6w{-Ji|wNyp45GVUGNN!hTY{4-MAllV0V97y+fy&OQo&?2VqMA&lFE~VUKIui@
zqb>ugG){F46n{}v!W0W~iQqFcu<GFb9RAEnXQDah0HnYym|OeY0=SP9n8B&spr}+-
zQs6D{Zf*Z~j17`>3@7ab*^VSqNq-RxtliqJda+G+aQd4;drvYwZ&VU0I{1PCbVBXU
zXA_=~$vK||*Rj9N`8qY*w_by*Ngo=^Id!ZNR{(gWxq4H1aeO2YKBP*Q%3lW)YlAD%
zC{j=s7qlKic6}&8T%+$l3bm!6aa_=sAmjy85H~==wg0AWQ>i)G0<Pfm^jm?HJD$sZ
z7cv*a<f5h@MlM@$G0dj@eimF%ip=IBt01nCMDkuhy0Ha!V93>u{Qy%C4Jz<M{M{9V
zeIVPZN@oi`3t!RRyfPtB=Tx_V;;V{kDs7F}v{o2Md#@S{+*=<)_8~>W&T1zwJ@F0d
z7(WO0H<Ux#u!Z-5TeE2c)b6jFXf~ck7;cfAOQF~WaHvHT(N<~NbR?!c1#ZhY#(f}h
zlk-eI>bIR(DWW~AX#ry010_F*_3T&BqtZV`#`+|9xVN#KUjz9=l1CA3m0kNX$$J}<
zz=-nxij1lJCiJo22XZY!YUXT%{_i6>J)dkGxBVa;i&fBTZ<EN^>Gx80j;GeoCKS?1
zfn!$Pz`k_tu9fGm!w52A1;;&q*$ZzTe*J;-&TWCSPguoq>ccFHgG5*I%pCj%mb(!$
z?MrEgn)fsKi2gu2hC8a;_&exE^CY&l`*F9WrR{@jWH?C43<u5g`3_pN=JR+4k7qb&
zxtif12^st#Bf~*53Tz9$jOHvPUK*5_Aop>EMUK}{-nTdpO<ws9nt1aa6wl!C3<tG$
zhU3qi=3U&E<~wMB&yaWWyL`v{97FeIG<Mt1f)URHEPn&-!6P@I!|x}D98d05O!E#V
zNqa34or;i06yfqF5MP2UHl8M%@iZ$gqiNnwd*GWC505AwHvTYCkr&%<2gTirid10B
zXvVQWj41i2sT!(cM>8xNM{RKgIrGu*JeNn5Y_8AQ$Xd}w&Q7LW5n<W|BGy^PwACWI
zm1!=G`_*_Vhbvx8Q#{qz1p%9s{NeSbEWvT!?c{URBhJr23))`h!p}dNh(mAceak9H
z?|mTrrIO1dNqWDCEYkZ~Q}3T8^nT9N`+2E170dNr`3Q!XG=nN@p8$nceH&9%nn4b`
z>M68Enn8oi5_SP~X8W0-SluUkCv#0l?cc|!ss6MJx;nqKo6A914RDj6X!U}00^<EM
zT$}_A#V_sVOF;H5Knao3RZ3zN9Y!yHX}kAZX;|4cQqU{tW*)8H{su~6<$hT2@MITf
zStmG|b~6nV6+u)vH48@fZUY{FHEIh2%m^6{8gw!oG}bJ;2{+qB?0|v*?l3%(WO&+{
zuZToxOcEZ`0+$g{AXnRIWLW{6OT^$iZoBXu(u%jig)g8F*>2^Hl=@e(wVP=`T0>R2
z6Oqa=v;CXaiPQ=}RW>Pj8)vj%e{**w{_GF4eO05;;X&kA#=|Wt{U%R$n`!!Ky|S->
z*>2}9LBD2KZb$NKJoyKnTgYs8@MIBupmIBsck<+Al=L#&T^iNlaZ0(F?VB2@IvL|l
z^@|wlY~SLvRg}IRgPH9fp1u?5N^dK(9pt|M3?<!2-m6hxa=;s_ZiL_2zN^u=X21SG
zcU5CL(oe|rUE6BxF!|+ET&NI!=?6fMH&CFv0r;`@!L}ygFc!Hhd0PD)_WkY>o>qT{
zd%tfv(s%pLrF#jr@>&#LlLtRg!ta6@t;BvhgxA&qUqYRmsMuN>cm|{n7>xJ@(jZ7n
zXaJ(WHePo#BG*eI(~<jYY2ZRhs}W>QcX3v7J+NOzB{c(ILc`{tcuCY77DzI)gO%97
zP9;&7;eV&q|0`Jxx`J&dI>cI=<8F^u$M21qc2#MQwm#FYEaI_kW0|JCpwZQ+-j=CC
zBYNxtZS*pRgOX=}51|oC4k7aI6#1@-{D+Fr`1A_0xMh?Ov(hhUA0t6znWgXrZkb_3
zre)}iGAi|GBFns>(J@-d43x14>?IQrp<}(0bQSpvSO`=5IwB=B$kXzi1*kc1l%fWO
zs6<l+xPCQM+)I*%es+Z~c4J!S#cc-(FYZ4mLdEe2757KNP)X#p732_aNg-S;O4r;P
zuku-FH(upo1ggrc=Jyc&&@^HDCQ{;2?fayJ4Lxit_)Fdf!2^iYzbvUv%%PDlpcSCC
zGwt*j4C(S{!SUB$)%})Z(e?dw$0ml$K5&}IKR~j7q@9H!hs!>yWYb|?{r4nQvh1eO
zWv``4<0Z)jQT(#~M5`kC(~;+bShkjG_qL>3q^O?NZdO_x02Qy{Q>4YD92cia)`I@x
zptyLU%O#On==0jcs!%%35QX|A6&AVtvi)3}h&Zo0t><{5yAi2x9kEdQqlDsWSR=dG
z8FO(|0vR;xj6IOf8X7Gbr78n|(zGFD$=IYah$CvrpdZh;DB@5f&oY#8t;n!BysRNj
zdKTetRd^R$cdo?0DsXAn4@ENI6E??2a|l3xU*E*5;L$J=9BEMrj0E)8OZnAMBn7>Q
z86^WgBgPT7u2kY_GpumyB`#m!c%iJJLt;rqucUSqvj)|(2;Zde`E1=P>Bd(I9F6o`
za|~M;yr6%bwv08FNGdvGYPeA*evRf$$8(HiHo=w#mq*3X51gx2{ZLu@OQLk~VUPZ}
zC{OTVkN#q)OgS>`tiVj^G?QXz%)t*|jbE@9|8<$5<CcP?;%zZ#cfZW0W3<}j?5%j*
zDO<Y#R+V+vwh`((SyJ)(cFP3S_st}>wCj&hL;v0D8@CpJwM<Zb=im`QZf>FV4%GMa
zGMnn_O3uCv_3f7ZsX=6MjoG?OCAL#x;jpY>Kw=H}$dxMRIOXiK<Qz~r$B?t`i!$eK
zfyJ!H*4-{~Pe|P5Q1jas2}dLz`ZYXi!5^1+s@BsMyp9D8c|F>%;a3*?LWxJSuls|<
zuM#->^>=M!>)w>eI)yyY%^H?WcX5}aC2Ob1kXBhEbFNV2B%@v;Z&XOgsPDGOfM%la
zv4-;{Itl|mCg{0OZnofmnZVa?v*4+Ms0bFl&!h9N?>`MQ+HLf6?fU-dh<=r#8+?&A
zW;;7+pPNN%<U*(Xb@pzyu^E11TRaz-#2SMWty4rbY~#_XAaYl5BDE^Xq6O;?3(+63
znX-91Igwh=ZG0Gt*{X$jHHvZZ-v-^L0fvd*#x~BxBEYu!E1Xpgd|du7tyCMRfL%{<
zDm4glDjV8{|JwW?Y;!xaHEXYP8a1kH?rdhZ?GmX*m(9IE?v_Y3%5Z75O4<enPOOoy
z(_BDdl#-(lSb@DsV$~>g(TiA2*lywL{7m~$eiNhXqPx)$wl7NT5s5YED8oFXFeYJ~
zbg0t_V*#bqxy1Nd2x&SGr~|=r+%xURw>FVr)_elVqtmbtf^t)@Bh=Z?!$N8GAw+4u
zn@Sr(ar!6rgDr>98g#Tp)HJkDJ)hWGuEfUf4X_fL|3tvi`SMVs2~u#tfqQcti=^|h
za3YTtMYitH@S7}9BFVIg+WhRvS@!Ii+4ZMQ)O2e8c`PgI0-&%nn8xhc=VhIr<(mve
zB_3;m^a&tADHKrFn8}(wJ1dJQH6$J5O}1i?Wy)l_Z!#svDxp|DYMO5{ag3Y8e3SE#
zIeq~m_{Cra1S)Y8(vqa>3n6_L&f_N9lJJwPr4~Zg<Xmm_bRniNYvuG-Uec6w6i|dd
zH<cnd*-H>bvVD^igr|*^fs-vNbmeqXdb&xGo?yiz0?Fvgtg4*ZQZ7B%YwA3k>w}8e
zQEqWo(R9*zw#l4TnWfmMu+tM6R9LdqGfb)JLaK8XDp#CSnME?^2v#aUSz)dzjjL{z
zMunXzvPlEQg~u1H%}8?Rn?kZ8*{LEHSTmFLr4r7vW+(JoXriH)K;t+vfmEooO%(i3
zw8l9_%<d4rx^ns~Oqb^ZA)Qws7AGN4fm|yBhljaL`3sqNC7dK*mmXel9wXT~P`z-f
zblN4@F0mr&1Jr`pbS*%7S*}(w*_kDrZFxFxTynQ!!b3#5%3Fav(ign~hkLnji=ydH
z-hIv|E|s&=T2m*ygsitpQt-}qfhdvitEEhicjamYlOx#^wMk=KIjxg#D&ef|bXG9g
zMjJgIL4Plu4LYJ~%v#UOFB{mUVO;oZ$@Buq^jy75W44^@OTbeu)D$A6zzPdHwx(VZ
zI|~sXkHE?onl(UGlUUbeY4xX(-j%ZLKwyJLRkB?uiBhXAN#xd0fpy@7zUjnWOC@0|
zVpW9jbruBl@QZ3;ziJhLO^Lvhhz1oU_H`mb(MCksL==npI^i>`r<03pkh;U><aHaZ
z`K0|OKD5Cgn<XJ?wR$?`TqH?Ngi*vQxR53ufzCmS5N)$CVgybi+`UC1XfzY3R)r!2
zT=7i|^EQ@^2~E@fl06=G4TC=eXn7soU7@^itRvjs8|}_(3C6dCBgVE^UQ0)~wL2Ql
zYwitqw&rz2dt%#Tc}t?L_ygJT?Rl5<)VTc~zX}&G$<5CTM_M|2TSE%R{*IzUeZj7t
z&QOfy^~R!RL4C!`jAccOJHwIQzQyg4-n`~;T#|H#oAcURTJnaG;4x3dpCk;v7K}&3
zEumO$M^6u#=8|ZrCEOFrZ9x*2s|v+p!S;|h7->a;?TRWEZw+@B=5{bU{3jmnjOB&}
zJ2F~BZODM_<J)^kF41s$MBxU|G2AVp0YM-*eUa8sUuQTLS7ofN6@SOPBo(33rTClX
z@n{eacY?CDx5eb-8No>0tdvku)({i2O&uoN(bF}X1Fiw2It~`daX_ZofL7<Z_M>(!
z&2Inx4xsJdx5W|v)@84YxP8hl`y|_WT47*l{km1+|Hk^&n7Cis*$9#^XmP|3U$zpm
zciAV$?RTcdfqr$zg+Sk6Z?ymTWA_mb;Vy(1BJWQ-9Csk|dOPtWfu(Vp&9-<71$*fB
z65ZC(Z8zO+pj!joHl$&|0@IT_ckysF9)3mgbK0}055)|W$|F}@-YBy!<l*mkigZ9E
z&wZ9-1CEtEJXc@?Lh5506ytcq(Uy=WaqqGFB<?XOb2Tc=$=_qUWY(U`m5BG)vm@Y?
znhCn&_-o>*!D_nkTCb-Vzd2r`XuECk<n>pyuYWQv&3?;eZK&*vHphjQL?e%rfq!Rf
zbcA-VU+*~7ZY!R={%h^)|BSp(bEZEECY2zjPhUn%uiNhXT3WjO;mdA8V$im$A32}8
zEEkElZMH?)A>@6R=RGF!uBJRFrrG~ubKG+0ofOTuC4)5P;q}yPT1I0A)n=s79^7T0
z^Td7#_}Wg#N|f^j&C!U|OFLKEU)@P6-D%rp&w0W==XJZ+(T3+KHLcDQFx+KjB{lWc
zWk$K%SLrD;eAVS1x8GOdtMmodGotYOs%k4ehCkpA)ccLveCG8BjM^H1nWvoR*ZY0d
z6-J$>qQ25y=kvP*zM5**8H~l5VQh`Z4Gf80LXg*29x%#00Z(0(ubR`zQkk)#dbu1l
z3ziS-uR*`3ZjC?RU`DK?I~s59ZR7HM)eY`SUzy>qtEjK?R0sUb@YNWd-7Q;;I47vC
zF}xnX-{-D2{I#A^U%AgyCNxQ|QUOP<bJtZ<O<)X9xvv_e7Ohs8!%jk(4b^Lu(#!bp
z*jJE`aV8QAgu6m9Pwkp&zmeec_?H;r*4j0#xV4618=5-em`!>k+rp7nBN~eJc7=Eq
zP(dRUY2{k2tu@@0bsl%wdNhqM;6si5P^~Q*3RQ)=x})1UmA9s<CQwtm9<Ai5RYs-8
ztFLyKpz#7VXdri)QR1mC^;Ws-)^IL&1KOPDuk!`GMy0R1zR{?ut`zO=sjI80vsi<~
z`CywyXu$nOl>;-GhDg-6pr}0*ukP(?4n^J3XmGoywl>U5^{;c+ayceHT5Vf291oe5
zPH63}c2}<VdyKWUbv39+6>}Sr$fc~cySh7Gp0Gh1Ea;2GLQ&E1o^`$y#cS())qzS+
zgQwCc^}4GosCC;qgY7YEBNyaD<<eTr0$m}`R<11BT<U2A)-t0j7~K+z#&{l@#$Que
zPySB5qb>lU{`%5VxLkRCB@7;@ud8M|g==t*I*%Vl6g`b2MAuUd>@E%X8a$G+s;0qX
z?U@|ysrJ{`c`AKMrPB4KXtfHShn`$oQ|%AbxlvU`kldZALpFCBJ@KePlP+g7y-IXQ
zRS}EtOXpq@Y;6s6mxY7vo>o|`HCQW5R9zD=){}40FhTtgX5ixn>R)RlRUt8Q7}fQa
zmHt|HsfQZCbRSvN(w-=0YxJNNQ$<ywlyRnwQ?@aqUi6|m*^7qDT+!Ow(;3DJZltEI
zujYDs%Bs<Ltfjk0bSpe~VN}(_+e<t&;`suyb-XnJZYMN*O>OP4QB$ss{9O6eCW9A|
zbu3LaLi5+d<&$e_l%q3N)-t1~TX-J^H&F2YUcJs=m*mHVehj*?kQ|hXTB0FTn_3Ph
zXWj5@OXDUwEVZxbubwcvSvbnaqF8WiqF>h327FcS%7ojO)6`)VSFBcJMd3&oEr=no
zV5xt5S67IZk3u1|Yb#~Q#`gG&4D)|oxVDwGb#@2iEVeD!BlL+uownZ2PHwoGk^mf>
z2I_S+b!!l&bSbik-?u?@qmlXqmqI2ph!Z1YO|^&7Sk;Yr%Ib{66fbTLV{sb_Mu&~<
zVH($ISf{2<nNl%WSw^bvo=`Lx?~XcKL!F^`$hmQ|81jQHTY7tp=3q?pO4+gbY+0&$
z6a<sLVwiyPyfj6Lv8AT6fyTArzQ2sdweC(2)J&d}Fcq<$j_on@i_Xqaq&?mtYEX$`
z(i}_tvP6b<EDK?jMKf};`r0ygz*FT8)cG*zsd3F{3qw@5)hiNCR!|tlthF^{I@*Y?
zD4a)HQ_g;6SXy8P5MxVXg5Z_L^e|Gji^yr@KyIxLjaZ2VfnkuVhJ&5qOG8GFsF*ox
zuB-C}JVrT|Ml2ko9>nwOJTy8dO=2<P#*(L^vZe$*#EA7a8$QEpTr7sLMDYfdE=PJA
z59_PfRM)Jlwwg<Jj3S!kskdT&L=SEi-l2SO^cFDOei($8qg=wkFp2GITnuX#>$w|f
zZbdV?nK?e9nYnPPHRWK|C-pJwV{lKgPfe|-TA0U(g!<xqZMeZxSHl-5tjgVpArez>
zg;D9Lt_XNJ_ps43dGwW|r)cO@msd_!(%sz|3PxyW$Xcmc45O_}cqIl#K5O$e4TkCJ
zGIw1W4OtDII$pkqw{D_EOwZ-B7A;aqVL3~W=$}hlP<T{&7F>bolO{U1u{GGqjo~YU
z(P$}c?Ut7Qke^t(VAv%Mgj?0r2aK9>AyrHy<-SIlRC7V{0!tJ}i&rv+m=}$fjxAWU
zbhn0>m#buKh-?$Bo9rP}Fjf*CcU>hdG|N0_SJIq2pIG!T_+TfwxN=7<Be=rmqSA1e
z)bLf1wU2WWHN^?jmoYVrM8kc|91$a-ZPEZ@U0;q~QloYhMtP-MtaD8FkvkTQh}7P!
zqPpH$dd@lGl~m{Au6S=Gw5mN631Q+vG8k>?ScL<R#mkDEi`$)xYnA{Cor__A8q^oJ
z^+sCaVH_0z6^mmyUfdJyj)z(>R&%W^vyB+{l!u5mODq$Umt2uh2&*Q`1VH3!S3-uy
zRs%x}Gq%E<xhXRxFPJ8@GGCnsi$g3lWu;5KHI*L!nq;?9t0!8&e!%{N=H&uQCxQaz
zD%_lecsEKdko1>hmQhk)!TI<ifwTSnPPP6K2V{BHQ!7i)mWC@UlwF!Cj-3{bqG|-S
zOsXr?8mFXSIl5qNG~5;J^mljmlKXftqoR`qyJ$&K*S(FI#7QH7ERiN=5RE)Blch&&
ze7S@avru9jvpS?)8jiet#RysEb63=0Z)KQ!Ko+4@B$bIVH)(3)9;;To(j5|0jI<Ts
zm8!11iVpJ1u?(=zYL-E-z_PaUgu`SiEKhjh(9P3`69R0htRuGE_LSA}^)`#~^(`ju
zxZLWi7UQF^pSELkSW&&+mt=Kwby~0#MV9rZG#id8S_?3EA}AEW5)l-MU@6y7?2CQX
zmK72<r)Bk}0jh^pC`Qb9Z#2>n?Ch1RI_sLM&LgJiQa7z?urmubcZQ7awzgOZgRIrx
z%(I7rB`Bd1M=NsqBsO4jWt4CPb3!YjePE?utc8=$ajFUv1Fu+$TH8u>h<tR2ovDM;
zPE#LlkHqkw*!&7ZrEFHKYq(o6>*j<Lax_0xz|PfE=f_Z|e3@2Aeoqx<QOs(ILn5hP
zL7{bRZRxxwJ3lQ@Zn<y~FHZCuqr9%B%CeTWT3m^z^8j>D+8L0WChw!LAjAm_jyD)z
z4J0+060j_F_?J3j4d|ss1Lm%>n#7DshdzAb=6WXg{SnX2_z=PO*_P=^bWtt>Ck5ud
zKn*8iB{uRo96w)kqFZrperRG~cB)G{tRq2UwPRVz?i_Y%$2z+s?Oc}Cw24umSlLgm
zd(@<;)(YnSmwJbRDH#tCCzk3+bw>>R++ehZaJr9EeM;wZy%QR%(+pBM;bo;Yb#?W%
zYFEW)xfIt`n<}bck^wP1$rXMTP72%=9s>)t+WG+BA)vbj>gxGs(&xuPB|rD#8u2|6
z*24S*)Y3j!E7YW%RAFC-V-t6MBTiu5b?Y$&N&1nk$cKbPY33;#_{?QB)_&N{lMNCR
znioIVgyjO7a>{%SM#*}Euia^oNo)oac0*Nb%EbsfqPJU5KP^X{bQatj3$@~mF4V^&
z{78^%<Hj~q4%l)-M0+u;S)*c`B}|bR*OVnlU*#>-LjGE@Sx}22S~ych6cLnd?Zj!7
zxx?Y5Q}?2S0*u1d{BVcP-uUq`&!)2u4A+<qFT@EDe2{i{mQyoz5b9|x^?1rK(PFP7
zH#yw#dn2)MdnDA#dw_X##*FUHRyD}euU!$|`?>pA8+h2+1BN<veueQE!w79!aV~=W
zr5fIxSU}Tx0-8@AVxr;sxrsblAF0unip8R(xkO({tY_7FJBd5djaQi70;MQ!PEX7<
zK}$za_+QE#gvmChPV-Z;mXItt_FC4awCA9~0DF-x))tQ9R|e8I<&aITriTwnVd|Eg
zBe4hO2jjc}NRsTSDSHq8+X&|?_pI~OsgpOXFyzr&>Ir5MZAvk)p@YkxQOw;{VsC}^
z@9vFLMfr@%5<edBqLks4+neWe)Dr5KheSgZ(Xplap@dvaE{)dK)|6aGQ!4K&G`jqa
zmNd_0pp=Lw5zBdvmc3$9<cgY$Nh-m*9wdW~INPc!iTTF#6E*Rax~s*=D}Ph+80I#b
zL=Ml!NivoV=31L|;n!X|t}>SzWMp@9jKw>`{K(&IuMvK3YT*ZEbTmeV@j=L6&rKMp
z39PThaZth?Frcda9*&5Jo)4GiZ#Ec-$~=DT-f#lJ*Q7WN?8Q1ydZ_$uz|x*-<*+p6
zCk}Kj>-dmp_G0Be@HRw~cOwjgDVak2)=|iKX9u!3YP4X((iLIy$GA%D{;|~Zh!gyz
zqNGO4#Q7KWpjy&s$InpFdrH0XR4y@7s6+EQPfcB!r;a9YIOMV-IzH%)&}4uGHa48}
zOP9Iblm{szkN0&oFrVdIM*RrCcZLhkYS7af^(v~~#!ADdChPa}T+8jSd{X$=GSpbD
zioPjPcGh$Gew7$6pPmJsQ$`CCC>5=cD9)-9MuN_w0FNpyCYmHcCA}qqO65`ypoitt
z-FAleD%4Zj%T4WpOyyM1j(;`5c~!T1!206}&Z~xT`lq<Kuyrzo9aQv$si|MRJ2X)~
z%d0WJdJMQcfl^NZHziQbGJ0_Wm8yG_EV(tIfr>Jdt3hd?o+j4#A09}O)g=X7k`Sb#
zHdD~1;es|LNkQULDPU(pkcy_6f;JBqv^iDK2`Run)y1=5ToqN^XyYRhWlL2#j!YG%
z-oLPL4w;<F&fpx1q;l5E;{6FVR5V{YjK&NVwa7m{lKkVt<g1r7TBLj^acmgT5t*4`
zTJR!OZKb@%E=pBhDYeqnvsOhNrXXXuAR|@KZL%ruPqe9u{=`Jpsp#Kqh*VJ+6pHt!
z3bV6l@%~hCW2mt+&MY1_YN>uzXx1s9qN;8q`h<E}PZ8T$ym-G#D9%w!Rd2;9BYz|(
zP{I=K<G6djQbDz_odKEJG>UV3!aY^=!#PW;AT-Rv{i&ktZ0Eue^a3(fFZEjB-^Dyi
z*oQZd;#3vEAflol&OA!c4T)|#d_GW>P3VcfJiHl4=-|USPu2lO;M_Pu6FD{xYa+#|
z#ssvubH7Tc0m9BMca9(q$kY~BuT`nak7ee&j-~+h{){zmG&x7*sFlgg_a9P8)tgP0
zBGoI9!_-r>Psj=VZ-`Y35ViK1#(LP2c`-H@uW3?j2anC#ukzJ02y~a5bciXsRJGL$
zmKL?uJCnoIR<vp{^f%O2izKz~5^8TX)xJ!st@70pOsFmCpthn*Ra?EHX;E9fbvaCJ
zMXMHUe?#p;IfE>hVYv*|t68czHLaworC!CnQu3*{z5i9F)r|97nI4j%T3;ynXUOSe
zt_*W!_(A3Elk)b<@NOBZN$nAtJ|x3bd83uDrl(YTHBj%7Bhi2i)fyp{UX5J+DfHLM
zv>MUW+t*)~_=7V1jtn1_VXFL-<@??!{OY~a*JQqW_c~qn|FJU6k>P9^&XM5)87`J#
zs($Ka?Pq2FFJ$-|8NMV#^}2Q{zj`72TXNvIUxtG+d{l;N)JWx5&q?>m-I&^)sjZlL
zHu@~-AL`2zqxn0_jcxD34<ZQTQ4w~i7!C<S(H@o?!LPHq?h@bPIDY;YDLmz#%b&<!
z9ByT~l<Ek^I#_P&_6X#O5C=#+XDc4#!;x`P!a!a$)EOiO8SCkcvs`}GmK&!B9CGn{
zaTs~st-*MZ<%T+pwrH>`WOTHGSEV4drNs#KwS;=`0~P(QBV-ssdJ<B|Q1Odfz=-mK
zUHCpmZhJS1z%7@TE6Qk&#aM1jH=d!wxqNPGsJXWt%)v-Ip7@u^FrMpD@nCavG_+Nv
z=(lYZhXqv>PsysNl$6?}|E2UyaM8OHcq4#c#Wt4=dBDGWE@I+NwASlPB<O<$ia%g;
zA%#~_B*Mv4i5uP?NV*D;vhv4mE}kEk{PTD!aZBYdL)Kd-PIRmC_t{*Wc%MYHN`>)$
z0>9Mz{xZq`9uZzgG4*F@E|Db*Iw%?OUMRPd;?EIOe9Ma0AWZ&)nJ%7l5bvbYwL}#x
zl3a$2@_@ESCoKBElI`O7SMZV|U6+c0-ml=7s;3IK0Q<g$zhi=n({;$tiuCgo-e^Qf
zdMf=Tn#-rEDtSYae@OBVN<vk>;#c+GgM2IhZMutd+@=dg`uieu5kGykCzbz`z!3@Q
zM_11zmkfEp|4xpGiMwnT6GboXTKTV&{MX?fSh`%Ye38N@v}8nupF!Hnug1N@cq59g
zLUK;arT7%;^N3ja(`LGORvL9IT+$6h0{2w?4**B}%KlEt@09%X_gt)()0#m1Hh`7?
z@GKXnI6R97tWpH#J+=IMkYnX{&USHH=WM}3-z&FXs{Ish5D_c?-KV>F7QI(b*D4X1
z_Y`sXC^FuO1(0vj^~B;P8S=nvFOf?4k4pav5pq?<|D5E1Zn2<9)i*W$GRR0L#eYQd
zA2~_>KY>^EEh=$H@()RVT{f)pZ&kkXqvPPQmVZ32iNvtuc|0)ti%2EvujKp{8CHIl
z-y}%$MR1o0%)8=KVFsA3{DYEzQ1X9frOC=8&g3K;2WFC`?AO%Q#3=$@f{;GVV!fQ!
z1mY8A`LZ}OSMi@?Vnl>=rAvh~B)`BW?w8B*n-VA<Q+x`&XcYcmOa5JR6{QGM`RQ*-
zrQ(P(c%7jCg&H_Tau_ZBeW{W7SKTNW-bv}OskHRBrwEs7zhz$#{NIt|fGplDOUc<F
z`BlCOU;nm<eI0LpS}(<?aP;@AQgM`b;9)_LuN*@%q|)MLe$#g(2@eiGCQ|e_v8-20
zl9#Q<PAlCLhehhR)PFvbD*(#x)%->7nW7?R9Q-*C*h1BDMRLTt%B*359n{4(*9dqU
z)<+}Zao9H^K8@`g5fA4|8%a(E{^Y_)_%TeaFPP=R5gt?P1`8T?$z*Chkc=P8)I6Vz
z&obxlWc)a$=H+Dkc=MZj$@pxh=Fw#Q1g7T4WW3JQyqAoh$kcq6jGx5RJe7=}Y|cN)
z_$f@yE6Mn&OwAX`_-W=mkc>YK<9d@7#p8AcZ&c&9CD+cTGc_Jt&}f$#OpUY2c)4h>
zA`!O_Tg3|kJO68rDa4#!PTnb5cK+8OQ;?kAIULFHb4J0Z462}_ex1f%JCVvT9ta!$
za+O+_t&;>KS%V$aM@Uq^UJm?7a!NS;@b;<!p5)&(-Gx}<YDJXrM^f<HB;Gm0l^{m!
zS^(+eh&OS(8mDgN_~HKZ2=FA|l_KXyoPM|;j#jT%IsHU-mt1!t$*=ct*YMXTNAk~+
z?BjsK+6aDlI>%3EjdEU>;@Q~}|3r%4(%)3X2TF$Z7q7&t^_`MKe<M+2!}|;E+ngi#
zKmD!5iNLE5<s6XnYZUl7)_x%;g--%M33g_3U9IMdTO>a9JmXd_X97DS>2;|neWYA&
zDU|9wd=_|T5=JCm8U_E(D00R@v619EIez$fxL_1|`rC=z4&6;$kz%PE{V|=9<X<%k
z{yE^u&gyHAKS7$VUm|q!Sn};1PM;W`)w0Eb$(8gukfP5#;75{k0r2=t{jhOw4e)c(
zj{UN~jFZJTOZi8n{6>lYjKsTonmC63b|qck1b!qt(4TT4JzdczPDg(?ldj*5LO%`_
zB)_^&>Jt*=j6UMc?Y6#&W9a+ybj<;t^jGI@YP>4uas-X_9+G(1WS14q_DK0hTADas
zwaWnTI`j{u^wZBudiAv_)nE2;{0yc(B}MH`7kznjB>UVY<*3hH9h@TQ?*X35W!str
zLf-F}^y(vBE@{s};OAh+c1-p!Ro1h>tGHFrF)2rV$E#oJ`If{F$?-?o^F1lwDfhwq
zq_9aCC#YT2N3v9XrvX1wx%iq4_rp|s;wvyC(nm+Z-y-n`QrhvsQRtr;1^+VeBl(Yp
z^(oa$ePT?tuU+Dgq}V5u<I!Gno>F$34Ltd?I_IrGd34R?_z~81NiSxM@D82j38~8E
z^5p^#9>|X4(bBfI+!j1wbYUr8kfn#14AvTohT8FokPzN;?lM~NJSL7xM?e`yYq!zf
z+1-p6n_BUrPs|AR_Tlwk`kG2S)SA0&#d3NBU<4w(<QNHyBV4?-9>;6=;uyHKx2tPA
zSS%_2%yJT`foIR%RUSOGSO#T<<}xm;UQ=F<=gy1(e>oo%fvQrIx;55qbkKKJ@DN$D
zY`l45Mc|QXJnU$ct*>@h`AQibCevfoz~LKl#)d$H`4C}20iKmp?_}X2SOXtlX)#)2
zy}X)uNK;7KfCsSDbBFS&Fe0|RYvyuQRQgIvjl$d|_-K@2w0Gg5Y|c#;y>P2hC)=eI
z4+Y22DEw6!$<^3sa4!C^ZVHZ{x=X!;JbJ`0QCwl}QmRtG)UTk>`uHEzOgu7)EX(WB
zr3HAjzc%bIF!-B0rU3I@k&p5wGk^aRPtaH4DRt$4XiIX)0RLHl9)jbK0UPvCTuK#E
zT7|@nEX1(1t)-%}jmqdP4&L4c`G(Qb*B5LKZ!IWTjySaf{}cfH25*MDT&0bT_({bm
z$c5i0dC2JIG!0c&pvqBBQc$3r3NQNotu`pMwn4)C<TK9nDte;%RITV`EuhUOIY0Qd
zEYv+{^z=qU#@28&j<<i3>dT*{#%DQT5#jInydr%kQGDtk*&oIGo&0+h{MmZg%5Qp3
zqH83SPWiNicp1~n-%d%gv&l=}VzgA+G%|Nr{`Q{b34WM1#D6QY+QVcS`DG`IZb|*2
zV1&;F80~$123}l`bw}`;bbPz9H9yI8{H@Yt3nhP$0ePjR=-v6&I;n;vw`rFj_7IOm
OC--aqFfSk2*#86JBkkq@

diff --git a/3rdparty/qpOASES/qpOASES-3.0/doc/DoxygenLayout.xml b/3rdparty/qpOASES/qpOASES-3.0/doc/DoxygenLayout.xml
deleted file mode 100644
index 927d94d2e54..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/doc/DoxygenLayout.xml
+++ /dev/null
@@ -1,189 +0,0 @@
-<doxygenlayout version="1.0">
-  <!-- Navigation index tabs for HTML output -->
-  <navindex>
-    <tab type="mainpage" visible="yes" title=""/>
-    <tab type="pages" visible="yes" title="" intro=""/>
-    <tab type="modules" visible="yes" title="" intro=""/>
-    <tab type="namespaces" visible="yes" title="">
-      <tab type="namespacelist" visible="yes" title="" intro=""/>
-      <tab type="namespacemembers" visible="yes" title="" intro=""/>
-    </tab>
-    <tab type="classes" visible="yes" title="">
-      <tab type="classlist" visible="yes" title="" intro=""/>
-      <tab type="classindex" visible="$ALPHABETICAL_INDEX" title=""/> 
-      <tab type="hierarchy" visible="yes" title="" intro=""/>
-      <tab type="classmembers" visible="yes" title="" intro=""/>
-    </tab>
-    <tab type="files" visible="yes" title="">
-      <tab type="filelist" visible="yes" title="" intro=""/>
-      <tab type="globals" visible="yes" title="" intro=""/>
-    </tab>
-    <tab type="dirs" visible="yes" title="" intro=""/>
-    <tab type="examples" visible="yes" title="" intro=""/>
-	<tab type="user" visible="yes" url="../manual.pdf" title="Manual" intro=""/>
-  </navindex>
-
-  <!-- Layout definition for a class page -->
-  <class>
-    <briefdescription visible="yes"/>
-    <includes visible="$SHOW_INCLUDE_FILES"/>
-    <inheritancegraph visible="$CLASS_GRAPH"/>
-    <collaborationgraph visible="$COLLABORATION_GRAPH"/>
-    <allmemberslink visible="yes"/>
-    <memberdecl>
-      <nestedclasses visible="yes" title=""/>
-      <publictypes title=""/>
-      <publicslots title=""/>
-      <signals title=""/>
-      <publicmethods title=""/>
-      <publicstaticmethods title=""/>
-      <publicattributes title=""/>
-      <publicstaticattributes title=""/>
-      <protectedtypes title=""/>
-      <protectedslots title=""/>
-      <protectedmethods title=""/>
-      <protectedstaticmethods title=""/>
-      <protectedattributes title=""/>
-      <protectedstaticattributes title=""/>
-      <packagetypes title=""/>
-      <packagemethods title=""/>
-      <packagestaticmethods title=""/>
-      <packageattributes title=""/>
-      <packagestaticattributes title=""/>
-      <properties title=""/>
-      <events title=""/>
-      <privatetypes title=""/>
-      <privateslots title=""/>
-      <privatemethods title=""/>
-      <privatestaticmethods title=""/>
-      <privateattributes title=""/>
-      <privatestaticattributes title=""/>
-      <friends title=""/>
-      <related title="" subtitle=""/>
-      <membergroups visible="yes"/>
-    </memberdecl>
-    <detaileddescription title=""/>
-    <memberdef>
-      <inlineclasses title=""/>
-      <typedefs title=""/>
-      <enums title=""/>
-      <constructors title=""/>
-      <functions title=""/>
-      <related title=""/>
-      <variables title=""/>
-      <properties title=""/>
-      <events title=""/>
-    </memberdef>
-    <usedfiles visible="$SHOW_USED_FILES"/>
-    <authorsection visible="yes"/>
-  </class>
-
-  <!-- Layout definition for a namespace page -->
-  <namespace>
-    <briefdescription visible="yes"/>
-    <memberdecl>
-      <nestednamespaces visible="yes" title=""/>
-      <classes visible="yes" title=""/>
-      <typedefs title=""/>
-      <enums title=""/>
-      <functions title=""/>
-      <variables title=""/>
-      <membergroups visible="yes"/>
-    </memberdecl>
-    <detaileddescription title=""/>
-    <memberdef>
-      <inlineclasses title=""/>
-      <typedefs title=""/>
-      <enums title=""/>
-      <functions title=""/>
-      <variables title=""/>
-    </memberdef>
-    <authorsection visible="yes"/>
-  </namespace>
-
-  <!-- Layout definition for a file page -->
-  <file>
-    <briefdescription visible="yes"/>
-    <includes visible="$SHOW_INCLUDE_FILES"/>
-    <includegraph visible="$INCLUDE_GRAPH"/>
-    <includedbygraph visible="$INCLUDED_BY_GRAPH"/>
-    <sourcelink visible="yes"/>
-    <memberdecl>
-      <classes visible="yes" title=""/>
-      <namespaces visible="yes" title=""/>
-      <defines title=""/>
-      <typedefs title=""/>
-      <enums title=""/>
-      <functions title=""/>
-      <variables title=""/>
-      <membergroups visible="yes"/>
-    </memberdecl>
-    <detaileddescription title=""/>
-    <memberdef>
-      <inlineclasses title=""/>
-      <defines title=""/>
-      <typedefs title=""/>
-      <enums title=""/>
-      <functions title=""/>
-      <variables title=""/>
-    </memberdef>
-    <authorsection/>
-  </file>
-
-  <!-- Layout definition for a group page -->
-  <group>
-    <briefdescription visible="yes"/>
-    <groupgraph visible="$GROUP_GRAPHS"/>
-    <memberdecl>
-      <classes visible="yes" title=""/>
-      <namespaces visible="yes" title=""/>
-      <dirs visible="yes" title=""/>
-      <nestedgroups visible="yes" title=""/>
-      <files visible="yes" title=""/>
-      <defines title=""/>
-      <typedefs title=""/>
-      <enums title=""/>
-      <enumvalues title=""/>
-      <functions title=""/>
-      <variables title=""/>
-      <signals title=""/>
-      <publicslots title=""/>
-      <protectedslots title=""/>
-      <privateslots title=""/>
-      <events title=""/>
-      <properties title=""/>
-      <friends title=""/>
-      <membergroups visible="yes"/>
-    </memberdecl>
-    <detaileddescription title=""/>
-    <memberdef>
-      <pagedocs/>
-      <inlineclasses title=""/>
-      <defines title=""/>
-      <typedefs title=""/>
-      <enums title=""/>
-      <enumvalues title=""/>
-      <functions title=""/>
-      <variables title=""/>
-      <signals title=""/>
-      <publicslots title=""/>
-      <protectedslots title=""/>
-      <privateslots title=""/>
-      <events title=""/>
-      <properties title=""/>
-      <friends title=""/>
-    </memberdef>
-    <authorsection visible="yes"/>
-  </group>
-
-  <!-- Layout definition for a directory page -->
-  <directory>
-    <briefdescription visible="yes"/>
-    <directorygraph visible="yes"/>
-    <memberdecl>
-      <dirs visible="yes"/>
-      <files visible="yes"/>
-    </memberdecl>
-    <detaileddescription title=""/>
-  </directory>
-</doxygenlayout>
diff --git a/3rdparty/qpOASES/qpOASES-3.0/doc/doxygen.config b/3rdparty/qpOASES/qpOASES-3.0/doc/doxygen.config
deleted file mode 100644
index 6433bfd0933..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/doc/doxygen.config
+++ /dev/null
@@ -1,1123 +0,0 @@
-# Doxyfile 1.3.8
-
-# This file describes the settings to be used by the documentation system
-# doxygen (www.doxygen.org) for a project
-#
-# All text after a hash (#) is considered a comment and will be ignored
-# The format is:
-#       TAG = value [value, ...]
-# For lists items can also be appended using:
-#       TAG += value [value, ...]
-# Values that contain spaces should be placed between quotes (" ")
-
-#---------------------------------------------------------------------------
-# Project related configuration options
-#---------------------------------------------------------------------------
-
-# The PROJECT_NAME tag is a single word (or a sequence of words surrounded 
-# by quotes) that should identify the project.
-
-PROJECT_NAME           = qpOASES
-
-# The PROJECT_NUMBER tag can be used to enter a project or revision number. 
-# This could be handy for archiving the generated documentation or 
-# if some version control system is used.
-
-PROJECT_NUMBER         = 3.0.1
-
-# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) 
-# base path where the generated documentation will be put. 
-# If a relative path is entered, it will be relative to the location 
-# where doxygen was started. If left blank the current directory will be used.
-
-OUTPUT_DIRECTORY       = 
-
-# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create 
-# 4096 sub-directories (in 2 levels) under the output directory of each output 
-# format and will distribute the generated files over these directories. 
-# Enabling this option can be useful when feeding doxygen a huge amount of source 
-# files, where putting all generated files in the same directory would otherwise 
-# cause performance problems for the file system.
-
-CREATE_SUBDIRS         = NO
-
-# The OUTPUT_LANGUAGE tag is used to specify the language in which all 
-# documentation generated by doxygen is written. Doxygen will use this 
-# information to generate all constant output in the proper language. 
-# The default language is English, other supported languages are: 
-# Brazilian, Catalan, Chinese, Chinese-Traditional, Croatian, Czech, Danish, 
-# Dutch, Finnish, French, German, Greek, Hungarian, Italian, Japanese, 
-# Japanese-en (Japanese with English messages), Korean, Korean-en, Norwegian, 
-# Polish, Portuguese, Romanian, Russian, Serbian, Slovak, Slovene, Spanish, 
-# Swedish, and Ukrainian.
-
-OUTPUT_LANGUAGE        = English
-
-# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will 
-# include brief member descriptions after the members that are listed in 
-# the file and class documentation (similar to JavaDoc). 
-# Set to NO to disable this.
-
-BRIEF_MEMBER_DESC      = YES
-
-# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend 
-# the brief description of a member or function before the detailed description. 
-# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the 
-# brief descriptions will be completely suppressed.
-
-REPEAT_BRIEF           = NO
-
-# This tag implements a quasi-intelligent brief description abbreviator 
-# that is used to form the text in various listings. Each string 
-# in this list, if found as the leading text of the brief description, will be 
-# stripped from the text and the result after processing the whole list, is used 
-# as the annotated text. Otherwise, the brief description is used as-is. If left 
-# blank, the following values are used ("$name" is automatically replaced with the 
-# name of the entity): "The $name class" "The $name widget" "The $name file" 
-# "is" "provides" "specifies" "contains" "represents" "a" "an" "the"
-
-ABBREVIATE_BRIEF       = 
-
-# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then 
-# Doxygen will generate a detailed section even if there is only a brief 
-# description.
-
-ALWAYS_DETAILED_SEC    = NO
-
-# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all inherited 
-# members of a class in the documentation of that class as if those members were 
-# ordinary class members. Constructors, destructors and assignment operators of 
-# the base classes will not be shown.
-
-INLINE_INHERITED_MEMB  = YES
-
-# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full 
-# path before files name in the file list and in the header files. If set 
-# to NO the shortest path that makes the file name unique will be used.
-
-FULL_PATH_NAMES        = YES
-
-# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag 
-# can be used to strip a user-defined part of the path. Stripping is 
-# only done if one of the specified strings matches the left-hand part of 
-# the path. The tag can be used to show relative paths in the file list. 
-# If left blank the directory from which doxygen is run is used as the 
-# path to strip.
-
-STRIP_FROM_PATH        = ..
-
-# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of 
-# the path mentioned in the documentation of a class, which tells 
-# the reader which header file to include in order to use a class. 
-# If left blank only the name of the header file containing the class 
-# definition is used. Otherwise one should specify the include paths that 
-# are normally passed to the compiler using the -I flag.
-
-STRIP_FROM_INC_PATH    = 
-
-# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter 
-# (but less readable) file names. This can be useful is your file systems 
-# doesn't support long names like on DOS, Mac, or CD-ROM.
-
-SHORT_NAMES            = NO
-
-# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen 
-# will interpret the first line (until the first dot) of a JavaDoc-style 
-# comment as the brief description. If set to NO, the JavaDoc 
-# comments will behave just like the Qt-style comments (thus requiring an 
-# explicit @brief command for a brief description.
-
-JAVADOC_AUTOBRIEF      = NO
-
-# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen 
-# treat a multi-line C++ special comment block (i.e. a block of //! or /// 
-# comments) as a brief description. This used to be the default behaviour. 
-# The new default is to treat a multi-line C++ comment block as a detailed 
-# description. Set this tag to YES if you prefer the old behaviour instead.
-
-MULTILINE_CPP_IS_BRIEF = NO
-
-# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented 
-# member inherits the documentation from any documented member that it 
-# re-implements.
-
-INHERIT_DOCS           = YES
-
-# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC 
-# tag is set to YES, then doxygen will reuse the documentation of the first 
-# member in the group (if any) for the other members of the group. By default 
-# all members of a group must be documented explicitly.
-
-DISTRIBUTE_GROUP_DOC   = NO
-
-# The TAB_SIZE tag can be used to set the number of spaces in a tab. 
-# Doxygen uses this value to replace tabs by spaces in code fragments.
-
-TAB_SIZE               = 4
-
-# This tag can be used to specify a number of aliases that acts 
-# as commands in the documentation. An alias has the form "name=value". 
-# For example adding "sideeffect=\par Side Effects:\n" will allow you to 
-# put the command \sideeffect (or @sideeffect) in the documentation, which 
-# will result in a user-defined paragraph with heading "Side Effects:". 
-# You can put \n's in the value part of an alias to insert newlines.
-
-ALIASES                = 
-
-# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources 
-# only. Doxygen will then generate output that is more tailored for C. 
-# For instance, some of the names that are used will be different. The list 
-# of all members will be omitted, etc.
-
-OPTIMIZE_OUTPUT_FOR_C  = NO
-
-# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java sources 
-# only. Doxygen will then generate output that is more tailored for Java. 
-# For instance, namespaces will be presented as packages, qualified scopes 
-# will look different, etc.
-
-OPTIMIZE_OUTPUT_JAVA   = NO
-
-# Set the SUBGROUPING tag to YES (the default) to allow class member groups of 
-# the same type (for instance a group of public functions) to be put as a 
-# subgroup of that type (e.g. under the Public Functions section). Set it to 
-# NO to prevent subgrouping. Alternatively, this can be done per class using 
-# the \nosubgrouping command.
-
-SUBGROUPING            = YES
-
-LAYOUT_FILE            = DoxygenLayout.xml
-
-
-#---------------------------------------------------------------------------
-# Build related configuration options
-#---------------------------------------------------------------------------
-
-# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in 
-# documentation are documented, even if no documentation was available. 
-# Private class members and static file members will be hidden unless 
-# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES
-
-EXTRACT_ALL            = NO
-
-# If the EXTRACT_PRIVATE tag is set to YES all private members of a class 
-# will be included in the documentation.
-
-EXTRACT_PRIVATE        = YES
-
-# If the EXTRACT_STATIC tag is set to YES all static members of a file 
-# will be included in the documentation.
-
-EXTRACT_STATIC         = NO
-
-# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) 
-# defined locally in source files will be included in the documentation. 
-# If set to NO only classes defined in header files are included.
-
-EXTRACT_LOCAL_CLASSES  = YES
-
-# This flag is only useful for Objective-C code. When set to YES local 
-# methods, which are defined in the implementation section but not in 
-# the interface are included in the documentation. 
-# If set to NO (the default) only methods in the interface are included.
-
-EXTRACT_LOCAL_METHODS  = NO
-
-# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all 
-# undocumented members of documented classes, files or namespaces. 
-# If set to NO (the default) these members will be included in the 
-# various overviews, but no documentation section is generated. 
-# This option has no effect if EXTRACT_ALL is enabled.
-
-HIDE_UNDOC_MEMBERS     = NO
-
-# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all 
-# undocumented classes that are normally visible in the class hierarchy. 
-# If set to NO (the default) these classes will be included in the various 
-# overviews. This option has no effect if EXTRACT_ALL is enabled.
-
-HIDE_UNDOC_CLASSES     = NO
-
-# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all 
-# friend (class|struct|union) declarations. 
-# If set to NO (the default) these declarations will be included in the 
-# documentation.
-
-HIDE_FRIEND_COMPOUNDS  = NO
-
-# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any 
-# documentation blocks found inside the body of a function. 
-# If set to NO (the default) these blocks will be appended to the 
-# function's detailed documentation block.
-
-HIDE_IN_BODY_DOCS      = NO
-
-# The INTERNAL_DOCS tag determines if documentation 
-# that is typed after a \internal command is included. If the tag is set 
-# to NO (the default) then the documentation will be excluded. 
-# Set it to YES to include the internal documentation.
-
-INTERNAL_DOCS          = NO
-
-# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate 
-# file names in lower-case letters. If set to YES upper-case letters are also 
-# allowed. This is useful if you have classes or files whose names only differ 
-# in case and if your file system supports case sensitive file names. Windows 
-# and Mac users are advised to set this option to NO.
-
-CASE_SENSE_NAMES       = YES
-
-# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen 
-# will show members with their full class and namespace scopes in the 
-# documentation. If set to YES the scope will be hidden.
-
-HIDE_SCOPE_NAMES       = NO
-
-# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen 
-# will put a list of the files that are included by a file in the documentation 
-# of that file.
-
-SHOW_INCLUDE_FILES     = YES
-
-# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] 
-# is inserted in the documentation for inline members.
-
-INLINE_INFO            = YES
-
-# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen 
-# will sort the (detailed) documentation of file and class members 
-# alphabetically by member name. If set to NO the members will appear in 
-# declaration order.
-
-SORT_MEMBER_DOCS       = YES
-
-# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the 
-# brief documentation of file, namespace and class members alphabetically 
-# by member name. If set to NO (the default) the members will appear in 
-# declaration order.
-
-SORT_BRIEF_DOCS        = NO
-
-# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be 
-# sorted by fully-qualified names, including namespaces. If set to 
-# NO (the default), the class list will be sorted only by class name, 
-# not including the namespace part. 
-# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
-# Note: This option applies only to the class list, not to the 
-# alphabetical list.
-
-SORT_BY_SCOPE_NAME     = NO
-
-# The GENERATE_TODOLIST tag can be used to enable (YES) or 
-# disable (NO) the todo list. This list is created by putting \todo 
-# commands in the documentation.
-
-GENERATE_TODOLIST      = NO
-
-# The GENERATE_TESTLIST tag can be used to enable (YES) or 
-# disable (NO) the test list. This list is created by putting \test 
-# commands in the documentation.
-
-GENERATE_TESTLIST      = NO
-
-# The GENERATE_BUGLIST tag can be used to enable (YES) or 
-# disable (NO) the bug list. This list is created by putting \bug 
-# commands in the documentation.
-
-GENERATE_BUGLIST       = NO
-
-# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or 
-# disable (NO) the deprecated list. This list is created by putting 
-# \deprecated commands in the documentation.
-
-GENERATE_DEPRECATEDLIST= YES
-
-# The ENABLED_SECTIONS tag can be used to enable conditional 
-# documentation sections, marked by \if sectionname ... \endif.
-
-ENABLED_SECTIONS       = 
-
-# The MAX_INITIALIZER_LINES tag determines the maximum number of lines 
-# the initial value of a variable or define consists of for it to appear in 
-# the documentation. If the initializer consists of more lines than specified 
-# here it will be hidden. Use a value of 0 to hide initializers completely. 
-# The appearance of the initializer of individual variables and defines in the 
-# documentation can be controlled using \showinitializer or \hideinitializer 
-# command in the documentation regardless of this setting.
-
-MAX_INITIALIZER_LINES  = 30
-
-# Set the SHOW_USED_FILES tag to NO to disable the list of files generated 
-# at the bottom of the documentation of classes and structs. If set to YES the 
-# list will mention the files that were used to generate the documentation.
-
-SHOW_USED_FILES        = YES
-
-#---------------------------------------------------------------------------
-# configuration options related to warning and progress messages
-#---------------------------------------------------------------------------
-
-# The QUIET tag can be used to turn on/off the messages that are generated 
-# by doxygen. Possible values are YES and NO. If left blank NO is used.
-
-QUIET                  = NO
-
-# The WARNINGS tag can be used to turn on/off the warning messages that are 
-# generated by doxygen. Possible values are YES and NO. If left blank 
-# NO is used.
-
-WARNINGS               = YES
-
-# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings 
-# for undocumented members. If EXTRACT_ALL is set to YES then this flag will 
-# automatically be disabled.
-
-WARN_IF_UNDOCUMENTED   = YES
-
-# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for 
-# potential errors in the documentation, such as not documenting some 
-# parameters in a documented function, or documenting parameters that 
-# don't exist or using markup commands wrongly.
-
-WARN_IF_DOC_ERROR      = YES
-
-# The WARN_FORMAT tag determines the format of the warning messages that 
-# doxygen can produce. The string should contain the $file, $line, and $text 
-# tags, which will be replaced by the file and line number from which the 
-# warning originated and the warning text.
-
-WARN_FORMAT            = "$file:$line: $text"
-
-# The WARN_LOGFILE tag can be used to specify a file to which warning 
-# and error messages should be written. If left blank the output is written 
-# to stderr.
-
-WARN_LOGFILE           = 
-
-#---------------------------------------------------------------------------
-# configuration options related to the input files
-#---------------------------------------------------------------------------
-
-# The INPUT tag can be used to specify the files and/or directories that contain 
-# documented source files. You may enter file names like "myfile.cpp" or 
-# directories like "/usr/src/myproject". Separate the files or directories 
-# with spaces.
-
-INPUT                  = ./mainpage.dox ../src ../include/qpOASES ../examples
-
-# If the value of the INPUT tag contains directories, you can use the 
-# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp 
-# and *.h) to filter out the source-files in the directories. If left 
-# blank the following patterns are tested: 
-# *.c *.cc *.cxx *.cpp *.c++ *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh *.hxx *.hpp 
-# *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm
-
-FILE_PATTERNS          = *.cpp *.ipp *.hpp *.c
-
-# The RECURSIVE tag can be used to turn specify whether or not subdirectories 
-# should be searched for input files as well. Possible values are YES and NO. 
-# If left blank NO is used.
-
-RECURSIVE              = YES
-
-# The EXCLUDE tag can be used to specify files and/or directories that should 
-# excluded from the INPUT source files. This way you can easily exclude a 
-# subdirectory from a directory tree whose root is specified with the INPUT tag.
-
-EXCLUDE                = 
-
-# The EXCLUDE_SYMLINKS tag can be used select whether or not files or directories 
-# that are symbolic links (a Unix filesystem feature) are excluded from the input.
-
-EXCLUDE_SYMLINKS       = NO
-
-# If the value of the INPUT tag contains directories, you can use the 
-# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude 
-# certain files from those directories.
-
-EXCLUDE_PATTERNS       = 
-
-# The EXAMPLE_PATH tag can be used to specify one or more files or 
-# directories that contain example code fragments that are included (see 
-# the \include command).
-
-EXAMPLE_PATH           = ../examples
-
-# If the value of the EXAMPLE_PATH tag contains directories, you can use the 
-# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp 
-# and *.h) to filter out the source-files in the directories. If left 
-# blank all files are included.
-
-EXAMPLE_PATTERNS       = 
-
-# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be 
-# searched for input files to be used with the \include or \dontinclude 
-# commands irrespective of the value of the RECURSIVE tag. 
-# Possible values are YES and NO. If left blank NO is used.
-
-EXAMPLE_RECURSIVE      = NO
-
-# The IMAGE_PATH tag can be used to specify one or more files or 
-# directories that contain image that are included in the documentation (see 
-# the \image command).
-
-IMAGE_PATH             = 
-
-# The INPUT_FILTER tag can be used to specify a program that doxygen should 
-# invoke to filter for each input file. Doxygen will invoke the filter program 
-# by executing (via popen()) the command <filter> <input-file>, where <filter> 
-# is the value of the INPUT_FILTER tag, and <input-file> is the name of an 
-# input file. Doxygen will then use the output that the filter program writes 
-# to standard output.  If FILTER_PATTERNS is specified, this tag will be 
-# ignored.
-
-INPUT_FILTER           = 
-
-# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern 
-# basis.  Doxygen will compare the file name with each pattern and apply the 
-# filter if there is a match.  The filters are a list of the form: 
-# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further 
-# info on how filters are used. If FILTER_PATTERNS is empty, INPUT_FILTER 
-# is applied to all files.
-
-FILTER_PATTERNS        = 
-
-# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using 
-# INPUT_FILTER) will be used to filter the input files when producing source 
-# files to browse (i.e. when SOURCE_BROWSER is set to YES).
-
-FILTER_SOURCE_FILES    = NO
-
-#---------------------------------------------------------------------------
-# configuration options related to source browsing
-#---------------------------------------------------------------------------
-
-# If the SOURCE_BROWSER tag is set to YES then a list of source files will 
-# be generated. Documented entities will be cross-referenced with these sources. 
-# Note: To get rid of all source code in the generated output, make sure also 
-# VERBATIM_HEADERS is set to NO.
-
-SOURCE_BROWSER         = NO
-
-# Setting the INLINE_SOURCES tag to YES will include the body 
-# of functions and classes directly in the documentation.
-
-INLINE_SOURCES         = NO
-
-# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct 
-# doxygen to hide any special comment blocks from generated source code 
-# fragments. Normal C and C++ comments will always remain visible.
-
-STRIP_CODE_COMMENTS    = YES
-
-# If the REFERENCED_BY_RELATION tag is set to YES (the default) 
-# then for each documented function all documented 
-# functions referencing it will be listed.
-
-REFERENCED_BY_RELATION = YES
-
-# If the REFERENCES_RELATION tag is set to YES (the default) 
-# then for each documented function all documented entities 
-# called/used by that function will be listed.
-
-REFERENCES_RELATION    = YES
-
-# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen 
-# will generate a verbatim copy of the header file for each class for 
-# which an include is specified. Set to NO to disable this.
-
-VERBATIM_HEADERS       = YES
-
-#---------------------------------------------------------------------------
-# configuration options related to the alphabetical class index
-#---------------------------------------------------------------------------
-
-# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index 
-# of all compounds will be generated. Enable this if the project 
-# contains a lot of classes, structs, unions or interfaces.
-
-ALPHABETICAL_INDEX     = NO
-
-# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then 
-# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns 
-# in which this list will be split (can be a number in the range [1..20])
-
-COLS_IN_ALPHA_INDEX    = 5
-
-# In case all classes in a project start with a common prefix, all 
-# classes will be put under the same header in the alphabetical index. 
-# The IGNORE_PREFIX tag can be used to specify one or more prefixes that 
-# should be ignored while generating the index headers.
-
-IGNORE_PREFIX          = 
-
-#---------------------------------------------------------------------------
-# configuration options related to the HTML output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_HTML tag is set to YES (the default) Doxygen will 
-# generate HTML output.
-
-GENERATE_HTML          = YES
-
-# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. 
-# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
-# put in front of it. If left blank `html' will be used as the default path.
-
-HTML_OUTPUT            = html
-
-# The HTML_FILE_EXTENSION tag can be used to specify the file extension for 
-# each generated HTML page (for example: .htm,.php,.asp). If it is left blank 
-# doxygen will generate files with .html extension.
-
-HTML_FILE_EXTENSION    = .html
-
-# The HTML_HEADER tag can be used to specify a personal HTML header for 
-# each generated HTML page. If it is left blank doxygen will generate a 
-# standard header.
-
-HTML_HEADER            = 
-
-# The HTML_FOOTER tag can be used to specify a personal HTML footer for 
-# each generated HTML page. If it is left blank doxygen will generate a 
-# standard footer.
-
-HTML_FOOTER            = 
-
-# The HTML_STYLESHEET tag can be used to specify a user-defined cascading 
-# style sheet that is used by each HTML page. It can be used to 
-# fine-tune the look of the HTML output. If the tag is left blank doxygen 
-# will generate a default style sheet. Note that doxygen will try to copy 
-# the style sheet file to the HTML output directory, so don't put your own 
-# stylesheet in the HTML output directory as well, or it will be erased!
-
-HTML_STYLESHEET        = 
-
-# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, 
-# files or namespaces will be aligned in HTML using tables. If set to 
-# NO a bullet list will be used.
-
-HTML_ALIGN_MEMBERS     = YES
-
-# If the GENERATE_HTMLHELP tag is set to YES, additional index files 
-# will be generated that can be used as input for tools like the 
-# Microsoft HTML help workshop to generate a compressed HTML help file (.chm) 
-# of the generated HTML documentation.
-
-GENERATE_HTMLHELP      = NO
-
-# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can 
-# be used to specify the file name of the resulting .chm file. You 
-# can add a path in front of the file if the result should not be 
-# written to the html output directory.
-
-CHM_FILE               = 
-
-# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can 
-# be used to specify the location (absolute path including file name) of 
-# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run 
-# the HTML help compiler on the generated index.hhp.
-
-HHC_LOCATION           = 
-
-# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag 
-# controls if a separate .chi index file is generated (YES) or that 
-# it should be included in the master .chm file (NO).
-
-GENERATE_CHI           = NO
-
-# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag 
-# controls whether a binary table of contents is generated (YES) or a 
-# normal table of contents (NO) in the .chm file.
-
-BINARY_TOC             = NO
-
-# The TOC_EXPAND flag can be set to YES to add extra items for group members 
-# to the contents of the HTML help documentation and to the tree view.
-
-TOC_EXPAND             = NO
-
-# The DISABLE_INDEX tag can be used to turn on/off the condensed index at 
-# top of each HTML page. The value NO (the default) enables the index and 
-# the value YES disables it.
-
-DISABLE_INDEX          = NO
-
-# This tag can be used to set the number of enum values (range [1..20]) 
-# that doxygen will group on one line in the generated HTML documentation.
-
-ENUM_VALUES_PER_LINE   = 4
-
-# If the GENERATE_TREEVIEW tag is set to YES, a side panel will be
-# generated containing a tree-like index structure (just like the one that 
-# is generated for HTML Help). For this to work a browser that supports 
-# JavaScript, DHTML, CSS and frames is required (for instance Mozilla 1.0+, 
-# Netscape 6.0+, Internet explorer 5.0+, or Konqueror). Windows users are 
-# probably better off using the HTML help feature.
-
-GENERATE_TREEVIEW      = NO
-
-# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be 
-# used to set the initial width (in pixels) of the frame in which the tree 
-# is shown.
-
-TREEVIEW_WIDTH         = 250
-
-#---------------------------------------------------------------------------
-# configuration options related to the LaTeX output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will 
-# generate Latex output.
-
-GENERATE_LATEX         = NO
-
-# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. 
-# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
-# put in front of it. If left blank `latex' will be used as the default path.
-
-LATEX_OUTPUT           = latex
-
-# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be 
-# invoked. If left blank `latex' will be used as the default command name.
-
-LATEX_CMD_NAME         = latex
-
-# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to 
-# generate index for LaTeX. If left blank `makeindex' will be used as the 
-# default command name.
-
-MAKEINDEX_CMD_NAME     = makeindex
-
-# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact 
-# LaTeX documents. This may be useful for small projects and may help to 
-# save some trees in general.
-
-COMPACT_LATEX          = NO
-
-# The PAPER_TYPE tag can be used to set the paper type that is used 
-# by the printer. Possible values are: a4, a4wide, letter, legal and 
-# executive. If left blank a4wide will be used.
-
-PAPER_TYPE             = a4wide
-
-# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX 
-# packages that should be included in the LaTeX output.
-
-EXTRA_PACKAGES         = 
-
-# The LATEX_HEADER tag can be used to specify a personal LaTeX header for 
-# the generated latex document. The header should contain everything until 
-# the first chapter. If it is left blank doxygen will generate a 
-# standard header. Notice: only use this tag if you know what you are doing!
-
-LATEX_HEADER           = 
-
-# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated 
-# is prepared for conversion to pdf (using ps2pdf). The pdf file will 
-# contain links (just like the HTML output) instead of page references 
-# This makes the output suitable for online browsing using a pdf viewer.
-
-PDF_HYPERLINKS         = YES
-
-# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of 
-# plain latex in the generated Makefile. Set this option to YES to get a 
-# higher quality PDF documentation.
-
-USE_PDFLATEX           = NO
-
-# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. 
-# command to the generated LaTeX files. This will instruct LaTeX to keep 
-# running if errors occur, instead of asking the user for help. 
-# This option is also used when generating formulas in HTML.
-
-LATEX_BATCHMODE        = NO
-
-# If LATEX_HIDE_INDICES is set to YES then doxygen will not 
-# include the index chapters (such as File Index, Compound Index, etc.) 
-# in the output.
-
-LATEX_HIDE_INDICES     = NO
-
-#---------------------------------------------------------------------------
-# configuration options related to the RTF output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output 
-# The RTF output is optimized for Word 97 and may not look very pretty with 
-# other RTF readers or editors.
-
-GENERATE_RTF           = NO
-
-# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. 
-# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
-# put in front of it. If left blank `rtf' will be used as the default path.
-
-RTF_OUTPUT             = RTF
-
-# If the COMPACT_RTF tag is set to YES Doxygen generates more compact 
-# RTF documents. This may be useful for small projects and may help to 
-# save some trees in general.
-
-COMPACT_RTF            = NO
-
-# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated 
-# will contain hyperlink fields. The RTF file will 
-# contain links (just like the HTML output) instead of page references. 
-# This makes the output suitable for online browsing using WORD or other 
-# programs which support those fields. 
-# Note: wordpad (write) and others do not support links.
-
-RTF_HYPERLINKS         = NO
-
-# Load stylesheet definitions from file. Syntax is similar to doxygen's 
-# config file, i.e. a series of assignments. You only have to provide 
-# replacements, missing definitions are set to their default value.
-
-RTF_STYLESHEET_FILE    = 
-
-# Set optional variables used in the generation of an rtf document. 
-# Syntax is similar to doxygen's config file.
-
-RTF_EXTENSIONS_FILE    = 
-
-#---------------------------------------------------------------------------
-# configuration options related to the man page output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_MAN tag is set to YES (the default) Doxygen will 
-# generate man pages
-
-GENERATE_MAN           = NO
-
-# The MAN_OUTPUT tag is used to specify where the man pages will be put. 
-# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
-# put in front of it. If left blank `man' will be used as the default path.
-
-MAN_OUTPUT             = MAN
-
-# The MAN_EXTENSION tag determines the extension that is added to 
-# the generated man pages (default is the subroutine's section .3)
-
-MAN_EXTENSION          = .3
-
-# If the MAN_LINKS tag is set to YES and Doxygen generates man output, 
-# then it will generate one additional man file for each entity 
-# documented in the real man page(s). These additional files 
-# only source the real man page, but without them the man command 
-# would be unable to find the correct page. The default is NO.
-
-MAN_LINKS              = NO
-
-#---------------------------------------------------------------------------
-# configuration options related to the XML output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_XML tag is set to YES Doxygen will 
-# generate an XML file that captures the structure of 
-# the code including all documentation.
-
-GENERATE_XML           = NO
-
-# The XML_OUTPUT tag is used to specify where the XML pages will be put. 
-# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
-# put in front of it. If left blank `xml' will be used as the default path.
-
-XML_OUTPUT             = XML
-
-# The XML_SCHEMA tag can be used to specify an XML schema, 
-# which can be used by a validating XML parser to check the 
-# syntax of the XML files.
-
-XML_SCHEMA             = 
-
-# The XML_DTD tag can be used to specify an XML DTD, 
-# which can be used by a validating XML parser to check the 
-# syntax of the XML files.
-
-XML_DTD                = 
-
-# If the XML_PROGRAMLISTING tag is set to YES Doxygen will 
-# dump the program listings (including syntax highlighting 
-# and cross-referencing information) to the XML output. Note that 
-# enabling this will significantly increase the size of the XML output.
-
-XML_PROGRAMLISTING     = YES
-
-#---------------------------------------------------------------------------
-# configuration options for the AutoGen Definitions output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will 
-# generate an AutoGen Definitions (see autogen.sf.net) file 
-# that captures the structure of the code including all 
-# documentation. Note that this feature is still experimental 
-# and incomplete at the moment.
-
-GENERATE_AUTOGEN_DEF   = NO
-
-#---------------------------------------------------------------------------
-# configuration options related to the Perl module output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_PERLMOD tag is set to YES Doxygen will 
-# generate a Perl module file that captures the structure of 
-# the code including all documentation. Note that this 
-# feature is still experimental and incomplete at the 
-# moment.
-
-GENERATE_PERLMOD       = NO
-
-# If the PERLMOD_LATEX tag is set to YES Doxygen will generate 
-# the necessary Makefile rules, Perl scripts and LaTeX code to be able 
-# to generate PDF and DVI output from the Perl module output.
-
-PERLMOD_LATEX          = NO
-
-# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be 
-# nicely formatted so it can be parsed by a human reader.  This is useful 
-# if you want to understand what is going on.  On the other hand, if this 
-# tag is set to NO the size of the Perl module output will be much smaller 
-# and Perl will parse it just the same.
-
-PERLMOD_PRETTY         = YES
-
-# The names of the make variables in the generated doxyrules.make file 
-# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. 
-# This is useful so different doxyrules.make files included by the same 
-# Makefile don't overwrite each other's variables.
-
-PERLMOD_MAKEVAR_PREFIX = 
-
-#---------------------------------------------------------------------------
-# Configuration options related to the preprocessor   
-#---------------------------------------------------------------------------
-
-# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will 
-# evaluate all C-preprocessor directives found in the sources and include 
-# files.
-
-ENABLE_PREPROCESSING   = YES
-
-# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro 
-# names in the source code. If set to NO (the default) only conditional 
-# compilation will be performed. Macro expansion can be done in a controlled 
-# way by setting EXPAND_ONLY_PREDEF to YES.
-
-MACRO_EXPANSION        = NO
-
-# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES 
-# then the macro expansion is limited to the macros specified with the 
-# PREDEFINED and EXPAND_AS_PREDEFINED tags.
-
-EXPAND_ONLY_PREDEF     = NO
-
-# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files 
-# in the INCLUDE_PATH (see below) will be search if a #include is found.
-
-SEARCH_INCLUDES        = YES
-
-# The INCLUDE_PATH tag can be used to specify one or more directories that 
-# contain include files that are not input files but should be processed by 
-# the preprocessor.
-
-INCLUDE_PATH           = 
-
-# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard 
-# patterns (like *.h and *.hpp) to filter out the header-files in the 
-# directories. If left blank, the patterns specified with FILE_PATTERNS will 
-# be used.
-
-INCLUDE_FILE_PATTERNS  = 
-
-# The PREDEFINED tag can be used to specify one or more macro names that 
-# are defined before the preprocessor is started (similar to the -D option of 
-# gcc). The argument of the tag is a list of macros of the form: name 
-# or name=definition (no spaces). If the definition and the = are 
-# omitted =1 is assumed.
-
-PREDEFINED             = 
-
-# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then 
-# this tag can be used to specify a list of macro names that should be expanded. 
-# The macro definition that is found in the sources will be used. 
-# Use the PREDEFINED tag if you want to use a different macro definition.
-
-EXPAND_AS_DEFINED      = 
-
-# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then 
-# doxygen's preprocessor will remove all function-like macros that are alone 
-# on a line, have an all uppercase name, and do not end with a semicolon. Such 
-# function macros are typically used for boiler-plate code, and will confuse the 
-# parser if not removed.
-
-SKIP_FUNCTION_MACROS   = YES
-
-#---------------------------------------------------------------------------
-# Configuration::additions related to external references   
-#---------------------------------------------------------------------------
-
-# The TAGFILES option can be used to specify one or more tagfiles. 
-# Optionally an initial location of the external documentation 
-# can be added for each tagfile. The format of a tag file without 
-# this location is as follows: 
-#   TAGFILES = file1 file2 ... 
-# Adding location for the tag files is done as follows: 
-#   TAGFILES = file1=loc1 "file2 = loc2" ... 
-# where "loc1" and "loc2" can be relative or absolute paths or 
-# URLs. If a location is present for each tag, the installdox tool 
-# does not have to be run to correct the links.
-# Note that each tag file must have a unique name
-# (where the name does NOT include the path)
-# If a tag file is not located in the directory in which doxygen 
-# is run, you must also specify the path to the tagfile here.
-
-TAGFILES               = 
-
-# When a file name is specified after GENERATE_TAGFILE, doxygen will create 
-# a tag file that is based on the input files it reads.
-
-GENERATE_TAGFILE       = 
-
-# If the ALLEXTERNALS tag is set to YES all external classes will be listed 
-# in the class index. If set to NO only the inherited external classes 
-# will be listed.
-
-ALLEXTERNALS           = NO
-
-# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed 
-# in the modules index. If set to NO, only the current project's groups will 
-# be listed.
-
-EXTERNAL_GROUPS        = YES
-
-# The PERL_PATH should be the absolute path and name of the perl script 
-# interpreter (i.e. the result of `which perl').
-
-PERL_PATH              = /usr/bin/perl
-
-#---------------------------------------------------------------------------
-# Configuration options related to the dot tool   
-#---------------------------------------------------------------------------
-
-# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will 
-# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base or 
-# super classes. Setting the tag to NO turns the diagrams off. Note that this 
-# option is superseded by the HAVE_DOT option below. This is only a fallback. It is 
-# recommended to install and use dot, since it yields more powerful graphs.
-
-CLASS_DIAGRAMS         = YES
-
-# If set to YES, the inheritance and collaboration graphs will hide 
-# inheritance and usage relations if the target is undocumented 
-# or is not a class.
-
-HIDE_UNDOC_RELATIONS   = YES
-
-# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is 
-# available from the path. This tool is part of Graphviz, a graph visualization 
-# toolkit from AT&T and Lucent Bell Labs. The other options in this section 
-# have no effect if this option is set to NO (the default)
-
-HAVE_DOT               = YES
-
-# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen 
-# will generate a graph for each documented class showing the direct and 
-# indirect inheritance relations. Setting this tag to YES will force the 
-# the CLASS_DIAGRAMS tag to NO.
-
-CLASS_GRAPH            = YES
-
-# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen 
-# will generate a graph for each documented class showing the direct and 
-# indirect implementation dependencies (inheritance, containment, and 
-# class references variables) of the class with other documented classes.
-
-COLLABORATION_GRAPH    = NO
-
-# If the UML_LOOK tag is set to YES doxygen will generate inheritance and 
-# collaboration diagrams in a style similar to the OMGs Unified Modeling 
-# Language.
-
-UML_LOOK               = NO
-
-# If set to YES, the inheritance and collaboration graphs will show the 
-# relations between templates and their instances.
-
-TEMPLATE_RELATIONS     = NO
-
-# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT 
-# tags are set to YES then doxygen will generate a graph for each documented 
-# file showing the direct and indirect include dependencies of the file with 
-# other documented files.
-
-INCLUDE_GRAPH          = YES
-
-# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and 
-# HAVE_DOT tags are set to YES then doxygen will generate a graph for each 
-# documented header file showing the documented files that directly or 
-# indirectly include this file.
-
-INCLUDED_BY_GRAPH      = YES
-
-# If the CALL_GRAPH and HAVE_DOT tags are set to YES then doxygen will 
-# generate a call dependency graph for every global function or class method. 
-# Note that enabling this option will significantly increase the time of a run. 
-# So in most cases it will be better to enable call graphs for selected 
-# functions only using the \callgraph command.
-
-CALL_GRAPH             = NO
-
-# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen 
-# will graphical hierarchy of all classes instead of a textual one.
-
-GRAPHICAL_HIERARCHY    = YES
-
-# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images 
-# generated by dot. Possible values are png, jpg, or gif
-# If left blank png will be used.
-
-DOT_IMAGE_FORMAT       = png
-
-# The tag DOT_PATH can be used to specify the path where the dot tool can be 
-# found. If left blank, it is assumed the dot tool can be found on the path.
-
-DOT_PATH               = 
-
-# The DOTFILE_DIRS tag can be used to specify one or more directories that 
-# contain dot files that are included in the documentation (see the 
-# \dotfile command).
-
-DOTFILE_DIRS           = 
-
-# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the 
-# graphs generated by dot. A depth value of 3 means that only nodes reachable 
-# from the root by following a path via at most 3 edges will be shown. Nodes that 
-# lay further from the root node will be omitted. Note that setting this option to 
-# 1 or 2 may greatly reduce the computation time needed for large code bases. Also 
-# note that a graph may be further truncated if the graph's image dimensions are 
-# not sufficient to fit the graph (see MAX_DOT_GRAPH_WIDTH and MAX_DOT_GRAPH_HEIGHT). 
-# If 0 is used for the depth value (the default), the graph is not depth-constrained.
-
-MAX_DOT_GRAPH_DEPTH    = 0
-
-# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will 
-# generate a legend page explaining the meaning of the various boxes and 
-# arrows in the dot generated graphs.
-
-GENERATE_LEGEND        = YES
-
-# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will 
-# remove the intermediate dot files that are used to generate 
-# the various graphs.
-
-DOT_CLEANUP            = YES
-
-#---------------------------------------------------------------------------
-# Configuration::additions related to the search engine   
-#---------------------------------------------------------------------------
-
-# The SEARCHENGINE tag specifies whether or not a search engine should be 
-# used. If set to NO the values of all tags below this one will be ignored.
-
-SEARCHENGINE           = NO
diff --git a/3rdparty/qpOASES/qpOASES-3.0/doc/index.html b/3rdparty/qpOASES/qpOASES-3.0/doc/index.html
deleted file mode 100644
index 9211a5de060..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/doc/index.html
+++ /dev/null
@@ -1,12 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/doc</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/doc</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="DoxygenLayout.xml">DoxygenLayout.xml</a></li>
-  <li><a href="Makefile">Makefile</a></li>
-  <li><a href="doxygen.config">doxygen.config</a></li>
-  <li><a href="mainpage.dox">mainpage.dox</a></li>
-  <li><a href="manual.pdf">manual.pdf</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/doc/mainpage.dox b/3rdparty/qpOASES/qpOASES-3.0/doc/mainpage.dox
deleted file mode 100644
index c945b356e0c..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/doc/mainpage.dox
+++ /dev/null
@@ -1,120 +0,0 @@
-/**
- *	\mainpage Main Page
- *
- *	<p>&nbsp;</p>
- *
- *	\section sec_copyright Copyright
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy. \n
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *	<p>&nbsp;</p>
- *
- *
- *	\section sec_developers Authors and Contributors
- *
- *	qpOASES's core functionality and software design have been developed by 
- *  the following main developers (in alphabetical order):
- *  <ul>
-  *	<li>Hans Joachim Ferreau
- *  <li>Christian Kirches
- *  <li>Andreas Potschka
- *	</ul>
- *
- *	Moreover, the following developers have contributed code to qpOASES's 
- *	third-party interfaces (in alphabetical order):
- *  <ul>
- *	<li>Alexander Buchner
- *	<li>Holger Diedam
- *	<li>Manuel Kudruss
- *  <li>Sebastian F. Walter
- *	</ul>
- *
- *	Finally, the following people have not contributed to the source code, 
- *  but have helped making qpOASES even more useful by testing, reporting 
- *  bugs or proposing algorithmic improvements (in alphabetical order): 
- *  <ul>
- *	<li>Eckhard Arnold
- *	<li>Boris Houska
- *	<li>Aude Perrin
- *	<li>Milan Vukov
- *	<li>Thomas Wiese
- *	<li>Leonard Wirsching
- *	</ul>
- *
- *	All users are invited to further improve qpOASES by providing comments, code enhancements, 
- *	bug reports, additional documentation or whatever you feel is missing. The preferred way
- *  of doing so is to <b>create a <a href="https://projects.coin-or.org/qpOASES/newticket">new ticket</a>
- *  at the qpOASES webpage</b>. In case you do not want to disclose your feedback to the public, 
- *  you may send an e-mail to <a href="mailto:support@qpOASES.org">support@qpOASES.org</a> 
- *	or contact one of the main developers directly.
- *	<p>&nbsp;</p>
- *
- *
- *	\section sec_citing Citing qpOASES
- *
- *	If you use qpOASES within your scientific work, we strongly encourage you to cite at least 
- *	one of the following publications:
- *
- *  <ul>
- *	<li>Reference to the <b>software</b>:
- *	\code
- *	 @ARTICLE{Ferreau2014,
- *	  author = {H.J. Ferreau and C. Kirches and A. Potschka and H.G. Bock and M. Diehl},
- *	  title = {{qpOASES}: A parametric active-set algorithm for quadratic programming},
- *	  journal = {Mathematical Programming Computation},
- *	  year = {2014},
- *	  volume = {6},
- *	  number = {4},
- *	  pages = {327--363},
- *	  keywords = {qpOASES, parametric quadratic programming, active set method, model predictive control}
- *	}
- *	\endcode
- *
- *	<li>Reference to the <b>online active set strategy</b>:
- *	\code
- *	 @ARTICLE{Ferreau2008,
- *	  author = {H.J. Ferreau and H.G. Bock and M. Diehl},
- *	  title = {An online active set strategy to overcome the limitations of explicit MPC},
- *	  journal = {International Journal of Robust and Nonlinear Control},
- *	  year = {2008},
- *	  volume = {18},
- *	  number = {8},
- *	  pages = {816--830},
- *	  keywords = {model predictive control, parametric quadratic programming, online active set strategy}
- *	}
- *	\endcode
- *
- *	<li>Reference to the <b>webpage</b>:
- *	\code
- *	 @MISC{qpOASES2014,
- *	  author = {H.J. Ferreau and A. Potschka and C. Kirches},
- *	  title = {{qpOASES} webpage},
- *	  howpublished = {http://www.qpOASES.org/},
- *	  year = {2007--2014},
- *	  keywords = {qpOASES, model predictive control, parametric quadratic programming, online active set strategy}
- *	}
- *	\endcode
- *	</ul>
- *	<p>&nbsp;</p>
- *
- *	\section sec_moreinfo More Information
- *
- *	More information can be found on <a href="http://www.qpOASES.org/">http://www.qpOASES.org/</a> 
- *	and in the <a href="../manual.pdf">qpOASES User's Manual</a>.
-*	<p>&nbsp;</p>
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/doc/manual.pdf b/3rdparty/qpOASES/qpOASES-3.0/doc/manual.pdf
deleted file mode 100644
index 5cadc179aacfecee0a4bac7b50679b85ebebdae3..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 710503
zcmeFZ2|SeV`!7EBERjM)DEpqB$b^t&-;yoMVC=@ePLY%>At`H;?E9MRvWJkJ?EAhC
zGsb!J`IPki_Um`PuXFzAod4@Hd3p^V*L`32`&!=Db>Gi0soap|;^h`3VEX)}zMp`X
z{tCUFu_b}HI6aR#J&&B7h8;c64SH5tHDx6Y6-hZ6?kjBc5)uRuThpW41pdCARTgGv
z>qKvFWDa5b{yV|nh%&J-vUh^OxOsmTB=TL5ysZ<=&eYk&32JBiLzL)mL^(pf-N(&)
zQW!7acWLS-cJ>f@J2QGG3kbcsotcxX5$t$|y!?NZ%lGpnd4>LsT&N?xqLHn$k@b(+
z@(TY^EdS49`L28yE9(pcCJLihceb~;gE{?>$oD%~ju$}Sgv1}XNJE{Vw&wH>_R5m#
zGU~?y{{+qnz4?AZ$I;2i+S=#@I(&bEhwmgj{KxP(8QGc|!A$ApZU2p2{y)LP$9-a&
z{6FB4c6M~Kvw=E7ekb{N2z|HB6Zi=Hz()?^^ldrSos3{k5Yr#aDexzJ_)o5Yzz=+s
zot><qw#OzZ@H=3>Bh7zOXMrEkC>lX+>D8Se_KwHX`V%nx+$R(j`~i$4y@s=s9Smw@
zO)ukSWMglAJVU`hp~BC7!WIR8prZcw>S@~3>)1KN=#^b<|5f^*kU1e+=m#?DcGk{E
zMoF(_1kBpj$&ud74)7y;h{=%-sw(sv?)H%5l@a<AU;-xr6Z!!dFp$fUN2#hf(tlg<
zzb#nS4rcRX;f4N$qQFTMg?^y;vv^milf{uf?nmP(8actBCO@n2CvXK$f-C$3-0x$k
zLjhpj>80$PZA}4B0W|udfbgF{7B~sA@G;0gt<N_FRO}pq7~}$kUx*Eq>juOOY72FO
z(BFhOIs!obrI*|Bz`w))yZsAr3;rDGM1Hn_AHaMYSOW%iIRgJTBZ>Uk1cXkE^e-l$
zWM|7IZD;EOaXYbgBEJLwdvH0azz^spO<jy^O#mQdoo&CRH6v@NllzaA5&09|f+tN@
z^atL`#!f(NI5Hw2ZaFzS0?`(54x5ufT=Y+f3!XGt(I1E_+aEdJ-wt<-{*Q@^{s~gS
zlZMKB<p-uRZuZu8e`PRrdm|WNNbW~Y_Z?Bd_2>Zr9Gzf*et^q=^O0j);=S@Ghy_nt
zG4GXQjE^FR**DKK`T@wdm?Gn1WbF*B9k857q~fRXe`w5m<##6V-HHWIS~2gHA66g_
zcn4s$-*VSYI~zMEJNsiB;Jxx^BRFBSygyN)pEZ(J(WKXa+Cb=ULv5f=-}C}v<o6(Y
ztjHf3k|4M6&rZU7Op(5G8BIrsnX~neH9Tq5ynp0Mg4`k}1~?{5lBT9y%75G4ul$Dh
z_l(Ke_!tC{lP<%{cZ~n{d?IOUWbN(<Jr0e$e7^<vr05geg!h<3eGk7H5Jx9^6_}l|
zHN*z+9+;iYH-l2PJt720DN71sYhqzz1hWE+%-Z@ds(TF0Z&}y(Fmuunj!755upuxr
zBOsT2!(GDy3Nz(W0SJxzzhlUM3JXHNEa5Q;`!UY9VF2H>HZne*KL4Mhg3!r$!+Xra
zP6`IHm@}|qR>z|M$i{?z2{^}e?4)RA6DK2=W9GyAM{*|gOFTJdXD0<anm|u1hQOcP
zTIiQ}am>_C3RZD<viPZP;{7986XHH;CdX_|&Dr)_&iLE4=mBnJs;(j_Epxokf`7*F
zWOO*@YyU>Nn~JoC5zHKNT=ww(k){d#5*?0tnx^AXg#JGHBSHWqj-!ArDJ^+JnO?)r
z&e{t4^H9I_>c4ADA#TwVf#{gC{oA1wHPxk+Z*a-WA1{~CpHb#LQC;&MbGCmwhK}TI
zMR}Fu0e;KczK4X9XdIKa8xR+WHBc$Tex?TWf2CI&dnn*u=JbE3`JYDn6Z*oxgob0T
zCT(rx=tzGP3V|8HOe}uNZ+|3Z!oNfXkslFT&f3lxV0+*4@lgSAJkj40vhRsN_?LJf
z@*^8d+a0M7tc$F*k@?Re{{*G*FOlGwjcEc2p8hEI1B-e*$KTVh<D>$}{AERtN!Z^4
z|Dq%u9Q0ZcfGzzT34YJQP7ZJ)Yw;aZv7ZO{E0zPc@^>Qnp$6Y?8Q0$y$j&WrqV2<X
z%($eW#@0|fbC{9+PX@vFTP9^>Zx8gKpl;lfKYJzLF_HT2`ro@g0M#)866$d{=ldO*
z(&ghlVxVgDJhD(H{hzvBe7t{K8@;$V0ncrStvOJj0W4FH{@aTHxZfIB65#KE4hiH2
z#Kg`NLI5<&0D=lJvLSFA4AxX|91<a_L~73!Z`}U0c1cxDnOdxQrZetsQ8!Jo=QF0>
zi5qY9u7wv@`AGU&E<s&f8u=+s-3LjGG06MrU-tu>ZGkIW+m^P%As7m$%pJ(AE3so`
z9&WHc<Mm=oZ5hj1TSTu|d*{U;R=meJ-1=<z?#6vFnZ^JE=c?^Z|7Y?DbK8b;pOLzo
z6ph0xIv?5OUlCD=Uy^@5(W~rOBh|BACRl2CCa~80Lbs!4su`gbfkf)ebD4X_LUlY%
z+(P`gn5XYl-?(AQh;>Qr?4}57^gtigcp4~fTjWgK6Ng(P>`B&L@dm=2B6j9<W5V_N
zw;!Cx3)7dwU~(e$g)Ld!e)$|jnIXIdyVGF0q4-Jb+Pk({LJA5;3a!Cgrx$o;#W6DD
zTI4iRF{<1ZbJLy>@rBEC+fXoH@?iFTY0u7!7sF$GA-)tIqH7JobG*lQ2^w2Xk4>sJ
zR$MTB_FUFoU8$0Ux>%{_o05skNeh}Q8Zyqtn)0Q%*gV8Fpe~Z0p7(4Ah=RJp&qC3r
z&z^^~Zg#IW9#k$r8okzib7(na#>3-P!vjX0%W9c=*XuETL5fKNaiGJ&s8jow?%lY-
zH~PZ+>yXl|ImRz|QZy^G;-|&#o8l6M(LTC)|A~XRDr%0CCgsh;s{P0Xot`Wk150S|
z6E9gRiY%(D6&C|~lUGzTf?YFqH7o10{J6)+L<QSlYmA7Ab#s=p#-|T2?~1fv?e3j^
zlGRrHn3UGxjEF;=Yh6~4Fl%kxrFj3=ywsqDCG7Dn{i1E#{8%f^nKsj=(?MS-@MlR*
z>1veK)K3rB6mVXXY&%N|UG?q>If&?)`s$M%^eRaBy=Z<Gd%v(nlNqUsVcG0-7H)Rh
z*C<oF^O}>zLwQRLKKZf1$x;UuV5vbZu#O^r{Z46VN%K%5Pr4r;eKGEW@ndBzu*AUi
z<$@fTdQltMxGI(vs<>!y=J4(TQ=ZagFQ+F9HNw{jMl*WLi90>!hzYvPX4Bl7JehCS
zKG?ROmzB5z_she38+wRm1&O|$nZ=oVs&slFg^0VT^L}P_)nvZ9pnz&*DJ-+NmUNb7
z^8V-$9;#A%a+7RZh9q)lLxqiOWyjBUE7Z%L2ZLEowyxmPIJ@Y=qW=IiNy8k8^O1a<
zfS5P@PHEKSijQMn{_02k1p{_{4zj1DkF9X^5Q8xZ1afK!q4z=*C6UX5)1NOj;}Lc(
zl(@`$UoF3&?kPA?g9lmhyZhWOcXF^-Z=v!t@~-DqFS)Czk63zpzR=EGZh@Cu=5Dxz
z0y%?bVB&uK2Xi@e;4<MZHa*fY*JP)(g{%*KnkCZHew`gVdl%2#Vwc*OY4upEMPmiC
zUP?IXhyNV`{_dLp6<>h<`L{T7)S05^Q2|=#1Ux|Ya)KO{vVVmdH3A+rh$GM|J0k0R
z--<EdM~V<rsF9SN8@=vPm=Y8PDsv!}iwN*?i-_tI0Fq#U2ztJ6#SCz}q^+$TAW;_(
zaMTh|qt^#w68(><{9PmeG?hEb#+Cr-A>dJWHg@_pIeD9Jv(tu}9tAugD-iJ9gh0(L
zoajZb@DlJyL7f~`Ah4rkX=i&hw#bj@C@ch!m?OQc?O;F&ZvtovkPRRbtI0_n$IBbi
z8vhb45Bm?tKxb~r%FBW<FhC#-;2-F42qX={!o)oK0$$j_7tSdh9BgbH0z5q2Q$z$r
zM1%x{gr|wmo;giSN=!(2hT;tAIWlr`aw3xRloVu?XUWLPj)Y)f0e^#ygO7uQPj;H{
zG}-_3>#!O`dJ4-QI~NP%G6<6t1B(>n@FR#0m?sX#w-@N|7X~IUM_jy9_ymN&4Mk@_
zm>5`CnAliHvj*<=27V91CdD~>iB}T$oT?GtWd}082Vse)7^L!Q$kjU682OE1zW4+b
z=P9W!Ffp^Rvat&Y3JHsdUX{KfBP(}P{+7Cirk3^{ox3Kc5HoWNsHLNmvx}>nyNBOH
z|3?9jp9F?SJdb=4_3~Bpo20kN?^06J((?-ni;7E1-<Q?aeXMV2Y-(=l>i*Kx+t)uZ
zI6g5sH9a#sH@~pHvAMOqvy0f<Khg^Wg!PwN!2f?K_KRMmfL@r`*jU(jM|xpkx&jv#
zDK^d}Ufi>is(3~Y=PvU-I7KEEmY7$A&%m#?Ms5u2B%ojv7-w2PQtg{&|2@Tg|0B(Q
zD0ZyZAczPH0{{<;6a)qxsJ{th1^sjVvj+dzz&|$dj}81|1OM2-KQ{1>4g7y=138~o
zhn|i{J*wpJ`C6tFF%c(>R96lde=Dq7H`_|gbm8p+HE(`=NF7xp_|^e$PKVtf9IZxY
zbO<ud2Oo$j7*KesIl)@C*_s>VEqa5!I)wRR)5h6v5w~#%JcHL!dC?q#62XTccGm=y
zM<~LC$1h=v<q$Njw0Q^$)h<#sybrgUG2A)_MiCp%_?T=j8KMrN(AbBdPwR%i+^u>D
zB7&?rq8}cDP-$P`>kB*StmwW^RddX61WnZ^iHfI(poIKGP<6syJjbv1g2#g@iQ%6U
z4ndJ(9q4NYJu-BIdr=429fu&SsJ(!`Jox60A>!O2h`%0w@>V$%Z&XdkKJcrJ6!$|=
zW5!@=64H?G5cK-hA!rPC2)eSr!-Mu|Jp^IGXWVjrxjXd`^oS23dI*w&qcv8+h;2_z
zvY#}jGpIiVbqv7Alu;ezzuxNO;w6o+O7{XIBGVCkH>WDUaB<fcVp7(4CVNrps4CoR
z3JN;M7~C%&c;KBA2)A1ekiPN7OKhD({`nn4@%a>e@zc{S-Ry-gHrX`s-0qAhylBEd
zh2>`msU7&rG-*J0PP182$5CjRDSgy*TWM&{n?mlThFh`%sW5BnhssLrjjA!Pp|OlZ
z5bnJ2>9=k}wcW|X`~qrUGrQl039S*d5|0O03EkS<cu*J{y{kw!Hd<!`zg_ahX_<|C
zDf_&aqnx{gFL<@wmsk?-nIvAAL6qGXoWSSHA?UQY*o3~PQh2v!d|wOFIc&r1NtaZ9
z`c$b!Ytk|?HNjMHrlQyQ#OOo#nM07-;%*;j%K1Tp+cB#)lrI@^;#6%fHF1QEMyk^j
zARp%pl<J0Vv=qS$Hg5}%->lS%HStWbVX{*09MoReN9TR?I{k9oBGU&mGeuV6mAtT(
zAlyTE*ITnMae;2_H45(#be#tk3)n>nU|$3z)$lFMLr`<`AqX*T<Rx+N#wqlixXax^
z<^Ug!1POUi(KCOly(c*MV!2si)znYuwX^%S%m_)sXUGn+F9eJ7tjR9++?_)yld4~-
zZ4ED+N<aYC2>(^`FC%NeRy7FNk<|z0vIf{M16oeFFVFD1K>{W^E{PyP-#6S71w4r+
z2~9?K{}42Xa|nuvI0UV?Y!R@cn_8>pXz38x@Ft0hz+*r7Z;UMW^T3kZOWNNZ5b#ih
ziVqfG=;zT&%7Epw9)kSw#=!d+@Tn=l2VoVMf#7_;)qT7GGzQ%UNse*V>{8Vl3)&o5
z&04=Tx&u(v1`LiCl|!+DF%ChW2@gRmfbUO6&XK7eghjzWi-Xrr9fF>Ddjh5ph8%((
zfH%btLBFa8KB0Zi^*5nQK_Ac$#t%XF<xqZ${W*I`9u)o|=+*2Y=nsf`;E=&hA){L)
zBq&Vq6ySXDzkLr_*AuFRp46P<Giwr(fg4x~H~1LoA&7o|hvU1;12N$j{hq^r*52TO
z4BY^g>j75XAqW(@hxZS%KMeODWdHHme|+{h0|9y9pJ)s)p?{+BiDdLoo;{-0|K!<!
z7}UQosP+{J9F#;s$qV-_C*M=g%fp+!ucot7F-9<huqxbzmqwh8+>u6JtT76ONaE<P
zBAw_qTZOY>2W4kIJmtUm5$4XoI2lB+3WkyZb*hn@*)@V*o07ql{gL2D3idwM;}59Y
z&p(q?r3Z>2g39yo+5$uolEXWDw7E~vj%wiRsGVZIT+9$1_qj+d>hn}u?S0TI_Ezg5
z{EcH*UM^7C(#D#w3HxY7RB=Sf<SVRN9GnZV7b({|TY2?uOwd`6;w_k}q0s2?E9ANi
znRX*>sj3Va=s&%*XG@lWz{y6EdkD(cp+m<d=X{Pv?Hr^4)%K+wx*Y|ELr`~N(Pp{y
zA?R|Z9735x;1C4MnA3d|$gx+7ie-Hh2>SWrh(aWcQG)|%L4D#}UTBRAk?(`e>gbqN
zq-B_~*la>lhhP{(el>z$RPT1DUF8qHW!v?%%saY2RY_wi<-A#u1m;0#xrzH&=tV1_
z7~lSg6#N)<PC#Yau%#k^L)`tkWQ$x}eQ_vDP*hGX#}v;HN$sW=Ib!;?m(<!~z2)GC
zqVW<n74P)3zV<B-;I8aVF_p&c2G-9`<z$EpXV*-G63=_IrORK;PO>5&mcN40!+3px
z67#wUg9yu_!6AtJv%kA)+gyfhx${P>6THu9;Gkzb-mTzv#=@r0_BCyUq4M+W3eM28
zhzjIoH@k*4!P!R)#cSF5A6{nil0qFej8v1<AHUb!`|N70*-W|DjdYyATbj&-8(C4v
z>a#IBkU1t%Tlu#XR225(CDOpinq29v0}t@iql-h0a-bXimM&U@!9k^)T-g&k#$IRY
z*sJ|~G4>zmmy0&OdMB+WfsECPK{k6VI!~>XS$M=sSL@iiQr(Kx6s>F>@ySpC3!3)~
z@8wh*QWTQV=~ln!lZU3~`!UCf8OvCol{)u;9z(umi;|JsK|k3&svg|mFsb0uTY1qy
zX_xm?K697@-`lXbsg*}CZEC)k+Oq_W?jm>{<t*#725Vfy(JUbXLwj%iEV#rCG^Yrs
zr=^qpM-!e+Kel@ru1l}IBS+{K8D;+bi<Kgqv!^afb;Kj1;UIJheR<5fks)nWa-*Uw
zWu7Y`sW3DtHM8n8B1vK0<z?BUFt?~;|3EtxjSS+%=l3<XMCk1$=VUCkm!72&<f*^w
zRyHfTjT%&aP8}2(O~Cpl^V$E4t-jP1?#oBvX8I=m6NP5ZAad2~&5ZFtu#q>ohP>%E
zizKY$ozp6?sq6vwE?M*G`fZvMo%(8D?o03l$9>?D5S0&w+pA^r-O8sk?U~%PchWnP
zXsfzKM<1f;qILohyUbbMSk2=jg8pCfZa;kOPfUN`_afhN>m~Xtn~!snBmf_7?nZwC
z?}!c_g65rrm65zCpe*$_Uu*APc0nt>&oJIWdf0@EhmO?3((8;T*#`!fnTbzJuG68F
z5_W*LU0!?NAxI(93V}Zx(IL_jXU@^U4Rx>7(MOGrg&jn4hKsV`^!Np*NXVip5CdLO
z69Y&wAUG|tOc=?KsbKu4mtp*9Ay6p4m8e5Eo5J^9UQ|h0Z4T`Kor329{qO<K&_j??
zS6}-&2`Vs;Zd^RG`=BK!(Ke7ZXJ&H4{@)l1Rtwyc<dFkP4Z)v1zerYKsYM#6tbB*T
z-PG!F<YO5b1_x&zN<0xhQ_Sl*301}<k9jf>>Kbd2U-I^%rJp_(<YRU5)iPc<1G*{C
zr*IbD$2x|QP_($2)41ceV}6h=(PLUQU~r)_UaTSjmnV0aMhV+~5}(Fb|FInHZS&Lj
zl?SF((LJpWR0_QcyUz|mXGeH6Q`!o)f!@~U>QYb_yX|LB0G2uQEYvHsXzooQstLV9
zFez~vMYonxFD6Y}l`JRtSuxD=40>gOmfCgzj}ddJhMFz@R`k+b)|__6qtx`z5kXNJ
zOYI&A50P}+NH;c_mFO}FGDI*^6frawpXfaSCNNO+PSmxHTGq-po;igf`*D2?PsL-I
z=&h`dFEMVy{VJ#~m+=H49qHh^SB@Omy8R^kLh@iQ-cKegLDP@qSuDtR>fIXaR|l+W
z4?$$+Hc~xR<J5h`6T2=dY=JK3!)*e3cD9ByL|HHPo`OIJvk%KP8X$bZG*-JFAN0ZN
za#gX`sL)!Gq*|1O=&efKs*RFE(A(CnsIS^3rGXn?*~TJSd^JgE@^T^04D`Mt^dZ|!
zUiBGJI-rAM#-+GC`-Hj6+!TJz#Jv#tgo+v@N}$=$+k(pU%Du_q>+M=SN{tepv|}D!
zrW^1`8uw{*lUg+?G!{nYys*Zoqd8Zoy+bQgKVa*Ai$6t>3J92U`}<;GhCMsMag?pP
zmd2vwd}VZw=KezHbs)tpcN_(s`~UY`IhQ-pmk^`%@vQ3-VQ=~^0;3ftrMOpv(i5?o
z5_L46nA^Obx_3G_p`5oSu%Wy5)|qIVw!(024-T}PB8~WB+AS`oiz~}h9?Rr4C;>z$
zPoO{>M6iFkutHZRZldaD|KlXpSc!?7Skv}CCcF4e*xJ`j*1-+&?73xq<=4hz%+?Mv
zjEu@%k@_1MaWfaIST8v=P|sjvPTA1p2-hM-G=fL3+Azkr7?O7lcF4`X`5?3(Yzvhc
zfDLJN-CfRkwOFVfVVQ7Y6iu2y==P$SbF6cfr~XC8#rAo#BGKld+#OBFre<9Jl5qCb
zdb+0p)wHZ@D=}2X8{9ADQw!H`ww-?OtTJEtAQs=Pj_$d`B42y1N(y_n&yu_IMnY|0
z)U<hj>KO0YXy1_RL(m=&;`$QjpKFs$RM8khN6&cij0f{9*$E|9TGcp(n~9u{O=81x
zz-OiEpf?9?4r>EFY^CxADLSKp{Q+?8!Yt$bEr5x6Cp*u3`aA%lnf&8lSQf^``j1*J
z8y(_l*raUkOkuv?NMtq0H3x)H9^Zpoz(Zk{eV@hSFLvM2S0HP;bxLL8)Tv9|+{5{2
zu}DM$=c@k1AafiJUZTG-NOS1U2RMsfPoAfDg-|oC+{(D9rcs_lr1i2(1RicxG|F%y
zbO}ZGg;n9Jhu7dJdL(qB6PfUcM)V*p^iFi=E<?tavdkgK4qU_EyV&USYfA7WcU%1k
z-@6JQ2tYTA86I%w<xI5`AA-VW=+IveL0yb+rS%fjV_v$QS#;F79Ty<R&L{2Z{Y?G-
zXWw)DYb2jj^zroR2KG#zAxV%-VL-APyu0`H5-<%d1{6zVH2l5h7FVYXIcnHPiA+@n
z^y@{wX}3JGHfTuY#q6h_SBR8N9B=i5P2cvfPYNzrmcgbWCCsz7xf44DBk9sSy&YVf
z0&y^Rw<8}({%AwgyZPE}T9y(55kDhI?^lCcK~zP>iOU|(PF+9R<7_CH*zKRa){}Ji
z8XZb>lVmAo5`luQg(~O2m~HP703)J?8LYKPgZ8l5W0yB}UK?X@QE%6<3@lE-{goY%
zV06!g6hD?|U2ocz^{uujZU~eC9^WE8jjA-UeQcW3!fY=!HMtzv9e9PeW`;P6ZoEft
zeHm)|!Qv1^NH?d9Gqc{#=b>IPO(?TBzyXdfu2=z-_j<B>lwYxhU)Wm;MGTHeUTn*p
z>r}vQy2$uLk={x}L=KW{zC<hPZs-H^@P-Wm?lQqI@uvJ|$Z$fm#Gb{9?zLim!c}#r
zQ&%-;(2bddMaOsJi@EC4r60KJUl7;pX0Dzkt9Zc|9c5k^l{7kCu<&Y)`02V~1N>A`
z$!C;f%II)I`KeX6QkhLkfv_gI)#u~YVu5i<Rv^(`Fs|FC%=$`v-(8u$xas9{*B7+z
z+zGnh0w)cNWTZLgXV5)q7~n&Vc$$)x@OaD9MvbiFMVD`WefZm@u$xZJn(bYDxWqDE
zPuB@|Zmh%ab5Hepk-~)vtcZ7SlsMuhVvcWsu<@@v&}Nn9wLZ0O_+|Lg#D=Aab@LEP
zSsCe~Y$xx*GPD;F=*5*20Q(48jF(W*TqLi`^uBwoA#e{_Jz6-sAEQh*=)toS9DN8X
z<7b5LcAt%}X7X%LNFB874I8;LBJFO)3`nAPvOY^pX>Tt{=MPt#-pf8G5Fm8H)ylcR
zAY;29?NNU(V{F<ouOvbC4v;tcM5F>)Ut|6sy*A;&yB<*iTX75@J-l{(ud$;=xtDEL
z%3r2DcxieQ{B-72QZ`lzw$pT_kQ-9169r9{dE3gbJLkZhlqO^2IsHWSUGNPxVEg92
zGv1SeAea}Ct%CyAvUjH8`_@gh%8ahADumAb_!|F57))^0aN0IOET??;!uqPO;RYB5
zZ2jcEJOp)?kY`L>Q``Lrr|@D*hDJs4LREB%a~!Yv&-AZc=6YO*QTJ|2b@Z}&5X|Z`
zr;`o%c4=9Bd&6Lt=etn{&NJx(ZQ-yfqP$jG<+SZLqlWdhg#y7;f@qG~K@U+MBA#u_
zvkyyBqguKa7ftFbY3E^!(Txq>Lbx~LH{5L|q4)$;gVT~C*pn<>?}2^N`PU=_ZMZp;
z@G}TQ)$R*2@4K~CkbFF!8h2GAnL_na_e#t|WPP6Gbrv4*O)PkdQCrEc#CU(ql}hav
zl5c&+*U0HEk<y#75KrFe-0Gj8*105z-{ZnrqLs#h(=3AXK%`mRGkTLkT0|-)DB`($
zZ%Kh#MEJnFXp~abGHZ!IxBSMkRR)K%zB2n-)Wa?KIn=qntx;m>OhVme(-#<Nd-9y!
zZyL0%NTQ6~dgQl>`FI<J$1k2&MYJX%TJ<ZDx1(V5pXZAM3tsCm2dimO(qo@ybSTE{
zp|uK{4coF&@7l5fOP5#(3{l%i;2JNJXL6i<QB8H}I#o3dlHjX55&Ko%*ovn526+xP
zi!b^`MemdQ()mLmtZ9W6Gb=H@Lx!vQ;!no}dk#TC+g7V&3tar(;kBbO#jcTtK0Vqm
z^BN}VeJhnwTMR5To{InVXzzhQ^6&-W=?j`nZCtwEL#}`om50#iJ3mgJC5Lpn+ys)v
zR{NEoDf<7>8tFquGuRC5Kf0EQrW*XKQ)6xLtxpq%ELGCHW330#71MuPQ<I~cvk49=
zXr6slbgNxENLQh4RC^QdE4PnR<IV`XoqoolaMlb}*a?(C{n{ZYIbgRn<=`cJ$*~AX
zqKg-H*w;rN1NqJA5L7^N5aLp`uWxc^%@q}T<q)*9iw<K@5(1JPE(3ZPxP8K>9msVD
z0&ot0GS%xi|LOIwDfIyVUruBgihTrcuz!R<+$d1qWv}!vV|`N<6WUz+n+Qe{44h&}
zOgNof(d`AEjNwq`U(zicku~Mx=e#}i+8D%|nhjBF#4c<SZE8&OI=7C3pRJ^t^XM4#
z>GryJho{w=nEM${6fCiJ1uR12m7RhmlB0<T7#p&C?1R%+F6d-@vq(&o>tVrl<~#b&
zm!4l@V4}7Pk!%S%<DL@$H|aPxj5t5R^F-p}qOpQi>l1yG`=3m2W-sqPNy31=hhRrF
z;WeqgXqjAMp<A~0Rr&gkYQ)vv^I&M7X$mfbP)P9BrrN%cziYmN+IB$MkuQYir2@6q
z7j*5<JYT$OVs~kC9R4&{U{~c31hMjSWX^zGc%olyqk5%rRl`Vx@d}|o1bcR><E)n$
z;we&a(KTu(tl2T~Jb^Q-@g|oo1JMVssb=1@dOU4@;Wd;Y%5xXVcSZ3iXLPV9my+8~
zEyWZVj=xx3H;}8DMNz+)?R=g>dn<IAsS5F;W6j6B5si89dW+<oZjD{Wr)cJkLVWSf
z;z8LO#<vg0WzUOv!KyJwta_|s5GBY9a|X<THqtfkz$A;n`HHvMqec;&_3a?DAXFWO
z&-F8`l5usloG9IOk}--D+^C5-z70h&=$$McJ<6d|FE_YB_oOiIr@v4QKZQL7H?~g*
zyhHPq{RO+h?g)=IZALrTxnr7`X$?si-rUYxE|z67?-SZ$=)N^V-VEUI=)pQZ;|2Y?
z<YB8y1+nJ??A#OlLC@+^*p-C2?Ugt(b4F5)*^Hs-g{H~pd*~uGlCSdFr)Ks6^*M**
zi3sp>H2B~A9*d{`Dv<%}+D!7>wmh{ByvO2UPq3}!T9C?WDMX?)51Mjl5pruYySi%$
zPewm#<ygT}tE2V6cDX~0q%s7Vwu!knJEG=4(ND$8m311(vT!Co{a8n;zVt4ub41th
z#z=+Eh#ZxgKbu;BRvc0nwNyFTSY@6=xS%C8m|c*4x6LL^VX9x|;z&vayCjk2dF=bQ
z!g8IaKH@WOrI6HjTyTq#>qE(mFSFcICYjHa%Gz^uyqx~Xaidhv|I*`0zx#TDTceNR
z8}x25=$a_QeYT7Q1l}QtF-qzXRD1~9m$<dg@Gy?xIV_zVLYb<hq~I?0HfJ-!$CLJH
z&V^_3d~eU$ss`$x1wxX-MHXLi&GW(K94U(`)jQ2jnYa1Q+~UjAqPi%nF}5BSf_G~;
zhxQ-}K6U*Ic!`At9T-^gsA4$doVXcoutx&fo1DeIRq<gP%FiE_p1a&;O`RwWbbZAT
z$>^qQ8kmVMJ4k(c&c4Oo^Ni=0$c1<DjTANirWJ(kdY)8gs8xL9?U@_NQ!iu*QeT^f
zD}yk;mSc^B1EvoW(bt+Y46kpvxhfhtbE;ory(ImZo<89A2UychBJl~1HwSOU9S{7+
zUv6>e6pvaaEV>9$an<Y<Ji@jUF_6T$;aeGiz**}?lN;7u6PAA^WEnBDsWJ<mC&tx&
z1lc2FbXWH)a1j+mbxe|wAA-z{wDhG6P*p-j@SO#?SCq-F9U;OBsZG6I*1Bt7;Nvk@
zwLDWfSI}wkCJ=oQT<BZwIsrz-ftTtIK_+oI=%_n=dH*KzBPD*Wro6z^@FqW?qhSl5
zcL19oiHZhC{w~^L{w4~a_<7Z}DkxC&-t-PQ2yZ$BZ7p;8JnZ||?ta_^-5$9yxYk`K
z1sQ25HA1Fp*w#2T{%>HxIQp3(uFv3C!Y>lkD6`L%EYQxF7|;2v8M|vb6M;MZ%wKF0
zrIH#ioDrrlj&K5Le|4Td1QkZr!V!9g_(l2@r*;>15I~=@!PW%N64p|6wV?5Zh#|j=
zgsP#;v&p3UjZ)4M_@2PwJK&5D7hTg1UyxO`$t4{zS9nhIKo~-<UWis`t^HN@e7O4y
z;Wf_n>F4>Tu;9XHR7#qUl&Eel-`(Cg1TD#|^B~phcG37ByoRAMIh<~bwyVhcafgkj
z>gksev-gOYyd@1qW2d`X*0=5R2YHl5D%6oBn`Pk6*a*H5o2ah(`bDOk=P$wyeZFb|
z<&<`i=YyY@Lx+}aEhKo-hFOtczniN_;e7K(_8FlDc5N7KYBfKoMVe?$#;Q{rzdiQ(
z*ivtsMN^s*x$(FvbNu|mtM!uloc>LXwUPSS$6h!tyIP-l%R=RG>PIRGp3yr*u;ICb
zmhX{Y-=y4ZKnU+V=@AEd7X4qjyJyRUOosqU$-@8{CTIz{ZiZ(W8_9qj*HDSfrk;S;
zgc723yHUZmt&qVbQcITyYAZ}?N}ZR~BgJB-?`hAlP=P`R5QZN=H8Mp%x~!yN3#q6o
zFOJF%-?T;<KJgN)F|*XPXOq(1sK}Yx4SkoladUHo@Z$x$Cxu&;T{%XTMb;#I6z$#j
zU2+$)eECwa&BN}JZ$<^9vAwP%a55FL+<c(h`s}PeHg_99E^Ja=_^+8-(w7+XT1cL)
zoUUGwp!4b6Hz*DpbbH1emRdl5o-_@~F@4`ao{2<!WzR>=m1qoZvbK5=@@koh+nHj2
zbOi2`=c87VhRTSuFE)zMbXw^4aI!Mouxit%ZIzL;b0D8CSFd4L#!jadHM!TaAS;P&
zFL`m{)&Y}1Pv?5fe@u=3Q%d#i@3e(8z2w)5eWP%94ReE+hfH%DMZz_0hHh^3aXs@0
za%lI^60}lFq_xafHLx9h^_HzYsqma#(>%w=M1<U=GSS@rAt=D>S_V{k{6PMRRuwU~
ziIz&Xga$|WYc}yUKFo8v(Iv}tV{KwBNb%k1cc>L4Q=TCYWrJ63oa4|nVuuE|1G$|{
z;6Uu#+_l-=hv)S44_pdfD<`s354n2m5q0o4bBG|V<tnJ(jt#kl_ZSFkd3bd=;y?Vv
zHagJNIW(b=)VbKdJxVn9!jK4c`t<WCJEj+}up<;5FT-T-%2QiTO)}-;DqRs3Pl)vL
zT(>e@-~Kr7=(H29&dWRhS%^WiiC&bF?~DdbiF7#TEP3K2IHdCOdV?Q#nEPq}qPL&2
zcm|B`wUez7DDdAi#cCkIiZ!JZ%X!dIFlRijj2BgMfmh4QYD<2PX}4lWu50?eb!v;|
zI-6+8q8(VPnQMiqpfu2p$&KZSUn8frX4r!!`VdLH3U9)-^OMBM*$Z`b(aRk}jBZbL
zMnn$gS(AA4ux9q8g=S{yr}B(K<1!qpgV_<Ibq(0^EEj^`2Q$I~+h(#c?%`uyTXUGR
zs%hRN*z@6zotGada_-D3Mt$J#Y8;17q~7AcJ0#bSj6P~_o!hwoGYk3OQ=hPVo$7=j
zX4h9%>!Z>E-!CI%`ot?nTJ+#@uQ)zm3VT;9M1cYG%Jxo$dtWurqlC3|G}V0B?dAfF
z1mWla;Dqo^d%kjho~A^Gme`9idCfU8Ih!m`f%;^iNKYYhB1WM@->_|DU&jgjIsQyD
zLl{J3PGk_)YoM5QzFEg(WE)_-l~<4|ITsPYUJ8pc5~xG^XMwHP<X*ug&B-btLru*C
zeRnz}%`$wetf8{QuWHIY;1E<&we^IKq+xV}{v&)SVP8*qU=XMn-i1yBwC+DYivF)<
zNuuE1G7{(|v`8<3mtFwnR0`B;)-3=nb0>5yUJHtz*8W@qgzQa+J&|9Uvp;W%W9)6N
zhj*wPygCF~h9YiFAa4)ieh^#WmYvyr1N^LOT%<p9ZmR6vO}*+n5r#$sUnNrqC7rFr
zU4;tXw34lrI)za}bne(oVxN(_yE!^!Hx5Q|@;vOa(h6JYLGjKQHns~7ossB@%~kc+
zj1VzF#-&SmOv6o;pTO5*z4$}r%e)6u0*ZC2C`W81H>mvyuB3NPGNKn9z=GZejyTZ}
zGJe*@tA-yEQU>73$PZ57Y;lGO5*sBQ8y(ASgA9135S-aF63EnXbX|!*r>5`k4M^4v
zcVyR6f^E2S#IM=WJ#rp-Sihs1->!`z_2e?>A*C8WrGZ`^v75xYj0Yu69r=r%o&LKa
zJgw$ay=@-xwqUMWv*&R%`hrZeWt+-QsLs84XW@_FfgFRPZs(VATWM6$94^llpw1UQ
z;9jX>=2Nh3Ab&K)U4Gv?Vtlv325SPV5lUF`D6{2G|1hyG0im@~9Y(}lznaT^j`=oS
zx!k_OJ!?TR4jb79J@*%V4xD)sr;H-+N!EZ4uID^Qv!Xv5uCJ%Mojn90yWl1ZJZPj%
zpdE6Y+>|!?ZO**GnRVDZa=Ftl#dyRvs^l^PCTT0>Hbcn}I?U6LTkQmglpuR-sLXx$
z_*XJst>ele63hr!4=3<>=x&rT1+GjL$wd@Kaf#TTQDHdweHQz@`ylNqH@Su~x!|pj
z6<xMn_L_HZ5@K*-&rXLqMv7|fh%_-e8uq$tc|T~Ih{H#=8Fav-WXn5I1HrAlBKU$@
zS6KiDNibiqRh1#%l6(GZ6Z(JI_u4G!UOH<op|%Ce6Jq$JG509)pFV2GpEeo3KAFwi
z{Y9HA-KQ&_xO;^{<TU!7zbjRiO>L?um~E4a*sbF>Omxve(|QOa0L|o<6J0g(en=VT
zNyeJG-IPZb`DSq<QcJ%Xdbu9~qxMV+4%gFvr2^xRJoR=xVS+C7)^w5GAh^LJG;+s%
zX{+MZ$d1CjuVxNGd;a?-#aClzgWSkVtJss*<U6`}01lfs-`%ucfCvxGI0SJd=xF(5
zO>l8s->53w7P}_a))BSy;o(ys-dI}(ktS5Vmu@ObYF#30C((K0^sPH9uJ}es^~W!T
zT)C@V{6!foC>ts3A@d$WiP?Gr7i@yt6R)PZ1ol!y`c4r&dmU>U;$811=r$t1VU)XE
zGJ<{r{<LLO*V|j9%fqx^u$W~snc!P!UixJF?T2{sk?DbkO?={Ee{C!ymXxp6*KXEi
zi)kFBZQZf?GG8WmFpSJ?c524-ubJYDVw|Nhc|RNC<*9(+ozyoTdFNNR%ljrziu)DU
zkO-dtvowbI(ndpR{p<uJC&E{uv=^z>ZL_T8+H7kY*@#D}mQEsqzJ%DygcplMTtu%*
zM_x<dfr~;n`@0uJmFN&f+y`)(R7<)d;m3?0!ZMxWguZ^6G<C(4*R$t;LG*evVVo}X
z?qm^3@K@n4S&YRohOZdBAXyV3=PBve<`M&x8i$fSR-%xc7P&*YUy5Ro?|0AUSf*k6
zSC3`jG0uw5mQIFvjEIf#_zJA%m+S}+n)yLDxw#qPyiqh|EB!Dz2HWoW5a7W7gr42{
z;>Ku-V9DrB4zFU89Eb>fyt}IP5EL#U-y;|&tyikE&9N9K<*fgd>r30WzUGr}ea+9E
zD8dOYZ?38CLIq?gH?^rN=}^gYrRAzc-a-%zp9Kr=8ppB?OI?}PgBc)WKwo&c#)R@O
zy_ly!nNtnoIz;S6f6hv?m*b!PI6-aa$<tG^M=EhCe>(Zr*PJbdZ3+1<p!QO}uyFn~
z&5p~UF%BnlS(&UFOhR>fRdn_mZ_RA7a~XUo#g*x;jrW9z%}R)Cqn!mrvblk>ETRGY
z(C3=fTr|f>Qq7(wPY7Q^X4M|8dRtojB+FV6oebJlJdk7{pw|Aqd0CG+j*d&S_&rRG
z`@^!tlA;&$aV4qnX6??qacHl}zT3H5lvFLv*C?W9982WYS$w6AsB%(-o|a?9GA9+<
z8(sAFMM-b&*SpQZYkIO5sj*2T#c9rvjH|!0e@Aj&o|8}GZCP^C_JNpH679LGIZ`&g
zUh`<$fa;><s)E_t#i0rhy0ezWJh*X|m5`UEx7|0M5r}BfyvDOP3XOqck4=tPSJEO(
zls*V+eWmL*D&5qAgxRR`mVz*2y<QHIw1&Vu!j&ZrY&B~Z@gIjUkgF<0jxZKhDU5EQ
zl*-LK7Q38M8~R3af!4}a#8mB1W#g&eoj`G`st%?w41v?GpmaQP4xYZw*Q-01M}6lk
zmq~R5J1mQ>K_mYP1Buol=#P7qiD(Qj;B=V3_F9KxmziSp+r9EHq8<Cv4<g53`N-hY
zYK$$7geb!K+?wm@RBJNxA6Cpmce4yzcvi!ge748UiQyKL=nr=#rmw*-o^Q+8OD+1c
zbr)b2k4cusj#$P2-$}}!HYBUWy+GaQw$UP`#IZaFrkP+*muRRaY^k%YXqnMygDuRf
zsZOX11eNj&Gkg=d!Gj_r90%`IJt`cf8<2~MW3hRZ*zvX4^`LvB+AI9M9(%YKD&k>I
z_W0nJS*>^akKrG(-+MFd7>>T#v_MLnYg>R&#a;dEt^KvktEcbN?jXl~#JP1GG_x0X
zr*p<DlL)nbn^oVNnK#zsGCz@l{Ht|@#_rwZ4fFNfM;(IB1*AO$sEFpOWZ{8U!2Tuw
zkKGL_kC-FM%_0nfkG73xuloLEo|r$qCzhL#tF1bX(&|#H`zQpq*l>-pdKvq+_fu<O
z{uLB)cO;WGyFJHL!sF5W*&rXXMHQ>cQ!h3R^ZkQTck)7u@5UiDFI<s-Nq!Gsv!-rE
z#-Iu~E~y#ez-<!N&O;(rBeqddsjt51+~=9V6t`BXA76i9Oc8@rfG5b7>1+))*1<{O
zPz&bAcioD-o;<>|r06^wgt=iV8BQ72gG;;;h^t&WzaRWA3wp69_>1t1d~?&C4LsQz
z`tblfY*K_)RDBf?XcEp%L@x|dqQoZfX+*y)jW<Qin8AhUTe5W>&!QxxrXN!)w|u>{
z7WFY_Aby9+%wFKt%wU0T^K8GKM30%;Tg)J-<z@|I(&BIo!2|bN2~MK|92yB;<VZr}
zhNq%;*Fwj%66@M1_45*gz;f^_FR);_k89|Sj+D6^{<ZUsZCDLaXYLGV%nfFKIxFTr
z<OSnp!C(XtjS*9FD}aQx-)V2jo}&(ERNpCurpdK#>!wU~)Gv-NjHfhf2_1DnjN1QW
z5~K0EX5Ra2x|4W*0)`yRw3iDa+S*6+dEYni#_kyvZ?6VzhCF(|Xl!8=xu_`PURcF{
z*U3`TC57UEyuCJXgUe(mX06C?SPo(bPb=d#ej)PE;pPww_s%2Lc>IC>F<O*Q8d7uN
zoNg+x8DRsJSSlFN6Ijs3yPtl>FV-=>F~n_+W1U-5YQ9%-j~O_!l!b_o6HlU6w{+PC
zdQZOI)WH67s?X=I!sQ>rEu2}cO{|T-e(mLZQcZK7Xn1=nzA{#uF4e8|<AtNxKe&<%
z><0_iIJhz--Sg#pJ2rM7P5{WSB)*`U%;qt`!=1CSM;j?Yo5M4q$WsCA=Ij)wO=O*y
zF1;hjWn8Bm7v)j!>=eeU7xs+OFUYSWFB^OZ0c@sZF>wo7+NCNJT5oWUxp2GQTZUnh
zj*8`8pA^?_xvCuCtPgKPf9mEDq~oA9AK?0`|Eit8kj5NuL$DV(B7h}sm6=?g_NL<P
zIiXR$jT;Xegzptx605%GvvPBeSjN3-QOhc*8^?dNMQvjqlC*ab-rvtow@O|`R`qFH
zUw0?1D{-T$KtQbZ)9fdtUK;M#mklF(tMfs`?<YgQz=6Kvx#5E}F6+V}k*VR!c3fZY
zJZSF@AJPe+iL7CGCz26uxRjTrXkVw+Yt2PJc&j44d`XwVH}ece&}MvHh}$BN6$Z@f
z)(2%v28)v&ocJph9BFR4i_}IMi!`p&)g{x!Y6tgJ0>@Kq8#i|SVwmcdSHUsBmZ4j3
z+Im7mS$$a@_%XMMjLq0!x^A((92cdU+%^7?c;ENeq*XukWKWe<mPKzY+_sCKkXq>n
z5#>#dfxrYvLS+QQSg_EF*h{BV1~=L%CUlskpUJT<X>6}t+wap!o*VQDu>A}z78>*-
zsvqRIqs7E^pHJ#O|9#ONGI+XZ2+zSTIUGd*kMl!163QKX4V|WInXNAa*V&ORcGz+j
z67HQHcdZwAMmYf)Dy8GiEoDgT5I4zkj#ge8pS&j5hP*-HVerWYso!MBFWN4id%!z-
zQI@}5BL7XrU7B+b9u-;MQl5&M8l7;^m>{}cc`A9gYH>xvr@IPr^u+HW2q?atI!;%l
zbi<OO_V3qw@9bn3C>%tiE~X3%L_9rEB#1mSqKBgblQ5c2r&$jvom%gz&<fn0Tbt38
zzVlhRcLOyfKKLqI=-oD5t8&xy7Lb}N&ve@`6bICKST($=)6?Z_hsjS5Q(W^*u#Nnp
zD&zSjRqb`wEdq{arafBg1*N3&snJxR$jN*8U)0v$>g?Zo{aax7;1AvT@})XzqAEi1
z+dF~CQJGD1t@9+Dniid*hVzjU*cSOHN5rn{L~9RxNgpSChBEH)V0D-kE%qz*NT?ED
za@Ye(52Qi);@J5*WrQZ2XlsPobRb<hrcF8)I8`V@Wk3}*r4lx+MBLUt)%T`vFQR^6
zYIA8Yo2g}8F?lvSXJ+MqvkW+2bILTVDL>tb#zxeNhq?FZRAz<kKCtU*Na+nM02%1t
zH1z-7R<5CW&FF?Wun{mcx_=`yVA5{yj4^n*<A9rGX&DgKEw}N1)nR=g4gWa2&vCv1
zFphHgIG1i+S)qY#LYi(-d8sDnE1Ije*m&xX)FkUzuVq*UmL46{$1^LE4vM||+*NLN
zb?H_1SCkmy!vy@%e)5!37S~7CFe+N1ielqz<2f<`t4o|M_@uIYr$7f!4O!76mG}wO
zqi4kh*BNeX-g{r^e6IZx4~_N<BoQBlu{P&@P|WrE4|FkHp>MqiCrZT~$CjxLi%By*
zf-_xlMc$o#92**ct(xe$O^X{%S`%L2J~vwujfRD}KyhXk4_l+$?vS#=w%jh=BdU#0
z8y)G2$j?0<!s&ar4nfSpK!d6hbny0PrGI_TPTeXZNG-o?`#NN%*U2K;*OWM%fru|Z
z{uKQ)$whTlDI2jwf#?DWLqsEzs^5xk@Is%j8M@;OwHMGf3&)}8Drw!28g>`0dqzCn
z3KB1(aSM66PS-GsEy(g&Tt0cYl?d+As%9p4L%-7xJJyd@+ZXFnzsn?J-<-3NTSxW&
z`Hx(rg`sEWI~TNw7FmN^*|ZIUGT-vW-)v&=m6MV@(0zoG`jme)Op|e%yl<Ilvr7Tk
zhTQ{`y17Tql0IK+R*#wA(cQD6yK~U^2q8AX{9y8f{l4P9yoi3+UHJ`VI>Ok*srz?M
z*Kx#S2zF@)pwxkfEo8=qdV5f(1@?R6Y$;41wwzaa5UIe={rSypavT?<jcOc`VMp<E
zNEyTV3lGbbL%81RTyiATXR3-4;KRH0uyeh3mX7@qHE+?ANhHG~FPT=y7N|zFa@i32
zt8U}Ef%(P@@0wSS<W(E&T{aid=L=A%(ARUB2Y=da*#E+EAxzz`%v*I2^=4wvG!rOh
z(iwln>-hVOg$u#&BA`q?+h2rS@+Q4@&tl6ogCL<;j1Lf9HZnpZ;TIG?T&^dtUr3-r
zktYpv@*{i)<se^Ar&rWYGQv6^f!4c4=NC9adcl@-#Dh$=^MlUlL~)y9e)>Dc&utr)
z2RL#0WYw=>V3mX#UTw|o+lib^F?ZeNC@71^BG(>!hhFqC&+jx8wgl*ThH&J;tHgnU
zNeT18Y4=^H2BD=!xZadgqqs}b`eMU==wKAmeCcyXsk_V0ddiEec+%okN<6<0_S+qT
zo7;#+&g8eEDRWss7xvE9{FF#DO6#xAijkH?;v^5ryfEzUs8yLw+!wb|ocdvl#b>8l
zBEp_YqO2z3)a&o|zPHV_yjzb>#4O$pT$rC$>1Uwl1s-roaFmT2*SFFufo_e^9)eUJ
z+(}ZfvfN0u#`4u5jU{a?%{l93b2WUrc03z3atOk%QvV!E(m=4CJ~X{w>@Ab-)9FLd
zz1(|~huc}0tFX?>a-sSjs_|1lZ^PBuu*SR7MaqLY%8$lnW5$s&r9#v33*-0h4T$5Y
zj0Ks*?ut8f!~i@6ByN>)t@40Z0Ku$}_MVzc#2pT9!ngM{v6U2`j9zp~7|_*>X+wAS
zaW1Wg@+^gkBg<yRicLzgVIC&2y^wm!(Xme8Qv^!XfTrybx|nC-l>{9kQnV5{U(|Km
zTYBf!y7%<kXLS@YCJZL$DR!3hMe**hw00;-E-tpg21^=9s1^;h2gPOF=_2E*OpR<}
zCq*}ESLR1WW-lswhQuMSxET*E<v4q$8OYOiv+q3)yCo}3gfpD(J|=Yinq!-DnPE*R
zj>Yb20b07^uz_2oP9658P2!yX_r;wXJocl(xrl)C==CSl`tK8_3XKkuB0r<aIjUs#
zDwKn5xtYpKfk!~xXCt2ikAUdJJpmp8k!PuJzF=5uN4a?ZG!ZRLRdA0&gfnT0o+<GK
z#n<s3=GVMWd&i~B_eLwebtx9g9T;qkd#2k!dY&=DpN1%0c~XnxO(<4(#X9-Wmew}V
zQK8b9U|YTwZIBIoVoB)p!m5FshO7P`<VAA3X?Rsn_tcm%V9n6pEo~H8j3>FsQ3xEh
zg^1l;nY3CZZF8{XuN3o?JQq)VrM{oo0mZVmXs{{cxh0~&p0qC6wpX&R<AWEYF4pj6
z@Rr0=)QWq26!NMYhjNL<Y{C04!+A!6J>3}?4>m%1?ln@)$17js$m~AqWymAhiq_CE
zWG{wpt9wThrE62`x>G+T!YYlOjh!hY;22GMaW5D6=!Yco4Q=GEc+1Oj8N-kyxDZf*
zc*xQJiFo|x_rE`xeI7yjrggnza%A)=k0$V7ixY*yOJf%XD@>V8x3dr>&18|57qZw!
zi*h>;*PV9mGNt%$B{J?e6i@J69^ztqN^e7u;$mt|PkAZe<<-8=0Q0V-NU5a@=#8dU
zxLA+;pl_%r{lPS%7-NufOH<>zJ%51<2BY>ST~Mq>u83yj=cp>49!s?9w{rzmgd)%x
zk=2~u(3Sbc#R0Fu4;xSQmC(T6)5JNX!1vh3@!vYET`;z~M+-ctv%Gq6S+4H?fuq_#
z7nts=jjNA3y|fUD6Q>E660E+Q{-8HsbK7!a<;q>I>`DC6;2^g>{RUY_Lr!#^1UP(E
zxlf$V)v;>n<rmJ&HcPG47YjfzO%2{4e@d}r$95%&xttEeK`i-F1Qt?j08vZm&Q+**
zYhF;((~0jDIa*<^CSrP%vM{Sqh)j_R2gbYOF0N(%;G^lrSA}@GiBRR}<*wNU^u^f@
z=C*rN2ff={9tta63T<>|ohtw3)=+d6A2h`b?(r_V4A@tD@Z;1Td9{}Y;vI0zVFktA
zJ?}7Vv0wZq+PgPJaAPR>fw9mB%oO`UCSw?AV~^o-K_!Qmb1p(>VieyZieje^Jfyz#
zK3;d)Z|Th5V_$Q%6n0n9W_GaC@OERGG-v&UcS4^3-j`VCyBjEdr=nsPI|bWN=1MES
zTBNd#delH^n#I;k36G+kcTZREPQf*)z%g-~bww1<I$jO>Y+l!7FVFVuvz9k|MxG+)
zaNe<MtWe*0OO);P4vQrPuIFsOZBa|k&cT+ErIo&HFqEY}>JtFrRAk>wQ;PZES6JvF
z#$vU!Wo0#ZM3(htRep9y?P!!X0%Ke`a3@O5;7+4ab{Q!}fn|)DcC9}j==yU|V5!$Q
zj4Vww=hmCD+|n`xdwTH&TRwg5?evxSpsD)tD~JW9N6Z8c-gMhZOT8J1d)t$t{@^oJ
z<`?C+2?^h)bdGWzq@^SYR(`HOqbFOPF5G^Fnj>mY=ds9&*J<QUi?Y=StL(;uCtm%A
zV5Y3NO4Ya#b;i6puh+x|m7j7FB)HIxm9*=b1(pU>;b0=mWDrH36i!gG=KVb~EGm;+
z3cVmUF~YOTTXgt{oF}b!6N6p!cb{&Dtot~Orh|b`=&2oai|K21MlW_LbScB-y_fpm
zRBGq<qUq5c7t*1FGf|G@K`#j*UiHhvm>1j118?DejD5}sd`{36Mk3FY`;}Z4v=Su`
zxUzu{G=ktNUUm5x*w6To1@k&)MC9C+n|c#fIr#>xt(&DXUD0Zu9Pbhkkish9L8Sn`
zW?pyGX@bulc1emJDd}Rj0A5?Zh8dWWZgmwXXbTp!dx3&>h(b5*1rwLAJ>ePt`))ge
z%?otY<)rUMnr(!t2%BEE>ee$h!7#=<Z^c)HmfW4n3aT@z3f1D>hWeOo&1q(!M1<#J
z!~!`!0$Dvl=Fx~!Op@V9#|NUQ+s|0_Xr<X;N)8QT@ouwGJzz_3ODq4Yw!ubF++jX_
z{`UHOccNqvu#L0iJ{y{lRWGV|@mdc5h9X+1WWvK~!^=iF&rYjqyd&8nC)oMF+VFtw
zmpm@ghqs`+zpMgn4++D;6<YmQ4GyKT=2#o#V_K6zoNzx`<(}j{N7^`vi@;HcC#pOO
zRT~mrUwkgP0f|SYY#sL~@jT2uNCOg&Sk%4$LEd+VHMypJ21V&02-1tvl&T;_f<#5S
z6h%Nlh)6f|-lL!(y%(uUkxrD}dk;l=FQF<mp#~D-zMh@gotZOd_I&&8Uf1q7e}oh-
zqP}_ZJoo);H#n4%ww!W3%%XbR@F)^(k+$la?%l8L=k6ObiAW4Hl{3+(*-j6VUWSNU
z6R0p3f1!L4@A8cedeUNNrj@@>^03`xlIDtRty330a>0~*au^!4wEfKWq!m_}ZO+fV
z%3-{p?mWWA;iX4&icB+gX+f3ydYYX{4Ee4r9ce&=#vANAMBAKefHJc>eusOwUT%+X
z_P(R11O3j-SeBLt$rrzS8{%D@znHySqr&6TRDnsTDuCHT-eUU2>~-E5e1yKUX>>6W
zC-CaB;n&gn`vBs##2{=t?JD@D$Kv|^0go<|L0PwC_*OrP0$jb8Y@~WBF<`qRLeGs>
z;-xi=sHYtn36U%Ac(!dR_Ygk{lup#Oo|_vS6MSs|m+co$@bU{9oDS*`W!%tndfvaJ
zuEOJEZo6YZ&jfOK{~#hQsL5b$dwaFMI)wBZP|Wx%(Z2$UnPw2R3V(%fP7a>$7iIaa
z1%)J|O{$T4Oc6K$LS+M#<!8r<(TT_Q_6%wDTKDPh6RXlmXK59LTm*p}K<xP?R2@s4
zlHv^R`-vsRh<Z)APbvoQID7o`<Zcc7(O?G+_WcG?Gu`+iJDfqN&AN0~lAgQ9R2TDx
zVDS(@H)+x@<kTltU150%e)%jru15$~M~u3!E4@N>)~URGD9o^>wOHSnUSWO%@-%S~
zflZYV$3HFiSHln*ms^24NgJqKBXX7~fXX#0A7=OErcH#}EeA#MPH=?BdNEKZS!NLH
zPb*PQ+e{3LI6FGJa=EXhKVUGHaUjt&h7Dgwe9US)G3H!s>o~<NoazjUeYOlM#OT0<
zA%aY&@Bkl3_FR{%=_l(-_s8pIM@P|FCHF}vFDl;W#uNkRy&XMD#US;Gu>prm?v1*h
zS>Wvd7WIqr_&;&`7s3@4+yjqVZ(+KT)>Ew*UsFRz#%SAwxQ-~vciMGgl9wniwe_*X
z2KpRF_qKQ+>8NKmafI&5pp&eZHap)+*`yq~qw!75a$>I<A)_DNhFmPjayw+`K0klS
zb^hY>%r!;w=AuYeYAWA1&n;U3t&^6I5Y{)xiuV2V2G72|WhAk>!)*6VW&R7A-kd-;
zf%C(4GQj4DEd=LhtW`M1#*a-KCoZh*5ZoZ&b{Kk0qE?@N$v4ogZ-8pXl>TNtJK^30
z+QR>Cv-vNr!u+2)51f^cWN23g@gW#nEPDn8peBr1Wi;h0B;E2hxqhIVr(tSAlzyg0
zH3-+iFyr3=aQ?Z$x%CEqjnKQ{=);_#LYA*{;0#{BF6brgjtq)Kg=Br6(Rw*xfkz>^
z?^v;wJ(%JKfmm*SOeGDvcMsu#2q{;dIC^nb<jO8~G!3Sq-k@tJqWkL1rYl9wL88wq
z7!{TmvGRrEd6tkOY};UCDV6>vcDLvof!%zNGktS+qW1^qcmDI1xBB<I?2hAaXPyb~
zG}UQJ0+1pDrtACtzx>xo()_*|ll}y~#i2OgyL|t&kNEU@-hjAje|10eq_wj_UYd$S
zNjeJc#hLnI#8q_KYh;Ngj6d)`Z{Qo4Y3{vRJG5OCZ#$-0{&Fmj)5p~e^Xc`l8#s-c
zh4Vgl(LX_LMtrv7Z3&!H<6;AL#-cJR&R2DwOu3m3(VI#yHI@kuLdqOUa~W_QJc|dV
zb)o5JC)X-{-8L~NCHK#AL*J>Wj;0zUkMCKl$lJ~Cd6%Rdpckf=_3N-m49jL$Z8j^=
zNZeLarz2Y{CAX+GUvL(v=G6&czIcgdZlH!DhIP1J0jKOEjV`J3%zkCe7a@p~+RKd)
z)L$cAF4%c!-u?X^;5lp|)~Q5~N20-~$lx_}L<)PHn_HDZ=1jj(8TUYx*%hJHFFqB-
z=VaVuCz1($Ks8y;?tJ8&JiW63Jb9b7&1n~xZJ{v5-NDiNblIYLv(9b8!eoMB;k~Ed
z-O)bS4aK+5Z?xVjwbrll<tbOGbG4gLN(<DFc%E-w{+#!Hs^t@>L~-@cF>22m3{}tQ
z8dBr}%IX6im-}ywd+gX+DkR)MIsCYGv$yoiqCY8DM1_P0WXq^zun^B=s2?K{sWxg^
z{`MnR>7`h@DSm#JKItxyV`}V>k0Aj6-aselm*YP0jQQMq&t&ap8aG>{g}i#A#U`R8
za=wt5Ya#&3-`;gAKhb1q{d#4&5x8+qbcj<vXy8jJ(%RzL2LZ<zO}(+=C%fb0dcKjm
znT_($nQ2FNK`DPz$(&0{OrzbdK9x&`y)DG_&C|CkYQV3dzF0#{S8E1iX1oiyhP&yV
z>{K~$c+|Zganzb5E~F;8-XD%krMqAoiJBb|&fWfc+aS_qL~Xo(rm~i46|WYz7VjHz
zrPg_CB-Agp2ilNO2|m4E6b|oXp?N18x!qu|F+>=u>mhuri=Yv=ld_#xbgZ^y7rP5v
zN0E&t3Nb90`Lb<1h<m<qJ7(iR;yZ!|IPV7oWt3*L33QOkS%X~;^|b6^&%T-yQ(u3{
z9ww?f>u&JZe$VkHocnjld8H%%cZclqZo6P$gWh&?nW?f&sgqp?JHDBa^}Wa$B<`bK
zA$z+KmWYXuOl`+Vs=w9C4~gvA_oJ<o`<U6tg<;(8TBbO$zpOlbyO?kM=@f-3!qJgP
z(0>LgaEmHNoI~auD~X(Y31JvQhgYylm@pP;2$68)f-yu4?Gdrn3xp~0`#XMH=F%HR
zVJRdEEp<9MN_Nree*DOnJ`Z}W=AMMDe~*ENYN8>^n}K7Eckp+ITu$soWGyp$D4)$%
zCI&m-=mEWyMuMHZ&%cTBxY#67Vf*qY$as{uX>`}lmGy_?+Vi>I;a)6}qJA(xi0I&4
ztuQBwG5m1%t3=S-ihNYV<cn$?Whh1AGR*TWG5LKG$E%J5j>ZSl6cj<K&t{DhV;%!|
z$MMXH&jDV(W63b^Jc;|-Y{Tsks4Ky896}3!rTJFo?9))iU76#KQ=9zydPxwEojiZ9
zh1HxHxdyuuYqiZfCkJ+Tw{2qnT3t%(sq0Q+R#{ER{OO9uqW#P2tBOPAdDCjq0f{zi
z4Tks5!sU2i9PsJw@@)#_Ocn0Ip<(}kb5#$WOY^{%E7PF#E*^{{(ZJ{<OR$yF*$XKt
z>N4M|ud`-8+NzJ=F<nd#BaxdYVQ8zDNzMv&+FNtd+qEh&Jefp)#cJ>_>89CSi_8!R
zs2s1)kf=wjdAM+L)Z5$U&P;a!Rc(Iv<%W8D!4i5<U$1j`_WCYe7Y`-^{uuji$=7iE
z!Dc&4+}GJ1y0)WS+M6~zMz6QYofN;_intYm6QG$4Xr@(gDE37>8vgjuz4kqm#ws|G
z7NkK0O`;8ywIl|BYmDBJ2?pBwAh{x8@UqpnswBJOVzt{um+n|EUiHtV8PIoVlALSi
zENQ{4u}T(vSujXDy&)NUP*>G6)g->^h^I<2zn#wa%%ftgvmaq3y052J9LYX%u-Ra+
z12$yUM~Gw(bPF$wY`<1%jnDw`qX81@UrMZu%N9kajeEU4gz(yzr)LY^*{TvRu)RIM
zFLS*&_8sT-7b^VZg0r?e*XpYPg|NzEygZ-~W;Rj=6vCw34So}G=mGttwrP2~^*Nr*
zi2|Pmzrjx@ic(S?D08HMfShe*Moy7We-;4=eD2<Eqpqw}a%7Ay{$e9)v@>aS4)m#u
zY;rPDnto*}XYu8EJ`3A-o%Z&n_Xs_7_p3^r>-@aEOi4;Sj#&h^zB`m?E9t3yH~~yI
z6ggOq3y;=WwYO)6M6u3;2fMEuNk<xc7bX|1lc{q1s;i^%ozI;%>r8ss!s|3YO9Phy
zT@w0f_V%e=q)F~T@8g_`3gx7d`z-=Qo%fa}b#oYYhnh`pF0|w@o$^FVe&%a{E~z)^
z0tHXWa?)SHq5oU=5lc&gU{A~Rr@zHxb+_oyQ^u$)g1{4regi<}%rbwcx?b059QCwF
zSj%l}?t`buu!o=*$hha-EzcODmrYvo;8vQ9n&z1X9#B;}%_5lHwLRg|;iS@w-wNC<
z6kCb>nLGoEefiOF)F>x~9H+Gb@k`0tVA+|e<(A9OrNkt+3!a_d%Qs02d3}Ib-DSpR
zZ6xW$p;mV_;dRg&FnnfS>44@P+nqzab017;HWx9NyMzGf<CC;BfM;v(I+Ff7B;wB*
zG5<ei@BX?DPDBahWat5PSPP(TwOI=7AL5a$>f^r8ZP?e#{c^?sfyz5lqBS>J(9|Z}
z(*FGo3@b`5+-JB)_$-D>?&NcHwzI72$2u@kb!}W!x?>YF56dxx!NYB)W1eaJwqRF+
zp!$;m*p2ja#33<P6$O;J*lu;+XWoJ>!lzPnYq2*lb?uxq^@`Ec{qXCh;$N<RUb@yv
zSF{QQan;#^Rh%#{K<d74P0kR}Puq62sZX-j88M;>h#jQ^#&^s=L8D)hBs#dVW4|-t
zs6Y$f<z4a30K_P3xku0b=F=ofTT&;dq4#iqI#iObyzR=WZ_DeUOr3v?{$;j5QHyjp
zMD;`)mD@Fny^5}Tj<yfB$sWO*ev}>hF-~VFUfdFusJ_KYOyc(PzLx2At*;V?s|-7f
z;w;a+#M@q4d#gG*#@EysI5rvHWwDj8t+YamQ&v{;)!%w%Fh7uOdVL}BgLLnlD7&^w
zuJ7?9^uimj3kxlfsp(_wQ5Rg?*cFNfr%jC-hcITXEe)a@NlRf-&820MN~}sPr=7*_
zh*uM}Y4}Fp!>wIs*}4b2(UWD8Adp#%1Bl`D4TiCG0iEIjFk^IGwmcI@Vl5=o6SsIe
zG!xbX!VwjvL|l2?(a=Px1m_<U&Op`!Z^bC}udIj1%dXNuXa!J|1q?SmUkrQXIOdpC
zn{ZSXW8VXw5*3{DiQn;)=t|OGJ^1oW#{EbuRIR_zs0pl(pDR)@n7@p%C|rbJGT8Lu
zotUgpGa@`97^qY^(c8zjXve?2!@kK^QO}J66wj<4DUZIDcjJ+pC*!1hMLJ@hZqsDs
zSwdYT<k?4vkwToPImFgZ4ZHv|G(0hM-Yejkn311^rf-bT9}myFPu3LrC}Hdxa*-za
z*erul8}qIYw(fcALYn@7xc<E~7iS(r+9wZtU5vua^!I6_onn{w=2k+s^OjGE&viVd
zyQs{3%u#V_l!d>BRtz}e`+`mgB_21VWaKmn|E&1P*8Zmd>Ko7wPo?JotOU`O+jVd~
zmy+WOvOfpQ0|c}m75K5g6ztJt4XxW#|FJP$HS6bS4JqSq--c<1>@Bpq-)a+aE1pHq
zsNPh*6QQJMtmGheQ;AqF#|ru-G1yLI&NYM1TKCE)GJT;ZhPk5AuXWzOptMZu`na4L
zS^gc7-tM?&Yp@X$c9&s+@Ms!Olhx+o8#`21u&O;a1z*X(>kG}_7kP&x^LRBsG@*~-
zD}=i|kB{lNR&g(h>B2K*l>yILnF#5ggJS+8GpuRm2CcCnR%2Qvb6~749hL-mUv718
zg}3ZbQ<JWcACe2MVc6tTULv*~#U{#D4NerhPs60BR<LT`LbqKuCd5Yu^eo)op&IyG
zk0N1N)@}ysg*EXXzg8(fPJK<mphYANvYhnN4X4+8b3@h1DYnk?TPtlY1Ct={B(Rgj
z{A#LcE(56{*FChC)aS<YE%rQ|jjEl6s@w|K<i%s^l9g3JsTWtt%~Q+lB=-7uqA}b@
zHe-=9LMxH#?q<3#N317=rik7=0XcEc63y_9F@xx6zQ;cuEBB`I)=X&AF0K5_$4`g6
zyco!=OAJV8N6F7<g0Kr>-5W{^&i)k@$B9{I>=WuV1JVI+_GWj)Qm^HYx{ryvHtmXz
z=^whw8Df1gZTB#(O2osTBMZR{SM=!2AbsbyqFytlwXLGc(n*=+f{mRq_D46=f-Oet
z&&#^?Ub_5gUb&sFU~2F4OO0o5iJ)O+?29jGc+m?T8CQI+jsuzLOqjdbG1EVGgxAWd
zA0LIfFn1*^q5_$fdjy_dfATQ^Iyo!2WJhwx|D7ky*0>)Y0!axUJ5t&UqDAvTcv9>*
zwhuNL3Cw;mWk4Zb(?sI)@SCi=9j8i@3O_}ynu?stGRiqlHj09%+T6LBuU7S}B>a`f
zdx5xL-e_>vKJ!T04QX}#697#&Wd0xg!GFr8_6NxUN{)^I5LY%R0042uDrPSLAWnq`
z4FXJcdz#aiJ4U9g64Dum-^OmT%)GA;wJHBB^rl=OZ7TZ9>lc9>FE)+cOe1m=O$1&R
z*fL-bpq>+N+cX)CZ(pr5TPSM^TE+HYVs$Wtob#2TSvS??ZhVUuTc!muiLEk<-PFBc
z<}v92+X8~kXz(%tD9g5O!Fyqf6OZ{*yk$ME!2;J5=PJ}demX+pU$r>mrzTE<I`ror
z{RBxAhNYI23?DT91WCE?e<cJtl&Db#3FN#}9j;j2IQ5B7>_q|RMGvR(-;zrVzjrY;
zg!~>Yn)=%m1{8LN@Q^7I=L|TcgMUl1I~c;3{O!B`M;|l%-o|vm_j}8(&+et4pzcCi
z!sXt6$ZWysmG6Kv*bc(~x4{H|_!w({w1M?@U(8_t&PiV`iIYtf@1W|drdl;_mAcQz
z9^tOUA^CP%HY_y}Q6m_F9YOCCu2i)y(0b@w0uIBY<*mKM=}Z^%7~ypod&-d1i_2Bg
z>kK<9K#I%OmWu##(Qm6xx?%T6Q}>}*pV=4bYTeMv+EaM*!irN9H6CV-VQi6syx*EY
z1`>$46=Hnyi3E8(1?nnmOLv+{?0~IQ0V+V~kdqn}wvzzRz}j-gn}fND4gP{}&0#h&
zU9v*VrAclNE?qwf%TA4k>ga{0n=edxzI!f))vkxP9zp}tb5CzYS-_-_R!h6P64vlw
zKv2N07YkgEdhYKJ{t5&F_mq*09-rG<<d{bf=QK1$I$JNMKN1V{p{8}eL~*M%uN!%j
zeRFTKJ$Z|89djkWXpYBm?v>P&x!x#$T4N=Vmis(!VjN@qDh@@r>o7_LSy{5&S-9J=
z_TB;k-#TNsc~uvBveZ10M(EC<f}o_aA2^C#Qp%fXZ0h|SYZqfzL@Aj};<E=x&&G%4
zUY&Klb2!)&$}F)DHs~>ezw;3s*F1m_FU_h0EBa1W49Ym#%^o+n<QJ_5{b1L1#E*UF
zUe%wXP})b^>w~KT>sQ&m-wJOuYo1j??o$=bE`J9kt3Y~)R8Zq3VD!VQlwgU$OE$Mc
zH{ZANa3u`Ja)Qa8(8u(7NDDB(IbspqpY00Se>4#LN%z%F@4P~2PlHiC!gBPCQwT+w
zD_BrC91u4?pmDUXrT_;Uxd4~4@g5EczWe9Ma|v{brVUEtqcOxg;(8_rj=$bR^-W7e
zSnWkZa5pctj>?wX>*lbKjqAl$<M}~(1;epn3H??>;jeCE=vP(CpJpCd1JO}U?YmQY
z9lz3DdB^aK5`?-Jbsx;WUJxyInt8w=U;C(#CjHjbUIC~d9<)$i_aJ)8qm(~2fgAT^
zj{Xt_rF|IaCXxLI&*t;icldY7_cB7^W$<@$!fKMAoY|7~-m{+zf6}*y)A1)Wg$TZh
z??H7dXf2YTA>a8t?lKqG>S%y`a0N8LZhl|OjM8Qx)Jyi14wKm08n?(3LqJ4gUVGqf
z3^Pq1It&&5Z7j*=+q%+fBTPT%14t6r$|X<YOP&l)jYboGta#P4)I-R&`1#7@uwvfr
zfb#LnMCG<g4o@wN0{Y0EG}=00!+souZ^@#;OIN06+CIIK2i=bF8<avOyjU^0L$Yc*
zch7=9a!V*6Tx@G6&LH`TpL?jEX4an9!B_@TzlmzOG`TeQ35`a$J|a<Pt=v1k4A|Yi
z^Y{GUIu~eXpDS3}3+?wt-KR^8(Pt9Kd=sN03nKB_+nriy+ryh-dkO%l@{>0+3PVYJ
z8T8*(KRCXBP08#O`hiLCT&b*$v;@sI+0u`#ay3ng9}iRx!)@D*6)Fw1uiTqv(Aesp
zymwQc=B6tmoA77$eSE_*gx*)1yfjs|;m7VV?3mvk=iN<sgU$+)Yl`p;x5C98o*du-
z4afF^cp2b$!6p&9ez!ONK5oaj!x;R#T+=Nqn4pvd#{sHa<!ChC76?10R645x6j<$N
zX~27*S`qYq_ul`JV94J`DxY5dUAk$b`+N9`&ikJrvQ>+W9UWf^m&XclDbCnG`+;+2
zWRy1r0TBY0k<*Yzn4L)KZSb?SL*7sa)|`QvNvDqPd7il@D~ffLQV;Zn{=ag$uE1?H
z-;FQOHd1a>y;k5TM15b-si<}!nhbe_Nto^r47*(Zf>C+TL9h>E%K5$wTZTFuf8%AA
zu-P^ctpr{PtcrCER=dIzf8*LRFgEo~D$8l#G>t@0-tuw2{A@-lw3VP&RcLt_`BwiY
zNG8Eh2X6=u5j+eBbW63qB)~FX=Ra8dYtyHz6k4xsvY0`+;kA>?pJ-}wUXzbmUXFyc
z?)`-%)KfF_B<e#1l*ezTi5|Y()|~9yb!_hn5&j9<?GvtOEM+kN2}&2D@~+|27n<SJ
zB(L=9ISN_8C#ER9$*_p-**i_Ds?`nI<*zzv;u%cD8!e^$*6V0bxZaTkdYx&{n|(TB
z9tyK6<XPld{Nx|q5JRG`p}@|sgBKrh`4DT7FJ~`7?M#STH21K8Mn6!Bmj+9j#zJ}i
z)cJthX}Y&uKXA)sFI6u3U24q1v0e{s-zajx2hzLBt0&zKyHT0BfM^g+8Ck&r3jrF<
zS^8(z!=-usmb__Am@6@0V$M<!y37Tbm`&p2(D#Y`X$(%ffN&$r9FKIj(RxOSsvty)
zW1?o#o$?PK_G^@Tf<RTh1$ys@v}63@G8>-PNpza)_?FORg>KYBbjr?TIONRZUX*zm
zBbn7zTzvJi=KM5^2M3HeyEvq=4Ef<CdLwIr(|i^(7Fo<%PaBz0QE;;Bi(&~6)Yv#G
zb_wLbVrk;A4b^PevyXs(vulp9U-~X4kIu$tFPcgtKFMmM*jYQLA0=DVeKvfRAR4>+
zoXa-sY01jn$FD7j;sTi$LrzGR5L=$b9ydRiErRR%?MPYsCBt+?Z7aE6d`Fh6x(mJ#
z2qgYURKtDh{Z`h{W!9m3!PTmme+Lj|m*V7NYPa+8V;ypgI@vkF&GWDxZ{#=xHZF6l
zd$%Cu1knO1=PO3tSsj*zTD$rlx!)`g`y{g6doG9l8P&Q2^Q`>&I^OE@-_`%MgcUlb
zZ|oY-|EWJHp+S2Np1iN8v|lMM+Wx{EsVn0Sc<RruSpgK-PCD@Oe?PwcAAk2>=<o%w
z`%7wNxj!0dhMK%fJOW)RpT*n@UoE>pu7RrZzEoo$he)EnWo&02k(&V2^&1V>5CmxO
zR6y$>voZaR!Wu9OP<6<Kini|tPan4<R~=1q%dAfyYP8$DOyWN$UtXl%8}+n&OObqH
zOkw~JM(gCZA&9>d*vjn1SV@U(MM(F}c*<j5l-bU1^;hvwO|lUgF~AdN)zY30?yF9o
zMr>$mYGP&TTx@=V6r?SA1Lfm~TNLzjJmJB+$5ydhQNYF8CmFr)kG;_EVoP8CGn?R%
z5BpCLcOT@e0dNH<WBnxmzerg-_ct}|Z;zW~s6L)0L15P9n+~@1Ql2u+#V)c*@80$3
z607hnJz;DX%}AR0!t2!R7dnB2R86#Q+F$YkS7M6JU3V2e_b||%OHdOg6!ON2K0%QC
zs@AjXG&feFq55q}A+XCrRB+V9^T;v0==iC~M9pF9t>mY@Wdg4GbznrpT?lf`Tb%)j
z_bIcCT)_sRBb6KKYH+&wmUWs9&h=koWNn28R4lCe?E1(qq#mr$Xd&KqpmP1&pSC^4
zew>Dj?th18R?F(hy0N4SB(~j7K0)-`v;t7J{Fh(QWjDq>#76cqDoxX9bY*YLhRb@T
z?Yd<%iqR2N#wHLJRz@`6B>HD|<~)e)LI?zdT2z-?##Pm3B>V&&uU(m5&?g6+UV~RI
z*6$uY!~%9Evaa^-Ls<4)4%XXzc|UxDIOD3vi#Ez)J_n_p%IJN)9X8>>+~`#X=ddT^
zs+)L0;0UpYe8V$_I_fy%*CN#3c_~Z*awEL@i6R8&cv$u4qB($&Ia7w<{4j*9f~BWQ
zB>_Gjw8QjBScrktB(G4BjCb$M8`#>Pcm17=?jOJ6|L~k>9ZE+a@(^G8d{SX6wKbh(
zO5jWOyfph0M0yfI-m~}daf9J`u;X~6ECv`7_Y=hFH+bv>t8^EZjJ;ey1yy$fX<wZz
z>-5x8{lpa*DO6Y!+~N23G1M{|V__PjDv+A`u@PyniEdcl8|lV|9lv$j+7pXg?s<Hc
zozS#osJ6c^Cy4dNDL;e7ghJ@Ls3Tn*=yt>wETcnBiP@6S<a;W{OfmK6w<?bTK&wNR
zE*q`-X*ofXvLxH=k`giPe4dZ-S+uOwvIon48|{pTp1x6^F4Oz+>XVy!L;3B`3-I5;
zUj<gee0pRTE<r5Lyq4AKs%{#ph%=}xSqmXl^Ek{jTV!P}mj@inzg8SNgSQ$Bm^dqU
zM)1xXd8zwU#_Su+=psJhbK40VrcTx}(Z=hF6`^M@@#%?EGuj&)f`fP}OjDbm<en{I
z=q#~7j&aNVt;i8rksDrnV$sZYs|Bp!b;0i}u;W`N;Fk0R+>)mwYkv0)-l@{96tV*G
zb%J^Ae+A$FGxvW;xKQB4zn9`QuxSXNM~!+{SiB`OvL9|KcHZ!J-~p?)&*ooH(`bKD
zZ06&M;hh>eSRnbn&@dq@FJmaA!lk%3<^PV~VW#2z^QvCGJ`yHGZX-KZU<5Vc?*gaR
zulhjs_MBuQv1cirfyzz&${jb0nV5@P)F8wvoTWNzW72A++S7Xk+aN=rp7$9nfMm+@
zpax4!jbX-sN4u$GWTyh3Y%~z^ORzkG`yWoCzr76q2IA$vB0&sl&xdUADw!KB8d)^c
zw{bX6VLOvq<l4gxoe6>a%*0{-G&x3@`BGPM7%c)*G``~x&e9M(SYDjGM`W{1#pqW<
zBP3r2TjC|5bDmjFwmDY^o~khWzEPRA&}@iOni|^!1|iU&Z&v@fInRU*%2wiyA00dr
zL?YdlLk5`^Y`JEF#2EAy@u{SyoFW%}aj1oGTe)jkekBZh@`=MjGmllNW;_?KStPHX
z+k~#sBxt*PG2~Q(|MS;!hm$@=45JV}mmNZ1zNd7w{_ev@pVN9k%JHXy1N(Vg4&tod
z<>_MuKR>@khUleT`biJ|HktIg{GQdx$$KL`fm(xaT1A$KETnaqnJvyYz$SV)D}nrW
zEE>(1;dnMPr-_8)_9<-bi{9zO*F(=6@2DiGNp_927N?i`W>)<K@vaRmT25s<`80kO
z-ignw>)9sQ_lN_B4FKc(1a;2y{f6U+of6#AABi(ON~d+Fdyv<f*VCHEUU;DPr>uWH
zkWzlYA*dn8r&MUfw=LQ>Bg&a-SIu)m(RPu^+*;fp;#wY=+=xSblin2%Gm_1v$u_a%
zX}CMKXMmqD-Or%et5MMzxW&?In^(2DtG0G<BKHexTv6Kk^jC4#f8<_&{u&T$ztm5+
zuRON3+%#f0f=r%&+(w-h-Ocs-J`->wK5rn=>GK3zh1z{9!($Lpcl%u~Tx7m?Onz3w
zWA3ZUd2(tM9-`&}{rf*!hY<$lR|D1i8-yG14N^_&1J8{N=&x&hzLBFYY+q4-=2m4C
z`9V&O(BsBIfhOcEAt+z@X=K2HEr$=8>202@%ROxmTXquYoU&+X6Tg((D=Ut>hiz(e
z3%8lLxd9v-=U=@VmT^)!X%l2y#|C$Tn1HhN3YxK9bVgJw$Y25}t5K|2+Y2XF!rZ78
zE%K`9J;Gt&T%f+3j-~HwR2Jte5BU}Yn%B3Vm1j`@kgS!<;Gl_xF&S+!=y|7?tO)pT
zsj*SULK|LqG;y|$>adKZJy$Z;v(v$s#wW)bM+}zq5xAFSH?I{&I3OL4{pV~DbM{C-
z<mVqYejf0)XYf(LDHYF27&8P;`wITR<autn4+UL!w9J%;`E_iZ&NC8+9XAhtL{vgh
z%0731$-?Jbfgz=Ql|MmCf>DiUk&TyLF;+1?!AkNZc(Uj{6D|1^cID31$GqeL=?wP(
zaVo>nZCQ&Bg{D{80M;-GLB`>CH<_PSW$}LBJt<qi3polzE39TatLLkma$8CsUnynO
zP+s|HzK$S1JY1@}-4rTEJuW<LC97n@B1nI2<FVaTv#d~x3fp0)%qh!3B&}ZyoT8#|
zG`-(7?Hq5|)yfe>KxY@vE}u(usS_X)r3Z9W-Pl+H*$@heC<B}_kfk&2M>BPUgyE`)
zeYkp^n<v<D-x6FFJ;HV-!rJLYx2lsFvmX|FGKECn?};0<?v*Pz!bNTKw;Sj-{mN!}
z<pw1C0NE^wWFmd*%+ng;0Vgcq5ONqs{N+T0%2LH|_0}f&Mjt>P6|DT_VSLojWNv4t
zZZl0^B)hXrLd`j;y)+njt+sJk;*3F*?n=9tTb3i`$eY20qL(_zOSfwL*O|GCeI+?N
zW<O^>KLV<bWIP*O>onn_*GM}e8Fq?`G|H@(*Tj<Glz&t?&bC?R%Lb<kch<nf*!=G(
zCFO6<dLI_SeASlmjI_!GCX8?^_eCo+^vpX4nmMou2}v$7=L5kXUkE&A@gv6-dvTnE
z*(P8{fZ0Bryq<R-fR^RnXEvHOp5^#hem0)fpSL<S&zY+a2Lk?Q9>qM|aOmwd>*n$&
zmGu{d5m?#?IrjKGAbq*uRMp2Q?x~(D%VFrm&Z7@z)nhOPh+>oWcNW1;V1>QXq>`vt
z^cB~MaQfICovl$YxpGm2jCuD>#(l$D_%_vIC{lSX<w<g<A|SRFHWNvW?zx8?G$z~J
z?C^@)pDisccceY3^K+A4O*FmQz<&(gyL&BU<=|hhj@e-!g&Y{c{>UQNGVkQszFdz(
z%+`QoTK2LuBpRWSAWCB}-<xgjTQq`?P#G%Jz++y`4FNln-sAWGP$&Lo)BeZT{{7m;
zU+v?+Rm%vpkU*fy?i+4w3VN_To!KmSbyR3hxNlEJF_B`wc}qK0C?w2*`d$mMgP+7%
zFy53epadJ+>{XM{xxFH#JFa)D=uvFf-g8BH6MUbLr3E#yhDXcb0>cRor;GL4T}<4*
zn(cwpd@GVx(9e+x<b+l(2pL}4pnDpNYnB&}Cu;oGMin?XwPI}{{ajH~3a`*lgXw~H
z2>ZlBG#4o`D^o@M{5uO!^&Bh=$l<msNR}(MMp#Q4q`htQ4c{VjCazQ64IdS&BqcVx
z<Zw^Iu1P%ZxgjtM4AR8Sdpn2GrSH#hJwHJm^Ea9>RlyZ^-L+%;8k^E`V<|hd`QI9~
zM-|*G5OXLXd373aG>6Ul3F6=&9@&A^dFP+l{{;D&*cKl~6^i09FJJ?}7NZFyFsAxE
ziqx%DpN0li8bhS#98~rg11WFz4qFoYZCZ0m_${<fzVhUFtS8+eY*y0LS_wVcKci{E
zRx#*727roMFWU$(54A7EnF2mmPD7t_zxq*f1S&lxyDvpTW#2Q;IpZMcykMIcK7>v-
zzh$b9Eg6VGLUi7GmYmeP->&N(mtwzuX61)^+dAB$7041&{vrk{I#CS(Bz>;G!$<xR
zFZtbby*~KmCM#R-uG6vO*Nt@6RR*1(_$G5Egve||49x<sU8E&4gO-b5V19XPugAF2
zFn&o~wIuCA=J*pYMZr4wh%XJvodLA|73L+!*wXK{Ra5YkAy-QnX5=VwsFBX|R3RHa
zJ%7VWs_Rwt&6!)BZQygH-Ms#yefO9rnGL@BQTX*542D<Of|p1-DK^udoE)pu1lh+`
zb_h2@ApE2s%c`d0AB5%E_+Tb734<A>9g(ef6_?wmk3Pz3E*pN)uOOq3s}N~wpiSUS
zn+gpOH~F9;Sps#hpBO_j2leL5iag!r$LMU}@(^X59e4m=4)y4Y#H7mW3}>EL3&#*D
z{NPS%PCyHj2)Xt<gnzwW0lnGkC(@wS@7~5DS>YD_{>k#=k)`TrExHGRt~``wY%V==
z72k#o9%TP$w35t-ejN94m(di7SHa!H3u2&Cgqg)~5gKl{!L3mk+fJvu=LM;=q423L
z=}rw#PFlY<DA{2lkN==r(>GrI^C@Xyt>_gKe@WUq(GE^D<kU}{!vc0kfx$$Qeb=Y7
z5w{aHEbVKLWKa<u+Ev2()@y@t6$mW9yZ`!#g*tzG@?pY9w-fF8B)gZ;Buf_<n|Z0&
zMYb1Fo}DzJfXQd>C#WD8n4G3b_>QQsLTF@MUOO1eGQy8S*FH?JEwf)R*G6mZD>chN
zx0o@8^3~P{3-nejK3h2~hDHx|_ynUIRGX>ANH|){7>|6;vdV>DDweCw-81a|!QL{y
zz3Ok$yGwSv-v18kH`?fC?#Lk)!MQffk!v89uZh9_E{hDL5<}Ni{?>dWvcF$Zbegrz
zV5p)VJzwR`TgR=OFfBG`*iA}R)&GY43)kYdPlV!qHf;=lXH#$^6%K;u;kXnjkJH%Y
z#;HB3t=e~6cAMLgGId)2rMW(63^}!dgq+L?I#=RwgXkuS0G!6#<f9}jd^tp-2~`By
z&;=4iBY`u#7zv+_ZBw_W&3H6snwlrZV|}@x@(xj`zoPe|<u|<rxodzXb7_;OY{JEx
z>eUd>W!}atn?%~e*Y{qOkdjcg-F46`c9NOc!l04HBNkt4(0l>(blbB>x;J3g+b*)2
zzZXPsSyGCL(GZ86`ar_4*i8ePozSyuyunCwyI0h5F->beS-FI7^Q86V5!Mwqip*@*
z=9k7F{ODneqpUw%OmD2$T}#ARwoG@_sgu0l;#PS1)=2fY1j1PB|DHf-JO)sw=4fZa
zTc9D#pNC^Mmf)y)I5qsc#8M=l9H?qnynxFQ?vfsFZPip~LHHqo73BgP0{Qd9slHF(
z(UIBMP}K?i!dAb=;N!ioYrZik-l^1Ouu7)`oZEn*ZvoySEu7s@4TEqZFJviqGUW~v
zmtM16)pZmL<a$BJa9?)OrNjw3$+^TIBvBYFE4qC5hH%JRwXXD~-X(>rY8RD<npuO?
z*QH4!0)C+9Z<#aLvIIj*N};K?*vO3}vZlEuKWtOqL9$?vgy>WmP(3GnJ#SqbfPGJT
ze3m)$OrPtC`wd04%nw}7$D}JSRvb0;(N){rD5m0F!~rXE6Mjd#yR@pJLBvzqtZEje
z{Di_W>`jz1;&U4|ElN*yzs#5ZBS5Y5V@Zz8%<V0WY1S=|IR+IxBr7nGC1487!Bhpi
zVe@5cFCVbaOx+yI9#a<b0&7|lQOrH&yUGmEzpDui*i2)VF2}*F>QnHsG+RD~dj3xi
zVEDn0RtT1=vwYdx;~3_He%(900oN3xWeQEBn5ua!8!!4^umd3eaw&gNVeVvGRTwkt
zovP}Js9Gl_-t(e&J@^p_hAr(ur{0pGcBQrLlgZTtqp-k(j5&DsL5N`NFhK|lepY4u
z5ySr0sz@s|U!l$tgf6w^ah_n>{0?(g!CZ+)oHR`(r=@4Dn)cy%Th+?$bh^ccEfN$P
zWS-UEg?S4^#~+nx?%m;6EC%m)6uK({z@m{N&mV<zRYRvT6cU%*+qb=!%=fClj7*iE
zUE_Z#dF#>TH&>^1`Bs$$4BWUcz_^zOgGB~1K4T$@XFD}Z%)?VJRnAEz^wHk{2hMFh
zSZ#dL_(HnF_bz(3MK^=G{&E)9slUG4hKshmn!7(LW?03)zsk8xayClpg~J87Aa<;S
zQ@2ftw+%U;(eYyf?Y6(>`}m9&#ZT?)%(8EQIli$?5Xo>wtPJn`0wCa5GA4=ssLlQ-
ztnOst@4fTB86@!TeHsAIh5w+F;|y<FxepMoRgi<~jIwgzuTbawB_jHNXJq9cS^yE(
z&I;vN)i997^ePi`>!W_-F1s8PCb27G9cjHGuM^$g_l9I&((lqBJ)9OSq<EiBSAUEC
zI8<{-GE4WVfMSum#(F3Gt7iK%iKcU!DCRIn==YN0)&Nkyy<noSfBRQ`=738eqD;0J
z*o@ybkMUoip=%`VyeI$~UQ`I>0YJkm(t8X5Xs9#ie{n0F2oZ}{<T*=#!@xBw&x-Nl
z1>Gat{yY&9BRkJy7%ts;Oc`=di^zloM3N6+^uh4ybgnOG^{lmpM>C$jU4}uLa|pqV
z1WA5@c1|#{x!~4U&vSkIy*k$svlGTcjYtjGO}7TN=(DnDjIXpqVY{<&&h1YO8x4as
z#z1(Q<-oiMM*Sx!yk0$jvo<x|{1Cq_n`9yILH61X&%4L(9>>1Z0_%NG{8KP}3=rJ>
zzb^B9j}0;NmQ1yt8l1|l11etLg)0>&C0&|9C(pAB)|tZ1B}WYpk8}3Dd@_JYY{5P5
zBdtG@)%?-xMZ2^fq3Y=LV9&yh<LF82%HTD1#~Uc6`^_sc${<SMMt847X=$M|gU09T
z5MRxxooUUO>cxtor8tUUq#e0qkMdse2h!&0dq@+_P^{o$P(;(KZ98N~6P<ZV+x#UQ
z-<iyKbCLZ`o7X1c$fU85AZs4t>Pbd{`<i)3C#B%r!#=aK_zPpZqSr+bgXWEuR`~T0
z><m%c?NqbTf`jm*Ljbxf+%xzv9oVoOIU{OVh29M;pUI%YUc582J)ON_EH&BkCO(iM
zRZvlP?is$n`l}YZTn+X<TDuKG>;sWZL{0JvJMyQv!rgk0rFbl@+yIK5jQk@=Ohc?s
z^Dzoe@xzz`eHIa%`K;sf58cNnT80ngbl6pR_EdOc+GBtpDiZm|fnaeRYatFgjsgE^
zLHBk3F#^RWyh=NpTEr6Dqr#=n)4s9^GuC?sKEJ}w>o5OZEb4Ep+CQ>vfAsuQ<JZd3
ztrDSK;ibCr>hMp=^DZ=Od2N(Mx&EX)mpr&eN43>+U0B)l@Hc3JAO~C?P#{HTd;zcE
z#JKSFxRvU`(b5L=VtdRWx9aAF!_iy*T|5FjpR}A?^FY*~a}6;eySYir*5Hw1-XDBT
z+MlJ19?G$V`%LZIRp5y5Vq+3_N{VwQ$~|~yDtmhmS+1i$vVVRycfu8$AR8M-N!0BT
zN+3gK&dz698Q*Riu_b;XYY-k|)D7hqVrSvHC>dtU&y~9W=7@@6<H?fjbbO5?jm8l?
z+0sn>xs7B2!HZzouGfUt1cqRHyt?X&qIs`XnZ3Q*v)&?-E}PYJwqk#S-$64j53s&~
z*5>?rGdN|%&%e^;5G)(tS?okIRp@71yR+v~II^@mkU?&-ORt}6+|6vLmttG5HoUGo
zwE%<vki<(!5+>V`*buS95Fp;q&&#jx=(Ms1rwpT7raP9q`mIRhWt((IIi@#CCNFG@
zi;S1eT;=6#I&5p>M6*ke0)@a{=h3T8R<ne8p?S+oAI?~aSuf3zzi^_gQEz)KZx+Vp
zt8XmIqE{T=In<?VTU)*?Y_~PHQ*u0^w*iM?P2cB2uRf1{{K_C_ov8^+@eSZAu;&lI
z{Lwe?pHa#Gg<|pl874#o@C@5HKpIM?`L0s#MucxH)^2${c!7ck2xhi@QyznA2Tb*K
zr>T1DW2-Bt;j_nj+C9#$;PTQz@kCGd2>lO{z}J%J5K#<Y{0O}Qslcm!#-8haG-D06
zKctpBMCR_q#*?f7_3*QR^~JCp56vyiSv=sMDr@DOfna_dtxg^P1T8OH;~;bEXYTRa
zXFKB1dh~H6at~s!@2s!-Sk2F9dw<R=>#&izb*xx5f+f2~M}-Gb`ovBBZ2MGvNApjJ
zbBsfQA)=ch?W3J~>DS8YvuEItO?g}v*}EM7Pm~f2RxLb@P{gl&J89#0P2nHPzVhrv
zxkiflYirZNuxGS637)Cf6hp-N`aD1&75*GD$*s53s$0qR;AP8gLEA-Gs67q#fgt=c
zwsXS~R30SWWjAv~?;5sat#&SxOa6RMS1)H2CbA1~Xu>ez8E^ckRk-xORjkzxl^dE0
z2EEXCmuInKjwM2FlMToY+MJ~WSx#^opDU%MW1`nW^)Pwi_-(C(Tg}fwL(RFbK^J^l
zrM_6M>QvIywwW{el+boqZ0vh=Ch9e$0_eG?-D&X<mW0LQLlBZ%h(k~0Rf@B4)Aj-q
z&_WS4ZoUR+s%x_wwH3SJeqs+L5OhWZJfW)TCOnTBOoEa+-Q?U}f2HiqZy^C|Z<1dx
zVRJKq-pn7=|6Z<Q%~UdE#CR~c!e$4iP11G6(A9aJ0dsi^HvrzuGxwjZ!2jLXh%lnt
zWwdDbu8hER$%gjYr`F_2FV^4(h6ouAYR34qbNJb)sV?F(u<?bTpbL!Xh3R@m6wM{y
zT>D8^G8?(UGB6%{ygQdJd6vS|GC;^;!$}1ICmdE4&zlMJQ<{R87rm(8liJ<GcaI(I
zG!ICbN;7w<o>Fdm+vE}%+0`r&)TI&Cgf_H2<MvI4JgI^*yjcdS7dE88fX+SmLr(v{
zezfG@62iY83npSSmNy~q2%rgBZJ%@-7TcO=6C{nylLs9f555}hbk}{kHy7J1F{%fo
zMxeDsLpi4jydyqD=6ts0X;r5|oL;LV$11%6dk<Gji|+5E6`wS0#R_|VmSKXI)i<?P
z;f{=L4obJUJ703Eecl~{>z-r?Ef~LQl)~KY7%ddZE#5c|7c9E8rWP6u<q_c8=$jRY
z$OY+r%iw4-uUmMNLG5#ODNSTY<<OgPuc8ilB~Hk>EYaTs+bfVU$0$^DjgDSi;@R0-
zzf15|ljRe<N#2ZSvQp63BYR2N?1>_|M|Ka2S4*Cw0tA_qH=&L`nrNzw4jK?F{YoQT
zfJ$|?v&<wi_WIS#SfaSfuBWy&pzW5$<TF9=5@(*pZHMxV9h99*-Cf~5jiX(=E`WQq
zeA}s=qdtABN}{PG6bh9el%vD06RzmBrBSro(nL#aaW;jCj9untTA7xTLN>74sem2R
z0Si<#+!TI!0W06x$d9!xCf&ABG5^5jnbqg+@Fm1!f_a}zha`}Tlnmsr?&}JTv^;S_
zij&82qaj`ploz7TKKg91&9h)YtoB*qyl#0@dlmfd#4oLVqsQrsKRPe}{`GxmSO!P^
zU9`n8!zgib;8&NhqiOSe)@~4Lg?~r!neCjv*S7BT<VGtBw*0Dq$>Wa7R&-Bj$G7I8
z1n$ccOu{oV3(VN%2Uzb+NyHMn;$6uWqnC8j7DC%YI!~{dwn&4}WZ%g+#TEUo5~gK3
zb4@?>%HBdxk9)-E+a(5*e}UY3{J~;+-ZG9!vCb5EzG4Q7f^OlK5YwusBV7yh65td#
z4H7afben$Of9V`#HEE6zNnsmyj>wb#N_TJfg5~#Gjx<z*Bs*ATWZR`dj(uJyCr*&^
z@oFf0-bbPs6=`-AiDQR|%Rw%m$DZry)!%5^S@87SY6Nn2W26__dusv`NQ929w-)p^
zSI_3^s!G=zneo@rgRPo09XTHM>LEi9tzzfJZq(EXRtF{DBXUlB2^LwRzuwI5O0U;9
zR`{fTPPNod)9o0NwtnYnZ0g($aP>>M{_)%NpM8$zoJnk}oSxV^E-=>E%dS;eE#5jO
zcCM`4{~RaRif!5;(;bQdF)Es=Slznu#tR-AOM6fmGk-O6+fR;c+A8$0*CY=Wm^U0_
zV`Gi8Fgu+yT3GJo$&%uwnqU`jkN=}LqcRT$!iaW?3Yrfu+-D0p**)T_PhUq-9XM<9
ze4W|6tUwM<puHNxDDd%sH7(_Q-R05bQ`Ln=R1bfHOg=t`Ro~|jl3mZzA#Gal&AlwF
zK@NGwIscnTxH)6O<d1swe=|7X{GZ%YPq=+O3$P3A$Ba*kYSXI<=WmyMk^dkf!s`>2
z7f>kfP#}{^1l5MO+-np9>g7ar^o6(&mBVfe8*ILtiJEi#0yI6B6MDar47M<<cKq2r
zu+)s+B-)%oTxN4Vx%L1jNSOCJm6*iMmZmz*QhtrUQaZ6Z<a3ORENdk35vj!9MGq>W
zXCLs@g+OEoOI)PsKThJSY@al9#;Z}y&=UPHS_Om`I)$3Fx;h2mUiGSt!FTyjbSyU1
zHxA`4oPEKIO0_^E3`p8Vsq1x%3+s$oU3p2SKV9di%ws%uDhjS!6IxyC0fn3*mBxxC
zw)v3SUlIquHoOsek!Wp%sSVU^0fqTflU~dtjB-*gV{_1!MkC`lEJlmCWzy5XDDHJn
z&<b7(2~k|yFuqCcVAf6i1`6?O=ez{fLx0Xk3Fe#b-wtBD^2$tWo4BZBlnWFUW0n9o
zP)i;+pNFU?wc!l8b3>u+ZH@Y#EqKw@Ox)C&8tMvsS53HpAl&dW1?uB+7_#Fl0g$-s
zB!}1kk+S{WbFF#f`?&_1uCB6f<6pSdg!ZK$hSQsWNJ4`^F`B9;dYu_pWQ}mD8;Kmm
zSkvs;u1po_s-_937k6v(zM$<{yLzR3Kw{K&Enr@~x2Lf-^mfWhxpmi43if3^t`X_}
zBY*$S@ukITZ%PbP4Xii|K%ZlF=0p3Z<m#5;+Yp*Sd%OCpi+SebKIS~ZR;4YMk_A1J
z;;$EonJ2M-<hmd)Nye*Bqk=wvq?*!Zu$>Fch}#hJi0r&5y%sY3qi@sn=AwA7e*F)I
z2Nxi&q+oXwHZRd;#8g`w<eX5h?`>rMrn_U2yFtY?BB_}uX;};6&TLX=r^3(rp|a$z
zL1Rn_gyu~q+VjaPQQ%*kyLvWxBANmCc<3G7*-fhx%0hg~6MAgzH{I*@3;=2jdfV^s
zyn#Rs3i>~FV?aZ;#2sp--s7D=LbEHq29WNO+U-5pNR6My-Q->s82%{W;N<1tc--M5
zyhs}P8g56!W-SxQS2J#WSUf$mrm+7(G}hCG_Wc_Y5Pp^t5F=#?8P5p!-JE;QEs8&<
zhpQMp^KQi(v{@b`B$rq0Yd~=B^S=7OmPzih$M2TKUqt2qUe)FQqm|{xMDs3SrXc}F
zU=vv1Kb&w>T8GX)B)U8Y5v%-5&_&%#uDgQEoRdl@5<DLVB%_lhyy`=?hc~CizG<(z
zSKCVGiom?V>Hw42*S8TBrwmYNr)NysGxO0#rdQhwnJ1!y-NzmQS8}9LCgrYv4U9d0
zz{#k$rqq4G&(lX~#1xo84g(v`P`oelxszWDl_h00lhmW`k+o%el|}l4Tk-WxqHDNz
zTIi>W{fzB?^Z<m#hs)NBG$&REJ(}?<OrEv7-){AZD;FEk%c?~=T)j)g*O(jRcrz!|
z!vKiaDyre#AA&UPGz@-5_4R(%OByLwpPKXi@dHbsO3zU6=-NtI#^SBfk(#9m>jPuA
zA=t!OZW~?NuoGf|)a|{ak1v7k=dgu;YIEr;_RiuhotqU>0kDa|wF;$E(ZppSCRf#?
z)=-`Q$DCJg7ElINYu@{#x<@b#P^?8B*axOF*G#z1WLz8OjHkV6p8kM(a`}09pM%GD
z5Kj~lcOdo$PTp#P^x(txwngQbERZN&7w|~`yeH+;n=ua5kMEgZ^|0(de_p6wudQhc
z*1o`kS9e^7hkf2?z2Wn@^xI;T;`SB!w+><^@6YKWyW`64yety?3DQqAi%#Q>=riB?
z?n1EM&epTvwTVP})J5KumC|_nadgw|dxLiczR6qZPL>C}6PVWw1gzo0RR=a(p_E%+
zC0BV9+=#P0HF}AMV@m2Ow}bC@&pPr?aZ)w1T0zM0yw<2)8Ux?tUYZXpPArDO%B(r@
zU!=e<oSQ))kQV5Bk&SIRQqUU=_9zw>UVdg$_2!%s-SyUjyhY&8sX+`9uBQ8UUVdVe
zAjQehVqa7q+86FyV&vR2mYugAePybtf^QO7#?>-`OI$C${vSd|{|Ghxqt}6KXe1+E
zaCfG#&8yRI;jFeYd}SHAC%XcpX&BhD*_WYQx99X~L?PY-K$5O43eM?s7ZaG~C?qo&
zFQnWqsKi_u&l}ttzp|6IpXz@OGXc?W6P#YQd<Wk<x*Rl**qixuIC~@1gKa+C<;WeE
zoCuZQU;(B{-GL0D|GrQ8pJvy8BfO6YiPyv&W&lb^pfkPEs3}P$IothZH7M=I)t8Z1
zpOqY&8BKE8^(?aBqZ{kTeC(IJL-TcwG!4gg0g2?*zC=d4M&+mX$39f+uxo%2c@TU1
zu*!LU%*BjFqiZ{Yh3p*i&iR0)+izz7TD!(Pr79HsEUZ`OwG|fmr>n-5+gGDj8N`Ji
z9A_i!MN1p)p^9~!-U%ZShHnx?3dRlawW1j(ZgK-yc69B`;&Izu&NfHV<elsIP2Bs}
z*SvGrbpMhgAZ7EAB*#7OF*IttSXMz_H*fcb<U*m`l^hkFlzgeeQD7@+ew}AuWLp6i
z3<V=SO14F~tt_h2&UXpiY!ke(=ubw>K`J3ZKVm(1v1>i3^-km#4)+A+N6`RFDzBSK
z7)D*ztdH8_=+&e3FWSms%-r6eOsh75TI&sDptS}Ahv$DZa$@@9b3YsjPupdQF*7Id
z*?zkPqJOF<mT&5BV8*s$Iu3*(C<H8J0AUEwu+FL{x$)k_9_Lu!Eh)^@g!ucJQ#yZ*
zO0PJ;t6Yd|MeQdEo7M0Wu5S@wXtn&ae3XXoA)fM!1ZM-Mr{5E(Z}R3Y`>!CR^o)9d
z_r3S`q99%O_w@>&v6>I-#}2)_w{Fio+~=!~LuY+lbW-5~0Vxn5GA^A5bosobf>oJn
z`Il3zI|l4}Ud>E#D>iy!b8*TG#vU<kEf8{(DvVUH=wO2Vi*|IoKH0fz)Sc#>VDM#@
z3nyj!Hmmsi%Y$rwV{(F{cy@GuU>fQ=m(}i}+Q*kF-wwp&hB1Zl0?~Q1%{7iy4w|0{
zWPXDj3|RKhh$Ee*>BFhwaf-QDJ||V2l1mIK*LVixbT$~I7zJw^cLMbTdp;Jf!O{!$
z!bK*g{u_Jm85LFAWs9OHh$Kk@5())Ga!`_l5(7~{a;As^mgF1@K|r#CfMk&@LXoo+
zIU|yDs3Iw`$ORR=+qZ9z^Ys^Qzu!IGXY}YkKNwW)!GPME=ULC3Yt1#+_|8&FAbfJ^
zTYw|VQ}#wO=^gK^Z?Lr)-`#V5|DLQMV9u0!d`R+KSBp#xG)p$wHHhL6aCd&Ydi;$$
zU9cg~u~oz@qwM5dBZ_~pd!=NZA@k+qvU4xBN;Age23$&1mwvadCscI@c#h`2{r^#5
z@gMyjhJzFwz0#AaY-D`+v0a2$4)hDT;q&<=0qqV><@#?_PeobZy{n6{+1Q-jN6e9Q
zZ#=_i1lTf^HrI)L@fxEKBQJm47`gC<ElKY~RxmHmEA!naM&Ad~hmgS($0NL^l3?-n
z{`>s(l%<c^<ib4TK}?v3o14BuPK6`eYe2rbsdwk%_2<@oKn7n9S_Egpzpa6)D=q8C
zIy(!eioZUUnBuRF7kYG8m66N+KsADsbG_%;gA>2A!O74L>#Y&kb2qcbCWrO-qjcU*
z0ewhBS<CpX)0SF;slFe;9sAmS`Fr>MKgXbBnIMJJ`h(vZ?sFCV=C`-o7qtO?i;48w
z1^8`hNah?(FTihu{0Iw6jDW0OWFN;)JysSl#Mv=})l9mr4V?tZdo}1EOkEMnNs4Jb
zhUfN*O{H1qJb}AHg`W`o*~^SxpReB*fnA+AhqX<gRS|B>^GqVvw)7{C$>UE7vetcB
z_U*i^cU8pCI{>Ru&$_m~a6fQ3ibi~1>ul<jhUoFB=3r^li)13)XG5pyH`J<_o)a33
z0?;!T5x=n?p6j^oz|P(?(>#5UXkLCM+Hkclxxqc0;)^9`X;7HK#D0OzXT3Ox3buUG
z{5K2@QmW}>YH|1?AQ1<jDx61%07aEUy48P$7F7NyP^e?eO$?(sdSWE<*7){nE$t9d
zs->4s<+Il@?;XpeN%=$+)>}A%Q8m`S%kgZ06BWgACsw>Oh3?+7HAw&02!2`L2MYa4
znO(K<JTWP71QZFL^KbV?P`8L+>`>+@wk(g_zROlhC4FBq5#e1w(xas9WYBx%%=ai*
zfoI!I=&2{)nVi|@lCF~sPY4X6=4tnnl`LzRd&pr(P8gmxtG2;%elPVnkl`H?1oXbl
zb=(I!lU3FYms+cOlC}HZ^-GLCu$9(lma^rnfljR-zAheWe=%e~l?MLK=gFymHIHq~
zK(i6R<GR=X*ZMaX{c{S*rgB48MQbBVboqKBp$sc5;fvW0ViEkfFV#@g1=Ck6IZ$%n
z=R_5T6X<{P!cZ)GRcCWuHPy=I&}6-NJ^CPG*Y=QsWM+oLjj-m+`{XAB<2NU3un3GI
zfkh$6m|Pj-OMkI+H30F|vd%JDGs$SlLdw3!5^Zv#JsIFw;;?j0UIb#gnKImH56GqR
zYk3{_&(5TtT+VD6{7i5A$a*8LrM{w~c1)mjdwT~}U0oMz8NPUlj8!@J>gU{G0CY+b
z8O$mAJbfi)F<bm*X1r#FZTDj~WG`?oyJsGX(&v<PYg-0!3lC4_?j{cm&3r*z@!?lM
zlSE%nL@wN(QW{%nJ8-ln{~Gdn&wXM(?3r1l^{WKvizxneo`1=SZVtC`^Y9Gucma?0
zLC8M>oBxs58(>j>f*X<YOc;d9SPoV(2bQ~NJol)<U@!^k`DEZ4i3%`uXv^#gL+Smv
z_bb{m1=;huTu5&2@0i>Zx+#l#G&u5}t;*$zf=_jB(!`c3pLCy5xxcjt*kACKUHSR>
z?CyzjG~t?N4477yEW0i+H8W@_Uf6p^=mzLGU<H=hwC7sKy1}`1|46DfE!<V#1!FhB
zz2O6&+&JfMlS+!}bpa{|jo7>$j3Iz)&WLv({=J+3TLYwuzmqD!%2NT2((53?B4CZe
z6#!;|^@h>_HjA*Iu3$TRz?vuxoYemJe*K@l_O}j5L9eGa>UO}th<K@isW6pBAVOw7
zJQD>vW3U3Z`x+HulOPWl0BAGRv-)RrTBM}=J*e0}f?Q7|x9IO6CvpAYEfC~7)#}`U
zAU8<AI~}K_iUZ20k@1no@H4-On!txEz+{PrEJd8r!Ut@EdLR$hI{*-Kyzb~h`+zkX
z4)4WN+wI8frmDaV1I}jGOa@Fj`os0_5>LkeNdk}6S5jmq(Ufg(1hDM^mVd(^n2YsY
zY}0i>gtkl9cmQ$$0gq#4mc4-v?$9NQBN%1pEogo2A$<21QRHrRs)^Txz=bp2@`Z|P
z>`9A2Yo4@m^*)2GaV}8G4*G)OOXAjd%3Fj#1A^_ve+{)4o99lVFm^m$*{^S$ghJ`H
zH(LxF#|Gc!iqywM=soPbBt-lK!Ff^fo1y}s&CI1X_j&Ohu8E3P_sdn)<5tXj@yKI0
z;0cx{K#eet25Pt=PD_$Eo^K7X-=AwD?E9DoREkJH=Q^7vCWAaP=w4zLIHsP-J^@;5
zUY7}14}FAIwR~E2a=y=-jTfSejrta+_Sq(E@N%U1iMn5gL3lxh$lx8}1r(zs{_9D~
zQpw><!WsHd#;8S;NzzxR5|HRYI!=vx2n)YUP+a(bq5pqIAq7tsr}Xau)tc8iBbO*0
z>f~9_6z+9%H7euH&K04={1p6B`Z1-A6=&(LRv+*C&zNf(h&kmZgx$gE_zORoI8hsb
zX5Z{I)qI6)mMMu!+JF`v!!lE{=hMH>VJZMH>@%zj3|54pKe1$LLRvCrYKx)4dDiRL
zyLR~PL9fqzcX>G?AeQ)Sux&KxwO?#cWX;Y4pX}~Bi>ep2yu2^WV1AqeYBE)fB+om<
zG8|1?c2AL{ZJHD~8Z-`QF|c@F)0yQsfuG}_pMRH`GtC!shl?+ms84D@RBp{5lhGY}
zm6|yDNqZtDkHCB05{)g<z)lmmP-S;}_65B*RmV?M9Gx|?(>Kp=0UGxVuq^QSg8xr`
z$<vMr%(X7neQU`7nL>+C(M?8~<cR3o(aqv3zOyx75Sq8nxN|AX{WvV30;@9Jg<P6b
zYW1FVNa)|wAq=G*80K5pK5y{%+G`c*F4DE2$%xYkDunw-fqlw-ve)zJv2`p3Gx#B-
z2JckwJR5#SxSRi29R~y@cm&w)@80PD3`-yv7;Dg-xd3*I-4~w(9XhivP@<(X?<GwS
z+~X#1UJL5MHjSM(s~%+s2sxP3UNlf>|KLNQCjB`j|NI?t8(-L;$Sb}@oh@Tp;tPGv
z{H(v*!f$G1l^HrD`u)>XeND=zUqo{81_JGY?sQDxuormn@^%1Vi9iRU+kzY%m%KO+
z>%r=1y3595s+vSkxzg~Ke~zXHSYxl?e#_N6dJZuiTD8Apt@ZZeOW)tI_QQM_5NjJc
zNc2VCO(GtaY+}a5^!R#&^YGJ9Ve{x!U_Q<)iUM+P@;Zx>!PeL=<O9$Ya~64U;jgNM
zq{EQfrFIy`5JHwWS<1Okg0UV%#Ai6yas=i}TjX9Xx$V+(Dx({4JVfYSx(4Y+knL<c
z<;TL0cy9RLdFNeyK_RT4NFp|`o9x>jZ=A|E`P_d;8aTDl6Y@JA#-HS6x6=g{IFcJ=
z3giejG-GZ%9~m`FfBDhLS$~OoDeThJv?V9LG;wlZv4%J3fIZtm_}jwL0yZTKc(^A{
zwybhS{t08NOF#L+io?~w0goFe%w1iU7X*^4r|oRC&`ia3nM~b}P&0Pk2lW}X%Z04(
z<N0RZ#!%)_)Y9)GSilY@&cvoBKCpHg6HIouq|TemdD2-jUQ6Y<U+mmf4H;-IGO=sO
zKim29lAmoCW^q2<WEpX%drKV*`@<t1@!qFSshCq{pU~IHA8OQLFFae0L5I*?GtZHI
zb1IA+Aj6;C>xxIg*#VcPFdWS#8;4{q$^_N8oW$P6JO++j*)`=L21U|c!U!4>b_BnS
z3F>(~B49P<QE~@YB$xx@i_1&!1z4)pZw)9{+W4m|@ZI8At$c<)RS?DX0o;<;F*y1#
zV1qH!iu+C1P%Y5=0^d?k`fVb!lEw6oiY5Pv*DvhH<LHCB*VTlH%mtfE9q3cO_qkmA
zFf#u@OU>i*Yo`aYZ)4-aTf#?<q3ZrBmE+sohuh8;Y9F{~4Fa5UlFA=&a)gFlfQ#c1
z_e_R#mTkWdeUX28!>PjA(_`+vuPc4>wP_xnjbzbRiHCL{xu5|Trz`+J#&}XQ6Rt>B
zKM(!aszfr&>}pwL%hfNHDwv7(Ay3kT9p57PgwMRCdwvmJJgqq#)8srKXp7PIC_z@w
zfaQRi<r%nl?*|}}ynFr<SZaUc@bEvt-QiD$QFz}T<^v`hD21eyniz}=foIeFQ&*k4
zGH2o&xTZ#7b>3ZolI*E0j5PFUEBmf8`Xp#<P33*sal~+DnO<{EJ4KPLwTPVkpUsWd
z*mzjJwK*K*hx-AuZGqgVH)6OKPb^f(T6IA{@5z@MK$(4r*Ze06>MKcqSx^u8*Md55
zf|mb{<b;0}g1N)k4%>lII<$Z*<0bEF%uznd{`ztcTeuc{#_d+G$8~nbLk3atL7?Ja
z%U)QAziC#>|8dyk<kT|c=@Bi{DH+gr-MdA#{DJ~k2#F@c@-L$62<TEM(0`S*7vc1L
zLRP(S^W<P|7@u0`2z4Tv`bDHNEgACY=7riyn-yZP&fBq`@7>y6mc^<naxXK}{I_c7
ze5X&urcn;4o5t^1RJLr#;K|*mZIH+c%psCJ6{W7RJInuu!{=B!%Z8l(osDdQ1z%6Y
zgA^wsJ=eKQXP`K+y)a#X$^NA*GR$3Dp?t2BxUtR60x~@x0V{{AS00eseZX*MT$+7Y
z70|cLf?aTfIukDd7{so9whJfhFE}S!6B}_+i5R`9r(!rz`I-Vz45J{>``96QB2c5v
zk_BD%pYEybG1Jd4B+Wk>;vi}ms9;NRy-IIE4H4}xXO)S}x|JuFmp4~P?*9(5NPx=G
z>&zK6)*bQC`lQC>ri`{(CdMkUsG6A=OJqMj^_s*d6MV69gnj}$M$p#wv-y+j@D(kV
zv)LR0>)`qexvw;dh_0T!cD5MqFL}wn0u0;nX*LV+G=R|X@Pao7Di6SEB~!(3(u?&L
z$^n)T)B5)|_<#Pj=2Ku}RP%JeF8Y>y@##^E@+qVIQaf3R|Ll{`QpewQ#oLsDBbkUe
z*q1~9$X>5gEg^<A+a?_s%R827FrXob;C74OkwYRdsstj;;=m5r_MrL%u~Zcl^liYk
ziW%hJKbJLE6&eHLz{iRM%I5eGugNT>M0{ePs~sX}Pf~zqA%+sMb+VG(lyMlm7X_}}
z>kx!=v~Z&rYHGihD!>Ydp4WLj<8IbID^I#<uJcvIu-KZW?NXR!%$f7{-4)D^IHh~r
zske;$RP4j{+tZ&%@63si-b{^p)2sEamPgiNWBc=(Pp`|4&s5&ZfYcCA?(ER%h;v{{
zDxMT6XWZcBu)R;#rQ2vwr<+~2uQoZ*aKZqv+hovm+h1XpKQPRHcU4mv^-6pf%Wuki
z98ari1yJ6Dmx5~?0Ofs?;<N@(-VeMB6|8Ydiv}C;orQfEfsPu~wv4~OF*qtKmtB3P
zZF~Ex<>3Z2$yv}Pl#khPklOfMHuSePl-?LH<~V|ERBWz&&@8jECPkM*3UO@l8QU+9
zAuO1z#mU<#I+Mr#U6D6@tGK&5;lW1p$IqRN;&l?B$4$tH;FknO4Dlm$P4rp0LoG@o
zokI4filD#;fiCYR8OJ_iFH#}{pqz`=Tmt$s0ln&YGt8eI(5p_~fPsJORpA}P2al01
z{R)Q%RN5_Qc>aoPpMIy(1r==dF$@J;MM%U$z+G6?5=zQk1RQty?DwubbO*Xs_@nl-
zyAc-y<givgp<RAvVMAHN@&h{;)vA~3oOkKOSi^PNkEDMNIjY909<1gw^33ofTI+Q(
zl?E=&9mp8-uP-tF&ficTfh6SE#5L(OPu#h*jpux?F$XzTG(FXudMh@-Ld0av1vqpS
z1uZS!-52k?<|EUpA1V7{&y}HLsHjRvmsjiBn~#?=5}RaF3Z&0Xg~^{|y5HXs4>}K`
zhO{2@u1WPrrzZ>yYYyw|s^*tM)4czs&GK2z=n=;2tj@biKvXIDs-z5%9V4<i<}ldU
znF2aM?3dVQXJ(f$afd2ZUW^w}yd32oGE~Iro<=V2v9CS59{@97gTCEBM8c=^_94+5
z<Koh(>T^l{kz9K1!94FOHM*XWncuf~HT+`+_PSwFuu#tpuI5E>g7lr=b>7~V<H6z{
z!*=F+vD846sYz)^ken5RqXj)>lIkRB+Hy<GeG&dwxMG+l`b;esT;FP3AZd>7%tmX$
zsEqO11R&FO<waW7&LZq2sng5<zT;fgw}tF<>A4uHhLx~Uz-CqRvct&^>xuXM^kjkC
zjg6ibBJx^m+e|BRcl$Q-y>CFmJj|$&g=0W3vAmadtYGQq$6anzWY7NSqDDL&z%k^o
z$l99V=5UO{g!g#G{ABG}+Vn#&NO&Q!Csq&Vx8DWKEsf1F{t<hZYz0v{I-{>UtwBCt
z)kz;h<---$wn3UUE=#%}fnumGLB-!#AZL5%EOw~TFrJ?v&i;ItLvw!l8nc$NRv599
z)Ttl;5f0H25Pf0qj@dc)Ib@$7s}2k{^v(ym0SG5P$thR?`|`Q+@tSmO!znHNMe)V3
z*5wjXt_|6Z`jt5UOyT98kZS+%G-~QO<?Z|{jdt~>Htg3QwzA(}kqcD!dT%g~Ja}41
zj^m>_D?xl+$VhUhFmsN}#UVI{2|NKDV*$ROo-llI`8k{a{-F%-h{Q*Q(c46U!%wMG
zigs^k&P1GKeZ{yI_n?<hkK-Mzo!VzRw40n>QlmTgJw#|?6YQ>B*%;vHSu`NshajJ%
zbjFI@r>~wY9gl`qN^!zdZcE?M4En60q|Jiz84f9LRtige?WpB7dNR9^*XRLNDp)Wy
zHRYlHj5-dTIgZ|qZ@2>w2dr>Fvwo+@?m|!pERt3(KemY+N~yZjP!0yX0Kc~?N~mOT
zaApgHg65NOjOlAk8R@j>>7*anUqp#!29LgVTWnfvx@qd31l|B}QuCwN|M}--MS#jm
zJe7~+HV|swLeI@3PBMXMVM*J-qMvt~`x?IOWV%wIRBC#YFJUdD>44R0b%(*rQOlef
zpj>lyNJyCMc&L8kKEt2^$)fT-fBgV!FH@D<y$fRX{W5kgoSa;~no;28T>U=5BY8Ho
zY1NZ8)aaq^SDJFSY(Hfh6AlR<+O^=#y!|7lAod~G`|Y7>!wa}=zxS2Rj?UrfXFt!T
zvG=1ev!EM<DKK5lmF;Tq1v88>)IQ6B`AEx}e&9Wm*o#X!7MfRIi(ZbqVp<lSi6sf}
z!*raev~c?z2y#ActWVxt(QIr<AV5`=KF}o0%9_M%1SbR+-|W6JW2Dz=A)=NpGRUiQ
zpuLeoAZGJ(a8@7AYexs!zIm}ZFhZz>L>@t+<b^SyHn$~(QcRuVnA3Ldp<QL#3Anmw
zruo7ZnLa*+tMrNQg5CK0WmT|VIR5V~fRWl)olbM7onJ&VwG>$zqOps<(b9e0^Z2n0
z*G$mWUqo%ZvNU#uy;<D*1&9&|vx-V}(Z`BN>(=%Ut45J8cV+4>gpiEHWP76F+j?lF
zw%OMywIJ$y1Ce=e+70x4YnVb3s2+P$-y-Rc|3KMlV|OWZMIS{)*S2%NX6e)_2b3+I
z2OnLiIXs`2j5zS}+yHF06JV+Pwg6)JiGne|`1uKUALDa+o;O8(qFXPvr0IUKq@t_(
zf)T`?!ey79-xJ$z6Mxc;DRn3|cgSLCLGNZtOT^kZ?s(KipT1z~I;k~zl{rI8B4y=O
zKKJTe2MPE=ILOnE3LceGt5uFvHw7-L$7MUcRbXEinA0NCN`B5moS?F2EWV<@Pfo2c
zCph5H4S3{h?S1_^=do4LShXe6OqcRqEz|5AmO1=Q3bve{eZ4ubNHLp6p3hj_8cLUL
zlz`gPPvWvl)nY|)M77$Kzg(ktlv66k0lEDUGu%D&-n5XcR>_vUaC5{U_A$HFjFG9G
zi{?U4s*XLMPkOTHkqmS@jenyeh-06fkG;=IF2nz!X#D)>`qomQw|qgI`l}o#V;Jnr
z5yP&8gv@}Jr$VdG1ir|#{9wG+$NQnS#9o~F(KV7wDeTr^?eXu~KCZ5CCGHR7=rxzQ
zmpYbMeu9~r843d9CXFV|tW0EFtZT-vKN~UYC8xon+BAC^11u_pCBb=mu`9wPKM>af
zgo`lVoyH#bYom)gEgPyc*vcAuIbs+r9@yI6ObX@*xlkq>5iL5S??4fBJMVLkp7`yx
zA?5%zgO(xR{6NZmsT-d5jUJ=X#s$??CNBeEXgKOgM3?iWX-s&V{P2cToWEj{*-n8@
zxFRRo@-3OZ&Z`UqGYB(=&C_}M@FZ+9fCg1x5U)6HYhr^)<qVNV9vT`ig{WK<Bj3G1
zQ2)jv$Uch|L#PgmZTe6=EDlQ%zgm)(k^D`#aKH&GU$})62l~8TTD<@K)0lmGDjSwX
zs51j91Mm#_KO9*9GIITQru7C{7{tkIjPtUlxMw`mPghNbwwmPGNl1+*eXiG<PY%^q
zNFHq~kqb4~)dLVSu<su<lYI)T1X2HNER1xrnpssZH#XMH%y01?jIO@U+SO<3AyYEl
zVRIHPwAlSREzr^Sv`M&gH$S+`fWRg}Fb>>%G$a3~s{WAowGw{tSe&LaluoU9%kL|y
zGgfvc0fjPX71;Hwk`(AE?i1f!s<zvaF2cbGy)^*!YaP9}a}XFQd;@mhYh}>AhJHsY
z?98nx?<{Vc3W8Juro%G%gzRCV?WaAD6GpP`P0;R_E^y5f-}-Q{{64WvM1fPT<b*(n
z+%~79=5r|wc)YvwwY1Xf=fruody1vLQCO1dj-n<)V}QVf7eL3QAhUx(6i1XmJTxvJ
z^_XLt9EZG+BzI)%>TpU>WVg;>bQV1!kzyu(Wf~s?tTsqk5GB?If_@8>49(|$8#|9!
zLgOX9#tzgw1%qe6%`2Azkx^$tQzuq|O3!XYya~{Bg}J*7iSH#{w@uK$RB@R3p<>?6
zC`E9XC`(2Vv`iXO{l0FgKt4xS5HZ}GIo+`EuHnKJrI(Iv#2JU?ljLn*oNniq#+Q_M
z`{jEroV&N+Ta*S>vmngU6D54Ho7<)E;iLPSfUu(N$<dGI>1IJBc}eNy#z5nw&~<7d
z_j-yBcKi={9YTdlA4uqetXB?JHRyZGz4{O~VWsE}&5KP9b)%hXBHRz=r);~GEb`b*
zq<h$-^MA|&bJxE+)Jgzz*Slu<4fVMNgT)>O-wl>NVQ@nz58!U~mDhTH-S`Q^^Y#T@
zQDY9v$Fbf&PF%-PU!5Q@;<PaD0nKW=-HL;gfZc~h6$kP8I&x=Tee7FT+P8*f*MdK0
zHoJJ-gU$GN)J8hZ#^u6_G+CV9D(W-kvsBeNkza@*ju_Uyva?-n@RLcNwhyhfR`>)A
zIT#7Lq$$zW=hkgnQmMk4m4Z%ho0V&NL>eK$rD(Of@K}gNia0#cl{hZ72clzUOK&i)
z)iKghW-Ov*t|Huf$fKWc;FsV$@p~Ng{NP_kh~KZ53H-P_m6&%0qz{wRCK}&4dMRD<
zJXr1FCfl1(>D_z;s-oVJ^GC1f#3K=%k?f=03t=_k`CjwWAY{1y4+~Bfu5Z_9hLaWI
zo6D$Ci1ujV$Qu?Oybh%%#n-27T$CU}Pu-qn;dL|O@+8;h9ol@HxH?p|;`hK{@maOp
zt*S9+JMySPKA3LTmiqR2*QvSE{Ck5A*q{BQyQ&28p@SF=^+=zWsz-spi2BM(ayC{(
zJViXEc|QBg#{!x;^PBDeORw?2+bHmC31Z@qY`~b3LSt-f(mkf`D)Tyy3pi-4TZt|L
zQ#N?><|--sir~WgJUen|czd=T);ad50f(OhSDC>p>X;W+NoP*OC9P#wq@)gnf3M+3
zvN!Y3Dlpz1=LKJ7y-C1gKS#fj-C{xb#2zBJgcx#l!lm1o<R?K~gB6hW9AW)0EvGhq
z^p!X>=a6(P@FwTjkzt=NXcEFgxNZExAe)-NJR~30IosoHnYXa%W<0P~{Bkf@(eyeM
zLvAZY1BJ8<nlQXn%h);eP{z^ST%IO;k|#l?ZeqDQN|BqS{T}|?P@LlN?8Q2c3`zBu
zSxmxbOsJn)2k)mO`}zGH+<%D7^QT(99Dmv3e4vlhEXF-Sl`^(VsQ8v`UB4li+j~<j
zR&h;m#g5}`PDZc>L)`skkm>=@$`Le$Aj1g?9ye$4MQb<J#QTJ*g{UQc*q*1A)(iNg
z@c_F_+(8o>C)}2eR^7_fIH5E^@pMW%?e1~`&O2j@!OaOxjI#s2(tTV>UV>CA0xj|8
z!$=5*hqMhu?QmcM`7xUvx$f{ZWKVD6wu+WAvBmXuuGuR*NrK6;D?~mMRY7#EO}d@q
zWm7n8Skm627vi(uf>E|f*z(mwmC2-*HDK+4WPB0{SA~%SkU#u6k(4?*3%~t2>*I#)
ze*1IIRRR8-zlZl)IV`cl9Kdm48xS>)Tqv~wVyecSTmmh)9){8sVOreXz_YG6TrPjz
zNF<^$c77KC?Ce+2LOI-kZ&5SbM<2g)<rr$ud<Pd3)sY0I9Q$$Fyw4dP{qd@sC7mTb
z4bRz3ga>p|4t3L0mgp03o)F*T59XI!M;ykd_Hhdc;zM<?4P>?ul~j=Lje;<BAXmK9
zznrTR>i2tO&5et1vD`}_iXqdM;tJ>X?b{9D2mC8C(Z^XioR}2U^}-Uv4{ww`O66sk
z^`FLwCMbPi=tkDrOLOcpwPC+Sq4(K2?T)RG5gGP9HBwK4gi}jAs^sC+Z*4K2_8sR(
z`bxuKz;?$17Sck5y|jb|=zqIP(X4Ow&lZl%vgVz+h&>L}LOBcZsi~vRY53;J(p8Lb
z$KvI&)pGP?Xn~Jkx60E!XueU8Bc1XC?KNwn>o;}n%IN*{hZxY9?In@H^khkz`zZtm
ziKZXvm1YSe9&%}he)sf>+k_tj+t`=q!C#ekE*v0s@K{M$`#Q#A@g|xOf3nAlfxUbi
zn9MdMS?eqBSbhf3DY2rCTCF4!Tj?__U>*Mz6pNz+g$uJdRegN$j`z93NwjHFY?31N
zZmX^nHJY}wf%vEZApnNLUwgwU3wOx#aF2mGg(nb_7j&haqQ;$^wRQD;1MW6t!c~J0
zb`gBR+_tf<WIemAj31Qlw+@F_XrE4OD74@$TZZ)!I~${S)^cK!mEyx~sgVN<MTkbi
z)aPGBuO_m!wT#$l_SGX|o9`At8*Q>|V{zoNzP19XthtU{Bsxi!iQ(EZ`d?i&XbUCD
zO2l)`?eT<6<%y7DQjdwT-QQ)=%l}3h{NKkDlmEB*o@M|)x)3nmTL~0`fdTWqDd@H%
zV7_<ZBoG-pJuLN&>b^a)1Z6w$kV>;p^6bV~zIIiOR4p!P_}Qlx?R^-I{%-pXNIEDY
z0Q?y`*5hJ*D3mF%oN%-0^v#mY!9%36xRiAUO=$Zjs5YVt&Qx949(1K<>}YQ7;N`CI
zB((UZl?3BqpJ60*{aT`ZHxnyuP$6kM^Q8D}CdpS=pBaI)m^JtM)V-DdrrH!7-L7BP
zK2Myi^zzd0=%E1r=?9wi_65xvRLq?<xi&eSuv9d-$EC8@pdB{}-{mFWLtX?)HI9^V
zLT*}wYDrkWcVn^rw9O-+xN_(k+RBibPv}nzkfiVwzGk}6Rvo5h;`u7n#?2@?L^Mlu
z-n|E$w??>TCzXDmuz6HRcn_q2`XId{7B1C*f6^QCcj^QgkTw94XD|jxO>vHivgRl5
z3~hA@l<X7PObaQrDwUE?%f#Yl=CAbjv8$sHm^%S=DRk2q;WlU4gK7s`&(GhCG9%5}
zN<T4kW%lRT5&$E#=T-M$ud=R=WG(8BXz%(M0Q0*z2f1WijNWFx&O&$-9T8t_QU_~P
zk2-SJRMdojo0Q!SSrmEGz?E@l*(L#Y11~9rhLEa`c8wzzv0kfYShyDUX=8kwst#<?
z3l}f-6@$krV*0iG3yeCD13puIP(uNWH!@UesO_fJaAJ;~Gau$5H@}Ehz}^#sLS0w^
zu?`d~yexBEL~Ww*xzG9Q`TBM5+*DKsw>Psdo<0dmPr>-Uvz>Iw_db9mXu|cot5n|u
z`%XjT$QZwUAP73KLDz)N_4&KPiW`Nvqf{QdD$luS(35CoJy@Ey$vTne_!)E^qqgiI
zT)lI(o$U$Jlfogci$>bWNL!fbM4S%Aifd0VBWKjH@tNew>wP$mn_VJMo@6L4zH!C}
zJ7(is+8-i)K{P@BY5Z67aCZulN3W8eir?wZ`#RkO^`6`_qeVdH8YY61dBi%4CemGv
zzj|P6r<@-R9qq^mnmhv@V*F5JLy~2b?5l?9;9Q<p2};J;=r#g#Xxk$5ST*$d5?h`E
z#RtwoH@CeI4M|b!nEebY{x-yO@a4cJ{ic2KcEb9A#ANevYRZNa3r4FV<cho<YiS^-
zX$DKOKZR_?1mjmZMvTE3qV2Y0wiP~KFCuzW<{a>PhNJ_jFodGK?qB1@zxV#XB8F^*
z5F;EA^f>bU%kiP|T;!3>B5!*rsBe8qbm&X07#X{5AMTYjz;N}V_497*V)yx+PGwzl
zRejX%C`vqy<qfN>TP{TjKh+h$VR|_f(k?xz6x~}v70xFgHw4+6DJv-~D5Sc3S%;K+
zaF4OdMC`?HM}<Q`uk;aM<NxH>YJk>0td&5Xd`{`y)hLnfZ3{}5dp}iy&sQ8;zM`<G
zqJ8CAUPcV@J990R=3E`pJnRHkVYZod>(JYoFjQ`H;2Ps}IB4`uLt<k$V<qO?C!mTY
zs;)ejSLnnGdvgD$#XbWfLLHH^8A$#lKm+CsWAd5Z98*F|c6iqBUpk;o?l!97No8Qp
z)MiSM=q2`YdvJB$n~@eg$AYRU$oDPqal&>qdaTZhzpJYmT@+p@vSyr}aU$`yQD#C$
z&UQGFmgq!mAw7F03l(5Nu8@s`Be`ub<#Va*a1eaWeY^h<s9b!{ro@r0msPB=3Gt%p
zB`W8P2TqDn@15r1^fwn=bVCe@s4lF&GUuVj(Yd3h;%lyqp?JE~RK7S`xkJTF%{E8#
zVs!X8Gal0zQY2Gme2OhBItE7o$x}5%corM>a!-_apk3GL+p}!@&_NS^{+L}dZ2cjh
z<7G`cw}Gq(N1qATdEr+E@HkJ%!B{AcTi6?yzEgbcV}7w~++i{)r4N>lqay7B)d5YA
zU)fE5+$#~ZgCyTMbatg|Y;%;^?$sU@c}KERoyg32DY%1q*%Dt*xE1htoU<I`jUEKt
zJm`i!Uqrf=s9Im9momIA7DaB6SWmk(hFFkiz`r9<`p9S|^miazUb#{vq(;A}4b^%t
zN@Stpx-MmU=S*RiN<&OVzQJ7FNwT+m)}r!EH*iy9DZtue53c8w1u+U=7T}h-KZ*d#
zKYc-@>-w4KjguD4%863uZZplN#d$SX)~UNndH6)<4;Sw%%Fk4R`6BecX~ikB*LeVX
z1@7UPX7efI!Z^a6Iiart^$vnN&nGA<KHt&zEl9Y&&}P=8e8FISgZRiQY9H#8m{Bl!
zksiH;eQQv2-r#O_I$}P@xg|0$nPQogOFec_aR3X0EvIIaG|LsKxCb=?f?K40jMbRl
z_`}CB$}(+r%GoYNafW3~`x!Jl2mmVDWrt^z%eN93(HgM@=7zr6cH=?)RptfwIY5nU
zv%uTyWp0Wqbk^Yi{sSJ}<I3)u%0po;<R1NFb!QGa=ghf~AKEeET?llI!WRJq81QrY
z&(JxB_5eqW!Y7<N#<>KeW)gDUq$u^v?%JE)Nl69eC?t`s%-f7*k@@`hnO|B@Jq#2<
zGzMhGOi{G18O9E!V<uueSH`1XPmF;9$ZnT?4GO;4BD#gR(rjp5tN1mU{(1ZWq?Ra$
zODWNssUX(ixo+|T*)vI2yuYaujy~eVCb}z&5f&B1Rv9h_Rg5O^xGFg9h~vw&^sVbS
zJ&0%r`@x<GrdRYdNVk{v9SMQ~@^Kr_(xhb-3K26km!Ieg`Ssj+pg{}|xFb|~ndmAZ
zyk|1Mli5?~W`_fhy?*?`xLF|gd0A=xQ3hM88nw2O(9IoRoxqhz`4ZeYH%uQWIj(9>
z{F`9(Us))=3c)d`b);<oRN|#0`O^{c)O(QX0C0%RPwUjNp=!s6J2o+|vCF%p*4GMI
z(^k6HYTW&3Tc2S*+s9D^aO#jt^_v!#V%bYj!f#sW(6O6FTP-|*yH;Jy)8zBG9ERqY
z*Ke8l%eia~?Ftl*;4A<$pF_hRV!_j8$C@<0O~9UiJ}u@e)h+3}F0jGF;(n^(+8yK!
z6b5Z$ud{gFNUnl;%cJym^f;c?voct$W^81fvbqkt=i`{r`tdGf3P`L2+xr~Gr>EpG
zq3ykf#66~V@Zwxyw0=o-MG}l2aVZ}dKq;Jhc2Jl+ipl3iaNA-w4JX0NIrp=w#3!=>
z(F{>4JnNyr^X3MJneP(ba?Q$As>E+AuN7vLvmTsBbnVSb_LK^|ecEzk%=7XA#dWm8
zOTqWCF|B=#Nb1XV7Y*(BapIfTPxg)Gx-QXhU3;9v86|$T=%WP@Dba=5JouQ{9$BRT
zdyT=}qa%WobNiAId~4bd(%R7li#os!lYGTzH|wpaJUNZVIZNaeE4Dm!`)1p6>t-*z
zO<W%3;4h*(OdHaxQ!4p{p&+WU-5V-hgn}^DH%?_2i=;s!2NP62J*I9xKg~;?1Vx7-
zV{)$~6e`GgPoe#SNZf}+6ZJPL55^8ej$jut-VgkJ@9ogGkNSJguyuNEt-q>sJ|A@A
ziV0Ea>l-$n{}gqu>QOurFDRAQoD?si^L}A{7g)xnR{${bhum2TF#eVY1R$@e+aw!b
ztJj`Gj-VQMvFoIir^;s!?An-QTo+r!$Ewec9Uo41Pl6>j*Y(2h9FS~SU5v>hQDK9Q
zg6%bL0PtLE#cpi*<@671X-fQ838l38BF?vT%$%#e*op6i!Jw;HuTpOmGz>qK>{#l%
zbyxV?yd*l;`qH(4Z_>s%h<LI5t#7#@dH7x#$k<8>(sps6;S_VHhks!47IZNgfr}#4
zHpy-LdqB-9ivL8_1F1+;sBX+_B+Olb)_GsZBP~-41UP%;zv8+7$WIOE00z933UE(G
zC}YtFWt;=Xi?`O6-RJ#^Tl-IYtS+!>wu0l+mbRx*;b^7UNuy?NuJqg_g}2kYJg4aW
zl(0B84rWpTB9|Y8ucnAj;fO}(PP1#%OVoTQ*_u+6kU6Pb&*uyZcz_oN&DjQ{OB`dQ
zOQi<?tjXxfoO!GVjHh6+g^uw`8?u7$XlaRY0R}+_^;fh24x6QW0=u_<J8V7=N0I(^
z*yQW)2m$bv6vz0)*&&*GWxWn7j>&1JS@BGECR4fnW9^xZgiItJCu@tePUt037$9Q3
z5B_mE_PDmt*4fw2m0=;j6M9ebnv~C)BD20e>otW-G;Jh^S5@=T3~y0cplg|B+dI&?
z-0dm7b5@^udUT@kqOje_r{CD@<-dc?2KE`Fsv%cqxYwJ$7~0pybA$<rv%Ig1%?NoI
z?sh>+Uur8j5TK-5SaTc?y1H)iz~{4|>h7c*ic*M7NZ-23MR`r{AX1umFPNG_kxIti
zqQVfqto{Bg9qip#S-BTW9ic&yRWo<s?*df*aIIT|143XV2pwKryt}aRi^#!L$*!tk
zp-FaU=Bz5SzI7{06d-cZh#JYRHy$Mr3ZILP9FSL@jwL4c36Gp*%}s!vpxpdCm{W*J
zLcxljcY}2H7m<N$HFC#Q<DT`Ro0fTypC9`oh+|MxXr$12tFlRc><fg}XQd<S4<Gg$
zgp?e7q4zs{zCRlxy^f`J&$5XnMe66fEr?*@b~IKOr(Hc>hErQ%3XpZ`tI1YDOtDLC
zL=Wh3J$jz-faVpS(xtbZaA~<=01G6xuvd+9!Dw+0$B&_OG-Omsg+11Meec4>ln*b~
z&r9r(a<O$V$=kljka_is$bT@GXEwuh?<aTRQp2&7)8!t$s-m5Py7e&}8SV+z4aYO`
zGQnR?`xg-;DesD7^^W}GcZy_-Pf_Bnz#uni%Wg5H6ihDRs`$%<rANr6%LDh!%>{+U
z)kauIc~A3RF;o)Uy%I8{`m70p_(%dRkDSX5M_WbB3{<6ocGqI&p^|saN;_Uo4+~Ir
zU$;(}SELv14{PnI#2B;%v7?2uxg)}>+3t2simBT9oAG^<u}^z=F@>LrsN!m_oJ;O#
z^NVKd4nqUNKxnZE&i&aIehydGxEWvC;d1%PmazeLX{|rVrDFv$yMgk`VM~r!=0}jL
zJJJnt%0~%UrMiWZs6TiwK!$u4&QZ}uo-_im_GO@DiM8ccN4^Q}0Va{+hCr~HCe*#y
z>_y6Tw=>6TUxDklT}&+bg@^?7KDfYCir}>HNaoj!LXe33M?*?!=5<MvnfI_G$Mxn6
z1LciHOS_O{L?n6-_`kf*x>7Z^s!59(gG|6<^73YNpBZ*mEL%Qx6X>k<W}6FcBew6>
zY{VFx@BvS0=b_phbwrwN=jdZg{?E;`F$yZAw`3yT5!)&ak}Qke4N*9m<zEwRhGefZ
zo_C9R1=6iSJYFJ*fAHF3RKb_U&j_}TG8)w*M_3bba>JJ|9jy!ncN}wRx+q>;{Cq3s
z^0PmjE1m<;#Kg_pKPwy%#%cdUSL{#!V08Kad;MZ<ZlN*sM&Lvk{)^<Kq}Dyg>vf`$
zx6&jo8?k9T;sr+IUb=zH!hBseQ#xVA=UaUpOZ<j5uW5m5u{G6Wi3cfaU*$PHk9c7b
zNygdtoI_a~VO-LUc#%Cb+i?<yvGn}3U0Jg)Sh#m&roSlxwoT_VBIljwd}m<o(rOEV
z-h07+Bt(w7VpG5Ih-okEOT!DHD6AS`A+V-q$J_c_w|3FEE0gz&>ID@3e!%eEs^(Cw
z>X6MzpIt}0C&|1>m!G#p`YX$qfmj$<BOXwSWeZSk8QT*Fg`3}u{UYqKp7h1BEa#d!
z-Io|A!Ff>%Q&9m%2g^t|mygt+p*vLpwZ0l+K1NOXp94`Ru#FA(llc_!!i?pd=z`L~
z8#~+pAQ#&FUFO*zcKKJb{$HXLe6U2gO81M1X9(yynUMy_n})ae`8!B5z~Y`|pUFEi
z68Z+g=k5*gkASS<?1tqpqE27mY;30Hqh5QUM==)6|F0<E-+W&IT=u63*T9z+H)$m1
ze!@mdrH1!Duu3j)u3gB!WU<rZx6Qx0G)vcwK)?5yIl{0=+?U_7UD;EAw_eA0W%W|G
z@|l?Yalr7kb=b@YW)144lqQhD)N0Lfo%>WjI@_ePsAjhmauzTw`Tqecc7;hX&#6g$
z>@-G7(*wNTT@tg17wsEpA(NW#io4tGA_XVGf50)E>=^HLPLX5N1?G$oC!a$y>z$E`
z-B&oyGTds8Vgr5=k*sBPPz!iI+0%SNU?4c7RkE@Iqfe6b*@cz&wg3;;px*+1Jk!~;
ze`kaI^~M5x<-*XX>X_(HEG)=1Q>EmZ<|hNK8}-LJuafrSIxk9P2g5+(h#7bn<#IgB
zmnt7s6-gz&(3=(WntR9<VRF{w2Vzu)M92}mUjyt7gxX{2P2<gq7IwW(U#Ll^X)~37
zyq*-ZIcEC-=ixUYB&#qj(k8>8x-NgG_Unxw1Wom?0k?%{LOZPVej-|g8wrSIe3Esb
zWATj1qS5>lZwJ>KLzJCV?9y7{dPxzNfP>vdp}6F5V;R&l<a`3%f8@j2y(O?no1i|@
z|FQR_Hysw^Gtl0{{gxs$nMle5_MMz%%Tc3$@!%VZze(-Qq`eb|%YJ{1*q1L-K>mg^
zgc3$<*KnQxW|yq|O&PP9*{M1uAcb=zwO`MRyj|05KAih_9AK6+8~>6_H%Rx1K1sls
zA)~n(oA<xP-qYlGBb4bTY}0pXHIHSQih5A-?yHP<Sy57c(-8xT|B`&x$=5Eak*fPq
z=BLhZC>aHI1}2#K$<}>$&^P{u5YPhVgRppp%zDqf&Cj#EMcLzUTTGGcLipX*4y-Hg
zF($G|$r8PX{Ls=8&Xct{LA#>=zWeGOmvRAH`s-~xF5FXvFu!c7=kD><nL^6PZZCNK
z7M9R*FQg^zH>uv6Jsv|Zq2;HFoZ+Q_Z&38Wz?<K`K@J7=neBjY(7;{MWugmQSwsv6
zB-!X_Kwnfsl>{wXGR0UwZM?YSNgh35CZ2<z6N^k956pMZED(#JP+VhiYx0BPXg9je
zg`uT7JurI56!T=$d&wtNFTxEc==o*GRLGr{%NFv8N)Fv$_2?(7gn0El_n5*6>%dRF
z4z~j}SVG)yEU{+`AFjzkJF}}#+Ywg-vv*n~M>h^nUlNAATDs@$yx)TC7MT`e4r7*A
zx^YhcY($UFtVxgvLz>xXH;EDLzYu%k!1R>0+`~addRDZWg4Vs8y1~bQK$ePzb^!V;
z$t^f0>-w8+L2<LkT*{rH%`6^~r$krQqLA^-ZR}giORqpwK_JaSQ;c`(EJ<Qu;p3aN
z;~Zn3u*O2e0*TT(oTA!~?utI|>ys8o>7lZ&x54K}OU%t3AIhhhn>YHF^-LlGDmvK=
zjNAn<N{}zlj_p1IWeeoR&;WqT82j->{foEP4fVxB?k_FI$%DO@Y}+i5#<D~MZ#xnZ
z)Z2KhWx&&M&e|#Q6(p0Rl`m&gpgFm6p4GFLoSO67)2q&AANk$LI)dJhn2S5p*l9j(
zx}ch!kyy<AW1)F3x8c~wiE6;*#7QzJ`nU<FIgB6yD#kpHoY@-LU9f8f^%4CfSj~~@
zeLAwMZWX4=J4ptd>-KK_@|%gjAq|174XxBM8<0G8E>Vc`P1E6Mn($Of(A6fIzT6Ng
z5kj`6W*aZn3ep^b+>Kc&<LUt~T5};f>|sC2U?$n>n*42}vuQz%1aE1`RlxI9>6Is$
zFiw1Vhqz;E^s$A1DZg6CtbWYfE^@_>N!+?C1-gC)dO(YUCsYqE<X7nyI0t-WRnZuR
zPrn^Qtp1^Q@}Fcnf6ag{oDBf$<r+S#00Tq$Q^y8Tgs@y0TIJCXADxH{$NI~94|OOq
zOy81!7=PDNX7)K}Ek<--ehjq~{H3N|Uah9926sy=nql;FDubJb?k+_k&zfbH0`U%*
z1{2iL)xth;PfmYn$Jo3ODws6SG9%k-^DwRn7+kN<$q{n5jp0l%PpKpbSlB`(%X_qD
zfrTC3L6FQMC7Wf_xg=|6PbRQr|6o5-E%<T2WMwgY3FFR+Ddb5u(OY5&H;C{Z+X6|l
z2`()hTJ^hAZ&P$>IF0kms8!pJLN?Zdda?3=sEY`{`aj?V|G%7`{;yZ|U%vkj<oD)w
z*hfE341gvA;XS)L6`^fop4t!i9|NjyrsZF=D`{NVBmqmTm|t(RtG}A`qI)#TvE?T}
zek=D(PU57TDU9D@tIS?~Zb)+CH_1m#wn*UQ(Z|iBOPDEz(Ed7;+-u{ve2s6%D95}j
zVGQimdw+*Jl)~;-GomxQ&bj?7IUshnYMPLJvc9{7Pq*k>U13_-_)wLR;W)e?-NU>B
zv(?1#0HRX)dpO#W04T}~28qJzC9meUBynBqqHrBVhsoP$Gg?_aUr1uIkia#iZ^d6F
zbU`i!uobEJTcQu2)~6}QaC;i0YrWy+SXFX4;CNYd7CcX)=%NhXXt7L3x2V4<4*R@0
zS*%`P!@$(NKT<H)a$03D2MpW#WPu@{?H1?LRI)=TttR3|_C@huah5C45_M4P66NGd
zCta_q3G?&uEF0bDp2}9ra~za2eMJ1O^QOu`+R`~{GA(I671^{$^>ZxIeo}F9O3er1
zl@kMj+SW5KyyLJA=Ydkzg9`3@Wjf^kJgL2Yy?lClwzJ%1c21EqifR0^?h__pVhRx}
z5#Lb28syp!Ef_j8StlMJ;8r|ESg6xz^p@|SY%}r%MMBmH0X-Y6V6^&hw7L8^r{<e%
z2Ep4EHIYMK`dqzBR1-v>7R9+<dtxDGDB%_*k(E1%V*wN(2#n6h;X>0bzi<S~H|Gs`
ziP3Jt;^_Lrbfb{YXg;Bp23=L<*2^F4z!oDA>F9mpHx;fYP@3(PS+Ym_s(=+^&N02)
zOUldkV-WDc^a1!Pp%;Lw&1{8zwwCCn2le49PrGVN6pH$pf<?IM4_Sri13vDM@-TB{
zn*yDu8Tyh8aVheAUp~tpBqria<m|NzRg5#VkMsd7Umtt%pRSqxvo$v0=UeRY%?LY(
z7MXzCCOd4BnA~=PC;t6IX&e9gr@mPeG-QMEYu&m#R>tBA)iNUI=26FPUj}FrR~)R-
zWn%@qh}+qT2AX{z400)+@M>ob;s)?<<;8$hqAfBT^H^Nk8+u=Wc)QDTw$iZZv1$2o
zB`rN#ET4MNJMH{-%X54!fgTE~a_%VsN1I<i_?CzwpK%JQrd3X!`jY*VMs-m6R^UNz
zy{&9NgYIJ{?8YmcT><VPx*!w9b~6u3zADU=Dslg9myw}g2|sTeXB-(5&uPGB9Eedh
z**h3g-^o9B#&hzC;9ci0<6ymu0ZV4xj;Wm2!5@QgX>6bYyMxW4Ss)Ge6o2jaiSpl)
z4;JkT-Kwff_FYrvh*awnh+)ur&<B8C;;)o>WgTv+|2|JdHmhTw**7}QcH}yYLABx$
zE(a_&vKJ*n^5eCs9v-c5tT#hZc3tF4ktsK9QY8AGC%e2nuy*Vpt-!B_a&oZVnN{gt
zY)-?-u0)A=xE%8$`P}ttlHj&Mb#v+y>Eq{5sWi>=4K<ntDS<wAekV6SuWb3!BID`V
zp7jOO{_-yN2lLwxv=UqUfU(P6KvjUEZF!I4=d5^@bKEFL_*0s7X=(2^9+nRk-9Fz)
z8336HD5Pbf#0-;$gt^<ML(TmL2M+5fad)QME`jdG>9@QqGp8Jo3uianj9$!&#_{8p
zmJ=n0D}qKpcXa(^!^4(^hXB3<$p!KKnS$Ei`xyoxa|r^kCTt;20Fkm%kNGQe*#03w
zMW_jgiQuCE&$Z{jhz^80fv65USCer8@984CKtmvVSQ{9&Iubw%%y0WebPjQcJTe#t
z15I~l_aGO54wqj<gp~QI?-K;!eK0P*;uq2Jc_AT%^L&n&u%(6*#r-w_I8?I#|A`y_
z@^t-YUtj1Bu*O(h?x!Fl-r$&v!HLc##vc*_?)uVQWM}p&;dM;llV50+m}e)04{8Pn
z@l7D?_M&H(+MOBWt2)o4jk~RD1ISTYobQZ68p3U^=tX6O(oudkoZ9jXPy@g$KR&Zy
zfB3ay%k3Y<2wn6<aj15!;aY-PI^FFxkE^Qigkk==ezCaiLD?G8<1FHDfRyBb)xU<~
z(Fo8fuAa-iWy^Tbk|KM4%x?jGB-W1eIE4(c7Xzu($f*%?6|6*nEnw8pw48l(=8Z{I
zo4cM{3lapmI`irE3>@-hyowR4>!&XZbowgmL{&>nBP;0X@C3+HryW4`R)daTb&m|X
zeD6XoAjLfinnQA+-}<v|@Bcc3m`wm~^bodsRt>@Oi2Ne@`E4Dc&Hwk(PDiXj_X_7z
zFeYC3?k^(u;vc@0z(yD;T(kVkZe4Rh>@%OVK*^5#Ua?nEavJ)+dA)15?mV=J+wOO}
zg8i^cXFg`mfYtVSVN!Zmd5WV>QBBd`KUI(HE&p=2;j7aN6w<QHB=Y<4G?bFQw7R#F
zzi{Iw@cj2y{bp1{yT>+wlznV9PBWtjNN;(AwdFQWjP~zs6}?zww_SL#AJ}sDt%TH_
z*f|~-o4xLD8Nbhw7x<KBalxrK$82tD9#A!9%4WCT*YEDWb&Fu?ebBnRN=Et|o)0};
zr1Lb_)0{o^IzpVy<HqhkGn{erh5R;1#S*vwo9x*?V<U73pn&WH70)Fskekb0fj@wQ
zF=xMsxKhjj|6|a({QZL}*cqc0u($Etnf0L4&k$7ndVDo_IU8|c-$D#{Km8)Y3pc=X
zN;a}FJbSn_;b5RK>KD;3BDG!Uj~Bm)7%|xdrD-4_;B|m&@9>VVei3=*6PO6fz$ZV;
z{q2+TH|7=pC$8Z>U^dXeom$1uhIeR>2=0!xo+m`7E1&iWZRxkq&!#xOTD*Ohq$Y8p
z&dy2&{>3BEc+7nCWsMA30G4H%cAd;CX^ExC_T{xJy&TKI&}oDN?3EX&TDaX&v;9=5
ze_ThhQN1tqld^Dvq+x0=<%e%4`Ya}EUK}J>l{N~uJ@@%mI?l^-(F2{L3p1N!uKb2`
zW{)_0xF2+*7CJx+@5;-~a@P;tysuFoX|S*mYHgyKcJ~c+b9G8G?rVOXtUIgwzKhK0
zz;c4{nOd_+c0OmCb;GtB)?2CoA~3`k{dYtIg<tGXj0vf(g)bz2dfU1*afNHVhKVS&
zU2?l}$&GJ~K`h3Ok(s~X-n`l5#<#`W_++Qa9g~jPYT*|P*5iE!8l?NT-p<G%!d!dI
zYcx9`l_zjdSLfReMRSI;3<e1-+O`?2G2Lz#i@TBa*NI-WF&vdL90<QYC^VS$m;CAN
ze2CL@wKJTc(s0hK5ykN!<s{pTx$c{%P$xNq1cnCsH%c0Idq5wFiHIbC|3PJ@lvW5(
z1LlAH4yECik&(AM4Mf(Qn|~RM{+ZW*bqN3Xn`2rZICXDr|3SC%s_KXd$7(n&+{j4;
zj_#D8&e#e&`MoLgPsNYK1At9`2@f>NK0$-<2q2~g4h2#|fG$+JUqroF5OA`5hW&H{
zmzINfs}UWbKA_(Ma?k7EXp;ZNx%jW0jQ<1Q2l>F-)LJgKL=-z9n49mVc%8<(GSt@H
zqwRev_WARynGpTi179l%$9_p)76W%taL#ci3QRriQ`a$D5b=2qU!pd?V_Vr2>d%u(
z@kVRP?QI8Nr_HthhrREPr@DXtw~LIjDmxJw$Jx$7_8!?gNyy%NS0qJLB&!rf*?W(q
z5S0~$L}aAwk?r?7(&w&Jcc0JQ_n+V6?(w+qxA!^c^*YzNUf1gy&+B?!Z&^?Fs^sAO
z?h4`9_IfG3OitF}&|d$vH4O`T1bMEjkzjJKXmheC-*3T7rP*O^or4R!Yxmr$DXiOi
zBX+*d)hcnjsw&i|Tv%97c2+KIHN5c-0fY`8wP5aFdeeL^$L+T9&PmBU&A=J&o(|)Q
z!sZT7Q><4NWk*4#%FHvbxqyXF!!bW-{J6lgy-S+WDl@QsPsP?MH}LMrj|@62?LH|s
z-n<6k`Zx|2uU&4Y<e&Iex<l2gv|Q_|@c_%sbD>&Kn6Q~=uLk|0`frQP_;w4dMaBfo
zjkp*~0Eck~3H}!{Gb{E<=;ry$tul0L4E{Pl@-zSGYuh%!|M!0C7rz#Nws|~z1r<;$
z?F&4j3tB7=myrCQ7$6@K4`Dr`X8V25rh3bwBa12TU)L5$yi#o{7TV^SSUc+}pkep2
z8%bb@jlJ>4#%PhJtARz|Oub=AU)=a|<Z^&<Okc5ajWBrnC+psVzOduFRS(K{6taqr
zD!4y2?O=MscTbmRUAZAkp1fAn`Nl<4%@t#!Q)<tkk#7^sG6TFr15SAtGcR11>1}dv
zttUS=bIcW?s5rf74Vm`y(*eG0+emB2U^Qsn@D5jZ&bq=h9xzu{S4sG`?~<J;`y!Xz
zG;`dNW8VV~%RJR<Y@B8hoVxBWb&Ou&jW_gP1Ud^m40;CCKIT)cg}YZT8<`p#K6Fi$
z3gK?Mm&sk4QjQ`$6w$i)FoSn$$hzRhASX$p>j-`9iZFQC#@zkYl7Av^bW1?F5|aXz
zIoulXF#$+;s&L$ze0Ed7BDd;lB06*f&uo}=4GZEMNAT$POJG?BL@Juso~Pb^hXhZq
zmQB3&f1*CUHt_13mpIBM*K#)STBl#$(oI|i8ON(iSs>t|T4QDQ(yv|(%>~}t`qk&J
zsa84!rsZbkx>>hJWwXuO?L<K6swz+`9|P9>LAY_ZVZG?V23{Gcp(79A0NnI#?fxe`
z{ikE;8do(IZIV_`Op)aGZ{Y2|(|pJJ%FZElTg;HGj_#$_>WZwiTDJSV!Q`Yn(vpgj
z4N_P#Y}6yelG;`5<2q2ovG9ut$IP-~mzbvvHD3aOj0i8wei*Tr?*L@-V3B*UM>KcV
zenP`0dVJzjx*<$yITZWt%E?~l`N{z?NCgjV`RD>#d9{6Dm{VQkZvNN-o|DQ8_%oG@
z#0Yv`uhq$1=>H^4Z3lsI<eBK%!DXL5hiYAeQ{AX2WaU{suvp3UnDvrzK3&$Wp2BA|
zb?)@V5dGfNu-;con)!m$=$90;G^M*7_u;R3)oQ<=KEzA<h?ZKL?yUjuofjNYnSOWg
zRvFoQ&gXLlJ#o_K_ed@0aVQ%Jby~7sUyl~MB?RjHZNy;zb5wwfAXeXj2>;Cbb5>Bu
zvffrEc*&pM^H9&DWwYaZ&dc5O$&~FRR*)Hb`t&wu$%Q9mtxTuop312?Eep!n$rdUf
zk<p1Ph<^I~E~aC%;&xy4TTl~V8?embEw)a{7qBE0v1nf1!@7QfC1L|FFqC!uXm(dO
zF8~Sj5XQ?puK*0<)QJtekv6g48_B&j@N&Sv!?u|DFH5$`*S{?JmnDC%g?>Xyax08i
z_WQrALQajpx#TyMsB*D$<kY9)&VnhBTneM{meXcVU+BKrnBo<y6LkMY)@y_O<EqYY
zubUj;#I!3U&lrP8;u!Y?^e5KdI9TJ&T9ZxQYvpZm<K44+)<qG*UXdc1B`LSk{H^wp
zbN5qL+T^e3j)O`eX{DO#ML0-o4c+jMYc-D5?f)1^XW;h1<ly0FMQ_TACf=;+&#66i
za=aL1jl8mSZ6Uko<$B2Vz<?=pyA8Z1A<>Q%Z{Q|SPdTiUXI3v+7^f8IiY-Uf2Fwvt
zm2Tk026THiFD*Ae%`WtRQ#~hL!3Z=nufw{2>-Q6_p^_rzSwWAcQ8HGS4Ls<2dtE?t
zr@y&h`ocU&yK(mSt?oArn0X%Y-9#_nXVM1TTNT{E6IWYFTmo&S5bOT=v%KTR#o23n
z1V9-?e<Lsf(XQW5RIOr}ye9XydYRz->kYi8Vg=KQv-63u#@1_<0izc@!Gt6_eXIG&
z-%sSWY$L-uCPuM=*Fq~gvTm}L3VL58W~?@DV<_u)Q)Ras069=!bXY%q`;*u#)wqbo
z>YBx{Zg;hV@iO~yFk{U%0DbX|Zkq`#joe>MjR~+C6dU;<2Cw>#`EuXS^kJ^^qSy<6
z!1P<(e$A`3E2{f9@Wk|gYZ=wolc)OD@TDdKhFSkz#Q(^{zl-?qPW+F0`*$b)M=$@Y
zPJe9`e?y(VZ0;iz>%S3DAaG91vhVyGz7GF=Qnx(=RHlz7rWs`xm74PDxVOHM;|(Gi
zQ_xqZ?Wdj)AFIzEFQl6=kEJ(^#8y;YOnqk4z1+P}eYdw#zsZ=+kjBA|_Z7%}d^-By
zmI8lY8vO75ug@C%%b$Pw^REp2D+B+^z`ru^uMGSv1OLjvzcTQDDFcKXU1a9=W-hMI
z<|cNyzZ_0llfhwdHYgkTi%mp?OhC)a(VR^{!rtD&l}td=!P(5**$@iehZzaT3n&Ok
zfg2dOArsIrH+5w*gd<T9I0nTAM?xVOIGhcEMnYg{BpVzh2tlI3$28qex`HVMl&$P-
z1SCX6HorhX!qnBu!CpX9KwCor_aC>NiIuIZgOH1xqoaeftGJV+s)VMDCd9$n;;^VF
znW!k+e~3kWMXWR+0EVMZgH1qB-+&E^fxux1HVhOBK?t(hyV=?reKQvQtFhS4v4SWx
z1pJ=uV+DU>EE*1hqJHqj|6nNiIe#sVtb@HPSS2<ASs0cL4#R-Az%Q2tf55>DpKnlb
zHaL8XC$fTQa0A<V2e(XMi9UY}gL?-zw+0(vuz;TGNo(AK;wD$HgMs-zkHWpD=Imgq
zX$}^fO+ZarmQ6s*+|w2JDH98GDYkDI4F8%*%I5YKu9j?QLAI@bWCF5ak<Fa}#kMA{
z=F;Y-4rb<HeaN=t(X$(&Ca()sa~y$-C*P1Pnd>FzrzNMD<D=lCpp$H)4dZ({udG5r
z$EQ3`9>z~6sX?2B3PYMoRrJRllPbaUJF}bN2ht7ydeJ{iw;G_FOh5`S)7;+G1@Hk!
zwgyh5T^!t;P0evH;lvEQq+)JnWg_X|$+o!(P*@=CfTtKF41&P|=~gpw22+5KBR4<(
zm7KzHi}Phu;pB9ajx9OGAR$l$_NQ{X`Q=~6ezoR*E%Q&8<(uU|f4x_>mIDUIZY{{)
zFGsJwic1fAcjb!is60!<Kq#J5fswFNYS!y339(1J2@k5nQfY5rymfMI1+w#UlCA7j
zuJsV#T+7R6FBLoV^%y19yu@el#Yjxd+WO8kafa^*I>%1$3B3uPGe??2_&oXYqZ~N1
z@4|EK+wGFq*Tn|=#ztUdoLA>(Eq6PzoTToN%VllY>7{$_IFHz^hIk$S)0Gz%a<AWi
zNEH&L;d!N3acV**=6qDU#A*G8*p+rn5HpPZ5u?frf+`lD!ylFIr=A*)^jdz-ANE3d
z#c?Bk>B^$Cr&38R=98pY$ZqdiszXB&lLR4!XBd4cp*s?i$F=8ni{$OC$SB}_si4kT
z%DNNM2RmNDnoR;3>n}snrbUZq-hFJP#ZT`4P!pA1!0PzmW2WljSyolNY)(d((Hj;*
zyj9)g5yfzkf!n2$3yd55!Kuli>WQ?_{Ymm@Di&VCMB<V2XBkhNt=-8X#3sTasTU%j
zTz>cHj7V!?rv@$Q+b2?m(Y6ITT#u13g`9+Lq7okQ2TBKrgwCWdsGbNK6(&_S=6-jy
zF;d64Ib9>&ODBc@STAO0UAPek%h^DDrm%(G9Znfdn$}*+2RaI6iK2GZq7zT5X$P|G
zp^l<!mr_tNY@_78VQ~(I;fDI3vh(x%ke)09+DO{n_q}0uj`yBx8MLEDePGe_o+$By
z=n$28M{CrHkM&Q}3)0Myh{9|7l_jdfp-g%_C+DidGWQ~?cI2m=+E;iMZ(lEj4}Sx`
zGp(`XM6E;kb^f3PS-JEZ1Mj<^WVnZG9mfXjm|iuz(>|8!G(!Fq3(GX{b4l^FtReSr
zA-+VO_DYE>Iy7fy<dUE5N2P(xUOUqnf@Lff>oH7@%G+Br*LEE|H?G83dg+$!b-EX{
z-EJxrye?uPFj0e~CZn-N^d&t3@jkep#dT4;xR-9HtRdH_UtTzViiU>iP*?NeJAOI4
z3lJOn>9K|-Vep2@!Xt~ZX&0@8#_{M|P4jNb-Gi30(z&x4+LIe<k*;;}xLvXqYCdb+
zUcKj(Oo~zQ28_Rn@34?^nP4d-kw4N~D#F7;+O>awjqKsYkw)xjr<J7-!!&_ig9|N1
z&;%)JGVm?c!J)=G0VlGae^8=1`LWmIG<9>*b%(*I$`iR<F-eRV*-l#3)bu+sFNE-e
z-K|!0cn;MaiWF%d)@`AAsB9y?K*ieI8Q(b&8l2{ttSZ#1-R0_y7%|T33XQ=mKtJB(
zk<E;y*gM=nxpRjUBGfPH1-Iw|BuTe%i6w&}u)M_h9Z%1Tl7kJS=TP1)-~nXaNgcFG
ztSUtwcPXv8eDIys#yEFtT<>Aur@qd!8sc%-r@%=-(D|lWzME3;BbL6SdUnCxjLhre
z*9#P9$f(ma@e5tnGY6F<j&nYBR*hj4EGKwLoN9eY0upAORbYUW@K3X5?a6rM7?gD6
z^faNov%kv+>(jOqXE%J%=WV_CFmAl2=ju^%J@+ax!If++y%*T4K9#*KQnt{3QzSnf
zeI3Q;&E;~qaRJI{U&qsK1-0!Z396|Ieect}J6}zD`oQG=6Fzm|JbLY}mi)c%cA$wv
zWEVIe5@vk3x&zU91CIAPRlL&8>J43;aB#ZZVJ%;e@H)4`!QJ|W3r19*+S(LapK16#
zX)nzm?9zBPer=X-m|~Y;+=P6GzJc<@a1qgBac+@_>J6?sx0)9#0uIgLRk{$<a4A|+
z?(2h>^W4|ptc^Whd>247aZC0G#szpfe`)z28<$Nb{H<|8Z`$4e*0_9j%3OdMhQmJ_
zdpP2=orfbn8*4b?vvr3fJ{xv8V$&?K!4aF*3|w)xPy<{y4Ia3D_8{QE7Hz(c+?o%$
zH6L<oKIGPX$gTO1Tk|2e=0k4HhuoSExiueZYd+N0e84x^{2tWSe5kGYP+Rk%w&nvq
z)aTD_&4>Ct-#5l>lLudn<TqaHmdnYu>2v<riN(<a{f`s7={BO-P~Zv!Ukwu!gkXV{
zL_-B3f^Zc0n8c=Eq{${A=b+`lCLqnmZE9)a2wWKm>@d!K`pWKaF3F!E3kE|%P#888
z1_nW3aV|FSE-~O9ivU;T56N-?J{)kYA+R41hWpKbmM}CLA_$fch!I2(IF~3v3<Qb%
zJHp`GIEE97KT8-24uJv384ehTfdRtM2nYs>_#2Lar*MC_lsGB<vxFf8AuvHe7kra(
zC?pmHOYpy;3%QN1?Mi1WUi+UVABKP+U}zxuFbEnaa&R~VgZZ`OZ(ql+z0H5NA-5zS
z2{t4e7s4RW5S)%eK|y@?Hv$G$ejC5GM*#mTzrGN*>A>T*q#y(Zlq3>tN%Y?c7#y;l
zV>mVUX9R3Zg&|QW2pVX27#64w5Z5Eof)E587l;2`g>9GnuS1tVvyeyx*pN8QiiBW6
zK!^kz6#WzV`l7<Xh;0jrGXsByUtbmyj)owB^?_mGn<^g(R2dZcW5NH6ux$&uX*d5Y
zVK5j(5U4I-;(^A%vLWFJ2om~h-3s>b_Z-_a<bRYf1Xd6N6$DBj1C|g+7}%47=wItr
zK>9Yq{+!a^5`Baq6e5U4vmt<TMF7GOfIuYX*P_3jtUs>u5dcqu0lx;00ptNmLx3HL
z`5OrX5ZX3={W+b#CHe?}VW5D7A)vrn!0`)(fWV=@7JcM)9k%HT{#gkFdlG|_FhLXq
zII;+^B~j>KOFr@klK;n*K2ET39yJ1sh9Ci`h5%a<3;(s`BYz<Ie@^Ldtt3EnAiyaE
z1OYyv0bx)8Wn%u0Fw`~?`{QaKrz5a9JQC~y1Qvjbz$&4UzxH>)C$<sx=hXg|uD}t5
z7Q|^+oGT4rH8cir?#D9r#na!$ug&1*k4pZghYutSC;2cCL&3q86omhntS{CGeDZsK
zZAM#vmM}PQBd~xlV7Gx=jlcq~{aU^NPP&b(Kd17)2>zxY4Fn&9f}jKeVPFFw|3<!m
zPT5A-A6NP?AZIY(ej$L;#|0p8up=?hUkg5JyQ<g>kN@aG!Z5%V3xXYq0DJ&m50D--
zE}664JN!$&whR6rcl2Rk|6u@#fdKLa1z-aV@D25M?AoR$HqnDW%Ptf!#b6(zfkMT3
ziD(oEi~fdPg4@LFk1Ko_*nlAP0O1F0(-Of@FkF1}H-urf5%vcaKA;FK2oi%}2V$W(
z&Gp$7!9e7O!v2IZx08Pf8Q3q|$oq3@AIGpw@?cO1(DiIcpkT4MFbNI=2I?m(`Yn0e
z)EW9~2LqSj*~9{WFA)hspuYl{ZDW6(h};AZzexEHlS2aErTxCm^!+fK_YJa!`I@>f
zX(K2I-!}jKecEU!M#ZIDes}QXDjBofwKlUVv2e<w{G*SF%lk7GOPw!EoaT{azxC)z
zbsr0<$-z2eg|52iH^q}X?~T0IJ<=<6)=K>BryCx%&wE^~2^4cUqe*HN*f<_*5qIl7
zrQH=g*w5e=TA@7SOy|$yKbk-xY%EO6!i7%}_9)0OhyIlTP1x{_56v9=YAdPmYnX!V
zd7Hf0xE}|RE+!HOKJR0m$=UVnj^xQf*@Rs@VHq5w1Wfuz2h{V(4rU?aXb+@X>Kl_=
zymw}r@0DkAJ{x^^WhK7~%YCS0D6^bKSC!L5nqQsC>4VO~!m;!fd$n_=)DqIz=!aRo
z4`^A~!+O04+Fi~ZIv^v{fLzp2ks<9exccA)RL>FBWX^O}&YZq)@eMC6y{&0ztvvNi
zhS<aI`kU>fsTVvQnbpa1Gw%0M5L%w4>pJk9<;)`6jew@M)zFAyqH$_J@;A*y22;!}
z^fabDx0Vi`p17o#I*vh-_^GZp&(P06y+Y+~RWaP9;-JaO9a;`4h@iLuQ+^-*V(9p&
z<1_UQ3oNhmNTvUH0p$scqVozAD(I&#;=088W<;K)7Ir_4?xwvwbm>(U#G5NH|BT=c
z$BZu7YvQlCoOm8IAY}(7`i7z+FYCWJGCS<#xpZSqDLgWw>W<wI!$r;^yP+|zFv+`1
ztZVlBV0JA~Boj3jVTn~y)XRWpA8o%{c>m#Y><hcJQ!4Y;^LU!v)G^3v@_97B#d`)@
ztWt&0UV0_|crNQRi3tn+2lfUfHB^b~DAq`5c{gcE=Duw{yu#e_aN_K#9KNbZkMVZr
zg||Z!i0=N%3I<o7)K+5o*Gaqj9vPV4K5l>=?Qk#8FeSoEXSUNM$Y+XNO6741sHhnA
zX-C%v&FID#6$iH1o8PW^@yc?%Y6sstp+jZ%r&J>+6?FLerBb=?d+QMsU$C*d?Qx^t
z+~BU+zOY%R-be1COYF~%?MrWZ^RP>tmz=n=Q@&BEN3YV#v|PW!pyP3~u~lA1`<f+*
zO8(Hhkokc~hXZd;C^`_k%4J%~^jSPsN$9Yu+_i_>)I*5iQ$wiff@`C$&k5AH8GpB8
z|E;`PzMDcdq{|lX-#glSQx&HisAz1@J4JFWM*K-yhN!RQferafYiRL9xk_%I;^|K7
znPYgQjq1=-aY7jnyZvTUSe9OzSDKZkQD@y*9xYok=Qx#^lJ;VrjdGGS@NstxVFVwi
zt$k8%<x4GItK%2hDqnxR*=s1(V-w%o^P<V;`WrJVm{HxXVio6;8M0|S0nDFPqPeQ9
zZ7Nw;(lR?0D)5XPD4~`0FSB+o#)MyeoqH;QCR8h(=MKRbuj9@#QUV)&*3?k`<b0D6
zu_q<Vem_W$fFgsx^xTirBb&<R+w=&4k^%bv57Hy(t+WG(g*MX&=&iH^(EOk8Kc^Gm
zf?J;hhHLY6;23?rZlxzc5c>IkD?I_A$j|pU1qsq9f?M-}=y~%Vvo#-PYd*}@e3-5I
zFrVl9M%ixC`bC}pGq4Q@N&@Fe5abC2H{)jjHbc>D0HOz_I6saez63Yhe5p;={TZ@g
zpc)4W6=V}cfIJ4s7{TEn*ur7~olpoC_G7xfI0gVa`Q9zqT+BaA84M(PK*9$X4q~Bj
zHbGptgZ?!X0ubqKl>I?}>I=`H0CE9YHBdbQ){spQ;Pe>Sk1?&kShg*|LH|KdYKt&5
z779UQHw#jLCj}N0ybebT{?Y{nKjC|pZSKH7yO<!Zhd@E~4XB8MKyg_-49HT!e&XY8
zrOSYmx{b0wm@fOmGc*oq0~<gPAZ?%=2o0_v^7$K{ZQGigTIvt;4CFD;pb83TG!Ul(
zo}qxh4Eu@O{vV!gi*!H;{=K^YpA#)#c!mH<O;C^x0Wt_U*cpvPgX`b0Y+IzWsmT8@
z%iy>W6^I!K^#OFshDN}_{jXzbGz7a%%>H03{e@*v@IAmJ!3D7pB*>hjVHj}#>u4J2
zS?D&>wj1yNbv3sv6AIvg7*HyQ!0`%f1k`4h>c>FR7t6F=$i9wF{+Yr2Vwq65_#VK4
zFaV??LB1W>rJvZ#FG?0z)@|(i^V%N;ifA#oI3Egd8n8W4Aa{U-{n{=GZnsOD?#Z8(
zevtIsRAo2>34zPZ0;Cdy{<UEOHuZa+Z8}VUm@?p9;8Yn7=>kp$3OE@cv;9kj0hqdt
zus^8%;Q-kKmJ;MzfaU@U1L#<s=ZFF(UGQfX^DpcH5Vu{{HXX}9#J4Zxfiw^b)YKrL
zU}b?*jsmU*s4)I9<Gv+tn<KR8@BSI`wloJ4l;i-r1Ve)KEO2R&7+livhqP_!4IZ#j
zwk_(P%b|TC3x|i};4YA*6$EPv+-z|FV<TYV=m=`qtvn$T+vxgQbKuaR&9LeBN`*IL
zr7x`h9W)5&Jdlh09Iyz;epdzlx7ZK@`n8GQ!iGRe2JCmSp=bIEkspgFs+zM~)Q!BJ
zIks9S9zS;Uqn2&8E~0pv2}xaUcp=)~;MDz@2r7C-{&`C#-2p9UD<^!3-9(~zw(psb
zyk8rVEt^c7uwP%axaMhak-P1-hsRW#mZf<Gx1vni!O5(ti8Y6#U0yl8_6|v}bj#0|
zSI?x0KT^nWyf(LRn_9N6qJQPor~#X<KWw=6=#qKQ(<Uu8<=yYC^x2bwbPwNl_qls#
z&S7>zxru*b|I+xGPb^2H8H>kFUe&B3*3E<uu01cKFzY{jSR?l6+4pKOm?3_Dy!);L
zdy5D};d9Nxqi#jqv$uWoM52qikL)2@Z-z&S`7V=)PP}$1Fv~uxB~Mts`%|6;rT$=*
z<IKQe8)w4`6XxZ$7W2R(RnC)3TEkq$8EFPlAG&fG4WjSnIFh<`wJ20p_ApjSstRhO
zN1RTZpXt1VTs^gleTdI!KXYH7u5_Gv&9RZBct#}3t*giCHHwnohPQbcUv*A2U2d#L
zb3v=fl*@fZPhn%ij4PUNc!$t=S1!ktTAv4JH1pN=y24A6iq6Qah#zv~rheRNt}Uvw
zpY+_?^Rx4#uZl5Rl{5XXu0Lzd9Z<bd)7qaJG#_7QdMh)f$<Ie)zzKGbVm9RNW#?f>
zy_*z;iZ3mxUlE1q80dAI_uL{-!8=ZDu|re9KuF=1X0?Mlt4TkE^bnOJ_H|?2*$YC3
z*SbuHb}kn&Tsi!yLB4-intz@^kHKYGj<>EM0;W2y_YlWN3Y<UYmC4>mN9A~s>jaN=
zp3y>C!{H}r7CYGNbjEAe`KW|78+*DJlrK+9zq#ZUt1Uuem|SFwnBpJ|Ua-NVtsdV|
zoF_Cx7gcN3z2JP<KL3@&c^VJisX|*Ag+B#lrFo%B58nm1lfL2<XEGmD2WopBc{Nd=
zaA}8vn&N8N-RYC>1)8~f^e5K+9uc;jJ;gZWnjw?Xc1if5cBt?GK2c$F;+w}v^B^({
zHJc3n<tJKFR~~u$xk__o2EI=!A(UMRdJ|$NO}E-jjvsz1fBD!I3e#-c<pVas#?C=1
z>>RheYY~jWS-wo?FH9D|VnxxEczfDg3f|urxw#uZWU^U92D|^s4A~8ttL6JL=_k%s
zc2jbV3?7L%#z!KQC^r3RK$~Q0Jg7l+nn-Bju33d|ehD5oKc8e=&8!MR&fM#~yUD~m
z@SZ*;*{hIEv74D>2oYj$sSqtZFRxB);78$f?rg~cVwA|!108IZ{`;H4-yvt|YGC2^
zkIe~Zw3r?ySKK5O^%_BBJW;4SZmY0FvsB_5AAGosVT6v_wUd?=^E!X+;Tys-2hvLk
zR`k)YXXSO~`SS-%HD4rU9UQH%Uqii;7bQ@eceKpeasHt5TP!2J^Ch<vw6LYNOqW9p
zdJN<Va;dGfJx)bE>Ke(b0xjZxvySbQuko0?ZW!28D&J&0f{{Icb&P|c_CxR7$Aj|l
zCFX+AT$hm5Iz4W^do2P7ZZ{kYx}ece?rz(P(T$A3JBN2Ge{GOx=`ecuMlNC@idx(T
z?ey`MI8){=mjVh_JmI|!VO#_A^8-7rKh;<Jjd5j^iIT36Ggdb3cmTbjzHd-f#~%MA
zOM0G|%7RKizG7C1kCW0|l)|Y5e3%5GlEM|D-EoXI?HW}lq*anWaL~ffJR)5iT5YfI
zad0Tk5rj&nzK18iuo6#wAaLmMz3EeZQU=!yS@>%-?I?7f_CJW)r{C6Ma*$lSlZ4jH
zu*F3KQ66m{uG4y{<cN`M*fDS6DYm;AK9nR!UtBw{hEA^yn#e3JVM_QY>e;Ve#&7#t
zj>_1D`y~FEd(T_Aobvd@J6;IgrWxd@H&-&jFB@^HQsQtiDdyRu=y;+^MWsaCREBia
zfi30a#nXgd*XF~ISnUedxn62w$8CZ~x>V_p7cU+!U5!uDXWB@-e;EN6zkP*lpgR_y
z&L@H>TA4(!E-rNqx(HKQdDSy<VLI|Km672r(@KW)b+t2@WH(diC_e1dks#@BSGOiH
z4T2gv(pM8&9CFhQx>TQ=!95@m|B%6Vf6;9d;{?v($ku7(<4n#2+SEi@4L&@1w^R<D
zyL7hZaarchow-+}L@?d_`-U$ED`dAH-3QepF_Fik9JvX90X=3R7R6KC6t$m$;y`Z`
z-u2jq7y+8#C(Ajo!7Rg}##`o&8d@K;D+ck$!gKT23fLHnUKm?!yn^{MEJo2zx4&nX
z@aDFBm0=v4#Hw^%hNFp(-3bfv+WE3Djt_ghqfoElX$LLCdmgRHzy~M%hTR_yFnmzW
z8{V&{FXVCT>Ji~?kA8cv7URHjacL;LrkBV}OGaa%mN=P1*)cgQBUVz3@|wzXGW{~T
z7ygxo6+`wh-K8bL4ZGjv_tadhf!&-8*%NWsX#Ze>Xr}e9y+U=}?lH`*bPPQ7d)3KK
z2UGhbl<JNV77y&0W*vJGqSq{B^ax&yCA3!=YK1Qe7t9UsUTH?;=I1W+>N*9bKST$`
zb+q)5X{9{BrSz$eu)LoA5~sQ>dvtDbg!H~3<&iXHJ9;e%jynpD=Q-3SP&=R4S5=lT
zIS0}e%+W?+sf`mv_zzxT7#x~^wiY-?7DM!j`pkt23--(-Ep!c>m5t?<OHU_{rBeuG
zoG|J?dP_e~3lhOVcJKBPH6BrY{vqQ;jriRmZ7oNTy>_`5r8#a>9>46j<8tz$a2vXG
zX=&@MhwC|yBjRbEOdfveq?Du<WCa}$-s2y-ms>REuK$~oF+GmOR|nV!kICDQ)15Z5
za*#k-oc1vnZ$X}s<)Ma37R}n}KOJ(KNFTSmdP1jv>W$Z2_Keys(IbfaEX5^v2pGfP
z4boqiz9+pwKQz`SS9(gRG$+xZw6tYflAGDq;5PN`IQ)^pCi;VPf!AdmutrwSkF8$6
zxYH*2enUWe%tOm-$P^y;c-=d+<M<iQ{pSv&MvY`5k5qaPm=pES1$^4+kWsU$WOKS<
zcN(v98JvVdWi3AJK*he=a%;T1GBq1>hekFuQzqlN)R-k65nS#+{T|wMWP1PIV{AHY
z8_yT~6tvSH&JpG<?Y((xgZ@NDKxQfjYk5L8uP?(}gDh9lM2)PS=}kv&-g)pc`_>tz
zHvS;x@R!fad0DT%d%uTHPgKHtlKNR@TTdm)5!3Dy>F-JJ8JpAB%-s<}XHB^~+<Orm
zM$6ln=cM0aE_FSXwp~gzCBfx>M6>6m8&}hcO1quZ!2@4D5UA`Ui(EH6<Z{j0C_YS2
zpKzn6&~<8OBxy(i{%O5|^?5Q~8@9eBF8e2BE8DSp;P-C>Z@xPx_u2XT2CE0oE)M$s
z2UtCJ3o{4hL7Nylb_+8H^-Q1de}I{9j``yB{<jbS?)>UsrT;JxA0k1;GXjK-0L6g;
zAPfryMRA}|8!h-_Y}(Yy)f^&g?hMY*y1}8)ZD7;pFZn~%fqGd4NHl+;4x9wQfV%eo
zf;#lc@1u=P2LB=QKz%(Fhv$GR0*c&Brz1f!1y>n|ME*E-+~l65xr-|di`@ooZZ7(t
zp=}fW0`V*yS9%N}6j0y`iU~Ihn0}IT*rZI&!PUjo(#8Z1-42L>0;vBT-uzbVa3y<C
zz_Bgr0P}zp;tJ&emA@kIKVk>_9<spk|1au56#02W|5iQ1*FfL4k-w})*gSmwWx;+j
z6n88{5TxF|KT`c!cYGQAlh_poeij03NSrSCd0hiI0*xzV{hXc?kOkN}0-yz-$ACl5
z2<YcToq#OR9JqM>^E+UpBDPMC|JHe0#OC4YF9d%Hei0yp_nU<gm-LhzUSp`MR&*^L
z;Qc8w>Z*jh!nhb!ulAmZ%+0+Xi*2DUk2h8L7;@F;65EDfx?a>W4Bp9=yU5-7A)SYZ
z3@t&s>O4JL`*@yNfPf_z&f_(eLn(bTpO0SRcog6Bf&SZ+{=53@hV0eWh7`@b8ut6=
zPninNkpzom)LXZtl&=oHZGXISKGb0^Q;8ft)x{{8B%<S_yiM%)x<mWcL(VdEiW2Xk
zffjlQ_prT9kZ~dyJXBJ>=)LELQVjIUEzE~ltE+hg=Cb(MPyyWpIdxCt*c&v8SE5d-
zXiyp2S<Nk7J)aji{+63DYH1nL9ImpPI2~ioE)eo5P<a1!^H+_O5hc?|3&^0zN5Q;y
zoem`DT3a`gf^YBJH*bz*QSV!2nZ8q8OShBSobBpNg^+MV%9ZCKkK9y@Uk`Bb9pwmX
z3ZWw)awG8XSzB3HUt7D%bKwNLO3J0qj2&E5jeJfod`R8%yd@3nU;EW4h6GKslj@vU
z#5^Ar8B5?$zqej)b7w-WPx{s(@`xNMHB4IJ=7lMD3QeE-9wR-&7SShD{vIW8BjS`~
zslq(%Y!9;?3F5*fyd(i`ys15%2N!c)&Tugvxuq!JpYJ9leS9$?sBhGQSWX6#@L=&s
zkQ>W+f=Z1L$Lfxb=+5!ZFclTkjwki9t(|)-o`y-22cyH>yR=Cy5~`o*o|r}^q9;(&
z8P_%2QsIO4)HUZ&PYcT!cbM<@;wP~^UB%_i8pYQRtx+8wU)@<HAZc#(WFjO38H{-@
z5?NpHkwj^v6OwQwJKK1b@1nQ!fQ$#Df%Q<!2Sse63yVSTMBLtPrQ-P9bXfSI;QH`c
zbuMQ6_7>tR)(lCO=xL39xB~vcbc4z?y0e+>fk{uO8VxSx)JsL`#SpO_)l?5Ep3nct
z!$tP-c!Z0YM3hR+IXT9)peXi65z5_{?@dTP!oPRERbj7GgI7-><<LGGeNvcI^jQkX
ze(((7+wO#WqAE*5@z^r;wJ&qaE1IS4n4&jlNq804Nq%{wKWT6C3Fm!bdx}VoD(m{M
z<#Er4Cs@uyrxnfxR>=?MbQUr28^w5Y)XW`dI4$3LN1bVX<g~Y)Y_qsWjsyRV=_`SO
zsu!0(R2^R`k8luGG<bZ#>TTnmJ?DklGYHn_uO8>NPPo29{Yt~WL&G8!?fupvPuWSc
zp$Azw^GiY=Cb(~~+`XboC0|B;`8tHlL~HP<Rs=hP?x_m4X~l(e*9?xN*Lkka(`UUc
z5Lb+~pg3)GHW-zu)W1J^cV$<Z-aL{4O`^5L6M1DwIL-LI@zb~!gUctKTjIm(b_Xb7
z`NQ?DjyOEA+EaU_&vPiYjL1PDE%1V3kP+3uLIFICd2}#|PJ!pbdoAh%UFLg~@&{ft
z<#p|rY9JZsjE1REQd3bxXoT#Jl030M9$$d*<H}Yx+<C;&nrNz(zM-1Xynj}Q_%&zF
zCB+no%pzoHf?6m_LElz!qMo^spF|fAZ=+hERO-wrmOJ&FpD3Q>^5msk;c+%P6ix_b
zoMdsZFS@!Olua(30h4Q%tzmJnex5Sv<r@X#4c*72nFF5<@1m@o7+&_S?nAH&Je`aG
zz<H!}H=jyd^1Yh!)G~7C1#9gO$F;NgNUuxLC%FzSPUYiCVEK0DeR!7Ga7J6!gSTDS
zs=_Q(<MFW^rqd&LXXy4WW1>T8MGopEq!cXd$#*}y|EV^HqtTH!kxs?osd%8nL1{|K
zw-=rDvJE5$BJWbTECf558}R97CRvB2_p)PTYRGO@&?VICklci%&@|e~8;(wwUrO&1
zzfb9XnI-N~oF!a@J25adIp024H|T~`$X(K;;=8SbT3RN@<`tLAq7G%fs!7%K36Rxv
zNfV)ZUnQFVmS{BO!%<a12~5E2Gk3D+ho<zV1r<I}F((jaSPxDjYxnH1^o{1Zz^lL8
zU2hlT;Npc?<4K8Rt+f!9Lc{o@mmkGw*VW7F>T+>e-MywcE)|nT>O}+ZAWKt0xcKd2
z@Y2ZP6Ht8{Li%8?R#i^#SP**{!ii<h$pBuX+1#3%+*6knO-#jFPeH*=;GB4sWbZJi
zJIvJhaJR<(y$1`qw2L1@Y{_QMva;cs3hT58E)okbN@uS=y3LH2gmS-irFjXDbzU&!
z96Oii!$z}D=N<}`vno=~b1$BYN}kJSC%9?TxJ(gnpAAz=u6pxY7}s>q8~c`KRYodv
z>ZE!Pg|aqb>^%jN>QTwix+=r?G=6Q=YF53;$5f$4)98e1245vOmcV7BTdtS3Hvfa~
zS<#A%k6PPF>jwv0T}+R5A&T#EL_0_CecY1~Rqw)kwttF_LFCT(={2LP!JicQPZAeI
zoUBWB&eg+`Xt5^L?Ds>z^i<&$F2?txIv1$HY_``&<rF`$Qqo7-gXcfdurs%p#s<>U
zTs-?yde)j3KP~0`YVE=4%tyjwqwnRtW|E1PlUMoC$Chqzjh~ZJ;0w>wRE{{dFCq1q
zUTk0b`GZH)i`sU}FeIANF}shlKTCvq4!n1gYm!58(VvgYXA2KyU(~Tw^d7?>8YA>&
z<QzXqAb7)@Gli<!c9@%81beI}K9a4hig5OZczjKKi_tX|9iq(Hd?}0lofKSLYiXTn
zJ1!YZJ$YX(bgv8>p}Ib?Q`wXKtXYWN9Rq1{3z2xAnRs36oe?u^7UnEV$1kR;?c90j
z><Zsf`*=1LYv0<3wNJtwpIW?muyy@IycRAw#dxMs8MJTcotcVK>POI28%Gl-SzmF<
zO1;A;4{_w@rF>J`fm!9Ms$H=TeZpdO!67iNuJ~r{Uc@Ww6^>>--xJk~6k?2g4P2p#
zYi(i$-Dt6_6ie(??-s&!S1kQ<d3{?1V_eDI=A#RF+KU!7#u^lA{5te2^m4U5kE)d}
zA5$$_L^qb5$D6Ich3LgQ=xKHOV!TZ3I+1kAb?{)IL{7ABgN4Bo+@;&oCeB@ua&d#E
zb}6c`zAp5t1w`^XL~+_SF82sRrKlvIu}99W&OMf^$acXsIb@cu;>x3>64fzthvnsu
zA)lH%@{FU$DRd;HjFuJ&+Daqj<$WfsE}5PvMD6qAP-T!Pf2JB`sgfCYp)_s&>CP4A
zRhu`fw;1sKw8Cz5wRW*ubPdo>i>Tu3I1<G9u98lKn{mm#5j(a@MMNvI@q<`(v!%@!
zN9>1nWuJYuZ)4TZU2*<Nth!YP22PoMj!n1fz(98I^ZjSf5&=>yxO?1*+RxX|pF?al
z>_Kex>iMP)3>@(O%YFZEV_ID7{8dZ~4k&>W|AJ6lc?pPsz!g{BB#1i=jrws>$CqsT
zwp8BcPyLqD|7#HdXbS~_g6d^7NZz4A&KP&t7#vl>RStpTiC-o2wkM9k&-r8A!kwA|
zr~W~>59)-#8E8<Zh5=Cru5JkVlk;$2*!6u728u!c7+s*r0|W~ab-2P3C}`XOnl?a?
z;EDzT?ca4J*jCL1GU0!WU!Wof0dk#aP&5Zb3qU4-U!Y?V8h29lZ}|0n!2fxt{2B2A
zeXc+k0>Gawx^Vo%9oPFEy1p+m`Mh&~NY~%DM8ma@fC7e~K-C5k)MJ7e6Cwz%C_ork
zKp^P9*%Qsu)zwi*K)}Pp1M*crG=Xm$q9Fjd@sp+fO;Q2wfW(3WBw!*0sOrOlJ&6J;
z1Dsj<??`n4v^qLF;M$0}KujI1?D-v>aShxAwus+G_)quB-xCfxoq$aM2**`nf$TG`
zJPZZ8pCG~JMQ^t3_)Q-FZr{1jI{P0;1QML*!yT~&340{qE*hNe0>~aX5{%P)ze((t
zK-gNjxPHT6+>fv`u@}I-VP${H0dU;G#Kqj@dya4Rd;9VmaA?h!)^NDqdAO1Wpp}2p
zI}i3fy7bLZP}zd30sV37yiKe3({KGe6jy<T1p@dzM)VEhvW?=;V-dKbSisir$09cE
z{g*HP3BH6Y$NchBzOIJ&uEX9}P!V_sU`ybvE!cLSd*}Uq)x>6lxi6G_IZ*=+VE-;~
zRH@tR02;9cd@{Z*xBs4{MjsjU)_t2|h4@2CA|G#$2eP+DGw5q^3LU3^7tnV_jx##d
zq!;>;Q}hk<<ch!6y~5mk^Y$~RP_g{_wlGENIH*lw(bb@mr};_y{H0zR40T$>?@3f<
z76|+3g*7kY`|xb;!<cVTSZ!^(guEofSrfnaJ}zF~`6cQXBOhDO_&pSt<X;%5;ZmSB
z>~f!3;w^N(e8Vux^Fg=Gv&<oc?7a_!M*4!0iCiy&RH$lq^zDE_=`XX9Wj0XBXXx=n
z2}H>Vg|83T?x$dv%Zpfz%l2ye5GM8fri0RL|09O<O!+MId>0jn-p7<DWi`f`=C&zE
ze~P0V4_7jC&r{*-6S;Li8T$Hkkh;T|+3p#o(J)yZ>8l}Q!r@AXP5jA&<SH-^%d=Ip
zbuDHiLhCut2N&|IdfeJut#ODyvE-%$mQ<l)Uq|~5K51b$f`~4$x6Ok-&POjFbGE~y
zvbu3_`ep@UMv%Q+Gs-5lpL{6G&*YAS(s3=-(byzinmc^vkIx@fwc%KXTSugXUF<a1
z>J&zuPq}K4RCb4G;J8J?1%A2UAXAgkwAW+!N6in9Jufjx4%_I|V&lzkD=8^CJLtQw
z^!`i1T$>LC40ZlGN|eXU%l6)29lL$-lyPW}5m!Xo5-H-S3e9j*bml$T8)70h^Eb6^
z<g_Pc3VT?t#XL@acF&{7(BI7CwPmEer^lTJ$@&XNgwg`)Sr?L?MP%8^JXbM^iyj%0
zh(su_U%QfGS2mQ#j}QsyyXzHiy{|DxU4^&51iz4<jV5@v8g;(Jz5s;AM=wNT0X`RA
z%*Cq`mu`lOo9#RjF0lu17xl{Y2IY~J!yGjZ+CFs;9(zzRud$Ut>8;cx9AVhXDolB-
zn6>ZuG6DNzjjLh@l;$R{3`LzV?pP=7GJk9&)Z#c^TDo!DHeYhwM^MP?Al|Iy&AVJ8
zlex!}i+ANZxEE`8(TT7!9KK_F^i=+uKULO+TzTp8*tNxnvf;^N=(5m3M$^@p!o~WP
z<XnoTR2}&<X3Ke!W8CYrbYwf{&kH<jZ(>4RB|4K{|J0P@mUSa_L`#d-TV#3qxw!X!
zk7p_RW(?e$11Szk32I%x?K)+f*WjC;LEGtLfAYH9VWo4GH|55C-ngG`;#L>W@V~1=
zY{k{-JXV(@F~xPG>`E`gL7DCYPQq^DbzYYQ<FxJ<<#~%cJ)iXAWYB87QY>Ju&fwQ2
zSE%!>#Xo4l<<s5+Mgnhc_?0;=zdZpVC7?o7lpU`)QeJZG0Fuw$Zc;GwN{40B0OK5O
zMvaL4yUAUxP8|5rt3sIwO}4%X_e*)yu{pFANw+>u$)=UxzIdnnN@Q+Jw4%vNNqXqi
zaq0y(^-9e!JQ`yBLuAs23VL2M<@Srewno!4g&z_%LTh^(nPrmH?<nwlCLG?|ME36K
zV@l@?wfR~BR#oo>0{zgTFcO4fZLS%Fku-5v^@kmPV~gr1t}nkV-aAjSaJqO*SZ~+x
zvC1{>RY^i`uR?>TXHIZ;mfSTHvUv=<UAHf7V6;u5WGP>)&mWmYM*aYAP@&6sJ*_~X
z@cfmAPig@}AtZGNDaP&6l6OQ0n~gJa5@$Ofln9;_QJxpEx=R_W``A#+U^eZc#OTF5
zd^Sp{l%gzKb<utNZnKsJcc!%&Q%`yJ*mb?Vm?k_mb!iCY$8uUY;i&p2Dl@UvHTa98
zJ6Mg^q`WKM!t62f41T-&JgYe^8+7|<tar_kWS;5P_U@8^myw-!8128}e%R~*p8OHT
z*RSf8gZqf4CR<tM*n@(86g_^O@AygdxEZm1-4PVA*_89QGo6Uft=Ycn3<h#|UpZ=<
zwrne3@%No;5#KaB+cx%diUv4vo4L#FcJS-a@+Writ3p7KCHX5c@?Fm5zc+oGt?0fp
zegC&DVfnsl2|Kcz?FRy|*(UD`O+OZZ&mHu>5rEH4_I?*W_<;aycH{f<wYWvxYS*{f
zy7<@b7h?0vzl{Ci8vb4i>zjr^Upx^EsD1y<6xLlm{RrTR`1j#F5u^4BX>|$p6Zw`G
z3-H_6SEAoZ@U|Bj=sUZ9Y&GuNe^uxs{8^D-uJlEw=f1PgB-$-mTSrrQ#}czAv(OGP
z;TTTEX^yfU8GTs{%@vLPt&%bxJ)@?+J`JjsR=PH~+hVds`_?|XD_k9mdJAF5q>+`5
zE~Rkzkky>dGuf2l-lRzfQ-sH<G;x~nxHU~ul9;t~OvD*g4+!}`O7wl)UNUcd)NA_u
zjn|A^YKf#~QG4#gciR&CV=JqdrUuWqv})`oEn7KfKJrMc$h<GUn?iBp=~{m2MlbDJ
zS?+RF%EYryKg_V3or%6}+1f<8Q?ao&Qm*|rPivflLc!7LbB+_4&FLv`vO7hH%bV8w
z-?&R@A1T*;6)<;xAjc-S2s+5+Ip+D{d?I(`wf7Cy{G}I-<Fj5rd}9C7`q@dfM-kpF
z5IGS;f%(oOX)|x0mpR;RXMAupDa!0{;m(WFG?a+5W*0sQ&L-S)CUsNPxF-GKaaq~^
zOq&%#HVcV8ihJS`Oi$iC?cfHAS#0mf2(?NY8uag#3#GMcEscF_LEJ1L)s>}Z+p=mG
zLZytjs(_^?$kE&%+Lw5_4Es@9&~lG)p!n^RE#Am$kh_)=F*k!M%9q^ky7}f0==-iH
zjcT2!-hJ{|gchm7P?%b5c>3|`a#FYf2L}z!d>{+aWlfnoOtB+_WjW+~*{h0?aC^0D
zg^yq4G1ZmVU33nNH0)Y_VtU~Pog~4AT!h#DL&mmE>6N9$(}b6Y?d1@Sk}-Si*(PEP
z#FY-veJYjY77xYVz?$?c;qk2Z%k#x>!ctQ9-eJCZnPZy6Cy?xU-E9hYv2NN1*u}`G
zQ~@Pe=sBrMH|33Tp938!G^B5?%}>$J32?@#zAmF&soCGc`gAluEzg*xGLYy|1=cNl
z>h$3?LPl!*_s0n#1da;6$jE%{fKZ!5(Lp`|eIs{Au80}e^%J(w57sRC4b5Atn9n>X
zB2*`cpb@=j=UyWc;Z(O#Nab$lB9d=)29Hkqv>zUq$?K8(QhkIv@z0)|?K!bLDc>43
z-(90}jQ^E`IAMY~KU!Sr?gKIOn_FS*&Cl<|wH>NNHA4qZFdkY|t}|N4!^`uW45pVp
zHf!M}DVS=ykQWI_yC}7^8xnLmp<T<EjR+e<ExWvyDDmzv=Y_b_*}W3YXd#kphan!7
zGx*_yrj$G{j;o)qi#4Gqx@oq%WLcze?+(pXSZ?0_<y~VpHFxmWulDcfFiq?o8|Xh+
zbNL9pNUU;<K@x@A+BHlt-veZ8tSWae;l6~aNA#M$N1uy%h+vqRDANU5UR*l3K$5ej
zn5WNTde~CiN46lY#?TY0EdxnzBYvf?HWMnt`jYhBgW__9lm+;K-0l#jc8>{`o615r
zM_R3-<zLp!P}m+4X?0=Af4m^_e6Ou9-9rXya;~%HBYGdtGrVxJ+_B%f#?UdP*|2!o
zYVYf7#R5|gDDLrTAB&gsRT#AEI_qO3E_OcZ{M9G=^yGJ2oDLVroNR25FxM_692p=x
zPoIhJ?{Fp8pp5KY$eB1)){|U9a;euRd{_pD4f6BH4=D$}aUY{mxi?Fqe6oF%QN<E!
z#{OJgWayO<SsJudGCnXqHhz>Ui$({*b2#Z_>QvZ$WsRZAh?CYE@OCN^sgY&Vp{Sr#
z3a{$Nb;87ilv(_>pANq>snO!UXYgn_<9&iL+5ztUenQ4&*014ioc!pq`r*E7E2tQz
ztEnk%t5OZU+}2%a#lwTeqOo);mkzvI+9yc0V2#gO_Lk-8aZ{b18YN8$hZdu92RDCY
z$E?izbNSJ4i4NRv&pqZNb^C*=fV<K0@F_Q~`6}`SSx4d*vy}~PX-;E4rm{`psd)@8
zAztvuCzyv-+^)0O;z4@U=KANw7`b9HyY>~Z4{kguvD2(*!N_@@xt76ltcOQ#9)=|;
zUnS2Xn^ot1KA1XENjea#_VDd90TS|Q=OR{vmkHfcuieIpALP(Za9>zx2-ye<^J<Jp
zwXSF}LMrK{TptbdP{yZ+OvGRwy*Q&P&`h!VxSe8ng@9bB{ZrPf+7p=4k=F)#7m8Ff
zd;5!xd)qyZKgbwVx|fV+cV$iBbYt1Q;LIg=#09)_&b!{R96sJ1cXYh7=AB_Xgmh2N
zr~2}A0bTh+w+0ufC}^%)-c1_toS#NqDw?7gI~0?hELT3Wla473Pv&v@Xl;kX#3V~Y
zK#OJSEgH!O{i4Jvu#cKFUjFeTv{#R%L=_b8PU_Dh=up+{d(Lt@;d1RhZtq<BAg9uj
zT%t!cs4&$FB_E2-Y3MmI&_qAYs5qHI@!}jlQagM1;kwrvo$Gy3kL&1dSI^(x#e5Lk
z`lO)TX9Df#g*3(<=5(#<s>H*)Oj3A{N-0oFvdb4HY1DIBKG?1zj`6a0kBGamI`oWE
zv+L^frh-nXk+9&>nmzV5QZ}9IszfChQ^;m_-`Hrc*+}6t58TV=5pR6z<WcInkB@g6
z&~veubF3IQeqho1fqU|UZi3%e;%%DoZ?hW!fB}HSf9sz7)l?X<)g&0P)g<`q?8N8y
zzPKO%EIWY&C=m*j4WhtFPAqV$k)Sy%?lde44Nefle_S*sf6BqZ6+Dr`eA`i|&(!{q
zvfn@v!PzG$I93fBD?&hTGH?Z*=KxlN1fDbu`(Glp|1JEv*<1Mw;W%$_tNHTpJjmkf
z0A}0JUlweA)1LSzLs8%e7!=e*ZVMy;i2WDT{FA|(tvvx+xP9>V2;w)t_cPG|kOXJ8
ze*ooe3ilUg{m}jWE>QU<o&n(CUk|?cwxaCY<4`{<4+EGV022{gZLe_%AgCAxg7f<b
z;5V(jzLbt3f%yNX-~CKq-{G|^wg1cP7(WBDse<MH_k8p%3`K7rT(}`0O60gIe&vWp
zEt9hdhui9kmKz=RT}<9J5e~2Y3%%MM+Q!TWbzkz9di%{(H$~>_@)#XH43E9vF?7mY
z<6z#ajH^f*q0aOs=lKJkPLY#B<zhzh7}A0V4onsIdm!0q6)q}%%hNL}k=L(tC92Xg
z@#$J$la-7k4=0t0zMFp1XXb74v1`yzYl>uqQXc+8?sPnycPMWgWkQbn3tdr8NT4Qt
zq!OYfM17lxy;lAXCP;eykoLg)edwE&ii2H{zM#X^uR9*J9ulw(jemZeGrw@!cH9Ae
zHFDm@RJjGUND(+uH{LW45$mwZnVkA`hQ8ogoyX)+H|mxGzdFs%w&B1NCdgo$7x@S2
zhr_aII5j+T3KieFJrsNFSuPNb>1<0Bc^jkiEQ0JD<+Gg8*@@nz$4#Mk+a(WDGe_v6
z*Q#<JajG%ua7&-NeovFmH*`q(k^WOYxqyp9&x>xPXR~r5WAps36=$&-taQ7rHo87s
zT~l^6*_FNSufi_*PX6gm!%R)ifqkPdWi8cw0wVSpr?S|#n5ph@;T{N#VU|Z}IjNNu
z^^~Vq<#STZxsf`9!(m6lrEe8Rr--i<Rb+NewLm{D*Kf3s6&KA@o$~r*W7y+&yfy99
zn)W$@+66=1S7n9rW7f|eWzum|guIIMOg|@ipibY>WAGxQEl<uq=#`YzqgRqeOvh2g
zJ3ra!w%<v$i^<@7!V!sh`}p=wQ`F8OF(vDSi_lAy^8IIQjmp;AymCp17+D9a`$Q%_
zj>qp62&nKpT()BW{uwp@fHYM)BNh3h_9c6dvf!*tB@flrn~~g?6oamMLzFg-3rP`K
z-!FXjK{T7FH_Z%Fj;2!=iJMqy9L~IJ<P(2AKQfG*$G73^-kc1-@`mZmg+L*@Tdles
zgyUrdGm#JM8LERlMA*(Rmb~ina%XHlWm4(?uuAx4FF9?_47Bu-qlnMF)<ItriV)gl
z{`|~ge*F=763Vi1N{p+~ym%xw$EorYQ?5CU0?TNnnqvnyEmIZ4ZBg=Ncj@b;CoK=2
zQ=Hf2)4xb~eC@+d$$nA27<jN*0k;T29MuQ;#(oO!bNEUkIqB4{5>}UAGOKts6DRxG
zT(^7<pUQmS4izPndt-0(yxxcA-0V`OgVBXaWlb03SWo<ikDivY({@b3?@v428;XjJ
znZ|$aAXdtJIJ7PHvf{I6fulPIPqb@Y6U-O(<rI72?*38(@o6{VB?0Yr)!DqZBH|M~
zU0lsxYG)^FLZ!HZr?{2frYI-+`bwiOaK|w)$u}R#Hj*z(u)(Bn?5*3?R6wopaAHs<
zzZY9Xm>ss07(Dbw^zb8Rr@W7<;gGoU+D0$_zV^T)e9U$Q@@=kpXH@qt!O5&lD7mHc
z<QNG*DKfoUVX}3>ax39uhepS53S1!^OA4BKaOEOVmA{obMChWKb$r0NM7`9*j|nu7
z^bRL}q%OYd)xU4`>IwO#)Cxr+b&t8lWZ$=!3mE!*PZs*J?x#jxBuPvee3O#xHx;RC
zPwr##DxH1O;FhwvK~SsaTh1fWm*ef9RY6I1N3|Xc338-03tW4`{pvu<-jLD6)z^i@
zwP6Pv426#EZexO^SrG~g#1Yx@oi}MVWMJQS&R~bmv0e5$S?lj$#Kjm<!RqJgnk7<+
zb`#dt6dHqXs=72j>~VU6jW^DxoborJr6eG1;T=Ej$RVpKw_M<PAV!roqH!w5er%u4
zrHDw~Ox0la>;qXzw1;Jft#bObqq~W;rY)yh8`?Zx5ij-iBu&onigE~v@Yvf3U%W0t
zP|GoJPtJsfS@JxTu94~ej#qp^W6xfk9PZrJed^BA%Go;J;(cwFN1mC6YwjYpN^6_V
zVG}yaAHLi3rs3-Amnbh8@@qTK8is{`WQut<byP{3XXkPDyA}_^22xvNJ8w8J=Ix!*
zA7wLiP&jekcK7*_R2kxiSJ9#3)Pb6zbxN@!h0?|x`2Cb>&h2yB3iW;OifyMpcG*4q
zB&kOJzTIPAqGZtPS!h=Y>1b}tVGDMZ@Q2RXW}N2p2VRn<TxH5*P7Hf{)#}*FDvWk<
zM_IlM#qmr88;8d11K9+>@iXCzvoPDMURHy9U9@y-eF|PQ3)0$2<d27*jck|VIjKQP
znOw&CxQ?)dl2C$!Vs-9j)yI$Gy+!L`7NM789<l4ZZ*OzTl)Lcyo${n~am;YdHIr6O
zq|2IwKl<s;6rMTdB9WtHuP$4-om-^ZzfsP-<nTO7eq=x4A-edpCkL*UbDnQl>+J01
z*h{D_FC8tPbx8d-*+ggzx{}H9;-k}ehdx9P3h~zrd1fxxGfH-Wu5M6F((w9Q2GoJJ
z)|Ne&nQULr)9?*4p#rEsM1>??(TL;_zQ8T@P!{om=AP0yW=r2>&!oI+ZBGlq-XieO
zFsn&o^j;%MyvaKcFvFU=nS&+oJBEw$F$WtGU0jW}rp<~^ne|bmF%%|1r0O<Z_W$s)
zH_1lvG)d;oxcP`(8@-g*25S}{IxzOBjVshp>}udg#VXya(0h4^plLu7_Gk&uS+TKS
zhU&_T&Ly_JHp>oBa#2WD{}nQKId6H1gW|lDh*KQ_8v%HJk^Or;ONVLGh4@uZ?2y@^
zKTU;=);W;j?{TINvpByevmS#SXcU$GC~Ixp7sO^4B4`pcS8ioEa*%{+%#&>OTzMgj
z%uq;D(;1?IXEdSR!<RpJ`@c<swO*?~6VF1IXhyKVw)f;HixpMC?A89Z`($kvM<A;Q
z>JL_m)6iSy@wMy>dwS>my{gmqVu!Bfs~Odk5VdMV2i4z`5pNsdA{|Sql_fv3IP%8C
zF+AC8El1v!{(@BJdS}Pl8ubN|W4Bq_ue&w1Xk&=d&)T1!I~o7docEp{=B31)^Hrol
zD>+TCDl>%{_FZ;NToEH!9ay+M-ZJSzN*X16<YZ5-g#FAziRgHbD374XfyB3SF;3?c
zRi3C>pM9VC?8Cy+Q+|Op_5a7(R{+(OZQJ7R?(P=cg1cLSyGw9~;10nx0YY$hcXvr3
zXprFU?(h%k?tepf`rdx;U8<<msgqf2?X?eQ&oSqiW8|_XUd5K(D{fS%Q>+piKSVK=
z+m#=I8HL6|K;mJSYuNIrJFYpu5}2jy<&O}UA?c-LCrPmUvZ;sn2G#wjqkm#mzW*~n
z?A0SE9`<8<2ZEWN*tZY5O`>X<Cs@;a!cw&9FY8N&VG(S0@WpR9Td--DN0(<849W%H
z-p}DrSal|gZHSAKRFQsaB7zDS5H`CM+>SbhvOtj>d7uC<!wco?iM52uAG*n0JAV0y
z(9W#L^9ywL!@J~<RP@)y;~&Z1b9DBb^U41Oo&C-s``xmI<#($Umfx*bSbn!!Vfi8c
ztMts44KObBQ{Mi6L~l>B5P-DxNn{M5UG#&1!46O)0!W+xoi5KGWU0T9e|}ub^OOAF
za?1d8KL!rKU<U_)bMwS4dlC(22Y_UN@DqR^@=I>nzocCL%;Ng(Z2vnOW959376Y)5
z01CH&V`6=p`+3sj0cgyy{vg==ReASsg(7~t*Z-Y;J*nkCN!qdlupS5CpjZH;B@Vy{
zAUl90$olUUdH!-A|BFJ{Ph1YBCyjN0^$r(+n*pHRJaHBQbPE7MiR0JY#eWqC{>)zd
z?T_JqdtlEewg7W&fN}*9K-C!_UI!2$0qhgd&;!6ifIi!=_w{3*%*e*o&D7Z1%*M+2
zPlI?*wiD0M^zW`n|D+xCOzQXvi9d_|jTZyBod7V+^;abR!(Hi*Q~8@<c7Sj%z;ogk
z!GG>z{$gcTHb%g78}l!Mf2JY+&B`nQ@jVVeE$XiYgnxHjzg;rmn+YH{{ELO3sVx4@
z!cXc?T!6Fr%fdgtyZ<<^zbQUEYneVPK0IR>Ju5IguSEQ*Nb$2u%m2LG@GIv>fZQe5
zKP@-(XjnO<ilID|>UM}<48BZLg-}veCBNP44ozJ%SXeKN^=Ka|?smgm11H;E)V(}w
z?+lxiwp8Vtf$bV)8Gj+qzYrphi5%6+>t^S5a#tfk+M(Xdg8Z>8i_TCfzd!dVrGKkA
z#Oob-oMpx9&<I^;*(M~r7J6LO(b1Gb^V1Ouo1sRHJG+Pbfuhs6C`ypEQ;EGE^F(9w
z>e;lr3&<!K*9*_?4}KZA{0_M0#8^o<r5s~Tao!(~%EkG<7s>X%MHgbuitD%NQo`#T
zf!S~wYlGNabw7zU_%OKi;Zp?Uj(pZKtIa7Q?1$b17lyN5HmaS^g{fU1Vs$z>-sPY(
zKb(J6rXmSdeU}sBF#6Txl?o;q@kfbrXciNs@)7>}2B+-qA+M>bp0Bn9bxD!zQn=Zd
z%zR$*lrj$A+Ac2};In1&az81?0w;+!q@X6XvsdclKDsK*JMbXRdA)xb>nlwh6xiaP
zl>_2w6dIN&#6pegJ`>|FiCV|@uv2e`TW4Wzsccg=7%_J)Ka25tA>ca6O?s6p*?zOr
zR>#@`hiZ!nn%z#(k2ztkfu>nimp*X|tO>6th|M}>%4)0{zT0n#gs<&vCwP3cm)^=l
z+s5o8uDh!2Mc#y&s;xs!H2e3-Y2)QE8w5N|NA`2gW(}#XYM54p+givMIgqW~7a8Bj
zbBSq+O?c21V+B)B*IKcZ%N?0T@}v6w&x?&AfWVFI2v|Y%M?JY$kg*yVy${#&(Ez{o
zoh7H%<6pOSDj8H6WYpuR&2ij>73#v83T-<Qh*c=(q)|=A^&42`XXZ=MObdcD%ZqOJ
zDqm|c8k4&U_D)P!geTC|Yz(PE7?HDsecQPJX+Bdc$D#O2tR@bl3T7v`4dmD{619uZ
zJ5;DMQD&^{!-~B)yi%knV^aHRo((jF04gDonh(lzJBoSNKrc;%clJHeq>=9Udt1J&
z^CTvUex=#bN{;-gl#LKJ)c#VErZ=*@@Ps7?9EP27eJP5kXNo2h1|c<d>lVSxySX}b
z#MAHl%ZD?+bTV<G9;F36vbe8eCX-k7chbq3#LK8Ll8{;Oz8@W?a2TK~ZUs})^J{;M
zmrb$}vQP>LMGf$tb{19VUy|q`Z_On1U8Aef?Tr$^Sjp6_wT|?ysWhZH+9Yy?0k?5r
zWp9V>*1j8md2@fr#ER1#5aW~$QV4n=&&3oy>8)CxM89r7GeA!<(>Mpejp1-OyB`96
zrUp>}Zab_S)Sarjd|g_!!5P($0@bo-PG&77dkh4F_f0OY(F4bKfmTNofoN3c^UeZ}
z?HMtiZYyu4ZUR#)N7QER6bt@~d(LgvSZeAA?yuX3jw<X{U9<B0^8?I40&K3Mg{4P~
zm<ylJ>0al^K(W9(*}o~Y@<9P%P_CMlCBhi#ss^E}>-&PlSH%N<^yW((A&7<#Bd?5C
zl#-2I70vg?W{b|&=_*+4q2ghJO?3Oi9%*>Nb6T-P(qq1CGex6G&Iz4HH}PSh6hbXz
zRMew*OARzM9p~m9##VpjgYphGu%-}A!YNq7FPWS4fn7A|`uc_nS1{!%G4U*PL~$bG
zdXmX!3W!C;`J*it-Tbnv-rrTOYYBO27hj;!MZZ9ca;bAjg8}#J9xjmB=qvgpx7gy&
zA+3y7<#Cjyd=N$1p4*Dof%`3q51f~VR=1};!N30!9C>)aJb&8-QTQ!<=z|?oDrlOG
z)FIc)LOE;m@+96_9|3N8HhW}@rhca6ZT)ihYB=!^B5}8CD%vS+i)UDKu{}TrbN0I)
z+BvJ3Yy$7Sy*}RV<aZl1THq%0P=6BB*BV`|OrAjUSuBNKpD&~@6V|s=*!aQ?hFGc>
zl>Z=b4WD|uceW`?vHFVjW{7YUH8z^5om?mN<7bV!OMd}L9iP~m{u`3bZ>hTiQ$uG!
z8kcSeBJZ%5@X=<YZK;x{rAJY&JlEgikwg?V<^;d2YseV}=D?D^+U2XURs{w=aS*rm
zO%zb3r{6qwVMCaACD&<MU}43N8X~GM+pAEHwdFbosT5maCFk;kN~mX4I~xZ|=&|Oq
zcg6|~#0j|$q5#bv8xr>sGx$V-tpF<v(ebu18asJUVmTvdK6oRw$Hd2-7ifk_lXm25
zhxPaj2>Dmwi+pg7d~~02ZLE2S#&C4WK_K}&0Vw@N+yRSli6lt%Ss^VehWWQ;9jOq#
z@bIIAGmPNDo)KjeU2hevr+vlq>yOdXY`AmY!pnbr<=43fYKi{#B?NPWl<zXsxw@rt
z4xUo|ab-4?;#nQ3mU3=%<sRR{ts#P%f~qJd4HqGSn_igz>^NOY<H5&Xq$TM^tj^s*
zN?g?dImC`UoGwbNy#BJ;E)&F9ZdXonKgf^6TO5nPmRfH|$r}*7)_t8KsmbXU=C+em
z`9FPZ^>CVgtt8wm5bNg`tA6UX00DjU2}n{@eP$;c%Hwo(`&(Vr7&8%OzGb8(ZEe}v
zHC+?#^cAtAxCpL=So*txYNhDYoBb-$1_&Q^zxQxj`DxV<E54+v(I4z(R9*dj0^0h3
zpqu1*BtHR3KvXjjf{^u78|a!5f@d2I=bIuyyL^umZ~hFN{-Tf+-*Qx;6Xk=P;`ZLi
zyAST@N^#8GN_cXgJWW~(eakv?R%VXmA$ik2@7idTA>6#QRsJ0C%`~)uRH8tQrW=#X
zPb3-o+R#40P%Nc@;BZvMSJtT{xA{D{P)x=BRiF*4?D#n$3!i_A$t}zwvv@cQC|h?F
zq3ayCK$l-B?@NwuvE|yGTKy9S9^LW7>PH07q_xDrJj`w$Jl+PsrCk;y{}R18B&s=N
zs;*eD!4UybyG>LwL?hWqjSm70L`*n()iyNlVv@GIox{y`Ub0jHs@B2bFoZx*qt3n-
zE-6R~^yJrI+nar*^WeyCit$hc@9U5Cb?(aEXwaE-T9(BcQKccY(HG}GP~)YXuf-cn
zUSL&1xoILa#JH0@po*l=9KQ6au^(qRzMH`+FuOC6$Ks>g&UR8oUoMdhc9oEObEmb{
z7BaK^4vK7~azEtV{NtFRd2ExLU2cTQp?OxzG51Jr4G%(b1xEHzri0`1EA|JSMz7e%
zjkVwt=SN!%Nr5tVpV-{SSzvk1$ItM=-YXKS&J(*jE|V5*EzEl(<4(<HjYaJ`{fk=1
zbY~qXpx4JfMf2{4PDg%~uccN*>kcEA>6yhFzxsJq_)c7o<?&+cS3klWD@2L&*Y;(l
zjV@9bP3F?hpDs;)`C?O+j?n-XILwBaoww+o{4pp}M|!=4;7UE88HB&fK9RSy;iU<x
zcPDKP<Eyf6-=zc`niQa<qWNi2FXyP2u2tY0%e87NuWC#nAT73>sS=JUUVFMvMF5e;
zbr|-x$H{0Qo9ywsN<a9kimlk#e|j|QP%?jLBv_8~fZ_yZd5cM|6Si0wGcseIFH3c;
z4FV>ZCgcdCS3%(kQbzEO@d5TaPITBVAFSS9Y)+9^_v$$BgJ+%}Hht@G=G7&kLZmE)
z=l#XmT@!XKc%ibEM`UX@(2(>!Wt~@l2T^~Oe@WEC1JH0JVbaMF@)?rx4UGanh;goW
zsA0k-W60}V$0Jr^iZY+?4~UtN6-vJ-#r$XYU4V-BQ<Lr)kMDoxzWWOv-=Cd70M1cQ
zixB;T&Mm<G?N2D?S?s@W-9D4?egZMSh-Cvzt+4~hi+=$z&y=?RE*OBa0TVR82!0wc
z`~v~~P4E-w;{wdj{dMJ^g@yhmnB|EC0Jt%K4gPb>`4`s=5TFGBw_gSSx#j$eV1U>r
zpu+GA)b~I3S@{#3dm1eHO~dH#Wk;Sm$v*?VXBhBFjP-ZNf`7*C1juCn>n{D?V}6=w
z{HLI=MOoHnjuEcqR&{F&D8fRr*7&d%et%RaC<EAe%U!{coN1#>!@272RxxwFN4p$@
zsxgi_hGg+=c20<;q)02+CzXzC@*${qW(4cu#q1(Rej5cu%tO@jT<Yj+?03zt7SeBf
zl(L!ECcMUv3Yq*1Y1rbes_HehAu)@Vi1ng5Ny3ewBSa9)4U#vp)u%i5wMAf#wBtfx
zW@elEle@#mLi+Sn*C?(qMEaJD+~KUHa41=;USiBCgW94HlqQT#p)BinRF&h~X>#m3
zTO#pXO<64@5LBHe^@h);SD8c37fZ2hf$V=gAbtV+Wlt$cJI9ll;G$8J&Wc{M{L(b|
zV=2=@%L1RPBfDsuUdB*PJ65rx-<TznmO8TUb!<=kDcUzScSIfRVBv-b^V4nM=|S&|
zR9+Xa4<iXEtg#@K4RnYL(@`9l99W!5tdZ2xRn)eJQu8hrMHxfTj@P$x9BKt-(vavf
z;~H$0oW<yVCv%2}CH(Y0qMK{xghN7KDm1A#S$%kt;`@e+h>)jpOK2!h2#7<ly1}t!
z8+w($S(ZcByxE83h#DvQs0%VlI!Kp85-|`9J818V?j<6__mXuQdPI`)Ub=+dp14zd
z0XLPReJuQ(H$0%5{Ss@;`e;{lTmJlAXgmd-n`{?YLi#(zy_%H1DsL2>%2dU~at=l7
z*CjE*7BEI{Mef0*F~2(Z*$WH><sl@4-Wi=s_ak>b0$GcXx*bQ3-iISL#^W-7vSw{x
zzd!!6jK!-49nt3>zEp}mG?<c4dlUQ-M{X1=B9F1UuTOrMT;86hYLCqUTk*|xJEjqG
zdAa#wrgR1*c%$o7<KZ$y^f<q2&0JfrlAIxR8ms5yK?g5zoQVn19ji)<N>2wz<_k&+
z1)_)Zv!ts6v@3;?1}tr>Ef%`d=~PKMuIBecoR|k1PVcck;rqGVa>z*BjQrwR0Vu=#
z>yP8tW&0n0KhK{P0Alq2f>iY{JMgDL*?%qj^yT+sAHP2<{sZUx$Kro{>;6sM=~;E@
zS>5TUiqt>*`gkTP{RcIi9dH3Ze*UxV(?>PwxH)#X)=kwbm3^7v6XKn*)UPtJwJcW7
zlrI~|cx{c{OH>l^-hJ<Ddm$iDxo$Qm`$9U5OaSTX2w10~Uc0~N7BjZ*6F}JJN)8>C
zUNq6Xsb1&Bppuy}hPp}^$q43jQP1YuNbkPiqAs0mBIU-)m)%7P8g#c9+W6RaIm(@A
zbVuKV73%89j+p9Uvtm7uXi-M00%J4d*Qt_WjMdNFUtJQJo@V3N1R_3`ztLJaQQ0!{
zXdV^)ihq<Paj;unpgC*HT71tdTkG@|DlLZtKWsr>`CQv!QxX~0o3|b|q4<Jf0^Mp)
z!FqAwV8Qv_7ys$>YZ`P7Q=<n=^YuycJ7ua&$e5H~8{ODh*XuOrRauILMaPJ|gzw(G
zSG6l;*0P2(;VT?e4-!L~hyA1w@?wh^sEZaMnNre>%s7`$m&@mUb%dLl(Ne^H4t^7D
zy+t6JwiT}jAnum8`4&s%(tvJ*1rUq0QSU2RQKF5coDs#R`Rn%?S|{I=LCj|FkuKh1
z5Tn#}_JYsOPt~OeylYuptjIT1?Caw-bMobw){`XAh9iVSeh7PcN$;tpePIB`qGtY8
zhJ{3y!Z9&+L`;A#A(r{t7D{&EhEk!pq`cR@H=1QMo!OIwGuH?1)t6BC_w2nC-0+Sd
zWG18t=0#_=S39xk9C?#c8L#<Li*cr_Hg09Lt<1Q!HmnVXrKGn(XnBL@K<Qqo8^DdN
z!|*f*oDasar_^J_g=`2%`g%;(xIFkqXENK`v4g{qEk?T)yw3YDJwBKb1?K<CQ8d-j
z3Qj(?iDtru=g@OW66oW@#WA`EPRc}cvy*q07k?qaSqbFz%Qi@@xRoy9g$V|nGfr{f
zv4og6K+OG+Es&m#bWuEEgj}%!0>WR{_J!A7e5a;K1$|uG-W{Bw=PJ)1?AijgIa);p
z5LHq7Mw=5`-QbaB=R}LTM}pPkcSX5OMZe#NaX@V&0n@t-l3BG>m7;nj<BtW3I$ml;
zz%_2<F7~p3pbVnVA#Ek%wx-C$fxe1_?Hdu;#xRdAHgUv_&dP0uDFjzF`ppa*P)vVa
zpd?t<Bt|A3nvA`^{YHpOj4`JzlMK)~l!A7zpC4#*=>_iyvW9EqDN*T1Jp%r`_kHNa
zwqzHgg=fAXN@Q&Um6Oq?j;cdN>DbE%X=E`au(vnM>tGd|b{a0SENJ53f<6;1<eB^a
zixm#@5?1Bk-DR9U2Yphwp3%~STjD{=LRJ}*m6D0_(9sZ3s<|X}Yf3~DE<uch$zXa7
zWvG30Iu-;WX7UB>two-mH&=~$0Zh9{uXEM1`_xfm38<0Dg2N^wMhZ6WDM)k1Cv$^R
zaT9<D_lfe-=YT>60q9kSJvAyHMnSE;w|t&~!3@FhOBpdTNAFFQ`+_oogu?elpf|>Z
zz~DvRXCS8gt}yGrWQG=UfK*Xh-jDg#YFCI%?hKkI$5thWUkJ8!#-Kvt(}nzDLbdlq
zR&y0p&kAxG4{`L(5?BfZk};GxjDLjpyQJPiqw`C{FpyqL{d8K+))a(0LB!AmfMyUK
zuNpczPbqLRhbY%sQ#IUj1eE`I`M0-xJlRIRV6xJ4akat}w1ip<0-*!(l6=bu>*fpi
z?%Z~Lu%+<Dp`GgX>jPtq2@2;O`-A6jK`by_#NfgsD{0B1RPPy~?P_uQKZ@<mygzK1
zJ`C1FB#(Z@SL#0fsiI?)Gt=6Pz)(G4C~bPe+<M20lB181W@}VYCG@L?r{#gvoB9Uf
zMjY?&BAtVkYty9q9Tu#}y)osl-3k@eX|owNJJ?$0L2L()*HY9znljc;R3d!{f+<eu
zx4eib=q_OqsQXavcHwQuXSJ@XJ;cOqV$A&@$3=R=RAD$55^-%tjb7FjU5lEG)q54G
z6pyWh%qcJcXTjIZA9#~0r|W;_Qc*oiQ9-bheSdc1u?`<0p<9+_aWbY(PECko4F848
zIa)05j$6$JQPVo^q8e!tk<>&d@`DR#l#PM&Sycl4P$1qL!CUjsbP2sdB55d<1X^%Y
zdU7)R65=l-wL9<53sBr&!z0{$*@f&`-H-b;)*X5@B%WvLw3a<5SQwMn7O+gq>}+Sr
zy$z)jhK`g<zjr)sL417z0&TaesZ$(Wkyo+r>N{uvnc(lWk*dWpLGr%W-oap+4qho3
zV~F@t1aAB6j33z1K-u4l*ZE|eSpv*<WwA1x95oz_ef3@ZhyLakuVX~*u()4=#UG|z
ze>_cpodEpi*Yyl6{$}a*kNfrjFX12g!jC?-1Cb`6ulk!N%riFif76fyplb<Gx*!5b
z@c%H)2B>NRgi8UGW`OAz$G^8oVEBvb#WUZAr)1@KBeQ=3Z2z3+JPZA|xb`nZ=wF1g
zF##lP0QN$E!G}LA)&97vf7$ZytYR*JcMxD^|F6M+?!f+H<)=nD6F}zVufabjlz+D}
zK#qdx7s&Jnb@)H*ni*jJ!U52e`Rl?z!@R#)_(}W(p#SrW;3sLKKUV%9=akPO?vpd_
z?~+FUEW>;zp!$1;32;^a$Ht;18tM-7y~tjhWi!oWdSNfqI);~`)69Y@_w4Z`v)8gM
zRbjx&$qeYqJBdRd`8?rD$@7Q2W*ef?%9`4&i|6O9*EX8sY|7+Y-EuP{Vgb_Gvs#Dj
z+zLViW~gEnN1)ygAFaHO84hBFlQ{A;@dMH@u5eu!*R@q&*Uh_5lILl-$tIcRwP`=v
z+Pit4T!?f~*k`fFpeyy2XCvF;S$(}9c+oZ>3*pIcnLRpsmL_(<Ax2H?3A%3Y;B;~#
z!hk?Ca+lpz*c%3=d6cet@AWD5CT4oRtD*XM2misMtZAs>sOWXB`O5>Vrn@7sFSjFA
zjPJ|nIW&y}4k4TOyyZyuCwXNmW0EM)F$`W<on#L+*qPB5)=C$$Zi;60WDZ6Jh<q4L
zz796>raG-zh(NZ-kx=!aKZ2nxvWR}q-nEqUBKanF__AoIm*i|!-$;YPYAD!C*U@F1
zV4}Kcs8UR@b(6C9?KIIG&0A&y5)3;Zlv%6V)^kxT9Lk!bEjrEJx#iY2DcXh%cLx=1
zTV$NcS2~zu`68Ajhu;M?Gvb%g+seX9B?dEP8#7C>f^5kkI~w<bH==VO7QVoES(&+C
zuf*hCB$PLIuzg=djGx{xkAS`PC5yLx6W6x?j>4v4YI0r|F3;W}u5wLC(f8|X$$_p)
zFvkGQ!(_hw8#`hi+DO;4z!INR?<rJq5}+LXXou4_p8fd@8YP)%Q=lynEV{I?#f9J|
zIiUa?XvLmam)X1uz;0r_s9M*x6mWu}=b$hg<qlg7A0)%B1VcrsLU~qAAbDp<?I3}g
z6Ryiqq)?=(c6tO8ue#si7wnamDK$fNI8~E&9U^;SgH|{(zOi>r-H}zg;95o1bp>G{
z0u}WUEeBfnZffH<^Y#)m4U15q@bSOy)Yv~e7_YMV(u?c)t^2g>quO_m{o#j=-AkUw
z8`<eRzs~z}M_g>`mYvNa)gr^GDX@LN6X58!FVh>td*e!rwOz2c8em@CS&NJ4)P6X!
zA}25X(9k~?g6UG>Ng(cE!1cYH4!E*7^{nU_VK#f6Z8b?fgM7m5d&v7`u9oQB5BfIy
zO&Fp_q1tEpbxqoEeEfa~4l(-F7z_%WkG_XNGd)PuBwWcKRe8FnQxv*0vXL`FI$g5_
z9SjIwx7cN^e4{Ggq$%&>Zj;(BqeF+njd%+`L>CCZ3+4*}%f9atlo>+N@SR;!Wlg!S
z#zpVGUxM|G5{N$vjQPY@>-kb>S_r8%Zz&c1Gq=nzzRSq5y~qw*@qzbNv3}Ojr8jnX
zRM=td;hdbhJ;0V%=6D|43k*|wK(;53U}$PssB~nkVZ9AtN)16N?+JX~-+<EMI|<we
zDVLkx5Si+k<D;s>5RbOo_Esfuya9;<$C!`KdozTE{6TT{eq`sJ8Z$ZJ+nACVTb!aq
zxx06)XA=3oNBZv?EMk_zZPXee;4hRKXIK%ErKjP75(H)+BE*N+Bb+D?kC5GNAtJLU
zG5c>ll$Z22v%QFpa+0rvu0M^U=*rY<siG_)`ra@bx*bEcM(Kd4&#VyREjxOAw#ev^
zd69lpD_;pAVIMXV*|9)W`ldYcZTg$`k``$!MGXAf#nboS-q{HYMa4Pf%pfwnd#}m3
z6}4-fr)EoJUQM<S%QG!oT4{%bwVm5P$F{@XZAso?_|?SBz$&OyD22MX%080IF%xf<
z308EzW}Z>iF%tgG0hVDm`?sdAFV}nw>?A0c9Wrx~7cfHGB1!s=D(n_fOF~w#p@XD$
z*(tEau;@=-1Q)~K&)3b1Dfx)LMC-iopszw9xZ+!JVWsJ}AqZO1@JB`*?v)TZ5jV6Y
zg5bs?wGC^IT0w@l2a;LU5Zn@&Ez!rucIN!xK|7$D<JQCw83aFk!5g5obl%?Y76^1~
z14`Zk8`NZD*toyKqyb?cTEz*|r@NnN54BL_kwRPtsuUPd*aITQg;l(aROJB@OkhSJ
zWT>{gR2||CgL#9Ch*_$kRwB9(+k5NPhwlTYM3_2!$%E_nhJt)zePyMJEm}E52Ah72
zVU<)y+C@LKQHFgA-XADJ!^P5cx%O?dtL{n*6KCfs70d97BwlMrG3X{_dc3XtB<e=R
zyU7nLn${i7kf9vDH7o@3P(iH&U(2w~BG7JpbtCqM?JGZgQWvSgxeZfz@r{_4VF@ZR
zY~*Wy`a|iuS^I8)Av-wy{;Y}@k)NyMm~ejRS7zcPrj0u_(3EJy6QH^A8wI43j06!-
zE4(EkeKBQoAkd9kNHZt;4v4Ja?^ZE;0!KzzZdx0O9|=$(7RV3hvGt5G0uV{glaS`v
zxFW-#OTiBa-q?IP++}))5d~yE+dQAa?T45t1AW1DBYBFEr&FVh)2!!+_8#w=pPHg{
zzY+s6v7?`i`(%btGiWjjQEL+a(-bc}>)5!Fkv&OSiMZSR%M?^G5-_P+q*RvUB{J8J
zvB@<p$-~$kc9(-et|4m@h0hp@FStv2_O<Ck;C)~>ORn|_2FD=t_*c&MmvuK)lT?c|
zfbFq7{jm-}tD?|XlD`EKEH~HM_@M4lPLiTIle~WAUc3$ub*4r&3RLyY3_>$}e^0!E
z)p+p8F8tiy4i*S5fxu2&Tn=L2uAoEzD&$kv{4u)<N%rvY{JV+ure^IXB#Aj0t#~&z
zqg+cY6_a?_B9_4(cu(nh$akwCbU5{H1LsZdx-MIKLPo}{B>p<4Y8iqo*r{cTs>X^9
z%pt%`-ObTonw#|_gOxGXVIFob2u^47N0&UV?-O#0*XfU4^BJLyB6RAKkmK{B_W|Fq
zQAC|v$Q+Rf8|J<!WNPgy3${L<CmA!BZ%bd_KbVBkrTc;bwPlOlZM9Vy9qYLD)v4MM
zRUaoI+1M)-SHD0tE%E#`K==^pICUAZIimugq@xChLz;LB&y5*LVWP%<h<Rn~A-XbE
zPoP9NTwq|c;1b;GFh2hR%Ll=A*OqL4KXMV@^d)jYxW7pRRWyI-Ky3hQh8}54W8^C2
zWuKf6oCwG|ZxohUuj}#Ni~c|)5;2kr^(s*n81Iv%D%ROS7x*mkD(&iKN(--jxqdV}
z+_w{9_Fgo(xcM^z>M)m|`O1UaXLDF7>~=<F)XzzAktB#>v!L+>gI~qk6YQG2aR~)M
zPC5RH`9h~@B*fStluA8+AqmVSG4wi6xEVO`(#47EGtDRk3P1TfPS0H^Y;img*~`wN
zV4)y!x8iS2e!_Ny49u5`#r9sg&Iln)+d4S=8<ToY^X)}EPI*3I=)h$x3u0MwX3<CX
z<U`BAgU(^8AWUE|GI+(J{Gd(Q^+Q<)y^wO4UJ$q`#wMI@izfuoEXTFdjo;aF*tV56
z<OB;FR++_-<sel`@Lk9IXVz8jKI*M&`KAT(h1Fr%cVT?urd2BM+#ijLyTdCc&aW#X
zG_OwM*#Uxgg*EYK$h5je^DB0qPLgj(g4bB6?UCKSf)1>;ZeA51vLQ#bC(=_S_UnE@
z&RpKr^V*e_nmbqjxSPLJM^{TrN1ONU{c0DKa-g0VP!A1AFUXf`agx{Q7RVriul3=^
z4x$N~n!WCK@{}*FmrPa-dEI3ClUIAa1WHhk<9(<KtM|0SK*i-3zg9b|!?QdBl@JRF
zo27cbKF}m5o`>@B5=t+NxS<*7T2~b*?En)ydLul_a@!|TS_9P%yQHD=lJxA$sdw2P
zj7sm)8o#0H8<D-;^-_K=6#MWp2{rUbt~s%@Q1^!YqM_4wn@NvXWYFH7*LPf(UEJWi
zM=4XDlGN<GAh2dBAh7vJvLp53+tBu`zW86QeO+MjMr-J2P}wJ5Zn78+FQ3e}^Q6B%
zBMfcE_2^IKrDJNka6tsOKnL&W11lj)W9%l_%&f(QwX=}WH8F%*D9LS;%U?b%#+i4k
z20p>|krl$n0DgEB5Z2qmgU{9X&_hIO?oCDQzzM_lW_du9{j?Nglb&A@O@f=jIsm-M
zcBjmrwk{6&D8oW}F#uVcj?b}=qXkTvabzerQJ!g20~d^X9DNZ<h&L3^9l~2@$Zsk8
zh!{eRD9nOTO*3k0kUDzIV8>Npk=}Mi3=XQoWh_i#W93mse*sAXDfWf=6pVJu^Ez{#
zL8{20-^P;j7841MX&7UQzx2hUQuYcKD`Gt-PbNFUn?XF#t;bkCrtDwz0)AmL@#g{S
zA5i6aFW@(4lK(>~;CI(O7C>eGsa*^ZTm13wv(mti|MLev`#)0k0o3y+R|pmWbNPuu
z$_j8>WqfKX0@@C&OusfM{1=z{&t9Fs?fSp7GiHE5?b8s!lM^^Ifa(jtfB={bFzEmk
zF_>8Xy}K`f-~VSK^^bG>FYOE<V*2EQ$;8M7I5U7D2S9X{>FG6~GY-%``sE<OAM*`=
z86$Wm>H7rKfA_TcCj{?jy1Rdg{f+I+4md_G04d-v6@q6LFMk8_pF{b7(?<Fm#|%JI
z8*t-(=<ou%Sz^yzHl99Yzjv+y9qyl<Ii5AR|Hrgu)@L3?e@szx0*Lhgutim(A?Y&D
zj?%iTdS)KPtJ0+g{ngE3l+$^yna-Z<B_!pGsGVFglg0rA%%S8YLVjf`+c(0MDA#&y
zp*afqH?2Y$X07!aM?D<kk6irjH%er%9qNOxDmdG;vZ>>Q=q*9oDzs8<Fb+w3$TUk|
zTV@?r8Bk}&>U^H{SfumjeZ1J}xdw=mfry+0Vv(CA<_;LNymWu0)wN7NoC7z7Nw%Kv
zUl4-rsGm4$)=tH5;abt5=Tk<UNY1AW(C_VqErORWhd#dzgxx*cqU7`Z&|Rf|fZRBL
z^hqbxyRc?7nL10*cGz}`*Mc@znTKjWo<~hi(`4B$cWswYCpfXjub|uOgK>#T=<8Es
z-L&S1gP!U7qwyqXb}jN2L9gW?Rp}2RY@8|uR9%Gl`}y>Fy|Uj;de|2gbakf3v3D&R
z{OV*8GP|qRrQYD98;@apt2L{v&6i4c-WFkeZCZgMRG~(v0&`IiQTLU+fO}DWP>Hu5
zaXsjQ6uxRTEzJH^aTU9b08?CNB8UOJX}THz>fZk8=8Mmtbz1KaUU5j4ykGG_@FA?<
zQM8U-vXIV%3)vyQzQ`l=(*aTIEu21n?>xRnc)~r`DLtGuRH~H~crSO#+%&Us=JAf9
zkQ--OWkOBchg8~~VNlwBH8NGiOA>4e(u*zbEA4x2PDoX4X#|@xUb<w9xrm1n2E<0+
z<VI_a9Z!cw8OQFe^=87#BAgf1GyCS~c}gh>UOLK|xX>REV!j~Ecp;G*m{umdrmH#j
zBnGiXUqwJ975P}Ft=&3Qi@#--9a?&FQO)4|?PAyllUi8Y2_f<dj1dM5<_$(g&62Sb
zzg~d<J|1J11&kTUV=5K6trJe|W9H=a(q$q6|3m$yyhNwD3y{jv8kV0Z2qHD=Jmk#<
zw{YAW+=Yq2)yS_EE)=XPw-{CeC2u#w9v0+$fm#Sp*p_59Fo~A%_iLz~-M+O)n`Xx_
zvS@Tuk2^2fTr*^|3n=Bdqtt=X>%@l~e5eFVn7yA5ny~>U@TW3uR#1p}rG<wo_`35$
zg2CtLlwAW)S*n)LI3T63U~ocs2oAqFOOK&+jpyctE~{_rGVJm!UbXl}!_IcZt8(uo
zH$Uv?U=;IYw=u%E9uUZZKo|I+yCX|~&Xyt+1z}J($VTls+-Tp|UG?da(D%oanC$nh
z=HTH8$CEIw-rh3DfLI>MB8(CTNmEy|1)-F%>rk4(tMAFK&V?&TVn!(VX2HC7O2!QR
z?iJVasMIW{yC0-a_~2%SakY`y0H{<40Z&eEi*$B;OOg^TIT<6#2bR?f-+6l|LbL3*
zI0bE@q`Y}C$)gfcedC~J*=W+Eh*24eA$UTR;j}G`Oz#Ch^hFC}0S8CeXBMAu*Mb^*
zH(%j%^2DAKPedp8NM1PtRVnJ3A%t@&f$B1NL871$dNl9Eq3zuH5C^&efqpcl)ubp%
zox3fb+owvq<&<#iHQJ{LZgK77uzlf%F^m3b^+fq1gQ^Tkm|Md?4lmpYWRymDsrNRV
z;By{Fo)fs+6oW)TK#tHE=>Q6GX!O-Aam^6FNfR%bHL=_L0z3s5(Mi>LvMD()(M!5Y
zP~D)ll#2jyWTIv6GV@>#(YLbuNcHZtNaG98lw3E9DMJY9d9UT9LJLxF&;7EaP-Yy}
ziCH?-Imj@Rr_is;=6P_#89PeX={7dnVc^aQk#BddcCOZ3-`dS<w=bez0onxKe7D=K
zEy4LsL*iQA>w@4e8X=Vsa&Rzb)lhJTwp^#_IKu2&Cis~veBM4mNz&vbb?80!D&>zO
z<_?h6WJV(ct`WXuVAg{PLwRJ`<#U=AgZUZ2p>wIp4_lS8@X%MuI+qN?Dym3kqSaZ;
zm<-1Rd5c=A_j?qNSwllk2bd?cS!vayY$8$?xT}<MpnaPu{4D(82d_azn<HTM66Q5U
zvym9L`~7Qk&6*x8AbFFzJdtjg7q9NO9W2r<$Cx(cQ6M6WA1ZR}peQZv^m>DVhpox;
zXn0-V8$c-F?QfQt+c4&lQ^sCFw3i(8+m#m8npe5|wMu&VX57^Ye78H-{IICt9D)vW
z5DUYp7NNTiOj3)P1^19->JpU<pXxrp;vyf+x<WXloXJ&>i;ate8VidSU@Cvuo(la9
zv8wSqux%_9;m(5btIlr%(Q%I0PHj+jcUBj?Ya-PQj57_RT$1g_ZlBn|1UPEW9efLM
z>B1WZax{VB1Sl=ipa@=r5NYB>#TT`+DQPCQKM;8uYVelwWD`mI+zk)J4u0BRs$JrR
zl}YMXpVY6^Cj5RJ^->{yz*zfgbblLR(FG}{sPr&+!gc)%R{ha{$S`PRoeC-|rYSc;
z{HzX`Jq<XMni`{JCmBanQ7&~CSvtJ2o%I15rWr<5AX&AjfTu)`(wkdBZHarZM38<j
z<nvb@ta#qu!k*#RotfUg2nTyUb*+=W0-<Y2U^5~G9B|2gK#TZ!yKn}Ar}7rtLz4I5
z6*@@HEdV2$iLKi>`I0<TEKtbDr1CU5^wnOTes@=KgV01s@G3^A_KC1}$Y7MjNSe7m
z9!SDKj02>!V|Y+8-0D-gpM(W*=FDYhJ`T=`H5He<V@vSA>6FqnHcz(R{A}WyffN35
z1N-}>dgx|>-cW+v%2v$_X7mvS?wipetWxuO>$C(fs2nzDrFS%*SiWW^MDNl>W2_V#
z-wNUwcj1E-<(7B@(bZm0(zl#rgGXL~YDqGS=+l0v6s`<|)EJFrGD;Cvb3w}}zgX1I
z>BUFzYT(v2opY+ov4l}#nJ0<r1-TVw?Q*QswYRtS3RCG1eC4Fa*vPlsm>5^Ge0jaC
z$rDUGM&q1sC1(>7-ierThe6o5KlOH|UGdm(ThyuiBStl}y<qo2;icP~Tn$f(dJxwU
z;o34bRC_$Ngx9d*+Rz%-FJ6A~n(`OS4K?0+lO(a+3C6TUP#LDiGO`b>J<_>UA>cwR
zVGC9-LWZHmN(&F&jyg}Xs@9a1+5_bV^>W_fW-~`7)UGsN5HXFeiWb|ewJFp3PJ@(H
z2d%(jPM07kl;(!Pe<HVerzbqgb-##tbf*Webo-6rX!UNbRmCEP+rpVBZ3kr&FxrwS
z5itE%n@#lWaGo29temu23E!;oz#zt*3eEFLa^PO9oK6AYf_4k$6G=0tGhH6(4g_*@
zlRo4F4J0H`3v!F>yS%VIM^JF8TpfG@9T}|lDb7LVH+Oyp%t!}}<CXZEE>_k8x1rO6
zpQ}N1WMmu(Vhz+XHxirSEfrYO*ZlAN2u&$KA0zq#v4@%Fa%5)iWHf}bQ)WdzrxFoW
z?Tfd;47~e*zY3;CPbDVbb=fycVPcRLU^GnUeU7AjrELJQx6ry7UU9M@b%!GoV6W>)
zKy<7l0W{yi0xZ-2MQ_FP`g6f$wg;aDCrb*r9Kv@V+grMJ!I_>^3j(w|OyTdr7$L6X
zWl<g)&Mqx{%^VXc-T_AK%KA$=G%dx=mfDKQL581`l}3XR;NF{mTIAcmkhWvgOYH1?
zyD{Bhmoc+G5;xVp!#P}5dPUXTws%r-<efi6)}+E@jT#pu=?5Q?<^0|e8}(ymNFnu&
zM}K_}>`i5uP<dPFX%LznGslGZxeJSA?n;ByM8sLD$&Kg!0;jQ3o{WRc_|P4VLnlZR
z{`m@Z3H?<)d-X^e=3Qx^{+f}#MLI1E-{{2Sx|1k;3++<HmzUs&Z@y<y(YdVbCi|$Y
zmUuIM3p#n37NiWb=j8ZGIM{e&ehG0rGux`_C5-0jV<RaMCKD2}0Ld%Qgjeso=GPxh
zDZg?36)>~@aQS?Wu>J&?e@o4u0cO_UY*79Wfcb}O#h+%`|BFr*Pm`vs0FoUWVDOXq
z38ny?EuJQz0Z@<m*E8e)g2#UbdjL)UpBk$G!1mjj{UcI1Kz{7s#s1dBW(9Z${Hmk-
z+bREXZ~g}C{lo=)@m%Hp8SwjeQ0-?M#-D*30N?>U6ahvlKWZ5OAoxQ@?e`Y|7!7#q
zsbBqs5t*2n0Iz;9B%Tl>KxYl`_U~`~B(L?4ddaK+8`OU_vj0C@5dfS2Ar!1p(*#KB
zqj_DHA(dk`lw*G<Q#4m?A4<&?O`)jphnILKP+=^dLdVreMjv*6iLXPZS+O_q{?aIx
z<co_d!y*@J?FL5ecPB4gmq@wLawsvri%_;S^<nZ5J$<)M@rj%%SBr%i%FB*Qs4DAE
z4QV6McTW2*`ide0S}!8Q(uYDmqAyq9I^AWk;JFZRg9((2(k&AWdms(O9Kyb-+?x03
znL=yzEN3(*Kg)Jzpp%2@jPK&C75g}ZprbnBFpXXkU=Io1KyvP}qokjwz4J{yX~vs&
zKVQ$80K;BjpDq>bI6K^*L@G|J4E@sg`i4wCNIl=QBb&;f^P$vK`Wlqbw4l2m!&bh9
z_JfLn5d0KRQXrynymFH~L+QwY8M2gc$No(Ksg$-SC@DesVExCu<E&anLycBEvgJA<
z@3!VG80*V3GFXQKQY}3tktpINe};U(h?TOgp-1>bB2flkoy4io0XH`~{H?t_!g4Y9
zY55%w7QDE-;W%<Cag_UHulfg>c)PcA0Vfu_o*HYS(dZ>Yp4}haO))TxDelP+39(ku
z1QnBWBnz8a;B|K!oK=!H$FwbrFR;3e-|uGmR@%81lhnXeWW5s58Bdqz3DcUcxk2lk
zVyV6L3%%S^I}LJIqvmVm<HC*<%bO<TsPc2gQjAh7Me09o);8}zGPVAQMuVM};l2dc
zuR%QotPXQ$!P(l%ZBxat8q!d#bfTsSl_{yd$;YVDGEPiL;ZBpqlzO4Vh~wO0w!)7M
ze`Z~Q(G>z!o(RjaAWQN>^BvO~iu{26Qo7JR>s1x_BymXBvVS_a9?D>H{-$?$>8udw
zyu@8sJB*)4pLwSaJS3%Q*P<p1Vpaleq%jm#rs4#<<dAD&Qf&1oC@Oi7nR56RqP(IV
zA-TI{f6SaidalT!>|h{Q%C^n+Fdi!%0#_lGmJ$|NViZqrvSop*cCSg?v~IW}y49m@
zh@W0xUNE#`bdN`_;upRs@zoAx+-e{*DMV0uA_crQXisj}#|mB@EQLm0+znXgBX?-n
zYHtkp5!+){IzOsJJCeEp^;+(V-gTldfjhecEDNP&WhunnZ!*w=8q|Jn;{(IW)DuT1
z`C4?W*WS}J%xgG3=tAr%IOQNyP!lKygmv1@h4VpHUqD_JHN26pWTcw77jT;-?w_Oz
z4WkOlYJT{tLK0dW6Yv_MrMX{;%HSQn=vQQ?b=kuQzpslFvB06r?z&g>YILhdJ@ZMP
z;Fl5R0-ED4?V|B7^^8%-+%q<S8Z_*D?tpScj9)l`i-e9>&(K8}u3zWf)vL^jx6+Q|
zR%0kgroD+M%oT*ts5?~XOS8!Z&1p~Rw&&S0CXRJ60o6Ak#Z{34nk4D~4*{XYo|5d$
z)A5-0+kqm(vqMtb>hx@Xi=bKCaf7G7M*uc;iL~6jUh6NbMbQM+f#^HrBZ8M+HnLm0
z?2hNgzzbDMnW9zcn<AimAe)%8^Hn)olG)wrJ;ZluTL%V0aRJteJc;&t#N)xxf-*R!
zB?P|;-qFJ>yXib<_3WJ(GIMee4QPXVyLw*IieY>ad|^G%4`zI}+4H&q?keQf$-~2^
z;<7uC0jX#-la5x(uu=LK3w!TWKnMo)k_GWUZfz@#8FPQ}6D?I8oJU)SPxkdmFRHb_
zjPpHdY>EokTfn7ijgGYQsl@R>CWo)ws5CnrwB*w{r47zLw;*{tF2q1^;@Uzi<)gGx
z&mU2Vp^uQ?X<@u*jZ9GpFW!sq#ztgTsa31)i{#qbXfXMv<dQIg&j@@nPiBhco1SnX
zjNu2}Oe2(}SExlc;p=1f_Sd!UKuVAVKGsf)LWf#lij^cv*sJGnV}ua(EFKYSBn0gw
z#(I->$8Jwu>Nfr5yT*E!j9lNkDK_14ZH&>vGyCPuD<S3yB5%deb)C3i7&j>HpMY$4
z6lJl0OmFY4GuizH?<v~0b6j?_8T>#Db$JF=7m3GvwRtX$<d1to0v7J2GJj{1!f5KP
zf4qQUUY|IX$N!>qp5k+Xjnt(gg3L5csr<#5c}hHn@<n7LPil6h_#DgLMx6qUwN50=
z=B8DR@CuFPCCfZQ3~lhmF`ouP{4l-F0_|64NfJ$-T}xM$N7w=~2HF?Nx$3v-Y`rh<
zXL<BO-u8r7i>K89Yl6zMx8267kMva@n$7qvsnDv7lzS)^SM%(SyanBwOt*@~PuWXz
z|FS-XsYc5<!tXQbjd*13SLb8S9WuuWG&0i$6)DU5iyVyU=P{~Z@)v)5CO*%>0FT@s
zghW|mBXfNr8&@LDr=|lN7aKrMg&pAe17Hn4sh23|+Z$Uu0$KyV@fZI!z`*)v;~&-^
z`}ju#49|37o>HP`p}*Cz*;oO5ML_=Z=NdMkLGjb`{cqBmXUWUo4L>~1SpA&5{3OBg
z4>OIQ`F4L4otXe;)BiBj=ux+_S!G9gNYmA)(-5FG2tuO{m3kYTJ7@Hw<s#P7-i6B9
zUnpi<O<Ucbp6i>(miHkXS1h@x8#P0{SA_&XJO7r4^mJe(K>qu_hf711H{F?tEyk&u
z48vv|r%7vcoJ1GP8D&z{0Hi!~k8>V#cr)y)>2IY+VYnQRmxH|C-y=NB=8G5h&Pr|l
zv%U=2C!{PC4G}s=GPronA=}b_3fZR*>#0b~9{Px=>cQ;q!Y{)aSMF%)!aykn)$0kq
z>S15u(ApJxzC0e5*(Eu=GK#2(vg|;Soh(A5hZ88Qc+ocd5yERpJN~-Fu4^#cn8{)F
zlw|`#cb{}zz>UTO11vjar493N;kK<IcuDv2LbC1)XT!J5;v#PKX+pR51>Y217>Bwf
z-MPMvWRqZ4C;2*ux0tCI`)pqAV2HP!%`7#zWD$5pPAa1s${Q)%w6Gksn=dev1JqN<
z9s@%>`Lrhq?}{h`Tba(kacfdsgD*5E5JW}wPvjVIeX@yazJ_9_3+j`+C!AlEu9}9M
zN5x8oAzDtDZhN2{cx&3qnwWX-R%NP=#ABi%<|$U3e`oQ9uV+$6y!n+hx|;}75Qwa%
zy|!kFY>Rpv-WuyG+tC@!`#0Wws<(ZLGw@Q?Z~Fr%ICy}$rC`eD&ui+^KGdX4k@Gbq
zWfv6CXlP<_ytfHMi-=iMkLWuH?VkVG;dx=O&@{7|Dk=p6da<&ok{66Aj%i3o(?e2{
z5~0jhS2w>bU%4wz)YfE{kp0OvZGN=VD$!CMUtcifeW2o~0w2F-(sYA*<S`Hzr5>M|
z2MyZkeg8*^)9k`_!R&ZBd?gwieUu^&t-JofLGi;#VTx-7_gb}*UTZ{QjKRb&#rEA*
zBqJOwEQV{i8D9(vLWNHvhQ#Av2;|Ur7{{U;PWu@;I<uJs4S|N7a91!Wk+KMGEf}A7
zwtT>jZ|t`u32t}=XZeBLwwEE}&5WuU!<+uE^z+H*yko(PpuP6_P}gXE%E@cJEid_t
z7)ziI@ISw)YONA;LC|a{PoWv{%eN^unt+oLT_Hz6vh7u#*b7+VpeBPWs9_0?Y(}e8
zw=%IlwvNep_waU9Sddm!#T1ENxvGhQ_zZK(T#vK>Ws11tJ2&qNMUl8`)Q+~Q<L&jg
z(nNv=0i-H{t=5@7Y%mkAs3z_-?#wWON-8sQ%bg)J0lz5_wiV2+_=I<U5vT}dv<p@C
zPRP1S#r?G^OZAkYODKc&TQ5())Mw0d($iI?fN!_EDJ*Bo8LA+CKR-aj<Tw{_GjGt!
ze7Q_e??y*!1~tNH-97hu3Z&MW0wCy~n4u&fczganEC7XWqQsP3n7x0jWM9=9+OIQ=
z!E?lQlw-vZn$ARtAGZg2eA%BW2<R)%iJT7;IB}o}XiDri+ch+^uV9JuF!eEWmg8w1
z-Fej~)+Z<2865CVnI?ju2-e!JCeQ(@>$S5dNcnvtDTN`|7U@BG@4ygy#zlpK!omWa
zML{s}OuPM;EPT|i*(LLg(}X89+k*Vn$B@bq9gAZVYa^LAa5D4+=aot@r?{f^MB@d<
zOeWZbp<vKkvyFsmEqLoCLwP_%=x&JvJ3{R`CA@(4!XyKa<_$ohf$BtekU0Dtf+ubn
zc}3nc;Jhb@>^x&yLuSO@w#xC_jW>;@VPS9S$Yx3xMXzUc>F<^;OpFt7Pf5^ht^?!;
zntRSe@UhPGdIWF=Wh9M;=g3N0RI_L<h-<^Ev3ko9FL!DuO4!EQqyg_<$0mL#uAFbm
zhs?$Z^K}~ZJ24VcXUmAU@Ij|yN9vZMhMhgyFLoR(A4rV79}GNBKC8teTjIK^el>Ps
zX2mF2N+dW-;-UApI2St^YCsQjMvfU7wGyqkZc`Ai<AYi)j5{oL6x^(F&LreWaU)`H
zv46czRme?gyb$?D3oR+E#rB|0Z@*b??G^=X`uzH}V`52P|GDhlrRl&v>SIEcHQYRy
z0T6*|Y51sI$5-?c$N3S43#XSrx!H4Rym$P@QCaFLh>Vs=w~+P_bk?M+d24YWOjq@N
zM=H#Q#(~kDp+{Lpd12FH<1j!O8n@i^5C&||ET$5^XsGa(?>)S#6n5w(X%l@V32Gm!
z&SFa<^h!>si5ra5wi?GkD9mx<m1dtTLO%7ryPP@t!L3C0q1&RDpcK-*HI%{v24%QV
zH^ae8_uHs(CT>U4{YGq?=Ejce4SEvOkUNgg_ia~+qkBLPEeLZqUp|WsoSpQ*))N?0
zY~kFy>9q25dtCt!1xrMVfU!{-z|eG}>XxTd43*z|O?u?*d;>v^s;u_NH!b~2(CY2t
z$kp2#FxwUp_!x)MPSnRw(AtDWIw?>5LVpYQBx95J)a~=<FB(3R95H^V^O$7@B%FvY
zVxFk?t|w*Qtx}fvh!7Trh59Dxp!+P)8P_!2KFKJ+8+cnwO_#xk@{wZrq(E>be!e6S
z3CJbco^44i$3wH16VVvP`>~Arq{A&ABhg2v;21}TBdU^H8;NQu*Od6a$-jeYo3)e%
za+Eh*z&>=ps)b>^y)O%H&K*Va*HLbp8sb;pl`m(dlNF5pZun-coO!rb%?%b9fx!0w
z|Kr1&dx$!_xCCV<14!_<Rb`&esP`SZQDYb4K+|181SQ|fHi_olUxhD_)<lHXvOKcF
zpxlgw>*@4<*B68bHo)^f>KOB2-_Gr2m{l*Hqq#`P-~yAln%=(ProH%Np0eU|t(a0!
zDB3UpjW<>YDI|rKgI`vu8#z^{Zb885eMp#BkeCh*l)~+oOP+2*zVp#H`vP)1FDB_%
zIpEJT2Y-_T{;0P6Jj22YDC_(mbHG0hcd!D6%74^t{<Cw{Gu`taIpEKsf0VWu88}&g
zl>`1sF7EldzXh{!vH|RR|0)*;a0dM;f&8150lr#*v68>)#j*adg#4qZ_BX*#hO^87
zO2=PQ*Pq8q{w8&OmaRU^WB)c=edf#c&r;VPCY66&#_y>sfSB?7&wn22Wl@k&WmK@;
zWERN!C{%y807s7o2Y*HfPX~`K^c5gYEpsL#3y)4Ga}F0yk1nKymdOT3|HFN&QA@ZU
zC?Xt5?iU~8AF2p{Z0gq^;@`|QpM8jbIDrDN&Hoz60Qg4#Di}W9(m&K0{<HZh>oXhD
zU*6Gwc0YbtZ2WQQe-tgb0OMf)5K4@wOUI?MBefouE%m?lE5=0Wrld6P8|e8yE6EUw
zxYvH5pdZA<c-vnlU#G$7^X*$nx8yoA)R;w0t94E5)X@_AQb!c#Jb#<6ZnHaOtbn~q
zc)0($Y;>*+3_4aEbC#bI!5zj!`3QpcSTTvQWxndCDgJSy5laJbgcU(FSQA$n(9)fq
znS}E+vAaeklHnuB*CFLv9doU3wpV3alz1E)WnG<o@}S<yeo&rpxJJ$szL#Q5koelB
zlf&$B)=7^Y|6ag0@x+l)g2k8({>tO(Xk%v|U2--wdrF231A?;yWuy{fI8Yp~R@-6T
zu`f3yELd1MNxPx#93-W*nKrX_O9BPX>E23v0Ucgbv;uwEeQtLSD4_hHQ~(SICl*s%
zMMXzXv&;zP`*>1m?q}@<XY2czI%bq)N%MiJ6&Hr#T)GSGbEp$LnYdS{cH~xSgt+ca
zcQZ<>t$Vf7uFxe|@~Z?v4zARRI18{kS(9ED@WsS_(~acu>9n|eJFCFo518>L;*r26
z#;jNtXN?z3b{S7Dbo|9&`7RiNAj12$(SpTaZzdKJ$IpVof2&sm4PmXWmg(|Osc9_@
z!<TgaeBoc!02HO3)9E6Y6n7@{Ines9g6rxWeNjlYc-in3+8JA4!Rs*}j>Mw`y?QD4
z2?l=mmx)Ia>Lfv~;e{uN)wx7(*p;Vh139)@7iIC;YXhQ{!a}Q~(1bNCpau#*pOENg
z2-Fu`0)#5WB&$g36A%Viz$J}ckkYVc27>4jr4rb`9x@uM?{Q!SP+}2t_O*D6KLjtx
z+CkpdpEWEM4qb4q`=z>n|9`A~by(Hgx;5RM(p}Q9XrvpYk?xl6?hquT8>Bm=ySqyo
zrMp8K`IdV2ceZZNx%=F6|MHQC>6&Z)mUF%19pfEi4W7bJlvj@sr#Z?2{c(!08gAVS
z)X-QBp$t3d1Eg6C@u4N0U&OHF2iq##Roi<M?s7?A9cyY{=l<shF!C>l18hDyqZB`1
zoD2bqOuvwm1{#FQdeCZsL9y)=xE-QrKI<JZKXmc-ccnv<jQ@b)0M!AV^)j31!$;We
zJ9=q)DyNZi@(>XIBeLyI>7wX3_Hbc@Zv0Rza!UEb^`pD>3rMhWa8!xmGc_2%n{b0^
zz3S$hBXnf@8yL4Vb7OxGswQ}1Axx8)=M7B_xz%PY`V~@nyXq+lG5Rz_6>KISh`>&Z
zqm?cS!s4DoW!0$G>L?m7(_lsPe6GkAxx_7a78Z!ZN!yCoramP;s<Uaa=+tbafFOW4
zc<VxVL80y2Ojz2cds-8O0f7Sn<11I`Y0kj;H6B4sk?H-Z8*Y_+ijYKT9K~|KPP1j$
z`o1Oc!5RBrhQ>SI5@8TedwE4|MsrCVV6n&{t%1fFuqE9QuM4eJ3&Ai#)#sle$-^SG
z-%E`_hb7$Saq%HnLM^}cR&g3(Ff@OIG)9Pp<Vjb4h$=Jfh}jydT9=Vh1vEMr0u7}@
zJBcqNLj!cvRbq=#)8B0pZRBDyGb186DO7F5ev0lHIbbWypM(#xJC+H=jB4!Wq~kQ4
z>P<|6&)B<<61d%p5@?P(?oDeq_PI6A%CGuKDy1_UB%X@kRX<@hvj|z(8*RDkIzu9Q
z=+@9PX~#ggFU28CLrME89{63g>T+Ky4LpK}bg0(WuE}xa-uC#}DXNBdOsf!`e!;T*
zbkFcaM;HY#5qQK`$VU^h7<Z|hxk$62$&-e~vS+49@YdMOfwh;&&Fa{WRFTD~rFVh;
z82;X6p~GlxxRSoGxh<<8N7i~I36@^L+;@oljzWtYs?Ba_FOqlKD7j$5ZPGer3LD4F
zTXqGN!V@W;v5k5=nkj0<l9yhH&-ZcEYflQ?$mU2Oz>hGpWn(vwDk8($MCHKEL|3<l
zqBNw5SD0!s;|Tl4Oy>k%v7w~@((4uBdLTz^`e|=Qzjm>n7p^pBdwXDdL^3IF9!CvY
z^5W#%GelK7r9#vDqyVecu5z<PE_8Wu`Y~%LKjX2eOkx9_+hQxh_fz5Tr5-rK_K6#M
z%?Vyanzb;h5~XugZsZrae6%0f=rZ%0#Y8x6{1R~8D=c2trr38?`DG|PRtx4_;u)yA
z*{eOBbi^xH&-^>Reu2z~O~XyNA^hT=HTc55v@ds-Y9dkCm3!-5`98EvVt5_$oO%lL
zs+{tYUsUK4svlbwWanVL!bkS0NY+AF0#vV^>UULF-y0M*b9{-k29+A&<Ahe}Qs-J0
z?;OKk`_vFTx*cQmkgQcs9!Gy?*|Nrl95y=A-facPIn*zqtHRJD5zgUcp8?(e^tv)j
z2L%rdNt*ohiP$_`36eiO@#Y-RS7g*TZ_3&eT4Ycq8vl%{oSA2!(!$eu<NnL!@a+Bm
z*`UJl?vugpho#1A@)=S52GQ(%opDb>#&dgzCdFqfljM)Ewqx3N6vw2S?QiRqNBR#b
zU+mB$Zp~HSE~o6V(d|A}o4w|hH^fXpIN5b9HlakBlYB$6ke`D`I+v<os&X7w+ON>u
zEFi)A#VR1QKV8Z@VcLt5$T?GK<5MDt^<W(M%ZID2Pm9YouMas8y~Cay+9gj@n+?CT
z1w;aS6ywbd1X|GHgDV9`no~#CldEn9^z)5hB3Ph<6`f&bH+r!6^RM6FK7fA0dC@n~
zO)Ef@j*oe<;C1WCvsa`k3$Pp%v<$A5$giz7LX)$gcy{vSh_Yuivh<}_SEG+U^EYMC
zPidgPFN1z0a{W{W{gF%h_sSr^cjtFt)%5@8(E-4ut#73};Cm$bw?BXIu>hcE_xsO(
z<kk{@vjO||_dko%f2{qZeEG5E|E*y=8xs=)&@&!r?ES-soQ(-61qaG|fkCGXTmZIz
zE42Pqn*QUx{NL=0g&hEtq64Mr>_A&EAk1L_8nrWV0;TC3z=3M!-=~xP;Uo`K5Bwrc
z|8ctiH#=ixVP*JcANO4}4<ydNxt_5A)ezrB^Z!E5|IeNM_00Z^t^T`b35-r-;9v)O
zqjP@i<+B3i{7ek&zyS*2^8_Zn{a#7&&z=26N%2!e*?+mSZ{QH<YQX%>n+~WAU}XaG
z9Y8qD478i$<ochUCfl!1^QREr|8i#l;O7ho0{`JB&jJj;V+PLBGXo9GS-5^boAJ*h
z`|HO3+!y$UZN6VFe@lD<%z6767X7sJ2gc?AM)m=f`@aHR;3NMZmwvO&X9j9)eq9Ur
zDHr;;km}zmReqv$e$uS~FF0U&!1v4Rm$8IDF+cwcEd5g|;~z8-E3j?xH?do-x~9!?
zA0{xtjrb15R+E0T@pKyla+5d|FH=Gb771<%My$EI*|KpnWm9Ul{e;88q(S;^OnQ^s
zB+{F1u4{)j2Mp*XVkGYtS0{(Fb?5|Wy2!T;dYz+`tBN98Rm8(#AqBh*>d+@$C(ip5
z2hNok=2$}m5BY0ON(A>+MdsH>LLcPmMeytLiti)GEODEK#mkXzB7|F?tBCU0cxM@M
zg!u;{A3N3G*j`NcUUOg*F+OVu2gll!#0Uf99W10i*Q;%Jy(U4Lb-h+X5eL(}zS`uA
z3KwNgR8#Hz(wiFQ`n)JS5C9xqmNrJ)5_2Xi48JiLRKv&HJ05aBxOF^VPJBF$Zb9yD
z^eOc2%=E>CJzIZ6_hQZppUw>}l#a?kRi)h`Qqw^{E%;G3RTPcgNo6*LeWsZ4WVva&
zRr9iJ|I3wM<O-uJR`HmZ1>vH-<}6&e@(j3F<*asN*_V_IJ&X*^oYl@05Z!e!to?W5
zp82Cy>?|mtF*v3x{W9KN^eCePVA}Q>F?1D9bDhVWRBz>Hf@8wFRg~~dhMNkoJ!Ukw
zv{L5i=ssNy`M5T#`Ln(xI*@BEpGz=r*~hzXcBoJ-p006fpDxoZF1FZZ8@s*E*bg}g
zzi9*7!;8rY1>+gEn#du)Vin>XLgR?B9%+JdxV-X8>q&QBI*CE9@la$Y#At=U<B8zk
z8fxO#>7XmA<82`B601JT9g1`ws(nS=na#ERbj)A<O4nn$cE>a9bbfEI>{TtnfKF3R
zu;zipajKUgeI-N0pz4tAyqkWwUI1?9>{!uhUW6u*Ehsu_WXFbj997ISN@iKp&-Cbl
zi!Nos{EP`Pcyn1RRl$H+Dvyo9Q?W%3T$c7eM9aXf92i5=Fu3SdXX%r>QAR5V&Amr0
z03{7z@NrTluPO4Fosxd7L8ELubUS2(23yQNw!U<3>^xRnkx6co%UK3j$lsnSVl=d`
zEKh&*9X=UZvdP8-k2Cv|1SJh8IXc5>WJ!ir06YEM@khD}mZ1&CS?#n<rB7?tcz3}`
z**nK-S8(+ssB|&ZlF;tP5spm4Z%w0llg%AURr~N10;YIjb=g9s6cHcj<@(MhOP=Ut
z6Z9+bKMm*93_P^np(~cXOr*qj9&6{Ropa#bvWrXWp?X>&yiVCC<Wt^k#c)D|Ty=9D
z#{<do|ANgWjXEe2dC3n;-(ZA9xkiisev1L*))S<-I7Px5U0@pxWJ`~W+Q;$DQ|%On
zLNsnW;B%k=od5x3y+nw6$-SXwyUGlzGFnDpbN-3@%oXLjSwA-eeKcnA${j!dqR*pd
z*$HFW4303OoP%1r@T;|&vs>D_0^0SJ$FrF^PpwbhqjUXB6**pCoO<4W#mD@b+RKt?
zn}0WV;4_wy+>D(GdjM`r4!L-V7Yo&%4)43A=N_pXhsD5yV9YTn1cLmLQ&sD%C2J@$
zesqJ>ZnYoZYPrJ6hfc=pY4(FEibQ7uq;fjH8H+TQMo3)l&dR6hu8pi@ChP&;774G@
zHg5!%JOJ^lRcy9Pt{Pf-ttrq0jc}fP3JN$e#ragVLxFOAdxlAWGxl7=Flv&4*3rxX
z<Tzt={jhE7(PHZuUI}<1nbY^g<X3w|>tt_9a<pH(f`c|U8USHK_4H2H;M`9fjG*3E
z$Qaz2L3=-E?Bp+)UtTql>lK#hG1MXdveVB~G4(En?hK48=ny&nI6Xt&ab$yRfZFTj
zwMM$qGZD`Cik&bgw92%XZ)>-LZR=|qcrNChr}62%`$54Uhusadl~6qe8P}`hkPG|C
z5<XPAJUFd>bz})+QBQ;8e+_grpEKrhFpm)pefF?!^S=2oXxe-STSl~`TVIwQAz_OO
zQw)hB++E}uI>`Y1#_cNEwRU#W0a}GLif5+SS-e7aS!%i0gD20;LDv^P(jenP(t?k?
zxoiD78uj@unf(DS?Iuglmnw|bJbL7J)H`rko}ZmjEeaJVAMh*<Y=kZ{Vz?E5VNcd?
zws!9iJhR{|{W4s2QHsd?VoClJp>}Cbgcs*EL$}2B6b%;+9vBMa%QAMBwQM`xrcGEf
z@M+V^7b#-u2oeFi@eCid*aho&(}mE9E5Smwn$r5W(WG$mWFT5V4%gqIy_F>CRwh{&
zibNj@S+!NkSmk+F3uVLiPP9ebdq|Pp#D|Vt93K23uA$1A)digzY=U7tvQ-Sy<ZEB(
zTZdB)kxAX{<D5MH^QTP<o_^G>B8jo4bk}2sU4ni4H^-)Q=_DN8Pua>1Ce#@@6l<?|
zIeCnmX{BZ<!IYFH-~jwcPL3BXA+D=iTfE5+aUpjFn``5rJ2UzCv=l$_<>Aa}KiL(7
z3Oai;g}GA7Uai|Fl=N&rRU${62!>J{e7(sI#Z`;@(D@=#u7o7$%U3-%nHC;daj3Mt
z07{AYg2Ipaf%BK!T8F_V%!+~Octj0Fh4@h&85xBVhIZ`utmtXNGjXkDOy{NY(z=DC
zqXnS!8OpVJ9(IoWq=0SVilR*Y1J|Twn479J={t~9XECEbNm0u_QuP~KYK8Zfh<<t<
zWBIs7&KZ7*HLotK6`bV-ON{_J%<YfmwXwA$5l>ZJ{_l7qj`;<Po0$XnsRHY27L;s5
zcLXACQPhZ#`E|8@d|oVR;59<c9i&7MMQ$Noaz!G&HZFjFT|&T)6(UK#5N#s!V%9Xk
z?}B8#=Nu|Z-I5*tokGs0Bg^Ujg_#W?q~-z;riNhaB!dvr=`ruC^#OcyUBN0t{iao9
z0Q1}hk@(shVI%X%43{*xl02&5)nlbo#oQ=_i*-tI)zosg;Co#5zE?M<!4Xdyo5WtZ
z%`W@0@7}b~a)gnG^P9qwGYvjrD*@`CW=lSg8xgc6>hLiqjN3V|_p!cu@NXQB$&!<r
zcI0)#ndg0}VX0(-^+ofJ!2PH|X@Th@*FocsxRh$DPCcJl$KB!xbP%lPf&QhQQYt{A
zM(ln0^DySoumbmtnvfpz<Exfq1)O!uVh<0u>KWn*H1oTY;Dr<31db(uBwHaaY{8K`
z99U#ioZxs4%99p^8;WIrL-|SZbNMD-Ht*zjICHgHvWHAxi3o-{Gm|QXBV~IlJ2xgd
ze4Cyi^&L(beg~BQid23TpZ|yw{uwC$P%`~ZzTh`Nne|(B`wIa6{<HrroclRX>KlyX
z0wxQwa{(idfxgr%z`!L|Ae`d>0%I1Y-$Rq{iBe+5_V&j5PAou2GyPuy3gFM8sqZ-8
z2de)Y6UV=rnghN^F8`tP`bVPF?{NU&R|mJ>tp$d}G5m%r`!!Pfw@bebUjql@f5i~L
zTAKfE?f2n#U`F1rYXQK*{TJ}^=cD@Zewl&hdK^IB=&y@^<&u7LSj<3#$IkIrGj+gE
zv3LIh`TWEP{=^mj8+!2X!XRMU8&I<U?Zf@|SgY>|Uw?e+|3C5p{u6%_`LwDpMJ_j>
zdcCjMEf7F&FjvSHc@7H}&f)Bg4P)S85(|A|khQmcNuBV~N_!jZu>XVxD=)zvpwka~
zRYarnws*ZqN#)SCRx_xi{e5c_x5uqmN<4xnmWjQSQ}qxF>Y!^LqW83!Q?0faOq~3>
zCqV&s!*ZfaZzIA!fls@~lKn>8u60p7l+E}C(|o<C>azNAV~tAX*VISmxMBwGVfeOM
zE6q(dp9iv&l=ZcTf|8SQ9o=*<?$`+it-%}?DWha{tKw>8{-6^2zBb(;Qn9(#RQ0mB
zIMQ&8#jXAG!%ynFa;|p=vX)aSlvMz!y$%+sAeKR?5|L+Bpy>^j+AANQhK33EDHYH#
zCLa2kU~es|E0!WtBSwecH}muDJzYT(n8wqP#Ost9lgIR=UckRPadq0o?$d4g(DHRL
z{_%V$`}nJE8~byFfV-8%6YL$j(4N|CuifVCDrpNnnOjp_*ITgyC6IFHSkXJq`+cly
zlswA$xh=B#I!F-ArM_7_e&^{!T5G5J1ku!FGn_XaN>D0k6D~HXk51+#6!dYZ8iV@V
z`6EjSqS!}v@SoXM9EDZ+>Q>3TdHLTtP8{A1&8F)v5uTQSH`7=X!Z)BLo6^^UQl6EF
zV3>cNvqI}<#;YB)!#QFM`e3l<sQ%p12r8a2K{j45n9!s}4K3y^fWS0vE=(~OYifWI
zd)h}##t~x!DP?Yn!tGRntiO6@g>EQTh~jn91^l74uMAp=e=*3?M;_DqXn2rHCNw+T
zoUA~FVAVdVYQ9o*-zUV-C7xuIqMS^M=Z?G~$Zgbs*Y^{42dmpG9a_)|MhXu1ladFm
zg)TO&68A+^oU!!byl|}xiFr$R-pi+v@1UW+EFIZIrqBuL8riXn&mFZ{G`H+woxOh<
z;Wj;&T0TGLDZUA2^zng#aR^_cALj{By>RLwRBOTLMBw+@=?Y;6>Gh+kWAlAu-L;IY
zA|<KJsH*&qLf_oPPIZZo4;;ZurmcxEf}4O$Ok=^xGn+&oa(wkBg*z{hWYo?kW@!G)
zIyd9SFhLG`9K`LT!9zqINkf8RR70B={V(kHWx_6Ds>Y|7eOX6JES-+Xmx7a7UugtF
zl+ZNnMAahWdF(!`<H*4_>|ia0<KEFd+#C^M{j6h8Qzq&>6o2tR5!t~wAf*iU$oi!b
zf)eYwZ2zUPgc9XauPyGgAQ4gZ_0WmWKCso^B!cgEGhS!a+&=A=PbTcs#$kK*Ml*Lk
zBi&acXTcFldlxa2Rg>N>E|A3MW@bP0d{@&%&gVdyBPDmWnCiB(q*p#gX66#sU#Xv>
zcK#$K-~ptGFnaXmvyBc6?>PKKog%t4dlQJn#L%>yd&Zo*{$yO7;b)}qOcFW?2Ue4G
zFfRq*&vxZ~yu{|sLJ~h09!*5e88~p#ebx!Ypk=K-*gz8wRZ|cYyo~F$g%=^qT@HQQ
zPr8BM0~ynTPj*n*CD9KjtQ162fElb)%%rINEDF0OL{2)WZ|~C^hUdMDRG{yXo?03_
zy&4BZCot0X<#xLtv@0plC?biHM9^d9`<a!F8Uhp>$By6H^)jIfg1DRDBnUh4F|}#9
zpyAu`aFNpc+Q}#7NkL*84{D33X1-`P?mrtE>!{|RKwz;*YHHn^o4u`dNR@9Uw`5PU
zd75qx5%B`%w_oJ%*iQGw!4rMx&!^#4@^C}CQd2wHVdWu5yTD0w*0XV>BuxR4d^-}a
z=xK^$$22Xta#Yen%kDXIa#Swv3|0sbP4&=6dJ*$XkUIl8AmY<igm#l@Ok^C(FeL?y
zxv=WQ2R1Yjjup*tcH75@<|^%Xqf(LN;uK7W3LD`YY^Qyr%|$Iem6|J!d8a$#gv-l|
z53AQ3%~DW3(aZ0Sa)%Qrd{A+O-Z94}5oZrs#8?U`%G{U6XT~RCK=+fRaSuphWA-=Y
zC@|5iSZf}SHRhDr7eKlfw;jS!njtilVz!P1?E2z?wvPq1h+*QyRE>o0WNY--zTvCO
z)cF{q>KoMlywNe8&sQd&%kiE!6&)6x|0Q|fo43^P3yUAk2>hiAoy6(SLH4(cQ6fyM
zh&g7NSUwqlFwVO`!4;;_!!Sc9VrgjDv)r&pyDGK1_lKfJc8Ik>5!CKJnRK?07mls=
zU`!e^B1Nn&D$1r%GS~OLD=LB-H(@2Qkrvt*jYHxcph-q<jOU3O3eJ93mW|sGV;L8y
zEcoRR4UlHT?iFi8cw0j6%r==X#OZ`X<78mGL3J^nH4@4sF|Vi0dIsQ_KJiusXrJ(!
zfj+URswxZm!4e5rET?g;Ow2fMKI2AeYhEiiXHL8`h)g;}vKvvzG0~XV+nj+X!h|N9
zyNax6-PlOf<u2^4fb@8S?(XSeXS0S;mL3En^@UAvN>V0==9n@(qzc_p(G4yxD<#Mw
z_AND`g;xRww1@>6FhPUQ_C#+@Ny3;04vmZcDl5ffP>qUtuJ69>q6;5KKXuef=L9&}
znc+qGjHEMUd)2U>HQI-2kK<F+jlxc8L{XbtDax{x+1N3pm9>@CNwAwv!=;@=WsIPA
z>Mk?hrrHpw8Q{x$7$jWghDV%-2FoSF1fr)=;E1duKfYCC8*+cz=FH5rqmGzFM+elD
z{Fw3A0!N+fQb4q0l>u8%|22P$4mY_JF+lG8R)jR7z;sgC>}W-cQL3v0i3@osLWyZE
z;oJh(_Lhy!^K~Z?yAAv~_G_~D0#|zVj4+x*JRpomKnu~vld6|HoIPG6%U+$`A-*W?
z_3+EZLRs>AFF9FtB8BX@3tpYvj=w^>{X{4@IfpbStmg<>jMx8cxg5`g_Y5ROc=Za|
z^3`F_L#4@HuQn;`xbhuW+yF+AaiN@TOz8)C3vcy;A;|n#=D_Ubq&}0-8^M$GB6~l{
z+{IoKboY|O4m%@6OoH*!U>A1GaZgmXei5wV^Bc8<CE@1HH>@w_#NhCa%IqAu;KSMz
z6P$EMYg-O8Z)^&=g%@nh_cI|QqEfP~7pHYO8M3nuQkPyxHNVrjE~8*1Cfq~|o6&V$
zNPV&j)KD`*eQ^Z3Hq<OlF6Vben|s+8@*7<9Q>^F@HsM!X^S$!=d7$G*zVqMJLH!2T
z0Dknw0sjb)2K*yH8W`X6SG@E6cmJsT{yV}0@Xe^21IStdgMNThCjX!B16Ijg9KSbK
z`h)NQQX+o=I)7#@{>zPh117*7v9NP|&yEEEnLcLVbm=$dkCW^7Oyoc3fd4|pd_T_r
zqay<vcyR*h3>M(%@3$bUZw6j$z~}#sn*`=b|CS^D=gxk$$@=kt|8I82{Ev8TW;UP%
z5=a;^1K5B&1hQE`_LS{k7<m12XTSQ61HH<B!D2ts(*I7N768cA{*2>(TKj{)Vh57U
zK*sb}7{~E%U-lpJQsBpy9rz4>T?_a%ff?|d*Zr1S4V-uW>)JoV^nZZ>e*%3!!NH$s
ztX~YZ{ygaP%arziJYoG)N8k_G#|3=BzX|(lHKZMuIMLefRZk^U)L}sUD;AX2?VMRu
zQY8(Jl#h(;Gey<b14&<a&Yyj~I}!sY7oC1vACm?HNFly^WFtR5;yr%6IGou&?6rt?
z^{<XA=vrEKRfLpzzSb)_WJKQj^4RN=@oK@UAa#a2#6XwXQ|zmX_La7Rs>q`kCJetO
z<g(4%(tVBf#v;TZwQ}pVL9bT&su=2%71Sk@c(|udhWZk@RNZj~4pX1^u$&avfr)|P
z&@^e1m4KHmM{9>0RL5H%u1fmVL2^C1-A?0B!lfZMrkc4ueEQ0AFAAc0LtEnVqQm8F
zuMsh;AXNP9+kNIf&gJnv3<|etykWTvE6x%8a(|m<uTRsyTL(#CNodW6dgTCiNcGjv
zpv{#)T<OTch8|No?K6kRr`>FBs@csD6<8E}NVc?TIl4nUQ93x%{+7`RGr?7tg`IdA
z&j{A7xp+!@FT+Gti?tKgF6QNr#_AjJ*P^v#3C*R4PWQ3Q$<s$+bElJo{c}Zn3K)K7
zF=v2G)`vk+UXKGcKHMg<C;M8rLoCu&8S<{MsW@1alR#Yv8Jyce+O#hP@8Q9)LZ4kp
zslk<Qmz;Pv-`$>ni4g+t^*5XNDwm63;i@=Ga*(nPPme+WqO)7j!%5SzH+{ybTwyR1
zQN=GiY)occ?f|_Mr!REU6#pxXS05@o+O5|mtVd`M;raf#u1$t~;W~o&O0oP_{hfTN
zgB1X7|KaU(!$B~@uL3H;nNZ2zL@L1pQbb-CSFkUJjBxkHXABiyXd0rL4!%=<#v6OX
z)5~&**~{$p<=rrHJN<JR+3_o_phs1XFB>YQ=w)@0Y6|NZUlSu=X}0#dhYmi`aw7>-
z8l8g2kDy=pon#vkR%1M8Xxti4wBfK}e^L*(r7rbl`>H!W;xvQ#NviKFxI)IO2Av5&
zca8w8zi0ZG)I@4(Uej9;(x~x@lA>o-30w|IH}o3XE`YuL-hD(JB=xhn%3KUOq&RF!
z)U@4HO9~23Wja0D4}!#P8R3{4o+!g{4<-t#C_GYW31F`s;A%H{Bv5VjK#{p)*4^~I
ztgR}=RbiE$niz+ywUtGzFUuKO9V_#FGugI>_4Fp`PwSUj`Q}~1613ptSz=-1^s|!U
z*oSMbMWI``K7i%2RTM0bbEl}*rti4<rQ+cuTv~ZG95q{#+Y#3ifLMhvJ^3R{?9L`M
zKWAiWJUkRXc2t^GKjv#HE2*=%vl6eEG`~G#uC6MhQNB#fKEiJy6Gp#UI-m&apTO)1
zSkd9~Z7~9sF&JFMSC3~rq9P#)60T`H((&?Yw@R)(;!1a#KnQ+JXCQhdxw_Pm$ZKqe
z7I1@7Y;H!j0-(>_EEUZW>vD&Rnqaj*M@Z5>JW#BEPZUBkXGVRah!6#y4$+$D0KOjx
zVGQb8ZxbP^7g&{=Cv~t~?p&+gOqq&1<8sB*n!(&S=aLM{4UaV%W-0!eOi9y4AUcXi
zK|C$ix5071S$^w&SthUZPU85=)jD^?S?IukyPF*vHnMC|dqtU=R52n%g0jdi@C}Ew
zm#XlX4r6-Lyoqt>;!DyF;w7<IK|L`jKS3{xE?Z_Lswk0k;oYUHfY6P6v+A>WLn0|d
z-=lzLeb2KFA2;yUqZhcutgi+lbo2J?o??4sV+^FJ9L&LWl0DJ1khEStH>apA>nU3+
ziKwjSKEZoY<^ad8Sx;a!i__l8SGu@3mGJ3v0<!S5XdUUY829ie%6GCNOvz>of&sbV
z7y(x8dgd^b;t{4ORrBdrR@&-;;M#YhE6c_N>GNy{O-}S}PRH{;+F14Vj$%oz#rMx`
z-tXH%R!861*S^Z?@z>tqGg)R+FK|Hy2s3t>mQRw|LEt!_eCGDQ6Y+d1?*%oVJ^WBl
z>tzAGdvQ~m7RR{|shCn99w=ReULS-I#p60>ow~kA4haBf(7%#rIp-geTQ#Uuo*$T=
zB5?=<v1A$kjMf_h=TJzrG%m8Q-B(~PT&cE#qYct2K{^R@`MNGo48~luTcgo`mW^p>
z#0t_rQq&7&V(~L&Zd=vDfp>*q(5ONN#@PEO|G0yxWyV~jupM9E2=Zz><4{bYu~jO2
z^YqJ;S1F};Bd5XSLY5|2OLu7xG$s3I7|QG6Oz>Ft#xj(9(LfQ_0{nZiXlTU2@@{|q
z5c!1KE!IK!_mo!aDKn*ki_4^SsrGxtY>@$mXwc%+ND<V7=XUkUS5cc$r~U%D;uGO;
zQ<7$2fFw>UV$y?}8iQ~U@F`zoCM==Efy|NZNYGg#GlUo?j4mb;&F<Ab41^MSl{X$J
z+G}k#PYv@8yUEZ#XfjLF#+EIwv_R%6RogT=5~4;?Q6UtDguX<~VWy>8LtVpZ#kSs_
zo=AIiOn?g-@&zS;l8*;In;{K>x<nF^nWrP~#Cv59I>@_HOf8z@9~5i^fnlMFNnxa#
zi|EYu@y=E|$sk;sU04k3blulpSEM`yLLwz=c+R?cz982MCiyYbNyTM4`=XaS^38hX
zlR}8x#6pn#*A+E1eY##$6C%rPuqrO-cQg)XlG|OXeW#Q<;G^ceh-Qm~A{OQ{Q(c)a
zKuRadytJ`-w3EYz@*GWpRs^gxS8!?GSlJS|DJq*TgNWEN+fW+Tm$`j~)pL0@DU&f=
z_zEU-swO2bGSvmrZ@Ge-V_{t*6i>s23HhV%`pDE?!Yc8u)+S&<J9T$Ctu)#!33nVf
zRSrhGp5_y(o|g`XD9)$X-Ip$MA=W-R+srhMAC?DsC{m<&dKER1vc~!QKcovE+xezG
z=f#Rzmsevyd}3P?d@Yj+41POR;1pS_uVXVa<GbS;En;ZIv?SRN@S3s2&KF;b@54q^
z^K990U17SYL<FrPcZ|ReA85Z!&nJ@hH6XF!JmS`a(tcC4`9=X1;!y7>zX%zCD<^l0
zCqcY`s~^9{u|ODl^>XQMew;zgz|omT%rtSi3-fHije=bQosP0mTI2)Ri+%8jwEOP!
zE+oU!I+%1<fM0wAANLnh&YZ)fb4lmG!Iu3ZWds#Dg%<_I1&U#|QSRAi66Emx&&5>6
z(=W_dX+?%&0G!$n0x4>UywWOipwcK%83<WOUQ41hJ*0sCqJ`&?+oC5QgW$*rT1zF&
z<qqH$-$se*yW^uwlp;E7H@ynzDv#NCUlJ3lU}vy&AR?cbwpuo%+uArR(GW(l){I~Y
zo-k|qmIBF9d`)hho2u@No7}DeyU@_QbD<JCb>Dj2dov*x4sh%DazT6a<XOKN?kpuo
zPNy$O`mkIkcr6wsw(N6vR|GFRuSHc*&KaL;(HyIUqh(4+2phrlS+cldggzH_egBi_
zxwsB9Tvya0Dn7+=I_5}NaDpK=DHgsh1@n$B*WI-vzY1<&>7$}~bh=2x>xB42q?8W{
zZEsix)$S%<@}n5Hpt@#K1Y8+1XFs|&u=~v%_$f6A3+n2hWD0@A?^ViWuIkdw%0{4f
z5px~4iLi7E(c9tLVCS>a;I1cs@|nC_KJx=`0>V8sRX?TB7jyPx!)>J=*F~)Y1XWMF
zKIOb^e3Mp6+6lqZ>1uOIfk1bRSeV0bXspDewNhcY+oziVB2jGF@=4ecLvrPaw~1cH
zS)TWBTNq&@Fby9{SP<D@e+!{f+gX0@vyy>4YP`Is&SV7w09HK&e2eJqr#7-hax9_L
z3R{sZG$il94L6$bE=fyea^wk^R$A-|DAAsA)(*M|1b#C5$4McHT@6>y;<^wCW{|n+
zeKdEH8c92&lSM^pC9v1ww%8SQ3TV7%Xw*8<EHBB!41yvQ!M^m?35O78B3Co9$WLjz
zgC2=BCtwdms;O=hqEattzlu|fHed6upw-ybgyX<egN90J6q{u?h8d*lk9jMcExF?|
zEsX}U<4S|Xkl6HII?^T36NBH*iEl}+iWsh611ONWxmY<Pa+w9o7F}bkh9$?d@S@qN
z`$}XR!8q}o&cyf7<e!n^ubqkS;Na)Z#E)>@|DrR&{-Xl{VE-XK27K2S0wu~n0-Jx9
zB7gtgpSSq;rN}=svCQm1lm@g+27J?A0y{3u?3@fhL<-bU0twmQcP73Qu|mcUj=;FB
zzu@fe+yCEg?b};t0wQ}wV?#$`O=cFLf)*&K1@4Fym>C6hq6Y2|D4GVgDA-v3rv?D$
zuT7Jm^qT)^)8ywf1KNjj0>{=^zuRL2@juYOn*$i01>7Vs#`*Uf`+jEfHjWO4W)}J^
zOw3Gw+1{TWz5dhf{c>)ctPIRRtu!+L=<EhmZv!6_@DT%TyxD-H`~TQ&`b$se*U)aj
zk5us={fU1c;tKfggZ;;+;Tt9Y)7l?AGk^ow&;gE|{-sgy{lC;7i~sXR!9R&!pqBQV
z-TDtn!#771&Tr2O_*whz%l03)27pOy|7^Pa(Hh_Yc0m4SYoH9rB6g|n?EVq%CEuQj
zBa@EjOP+MDdfB7!*ffXNtX46|yl62Kin$~G3v;?F+H?YnaO2L4o~VAkVDs9Cnx3_~
zXEVALI@T3Bp7c}kt8^R5YE~{KbcTwBDluIg2_;sIcjjg4J>@paX{9Hr`;$6hPOSyE
z&CQWeM3z1s3N4>?ObtFQil?eVxLP%hZL_U;rCygkx;_qHrIx5Jm0YWW?XJ=?YnP}e
z<dv+uewk+UROwpepdG{T<>MULkoxA)%~byCTJ@y-hJw-GSb=<}Wj7>mfZ+VR{tlEH
z^}wW1S_v&^08;*P{eV0p%)i1qS5Go#a*Sg@D1T1$GY)={lMfR_idx`cw3_3yIx;mS
zRXG5+_R4_KC&#+_cit|hkyrTueupfJrPR?c5!(xnt{10_N)VQ{pAp$cQf!ky5xL{?
zHN+y?`{%-s=7xt}f+i>I4b;f?xX-bR<4&d&rC8i}3M6SgjgVO<>j$oxb#^xM+$A6$
z?ZGrd#hba~5}k{`3kU18ao9gXtp!bN&c}`NY!K+!Gmk5;hh9Q85!BoyWrRI5;KP~`
z&<c5m;ulVZyR;G7a7edL<qsw&VIiH_a;5nOJ?jl*6T0X}R1o7sXJt%8{OeP%k+Y83
zZRGep-%!rj0}dVyk0xjPD;|Q3v+J24^ZVslX)Q0#XDBT4q}9}EYo@NW-tQWBFsTFu
ztnF`GU>w5l(RbYhDlnkq4`D-C`fcXwU$#~(H@c@MzN-shP2++o-?<{LjvK_FcEOlC
zU4<7TW9d`Dh`Q@jns-x7r)&%hZUdFB42p$PGwO;98JnqNyU#kWI<~L4Bp!?fkA}m_
z$JeOAp}4)%--QJ~>8&U_i!eJh-h9Bw#^=R9WT+~LoleI2?CYNG-<5VWzopY4y-Pga
z!f{`ar&{MUm)c^a)3RC$P~uMbytEr0DiOhVeJ2JpHn^k1x$NrL0L!h#<<Ky=yw=D1
zrg`%-DV8{m_RU7;$L&d+fN%kaHC6{pPS{Aqx%6JJ!#J1pYEfUmiBD{MK|&IJop0PD
zTz&4oUVWW;L=#M5<y*Jf^*k30m6#NI-BbV)Kn@Qb=y@hPrcNqGU$gouks%H??4#?(
zrrMd-hxZT}NpY;(GSW`vX`OC|15=TgA_{df3UtXyfwpFM<>O1D*#fX=1StIYrAjMF
z0k9DqnWPNM(GpM~?H6A^X)pO66D1ypkmhfYXJTRUt_OdFYz6<k4Th~{eDJx34dx+q
zwYL-F$@#d3T)<XKRvs-we*Z>mZ_tvIZF+b>T<qp5xAAlUm9v23@%HuP$8`Fa2iB<{
zUQ9){8D%n^DmZJ!^o%{!2@Lxn+#%P85~fT@oyPf+qzxbHbClkaN#1ivn`lSD)#P7*
z7BWck$L@ra#?xguK7M(;n!}0qJ*0p-l<<qre=ie?G5wqnoiq_9U6-oS0IaaL+HoK8
z>-!4t8}G^Iios-Zsqe_7#h}no?qYcd)R=pHs0i1SDE9r|aQL`=`E)gUMHD8*cOK<(
zi6yR-<EI1#!B=`EC$jJ|gm!z9Jz!-LX9tZ*L6Wo2RnZ~33Py2))kmN>?^b41MeHrK
zKt+R@u|`Mbs#!X4U=rF*+L-yCRxA9#%(VJ~r@OHe!yJb<6Q>)cbbID=gHhoq?1X$i
z>~)|p>VYE|y5x+z`nxx6R8%<=myrcoE~$I6hzrU{CJIyJMmiy>Z;0pbxv~z=vyt6k
z{0XE_olLf3s)8dN$z@DQru(#{hSUjj^OE{W_cq>L+Cvu0*JvE5e}ydJ<LS$~g%M(~
zd`Ac-cfr*|=8EkzMUrNIKtL%o#)yA`g4V;DUs_{uW(58ItqpFt9>AS^NjveLI5uC8
zcK<<?IK4U(bL^4E{*4z8XAz}f@iQb20Kjs5c{ox<;fA}IHKDh#oaCwm<zBe7qm&4r
zx$nBEK=AQ;lwxz7aGl8L-jM}@Qwt0zJ?wU96z9b>c0F;BRTkn}@NM)xzukP5HVAs<
z2r*hxo)vv-)|?UCO#D2J7oF2M?nB%#BYb)a1sSDSMsHldo3%H?a^g4vRaKb8;TIB<
z<d{U9%0wiUz?oRaoEoY08R{%b_zp?ayDbP<NBt3CMsb}0>5Vr1HqPUeTa>#eWd;o)
z>!9Jl`3E8SCsjImTd`7#wD>CJz44yvB$}dj;ta`rrw4OF0W22Gc4TB`a(7a$oi(L1
zgyv_Y>0A9bZBD`~>bAZMllY<I1@hWi2%C*I3y^cATq5Kriqtl1S(`f!^}}1rU>*Jr
ziTI=C=XuY#pVPf@YWkWG$!gZPn_K{qVR+r48*l^y_R>0aSbYIMk|@k|WK>E{zZ#?a
z197JTlaru?$$@*vIRWVB3Qh&LxJcXGu18;3urfWjpj$spMCcI>Mi8;~gNmvz8)~_o
zaYF2wGDRQ)?<9DFpAV6B>R&u9=(oEHB4H&#X7i+6zFBVGP2<ZUa$wyjo4hHxE~qfu
z)H3FfQmmp@&ODJrfG?n-JRS9RXf7gT?*@PP+_mGZ^yK=~o$o8sN$8U^-rlPk{&-=Q
z5jd9QLl_tp+}PE8g`#z7o)Y%9;c$vaEjNvjB-W%m0j;`??=n=6&FH*5=^<Xc_YfwY
z^HmmkpNoaLh5YmmCL=|!wBT9FT_5dxaM|bE+@5h1d4(#Vri$3}e08Y$oyO6?n~QQ{
zUlEMA{tA4_gWO<-EaQYIm7zRS(+nelScT45xQm(gDNU`ft@oo*5PF=+^GMIrG#A>>
zO*+g&$o48*xgmP;!(ORGUW54k3}4$@g-3Bv<!#z?Y44}l9F1^;`ocsVI&SHf`e`&+
zy-y<Q_0!Dq$5kckEw$&-u|C6`n+M@VyiiQLB3uR?CUnV&zu)M9C#@%j*#W_yhTnIf
z^cQ*a5Z42O5(LYEnFGsxvz&M7JAa53kh~scxxcdLr0=+WklgQt<hd3MBkbMe+9|q%
za;}5=&Tq!OsnTcHb<mto&Po66IcGHWMGmMEb(&@2*SF{iyO&x{E1&g!ShMELu({&H
z3)V?}ub@l6*czIB$Q&1#a^(&hu>H_4vBus`3s?ocVoJg9i2e#*s_=IEU0LNI5q3fs
zf+Y>~yJOr0-asS}-#L39RuId4<w&pb{^Yx~c%P{8NGn8q4_xETjB3hNhI-71v}5sk
zsl2NiaI8A|V8gi`r#1Iw4Womj&YC`|kMs8NlGLfT9y$rL>R5f2U`H~ak)I8m^&C$i
zs5Iu@=d2o~+)yG#21i<An-Lr^r?`kS^`;^%C#`H|fI}-w;<w^VhmkIu;~{w(Y+efv
zltxF>>A1AD9=|n&=0b2<NSS&c-b?E}p7L~A2XmFzktgiL1;IHrcGa!?$wDSLD!5k&
zFK$XysmEb9JFXrJlUoh0xpK|2FnAT%O;^0bQlYkYQC?lL!xDZr_-?FqZAzwpqcKt(
zL!waTy2PQ9EV$l3q-ioN9+SNB47-s;B5BJk?f4xrud9iY#B-+wH{LB%UO1Ica8IB0
zNFx{sN+#kNEm`=aiy&rIb9U}GB)$gv=}@Mo4Co<rc$+T;b(30Bl=$8kj1ydS@e5)@
z*_^1^anpazBISxz=XTq5t8=|QIYJDNr3NL+>JYyXku9!%AwT(m4I<aJ{~N>%^pN>y
zLHq-|{EC>r*UUd7W}rG0Sb_f+i21t$_7{!vPpa7e4n}>W#JPY9T^68GHYX5;vjAZg
zCvcdL1vq-j#>V<v0{Rcq9+)xpE5Q2;-}?R3{??S@H=FK%9`H99_0!saj41+#D1q$r
zUtko+pTz?IVl6umX>)M>buAE@{m(D_?VP?jVE+ZbeSg6J1AhC7+x=PH`x6KJC))S#
z$##EA8UF`JV`c*SdHzk17V&B+a)}+q>soa;8jLhpRXeJ{s%p>LmlBZ<f_e&MM(Q%f
zmqACmYR+c`CuVUGuYPw}F#`w5bPS)6A&ig=>#^xBPpkED;39-o<AcBW7A2aLVwP*%
z79?g-$Pne7>}Q*jPexaEq|@QlZ!M%|g?Z$hcU!+`(s30T3=VvL^$5628eOI1GinWp
zM^#!X{*qA9e*b|5sL6o5e)dGoUuHc#GyPULo^D;CoF*3XW3HLProyMPwkJIW@HRQr
z95fkm>;=)c4nu+dC+r#(ZRPU^vRw2vSo3=^%H}DCC-c;ojvpR36t#EP8wkWUtXxd+
zv-qF7p`L0GKi~IV#-^jZjjexp_0r=$oS^kFDQ`!hni@5&IQx{!r&=Ygu!UH^=nW?-
z4vxW1wT-3jODF=a<SP|^>y{VWxnbx%>9Mf<be;&0shpFfRNH!2RPU_<pYIpFpl`en
zaJyRJ1T*c<XL<rpgZF6tvh^YDkZ0O8=cs_><D#MIJe;a>$Vk0GhKWa^hA$ZNgnX1+
zP!3<eLVF)70;o9W)h2zSmBGdpj%y(%6~_^$t1rBB=O&<_XjVSGWS^0U%qVV>)Q~|$
zp$vc_r<8n0ZD*zY)<jXnR6&x6_{_<ru0EZhWQ*Ob%Hen|i!D8eb%Z;>kR(j}`KDjd
z*nP2+gFfV;&@O@4Th?*@8tCPTD}TJNA39HL9fIMPCuX?5f-0I*N;am8uLh$rbr0Ta
z!_7D1ztAY;G9tiAJlFymta#~>c0jMlCc3@uAKv8zL73FWenZ=BiL~$lralHxajP5I
zHNW06@oH}42AAYQmtGY%n@r!ZfL32fTmB@P69B<~jDSYzyUlBTV6ETCis-Ff=u<YK
ztUz&N|MgSTgL)puTn$Ksqyq}h*pqZVxk^W#Xl@#(c1D4#AVn0mgE8;G>EoNF@hz|0
zeicvqlrV#$6SIP{FNv(tTmn00-t25XGr8pPHtw8Bxx(ii##R$K@)QE>+wSLD*K2Ga
zzG%-c)bZK+8g;fN-*YefqwwA(weCH~8*$LBJxxgpz7vyAz>I&6DTtkyzmOgFHlImg
zY(9_OhwyIGD~h?r-&OUis1aMd`f;pR5<&fA59ajRw(pu;U+t$OEpJB%<?S{cFAxDx
zv<KI>4x*p6jDZ{_gwrjNGStG_YuL*!cq-*WL(?~JhPEg77_fL+J{I5Um0UYlv9%TK
z!q(3>U1NXhrYuArH)E)lM!NJ}JI7enTDCx_=Ei1}N6N!!Jj)nUB9Sj(Rx(}+Vu--W
zO8Ue<&95PcqSrf7XT3A|@?lHC`>`FG5l#RKpjaKwf~m&%v6r&J-UqzIg6r{jUu}QB
zJ^#g5+xO9lpT=)E{$WG+pZQe$$#<CZTQm5NtMmJ%{e7V4Z}r@OoydQ)_8+q>|KiJn
z^UvSpe|g;>YuSOO#mvBN*I)1ZZ}0k#P5+C2-B0)ZpDy)ZhSmRc_y4`?9@yaf=WG9Q
z-E#niY=85*Z&j0a_$Y<~l<QD)SiL0xAe)PAigd_EXEW(x$79$=QP9(7$78*)yjm&n
zaL?6V@L!$3`H<&!wQ-PK;lX~&=jM0audv$I{+{Rs-&!9!Xc#G~`wlH*Wb*1%Say;*
zELrmPXP-LRI_=MN2`p3I4L7b|sg4w7@J8)aSLubvl<}v73l@wO^W4OEWU*rIz2cok
zwJD4z^<6lm*4|2Dh6=vbe?=|@i=#`&g$(v|nJxXcxR5Wpqncsw4r?j;DO+O(9_MD1
zQt+1il{g`?8h%fJ9jOs_xWwTtw~hULsW87f%!ZQqZEEh~<mnL3(i8-BhcQ5vC<)Br
zNDod1GhTB~ChH3({o$;v=_1EV9VdVWNU3CEbuUYJxQ`;hO7$2^?C#wJY7PD>YL|MO
z@SYR0X8hH3ZZ?#9pw&iA$LcJuqGe3Xon*WM*})r<f~C=V@>RI&En_#%;%fsXTlxD{
zfo5&GEEQE8LH(nfv;q{Sc3Or{=b!_jtSM=YGDC!kl6$(Diak2d+ie6_$#7j>cVuE1
zCp1%0oAI{3lOt|}w%bOHjSke4#(EQ6;|bHyRyQc3GqMgNsNHzPWT?vFHpel;Q$>f7
z^+a=`qlq!TUnMTb2ca*Z^b$!RdvoI=hfy3~1cLCI9A=aIGHy+#Z&8{<{DdD$2P)PV
z1Oew{h~blKjGcL=io2<}&F*tqnOH5B8+8R<=dX_KF;^I}rsAU&1)6hU8#im0BigHJ
z)iBon4mXo7pxQ&kP_ZWjQ#+6Ez@Pg@0aP>K*pyh>&@=t?C^25e$*=S{Twk1GRO(k^
zxO6)pQIsRVlV_4s1YFo-$8n-?%}u>U6XQ(kn@`vA4Awi&xgvW8iK-N(od!Kp)fodm
zBaJRw=~^kIG3k+yzG#QWglIyX)}nDz2wL-TyT-)Go<lZ*eW76%G;e9E43DsofCu^`
zU$mkP52@nvLX(5E(qJ)OsnoW_@u-C!3*nu`aPCCIhnnOaOeev`yqB@dq&T^>5|C23
zPjqtHNPajl5MAM=0~-N!zF@OXx*DKx%W~+$%S@lk(6QlJLxUzr!h)U|oEdxET^`<8
zGQ5_mvL&i%8%>7}y(s+rQM;8O2~P})&dRY<K0hua+wU`43ybrqkfBV-ykLhJyB%5F
z;OkWADn3zGTh1)GRG&QuYPLeviroW6O%Lr?2Gy#^uips}+*vG;um^PCXLh$eG7GL@
zN~~<E`whmXIcZW2-i;nOBfa80Qpkm-msDT6gdL2ur!(rX80h-QzBhY3+i+AUX~&8M
zur+HCq*OqhdgjcHA)xvO3aK2#`J%$mIuEpz>d>Y0v5LGy>6AJ3g}7}2_yEF+F$In9
zG1%Imyc*aI11$~CC%wGxQ;AM;6zm~f=C4qnKx`tyWqKyak!0-wIbusQ;8toqQ271x
zOQPLDR1a$!)MFL#EWT^DLUGLAmD!9mED}0f*4&~^GL|-vYoVlky%esPrxuOic+ks!
zJfS?^-BYMTGAQ#}m{xqK$dgvM&ld_)XJ3gWjQ3vRy0pIFdoKyls_xJ;^Q*0GP?F%{
z<q_iSp_aB_X4!t3hjLFHej;Af6M*+JzKuKGfepe+!IfX5(jE3j`Cwsh*rk7bClcuP
z`jJc$W1xl7K2JGA@NEc~<eW~5dAt|8VW;m-TCUC32@ZOV-dO|kxzu7}PoWuXD`lx<
z-Rg-S3+}xZOB}~uc~6=-rmVSLizW-w?%-+$`~(3(W$T)s@4+%^AA8osSgc}4IKFfq
zc7^QNQ$`T>Udp@EatS6>Cct(D`PjT8lCIPa^L0s6PV|~JDL%N004Su>#|VTFP{%wl
zEs!#TFyX#8xTda;ppzodDkur61LN}&i#~;w<5HxKd&(Wc)yI~L{*}cmS}6U^qU`)#
z*D-395)4wQ<oc<IMbCyBpy0B~o$i8@-Mp>_#m`si%~q}xUYiz^D8IX<8^yae3UZvg
z!nTTRvW3d(H?mTVKsM&$i}SI&Ec{4~fUxjz2u!0nf?riVu~vEL%c4M~+J#-<OVd9{
z2}4UG6saPqfI6X!zI!(ZIlCZM;^A<)c|<?#=~?jAM^*J@#!31t|2_a4fws}Z;jRMO
z30JzV;tJB&`=nxu-fQkk!9{iD{bNn+r-=>;5RXW=RaUQuL%(kLBBN^hu$m73t3$N}
z!wK4E-hiIHDfkfo`jHHN8~BeeRssr;;gwn9KtD`yAx@0kM)=?m#nIpo;ax5=6=|iQ
zt{K5&I6Z;!a@4y2rc&Yjv!3_&mCE-o$)7<u&>rYN0pUP4@%yj;(^2bt#qbLV|EY@j
zFF`nf6Bs=V<XVA)!mPk1!Z(L1Cg7kiFf|G&sQ<k_{SOch)Y|_F!hv<gKg)z~_09KF
z`&%IV-!J_K2LJttFtE<~H?R6*DH{hX0~=6R@RzFU`xE+)s;ZxePoRMQ53ARoq9FcW
zS;h6|^7jv_lM6U%`!~y~S}je7)B!YL&eW<x`CBRnqJh-(`Ox~7v0mBq;+N;X6!@_L
zQLl(iTid^0GJzA)F{AlkkRu4Y$oc5%nt*Jqn)(J@i<{PqS1?LELkTCvk6yjv(~BeR
zG|TLQQIHH+Qa`mizar5|7~H=Jlw?Ddcp4hqJ7gX!7#vA!<uq}v8Y?h%m!S;f9h*{o
z@5cSr?!JHW_GofN971{B7a~g_Ni@?phZ-(m?_qZd8GmGTkqA@_LH4m+Yz+K+=9C@Q
zGQJmI*R17X3bF~$Ho6Q-Kb^c*ru_S<ZJ&M-73Yr}ZEY~dC|MGU=GIf!-SMv-Qxf~B
z{NE0a=eD%*dfgMiTgrBzGK)`(>cnYm9>7y9-BGL)&kVUccy@8UFO9Kp9fCi_*ogIf
z(ayt{^;(e&LzeFN3nO?hbHfQUvyaO2SIlZk#AP&`B;h1DqP8d`_{=$L<Gsv?f}QKk
z>aFg~gV^QKA(GZ7+W{{wR2kbvz>MJDYntrw4u&~Kr|!+k(h$WgTB)1JVh+2eH00bE
zz1g4^jtlC9eXuv-@&Y{`t18%tda09)_r8V-+k$rMCQ!y;)!v#7<B1LXIHG%0-Gbm5
z*zr3&@Zkn6Y%AN;==c1|@QYqy&AOQ+lg0UYSL>-ytp289q6s~>N7F3KzInaI83{Cj
z7SSVj%-W)b5!6_S*)umE*fP*DDk+)9tU1uZH_-G6C1_BTh7z9v!mA|VRdCH{4S9v|
z4`-@E;3CGB7<I?%M+4ztXlJAFL2`s-);;r)$;qn`1l(DCKC04ke(?*g#&x=;O-=~5
ziQ-V8#sKL>%^Mo<3V{~}A$V9Lmptd*$9uZpxb6{h6M{wakAQd@CG|KAYXNx@j|P7c
z_7Q0}521AeJdc9HFBcVUQs9teh}l(lc#7k=O9y4LD?=2^gA?TN{fL5BQ5T*!k)t(;
z2u(kF>KI%0ouWdcBXm+6t%y;?wri*7`yNsEM2Z)N$-*=ChRF(ynH@G#50wZ>;-pcW
zfl1ro0XY6ipj;n>NOpKY;zNR$A>;JnsmbQSise)iS7qYgt(~lJm#H$tQJr90G;~8$
zhoJaHc0p7F?1x2Z6AmQG(^sgX`u)FJco#$*;|-&k>;zYJ%!l!c2i{b3D#19yh-UfV
z!|{S{gwxTvfD_p-fa##Yw~(`)+A8Z7`@UH*U0r*JbqZGGWvjchEw|zAARA!Yt}*hm
ztR7Zq&P`i7GeACe1((3cBwN!qGV`lKEFMO-1}k!O;VlD7&=PFsg`K)kqH<ClUK6Sq
zWgY8WBk!nB%GJX~fkA5Ohm2<*=LX!Xiw!%XRu=Qsn%B)~kDvKx4x;!l@@ITbTd|Bq
zE)qNqmREWHQ9TAX;JL~)Cx<C+{VxCdLWPsU>!&^DA_7Hn6~kKn984!ug!14q65`V5
zpB(pwjbAhurw$j#wZ_k4U{NRoE=sgpth}G?k?~BhC*iXwVVtq6G7L8nr8Cy9(NdrF
zhMAmtM;iwtLUC}i$!#ZS)AHP1cV~Zid1{zeb`UwKQ`XYhIY>%lv5$jX>%Qj{UHaLv
ze8e~S^Z-#ilEeyS5#l5tf!ZeO-iyUnZ;6cRE9|AuaWUwuj|e0_7=`j|+1wfLf&mYV
z5FLWJqKtkKq~OQdha!#tN84LR)s<}9!??Rca0u@1?(V_eo!}7M9fG?BcXxLS9^BnY
zu;AZG`u4B8Z+E}j_r3A`0|UmXQ?+Z?-ea!0=9-HDO*Pmjn1piO`oxUknvn#%FXsYt
zioYS!JsE9QRr9M;gg3rm7RD|RXd^MSJ)&`d0Pg4@AvPQj%8j!L_qY`wBi>mfvv#F^
zfg895*;TA1*cyC+_Fi^nBL%n_v~~F|F|LXrWkM{Jl!Wxr&Ctn}?I=FWyi2q-)QP1R
zBrWCEeFPd5><Q=G#J7Pw{`;ZAs)1JRN@>KN$F#N)Y6i9YH;Hn{@cbTQm&jntLfRKf
zZqOj0l-C3KNd2lFH(&YJo;a*a@xUA!K7T1Igi4b|DYU|^msXvaO+8N}5{+A=as`#P
zZ8Pf66;>J+P5@?Dkm>QTQh_&;vcu*p)IW5uSqz|@%GMtrG6*lbp1g8R)_XlA8v}}A
z?S_vv@B<wp@yR&tuV{iow5IXLF5#u}#V?-YK+h?-?%R7r%*=tmREt^yLS{7%C&5J+
z8+|tUE}+ctwnkj-3uq7)IdxOQdRCTK2iNmV)Cy4YkMC3@A<whCazuLHVe&mTo`Jnp
zJZTOT8;@MNF`<*2|M9^)LRTPpH10YWjB<S0gWhhE*9qHKSvVP(3Aa_kC%MhhB8MG{
zvL4x}9R4~U^rVwC#-QNtZ5F+=-b@PzmZ+!j7qgocwgsHhUWTI!&%2`Ix={~18*T%H
z^n=^alZqa?ElYH;t07i(_4bwJa0s^=I@al(PD%Z1vU)!8>@5Snm{aF)Qf7eFVdY_}
zhM)BycX+Ef;YK%0(aTlY^007P*W$4_+<Zx|`np4$xpYuvF=PF49&yf8GPQi)!WI*C
zpU8$?6k=0LS%jWeg(H#})OarXg0}a-t%GUDrK*G?zTm4((k}lP<jxnnz^6-^=HT&R
zhjN9!#w;uvf>8aPx!~hHgl)Bu2sh`#yJuK=FRVU)vIia1_Gv`W8`OElybaakD&&xf
zhJb*|W!^H8y1LHFrtuTrg2%<xIoZYaM!VRkU>H#+^@|m~;1D`AH80ZT{M;!}4HmhJ
z6{TZ(3`jW_ZaqzpY=RN=iX^|(q#iT~Ed@2nB#@A-tdpU+x;+lC-8Ay>Tt6C4b)mES
zERYZ0?>3PvnlP3Tg|9wU`?ch2;_?v;6@bJGHVE+_4@pHTQ5Oe7?Ut)j*E*GGE$V<9
zp)seyZ7y$305K?BIxG()DcSXK*jM6-IL#x_VQnxXLoguPc&2wra(z}D4snhFoEogY
z{A0YBwMaj|$WRBY9jY}&W`uNWNE7W2xryqx`Nh)y-iww5)sx3fUT``YW=Ph($aimB
zfEBo{<36ZM%V0yG@6`7v^d#h~Nk6^4Z~459E~2rQe9poij16z0!`=QV$nSkdRD6=0
zPZBrac}Z}dznyqba1F{zF=~<F>9)MGDjSXLmnVTRW(m0UP(ltje9N}}-2!w=9B>Ya
z=fjpEvW)@6mFueLG3czvDJ+v}MTjHq-cVh<)6UGsK<%u;41!W(eOOEaVrO^j#2>sv
z(5X!go*Vj;Lgu((`hD*?Y;P`xFUk?YFgOCQ6pumu^yJLT;C0QG=J#xr5^l=eh<(8W
zd-wSSF!Ew@B^KYINxDK~56o~0*UQ|PK=CSkbl}<p>?1s<edS8nvQnEFd|3LVm1zmt
zwIqAY<4UWfkf@?aaKiv#FAsssv{BBdF;4twaEoYdDQ_@iLCIHo5Z_${P~z8-F6gnQ
zxXLcvK9{#VKaFR|y=bJ(#9`h#((qBn_tvE{f$gVJ5(h@xE}q~C8_&J`IJ-WQ*{C^J
z1K8=t7FqrCC;c}}#z}K>kb#9^MeT5@1v}RG(*`%-Ul`O!9gbnaV5-L}=ofrC&Zy{!
zAk^Z$BF3;x*_QemX484ErC+TxZtDu>^t2JutsvcX8ay$QKg7MJm!Eo{I(2wNj@^u2
zEmn5*q_ccNhFPkHsPBR!5Z5;luU-xyVTd*0L=e|5;HDZkh(99?D{Cs>o?m*ul|`GZ
zi+6(*i<dR5t0z2{ZEu4NUIOXa=16QH#PA6|6H33MAfX=bJ*m1tsE7Bm;Hj$!2ON-E
zNwW8*B`G?h92QK?gbTph<DE$*JD8zP4`)z*J;MQp^oc4*`tr48u@k8nT(wna%_!y`
zTLTb-9bar1!46Br1SBfq7ruf6kG6T9>?)dSG1<Ci^J~L*q|nF+=~dx;KAN-wyHD2X
z9&%;+@A;>ez7GM@zaI5KUzc%J`&x%ONJ~*^-GOwxK$h{EkpD#>2*~687H#}>Aovs3
z|1l5*_{07m1HoSyY8HT15#Wme<+}gUGXuKs{_^)P@=F##&yXK~|G`@MXXCwJW1Rn6
zzzEPw{}AC40{XrHY<U4)b^&T(fV%mIu`<VB%b9--7XhFDQ@Hr!TK>1|`lDtT&}aqV
zrUXzk|ER_Vh?oK1ylj9*A3u7qFmnF2OYncx4F4gn{BO7I-<Ai6uYYVCpf4goBMjIu
zKzz*zuv_L}1C)ca{bjEEKQHh1iHQGpc|XSfCIx3<1?1cT`D7**Kn@;|djMp_0p`j-
z=m5ZOvi~Q?;6LT!0ebiU{eAu8%9sI!1i(xHU`R&C4&W!40d?~~Iz2Km0``%U=`T0#
z*8}$#B7vU|$N%=g{c&Lcivz$N1gO6OR%;vp83PMop+A_EAJys%Z2$ere*Y-^U`~F%
zW`1=@1CTNR6EF_ICj5m00bqvx+iw49=r2hnRzUdyGvFQcC;H>(d+FC5`J1HDKW*^+
z2ixTTQ4-DuuxR+@{RNPV|F;719|!Zd3;q{t!py?>j~m_0>qtAUvY~XIyc>5=K__yH
zA2%p799rf`l*N@^s#qq4^bnC8|13<z*_f!;1&<O41RsVBZQZzUsK~6~n*VqLnFbVz
z=?dHa{c)-6q>i3mvXv0ago7mFWJNTINNSN-a;xZ5#7X)9y|f<%SC>AM#>@Aqgf^QB
z@3^s*HO{`V(#Ji^rWbYFi&g{vj<QLgwwLqMiJK(?GcffnaQD1%#iW7=KxKOJ#^p9d
z=cBD3azG4U0|`6qPtN4Z!@>D>7W>OK{tOC=q+ol!eXtT4T!k=B2C-C73`FEU1KBjG
zW<|lw7n~lDI5>03e&#P06^r`>Uk2D!-B;SzRaI-0O1V{FO`wUD2IB(W2SXiH67DrB
zT6eX}tkz%^=zFfhKcK^|M)})SD0aur774021%%oc%EuDc5(I@bL-&I>rNjx#GJvEZ
z)zNN$GeTqO2`vjN3%P<#ql-|zrzUGx;4rmbTE#r}1H=g<wf<(cPoP=111>j<UNvoJ
zV(Sz}_%L)pMkz(iVEiH^vfbRZ8l-R^LCmq<$P{kX0fTl7f2!-TbVVRLe9k>bdx9nE
z@|y1%1b6yKnEF=N*d$c)ON{yL{Ow5j8ymiAw#QzhCZM1j7ZSL)(C~FyK=4LQBP<Yk
zp*3$L1^F`#M-=BTPPPTE+-JR%cFP{mORO~^%es5Mu@N@zgqsHO+(Iivnh?ZQ=g1Ik
zk#9+6$oOGzBZ*YVe0}h4qC0-&>7=I{tP?@QmPx7(;%J~@R7NF19{nO>p47ZnwXma~
zMR!MpqzWyXzo{UUDLzx5{@te2(nVF{;6=)W=@EEmBCV(TEQ3--lsk-V;4sYm(fP|(
z2j0@RT)g^6JQPKTotlNGrQ3Ze22w_hgf~LwSOxNhTeBV@mT{kEADTpUlJ8v@0)%Dz
z86FADE|G4o)%+X4k|k3T0lP&lTevl{sbrYI@by)GizuxIPUM+1y2xHmVN^0>3Yr?$
zpP4(T>)t54YOIjPQvN!UNGQ7p4_j9U(H_0pP2Pk%*1#?d7CTllzm=$vSQNT|ZuaDk
zzw0aMk$XHIG$-4}7U}a*C(y1MPMkh`x49J(xH@lP=Oht-Qw#wY4*sHC(ks&Y>gHIH
zz=6aOmJcZg3`GeYK+M(FeUwoF*y%@m3mba(0?c-eBsn$M2Pg7WM+e8*;vLu4rNr;R
z1z<z9jle8JzvWlnsVNAts3P1roWa@yn>NPw<_aLd*azkvjoog2hu8<YIx5#zNq@f*
z{Ai_KsB-V3>Sm}7MhT~F!`Q6`TELo!@^(#v?ju9H5Ro9tz4bSvuz1Ash|9y)Lpz_I
zJoBV&ah@Tyo=3yXJGvfg7ISi@6hMC?cU+UwP)ICH?0z(CHTjUK!$jW9VEL75kO7&6
zS?4;>Ll*b0mD4$T77f0vq7O%50z}HSEYv{rM931|9%0oiQcx%s;T%bsN_?lcE9sbE
z*%Tnx(?fCHZ@lIt!bB4eckFpH51Ey(`Vh7bH!Ex_P6?cr-W~CDbR2<9;lOlce*4JQ
zbEj7|CcAXV<C6~~YTD_CUN)3SCcircZyd}d)a$PTILPsUZDsQl<(G5<Utc{~@vc$-
zB8LxTRIW0h^T9wZK4w|pW>)l_nySOXv-gfwcc6JLJA_I-)1JW~RE|L;V`}gu7RWX6
zHMM5L7Y!?$<YIvX%nVrN&lqd1^|YRC@|stt3AvUxbM+DK6b@eZZv}vp@Z6|+zroUs
zREH{W!_Xn+hb8K%WM17%yfTM_k@YMRe@BIjL}QV2Ke6Y{1!I=KB6%@7**t5s9@TCF
zFC|Cfs4$wRlSqOgBU?R65}F-goNU`V$|E4oma>oPZe{lkuIV+#;iK+;$k5lhOS618
zMA@kVpTRej+!)2Tf6}3Av+8o@{p?a$7dV6jjB^MNER7Xrl0pPH?81Oo12Y(K2kC>2
z-2#?X-1!(3uw%8#h=aOvvKjQ~42P_aA7y`iXUU_&YxPDXxj#juEO9mGBPwv@E>BcE
zly&8V+ioW3JaX}pj9{Mv6I&yqtLmq3PnF?3_Kr}~R6HlsqGxC0vY1{-29h|9laWPm
zjL2r$*z%~Pl<XAGg_jA`idc6P&K#bKrDBB%qJkxT^C2_t%6MWWeP}H0R$96w9A|eB
z;{%7rJ&<2TMbz(7#?ao<Ac&wY+g@2WG>!0gm5s`FzSwrTUlOu{c!H{=cM%jrKZ#<H
zXK-oD7gc5zeMfGpQN4_La%@?$<MR2ggRS@O{VfI@cw}d>GHphw3g3-^jvE(C{9C^M
z)=i1c>MQ{$`20t}5gC0~wSuzTb*9<0RcsZu)?B)A-spposM}S3J3rLEbmx_tKTV~-
zoOFwgaKhpU0s4_6)ATcEAiPEIJo{sQ4;6$fYK8Ufi#a$6cCtfW;>1m0-aOAlj9X+y
z0Lj*|sU0L0ddoKwMa$YxA%sOBlE{rU9D%`#4}d#aFe`Ib!i`_>Wv_9#;$!LRI@$v~
z5Kn6DiFr4Sg0BfAT`wDw>w)IBHQ~MpVOnZOn~mA7SEsvznf;)V8XU09c<tNEAsVa^
zW_)tfC2e^M(}~%q(8ootW~L7DCVH1@tfT7O$HZzj%5(fl<$ce59-TwrVIy_$;2T$U
zGUOqL2X0&=(gIuF)n5Pe=2;N~yNtp83s)Sh)KQw~*+p&nAq!JW?|utwJZ-A6A=i{6
z5Jdk>L!5mEN<UTK$ZZCiL<ZLL-PEymkNFO8Y}Yg_;%m8`;Z0{8){oyfQU|v@gyaBc
zyiacDu-jc+E!hVHV0pb5v^=IuD3k1rbV3X*!=O|`C+BYfEtql0;P~#0ucKeX@RI$r
zA?&6r1{S4c#$a;aO_d`C$}!Cq3;#AvBLdw@)L@y}^Y&*>AdI!}A$KFr0Xwn?USId6
zbA0G;Zv^9V;=V-!$(Y(^`OP#gEr@rJ2cr#oS~FJir0>jk(-<kT4ROnCyj<R#oHV^&
zpT=Z!ZXr)`wP!+YSZ+jpmA~X!IH1fzfGTmsQeA5{3#8*A%;Jd>2C7BF5Hcv*E1JFR
z3F*sth(Fi;-rX%i=CEnR{QNTH)8T&6^SON~T7SrN^*s4r>Wq6w+N)da#vw~isk%T%
zH=XF#7hh_^OVOjzJ}{+29AOWN+`!YKaQ2cYq%hRs8^~_AK+g`)(Ej~Zo2%~Tc={=n
zz4f5Wqdd9P8;Z%MCl^nTu0z+@FNkM_<!sCK5}E`n<OGHu0r<f)-78>NaGP<qsQSnL
zV2<hC!d@+O1ko(4hW6}>4NH&Ga#*Qx9O=7wmE`q&=|%U*;<e896|Jb}f*E{v3&#}3
zZC9G?ixD=;M-|=AbVDyM&@l%x7|%dAECCc^eT@TvS9=9RX7CV9Yy^_L^OleDzAN{v
z&V!TQ?NDKQX?q(rl@6<518IFcUs*X%F17J~5-<b&%BYBPqzHx?HL5J9R6!+w)8oYB
zwr$tv$F?_>IH9IHiZtPbUTBV_oDC}qrj~|alZ|8<u6g=7h;nS_O=1g=^HMq>U#A^Y
zJX#||h67l7jn#Vyg!+m-nc%Zq=l9UZae)=SH(k&ly))W`?wmqmpq6eSEsqxi_hXsd
z5rI%oH58In+!2G2R|q8vfQD&ATAvY%`t)KXMNemgTUn&MJU;gn`VR1vXzp3X0}MMF
z<{yZQ+Wkrgx%T>#({VqZn{`ANdxeY_MtFyMb!QVAU=9aPF{q`;^qGMpct|??9kmlY
zHnu4XuRYFYr@7;|eAuGW&r9k|?eg;C(Nzqek$7j(gH@fYI=q^+Vh9?`qt@Ra+tW`i
z9U5O|uuo|S)|^DFtJa$k#TBwyrB5d97Rr%9Oxaf%0G(@)z?&K@zjQ1IIru<R!qtO!
zU9l`G3acjol@ec|N!RDn9Un=8TjWio5soD%`}_^#5?s)L2dRH^Z3sA|rO8&21U%ZY
zd~VBw%Xy4v0I|(K-=AHFOPl|8q44^y&}L4!MfGu2oL=#$?yPuB2ICRw47wQ<`ouqE
ziUH!s^#QAQY^$&*XZyESDy}Zht?`^fOB2=ACD2GNs<{%2dJdT+G?Oyh!&_|6+}Tyl
z-)eZd=BtxZNZTI>LFYk(-%SSE+A7LlhZE&N>orEcT!NDngw86uG-oM%PXipjiyCK!
zDMgnrKpvhh+iO`9(YBLWe15=?TNKiD4CJU-(AWpx-yq|r#1%RAzk2iTn{<B-!p9Ki
z+c9YL^+rSm+2T0hz$+x*)xSdH-_~zLh~`XjmWpUAs@L6AjW58zhGIM8tDb%>3+R4g
z_Q@>CP}hOjB$o6oVQP@BZlD$g+kHFA^o-zEiVURsE}W?ZXCcRy6t%HgpW@68?%AN|
zn0|2n*54%`xafI8^m6H35YzC|qGT;W!9sCdy=_U5#I(&ScoC%mnBxR;kKpQk#b+pI
zC;l}|fE2s)yj7zDO0#sHs|w!8dRArO@=Y2bIB^khxUvlGXtmY|_cn)&UMGH@Jb2w1
z6HTj!^^LeKXtx!f?Z4LoTJ!;~vrx<YGh4<A=(_b=to92@@o%*Yzl3vtV9Qt;e#F}U
zymsNwvWcHD@eh{lhi>uT*swpu%l|EJ@^7>bJD_#}z;>_$_^%)SgPZ`C0`T?hEI(L^
z|1InEd#UCBZFww=?10jK07n5RtzZBI<Sfhp`*#4F^22(ZiHY+s*^~c7I{|ub{qBwe
z=!Rcz@judd{BmFa1KaRBZ7RLkAN8!iS6=+VkNgix*Z)76hu?F*zv?v@0oDiqP_I#;
zt`P%Z9=x;lh<4xd3Z!9(X1bsdp4^<;>JQ2BEPC1dgNqx=TUA&&jRWrv+2MU^ohMld
zAf?}y#VG51D5cnFyZvxMVCK`t<LN@AEdM}!#eAf5&Vvp{tmK#cDHUqFyN#wDF+JIH
zEvk@zqm*Mw1Z2$ZO4y++eHCrh)1mTdVs6*SosQimzwCW~R>;)`oez(vv%8BU*_Omc
zye<y3o{7BxMJ>(Ew};a{yUusri!oXBXijU><IAuGXx>Od9pBcopFivqL}d048Mevx
zNqkxiP<pj;?S!APXFi92M}}m>u`vp-`&MGU(7a|f#cwFxWp{K+Fx6T{_Z+`joV{_z
z#b}AGk9}&iR~DEXXVn1`ombW6xuTM+se}*UYfIjv52K(5rGvS+F+&)$=eeexJaI#F
zt&hpe2j&D5zBsA?CODqFM<Pg5fVQDtVqa%f7XG<8Kgz)xOP2lekOt@WZr_y3Q<;Z6
zf>YH~2|DH229)MZLy6|#!=hMcm;2Hi-n?NAxZ_^zNxGS!I77Wp$Z7E|?qg}YVbWH?
z_UeRd{kO+HWCUx2;O{Kibb2uIkhLsawQH9gcEbxBn;Lb}A>`Ry$gLE)7n0N*M)qyh
zSLYL*8f1(ak9`honDxMN@YSh7_%(Fx-+)gSdRI?i*4=bZ6AvgKD1N1lxQefwxNHB$
zO?TSeHT`*`OC8F&w38N#7SwK$O2O^pSLB}3>2)HoYklCa0x20DV*TY<{wib2GypN`
zfvXFK#=*UE{cb;(+Dx6<2i{nN(kRt0%?tTw`yhyYGlIY;dTUO44il@%eH=8`mSHD_
zr~%j&wmvr`ul-o7fx!2M@F4;q#1d%KObz8T>coTxl#w^<ahF&%G5c?)2)KoD@V=98
zG@00*RJ<j(PXj*uR>~ApV&p1=*jl>k5{-x}NW%#oqvgPdK1l4>#xwG0yuZJPo(HPh
zrb?Nnq_JLeYvprOz|<icW*WPDHm8a~?xsk?eO<V24GT&w^o{TI^9sbuf^wB)&H?!_
zL|n8;e2oHQEpk%QzJdGuT61^D%2rrle7cpcbUc(JVC3vT0SsUekR~iAMP2S>3OCeF
zre&MPv6?Z8M%>R9+$jdKH!WJW1q86Eqi;`M;yhmrOHpcW0wbjKgf64Z+AdP{SAeIB
z^RB1i^ZOqt5z<UkC_W0<Ra}gDl)C4TLr)PPjUlc|rsUiNbBD_0;4ae|O-R;tYqI8k
zQ!jDLw@49aLZnOavz6x<U3JCXOgHcwjP--*nGzF!ni}wEn%Z^3Dm{r9ByM=M(CTo%
z`vz?)^+{seX-!Iy6!8fe{0q;bw5yf&LxAb0NU;6!!CtZTFKa9xNshzcA*cnQ(dM4r
z1}5&^WGTm0pj%qd<VTnsR&#9})uuh5x}-2;=;$z?B<^(X*VNXO8$Ku|e|c#MriSHP
zueoNV);=dshM)MP0!kUh%;LoBh<2%}dRab7bxjhZvM1g<=<(rEs6BlTMl|Ck_0uNv
z_wUzvmGtRf8(LWBiDTUE9=fVFUd)`&^w4Qz&MuWtSZqgx@XT6cW+K~W;`WO9HLFM+
z6;npeAWU@9^8$g!J|mK?f4u_RfqTz#AuFQ*n}nXtnd?h|l~Zni#ehCzLKLU4iG_la
z_wG(-K8fT4s4C;`kRg-r-6j)grv+&oR=XQin8M3!w_oelQ9`!ig>>`GBB=I|ANRVx
zA;az7v!*?SgTF+e!9LvA+mO4ebsu65GmY&hpDJ2TJq>!BW{sRBt&>*pyNM+*WE=SL
zmOZX}@`Pe#@fG395zaigW_NoPK7nSViA`@WD{@dKO!2vR69?+kkC^q$uA7rC@u?<2
zhtyJXlnm117`>gqb(C$URd#3N)ZPWt1-a01g>-LHB;?DdAQ5ud(W1Y;&3PY+ZNHD8
zOCuvn0gKrN9r8|B9b7z40UR>kDBrY9NT>F`M%mE;SJ&g*f5i8t1i-T{CQs^fuA|t-
z6-UO3Jb8gar3p||@SfE-ZLglKjM9iSwg$t?%Bxg<0;98v<W?O4D*2+VsP(GL=FW<4
ztwmYEn6ph6a+CR4+ynK7P(m_Jq%CmY)q1D!Z7o%2HIz$r_*^iA;`ewUwMm9R!CC8t
zT?p@C#iRxnHuNnb>HCcQ?;ut#+Y1y~h#;;ys{3PsV67wIIqhswN0NF|K;!ygUzPnK
zoPb)TF5kFWC)}r5Ult?Rq!BdRMX!2Mru(~Z;r3Ut<f0*L8mE_@E-7%jv5{}ZntpO<
z3?y4@X}Z{=Z1TZk^7WBKgm#;_z07E@<HzmxCTP)m2U;P;);GtfQSC6;<Kh~L>22Ph
zP-wuPOU&+q(KTYxbw43v<7RXfIgQWU5~Zycmmg9Eqz|krl?z=;*eXl>fH6}yxLE9v
z!?yXf1)K*uI%FiL#+VJNI4Zgm5g?~v_6>=*+Tnwj_0CNnVM@~zgoW5vOi-t#vtQ_t
zMN>P>D(+1vEW=n2@(1YhEwV5|vrt@oAd?*&OTu=smq@y~hh<$Q3C=$6jc9?)=VO;R
zdLd3bcIpJhq|M0|dkZvgQ<CNx>-{eb+$~iw$`4h+Z=V(VD0#^PrUu4A5``B7AwpzR
zQs{fhBFDkdmP6|v>SaY-+-iFb=65!t4bX%OQ3X+up!B?MyYf+T!w<TmubUj)^|1s$
zhD`b@gsUv&bd>JOi1grAXvp)TBVOfmp&7j2o_vS;d=3M2o<six96QoyMAj6LvU-VB
zNW8eSP2cog9E-_bZ~fawr7vYL&z*Vjvns}ixGl6f<&vy^dqpf-Y+Jr8_4(|Mq#K|k
ztWQWqq4}iG*V*0QVj9K;MwoozU|gWiMhSJwfk~eT2yN~y{|b=*UZVRqfcyu_`8PoR
z6G#8!V&Z=R^4}`10FK=Mjv9X^#Q#af6)PaC_FD|`6LJ22?7u3mm;hGaf6>N=^=C7h
zUm@q;3}#~m;9=H3*XRC0p8GdrnE^>TPKG~c)>r{q=-*cTH<`6Rz}r6n-{0lj{*jcJ
z6;O-y+jPG|Q)WPu=YI%IGc_a~cN@_GNwLd>JndN|V0&k!5~U1mPwl;L#EAzCE(MFE
zniGN2h?tU@df83BTIo2{juwXQK<h7jIlKowUo-5gUL1U!ujB>+{xI8bXIl6<B#9H|
z<k0%qRX%&2e{cfbia5zcAh|mS)0<?~aNxka0YxnQ4!!2Oz`#7Gp~u1+gIno3V^i7S
zq*eIgF(pZOc_lG-g#cdu!|a^SMxiL+gIyawJ~y{7$FqrG_`KquY3_+74av(Y`CiUB
zImf*&+=Tk3P!-jp=?g9s5YJ{>05-#;b?<e(CC@pWP~rlqw8PIJiN!>77AuZI8eh=W
zA?QHSn?-f5yWIVTI@%mnQ#)yE36#Ejy&QWKeL85_=n#a?=*OHv%*<3FhAPKrPWXt~
zL`M5M?=z4&@Ex@3_EcyBwg|Ns%iNaTLbt}f<#i^p?`3SV{rQ`4Q?h^Z)1C+BW6tSA
z-x_^7i<YN<zThLvwWvws7hj7#J%{am9W**4Lv|eHpv|{3PZcGy=}G$6-L8(xuEFW(
zX;jR3w;)jP=%DmG^EdFWj_3U7&o#xV)j$Zd%avDa``vmlNY=(C%g*Ck?c1`V(18L_
zwMbg8B65p5jik`%feK70U49)TlV0lKGnpv|(W>>co&L|DQwj75;ci<r#+xOQ(cJ;&
zRqiSup*W<8H@hQ<hZSJ4!ARb@7S%tO!_|cPB1^-EQGP|0;Ok~7OO81kJlySDz^!s;
zO4mM1NFt6-OysE_&n#0MNN*P<1~g}Nmp=J0Ypf)JF29{sCb?ZfOF~;uUqmY&@-5Lt
ze$E0vVs%$vdIM3=;|NElZ4EjKHno)=U)H@dnzfYbBhC%CW{OLyY5;Fa#3^u&$g0Jy
z=6fh;zFk?%u))~C%a70^nw%<&YWVfkrc=J*j`(kc3B4HFH@m$y1#^J19)TZ&OSBvr
zXlHx4aD>L<J`&DXI^np12pOm-Bdk!}-NRLO6etrWc@Uqc<xY%K02R{)`|j_23$9Or
zN4M;q^MZL;TN=LP*%JP$P1%dVUm-qHz0($w3;tOvYeY61!-TvL7j5FjIpSuXd@Zq%
zef0!VMN4FmmO-Hbvw5FW6sM44^nEvorF5AYGbCH^XMls?*S1jm<JTutJB~+ShuANx
zi}6A<XSvwN8R=}t;@C-CIr)N98$3tJUvx)CwLQXbyKF=yXr2}cC@ENz_UP!BD=K;^
zS6^Oc)MgFX)hyN~iZB?)M?z1Xmfs_lOl9t77%v_GQQjj(g+@>af`N1oevT2LA^!vx
z?a$yF`%&Z*m{=Q_G_wEO7QoMkF=+ux96x|YHG%FPLb_PskfUZ@h5gh;>i#?oZplff
zhJG|poPgsB4g%AAD8t~}n*BDB6GEDQx2bN;yG6Ssr6uJ?eM%Z1R-4k6LrppNxaY!{
zO+a$6o;o{P!5d$;?b|2&Ey+%}tsJH|qY0edz&wauNyLU1Uh7K<!-71<&t%Tc`q7W`
zdCS_QCtyhaY3~Ql3Y&cE0+mI3yPJ7z&9(Z>MA@t`(ZOmbNTft0-QpBHDMmV$wk52P
zzboIndHWHS6awWb-172<FuAmX5a)Iyv2yONGfbU(N-q%1W@(x0m(xgT8tB8JNO6B&
z29FTx2c`7Mp+m<GjdQ$X;Cym|vFKhPj-;c4IUu-1?90_}9`X}dkA#k!WqzXxI+KqZ
z!WXO?8dZ>MVfggz065&JV>NbM$jb_tN_ytG=JRRKS}fi@L)#GPycm^8_Cj1#IaVDo
zHLf^lFC>eXlucM92bTNC$Aqlhjnh~7fu{*FUnw5sVQ5fqu4gyTyIb2a#t%TXiVU8X
z8sCVtNjK4kkJeb`&dZZ}s!1xVwLcer%BpZSmM!o??sX`W$O6G)#_3NGZ5+uU-vnYo
zLC}G47?Q3=N?|!bc#E4ek!~@Tzz_Awk<K9r<HGvUU>R2jVunyEpy+&zd2)`@4OADz
zEpdWE%hU@@nr6(CHNgvbIzcg56Vs*57+k?&V#>i~!9%4ri-a^2g=MpmoyS#7iv`p*
z`Y4~ac9(v+>3!;Lw?ZyX6B`LGr8L*e2!6yJvg2`Q;p43vqYG({Y{hh_{Va5fGvyZ+
z0$?sr`&o0?ZK7RsfB9f4Jb?lXcC`VYk+Qz8FG3T-D=MqCs_-Px<G#8A4sf8JR5Vw;
z75#8`zUuOF{FsB;%fTqUrn(7)>-A&}xO=97PQcoi3QwwJRI^9R7!oZ&d_KO!eo*t-
zB<Y(C!=}y}p5U#N$K|;19j_pDLU&*p$o;9Kwbu&rrk)wr4?Y<$7_a%VKYn8>pXRE%
zlBZ!W%%f^{8Rv`#Qe|#+;1bd$_Ta)uaJD#zmdqVhk|X}$b+i`Qh0TQOqW5_Jy~7P$
z=z9W`yvC3Z&9Zm5M|TYWtwtUvvMa4A`Z&f_#FR)c=b2Y#x|w%otnVn2=u?+G;bfKf
zYD&x=WEti<J4WD&JRWM%asZ93!>%Vw8dze{t6;$lo?!G>YGg$JPmA^FZk*zhet0NV
z^u|6pBu%knCqWrjUY}VKnwAJWc<#c^gLe|ozu}Pv-|mjCC)tO^3SN^ZJ3_B-%1@QN
zm@->`x;(gpQKPP%ywP4>jsi{KSWZxGfzKEDp4$nP4`G`mI??$?y}SUF6;%9`#+O7f
z3M-CG@!coSltaP`+vDK?q%ZLh$9j6S+Bp>BuB$Z1vr{qo!?S(9ersQk%E41SE&cjz
zlR~Hc0_!ui{dwskD{44ON*q0JzAoqkx#^c<=FR#c+Gr>UF2S2!w;_a2c7+N;z&UNM
z6WH8nkQOO8z7HST?N7uFwQOcsk`mH|Pk3@_AJM@Lqe#fUQSMe@>@ccIVBw<y0xHnr
zN(Y5d-<m2856PFUN&=ROF3~Yxc7Y&Vh9CnqPZv9C{I24(pSOJhgh9%s(Ff814eQ;F
z)&nd{5fckv|K1;#(^ZTmc6iSQ$!xGm#5x<A37+=ubRN%+MAv*n`qBTHpeI|WpCR?G
z&<(34hsAoJnc`wiGcIz-z4&lJ)tX8RY;21=vxqVIYbJop4#M+tE6E#s)inMDWSgUD
z^g*@SQdZ4seO>Pvk&&WS=r3?IK!o-i-ui1C{S%V?5l1uruZZp6;b?%v%5T&DfTTay
z@sHS7#y@cJe;WHsog525k;n-!@BDY67%PBp{U0X&WiSA40tU1E#o*ru9e*?PkCv89
zfUEiQ%s=+*R|NkzK>82Z`46a?5g;^U`j=>yUhH>7@jrvr0CC!Hlm9<h4fxJ~2&+4=
zbsTpZzpi!pcZ9&rbM}BiU6R;v?X-=sPRPVdPtNj@rXtV^V!R2T3;SgK{CH}ZoLz{z
zka-j#0MxcP;7yO_cBi}fd~>{ZHh%+|>c)g*5i51!au6g1jY>r?r<6;)w`SWx@O<}p
zy0|!Ni6>TL2NoUYQ^#wzH)v~d(?<pO)VRuWFZH0BitZjjojgC;v;k<d;CXW|1v1hd
zuWKlRR45=20cc7xv2{F@4QdDLx#8glWpLCZh|fb?0F@AY(fjCh)2G^PJt^)QMfy!C
zPK>HoOQJ-CVhdwA6pTK&sku|PB8tgdZQ-ov3ya0AM&B6A(`DS?@j1UspOzgT!Nydw
z*Z4`{pfvCvs{&)N$@SgXV`+K1a#HfdkaB|!_io?b^H4Sj>J(h(ZZ_F>qAef3@MSjn
z&sYpy-2tW{+CHKAHMd`=FsKj=BK3_w+X@I999}Hvb<>}*+bCD4Y-fKf&*6Pc!HW(m
zkkmz^Wt!gI<0U!0N<*yCn9ahfUPeY734|<&lu)$zP;p_$=k;t6rhyl(K7hALMtwBb
zHjm?Ggwj?@+tlALj9|?mQ+M#34N~G~=jwqYBZ^!=9Bvp}ive@ULFJ>n&4J9th)2Ll
zK}a2b!^TgG_a+E$Q!k6VwCj;IqT!scei1iDIxm)@j^zvqyeG%Z64}y8xLpBAKykaG
z0Yh=v)F@D+q5ux$)y`zySLiBDo9SCu34*FJo+o<-DmW-utwF|=B*7GEH=Q8?9xwb;
zE7fOXkP?xLZs2y3>6ev(vNuFV_b45GncY@c4Al6s_QCwgX~n(}8D4BW;+O+*a|q?C
zLuC9MJEDiB=IQNJ77$`b0$6Gf;;sq22dY|k$vH+n^;4)lh{^^k9IS~XOIsG=o$res
zDix48PBof`Su1?yr$Yl(i6O}xob^%6hIgUVIFo{fcQ?TzJsY*4+Heva18^*%O2zqC
zP#t?h@7y)=&RYeTTa)wgiq450wij+4t%#y$iG$NaI%t#8GAwgv%`Z_yFJYHOrSUUL
z3XRw|6<s0>B0}@3xbc^V+J_~VFg4$Q^o>(M_1PgN77=2xYEl%U@&{99=<Vw)O?{Nx
zE}s)H$CIV!FBTjdo}QZd-jyIXg!p!tLn_e+meX23eL6=hBl0W!@VQeXT+&w!Y)}`n
zQy!>yQ(1;xAh#BuT|`lfBt<UHsKnZO(HX*OFi2XV#Ji1mVHmJmKy1&<PYo2?mI^KE
z{8jv{ID_ERay&yib4J==qqN$_mh;>r>gp%KN3`7|F9yBTCM6tuf!nleB_S)xo&tcF
zFdw!^?A!=aCMS~&Z2H>TEf9x-pOK5)mgM@O8cPu5%8S|yi-U`i60c;}(RichVv<U-
znB_jY2Av(c;`+@tqYTAqHBS{z?qqP3)i&o$i6Y`b$J-xvI4d!Y(o?&7hQIjXKPLJW
zQ{F5-2J73Iel_o*orOY?jPb7J_AG@)6^?$xO}40T!qqjlhL=f+kb_ceN1@12{P-D1
zx{WRl^jcqop~cQ-0*t$loSxM$RHoK=bvZKf4H?PKl*h1&CgNp>7h!W9qPDfE!cG9(
zV0?I|hcDkg+?)$@Y{^%8_3q(BAqD$)0@KCMHmb{&>jvpT7x269LkVZ-!#lr^nwRZ=
zs6UEmT<2rVBP6}&Ruz{H(kKp^?jq>!VQ?nsajh(@8l~4E;;(8qGhv_8R;cM?R45Bo
z56P?%98rH^rq-0+qgKK{)+D!n;8`mF?s_#e;sJ%R?XyFLFfK47dabzjgmV4ZB#nM2
z&0wb+rLo$9(~y~4`Wk#Z@haegjfuMp`_2gD!+n+ft2&q_7b{FI%^Ggc0>bUQHm|pj
zwQkf0ci?Z_){Mt*%X(r>n(ALYi{PSGPAQ&t<HZ=Hi+DHYom~(+T9R<})|a?840V-s
z_yY;hP|kC^oKwXWli7l&y|tR?`e_EBEQsUUbNXk+K9hjWL5>ReT7_mvcZsidD@953
z8<RYOyPtB9PhT1@fZvQ{7W)#QyM&>@9Lg&V7I%Jup6g3D9cH?c77GgvRtbk_bdwIs
z#zR}D+x}kVbZV#dZt28Wi1VCcXe^-sE-2a)STBpj#<vHdxXd6{O&))A3}OXad$$w;
zWy4u_1~NiR89o$h4wFNfjItC7r7EG03-06%Ua`fqKWvJKbGK$skm6f7=H1-J_5ofA
zK~Yv!-V-RMSJ!iB5J8Gm)ygC@m~&wLz|X!KXO_56=M#O5b9h1Q{m2|;XNC@(x6r3~
z#zN@HOdd(c4g-?o_AR~yS7hKx+VLPHcknDrnsU<=MqH&C1gwto>c@1U8x#X;`n*?Z
z1a7=5OhvY|DpL>T-wVJxPj2lu{d06>UX=u2O&<;-B%QRFdN~{}oO<-JrCCJ@jS0=n
zK5wta4@DEAD6YQ4n%m&nQ~Tt=#p!lbjwIxFyn%v$dv7YgJ-jsDhh>Xbpd3MQPYIVd
zA01JSLQKrhVo<`jewOvA)Kq@fXkTS6@s`xNa{uzMvSns&p}AYFntg6oXf(hV$hT4-
zCX(p3SkkR>7$nkZDk4IcZsfxadQ^`V4*lf0JuTmoriuz$guUC$J&M3Yaa~Z^TT0ah
z1_Z^2G(!@)T3X>D`-`menQq>l>HTKCzJOwvBBaTLqK-D|L(T?$l%0cdaxe2?FY{1}
zvzp1WfVl;l{)UeX?=cH?-`?^+@%Q6Xp>RW^4*14U*hXTK5)8?c61YxKpW4uhtO=jv
zAf=e5-`GiHzM(=PgZ{8u)4qgu{}pOt^v$i|&cJ4i;DVEw;I7}U%xGc9cS4G71-~)`
zePw6bXD>o@U&ya^fZrtc&xGf%d^ugvk!HSKi$^TrYDC0R*s!T-&>>6`?O+K67MP}l
z`WCwk&?$Y|>N0O3`kO>+EI&)+B9OOyp^e?Wp{dwLXvnghYj$}bJOI(w^E}Gi`SA;K
zZ44U&=YeWmOERNK|6H7sJX{cZ#yb?WH&D1v_Rl7QV>@hEt&)pbDzdN>>(^Dp<$X#)
zHPpztrjF17Eyv#A)71<0e}$QUPvrhJX8wt{{tYw#tbF=!xo7(S=b^z0D75)^5y&t9
z`xR9Fw<?Q2R)R1w0E{dF?)N_{L6|rJwJCt!>_1c&EKGEa%zs@8@>{<`&VMV4Vg*?B
z|Ash!V&xyZ_Kzw-7y&%=|1kDn{$c>Kn}PMu4GUR+8u<UZC4ZXum%)sj>~w5^Dw;om
z^q(de|7kFw7Y3lS#GeN<{ysVIUnBhAVfr7<2Y%nB@NcCc{}BBDh1`Sjzvx7;roQI5
z!UpGar<Z$e5bs(e6h8OP)63k^YGK`0D_Q$)>eeG%bY<CkWLjM5u8Ke)wsv>eruFur
z8WR-+8VTeQl#to%h~eOV_h@hT6gvj*b9><1)fR0I$?bb>4n;VtL+<3%t3mko?)1+h
zSS|G7A4b>6<QLp4n6{slY)1=M<-lECXI+!&$@%r<D#>x_^bs~r;&lOb=^vQgucA4z
zPdl7a$bFs5<ZW?BT82JAUoW?$w5Kr`k)FJH6Xa#YdDkHjEJq4TFNUE>&0`aU&h3d2
zBDeiEm%tKv>n*V#p4V;P#fm8x>{GYbQ;M34)Jf$EC+DI@V`(K~;N>9qU{D-6v2B$H
zN9M?|>j_!rLyeZ-dm@rMHaj(mfpfLGE7{h<%r{)gMenRsQKfa7T$(5?o{I~a&()9;
zP{vV#U72}6_c@rYx+A40hnmEh5#}Exi_{pDV9QOs^I?eX=5U)SgvDn-=*B`ntK^r;
z>QZo}eSQ;5(y9iZsbRjucCBzQ*1M`nXFhTGzP-e5IbRTI3y;E<dE@RQMUHFS2FFZH
zjQGPYED2tn;3p!w5{)|tKISR}8GC!MaTDE?K2fp25yTv8342$*Hxc$C3+Y%;f{dHz
z<T{kJ{jU+!@|~dOy%eWyN~8T8;FLRPq<8L`s73~L7z7W8y=G_nq|rTZaY7<uZDH=q
z`#_xtfHp4?fT~S^2!VhSp7X}M7w4A{Y2rZyyc}*rHaLto7#;%*GEr+LjF90^GMcpx
zd<(3WS(}$Q$O~J+xG}&eiUt++k26Mslu?T?>f@N)@5E~8{d(tPDZ^fhsf3XU{i^AS
z?;2Ob9=-A~z4*H`pJIT_*68`RybU`|fTd4b*HWTP3*g}1pw5;Z!BsL%Wu8=HI_t8R
zn#&2Kc^VfIxO?xO90tv(ySFeICN6(+kHeI171<k0WVP3|t^7u8^KB+2HQktcbg+HV
zqe90ef`7Dq%W%V5F{H-4GhfHL5C{J-A<eoGZE37~raK@kl*X&{m`7NCl<<9zQrcR5
z&#=X%XsmiFCBpeSGUOYKN*%iKWm<=7w-Lfs*sg~@TyR{!ieofx{nLFhVcSai1!qY#
z3jg4DUz`yP*pd>yJb82N4rDyl%=Zv;*;seb4pZ-U$QUZ@pUg;m%?Enjlu&uK)h@Aw
zR67BS5=EuC`Qi`^Y9PZzq^U0XCIiuu0L6!qgIdVbEgME-f3^30AyEaI_-cqRC}IE)
zR#L1ot2S`rv_{q2(1FLC#gW2RNT))j(`Dz4a|w=^()K~Gcm>c>^D}5nkUf0#>}}fP
zp}O8#Vl_RcU6<SkubW-!Dt@|b%n3wa%FhCY-%qwDr-hBugnLrBHyUL-6|?J3EtW7e
zcro6{7$i==9Mor!Xb7TPBB(gq2$*y*8!cBa^WsRaYb4_+D1)qM>FGmCAmQRHL9eEK
z33oH8Sx1g31+Au82f%VM6fwznL0@ypQp((4k{n8ytWAlr*iexQO+QL7^I43%UsEa>
z0PR~3M0pU5N=$L8IH}^R>Fe#$Uzd388O@aj;YmYAV0S_`xOJ3W+cmK=u7=vSr1g_G
zr4%wpcjwg<%Bdoql0SbZ<TfV@suMzk(^~zu$V36-1eCf>Da55@kga}r2Wc!9bH8xP
zwD5L`kMb_4u0r)D-(qy^iwm)qbsO?Xe>GF4wb7kqbw!(^)uN-5xc24-?6quLj8x)0
z{yq3>*`_WE7~cap2qh<)#OyI$BNTy5)&Rn5;nW4P*xcL0N!Z!9y;EMctzefuay`z;
zP_n`z+hNXlxSb054n1mPys4y4uXPp+xeQ0Wx@-Z0tvko71q~+}ZZp?3yYvq@aKZlg
z!9sAK21TKC``&1A)k>WK!({-c+b(gviYqBd>`Q(rAWTfJ!9)+k465OU^y6a|i>Am7
zOA%~^=)2~k5D3BSCZ)))%uIUc1M1C-D+=0+*q@sE5jfDFNI_NPVhobU*u;QBh(^hS
zCG8ytJ*&sC7IK$kotohweh&NF+>!`vGh5fMkXVDUWe&!X_#a$+fkQnh8Q%~$@sD9+
zNn%QCsj@?6dj%Ps)@w^6zR4ixzQpTxP+Y#R2{>KFvWup>wxVbMzRWPcZgr^NXR&h{
zi31%qvmcSCQBc_Rsb2rAFOgXMI^g6CG_QW;n8YS28m($VoPu^i%dRACL-#^Ftx^>4
zodaLt_ERX#p;<;yK>r21jNzF{g_rmQ*04K{yZo0Ey)iA-wLX77p6KhdEZT#+x&rc5
z?CZQ7DJDZ@dZJ(-=qu8%AP5^4-lx!y;H2LL_7BEJx?m<d-~0uz1N1Qd4R`zvu=}&9
zfa$*y6|e$w**|0nzav8dSmL*7e;}5h>-a}xC@TP{{SRY*;WOF)+HZ`N`S+yYpXUA7
zSU|-s0C@fhC^7##y!*qNe;Lch!2u{{{|ge9`S+yr-%QNL3J@U!D)aw5^Y0Y`e>a%%
zhfUg_&Bj>)eYpSQb}=&mWE6k~Wq+Fa=Z*dq9R3Zy`vYnFJ39BDAvNaTpM+m&S_Ve8
ze@fF%Xh^%Rh@h<>yn|9>BF{3%pNSkTy=KXH4})en3ZhPN%YiZcu}pmC@}$Mp^tu*(
zh?jIKiWO&KXZ@nH3iM(Sw%u;BQzJu1TWN3Fw_i}EV_<nFGA@=`zBHve)IB2cF8$Rq
z{?t<E^FGf<Z;4Cnd-``xlAUwZ+@72}ejB@ET$zep!|^l~4*TA>L)&e23lVjQW@wdg
zJjW%mU_8OFU|~jkv`uDR&&Y1n-)}Q9kr0$!P4~W_CVcdT#xh%<Uy?WLiM7UtAF)M5
zCsO1HP|+(iQO%y*ml5^$chc4_ohCy__Z{e_do48orfRYfg?RAsvRFq=`?VyQHG@jn
zN5k5cx$udpV&-Y^^V{!8FKdyKT?_=T?>7SVCidqR8oVgKD6>p65Iab$tbDsu22&Ii
z>DtH9xTd|?->tVC87phGPGW@_nM!H`MVi4OwJer_OH6XM`wT`7Baq$qHd6F9oOI08
z5qbKn4u=7|)dvIbp!O|54-IH-I<Grc;JxN~Tdg+x{Mi}r&v2Tq-}9v(S*4pS;ExN{
zPxUPH&A_+O7<~jXfFbAMwbXU7&*NFD$b)bwr?497RohveCF~_7u_QwR>tTmd1{w9!
zj0a?=RhGQCr@aNI)XPut(Y&Cmwfs*(L<kP*y{cH~O3%mwk;rMcZAH_xkbqHpII-|x
z`mzk!WOsUrQ79cgW{Jz-QgE$$+R<6!(!~+-D0nuLa&my=SXeCX<D&5_xWKTi@dz&7
zj<pVF)JIutHZ^?31Db=~VboOaY6hDLP9yN6^ncHU<ZN}&1*vjwvs_2XY<*AVyq0kP
zNNVMq78_LDFmY~@q?4riiXZ5g8HAi!-=Pp&yikit2u<`9eWYcDWbr=pXtefAYN!Do
z9cR-OCFA?RHIlOTTAx;WxpT7#L`~)LV*|HStKGF$kG_G~6JKt2NI$FC`)Yoj7D0Pb
za@7o=1L?YGfqH_W1Brsjk7lenW=!ZVPW^=N_O_nlxpu`1f`YXz)47e-@=%o8Fe;hK
zV$Su`TM~Z5i}XtpxL*BkdFQH4M}z>fEH67^qgjs#=q3Z+JZu6tgmJpatJv&wo_(c{
z3c-^kkL;hN<dOJ467>sj>mm5AVR~P{DJ&2uI~NejZf4gBa}1U-XpGJTiu7HQbAnHO
zm;VNK?RX6)4=i67V8Wd)!gG$4q4>d$`JK+{@wZi5OJyL(7q*V2<}z(L*abLR^!<}h
zwxu=LC&n!``Fz_t*D|fvxw3C6r-v_MWb!6~2rl_e;K*W*MXe8q1lO&r;4QzupU0YP
zqNv2hHGR~7iZ!(-%6&POsQ$)LImM$AD0Kr@e%twS|2@$kOsdf-Lu(HWcwolrQ+h#V
z2G<*LD2t|TJ}SzfQmgzPE;2(*ik9P)dNlr6;4wZ+=eti>?`3Ha42!lHx{W;A{Gs#r
z*I;n!KS_y~X4bP#=N|WoiC}g!`88*8KN=Gm?@e>8oD*lWx^2ByjBUdPRNN3M1jytH
zNTRu{4~Ch*K+IQ4mc>2ei5PpQjKNCuZ{Do?-KonIDl{<OT-4uMIxL#zz(p8ug%L7^
zJAaz&4r35%(t()OS!|2KzCjk0K#R3J|8Uhe_AY|`LQj4xL<g!ALiZUtIhl*!-LQ}z
zo78Ztj%W~_gg812gpntFK+a<0<@?D=cG0GxWeRl`!n<^6coIF$cjOB&G2OVqflMz|
zwFCJK0d3KOk@CRIxNo;891rL()}KQd<T2jRHTZ%_`|d?uU+Wa2o;pGeJhd0E6|awL
z%R}k5%7E(0_~}E&qGMN@6An&P$4V@_vu65RrMa9(g5{OzU5jtRda}83v^GKeU`wG4
z>6C2jyXeW{;fsk3X0EoOAXI1Mr3xz%?-|(hTeBg_^+8HD28{4+XTXMH`bY1GWjD)w
z1ELH$^=(fP0aI~>lgNG}gI8im;oJ&uHFQLQjmF0&&+H{a&}E;?U>w}tVqwn74rk>A
zGEf~m2S)w@#GoNe?7M#|B_Fw!Cd{FNhZLf^0CBJU(EitR5?-2^xN&C<Ydp*`rjq10
z<K_PL`skM<dm9|}w5zKREE&ZSl8Nce5Dx^RJA>zq3~p63tow6BH?q74^3RMFG{HQ$
zi$hr+LZ{JONj0hH)YfG6nRc$b@8geG&S{KJEKKki#sdSQfyz6jWp6<9ZTxhUS8Hh+
zSBNKZ!%`v>Z&5>^S|MW3YWwAAVP_Lu%PQO#c|gtaQiYTIuj%9#$!?8T-Y+E-*mms5
zU&zr&h{X|;F_Jm@>Hwkm0ikjTYHKe(ag>yrmg&@L+o2>Ili&I!Z`Y|};riSg+!f$q
zr!MU~`-YJ~=ROb|H)uG|9+u8fbKl&UH^9CpaM=}Ga1qSDjJdrT`QX|co1S}%$d0(*
zm6bQU&ZF*Z|J*Zz2md&V9r@HRedii-{Cduz^hHR!>{@rvVqOq;<O5_BMN_@~0hxQN
zRqdxQ;>%Xmej<K6C9(u}xr3#>5kpdaQ2xN4%QN;`tH)#4`MR{>4ksU~Sa@{5GwN$i
zD{AH$I<mb;_onoCj;_EEr}E@}dc%3n=|<paNnm!`=FoO~0pCr5kO#>#(DUk1xR86|
z;LKt<jeqUvIZzG3H&@y4$Uod55cPuRR6FRiq*M%v6(iUjZodV!Z(R?`H01-d#u>Ku
z7l;+$qw*V``D?`b6I1+=ie&yNJpHFs<X<3GfQ!p-)BeC<KiBb($UWAdlm9PK6d?2c
zhvKQ<v1I=YU;!L%ep~pj0PBZU&_B$BS7=JQ{^GfEU2~HKjtVPT@A&>{J9XE@G^3fB
zkCaovx;_n+4hD1N;Z8Y-%HBdT9Pg3X5YqCsRC9XY6xHU%;p^GU_MVmnI}=KqIXU~|
zj%gFB)UmAPR(kQoOZC0;n+OqY-6PVq&aNQ4dS`cs4;065aK<m=tvA{qgB~99-p@VM
z6o+S-#!S4mwl2Av<>srT4CypK9^1I%*Z$Dd_5Dhyw=FL!Bg>%Q){NcV`RmEW^gfUy
zzcq2L!QpyUIh&XiG9~EQ`>U7jqKHt7h<z3i8)lq>NYM<k^tt}tZ6uU?EmGyz=<2U~
zTIn<{_hd5ndle@DovED~f`C=|w$1!%OH+m8<G#(;X*}<U?NGAt)*>-pm4!+#!OXk9
zn^*1ii}X9MFNk>XE_H0UgkO2-Hwq)n4psb&2VL8HS<}lHJl;krQ04xAtbJv0ok_N&
zm|3!znJi|O#THx4%*@PSvBh99Gc!w;#mvmi%;3H4nU3k6_Pp)4vp??_QBhKT_f}-)
z$&-iE6D6&{s^(NS3PEEE4fNcRBW$hFy%3oTYhLS?g6rn#>f@pn^9<w>!E(XZitET{
zNlibWC4O-|-71G7G`3Z?{;JDQ)_R|R1c17vtba0`-xU``jT&<|4_YPU1jVHgUBIjp
zVkY?HZmaMtR3};>dQUN5!Q2CbRVqL>tRIQPR#qHbQ5X>VE{ZE77NqIBaJ@w%8Mof_
zW#0XBKU)?FNi4@|9@D!}nM`Jo_YpFps29|oZPH@O!d)0V!$81qQv^S*H(N7RsKAUq
zin+x-%$Y`iB$argg_oc;H19xw4}BLJFNy8#c5j^y0UrM9X1SVmlt*L^SvCQIy`d3b
z@Yooakpl<MQUA_eiTcKn-sWq|@iN?rhmv$G7(UV$shd5Sr%T&nfqTfK9W^{`BMFy)
zj;ZK*+lN@r?vZLL!QvqZ#3l+4|HEy${R7cf6%z!@!@G(pv=!}<z|_k9JMErRmyL&x
zO6GO<se9h@MMV&52CNO5x-%_X{g1^I>B-2&ANeQ4Q-Uyh6s`+8^eM1uka5}_b7=MV
zSll?G!NP?rT{)7;5ZU7h8H35MQ`VwgQwfdFm*hZQHRNS+>hBHcGcXt>k4wWjLY&vz
zytTjUV6RC9o<7jF1x?Og^g-0*U>R>3@Y8o>i$wYBANMfa=7HqJfgJS1FeL5h6a#@X
zWz7uK3N++MXJW39+A=8Jq2_47gjn2L1~O|vPLA^8%{h+;gT=w*-;(=bNIVA-5k(P#
zoU$=)^rpX-=)HG~f<Vm&;W?Zq1imZ{%m#<fB$%xOmWfClmp|1J&IJ_`>|xPH+@w+S
zik#6E(XN72`Sc^P0T*VD5f{|ybU7Gf2g(encG&71dAUEMFZ|;40M><X#6?j_K_T-6
zxBem6(ph6)5H&@xj>XqsNm<kw`atCCfPABDZ#4zagE%M(k=U+b<XMca&G)fQP=l^y
z9x(Z@5EI&3d>1%+S;HmEQyScNFuJ0vZq9}aC)@)`w9z$U4m>qzav*aiAHbMxDTGWf
zIB47fo`4xl#7>c{cIz7M>|0Er+NUb#g@iRMWHIN#iScg?h-=gEC*MG<2Zt)uh%xby
zA~?kuu#rdBgXndSsg)$*e@rdxIT?{9v-an@Y>o#}hq?|WV`&4c2vg)hCv$`>(WvZ^
zeD@<4KhN&|$d(O>d&7<+goS`06{>t6JHortj<lzyD0u2`g*Y>I-~dXksZa~NcR03e
zD@%eV81^E(s?fJD=lF4t5eQ+&V1vtLTm1+(tTiXqmW9HM%P{b*jE|-`6gWl3az0|!
zCNgPeTwJ)it&0m}NBx^$cQ4bH#TK-MDT}P5vKY*y(jF{=w{(;4N??lN@@?tFy1piE
zWW=ojD|g_5^PWLpBV<-FifVirh%~~U2!^ic@uC|+nU$mTBBkc3m3ImiPr6Y2A}ZRD
zAu@huwyh?!{+ElD9w2MR@vc|z>o!IjDY){d7V&!U)uYvQR+CacZ%$^DTw*>H)aXGc
zNRNiK45cC$N*sQO0FHx7WzFg?zjq#c!x-3?P-NQM-GVzeSf`6sysdmY&4(jeOac6w
zHLoARSGe6vg9;jLUOo-_3t6)4%aj0JM8s?Wk(QkHf5uU%tVZE+s#A^F*%R75;!7r2
z3AP{l0%9itlLHd|e%MakpSMXvec>HJv7N2f8u|${LS5O$^jccYG9OZ&f3#orM`(|}
zh3%be9-t3))y=q?neU7!lA)bJpX<6m9w5J>8ivS+@K%d=xq)OB(Nb5_hDGg_F?|gl
zGOba@lJ+uE{A9$k3;0kJk>SAp;TZTww(>^|YVdwIc~^L8@$4;RE_A1A^A&1z?vxks
z#At40*{+?L8^TuSTMmODC4&&Sn2fbhTVQw^Bg6;;5ijl=7E$2qK8w*K5&?AgE0NpJ
z_x$`46Gh=7V)u#yYICUb_;lrqIoKtJ&ri0L7eU?fV(NMO#?a%z@7=?@DL&!fmuN|e
z_1q^$v>t~YqZBV;+30Z0PNvakPK%3XQa`II3iTSia|K`?4>*qQQY3^M?I6!^M7)42
zSD_ZC?YFNOnXZDzePXUGttc+?knmsejtQ&Ct5I%g@owSv5#fo@J7)?pU=F*z;c?48
z+e=YhNW+5!CEEE29nY0oLU=}|yFHd^($n@un*ciiO`UgCRW)6|6XXc5&{Q%qrDml@
zDG=UB5ks&g&n1_Csw*%Q`Gl^SBas+Emr8Yeaj8*V>I*Tb=Fk^R679>94D=v#s%eVC
zp`eZU3B;rdQxEU>gEnD>9i*b2LBl#h;Vs{-tKe${-}dt>D|6r;B=#ClxWmSkXHlxf
zCtntPRqUGz-IGUwvvv*CmIVC|DzZyVc_ght4p)UHH!0fIwAhDOF4DH&8IGG<_Y_d7
z?xCmlHzN8HOroU6B_;9Sy1@CcN%##0ePJP5Q|KX+{27Kb^7Y~!9nu^>6A9;5*jQt!
z8ASkdsn3G(JP3=zS&t8K%BAKCX1=-bDC$$HkG_@NB3a3hltFro$evyJBeZ<hTw85M
zlBAF`XK;Z4jGgn7Jc_6Q<%vmdS4CEP1GXjrqF(mJvLY;Chp+@<2Da8V!BdM`3@)}z
zC(!k&#!}kf32r!eBG!3Vj#7=6BqO2*i9-z%@KAs(*LXIbZQ8?)-@X<&6L6@kE*`Q9
zE0kReWIflB=3h=03q~R&l});gMmr#@=C$3?jli%Y9?)L-<FQ~L8GrEtLWR9&G<1G)
zJ4>m>;7g67yL)d@bw^`KS`$oyLYYYPYv@*4I^&Xy83(+5577ruHUXgFY`S`)8NpI}
zHOP5llnn<j&A@;@EzO1&ny-FIJRi&LqO&+GNzddtUtb<yLz(*3jFWd?H>T%YhjjIl
z1MxmrMU)TycyMvZc}m{HM&vZ6#Z6$CPeQDF0K>E4exs{2oh}~?L+vT6-&^eIQh9<x
z!mr*6HaUS!<`k+Ox%%c5(fLC{1j`3m3AS6_&2pRF@unS%@^p|e9uf%u<J=cZ8M2MT
zk%IsxzEX28Ya%j>#eiFc*hbzrFYwFYZ4&VaqEa^EITOYP6PLj^vjN%Rz=UqOVJg8O
zJ*Fz#y|lO08@`TygG(kfJJKKf0R)%ozU8qOykl%mj{*T5hI{iyKDNZcyr11t>3eoO
znE+#5nV%dHqhXaC)wOFhdf|O8uA-9G1&?CgW<pn6iBs`ga?sh09Ma1RVJ6##Jh$Dm
zE{X+`b*K46A2u;Ub6VcFAAv)<>D(0N@X^5PTT;#o*_Vj0(3~|qO1i=tnm(SME_r++
zk+Q^B8ZaRX+U0N4lz?A^E28oxiD9y+(>?*-dGv|gKL-B=Nn!b;-1}>i@)MW;nWO;5
zcl>PM`+L*TekUn^@Jq1#BbxD#5wfg5pZ!ig{yV<}3nw6K^tZ%JMnGr=z!1&K@LQHA
zAq${`kCE}$b_oMhJzGa3`@akvVFj3t|3yoF<5NG6>~B>782{24_@}l1bTt1QQu+%q
z{pTV7ejfip(*6_W`h%qXZ?P7Cwk-V7ulzTu*)OzjKqdvp-=$`!sv2?AY-rx&l@|*A
zhe&#A7|R!RU7r$x8;p{e@N_Y)a!AiOoG-|R-=6Ug2ei5ET(bQd`9xyBM2$)BKgV}z
z0EV+I?+otlY&u|jun<Z_%bnXFRx^<L2Wg{Gkj7`bG+*#N$j+N<9NTjlFkOK&c24oD
zohb6n!)ki*83EMqYp(Q9odZgVbH=;IPls2#J2&=MMjti@x^lH?#k){AeB($%+aHau
zHWfDBZ!AmF6T&4{l<)Ly$Hx-xFpRZ@4-&-bu3O4r0<v|v^EIOvIP}ZVNxzO!!h;y#
z-lo=QH}bWW-qZ|o4O6Lin6PCCN)QL~l3W;P*FmV=+SIG(J;EgLDAa$eS`diu?Hiqv
zQYoU1edWM0iWk<47|KAqT{;o>WI);7_T$+n4I^1F?w!Ux!JUM;BSV6q$iM1M=Td{i
zI>?(B(V8v6ogGF+xo%3(Y%x_T(vt33pl6i=Se2byMquhE&JK=exFB?#DU(XtD%5Kg
zBiF5!Fuhb#6qGUgFl|JQ1cblqc1d7<tjh^}2*5O$<HfX9eVWS4cOHfv3B~H(Y8$hb
zGlzZ`2JIgjw_r>?YUgTyd1J{1>M>^f#+b!3jVPxX_s-4GqUC+x=W!z8b~$;8<Q=wF
ziIX$Lq7aDGJ0B}!lvwfYm8oKj9<8Gms%w6wIp0)bG`42~P~;6ugwUc@+wvhDL}eXM
z?SejF)S*DsAu=!&>t@}eAKKuh8U7pG_Zhg=?5$k-4h~K;gA`(TUj1h*l|CtB#N;6S
zK0cSuiVz>UpEVKR2{8v>HWW`s;-4IvOHK5<DDOo;OppaV6xT00$D9|9d%MZQkPkil
z`0xlF^z9DDsB=)r1s+(AY6g-7<>-4@rr)P|Gm=i#txD_wRAj-p#Kwf|8lj8APqERT
ze4PbJS#r21GLgp&Kfj`1f8Li*@r_zI+T!_UAht<E#Iu1%tzJfTIzGx`++lTEEf|Ay
zBjdBG4g@q}x?0b#q(!{Zv#^OBf%_!vUBTv#S#UOuq(J<ciX*sg7Kl%@7M$C364<J`
z;&zA{1%jk>tif>~9ex6s3YEpo4T^RP`JD_~WHB6bi~s6Ib_(f+oL^U~?b7LuyK}no
zQ?;M&Q7LVY4Hx9OhF70QcoGAhJ?}zfoVgauVoqr%zO_#FD^$g-^5`vG^*bxO=!%N3
zQk>Vu32XXuiscx2h4FSOGf5U&YEy*wZV+XZKrdy~wmF>zO3!+Wc+jX3c3>`I^(7Va
zTrSl1FUDEyN?3Fi*jjkzDCOh}v*gfy2^DtH%9Qo>++kYgO;h4&QjkgsBn&NIlZwOB
za@hw|ntZa~WA%s#1~#wn=seJTmQ3Ina$B33AdT+OK4=LBc|k9DN@`{j3Fse9;8uD6
z)`tQbu%R!v&RC=FQ!bxl`h__~JL(pZTK_}0qKj;&<+v=J487<fC8!4%(~9Dm@?G*q
z%>WZ^|CvU=I>Bl{>_sAyB3$bR@JD=1e&62pZ26hW8yORXEl00bN7zVmrPnpyEox~H
zl<Bc~NS9@N2y%rh%4HXk7J=LnyHbxayqkNZk$1LKLJ0Kv9DF66pP@-4;(h5{XL_O4
z8(BY_G{)2u;fAJ0Jd^qJf^z8y=S2i>HftX1`9;hYz)3(MA{AOkk5y0SCZ9Nvy2Mp$
zY~dXw(Q?zOovDN22BOTJdJJQDGFNil$iFpSd9nJh5K(B_Qi_LII%*o5VRkdGmY~EO
z)7Xryr$zCZ^>i40VL)-)^j~0oXDWDVlep?2xxqRdyG=KkjvNM-t@j{AjAj=bh_J1l
z-brwZgq&^Hgpnn<+f`dn`+U#>yz+?#c8@%Svn|5FTgS$)fX3+RItQYtchmYUq{y#C
zuBFA@>*?fV^|-o=aRV{-7BCgPm4G7y4g^wXxeIHE64N%F!QOM~qS3jTnid)Ckwpi+
z$kd=1q+Zi7f3)ZuBKnr#%~H{1qSdYPI~id7y{^HkaKT6N;QV`NeRT^$F+BL9ucM+^
z+e4{$vT(?>H=kHjTFJPt4Vg!;yS;2gQ@b(~orXd}dJzY?iu9{3T<OQWJuXLA4J}B(
zLl4hjYt{l%KUGRUsiPPyE_kjrNjkO5FwYNZv0cr2+kXC0{|SNz0t)c}nEaS&@Ie<*
z_>_QsMpy4wkmmocy8;5Le&Ux;Mh-Sk_69}{gsgyur2i7q{PYg|DHWfU<&Par{~e@Z
z<OJlnvjLoktSofwfbcRv9}^t|AX=Ll03X;G{^wcdfTZ<*w}7(zyGjgjlLG$#zY~_g
z_?HH;Kdt>|Si*nsHL(7>ui;l~nOW$5YdZT&k)QS7jSat8{97D9D<J;<FN=S^;QtGp
z`cDplKWW>44>Ga-@zwvXW-|b$0{>k#8!*mxSrnz?TIE7qIn-c8_u?v}EG*4wsA2)R
zJ&6<&3O8&492f!-AZ9Pq1A)PtrN2*Sn7%$bdwe>^cO8DtZ+vmHYu9RXtK81NQ`{nR
zZ(gO36UI8F&&BM>R<m{)URUu8n{nU%JbCoGG+$a_jaXsaJ>PfQ3bIUu;9goWveZUB
zO>AR#HvKeyN*|tjx>XqzOwyi@FIz<|v1R0}`SMoSxYE5qFDd&-=$@C-pFgYa*7u=R
zb&alrmPdY>TZLeTPeJEc09?K)!ZJ%AE&I*Km5Z47DadB6f79?Ct6oTf&)Y-$)Y2Cv
zQ{n}7#ij%BaxbG-+lds%HjYv3-Jmzc{?pduN`m#*V2VyR0_i=CFHvZxYZdH$eWk|j
zlcg`o7>7&WOTPwL^&S`QF9t@rb#ob%PhB3XnplEmv{ao<kIE8scz<Oc`x1}w*lzEl
z6`n|1x{6(9A!Djl=RPzHnEI<4Q!hi_%r*+!xLw`0qQcS2R}j*rO_?F+l3#VG)$cJ^
zG-96{fH_f=t(U+^WKq4Z5T$~VmGZ%gq-YUS$4&6+N$y(zoyX>}0Q?;cFXMS(>TxCi
zcp8q`ZhxhtwV{$F-PmhYdrEI3<363BC*)vBiY^Y=^#TG<Q^SLZ<>!p447Yc+QU|47
zUkZ)e(VBHrI)K=dYfugQd20ms@HGd^nIj-kBZLqc5HK^k()4i2sG)nPB9Nau&MTMV
z1!XI4ytp?sF7;8VP_Z7c-$B;gJ(;yvcdd&hCBn0fmMqmm4P=6QY(6!6O!k$CFR$J7
z)n^{JS@Qd*^rjUG_~0Sn-UdD?;St2;lM$?>bwOVa89i#(m43XBus*~aiBz5qSjN=~
zRriNoR3BHdLklS-=DQF^00v()051v+lxJD-;b}q<X1#Z=7_?|qu!Jm-EsrH+F#X)P
zqB{V5W46AItGYHtX3!vIBw^n{3fHaBzN^DGKdZGF<=_Dc)L|GBr*r;xTH`#HgkuFX
z|6NLbQ8@Xqfvbg$pNnD8mzq<bF=DZmvdLiIzA&yX$_k3-Y0eau{%F3*#!YB$l(0+p
z3W6(z^@LbT|J|cdvZ5g-8tz>3>|Hdwr!ievCgz}3#%&f|hU?Bo7^?Z`Jg&eT^R&j;
zJ;!9k#kVmM=c$@qe4NtB1!aU-Gsut+)@(|bU3ReP@%uiHv<7FDV+koEFFmk4g=d54
zHVJ1Mfk@@lkFX&QtZ0&L(0UcYK$V=|)ZE2!GBhXQjnq^?pL8vx6WtYX2(Itsocy=T
zoQty%zwsIA9pX1LaxFn@nWCNRNq@cxz(ZGujy@t<CwjLzfOm=U<<r9CQVC^<CjPFX
zk8WsVTpfqRmjNwtbu>N!+@)GesZ)WtD;=}R?l(VZ%zy)>vmDen#?O>1kR@~CX;;AX
zjCXFF?#vK2XyCBpXF%?)b<)kVbxujy6i?v0QINi_$1gaI_hxfUseAC3c0Kkf5fz8R
zicL4&5;%BlsLbA&l-oi4D9oweXFGR~A1@<9Br`Ds&>ZtH{Sa2;Lwl6s@bSWd4$4`e
z&8nk8JqbAF!th+g!n&IMjTdN)Bp~ME#vyLLsX};@zfb@%s&EhTp1}mi9nhlb1x!Ip
z=;&ex#BNBrh{|P1bIG+Yxejo;rHoJILM!P)5+F)QU3estf95D8T@u2j=Fwn(FWX%u
zmdz3Hl?<`jhVHm8ok(ajhEy(-4fL70ckB@EHkki_C)JIOZ{`sh>5LRueh%*nu^5(~
zwd>u^r-38{S2V`Z5B{`cPq*I%yvZxZ?1#8-l$V!R;{?6YRN#}sw+y^}qKo4!xi3(<
zD28#=)OXs$8SB4<Qx^lfw=dQzpd8WXBSPTfE{5rW(Yd|*z?F@4ATxJhBd0(cgpP*F
z&D0ChGu*RuG(11@?xPosRGO=@;LubJ_ea+u2+wqesi`^zf>y_|jLuJ_PP;PBSm<`N
zg<V8n*JUD2fsjszt42wc^KC61iY_wdy6jXi{HCWA&V<EU4p$Kz-VU+77b<R%#Ccay
z2A=%k?(Ih)aupk@h0RvM@J#{~T#LFx#LZ}ut|Y`^hWfvr$Ux<>o!CLYztih-3j{-9
zn?V+XaJAH~jX-gs8YVyzV#1Q2=IBq?n7>9Yg9s}W9zPx=nP|c}XX<JSp-fIoOJ;Mx
z<ucyeyY;1)Vhn#eKhmewAz8+#wE8%Xnw7fz;`(h0hV!7IljCJxG4GzgOK={EC8YSG
zIS<AW(+}kb4GkDIV-Ky&`uO?b=ZzwwY1|TYB$dYTnTG00`daCpR+8Wg$z4Lv;xJ*i
z%)VT49{TGCX1+iMETh`<O0YIOFav$8N}GmcAl6IS<vL9|7q_|jFEO7h2{#MHwbd6e
z<>3`58w4l~rF&uF<&eWk{Ixt!a&R4JfiZ%=(hU>7We4_~gNr?X*lL}|aUB`Se&Ofr
z#Y0<0%eZiI;@xos?PWoc>^Ti95<fDd4&bE=*nfdWsCg25g_h+?g+s)S(wH!c&^Q3<
z0co|J`GApAyrn#}^iDX464M7*SGFmVfK%6Ft5*R6LJI8gRq2QQXf5cqw$A#O9~eba
z5;zj!UI`+%3_4jBl^N2RKOjTp`oz9Axx5$S?SCF1U<T)h!58iRvNv0MKa%&Y^e|@D
z`C*89*G{Mt+1R0X#3MNL`82!}!l~$GBJ)lDG1Uj6ofD_K<I-ea1CTN>R@P?FYm87j
z{3B_a{}o#fd!tg8lFo|;cn30$m3_I_2Y9FjPvaM`m-UZ2)vuMjpP1y&N*?QPMR&lX
z|7Rr+P@n$|H~vY<`}yp5sP*5$UKR!pI%dE$7G`D+z>pr6pAsGuz^2R03YaF$@$0ch
zzfb33{i~SA`bQV>Z(`ohQ~O(D9xEWd>(4^qpVt1<RtrF%oPaSge?gzDe^k)^vhhC_
z|285C&=mKJ#s3c0f3fp#W0g4pyymYv|It?QU&@HTb)NmCPK19C1hf5d@83Z%>)*F@
zjH;<QtTZ8e&sT0JB*wYw`T{{sxlOsBo>(m^YbL1MG#6n-1*;)YT~Ea5WLmRy5sA&{
zmD40(z(cW3Ms{?Zp(Bog;=gPkEKld|SBjuOjeW`p+i*l3pww3v^^^F7-Yak0g5V9S
zV_h3gXM-+@l`6;l64|qTYouI$#d_MIEslRTTS9|QTvl2}%eB(1_Tv4tJy|-PJ1z#I
zdL;lBYOGn%B~Z;G?LT{V(75ha73{(fktVUwQWM`Q0Nw4Hr>g~V{$&l{j)ILK98v$v
zcMMI2UL)OjV`%H_2ZCk>WrQD_*YgrQuP{j$L$R=VUT;fM;w;QDmuU3plKF}ccVK69
zN=+rtD6@n{s<U|S$Iyo<p&f`#kFS_q&{RYk-&3o*Xo%TrYwlTB9=V(?2eTc)zoP5<
zmC+oPcsbjq6vHGE8&^jJ7?J9Z^GJXXtKs=_#?Q$r_>+>+CK5X;9<Q|@m_LnDa}^hD
z6l@6ArLs%6`zm~ew1)SlF}U5g#`C%?51+#-T-s67NK0t$9R3Q`Vs$QVO(U)bo>H9T
zImjAUsHMR7_|;2<y!_ex6mLmO2|>lHjJ^zO&<|Riv6x}G3*HDMI}uD0eh?_IVM~A^
zU>|CX=(sG&rX(?TtUy~Fne8mu(K7-Fyw_I9C376ZCThLYo!{j$pF&IYtHidWd&?*e
z8rz(nw^p5&qy&BL`=$+%L+#~<z;!s%QVFI=J}Wi61tPYH(gWlVRAaWgP@QJH8aU9L
zgEiHMbGzybopxo7YGS4OkRQ&jN*r#YQ>=iDs}7#XZDN=;Q*p@iz(gp~Md%z4(8#tf
z+6r{`OW9!`G;&81Cg!3=oMg}r-amNg<(Cm*I+rU1Aizn&Q@qC%KmGs_o4>T^!ghhi
z3?w{^j72D$CaQ~bBepYhXJTcYAh1<-?Ht5zi4>I9&-rPf-bzLiN~xv8i<<gr)ooWj
z9|{yp$E+|u`JwWfj%Fxk`2&)<)oWw`zJhTC7H%T+e$DxD4F&R~$(L?z_V+{cV_ILA
zC*~Z7EaHnqgGkUZ%~r=L<ru!^SQE*j-?Z4CA6_I5l$o4$)^uB1UbwkM)L^n~m&EvT
z5n>^YuWv3eCnNKMM}~Hrs2I(0af;&~zHZfItpM9FiTUBH#cYwus)C7h316aVfNG8m
zl#X!p@^aN+ra#u$V28wA_2qLX)dmBZBVM>GB;`9L(Hy_rrF+$N3*zwcEMwf8IEFy@
zTMV4UbTH?pdN4h?aJqi0C}J|1%QlzOHpDusIo6*)KiI4(8un9#C+ib<nU*a)8x>JG
z>4WYAQLCpjwq_H|jL&CO|H7lrxEo$g{&}I4&yeg}XOyFBhD)d6e3u>45y^boT03tx
z%aw}zi>Kb?<et14Sb=px{SBnaZs8fdaC2&gY3G)L`v!3cCf$s)Pi8;FxP*Cu(ouD{
zI}3Y2<YDpfn`%=;z!$97Cf$0a$lOxiZd_5*QT+thX?OUM7W*M#9sy0SI~(yJ$E(=}
zRV%YYks#9(7q&A|6ia5>LEG7gZauAdc6=N($-uAr0rVybEDfeTI&~@S#21t`bQGu5
zDM-|+AMXd1K=UmnEspKPS(_-%qE=7|CV9oa2d|)Zez?~}=))t62pt!_956Y=5-v9r
z;UgfbGjQfQf3G{{?=mtIjbws*GQl%(pc~pS&hk-Y6ET%{8H*f6p292dGC$NBiy&&v
z7-1ow1<B=b#<hV^VdCp)`9Qm|TT_m(IHGQ|aPT;S_!7cIt<z3@@4_ul;6?H&^3H1M
z^m04lN2n;2fn;W@8L!bS!;gJJG^QGzptFL;DqV=<v3+DyK5y<Xr{k5zn4oPIDV}(J
z<bF${(pZ@tDSBUF$c0~=k_*E04jJ{_Uyd)wf4K8IXn}i!zgwHvEvw@DvQ4$!NO8XA
zo8gmkn&!8UUjlQUN)8doLCFP;qiJ9(8z8xHFIVb9px;sEg5kAqFO`RyYH%^%;+8jk
z5ql-*#~ig=g3ph#OLzQr>uU&NM?s#+ms9QyAz>V9hC^^5u{8nK!v&0Y(O#@PuhB7C
z^n#Y^yl};#hPdj5SmI9c`<Y{W>3BX*wl8OkxM?z`ve;$Vd|vZC1P>dB7svTw`o3KI
zmi)Tz{vWw$G%TgJ4B^`DdNtXYP>2b=rHsKpO9yd|l(|*~C>g4`YU3-H<FZ<zlx-oH
z_WBpE;xk#F*rGx~u_sj=0`-VKRiScsDGm@T4i%;ynLN}xnP?|YYx%PgcFrfobfNoh
z(~selfC9lX{BUUZ5%D6F0a9NQ(9QJI1%V?P?0|1_pZ6BxekS^0F-Zo(TaPhQA;{~L
zq7ey9WZvJ8#a9U{u#fJ47j)cx17@o)thxIhW3<^1UzW23(V>4qV!(E2eNRRL$n37L
zjWjkuf^;AU^5(}tbJ}*0-fLFd5%Sd$eUgjuTkhSJdAIm}p~gIlWH})wqIvhTmSn%s
z*YEXC%%>J~rxf7HGzTfh5>=}A-WD9l-|F+rzrE@cEiN_}<5Ra>9(LrUDu>8iv<~qM
zuo(M74;eIGDq{A6WmPgzg$rL50x4mFraPODZ+)xBOy;zyjpRT>(jBPmMyGk^Kv8I*
zQz3W~{jSDjzw0ISF@+B)jtb4_>IXS-P$HsO-o%j)TI>-rQE@-fV}#<_GPkO-oAosV
ztyxqCLY%VsMi&sVOhHHH<_$=wvM;p-f95UTo2ltUqZ{-4zP+~u5|-~$_BLu%`jl!-
zoVMcdEmD!yM$+blIv+X0nXPj|pTHgE<@EcbJ*uvZ7~`uX2T}O(_{{80zn@&F32Y{^
zm)qQM6yS1^Y~?aDct<3sqvJ75PrM=219e$qPibOC@g0ZTOJ5V{9`ExPr~l|WuHon@
z?AxV3sa)n%1N(xxu2OK368G820mba#xlg-{bAtvwHh|R0XkRtvv%>;5eAFg%A-d!_
zzMSr0pt(7}@hW1%^hF=*)kF(d8h<=NMCH^@!^0!1uQ#_(D5;(u7X;2(7^kj+VrdOz
zyZ^T_v(MDf@xG!b2x$>>Kr#Zx7L2a#bW0T%pjaziG7EH?;yc!zuenXOyM14sJ*{Mn
zlKrJ(L>2M{pyNzhr;bw!Wm<WN#Io99E$rdw<lNU98jj~T?^(}u7Kz`zp!TR9R4q)1
zJr7EA9TlA#XuR|@W^>A58%HtZT78)CRJq}HDZz2n_`0csf@{#8v-;|zzmYw{-s7h&
z+}eUA;E)%{>|PuHG5C>n5xRaeGVpmKwAg-aHv22PzHI`#jqJ&H)3|c`29SUe4w$+*
zdqiFr*DMC~?lVlKbte|mG*kMKf?^la&Q@`jWTio&+O3O>)yY!nAL$)|Z0s>0An<fK
znokLrdLLiuLpB#xe?e{l^AG-oy?;$^eqzl(lN&&z*ndZE02!NqAvZst{r@31OsoL0
z96(7}IoJU)4uBD)fb;@(4n{&YK;SP6%dcmm{wvmjk>TG(eL#8OU#IjN$@qC}e^b=|
zcY^Y#wg1d@U;zyK_(jto<6m+eezBMpFzA&7Fmm)S1m)*j`u#xvzY!FA(LX0M04U?n
z=nBAT01_NH2$}y;@cHf2|9Uzo`yY!Kztb8<z|+60?PFOuEH_=Y`ULT<%B4e*LtV_a
z9!A8GTUi%4*?(G29W>%%fYz|2{In+&_2%VLy&;6+38<7|SX3`)>8`^r>G06L^sZNE
z+f-DnUmA9MB|cH<Se{{+4S~?1+kte-S1s&>%W4mxG-!rW-dSIHI%(85g|t1pQ+ZaZ
zd9S^=zS4yP8mp4Y^gv87-}+xqN1slyp)1#{EAT5hP99+S@W|D^l80Nm+#c3;4(Oft
z2aSVeWx(xP6LFe&69Us=9G8AkdD9Hwus0*zTng#NY14oRRoTuoZiWHbytR_&CB$@H
zRc=)bdD(nAZaHbqsj#M2!e7oIJE|2TyY6w^82C8bK>FGQr(bzyCU2I$EM)QYzNGBL
z!3sl5vT@lw;dro$N-~%5n}I84wQ#}sYJ}Cj$NZ`CH`>_VFiG?*${jlDwjd-Bdg#4F
z98HNzaLVbr>+o@-Vx7rgVol}qyZkfSptllR3AB_FC=bXm(hRcZ%4ceds-h62NIq^~
zy7VVzB`=S~WV+{o9&E<&T}(rfS?3a36}L%_ydXx{AY9WQR}oc!gc;>hXCRQoX6a$K
zffq`AOsq*=8N0+_J>8M<xWGrGQ5q^t#K(B0Zu{Qm^IQ@{aze&Rw&8e>mQ&N)?Ber}
z?G{wP=c(>uNNPw>)}=K?EJXa?S?lvHLk-;ZXe$;SNJr9IiCjIVcE!MR2pr*rCZns?
z;*O=Y*gX3K2vQZ4PNWaby3nC9fuPy`Px30HZB<5K-{e#{$%Exf8+t{IMARHhYSp8T
z8rb&Cl-4yGCbd1{cH>5Km{Ys)FldUb%_wC_EbB;w^Q%c-5S)c$bf8#kNy<RaQGdYo
z$`4IEmmZ-3(sVz}vDa0pUDuTlGar_FpKJn!dr`NXM(bh@D~Xk+AAQv~I`c|IO%a6`
zIxDVW>juqA0EsRphnjgNyBEOB0$L!-0-`i?v8hl$QG{+fZc{<$uVpSn$YPou-xQCV
z1d28oVN8$t5Z?MDI|jiS(8}63fm2^)bdWWKKVT?8<kx8>2Jt<M;UUcXJ|DX)^)~gs
zps%fvbv6cF6IlqDxA?v(J$uf{X+kNxw1Nkv!H>&g3IQWayi_(&a4{|$5ht<%Cjh^E
z7!Qq{4b=)0Ka8=)=TfM1w&GKgK=Fn_)XPQr)=j@M*Ro&5`di!Mp|O|O^Cc*`A?Raa
zrUeDhp44}JBs+dg?0UtUxMP`jyIJm0t8{lvVOt4M#N_@8+qc{o;UAX$(;qTN%NSVP
zCZaRb5Km%!!^l*joS9!XY91mE>-3R1R<L*xOZLEVmk|)!C#E4er0H)1A?5@c4u}#c
zLu8(oq-xYsKd@3HhXH+bJArh22Sl!(ef;(nIm^!W=&%p&$|VJuCaDKV{yQ-;=M4t=
z*m%OY6);)@SC+HFJI2*@<-YW<(S}%zs-oKQ{Wc3k!M;hG=V1^{yMh6DXGJSQ#Xlk^
zC!$_)5(+aRi@Xb`C)DVhXX53(FM!Ack^?>>!=fH9`b@aB@Ez>ED~OXthy8|h_9@Z$
zIkZ4sSWiH(8heOO9p}=jv(|-D-I5@r)_zBI4b4$Ik8_I89jdYa+EPR~jd*oKnPx;+
zcdogV1Ehm@^ayo6ZNv+h5Ap#iTWekMM*%)EME3m6?23Y_l39<VG_1AKH3i6h$?{hq
z|3xf_G!BvL8U{4dHz#;4HKo+;wJ`dnTW!|&;OX*P^F`0fKibc-aM`tvcapV^oNc)J
zUFu5Ab`7bihiepcY`SV?xqj^X%<*%Y5d{qYa9)qRNec7}ki)~*ro9~f>?3F}zg_^I
zO<w@sY(afrLe;Vzj=5$wi|)elt|Q_t(ycJL!{4MywQAn;+><eKk{Bh~(Tbr1vQwOP
z>j<_0a_<YSITr|~CAw0km0)cFGEB*OjGU~->H#y8N<KquVR0f-5N~U}K?+vFMWpd1
z$2XSp_DZIHzMC;B6!<`cI06p~=JV|Z5-Z+Bzb}H;1(Y)5++a(*tIRZKUjl)Re7mP2
z%Tzaz^o&)RE3nVrN6>>soeSrU*$P;2O{>{|z~WX9jcRT5sENf|@#LoQA#*int!ofl
zY_Y;BcnhSNZa_+|^Xr{6Y}))5qyA(+(-jsO3E>N=AdOOT33UX&2R098S%wx=%U1PK
zSfARPN69m#ukT%{X08K`1Bz|r%EI@Hp%~l}SgUB|nDT2V0eiGoZ?QV1&e{SqQ;ebZ
zo*CI`@z<c47}xD6ASi1kW3S?12jIrcypKmtA$Wy`LOJR+nAWNdIZO9=mvDCBMCPK;
z9DXL)F|%Hu9itkHLx9n@UCqODo?Ao6c+<C6W!Jh)crXcS6a~`hwYzg1)Aoo8xJ7h$
zU-Z6j#(^SPd?rgS=<+!dM7{g|J|v4XREG;BrVm-%=_o{{ZrPOEP0XK+fh1jcJ$d*+
zGayNxn8#4^qB)|OWNTrW`Z_i|mneM15EO^{c%wJuoN5J^$J)WN@LEMi@+!KQIGuCg
z4t^gF#wOCR=DD^b6ok`ZtP$)=^2hhub5LTb?!`&9Y}|B+p_pkx9*RO>^e8glmMmbL
z*NC71$0}O}b<BZ1bhgJI3m64$+j}fd-^N*-hy#WizHg4{Jj?V8Tc%vc$$WpI>Kezc
z8;+LX$jna9h*G8QNf}Usb@>>Gma2J892!OL!`Vcl;C^_im-j9^nJ?T^MAx+_42a`#
z(L|vntqagKR=m-T&8!$Kv=6s?Az_GHfczP+A(?R!62~x?A^|!PGa%?r8WOI0GLGHU
zxe<jb)(|B3*>rJHjZK5D14o*Tc`E7#o6{qtd-wza-spqPn`-s@xUg{Bes*pTuk^P#
zze1|E*$clo2BLDU-ox<I)4ih~Caj)qzXH~Ol$L)Dtbd}Ne*@P4^w0dwssX@`@e_yt
zD=&@hr`zVwRfC_;e#e3T+p58zhvqQ>+$W5HJP9^tb~+XS>|<gBWKR4BtXbITSXuw`
zP&ylHOEYUDdAawdw*QW;+5g>p!uH$A{jJIY<Int!-~Y3pxcrZ6e-Gqf0}S^2g?`WW
zM_~RR;PHQU{{AVs><@0?zo*{={4oC_6#oReY=Dx{-v+vhk~Yf>a2?Mo2Sq^PIY|T{
z!N3;PaL4)Zi5l|(<}~$;Vq-6+Q$GAt_DgxN47v*vGZde8#Jo``WCP<WCuh0TX+=cA
zOoDL)=WufJ469>FN8#c^Nss+krjMu2+~>#ks3pusY^4wOw-%pP-T`Zkp%n&<Kbw=A
zZd+;HhO~{~&#_M?Io;5pM8~2E)+E~*5&9YT8IrDx-Xo@=**XhxC;**`jlZA#z(B{k
z!1{<^PVBAj{537nt4*<=RtT(Ad(2QYHARX_kq%YxbH$Zx_sMVxY~uScx9xyeCQ&%O
zZ9IFNbR12xa!+kU@CygM^&~ou_%U?rk5>V(THJZ<s>aZaBcx2-C`Pm?$S{}@v#S+n
z)QHln3oF(3jj+h)s$>kXUJ*d|du3x(29U~p#-tAUaMN654%sQ++c1+2R7GL$ftB$T
zV8dsCY+UM(V*(Us@Mx_INsd9EI7DR*3a~tpFNPo~>wtU}mVz^<&>=)o2qZk)CQ{<_
zm)fdN+yMpP9CP{=^umdld3^7uWux#=B^MU7VfsdOC^@-UJWw^$!ab@GMcGrSo<;Tz
z{P5hmzV2=G&#16M%X(ah*g~-GqzB#p_6??|T}5JW5GWK+(k_?;jxO#7lN*m~^O5gC
zuDEC&ui@LoQ;g%ofh?DdabEf^2_27*hae*_9N0>^FXX7&?fK{%(V6S{K^<3T>H{z$
z$4Qi-S4s5l7nF*yz|MMwF1{Z&Y7wxZi}(j~-;|F&Z)n`EI`R|@3?D6m6CPlf%h6%3
zUU00lu-@It!e2Nz{Xi5s^pR84UK%p->}(>XFh~a3QIxJ+pMR!TDUh<AQ2q9f*<C-s
zrWW%-&<C{G+*<M%cOUy7En5HK?)#@J@DD~o_TNbM->;bd;_my0)9z0dQ$U!`zkX={
z?cMjAL6D7*{kNzbK%_bo8z3VR@S9}^^g=MR{tpv5|EJ6EkAL287ue6E`&+^S>(BH0
z{qp+{_t*d6yJG<G_J7=#fA7iy-17fA$=|s-10W{pZ*uc?!2HZ@K*{l3CtD0_+4wrX
zh!CnuP0AmdX~A$=9AxAyKt0Dw&uB0=clGsL!B9RnU-{gXQBd3Qf^zP1Rs<Ut1pDPS
z>ghhy#Rbtvk82}d7+Iy;nt&8~QwmHzKAI!UUkl$8!H1(@e~cxyJY<Vu>80iw=PZ_V
zj<L4Yv`Q&rKzuv+%RAcj!W1*R@s2epba#+egUOv`pM@Al64M38yX$up1jp^q?F%V0
zr%bN8xZn}7f#lnMv9`$1p!Zc;!U944daK*xMm<i+%6M;1^<MpY>+b&UqH<!wA_8+U
z&-QPZ&;5vBEFdL;-Sv9D?GuX}IT=NJo^l>WLGa%>sm&#pOUmN~bC&u!m_LFZZa#*k
zhRfUBbJ%|Lsh`e3GmQ&+?K}J~?b2UYD+QrJPKlh>4QU$WQQo%Hu8hN@x5?<mb{8IO
z;tD(pQKpZ@k%Ft@C`M>?(OR@b_kOsJ<_N~6KAp3XW6(B_o6X6GV=K9<+DjqRRu?Q1
zZ_ltcL3i@}o*~e=n%ZomYi01`dyPb#DikThzA%^wwaucWq1?+)K)Hk+n2xj<D@#-?
zJx#)a&AAKc@Iz4hTMtF{FVf44S7lTgxI*LvGS~;`-bTUpv6Jt8RftH=<f(~K!ofJd
zK$%7{M&EAF7Bl~t;CYF6HhN--F(8#UC+zow*bYe2l4w68NkblOTjNs~i`HF2Uxx~4
zu?jDo+K9eJcJ16IftCxmaYTXFd<t@#OfI9*iD_WXoX+7rKNts2r%hula9{XAwdxy1
zo=-dWNjTK5yf`DgphBkLh<1nAsz;g|1NTNdUL?sGUngrOne@Y0`o7tXB$n~*A)E$v
z*I4&<7S5)E$IgbZh7B`A*yY93%_jdt^?B(C_7Q%PR+Pw|d|E!fg6u&OzAs<S0CFr}
z^ufi{TlW>O=%p_-2&@2t9CsVubHvEJ=Nfg_ssTG#I%~b8Xnd5lqK9zZ$KZOi1P!a}
zs>4#&9o3;h@kc{#HjQ4TCM)Ur%=rPaG$?T)?|?6guENe`y!KiG^+Hq~-VbNoo!=LS
zAK>{FMBq82S^HEiYeTRRxC|Gu+0#JjM0q@n=UAHF3CQ+)BIL`8%vo|kMp-!v(J5w$
zqx3K;tA4~hNxHa+*`m<wjRT6tQpg*H4v*87gidNYG8vk|ZjFcw0l^0oMv%SG5m_!I
z139cd1~}KtX&Gx~V=2k@1-42Bo-XujI+(*bcm!wmx%8y%KpY7T*G&mH0ymnBptJoq
z-PEFJP`MgW_1x>a2vnFkV%%Bs#-TJqcrA8-v?Z8w?qzkz?iq~sK2Kw223UbuQn8K8
zM%S*>sHLSfneRImh^Q7IDYcC4;$#B1joGfbqtzAz^a2cqlssKVCx`D+vDJaqoM$WL
zX1)}3^Ai%Z_UgG5EW#VEs_epS(IjGGtSTdjSAHHXZ6cmv!TK_N=4Wgn)o3*et$i6F
z^Wr<iI5Dgi0bO4Ym%9zLTU;0i(6d65e2qV8iS@F!GuiIcv$)^ux!muLWdQ9%d|>#H
z9fgE;1CYSDSH0eXAULHyNDB++1cYn2!+^znL!k}mZ}rhv;^#+6ixXeaHG@97UH(en
zUd&Wm!VHCA9=o&b*yr<kY{_6Xi7iE7l9gQeIgYi@_{0M-84N6R-?bTcbb@vy#dj&0
z>dmK`|N8-z{EPC|W|%lxCDT^6r<zMeeO>PQ_h-tE9H-D7>i0!bBzC=cIAL1rC<1AB
zyADCXt1d{91bAGKwyQ)Pjg=FaX~9QHLuzb0iZM`EZ?xGjhBB&wFD*p|0_&FK$~z0a
zqY6Xe7^&#!wwVDDm`2U8oNQegMDgebnO#BW$Qqh~f=#EImk>X^5?U)T_>jX`9q~&y
zRRUHEFP`snj&#<TeGP5C_4s2uZYxF@Ke@;Y;)0{oonlLXuc3ZvIIkwJ!4(=k{a(G8
zE2NhxJ?+0!?n0Cv4du&nO>~f|48C)wywPw&T<KL@iU<MHc?z0;)sB;7KV;}Y*$*v&
z`TUU+!c1{EUx(xAd{?YOg2ip>M+N>gdx8NRLQ?5&@+?@Q!N3o>rlqwYs$}Y+EANjn
z)td3x$-37dQHd!5^~ZKBcbymBVxytN@!_eTxKm3!b*MSM6>izyWGbwK@W1P-u$t`F
zu{)q{Bjf_K1dASRek-`ly$0I0VGEQbVouhdY1RQNW*rWAUoT}%7g3>KbC15hs`RKX
zoOW`n;RombhWYZ@M}e|dMN{+Ocxw>h`@|T!QMiMf%|#De{)!gTBQT{FH^0$&MBiY&
zWvuVej3R;ZK19iSyR<Q#Q5iUq?SQG{WA<$Cbr`yAsrk6Ta^~wSXhRNLA!y6?Wy?@c
zdTg=?Y@5e~SsO&Dr()x32VOqW%>l$dSr4+$YE|M`g4Jw?_SS%g2h3&bQz$^Fz1}G8
z{JeU+ShwS<Z51qN)m5UYDd#HwL2LEFB5afTTkaIeeKh*){6(nv^PE(x_AyN23X-Wl
zO8=I0mR`>-)V7Kqgs8_!D5myEcUkKv|4`JNp$*fo{DpFxdmQYVJRcU!R)g{oTyRXK
zw8vfh8>xW~z-IVuomqCV&gs@Hu7Q+7KiOE*ojg8w*cDFzYx)jo(!2NPVAB~Do*fJU
z-Em#C;0KjiE}INkBk{)giyM1n<v!3?JD!*R0tNy+`Ts%;zlMQ7LB*dbF#AtX^0$q1
ze}#eU|1eVh2?qXr_B&Sk-@?G(fDZ$J+kP@Ka3s_K&|W$QKobR^v4xHu0Do8jv5NqM
z86hhxz(~jNYvbI1?1AO@WBKhj{P*+B{vP@LOOoZE*8YS1{)ZII-zfaQUd><j{l{9s
z*dRJ~K*rHu>Pj4cG@t#4#Vmlz8w+4)(O(z;jh$HlW<S8!;IC`{$SeNC#>||Ibc}#R
z^S><q`Mvu6EAYRD%YSU`{kIAjAdQsyr<U+r1?*3<z@NK=|JNb`AgldfFX?y0&B^{x
zl>mlb)XWmlSPj_JQqR#y*vP=f&<K#Q3gh5tZ=`1h<GPfuuI8}AhTwf)dC(X`vx<wK
zLs?)>kXAg>S8O>tvefz<o!78q?WBQ&m!7zTr9%C~3)p{VlEfpbO_fIw2&@ay^m<mW
zSH-9E;p}c}*CM6@%p_5xx@EP}5QX_&c!I?HxKPd2Gi83U%ClO7q)a+?YDW2Q7pcPY
ztZ=8bR%=ra9nFfwSK-#@;nUXFcb_gP9eVKIyz&N9jn_9vJaBUlU#{kt3FtdI+aF#(
z09kmFDHj`Ss_Bv%&lQwfIbB{?de#Rt6R4z7VY;6yE0?N5^%=pP+B-R1p802nQzXx-
z=eYWrmZ^n-<T?fr!rHH}><Y;fh;NO2=@vM|==2DW==8LTEk=1XCjk#H&j+oz%i}4<
zg?^}eMZcLldyah)S#?%(qsn1A4lB^svDVM<Z!yGX<$b?s>|`2GhWB+0AwjbynTBHp
z@9Ft+bawQfUGk*YwwGZG!??U~c?Qufvfm_w1__~q$y^Xx(D${txM+5DUs%(YFTW$k
z?ps84X=F%Be6Y-;m^1Xz(a?!?49Tc|IjET1t6bXG77*(V%KRLiradpo`z-xJ;UKsq
zZgamYIi<p=J%^3E7~__lA-%?jxkVL*hzlp`x)E}w%QWV4G|zXa-<aR7Ki|F)(ab-7
z&RI@c)YIxRsu(V7SvEQ@`*Bfl9_4j^yele+_ZrC;!N_BiP%r`m9~LcT!P<&}7K;9(
zim$A4Zg;h%GoVxDqWQ^S759>OiT@(-7#rc?T2?*s8?BiwNt01AlMEE96wV5gwaO7#
zypobyI}VqNNcuBXn>aS#?$}Gw{K;jh;4*9<IjzaJWLtytwT!Q=OqJ;8Y(}9_u&J#-
zc9)|NFc$`2E=?DRVKiAyR!*PYkDD~~DH@~(S<*huh%^?Fg0mB^L`q4aR_2UOW;dq?
zave}IrAwC?@8QduEd@eb_r+~im@=pAWQ#Fd^G>@PC9|$rvDnJ;s8NMT^U2U^(=v?s
zZ4TDlLBUAmdTN{y98wmNSiS%W=G*##a4F5Dhan<K#)tHieO73o8K}*=kG7#_2T6z=
zk(tYRv<?&mmDxY?=a_UZEE7Pf{@7=*B|OE*q0SX{PM;qtm&&nohJ*7uDY-#$riloW
z9%33Q*uf)v8W#@=>>#rUhe}d;3QK`<UqebdyvSI!9glc)8uq!(vrS3~j9wcA`TPhd
z{)J0lj5K$s+Tlw#8jfjuHG>R`?q{x31Et-pviIdg71@H2Y*v00Q*x;)y~dE)lHYZQ
zA0u3R5bM%=QbDXi;j!|7iPW)+o*Hsl^S08s%9A(|!Cvf5V5CFu<@9RbLTIT!3?D2h
zyjOpUok~S#ukXckujU^*1nw}(;C;w0K!EQDtEU-{$y}s|K2gempy=5<F#OJtY~PAT
zXgO4eho5YJi42m=;1)P1X$U8|d-3f~Rw0yMf(+lSS)J4%S$vmbz?4SJ7<~{3j`6aJ
zvN8&f`UE+_Bo}0+zOdjkHpLV*-^;`5bh)C};W{|(3ag>yd#B?*o!$mJQ3pqjFt)&G
z#cEPUhsIppc-4h%Wki|z8SXC)Q$@5TIYQyn>cTcrU{V7Fj51gkg=aH1t!&XH+_tiv
ztD>XGk-!A<>-TfPR<4TSHH6D<c|;}(e(~X1T^}yKf*bG{Tcw^aOrc$d9mOzCe(O8%
zo8a7W@*d*a_no+1fJp6OcDU=p>i(3A(Y1=pB59!tqMWVtt;q|4g$4rH!FL3Kdi;x`
zCIhfdiE>E7>-DLK21J4=T-H<%d=oEbgm!l?xY|0#s@L|43UO>tmHh#`AuZD;?XJ5u
z?b)kl?p@b4GFP(UgpQ;Z7?P+M-J2st(|v|ED|SKEo<3*;lc_7Wod=`4KFNS+?py2d
z@@esfOJi<zPQkf!$E_(e*H?*Pro)4J2QmswX{+<gfhS)vy3h4&6m>8hwd3;2!qux6
zBdqvyZ)+G;`U1V1i@qVSb1U!YkOS-lN>0Hz8tGV`P%}VIQGnv{^ruXGZ}m>SwDCjl
zylokEo#75R>W918hwoscIkIvW1~}1t9%C68DC_ukX4ZC}%jmg}qTh;M-}owD?IA5L
zJF3|=nVl*9!J5YQ#oOIInYE6q+g;j!xO0(2`PlIQk3B%ge!1#ZuXxqfHUWyzN$S0X
zqV=4=xvlW3-g96$M-hb=pihBaO5;Pvw64lQ2Q%v8)ER!L#aW~rjtKVjo{z1nef306
zkTjL#9M`Ovtke%dYSr3or=5#O`;I@l)pbzE5H=Rp5QVIJR9jVXu_KxpBzU0~L4&VJ
z*9x9Dbmj00BE^RH;U<u|hFRA<<Rhi(A{)!0PQ_fmfVli8YmBTfEV!Qxy1qOTf1g}2
zf_ZzOezrXw-6r9Cw{1S4iX=)$?g}S&AGlO%-~&5&gCZER^l72W847gZoJHl5DCx@-
z&|=OJ&C!gP6MG&w#|DbrgZ{U4`05nMC`R5GgfF9djC3D!Rcw9)!EaLovFrc^`G{xu
zKUPZ-nvD^dohN>4(y>I!K%%PZ$%l!E?dp#{p8L`KxKB(zJyNnW{WO0G#M%fXlOg9F
z0X!A;SeS(U{xrk3Nui&8U2dC*zdq|Dj4KX!MG_3Rk8S0BIGi22<g?pe5C;iE;1?7D
zcSzLg$-N3c-;iiZgr^flV3Z*(bT0DxOUx}hKiJDPm0MgQwM6oOQgd}s2j=fQ%^5lz
zDw2U?=w=;x@N)C6+_#+=Sf3O=tn*|m6pMMd)r%V^%JL|^`DqGdmU?=u*SSTCmfxTz
zl@N9Q;2~J2EKi=aKeG(_jO*BXanf^I(SzhA1mq$jqPmI89KTnziyLPQ+MNS)um>IO
z6;!2O>Oe^#<cvKfRQ~cTWA$N6UE1FMKq{t585Cvyii(s@5Rq7hFZBDnIT@7tPo|Jr
z!<}Hg3eY%ZxxLa*jvY)*jv5!}m{xoc+f>QDPWxW`GI(`g#m1+SRMe>j6~^G<9i{kZ
zCNQpP#Qlr59fuWE+-PcIxgdcIyuJC!pF!mVnu!DFb8_GzVE+$$Zvhq8nr)31?(PyS
zxVsYw!CiwxaCZ;x4#9)FySrQP;KALU;QA`(^!fW(_dVTx-+1r7cdG_fquBe~wd&ip
z=9+8GHQFZ2>WVAfz$?3oaSUUlPcdCF7Rq-@(WxoaA)jwj8YPB|H!<JfHo~dvAOPDa
z#1fIdTd<Pgx<k|=t`!jC8(SsY1Kp?f>ezn0F5(KOl^qRZL;T?xJSfeiKgWFhl}j^T
z86Fa)6sXVnQ9S--@Yv23=T!$7Z|K#>5u7E1)oa~h4@EXDdw61eKK|)7k0b)k8lFX2
zP;@Bw+|<~HDIs$H0K1;)Z90~7xe@L1PKm*ds{V<_kGu(_m!Gs{Br1*jluN6n9(zHU
z7I@Hak-tO-bnlJmW${<3s&Xfw%_&7)3SGvDZG^3<9=^uzp_AO2f6c4ZvK}F`KVv`F
zwOm2S2!4NZpRfQ_(5>;ypm6NjgO^(efKorofW*hJ57ld(O3+wcZ!W4n7zTUV1n_qu
zD$w=rE`OlT3H=IbMBTkl5sm#}YtijRP|hgIlywl^U(D!5Qf`N?c-XTg_gf^Nim5|H
zn`{wSn+Mv)(ucA*p0S!K`$r<we1M)s=SE{_PZ;gUe0~&xs|h5Y#)vm7i(O^6wYbyG
zCf?am_WN1HX7|nqm&ic`o1{)RUrpRAWvNgGL%YSx^#)EP2y!xGtXKG(tUef-$e@AW
z)<7c*7;6^S@27jMIt0KbJ>y~7H>UcQJF@+?2&tLdo6)2iJ9Tmn*Ld&t(Ob9GLf$F_
zFg+ihx&uW=)<8;U++2{uS|-DUe7f&6Rs&bf#vRe3I4!0}HHY%NppX?n?02|0$xy|5
zmu2k~1@gMQ%b3br4G?P7eQc1Q%hKmt*Y;t3@EW;584^BYd%5?7FXb3Kf(*njY#_Vn
z?Hg3dVD8CTety3d;8mc%AH#ybxb1kDh-l4SyZE{-Q#aC6HHQv6oDO`m)Ggbc{e=M$
z#Ct*_T!~7t3Tcpuf9~W3<QTC$_D|3<*RP@ApQGiU0Ksq3G7wb!XQT;`;rzk20y8E5
z_Wm<^X8T*)RW=}?`LX;rX!@ryzd)(~3{C&&9LojdA=p`f8JfWAD<)uvD;8kpCOaz|
zJv;lKt1tY5rdfVZCI9pr{(J0V2C6}D{;=b3cKyg81(urqGqZk`K+5z7%YMx@|7SDC
z3WViMKnX53CQhIf7dsIv&_YHaZ3OIWLC?(c=kbSsQn>fie*SNoGa&S41*&?nZ~`p?
z9;+<C=4vcJ28xA?8JK<h=lk-XSoYJA_upd~6HvB{9T*f~=47P*QECU&?Esc+GIO!e
zb29$9YT4iI`h&s;*Y6`3Kb^UMjkDwWeFWn-WB;b`0nF$9p)LJ~2*yw6`Y#jmA4V{K
z8=CmX8NI&>TKqP>_b-u#>_9!0KZY<kfzh(RNE()@YeX%wV|r%j=w6aBQ?ulrz*Ztn
z5zXfdd^KQMOeCdbtv{L&Nx+v(h6{bZNyl@v-4ahedDl-WOh9n4)NYU6_KogxV-<^E
zON-aTG$vqzdZtKC(-O<Hn0J$51KjhqwQZeRt22q=wB5F7)%Rkv#Qwpk;dtBndG3LU
zFY~jKIy{Vf6zSi&&4-UZ-kl#LhGD5-WiY@W<pP^C;uo??V;(;BJw$!1^0Drqt-a9Q
zi!?gH@?ny21F0SEYYA(>Qg58mFZWqQnd|qb4$l$B_owb6HhA!3XFAu#dgGBK6stV#
zFU#t&y}4Duu(^+)<I7lUpH8Rp7Mas}wNve>lL|<Brj<8qm$&v7wV1_PyU>>8POLZ}
z$m+~#QTZt^+xcPbESVl$u&+{X>4PK|=`m=R^2(C=;O{%O`OgouwvR^9pP}LsA%%f4
zI1QDWHYqzFXT>^+z&S0ysW1Rf+F`hysvWSR%b0bXF%ni*;z^oZ#xm?8CFrjW5Sl8p
zHpmYr;`q|0T`HByyQU+Aa*%EIT|Yj48_Z04<LG<oe&iiwJ5IZ6j$e^WII}H7#;*1A
zFx;YE#&`(zbY^u`+875Jjg_61DuK~Wm;wg*(>;&oxX_!4fHh#Hps{nLfKaqnQXNH$
zuWCzuAq`_TqgW~pk7qMP$a0u|d}k5GP0&L*=xXIz=QrjP_T@VFNtw27ruWdPHCADZ
zn&b;==HD0s-Iy|kP5MSb(2;x9EmaT1GIxsnWcVTa0DK@b$kd|Qb^~dP?#mZcxa5`F
zD=X=P)bB`d{6Y`84CS|RSl&H>TI_bt0_YZCM0{);>xJChEjhuRUg6`R^o9ug!N;zs
zm#iAmaVvOEnk#GXnBabK-J3cF><+Z*%9ye0EybRIdnLhmIdCE1>P>oGd_y+=40coC
z-eG)lhi^|q_-KHg7VPyQ247<I#Q$now7kp#Hvo|u9m7vn$Bc=V)UdN+)@vIBAAkJy
z3Jn0q$IQlFBauwL(L6n^aSDwB9t-boqq6n%qRVKuxf6+;g5WmXDrVENk8APa05SXh
zK-F+x-AT@_T9*$AAzx*M!>tF@<Vz5kcq#Oj0k%59^Pu?SDO}V}O3&shN@U?|j9;d0
z`zI92jn^ROGYPx=pVFPB+!Zsx=c!MxnrU6X^oue34Sg(yU#=Zi6eaEIt(LeYqo3fl
zsv<U;jcu!E?c?-Y(Ks)U*Xn#mbIRPiuat5hz|dFXVT&FXdQ&9v+97vMMgTV5=MXwh
zrdPhT!?~eB?G6Pr0*m`)BV#;BhheqNC5Y)Wl&6#PNuGvw%W~br^Xtd9t@D~_mzh(D
zVjAs7{=0Oat^#BGP!w<?T73*Z4-Si}<6^dz^Hi_!&P}p5FZA;9iIZJJLq(S3@JDn_
z^PVSdZy&5Gu%NfTs=RT^-z$6OsAU38#tsqDi(qGC^&)AMWIJ@Oi}d=+T`I_5mV6T9
zHN53UaS^Wf0w$*CJ3RdIqru0tCRS|V9QV?du(>mH2lUeXSRh&77KE!wuP?rEPqND8
zr@%*r=%drG-F#vb<V3sYaHO(_ysiuq1N90El?G4F!H%LNk4VMB`OuNH_h!UDp4{Pj
zBnC!A4vRZNDqFFumhD5XM_n@wZjzkRBk?9MQ=UM&7Z9$<1<B1{<${Kvp6<W!RmB^k
zlyPS}Z=>?2+hI!N6W@_UG|D}XfLy#GA^pA2qH#SNE;G*Y%ai5ZWrT?^culN9F$QVu
z7gf1^rUJ&Zu3MiaE=rT{J*kFyYtK;nOKD-^eAkY<!M(`bA91&@=1q{@dE!qp<Zx7*
zQu9!0VqKyXLX>c!z!-K1Kl{KhbL~12fnPt+CWwIh^-I)4^~}D%3;PTj9_A>L9#4^3
z7G8sk?1oAsW>9cZ(fo7;-6vFvd`463ca01=X4sR1$}isW_)R3|%N>_>vai0Hf#USU
zu4!XrY8!%+i97ENv}Z@5$W%?SqZ>2Qckc|^wDAzD;BbP|XnK*FGNd6(q9yi8y|x1X
zAS)OF3#yDx&`uVrlUCG+Pp-2KT|?2<hx<6^)hNbFoDoZ;mrx7=i}HdecISR1qf&M>
znuo+3*F6sICd)0+Vo|TshPTl!jO+)cHvEV$QMNs0_!Kb^b_}9on4pfBUB&cZ!e^nr
zWYok2A>ohRw~s2h9(*YaHp*Xn@8}+r63N#@nCfEZt0G-BXiWDNkN_ubJru09BeO3I
zE4aM%*+0vqBZo)~7f;nmc8F2hlK69{I2y2xjK8UZWj`lbf*^=WV`a(}74i*~3CJs7
z%(E7%$V#3Bj><W<>iQ@Q9V0)V)u6%#>&Pp%Tvz_IB8FaFen%|@QqctsJkzQv^asCX
z5A~>S6eSlbMiawH1G8*s#5s*Vl}mb^Fx1W~$RP?%%F5JEY8{ieE9-z29|>;8hLIYL
zLFOl7zHQPBNx9J@WtWid{M6HnhYhse#;kiq&lHXq+$5H_AZs>yF(AYX@PRV-XGa=e
zABNsF9*%cAb968)SSB4Wf!2Pdt9iO2x34*QR<UE`YZkR750OA15o07xnI8r9^nLJZ
zG<jaDdl9g!Kwf>i+oihYFA~E}EB22W0D06oA%e^dfH<-RJGQv6oQ<b*UPFE*&QFFB
z!Br+HO!1+cH5Ed=TkT*igbL2wrS;Qlq#tDMnJUMfd^l|CfGVT6gTq&_N;!e#B)y>N
z!?XjavL{#X=#M%jucZ}qs!+CEcuq&+39oGoGL}1h{l>^S0LW7Y8;57@2eaf|S+J8+
z7n$gsvk<;W&4u4yXXuO=sWXwv@$F`#)U=wgp#zX_-UT=2y2I_qA{Vmd4t-r$!ol4`
zxUknI1gaBD1|s9M#+serGi4m@+6sl#eykb437WlU`cU)jf*}jb%yP?Hliyp!X^pz5
zOr@GoI()K<<0{8)x1RiMQW84bQH{^)oM;R4W;LU^oiBBKpsMKtNxwJm^v$~>d-!wc
zEa>(|Mzy_Guumiy2f^pEwDTbDHEYH@??1qee`2nZ=5P@?U;I``Lpx!&I2%*7EC-Vt
zkJ#DAzRG7)fsBj&hU*f!PkqPkMEdep^aHL(uHBGvD}A<XaKyK77pFcb7@Y>*Y!P_~
zx}$txLt;E^V#n=7OJ)0`U|x`JIO(9|A)29;xz4XEYREM6JZA5^#RSFJ+=3>tI8Aci
zywQhZsY_|r0x`&+!1TZNs|+4wtrN3pyih%D>U)FMM9`Up`B)Kzl^B1nk8y<rl^q3t
zcCnaUA0=0*ZFQ@e%ABq(pcInbX$JH8GhJhw*F%3iWnNq|9qW@7$_=u2I=bwV?F>ve
zOG28+I`n65tZ1=Xk$`(riHm1~;1fLTM%N9{-4>RIjFZnF0tTfTJp6BQL%hhs)nhg(
zsRN1yLxsx9V)$OpMS<-;Me|<5AqdY9;b-*5ImfN5!5~rZ1qBdvUi<b=h6dih9uuIq
z{j?IAxq8Z3E-QQ?MA=t8swpuy+k!6*|CCcTFI+0DrdwDTidkB+<_s=QqUO3zvpZTv
zK(J-v5?hB8^}WQvd8B9wU1UtsE`K)Jgd#Me^5E;MZIm!Rw2c_fSE{b1J1!e+BEkdv
zhjr&5&3S8?9vI~7`W17*2RYBj$t%UOpqN6p0CpjDPt<v+_V1p!t878dlp%yCQ8sNd
z+b3W-qA`KGMa#POE}-Ncp}wCHa*0U=O_AU_4LY>n>rVTn)Xo>Q2d;i;r!=tmG~2}x
zhNU)Y06X_&N)VC$Mqld1x|>>}bB4qSk`W{L3n+^kn8E{`2$9&)9|H=&TE}1c-hUcU
z_}M-GT|nVKrQd;-h(84sfGO912ps&h>=y*{pTS@zAV3Fd*Zd4NfepJkfasS42sK&g
zxj6ngqyvuoJv{%(*8d*6Sb*sO9KfUmVA&~95*rA9nSUr{a{&VzKv2xY@#iV>f0M}m
zy<O};0{!ps2}pSVyifm9dH_(m@}G_Unc`>W;G|~;ia-7lYW_G1ewmPe05yLD4gYbX
z{BM!wZ~5`x){Zg)@%hggssBdpC_Ce?Q)z!;%Ypjje-&zOt4T&Ja-z21RGiT<uk8er
zTn*_V@%b50fWec$y`e*+HA%5D7eyL~Sz5*(@ml=|Z1`Om3xHvSJL)3|GdNeino)6R
zY}D}&^U`{9alYy?PoguRykJZWYgs0dsFhQvpQOc-N2NQd54-7-b+g|)vUM$sVHE@S
zj9rsP$WEL8mayGoRAjKmy?h+Dd3}GZ5a2^>8`L4Y<g$3gZQ&|_!?Cy>)S^?ay{zTY
z8dF(oLyV<HD;VD+So`I>MO9JxiWfANhRK5=3M@o6)PWjZZ5&BBYus?(@k~pJO=}w~
zj`cXrOw|`T-xhO3t%>P;+KhxG*!RQcuXf%pzIu=j(inEEG(8wYca&e%=W}|ksb3S$
zyucnO{h}HdKs<<cLwE4*^y-nIU)P*YrM`X{A8xXjZ&U&DaGNdMQU<eGf^!etjDMlt
zd(1_!vdp|l7@e&3(n@xyJJZ@tQIKiaFaPv|2t5|tl0+Qjz|b0REe8hrD<tl(+$m_L
zyH#~lDoRk=Fpm3D$LPt&^=nfbo~$^fi>6y?t2gmwEs4X2oHC{%oACv$cATT-jD{1q
zZ@eiZ{1BEv5+mymqg*=Wlfm;1*H5#i2kJLNSIHc)M~BJ+`P~$)C5N}m*T)<TC|z!5
zP1RopteP>bI1mV~`;5k4m8^{SB{H`}_?c?71dJZy_p7%Ka>8FrJ1;x6mPVFAE;tY5
zZ(LjH)Y94ybYf_tus2B0Zx(ev9rD6jg%wjqG%(|@*m$*jP}VX+ouJ`~!+c@SucJXX
zwc%+U^c?f1o(#*!4SZ`l3RCC57Vm;aiC0RMx?;tH%9j}ufiXW2!Poe<rHfy{vH?oN
zR`GV2nC&V7u9Ykm3qOb13dN#ux0ye)36HDG@FGsH{LTof@XGtRHN`TFBcr4fxA|5I
z(z3(qcpyE3D(suwS_=~!Rx6(z#H^hlTxYa7Z!iUY%?m*ALXVmm4FAI|OBD*{)$!Mm
zGIZFM_LbYR=QHjU@A*D0Pu@o-`-g*TMbdbb%rq&i5E&u-gYmaTsOn^3pHRJJcH>Zb
z15~5wG5UE~2<Q4QND6|(#Ndd@x6)0biCvEPI`{cAY4hv@DWTlxshuB!!5s>TJSdy_
z5+MPd^!_EZ8MiMZT*8$vq+w6E+DAGgbgZxevps@?B=UD34d(Wa-GS{ElIQ9aLOzM}
z)>U`azlJ2h=t?&)kM$EwvkfwYwxu+u^G~K5UV=2;Gx39l<%V5pj83})!z)wld}c}B
zK0+)Pt+5C5ko9+md2<vr%85Y7(B~bYh{7r`nt52r`oij+{ne&m)=4wdb;<^vM;yY-
z`3Ys#ek*j<<)aU$Hq$G)d+}QdzK^+Y@W<Ac0mc$dYoG7dIN`APQ({}rq6>Cc__O42
zLs6UjU~V@=D70FgOTPuDBa;FmiXu4nxCIOH`I8@XV*)!`a_X?S;YfAR1FGK77u%-l
z8zK2Y&`3uH;3c1<+U&979lf|RiocHb-W)KcRl2uSjUguiFApg;pe|7L)r7$Y2iBI4
zhG7uUCq!e8a#g^`L)&+_v=5mlnIns+A~&&k&q*4wQ3d4E=4fmqBg8vfYtVnH(<|{i
zM}RW@<lvAjbMz7K#!zCnyC=a=cyyH|C@_?q9CNggze*TA@ZFIKmBkY0q5$fyl^Zm+
z!KR<b`gRns$0n+-5L5*mU>bwNEm;7J>YHmAG0j=&f)}xUaJMidMT!YlM7CJ&(DLSk
zv$aZ8$?{+v4RJ42d%M@g0L~mR%F*mnb?@rfK|NRE`LsLa)0aU*G;58F`Fw9~236|@
zBR_il{A4gB=G)=nI>xs!M-i(%2JR|WoW1XmA%Bk{;&!Yo=ssD+)4-w=x*2T?8e#4e
zmnggBP3}#pU_Cgt@h!<}L_9F)E9bL+ZQtjyPfvt&NrZ((4OXaFWreZ`RLDZ61n5z;
zFp0jzIRJVUT@V#E{gAw6M?Z}Gt|SM(rY%T>Gpp0^@8Kq#dj2L$3)T2pHp6`t{&iw+
zp9?#n&8cC8aSH@g^rFcxQf#E(BGTClH*ah`Y$gwppqTPSSTqQ|dcWfKf>_X|i{!hC
zce})U*na=eY%P59&;;>#T>fa0s%saPR0sB4kcCtkqhD2328Z!Ui{Mz8^j6ajIZq+?
zE^)JINHJQh4X{8OH`G3O=12*FkGvm4{bFZ_95lUZe^PC^gb4M17%~GOFE)*?eT&}t
z&HSO0a!oe851nIu(bd1`IH%_|$pBH)UiO%Io>Agg9<PhW&2p)I`8Ui$3%jw%5Tv_O
z6`dx=%X}Ejv56<?V32@tDHACs*Gju6kX)2hBfDN32M2T36O|gN$xIrbcO%<oAz#H0
z9x!vD-b5sU9jVOcl;~!D1sq~N)^VgcPPWm+Aw<U^fO5||doP;T-1XEoJA)Nl&=D9!
z?1sVemAg4gZFfU!UUh@A^YvCPWbnIm4{G6)zM^k^ynF%ocN)=u-W6aR5_KmZs*^R^
z0Fbj7PBgiR4tBob#F(US3grvXO03uO@p=V7^i~3|lw;=cS(h_-_I=fWS=w}gyV*5T
zdGtOTDlB;rzs<w6EY%~=6n*IfbNF53=9a)0^%K7u{rphF&yLl?anjJ3W;U`>-}+sz
zCr00Myt=_=0GWb~tLpLbmhDdu&9~nD_U+6F5}Y2Xc*AqmHo7Qx8y23SE2#n0T784X
za~L5dznJrT;7~nR0l!=xL^1X^tw9`_qgC=v_mM+A_xBWqog_XC?)(I6xt{L=gY!93
z0y=zhsv(gS`&bb>(!a|G5Jrm~VTHyQGqh}YGcZjT6>_GM6Sw)~j3C*MFI#Gq2Av>y
zEj|mhrVL#hMERStD$lE29rf_qIDTH3#<cR|<Z0s==?jpGoH}`!r=*&s<m<>Br4$#k
z>p!-`T<jY4iSm>mMy9$y&4v)zo9D!eAcR2aTA$qsmSXl4IO-6|vWaHwx!SSCN8ujS
z3?-E!$_ykKVs-YoG9fC_%eE~-*9Y}}?d%XDAtvvIZPVpN!gRD$tw-e@KSCv2UD2fM
zQ8dO|z%c49LAJR&+*AXuP5xan?XY5M{~01@vAg3aZ~#t-Y>R~3#&Qi<yFpSYNFAre
zE0$CbsGYUlNc5J$F4$G|k@nv}X8;hMH2K*|gGs+5c4>X^^CmQ|66G*RMSu&o)wf2(
zrtA)LC!Gb^U|JlTgu%;rS~S=@rc1=h*$fPgbv$I5k+&c+Q)h9sC*5XigTB`UfkJBX
zoiw)N&PsWdeZ~43Q<n3noJvyR8bGZr^JRQHqm6l7n3m~-v7o6l4|j67kHLCJ^xfj0
z)T6Qgq)+mTH~SBe&`+r4M~?3=!u9NoKe!qoIQn;MaoK^5I)9IVe*WKYP5G&K7nVWM
zQP08E#*m0X%GAmnmO;?k&cM)4;|GRg(qa%}5NCL&@!LdX7FM7n=1+;oKLqQ6bQ{~x
zHMwlSn%sXxxBWt$vi=SMu>*x${_gPqgfxEUg&9=U)QLD*=^26DPdS0=Ie)@S{ZQ8Y
zCu4smwU{_K=(&J0qrXQQKhN?n6Y>v`#y_rN`?bW(4}R+J5aO@#Q%t`GNxy&w79dRh
zi~LlZx<>3`J%;D1P8WwF8VtD=cv60nEw{{}6iZzV&%Ogb|5tP}Td8PfhLvTnRU~lA
z7}VO9@&?x`Qe<Avxl?zj!_LKzJ~uZd8CK=m&r+|WAW0GR4wfxiImyHAWQA|xlbma=
zS00FU4XQS>E>`1?eec!UROP*5bCa|Yo}}y6e4>O2m|nP7orSeM)!iHKwhyjGXLe~q
zYe1tSvmrt@zewJwobrRO;EsEqgLYRrES@g8`6{3?`kV#;FA*oymG_W}ZOWW8DGDOF
z(0pX1A(t`lQ)9Hl*4a*<Bp`jfmdW8T?;J}AyPt_Z!=i#@q4Yd!cUKI88Z`)COjx87
zhSvC__pIb<_CTaQ%xfh$l{q0|dA1CX9OP4nN)WoIBx-!8ExUGWopHDJ7tO+o<uu4j
zXj6#uuex~8N+PA|H(5820~MR5r1$z&?QIvaBnsJv?mKOEO9k{2M{OQ|abZ&a<jYO{
zZtWezlo@V}<irPqv)G1El5Fgt3tz_kk(F$THKIXM{J0||VG$VZB;md3E}9|n5q;}E
zY+YtnyU#qu%IAF_7n(J1y}*^GrOn^3wy|XpVoBZII2A+=JiUe5TKc+vWC|#Lm!e{*
zKXO}gck6yTS8_Nvw+l0F63iOeCTA;{<W9L+<s&-ae)`=lA1;R}E6bNsL=ST;?(X#`
zpNXx$H%)GSF0HBxExyK=4CJItuZdCJFk@~`SLja@QW(UmMouoA6iF8i5kUwc!LCwx
zNr_+SnMSo+*aLb#NKk_NRIeJ?cZLYOF_9OvwYQ1&IH_hHB^TLcNO)1TN~}oe8!)^<
zd1>z+DA}e}T0lZFRu4aQPd9lS44E}BWm9@-*iH&qx_QYc6zlv*o4p$xikauLrl91+
zkRVoG_AU|IX=qn204nZ`GPy*-s6ou<yFN;w_N{#?+^Qr&dd6bY7{Y7T{M{;*27j@}
zBM(Q<N@x;+0GCKVh-7mB`x@gn$I4(Bq;EC&v^Xe;ejU~ITv2afds~|WYMQ-HirzXc
zv_7z_kO$n=jbF!?pWHvI9!$s`P<=d{I(*dYoBWjHM`F1qMZDj@868L*A~+GI7o%PJ
zO-bcwDKGF9Y<Iwp@0mfN7&OW^A<(SIOJsLsR*9*1?;zndrdvNXyJVqAiqjf0unQQF
z%2#3B_A|x_4SW-Bxpa~cvOwG54-0-!Zcv30S%02%-%mcm_8V*=mVnN~OKo}yU(E6n
z9*7{4R9nEj5xKTpIk|Yop9SI<Q4+&w#GVKH!(9q-PTyi(<M*C}hs8_AhAq7?prbkJ
zrJr;#I2&O}UW^hscJ^?YM!E(LrM{o{?*%S<v>1a*$)sJnA&<S&hUZ#NCKa_(F(P9k
zl&+gpOD!AyZa%Ej!aRebmYmVh!n>@7TjF7LAV6c*S5Y$QDT!NSZO|Eb9$6PO`E^Xa
z%3J6HuaSq$E`0v=(ko%&O&n*@m<LCd=1CFB$f%f``Lv=~<=DFAF0wZU{!{w59BuP;
zTqMi$-t13AnTe?1&l^tLYc!-%kKmd`SgOhB`w9ht1!#y1`9+d=5pGAE)2fD=1e&&A
z!epj)=6K7n#2Sl>*k`{($C6el@OArtPC9Gv?5HRAVrdcZ#BhQTVSlXz@&ekKF%RKM
zuXT{U><=;t;!Rc7x%xdHZ|$b}c-A$K>3~%9JvLn)x-1Tsb`;Bk|3KQP$r4kJG=^h1
zbS#b~X2HJdou8ztkPs<h^5o7w#=!aAXD9I`2N`p6Rqz$KKGtm6%{JLLhFIRtX8Y2o
z?B3SVfPjViAY!<iS<9U!`eK62%P2{2aVA$$eR6(o*MZHE#au^E6D^q!*eQDx#|!k;
z&V!porjNm0VD(**bwx84*^eIxscWs}dqK;m^+QwLE{x{vVaTt_yq%)PxIYU$=>aUC
zM!L&rdgS0N@V_vyxI?6SL&ART(maFKLH2-3{T@&>MPh)praBCI#X`~HrlCR@X7Pe%
zyPc!tUsNG>dQ|SwD&grMY({Rh&kbFK(jUbQ-xl*#fH$lM9P9+Tp=0SJ`$_7I_9Ppr
z8P1y?2Cm0d{b0y}49~=AgwXL-1Ud|$f);WhWQ(IIQtt$l>CkW@>qdwFsFj~?PC#DS
z<x;e!q*?Ga?!v$q2T+?aZpj6UvJm-90D)Q>7gFw8BlG<fI=_``Fu%9+cNB-W7WI8;
zXhd%xAsm^4Dc!vV{6(7@vTTO>Y*0c&@~5i@jy`CH&ziq4p1j<1^KhD(ahkCz0qJ`;
zY;}@y{h6n^zu<lzq*G3M@r&p1b>MqXZsL3!zO*|w|9bm$Si~HH)HkDZU4m0AYO4Yc
zV4Rs4-`-@;b(PeS@afO?1el^)%J!YN(99*HA5@Tol{oEM<$K$3_g`hW&M#}(y<pG2
zrcD*|YwF@0e&k`8z{u*h+{8C#j4IZrIfftHerk--D`WzmYzO%nYto0kmAOG>I3mh;
z4ThyJG~H9GzMm5mH7r>!t#6}X=ErUBQ?OwMd1EK!L<4PoRKOT>&Y9hb?$jU25_(8-
zS0{L6NoJ3zdbvNMR?Idrq_(Q_bf1~!yfFHF*h5#^kk>{Zj^3lRRagroFSGa9PA9*1
z3Jenqs(NW%+g6S7KBquo<&HE@IJ@$_xpOk^h%5{zB%3TVmC{;G$%85UT#7!_8TxgO
zLrP6`qQqWx|2RMOrjut_!H;bi&BRWapel81Xzo#W5Hsvm{%{=})PM`0zYb04PM3hZ
z5!$4R;5R&>xN{Q;pDW)B4smVPo%!bBk;LZ<(D9EOrC5x9wbv8$rUOAT+<J^3p+CA7
zGWA<H@Zd2N%5*|O;1JMwHdZt4ZUbqvQeabHXGUH20CRQDSTmdBPbVPqm*A;^xnYH?
zAYGDSJ8ob$@@x8?LnHQYM?>3K?bsZOG<G%}<aix9w{+OB^MxD6wAJfxE_R+ju^Bkq
zteACL$|(oivzzXi*b`Gft_zwvt;Sizwz|{uOXnwDKCbeVhV999n?8bP$_r6>LbAp!
zn$r)FGx`zUr=vvcr0BSsUY|%y%<~Q2ODg1hpcT9eI;{xDL#X)Di{}yGS`jOMXLXE|
z5$V%X%J~_uP%?*GfjZ&h=vh3YWB;HX;d*{lmmy4M=cTol;b;KFdE@Es1v^thwvgKh
zrO`j0==SJUIMNG5F6ZQ*DDkrctM2~pX#Tk$|C2-a8%Bd2*rxJd@Z;Hkl3Y{(7E%!z
zS{VQxQ&{%jw`Ka-j=vU#2kJ!p-Q*vB^-sUym$Cn}NRJ)J9{ks1fjM?ez^t-A_|?G5
z#Q$<IJCLJb1}5zNaq#caz@J#n#tD@E`D20K54p|%@{8GkBnjuAsQI%4|My=GX5|DH
zVE)NBvjgQO|LeiPEJ?0EVLaHGf1RoJ4|dcKc8L?H!1>4d`t{(y0%CrRRDk7#Kp`by
zcH$4P_`3?i-z4V!rqJ+rv<edu%TGN~75-WS0d^o)_V>B^8}MP`0;0gb2z*A>maP}r
zk-ctysCg9P#E?)`VMEqx)YJ)7fg@H1)gy)WkqR0PR*_Q7>vU`;8gNN1(Sg(_ekPH1
z9(Y(T@Xa}d7VQ{a(HW7E5ywOcCru!2ZVWB_K&eYA!4guKIU$VV5#rS}$dR@%h7}^F
z?{irZ%s*S{7+;}eep8X`DyS#wV5?{q);G`FWouQ>^K{ciK%Y_~ld-hV=T^G2O8;i`
zjpm4zF}P9>N(ghL4ar-Sz}%7SnUB}oc#*K259qpaowI3Knxf&z{#1UB7<9(FEbcCR
zzNEWKI^!&c_=fl0_f9>+wTKi2Mc*2rA~EBL$n)~r8s#$&N!D54-xw~=8#>o0TQyMb
zl0JuuGwXc9HZgxExRJ-#esHL)!d~Y=eIpG$UScFu<I<vhY}D?ptcYdkVTt1qk(dW6
zSl9a&H`VkFQUVNPklTJ3av-Jjv2Zd?rc})Bqtr`vqjE!AaT$4~Wp3ALp5eMlvRMJF
z+Zl%a67JpEClyP5aS>S`OGu%uvbVGEP@};?USVSZZ@ls_$zM}bjnJUiFkn9GO1ih)
zZ7yH*TvT-+rxVe>CT7Zx;ooI|(F?(D?Q<FQ5u!&f=sxRKQ0>|^UfiixIK26CAJY3!
zafUt-_mRx2npCkIM-S%){h%GLikOOIzH?{ylPGE^G+FxAP;)q&OGz4{0JXVIw>gx0
z(~1((g9*EbmJ9ay_yUKbH|9cQ9BKb4nf`lt59MH?z|K4!D3Uc{&NM-Ekzk^+Td#;u
zZwKCj^*6LQ8#-_Hgfb#*v_YAqIWMAiU!X|PLKya;=WQv=>wz+*wj)Y(hHZ?%YQI66
zdSUxSkkz-LWwqjJ6C7!hlma3KT0TB$TF*0c>RmYvr+7D%bsEdO<&xtgSrN;eHFd0Q
zz=P_T5rt%E4#O_eVx)q&Uarh=_%@iV#jF@q!KpJCifxk<+i=;*Rhcf7`HL(mlsB(Y
zA8+5F%OP5$sZRXW4N++3y0+#!dr+wj{@rqmD)i#QK`*J^x$UuIjN;+p;=0Axmo|E%
zpe@qJCO44*-$B2zX@8JU$OO?<)Yun51g~OpS&=~oERBY`1OWzDW<+%EPAV(D7dKhm
zW1fKD2)(m}lEuTrWdvns@Lj>x_+Dtv-NU?ceX=Ar1GmeUNdP7f$a0<N!|0moL`6SG
z$sfPN=WZ?^VtBW<S1HS9s*R<cs(zK0OiA1`8il~`u~c5aonLcmvibhxX{nwASr2EZ
zQS|L?W;u1wb$Qw~v231LagD4V*er1qb>wbhq9|klxe7r9q9-kPb^v%^X1z>mmDvT!
zr(HC}k(yu%^gM=<?;-w0K6vkW$=<Lmre3Pr1QLIrR6$843%2LNI69R4@*0cTFO+rk
z3sye_RfI;O5Kdi0)e4OrG*r!`sd9#i`N0=<x4IDO179`kyU!3h&ns4!s=8xkH;M-^
zXs3N3g;bOwHCrZmko3?ehyk>H4CVzyCs_<wfmj1U$J5P)zV%Yr0opu#BkuTEW{Tz9
z8P67dy62{@%hp;$Xu*5?AJi*xU)`COnki>~XuQ{{^lk8&R@M|k-%$n$^+T~T4z8k&
z5By+l8p(`JLB%m1F1T_{K|wgjdS3@}82&v6tICEM(@iiO0f{<#E%@!;J_@en+!6lL
zoh7u@UIQtd_Y-9bIpwRV{q_t5y1dcAs2KLkb}GUZ%{PiNT(qw2a_W6g)3}hrix4?V
z2StdJfbsr9*t>eGo<9EeV;}PCdJZ{)e!LoS!T5(!trDl=>#@fD7Iqg$AAfA{c0Q9l
zAEVVP@u5$j_Hz{k1PY+6U%3RUeS~>;H;1x$4?&1Fg0Q21id;=b46b&{4)2Au42z6n
z(E%RB*p+X@ne99<AASrYWUkyao6D!i{5IT&;`MMxHBQ$o?^%mt$r$9@uYy!egkVdH
zb8o+OjY0-_q3MCzf5MjVaKt$gB)+_hvftMUkaWc9HCssG?F&4j`p$~ST+MXd-a<86
z3p%W<z1Vjf4NdSdr6<cG`4Wl8)!1t}6^usF*mBI5VhVRAEhCA!P>2LU$%N9xR=!-i
zK(m1x2AQvrOktr*yxB;>x{;D2rD;Ed4{U}?CCQuiv;`8hRP(L5(Y@(1C>nvz^>XJ~
zSIo*5&p@|3o?qJeLS>VgKwkZn&KjuBqJuw;Ls<}7fB&H5YX3@alvftf0DfrX?Kx3J
zJT-a7`Db~ytBmu&)~S{2`l*Hhb=f$tB4cG;WLjD-T$y*MGy5a!61td17TksI_0c8C
zX$!vFRGa61MP_`w0wo5P<tjGOatRclKYK2R(8S5))Oihf66H1DfvQl2DyT%ZR%^7h
z9_$+R&T6CEc`YlTGO4pU>u1gw>(85v=d_nhg%z|khi8Wy45V<I6lb!5=qvii9vNDu
zJ&z&qz#j;aAT>A77@@Hqkf2~2#z~1%?H*fUJ6Tnm&8Acj4TQGdK=jZjoZ!8y$Z^bz
zunc~89e5oQi8iMDl=hrLQq=|F!CL!N9>>V;3K4$Kk(To0{wEnezmY%ubAR}!`}}YH
zVdj4+uKkNY{P!O7?0<7KeiT&vA->K0>*9!?O;VNBGXpAT{3^mNZmDZ*2+N>iYT#f(
z#KgqG1<N32XliWYK*Yw*4h(&pI@rq@+P$;3w6V4_v~nQggk|`fi^9du$qHQKowbFv
zouZAdJ}_J^Wawn7Z>S(D_;*1Bp?6AukVAO+_Ob>*mJksa0e}FXAAnE5%QJvbO8Bi1
zk&>ali4}00#-_lIwc=L#^hED}Wa|K476JYMP*6~CP;f{vFi1#naBxT{NZ<qNUwD9k
zf`dbVLHyVN6ePsIH1y|hdFcho1^_@HKz@1v{`LR?1p^0e4H^a(4g>)9^D^M;&r5%`
z5E%dp0tN;O4h8`M4i4Om2XGkx90dZEnCUGfn!GL)i7h&_PjnXaYr(26426j^QWiZs
zUl>?SENmQHGI9z^Dr#0Xb`DN1?sr1MBBEmA5{gR7?^S@jfWCpDk+F%XnZ1Lfle3HK
zM?e37z@Xre(3se`_=LpINy*tcxq0~og+;~HHMMp14UJ9B-95d1{R3YHhbE_{XJ+TV
z%`dEPY;JAu?C$L!oL_vuyt=-*y}L&NnhcyYFmR|JCWC-F1OLEKz#)j4AW`4SL+RS0
zkudu}qYFl7RdvC<W>Gl9(6gI>#Uy22Cp-UP^-o6sHJg3^Eu;UB&Hu;bmrei*G$=cG
z3K$5`dK3^a6p)t%06Z87@B<hMfFJPmm<j3)3!nu2@7w=Q!T%lw07|g!Rpdv>^Zt^J
zW5up#$f~dTem&P8El0i|%r(@XoEARSn2EkBiyt};SAR7!lADN)j|<2+egWWk?e=te
z-LlN`-zf?}Ej}~90LFuSUjS#urCt~L-bf9PhA)5&B#0Nl+PLZqU|SkMXni~U0(e6J
z_X6ljuX+ItB?I29o+m!L^J2XK{P^@=0P(B<kB&`-$6FVM7XakPffoR^3E;u2U+3=Z
zOy?Q?D(YG3E6DW=AiU%I`dY^$&LQDrQ6|{$jlsrs?w7^6)##fgKs<j+2(m_O$UnfE
zo!aL}tf_9#u+8s2mq>i?lqKXR|6W9Jm#LD-Mht4U@(Y>3W)8rM8<6n-tLp<&lIN=z
zz@Wl;;*%dQ(+eQ;kQL-|27nHb1Gkq-_%b=v&AADY^x3;O(;!9`05VA7&BY3`qb@pt
z1YtgIh?%&7fO;)$x*a8>;UO{hzRd-ZT=Fgch?P33K)mjXJ_RBK;A6y(t$_#-0z9W=
z^l#JFj&my>#UDjDH=ng1xdaHEO$pBrv7YR8wgqrmf2;`c^ZNhae{&x^JAIZ&4DUF?
zS&w>xQp-fj-}`Y@CTecf;7oUhe@|vijIw2ijw(2SThsEvy)bla07NMU0^n)sPBO(%
z%0>ujhIK<g-{QS;2yWW&OqjedF2UU83T|~Vu>Qa^Y+!Fw)q7ud89j(!LBz0qSLIW6
zulDowLhYO$2Ur#_y9=M0xqRH2>^^;<=@lJmt4+A3%P>2;Rv8Su?z+KTcz1UTVEiTt
zDTkDMS%K)T@W86U`)*yX1q*PiqDTtw%zu^y|NL%W0C)<o<KGs%0Nx=5@Snxa@ZVKu
zf<2>*hk9Ky-YUKTV5EUBDbxV||6S|<qcZ{j0zkNp?N28>&x`Ws)$+(T$K>u0DLD|k
zw~BVnmxf;eij!IukJ^@-4YDedmk|?*!o3#s=YcfLWPkJNf3i0Qwj&$lb)nP>yx)Bo
zhxgj!9tC)C{O%$~n6X;B`xv(h`2zSx*z0w#vc3R#=n#DYM5%ThCIjztfXRPx3hrJ2
zy})mM<a+_=6@<P3F89|K0Qdi+eOsiMb_#cpX-6+)ZI|Kh=ADN1hbQ&fvDpI}7}~zp
zF#YQ&;!sUBP0mjq`Eq>*AU-;Y+nMaVvCrs<z2o<IkK%Xx9gk(etIq$kpAi_KpGQA_
zzxAE#(`J}uvqoMvO_*a(DDj+a*JN5AG^af(u{O0@oG)F+F(RUTZ6!tocB;YYIgTnk
zrRQ@83#K>vuigHj#-Crm0AdQ(PaiONRbBuEvXEb)zW#K_{U3b?B_#J;sM<rl#mPBW
zN~WttC^6Ys(fUY9Q59)lv~c1|+?c#<s9g6TDT^x$)|T0%NNbyf4{&i@3Cr~y^MWHi
zcM2%IweH5fF2D!6rWPiQrkKrds#s;6$-?E;rD19ks4RE%HpYiK5;!ZH{h(VnKqhqD
zr98OlxGLJo)F}e$qUxn7LNjF%MUmU<HB4RrRi&q74Na4DYtgw)&5<-;7#tbs=a2gy
z`$*#eE)9_#CQ&G+hxqq85g$TI-TBH7KJ^oS&5crv5}VHPlbOb4uV=}{jYw)rKQ7$~
zT{)oPqIc-hg)x#tYVk52$K?n)VncYdtD=LpY@(jmm!ci3M^EbSpFS(;Keb+8_l^{`
zL})@ou)1se3s}q~CsfD3=O($o05<sVs67dQKS#08>)zqa65#Pa+TntP3VBq?yCvz@
zGVfJMXckE^Zy0N7OQ$h+X{gS=i<KJ0wQ^F@8sT7}4hQ;8_8(RHR$&gEXBX&G%1k+u
z6ACOCERKmf>S8u_X5!PVEZ4==#mna9ze(fP8Y%}{P!WNB-2(&beM2+(NQ|#&-jL9&
zsp5>XAaG;fXH~s4A{Mwp9{-8=;**dq6*Ct&b*dcX(=TVQec~I&N!mJ2cP_r;RPNC&
z23zY7Hz;&VU;D*kOv=JSazmhrw}dEjU-imCiT>_(@fS{9LjK#RXNioUjw7XWo#!-O
zJOM(1fA669f5)W)WSjq$gWBV}wKiv&N^$a*4DmM1^X`^y*S^*W*nV%+N!Bb-01T}N
zS0BI~!o1*7q^ToOVUF&qaV<kCQC%a<p)vhIrf%+vkLchdeX2Tz!!&E#o9?qNb9gVa
z2rG2;ch>>IU-1Yga;43IpoMKRrT=*SwXTV-#@?gxQ+0EWxJ@+zUHK*fxd|*?n5IYC
z${4e1UzLVjCK6MII8YkZ%<yYDuYRdZhg_mSeL6qt{xgY7U=C)Mv5OUMw;P^NeNCO%
zwq!~hJC7unUa&{`hQ_S;H;{Hh^WIwz>LpEgfy#4{oT7J@rAtfSZaWl_M`D+jB<?-p
z(7x#mtlzaZ_%3YrTr6nkmCnKxitoZqdh^b`onk{mJm98w=~1}#am+bzPA6ve2(-rT
zvvoAI#Ei4hc)(uPa!HY{^*vF`QZQ_u&a<INKigNaL>I95I2UPn{%S6$wv5l<oGv}m
z+R@aq4u`hY*tHl=%m`&aAvqu_$lAffW!M%zW)Nex%n_FqbHdAn^get;tn*t`kNQf{
z1_Gg2>_)81^~wqzhUG-98xqR=vX{j>d)rq>&2hN+3T6mI^_X;eH?od7OJGhL8iQ5D
zfbrXc^$tv?NfHm0zGV8qWN?&)Ve+I6OJk+y3DnW+A~vsELAGUntb8=1^QUIGQNGY|
zd_kP|E<1oL56Ql3ZM@l5I<G?Y!;kG1;4tJhr3)?`+9BmP`)FO(d_xQS3c&`Z1s30G
zBgoUI#L=U>F*?EYPeT9oc;5fL?h{uBwo7wQr@CQ(N151}>Aky=S~{muTK5)G44lh$
z`g>CEB6O=GG1%N$yO@HZv{SC`R0SF7vC>T$6bY*OZ7vZS>6K<Pb(I~nK|z+(6f-Kp
zRQz4?MRCe*dRgtXaqLMivkpjeI$NMpUo)RSmNRPZ^Q&`=8jDJk*>gRO2x{_lbA47%
z$68QH{}q@pz^84>sXSVPWTniMxbo7N;=Lhtvvrwbn-bP0Q_kT9&ceYg5gFSo^t){`
z@UrV|@i~7UDy2LuZd)fuj3)Hd?2NCNSQBP8qON9(YmE_x5tSlaj_?KWVDLpy;Go>d
z2oBQ)w7lDb#kK1cI5Y*O!t*611x5|h<pXTo(G}3Mxu-CF5)8^NgIYX1Y!3IiUmVgD
z==7Eu=w=NfrH<9sk{9Fysh!{1;ww@RZMvH`*$l^jkbzUiDa*qibZ|;lii!EyFV30=
zL&hm_ma0!WxoEWcSRJQh-Y-KxZYo@RXNcEtTbsR&nKF?NDbi8C;G*5XsXAk*VU7oF
z(AXGXEo3W{xBNv;b^R@;oa&{VIA~33Jln|W1Q~Xdc>QF!K6C-qn1UIjzhd0hv|ehm
z$S1!dGHCVF7J11Q%i8RinZeqa7E99kFD}j-2}v8~ICXBEY%XQTq^isGmwaYJYc-aD
zK4aH{#pSUK&Sg^ML0sLiW`B<sjd&4tb$dDWk|GKDP&<`YV%L)C3_8v}35A$&^35P-
z8A%;GL~GG%1xw+DskH248LSj5CV9J5^fH%+jrXx<{VO!5$asxq#__tSi>f*+xfZ>E
zIRt%68#>)Vz_^{RH!8>{w9wBFS5)gc=QDA-aQj$PGU%HG&L$#?shij$z$2<%>6k{T
z!&$~9#2tN;N@Af7yI(P`NqK7%yIUbPTs>5M;oxZ-3>GlfG(%F;7&thqPd1lSnTy6N
zxm{@V-AHT!n;Gfk@$U6^4I0dAaPWpLjlw3U_X_^%?MrjXW-#A}sO{x#3*>t}4;1#c
za(10@B$evqYhP{6QOAaBD5Rp-ntK0yPXdvZou<1|ece`-z>{L`uSSwgp%N)%gQN8d
zMnwc1!;lpDEqJIU=iW>QV7~Dx%+|a(P9r>fP85ec`D1?a$Lwt)+>_T3p97R}NY4PT
z#QD4n<KkI&+2a%}>yKj8>lY_T8w^Q-d#`*Ygl_RDisKBbFO6_R_tPGjyo;Vtym9lS
zqXwL5PV5`aghZ9=_p@_Vjd(N1WNGT`;vg5xICbKU4<e}+leWnY2jo*u@l+SnVeidM
z`#2>gGH8=!3?G^vuu5*~qS^=@bB{S!5N|Z=6I;tS9a6O86+i^=2_v7$_JHtr)90#)
z;=6UO$#Gff(J33;aG)@=_?GJa^^Hm_tr*<MdFLRk`(!m0WR1OiOm6wsRD7If8TPz{
z-L`BDtb#xOwu$|8fz0C-F^+O+({hZo==&U3)`f7ZIo2h8E<2HnE&p9BRq1lv!|Rwc
z$M9xN1+AsHbCz6nk8acC+=M8WK~f##<ZB-C+M4E2F++_Mjlts+I(X~r&sht$t@`8g
z>+4pA7;MGcsic|f-1Z%0s!G>!GhB*sISR_A-G~ZB5TsK99KoiA6qLiFqfiLO$HeDT
zTM{x>xq8`I@<S_Q1q`yA!00P3QV&+-D<{b+(h2c5!*u-e)9REY)TE6~THLdH3mU{9
z5JtZQ!k-WrDOZgg2xhRP$VYG%$;gT+hV9f8ahxbBY^6l;=8P0g+cp<);BK2yi?c79
zF@^3Fv6qb$O+_3i+3xtp6yT_hrE29Vz-n9DK4{(&%p1f>mke85T3WlZjS###N58Jp
z&=RC4YK%E~MJ;I%D26wJTAKwLv@|5&ebtW|FB;T9JL4Q^roCNKBtkISq(;TT?KC?X
zAz>e8lS;aw({hk;GNb%uMsmtbT81}uX~I2LCE{sMh9BJ^Q6Km#qgF6)b!^Epg01K?
z*{euzn7i^1oD*iFzo-gX(;t<d&{>4%$;Krr3SJ=OVfayTDLx&|;JP(`%1yb^I~eT~
zj7cqnQqZ1EEBbE#e4UWd8pINFZ7QLq!j@1iCtaW*uM%fUfwNK?lONArHb||E?~&m1
z7ref|z6w5j%UMs4Hs$oOyJ+b?w^2^%z9B;V(@cEroKrprb5=uR40|#{3>t~6Op9hB
zwth0;#N^*)gur%R04Ts9vf|mzQ)oKP3t+0i&jf7mzl&xI+<5QH9*vT3pwP{_fb#_h
zHg6u8g#hwKdGIbtL@a~@x=DmyntWQhf;?e3sh`7*-jvK^K|K*YcUnVUI?vgSfx}=q
z;bgFVunj4O7n)b{1pvJoYH9t*R$|)FaHF2L9Z`G3jW+1QhDH@inLel1=*y*d=YCo@
zYKgqku2j8#eb<=ETM+*wm?BdX$(}*o+%RkFl6;n8Nj8YBr$27AlS)n5t-V<^A5?;?
z7Bokm3`|fd@_K;k(Jhj|lzE&=!*S!UF~;H&JXvvECk&8#<TW`yatCp?%`;2K30>8!
zi)iJbnznwt4GvRyFlx+^ZwWz|8gzCfJ+B_=vf6ewsL1Y>!6@D&%5mo{UN6?ooZJq2
zUBMj_&hGyLa0^n5k1Lji-5zCwMJw8n3y`qsY8sj;n(Py-Jvlw?_w~1`bJ#wj7D**(
zGVMW;L1f)Lj;HtQ;>K*VJnR16+3*H!mbJ;!a;rA9pJo+Fqpq)R8<?mf6N4agKgLmA
z8|xKanAeuO{eC(`*OmQDIjSxzFv7HH4E;>0IL~xQAxjcTrgW>g!uWmMHcXSCW%Kl1
zjsjfzc(b4C$C*57N<n3&x}rz9P-W`b5|Q|Qw)b&c1=!Saby+mi{!{*>moESVFW>RA
zWG$;>)|N8RDHS&qc=d2?!iF}Z`bb-1_7x18EMXJ)A<GfDJ}=Tym$@=hF9VvzQ8Sd>
zg^s!s9^q-H@S?B2JG+)5Iq_U8=$4}M4^)eb<H*~?=I`x%%?O5)t(Q~^i&gAQg~^X)
zlvMacb{4i|V^UEsWfy9?DuAatn3*<V{nzoZQ-*byGE2aeY^>dc7;!6wQgdxXxJDDy
zNTf)kU{`iTyM=2x&BFSD4E|6$NrY&w4lOYob$yFYZ{E1b@Wyp&bsd*<g+yEvhS2s<
z8kfkz!|}YZN!iJGLP48B6Maf@s3+bz&3a#Y(OTZuN4m2y4Y<How-O%gvd34}PifA0
z8idF217%?*c}?NiImu&cR#kAchp?`~_pW*HB4{X>i&-j2ib?FLRPV50>-L0OSPukK
zV#B>0`AspnzS|etG!}n$*M5~pXf9L7hE*pK&saw_S(Z0bBx6ReSf@JQY!hd(TJmTy
z`zX4J1X&jtyRKzrDXR8fq=w@Sfw+ZxO7Oh&QdD_a4V=Hi*;A-nHxybh;dY_<{9Mn_
zn@cJ^MwbwAOEb=7<zht%&Fbq=MpKibCN{H#*7&+{Q8d~0xII0}xBRt->%~*ym(6nF
zJ8>t|_x-4%0grCC97T`j)5pB>WqaBr$$gir)xx=p1>RPt4lSQgtZ>bPaPOvR^*Fw^
zwTRm1nq`>k)R1WQ!}e_%WgVF**~Z!>7f>qa-;qgEwoL6xq?y?laB=lJY_oC6e4M;S
z&(kQH-pzy6ETXnA8pS!pO+5gLn(=$ai<PShN)-DAehm<tE#AB__NFD9tfk`~+FPx?
zo@Jj9b5yh3ot7k}EVVAEbEV5~B{ka7a3#PYUpa4~g7<(IikGOPKom;NovTrZ;M02g
zGDs!DKa?y@M;pZ*W8)jPGr4bHyKS&sKIJ?)YuUtR<veP?lM$kBc@@-)z|prr=<A4}
zz9}BL0oP_W*vbMw=X8UaLgLG1?N~x{N79fqc3Z9&$YyBlF9us<yYA(zUYc=IGz(1B
zZy(F){jdu=CAYH^ZmN4AEHPFr!z5SZbWnkzkOtvMHC2Wi=GL8@7hXVZI!MDMQ^%bU
zwq<raoMx9U{2=uR(RMqyAzseEPB3FlE>>DrYCV#Uks)rVK<?D+wxff-FPB?AjBw%d
z(ZsOHm!Eq%c|%cs9W^UB!s*V{7QI+Pq4{Z<UvITKo<{kji04p%^|hDPVhNN;n<G`P
z_;e4qjY@3K>~^TB1lvw=p@@728~7%fgf)sZcYRD~ipxj*j$F3Sgwd;ztsLtaSUQK8
zF90NW9z%~Z(`UNogsI+p!|MQ2b>aVuz4r`iYH{1W-Nk}#MG=&uu$7w7lnzq1D1ihC
zCM1;5L_kO|AfZd!TfqWEG$gPM2?$9D2?PW}mGYM=p-Gp}qy(f_1B$TU{Ll04InO)q
z`^-7#+nLE^)`vAKGb@>_`@XK<b>BII@;3dm1Byd>dwx&U-_~GB2^F%BY!%<xR1Vj>
ze+)_dP2WusF{2Q$W`@3dNp?_qSs56COIR;uJb9|;(A3gP)uTc1et9~5#mNlR7KP=I
zSXiAum@Yv^qB4_^fa!e(hIDz{+6Zb$vM~whjXN6i`-a7zwZZ6Vm+gy?rj=e#tKzYn
z^(RaFXRE6LI`%>G#3c^K0Bnl;=CBkIfl9ITr;M7GP1ybPG`OiT6u<&1I0~3zo9rZx
zF^p(D1uisIS!m^`LF9WuNZ+GylBx;GTLx2ZUSf~RMiEC}bmOgDz<_~rQ1v{7KDCz*
zhR@-p)Os2ACA;8t#p`uL<tiKJo0jnMCptzmGu4xYAN@)yD$*y6l)07MlA&UJ_GpJ&
z<`?XJ0)OB0Kt)^^3zNJg8~G4%kp>Jdq?2nXs!1F;UW!g+ePlRFFd^YIORlQpBV;uO
zTEMeEg9;9Oz=9KLYW1!VlI-!Y@o!2@$GRf|m@0~4?2zag3^~1KaZmio%v1t6cXU^$
zqfy+?OR6ZRux0<1B$?Csn}lJd-PIy3Elv8*0Z%g#(I@^U+aT}HMFq-^GVHT5spIBB
zKKrRi{iEHBxPuj(F|MDe4C^7&Fa%hbPJwi5tpE&Ab(NU8rj!E;b6WtN-&vjEQO%t#
zkDr*pf>g_6Fza9ElT3|6nCQ3dG2Ox*Gor(Zh<a|)96(z`BAg(JFUG=MgXg*=b)vEm
zn{6jPEHy7P#1K!pd5pDwE{YILbka$K5z*Z+jtOoj0F;@dHw6c8TQ0q}jN=7r6XG@n
z>5JWf?o(hR;9Dr;yfTU``VD{EYiu~V!d<cOJZFA5?Z)~l!#CjQkIVNgPW?KdeP({E
zsIPkznQ=4uN?l=)(#~zW5TVyx&YnIi?Kd}7PcaAat{#Bx&J}FKoxWrwg0bE*7@VJT
zWS{s2Qf{>8c5^M%SK5h*)V?<*!zPrL`x{0&?6{^8ohlyVlJGXuiefzk^6avO&*V?8
zKTOxhWS!UlUYBPE`7%crb)rzR7ol?};<aqEbwTxY=*Et@Gs<geSGSUZ5`Es|%}{+`
zKyM6}iYiUibSi@fqc<7t^1l9ir9nI41n~6+6ZLdE+{k9@)(%$FcXh36b@402{JQ|V
zr9AW_Zk^PYtf6rmT^dP)lntDVwYS8t++GZy^sB;O@~yApmQa6xN8oR)1ZgQ&zQ%S(
zd?;x>up&@=;il^XGNczHb&&!rS0PqQ`W`C)H->q8sPk_>_DH#y1HYP~#K%(w=ceh^
z3oI*R>ny`MZhr0;1crpc$ANb-GgH(_e2&#eJ*<Z<C&al@Ue?a3$zdXvDth*dTjzb^
zhF}619ybnuuG*8w@8=4`b(LuL;=xS1BJKFVmtLVkaaUrqxjzcCBs&i(tRC<+ZW670
zbP3_<QK~OOBuJqp28o6;2UCSmd0-~jPrCKVWvR=w6MJfdEiFzIw~TVC6>zM)z(n;+
z>f>6OY4Il~y{7TnmhWn~Nf+VoO^r_ukt_Lj`0;J0a2UU?<;O!;CCkGvru#I(mnsrh
zrb?o`N?xcM3&1sDN=r4|FCb+4B0@EZq^xf!0|rVnp0Zqw2S{%E##ICOhot;1J+>j7
zEQH~~5WVdbA2zJXvXF>oSyQL4Y;vHQjpjXPP;erU0gAIO_@TF(HgocZ1B|Ktv@md4
zKY--I30-kKUp*K@5DwiFm-63oT?}>|b<G`jyCFQAZ^t@g;(APLb$6jLR1F6LBSfMB
z;xoxg^#$hz38kA|4UC)M0MzaRTDpyZQUa)yI-(K2n4t`Lo9UE@Dk0hQpfG3N_r1UF
zl;U)4e?Xo6G6(pwLJP6x;*jY3CGL{E@<w@mX7F_J;ax`9Gd-e<{>fjOQI4dri(978
z!-7osk?D=bSi|dwSMsCrmOrbGC>QFqU2bp>r9!^q_HGl&LZ_Vt1{#uG0Z$P%U=)E=
z4~QL1vX*0)B~-JtP@xNV$psyNR%{y~5CGqwHCBEYAW^)6?&lm8>wPqrPlFUjsf!=q
zZEKnU{BtOw$8CN1*$=zog>)0#=s5a&(NjN$=VTMzeZ<2y*ekebwCw_}EBaE>Ec)*1
zY3Z8<KLte90HRI8u0oQ8p-a;RxUC7;6M5Lb8YRHS-4eL>)#Y;C2;XA^^DHUZAQxT=
z+aTwHnc6j1KFDMAtRpNfwG{7n%qc;Hu|v2(C<V~cYX7lb(_jAg;qrn7uK20lr*=bw
znp1tX){KCm2WWy(qEgblQ(q>kbkEJv5Z?9cN4Eu8nmzi==~WlFWYZ}k24uHqRI@PX
z;5HxtLr+s?q<S#r>%Fscw-cSq!O_k&h(!lw^?s&F$$Uo1y9C)pXG9$+AIMRf5yK5U
z4h)T29GIgiEw&c-E3Cf^=$Q8X_|Kuzkg29Txx8Vk*+VBbEHd_~`_#uFoeB#I0h6<-
zUzN_!5jB;OJ!21uQbs>K`gD>*NP|Q(3IR_qzBcZ18TBAhg-j6uK)K-e4k>pi_lhQU
zSO(K?G9Q$WqO<;jU-@sk(EkNNXX@H!!S(#BUS{#=jSWz_Hv<EfxP@h=Njwv1DB|8o
zNb$`39<CTI{Ba@Zhk;+?<ae3Sp1s}4A6ADH)<?2W#DDSaw^*Br-;g{M-*hiV^PfXG
z8Mpp9)PNWH-M4=di7c<zD-WKy>mD2!iki+pj{YCGM!5WueOk=P4dCvV`tw7DVRqLq
z1w0P=HQ~c0>zh#qq@#ChOIT*f`D=g351ay7Lk^+-d-Ah*_79QquWa8mDFj9q|07yU
zbMlS&oB!fY{_pCa|Ic6gTjtc%KZlYTGnW?fBEA1PbbZL~P^|8MGPM-M;y!-ok}X=q
zkC|SO(X*ETML!5KHHarCi5S0IA<DIZ4~Y)nZy#R#zqd1OYVrq3TPFUKZj-|IQ)PcB
z*nRxBOKWAibRVzx6a!-Xz4_o!yU~U9qk;av2((Z9h~2P$R<3!(V10#A@puNN+?^d4
zmL8Q`VtenMQ*D{M`hBOe(J0&v^vUtg3!P=2jT@J{kF*UT{)4uQ3q1Jbp<1z?(d>C-
z(t|Np67L)N++f7T!?PnTY-%R^a_fob!YQa{$EQ<+8ZyMw|3<<1_Dv<DH}iU0T#nIF
zkL=dIpg#Nf!QyuC{{X=lwZ<aQGfpG~HR`tfbLdB|2+ELkZOY!7pKkb}XzD3qKmSdF
z{om96KW~WmAG^^T*KBsPPY$;JIqbcswYF>+?CI>eka5~AHfv(pTDgdUMdRLaidOQh
zH!Dma44LYG9ewkkjNtzbh{(~-?~^OBpT)&5czAkXy)Dv|Ow7xJrT8d1!M(>zzn-dr
z!gJ`s*T&sH)wOU+@1y5Zz%ns2YVF3VJ~UZ;$2g3k>OYi5BKq?dTDcqu1Qwj>kQ}`S
z*fN1R5i*o%#cSFf%g7>X=(n^t4Ka08*Z0w~wPc|Mcjs}rtvUzNI=yOBc;9D$Cb7Uz
zWR7waf%W0UouQ$(i?4Lsf7Te1==mui6YhhoPRx#KWazDknIwg>qP4^IV>wYcSl*QJ
z_4k9UND$KaQYV>6>rkowJfm)?EE`{2XC3b+1<?u%GRiB4o)Oh7QJaletIc<J7UN}p
zI88lMmRI~3Iu6_n02FgqzR#A9)bN2_QCOCH3r+WvN0YzDv|f4npr-1ks#V?Xq)l8m
zl;S5{j7E`+9Vq}7Plq96NfA6vOYnok3gARDDk@Y}8AW-+z--X^A@cmtnL;mAF3tY6
zHPw-#YVx2!%JA4$>)G0c!2;<$lZ^QH!cmk$>r(rV^U*UG10Yyk=U&B^lM5$8=98n+
z)mFW&Yxhh&YYfUp$LuXe8iLP$m~g9i>KT4!dy+S$R>cd<HYFz-@r<5(VGu0+p7y7S
zHc)zjOGAP7{m;c8s`D*^*zn+4q~h|l09Znj9!~LZb$h`;gCGRv>OY6LEZAH>Wy9=-
zux_w0)M>-SdDn8rIvHNp4=p~5SIh)$X(eQNnIxd(+E2J-mn9Qx$||P?sB=W^o*v`q
z^C11Rj|!LQbL1MDC2|HBQ#;Y8A4`3B#$(0m89irNa6Bous>mP7Qi$pc%}g;{0x@jh
zF*||>6lJ)SvLr*?OWA?H#e%2iS+uRVg4bO(CMxq5!cPR7y(vxGyLW?b>l=NM`w|oJ
zkj_Vzw`Tk8?Y!{hqCM{8KyO;xb&Muz;|fe7qxMJ_*p|60e-Lq@W@0&y(jTS>0pjaT
z23fabL>U>k^Cpy;wbrXCwB?2<01Kzy5?zpfDlp84LR51Q)%N+D4Yzi6H%@I$$Gr^e
z7!5e#OrEyk-0S=5te<(}lj5<H<DBKwqpQ!ETW`cPPIq&Dkx{gKZ^e`EvRs6v;m7>s
zo#c2BZEIz<fxH})ggq)uv6H5+R;Z{IFw!zo9{H}V*C6*BZ5;tsPpA7ySHB0*_wSYH
ziJtwx%r~5gxie&{J)>r-9me2WJY97htLL0!sJo5o&pzsF4-6gZhwGhx_+-ytRugoe
z9;BOa=aRDIA`ih<71MsiZ;gM~>=?HtsMOTZx4tIww5Z20?5#G)E8znsoGR$*iX34@
zlhpdNFT0KL6r34wD3r=#_mv^)_P)2RC{8JeFP+`>5JrFbDkpX)X<6%4q3OBYS@vjG
zNYus0E~BVhosQ}wOj|?$dC|V}t!Lv>UsFGeQUGmMA<@qV-74smZ$a>wb>Rb#ubyaI
z(MO?TMh5>m<m)!b_2417I}KsKj?EDR$L^%f2YklBNQ`qCHGRL#twc|+J^yiS`gx2+
z2o252b#(!=!SSb9`D323j{uGUSu>T<rZUsv>iZ0$Ax`WNzf$f4s!`POdE{yC8BOhH
zrX__q7Ht3@^UrxMcMS(YfKE_$vTwJ<9L@GOqQEmd<24%{tx&KR&=BPw>lLm2ZFhR0
zj`Xo!W}jm!HgoiertGG0rKrE_>Ah6Wp^d+@{Cy5{zy72(1W2>LYq_Y~WLExHb=4E+
zG)^PTZq&cp1v9B}(Mj4Wz$7V2c)&Z3j4M{VYiyP>1;72zq3{_Qz3OUgIKgdD(Rw2K
zI*2<1qGbaaj@#@_s7_M<y3>mGl)`iE!<C7vel;0zu=#t<ZYulJr|~YyAjQ$^`R>lv
zXw6Pb<G9{{-w5NQ-_BdwJ`h%l$v$9p0AMg8$pS4oo69)Lqro)+#_%+XMKo0Rsm7FY
znA-S+-t3es87L(s(gy=6*(2m$%OL+&+*3V@R~SGsda9?nBm49M`2}tIU4Y%su|EqJ
z`?jyg?4&=dD4`ddjCc7mEiwf4bCUt`T^xe7ekM`TGV7`_j;EjLR@2hk?9|k*d|p+(
z<`k^dj-}iYY^h6)`aBS1#3Oq9#RXw4@sX<}YJplgyCzWRcQ!w6SpG<F&K`&n%y$0y
zW5SEIKQ<0?H#M6dNi&{I#A+GORYviz#dBNaMme3rp)R$o#R+3qs19Dv0u8{aDUE&O
z2VQyLvF+!O9>>#xAl*&agiHnuG$ZdLI4^&*y82r#YjXnVfRvjh8HIq57=RHxVFpgm
z5d+!hh}tH!_R+{ELtC(t+A4$Zx4;@JDpc0L+B!*Z&bV6ZQ*a(`jJ`|&M`I|uAbLY6
zHuYltnF!kPUO8V|og>x0R=MJe@Re>N!x^Z)P<5pR#}r=^2Z|y<&9gZ&Vzmoc<TkOG
zT9V@Hng!?HSpYpCS*eIXg;~Nq?AnUm=Q^#?mC8z4e34deX+RX$4yQhlHRZyA+nL)L
z2j!k;pwy2N#=RS=94pGAR)0#OtyofG7ZAcEp!rT61jw*4l*=E$8?b9|<@R^?OOqLE
zFUCr!F3}}<G=On*MM7p0i&Jb*);g72D|K_B8nDOM4-n$lCz9VVlN&L^;S!#0d4F@b
z?k;ON)r2Xm&~2f$D~MyD`=q6-)(*(E4J@+_#$FiiKwF+b!i+(%ozDY4Gs<13{8=E(
zZK51JWy;8iWr<M=Ly~(xCKLO2+)2tVE|6h3Q?1?hGmJnHNPF1cTbqe{?v;1FsN+<%
zFnYv8I>+<W?q6%7QCcf1WU?#fO5@e$EmYetfq#@rEYv7e%^a<z0SYigZ{?$%g98`&
z+<P+s)h@}I{PYr{5Jo{Stxn()@`yo!@!>ky#syrv0olKE!*k{2RL`-|1=+_Z?Ga@g
zH%KL!Hhp25Fq_lh;F{}R0S`6^uil;}8Y-=MiIWZwymU=Y8JA}R(R6_r2(m#0S{aFc
zQ4ARnRO^(keL4)wc!+D(lvJXrHG~JJv1Fk-2_ixFY=*|b0E!&?&!MCls8e}y%J*=R
z3jSxSNc=Q<I_a0Fyd_7LY{#pLs8b}R@CcvJR(`CCtW7~fWab0J-EsdZscMR{A$u2j
zyqxfOS5=Q?fwr?lFiu5PRk!LfDfArhbXY0n4)>uUih6FgaQ;{&EZVrr2$8LUAo>Dd
z_HE3!+Wj(ByYXpB!oxO7Lo-+yd(Cd3+-0WH@l&AcV_R-*I)AJl=2vAi72$L<+~+~{
zmeQCj3<l=GF?tKkI43xr10`o@@WYfP2Y}#c?%d4B8~|>!pxRK{%9V^oEuP+Gc61%q
zLJYVquv`WnWyq<nfImMg9<{OUsLo4W<|w^IqiTi0RxzhX#_i%yCVy)34pq2<kr-1Z
zzEc@_5Z7aoQF{r{-Un`laWt+u?2i|;FqUHvJSV`s3om!s(U7<<TfW1bfGEP5id9!3
z$<0=rAS-J!Qo<?bt<!w}7zUQDk=ytERH?q{UkBHA?`nD<oGIqI$u*QG|MH+FE|-s)
zs5n#>>yAB@c5&oo;m?C>jv=`A0KtCFkRqa=^TWI!bFgMnHYd#LAWsaZ5tyRmSs^lt
zv&_a-9|$~RjkSZKn)pVo*(UgL^LVsf8x^#{f(DFHlzuccXLG572E29C<VpWw*~g=D
zR`s>D6&q6Yt8&$O_*)YENI!>Cf|Sk0gW-r+FJHx&S8i2Fb2hq74TU^-oItu8F_x7!
zmq%RWpGC~jeb0lVp%1&pL3;C+3=Bu1;Jwk_plnU*2)8gNs)<^_TV%W$RH~a3nVzwz
zWyQlU9?(pGwwM3YCvEvB)p7v%y79Xam2JDaYf70xrQpg^CkeMMtFV4Y!nexYuMZUr
z7P7hZ)aiW-Sj{@;NwMYuNotR7Qva&RUx=&7R(b`&vwG+q0Vl43M(^$m`u5wga74P%
zMswGc8lrW?sE@hglQ|pnyeJN-RGmkQkk)?A@jb?Znn^B%OnYZkqIto2WVhaV7F7RI
z=Wx@}&a}+ZfQdXdz(Gh=ZR+96H7T6oDAm8MB8*_~pM_D2INDgv10Jts6|1$TiRFAf
zVZ^%<&~ZNN)5#Y;$aJ%btjpv^lP+^is84@5CZ_mxBaX(g0hS~%pc^~i?!B3vQjZ!?
z*03&lgl7+@zWeoChG{IBWoH*%6guyCni9y60Rybe&h7jar}IVOjFu;~PkvMGL(cij
zldyh$Ghh_9<5$CT>x=2(ICocCsZ!dgM#;_1o24e)k|%Ai=(7hMFhb4T;|Rh0)0R_l
zF8Rhvc?$!ucTD|@QKY`)8gx~AfEXj7%Fg3#rJn0-S}8?N^cvZHI^(;3iX0~O$j)*R
zox?3Fd;Us(Zgmd%Wa9MZnG2XcBOTuS{VRoOS1uhh?ZX91`qLK&u-CzxZQ}`xPzM4$
zIMD>Lh|Hf6Y5Nk0g~9wO=tDYz1i|ND_po!R^U0BJfxwcf5JI8}#v#ES+yZ@opR2vM
zBP1kGb^dc`BwnhpVd}!bJ^%XF(BhSg54$(~G%uik6Y@pF49CAMYIbpjXQI;V{iNuX
z@Ug;xU?R|daXV@p8R)-hUcxe}0*6>aZqr;gD`g8lIT2aF+li<&r3LFzgaEMu0EB3Y
z8j68=Xt(JLjyg|NTl7O{nUqUo7>jeBF6%8l$);jK=Qx_rGdsD1q9DqRP=ir?a|r#r
zd&^humx3B2zqj^!dO>xUUn!gJvQvMtno%OMKxbL1GFE%Ufp68uK{8PN-xqiiL}L}E
zOjY+O@fg!{D#ZiSW0(+2F(WQ<5r|9GUmkEbc||57Rw?%jS93NjTHo1=?P8i*oZi|h
z2x}cvWi@0;UA3CH665nusBmf=_VV>hKhI51sL3?aOrtmJ%O6@ewZ{9lOxT~!2k3vu
zUYoMwPFAsPsnI|<C6>N&DzXUO#-q88dRcJEg4DNR6o<Kh9i4)Eg!u=l>OzvD7|wcP
zHYi<UcsFsQpy{7OCjvQo<FRY$YmO4JNpfb|hyjnO+@I(#r1}e2le=HddH*~YRMw{1
zmN9!n<N13T02J|iCdF$-R`Bq~>+3vW8H`AiaoKwj`i--4sq2eT%>&_P=~JD=)q2DA
zmZeaEP&+W3PK%InJqzEbnW{2IQ4_C=S91rjaih{Defr4@mnz0S_|`q)HBP%5-+Zog
zU;oz%UxM7T58^>4ZFB-tRm><qzcG+TM9M>+^Uex)gr<Z?Ar3-Hea}wDEaoPCEGjoM
z<|`myX`jXdL8QE79fbs<%772;C>nBg0%kv=Hx8L}sTAdc?9;Cm*yi{7t0oTFFp{jq
zFTDDYKx_SSLy-mK-2nXl7^%jiP)(k<!mxV%c6!VG_K5yoCff%DK|@a(amna$Jt3OY
z|E1^H#`)Hh-i-%3j~jPT?zG)F0k&hY7XvTvD)X?>)JUJV!|E$ukH`<I_$znd9uKr-
z1p4``q(cD!o=;J#KM^2vuy)2}uP1$s`*!fcGP467dWl`obCGR5(ny6wWEkqt++(aF
zXwaDFAt7q9cS&GllkmI(NEduZSR21*xoba_*gO;`N=7~}yyTiZqRHKd2v11UfAfyD
zoNMiyp0t+$mRSo&?&$ok^;ZVDTgQQzbAG-2+~hk2|2_gk=A9*yOw<Hv4#o{7-LAv9
ztytQ%p4nxv@xz9iW3hBvbR%iz+8_ZjT*e+-fiN}gPaJ&wel)qfAoVwA*QMOm?k^}A
z#z+0C<=0)c5tj}oh7`P$6fYE%+P;1x@uT#{+;#ohvM?ZyKUO@^=P~2<yjr!VW6i%W
zwRt=@Ll?Yd+zOPiC+lB=(%piz!wVVG@6_LU7!o<{QlYbq<0Vh_i(4|)vrkX#x+qq;
z=p~<xx*DzNXsq;8V|Ga)8N**Z{pTNB_q79kwo+dSV}7&|bfMJyR@utp_s9?^L+~|u
zf<&^lxU-*}fB{bIS4VlRgcAfTTXCj7(jXIU*(*F(Zo7A__<kLm@@64A13s5s_OIRs
zwD|FWh7rs5uON}xLY@n-GpbK4x~WVad!_TixBoV((950dDw;&@=3sN7?YBbe>13Aj
z@o?-%XA-;Y%{U1fu*r9N3T)7;;~Yste%YD<0MR^@vdhPZ*Y<oeA5`psq2FCh!qOsW
z$Q!PWlNW1sXHHt_+37^*gaps+r`uAm!OP0)H*tOOexC09>%J`PAN}HQ-Q3nHovQ3U
zD%z-=;9HCGr{3mHt33p_itI8Hvpwp)@#bbL63}XA@)FKR_ogw-1}GQHlPEzC17WpD
zs}76c`gmMk_PcCt)}mqd$k68aeM0<Y!?cdX2;QD##fN^yP&_`rTITK%P(W7UKHhjp
zAetTwz`ggt3G6mo&{5Y=!0+!AArFLQcrE`nGQ_kmJSA`?%KVjUWv_zDkE{Ev;Z}46
zYw_imSN;!u$!=pF{^ZT2G}M@#M(`*)gB)rS%lTnKYF)5&y>apbI(JV`TW>Z0+J#uk
zh1qX)yABEYOh>$*%0OMBO+nZ7J(W@BVKpbF(RfI{w_G>$4D{UNOuQ?+0CE)yk9-E+
z?BzP}>3t*<Uf}W5VX-P}!j$#FqD0niuEh%2Ykk(zbxC0v55EZ4^U*#_)mrSH*!TKd
z{aXb5k8~ev-SWn^pa&cG_6KDG8LGd|tSRdeDSqu;%d@#Nsxq7PF8w5qV`lm`tLCfJ
zu?5w8)kVcJhEi$;9_V(6`UtBUqMP7A@s*m`Jv%M+wAt3Lt}l0hGn3eOWq`}EhrA@@
zl)l1*r{f@h{~lGM-ttbp;lX9@g9mPW)atc@34^+s4P|y2T9xe-OmNd%1hR}Kom<@p
z8PO=ndRo7S3xsdRIJaSb^#}N$Ly7h|^$mhiaH;?Ald%!P6UR)yIa-(>{B!6-&E7?N
zbVHQI$Xt`8UAFmDI{?Q!-5b(ggf@2ueWdI!xQz6qjldoE1yOyB$gM4{4{fiZj%l+=
zF>h1##s$&~a>d~nCi)7C_i9cTIQGp7;%?C51`R4|urFBCvF3Nkt2t)rREsBvCzCa_
zW-2Rn^=F;PR)2Y&ZNJ!j=DjjEqVNvE0dwia)aRw(vfJDlb7&Ok0Wr#fIH0NAB?f$%
z5^$a6IWq8=;-xZO3f?mwu66TLKHs6#kE7h-#5q#j1~4I@t4MDXgaEmx>NHU6vN73Y
z{!{<`+UwKcRW4o^dFgJ|yZ$gW2^DOP#9k-YA+I*2;{rH8*)bh{DMgRz>#L0mZd!u}
z;}X2rjLXH*?WbI~27?<kr6;(vjAND#$w`W9mdNy%2}B>k>gJYxT9(^TFrBgC7|u9W
z)$ITy`fLU^i1KSEoKY?L-C;fzlw0l%Z}nG(k^}ctVj)-aqR4R7$w`)GuHNrsih74W
z$iKp9wpg{;Qh7i^GV${$d$KfDX<-15n4<#z4OHFIM=7gn`AHp?F(D8Vf$4ZX%6y~~
zLv6EVYtJTs{N35ze&L@VV~(=<{)Qiy4AU@<xXBv^8M&D6oa!g4i55c(=}um+Q+S_J
zjGl*=zkk|Ou{Eil!ge4KeZiaU5S?UuTh86|jIv?t$c89AGc(H7E25eQ4ARm`uA4Gu
z(nj*HX(xfVKspJ2YN(C#ZIdT<p*wrJh^*D4c&~uxt*_Z&UmvZXxi??~m&>A0UQIHw
zS;}`C)w^nn>y$7A>E#LxEe+-zgaHT|Fl!|TirqvKDO1uwTpDFw`UV-7;#S#lY$yUo
zG|P32;ZbKP^WrZ~yQR$$XQZT#zpzuYV`o|H=l)P!Z=4HIIPLD%C`8M-wFmth4;HRm
z={)V`?&31$dOad!GdaN6*Y;hS#))5b^qYUa_4__e7`iH0ky0vLGvQc5sAd*X=BSrx
z3C0ZR4l)?D5};NSEG#WeN?@BIXP|~qI%!QdB89+GdLs~P0aw|ntIo_nNrTo5%aki|
z`}L1;-~B0?#9YRi?Ky5G!M|wg4k_9>ollnbIN7JRWn7eaRNE)#UIFiRUO&_@fhlGT
zHb(`gEOeg=yWrJzUOwQ9S0{m0Bh;I5#B>=o-bOl<7tONT5k!fQ1O?Mv8f$s*mDhjr
z@c!TU<Da%vn;O0WJw#E<w|-A7wlCiM(>(J(yyRbIi+?%jNERuB(rq(m<2X$8qir<G
z&0<teV`%$V|C0Owepa?5;M^ZiA${9_cRYJ>PTFzouad8v{|M6kuO_F(vky*+{zqD7
z8unyY;=i6=XZ+8J$N$AF-LD}x%V;-e)IPH=>KuE2A*8>rRMb%A0QHemMpf;YB6{KA
zAh{;?Vn+(q7|>*Wj{Frx+wz_kxnl~?Ozes>r|_32>}C8>TG^}-^a67PjfxrC82dtg
zl6tQuag{ZV2&kTNeP-HP+JAqFpsTm~Tk^&7TLRNtCE}8q1+FJdwCD&$r~?O;2uU5O
zk5Y}X(0zxPS&{TUp0VG<u%urZWI3T=PcG}%iB!nT3l+J<NhLF~hzEH=C>-<Za#W`d
z3mbm@abe?p>UOk$oZ8r%M;Enb>?_|sg+3OgHpsp&ehG;PLvxl;F*{(%6Uy(L8Jrr^
zJjcJBtq(UapJ5oNzpHHba|3no{_fEROYoLx?jk!mOFBtXg)(l#1h>=-%#GE@i=T(!
zFKFZ_ctghq1ZyFiy#-GMA<6g!DE=YOHeHuMlntf$9;I%GHj8d#wAd=Y-8lMud+)EG
zW3zs*xj0^nbG30=!G>y@-oRd)bhnZn)gPpGC7d3$kEsyCx`KZbX2p2#eYopKAFQsU
zFrfetN8_fHcpMTidIm&t9bkE2Kjs`(Yg&S}L@=e_Pt8^@@ZH!<B?@H$+0;J7RTMjz
zE_YVzPm~TpQW!F*Bw8^eu18*ZU4NAm@j2*B9_dJ>d=^h%-QCrr`o$d2rjyVSRj~VF
zYA$JYvb&5hp;UhLkD--c+s)qDnQvq$#ZnLmWRzni(2aK+Zz#(_g`_2#s8b>5gpgiX
zpo&{bEf8#|4?^U9qh?|X7{K6|UV8pu{$lGnawf?N*#(j3)mQJCN4|5b2|C?T^d&g*
ziEEcmlwWGoaI&A0?DcnqK9w?aWu4b4m1R-Gnh!@mpMHPgYScYYyVdokSE|wj&4XkD
z+#yT#=!Fw29mhV-)q3hut!0eWZj4l38%SUwMp{$6u@{gUw}PQ-PFfi5!X{Hxu2#$@
zQKLFnSK<^RMPNc-Zpqt?bIHEZ=-3M{ljBe7pwW44zCz!=Vf%woL$yZ_v|2hJ(s7Ew
z(Ok{9o8A2xdNdg38zveWF6c47)`~82I)Avgnq@Fnqa4v-VZl$4lnoE2V79t(4LaG}
zr>hjKl=R+4xox{}@<hx}A)lR#nS({x;;-)E`(Nc23PnOxxvpGvG;&j8wzeVC`c;4!
zv3#scZ_XuIYyFwWRi?c-SYH;Cw_+f0EJtlU1R1z}_3e=JFAs#{(%QEmxB^%nKLJVN
zNM8p5QT0*&`#WnmoP#41Lh)wKWGBj>+ofFcl>YMF>6umfR-pbhIoDpXw4m35*gSw~
zgYMm+k;?FvaVkw3*J8_e2|*2))}_@rO$=j2yO;veYBlJ3$Nu1VWoW%jS9gG3)`B5+
zq|GC)v*F~|V{a<ntuvo6pET;8<l_AKA@nnPrO@6+&V5HUKHB&1+`|qU1RK!K-O#vg
zilC63gk2rDOOu=srodC2Z0PCF5?70?=us3-KN$jqSB!Om0CoHnhhD{DF8*?V)6m-m
zvfJrTpGI!J9335PeRbnESia44l3F7@IQP%|XF13}jvtaPpa|el2G|(<u2N+byE^)?
za;|em$`1ElsXQFQTN!_+wNkNH88Q=p@t~cY(R{5g+IdjF#m?HzpdhauL*A#z#yH+T
zS>yhRAIUzU>Hg*$Q?w#tOmn%k>#}WNyPtxY<a#etZDi=hWuw;+FfWrO?$=?|n<m0B
z&08O(qy?m>T%eg`u)nj7kR-O{c5K2IBbE_@qP(J@RNye1#(X$folEqYfwf8j8lr@<
zmGAAPm%<;e)J@dU_L<Eeotu?Y|9FR}4DcT^9&3)d+3IET<m&I~3S}jS$)5p^RR>F?
zzW$7hzv1fH+k>Z2L4E;K4}SF3@sR{u{`=n9;CVP6#7{pJX|N3!WO5#owLAju27Jk-
zbuO4ms>(Lp=4-GdH2am(5@3q0PxSl_1^VE#KvtTBe4m&3|I%s>G_fVM*P!#qM+`C{
z-&OjOu$rPT;Ni%9qb)TCjH#~9EzDf(3$#-aLZPGXt1<J_7K!x+12J|Mq9LpiH@KCA
zR(FFFE<Y@ygO$QJZB&lfCTu8|cIbSd>1fufwUf@MpPqR<64KW^wREiz;U#8N<l)@+
zy^qlmVTK5X3-RN2ty(~GpsFp2?X?7V{9a4%3;Z!LI7Rj0!^#f|>$0-n&BaU%efYfX
zInvfF|6656b>$#O{{&UO%`(v|*(OvpQ~#esqi24->mF`26FurmJ0#Iv(iB>N>z6*^
zTFf$;tdH!r))umNE01w0S*qI37p&BHj!C#umL;w){cV1Mb}Y^$uryR)^u9Y$!{QoT
zB&)P_$XU{oYRReBm?+>Xo4ZhBKPDO}@UNi6_|x`Jw(E3TbVvPRk-^mE?WHc@glkFI
z1)XG<Ky7ZZ#F)$Yr|bIfy(Gi=*sqPFm+kd@=~*+^qK$efZtfVwB`AgHD`m&l7BlyE
z2N-)%ZHmBZs&IWpX`^WAHDeIR%~BAY<9gPl3_oo8X2VuS9$di2g()6ZRYR0IJaVYT
zWaUhG{#7HA)AYN3Y5iV7;(W={JkJR`{3JPq2_;NgU67ETuNwO5oT4sq#W`y*yVEzs
z7;&}1l-20$|C!p)1VAYJZM^yCoaDgU6jiaRp@K+&7Svg7AknpA@htU%^)Q-C7ufN%
z$zAXqdvR%|3<w6tt5v_N7|qt!9M&c-WS30P-k3`G<95&AOdnco{XD*qb!6}Gc86Cq
zHs)ndaKN<MD*_sOSGO>HqqRBkh1&XG(P)G)BoWN?H54U$DS&bl{d};pAu=z=BaY`l
zvOc;IZOhwjN1aX7PEH?~hXHsxwQjyzR$%Y~dJ_l%*L&Pt4&7-e_2Cbud{-%EiMWtT
zv4_9v`cK2^-O=S8XMt{*v!i3y*auxwvpTNJ1HU&Gf9jr|h`F>pb*$b6nx&x4I5!iy
zCD>m2m@+08G&0mQOf`9#GUKSbzjDC}fWu+CI^f9P1=73<a~ilP$nS3m74i~Zsm*0L
zlBqyK0&9~mrG_XYrk*(HG$~GyJ=g2I4t*@ZI67^`d+i#yYm70nvrJ#7cqpbtE0})M
zshA49m0g|Dc`e%893m>}+Z$5~+MCEck4bS2T|uu1l4m(?a;q83nGijJY;F0a9;m3X
zVA)*<RE@Wd3^AU$I|PTR_0KsuB@ygI{=;_2dW36De^{OQ7P8dPq`UZBF|+5?YYnuo
zCU+=p)H>uhGQD+QYWdAp3jC7{f_L_V7XF;xSpMxC%ahR%olG2h(+=5ee->`~oGHiS
z__yp?cO12kvV^lft^ljZcniO0^ab4HMB;kEk!-WV6{wyd&?<^mr>HL->|wS~DW7~E
zE-ap=y58J4yh5*p4)NQbw5%pPn!01d@3%e}dUCV6!dW~mBA$i8<<vGHHm9nR<RTxW
z>JX)h?FsOhQg*g!o&XYP-c5)Z!$->)TapeNgBuGAzOeNeN9k@0?I6uoO-qSnq<%=Y
z9$mCR2F-F|$B6!TgLdOTgUY7t#r!AE_3KAV4#p@)2=V6MwjXJ9-c>K~eKLQma)7fF
zpF7S=;Cr|<tDNbOJae<*o*BGAbvY^`Q4PlG>asw~hQ_9#0^NE^Bx5iSd4VZoNMDin
zx3ZK@g4?E-Tf>0>QNi$>nS>caxA84uU^PC4uuJPwY_$-J37fXX>hKjL(D&?sb0<OQ
z;#v!hHM3>ijw@$0v#Rsjg6<?O{Ay5qv5Vg#@%pR!>(_JlLlgb)0=}>S`KpE4+$LmW
zH`1rt8^F_N!ndbD$HN7AZ>lf01NvAXlu23&m7C%Xt&`o(fohX&(-CkA)8s{PTBTx{
zyFtnNUQ6#p-n*$TZ}8i}z-7(CuMSbU;~pqCuz$a)!^mh$(TKh8mb=@LB-f60?<+;0
zH)_)W=oP^2TD9&#x&xPONzakRWtb6tyN-t<smhWITgq!)1|0wDf&u(YCpVedDL7VD
z?-yxo#{$9TglThjKuk8JY0o(Fv0a~j{3#JjLY1m7K{NTTrhNV8QdbVeR<ac8;F_pK
zufE+`4|h?LFtD^qQdGiPzk)%oGV#o0=0#MBzqR;ry^E82MDNFqEJP4FrhGiD;HVX1
zf15qPqHz)np0YX^^B!S~$O5@7+(cxpE#3?fWEG+hi5p!Cup(Wys_`K_NK%UY*qZYB
zWaq204<8t%$lnN3(uY6cTLrw?alL`S<xD$9>{+yxLHdfQa=2e{*^~!Bpn1F%b(Qbj
z%rtOaT3W?$bhfF*`?~lpth|}WzaovlTC6DE;oq8v%N4ig376PfMd{NjwPU|rRL*mC
zyz=eI9j%1^9Ww%F4R{<#POWB0Z%$Pg_jD~AqYVQ+1C}!^+RU+jek*240$~XS1Bh;C
z_daOGjS^r&Hd3h=5N+I5wf8E~=!<iV@kShV=0wlAw!$y-wu9%o`L4g@jPEn<FZ<LI
z)cSS<rZ3?pu|)6#e)P@oR>$5e?|BwADJ<j;k&P@^-NQpdS@5Lc?C~U;T{~(kM)&}p
z^teFP9BDl!xSd+J=1&)xRcp)&%<X4u1KGIx_;~vDEvw1*JyLH5xZ~Q*2I(kynhG;G
z2*Wp|n)I;s`b3c<JC<5a*^}tkb5G=qL@~sU`-Dlbq(fU+mq+%{`qwL@iSkP(ZtX50
z%G_AC=IRnCO^BH9W~uY&Ul3_YPu(zk8aIQ985+bdcH`q)w{woelEO<91dy}&6kl@>
ze+QObeQ10AFN+U1UbHpWkP?L{2QryWl@&wZ-G;X8g?}b%b{a}7RtEo3^};ND6E#M<
zwgcVE@C(0ed*0L+!|stXHM0tIT4s9*9O*v986&%B$X9xfQPt?D#+XH_wP#OL%jfh{
zdY*ga0;%Y|b=ERTWUx*TJlMX=f7o!pFO~a<;q*Cm`HQ*uqCHEq;c?DM%upltL}T!m
zhslpN3<9<94hH6?8*J{_{ThB7v=xO@C6+wHv+<Upg2-x!4n~5AR;jLUrp&H%?NtI_
z+;(fPu6Bbzs&xyy9oUW0p;o&&%q20U4C5cdn9}8)#LHP(!yDr7)n~f&7CKDHVFS81
zBK)ExXmy-%Zh?Mnqw!BKtXxiu-3_krN7*?4Ir{<tP(40?!lSUfQ?_LDZB=M6#Uc3t
z3&MaPCH=%UMPQQX0BFt7N5k_o69(y|v_42M(T%3Y>SswK@<j3oBEfCSXhOVM`{3wq
zvD*OF6kHI%zyH03Bq&Iee;zDFa30nDFf>%bbSL<01u|aNbuI5?BsHr0B=spj|FObS
z`&j(jEr;^i#k{ZCt5>Bl!H+97jT{)s&Fvm%DFw)RDj3d$Cg9jq4E)V9m5{eWw}`!Q
z#!ziyiv;=G&D5Q7jE(r_b!>pLSoi!uDze_p!nJ=nj3W=Iwa)Lsm3HK~^U{Tt=Nd;=
zI&G(qp#uIaY3vABPP)>eb^pUF)1u?V*!oj4zD}!~cQ3(VjyUI}a&^OB$Eq)2oC*Si
z(2*v=aofr!R3bapQ_A7Wqd{K6LQBIF#3cwZif@~x!vKlpvxE4?cA|PQc)mRQ(S*s6
z>8103c{ZE;7H|O^p!{K7AoSq>T5*o|D!vi62`&B&Z`ul0eC>1d#l$aXP3v187N33o
z!i*uq-rmsEq+*;CpW$oR*w!;dt=Yab<ar&~mRj!%IdP3&13!PU6+a&q1`WmB&YwsR
z38uKo!cu(Ccge7n<-gni6j}PuAy{u1)!L~~9(yLpYWjWdcyq?R1qcY+v{aUP^|!lM
zra^UElxu(llU$tZ$RPD0D#Xjz5{TOD4q-SV<=kp}g;ftV9Uz(x!k1wen6kw0o^{9I
zS-s7c6=aVqOn8Q>E4Z~hC>}vyFbA=^_2c5}_fzr)Z=U~NTYq}^Ykn?yBC5Rj*hx_D
zo9NNZFL7O1t%^#6v=&VP&-V8L<tNjM$D(}+hhH{($zDw~0jRa&kHc%Ac2i&4#Kl<3
z=c>Q-)7g!2edc{+s={5Sqm(uo?l2!TQS8Bm_o*-G3d=9|&jqCyq|v9R{GqUXcDrTp
zo^7Lv!Vg6gWY(*SKJB-zH-btVN0%J~1Rz$$P)}F(a>b1FlgXaU_SE}v-Ya9#z#jep
z$uoKaySF)?)zGDG{dy<dX*tBwyxX*Xamao=6*y$K5>ZsY)ljF-;|V<#ch`is2I?=3
zFY#MhML>+nRJpr>En;|Cw4%N24z_Q#wZG-I3ufhLm#Mbixv3!EbhLzPxUJFd7v>YZ
zF{qmxe5RQPha$#pqRm}eQz$fOuuxCZC)164M<6p$=QIjyEabq?)0S1Qw}2bzQ`-LD
zlu_AA)N4bWE=iiI?29WbyS<)%#;*5_D}Y(`@y87&x`*@3chH5aN<HAGza6oSV4NR1
zcTGWOV&azz=QjebwCGuaBwG6{zw-fmMq8jq*?M<HR;?mn4?-NYK6+Hlt~(gV05l>o
zs>bi(i5duo^mRx->sZxZ`<}5f0HXEaal*H3Lrv@BZ&PNZ_l#f}rpzL9ncbu&&rFBD
zuusd|Sm<2e*6j+cj&Yb+UU|i%L4usH`dqqjIGF#hO3@@+A6ug%Gs_u{#Tq)UK{7>X
zlM?802a*wJBL(p}mu4_FZ`9~Ec&P&zGB@i$nUVqJg+O6Rk1=%r#$=Ydq5n{U6CVVy
z1ky<^*SA2S!_BPabVT;a7);C^(>pa2&nm>ux|}%s#zUt5U&FQ!ybZJ0UnT6_8$6TT
zP;%_Liv8PHdo}Ypd)erfx;0EbYjcnM(K-9xOkA}3^u4)kT|x@N?c!)nJ3<)OHWn&!
z$TBB(3N$RZxJu<=19`18Zbd+N3d0bsmDc2WWF^LUTCV5Jx7L6FQuear5q;-E`2%Ft
zARYf$;cB_u-_u!^Uu0Fc9$f{G+{`a7=r{}u0ojg>SB70T?$%4M1VajEBCA?@nF)*!
z^#w9KrYfASUL4lg7aq9D&N+USr5P0jO$3I1VMwWk-1V8)E(BY*)l0^;r)MAgTHtOC
zsaTIJ>Hm!tWa@D?X|nlel_VRQc+quMVUUrNnAwNE!J=FVjU*+KpeRXne|fq=<lDCc
zsVsm?%fth4%NA}6dK$5?XWa#r%#rSM@Z8x9v7OE~%Oft2Rr6Due9wo0JIKKh)6D{X
zK*wVC*-sveWbAl4K5RcNf6UcRinB(@Q1GSB_$JLPi+GHKTZulKZV73Dt-qgjOM_zu
z=(+vL@0=Xrc6miy0@Tn>KbjeeUw}x?%M7KON~RIDW;%-tqtZ)i`j(K@TSuQa+@sn~
zlSSl(MA$_Iml}7AWdl3EH|2%L<3@e+g9bE%!mub!kajs*$Xsq){`k!<Ou_Zf4Yb<(
zkZuaR07$%7ouRLnuVI;DtWY~ZdtM;c!3QRt+V%q#uiaYc4*dq;Vf#}sE&Sg6ac9se
zNO*8E#B?mH8^MG*kWgV#vcawj;_-gRekjtHP=}k%&-Ht4C}+CVW}RFAw7!=ZS#<ec
z)i@h-cjYD2uIln7u`S&>u{b-XakPRr1fJ4Q%pIL$${4i(qO~I?pypfiZX2-FcO4N3
z<*)axH2gF?%h3FxG%igYu+(wBgDr>%kH6P56n8N)sJZ5N=fRc1m9Op8yDyF{hxX>7
z{&)pA+zfYLG{m`eci!3r$6$rl$4B|DZ^^dF6q?eQH`*8ikEn4fz5S?EkbsSk>ZVch
z#-UJ~8;_{kUb3NYR-ja)eh9yvxzRQ0g6f4FKU0crg-19ZT>6rwICQ5%{<lHR&=Va2
z46<Ks{|iP?NRZ$bK0rw<irFQt1D2fj*UEi`I`lLYXTwaJaDdz4)Y^@>UvZThX?A*3
ze!hk7U!fmXp+Mb)v^k7vDRc}E)d}p<S5^Z!p!vL*UJO{b-D_8lgK{YsR1`4ZPgjRc
zuI$vgt9a$<x4%8$`5f>LhWGx!FURd^6)Ht{j_Z<C#<l^`YB1VUx3_t079hGij?Z}5
zUzVz>%pRJ1LFhBc&|?6Muw9PGa$yNgs$;c^Eu}W_)UahmE;P!G%2*yl=jHrcKosfs
zPdu&;Z*z3*xf@5a_j8yY%KHMr!Lywc!KS*L<};(NBa8gIe3a$;9A^PRC|GH>)f~$Z
z)9n1VaD0115=n#TrXcDTM49@*CTN&a^<9CQBXQ+;=x6pwD=>StxsL=Kzyvvl&Q`~M
z>Q6KoSZY0!JeRh!-~e@?WNUMm)N10wHd2Ky4ZGdFV@nzJ3bpTIuZDh`gso2siH@a#
zujzFkIjdj8pMtBiyMOkpa);3khP*}ylO1soqPM=|=~^kILX_*MRlb^NC8?^cmJ<ct
zc!lB)DIPVhf@Zs$^wC)?Wo<v>4$`<ohHk31*|ojCl)JPpfKhpz^L1!0YXQY<>^XbF
z{Tx=@MO5tU|J;f-ab;~kFS@^asmd0nZhg0=u6AhixdvxK1#2mgjAAL_)*MrZi(RGz
z5JhXp9OuFm<w96ISQ=F%ZKP-nLY)VgNM)m~kSY(4aCypuPY<09st09>^G8<G@W3z;
zcmy7wkg8j-ye)G35r}Fh;%=MJ?6K#d?=VeXYA7h|*b4ru2x(GAb0+LUut8F+EcImD
z6)Egc<eTS$iqo}XMg-fizkL0d$Z5_GYqxV*{lcO`Z@Vh!g}!;BH?rEGy4VOBOtpdF
zpdooIIO83Ta!IyS^!8!2G}*0>z}1W_@16>bIDf3)jg4VIxb=EJOg0K8C;ZKiBzJxM
z;bq1dVyb`o1eW47o^S;}tIcjUh;aN^$043tIQsN!)Y%<;kXrgp*RboGLpib?J4P{9
z8aLhf;ZKzB&fC8ZsqPv?s>=I2B$NF6!!G?g4*?bnUHTb3cF#%I<1<qJ9-+!0DPyw<
zo6P}OOgYA=kw9Po#vQlCcdg5@5!0%C<vx8)i>A}6{P}9KopNDuxvSM_XLsuhvh?98
ztHRPF=VfE=_q!~V#(n0C8#VEGTy~L@+}>eLABH)`=sCbTJ0mLr&AtLpFr(ggEjWy#
z-`+6aq(QRlqCwpBZj!MER6bjy8oRs!XBl^&pmsSVF5bpAjf^(&*M2C?xXIQ}p2X|%
zsp8AGLt=%gS1=ZQx%$4VN55N)7<^OR4`2C#=-(g1v@&Z2Y9>KbLRbola=+dJt-|Qi
z^bXLJ(+Uq}$_5Z%l(C6}J}rRdxra9h;EvY;>YH_ECgDo||ALI-Ph*hN@n?Qq+&E~M
zzUFyqBJ!!t@b<ohQJ}_}-WQ1NUHvnrK~jUAI@{IH3V@{hZ@V4;{KWcqva}%nBZ(tg
zSa9!^XqXZA-}zI_<hF7^{H_c=eiQred@8weUko*Qa0IRJBlF+c(|e-REgdWWIrJA<
zl<8FY_&)-w|3@zKlR&mODQq$|&h+Egp^w@z#?0MlyFrd1eRI0iQ^$1)f7#}h#L%>p
zv@IZ6SIy(G@$aX_`>{H@Cu{v5veow0P9_ihN80bd9{2gvmS`(O!#C;Lh<^@wyB8ni
z%1i#wq16A*oF`+8i@U6tlQfT|E}80s?n;8!u{%#dNM*%|8^2C`f(x8~zg9uHGp`lU
zNRp&N$^o!E`k1$9S{)u3vk6N}F`k!?QnPC{#7Lm6`UFWN2!|ay1ux_~5ttAO7OYjX
z1p+yfmf}Y1TrMgOY4zE#eo=o>d?oclM-JwpA<?jj=7PDhs2;6LHz0@69o#3Z@?X!B
z?w%(Y{Qc8US$#&cQJUNrsh{2rWG3ScrKC*;quzXF0(g3WMxr_*lk~78Rfhy2vb;nx
zj0u;y+V3_tC`n<;A_i3BTw0GKnZOs6yEG7m!J84arsX+MYHYlaT)7^X8NrTx{Kv+T
z*TM7-AFa@x)LT`x=Z3fB0;0cIrL)4YVS$0jH<4H^zX6FqbiFHV$dK}V3R*PDhVH+~
zxd23j(_}($3n-|l%`qLIj1id%-p_=NxogVm*MSa&3>Bj9ai_3q6czw`B*Gfa9dp$5
zBScc{<gR~qNev2<b#ZvRV5jEoUn_y}B|9xgXvXE04d^#RDrM2g8U?>Bqd-ReBkSbO
zQZZ9;72?#2pX_bI1T1rCfXoEG6s9{txLz<A1u+F@827=21uq-nvw2J0J|T{EIOt(;
zLXIr^>V#Gj3mO(%Ty~pj;XkPagviRP?0!3+$oa|vxt~YXl&&+z94kZkMVV?Ok6)|E
z(?i-T%2iqF&a=<#&l4&+N{`e7Y@eS)re<jxeWcV4dNjO!@xsmmyR2xyVpUs)725%0
zzAEag4KR<&xJGGNJBsCf`6ygN$`x5V-mV?<KK_bM1+i;_L&sEQAo_sn=}6j);NbO}
zj^d`Nm1wAQ(Iv~BiW>AL=iXJ~fKquZuG{gi!OnXlzV41gp1XVf-uyG$DXy%iG?h1t
z!wgUade6AKTsE$qcM4pSREqe27<;d%CeSZ#*Z<5oW3O~6BM?exQbLn5pac^pffPcA
zK}1MmK$?KG83j>_NJwA|0fZ1j5{g1XFC)z%2uMPaE+rr(RHcY8C+GH@wZ8S;<RWY3
zA}eLT@7}-tJev|2mGiP;CYlYX;!P$1p7-!|6B#I}Wmi06<Ogr69}M{SzhjZB(aVU^
z&qE~tpX%d^sB!AEzjO?37!8jRvU5xKGMoo5VT)e#JO8G7j7Q&|UA7<8)7C-C<OeFZ
zdp;{BMh(l*JCgJlcB%LFeHEj-wMy0Y{lY8FpusN%i7IBjD7Ho?D-2J!-4sKuwh$?i
zyTxCtsyN7$AXIL;>A{;Y6ccW^Kj(1B`g#`g#Rog~b2amG<hOexHQ6uR+{Txk=y`)R
zyDlzaUMnGPI!%qJ)#1?7F!Z(i1b~*tpJ_BWh&AgkBS_L#1fby?j2n_?Znj3z`hi(O
zn%qEC#C3k@>`Xu0g?tj72Q(6Y)$7U(6I(P#+WQiHs%u`9<hcoQ**!{F7zn}Q3^v<$
zDk)SN#i4!|7@a?dJ5uM2gm-gIgI3iCK3!44BVONN;V7|*NsZ}3J>lxMvTM>?6Q#SW
zd71-xILX^{K!f`aJ4*!B)ySakDf4po2c9)q3yE_s=j|0B2VDA3Suj(UxbK(#rPdHV
z4)Hc#SfJi1l*|9`SnyQ}D086jt`pRX|L?7Cj7DE>PT|=5dL^xkk3xw~V!9;}MhC=P
zkMQh<_D6&lgy$#O)!RWBQvBG85pPE_H=?>4UyVog9s#1J=Gq*Kml|$&s?lb3+|Qby
zH@z$eR?gqq!8k{arntII%KYY=`}o8kPOkv~5wI(DFB*u>_2|koUCVJX=hJRfs+5a7
z{pCU(A&gU?kc(5NC@Dcb`CMH1?df>Oz~wqE^Gt?@rcKY7ec!!UyUo*G+GD#()9tT6
zKl$e2dQb0D>w%c@F=1IYl2Q6LZ2p~=5whp(pZs}@jeIEXBIo%BA$X?{)7+dFVOu@V
zM{j&3X{lC5U^t&a;fHA&Lu~xW6Qx{9kkTge13%mg=aB2#*K7g|rVNKKwN>FA2|&)$
zww^z5r*KK`eELG&rWNzkp^cy0z4EfJTI$cdVtcN%nopE9z9d&%ZQ|5Tb-y`t+5P)p
z9ce@GubO?q5F~qbzOn7Tv&LY}ChAb6<qhdSi;V3l7)9}m(38e3a<8*8;vXVY*jE4?
zXXs_*gI4Z+$bpI3L;s(Ybgeu7J0=lz!?V6>Q=6hl!~UFAiof&`iw#rQ$qByKv*XB_
znDFdY*qT0XtTBHeMVbn~^tW$)iq@0ISz`_1`XGiP)_2L4&^bmaR~3GB3lv5kHxpFS
zXTV6NZB`}pbd5wD7bk(Wx;0$J6qB-o@|m-jo16<laF9lqp)gzd;tn58UH<stNPRD(
zjStN?GxuLFcr~&2XvioRpX(Q_kol*V+wZ$Bk+z%3W1<e}(rjg~s+>2;!XFtIju?(o
zE#`k)IrMx=4-R}I-gKFx@ixR2{}Oiy&DnuFM_<c~PTZ(wtRXs$i1A;T#!H}Rbx9Wr
zsGM%@S&<c)BZjbf8Fo%tQ+#I*ax=~+5LO=EUT(O@^ea8>AT!fg_2g)3&9IcykY#uF
z3jrT;hT>FDmT?FUOgr_3SD;e)By@K}P$-KQa&W-P3k5ogcz%eOpwRrVc4xq6OW5!c
zNhv=S)hSgvOh-B;jeyKkE|3AhY|fks1{h@Y$0sJwA#W%RySAr3YfPW{2x?q^)==?S
zD(jW^jX+A7@m63>VcIpLS>HI;GUCsy3Hy7*Lzd<Cv)lSVI|sa@P-t*A)|?FNI{z}1
zXCz$Z!0cEl&Dy^Mu5_EWT~6JlxR=6)$#VFYV3N9O(SD&Z0|QxJmrM@V(o`intLE-{
z)nv`~CU+dj3T6}=XUhGYI8k=J|1@SGYu`Mvo$m$J6oe9?t8f0j`q$bj##Z5oU!<II
z8Ob>AH6{_Piz7OcHEg=+Ww$5yRkZr7`YuNhtn8q4yLY*x-<{=lf2F`3Df+?K>vj$l
zo?brGXx*`3tUYxgUTZJeW22cYVq>M{x@y@Fx-0HJy?x2#LRsyVh(OWt{xP|e1VS<8
z3aHgT-%-bJFujZ!<Dxn}R4B0o=g2S0Rz#k22jSTsquanQOSboev(bLuR{YCMaQK27
z()C&Ic2{aiR#IUvkQ;~y!F@IJPcHXHFRPbBoXBLoz~=mTu6V)0Ju><0>0G~}9(I%i
zk~o5?$HxrT<V)1nHl>Zarq*O@2L0MpoBsU}isrm<`1Jsn^epc>I{y@Ap8mA*lwtZl
zBiiri)xLkviB2fLW82h`IM5zCxDfTX9|A7!m2o085p4I|TJ31x#B8mp^o99BVtXC4
zJ<sg68FSQEI2!+<K;fR%iKW+ZMJsRt7lw1;3W|p+E)BPtAq};wCwo<2|9H0PkJe0i
zrsc$w>#lB!KE~F@+yKX2^u6U!czJ&1DF6Nr^-TjA(%mPq)Fz$9L|!V;IWf$3Y@kY!
zam*NyDQY*oY4QCmb&Q04mwlPE+-ur(E%pu59bG!~<$U6ik<Zf>8Ge%!(PKiv!?C6z
zlJm##(Y@ES-`^-~eSfAIe(KHxuSU1jAMl^`!)ZyCm?W(O(>~Ai(Rx(#5^o6rFg5Ll
z0A$sQIe}aEzbfFIn8wrz7i-;TFf8cq{9KRG?Cf|5wzIJ$+9c%&gtt1kNq?94iXUu&
z2>J%Apksq+<2DL||6?@PHsBk39njSRisgs)ohZ`V@C6swnR-_`QP2F`a;?~(621iK
z)_Wka3@$>DTtHB={O3VZ9LeK;lb6v<KWcy)Ev1McdXJ31?VNtumsJSR;E;UJL5Fjh
z;tfgbrsi7$qfdvl@D;WW3R5G9aMS*xqdSgq(ek@IJwB=Wo*8gvcN3oV4j{e>w<k!7
zM?Hie)<aVGh>#R2GK(CDc$AvD(=syJ;EaWS&!S;KOdg4*4y+AH^hX*zddluaW;w-@
z>FU-GjOfXvLU%GG78p+dMI;xLXIh75A|+TTYV({!Q=IMC6=q>J4>Ajpn@Z5`IW5l1
zKz)gUzh`E>`OfsuxPM(WqFvgQ6-tsI5eZ@7?^V^@A^w9HcCo0z@rI1U7#O5I_dbh5
zF0QD=>Zact0ms_J9Q?M&8b=48y8)<P{;~7*5>PC7OWFI=^{g(0mi`zgF2hGXM;~s@
zZ(B@rS%!?!&g5L>q@G`n1YaHT49>Upn%cGXua4y&AGfED1j}K~&6Vo49*3ApZdA=i
ztJ{vlrzntvXne)-e56bE@cg$ycgNAN1L@jf?17$B^C7$Gddc3u-;L^0?hMQM5spbW
zsBpFq|DIHJ@Kvn%*71Yu&?8T8Z1(WWQ@!`!IGiEEq&D@brFJ}oGv!e=Y4Tj85haMK
zq+`|_<0imBby&nS#UYm=y37#nS`c|*jV9@lX|qgP28heEo}b)vg9Sl$9>m^frd4!S
zy2F9(aQDO4$)CqD(#pJbR0jN307YOOSc~QiR;*<T?sTeUk6pi>_}4tQo?`OX!qi1)
zWyVP@UZTGK^IGz${H`G>D*JFYX+~YHpc!UUIym1}+?+;LEok(bDIO}98%T1in>yNp
zrr#8bH*GsRIejs26S%F(-y`RK-ixHWPeV%Wbygh>dWzq_Nc~~OA}2e0toU0^t}GdI
zA{QmN_dcuVjBn&EEpsIiI$tgSg;V;?F@h=23E?@gkcfl8U<hiV8yFP*iViOG<@$eh
zV}Xo<JlTxqe;14Wdo=C33~*&CKm({jI>eT)iT)OV8WeFHun}i%UCUNQD$$Jm09SFO
zY_N$4Q6b+<myn9QEs4I=aSWz_eppkK6AdZt)LlbjRk?&>u3wrzf)y?Z4eACw7#|fs
zwPXL@T+D0ok&ODKBebG9p?Eqf^tw-tT>#^=cpIe7#01RiYCHY7(($JpJ&PBb)Q`mm
z!diQ*tf8IJm7E$HQa6HGX7op)7>8Z?wd6}K?ixd0Rj3ZkO!HI6(A~p7_wsP2M+@|m
z8b}r!!gHadX~bbfp{fhZad!s?l^5n|Cu}*3GA(0D&p(V~zm*@=7cb{4HEoDk(~gYg
z^Tow{chjt%x+lvD$T)Rgz>~nq#lcqxhBv*pS2voF|FWP)uh^Pu^)qmYl?&V%AoDJF
zEL7J`GYlLOnzoO#J?D%?Gp-~8%nnp3&1QqCwI=2z#fv92!l2G7fego-++B`WIp*x<
z(donT?`E4n-pX;=IBZ^^N1rQH%%*syH8>x>A0fWoDV=??;yAeW<u2-8Wr%hqOV#*I
zr)Cpz@<lmbgQqVZ@b%}T(u?Y&<0y$NGqWu_@C7E^a~W<(zU5T1m_mHDnF>cGHcto0
zzz*46#d)S6x!5hZvqT{ltb5XYNaEUm$6Cb9%Xf#jd3|Ty1-8RR0(`j*eH!cjBu_qP
z_tC<ofplOD%xOvV%o~6gSoS{p%b-`Mp;LPk%CAwR63io}l=En7a6|eNwzpxtfDEPv
zK%EMVkjyJXbcDc(?2*$5FKao8Oh5J9aVi<$`FkKwsc|8cH3LHGKPD9kPlc8&_c<{t
zXMHk8q`c*`Ixr8d*m%pl%{Q%&qB!+J{-|o2MQx^9BjN0OJ^9S0fZto1UYw+bU6CMO
zg=nJN+AhKK)yhqQ*~ARP&UdGQ@FjHOq%)F)14d}JIYszz82_?_W-+F>iv_556m=rJ
zxrP0ej==Y5bWsa6ZThH-2?5fw-iX%CfSuIpJxit<nYko4^hM9tm;v~jh(V6qVDZ`r
zuWn#>!r3W8lUOzMd>9gNS=o*LzU@{PkT6Uwwbg;@ra7;Y7N2G@Y%fICf^a>fc95>V
zuj<{lm&+Av9f<|Cq@p4k1}>^_23zrI+1V-Sx;Uz6%l+IrXxS2qxioz~Hnhe1wR!T#
zwJ*^#b(G;*-offLww59d?7muE-^9I_UZweD!Fg<1rtGhQ$Pnl?_DM}!944f>@>CWL
zX|<Kw1QalT7WIfbgHWdsQr(;u4?W@OX{K9TCPI?UKRlT9&U3vkhHXZ5t5K{T9@4G!
z_mXFfX$*VXq}%9HLwdQOdm$<kHKOh&TGK8<!k)h=xq-j8^3Y_$TjtYWMgLnn?P)L9
zY1%;cwK2+b8(^F+DPmwan%)st=AmDI9n=v4u)q+eT2TXVYoOgxHsF8>>Gd+6k+*z!
zG>>c~erw7a$rhlp7^lLr7C_?Ep`Xp3S!^s|Q+b(32zI#R<-QB66KIQczVB@RdWZj2
zA^70oZSCUP{HQMnmMFh}H*aWffj`&U=SZ9K+tsLaII$76&|!)~rNvP~op!du=geO)
zWQr$UPP7%z876LQBFpcN<=v!7ON+1J8Yb`eJ@<bdPgK|Jt_(dthD>X&>?%-jeEhX_
z&#q9MfqV*LkCwxPw58V{9fBjy#8`d6L`TS|aE*^{KLV-v?c^wH{TQi<Htn1o65ln?
z6>tb9J*QX<ots5;%?cbUxr!k>dj;-P2t?%F;zJIkPuj7TgL09W(8ncIj|YfdTpNS)
z;8oF}zcZSI<^6s5Y8M|CbzA9NR5m`>xR!`T4!<9#lY1ndf9D0iTicT1{^=un{dDWB
zMpxQg)QvGn*D_FYF`1Oxq=ErbU)9szi}wN9CxNdQ=wKZONegq5XT8wsmgeZ>%-qv7
z9c&#4MX{)u5a&^lB{Nno$P$ue6;jL(o~dm**P^BP6Si%_m~og^>k~BKc6L8&b@u$~
zmmv=?r=Zq>rd__&N9%QpZ=l`V{&OvLS+pG*(#c_JF&$(EVL0aN!>Q^>-QvPtiK0p*
z-PG~n%5tPLXWl97z#;`_;IbfGsI`bbEU*U5FY=f}_dkC>d*Bik`K|@fxB*B**_XSP
z&#5XA26Vj>-rQ2ji_NokX|8`0TSb6*p9MXiP1MfhWiCP*z5r4SrFGq*mCDswiB!L0
z!HxwlEKz&8WMW-0(6;#1vK|}b>fUSn72uFJY5!95dg=IB9rQxptv;g(`P*DL9bh%r
zo*?^aQ@+6nU$TtJjG~;Je}X-FD;)0)s<OzAw7s%Vm%gH$Nfyo4aju8#yy~<WK8eoJ
zF{VVhyZ-CoEX!woRxP3|ppu6J>zQ4r1iJteSwJFvl~2lYTmjv$dY+Ty4y_KtJh-1T
zv|L(ecKzKH0+|}$gA9!R?^q}7!FpSZ!XPNmcwt2RwJy(}_w*j?j`=}b<^2oGG80N{
zmSI=9JT52XqLxbRr|95b=Rt%ICWQ<h^mPotgzjSbj|m!mbbA@Q7XJr1S_B0nzmqU<
z<2(av2I~eJ%@&Qb&5ZaAm^zZaTCbw_(E8G*NNMgld*=KpuN6hBH~94nQK3<P42fc`
zPy8B3|21DzM5xq@T~c-pqG4|4$2_~65>cCC|HH*`=eKa`{aPySwxXobFgdQ8=9vGz
zDSw)h6lmLrUb_9+JiVR5?1n&#{ejt!(3q=X2gLB^0*mo<;lH1Gpd|<o0(Zp`lRIVP
zHjh3mtHgMu|L9$_`NKpz;auVzBxWVU>ZC3*Y9Seu1ARVm<9H7QFia2f3zSnw7}X5)
zOD$B<);Y_}(xuhr6q;sz{Z<LFW2S7Wo@u_37z{#QyX+G@;oX;at6r`Cuxwe}`I4X>
z(#WU8Se?`~%|0>2;u2BVWO)Ggar^APo)T+Uyll`u(;MU=t1Z$le00<|K+s-$Sy8??
zbocp=1ExMOKMpu6jQGj5!f4#hk8B6vF+VL#P60~Iz=k2He6<QirAA={Z5@1p43f)A
z2k$;%7ljCSsvL4gkrAE%jgq-j2&NXgp?JTr*IgG`Quxa%3*>$YUYXu{FYo+JXLD1F
znjivpX$6(-_{MEH>$5w$)_CBZMEXPGGG&NN4LSOzF~*-=KI?8KQ#Kavp{iw4jsCI-
zg%_KB5<(cqd$b^|6wf0e^c+`KJiaLvLW|P2M$J=>dP6n(vtz5nc50xT3y)Lins5*e
zjzH4G^W1Unlxb1IjF(oMdvJ#XY~-79qv3d4+rf=D7ke+hRD14_=jS2_-;JU_a(#o3
z4q22ibl>0kr{CG-Fu&~D)_=!xNQY%g?Y%HoXsS~I04<bzlcQBaKkMg=q6pPa=U-+k
z?MEU|RF|e1wUV#*7apa|*IJvX)-P>Aam60Jn~Nf*cx~%{f#2+((JxPajH2)y4!|Di
zYCXFbPxQ=i;~Zfr3xD`K>6EE9)dx4d<^*5<*Ks$(TMmdKGT=o{X;k{DZeD8A72r1c
zR>`0WWbm;t$yLBaQq`CY_kx;7W5rDO%_`>4bEZ62P@<0NoGg@;<Nyu#N9LJVCl6X(
z8BTs?wBqIETUX_A5Ex|FJQ45wUO8~sMzX^#7#OvCFWd6noxgI_Wy_ZZ;XrsQ4t|Lg
zEFIN?8JhjZz%zz$ekZ%iR51B(h)6FZ+{%WP$zK1#Z!1dYEZpK=B?Lz~+HyDpt6Z4X
zJJ;$u!VPqGbi@h&Hyko(9JcjR+X+1UEn+ZbBvXB~BphL_qu$4Y*bP&)Lha8(d7Qab
zCK{?!uZzo~7XCcb6V7=MP-q^4Xz%VtB)Pa7mZRL!Uh4MK*_;t|KoHh$FqNxm6;3~B
zy*(0dZC?m6sCZVsEJZ=zZ+R0FAerldg{f+WsD_nyinzk@agTqqCe+9Q@k>{Ze+*ET
zTPFFzi)sCmc5w8Z*Ag_y;C?8~8&N_J<Z+5hZ9SJG{8sS|-f<@8q(aayB03Bmt`dnf
z$uM}r(vJY#ErcEod(Z7)or{*O+kE7>dJ-$UQwok}RxY?<Bir6DPh&q!y{Ea`JjgjO
zC%(}UTo)h%MXm&0pPncR;!H`P&cd|^a8`IOvgs~2t*G+fefh(M!)YNBw}oiRCd-w~
zmU1zM;?J9;_#6a8Gv3$NqaN)Lrk3G+#A7Ix71}T&3Z4;3cV&Jzl&Y<DZ>oLm8b9l$
zTGefV)hBZnhg61--u-kgAb&r+q}mlBgiKwNnGx4mRH`sh60>k5!ve;x1zAjWhS{P&
ztsH>N8gM$SL4V|OC0m0R<B-*}M|8^2$^o*Yp_&Iu*ncNzR&K~L=9e*(cafWRL**4O
zU+q1zyzU;YBH(swi;4xRr{#%&!8e_y34!Bo|Ejz<n06Zl{2P^cF81?Fs-Jmgh)*3`
z%=!o2UL&Srd(}vH*3a)m4>F#?V~LEDsLE9w=G8bQEKOYoazhL$NTJdc11C!lwjxM?
z8^l~{fN5U_cfm^Hd-RY+STLTBqeM)-WZ|w&-N2Xi1V}stO@7Va)Tj|NOB$-$j4eua
zF(-nQpV$PK-jb8Ob6*UqqQSZ6CkL62X({OC7!kNwL$iNEn3|~NQX<9!MFgB|gX)Cw
z%z6$fWlWdDdEZp_lTgN*R-3aJTo~q#q}jrekC-RWO#rO=@th5lox+5a;hKSW^r>ay
z_(wbA$}()~od!Vp$_?#mSDGV>yW-U~@Drs^CTM|!6&($478kj*h>3yy3O~)*8qFc)
zw^AceG-z?NINj=hT$5M8EvTl8G@Tz)$j0e$q(YzNvt7=X&F=2%lE=^d{<+IJXsl#H
zLMwC|ill=~!I0vJ=^qoY{~bH`J*K`<5dP~2A8e+b#p~Qjx&Mys8X6W^7Zfg2xMhSE
z6U?LN*?O7t^ZWNIBGBWBY&(VwRG(Z)qhvi^s8uEH17j~`WdIh67_k1Vg6t5w>YOGR
znL^Iml}8I6$Met0Z?tj}gM@+grf8>c=C6${T4#NpalKSl%}tUzi|}$nfk(ULHwT^a
zvJe}c_q&(g{`A5j6r+*Sc`rw5>Rv@|h3{{DhEQi<%qf_CHgyHY5#qv^5hY?HSg_e(
zCG=#y9V(IGvHP4F=uGG$NkU5YSzywo*p~0&Vc9sRTt+~{Jha>9>C+$LXl%!Sn^1H8
zO5gRw<F`%Khj30hd;DXWxkA!X<Dkx(N+!BJIIQ#pXS|ZLk3U#xw(b4Zo-(^UsW3H0
z(;6zY>73$hUK6I#)>u&x49yy=YZrh_!eUYr(VFN)KO~X{BtNt3%5#51LSyaE>tZ$W
z{m;^Y13MuL*`#Fw+S6ZLKh+Kk)_vjmYs`aN<DVaTx(}@-&HnTYf&knameCHX_;S!Z
zP^@X$oL_i<MEP^i#UxI3X_DKIp3$eOB$|a_iMaocm4T~^=ZZ+5V}h}e`mIoi!+CE-
zK0{X_#>8wmU0se?kc0`_jY}*AK3k)CAxJ=7k8L|`JFPa8vWa~xpDIYHopVf=%Setj
zwzy@a<LCId=Luck`eX+R%<L!Lli3B;5kg@@ZRRcgnwJUDzivx#ejn&LfmL@o3pViN
zE)OJIiYYq3+nFo!AavwqDqW+EQI%&}%s$%6Vu)@dcfvO%@2%VBLB<`U?au>`_~u)1
zBfjTGSfUUBgmmGMNeLM~oVC9WEI_QN6f-)(R&xQCV-EZK;`sh;SY;XR;hji>1?SH4
z)Jg9PHV38{+X)`Fci<6;F4G0qhog2UND`V|UvWJ6We75H45y!?%|bex51E<9xQcpt
z4O(MIUxYaHIzx|}XO$4$PBCJeV=H1~2E$hl)!H34N?O^Br$#S9&iW9Vdw=lju^?{#
zIC7}WOZTdDpzdRXr+Ktnux`WLpe0l|1C!P&9ITHnnw`0RJxoj#$o{4_o68mP1hGq9
zXzc7#x9K4DHknduY0g!NDIY5t^YF-}Ac$$eh=!bw)E*^?rFdy#+g9I*^P^eE8V7~6
znynrQ#Skt;d8lP1*yz}0EvuIy1Y=vf`^CX#Het@?b`W>6=HT=67(2O-?(xIt`X}J6
zS`4HS3(^e($pwLKhwg-b9;)`{7yq#n-^<HZW>ixeJbMufDDj{e5^6Lt$lSEP)QT%O
z`(1q#Q2V&)x5}S(r~OZc?DWbWGYONqI__)CmZ}YH36d<X;#U-TW;H!!Y?UM*IJ+b<
ziVusifL5bL8B?tr6a+FXO@gIicfBvjD616fahOf%aQw6rS~@lWzDOE8Tng>vU3{F&
z_yzb}_mn<q+4d5*=x88hxuKAM_XFtAd|1bsgN?XO;&+lF>#fiU^+Re+6tnFM&u7Q@
z`0!}H*0WzJT7QdpN}>yNM9gyz-aI6yQ$2ZSYD{s_$Pbv{Qdw49Xnp69x*&wXFk(I>
z`IdFesJX@3b-+`y20y2{XEO$e45{U$1xTCT{H8c_CvcQ7ibNKxnCCd(i>1FXkaD^Y
zO}1_SlLx;EfpoG6nr>S)RN{kyI`ObEhOSY=X_(*}!^}$NBEs`>y^sWmIw2Q6KDrvK
zzk#1CNQ>9P4ZG`*|2vi()cM%caaNPYU)LG>1CrEa{O~rUvR}IvZ-k-?mmE@0yZ8p}
zHP>YGy<YEhAm|y@XrON(x+?y^W5ZgVy@0hP+dP(7VwlCoB+l9NQ(6bB${$|NzU+vX
zw~O)#WSK@En3-u!4zniBGfN8AJ`bluS^C+svq#Ar!5ux*7pGThq|FH;>Xg#8ePcs=
zMtR4Iwt=~Y$pyUhgK8ZC`1c>?4nUudBZa>h=w1kNjZA^m1FIRUL!?hLH_p0@LPY}7
zf5#FMuyQTEObvHkDhoY8m$z-Jp(zEN+-|G`nSV@)A8-pRjbE5}USKW^ZE7w`G@Jf8
zexN2^=Me!Ocg6d|mlzp6)%w8*EsrQsVfH_-8*8lWCOft`zFx$&sgs<Z$5%QD2}W}~
zc9S#cX=@`8%|<~D!`N_E$=vFvqD7HB3w)8L(beoT>qN(-vp6($P;#31uZ&<}L7u}!
zTm@#v@)v~j%_a{sNrfk&VpghTbfKd9J0(v18#<OhIl33=^`W>n33R*wBT#PS)>S*C
zMlB>sLx+m%xP$5`311vMI#M>wYgSI43B7r#$=?p<vAc(hz^Y55axQx3D`k^!)jNrW
z6VD-nyg>J?#rx^*Al2*U5$ceYRW_6~7Lp7CN`}aP^#z^cEV&aCn^`mf@U;+mpM7G-
zq?J>XGt+z-y{VM43Q{_q&}O+4Y}JpBGccR*COfD-4+LqYTS_h5y{1{;QoE&UkMFOx
zZNY}&sD2tegP`j6EvB96FqA4hhplV#<gf%R;mJP5-bM(|Fb#Kl3>{7jCo7eOnezJ5
zSM0@G=0lE@B)e`Pjuy+bt5>~GQ4+lq+jnh;EFeKXR`SHY>peGHJ}@-&Cf;FHa2QHw
z*W)wDhJWd65m6RT(#roy><Ln19xY2Oud&c%jl(3-)#Dh%0<&8h2ZNdN5V`SPx%pSk
zHEqIp=o-==%YaQY8!OBpcDo0Hz20daa`@Hzck6XW>>gmVz;SPd*80rIBZlo@x>mwL
zg${=n_a3uRt*JGSoBT?ie4R<T7($591_$0vGxh^<m<=I`bcTf|5r-gpZY4L%f_1_|
zx8II-(amg&0UB_fIaxK~@g_NVpKfoLp?fE{%wK9~zi2#dFhJHWY+7D?%u+fZ-#^Q<
zzHkvRTyggRRM*VcJ`Je5VN?=_NAk-a6=nmr#FC-0-)<)j00Q^wN37pGU$$6xt#E!{
zR6!*b7zXEL>n<Ax@w8WV2&jmuOr(=LWC{kwU|?EcomzF7$_FM3VhY*(VJU%|r~ccf
z-Xo(p`_6BXsk#g<Sg>aFH)YHts$$AjOb*4o?ltHMu<(7Aw>o*161XmZ#ko;L)<hLl
zhvev0r@QtIi&3Wizw}r^Gn?e^%$bLfqQ%RyAXEE1(*|d)Fn~BIu6}y53(1COHPL!c
zdlw1slLp5VbJpl0+Ls4OrM+yo_UH(_RgTeT1(B}%B;&6PS0Ns<R-RgE>sv>$UTZwn
z2eBK|vAo>Siq3T}UYFYU&Tfb;@9KT;fk)4=F%LeEIf93Zn}x%@VXA96iQgj*OTH+L
zE<2z%jY6YM26|1Xdf^^+>Az|&NT#BPI4imDoDQ1uj889Z{IKtR=XG%tuQ;x_v{#I7
zG6>x(mo;N=-N<JP=LuVa$Ofkc@@#{bnFhE_+Qf)1L8h(S+0Zq6yGUmHqH_;39kU5s
z8K?suxmi(-pDn*6zGAl}oQcXl{piJE%8VY3cp4R6eY!nvo`XEgo;Rit)m8f(f2=SP
zI$V7E&n%e%O?Q7I#D?;+Renh(sc&jd&Z+}6>Hs~GSiE{CFf&iyO7F}ekBk*WI!>iS
zWmuckc<Ix?3mMwtj$GdGvl#N7p#NWD?zmRD`ROV3-{z+NG)S1aC`eeGvKFU;(!~kE
zr%SEd>q_$pU4L5G?<7)!lk;V>yf+3PT=Vjna<#kKAPA0d@@;H-)aMA~iR<+mRv<d9
zjcD7lC^7uOwjbDVEL!hzyqY)G`$F!lOG;VMrqttKAv#^D#SV0w@rQRhgC<o|OAYyr
z5BuI2IdMY9CYlfSGn96sGar2T<w2_yXUT$nD^_JzeQ4dG8%yWb6dWHMM!dV+Qgo+&
z=;|A8ElT_<>D_Efis!A@klRGzZ&&0Kp0;8$PDcg&i_bWR=idO3g<l6VJw9F&2w?RN
zFu2yozLM&);mh4}|JH1J0zPIN3l#URiNk?b^3zM!7maoXT^#E8YPIzg{w(ji$6?7-
zloG!9N#DZSP?d4R-$DLf6+cSe|NKf}{kyVx@b`mv`%0T)AHYQglcTt~?-_d^j^_rS
za~)Oq(DpPg`M+b{e;WTbcFpvMKc)WsKlv}L686OY4;AU>{~cS!bN@Rwc%zN<zc2q^
zJk9^T?_Mkpel-@A-!%swy6usBW$#XHT~gIgnNYgINu*2=5{!PLfPVD9vPDS{Z><K~
z1vf|)|Hm1mihr1@^pl-TTL@GKqJ}+bjzlF@G!~CgLa<CV&j0%$fEj0;v#ws5>iB^7
z25jn9D5qmOCOom5agSVuPojc~Jai2B*LzC+Ym;VkTwo4C?_`u`KRb#$HW8P(cjpfJ
zT)kAM&67Y%zQ>&*ZIu9xS_m)A6AN`B1vYmoP2vw~<^@fjeh;6+8|PuXvqP;Mr*d`R
zAQPQ@e$##@VdQJ@Ih<U4I?Hn}w?nRja*q&K;qAWUNC;p|r4A?*k4ZgZO5_Lsjp$8<
z-XTE%?K+%md}R2(WeNhv0rIe8!J}Ke(^IHK65#xUOBu>LTyg(3+V){58=da*KJV0*
z!e1n2^OeOY<{Ii<Qqk-p)+*Q^u0Ky0^r$U3@!ck%(3?{&Cf#6Xy@I;5PA832ipmJ(
ze`skrNYbZLXBuz!$Zs1xGsK2WOh`V<_ecP$KG)jTXmCB}3h9Nogm7;)KttQn^G!3F
zPOSZ<c8bMwIs{0>X|m3O@3T`qHyN^eet`|4&R(FW3{BI{It=4PTdbHV;48xlL6uH%
zYu{cMMgEhWe{wkDdatxoRQvMC!{4)A47?_x-X6n-%kDS2#HLNHiN<%nSmhQeJ>ai?
zLGN!Qt_UiV77{ghq$QitEodsk+>;;6&!wBH_iK7r!y}3&gp!@M21!6p2!?6?fekOt
zB~Reg;hqf)iO;S6GA#*y@DGW(j8iG=X;we`L`~mtJcIKFZk|Xrh$~qzZ8EQwPFW*@
zv&1vfKgWy4NgZJR)X+!6xWdKj(5=?zd9I(Apw!vysbR9Sd!BvF6oU9j*jgQuh<Y3p
z<aPQJ8|=<{LsHm9FC)o7(i~pbtqU0hGf#1xpkkg_cLXOM0din(>NZNsV<KmJHTr*8
zuFZLf$+Z}c465?9xo$V(<pcLWf0mi9GN7Ebdtw?3HoWo(#$+O(7ems3c9yEIa?_>_
zsMI-iT@UER6t+t?&ojey)HZzC_)uv6FxH?dH6`ReM~wdl08kX1v`ZSmQEz5_buwlq
z&q{I-bm$EFdEhR;LTX~E>g|<@ex1QueTT^6?g-2GVZR>))CDVtT6AlxoS!dtro-vx
zb*9s!&a}k#9=Olsr_ZcwP8miM5_7<b0W!2Yr2`Huqr=moA<!@eGGBAlnc*Sn6|uV+
z;f3ACN~)g(8iypA*4pZgb{Vqaae2G{RGPr};VTM}9{lxK6FeYIj$XOrGOorYUxi$)
zgM4AGo(QfUH+gxSbXnz}ba`fjk6{hh?KZCeq6h97>jkU}Bu@os)=IH#gAl=#bZBe~
zA1BWy+sj>Wg<fkzLE1<&=CR!-(r}RPiP3|qG+zvYd#nGYM!gwSbYPWEw=Z<R!SLYl
z?XZ_@GeV3;3=O&pZ#ab9-*U7_JkwNtYwO?9$OpF}ePI!*%SKs*Mlev{MI=*9TqWLe
zO0G&t8n1P`UNcqR>M{V3<RBq{x8I=~mhV!PAcNsuDVFx_6pmb)Vu-RyTBjv>^>-f8
z;H9>nZWB>r_LHq^;+dQLcwQOaI+;i}qc<@JpZ8QJ1rAgS@&<O`Vj~%ZU@QbzD4LtG
zf=pbX;cQg7#6j4BiFlYNNQc0{VhVxhz@Gf;>E^D23VcIQI%uXf=Vmpf?}}8O@Ze@=
zkkq?_+b1e?!$K^ZYuIO-Dz>@?;_{;lv&<bnn#Ps#kL3L$W!P4(oU&!xvr~EI6?D^1
z+mwvg3^>UPyu1xs<KYCPNb-nQ{H{SF{d`0pF>$}r4RRX*>}Pz4y$Gs&_SI1acUCrg
zXRIGqb?aq+jvs!}Qz|EvRo*d@6M1Fm$5)T8_)zOJodwOnu*Z8W=N)2P;R#?kb~;)k
zey3a{!Fa|z=N1~{{+xyi=kH-{p_cp0Jl(R2d_MZKjos8#cP~gJud)QE|K$WF>Sv~9
zguDRT*QpH3JyTbm%}vF!*<R7?l8RavSS0M^b)&K7hU=sI*Nu)A3SMp(w29#&Jf$Z;
zuc=hv%@3bf1SuW~oKtUP1_SE<=N;3?_L34H8pwW%H}B5w+;q5~J!A2A(Ji4M@%6ec
zTM~FKm%*DLlgj(`A}Vm?h#jtaFR+Tb4C9s6^TLRgUzdZ4EK1&7aP|9{<L`^-;JG<#
zg;HdYOq=nekT{daR;N6oyyyfM#9*xB!f3)>+lJEDql7;ahTUzmrKB`Adu#M9a<oE1
zG@ksr{o?O(EkJ4#&Xq1wRGW6CR?yu52!O`BY$`#-ww@GAV_Adsv{TT0wD=t@!Yi)q
zhR_oHPJB12_%y8*No`Gr=wZ#aSoj=rA3Lu#x%GtLpGdW%MfBizn-B%o$1yAV&i{6_
z?EYAon!Km2Em4-1DeGV7k^0i&>5V?y|9LbJb2xeSy%-J+$W`h@h&kLQlDH6I1lwUZ
zeCGW!NH?&J$S6>~E#A!ph|jWELB?B{L(WUP#zFFsd5AYZOz7lEt%&eEs5}+rJ^gkk
zVRV?F`t)vHT{HlAhlpoC3kt~Ov2?w$d3DamF1$K++&{ACgh@gGJnwq5aCJr|tfy(Q
zryo&F5;I+RJD`WW2*qtu@%M_ukGf5ynI1TWP$wQXMfQX}*&En+az5Q1y7yq$Y0xOf
zCPaSw^!eAX8(ep953ecM@{WcV`oLF9xY?6M1-FX}=O)r0CoRI+!MlWRdwWH^yCIel
zjcNs%>;OZD(pdtAX|QZV2vwO&K`b;Pp-WsTN)^SkdYW?1DSm$bW!MIN{fkDEWn39f
zoOTMM_g1{##GXtbXgfBjKQRq2<-8S^d_G114ZVtl{Ftrd(R%<vl{JrXCMc9)*kC#|
zLcV^VyeQ5TK#H43!@xt<QDLYQx-6n9e7>k&F*D_H;;W)Up6C>52BTS1KbzHL*`7W8
z-?4yoQTi#zGmNL8aqC(~P!}WN)aTx4Cghs4+UJSD?eUuzRi2w>w+(murI^nBc>6C@
zked@(FVr+zQ}AdQ$m-BWvL^OR=HJMFHH$vXSqGdCSS6d<(AS0&4;Q_lm$L>A%#h&<
zMuk1crR}=pc~*i6R9FN68TO_6)4{tBx$XQFIZ{r><&KCa8NW;acg#>~+kz`_`F)BN
zOaJfKv!LETrNCJR@^2*OQXdzMsH)OrH6l_VFcaOo%`6y<4eHmsyTp}Nz(Qu`8JkAq
zo|9KvwRWT9e-@;#6#y3(Jx7{9Ii6(v$-(QLyAcwSAeBXsEQ^%(w$I;eD^xK!q?`^s
z{gbojyPy8}^W^bo>MdNCr%d2B1Pb5cuFfB6y07&in<$tRLTQ&i#|2=HOOw>784P!+
zN%>}flas<hMP@<F%%EHGezWU;WkqFlT-s}gZz{IVtQ@3jgwH8b!ygCL3_!HUe=_-I
zx1qZY<;%|NSp;YXYhUZu4h`uH1->BEp+iE`Ma9Z<GF?tW>ZL`dZeTZzM_b5sDq=o>
zdQNq_sXG*q$G#2T4<lS>yl3*$%XuV8S~RRbeK`K?a_jh%Vm1HXs`5t06XP;L4|__%
zc1;ycF=bQ})B@Km@}N_F-(hO3YM9aSydnR(lz5i2JbthoDdwY%HiXYWVwi0gpDV%@
z=l$d@08b0S<i%5nGIskeXYP0}#^hBgD##h&GUiMb$%2q!*}#jW;I&x~hWQfs0;_Np
zaQqj!7vHL08(BpsjP6sU#H&t;$zBMxhK8e06tx+Hx=aE%9NBtjThiw^Y#4o>OVBPW
z)nmuGPwfshx9XXUf9vP$ObK#wW_R{dLqkIkZ^zcm7p{{1MMtk&&X>hp**eVdT|LZE
zH!*2n4fH9<KkAZhwG76S;|>fuL1Pmi_mVs0yz*Muy4+yKSf0Fldz=NoKSa|oaNBEA
zoA2o<_55gg&B#zH>2?&s`^=2A9a5OJP<T>6R}WZS>Z5+H0~^ot$@a&QP7#q=r9xyG
zi-7SS&x5G4V$`q4G3*3n<MALUpAOq9c}4O&E(zCvG`=zYkM$n7_013Xu}OXu2G1;V
z8v%DtkF~SuGs*TzHDKbc%)=LBR`M?u?N9VH8<(2a*}2QOewvexiP4C$@@|^g+%b)a
z89g}RWn#WoDK@T4(9{IkYOW<il_LNCUj9VY4tK3QLd4*W=qUMHCmtQIP3B(Fad8>>
zI(<3)av#C`tQ)7s!R_ud(O8IOeojDKh(=9z;QyK$>ygC91~EVbA&Bo3>tGw8sij|X
zsu$>HTK3t}OmicKS!kHI;%X5C;RtPQAs?MM2=F1A+rYXwNS7wkRk^PcztVdo#iS$f
z)Z1CT^P_v!jtw8a26pxpFg<$kVJEeVZUv+lkxyTB@%sF}F_Zixv{+uQ3xV{kSh-6H
z(lJ`2%i4*7I9M^J6`4jPxnm7+5rAYW9{GT(N|(N~Drk5R>FAwq^etb-|G;eY-P!4#
z?8@@xZ|U>K**o9TCu)4~g1Oh!Nw?Gb!M?%xmZ{H&uX-a15g@^zIaOY^Ki%-gg11tI
z<-km)IqMoKH_iXJRDB#3yM?Y)@)tWfw`R2}JkSr?^b^hgC~8hOh3A=@<fy8kawz;(
zZs_5pqI(t-_#zz3yOcINFU;MEOGZ~W)r@>C_w#6NH3opRIDDxE!GZDo)adCbf>ExQ
zS)xhegk-~;`pYeYb(e3*BZ|LURM8I_UNA+Hx4sj$VnEWpy^5}F&JveW!iDbT&>+wq
zhJuRv6=uwb`%Fh7#0|*Krc*fo9g8AK%+Ak%;QAy_&pGW}z0pz+NMu6DtmDNgTy6EZ
zw_2g$l5&hgV_<QOL+Y&=zA^{46WUB2u5$13)z)4cF2H7E=Mhk9w5G1(6qG(R-VD9Z
zz!)qnpu|J-k_nvVfhLk}Vf}^-4AQJw6sealfm1Wu>U|MICF_cAnzqGstu6=}Mb!en
z&U#j>_Ybq;c8y2nrKptUH32FaO<cno)y6#g)kYp83JR}$?LNl3Kd_YjzyK;Np}6TP
z2=PfwG|gQzUN}rlZA6iyprncUX-@4M>XPjRs%D!Zsqi>vNGeQxU?-`LIZJ}ViTBYg
z;0FXGv{ppInGL2b>beR@c`__}%UPw-5}ylW%jWn!M#qDihoUdFBZwPH$@krDPPiiI
z8{ex`ucbzHxF(o>+803Kc)5_BYwdhkD;tI`nXD8^sjNUlt03H`p=`W{H~gGcFZd$=
z0aN=X75VLbv>JJ}a8*&x>3J;_ZKhc>PTaXq@|SHOQd<Crl-3!8Mej}Zr+Sl|7?BkC
z&WGCuHVHtO_IsT#rf5&_#&|Rsbjx<EL-o-OKz9N9kV?no)T>(nT)CBi-zDuZq*;3S
zcy8$eqgRU6D?T#iRKJYb2v;vR12+K_>Svycy~dAmy#VQ6X=zy|J=%0iZ=~t`C&Z_)
z=&GG<gk0XZ_q9f;Kkc--p6QWyJUskrTpZ03OORF9Po#z1dnSjIn6o{p(G@{gI2`ik
ztTf<6Qx}*Rh6;Jv9Yb9V9Mj&P#2O1V_-Qqsed!tq6c&JmOU(4q0=-QVuSWY^?rc4y
zwrSk#JDW=ga2^oyuVx?TR>O44E0psC)#@Ne<0amn;&H(02e2qO<VR)ugX|P=w5%YD
zuCBgrS4;&Ihc`CQ^;~ac9}Mq06L;%51?!sH=DP&zz~UAF;3O(_cym_;Nzc*A%?(}P
zn1y|}-*_^I9ApfKpH1-CD5Lz9zJ68Ua*J~MWl_4XLvjX&A7Lmlbu;9*h0#}TDo^zE
zWj|QjSx>YsJj$`I&S`%jpuoh);jyMd*;38=!AI%t+<@z(`au*X#FNRM&R&Cn;vE>8
zRzcnmC~0&JTdMER^&1bjF`IHxEz=#81<FQ8&59pEDsea;{P|Ay<(<4sv)hD@7i`{t
z$MD<e>eJD`!dP^@ooKkvB_=SG>dzsoI}4H<djK-5ug(G#&ihMlntK<1%r@`(7LNo3
z8)6okeFFN8^Jd;3l6A}^q?I|+M*Dg6SfwU@EF@%hep9htqc!u)w|kj^KE8oP+fAu|
zbQjUTb^anIgajwG&&2Kv-*(#{=AX;j6_A##2v?Zq=q0lQYBo-S>85470qfj$q_B>5
zpk;C08mN-`1R`}2l{3Atw}@nCP4A}Lb%IZwD)=Qp@Ks$L$g>esNeG?e-o3Q(bTR8G
z8*8!hN^u*~FzEYy3#FO`s3|-K0gy%MjWuBC#rdd*>@j2rNkVl@n3(0sa=On9470vV
zovuzv*Kkj~3N<k^Yl%9@;<y-$xQdC=*|CEHL>dZu$^hFJLWT@d_kDgj>dlNYbmD(K
z?Umq75wz=zvDHgNF)BqTO^OIs#v`-8-Msw!UzaY_UhdcVwyz@nH|4Wugl%Y|(3`eH
z4uGNqG~LW>8p6C+&91__=%z3rf+00012<sE*y?-iev3Ul9(Q}-zhea%W11$KVNlNj
zHqsz0G;5Kx#<B{9N$-R@p2yX=54E&b_KTrRVO9ktJ-QU_Kc27mZHrFX)3vwSjsns7
zv)FYJ|62BLh-%JhYfG(}Uuh6co{Z~^is*_99d1O{sFhqZHJ1=)u9>j6G5#3Mn-Rzk
z>zG~atRi4b>u4v4!O3Zv92~-Rzin|nfW%ne9rTQ`{_E(cp%%rxcuVU}x%9633eR3k
z(c$}DL(i^SX{4<8R4?Pyz{l}BHuy`Or=HTz&v&iO*6v~jv?nDKWI6OwjQGglOmb<(
z&~tai*EUROj<%<7FF<0#`eFIQ|BjtvyVGyIHR*xpK46^CDI%M`d#M*O7)IpVWu*C>
z^4;s9DV>@2@y0$t!6a&b;OiLshQ1IVo+^}wj1IwL4gr}6`qO_bKINm6UPa%|A`8+M
zPYyy6j~8}AA#KQk&Tdf5+loM=5mgb=n0W$fd98FwGZd?-u1G*ZAxuC(@I#%>Ny{tT
z4Ef-X>o){S2%l3p>8bOtM|?RcbB-TYV@(3?%5`^S!ZY)Jwl_I<<eTkQOse7y{Q1WI
z-iz=s=!XnLuEkApzv31;an4kO9ji~(H0h0Y9JPTXGYoR{w4<Y=T?}dFlInPP84!#p
z;NV7Lt4Mx4np~`atkD)mrGH%Usb1XaDz|>-(jh0xXXtbB(SOKYJGks&1A2oG&$7=1
zNMD$kDBGOYnFo|?Uwtm?%Z*aAZdly$Ugt0|p*1ZcX9$$I&?**Bj&9V5(`(OWn621o
zmk4G>OpIepHrX6^xCD2;>g+zjjb_bW4iTn4QRAwgzdLinPia&<d-TUO$Zpy{JF%%V
z>*F&~yK{NMqG*ypM^6`W^zH6vdgMGeMc*V8c{o;s1@;CGnNI1}5x&a}gQCh-(Xg7%
zwiow|y`^h;B@!*{1sg318n?k^z=Nq_QYr6S3^9$#<5@6GY*^CJiA-m?ZygNpxctJ6
z&=n^xXWPQAqNKLu%3o47sWia0O{h~ZUi(M;qweg<G^cClOJtotrPz^f(MOx*sFv?~
zx~~%uVP<x{4ll!O8g1`8QC0e7xN<{7&Qf!-w%%KzC>KFy>DED0Q+Ww>U5>gL<T<>M
zqz*5s-ms1H*VgsuaG#f3$dH_D!0W#&jH|)Nj4BhOR8wG=U?2%@bDbOEo=(LN|NQxW
zd|~_NEE4^Aw``llvaK!z?bMhWDz<DjmHc-s0!j7A+YdhF-A7ZGIWSoVAkY~Gjzp-g
zoR$U`>)^)h4uk3w(-vjJ<RVuEA+J8R)0Px<H@%Ybxa`q=-!Be-Y3mpX&YOtF6o&62
zuG<D>`40|gCtxYj7a-7(bXRT5C~&y599rReXcu}3jGoP9bZdBLrEA7$qTtsaL(p)7
z*bZeK3>zyd$21gpKjbG0BR0B$nGwn*#WO&%gnMGW^oJ?=jYCG_mjkc!4vxz2ey@GP
zAG4^w_vFN|MpoFRUhYK~Z3p=~#OMkN*Z&#Q!p<dnki<~GQ>ke@UNwN*ahDIrYJ*ID
zXCmc7(k=(B4M4P7DtcUo^2I^kQ+czSvH6y@!ys41<>4`2oW8V!dF*TVIOZ#`0XCyl
z)2&ihT}Sf!J-#VMP2$!Qqv<!!n%@yj6D0q@Fnmd{keZ>jhkXYz1Mfcg+`zKN%Q@}E
z<ycZ9<M~cxhwU)EI27ZTJLNJJ-fm`4*CU-hISW{HbO^qD#T@XY=9iuPT))F?La-w-
zn_u0$@G>$cXnX9PLQ}b4bl`)oDtqCC)XSoiwjWB#P(*<_-AIfyWSD{!#X=SJUh@~B
zI3yF%It%@>AaK^S4!~#vkV*=3@ep^<4`E3nSyP|vYl-zV_{H><%EuQSDvVpPtQEP4
z;%yJlNvu?uhsVc>33ogBYY^>cfgmG<yws9Tc?<10ndedFVl1!&yboJHDONfyTL+Gl
zX#K22#_^LzWYSs|(K)Ut9nz4AC}r|w`hZU98UPrW6gYLNf3&S^xs*|<(hRYu6sYbi
z0f~m{dlC_6VG(Ctd`2QHb#;En4SXJf(VNpEHLb@8Luy_+AjEO{KM=yFBY{4HEDu+l
zFw#lWIUUxC#yY2Yj<m7Cbh9Nz*PdMv3*kKg|Bxfr;-)(vio=ixTx-Agg+=jg=m3TF
zo?i27d2ny<XtUN@tyZzy=|_)tRGUt$jX3$9Zd(7EZ*d?|UCpz&U(|iBK&mkEIO|qV
zFrMk-<9_C*|0C2-Kbv`WU*{pkdKuHftOaT`F~hQ$3Up3$CcAI2P1KfqdDRh$#2lmj
zF^Czn54e-Nu!!4TWXQt8Ob7B*X%F><uO`*W^rL=9noaSz$6l7tFYSb=jaw*NJwxx}
zr?7vHI=1>>RQ>zwg<_S3_p_lt1Faist9PUCr~aa}#?Me!C9gkxv11xz=B>B9v5;23
za&V8bu=lk^b9VAeGbC)gb4=W6DUYFJ^@6S3?1>pwi?Qkl%AfPk);96)adrOq+%Qa{
zU}*GeY&SNtX0-h5_oB1LvLncBsqv7oGn=QAo}Var_NlfuMNJGW+SuI1cDA9FiTE>1
zEQ|cN5J;#d!dU^>GE8Ej%f+Q#yz~ui(%+3AGoUgo^L((#au0NfFn*rJg^CZ$;+sI4
zSj^j7Wc3r?>I(ia!G2Ti4SO$T4qWhiwOlpU$+c_Tt!%W2|HZ-QR}WA9tlpZZ5aoK0
zzwW>BP&FMAOFlXfV!73Al8Sk47W1TCFPL+M7|*8yhlGq?IV33;C#kvuEL;^c`Qfa?
zVL7BY+MwuW8%zB%;a;nKNVbgEC{-;0%;@>=;Xm#FKkU5+SX1qqE*vW$Dj*#Mq<0YM
zT|ruq7J65a5<)<FCl;Di>7P_70YViBgpN`Lq&Ml(r3a)$2;nUE%s#Vc?{D^*|NPU=
z%y(T}R|u@Eth{;G^S;mh+|PY0uzaUsW~v1wh6+aD@$@c-B+GU6^@-QJ<wn2*>|!yd
zF9ei$De@@{$dsDY$5Fh)uL^EZJuS_9nrPz{-&m56Uh3Pk?^okn>!Tel)rzhwg7M1M
z)aA|O<pvs1I*Tn5=y<x`4>03yvM`$~Q`FTDmpgHr7Yi!2w_=PEf_<Xip5-v6+22wY
z_@GJM(TLTgDl4b>R(>fuujoN-H&<C{iLJAPhS(AhxO+-tzyEIWO|xHRx%Wt0J~%fm
ztiaPQV~JxC_&JpY%Z;ug!Je$baw*>oCPd5yC*H`ZczQ!oZABVD6Qg-k``bG){9YIt
z<FA5nLn()YURiohw-GIYl$-lCgVlVAFcsmM`e8k9N4IhkNLqT@QSy;!29FYDh?J^Z
z;+>+YoKEOy0VFsLoReP`uYD~^#q1idkkMIXw3<v@`bsZ@TS6|idfQlw-OocH?Ccqb
zZ*`29dFFiRS{S~y5|G8I-;c$2bz}C~Dfm;o*0ygw(uVPA3`P@o)L=I%gEPtrgT?;#
z{G2pvhY`E#NLs`F!a{xBVXwl?9z6|XkK6H0xrYe*tx?aWl#1*id#<x6@#9T@tAa(c
zPif`hsT|A<je9h8_Sap1dVAAUe196Kyc(Uf(|AVX!|vg5Ed2FOrIO|AT%keqA?n)R
zabS93KZW(RVkKA9wbVInt6_@toaObJlJ_|?8fFkwSW07&Y91$dUQ>oiKV^QM@6((G
zy{@#BDNed-aUpwwGvjYDr9}ddaa|qc>Vjs&3rp5Flj@blhTm{u65967RAfpz$s~H_
z#`0cCJ)C>?X7yIy6Z6yz6IBDCw&+pLNzajo+~wt~@HEImFQce)p6j|Aji;}uv;uN{
zBk2Vsrk0XhTd5d<Tp}&-?Y)G^43!|$+SEB#94D;M4C<x8GU};Ff(hMb)TzwUgmEP~
zW%9838#81i=TfrL)O_Kj5bTlTgLvM!6BJR+8D1=>;x%8rcJx*!UerC^Zg!y;azkv?
z7VNP?&1+g<SNFZ+iNi?i11m8kSl8`14+#i#>p2s)b{Q&H^`jCqGGe^jS*lh#D+)~=
zy{vfmgp%tyf-Hk6BqF^&Tym-zuCj2f7s^F=3VcWAC!g%EswT~jrQ9KoN(TzG97tXO
z>ZuHtPv00W<pYb2Qau?^%}9rf<=V>Ix+nR|_yoeW!2x|3UIV?-D-f-k@@}g<>8H6`
zT&0&6-;u>2>^{rcyCdWnb{ev$VF~85^JdU4u*q6Ob;KNxkQpq4Z{vW<T=OwJ<ZH!S
z<w{Mp2=nmOR?<l+H;kiUlAp$Oa*~RkQN7VGY$g316u8X*cSECw42}}q!zwc|@3B9U
zhckG2QCwq*LPgqyZhhm#ycDrDUK8ud?eUo@$?(Hf!7{GKXS~@&qPZkjnrR<WoF#rP
zX1yrlngp^4=De9$8s_6omD(n!9N~>_uG*F_%1HGJnx}9!re$%FRdfrhYz25LuvV=C
zOWoD@>P0MJV6t|-*K^B3@=IP;Mf6yKslmyuMXpqdXr^p?NY2A4QxdxC*<*v1nW_i2
zpMNHWKIqrz5fVv=gd#a%9o6-#km&KvR6%Qx@z6a-K@B8Fs+V^aM$IlGv%gQFPhN)<
zi~0BrTV0dllsZ@C?Ygkp?JT&n$^aSQtZrRgdQ?j@W!co28n8oSuXk~lBsbJ#XDB<&
z4z;hEArrI(U1%=JDH{p4As$YHte$PtjpU>H%r*oK4A~)-s5FbYAss8fUCO{Z$L>-0
zL|>=WteO%eJxYbI^0{6jPfx~;OW%aD7tSENRywpk@r!GQ;cCyTlov{)Y(h=oc^8Fm
zw)Y%0yQGm{XZNMJGHjvJ%pG2CukFE}lRm3T7%p5=y_I?@nIuECjvjes>>81_CYGjO
zRB;>SR@kB9?sh6=h%$tX)w#JF7N*xVl_K$)h1C_`tY48XFx}g2OU<t;_Q_Ho&B>%i
zk5OtojCRhnuM=rbL9SP=y33_CPCg?h-hb$mUD%WOyw6mL3fiidxFqvD`-*maa4z6;
zFA~Lo!W;6ax~8UxCVuKkvM%{FrR@nwG}QJW%`(UJ9W~A{+&!hFS_L$T2EK6XBbT~~
zQWl-jM$5(C#IF_i`-Gxe>{XI^)%TZ%Af#RH*EQER;;d&LW`Chn<~SzfElPw*o_-!h
z8mA6f9WrE&)8yx%l#hF;S*i-I0s>B!x=CPtEg{n(@Myuc#2I032f$-R!(T&VEM=zv
zLX)AvJ+RGzG|S*apA^r(9CS=ED_zYl+L%zb3hhzkNh3Du0f5Mw6fcN56l=lxMe@@2
zof#c>io|biD_5d*Ufl~(bZvZCArg;6jV9UvvRI9_TEhk4#B3vRc1VT-xN)Nt;VA6o
zS}N%4ARw18q@A%koe(RKE6G=l8~_{4*c#Tlx#=Bw+Tz4Z2Atppdsz`y9<`y1j7XiE
z$^u+snG)fgEnzv;wXa<7LUS5x>Mkt2QV%FlGMgUe<K?E;z%;}s-j?P-`s{b&_gtgw
z3hciOYmjO`JA_0SP4#J0Q{{~1A$Q#5eQinl0bqEf9_NWYVhJ;<nnsR|7yA~@HXUvs
z?oNhujJ4(I{*32Ls?Nx9dPRK45K|o@p;WRljE<L=7BbNbHel7#P~|)A&)YOe0Ne-q
zOTh_#lhSUVRIiCdnf~Z6$(t#XlJqxqgA}Op0V*T9Nb1>!?waXm%<?$*a}Y^h^2E4V
zO!mU8zV;6Y>Dk@Mcfpbu<W9XKb*oyclIc@<n1XA{);TETbQpQLvr>{Ruy9MaZhkX8
z?R5>Yu_eJL;*M`2C}Kc?$&6YOg0|!;E6b~fqBw?ONyTNR8dANfH#fB9umw`jV2anc
zCp;pxxPDlSGwKhQHBZ7zi`IDy1-@p={gwh|&_rQslSgx*A&<0bW6>9+<{$w>8H%%j
z#wiofuJC$wZ7t~V=h+0K7B{m&-J_+QP9(4Y5URI1y)lV*ELK=pXaq^S>uxE{ovY!k
zK#m>624&DB3YCjXgt_`RDY?74%f-uTaKuYri)@;VXuJG;YC79Q5dIAl!viVc<f?m`
zO$Su8q7U@IeESKB%UezB{ScI-uQntB-l`=H*23^g%T97kPl-h61w)2ZMKn15Eo<-%
zi;^G174FOppxTNznOPF_KWK^SGMGa*Tm+W<!(wPtf9N#USATWhxu2k&*`vQkV}FlB
zhvhN~pyGsO%<>JSdK^V&v4ttT0b6ZEJWoV|hhURaNL&4SairF8hAi)p+w-Kd3DMVH
z+IwcL!tYcxrX&+|3%dK}g8Gdyk_?_RLBf0DRo}_Oor0g^9v9h7!fm~N$Vsw#XNq%a
zE$1|dgs@o_HK!G=glEd+=mT^W(dM{Bdq|RFoTHG0K1ww`Eix)8H8_`MXiYU8s)v02
z3e8i^<G9;LH&$?U+HW6HDx=}Fn9GYY&~_PbeDIA0r!HQ~-L3{1b3!bzNUCi~q$n}o
zP~O%JM=aVqv9o<QU+SyAkW2nW6F=GUglMY(f=oC{98)lb(fMx#V6E-)(_@T-mRxHv
z;(kLXRenE4vd}{YXR~K>&*F7ah&dG%UQ@Gi`HOkKYokV#5uk?y5>ZlFO$OFc;e)#v
zBMib}BqS~?FmK)cS{7!~`qKvs6{?cU7k=w3wC;scd=$a^WAk0YnHibM8^;e}x*Kmi
z3(dLWg@uL;LQUJJEXLJ2pK`cEZJj<1J+u=Oin<V27(m_7647t7Xz3^D{)m<$zv{Lh
zc~kP#daf^=EBJeurjO*OOMnx?47(E2o)P&2B&B6@1RkkIJCs#7d6aghpr9$Th{_9f
zGSxkE)>b|9lZ+wDDSDsyd{z0?LmG8E9Fhso?t%QhUo^9nzfhQ7nYY?dHfVrN(7qb<
z)mIO%^0l@_SIgRY{IImWw;lJ}Lz_q^@YSyOgBSkK&T>Qp=b)?Spyj7~^JmdsEhphV
zkQWLs{=roa^yde%EIgpR$f?sdf5hig4wzPD-uPSL!vB5r#lK4`{Fe!bWUmf%VkJ&!
z+o#KKTKP2Jfb?t{5ZWW#j@=x(y`jE>sAkG(H7FET*zFKchc4{swa60B^R<h6;bEdP
zuRK|>ZP8r00FiXFHe8O%HdIliN;~rTsnt&+u67O8bNJJ7oVW8eWH^Q6`OJo~UP5^v
z6$-`8JL>Gi_omf3-Vg}<QHA>>$3Oxn=~rF;Mo#(tMt#N{2KM-fa*FYD3A?{%Lj;uL
z<&BV}w?4W#;a@hzzL90xbENY4fAw|Wl<_G_l%9)Ew+Z<;rVTZ_J#)Aru|Ui(R+=MP
zyt7DUv(xJ1=UGQHm{>vA16OJ2O-YUxhk~`&8#dY&qUTCsi#p`rWy%V?OQ*id2Oov*
z^Apd_IxnVb+);jF`;3{2Cr4DKPwLXzyKb%s<|Af{lm1LT<W29q0=k9gK(o40n54X4
zbgk00K7NN+<qhG0S~1DO`uMWB5~SURx0+^G$jVG?49P2-zgsL{bhZ{l!raZCx?*E<
z-R<*`>i!j38Vw`qR581cP#Stus$_~f#z#Htl9$s<w$dtHa^PIU&D-4012!i6b(|8T
z&Sk?5n#CHm`NNaa{M(*&KMDXqAYx8xZ##E`WFd`7fgJN(X=HOc*?dzYiu+78iN7mf
zyPK`eZ|JPW?<t&tZqlyU>X64MG;=(Lxe3rx^)?gBUjnj+_@WRsNV?57Z+|+%*DS=;
zpE~Q4G{mpovskxqsWOSzC`?Ye&LVq2n`-S#wp35<^ANM=M+p@R1Cf~I%uDD!%k74_
zP1*#`D5Ge1l3Vk?$by)VUA7OEZvcy(_x^DOszcMSxhIkD0_tcxZVNmo{Aky09gWPY
z{ptAR$r}`;Kv)J6A`CUk$uH$q)yvg#*Uz&Q?~~cU)EADow}P3^k_1%1z!tqux-Q-3
z8aOYvF|gW_+h89-6<~)8Im@Ycb2vJ&Bt)a2HTz~$Fmj72pw)8<>UK-i)i=9MzB-tc
zp-*r|(<LCGO-}FYY-e0x3G%cj(e{Ro46Z~BH886&+`AIxnV#Lpxiw#r#M71m2m{6y
z6C3D7JuTnKhLEW-NI5B`dnOev<%S%I1lK*C)ijDqPnYIGfyJe7FE?FkSEQG-gfG;I
z%ZsWH0|;?Y11xvmbjA^*+Hs|0rQ{+2N}=QA2+zFVB1$W&?~3-aYQ|QTNSZ4tjC>u<
zE#)gJ@7U44hcwG))=Y9QpoK)M(YtdkbaP+f1(Q1tos9B9<>V#x+&1!>3$B|v<4dJl
zp;E#zdag%rwG7M^$rS{Asuub~V5m8tAkuBCjqUfz(~s)M#rb6&j?~x164vJ!8pasE
z5R*F%Mv<^zPF}qWh+nU8(krD4m)P=3q(->L1MEos#yli_8N!y?WXxh_sDJ@OjA2Mf
zO5u;?)3Pjp^?<c$H0FfF&Poo0>*TYlGnkLRb+z*n_<S6DJ0z2B)HCr~&0n;Iu3zPU
z^Z4t&udYn*X-VJZ!3VA|UVqqcPisRBtWclyS5~|s1>pQWucO2TNRMvfdY=%CFL6!A
z^Ok;rw31#zt6;N3w`r><Z#7idFMUPZ2olASB4%ze7+mV?&@n6IzZlkzpuh<mjCjP}
z{9bc%0xK14geVjWWRm{26c3*eX-e!7xW&_}<?15Xu~9L7^_`n<nTfjrN=^=<EoN(~
z(c(2&s+y$2&&`9`Uf8O$5toTgE~qHzGn3UxPLeW$pN=KbQdKvu8={DqoX$b7f$HHx
z_mw^}m;_3Z2bXZFwK;d80W-MWD6WuOB9umxjd}o@aJhpK?UfUG0Q1c8u{Sjji`#al
zXFe1|I<#xGq4%a09O|@n^*x?^Mn@Obfv@GQ7lQS2HXu>!DDzwEb$y(ApZt||>^X^h
zwD}+zCSB$udIg+xqlxjJ><mR<S3qE(5aq6-p(=_5SkI;70oQ6$Qtd;^cLanzHf4_k
z)g4paKRS2caB&rr;mXZh-#5J;*tjWElUOq^$}|}A?!-pt4E4>HXVN(2rVo0wZsp20
zw9ZWlDcPUMel4cSwAEulZAwO@^vk>;S!stnt5IIgSh^v{id1b^Y5=|ht}$Z3ka!zb
zdup#b-;S6n*Tq_gmy6wCZ}ZD+vB}YNxp8ph<%d5e;}~r7WHV8-S(x4y@{X8Wq#^Y$
zg(8;)O4uTMGcADj-|brwDRm^H9!rAB@dmRRr#kj@!y#5a{d$R0A=3RK>2=WF{1PU*
zjxJM~py4TVU*zS*<yqBi4<wz^k5@_R<xsxB%>cu_;5p7lY;)E^uX(YhV5z-7aN@XI
z;?X1ZWxc*)D`}^?0&bc|w4{&qo=^3Fjo7}IJEU)%#)Y!^_=@lP_fYYv=0RYo=})Zg
zwOe*RA9}J7=?s<|<H+6_YD5aixspd}-<X{UtIC+D<2A@x(wNf7Uoqb}0$U)DyIN?z
z4hY4ay+ktE$@|H(@r#yOk;zPHiUxUdzV-1S?8xfc`V322sJN1+&$ibL+kn9a>&ETk
z+H$q;p86tu4?ROyR)^er0`P;&_4)JT&n<csO;aj{E01pGK{xS4N;`Kwll+o+%gW<M
zJT}0tjmRx5t_UULf$DFTY#o>yK3=?ua&hrq=+n_2tX8+LMURH0gkNjB{=E9jJx9U7
zg;_~0jAl4__*|RO7}ZRpZl`U84q8y-%qql5mH*f+8|d)&s9ae&N=%V0kTerA)-(8N
z%iw9AZt=JXE&~UYNqGAY10#lq-P~NGhAoB+keqv9M?L)l-Uq^t>kiO)YB}3-Tx2VT
zHr4OD0^=UXM5s6h@DwmxygKZ>3r{e7WqFUJ#D(^AsKeEy!g;f0%QkP>myW}por#4;
z?N3C!453eE>S&vYM?={{WYUvv1*d0kOLG*E*#o>u)6QK^K2K;7&UipoKbBKdN*n09
zmFkT*21l!R3>Y))rBrBBK+5R?Ut6??{DXb&Uw1_OO)ty8W*-)dM~r&PQ-1R6M=flk
z4w^KzKBVq5ra5feCf%s~xkWAfxUPonVRWv`J=)d^-s;v5AlyLPJUSocTi$h+g+Of#
zLMyB$xe`Q%ykPN+ef&B@;`-h<6YP#$H{2cFv_wMnV7Gu&z<E_N^f?)-w3!!|Lwsl{
zYB+?Rw!ZcKNO!|%d<aH4P+&O)-p}UNl@XIP`tk7S#~U^{@8If8m)I)}ty-dJ+|p7@
zBw~RGuoDj}G|~75m!Tc1rhE2kXT+#3vwuyu=GFc%;R|%6L;;Ro^+#+?mX&en2#cD|
z5TTz-$O^I?z1%Y@__nUVN_M@CGh+dhqKc^;brENbEardLqZa6UB{WRFFHg{Ea5mft
ze(2w>0k$x|kL~+#=_NL7`^l0pqt%qdiGA=E<cD7E=(d5yW=is_tHZ-RrpTsOYi)6k
zM!J1WTL`<DB^}LUyO6n7S=sPi5F=Ap1kjDvr`&d8nU_5EP!t#8KRwOZ#J$Z>+WYXt
zciLSXHHcR%owqUz^`RFse^mX3y*Ajg8rfS~403cpplRy%ZVvY#E_^5P_f$x@S{it@
zw6G>okY?39{0=&*Gz33lr6HK{l|;Nck0S5RjKu?vNMg`3KJZi@_xe-9)H=BjWM|Dx
z#(*IcX{T7@LepLLEK|y9sUA>H3iCYm-61ubu`Tdtw5gUutz6Y7z8V&dcP-tJtd{_w
zBFAI(QT+XSrxWjB`{7c+T+Wv*P}|a%BXmu#M$_S$NAl{_t`?^99UG=&do-@Y?ZT1x
z|7lkKm)Y}QD0}%yKb<0mgeXht?t|mY6jM2R`r0>-prwqF6Iu+=g30{US=q!aeu$-P
z-b9}~OYR`H9A~#ndV{`E@Ek-LyVWh+S!hhS#X{~o5BqVQ@VJ36dTNHM<@(-Q70$3r
zYsdcla&?ijLT`%B)4C^cK}L?mc-*Lg!Q+CyZWb>It(>|vPmaT*ise+OdEDGyl+{~^
zR;NXT+#rxFht->6Q?`y51}<+DwELNF+Uo`Q#(FL)lFxPBrD#+a65RFFjD)>pP+z2w
zgAkj9McNd7&^w#qR+Ujj{%n{7Q@HCaMyoM+EN!y^o(rmSGew>HR>M}qh+CEQ$4<G@
zS}JZ+gZk+!-B>HP<G?jfY}VG#UvCDOm;J+=MSCs^lh1WXP%J%=uiinq!k@Ei+*aZa
zeL%vxNW4+PNA^|KV0}{67%rC8ILfxR%QZR!)sHMJl_ch=b1S}Cu#4Pw3@{5&Scy+B
z;z8atcwZo0t;I<eWVM~QyE&Dg!CVd(3|=*eSmk~*MS{d63Pc!2=}$N6f4XxyF`1q{
zS)#3T<J;X%J$;L^b!e5VpX@G8R#|;1(}=_HJ&)xAMk_&ak394)JvU85erllJ(j3cK
zW`Ohpj5LGsQHtfO0o`jH`is^S4cV0Rz2*RU_J?U>d@?kzDxN4^l%tPLlox~TOdbm3
z<}-6Edj33^(#SFW@lQk9eY3lbfuG&hSlpZ|aari5L#;7y6gRC;U5{4pl=uL>Bu}?S
z%_cOfsywVwG<zOr^N_X|TBL&J$}WbbChCGq_S@v^XB|UfYB4JvZ5jnLCNpwH@ub12
zQpjkXcf--w;L4NxrxHo&@~N-#65lT<+G{CY6B`{hFcvV-_pWyq+wh0xk2V#cjg1P6
zB{to8d0Qc<J|U<!rAw+-K_3VH9JDa7_0D#oBYCsK``B;1KqwbpSaOs?V68Mv?q{d2
zNGfZ5P_#j7YnXm-ZbOnLUr~r>`{_g3<ESJ+AsRe_gp?Z0^IIIEhI17?RaEsnpsFm5
zhTPEcFvSYgR&F1UzYdfkb{3_b!Id;o2S%C=qRUq3`KoOdXKU3`#R|H&pKNn(k>km+
zTQp|*i8f8i6YVKn`#<eQ4>i(fZk1G-YYHy)KHfvSqPae8H-<)Vh4nh*^7p53@|l=O
zWUE>XqEi!X`-?E!lt=MR4>vTxT3|O6FV}{F+xM1TzNcbqrL@;lg-lgOJW`i2Y*yW;
z@@%D7h=sk#^;YxF-pSn=Ql`>#^cpeKbd>^VpPtijRjBhgRmRStY#>-(71!uBK15by
zsZ+`5!y5Zcw==RhA)w~ifSre@YbHz}zX*(tem_$r4CNJD;fv9S_qaJD($b81^%LBV
zk$$=vG69uIGX|^>*QlqXkVN$8A%NcaSap5!3y=|trN>lOK`M&n;_Emg8+yeCIDq<G
z)Nr5Nu!h(^@I02iik4R<W~Zk|1T3N#WkXlkDgxL&vl1ieRC!f_ziDtpEm3RZF~>-0
z0Mc|vN&b3p)=$iv_N(cq>}t7;7JfrJe$rfdQfc{>RnzOkgm{x6`;A6X0W-T?C|T+z
zd+rzAFMDy6($R*h6cGiy2GOHPW}_Vf%>p-7xh*Z<^Mr^>x8P?Z@sDP^0i6<#IvXL+
zonq8}t-I9(Ton@Kd^RRsIGyQTwM6zhl_iGb)utXpNYiX0*PLvrqS2T~Lw@kvxktxl
z@NHzrmj|O#n(u54-=bd-dD~%1R;;1gGc(Q0w)WhNtrD-7-LtBW5ne?Gx-0srt}Jd8
zxs6Y-b^gQA!w~d}Z&{EDz~?!RA$MPW{@S8xersYub~887EwyBTZCbEhrySM?_Yq`^
zAV#1C`t#u8x1NMbt-)~44BN)Es_?#yT!_Jj+(RcMP_Q|+>1BAM!aEXD$J-<KHjzoH
zE8aC)eG01nro-TMwrp*^d(NSwcH#;IlE0iK8tGY7EHNB4L!F!5v^emJfhekc+N^;v
zQrZ0OmUP~5b*a7Dk9Jdvw8Y`^t@xECBc6EeO7*x!@y9QCI<s~-w|T$MBu@E#0*ImS
zMv}fQFA*zgb=0+z2mj0^<}42ARy8^{=JT@Xk#qy3@+A2ZkvyIFio`V@pq;sgN%EV~
z%RE@6PxIJe(wH;H4_m?V&0(|8YVWF<TeUPwnCb8n`|zo+pKk8Xs{Tgj0dSxuIE6#%
zB(j=(wJ89OBy7%^BG`8xAaUE(u{*6MX<NaU7@liM()McADG7G=mD%%xw}13_aw^zE
zoGq(x?BE159h&NTCP#jW59TE`3r@ptqRhR0?T=rx_Rv#_7JV@Gd@4MN>SO?fH8lI>
zjrV`}H{xrKGW$~$B`c8KZjGa?Tt6SL@Dw-jYVg#FSH(xTMx{Dm^Q5|MXqG<X64p%Q
zgJ?B-JMIy?Evu%M{1B}LGbQvYOPqu5a>aOF%FBE9d8>l+ZG<#&yqE^H$-`R(S__K_
z`QK3c-Mob(MtZ@%qjl_bNMcn9-TWk}sjPa+vrYHpc(*lYg(bHZ(Zo7mU!=5^A;-?f
zFK1dLujBv_SXo{lz?B9}(P0qHj9UmrZROI<Sol+LC0)r@mL5#j+~`ddnJutb%Qp*7
zUAkCeqb{lSBY);6r&do8eumT8p~rnSv22N?*=`zj&C_L@w^h&GxdbV_D_tM2IU9-M
z{ZXG@fi{>)PmuvzB=L>3RFxWu9*|uO5FTu%m&hS7)SQy+T}8SUZ?H6FTFab#zmtiW
z*W^xVRL!an>kV<;SFMxQ3+hi2;P61I3Q;ZLE8G&Nns?_CGnS{vC#v6S)mJlf<yUiF
zd+y@mmFNpC=I88X5{5M`Ps?sR{5RXOUq~wZp4`k;Q9E?V(|%+Yd~4d`p(al}eOzuF
zDXWp?=Pz|eM)aTWd>bT6#B6PUo6R&^FJ{4m1<`kg20U*^7EEfcO+Ad~*ksd!OHn@X
ziNaKXi`guQ4DVbtXL*syG_oa(4(~79gK1KEdsD)uwvGpGU+<&hMQNcl3O`*Gi_c=4
z+u+g3kb1`EtNgOUS_fErMGBxII4G?_78p~HZj2z!aJQ%@;fzzQR`JhJHN8@zbK6+}
zbM_`v%o}5;a_2@=vCE|Qr*SiXhvdNxuOoX1-14&7PQ&fD*6PmJ6Q=r8<7dq1wPxG2
zm`Xl$h<@4SlO?~;$m|Yya!uZvHXTF5nwWp0XSpB5@B1#rTYABtZUo-{nBsk@UDqdK
z021L<jfolN<&yMg6}(<7k4b^k5tYsd4KUL*RaTG4l`74OEx57OR*!q8X|B@7zu6gI
z)2nU?CG0udlZtJ6yUX<6%PXTFG6C645qcF!N1V1j`Xp|sJ$kJfV<MQ^W-%d95Hkmr
z8%IkTr6x)W+r<Z9O}j;>rf<#LhYI$+y)kQi1mO6{8%b!I2^ppDM=ej$a7Mk+11MyI
zuXS>D*hK)D3ff#1&~FT{Oi40O-}!PO0e*T92kA9L+%ji!z5(&9(tNWB(A<|K`Jd25
zj9TV6_A<>UzjzI#t$o7$TB~BiRWJO{{-4=w|NqA4|8H^qf9>mjIUl`7?2Gv95;_~K
z&mPM0-e<Y5`_FS{q5%T?IViqhVfU0;&iEWupmTMQXz>3g;r{36{ZD)r;8N%y)6zi}
zZgppfD7<=}Y<=oxMs~N0OJ1+-EqdV=bkr_uT}8c@&5^^ag+ve;k=O;G<FHNI`W!U4
zxuk<99d7xl1NdZ#0_sL*H=D)IK_8p=&q0X?fVb9v?m^5dMViF{H*v3X9AXy*-K|ID
z9^`J`>%Ceom|m1ZW9-NFfJ1evXDIQfcgnSe$XD0CUA%Yy>uGbpivmZ&X%E>d;HM?_
zAL^&|FW092wR8AC<|b2khVusMahjdhN7u1kgx4j5L?a%*Mv7RQsJt5F<K4O&lRSUx
zP+qxh80uFLR?S3R(`feF$miNmdi-X+1&$2C_evY&G+f`2cW`vmUulnc!MyShwL+q`
z<85i++>)PKj)?YH2uV1v7Ye&TyTQ`9@X2aR&kM1_zxNscr{2Te*j)lRC8}kMej)aZ
z$n-rK&}9r%Penc7x!gg&JNRv(uGwT}y2BMvmfe5h#ZlFDrMH7o;?n2jANQ`uQO2QP
z1n^^bzX`XyT(ovzzLTaKX3$(!JKA4u>t6IxfQHG)8f_&WoIFQXGwPfPYY{qR9d&Ls
z7jN{+fxS2fIR@W|ZTUyvKK$(2hei?j;y`%wjfy7!+NTW0;aQ$Z+baOk+lra)$CiCf
zmNVUoTwn0^Q)H4l=tKv6mL?}NJ|xnxr^`vTw<Jvaa^wTUw@crz@?1Oz;YD@A6xV)q
zAaJI*7NA?;mtmh6&Yghx6?{XoBF{lT{eQ)tf5)RPEVH16KM4wdoV&0m9Dbms+~HN9
z8M|<1bkgsFdn9Mdo#YCC;wpB_*KJdWax00tMm;7vN~SWNm}vUopGS{>bN{{nXX1qx
z)+O9Y%sL`dBv!W$gSH>W-!-&Lmg;8ojC;$J7?*jE=hF9BoHknLgxCb|W_+ZGw*+*S
zG90oF$SZ*L<TnJa9;CP+Go<*P)duikyn7DX^8g`QJ#!F)N?qrmi%-r$i{C*&+w$ha
zbI|*cSi)tXB;4`KsgY#-%{k}+DGQz?>>LycXcAGDQW9R#DB>u|&Ox-m)ZsScKD#V{
zz;dw=gkF&S)inh0pDJ$}`K`6@5+m$%ZmcEc;(}Y^*fgs==fk{qzM4BfcTKk^V&b`U
zK&HMp&N@O-zW-iA%;7I0lmg<c1hJg#(@W^k0%E3?x54i!7-3xqs=`d?psy3Kb5LPe
z2rwRh{pJ1ygk%u=26*{%(9;3{Pd-@xHU&EQueC7z^)>%vQ23V~$cS-ir?NQX#iXIp
z!#@5;O)U=N?C$RNE@A{T=dYQt_F*TbFNWSvG(|wq28qHEZC8D`2`~M02p7zO8BhdF
ziPv(J=b-yd+~*(=;Ms*613jN7N7%B2wveLtN?|9qIi~LxYOW3_XXz&N)w<s1Uxpq@
z>c8X&d1@@;bg$8MXP%88WRD{{dWL(l#|6~U7{Q_Z(RY<5$Q>1!8i(^$YJUWzkONlL
z>G5i;p}HK=O!uNlFLi+YbntiWUm?EJ<^i*n8VF1bz|5i({iJw6I*h<xn+J5lbb-lt
zo&H<a$(40Q+!dE|P-h`9K|`-&b<QpxWZ^GjfF;cUh-QEFk#-0ygzg|ZoU8vCD%WKF
z$lLsSkY`hgp2kXr(X+X@Vkf(YPwS?@D0V)cOB)V0<nN<Abct5(J8Av})#g{6*#_<t
zjw)b*=7Y7I!0P{&F7rQhrTo)Yotfkm>`)&Tj^{hkjxDg>OqiKpkLGMn^M>1ad@$dj
znee`)=ILGt&D0Es&$9Ul#V^?Vq~3cWR`J5;xIOFj=N6JhtU}zvtZ_S~Qy(&Vr9X}f
zSug|sp)Hi(Ts!(6W1eOlw`AVSzrHiJY#gDX^G*u1>e5WSZnOuT0QSygJG)|J2Ll+K
zkuRFpHt=C7+N*wcnO0^dRU%g!6S_+8Tqe9W`Y46Om93jMUj3t-d6#vSQ1K#X^fzqq
z>f%7#)Rx(!&?Oa9G1QJY&kkBp+MX~@Npdi)UJx;C5-=?<WL~>aQEx)A7Jg}Ge)31F
zyU)!w-N6)fm+R|q9aAWwHSfQc)=yakXF%ro8Q%?Gpo(SFWGs=+`r^}GxM+OIX1?$*
zBG^CP(|-iAze6j#$+&17lZyj{(0p?I%}ceJgoI@@{e`OnvvaR!Pby5fLS4c~-?0R&
z@3X^aPW4{MEw&wnj_ioT0&lIh?srO;>^hf~uhb1as~oY`qR3Dk(Mz5%u8p7D=+1vB
zbg_f89^IqT#?~|RvSM<5o}rPkc%7D+@52Pzyp%{AA+!kz)2oW_nI-7N`{(`kiIQ-n
zn5;oX*pvD>ap(4Kc#DJ1_3z{R7VE}$aZ!_2(4&;Wby~l`FXNZ=-`zpiHoPfkb@=8T
zFX!}w+r#A^ovn(O?u`bQm`vjOSA?VacA4ABQx^<JSG2Udb3X`e-y)r$OsW>-LpyTw
zn2x=*eMxlz*oEB?!Okc-w=D2u2Vd%cxrcCCpT6j#ew=)&J=pxLB7bWsAiQp7gP2IG
zEWidE7IM(*LIDH18w(t!GUqGiXEtcwPFd_Kvz=uwY9!NR?g%xT*DPNNz0dVRz$bUH
zD<pu-1%_CZw+9bxi$4mCfK6|jJCs!xEp6><@o`M58ZqlR74rka<5yV71Dvq-!B5cj
z6XIvInwYWCrl=IZq;wLmVzj8aSQT6U`}$|?7B@Dt8t+{QSsI9b8=%nW?_bJqT0c?(
zWPXPxg5P3VJA^DaocAqf%?A8vW%DiB<{|MHLf+j{i;)B!)r0=|Y6dLJN5CE>-gHU$
zlv;fLOng@NZvcV+u^T@rod^ZWgsx16u}1L=(RO3#=?1A(r=Ktmtj2ApQ{}rVUPe!g
zzJNYo-n-92v3?G!I1cxyTW<F6lZC*dWyW__hu4v(;`8A#1qF^W#o9DuFGf6tw~E!H
zUU4OJ-4LltT%o%($BK=O7G^Qu6P(M)Hr=eRNrNJ%O{O<#<ITWtHN5)V{R<^tUfyKm
zToP>5$nPgt?XkPK{2NGa++`BjQJG=d(h#8lq!`449qX%tCR;;?sx|XOzM5N3Tg}ay
zKT5rl_`#~r^^zu2{hg=*-a7x4xVzs-bMZ8L`B~r0T`tT)GZ`8Jx%HwKeLS0Nw30W*
zrMcg^Wa*8d)BO7st)S736W(4!GirgN^N;4#rUH<cY&w7no)s<<u-%9V5I<sKm^T~)
zl=7`cA3+eUDku|^=2GSiTjJV1x9^gedsd&}W!wq6fSa6pX`Vm56{$G74#YfJh9}tz
z9+wF1c6wdCQ(kDbE@8zw?m_C{?8zSjDEN$vMT<Eai<lSwrsFt%L_Rk&tKoasID#(m
zVpf~*D`rWY&XZ??2fL)3DIdP)KB#-OsXzKSMDTq>U2TN9dAAr`bz!N=JO^_`OU{xx
zHP&}Fq~rkFOWEoww$!mN{A#w2={mhOuhKN)Bz_K}z)5Pqgd=N{R1*ETr^n4TuFQOB
z%9vqkpitq^7k($kaY?5$>q6s$MR{3GY*TXVf;mi(X1JA!TR-&}QadIm!olxcRr5})
z-XFl0$tIWLhu6!|s##9Db060k(H~iy|B6Tai#|{ti;o|mM|?$<orBiPHhzyXOt^6l
z`rNV9a>}rcI3&j+L4kh_vau-QVhP}^u$C=@J!?XS9OK{OeSiIue+#P3f8sd-t<+N>
z!Ri*+OF4^3-6f>%6#j0a@ogR-WhD#Tk7X0*AU(2czaP@V0p2zW=TGR+p$5K%kx%Hq
zO>O?P-TMDDXcyMbLDWHPW_S+l9CArZTiRIw%_v=N9|6iYj=3zfN$2%Y$bcz3!yx@)
zk2Hwc8_Rx_j}_i#fgZJ7+<0r%AI=P;Crb!)*<79Kexvky|7RidRYf=B+k-@Jd?=6J
z;8OM&Rjc-Z-COyMHrt~681mAt@ZI)omiKWs*y;B!x@B!M6RRf8{2y1UHy*=A$^dhp
zd#=H^of{<eWc~8|#jxUh9R`unsr3!s_?U76O;$(N32guXc!n?oT4!Izd|NilEYCp-
zd%&(=kzfWp%P?+KI0mL-7+z2@Qt?0tXAJbE%WIy4-W4fMSTqm<7lDicfk!J=6&;7o
z0e}WMaIS(qdqT+kH6T!^p%6dB)lN235Ce@s;-hm8N)9{nw8EYONd_AW7EKTUaM*?g
zYdp`PFu|_y9AxRX`|}t`dOz}>gZ2jr{l#-E05xz~`EM_k<)2+C%jB4U+b#khj%Xdh
zRh@&Bc0&-@+|ibwskvvKy)A2Vzmlwib5O7HsZ6;~;O;pnjtpmpph0}M#Fi7N{89kj
zHv>djoj3jz@PqzF1$+_*guUS|Src#_tUwW6XK~<+v>BKxt+9}E(5hqX;e-$%BjN)j
zLs{n_L9(rlriJ4c!b8OcDBkoOl<nQxg45fu#`ozF>R0A}>;jqI3;lD@O7<T^`p1y|
zF-!l=Z~Qmr0H4!10_eo0n#3t~nJFY7%gPJpt<5c<^`(8s5~AzaZmNnIs98|o;+Z9r
zmCm8lwW!zDHc9p-06~1Jk;+l+>%P0cdPhLOI7VOvUv`Yq>c58bU62mUHJcl6(e&1=
zJCZM1Toq69yZgDkOp5gHtq#5bfM*?^+(IHJiSilIk1>xFyxI?&TztWswDgj+zLP1A
zG>OQ;-jEwq8FQZ~15bR0Y9u{Xn+h!dSf58*l|z);sk}UIe)6-X@~Q57e6u6b%h;Ku
zF0l(6150ODDXS0pFSA$Fzp(ic{{pD!g}hlf2c3|t&*NwTsHTetfNK5~f4b=J!tCJf
zCO~r%9aZ^RDTj0P`j@U3)XMQ1a-gePAWy!JQK-xQx|e<@oxTS8+0~(VyE;&@J*Alp
z+Y(ydsHG4Mvvv2Dqpi2iQx_(TEZ?EU=*rdz+6w4_WA3>*Tui|Wkud|&{xu4SuWR>#
zP#6NBq_uKDcvrd!MCR8Smw?rQ3IImG?d_fs%VZI5dXN!Jej!n$ue(|f?rb1%(u0aW
zp8&Auzvr1GfqBH0N8qTzdPfFG`!B#Uv|INH!_Zxi>Dm=d-jeJL#X)E8XU|=}Jz2eS
zT@udm`UP2j?(+?LKZ>~fWPfh@`h{%%+a*`M_&}_`GA_XrirQqEBA<uqS$u-K=5I`u
zCEQc?883II)I?fM%2j}mjLK}@<>?OQ^61CkmjW<FyR4gtfi(bd#a8Vh&OyQszhFEl
zb`4MX)rv-{NDC})M*wpC$8mp*%YUDFpa4CqRJiCc$A~jsjMmYv1DYHp>g7ixQWW_1
zRAw$;{)wN4mD-s@2e}r92B_lh(s?|2d;cq*<?N*#E4ILT@3v$XRij#=kG(!R!&>JR
z7hGn_n2G+dF#q0k=KFjB;-%ikryU@ogA1oTHU#RHFxSt7FR$k-cC9<!0ad?f5yask
zLH&%36wiQIP<UPbR<`x;z;C}>1R;9`aPD`>9xPgiGgDU9$L|oIgDMYKXKnp`p9rb+
zzY<ydI^ZHPr@_1G#KBL^X%PEe;mS1Qrt&$+orV7$qPOJ~h^ugg0qpVC?816UwpZhx
zuE2|QpRSG`MNmCZpafB${@h>f`gbip{>PU)|GnRlS>#lO&|?Is>Ae)+7Hu8&NzolI
zJ7_2@0Tc`D-#D|TE-czcNZ)B5&+Ip_yK~Qy{l03zCydwyto0VK#9e-o)=KNuEQkZ_
z_patTY>LTvg}DkgC@saUIlBI~cKrft^5=&JQOKp&_gNr#R;){#VrbSCu~-(L;?@(R
zyyR5F63kU5nx37LMblMvayGQ=S|${!(%WpB<}Rs!pUEZdo8k#aY6{`j6bryY(7mvV
zBO|=w0y4YcnlK=7HURlH1B+~q1`4qtm<>dJLy#|bhY^-2Tuva94?s5Ijr_$p^Uivq
zEEx-jafYC5xF=Iz$jBAg^JR|Y<L8z2nqkIw0JJL?s1A9CjicM^5?Eto&UkLbs{G}`
z_h*dp(yDbBQ1olGJqJA=xQ_TP(+3I^|BF)3uL3j6#h~E{GWeeCr%pEA4951yOM|nx
z=Sv4v7mkhM-C-2xpnMp<@RI6Z+(4JwfP15fYtsq`U+fco6ly-bIe!Co*L?aDqo&x1
zW`RYXp31GOzMOHB5UC+eS@I&1JGxg9crgr6j2{4s@v1&80K?~3`AXBroe+QrQrcj9
z00#c40&Fe*^N+f)%*;C)MCP+duykn;y4f%;E?b)ExpLfw0JI4;S8_U7-pS1|`dU9z
zkIP?`)uX;jeIIliz;U@hW}T2De$c_P@LzN<oV~$v6M|XroeXP0Y71<uo(7NrX(`b;
zNF=llNcB5FEMK!OBECNWet%Mi_#twi=#Mk}F%JLj<^zj-&^Uwer)8YWA{Tc4bxXye
z?eH2*a{y;t-<h@yeou|S^e&wGl*+&<HRROa^~yx0Ub5MbB#?~l@wZxC-+N?+2**<^
z#(2W&3?}DHRgWi(?!y^V9wkUr@=0*O-ak#9ePC<1exIcYQ3>EykuD2!^LqlXe=-mR
z1_0m+3dEK2@_n`MKGxqx07AzBQ^3o)zn3}w?*9TPPj!9b=UZFI<v9KQQXnd4@5;HR
zWn~-I-JCrrv)Fn_-_$H<TlapwdEv3?1H$WXmKDut#bc5Ex)glFISA8oG_p;0lQ68n
z6xvAz)1g|ixwHbLH@^j4ucs&4Ao<f)VBo(%==-l4l7H{In3+x=1&vC`c(?76&*0#4
z`uUF7bx9X{4+71eR{STK_D*Sroi(-4xmmpzT&Bnvj@u1a`anTmL0#5Z!+BbtT-|D{
zPOO+_Iqgipl3HN4xxH=|sN2<s&V);)|5+-fBl~^R+_%evM3e6Zwnp+Yv%aSi5i*?s
zw$9?OweZru@!~Q5WdIBil{HsA2aVW=6ka+I{Z@c~X@gfx0{I>O9=Dq1UVto6?n46Q
zzN<bq0Jmt|;nMn_ih^WO2xmd#PpRIomzl-WR9@?;njSnUl25fD4KKj_upJ=`KYZn|
z<hm-4=?`yny*Bg0H@3?<f|+6z4_#nFsP1`1FVqSjY`gIWC_{yk+|EHZ8ctry)Z0^*
z(4)O>@9Tqk6LC5&tC5iox>mi6cZkMYE(93*E|HN5o^4MYeSvv;-vA8vI9R5Pp7*cA
z5i9L+9|Le76+8h4xmozBWuEJ_&Mh9Isy|gzf}9Cojb%y;+O4m)gq4;G?*Mz_#)opn
z({)ZclSk!bwvFGN%IBZM-*d;kB4zqkX!MJyw@2H-3Mbbg^D!t?`J})%`l(Yk7N@MO
zD0=vw6iAQb@BliS?cbV(Gu+6+cm9ce#nPd8LfZ%+(IHj<Xz>LO;1z!y_Q!zyF+={i
z7eFNc9fLza_o+QBi@Yp|+9yZQUm3fSnLefRF%zb9%aK;JvzE@GbQy~*alZ?1Y4HDz
zI`l!30v6aTDH2FtBK8U%wIl}cjg$!C`CXsq8l)M$xl=g@B7z@4k5)6t@fGH0DDn@!
z@Nnt(WRupDY6OjzF!zbmdt}QnT$LhFj>{J&G`DoKWG^6I>)^QuJ@<)xfb{TB)mI{>
zJu<Tv?D)&Vje3rYr;v)u+RX8p;=9QXi{8=KSdE{50r5luwRBfHk*f_K&<kcweBvwJ
z+WJwpg48)vTC?BL8)M14%Dfoj9$mA%DJ%O=-t$zRnx|=})<n3UcujadPSbFLBeiGx
zep1e;pjKtu_41qV1V|iP)jTRaPXiu`)YiXazLS;HMFdWV_A(|H-O3b-zCzziNwuz%
z(W3z_O2~X8^X62tK!9zeC4NJ;8jQK8n~2uMC838UFv(}ryz>E%vf8HtktNLh3eqE<
zr2hWp)KimP&l1*+G~A!L$kiVFCU{87?;Yd2y;HK=H=cd;ETuQe7E|{_EasM!Uqbo^
zgaL{KETy3}>RPJTT2R$c$_qhp0AlPOKY87&eX4m(rDr*c-18GsjT#Nd2vn(N=#d7D
zC0(=p9-?iY7;3hTHFKa1B5xV_hNf|C{w<TGZtwcY%V#w?-mw~z&)Xt1bUQcV9^6Qu
zh^3Eyrhh}3CA>JyWGy^Kh?-NutLV&*&X#3PdcVLt9&*exE3v}cNiv(SZGh?w>P?Bu
zE8W?bp4YUVtvBoK=IRe>>twz@DCXbtI*7HQ63bZ{vXkMS_Z1gvDXd%L@0(4l<x*q@
zohmunwiWDa>T4_Q&vs<oPJU$i=mW`#tun1qiRc#r?nuBUOx!*!+7SYYs-r6GFQw}?
z&`W%p&Z|9>M2$`n;MEVdVu0&wbadz>6^r|8sA`$5H<``)TC;R`SzXt;YCffiXHE7(
zU{gOGX=!CgAa2g>yqcMroief55cpuGLL*QvQ0htP2rzAWd5j!J9Pf1oy6MMG*eaql
zGvgCt__CUW)V1??xq0h?_NlowGf>5y>yq~j&Qh%#G^hAwt;kfo5|Sfx!A64{dDbDy
zZBxc!+n&2uOth#+X66O^@@Si>M#W9g4RoGF=C*YsVncj_9VO$vF7BW=a}dE)7j!so
z;wZJ;oKj9gLZd@9OJGHZ9voF{kR-OeU{snGwIs;{UKb8ji$4ikQN635U)@lzofaRV
zquD91Zv-1YR`_z31{oH<YND2J{@{r{qh*OERIF<1QO?oQ*}Z;^<z$yi>-4#)++Oaa
z%X{sW8VnrK*t&Ab2|k(1#MPgSYf&~*E%q$4eJZMQ2Y{A_M0{h;v_?$|QmH3ZPeahl
z+q}u}M1CSc4zoCcXH~=|%mBsj6+)k-0Q3XmN%WJHtq{z>S&q}Vlk!<1<5uon=N59h
zp#D$HD&;m;4m(1_j%d+G&BIpnUke6cYLo7@r*oaaDX39D+}$1HLUio%302PKLRRql
zA7&@+@S2+H*i1asC10ZA+q4dyiI2Pa_zqgjhy%S>y=9alco}noID+VK_Gc8$^cMQ6
zs^^9}$Q}PS@0>DV+IA*LqzCEJxuj328U-5yROy^ufe!3bd+KZGCk)0_Zc%jxff>0y
zOU}~fsquyHS9%5cWMw7lTztCcyNl<}oV~Zp2foRBYvJJTdlv`Vc5lWqSehl|bj#5e
z!k5rBAK*%~wXp+~L?Rm`2=TtkmyR`2tp+~=6I~7XRzLLGo67XHT5n&wm;W<;O%D<u
zxw2BdF5?<J-_oDLKSk)*%ic~5oHcInIH)u>9zS!Qn(8v#<6cD0hk4#Gx}n*TOrRS(
zFbF$kR~CVWi{}WkrGv3^9SYe#DaWybQP0;UC1PpgpO`Xl-ZN&VCAx<G-jjUIaNW@D
zsCw7*%zEB+1(Lr6zL|cjaK(%gvbqXYR@NAO?glB$=9x}b*lj`{c{}fFs+3J<%3^aD
zRhlCvKHzNHlf8~37mL3-$uZi}r0!Xo4-|={zxW`LEt8n)OQ}1ub7!QK=Eg|Jotj3n
z9h<xHsI8UA*1on4^z5T&vjz)1Ll!MiNK8e9N0F+b-HJo?0I9ovYV??N-p^T7nWcVm
zlhAR{z9Zk%)&LB>xAsC#cF)Qcxxvg-HFn6vxh+h?SWN6J<oTdbg~8k+(NzHRmD#N0
zj{l3RgXvEjX+ygVdzQALPdULEhEi74oOF6eT6L`(Jy}L$xvL(4|D1i#x4b3$BCVcN
zMaA}gMNoj_p5Y--1P#vW$YdJ+Qd9qO^q^ZjTJlr}oXqbF0HDgltJuEaqpJ1&jyEZW
z&0~*G*&c1#D_HjVo1|Z*Nmu-eHHk>hP_)(4E#880^OOgnT|L>QMk9|c89Z$J7gK~P
zq!*05k2fAT>@qw2P*X!&P-@BlOiwCrUQ(dOdyePT4Lcvs)2g7REv9intBc%`DzE5c
zYA|mt*&AtwdMZq2r<Jc@lEmBum*YFU1wZ=0vwgnQN7<OsIZ+ny^itBH0U`2}hT!+Z
ziBryb+`O8pf>J!SJX4T3?UIDZB+cRnXNgUl1(n~2!L#1Yy$9nh-0~m)33FB8#+?F)
zFMBKE=-LF(T~U0${C5iiikgk0=b$?{#m(Q!=Lb?G9sZUo{lB{)_I9VP&9@y}5l^u0
zeMvC)fgVgJjjmv>+s1t93^<BnQ$G#%Wq;;;4w{90rMl1+)Jvek*(}W8>=OROb5ND@
zh)TXQy;rQVJ|-ohO1`)MJ-*w6;DqofS^{1A*vR@V(0q}5Yd7z^0(}f)hLNSHX6hW{
zo8|$kXea{iHH4nCXg2kDnPk%WQQGn={hdqG0Gr4zKo`i-*Z>^mnz;+X_*afbD!LeU
zMnaQ?gG?j#n}I^6X%>~@cXfaU(3%XqB<bM7zHmni2`=(J)~LF~r>Exz!*K@-`9ZW1
zzpQ_SA1h}6K=bk~3xT`*>bp-kLJaRlY)Fc9zb9)^xQ6&lquBOSm#F;D+mOGczFqji
z-PZDx&+6Ve=!1Qz;sKN03x%|Q`ab?Nh-3r1Q|<(?%WK3I*`neZKXw}wI1fPQT_x6s
zeTXCw^e<2B19Ux*E~WwI5I#ih&Fn**r$Pm}ID}zyRoYXH6$^6{3$4qU7Dams-FoKF
z!oPoQE9SlP!W~Fl^YJRc^q&7!;-zh6!T0<sm|t^gw?2@^0Ttd00jxmgiUcy(KaTrj
zT>h8`f7~DcKJg%+_tY*xXhO_7B#5V}eh(3T=yPXSh<<Aa-YE3p(cKHCW)p2j$9%74
z-@Nj9M%U95whD3z;yVX@5^JXFT|!U^9=%&aW2x~si_bFb*|`b|3#Ipm`3om>YQy)v
zB&R3A37o%KNQ2J$&k|J?7;zq+fHUtimd_J=b^xn|{gL%qAR=B-88E=UnuXJOzqx!9
z`%l4GF02!A5ih%R3fQ~DJ?Xh?(aD`EVJ72(k0q*tO&4g!G1zG~ZT6_XO3r(g?CPe8
z6x)MeMCf?<q!4zw^hFT|JlYm}Edq6G+7M^S0F0j{@>xF=P<QR)-zd$?OUm5nMwVNO
z%M-ORGuB`OgJl%b?Img=Ad2XjnK><Vg_C^W^2khg)IvbF_0uaVt$c+)_{Ngi+)9m;
zz-G-RO6ftm!8X)u4#f?aVy7t)U9d<~ir4h%w9{;v+n;y?m-_IQNL=&IvSP@gJd0Kp
z#fu^9@5kQz@9s_irggO;KPMaVUiGovWs!S#9~W=vX5ONx5lVvgpU8MFfF-tj1>}tb
z4_B>HVZn*hXEQk|>E?KYrJuKxbFtw0MFEQP4#SZE9dA2(bG2%8<H$FR#;`b)9g|`>
zDlyNh^kkGhnLq9M*BS?t&--3&ECScFjzm#m!F9fXjX-5Wq@OgOSh?2xT;Ae>Fs3iZ
zv>u#uc9u7`{cJG_Uzgd0tY5bomlItm7QXuRLC!kGS!Pf7Wmi`3xEI}=nHr`m$!brn
zG;F<-jqT#q?Corp42$I+tyonxF7M37TXV!~aOP-mB*km6=ZF{#M|r6Kc|`tt*FyFR
z;dq02$sG5;*!%9drq*ojAS%+UfFQl8bO^nwR1rZGq$9nAfJ&DViu5KZpddw%-lRk5
z9Ymyqgqk3|1w;rC;<q_>=FTaebMM^pyYrnH|4{eN`|kbjx2|WcXRQ^0<c09A7c*#W
z^m{c8*;L00$d6=1ziO;%2({-FmcN6|YsagV*kd`f8U5|;cTiny!pG*txf6hqTAgm^
z=`p_@X4pk*b{Uf+WJ#}Eg^nS3KZDd$c+E5043jsW1Qfv%D8VD<Q;V^G6Toy{(g7rn
zdw%SIRI&{imWh$%IP0I-cmFE(vkt*;Rd8?)OuJHxd5ltn`aD3T`q>U`)5?hzVMW?@
zlDUZ;_4?YiO=^<WG^<CHg+Fn)IWpi|e!aI3&;<jb*!$?FY=UUFREKS35f6mX=qjzr
zttBCk3tOx*8=27+sY#KpA10{`f&k-6w_jH~3$d(_G~gSPL;eM@ZA6?pJdsDkoMI2D
zHk&sd4k_#*NkQJWty+g?x-qb8pl@rxa*6thydW=uiAGO#!RaEZ6c{kX`jziZ+z0Md
z*H*%Zn?B>)&p{^HZwnSj90qO%u|)-xsX1xll`R1@(sah}pcVdiuzA1~cxVfl)N{K4
zW+t&c3x*wrGZ@4>$=_+l1tL^&7t#T;qbmsJ-~%u=Gpgf&FXaI!k*Oe-|BYwRxM~GA
zN$<>^(qk`T6@h54A)&V3_7KvXH;(?tuHzzo9CZz~b2S@R3Q`DWc|7P@Su))zPSR*1
zk0S_H6&;QKwweAXX^p!Cq4JCfAFUm&C?~P?Z3^;@W(vd42xUtk1f+c;+6a)(g^^Pg
zm)+q`x%Q6rB~Dv59+b`=36T|#{mH(AaF=r1)4v{~nz4LXGk~>^0KBD>eWbpF6w-Ks
zsso(>&8iXbTK>l<HZSWo&k_P7ZivO)ZqO*ecx=wvPBvHKSTV_c1H08;O<D*aWxCgk
zhh>K;5{_S~-6cvCWdUav=XtFY^0~to-v)5@D<Iqyh8mFM4{%l|fLx#a6r@m%I|~qI
zPXK2N1!G{dN%JS)Yz|P^Q-CdLO(-1ZX5jzp3IFPczh^!ie%&t_Yoh2w&vo3zJm^YC
z7`6^?oh#18^2wRq{W|A-RQi4ZMI7dIP_nqo8L0b0#$EieqNAUJU)uw-a~TtH^5Ro#
z_huVb;Ex&K4d0A^dv2PxK+y0W_Ze7m)kBysJ>aLb$M_dcr28JOFNh_s6!j5U&ML2(
zkB$JJe!tLQ9H|02{rW@?6Y^I~u>a7}il9}YiXfu0`8-w;oq%ihf@B-QY#_bMrdk&p
zjQT$Xw)*rMP(LQ>5O{LqMHIf-pbv4i#CRxfN&W$V(vx8O2RMZ9TjU&~0_oNmCEUBf
zLd;S0@FLXZ_F0x2q~pqdIG?%-z!}&QU4aV~B!bN!XaEF`(>M)9ezdT`MWhy%c@>6D
z07znDv26oKmrpIjK{JAYJw5M~Rb7TL0G4l3umai}P&fhHIu`V}0<pr%6Lc<)+_f2Z
z3)rt1x{<agWTrHL;JcOd6C)4Su8#$rpx*WYmS?$P;MBMVh!hZ(;PgEy&wJlN1aM&c
z1DJgOpzR`v^YJ~Y@S#S=o?U-h6WCycOi8CSu&)udA8apJP0~?!jY|xsnsmDS@#8!m
zy$b&uyksEq_P#Z6+_l@8O2w0tV|B}&$=52%m6C%4H6ELTqi&QOKVC9HRFo0D^&5ZP
zCmSaSPyuZ>ycSFt=8&+E%tTx-6F5&dp@dCON8|!)Eo)HgSu1A1doTTuqoMpdA`1qf
z%uVgsB;<SE7#{z$JB;#@y_tD7MhfL&dGeOBt)R@}5#hWm4c?wdtM%2)t_vKwrZU<D
zB|q2M{F1BVvweK`vxzB2x$8C8s6=nRJu#?iOpNq$&&XP0Vkt=t!jd>IBbhgx{biBM
z1_`%5vwl_!LHy^HCPB>AdENm!!yahE9S)7ieqO^du~<s`9{%h)Gwu&-U+>57j+LMi
z@wp~aWHlDEQiasvta#H}D6??Aqv-T!0!}`{_Jt0EGY-5yd~TOEp_|3$-aT1ewL&Bm
z!GnjVP4ekbM{VNODhZW6Z^HZuA^@vH6ebuscB~nJ@j?w(Ha^+DtM2&ijDYQ+m>PJs
zsa1OI8e5RI#1&neK;&L`jNRlnkt|jgQ!LAld?`v(eo28^?9EY5Lf*{kS55P?)zu?j
zWa7O{l?M;X+h1Di>A_R@vfHniGk~5p{+5IC5AOfU4B3f9cEIQwcndJ>UWhJ!zb<dO
zS|uv5hBS@seZIG^uXCPhu`LF9rf+N>Q(|RLUR$dB@aFP&l@x}KmF`cQm2r@k>l7q@
zXb;iQpI4!urT?nPBI#+?`W-}zBeK&or71HbS$kwpQC#UlGa5HTlUnrQvj1lecQV3W
zdV-6GDiY7$*Cn!-NM}2gB^bB>Np({;J!iXXw3wuY?q|^>uFb$c)&o2;=Pt8DG<pDc
zAMo)g_^L1(#d301?>p!N3>)6OAFZ_*yMQzQb+Mn93WM2=;`DI}ODyO_EU+BY$w>h5
zQwwnF&N@*88at@$jLkju`~26#{=H}a)o1^j8b4cgf6nD(BmV+@(49^tYm)1k)HP)9
zo_NHR@U2LAwQbwGac`VTRLzNyQColhTx;K(<(et0LoQ|}T|I_(%bRtA1AVzXV#Oa4
zv7ZihWf7Qgi#o4(;~K;X6sNkn6zSx+$^Pt{(wU~gul=`v1iF7Rnw;^Momil255oz1
zKF~!geXWWzJc-&Dd=$oMm*{%sQ=Y+1kwCR`{sV@f9firMeGZJ*u}D`u=D{7KXp;xz
zg=U+z%@!(*R@u7z385s3Wi3&{4Z=(QvhAn`tt`RfhHy`JO|nskdqwxahGroG+v7BK
ziE7CBEvTqQU0}aW{MG8l>E|K(i!Wp-KZ5{h{_6-=KM3081qd?$39~fBRKBihq%vB3
z$Z1bqhquQW%o{DQhEVJhS8je^Evn3_Vz=zCPMr5a?fO~&r)iPL6d0Ks^^w|DBjDQH
zDz|tZ4ati`ql+f=N<`&px-SFCSqBLw3~6JMFP}m1LCtRATiX203F4P3F#H!gN&c0B
zVbzxEJEp1TC7<~suT+mhAJ+S;y$$SPuT|$Dd;5xaa~fZW;DzEP>4`Q4BF|X$8gx@}
z=BmUIbDB0{bwZrB!eHvvP`v25qBt-m{m>+JpE4rqL+#lJP;dIYL_mq_s|J3FP}i$q
zUW4Q%CaKDLYHOMedhih96$v3P!J}b5^B$Yt!x%+EKYULHGrnHr3}I8h6~a*lD7pCg
zy&L4pSAt%<KF)~ozA&}8*4v_1_~q#3z)EL=OECfF*4c~`>H_lJP}1f%J6&ZQeT(c~
zQE}`lQHQ(?1!n*s2S?Y@Uq&SS$$dltH?FX&AqyN~7CFGdm{VWy+pu!-FpLDCC8mq{
z4%&cJeFu%+Y<v0JXb(O>7(Ru=Ykq*%sq9`S?8p@j{cY5Z{}817+(`KE%{>dHs?jDN
zCBipvwv9b5JR}0i53n3O*rfsarYUTdez^iVmW9oj^=RYm19m`o^YaY+jh*ixHyE2D
z!Q=vN4R^zzyrTPL3>yq$`SAnrk?C|k<~wce%#9h%Aw}Ru)lOlIDPlmoydmvqCZ@nO
zw<%fwn~Y>hj%|cd@UUe~wN+Idm-NdJx)^P7ULR4+&_bh2`;lPuH#hWbp*<=zahP9F
zy{bN9iT7p5W)_eZTc<w6r_Tzt)ic?$ug;k<duRN%lj7&aXk021Mrx-(At1JCbhlyW
zhR}E>ZnV+PX*0VrZ9$(_PuQ5#5RAAIVPKJ@9@DipuwjE(vW-{Q@HaY1_Iczg1E_*X
z*Cn3rCr?PrKzhAV^_n&c8@;=$`G(oc;T*vw#jXi2wA`j-2RH5CCEjgJZIOM^?MFQ!
zOZd)#*08aw3R={M+^*Kw%JrNuZ5}D*svZS@-g4%S_v9Dci_AA%!Xpufy5v+99jN75
zg%K*s$D-%_oqafvnidn<W9jYk*T#?(Cy9W<5FS!$pSv|GSPZU%3RW2`kMhTqGTwBW
zP-BbRy2y;^n%$zU206N=S9}V0K|*bf2E}T>F+y)#aGi>SD1-@OHFG+h6{d7}%9_WZ
z#Z~kr+=;}oHAd4}SAbaW6_uLRLgMnp|J`B!{dq#fs<cv|fCt3V8=3kgF8RR~3(;yb
zagjMDD-V-%fZR^MxL_Y?TPoHZoZiz=nRc?}=2%^Ll1ul7r@}2+<{pe^vitR!%DZRq
z%<i4Ux#nYUA?{0`kC-K@AAQkDU4bybKgm5;SFH(FVi$?@%H_>#I5u%-tj7=Ip{yl%
zgC)htpwdB}OWhqKb_@xjQ|!%=9ds12=O^vAU0ijn5@+SEDUDY#UfJS!-YH+>=8@OM
z8Qx!^AvXQq-<KRcrN2YvIbty81|wdV7Dy=N3hegwa6v9dOzRsN^F=>Ne7ZT)VsI(J
zLa0@$&*m;w$V0qUAf{#$k15t1y=ThbIC3Z0IZso@en+gO)@y_IMs-bTm^npCPj}jL
zaxkaXrLXB8Am+{=>mryh9^dhFpXy{G_ql2AO(GFT%DwgYVuuFUM#C<7iQVGi&5Mbg
z)(as-s_gy>60YQw{kA2lDDX(-Ho5UCf+jg8uCOG@nr>eqxT;_ER%IFgWy+3tfnW<U
zFYyokBo}bd7CQ#lN8<8D!}5GnFl$WghM#Pt(IB{<SmRZL9yfZ@SN_OLfCDPG1uddh
zO8Q7KRjw))P#3ysZRH^$Zo^7WwWAsx8ZCcG6;-%RV>d@x<>l-nz7Ky2votb7K$;Qa
zFH|Ac{O{9HpVo|Q6J-+MiAVF$G}O838WBV7^V^{~ITI5f`J|50g=0z-sj5y$7t)Pq
zsPb6u{PsXyqA!;-{~iRhX>H`mT^)IU3|gfj^A*a}|4umjiz7%<ixlg)BUC$V)Hz3y
zq&n#4z3Gn7G^IgLAGx&WcINcixNbUWk|DaD>`rS!<=?vGBBs=(jqL|Nb4|M+=-`#D
z`blAqiwr%zZ)uoOA@uoYwu)GH<i_7-u2eH*$D|7d5=hiMr3>SG*8u-|v-5mPc2`mx
zVrhS7lmCoS4;~OE)9%p6D{GZ*mS?9pv>hV=)=fE9IXAozdUtGi2{pSvxjepUT@!mO
z$5x%VpcygSch|jX)^J=hiWOf0mEJ{uX(d;!DyY-YG$?m^g~Fu07;+DR(G`n4NVKlC
zgaVF`rb-kl)#a(umt2ptBi28D<xzQ111jDb=tyIWUvKl{+p%5_a;mHol$8)j_<{*R
zeVY8nQ(x(O=jg*FaDDJ7o0SVwYHuT{bxtohgf1SX)54PftHt>LZ?H=r<c`d~DWFJ%
z-eRGe3P27}7|mRFKWa+KU9r_k)36QEcr3P+^=^&&W%zs~F=O`<`=O}U^K=1!7W9LC
zBXoMp7xl_MJ2!p{rk>>f#gI7W0yev01-4SxP4U*nsB2F~2zh)vtzHkZXD~A5WTB~B
z;0*^gHMq-zB_(ceqi+~g)?eamJfrlAxW<#%7!Q26?-=zR)DX?G&Hfy8+#Fzfp$Xz8
z;3N0;<B{9zkxfHQk>2RW7o2fbZ_Z_K^WM8${<bF8QJE|5micE2l0vh|3glRD(UEmc
zu#XgJ^iFsQbVPMz{L)9RWNNU66V*LxQLyq@l=Z6;LNFdg^CS(6pJiaOf`ElIDA?yZ
zw6XcxlO>olHGU5G_&oBJ&a|pzaIa^>m@|K`RqK+!^uSrHBzn7D#^XIYBq`U^Z`@0E
zk`2hou(9AJ9{iPA#&bc>x1g{w0GdHEPlRt-`EzT_UwvtRDewL83O@<h4ff3NIjJzQ
z+T}wFO{x6Q<ozW^>onBu!$&u4T&ZqjWY{%7eU`aDKZu7+XIyClAFE<H5W$JXC#jG2
zlgupYLKZd-gujD|YSMc>c5QECchswuaa=Tbqrb2B`s<X&Tbkv-Xl7zcjo$A#WeA28
zX7X~SOszdlExJ<5-pj6T5Ah<+ZoAz}pX&55!B%Y{=HX5zD`+qI$57x9P|ub9^F}5l
z2ke7(0DTJ+g!1!mqD8maw~7(JqCHwWLfHxMZR#k+ZB=GnOhA<s6W--|w)-q*c#)&F
zBlwIqL4@Z$MNBN3w>`0mWN^dP`}nw-lD{}7JLPzK^`7j3#M7FvD0jG>bXg$B^7K0t
z@8%89Bbf_}_?BD04i5RZsYe2kRG*rL^Tg$u8X`3Or?}IuOcA_AY~&J@bzQ+1#nJMF
z0(MWHT-dxx^LNl?K+*k=b%&Hmv=ecsf-Oj^v9kUpX(wCSRr)Ke)K!<?SOvVX<a|0v
zaC7e$i|#bOFrpi$ZVLpZzdr%nn{fMWbiIF=M1Vis4E~EZcLpv38Qhx##emF|k&&;g
ztA!#9pN&*q-Qa6D$r0Qny+f?-I-_u{m%|snHrE%Tw)DbLhvWTut*B;u*m<l1d=xzM
zbZc#R{kjotLzMl~qFR$%&K3HV=NYfkqK%DhE{fYcDlg31w0e}m#2mU~F<`bM*Bw->
zW0zA_M)!m<MN?;3^<yFP!(-Ewa|vU$WN&k;nqLp=Sa{OeKLvH50gfcWThCb2dUSAc
zH9YAEy8PKH(FEU6gF`TmscXCD<k@5=r8-F6$tJ*t(K_g3P4cY0zwFmGmdeiMSgfVO
zGgl^Bq-(1DX3>@y<1vg<zo%T)o@q4O;zc`u4r@o=^X#P>Td|goxX#6X2Pcr6L<^87
zQH;s*-UL{S>N8?h#M5f3hqIz7`Glt$9%En`_bk~hsQubI)sMkc$eWirhb=itBWGza
zbI4{o{;4S~mxr!}=3cJJ)#-UF!`*gD>8pNv#Sm*=1o<_^3AN}m<sy{<FC#!bXsykQ
zNUbo20#Ar?GIp)pNNm0aKJ+b;mNv`IC<!t}>>4v`F~y%7m&luIz)|gGdiA2B+z(@l
zkVoz{homuLf_Ahzi&3dOn&XH84fJq}8?g)|i{fz+9p&qV=NV55^aUg?Uix$n<gI`e
zb#5;`y4V01*8_bO07smaQ)3ns12C*^01*?=T7VjOR}L^{^*56>ujrwdafEw-MNjqA
z`d)*aI3b$73|O<|f&Hpy5Ac%}#{%Sww?^&h7@iTpn#Ft?TWBuSY>(FyFlwJYf&n=E
zl($PVg<i)6JCniY(*dRP7r@w!Oz~ra4HhE@u+)G6kBtS)Y`<w4{vu0w6ZN1xxzAm6
zLmK69<tF*D>Wv+e2P~80iTi1G8gpcoe$H{1{cMwxChKK9s;8Z#@GbXv@4W0k>-4nz
zDz+1^%>8Fdlz-Y~j+sD7Y{c54wkb_LOS-1o=tR~@Noq7uW<)njKE7xtai1!7n$Q{H
z4!Oh@!Zf~nlC5X$m1&1vrSR9qyglZJ%u&gz$U*@F!@2B`!na;5=m_}AZutrf7<)Uq
zFo&J?0`c54CuwF?UHOgyhGXoHKF%{OHn+_&k<)fAXheJ8@XV^h4R|Y|H+Mx1#D<M~
z3Lp7svT9^iSQyhYbADQIz_fx^Z7{YdMg=Mph_pZrZAzxYfqPBOzU6K8(ljH89sg~;
zqh~cx=(*yLdhhw$<dfuj%oH{z+;`P!66Y^9li(OI?XA*XS@{m|PqF#-h)ryAbbnP-
zsE57uN5{>gEPbbUJLkUGQ*pf!Slf>WM_D6w2(HsJUiM+Z_kp_j7MnWwaSo5O&>9qH
zPwd+Sds}*dBF4SFy&Oxv>@dv->LbkgV)nn%O@>9+Q?i2|%M+hOH6t0*?;AzNk{j6j
zaSU+|IrBA0A>U?_7B_UdSHK`<y<EeuSw2EaD~lr7DT~hDo&b@col<x1EZiHHwJe=1
zT$6@{@{G9JF$~-uN#6aIBS9%-1f#+PimS$7=@>SdUERsSbchE@P-zp4r;}7hJ1e43
zf<q&>0SAAv75?XDb6!G<JartdMbD?YNv<-KU8UC_eKn@B*e8r{3IE0M_HT#7;S1y(
zHbD>wL@V0Pazs|4$f7xTs=o3%2y0>$3>EX%B2QtQs;km~)NL;LPlg$4fFD5aEt|Ao
z8@Tpfs4UFz8`X5j(}1$C`^Co^4nk|D%qi-_&Q(kbK#q~}Xl7oBWs$OPj`4Ns(0XaO
z9eS#u0SD2Pj1=Zcc)F`eLw3G<2zO?uu{m~cr@*lNvBfH1^#N^CSlKjUx|Ks&u3??A
zj}|j?hxw9Cpw(5ehT(S3Cqe|ar$8O=u>RQGvtAwj0Pp}`V`git9CrN41w74nW5-Cj
zVaC%d(CdXXH>%*23sBaDmqGq+uM|;_bkrN-@a%3Y_z-bHdg;{#$u0S@PLSC@g6*D}
z?N}2x0wb;34w>Z}0T0l6I_w`Ev>Se;-A6;DT`M%faJLW2G<UiBWaE1_@-N$Er-tdC
zeSQUh>*!bVGaxws9|pkvca`T4^`QT)n|Li)tsUq@fU8U+<W4z!EVnXv`lcN}gA!3x
zmwU{C_6S?z6~#GD4KJGED_7}0semL|F8SEN(H_q#=jcDLN~Cf7YOkt2EQc^kT+df`
zruxw5>sQt#@u}c>c4sfO?BiaM>r1mrc)eKFlOosHXL{~*%)%qvu5ucN-T}_pb7mNa
z;dy09MStp6i5tc>fY~~X(>`CW@AdhqCsda7=jvb%{X}LY_I?UOMO9Gm8xy(5SCTYB
zX19!tVgaHeVe#<xK&?$ZxnpULYM#=^5OcOlyRFOk(+!tU_6-lL1WWdgH_yzg?etpb
z71!2AWj&<Y_mD`8e-|05&cBrtRS4xP=gkDUx&E++f8QNtcRQSgL;?9wE@7-XH{a7m
zd)Vsre8VuVa~WFutQ~wOIXK~@LS^lyZ4XseCin>(0V}hN<uQ+P7R<Uj2^C8-se#NM
zrPE5V9$Jc{yO&@MH$beb)E+yz2jBA%(0?o=YI4o^3c(=r8MIt;`=+eDdxf!Sy#{}0
z=gdOk^o;QUhlX6oCWA8VAyW(${o6nqLed7|o0lKw{dFau9N&TU0vUEr0<fyzLD_yx
z8RYeQFwD0|Tpf^(hV<I6X7}GP$!XZMKkr;`^-)DD%~7~^53!DZBPY-9T1@tpa@|i>
zGnI%@V?0)A*ABLPbjW}Cs*trweBLxYe)tHEfWI`Du_XyHUY2i^JK5ChD>=N-Sbj`x
z)N8x%Vw+=#Xr9cej<Aw&Pf}5xt~qZPn+W=x#nU1#eDg6&Sc58aGgPc2$VhZ`*A(ob
zL8WB}<r%RavaERQ?ASItryfiTafO@IXvuyBX`z|}AIu7lRGtWIcQUg^n6R&y`c_|Q
zkQETfjj>Li+|u2nt2#6Gl1~E)aV&Xv^3GV|BS`MW^%0w)4P-eSo_;_1?1cjl4X27v
z?M3m$?`W%Ar)ooH=}#!M85u@@Ji*aTr8_Ef^Q9f7RBH^l)Z%{gBKu@e75x2hsUUQw
zSS3$@qR*K+vzH~6(%OQI3E<-bq;Asw`MJZr9Uj~TF$EkD;R-7u5d>Qv=}5=0jsnDq
zPC%-io&_SUr)yEc=7Rvb)dCPGtKG#@0_Q8M4#>l#fNUD|6Zz_Y=|RkR3oPN|>C8BG
zjE6S5F|C*>Co~&!cSaMIGc&1DZZcyReb(WClD!^2b$jamvIF%981Kj>n=J;xB});B
z%_EKei_!FV{`u1`kRMKxpWUQ~mjce%an-9H<X<-xFDGHIrPAR5+nOL6z%3W=M1BX!
z!jdrB1hGDDl_NL`&!<{62Kt{(vWxrQJzzl?h7WI=hpa8h+<VN0_-yt}jWbyy$>&-o
z9$(r+tNYLK`y1GH2yQx)^(_oAd<LXFI(FKMu}Y)Ho5mGs5|C$U_7N+FzWoV}g+@XA
zi=BC8yNl}QW~T=UbXht|Lqv_TXDVi=PcqG}A!g^)#+Do4%xbmAnyJsEU1n$X=?$z+
zZjH}E&a3Fu@I2}{?^m%Cij)YA&YjvSVbi=buJM@@_i@aS?JU(Rfi$o9*33$AQF6h%
zF{{WIntp=ubL{?dp)ClWXHD#%NiA>0*j9_E6B<QAQm!t<b6@3jF*Kc;no(R+aqlOh
zy-|{pRY*bB{Id=LpczrF@^?_0LLc@DzQ!GlZ}uu_6M{b!z}`fljj<b`EPqKF(Aq!e
zw*0dR_WOolf<){?v`YRCfhrLGv8ifkD{;STW5COjAM06GAMuT2m%Vx96<zX$AO!>&
z`&Sz70|O4wBGwIEsJ|l^1Va)JmAXb#ts!OW`@;6@llv16WS2&?V)d#adWr5-h3ej}
zbjxI19$#tBG-aa~x+LQD*9X@PWtp#xpz13UmygLd_f|JbzSO+zJbB}`<3h933&H1V
zTPu3zRWSZV+DnmH6D2r#&v=JO>vHn9XJ!a#l?qGGc9nFOne_){W&<AN*gN~}&S%HR
zhgxm=_|k3F^)x2AUuw}43^kMbQ095^94k3^Bp!v0i4h8p^i;%;^x6+ANQ=*Tdo1NA
z7@$~{ETf+}aaoJJXJk(Ou~qGtUB{w;WdyVik=?L;F~oP!8XNM~k1Y5*EZ77Eav*00
zs`*6PQ~b9%Gfw4!UrGo6z*qPmnj|n;_t6T^s>~=bZpB%nt|{sQ3lfQ6O|Dmys9*79
zt*cBpxKTn4?eI1(tX|ZK!u!ho`dG&-+TJ{QX+(S2wQ{tmN(KUDSmM96egA#!h~vDp
z8pF&Nn~D4IF=+4`myV6F7#sM70kK+wCVi}vcch7WNhPUMyZ+#O#Fi$*{3?rMZPhDy
zc4yI!!|<w3H~i|@Sk;xK@VDliYSz`%{$8pP3J$!leFOoJ-xQrY>*;c<Ch1n4#4J~t
zKEKo6RNso@Rx*pAP6(>a$nDi_?$Nj53}V!+&K?lfT1B$weq(UA<`97fSe_kXJSoGd
zk3=s?qFb#x_O8K}X5|CgVU5Bf@%=mQnoL`T+$j*ThCz+9!{OQ#WUB27)t^!qxGMmU
z?YVF_DY4hi8SllLKZ{;><4*bFThr61=~l#iL1}@m*@0H1_j2{%bgz3MK3>@rrhC6|
z5KEph<(Z{ZSsx%!YBQ2k35O5v^=@o!Vq3);Kf=4pa`i7!W<3lX-(TLmpxl|CHhQ>+
z=T45W)SPfEa$_N|e>xDUlAF_A_-e1#vE6X;)~7i2M+XPvLg4sbnw@qiM6Cbg5@8y>
zRk&;$Uc1lrlN#(*^hf>9Vxw7vnS-H&L#es<V}MeV=sqm^&SGiKXl$<=5@NT=VqI};
z(njAC4;Hz>39@NEzG(`C=bbdOuX+F36Nt93SFA#M%Z%7FNGD{Aag|4uZk%7B*md)w
zMF2G1_YVF(tkP?Ocrl9$<eTyzt*L+7bWMN)lh)H|uFFt&J9IlTyQI)w?oB@>F>4Hp
zC-{S~9ud+wflL2<>orvxysw?<8^K5@f1<Cn(J^nHs}U0eZ<w{eI^Dk8u&LqNRrutK
zV|6Es2z4%3h*yW*3&RGl8+f0Md??a*O_T*1luJz4;QbBa&_Ye-5JCQ8b1y5}{h^zg
z?YY-vv=oxR>T8M~Hm_fgm~O+1huKPQ%ZjoDxJ7#yj_sv*M-tMQsJ;yqKl1M6l=ePJ
zSFVZPWSS4pv5A_MpUK>$Ow}h11CvjF&>~!k!&oZssD>9oi%R__EIGBZLd#>RV>edr
zR@zj*xP4ciy`sLRPQxt(A!AAMNX4Ht_Uew{#t;gkK(TU^^z55PSXNcKcKz75UN>S5
zs10iQo)M$HmC;#r=Th+F%e3MYotJ^cWS2|InxI?e$5#H#^$HscPPD@dcgDXJOBlV!
z4bBxQf<qhOxY6XR+dg~nVJAlpk>0b6rk_3xA!BF)X=6Z^te4>A8(nGSmDN?~<!~SM
zY~x9zsua_%Bc%g6@jx#p{>`o6{a81s;3Q|-{M3wZs&iPHx!m+|Kgi86wxxw(d|7b<
z&@_|J85*eO_Io+YrGD%D)*!HZLolYsvQQQLzL#&Csjd6+!X`D_^Nuz#P%%ouKey8$
zRC|X!EUj&KmKdtjq4kizN;%kF&DLyVfh(Xs_Dh2%nF;sR0Gr)$6<b#3lYcFl|GC5Q
zpB7QDa6uTlV`QBdq9JXwwBtoSGP#~lyVc$h?!|5r+9cgQ_Im!B^dv+1x?RA14%^3^
zQ5OE<%}5ih&Z%?HZNO>K9CqjeKv3T12(vvY`*h50;Aj=F1B@-f9y=PiP<~P1qzwj0
z`27GXEx}y{JdSc;NTFtI-6>+3>l8e?TseT{rUP7=z(A6WdQ3LpZIt*Lh<Vcn7-*IN
zJIezYP4mi=5e4)sGo10Mk%ed*m2^U8Xp5r;B3$+*12&dlH|H`PmK!IAF<OY`ZJQMs
zcl5cL3pu(|=-#9f4_;+UYg<+imw)W*&Z$nVoy1#3^zvTfClCQbZ}@PiC<x=PkP6cL
z!?pF#Dcdb+{U++U#A!F-Esf97x*DaL9<*O@PP#wLpS<g7c6)Sz$|#>`*x<uHge*hi
zxxf9caoH1!Ggxf`4I~TgDo$#6vq3piZGyom`;4(s&yWVS>>Y`#LLcM$FL4y8Gf1ah
zx{mUfYz{SJ^pV-QwRNn4#!i?y4>5ons~bnE_(eQ#7Z@tcIOrwDOJ2jLW~kNPNxa4t
z`-q_ECq{=K%x@%p_nr?Rf<J^&0*TPfLhbOkPFQeq7T-Z7r;*oh{h?g_e@?CaZ>g03
zQ+V!&Zy@-X4!9|Q$p=znx2Dkp{xWXKpd+W#uEPHkt!lg@mfOKm+Kyy$U%nNjk)%#{
z*TmD2FUC3Etu98&=Q<5)EbB)5#}Jvg=M0lZtRSBqw8H#S85%dDR2$oJkSrKc$-)1r
zxG3_33=(|{@9g?X66wdMg~l_Csry5!wt117yjnurfnQD{`j<efI690G3iiwmO5Q0w
z{$?T_LB{}x=!uU%)vTI%`82!D1YGkp2m{|j9PvVGH#?udFEi+WrW4Ll^QyS0g(Y+E
zv=>-XnyYCeT}rUZXlQZt$!z$Q;RdBSyK)VQq%-U7?KXnNf)5euBw8<;uhl-P4+8n}
zWcys+p{x#zW%A6wt~_suxrrYWB!7|TE9c-jK(+R-pa}mnQ6lhxP;KZ|KL&v;&g7W4
zrL*ft{-dRWVr&)_;66*sT=oL{acSQIr9`+d+`k$bN#Nnm-$Ic0E3y{B0FDEoQspAI
zk1xssu`6=5&JX@3&Fg=CE&OnS{4d}1=|ykjZY2TqoJ+{q69R(_@|=}lQeN=C9`C;a
zzB*-o?ie2v`yF%(E2GHbu>#~?U#B~9F<Vq*xc~Qk^Per_Cf+U(v}6)?6BApgiE`Jd
zhnj9(;LoY89Iav$<<y(C4=~|(LWVbFf$R0(I;RFmW{?91L<n0f4(0~KGCKeyjdVbs
z5hj6sJuU={j$eV9_2mfk1#5jQ=m|r1z8dp+P&&OLeqK4ucuk&rl%MtMWnuk}8itpT
zw;vYiN;t9U`#-Wf6B^KLt3ZUkqn?d+4!7DNaktiUACcAdefqv4<O-KK;!U6kQa{g!
zmZ-z_yuVB{#sb)Fmk)q<wJwzc9@M9t6N+viFa*3AM=l1#X#vs_eTc&0$run2%?oJP
zt}h@@bpV{kfi0kXP(NVqeen~;<(D7Al+-@=8Q0$WwA?cov9@(Idx;;FxUJpTu#0?+
ziodT6uX|1H-AZYrOX80A#)^Wxnm_Yc7DB)95PvsAocjmX^*_=+0YY>KkQfCk2Y9XI
z0+HMHR_77FReJq@q|f@x(!*bdA!58Z(2?y<V%Ncl#JF-z7OmRRZB~l4#3YgFZm6?$
zp6v$uI$vvURpObIIJnv{rw#~6ORq)V9O6e8ZqcxFrw}C*mUS#%mb-d1)71P}GJeTh
z6b8wiV(f^S<Vnyh$#w#NdBTx6b~gMRURktn&ZK^4u5qvT4%I<xQnRD?Oc|!i&1Nji
z$Zp;XSydU-p{GaPCCE*o-))~ooE_^(sf6yBcl4}@DfZEFE$hoM2H200EaZ?H27_SP
zJ82N3wYxQ;A=&gn8zT-kwT!C8C-V2#J86g3c8sTc;?(dCoHq{n#uFJrVg>ixZ{|28
z5!!NB!n2I!-n`|hVc*uf=i>R+>Y;JJsA9{MRCMO<bNnCWqyJ9~tvu{crt*BTGZ4ME
z9m=5F9rkTJnP0k9cgd5##F<mq_WoI;$#%JR|Bn*%C}?DoZ(dOg{;M)f@7p<tarsMT
zoAhFG4*lE39CgED-uFE@w1*F0On>flb_QR6t^s|__dpmN$N9XlkD-<ze~^G}L5s;g
zogA7uD%PDyY!q4O%@xlJNqJsjqQ>U0v2SN1q_kV!<-|=nl?CUE&v)w~F#9WpVYXjL
zkA%~;7lwg14UJyA5s!MYm5&q&e$(g*5uw!x0&J+WCS2t*oX$_s#c;Kqu3<t><%i_z
zDrER1^HIAO6v8A~k16A3{h9vy#9ykqfB))FctU&CbSaQbd-k;%jt4zVgDz!d&rVHc
zJZ4_~QWIxxvY~rV<R#<lz?dS%evl7guHPq|Ol9)gf=<=3TUvfDlOgUCaqUB|Z*oTW
zhPQl_-*yTj?)AlZYYMfK<lL`#HKTF1M!22hJ?L912hv9zZWgvM(m*H>joj308=jHn
zwnPtVtnbAyd~DXx@HI4{B8_Xz@Zy4ppcXbV8XU)~(;gDw+oHMmr;w9_*sDmWR-2z-
zqc7QqeVlhH?b^~kTW`R7p!l)WcxgP2mpj7Tu_l%u#j6T&w&U9k!%IN|`M*S0=g70s
zjROIt#bS{&w=PZyxK70q`&Jm}=hoHZK3k8LY`d<1&aJ($y8`dt;_KXaZon63sPqMY
zc>tp;e-WdOQXpMvx`v)f%XTT8g^YA(m`-~dPCbBE*O>^AN;&hz?~k;5sBR7!Gct*E
z8F`<J=BTTw@zt9}S^?=Sx)t!nioM$rdf^J8B}?d&s;y!VvS<kTvnt|~tJbeb1wIOf
z+cXSXRK!ef-EOuxHXaWyE-EtM%X(RR){g<B3>=J>s}w$(Xp`@tn5>aX-QmplX~)+a
zQtrTO>S{%1Ga5%XTn~<YixR_~^9&mdqsu3Oj8hdcCjO_sGaGdtrKzm5cFEb}v6dOv
zbB$IR_;561bE>s525A-oK2V!W{T(_jLE2@t5$HbK@L?s|-7dU(RB50*|40?r#9jbu
z=FZY_`%|aX+;T{w-U9~H+=u1Q;5#Di`3NJ(^)$C+y=QCViy;|7S}%=NIa^q=21D=P
zy-G4q+Id}<KSoXG<9+HuS_=ZkSVK_!9|F`L?6l<Rr&JRzt@gS7Yo}Q-@1&7tkZ1hS
zdHqlE+n?>g`6V3qFJAo#B{lL_4XQh4T^>!mqYaGnCXA}1mV^A@^_0led;0MAe27>C
zwlabd%5ry_!cf@5>)v)AkypzZK!K^g6N>{Blowz~jRM$LK|uEBOb3MV6*F`wjs~9l
z9V7w><NpAq=n=L+)IB;3tkYedbO#rgwHjG$KXEW#!(dwM2K$sQpGp#^!Sv}xuH)QE
zW=u>h&r$U`mFpSgMQqP+1$3)`aISd#zYSdTM}YQEczpZx{RUv?ovxv4v$O3vH@RlM
zggZMsMRS(-v~t9N&!x_M4pve=_xhE829+96Xa%vD6jp>9b4>{oED~5S$f&DH;0n}#
zWo;FqKbaLr|D+`9e$4ewkPGt#Og1t$+|LW`SDKCS*DvQE*oSUbEk7T2`ds0gW+0n+
z%<|ATyINedavs0_Czyr7-%oa7>DVTKa=}nqDB*6F-2>zY&~d%mcTlW;^Lp}k&^6G|
zzYrPuYk4LBAnWWTodn4O_%~x6jJG#)I<X0s{hCqU0|aK=QTt62aQG0`eE+RnRdaZ0
z@%d;Mu!!35bmy|sT4I`U2>pg`>0*UNCzKx<XkY=FhuP*kHC)PGp>C&(wU}@`8(V6)
zrpL_2;Am+4Nb42r`}n!ztBZ_Kg$mTr+ZRTO7U_;8MmpMfHR#aJrmKSu%GsXXCWAH=
zRXeF&g=AS=#p&U;qZHGcml56$$v#aCm9d(d!sbD@4VIhJF0m&XY|cWg;!&*RBhshb
z4)XrFDWWM|I3{v_6#cC~J9uvYL0If-Z+rd?Bl$7pM(GRE&7dv*%FwT9koo&Wk_;C0
zRnyCu?0vgx&(OBF?gDt|k{mVqEf`Yd5;k<$Ul7>Tq;|A$AuL2N<W*o#vj+Awj&vY8
z+lo05w2dkm*we)C1AE%P3jKMyAQ*QEljm75(Pb(!wpV=bSn$?pWgi48ssbN@ec^I@
zH{Aywo?cI`dA;}@WYrbd71xQYnoO-O_?O1?@7I|AKH&YM)AS$aH=mRpo_VJ_Zxrzy
zJw~*Hp*d90%^M;EsdYpR+XU*CeZ_s6a^CBkq@36k2@&ooXo)KQAcVKrj4o&rfDNve
zz~;$A6@JPpe62JxeNP&aj=@I`;O?F>iirwQ%O_;X%Qz4QaEv?6M%-3_sRK;9Ily23
z@_<j>;zdAl`M>rg+6KX68PBxi4)L^WJD+OuJ>!>eRo7KkX!L-4EQ19(D-+$%v};w^
zxI-jCBBH@~)p>lHAm0UB@=25$+)-PBP0`Wd?>uAw3@GYQbbl;XF{!1WCF0!>Gy`g+
zO1ry0bX_jWCH2mPOD}21`1-ZtQiP%BuqV^zxeu=`qk87?+e`LLIQCsI20W3K{I>0W
z6cbgF$fRS->p^N&L*K$Pz3ccaB8KfW-;`-Mi_+2h>0-+|gGcF=mnq01j!~AYL-zf?
zJJ*I}`HOtZ?PYb(cs`K~{#HJvdJ=8x#ozcT&iQOKSO4f^)9c>(UhV@<f=gF9Gw?eb
z+>VtzBenFGhp~{enwH2-kNY9QWWk(IKR#8-zRcjBF;rES$~x-U|M3873)VZhYtZYG
zLwrF)#NT)7Sl)A}LCZYP{y-XzNn5`$CcuZ-OQC$s$;r82Zp@udT2*j~HNlWO0&y{s
zTtM-1VqA;d(cg>1|K~_4|93Nsgq#{vs)1IHf=`5%^1DWhPdKM~c+2XI?>Jj41C-5p
z@bpGr{VOLM-AkNApryUi$R_IX?cz!k3BzG$G5$NBO(zl8VGyp){z6{q>8ZB($`?lE
zS?`m@>^>AyeAZDUcsqEWAk61MHs*$8&V)0+Yi-FEM`gg)a>R^v-K#j#22Ea8uSM1{
zl#bPt3%ZgSr2p;?wp&W+<AzoZ`X-1`b=#&`d20He#BxL8X=*~_xDBX7et>UP+?pgu
zxMG=vXc-`l>;2OW-)?#goDau_Hm%Rq=X_x@@NPEJ6KS&BfSOeawp1jvXV4A+olWGp
zjPjR1Q#FF)FrMqnha?_NKhnz4DCX~k8*BOI8m=FuJ$I9%yTeZncVIA0H}`y46{1k3
zQbWYgc6_75jx^S8n7b-cV~1f?bHN=Q_r6l7c+XE1H)(SBU9G|S#EHkT)ZlZpoh9<0
zwrwx*S$r<e(<XQ|@NRAvD<-n?fLVB4)+|pW!$^xI-1R$%QugNENbQOS2~u;p=u9I6
z#JeZi{j(`d*>B2}<R>2UuXApI=E$p1KJp1p0`jc-o>_NC-&T)1IXU{6<%ij*Z#i-K
z%6}l-5L`fGXN_8+kCBH^L7j2P*-_4~&sG(gAsMU5p`mU}7iG=!eG~x?beRE)*YfQW
zQ5LoE^6JQ2;`}F3P9+VRIf&~Xe1rKSGY&en)B~Qbh1C(P;&PB~5lqZFpp#51_e>m6
zm4H`PBcc*bMH|M>y%W8#*Qa@-yS)Z3-(X!EqHYzjt_P2%BDQO;r1DjH%=yq!6<O?j
zf9HJcJM|s)25)C)+KB2Egj{IY##VDRTU~9;zR4|8va4d}pBoX=mU-vGhqmzQAM=0t
z{M_Fbdk;MqNxw=dUx@0byQz8R!lw0$+dJYF1u(mCo8$Y*%~&>ut@Owd!U|8^%(<ns
zPTLqZl0sP*kgrCb=aYqwWPzZ!pUNGcKn@suKQ-rzCmumex^ofTk2w$abyQ02`S+?5
z-J#ZxXw{Gjn{ChhGXu)@#{NDH!9R>l1HixkW?KkIORz%di3l+nGs<=8JFoNBr44Rc
z!`!T)Vl(;CAM>DFoY6T!R?m9U?v+e%b<r>R%OwB*!_5CL>IwhvPM|--Df%b<2K?`n
zMAlALF0N0k&7r6NbGC3G5*HR?6=DVc$0{pJBzWKJku|H}btflhS0cfCZWgXTUEj8M
zf)EMbaDHNC{lw_BxUh+!nxMMiO(S98Ldb+jP{-QRmDNa82rM8WA<QZvBrX63i?E6c
ziwKAaOR`D`O9_aAMS*hHfw!;<-eVP1b-wS+DyYPI#nR6Fk*oC+0q|8;d3hpvdDg$S
zOGNa4tX;wqq5@JPBCKF3uz;ADFsrbnlz^ldm{m+jLI5oG<96BD+qylm77!N@{!POq
zf7UQ1RwHo{ao~mytDwGtA*&=<KuA=IRZ>VuKvaU&$<5Kx<QE?$^+$EZeyS@WE+ioK
zqspQ`AF1EG_%D@(#DGr%|MtzIr$hDEVg8-U;*z2QlER`ts4ViIs4OllASo{OgUUZ2
z$lrOhh=hcIIQT~$Cib&8t2jHk{xpG9fJr4HeA<^lx2s5qvx<nEe%|SY<mtuf8&5B!
zgjhv|{#r~}NQhNL@^4pQ;J4GK|5Q*In5?JWt8+RF1@*No98PES>10)him-}{fdBnB
z?I+Hb_pE{0&MK&_q{1qA-`dmlw3NB6^-b1a3^p)^f9vww)=svrcB~?zB9g2>{UH)m
zv3CS!KF~}@b60C6YfEP<YhbPr{bhZ6B03G!W2b8`HXW4avC>_fM}-yU-KJ<E+z2Nq
zn@QVZ4eh=BnX1spGU5LCu!f&6vjntw))C)@O8xGaskwtRRR1FPEY@}R;Oh&PS&O<>
z;adwd&Yuoaz{+PwG})0?dzNJ9undGsL++}|RE>pj{utMa8KhmQ2vkqGva|*q^Dyof
zmG4Jsx8j=S)bBpKOV}@cgdr&?Qf6|SE1HYkGH<X)Mh!qz-GLPPyZZ1l8FI!D;RwQ@
z$8}ONhptrK4hvVu6gQtaL`k8-KXXQL*e8-qevby?YFuE!q4gprAGlvA-Pl)Herk5V
zg^EWOa6ZB9nQhBsU2$6{j4tPm=?CPwgxLuR=Wt`2uUP~sU&wCM>RFdg++n98yw+CT
zT*u$rPH<vfZyBXLlRznNeYcRTdpO2^^ddpC;r7Xggl3v<mm}f40J4Vki=j<?1p|;8
z7y4b!jfN}hn=w6Cj<F7GQ*|otFDW@WXErX#QXg$zJ4DCEvV4LaUNDq8IXIjw(7B>Q
zNC|tc6ZYs9TYgvp?h#&A2Ry<+=tyXws05xPF!L@i#&_b51bW4Z*lf&q!KV0eWqtJs
z!Ob?C&=+U#-g?+V!KO}eEw)jr<8HpL>HQ=~kxRG*H-0Y!W&B9(u88BnTKmworV>}{
zF!3ZQ-pDNE{?P^Qyjv1dPUhaCpQhj&^_eBp8-}(fP><TWa?8y_D|&VIWq@F?o^osX
zQ@QRszi&f);Jh?NKBL(y{OxIOE!OBy)0T4&UbCH33Qd1+=^QsV5cS<<zc0C$i$wSn
zj&<tdb)_g30ypBJ`FZZy=h<Rb6%TdE1{fi^dba7&jgy^v9!`TRw-XG^u5o-BFTNzo
z|6IkiokG!4_`##0)h=K2JrutFCU|A0S2HH<T_eXlg(FHbo~S}A*GQcM#-i@nFiR;^
z^s?@FHn}v2RD>wDsen(^#^RjC`J?ereU%sU=}z(aAE2v7jhMV|l~(8IiAB#QlebXZ
zH*<H21k&Jg+#Z&MubeoT*4v&7uil5G4QOmGv(mKRm!(Vp0Nb8&3OgonKKk~Ga3f>L
z>?m_6biT{7ytZO@r4i%MzN&eV6LKL>@RqM)`^?p|o?Baf(AneqDndh|o`H|^ANUdA
zMUrzi%46;YYqU}h$eS-Y7)BoEn8mP|r8gRo-X}S8@sh}BTPML0b;fsik6Zoo?j`|^
z?sW=@#~*E8J_`IoPjrwK=2&b0pu*<NQnV_Ix9kIM3gak=+r?jluiKC>RjOR*DDExc
zdiqLrAaaiF3f%;5hWWlECA-V{;|1y>ZBD$q@JEMkZ>CE=xD=qc=wdnL@SgiO^R24C
zk&XZ6bD^>vA1?qCSeL{msX4-&nk^@;?yYr!iujEGCD=XGP>php;Miip$fLr%I;Di}
zm#J${2l$P96FjC;3fb$Q7&)#~_`Q>7bPtr%eR_>kdhNpW#~!;q@G+vZh39sJ{*2)5
zYdL}xN?79;fp5(78--=_-3RBZKBc!2w&j@k+o`?#Qkd5<kSWrYc&m*hd82Qd<VBuH
zU0czi>n3;1lbVH`m6C`L#WER0K^p6ulDq2_uFkZ4e8J0&uFhH$sWMN~w#?YC9O(uk
zif<8pQ0Y3vC;FBtvR0Y-<icY*z6svRu(s_F-g3&UAjw7ox{EC@uX)xuG4Lev)Sh3+
z4)lHDwb2t<CxCeFd|+7BseFY-H-X^cWh}PIpEhINz!WX_`JGw6jhR%!xNicv+l_=!
zwb2^EvS4gDsQ;PeH%5_EyavI6NtfI5;Z+jM1KjxBxp*%|F5V_n%`aNlHJI&3e!Wl_
z*L`!iM`7`e{wJ@f_h<R~EjnJKLGPb1WlAvfl|H&?<g<XcpfFa&ao2)?-q~&3L(l#_
z$}@q}p(01py%0@7A!mKF_5S#^<~djPkoy_QPXah|W8G4(SX4Vak>i=NOg2-KR5rfl
zojA{D^*Lasv;QjoSBkmiCGd3-?yo5dsnU{7$y4`UlI73X2tU8dYDV0Xa%T6vM8>qb
zfs|@#={H_|HG{mf(?&^a$Y<3c(`_HQPEHhi>H}7P#-c^~$OTO65;eZ0+?iulZ!LOr
zajWL>jr+_Rt%uxRlje>QCmpoMvaTHSjrk3?PBL~l9Xr&??c&b(yn6+86eX_3((>PU
z54sYuX~3t(XGh7jtGg%gr7|n{cJV8v<(O8_YZQevkL;BkF=DrN(g>A{tNb}%#yOp7
zA|7W|`eMf5Ij{0TE~JY+%GdI)wBo#c_ma!!^Ge~bVlQS)`Ykq#GT1q45~aJ-9@Wu{
z&CX{jo{=Kj7#44k4$5JgPGjC*9WBjsFUB^SJ-$boa_{>6e1&tg%;FK?^NsI_Q(9Is
z;9~u+;-kq)AM(;tD%kDP+?0}hh4rpF*XzcGnUNOJ`uu*PEqV~bGG*mXwM#?u{P_wz
z?yQVIgW1J|(#RG|`-pe9X>Ty<`iGWJhh3tEQSc>wD4@8P^az=tS^J6%GaSX}>-y;W
zb^4mC=jwA}A(A9XSS^Vv1?!cAT8R$|C!L(|f_mY1w{^EP85K1rB%Yckk_P*lBq(1|
z9er2ABw{hu`t4}gwz+M&qiL*eRYCI!%d;okVFTvR4_;@qr&9{)4^|zDiWKz189EPO
z;YHa>WN~Wng*TQX;15SRLnR9FDqeWcXz6ZJzD5X$<W;+gE<X4MRhtjC-;#Wm{E6g3
zPKk%>fMPgJSQLm@O`N-+T1RF59V3Lno1I&PkgD-3r)@wEVc_jzD|}7)hm-+A*)Ay!
zpm)j9c~(ccH?{m@lJ`7xk2|)>uHg~P+aAxIb@<ew3Z*?~9kn`=2~q8Qo#y(+zf6}H
zVXkgc(akW7j*OOTe~u`kw=><#qfOj<yLCI5CtBuJ%e=~Z8|CHs>5N|0tqjFQ438Z<
zy$k%H)4idO?DmCSMjt`-1m$I2RZa<~^{q4Cw8YM1u3jwcZ$78%e?zqh`(ehqKHo;$
z^J&A)b9*-eF2?QF%`mO8RVSzEJiY?=R_@Ri-0*hJ_$VIE8b#WfFz_z+6-U5sShU<x
zwv@o7FCz;0L7%@@UI8zCf9w$$DE8_*E!FJ-mmbUpqrdcb0!qju&L2P+(Nh8Zud?mG
zK^Sd793v9E2?&hVPOdJj;y=k^I;TLz#o6tNrS<8ppM*amK}~Bbd-EI4o~%DX8!;&{
z0TDo=lmr9ftP}uQw9TIYj{ro#QyCa|!cXAH1rYzlMgL7G{H4rydheG_`LCiW@F{8%
zl3*2<5*H8!J`pSmv<$!&B0`b^LLy>6L{lz+)N1eS^ds0J`fJ$oQ?CKS?<s7Nk_3R7
z=<mW7;H`iAs#6jE7j>n6sw*lbA|N64TlrrY0D}KvWf3tk0b%jqU<YCF&qwxmUMwsw
zB_RHT7ysJ;|8*>W2RlgpP4NFC?C=YS@kc;HSVV$V9Q-#V@k6xmix~s{`HcAsN)VP3
zmiW)2gkpUom#JzhzlqX$qx?&RY!}dpY3^N{oY39jNI4hgCn8J{fmY)USJ>^0zBkqD
z`%_&KuYU2BJwCG2LU7JBqh-oF;Ghqco6=cWBm2#>rJQwPw*M2cW$&{urtvm>Q8%}3
z)_u{irhalmrX&a@kKaf1rQ>Y|*3I1;Zc1}i8m+#4t}aH*HS2Rp>}Z#p;~4ME&UN?P
z$BTIOl|_jx&a;BE(|q#$l6kbA(;6JKVI(whe0@5ld2}rD=K;UfS#LaQ#iv4Vs(7Sq
zuU|nn?v~KO_yY62-dNwI$@IFD7o{I@KFe)8`OdO#!g@JWJNkk13(PQAKn>YV+VHT?
za{}$7&PnDLju+du>&qucx?C$M$<*6u_EUV*$(l_}n(NYJt`j|sVp_PF@2pWsNFJ!6
z(rVB_&{cA!&_|R<`vP}n$4;@@0QK7R{@&x1Io_uu{PNi_{StGg8;Hz+?&Q3q@$yYX
zfe0_TU9rJ>rL=3Q=9f2pjbcd|D%>r$l*FRy43ET@j^4)vwtTNECbwQ3t<|mI=3#rm
zU9I){a(fxR*=mb=n?xQayizZ=V5Lgzh9v&t@_C|WyLD;jYO4h>e4Woiq_+hX6>x3o
zvxUZ+$)2-mLTMp)s=j|D)OdY6t77;3^L41#8)~}HhxSXaU_Lt2g{_xn3M;-V!nAo&
z^un|uggfJhsSLpmYfEBud_g3cXd=YLPay|}%jeafikE<15a-HLK0snv8EBFV9JjbH
zq;^arkLLV4*#L8p@741AE@sSAIeO7)p~_co&?=7Ab)2(ss*7ps(G9r%VEp2mci~)~
z&UKM;p#a%H<HUT|>@`PJ<rmYMF*!E62p1=N#XP>REX}a|S_9>|%|r;{9hukc6C<h*
z#zWVXw#oICc2!2`j_A{UbC-Nt@i^I^-obmxM@rDuz>%*!H64`lmT&pu-Invi7aTkN
zo^5|8&h)?a{_x|>tOLZ0fIeC49HZ7tkSrbvcU(Vc9nWN(NNKk&)8I?AcmA~-5AwqU
zrDOZPyc0>ICh~b%#@B`ab}o>*ZBR)jMnnhMzTn`I{($$J{+da`_5&^@U){xT|Btn=
z0H|_nx~97ukp>CrI&^n8(%s#S2uL?brw9_#(hbrf9RkwbT_XQcuirJe@Adt@|0pVQ
z=InX)6Em}B?X^u(pOWF+G6<sA`@*!8oEH_TRH>igT{ii3PZVAqoMpZZbs4OC%3~H&
z!6GgZmg@MiXCwfn>@=;`*m0B9C32VoaB%Ygr2E#S*E@oKel7&o?<UQ2+EY;%b8TO^
zLRu00)2*(`)Qe4*SCe(T2~m%YA|Fa^9z<PLD@WuZUK9?Gx8-?(yp|<AsTeUdS4L2R
zif|5xn1d<UnDt$b0>zJ(#&1s~7~p<#w)dgStBR)_;XzyDa4`eYCyrSCH%Y-fMGM{l
z7*S|V$Mi!1R?SO>&TjJ0<!?N#4nj^-n?FuB%LTZIy@%#n^IhaBq~_nnc!{6pr5p_^
zBFBtQK(pAG^I$o@-xCrO+$lhxF_iSKFek!$3cM^N6AloJZxH^_d0~sAs<)`R+Vyq4
zDoT@ItlcJ@lh5pnd|zV~=em!Al8!S##U8e4?&bg)F&a^1E^e(1D<N2Y0K-D$7-65R
zE`V%FkgTDMGNhdX4dD?12icQZ%Q*d5u_S$1V;o;Sx?;sNmcTk>lpIj{w}}khhAlk&
zA<U4N`Ou_CpAer{0UXQp@z-K8%F)rZ;Xp%DTc}p6-$3Dpby%fIH#Ad;mw~v;&wChC
zTj^<FMrTRyH0QX&s6rS=O~O9nuX>Uj&hOVo&0WrohIYhtav})cS7x|W*iQnOD%|U<
zz(oNW%!o!*9c;Co-N2$U#g6c|!s}`=eilC}oMx2NI9M)xj%90f^6LHOCnRtq*QyB5
z86MU&c!#4`&~0SBVz(S7Bnc}D_^0BJQXk38H*$tUIx@WTeDB*nWyDo&Lu{pQzh!WJ
z>2fAQK9yyRaMEkG?3JDHDOUr_T6bpL=EM6P-*kSOpr*2jQglz4=G=&9T<_$D8Ca<+
z^7PDuUfIWZH^(*8#qX%ow~14eUftP^RcA`CBdwX>*l76_tc;M4W77sQQ5hV`lsyTe
zuddxq8{&H5y4IzwYc1B6wi~3w;EXf~)jQ2W&v^0}{BiS#fo2XlX1YkB6Q?}h5b(s!
zaj5L8)B(KYQyT!Yt?g{cfGE|@dbWMTXET3;u*TkG%yJ~x<gih=oWuv8L2-?&)3S)e
z?5)`A1zIO{CF1BXnXV1n;xFrTBDKkk^{wu>u9YTO$QPP2nxLN3EOlsK^wrzoL}~}l
zjXm@wjY}@yT7V~=vMI-|W?mjT>cHVba7A}|AV<YhCu=*idpk~jBXgky4$xA{^q9cd
z#sw&+k=YLs+Q3*Vy@>FBZ;rrtkk?J^`M9-Lz34WiS@JQs3|R|KNb#D;8x8kUw_5AR
zLT#wi7Luk1o9<cp>n4<YY5aDl7A#U}^jKo>JhjWtlUJGDSJTUP$`-ZiNT><XxEnqr
zygM&p30^SXb%m|#k`D~iH(5V8A@+;8{Gep3iMX<Kw7hyd_WGDzpJZvJGW{LcXv-FI
z90}*u<Frb4R#yY!Jq*f~kw^d_>?sH?@^R!nsVK{jVCVg*=+}V%b1M2ZMFRfUf0v5>
z0G6}-2$O!vlfL@#S913+nDlQs)=#$of=L50+n<xW`-~Y`>6kg#f6MOJSbj}O?=fa$
zWv62W&d+a9I?Jy)>3zmP7@m>+9zdJ*S1A1+V<tubuv>7S+dn3>|BzSylra8;N&lQZ
z{(?~d2Tc0kGf5!q|M$80AG{gZkNHEqxk`ODa)k}e`=Au%oZRN!dax_?w00In(nm7>
zveZ4%bA6^KJ=Jrh!U;SH<B2T`)L2wf1ZR+AO>(col#di&7s`&9JH)-}<s144gwq;U
z>6WPaN5d(mQpx(07MaSCG)QB2oR95Ik_<NB&3AV<>UlOQm+hJtD`lFg<t<E7=j)_R
ziQ;7kQdHE}`cLGhTGG60d2bxL^mAF3#5zNblciK=E2A<;jSvl6$f)9o>Q9(!LYC8V
z1qE6pONM;7q=Fi%Oe|_7nS;>g_Cy?;<ptmOJ<w#f<ijSrJi0lMP_(&FDW~0Lv+dWG
z@qhY?Q`>qmN%e3hD)YoU?DacTSRpSY@3oh7;A3^1EVP4{d%9$E)|;*^21;IAp&ppc
zR2^L@+RmD9jsh2-z?USU1Xh3Cw}HFFu^m00(Vp??(vgHyP@Xi_T-Wo1dvS#8vW38&
zqJ}w0Z#nIqu12P_VJ&-{#{;A$aA6=mRJLSp;Kp{T=E5G*d|w(6dQ{}?I-N;xS4*{S
z_Mwf9O}pv1za96Zc72S*atsarR{!TrY&F7?<}UC=ToB^&IVRFQCAVm4iLsov9KA*|
zH3mp5VQooH4V==@b3kA?gEagc#Jt;Qij3w~p?;u>ms{LRqB(q|3CrzaT7J{ig~VAK
zDZXu+hJYQ1j~w#8dadXCZk<mP&JbZ!U*-;FKY4R(F`+^b8Ufweo;M)L)7&ldHdaz8
zil`L+DawaYn8?sq1aZUbh*ic-2Oo%zQ*1a-eW9KW(xxLpU-;O$YL<irj)TFptraY=
zYHR6DRW)^0AkEIMqhh@t7QM58d+O+rFs8C+1Aj`(mO?n>Sq=)bkgfP(|HHv}tL!nR
z+w<5(vH94CZ*mtt_Y8aR#hFz1S)$SKM8d~H9l(QUT|c|N(Q9%;tJlTpS#dQhp!T_j
zPTW4Bdo%a;ZSOg~lGxURiiQMGE)DK20VEU)72(YsBr=p<QHm}it;-^A@t*ixs*C!!
z2+uGgp`p2?Jy4;v#2@NM;m9a*+|=cExWBGgRlbZvqU|lnYNA!8op%rdJ9yl&QxMj0
zMB$#ag{t62PhQ}PV}r<CzyX`5ldK5N_ngp>5AvBVo<^zr>T4o0zUM;)nI8HC9~L!r
zoR&2elTkSI>~=LN+jwM2x*BlnC7qae<5FifO->f20q*K@*TZrgBCs(b1n1Ki0RMFA
zZj3;lMpLUJHc=%5xQ@=&F+wkv9kXyGyhrV?v|W2_uSHzOZIDFlU!Ejo98OhFk5=(X
zcSdZ!bQ@KXc2{}6kz$8$oaB72KDD%HFj@cZRV;_zgHlOeb-0JZp4y^j8ty8X?(=&T
zJkZ><T%1TtYG5n95Kg}Dr(5gkqI}<-rsC%`J0T%V%6G3|e|Q8@_CgMn()@M;(mp9Y
z2n(f_GrkRdl1!*Q!1}$<VNiQnYAEbkjnQW&=vPrSf*C<YXZAjQZyBN3K)X?86eG2a
z2cta;W{hD2EM+^0q5Gd9u{aoomY-FFrRdxBB5DW-M5CmsGdx@9=)SpHXDeOr^q#RP
z{A8lj-n*gFtX*;<hce+?avPBXl6Pm2(Bu)d6~4+hXuCe)@R4@o>}kno8Kwb?+Yrbx
z4WAtgMkr)8F9RMM8gM^09oB8QtVRz$NU7lL1P+*6cw*ecOU9i;!b9Z<Uu2{Hl;Bkp
z$pVvtaP(*=K51$+Tt~6QC$f+GJW4-0pM5aZr|5x?kIT>z4qfpws_@2nG!hyKHkTOP
zSTq&Cykr@!!txp_6k+KtqJ^jQvYC+BA-GDpd9Q1gVS9Qs>zz6oz03I_Wq%#DUavFE
zD(Lw3tiYZQ`1Zn6eE(~b);AhD{EkOYpI-pp+@j^*ecpl3qLYKE4zF#GEN#o<w<L>4
z<+-+fJ;@l2Eu1DG?y-3?8Nw+JqH)1nmae#w#OysETNC7<;uaVK>iR6DJgG0pP7I|n
zb1(?aOWhKwF2rBXw6rfsk)P0kVH=GNPpQ}3ALoh6`R8=C!AQENPtxjoNVScEhGzmT
ziR3V3+apfwW3!p87PXx12{$n&+@5(O#YwJmizPzVXSFNPMccGBh4C+72@)}P2sZNj
zIJA~*un`g3;a$>tnfuYkzGRgNfq?FOqQ~8CB}|R8yXUS%odg{Z73vO^_Q^R9A^|x;
zPz7y~(}-<MLRbn$%3H;^(RG<@u6*}uTXZX&DtBS^VIRyoQLYTW(SsP25>^@lHVz6e
zsuECLi-&DDUSO;;?wb5QzPl(m{j5BBJ;C5R_KV40(MsuW53X+OVfwPvYE+WE5oE;U
zHbcPLT!YV?gdKJ&5qV}8@sLFxB-#xz6^poKJo8E|h^FS9+qlGGOKwuiQ~}FrdbA;a
z+pl)l*2@DwDfZ3}!?AjZ0$zM2iKpxRmC>PrXnJ8k;g~7DRFyZB2TICixkUtLBK;U_
zay|Lf>!8%q-4YZ1-D;RB1g$EAhgqC-=Er#filXM|HNI9tvu$`$z}*oDaC-fBFEe1;
zCv2ox>a5y@g<xQE-(aH%`OTcB1e3g7Y%-N6`bfYW;=mSro%CAvs!y-Fo1`hE;8HoH
ze2lL|k+1COq9y2lgU2f2%9Plr+ETl~rCK0G6O%Zy)HSCyQb$omX9hq2QeQY7jn4Z}
z3j{=~3=FkmB|Zus@oZ1Ut2acQlc!+5c4utsDE9_~eY9*BXQiE1qRgdy`@{0v0Nnl5
zuGR|8%fc&txOebb1Yo0UU|>{a*&su{7c@NCoBp?-osY{ysHULn(KhMdC3e<xOBeVv
zq!{x^T-{V|f4mYTyrVGth)@4m-KdP#JN9*vcC#qb<vfF(7xv6Y-%lA19O4{I;Qeq|
zDk0|m%iuk**f_+Gp+<6%UwZR*qFu+NJ)9zVuq^Tr$5ucyOU%IwR<7}kDlZpmbSG)`
z+S%5-U67T$k#1=4sOpnB+|U>!=+kzxXo?pMq^UjFv#*ZGM^cl7o4d%)b00k;uks{~
z(@P5lrFK{sSmG-W3un%mpWP+#_uv^@(FU1<f{{LyhAo$G8a!%yljZ-k$qrD(&^jr8
z#0Ia~rnr(P>%}M_kv-}pZ`sn>2PX-)^lm)nwJMZRp6%N*iR8Y4DR{o6g_Wnl*$<Wn
zz%+>Km<#+eDY>p;s_*z>vs2XWsRvm9y&m|Qw*6cWu>OTL|9ADk570ErH#!6xknHk>
z)9^Fk_#+eKTZ!_&g{YYsnSq2KCL#a}6A->-Ct_w|1Y*G)L;xV1%K*3+q5jVIVg8j?
z^VQYwXzl;eEjAW9MrI}=R(7B}K&YA-z(@y7<XM3THOD^y=ilA>jfTYfYi0HIjQ$yH
z&iX5Q{gdrqusI6@a0>4s?6Ce?S>0pI1UxVnAh7>iaRfZtze|BH_27NRj6jwakOA|%
z@voKDJ;sbcwhtTNKI8vmar9#a^pB;|7mm)q*F$VSy7C=aXJKOf(|Ty|iKgu=8-~wC
zDM}eOZZS@5J=zm!GWCkw>B+a})j@A@a6%$kr(>ymWo{2#ktBL!i)ytM^gQj}^)I;&
zT@8sS=btRfZ@ixAbZ~JQL<>?N$=4flSUaR7@@Y+DM-330Q#Tn(_pD1$DcY{v5G%(+
z^%C8e0hM~3T-f!dw6E^H*5kKY8_w(VL*CnxO>@f&O*2~YMOdx`_JdYMR}E|ZGZPNy
zK3%OW+YA)2uxF4sXeh*J6!Sy-9~CJ4n-Y=8d1-hodW+E+L@AN=8tt(vr<g6~bA$jZ
zW}A89!Gt%m4+F^BFi)9?J_PT_J8nFWub*<v@b!k1z5uV$-g$PgzUfU2FsW9Vbr>?N
z`IvgeLr}4zY{TY$<U4IT)!PGy1-~q2R?D{NeYWjoUv(J|mq|`$1e%10%I)C?p9$eJ
z&YaCdxO2)uN@8R<H5L)uS;E8?nG8pnze__V3Z?`#261Az=EfZfn>%;Z4l=+Jiwe6z
zV92kRNr7(e&|o`dY!4zbCWt5r%0^Zmj5sGV^0qa{>O|JdiaeAV4MrcmnkHpFFWpl>
zT4uoRFrh*(XTV+{sJrc1ba#5<f?Ovxu>hJ2S{-9||FH)g-EwpVk2#+g);`UF<<^_g
z__U@v&by<j$BKZx_y!u2fJ$C_UnOb9m0Xr6=_;m$d<2si-{c_n$c>YLcLPU6K53c`
zsc(lK55AF|c5sg0NiJL`pp+NM3uT#tMDDNXsn@`P5F?~z)e_1Gej^7f_2zgXwV6E$
zr|(Mst~@C}!myR-k-#vttxDpoT%4tJW};oNE6Y$ad40e4NV>oM@_Nu}XK^E=oVm-a
zvrf|U{?Fldgq9qpHpU!9gCwz`?_Cf*vG$H<nBlo5{kn@^O6!ke;fRt>Q&qpTBxlZb
zL~+N5njrIJevy$D%YbobT>Uy#J8wvok!SmTxUi3#85cjftF49?7{EIHDF_&XRM~KX
zD8Dz$tHcN<_&|zi@qVzrwnLbmBJG%M1XiU1BW-+XyLOTVCS2!11-1y;Yu|c~cX<p#
z1##N71C~-z$w!iGmaXJZoSP-6fb=MK*{X6Bb$`VoQR@RhUcRuJ6B~!T9%WRoej!j9
z{#;u9$s1)XrGjXed6cjTYM3`*G`2ZJ^VXQrPMhG~Y69kCveF`!Uin$W2Pw?8{j{_G
zp#0#P?!}7XDwd1qN>pTmXd{bv<4-5V>WZ(gIkuaL=OR2T+h#so(++C4U4G0*E)0bC
zGjH(WM_Z3_Z%C9u8n=r~wjAdPGDjiE@8;RFiji%u#c3oXJwja2`V7!7#w+tw+!L!Q
z+%JfML`7A_{YXs7j@HReWS3dlWmAM^6RFc9R5NW2Dv4}tm{?d~#X=7<_f+?VA~v0D
z;FFr0sTyZkk(wqPQi6mQtjm`)M|4$lY8D7QbC(*2o%^3{UsU%yfI8P(cELpgT2y<B
z`dqq*-D=DPh<9+R_KvuUbgJy0jdw^_kA@!>2fbH`#Kx6kOcuPt)ey!B+7F|93*3Hs
zt|gba$$$3Nu~M2sEE~5GTwgRRvwbSG-T!jsJqO*2ZD>qToP|&k7y?dyQv1vtILgBm
zh?hLk-LT$hhOTg=ySHm`U7u!yS>Qgx&Gbfde^SZsriX)PaQ4H6KVjj*5AT+(IKfQ=
z7b4)PFilCrqR;Rc9dR1Rl+>07Y8ibtS=D)%KIwiDPq<drs1sh;zrK5kLx@jW7@f7^
z5vPWk^i*L#)GC~HlX>;}Q)1@?EUCE8sx=)T&(KLvC@?AouI_5@lY{$7w}kxWxa~L`
zou*snledKyT@ox3VdU{Fh4^I(=N)n7GyuZ!M7EUW_noPz`r(l}^4)hR`hu-uv|e-V
zL2-oBcq-B1L#^G{e7?G_i}z$#Y`>>6zc&8A27#ZmE4Hug+5ZFD%JySY_a(Z1_2bXb
zR<<8Oy5DU7LR(qcfc)g&0yE=psO3-NekT9}=m0>#@wceX_G=<@k1-oN12AE_$M)Am
z=000S7CL5N+xqu`f5ck<Yq<1hfE37n08)Ry({%qJ1^6+Q`JMuN!J_{V5-ol*Ycnf~
z;ytB?f(RxbQMF8WAfwdE>L3Ej^<tKnU6en@GK5xSv~gOYrMZ%;l8LJS`OY=*x{k^F
zwB+<;{LXUK1jDAxPhPyqEh{md_*h(wHAAIu*iHNzRB_q>D3k6c+S8yXGfwMJ*|+7p
zd2<PaPF;INWG^gO%SsCicBT<!hpOl6sb42<WInQePcem#l=3ia0o0QwlJxPsG#wrO
zWB(?(gbuAI^%Jpp{VQo4?f&-qw2v|=hN7)8o-l@iqayTr@P=V{i>UfYu{jf~>#WaX
zcf0aFlre2S+{5$^BS-XLro4Od2`=_i6$xUBi59~G#q7=_de$LV-P)ZOl2~$0ZSVV~
zOLzDdnpotaBre0bKuMZf!NUq`W7w^XHD2IG@@19_1?_LG3?=1pReNYVTPL!l9Bo@t
z*}AGCKf1N6kaG{dUI^z4cWs3{Fb?v6_oNgvMyc@SfkQA6Pf-cMUZL%4XMlm)<gV?E
zaxoJ>x6w=cIY}2&nP!Pl$St+k7^|e-j&e7Ub>alcpg#1vLdUI)4H}=XJQc%lsh~f<
zBJWsfuOlPlSz%Cm6R^R+cTUK%t!$}4)Q55+ho9a8@2|i9-bEhln4YXOBZ0bCdrCG=
z5s|xLOG;~N(mgDP&Dz1Z`NRHfqt@0bz}>Rhvv((&gb*{=slf`#=S4&z-CpBI%s9{e
zZhY^6fi)>r+Yu&}G4?QLEK3=>cpQS|kp%>y=&&G%5@aV5lqrL$v#q1$y=9avY^L|{
z5qdfX_R>XzO!VjoIx29sCpHn96kJ^U?mS>Q5S;U2hQeg_dzz=^rWEYjwuwrpGcp-V
zjpBVOnQzG8XkRm<eu&mNyio0xkYF$c7f*MQ{1mX%-W|yn`f^lE)3L+p%Dc#-nR8Sn
zry;!soEd8U(6{ax{3HyPhtVK*P>^R(7jJ8}y(t{cxtpSJ>sVZ2Vvsl}<}FWhW~0Y*
zKl5Dt=-#K~9Hh^gmzaW}<qVRIoIvGm8o0?fC=+chF?C1-U56+_Q57KIh|#lgxOTPb
zv;%m7Unr6?AA7ODDIbi$K|;_1-ZlqeT)I#WX$^t7b`yl3!L^QQ99Qfa#b%RG)<VZa
zpstkQP;<Njx59DT6rX`De>*hBN-gQMDyckv-mj{2QL!6JYvU@TSUoI7nH@pKtNlEc
z;&R3vInx(6iU;fZArt{R3Cm~Z=jL&raw~luC||RevWibsUsd2hqQ6qFA6ko;3%g^R
zkq=x_2o_yVj;HD+d!?k}ix<nA{ei<|FMP`<pa(@Ti77B6B0byGdxF@#tCEkP{$;Rx
zjH_BK5lxsu!rCA)dyhQYr!8X21@SrhZby<2Lt`Upd>Ec20gH|<oaGElx9&*F6GTKo
z^3(>Bpif~C61l4BOlKchrfMuZF_)>Gr)rqumly`KURPQxK=cQHkZyy~fbc~u**-3Q
z2O}@YHv9~%UqVGF{d8^^0&ABaFv`k-@d{5nnu(jMw(VAGQdG@bK*cabUzom-#%aLJ
zQ{^otXRVFQQBKw#9tRDDSityLho&mVe2t~j>5@p{iJ(ctXcKl89n_|_Ldy1XpY{1P
z^qg_|7RIrRHtRTef#!^2)KrSi?l7~@gfiY-OG(kI7i8S;IA?u$Evd!F2XX9bu9c^m
zC)#eodUe8|LGQ>t4@5$&_jL)k#6tGXi*or`wlYZO+&vtdjC26K3rBnfL1AuDrbA;i
zHRVPmRc|z>$2Y9}nhSqeCQ%)%psLANU-fM9rZoKuHD)apu#84Qh@^1|k+pIM{|?sd
zOpCwsxGws7?ZlqbhgY%NlXm!$584f<jUFO6I=GGilZ~DL|JlMPpx$?kV99-P{>O6m
z{;n!;<;hoi-VX#nwl5X*pLbP(IL4Q^|Ik$h{1~~u#_Dge=T9hez_#1Z6*v$c`4YFj
z*?z12SvWX=wg2ydp6#pW-?5N?2$?@M6@O|s0s)xsQ4+WW=0}3t55e@$N@$>Se;>v7
z_{ju}{(l%h)77kjB{a(CeO+Qb9HJa7Da8ZGbK<?xDv_91AtU-y{`}SEn#nX&BrgW=
z&KF!)6(TVyWurkN^H=5_OB-|YD;@GrW;bu0PR~iD9*RmxDy>T=B1M5MLl1<+#UBQ9
zjXihV@_Axfg*G^Sk~19<>?69rfhOK>MMpN8I5BNnVw_TPVzQJT;k|7V_~!JE)<#EJ
zD%W+N6|tuC(&F0Ki5q`D5+_L%f1Qjt+XpTXf=mY8v>8`hN5K@-Q}>~y7tuW4P{g+i
zTALj^$CkAIKnIP3N`jQY`~U}0AMxWA?-A&n+lBdVIRnzXZr7K>;f=&3%gi}6;ublG
zT(mT_8ZljoZ*q*NI86Mo-*q>IR@|AKC-fut@HmE3g}srm@%HU3I2ZBG?W|Bc`A!sB
zMsOmO2M{m0h@#N1$wWP2NawElb@c{N9S}g>-*b%-#6H~xMTX5mh5H=v30ttBQEN#`
zbLC`KU1<=mysgH<g>*d>;n@JTL5TG7ft3>WOH(Om|GKIz_r4hD{6bZCl|F)Ts!^+4
z1h6NJiU9Y5=1?w$Q<NbWBL@c$2krnpZz9Ef9`6YocZNt6d($S}^$2BchzwY~X`_?#
zn@;!lI-<zQNr`o%UEUS?+A|w5-dM~HW~N+yBa!39Z$}ev$Y+x=YKkdchkCI&6Xk3I
z)6`WX<_|89W{lSQSRh5uGOG{MV>X6oI6T2t*QugQjx&Tbv`33{;Epp#tZaiD`rp+n
zDjG-hc5R=HT!4wjw$@}LD2R<E++aqHMZ~Zm)a~D)4Ee?dclCJZnabr7@wuxmS`|80
z4TK6VhxZe)3}`R*liUd;(?3wyH_0fplPZQp<85^J591mUXNNF(<9xEYCze$1S(za6
zE(*-fH-@OH14j8>RKRjRg;7G8@$_ra@`DCb%_=D8G9)L=tzK@EeXUem4h<@@F0w6f
zhZdp7vocNOYwf2(t9y^5z|jH(y6I5a>2uL$z?{{j$k=9*C>WLo;aMcy3c|HxABMv!
z%YrG4n6Hrqcc_W13BM-K+ZP2G^^~#Y&X8E^Ow>u}zA1ggJqg?x&`(rwG1{9H3%x0(
zel%-qHq7Ov$iA}+Mf4j^;~CwQHbvvvr9$Vt`WN#w#H_`P#ginn>8x4U;5GBCa9Aiq
z)=j)mKw?Uq{c@qo-9dz)M)3mkdaR6WcBp431o#4;$r;ES;f~;Fa^{6ovy*4pPpB5B
zWx=w+Xt=5co%FqX`|K(RBKeLqT60#@#n@mGdtDY*`g(vZ(B&Yx*6br*0MkWA2Wtlo
zD=kI#uFBaceg;uMhmJnJ_-)^Og`&7_sC07LHZ~{)B-~8c7{-2E-X~%*f%dHBa;Ud7
zY3xP|-GFi$0tX5d3j3&Xo@P8x)WuDI9tM!C7B`J6y}X>EOi|y0wTm}qBJ^;`Bb)P~
zJkh7M$nz}a=^B=;$%nqD2hltM(F8L-3~7XNrB81^-Co}kPCX^a%#_t3vFN=+oOrZ1
zn>ZW#etkGZ2B-7oj!4ujv7N*l4jfMEOwANU8D7g5#TvUBZ92Hv$?^E(b;H!k@Erb6
zHeY&~8xhVn6%=*JUN*&DvO}!%=0ydp5q}8KCg{=88%$%iJ3rPJ-V=%D;RL-!=D~7o
zUqXio#pk`W@`p8-gKF;n49)f;r0Jfp1%!3~etO;?w!S`Ke=bh}Uso^wad~=A*!s0v
z`|8J^m8XDTQRkm*zm=HGtiWZW9QTB+ub%%mVe6;h^g|5$v*5)3V~G79z6%iV|HI%^
ztS;pU<hwNAsO4MGI~hZ+NJJNqsZ@7Y+IKO#&<vWhK}QN}&dI+bQOvkpumym``axX9
z97zztu+`(~KAlG#8MzIP{Ost&#>46UJ{C2HESCyT=0Mb3k{z8iz14O=Z0u@N3L`G*
zVU)<5oLLza%!%70!m$O}j}%AeLFJD$;884juPmLtIqQ<9pMOSDL`!km+3thOkh6(Y
zB1k8Ie~?aCxe3<7U7tj7>-<iS7QbA8Sz?q0cQ8Rr_j6VbN8^pPaENb#t;w1L87^tO
z6i#AJ@5q6e)2!B>`JkJ$(MwSc>exjLYF6#ltQRN>$)+dZ>(nyBG+fL!^m#XMj(x#U
zvsK}+K4$@4e&VPX*HEXcEW{52GcrUBC*CBA%&t3nMJp)G^a2Td>*d-wfYe0m=H`Br
ztCqcnynGWI^^tmV*-g$&+d_BMHkK&~;lLatGL_IARYOswmrhI%PU6Yzx6_vR4Un)V
z%5?K)buC2Q9B^nm1Pemq>osc&!c!;#wi*o|UB{Cc592{1aAM3q5N<vgh!4j*IRsSl
zsS-to^W!{IViVa>U6|<R4w<y+Y#($XU_IuH`p|t%2Z2=r0vm#qvQtFArD})r*zv<&
z81?zKBfc}+3)VCRv_U3tu%Z{38H(=#+{R=h57NkBH$LniD~Yc_ynHF<s=#Wkny}(t
zjuaAc<M?SUnsTC;ed~52?M*nIT(}Aau_*%;OqTv(V5EqXcJ>4bq~l|*C$@G(1zbc)
zTMr=}U3WaT9yQcdt}yy1;jSbsq9djDj9-yed(u|I@!*c-(+v19d;p<ky`*AK6cJDy
zMnN<`h$ysicpb7#&8g=;d`QOl5bz|!4%fR-^R;9{j|P{Au-7EsP1(TV)(9_61+-Ri
z>){9W2NYQZCvpqv#of3}8pf4;3aT!d&`}se%4dgVli?$C(Tw2S)$GD(#D?9(C=V&E
zX)s=;)yi=!T8I{8Iz$Tx$|-$XWC%fkSCLliD1E<?rmZBvBa4I5oAj8(cRM3OZ_#1q
zrps4Z9^adlICMYvFpgE^%1jsMbh$MjlAhTV^vo|@0m*(`2+UayZ8-h)$d!nE6vp_z
zb@l8N6wyxI{9$&_Zlp+1dpeH_97hTAyhQmNKg%cF57G)~maK=JstKTi+R55GK?WHO
z#fZL$l#nU`ogv7QWTuy^pMnVOvT|^{7{EDDyo@6|s=W25AKG6>V;Y}V4aATwy4RQ4
zvIIQSTt*@)-6ieH(3iq-r6D{9#SnlHTv_vhR-;S_xZPKF@0pTO;1a<AEegO#_V6{t
zp$IYJrKb(`6gTcUf@{C#wY_<5E7D+h;@vebfHFpQhB$)sDF^}8yRSkFbd-L{>R?LU
z@+>F>neAC2j0Ii$o?TB$Ujf1f+UvJc#&B9X2__Q&>{&gE;kfCxHZPq;+2obia5%#+
zD8f99kxy-uQsn(Oe6<v?tS33KcBF_Fmj<`Rl(L<x7K33OOIq*b<zX8LNk6C18$u$m
zw3QYUiPh-m^FvDOF6qL^7u;wqaDx#fa@~M~clqXxn@K2bU=oZUJ0$k_Q~Q;7WYCkK
z7N8?TP<{jl^?1NvR>}zxIoiCvEHMFo3O^{w6b~XdR<h2cbti?%^Ugw^u#3fqxF}Z=
zS{>0_OLLa-o%1SD&D&wuTFn`1YgHEaD;Uml9*TkhhLlU$SOJRksea5@-F)n4(YT{W
zW3r*@w$Zr=ZIH*0`*c>~AT}6Z%tK!%V|b8viODOWkgxXNBnt#&wMHJUPmoAUt!mQd
z6BSf%!(_jaId}wkz42^aAs9nL@JPGfkQJ`Dhi`124~9}YSTdlLE8!-#Zq7Ke69d@`
z&)|@u^f9;qC6%Lt+}vT&<6FDVycFvfHPZ^at5e20Rn<ZrkEWDsVRQ<15x_|21Q>cI
zVRv9xUz_gSZfHOwIon(pp6jdHtY3wIU-h2azW^AOYdpV!rpzfg?|Waoh<?Z!=zr=b
z*Q5>SmZ0IG;|Mu=cdYI!N^D7iFLXL#FWzgJh>A#2w7CLp)Mjh~clB_ldq~Kwe>9!_
zZIMX{ppV<SV9fia*=LZf86&!x!t9e)=vk}9d_xg2Dwhg72MgEPta^(96&+QscaM$M
zlwWbF@UzyG1Ua-S&?or9=clIO`mm1V)R)^<x@?FV#h9L0-yEs9?&&w37{Y{LjDywh
z+jM?5o~>_euOEvvO(L=%wo)zfu$I!=);w`{uy1%ThX5jgPK3MY&{%y*U|=MP0x5xR
ze6$a@x_h!V(a_o)Qp~hqv_SSq%kt^MNowlwU=jp%#DbBUY*n_R?bKLm>SIqdzS+zj
zIh}XA^A7|bOLWSXzw}pd(O|234ky?+a1lDl!ypOYMU>~!NLn@>hT^9eYYUEoB9c4n
z?BL0Jq*bjVwwspWJM@68`Pm~avsk<4oZbV#&I1Dr36s_xxgC-$-%;BYOwit7A?MYt
z<pKlrLV02MedsfRTO1>Z#+=snAf_Yl?!4@Nr~vRjVpu8Nw#qhj&^!DL4m=uvja2_9
z@5?GZlm=6mDo-AuU32<txLG<>T!q#dXdo*kPH^8f(4<sj%iYSJd#k@u9mYP_^|A?b
zkXk>3YX~b2iBfN%GjXq^C>uN7c5+B9m82XlZ^Y$o<a{1`D0Dc{-DqBM^GU6T0p;aI
z`lhza^h_@cHW^^;ff4qsV{iVmPczzivoxcp5|1#hF!1EBwkPx62L@f^1<Q7hyPg8H
znLakat@sJ8J^sXU$)agbCqX@tI*i#js%Skj@wr+99SX-M&KiAFCB9{+p<gqKv{#}s
z1M)7=*GkxM&F~{MQv5p<mMU}9c}P(lst1MBQ!8X3-Tu)1M0=g(K7(x)7-d<@0FEc|
z&($DJX8Ywf)G`+!PmmlaPwi~WS%WtH*ZUW)*PX6+hK}leR_ewk8Q;D<rb?OS-Fw;a
z4ldL6WpM0K5%R+#jj?va+_{*5tB#jlEp(IXwowd3)i;PJec8Pi)Y+@Ocp)XUquS8K
z8D?hr^c;wi&GX7!PsM9p44NrEDp;NQtq-iqb>405sci<@T(nrrgHd8OLKhp4*2FhQ
zfsWll>%Wbsz9*k${}B(lKcD@Yll_v<{)No^A(`d(e3t#kKK>UN=Bppyp_Ko;auG=W
z{yPQ#&Gs7%17K#L`vSTA+PPr-iBSCy#%#<$QVGz*Uya$=e@y=W!I%}e$drxko@NcO
z`}41EvjW$Lvfd}S0z`)X)tH3?D18H*>fhb|wE*~Ovj4)Z06?9edq@5er~8Lv2J%(t
z06_Bo?<4=%tNABmAZLXGNXhxl7>I2B>oGI40!4X%{kPwZe=H>aVPwWHR2v|M_q*|b
zygI*47$XY@9S5+q`pp>d<1zmqfxdszy7{wW3b=sw?+N@~OtAvb(;pU7o$5Qbv+QVh
z87+c|a>v;Ql#0uS_D_fscd)c~Nv*x6!5wb1G%AXVY3gDhc3(^_AOQI?q_2DTz@XnU
zW-@mqFK~-1*sduY%y%wqV5{*3^ube&C<lG?+G0dSC3v2poWju7=)OIF1K2T|H94bM
z6ukCZEInLEWOyr$rEb`rI1wJytjVIg;rw83i2V$m8OWFMPj4(rpdH$^ACNVzlsh4$
z|2Rb7tP?==;7m}iGr$4SPgvM;jg}4jA~Qd+eJ+ZC{9)j^$y!%fd$c^3K*Qla9k15#
z*veAOAx(dV83y`u=&mtX5>o*j#qFnaFN}3c<gupngYcXYwio#)t*+{C&#w3P(lyza
zQQ%+J?P96s@*J=1A+nh5zBf%-N-oeXt(ZiUH!yof6ppK}(+V*h{^+uI#DN_a8Z2^w
z6Vy^z@={_%c{=p=eWfoIU%%vRv1_ijLftbl_Qt4XMp&0rDx~E$DAEk|+Ui@UD8B0S
zndBhua+c$2`}!=WfV^OIs8FZT`L$H-m_*&YA<!VKEQuPXgH}krF%}4J+yFOFy*P$0
z0V&i^a19>nm?F>p_C`EFH&A<Uugz0x+*KYu$EPlu)O+FhRNJ~(HKE}p`!wTX0=9eV
z+vL5Poq>JJ3YQw^gis!dx3Bi?QhJ`ar<q+^Lg>d~k;MA*K=(H@n;q!E<pl~oFZT^3
z0b#0qM7APwA%FnW#9V=Mh2*wqp{7Yp_+EWDZE^VR%z6M3qzhx2vrg-yt0wwXHmXpn
zG*MIqF@K&Qd0Da@p5+CWyp2V)_M-wfRE;d$ehM@96=}5@N7B>WgMr!t&duf6Jp_|&
zs~CW~JxH?b1fov9xcI~q2`@znz=TOE_Dp>2a*{Y^v#>-f`SwabukF;Ly59}FR6~~z
zL?8T$(HV;>Q}CpugB<^G@I+->Y+P^{uQHOj8AyIYDTV&a-OF)lsV4@VOOmt9w>#%1
zKDS5nvsX$-9B#2F%TdAa4f%1)t{2`B&iZwazDtO?L<z>of<e<<p^1y1q~z%<i&G_k
z$uP<o8NjQK8Z2xg+$zbx*apP}^*N`b2a?G=c~`RR;?ZMG?Cx4*q{Tx#DlWh29;q;5
z9v4D77)+iIpV=VAMTm3=Z@b)Fqu0+cc-lZvy$?TsPF*cIc2L+I-R)cYWZ^Syf~am=
zqZEux3c2?&NbWXD3NjkY@GeZ}A;#<Wu;}Pdx`f)*k8G4%OvBD4qG5>fDLJs1cqv|C
zmJ4*jK6+$1HR;>FM%q9|FQ+I^|0cKWiD|0U!)>|+UO|KaEm!wP1C=Gy9FHFw4j98(
zfo}<5wQ<1kq6%}2iB{LHAKP0Fx3qSm$b;@Y_P<GZ+qqmZi=zZ&eWzQBf$23@7ISeU
z+c*$ogfqZBiZ2Uxp1kZQOUS~Fq_M>07C@56?i>LWj1XJ^R6;0p_qH=0B@m;Sprm&&
z8ChZ`STQk(F!#=>BG8+DW|Rum%gCupR{m@Y^)awN5h^SRmO#Eh5x2#!Tieu8kD-F&
zG<}d_+Sk+Kb4m`kMlgLO4GM`;L81g4@%6i+)#U3}gQ`^pGfDDLTIkQD1q&RMd+%uJ
zWY@h#jG{yrp#{1=T<FQg#}`=gW12ib45)d|Rmv)7$R!V{LSeyPTna&XN{P)Wxp?@{
zKlG9vH$5y80ZnZjeajD((;+3B-`K_<uPX?<?lW1lU`S_{7h>ccLJRWyx+bcU8n!;3
zl_g>wNY<ytVO)>RWCZ2!dJraW7$@%n2U?bHV^L*wJ3g2Aqr2T8bNHG2QD?0@No?`m
zBqvroEG>ejEB*x4KW#TA(^Px?Sly~7F%FWwR=uZ7fS*V6DYV{VTNR9F&Ky1^H3G>3
zy_lsnh5rXjwG>mN-D>Bm8!In(&zq_`KB1x^FwMABv_17m+i(@3{GvL7*|){W9jAj4
zq*D<^D5GzBA~6>kC|BnpC+!(gS?6h)UO!ROLkffD9Uw*$S$Sw+J(jUs@vtJO@dK{7
z9x<b|iz=))KKR+8zQQy-j#gEhP-us;9)kLTzD#-X)68}>(|}%rj^Gqn!SDx9oEW_=
zRX)QreD+!PBc?P0*%Hy0Ch7e=ycW0ukBhM{9wsqKUegJo1_`6O`+lj=fJBgIU8Ckk
zLN~s(S`ATPgOj$zT$scqR{!jkqxW1oR?fa;N@R3!U(FakLEr&;qi3c(`T@GvWjk2|
z_9LdVS}b2?P-J=(KVc0Q@?wVr*E_4B#*YGY*&|Yf2k#ECG8&+{Jw1p&S$Z`z)P3$Q
z-Ool}I2jHRYN5`$zKyMkE5IfTdTYMVd|866i8RHW$7K+Eb5cm{0cMzLA@Hu8w~Z2n
z3;kwn(UZE_CxT$p&LpJ3g3rzkVC5OvQ&P;f*bP6ctxO)!+&-8bj2M9t=>k%^uc|W3
zwy-L*jF$Mq27@s28L_)Kk-QgO#Y(>xm!QVQ4U6hKVU6g}o@Y+;)w2gBCFeIh5?B%%
zRjS6X3f1CDK(O#%c`xsOTucfVF}!T)-jMB!a5%zVqp_~^q^0pMxolS$AmeLlxb3~f
z4rh3B7(JyDg6-DuN{z)O<xQ0WK9`$*CvC`H$u^C>CjlDvwkGyK1ccO0s*7lAlUaZs
zMryj4)$!fwGxgfnCELttuNhn`Weh1I8GMII7|&z&kEu9`8gZlQ;b0cQswVZgsB_*(
zdX<!Qs6J*lE^&EL_ejvBa#hPk;zT5s&xkb8rLE~&_!0irYQ7M%;w~P+t7YDzR{PZ6
zx9#EsIoQn>eZ0zKqPfvwmBtm1HD<5uET5(py7gG(^kg5j4Jc?=-&I~ogLEHkCI=^B
zcDiAhi#Le5bn;?Jr?uXd<7wccc<H*zFHK*A79~Nw+qDcg*0#3V+3&C(COPYHIdw~^
zvQ)EXF#_Sh<>Z|TMn>eVuwTF%9XEo(*#=3^Qd<P^buVP{eJc{BvhG+2BwNl<;#jOT
zyXnDO?L4_a)q0HG#@ddwojo}zG<@4hJum;Hj;Tl+k8tFJ@Uy%zTNtkioToTE{wk^_
zZm>M6`VLQ?%jk4cHmjXqY$%Y(%a7zy;?J>v#2sZ8=k5+09CKw6@RTfOjXNXVU)Ygb
zdCPkif#?{Awp<5B9#BZZG*zx2g67Fc#u^QMD}{#m8IuW;k@bN$a+|A@$Xk>cD&t_^
z$F{8@FZZp_HeLxkGs104_R&XJroJr{fRv}gumO!nZkKCLlCH6XAHjbsV_Npu@`E8}
z>!vTG&eO9xPCXg}?oZYFU{ul7tl{lZZif_%($<LJah9_OEwSc_0p^Lqa;tMGE@#oN
zC*k$YlvpE^&QN;fvi&sBQG$?TWz~Ht(F;lQQ$c2<rSTjdrAPAZ1==-)4@STRiz2l!
za*1?51-08Zo4BV!1Tw4s&LZ!x5WgmxKUatx|Eos*56c|@-+Jq89A8@}L~I;iYA;3L
z-(Sk8UzR_7eed@s#WxrK*UKN6fsGJ`FM6jyol^#8B4!|$pB1PE3IHy6W@TWzcm0F7
ziLI>@P+rxTj*;otB@kcTzt^?<v{!*LtUw{w@7k+O3_wj&po9+-usOrQ0n}b)`=Y^0
z#Kgu5EKga0Z~IHlmHvyEE4=_W_gAe~dV#NcLG()WDvA<cz9`L{oa{L1>0Mo2>GTct
zjclE4ZLKWLo#<>GOsRmC#1HI<uakVQw|^iU`^EXcg=1NO!lS?y*k4v$GXR$}uy8QY
zu>;E`CMKZ#Dm&A?^Jr)eoI-J1XGcqYCI$u$qu=K8#|rwtJ<UIR#|#u?<p5UXjI2PR
zA>dMUW;S-9w@gI96)#_?mjCV^<F6XB0H74<-}lLviu3D@^G7W@#^2U<{ABwd8KeN<
z#rhw{{bmcid+3;0e=pF1SNR_p1I3sa8GbL50l?ei-$!O)rUR;S{aPgh7=DFP?m1#M
zp!P7+JzN^VSB=;2m+5a){q2xh0UUHJK;@?2NB$K`xo2cn)-NOf4&nfS+Fk#8$gDut
zEEDi{{e9#gpQQibHVg2=WMjW?vVSb*|H&9Ae#~%>HUi^M%Vz$;_{)=y9jLkctJ`c}
zALZX?`yT*}pZH1tfyVTcUN7+eWBMk+_N%V%Pfg@6*B<j%MGN4~B+AGH1Vz4TV|>%~
z{a3;;04PHC_c{9u24Uy;tEEiz@0z_4DpHO>!c+5(nw^KAi6jI)+A5=SoN}^Bn)0y`
zQ#CRRY;V#rvC=E8iz(M<<RT+EEF7TlyL2us^S7;j+O2-bVGC?wfCG0lJ5i;CW4%hm
z(ResI@buOPVR}jsHDq~I+JQ`ytLu7+Y2yuRx9cmZ7ZP4JxKs~za6WmYzP>%1J!@d4
z9?Fytt#BImte(#)9=Um)dNy%(umndYsygI!WK-#;VJB|_nrWcKA`?ecidAz#L%Fzb
z*qR@ZV_K>X8S2;<;OC9|23r?v*SS4Za&0sfO%+u}%S))eKiaWNtKG1gn+GrkYs_?O
zQypgjG4$$lol{#vxnsT{9kCieD+cNdP63LzY;iFmo}%X_%AKz@ru{t`h;iv|a>d1G
z7Y$?*uLj|D2vV#!RyT5oA#4Zq64st_am*q@&vxnN$X~oLx6XKxlUe~JFXfC|F|L>_
z%vdP*rq)}gjy@+05Nag`p%4z&N4DpU&vSsK6t(iU>dPq##)kLou4O4M;7Wa=uev|r
z58p7DC1JfE&(0|4y-tFwHP`34X^i=}Vq1P`#GKIX4kX&bKsv%eKQa#@>ytiXA-^*H
z<{M9z+5&0JYvJi=zb=flmVl(mI&3g|6t6OWf3Ez4531hDh<a`6s@S!4E@m@N<<!!=
z%u;>s`s?T!uQVpo&EY+J_L|?GDJ$33GwmaJ*A|*H!R*YXTpO&Wcc{7}u+~`=^2DRt
zNA==|O>gh$GLYExA2IC6U*uJbiBG#wq_K3t?GSch<vrwgS>GGNBvml-7q^JLC@!La
z`xqlfKmbCKC@Kt=E_`H;_LLiM$&Asm`VBYBVPt$WJ@d|u2z5SGhDOjLTR&o`(t*D$
zU(CBSG<O;^8hXcNZc6e3EQ*94q)SO%>^xds6gRicepBwCEiki9@_Javu!3-76lxh~
zjEI1)nJ66E5Z`>CgyChWfOG3)xa0Hpaz*bGTS00eWtP>n&YvdnyHCT$i6Mthu1H_u
zPQYS7b=7p(d}yCcgb~Gl(XehXW(Pv`%n52?Awj}{;F3EsxTD)8bS0tkMz_t~&kGJ!
zIU)*%v*EOD_po+gTRX}K_L($d4i6U+4HRJ5R_p|MG)vFV0`t8XDlBN94&W?nw;?<E
z1e`LBIqx8Htoz~`dHrxtBuyys)kArLP}JUJRIlbcP@z-g(BCssU@G>*4SjD$`t6?o
zDI*0ieoY|%TMo_-87c6Q@g?&F05h`RGSII-`~HmlKc*(kz?H4QF@RAH7)FSgf!I3m
z(}C^-6O=DW-#vQ!U-Bklb7M0g&ceWG@JrhC&ENk|z5ydLBXA;Ffol6Kz^og%IE0P?
z_<UsricJF5H2>K<X<HKmV+T{9f4{EA1pwEV{QY<a#-?wl_m5gZjKDt2f3W?l74#1)
zZ+>M0|2*zD+b^9UCg7^wUt{98i|PBE{zFXsDGdG;Ab(y2``?nJ0AFL~_rd=PXuvkc
zU$1`;Xw35BY7Fvr8%+E;>4LSV^9ZyU2#BXNh%|_pf}<GWG}5QiGKiQo(q{<aw3vd5
z7%y3+WmpV_8W-!egla(|qETe;xhcP@^?!Hj{+sf<-0I(aZU8Wt1Mj8(_NM&1VgdZL
zrNo!3=liY!|Gsm>x52-=eD6v8qdp7ZtLp6c&-v?o05463Zzts6&qsusdfY4<g3p5P
zQcWK-V~5bo9^R_D6sJdVCN2xH+-AlfOh~07BY8f1gZjlrnT@5Vt4k4q`Vsmfqv%>;
zdmT+-MzxgMl;||0n-Fu1X_$ni4z_DNdahw+txh-EIp}dj?%wnmsmU_&aqbD3u1~{N
z>RZKxRIMy5-8W2aO!^W_5e<$ijv$aR(HXM5<EbVEZln8Q9|5eeVIC3d^185j;o;D1
zH~ytwO}W~c36EnQdbD6jD7iZMlwkWY_wPCR^t}q%SlIP~F6?`>wp!2Bt4}2}%I4~`
zx9lLm=UU?Zinc~7nWESg&*RK_fvq1izc`4g9`WWTFySVh)OH{lz%)TK#o>n8cx8Wi
zx^XtU!_lGyiDI(SRWftBxy<wVu^&Yw5?Ih}wMoB9p~x{Y=S47A$}^o%yPCxz19DnZ
z^+(u`L9=?&-gIbd?bLrP79g!TjBI&?%L(DJ{-BFrc*4=<+3R5Ym8Jt%quG~XP+C@7
zub}Xoqq;|pPqr!o>EI<9rSIx1C<M?UUa@KDD1|CSwl;WfYR_LK5!yMDtCx&A!m#C#
zn~N-aUgLZ3YE`*|cCk}rKBTQFu{!sJ4tb!%czUVXE+I4#m1m#|HzfBA6^lsvEO_8S
zWMXf}%H%{PgNDf~j+t<GH6%M1LMf>C*L}=7HVl|qNnZ13A}Z-LLSD#>u!<eZKF|0!
z2oFQeD*_E1o~UWhTx$*w#zo_lVGylvzuE<gscH!v!*=R*pFJ-XS9!Mm2*RDlAqGOM
za+P$e7D^IP0L$xLI!4qC5`76BF+rz`6J<PJ4>D*>o+GJgK<ZXfCitpLFyFZUOD)T-
zI%iMGvIBcmgrU_2U(?*z0SAE{+2%KV2FsOs^Ftysb{*kKq3JTGFhd2(QgfBD?Mx^F
zc}y9ivHiG<u%^eW&MHp$3iF-_A%uowSW<e!)pI<&0v=3;brB$1hV5p_Rxth8I7;5q
z#>KJTOFP>!F8ZpV8ZeA>)`Bt1?H``534$t(^U%K7OyBXzH0Ff8kfZ>+`6M?xr)tBO
zI*;4RVtf(CVfmnyF|k^QR_(#@^|e~{b9tI7B?JStI+hkiHe+1C2IkS$@C`-1WFKx#
zcdH=D#;}G~kU8}@ta%%JHJDrUa5jno2B;lM4;_ZO8GD&W%gI_~f?PdnF`_mlTXt|E
zK!n2r|1nY&t-xmt)5ir#s>>X&yglj`NG$WT1mfSh8(Rl*o;JTc&gS8vi-5CxMeV*I
zjo9Bg3Z((Yfa1?=kjPe$n(+z(OPu6Mk7K;CLL<k1YJdGh6UbDa(iV%XR^1CVHx7)o
z3!*vEZkwPC6a^APt=W?_j*oCXWXjt<uZEq8Xff55sU9$9os$qh@9T2jqz>xi8Qvah
zNC-D5hIU&zHJ5Gp<n8JBoH$Si{E3;K`@=4>N|Z~7r)Mjn#&zS5cG=w#q$XnxeCd01
ze5Q*!Q`fhdKSe-?PpaiyO@!Psu<BqCm9)PB;Q)_O2pHvR!55A8-8F$FQ_`E@4dbJc
zs-%iHel<Yq!l92XJWcAYf~dWK#d6?_vJrblhd3T@GQ-^9)Rmm}@H5FOz$*M?$!`}_
zD_n5-eUi)>dpZdvyHJ4CKvwKJzv<R{Y4#1IVd~5z`tT;bx>mtTZmb6|HU-Y--jx&r
zl;vai8&o*LFoqk;*BCPLer8?vAn)5pU<m2DEVT-W7K+%TL8~a~=%6Yx^Nn+*Gscmq
zA{${z;;ig02|YQjwo<ku1Nq#jUJeLJh@c;}3&}vj#u5rW6GAo$pzk$ta+}5S{J=D*
zdg-v&6+)<!l_+Q5oW^aFp-0umKCt*+!n)%P7Ipz`n*ia{sgg6-Y!y7bXfwv}OSHfz
z)2MzZ%F!xibz&+>NL%{tEXYZLRc9ZdPle)pE`5bGL*m(rFBx9*DdISq;mL2F7*;V|
zvoNH9<t!cJ^AK%4Z)#3G=T8?{S6K;s5Qg<Y4_bd%tIMJ-_jRGjgQ0+w?Tf@XFrQq4
z4NNOFHXqnF*xTXBI5X&^i*j1(OZEwDM<EbZ*mNr`(9rQh)(Ol{yd(ADoVUu?gw3SO
zyenMUhD~i9B26I=Urx2xS)W?wRm*#&kvmm@8y<0KhU<fIk*bWEDqHNjw=Z<|k%?H@
zh%9)dsAg5-x5$s4z6#ZYwE8rVz(SMh!=iq<k0G8Ry^f2YY;4Lo3~~o9Od%6?PaOZM
zfc`z0-5<xltE~SJ$C<z8#D5yc0bhm7fnooDgx3Lo?ZyKDN_l)uvVq9Wx4*x^zyM$-
z{Pp{<8z%h>{{p^l*7UQ>f9>}3KM?6(yZOM;tC<@Cx9s8IfTI^THa9hM0!p#70{6i(
zcXE_Bb`Y|)wzIYQA`Q<DNB>tK2QY8|zY(&vvUO0h(>DZer6p|aVs2=xC?@!KWEU1v
z{%y0LyXm_{5EMyK2~iMGP*4zY;4hH7SrDNw-~IRk`})n7Kf%7gz`-HFzr4V}zWf8?
z%L@z~5)uLe_!|P^%ZL2+0YED-2yo!L{_EFWD+nqKNC$`^I4B7S7%C_@D(Kxi5JC`8
z5OAQgVBnx2f4!h#VBw&^AwVJF!GLezLIDARga8490fPm)^yN+95Rg#NAfR9{u&8M0
z7&uSVlP29JFtLc)ITCAX$KkL^NEroinOs?b#(3X8`F}q8i!~%D1QZxF@P~A$AYfo1
zkf2bIkl)_`3Oq1KC{&>62O~3^eK|BbiDRXL{xiRr!@Cb42*A<7P{C0__&|2$94;75
zRT5c8LVKpn2~Ekh(_>UggD2S{(g)d?V*Dv-MP8>Q`@`9glbsIx!Lou1>z?Cw(;UN1
z>swX!BQOPU9ooK+ot*F}oV)|^nd)rh3x<ZY4OKThU!hTbGmNXY;P2y=P+c^kT61%r
zehJv<wX|qbv3%NMAsk(s!cQZnl@^fFGXtw#3|WF>CC9W&Q96VxN*1IX4DGwUwQcdP
zt)Dqiu^k;0L+Yhp7BTX1>kv^$p#I9mDlZv|C&&3%l)7}tj={voXhpW(Y|0%PSqoEJ
zqyf8hd7{wD|Hs{1#>KI1?Y@mW!D%G8JB<f-cXxM(;1b;3J-E9|fCRTdaCZyAJy>!(
zYrp&5S!?fg&U?<ipY9iaRdjXrs5whU&G~=Evx*QsJwhHtg+D@6ySygH7^Zc7q!SSR
zr;ulRVV@ru?KLZF9PdvJt#I68S`8>H<u9&|(viI%7|hg6$Kb=W>0u;EJZE&WyrYai
z260LqK@BJ*vdeuDj?w$X8a##U(sx(a#3NT7>ltCj(<9ANs2mRDMCQ%4P*J_oP=U_W
z)g^U-Zslqfu^jseu;A?F{elJc`|tiKqWK>g78YW_6-Qp;AuL()j(rO~P->Qmzl`WX
z>ks!LQ1+(-BCeK8I1Y}zXBeEH03+f3Jdc*s*i)4jh*ZDdn%MgHwr^1>pWE(5Zzkm;
zT%6d^&q&`Z&PXJ4aH9IC6ctjd-M*=>=}bnM!IP=?=o!URTDbrW48uG0!D%_Q-K5tE
z_)9-;rw-`Ywhr6+e>kB=tqGWaZVjE;HJ1dDZ&*Z9gbmKE%C#YFEt`t#GMSTqx((3{
zXZa}iX+WS}=+yUgvKbaVtH?e&GQ-tZXs4-?Gf4@`g#+hGo;}L!Bi??3SBP0;ejmHq
z2xJ%wR4NE)3SfZ@qXJrnX{`#hg<k@;YA$6N{MXE*pfs#yl<?&Le9A4TI=<S0_@uj&
zruEW3j$OqL3oWXvQ_*h#<#TM8UAbz{Fv)s0AfSOK4=(zr?-KdC&{1G-e^{H#)_zhb
zU__HQ$~Nl&Yc@4LnO$qgc><;t!b0z3OS~|9qpzXovGHE!ti`Q)t!_v^ASw9TU|F6}
zXDAK3I1_KgIEXCxb5=dTlnaS5nK@D<5Q7`dGdLq-6}fMisQ5_8X!bCi-mgT!2ALPw
zu=z@A24Y4(#E;bw!@2~{RhFm3L@I1uJ{;+@%=S)>v)#^Un`uHjHB>7XyV!E$Ah<}~
z8gg#Z7HMu)Q8ceYi=^cW=1aT1lwB=Z0TLWvhT?MKl^c4aJ|nA}wNQdkH<2mNN6k0l
zW&vX+uyP#ePCS8Z`R;MkM?x-@ZOF}fO(GamuTG6}cGew8DZhOheW~^^AWGP=&3p`O
zW*rYPpH{*i!D08({;))H1Uhutf?v|Yq`2bC=M=vhAr^!XW|O}gO)GtX@WOOb;i41w
zm~d>3f5ii+E<84cFTBQz)a3i&^$kf{J|aKF7`{+O8|#_}6H6@sksQ{U?@NGNsti=>
zy3uwxK5bjOJ6byX-ouu@FV=#*y^C)+UL_(_mJt;+6~S9tWK3fhmSed=3o!%=m}oOU
zL4cx(B7i}Z0Qaf}HYRE|#jRkqfu2v3RMl1wt)-RSmG72V+pI%=ru9KmIa!`Jfk;wC
zJGw7XTW(Y@U?)w`#UdD8lcDLzkooc$lmhGHRBa)pyb&rCqs^MoLCnNH-mdh8VDg4#
z6@(Yojykx87galO0_u+DPDNvgC~d?(w>v2<^r`+x%ABDk{?z|Hz)9nKNt&PoXwc@f
zIp@U>5<+lYT=V$Vm<A!6&$Kd-u?_~A)u%ECodTHznB>QVRn;nV1M&EytWDyj4=PON
zUggabs!YbL16lc5G$N&S<)J_Nmd(RNR{5CYJh$^$!ku3pu1i+xWI8ub=;p!ay5`zD
zf8Qhbs+rohC&b+0`@_j}n{K+}clA~3v_j^Fzi)n_Qrhg`%LN=s+C7d<o2LR59)Oy|
zPL<9oY8E>v=1j=Tb1@dWP#3x{q1)42I$qjZYJLLT8aX}>--_7hI}yJUJV_#JH+!FL
zbqf)sRZexHt%);6Wxk*{^lA7#Utf=BA=Rtcnz8M6X4|Vnec05I)49%gm_+rSdu?EA
zr=;H@^DzwVJ7~?7ic*c9i0T~deZR9E=Q<GmgKD(H@DK)tJrl8g7tqQH8u5Y#fkV$(
zTUH%@CYi!iE6A3OPW%FLM+?nG1_;^)7y<yVOuORGr*fI*z16{w8DmZ0`<bIw;-x%N
zkx$1_npESfRIF)~pRi;hCE-9sP&I9GNU7&{`w7O3)G*=nH=%lco0cy>0aA}A0xZ_v
z&8_VN+x4mC#<emlGuxB`6zN}6=4SM9hI$NrdXY<*Zu@@%x{<5S1bYSEcBvm}>b`_3
z>^@x|%C;D!y7expSEkgZXP2tq6K}qMqhEPbRr8~-uAe?!ZqYAp;U#QIfvgLn8PDdr
zWau`|wCw{MhQw(!ZHKl44o?$RET=AQS;)7M_DRbCuh;}XBjb(lhN(6+G%&O>5HczO
zDk=Hcz!9e$Hm0aJzB=?Fkmu=%<RHKrB_!n*8+FLLXV%1v5TF}u?48i*XDC@VZ#?y0
zVy*a_TB@!+rp7Ea9@9a#;)?{nyxoM;Hf`;j8>(_P;y$5^k@#lDlx$?uZrA89SMEjd
zsXS7}vurdaF6;S(k?9P2045^$cG3VM;9KW@0$tN%j&`Y+kuUz_hjoh`pytxL7W@0n
zil$e&T#R)cjH^D(nM^n@Bhjcyr36gTODZX~Z!^G=W0m}N7;L&B@oTYgI|AM>KFRbl
zwtwBDn`CAaaj(Z-A~q2;KW=>qt(sV2N(rEZR){`viPLV!wt<n$uIwiu`N*G(%EdP&
zQ>xmcwOQDvTs~zeCF9Jh1P3sE=X40OCy!img*MWMkmhCSbLj7TTuZl?9lp4}41NH3
zzqlvSAlO#zm>wJpK58xYn$G^VycoZ{Y$9PzC!!3^mTk_dvE=bE<U&7qkir&NM_61`
zP~NV&7$yhr#KvbtHu^n{MzIbK0)-|pp-_utaG0(Gq>#;65h#!~gB&rJDePg(kWMF(
zBwo~b;=v}q^vQAAO3iI<=75dNGq7>jy>JNdu|04v%Lu&zzb}M|L|>pTGCsQ+T;Rdf
z;3Zz+_Z98w+OGUF54n}c&yJ@#wCy7**}~>F77l}UAv=@&-Y7hcplS2q%1Am1BxpT#
zG}%JE!U*9gny~;g9QSYHeRupt1LEmqZRCR^n})6SH%0W#u*u|iE~?Va3CB*&GAWXT
zvyDqM%-9T&?&LM8Bq*_{=tV<NID4{$kW0V`1Vozf`&v}<z}JI>s%5P{3{8=4OOR+f
z+ptVq_|che(i5GQ&3Tq_Cik4>wZ#tGYqDY3pY>Rr;0d9S5D0#2-FZft9lksl$y99J
z#DOl5@CX_bs++u8G_V+?88@TxqgCJOIs_XVQo<EWu;63gD1|6{Nu2dzO(fsrhX*Of
z*Bj(nIaAKbxFt{7%X{C0b%2-nGM=>$2<&@8{fRCUq>4FIqG)}etlwT#B2q;Kp!*8<
zlQQP^eUx_WS(Bd~*rC`rTTRJ6x9?tk*X5fbElyLrGji)I=eutC6CjI!f8y=-`${nE
z4iquoV{_6%c-j3#*oT;Vb(P5MSTnNtA*sZbBz)casQIaCYz2TzUpr<K9LXdi!~{&=
zz67iD*p6URU>i?xJyTe}bLXeD6&MOBkEaLBH)c3jCT#v1HYU~#8Do)JfPAB`IPl`l
z(Q7a&0}>M)0C9L$k@4#Cm?SvOf#J^!?`|6E{D*5KwvIVJXQTA*P^NHFNwP9cz!qDp
zCw)u&%z>^An=8z2K0GT<;f#@lh^+v{r<yR^*BuQJ+Ssl+&Xnh6nWwf>b_UfOrl$E5
zut~kSMpz<bhYh~5=p{QnK6TOis!SQy&WO7;>6so8^$E#&%8x8~PohKdPoy_DwawTM
z%cVta>MI-{A|QX{_hOycqVY2lg=XUCv!D5>2WsOrw_QUruCG^qh$;5&ne^GmldYfK
zd<w@BycQZ<NlAIDw)=Gq8heI`$0tutNl}$gX;f(<4fiE53Ozs^nw7QG)|;#0YWzyU
z^Mq))e#4Bo9_bF-j6DloCfd9Xkm+4ST8OVJ3g{axm~&ylQ1HrNdizlWaW^{BuCE~&
z&(h6j$!0223+g(t>v_zhe*Qg1J=`I=$3qHq-m>+Jz7f~bdT*;A^c2nPqiW-w|NOEv
ztYWj<Yral?rf|iA*<L2!PH?ki72Ch?q?m9XG$KJy*rhCEK+HFvDmpQYI%_Y=(n(4v
z(u4<^+fpR!(3l0z985cykz8_fYeN^BLA>$!76W7YcR|2V+h06ol7X0twb;y8ttv3y
z;s+y0?T$FrI_y8g%Ak)KU3Z|ljEe&x#J-%h1Ul0gK}5m%j$aSR3HOi@eqheR%MStD
z%o6g5*(xAhO?b}b8edRG3_#B~^D59_xAvrvere>NrCWfJqGVyCqjmMd-Gk47q@r@L
zX6%`V0l2WJT9EL48W`C-Pe}R(;j;<ah1WCCI<Q&n;hbPNjdO34<keX=^=;_kF+9Eh
zSL3CAbINsf%36^Pdym2~9Wtw12phs40UtX#=;db~H&VexHp&>(z8JaH9N{1UvJiJ>
zU1Tt4WC>VkFgYRt$x??0cI9xpLII<Aeq`vQ^A}TB^e)dc>MK3|1l&e<S>z&BbSusq
z77BE_1+g?ezAIUUImziMb~)h-KfHPW6HxXdc$)Y(O`5ug`J%W6_YS$d?kAu}vA3N^
zrX1G?Mf;K-;P1lqE>p8BK%`OU9!bmG8AY1k9Noe`cRq0eLjkrhtx|2^Cx8Z7Z`2fG
zV>=41;4sV+A@n>>>(F-PzU9;#$_)J4fLLwgEnjMDBZEZ$CcfHc9C0{QRb5>*5Wj3L
zWg-KURFcKWcVv?l29xi4ij@uQKfqAuvvJ>juHljbqkcwREP!&WIXf=7fAib&txbiH
zy1|aO=+>sXA78V-p*pVL&MDfgAJ<ZMnqXO0YG8XUNT7Ow$;V&?ho*FkBaZfSBblq>
zPr%$v#lXycFa1^hBKXg~&ZyqaJ7uco^SpiA7|EJPeZ+VglDd&yL5qE-;-Q>(QQB`o
z?XW>9B(ij7XI5-sYB4!u`;UG~5ipOM!_`BJUvI(;13q(bB0*)XBi1)IGL34}^dX`@
zmgW@CC@3qiu3u9nt#?N_W9GgcQ_>xG;5<yG`w1w?SZJ^_lWoAi<Q7t5Pp2A;2VG^$
zkCKVW51ST9ckaAuB&5J=qPQktQB8r@(!Hr0M{4gMCcbQ`iPz#ExC#H%Q~Bs+_1x2*
z{Nw#P;*M$UQ!2W>w?$VLYYl3sWDFGBo2c<>rE3W8sLo__N4{jH7IO$EDHkpXVtRm<
zT#QL<>iCi!3=-c@Kz@sRFTRtu0)hUHvo^==vihikOvjLZG*4_ox_Qq;m$Iw%$&VsK
zrqraq5)<oMo^PGln(cWB-?`=u6&Z=sXMbCsuPpJ+*`TX)<|n;ytnA6t5oE3cjF_<Q
za`u^$aa**I-<eSB->``%Y*5YFQMe~VX0t9!VI#9Z9!s)tFW<1V$q9!{K3|R9_g4Oq
zozQ|$9hDTwloE)nH46VdD?#+@#pAV19aZg`KFKqQ;7ekD|L)P$5)PkAzRwviPrVco
z<rk!1yF+PZ3Mv1YA;nQ-#*u}osxDX{sBlLq&rj*I(rQTiQn9M4O)T++cDU^35;8#J
zEE%Wy8Q}Kl6FU0q)%eWuY&J>xn%UHqTf|S$AgRk#c0aL)4@p!$hFLxL<^HxD(4tmZ
z_1ng@zb(_iL;X21!|>EOqbg!+-baU7Z(iA(*L!7Tam$LH<LYCMq6Qkn^o__D8WJDk
zjYxwZRTY;DqCB0(!X!lqz3S(4=?U{VnIXo*ulj~pbdA_OPh>IS|FW4Rrsj!uV=!1*
zqHge#CAv$l?kCFbZ>;d!vYJ@QUp_UcNo;-7K*ckQhdwlJ`y6nsd)yOd)kr0=TQga=
z935phY)+!K6&yVe&~tA3YV03i6q);A8hdU;{iu=xE8q+?Vcq+@v^6)9xLhL*k<r%N
zm_Eyh+vS;EQ(w2-a-hO#*n-9m3_zMk)q|)%+V&3!2+!>>kFB$9T2qwrCHPiXmKHz$
zt^6-z2QZi%HP4`kUg<$gBYjM28XD||8T5|wRzap0K9r00Rv?yd)Y68hA+pO<S%gQM
zB*Er;dd)|_V@u?v&XN0MBS!4F>K&f9cQ2dHrVvHuv%nj#0L7NnSG5@Hd^}skvdq`R
zCr|cTKC~eGglEcjUg;FKs@2b>!VXa9MwC2+8PRW62P}}B$)yh9v=n4jxrK(1WF~IS
zo|Dm|_pE!Y)NYz*N*W0W$JYiA!&Y_Lua;^VT+)s?Ov;K=UyTcqRc^RiCwQ+g2FVOF
znuiU}U`_!zqS~gAjaPiXl!&&%&Ye^R+@q<qek5VKUex|>HT1C*FVXEu0!X=VMn%)#
zZIXthhV2t<bcHkM!mh@P^rgPQkg4vyM4$0gI<M<3yti?F?b{@z({80XI7EyJ9|Bn&
z(V$G5Q95D6+-h1^QuGm47V!OS9J$W1>4$B$R!7fHLWw?#^~Y({ZhJM7rSyBAmU2e>
z{6s~Pez=|Da0*{___uCb*$T?BXmSMDST}qpVnQLZG>VlR5!|6^9^G{?_N}>;wrjBc
zDaJ2)Z!ckXOrBgm5UgwrQj?2yZY;3E@~`8yW(c)oj0ccB5XDAUuYDgCm(|zyyvQs?
z)Fnj%Lg~Uz=Mpf$mm*sqlmqktjX>uiu=GbLIE=GL+cHm+bZlv5q5ItDC9S3Err&z=
zRU^3Ip#+De0>BD3_BKXR>q{^N)spqPqV?$mQ3q4u;A7q0?fq22m!ANpbmrDZ;MuIb
z@R9@lvL@9K{JH(f!Su*V()MgW>#8tE4KX=&Bt?Ge=hs@5P71Oei431%q|L;k4y2+3
zWWGaZi4qD;;N#LstKR5;`i5yse+nQ#06K8NL^5O3<Vz4JBM_ksmO(>zW>kJrk)?pF
zAKg2TVBDwYmZy!NoevV(qGhOaNrx)0{p|^)@Y}N1J*z|MJut5|cN#k>X@>l>)~ZF?
z2Qrq=*^6m$HC@RHDJBG}kIy~dT+sb-bab2AZONm815J#V#;CYcG|@zXhu6&q^50Tk
zKMs_m9`oX%te5RDHtM*SPuq*2cg!Rvi!f;9#<Z#H4o#5)*|MWPikYikb61k?twmRD
z^36blU?zZ2hCY!;uY&#5Z?2{v{46;?+gfz?=SX~K=`f)|#g1CDyc5wC;oFKM8=!i#
zJVR#NTxcX>l1-sT>Iy|Fi-SqhIQHETw7u`++E1`@b#anPu^g?os!$QS9DOGyoVpJT
z^}4=+fbA$NMSm{6HbK+i%o8Jt1XyizF0;Y1P@z#iUWT;`g@#_f0%Cf;LqkQ}#A+Nq
z9n#9@umIoDQ&#^nq$7xKAu(*=yggOFa$j@Soj<HuHe9tJ5x0y%&&1{W0^E-@IB^M+
z$#SJHO~kC?+ojGk-r^K-Gmi8z_E*31*xGzgSyOqdUi0H6pONfG_>pR<X}>XHR#9Da
zlRJ{RKAsHXG>G&tGE{}LZ$N#RC)4ME_G$r{9^k2v7+{Cy2f_N)bo}nRC*6kgOJxDG
zokjDfYnNlQFl%^Om$z0)ms$#BG`UWrOZXsIleL7xcNrm9V_OL6s~KK~r<IYv5B~l>
z@NR`PN@y$5JwTadPEKRMVU`|0(Jm($n9q)Y`D5MWm=o`w)Z+B9$J$&?5&(gWMd~CQ
z>9Vb}ef0{-l&Xd;i%Hxf>N>J|%|I`H5!SUnO(S8fk{*N=Q&LU_RFVQQ;lay|RE&)k
zvk~@aA!Mqv8Ew*sv1FKMuK(S@bU=ILid!WD#&w*(+`?C}n5A>Tr3YWJ4x1FpNr$<}
z5~ljZ=#QeB3DLoR<c<PI|ICEo0ah1GWc%0;$GjppQT&r)qp{^#3$utimcY2;J{p!H
zHxAm14W6ks5}M?XB>{Ns^XI-3Xyd4pro1<UulRb$D$o5<9<Fp&>eO{M&sYA`PfWID
z%D49CAg;UDyLBw9d2TibQWiI@U9U?jdS^oAj|HL$IanduUz~~B9mY7W+Cw|H-{b@z
znNJEUh8K!Ow=_yen}QR_bGK2})VFsc#`H~VaXvMBdrp*_0Q+UE)Y^ULwacwHP}yC9
zvbAUm3xe8t{D0drvp7{eg#(gZUU3T!Y)VYE#ao>O>c~}IM28D%^40Wt9VXw6h^_d1
zA9jZ%zTOOjoSenRNuj6c4-|eMi!@~Z{b&OhuszakxXQw=_fPx73>T#0wMv!|rIL%6
z>5YtsJIL~x`s-Rvq)jRq7iEGXmSYNg#P}MKk(PKZXU5ZTrdU3%js21T{2HqH?_}t|
zR)~Kt|A$%8Kc*-DJjM8bt1$Vc>fizkE!eriN(j&&0tpb<+v8W^NS41t=&X#L-kQ5G
z{Z(D^kJ-}y7}ozpT>^ID0}C^_z&;;fz>w{iH8l@7Dlp;4{mUf#e<&`oH@3EW>-fgi
z&B@8p<*ksjlcKPyxaz-aOSt}+(fyCH{-0}0*ujK77{v#Hez_2Gk+5@c{h?3*v9W`V
z1%C->{sxjXbM*AGG<UFav^W1NB?<Z+&i_9pIDbhnev`iZVl~0FB3Qva{A*&H9bAlp
z9Sp6qg1eaWZ_G+8tS#MK%$dN)@o$0s4*vhY42=7i%{Vs+E4a_W)gnkZSir&}PH>#R
zn(<ek|9i9j(|sIV2>h>s{V^5)f7_vdZZQy8b@hiY`Y)9gxYQ}Qw!$x4bXE{pmBaI&
zvC@A9_HS3kA5P}~4D1&Z_+PH>|AQK@tbeNS_ix|*-w>t0pF)50UG`tD=>JI^@`s!K
z-v<4U9|pGx6VHDz&gA+7zxd~u|0BuBEb$v=`WIXD-*MDm#N&U#6C`|mzpx1~iTnp(
z@xPE;fVVQXG#CAoClS{l_M88V>AzqKu=?wNT&F!(`xh1YhY32l<#-<&K#{sdy@73)
zTfM85ty1mujsI3c!azv)glOKfVeh&Xpb#r=cr-Zxc4__lBQ%yLFZ0DlwSm3H;(vRf
zTO3D{m|ORCQ+&>d2yLV7j{kA&=MPkW*OKPELyi2-^rxhonwv)e!u{#Ws{5+`rw@)-
z4c~Og4(z23Kb&QWs%xIceLY8Qw`_lGe(-W@()KfOR-(ITPnTdwz$-^<WRUGH)b@ME
z@}#VOIf0RbJc`~}_9Q)IKqHN@J$JY@#-4bUHgFxRU1~g_QoPmSUv8^JIri*ar&I=j
z?N$+^|K<x!(KvtP<ljvdXHXzdDDLp}-1N9_u`!rlaBV=hRQ}r9qqT$6#DI}Lk>(kS
zC%Xl2LEYE+Q68EhrD6T;J%_*YKp%I5ZuWCLec~f*gf$DxTU|1{GWvRY!2B%Vd!mnN
zJpMC+WFC%;Niiixu^g{5(O4TeSb9F_gbVL%YP_)Yv<KB-2)_$aPI-KKF1JdVRfJMK
z<GOEG=EcQO#ta-1$&827(G3c)1avy(Pd%R~Mj!&;)DOi$qp*B5#mi;Ka=LS?@JI4o
zs!aMyypesX@Avji5`zUGkrWtAAcSFxq5jq$0T_bujKXscq0I=n1&F`9*VmsZSH^*=
zfjA*>61HM+>PLvaL10J%5Gt*Z_k1gD)e8|3SqraNmjp0?QW$nnR~Ma@8m6k;^~~_8
z8RT*=HiKxsH`Yb*ALZ#LIIC3lyrFYFvESrye+7{#yjHf%lOVpxo?+U@;6)V9Z3^Mh
z<IlDJvo^|kmY=vWpPd>nH4U4<PKLb$XB}PJM_vRj=%{-kbZg-Wx!AiQ<C8?RuFi-x
zVr{C-d-PLvuCv(km!0JW!XSYlGd~<pLk>4!=rvzsn)D5pM<y%N_cj7PDgCIK2E~c&
z51&28<i6PeKco4HlmXSYv@O#hWL0qM99LENN$MP1$PgR)T2sCn?*Tr3^;e?o$iYfT
zZ~D5nAGg<}j!KKoNIh+`CJF1U7lUolsDxk3UA}_(FmaUuX%b<>k)c=GHXlE9BR%l)
zxu-9m`Lo7Y#>h}HJmYW_-p~g|b_b_#Y%MJwFx$yEI7;~XFr773{ld4=M<BE5Q2WHw
zg${SLj=hR;zS;wcR-2K=W;>+tAEg1Dkn)PWhb!OAt*9QqeHwvei47nv`%rsicvq<|
zsj{j|%^JNY&evS2)NizpA5#*NG<U#JNc0Hrilvhweqe;^3sQ!2SA8*Z@sP}xQY5kB
zM>+)tA(m~FrUxp?`PHo?mTU1PNI3-Qz4_Io;$?=+%U5b4X$*Kx-?5g<v_h%!bD{>h
zw;>ig$hl1oGw&En;9tM3{<w#lJ69|86yOZQEumei7&3>6$n7dkL|t2-#{N?!)@P)0
z#f-2qY3nj{xAi+EP+GEvT_6y8HTOum0#!W_+hr~%FsMVvX7`MW@pVt=Q0i0A&;?9M
zW^1uOmU)Jwki`6~6VQGg<|Y8W2?u=%g6f(Hv@OvAZD*DAb9$m8*E$d-sLU<ow2b^?
z$YYJz>Lw#b3E*Cjvn?UXybt^IUBSEaImIp2&T)f!&bPOjNXEErG)j_2o3H7i;Ndm=
z6e;cNX_fodN%*;xMMYS%#UXdlmiHd3aDc`P#g`R#+Zo<cgc=#VZ4~_*K#>ci*GTmW
z1yE2iu=vU!J{s{JcC<+@G#Lce=-?Ayd#h+fpK1$belYr+JJ94lj@GKGn?Vr_BjTfu
zksM3*&W0Wtr;*z3>{{aJI@#u(krBn4$XCs~)kG?7_8Kt{rkZ&%%!6A6szu(AmY-ml
z8lhglX!6~dc%_wP!)J~+n+8lWN%jNFU2d!iGd_gdxb}9!1<!xePI(<I{yx-Tu;b*g
zzl-?H;S48QzF1e_xM~3dV1ok>P{%6^`H(o6UQ;07f^p?LpMMs<;=YVhy>v{=pokS*
zQL82e{FrD1Mf|Go+CiuB<Nl)l`ugFmNjN1*dWUG|O!cxYQkf#ou2?+<+B?$X0uEG=
zvqoBPe^!PzLV$8kBS~j@xP(vcAw%aQlLPbg-Mu1?98hnQ1P&30a=I|@r_g!Sb1wd-
z745b8ys3Brik3i+?wdJpX?U-k(FY#ND=&Rc%0f0uBeUq?hF&g$`>g<62?Q=%ylq#J
zFlN;EnqP!N9+QYYn+gnqe5DO7;yhnk5xR-H=EIliL(F?ypBLF6kQA|UBx9)aVbyRy
z^s4Y4&}D=mQW-Ek$U&QL-LPD=WF#@5HxUTls@LXW1U$)P-c>IN&d}*pR~{p*B50a6
zc(2kROcqg?gPbZ#L*CLnhY#o4;1b*hL{6+T3%FcHW<{B64lGtCjH10SZH2d*5OI8c
zotzGkLfg+z`Z?t7nWMy<BIs;e5X4l_JbhZ<AH*nRJ%X}YQQ8oTOG@E(nU!teB9E$Y
zPzPPFzQ~PFwoaAC(C+JoS*H7VFDnryyL$9lx-6yBAYHw8fEmZh!x{B*%5qs>(_UGz
zq|=?H;bpsii3w7PZQ-DRd|;rDndl8?nLwr5s&A#_RUNrOX1T!@miPD&yTNPa1CA68
z@6=cNNNr5(?Y$&BFJ?f2Z!eh2**0xlHzsw=mc0AklH}!}bu(Mpz7va!tBFg1mcRRR
z#whpP8AjNh&1iD8Ba=gow-tJqS1&M=&zR3m`<e2nLiU}yQW|Q-*yNKjx4nT7T#X&m
zoP(AcRPhzAAwOJm8FF}rVH3-vF7?CtXXf*?xF80_uA6@1_rx$|2Q=Kp$3*u(%e?KL
zNE}#KGinpk!fbL}X8B)<&BU`_*b)FKV#CDF@*N#my1C8~yg@CKsmSIh6PTFReh+<b
zx%G{{u<4I=dFuBKoBNoQ4^Qa=UDw#C#)|ztNihfCjf*+{^y_+<DvVXZd2bNeyS!ex
zu!j2C?R8s03)bqksXDh0(wEEm7Ta4Tl%g)v&jy2OXxu-UgG4)`AQn01+9=pZd7`Fz
z0_lFd%f?o$`48`x;H2(fLZ<&r-TuSQ{8w!NS4TG&Q*&3aMdPn5`hTpR`47}B+aEh1
z;Q}k)|DbEx|B&PSIgbP9;eQS7e?!-Ty&3<<crAz(1Omgj|9|CcAzc5Rul+^Wf=Sqa
z@wKpsV9pf@@)v+B`47Gp3QXL>!N5Vm0KglA8CU=W7{f)+_ye$=$JSoJA?5Kz#Kj|{
zU~MM{aSOLmN_%)kwhptgD|3j7s|5ZnYYPbt`-`>Z|HayZ=~@_QSa?{dKLP=-p+m!9
zkg~!GD;qmwk_8sPfm{<mZBvM<n3y(9UtnRAiv(S=aW?%Ky*<DD2l4aIS)o7hpMR}r
zuvkno&G@=DWh2x8yF@vmj2CTPpWT^x-;034Kp_$+`V`?T@+t|byw8D<pYE;pXpw*?
z@}D+v%&X~N=9_NkZ|VjXqkEry2MuQr81orDPD7RiQ%dLE5P}36%YI*}9Bg<(iEYtD
zm1%0#*E$O(7FFk<@Kqa_unYWB0Dh4%6<;T(e-h=XbdfjK!s$LCh<dIYPxXE7Z)y*R
zBMwnjAXd9yMM>4Jzhi<zgb;YPp1-?q#Ej|i*wbP1pc|sceyZ1(#yRxKAkah7eKO{J
zwNKlkFJcs5UGe)$RpjEUP{l$od=$b_7EPDE3vdb_sb+lC!Lz`43R!4Zz`BRU5;AoP
zAXy_tvt!<9>Cm}>?)ZHVf7)Fod*p~r+F+4NBmVoPh-4P>grrdhi#I&97Lap;6^NQt
zyEUdNm2^W(P087|$VC7a#D#W8L;rwQ9ovDyDeF5#-J020sR{34{#fIBiyM<swsQ9U
zR*5gvjTw$+2g3%0klL+EM!nAB<Gp5G8w*dTKHuEjHfERyznT67FmAo87}PYu|1YNN
zU!D3_OzMCAiJKq&CCASBPXIBi=FLO2Z8P>?{$mBd=x40(fMl4xQ#~yyT@lU6hRp01
z&))c&lt)wrxgW;Ki|?icVA82Jf>EPV{0Xj$_Kj%_n5l!Mw;=UY<&#=>CP(haGYgg1
zVash$=-SatZE!oT<2NB^W98$W#3WeX_=b*-9=+R(apdp?UI`Dvg1{EbDRhY_1y0``
zm``-qg!Xy;Lc9=lMt&eqj1!{1TYm%t;pno0fJw|mMC_Qp9iMYJ;;FDCu^)qtQqq0E
ztWtz59`dbU)%B_J!W|Di+$$=N6Iu$#NaJ<t5k&J7)TP@E0(_$}8jAL`BLPsv++LDv
zS(GG+UecHdZ=R2F<;Xd7)-aTJg^=5={gAemlRT-Wop^41@FzFiS51!3%Dk9Wsy&7>
zne;*#T8T}-2%8PU-RNj{CqKEUEk1jRayxFiSS=M3UKrg6-Sn&dd%v{`8K`(`+?`w2
zpMbOr!MUBP29*CVI}#iJK)bkzV8<>o!I=AYiea3TrHob=metIaG4sr~1X5KUXC_o?
zoCTm#)p-_q3VGKN3{yA$p4TT+kj~VO7AC=Lt?NzHuALvYxZ4$8%;%GcCn#n2UXuGL
zR5Z~QU@aP;%M~veO#~1j(o&c6zE-|#qv|_~fz7+w7k#IfK_Bxp<1}W0eKGQ<GYhsr
zZd}LEQobJjxiBU5P$wFM$Q6Z?kZ<iPcU>*=wv5VgX&8F04eg8LD8sDxB~-o3I;j~4
zX*g=GS@E4cZRv*Ri-`-O4F2hN9?p%8ET!2do}`0u`_!tbAc7&)ADZ2@Z16&|f)+mk
z<M#|k9q*nDihFRfUIwzG(u9}wZ$=4Fw@a7a8}HkrR;HQA@#1+IrWLb#9*ePuq7PhQ
z#o<aV1R}dZn$w|R1kF{_Tm>M-&`F48f?jYfv=uQ99)M<zn9X=%+HF({X(Lp`IO7;c
zq@ov-4XQY<hy^I~GS0lT?)9<_W*vFbTo9jV+|LM5!v~#D6u6*+B{O%FMqKf=<}{dz
zg>-cZ{kqXf6cX{p23I~Z6h<?E)fBcm3n(>)MyHcM0XODtc50_DL93PCv0_+dX0wfI
zL$W!TEmOE2oIQnyxC67X{%JLKr!MpI+XA_4_oj)FKLHKrMnCL*)Mq!~A35LuzS5?W
zdg1crNPH-q(6R@J{}6YDDTv7!J%)!%x&PMLDhE+Cf@Mt`VI(_>b$NMi9-hpc7TwxG
z)ESqq|15@eE`U&b0DdOCBBL&hN_!$>`1Sq>Gj49BYp6MpO+hr0+so(UUFX7L3I%r|
z=&t$AUCV$>(7BK6Q=QN1b5-@LBk|Ml<JAadE&kZCV^Z&ISb69MN<7nqIJxhTM0+xb
z*md{tlv!hIRy?|mw!qW-pMZ3;_-UV2k?X@(f^*5M9k!NP=^O%(CA4EFC3DK@4dq@j
zRor|J0aeI~_93u$`zE`0Ege^N?e6JcPfrcSvNf%X3^-67#`Acd_$Bz$3`5jxr9EUu
zJyV;%eZRd^&C|!NJpTSNaQH-Y_v&D=MW25wzQ}gS5iT~Er2%$L$34TOjWmGT_U;t~
zB$}f!?2rd5S!_Gc%6fjlM?K*~hxv}{nxzt72~4-o3jC^5Vvk+|iqlsN@spyUGfk`w
zglDlb=FGVX=lZ1k5AweHP`P`AE_$xSW>RRIK{u(RA+6|~YoY!$fjIdsrW)*}_k)N-
zu!H!FecOqTl3r}@+zko8t;7;j_K31{mLU*Hg&dG$4U@_<9>u71uiJ{G<t-u5ePpO#
zIj(a>RTCEOwfm^0D?U&c&WXRHT|4>RlnP50xx1*xuYXya+!JSnHF$r#UMRo*8f8kX
ztX`X5Y0VfhD2O0LoJhqDddCr=7OLY(!C9<{nQ8L;&2jg5*((OtMXQ8_eL0p!!aysP
zV$0;=$&<8i8+W#z`dxjfBHgUit!q>iDl$Qi_bc~xg*8tNxHca8FmL=0Xf@igQ$I~g
zU#u0#8tn9^>=b_28AhcfoPTpWZ=+_L>~?$R`+?{F`@7|pyVT5!)9-!*jcE+R1H7*}
zzx@#Qb%H`>=4R{4C7X?ztM)j-&}I0r1+a^r(<5)!+RK+JdMY5fT|41Da)O0|L=O-K
z!V(}u0jf#j7Tb6Y7JmYG4Z(EXcmH0y1^bz(dCB#3?okdJdCxxQ3A1#`@X;$z>%uY2
zm>(4{U|bl=`G1-D{QE;v6D4ky5U;Y~E<L838H2h)=eTjV+7W20$~Nw$Klxm(tER&e
z1Ix`r2MhtM(T<@Z)}N3|D7&|x3S`SxEEvA#Olsq$oMDx3`<l*%kz&$bd=X8oy_&`~
zm)dmr;xmiv>9BHQi}*fNPTIt~3AWdWZTMbmEt03fqD0Kv{WSAJ;-cDvw*$>CrV4Q=
zjQd5?f$Huj0Ixm{6iYL2NpLr7YrvKkkefeV4)+{AqG|@K*ZwFDsP|a_P8X^>5gFUO
zJ{=L$6}#jl8C)WR{uEt52T2=qd1A~JY8uh69cgMVZu@e8w~j;LKFO{(WPP9~k;Mvc
zfoy_3=+k@gq%l3Ew@{?gtBLon1ov5%42+At>rhqx`ckVJh+}CvzxToC2U6GWJysR&
zSLGSnIUhksDluj|*Ap-MyseXHJZ9nrdw*3QgF9qYR5la}y2wK#?Uyy%*D+@SBzakj
zXZA&JOVp3sUR&&&G->jcS4+^+^dZ$DlVTNN=XplS@;GMOC9%ZdkkQI$B7i|{A=7Z2
zZs*0qTM<hEW~l9?L&q-KkU0T);pnA)MlaSy9@An6Q;mJI4U%mJg%V-7wND#|I9>t7
zR^-@q<=oc@+c{#kpS~BN?-CmMhETvfpLjvqgyKJ#tsjOpej+ES$~EcQ^?7}0+prOz
z&NTlpqrkYi75j8e9QbbWgQbqDLAGY~{PMCrrMoUQu782?XiKO;IuhR3u5@Q;Q*7TX
z{qctpm6SS=0(f$g(8d%w!i>9XkIWAzUcMm7{jzyv$7RUu3b=ZuYCke?j7|iR<ZSZP
z5%6(iasRoB9j-E)vXmV2AV4;G&yFcZQ)#y#^U<i>Sz?Cq=>Rjj%<}%&G|c~;EbBEx
z%ll^g6aDPqr@MI>^}JyB`LE=3#*x#^HF}HKF4xXyX*{%pL(b#dN$k1K@3|I-d!HV#
zj`cj=p`5554{<=K<nY&DLc41VF6b?N0Seee;!bXN_9eiZQcgE(w$$o&@r<JZ(Swts
z5nrP9bZS;#S9sFfWGt_OZiHls3+b6I<6O+N=jEuGO~Q#ZPIw0Q;loseZ2}@lu+MT<
zaU9*za<uagkY?)^P``a$yjU(NRqlRlUw<mL)U(8);Pr_75_|@K;M8mc=_R8A;p7oF
z-8bfRf@K`mOzc112F^H{SMLpdEYq7hew2<fIeypRqxBN;Wjg|PnNKXnIqjQ_T^dF`
zYch+fS13#1OlJ6)Y%yv}!tPm>Dh2Ybi5Djh<N|h5k^_zIN9NP-@l&<Au!CI0Nf@pq
zC#Y~BbnYq)${k8i`5TLr;qV5xOS7zNE6P4|Y>i^do4>^>p<^TahZf+k6U_fW@BTUU
z{9VOM%m$mtD_yRb!I`GmxqZ<KI!o?E6w5D?l(c<PrNMX?f+9uB-Wk3h8ZB($&}XH*
z$7FM9oc&=JV6!3UyY@QQ3Oi|pb%Fh@!fM<lLhzwcZlWdZl6(xH&CKM&q%&O9hqHn4
z;I77d+qniQ-4YLpQ+bCgafK5k@?}T&MpTFNbc}q(I0_q!9)+hwQW1Mj2Dhx&w(*#%
z<5SD9Gqq6ET}zFR!e@NCxH_MvrMVgBI5xZ4uiFM1WRDd~+pvvW2a5=pP0V2>=~srt
zpM0HsRa*~@+DLYba`+0ROt}-Tp<{Rb*{Zd*SOcOCT|N_m!ZTfPWwBsiy3!7crIr~$
zUZElvo1NX!i?4|XbZ$vJ#^^N0nk<IL6PB^=E!j*nT^R-`>F(P@Lh)-)5HeT#eXeM8
zQ&J*i;UQikLWtJSNGXzXl9J148Zi^Jq)CEg8^n?TE1%#21bz!GHQ<b>ZQWMqLuzfA
zmw%tm>)Kx9GT$skwyYnF847+=p)?Cw^1=d+biF=tj?8=AxOdNkv@*pM|9YqS?C|Gn
zSG#7g>lwY*-}Hj{@jm8Q#^zr6C!qRBU{m!cU`j@u`1$H4{yO0|EeN>(v5?rmQiv!E
zx-FqN=1IB1Srb-c5`i{o9TjS6Qw9tLPC3QGUN74wq2F(;UnQjMsCRgvLhEss-$wgV
zzj?e_jPp!1O|wI681#}$w66=GQzw}vOv&F4Y2H%1@cW=A><k`=ws@d6<{yC23g_@W
z)p=(~dm9?3G<Hmo%*H-aR9B8-<74dSK)If}O}o`$Nu`$ZC89`%g$zwhJFs2cc4HBY
z6nB?-X{x`bx;1R8q(@J4w!`>ZU_oTkRlvbbnTL(@JtEiBSN(%v@~vT1KmT0R46{t8
zHw^7UfldzMbhVU|T8V~oVEs6I5tL7e5Sq9#v~c&SkHKQg@lth*dhv3)dMa(o#nvdJ
zGL2tK#@=}`-k{6ji;?$f+yS8;$4D3cF@lo$kW&!sPXN~R1~PwX!ph?75?}J(rImNQ
z!hzRzD(V$JGR0j>F-SSEnIDc?#=C=TG8i%vBlA?^TUJgoW<2MhNxG+`vYtQL={I)K
zrPWzBt<GX+QZ(4%`ZRuMO*+pZJl0Fja)i(h?>Fgx5R`mz8b6d#DIXR8Z`sm6IvtAk
zd$j!Bo8{qffe*(UBQm$XyrS=^>Z@bHvsmA7lPG=ykYi1>KWvkkRhWH7m6AQjl|&D)
z03>9tXP8&>y4F!AzC1k4oN9svZ#USU{}6jVl#CP4#%a}P=Y#4-emp;e<!>eWj*p0W
zlJ|#TO_n}b{`N&(&KY(2d{0xH<0l|eLvc#0j(Hcs#xtdg=lHVoSqx=8ij^vn)YwHA
zJB93Rdk@JZ9Do~;dW1@8Z55~Z%oCQD-;HUqBOhVtLQZJ*!8niLQg4{K$M%kQMG{y=
z<91FaUo_NJ;u7DQr`x_Md(aTL4{0fWN~@DrCG4V7r$jDwE)@3xtWX2X@j8Dg)bcC#
z-ue53)nj0x-V2N&LU2rLd)+XDcA9Zyt0m@k$3_+Z=?}`$4h-l4T_S^Mi5b>vAJR-D
zpOs;;GlYIJ+p{rGi4BpDuU!+%)v^|py(W4{ra66;c}FuP_3x4qdHYM9V|<4r4=*%u
zV*=CjsD|?;(ZmsS2JQ}tya)=*u#Vn^x&MG3C-5K!Xq&Q-hztDIR%%y*y@Fuj8d%u&
zBdD_E!<zP$KWotTG7;wBte$@2<&RUYk6hG-u4}>t<J8T&r5~gdT#;iK4jMaoIB1pj
z22ofRhN(i2BQb(d6#Xr6ya4rh@!pL|0<_+$mF=-+;{M(8<cg>Sc;uK?hJ<-*X53HY
zawIf(gpdNGkTMEvPEOiJS|+S~fmKfh>|8*?w%_}-ol17Qro+qo73r{Q58G;1DJ`uK
z)nbx~rzzsj@T@fP8VSZ#Kc>)*dZ>dD5K#D$1quiY5u(}h4lg)5k!6i5r^N0}a?{0!
zM7&w;AKS;fuwIek1@=5Mf-sEvZCoq`RB~Jv#a~g9SQvCx$rL8f-_j{`nn{*39UY&X
zz+v8LVZqUf{{(1sp~p7vVM;}=o+N^VBa3)csH>~vPG;RuYLg%tsYsE6lG6GJRUvXQ
zX5}<&Le`gq6tKS~%8TC}Im8u4nmBtYeyWZ2sy&CB=$DawRWQhMjwJtnaS2m^sZUSK
z^^SS?gtWKc<jX1lLaF<g3|_cy6%P}r6@KYOlrWc~0k$||)(oRTpQx+w->(KOIL3>9
zMqNmL4UMUU(|QRri*i#97AAoN$17Ma4f9v|BjuqxPl;lxXd4$I>BcdTw61SLhzMmR
zQt6^AbXMIuqs{%UTypYs!CqzGR()6f87PwazM7`BRIZ-z50uJR<8`@T!LII<`4Lj{
zvrvqDH^krNYR?H_TOE#PDQU2v`b0xA;LbowDIRKsdVrKsOXA@}pl$mDEcJNQ`3cz1
znyZW%sb;gZ+0ZZF$fIDfQba%Lt<H$oYCPi~`aX<js_);Q{8n1ZufJY8yD?Q^hNdYv
zvY%)0#W@fmd}DCQ1KtbrBZN<;wtUfM1RVbR&rH-ma-8`g_J2QJ%YJVWZIs6eylf~N
zf8{YxM6y!zWb(3l3Xx0-m|1$7=$4Mr%q+8{fWd4zsNkvPJGaTH_lOcKIZa#ZBaXJ2
zljUhpn_x$d*YOicuZIT-)@y*oudtKL&cAnJEkmrt0QVx57yTSlgp<puKH4^EA-mDc
zM}A>av`F2LK&l1RYGIuJ9tz*@*M6n!c#af!EOa7dOTx}A+f_lqDO=()7>V<BJZo3m
z2*p)~+9D}eeZtPpStzMw(kBF^of?sTDT$QugJ5|zj5xl5HCgyenL)2k?a1B8da5tG
z?sl`Z*``1N;W4z4S#}P&T|&xt>RiEjSn=O-g!UG#sxt~08d=#YctVY>^|2y)G@-U0
z!0J@6UiNJjSk)`6`loBGS}{h0|2^k!IFKs)oUdNHPJd$Mgeb5F4$@IOEuTi#n&9}!
zZ+?7~77c&kc}TlrBW$C<C{Jm<Ih}-t!Zf*rP}M2<tyh$3T!X^E;gVO@I^Jq^B4s~|
zML2r~FVO@#gG;=Cv?>oTsh0D%8O}~RBzgqYxI4=BS*D<m=w$5hn)Da_s|>qHg0J`e
znOEgmLMswkWnY<a>9RnfAv?$Pw2o$^d{1F1#L_u>jZdJy)P-mAxl<R%35CJ}i-+J(
zhq9`OJp>#6dNSF}jx{-|Z{@OnsSSQddx+Q4D!0obU|+({o%`r30R~@QgK*{I6j~Jt
zKu-Q_VQ>8<BYjzgtV0j%uq*3!g&~>aR?maP%y0LohZoC18%`*?+c8FW=*tf`8kbnK
z7^mvFQ@E1QUFE7zqIVLCCsEf4fZxVQn4jr7O=X>xQCuGEpj^vIQF~vd9OWM|gyY#Y
z1^4&o2gH=GB8bn{%+s7{cskt5z1ebWZ(!9LX+jBIf(?hCC(a}K5{X+L<ugzzGEYBI
zHPUMkMTG4O6~C|yvf?^jFQfc`AT?a%+35D5{U+vYx94o8@5r802L5QPcD5N1W5VK;
zhBc))jaB6;%3(ADFVap*X@X<A!rJ&0Nc7PJB=fhAgYqZNOEbT~k07+7EXRiF>f4w@
zPjS9z*aJu-_+rZeS1Pe5jCo3#ibj)?#ydXyy04fe9A37VJl|gt#=a5GDee%Yyu^+A
z(MZ|uPK`TRAFe0ZtQ8Mw{-L=Iy;1g3R`)<Cai@zU6h>+@Im@Ii?Kv)yWs!tdvk#y%
zXo)4z@606R{kO4ILBw$NBkzCs;?Om)eD%6X&F@a<bE@-ojvtw34vw7CUMR$Z9!V9R
zv{Fy#bjlsMDvDIDDj!yU(3vz+th5^qa@qDQ*V6GJ5jp;KeKslm3gFwFZ>UJaq*(TD
zxdB)5Ge|bl7&Fv5l`^K8vw*EjpIi$71c_O?YrD6fkF}UIW=r;>p>haj=_$T&?|9E=
zL1<Y_&VT4q?&gy+`jSV!{7##dsKI>AC-yQ9G}QSLPW3e@;VIlGnai9mw^-Q{4l~R(
z`f(Q$^Hq0r{TbR7OaSCP=r22k8vi|SP$sh5mTvSRg*J@2uEVZ=O36Q?f)S1ZtqU#;
zLr&Ac)S09e2%ccaNmXZW?9iGhr+A6H>`cJ?c<j!;&0yn%oqH}yxxf;a8>f;~G?EGh
zNg<VLDC63?g7ex@N{DFA3^=&=O((~>?**`8jCm|SbtP;pvgInpkPfgGqQfrd?6a@i
z33jdY)_vFGk?<fJ`cnE8b>28V#pc!bWaKdTl%1b|5Vfd}YB92yT-6kFGLf&-wE8Bf
zbR4=A?Hp%64>^0OL|d13vM;hfPlqLI`JtM|c{(_R(T3KWe%HiDnWSUI6Ttz$Yu689
zk0lXcy*gVKU454KNqp|@B)~7+20)?(khXMaa{m4VsC5dDWDo$qHE{B9HaH*?(~FX!
zuLWa$diDKN;wIxk8lLKdvb&{Kqefdxxt)4W3YEf#vQ+y@yr6uzH%V(?|LFG0T6$nh
zOL2(8%6W^Gt8g~Ee?j@e+XncQY<ALIC_;i%<+hlGBm9hund*2zsCY-hnF)xgT!7&w
zfKoZ~+VAdLZCc@d=IDMe&SQ`MdG7#4VSOi2{NG>g8lRp;oqu>{)>*}E^GC&*?Br5a
zBX=><N7f@j2y*W(QlfyGC2ECkYNj)+Qwp6Rs747^Ms&^K^hw7AUE5Tz7jX_k9t-Lp
zxNuEAtVe)d!!2n}Ez-?C979;h#<-L6QIQl1Wlm>;B;9OBU;<oqBXssE@D}MrtR@+0
z#u>J85t4L%Z@@o2LX}@?cd{F``a^DT*>?xNm}Sqk><CALK)6(|BKvkgghFzW{`#I>
zQsK6NAFfCy2ix>M-7(ObqE)joIBT*gUV&3xvHD3(@LX5`r<vzNFJoW5TDtj+C=ks^
zYpsl=%GfccO>3}D`hCtQhhAnZ>6x57D#{TSBc2R47W=m#A@sk$OIC&N=F!_I428;q
zHmVN~9wrvOL1F2}cABxkA~w7<ap$1wJQ@kKfDY0~%-oCbY(eJV7U<+6(ji}ghC*8x
z3?1yo2Nn@_3H1g!r&Kz=Xq(S|SbrV4=k~pZK{@mC7<Z3W79wMwz?+(5s65G2L)|(H
zGkwAZW4Ce+10w~<4g<0Md#jbd-mda;GxYX!6fRwpZ2a+ghd+0`1Bqbo#5}3x2~`B0
zjx-u|^1z->rAyM}^BF}<Iu0}yJmq^^JR^7U30xrZDsZ+l652WNK^JLFU3HaC(~UKf
z1|yw34p<yCi4O-T<-564$=mIaSPG7#8^U4wR0@}Hp27QvAmw>xKmKT#K?%PJJZw5<
zzx^!4P492*GJLa}Mz3K@%pIe!^?k~nn?wGJZ;Up6J0@E;rH73vdqjDV9EFJ5z$HoS
z2d#Gz5=rgNEi?w|(&;M8&L;;G>>8h@!zL*|u%b|D8f-nwc8%fGv6P0463l+i>EOUM
zvJx0dSQIsH8e?2R?{uIrZndLGYld9i=;gB9wT<h@>J1HLplWJ@nTC;L=CPOJT!32;
z-u`CT8P?G<6_i}|u8JnjF6EGBtr(vTsM}F%IkF(|rYMegA0{eA=W7;jkKPR77p@Wy
zRyI)^+iLpy5|M6<>*ZpBMh?w#tN3=+DT{>1k$HoHLV;}|HPLnp>D79w%pDqo;gXoK
z=3Q}vo9QV3-JmQ>Mz&PcX(@Iy5ane$M<v>KS-<G^xrXx2`U=vm8HVOW9Bin7Y)_Wu
zMZ`lpSEdwOGlK_as@<IKIHw-61$9Pm$>{g2{ZZ*GME=Zg-|2{4ncb_IaP5tzv|)Ku
zd>yBts8HOEosH+u5<MnY_YRAA3gx@AepMZ-{tch}3=4*6V2x&FC(biZnM$+j%itIN
zS}xOK&sojp{a^^=^t)^m+A~^3G(@$rp`CoqC(o@bpRSoU(wNByS_Jem_SaJdfu3T~
z@Xbbza7{_eW_8F1nF2CwWba|2)E0!kv>|pHMesr(p6xh>ykR8-@zL|3jyZJe(iIJ5
z^2EVLRBiv(fpvM&5wMZmM0=T3hc*r2O|?y~ks?d*1>|yWqcl%&3F*2mwrO4>_5BlE
z^*zU%sSox*1?e%`=8j&<l*VVC=58HW$w6*XGn}vj4yN&y&Y_w^Ntel3GoA&OxkX@{
z#`qCP%`38*2iBZIomQ*mn~2EwS(AQQ9aeOg#Wy(G0r&xg>I)-$Z%Va22pv=jkJ*O^
z%6K#Tcd(0HxM=cN)!>UE-b&?EYd3~StMSH)G-5|nyCNBvCV}im@2lT@dl%kf;KLri
zAW?aXf_{(a1s0ZS<5$hGORmu?iNt%;(tEC9@phtwdWdE$6-SaPm(GxeZNBE(evjZ7
zyR*-*iCzEyF!$C`Rc>v=FQRmJN_Vq>MT3&k-OZxArA4~COS-$HyHmQmq)Tb>EcgEQ
zdD!)g_q^Xf$A1{ZdtGDBd)9r`ynf2Gj?Q?o7!>oknAkgOV?!j9S?BQVswI_~W2E0>
zO4@`)yt*@=H39kSVtL0?V#l3HJzaGv{$4n5%Izd~lfiLy1fa!{8sVJOO)tXK+A&WC
zCMDt2z^H@+ZO=bD7})6hVr{;y++%5_)YuHl7#QVELTK@Uz46n%eed^Dj-b?$8uz0h
zu851w2;-Hz0DJ8S??FTH8P><B_=T)9OZk+Fg(hxG_;>XRV&Nb0QzQ)7NgJ8i_n_zZ
zUTAh&$POE7^qcib<=_smN!lTczoi1DbzOSu`{_uUJ4CgTxynWyWWmR2EGG1Hg}h9Y
z2!(_1rB_Y>h2Z+wJ2_Jo8;P~%#(m~>$>Un)l1SsA6E8D19Fv?ge`VGr{q}CG_)<tI
z778WGR*@QtiZw^}BODn`o+x1>6sB7#BG25&vu!8$Y|+0+*XFsk8?SC}KL3%hhnJpT
z%44u{eDRq(0P`e+?eWs4{03FJYR7@)ww@;%J;vje?pt)RYz0--mq>xh^1?$h@fc>C
zeEjVL6mv!$<?F$BPRrq&bwBnOh+(b=zK=5WGk<irjc%px`U#=M*J{i5Xxb@wec-8g
zK0W_$(+Ay^&i@la@hkfz%F+81=6G)cn^yjZ^(sM_ANr5dxOv2uErRHroi`s!i9W#Z
z71m!&{P^n0d4cII-uWF4iE9#3Oti?ca4v$qzuAjF!&)`8#>(%3=Ax7=5c+OSKz8HA
z5N`!5U)|JuYX3rPGx=i`2rf}JtG+qK>uC~!8A0bpOR-`HV3RX;y|Cb#c;oV9qFu`6
zQikkVzTDR2*j7G~abj`f`JclAtMw?ubvsN$LqmP|LRZ%WLoQos?tIyuM=E)Fsis#}
zFcg42X<0BWN`7*iMGLNs(@1!%{xY|xS>-^B>Ek95ttG|BT#>}Fe%>OixMCXLxUj|K
z^P9mUN5W;}stViSe#g+0W{1UOZZ{Nc`ecxDx!kP-Qg9yXelM*qTA3n77o7CDa1IBp
zv}|EdBE>K~!$xu4)3g2Xe9H+_%4eJ}B$%oW8nlqs&^!1yQI+gRI1B=?k1dqnF~d80
zhN6&AE^%qtkYn(uM=j(HrYayxVFrJCWl$kN${Iu$-eKgQ$F(ADbW#^Vr0F(8*yB^!
zR31kd$xNOm<H&x^8Dr#z&?0&HQUSx1iX-HBFh$&EFatt@$-FGS8XWWRaPlE-tZc$B
z9|Z1uYr&yoJ)J!<8b5*~6PIo51P?h(A>HM*&h@liG?jJ%LX43?bW}I>ce<-tjcY3>
zADY48Il5IfkYBy%c^TEk5NA5o<N3vQAfVQdDeabb7uxU2u~*is5Etx6^4siI>R|!q
zG#VaT6U7-Y)LUhB?lo~cfK+%jwhq&ydFT}FEk%eS$Rg_FU}1Ecpg%IjP>n;XCB~v}
zrZol|BHin<e<2MZv>Me^MQ6CJev1*&>#BVg6iC7qy1UAlr;0RHbe@D>y#6mc%M4Zt
z(JZeL&Fb-!wP;nvWq4X@LL2E4RbO<ELKmP1jl9u5O^QHeRT|OLF21vdy~Ky(v#wIh
z`79<SGE}j6PQA*rHHP`@3o4H#{1=O5V4U!(Q9yb%5>V@D4+u4a1+7hixu8<n=$H<T
zJspAb>6$U(0lM)$u$;~O+&fWKyw`{noqExJ9$NZe|9^GRxh}UXq1CzZ!rRz%?xth{
zhnE>izNDBG7~%@#9X+&j!}p@YxTt+EN%tnX|F(0z>uiOTZ7a7-$pd-X&`EQJjqh9^
z_Am4C;uCjTpv{<7>>btH@nx0JJZ^;sa?F0*%@RDH%#8&BQ5;Ze{aD_K+l$bt+Vt09
zNtwel5$?)1G_h2&Yw8d}(aBjb9;uVvu@Zic${%PLhj5Vs8F7~%Y{;3Y<}*O;$UgOr
zWu55Ks0Rc#wl5E3(Q>MpteK6=s*w10X$ghwn4PmY81J9W<DWDk1z2VTS__#hopc9T
zxo%m+Fu6h6y$M%SZDF6<@$tHGtu_29!0zXU&vrF!@%<?))u?3+hAOnGg9{~l1!xus
zDldh(aPvVuDZJ3daR<E|DcjQfsU8p`TxvlpdPMs-9St$rB7><$aDu%usykb2Yvg4P
zKynROC&f1NVKG!h|Dk#VTT~=;TmE`L+xV>EgsYCr-j-h#ns8v=5xV&e3Q0wW$sB{9
zsC`(pwf>Kr&%BQJM9&VH#cpuyUtKQsN@IYT$h=Y+5Q>-5C|GY?th?Lc$6py$YYl8|
zGo*3iTCvia#zu@8S=fxZo0+6+r;?e+{gy=79pFUV?dqt|jAu**HzscCe{2ee7?X0v
zq&9q_p{K3VGJMShQ*%2_vS)2<H$ngtisIWWw1(c9V%!~z3O(2zAb%9zy`)gUC(_PH
z@t(yHJ&wKbUlxJ|{Cb2Fqz>93?yxjCD`wk9VTHbzlj)2QEBn)HI-zghq8P;irJ92+
zH9fDz<dLm)5>$&Bhq(Fu;%cJb#bUGdXBDlfbVta(>APUyWm@ExvLb$M;@MJ%d%18(
zL1UE3A78l0qZC6j&6aMnW2d5t;3io!S&uIlpxYXoSo{!5ipL51m#)1q%rV<3zc8DE
zOWRh?)DQWHFQJ_fDqtiLE3~8O(Anew%q~%8HzKl=VXw<o&J*$J^xaRhyYU_Ulkq?|
zfA>tw;%=}s`aaKkL9o$-->Y#2zbk={6g^^FntQuVTP;g8Ny@BK6Y7YKAA_DN^Wp`E
zq+zd_pT4>U0@B&2L4D^6k7E2Q`&hd@veB{f<3&HL?eMbkFU_acG8$CKi#*m=N)Ej$
zGt{K!&_1;`FM?lhFdDdsdN}TSEM-rSxeI??S8;a>e(=&Pk;hUS%#6P0oYl=V6j+!0
zUp~{pbIsdnBu3Qbnz~yN)~aG(hmq808IXu<8e-LYu`u}~YmNeGoEXuT6)z+W=VmOZ
zn6!(Q6c-bAIT>S^TLR`OIAN+b>H}Qc*m?z(`sTU@E9n_aeQG6<P0DVuaH?x2OL$X$
zoOBomT_L&XGWT0^u{r(NTw0+t?4s%$PNWCb`-v~R4_ICWGY2hFbaR0x996YT9Jm~P
zaGMSDeBhfp7xom}W8^`0s?AD;d=drK7re+8{x1U>4mJjLyY(3jX6{7bjp8y!*mMns
z*0)B6VfifSyQRpmrkUTdE<ne51asDR8i3g2;MrX`pN*G#+&#A^5BlL8A}0vlDo`PT
zfA6|JCOoYYWGRZ<cRMFV^qxO^6(RP;s$@P&_|Y<9+VO_M*G_;8Pq(DLh<wkYZ>yf=
z^;n_l2~nl>BJ+Fm(zI}6n#Li6&Ax&}ZCy}rB4s2+qFpolRn0Jn7n?v-<q}*ADdM|Y
zVRF>x>1Q2LElJo533eo~Z0ImZqJ~Kq^gZ9Bb$u^vGN7hy9j-xAA!}ZyFH_?TlOruZ
zO4kD+z8ZOhVg~GBa;ZNAfeQ(~!SIru4NswVOxy7?pMU(w=5gu`wrMvTcPeDhHsj)B
z1VeR;@q*G;QPX|jF28<&idAPEB&{zOoboAY7x@-6*<SMP3hnyd4P%N}?7nn*RDAhx
zR&CV7!z6;Oc`(oBBpmIUYI}{Cm?$ZJZhSO`)sR~#Us48DiY7J;qMr>B%NN4;rtQR1
z_j`PLYPhNvwAm?9!Il-MJOxB*(Sf|mFv>mr8V?|h*kALA_WI2-Q?tRV*^rR)@*fw&
ztt`q&fH$xm=&lIV?ewi@FYn5&KA|FhuN^O}r5i}S3DiuesD|CpoG+iW65g0-N~{m2
z9V25ENL%VKdOa1e4-Th{)ZVOr`npl#oXnM8qCnJb8^?#xb2t!z`PpEpB8NN`c$*cv
z!Cgog{1!gYtSQ|e3O(i({0by};u3mj<2-x|xB!-Ti$e8J2y|Ggqqq3Dx$1#05Z~=|
zmBTJV8#z~!aA?PuD=Mnsnw>ZW6fz(%g}*uTri+KK$m&c52;a!%j$Ku+239H|FPI&D
z*3CJtWZ6!q#H^NH9yQ=@w?rHBzO0dy^W{iw{*GVvAt+Aa>qG5ctJ>8m*E+N;d@@MQ
zSt9$vip7O>xP>G2IMXdAYy=fM-N&0r)?5w4N&_ni)l4ZlIoHPU0>e^!wlEbT*<K<J
zmgize?wuF76ptJ$v2Ce}<Q?)u8<^wNvfgH+I5s!J+{F~isxukdYuFMi?xoV5I&1ee
zwDg%sVGKX%H`|gnzFx2PauvF)=szul1Il}`bjE>#CWSQDdPP7c(#Ol(W*6-^RNuPz
zX}sR4PRMy#`**NV!Ib4YuYz6Zc5k4{Dc?bPKxryke#0q><JUj7u4%L-*SG2*CX<V}
zjMov7RsLd2LP#c8U!fV_f7v#qocsknpT|e5LE>PD1*gh~V0p;}rGD<3^FyV5yjMZ0
z_wM())C`%jG67kI`^gFSo}D$fdQIn|;-0)D_D8(CLpz^Uuf&D6*uMMHom^p;Y{Z04
zYRR2NYGpE3ZX(_Y3N$@Tcjd?NmA9(2q(VO-W_1%ZN|4!oAxR0{3~;=jpQA>yNl#IU
z<TKK^<?Y<b##Sw<sqW482rDb@k;i~!P6tE((ehU{>E<D-0Q}%LwYBsC<D{K^7FOgh
z4P$WAM8IKmyFD5QDVsu@I^!Ue6M%nVMwC%FbcPjL11wtLy!^!rdj;=B%TO4j*~tXo
zTVHyV1{CbH*wk+w@8+*f>ssnlPrJ>J0yE=hwqbd8`S@Dmd3Y`!Si`sC5@Hn0qrcA1
zbO|dj+ZMQiY2<=SRJPBKc|LG~m|QQFgFW&Sa6vUaD&E0u0)tV}KrL;&swesZ-29AH
z=k!(0>blo8@Jo9Ieg361@<L?rF9H`JV0pWiwvNG>KEreM@4$KQMUb||OVx1{r;eUc
z<vd`>8I`TqM8EO0CnsUT(CQ<=A(r|=YL=Yk$|DCO`W-K*L%&7}s_7N5zVQBk{uxpk
zvD_eJtgD5V0b&RoAs6;-$j_u7Yq(z_%m0L+)io2}NYR+mLAA+uulC)bNz*2+FJnW*
z6G=4Bnsx`5B^68OK|f(aq&A#A4pazj+r+N3_XJhGu@t55$77Nrbs^2perW-r1IRmz
zLReW&JKZopf`F8y>XK9!GV43=AZ@VboiJxlGr937lrfqyrs*4&V}_@P+Z@{Bli?t|
z;M;!16Efax%rmFSVjIvP_3?sdI|4Oh<a^6YAr>dimuyZ!mg%-wRTvSt`0$3hE9)1)
z%gMx)u@9bmLUAit=%f*EB%2IKa+FZT-725WlC$atX}7JT6HZ8?`1{4ycOLAFM5TjT
z*(nxGdzl*H6yp|oU#5m769%(ghU5+9d2lgH;bjYU^I5#c{A^|)is$R@K^=y*bxYQq
zCiz-lrVC229)S6;YPq**TU&>kj4!x>yA#yFwQ}X5)l6!tHE*c@@};>1wf<?Z|LM?I
z{PPE4*DeD5VKJq)O52VAe^@@S8J)EwG5H%b?kLJmZYibyGJq2BOcU0kvo6IKUbh&h
z>obp%wN-*YEPu^QWrIOpx*zG~<<SRy@a=0UCls`+8Ul4Vd~CKGIbV2EGnVnVIzciw
z71?oN;QY*{){3Y96T<y_;cU9LjNv78IyL4}tNUE>Jgr%SxOnX`<B8AC2~8xhP;oT|
zl#jgL=E2>kZA9r`>UdI2igUWQiFEQICpP$Ti<uy`cNM)|H2)ZO7p<_4{0xncvyqxn
zlS3rzvD2IyTi^D{<#owVi1$A@#q-y2=Hk5)KA9_q$~~#@tm)0AR^I^Vek@9Mgns*w
z>5VJ!gH|%@_sVoCATyVJ0*hq^yPz3GLchcsp)2xRIlYT!62aY-;X??Q-h}LqIs)uu
zpA{+wM^&=uZ0ZM5m?TTz?ypnk96To?PR;MQHJC!YPVOjbmR%*NmL+iG<JB;m(Y<ZP
z)pND&%fguUic3Y{U*J%XJ9xO@_H)4C(_^Q=Q{?-Lm#XKD_{V>Z_34?x*9T{<pq3Hr
zTQXsf5{eSx*9ffleyV!<1Z=_}>Eq$K=A}qB-lS%y{`{_1B@C;_p^OAGpPi-}j+?w%
z$N@i2>b&n=qqt&?nB<VcYVA5l%w43lKA)_sAcQzJ+}TTu7u3+#DBV{2LO3aJ!f=d@
zvL#$lMeWgj@``&#Pbf<lE$Wuxovq_kEd^H{_VIs0xCE_JuD_3>t4gX%*9O$@dFF0z
zAN>Fap`bP-69AY)lVq|&BIdA=$|2~nm<O@(>Q;~=;1uyLSRn#+Agnbwjjq02*Ilu7
z+PbyI;6ItqLLM(GZo^D3S`z6;F94Ki&Y?r^t>qPSQz<54M0}ef&cc~`Vd0mxCsVvH
zcy>&%?S=LSwf#xftn#HqZ@{T0g)*E7WYdZ=1*u=BwWnF8EK#!KE`IopHlyB-h2F$U
zf2KO}$W1{T749g^W~8m&q+y}p6tC5EfI!z$z3x?XF!uLB3hO1SD5JrIRpV40<O>i%
z&u8tUk<QA>%y<i}zJ&sDVgRB4x_$GNYFnk{@@ZS89`G%(K_V-k=q^J9;N46>)wSB~
z)s;OnVpQ-<BAIfYYBV@EE`+^0{3T6iFd1y6<<fNr-LR7Vn*C;8)_yB=7jR(}0WL>{
zV$S6%GQm_wne3)<OfNjp?$mL`wQB7KNafmFj{DXCh^x<?RsRzLIqJi{Vg%3G;x2y0
zhn6W0^`8)?362LuaE5GQYo(Q3kMrO4IQCa7*oy{`O+3}A)b}_N&s%r>$hMm-1abSQ
zBTFx6CXPclxctJf$}F5IaM9x3RUjBHkr<3q)uGX#r<~m$EE>X$MKA2rwPe_*WgN^4
z$Jp-+BvUQvv+`*bQhueinIId~U)u(IBX#zDAPB3=0g7$x8Fu}sfx#PD?@eYErb}B+
zGBOLL(jad#Y`en`NFJZKvDFl$k002WG-sGoKTHUvN72`yjpMH%b_&U8YvT@`E8)`H
zfefMOf%<S+#jK1FsIB?t$bZ*EB|0^M!<g~w;1SUK&B|9~n@QKBRHw0pLmsorhq+oK
zm1Z9K;SQ$mxJ12L%W$=}r|Em_FmPsJIqn19)6^`*`lE#^Uxj^I`ArTz|AAOX*f+gQ
z0h36@_!3Bk|17-!leCxcwW+D02H42g+DK|{YA$4%R$7``^99&IC9596FXT7i0Z|hM
z0nM7syFdzS>#;UhfA+>>rlj;)%%Sj^Z}zvUlmGUtP70mE?p-nDx1nEIkLK99s_&F3
z!C8-uh%{4umcOzd?Sjeb!C8;jZKWJs=4C2j+inA*pJs|Uj>Kz<t7uS@2q_UV?46>s
zCG34iQasjrAWiB`2Ttif+~m+PdT3s^segLrtu{sWUO|%V)fy>nk%3u~aq<q^4XoER
zt~nsrXkh<l>1cR`IcUFAZcf42;gXSko`l^qNATc}2E-k(^J!U|f8acD_mUlBY7mbk
zwp4Toy>jrYX{@*#7kH)WQLeE$bT2r8GD#34S<?Q@v+>M*O6L;~F2PysbutC9w1vHM
znD1xl*mhPmNdQi$y<7j0tq~M`oB0-aXiV1a-qf%_uUX}R5WE1GJD8{mHx%TEULY*1
z&P5hakN201d|l~K$z(^pS$vnwPRW&wii^P5Vbx*hP%R;p42m3b@W&VjOj!?Yo_EuZ
zt9!D#x=uOD@6NUdx3*w3MU`{IiIp7^^5^oO92RV;w$)bHaS##C?R`(|2#VR6&tV`X
zr`kLrOi17~pB!nalAx&xa2CCKtvjtsh<ZgniidA_^yzwll7x{+d10{zGM2ww`o&G(
zKrT{NV_cQE5}wB@FAwsYwr()Irs&C37cigxx#y+o#i6HVpv!w@&}gn5_mjwJi&4(m
zH1}0^%Yx;4o~<NdVD;FT`pGi4MJ*zWUNeC<2?PY*^fOO~j(gj9sMF=Li_x}pldMB`
z&5Gkq)sjD;rX<Xg%Q1czx4uk&dAqjK;Z*O_r%Of^x{_yp3_1`P*@|XjG7q*n6IlCI
zP0e(EvpKn}GJp-VO$NZB8KU5;HyWQIx&@BkSGADU-#N5ZDvRVr{cVF}kH%H={0P)e
zTdak4>PgPuTt!-0O0AKVHS%rZE~xgUJH&{tmk?$tMTCTLMbPHYT3fkWx-Qy&_9}1Q
zq(sR9pg9MpWb(eeFFNHIa}fQ6KiWtoYhHf8-Jph44tjN;>=Sxi!|Tr%H>@?>G*3*z
zL}^ae7MAAhVQ+su5-5X4z#fKXg$!tVccogtGc{y6+}I@n4y0|<K%y}*(%BB0eIf{S
zzzRUbdC@>@mGk55*+x)~_!MWiyy($y+`W3@)|>KxuRhBKGP-aX5Y}+$XXTq%WAUAV
zGYhe^Q2MZ{KDhPrODO}WYN+Y~+V6`><H38n+}w47tx!pv{FFW>Ngz{$Y7*uBAcGyZ
z7~N+K|E$}R+scEDC%HXKBzi166zpReyifUaZ6g!-y-DOcEPYWfj%d3koQxI|=5U`+
z@$<eg@mz2&2d*yzc+(7#Sjd{8LVZ}`kszLVJ5^jdY-Ud8#BetIrC8E+4Y<B-g}2N*
zYNu=3$12J&ZgOxW{Dcq%SC7?u_L|Q^vRIu~YBrRXSjM<}NRuW|;<4Xbxs@9!Zr{nG
zJCCV1v&$Pb@f(NN-$Br&p%+Fi2<_WkYiELwHV@T=ymv8eWXiA@)FJ`sb`&BJq@=P5
zq@sbpE(ju(fpbxie8IN}eoKol6YS*mE7ovy14fze6Np!YvxdvhB}qHyQzQZVHDv6q
zY+3n&NQYz49hTpRtga*5SdnVzHM*h3_7em{Yu-#gpCY|8!5s5%%k1v+MBG2hO8~K#
z_P!NO)KhjzAz0p5F0S9eXSjw%v5MNWW%WEVCy$<3lJKuF0PK<kLXg!!a4nmq-#GDb
z@-{tN9aAl+v;vq8lugsZT&YSkIz$n1%VJpeF(7u~UC@cqd~DL4FD81X?y6Y2edrR{
z^j57w`H^>~npiS-`4hs|tNXFsi1<Nb&8PHUA&*CUzo0pGXSc|`ePe60EShp;zM_*=
z)KZPeN~NermTNMrF_1M_2sMy?ZL9IOTG}#k5z3oG;mODeI~g`pRvtLAZsy7o)VS_&
z82TjBwjP-3b9C=CNKwx$7f2hMzyWK`?_=zij~EAMyOzWsmSnJ`IIl3%60fIagd?dm
zkI$;r2MBlWD){HT8V2MPt!PsgvD09!q6Iix^B9cl2i0NDbCl>90uH5wX`MfRM59VI
zgZOOKtdor{mbia=T4;gsfYA+D(IiT-6JedqpXAQ28s-PYMhDkQ;o4gI>tpr&%E>>|
zlY8dnQa=g@9+WDu!XGE=vP4<vBF)QEI9Q?e%3<{G(bile1eK)r%Qxo6Z{Okgh_RRT
z4O7ZHA_++Xm#GEbJSxWf@MRi?6!L<4v%)Dq<9Qu{uniU~s?P^MUSQbtzO(Bk-E0%_
z^&p!-e|3)c_DC~Q$)%uyM%taLVyr*ko*f+H!qiGJ(Gd@OX}{C_zULBmuw8~_HiIiv
zY^n1l;SRbsDV(8^4O}2ZY_EImBt*_JIU)mgW1o#73-I&0hi1DrrwW(7k<0e##r0=%
zhVS`wmC*U)1VLG`i97q6bHq+;aTF{sUYFa2)p}g*dgvv9*3Jb?;v1*EowfM)J|iUt
zy;*fyrekTByO@zAX@j~Xa$_x#?4&3wAB?3jh21z9d#AY-U&inmV_3f4)QC#0rB}>3
z^49a1p2>2O&maVplEawGUEo}0N2vGODpHg%XEcNy<o~X){2g=xSBPayDbv>kMpo_|
zNsl{<5}MLtJbi1yXFi_HXKudUJK*dEL0{mzi_n87F>}XcsrsR2tQeQ_G;N(LL`q%R
z342dd(q`8{ptpL4vQA(1rrq)^cRs14GZKAhZAlN4M#x=-=y=*D#t^rq>LxE~OwCmw
z=W;>vNj@4d906_JybfyuK0A}q%D;9{ON&GfK2=u@)4f`!41r-7qvZwr<-^0rO7CYq
zx)nY6j8Ziyia@X8v^7<CtRDledV}P@BqXdKg|0Zi3KjFdOU5w_kCeB1bwwZ5ipiy|
zlp@jQbk2n3AdtB0YBemi;*(|jyI~C}fIc5%0IRoW@?iDMY9VpCu^P@$B}oJjduAiQ
z6CE<us_IHb<Mo%2Hqy_)5(Z+P9SW^8f3Dd5g!7{9Q-LpDZhanLR|I?#>tJ<FGEimc
zfdEDGN!X7!E$)qirtD%kMNkC2@62E<X9d(uFeU6p&4rH(t#Bcqpbn89C?d78P!@pu
zIiNKq02w8K-YwE~%io=uC?2-L;sdI>h=I;m;WY?ORJWy-qm#S;cw3Juh#HIz_Lx&H
z&=$7#ruoz|ZVm-Yxm^I&ez@c#Zg0`Rj#QIZEWv3EZiAOvkk{ag=QaXw&%`vE(EHCY
z>vG{JW_pszQ{*fhfu{56jtP#L-J&|qBM|^?hG3<BX(m-rY&Ew<X}dTi?d%pTwkWb{
zhE&`zswYMx)1a>P7JO}}B4*yp_s_ja5{-^sB)>AHhPzpNfxl-;g?S^P`HRQ=o+)K-
zQBQ;gsW|d0Qwk<spp$W#WgEuZ5bMm#udHD5toLPv++GsKPY7AAOrP1*zm!3R<}fN_
z4ssO4r|Mtn3ArdxvoYHA<`^9WD5F(4g=AfUIZ`Y;`QR{RJS+4ZY#@T>O?{K@W6Aet
zV|MtTjFhtL@4oQf3D!A;pW=47qzGG%!p8#bQw*&}b^y}SGn+(qBF&N!_+<S>(**GH
zhRf>F*X5gTS4idd>ab5(;G*R<lKa=K&L5p*J@+wHj1&{Usxl5-CQRm5kOuZPq9@S$
z$?^tQFIPGHdFx{}dCeCiJUiAj2tqC%lXP7K-&K3db>4@drWuV|a&@J~HQmG44NAO@
z|AM8+K`~nGFr&uM4^j|^caSu<nXukzDzJeaCOed1S5WjOPfJRxJ~ogA4obV`Xa(e{
z!$>zYO)VVcXs*e63@#&J_2c2e!C}RC_%tp{fx3OC63HM9++MeiT;aV!jON7DB+uQh
zFXQK!W!3DiDmSp~eWO}`;_cG$6GHCErDI*;zT9>*KF~c1tGu(BDvE}SvStQ%75t6|
z`=ud`OxBT<=@M3D`i+pJ*WEfNi%4e^38idtCSI!pY*smYH_aH@Nf`6G3=2_Wiaooo
zruZD*jiih0t$gK+R&5?eUU*$DwhbEWiQiez{Hb9<y*i32Ir3(H+JJa6+Mx5qjT4gU
zxk_7L3RWXVo6oS@YrH6MrFK)O!IT;vzcqr?l{pf5m_jp;TGuqZxDaJ8h4K^YVkOv(
z2>uzs{mUwxe>uel%c|=i%zA9dd9iyMVADYE)QzKDiAcpeAXtasIe7!kzKGYDtV1gk
zwuGVMQR31#*~a|E7DT)95@$ZUmLc4_WsWXDXWf~jl}!G`D_x;^q^K2!;>sd{xh+JQ
zi0|=Mlj@mj&K6|w5*CO<!#Ydzo$`h1-@ZTf+nuh62{~oU2UmTpoSS4M`MhckoR!l0
zB-o;B2z>hB^%LUJ>%;S~N;lVOuRT@F5mSP3fR<LQ$#~j}JSZDgZJXIqrH`unR`MRL
zE{Gm4t_Nyjhd8!i!!3!#PCZygJ@{t_Df9Uu6$;*c%G_uCw!D%?mq9msmngwZw7Ck`
zzifar<XS=MyTz48$B<b!w2lyO<O}dmcv-8>v?D&qK${y_sGU}G=FCjW5({5N0K5L{
zl*(A&KG6i9Qnr&yN*FXowq57>kr&lvWW$Ot-1W(tLVDfEs`D2%0=J=#Nv=Mxi^*_r
zK2Y9vERLy7g9%f$XQusn?QnjZvK_A+q!un@4j=M_PL<=<aMn;wjXGU=mXMm3ns48N
z^T0D~aZod!2E$2h4aRF!JsFd-q)~c@2qrQI>WSfZt@OXFwXp^kDmjipD@0LpeZ<8o
zHD)BC>4Kf=wrwgrxO~dnvYIYqm99;~YtW*<P_Y+{Ryo>Z3)nf{OwOY^D&WjC%FTr@
zFoh->rbG!Wb>f~AHl_`gSf{3mp^<;v`*FeJ*&cl!88v)L-?a{27^~hq6CBa_Oc8yD
z6J+B}oz&Yu5v5p=5f|<#O@zIaBV%GjM^N!KA1Sq+%*jn1^|pWnsrevSu?m5gQXcoN
zCld~)-=n%;6)lpqTKWT`(UP{-xmh?FUnDKA*#*y4m`c%k5qhEq)ce9BzRl|wHj6>H
zyjQ4|9>=f$nghoU24wtkOSe>O<O%Ku;j%vZ!wI!}UHyv_BRZK_=l4d{GqknaO0?H6
zle=@IpICX{_oc6b#%biKE?|;>HIgaM+Y<=cv%E*@4{SSz!Je!<mn-Z@C<CuXb9Ob(
zarfEE7S7j}A;&<R{*)ZP-V@Vn=#>~`VFo6i_D(ca?G?5W)G3<Ne!!`NJ;eJLeo%dO
z!K&>VmVsjzm_7FwAFCOX{FsI-Vz!Q1u?G~P%CtZgB^uaFMj)YuvKbQsE}F?`HpOJJ
zUIWp{jYb<1$j2V@PZQ9Ok6@Z97#4be?UQts_;KqvqvGPVQ2$pFTo^UU^g2FcX~8Ia
z0tLhrY@UV7)HfA?aH#P3F&Ag;@GvG_%tJYLzqwb~3|GEoX?her3z5d2AKkne1^_L)
z!9v=`@n;nNKmk3<{x7R5??9Kxk1>y;_53?1^R)2o=f0=c9W^?w605BjS6v-~;L`Gk
zrCLvIg_%fqzW}D-TISzRUU(JWKYRpq!yTzAb#nz#a_p)CUHj#&KW(d;$09~A6KNI?
zeQbM7Fb{v}Ytm<#L?<THf~yHiBr<plF^XF`z{*r$(Eu4ayv0SW@%;h5ci(80eTy~3
zZpM*N+(dLG_g#}sQQBmzWYtRTICD)F`DZZ2@ecmFjp5&WWW$=6V1bNl;1p(<VbJ*n
zc6+5M(DQf*w@<n}qI4~%bh=qodz6Oufm434noLC|@B{EF4wHR#5F9=`+O?LPJ>CmB
zf!3dI<&Fu7m6RXXi%i(=|Ez{%+R%T*#>q!iCUUc-ozT9HLaQ^djvaeLLSC)b^uB<|
zm2-cmzFr56nw_tE-nlHsvUQ2N2a4@?$oLsY$lzNH;mXKoxZ9ttw8uRKi6D32Xhw|Q
zZXPR=jgpw4nw$GVq$>D99FqjU?cjEtXLR=fMfVfg3E;htkg`wH#Yz}Rzwp4n=H}xF
zmwjm*!bQQ#F|0VcMs2D<X~{(<nR#k)s_~|&-=!$(`84r?p}AlXZu2i_ZlxK*2=AP=
z^U!fE?x}E>Ypl^&bP8^6T)ZNp=gbzKA#6<oz2N~)xQNR*V~S*-1%WT`dS`mxG{%iE
zZh-4*+}m%NfKT0G^-7Qf=s&8Wj0BOp!c$Wu>wKv+yFrx=a1?=ac0x*_mtk7AClB?7
zK4P#`@bhK5|2jS#ihsG3a(M>^8GsSL&MM%$WV-WFLDJ-)@^sg5D`2;!XQf(C9rq;`
z#{k!<ZH$2%ncVr{9Hsx;*<$4q!0C>E`-PLeey934T*hL++Qw&|m#Tlg(vSE23L(^>
z0(Xx3(oO`upQEo5YUP_IF034FiL54*SbL%RZWcuFt!r@g0IY!Du16CC6`0lyPAOS4
zK6Ud*L<85cc)xBgVExeL8pEB(YwO)1&0G?CYmyV+-T?O&n!MT<ZRnI9;ynXjfIUjf
zzQFSlfXeH|XDi~_dHM;#G5^}QQ=OMY06=5#U>apl&*!#^t~3BT4UQ?K$xaO2_s<Os
z;2I_|q`_&lw)0oLO<gXKPp1v1_;MU*1>zY4sCF|iJ=Sx_fL!LpQe56DK1IG+c?t>K
zQj~mAlRinrSftAJotE*nEoUEYUP}>{%k$Ow=gpiwTy;&ZNnOp@*SM>mw}@BCGW4nh
zbs<X;&JkCmbBI>Q9>e>`YAfp=M7{%OT|R&E^07wNd9Mqp+7FbeVp$M$w*-f{9<iex
z(bO<h{Of8qvV+hdWgt{^OJ<e%DVthBwXu3Yze_xZVc0SFW|*5w2Oq%|n2S(RQr~g3
z<o0~pzo6&A1g=#NI96yb!d)}=X*dGpu1P#zAPhPpD$f*HhRVKj2ljugZ~L-#yX^^A
z22&|nm|`gFSbalN6STxWMA8O5C1e`3i_Rga7q=6D2aBh#ly_U49-;t{r(?Ct9MVQ-
zFqa%tv>!&{%6=t@#MPCy25Z=MMEqa24Jg(rNVXPH3%~0!utHyAlnsdmnldCOmQZ=P
znLA?9H-V1W7eLnReL$ceM=F#q)BB26Op#&(O$|<TLEuSg+FEMtlttXLr`(m7iW>xd
zxp;Be)WB*QLx3hEkw>KDz;ydjy-uG401}E^Ur_JuL+-owAz1qKr|mHPr!PWB)B+B)
zr1uxE!tUEl@#|U=w%ru9f$C}AjwN$tH(pIKIJO}7bY0Tfsz9KSLSx2~Vse8MDNQF7
zHxLGW2tT28@niOE5RK&=etKQ|4vX<XUMz#dO#Ap$&N8eKV$8c!ajOB|h633o%AU3f
zbikWr;U?heVe+45>d!s;EdPUfCf)r2)!paF1;?*x`yjVp<YP%NsDJQFwyh#2&9a_0
zzHfRK6m2A2S~O-sulfs~!QjSjQf!2!F;$iS@~#(VH(M|I3$LWWwXX^Z<uq$G<a!Fw
zs^`Y}#DD|_9PMhQn@fau@TuGB?>PsnT4ShhjV#f-Ne*X9RPf$A2<f>tb5-dxn7$f6
z?mn~A%CPABLt*%fUG+?r_+LEvXC%dMwLAZ)ul|3>1z-c$+2`P5BLRZ{vT?AJuz~ps
z9AIDpke!tYzybWPU;qv#_P>Dv{$9-Ve+|pvU}Itja*_bSNQGZz`~kpUlnQQePk~JA
z0JeYZ+5d}D_!}7DxALa{+aY5i0T)XJH>hX~aw5@UXJ-SG3cy1J01p)a%+25ccjy;J
z157dia{hD2K;}-yO#cNz!Os4y(&{gMz;9Jy6-j>M7J#`NEZ|bCTr4b1U{rvOvz3(&
zynq0Sv5gV<h2S~aIsTh0$8WN1V0-`z7;x}cVOFl^6ZW@;e-{RD0hl<s{wDi-4cb4(
z^nZaK01qL{ufBs{Q|vj2fLV;21^nn&@!4OGxLLu3lHaS-{vA=k4z6JP`wxFTlXtKM
zDH((3h=f^QM2v)4+1S<T7s%ir5CzXm+x~$l_%*A){ex!~1EWoh9l)(y88{h>7=vt$
zjKO0L@95-UY+w!Vy40t+`aZ1@{lkRbrnqN=>Ld>OL!!y*48M8o-1yk_@hXh3L^mV!
z{F}JP8{T_}m!z_f)%yasB!28{_+yVI?6JI!1dV>vxhS={H^kM1`Z#wLQ82a3AJ52r
z58k|UbvI&3LwRy|m1D-0lW8Yg(+40jqz+Nt>l>hG90~d`AwV2qDlQns673A*Dic7u
zp5$j||80K#R3CX=07YA5bJp*)36~{2MhdqsuIt;^qnKr^2-4wr63a<kaYZ;wQG!So
z7aej<VW#M*(Ceq%^He1#81gOBPg>~nHwy56G|oaYWU^<YG>iC@J_+ImRYcz$-R${Z
z%RLOT;S?vH+>S?k<D+jH<?mvs2VQ;cU@|e!dtxnYgh{b}07}JLz014G>NS5?NDcd1
zpCst+M}@N<?a2>u{S{PWf1R=bJOvzs2~M&u`ANLF%o1}|Hm<aEy&sF53Ry#3mCZkF
z@({bpp0F@uX8Fpb9bsWuDT-+a%<pa?jf<^?-V^MOY{Dv|1yn1lnc=P`(pC*mk%S=R
z7Zpk}uS9%#y+9AjnsPP3Y7X{o)fNq-Va!7Cz6o7}eXDH7n;m<c*Kjn%O=+u`hW-t!
z-rzh%lX%{q2WFXMT)5JWjo5ANr_%dy66j0>;jW=ugkj4rC@M&Uw=hI8gSk!^y7Ik6
zYPq6*ulj;-*RCnpzc^vX0w{{mf)q?l&Uam1Tpl;MXFh6?lHSDW_!MYY=N_NT4s^rF
zQClz8$4qkK;#GQGNwK`bWsKb@nD1wat?DHRN4z1xn-jyfe00n(&eFG6=`~1sRWfmy
zaiF{+UZAko5VLyy5zs}~IWt}tOe9`Hu#gFZ+8taYCKAl@iCN8FbLINph(C#IEQ}_Z
zK=8nH*l2npi5bIqEcgO@BKy?_S_dR=TV_xGjDZu%#A0eM^u(qjgs;{*N|HoMsotkp
zE^E4>90UA-s^f(rM-Y}Ac7u~h-HY>rnUKMie(}BZ$Lq8MptYa2uBb9w<HWZRdQ^jY
z%xPBzA^h{)`AbqHl8CSE_HSv+BI#3f<eO)G-ErLz&+4<5V;AbO-h{DQ#(t1yj*{zV
z7Ty%7(V|(Ax%Q^98meFz?TZ)<WASNv;NmQIj?I5cp%|&dGAS-(&2^j29~^Zg+V7H4
zoDRF63?-Lyw;dvdqp1n3XIB@w1Hk4=(w#ydtU$irnMfW2e%VE_M_VJ9*q9p9Cz@gR
zqhO67b7zo7Fx&5X{VE^^+SF>T!VBq1gtqr*>WU2NldHU5gyZ;_R(?}fu<aJ~ieo~D
zkmmBfV|*x8_;_KGFm3uasXr7*OD7||1kI!*6AGtA|ITv3BDPkCZ}NMz)17W+BV&k6
zCTtzw{P@$vNZjJZ=jn^G0X)iMd%pIhTY+PuycLDVNVfYd*Euxe^hu?5C0_E}6`ov+
z@{E{SUZN{e?p$|Zx&q3W^!yUutd#ZTeJRlGyH~bV8_>ff*N<h=fCUmau60`x38%`V
zx}v%R9mH~tHc)>hsGHe|OPaAL?@L{-Ebsvb%PWw%GuY_0!<xnUs|IgK%r;>_7avNO
z`w8emD{evmVstxtXVICf<14E(^fm2*L2@@X7EAruH*$)fpWJVSd@g8~rSxLSy80q>
z-fK(W5BB9THiy3V=iJViC~<`;7i+zmXDV2_D0eGJabtpna9EYxA~<1iwkc@Q^6jc5
zk+q`uHcyoOldsrCb?L($hS3tjk^L+C^SZ==zJ_;(e2z92<O_Kplv|qJQ{_z&Xs&hC
z_e2|5PF$;{zm0QzY1#lP81jB1C44*Ak~Co@Wf(UB!3h7w`f_pT?qD&{^MoXv)Si67
zmaYBv+|6?eBQ2O7jp2%jhdlXBhmI&tMxFGV3fkoy`i{-Ar4RRW#xx1#Jn3aRVmCvY
z{G*?;m@$-7!TDn<7%zzDHi#LLBnT~(fHMYXDIB*&Ld3#MCyk28A4djXo;qE1&@dd5
zkxAun-uGVb64Jdgd@z;;58wtyD0Z=8lwdz`h3tbi<jg=@o5&5t=-Rr1)5U11GFBnw
zeQj(u?G#>KM9Hwj3ogR3vCm};iN*%)Wz$~YjddX_Dh~O-%}a%qXeiQ=d-Sn4vV=@s
z+etLhJ)Ef=ET{HTO>clRvNtXdFE5u}dH4l>dfRifv>v`>DFK=peKJRdziVnaV&{Q{
zeh@w0yk;*;+1Yg{CZ{QD(QrO|b)b|mP*629=}4c#8LI=rECa$<C993>6J-I`jX#UR
zy<}M%VQR>$8e;Il1l@igy+Y-)-dfypbFjbrvApw5$rdpnJ>(>pC8xn~EfW4HP(Ivq
z3eDT^Mc|^MZJO%EYFmV;xoU5=Xl1A7W&|z$dY`$b7RaRqL;2u@dYv*9ZGBcHjbGZq
z5}m+Ganf)q(09A`X!~qBiLJ$tr6ENp>|)0gkVg1PreIsZu9pXb$d~)0hDY+}6g|0n
z7m&ZOA{??4YD4oQP0O&r(I(w!!xM=%<%{iOhKt)Htgmihh#v!+F$=DXr$;gk7t~Hz
z%kVn)IRq@ZfXzsyJj6Ufmpmy1n#s?C%(pd6lmd&k_V`r0d?h>-*C#@|?bn_`sG)f!
zHtd!k?PH1>1Dj;6h@G!rmLY{28S_kxA*k@pu1a%Yu89ba?6G3)@c^((AucU>2Thie
zmwRY`&@pXw#=3r?esqr&L{#q@;iDT(hH)I~Y^6KKdg+Xjol=lALdQoCjG*jrT?A~q
zFCqsP8p}B=C(hi>mt^`o44zy=wlUILb@P$zYJ6!w^dlUFyVUkX&0KjI6wR1fE4SY7
zrA=vt+ty>3fDWfu*XZ5EllGm9zpJW)1ZGnbRxJH<Q>N?_o4^@pL}gR;)Bv~F(SYMU
zW^0^n5qi=$P2Z!}ybVyKab~bBlsG=j_pE9#Gwhn8#p5h|+x2gCy0L(5=jkl(X0n|O
zu8=p=`#W-G6m^WT16iQH!L(R*MKf|N^2~baW29sGNVk>%vKVACONdH~n#~lbM77V(
zf3}e$9Xr;o8(cJC0=Yhu?W%Hbi=7F)QaV~?EMT4+IKCSoy}d`Y?_TyaNOA+D$TE$4
zv?2kqOPE?8B+=?|RurxyYd?;&oDAwx-k|XC%@WPijLvaT?=i@jlZ88KHRfC&voM|F
zpzZt^Buc3}eTbMbyz@sCi{4`)Eg|?Yx^~;*dv0m-Fcv6apVR8Pd8oVJy}oBbps{WV
zR~sO!M)5`@mX<_b#w*8Mw<KtP<&_&@DrY{nL_Iux<Wp^hZx3-oB*ZwTr2S2x?>Nm2
zr&rKh4sTKVT>3HbU3!&{wOZwz4f)oSQ)IJI@!s843lFq%8$Xts&mATWYsKh^RYgfB
z{S0Y5G8@-lEQxkE>43E*!;&|9r-svH5D+64CrTb2RPGEpPP(h!;n+nlT~A7j7u1Ry
znu*A}#L8A4@m7AkEU6h=`X|uCb4J*|fF6ERW&aF%;QXZq!!yem8<`sj*}9Tw{ZfTF
zxH*^rV5}Dx*i8Ek*(Yz{U~J<AR)&8yqWIiA;Q00XKeY7UH2mLw^1Erl&BX*@W&1bN
z0!*>{Z?gX#i3Zl;|4X~S%l@L#fnk1swMsbIf4%e{eg6M!Sv)fs{%lzMtBLR@dy0eY
zS>u102CQJH-~Z4w7}JpYMK5cgsN66>QKs}gr+8yar&4?EIzl@j3mBeLHFn&1hZ|-b
z8Y?k;`S@_s?}tt~9!{*3#t--EZ5FZKhgGD>9UY;#54w*aZ&r^X>28`K^YPd_>+WJw
zv09O|^z(4)E8|P?druWB-eu_mvgu-~+q8V<V{veCwDZoX+fx;<mXuD%TBc?PrH7#$
z-FxOt+qvZ$x}*2rPbb*(b#a3Gf;!q`%mUghsG(F0tfBHTez37SO%m8gVS-P2L+EQi
z;^oQoEzVRcFotlmwYwW7E1138vI;|4W9j=8svC=yd$UT4lBi;<hT>Tzr|&5Ci>+I9
zTWySgY}~zHcf0$^frhDUt-yxreywOaP>?$m&qj8&V*BKw;}S<ZVL-jm@LiY%h0`2k
z@8T6<BZg~cK5@^fjf;YR7t&+$O#f>aGdXRYIqHWMn-a>#?xDo#k6LW9uQ*9^<4#~Y
z`Vn_HMewoI;+>bVR5)}Jb03V!m>w_M%Z;>Ik6JZM!%kdAZInp4UtGk|#`!y*bX*oG
zKoFa<ehUDYxA^gY!B3kDyjku4d9B**JXUdCt6ye@c7AzLExk*jq!EAVtZ^m~5kgJF
z+NuWKS(z-$TnZ#4NDmWssu0`X5(ZaHfy~jT8((Oq4;!8+KUw<Vy#I+_tMxtUi@8DC
zWL9;`s{vkVSklP40GP`1XmhtCYE(234}qjve-0BdHI-c0p^lPoxj4I8x_tk(MIBqH
z-W|EBC~tJH7}_D6)Fzfx3#6Eev3=_|OU}3Vyx$H?=5OVeS{iDPe8>~bVl5AK@+gNO
za3OQJM6d`jl+hWvHojJczxob?2_fAVoYRPI_R8}x@u{KoXgX^m8d?2N+ru`G{^yU<
ztho2v=uR&9oLv}cAHzhrUev8|d)q0wj^B76vl^zo!L;lEdQz}-)8Ck!iHaRLyDSSh
zGT<LD8~Aq8N5^8kMiPgXLdADLMb_uv7}&6qn~mvJXwmwBzk!{OpVvGg5N<+oW^ip4
zPFi3v7LONuL9%Vg-OnM7VRAdIfD=>}D=4XV7~k$Gzef^oC|sh6b9NG9FNe#7`+gXT
zB0F074u^Z^bMPnFG?>v(v}8t4Rd+q9AMb_`<wMOapCHbo1RLqFONW#Jku{CcH*yh3
z%KI$g--2r&5}*h<Z%L*A46E3TEZjbN@R?wMNI+dAf?*Ed!&D#SIMZWnKXC-L1xa!~
z0^(=Xx^wy@C_A*~DCF1kj_Pf2z&869!k84lSRG_9j#K#yQRMHlatZ;BdS%C3ZGv@`
zXvpAO>umZq(QF%w`-oAIK$7p__uQN!2gi<0diZ3>r30+J9R4iwM{vREbj+M%6C@^%
zH#}5z6Wxk=!nPrfgqsgUNaNUFJxo#9OrrRghJgGY%}WQJb8R;RS5Wi*mS|I=Sq~$-
zH51sj$4|Ajec4P_ClXX#<j$KCZE{5lhVu4!SZDGV(b}QLUrJ!%xtqovMwO^TESFCj
z#9@S)eAC-u4}Sy!G!|L?^l80|=rc12;VdKH7IYD(BatID%%PxH5MK|KRg0R!_(CIu
zu!gMqdM%b4zG}Y5(DTMec$xS%hRjKiiqw5V6Tb);97~5rN1k3B<c|L~Z}2{{-x)v+
zRjg0(GH**aKA;<Qm+{C^e{~w=jXfNkj|s~lEXJK)y4IN`5<r-JK4|ITZC;~qCY?CF
zvKN-3?rfOF=U$u8t1qfu`o>`%qp+{(u#P9EL<DryoLO`Zj5E9z+S^*&n=|`*k@hG`
z?HvKZtqf<MY52Z_UM9BOy>(!iUmmU?J%dQhgU^b^Q|3iBWpQpP#mN@(FXUQ2C5-7Q
zIwwA6_0h(8kMo(XyC5<{U*e_o)XYipl(sNVo;%7|WySvfCS^=En;B*i)gFSMrE5B-
zFD?oH3a$SfbJG008nc*|urThKsN<MgRXk@Ygrg(yctgmAt$^7}EM65FJ<3zv6q#k9
zIe8xvz2%mwoLgLSXjNAzsQ>HkZt48ZVY!3A7lLmWu&np*MD_H$I@Mg%_Y|=0-;3qZ
z_|HYTrqba+$z|8)_3tKroMUF%PXa+~zq)@a<i^&Ip=ag09$RKL#`1{~j0ns3H5AKe
zzCAg!?dKm28JdX+;?Ghb>JQ>WI|D6U7QjQPg9ML<`vY@&y$`LFaI(#v>Tl1XH`@=F
z6D*@*+V^cI?v9V%{dA%Eep6k!_Kj1HT2r4pU{{SGwA4>lFFDYBuw(Bh$<J}aBqs|-
zGq*lcwhR{``7sE!YTE;AX3(8`(c^ag&_thylP^EhSg3jrO6l@OeL}f!)RL%@O$%$p
zd8`)wRSFhOq9h(g5^Ydpf*urWW%eXY<Tof9#Cu50{<cIaVTAD2fHzXH7st#Uh$$aI
z$JX4s>%5)c4!U9lWTXs-jNrd(d{uq3T7yt0M2WgJ^ut!<4Y8=oq6_HqZK1nU;oT8)
z1wOt+(gr_GYf9JCxF@K=|IxH=S!#e_z$3P9D>|=lny@rpYRV5(`E-)N@8`OAkL2!|
z5;>3PMNa{$b*-f@#r^P9@gw6z%!X{3i9PQ+PUpCJtDq(ddk1e4?;+9m(e2|T0}{f#
zK;Xk*7asA`X-3Dv$4MgdrgWBdqDp=Fxm<e+(zxBlpO-(QuW;Ru8zNp9VVz}rZA*P^
zHw;4Mxf`L-l|k$&XaJE-8XUpYjAmeAsU56iN4u>Od*6x-9v76E3a-3R+Ds+>DPXs;
zp!Xgz_O0x9VY`6_4Mi_V*93?G3l_gZG=@0bwV!(yc0VSHUZm}5Q0Y`d8t3*NeGtBK
zW!p@}Dq?N6d9VU5zeK(ExMbCKMT@dzPuLNs83@wBU`pCJLjO6w<Mznw2z!&eV-hHX
zZAUlYfhFvZSfQ{tWAYQKu9foTKPaFa&waiBtbqQe`+&90XQVNX|69@+`!6L@@y}$j
z-~R6p1@Hedg2Bzg#0A!2f!qKlE&$k9%+1Nf0ak-KS$;vb+5a(4@Xo~6*2%^e?2~8x
z8`~Hh1o@Bd`Ac8^ZP>t$Xmt%uu)h7@T=A@6|M7p3{oR|+2@V%<ar|B9<@l=`|4;4y
zE(_#hVFD9*|E4c<{8dl?Cs{U5CN3~7`0v8cmF>U1@b3+C{CaV4Fy`;V&yAXY69!|?
zIoP@XLHM~7{$GUI!QJKt2dMtmGVrhV#Xr0=8`w<Z01wXJg?}yTKi1#Bv<zV50xu8N
zzYG7iBmNNnpI9e<3TJ?ADgfY@iSaA8@#nAycv%1dzc%UbkDkXz{$kT`fdh5FZ>#^A
zO#^(sxc-k#!^-|YCo<2fNkuIKQQIHXzGCC4+uy~~<eU4jzYBdO^U*}-Tl7v={nvU3
z4HpW$@uvr5?mQx0?nU5>&`c49+)d6(+d%e41hey_t)$tldk(3WViMpwv1P!wiD=^@
zS&FVyeCk+TLGox4i81e#Z%uVvOcMM{jr)aGnKTv>zOP<RAD(Q@T3onwy<tUK$P$lb
zkn;O(bFqnm1b=%C%o6FG8<ISQ{LM~jhPT1~!0?ruORR=#4`c_RD*kxXbz$6#n-7f%
zBaa=^w`|SP?UnRi#XBs`_E45H&9fZo@MxY6Z&}VSn2oF!u|Bse{i^C^Te(K4-AmQ8
zB<+BRhdSpHh#vJMMgPfH%9<)?m-%mv^heX_yWoe~tQLjqV9KuWar9rv34d-`BtC*n
zGL9H5Yr8F{bMk{R#*k-_eMc5Acz`#YhrvP{Q%qP@r=zQ)pfG+Y<&Wd(EunHuS;31q
z{++2~C+J--{1&5M@JI2+ZemAJ5h~eelKuyfFP+f>5ef{8TEGLgi~3iiW_|$=)0Ct=
z^=&zESY2yR(7L?Iy0z)cK}FfdL!`&@vM9a1HwY3&#HN1EvS2!EpO^nnx+6%3;GjYm
z;A=qaH~f%k&SI#}jA-@#4OU4C=luI8^?8)qbu7zy-SJ!8pC6QRTdwCO+R|F3GH`Tq
zG~SbuCo?*BVpX7gy*Kf5caa^LgZh71d*|>zzij_IPSY5TZQHhOr?G7}Mq@i^Y}-y_
z+h}Yzd@`LgXWH*{=Jfo|lRtA^>)QKs?;Ef6UhlOxaCJ?!j&$&BZ}oK0ZP7P)82~^;
zkX*nFTuQ`uQPCk`7u9M0l2MWdYxXQyZ+n*GzzY?=9hWzr^i?%N--1fb`s!h<3Fy6G
z1xM|DyG&nlN-$D`5`0x0sfhlOUcIW%o9ARyJsd})1oJEFW4Rla>r1Ik^5%sFI@L>L
z_RX4cq73<gsxvXRn56&ls|Wj$5eb761L67SVs+0v<S<&tWACMeqZ9fVPME7~fGv|%
z38l?!t~KH9%ETq{k6oxf5@kMQ5Ei)D$?gz487N#ktH{k&dZzc}cl%+f=NrQMCwLiS
zfZUc$xTdnXwX*;|K*HjMN%SXp)*4!k^lLkYd5~cs<h{?@tN!ojNT>Ga_zEFa*17>}
zm4K{9zEGYFGh1J{x1AB^mT3_;`p@-&!YFo*ugGN$H=zhkGHjeZT$_Mlx_On1Q@VGm
z0DXVokzk*8upCvJyzPdPVn)Z6HHI_p+023cG-%I8iaVXx8s}@=T=2Bl1L38bI=0L!
zJ1pRg=PE*VeUJ?L=For>5@fG?ATT^E8S#_w?j9E2OUHKAbc_5<XW{3z8VAnMnLSf<
zT>q`MEu>;ZE|9#F@g=A5`?7vqEAQ|;idYO^K<WePIDBII_?K!_CnD7sioK3SEb_ML
zI$N^vbPO^F=j}t7`MH4^n*v>$n%M3B!I{XD`Mbt-OZ$RChgasPX{nRflnX=@%$Vl0
z1Dk&JhLa=mj#Nv02>4bMr<v_?23W?_s#=`X#kA%y+2q@cH5UTLsWLc-FCi&YR7Qj{
zH5o3(5rHA;Fmk!}A_at4yCC+pav=?XHTyDE{=*DrMObz?d%F}h1%ZQ(Zk+mbK3?DI
zL~82&<OZX_v3<-zEH&1Y?;2<K$3^y&K-&*!Y$l7Y-mkx3M&*<~VoDIp?oFPjc7${%
zco0r#@qaF-TyHd8NVfYXvhZxf0Z%X;<rY@J^^8;w9LhEq{R%r*#XMpNBuI|E8*j=A
zRo8Ql@%bznQRhB_UGl?=Z}qUd@$EY1+i~1E_+%JT(a~}A9;O2=g8|?8z|rKE;Cj)(
zMP7n0;2tsqEg1o0SC64J4GAUkgRQwJmR-SIa`tJg+9Hdq=T0!wYQDJHn+!ECd*sq-
z3DZFp$UI1txVk%a@Gyg|+4FxKGsH#aww1Xbh>28U+w!9u)ga>8-gfy!ILp#Azd12U
zqp}B%LFOK}%2JX!HqDR@bCqVJ%bC!5-<_Ur%l!S|?TyL8Ju}wu8xCatnNohwfj_<%
zzj7e+k8jrhL5O7jE2R9G$A5v<{~$!N{S`BQiT#g4B+DDN{Q9l`O)N7TBQ+!KZ#)if
zpUD3?n2C;>ndvut!~9pi`Q6BGaK!kA-T#6pEPv&j-vl$!Gf}g=EzG|L|CMik6U@lU
zKut^cubvPVmY?&4zroROf`1rFe#1sT?vnov8~sW^|B00T4G#So0I~cAfc}9$xVZnb
z--AI`RE1X7W{;jXC!N3XdKHQa2@3j}5}FeF1OF6K7^UR3q!jcAO353jFscvy@<`cC
zBT$U`0<9a(ngWdg;o5K#zj+Yf3_1V2xqkm3{+Rc#4<gI|^+9~A8~k(Fzpd|&dHhj^
z!}4SB|G2(?xE?=08viY_zol(|D6)sEuG=p&LwTL)Y&Hx!*YJnVRJ5~+rRILdppkA^
zc666D=${ix;1sh;+0}i0004@iQaQNkL*R?@_kqRX4Vg3}6h^TI;gLC8OKVM>{5oxg
z8er#eM;od?A8UpLvIM>L(V+p;(@DDKk&{A2salIC^0n!4qKwBC2aJGhMYY$d>dZ_h
zlDOST2z6>{{4#SbW^ylt#@}9HM@_dMkp`*#>T#vrWvqy7>V_E2Jz9ZFJ=tk&h?C{O
z1wWmb0uuR!%ELA@H?oDQbx>yj@mt@QZ3uWtPCPD@$lCrsD@+%_C1Qbl{WTN(x+7PM
zm1fg=YZ>W`jEoiU1{RlNhYPFCgt<?%Vy59BM@y?PFU8mKC0sZ!v8#h8uY(~_0T_ig
zj2F4f8MliV4t*of8{O+u_jgBuQi=RH8OZ+S!7V-c%a$3*L{uq6qe~Vs#1iHRgrMUi
zgV7N8G$ey+O^c@~rK-@+1Tv)auuBY`1=_f24(S#c3X_+nUE>}?)I%vZfr7AvS%9ei
z;GLNwB&M^@57{HmNaCR2aj^GsPT`)(+rk##`x%uIVxI1_s!L<&x<3(Jy{Bth+;}ww
zy=z*;#q^z6B&JZ9_Qz@W2V`Bmt7XvPnO94ZEDi9L($9MJ8Au_QQEm*9A0yftGmwZ~
zC%F6=VN^@7&3ezh4^q{WRHC8>(<1oAJ|o!?hQll>y`_M%jTxYY#2ga)GqD;LL=`AF
zrI`7l^eSkDYVJ`L(wMhC79@!)?T4?xU(yx}`W8pO?+l&V2{mV$Da>|EJW`T5iH!}Z
zc(28%1j-v~CsXycntWx6vb6Jkx$wB1E5LsQayK6Jp(^!QWTQJw6V-SML%hjCq@l9}
zTCX}lG}+@4s-Pc)nZ)QX{gMtj$4O<0M3u_vOQIG|4|Z)-YTV;nt`x%yTaBPKo(tMT
zXt^;y<!*_tuEU}<l{Cu`k{Xjs#M+0gW(6g-1aiQZLYHKmS}I1%2iIC9f(9gW^`V-3
z6diqZ*SJR!FW$-VxX#r>iT457se`Z}4`)bg7J6ybdiYUm)Lp}SV^~D5)8AgB?>%X*
zNijVRA)HQQ^yM9t9viPvdkJm~7vtbNG)(WEWl=DiXasjrK^|a;S0zzWcn+M2)u*+f
zfEZrDZP7)wPX|)7A3s7<7v5eqr*}w)D%D8aO-pNE)k_d6%``G`3RuOat!7PiiwgAl
zKB*anq0NXbMk>Smo~y4Bfyh;IX95uU!@yg%6Mm;>s5871WzAQ$l?qSSvQ&$n7@gL^
zP@sgSp=<%dJgb6I_cw%dQmt=C=TCXtR%TTx52(o>c|C<D*|iuAuyo$fRav&x`xD7Y
z3m+Ud65)&rLiD!Uj!TuB96vn)uwA$Q4oC(@fGrMN7sb~qMuhjh5?@`=5Q7<Oco&W|
z(oB!)W!WL^Qf-jk-+!8Hr}Q0r%(aSdMhGU5<ntJdEXFRcQi2`ne0Tx2st<ts&Mz{C
zsA%X`<H^5d3n3a6bG|4*5VlU|aGp-!+Zc{t2*(8XmFDE3uBZkb>(m|G0oI7;?vM<_
zXz-iJ@v8z6;}c(J9QUqg9`Ih6vDa`h5~b0>jQ9s9)F@%8^l}`_imvIzQ&1Z7;oh8d
zh&v?}g^*6;#N{teFm3}44!v=4^_Y``B^K+Btg>Nb&?L;jN=U3BmNnh=qx^C~Br~t`
zPJ%NqT`O~kKnQTu7&{EdVSx%L>KrOgMRnGAmP((>_Ex!_a-gs(w=yCUZnq>o^FWtm
z@gJ)w2+l~OzsSt*^GL0F41;UM`k;A*!I*3*mOkzWgH8IBA<Yo%Rj<ry#c}PwXB+pl
zU7-fSy94BgXXS3;=H+b(I5eKV!boYRph_8jf~TMt*WTAn2FhU3@a2F-k#$h$A<>Um
zektE688c6Ai)=vQnATFTVqFuL&EVIxfcK$`$C=>prj7}p_3aw1_X*re4IiaY{S=57
z+T6cRUCH`s)>tdP`|8nQOKw{*YlF*eAtTvHN?UzSpi;K#=x8HpJ$flX&nr?p?wZ{z
zaUHN5c4Z>+;Zv{<sDDHbH?GGA23$r>9N6H@z4K-t?Y;npwX8ltsD8atxxnaT&7^MJ
zyP6j&<>MA84ephv;w~}is4i8U)#7uzYu@iNe%nS+f)gN968#NDr^rmhQ^lpw4rmf7
zO#$5ft-@emFhFfp1~ih0{F<OXC=CSh`SniXfl0j&MP~${(frP}8AyNF(|v{SJr1}Y
zFmq)wdlsM=+La`~0krqfK?gG&RNnp310d5|_pOIELu+(BW;YI7Dz&w?v-z2#?*Nb?
zv@N0-L%4F#QXnAP9~YjLC_nU@i0U=8&c2sAa;=JT)S5-8(~6di%wl<o9xL3_e56?m
z6E2f%)n_HC2y;k^K8%>{uzge8HAf{I2u2|@unsMMfDTzK!!<1QWCKYJH661>a<bca
z4R4jE?FFt~BetiD0pzWuiic<Z(g2xRjMIn?wR!x$#pUT{%jqMXqP$&SXpHpgNNiW|
z_D)>>W^Ei{8;LPAa*1Yqfq5^;XsJ_s0gl<nNJKjCGyt;PNtCNuJvUO}pa4|)aB<c7
z7$1KP`ZSFD>dE?bR+&B=ZD#Nt(+<rAS(Aa>Bi|!Nlg+$V0h?JH_9c#W`jTmja|-ZE
z7d|82#A1Hl&t%6e*dFUDn5dLfQ2EszUla>Um%RA8bFzEJ#=J1`^8ET~{rllcjZR9S
z$vhZ26iT6^2yg`JSao2SqRE3%o%krZ_*~A?&CjeBzS!h7x;S&BdOdws@$`SZ%Qp;+
zaQ1h<CPFkRPQ(X1PlL6I&@_P=jN!eVYi)7A;@Q;G;QSq>{hR{)8%q0E-v-P7&bPt(
zca{AoTQ}=p`Txy={X-f2n-2Y-(<OS^zY_c}vHvk$V)}*fe$e61<NiICnf}cJ|7Ozo
zYu>@|ro;aQE&nx`=}oYI1HHe-ve5l}o&U|iOmBV{+TSSP40L}@cz*MXnP}hC;5Wnm
zUq@#Ar5f>XMrLGsbKLw(C<EOeTxrI)aT(w4hrf=?@YZMXKVB_{w{ii)?~=<u`Na%x
zH4xh0q=$bp>$mO?YKGsLbLf6X_kVM>=xN_n``_LmKc_tZCYX-pEuUul&9r|!Q~xpf
zpb>scZs^~tK>w<t|5;x0X7l;ylKfX*^0%w-r+JB1w1!MKFI>R-oyy2|%);X0i4#q`
zSd@k&(#hu{PB?WRQNX+^^w(Am>i4bbBWVm*Coo-5@7zk$=_li12)~ff;CX^dVQb-c
zu}2P(irRm5ai3_%Sh>Tt#(o$tvt>7EAaggSAHj{<K*(JP@)thCl9WgYhq#=;0Mf-2
zzZp~WO+XMk$Vxo#6%K>JbQsbfB^$SGl%8OU2P26!Ywcm(#r=$czt*LUWIoD6QX<^j
zs)MwAz^Pa_+gf8qHqg;-+-GK;S~0B0ko+O&rL~xOo`1$MeA$fNvu9SGQA3dBm<vyy
z+a%S$k0S{s)YQ5KK;+~_@_V+XA`WEqD?@NeV7JXd+p{ScZ*D2yk$!NlaNY=aF{#JW
zqB)HWvB1ay2!0N9ivCz(jcnrr4KtDh=x<)QZ`GcEuEFnLxIdQZ*R=RYC(GNv{?I7*
zn-}h1pT-~a_@lI#f%&hmfnQ?(bK&oo7MwpSC;i$G^V_DEUmR_$KfhW3QT<_gGY0<n
z`=7;+bk$|Mx9X4QxXONXN|h?YO!#Q*Ni^4KVF-!c(qOLVCsNVwBz#4<+U$<At?CV-
zIEItY?_xUj?v590va-9Z;GnU21W(=WE;kc{T)1)NSkq$$Hw-yMr=xlYKe9{p#7TnF
zq;}nH+`3>!#ISzs3w-@F>43fh^Xl~-dM+QB9>(}et#n@4Z;}uu*2F0B+5X9+MZI)o
z#MR!29X3`C2y&!1exVp|fd@L4G|P=JdCdr!(zQP{Obzks05_tWQ9L{(x`r+#tjC1;
zY|(Bmg-5!ZEO|4b)gSsR>%zIbSbI6r@&dS0mF^VGthPHJv5(&6#_ng!qg7Uyr|Vfz
z)bP~hz5`ZF8T}RO!6Ag>+xz$^8%!D7H3EvH+@`u`%`*Fp#VWpR2bkv%QrX|OX<;%F
zX~XIkyMjluZGfc1wroXd9Tgwb3A^M{$?u4V3@}TB46xHqC@|hbVFwz3eQn{0MM`p<
z;+bi`yxR2v$ST~*zE?UsMRPnUzDR~P(q;cl$ti~rv1fnI9$<d#3r;D5DEewwrB2np
zaIw8Ly)}F?wZ`e)rop}1A|`Y9S*9caBBNIqY6Kn76j3CACyBC)izCthA{>jWvaYm{
zbGe08O5wA1ivNW_fQu<>rc`N&yDg_g`!WTGT8@|Xd4HYBOj6Y@w3l1%1&7?hDK@9J
z)u2LjfYDyzHzsuNs9KlNCVTI^&eHC8_sS=$v?%<|1J2osYY%4)J#v^*VsO!{5yG|z
zeY<=J9$J8wR_;#Pt#q!)0aq&XLG9Q+51Pq`6Aj`E=3rkJ;Hg$hnq!W+k~9Q5!8<<H
zR2yq($@X7jH;hCd^cR0E2Bx5OqCGo4F|FF}8*?pL(nfi+RRfONzf(zD==bIFof0KJ
zq4f5?(Ab`h^T)PmqD;3Ov=f#9*(w{P#sG#uIyO{6P&l_KsS{l1=B5!yj3F_O+RYQG
zA&&zc_&yI1?Bq#aLJ7Rh&&*=o>Z}ApZDaG13TqMKYe+jw=#~yfp|zVHK8w>_#s?#H
znZu`uvH&4RRfdCM>Zj$>Fupj1nA&LXgfQ&6raCU5?7&G*M};)6whiov4=`a=!rpL6
znbk_xUX{8;>mRgU$R9!xWoEkGjv&A>O;7f7DJU~(koCo|HCZORyNOVJ7nq_dOzz<5
z?A))3B0hxxu{x@*=?swLTo24x>D=bH=-b251qmwEjBx@364{Nx0s-W>|2&^W%~pBi
z7IYseY_h+>M@><yTGJR$P1(AXHcT5wOGq?+l0?B$;cKM66yua^7Mq_#q*bZK(jMB7
zKBF!XIgm!MhD~n!5yit6jXaWaJBw$J7`8}79;GAU9maCBQ3d-P76oAURkZQVR@xE$
z?t@g#3eEJz)$O!cDF6q|JN2TrDC+KM?W}dQ^-8A&C0D*PYgx|3Q-j(@RiAu+L{yps
z5}5?7k6D~5G;yfvLKlZF5b#zMk%(H5)|TUvCZ5%^%cin&oMQef=iQk|Uc#|!U_g^}
zqGZ=HiwZpxG(NB`S4=jZ<zrvrq)_^5$O{m*??ANl?eTp+nFg4Pp+Y~}j*y+hY6C)w
zUys!fl*C^bB)mE~MZBHD$^nZ^1t{;wHr$fBOD|7>wDVaEE69AU7kmwYpjPYPzMQoB
z=+Z_sIttjLB9p?RDc@dqYR%N!5}v48vCeye$wCLkX`#Xt;K<O8%kl2Iti+3}8}iXC
zaw&+^*}^QHzFwG-b4%l7b&#V1wn$z0{T>Ht!wOpb0sL9Ym8fP^9}B=Voy{^iKSEkq
zkh>??OYoB~iQ+8Q3=y0D(<p@}5d+S6zJirDzLVrl<Gzd&O8LGgWPs<UKEVge&2tRK
z$IE!;9m7c!yv|G}g!2eLmnU74L}JgRPDZS}39+baS+CM%^xeXNBVmysN-Pf%Z<6-#
zeLOr6^=fry=QBnrpFTpQVa2<`kL`X#sr9HxrJggiH*yV(5cF2!TndG3j_*E2DML2x
zrkaDE@{oCG$hT8`*n@!y7cHy6Qgeoxx~yxFz4uKR@p_)WrG~HTm$TGHZ#gts{?c7k
zGE#I{J02z`6irik_zv<uWlz&~l}cQ&pu*L)f&=ue!(^5OYZMmAGk=1~UkCi%Ou(k!
z!ud&))&1LUyPv=oVBJ`E7XV&@GQ%%x!85O<S_(AUKJ5+;n-|bH^h;nm7Ml4lN{Lj!
ze8`}3dg3qP8)0o!#mzCva5iSVS5_fK9+V#TwkD1iT$i1J5uoBaBq;-95m7y-oSj_N
z4zSFWmh50YTzP^Wcv72xqA(ZUC_y?Wx}rsEagd3fOX+)Sp9JhAbAI<b?)bS-^l!N1
zM>_M1KkA1^{{O@sf8}Y`AF1P?81(6B|0?DG68j$$J(ge0{r`qL-dLahM_%?fN6IhG
zm4D0Ge>1hP{H;0q2ZR3^b+i49PyeU}&@(XoDWJrwN!f2RBeWf<RN94T64?rh%?+%z
zYA~$j5-;ZV7hzVj@e-2a5d*^TisyKDKqEi{;U2iqP>bUGoX$UMJNK0AW0qy_E%N9X
z;cycLk*f6#%+iHuUz*g2Z+TXS-v;Tou9f8^Qa<RvC@R4YOVJl_>aTySTOQ5a3gXf6
zdOAO-v|nH2>L1{;SZ%+ldl;WijMQn~ds3)hR;?KE(-G9>>AY<S8a0!?7MJ%N@_I?m
ziz`sHrhB}ne`KFNE6pZk=j<;Tg)d#{AK1ndA6~@qF3*PYdZbhD!?-`3<5m-1CJa$I
zqALBoKYTpDICSM%7kT^fvbc_1Z0>?sP2}-J-(w=sY+F8h@?JN&)amFu?Dw8a_>q>+
z!ZdSNz8CYS6mmL+#|aPNR*w2Nc^X|6tJxn(#CAM4E#pRpg=bfbW44B>=L{?DUBe<%
zxzy#ALCZ#kx2{p7Mmg9h3hBW^Hh9Gpl<9`d4aXF3A>@~h&-xv@IR*%+uw$AotX5CK
zgqwx2*mLr=j=7^+g*C-5hwc|K9q^twbs0K~$DLR<(Nf~OWj+B@Tq_IMtA<y(bvvIv
zKqFRQ8#%(WxSN@(4BZ0=CzAI<%DH_TYZ-ANS^O?vlJH>?Z|yW<lAf(U=vm)*;cO<b
zq2AW-+}%1zO<;tKrAm>h3@WU)J@^=BHR!b*TLt1tY@`INQ&G{mh|gb(&iA}(xHyk)
z+-G!L9N4+xk$nYgV#GeI$dJE<M?v(n;vS@m!=$IJxwjO}Y5_xwnf^#m0x&37tR7+h
zhhAT@K9IX<Ar4c@v7}-m4kl5U^9{ewT12$rvu!qZv~m<W1Zwdq37d>4NSsuR%{pu1
z+_vxG$*P(~5u>aA#aV*vVQIlp2`DT5I}j)?xp6kdwsa`;9~G!gG29W+Z7{jb)|up(
zuL}4fwn2v1v+2_~prk)j=Md{kJMTz}fwH7d@K54ak!K^d6Wrs|HrAmi4NujP(Fb`6
zNVsvegp)1@XmZ`pp>w-r%{Ppp;^J>d1)h++L&{AbGl(6?Swd@NL$aCz(EoP#9<<5K
zOn%v+y(+u&Q;L*KFbW)1nSc!*i43Ta>!B1`^vMYr!O~ZAvT9=JJ9CNoV7rs%d2Z8+
z(3Uc6S7?YJGvPyS;rL2hVP89?JC8<D@xu^up$@>DC3$NkqfX%782t>1C5c0|7=(aV
z=U5?@cZ|KOATGSZvss8-zH_J5)qA{RGqz~mV#^zq6jZ0Es#NbylTgEf9Lh9K*?>`2
z8cX-sFQl1tSCpmb^}sTQ$`fiP7<38k$i&Y^sPzI7v%eM!u4vbGe{!4PokUfUob}=V
z&a7IwvdYCFc=XOFW0rL5GOPuc@2kT#=B#g|aOe{ITI?O;8fBf4hPm)VTr1z!0k6S3
zB(NgBWhGE5#R^-f;fXvNXZ4EOnpnaIF<k#~p4VLlh`PiAq1Z)Pb5KX7B)Q{t*%t-C
z$^gx|F)2kjJY8ps-MKL1p(4Sq$2>N^h2|S$BhBvXPCu!SD1d2B1LH`&S`ocFXA#}E
zImPdhZv)BV+5IHH=Y^)TRSlWo`Mw=lHkW542sB!*gArH;6i2gfWRY?0ZyUD?e46g^
z1?qqiokAHf1(1%@+5N5+1f-=BS|DyodGIpw?wJ`WOO69fV_RDD{2OoPnu#?{+#~xo
zx%wG}cV7~yR{3)BLcZJ`Agc=B#cU`*0K@FJcX+%}OrWEJv@wq$E${H3)0%?Rpq9LS
zP3C<t*<^TG*072r1wl<TR2F9T5k&)?KreJ6$(^0!`UJkt2Y}-VCTfY%90N-{8THlI
zDvwE_O%9pT-@oqVa#fc=Tczll&wW%mHcZ<<C2T0cB+F?vvU~rulA(uE1ac@Y>>(NE
z3qGbnb(zFbt2<>qJ~{QE6J@R6uX_8yE4c<0HrUOvhlokSgz{vEL>jt!(E<~CwpO6G
zAe`Tuz{kv%Y5;U2kO@y3!@=vy1%KP4eGvfW2Rq!M{BhJ;<27^n;_`X27KL?T|AV;8
zJe3(71riT#O*^km%UT%!Gzt0gT%9&-kzos$V*l~`QCc<v_X<z-xDREP-I8JzQkI4i
ziPi2Aqeg;LU50RSPN!MgnKdww+7x~7Smr;a`m34y<1BF=aRYsI>)dISE9X$j8Ob|^
zUN7X>pX=y@mW}^{+46LAxfGcS?5BcX6$S%slFClVJ7lmF=ca6WMwLIjLSn%BEPQRa
z0#rBFX%3HKD()arf*kQ_>zy5JKBoo)V;B_QE|LVWjj69IEnSH4Qe1WADkEbZ)-aNv
z58b|*pPE7LW}`bwU|@7F<LZcXN>tnSVvvE_E};<H1gf#15p(M39bp@-TV7in)!G)n
zi!nPp;3s;1X~T9*CSa}Wp<SaFBr~qGm%6GGJVB<$05r+SvL*#IYyNeL{5k=6i~Cmu
zlA15wV$9P|-Mjpc02&)qU;w!X!jE!Ad1f-@qfg+@256>&!ONpT^OB$?<|Ekt7Z&n{
zmf(@Td|w!)k}8Nj?>XPult45-xr41rg3Qmeuwzfw%|S`&Szz$BGP*^$95gM#R~&4P
zR$_13Q?T&{vavhzERtq!C~<W0zb+vOMyys`;O>!#p4EJ^v#V}+B?cXT$Oo1BCgs}A
z6w{ua{a)Yf2%w8tB)dFK(CLoa=t=bkzP}K!jpX{$8os8=;{}cPu}tF?QldF>G3vW@
zNFcb1DGjxFK|{KK|A&Roq!WUbaKh_&l+6OTo@y%2O4D|m@*2i;x7LIf4i&t*q-F&A
zw?X4!Ecw7EuxOjI?pp@xJRfL3Fz*dx5Z!rN;$t{?=O1BUX%Sa9w-G<CnM0tQ)-_S2
z*j#@-NuSrcRWWHwoL$pg>fk8;yuED(8<!f4g7Ch1etzloWAbay#;0X%$D=m*oh2lV
z#oA=cS1q&`aHhqI?He%M6^rd>uW7W_Ww$dAkK||Axfpig-%#kA)AxT;zkcA-uj<#2
zZp;4*g}&*n{~Y#j!t9TE{E_;_@=L4dzahzA;PL-~5Pt^Ue-I+w+pa!;C}D-GY1uE=
zBY56bZ0vtqVqhjXoIdolLs(W4ag^6c=15=Tej`L(Nd$5LbtP|46cA*>`^_C9VgZB&
zF8AAGYDSpbE>@S;`y+hcj&0oc$|W`kQhwh~C^g@jhgm!Bx5sUxI%{-%ZCL|e*_Et!
zyC_4uFATuu)z0lMmirdUZ*1K!Hu2DD{0tk_BXT~bf=wF0k6G<?*)hXftDNhIoe$)(
z!#O%KWC|yWblv&x?B>UnE~HB6z3g!6lI4Unx##fHYbRl3q;wqe-t_I*^^|81r{iv6
zYCktU6E#8`e^#<VEbCtWwsPFUbarTII+EKar?@j^rLj<^r%fo?Wpv9o1j@~t(KTdl
zpGaE-vK&8@oToC;vc7PlvT5dK?!+?Io9o5)O|4&Mb5@E`TavmXre>93+XO={6!```
z)Lq&fcq;C_dhe~5$ZUvwZC<s(F&Z7a?S<Jo5kj=%(RMRhNff4Qr6et7xX__&Ld=kl
z(u6>`cTa8}9Yp15kza$XKi(mo%!Ca20)~3(`;5CYgC09}0ODNzaIopq*hKl8teTug
z+jf1>Q)j)yiH^)B-2lEIU_c?g`lLqHkZM|gO={c@<b-I*n$7KLmaDnlmv7%*7BzX+
zcvgAVEk{P@t#Me6WN!0r3LXm*-32Q97}ErrnWI<McwQg$wA<VtEU39XT_0N4$~!n0
z!C`eWF1qH|=Vc<1tH+NLPp%%eS)Pr|19Un(w0TN-R=Wsn&IgaVAxWl_&xTnU&#Ylb
z_&apzsG^3P-T6#mEb7zPu^Dzzd!kK{<p&iU@v~tqM+9NI-~^g>4~7mpw*+uHDAOU!
zQHdw$2<ZAfovp+qp<V=DAnSQlX<#@dq7*c*qn1@oohW8~Aa7x*y$SihwCo(`MlR;r
z$N6DdhCoxxB3oIDj07h6@H?TEA8yE!U0jK%%kt3=H`*muHIn$g;*1Xv#xg6@4bs;d
z<Sx<SbPdZb=y!5;sT%A`wXk~0#0Z)U9}gw<<uK5Q_;q_3>s%UA6y$z5Km|$cFIEur
zXwEZ)XQ{=RIRlQN#tVhAkKh-bwDAwAC2bd=zSx4^7lZ^Oi%y4+*{UmJS9kQGjG`Ky
z(uJUQhnv$ilWp%k7t5G-dOLkD|EOk>qiISgc;DoqI$!DSxStMFP4a3#76$ST9S;!<
zj68EekkG6)WTc_REYi6BY1pp<O4(rev%GtsE*6`rzo&CvErFF<ID@5~smxn7G7F^T
zmCnH=uBUSAVDT#@gCw+0g1v*rX>J*4&oF%3xuT`AMwyehw7ktJYprjH0;zZ3&ieah
z`!}u_*PZOtFOpi$az24J;A^{OUt4+y>$K;kRAjg*0Br>Hd1J9HK7k1e04su5cd3UA
zPD3bq<R#XV@;<ZfE7k|_5a53M#JZ>;5NR48!y_Og4=5m?HCETrag}&u$VjnODw)#V
zDFKB#oNVT$bBzR&>O2bOfTT>u6;+ahq8*wvr?pTx3+9Z!zEMVzh#R~?O<0R4gcL5;
z;%QACf7w-?5v!s?a%E)@{Y6q&o7)9UCQNak8JY|#JwtgujNH6-#z$z+a!}?NniZl!
zNGCW8wH`Bu>KzwIcwwqF@lrhfG*nE2cJXYPVB*}0zVeXr7$TYL2pB1pU>-hEm3fZB
z*Rg(pZr-Yjsw=W(teP-T4)TLT(XF1+QAAFAs;g)<VrTm$XyuJ?pweJ;^qsUcXULEf
zhO;c$HXp<V&dYqsn5|Vd=%`u@8M9Q%*kW-No>|H&+aM4eJ%bTG|7K|Y5{!omdw9RV
zI!FXHdO~o8YGOQ>jq9FSX@rvY-LY(_9N|gqFG0k|URp+T0A&iA$>t^lj8?vVomEH5
z-klOIxe<AuEVJ|wWU~bM<t}$<{VTompJGv@trcK#1-SN84>^jGn5%mkPv6BA4QREn
z=Trm*AzOjvU5RdH@<Y?k)vFo-Nv@Z;m!8wgVosBM^PK%o_-XIv<3~5M9E(>D*~q&g
z*enEg+PTspFRhMaf$Q%(+(!+}n~Lp;BV6n5awpg@CYoj~%+ldy>24kScC=Rn9J((s
zrB}v;>8B@S@oGz_lw?cKKpqO_q7NC4uhBgRnJr6ngx?o*GbYXW^bge;=ND*6B#bGG
zASkZPgN`t!M_HvN<g?xMwwWi{Yz=G&C6d!M3N&clA-#~AO*U$Bupb!j!87sWW^(MI
zZFSy^Or`7A4hf@p%V<D!p$y%2t0eC_=srb2x)C_mPkk3ZfVY)b2ldVcN#!z34V#Uc
zq%=gA@^mAmJGYIePMRHC8N0w(9DF#2rkV#l7BgJWBnBxSl$#Q6QBGac{}5w~6x@7K
z$9OuWf0e$p6srb~f7@<JR@d3+R4M)1qsA|j;8>!xdU_aGsg+mTGA|&uTgmQRN>G2A
zR>JO&#Lka4AC68x6tD~in2}y3^i4dLrW}%|q$Ahc@9x2A-Ko*q3Q!fyHXVvwW!57t
z-MgE^v>GNDuj^s63(mM0(4Rvgfj#bK(D_uvJiCN^;m*uzjYD|NWuQ8I`X*o9Ioe`R
zaFxTwc%NFgE%_BZvcI^4X0#`(P)5ONBFY8fIOQV=Nksh3qN+3*w!L2DSp!%Mv$rv=
zR?$-tJ+8VUugkUV9*x}U`-hHjMq)b8+VE9`QTf+}5Gb6i{Zbner%_WIl{4+D`4a?0
z=k6V1o+JwN?XY@9_i@b=JCR@@G{_C0Q3rD5A*R)blBA)v$+SXYie(7U9V$Z0DLr1M
z)7r*;3+doub8m%V3`phk#>^IOPQeB~Mi`at`0r)r+^_}PVm1uv0mI*BRGX9B*H^4*
zUO<&JgLHm}a6eZPe=j7y?HBk~qW(ol{Lwu4e?qvQ^~)a!_qTcc7sCB##|X<Wor^yq
z+)q;8o3O|FrUw47y5RniuJ^WM;ZIQc-_-WzcK<_a%Ttqz-L8j!)Ai0tmBeaAO>^b!
z)K#3coTU21Mt3%<JwJ)c@+S~70+5!yc%l=c<MNucDzQPc5&FEiJU=kiN=py2e>c6|
zUOo<@S+tKSo9=mtBC=MhRx>`}UM7Cr6G%C`+$odv%Jgp5L@tqHF0dSI@0zt@d5xof
z@p$|eL^IxUG2c>fT^W@3Sg}Ua;pO)H#NN7hR=t}4Fw}ZKL{azYoAu{WH|gv6EVsnx
zr+^Q&RE64lx8E&qxu@4lV<}y|#OG6iRhy(>gZ6Swj5=PMq{UrsC90)a&MP!tjbbfo
zD7E%98p~tn7dwp2$M0^q7ySm0)`}^?GaD)o!B$+BVER9Jmu6P?9YT~oggf^LO^Y#U
ze#nh=XjI&L!OvVD`)o00Za+YAM&~lvr*cKZ{lt|*Qg^F7W!Fex^f|%&u4{J|kINz>
zT*(+;dPXO3hRLeWUu}oqUr!N+MN2~-UNxuCB)1A_q&!eUwaep*ejzR!dJ)fa?F@`S
z93>?)Jkj939luh!%Pjx2=T+@9WU42Ic6F$nOBrTHYh})aPROGTjY(eSd;K&=yL$yk
z%P*+bK^)ZQo22rOU7|t}b&H4!atK0tXkYiLfpd=pW%Xw#7i31HckFy9;TO=g8Mw7T
zcy97p_6hP~D)9&gYxoMk!sX;u!D25wEnpdb^kH>`w*0WGqTabOhLi)<kBfeT-6jzg
zT11v)G^hPtMV*?8eZ=aHYzN~q-ffWERy&(VGCfE_T}hLaX9yrydch3O42_`>8bG{a
zPEBuRx~krl_AC)oZq3dx2BLzG`{;4Mo;9@)z-t4$W1Pt!`=c=$s=pY<X|A5?Tk!!@
zPLX<2y_Am}7dgC_vWr7yq^4X3a!iyl?)%|X2&70pd@WunlMK4J`rAf^<q>mlfb#%3
z2mUOa^7)2TT^!Xrb{X9_7gAwmplD<d)Y{>T!+XfRwxiGUEAPjcz@$zxLc%~21v%~W
z{5t)282HR<p2O#EigQ1h!(T(Oo&b3c`WlQhOn+*Au$=BA*EqZUj)8;~0HUj^-I9|V
zpt9*2;^TeRiH?RYvGh3LUgki{h>W&`MM5i~%w_}bfjAYlZTR}w&R&50QGXNVnY`%L
zNDsN5@G^1MU_^vjWjJfE=}880)o(L;QWxIzBbuV&bFnC@+lS;^ED`IfA*W{;>;=5|
za^VL>)BL!x!4?@&T^8~l7|X%=vtwWf=H??IGi7~vWqVKKPYOrxyLFM^8J7^S>Z615
zN6V2}d|CL8bu7ZE^$i0V6+eQrEy2M<6te0mzF!lk)#jw6YKhD3)DR2@3AZG;aGDoY
z*P&u)zNMc;!*f;#I(8gFm_!o{W=(J|n$4wW>fhmipt7IY7blMYDDs)lvJEyc`0gta
zkCT}{lEr}ANfA+W_{c-<g%82=b6dt{Ppo-+J$>8<<4j4hcl895hq~|4v7I(iK$2dq
zkNotjLsq!#OMxlxSKt*MMI*RPf+GR!IhL`3PU>~#n;VtBmJ@gy<}z1fKIfD2TS>ww
z00(+8+EEFXvOCj9ae*iW<$s-%b*lgba26I=N_+2ypM88q3f%nZc_d&XRQQ{31z<)L
zG6yBrn|YHWVP+98s16RgEP}sYZ;FI*d^DSQL^1SZ0Ru8M7|sG{tz4-DpKI`eb7Lv+
zkrZuSrM9Qmr)T(thZZ?pNG?9U_8w`_v70>8G<~ZU(drI4Ia)Mi6KkUlP8SlF=hbhM
z_Uw!+6m@bb%DP=g>LL&9L`z?6wwVmfn8?Asg<Vw=BsJ;;;X{&$3u1*gX7r{ah6afG
z?9O}fy`j*;ou^sXj~C$B=~hhOKeO7M;U9ro-0I4%^bTo6B387XqND3$>y(T4fwIZr
zEKhT?)Ea1A$x32@irxYwsaxy$*Qv0p>VzX7gWth|iK%s-!z0)@_<Kt?wD?7aNTFj}
zN43m`Q^yK_k2F+Ft9v%qj4Q26emcl}r9-I=Fdal1>1ZZ^yUaQY^U3*&@hGP&DkSzD
zr5y~Mz({)5{`^BKz2L~(p*3b20!G3c%C5ccQ<s~Kl4b!)4D4BSY}Su-5Ecmp$izcD
zfDi#vt-wQ#rlhpG!yL>>LmBkvaDFMw1mA}o+ZioW;=V=EH=fiO4Qb%qhmH805?eCu
zT!`@`h`qqI`R&XpZeTI<DNSOq@0=Wt$$IE>ry<G#c=DJPG;$!a*vU>@;b4}i%2CtQ
zew=;o!F027+3X1!D|gqw<b;wCSa^XK-<>3rcF+8p1H|u~jbrY1u!{t}LDq(Pb5X+G
z2_17-7731)oBN{sW$-LSi_S{6_6}S;5Q6U!V6m!%UdmCDEH)`ZIqu255K{chJnXuU
zpI2%A6c5`ZAsm{-^_L!ZV-YIaVK37>LsE2Mw@y%NPTYuekEBeN)cDP$PFVA|-iuk-
z0;2F5vCk9BT<A&`Hjvs6C(J4NN$3EedpKaQ;FVHO@5pe%Lkgco9!RHP8(<&5*oS3>
z1<h4FIvGSU<eET_yPv9OnN^;*+iYTiaP9jd_Njsz)8$M*&sTAer|))kPc3cT>$nW=
zNMI;6BYzaeS9b`-yji5fCbFLttbE)Ye?K;%Vx2T938j7q`3cX+ZfBo>@ODUCJutcM
zoFIUkCKRUwk58~S@1-z*XbsTuE2Omq#Sl8`qN;=n5b7MH&rD6|sYCF!e8c`Xm#qx-
zt=>*LOsI*5SDL`Q`9)zs2YlzHt$u_$07zE>5CPM9H6qIb<3S3?ngRtkVI<&i9!kRF
z>Bs?GD>+tTe^U`w_H;00xq=Kp04GTpG|^X+;Evk>9$upsspfT(l2POr3ov_Uj<eWt
z>zY?Uw*5=5-yz)3McLmX+~48mF9`Qv<>CKV4;m}&&*I+?g!?;g{lVrFEWh+f{)BM<
zgtotwaR1jT94qZ_+>viq&p%e--s-e}+eV=6tOCxi2xY;M^2E)up#VvA2?;b5Ujs=f
z1(-^X88O5`)J*I35vI>NbjV6Nfif9$tku=psFfV1ulWAk;gJ$VW=S@sQ;VU}+K1GW
zXS;@FWJm9ti|JyMjYaD|Ggok(^Z}N~t#-=9l0)qBmaU%IOcjcnx$#JqiuL#ssVcQo
z=8|d`i_g_=s%zHseY;+5E2kSJ`S*<`Y)2K=^UmLZ4kE!I?XpC-gx53K6}K@wsUnjO
zDdy^v5Zxx6?~iyhWP^$rC1#nByPpCN##7R%t>TYlB5k6!oElbEEo<?7p<DZJSNk<x
zuVyOxA${C1lk%xc0vZFgh~TNZ@ZCDp&&@4y2ih3m!roIZm_efB6OuggZ8;3|GHydZ
z8MNU=TCifCFm28`21^*jZW)0N85muq+>op|&K<c{NR{$o)zGmbx=Koq-Db`%nipAj
zIXJdpI<Hfo;W+hKHfdO~m|J$c6?wn*tBx=F5bSvpmI*21^9OK+X}fDC1HU75?MB9b
zfcF)-&aRLNlV~IgLx)^OMQG?R?WF67<q~<)^MIM0L=p72ky>>!dXhL!cz-L)e0XFl
z*Z8f!yv_C!n?thX6C4U`KLg#-yJtO}^HY*bV&*Dg?sCzyyCIByE{<T3mOQQ}ieSkm
zw?br`zy}kQl%*WxI(G$g;3Hh(Fk)shxIi!_IL>nsR339Z%)AN88scmz;>=CM2n{2-
zbzj7hHH#=;##ln&LXod%3StY;=8{5S&9H7YI*Vx4(HMS;nwpr<Wu6rS#<7Ra-wH0$
z*?Q+NfjyUMS$sNQH{GI4);DL~xeH%g5Esp`fxS~sGlqXUGizJgHV<HCjen6=BWL&$
zqM-VX5I``uJrWD(D6zi{vuL4Ne`4=RlM0Nsx3O9LVEvnX*tJND_aW;R0rFOf5uA6b
z;o~q1Akx_r;JYa}`6d_Ik8#|-2v=*G-A;jyrHYL>>Kzb?U)F5sB$~WA_&gyb_u$Tl
zT+~9cJZpl0Nqq?lr*&!%4XN_uS5JcF^a&P$7Dw^i-YXMXLa^3cyKU>2udjJCyCD^E
zZbKZlVz})zE-qz6pAzanjqja5SwdvOd&Mee2-v&$CIx=ieap$e3Ha}rec2+EZVZ%l
z?5EB%%_U;028A>=#ssP6eqTCSrD+&Hgj9ph-m}MyI+*d{xJd7T=oTm%$k`G^FLqd$
zTxOLp67w^`Q3?czs65-L?QxS?u^tJ`DKH0hJ*f$V)L@ptp4zhu&!D!1Io`(sq+|s2
z9IO>|?0kVprF_0)$*uH$$xqoIf(21gSVXhMsu%fD$JB<q5Hy0Q#cA7Sz&<eBLpFnF
zgCi}D6tN**jYEJM?C?*+)WQ(*iG8kh&Gy3AOLHinBLNE>!b!s3p_7GP=IA9fswIyt
z=)XWa5+w%(1|<c=nV+at4eti;#jsA+dch<n5fd%*!)Zbq8M1BPt>#sUa63}tDNfYl
z+PX;J>sDalq!^jTj8r|5v<MDxNG$vO$cuF_u+*TqnIaL*6SqDVWxz5{J5`%K2@zdp
zq6a2#YJddHd-mO1IKc>`{!9gJFBUaJ{86cRz|m%_PLg$PVmS9*lAm9^vNhBph`a5C
zm0cE;oR!SUR|IUW><mScQU!?Mq+(1Tc|+&#8PwupfGbWXl;hkB$oBGB^Y=TjM2ikJ
z7c<5FUx?97Dt7iE_5flV;oX|rvYRX8hiNMihR%xn<#oaUm}eJq9Yw`qOROf0Sp1Im
zj8Tar(-W0Wi})pLi1{~WJ^{G#QCTzqcccd0M8mU?`|5M?#m?utH|p_6^e$njWc2IR
zk+5~2sWHs)PwR2Sew`+)0LMcMzJ#4SXK?gk6wR(|^5ItT+$lUwt_70<Mg{ewS%=$r
zwqb2N^TdKjtr%jYOGSTWee!L4INZ2LXkF|Xt4H@&Bu1vkHbz~PdY&=iuVoYD;qer^
zR$gdqrt_=KNP~3o^}554#t#*yC6&-<UJ28kS~n%iU{#QL7c^beB8Vk>SY+oldN*yS
z3P_YlHwrf*ECOmX5pc}GCr0vgn}!)-s|m=1C~u#ITYDO4;}(-jw5yyh7RgG<b=B?f
zE`im*#Bj>aGmRKc@C7C4<0QH<c!{_K&|nYs4kxidW-T%G?3Jx6Fr)W&BdMc`zOoTx
z;sjvyyFGbY#V6KwP42J?Vb{=pe6bSMvOxb}ZKFj}2A<*p=BF9mgNUIo9$a)wq_Bqj
zuv6%EuL%M*<xlMmQtuoJLK#oqt%NyCHN2j%n!_~(r{QUctPPKZMg(J50s{=#BSjrZ
zDsg!z&mt8`-AqJ=$2yFf5sI3r8&becq{-pz?lJY}(r?BNt(^IAD&~6GjZ2BQ<yxgD
zFD47K6HL~qdo%a}bLrgT2Kyc}OCCjrF^|j8#_o*H_iiN4Ba<_Dzgb+vzSw3YVQg8Z
zG1Vhia%0|{UE-6SW8vcriz<XGi52beMev7=f)R&jm1KR47RU~>O3uZb<g{nW{@y0Z
zAwwFT)%sD&she(+_blsbaxP!j*EIpP1a!YSFE~ImoOy--Vdv9fa4ZZb4HSb`_7N1R
zvI1zYm~)5-P1j0_T04t(6d{QqZdJYV@H=LSUa$VgZ2%FRNtV2lq~N=Z;8(jkfvg+C
z;1*VV6)}e|h;F4%GJ=_3^{-L*)7n0UJ|^DWwAg9Hm3*_RoA$0MpC4{fQd(JCrC~|s
zn7+2iUT^hmeZZ;GXB7HZ_+ou)S@~xz^54MM4}00q`Wfqwrl&vF&scw0=YHNPm-WYC
z6#r;1d4sRN&*P8uv%e4ikJz8!>;I^q{Y;_%t)IO~aDS+u{f)P_nW5VDR6yDo0qL!d
z4K62_krqzCkx4_*kVu!Xku=0+>I^FkNMgNprjUswoT^VuvwIW14v1XJ)ud(Q4<AIv
z;;a#`dxkkryP%yUKDs<OrIjbx)`rhdpIG4#r5ZdWNWG@DDb7Bbei&~Y>E-ZPvM{2V
zd^&5;X@6{7zFuGB6nUijn=LNy;pW>-kNI7i_mCUbCdt)?xohcO<+EeE`WUL0Jf_Uz
z7<WkAaNIU>EUa}b4WfrJTX;dso=MY^U;L3t$GX(C-sRr<uqjx_Grg8D#r@$Dx0>X<
zEhbeglgC&=MeI7U#Q5Der)JRD#4;sYAnpp?`5XIM0fEUznXRm(W}&MBmnJbInyUh1
zT3MECqo6eB65j%2;sEOAyouFy`IPk|hLQ^f`bYEctzmm~FX1iL=%b_Cs|*_+1f%BJ
zv>3F403%*YB9F8643tzQscd_q14#o{V@6k2<*dUa+47q9YO$Vc?k12){j&|t8u^BR
zN5w@=hwYu2*3G+p+1BIbH6nBiB5FaSYbJS71@J8q;gaTZ;@fRej%bZt>#j?+v@~H9
z^heydVPc;*Cn8F|+HdRhb&*PEY}2y@=?Hleo3y|sB(uJ^y6v#Z1MRYh)z*(=jX>4S
z3^*sZlbxp=$71<Pb|s~aAUU#O%|h1?_@H(tVT@C1*Z|fK4FMEv=V(CYEq*bXtyfnW
z=LoQ{zny=cc7YLTTv-aNQJ(i;!5#e#bl9y$*g(8B(rGyyj{=@Mqb;|yBy>>YlbN+X
z59{D@@kDv^Za=-lp39DzE>tGLa6s3tDtW3*Pv=hBL5j5UN}A`kq$wh7_NJJo%g1b^
zfJ`#ov`?1em6%=_kiq3HO9aWZPi~elDGfyvWh`Mywxl$@2z&8^B@7BA_N9~@9PfLD
zF67wh(e7gs{DChprhui%Eb8Zs$qkYx>JWm!EHR<2ce@|2Z12a?Iql$40xu%Oz%|~E
z@0cS@$k#OIk+#Ib^z~FU+{8<;rIJR#H`u7K6Ac$SLsD>PXWZ(x(;z&<K6sq!Hv{>O
zyW0y_^M%2#lTLqwbY-RMnZv@l^}8i8HU*DyG5G{+R7*Y)p(yHkFz_-1&6`8!^Dcob
zxO6-@<DN3J0m!$w`FKqUyEKAf#9^1p0)cH)lrk0$oR~;vo}KhvT{<~bJ)t%HZ)fLM
z82F^Q_n6@oJqh8F>Ys15H^Qy%`+M8ISRm4mV(2xM`J$DmO>E=Z+sQ4rYh@r&j8w&3
zl^VZ_iXsKER1=>NU1b#oA|Ww_q-73KbR=CKqQ`9plt*!sB{VsWu4}v~ChdO~DWlDT
zAe#082le9=pegEH1%%z-!X&K!K02X_hc`0eY3)45&&PL@1W97xuvi%P*vkFxKoH8}
zD8_a|aq__X3E)Ju?ZXEX05x-C051}|hyzphW#e+fd}FJ*1k7eav`uN8^M+|2WV}^z
z7K-P<+@=g6x0m-?ylL_|76zx}IYR5~!nsqNJWKLRPF`I%1R0G4Ee<SAv*Wj!=KOcU
z0eQuZm<fEv#Ke1$<|^t#>Q$1tb=ZX*No?1&6_!SLjt@ieHZ)k4vKHiB=EJel^z{g1
zp&Zrp6u=1@<uH7D&b`wng=FbEsWT`oW3J0-kYitQG|Ci+C-c92kOhz;kK^h_;RCGc
zMi9q)j|eR6n+$sy%F*ag==KQ3yL$yk&1h{BGrW&yn$aRsFW1n8vy*8kvurvbjFg}U
zjqK5Bm)}?+TNcMgD<zc2qr-KIKx~G;Efchi_;N#pp98)_hVypj{G&}X{Y(xa7mq*n
z_0%OTsF?oa&L;BiOaZlA_u`0<ukbg($M>XPD$go`zQ=wrm=szNX34f<tVl{de@WcB
z?zzrFcZIK(2IJ@vogxQ`p>*xMCy<us9wKx-=qs7fe}KPi?i?h6)NKck7+otpE~!7=
zp7jEkoLX<#>q|xUrcqj$A7H}h!@~gYaU>;*rE6O5H+t8Z?r}KKe@n2QpMhrZ{5k?o
zo@JeIB|WejPZd70!t@RhR&GWBB+$6y(#zO1=)E@(Eg*X%z_ZDoVit&`b$uTn_UE{D
zC1O}0){o8u>uw&^j01e+V8D5_ZfcF?1K4|;Ip^xTSI&j={CYJQP0v06_BtQ2g(jlY
z5NuTx@LaLOKu?i3*hQ4?T@A?TbeC2w+48@3Ck41GHV9e!kAKY8XvApPjdG7?MyYm|
ztKR=SotsCHkWmh>e!#xn7v7N*ddI}`5!zQ|uc0b?o9=Dnf!lV)h#}NM6VUWoVndA=
zFoD}_=T{-fXPpbw`ov16@NM1<O>w%y=2N?9CPxUwaM4voA2sUS6A{^vfw&HY0)~*W
zSKln2Fx`YjY&)*RP0cbF?>KO%&CRN!u)yT86d_i~C2D#-ym#!a5l4#ELE^qDa<@Yh
zXrQc+--?bV0Y7a(8l|1<)zXl0vhuDo$V(EEMJDkYq&H2BfyN$qrZ>XI&V_QN)}BC%
zzaoEOR<V&)JuetqOSv`E!BEk(6Su-!o@~+x*NlmJz!uYO9j-<RaRzgCSsfkVEHFt&
zs?jfCV@$3S;M%BIZDaZFpHv570`l37v;)TwIa+$+qLa$jhp_pYk+<SVeY3l@ys)48
zOBQ2#AK%Ou?a{^AwdqKR+};DN*29v+irj4sX4HKcMNb(0n=%SjWcSNuvYD>BupmTj
zu*S#IWfk}Qrc881BqN<T$j_bLCZA>7TFqwt02K%=J^>PXwGp=!4X+u3zHCRlY$LxQ
z(ykH9RRgKN9iQyfa(KLY-0IBTD)j;yT7M+{J3#xnq43`T+7IFCS3vt;wZ#7h(0-f(
z_7BMV0cbz;wm;dLmGuq8{>hX-0ownM>ew&M@Bd9}e}1M|y4tYS5i{KC9)-VE3P4B&
zj+jMG3y{<#KB)SN1+`U67SWJig<x?m(t!8+roL!&_H-7-vRl*@tNkui8)Zaa_5Jt5
z%#sft)Ss2A)@kP6l&;lPn%hS$IVTmJS9RnME01-m)XX*q1{uepkF#b~n(IAx_4gh(
z1G7_QH`~Lr1qHKq8Wt#zCw5A$Gks>yjg9MENXw5weO9oE#al;A{~vpA9anYJ^^Hm+
z9U>v6C@Bq_Qd&YJM39n@mXt<mM7lvjLQ=Y=TLh(BP`bOj&+NUqu8Zrv@B4Z0^E~H0
z=RBX!^9PIB^P5>Svu4d&-!;FzXI_oORr_=<f5dKcCCywNRr$(vl76E`7m09mEc5-h
zTW7IbGkIsq?A%iJBxn`-1(P-jnW}l8YK~S=UeK@a2Y@eF$&!$nP`x!olb+>s-MKr<
zQ@-Lszl=rj!Y#jX`MsJ|d4=03u|v+8Zn?{s_WUT!lU4aNvv|{78;yi!<9ey(Ap7#|
z`O`{g)f8#3PiyNLx6BuKPLOinefrr@%Gs0u8N0^jrb)fSPvLUG+?p!cvZycm4^y3A
z-wJ-W_@m)*-`AUYBNUV=n-R5&2CR4|w(XDGK4@e`r35hhy<@#Az#;d0DQ9tkS~R>L
zt6#(UoturHu2_W3@%v1<CstbuGgFr8I9ubJlvUe;Y`1^q%*`Ah2Xw9;SJ7CQvW$Mr
zT|r@)w?;=GySI9qb6+akg_*AK6D@aF#PPURlBH4<77xDN@dE*fs*$Ahp}Qk0CjCr6
zAdxm+mU!PcDz-wjk>TWYX*ES0Z5i$N>5Iov_*zv9ID!mM_>tb;L6aD~`=L_Q03*Vf
z+P-rCdH%j@7KI$TVHYjhHfoRZ?x(P~Ic8rT1j-=97HMBl=IAZtG)BK1>7(gg@=FC*
z><T$0FOwA9L-y_(%85Ak7C<Fs`6)gn@e$LRK>a8CCy9XHdv>^8YE1WKUTF_XeaPE;
zqyNlxud*yOPxtW6cAs}V8CmYPZ*sbH`j76>PSL+wJtVhmSs)Zv9r0(o{f@O<B;5aq
z@sUmj4)QErDA@CQk|!>_H%fN=JeYBAHKt$i%f<1r+Ou@AJVSqM9Zqn=_XEpEzwbo)
ziUjGWUkkz@l`pa|L;IRVqU^M<QTfDAto^>@7#U3gTR_VqpHyN-%{nO?+RsE$_MOm$
zLKSqO?tyeay%&-EygTk4m3w-cH>{A;1JoS1<M$-o)A6WtgI7Ne*qTWXtjg({<$PoF
zMY)m5q-#aH_PQ=78Ov}@dj`p%Ik19;6{m82?)^Z|A+3#x*9PH@<1dyOiLaPY%e^7J
z4+6gr`AGP04Ic(RDLD*IBp@f|4YTpGqt;>8eDUSywb5I7!Q}QL?dWIKB0(R7EuIgQ
zEK+5%2ENeFyZ4e@VDMUgN<wn+yT~U?4?F!VKlY<~m9BxWiE)(QD1O)XIGg;zwVp=r
z6q&&K@F-=g6bTCf=6h(&9hoj_(Hy6d`3YM#w9=m^qV!}Gq&U@r6A<fqyoVW8(X?^X
z-tXt_&7rt6s-s@?yhTMsGRxA9y5lmDXla9obG<9?<CoXh9ByG9H$)va1m=5g+*3ZX
z#VOLuuevSF!!Mb?6s&llsp7Ko1lNC5WF%?0g~ObE>a#K?f^4OdiG?CN$7i$q?`dau
z#1Z-HHNIBOK2*X>&lmPhw+^l|d4zDNR}rit^PnKzYQ8A$bkl;KY~qLWG>WI1&kaN-
zrIfw~CJN_TthNsQubZBxNs_U{4}UR+77^^d-tIH%y0)B)`ulb%$1a;iR7KAEJ@sYk
z8&AphXl$D}I2rcB-C0pR?86V4L(zyHnyh+Lhd89Q$tp><G4zyvq^I3QeDS)FNE%Nr
z_iM2DgHbjm;%UUh&a+_kM|c+fJuguZ*W?Tlv5KExD-?@)2rfR_><FW4<<_t6$92ra
z8(JIoDJHt!+!#87ew4-0K+J)og!1mD1bVuMrA@x=4J~0sA$Cp{x(pl=`Y>V^tHRXR
z^Jzc13PmCwZKgyI%>5J_R2dRC;X%~uE}oL9^Pk9=40hgeLZ7+8Vf{;SRg|v&Vaukl
z9Ture^E>Rgsy7dc=CldIrN;`AA0j$Y*J{QS&(qAmyTv}_`t6CyD>?Ge82LJb92r!P
z#2(F&)|#HaRX+D#G!o<%)}ZKnY_-HZY~D-Dl0}lgoE6(4A#5#sM*XbDM2bwNh<EKy
zDCb&y_+5!qASrth-o%+MIm9mVyPtPjUtAm|yDKT9&La~|#L?c+_og9|ls;~WH@~-3
zeSN|CZQIXq+pl75WMz8XuLSFSori7iNcw&0n|BmBSY2iFI&z}bRhY4B`AM_C-pjQS
zdX%K{vDemPH4SsU2cx>1b$C~ljxJ*BS4e3`rf6`A59t|w(n}fP=O3TisEW*o<Ivk#
z^h%pdyjitUNK1JfiQa+f!Gp!of!{rcF<yb;+Q|K>=Efm;(I(;DFCSyp!VEuFniR@u
z#|kmUMm@;LkeNGvoZgl|rh}r8j<+YYQWs2p=8|$Jku|>jxXEz97PXh6Ghy3hU+rhh
zN7kZOJ?+08%M?Rpgap}C=(2~Lq#!SPNFRpATCQ36jCw>S`phV(<mP7uHYz-wtb*7R
z_-iZLj2pN)xe2pT3WuZM-#8a@r@j3~>&OSZT{VA^EE$vihAT=5)k{4x*d0qAw|B&O
z6-Vj#aojY|E@GC^^r?s)u1VT{vn<Q{CY8}k&U>a`ZYo;|V{at??8aYl(2D+|u+33C
zUp=B<czv6joNza*LPLAVu*|fXf#3l0M9E=Y&Or9}TsLP%0&8E$&JVv?_6s9S-i)0Y
z5|w(+heZgqZ0fk;I(uCVotswKepC2M$iqu`ww@C44Hr76l@Tb{NBL*6_T9*}3^pTZ
zyNPLhn~sD?dl9LJ^}mH36`Ea0s<I>;P0)FE#JOzWSYz)@el!;IjMVe1#wTBD&W1dG
zQrtW2i2PC=X%(b*8+k9N7J;YFS7jzXnHxVxIId7LD{mf~rxb+zT>kE|+a}Mda)B5o
zEjaQ|fZyK^j``=n4~ByN_F9w|c2v!OWoHhYQu-Hog91NTKmNU)xxby0a|QVQ*LLRE
zufoYcupT(8{@>l1%Tt@MndT*M?ba6QY!A!+rgw`V$<QQ+oUO6?o1t066BA1+a=gaU
z7DIDF+KaQ&TnkcCBm0&0#L)X$gF<!ZX*b_4l%_hYv#x&)=zgX*%x-I*77{2|v9xO|
zc8lb!ONY0wL!*Q+U!dM0@9ccL%E(r&D%eeUWxveyRkWK|y=x+lrM$|-xN$0iZ~pVM
z(u3RvoNo{8qFuFTnBz(G<F*ODQL#F`<(bZl-_k@KdsCyk<Zp#fZ11==grc455ZJXT
z(eFglQ(O=v(bQ$)uysQ9)(Bg#-rhD3?Njhzhh~N)$w!O*<OEyK088R3-D<%_EMbG%
zzBaZec}6h_A{P?3tRGccpFD8tOx3h{<I<!&YKTJEY4;-X&8ap0%zYy5=cI=jHC$Dj
z=^riaQ^}3KFENg+y}Zvj6OAlHIb+Q<-^f}~uA?%l)U)w6E$A&$awB_)%nJp>!|G<-
z-sDlkqb2H9HTPVhM^0kzoT4_SQ+!|2%apd8iq)^#WpJqrBL^!E%|NuwpRP?SB_}Hw
ze^{H?IkeL#E{)mS9#=5^$Z3Y+Kl{pFY6$;en8;#}aD?&pLbco7EzHOI`hCG4wt`<y
z+V_6qZE(#Uu1musEe&Whe}Uc|qqz}UZdPlUQxQ5P*dpz@pSn0CR&(A!ho}4c3}0;b
z<D=`g_;-Jy>O~?UZkO}EPgePg;y*4Ga??PG(*I_BXpHZGddUM<q=}j^LSqU0AIe#V
z(RvbAW|8@EzYVH$MIC-C<qu2nUA$dqkNnDcEbJ$SsN7Rq966VC?DXh<Xn@$-ykSoz
zMB?}B*GyoOCgw!{sBX@?d$7Y4$@dXe@ayHE7?V+B-SvcrWl<i?{8L!BY!<r8PmOx5
z5RZAw#u^f-i=A)LIX!#1;b`b_bYAOn?|2HUs?0KgQX1hNQ>nN5g?%QkST^&llWVnN
z3|aCJNm;#5*)9M5ulrQ-Y<D#`6h4=#y5s*Gi@&QbuP&psx}oRf5uRvR7GU^Q?mR-Q
z4ntI?&Ea)h#D-U}(}aTADs!UCf~z-y(YUkj$s%@pf-|aEcdz=IcZ;g);&6q|lv7C8
zY{u5HKFW;>`OIj-t{Q`3;qTI$6?eLs6AwGB=oT$Uvbc-uS)Bz!6_TX*a3{6mZt|`>
z*2TLE6uL=bRA@=5i-k%_Eou%*B$#>@6A`98`{9c=f`(U>Y`4&X;1tZ(H>)L~z?Y2a
z&x2=}aQLPsUc<HhJRrS(6EP0GP=X?JN^iu$b3#GaouLPLsXU$A;m3g@vpW<0j!vbX
z26yHZ<~Ipb`)!hdpC#pKg!jQ!_C9mYqNG`qDBe9H<jTBUMH&0Nv*S)_;;#8#Nlfo_
zj(Tn;5munRMAF!)R9&?eB@w%pw(qef#6LrJx30F3T>Jgt@Q#Z@>(+~~+#lo_9Y&)j
zCbSrgT-A`(L*Fkt_R%apD2NSSopn}C)UJ!*3+gmP%EBte?|;pi=F#;=^;Dvf#b*X0
z^K+5nAZ2VeMbM1ivEI=8T%lNdn~sNMI`$BQ*{#q-kFv&(Gp}mFN(@zWLq48*mqKdr
z69*RllXr%B0=Vo=-9+jt{mgvm*M6t?KKbsqr!zJp5+Ol0KQWTOrePCLbbqyo#;FjM
zm%ven#l@j+V)$Lt>PR>pTPLBwyHq^toBCX69>e4*by(dyB_=;;l&q86rKLXe#|H0o
zxP||^_-VQ8z30}ReAtUg*1=mOD)WfNWh!}kyPINmgr%WCa3*)laY^H4lR_Qw*6TvA
zXT4T&-&sP+i)rKe&eP^z6V6N?Kpa)OoR>zZ4z;bwD;trVAG?^kilfIqc$a<4e-ULo
zvY#@Z8R9seq_^=j{^GSqY}r^@^vthU#o6{{8H<5(XmUIdzjE4;cw40D6i&ja!|UY1
z3^5XDw`8*iczp;B3Hs(0ZjoY-e_28fuNIL&3+lg4U@-mq7#q1@FBxk!4*Yg3ieiuh
z#bg?};BYFccMRgik)zOFWb|glp0q9IJ1gI4Ol(E7_JQ}uWS$!io8hO46pI`eE#YP%
z(KfgF4Qf8)t$DS3S)2#^e5`pZga4hLxhR)OSx+l6=cu*woZIMWtD}{It8LDhk;3z~
z@aN;(-EEa;Q}_H-R5Fx5{_1@53lkWjurh_rmmVS}qC^_9sgK{+-+ZlSZLcUGX@L2l
z!QjX#Dg`lzK3!e>=ekJ|l1<su<F-YD-6x3=GDD;c$<N;?ztN0Zd(_kFYK2<$Srqj~
z{*$Og0uh5C>aGwq*GI|}foaOzXm<$wInbx-T5nkigo)1Gq!JinZXdEDn=@{yrT@xC
zmE(@Dt8$;2WFcrr0OPZiub|&@hw<-6WswHrtXOvM<L*h@M;6JK5bWPqZ&XT7(4)AK
z5tsCgk@Y6CFmnnv6Ip|$aq)?cKxXBUeY!e6XXO`na_uk*t3D|YO^K1>PVYQN^hOc$
z4aTKgrh@5(R5`N>Li|1B*DlEWj?QF|8cO9#DVrezcX{iA#?g0-Wxw7Dn;;Nb_~;TY
zFR1$Zp4IOs3Fw@+1Ny_~veAMyB<$*Tm*yL+;;AD<z9d(FEmE!PyjHg*GFjGUieja0
zU@>&u6IHNvO8^PmtTFcBOq*UP$ey=lMuwHrxD}I)>#cxRN#@jr`M_kKT?@uqbeR*$
zQGOTrS=-&-v*WImQ{ubv1!)SkVUAM~)*rk^!ap%?XWWhI%J~$ynLZ~oBYb$BCR`bB
zyjyKVW0*zbdz^z^ahov}8~#Ik|NYtkl?*;64E0e7u>~?}y5z*>R)%M07e3L_wgdq$
zW#gt^e;!yeHfiB&PvoHGpsZ!zo_sdF#pb>e_H5q~g2!=)5Zp!=_Jn@)#R==Af~2FY
zd>{6Q@?Tn?1N|*Z=86dAjyrExnrz{%_HN&&!MyeM(~+W(r_mQgxz*3lw=6z=OXKWK
zv^_prr>YH8KtEq1^jpj+xPzdFy~gXR(H38iu)`oqR?t%&NL^`YaD(nWnQ0AWTb#5&
z`GQ%q`-PI>Jo$Me_r^ukdufRUp0o1X<AX#mbSGzm9xMqR^9axRM~yyoIQylIC0Er~
zC><|=f^`;)cm9rGHXNhhQS7aJ#FO#5^-1(-0;>C7?<~l?UCnoGk%aCeVNRsU%QFSW
ze(1?#S8u=JRk4wB%zF2DEPKxTm!`7YXoEqz67Ogv)^~ymwOOL~=<W$h*G`P(MaB@)
z7{yzKzJ9#uBt?40=|n>>bNpMUhp-CQ)*y&N>lE$jCg18mgSS_IW%@7R?d2xsUpHj&
z!j5tLFTvZZ+o>>k3;T7*zqPOVw=*QKz}x=^_cdY1DgLqTdWm(xL7Z@?`){0`Z}3br
zq@NAXxx0Yvz&cAjC?SLv)eG5orh;zWH?+r!;MFwkQ^M-u{OnUFh~`3mx;*7bwLnP;
zga-TNtm!7tWFf~_KRm8!_SS7LjIan3bjnX8SySu5Y-CNnQzU$$UN&th$gkyHkq;1d
zN(G4;<8F0vgg*^qm-EM->h-66^SGEd{f?~E(}?ez(c`R9uR6p=mR+=ISe$b3x%+OD
z?2S{0H{}nd?p9CSHyAVgivR7tV}+!1kKsyyTK#k}XBGP#n-o$OdG{B-+iw_?B#Un-
zed9CvqMT!QL8cdq)s_WtV8Nh$kf^o1o$}I{F+bkm7aYswuCHaw?KsFUUJj!!rS>!`
zGvObM{cb4Z`avLD6Wtb>n^uY^PAt>){HY^T&@R9HNU*2|j)KKdyMvalP*H1a<(^tL
zd76upe%q^J0{vE$&JtV7pP4I9r6&$~l%L{iOlpSsCV$s^SK?kL>2Bf3B%EX&s{hGh
z{E33~uZ3AuTff_G7J*vY^??)BE;mbJC6{rENA0s4$DS{hy2PF|+IC5IKWMLyyDxmG
zkgbmEYuRqXgq_e#VXl^3b<jU6Q~oJ$=-lr{-Lz5OAev>Q(Rl@DLiu~lMmdeH!o^p`
z-Df!VcBM+|@$*J1$wuw{VQ<VIX;iIT&`tdgF!Br`N&K~YUrTl_v7X~}CjPYq3gKZ$
ziLV6vW*S4rQZCPok+in|Bvs53RrEhugB(|XGxX2bApCsKzr9<4|19a>J^k^Yyjy_X
zyY+{XL)RdzAOB{EdG%`=SJvQvDa7RY<4j2m4G!3g56~BhSGJl^k%4|<@ci*M<X=4J
zhW*40TJoRQ=l<hPuz&R&c7oHtuFv(y?I!=CJ{LH&78bPs%jcI??_WN@46*;!^Z(8G
zIsf=R^uMUj3Ay~P^}l)!j<5ZX`p*ly#0@4Na19IffAhaT)r$A8Y3Jr-W8tIb279A`
z!qYIy3GTar-m?fUWC17jvR|os`MXC94Ia+FKe+U(-~k`F?uMF!7o5aQ4Nt@Q|6>}S
z^B+@oHV$y9;U8Uub@V^3@b5ppWPSPXYQ4WdxfBnq?Y}?yTh+fkx+1YZb&L}T1{|9P
zqgO=&7ZoQL2MajZ7$&x>E&k`ya6VTlY<yR`_2<%rutVqiKS!Z@0xl}NLisqMhiG&2
z@PeVjDLjXl51chk&C3ndH(nk#pdozh;ASIgUUpunp74U3JgND>(cd5kR+0-Q4hRPq
zG@k>?jfa~PP<A$Ow)h`~E?L7^T&BT|N8qAR9_~MzxojB5l%0bE+Va&b9xf<5_z2)K
z_{a3s95`!kF6d~kra8g&nIH!)OIY)lQi11lfz#A4TLIT0v0YZZS{wo^!o|xD3UYzt
zd||08^Yy2x<pI(I(%|8NQcg~wG`w8kK2#tdSf2pppawtV0P_NsHaj&J7a!;uHv}es
znB*a_70L&0Qw4Lu0Z!ru^5=l+0K6_c8#D*-hfqV{iw~3sX+YVjx#4vIA8ybtuo^(H
z+^}aHm#TeL&HuJ0sJXywa#3@0aY2U&<jw^s7tHc-KyyHgVA$YS%uovLIXCDb;0m;h
z2h@e-0PO;H37mP(L(K<0>mBrv4U~qCpBv~CA2skvpbi%Vsw-ST3xRe*Eg0a%4s8$g
zo&!h?0>;A)#tbOn#qpPzIWM6JFk>8`@hcDhbS1nT5a=j?`2+g=4{3PLe@xlgAYi~(
zcwe54h@rs^TOV-B3wjB36-GI@!1?mj96T^P4Y$_+f<nMb0wV`A36&xzpgdqDasJH`
z^8zEo3sw@)L%7w0v*!d?jRAqc81n)KP?16#gr|9UplWn!tAH5)zp!NR9>V*~1qK1G
z1c6Zqpd8fjp#ut2WT3)a&;`i@N^$c-4G6px5129#Ie}9&xnM?z2dE8nd4Owofnfmm
z!BWFZ@Bl>u^|^U~;4jk<sDg3xfD&v_#e`lz%zN1?9}hI=&mKTq14fITngekBL#BUA
zgC2vcU||$KO>p-MpSY`OPVi>r(g(m?^WRhf^aIQZOgVVjp$q)qbm*7mxPU?d1%ZtQ
zp1!2OyzySq&&w}s1O5E(4}<>y3gfPb`|?#75I4-o{15#f3H%!pxS~LRf^=@EiHG^s
zzf$ne|1pL6256Q4d)598rf{zV>k_;)EaPwO{grd2@L!o%k?LQ$SBi3g8x>(6Apr9L
zLky$5zy$zT2BT0P&kZhn0frey;U!>0g*tuU0ATb=C*f^^-`_$J2h6u}12+ON7r++4
zJi|~DxZMV{#tp6+1?|8v4jkz~;Cw)=58;KnO?D187zhS{1{xqifiDDHVh&&&u;2jM
z!oVB^fE&;~zy=D!pza#N&H*SNFBBy40KW`;6NDE~9ssKT806If|1Et5;zJ=99C-bm
zUe5LvE&4N}<A99-PT`aGms-H8!5kCxwh10?7@B8?c8M2Ag%4mYUZ`UP;1;+|;BbMH
zg&8$2PAI12=7wTHfFq&9;f3N(K7cIQ0e*$zWw1X29XJ=55r7AQLx;O$FnADQ09?%l
z>Of0y!$3bDY+$^=?E>He;{|;Rq2`3m`BnOt)^Nc@&JF570K$YJM@|?=*yL~lhY#|>
ztN{vcrs3v+_Ma0rw_HG^pk+`F1Z_Ys08#_@5kbJz0`v*`21<i5K+!i8#c=@)3q_?+
z4FS6nAfFom1}M;nmV`nO7_j&oDEu>S{5RJAB^>?7k{m#@peW=odI?$CprD!?M&UU;
zpbXF~ZfGC?F4Tn%2&OgA#Q+8W(82J6a09Bd(2|^R_zbWIm_VSRU@`#chE5<ftbmuf
z<P76*nFjF<n07FoKyzWOT(%4234%M&IJ_e;3V~?`y!-IM{bTxS4xBL;42)e(gR{z^
z!-cz3SmW?om-ztv0Z0g11VaRn46JBqnJY`-O0W!773flju1F{a%ESQ`77Uw1z)S)m
zLSqt`mR_ZB)DOx-!9O(@Cuju%TZ*tXzy$-K(6|x+Q&?oi4dO6R24)DLi=PvwIp8VK
zQ!o|~4Fg>S@D{|!U@bw{32cm@U7-J*(D;vw2gV-&chDY)e*g!tQh>1ly#y-=)CJHM
z)*Egp@VzqLKZDJy%jT~|4>tvzKpO!1gc*lR>jCo@|GV^6b8^+Fz^4sPFFgs=q_DxQ
z9LyvG4dc5s6cC`PK-BOB1j=15D=>VZeXu?Oy9qWKpaO-NCU_pqFkEs6p%qw&Fc)xX
z@?j>74a^bTqF;W5TmQcX`oE_B`V40VH?ddKoN#jvb*QlB;59CDVQs;4pbHFuc(`x+
zvozelaDo|x23^oOg-;8t++VW>j!M1U&E$rU3`Tij5`j|=V2r_W|1gL6&t{2-6B<Zg
zdI-3ruDC*I4PKb>f=4@WDZ+VjvO_ohU?t!~fR=@&pf2GLE%`sKP4Hg8`*l^D{^kqe
zpa3Qb_^fk-?E|nJAkY!OOTu*yJ|%EIu-n6dKZb2k0eo?(l`siI;Jn~zU=M*?fWwMQ
zCj^x#oG;u({jE1J8_NN6QBdh!NnLTeTrf`DFkQY>t}E#)$_2~4I?G@u2|DBO9>P=y
z&ILC6u+s3)oUoBW%>&%bfOP?H0XE+dSUwjl4Wn0-?$3P@PFRO86#=$5x&I3k&IeAR
zXbM=^E0i1DP6*xig5_T>d8iGC?<4ZUJUI_+NIbxD0tW^BFVvwyjRW+qKH!4js1UkE
z07nlnL0v8u7+-k(%QnE&Tw*}jroq)k#m){Gf+nu4GYCwVu8QIx)9@S!s0H1h0EPi9
zZ-4~hTV!C@0ca7}>;j$_3<J773!52uqz)?sfjx!p3;<6Ga~tq7m-#Rb@H7No3(gbX
z3al2qJTJVJzjCh3?O*#+{}cEBzrAm8Wg7n4e)&)B2f%^NCEx*83TzC+w=J(=z)O1u
z_q8wzfo-0`F&YH;1pwl~jgP>W!pi{H2<HgLkWkM7g|N^)FFxSV0E~k=PndVU<OENH
z_~pvZC)ZzF9k5mSpLJYtllni7>;I(V`ooD`y08D9jth#rff7UAA1n%l`9bgj{J*l`
z{@g#|-~kvLwpgI4%SaF&5B{eplnaa}kRCJuf(Aoe0D*%D5k}#l76RT&Ln9vmbzxib
zJm75()Jecx1=xLnzNvvl#=sW=XazhO=o^TQ!3+V!51<T)XQ6u#upC$!IM9IrFb5Ve
z4AMbB%nnL|1qcCQ7>FL=un4rw2fk<l?*Wh#EVAVQS6#x#ekFA|5_n9@4O|Q;16&E<
zc_qyOd*1@D3(w~O?Sei7X9R#Icuxj`Q&<iU=s6f8ye>?(@HCuq!lH6cP#(Y&5cvTA
z4KE2cOF(ZyoDPMp(DWq*F7nC-_8+(ZVrW3UDHs;aiNh%`3|7M_2OONkp91>@Uc~(E
z5Bj<dDhjB0!IvvQ8l0*N_5E<b4oZU?ae)Xxj0crHJR8oG6DD777#M~JB~a!d(15;F
z;{_rB8x=5M!UYQl;1pT}&IJAp&g2sB{V#p{U;BFX^JQ%qe>jiJ9>Y;PjKatEHwr`y
zAQfzcaJgMVAULjv<#WP%&JE`Pi_YL%FMKdC2(NoduW*CDae&6pP-Ju2BCIYv&CYfS
zsA13s)))j9(R0Ffk>E1?BMkyS_^@CU_I?5mQ!gh8w$}n{h!Y0TVPb)m_}gT|h5^sv
zh0heM%wMIsVV^l+{on%cOhFtDRwXoSfVBWj2oDT3{$WtKz+Ms86_fJUZVuQS1M2%f
z^uLD$u1w+8uW-V@Bgsz9&c_SEV1<7I>z<39gZGb*0JZ&*q-N&^UgY0A(I;ph>2;dM
zK?COZM3M+U{xbF=a{)qovyn3HB6GRG>vUyn`noIg@x(Uc7YE8?8k&P=r$4ovUKNyn
zqP-WXl0w8*%zn-Px2ixNM<4rZL@#}>NG}r7YgkwvwpdtCUZ<tC83so+T(C5RWh1Up
zbeQGxT_oSp#~pAu4ZXf1;A-49tcr9u;XXoO$2AFvTuVVb7S=UdRMg-L`rpGgh+caM
zoq^XlY_Cb*GEG7v&k7C7i>r<G%bM{?y?B5~kHvrxNKUT(>U<mruMqKf9O<$KlKw7*
ztjBXIO6)ymgvStXLt)AC3wDu4^W{b3c4_&I>1ojn%Q;~qJ<G2$(nzn_ZMabA5h>jF
zyM$>Pw55^s_ww8?T7z(cZzP$S%x@MD-MghHN$lc8Jt?O1$`JjF{o(M8zB%$2H0KI5
zdTA-7Su>BH7m88m(TLcS6N3nBeeCB$qxB9K20uEEH#(2Ky&=!bj(=Jh{-nL8$0<WC
zET{Cbt9LLAHObO^H#(&*{axpGR~I7}P8pYXxbN%rhR}R5=&$VxDV{ob;&|^HmIdS3
z;uuD`-IKnEjt$`tP`!2im#P{<9tFkZxxf5y2c_}hQ%9cTi}-@~r9lCu-`#qLJAVH-
zED#TjH(OH9?Ocs_C-{2t{1~tCEG+gK1wt;?s<s+Q%r#+wYdwQKk>{+$Z5Cw>^~9tX
z1aA6Mb1zNxFxk$fkQWUMu}%*?c3$^5pdym5CAe!*Uc5Ne_SDlyCW$5GN5pccHT?P7
zE!WN0qvc{c<Ypm{4T>?lvn@4^`&2=<+b3JWj~zc+e)p|~98Y9CDagNjCznO?>{I+j
zH6y+8492F8K0*&i5yv$W(mGcZ=QGF6V{L(pyau=83k}8s%l(bT^Op)=e-EQ>M*Gwk
zAJR{_aaf%@*eIMj+0r9itcd=cPtEYS<}XB()Y;IcbGX%fc<~1Be0=+&;Nqfq`$7=$
zg6Hm2I+^x*BKq^Zt&7woO9R8DeajCapY5$rN$^PHLfDJW>$6Q7+=5E6dVX}YE??w7
z=3REf@^>%wn{zXLHBW6akHHnrW5dODrb2hNKrh&#@lg$F&YZmCyvp&~mq0A63!^o?
zT`Xon7Gj?eq?4iWx~A{s{O2xZZtR<Sedj#L_@-jFQtKtc{XSqPSy~ospdR$%Qhht_
zrKBn`MYcVwo<i8Q{Rs)C(zr&o^cEq|e0A!a-vILbT5W@NiW~kdcdboa7nB~%v)lI9
zYB#B-uk{3+M%Qs5E;gUzak^_w<GMJ89*a>?rd{;-pv4O9zI?V`|E=p<QuKuw739M?
z9;Un25h$Z@1U?R)_ec&byDc`oYjN|N5qoxl>talHnv5&kbkV0x_sDV=_w)zxA+A1R
z&#en2Azkhx)?#OIqh&+WW2?3E-owz?GbACzqC+%Saor=_Q}pI#T-OfzGodFgFCOed
zE*e7GhCwxQzEdQj?2SX*)6EsYaC7ApO19Br1y8Hak%UgG&VnxbL%iko_o=W?4w$!l
zb1$aqCy0fUn0oLmUX8beGWFE36rLqZT>Jjs-ZK8y_Jci<1V8(l$y<Rl@?o#W@*LAU
z0u8b_dedDcdM)QQi#_de)mAWvD6997u`TX9d_hj}p+!b5+SzbOD!ER1P$9k}#Y|mk
z(&~8_-@Gp*GP`vrwEE>y;1gHIusd(UhxV9hD?*c_eO*6bnMALx<}<X0wtSmYcsTal
z){L7(aa^zV=t*L&IT2wYVHQQyojAF>&0|OC#2bo1HGI4rU;O(*h6tzVmR9Q>L<m-W
z?|kL<V(LKD%&Yq7a~Q6<>Kq=t{F2(c#Z7m&BoNiSn@|cNNpVbp;_dvv`a&uefwTZq
z1%gFdFqiPRCHa02<Z8W0F%tSO-wK|E(Wd&l%V-6X$DsO|98;24r1yVG8Z%^77AL7J
zj9#sL?JsS1XI2-b$|k!+!97?9!YU%J%5yz_;bpt0jUQ(W#=x&bin-g*V|$lo6vhvV
zsM;5Cr_9L<C--;yZ(7{Y)TVm#Y+vVV#fm^Zt=v!INM|*6;<Zten|75=15?h?H+vt+
zk&iEsb&OFl1(|nn&3<n9@!U?^z#Mn>)|<yO`wEH>ekvzwE#JNYpRAbsrJ2<M9m`|O
z60X;|#0vxmv4jPpb7SU$ZGDYN)~oBLY5TY^c0=P*>#ND%<CJeZ1Wz(T2&81WAK~`~
zy(tyl3&?t%ftW>0!`&s1dr0tz&J5Sv0^iSB|Ba<uKORzBEF1EjY#bUP+Oe|SpUrQ@
z+hSOjt_zNHF3R4JR6Bkb_)dY`+i;nUPL_^sr+qWcINK>fw9*$>dn$3+fTff5u4dg`
z!w?_Cb^QJhn4-@+R3f>mZn-Xth#8W%DviGPn5dQXiW9hXYei)`OYes?%gbm(R+q7a
zdb?gcT4Tz{z4nnir|hyadNC?IE(h6and4-F3EihjY=&8NM)KX(-?d8ZBBUR*KB=*?
z&X%^v6{p081oie%{^s6yeR5orYG0k9lOlfCki}k5LC&hk-+61~S=Ye6@^7!T=;*26
z*|NsV&mnIMOW6nqTp0?R10PWb9-0<Pj0PQPJ)}IDG4~=lNk15IC~A)?8>hNoGk+48
zC;Q?4$m3mavf!`@719#U!0mNxV-j$cp;}ddQFnC2=Y8ULL+?kuUx`T(5AF==uf!y2
z-gtgvV$^F^91&YT-i`*VG_dal#SUXPzVEx26&zJPBQM{z{|b``J)*mVp-Aj4e$Al3
zY3BC&5Ail^{<7fZssSgLo7IY2u2u}1J_9H(aegsvnVP(7B{U@%<cKaJF<H*@&+}<s
zQ&W23j~;w{9(u37#V~kaR}~BL$(~10zIL&$(>}Sq{Gof_3OCN1M9P%0$q5y{j2icv
zk}nO{)RbN;Z*9ri{32?7xNzbTa{KNb?K}9DvXVAsq;-;#IM@1K7imVE9V*zEYw8yg
z^O8N!UJwmA>-2m?i;VUxUNyAm`gLC1eLd=v2*n{+wB^r*Uj?&lQ9u0{cN3YI59<&Q
zE%3}RsN!NUxX#<4Wuv-1;bs+r!|zQiRR5`MC+cNZVvx<vm!HC^com|avqa4z<XJZn
z4Vf$+-`2AbG^D0*-7vbqx>eXnEE*b^D@wLa7{_&?#>(Pp>Lc=nO6S87*+oTER}B||
z3P1Qc)02gKr6Iv=X@><Yq5WKum7hn1&R9olPIH;DBl~nj@(+Z+P$y3wVrM6Db3E_Z
zRUwmiT${~7PWC(Keu`9x`Kw+b(}%tRUkf4j@stYj0`Hbt9cSOX*Hmg_<v~PkVWWB0
z(K63#(d#2!ItZ@!QFya{l3foSu}E5&*8cX8mVvV3Q2%HBtVW-!mOe*TJjUrci@Es#
zfynl+)%;@4kk^Y9=r*&Wft};+!ge+9!F4|Z8yOG7<OZE@FzS1B^!-TCLJzjkCg8yu
zwoN|%`RmsZmvg2?^EM0X+g9nQ7b8KTw`296iY-V<d*o>iOq9zw#}qO4At2c_`#nY5
zxr3%`D^WsE>sa=N6@eiZ$Fo7Rc^I6%4#94h8S_Uk4tLZG;7ofMKwX<%xSX`Hw5LZH
z(PZxYHj(ps$*yaBoIxVwvn3IxLKgN<(frBrmeDS3r=CV)b^*-Fa5D%xp;*=P$7DYV
zam=vKFlb(o-yC*Gl3a@;^m0JC-tp_4Gqs(+%4qXTDw|JJC+-rav464DTwQco#_e<^
zvo0pAWtnK4E<D0|!?)Zcxe3=xnlZ$tB`~M7(vaI`Gnw`<HjVkTyt|%Gg>`&n=peey
zlxa+BF;nP>P9{uoR3B&{67fal_D$Z&xTqMurtYvb^uq=O7S@3W262lgx8vQuI@XrI
z#5X&cBRN<_U)IU=eTuLt_2G%E_)gmLWPIVra;vSDV(Cdn-AMk|Pr#{>IXyq*7?RPY
zg#0*V#~-vVlmGVbyr{^3Rkr<Ah;F_16M2Q&F1lUEJwGvHac2kZ4(w^^e#C>CU-QfA
zsQ1WOdnJXEh{n5gw2sr3&uhl(-Z@o}lile=d{;F*6;yvdS@KCycuSMtHrTVI!BdJ{
zMsOlMP`Oi$0--ulLDypqa|H2uCFa@SmwTW6_-+adMe)-pyeBLo_Iy;nFDJgpHCrnX
zEVEa#pwpQ8^cSv8W994-goYuHg>pt7<^3#~T=R;_GY77>W_>-c>5sPKXAQXL@sc>e
zuapV~ooS8N<2dJA(VKtun}@ugqzd~%ZE1Lr(A&<4W$}#c<<7Jde|Dy7&Wa3gCEsT(
zM@Hd8ZcFipJ4;D;{t;?Flz*QW*Q88FCw$<x(%<j=Ih|<Yf`hu7I%7tnDNUWS^>gUS
zqhArhW{lc(bqS+M_&AcGBviVRH3QKczaF9SJ;bfV+7(hNpBWSonck)M7JJs2i}+Oc
z7dRiLDGQHM49x&3Pgn<erEM&bDPbu;A=7Q+KK{c6y|;*miv!<%t(jXxB&~|)YwYYr
z3YXT-45>Uh?Q9p#!*mpdmSiCh3Vx?72R&9Kd^1Kn#-rGoF*!}N5g=I-`c;S~lg!NA
zFf6z7K^Yyfh<q0sy>4gY7zQ`jy%D{vn5dlB6VBtygI`VVbF`B^2-TU?mh39R$j~i7
zUnCpkc1^Ai)Qu9Ei1SRn6PnM+qc5|zk-1%*m_zgQtIT%D4ZmlV6%ohpld8?$2@*Zw
zs@-{WD}`9j0S6J82yvF;(`%0TleDS^W+neA9ks`s<`VPjF;uiZ6bL6V^^aa7ZU_As
zdzLoIi=8r?oScp*YSH}hv5s2!6fRow*X_q)_EK_ek2w*n4rLxK%8jrQjG4W-{kvQD
zm3@*x5Q%5EQ7GZDXU48LQzW}C`Uq;~tuR~Tjm;Y7!`XB4F9k&!Gx!DV+Qcm-7~mSr
zueK$mjz5lAJtp?Q59JXD79$R&i#9pj$u~97e^!Y*@p{o#tZLR{@Uf6?)x3U;r3NeN
zM>7*%2v;;|tk-O2Q6)bPPF4F%Fme=eP(+{Y5j!?wZKMyDPR~MXfHs|$sJ@t^{Hq%l
zIHnd;FKi`0yJB-_SbSi7x}3BlmpnHe{efq@R92&hx+jF(+dA2cm7DjTN#~uOg$X~2
zNMg@B%-J%AxJ9`mA&LieqaJDv>yCjhNZiBoX_S^JF>8<S6I7{Au--XBCUkL1Jy$n<
zzrxs(rV~+!`dyktD^K5<>D7z8snPZNi07H|_>XTWC1s2`jg!A%6(Q^*uSojhSer!f
ztY>5)tA+=se=dSY*D7-$)k@c5(&uLAy%x+q?%a#KiJPxbF>kdcO&L2zCZHirb@G$6
z-^~rkeN^(I;>PA{@@JSE-ZMc)5L(F|?K`No!G(TzEXw_t<G)qJQmrvP@{-(7=~sEZ
z#Kw6u$b!J(Ti<UwBrWQ2fpe1*p8CP+_PX0i`7|$1%G&j>tv?fdc5J&#-&40E+NX6p
zZ;-}La>6hs;+gDNP)Ioc9OA-EA|DTX3Em!IE=l|*G41(%btyH5*%Z>o2)Ucvy!d24
z%6|xmwxuh5YyODw`7u6PoEKh^H)kOG$@kIgW!PE-nwv$apF{e#^=>RH5g|P3CD2j9
z2|v3pD|(w`J`~F=Qp<_Ru)KN2@!7ZLdo3gbOP%O+(R&J>NTK|8b4XhpX{7Q^GnQdt
zbB?L*cLyR}gzQEJi^qCD6Z}@vHf|XU3u^JkM!T-4tRG9ny`%f##gSUycM&4r5g+mg
z{Odd_g%Kv}#y3+P8{Y;QrtMP9;=SN#+O2x5Hu1n69b4|i#fuYi-xp2m#D4AE?sIsR
znlZUAGZq=KoibioS42y&FpfIcGZ$~paw4y#d%+lA9;i>Go!XdxjKy=^Vyc>(BvIoa
zBYgL^OHj=h9pPh~rz)2S%Fu1m&1swOtASL#Z)!Bf4L)+B)29w8b%*s&_`QrbNq#AJ
z5W@Cs^P~B*?+K#64N#aDv#Wkhkcqv~_jxZvx6{J+q;5nw=Pt9-M`X37i`|kX5s&Pz
zjbc7-gvI9swKfXoo)~7URJ6H;jtOO0OGFN4ET2a<a*1tLg~QWuvh`^2tJcB>S!c^>
zeiMGsOc<1V$jKn%{#)8r-PExLmo$lIXDXyL;{eldg8tzcyD@s+NogC;CYgFJ2THo2
zxl13u7}9s4-1t%&!h*%Vut3j%#h&Z2*j$>%S+9yF*J#5XPwVc~$wWs*SC^j1PJ8eR
z*ZdTJh1lZOwX(3Wm(+ZF=&^W^moEjn6xMPZA+u4KYkBL|y_b&_!LLzw`%lIy5Ir|7
zMF~s?PKc;Cn4`xfD0V6P6INCjKYg_w@RTPZqMhZW_p-*JR1on?!oA^I6s3Amv;IlV
zm_9DNEW5Xzu75)NK8Fv*Ic18ALe0V1V9sQE))e0L)rQp$?uM!BUzAb&+l4EOSrA3`
zUkU}=G4RpOdcRE@pz%;?A^b*s{Cl%KZ*k6mp;RY*K|~%y^#MkrB4Ji{F@ED-gzN$S
zgB1n@qNmRJO`bNU?+0UZNtyB~;y=iqwjGOquC%ch8%Vu476_wLs^2BO@$#B8j-%Xm
z<Gjj%wJdF+YvEbpXTe8vhcPsRoJ1)xBrJVSg4O8v<40m2R?tXcrGBgR&XcWkYI--X
zDR6yv(M9CfQ+)0AyLib`>-rFd6+=_+sSKUSmyQOlB$0`q&hDjfY1I)qwISL^oXMmX
zAgb7e4zBEBRO{rv)HU<w_cRVTD0;R2$i-7u4V9c5IX7B*{~+kMU1iN~w@_Mp%j~E6
zrh$hTb>R?MrVk-mHL-?oTPR&pW5&6{J5m>rwHs&LxNco+(F^1>(@Jy?b|Tl6uzlys
zX6&-oEhecm9B!AH`jRGv*dZZ{&M-3fWMkK*E>9n)Az;t+czX4H)YcPH5jUfi3Bi{h
z5M>>0B3JyM9$zWK`l)tJjX(VmtxD;h8Y=tw41JuuPF#@SZ9j(F>(X1BcURMs-I?l2
zi?Q19>u3{k^lsQ;2&?cQC<O}X-n<C^_HoeXzK0{y60758Sq6<Lo#yjgw<t7~Iuv1b
zSG9?U3t4kpwZ2rU@%<U=k|i4hTdzGC@6HJKt5-0rk<EG6ZyEKgf7H9c$n*^r@x=0!
ztI?Yl>8l%Mp=x5oQQbMqb}6QG#2`6RbkRMbX@0M{w2+?ap0b|dxuQtRi;W_1kum+Y
zHM?cbkK-KGUO^;$L*#8KVxrinKYy$KWXC<tfard4p|VF}rS4K|%wmg}od)WbZ8=XL
z&%aJBjI<bT8Ig$FAVZF)D2cs!K=Ne<t<c(l<%r=NZJC>nlGdoJvh`8HQ_r)7xg)PC
znWEr*g)t%q)TGbPA9u~=)mseV8kR^<Xj%?al14gJeRiC#v3*AG*kSKTLvQnP1`oBF
z_k*Ne<O5G}DwC;s&193UkY9%Z@5X`~(X8u9-f-P?$-cw?DnIG1h@yFax4_O~r6sGp
z$~s-s5U-ixstSJ6(+(V0=jI2rD$jX#`^6o)+#g=cwa(e|Ai8ia7?Q*^J$3o=Xl6X*
zt6c7<ab$)adY<%e#DzX;&dQcL+a)f&X{?O7D}@{Pd|9oKm2Q`j1+$0MEYb>EN|TTg
z=1>!gTDPP;No9OL#JxfKk*JFXSNsz%k)C=egSy=D?S$yHoV>?Ra0%IoqwR;;yX`Tp
z$vf_<rmADsiiBAT{KB=cEwI*RJ7lWjzF|G3kJ4d@)panj&)-c?s&JEV`7RseetEQM
ztVsFsoe~tkvR=0e$}YR)X|DIASZ=*aBn2q3s{Ov_*<;7?`)|@Xq@M5cmDeymRC8VY
z*2Iv;nc3NN7pE2Fc7z4Vm;~$aVe5YN1|@5Z=~qQsB`pHKqp?87mpYGg5Z!NN`%l`|
z(4ogkSSDn=Y_`B)xZoyqG7hZ{Ks>T?#}7H6{BBJ{CnLaEqj?eJnJE%-CNZ{4c-Yuf
zkzNwr^Smij^3jWvri}fq`)|&SI-52*9?P$T)7B`D1^vp&aLZa*jSv}1ZqhxU?<d3*
z#qf{!A|oz*gK}-2v71NYW{t#N)_UhzpHpoj-_47{v!w;TLsOMnqdPsZPYr(TmMU>D
zki9}VWcbqHgfE(*O~2mwCR1SmOPkxe7>CX6F+<ciH)KJzk}!v4FXvP8(#%q?DgH@I
z0%O0if1C-etYNFTX=|m4xDz^=5IMJ7mlU3Li)x+<q_42Xqw~{BoMA9E)%_$v9(H_D
z&RhM(wu!7R8Vl2Jt8~BIw{e)P8%kp47IY0VB=FPlC2O-pes*C&xMP2Wp3*gB-mghI
z%7V&ni)q&x=g^EXPtovJTY5HeQ|gV7sR{vuHV)BGnxR}47volxcBGs73Cnlb%_FTB
zK5e}y*2E95M{Syj9zHR*dpwmt{-R9yzLBaxZ$I+0YE_@ZY_*pS$aEr#8P~TZ7It@b
z6IAE#X*^9G?INU*-(M3Ml|x=q8a!&T#i1V|SrPU2TAR$=!{ob8*Tzp4=s_eREl;Op
z`Tfc8l9f~zW?vf513Imz+rs^#p7>j(<@YK=GWAFtM!Fw#be?=F$vV&y%6RcY-Hxd<
zJo5<>73wwtHUq0nX%?ze-?zSTEfSU^yGC@~&o!lbD^8?z757QzD+3t<Gp)mQ>PL*8
z^A?;57vDtZda@o+136CnrZk{nJoaJumuQ?8K~vIK`8)LH5|~cOkdINCrv}__>L9OD
zD;n6qRcyGaEd0BQ6{Zo&-1^N0DN!W;CIP3fi!1m}^sKZs)Kgk2inok+Db3iBXmgrb
zQ?jnd1&`k^>&RB?GIXcIz?yL~Cy?Xl7Qv1&iqT$&jHx0lVquXX_Nv&@=u#t+bKe{&
zAzOT(GGXo*=eD}qxOhXg&v5eQ+?yuijwOqdnT-scLCm9C_t8|e2HVjmAtg~b1k!6+
z=mj`LLX+`b^BLmT1yqVeva`N&NAG=Zfb8=dm5!xUc0NG0dXPN%R&t!Bj8Pp8^T|$K
zb+Bu|E#n!iBmMN2i?c*y4x+$2_UogSc2+S(-hJPyV^uNsbKWlzX36-x!T)YvUb{JP
zltFUxLanR0PI$_8gvd-GU4JmXZ1S6fnll0&1NBl#&$&T~G?k9KlJPC6{duh8*0V&4
z5BH?K91(O6h9_#*;%sC#$1KCVD|-TOc)uYtBe8jP#FiC4?XR76G!@Oammnv}{?k5E
zH9?D`KE>|g+;z87+7|XfoF~23ZDk92MMK-h><YFrxUB2##cmr3d4XwcLN(#x=N!fY
zJN?e0YU~wp30(n7W)+QtX1`iQ27|+?o8zT4-}R}H^E@>o(rkKGF0K~R#z_Bod_YJh
z3>=SJJH{Gyih7z>VP&j)RL-@XHextGdz@WqCvA@^bsX_KJxe9f?6b1hv}W`;=H8*K
zC#oNoBVoM7SA#oieBZ*T*oR(ukdR9k`0s=xeOxDO?K%s4aq=T=vy<C{4lS>lriJ1|
z4r$9yt;4G?`AKz0>R!^$s5f?(#l=&+4^<AW$Qy!3MhOCYoW(6sqb$&jiP>*8D@T2I
zK%s4^+iKl)VV6I3;4uhrc=j|m_*Jpw^G2ulJ;I8F()YE3b^HwN<x4wL?zJF(c!@Wm
ziKK_K(vd;qvppf8^353WKC){|-~u`1g?k*|B4lSk1Ff)2qqeU|y`CId_Qu0{#_}@H
zTvoGY9`7aJtvIYX^V6mP^;{ciabI06<)6|fQG)AAY3F7L6c{^Q)5KNOabH?;e)*4l
zD=2Jy`^|&8D^dw5yx2vM+}AI=H3`y8b6s##;+kmmo$_~d6#mT<6A*#HI!cyWfZLe8
zi&rQ;Z@8LFXAI&G|M+fx9N8&1*JfB>kQW$Ni>6I05&!6E7uz($KGFM%AJy63HtL-k
zso&Udi169z`0<PrKCOR-W@;C?pVLZ_McniJ8>>yd6ZS?wg+6+f{QQHdtU7G8_(TZ<
z<a(^r?FZ}S8>Y1-2~|?jcmnY!rFN97rl?45%xt0e6i?L({AoH%bu=}0B52w~8~Y2T
zd(Q<(V<4@ct+GqHX(LNh#~Ya#H#}oT%GcTA6i)?jZhJN>X}T_m1n?5H?N#5njdopN
zjEDctNIkouXMk}wl5$P99POJABtzHl@VpM}ZF}4i&iF%zRPT}Ge34)L;RCy>m=0B1
z^SbM)Kg>nGZ+ulh>r2UJM{xSR<}0X=$VW@;{Cu0zihWf#K<86>l3%Q?#q&q!+lq2G
z;++KoADlxl<c2o$#CCW2IIyV}+`Ua)zIaYP^T|1Ejg?Ar#F}BFuwwXnUDW53a!EVG
zq>7O8qbQEC&EWX5sd?^{#A3XWd=w1yrh?;-%Lk`obsJXWWMgOu3}cKa1X;?cUnG_`
zPavnmyYs(k`B&_!QzG#4W3$hUe~5{luRM*vxwW^EW?ee@{c|_u)$KF`@ikga6J(E~
zVd4)cs8nu=iAk*39X+Sbokfpkl8lI)ztu|XE6os~g*Kb8-pw$J@9KESv!^I}(;$Mm
z?1p&fwc+gk!Q;CAuV3p5(s;PGB=Vp65&S;Zn0rlmgHh=7%TG^Y(4~BqxlB$q94w|e
z=0Bn{E_{EMN8#Hl`C9|C;wSZK!|{Xk_X1?rG*&;G8F$qld@>EQtvOzNai&;fxfQct
zw;+-m?I0E7#q>Khmq3u#OLI12=%s{LGaB1(Gkcui7Z?hoA{C$ezupQ&m6+Xf*7(7U
z`JuwQxlZ=_-H$?Z!fga^b~_TgS?X&o0_!Mm<0YE>D%V_5R+>tF-|0Jpiaw;BsjIrM
zhM!Y<#QZ)W>0NjCt1UA59e%2j8MTef+V38pl-|(1JV&2sUYi*TYVUfF2KIMK7tA$h
zw$$%DGQ@o8Xq9aLYlO^zvTHL&!_*gf+TmKvsC{FN;iIp|pZY!*s)W}0+$B}CQq0DX
zq8;<VCgtN&7I!V;;|$^q61*u_Afl!^AJF5@YEBgRv@upUQAYh|LbM!@Lz7U2sbgn~
z_w^@A^`oj^TWiSWI}4)y0?zW$jEKZ9Mto4s+KCrU&mS|BGyHn@T`k8u_#=ypuG9-_
zO!?}M6>+l9E>cra!WTz!JUgDMkA$$_w+?}n?vKCfYiY2i<T0bMvl=svK?pKaf2xss
zidFqRd9$ZZE0H$7gEqKNB*?FrKaIUZr$g7WsqRO%szv^<kEY@U5)}F(dLuk6is@--
zn!~a!gUX3bPmCgcEFWm(M7J<vTE#KxG%1$!d|SV%TdBc${@C%BuV7I&+YKx9;gDyd
zZ65Y#iMH)wp945=TMvG&|Gow;OHNQ{&s*#{oQZkF^3m$uQW~vcEMd~yo>T;4D%S8y
zrK;TZWpjKC?_vCN{|zr2S6>rZzm7~2^eL3Y%+&XgltsI=kQSj|ZzHf<zeyyR<c`a^
z3exwTAIkPARpFSQzUmBA^>e?&Ea%d_=T6YBF?--|=AqgAS!BC=i8e6BGC#=X{=m1p
z1NWc4_YRP099^+E897CE@$;S}%h1=bTxvYra2|`lcZa5AuAi9I+6*ZMuK@D<aVkp8
z*UV*?%z=AO3*Xh=-qhcA-I&3heP3IjIV*~9?wFf?7X`A`Mk0rt(}t%KSMwtOeLi;A
z+Xh{XugDuskLeRrcagc>ZH;eJVBL7q{!}uOqncwXli^rbQ2t2ImmcA%fPPiob(VMI
zM4bddysvPi$5&W?XXOq_2=$MZ<SB{OjXm0v$Zai{q<iBRQ9n{`I?_FHJzTWHe^uxU
zhIRUF??WWF)arvU?lj!ZLrEsQgNB0zh~A^&?nkWL;}v-?SlAbiKVW32-qtCR`Bd7e
z6mc(MI6tDfKXP7brH)f%{2n$7VVNy|_Dz>D-;v&`U|FoZjpTtmVq(vZLvvPMGY3tJ
z^orySIgvwwCSEIp&>_Mm|N73zUQJU&%4FKcBS%JE&r~F-nb-2tYZYkabB~qP9-pdn
zFlv5m*VFI{HXsSY+%pwpJed4oHgIyww4@e;fsk_#QrBXU;C|q4_&y53ZNL>v_D2v?
zYcDDq8tW)=@6LU_I)k2oSVKGm#_3>7A>3VQA)lb?W|8S5Nx@2%H{U%f{31DS{&>7^
zyv%Ho^YiR8S$OWN4-GF?Ci(ZZkF!#H&(4nu5xu%yQdADg3|>j^&u!UR=J7Nt3e8ei
zy&;BVr1&_f=RW_GHFMzg;^}idYeGqUne!uSDGX%Mc@}xnQjBgy!#=xa>qsjBw6&_Z
z*9e&<-W8Qin5VW{nkNrr=6CwT8<1%>#5{LRk+4=udb26bMQH@i&HU^$6x1*#Wis~U
zO6R<rPQEQLVlrCYJA2UbTyLs^V_)_tiKoG#(~AQ6n`)lt!Q-`E$FTA5h(=HKCvi3u
zM#SnD%LmJ*4K^J_4&o97U+hyve_oGTW8ji2n|S|BM-o$Lo*SdY%Jkd8>AhN<A#Ws+
zw?Tx}i!t#u0k&opv-xQ?RjhY^31XYP#klo>5ZP$pyX7od0b0o_BZbd6eKU#dJbED7
zk#=O<&hn^tsz7)D+Q{z_HT$g^{`jXYD@F|`xwq**PY{GR6^%({Qy|Y~-1{c4kh4aS
z5hq6H7Q2i;LlhlzkjWh}{l%`JK4551RE87VF9YK?&UJ&xmxXf-xrEQUd$^;RX(}yG
zJ}6n4VbYr>BMjYBFl=UAV!H2N(Il&j7L6*ZVS0_yF5_Locx^=o-P0Y<M2b#c>^0ql
zHfN5_LR3;mMQeTgY#Fj2Iei(0v@Y%AHUk0)PCqzE?O%j56y2eE#lv-z;A8imL1@Ah
zAwHSn2Va4J6E%_-m4u_uJ2JMBB$)XT(9*+;sPo%i3I#2(=!MK$GQKXHlwc1Ei0xtu
zZmTb@>Pewd_jS4H5sUFa=icr$6?B{LkBiI|9occS&?jjT__R@*rQ14r5&WV%qjPR6
z4L|=JXWkqt5uTAafRv_=EoD2qKFE8`xuj34$jav@F$Irv76P+9%8!Jnk4|4{+rA-5
zYf<s4r~bB2UV>UhUY%<?Bkv-=^h6Bu4o4!6$<!ed-^G>dZ5!FcRG%;%zNwiULtf7A
z!=q5a+xJ@K-W&-cj_F23-ZD?}QfP};zRhh=gG8|YtCL7(!YnqV!?l($kUYr4D{Cy;
zF~4v~>=F5x$;&>|4&?J<PqWO}zP0UG)wf1>zAf%K$Ji+|Ro6x%_4tj7Gb^sh^oW}M
ze5lGsT7k-5fO~z;<FmyrX4FaEFU<3Z@u{_J-a#ez)LYwdAK0P^+tak1OxhOd565}z
zJg3#|HT%C9JICP2o`-EG+1R%2iEZ09H@0mX8`~S(wry@~Tbnn(=l`(k)yJ+gXXf;L
zIMr8m-`8DUea+k@sM0lCM_2)q`=Ej&v|oK7%(1%4@!@E{n`E(iGMT5b7u__H)Cr40
z4!u!^kS(6(gR*^vX-a;n{w<1LD6L4}&_eXXZo=&5WNmXecqr8>-GIYfwK^xQD}g*r
z``u19Ca~cunX)`+S~eTiQl64Qql>i#ys8=S#kOJosZw5?!$x}=XeH#{y(jXQ#w-3H
z62emr05(q~P7*9KX;9L)&`$a)H_J>z7--p;?qijmWDG+$g|bzT#4=@rCM=I*`nEX-
z=YITMdQ)6#SnKXBy)ml30hJ5UqPe*2lu}`F>`O<+iK?{Ct$UW4M!e_IG<4BD0f*p=
zP9l`Ln>%GIw}l@9bFPM^nAhxxkc;!j8{^Ej?cq0Kt>Dx9Vk1e_>u>l9p#=}2M(^c`
zyuFd>%*L{i;^&EDA;ke-JBX|-$YI8qizqtKXP{X7u<v~nL-f2NWHlA5H#`?TTH6JU
z)I(<ObtmAA0sDd`5y7{YH!{wJnSB@u_6!3EqzSXs(w2r5K|8BXGC>-CTR@^EX#cN8
z80|4xxGGonL=R1sU5T4DCKahpZ;k~si92LtQKWtq7!G0g4e8>(eQZYa_0t*#6l|W~
zo2ZlN;X<im69z2?(QljMj3*l8krlr$5rd>!$L1w4`zG~%fM#8EvL3g*jh@L~lZCwt
z8ixu=tk;<icUdCZ$f^>HUESG0EkOUaoF5^9zE_&2tGwP^$F>-nzYl_F*GtVhNzE`~
zNJYAYeSp*M8IK*stmpGI6u4$L8Q=*N?e0-4$miNc2o=bjIXB2v65~t~LywJf;bAOL
z=z%uUAH$KRidU6u$T%kmAqp(*upW&}d(2^%G~jof$dl4|=Xr6d3|!PoV>6L+DW3{p
zH}gqaS8X#*6y{z(xvV>UnA}@V*K=Zjy)?u9N!%3-39}u?!-w*kb!kJig3|(^+G4iX
zfrf-ky_@f+yh7%I^R2E#K5EXC7z4xbL+4kwr(&tkih~Lq#Q&LLPa>gh%}RmE^ja<(
zYp=fbX|A+Lmu7Vx6m)9@Wj2hki%I)>sm%~l*uH^1(Mx?_W2M#3E2Ief$xi3R!-)W;
zh?NVz%0xCoK%G^cylg;!yT2OKY!8ljz(3M6^P+y0Ow{g1BvXyKg=BF=2?3f2J&kA?
zNdH24VleC9`MoT@`Z^iZd`@FRB>=-%2l*`jt;;p)*5B0)RAR6?d}TxcvjqJ3cW5Qi
zmYnvzFH1GY)nNB{UIoST$=PuYT!k7CU`@5amZ6el_f%|;IP$(w;Ag({ZN7aqW1fi;
z?5reC32QaAIa|cnpASYe4``pCGuN9{BCBd>5$p+X=t|IaN-S3<I^Oy#&ZoDHE6=0J
z^W&3)+!<;!pU-z17^c#=Pjmljef<KJDhjIf8S@g?U8HoKrSw6Co2y(4dF7yn#8d0F
z5uLQDHZeNUSqJ0hQ?ETB_yCF!MchY_&lz^`d8Fu;QOaB6NHc^(DpSxf4TL$Ex6|se
zqQ-Xscvzi*%VQBGGIAiA2LAW8<OyP~P#=`KI}YWoNW>nYzt~#9Vs6`Op0DB5s(bTB
zJ?5<g1`O;IdO^hlrynozB~Jjg-d}28y(07H=JJ*xYWvBHxp!`GUa+&-U1t329Cip>
zo(h;x-<)J9Bu;ebR1v0XJ7~#2+~a+hg}v)(L-hgsd_s%n+QeMRo+M~w9BC5n$x1s8
zbAKl4@+XAX+T4dG<XA&1qV-*Y0w)w>?(U8i<e%sF*KtSr5cCy;yBrQGvoxZ-^HZR-
z!t9(#`HD(P$vv}}LSuI=J=oPkz>z#SknT4`jUzX3uOlhs46>8i{w9Y*VSX3^YjITE
zFY^_xrCBSTlNDw{GJEJKNrO5`q1COL?rVu_wc}*o{MZvNF@2yptsU}KyA^93`P7<9
zG?{5n=F~!%DJK0W+jJ*i_GAWg8DpT|zD$IQu_7wU^})RxN#ZhdMqqdo+2xTUumiyx
zWN3sGE)A>(*?Nz;{poW}dh)i#yvpO4BDh)a3z=c7mi70_!Ehm3c`S9CV|wQ*{w%0I
zek#{AZy6C_qYyEOb=F1;cPoMKmrx;9Kg66fwR*<C@3MG-z%aI9!*Hsu*nJ&t&=<E@
zof}y}^YVFcON9#}JMyN@Zv^CD1TUQW%$7zT9Oce)qiJ(&#j$B=qlL(gT&z%02Mpzq
zT+^j*Fioa$M!U^HEo@`KqIsR-Aa@W@(>jOn>w1o)4XGubDt3l;x6^6vyvezC&|Da5
zJR)N&K%DS84Os|G%F5mnK3GNLSu$f=GQtCk5npPaY@7}aqm;!c<mS(x!WkE6-w*Eg
z=6tR`q@;;Nv(-{NRreI`)5_=%O4%g!*<^fhqpdd8j(i6##q#-=Uxg(MGT=<eZD!#2
zNaoa8obc!&<3lyM6u&O~o)N#0TmPz24=N@9H1E&~J$0JVKj6|Qy$~`8P~i){)lg11
zNm9lgIv+C05YfRnHaK9z{~H{I<vBiS-lj6VupK)oixcq+^=eu6OeL{$AbF@4UQ?ni
z8L!O>>Ow-SLmK4z<3kErk|58x9GsCtam%-KHiExN>EO{|s1oK?=SpkJ|Hz$20RnxP
z*n#H+H+Sk?PQNll2sAWJIFF_Hr&3(fDaG^8w8K?3nL-{6{j~v5edid7w*7NR6wM&|
zOJ#ur>mXpL!n7gRM$&Ks;~b@N*>bS>sT8&|-mfBi#zJQh&qM7Efx?HsU1uUwca>be
ze!YH1EXh4<avvgt@TYs6f}jp`uek87n=MwBmMa)qxN+$WL&-Y?B=x<J=c44WnA@%}
z9NhBHIiy#-1$(pQU%$ZeuYCVdzUHh+IDZ7?mxjyf!U9y%QN1(v1oeWTH$3X9iHRFG
zU}1Xp>vyUsp3{T4?Q%3Lt@mM=opU)P>$I3qrhdgNh3IOz)smFh98P(>rxdxbD*gO%
zr8W5jq~xcZK;U|JP&Ly>r*iV3D#%*^%nXvprbe=2D=<e&#2*>_uqDH!tSk5%xznM)
zBT&2>lA6wL74X8>*VjOcO>{rO7EFgqtR94cc5-IVs=8__kEH~p6iA~dW_~wwr?c{M
zT^{>%VLtrH!MgK7Gs~Aw$Y7~`(EB;|Gc(Em4IItz{PRNP`!pANwzMGxPZH+|d}lh;
zR=BVhp*vSQGiQPyOqM9!2|mNv{WC@yrsVyts+3tBPDWh-==~&0KMHz5%`qYowS?ZR
zCr%ZPowc-5(JGsEvpBRz+f4c!Kv*61ef;F*LsC(Tb(Omex3eg3xBqZkvs-&TE-_}@
zL49m8HN6E{Rjr=!m+m<OM-T`D6J4mbtD|-3a3fZX=RzqWD#c1Mp2|`-VU6YVj2*3Q
z1#cC43Qq<olw<$EAuqRSf`@DUk$uOwkm=hkhYRp=Apa;!>aU_kt}5Ubnc&fn-G#bW
zB^E4Ot;p6z<jvA<!7OZY9U~<9-6$#qUC<*`iN1vr-SG5~8n}}6WSGNhR3*Lxp(dm<
ze?yXn+G2z4+H#kv7UOc;P8M!4ObDOCEix^az;8!;#?RIp$FCZvKI{F^Tzh`|zM|+}
zmGHz+4BKuf17d{&)itPQ-MSzlKjL!L`QE*H<BE!`C;iV#$LvT1O3Px^))|DWBIS`f
zo>cV%_(>*oV1*>qJQ5vh$WX2vcw&0`^laE(ZZ5|X8^P2k)P(5!;4V*$K@J9Tdqq1F
zr>vkSpg$<l>Cj<&PWHI#e*8@->Y{;Bl$%lOppU8EhQZmX8vC_K$0CS~Ij6d%BlK1V
z9U$@<Y6AH`M-*geFUWa@gn7nJm0NjcelU?&yam6B|7ntP(b849%nG^I*G>`u)q(g^
zy-Z|o`WcDC+-NUIn+CX9<a9d5J|yOvD|RKcBYT@1BH@cQ!jn$mVP|$byW^gxUeW}E
za2W-o@}*$5k&(jkM4FfQZ1B&O6p4q5n_ulK|3M>km47YNf@o)B2ZQOK10^hM<}31p
z#rqceQ`Q^#N3`8<raeN{Ei=CzsZR<5%PKwjb-C*0E%rN>ro48ZSEs`^32F2r65j-v
z&yE3|W;t?6`Gf=fmYoVEB#r$abedH_81yM#`xR;%JVxedtlbaDlEo=IGa^?k$U2VZ
zx6%;uPmIuE?VyXe?w{YAeqrIddW5M-hR#hIbUt|#xMvj+88g$$i92<T=h>73cSH>9
z3izo$Yy|goLtUhKMth|@R?$yroN5Q~Grtw<lEwdYR0-)-FZj-d>a2eszbo{j%r-w~
z$qmEj8Y>Kdp^kuhBdRm5Wvy$2xvG?aSA?KB%Y|DV?`6zcj&-xMPU$LZ6`2EKV#!n#
zUiC?%d|XF&1!})Fjg?wXuJXl+2~pQLx8Yo>8jh}Ku|juzp#rw2=Tq6Bo=|RQD00T>
z=vaK>g4^&aj1vr9pi?eo;i5SK^=!>z<C@M~8PoR+P-+Us0ZhE6{js8D?8M)?2-`YI
zX11*lTzF~)Qub4PZR+q}zyHY}Y=-tu{V_upP#ldcG=FL;l)epS4Cff4)`q>(<0#wa
zWZLCedr}VqMfHaWJ@dMT2NhdZRn1HT$1lSThGB^^VIz#RMgnWstwC_GPN^{?4kN7J
z1=F(`Su%VWi;9@?!h`6AHg^Tm@V6nqo{kwKPCjG4XSkUr`-|i~#Rd0_w>YrdCJvR5
ztaTq{=a92>{wmIckkR)kZSrV6psq+QP5&6$1LOn4Z`O@-Fq?NaP+N8Hdd?gq>Zcg|
zj=Nx|86#QTuh35<BTHhyy0k}3Ag6I79M45pmfeL7R^=0iXF8>Ghp?biY4KtUIigt1
z{8oEV082BsnuL8`T!KtWvAlreB!ATX8S{qpGnJ|(AH$#<{&`G{15Z%q>=7vX@k~Q3
zzUmSL?!HosprdI{*=+1olyq>|b5_1Fr3WAl{Q#`pQwVW5+A%Rqh?q?*DcXh-e`-zK
zLYylBN;Hh)pL-X3&O3RYL&%SHszquEA$_P|E~oozxI<sJvJE80eVc%RYihxsl3cZw
z+6o$eL1`yFSFwtZ6SDJY-755b@nQ`cd>3Nr2^ry+z%3Zw__epJZ>y~MmGuEvFK%Ld
zqUaShHHrkM#KGK;>{?QBP(~>>oJ=?Q5>Vwm)@sa~J%#-ZgOS~Z@3$+~m*-5uBhK;T
zJ%#v<)DA@0-+l46l<C0>eI>-Agg%_}%Udgc!Uandu=npMOKXq2V4I@R&IDCFu>fE4
zjA_(-vLf|M30s8r(GdG@MIVG<{?Vze^^YQTE41SWQckb0@RAtCEw_zJfx{<Rfj_8x
zaI=jMbQ?@eYLCxEn&46QY+6uQGFdvPFR0pb723f+w)ND{2bu3p%Lv<QZ{P0pSU9o@
z@*rsoN-q~ZU#f7Wr-0Ki)#S1iMCMcDpT^HZ=6_`C)RYP&%+vUa{n&`S>`EhD#<2sK
z(^ljWjDq@y{bCr%U=S{bU>K|c+Jn@D$jSyuqu-X$t}XzGixsm4oEFhnhmHOo%U#`v
z*<jzmvvlz2E5(mU%o=;|R~0QV+3#ead0Blb3Uv;Q?#*eg7xZT}ykwc$p<JjzSvCtx
zpdD5XN(?+3xl(th4$|v2m~g19v~F5f$QP}c2(v);R@qZ#k4I!vYih`&Z@^HK-EhYl
zNMc1!)35MNE)^5#T)!8UDOTL<yi!%KG=LbS_0}XwSOp5Tj1qt0+d0cTeo^(EE7}R)
zBkfXH_LU5=Hb~R;SgT+b+XiEX*Jp#VI1-1rdB576%Ew^jlb3A5>_g3?lGEmgZW~R9
zC`X!sZCeUQi&nwT&SWlV@5og6WtW}+QZ!fR{-IJ6eN%~+00Qf-dCI^PJHFi?*IBa9
z*K{bA`!*{s`cQg|)$P57`r2rO&P7lE7be?|Y4B<a^Ibh<f!bjXO}E?btgh*n#a*gr
ziZN-V`>Al+?B3bR%gv)zuy23op0|^e<n~qeUT&f<*8zZ2$Mf#e5w(YW!&3l2!{50S
ze|NqPmVuaN?Z%Xf2#;UF+IU!&;AQ<>+zJZn#MMa5Cx6dh=FP&(;}|Xl7g2E(lJwy*
zW52^1YGlUo=Zfu{bEa9Neqg_$WyK3V=#s#b*s@VQTjqHOMU|McwVQax<cYD0_(aOP
zsU`yCX_@8SSG*(pQnoVImZ}w@WBw?sR>IaQ)$`35gEv^}w$XUgbg(rN#1S`+O!Q)w
zr`w$ym<gfMIg*pHJ{hA`2U%E9<prWAj;6o%D#@;(<5*h^8f7hu7!rBN&<;k36c}Pf
z2OpW)@oHq&3+Q2x?aPvf7-t!8yY;y_SkN+OS#n2qvR&xUgcj4}6|<yaZ|%eC0#w7x
z>VS#cszfMJg!;Sk-Lequoj3KoV~7B1bR9Z`;AO%e?Q@zG_iA=?!lz(3ylYW4ddlRM
z1&BhyFe<51qam*+>Ygkio<A^H^R#)Jx|Q|1;CwDQG?l{XJtvM22-AUO`oQX+v&1w4
zL^dEaj25I77z|A4eyXc=D`6!u2E298U_LJ+tsq0Nxl5=jYdD}F_lt-b)WQ0q_`5mc
z@}iFf-4XilCC5G7-sc?~bG)k!JOfF~d8kzP;{`u-$XzT6Y^80`6Q<VcI_z)#A%h!k
z7cS74W}eu3XUYS1ir|Y#^@+^N%lUhbHW}z$Pe+gOZ1(eS{jBrk=z62i;!N@nbD?PL
zg3Z~eSUO#9jRJ5u5GA>m%1M&`wcCuEq%~SR8n3#YGkwQDT~8GB%f>Nq3582%-b%N9
zmDX-E)fBV01|o!k(nwi@5Ue|x4^#2Tn<Jo6N;qh9kiDVa4|*ewW0({c^QdPH>UHz|
z1jG|~Qo}|)(pppG0*7dh454&LYQl;o4ihrH_jW>I*E7{w=swC!Rso?|DV0|6SteGy
zmt5XeM@&4KVAy59#3f}W*5D&gt)oA!otBY(Q2?5%kY9Cb=CaO;t{o#S)ykk_?@%Ab
zyygSzpD?UJD{>Oa2LVQxvOHrt;p@<?b4<xF#p@g(W2XDW#CPs^*k`73^NjC4k|PQ}
zf~;y-ETwc9j`jKXKR^OGhN=b?^LB=wRBchCLN5jmUs6o>4HS(ih8xMUxI8~FC2*Iu
zkUc?R%c*iQ_}gz#JL2b_?_sJ2sVtsm<u6;df7K02UGiT_-uT(SS)Mogot@CrGd+f&
zo7}{}^2W!3Ho+wAuK$Lj`U{Sc6@EX-0rrT=a&B);Osb?%$LccHBUG~4i^7rNuy4*<
z^(2YGhu(p5`~|$Pc<cJVDZ}`7X#ZF7;lIi-{^^$xF?BL_w6J%!bA$%`GnO;7G5v0f
zQ5F@^kf#xr6;>8z{EiokSsR);5wiU2BSJ!U?u6Rkx?nn{e^U#58w<ZBEV|GDK__EV
zTjzf==g<IQLwgBR3o~=)f4oP~fPa4GpKIUWCFyKvZDA~EYi4cw&rsRf)JE-F76A>=
z`1c!Fm>IwCH#c-tHgzVX1PB6z0KxzffG9v5APJBL$N*#k@&E;ZGC&2O3Qz}V05k!H
z03(1gzyx3l_z5rrn0wfpo7w^_09F8NfDOPFU<YsnI02jiE&x}68^9glLG`bhh*^9;
z0|(1LfommbK0d;K@5A`^ng6%{>OUjc-x`Vk2wH#JHJF*%|6}yu6WGj5?Eea2|9`aE
z#Hc`VsJy=;m|{s2*AF>VY5wdpupS}|ssosw$EF+O4o{|m2}=Zjzv_~Rg_3DQMl<@C
zex0ZlB)7VVDyml~yy6M9ynpe}Tt83VXK`M?`)*&pe|pD;mS>iBvxXqSt}~zD`oJRF
zfd7dA1pr{N1q$I3%o~A)ga=^uW_z1}ga#l?>WIL%E(j2LU`QPda_J&|kVL?8{TVF=
z{%(^qwhm=v6fOk*!GhfaZBjc4q^*Q10vs;Un;;0W1_{R#G-fQgr~tod{)je9Zuf(M
zBP(Ltk;?!NxSo(H0v=H^OMwC8N`ib2NFE#ugtL=C%3M3xkwb<Vm70qSTQbss1dFzO
ziH(;KdoLISpAg4HGyv7W!lE|>6}b8;RKy%eFCXz;doT&9KZ3kY0LXQgkV(R9fK!JH
z%mmopt2gNnY%7!C8++|NyFY%dBNq^OgeFZ(VwVbJ0e~WD)I<z4N#t12nHmvfPM~*b
z8*40L_XL=f%M?nYpbpEPk17&$PNI*&z{3^U!0r-0{tucux=E5IV$y(EJ<<AnVp%cM
z4d=!jmAjY-sKjwClJxzW5N$jx0S`ixXV0#)7z?lM6l}mra(uBUr4IRcCcLPlVBjS*
zq$^SPdl?}_Z^4{73JNZe7>EHZCu-ATf7A$QYET*(d#9*3Zc-np&>zuWcS_LUFGL4x
zB~(az4IKu)tK0wrI#BQ*3WQ7$kl?=&@ou}gF2IGm*BHPw6kz|vg<^#R&r0_+wFsn{
zpn_))zr*Wyn<m#pWmjbvUY$)_DdOOW!6HJqEq!}jK%-+E{28Y)!@ue#*MTePdoEl^
z1Zf673-JT+`+@9tT>TO3T;j^mEX`{m&2VmvAuzxv%@Jq+()$Ckpv#65q<v|EPf&dk
zyr-6bxB-87k$Ta`G{es734Ra&QGpcPsu402x8Z8wi-6N2zxCvI|6x}|5riI$d2vhb
zCF*0UD^WHqKR1q!H!rmF4YxrU@<(M47lzmUIeli+OJ|OV=WJY?Uo4N16)g5e10EHJ
z3-715dXr5TCZM6s-`rG~gq2K&gl!N5j2tDpM>NYY16!++hFGgbWiZJ!1dN(E{il}-
zsMClCvEP6w#daAJCa*G8XNMNozh1k+ATm#9g;rS@=&<dlAp<y2<SPP7tQdVJf3)~f
zy+&<lY6R#^w<W-jL4X0Alms81Y4nK7u!*WFmtK=b>^$xn#}(VlPv@W<lZeT}dnWz^
zkKS?jFj7pEKqfA?+2tIrsr(6p5c;P0C)K)XhDg}H^?JOJ(mX`b3m!bR49PjI&g9_F
z;HEG4peq%Q6e72R8?c;}%bOO<p^+_YwHH3B8I@3(y=8e{l%#~}z{qhlGF9U0q@WG^
z`ulhN4z5|`%_&d_&+7Gpfv3`JL>FtjR_4;f6N+^PUK-sc^2htnK6)Gn>pg)pInYv9
zat#N!wurT*Zr7mKS+-I<_1EgtU-(>wthaJCjb=~z4Fax#85yI}WUoq=Ty~M}Cen-|
z-h6dFGCH34Oxd?qvhM}&BcImA!P_y{A3tCxC?cHOK3C9bLo`#o%Ds(f^A^&<QP~f?
zIg1xA|7=4Tsmj!mR-dHec`Jx~<elt;@?MLYOgu3kC)-B`DpirR4&r#wAuFYq+mwiz
z$#@S{7oV4&J+7GQOL*B3ZYHXAY4`f7D{A5+cu)<O=Z;^r^J!NiJ-Xd&-JdV;Y65p{
z9iAeWo_jfO|1$q9+McBg4)UI-`AH2YHu)9Vwxf6Viu*N_r*{4BrYnU;R417kcP@pe
z#sKi?zHZvdwi)V*YSXXJ*`d+wT+}deF0Z>jydv?$!j$M5U_V!Bm~kQdkQ{Ct&B2z>
z&W#xw)P^)XJBA7qSGc9JDd(W$(i)guIKB(tIDMhq?TeKVTzj^De*sso@xp39*S^<v
zS&xQU$8LZ!(?0<{*}21H>$JC66+JWT#`-mHJ1z2>mh{oB$DBh8Nc6T&QMrTRrdZFG
z&&e#4zVHCp8(44p>YM$&eHV46VY9EJzFx_3$nHOfw^de{>|XT!Ti}nmKe?`C8{-@$
zqk(vQuM8+pxzx}^FQP*H>ss@N=3*aX)K<5*8rFs)k7BF+dO}&U?G(KxjDnqPGsC^+
z$dM_1QH``gy6nw$8dB-YbJ(ooI1_J5FVR?zz4K%yJse8c@3u1P1p~mk!nT&!P+++m
zF!dBv^^S0n=yZBS&v4Tz?pF~R<$Ho7lzZz+HPKozq_BalF;$q4%8Ik;jEw$)LYpbm
zp=YlXB1U19ZE{@{ZYHL*^O#-9f3vrLvMSp_`BfkC%HP9w+32*Yl=#GrcNyQm;GW5i
z<UPE-PkiaH$MxKss112-h9*P-akbppgrls)2w##pF3)58cd4q?LKu_Cc7iuV|Hh9@
z{7l7dQU;1AlN!<Za#HYj2Zv^M!<w6@W5w^J0w>8{yuaMl6b~pWpHDNvZz13wxIP`C
z6v_eVJ$THSy6W0~g9~1Tsh(i#7EdnOHxt|0Xe8uN1IM|baILNjfwW%jchgO(#EZ$~
z$+$ny9GK)Y6IL)c!1Sg}quFT|jlPmG=-3!vq&M1|@#^1d`_!OKaifRCrw;iZedkkk
zeHhE!B~Hq7_2kAGn^&)Wqoumq1dqPgo@9+=E6y|R-g#t0<}V))bM$NI?~VrU6cqa^
z_HCUczVn6WYYcA%A>*WebP;4`>ZT@V<*t@Jpz&vp+gf#9pZ%|M&)EyYEx~k4O{CJ?
z#qW>-RdteZN368eiSa6DPPHdF3K@5`i6CZv<wjxSseF{BR0eMMM_zE_E{6D-JU%p~
zj!!CEo~1COt1UbT#3CPyDvoa~?-?$qPWaOk4&$TQ)P*QKryDs2+fLsFqu9y}g*x}c
zQh>U^w%hOB{Uo?WsHqT|@C)F3v%Q$2fz+&mP!%?x)jt8L_6sU!?V0UJlXXitHC?tX
zD+g)TFSC5QBnd<<T^&PD7hG@UzonSvZ*B|gA-5ck8+M^Scn{h4a?lT|2(X$BjSJ^(
zbgX+!{*+q5H0;qNyo=OJur>G0k+-0iS7?2>%G7MYo%1Vwmhsd56*z~;)rSt2rp<|&
zh-v2Q=Q(w=Bwd4YX(NHPU08@(CllpshQRi_JJo9K$5;0KLqC>XXp7$Xm$s||(Ys4q
zPq(vBQRC29Z92wf>~jXBv|rz=0ma6bYeheMLAJ}77~RSw=!vN7ns4h}8A6wKXa$ep
zXj9JQfRm5uLu2Q;Oj(zxNmzqpZqKxx<FtN9!Ye<%zD~_9#wo;ct+=MAjiqE5pTBwE
zG2-Y$V*g6=VfDf$R`Qqc!;=HrE(7_!B9d8(JK++!;_HAcf0nbMANo`F5*XU&4cKPP
zZPnD6Vo6gBf+c)R(a6tNn~Li-M{zEW;V=fAA|P+J`*IXsA|wOtO4TfsmMD^VFQJI*
zG3TNWob+**9#uTvk_0gg{t-XYOaT2fmX$?{_8#L+Bo+6Iw{hCws52D(dWNXc-sSlO
z-QE3<;4%v{f)-;E`?1E#XKlzgj6LOFW}&!H$wPbaAE|VHS7^C+olZO!BVoQ`@HU0C
zb8B#yHO(VYd-s%(q$0-lenWA``BYhZ<8T>heztD&ce?y}97;1u9Q-m>R-1;{G~+(j
zPU-?Ba_cP!r3UVlm({7i_*}-{{!S}8er+?~B0<a7FW4b`#Ik#GHc;K0V5fgs%~p@i
z@q{67)w1oOrFc6|eCYU+G~GxyWwdq(8680u{e3SevfiD-EtpZ;LOJ6sf$Flsq0Dp<
z9kM*)laZ3z@J_@@8-FR@>J9Dipxs<6c|JV|j@0Rt36gWHM0s6^`dNz?mzshcU!fjT
z221DTzCw?}Fh+(B3yOLF(S~q@*!r{Xw~oW`mb9m0N74hkm>NrmZNbIe!|MY$n%HG>
zyWr9$;=bRz?}DiGb<vRMX-m{(v*l$*rp@<}DExK`w4S6^Hfjq&13u?*`jdv7=ZK8N
zb!N35xq1D`@Fdha2J;67VukMho7rDrRI|6;1s5T5aUep~cF08XTi(f+c{;nHyr~!S
zELrvnd+H%2*hmq(QSTu2D1D7-{K%v~OQ~n%3{XPAj;D0-i!g;!C%W->gk~ER;Jw*C
z9!F^;4(j=-H<`F`R^J&Pi?INZ9AOnI%7`49r5=Y(F>w-=T?}e2NygXlheRL=HPe5J
zyt6H$#w)x$yZjW~ZeP|LZ>Z!+p0CU$?j(BJKiyw4@d~{TkcwJDH26NH`(ZHr=+oHM
zR)2dj&(tlgbv!TL$5`ty4Z7iP*gI(-T`VNbtLuJ-HB7)W>m87kg>^q!=5DOX*1sU2
z7;t&~Z+MgS--W$|JUq~V{}*qn%ZZAJN`3RDq>9S_i8mR)d6W5{HiB;vA1B*?;pVsA
z?SJ6rKg0jPO*$r~Z?OFD#7XxLoc@bB**O0%%qjZKn92ZqfW!Yl#eW0C|A~tK?ymi}
zY5pfFemfNZ0}lTw72x=Oh<{!C@2JShz|8co=F@K)wXk+Jb^NYnv^I1$6)`ooGco-J
zQfMb<M^i%^X!rFPH!zijuV?moBIG+!QWHW#BF6R(ZA`MZK7U1=jg$Czg?4UI(fDN4
zQzb-F5iZiA1lt$+p2zR_#&Om$C%4)ArkDQn_Vf9$7Y<EQHD5~>94?4zXh&XK0FU<`
z5EvaDD<Oe@oV<TnTwHG2FEmhLZo#i*lm={}1WL?J(GNc&)OZC3>_TPGLPi0)Rv3Y;
zDJURRAfS+;0OCDwZy;YjzPg?O`yvhjqzm{4VDnp`#a(&@RK^Mr+})7iwY5S<kH5a4
zwc1WV{zgW`aef^?4zltk(pXo)^1#G4gtHm3s`})Bd|FvyBcI;l5HAv+%vpr8Qdb5B
zf*>xU_X0>Ri#YLr@EDkT`QxL4iM9*d0QFG*XzZJw?J2uOYyc5>Vpz-t_i3eD{}CMw
z!T?Q?#<~a=;^5#^7eEZ0b^+wWD(epqa&||daWCI%+^atX4CLYe75qN&u1=UXVP{s`
z#JY(E?0Os6whC-*DEIBxsV7HRNBP7Aa;<v>1Le;DgXvexjd=tbY5>#|2M<m;!3*rq
z2>Z1cRxj3%XpV{iee@7Ta$Uoa#zfP$O4*y8olkF$dRuc()gP~5C9~D#{$*KR2(vjb
zbZj?<eq?0)9u*pc6S&d~c60`=s{8>Hq=@uY(GXMsEaY843{oHjR7L}IW??h)P6Ip&
z1N><N`d7)A<K>m`52Qa`DEK16PnaQZ#P_`3D+M6zQ}D<8*PYO><nRCif4E6(IDdgl
z1Ny-)v5~U;X20>Yza)k^K+1fRFTlXwJ$hd}>Bi)f;8%MGkAM%iD32yMG*y*fZGQ_s
z;uQ7pub^&r(f>f$-^1Jj`+El#03l5V?f4D9mV)>-d{r%h4gBTW`id+&GJ=8lkh+Zm
zEAjSYcKEVlS^Zkaf$#MznGH1t$A&iea{D{=68uuhXxIC7P4{wn=ZEuGU-575{nrkK
zlB<*Bn~m8Q%8lP@-!ub7O%GyPfUB@^iZD#9CQ|BGD7N6&ra4_1+~m}2PnD%P!%xWr
zoePGbX*kq_U6c=H1YxQHo_w};hQ_AP+<^x-b_UKY19}Br4(O-zy*~Hb+s{<E2CwDV
zv<a|~)3sc32Iz+Hlby>?GGWGwr`%%&$GXsskM9gMks%Of1gIC1u^0#m6tK5{+F?_G
z=ieL;AV3@pZj1x40mnPA^{&L8ehfM~$j$ht(r1du3vg%omxPcJkly4xchJ`y&?_iV
zr|VaEKW6Jz%{h|tfFDs>c&#6iH;`Vbe(+zla42I`zir?za%8X@Vg?eBmRELI#H7uA
z{R9G(k^7bxf`_WDcnKLF-x<0RHou9UYYNIm6iZniPWYA-_bY|Z&eukz*y$iIQw3x@
z_Q%DvT(8>RK*X&o7Na)In%rNU#kXP}^)O}DyR+qV*Pa=?&zHSsZK1c>sec&R3-2=U
zNJw2jj$W2E0%xFj?>RJoEYlR4yh7{^joQ-chC<%45o6(W*!rk3?T9aBqd?(XtJM4w
zT@=n}(q5<5H?ve)h{Ce<SrKLzdJ~`#3S)_L?B*T;1&C&JNzBQaEyr7PlEglj5$d0O
zF0u>H4b9>sm0jUC-X)Qt%I1^KiPum{C>U}ixg9f=uKF-%&qEYAs=F?fbU5dbyf$ii
z?f825Eo4RT*2xZTQ_ry>w;uWo3k0O)C(4T+9o2L<rcc8g5B|RXCC4F%<yz9G!hN*u
zYi6&405f-!fqjmv*Xq0=p~#GP<}v+OgGTSH-P&WKCgo8$xx<Fib)j(FB+`_O{!Ato
zj(_7%Cq8;<WxDZGJR^y(@|?>1&v@s#=+*lYAY*7?U?n#V%eFqe?RA1K&=B$wzU{yj
z4+uKCJ8R`?G$eY(<b-V)R_5+gu$pPNfKGxP%~DlSYIiRV0RFq0JAH=L6W0LD=e8$1
zH;fIyyD&U9IO+#xgEX6#K3`nHu7zjchpXprnvaw3IaI<eTU~C>-wz0!&G6p*r2Wpm
ztpp?-XKt%!uATzf7!7jUb+WhnNnpk{u0uT^xg;4#%ky=wCD}^NK`CH3r|jdj&ZCs!
z*c}v|-COLA7usr-pcZPt2DDYdT9DD7k0!3YJM3itj3b9D@SMl!#3w*mqiwNYO+)Ep
z)DmTH_UU+SQXwRY(-@1JoDv4+=2{Pu`OO_&LYETF9*@&{NWg!13I}sE;}!S@b@mjK
zhShRy3|KKWb*A-yUfGrZrO4%gJA(qF;b!yljKfRZw9|v9`ioELf%{F8BO7hSCegXn
z<6ReRcGtqAcI%w`rAk0T+PLHmr?y5l%14Fq086|hO!B6>i3%rIcyGqim=>u<nR~C}
z_tW#^x$xcUP0p(#adn$GoAn4C(4Qy=={;L63PHPp#~@aWuAmS#@CnK?Yf(6g<`z3g
zokXPQQax*bl6H1FVLlapPkb^4RFwU+_f=wK<lMgt9X$z%GV=t)=2US#jSd)bjz<M%
zH@K}6tURKOo>A5lWz891C?Gk%-4it5KuU*{-;yrbmol7`#hl{j9Xf>D^>(EF^r~SJ
z4^7YD1U#u<E0a#@TW)1|3f>qfNTh#_-I$Qimc;(17w8vb01&81uu+lzbwAzAKFe<s
zgFT*X1x<aPB>){xKug{(iuLyblNUSYZ7=St=0)5^Ge}<d*v|v!)DDMzm{>{T_<aMn
zG0ZiV3y+;TS4bNRCpY3$nG!Zldvsu7B)zHI*GBV<+n4b<V{SIBBq-c2kP8TKv-qv!
zs4wC-rIvFS;LiukvauKDyEE-`elO%?l0?^874wqhkg_^oS`c!dl)dg@k>1;(JaH}1
zhT?OB#~b%BC+e_Q3xPE|tZTIGQ-C3B1^aNxvZ#l#0q6FSa~ztlUK)KLn3*OD@}xUA
zgJC`MxlXAbhQdiLEb8$1C>E2{d8H`zvtav{oAgwJ1pv;T(9paU%Q$KYt$CAxAGJ3{
zS3#+0A39TW16QB&+9f2j)9|`$EKf)InRrJQ5E5oIC$D*uB%H~u%YkE%7Bf~m6liT#
z1xvKYCL<8o&85)Yp%@G7>2RC(d~?D!&2X;`cmAj<ZL}wRIuVyj_)@C^O!2uD8uxju
z2L_ykfaG?oIS&hd+9wLU1gt3}|B?$GzF@Mn@F<y42Kx>qZ&wMobfvxr*`Q6_lqRpA
zCu|J@5@@z#ZWnr&sXfpeH4+HHuG9gWn_zLh)m1`sLC3M7%1U10Jq1r8qvyU^RlKx5
ziQzt}x$!u!I~sQ^o3Am~HIDn!(Egq~l3O9^(YZ$hcNf{0KgOUNPkKP=tnWDibm2$m
zP`}OJPr%C)-r@d$S9P{)5%1n{G&^ZeMTUs#(;v@vW#86wjM~b{sl>i4;Cv&annza^
z?*a%x>BWa_O*cyOlW|8<&E;-C+80+;{Ttd#`|h7d!-BEQ1LL-&7h{>_mc&m51jwHO
zzvLz0Fmq`L39WKX$>g}vY6tVqt;b6)kOn<ciOlNEM45tE6eTKCH-1mari2|5fufQ$
za|Mez>DQ*}9e28N?`QHxL^&SB0L&^7x!OM+4jwfxSD*@dsjoa_Q3guw2(<%;*GhhP
z+x~!GoR>0=T>@T`8Q!wVsH1Z!t?}H4Rich#P65&~lshI=ed9|_u&RlsFB6oXnIfYW
zfp;UnC*u+EcbHwtLQ*iyY3`Id-ooUl(49^Z$t=5D0;1H$86ThtrJLifbhR>;SmRD8
zE+gwT2vq-pGjf_)Ra<&4ME@fJ!=WimUp2m*+76E5{FQ}tj4^+2zGxig>a}<}-<x8H
zxku|um^wCHoi1#GH@(Dqpo#FY{T*TYrVaD`1h<c<tz>bV`A73_Rp4Z*9qT0yGg+H!
zX+tYTC75W(TDI$7$w?Hg-BdLTh9g$2`@Z9-9$$h_C7u*@)DXOPV-D7I11S0XA7+Hu
z;Uxq$4|(w<hv-ugizsnC*;YuF`4cx`&U=zO_{UfpN>*p%gG*1o?WI);-Q_hdA85_*
zznrDGPT@soW+h+7FMsg@PusyG#BepBvWu(`YdnejJU67D{nDTI!**Y@m+jyyPAB|U
zRjwXM*0*Cz>nb~zN)o(-!ZkJIoUd;sdAlfQxchB5IO5igj1~8@?p$kv%-oBA@&b`w
zvrC^m66}uw8Jh$`Nx}~w4!0k7WvvFKu-LJ}=kFXd?Ok9GDqUGuAvJ9QTvG6Mi@L+X
z6eEV@XjqLfuuoSV<jLY%#1mD^t4Y)>uKZPR2^df^JTP=z;_qXl`P{1y48o6-z`PxR
za3R4}$*?T-Vjw-REM7bzqH$@9KOVBng;7vBMmH+MFr|KZ$wSLD1q?5JW~$3pXj|6w
zKx^o>9qW<k#P@->G{MGfo8aDmv|9DY=t)cK)_7}AU1vYK0IocU)G+Icx*4!F&KFs&
z9`O^q-;ilap`>k+Jk$CTNzN_S`_jmgbxvdXG8$)lwam{O`U|W@5IeQQJ6DvIZ=EW1
z&L-N;)VJ0}Vf7!r1wKd{>CL=36(5loV}d{E+m@8~(-&&f%h-hn$Py^$r^k-U{#+mP
zjB@r%WkEnlm5^<6n5sW&i@Plc>O*+?N$5ogrtBx!&_>syj`tAF4IcSmy$g2==Da~N
z@IA-S353vlnIN8!PnyNnLzBkx%iPJ@Ng31>piia)e(HZw>H7@H?yi1xGj>p{)!E<P
z>~#$XIUa{=u5nI)w24>jtmm-BG%F(W{hSLz=8U7t*z1h3j1qcz{-XVQ4FuhBCl|bu
zTGX8Ox;qgkua=E!2EBL8;D`Hb|8b5!=zZ@nP5}3erV@DtN^0&m_B;A2<M|hmIpPIm
zQp?y>ZuFy;CS40h_<f?m&VZ*D3W~jxjl$r#2fd6Ln}Y&HPkR~z8pW%Hu_${YSit@b
zTV9pfRv`*rKbRLU1PwsJk2yuxj}!dGKw)+33#IAjeDWdUUaGkDa)6rZD&gv<^AllA
zs)mCD*v(&*1O(XiCTDKAQkgqdFx`pa*<W@Laq^KarfCaQ`p&)_UW|{6%wYX0)!8!-
za1TlHIMqnBK+U|4_Ka)X=*8@}<K>Fe8o2k*N6?@*<FogkG_|l4=zMDO;w3a8EVn>)
zj|3?}o7bPXpR8e~e%7fj@lO>JE}F^f^49hoY9=iG;UYwZ4yE;!QC_7v)a*PC`N1Zc
zEL;@%r}8AOxYQ}hzXh15^d$%y{M~{q9!qFu6q_Ti?_kI$vwl?K1>8_qFPQ}!{;0Tb
z1-_KRF^t~|AAvrshRC+gS08p`U%F%V99D8jAebyU=L+Q8_U@St>aW_|$H<Ildq!l^
z7B)UaOK;Xk1OMu+p~kXf>E{*;j>Y5HI|f5|8arf4Y>>~eqTBfr6BHAXbKya;LV9t1
zsgP6pwQw<u0ib#tSV7+wl1d18?$WFddaG(VetE-t)F(FL8~q$?28b0%2=N7L@Ba3a
zYjuq_x5ZUZtS2zmPI@Kb0FC!^%g>sorm~V$qYb1&vn8)c2LdlnJI|MF9_`eo!Jl(%
z&c&1ncBFw@s17FW-3cPxG1Cr7p*hnA@ZV50OQhObxVJ+)qtkmgieNBzPHDt_?r~c`
z#IR-o`~)mjspOgqs1nRlcZ`5Fl__%urt_$E!F4m|@zScWlFB&JPd=Z{+HxBdkGP)o
zs+J2j^Vr9s!EU3bVzWBWL|8|aG|BTaJnEF|Q<Dd@bCXU7X<U*^I4(sd*D`-=2-@NW
z5rm@`tu)lmTs~`JLT*;s%lfgqHg-7Y`H$#$?r&qy*&o=;2B488WjOII07e+JdZ-8Q
zvB|cLwxucZ$Mx~=^0kJ4Kk7EIq8;9`rr7R|5uqeA_<085z(wc$DL%Z636cmUC!5sa
z)VryEncS~Ipy+1?ce>__d)*9bXjG*v@0Yp-VO;%iXeiaQlJ{mg-8?K08q(r7NRkUb
zmGyQ-(Vw5VR<0+K8FTqN5h+rLAWCM-@745mbp#YL5pP_lNM%}j!wk66{Zl!)wtmEJ
z2NFRkeINX@i2n%i+frx<M(=@4^0+UfRwyDoQrFq|Y)zcYp`O`;iZO*GuxncuI|X*y
zbGNndp3jSP4M~{+iKJ1+sOn3uH(Zf@#tP>P<(Cy-@#2smab+N<<{w90!(@ueyW(A(
zl?{g<aA?I5c4H$*{1?!Tb{_8OGjW-w(|Iqt*>2UQ^&aHVtY_14WZto{0x^Mt2l+Of
zc#K2q9v#L9FfVkmnJ~I+Fgcf&Pf|)<eM7q;N4G~3K$P86ag7$@bSFmKL4HrZufp^F
z44aMTF}BJD<EVQzx68KbdNJUh0kJR~b!qyfO}TorzkI)4oIY0@)|K2TRxkOPK3iV4
z?gMKhpTf?iY#GdE27s*$TxrHCOMkYUK_!q&4z`i(geIcVUM(G+u+on=Yxc{(mkVo`
zmh0Laj$E7<hxui3&8n3b%oyQ<Ll&w(ec3uyYzL*0V|2g9G2)Abfyt@Zi-Rc6BdCHe
zG9wd_S>b=_=AO+QQ!J@XzbLA}dbr$UI9^`}sUq`Poj-RsUhNWeSigl99YAI4BEOa+
z+T;`Fc3{@2Ns_TfMdLL~ZI_NaJNisa+wpB=l)x2F%xg#=R2Z%%QsD~%CCNWY&=x2?
zUn_hH8Wx||_zh4NGkTE}IyaXKrY7{M4Y=HV$3%zw<p+)4=n_|)aEWFEn_jmD|EQ*0
zG3e1zG^^4hD>xF1-?8-xG?upD;7>#dY?bhns8aK|I?{%Yw)5<AVaCr!MLCbf`t@D2
zqG}0#de={1_r0^mJVL<<E8GkyDXtHVUXTVSx<`$aR$drdl37V524_vz%TYBIK4J`=
zb*b_5lVdxbcw9{n)r{;pdmux>-OH@?_)4r6m3jJ+m?hRAY_u=;bR}qY-8?Co57-*y
zx6;tN$me#Y4qtRwnL<e>Wo7}0QvYUmzZXB=ix{K2AqaMkE>{o_N9wlGkBT|j{q2Y{
zEoD-pxEAhquH`=y4=kt-Y|abS+`9N%9z8$6%bS{zM0yk>Iuh2v8FpL$)^^-|MvBWG
zR{pcq`9;My4n=$u&IFZ=6XY$)Z3>2|8rn5+`zspo&eu|XSDaeF3ye^~hc{SXQsUBi
zf)?<LUW#tCV(5>y=FS^~__$PBGXw$-0AAM9O=0nN;fKaHb~ot(EfsU=E&8nzye!Ac
z4=Ad6Se;*|W;30lW1<fUkj=A}DHyTh6S=0TkXBJjB)wX|yLe;IW%$13p>KLc_(Ig(
z{1PN>G0U(hc-4E-uc%ktEbVgz_x{G)j}y>sj>s95niLS+`r{4lwZCxyMv=}1qmE75
zXXF7Nku=3dBI4F$D${oUN58Pf3R1z=jH%=&MxT^^bUriM+}8xVKn~y@l%|N73Yp}5
zX($eQ5rm+L+hO~h6sK{lN&T@_F?nUcM+WYee0uzor_i89=(Pd$Bgm(Zw+vyE=S-6j
zv_T*@i*}<t^&-sX?7C2}^W}wT)n?3odmr<Gfi8kVa=s5~1&Fm&VypTr{_qrKOc*<l
zTZ$r&5!1Qi=lX4hLSzi%8ILAwJN4@D93)Zw-dQm_@!Gq=T$0FHJof@G?z56<_;}5H
zk;BLam(4}3QF!K$(5_=^4!#+^lJuNT6B7(Y$8&5>${}sh<#BKo@K>tzYgU-f48XEk
zvDt#<P2@PcBBcarU=|%Ks;C3fn8!zcg1q+~u)r4^eG;X<q<ebb7$g>X1i;;={BbOC
z85cF@=!<fxD{xonC<3EyKxj1eB5-KX?2`0%_vWwVjDzn4#ZG`dm7{Ct=Q|~3EwkMT
z%Z!Dv=rHt7xjnDV`AkAwWrp8cJJFKB=olL!Br+A+qaQl@JA;c!e(4g`Z{g(IIIiKd
zD+IWwGmJ;k%eDuP3wTBLcr-c7?^OrsmhpK4DBrj(U+LS{xLdAbVyG;>hG|!C!eEkm
zbXGLu_;b*+a;v_Z+)9=r&wVHi*fZL6^73l(ZafXIbnDCDvLqTyz>czx{hLfff_Uil
z7VYy&F_jNZ$cBa>akYYiC$LK$X;D0AfPnhSOpHo_pct20{sRd=n1}Osh;g6SPvJr|
z7n^-^hVHxtWl15wesj0N5E?g(z!L)t><jlKEaG_&ht=tFzr=izrA03`$e!0XIZtOm
zz?AMuuz&P-jL#G(n*&Kws_vA9gFjgZJ6KZ&+k66`zm^q}Nx~EW+MRKEVigtDJznZ-
z64s%|B5zK}{2fOrfiT4DP74%YhA66*>_H{hJv)0mmLQvtprEHaWqm??o1EC}IXO>z
z%E0r?4N3he*VQN*Qe;T9D|oHw#m1YvjP#MuYlPW2z24O07^MwJ)oqV=y2&y>cvK_k
zQE2tKU$;%F!H$d9?pQ+E`bfGZgs<pIOs0_cJYQY|K*@ok1e+&X(nbay=Rhc0OsV${
zU_pkca8#M_ol{wLk?Wqow*KWZXhH1?FmK4t7;)`u8@5C`15R8^Mq?Sav2wOi%CJQC
zVr@n4O0ugkK27#<=Kn~#;Yyl20=YG~XqzW+;kAtE=r4@}EDBuG8Y->?>0u=qaznMK
zBq7xfFX0GVy<0nBQ#7Bxs*h(k?8V?6aDkoEBE0J%Gf_qM3`gemLK`0TlIMXe(3+Zi
zkn21?GHF|uWKGPvttvR?M1{rNU*2U#x!2pVLJ>=hyNG6To_g4Cg<#ezej<()o?tso
zQRI7fWR<#LZ%FR6o|zWs^K2^Luop5~tw29dTqo_4_nbl5zln5jFk2t_>BpTTpg+sG
z*BcqGC+nW@>^7<YSrfa$ez|I_k(L7qV_|qzJq$|p2AN>i_j-mD&9^+_cm8gQxD2aq
zWa<9XR-9PcC|p*<XR?^fauceSsRI9e`fO-2y7o*~5_fY6epUr?lx46_bC@7^wcr!0
zqk|5U#2uyDU5n|jCmDxZJsX1>61z2Q28K~v#%_l7h4`S=Y7pA%O?x6pzSsMf(4CIm
z+wykXqPUX4ufyUXnVT(^9M!1u`!3lLV=9&4_B?1_wr4&!Z4Qx2=?+>r7<DthYyK<^
z(C4#2W;S&#3O*0O8kFqt%efv_v`kPr`7Q9ka{s|Dq+u%XAz=S$04r)QJN+F-qZx2z
zRmhQjK}bf8Y9dkJxvw%i<|W8xwh&3l5U`4~s1b$laZr9=*LHdiAF}X?QtR`>2(GfG
zu0)*R_rzN+DZeC3*58YJtW}r1w-J~b*c7^|`dM3{2eBq^PoMG|e75sI@2Ra<t^5&H
zuULKNYA9lxhV@IYdb;txFoG+>Du#5cqLbi6_3D|zMI|AAQHkIZH@Id1Pb%1c`;?{l
zWR#Ina9fvQiy<ley+J7*a8c04y=A;6Emfg>7UB_7Kr=KUoP=sZ-<Nrmq<xu*E?W8$
zg4{G%j$rc<jMKsPGZSI6Ktjzok<0k<660Kpeu1A5zuX<%C@UqfzZzOuG$<$4`fu41
z!>=lP7d|)sRaLF{v=ILRL|^lWL*Y|T?bqkW5^wa3hGsV5)1zGOG|5hz_Z7j+<|OSc
ztN7QsN3#OOl#U-+mJ=3SQo)&u(SwWQRG*OTpCJ#mG^PkWp3{5D74O(~vE9GB>*W1R
z;!n(5F-8Z$psZeZtDf(D9$pJcyLLZY{c$+)V6KF3pM-S-GrX=#YS4*F;YgA;O<{(v
zZw4ZXmcUu_Uy>xuwc<<ElJ2I_1&8V$3MBe1dykIyX9f?>BbqGlL>&^>l_aBLrit*&
z$9&Ndf^;u}Lb(o?<}dI%aL5R+ljNQJ<$^g=UUr_5bOM^@F_iW}s$nW59g>~{NlaB%
zhO%0iE1n!EfC-Mu`iHc{K5$s5J_x#0=Ma&@kX8Q|Yv&XtTexWFwr$(CZ5z97?zV00
zwr%gWZQHhOyE}bygL{&5lbbQ}SYtiZsP#~_>i_2aCM#pqrIooCXmDuFegvL~?_V9(
z&50BB9$ikFW4((XUxz^`RK0&C6Vq%4sQm7JldqrPQ@Uy$b^S>60<gB~CmKfsa7PIQ
z!4@eA(AZQERE+f0glAfvcXq9#2UtdTfKm!VU5O=d33^>alenE1P!#?vMPKKhf@dCq
zoWWqzSWbuB!~~fQ(9VVULn~r3AuXj5?B}4F%*$PsO-u;+O1TJXe)2$!B&AkG61>kw
z+hd=MI%`E`p!2dlqfsl)8g6|5;A%H^Tfr>~7wd^GlVJ;@junKmvfCqBiGly^S`sAJ
z0|Z;#q6xkI0e=fK?O(U!;)B}t4`Y`z)Tr*0aCbfN5yigoAk0~+!p~9{m_ZE$X=Etf
zzkhukMnubXzxp{f5=WIimJ_Ty?oXQ@(3Q!*rj{)ychLgM_=Hc8kQ#X%0_Xw}KNuSA
zvsy}7wC-!JlJ*#8ap0J!bKs_DA;|!Ve93Miv`E3_4QpP*3Z0=@yY?!D9EnvU(#QA_
zW|u6aS9}&f!PJh;?Whx_NccIZOBZS5ZoYOU&tkJY+wm`-dL{}JG2PI*X?2uHsSw0p
zqVLuhqSiEKl;u&cU$6Iu*rOsLIB{uFR5;{A!J2PtOJSk~DBDz1hB&v^R4yvQL@8e@
ztu}=+oY9MX5hO^&w{;m3&$sxjgy^dW)Il<+xC#Yk*5c!AaHEY<wFmY$QH(XxR2Tpd
z*U^H)e6@t&jPe8}rMZ|vP@lbCtt~7KyT(ah{1+&^E_w8-gM3-OsM2;e%^P&agA)}?
zFNpLeVP`dk={{$4Jk+X9Pc27lR<zv_EL?KFNT7<tj@rG6ZgVjf>UUMOTh%xj{Z#PO
z`uzPe{aGW&dSi??z`C^#<~KYhz6FUC6#4pNtsfV)mD&PgCfg^1Zx#wlk83ZmI1^AB
z%uU4OUp8*XReZ=xOP)?qU%2e=O65HfOHi|aYonkf2o0(A1<X<E-kCPx78fYrUW&~N
zE4n#JL6Q~qCEH@KXuBsI?mh57bu`aoTU5a9@L3>ad%ogOc*y}dx-bt{p=lw?zTO1S
zZzzlxYKm!<#|_qgt0j~`R1Q?IA^#bSyYrF4?2EQE6KVYw4Q^Z36<#k>p>k>D>eqb)
zWWORdEWiIovxW^=cY-&nexWajd}n*hw(=8iN_q2MVNdma(cK2KL{WrQ&^0(}dy3X{
z>Q32hG_nxN>A3(l0KV`byRcYO$3g4}k+;kaz&Dikve_QNsrPsR+|utHVA#O6$`whM
z!p&Mi_{5`G(ohF~Fzxibo}lbTj@5^0+2?AX-9vDER%5`Nf;ZgKKb(=K?*<%d$if0C
zON=|uU)_d#uRBeA9G(68<!xiaUl-EIdlNdL2t`mFCbdR5^{|)AyAvHk+dwN=t7uK{
z%IX8&sW{r!L^HaNdqbvgPp)5zGpeT2k(4AHOPY658{ae9@w?0Ot?fFS_7RrO#M7<&
ziCE~qaMJ)TVO0n{@#9zhxr+OMiTN`mta{kSPQp&hO18X6Q{M5EVT;&4vv!IrpD2vn
zLE+FH%UgYt9E@~vvG`Hq+eL7r3#G0uRwy6s)j?>(RvQV=E=b3A1ZMZr+#eX*ZWb4v
zZ=nPes27_}0VCrfa?8{x2xHpQt)?J;Ki8G892~vp;{skmo|8AG=y0iUuaB3Y`fF@Z
z)+u+G`LR$cz*-{5`wlnZW1Ct#-s83>{7PhAlj4sNk>`b)m4mS3yxp^pCw$P2+s5CV
zE$k9Y&87yF+l!ZM<SIvf?(vcdfsmGc#-k;kr&YK7Wa$33lB4)gGEfeJ6-vAEy~aoZ
z<)-kX#7HFwY9<xsw>rcHeUs=PdQ(UX%))b#I_z6wBve6h#Nfg%B0~3Q<H`YsE7#Ay
ziO*1%;A+&8E(e*YZAwXXHsU9b<qYRoqea<`85dcO#=3=HxnZ%n>vtB$gl{+yzo)iX
zmQuP#psPmJRT+w1X?H>dm?7o0+)K{ItoJBgOg}DmKtTq5@Niojy5dh;{~A&G)`0Sx
z+rYP3YJa-i=U>0ALY2z|hsjy#>pw&#IuBV2IbNswMtKimb1zZ$Q-h_zk@MMGo3K97
zvhY2c8)EJpd(soqQNBy{^A153Uw$AumS+y7!cF$bTH7v*Q^tgc{;>fOK;NBF+(~E}
z`vxQ@Og;KnF{PhG4uP$q<$o%sB&Q}KE+O_4Q<9bZ4^m3BKarmQR*U(mmHbmJ=4aad
zpK?n7z7|8v_>YVKK~Cxa*o~2(H?p<1Hu!11nEgbBelkQqwHGTBM@M?Af9v(o+nG36
z*c$(%_VUk3rhnv?{yE9?-!_u}ImyJr@b7j9*%&zgW$ERoaKQ1OPBJ-xv#ejfH!mvv
zWB?-a|0b=JTMkiJDmREA4M_Z15v@#K^Ng9psI^SiY*f9>r)acHQnFlnE4jS?$o{_f
z+;-f0KFGAa`mLAq{-(_iUy_{{iv(i=3|<h6K^V)RfR>wIP(TR@05llnFQDjvher%D
z1o@Sco;eH>?IN5TTlYOK2nrjjf5W8;0d`#yC<N57kp)1&2q2+^MnVb=0wm%u@RK7}
zG}rK>8UO|aK7jykP>^WB=8S<F8^n&PsUO}M|L_L65qt$Cz{I5N-QEQtHRHen3?=~N
zLWtzuz|=#8vIoNWslsv$z3WlF99%MDplcu^%FD|e{Hf1ifU+*>p@+B;(Z2u$vt#F2
z1BL_s0D_qb=IH+>Kqfl|#8@TX_&R}AY{HO-z=G?c&O}h)n1jXNjSlLa1@N->%PVHv
z>4L_29oBe{2_)F5ZU^)W68x5YR)44#fjGF<hd>E;aPcSTM3mt6qwRx)Yk^-v8*(!M
z0Yo6VNFg2=g$<4gbrV34L%{Cnzn&rlsEOwT2mpUU=V7P7+6}Nrb`j?I44}B9hW$!j
zjAcMgtY^jvvnT#u%%K8;4c)8W)H;|p#*L$pFTB<Zpap`g{{#g(x*{@+6lrPeXI1@n
z=q(@lo?Y!_1c1zKNK9pc0B{BE>CAm1>Yl@PZ}0KN_AS4K`0&zEpaan2foQ?+LyP_<
zes&IY>jA`Q;NF;i{s{`v#svxhU;u>!uJl|YNZk3>?JwG`^E3Ipm<OTv0R1_}g8=w=
ze4jog%+_T<nHzn_f8M@FRaBa9sV*SC0l)RN8XKda_oMGBj{OUQ0Dy!F4p?{t-tBAl
z3jftz`@M7~h~WDO_CcOUHirfZ0Qjxnqqnuq!Sj_5x~)gu5C7r{se_U|ZU?Z<zt9XK
z1(5dwfAZ}K=o9*}?e<MQ{vGl5y@{~0IeOZhcHR8>%^;kKD7WRyxl?)(-CG?D2i1);
z^6kPJ{H<Yv%K@`{^|7rc3j@-PCndqnGZ-we*C%kx?-7KJu?re{O30w_&M)%jiS~je
z2SK6@e;D%exVKZ_*YjPs+YER1$5(aZ2Kn76pk-Gb;oh^Q+nB>=^lLK|3n@I{b4^SU
zQW5|pD#R^eAJRXC<mRKNnil5jG2<El(SQUh5FT)c@h88eXnv<>l#B-c2jf#I?wgu+
z0XJ%B`EwG?YqI-r8yh~<vp4TR8JXg*_8(zYiTPP-QZ)IzL%~<+^-^)ue4#*k{5XTQ
z1+hxBJ=l7tlIz_b<t&05;!rgmljVYoX?))opVFhhw`lwbdjQp|_A3K&+BgW&q%U}N
zTba3K<ZcG(6%%w?f3ApTj3QdTp2l=iby5i77mc>cqDMidwHieADJy5lHj|sH!<iaS
zHB4mK&|w*1**e9>kvt<iI3uPQd!^%&cPZOW*{?^NaT10-)wFZ2PtUVjXGfRN5dR(`
zHhg5gQyzt+YtxsPaaaV8%4{{xX9~qcIa%sqJ7b}G2xp;Zf<+#5Qn`o0u16-9XcwBo
zrR_dJ9$-hROuC<z|MFZ&NHk&$d(uBm%&g6rzQ}&>J22}7lh?%J<2%PT;mB-b*?t}j
z+q<+Q)+1XsdY{YR1o(cI3PFS`JydWcG?>?Z0hx@DJ6q3IAQ&_V<f0JWqHaGww6T=_
zTT#nfeJBbSUEse2&^yEyRhJiu+q|<5a^ZnH9LI(XUK|xsq%V`cwF{(4HbSt%T>P8x
zs{QdI`eGm)#=!)%U5$Sro|68%&=GemuLeVx_^u(>20m5TmZkg-99{f*lT34Y8`ttf
zgss1_slO#SYY<J8*PR!F$0~_JnPk|+OG?yIQOKIltbv~jlT=#Zw7NK<cnReGNyD!p
z5r1hTGu)&buObLyxfgMxcsIdgPa^Gk!N~igper;UE7Sn^Z>WXR;IrD@#`E0ib0=Z)
zJYuuRND|9d66Jb4ZwGuIDs?)^!J{l~z5vE5;L)Aw@5Qoxdc}1fmAr9~-kB|Yc-nhT
zxR1*VcB&KogOA~7e#obY!`d@))ao2R%6_AYgYg<$(|gKs3V>3}QS>o{*8|R>HsY?3
zn=gsxP~5Hx4C^nAp7E^17_-}g9A`HJ8s-ewJClMSUdk3>{M+us0;yZb{Ssui1PL7B
zV$NT}qe(uaDtd|!I!F;*c|`XtyA3N<QS()1?MtCi*~?}c8br+mKP8R<NA~pmd`(=3
zgzd~As6o#gaZ;e0=JeXxvIyXxdscY$bjewPY$QadE(ei7!FwL2dw$^&Xcg7k4r=q|
zPKGi?*bY|zKpl;9QHfB!5idEbaA<gl5z~T=EA2sAMn!nG>c;QbiuPM8`K33(J4>**
za@LxCk$Zmh)+FQxSht*e^hNBvVDeDtw0Zm15qkE|oBQ}EgOg30lWrSI65%1{ZGO~t
zsPoERI>4W{EbQkmu3g+DvF}_p>?KbJkDEj<{xb0sfv9PLkQbx)Ou${uJS179^WNaH
ztx{oj$<2W^R7_YI1gEyx*krm#6|%GPzs()z@9Bp#+^pau;^1wY6zo}3tq@mfjGqG>
zpKlp+b)a!-;&_z}rw;y~ne;XYW=2h7B2nmivz=&)n&i#h5chNU3qHH`Xs->I9SPjd
zH($&1phSb>f<g%_s2L<x`DCi%o=*J=;M1}Gl^p~^ypHwlNb7MV?A;+bte7qlnT;NU
z)gJke%y1G=!U*>?yy!vas~<Pj;KSzL{>J1MYY(acp{hXfE&;+sSl5p8oYm`Wz-X+$
z<sHF|_Gx1Txf^(}Zc)TQuKs4;B%~dRPj7Ohj`PkW1;m~IW`Q40WdM#%?vu$tIJ0uM
zjv!GdO;5H#V_ki`J6}A?q1aXQp}7;e&exT9j(HLHulLda{XQo1W{#{K&>K*}_3RcR
z5%#9a6ESyTEwO<KkF5#1jESZo-5K*END-=@)Jk9YV{?D&_PZYC)KaSRBVWOcD{r&6
zCg7BK!}XV>w?`hI&?tXX;^YO1;A)y2c~>pnE0CF4xohu?O1E!*P99b@?RrzCsLz0&
zH3^6IiDjL8c;n^VuRv^i--ztq(r42Bmc9O>-$tfHa2j-7kl2H$G|*?QaS&C^j=JYv
zNW*J5N;+y7fvb~?lAk?lUFT=6F|u{;Yl!<AwO^?5q(*W47x;O(T5rtETNaM`Ih*UJ
zcSVZe(ji|;QT?Eo;ejhU=5J!I#J5jwG_y#6H&+Z)cV?Cl@@U>7sX$dXa}?s~E<2d_
z&0uKHepr7`!nq#{Z(qUYp@%)&_j^{)gsN)}zREA694#970y)Nb2nOyg4qAfH!fWWl
zTqpw9&)lxDnO%q`8ywV!d1k(XN~Clcf4&0rM06f>OaU&UvP9VKyQ;Y7II&lR`$wG?
z-&jn9(Va?qEy>T{3MR}rH=}KhyVSD=bo{E>>>{Y1Jh*fbg5rCG(o|gkIy&tfZp|d*
zrb!$5Rg%|dfEE}u>$l=7HQRm`GLHKYq}@3Oe>7CO(4}9hE$=)S=w6R#bT@&(kIQ>D
zB7lS4Ub$C^k?pU)nswB;MtOR>(42xV=B=u_YvDX;lil0WFb`xN(`Y-JRgX#oanjdq
zMI=+atULSyLA->}kTGpIvla8;nrbg>8SU_4tjdRF+kn6Hs`ols`@NzlusDRO6;0P-
zD(=vqLY=V5wp{FUh&=x*uUn(A%oIt!w1PF`-iupP+*rwR%b+Io1?$Y($yHgzP_On`
z{!-@{adKBP{ug4sbJUmW>=C@|ee)Ctb=NY}t!uVM4|MQ{tXTPW1-irbXG>3%<$gZ%
zYq8<;d@SXWPuW{`=a&)LQ33~2;265nRfAUfRb1lXZJAD0hLCRD9Zg6(e^h2S{rRO7
zo`p^DG4zG3i`>8rdKW~=QYyO@e%IiOk;so8$ByU=Qymi%!@-5Xo!&}BkQc;^W8jyT
zP+aXw$lvnRZ?E?Hc#cJ%w$*k$hNjUS@X3@ldgxzELfk8%ZfoYI1@aF2QBs#(@b-Nc
z>7MU_df~F=wsfkI>rSI<Oo^$RGZyE?>&nU;vZ-^8P>G0=kk?0r#D8;TTlw{de3@m<
z&c~3AE7hd#M&LP<c`Psm?}IPOr=?3W{+?7(p$>qQ4s}JU4Bl84H#<=Xu*wgEO2dnL
zz6TsPr)nXmoaz?SqXoZ!&2IVjpXG>b7?0T(Iwa2&4-`dW7Y$)TFFc1)Ydv-76B9Do
zNL(ObHIukC@cfvZDrUBYEg0i$-uP@D?`)!%C{LUSq9=oUN$i6Au{Al@6J(3)zBi8D
z8p93J(_Z^m*e`xZlM}ht8@vmZ>^aBrU9GBiZj`Tmo~;L@92hl&_C-a6^To>E6-r@n
z@volVC)oOmx#j?U9z1C5F^aSK9;<F0q0Nt_zuMlE0ja;$kE&oi)~buQU-<+dbTX1@
z6<a@2Yjb7TeHZQxffu2ij}=%uMfbzMNmY|@9c?&2e540u|2erJ!7brErFErfp&VuK
zje&~HJ#5wDbOd+hpfj%&?7}*31p0pR0v^vXV)<3AQxyl8GH!rpQKfhmPNltCh~Vmm
zHY8^$b0l#XF<IrD#-$K=!#2_g7=d|KbwwT|(aQJzgu~}hV6&zuJ$z-xr*oG*;uN}Y
zk_eTFC>35F^44gF9!!_y-x=S(r>}Nt&j7fUM;@Eh+(pGTtinC63%$K{=ZkZ9=;8h#
z$X`IbL_hV|Z*TE?JVd-qp7&VBjZ0noX-5DW_<3^wegm$k5ivagX5Pnt9HS;HvOsGp
z@sEvQU45Oi1xxqr^6*Uw)2*^X{!5J(TjlI8)A<gkZH@{?X!)(YTIn9CS}Cxi$+R~4
z*pF}sOnzbvQ?eVv?=dJHo<LSFb}>8a+yjkR^(?6Lut2mX57F!L{7{jX*mPLY<yuyY
z$p%k8D?ERb?SnQOpHtzhFNr6|Ef!j$`h5tWof^s7#;@aXj>v53<NNtE5mEAYzUNUm
z0Y1W5I)U>!BcLbw!Bs`V0B1FgH8F+R9L#liNN0ap%UX4=WzEB<ZPq=#m=I^9%nIiX
zsRFBO?0U+iVGeY1zdPhz+2j!S?FhTcoHeV{ebIPm%5(sn*ZXpV2l$mobThaZNaFI}
z94j=2*R}x#2}y}7Ey2W#{L91c&5nY_qEde>`yP9YJ!;wiA|Bz^<bm}+#V}R_>G|W<
zF7_a%3upqL=VABtrXyRnd%qQlyYv-~M2n${a!>8~;d|o&qib3=*jz%YB3?l2KUzg<
z%<TN}dkF2oDJzj@x0Ads8`x%yO-=!uB|%{v&o`}e6y#<htZZio^Oh40Go<lIo4fL|
z9y*-2@xVl@rmZU*N!K5uO<rNgNY3Ph(L(Sc-}cBshA!{5BV`MnAhnfg%TAzcwhu>_
zhvL@qzWYL1QIvJ1;$BxYd!Ry#qZUJ#LCzr|`;B9hI>zg&`f>`HHI*oH*_6bFj%~4G
z78WsU`MaWj<2>CI--N$iyx0&B20QR<YekrvbHl3|AYa1xmM)D5_l5lLdlQ_;i3>uv
zh@Iq3>JYUiFciof^GrQii<0=QRDltBv+?F+D=MenD$V42Bb`KD!_j@$48tyF*Cmvs
z?p%5e;c;<9%G63{5SX{=z4&X-ox-ptck%UCuwmR`&2T`(3V3L5;sn3B+F1OYYelGU
zcvGncc?ta_q8&0GYZ_a{*$u~VMZCq9foK=b$BPlcMyx->``5bAn7Yv6gSvKP55JD|
zP<{$p`dT_kPKS>Jvky$?(*fS=j_DAlYF(v$h<;Z9M5hT!!S|u=ovuMl^aU8w;Qm)2
zi4NL6_dvO>d`o=?9Ktb(6X4T6wq@k>Z!Ule=9YtWY9)1U&X5cxt-8)I!;_nl=9;H<
zQ(@ed3)P?DZsAN&>Hbht@ozK9@+5d0a9CZs0`oUM)}Iv;JfRHji+XsIXWgm&@aMMn
z_p^3)g+!q=eZUE=uru?D%Udo^@S#Mae86I+&{QjqNk#Q=VkV*ss8zlwtw&FOgQ7V<
z2dMH$AsL^K4&h@OQ&c2Fj`U{#0l`xIE6k-v`!UPF=~plCQ~2iMi-)eZ131(EF6tZC
zoRA@J0_G{Fh%!FXk)wAuBwHvUo@!*g+df=%1fNbkRP6-#*fi1E6txyEuFeSfZgfH)
zHaSKG#KiQ<#0TC4B0L{D%#O`<S@7<CJe8w@L3BaG`MZgTLb#$9_JT=4U~~Rzckfn5
zFAf{nsU6wul63w?0w`pv`ERE8B;y4A40V56)?B3!9k<EA1Rs3`%RDBly(hxPXWH$?
zm}*(uk-~5Nr!5Jtu|>&q^4Ju<mL2~qd*3z>B!3-47JM7YIMOxls(VaR*9oCEiKYw&
zY%*=Vt9a}HB%Y@pDv*JhQ8Ut0$IA<-=YnmXK`u_fwH2SGob&i+GhH+fkMxnH+zM4!
z3FW<YB70|6AuxAsUUlN(8IW<p)GlYk=#}w#a0_Mb@wvk;9@I^R4<dox%j&okpLjLr
z=}%3a$Wri5I+Ymi$lefzGc`9~#zlCdHmX|E%=;d2Z;9WSOEh*(-ak*1&!#$Hjv*=N
zh0UQXJD;lf?nJ1`v{j9^Fg%sJiEwT7LZKJuv$OW`@s7b~q@Px54~3Bz@rw9?5+0kT
z)2$pjMdh}o%gG4E*;$dgmS<-c^ELxjV_>w!oMHg!wizq+beN`F>**I#2;-1kd}3W-
zu6_lT?C66!Y$#Y6>?inFclCVXHgh?uP}B4t+L`UlS&UEhUBh@Xc!hK=CKih;Y8&ri
zVR>tL3Q9GO_|4i@wM)UZbPZv?#=nRXq@P-Tz0N0<G;d^`a$CBA28qSAk<txrt~l?Q
zu+|w(Q2$!0t-gi~Rt%wyOF4<bHjc8xJjk*%g~ZDWob+&Wyz6y9xpo;{C+)r>SrJpG
zUE_2gUmSFr5VzdAge_TybdBhfffj!L3tBh;MG6tm;;ns0{%RJVDpb5Btpn9iYkS(9
z`!O;Eb%kcmk$c2jVeWCmKv<Qg>AT(1AkM_IWK&#aU48k7fw=WPJ__*m9R2Lrhw{@7
zYTSvmxIA~)9$SHK?^E?@@Jl47$5ZOKHMrUf`qFB3vlQ-(yC52xCOi(#XC^srRo$#V
z9?TU!U2J6nH)<5?I1IgS9*nGF3dzxn)`k3CD(0!67=lKR;dz|uBsIZvqN!x4L#8YZ
zi$Dpe+}FZG-a>{#fz-&~?~bR3#<0z>WEom{!6D&F(_GZ;%;UKIV2W29lx35Ii6Xg1
z*TjLNoB8H-sYX(NUYGZ3Wa%>(3PXA00D?S(jCZ7%p3h^Asz)id9M*NkHjU0$93IS~
z8c~*c$$zmz){+x6d$ZeGCE}2v4s%i&j#Y8`<gD?XG3?YAVQ%=`6qi&4v8l?qs?wwM
zk#MrOCD<7BDFYtU+9Rdc#&2t1%)-;h^*k~GND3+FpiVKdZn{q@xeL-Hs{Q+!x4apv
zWE79+l9g^)721S{WTCUrwpfdQBc_;1stG_#_f|hLdO30D_GIK&%M?kCHv-ILIPau<
zQp=xxb{dgQ<OH~TCecRrEs8jgoK{`NI84=5D(+W(l{_<nGlkEeZE3}@n)|g=M@7<g
zNdA|;a1ZXqN6_^h`WQ~CsbjUngT+RU><alM3?TO*ZTr~s98PvCc9gt#U_MdZi%M9U
zoF!e0YFM%fn+ecILM*Hi3=s;pH2gk^Qut1ZkD^yR=t0yQ1oWZ_7`Fojfbmf(rMqzZ
z<$RvX0^D#}asS=wv#P_vkZdRu&}#m@8J;_>KXIr!WxZlY7@r8yZ2@VC`(F<x14oPP
z%hAhJn}KCysZKQph=SvP7xNpW>2r%7G#+g_cY?7dgU=f@`22YkUbPE6(3;t!kjFd+
z)mA3LSbC(2)X4W8ht|PR(V9+bnHL*|J7S83<}#l7&3zERe3BxkI&eicECH=-PDLtJ
zv+6o_Fce6gcI|^gc~l#C_kB039YvGEhV3rAjEgz3`Z8&+<f=^el)3&$ur!2_!I@q0
z#tO=9NOD8{l8jbHa~GDd8qjlN2Is!15XjjKl9AIK$uHAu|LrQ|RrAa`{a89dvWyXM
z);M3;aJJ*kS|tGB4>wud^}tL%L(RG<jCcA~=Rs<pL|EpvMz35?sqOx#k>ZOJ1h<$U
zpnD@3UINBLWM|kIsjr7^`7JP!GMX*DPDAWmnKYr<+x<7ocCJFAzsysMqA;%BuT4$(
z59cq-*HQ|t@5uT{Cc2Pw02E<902};98O03#;>|cGj=5H|E_>$Kb6uN>c5X{475>C)
z@V$YV=XUn6JI%LA)E>OnJ!a^@YinNa!%3lxNrWx)T{H7|Tl2%OrH4PT7yGa6G}T#A
zlT?vAoXsB<E&0hR{p!Q0iI>HCM`qR3r3Ytc;q0&eTO)C+<>PS=i*YbZ6s=0#Btvg3
z(d)0`P<sA!UB*8`p!8^LS-zNr=Ip(NiR%%!?QVD6)nPlO4r1+_FQeKrMD7-C;xWIt
zwkXXRFUs4{OgC@`O$Gb+*C;$v#jcg;T7jV!-MZ_-OpZ&&F|W2Ui}80C|0&x7Di4BR
zeTn5P@SPJRU9D#b3{;bBg&1-G6oVpc5M$D0gb~Tb;k5+^^tA^8m;*kAdtzGhF(_w_
z$b9(qZYrmSM)Ay;e%Hj?QRL!?!4|Dsg~NS?+xSgjN?Mt8BWN2hXwmT-RAFSVUlp}0
zds964+qfISPlFdl2^(1jU%<u46<C+c?>&{I73yB`!sN5C;nXK{2=yVYv2|xs6fr$R
za`&6-+8~Vsj~jEi{7ggQg<^|#^)SexUiWDmAGo2OHeBhy=Y$z8h$Dxd;;t0?8D~l@
z$OFNrjzjnNq#}rZlrRhOtJ#xST~~y;zUdRGMW&c%2$Wj~BQAUOCrx}MIW>8tdD!`K
z<+`J?#dCQEQ``jRcUh`Dq6bNQjpwblCBN_D*b~y~fWP<m8d5oZ6bZ+RhDIRjda_Bq
zpQ_yE#Ymx?Ssl&y;Ixq8cP+jb<uRxwo})g#s56bCI_ITm7)dT)>zEcNF+;$DoC%6b
zG$(K>Bdv3cKek7;tRq*qOKhIYAkDrwX%aueC=9U=2IUN7l+e@)fp4ImlCK{q6Fm4f
zVhXcLpjytUTZnJ&5(~9(%U2*^9rE>MHragn2}rA#bBhtwmX>K0B{3L0z6KtfT6^38
zjd^~K@NCUTQ+m$#Z-fgG^n^I#fXqi)-Uc?gnu#8WKGwV1*$$<i57C-2w8xOvYobJM
z0fkubJ)Xw2>h@{N%}E{IXJI@-^qFQJH&d;w%(qhLw>1V(NX{V3wOxS)a@x!49$JPY
zv7y&tSqo)4Sm=uY4&ch9^*-SZfvcka6$xeie;}cPq7w45vj3HY{_w$nr3d_nuHheg
zKqeN(|4$s0<43>uA2<S4{+Vt5gJk|Q+hqH9pY4BG8d&}zhxoTwOayF996#cS|J~BS
z&cynkw=_6GvMl_t%|t5`TIuAjNbjs#lkqO&?PZ!MMvNgCQ-+Z)(<rM|*L(y~(WVRO
zCsM*78p#p4FJ<28>mPU0Gw-qYbmo25`&#w1w|7-Wg6y`0!YrOOxVmsbO#lL64NGoC
zK>;D8UvIvUehmaBc?ly>M9_B;**;6Kr#^yt;={Lt!jqqHty>lpO2qTLU;%)vJP80I
zC;`%3tk#@90|5}2^ylTDr&utRTwMrH0da^Kk%AozCOMOaj2Q5~S;z?b`)9-*|63pg
z%2F+$?tVZ~^%ig-Fi}7*g$m>iR6Rs6Q6MpM44~-8S9K^n4RV?#S<PQ7>+4z&XW@uK
zo*ZMhPykyH70CJ!!#%tmJ5N3IJ$`fxs9)e|2$<yh7ePHAv)FmKHCqbs9DeEqhzuN1
zfiX8CtNpw`XtPH`7<<Msu;7d1_qRO)z*ieK01?Qmd|P+EFR4(j54f-aLqj<>%3+`Y
zClDM23C@i2JX+sg4G;i|$@e&7_(1OMtpYs-5M*ON_s10u)L%+000o|&?i~YtJP39g
zF^E8cuK<dVe}GL@#iucIb#)G?2*UTp9H>5ExPHC%n46r@w%nO5itcU@aFDUBU33Uf
zd3FP2l)g>ms=T`tXkF2#m?^|)(1G6gW)V_Rz!q+R;oi(3-y-%W`@nBDA)nG8viZ3&
zW8(*m2eN^`3o!g0_iB)<YX@+?3;F!~z8&H#ZHxe+ALC~L$lXE=B>E)p(t&k#BR&Mb
z4eSqm6>z%|2(Y8q{he8H`%oud5X<-7@||5>Qx_H=5|8waKk!{kNttKwmy`PkgMfkv
z4vYW+0;46E|K*!|(wA<>_NKe>O<_Q9(*Jw(ufKx;@C!}1;EwIJErG7@ZNPDL)-LFm
zju3hjIWx}AYv{Y0zC8oXxBrK)ZO?V=H}<Ws$*ZsZcQ>}uy$F$ShP%)DH$3wKs{h05
zIN-F2JdF>%)6C1Iz-Mzg=EpSMdCw(ccGb7_Vvo@6d)^<10Y_~FA~F!P_X2VK1q6<k
zPCg)L$G85%ZZrRCf05jsH*=W1<%GZ(puq3(5Iycx=r4`l9rCAPkii{Sg!g|Vo8v!Z
zlL{)N>ANwSXb%Zsp&$56ID{NT7YY1>;H9%Mbl(RV4*^Xmul5B2AZ8WcPQWwv6RW-u
zgTU%T`cW(3j~}4HhV8AMs(v;e-V2*g0`PX<_T4o)0Nik|fiXVW3_}{jaeB>AzojUd
zZSUc8oru<H=n+xCb%d2f4Q?566SSn*s>ZzLp(Xwrp(WeYWSi7gD5qmWyN`4uyWY#6
zOXw{XE$0r4XS+SG@4gk{ZeY_V$T4-0SJP6din$^@T#z>!Mitt^yQgSevCPEh*56yQ
zymv=!YQ-+n%pHo&rA2Kt(izbjV-M%~ginn+&9^`59I&em(dD3-=~^}TIVa|oM&pd=
zk@$}CzenvJ?Ph=1H8hO^SUZK1yB$UJOI1zU(o_fjl*^bwQOG$;cd{{=e_SkBB%MY&
z-L1l4j<h8~$LrZL{>lpYGnrM7$H7aquAl3r6?<E>0*RaGLPE8LMwOP#J%o3PacM@+
zU2cl8@ZiBz=Nk0213c?>jTGNu^4Fgn+G+dLVpUFGASYij($Rx-LP=qS2A)qaV_UBO
z7#27Ize6pwaCFasE8Jx+qpwId(+_xb9gG}l&4Qcn`B!J!I32kH6qCDUON9cp$6?cu
z`w-?J9Si}{9qGe(s-#SoVQRH+^oOEc|C|0Ea{)`r8Od`Hk-SCWfm51Zl8uw0z;F`d
zUZ+{ba-oeOh0XS8t(pCEy_QPLYuXSIMM^II0bq`Ap;Ww`TSDM8IxF4FgHC^9C;=*z
z3Q-9+W_ITr0P^4`ZED1bwnMXjuwVM_@Tqv;S-STu`|GJibx=meHLrTH4~55hC}n&n
zySc~Z?pGYH;wwidopz3>V7dPaO0p3KS?L}LrD~UboqFu?(K{!-uqU-LE%%Iv7iDMd
z>?2*m;Hl5o+3XASx$7u+&y`QZWpu)6rzGr1@zK}ZYES4L65pyzvo=5dYaq4S1nk+Q
z#Zu_+-x>~70F=Zoej9}#5026fetasq4sRe|P5P=j2s>GR6gd(8YDH+n=ZQdbV+rc2
zhAbBnK36(Tq$vo0x@nn_1JN+%Hs@zX9em6^&rHI}ejQyDyNuCavI%Gd=$eIRj88cy
z^NaEUnVB;OhcqTUT`G5{txL5vNsvIxdU)s4XT9zX$PtFlpo;W(OMJV#4jJ5tPB6Q?
z%w=<6U&96Xvpt3sJ1w~?+QOA_Chr?U$rz7~jlH2mO}&c>@;ij+@!`=Q^K_?|iVx~O
zF0L=MgH^n~m^Gn>T{<;d?gJ@rvWI5X$Vquy&ao;~11@zIZ7IWn9UY$pA4=IqNBF;q
z!oFq!(Y<IRp!({%H&Kw~qdpbVJq&i8-@R3&RU;_-?=xjMX6_YXzdnnDckUI<?JT5h
zJinWAdoNtIHGAh?SV<F$aD^sRUec!^@r?b{&3Byf+_eK*DMD!~O{t_&ODSDHm@?%K
zOgrW(1oDY5T$_8ZGSEbpX^t;wq33HSNs`$liV}-L_xGbGggh(!dNf|@AGt`Of$e7>
z#=mI6n$sCV1cbM}8PUw&&r^|?%Kom$C&&{}x}#|k(gR%KVjcuh{27!z?~R&>=x4u}
zZGcAtKt$_S`;>bP!%QjIs&7eA(agRi%M6-d9tXl6K(zu-%02bc5;5{2lZ=8)%VP=9
zsWydNgeIaQs9ckQACT2t==n`fY~XOt74o1?C!+)>=ytj32$u^~mq5r4tE?=xmMBFF
z#J_U1TzItDjmI4{f|AO2%GmBRVP^d$@^w)50SL+MpL|&UcLoB;3=~-MO5sHt*k!h3
z*W~7mdKHr^ytIHFo05)N#pj(DLr}VJ!%N-u*)GNf7HfHx(G{qo28D%P=vi~ZL_GBn
zZ8KG~P?$7g%-eXi?MXPIK#lYmc-U-scQLK#>$$8e)N88DWiNO7*Y1b}L)Sc^L_h8O
z)U4X%hWeWGY3}z<A!ec-dxl~gpWj83YTDV?+?IK;4mu+u>FrSH9jNWlD*7#7`>R=|
zaLnol&KPijdxvf6%ehmCWM=#f)utB=Sti}e#z|`V5tYF6sS35fDr5)bNzUHYz&g{<
zq06#-M}&Wv-dXRZIi4O4-V6p4Ujn2=@@{?;m)rs2V(SI>zTp6}^X`digXu2Az|esE
zs0}I}6(LezpB^dgx8<{?YBR<1Yy6JuV)GGqpDXq>RHr7h_c>}qQ-&d~z&Wg${{X+~
z3!^2x$=heIi6&U9sv68BGib7khK$wc7+|?!Cb%Fzf0i=ibh%<y&R>4);yGSajcx-Z
z+)NVG{98MjQ)z^VmI1hv`_XyM*uzW?bZ*yyFQ+7aXSeeG`F6c75CstW1vZ+k){*mW
z3l=P{+E&h?gMRgyQrM9utVbAU=I!H7^aXzvJ%${vGmWy<ZTC3pgKeq{*Z#JXU6A;^
zy68lomc;a|=>8sQXXAwj+Iz1O(JG;BQ|w)IUsE5spJ`HNWXl^kqvMiRM_i24A17l-
zO*2A9eHnGag3%M|1RBh9t1v!pu-ZuWS559zk#f--b-XIdL5s?LY`Ym-Q<Btlc-6&U
zNlD??IVB1GNAK%jqrsX96SK)3n?%CF$5292tzRZ7;YOrTLV>~}9?((!4`W<DOu-CG
zwt9`al1z)2O4@sK80YAWK^NIFal^9HREdoU&y<Hub@D!{a&@qy`K}Wgk1BANOwvrQ
zZS#nqDq7}_aq}2Qh&_>~UFDg11}oDF_@DVHo_$_pU!5YPu4R+a+As!tKKZ*|5hbT2
za19Bh$NO0KrbG$9zwYp^CJ#3%Q;qXOH7fWeyzYp!qtDW(hPCZn#caaXhOA_GF?=n6
zr<!5h%~?h`Gq`l@xhG$l<Tf@Ygh<QvlTv9duajuxZxFruPP0Td0{c?r9^IW2q@2m2
z29`02@6|*emONpq?`tX<&2{$6vr7%5_@#Z5BE6S?-v_&e8+s<_guqS=FG#zj0hH@{
z3N1Ow#Z<&EG~{}@@msu+t!6;k)0U>zfFxJir>9-wF4GuB%#!PT{{8Kz`Rt#W%VUe{
z>3VwTK{<a=H9k?`MQXn_r9N!_jsyQ*tc;;-G^?>TUYUso7?jNAZC2^`u(+3r4zKs9
zXL0f|Cj<}#@5Aqwf$vFc))*(7Q)z*K2@d#P+Gx1kI=LL-fP~#;j@VRwrk$CHqw#C#
zO7=nYHWTRp>(7=`Ee$cbY)tyN_B8s>X1dUd)GTbB!z+rIfk#}pQKv<|mtsT^6bCvW
zXy1EFauMckM-6;`L1$?~hX-<9_^x&2!V$mBu&fciWT-enW!CyYYzQ68aAJ%8b10{%
z+HW%ziLX%w-r^x6_eSFgG@-w%J<Z%zd8_BItZE~fkaJoNHRN98RCbGoOYennC5!Ip
z3!M}s=9Pc?h(akeWl7jqwwui0YukjLfbz<Bm`h@AKn6X)53lTf`YI{|Lig_V-rEMk
z?q_~qJ7p+!T7X#@YS&>ES@4(Y@y4+-0d3w+J;$FAou3m`x!(i%iNu2>4BM~{l_uEs
zn+XCFVgvaNO|q?j4dfWPs3KL0z~EMxnI;6OJEmXsHE!ulYdVy2d?CI8qhNl;hg=*0
zJ(=hE{x<p2y<AuYJZGdO>#AB1JP{PdW;`oLzOdMK*8sFsSLK;fq5&Kb^g5#-+f-8e
zuFyI5Hj)(h`K8U<U*V(S-qdfr7+llI3l5zVx`>qT$FhvDH=dZKOP1x&o|d<FK$=-A
zYqdx|+otP&X~WhK#N<Tg>F(v-95v68nEXDqv35JvaR1vdK_@7l)3JrEcboJZT*X7U
zBb6H_OC2U;<0P5>yMfcZTD)_)%7mC=s@cr^(Ii<Gt#cKPAG96yt@9L`95k(Nb`yLZ
zpGqN;3cHY-k({WxxJ4wp?j1{d)77VHQ~h}`YU*vp?(LO8xU21H!!ZwIlu&(KeA{}O
z2=)9E)Z5IZicDtRc64OthWdMjw(_?)7(}W>3NGfy*lGvHu0L|RWjxhx4l7(02mQMy
z(`CtuB*Bo|4)$;cw6s4OxR^QcROXy59hAu0cnV@JDEWLCivlmZH_0mPh9dICF-cAc
z9Q=8jN(h+}9JUk8P!=T?@iaGPX7HYJO#KsTof6U~Y}fb2p6jG)J0)usQ}*`rf#)I8
z=^_V-q98Nwdcvs*birCZMB>%pUQz5y(x3{$K!&iyUN9J4!kc0D=47tVDPK}{>23{q
zGxl=pH0%jmw^)w^*-NCg4v;9H!E$-Y?qcRMaMpDYYVA8oAHN?c(qQPMR5c#N<0W3&
zgBd=#vFO;4hGL?5Sf$H@`Gt17zGafJk^~BZf&reaZx`+8QX4(ivhISzO$CFP)M`RK
ztpNLpjw^0VFI7AJdToE-m*MSiOPO4$2r=Y+2{r%^t;{-F8J({@*M&L&<|-LeUvKz_
zHwgB0Pa`?A6UTYtJcYD3@eja4I`zL^WnL+dYun0Q-zSeaADDY~#+-D2*^JBit5rX8
z#kHUnAa)rn+AF>^6Rg?9j}6BKH^!2P!j;}$=Xm9>*spcQ<>_d+ku*E>(KPQbC>L6H
z2MsZ`?iyF}a-^S)i`iv5pJqb+6b!7zbPtW!S#lXN;*fO?bsuO|Bk|_BpWPM+9^Kp}
zddo|xL;9nvFEBe5|B$@7kp$ld|KWK^xc|E|0A%3TpnSI<Njw5z-I=PUN0>5(KAjt5
zL=!PU85Aiti*~PapD5>(z_}ZkImz83a;{J<!nbPN5?0qPBu=xuW@eIyY?G*=coK!H
z%t`%*%zz@&ljz7=#+iPb(an0rGB7@qmyNC^q55amr(4sBsXTHtx}H@T0;I9JYoa@N
zM%vo)ik1G8At&<iju@^^I1+`0<yWqZ)+T)?)ai7F88f_zOiRQTPK(Nkx8*ymeziiX
z9#(N9Uh%{{%6Nn}{w;%&H=kW_POj3=G75*Yz?p?)S@^1aBi7R&iNkw9(1D4u%~}?v
z;D`*s_}<Z<zxuzbLpcwHW*gwh8gbM|t?e#-s(ynsJ`?Q~#MlO?1=8}<LBm+(U}k=>
zRE&DEap<^~-k$~LQjAFvWH6_)PW;7gg48wtIjH!Bn2x6t_v-e(qjl%ec)&MgR?r&l
zxjdIHg{Z#%GagoZHBtD*BradTyr<?ACeZ!D<YDqXT5@!3T}Mm@MVg5LTkiRc!U`mr
zT@G2iu}bhEGj8R2ecA^`W&ZLDNoHQ9WAb%&$2l`f>qy_M-3(%gt940&M!6+9>`OSn
zte)vs^Rf!Vcj9p6w^*{^)%NBUtU42#4}7#(naUJ-3=dcYB$mgtou6{yw0ZH;dR5E^
z2<)4WZ4{cm!ZO|r8R0D%AMWV%OkLUxZa6=E^Saz_maW_`RtfQvhw$Z(7b~PxZ))Si
zdrnn7l<nMnkF;Od1N(WuKh|CsC;cndpF{XUEJv=CZqxYMC+EpE>>&2|5A>c|8_8ng
z@%K~|Vfs+Y`7@+b5GPx^Zj?XrVRIZ^m$IUW6U-qP0mB=GIkL*uGHRZ(6?IJGjaq?_
zA1K*PgI=T6iKA7V4|Vn%U~R{+n`5PkAq(0yw#VH#qvtKTf1NDTblxo{Zka3pEs^vH
zQ(+0r-8(YY18W~B?sxE!z+F3kqWEK~DVb*Ah{RMz7TE~@7r5I|%9d_6%9u$86)q}G
zFLA8^twHW)zm8Yaf1{@T@Om6o5fG`~rNzGTID|u<Rq1IoX&l&?mP$KdsbLZQPRt~m
z{LHx!Tk3*^7waRgsVXQsmQqFvTu6DJ(;_GreIV#3ad`c9)v~%)vPg&JOdCey>)_wD
z7$biFCiF>s094W)RMh_0Ru=ScVFbHvL`>cx1Zo8;Yr3|z_!%BA>89oPkhZEym4*s#
zz!#RaV=4J)qYpy>D(*ZA@p+RdPK8DBO<P@#Up=w43)^rofcNA)hoOZj3*DbEb9xv?
z$08xsmUq#I^21IkMzaNnL_y}J&&$N@wNf030L^WYTcGyW(8me&PjW@(J}EZwN*;7%
zLuC{S1)SUyM5(t*?n=k{a?w%!_-T4998Gc_5VGV8$Y1DtE^lTlIic28t=+r_^23>9
z2?gO}=38I`F8HM+iY32?)h=V-$S8dQvP}jvX~+hL-x4XypFTI1K2#W3gUT^V$7Tj_
zjO&uFDzfijY6~Na#%bla4J+Wcr0X^kMhjbco+76bcb>{uLC90mZ!W|khX&F^-;cOh
zyeQ*8+3>yC^>2plSy=mb%Q_B2DAm(f=Sb+B-IlG7L+Nd{jmL11Ws7{qCuw`5Pc0mD
zpiO35T(M#HopAbaL%F<4<WYXenU&Hfi6-Mtem7kHSK2OG5xQ2h=j^x;;Wga;YpRan
z&Zt}B+h!nb^ezmw_3c)`!td*t68Ry>{1KdR$l!Gf(!Y;S&))IKq4+^qzE>$y-T1DC
zmF$8G&h)FVt81Z}5U9Q?#v!YZ3O4UZn~~G_TekzrMEuSIW7J6=#i|%q73fgd-jXga
zT8)EcgcgXPEMPIR<{P7i8spDZrDi-zhWge#-CXx0DskN38MC^lrSBBZW2k1;hy6<m
zTW)QdShco`>nH^%!i-Uo2RHBg6Mv#j$qsns&lGl3lZ=U+0!uP4RVo<?tD_KSYY2XG
zjIDpm`0<P>Jtv(#5*|YHm%)wDj_kvP?a+I#4?T59u;Ce={n`i!iRR|0^5RB@#AoOr
zmZt|fXb(;~2+(|WE?qmBk2c$Nc;gPngTZ%YZR7ODV2ehfL*x;JR8v7$Z=%Nv<Ncmd
zV?R>=_93cT*?ZRkdvo>J&g=_U&G8Z%II7Z6WWJlF*u_Jjl8501<2^lw!~op9vg%|^
zb#uO+ZI^Q_pSyz0aSwcU-utaLZauxx0yDUdhhZVN0)Tm)GIYl4Q<%zL?Iy=0hCVQ@
zGb#0!sne4oJf8;RPL-?TagJT&vEtZi|6>|a)qm}jo`2^nU-Rj_xtha;9{Lq5Os?V)
z&a&-tIu>CL3j7+GIFs^L216w%)~=??I4r;9zgpT+w6S+qLv^SbvcGwwgG&w^DBeoU
zk=5HUOr@*5^t%?;J>h*giI@}>I{nbAItm?Hfcc`GSPnm}kGvVtv}v!QGe+5lL7)*V
za(vA8!a*j02$Dz-8nMzBIDmV;D?v*;2&vTv(!(|5W;eG#dX?yJRy3Od?E2}%VFb)_
zD!aP=fGL=(mj|m}3XU6bOZv6j`MQ>nO{*uKb>C414W<gctUDXM;mjd=|6>0dOax?K
zvzElr?gsrDTFiYoh{N0_>E7<#fz#f|YeG){+xAw|qq1GmhKC)AS0__MEt%0bb=ny*
zPL3!U>}{_a=J0}p*07c2xX^~VpyZb6)KzoO-5*+sx)*nL;pC)^$M{+A63ZDV<6o8h
zQ&h*<d(7y2bj0q*>aJ+YC55k1Reu+xweGhm*{%>ux|h)Q?RaO$SOnL~mTy<_T0G0|
zMu}o87?lDa$vvNeD(|L(9l#ywhWVF-hu5l@oXI#HX}(7JL0b{pF%3y_YgF|~7usyI
z+1b}S={ngcgprdJ*#-EH;fWq)6KYesJKK&7szlN>W=gNqpdFjuo!T8OxKfGfKEulZ
zXt;zA^7PCG#Zq>pc!L!63-L2dgbqGbRAA~gP{rWV-~+sz99Z4he!S638;WSw3(@ma
zHkkQK(X)S8BrcaY#|iAA&b`FmojPb3Co^X`sD%>68<2DuF%e;}D7_M8*OVIWLz+^r
zdRZC=9k13cK*OTxMT;?jEc8mIEK@B)@6rdnI&v7SZixl5*TZ6*GV(W|Ph1J*4k<3i
zrrjJDUNI2oj1{esJxV1KYXcJ=G`W=`i)|t}R~>%sGF75~XnR!?8<8KMax!(xrLsTW
z?w3p1FzpI68fZQMMRu}0gpY<BF6e1hvA<(3Kt~Q2w&95~cfp|WYFC+?(`&KI&``%O
z&LjVdR=*gtc9)@bLK^lc$<k+*w-{sj@;3WihW61+rOsLIvaMgMX%$z@%r9Z%2W_IT
zrx1sC4Mt=p=CkpW9$G<8Dh#D35jZb`8m;ZUk!5Z3w~CduM5KXiwqUV+bSdUZZERj1
zq|E5v+;Y{R*mymnQM@tb=^eWS@06^na)7F%{F@c{{K$hcRu5QiC8bLEbWRYAJV-94
z6I;&TP9{N6CTNLR!uu(CDnEkr8X#h4nnV~Kt>el$^3W2b2IW)3BFCEZmAde#C16Ll
zu4*sv!1`zN1D-J0J7Rudv;NUaFnl&PL;<x(tu>t!VZNNbMfENO3xBpakArg>+Q2@5
zI7%hj3xwgBEQLM}gpTvH^z}%+=hBe#r+(tB1+%M7mykH$Ir`w}1Bz|B3{v3mM3Z0>
z3EJqz&v7EBi2Eu*rs+<XAZ;SWi>JAuHadEgT*czpXGY}^DIyBKEwV?p=Lm8X@e7WJ
z=G5+5*Hy>ux8w8^_0J;59_z#ClOYv}lg(N#mDbGEd<msq;waX6$}QFY`aH{dEhD$J
z0}ioP`@QTMiDi>?`aYJU<6FLkJ)#;NGVwRCI33LFzv8s)|Bhw;-^BXDszQ=NLjRT1
za{Nn9OUv}%cib2m8U8<kTK1o}`j0^E|5`)+2ble5RQr!Lw*Owp{hz4zf5O=xp*}P7
z&v4xTj%qoWnf@PYsGuqfznAGLNp%7sgr%4hgPdF?q#*hkdJvcyU`Pp~BvPCtC`q%V
z*d<(DB_IMMhC@B3JiG3<+Lt<iFTG9I->-Ie`o0?!FVF49Hw9@4pzJV^pwqJ}pb(T-
zRnY(e0KK;Q3-D>1n(9!@;oYTRrD)&>*oE{N<o^^<(EtGnubmtS4zhB%aKIGoYr${s
zfZcNb+(Cr_`2+9~C>+>|a|odjfI9?h0xh%xQrTe)V5VrGU7h&*X|5AoSG~SN=!4b+
zx`~PearlM;7vd5?P+))n{|$^~LkMlh?j`W_gTsLdCgA-_3Q(N@3v)s_+_iUf+y~?2
zaO_81LbA#PV5gV^yMft*5o{5n0qh0<W)Xmk<sWIWPXlaYQ#k%{ue6}f!3OyOT%kNE
zFn}C+q3;6K2N3|crT{x7E&`-SBdp+?HS)t{`gMB%?;IX|{u`(b=<@*&?9p!tB!)Nw
z4Auay$bCZaD-OV7pTix61n8yl<qWvU+I77pP)At6GqyjzX?H*n5Y)s5XaoM~p#B%u
z&ap?dux-y{+qP|Ek8Rub9$R~C+qP}nwrzW+^E7GuHfhr@UH{-tR&uX(UB`KxW(+{*
zp-#sx!RtQ(qYu<mIw?m6$@TW&;1DopkslR&oD@Jn-BumoGdW}HeY>^{Gd&>G6qEDk
z=nzuqVtBk5i({bqG%qO=Sx_(dS*#$C2p~|<chJxP1hN33+^fMSY92c9VxxA%H}p@q
zKz4Fst;D_Oa3F)&mY{7P15Zw1ZGr&e|DXpq9{!U;-MhaBqRG=I=>;-`4chxo_>zUK
zdnew3#Rqf-#{j-*9f1b;e*Zc#_7J0&;+Wh%_)iS=$eoOqx+1Ud?msb9CMJ7VKXUXO
z2)_t91c+TwC`h1x*KpfEvJW|6pRnI8s?Z=`6&$<#m`Mhzw;epbe>R<ex3$&3+uEm^
zECb-r4=C7I+%!UfW#3jiAQX^*8@|q;9qe1~!ynO4J&kX@&7Ye96z$6^-wX}k|Ez0A
z0WELtQU-8SbYfjS^Wb_m;8uM*u>O2s)^iOao$uf7sG5+R+VU3~EW6^I?4$02eJ|;h
zHUKT+8kT{I1lW8toZV<_ZW+J_2{i6b&+5rd>!2XK>NR&X0dH*aQct`7Mp1z{cdiKT
zU+jIyVmoL*TygIi7&h-$xb7kO0R0dkV!^L*39m^6{K#*%wlxJeeUb5c2Vet<Zv+8c
zQ@a3F?Ln?<82J%FZpD9oW%~bK0eB>T;@tc{0q`z-g>?}DVmA1R@DX|szI%jq0b)je
zx4EDJItZcMAU*ERYG1$G+!0_PhY&vpGqSzi68~mYe^@h<dHAhXHn1TE`1om<w^lGm
zJ$+*CBe5w}L+AMgQx#}bQ!Y1D-BmWLQI0GV8riQ~?V7@hdjyWXe5F@Q88bv)ch3MW
zyc2iH0R{>6ka&|=0wL;mz`3yeCyK~YP_g;>cBPQC*$W4e52s9QXpQcGSV{RnQ6(u2
zpL8tHsL=22b7tc%k(le?-Mhe%v<f%{K&w46J0w;`4CSHpX$PY%$*|;(50B$8ZoH=8
zx3boEJ1;XRQKSj}tGqbmUaF;>8M4;#&)xP6!F`K7ajGV(HI2r>WP8;-2jd2xMk~pg
z>%};Qk6d6+dN~=!XBA0RW4+47KhkS47ztzhE43m_uG@X45*;*pFN)<pTeG7NyPe$x
zm_91o!M`LeCg{TTxb#?egZ?=<jAt@aH}T7Kwcoy}&;@w}n?#7tG8dUO)PgR}7QOAP
z?rCX5okKVD13v1=9Oo3oERr@wn);^dz}TNQF=VtV(yqV@BL!<`yG}nvJduY}bKbZv
z{WjC_A0#EefqBkDts7!zs8>qEY4*?2hl%6yXE_4BN;R_~R$q&?F09a!l<x9K<BbQ|
zQql6l7*XiF37&HNsz21Jy>0WkU)kR@&X3T$Na=-HuWx>QW2rIEU@UcHet14h@Izmc
z1Co^*R*1;gYzV>YNJ4S;OchM&)U|YqqlZr>XyF<h<XBuK!X>e^)6V}+-CSejI+o|6
ziGTF>v%F3&Nb>6``cFy4>60abBjub%X^ou*()^MX1<DS<V!E!AoTSTF-BUdWJ+xa5
zo-)M*LM<t=Y)&^!&h|q#6!kXX#rZrC`1oq(dQ%b&RoTns<YdWxTUYJI7yS6O_9NyU
z`}Yf^EsEsFAH1}wO{D)#eQyh#C$}G}QPGb+(8r*+>7ic!Gh^`U3H(r(@Cp+!Ky69z
zv@ovDofPxOo1H7@x3QkFCq3}NhSei8?F}@(gB;LSHO-Sa`%uym$>bjaZhTgOhVGk=
z8{ieJylB#9%ZCqt%Tdeb|NVL2+LXhYAm;`0b|x*lE+E<HlsWqZ%KvqG18O!5gTKHE
zKU-<&uEIDnROScA&RcbA1VaqcE~+egC8WcwoYC`3M(xX?uxIR6;?d|i7Fl(Yg5@T?
zm#4=%hMV$@Z&92pgNV`CU7wtf-su+mRx|C_;OxyBC-YdwGyB{*&l$)k$E!ETcpBMV
zDjGnvEgn7$%J_AqjuB0<_UI|Sxd)UwX9^<>32-MrOIu!%$J8xb5v`uci^O%a4{5=}
zVmij?We7`<%{r%eNh=8CSq$)b6^+<--oNP-_DLo5B>`2BI}4<+b;KO_g?h++Vh0Bv
zUTeJ}hH#?V1&|$xSuh+q)i+lLW<PjlLDz|w<%?PvVk$2QplVFgJIlr2I6GhKZ$4z6
z6quzpb;8RgAMGNiX!z<VlNZV9{J53!PI}ozd`u!mDWc=QXjE){u%CXgM_ZY}%|Suy
zVMC^AG7=F)2yhzG?A3ui;-XB49QYSyoaV^$zB*GUHJZap;3^#ewQ_MIt@%B#vW=(m
zc<mTHO;VMQXaw9<cO3<#!|5+-l`@t-NB}nGuithJ*n*+ttADj_L8GULCX)a$%1;7c
zhy9zidV#v>Dos~y7#h7vIcrDe_e|k|7%pWv;;$QbMcbm|8obb9aqxnbjIgyeL3@O#
z@98tGyk;uSu@NYbJq59dI%B(ht*2A-n#r9xid&cenYS&U8ybqL6$(+*l0*QZZ&gqh
zgo2QOX}k{uCxroK28(n2=XM0T(EF!g$=E}g&|7k8G;iD1z%PG^c`zlP;@WxJe{Tq5
z6Wo;ECdGN)2E*f5&dQWE<6Ie}({62H^HOz!e)w^PM#@yLU8qe$Y$Pw!AFkKGGTW-D
z++kZz?AuQ!=?;a;mFSEF=6qvxDJSB?g5yQ5e2Ht`j0$&0+{yV@!}K~fQkDv+L7J`e
z^z6`hEKD@$+PyLxDFL4^#Sj%?hljO`-v6u*m$+CRUOvzpwY2l;ZL>?@FO}CAt!=RH
z9`8j~MY-1l;R0hqc9qH;e8tFoX_9KYD-v_e9{A{xnS@N<^mALz+&mkP%tw>{5>GZ?
z7wi-|c^8CWRiR)b`}<ij=L?*d6gyS3bxUsN!%_9g@qxuSHGh(1Pv<rb(#AH^;<;<X
z4@N^gT3>pJ3hOs=b=}FyIN}J1piKfDvn$jUji$DYJk--9&Z;W^A^KecEM<a54k=V-
zA{C~kRp^@+E%s~UEuSh~N|f1gnx0MzD%V?zS7wA-a*kWT{7UME{{%NuH(&=C<Xh?E
z&eAB{ChSxtFH?6Igy)I(kuL5O4BwN;iYYA7u}>>`e}Ly{Vwe)OL;!Oj(r%AU*0g!2
z<Znss?N+!urZA5?E&?()DR+4crr_(E3NUU+d^oW_>_FHCyM=;0ov*hlnfQ`NY6U`-
zIT4|ESJPJiwro1}MII;gXzI}-CN^Y15XrwN6PRxVj6E<%-KYXnexgnH(>ek>h<rDf
zCiM<iQ4uL^0+o|@Vl|=HxVMgogpA!WHF{c~*4_mFlg|>;{LQO4IcVXe9pEp#OSdsl
zs!2`zG*5Tm%TQgGWP>-B`3c<=d202dS94OqzT)w0smT?Pk_tGQSfj&@!xT)GF7qx`
z1qlw-)L*3jUJ)Qb#Uyh`qs9DG*Xn9nTlC?4IJU+z;D#Gsi75yMop76(&c-Ty;j7JM
z#T(~mg^n;2JQX-v=XAA%(N8NX6tZtAWR^W>T{2yNBem(EAp}z?s))bt-XT!7h)>}@
zthE3Xgy7Fsl0<%)(;!OwxAcq45`9nCt$~Eq<2;u{`58a=Kx}SbE>@Sv89L-ho$I%4
zkJ0*p+mAXFXFKV=bmBl{K^7MmvXs*hav7Db8*?|BYQRI#koJU=s&ulTt!GFLPvhJq
zR8rpi2h^|k*7M@)kOt12Z|3*fvpx)XgqlfDlR}%<cD%`}C0mCTOS%_3cu#>(AYXm8
z_e$~jr{gTzHOPlN8%_&naCM<5)?H#Lj1SJZr+O!e6Ej&-cXZ3u^Bs=h%CQywLf+wv
zJy19A0bCYUndBViLV_~H?(q;6RMq7Z{0TX-*n*$qlU`wZjrgh=*3~e9stpu6qS|?m
ziqCL-3{Hj?d3&`;H~83(kCIH5CU6*D1)2VJ2;hNKwF{I(Jj}@am+s!`1lcf}C);z2
z${jJ~XcI-tAseI3BIOUD^tu?mib!2E9t#?;k6A?V4s4yKS-3R-w$qB326h1dbm=vw
za?FX`9Hb9g^b+CQVF%kj<HvGxd06fQ7qJ>8JIqaL^jKPnhLDk=^gXYZWo4<d0Y9qy
z8PQ!^g5@A6X6kt_RZ{LbMUdqi49-Zuh90lvy$scQ^(&huz{4LSR>upA;51CQMT(Qp
z#p9*c#q`sliQmDkWaxr^A_Y8Ih~lQ)z8%!pBq5u-^PXHXR$^p3H-2r#KFq3wHYqEk
z24yyZa$4;?7GHC$NHPN$!#j9bnCyv8>64Uidw!-P6k!P^vQ#Or0*;>D()N=19G93s
zB>b54h!-79$Ne1hea|&a(|zY|HM>GK%xLwY&m~UK99OIeoBY52C?yne`rwurT$Cd!
zIldplRn#VjfyeGGFW(80e!X!9CWHl5yVIt7b5+4m&<h0{8_mXP1tEiw&ty6;=PaJ3
zkO~(Gc9qKJr!Dtnps5El;TEm6$)GmfNur1{cK4$sjw1}A_8Ver<dzBIjZ+IG)c{b%
z;wSg1emMs(215IhEb|G{x70-&DeG*xzd>la*>#7?5suJc{U}RxP<hjG#}lvMbxZb+
z7?;H%fHX+S;IKItg7enJ#rK=!4$uAcUTT{VU*l8k5Q4VIE9aSQUL;$ulYXyilP3%K
zgmX(#;i_gkl8diLbSXenPR?sn$!Sh*!z98OvKV2BC7sJD9P*6=WKg)W!20*cws3C9
z9aLi1WxTo?f}mPy`I8SW%Gd%R=!X&6>-9~|fM-eF0_$7kI|k5jv?QV=P>rk6X2AJ|
zVFwoA&OYXI9Wv~?E%4c`k!eRI!6*|)VM&sl-OSo`#3pn1ZbXkJQ4NbjifS5m=I38h
zNh_2wBV4qCRo2V&VzVg|W5z`xmGKUj@JXMGcZQhRx@pOWEb$X)^N@e@W}F+dnq0|m
zfu4K9KFIyh)ACD_rcN*1B;V%D+jov8NRHpDsk{HI*dx}=+WMv|mKmAZClOFSrm4$p
z?;+pgrJdG2><zZNh7L<*B6yR^A0wsdRpV|9CVi@072n9U1;3{A_*fJ)1iXkOZ$W*W
z(|-xmK(6DN3Wa}(oLR=tX!cPpjiq`NijrSY-mN8PzpcOU-xv;x`w(qB^!Uhh;?Xhb
zf98I2+gSjPk%6v6;M=pQJ2LTO2G6dNm{(YyDg?QqJFF=2c_`eGuPkO^s&#X~5HF9f
zXLKMFun^K2F}deYVU-1-A0G2q&0myLjknc!kiZ6vWtdAAEg=|>tfFV%%?6f=A=(L`
zvmJ&Xxyw`ewtO$dn}8#%Jy3wFU5yf|KMa=%wR>G#Es&85ZHNG~#3X%_M$RUdF;YWo
zBoeZ~4xAV&D9L-fybn0q&34C*-uAvscLxvvRtvw^C4m1z5HoaK<hynNaIXVyE?I(0
z+TFC?f22vfwqFCVzpKlN{Y#6an2I6H4LTjUi|OFeqlz34K!a5UnV&vT+^jDIZLYmt
zM-Btw(fP%p%b&?a-j<n+BOveQ-VtD`)?(_N#OT&%q9WP{cKrMMOW#pyzWz#J?XOpp
zi;2u5OHqRsJ;MzKXeX&5581En1&m<8N1y=R60U`i4mPsF$5RNdlobyldpQ(Qz3yXB
zYdj2~sAW7LLEe5@%MuzvM^QzH2h89TVMGFuidQhfJ0?;@_WkK;Gs(09->MFxMY(+v
z3b}d0a0g6MTUrKxgJa6_V$(|~V;?s@m|TMZj^4OzPDJi<&1^^eG;<9nExR19*RC`N
z7)Eg*8#!_>4@jz|0z%4ke!o8n6eLN-Y(%EYbuga&(|IVDr6ymO`P2Zn_zng}iyEyC
zSjHS--UY4M=vC7l^UYfZ3J6)roZt+=_aP+oR6~-k?a*snxdHx9*}$bek&K(JyM!GC
zsXJdH@6G10eDeNebr$sKi(S}JGNcUOOc|^}EK2rixuqvpF0;$%Ca`l9mVemXBL4EP
zK8sB;;u$m<-UrI~3Nr=WR&L2dGpZ8M1)pX_2Wo2Oe1~Q`NKg?K%<&~p-MvefjP{sC
z-I7l<?WfJyG9BYe_QX@Lz-|e|+21m3XGUJu-)duNo_%~Hb)H9jq#7G@E{loFqYph=
z-t4=9V-6}OIig)2UwVeL(1RXxxlQ<KNhCxbC9vB!9FrF?Ef0g<YiM(pBwa=05jjE*
z)Gm>PiAFl+To&UZ%(mK-Z9$&8NYMkLr<k1HET2LGwEfRy>@xi%&kwB};R-?5sQm30
zEZT#pI0@;G(rC~khh-k9elxuh*UINw5#d{jw?|{HU7LSp=e8la4kJ(mql-tY*}c=7
zjElt`!3-yKA@ICS%Sr(Eg~7tVxoK(c_^8PjGce-2QMw>TIGYEC4!xMmS9>^-2DHS-
z>T&owujE=yuSwsn7Z%<+-WWVNfd*A~|38K&d`G3}Mz`lEm+y!f5p6lyeXa<Zdb>!j
z09oIX^2R?+G8>%V=jm*8rc93i2H$c&N}=>}t2Lkc6)Z4$%zZ5N9$_`T1!Qvu%V-`<
ztV~lJLWCBM6Jn^8p_l^;B?pF~m}HUN?1{AlD;}N_#zyOxAs@YeMJQzJj~}E~ErzV1
zUM=`@NgLyd%UQ8`sl|JDH%DVT^gQ}(689~W`tS3Zsl$^~mp6*Jz@+(9`JSc3G3!6f
z-c$$_V5))>QQd@_eo}g#T;Ri*L{6P1_RQUiXhc<pjvA=VC6TyBBmG#Z3771B8W&5i
z(}hBi{pA>^oR}io4QOS>^P>IOO`cRRWHa<yO5I|INm}oTBM(4|_+=gjf3~CHSU@Ld
zFpqufjn3_c5u~0WSB~}6K$wF&!XVM#D|{D>zb1*oPA_Uzh8j{Z!T;P+#vvOC%<f%u
zp*&V_VrjRgU~TKZZLp_Lyy;EkL=BcdydiDvp9_w%#rK)sEw$WrRJDKE%_>1;+`(lN
z^G8DAuotFADy6RIbovj`z%O`$;dQQQYW|kpr!az!U*iYQtvnb%y0<Ss_COIy&>7`Y
zh{v|X({r#^dFrW@*OT(aUmtqWi}Z`AJKLRp?Oyp*VLrfZgr(PJFOCZ%W^AhMr+Hd6
z=yB;x$-?c_p-_!`KbP*0UZnLHN<X&B9XyjXOL21nG)~61OG!ZBYk9QYDt{OQGWRki
zHSMdkWaV2lilYZsPOX$uY?$wrr>xt8T<e*ns7Ay1$$FDr(MVcM7s5-YH&?WIz%(7o
z!|VN&=H%EIyiuwES!J6*VNfpSR8qC|nRM~L2ig>^f9=<Otw8;Nx`b_q{ezNt(#gDh
zeV>8;Hf&lTV#D_*X9;j13b9<cFadR&XdA&)nK2fsUAwvoBd#WXkk`Nnvk;g`C>`_n
z2yD3_tYD<?D!Zcn7uy_VJl<lcEb6FeG_|jSk$x&zeEU!B1;^Iz-gZ-=n`927`L^_a
zI+>M{z!1M+7_P5WxN6pu`Myl|V5)*PXp^mRhIF2lrICS87n-A(Z>2=CsOjK(^yEx-
z?!*>sDSpvIhF1rm1EhAIhxF`z$*YEIi}vL9un0YBm-Mp@w5@V8^s~+DI(~H)($m5C
z6uHV3Ef4aqrOrr9$T2P+7P<J9g{^{K`_xndXK*qNEBi4H%EDvS!LX`IOKkOD{jJvO
z79pLWAi&kFFr*twWrrn=9c?8mdY@Bbyy@s@q_=H5!+g6p(kG^-+u%lzQa>peZ@E7N
zvnoujZzhRxfj8{ePxbYzh_9nQ5l0GAun)&WyzQd>8&7t6*gjCrJp*bPZwRexW`5nY
zC&wB|+t1ex={F!6S=i%$(;;3=gCc#pBvwLcG&!JmB=xz@iTBS}ux?;POeDFE^GIpY
z(f6zSpNCWyvYgT*bL!vg=aG}TR6mW1hU?~)LY<02ovrxTn?%l;*<$&)x%Q0yWop=y
z=(!&zn5_R@gc*vdLFJ4?az`qk>;8VVKL1d?!edJ?+jZI4m>eM@BxSM_GlkZcdSxha
zy^4g+dg8rcZJ7l^q1J0g%L=NP9BHP0O-H26OT;YC;~nxaJ_eT6az>!Q-ac`wi2{bn
zl5kwVTYyo-f7q-|xK#PI#i>|cZDfpFHuPeZ)Z1V;%CM?wJM(vvZ~5(%>D|-#Nk`q@
zj-!@JdxHq`LMWsrg=&5kGYU=Z%Cz<{7CiNGCscf!nTW}SWEX2W88FPL+`MF?RJT26
z?sb0!HWwvg?(6*A=9n@|Nu`qKEIvDDlg`ORzL*Y0ysdzkcK6+M?;8LNzBC+%v64|C
zrik0_pf$|4<)Ttv93B&7!zX!rt($%?ehs_!D}ZUGGG60$cPXPXDjov;2G~Y=36^nE
zJz;$i=#J(CEO|rKN4#YjbO?HIOjyx?eCzJt$}Rs?iH_sjDW9Z8K0E60P*&H5aO-g2
zTWamQXmE-W5fx-(qME-H5QjzOvssDbuie1gnUpqLYwBP9CH7VW;Ef{nT#q7QlHsO0
z+%SnDc^1kqJ`SMf2q4}&C}3=9X9J5_g8Dc`?wa7dRsvgww!S{8^-L}VU#YYn&&9ST
zAHK6Q!YaU2HnLL9>a4Hb9$2bKWtMS5jWsFwh#uknJE^=~aJF^~D=jRY;;K1lOn*Fx
z^wL=xizn%7D4&|`ME>R1nG#5wDcTi5AO|gDm#;s(kcd-v$m9UMCaXolY2SR-+9CyZ
zm6|zrH_19uU8X{RdLIwnyg=S!hI2eAdxOk=`-7334J!`E@S<iD;J}P}!>m-!mg5);
z%V(;&NGfBQFJ&7Y8JvjuphKyIa49cREq+jJ<b_xlpAJ{zycj|420kM2%GKW!24-dF
z2ol=h0IaVBeF~Remv{<O166;V%;C-(&JxND&%zP<vO{_cN;DFDOzHiB$I}#BWYUrH
z>7&<|@xnG%89-<ZjUc|GlLEP_Ylsp#xPZ4zk?oDb;NGlg**S@KkVEiLQ13Pjv+y_3
z@^zADYo$kxyKVnYO7o!{a4-hCQg>@e<YT3n?X$hlqNX+^Kk8vxq{WKRcE!Y9MmWW-
z9cva@s%A1tKv2Q>vfUe`4Ml&lmx)5pr|*pi1!RPyc(8EtL^AwXr0=qrb^yk+y?S>U
zv;{|Q4u8(QT|+W{KdNAQ65bbX<8PdY^2tBQL>sx;5d9lpVC|9}xnnRDno1xjs5d;?
z%r6$11%hk6%*ZKZh47lNnui#Tfhq4e9)?$xl5dmkKIW^lYHDB~{M|oU37~n(5Hbvu
z?R)I76fDnqmP#_i${?O{T4x(2Bj1DHN&#?mI!#)9-!;-(uCK*t#{`>9<C&);`!?@(
z&xMBUq^{#ZSieL>b*c%x5G^<Ls82d|Nk*`qUF_DvwAtzgrf+j`(vG}zSNkqjOK+{k
zH78%mGYtSK*JH_Z2>D_;DT;rgdxiwmgT`MX<f7%MidU$j`bBD!w6%~HQg%d`*_l-H
zmZ=)SO&qiU!4LBu(zTVs@h3X3O3MW}kyI8wklgOBtfP2kBlz#}*jXYN{*h1zn^oDt
zZJIGWU(ssYxuMFC5ijxD^%tO)P$x{e>SNlXVIg~zG6R!k0c1)=Rq~WJx4%rqytU#n
zim4r&qQG4p6bA%lB#SPrp3V+{1NaebY_1zh?}{Uo9w(q1><c+}E#^S(CNzr%FA<j5
zU?wQ#p}RGsL4qMM9ZptxS>XpQ=GsG(AR{fB3`ro1l1QTag?|!uVRC1+6&y>OOYWi>
z`>oBch{1KatVd^+0_c*`6=_0#Cu6+PzqK!G?8OGt9Y?JajOP<}<5E#gJM#Kyh17y5
zl#T>jbYUsQnqucd)7%o5(uA?{NE^nl3CD{&TD7JwTc_iI2_ps9<1@WuMry~5=k^Q-
z4m!5c{=)Kwhgu1bjMJctPw`pDNedaJ_AO=rbwTU_+Joj4;40lX!u{3}tH5L#naquz
zhm)wOOjq=18o(&qSbH9SbCd>eoC<?&Ls~6Vjh{C-C!$yMuCcVmlXWuxT-aKVrj_o{
z;`k@h{n|K~kh5X)K;qvueJ_gRaQ<R2DUPYTY0^#bTsG_cRj+s4eKnv^5cH&1v%oTZ
zX!sINFpoLEnur-{)+<kMMcYYl*{hnJ8TF&YZ`px~VR1zWDjJN?%83}QvgF21XCSMG
z4I>K=(SVp(bx{<c!|9QJclA0DVv0j8gFT9M_1f!&^0hBWgh2H+<v@tRVWd~_6&kB$
z5A>TbQ5B8d@aAurmnO><3WMZ^R%O@z;M%ayUc%KS4WrKEecH!%^i{A&3-ZvopFwtw
zAgDC|VKVS#eo|Z`PMC5?XgvuGw#Fxu3_-)wWEQF;vfDzBZM3<37;qMw^ipCR>d&m|
z<g5?EoSqA%ZLnRC&8T(Z@(ujB6n>H=&r{EN(|luMSBI<fjOlNY`JLq-!@%DuBc!Sv
z{^rR*nn4NhH^D}$8H7bTl6E|9kbc)MX@3ebSL2jYE~79UFhYs8qT!+jd?~rdnWp<E
zyaZ3GD+dO^i(}sJii{$^IE$<t?jlo@=ApfX+ApA5Oe!zS3+s9<(3GyEajHX9Pwacx
zi+1dsG`kw9e!v^R$T0s;cn9Zy;UE5Ac*k!eu#l|mf5$sG|1Wq4=l}4Lv$OvH!aIId
z<^LDFLx5iV*F^s78mD*v-xK-&fja&-%!7^LzZ~;_VjgTP|HU@W$iVTx0{Xw2_Wyx=
z5HK=waI*h*-}vvDEv$c~=LG*z5IC6#n;6*{n?UjLK{-18+R$yF+}5L7K;<)Rkm({(
zX|M4rTR~iZ9|%?P*xhT|YsF1mC=qBI2TH}ws#dFriM^zoufrd+>9k3bne(od>!ugx
zyaL4)!=&0^^#Ef+eG2N>C?N!bqI}3h0DF7;ItP3E{P{VH4Ix|s-wK5@CxL3)XN1~a
zUk>rM0GeESE(_?>h#V*o7<ngG5P&v-Ke_@0IRxO~ehGm>yL!<GY65<ZK_OkNe}Z6h
zApiq6<<ANUrnjq~n_Xs8jJ}^B^lJ@406;-r)xTlD#X0=7uc~Nz0Z^e!aYxUNgsGv$
z{Llu+@j#xwYfxbNUF1s}2)H{+O2{*U60orcmsAtuKzA+Rn}E#&Hvd%P5m@!&`vI1t
z$>#RN*5%CuHeOwBea~A7Sm$P=RTK0^^v`WWfw*{v*@J5ZY6H@T05_r}0$NTJ-0Dq_
z`*PL-d|$%?jQ&cqw|3@yN$SCWFs%(tKpp-z19)x(+5oaWM-%v^S2AHQLXQL00%-m0
zneUx~xrepf*4DZ<)_OU-HvEP5tNaJnyAQg%bGuooeU+7GYi;`+?fb|0hj>aV#iVyx
z5rLc>0pmRQUCev_xtgFNrP;ILhjpz*Ka_m+#jbA{&)Vhz9Ky=g%muf$D}q~G;N30-
z9re7&xtV~%FC;K9P|gn;;4h$GXXWMUk9f`170mkr#HY|#K<~~9<~~fVJ0ReFOABDv
zXYPBuhMOONyqarl&y6qEx0WC%5a9B&s=OcR=rUg9`^_c2W$cvR{lu-#RTqFL4*e!D
z5XblH=Y#RRAiWjD>fFc9weNI_j*g0yq+<9R{ov0k2`Qm2fS#%?8~`;rJUGBB_}Hs1
z-sf*HpC9%W{guDz9PQZ`{RJNrwU-USuU8L4H~obk2hVpQVDBc$0Oa!pvOE@@E)W3b
zr?{ID96n@e0{!;K9q=3G?dRYNU*p?u@n<K%cncTzhlTmm;^!xoHuDm<>&L|{ub%tS
zgjSh=egg1YPmOn0uc8vDzS-6FU9SRYao>cOq_;Wxr*UpuW_cUhxXKp3ss1BE=qJ5j
z%0tYPK^%AA^k>8bSl=6S^=DNVZTj~H2-<&Us^FK&P4RxeAvWn%MY!Nci%AGU*WWw0
z+>3f=dPfff--Y&Cledap$yo%eZSW_YO+n~|=JEA|9YlSK3A?`sTrK*R|DwEm09tka
zi352ByiEQP=g|gm!TvyB{*eMa+y$)N_z8L4zkbEf2k_fE+u4U!SnK-<-J~B*$++LS
zk63y;@&Muvu+IHi2Krh47RmQrq)+1DZtwUKyHAn)R`T+{vwKNdt`p)V{4wz9yZpYf
zrZ@TU)BdUdQM0Ss)X|#`{vP{I8UB&_eh3Ek@BE`$3f+J}oyQOoTpOJcgH)JZxnV@4
z<h0>|8Cls4U9Goy!w{>8c~LL4tX)H*7}8#s%x;3=6G5-Y_m0=hN+jYVyKLU`;>jSj
zn7L{zmW5A#`lH}QaaR_D6BMbU<u!6MHzddYO6!Gv$u83Fh3@JD^CY;5J)_H6Bj=qY
zMU2?6fyM3IDQOO$lu%j6#W>t&vfu=o%*iag7%H#&*O)AmnSSzYM@@;toj;or6**1e
zE5@)*(hGKl^T5{3OSQa-bBim?A`CWcbkFx7Qo8dlX;VsxB=domUgJ9F#S)n=Z&Jui
zLeF%_aN}UIykR90tmSdhGm^#&)b^+ZsQ*;r!=WYqf?D%-QoC&F_F{c1-y!cw+fLxn
z^H)o(##QaqVU06Ch6YN;&kI~U2YU$@sM+ClgRP?)-2Kv^D^}>+jcNJTy>?WiMz%~Y
zYI*@_9AC#AuBnSONhIg10*M=Ru?S4LYfwkyQFzbaSr38!49l6)NepO<;%JX?-*fJd
zOUS)9_QD<WP-DTR^wrK6#d~rZW`y*PGhXp-{YLwBS(&;TEj7%;9gs&LRea&yXUWqQ
zEDv02!POZ5W!)9*C)3`ap~z_wX!h;p_e<LeRGWELyHJdC3~ubH=7wGgw}_VT`j?0Y
zT3%43J3G}-y2&%KbgJ!+d)OV6mrKs0_~Pe#da540$^NCdE@9JC@B64p7jVzAwM_eq
zG-qG5lEKVNk<2o(9*?IiOM92<u}<WgQIJ&xpG}hlHShFD3QgrL`OzQe15NfYPs%%p
zA2<lP{bLKcZ~dC@G63+vg>6L|P3LI_nLT_tAs=!5h8#vxmi#P37&XHsJ+pauO15-6
zNLE#26y<>FYDfJil$;ECoAC~IY14EzWP%kjIJJ~KqjXsXS1xQ=r2f6~%x89eZWkhe
z1a=;s4vuH8j@bQ=1E}|N`Cmxp?FZ|>)dRtD(XY5BUKQe184d$f+je`mREP(%!0PL)
z7Im&%;1mbk@SSDu3A%qlv3C8b=HV3vwVs`w8YtV89g$1NGa3FpiMHX_`r0IeFewhL
z{<{bv(7G}ZBOb7ZnwdR-Wg9HXvf3|q97B;0s)JfKS}>~NkCM`-FkG(d(mb_^Rm2HY
z_@~<6d3JXsB)wZDRs^|>dxLqylbM#3LnzZ7X<G9dO~`F_AwfHlfOO+t{S>`ibIpm7
zPsr2Eog5v^W`H+KlPLsvSb8fo?S*E$<I43L7Rj2?#jWSonu6D=Sc4a|wS0eNl(G(8
z)xM0UzlsHzvL^XSQaE3c@}YM&u-%1uD}s0&iQ<_gB5OymGu5i-sfg#c&$;*xj7FKH
z{(uLGx}QxC4x{{`wj&B(D*>Q};yPeWSs*x=$#HF6YUM4Bte>n#<wN)3`BHxI+vzbv
zmFce&)pgFxcdLju>s8=Iy4joP5KUyV@>&4)bDVO-eE%~gLh~0kC%%B!B6qO4QrL-M
zaCJzK;zeg{Wj`x1uTc9MT^Z`{4JlY81?hVzo-AuL<<_I7E~vAn=E%oa$GPgValdnc
zZ2o}7?5X_Y6aXYs-Tde#wk+2M$^=)=sMQeSzBHPvxSu4e8GYPLxu>G3&mjx_LWoPT
z;$ye%V|kdFzn#&Mm0_2NBqfMyr0@WLwJ%oA+OdGucBqQ=@Ym^=GrZ^`-{WU|=Yl|n
zU6m!xQo0IHbEymKZ*#eD%tFN9n4KWWHII6i1eCuRwV;b8F_?)ch&4aXuF^wsize~_
zcWp(y-MBq?5cwBXZ)}YJZr&W9$V(s#>&;;0t)3~0_1S-;+G+%6PDByn6ssVEf3oEo
zM5{3#kGtT`w6#HFC-PH55Ge!Q8|ip<lxy*1y(=e@9@;3;IS7z23+2`gG2AcZ-D;J(
ztTLar6bAtHjRE$Ocq>gaZ}ryLlU%wn3spI}?9Wl{<}$)c38|_bG@A<#)JNThoXo!W
z5XxEz+-cLcj$P%V574=sF{gn9pmZQbX(ddaarYzxknuJp%e2Gtn=Wv~@-%n$WLdz{
zXCB^bdh0_a7Dwk8Fc}O!yVD<HNiM5(Z3Kg9nY)-dq{2%6kn)E@rDI-SD!1c3uOn;h
zqdVQuCUu!@=azqZEYPo@&1y>;JFxB=0D8hjUUHg9GN-VE${ZJ?7Ke{-wrc0>9fTux
zDm^`z<=`{c`-f6+L?^YVEN+G8Hxc}E*BNu;m0<eO(HRE$XHwNi>S>r9u&}#SuOx#L
zpc?S|TG!VB-zMoTI<7u<QxHpoxNi|8BBfzQon3Sfkm0m`7RBj>R9Ag}ZV<ElprJly
zk&eAt#E~qr&eFYAv#w?B@(|JtXRghqfV^Q9I`C%bli3Q5PKXfo6x;4G?g>>ghub|f
z+M(3!)Iy2(DYvmAL@1%x^z-PS;h+#6b|`WNAZZu$iPV-<P_yXGGQQhF0ppgaM#<2+
zZe<q*=JZ%56Frua6K%4|4XPXKWNNy6$x1wncJ$u!5#LqV+Gi367TDORP3IpO%UnXK
zT<9K2uTR$A&5US4+rkp;jl==`N$L%bu$8b{6Ne-in|i!*$jflnM#C5&uY<-cD>iRY
zw@;SlTnSkvf%~YON2M(<tYumhQhB#(49Cd<s0yh6TF5XCbJv>B(HGOTTNM{cbkk@m
zhBhEgyb^kvGi>63gv`IeLmcz3NQe%ybIGuUX+FU^Lm$?EL>p-l+Hper3qlQ;9eT%K
zyHT)8FWp|CejM*4m3RLewiO>unLKpp%lBx|!8r14vZ?&zk95tN#95O?>Qjs9fNnA6
zsOeG6paqzVIz(PhfJDU2bQTwk+Q@e<O%?7}MMY@JXLevHM{kG|2+OIwfQpy4K6lh*
z?1fX$({R+x(pG$tWA-5cgMw6pt7T6v>vufyd2O9?JxQJcz37^fFK9;N&nFax>{J^2
z+}`9x|0)m-(}HlhCSZuen_IaO^Xrp&Wc9mBP%^YmNU46gDHgd5!?8rh$^poKWHO71
zpnJr*4;`GWc<cVlrY7P4()hZo3S5RoxvN9pIIo#`+@6VV`dMXp=+rnbw)#n#mp>hN
zW;~+5#tkOi_=d4GQL3n(_mo4rEVs1vGEytSst#`@jjPcARRmg=;oWM`nsjTMRRq*8
z^Y`vlk?DA^{UgZbLND0k?3Oit#8`^pT+f4@tX2rvf*QG7Ue4aZS-S#y5)ub<xE)fs
ze?G%*Sw0uPjqZF4s=PQ62ew`23s2l44@ImI(mTsWvbqK7J2+tjxuky%+$O{&UFim;
zrO%PH^G95!_rA?ML$itn{^g@o7e0X|dJM|5tj*OVQj~YRBM-$WYSC6vqj=oJ(<b0y
zl~+lnH>pV@F5KPR;U8MoGx^etq}QGE=wy0YgP3SFsRn!?SkH%j46OE_Z``K`;~J^_
zniE)}r5m5T=&}CuQ4~BKzM)JR${?3%{qd1mc9yVyEX&Xs11g$Ip|kMj?1g&nsBz3U
zj~y@X(H3mrlg?`LZT8$8-TG78<!jE86<63y-3epx41c9P)Ffz#*;vf^K?Q7=hmaBe
z++~)s_z$Wvwkq1JwC<^T?JF>R5CohI>J`*90(uRU3zXjGr#HUNfxA@>6<w|GPNmVT
zPv}$u?9hvIk~^>c<nWa(Uw1$@?Lf0}YxJ_#@9XCenD`(w%Il0%-iTmnQc@AjD=G7!
z$zpW#RWB#Wx&Z+QQLPpyxE5!nnc8BLEQaoK?2KuT0-DQDSiln01;m#oJ*_{g#6R1(
z8}fFV%nehkelf0XViZSm4PX*E>o;-kh&>!bJjL9xh<k8!+&(iR&!+>LC<=GTGbr47
z#k3+wYaVp+gn2|ew>A^`7@?k4X&{*)Cw(T%n_Pk<W-k>93KR=}c}h8KUo`WLl>~Kb
zDz~zfM=Lcjx}fXY6khh>;^AjTUbz3vdzzzY#uuax4KVc<9mjtTHQ2w^IzA|Er+So+
zL)?L-vL(A)4;6S2%b(GVkCm*_y}7L&4#W8(Em=?8R`f&b1%d)oSt@t*@u$<AEU#eE
zh?hxF>-R^z9B*YMxY8DQBReFH%ytXCB<Z?0{;er4q--lL3_55Ls*y8&a|GiIfaRew
zafYzsCYN$6Q4nu+xMEF}DfB;jxr7_qyYPkvRsOCxrX&xUw|g870Hba(WqsT*M?6{?
zcVQIEg&JbTrSK1AwuS~XKG}cJ|D;-Bp=r<)+dbN~HGZtIgwK$NTvDqXKizFSb*fV{
ztpF)D-IHo|laK*y1NpiWtUE@*ABmYrA|Y0raTDm8(wRtf4y!87r}T{8VaY@$7B7ba
zF2Ud?PIt7;kfd8R3W{wEJ=4o*{zBUfAfj024#nSfSMi2QGT4p%pksN)^pzL@rK>h1
zII3dv-q4^+man!CfTncwnc&4ZFQoyMUcLdnjf^iWXeXI|YL`w1iLeog`2Z+tOywR6
zU-ah%M9C%Opq*A=siqoYNgBE_D2s+UQIcsKMadBm!rHi$oi68*^s4oU$)(^Q&MtaW
ziJ*9EOm`k=F5|L&QOEtU^X}^4?HNbBa{@qC9<Tr_69Z>xQC_Hp8eIU}I8t&Slyve`
zC1rB15<_{>Ac}`3h9WBz&<yRh0+>2lG_&T8L?n)aFzJv4UjF^Oqg^4lgU0g<Z}<Nl
z*tQ}B0SMBhK?)9m<N?A6IJI)f$<L3kg8)OG%edrIXv)f&PsBEM3K<hF7h`u*`-HCi
z#X5)6YB}A1wbs$;i)lG)iiqwKw3|OS|L2O%@6`!F%CIhNJ@#&#)&kQQifT)x-WO^$
z)+2%J<Fgd!6aa7!FmQVSb}||qi)nYhtoHXwM{`kJ-rdvH8Y)|k@!&{P0o6ULOA@;7
zGw+_|<D0xAiho5@b9vAHte31wTLjBximodvd;?^jrKmf5y5MI(MM<hcz)q=>QgikZ
zCu`soAn9{u!JPHY>@aVRL$YDx*gUUmG`KFJcMx_+H3ld$SU<r8OL-l%P;!#ggOa;W
zKU;c_fi_?4{`p`pM(1RIj8&ME0-vTFJ5c?Oi4>?RpXeEv(4#hr3I_cyw3>m|ghlGu
z@jxF~FND;Qj$e!e{$BH8<`F9TEyn-iNqHj*9<--qv7-n7tr&%9e9<H`Jd#uEGA$o;
zW&^jpD~Nr@f^%%$sk_ZnXW-_#OZvhiuK91;d9>w$-O>EO<iZi|pQlzsrUXYZj|KOC
zc>{{TAE@awtw!_|zHvV)w4JVDdCM?`Xq|F&6(3JyCOC0_8rQw;x>8hRZ?|S1baQ4m
zZ)x0xjN09@jAGtoF`ynDa%7b9pA5#-?b-=cuA-Jr=@9cZzXqJ^;K+G~3bBEfe>ZPd
ze?O);-S3Y6>1W{Q#n@@FuE90<@O#XH!*bN^&Bj>TxJb3c&~SM0%Ol;9BBOM(z4B5&
zl@-re^o%vO$xdgDNYg=|6<YwRd&EGR#H5LOYR2HMaR}+0_~^k`0`@R>q@X>yiW+a%
zl{KQ)7LfK(-vqKE@~gD5=CEg3Cn&We?UxDCJmYX!i>)f!J98#coYcobXfXFSyVk&+
z>#m)*y)rp?u!I@yhBwL4eSJtW@VU&T14%T1_<+XJ01o+hk7}m%28^`ct>xFdoi=O@
z!vhwaF};`>SLqM|7Ln{TT<Y2sK|y=3)5>k+9hx@D7r=7Vm@B6E&l1^zfCe|7z=6}?
z9o0()$ySj0@^gpFt_$Oz!D?sWZ$V~Xfz?gw;Z?wbj>x)t`l7s;0QcyyaDvhKaY=yd
zPi@%^?umaQl*drqnWgMpvHF~{>l~8u8q7Rpx3afzQ1?>e3db;&V@LEe<$Q7xC1@>P
zNz6Z;j|K!hI{VK=e+AWxU=T+447|j22<^X!C~SOt`|GB1mv~Io#4@$?#o0Pw;^>pX
z=tXjgF%CfGOq?#>aLnabb$pH_PFId2+kr(~@5v=iiU1x*9XV|sTLh6)?*wmB4e@!x
zw7g_s@pqa@_UCvnzJ51oFU^x>x|bbXSgO<g!y<LblLjvuFx#Jh?sZu-q^9fZQeg8X
zQfdWP`<eDEYO43ZxsEOaww--GPt%glSt#W8@6i4pFgIA<90Fgcc0e~lC?w<-I(cQ<
zeR7sioae|nMKtogGb!fbAVj}*bdLhTf9kGr`d7@rP5!7*WVgFgwc%nh`=td4`lTnq
zCW5+#e2YE9k9U*ifG+uD5<Na&3$_O5qv#y{BF{`oXK`oXM2986NSJWd#pSFnNbbwr
zMo?Gsn%-u4venY;Vw@?06Sreg4!Y6Ui3h7mvJSR-W04$cnY578HZm^jGE}ecGpmQI
z^Rfvxj-+`pPB~N0aD(()?l-}UT9Z-VqT0&xOaP<Pw?6b}pt`e-rKtSzPeSd_ke;JX
zpgo`Q(N)=GL<AQD)WuqwU71rSvUfH$jtAT;q$dAzl4|Ci)X%2|{(^75^f;?!Y^|QB
zyrDh(RPZSxT>%B9$d=xz3d)mm$ovw@lzP644|MS5Q`Q~b*g?~o6B7?7Bu~0LnPkb1
zKiQx4R(24bZSV9Kje;8oo<KgaK&x5B(THN!==h6xMJwUj`97o%x3Fr|^3>2ly7#U>
zS8pU4n<}oXtr@Emhep_jAruG07P@K)60t_pv^?;K)X8(UeXW%UXc6Zbe?7E6!YK|s
zRHEpDpx@5s^bQiU4A%oj_8;m?{&kekn2T$^>rmr7^)E$Uxm<WOg!bvR+N>KFX<hE8
ztRC}Q3^1l!cBShVL4bR{^VjEgF)Q07_2H)ke5trbnfjqsyg@A<F7SFg4=*uVM6UC|
z^ZIx&y^a~-$x&eICm$`LVnfv=T%gO;<SeovV3LtX|Dtt66@I4DX_(C&UZn2?9EDac
z@a!a9N>sgty?b&DOUBYy+%Yj;<3$ap3%-1Z-J}^d!2I>{INw^{RiE4i(*4{%ld<4g
z@t53k!|Tjio23R`DHeUsR^aLJ&Fv<it}fzqz~-cDxV8vR%WQWf%A{_b-LEp|So~3U
z;;mWZ9X5V@K`|M6ZlS&kHTTj5Xtv5>`mr)ckxF73Eg&lvpO<N9SY|a44*QjJ<QK_r
zhu}lj2S;)sD6GclAgW>!!UFIusXn0tvo&zNVc#@iIx8g<pX<|M>*V{0A~CjTq>}vV
zi4nuM_+{-CkdSMonD81{22r9D75!W~?*YaTgb`APs*`F?7f%!;<yRB&nmj;=?0Gsl
z`bN}_WuB&<4g71@7B{Yy)@homr%!~$bc`$vb3leXh!u9v7<UDR@$3UKcgkhjRvs{3
zlK-TLwh`Kkp(WI*#pBW&dZ3@hOq>CF@82=txpnoUc|=X99TFI#z^LnxW~xq;%xx;f
zVwNS9$LB(xksj%G5Ph(6aN;<RGVxQ?PA-Tl3pm(T=dlfKVsZQ96eKd4-xw&~e28Pv
zz=sbr6J*s3Y|M!D!MCR$$%l@=N9RU*^8wR`0QD{u&1S+F{$hM^e*jBD)caZ8mqydC
z{_057CJq&ax2zvh30}Pu*m8x0HZ%%^RTfHV&NX2bKTAu7wZ=@hej0*>Gc8@|yyf~6
z3zO~DAgf$7%0)uq+Ig6;x2L}?#Uk2NYIf#B4-*bIT<=(9J)|y!&vSrQ9@S|?!X>T)
zFB?jmnl8_A<jf$Afgv}hbgxW}TV9BHe95`sp?_NMol#1HE)(V8JL=n~yz0NiP@PVD
z1!y&8;b^iQtY@km2cnT`ZfS1%e4bDjqIAsC+S@}`Sp+&wnjC9X_Rj>UO5bni+MJY)
z+IeveL=8AN^({4&ACX)UKXaK?LY8PkP?Vr-qqsv61(--WRBP~@ljH<7F-US+9z!db
z0^A^}Z;QMJ6wHL&W`&iTuzZ~XYsB{ee(T4cL@i#PSN6;(5x=EtPP8Cb5wVpF7t1ij
zhwPKn9mN`DD76Q3;k@5kHc^~WA6O!RP!<y2Mk^O1!;X5aW_RMwo^(Ht%~$u@J>mh-
zN5t}^i+TlRe}F5*rz};y%W_*c0rgFc?(cRLzRfftsPelA*Wqtv5HRqpHN75(pvkZp
zosqLP5~d%+CjnRoj|TR$IarD3wo1<yjxuC6k_&dMuq}T(Esola07G@^ZZqs!3GKNn
z4Fn*(%R^~fc4{0)C3Nq25G$c_X$&0$+@5Z``C#M@)A+zb)X6}QPjYG8v1GSXk3OK1
zgqiFqx>q=*p1G4z;e%|tc~YMB+D`uU%<s(^8|l^<zxA{AZg$P9b=hHH#xaITrbb&{
z@Uh%MpQC3dH6c`fE|@4OE#9JwGX=;D_rLhN^6c`mQ3XH72x<s_3RMoD6`Y&W?P8O!
zWwl)Z>%=!-2)V(HpRo%a<ZTOU$(^dF_xQ}aqIFLrXy$IP%I&IsSXy_Ztk}mSH$o;^
zP6tscrK5Q}N|*Cj=y@9W7^AldQ7swHN-2gtea6}@In^=j_(#J&Ys8FfG|FGYf6qZS
z9y0T=zTY>O2yD$OkEN!)vF#t8UVO6uMX9fIim8C-<xuq(HnP;aYeA@HfzOz*Qn9t5
zZjBYNBq29<FCXAFNtE81PQ`ypHJI_<;5X85niv5XQ2X$-lQlt6AXVkR@p&NRcyaxy
zwX!8Y=eX~F|J~QrOsFwxatPv2-dL)H8}nQhZN`^9bIlYkA%-fdZ6x8!^&S(`$3$C{
zC*(|*>C|~}gI%ei@FNW~xbRKoI)iwn4R9C*T9$2e{G*<`K~RT>$~r6oGh58BuCbzB
z*z4^tq*01vg(*R4UqExU;sDVKKYMRcbVBXNd9&L)cYwu7en=K3riP+4C)bwKbHR#)
z-O<z2f0RHZQ58wdnz}oCjfLXfVWK-Xf!dO-5*t?OX5@|fM{`G}iR48kT59~v>NDiz
z=f?>JQGn_@aTJd8NE??Psw>@sd5nJ1X7!FDKx>fOm{Q{xzqD+5410tqsD$3|3I(we
zFXVGruqeRc!X0&_rc|l9srxTfR*K^|zr%#=*wmYB$H?mO{oW*)Et*e>P%Wxk@7k=(
zI<DJ-p)FK2g@B-$-Tu6^Dx~+P3$v*^(?aUA5mA~$JL&<PZCmY-?$p^vJKv4Bmyklm
z<32vc^I$pmpy_B=%T7BA{>8?|IBJnk&vyq^^QmQau^h{Ry?dM<0d)8VqV}by9t)mt
z1gjeU`Vg`jmZck<c@6dKyH&%=jA4EC(Ia*XZXbknBSNRPw~U*)^Nhr;qWGracbrtT
z=X0)+)@2=FD&h%u76#qH6st(%+wATJD?r`gnW9tU*cw31I1qid9vFEd>&zNm`d+04
z(Dp(ictU>h>rrLHt!fiuJq@3*m)5f>09vEUtix~-E<GQUhmgF1bf8-@ec$;*wrU>_
zC{qY4;uL-Jk!o;Y4DZ@~imXcB^uC3}x{#>kV%TZIBU)?jnc=`xQc?{yv;-#-TYWvR
zmW?YMh59c%RdTae_C+~-%r?LNO5Hg|-HzrSs3L%`lK<~x*?KO_50yr6mH%z8_B^|R
z7YlFiX5>mzyuQS(KQUBOs;MR)bsMQp-1a15_28K`7dP#@eJdDZF2QKZP#ObEyFn{5
z)h3N(P=cd+IqI0iPoi4?Rd}If@_R+=(0;$T01RZ9(KmU_Xvos{%K)oY%?i}{(h>$8
zTWr{Xm{W3!Mz;<pa+2SAuz4eI*}Ejl4Q78i)Y*QvbF?jjqtLq8c?B1r!?lBy>nupV
zoQ%%idq9=xb*dUHD@rGmnd7+z6;KhxE_YdBVh6q_%u@1>O@L3{(dilyySng1DSIoJ
z@V~?)%@Pk60>W4_2%Y8dGp2F#%1v=_hEhFZpXXgeK*aK(jryQR>|88PBEieadIbkz
zi0KYgq#f84WAm`|hNfJ)wiTGf$%fpQ8m<m&@Hq&YL5@BLC71}&(mh<9*HBuIldzv;
z2xU>Ad?ANh=wW7<wNulqiEu+0>A1|V|Hj%o25Az8YntUQ+h&(-+je!?wr$(hW!tuG
z+xAzsw$Dz??3tJ|F&nY_@6A6Mk(m*h`99YTd@QqYq}&gW(+EfLkHSZS-QgOsRuZ8X
z^{FUrbAZl}=pC6ebME=JBMMVgW<jRN+qqxB8FN-|?<l_f!o+BWPPK07-d}9=O$kNx
zA-za`GBZIEHoI_4=zjws=~1fbO+g(H5vQtdk3I6Z7*cWh#Snc8ec84;kB#RcP3d@v
zE$O(;w}lgu-wRQdGo}Zi=MOaK#TPKrzNCif7qybCF}-w|Z#ietg3J>ZSH)<_$n!C(
zI?srhGa7w-tX%=6Y&JzZ`oje%Q$ik%|9Voi3{|2|clI8$<=WpcP)!olri*6N0inl5
zfH!qg_-o?3BVgwhAr_@y*jQYq(q}p{3$lPF0?D7K4RAnMFW@5!P4#hS9NXA-vzamH
zx{60T!DaTNz%y%4fBD7;6J7%~S$h@2*!NWJ27&9hqTP%^&eI+^6l*Rc*cFY?0WB*X
z;->w$(l}*_2U_Fk<00FJk2aXzAR}kOfR8qH!jUBvw*j0Nx3sOLZd3pZUTJw87&e+I
z4T;d)h4yk8glakkY@nmVhjxfG-tew0m&SR`>jq?W92$zPa_oR~nPfscYc{=~XvCOL
zS8+opzOWBfzImE|G+f$j9`^*p{jzduL}u$mnBkguBsEnF`)`>_fM|tf?Y$w#jm~=d
zFRG(N3yMQcyoGWXWCZzYpjwR+cPnq#rb+12B>1CS6edS?6~G6Gq$216(R6n{Y!!DK
zjzihgsL{%pWhb7c*vZCKZb$_m0@NRs+ufMI_TEWL{TlYtw;V_D4{40gy?If_h|~aP
zZbPvmS(ZMYLXJ)_@)6VM5|+jEb`Ddg$}o+Pay~no5w_BPR3ah@ao>$;baPz%9vSP1
z{4t^`pD~gA1cfp145!=1@1;wVzUaaHyuX#gpX0vxDQjC#o3*;PD|`=;!+-1XM$WJD
zvaTenrKKL`=ts=FruQ>HG9eld>CWx7_KwH_0x=apv{Y#M99XjN%4!B5EKO#6JvKEz
zwCnw_Nitm-tH*r6AniC4{sTFyS`rDdQ(cb%rdj(gu%y4=fkB@Wi2JxO*5~kr<j<OZ
zR{<V^vL6}H(0ar}a~0eECD0lJhY9>7FuLw;un|H3G`6s4Mg8~&Eakj$86`?&{KDNS
zKzix?)P2~9w6YGL^}R(Brp|>L#jjE&3&=IXnZYt{<*{ppI&$<?EeKRS6omWgcNJtM
zbiNEk$dyy3v#UB;xj;VhwYMNk#kQGL-x;(y8osKcw7X6n!I{;zqb)sWw4-*8ahwk7
z*^3ayahjwu`gICZ4IOEh2<|8;aY|<KFr1z9PE&sWIK6-}Oc-u5DtAS=*URcR=Z?S7
zElQd8>~0S-1;d_R3B$<fv+|~<SyVE6gOLOcT?I8Gg<raxtAZyJAc!9xU!lzL>wDz1
z<4CN@wewM+)3mA|M+Wb)F{uO9F+nrFtq}u8g}eTE73|O8p-en_*e6i;%rMM!{C3pe
zAbn||zwYv;{OZc;1JB~7U6xllu6iWjU7*%7wNmHr1t?lDGg<9M*{Wua%ZU44m%Bpz
z^9`d73&x9p(dASrk-cpEZDpvxHhb9Tq-HvHoP)OussyNe%2vlquW!YAIc70W0k2!S
z>;qfSW<~W_WB{{Ve2}@6mx8;@(JUPStQXN41Ue|CgMl!2O5x6KFfz}&j&9^@jOf*A
z!3&<ZW_L&$eBIL0-A=F3)1<&)9r6rNZ^K;Gor&?N2VRAzWAwRu42KNpltTmY^GmK|
ziNDtqt>^717Ng2$HyC;3Gh-;7_QJ;$EU-0IC>Z@o#ETxYyZn}8+NKdRYHgflK2cPc
zE0E~`n7J9GKeyVLWGpxRhe6T14!;v%dP|RExy5QZgS<6{VA?)?fiZ3nH#^mevV@t!
z7aINBagZVA{|>CpypukzJLdg#@5f%U@rH#^r1V&#ebmZ<xuD2JUqw#37|<$SVEYCv
zx1DEis~26o?;B*B%mCP5bZdX{L#~m5t*UodR*2&^nzs#US@vg7>7f`ax|QE^3`LFi
zRbvWTPV*S&bCtc*QnD0af)F7p#8m3_i;5<0rQ=0`F6vk3zVU%4w>My2%;7*}u*`nz
za$SuxG8SChNow9Wew(t4mT1_^@vqJ<O*>^t`wi89Z5GaaPs-02VgG=*52sk%L;raI
z3x&5Pt$RrOnqyHj?yKtvzW<G8eope$XdX>fKYc_vONVxGbXodv`thdMH><jup{@x@
zKp?ZYMMd=$UuT`M-ted)E0jW6u;Awww=1x=Ec)xtSNSI4!2FN7zJBLr9@E)0JC?c+
z+RAB80#UhzOK>p;TNZ7TBhwJfn+RdDWSHr(QB|Zz_XHAEy-UGH>M~n^)GFzyi}zt_
z1CRdClQ6NeH}9ZKq}mxMfTX{6r~MC+mFS<`FKww)e51d@Etjh_GMN-h5`{1egSlcP
z5oR>V%5SclO$N916Z;Q^uB*~bvEz_D1K|mpQ?PKfO>KG(zcug*5f~kHX5sInj-L`d
zV!BeWYbb0OQ0C-9)VomcSqY?^`#`zVGa*VhI--RhJmjqqxl7bXg$fT)e!m{E#cd?6
z>g|)kU5!*#b#`7CWbDBeGU9n*B%??UCnIRP;q=;)5k^pcNFH1+#AfQEIqVy(>xAwt
zMo6a{y!wwRQia_QaZ+*X*D*1AQ8pd-^vP;klFF`rnwT1<3O&RaVydPnX7>o^JM_h<
zDJ%k3&b@MVL)n-lg>$|OA{k#tHSB;VWe1C~*Di{uKpG#44KN>4JyxmNvAvVArn)^5
zW+iL(*bQ70+fr#}EOyaodyu_$lkiGRi@NC14gm2A+4mABHU{suu9;vg2ny%V!8wF_
z@e-rgpA06w<mQsy5uJUmtL~*yr4b~=lxG8PnuLw)R5(D&UWt5$+2XV8n>h?G=RBYO
z&2d9cf^dQSGQM}p?&~5yPE0Ql&PMKtjirO0{WWoJ^V1)N#REB_24V_Dl#}1S+xJUO
zN~yK#o3g%xSjk_=YZd?Afvy7dlJgGD2Vh3M^WA{ak^qwdavu(}K01*8&p!6&e8jM!
zywBew_nJ)=GX#2Px`zd%Y&~Q#tuS%{OFrf`>iUJjeQyAm^?qfeJ0IyULcMx3QA*Pl
zE1h2qO<(}gkDx~%WDs7o-4+puHxFGy@hByq_vx%V*&6B%)&^fB>VdroXD!sLSTOOO
zk%)?m=x^gU{G5LBo2e$oWvgzEm2Up|3X-Ty?(Ql5`}$eOkBX*3Bc`^&Mc`O+Cm`!N
zbesuwTR7^Cc{C>ys%FB*j=Sal_3)`HF^lG;I(%q{BKAHHf+_0Le@$}XTBcvy<Yfp&
z@2hN88`Xp<N{G|AuEWMlUq+TbMAN=ps<S;{lf;ORiiO6OCDi!&bhsX0QAEOY>zaJ~
z>-CF)Qv7Im_o5#~WI+mUB7mX7vgNrQ*N_oXLm3hO*$-}x-a4#YB^L}e;0qX!CrLfp
z<DTR({~7G!g)_ChmpcmSfxSm!TcEcCL^0^#I(et5@JD-UC5FgARuObD<%ut|Y$Rwg
z;(KEY@0QpDO4^lIJ9iOQrdnW;2boIWCPQDU(>Cg}K_X>AaOt!bHyAo>$<^30HDc_S
zIID5`21cYpoC#0cDdPpyERq1VT8PHg#lWR(X1!c1xUT!LwDH~gD2dNA5FJao8N>3q
zyaGcxJwet4r8OioXw(h?&<~trCzVjz)0G^mp5-kR@~P%zII=wSOY1-%gsS+0%ElHX
zIu=o>i>~~3!@rv;ZmAFZNk&CR<{0Ua9v-~P(w7Z&Np)dIcfQDudI>S#Z`7+kO3^3E
z$yoKbjQf&Co!m(T2?8jIjI`LJG$L++bk?cybRA<;&U$;}FF4=j)WlCiy5jk26eI~6
zG4ft8N3HJ1wNY;`Ws*mn8szDBrqvY6IOw~NBicJ~y~?!-;(K?+oL~@XUL-E;n>dc1
zK0Domvi;fG(bUp(zCH*KfRivM>tuw`_~P;3{AWhjaz)~CX!Jj`TP%e_z?3L~f%Nm2
zh1A-gO^w=a*N}9a@5n4ls`3v>O+gRKp=fAD5@Rgltr+%f$|k?W%SJhRZn9BcBA+P(
zfB~DERDZ*UF(384otwD1+YklKr0GpqZTle<Gc(J4+P>^lkv&NoYi;f>6tPxcb!ti<
z+FsY3s7+Jl0uz^GOw~w@cA5*%l`5GM61J7_cW1)ud8lkb9XZShKo5{JtYlt__gU|a
zD%h3}Y2~j#=0{Wt4WgB3P%Z|#*{Kc5UcBl+CW6gN6_3tocq?6;WJ#Vu`6Cq27ZQay
z$R8}{W1Mf5*b*04)WeC`(qwg#x#AE4kBsBs0d^kPZjAO=!F(O70wM6S7_vMip_A_|
z9QpasqC8Y*POZ$94y{=F#n;T&JOOqKd{Xc?#Ee#UL&>Jwxzh^z>{>>d@4eyO)m^k{
zSFWl74*45MhuDP~`<d@CBQCom1L9l<Z2lrO8y{TogI7O56ja|Z|AnxhjqyJz-v3X+
zeg!#YVIkpv!hR*jf4cYoqF;}eh2g&o`x!X@zX<#PonG15#72#f?Vp3-e<SM`rdOjk
zq&Ky2HKDhlx2CtDx23nUHKDgRa5S;CHZgVnZ!1R&GxL8-YZoVaCwgakXE!@~7kW<<
zN4x*Y3h;jm|Nmzz0MmcEG5^B~z{2#e?(e@H{VV5VX8mV6_+Jwr*_j#tn-PHU|AiC4
z4O|5Uu*#nevOvUW=o}58JBJ_(W`vN@4`S4JCfO!}L`Gg9QX)2(%O);|T*%K09OC|3
zx%v3`nf2IhbZR+0apHgR1vt(4UyYawRmee#m!X5iA|s<gi~}#h$rJ+-#<wv*l8S+V
zWeh3+=J=aP1*9n!a6<hjzru9zpuh_mxZoI|4^933L7jcJfC~<Q6e&3sEjSS&2_Znn
zzmLNOCV*M`dKk$40~iD_fWthKfR>SRz9s%fQA0aT>WKr<2O|(tQB%{ta^vP*0E-R~
z>T~n~MLULg>e~kUZv&e`Lc&Jg|ENRjf~%uWD9ecq&CbeWUhUIH3v^K2L4kbgQ>gn>
zF5yHzhHwG<_Jd;v^5*phKqUbIpVTni@GqG{nk5rpLV@d}0vI4s0Wnaw0o;NKKvUa5
z%y>Tj3FN5XVU6De@PYq=&p;8;(7(91bLV@J`}cZ-`Vk=q22o&N!uoZ9W*FQk`Xr?F
zFhj(kK>F-U^<wqi?7)8GU%`a72jMZo_rm-FZBmg1(ueKgpZ5@sXOK^Btz)11!VpT(
zvtW>23Fa;*;o-@%Pi7yleP_{^pa5HR@k`h>atR{&+zar_$~LekKh;Cb^5zkx*<+w5
zD2pA*#m7V&=jXzpfF^_j2`Dl#23DX4LWOfcdrR5c+C_Qaq<EK(Dd*>yL)nM29})0}
z7|hE5OOPmFS4WTPFZ3=D1p2dhYZoD+g1QG5lm`^6fPtBHlzWBAy7Y#C#q2=}3s(nh
z5Kaya^!5Jf(=cd12?gx-@-zDNVlUbhqeANf3+@a0y;)QeDgX)+G9&;*#3+d<prWFN
zOBllm_QO57`}#BRtz`up`t=9rEH|3V-V99O83FR|&KLOLi@e<JcRQW8zu&HYc;781
z^xjw4a|@v)BG?1jyWj7hf8Bfpr~kV7^vr*DpUmIdI)7U3znOmc)#`ptZU4w~hYs^m
zOo?F<GI#<1*r`*W5lCADxdgtsf3~Xr0w1vsrP><)y7@iO)vRw`mc$4P?(h{P;5QAF
zw1Wo`hE39C(3^whA5Q`KsK+){Be=yNAHi;_)GMnnMfDG8%W{D2KU$p*kro5#lOkWq
z2V(JvC4zxP8m2BnMSf3P`G+&$1`GrPG28|M&p?un*Lp-6zETFh7Jn$Cf`+?)hf#nd
z3VzW_jRA>(<c;xf{is3={fZc>I<d>pxBQg;{(=|CH>U_THh>3Ci0jxzU4APRd`iuo
z8~J;{Vn296l&i1@a$h{l$!UPD=i@s>b)RXuvR9cYeVNf!N_DeN2kN$&2}R&+{I}~B
z%h@2;l&k$N@Ms%MXV?9_=AyBcW@t+2gep@oO;<2=-?r^~ZM&n7n4SR3@euifz}K#6
zes)KVHg;&r=CAdN^m&4=iYye_AJUCb^(mkzJQ!0f4oC$d8j(*Gar4OZMY~0nki%8$
z4hs;{elN)blYkW~!*R1l;7QgK!yVpe?dTO)nzm9ElBBHwORe>e$A<v<NwowL&PHc?
zaejB>$PT+Z2y4uYZvk7s_E9+8v_lhP#Qx%WNeS)iwrqf6e~uiXqJ2|QXXMHo@iyR$
z);t-$lg%;BcAzqy&TQAY$bc`ev$eB>bDigRQ(?IXoo+zl>Tem9wRkuFR4>`j!l^Nu
zOVjaR%)TVYs}0$rhm|k{D~@v5WzhfJI<ibKI=p>9e<W`Q5%z8d2!o1md@xv7OVTAT
zTsr$vR4Q0L{k!~vjHTzSY|I#Yc#{UGn~6e0+eU&GGzaB8%L=#0WF<Cz*Pjd58C-&T
zUBAn}50fURy8fyN|4w&G6ZbOw4c70tSu1U43z1zJrHz4x#umr$m4zka)XSKD_gnJI
z{LfTAInAG4ac}2z!)x&D5llWL{^2T5zhctM$?+|?n{>%92^cYUh`7yV_NFfM>Flto
z_7=HV%hEu{G`7tlu{praWi<ZlRAJ|WPE)H$U>(%aa~s@#zPNTrP{!}fuy2f9bz-ub
z-kE>%^Y;gDLo`234<CXSnl5+7=_NhiUv?U7q`p4ZOGqLYjeqtiYE{Vwt*Trw$jx)a
z24VjKYTeNeLCn~pT4#J2?rt4~Q;@DKtfyM1H%iM}!HxKWit*$cli04S(PGPWpku2$
z>D|P|%QGtRg`m`6V1$LuU`V<e3`K<?0S{~Dk#s1#*pE&~xO0_olRQM??o*UlV=R;G
zgw()y7jEYEe>63e2{e>C<`>$!P>>@kIM*3f(+#yC9r;~{ln^k^aS_7J=crAaM=L7^
z1EB7wB4RUfPo;Jz5pU+Ke;9RK+AuQuh$llfjlM!WSH^c|@P;#@yZ(rjYmCLGu@@yv
z$QEKi;HHD@qWV~P^+gqEl%lK{!%5U1#2iO6FyKmBC3q4H@YhoEuln%j?~H0FA@q>H
zZ|gM}wr7quZ||lT&1c@9cp0SM-a(9a;6p|$WlB4)fjgI(0enL)Js4{n@{C8_uRk4t
zM=dX$mI|6ABU_GbPc-4W9!<yW^9%64TU;uHiA}}@#bAGa%Z0O?qmLRz5*H-DM8r?K
z?S5$p*bH-^-KC)ixWViV2tS)=+a4BdM0Otkc9nsjU8K5uhlO1yhh#dF^WxvS+f3!&
z@HV9)2&$O&8ja5-(mzdHsS~#06=uft5p|8YeXg8XW2%QdowQ4lEW-gB^;7Bo1$a>v
zdLiq^>cN}0Kp@bB?1!F}g*4?c?$m{rWe`4wv%naVUfue;aA_PLi+1TqSElOs2@kPQ
z@`U)L7#m)cn-YFF)3W*X`EAg>c-E}8mRLFp&uWkfuF4x?r8V!YY(P(vrk8HhU4xPz
zEfDu7p~O&hHoMKr{j$yk+ul>rqr$gF!D~#6nCY&0gk$&MTLoHB^H#1%Xu+w6H}BM?
z0%8v4oGs!N*(EG3ld!t77d#L{KC{B!u4IrxNmu$;Yg5~@3hztfPCH`k!}-_DVo+AG
zKjQ#K^>7kOm9+oob~nuGlZaga>o@Gb6xGjv>5R~u>}g$7-I3UAa^6%J)B-#zYZhz-
zr;8zv96=$ts>95Z!ZoF^1;06Mn^9|)GwsR(|6U5~-Ty%lfKM|&<wijbQf?e6_5|!l
zzoy&^_?Twh*{vpdG!%x&dqQ${t<RoJ<#_g3-Lno6V~^dXjYGddOnH40dIrXh`0g5s
z377QJqPK4|yiT$9&rXc?xcR7Eq`>J&cNoG*HkkC{J#1gFF`DR;OWKTX;`O*A@h(n1
z&k>d#^dC_8qS-oRk+k?MxxkEBQ(IDKHIVRWYCr0<juLt*Q+|wG4Vo4PI!SGW;;(RD
zv0Wm;+(h0c;>|$#+-3EGf%`G=%9Ew;N#v10?iZ5Y=5N1r*H?_2C}wY(WZrH``Lf?J
zE+*rbJ%=vA)U-E5P6sb(8nGX@7Jk!z)*6u&aF?Oo_R`kk!j*baR#-U!$DTEAs$`*i
z;@oKo_rO;~W{ioHL4#M1a7J>0EwSy4g0PgD(x>WaZ0^t?0mSjub||3VmTtDQC-1f#
z!ggks_}SUha5(dTJI*U|&DbVel~D*y>dIdk!`>r<O1H<%sXr<Z7X1zc7z1c(G8f7F
z;h-O^=ofln|8RO+ab<eQ@=$CPjX5P}&~vn2X}-Nn7isBD4AgWqn)<dKwWsQQiUj(x
zZLx-X3`K<J#On(Mw{i!6TsXx!2&ZRUIXzi9YZTCouTQ``K*#S7x(Tie30Y4#kPvrw
zpZoZPhDp2kOtG|rqgW)DBo@jer-K(~FgtL7vAl2e5bHdlT|ugX7qPkMo%5xAb=9wB
zdkWxCJ%9CpbcT0oJSAnR=el-~rN1{|dui9u)-QfUoU^}1Sg75bIOAW&2wThFzeKOm
zK~ou)C<g^iH@fvK()|>r((14LJ=n$%-IJNbN9TVGonaQkc)iZ#i%-xswZpa&DDo(6
zW#%YgvI{Ot!A!+&{5s?9@EJAHPmWi<#<Gu2WtBalDPe(mu4icEb-FnkD7H?J;bvx*
zrBZ&sC^3%>q#V^oj$`gplc3qfomk!IeG^%(pr_E*-v7|)AN@mXh8>&c>*A1qXka*>
zXEhmA?4e)iqJv*f45K`qG9%#ZSHIc*KKz|8BODx=D9KQ1)TCAqw*&vQI^kc-lOwui
zN?-p<o6)Y3+dXOO6PSVv?8=b6q?qM%)pV0-G5b7|>M~!Eci*|8IcibJ`y@S{8rf=}
zxpk-tphxFUqjl477AIJQ)msww9_dIB?@{66bWP=wSHCp0o`#3#a8A60Wb+vphUt!g
zTIdFNo}fhIZ0blu=o~^2#q5k4gc%4EZdwv<)Jl4$3O<N-G9vG)3ta4d<Pc1{b^kI^
z`DoEaJ@WpoN3iYTtK*F!E@W=!sov{x_z+`O_+%GruU`~4`Aai3CZLax1wbq7{ejc`
z_6d*jE>=tX+>`idcHy#Wi2&E&{wqYy<BZP(JlR;gMxKD5#!cy|SWUmHS8Ww-KA7G(
z<i%ajHbqDwLG2nKH=QExu)HzIOYutySyWeAQ5~7B&F{OV04mNyUebQr4!GI~fg^<P
zA=aw+lcB+XhE3EUg(WRr;E?N1=eQPFh)w2gV}>SOS#hlke)!XDl=w26OE$n)9Jww`
zOg}bv)h-d<rW|H<BOpMCA1rbfufimizih16wSf+v*A!cSxV_mCM(1@oGR)w5OcagV
zCI)7;b<_$ZTsS*lqEty1n&G*${-yHoTFn_WOmwZq%T>yRUiMZe@#-ig<5<t51nXWG
zDI;0LPR|ELQ#*67XQEYsc`e%&!wJ}pnw~<tikB_orkL%8K%^^ljGSEMAdVS(t=67C
znhH)2s~fQV2KBJK$bpF>bF4cT8r>9n99aZ$zCkmnPg+U^{bfBr8n`l)u8Q~mhYa~I
zBhrf?+;+{$b~n3KdC@I}xrwBleiWqmH_cret*%Wg%1;`8aDz+N(qzQQ?LUtl@bdl_
zlDia1$$IIUFf!e%i)Tm{BxoTk3U;wXnuE4Y3IQqKDL{8RR&(5s8g!F=@H?P5m+|f4
zQ<3uXI_)_V{5@PPQsX6|l|HMURAYJBG;n-*H_fDN+^xyqP8h>7&(Q48nSy2XB9hTF
zkq_nWaAyy63;vbpGTyQ!!l(Nm<_DAQ^KuxY%7Vxj5}Z54dGEHP69&b=RjSAKOV3Sx
zB?)`M#^)@Q@F}d#VwG(v#s>DVt%{AMc!-lNqq4;60UtanE4j!;%XJNTSnX0-f~^LO
zRPSJB4xUZMlJzo$s^c1SFSoK)e$YS`;mWRuub}6nhh|y{K#QA0Y@cplPFw;wb>EtZ
zYe+x$srAmsc|Y3fS*7%qS|M*A2_8<$z<XL2RY5SsEhS{nZ6hzRGyECN6hL$v)K(hc
zBfL<C6Q%+edFbN$gYU~Ga^pEf|5Cc;A_N!B&uVG_`E=hd79B2*V$09X8#Y9)Rfs<`
zlwz~%39seYS5F0HYt8MRr9Mm(Knd9k52|+y%^yt`#h<)gG#zUraXXH8OKB?=zDz&!
z3!YYO(9`qx&qH_lBH_W?c=zkE(~mVbGzcb_#?xrJGpO*q3!D8u$h-;^(q@GyuXC^3
zl!=r|iCas*2By6fL|<&Mn^caq1lFCkRVe&W+h~0wMqqpm5KsXFPBg)qXm|>0DdB?C
zb6(hMNK94o6?R0=j;(xPbUkP5w%Zd3zlqPDgJ3kV2+-!2z<CD7<)+edQ5a%GMYUpg
zICa%=$(pn8DQ^@eiM>02mIKwdTM<^}-=2By{(W2_Q^gD@L9v_t`0&;p@M|ryY?=K_
z`YLkJB`qVV(5+(rZK+T-tZb)?DM)j1EpbJ}<0Oh&d}~SNQx@8<7yRe44m}j^;dhSO
zJT7CZy{^N3JE+0TUIr9{yQJvXY(|-GEJcIXO2f4ymGvDm1<?h=^_*@kQJ<q@BUgv@
z7Pe{jUj;X3`)YiJlrGbZtbaQgM^xtpp30TxK&jCQy_d(Dgyih{F}-inK_K3C{8f`c
zsuo1}81=M#Ow7@<SWKgy&<v+lxAqM$@>;3cvHvsk&g$~ac0Q^bY{ry$dc~WQx(>(#
zN*|u=-zTdfp#3t!TZ%5auDOR9Lg88)CJNp+S}#U&tb&Xo07a*_!eXhMZQA%my3c!2
zH=q%pppH_G!XH7AlEEF!`ugK@3TMnRev;U6^(n(I#C|qAb*4%M%T~;3(sueBJV8AZ
z`8l|MYG+JPot0%GBiX`cF3y@!J2e9<_9qlMM<Kzxc4i%;ZorbI(ST{=4zX00`Pn{l
z6TPSkeT)SrQ~#{Hii2&JI@aN1-_LVVB_Jx<^>NV=^X^x@6$8@CQY%}>z^z-P$py3M
zE<6q&W^BUvf@G_`8mkX1)&;W!2k*2Vliih~tr|?;7n2<q=Z3C|q0l0#f)1yhxaf7Z
zf=Q2;Gm}k~A#RGa9Bwu3QZEQY3hNpCCS=+6D|Smixm-eqoBrHeOx>01wUN+cwfbr*
zPw=_PK-c09-08j5pkU=QnAk~3V;t1YmE1U2o#mnUcH16PizY?Lzb!70ZkCGk3fN3C
zHY4M2FD{E@TCXfH*gf@`?)r~8Ih@5u?;2SCywd>P5+Hodu$6_Fp}0Clz|l}%P~Di3
zqpsm=!9|S;hS}ZvXlzTDYfw}oN#{Mm=NH+7yzwff(Vc=BY+{x~+zWvR%W3`*WKDfn
zVr11ZWdIOXTX+G}=l2nF=x9<`bMS**s0aA@{Y1>b@TWv8T2W;4UOchlGu-mCzg8+r
zN$WrAbavFU=CMku4_FDxORM&3qY$s>GB@!;zs#iaMj7RoQB7oXYP?CA#Go<bKK@Yh
zUiCQ>SN)E<3Z+aWgQ5#ubTkBq@~Ho%DVd$zH_O+U0sqV8Lu1iW>rA2dMx<-<DTvWy
zYx?Nrz9&`;Rb-A7Gc{K;){d2qj}A{APl3b<WpBizm}!ie?~P&PL)BPDJi6Qamw~M;
zwQScaWv$P#iB`atZ|Q9y?yevhUgb0?V-i!&A=*WC-M%8Hpu%kQU7)z$Z6$t!M(#VS
zoP{cU)Wxbdt>Q_`!^+wArm=(wgOK%|-2PFfLETvT&E2FuoCAAQ6-s7Kxd-s?{_TsB
z_SGs_*ucRy5wW5V<i6!!+OJEk9&|fw6(|ak3aom~q(%GN5?vJM*>K1-cdFL>usgBi
zX3BBZ6SFUF@g35(Q@br5iGi5`+UE&GU<+Wxq=e}OnRsHRqn5N+auvZMN}bKFY65Ba
zW1$}NX`d+rsZ!)!R%AgM^*hgAp)bM+V_r6}W~vR5uhqGJM(fF^&MrC&N#9eHzx8T+
zwv>?X8Wp3b<g=-Y`8Z>YGOA}w*7XWC-2~#DmKYyZvs_fF$a-0TWZT~rZjwF^gO4I)
zehhZ&X#P;*@!`Q6(mgk^HxkRw=1e+g6EAqgNPo(@d1g|Q&`#xW#4A(OWzX0BVj96P
zPm_NX;sz%sX7^Yo$$2Yd>$)E>%Cg^O3VU5MPWm3};4l4KkS=b|qcukHEUW{<@!8DT
zR1%;j(+6eYIL9ceE<{4So2i!k&Ua0t@%56ckTRENWh=3?2^b`u&*cK;m=f#9TT3K>
zZyP6GLyL!a#)2CKTqME7@hJy0!W}Hm;3(OrzLfsvPa>RExkp1~(BMShe%m@c$1>~H
z&FxZ$;R?hG(}^)P!TT-dyn@RwVdF%3*?=A+n*b99zaD&7(n&s2OWonI9B^qB8|)=Q
z$#>kVwV#l5nyFQvq|c4Kl~V8`D9m?BU&a<lf&r*k&pAsnVjV7aLVcU#HT8N-FlOKv
zskhel5#pIBfgz-!a*F77);qo0+B(aI!itbQVx`?Yl=&gsVUpGS&dHV9X630wP$MBT
z8rRSn7b>I~bWFtqYnAg!I5?phKUoZ@T%{kji0MG5M}WD~N=!s^9W5wfeFh7suBlho
zwow0jkTLY9JdC&ujnBF-*e&+cmU*02CeFbHIl|&Lv{g5iT5T0yMZWQZ$P#+14H}}1
zs^jhW5SCnw87S+Yz+?-IMJ8pJv(%Swv%t77MS}RBHbX#@8esv!(f2qQHy2$IFOJ_3
z2grIah@EW_g||11)cq(MtcZ7a@zBO_E;t9sC<|kX?K5k}JIK+w6MDc-MTo#yK@XE?
zNiPIX7cx*rp`3xHW@xV+GL{fNja@9x`9eYjn}Fho%>rBfu5r57(GH@`mM`CYdhu*2
zMyCW#?u!i-%D{Nxtd;Lu0(sfj2CkE{W%1RA^|0D%XO<WTT<w9+Tp5w@!Y7e)7gL<0
z=VE5kjX<ROwRtsAp(ypErOGu#z5%+fb0S#}6_cShz{oBKx$gUR9h31sq9!TE$XoBx
z!t0~peR4T<>L}J1QhwoU*e;6fm%5L8T(T7JG@Y=WrlaZO`xs@nvnwX~X`MQ?Kp*q^
z7i_1a#ORmk5?O9E30}&sQL)4iuChk~4WbQYI>RJakJqJ+jtygkreTxH=v=GM(Dv%6
zZoVC0S;|z?3q&&^P}=hR@r7?<AG>FHxbWP<SHl(VovNA(zgIIrCbJrHC7A*nR-t-Z
zlc-fnFxDG{Yie8fwh8bGDCfY`2#55c-9mXyPb$55OpgOTR%`G$+(=vfV|Qmwtle_~
z*5kUM&MI>QLk90UWD*J(4mYPvDsNfmd`F#ngMSWHPxGWTE7}OBj+VwOOHW(y4AfeU
z52d%U6r8Wb5HnapZiy=9(0XFD4DV@4c;czXUPvzG&;U=15|=t3<gtUwzSujH8_ZIX
zTbm|k_gYR7r)~WAB&51HkGXe0;Jk5U2S?DUGxg5@EcrfUn-Q)u=?&Oi_p_zAja%P1
zR!WW-YSEHOUE|GMEa8v7tA%M==v8hvFCFChqRXxm!`uM&qhi_6Cpmt^+qNyjp^guu
zA1QQy#on9YH8iCk)vfQX+8y?y<<m_&Vs3Q?K@XAhCbyn=;!k^Co5|w=guzsItd=zm
zn!Vwe|6tWPVH}iz^7U`VThXxk*@_-4WJ)#RuXDNN21<<atq*ILUoUn+q*xM+?|Iu|
zpRQprg{Jx9^WFO*r2kIWO}n!_EBCmvjDJ|aMWu$kT6oYdC(8kEy?GsRgD*QftK7-d
z?y;0KSYK!V3@>WSuk=EQG+oTfd`@X%u~W!?E7`H9PHLxq0NHKk#xqrFo>9u&VKF8g
zIY=;-AGdSZ=KnBWsF5lVk-qFHGPLJtC&b|2kqqK-+HLY1#4{v_hs+mV)oMZFg}B6H
zy7SWB2h7RWF`-EFt5+N$;C6^$(b_a-<0eO{&-!)re!II`=Y{I|GUHWUTXRiV-S}o1
zp^KGl2{h!kZtG@VSy+{Q`!hP&qv8$2QW*V#Yh`o0-q)bpGoqs<F%H#c9}Bs{;X{7z
zk*HRFXs0gi+TF%woMO3y<ffMq=;U2$3GvRV*(+VCO6n6-VODRb?AYlOhvd#zA`3I^
zBnyz;zyOQU3H^j^4Z#(~yKW8ddhH*p)r(x9G811pKcQ6)>=YF6nN|2?a4S_-S=Vq<
zbv_s9LtG&|{>4`RG^FM^0T!$7V&u}~EH5ObcYI`eLLW+P(HG5MsI3c+Gv@Ocv0ug1
z2VgeVC#W-0Xg9+7Qp7H?51%WFOwQ$b$#HF8oEDuo9`(CK{65~Hb?wFT6qv&F+3!X@
z9O4Q%GfLmTPMD48I(pumZG1!{P);oI6H?b8eZcBvY<eZgEJ5S@uPE`qptJu`fA@c)
z#NzUTYLfpE|EBaWHtoNH#7zGk5;Jl#|NjDsIhg;qkl2pi{vY-IUwQ9;(B1!;_p<z_
zOZ<O$FYABiy&V66z5ff|%g*utp7*N!YxuiPgGd0_VkvNiB_{&l6Gng`FfmQ_F*`&%
z1VxmJ$iRRT;4RM4<KcmU6Kq`{E5E)wPp@(~PuXv#U-)i(0463Yta~YqAzDEx`wXP?
z@$v#7d3nV@i2uUphR6vC!$&79Aq9o{{mkz)V)`K2A#s1-?Os7cGC)QwPzJ_Ru+wtD
z<^kZrfkJ=;6ru9|{;SgyP!JIKr2p6M2a*SQ@4p7p^a^xgkGTdHrwR->i1A<JCRA+w
zbOEzpvj_1%I56bt9Rzlq(FYrX00T7-h=<oVTZ&;Lr04^(1qCMQ{z(ZGo&b(;PS`tr
zd3-zq<?OucM|zIq@Bqp|ECLlExPlOK_qPG=-2-V7oY~v$d%e{F4uwZ-<zIrZ=Pn|L
z1ct~%RS1W}JPC!i4_gmJ0$SPz0%eu;kA=Z}4r@Fifame&&xHUw0{zYg%%15*5Z3MX
z6XM{sHNrrizy+*>UJ)uV2#EhlLO2gO0`}K$>BSXPVCSI1KLQRE5L}@M+YJc<nsD?4
zLWJJUJ=?NIM8qh<$;APChavi@WhoO^3tXq_%g*j+D8kvPeMcpNtb@B)?tb%YU50{q
z34Z%_XbI}GzV;QYb8*}EgAHoy2wFz{c@!wW{hnVBN&rFv1_owyAOyOC2J%|xxc!l`
zJ-Y_^w)w0UTfVuu1aSji%O~{zB68JVBFL%583TnCAl~Z#?)lliwSz){1X~lbjt8=;
z;{^+Sk#|kTwtfkVzTg2l0B1lEKtg~-e0}=(Fb+6KrNxDK`X2pqb9#$$UXpo~ZTI^J
zdr9dLUO-;|qpr~%ToepQ0GJWjAM;}G=Z41b@CW9@u9^<=-pmH%$5<t3y&w3%Pe7o1
zTaSU~`v`0__izB}^#yLj9yP8XDAv#1j*gI6Kj<CkXZPp_@4naJTRrtB==tYQTmv`i
z`{u;m<R^a>p&8`Thdg)WFhF8S#Jpe83HU>Pm3)ssR0GBu#=-4dz7iWbI0KgRFYn`x
zcyJq~a93YT2VzmoAOYBW<*%|w1xVN`7`LELO=ck7JpVqwHHAjg!(dG^+p>RuIsuA>
zk2lmQ`fT=_dd%b$#Lz%Qh=g`LDu;@RkbM~8*m7ERt9H7euW$PHq*P%3fu{&{K_s`n
zPs%E=eYLmJPg<T7k;E&z$cPj`dtqO+&{3eTieA@tSgOeU3n<@lUqt@9zoK7-?`nx{
zwvZr~_P&23LHFz3-_Qa>cm>$pnwIRbCC=hi=CEh0PRx7V*C0s7HP-NZ43A%O2bsI}
z5)R1iSB-hMgmWZFpMQwlyQoGSYPhDJZ@hT<z`42$H|QP~@2030e4lxe?4%)80QNs`
z9G7*X+hDsw8*qGAYkbA}<uISlsxPWIYXArl&Q9aAJlv{X6`fKi5CbJEPi#~othcpN
zBko27qkn&4$kz4L&tw!C^b6fH9oQ#adm+i==0+!V<M)^-$sdh8>_(tb-LmQ+M|0;s
zrqeT{I$IU`2v|uChC9fxF-)9L_r57*dv3J0=_4`5b4f1qv|W9IKyDx1{#jk&7q-UJ
zXsCymwx3(LHBu-O9i?F_`^|~LNNxaScI~IC_o&2;SlQ}&I4>Evbs;cZ5njcGa4lY`
z$J!YoWn1LEAPYAkRb5W~BA)26eA`s+Q6>)Ml96X}<4xDx$JN)F{M~`0pfs5*_C_o1
z0uG#_GMs|BcPq7Yyn5Z~?NFI6<~PTTX=PtB0x<0s*!|0nhxk`2?znU_az9b1*rrhU
z&r6NAZQa@HYqxi?;)~@kn9_V<!~MEq5X$=1>@*hK_sShg9vj=Nt%|7m#L5}-+_o~4
z9$uwsYf~r!{pDDjWbZLsAC9QPKv8_Fjf90%>usY>5p$?@4+Y)UUHfF9e9#1cdEK^S
z?<;K$c*T=osn^}kYD7)u5$PdP<E49!K?+mV`5n7-K*0$Xwe<;tI~TIYL}GnVeyn6-
z9tp*xJsWT!c=&#^g0hEc<BBzT3mcLOhdm(&uS6P0=BXVdVaMBw>}!kgGh*6wRAOL)
zWqrm~KIo3q1lB({bPVKRfvLi55xG~Njz#u6i7YnG2lIBy%O-xr!A;5svo{^ZAIY4Q
ztOUMaA3EY^MfY)9Hz6u>p{?HSTRUt6v~Vkg+L8@bwU@A797sD9!{w?w=-4PNR3T2I
z9(T6Z^+$cJX%VoJL&x7-EdnsgK}3;uFIg_aR}dl8<k%codT&;urf2BSHXAUpDuMye
z^`O)fqrzI}BK+u%I*`gSbA0avL$537lIaObOyL4cE;3U1aB`9Q@mS&s=`9Rkylb3)
zfFeo15QX)i%S!uqSf<4~2X9^3Ox#=JIdWed$_zis8mI8`^E0wrXV)EEXDmoG?=RFN
z$73Wy+n>zNKvipW8oJTSu~fo%yS#M*$E;b(0Vg>iyqRqL%~Np+^Data2P>3z78aXM
zn0eyWKm#VfIc@+fV-jrRmI(+)*ArIsspU<W1f$eK;jR0@m<%97+iuJdvbnI)r>Otx
zv3qidhBjaRUK25Ia>I}B^0ySFNN)XXv0IX3s-|uKC|^C2-uuR{0*sobV@*#(+fEKk
zW5L;c<X925$S&$0r728>3JIll@!8{9<a)4(J!qR3o0Pc6%`66<zn8T!g+aUnbhoY*
z`Sv~h#Eqm42f1{Ig-6i|G@I|*X+u=h+z($>Ewnq}uZX?^dcMBW5+h~lKh7J)CBRTP
zm)eEj5xz{tHXT%$lA_qi-I#loX*eTh)nr2ya09!Y&L#fIExm@@J>089crJ%?NwXrh
z?`AW0azGIi0KPBj+B21AWz=n0xPH<vYabhP#|3blK7TMU-WD&YL>1azaU`6lsyQe>
zanP7og5@CgzJZLnPWpwK-x9a-VGaZXOUI%Noeq34%thpap<vH)ZVatjR(oMwEJuw)
zFK((laKS`jMm0P_5?O6Dx4i_yOpKook*WVIKXQGj6doWO06acRC@JT%U1lq&bkgUL
zdx3?y?BHk}l88-+E7@g;rfS~MsBa#(MprDq#AT@!Ju%%v!xI|Bzj(8ip7x=`GpI9)
zd2_pBnvI9&=hShJ$n}3BZJ@Amiw#(?iu2}Djl8HpEEWcz*JZ^nQGiqD5~S3H@tk2~
zX&&;>d7#?^q8sO#s_Nyc`;UyO+maRBDHpBir$bZE#_EAydc4qyV$=!?8+Re+DTl;g
z-=&O4`FEsC;{f|%x%qBpmArZElV6qth1lq#YI6(4Y#$N~KB4{(UNfqLn{G?h;H)~p
zeDk`D1=S$|GvoOxYa`?nVv6)~sL9S<OM`T`xGC8%x!m>(*e|R`T?T(~q2hcsa25tJ
zc{`sL+?Me=gBZA$e#cqqG_(=WFk5x^tOY@^Qgz;O;}NG!-{2mTO`F(%(wVgqf9DUM
zO}wAUveK;&a)+ilwW!9j3`}W>$->pH^r?=9Qz*wlDdX2E?rW^+U;*<iazAv+cc|Rg
z&+yuw=CK|DYk6%mBc+%ULr&ZKspAg%Aef1Vls_q1Rm(9!eOw)HKgZspY{`*i-=N0l
z$JmWGk`28~+<e4%a_8PdBi!FV{!9(E!l?4yc{&QBZfb9Q60HnZ&-~?7TS{U7ZbxVQ
zz<+znta)7*Mc0>SpEU@|RNwu@8q~GaiItshW-Qi9liC?;cqx=FmCGAlx3ItgIKkEg
zSY+LvaE%XrUVS9+<qWmbSX@qWwqFI3+z7-B(rUM0-F#zW<W^QVKg=`?g*Z6|6rqw-
zZZ^J5b-9C6`yj+HL$vFx1<T_uQmmR=$ve|z`rp97>hKt%3DYX0P;hbAryDMuLdJFB
zcfHq5+Wg|xe2|hV@@XVm(Ix5@f?+}Q5$TP~>zXS-cmal_r-jzoLM}Oh!!8Y>LZy53
zGa=r!9YrgG8@jTsbL>QOP^W3^(og%NGt})0)~S?jW7w*Pi(!DQ!sZaKe8`d;M?QN-
z*}hrt>9cWUZ`v;?o;G?~C)SqHw$M?i1)EaAwDB_KTrhvs8GcN9nkwD3nlqnN*d%JV
z0hgJ@BKd~(DLfl780UDDv}}<li0^0jb1tmVFz-Q6&F#GsbX>2G>$EPHfKnI~o+I<Q
zpNTvZQ-<or_Qa0x{5O(9BIx(KN2=!uE*uTTIv=Q8vW_@Jn`dkL$$Ru-`@sPb+1xEC
zDEBqSQF%{I32YnM6=nmkbc@@pzQPFCt`&NW+;E}x<7SQX7Nx;aR=y+;W3&5vb$xFg
zs74&nhAr9ekHDqmeTUOc^luQ(Oo$pJ$EH@t>yv@##EruqRt=ef)=s+`KqPKjb+d(9
z)2swDPQ>kgtMMDj9BV2)RynkGZZ%esfqng@o`e|+xMb5KQObPvXOK&@yT!A4kf}t?
zVhT#JU(FDslw*32ZM09%Tn>t80Z1Hh?g8bnEcN{u?;0>HK~Dz9?hct)b4g*8Ysjmz
zIF}-1tY-YHI#fv(^zr^!obzXuyhm2KR3*_mHz^K;G_|cjlS;Mt++hD(OmG*>V#@fc
ziUzlu#8Z`$)M`$XtMyai6+3;_me+B2CBP|7r3btMU>?|`<+l<I+NBXEBmINr*tHY$
zGn7%j5U5O_>bx-zABwkobATw*5KAMUgv%uVitTi-V3;||UQi+RjxC(6K`AnKHO@T@
zp*KudUmFvBa83Srbs-3|UP^MA_7WgIC@`uLp@{jySRzF?>%7V!4}Hw#LAZ1+R%Y>?
z-mp=ff1%VxC49C>+LYuRH<g-vW4BuTXY@?~XXgyt;4a~KNAv!a;lvcwjoc0aJ>kVE
zlFwaPRPTH}<osO@BWy0`@(rm$7LEO}CC1NsuCyM>(GbTjLvePT2+<Xvn1|(K!8CR2
za*y4(|F0xnLkovLd5!KSj+S5Ti@T#Gu6$mWx#LswS3kB*CmMbbxpn)pTcxBtfaEu9
z;h9Z$q)$V}#Uv(-HQB&}RKcK#yMCJD%Jt||-fsh_So<vB2h~Aul;76u<trCi9mQ#Q
z8&6Eq?wYk;P_9s|Yt;N&%l9QX4cjhi3@`S7OnIo0%Bv+BGYV8mNV|`n(iQ=w->CGT
zwy2a|&42cCaysvpj2!|iTJIXSt{vxyXlfT-+N)^4vnTrEyz32em`*FWE2Ts?T?!83
zltyYJhgAm8j+DqPGWrX}k9#UfZX69wqDMZOAKbN_<zbUB@`P*euCE%&mTfaIHkFMX
z|16vwjh2p}b&FR(vbZ(j?^8}cI74ls$~Ax3?9acEBa0#$)JW@jR~D7wJr*>rtf6#m
zbS|R0ShV+;y&XLQj5tZ^-E=*H^(Q{Oq%J^RKZ(!Y14Y4Nv0!*bSWy_(xhIBD&MmEz
zTYTXtx5`3HZLn5W^K_;XOo0Bn5cA6Iz6ues%Hlln1lQ4&5#cGEa81NX-X~C52uhf8
z^>Fj%q2V}1X7S|i(B?B$GVc)1?NTU5bDt-k9WbmJVV>@5zIH9%ueS21dudtd`t3Zu
zfJj88<G+T*KN+uBn`o_d(R|ME9AhdVQy<QX2^!jD{Z!1jU>Bg$io^&4QqH`2QT==^
zsJQ-=Ig_SpRoBhX<|lB}<E4N4zIpQrtUuSZ_&_0wo0J&@J%N!x+1PWny$*+Ex9^8E
z*R&(%y-2Y>4REPZiVg|XnKgo-I=K}VUHLrwQ!BLV%+mdGcPp=C<PzQDIUa4fDlLh&
zlW{X$<?JBFw4^m!z)HrviB-{ST<Ja~tW$o8z}0B(3d>a-%u&(_aWeZM$>+-xvuEIg
zee8;;ASZR34URUanrZ?}&o@&FkG+=pl-c|O7~MI1tM;;sABqi5hpHK?JXkv;R54AI
z2^9xer12dIQ!P?W>hflSeYz3<sj!5hPL_|`|2w>_#G@TikG7;I7j6cS&-@IQm!=6^
zL&ilru?X~{?ttEMvxsHQ$W2vw?(~Zs%)$fLzs)u7&~pz=j;~*r_5V;fA^b55gDQTV
zTwQ-UV^q(N)f*UOA}NbpYwvr9zw8>S_kO{ph$J(@t80SZ+t!<~raFiSlke!F<M{hb
zWW%DE^Xig(x&i6}pr+J=lm}n|@D52OFbdhjH9?{mbhnTA%LXFX*^ML`q&09hvNt?D
zh^$W7j)T$NsjiB4+UO^LW&}!&F-;3>XAZWcTYF9qVZ4=NBndta2H{LkxG*40gWpp|
zc4QKFbz)PsHKIyE-c<1N)Z5~&V+&QK8>O($LG(f-wck>j*L}O+M+OT}Gkh57(Ai9z
z^78Ymm>L>2T7L2SsLs4VADZL{WlHM0CEJG%Ni@891Y01@v*_nGyBKHY8lzxg?ok{g
zSq!{nv%S@jaEUiJo*S(j=N2SIV-<~t69hfH$64uqSHWK5;l-9nRm;|oPeEpdv6vmc
z^g+fJ%xTSOWxQx$w#V8<GI~>RG;ubBanVULEaU)+o#G8yQ=>V1yo(T1ppTT!r0I24
zNG7HF^6SKOi3jj*){6KXY&b<B#{ZTJ{w>nBNgI8T;dzmhSg^9}Q3B}{ZZnZy5z2YT
zi0yC7=DKun8)0RF6mP5D!rp4*q?=EfQ1%SiGSD3F1Wu{Cyt-X@1?Jne{UQ~rVN@G-
z)Xd4hNFCj@GG^}BzTSTfiPfM_CWELrMY%cy9At2Q-TGkP7yVdv7-Yxmq7Jd)%E~sI
zgmvBNE0!$yybi_@N8k*S3vjoY#A<2sF(^evJ}xZ+AU_@x#*Xi0-J>ZE4K4DvtUEG1
zS-iUFYjK+{0)mbZPCt%4U0y`~THAo1ZH2T+|1Cka4wR0S9gPqxB>(iUzRyNa#2IRy
z<O~i)dZEG*`_d{n4a#gm@M(`l)?H;Xy3}}%utt(XZ;7CDm3OXqA|EN@d~o#|$KS!q
z{lL9pGhLwyUqOw)DV#>EWia}>M{~nBCaGg2N$aFPsd7day8!eiKXILG`%dMMG({W?
z!we%V_^4s7b9^T-Kk2pmD+)1DAJ}~$5*vM>jVnRy5KL7Qyrdgk2{>zY$Zk4|`W*0K
zA4Qk@!t4Z#(6gPw;CggFNHg%97vc5tFL7fq7rvzD3ynbU)#$cW*ehs|Uv&K&e1UO7
z$MzDk$CaAZ*i`e_bZ}Y@EW@-H7cB>*ii%XZX`WV|KdQyYO*#2GfN*m`jYZ50`o)L8
z-OKZi5*SEGj)a={E~jK%n1`LD5{NcpTIXm$k65~gddXx>N><QDPfM)w-Q;zGs!=}Y
z0<s3-($|4wEP8NFNs=UC&rd~>&8v0H?hR6UC%~J8uRkH()E82Z35@d{H1$mrx3{fn
z*(>Snc9PLOnoPgj_k1+y|8<Ab3Lh2Z*%M469DlsQV5c8vv`T5;Yt7hU^>`kf&TQU3
zs&d_SrFPh@o(+2r`q(Kdi*S$+e>hgE62Or^`)Xh02WaVNQ@lVvCm2ClIl0~3FC9Oc
z{u*%k`oCB^#~@kSZcVps+qSvewrz8_ZQFMDZrir8+qP|6)9*R)&76t(20tc$RpqlH
zqcW<ZGUB<`b+0B56IDAa!Nms2y{8@4{OOX$^Ocfv?y~EET%RE#)*C5KQ*Z^Q$n<RK
z^m@?SF;DZbP>6Rh7}Z+puDp=yY!tew-0?`{ur=R^08E62*K0TmE7LFnzpvp;jcBHX
zfpQ+y$f$NMZOQ&h)%`%%z{gJFu~&e;jb77-xX<Y?=)RGJK%pbBnmjDYdXrp@vby%W
zM|HB=Wi&Wfjc643ZP65w5msaQkm%nFp4Uub;!yIaoV<sg;}47`nfuzEXXk&DY3Uqi
zp#jHUUQ$0yPqJr2W3M{;)vWhv>YebK3?$xf98$JYme!bZP}lhU-}(aVSja2ve5<KO
zsDC<U(!lvwPi@18bI2c+7U;h}7#nkTeA=kqUT9lu&#CO>4zs<WFN{9OVXOQ{E2(jZ
z$J3l{W*2{%y3DxQ?LEU0V}iGKhdn!{zp}(;O>xa*YdmG}T(Oy5s}Wh%Ka*(Jk5F_x
zwb*H6<MTb@4I*pzyE`KDkCSn$7ept)gpajxCJtki4{pci@`U)>+OqD_&7S0pL&U@?
zUY!VrygA+ZKFqp8LH%mE*&PdU34``FtmtcrBBm$`>w4_IFs`6x9`Quh-S5jjHt3Yu
zy4lnbxB(Kn)Pb9YQ|&z`=6a87Bw;^gMuSUr?kdht#6k1!YOW#zR?Aa6;3u4Jq0Q6L
z3rhdnfCf#^R@{d&$6vBsgC(3)9kB{o?X{PbtwcXZ^OyL!hYQ=~1&+^A92m?cLzJ|8
z@1$Ezz~wSNe8HligZy^bf+RJBO^5?Qwqv{66PqDkO1}N|i&fcy^v$W;<Et2d<-PH3
zR7-}9hbm^(=DWajMIk=He2b+_c@*LKquk6?ma9j_4zQ)}$slbu?npKBPezo)wJ2Ft
zM<Rt1yWv=OE8qhWPAb$XmACAjj8yxc=u!V?rk!6P#;C&KQx*B!!g%e+SLaycU#s&_
zy^q0WE+F=ns!^p;uGvF44_=6No`g*iFZ&21PXMi445*UO<mpD<uEH=poBBohg=XkU
zgK~9f6x0(qr0(revJ5Y6TAJ<`8~u+XH@k4_MfJn%seP?Lr~-uBmeq?>>%WZ8&-n24
z<5)vDtS!V>Ie|@_=y4*axzSQ!;Nmg$TC-l5RjEd??D~sQ8vC14^kBSGjIXnF)V1g8
zdRlN5_z4}qC|=#Vh!6=eWX|AS3;q@eTL(Y-*W50bL2(5)2NGk|ulNieY*)$NuasRC
zQbhGqLm^^Tm+H}Zt53|AYx`~5MR*-@rZk2Ey6I`%7ILbDnE3jYLcv@|?s9gojRMfm
zG6)LK1L!=`4TDI5>4I;1z8_?!tZz#eZVdp<XYaoO5xGu|c+*gr=R<X=b(g-Cc;l>g
zjXkKrR(EBb=v5!#LB-QAt`X9yf3fMZw3%zCRpPXI65nzf>qQ6RH$>MV`)TBwRDzrR
z@xzK8Y(%#(g3N~JCT8y=p(n;rT%+lSG4R%VUr~?c9G5&{sh+Zmu1jq)If@C!a<VrU
zf(3IkgGQp<Zb7gk#>lFy6{T;?Azz>|8lzPR2Z3|rkQcyt6JKyxKn=f#7e*O{ooZup
zw=0}?KByW*W}6}9y~rHh(c<M4dv`Av^c9`<ZcZ?@IIQ=d$IA_LXQH;jn-&_bVq-tW
zcX(-%^?qEmEE1JD1-ue5aFc>pAKwGX@PA1Pzecv{E^_x6+-EhiFZy5#b-jq*m<x10
zrYQjdC;DmsS!i!l)Tl7t5zT0vD*Rfb@TDsQ%#cK-R3R5QUQUnZQ%zG`25jV$lPTpr
z?a{RCLb{qq<gxFd3B^m6fK{ch#^$c)S~`07L6PiMti=bxS{a`7l~BV%@}V(P9JaM>
zrp(h|Bw*p*t{N(GfPdiaR~(@ZY-;eM6^}07pH;)r>XT>EA6%mywHxg7I~k~JI->hD
zve>7GduhvMhO8)kg-6Qu@h_x3<%`qg-JI2QPoN>{3HuYDHH4rwnq5Vv`6P0u;FTlH
zJA$oZBIq@B6SO<%^z7=$6{6wYW}n5*Q{FL^ZisXR)X>e0vD^{;N3kMH)SO8v@&L{V
zT%ApZhAnm<k_VG(T9zB8w^%JyV$6|+KF!X#HYp~zL_PcRaEjB?3_HSk>8)jUCOK3U
zoc$<?+bE7=9m(|}Lnb|KV)RA)XL7H5cJqO3w$ekjI8HW>{^;?;kf+1})Nx)TiAD`i
z+uNnzRWWwHN5HmQ@xnp0zQlVK=pdpKXvnz2Sv`6pPqsJX<#a`@&Bpe*Ij3~Aim)wC
zF>wdFsKZ1yn*jLl!%y9@(U)D!5w|vF0ba&D@=Ol0B40Z(s?`N&Mk@!i`MH=U*fMaT
zdyL5(ta+2KKg-iso_%vpu}*3PjT-^hxO~#!(LLAx5VpW(L4tu*$tBT|0W6AcfA2wC
zSC03WB&Nh?bCcDJybsERfW1aze`Z_(1X$+bRvH*K<4mw;pM**pX+8+D05iJBkS;5q
zwm6*PqxJNBJH+Oqa3!cqeAR&*An&4dW#eoKFbT`jebMo!Y0BQRQ2Er>d;zc0fn=t|
zq~un2ezxZxLjRcyOz;w#wvioYZA3j=<NTerOyM}gp`I;duA6sT&aMjC?%^ZY5>%5B
z8UkAHnnNAtyb&zakW?rMQij>3iQZ{@+>Y>7+;Ry0+_g$M$?&B57#n?t(u`g0N*7M0
za$z`#xh7cgh8y#_-l39w{N|uD=7bjm@8MXbtX`o=y3KrH!<3cU64IHYzVy|ck=I-S
zVyfDsouA=RJ>YoVJ3xN_NIc5M(X~vR`x)eTML^#lkf-oHfRME3W2*HKu7O85j|Oj4
zS-_Qe#iLEh5DzOza<|&;EutBy7hww$B(-IfXs~+C;4tzPV4mmioNcQ}bVFD0kS69C
zFTsTmIE*Aj$TsW?XgokQ^zSG>>wiJq|2K-SDkq_#sQC}Y=lJ&&pPB7nsy92s{}+nS
z{?jh-e^9;u)9(Jq-~L}|{y*&gf2R3t|K(Qy2hC^u@BIG9-ezI`_x3i+Pk+LH9RCmY
zw#q;Dc1MaVpv72x%+|?O+*Jw&!*Cevu%80apE+@ASVba5B0|dOq@<uih`9j20D_Iv
znyYKyYtHAK>;~`c`{8Ha?f1@YH;yRjU}fz%S$P=}AX*SKurZ*82N~lZdpqsN-j@73
zU<4-SDfrDV;ZBoBp~rzl_`ZE%NkIV;J+we*UYW^@1UPeh58^ijpf90On?wTvf&l7o
z{N*5=Zwz>b;M)q#uZNKrD9rCI;l2>J<xjye0UOP8SVIbMG64=SJ}M6R1qWX0CZJ$v
z1_6`@FVG>-RZGq#^b4?9+>8SS_oW7f-<_RhO-ogngn)n_WABhUO1KZ@6$B8_t`WNz
z<P2KmRY(V*=L86bKzC+)Fi653z|_WWn@?Y<keUl1AONV=k9-3&;${f!g-|<>0^rHo
zudErbcLqD+WkBN^9$cU|R}B~dI`CWe+3cZ4*lzGzKdgayZX7!RB8XTYK&_P=MZdI+
zJZd-(6c8b3oL(%fgdPbS{x-Cihfr4i2j~j{WKazQK%{KvUU+3I*x5GL)Y5O<qYM4m
z+qJ2c{&osN9_|8&=6Z8_k7WZ2@@5xR;1M4-72rUaeh^<qmr!D@tzY>&d&jhfF@Ena
z0!@qG550ng-}(#`SOkDS_O`JwpgcYR%Kin}9d(;V9{WQJ;s^Bb5<b>N^aEI{Atk@i
zZngXcyhSbjAU+g-!AIVoz8|1}jZ29EW9QFZ04OVufg|uH=RyPH{Am<>`3oura3!F&
z9~hXwhu62}qIS(efan+SxBd5~HX@G>wGb+;|8~$fTUj{~qn|z@<Bttp3IYKD7A^r)
zSQ|OxYxbJ?t+(M@Ad2vk+vNW>RvK+50%ZJ^=hxNIWB&482D$UY^TA)9A;H8eWBvf_
z@YVMKCjf97_4Iz*-hRWqZnJz-kAD-se^-jlPtV`CpuYUW^AYG<&31BKDkDB;R>Eh*
z43B|sbE{Kket16q8PN0nn_LAORzNlkD#*nBwN%i%Wj~`lIx)xE>Lonj=OVDRha3lx
zWza+Lw}&Z!pgzJ0pY@7{<5Mp$Y4^$x&ljls;rYa@2vg#zKRlm`5YjJCK8|0}dcTev
zlAi&CJBv<!!d3@ZI0#Xkg$=~72p!5!BwyV3Q3(kK!T;^Wz!n$!TXawW0MYh>*#9P|
zJ^x#v&o|%K>E2cy2HNxO^$S=WSU4b$;=p!CFPZE8Pwno>wq*a@i`DmgnV%q|yxCma
zv$$Cs#dCY<(mWRG5x}2Eg<y#s=kXCQ!9K@QwjO*G;Zv_gPQ>w2TdLDQ9c5@MtX||p
z_ov)G?#V}Cc)b#=maI^14wB5MnwGe!?t*J<>MaHL)p5ywBIDvt#JhR&L81-d8q5-*
z(30K}xs3kOup^c{G3fjHmc4<Sz~}Q}?8St3V{}wxLCB+9*Z%QhH>hlf5=>6WS;5M|
zj|Fgyzh#NGi3_hrY8PB%K!txgAPq_T*r@Zv&E^FnlTGuxv)S8h*BI*evgP4p@4+f;
zOivglKvGj_9wrSl4y9J^YoJZ(AQT{NBTUr>WcCoEH=Di&94~j|Ku`TjwI;k-auuy|
zS?9R&IABl9wQ%;#XPON0<d-I0)E0-9Y|(7BH<>k#OvrR~0gH<`6s>BGhd`oeG{;;L
zB2j-rNI|?XDYvA2DyyWfyFIQv=I;V_OW5XrCQ<jS&D0hR<O}~6#JmVBnv^I$WFLs^
zfOf%Y&r0*Z`dUr1Pui}YQkLjUI2!Fdm`azW3zVB+73tg)Qh6`guSRJ(5Lr*mPT(u5
zP3tCr$)6rlL$P>L-|}np#<6Di=sWdULCvY}4bz2eD#|&|A8&lz64!Ojb0Q#K<EF`1
z*s3R6%PjaDW?b>QdcPW`=-A#2#`KCexA3Ou$SCEvyYqx0CS~{iv=JYB?FWYM-WbMu
z#P9eFJTSCGuCu5+!u-ub6LbbBWCpHPMikpVKVqarl`FaXT9h84i~HU}^D}WbVLN&Y
zLf993&Ux3X<La+LBDag4gj>g-o*rZ0m%)j1mH0Ec>FD(lPEaANqh<xR3`FN?oak8K
zSDe&gOf*>ox|z(3wRBPn$3MK;9cmNyX~-)%FTgq#pXkep0oxP7D%~GB8Oc68mO_@1
z@AW>PH+6sNzubqEinv#Wg7{5&)-XO|r;!@M0^ROzOVXWSo#+&CJ#*0Ig1TFhtf|f@
z^_2-_84QaPSXLsYkD`(ttSxlJA2Znqye->pi$0R^c~^<u!e_tY25c$%(`O}?#3R7Q
z?L9?dVDxUauR%%By7``usFYDDpt93@mK}T~T`Ovus}}|ry7Pj3=FCqPkw=9l6RWU@
z^xro(xQ5?sQM6Gev#0a((JZL>oP<>-%7I3jG~B|II&2PXE2F&(;iC~Xy=kyJ@k$+{
zJI17*Q22<BmeyzH#+a~N-?`V-cxq6Wy?lK_OksLG)B+(mK+tt*BMM(}S?VEnM_FEi
zi<9^h^(?@VqTJ=!UK|?8+PzG1(-;}h+}SCWD5n$T7<XR|C!N7ZO38H{?)Q9R38tc6
zFczR=VnhsiR2-V7mc)FrP~dGD&dP1cu@nfIhqbb&0}gm5esLou*&J1_*=N!7roZN`
ztaLWItWKvetRsL%N_uj+Z|exegKA@^6kn3w2M*kBgtW|b-hgG7Img^0AbA*h=fxNo
z8ztfts+2>#Xk=EAbWnS>-lEi+?98c|rwT0FTjmfce0B;N^r*W4`D&hE-tQ?RV0Vz1
zIM5OETKi`#yHyQ~u0JJrnzR=v0=v2*-qA&)kB}GWX3b{vI0ugw%z{Kne~$<IOhwC)
z<9lvR%(E}UrG`e%V_LthuCP!XDxRggVPAXy_C9mbM4kFr(y{R#d0l~n(spBY_&)ML
z9pj`+p5L2%k3%yA{Nd}LxsFc`o%axYX+x5)d-F=aK)1cV&J4%6oef0RIpP=HX@bP6
zkH_VhG@-ZLV5=F@HXvti$uXI4zf>Jt;X2=UpnHW<DjB%?vK-bSXVjIR6<jQgS&JCv
z$2>)hWc6dwoc2LuA{rtWNoB9@t$DE2z&DLHL%Kd!bHlm%IdUK&|Gew?XWi?nWaGeT
z-<x2RgkNtVfi<Uokok9~TWoOG%My9gCj&0YDQ}&9^_D=o(B^7&zpIy9CVX;_VcwP-
z!fj`o^=f1pIEZTQ>n5bhdP*#r(k|A4@v@38XYo#_$*L#zca&%?@A-CLKvvMW%dA~b
ziWDf=`I6V|IxB)DfW!4j+cGDb5?<Y*AMC=WWF0OXvD_)KJd@;7h!}_ZjIICuUg=T2
zb;>j!w#{^#&(5<WWQe_M?(!DaeIhRM%z~VwYTAUiL)u?Yb)qlZ#5uE~DM9mM6KzNH
zOPOxo)!>kk-g^b<mVlGGY@6n!7HJ0023a0PKcR+LW(%ypb6H|vqa|wBm*dgP-ufW8
zoRQ8`xw_&FQJ80`xNx@|TZMWu8#|noZqk>BQ%RO>prNMr;0dE6h_fdR>7#?_p|3)G
zor<G5tM{lQiw`!mmV4t2`n{kZN%d-mPqi8<cN24H@-ecq;J;62!(<d-Z7;rQf9v|3
z#8sL1)Tm@2@$mDd=y9PM&0#@w?F=VI2}QVjs2y^yTBCawG6sqrz7Y1(c1kM@p_;;d
z1>M*kX-U`p%B%|p+dY6mxh_(-D~aetwblN%49`CWKyHH7C1!?L%1lHpRk--la3Jwp
z=1GXRpChr#;dqD+vmLoSkMmk3^6+`0vkB);+u*2?6sLKRcvE@;WmeQw;v?i1_4)#~
z9MSJnGvJR5aiz_LUP+Y#J^noBZZBBj1S#HHUrELTe&(n`fZx^mwHR+<vh*ni26g5?
zX&B=H;}cY8D9t=6gnmy;UYA>uZDvAB1U3n<i(m5W?yqA!z;9)M7=oM$7a7;e0Z5sA
z8zz4Xl^^0+cs#B(c|gtTUp2{sPc}H^_G-FJV#*0S+0`Hj)}@PfwqP*6ZO<mF^u9wx
zZ^S2R_#AO{+)tcO8bW${IurhRN&RuW9l7omk@*p2bWgr_EKhXhVH8?*D@esEMW2*Z
zAll$1DrYH6yXN7q6_;qGJzWH>Mw50hWP6|9;9~VH9&d9~iZ6MF$;inE8PAOeON7#z
z&4Z(AIBuN@zT@YMu7aBaMTs!z=ysRJ*+cgW)QOFbq!K?i?OtvPnPjTSXPPyRk<Gds
z|4jJZJEMDHN4jUv*a1Uv)rGg<{gs&1<^}W1;WWPPj!JhRc@DFw2k~U;@}@`?<a4v%
zdq(1Glj~Ls8#yvtQzpB*a9I*#$hY4lM@_ShFt^2oF*+PuW(ibg)o*)J<FY0i58x{B
zY7rJ};lUM~gn|bhc^Egt!Nc)gqz0fmuP+36Xv*6T!ddOYd%Jxt44%NCp|9vmg_0v%
zVf}Ww@B}7yx^%pR^X5<V#n^;Y2qtIev15$o(*p4gU@CI#)VZKwi2v=AAt(DiEYU?g
zR<td}bS|dS>a<az>>$xv(uHt=@>Y}SI1*<U?NeDo=J@;R>yDLcs4riK$iu*joCM$b
zxLVD8kq_Q0(XJ>rXSiPPZyDB}2D5ggYSut($QxFkp7BCiB}G!Cv7-4_gqrQhF~pNY
z9g{LU#tw4rw2PGW)Ds1qLE&zUnRyy{ey$8zNkSo%UXt6vj>7^&@gRQ-=9-~~irl+N
zYfFxf{-Q$>YmN2fDve!(4p=ma<H#ZvT|i_L&3J-#_JjwrCC&q_7IzbUMg?J!2s6hW
z1CI4P6KB(wW<@<y<?C^OMTH}9om2272meGw29nQJ?bY+Odo=B549%BgvW^7wl;dF)
z9e+671a@cWUT?U+PAxwnRJ*xl2@GPag>bc8E7`i2qD~XMo-V%)wHt%f;N^Ie4m}~v
zYZ5{ANTerV@K9<a@n0=sd&wVui-z}&4Qs3({vDUf`i1rzwRGwEXUIEJ)q)-5Fr{eG
z_`Yl0t&8M5x&i9#%%k4~IX}IR$a;<vZTjR}#wYt!zpNq_h~u-pkX~a?9_uI60!0qq
zY}>mmPgMWlDR0Q&EF|Jn#5X}Z*(a$)!BRBtl5`=Jo(}JCmS7ZIu?UNUC*{+V!+PQb
z?Qrp<N~+=s+Ls2?fS;aaSVbPY#pi5vfAKuV0#zpVlQHS&>=o$h-`WB~*@~xe9To3c
zw|c`*nI-QXu;+0M`f|{w_XTTA*7Q}u^1l|sU?oMzA|I?xK#Dp)eJbufh~?6n_F@$x
z(SQ$Z+`g@~7wXKmi9qW-a{_RVq&`fhBlj;_bPSWuy@DKZG{sKZQ#90YJ2IdB2gz+k
z0?}V~DC`0n#Y3cGoUi>@)j`Q@eb^G&E}BhzFg-d$EG>H*(C3~J!f<suj~bum-m8y8
z4*(zyB4RW#A(Az)?Gx@iyZ4SatrtLXE|uZvT8zbu&kHdKm#>2)bnGcdEeE~geMWEd
zpCs$HOI}?Hb8Q!;S7;gAs=vC##wdl{$*{r93HIWNC`rvgL0`2L$%Ot^EXOQRrQ45U
zW0dsKV=c#$*Vn`PbA?CBj5wbN5LG>t`ml#JBqDs6gOlfNDgUGx(?ORGF0P2y4zMb^
z+j+=X9t;3uJzB5}2vp;cWknS){_RWGp$>-E!%W`KR@z^!xQb0EilBa_@QK)8(MkQ=
zU1Z86s!RUfn|TR$15sq6P!aw8l$^ODZmhvd%CSl45L0)P^swtRV!UwcXCGCQ=nGAm
z!RgW2(G;r1Xs2f&c49PoydwDs_Y}NdK25I&-@pnN9_Hk&sU1t;7wy@_H=xFJX)yDP
z*|>PlEif+QLT8OtPd18GZqp@OizdTjF)z<6^!2sBqOhprv+pBmAQ2qI<ZOs64mTxz
z-eho+z2IXWs-(3Qzv44>K2^lG_~Oa7IxA@^uSG+QS16oqFH9@7N-*JLV*nWtsUY?h
zJ5Y%kel~3wP?Eg;*_P~=y^=faRw@98N3>MS9(>?g%n2&zOOC#^V-6jy<W04+ZvED-
zVzFORAKtExosv|PLkFTr%vhJ%bGuoL`!M=lq3dSh!g_Mhwx%P+CmXCx2|Z|{JP9SY
z@8I#1E@&T%jeS5~wlh>L1$itmns1RgE^{=&Onox&M3Uo}b)8UwH<k0CbNprgSDiB%
zW+dfjcY!pV%F)NTp`Lbr>-m;i$<$q!?YLR;!pJ~L%w_p>w@FoErR$8DPT7+c<?OPA
z<C0@Yasjz9i>wUhD_AMHhYFv8aeUn3<$HDJ;G+FmXFA-(U25;SBDvZs7^X*y?!7dZ
z-!%8cjcNsn$qi5!t5Ih66GgQ=?uJHpQAl4$h1pC$`|SIWVIYA8<>kR4I-Af8(XqRX
z!IJ&NfedPI_QRGLc#;7y51>XWv6er%09ck`S^QWq5W+eAT2>ZGGvoBcI`CjqyD6S2
zfS8)vz3!AMb57WP`>0m)?!bB1B_y7>HvgvA6eR<U;3VKd^1vc=+<u^uh~UCUAazE>
ztAmU9liHU#lqRw!VCtl7pWj2kS1g)|F6g6#t5&S?Fn!6(ldMo9ALvn15UVL8HFF#q
zPrKr-#3&Hq#-BlW>;vSXh4f1bYKpXd6m@6#Vk&FFSLNMvZU61;TW2@dU#g#5w(lW2
z{id{#Xz480t1{}CKsTj_#V-+<K4})3^r5k$-GHx<35+Kq$mu3@u%uUHK4zCkKR@ze
zY~UPTrrBN8*mMdld~b|GxDPtmreE%j$<?#Q5TgVeR$yG4Y;mjWbj3^EY#vzzXUwI!
zNh6q5Lf4<!?yscI)LtBAQnp~k4ZvUtZDOEC7tu-Ka;DwVGU~Mb(ytm%Hd-cLu4e&v
z!@M2-dJYGs`}*qLy4D60$t_jH%S!GIh6rL&f5jYBm#H>Z9#_{55+Vt`%N<_V@MGzl
zDWWs9w;o%6%b)(zw}=!6>RqP9bOJj{u8QZHfFB<NU|&9lxF6B!I3`vB8u_5v(KZs(
zqM1|M>s&mmkERRh<jBsr8`cNE4#n%De?L&cf4Z$Mog_c!p3~+<j{vEvQ6t|B8sp3%
z`@v>`aC}f2O3J7Jq^C-?gLqpKVdQakiqDNY_15{8WA@vM3mVVdAY|z@Nj0707v3vX
zzTLUR=oZx)_cpe#iw&!25@t^DH>HkYRrjQS>Xub0a#OJ-pk+SMCePX2fH9it!ve+o
zOpeDHr{0m!9F3a03+@li-(~&8HE&$T73}YZBx!XTo6|PZ+=#3nSJY~sHitc6jIJ`t
zZFw)+Fy0ev8$E;@uC`nmP{UxN1|-TTyL?MAevNzeR_h8@P=_p24<ZRWN?m>xbeZ=V
zlf$K$p|t2!Jt_}he?5eMeB=N&J`1d{QY)a???s_RRRqvVDi?sY;jwr_uSVRJgGtaJ
z!dA%<ILCB(+Kk|rFR~MT28=zamqpS?_x^dfme`5vG+pM|Lb1njTjAFV-6q<p%WLoy
zn|mhI<uyQQ$kCX@u}{jUegsb)qv<kPT3yg!L8R>#{*%s%1^M7{wy0@T{NeQ%Y>pAh
z%hDSkqyZCHR#s2OIGnmsM#A9?{15W|1#jue&FX2<5kh|d-fI&g=Hjd6Q*Jb8<9g{h
z#z{1-qJt$|&S3S(`ht!;_ejK)93NAOhx{*EcFD@0`Pu@1PMu0Qb+U74ym5y8Vr&2U
zCYMcS1k~h%*T(Wm99>6@#t1^$nryrpwD-J4TX@6qjcKmivd7K-eGW!~#aw6Lh;3@v
zKha6Bu0s<@mU=Y9&$=@Lw!u{g?Sl`>xh~OH!9Q<txniH!Ytz-Nm@05VGJ4LJ?e1U4
z42>Z<>y+7t%<WO|)KJ?YmcsHUCl2)ILS9Rk1_Li!+~e`8LNR|?d+53e>5afl-4BNI
z^#<{EPbRc97EOVnQC=<w{j;~w=wL~T<h@~4N=X2(Zp12iSA5C7-_Q*fB&@D|nozGp
zUG}?90+Sv`mP0KjM%S7>X|n1)sJuyTBW{sC`L>Fnr!$xDssLxGCg*P8tqxdarSOCG
zH@QfYQ0b+QVZ&Vq6L${8(wx8Ngm$~4{Y9J7Iv*Rj^hToNFJYB>)Ma-KqRVqMcH0b)
zXzJFju+_gi+`(-XG{sx}znm0V9pY<rsq1pZQL+y5F6bU|b)Pdx=Xb@c4^wN(Q5mZS
za8LSGX7E!Wn+%&i^@*C_ghZ5k3Fb^f?+d<JWh9WI6H(Ub`;&qT64}7O5YHwmt#oo%
zH!)84azkpB@K1bHlhVSFQvlAYzw$DbQ3yN`C<(TZII6T|TZH2>Uu2y<wwE%x%2+Rh
z3KLXDZ}&vKJft=6cEy>*B+}(a8<3Hqy^~17p`T%&gRZB6%k!jPYxGDi0NsOdgL}NY
zVy>h~osECtKtxWhf2igL0n~H=QXmTK<L8mJQ9rjLnCa%4r@~m-(I4Y6fH{F;yxn3y
z<SM#p4{(ee@wU~waXpF_-?^m5Iz^x8->$&c4dl<(=$Z8wH^fE~1rNwHK!Ac_B)L<r
z8zx>qM6Ej?acR(@JD_0FB)d(|3ba29P*zJ+<w!(^rAR2kK7!nN*yhWyCS4dzBA!P-
zmgyK#c?Fl`R+HM!`QS`7c@0I0(c?~}Tojq|uK)CVM|B(PjyF|BG-*`lb?<Jas^A+&
zo6$`1*8U-rGXuuW+%)6&(ft176C15Xz+Ln!QZ=|%x1r#X$Kq8&$Me^SG_755kQciR
zZR@k5X07noBklMO)Yvk4Qsr!{58LCFiKmD)p!*5-b$s%5b8ubwFFA%(`rms89%B0x
zZK&7~<_Cy+cw#)ujv6>KGNiI*sH(kM*(4lU5);Vn;+zR*D?BWS1qn)wQORf#(P`l$
zsIe!#9Wy=5&QM3;faY~puxELayv@G0S==$svFO-s9GTfNW<EDw`u0+4VDWV>RkS04
za}|!9a%N>4bbP$J<^^8l6b}7)eEV2+%G)g(@u9sYDeJwM8<eO4Z2|N8HYI3%UD@59
zAw-0IsD0j_tN2}b^rzKMtcgcY7E>Lauxa0Q3E>>b3{!Pmqs&-6E5tF`^l{t<>z*!c
z&DPs`PR>E8YyAv2SJl^A>nyv#DG|7rsq|}jaaG4t&V=J`JxVq$cNk74hi`fVftxl2
z1nOD8xdfZhc;|1HO%B#?Q+PtICAIr8+bqrO$ilSXoz97abM(u()#g+sFH4BfZa<s`
zo4~aN(S<M=axgQ|?s&Lb%x1UobA6Rb$jeEMHr%X>nF<Gv1M*V~kFDp6pjnW%;2`qi
z2kocBm+~&%{rB3E+>ND&3OHsc8eVgLMyW;iu+0xARN#%MX@AuSmqBZV3!Np~_A*6L
zq}3_N`6ItrVd-?33<}-l0C=W3DXX<s0)pfUPBT=~7YwYezhs~l?+32s*Lot=e4D8i
zr9a@%|1Q6)R63=}O^c^?WF%FNT7T#KbtTvDSi|8=J9=SmyDM`FGCzs-Xq&;jD@>_@
zy11$$XFsjFxd+n26MpU<K!O+%Dr+^&tbn%vhO9|erR=SpAO5F4%?n5y%OLO)$Mwl2
zZ1r+;EGXO)T<i(ecNcM`_EkEUX7bwVk2#cQH*jWvPg%kTNsHZ~m;8y-eMpGYrIJng
zIZuXmma&Om*$A1Z*ue#)xnzQuKnf@Zk{1snWJh1~Ds98)!}-$>$Jg4fron~9Vt@P{
zo>LC~a)%WSZR_-H*L<uICQ}8!-hF#^iT<eq+>@;=$M_yz#}Ea?N-TkP6l`^MK)vhK
zpA=Zg!jMRxh4MbKFmOh@SvzoP?DHO`N3@wyECI|V$Eef1?#uo=S7zfudf?bT<q^I%
zA<ZA?weQLDwaiY?(^26~%}3qL+61ziZImzmbYr&%AV)za@898N_W#0M|8IC%T1s6`
zK<pp9{1fo~?<8?%&VS+MAJELs%Af;9FW_ioV&n7^3I4-vg$(S(P5z1YCSd!={uVa*
z&ox@ce-<U246H4T1Z;jjHo*_^{m&$E_Me6S4PG{}cQ&y8KLm&WufX{qy!<}{XO91J
z^Zx?QOzbS|{}RTT2pCzI*_r<J_D87y*GmFMR!#=i|5xJt3@VSKJ`V=9e`VuO*xG>%
zfu$YL0S6b@!R^0uMPeQR3Xia{`Evwpja_wTUOa!RJfo>F$hqcTm0V`|M`%j~OAMkI
z!z6*Max&L3HrxRZ9;hg<r~_73;!IRlUWh9zm#0jz^?r}VmoE6n$fgi*fAP@_;`obB
z>8H2GGq1?w8H2~!*8$er1FW>ZuDZQwXaHVRR(gFigK+*_ELxjZ2JmhI05cAE1~Oa{
z#`t>I+`?uTCg#2;7C;l33jiA*AAVnN1dyniUrY*_nA@*2z5#Rw4QF<66f6&jsSbq8
z^J4_GDlfR9frk$|GBXpqA-)Qkd307gECzAM^wb*A4c-Zes~t!NutyP$0%H~ETOt}W
z94gP!;PjzI5Y+hS!t?+F#1E7$g^Zt@{00%7TPcqi00%dZh!$MF5q$0QH_dYkAn{J_
z9>7Z4>X-BL^r>D9&E*%DwXq>keG_~AeQhH%c$&r<0ANId!^G6pR1^R)_q<++D4Q(7
zjXSX#u@pA3QEi{zLNq`E(O&>!yO8gLW8*z@i|dQQ3uB9WYOxeOo1cI3T4q$>)it2Y
ztn4_STjUlekPN@`tG+uq$Er(y8yDSgA=4C>V$+^E2Io36g@y(vJ3z_EZzEtd!q1d+
z;A}wc%F4>lN*sVZV1N@F!@fIezw%I?8+y`pdPWX#e0t9~o?e(Nu$hrL7>KXHgG1v3
zC=d?rb^!0rFXaz(!n!&@X<C^qz+rwzDel7Utv<uN)oqLoUok0HKR9Z)j}?H*9-ptz
zLhu|0rMT?q5!?G7%?Tn(N;<;gzc+G!zo|q-1g-$+$!ypFkh#&R{bFO`dk2QVZ(eIM
zeW7o94!#qpFg3n}uHZxGTucc7wzoO`BA<JHX@6tFtob&Fz};*^KoQ(<00TDml<1`<
zM=pYVL|%U1!+kg(d~ct8-`#z4eSW9L7e+=;>Upg7z<&8Yh&HFN;eD(7jjRq~c4FNT
z+ytzA>M6nQY`@dMvotx?efDTtm=WJZgsr5ne`twIkxNLq=9MKBHG~gsD?Rk^`faYO
zuK_0W%ck_$-~b?FQ=fm&`ln6}VtG|?_;);}LP2bmeTO8~G6SZ6Y4W<z+4#j56vTrg
zQM>i8w6y~0TJO-Zg0_FNZUCT}a>p^hKmcUr!1ux&ggsQ0Y-j_Z#p~twAhP?3Jh1Np
z)`oqGb^@T?Zim7565Z188Gy<i+75#0yFX*s0jn4K5W@jPKG+UI>c_uE_i7va5;LH;
zZ9`0me6a&LD!lxhLirZy)Tfi|K}?IhvjsW2`;UcxYL)+~ZThG7^QU(9pIVlGPF49L
z;;+n|=nX=~*c^c5&`Ob?Z<Fop5C2@Dx~?EToE70iKArPB#qops+w*5@)^9)kBeJ!3
zN}8W-$KZ{f?3g~h>e3dir#Ncpf!!F~k);WQ3n;rc_0g7?0ddE+XXnHJJA8Z?&IP8C
zmDQpCMF`9kGG~U*1gA&%ii}OP+4lu(x$lA<M2zVj*x@T6#g_wge>*O6AKmFwCi*h9
z_o(G7m2-6HHr8Ap_@0ko<=ggW3>}-6+lm&z<yS4h_*d98)YRq~JOo|pBR+z*eI_44
zE3U6GRCkVskLM8(eiN|mBff{-Ro=F7Z>YexLSSOucNG|S132Hl%Xd1AY_o6ScQ&*v
zgO%q^p}&u72C_DUCw3}d#z%z<T#pze&SW3x0SpUk^Q#;L+|+i&x9IN9yV$)R<9ZF*
z*DMg5orrHJXpaPqEWg(HHyqV6J$BwXe)J9yw>=PF%=aC5>h#zVAMgm+)SavP&3pfz
z^)AGRkxd_|qYLn-J;24lAw0xY&N)89#KdLn7;vu3H*mnF=LcZl;oXM}+{GC3$%||0
z_r?qvUChoE$M<7IK<d8lOU<<;e;)S?h=mWBF%g2b3hIWJ8l~XHa3c>)OZyfQV#Olv
z)S)y_=D7(Wc<z@?zreywW0XFFh($Gtlh<%&+Sh^2piT=9vp%T?)d;3wzNgYd6lKl#
zu}KGK+yaWQG4J0*q`Gi-tdb9?O?hz9P%;6dBgUYTK%*8f^6#=C3$@H^1bt|V&OQP;
zGM6ukgiQws)G`s-wyR%xMUjZnhoy;U`BYVGZy=}^nBLTDI3h3eMoQ#dzxCbQ=o?D7
zgZK9mt<6APH7M8!V@hD$pY*-x32n5CrMr+pnkbf|)g^#NPL;`puXX@71`#7->l}`g
z9aJNzoj_PVT#;qW^MxMU2a%cDLMT9pbbaDTve&VVj?A2vp2*Vna$bi|%8e%o0jH)o
z6*JJ1KMaj%uHhAwT6iv~YZKPwbL-S6Q(X214Nh9xbW;m<dGb7E`U#(<HioJWs|wv}
z;1xU#-EP$lY@3!~t*?m?`oa&%BVpH@y~&DbVBn<N=-@N1hY*<OSCUJ%_UBKM!yu6M
z0WX$^O!*ynsD!u(m&5t%bS-f#L;<5XY$talJZuD^Ia`(VR7n5C?L;PCG~MOkBptNf
z%e+b)=x_>o;u5vF4Xoey7+PsFmUY~J^g7EV<>k85FJ9vh;Tc@SsA;VGzPwiT9jz<N
z3Ev}hCil8`B}=KRT8bvw8;uqnJs(Qd%mZ5SMP+=p+g>~CGB-BQUZSh1Dr{B)mkzjR
z)^;HdpmCyFgz))N3T8@?YKD~J!dH|QaTEF+6>N;IF!7B>mcqXn+qfqinO$P}er*Mn
zt!_Y-eax48a+g3Zb+>;jb5zEDhKm#7CKC$r<IDR^!s;OZF<3sDp}7@H9817)=gZxw
zBf^@{j03c13>agq9oix~yhyH2)3(h`-usY3-<+xq2JbW?5K?Ur+G3h_g_LT{mh8LU
zRs?%Cs}v;_5>~|YS%WW|p3PNl!TyqmM7%>|=bY>3=5P|h4SA~_8=To|X6&gwR*YW%
zT`qLhfR<-X?)$||xJ$7VT%!SyUyGJcc_QWAnk;QUne%vy{suG;;3JD_m8n@0ZjH|;
zJJ{dsDQwu$J045#KUfVMnXvGFCZ*X~Q#m)|`D-(%CX)}p_ASfhg;qy_bDx~Puapb~
z{FhP@bm2+^b)@+y828PF-Ts35VHzSGM5I~&yqbgOuoJ*}we`GQKrjLb(qAE^mhk(-
z%0hVChl#&j4B)Vjk%_O#k;P&2rLl}b)hNmV;RUyUc$MW&Onc@P<xD!?vc|1YgkS$|
zhd4t@*c>=~oHn34nF?Ukv=zq`++*)us@ZmBzv)PnJ4NTo+f0QeG-o*4L|H+y)*mja
zs7~w9Up`zzelXC?EKCi?fTR<tO2!tU1h0?FWm4N^Zx5_S^40j^9X&>aUS7hJOspX7
zF2iwc3x1Pyv9NpoN;*tPwB@T;(Y9NmU3uPbp)H9IATF9&=+ZLY{F(F>y9`94<e4@R
zL%k3C9%lVF<_W!=UYJbN16l)uC74Qg&uHhx%Q{TkC*=pew2AUP9>6UoU1VW1nJ0}s
zLqAv?Z_^J!%@d9I>u`E!X?jw^vUYTtDpST3ln2{bdLEs3NCH_PH`9-oFOZ`MFRY>G
zBY@Uz+}u%7k=;V0FU{efl?N9Tl`Vq{*SChidejb6(WO7LkZF<<Lz1PT4c}ira!^U3
z?AKZT&Pzatq`im~@AUm>4~AQ}l0&M$b4KmgEdK5V!8huEW|07Oy;nlo8im+OwTu@I
zB+7y@bqF1#U;4O^j1BvJ1RD5EJIEp)(mNaxZrx&ZE*Ppo%}Be8@wtX8m0t~?K@|Q_
zJ1)EG6tqO<MPe@k4H(V|)K(8y2wA4)bwp0DeF!1guah|DPdxQ@w7l)MvN?~V5X^qL
z0@`MN{tU}YeAdP|zHq3^Nl4a13fesTp-oht+061DPhj5LFz7*Pm>wf7YG=L?+oYbI
zp(<}C#!=q#lLt_UA{dwPdrgy3;H62_=|{TuPPwJrRtq)6l?(ztwwb(o!8^X<q6W*e
zLVR2dgpq|-JUo2z%MI=~Kl}AzXBLurq<AyZj6i4=SlDJg=)4#Q)6LGUn+CfeKyWcz
zP}GK?aRr9eV<6gFUBh!K*))SH`$x#CBn_OTQU+G;E$@$+i*pOm)8fZuS<Tj>T+hs|
zlox*H2kc_2+VA9prn!>#StRyQIPrBYfv%gu$V#b$d?$Ixe8P%D4g%M^81WFrVW@IM
zj)Oq&^_+ucC{2uBC+I5}zE1g1(xb>fdqLvczz;F&=7<Lj8yvEcpf(}R;nxkswABuS
zm4#}g=+mh!)NNkKE+|FOgL-#)VT~hgr+2yruDi0~Vo)iJ^gp1+!QCqsvG9gr2!P5B
zMnO7!K>4N=qjhJ3U}k3b3W9hjq=hDslZ!jd-orKWGXVn$U(N4)1)sK@7v@9X>V!hs
zZc?hUn(4rI?0iV*&Up}A^E1qX0$t(zX^NF#+WtZ1LIID|_5spAMvNvkF=9^ZaR9GJ
zZjP(@;aYC#DPT@j##w@pJ%&4om9IbN$_FOanP>@Z`7J<`nNqQhPkdE}HO`j_oilK)
zS#cWLO=!E#eFWt#nXZrDpr*xECVp6wsCf}ahwvVNu1fxSUNiuHMBZIsTUnvR(o?%l
zYo_zh=^X1HsbqBFGNWM8zlFTmfe<{G7+}zw8DRA~Ek~`lSy?lO&GlmF!*U@|Jea@3
z_ofK96duvymh34M%vi8NvYb0i)9LSSd7{;w1u>tKOFSQ)Z%9+RMp_T*PnqRpN5$@a
zA_e3~&Wgrx+iK8r11YB%@!JVbG22WS+b<`Lg2_=sUJ4p&&m0+r{izcY#TW1(+37Os
zv}Gj8SXx2eK~G!|iK%6DP2Sijj~>k#6syH+umxFuPuw(93m@Hzk+jmp(!D=J`u;@y
z3o^YJgTH&9&=9x3l}4PM!+^|)b?3e>8WV-8S_Yq2zyrbSL}Rv~T)r^QVohc=8K5ir
zlXA`_M!7Q!Hi`|mT)eQz>sYXY6?O2A2sZ)!VoDIt1qrCJ&2B}5y2(w6(!rH>J8qU}
zKuLlw%zvhl1j^X20QOH`5+KBkdM}!PyI`ZjOYGG?`l>#7`pkzusJ@@*F2k*Q9uV4l
z&C0|6O25v`FxRvD?9L3fxa3xk7EgMGAiz|0ioDx#^9)p-(F*U0P?{zHrqp-eg5mc{
z9l^c3?W>|u!jReF{XQ`IjrssH&(yTiD7us13<oht3E%Cma;zPuJ+Lyz9#)giQk?Z5
z;XydgFkcRbPp?N=vW9$LKWN*A1XV()XMH{OZGSC1=I|I|9q&LXCLc`b4(+znt6A|E
zUfSd^=$mF*P)Vcwy}=u1@uJOnacI>yzV1q4;zUWcN$+RlIaY4C6=c{#Oes7(buTqa
zOI*{!d3@Ow?ev3L1Nrg|<`<OO)c83GMLZxM@Yq1MT~jrLyoY2+m&r%OtmVKt?Vu0;
z-a@%9A$O!1Xc-jWq*NR>)R_iLb$`?Efq{iiIkD;IG_DXBm(VV}!Z4dF)^Xk?O=u6R
zxO;Pfs&#j%Hp}XGRhu}_y2H3}N``Wu&Nu-E70#*6mjqQ(=-aLiy&Z0J9}KJ^J1DpW
z6|8Nt`Q=MT-jjOcr^xj&UUoQ>33}#IQrBRO*}Y>bv&B+jAra2()8QjBkXN00<lnj&
z1qm?xNP9?AyP@vKo51i<eV)0fI>~ggD`34a*ua=3d5E}kPTc`pr0yoCQn5?|6=5!Z
zB97e%5O5*2D{6IwuLlhNp!`9j?;FY!*oxDjr&9P>vj%7<F&ZyCPnfI@*s;^jejb+J
zX4{m99cVqFYp<|l180?AD5%+eu?_&U!0W^2nK%Kl;OB-7hcZbWwdTyO`!v|AA#XKx
z3XKwN&Sk(Pxs=Nq?RF(fLE-sZg|GpNT;<O0On+jWrnfVLc~?Drx|nDhdgz;!qQ`rz
zin2U@gKKnDY*8pD%ptK;$DqP`{ye}Q&h+m|cUQNxmiblCiMnJWts=AVpgoTG)%CvJ
zbW5(ih{-vaZ?Km7z9u^C^07i6q#k2q?YNAbKZyG0E(yAX)5Euf-S?Gkk9FKhCzs`5
zog8sjH>TZ0XZnoT0q$r9U{SlM7#|C_26@A?DS`;#oTZE|7}fJ&5a1qRxipNwjhN6^
zIXhJ3@nOW1!mvW8tl-5!kL*CW(qLPYp_AvFy$mRatQy4#0mbDSDA@OQq!=Uok!+|Z
zM0ih$L%n)Bed8{q(r{2xt*njbJr*Ejc!>tAbd)K0L@icEU;_1Au^4XOHsu$x^n&2l
zelfSnZv>~wq*&LN+AyaAkWP0#Z4?0mR){*&Wi;!z!N+_NfdPS?6%5(uuu6q71A(xO
z#@<xZ(Cc(2`Qg*Myw?p?-t;O`5Qx@igr8s#`BC(0+1m`-{Z2NN2^E;eu<=;;lrFt=
zB7Z1IvOgun9POY};_^;s?y|FGN73?=A+O$$(3Y)<`?Z^HRMBL#zqq&B8gs^0AxX#+
zg|aycvR@}=-fUvrHW4iIN|JJqx!l>K3=)#Y<pJXUOJNU%%5k$Rem-Pf4$_rwSAQwJ
zuypx4Is_8beP(q<^u-cQsz27EY8ovHu6;M|QoC~xcLFj1*FVwbz2?k8)?#PRf?GQ@
zOkifJTiiC%7q70VEcf%JlY@?DB&}D|kJH#;#9G@e_tO2sG!}IQ#LDT$&aV^5p3vpY
zjV!GaeWWGOq4=O7lmy)+0s1$@%anWKCQovrn~hwo={oM=`4o-+X1ZquIp1B}VKI$h
zPcVUz7tRudTq;LKWfT%eMtYk=>f9hZVb)DLLf4Q~8hCmHGV%V`<53Hc^kQIsm8*>2
zt6U`#;l)u-Ci37~KIGxihu|TNQiT;!dUd2~JxIG$`sG;9p-n@{NVaY;^sa1=L95a4
zt5nqooWL{trN3kuGI1Fp220~A14Kkx)3zr`HVzUgurFjk5%JD#L<{}%g={5jk|x<K
zTG&7Nx{gJ^jdnz&@)X?hjMGaPgNQn}`17lpve)PdNvL?M$TxgPQxQkIiOh7f4H2S)
zquXteCD*5=O22N^!)O%gt_!Me!qNDd9xLwl6Xkui%}saW`zzS@bhBNWfDF?rQNkUC
zumoA_hz><)qU*r$$Xs`Ubx0HmLQLLNW7&ULX2d*AGBXE6ds&Fh3Fph~Iv`q%l{;Ra
zrCcS{eM3rxVi~XWti8%>HM-9dWVgujbuM$e!$!ayj6&^M0TMnu=<QMEQS4*CR1)2>
zEwws<#VLddyzAWC1tpiACVln=(bQ()@4B>$KbBdo9w6ZnNEG>+v^WFiNy}ji)W(oC
zNe9nc6przrX+#VvQ5|i~3EwDNPy)L|no9Ew=p4U489Z)#TT2`IHyAfboYE9CEOJOE
z(@;n?rUh&8dU>I}$dGI2Le#Uj(AM)E&^zOgatG|aMdRdTE2DuLowZWCF5P_1j^HN}
z>=mpTMR?}UxPFDMw$|2m*HR|?<<pyeJl(yP6XO7esBj|P!sgn_O1-k)yKmihJPmen
zH1=W3Qc)-pA$;aJp2lF82`CZ&QEY#jg02tMOrnSiwuO8Mz{AwQpg`PJsV*Ur_U);L
zTP56sh-3A{VgzRoluVo`r#b_!Co@5)I%D_b9fW5<bQFBM>w600OEhBxcew=WE?tbU
zCuN=^JiZ_}4@wCScK+xMb0Yp4LjiWvwm=jyYn8V(K-1vRaNvwP?L+dyyR>KvM;=nL
zmm{qAah5Y4-hb~7(%hzXawar%{zDl=48SLC*cSD(6EO04=9n|{ejDUzJ9UaQ1CCtT
z0Y0JM!oBTHDQ28rS<9dHUd^gKq~Y^4X`L-CriTyL0~zX?>Bf^dEIi1<VW<j)1Q)o1
z=B0!5aO&*T%-V?9A{%wkbZZ`o3SD%tmmqGLKpxKN+2S6{&Fu&eQ)>EVA^?YKOcRQU
zpDuybY7{20@r+=`@Q(M%WrGYpiQqvQR8!Cs`Q9WGmP*#@yAo2uiZs%2)+DdpC2o2t
zCzXCQuf2j2p6AiFR-u|l^|kQO-W)}alyHP(A1aY<^JZZG+qpVuveb>SOhJ63E{)a(
zd|Z`GI%f|l;89B-<?^%DjeQ<(9fQ}3`gGQy>F?wfi&r1?X(LK>R+Jx20-7$D=hC@`
z&BQ4b@qE1MtN_bqRUIvBM+Xzf7AO{c+1rgrjg0-hWl{z*?KrU{PLQF3sA*dLP9@k|
zoVrxtT<eg0+cWYxLuZ8#HEY6(w+50pkvmNZ;;BD4s(7gD$*X!(I}d(KAhuyNZ&<~p
zet(wED>ECiHjiM_B6?R`xPvkmBFrBt{!LS-;SGMrb`}6J6UIWd{K(V>{KEyzX&$T^
zv*ck83RwMI?bB(I6fQcs9_7w}u;JWnq6RMev%(YGLaK3oPt?SqOC}_K6*=!1eWp^%
zcLW}xpUxD{=owbU(MS(Owb#Rof#n$v(9dpKRY<D-M%Zvf*qC!FsXoOok0C?VV9is?
zq($)T<^25Sxd1g-vo(hEOV;OsMO@2-ZpY=^aL@dq%1OgJ3m6OoK!O<c(xz#DN#1e@
z`6rLC(SrdjyS;I{#S=1>zlcMlqU=PTBbBz@Mx)G4555qTnuq%i;WTHzs^mmhyKAa2
zfW0rMw-px5p>vfqQhD~S%pLcqOD$P{Q;7OF&AA*R$lDiooxx{%3T+BXq%Zxk&N#h^
z%Uu=p7gc6W^{fZAtfYx4kc#`Gno~d|&l<Lab4Bj`t!W9X2Q6aON#`Y66!dN(@V#H+
zH~`F;OH3AFiOqH&D>P)cx`dN_{}I-?@Vr3cn#)&)JARu(hd;+7Zg&{cqlU!u_viW0
z3G^LWm0JBtWw5m|yxE{cQf|5RVxC>ve89g)Wd#C4Fr)vx&;Gl1gON)z?X`MWj;~#^
z0Oc67O?^?vfMENR<(t+9%;qWY0p|;#r-XlcHcCM}YADm|8`oVFvH*N8!VMuJO{j6^
zVucc{5A3(7-CvEfmS*ztXTsfzEK*mH1$HT++5$GZukbe!4#t;xmjA}sI|ug?v<=!Z
zPVD5wc7CyK+qP}nww;_LC$??dwryw6^S-rP`&I4Mw^dy|-G6k=-CZ+PQ`3E2_qTKL
z0o7`M<VSnd%|&+ME%&X~YBM<VTcjIRjzMz$3bOdgDoX^-wE}gHrW|t2M%2D=Ack)8
zIFBsE4n4eL<*z`*lXbStQ8sK>sf{r}QJ!wry#=0L&RvBA_m~p&cPEMroRQ_$>Sdc4
z9{du#t<us0i!16zx0W8h+etnrYI|LimwrFS9XWx=8~^7bxsejck@6>pc;Zz@pgk2(
zkJ9oMHhQ1;30!X5xcn{KG;`2m3ym*>hb}vMJC+`o_e1k$3Ez_XW~@Q#TjQH6YznV4
zc05il%U|vbmD_mp7P+abtqTN6rJ^paL*QLm!^<!&EjD{yU-eSv5&{?7fq1Q2*9F0y
zrAx<D2qcK^3@3k9e7>}sgeYvZk3~3x?Rz>@GkpF(5+M^@dXaFzKgXdU1rQaPg!>m_
z_f>1EiuqGbveQ(17Mcd+HXlaexxUC;37}c!07)90y08-{K%9+<%1#`snLrp4Qsgj@
zO;ZMDy4BmZW9jzZ<`Ip;^K51t-5vf+SgJEBb+(Aj`izi40shX}@7Dv>bKjlc?Bh-a
zp^-Ti-W1w*8t6>mlOPZb!XkC>hIhPDj<KO)P)+5NGQs&$n&kM#PCc7U_wA3O(dbmT
zt?dRrk&776D*D7zUECNj!@ZWW_le9@(KtONrru2C`zca)n;&O$cc{8f%KP92XS=n)
z$Bos^o-OKHW51?_a6+&YuAKvLwJu=WYlD$<L<knq&6wnQa==?lyGZ=UX*#n$G+eRK
zZ_+#3cw5mYz7HpYZFznv0l$d<d~6foaH#mRXFtDVSDv*l+CRl<9ok+s)#dTgDIFN8
zLY*D*A6duG+QEIG4N0!3Qy*#w(8XNG;cTRpMG~pPOnQ?O1T6WS*2$t&0}6twp~(hK
z1EfMEl~i@*4T7)BVLW%pQv0BQLD{x@e1<f`VzkZpydTN3J!e3u@Lh`3d0U0w34g~d
zb6PKqKD%^SDs=;~mT5NPjbsM;*Hy7daMXuy04&LATwmHt&%ImgylP7X1v-Aq(>VLM
z<^QI~>YJRPw^~%~by@6Ru`h^|&{wYT(@X^Y%4Yx`{~}nk)BQ;uf5Wt|Up069r`h*?
z^p7^a)|WvT!{HUGBVWXO8Xt<xAF{F=-HWmg^2VYM`)i_T6Z7MFg&4_&97mF+$W*>&
z5Ym1arG!JZJSO(;HX)%G!>Iis<=m{9cA)VAM^3)X{zddwlC4-Oy6rGmO#BP4R<Dpq
zY6ExO$DS>Lq87d=9APNDtaVc$JwF)X_d%{u5S5nG=d5yYZMs!-uko5)0DvI1NuK8S
zh@U2#;9_|edtpvi;DHMe>2Ujn!YV_i)8$U~fhwDPM{`H$FI+@v3<9zcaX9Bc<u`_C
zBVo4bj=UZCwyE^;fnjv($dr%Hk-)m9zkHT{77GpFnOoLA0!a?pD&lOCMrZYqHnW#I
zU$1KcqF6CjI}wIYC1o~$TFzI!j@+GkpRiXhXxg+Un6%zJrMw2YH2+3J5z7vyj6_hu
zXi|Lt`wFpFEw6%~-aAYtoqfuMksV1OABbK$iHZL*n25+fG?H-93R4&B;O2dl>5exL
zM&Rzrd@Gqh4wKv~e;Pq>Hw&C&TYm^JMnYLV8sX(Tm@L|<-w~WM&Me*+y(Dqb+-T+5
zJ(At5C4oR%#`B${pPH7E5bpwt(aK*{2!+HTIKi+x!&%qNoSVo!OF7Ls8YT>1D=$1{
zE3*MA9d43TXYHq^<hJd~$>E8lRFcaT-<Y^yY27i6=FHRSbf7LqhF}fphj&oN4W7J-
z3X@0P(d&hh9PPL*n$r$`>rQYweeSE=?sYzxURn8_N46xZEvosgT2FFevJk}9Ep_D3
zltH)a@%s=fK_xJq95Mg*w{v7A5*`53FO(O}qzx?!g_jp5JzQ0XkB~gBpk%zcG~_bT
zrTtIn=FZIzmR6jhS5CRj_k!VtO_ld1*8_rm#GM^8zEQW4cnI&<tfa6zrf5$$WY<Hn
z*+kD;rD?Hr8oiRU^)`{DNJKweSNvAQl}DiGwQOoxHJ^Z0&IA+N()N&6v35-Oo=?<b
zmWY*mU|;h(-LFL$m?rVuf=-ZgGh<-O5&<41*K<Q;ueEIl_Q1ILRNn+b<s6hvbWz?@
zqc;*|LCt_R9h`$`KE^EU#yko;wEk=6fxLJPD#%OI#zH0)uQU>frFWB$aS;CeG_@Uw
z^SL?or*mCtDTdCF`u5RAGkLGzaY)pl?nwoxp77RL$?izbnhKn%p1|M+Txtf~=T!WP
zga|+6m>)_d#jPw{E!IiYDIP-Kles@gP<l}}_Iu?jg``AnmQmtSC;>8<#aOP<bJeWy
zL-``D2A~Z;llMAci@5z5dKv4FH0GV9Vg_0^t)aI`e;j;!w$pzwr&=CZiz4pT9ipjU
zpuiOl(O?F-E{hFT`iG&__d*Se8#-|e+8MC74iF`lw7)PCLqaD`^4&3~ynZ;(bB=Gi
zZ$iNDIC=Ctn4dZy%%^_EX{e%C%u7MPd!x`s4#ial^j3^tZx_6vz*%=#4a7KM-bfQn
zPJDG7@xsFOpDpGrTa_KIamKP);rDk0dOttmr((!mNNj5>UyQ4bcymw^53CDEe&04j
z`P3abwG-45Vq<J%d$#NlGo~JpsKsGKfRj$QsYK>QmS1e0dip#Y{6MH!hQ@3ZI_n7a
zY1(hdwJMI7K&~hnh5h=JLybvauUV9}5k)XUoaI&IxBbO~jmYSWqTq;OKc?lpwHL!g
zXrlXy6SKPi6Mb$3^~mtze`@L~W}rq{m5Ux1i#G2-*f*nYm<Dh|RTiWY^?KRQ?^P5d
zIbH0_|DkynNZp6n<%}cq$1U6l<6Zy@k*5iMA>pJGf}Nr!kra3Vjg?g;sb+a%6%R_x
zz3S6cFiMI(YQH1hqkn@f2HNm(E`)L%VJ8<2?>6*;-@^IwO-As%Qx@*o*9t%<Oh(C|
z68dGL`()in{Uu!N?4`f+*DVZtXn8|x5R5^hIRs8ZBlS0i1e*Y!4g9{_y#74}o_4A3
zq89Vr=op(vRtrMNxLU*_UsB&v4pHOzh>vjJQ_>!n#_3%we%Ocs#_yqQT9L(V&^88N
zlfpMbZlv|Mg@;?~J}(8-ggz0=Glt8MMf%BC1`Ke~<kDhrXwiW-SbmF}n9($)MoO`M
z89GN@DU(-HtCQQ8LIKt1QtU34sC`@l<RoTB_1GBY0D{t*j||G<i5Ct@PvQ&9QsJqr
zjZ6Fx(A*ov7<;0TK(;rW1XBE0Qdb*Q3SW}W)8@{P1TYX2m}@klhnS5;%xmpryj5MT
z_L~`@1EKT@!_1@uJ4bI9vF>cnOriXDL{gK**-+|jD<#jNDH8>(OYbwG<Hrn&)nuIG
zcy(n+HcQ5&F;mm9bTv<AOdrHnHK|*8dLRycqS!NLC}hn+vKMcAiVK5eN6e`bp&&x`
z*spjOos9LePwiMEVZzfu6na#t1ab@e`AZYIM#TkOF(NeV)CaUw!)I&8YE(WBe2{CC
zs`~|Q@3coIFyE5V+u}eTB`RK_f?z;oM=OUV{>4k)ieu}|psT`c_k<a`{YZSJHae}K
zh4=JdLev*u^^4DQza@eFLvM4(c<I`^W~Ps^mMW#_FXJbLtoF(*Tw;x)A{uRQGa3Sw
zdYW6V=Sjgc(y_`qIMM-xsrW^l>L}SHm`^rrD#*y=PCLfOn4yF#oz)g@M|rWQz(gyn
zds~9lNH1TCUq!<4`So4AxGI-D#DQ1a?ky3=2}&ey!x76y;mbs!MFVJBx~UZB$21Z=
zo(yH@y9-?)#>zclrJGvoZaSHIx;5ZqhqGGMa-Cch=A^ue3_g68R#m(~sw!;@MSOB2
zVWzapEz*1CLC@)6`qlp1r9;`fW8&U=_)Rk#xl`<J?2f0M4@YQ9=xseJ(QU04y!{x^
zgeY>Ugx%XqohccEq2q_7wqGvov%p)N%ksDmJrJMAm-H2Dhlq1_CI{wlz)5*?2Zi-|
zibYS#hHNzYgt@$WV3K`472Zq}i%+mOhsh<ooI$vQ114st<Ejl|Gw2S3N7o1M=$T^g
zxo;F^JJwy8c&;A9$0<0U%w%rJ_{#|I1`NGx7i`Z+Dn?9<?z<I32!4_MMVk$$VeMR9
z1y!|+<e!)5*=-dpYN3`qoGT`6v3M~x#^sNlB&E7a5)n9qt1|I5uKNx<uZAheX_nc{
zCD=85-o=hAQMsFOaVmQAL4FuHypane;U8soICd5+YS5fd!2{&#w#*DUQE%BX$@%=q
z(^qFk4o6+Z;D)(<%+r$I6O!WDB_U{D#5ubPL3|K>zwP{i)3qJt3i%(?<Ba?mYa*GA
zV42bS#8mz;Y{47XS{?nV%`7L7GoBncS)G^rkS|j7Wm7}Ii7Fk{totqOl<8P#ZXQjp
z6DKJ_H}m2l(luDzU&FSIlb9#WjouD_6FoTbY$LA{GJ;z=*{$CabwldPBQl)_#AcEU
z8FF7m+rh07vF_&C5cJWP2!|^?kiOje;Kr8ail4BX2!r38E*bbdGr=FzPngh0;p%WV
z)Gx@15;Yr660YuTPq`7Bq|vofLI>&mz50z`e%cky%g{uQXRzJiJvPm#M44E^D{jVp
z5<X>rEzcaJ%8B7HvdS92Qz&x(nYdj!e~Pe)u35@<vc1^-VjL#_5GBO0=RMQb+BO`D
zYNdebPVi7iNbNG8)-Vl04$aj|Ig3hLDa1<YQ(A*;URjqVW^5_i^Xm#_=pyB-WLnLd
z%Y3s(>w!+Pw?fO`dXUqMoRfKeFhDx7xJk6VG%ee_PXu<Qj&9GJMT|@%7xCgMHsK3T
z@WahtW??0TBIJhh_10RA=cG#_Dy~uc#sI5{(ywu(2Og!qh@`XJUC5DqYIDv1>bXw$
z5!FB*&1c#vh48pq<;gwlF3j(|%c}Ei%HJamd!Svp44?ay1qAfe990ja8XN#zM7ebq
z>_;0=IYRmq_(s5$E$&>~C4<j3)DH^a8rI~nM3Dol5h_^omS{v53eocxEH5({m0^|B
znRGAvls*fu7?Gvl4OGs`I%mlmgY-ksqqxyQ*)D^zJ&F&Qio?k%=NYwd+`KK4nNyaw
zL|XE>)IweDV46hrR!5-%$Q@~sbX67DZlP}RvhhP3)?iJ<8Eyv=2ZnaehnuWi*Xpj6
zlmL^TK$L9jhR4+-6@M_hT%fQ=5$$9Pu(2!rOPq%sOgZSR#Js6ox6hpwm4)>U>~v9u
z#mYJ)Icgp(cALfP$`p;il|?1WBkiqCQE9q)%sy#uDfX~{_$3O0W#`$1@8;@lw{4P$
zoh;ASN`+a8u%JFgK~KVDBm#hkWA|0aaOWXkx%84L!^icbmWDqWN$j=9;69H%>sP4A
zXi}VLs%Km4?`x#t8_pISn6WpqIk(IZ+>LT^(&V(FN*?)JaoK!*#YIqA$5&e%`n$()
zXTL>VrTS;#=6SvZ=r#4GK80<AdFW+{&ejS)?LeEXe3FndEMaF4%cQ~}B3gj<mfvZ5
z5Uj|#lPPs1>!~khXr%<R!Y+5tc~Tg&u)nXw;!2DFHGyL9T#Xv!rIW&snC1k%V9RuI
z%zthQwy@m!Lt!HCbCPe)Ek|BinlOS7og@b>wpQAPBSnd>3T6#q{BgHPjqr~$F~_M+
zCXi7cgz3`er^i}9Jjcl{21}KiaOg_bljz&AYv%4zxE@o*UB%UH)vD*<Ow^TbUPqxX
zr@2zLRv3W@yZkgViyRKrj^lnJXAns<vEHiMzyB@WI0w%s+L3-S8c6-wy((GWlJtab
z_!RsR>ddrHDstBEoxc8^T@HYNH1@*y+*VvDH;;Wq<|@znkEdXB(P`zwD(Mfgmc<X_
zI9=OSi;}g^btHJ>VMc-Ca5flK5PYffTn0z!%BtA?#rT}u8(rGtlO9stRRgjUMtA(d
z<%4kQ{o@l7t)c?qJWB<(*(Q$3L9L&1Kb8<CC@ratGN6G36kgSM)UyHLM0fFV2ZT=%
zn_b}^aZRN7Zup|%TiTr3<=S+f>f3*-ldcu&U?rUPK|7Z-VF9FQ3JxTnEp8J_8w!+g
zWp`U2+C1knF>{w%qL20Ue2Vlc#1tkgTaCz|Q-8@|M&snTvqQA46%v^<()ja)YC#?Z
z&GXowoffw)+&`?->*E>q*SwzjiAfnaK`(VpM9987>?rw1d$YUuTkgW?TKP`^0n6Qs
zt&hb_!3JrVZagY9xU4y!mt|aVr?!%0Cm2}9T9CuJP1+!`2Xh$>noJ7YnG^G&a0AaN
znq!7jf^YfHV|#$vO=J;@HJF`-dL1h~TDGrA&g9l3`BG}tq6tRbt>iIEa%!o^6nnj0
zd@vHq)B%G!Un;dIRR5EB&LX&$!sioW@CUn>o+K{YS&cz=ZQim^{>Xwa66XDa-Tf8K
zFv7C>A6g`RC-d!Z4aSN|;~#~0F9&1ec;v85J>;+M8SB<?3P0G*AqWCRp{3f}35Zg<
zK6F_g5bMJ`%rcs{4H%yMu}7JYMX`N?CLdf=^A;`1NXV(fE->FW-4+Y&q`v}((e3xN
z8_&3l<n2sH##sDu^*ukal7&6~I2K*|RG~Vbgq54#6JJJ}x&@h-urymPRx3$ktv@T*
zt;Kn6HR@F{SZXN~@=NWl8JH@Ul;HPO+Hc|HAD+k|4}t;+NKlhUi1dvN@^<+;&74MQ
zm$#O1q2|vUtpzMX``QXx{^Id1eSydw^-IhmgMDO_wtQxy=3go>a%SlBlsypAArGNq
z!ZU)p(lRU1i_E9ziAYq6E0EN}!Y?G}-U~y;{nya#?kjwJ2d(1uZ3o)t$j;)m#_Qa-
zSoI{8o<_G~m0sN3E-g2Z-zmY-I<{JhuIHwcGmkQ1#H|f%csni9DHZWYc!m}<$J%OP
z#QlX9D;fms9+=13P3EIIC4Q?bg2(?Xq}yGvg`@Q|j<3J2t4}=2U}V*^<R;ZQtp`8k
z)izyk(Ej+`;X`gR^gKnmP;RR~H@;))cOQZkW5VUDPMLW4>^}<MIiL70FIVz@72|5u
zp5nXgu@uo{%dXd~18wE9glwd}pXtz1x_;Al{yKQ1X3_l)MQ{fTD8Fjs#;93po{sR^
zwl<9?tM?P2B3GSLB(_kgef8y?!UxMM8oWk~n^Hq@TvPs}K)Ff{6?R5gkaP_HDtdqv
z*6Dg?Y5r>;_d6-7V-JcTMild=sb%W~;|A3ol{=CX*2xKGgxG6yOctfRdqS&Cz6Gtr
zc<K(uRqyr762bkTBr}2#pAvq@zz(xVaY5H_?iW#}<I}Hd6gx2K>h6{KtvgsXAuLob
zymwAHe7fe%JIJK#3g8`;#lz4$diFd*{I*Id7rGr9Gid#Q+-h(3@<VDjd=jTwNH+<~
z0^W`2PM4@ev+YA>LBU$*pco4lo;SkVBY$HVbxSjie4)vx`OUDvM8%uj%#b|1`7A)>
zPsE5L52EcLXb)t~2kcV?dIai=n=eH0eX0&*)p6#vk(nBuJkC+r`jPY)p=bL^tao}@
zkuurd8p|qB0y(uGPJ{N#?Rq}-KuM{}U;Kz+>%@S-Q}=T*EQVx42xwp6yd|u7XH%sl
zs#{xAH>oSTkli;;Gwy^Q7qfKXEHAnb>-Z)I-iiFv-be@B?z(^u-^iEwt-bg$<%bH+
z_TOdpTs^=2^RkMY(&~aUF$?8Helj}4yJs$wADYISM*ZHFxHX$4Kd4<~13TFuO1&od
zt{7GLmc+;-J{V3Y3)q`Qw@#%>p5bc+?V}3ykm{Cyyvc-m`VYVBxF*d8tR7U7`i*vK
z2IwbJ^G`UQHkQe3SYRM|rFUeM)KtEq>Jlc$<Zkwu<s&e@wi05Va3#zcN!a!{)tEw7
z*g^0Vj;HrZX2MKPUEz4q+jHzVvs$@hX|W0Ge;Y$xI1ACvhBaF?uHbJkm%1zZHRz%h
zHhro}Jg3xhCXyLP(VvExsg+CqrdX%mZJJZ2P@4I+nb#0#ETQwA_m!Vj;|!$hshxd;
zLWl_a{elG9xM;QNPyj(o?SlVUwZyxr6R7B^Rsb+PwU9Z*o9D@gQsSVM7gT#S;a9oa
zJHL!ni(c13civ2~zEw#zTbs!o1z-L=Kj3UKpRWer5E&=}JzjS5l57jQ<R4`^*4;`~
z9?;-TP?DzY-srASjA);Uw?`K<0^;S}VZTA}(&(6*sdD0z6x7n{o(cWJK^TPM_7@NN
z%p}4HvdZ(KK<WIcR*Vs9IMVjq3h<p47bYT~!#Jv4GWAEMI~=^g<p42E9@d?dC(OA^
z;wlOidZYC)fx5HmyjPQ<rBA1V#ZU&SHj_>hGWM)wg!t0@V|P62CSFSA!$F}55bUZF
zxj$&cL*`-5k_rRm;zgcOf52!Yxi3yISG6o~Q9In4JF8*QQgLJ?m(I)HyfA6*XYG=n
zNSer+PS`zy6SEsxb$n#i*TX&NH4S9}E(B`%VmY*1Kcs--UP%-<#75$Ul=`MToXQvq
z8!Ac1e6p-;{KP0h0I9<!Kyv+1tG$WF!zj~rJ%;^5G^K1``m5X4YVx8S70#n)$W)^|
zks6EVJ=3**VlR_4gN13{)Rm_-_Z>oSQP?Ja!V`*fJ=a<3=eSqFo)csmQLuH+Kb1R@
z2f1eLYvMB;<G`{a6h!$e&_D*<Pq?j}B2cmwl`3nuDxjZu0J>=FeG>Lid3~y5X=~`_
zH8#0Ha@?j`tdK2aGNgMnL?_O4(D-I4DSH9aVV&w4&A2KpLAdJb_rg>p3cCW)DH(W)
zj*Qz*6VzQu61#ob#{IOf>C@GOh1l=%5$8DEL1T6s7)n3`-td_$?&<}DEE&oc`j6`9
z*d-gG@TYdfMUx3))GHNw;IO)rp04Oo0a(YsT+pE;yU(8s;@9v#cfB7g#%O6ZKDI6B
z;+-4%WQFkguZW*w*AKXyzT2AYqMIEiumI(zgBCH-Fj=&{UA|@?s}ZsDq2+9y;^aQU
z8oEg~vz7{l6HAvb&%gY|mW_Q0TP6j%A-jaIZZH8X8~kE3T^&#---HymHs?>(t8wm!
zGXq2Ezn4<Q+d%#)vp?=(ksQZAE!u69g}A<!Dw&!Rp>6alyWO)F3+zrJq$IMltT{Nh
z<JxcBJsop25cdiB9A!C=iG`);szX`2L_Zcqi)d3o{LXFWJ-cCVHVMm1>(UO&G5na(
z**@DQQ^-XPud;H=wbHhL5J&$;IByM!ztVYwSY*_7doxFSVd}vCaq8QoWKL+#P$=UK
zX5q>RSR_K_%~nu$cV&Q!edp=(;O2U?#qH`<=gWc&h5f1qg$v84Qc#MDTK|YcZOc@U
zo=T>LCFw?A$iac+el)(g<!Y+>Zn4W9_EL6A<0bhZ&7o+BVBuwTgT&D?ZuUcx;fM;)
zJ=m?lc35oeBRdp5$42V*MpEt~kS_D8@TZ;7x*@E#p%r%<ZaRt-u+E{cR=T8d`@?Ob
zr?JKg%g%3Ths4y4d>>v`-Sk_Qz4V6R)l&8MocfEY<h8b@W~lycKaYP<JT$2p*Ispv
z1V{8-9RYaBait0U{)XGq_bHJiB!XE~f3RR;-a=`oQf)HE_fko%k&W(e+)s^5+Za|a
zhtjc|Ff9cL?}Uhbd}JF_1CvyHY_KnRaso3nLhL0hK}w`lAk7jKC}1v%<{7D9FZQ^B
zOa1jB%A&-MI_E)<oKo_Ppt|VoJ6YFI$TQuo50$G@J_(CR*UzMlSE$u$;@Avw^s&+>
z@6N2vGKwYSpiNVSYh|P2CUVIcby)qV6vT(x0?oI;LF`-eZ-Wm=6SG7Ebo0+Zeuwir
zkqD6BiRCxQkZIR-A5BCB85=evOnb5^LhbYXah?*|&UzZ%cW$0Pj@3bj_{x}zwMnSh
zx4sy5s-`q&LRcjjkJ*1cf~GwWp6D}YK-kHukc4<hj{TElmVJH27wih$A5V;JJRh>!
zTBh6|$l~2*b*K<E2=dt0ubV^o{-(vb&h;HnxIelXu(I3iF6xA}ov-;2K{*AfTWdP$
z&pkb>-_B_HZv6hx$4*-3O<bO03bkC88{Ffpe#OZjk>8Vjn00Pr`$9#*@e*+wP4Wn_
z69}IOYQz(Bn|_2SGFi246ge!98Z#L_O>|n3%|}3xISWw?YTyoNYTLE+IN%867)F@6
zW&Ti?RA?tA0eyd5a3oq%OS+SSs=zn-rly)$DwPs6f^y>&I==7?c?)qEVa+pxrZ9$5
z^y&7Ah_J26rx4Eb=@%HZi<Pk3^jJ%~&Pg2CR#0_xCSED`v$Gu&csGKYaI2~@;$x27
zaZ8PpsRlA*$5i2>Iu`#`1PTd72n_wLSw-hIRJ4p#TizlhR|stxj9C@ewOpK=&TVg5
ziRQbWOcn6qVd%W_s(;noxsNhLjlw*ruGNK#9jV-L`*(_3L?6yKV+PgHCif|{9YoGp
zI(h{A+J*wEl>B!vQ3leICp9y)Lzrq!P%jCNj1wEf8L1rOfWq!5zIow0Gl3cH(M|Yy
zM%L?d1<e|fV`So(KBQ?UR$YbH@cJb3k%L#+5JheD@WeS})A$_*vj~F+;XQ+PGdypW
z?<Rze@&$O`dPYs;hZpCLmiKmK?}FH+hhMxZ>)jny5&8J<Nw_Ck^LFYEWNvRAN4on*
zai*AP%H96mQu=nja#sy!_7kf<)jxaQ6MtJM$rolnZT8<c7Bs9I$*&NiM)<#GB>E>j
zZfST7y(IPt^DBtU)h4{lo8pH$GA{{WxNp8oB?kY7mO`v6qddUDd?b$=kp$g?ZesFn
zn<A;q=<;G+A1a}GCNm~^hSlL$-PDEQV&Mi)9CmVS#H6v_Kf^>K%ta12JEKVsK#T0t
zM8=ClfI+IgRuAlg8QtFvUyBl9&B|DN!LBLG7#B<PonMH~CR|vYJfO?WK)nUu9>U4z
zlZU=WNsX9J?RD_$a}59)XX0y4NS%qfCe@`)!N(72lSq-|Tv^@+ZJbf%p>O1lKTZtz
z3*VsGEJ96G&lv#P^7qR$2Z;!Ok#SKr>1mx`;>j{Iy8h**xg+humGFt7y@mc6XJ+nn
zafJ~PStZK=yC`pJZW-CB`h52?l#;!O!GsJ%68Z(LZD%elEY*doCN?3(vxJ+GTv1Jo
zLBuv}ky1X(YFR^UyviK<#~F}9kc9a1hFz{dA39@`L4Hn=(7E+Kz6T+ly=S3H7kQZ1
z40j&QEbs3##9J{E@-#gn>YNa6$D3Xx&NTxw?Y_X=I%lM*51ceEQ<Si!L;j(B3%oHN
zX}B(N5<Ga+!674}bQ?yE@bv3%g!Hn=OoU~Q|GlGh=`bTBEO?)g+MV{Y-HwQPtlZv2
zBYFPTUKi&Do^vv@ICMbnDB4p!ITqDcXGswr9*#B2Iuk%sS(+r<A)^wT>Ff;uWagP|
z)nbX-(eD;jdqd)>T}_e9eMrw+EOlNiwpxzHAg7A3th#QUD*OeHv!Sf&Q(!vEP_vd@
z0AwuTZh?agnndKjInTX}fT(SU#4U_wAW|uT2F)+sDcA7#1<4e6!+kBMz!_x7TKp^U
z+%aQD7<hnS==RIaF=EK=8noJj#k<9#r`z+?mCj(A?{9K){<v{*C0N1C?0&8NEi77%
zU(qA4loj+nyABI7>#$-VYi5Z==6}kCMtILXp4_XCV%ycu^o~0=g7^yG2Jv9YCnGRn
zy1?GJr$er!K?hNu?h<jpwi!r6eN?$Rb8ho~Tc9H5v^J`od6sL#`sT9f(tgdl6$wM%
zDWSbzrB!_!K)}0)X?DS-1eTJsjoQ6vlaa!i`m&>rHLQNi3Ahp95X#a-RY><O6oe|A
zt%Nt-=qfAykqeEs)Qv>Y-FjR;2fM-ZBe$bi=%YB!s~57x5ytw#R48eRi?~F)$<cWK
zR03lRev8deURy{xd5-`0#G*=x6PJscm5%FnymfBpDPTBydhTh{`xh+y9##)_ThW}+
zzly)#fU;QS&yhCQW@>Pfwo|!wl*FL3^Ru~sR9l7%I8ot!;9Ntnjx1ww3q*!N$wC<L
zNc}Cc1SrE!@roHv(^5iOx<02U!{^KV+xk`~iVKY}4QFqdjFgDmXP)Sj-j@5m*&xua
zDuwS;Rlx$|<r;Hs5^Y0dDOG>f%-GJ)?$c}j`2z&PYW?zm!ty!(H!}VI56c%76&4Xv
z{)go&Dg7tc`Tq&bXJz<rFrR~i;~$sL@qg#?|3l*czi|2g{PO>c%V+*KM#$X2QOU%a
zfC3-@5CjMTgaIM|QGggg93TOZ1V{m-0WtttfE+*`pa4(=C;^lKDgaf08bBSO0Wbg<
zI60d*S~yt&3;~9Y21ZsU&ekTT&i}LR_#b5ezzASuXKiQuuePx<02l*|EleCuoGhFG
zCV>BP3II%Oj18R30VcNpeQ`IkHn0Jh0!;r?ECByiTulI`)&NsG7e{~@z#L%iVQ+3?
z3$OrK0;~Ym02_cUz}CXn1YifS`&ZllU*rP+BDFIH*ju|e0UQ7hE_Ti)#)j7a74QG-
z{CB|r)%)+JBftsZWMX6S-!pJBas78!CkuCgll6Zhb_O_`JDQjP{-eov0k{I(0PX+}
zfam`~ng1VO!T-l#@Xz8;Phe+gNx;qhKg|RT1PpBK|I1UrNWjj<^uK!lyTE{hotg3f
zS()DguAFRzO$UoC$(G3IJODIL_|H}V3d_(7MmZ0a2tq|LPmmKhzcsHjPdYCtp5u__
z<@eLG%we^h=~2U#cM*G{Gay}4H9~82V+*6`9M;?R=I(C@!~w^{!|Mx#NYM)lc5lSQ
zbP=>yo!@h`IvGkp0ecCL^pO^VgK7*aHhyTcD=q}*2E~uI_iGmg=oUKS4mt`r0LYg>
z{?SgztqhZYdl<$VVRjd|tX+to-rAJI4MC4$bq+dq^5KQpe<~KYmz;`f@+pjqe;5u7
zd}ECt=r^l9c|0~_QBDjs|I8LVY~aT?IapRO3eGj*@WjQ{KUU`e;=mz{Q?kwpxU<im
z+@B5#BG5D761ax~l-XvJ-%sJ7&lKX6E5N{y>Twfycbion5s1<rd?SE>z8D6?5FrpG
zeht`@>Kup`0r61I=!p-m703?_vUg<QMXvSV(SC3cp>O-<x&|k=huVPGK*Crc$XaTs
z{6A&Wpf3SVz}5uMJ+L;~2G`<#*Z}O>iWu7Y-MFF+AXF1)AXf2Ezq_C{fj&HYOa$1=
z_bB3<8v0R2YW~Zk&8<y-M0VJZmAAt_`FPy=>+ViJmKE^*Yq+}~Yz}|IXZRlTRbp;;
zI0Ah+FmaJ@0Cq9dN6RD<7$g)(06)^M5zr_K(0!fL{#OeB&I;&bSmI3@<``k`&cGJv
zu75G`FCZ@ec)rzddTan95WI*#$mdVhE1>`Q0CY9zY8OZ=o)|Omi;Ghc*6|gE_yYm_
z18CBJ^zHx@$j|57qn=+jy^Cvb>80fjpY}LqK`DVD@#Hh=o=;Lk!wuh`uLY6MKT9tj
zfq;qz1Q8t-80?d6k`Mef>l0tm2K!bo4D=hm48*}6GW>fCSfjJY!~Kf|)7sAlj%&9E
zi+L;I02ZY7E7#X4pI{dLZRq8v6V$Ku^C#xJhT>=C>}Lmsl1N+UXSU(X;`V25qcuRs
z=ck@OmI)+Q8xGFxFp&RyWfAJ}gxAXdvJa}|XQ{l`CZ3uI)WP|yG+sZQXcf}91}=1i
z<BLPY?*$lZ8x0oNqGO=<Kh^>^e8g8i&cpe&HUc{|%z7=qGXBw=cRNY&@SKjXnxg{@
zRFGBv0@m(T{=?uqm|q~DZ}D1|2={MBt-zh!h;Hoj;QqxJK=$@whCk<j;|cuLj?+%%
zNWgYezG8wC8lDl5Awb;G{D^&lYVPeGF?%cC5s(GWo&AV`foit-#8I&pj@gjGZ`Z$q
z@Q8~D>`+>E-u2M-ntm`2L2m=U!4u$Re*(qncKrm9fNCy!2pH2g>_)?)Vc#%>MO}X|
zgcWaJedvufxZ@YjziWza37v#~z!TaG{*6I+?IB2H+1Eq#K)u<C$IqC&L3qtuUMI|#
zKkRxLc_;k%X8S%!1P}1$cU^T_xOUw)m|ilF!AN0T+}_OkcW$AARsou9D<RN6HG{^F
z{OQ9&@*uZ<`Pl?D$ga^WifFhbPtgfS8}=i;r4-W7qbeI(OyN}b-=W^=>mF_!H!z-h
zw^BzOBwv&!S?5>9u+-jl*sW-mG4pp$?N%ZE4sYt467Xgls&+_wP}FRs`JR%{k#Xb3
z%dIHZ0yE7Jg!FWjVI-j`MZ|Y12x7LE!aj$SdKre~wC)R2hrggwQa)!qTXtZwj*Fbs
z!Q({Y6cS&`XG{<%UX)|HB^AEG1+}BUa>vj}h0$B4gp1Oq`DvYHIA9T|`lm`jdN>+9
zw??+GlRXT*PYi;L9>dpBd&;?0f@TJl>04r8n01)XrsgWa!n^dX5w7w5b&$(#D$qFh
zWYY$b;ypp*sxb|XQHEB_!M=p})=;~eNlA$lWZ$x1RjVP%8e6HPvKXZfWN{(3Des%I
zB=}5Z@l_YbAIP>$^}Th?Zw(+-UeD;VX7k7Yh6vpwpekL!nB@N2dJfHD9FQEFaVN|n
zdk}S1KWW!`_U5>NL%NPL#PeOPxHHt)dHy-xnEUsRS1X{NACyp7<_ojzk$=zArWrwN
z{o$RcFjna(I;Y(nQ3;Fx^bQ_nFirI`)RbCP<mM$Cb6NY)ZIEgy_ygqjVGzOB$ppzh
zO5A+CG9g1r5i<3bx{{*@@3$F%|5P{oFQ1({>A2P1ai?<_M&(O(`O&>qn0Bn6<aJgW
zJ$Kv{1#>fR*tGtUic$c9(|McD@M<5Q*fAx~UdtH6hUiu}O8$t<Du$tCPzFc-kBRV0
z^XXQ~g&29LY(_<!6&`TjJjpjg>q{wbAMNf1SYs|0+7S$2b^9$n>dZXOwNsD00rt=2
zfr_3B`?KR8Xo1za6A_*fCm*G8B7!WD#Kap>0cjJ#7b9_(=5_ifpGBsNP-EtTlCY;8
z#vSK>Yckj6nNWcr^-+Sof(hx!?uq@2d$$s2r@Md_BR+0^K~1HoF1+GN7Hdn*NPP73
z{K2NciZ#muuZWjL!IhH;6YY2I5{zhR!9$AKZI`B5kc0`EKTG!SLj4Sw2s=&txFEyp
zJhoiii=l1a*-NXBVP<U4=Qjb~E1TFwy4?=V?xmc&Y8ShSlIf>c<=Wxr_mF%S#7ZOj
z(<~&y>zI1aGP&9!Q^Ti$8zW(YHUIgU%d1O@;ak=RfNIPp04t~4vBWv!dZxSANRo4a
z&!zwTV$JHFfg4o7P^1WQ?L>-vhCE=Ez3h-Vj58Tsdss>hbcNY`Cuw#fR=tQwNj-MO
zo;N9!fS=ss_zgJE#?k}qQg^pKGgF!J(moV(4i8eG9ctb6lG#bc=dVP144y)6>V5VW
z=f6$kmh79V%M-GPV7t_wXW}82t&RgkO`2$XVXQ|eEWp}rxO^Hwj|UqjlL(|-tXIrK
zDtu>0&dR5Ylc4SszCdiI^}s}pWoXdQ)4at@7NPn9Q~`;%(4*P^U-J4hRTmkw&w$LF
zWtFAZaSGB0&YL@L^(iU1^?a(HO>d>RVOF&i(h7gl#tli>EAgP$%Rz5+#hWtJBBZ3%
zxOuKk^Hc+31S|h>;*ZTBFI@a7$mi~57mNjepK412r#fiK7s0BRquM%{`FH69At4$V
zWz~wSiYm%41}9~UlimTy3PH6~+uX$TUdtiV3}DuZk7c40498@W7z;%_Z`AuFTpW+c
z3(!a38(YK>%D=h%sV$c#<@hH!tFSGdHLVk7*J9zZw@=n<JRyF2jlFo$*3BHaskQXk
z>6LGyEEKmc_CtyQE&Ur-i&d-^%G|$g9ii<@(&PH?pzu@O;(-|Dg$J6>2h8PysS{~?
zxXM&?Zj*Cq^nf0Mki8`9eon>tNl<(S(Hv^mghHnXZt1Sp`uO?l&7%ka1#DayOH?Pd
zVIhuq^fie4S?Gjz%?`)Rd>7H+hvW!;p(45}UMkcoV}de2%YyrR<P)kA)k2jmO5~7$
zSZVlMCSh+R0iT{m3_L(n1#v-lM8I3fBi5q8g{a=v)3o@;0%HFl+)kHo7wbb(=x%Zf
z;aP6^q*hFX_aYGFoFu4owSJvX_iX~fn4|m}svVn)X)E5nunS49=z>K0^lITW7b;N|
zo3ji2G23cul6B#l;<EdqZAh|E2+8BK>PFxt`^pDL?M-U&Wrh#GuxFQ1)dXtAb#4~+
zuO(@p8PMX>61_nM8+=k4w1-NRg<s<@tVo;>XF-@rQ_{31!fFHUZ^L)?X1}`!(;|_o
z>~ssR2MojKj{Ovk?v~3-%J`a4k^MC_7%c=j{L^E)VRy&8zOjWZ{_AiSonJkWrj-fO
zv3V?1Mkxr{N}gpMw1jHOM0`6+U=BpND-|P2o;R#?xmEU>oZFF8Q@lr|8CLzc$mKVd
z^}dIUe?FcTEPEbnF$XNdSYw10-@^_{JD|dfXokEFIB+lrgmxb0sY}3HV|Y~M{zg_2
z5rOd7*kcH?=|3H?Yq%>QF%>(!ewZVn*I*GXI#cf;)i~<`wvvqVs0t{*GSuB<JaL<?
zNUw~ThCB%TH3>608sfS?hx*vm2+55<-un(4$bD*dB8rlyy*qC`tC)G-a%N@U5ud4)
z)emyPF1o5;6)n9S4%iNuV1npP?e;6Pg1VAv-W?(f05NTmGb<#?Yo>HtWvRZ`d=oX#
zM<)(Ut%|O>JhYqD*LS2$i|fuKGLO@a1~<Px**){h4F4Wyz8AEXczepJ{NkJfB3qzo
zs(-A?(1A&wtTDs3aasusv$6p-V;AqfEmq&_=ht9~<V+&i4%!!qbGqz%u^8xMxvy$d
z3<zCSAcr}E1gU_HHHfr;ZOmu`U0&^ixYdM!Oew5R7vKm`Ub_J6he<QE{Z-Cm(lbbu
zmh@$GvG7hlu_N)B4l~g)L1*osETm!{<ro{52zYLMzSEhu;ZO}FiJUtf(Ro<4uc;I>
z($h%m9EA+K<tdIW-kyTC#@lNkO)w}pnMXs%Dg_LymvZ<|$sqT(H(rdl;dw~Ac&@Yl
z&?gK?n@mbjoPy^waam{1BmA@LK#GN?=C>z~_~99`y11jvZp)4eUXV(?$<F<=zl%g`
zL06&;6D)xHm1#w!k%Hq90VY*9ya>gH%mgw%nn%q*6-5#dUA#)1+vGnAH;*J568{YM
zwo0c?BNMtx-I6>ris-Th=u&e#y?5G$3?M!hL>vCy#F(&AJS)uTA?JOVN+69q*|Z%3
zZuq$v!<hODl+haIBUahb5gt>yN<BVmBltA@9H2B%v%Nl)8%!%aJp#8&L|OwUfLKWe
ztC>DEZE_+&rsg4ODijiTR}PUwnFV&eu$vpXwV_d{kQQ*NU*_fh>!KvjIIweGnxUK?
zYva6;ygDgJ;#2)fEQ)imWpJVe5&OWS!6t0N416lFOkbT+2T*OE=gm`Sv0gl1XFOKe
z+%=M@=wOMf(lPj_g<ptm{+_}z@qB5%b;kawgSVnC<a9QmoM+Frduy0J%qPv<>R26G
zKq$3<o8enNrrS+TIBUgL7L50wzEU_buWS@0y>4rO{di3O++{*8C#=-0lqi$=Zz4iK
zyBgTs*n1&+f_4ld^1{J{?u{ZL9lUU~`mY3Xpyh_=hw)e5H)9v$Oq?CF*{FMEEpYM%
zw!|~kUyMYd0!O$t*4Hy?+n~A;&BfBN!b9EOrr_b+4mPuqti+5BX<pUj{50}U{3}5%
z16Nzdeu5o?!>9Xd!xpiKW-L_<RtVID3C-Tt!0OjY0f9<hXu?Bfgx|+wtFsCu_dQ_?
z!&O0c@E?!V4_OcM`-A;vwtKlIsKjI#nhD^y=f*E6*3F(ft%eXDWc55lo++y$AWebw
z(0`EIbX+6$Q|}23(-H2tqH?8MI*ubhErJM!qgzeL1Lgp>C#qmY&>_gwc8iipUs9QW
zEx41?#+|+#q>?{1lfbLh(n$;h4Akp^8-pCMR|LDV1}aXWE!)}U2x5IrBh1F8WVE~g
z0&sKpS}InwQXwltD|}W?hF;|fWt`Ag+knSrG*S0c*q^KKU8woP(F$#tVgxMG${-yg
z!tm281xK8CIQwS9VLD+GG1xR9%VaSzB&T{A)=$gC-n)F8l;xzRb;=j%$4Eee&R7$x
zmvZqo^yMiBX}!R;Eh!-vqmFwWj7_u<Fq;q&n7e^;L(YKbF2Ko1xjh_sPE-`GOUzDR
z-iK`4fnN9Put5}2Dqy<pqBNfLXhNg5G&-F!4w_ez*=3$3<Z`qI<xTFM%W6+;A=XGs
zsuXJ`aG<fQr2ELarMkU}qC8Kx95I<)YlSN~SvWZSSXKDe;GL?7GP^_Fpgc;AW+n<4
z&>4+<Vuq^jO@Wkamq3*=t$Im}BM^qm&7RQ$3ui8fjOVBBNTnv<g`iW1RG6(<+~|vD
zd}p0x&Ai>pW(Wmd@CROty^UMTYJN8ymEQ>dVJJ&Y@@*_-NS-eC5=5wCM>E^;5#{z`
z7SN{TZK1sMTNK0o`!wp-XAq_wHG37h))M`P{BH-HZso+0d1g$!pJTm2oIq@Jc{!?C
zu93OK02tfeVU_>TP@MmD@lf+F^?<NNvx{SSH55}y-FO3eDi^$yZ|=Voc2o<8)GHoy
z%FK+va>a$eRzsWVF)+c9NlUIp|FGsKk>5aO9Otmo6-`2TJ<COs2A^=xxRel!N3v6T
z-f^yS=fOL<oPt5_VXyl<;2uSELlJk4Q98x>V@X@l1t-__c|F=I>js}&E($3*t_0rk
z@S<$k*5zBQ!yM=8vX~rU&I7&-brFed-ShHSy7QL?1S@kQCh(kODCdPtXhjZ=GzYVq
zxJA|p-gJyS6n+~HKBBP7?v19?!{g?V@PVTSBS#fO{y8&S#OP0?noHQyu$F=UsBt=i
zZ)UNEw>(Z}&v{ySAyVvFIXyM2x?&RMUKp2u+V@i4^7G*9CS+%(1LSNK^mXHslqN(C
z#l0c16q1hqCXZxYdckpD$g*bO(TWM#I4~<c&Y+Y_PmiI#!BBW+Xx1hawsb##zxZtN
z#v>d{OT=1vbplAA36h+EHzKIV%}cO(t{JUYTPt)kuN`ncIe001d++Qb%3V}k8b@LM
zC1DO`b!%q1Ki0kQ<omfjyZx$I-eV;d@0(sUc8deC^=_IiZOLnOA+q^1wM~6*i7_06
zWeAz3!OM8?FXQhBX__eljfr-~0Q`;&r6GNyES0zH;@;#Qm)yWOM`qDudHwg|j=w>I
z+9v#<5~1Y#vHWdFEBKRsk3{s|5jU<?L|Oz>5sV>A!^i8mN3Nx+{jR_kqGnf*r|ZQo
zGLt0nvV-d%?QANdu><<F{n2JZCB=9w&dJe(?ONqZGYnM!m8m!s)YiY&cuH@};AVUa
z(O^fiuesN0q2J)g2D#OTIYHbNcvdXXf>XQaCm%@}iT1lKX}W84ngd%CvQbyc6VOwh
z{Z`mbb_h~JimWgHw(?Pk!Azjbb<fU8>JsVO;Tj4YMY1K3)n=D@@P;DZlE>e&75G*=
z)*=dS`*1;3LdB2v^*{dNJa62;l@-i)9k0#2RRl2S%z!oE83<#fwOXT0Bup!HAJcr4
zwb5efJsnmSUSH3rnE-7~HuJV)5~_M!Jy5#upXdy)UrnU9h76pz1{n7~snj(GckJGf
z#6v2T(sL}+wiPgWE}t;2`Fd?3dV1!K26nIeXwXL|+IvmWTNHaZ%BA4r%oG<_(-Wgb
z8)4sOI~wT_e=nZY)4fMBSB>a>CiFq$sp@-yd(UqvC)bJHH@o=f_J(lGo<9~p-qlKX
zc$Bx~lq7-$5p-~fp47u2R<LPbrI#RFbF?B~Q@gJ5SpM@k_uvX=>e+iXH)h}xg?T$U
z#MCC+?EY?Nky!ODR{6Z=&YD>Jwy+@T`Ce~67a8qW{y7gKa%?QVCNvZ}dEhM>sM$hv
zi}s{j^&&61{t63}f#X@ER8pU8-8&yyZ++|({f)YCPm|{i<?1sTsKV1cg@P}~7-9<(
z!`X$>xW+-h$cu)ODQLnXD~ju8U7NO57hq32%h8$Ls(OUzYbsF!vL&HjkEfhGPC@l*
z`9QGSaV;k_)X>7!4Kfo6O>=*^9~TU(!7?;9VBwB`>A7|I>a+6Z-5N)9xy{ivZ|k!t
z{I{kKeTmUKJrs*wsDckr0?rk9d+umT=)7tpk#_(27ds+CEnHsGYNnFRUZ5oI<_S0k
z(7i?V(^x&B6MZV#L{2Io<9hp!PEkoPrC!*=<)%NCy!>l<!xj73NZT;NR@H#W7I@0p
z_Si1i6++2=P-)o^Po)(O>(oIs#A%_sTWqSfgPZM_4Sc$C@9j3@H-UUvO8BmE-}l%;
ziVU#ddi(XK!|;^hbOr@&hsUF7ZG)lj1N=GVQ{NoPgv(>q+f2#Z_?CT=TKb=tC>I%T
z$<f`Ku^~xekQ=kdpW+F3jU5M;#h<Alm*JpI7@<bgt5%<Yh%B3Tf;AP_h(6a;Ge%Jw
zW|5k`n6Ul1g4$J`VoP!)HaoaKG;sLl96Kl{R8`x9$+sU|un0}C5Q(Ma0hXI017-~Z
zjjG+3yXRA{nsajIbvu8ymhI=nx7lLa8pz0!IWm-Ov3wxLAIXN3VV31k!SW6rrSvY<
z5bJC&8UkTIXd{H27YSUj<+|~|I*l$!wfJOEQpqb03L7F1wED8?&d~aANhd=}p7J#i
zYE)mQat4BhA=@8UoeF>|D0AMoYSsxE2VYW1c1W!^pwPVJ^95U|2uj1~h*Y5Ipe;M5
z+Bw70w}}i|t!KKV&y_vfAzlyuBIp(S9h5gd?XrAFqfEr0tq%Uq<H+ia<dOc60cLlv
ztJ_m=iMQQ%UF4uV;uZq>q_D3|S*>IH+O%)P6%12U3zf7<0QW+uPT8GayZ%vMeh?~E
zIq))T59*XRJSn`AUy@`zM<IlJMBiQ}{O!j|`0Eoa6ph2<{U0LAXG0TKi<EsbA0`V{
zxdWp@%^ul$NjyXyo#|zNg$qNj#&|h;vuz9dG)r3rMtnRpvTC$Ji<tb6n#&IpI1Qi!
zAzQQ~OP#>GVK86C*Zg^vUvD7qSP#^~o%C{CH_#g<{xJf`4I{Bt0O3E+dX!lOLHgeD
zl)pIK=7tyoZ*jx*9f_oiI-TG4(n2yTO9l{z&}pR?zn%;+e4z`}b=+!Ii@%OWxPP|T
zNPkV)OX(<C7$g&|Jirj0C#}zhH?7P!bT&5VC#SG9F8F|IRQ7qmXL);Rvdpz%I<-j<
z3mqw>$z@mnE*Ep$u`bUAF?8ki#(2QDX7DAHNW|q#Dj}hr2uEj6bn6_1O4^C$Y_{;;
zcrT6)gUc=Miifb92v@YCVuo20)suRPFs(m<A|Ob2;>RsmhnhCWdQI+6ZW0e#&M!g8
z@SkKGDvw6Jh`~4lv~eG6wcN~L9$}CuIO=fATO=mT+b*q1F(zB1v~<b0cy`^q1=FU|
zZ@n0-vtenNK-dBO+vj|YWj>2jJv(%d#uchoR#kWgg9utY{OJGVHGjSV-9^mYKy3R;
zlEM*WD?&GbwenTRbjmS9toMu69~A4Vfl>j7nY`r@)J<L5Ud$*crSQQZaj&IV4C~Sj
zf|#n_lW2`^_AIa!URF1F#4c>CLiByPQUei|<}WdjN^ykJ$&0edAd&d|`-5bT@Mz<T
zn-LsxYGBeea!B^YQt{S%Cy!(L!7+euWLla_7GlWcIVU*Y+LhJL8&O#0xJ1kWzD%(0
zK;G~5@ULnNpN@E3N>$J<qQ4et-^=sBiiyP?;a#^d4Qk)4iG+CN!Iuvq7J7w#1-sV-
ztFpo(Y*qu2*!!cCGB;aM!e(vsVEtVRfB4#~a%O0kp!yAD+eq#kNL{Zj)`ZwlHXC##
zXG^$1oy%L>pEC;#FW{@;28OV_;NV%v3zqM*ixCO~Gu~wgh`{PMSjC`eBB(jB;i~^Z
z+BwFE5-sSqZQHhO+qP}nJZ+sePMfD~+qP}n?tXpmeKWa}%*#yj{?$$@mD*L6RI1k6
z-$w`-95riJf>T)LULOXc1->zip1=6&OtHPkb0b5E)fgG-bbA&%PCwS4g#E4zE<3}o
z88OwdWkNnnS%PVwa?iwrkgJT{oNDURBqk&5C6g^%NTWpTAfYm=o$|~Cv1|E6Hf<*|
zmKWr0)WEo9slsMxXoY|OC*?3IwNsO>q&yZo%`srnme0?QrNS)}C~Wk4wiW6?t`(X_
zk%Gp2;KQzUpEtifB_Drh5gzpb|0lKz?`fNR>y@bF@vL)y|E}sdMm0}wUmVTOka>f5
zJI#e_^mI+<DsI#ESOez0wksBS|651ofx$&`bD*Cp3?w#7IOQA>U5zboN?#*@4kh-<
z^u=k&m(TrcwaBP(V;tByX2v}!hX?P(WR*IjtdFD9lMijnf8-YjU+t`!Z7DohL)06?
zIpra5f!k(}j7mb?zA&>`2v5J)=E&;%ujA7+v)WCfEaM;$;Ax~d;j;k~?UGgz4&v#$
zZ`19WSarrfht0kv81bbEzYS%aa4tSp*dO@VG1MEP={}Gf@XXp_jd)ixu>aUvt0`rM
zZbH6ScTq%@BBEpzq3k(xVK<;r6>3OMbgdPfxg8O@?lfIlF?`B(L~E5#{(dE#ip2jg
zo_XgyL7fRX#1<IW1`lDT!j7)PO!W1zL=|p72naOVd**`n3O`=L38Upg%7AQu$tl6T
z&<gR1UTaBh3~4dl!rrsKIDsTl)4nr}vvhq!>^Ns(bn~cf%G~9Y&Rgf&@rO2-X!%{|
zkvbn%rx{FY^ct~VK|}sowW$IRg;<0Tq^!^m?}1oki@0Vu)GM~ujDKMgzz-eGGiD#P
z^@N>v_(7mqOgCf7kR;{WB%hlw^7|uOq+CgY6B`AO;HP^n<slQtzl`JSHKam)xX{n)
zUB^VEZ^DC}!pKm54W+sqQPplIv!qd%nvcx}HK*@G8@U)fS;H``BQi>7Q272)w{xTZ
z5%v&-DQkq!$rpIps?y07>Jk8mrL0AEOksG7a#8P6rk<xrS~w%NBndWUisrQ=6+4zc
z#|-7S@m0yI55}fSpp&ZXkUx~-K2ILwkT}f;f>|1Rm~!<KzKRh|qt{o>UC97H+vi!T
z5rrKsN6~io<B{7p(`&&j=K{ztWdf$jS(3bFs<D1^rFg;+Pj7n_do&8~RRduZl=<5D
zS7J$w-3le@a?gngglvj%K91<e{o|dA5m>|p-k&&Qi2AzFgRBY^N*>PJuMop+Pmxn>
zTv>QAY0{DhQZ37P-sckypcD;Pd%+%f*Hv)i;vNsyxHWAUrv;5HF5)lEKTiP&xq6hB
z!H)pd5Jo^oliuL$#3`^o@>cZ~3`K7}o_#abfnod_>0`#kK#aurdbt885uSx9M5??w
z(L{UCl{5IGAp1By=QG-jIc|^!gDPfzEnawkzXyVx`k$u&4?CqJuBi{#5#{8g)AHE$
zrdNI1L_nm-MvC9nm)CEyX|mU@tF!o8h79R_`fg(6A|RCvBvemRJ6P3<Q<xX;&{!PP
z$>nahsuZsQo)<@kB!@33(DANZJUm8KWA;4H6t@%A4TsKvMaJT)o`_=BD0B{oLDLzo
z+^5mHu358P@Z*LQso4jYE|_K-AC0Ix{t&kI)>!velfk#z`BlfHSJsen(=FSgVa;vo
zbgm<3(-|+o7OAiLp0Q8Z<+=9cTJ`^^lTU0fwRq0gDO+LI8d_Ii@bL+P;e?USg{h|r
za_TS0bvMJA_F&X}%WQ9*66`)TX?WrB?(oUZ&YvmD%D{xugC;8FA7!QN7JzoZq_>#(
z456Ul&)S+2qCVTsE=*^UD|WEG>l09k<7Eczvo`VCFTEIEkJ)64DO^u32Zxnnx5T~~
z(0mwV3v6}`Ut-O7(&^cU#*0K(Y*7+VGTIA4W695KG?`!9?^JzppDh5oee%i#g7+18
znr$h;oro1A;lp+OGYf}G8lvNRn%stu$Gy;JSW%GK8S=mlG(0F{QuJ}A8l12w+SSy`
ze?BWJ#1NFK_(;dyCAq{E=hC-zf!f?4uVJ~n5f5fap$N1By#QIr!tQS8v<a(1C_l7d
zmZmvO@Z_FZ?<uVk_#mTEsGy>d8<N1w*o_tL63%Qfk=$Qp$=BT9I&=WH?ks#xzivFh
zWEEK&X4RD&o_45=`d&-$<52x)Du#^GycaKq&Q?w_KQ)++)3j>BE2@+8HJFA?zxLgR
zGMa9~_Jde24<c!jqbYX{kALND)IMYlV>Yy3d*+K&@_mjd0cMe?HJPH715{9W`tq51
znY*GiXTd&B>tN}IgO_XKlrBuGY!KgvhZm-wqcgyzuI)z3SelK)J-yw$&6{t+^Q$RO
z*kcGXLRpa>NmY}0XXt4KXgJ)jI7(F2I{aynd!AyH3}P)jtvn!GuMl0Sw1==V&Wt8N
zr0Sh@<6s4QjD=!(DtN2LNj<P=-oRVUj$*lPpw!T5?ixLVboG1vYdlRLx_bx#-ME>r
zpdD_?Y^H(VtJmd*Ix|WxzZLJHYt!`mmCMu(db(l~8*r5J@ExfO71jBo8KdBnP}&Bh
zc>}gmh{pAj^;3I@0#`_xLro;UzkZz{x)~Z8c2C8~6%**O5P8$Zmi3kzte-o#{bZ{i
zY_u>8F73ov$yy(oGz`9J3e8kR18tsTVq{yaMsN2`xNlgbI65qi2k7{=3?*WnYeef{
z8BdhqY*Q}DTo6%=dKKQTw~W!xWy`M#M*f9)IwNd*-4F3lwRi`KOqrc?WLLkmZJ~0I
zFKb`Y!a>>N?-JY`d{!eE28}6?y(Jqq&$88Qp1*GdIcoyMv)|E!vlR%BZt$~+frnoF
zYp%phoYU^;>MHX9_ST~#+Z!PF8WOBy-kQi-R`uo-G(D0CgoYRk1(b?+)@k|`T>BqB
zscd5pG=<s=9Y>=hn*3z-Vb2}*r^J(;!sOuqb|eY?l_azMbNX$sf}kM;fi~6eisPpZ
zMC*9pB#boBi`ipW#v%}Kz<2yk4vLtX5xX4q*EN>G>Ps^CoEtp1r8vv4390i!^)zXH
z*NA<ORJ|5m;=14--Fqy}2_E@p7X<&}C9WC?+P<yYC4zC<!>eT2DCr_hvl<Gc_P?j7
z<<Ib`k%ywI)DQmr_~|5lwfO9Ak$Q}C8x=HXtj0nHGby4qvZh0Bw{HupZ9}s(l;_GA
z+f*tAqapYb21OA&k-j%XA%txf55}AGD!NfxskU5sq#Kw*$~<C`4c?NX{oHRXV6~!n
zUJ<K>MOgG%dk^gA6G+f6H<fwpXvi2zBql8hqwYfp<NaHr66(6<O^i}y8<>pu;!vZ|
z8`M-C8arXXi2)riNg=ajyotS_g7*;D5Km<-QIPC91)1T4Pt}DV@rGKcz4*uz3EMp2
zd%-J^xfpB0)FJS*-7IscVfNN!Xgm%de66cp+hIaIx9dr1XfBRumNc9<L@9D45_XrC
zTJzDnge2W!Uknqpz+36tE~a<d42_@Nx&?{JGrbo(^$qSfEku4hgsKbmzaOPU<k<Fk
zW`euI@zQD`biCPOP3~T&$1U~0Ds-8Bk{Dxro|UddmDhOAB#6lu^*>eqB-Ty_^`wPr
z7R|o!9$FE*QYkvrG#qA+rpEBGjXU238qp@3kItJsUTbS5h{%_)-ndqT<_9Bdzl2W5
zJiRxoE9we$SDb9^u{I{r!>6+mWvlXZ6n1R~X!m13>v{Erz<M$7o~*Z;n+n>~M+X<b
zEv5a&%c?^%&77AWRW1b5xy*U>2VkRTQ}}O;NOs15Gu{3tBT`sdQczvwUq<9ld(VGs
zL^A!uh-4(-Was!XB02v5jYzit!H5+5aUrGt={_3%(`EdZ#c2AYAzJ=l_t5?y%HhB4
z!hZ;ZE<ZA$JG~d>&-vTixrkZ*^dfQm)A#$Y1|ZYFU6=ppFk)n3{h9W^Moa{3%>S@D
z|26t|1CX8h=Wzeq3L|$=6&2fMw#uYm0W4uxI6^{&(rt(!U}gb<hG1r)I6_hiN^OV;
zmLf_?zY~x^3Q15b6{O;kZm@mNe7C;!&)=6ct*2I(W4-mcuNp&B=T~G@P=j28tOyY?
z6eSrUK?OE@CN8)L2mz52fBz=hKV%Iv+?nAwWYUwSfPxh!Liy?!zy$?0c1Q!m2(D)q
zAOhgU=OBOtA~8iJIYc2L0icA0eC>vbrvMcIzJLw}05b}}Ljn{{ye%W{1X5s7XNS2;
z>*fOlG#3<5($kYWXy(p6gB0z}?1kt7h;|J2+LLnu>jz{mWQK`^d{Ku0BxQ#gk`~n2
zn4Xr0xrjg$&eur>6$RkO7=mR8$<L$ErvP~o#b5yO7Wy_I6B~j~Yww-$-!YRwkH*IU
z1$6+xIf4<*b;RR?fItD=fdij#c?2Y;r`Urx{^W-d3E;N|mWY)6!o8Be)(<oY-<~os
zH!mzf$3O%SZULfW;6P!Rn?vt^F%|(}0J<ATfDtEl{4&sIfPr)C!~B@Sfw3sf05HJm
z<v-2KQe>cq(Sm^u{?4cPqK2s&%YX>ynwv+C?8zPVJ1hYd7R+)qg-7@<sK<jS#0PpW
z*#Qz3(D928c=3qPCNS9IQ_zj!mvul`<xitV0UnW%kkCo}4U)SBu&})<`=Rb}4?urn
ze`t>}On`A7@&KgcK$6?bfKG8G8Y&v>n+E`P2n;*)RkF8@lwL>z5Xfl^K>n8+M(|1A
z1qa6URd|RMAM_sFhSZ^+98%)L>+7p67@Hmf2@w8e_`}_QaFVt1v9dAb+u);FP8RkQ
z-aypU6hM&@6H-7)NDb<DVlqh3H}26j(8o^!j}=VlXL{xUKcXulorJ&_JY37(=V1Ty
zbS=V*vnfcRe~v-;z$6n2!i)LCC5g$8h|iz!eGK|?dH4P2U0?C5dG`DISA);a&9+tG
z_S?5O?9;Da;4hY*m^yf{W;8H~4JU%H^#!Q6VJd6^LI(Wl@1--No|Z=i(U!h%I%1eg
z(EBi<XhRJ1W`47iYPy}ienW`Aj7YHWuB!m~2}nraMg6py_25@Rg8N>N+Mt7%^WAX^
zKi`^!-DvVsVrT#b3OfKyI_HiFFagN}wiF;|-`)*Bbsiu{!OnmV4?%$FB#{^D;?>fC
zd&S+k+X*moPJ(Z_UtooYfQ}n6PjP`H1hJ07g5SXLJ%oNE+jree&QF4Q4%F}7ZVr~;
zq~BfmxeES&nOEN_3+s&vy>(5_9V@A#a!$?|Mu+7upA&7oj7=DP=HqJY-csuthdUQ>
z2p6M;F^8BBpNEQrG=90oiho>kIrsAwo}(Dtuz05~K8Zks%XPW739S-Jm@PLuVLqo?
zy5VQkC~~eLt=^Sr$YzO-x&|&;FX81h7cAS4`-AZWl7}s1p3<awEPgyqgVeHR^5ifC
zv$-umHc+g~;5+L^H6Mf2*8C!FT;eEB3@}~)RnZ#~^0l$tiJ4n1+T4~irncQaCW8KV
zIGsOfV_4*yLA@F?X1IjR`LA9!it?=$SIS>2EZ5<Orr)3pc?(wtzIjYu?Id$0v=DKf
z#6jcDF0_xmJ`z*Z!M>&yf6-jGk0Q*zsjaJ0ONIFD&-U|N%EwJFIjC_rVzN|EW#08^
z?GEZ1Y2b#|W8zXy_-M@fsJl(I&_!*JOn;Y9&KK|~*TNLw#pPb?*udY82&$7kDU^J1
zp!mFr4M?^;3NfJe?FaibuU~hhDl8)HzXGmJ-|CbG$95J(B+P?ThoeEHtouD)KE1Xp
z&o)rVHjMXyu2C{l@1DEG-RhNrf00zWmLi_pbgKqt2Sf@@9=ciX=Z|QY+98Y|yxyzs
zJ><r%(qBjOXY08zuG8ns<L~)@R~<xOw@*eu@u>rDT9#IvG3_rGmFR1@PH^6vb`{%T
z^F@p2;b_D)<BF@WrBuiq7}2qVuBw#P+b!rUSAIVs*2P}%5-0aQ{Y3-exu_#b2?#qn
zwv(PmVpJa$!Uo30e|J&C6m67ZY$#9NHpMqNG>jgVI$c)Y9&_x<ewy%L>%Q1T8(O4I
zC8S@p@YY;Q$F^P>5-BHEt{qHKp~xRxfL1@Cp|qv1HOWJ5?2Q%i<I2~zh$UKL@Uj`^
z(K8)nJ$a69PXx)5Ops8HL~leG{L`Y?(Ti)vnOD2;gu3m{AjZ)RS-q_$(8fpNa%1$t
z^`cN3Zg-aDqdC=S)BDhcA7=F-38kmRA6M4A&=p;(rJ;j;xC)z>1)F(&vy~`0{^g~y
z>ld(a8zV&zQ<2pH>|Dt`VoT@D&Y+DiiOj*b3-cGjre2$@?PBq__YKiHTKxEuYZkI)
z-lbbI`?yr(ZP!c^?}mOg+jf|ZOtd;20)nImHJgduY8#RN$bt7civ!zaN2H<tjT&$N
z-(o4s-sH`@daI+`&oqg;j<`em!~}IszNI9;4}h_Z)_m0r*xgMn`y^h``R=E`m1QKC
zG(nCuB!w#$c<C!3w`X=OTID${Gh#5DN$ipW*QzuO3$;>>#GV6dTWZxRHU2%w?Aw?&
z;`}-2w(v8LYy)=)7PhL)Lr)X68PUFIlvq}FM3%*z9(J?&8A38PeehjpnM_HaGEfl?
z=1Ju<YMS-<?RSAPK9Re^sjo4T_hI(1W&_kmW;%M^D>IP?+kX<cq660?a>TBh`$wXG
zZQ^cjFTdL<2EfaI>~(eCpkn2bJHeKCmeA_Ojpbw2Zy>b)=?0?qCFeMv<6(y{sy29!
z*D24ENg-iELAD?Pl()Bf$<`cfx<-kutZq!)<yTVw%LzeIUl2?@z(XJQ#Q!}+CXUM?
zNzwCI&@_St^*q_H{^`ibeQZ_v*O_>KUh1oS6_SgmFc0SXMi##zU?XnlRy%m>e)CG2
z2kj>o^$DkObd`H4rnwAV5UrtJu1X&*f*Y@3HQTzzM#$uewc0LgWrNY8P}e&bLbrug
zX1|CBVvbO4p_P5ilHS*}$QDSG#c-y}lk$8}EdI}&T4FP*uyW++;P~$h(1FbGrGQpD
zO}k9y6Bnt%Mo^@RJAvq<2Lg9bCi7t|`|ia-g*qXRy4k?2(HFZjK;}TrhRbZF>WrjN
zUqo7&@$3)E5UJ>9iv-q)WR55mB`}#?MD2=r*h>6nPuk_j#Lkn;X}frx=)TGQf-^Ls
zM6xs41HUazTHIYDVp(+!n<Y&BdNtCyEWG;pDt@YYVnsfHa=m2MOwaLXnx@B?LIBo9
z&z8|kt#0V}U8h2Z$@n1(1>H3cdrmR-T2EK>4G#Awcf~IMoUMhhucBbRkC|2Ebv3qi
zh^q=}iMHEWDxypGR!uv!(q*z!oC{1!?(&3m=;V!{Es*L2F&*cQ?K!n8y9qQ)Bom5n
zV&~lHL<`)LEXmpu=r!ZPiA`f&+(z@?UH<70vRr1hlc|(?&Rge^hnYG2r2d+=?V?J@
zq*b2!O|$~te~%7rQu1Y!Ta0e5=$b&-?q9;xRp;5gl@K=kbN2SNw6z`yt()|nk6F^Z
zYwo-`%e830$%ew2=xF!2p@p<-Ip9=Rz}wEXFtxJ%y|CP)*w-(Lc_orL^}RN`*=BEK
z(mV8cyfk3nx%Lr?jhi5A(|o>p>@xIsNQ`7@RKBok#9hc_B(t7Ikfki7jvePFi)7{2
zx9Br>4$oZ)s`z)SpF}LwW9t)phyQxv)njEy)ac|Kn$igowVsw-<(PE3Q|JKw<s6ex
z>2Pio4!MQ2NQ1i{1gRZYb<z*q*_L%xt0wn$eLSoc{k37`lDiG^{h@OGxYYA2rYQd$
zOSk^V5QQR5#m=uT>z?wtLz7vYouXhrU3;bpp-uT&ki3?#UP`BR#*2DRE$$BU{G9Zq
z6{6kaboN5`CyO*oS!9AIf{$3}r6Z}gZ__zGDWmL;%yjof3&n85ms?T1rl(hCT1DqZ
z>!??^rXL!f+m)vk!4>t$hjM1TsMLHfUlNDuLnC{bgpOu~<!mm?+MH)xGRa<V6kaE>
zN)-R3Q%>9|xs2hx`?1YKlyimCVfORUOq`h+H>(-$etzg}F)HHxeMcE9%QNgxtRU{`
zl?mO8*DA0t+a8Rg$GoDjYdQWD&E<m<`HK9Q*D?+3CdSgx)J78jx~Da{m4^&B+1NHn
zoNrxH!Lg4VD)0P+dZ<PWr^1vvQf7roC6})pwex{Nq4Cxjg9TcxK$`0pV><nIn6^fM
zdL-Wl;YXTr`**w)d?S|fvqBl5N$!aBEMxXy+8vehZF^12W+k{QUP$u&R=?+Z<B&(}
z>#g>I?kL0{Q9gZWEI0r2*|L>kOrJ>w3&hubT<aA0IB|g)Q6AtEm~rvkbT`KGi{2j1
z2pjMHH_%j|8-DwW!wkROnh(HL;!FWn))(G_&NYe{ZTs=9rw?CtP&IzIsI4Of-8#;8
zi!L<kRQb+P;Dyk29x|%>QSxE*At|O^8Y&;@$m>@Cy|8eEI!#EldVq3wse8)r3f_X>
zN}en<_Fg@<)Ejy1*fMqsYFa*b0QP8jP~?BN&xf^C%%|O={g|}(QA?W7_efB5WXJB-
zy4b(^WV<yC8O9QAl6FeIXOL5IEjldV|8~)t`B+G!e7mqMC|PpI864PpO2K}SZq5b&
z!GEDK%Q&0Sfe%E1jECplhZLC^LJ~~KVJ&d`5dVX{*w9&lLw`0Z|K_tKC<&V#0#`2w
z_gAUIB#x<(Ua5J8GZ~%EieW1?9q4dGcJd|rXuW{yRXvh^d#RRcTQwOS31b1>gmHSu
zpTgFX8cx{eQR$lpqPs!;W9NnAN?2og3e~FUs3jV+geLVejfV}b!^J&5I%X}KLHsQ(
z;tIMqFo9AD^HJ3vf$ixlP4SlpwRbDe>?NA51(kE2Z|Yq8ES&hLM_%1#3DL21d-9tB
zsOPHVoB=Ob3j9Gij}cT9?SZX5Mh@fkY7p)Rhfx{+1L8^)^*}>+80o5Q)j{=3FP}3d
zw-aR7>1bF-dO*c}&QN6Z{d*A%*!f#2=vL1|DXZNv+z3}SYi&or%S_@^b5WN0qsbA0
z6$*<6;PZXU3YO;DwJWn+;)Xn9>iaYrT9~AMM0AIv{Xjh)V9{W#mWjH}tdn^&Xk!k7
zH3UU-%uRLzn;Y;8SSc$a9+%~BbUCW|Qn4JpcQQNuWhiBxSq*r~<S0?DNb71m8<Lh5
zKNVf?MN=ttTpwLM%~u<Mj4ndgCS1JcgXB-wL~M(a28jKelDq6Li&p|fh}*fR#ft^C
z!e~sMxU|khUyECp&EELk#g`5#mcGQd4-MD!O3g*DLc+WU6kaqJr}8w;%&;uRlJSaS
z4!0EHF7paA^7T^^)PuyXI1~Mt0!mc`zrPc`XK5v<ceO+YC(1=C5`f%KXY*!cVYhV;
zg<kNl#`9MV@}YUZ^pnsa(PeRclF2<A{I3-E55wd<^#r|qKR0Ug#w}0HbcQpO&$@W|
z2aFu!xXURd!)Qn-={7Cxd4Te1?Iqm{ul1O?xS*=l+U`S{vdcs2r&7DXf)E&x;>QxZ
z>Skvt%d!x3mYJ~G>xNIW>eBJ1SdmwoeDodto_$0^Pt&!H|9Beb85TOJe|$|Dx-4*a
za%-5(Go;h&gQ??Ad!bJ1#63VevutM}UbIGvMq?lbBU8>;{2^KUdyL>z{e)@Ue!I>c
zI=OnC5HSa)L$SwPR?rZuOV5e-WQrB_+-WP2R!gU=!8X2g-6FF?P2;$saU9@(?q>pk
z!o+IFP|WDJ;~=XWQ9Rj3y5K`!q1~P><F=*0-z2!$gEG$kSx#)#Mq3Hx8bQ}Pqxi8m
zGH@FT#?1?z<ncyLiq=lh+`Je*ftSh|j?k!q7s!Gx?{#D(r~tx|@z`ZdC>?I2WS?9M
zCvjpyPE99}64JVOBYVCmE5*b-HFbI|nF(Tj+{n4jZ5N6VMtcDm(S?Q+m7u5&wp}5W
zeup8ITMbpxg>`7a$6H@Xkei>~$*V1`ut}(z-9hg4E&yiLWvj13_;=G_R*YZXr9ad8
zqz|^b0r-6;+!JnLa6j{AvE=U0_ptWZX8+%#v;v2OL~hn%I&ZL}v#Oi6Na$7pfuX$*
zReC-5`1qk?A2G~6F!Yka+Bd6e=^iprpK~3oZm(%Z{H$}r@15~ECiTGcKP6h*5Ui$%
z<eSYvu63o&iY%Pl`!Ux~f^k0BUYQS7lKOKz3O@XJS9<2@Ev`(<Cnk&^^VIjWmS?L^
z&qj}JA54251?aiv7PH4sg*mRb-IUFVR@+oF*U~P``=<L5RG*oAXz#Ppba5^X;_|Co
zcyg9wQYLzf&7M|MGJ?Iatu#0HdlhmYhUF0JP;sF#)lN54d2x<ul?y&DStzt>af34l
zLeU>S*5Cri71C{$s`?u?V!Mwl_4tqGX-<tHlS}l}P;sk=FuGH6w;r;prE{|J1hC8c
z1nT+^Ssm$H9x%oY6DF7Wf?eNjyUq&jL~+$$CifcYW$CYU`1qc9hmLFaI@ey?!I}Bx
zR#2iH>HO%Z^s>)z@F#ydM%;<lGtDrww7=7&pfl>A>?nQw_|b;sNb3AktpcN|NHakl
zh3S$U44>gDQFM_*Q**>Maa|T*Fa32jIq(ID1SAsqbwgTcgl85RQ+WEWhE@3U^PcCs
z&+p;AW?`a`nnMe9+eDR$$1Yq{feKVHgnt9Or+ZJA7<GfF1E;?wqF(*{6GO^#t>G;m
zzdHOfTWAZjd|M0YoJGteKy3iqw5*riut@%DNq)>fZcEW0xGKQ_v5p`)7xkbPijN?g
zCtPk@@yQYC)=fTN`FTqucDd&T`<4j!t+ZxC+-O9ID>ZcDBZLWMg%>7mh2H9);*HWr
zcfDjgJ4{1PNm{<(vHVGYmV~^cd~;CVuB=}Qn%@lRYU-3Hqv_$DYY+$+6-7L~JgZ|z
z1vtq^xxnE%_Y#+p%*&_Od7<W2&#i*t>4g3g2ot>y?)u%qc1H2GwGllfu-#^2ya12Y
zhhk2?jjxhtMWUii`%xIOur3A)SDD|?N#CbvzwR=jwv^(^dGM0WM<g@Gkgh5Qy}1j(
zXWu5V+7Xs)Pq~sH=9uBp<608+R(<f+TCwKB#{_~n)SgTNo@{x$<!3ieb&6!_eF>HB
zb}Di<7n718bXoFIo>{|8IiHnKAn3EWvC;ucyp~cqkyLp_T*8edren`Pay&v=$blg2
zZtos)QL&`~t6vgLtDJv)U{BMF?vu)AZhHUXAk|}zYt5oa&TII7(MbPZp(GAsI+)tD
zl0%Lf(!bWrRcrfeHS(dkRQVy;SqGQ>Z}@TaTxn4k+?H9Qv+}URKv-HeZ&@xH*|pLc
zzT$kOsahpyBh!0B=x@1ZhRL+92rpcBB}Rj`Ury*unLc}Myn%PrGyUN4aZIlBY!=^{
zUsmn_MHLcy+Pgwc-G#WghUiV(*hKbr5;cFUU42Zx&1!ZI2TI5%m9=P=?!jNL>U6UO
zyXfsqN9JW8J{7v=Z<>;ZcptJ67biR_EP88ZQD3~=!8-9&O8qKyolgwqcdv*s!Re^+
zL(4nO4dWyA8+bx-=Y{VQ@uo*Jn&O(?{xA@?S}=QcBp`<cfq4y31Jx$+3)+&u3zCJK
zTOO3jmdA`?Li!d6gAaUaR#S7QA2-Az-L|f}iY|uV7scJEzLN05H(ULk6oU+jK_fWJ
z=mU61M!2qMcbhJ8+}VZY`QSYM!r@em+>^FVg+GLn!!FQcU;mCa%O<WI^M2W_>Xr=`
zz1%5oC$)7xk68@Oj^X)(at)cFQgI>7uYCQi9l?s~Q(a$&T$P+}Ud9%;;J>5GLeEtN
z-j<pWYqNke(V3s|q%}@U{n`5gAzij5Zm}PQy5+Oq3J<FvzR&QSix<GtlW|2D>8BE?
zn(cHo`K!#BJUgX(ov$9+dj!X)Fb(T=FZ*OezO99wdTkXu;`yOJ`yPEl@D9HuB8PqQ
zVL6gzz596HnC~P`obFk%y8R6^kQi7~hc(O;!x~$~)uu9NbovE~H3(vJg9F`D0FdOJ
zE6!TI>Oh|wblxukCz7l-e^Y+x3uvwuy`@16H?@Y~3IR#brAP&)u!=TE)V`LyjG5Be
zckJp+{FZ$2S<)bg?9B0H$P23Lk8Cp58@t65^6a#(9u&_Ca<Q+2dcn76(3qcf!}pgO
z)GiEb#LSXA0x#=)l43<VP&eH)(r~EGbR%~*4;kk*c|uweFGMmH&|+o?Djq*4eNrhC
z4*6P4al0D@+2pQDk?xqVt-%z~G%6dEhWTvSMksieI5#RqqVL$*C!JAgRMqX7TZt&N
zW!tvUFDT(2CP~{|>Yz$j9*QT-@t1ejHZVhQPEy<J1iI8xH#yp*S)-<_-#~Z+yiM!3
z0};Dukm0b6N~LHP&DFBa4IVqV-ut_>2Ke%gQ88G;@$NZ`kFwFNJ1ysrMTe!1g)rmr
zWW40XzmasG<@Mc8FIYlYnJU(mQhKHfmD1%1cHti?$siC`N=YLDWkdz~^QM8T6&!fB
zVZO%tck6LLk*&u$7dbN`N@k!!t<h8kl{}5&N$k#aJ)bcI^TmqgR8iGNFVqavp<!Vs
zacL82kb%`z+0$c#s0>KUe_)>3;(<3m9AZLkU2SUO!DjNcx2tS2k4}eN*R6TI-u*h-
zMvn9myk7V;8XwB3xmdkca|j=vX}lu;`|I3q$xpD`16a}-_x`(2zq37!+VOE|+_qwk
zDYcc9<~L7%eX`8scn@pt(_l-2!wr4b=W^wtY=b`YXIEE!3DI8C`*t#xt@c$U=J)on
zWz3|zFmUgX*x^hfx$M)%3ng1*X#F=Mj2&B2bC0b2xNy?<b}qenE~WX%f<5uUFRoPM
zZK_d%m|y2pGQo;QP>nqko{Wkr-Bd&qPQsir+f>kKvdTBAPwDBlyMhMT>jf0-Dg{c{
zRb6t7pwnsj0|c)Y0E^8W7mTZ{Q(hu1S|0@p^Nl{P^stI8J>ao&(vgAK(vNFxuQq?T
zXi?KCQA|-a<k9+G`P|8&y#EMB&uSjldeHwY{2~7%Y{$3y7;2k8ZSzu6BK1CDj0f!Q
zNlr*yJ?Tl?+Q^CSp_6k%{*3^+he}cnMPB!Y89ot0#=QlGBR?*|_cBixy@LI-OwML{
z%e1LsW3+pnWBNc+U{hd>|1&E{<3>r>!Br%H#}%Kdz<YZi5_#t~vb|#|6`;E2f!ggT
zdVW8OJadf1<;eE2bO=Y8iGGQTpYg?dvTOdhnm1BXN=}ULwx`U?(FHIv$wM#+)Y6U*
zi!nBa+WtjSqvi_?0FX5Jxgi6nYl3xzlNkLWZ2Oqo={8u)6Q8u)aB(H##<jp^18ITD
z-TKtanT|AqYT6V(Oh>0ZczJ&wuBZjq57p4tkm8!FW|!vV0T2HhJL}e<sM_uhea>hH
z*xJZ-)Z3zXg2C|s%Dok)1pF3YIug7dt$o3#5_#Ypn7N>@UPWvIht@rTm*>?%&|<$s
zq{+9>_7<Y+NP?<I%(y`%BR79`T42+)fy;0D+cenxvYy4zW7Sf8Pnl@4{RxgG?kBEE
z|MZ){|3u2kQGE1iOT)Ks_27{#M#V4==7R!$ZLn>>t&UY3uSvHQgg(bNo+AhszC@xv
z>{&ilf9|eD^w_^Vje@xI!DGpmRg<D1&4+$FHK2#ORm=j(pJT?c_;pm~v(BD&_WWzE
zBri6dXdF_{5}NiIsJJ|;d1G-DBgYX5*{(f|X0oW}T@0FQA|}IncExKjP{4kH^`u?r
z7FfcK2POg8!dM19XUd~fE=vrVmyVxNSKjMMVWo#)g=Dxa|4VCH1(WHt*v>XIvVuzQ
z@*4osh*I~zBg;Qb^glCH{wK1mBB~*yA^uOY{1bum--2b<e?{#uGO#fbu(L7#f0Jbn
z_WvhYR{B?#%|Fs={xd@6zw+XL(Bc2ei&_3{w*SG4nHV{LX7eAsn2~{#ll4D_{|<~9
znV2{j|F^)n=_fMg9E(kccr%8<MZ%qOZgUfcp+`a3-95FCU^C{|94?Sj0ukw^ZqQAP
z_jUba=07QM1?#<@JKr+yG`EXN8CHc68UpigmVw^n<ir?Z2!g!AJVJ<_ogK6NogJZ~
zf_aekwZJdLf;r3JOx+#?@H~DPl1xDX^}jMmCs2*bK^y=Fn->6?8vt^AgmQj_aBu+V
z;9)!dFoaV=0Ft@fD?qseV86pS0(2Bke-NAAlmWH1x<$q}4-m#=IzVPn&<BlwEAYUD
z;FN)LF0cUPi4|;Jm>CQB8Xyb7k-_isW&aQZlv+1eR}>@j=bIY?;I;M|!OTiM###WL
zo~k7P*Wb`1tGFhBKd3ktrUCqOG8#4+F4q#)?Nz%9u-4@ffGZe4ArN;3=U@lu(ZRU}
zfD<@(6cEcP5&%{FcmMWKq(62Iz*i}3Kw`}Ow!^o_Hx!85%Q&Wm9bBzD2$x47ZUsP!
z0XKqxs8;;s;(RhFfJ@DbDMW*FpulI~l|T*d42;#y@@OD{B_k^Ug47MZ7v0g6fw($4
z8+y$qedI-VpFlayp#dRaZ8JD$OIz+2R}RSvrU@KvJNo-}R-<cx2gl0?FikHUT1vNs
zMhj=1HD^H9hJaJTcbb6c-)>Vi0(Ag-aPTkyaCCqJ5CG2%?fP%1>eEArH|fz=X&BR;
z<9!H65Dg*8fRCOOf)n_%IOrK%5C>N`;MdoW(ii(*W22BY0DsznCIr+Bz!$b>^NfBU
zZey(ZnAec^1ORh+X9C3P=lJJD=kA*VI@a93*?y_}ucDeJH5?Y|e@wpUXSHw)An){#
z4Z|87?i~U+I62$`zr1n)`2AqxZJ>{^4}B$7f|sB2&W~7%V*^M)h_6XNt&Z;-hi`Kl
zjBia$vL1hPg-`)=Ob~+)-nUSk4xFPI`l)|#<MG39`zwFZ*Y23FZ#;>$$*C)P)_wiA
z?>H<A_@?`}VF0c%yQ-N+g&={{{#}1rR{{U(I;0Vx3$yQTElza;7v2G!ii<B=;uTS>
z3&=VJ(Dn@NZ(iZ=Qy`WO+!eq=C`Z8WHXDG7OdOo=f?&GLR@w(4$H1pEdQ^xu`R^>H
zv0fabTkG2vk5>QzbMstus)k2}0qC3EUv_^%NA9a<!D{L`1$6T004R9*14sujAJxTQ
zJpeC^x<&qx64O4=9iD^SLwAqu2)JVEPizO!zF|KAo;A7+fhBs%js#Fc{T&6u0dP^)
zjmQ$bW)BimdW(Gvu)^w3Y$227A5VY+__GC8|Fir2#Lv?P9kg|qQq_I9@jdpPJ?f7d
z1mJe>k4Vpw#yJ4D7WXaT;o&EFap_O~u}I+-#ZSOOdIAqonRICX0v+f-{0&NIH|3uu
z3~%-b{;RnDD+^~0-<n?B`gcD*ngZqvJOpjmBtC)`_Bua7Qrsp!g4EI%`@t`yS7N4z
zE~BuIDcHu4uBk5t;pwXvZP-WGUtLN#U*!`#ncouM?IS!h!dybjxOeR~8#6a@b_gd`
zpQ)te<P8A+NnyxiONZ__6x`_Cxlxb!U^6q_n|V=vS7(5)!my7H@4!OmyI<hH@cQ=S
z^aMM3zXKGQ^!x=V{I9!r{4aA`|1c+2pX)c1xCnj+e|sSZ31kyEFAQ>qw>tcte4lw*
z=VD}nU#gszC_m3qjPL@E@2GmX_vBlO9I-bo&Ki)VEsGGJS4AJ|FFEoVO^dT1=7sgH
z3s##man>~~1Y;YJ*SzQ7)P2whq@W=vR73taRmA2Xm$aI$m4QWo!Pq*P;Qe*`>~y_g
zgZ_EpvO0^-1F!-tRaIL$H=?sY;KT*LWAyOXVp&)G0j74}F$<PygXOS+%&%`SVBdaV
z-B|LfgAHbnSv(~*tR5VEu5}t3<<07~tp8Txrt`4WNh~o;0wSUAkiVs8ChA^KI;ne?
zFa0!l6dPACgHPm{xs#;SeqioFiI10!ltwy%tk^0bk;JDPLB0GZP-q){d6w!=sdX@<
zIK=iJPuC9_D0CW=kY2~CFO!E5?_B+F2IF57ow@N~X!v9~wo#fPT@YphK>MDwp&QZ}
zja6{4*seZS{E=^+I}&|k4xx4NzSBptl?tVtqQ8v{;NA8ewf9YA9o8(v>@d+k9ov{p
zKSheooO#>D?0H(VNe~=ywo)_@(a6m%8}tTx$CRV-6NmgWw`<LbYTSdfZazeHt@Chb
zqSnkbC#XE@n6(7-*I42;H1I<GQBwKY$Zkf!;uA8(|N220-Gd3b1wAGWw+{fBzu^cy
zI)hZ#RZ)TPPT{AY+fDPUpVyok3@VCfcN3f5RYsVg>z08NEAqeSsl~8MKt2(I(26WI
z_uoCGw}J>OkDBU0hypLK4g;H-oi35$JRIly{if%lU_|;QCp{OIcM1L~nzOwMBp^?K
z4$J>oV@dcmD}Ylje7;4rny+9c8A|69agZs$I=Ej2{4^R9K?Tyla)5o!v}U@-->_1}
zZEtktGYb<y+7k)606y8P_!`qOz}}8Qw#W5x;MD{u*Xn|V{@Mi9E;Dve)Fj458}uBr
zdci6KW}_tlZU>j0^4gg%IWks8wO^M7+OEbBOZilGr!WqHe9VbE93wVscFWD6VQfAM
zGVE5dN@^2=2{<LRyn$r5&#Y$|0Zq~Qlz*Gy<qI@$8Yq0?{L*+mnVMLVJkjLXY@&*0
zHf%WY#oVW|paz&Kl{25}Y#BPR%m7rCNS*M+Rd*$|@wCU4J%6Q*4o6U$oi)P*EhA-3
z(99J;#lw85fky1n0D=yiwG?#mZu|+C;7O{)Sccw8waMoyoKgHxDW(s^41}*VL$~Y4
zFIP;WYH)aQ*x`}RbPPFnzv8@+V))6#GbP+RnORF*Yt_&0$~Gwt1H!!w$!>L*;C&WX
z5CrUJ^=i$0UR;fhH=50bur-N9^Wfja=~87NYW1o?Da1~g?H|I@XH3W@Y)k+Pr%0qx
z>P$FXsF4!xGr`8hchyylo0cf?Arc<a=b046R4siBZCq%A*zOAO9di{B(Pi`o(h%qH
zr>a#YoW5cdN%teapj)fsNMO;G`(-Axd8SkHH>IE_JQ<^A<m)5RZqtGr&(X5Ng}0q}
zG&J+lmuOmz7jyHc7cfkyh*Vv%wjl3ItV3>1UIamL;J%`nwC)<6kmv|B@8E7pK-t%Z
z5zawy2n1ur`mUv1ufx@$6?cnD<_ZX4#|DH=$xUUeT#B_u8KG?YErfSNd$KX}8ncXB
z?5=Hoj!29#6uA}sgK$-9J7;_#baiQSSE5#C-FV0B38RZw@ZK{>y~5GH{)SWTH4~=>
zSjl_Gx8cn#sJ+Ynopp5Q^L~zj8aBNn(ajZOB9s$of#@#KD8Iq&=>$nVpL_~h<{=v~
z`Ijc2uOvpzGK#p1v;VJs9rxLqn&<-gb<Rbt*3X7RRff7`Cg}VWCmnma(xQRAB)o16
zO4cEpZg^$R2}dO0(wm*`>UGH0)U5OCUUcXTZL!jN-(ccImYOZwn9bEL_p*B?Ah$zg
z+)?7ATTv*UGN{;gx@q#{6Pd4TOs9Drb8vO!*W0<ao5tao%W?TlF+L}*I@idO1kmD*
z$|KxgU(9OP`^#H+;q;dc{^sFAgj7M&4;P`t5=AE+>B19q?={S1Pitm{+)4o5r)6DS
z%VGFMzYFtyZ5P6b1THI?`?aWF9U(773bZm3F3Vnv`jWWa@p@MWH@PS`VeuV*snbQx
zw&D0ne@wgOp5V|}IDZq%g6F)|GroJx<!bK_aV;z`NnfwVf*5j+Pg9ZtbW>l>prcN*
zoE^NY>Sd7ZuHs>ie6Gtu98A!7#7o(GASPkgalER@AY@ao<-InI&G8+~_S{znmg57x
z6<dw2(}Sl3r&XcnQ)P*5xPu=B=_l`nQ@SF0gLZfE>OiuDUZh@CxnzSoEr}lb`z0`=
zv7b<k_eBz(43E-1|4MDTR=iMKeVsEQlxq8)iiWxB;=l-T{mxFlmAY<g!9${4Lm=NS
z7Lh=bM_!gLT~P1Yrevv^9lt}ge_Thi2kkn1f6wyJdClH^*5~nv<w~-SYa5p;^O=l<
z#p$Y-Ue>$gg5)*-LI@-ay-QET^F^1(ExboKoSKF%NkH2l^m>x=U;j&DSDs|@EZIyv
z;uCrjR<d8>w_cq_kAM$J^kD-rm(+GGnkA5LRcx<cnfLN%U(|fQgr%rqP0oi>{pn_0
zQ%VemkSraqmU}RnN;BhEN7q1G#pa-2VjCFt;nxY;y-n#)Gz&U7-7o+A!l1L^W3GYx
zr{N2XdX^?O`@%X;dtPxLtxgd(Yi>YY-=jLebTVmgN3!lc5@(<uJeUd2;K^7V$msl#
zYGao?<x-FHjTj)Vio=nBcv43N{J3#TA|AKZ^b)2-(G=yw2v>GL1B{J%&VfT1HzuX4
z+sNESKS;wS_qJt-1@~YlQU5ellA=l9>^==AHPu1f%yrE(qSM0KDT1UC?F&*YX%3Z}
zM`Fz2v$Dh#*N+!zlF=CePW<}pw+7%C5H|C`PLsET=elRctI@V-70vKt)KH_Ptn6(-
z$h7HSQvB5Q&z{tlS#P>l-RVKJ39E?{8bHyuVT!l2Fdat<5PCY#T-wasg<YBJ+>uzU
zbxuSFjEfsb6C5h>kJi1=F~{o!4B^;@Ugu{M_~|%Y9;S&TN)=036ukzLf|=GcDjV5A
z@?3p=@zRGeb~$D5ze$u$e_AeHcono0qg~}Mh14@=jOY)Q)e{^1%$C>nR)UI`6)jVq
zrqER(JB_LhVMNOlW;Po16x{Hyo2p*Xsw<^U2z}qXdLO%Z&vcZ}d1X<|RvH4f&a&!;
zoXys%pPN54HzsHhY1GN3fES6%8W-&ml#ntT%61pUQ{Gv`X3fLRyD7l%E9wbIoUxmG
z?NRL95M_0JTIt;)UpSDHFE9USmaq*LA*faj9aLu4yc>nO43x7Irf0rRYOfYhj}BkT
ztf{AQo6pq)Ea-iMzTk&87SwUh;dk5Y{nBd-102>j6(JvXO-cW?$^v1#J)-hLrqrST
z@F6FTI=UuHCd6P`@$?Ba=uC;vD34MQ-&VRgrVkt67$xN$sgy9?6)>eqO}nBzzs)CA
zek?-3+gg)mbYZo4@MK0oF3_Jb6Y=LW<Ih=0p~l$sr)32P0Qlq1#3y0iAJ$m`=1m)6
zqhM2cb1d2F6+&vA&?1K4g2CN3^d(?y*wu&5r!@-ewSP1PdFOR%u-2rKXwMthq%{Ka
z)nj3a0G3()!bMXDnS2t{_N!nhFqRl1gN<$~33C?|)$v7<^79bQ3V$v~0N+yw>;cyg
zdc_)5rKR)LX*qH`Gf8H%w<{h)7|T_(($^{O67<aG=Px<phk46}Pw4$_*NfM&Ew>kn
zN+=8`oDY$G)Et))4H2#ZbYk!iE~DN0(@0&BvsvjuVqtG2N+HEonBRTbC4DhmCMOw=
zTh0lK4V0o2Y$WFMYW3dCogMWmDQ)5WY_8L)@knGiJ;;CvxdHez6576ni{c=%F*vxH
zX})Z&yKc!@jpu!<#QjRO-(C1U<rO;CfLB->bVjc;Q6@1JRy%(4+wb0~*~cjdy7q{4
zvq?In5f^sr7u{N5>P;30)J<2Krim$fKn?mEF5zcT^M|;(&T;J~b`*h0m1~EnV|nyQ
zWklEOW`K}(DET|*8!XoT3GWkY%Q_VjHzLFu?o}z0StX!SZHcHfnAW}D>LM3$+?06@
zE>|TqGHj~^limEZ#;w|?lR!7$>JcM2tRxG-Z4$<;AO>b$;T6bY?5V@bbNl7;!=Zz>
zoy#Yi&~=~F;jPcGbCqYUxN20UJ=^^*m_%|On#oD+>@GwlY5U$NE+A%?w`~$Q?n!8^
zy4YX28ZWA;-c8O+=DAU_&d~)PM?c+!Od_5n*SHXp;$`9ZWDcXOVOI{EF`U`mkwB~|
zXREapdc<C>TM^&auQA$fslwgE3t1&4ZO&8F3Oz+#<js<7{neKTuodSCF)C0-MOdj8
zAe6T|IeBAq!54bW;n8IBo(kTWpIh?U+~lagoxiEld-#}@Dqg12iqGhMm4{_3Ti-xg
zDP>J<QvSMF0!((TVO+{(LqRcI&Z)|SLhfx2z_(2o)R9kz#GQZdb|{#p;d>a48%=}W
za%;*H#7l{LIh$ppf%y63ciXbbS(zM|M%;wV1F`KMrXUSorw73v9peBj$^!H47ti95
z$rs2Xnz&{ZN(CAZ^Js?3a}1u=q$nrr6C*vO+d_IJ(c{;4-awx#PjRAB&Sz4_C2^<d
zsYDsr0ad)?MlQRkmukzmJIFb?PJ!#kfTjRUuZ-Dlp!G>Q!Ey4{C&4qpu$Lv;yTHB`
zG=#2HSgww*U*_jF(WRHwH^md1$w$CI%>G{L`|%z(+j=@+y8>GiMRi>n(uK!*(8-Xm
z1p9gK(=7LW<)mJfPPqFg5?hl+%j4l*nM4&L#`@RJiGoCiQt|5AY#|JiwahOkY-=B{
z9vje`@NF8oTWJfBHV}~#^w>$@)0?tUw6A+t*f^)=KzC>TR+vccwI%r}hzjplyu?`H
zqC9>b?W%UnEv6?W<umE8cXxye(zoz*s^PV6xsI+1ry$M6va2wC-Pwp#;^mxQB-$o{
z1WDqdCkPN3FS!?B^)=wT91KOh)c&x|aRdbhrBE`>kz=}dJ=f5gvkqx}?)T@6GHk_*
z4CQzeLhC+!Gw{L7rq#Kvp6;TI^XOa4jM3dq$4?rf<o=q9w~4FSPe<P(7dXtlFVY{?
zq82ep#t%IjgwhoXT$x!-cSs$?cpnDQ@gSWkq3qV<#v?Vye)}@6>S7&rM3Cp<=#MW%
zz3Pz89B9VK$mZ+H6*FOnZ0zi;c6KL<<c-P`m>+F9WWHaEk<&Rn870NUqLL_E%$SGX
zg4;Fu`wH`!x4i=4RBCO%0un-Wwlrw<+`y;7k@edCg;u^{9BC&`K4wKL7Er_B_ehfq
z@(agy+A^B^fOY|W5Lx-!I}h)}u?PhgSpHwZ9fdoKUq>X5T&}9?Pw^^_d!Khp4A;w&
zMa#;#U1jFibln*)@v~$l5^N9gNYbjLec|5{U3)}6r}$M&%z?PIIFiEVc8=%08VZ=~
zYM)O!Ak_m*i2iU$rH>jaL+3mp(C@;X;Z4IIA7a?1LkKM$u?wd8M#En{zw~x%xwEC-
zXG3{qeP~?zz_TM7nTwGKk!ka#<JzP39}=hj3@s_9!TRYeELrk<od;0PF*zH{icB!P
zmNHXsZ#qm{D{|NqNj$LHP_82p*5k**wg#0PTM3#l#+h;U)3ggcKTG(AzMga7D_4HX
z&5C8?5G3l&a+gfcL-%?7wAp0~7FD}xI<LL4cwXn%uC)2!!~PlTV3diNW+S%RFyg<9
z<MSu6fs#rfOni)Ieh%l-Ht>5{+c;01l`6sLN8Fm{=ZN{mYLltxOC_8|&!^KM`C*yg
zBD70HMc7UPVdL*wd9mq{jd`=b;Z^4N6B^c6x>q2c9ldz+CtG(U+PG}M#*_>s);;vF
zpZ3>TvY><dGJ@(==lJ-<RJuq`jC&SddI)JE&?YOd2vwhh2>bNbN>S3^Rk4}?cbqT}
zr<Lpe`cwcE<hjU40UrPiPxmLc`0}@7c~*0E%l+ILFR;Rqb4ll-F;MRmz}JnC)XzPp
zxZ9&hYq!w$`#@-m4Mp4x13aeS1%imU!$gNBTo2)_%)*b7mv%1TVy4voU?w;|-*e57
z0Fs?$?Ad_?ndz~aw@wHC6g^S`{=F)L@-!5@&m{LhxxtRvruTDx<^&4P9@3@-vZjrz
zQOR@QX2;AQtUl{9%#xfUC<M)((Eg~*cu~Etn35cnTTPh@3c8E$2L7;yDreAycKXxW
zNOS9h!By(Bq7PkLv@)!n8`;nAmEoM(o?#$L2`Tty1!cckBiP!JlnyUfj&_!>o>HC3
z`-AzSTJmhztCX=!dnOdnQ~AR-7RY0*))sNh%{%Xxu@q#<@oH4mICdwI;_09*@YL>y
zfkPF|U8VBVAjB7WBBVuS^rIWmB(oUf($D7C{F?cpQjmIBH-4;~*(^|#%n6wkJ~9j2
z(;#<Gkke0j-Tj~S133MDe=Ye<r`qwXiDRWGSinh{%HA+ZDcED@Dfqa%F&6cN+X;}M
zvkKlOUNeOpj}cnGZ(#lpY3~#(S`=;BZriravu)e9ZQHhOo^9K<ZQHhYom-V#sr&Nk
zRVD9de2lM^wUU+0ncjM{`>e-}gNmqc>~D#QTK%}-zonzW8Xm`%LZ&kWq*GV2$2*=>
z#`a_99z={9JB9ifV^PXyx%9te_K}~E^MJF~r*IXoOgJV7a#p~#CO2hDjPDF+Wt5WQ
ziPFJ88$Va=wOFVfiL{hbp-#UmVIoqsm?F<NnqCm8MR2d8e;Ri<ki_L<_P!N9+F+iZ
z0#E3LcR3eMBdt2-81>?t7l6PSpw8AG2=y{T48!4il-&m8k_lx*JJu(f+B&)poeTp7
zmF8-t@kulGC2y$!?3R~Uty)}csaCw}JP6H}{%ri-VSQ$|0JR$w6K{;5qRw!w4inTb
z3YZQmpDtauATVNBPAHkk_N42EZy^0l>d7LKdx&h<6@a-KME@b|C>a<{-#klvm4pW#
zTQfpJrZEi`R5Ol>Qn)rojVI|G+rDkZh#dA#GPADPI026_hU|%}nM5Q*ZinjO$pGF$
zPvEsx&pX+nwJlY_35dzAh;#6L$Sn0y^zQbiLeDAE|2_~HB(gd)%bu#1Op@B%5~_s7
zC79Wy2HdVh{i~_EOm$}l?}xl?WV9;d!s~?%Ol1Px086+qx^;}iR+>@DqG#p}5|U_9
z8TXlcRE}e(pjwMihge8tS87ETFQ|`wr8J*)3g8KY#iAG5N@cehO$MZQhBo0Nnb4~`
z@8@Ay8v0h8uDjl#OsYmw=wy`R9z5aFejRNn_A@fO;yPF+aCgct%=8ldz;vkNk(ZmU
z_(KcP-6<2$-;WK-!~6Jk__ImWg|2vp_N4bT3k)%KOXH#?SQ>##%z(8GJ@9flHF=Ol
z5&Aj9C<8}#E5`9x9X0NXXv=t1v@9TmZ{U1{QJA+sL@o76b1+?&dEKZP6i;i6jEyFt
z$II#Fk33m}7xE0AFH+Udos6OHvW}bFS++pWFx)LKpMAp7rai3bQI7n9A(ay8!z3s~
z;1r3MV?+b_CT*OrNc0;Pxj_DnS=>Lnnync~<+Xb%q4NC2y&A9VBdhX7yR8(K7R5#K
zG7)hloQ7K%6E!#p)pG2IPswOs?VY$@JZl4Kt2z@(CzBP)zvE%&TYq7Xd&%Uw6p&jM
zI8pPxkluPz7Fj)rNN?P_(th2l9kYbHhId;26Cl+LV0d^2pKUpKYX&m;p?}znJriYb
zCNi?PwAJTA)7u2b4IJKz##9-Hi3y9tQT@lPZ$dZbwDvJPDVLU!#6<!Zds29>*5W6W
zkpJ~Vd_1qKUnFNtA}LW}&Uf+6pYR2FcKgIOTPy~Hu11hh?9|z!iAsueLDqO37r#3b
zG6i@t8|W}1mP^=c_1i%WU8Sp0YY9}!sN6x<VMP!GA<tQ!`*cCgY~oqy<}NCpT45jV
zz%3TCSDT<8wHrN#6DZ{r@h1H(kO4ub)GN)NNGo@TQzs+>NW4DIEv}^`z7xA_{-MQ`
z`J1@<rzMyXwMp4FHAbW!cHcQ&N`sEXY4eF{(~bUI!Xld1q{wkg_cx5yqTyx=H<b|E
zX?rHEVhq*b_7SvwSTS$xo4-*mo0Y#GA2CaG%|-jl_??ynEZ0J;u1PimZR3wLVDsxp
zB?Z2v-2j#NOUc)zwHeNVa~P6o_AkwLn;NHU#UXNDyeCaM!D4a<pYr7}VEB2UtxcD*
z%;20;P-ZC`^_0Z~y}C42>}ksf<p!QN4VQM+`onmeQA*2xw;t*#KP~w|c4gwe2*f6`
z(}NHKO-_{6siR4(U7T06qTyC^&-uH3A*1#QfC!|#sf0WVn$6az1*aDa)KG)4@;G!3
zl^QDY>==bf%_2*`4jH{v#>=`Ihjir<GUl0CG83vN+AFp`E<wnT1HUzi7Vy{39woKa
z3B}igynf-Q7Z&n>m$?>a-1qF)|3dQEyZ(w}{!7{Bqewu5?!J6FFnEg3-uv84T-h~B
z<_hVo;xX1ecCZ{X%vIUCMPg3twtSaD#*&flE$KILf2j}cNI3*Cd3`0Cc5@3WqUa^k
zPhx^8IfM3rZb{uG*33RR&>EGjjL{yo<1Q>WUMqZiVg#p^Y|;FOgk5l%`QDSO%yjcx
zd?MA@r;bSmVT$`SY~)QRxz#U@Mi1l5FWkR{wu8Lv21J0fmQ$RIZSv6dENKskgiw5H
zG`c4pc<c=5_|fPXbDm{QN(G!`YlY$?_gvT(-QH}lN2s`s*{mS*_8R)CvLe2V%UNeY
z99Oc{OW-A=S-6;0<WR6Z`-AtjNY*3Q1YmF+7(X0~z1SnwyyTE)$uX2JT^zpd|C}e&
zH6*uewQPFf!waa(7FK~By1sc&a$nYd{@L+rGEmN|XoQ}KrY4zZMdU&J2I+u9xrFFo
z{f4oo$dTL-wl0M##WDUiz%jpXW0oT0)Ngt^bAP2MrueK*OnVuWSta+$Ob)g*?rCks
z<oF?%_Bvj2b7!1w5_r>_(J^4ty1TJ1tNoU}Z0z!!Wk-o6axfHk%yBk(FC>nzWQ1!@
z5%)Z-a7<pJJ4)C+ZOQ9a{wmQ!<D0BQfGmIn{a}b|2x~VskxX$~hoIbw!bR0qemmt3
z9$GOtrvD3T>*fa$dp;hg&x0Q1RV(tC3gvx<9&y+%h=bKGBVsA^QG~blbT6>rPG=b+
zaDCb>W@a&YAHa|$EK$+fKjvZ*C}24dxa?|?6?@&M(C=x4%`-@3WC`(Uy8;P%e=kR*
z7DE8I6hj|n^)#bQ%R8f_*Qb{zWvEP@OxqIb#7<ta9{#ZEo>|xWssRlv*ArV)U3O~H
z`$$eHf*A*5UW1F!|54&ctCQZGl!)K87OqOq3UFr+rmA)I?vv3%sW>SoA!K~|ZV*Z_
z$)Wmy*H1>p=wD19#>dQm^Jq#)V_G@u$nI0xENzwXVEW>Q0_c%Y%Z&uw@$$io&$!!@
z$Qk;Y+m%7oi-Q6?W-Qlz5`++J_>tSnVzy#ZwByy~ZTBE^?0t79uey31kS;xpN?0!u
zcxc-j3D+nA?21|3^Ng4G<YNxRof`C=l^1fgD^i@Ift^J^fBmL=rDl0nn$`tHbm}&m
z?K%W6UedfP0@X<KT~R03G7|sV5tMB)dq*f%-?K&Jp6^0gGc_{N8FFl9U~YXrr@h-D
z!Tx1*CiY?$<Z++44g>$88EPjd!lV{oa_lkjrw4h=^uCRvwwY$}uFK|$0JA2sJ>Z_*
zJF(z^<|L7Q67hB2mSLW889!e<|AKqNmYz>4J))Ol<w8K>{fJ2{`xryQ^j%!w=3$v(
zm>|lfs*xW=58;06X{2mkln%s6q2!cw{QFcb0N0{!Hc}&uO%+y5g_GCBb(HZu267IT
z3&|?#V|)KGG8wMDi7(0wE|lr44whn>*C&g2<_~s?RS0pc%RQyBf(x;rY{6XH{^;!G
zOc78jzk_|HVd&`L)+bx428V^Ib!A9qI+=5^9Bza|u!lR8uo-+UB)u-`ww&YcpME3i
zxi0L7Zm=A}e6?;H&y>_R_ME8D4O^?RuLDfm#+9=PSw2pdobzfRKsMKhKfM=4KraQC
z%k5Ob3QxxzdqkDV6IuO+Ccn^h){Bd8uPYS2Fo&NGb|}p}tuIa+q`l0I(p<1H5VDi0
ztK&(#A@U)InwKM*(`{&o=C=jE9jKZ>E;?h}BwI=z?MQeQB5{tcrp(0H?oh=O2oWX>
zMZFpyl<y899w1L-DuH#J=a`DlgFf<lS0UVM{K;2|ogHdO5-9=TH22hPU<({biKL<i
zsA*6dMwR(4k1ZUYEvOPlo?DOxfXTN-e5qfjixA30_s^ad>#neF(sH@QQy8~a3|x~>
z1d0b0AbjvQNi(dsdtz{1FmgUT<KBM`jT~m0g=+FCWMHbK^)E>gW148gU{RCL7E=0L
zF=#ehQ+!UYx4Al>1gjpwf7oD)fWkxyQx()*_Ndo?g!<P6j7Q$(Dn^gO#`0dXhKp1L
zdEw^qJ2I`3DJF>-v12k}<NM$$dkk@;XFX{Ak|QE_-B+^E;<xR%zw(`VaX)M~Q#9P-
zg6_3UYi8jOhsbBa8tR(pP*PYXyMWKH4pYQ`+0Pxl>9lQ=M5*dH5(ijXRG&*%RW8?8
zd_>D<8?|JPcO?2hJw9RL@SR5E7wWkDXL|E-yDdY~x<wr@?pEYnE!3qQ_ZvHKO8%B^
z|7b2;iv)4L39Yv6%Wm%BvPuOXu*X@(PLg5ra<?vfyN>eq=%D#TlOE4w;z(I}c9)wZ
z7eD`)62=MM-w1~#*U<o*9c!&{Phtc&VDh>p0)S0<Q+qVHtL~SbDOX{rddk2@Dwm%(
zrsYSLfSv$q7z1|drb!%!PfO*1=e>PUe{<!_WU{sb(@tLKI*C39OR!DjoREJ%<qXPQ
zxW9VTA)!Lyvs9q(CaP`B6`b8?sk@wIAg~$Pyhj~kC~pLtP@(j?qtZopH{Y8pU)p%x
zt3PC~=QlPC4Ayyci=<YShh&D9dor}faceFPTw(9nNQH=_|BddKm32i-l9!%C8-d`$
zs+{&jpPh18^cn)oNJ;_@RXXW<V)F*5se3&*Hj(1TEg|Gbf}CW2x5|MB>jZ&~wjcqd
z#vfI|>3pH_yU4luSMBwO-dgTkoz>1_c|%OnlfjH+#%e6+nZ<=Ss?jm_e1NeZr-oZi
zIyX#--CSLSOiMS{T}Vf=h<;#3ER00&1eK{n1A93N41~d!6BN{do?zaoQcGw;6M0-a
z>J3XGkr9+d^pL!rKG(Bc1I>)kB?N<YBD=yoXNuevhNh(1ZVn^eEstR$C)o0;6_`d>
zplmj894sVxPv-^XO)XXDoq;7Qi(HDo<9^i6$<$CWuhS+Y&1K-Yx;FW5-e1;Qyn9la
z`L?eq-`I3y?$?5UPFc%IuMRN?<VvF!TM>^eS)roVHCKI~yCu?~?h?;ciycr|Ijr_s
zHEc1GPQC3?CN8~fWXcmOTHj|UBOzo=`0rPNa&<68AA#G6upCUbn*sNZn0w-BxticW
zrav!a?46}Ol@tbNee-|JG5E@gY^`WW3E4NpFpoCJdU%GH^>~sr6&Xg6FxdZQ8f+x&
zWBdK-@MDvRT!9fQY9r&S&X87cLK4QY7vi4y*}r-4Nw`H#((W}K-t2e<tN|a-AU_Hg
z@^4Bb{kxIFi_CW+h(_GmYFUZBuxD)GccyhW{stbIY+X@PKAD08<9%B_d<=AMa+{l9
zIPqKb&blAHNFhMV1L5pq4>yp9ZNu;Sd7_FhXb4l!fTuQ-cd>hbc<zgfo9Zoi8qJ}6
zyh`;^ylJ2Z?h7UW%CALS52L%?w8-k&@-a1&8R^=!QBkwW_pi~jt%F_6U+aFq+^$-{
zWC$Am<`RdTr(&*6Kt#C2e7RNDii#Y)<8D!~BhrEUk0;job)s)S*WzoFtIAhrbr3;p
z<4pf-UJg;W-^<RYPm+ryV_mmQ`aeo|Ew@LRXlgO~mCyBBI46TAFHRp>K@2*o6cQJD
z!gDP8&$!Oe{CZB%pP`%N4ke!yAM;9iU_6`?<enomtNzjNvr+y>+@&voU)d&INB{+u
zemz-VUV%Kliau({*Qs~vn8U*rM2nfkQTISxTxjFSBYnOwXDnN)B7p6K2dwGKo6}O#
zUeTa)8qM<OhuG1~kmrZN!)%*1kcsD|VCmc_4|*sW8c$SQK9q#UX_@s_#>Lyf;qMv7
zWI2(W*Er)x!BABvZ~vJLL$(sAJz2?Ehu#*7fCUZV?Zu@E3-n^3piuk<gIye^EIp7i
z1F$N^<iR3W!<TNFA5~#NbM|t;%I+HKTj3RL3Z3>v1sinQzxu&w!KOb|-sejyfrbVk
zaau4*E|XN*lw4282FqrIXzW|cOH*`_5BtyqYq`$8%_P-o+Oh8}x#K&<#st8Z(_39q
ze?lbNCIeD&8cVVbfqne>z}o&$!0ys-@Q43XrGb+tp-GR$pa;30L$j%ku52V#n(qOy
z7OqpbGAYWqL}(P<x;u7x)!lr!(+|J%0dKhq;cq50R7n3xP@7D8dV=YS6N=%a4~(8R
zibsqw=Shk;i0eT|Y%;$pv&&M8t?aQX%Td3qDJc3V>X*cwR?Z4Pcv4^Sy&wurRE7ZP
zhr-`3b|9V{jGu~0ABSt@65$JtPCR+bVSeL{qOY^7E?!nXB`iMO>=+y=gc+BA6oDaN
z^i6mHNR<WyJQ4@hX2+1Wgl%P*=RbW+&r&5GA)0Zh&*sA<6`jMsqkq=k^l8D~OR%Fp
zZ>A93PzUXagz<tUTaZCUYr=%lf$Us=z%8N&Qg<d&%Hvr@|KsX%cu9R57Yv`y!AY<F
zj1RgA&?cs@l`dygk8e|Ra^A%a@F;2K$mjc^J-?hSJzA;uFExLMPfTxIWJcF~M2}?9
z)U0@9(lxGh{2)4t#<IRXuu;vb`}bdO1zQdgQ<7X#TU#YIyS5BCujOb%i%0#`(qvlx
zB%Qv2JYVC7gDpXSL$YZ0oZUb{L`m6gpTX;~GHJpEdx4G5k4nOvtKXIxRULom+FWH5
z?j3Y8xvh}LH`i7_L7=5I9%p5Ds$x|HfdT4vUciZ#v<^O6MUWPND0brxkcSajt;mHV
z@YmmEFUJWH%FVQk`O1O4Z}^VULmX+`fPkoX2Mq!`TO_z6u=5BrFbSfXO{fjTcdZa5
z!;js0t56<>jKQef><!^;2<@XC2vsUx{RpNV5bo$9pTGIbM1b1yd?GZy7oV=YNXtGr
zoBkzE-GVJ2kM}f|hfb#j6hP_={r~)5;(u~p<Yx;vdMijL=1V%vV(A-Ien7d5tI|D@
z&Vpvv+8FyZvMoF*vv0=YFEn)5nvjtE%;N5#9__!n-W766GJF)#QaET5Gk<YnY}&mW
zK@9_IiTknNhlr|%>5@%IInZ7SI#I;~ez8Wbw%q8~yeEbyxH>$3&#lRC27to>MzyUP
z<h3}mgB#E=lL2UsCZh1Cw@Qnm835&>jVJP%{GPF!R_+V59!{_<)_TUxdnqMsotX#M
zbrEb1x=uT_oI|B}^v2b3>NJ#*I*?r1?>UUz!c3&9`=h(;BU#K4ojVla^|BLe8O6Pb
zL?dT`?IvAKRMgQV)znI4Z-yIrFB^6lCf<gKj7E^!pwxOHPBID|DK-#vIZ#t}6W4|H
zb*qw!U4dDg8sFLkuoYcPh!z6s+CeKDu}o8+63TlJR+F8WUZ8ihea2ZW9a#ATvH0>?
z(Pb1G*jc+3k+2V)WL)2N+zHy<jYI>jPq8pnE$QC0Xoej}01Tf9dz$|69i)Hni8btv
z!Ah+7bf&UDg1)qVJzVo=RPi=;OxraVOa=gJMieqQhQHo`%zn=;Z16P`5UPPD$hs7P
z0eR_JI$rYeEk1mNQxnx|v0&@#d_0_1Mj(w?`Q*In%;xb)A2G<bh^hPR7apQ?uv&KO
zHq1CFv5x`~+QL#iQiCQz+exfja@P6w5JH?r)DIc2>Unb5S9QD2=Q`)m{xL;+%LR?2
zC)4QKIix7LWZaE7=f4g|qzgoKac5I4L?fgJd0lTP!vNLi`^nh^KibM>nU}ArYxZb5
z9)am+>(s1)SJGx15skRRBKIj~6bGt|3bLWGQUWe9Ca9E$Zj2gjw%L6WnHI0-eJD*6
zN^oGy2|f~fhtBnaH4<ZH+wW_VNsmlEPff4RH87RC5+sy)YLLM!YT4SMor$jA+hCk<
zXAayUn09uS=sB{tH9Y#`UD9@^t3DcJL^J0Vsx|>zh%|+*Nc}>EkBU(|>3|Nw9h+dH
zxZX<^OijDVn@x5xG5r^>J1q>sE4(CaUDc-4?qLxwZW{R*RJrsc-VULv<a5{)WX3+y
zgQOpg%U$N)KM&RYHR|jY&9BCBDLBYm!s&*Bg?+4%oFso03s^#LKth6Z(b5L>9DHvT
zCV|Q*S|V>O3F;z(H1qR*aa0fUvw$-7CNtb1$*7>X84bm~_F_xQy@vT>@&Gk=SyUU;
ztq~*y`hNcO$2@u3&vl=7LdIBk`jfEaXYF3II00>MdeBRyGPz#&(u`d3&%PfK4*3BV
z<it&$VPlN{XK)<w8rKn5Cxg}~&y$%}N&zh-tgb4=gt(LjcDeMocCAeg(Uv%h6ZrvK
z)N%8m9<|1k1A6PfyvIxx>*_N`>n}^TZmt}70fE~)ti?$Z-Cs^FZw)`lW;9uF*5=`z
zqQi0D=^C*(S#du9H}R)mfQ>*pi2pafg5`f94*pMkg|wKAs)Xo&=PNk=H+%)tfASTK
zZ2$O*KmY&v3a0-pUm@_%Wc)uI#{a`#{2%t>|8N(p{c{!@{->V!|EwzhXC40U{^9=*
z@9_VPa`^u#6#h?n@PE|?|5yIwKWLBt%73u_FJI$-@*m7h|K9fh<Ud%L*#9RE@?ZE5
z)_-Q<|6Bf})kHaaa>ZX_j^Ex5E70rk25XD3jrrz=u76u6hzkVb22DG#Qz(9q)2Ajo
z`*Zp$ec8(8RjIlArN*rrr6`gLW`N99lmL=T%M6Twm<gU=Syn_u7<mjv-_+E^*wj=Y
zKVQDtz6I=iT_9f`oP$$iZT;a(NOTF&_zXrDnaLR}YGQpIh__uG*uM&Z031F(DJ%oC
zZ(wTbTTg&X49`EkpfUuKKNj(Bd<N|(n4i$n>~`PW!1M;@<!QGUz|sf{AU-QA=|}GZ
zzyLe`Z%BOsE&t5iI*u{$i@CKqtb9FF3jl}LR~)1zualXX7akcI8ylOb5H`6emlzWu
zmZyJK$iIv}<`IaaGiV0Dk0dsMojLFa7l$Yalz&Bg$YO7+@|ASXHVm!+z+GT-2*cP4
zWYv+O6_6u{|2MF5kqIEw3&Exz^yH70Ki*y51Au>YU+?s{*eAUZTFb8@D>G|LV?A5b
zb6p(^aJr^?5C9ZXL-fqe%oBi+R#GnrY<136-$!UoDB4P>sD6MSs2xN+f+`S3pwpLJ
zsO(5A+RKb=v@F_+pUL=B8m4{0u#JrHwY4>W%Zx09-!Qr9J_I9}+D_2<Epbajvs2^q
z$6yIs3t5R@vEhN?Y>}ze{vJ>w+GiY!JLLXsCgvDs-^|q1)bJD_00#j7&B%$gABVzo
z9r(8u^4lsSJ3KzU7nhemB|B<-TnHt^SJ2+E@fj2lJ6DIlch^t*eQz)T2#_>QbPk{>
z0TZ-VFyBi5B*CH|QyVl6?H1t1)E*fKQ2Ou3*S8${mKm9A%L<;~(67s!^uYntg=D0&
zANludOL%w=S6@^BGK9Xqff*P;MteJ8w)PZ2-f#1h8|+WkZ?7sUEbR~SE+38LX66Q9
z+iwn7oA0m8@H5}Hz<1t#0pM3HxCfWLUJ$^N-w1zLM$S6M7wFe756rLP`)_7IZ`Ic?
zM$j*IQgBrBb1zBRFZIK3;-9Aax|g>&n6^fp?0zL8Gw@v0&0p#Yz>in^s5OnOuU#Gs
zGoiaGh|NrmUmZzka!6>m_=v)g#`^bOt@B=TKvr8D18Bu2r-rx72f#l785zH`c<vw3
z)qUvE$ZuUg&7eT=tG^v6tt>4}?^aM6Ya9Tfv9Y0e5POH<$XtEU5O`|H@t?h%IDJFr
zR;Mtkfc{C3fax0?L|-lyI9C8<`@N|@GB^Na34BA4`bghG+5lwrd_%CdBfSwxn0+Mo
zVg3MQ<9tIv`iUPxRRCn?d>}7$k70c~Ccg-4om==oUTMEV`nOPi$ZgXZDF3lr__tI0
z`M0C;3y1A1`UvZ*FZ(96r@8Qhz=;~c2O2GW|F<&#mf6ym7~}&<W&RM-mpJS_uU#}{
zd<PFa1pAKJ8#MlK`}>UuP-^%A-hWW?+t<rV`%~8V6H@bA_qy7X6LrWp6(BP86R>BP
z{u^WcZ+fj|1>wW{k@Q)i{qoxfr~VrUs{h8x%D-{J`Wud${eeGurLUr8-}-BWpPjv(
zr{6c>`r8OAJMuHjx|YuK#$jd!%lNHC$GYdIiu+p{X}wo=-LDq6oj1`K0BvA*eeeYT
zxrZ9on}B+?(?1O7P5gE10nm^7DaI49_wZHU|AQW&{j0#|TGxjBHt_ow0PZ-B<L-Iv
z$odB2V>BV)(b2W>GYqIrmhRsZMd<tn?ETsPnTOZE_G32!%zAwT52QYOF9p(<$|V@}
z>-@;O99|nn)D}GT;{oY&oArCwlk-Lx!!?esYtlEH!nW4H-}2R?mfjnwW9tWZ%5|+B
zANZqC^t$80?=9*_EkMd;rBR}7SdLFoV75+5e_)^2*z>k&)f!MVA9S4>Abc<vJw#!<
zfBmwpUAT%%xDF%<E!-m-3BfR{HVOzKQYgM;>n?!Y2#5-WVps9elNVu|oL-ESohP_)
z%++Zn)o5G~*}g2-nv$ax6RPsMR_jpi*lJ@K<XY%FpmfYMX8gv9kASpgDbE%r8m2b?
z5#dP$%-arjqZ1#-`8VPb4mq;Ef4Qs{jezge*M(=C|CKRu2!WFUvrPTLeDZ+SUQK^i
z+4fAK+Y^6wZ<&1l^HHLukFJ<|I~z5j=v86f%DcpjX{#gs`AS4}4zeOa?H~-|PJDR{
zd3IWQ#NNKye-#`%lyfRf?{TgnDw9G%tmZf;<P-0my$LCe<UUGWsk?y5R^bCCEQy&s
zSB9-!T<woTXv&{^L+@uw2dv|Y7|5cCJN@JuVdqI_13deF{Vd$(8c${kbW2dhg^A2z
z?&8iUtIA07_&z;PTG1W3vj`2G6}$E4#s{Eh4fh-^1=Y|Eg`4Et=Vtky7c{Rp)JTb=
z8GV2p2>q4?)$N!)BKB-QcDF)s%<GaI+C6ccRA2ZHRs*!nOwtkJH&LKm?%Mj82z%Ee
z<D)Dv89i$ceT1k(5Xm0t(=>}>nh2q(Hy5NLr}SbcR^hg7uDCh`*~lL*iR1hvm{B4K
zde&X5McxaHO~%CZpQt&dFWuBx2{ugPV!1?^hd40P9WuHe40uGD7Du!$VX%Wa&!R-0
zYRsQhk$E>3CHX)s>4dKXdKC*dQ2_;7J{(<3P<n(_ztCN^jh?=0I5^1oEftd}<PAAc
zHOQ+@@e>F99v8_m)B};{K1(NfBZpy@r6VY;?a7N)Xr~sx0UD5IX<7Le^ve;xQ~_QU
z%u8xe?~ww29BS1j@4DW-T)*3}q0A@*U2GLHmiZgXx0G34+C%KkC4$}1o!1@tPMNB{
z@hZy`cvR5N=k9O`Zj1CA%qcVl<8X(+<@@lj_CM7eX{RZt<@T@VDglZpJ?BW!-I`ji
z0dfnF%qZFIy+p1O%-@<ji>j<LZu2$h`R}}#NN`zWQ&pgJ{O8iATsDt#Dy8@_^TM}I
z>gHb<Qw7*Un?-Q;?ypwFBKwlgAp$vX#{LofQcAPZH2!utwdwo!%h%6=bR!>B1-|S_
zyBXsU?Ixm9@@*Ri1XS2B&0l4jJ!>l1gnB}Rq~J-*u>_cS_Esq~Ds_xq(2FFne<k^5
z5(ho^m`q2ON&V#)rmezLjTpA&8?p%0wQ0#`rri@S8azO%CRxu*v_k#i9ls(5QI;3P
zYKOSI*EX$LVXGZEDwyJ8S{*~S$3|_pu>5R&mr&=KozncpV8mM+N8$(PTC=PgaCO48
zWHEV;$wE7>Z4RDEG7jb)vBkm&hS)4(*hth`{2XXD^YReuj2ipzBx|?4Z;;t=9GhB4
zIoF~v4?-#hvO58B_dMv-Y~>u^${WAub_kQWdX&nSu3v?hGqQBh!}*ePh#Hqt+@!f@
z44uj&4V)B{nT~Uvvdh`aU%(5IHij>Hr4+~xxAv0guA}Eey~-w@YJ&t~&QA|sfp<ot
zK;EgqqAZ%aBuMYQQ4a_;`l3`}$QK({$Cnkw?+YMc1Jhu(Jl{VcV5$e)gqg!v#zLgs
zRkM{5#&s(5^g*DA%}p~~?Av7=v~(sCuA{L;Yh2Nr16INhSjDjL0Yj;YRMggTsRN=S
zScYytB^-UnkNROzdEexZK{NKvV(NC1<P1M^=`F6cs$%CxFhY?9z%yAUe3Cz1>p+kS
zUe9dzeJ9`Vgn$MaDT1%nG==r8gPo9`J4h@R?ug`)_1;FUmmUBibXDU9EApMv5B$T>
z3Q~Pm^7pidv)#azpzJV*d{7UQfq2kdBIQ8??ul41aJ^B>N3Qu{^giI>!8&YXO*Su*
zp)E6D?-Qs^9>Vkd*HgxyTk6v+@&2V9$>Y|hK=BkjV^Vy=q*I<escj&8-(ZpFyt<6>
zXknO;#4@0EgfiV_x(6?+PkhK9k=+*C2-|mgoN-RivVd1>mEdY8cDjhr64bSb4``LB
zoU8h}cdh#enM&ez%VV1(o#A)<lFoEfle91*X%w1j)Q^Wp(!++fim*2>L1bFFYX@G)
zbu=K4rhU;rou88`aOf<dCj;oiHtIQ!UK0&3AtijhM_k=n$WRhru(PTofo6BQ0B{Dz
zj8i(NoekDrF|-m#>1TtFN%0Dd`t!|PzG~Uwl<<}4)DiXUYkieUO()ywHIpc<@}qap
z@*D2*!W$*WF$-hE(2UZH*A{zoV+8ECW}tW27}b3f=O!1zF|Xkbv1zH~P@FrtHfn8u
znkxsd5H%KL{y0dVVl2+qMHaGtGl23LbVZ^+2uMp4@Oz)}5`Vtw<ijSa>I<y_87I6+
z@a%h&J+HT}6Y)MduQs{S%CTarJ2I2rJnHtltA*~m1zM18E7GcA<RkETvb&kiXZ%3G
zYYKKS|5oLQruPa6NQrD{ttV!u{&Zw~8)R^TdXV4X$^t2TdF7P92Y9w7zQ}f4NkK35
zP3{iFUz)MbQIU+mK7^uC)`#Tz!sv=xj1th$)!l;JMH}<xdHcffDvfI|_{&nB-E-zu
zab*b{`_H$ier)ez@zCd`X#UemHOsvdT}|vC8r}GY+4Wl4CA9!xgI*f@&}3B>RsdqG
zm6O$fk(`+3x)pmM$s9d*4sH{3^foRS&-O)I6Te8bycJ_Dez+B&m^PTv<%zybuL2k9
zrc<AtS0vQUC0?G7rbu{zfw)gxR4Hq<w`D*9sz8q}lN!EkJ9&NW#RguM&Va|vV>2ds
zvY5TRJM9n1;1^1TVB~D}ARhe{y`{SswJI4iBJCx~awP@vno^#x{ez}968bgRT~^>L
z*3oBUq(NnX14VXn`lbiH!Z+U<J*{s)cfe*^>)t9V_dL%_g^{l!9s?QOag>;2Q^NCV
zXjV5gr0@NEzkLVsR}uD=Cj|u!7VBKbrMa<5e}_foU^`=-{x1lKz9|z?r`s{d#ghsk
z<xmgvpK`zTJJhFh(3j!Lzj;J{P7A%ZniAU5%L%5gP>6ZS!9>9=dF7GMHs_5yZQ?Cf
zKj#YRWr>p3Ze&aku_w3o8O>1Xc_1fbNY};GT|$H1hiOKe=!h@LBYCEY6BFGFj$$7|
z2O1@%jG~7}A{b(fRyU$bTqu6jbwGnj31ls2M2wbWQqk_*!SL)8NEPOdjkC$n?nubg
z3`eTCxNYxM<B1)p?q2vqRz_@R=3$q2weenTEw=;=J1hy)8}B{f<&sGz+BO1-?%UyE
zg&y(kObOr*tB<3(CD7wJy}9jSeV(GI)6b5Lg`5m!rM*dW?EawbrLOQYGDu5fL~+zB
zQ1!BULp}9<kgK`Ycq8khGZJZ@MC9BVnYrgy!E+==709rT`W{lQhFQ^>J*&CJIZ9}f
zkJOF{e?>M^di;M+9&=&aTPrV*FHI~O;B7MfigiPgWF(H0wrH?FI~+ueKR~8l5RJ!E
zd`1eqA9JunZ5k3{-SE6-hm9ZRmG0=Fy!9@nvBL#jLRW@{`*h093lULeecf>~<2c%<
z_*FJj8p7-qG~j9uj%KzvK}^)?6HBZuI;3Bdrd*qye*wiP+dJ49Mens8Cz!aQwTE2H
zr^J-?jcyo%2e3h{^A<BqZM`e>^3)b#m_LocMjlVor<wrHwIVnU*_Vfp;dH3!3`^W%
z7&1@GhndobMLfbpEU^U?t|b(|c$y8;Dys?eFx{wn<6Ede2)z5_44dtOnPmAG*DC%V
zDB4PTC+d?1%0<H|I!wdNnQBQQWx``HF38#WA9m0{+SUlMCw@P<<6sxWE%6t1XoVwo
zmAraU7j0zSDG1;NhVvBnoNk~L3d}-brdXoWPdohO-1LoEeH$!$3QB>)@8*?Svr$H^
z#q6A!QM=v8kL#qTrD5ZHW#@fJH7E14U1UabdY(IvOJw@(3EM&z#06GRZozYPBEi(X
zvQ=M7)i^<$ZGR@&EaT*ZI~6=1t}ibvP4{KFYPXyj0xsiTtHZ_DpqPl3KqMT`<l%d7
zes=6ze1D{4D7DR)6ox7iW<6}N8=X?L1-s(xb?7dZa9~S7M+_jdGvd9jUZ52ES8~B;
zs8TeGoN7-kUtIs`{X0iKnH)gWo7$FZX6Be`Pcn+oZea)Wxwi+)lSQum>a*8346lG{
z_-ua+^+$HfGR94(zfXO7mY4rKu_qOg!Zj;>GB8(9zU<Q?);WQqgoD}((ytb=9_{Yx
zrI0Q9nm3k$n1YJ@3T>;jfW{$es8jhl$fle}Sy|5>!Qu4=P~p<}>VCR#3_L3PBdR@(
zQWh6@r|wk&kyoaD)r8ib@5s|a4(Wp{gb!p++|o3OSg<;=V?W!4TnVXvwPy^;>h$gF
z^02W{qdppDIW9&MNH8CEnM@TR9_>~|D>tK$34XFP{)jcQqjDmjC`tvIo|aZ9hV}d{
z<%3i4;}hnx{%l(;qjC+Z>HW;Tc7OL{t^s586ni6N+wvCV$-vXD5RU(>uOuw`NExeV
zdq)Gw2E)Uf+=ep`u;gJXZgPfkxY5Q3F#GVUn?<xoO?R1}D~ncRtPEc!_^G@X{2`)5
zDw2^L!A;M3&~E7wELNu$ALWJ#bYuAvB&mcy;YN~2w?j7pF92FWauV1z&jGy+M{?nl
zZ5%QDbvcRRQKjW(u2CMO6YE_y%AF!zr&J8qnX<=$SE>^p;o9}W@ptM?`UF|q?b;>G
zR<|;pmFEFFmvARsvM20r<?>wY6>(+qZGhpbHBX5sEVD5+wV-MGBp@a+sKjVlbtGrx
z6pI6+a|+4PwUUh@7=V92^X?f*7i%K-_7S1bv(?aU3)hoQj^vn&uPeo@BvM~XpYheZ
z=|0}5<7-DwnP8RE$Oq}1;$d#V6~CaYe}H8{!&V?J6$?eW>g3Dhs%xcgQ%dTz9>9(}
z(5q&pnBY8j_%ygkv{Zjt75ah!_2+;Km=>cRxdB>N_~YC52>pkRvIQLk&dFF+pWY{#
z()>K9crL0HM~UJp&8r+aNYlAwGBSej7Lsdq<ilhtz3#MhU5Jl+v->kJAa_6s9XA^)
z(Cv$SfhACeBEZr+DmPFPLhn;Fnui1hJwk?>3Vn(fK8EW{QLRpW&cy!VQxL2fIN2YG
zQy%?=F;8}4hHs~tkC`ctQ=G#yqK4Jn_il4@Y`nyek?qg*orQsG#`g^e=F77qUBMVS
z9~dlkAro0?h$O(B56IxyWOn1CK!7b2@HiE3OTi7<QR#{PlcRq@BVL*!W=bf<#Jp_m
za~<<NLac0;I=ZCk!J4PPZ?TvFBTFcKmTR+7>bH1z%}TV!W6O8IHa%#Eb6(nexph>g
zJxY-%4SwypmE?jkANMHnJ5b96w%)aAmBQkE`dHmH>)S4+!?!OAWcr;$Y$cD0FH$@f
z!}*j!B+Aa=n+>;C-P;Q4gjKO;?_qM?+KTidPdqbqch6LOo7^v+$<XVTV1`;}xF)Ot
z3KwKHHYEa%IimZk%Ml2*N#vMKioWm1020~On9*@)l*m37ZbFUqsF^v|@k-38maX0N
zW=5aouYIGm2?QVzD@m5Mlmd?FUeqGvi?|bMEL`SG&o%6!zxdoxku2JjJz3!2*}nVp
z!pv*;iwQ?PX5A$qK(qI?is5fK5f81&<Y?5^9J6`4>>ts)9to4xB`@tvwGb)FoC2fZ
z#sHDxS(D{vxS~_NqANiS7LT|*jL=^28xcsCZKmpJ{MaR&0kC}K!al=}Xz*btd@HpS
zF78|1)8--jL^O6HN;jK7g^zIwQ(nh0A)Fv#j9&;Drln@kXhrcqBet}nn}sZiv6d=^
z-cL9mIl%&Shp-u{=BEjFjek*y!Q#4uT8B=rxPTe4RCfpUI5uL5cN0PVib`vE>r`wW
z+74i^G3@T35yN+Di>fy7#GH&NwNKDxeyF&=wY$=dhmZQEXI+Lf6snIb!4#BWd5}*~
zc6`@FLh1tZo)wIZa~?g`v2;G8@h}lt803hLVg6z6_+nbwcOclMOG;pXDQBp(%1Z$l
zlz~PADFm>z)dI;?%2kW)>m)uuNm}m#4bks3k8&VK<S=ciKD6Gh#yP5LoiOD?a_Wew
zS#v|@x@X@P$m-0KXTE8kd0|suvPj0rcC-%g0x2)tE9_I<H<~e<!o-(}7t2-82>1@(
z&!x^*9HrFObR{It5>U?~Q~3c3Aa#0E!A;Cfbq`xpQo7|^(-id8fr{&P_{D{O2PZ9Q
zZr~SGl%-8xLKrH#af{8x?7{bYpWGA9k779@e%ukdb3X0Z+1nr?)ql%(ANKz6+B+Y1
z(pbk&7J|UJ0fXp6N@Yi6k=7mranvbWrnVT`dG@f$V_lSxEs}7Q|Jnyly3$NL;o-Ah
z6S)zf&5O;kjgo@jbR;n94l-!=48iUcr-4e^Fk?vWfJ7tUJ<vW7bQ`G$)991CmngBe
z)}Y!Z)JA~7HpeM<GL7H0sl^)&gr0fdxOa(8UDd5#F2PpBWK(n05p|c7mrl9-TDoZ!
zr8v0b^^Pwt^L6?dWnKmr-#sIiPtcP+iK?P(;6B47&^3<jvO_5@Xp&~*Xr*F)H%=io
zm@V}WDd~kQoeoyX8QnuGETEUb`3zFvZ8&@H^8y2VQ$32I@4{<s+Jm*fzYe@8+HC=C
zMP4rm@)}EOgp(7TRvM>eJ71teOxE?#V{eFXYWO^6sR#eY^Z74MH8hRlT&+(GGouB(
z?)RJh?Fq{^dnI203;3=GCF+-e|56uis!P4vB-iI`m9!Hj1gUG{@bNf(lYJnrf;cw|
zd7Wiqhm+@XGEdCc5B*b1;%b7ex*NJOlEm3L4!W*mxKJ?&;A*F=NSfg`i1FRw8^dPd
z@(Pj6C7=E#oO^D0Z2%;o_R;kjAtl#)mgW^EX?AL}W*<MO7=awTdyo*sfJM~;MJiad
z=|FBXf6J)%@4gCc=yQT2o)_+>pmxO0ioIu?a}l%m!g^$zyVE@ng_9yKTEvkNftCi_
zmdR4YXA?Q3D6e@rH%hA}x=hd9)IpQ$eB667agFzHL**4fPt0<xi@dkmU%uMYD@sld
z^86W}@*wVZ#meoqZ6;?=rxf2oZ%#X_2lp^hdg*4f#^aG1X3UCxLdfZK*(D&S5Vzxx
zPj78iXs5&n$zely%gaP1ie9-0u`=>(4VPdmn@oo`pH)qaxT8QNV3%hlea*-$RwADz
z&96kQ6~rhhgW1=k<d8rYLWt4q0{GKzpTXUH!Ssc>8C-3ZeEca=$1yu$IzP|DTRtX&
z5EpNGf^l?xB`RnGkG}Tit;AI2n;XHkcauf&H$vG0F^zeH4}w&~x9pMzh7gr1j@3r+
zRKk&NKCe)aAYta-TYWd~H?@GkO}7=6YDvc>nIQh0KaDgu@h#Ew{n_39c>ze==t7FV
zjdkK7WnKJgQ*lZ6Cdzx|()76(v`HVFWI=r;tK21|wO>Y6Ez}mn3UnnRSZrRl4E54K
z8S?d@*KZhVs~1Ura%z;zH^sS#aua{6g2PClp#MVWOiI6y<8qnZ2XM1U<<r_TB$=Vq
z$h$IEEhW^y1V(v1w+J2Ytdn%HJOiZhioahE3sZtZ@eNuJ_|r0q?w36<9>G!8?!@D^
zA@VGM^OTfWN$Py8yVw$%%IDlt--fkKuj>jGct~4FCUn>YPFa}cn?QY1$TNjaYL@ey
zX~e~cgdE0_MAf7F%x8#?n+cl#iCDHEJ<gVZ2=e5S?)V;FMc+T18^p}&^S!d>*1T`C
zFrhi76;ACiog*%cA!p5n7M&*Gxd744nZFAY)4|!E!ZZtfCqg0Bn#DdXG=eKNJ}Y6r
zi1Cz#1lIH`YrzXtwsPdUolQ|G7sP;<zth{?QCFmewI>mG!)P@FV%Kgw7_iBp$fwDz
zzFNx1W+R+LYDr1HCi1`Bjrby;$p$*Z+q5ilJ3pvRUic$;rp~51v#9bKYfj@nXZu1q
z0u(lq!+*`7ML6k%>xI&Q1h$W278kzqY=9tgy@3$SaxU;A7O(^c|H)ltG-T_5*L6!F
zsws14Y-xO(iD{1ou|845T%hFh;Z-l>Sz_x?OAa?7@gS3J-@YBE>>rxTtB9XM#ORVE
zM(epubOF!{4#^^Tla!Ed0yX}88-yNdBdb*8!+}uDsi!$t$_kZ^9*;z;x-_Rh0#fuE
zgf?L5pP4nnVt!t&6wA1$ChIMw$~m?}*M3OJ56a+_AeK~{s5cJR+#XN&*8TV|A}H1R
z9VFc3tVs+X9IO6Vhg_@yReBi7q!QM|G)|p*fcAY8xE-g}GRib`)jHAL-FbOXuM;ZX
zvsaGMVUAaGyQ%cIEv&?{K}B~kY*(80EfRGLlv}H!!J|Ix(GX|nJ5@ez<zZ&e8j)nK
zvij5feLFN9TJC#mt<KZWdQE4wT!nzs=l3ro;(N-eIM{SNQwf-Zqa*U2M$PrM(Pq#K
zkGB%|rxIzk-5`arB~~>!2x%+Whz@oZIe3rdaBJpBT;7W6Y9^jsdFA0<4$I$o4oiw|
z7O@M488}=rLxAS&GHR_8TNa^igu68&qRHAJdql^j7P4nlm`*8(9wFC-J0}(|7`hwb
z1?`1&=lRu3aJpNkuC}0^TCbpvOg=9gue~|Qrq?)&_(LdYTxpNQ(_}X*XLe^Rl`2NL
z5=EF{^R$l^jJw>-FcmB7k4}d5tw9xCwTMs{Ob@`@=_k@lQ0QXTeTzALn$BK6cRZMv
zD05AF0wppx;T7X|l2}vu3uiDI4Dfq-J1Ed8IQ(2A-7MWvHDtJ(G4z_LAZ80>d)~q{
ztt+iw{sd_tG9|-7r!t^Dnm@ggKO=#l#l4xL1Yz@ajcW^f@ox=$vFwIrFi>f=G(#P(
z8%hd3V}9rO^Ql=^+5GFgLU2FR250dmC0AN`R$;Z-O|0Z6x#0%=$FslvfA6|@-jMB%
zUy7F4JV9DVCPI_WA$d2Ea;8RWp&9LSMZlnX{T<kG{vCLE(bo$4`8Ji8#=RuTNKXyK
zb;0#{Fo|zu*t#+9VX%F;Dz%>yDtP<VXnhJlQEay+|J|WTHbmxvmrAf>V#u#pakDaX
zIiU=eyzPtF90QQ*H$GIdO)3axf{X36I{26a_EP*1KYu4{Z;QjDu9^@v<1E#S>qq$r
zbGgY4mC)6uhXp&E7-A9f;=$#a8Ll5R(-`^BYZEZG1O^KSBBG>Gubx-EFo=D&?vXiv
z>hXHczEvmdPJ+sgC)DW3$xTaV>as}j%{_@cj@3O_x4~)i2n;1q$j}zNqBBUO%Y7f>
zboG$utWoQpyqIM?yq=13sd#W3b31f_!ZbOYE%7F(D>>5DlLV8ZLL#fN-ja}?w{m&X
z$Gl}#NJ1i~4uI4OFVwwgx>b2xdNL{4;?bH3a^SzWuNP*eKUS5|W4949_?e5<+;@KK
zb0z)8SB3^==R0XA_901UUK6H`iS|^%cVZ)mbu6{?)OM43*v+JCI??5G#!&UChQRLf
znUZ=j$l*P|AbU8KsCzB-#L8=C(*V0}WT7(6x(NSrfqw7rRCtl9*p1}#e4m*Vop7ls
zY%KzT5++KA0Y+UWMWy{^`6{}SEr9aUWk;mP&U)gw%C=hx$}C&OkvR`tzSi5fzE5MS
z$)g40{Fv^z)J0qLqRbBtI`k`_Kq~oi{y-`4n{_*!!|oB{z;1%;y%M^zsyJ?~812!d
zW>Mp#N4&DMx&oEU9MLycV-J=h;r5*I(oqw!z!DgW^{hokx4ZBOodV>j4O>aFjgG@N
z^D1ku@Oa}5cGSSK2UgUdnV^A>1CCs}Smz&SJQCwvfPwBt3_rPul{i<hh@s7fA)_6c
zPsUp)2`9w^z%|q|$aiE83`NGz=+(KTG-QMuX-+~g`1gPE`)jOp(*@;B?l6{Rpz`8-
z`?^E9*J<$^XR<DBF)y6?{YV}*b%X7()bob6(UQO!dJ$BS+GsWOGjgwe^$<60jLDu{
zsf-qOwe=w`EpDcDA#av}-FKStWRhk(Isah%Eq=BG&!Fy(30>xNu9=iysFMM%h3p>N
zNgRw3ym&HUO1mdz&65AY^KN&9W#s%NN^%L<fEL<VpF0z;3fsD?fc6qtGU8SQUQY&k
zFC>P&UV>MmRn`Jw9$}vv)x@+@#Yy&2qm=xi!R4m@2;GXU0=AF#^;|scF@U!uQK7r0
z*AR3`f4Li5_8GKef&rZVJd{q0UdGSkNkmOFV?T6syipXUw-v+UqkB2Duqn0P;~bs$
z{*9DDQ{%eMTTH^y?5m;MX4;Qz`dr|_(lg+189P$Dl1q`;$(*`Ie4OmuN3Z*$UR7r~
zT&tyMA(a4Ib{^9P#yy~0R}L}2h0sMR4N*GbmAdSjcj;g)V@hcCqQDE22y>XP+S!nn
zRDS~E6nRKAOl%{cD>;wJD~}f!l40QIVOQEaT^IVmAT47}29SpB%4``SeN&l;?pQ3~
zspw)Oya;N*NpPvO>f{-HsfuSAXMaJ1=d>F3>79`t*cEnPc`f4nD4(^0pT2%iF7d8@
zJ2RyQ%hJ-lxcXSmhOh^jfES-urw|vD-oRwgFwB@;=}@ESka1xSW#41}dHQm_hpF4H
zKB&%yajZ=Yw^U3sR8Q_;p;_H)HJXBR0&5W;QJcNG^@_>2TsRy~s)?LMU$qX<JZS|Z
zjj>s){?^`0GwQyF;5Gia{E6n+zVC|Vs|NiN3OOE~z}1P59c$o?q??NwZ|;s4_dx4-
zUjMNmlj?V5oJ2)a9Q2I^{^)dX<cN7(WZAoUAJ1{^_zqtxI8yvLsFL8z!log0^%k{Y
z3~6;OP2w8Cj%;+a$^e@r@ce)htB-7uIfLRIH*rtXbrob;#fD=WKxwQSIbL&1U!^|l
zYcyH+IL)h6Md$Ry3WA(Ss4_PRv_zklFl}XVeu>2M;u`SB|8=o8VPd+_;b#kL%6+Ys
zOG{7Cat)S_;{(k=B`F-MHv}<Zu7dn`1pQ311i3pR2duk(I0Fu9Cl25;vOBiWg2wPR
z8UKUSgk%@FEp*g1!HhBnda+Pw3TL5QfBty2EfM&9n@jT2_jX_+>hDd0g47wy#%oXR
z;Mm~JRp_Qb#Q7Dmu@PW)RL8JFUZjSkZ+hyhNEqb}TbR0cJ+{lvw=AzEH;sH(sg|;G
z?r^v7nh{e%7zxhjIr%B+i5Nu+4WSMtY)i9Y7rT7_sTG77<jHq9A#d;@RivDD=v~5N
zN=0XUj@Iyr2R%v{U$c2r2^=~|d;7xZAY4jZ_j5|mZe+a?(yI?fA9vJk=jj+b*@#U3
zFIfHAmq$t7wnEE<^R%s)T#Xeul^-n7I?YmECM$zLm)LkuB2|^>#S!HD^UN1kBCT!W
zAOAR0szD2>{mX4|6sgmppre9+r6i)M`5X8v77U5V#Qq{{g?jTKmmuqhslsF`Afe5n
zU&U@wv&bM(Y9GD!kXKS|DXpSn1jcbu)t29}Jm}FF$@=!^7AUuaNPaoDy<2~+ajNuK
zSU~~~qF64owpC<j`3D=VBCP2A!PdpSsm{8Smb2xP&B|D(Wg^?X&!E+6iGhU{idyZi
zA+5-9VL0{~C}=p;98(h0;w1|7j@>O+#01Z(ppm0_NuyVfk3R&yJ!b*gmosG9SMSYK
zZ03frNU9|U-0Ox9rFZx0X)GCR+ePnig(!kY)>tfa_T(DLTVk(v&*s%^o*k}hBBa;u
zO{Z!H5Sx0k!+1#=Yq_-Qo2H|yNRrOV5Cdr6F1+X*gNW+qGRbwxjHI_Ro_TG^!)ZRb
zesA$QHx6|yf9Xm!j8Xm@Y4;Q)S`a39zHHmJZQHhO+qP}nuDWI0uDWI0cJ1w+o!#z@
ziQS3V$McZqC0{bm$?x}<lZ7)r87pmaSpttN-_hMkHHP~saY*`nOj6N(=w}pG=C<Yx
zoRN~)Ayiqc^Wv;8cOU8@-wLqGGzLS?7@(<I5i-`O|C-<oAu1>qv?hiUWZUIv-YhL)
zR13WqQ{}7D%1DQB)H$qAU$)ZS0&4$$wbc)t;6LHm%z;|VI-Pt%L}ft<?|mYakOIq>
zOQ+-GvgGwmVjJUYYhjph=GIH#B+pN;BSHTJ9WX3_C;qkd(m;}no|$PnsSysxzQo-Y
zm;2Ed!a`?ny$4tQ791Rqq<f^!Oi<xZI?+)g;sQ{J65BeVTf60Bx%2wNoK{?llaRYU
z+_TPll#vjs=3q!^OLeh5ka?`ujojb+^&@;!G64ADkvjEe!X5`R@oC|;<{)E_#$UkD
zy+u<R-EFfaDF3holSsDo$^NK;{hnw=RaATBz_J7+JW~dZOm;-JIT@rH;)Iq~Ce#J@
z1nWf%+yeMX3>~4EF^A9)|EHiN$51%PYi35wP_t2=ZM1?i46!|8e=xoTBV_9mU+7UB
znAL|!r+(1bdo$%)<xsoXa;vNfm*i%&SAB~gRL=c%t>faT%PYgv4%C6i#JdmA0GHJx
zBD)EvdoSC|sx`VZJtJL2Ih3J(2o%Q=jUuH<;{ejs2z>>a)EZ?0Vc1d&jOBey&$1Ys
zqmGqqgF`c(LF;Y#qqhZdPW!?g2ewkfwUcU$xN3JIRb3Z#B9(^qvmag4adcjd1^rgk
zzGI*G>jmwdK(8@yqy1lOfHJpv3%E@@5QcplzV&mgv&g}|_NA<VL!}#y@09XSkc#G$
zZy*D>!sXPb?DXHi(tV2Neiz2Z>9n44Cfj}pmx&|FV<K7%rbvKpnX^5~>{?KjfYLs7
zK0A?l4JL)yZ)o#AA@k!h*l*nhz28bh1fL6xmFp=OnSuHPEwGSwI6dT|)1=ox`z4Rt
zr=%7=vU$?f6MTrBTK;oASb}<WFHqcZRqlE0(p1enw|u?BzV9)3Sn}^f0J=bAeZ}sM
zl3;kk@GCqUXI2qUUe@fWVTopcq?~)zey1R5xq}3bmcf?=V#5gs1qYSO0Li!|MLruj
zxzDKODJKRaOKE5A(m&}Q4&y0P?Yv-1Ss~X56fxvhXBjIGnBCR*(jh%wYY7$Y6nSRw
z3PSJH+jF2c<XUtu0h199Wdzy$LpL_GBYk02Mr&S0Kk6q57fH*w);|Zkhl}LI3|GZE
zF;l&N{H_ijs_+iNljt2O@Kp)JP5*w{qvXXT0*1PLb~X5^+~;ld`RjuNKJ1(Gx;AE=
zTi|OMxjeAeKdpsS$|?b8sZQ?Dn>R~y@qk#}Cq=;7ve)WNL!SU^c_q!g05VX1N-tqb
z!N{$DVRbK17it3DT~|$yUs2-rqS=>`oi9Y#M50zPt&a=Lvn|;N5v0s|)U&aRv`m+F
z!<9mS5rUAsMQhK81Kru`p=tkOY9We3H%fy(=`L|B|5&|Sc#r-ZGYV7ucnYk>{XC`H
z-W`JKJGvE}E1A(Os2r3ee}%|d<oo4eS3=)z5Ig@J059f#WAt&wqn#pfL5$dQ^05QC
z(6-}&WllOkO-vnKTX@a&0w&=Wql%1u>KJrdUpd<%p7y$s4B*`7na(%arG3A9Y;aW%
zdH`tRA8C=9zKTJdWwWjE93(z><;2GsoldR)USE0PRW_+L3Ze@cB)I`A+nmPaE{o7@
zR^YIZ$cz_bO#9%mbA-Zsm+7N_**~($PyK3GYE77Wbv&dZ#OVN<9j;`lU!<$H>gH|@
z*yA^wJsjFx)R)<WAw)&0<W~y#KvLZK8q`sr{;O8Wg)FqFMmty*uu%SrI_ckf)YVZ{
z{pqRzx*2crIQM5myL}xDyc{wNim{q-;rQ6n&BVgE@C3`a9&tDA<OMS(BjIdcDE<{)
zBy?RL)9%J%Q^<rFvUgbz>`Zv$qGO2}f=J%2;+od-J+Xj$>*3$@N5iS@c%Q?JcxTJi
z1d0pFJmL=7h&ee#*;=LCyErKR&*J$@-U!9~>3gC?&biNx!ske*ev#{nQ*&!YRFfzd
z{CDN#3Zp*#B=!k87-^QL(r3G%13pU1N~c)h82w0D*3*s3sJ*HKsb%laOI0q-IZ<%b
zU9#d~%*O!9#gHDBGJJ)0^5%1?u_@H%ca7DeUJvOLIRZ2XiUW*GO8eq1vUK)@3oHqZ
zBq}Sdoy^g(Z?fGD&NCKM2kQh3Y>uDFq)~$Hn46`Up1+{SsVx^P-tO6G6qPsu-9*So
ze;Q=|K&gkR1S-!&Hi71@?`wDM9hY4u&!nJgIxqQ~CV3v`eJ8$m=cAR^wj4=~rhB?m
z|Bm$HlIo=6(YCAATmZeV$+ToLdjU-LJZ>~xg+wKbe!Z@#ebPxnnqL7UkSKBLCH;(v
z>4$KT^1A`d0^ofqe>(uhHGKM<*#;8jTvJF_-j;Wm3l)c{O3XS|&WTyR=numepK*Mr
z3(#nK{P>b+*-^7`@UXH86cS%F;KdyW`%$Qey!xEVNEcX8*M4u&q-B=MoKz}Lp%*A9
z*E0k-=8?Tc1<zB;Bk89N0)!Y3g?`)#@$MbBQ{y6l;kklde1+|LT|TBbkUP`_VP7YM
zJ@>A!rYreQtx&`7-%wlWl=Qs3BqylA^^@Iil}j2yhli7rgu~brKh0^oGiU0(Q`z|(
zk0?OJ`RkM+Ufbs{Sl4;d&FG1aJi)nBvjUFTWYS%LS)Y}bAR}B?{TyPi7Zr!6LJxax
zWghCz#tpF!x04ai0dncuz6)uBay}kr^$LZ&^AuC>^g2lu=SqM_5BnXwJ_vsujzSXu
zgQ-Dox=_n_y+WNo;qIfwWh!{OkaD|q(dLeYai^Rk28E<M4O!!DmNKb%mU?q=VP!q9
zwKRt@76iEUkxlMScw1}S{-Gmx*etCeR}*z-IB;PpWp~<B!fIu_hNe@tCYSMbLfBMs
ztp*+sApD&(`UPhTY%W?NLavAXl0?O4e90KoV@3YV-T(Y+>)6FSS(;uG*yZxR36lxY
zX8xt_76D{-VMUkI<MSMkNlon^w(P-3#JL1|%&!1sruu~BmJX23C|YdI+6|RFw>QIc
z@kFLwp~~5>lY}c8qdOxVvqq%^7i4Obnzrmz<IUlk2-*JoZJSke+Hk65Rrb~$A@DPB
zeO0%bncX@}4i`8d>iW8-ttUBm%lxSn;bC6?0hvLp5J>$xGL2)EQCPCXwQ|b8T|lSA
zm==oXMnrK*En`iS!$;W`G4YGLsJZJNwvzXqB7Ihwmp)DGd%QoBhJIu)-}jk<kg;*2
zR7cHn!W_AB4L_Ns`AZEE^l-)nz07zktqL?7;B@wv?cL>VyXz`KI#}*++qGrGn4IA0
zPax?mMEgD5=823Pa$FmZ4|d_ZO(RY30XgN^;12jQ`>P@dU-Kj@uPBqDg=D)-gvXnw
z^D!Kv=V&&l3$>H*8)676C)W3>MJ`w)u=va0j?8>MMA6ZK&>k#)>cNZXrS%bX0}U=W
zyjEnU!R>6nAH{OcwYuUCZhh2fepj`eVISvbF@|rh8ZOj#HqWE0UOs1kZ^;SitRJHX
z$eaeCt}UWNfKg~gCIN5?BH%$ApD0hNde_Rrw;-e`OI8PK7A=XWXsttF!ua$Egj+6;
zE|=sJfKE~)-6>3qH4BcJmp}+<9Q{*m&ucGp;nCwO(l1c>!?;o8oMhzEeVCbDY&PSn
zaev%JTvO_e)D$rlw~bd(JU84VW3SZ6oF~t~R+795Z7Z0$XRk?o-SwmAX_QZdBf?4U
z|KV)Lgh87Jm2=Ej(Mo!p*heH7{#99aZz0Y!vo0vgQt8uCCEPtN;79Q5i5M_o77H07
zDPI2BeBW}{CW}@q?t`ab5x)JKnO1;;etG(E(6jA->cMCe7yC=9(ZI4)Q~bn~=F$n+
zPHyi$4C1cdjOmvLa{@T3o-!mamCeh>!~YfoYw60<M6yJDNn{di)*f?H_!x_dmMj#V
z-`}ZBaDlzXlTE4hp(#svHV9sxO+fv$5i9HcN$tcq2+%KCM-Y-Ve}`jDQ2Cn}a_isZ
z{BC^A){YJ(F}cAPS6=g$bVGiE+?gV&X5k|Grjth`EhKc;+-6QQ`p1%?G<7A#(xZL9
znEK1;hSuOnT|;!vdV2_N+e4ScJsgt@LbSc>_SYBWb3jvc^egs!Js)c!fowGTT(X@%
zmk%mEnoK>~TQ;L`8`&j}>s>?2zR^9w&93T$EwrnNP|n<+_>^bDK)NswE`#zx+2&!B
z4(o8oD#$j#Xm)W%b0$w-VAAVucFTy=r}X;yN@B|szI7AO27-z8U^rO#K4{+HNMFRR
ztCrSLZ0^Z_EX9wAHVsL(O<@0{1akNTZK$sNx_5GvNw0<@7`3WvOiw6-sIzl^4$?Nv
zc*JI5>;NsWA05dDN5o8SoQ@%zgVh`d6aRhrz%xsW!+;>Vn%_p`ro1F6HvFat{BJcv
z;VW#p@*S}MU-HuOWUb~XDl*M`(^+F?Vo=%ON2F?V4<Ec~*UWxm5pJ!hByDkZRk?0Y
zP_Rb~M@qKv*R15IARoWgd{#MnHezZy_W3+XJ#hbN`P?K1U&@Q<znray1m=`P7aF*)
zpTkW2+nb2z>T$D^zu1;@SPS?iC@-;XRXBj<DEr@RV%8yR1kVO_i^AC#l@;REf5P1|
zD3_b&t=ayOF@4Q~01-^+uBO%>$ci$0`<)@l)8vn`(>Zn?mSz*eQ2Bbk7qN9DodMG4
zG0Kq29{!~WpqHhp$*wF<q!@ghsHYf?PN{8s<*!9ufL>02v)O_YHwZVu7;PHzhcfqv
z6gynzXSz754lU;fYyOQE47ScJ;f0%owda`I&B$DHAKU?@_jtva)8a)2bCf`ruk@uQ
z2sv%GD|xo{h1W_!nDu{d8KhVsyT~yG-Aak#XJ;&W_`9swFqjWSAw@w64gTW~N4Y1J
zNsnS9Tf{gf_r$HtaM{{MdA7P!_5g_9p$W%k;EL`;v4xrP;PvT^Pu$eujKz)=50%--
zqD?-`d%Ze^$>Ju_%9KcL(w`4}bae`Giv`eQ2*cheClZRoJm^)vlc=a5lov}7Fra8{
z7S)D(CNAIlqa2aq3QF)4_NzaZQ<(e^9OlUxQQOGKz7jI5;PK4Lx<5R5T-hef*oqM;
zW9q#|tV0<cJbC((+qjS+wBy!<EIg>-L>AW}QYsqi?6_kxhxi_b*S<Q<%Qp$1@CiwS
z-O9jDuqZ0pgmeadzUMzeKsy{nmryV_%pA05?EQ{_q6Qz#`#ik+%T3H9ac!t>P(G<}
zygV_i4q6=WEZqaEUB>N+{ua)PmH^Uv#m|AapSGhC0Sk3(-Kc6T_d&$x0nMrPzFx~*
zBHuJ--wcki%C;H!3!&9k43L?`p@1?ZI*lWEB9!_;s%oEFrMJIHY6pq97v8zbY|QmN
zG5)%)|N4F=FUqj{27WWW*(vh2zn8WNce+138sGQ3d2&+dD}4dqhd$gvnuU^#zPmbZ
z1K!*w*xk_gZs46%h)QCYA7Pdzxywojjb;Q4B-qc2@mxzu&{=tcJ3Zgp&#fhrk|<0O
zH?BU?PX(Gx15V;M@(-$cH)^d_X^6c1QQ2?}r9$wOhg)<;Ou?^8`1=dv-hfj|?d%Vm
z=bBoy_l_v&@8qS`>E4t3<7^QR(P~MSOdMJgp<%Z%?tWUdaV%lNS&a$}#j<8-FpuDw
zpHWX0u!ix|un?w!&5zFdG14Wzg4@Noh|tv8k=^}p)V*oC)h|)8(9)>4n-6WvRI+e+
z65(CrDx4-uC=y|A3<gxn{TZ}8-MF!>UH>_RlHDu{I#StIXIjZw191z*8Vc3N+n{OP
z&M6Nj#3X_ANxn1~|3WL=q2{=B?G}T0W53v!P3s)g#>_(6<<CSIjwPS1U2xATIpF;-
z)I)$-Mm*(lqNK`H98B$;RP`+io$fB-EAL!aOBCVvU4bC&)R~pi#0?>fL^o&apk%9h
z%WZq898G(09YYPV$(_B477dZT8L`TeUA!M{=O;7KXnLyLn^hwvA=S#%jUQXbcI~xj
zE{bXxd;YB^BhHegmo3e|<G@)Kpri@4iEv?y)csK@95$Zdx%bK|#3lw4dJgTPMo&e)
zTTDZ3!tYy8$27{zPS@?wX%J>}4TkSL;1W)mx$<Pv*<~Dnzn0dTUl2#xfaF|F2QMW?
z?>B+`L?|SouGgwlu+E#{{Xv=qpJc^O4cpf!5H^!))B+5Y>pd~wvV?<yrTJ0#g(|Nu
z7Q`4HB%{QtSvA|TialPd%p-UAx#Dzqc|zV{f1VpeiHJd%*AAA{o^}IK>DRX4Hs&oY
zO}a#mcRjn?)JGHF+#v&THidQ&v$I#4NQ0Yql{pX^nH8>Gg>9y~y7~DvT8z0dljNoa
z?7tf`Z++YSoPs8N{V8-Ax^Bwg3zADX$+(c1Zyrie3IuXP^6Vz6b`_avFjCY>O)of4
z7V8@6do(2ZAc2s(E*JPpXOHh9th4A_LnP%KkcXlOWb0B%ZVc7o2%eN~i26ZNM+jn;
zYMEbY1XGU?z&cw#7o|o7`-#ndF@x`(MdD{Rt(T~gyx^eGSL1UR(u%MqWghk5<~aK1
zhd!SI$zILJgC^!j)#2*I!rmi+mr+cw0K1#C15VgFXfo`&P5+>Md}GL^NIvG1H*CUM
zcCUpm&q|$=n$_xMho*kFfb_PRWqHIP40a0e#THcJ1w~kMf3J${$%j>aTkT$ydOv4M
zvq4#YNno0J2=v;pp`f!4r`W#vRQm7aOfQ(-0%5U)gU+>%2@KNgLDgu2o-s#DNH@Xx
z42=pT2RcEel|#sdTj$JgAyIswiUCZxj~;3(_L#Ag*O`0;yc?Cm=tB>as{wj;J361I
zn7kT%EBz)m!)3Q8iq^p*;^aqz$9-Jy1E_Ba)^`Ao#Q$6ZmUHQuc%P`6L_wXRuAZKT
z!DB8~F6Lc*R?s_D%+b)fo~~7l+afwt8~Z|*m@6Z9i)mRe?+O)fTp|`)tWI#s^A<J0
zh)<xooIpa-I8*j1O?#cWN38vZGfzP^As}5FPg1be6ds?zsCrR}J8={nTb^l|X=V+i
zsuSgXJ5}-8Vr=tlu>ZYmP|a#m7ZFv*QVDn}^R2j?F!PX#gi=c}#u0Zlhl}z<A;-yw
zJD6|y6KVlwq^6<!@hpb<O$;g5Z5T@ag5@4iQ|!gRlZe;LL8_KWK8hY#epMYWbBI+8
z_|9@|2yM=X#>kd6vBC6$t?UR@(vI}%u<<fteIbRpQL6EFGN`hc+~Q<hD@3?HNO@=_
z##{xR4T8vH&2XZ{XKstM&58M>DmEM+sQleY@49lmdlQCURDRJ+)yqT7iZ9#5`0w8p
z$Vc*<AJw9}E*6%-y?KT>Qy66`7${y;>GF4g`!wyIjvk2!D-0Igpm@f%!dis~zuGk$
ziP@>Wv3rc6`j~TH91032MfLEeDW2$RokZq<?kA<nd|ox(O3!(9r`Ds)7>KMEz%Kf4
z{ScutB16x&#85#_uLHs;S>Ena>IyrmdMW)^i%s&2?!o}M3Vvqm=d$lxsR+KLDC!H<
zQGyifkl|#)8_Za%@jj=^6t$NlUY=|0%U(GC3FW3TucD{g2U&g$e-28@nPm!1echd2
zB1fPeRfZ71$<{lf*ajD&E&XR#L{tpM1XcU`>68jc)**3-xT4bCVhol~k<dSrjw`&4
z6#ZIl9D#7ivvazV9<{{HL?bYirOw<JFMdG$@`(ypC_EQZ?YK3{LR6g~C(evF-`mQ7
z_$GQ^lGI~2tETo3MC>KunY$ga2YSQ9z-_4g4g@Qrc~8xtr6L)nl-&%nxAs{%Z&zL_
zO!LcXytH(Z`l)@}&rB4{{q|-}P%FQ%J3)}4S9YFrcE{Y-sHdYsO8n);1u1#l#f=|o
zY%`{dfD=}jgDUB%rK)xJzJl0q9`Bm?W$`G=Ix(>hG6j*1C(~==4+fGZYur^MHKEL%
zmfSvVJeTsi<Ftnc-jCEexvTIZ@z{V?yBX<s<4V#UGS?bi0V`NGn)qzLF2PbaL7m`G
zF@Hn@(fAcC<}af;Lf35C><=Y+xg{G0<x%yp>&Klvb_8r^m{r^u^KDkV<y<Sc=|0n+
z-szc(2KIg5I15QNbac*OiIEELd)8zxM#M4hdBfnU_2ukF@{F`3a;)7NoMR0k0?uA>
zp-O_<p}br2>RQ<+)YYe94LD3jd`2)9=$%30XcQ|W?$w8O7b5>9{E+33o@N(2gT@nQ
z)o;6C3Ljs`kNc>mz?*y8GAj9TXnZW*=DFCe=I!jN?>&lm{sMg=nJcr*<{6EVbK!%x
znv+Iw^aC|388EICbA{)|3Jv~e@bH8?qdL`L;l$Q)P(VbZSB90|!w^jwAp}WULZtbT
z`I0mSJmk-)Jp=|k8QVBK<T8@$QjW6yOb|^GM@;K@B@*iC((fM9)}IA1vw<Hjf7|DF
zGW!`m%?sV2ZyLT8&E7a;KPMZ^eYoQyWFj0#S|er@9h)+{I&2?5gF<BUXo$!nK*c)n
z+NV;V(%nejmo_}g<T>G&hiR5gqxWs-NKZ!7AbY*6F%mnTqui)~Mf=vN`!y9xUBMs>
z{m~^CplO7jr4lq-ykyFZsY))=!L|#`czXR!E>Gq`UuTNaU;Hg&nCt%)$-(|#5QzUT
zl0#HfUQJQqKS>TD4Mv9lwaUTH_&-$+PELY<O2q$fm4lI)?Z2=btp7X;VGBbiWm6Xd
z3VK0$p?@%k2)!u1*#F6MNYnohphKSiKZp*c|3o@e>DB&09UAnS|A}=N)0@zn(wkbE
zTe$p(#_>Nu4okOxG{^so%3){uudMpNLvuLTxH{82(*Hk&4pTQ%J9=kJ4|<pXyU$AR
zO7BMRPVYhQN$*APW$I-A-@%Um;5q&~*un8%_UiuvI~dp*{|nf`z|P6^KOg@a*uliW
z$oanucD()zw%)D<Krh7xqy+RU1Rc%i3PVV^p8^QOZqqJGNJ>B>Ez(lPvMdr3Y;VtC
z`j`H`ey(zvH=<3qdhfpO%rVa^QdT!cWDCm<EGl@Tqq(US005K}l#xxrfdB;N)CdrO
zgW=|K4UeFG<pTxFAzvNA1a3fo83H*0Ww-a6B$3fENP|}J3e2vc_0K{YoFW>Wq5=Q`
z;s_A@_Jt{s0|t~tVh`gK^g|^CZv^Kuo(U*`%accxrb5#!^x*@-n9&1XeSLLL|3|X*
zx?c`~8QD7^C^8y#9WHHQdKj$$j5Rnwm*<xpBsd{8Iyx8tJG-lkyE-zLnR<Cb>4=oM
zANRlrQ~{JbICrc6BtTyl7)AD~|Mz7yN)TM(PY}m%tzxiNdwVuVARt2^?J$hZEzF~1
zD?2zhpustyRZ~5n6r90lU#$MGZ9mN0sSChW&DC${=h&w@LBzNIESZ^^0yj9aw|qno
zX+Mw=G#G_spPB2ugCSVPdiF09OQUPp;Fs){48cETgCV%jiXBiQx*9;nfc9sppg9wH
zcwi)R1=HxWTCz>w(BPED)E+Kq<8R=#ZVsd03OS@BQ1$@aPV)8cXKQ14=ce~>I2wYt
zbF_YJMi*Cu<+?DA?m$!0FUEifMxSY#0XhIX0>n|UXCN-10X#Exn?B_-FK)j-<zN3u
zL)|woZUI~Xv<9*MUIb`{EBLTK&5_s`dk05QFV8>9|E!LISvZF9j1B;qfiwi~nB4_K
zqlS@P7;HfD2yg*-h-?8jfV;Ih|9lt&VKcbaKhIybKWntjl~L5v6;jOKn~(llkdPJN
z{n?@U=>3BOupkY<o}T~ubzcCxzpzich+msO`bwtA<NU|ph>EK_Ku~+XUC`_I-<y$}
z{6hnGa~ItpU-n?46gFi80Aah-1@K2;Sp$BJzy5k5eL6qCGr#o|zqizP{?bWqt*zfv
z0sHvTzxcq&+!3{>{A3M<Zmxl}5kM$yWG}yKD$rlATGRnEgmrGe>eQD;K-wsd-qPz0
z%?-|9v>_bQ!aV&alL+X{)qUpYyIldK4WaA7sfDu%`tf)f&<Jn-E)B=b&4IjnIfm>n
z=}<76wZF1druJa0zpWuSTRZ^B<mAZmFzW#Q5!ib(aGPq*4P*Mma0bS$;hp_D01aTi
zfM^QjG5W8QL$C(OzoZ}89)LClyTjlH$zQTx0Bn`N5$VYNWS`mX05(p2k>LF=AM680
zR9|EoG9$Yn(Unj5_ke&llYNo6{bdi?90p6j*#>`C{E%zFV}BvqKy1kRAk*fa*asdm
zzs0v0axs_D+@5}=+S~u7TCe;*e>;)c9{CZ}P~Z5ELRJ8|b+nPY_-X#7YQ9GGLEeR-
z9~-`a4IyiP1T@I~v4*r7-+>LEH+}*e%znn9wawW+|1G6>f)82Qzw46Es0F^7F|dbj
zb9kpU{T2UsJ^6VFUAcM#AGC7&05)Xh{taxH`dva_4Z6gKupYRE4{6=|8Pgn?e0cl!
zR*#?1U*-q1I=Kcc-5)QOftP*#pG#5@Fz<jF2D9=4v!H=h)qxpx)B}?nn#&=f@o-A@
z4IG-Ge3%8D&eGACw8Isq#nYqc67#P0QrKE;R8pLJCiHUop0z4UYn~1|4T{n~G?cQ&
z<#_aa<VfB0`O5P9%R|1)Qb`2}6b=_*%4G4ww!-MFf3DPuhwcmOFLERu`~?*#W~g7=
zH1DmoMGZf!i%!3?ys4|`V9R<gF1O6*&YIP(jCN|o&cn&6%ao>|eS1bD-08=3KUR0S
z<JUxY@GvP((OUdOun&UBN!LGhlAv34ocT^Cbkzbk?Jkt<l`(x9bsujIFf-lVL1`_s
zN8qj^%=qt(G+5`duz0XtO#-fiKKamn%c1NaT+fPH%jZW`_`911Jm=#8lQ?gHpRwf>
z$cL_6MI$K{?nuJ6xod)6sfbs*oI}kQQ&Po+b*XS!2B1(+XUv?I=$S8l`}j5(qzm?B
zVCUMtAr{)w=knSV)TZ}~<n%Tnq9M$c-nZT!rL65*zvJS`M3s$0HF<UOdQIERnAa2b
z-#=9*ZGo+fJ%%o*n^6k{EqE}bV!V;A$GR7c*-wy@4pzv|IDQfVfK5vo`D3AcTH0tR
zm&T8`HVm|&#=RbPvUg}?XD906-c3a$PddZGwj2mDc}bfq==bh@Q2KZcf9^ARixE<O
z{ce}!&qWsFr4x*w3*Zn_ipU(7Kw8hdxLJZR`E6*mQ}76F{MN$3KGfxwO62|}G9p8;
zB#{N3AL)6iPfAoUof~E09<^-$Nqp8P=S&@12U|$D%;I;KF{^A5y02&rxmZ;6PePpn
zqgtarRw3i(Lne3Y*<0z|iF(%$9Vb~B!;kyS84;X%++L9B+%OjZZie3uGRk#cJ<u76
zG-8T@IqrUJ4y(dZhh2)1^Fvr8vxh(|cJp-g1LE-JGNZ@*G)Wgsb_OlHBwm1GQU})l
z5P*N02_F7f3@HeoAWs`wXA3?qO73n?V})zLh6w)sXEvv445W)vzu|jHFH{Tl!g8o@
zFqkm7aTbU^QmbEM7xy?EH|U{VOKV=7F>U3ueyccR>3GmJrUAktY?ByYvlG=zVQ9*i
z>(b(*&!?LgAMkDK&2@Shw~EK_o{(K-rNzV4)KkAg*5Qw>k(pforhFx5Py~Qr8Br9S
zg*-Q1YRw2t<X*KgjC|E%UDI%^X==W3-B*edV%BGk8PurHdW$(@ujZx=$9lI!HQeiN
zHAosFMWqz}Nz*G#5LsLEfm|?7H!NM9F_3bFCp!+5-^GqsMc#DXY@FwT32KID8P{(~
zq}?XykwemVqL#9=F!>5Ar<K$|!cYs~q|h)OFYwfEkv;e`KUO8$E|ofER+{AeZO|aV
z5ET;@pD@l8tq&0qALZ^&AMqqF-|CQ888^d_R-^{)6}jq}K|*1}?NRe81VC(Y*1}e?
z)L}-#uP2YJC(b_{KrO9xFqCYP_f~{9CZPN`#iKb3bO@mNOcA_EH=PhoZuJSKwfG*{
zgqb5y=Z9m`GPnmm)438#_H>-ksp<Jp|0MoH;DoD@Td^4EM~%1lLJNvtIB)!&t<#<k
z0(=#;!oasY@^PH*9VI#TO{sgyB1EA!gxR;gJ?kN<f=j}+_HJM!|Koxw=fsk%Ek!{i
zuRTsoRC#7$Gqe8A4u_p<UXhs2{zyD?&`w=;?T^t>iO%9ylBri77&@os=To7GMYC{b
zYOj5oLRe+9>3hx_pP9XHP>n4@i=O-)oPKY-wigy_aw4HC3dTb86fQy4OKsgA+cR#P
zv=er~-rK~NT3D-`q_vs$m>mvSm<~Rp5l3T3DaX#nb=ZS(%hl!zD;*KtTxsNZ9M6%b
zgShpU!_GgCz`H1tmKu{*67*wet@9B0r+N<NmcCaaGM&K(%R?UB3XO&s7YXG>h)prQ
zefbe@|GauaXW4_b7rAtdKFX|~!=qUn_~KkbF-9fr!rG!M{vAI<s;z|@SwXezZ5XP;
zcFSvsv%bGFnZHTyu9eSM%(Ec=ev0yC8Hw{bdZ+7~4li$(@6*Uk8Oa_@Sxfz*G#f|p
z(f0Avm$)e&k8Y4kZGNIP8y?@%s?Tq57^7Mj*f$0<cGE}zk{V_p=rI*5ZSuW9o^0U!
zU|wtfW}Ts?eFjT)dg^@T5shxcms7^3?=eyNE0cYq?gs>ON>&J9*#A+eZ6*1)wrk$-
zV58d1NrreBk=nuv;%g}-LP4yRe#sghO8a(Jdw02Z4s|QJg}2;ZK&FZ&bPFv3+?1R9
zp2OHC+AOOhbDJVqWJ8AH@P=)8*Hrb!v=vBS7p5=sg_hZp?gSnFZSn~!h&76>oaL_`
z()Te&L%TN{$?gl4>f6p|B18X<3TGBO$4)3h#JDA$8_L{#5FPr6qtM3_G&K<Jp?ipp
z_2o*kVQK!hR2kFEf%#L=Lgz(d<)09r+|6Cx!P{#F2)+>O!u^vq#QmDbEl%HS%Wuy|
z9%~t_4*A%9J{L$BAh>koR{!@RW?G7m?*0%G*R?qRaorOQTXBo1G!5|+INh$?FgwV?
z!o!iHUdnj4B{pGFW0TYhNNxl5Vj{))I9It7WnxKG{8-7DgHJ6YAm~j^b2@{qxcdjZ
z0yn)=sZa(}j5t97%Bs?PVhGoBSO2g-4gvmlNQ6d!abe%*ydhwWpAxlitm#z6A&?G%
zU>vC|dP0r7Q0a+-w^mw<`g5I{iuENI6d#8*KLpAnlf2$H-w6q~r_h-N9dHikBaW(^
z#obye>_39|{e0&{f16s(VlpgwN4?p!baD>PqG96WS>m;4&D313!C6!4_Hpv=B|a$6
zi{3@|HYKO(?bj%vq(2q6J8AgEW6M6S>*Drw4h@(>1CL2;5ry+JhBFma6=jzQcT=XS
znR=Ot26_~gbng-ESn+0b6-ssDo9C3wislTDXc!=Hv(7?`#>m*(wy?d+HCnG|w@X2i
z5GLiZK1;+|;v1=C#T=5?^Y6hF>Sirn<odkrrDt7CEhH~ApBkw*9Bv3WZ!aI(HMwnv
ztR|=<RK6WFvmKx3u%1pte0(J*8dlvdtD{j^MDHW(0_`OtGsN}zRx@WGBq`Vpw8@|!
zObK^k#n~<<tjAMrJdlz${Cd6HtULB!5>8jV^$@0dmWzZI+1vy%Lkc!O#_6Ufd${={
zdq?I<NBG<8Ann;Mk0z%RiR>|uDl(&@-Pe!;0IlxD#82{jY`z`23jb1TlGAET%>JD1
zV9ik2F|)Fz+<^%Hfy)S*ztg}JXxGt{H!FsUblJs2zQ2>FD%IDhX5kq(PU?_VJQ;9*
zg7KvDcyMi{eCCp_aMLvAtpfYgaPtUNhJP?k2O0{}Wq{cbKmDCoIpaB02R=L{vT2`2
z+Q2_?{MsbhN|0@qFGv9i-I(7$r1oSL*4|I}-NB4k3wX%<!fEC~w>fOjyZa@QeYM}z
zw|wd1ia>5eXz65cv=^8Aub4sq7P9U8o=26+_2N4`w3Mzk-sw`2GyW|=C2)>p8+h;3
zR><OB*}UCn4;_PJAI>AawrknkSyDWS)~;pnaHR|7o%8o+PUd;ck#+%2SQUh9Hebwx
zTVBHJdaRD3Po)uj(#G=_$9LunO<yU__B)`8MwM#_o}UkfD@wz>*4Cn6<%CXBC>74U
z_C3Nr6xdwFSc71A5j_7i9|N-Q-l*hqWOW|YbKH7?0LbqZw{EP|J+QQM%{UyrQ{%j~
zu9fDX;1mdE89B#AZ7}NTKBdcxz6HrM9gctO@(5-ja)p6s7aP!~isSfJVw`-Z%Sx&k
zNV0aA^C^kqUqDdodc~E<HR;H(#yMw=J+&)3FQY!`@g~K@^2ypFN)gasgD>++If>XQ
zErk4;{WA=ed=~A^C1$H42Z<dQRuPZ!y~28pYaeHyJ2Hra1^bPTOBMVY_vlP;>R6cH
z`inM~UE@<V#8}qFq-l-|2Sj<Yi9L3H^R4A6!y}M>n~-dWb5e|m$r*WQM#kRgAw!+h
zx|c`0M~9>Rdt&+1<TD`dX5AMGMG7<%&KH-h0rrbD{iH)}46vw)4EyvTRph%Cx{GQh
zK6|_~xIE6+jFFVX)e_c;l?aRd86Cqsr?t#2l3nyDmFh6CM8J=TbdXif6CXcBUK;d{
zy1`ws{H~mc3X-1j?a1hAEdx8*t1$jGrCrJwI<Qcz*)9q^`!j-O0IfP`G;f4ubX;o0
z&nJ6ht7DEYK@Rc27tgJ#NwrxPE=}~VKDg`45o`@9{Stfu0H`WgD~KpbhK~=k#_MPa
z&nRuO?OoEbY-$das`0Gr7-M^8JBNgRCvlj<{Vtxhe+v8V%YZ=qZ|w0GS9|{K_lqI%
zySInH6UVI7=QThw2&VtZqX(0VRh?G&&Lm>@sTx3$+P-ss-kWPn&E--MxHvzW{+2IF
zRGOiH#mw>`d|AEKHrzXU72fjk=^p!|-9T7TJ&0zLlsQLs8ju9urnzp_`84K2uTQL>
z($o1ps<T<5ui=2M_nDb2|B~W9FV;M}c47`0hi?=I4>Ln(dA8X};zr6xi$N{d=oBH-
zu7eL_o9C5LI(Ez;Nv=i?&bsP%A}@htp3eO@ypd%(G*3-<>6nR3lVoNfWX+Tc6C0(?
z%Hb2r0)v=l-OJR7Sh;k^xa8V!_J&ezx)^cPPafX{wC3=@4(MWe@qpwLe&lL8U5aH2
z<!oOcZg=LD4#bvV^pU7lU%J8}-^b10z;tp}0wEcGPQvR4<sYgBx5)rQnb)Zm87Mh1
z>de%GSW{sDRy^1qpuz$6nly0JKQn9hA=w^LpV)t13@Od6!)!+f2b5?%PiTk?xMe(M
zTu$nj<3S&+qEI3ia-=;67l4*L)*_cpn{Rh52prI)a6LAR^!#Hb5EzkOQ7wK4&bY+U
zdU3stV@bAWQF5Cm-!nOlJbMRMsQj7+4%<E6vQL%{JvX^pHcro07e}Jm4(onydHG<*
zKLf$V=kVL$8R0G_Aww-M7ElW7XC)TUH6|F$vF47P^wT`NW0Mx|tse_OImyS6aisoe
zq@Q;&iuORCst%CT12B&9%JE{Jec6>*Ed<K3z~AAZ4-JKdoMj0x>$|5hjo#Q`xz71*
zrNbm`Z@_B61QN;a#XadjZ}DGXSwRyzx54*z2s<a$(aEA}Poy*8yf$kE`b;wTeGDa*
zKJ8ciy)6nT{(x;MwfCsA=9?-opzFkVGm)cQ=7y0cyL@=zmE*i;CjNJ?tXgC?p@7t@
z$~!E>KnV7i+vN4&`2jiI5W=}|RnGkegOUmZHAeTsZ;|JH7?C^pO}v&Z>dei9DG33*
zFo&agZ~#;Jk2t1F=1S{Kr}kMhs;{BM^8L)(<+uq47pdMdAsPZWl-_e5QsLGFeEq62
zr!j7UkA9Nt{Pw<>|FmB`owkaF!fQgfKRp!uvXat+RDf>E?$d;j!IZQ1$Qj_pJACC%
zJi!vEZs*Wa6Sd&z3v(YtSR~fwqx#i5TEMZ6tkU_p{k7rJz52J+`z*@UH@2JKCfOY{
zs$?PB7mMxv7PhQmP1&(MpLGTZwo$CW70Phb#~u=59IBQ@ei#ln$JXR@ro?d~>lz7x
zLhAuRN$*R!N~M3(pA+l>m#@EtOyP{o;sRal?}puKb-PB+pobS>Qk{K%^AE*ZhVxOS
z4@ef+3eP?OEhOyYr<Q7lQE{<}E3r?03nD=Bn>Hja&?0sxYwHI0%fUd8x@H*C`gCK|
z6tocrT==1+&IEpz#(#av&lR$elbH5Ro5r1$?XN>tqn52KBvf$1wA|`zzo8gn3zKCF
z+IGSdrK~L{gIwYVjGu>_;W%%XYusaOOW`8*)m6Vu^D5$-%n$ETfIC1KxLs-?yuL9~
z?X;PP;Xhw--_6l}xmWc9{b&{cW!-t9cy75WK*!hH&%)}qu6>S!O2@b|FNCKwYx5>&
zVK18=u$K&}{8+S!0-6#ug==8_;1BRd2@j*sAD*e3k0jBaMDxvic}w^H7H@3{dFN~B
zzVB2QDa-Tr8>uFHfF2LJ^<QqHXBisF?R<{eqjk{z@hfv?wZceY+=D2K5}eb7Pan7w
zbVwhgEo&q)pC!(=<-!JPjkFrHJ+XS0rWxedoHw<1OIGbF&(x%aLC$M?$ArDAsUIyi
z4Yh;jZd_tkgV4yo!H@wKl&vH1bU&wxKn(+fV!M|fTQOhrz$w3l4d=7nedx7`ft$q<
zO;8}1@>3{?j;V*gfwFsOemZm?)?ZpGLp1lhs;nrPt#o_3+T1zSlj6X%=J9wKNff*j
zW>TV=l3az-RN_tVthtClus`!^CLrH>4>;k-@cC0;DI0CIm=Eo6nk;YXjF)D?GFSpD
zBHHIE%;B73spZI;6Ioo>TjbyS8}aaT?kd#&X4C`s`e$fXcyi^7b-2~SOBMI({VFCG
zYKij_Ql!@&KDg7v307M-z-D><7G72UF;rQsKk*DYY?;^w-*FC<t0>sIAIkD$`_?xt
z?zy<{f~3p0_Txp^v(}gWcH(I!epyIX;i|BK_)sloulpcr_?Vg4nNkUT4fXnb0xw8b
zN{kHCvUmUj$+mEUvVBoe4ioYH?o;+L$<trVEJ{)>B|AY;-gB;zM9~z~Ew9`UQObEM
zR-V%}u&4BVRBDUS6>_r;GBJ(G!LwNK8VgTzF2zce5tz-ghDRD5VfR8F!i5@gssrsk
zYHb@fQX+DGIWYTKIWXpLQP!h((^mCRt(ysP8iZrd&ZhFoz7kc(e$J!2$O<{2l_ZNi
zoSN%3eo2?c$|9rus!NP_#nRcdc~K!SdFEan<1dpA9jWPn?|kAYQy5toX%-y#NqY%a
zmK0F@)5w^I#1??nr#VefeZaU{d|?cHaMw1q3i!ZLAy!xLn)Y+qX`}lQ(uQ8Ch+9j~
zIWx3n206G2;|0^4P%Dgn=op`N6y*6C`|k8r=6>rC`kn_jTR4d1AO`&DScMnDy(jYH
z3AyKQv$(Sh3$@N-|0thdf)q+rgo_}dn7hS!q{z*X3Jum@;6W96h{`@YnL;FEqz^Jl
zE1RHY+pLdq(JZu=+D69o=&W?IW89zQTUi`Ee9D{)M^wnIS`lx;o-Ka90+d^H&e*5z
z_h2PemcsYXu;>L{{Hs6Q=J8&`LkKu=2TyvwS7btQg`vLwSxd@U8he#Rl7!i5lv_~J
zn2M;*__{1XGW4g14qm=`@RU(ST_;kQ@GacE(d9jRBi-<8OG3fm4@NU9l=X>ZbcR<o
zq&2?;WFR|TB@lDG$<l8{CBsTAz7fI9OP(npbXmsUBGV6*c7n%5pDL`$HPO?Hn}3hR
zdBrULZQv!h68`czh3HdBVe6502wRaTC?k*SCnJ=Hk3piOJ2U|kv41pVij%FZ=iZv;
zswQYvi*(&cOF>%IuC|`9{_>V!l17O9+>o|~DPO<Cx-qJ*(Um{F-j2Ut$?U_q`QJ?N
z>NyCW`n4CIi;!?VWWc(SF2<!gb)Jy^RVW2c^ypy{W_<4;XY!GP<)k3~DB%Ig{CSaa
z4k72stvZ)0RFdE<5{tmsP2LxlYuDF-Q|IyfCLX%tr7KqN(QaSkYdRyUmoU^B>G%ci
z`6U>#iO_Nr+Q+#qc(oqY86ZlSguO%p!bniW>bR%#-LKF=X`_gW%22b9t7wEcPwu;8
z6AMZaNN8aChdWfJNqZ43TV@-SB_DYoZyVJ2PnMIgH;i}a^zGuQuhLv+&4MXI&?Yf(
zr*e<HX2=SFn`c4;Qn@L-B=B?#W+PVxSlv1VbTxlv%}k<bZ7>sGyJ8>bK^KCd%_K_P
zq~z0zN=%6-uCQ!=f@zCyguipqYw(PKzeP9R#ev&m&4hIdH*<u+v2baR2wr}X&mxJ-
zY_U&3m^JFV3wmABP7|un3}Mg6KBB>5)J__Wp}M7kj~O&tqClg(;&6AXap6oad4w?b
zxyk6_gvA%jeavbKAbgu1iRX61?;j^u2rlo}oeS{cDdi^csOJCX3=GG;&`y;nS49?}
zhCt@sPcf>;jWLjSS!gRIimYxz2TcuS#Kq>+tgWp0@L(0lz*lnji)exZg(+Qoc`ucP
zuA^FdJ9-r&Fg~W*+PF{XR?u*dx-F23vnhs%PPpk<cF}_M#&cwpOJ~|jbbnb;gE_H&
z(8HpdVO65tt4(*WlK5XYFXY4J=+_@#!`Me~{xr&`*Z~yd&pi65`?qK{8m5+<+a`KG
zNO(N!tx};Ne?3_zL{4uT5;0;z=t+yEjYdmVZ{GMTr-7G@E?9S|xeGHG6zRfL;mNJ)
z4xTHNXI*0MCTHnW9jX&`B&4TXkQ4@24Bz9dbmpgA>`->t5_YpJ|0-xn(T<gl1a6d2
z7T<lu!UZy6?M&m)w%8RiJuH_K>!8QCPE7Ni1ma<Q1=bI)W?@);TCq1a28J0aYc|_@
zHG@wzWamt#Y%n;ncsk2at1-#zTAq9G69=gkD!MsIY-9(ymj;tm`a-Wrl_f?xpe0RY
z3PzCC><-}HIiVoxT9Y^|Uykr~q>~`(s86)7FW~Wp+TKH#i41POVsKy$ne~w7)H9(X
zitoGE{gJCB7ft?C9S{AkMeW+i1iwe+Q~*wG-yowm3^o={&O~OZK6|vP*Dg~d928Tj
z{00h9R)?;Uy$#&x@D48@a65QcwOB0<NBO3rni4}ka9NizOyT7hMIjmn(!znk(nQ1|
zKcfdag`&qFZ7aU2SLo803R-K@9Wzxvnuy3<X^P>+I$S2zmw;7&M#n5rH&VTqEsWCW
zjAcgJhdntTU}fQJUsK>~)mAuis;QPs0JB#3lXCT_B+E7UCzI7*R`w%(7z>rINOqK5
z&imWXx8?Tv{8Af|)+(=p>65YCYTS6TupQFJnn@((p868VSB&vXAvix<{SGlZos1%?
zGEoS~w%yEv!3EH`git@}L-8!`Z6_bx^R#7^p{KpDtt5KP1{04Qkp9ic%KG((@vYj(
zZaF9m<8K!kX(C<vd`VE@y)&jgz77pPV(GlP-u<-oL`=D?p}a$u)Kc)eUBeFM;;`4y
zHL%Gv?`5+k+;SzZ#T-(f$4kt9`T3L@UZ}pbA|egMMDN_67If_uV<SEzv&Hp{Nqt(X
zjn+kj)3&<BqeW*;*shOz9x`PLE~Qgu?xK5zT+K?{l%R%7-|Vp+4rfsqXl~A)23OXK
zQsalaXw(QKRjmdTeNFmsjAyXV9u$Hbmxs&4@U#`{8zdNNo+L({=#r~9nop`{s5@ZF
zE%XBhK)6YI<GJ=U{?3n$OZm}xg0Sf|*8`5H>xx%j{Q0;~D*unvvH1wKmh_|59guz7
z;P5>e<xiK!yN2n<@dagiDos?)ATjAw0(kmi^KH+4ar@igieqROZeaiET@$?*DE{}(
z$a<-FzJaqFYG0>c>;%NEC8NsOGDlsU7U*yb8FK~W*|nqew{FfrcS7!<NbnwE2a?nb
zbq5KMFJfB<JrVTC66>W@>+KzGEc+8<Gcfj8+DYl*Qs6a7<UjN)SdDz+7#$7<KxeE$
z%eZ!5F`H8PfAnqr{r&b;=BL_ed+sxzj|3JrJq-!W+__jU9h#NP#hW(C48S5YL>0o4
z21|uC_J6ALpOfLphI3#Xa`ggKFV&}s#f-<+5B^vd)HK=`9XWoSTV2Hrw_6kghkml6
zqS)7-MOnyU!M~NV)y9n5im51sp4avd<#Cl>ryLuHWAAG1@X2m5<c&Te7_Vg$@>MD;
zMt5W%!!!w{Ss!+58yA?6<u>{P@Jhu3pqVD*XwkRw;a|Q%D^t@ET%<S$jb5Vcv6mv?
zvZYkdenr~LHNI1;m+|kx)r#Ny7E}4Saw1=?JV>E)Bw2gE??)UMwV!|2!d^<$T}5<p
z+^lblS)q__twh2Na5FaA&gbByzg$!YUtz`HSFy&Ac<dIVc6`sx)t~rxFVxB;xV%VZ
z8{|eh%ES+<I=Acd=Zv?JKQ;{Z@j2GVR=}KH)gG>kTc>TvK1JW>YuTEzRR6}T7?}qn
zN-x+bbgCjhI(Hz7AT@~u^b?wfF1QJfiH)G&_UCb35cLuH<crg_C5Tyc6HN7#UXx$S
znk=UifUY*C`)EfiU9$b@Jxs^Hk(P*U?!dpt?Q58Z0iG*g0~n@ZxVCn6WRF(8byDXX
zO7vZ|i;h1z#)VM7EfK??rYS<NQaq|n%s&VWzV5O2q9Re~2dKe5EUpDp)Xxhp8JB9a
zs~B-L(rQ~<?7Z?^z|P6qHK=IvAKFRdzFeL5o(d_&uYq;vr-!OllE*GnC>X`?Go^eL
zWB$!l)Z#F$t`r-5U0foRcm8EbkKXCbEUM*b8>dpJSV7N2{^1;LyQ!)B5^twk-EIQs
z)?=DOJek03teF5feN~^Q@bjZ`6347EdbU@BeZggJ8WP}U`S)3PhgEC!yaQ^uS1A>f
zusH08#Qu!S`F5#a^sz$=+1EyrsaY_Lsw72>21uOy70|t~Wywr`QwBybj<cbcC^I$9
zB=(|4?S6`*Wi~#xLm&PiF%(5|2hF0Tg;>nz$K|7}RI?sBn4GE|D^xZcf)BPnqq?r{
z*JY`dQA&$8_3o{ie?L`p5%<YA6pQ6UV@^Q!;4wYzP!SEaB)u}WYw0fzXV*oUQUtd8
zJGuoJl;A-&b!<Jk|68sb0;LUIO!cxg0BCC2Gz@;mStO3lU2OUFY}LoPBiZ&swg+On
zG`O27P*I#d$uoL%G+mw9tSc6359-xqF3KfTk)r=XiWAAVN<gUmEpp>Mug^R)-EQEi
z%_rN_7>fqUp`si`5XazvaPO$;X;M!_uxLiUj1FL?gwh=5q5RI?{vi}AL)^k55B4+D
z$+kah>nyDA@%6wv18Xq^D0#aCWSSfPW;3Tj(73&ECB#O}?kLamt3u2CPq+J4TEV4D
zGVCPRRx?FLk^GZ3>7D-KIkLTjrgC0R1_~%nkH*y8!_4`-a%CSD{>aLPQ?2<PM{9`B
zJ_80iN6g`=oib8wjfAHHi1$ZLikT}I#*!Vh8~())Mu|IVFXTQ$#%0I2mtpr`H`V8$
zD3M1eY4TS#4Ezp;op75US@Y(0u=JO7v^GDiMDj59bqbfDy264nStg=3vwKg@E8)FP
zh-fCIeI<~|rn7kYgCF`$DuvWN8eEPYBVr72QAi8mN)d{H$Xp3p1siDM)FLP!DqzS?
z?6P>q6B7wFxYfZ6qSk<jVP4G#;-Zn@I-;43Ow4b+!yJgBki?7t_h@7{8e$v=XLXRg
z7@s^}&U$BGp|BMrc;y}2u+0L}3-pOkGXL<UPWHDpoPFZaJ*hF4r~NJX_h;2Gu#wk>
z_6m^{Lfqtle8$VWg}+!?wdp9q8+>5gx3qYjpg2hA){B?P18D-L?+{5eUM&J)nLPw;
zN-#j>Cht`^^zeYQYp3Yse3tiEKYsQC_NP>Qutzwl5#TAN(eMm*ScvTe>@!Kd=m|3(
zCE14H{+^BBaosqSmD?Bp2WRgTq*)lPX{IVOD{b4hos~8#ZQHhO`%l}pZQHiZsXjfY
z<D8i8n2EXD-_4G1-|bk>dft~6#QJe>l8wt3r<b3&UIq1^Cc9Z8WT2tcRWq8tE%H)!
zbrjdoP`Bn<bcel3zaJHfa|KB<9w*zL5H}g4n|RJibr-PQLFMWp+=N=8SY0j{ruQo(
zwf9k@s<Dh#1pJE-fS-xX{O!m(hE$$g52&N}MuW<MQ<XY#QDLsB#lcxf=13;;7V&?7
z)usQq#Hvj=zhCSaZufj%b$nBqA}Yy@vIAXflrBlgwU>h1Op(h~#$7_rK|a3SIi3%c
ziC@uNV<Ny3!L=khMN{UOQABuoyij6u8dcxZJ#!aQ@gu`*GV^`_n@hR!XyD;&d^GZE
zUq@3vj|4x#-`JzgSS64=UFB7bVuLaE#uhiT)FP(HPwj?QB8&Q7?@owlg;TM(W1_DO
zuPWidOO@#8DCesL4W$@}Fj5qTt%tRxmCZ`c*P5t60U74tm56-#yu-AkEeEfvmXkiL
zSjK8<MnSlY;()F?5v9r)m7W5<UzXGA-Z+W-=;#;o`@(}XSxlgbAm`IqJyNzVk_LJi
zI{A!{acHf_KhX5ZY-hQ&(>lloqpxkQe|pH(L>leN6&Q!LY*EhgJ}CpMhs-HPqD%yv
z-c{mDjT*vzt;Jhp`dc<^0Z8Sb*1%F%+)i?Y!)dX<ne&p$mjl-L8`oKHiHK=u`{Nc*
zV=+6_Ou~Mn{koeoYV?)q;Nnr`!Chb}hBj{;I2ibPbW?`$+TljjwW$8LP&Q_K*r}wF
zKxPTApZ9*KQ@N}KXLE5!L>~KE%(IsJfAnC&fB>-YDxe;~J8Fp%YYV`K#!sJiy71~2
z`5yojxlX!zDMQ%l$S|616)SBhma6h2<#~10(__sw_6v4i&3!*gjmPzldcp^nDE59h
zNjMTAf6x+h@r(I1Do(P5xwN0nN8%j^PReGo&HpgcaalCS$3**W%%y{gMVEa$58Wwt
zCv*Ce3~Q^8SytfUqT^aRmA8C~IJ5uBW#uWEatkh-B;hFA`=~XY?wv#z9TapacqQc0
zB~JcZ2(@JOxP7E!_@TM;8I_jA-2IJ{HC?7M-A9xqd9UQ9`ir`8wy1{!ChHlD7Uhu`
z4gfY0K!@Mdn=KGe7hsCHpeSDS>W-f8ERn;#7GU!vhEVh(qqFc~WE3xsh)p;QeSOB)
zoH;o&EbS56q2~|6>=e727>|Cr`o#-NtLTanF@)jnNc89u)OQax$n|i=*9a<h4fB=M
z^2zoG@Ck(}V=Vz_-mxWW<%(PgKv~|p-om0bh0V990t7(=3N5Lji5Bgr;+38KoNgP0
zdk>s6Q6k4YY%H8V$drM$XFO@fs%_rA9_W*{Owuvh@ik$xUoH2b+JSR}Jp9ZhFxun9
zqWtZ2l`$Obz6mvDwG3N5s@%W2{P`Md<l9-7ItM&2Kj|bDDjF8Ah@;ba`KYRk|7;Yy
zy%BvXoUAz{u-UzPUKgQas)=m>NhJ_`RZ|4f?5<7T!`V`^Ci<Om%VN~RDKVwQ-P5sV
z@uUt`joRP9#nJDyWjnX}N+#&^V6mH!wxkp=Zc{#4aK=k_<331uN$Y>>e`pIrxi$E4
z2Wt>&zL=sIL)CF*$|_@%+Qh_!lGK#8n(`K3eQAYZ6jD!GJnKNEa9|9+&5XwZOP_uI
zf$dv|C^MFU9y82wdU7qZW9ViOcqad+m~0oP<4(EUmd`2CRq!;BZ*!ReO@f+D%+njj
znxjfx&nQ7#`#`$>_NvqbrM=iEscExTI+O!0hmunH{ym=AyPfOM@{%Mi5`&f@_6LVX
z_Nl+9aAl`=$g09NQ1tkKJ)=)qQ1z%yB~gpKo2_X<m1sGNTB_ID5;Q0>L}CQ`^|+BL
z<O~}q$Dk;A0fTvk^4`Pov|;uf*1aJtm5mVxP=*`VyRQ-D&XcG>h>1F#%-@5wvM(&|
z>gEuaemq1Bx<oq-Y9cByaPKuFLiQN}HL0HnWk|L9LY>h{kuAYIa?;-k;(2RTC<F-%
zzsj`th1+0!J+HI~tqtz=e3@`R2NyJWsLrMY%qU~}^?W#^OTx)td<H1Oc11~CJ`#7%
zK1frZf=G*73khy!8m9X}5&rH>CA#+~|8kIb0G`6I4TRkje*yIzjQtvlT_>c!o>qgG
zv6mtc?QZe=jq@#Lk#hvweR}zahF?PvPdK3x6{LTx{MwVYLD0jQ?#+BZzcw7bh?EPd
zMzzmnb@HY)$?aNI%&}c|aajBhz2m&s2B!pp_OS32>%fBMuv7!b^54<3@bd{??ey-=
z6cQ_2VdKMC_HH4#fQwZ-9lEu-n=SyC#oE!3mrPg2C=dOLgDdwqj}YnKOoyH9QS#&D
zXrr9z1aJg4LXN)e9%vmen-^`+>)i9Uqw{kc$j7`E{n`&|nZ3m@2PdXMN6p!DYtn!;
zp&!=~@@xZI;LPi{6Nq?yZo7!vslOj{QTZ9&GZJ`d{x@UC&rG14Q_Q9hM*`n!up^aH
zjhB@#tNh+dH6$%9A97wI&lC|7N3(B;SS>c;!en_3vpmsRtSLExgQN@PUkjhSKYW!p
z?v43ohJVS;DmZmKzVoFn!97v0iEf6`r<8aQw~or^F0hnR@cs@#tYwFt)2&J&cDBV=
zHN8zE=Jv~qldRI%yGhe`(b1_jSGKpv3qH6M>pn9^`YWDtUz6o2>AngMk+p*~CaL7U
z3no%HyrtyxE^dQimRA7+c=$<93WIM&eZ~h#5r#gh{Qg*}9?n^D>f&o^5xz#}$(H$Y
z&o}@9a>-PE09ax|<rc~Z>cB5a!L&WPUvV;KQy}_o=zq^moge5HhRtTfUlZ568+y$v
zfp7&SgEWoa3Sa!3-@i^cyF^mSp``9oTDN(dQwvq4_azRpMEY}UlO);lFsHF6CYAif
zU&Ny>V;pui7#ceY*MC)-2ABNnQM1udYoY2(Jn;!%kbx^ay_cmP5WEAHSd;cz2iCPq
zc~*)^t}@C!a+~FV{Nxuli_HArc@dodW!L`y&5KYJl>ejnKdVY5{!dy2C;NYD5$ylU
zN?AGo|7sDmjQ{rgm7I*NRR}o#85nBf{~<CEu&}WFcQ4|9S`iZTlK&4M;_v@$K^WRv
z+1k(>+FD!d|4$>r&Cp8U`hOObn$nxmo6}p;ThUw7+x+t$?C9<E9gJ=Mqu}&Eo*e!|
zpRlKQpm+SIH2g1-;lCRU|HD`KZwAA^&QJydTLTLM9-jY{6c`B@S(yF_9{*W6%Erj@
zzfS*EU|?rtWcm+(f#9EZVs7PR>_9*-YNhXFENpCOYh(<?#|P!;<Y26C4du2T?Fzw?
zb+z0~(N`+y#K?s>c}klv94(8$Isn5w4rAyfm_(s~Buqb7@PeitCL6YG`6mc6KhidV
z{f6^|_orw1rApoMc+FGS^X8-dr3ug!5-GDHAOIXuA5xYZhXGtmAdXd9Xsnk@_Y2Hl
z7)+2r9|%2_5aa;zV*`GdDIiZ9k)rU?mmq1D0TTS81HMnsUxR`~eqag_5)C+DsHhKV
zo&Z4}11#bFU$Kf5=#9`OqI_-&fjnq%P#Z{VlKixeSr9Hx;MKzmaokD>JY-5jLhA!J
zZs{(V5U~}3crF?kXeg&X_qqU%KR5+87|g*_EK+B^9n7%0hWO~ffDpp|AywF)jj_8e
zU=IQYyIj6W)DV|YLH`~K&?Z9JTK+{aU@ZY5j{c__Kj3k+17N`cKebTee((^dq2gA;
zJO&no4P3L_>fej@A%s65mfycxK|dWi1!@Uib&tM{eyD;3zGXvJS97zn2*v5a=HbjD
z?Sg=vm05l<=A>~8Kq9?EL)tru=syr%BLoo-VnM%pJ3-2e2*N}5F?|lqEP+Sc4YxwI
z6XE#UqrCqG_naCRFGU(3`yDda3gVM}!v_M}(6`-@It$q2B2d5+2lUWtf+1OaM1n6)
zsh<)e?d^jr_545tIs<=SH3aJW84w_dBXPt1Tf{#$2StBh>YbgyeCCGyJ{op#+dqhM
z0ADvS@692w(qF~*i=`gK1s}}a(v$Q0X1<k!1_B4ogT#iJ#WD#M0QuI&y$D(RHa0wo
z6?Frd5>R{d_s`V@`0}djp@NGL;&y$*{_q+QdJAnqcu;-%Qv6VtmWJJfy+evi0RIXa
z5)fd(Ac0CkfB*)3Hca=}?Aku}bhRT40v7zgwH0v9BS9p;L_j<}eUa^d3ShSREe9Yz
zO(CpNRAv|mZhDSv;gCSr^m=-KPDo#~?|&A)Yp8xIK7T%hscd3mdJZ~$fxiJ9gTMDq
zpCJ5~8nD4Tur(+yLkNDbEnq$@^P2SJgEAL<#g-TmE@LPNu4=i^fuMhmgj>rAc=YA9
z!G^)VQ{%pKz?%JuL4GfYv=RAqIn`1U%=!-1dYT^idDpcXoIG)Z4OB1jXHxx3?VQ#9
zAg3XN3(B2Cpa*jKRFi@^1s$fNLOFiZ+z}uYB!LQ)0XF!}2fUs?>!+(U1oev|==12q
zFu?y$=?}w@|DUv-WB&H_*1WmZs?RFe%lPkR-yRS>2Dvm?Jrg8jVq_~_)J-qNyxv2?
zT1FbFTiUZhVKqIDl?2FzDjt(2B+IxQwvSwK<6`Ja1M{8Ys#AxAIu7UE2AwI515yu?
z-tv0$5sNQ_`}5Ovx2zdP<_{7PP}vmwfPLCC5GVE+x(UZ|mwiw94jWK9DBYZL*9CjL
zO%_{a5|%$1O`03j+lA)yOj4NM+DjuW8ZTHjVYu@TM%7$HU{xX+q2v}Nwfu3`yuSuL
zl9cFP9$%(+P7Ti7zLf`f?F?fdFpop(rRg8M75W_t-mLB;CqZ7WP^j=N|606Ro4MXK
zT0DvfN}Zn=I5lpQ7WB=`2CYQ>R_P`1xaJbWE$_f@iRY*Am|5K3GaXIfjrjmEB>fa9
z727i;q8S$bHi@0*+y~uBbNsv;>D#y~lB~N$;R#jasi%xFNL#{AS#ar2`^)NeAl1T>
zJx7B(nfppfiRf1PMow%!aMc&5L3~n@vZy<EFj>cUi00Gu5#p|Gi^cC4)=Zxm8gnO;
z9W3T;NtPEI24DJn$s$ra*Q#J0?L+x59Y?6=$Wuh~%9A)bYjWPsf+<$by83hxe|BE7
z(iWo{YT$#SkrCEh#euZd=w*ffjW?M<weh=z)*4SM2H77X-kZE8j9J=L!?CJSAmT~#
zMki`-|BF?L2LfO*ar4!i68O}%!F#!Uwq0fR4@sPSdf>9}qtL}Nx15x^zj{qjRp#5<
z1jUcdu9X_|f`*Fd+3my5p{fygI2<lo9(B-+ayTwyH7yz10qHh$a+`txSzlcm4cw<r
zJW{qS<)e>sF@kb<Kmkbi+pym&o%t!b*LG}_ZV1!?itZJU9*#IdB7V;{d4=yqt#NOe
zXN;r%Mq3?U96Hm*=6K-8p1<D%76SW;`?!+MIeY60=!%YZO>WVY_&*V0t@<|NIq+d0
zLW9@tv&muUI4~*s>wroQ!PGXI+y6I%x5%n(P-(Tlch3BGiuy`)T(fIEn3wA&j}i&P
zav2XbdCSU%?UZ%8qnjgMV`yQ~^rf2y_}W^1N33?L%(<*vL!IPX^?BKuihI5?nW0O#
zO|hfWM$-UmFpl%%k>^j<;U1ot;KAM9t_3r-R=7V!b>qL*w_SGwoTNVN&r*{WDSF{P
zE`goOg#5+Gd0oPlmD;Vmw6K!KlmqwWrLacB9OfSFqeeF8TwgynoqOiq)H$=Ul5*Op
zV;XE0kd3vxS8do<aFYl(iw9ofP-HG)kBN_)(RE*{yF#t;`S;<6kdxCJUC9GlVf!=2
zSZv=?g0YR1=6aTQm+6BnH{QLgJLSJ6U!NwS`TQQaL#=$OBG;z_v!PWNT8ybT7~*zD
z$TASW=>i@M&yA%JCLD;zWJq-4k7Mmjr+?`R1yAtwOTNVt+oCJ&Kr--OZZ?5<(a*XU
zKg{=6N~1I;sE^6@nyj(NZYb3=2u<!&PA7=C;FH_5)oLfIDVdAVZXRxTEq_^)R(l?z
zG>9AakD_(p-#zAVkY~y@M~^VjkfVJhqDMpMy^zl3^5MBgb8N!r5@mOk(cD~!Fo|=~
z9~y>dE-NexPc0=y8!b;hQF5F!k@0!^8BT{R+C?fCjM#*r5(N~l7_4_ws%1NI56o)j
zJ1?f>(;O9{s`$*E94s`C-M?A-?1!*l?uMYxD{0TQ;GnVv@X|^dlI?^X-4iTNXKK(%
z5tb0f`FL7K<IcCd?+&(#%!ez$P1`hx87VOFt~Xy?BP+P&Nf2lF3x8Pru?pQ~>jX&X
zxPk`i092Si;;zh<>fA+zfuaPQJ=SG}{hRiFqiHZ86^+M>Ak!{p`RyLtByGU!7Y_2N
z<}BVaBT?F<?7gr!tvB9Y7oSlQNpSlQ0_R0ek!oCQ@M(3m43`z`x)<QF%Gzl-Ow`6s
zZ*@9l$CF!41paGC)eib=D7=5FjW+)qS-9gFMP-MEtzL7n$#OzdWu9E_EzP%Ym%?@W
zvcz>S0Xh!t-G|Wg7SBg`2>%Z%L$zv7siRgARfUJtRzh<C<!u?HLI%f0-jbIx_yBy&
zeQ%RQCko%Mv(l<fFdTV~U*V{k{BZ&8aooC>j=%Z4<KL8T%l}R)uJM}eP00^k@}c<$
z^e=PZm>sWPcQ!56B5mqMSE*M;zDmw(Hl~c!-arkMHZeA_L}rc)o}_Ku$JaSFg#qhU
z=-fZ4Rfihc=d|$qsHSy|t#wirCP0Yd`w?6i2H%$@ZyQQbSymU5qCVMHjBM?Pper}X
zQM81l*2W!ZJN)e`roEkao-%u^#M<ZRChh-&gN}++WZGkFh{nScR+^*QN%^$DSE{N(
zQNC}!{L~86u{Hwt34{ODm>fO8Nz@Xu7{ls?ldV@tok>TfuCkjer63xvRNf(KYQMIH
zs9=1e)&J#W+~+xz{a_oI_Y%wd1vo2Rc(U)#%eqXSKgXZ^4Ts@3In5SC%_nM)B26bm
zG9#p_y>rfJEwjR9)S>RdhOOI#PDDLS`!}=Rr`kU~YN3Diw30#074I!NesGy(!i?Nx
zl5SMEgoT}T`x7y4a^$VnSM~I@T|dumX0Ufaj5SyOSrFdYb3`8i`%E0D%q-#d=iacX
zaefF%Ks4~Qk4sU~ur`%~SH#JjBA}*#OMOr{x2Tw1lh{4qe)Lam=fqJ$kJFY+x#z)>
z<!gV?Mh^2AdgzTPhjV=PeD&cm4pqaq6lb|u&aWc40YNMXTsbW>l^XNEx2Hcm+Lkrt
z6(H^tDceXck7k!;4)&aejqvB=46tI4<R!j}PgPR1^t56fBbX&S;7hNE;v7Y7&)o*t
z62mkP{BO!xybsF^&nYY&Pp-s**(VMiRQY!=SdIWyWLu|St4OFZX@!<KSrQvhK*z)x
zOoDT+sV+msp{28~W142^?kV64y6B}_mk*<5&o#`;cU7&T#p%s1u{=b<k#CEh#;<u<
zdPUew`)-)YLLM1hC5EM(zK#PT7+IU3Yen3e9X>-sUDJu8tJNmT6?M5dO4n&FoGujC
zOXLS+YgwJ9+gK!T0mq~flXK&EFGOZB#a;2H_3=~E<R8Y>>*PjrOVKIB*1EAR$w_~^
z_~CSq#=t~mudP{3?R%lwo0rDjr4y1DZ!xOz#jXJKSok^6RVZQsRtToJh$_cSwCgo4
zOhHZ`S@arivS`u@c=r`cT=Qa_Bnd$C+9WbF^9@D?YBJ=((Z%kAVnN8sy}zr0PW#>@
z!bFPic+JSoSOfujVG=?^#)U&6l6?j)PF+MRjro3dMXF9FQWVmkIbG_R<@&G<;i)v^
zrKoJ-4fb=?WGu^$!rtr58bgFBe5pJXjAbbJgBARH)aijzE6-!0)6OJ3v~=W<JJYT5
zdP9+l{o_(us)3K6`Fc%uhO3Jm6io0<aZ4R({b^k6{^*l7<(+V$iV^BKwKW#6HAeBp
zeSF<mVOr%_+qx)kRzE%;+>-mUj4Kh+wM#?E;-q;}J`Uxb)wA-h3JAZA-u^uJ8PKtO
z5UeGwMC9kX%mj5FY2|;6!Q<Q}xpCMnjpqKmQ~r7?M8c-rDPXnCuMd;7AIff2kou}w
zaJw>&X+9q-ZfIDJPzH4V-Jt6+=VI;nID?pZ=)`bCZ?`vxmU-1cV>|p1nKw!C>wdIr
z-j*(ib8*VjC4}}VFf6f&Z|)iDucG(zu}WuL0CN0HXlJDCpl@Q{ShP7diV$klEmtCx
zl2R<Zu+Hic4K94wzlq1sIaw_B`bDk)RrtNc=Rb+J8!|OC!OpE+j|YTKo;d!vaYxU6
zZ(BMj&z%gT*FF4J#zt|B{zNM0I>KhNf7>GCaamSh`FLAlLJpF`N&&*2(i)_~JOFE(
zdJx4uiq8kI+*iKW@U22x#+v3py6C0x`WQC~rafgmm{4f8gt41G7ul*@3JR-60)xMR
znrAS;Rkf;q+8U5gtpm0AO`T3}AR6ch6{rL%d~E-orLX#=8OVOfN*$?Kk|NndPB+7!
zOa7BKzb$9FTogbB0lJ#9Pj5JOXDCl^!UNZX3-XLPF3n#Ub)}dxnPBxhuB!Bgi**f}
zG+QGC#OK^ny;9A(b)m8L%1ombh7}}2$Ca;gTIa4}ew(t3{QY^6^vqvlD$PB~Ie@AC
zApE{t&%(ENdIjm_X6!k9qRXL6jwk7~Co-##{7=l)HZNQdjja-qTzAm*#JZ;R!P+wu
zzf<NNDW#FGiwGq^WKC`qt+H>eERDmnhAD&9g$oAPRJ{dP!^>1V*Pq>(@SY*JhChUi
z*O6jZSRiz1@ol(lQBvd^nKR)gwG0c<3r<Ud*hh5wXiaB_p*XNNb-j3Uf}SVkplwgY
z4Cq?kHq(=><Gh)3kvEi+`jw$RjaWV^uQAG?aeDDr$N=k(hHRtIbd6PEsoeAcwC*J2
zxxfQfJ6u%B2x^UzOMPOcW3c!U<WOGF*osk!W#T<XqTo6-OG62%M?>tE<=OEsD_0pK
zfREdHUtc#)74Kp4%#412-0kW0M^I{7D4s=n>R4Xm1ecDx%J~L5r`HBJSP8&^RpjXo
z?e(Ob!{v`v+2UBFWw*C>m;E64%TlQh^{UBHttmhM;BC)q9m#T?<sslNicq1GjxGNV
zyO&#d>QBA?#7^`i#EGF=hfjL)EXvD=NkikPaeB&g=Rsg<RNu5ru*RfOWo?F54O5`W
zpsBm_J*X{H>u!4gnCKh#Tf*~zmc+^dCdEZ+i4|-=O%y){)r82UC15^g?R3=)#aGv?
z?QXvnDon^td0cqvU&`Y`MM5OE@A_^rwX#mNfHXs|t?`eH9zi8V<=Gp!&#>6c`)^<i
zaFdQ1Wl_HEHbr&%+sNFyP12Sb@=1HRoJHAac`G={b|z3~_MoHNOgV3?;8;ZE&hqeY
z+7R--n)yOb98~c!TV(D=wkE)?d@qMmA3&zzI5={}yNNj*{rMBbh$+9aEGYebs9NW7
z59yo512tdPc&km_p<|%Ep`L%=`YO|W@{7LX&OL;EcjY>guZ5Ph3X(7yzVTuCAZH*_
ztTYsRU?;TqVMOIws<15C7QY6i(v<JgW7!jX-ogIMVZ(9FVu8~4Lq{PyG|@kvB_GJu
z4N?o?wJ9Fl0Yl?aKM_M<d(zzLiOxUw)_56cTKHnjs)>?xz-=o50WL#_K(5a(KZjq3
z&}wG`c^|)iZzYd*y^cc6nWqQIPXqU8iq%9(Jv+`>dv`*@I5N91wCeAAdTzbz&B*x9
zO;<;W@DSe{<76f#+aKE8DfHj?7|zhT7Z(^R<hpKzIaY(LUf*p4AP-K1Ies(DA0Zvw
zZ@&(kn&`{=+un--6Q%;iRh6QL(9VS)#24jl4sK$<ZK^Jf&GU?R5Tu?#=qxm4Y;&8M
z+Q5OBO71rHJ~=U|r>TY5{Q}V{kVNG%FyUCsEBTW2M#<8|dGfNEbE%ABPK~@O@<KBl
zVDhtovm%hK$8uGkn>)&*Bk^BjkT)qaBGJOyEG%BCVl+QJut$!0ShUUI<8ygqz$Ug}
z{=&y@4@J8A4^ugyV35EDr3b~s6h?=9g&`+R=LPykLO4mBjK3G!mDKJqCDrh5+NlEK
zDtLy?kLfkz!+d!xh%_Lyh~@E!NCfN|L=ILVAn*t3;cJxjzq|C0U{D$$oqbI4ek-_o
zP^uTt3&{)1Pa*d~Ii@ah+pn`nW!|_A{GwC>p@fP!yP=_0?-;+bl=PtUXgD9Ps-xIu
zUBRfy!WxyFDE-8^!v(dXyPH{Fn@Ii;h^IgIVsVkvlt1aCetIbP$E#NMI2hP&2nnQj
zMb#jhFI-r7mrNHsC+JZ8riMT5dR3g3*40YG_T#JY1`|<FXwcG};<l{9DkF|<S~O70
zqF_+T$#7nk0q3oL8SR@(ohIdzrN^(yn2YauPS)XIVR}q!%IP?X2fTKSgf%*3fKE?B
z59Rm`A~;iGFAvucb}<{(=m5GAKOdi@wr9d<kFb5f@+89yr89|uOejv=uH>_5vZ>5v
zq%UDk)}BwykhTw|%h7^A0~^wX+;txb{n{G7sFgJ0#wUaBp?z{qPn4o+<Q&HChq#T!
zMZEe=8lFdnW@j3&2aEwu<fgeWL~V!WLY$fuTTjxyzk_DQ)0NuN-u0RUjsp?t$(m->
z#`ITI-AU8~c1jHlF(bc9bGR8H{^CX7hx-e{i~@F|woPoK*;8EySzKEuoOgO431k_V
z$*7DE&WYK(B&Pl_35f0Wc&+KnO^G{&1eV23(p~tfXeHxg%Zcl%5cU%Mn`kww`4-X{
z&JHo=%{dv#hcEXOlF?)gg=$j8r>I9|*f=>cQrPOLzk)rL`qz+qM1ON8$cLf}ZvRbi
zaXT<>OewyBcxU&Dm=%3vLSrsqjHR6_>Q^tH<6a!ic#*HCFOIaE2+l>caPJA(*m^E!
z#3c05m=z=Yozj*_rW66S6K^#2XyXC#Z-pwGWn19JHuGtw$Hq02d|48&vs)1k-&q}6
zawnAQ0<>F;yreKd=}OssZ);OSv%W;<(#yL1`81$gV06hPOSpGJcUR~x=I|pN4>$7F
zZ2u)zBgdU-V%vGmu^qn(WD@;24;K>8`_|EfiNJJqg(O7eD+2{~p}b$)Xh(8IDwuet
zmq@}4xo3sYA@YxRabi%UWr3WEE*bV33FozKpEvt+{^U<O>E<JCdr92ZG8cZtg}|8I
z1iSIml}ajMs+d^BQ-jB&Yx}RJrGulV+d>A_wnClMkV5SgEEUCjR=mL?ns<}KQ%lkS
zSWuavX`_9ewi{kKM_$?*DQb%mTQW;mv2A0I(FMO5wj~qe_)bviPMt|Pce2kk!b|zO
zwzStw;WDV2s)F6MO$@-z!<>5e(1(#afNX9?rbdK!FB-ZU>&e~d-TVZ3zza@2_AH0*
zO@>Yq5WClQGo5{W+YBIOuqa1uMPzr_s2eOjSy$QQ)Rk-!aCV-MW>nqyU}4JAuCSN{
z(S_-_FKWWVLx0;<XudJAr-ZBd%JR-JL+NlsDdu~huNb+;8q_HAf81VtH`{n%?FO97
zA>$$7iCC&DGVoSQ`=)wW%^E%yaUJ=%o((rUi;{fg%W(F9YV22dfTU4s<J<DHVCQAj
z%G2J1`;ITxXmW3Hz%YDtn-z!`Vk8xpq)6B`qR+v6;efySXqj95fL<0wD~*WiqE+^u
zOkj607LGiotD4UJWcW(63574WxlL&Qa5kyPhGQi4ufyBaK9=m6wtx(r<<Rn5MW}T-
zY5U3r2CR?K*BFZj>lDiEIOiSFGR$Xiy^Sskk8+zZ0&bdInt{<+*9Z(x)B4&RsbshE
zlSR}(XU25P+*0FGXv!<lYJL;?tWAF{qYXZGmE?Zhm#2cq)bEfIZN~bI$v3Ogn%t!%
z>IPved0o7{eUtRxPr0-YtQ@72qywhwULgkF=dJ?iNiOZdyvC^Clg)JaKJx6o`p0E2
zRE%e4#k6CZ9m!CYZSw?tVXN16l}w5Ka#Vw>ZTryP89zdm`fOKwj>mn(S+UTNOXBrC
z>9;4czdeT}S`BTVS!Tx92}ZyqSa5N^mdnA8zq^>;mTrlL16MAQx6!rVY1Z^-DH-?R
zVeRzaC}rK>7c#&B4|k38)G0ahxhH$uN}Wr1#s#{R8fusb2A2X7HWzW?hr?<E_2T7W
zo%Q%&bWYA~=o&bG<dtjQOMwDX50XjhH;JYJiCwh-6YXm*Yud^YbcK8>Ni5Y4iMeN$
z)AM1WGdXieL=hgK7d0r_md<3e+(We-`vk3zs!#L<m7ABXu?)h@rb;^1y;1q6+K%Lu
zzchqMzbaj0yqSwrmXVnz+h+p-D-3%oAwIF#nbsNAlT|j|M7NheIpu0~bCq4OJyf=y
z56++fC?C*KP~N{M5;%&c+gG*jDe|}#mRGv3;u<;5J9JGhlvpbtp^^5dGJFQ;kNLa1
zOrwNe%Fu$p-d*CmV?B93z0(NbwBKiU-PxTLz9P-lBOgq~xJq(9<7HqM@CTuKqIz7%
zzX;d#p19u5Q7_Q!i>h`zUAu#fX<Dkz?hK46IY`2N%{!fe$&Ze#w~aIngBI$JPEcL5
zmSYjg*L^rpW#J*Ev31?O7^vCoRFulamPk=nAubin8?^niQG`7(CC}oDn%;ju8TmWh
z3+}!x9BU$0VTLrjm(v#VN9-tMP7}*)Sbw;zcdOA*Wjvzvtzz8e@=W$09M}%1oi4B7
zKk*lar3WmFwgJewxZ(Kgb`V|iRkUCU_UiKl?aFFnRlnhvX*SOOH&m8`@xSoH|5qxj
zEdOr^^B-w-5}g10th#>*b+k;Z3<Qjf3@ilfZ2!Nr>i#Q~{l{Vd4^Z}hfK;ddMo9mI
zhyHg$%Jg5p@IN!&7+E>~z4bo{DLXSG>wlj9D<S1zW@G>VOh{WHST;4$+31ABPXkVc
z9=APjx?ZL+U<^ekPlcoLx{&`kAqwB*zhDJG&UHWrgx$+HINp40zx?Q4d@NV8K5);_
zo$z?_e@s^D7cR`5BsGPu`6ubggOSst$6^TRDD5PrLi7#{%nS_-giei`0f`C-_#UiA
zi4wSSV1T23(8IgC#?gX=Q?3%l#bhv02uy*3>>fefLWSQ!Mc&;4y}G#x|F{ITMFOK(
z@%B9f$<X;>!9WEVGLH{zdTsW_$U+|S`SOFP`BehxC@AR1?Cb%ltHyC-m_h$CTPMmA
zs2;`jZ$|LrgZWn*<Mo{!AkzQa-wyfU`1$^Rzur06v2PQ%NR1OnM=uCj03HmY%`@Qg
zU+E-hqm{OxZ>haLGYCvp?`;pvI|D>eY85x69~gli0VI@Ih?~Hb;5E?UHIVmTSifd$
z$oJHSVO=;JKmH-$TZg;v#A~_78ezTD8$C?|gyVA)=qF$yTwpjoNXXo(!oGhi6LvsW
zdCxsK)`WH#Oxw!~XfSn6LR_DG?X&{_Vlwq2jb20XaZce}gnHX{^+xGqHhbm?CMqGD
zQbXI?19Blbo@*W`{k8-JjJCX6e^^$2K|KV#e|0$k30>j(%+<cw9k2xj06hGZQNLda
z2rqy1VhF$l;>482#pBR{JfMKwL9<(Zt@U;fz~8MuVTn1xUfcRMf#Afka)D|=A>aJ3
zAX=WlfDx|oa)Ek&*gidjrzW6lAwUs<xZ{X1BEA5Z;uwdwD_Cp3uC?sFSrC-`IzTfu
zEBtaP;=yYJxd?!t)*l<jmh|qdkOagJ{P7=*qeH!IAbwds1)!2r1bDyHR8>d<8X8E*
zSJxb0s$2GtozfWE*O{|#>H?}oU~q)5NU(YjU*x;b0+<wj!vUz*7dQ=u!W==Ml%LU^
z0I@iTFt39zU%Y@F!XMxDpX8$-s-GX;_yh>B`;F=Q)PbK7n+vFr*UvkF^EFWaELa%~
zhd;SL7Zxz@mbt8g&wB7HKTYLCR`JY4(9Xi=S&6ECkz%{_I54Upo`ILny^y&Jx*G2t
zn4`P|dKBUi;1ECUu5k)HJU@m)F?jzH99nwgX+Nw%2+1#YKB)fqGkoY0kCIb>)jGSj
zJoSlE0kt84z1|U-F5w-1Y3>8r&{4qn!vGU-f&<~`U9a|FP0YaWii-Aiep{f=Kchzp
z?~N+@?a_@sb9@Cp+}{GXUTs?Y6F`mv-{~^^;J@yeyZwr1s0Ypc5fCTZ9NpD*{DL1I
zf!=lbbgl^aR|&=&r}#SJ=M+M4_5)7^v>Or_@7>`16a7^|AOP>`YIMCbE~0XA^K=6Z
z1$GY(T!((FCveAb;;L_*x>TQ}la*<RL0y5X@VnSMJ^->fw&pX7g`pv-1Tem~s>+8a
zv64;?4aj&XG2(8=d1-b7Aa`aJCxi+pTxzCrA5Y@H+rMuk3LC-~*kvKe#%hp%W|T@r
z+Zy`pWO1&!UB9>4vHv`8RP|@Vy*8(x^7W$1d&vr<iiW!pW^UBQVi`(~)J9;iH4%gn
zJFw5(GRU)LwF*W9k7$oJ4JG{Cs<X#V;Cz3&GnDkuXa5umbd^Y!GG(QG#@AqJUyIKJ
zaY`7(Xb|HBOUT}8b<?o==X8#;W|?D7iMqX=NDKN}?gRCRbrqy|-3ncq7Qo^)G4yec
zZC)QCQ(Z(`1>$eE#%3nuXK|D-U>;&rTq;b5GIy)o3Eg{KC(Z*gSa9=gnNRmhO43WS
z%A;c&Fam<_HLYd%x*Of&lSd~^)2V(VV!qyM$cjV!Ibhq@>vW@yhaxD}BUx4uS;%+P
zdBSk-?|b@k#&7KpV#D-kUSL^e+r+hpG33Z6ONWzXFrQh!*^;tSM&<btFdHnU2r5<h
zfxKumiJplD{9c@x=&D&*&!e#+Bj)0lz_%TY1=tF_&IuS|lL}q<m#sRk<zj;H7^D_3
zowr2V28rrQgs?~yM}Xb&e6YBC$qBv?<m8%s<=MQzJ5_*jzZlx^(NI5i!ZDL)BGH<#
z1_^+)>hte;h)p|jy{fs{aLQdTGt6eRKBkNO8+8EecUor7dydmbmqjxK^A)*DKv$}C
zV-;I6pGFjnon-JH8<|UNJS!G;vv>H(nz2>Tuyf(51C&_?p_@m2ugSxiSZ-RD%(yN4
z9d7WP%{V^%2P}OE>1q)#O`4Qyo|djm-;_9i0U`1|d^#$8KS@pzkSX+haKwxHL$&UR
zgxOh>UkJS(=W0>N!ojX$BbYyl%m;WohNKmn^{-PKw;Vj+mCp>)vMO&{fOhvnH|8Ud
z#shM=KBaV9Wg3IQy^Zv<S;c0aTWF&Jic~>nY=*)aP?bqpA?%1~X~Q!yXlbVuc$S;D
zRhkd1{FPR8fo1=Owf~H0xkP@fb;E0`2Z9&d;60rjT};b4#S3G~;w%(=6Sk3=+?+2X
zN%a}OS*opg4KNFY{BzE`?jnWprVkY@PQ+e3;J)OZMlcj~?{kwrsDgW^wBX=+ZfTzB
zxlR7UD%e&(aV6lj6ymd3sw$ADZ7cPZju2Y}FQC+*vQ$fWnUT3Nz9?_@hwGE)gBy>a
zmgO)^!@J4pIa^FnJSG~saYU;E(m`f;Sly%7w;JD{YHYoUo-$=9L8&Va)VuDV5Tph>
ztoT9$8qIa>HZmQl7PCek_ekm;rca7RNR`a*u?Er?0*UqxwqO+=(h^YzBZ&y&O5d)+
z{DFY(eK8>mw6&yd!uFCJuXC@tFC6V(<W>%yn3$F^o51t&IG--)MDyq_aB<e^OH#Ph
zi}&l&HNiCd2A)fVsJiHAyT5!(UuM8EPV;s_zx0K0moy;wpYG}zIffjw3K$p4n?HrF
zL1$j>kdNP{_)p9;STLF%jGvkR{uxLNCrNQmJ6P0-^@4ho&N@0ZEiG_mqM1wLtJq4_
zk#RB1ghqZI{17L5lL$q}tF4X#&?uEAlbAVA0<es0)v5Pqk>GM~=V)Uft(Q$;D+as=
zh<O@Vr@~%71=E;h_x>j2+*s}}D1~bGgh^2U`BhTH93BnQGdaoHwiw^l=2ubu9jwIx
z0B!gzo4MC$diqzD)54S@fE9m;MG<_fbS!^g<#`^z0z$W*U=-Sd%H$a0S2#}!Ibw>N
zW-!ULmI8eqD$T~DUs+;y9O;Asfvy^T@W9QCR=bQ)5epM7U6)xxo)Sb9xA+<@>c*_3
zw(5Bu)Wk}?Ov4p8wN0S8skFGnE(V?0R8iP>_5S#_f5%<6f~P|9uCQ7kK%V}`At49=
z#ghkMf?ckoVm`1WTR#QJX(u6SIv7UTgM+_{$Uec39ZnV2jmR7O-(7Dd0b7r3<q83o
zFHE{8-;)n4l7n--oDG70D0X<Kf910HYAsQbz^<)&88)^u@*%97Yo-{xHxu(t_y>Bp
z8HA38l9+NJLXvKqi+Q+crs|zu#VkjVoq|@*I>DVeZPDR%%H?*NxuCb3dD81BTc$KG
zto7`1$`PLj90*~EY1AYf9+1iAF+AGi#$8JLjAk-Zx>TZ@yd66>XV=tsvhW&)@t-=z
zqi@N|tf$RJ#uM>o<AziXo78nY!&eGj*+<A7yGC55?>t($C_1pQ@50>Y*(p5(WAqID
z5~gud9G<3+-)oKGsbT6UKBb-QMNU8E(sTpH;j9Aq@oEToBWG9TDL&?jc%NN15~i{q
za33#dJ;wt4iuuaC4XZ@l*Ixd~7OZ0bmK-^ZEg0Ryq8Mx_(q|mVEXZai5Z;S~f?uEK
z0hZF@*T@Y)86kW(U3PO!)$CVX${DGv@zA9*8_s24i-B$VuA}uF-1<Z8Gvd90RG!|B
zyI9K+NZc_!(08t|!Ba4JNx$PRY?|YgLykT=NuK3mbRP>z-^agc@)%-^*+uH`Fkbey
zKr|9ee!j)7#~m%^y$3%X8!b`gRbB&Pytb-Kmgm^I>g9oC?|`onO?0Y_d2PFC)c>ej
zL3RANuuh0J%w4u5n;p%wBkUnKLr5Nobx&NoFNY@ZC!c+#P5ct=mtg@OqREc+RJ82x
zN%(M}Ut*(qm9>0K3w~o4queYwL*E_zsT(hFJ6%SQ(J%I0a-!m{#kvEhu6MCZ_Ue{!
zAuf9J<_4KpV6JvZw2d)9xP--Dp~>~BMjldmfdZq7r2oe{1@0Wwquit&%lap%qW)sR
z7yB}<J+KD(hkAUowN~ekh+}O^2%{W%w$sk<ob0HR82g@F8pb^lwU%s;I_pt;Cx4oc
zl@@$5xbK_que?>KRC?xjpH^jp$KOx*Z+3;5(uR9u?w$S{@-z=P&8_;pgsU&5d8Iif
z@8!L7*0nhLK(BzpMb$~UaBrWtx$f9`C^ry7Vd*+g&AtZGV>xl;)iyLIx|oDZEIgya
zAe;cxzmV!!<=ENLWajTCI(__0<*IFE;g@=IpbXJuDT3&ZaY~H0s?n#VU_PCm!{=!7
z0mqL)jsDv&E4=@)*yV)F-(xqSf6zoU484~T8RNyd-K85?ppWu&X?1)3K9Bl>IOu}2
zl5jT%jP6}0^T#s%Npnk-;V;2Zh~|pBF3uDK85z&-zq6qeBrJH*79N+h7cI2qAc3Dk
zN&AkTI!~P-sSy!#W+yZLsK3;b<neUt9gbQSD;nhrunu0VC!(BPF}(*&@G812bJ!^h
zDNc{YO5blET{@53`5#EiXL#Nu2%Ss&k%!<3C2VZ|N?M^7?O3KT^YpY^&aB1?1?Ry-
z|1O!s5f)l2r+}9p>~^)1f^9SouZyO&l;_Tk=^{8olrgB6aX>RIc_ubL3U}+}YNN<J
zTve)?ZbrAt-;RDCEA?M<?YneRPVtC;0)9O8+Sk9T6x4*$mow=nkG@0YY5J-m?@)o-
zoyUIIH)L!boG2pewrRJGO@Sq@{%lZ~&(ZrtV1e3jO=F7M_AE!nv9ssBqIXBn4F2R@
zpe?<sSqQ8Hok99i`GbsJTbDi=A~SV;N`^hCFm<Ohzta{V7bovSA<XgdK_ffA(pdif
z=uQ<NP_1oKq$oL_cZ`@5z!Ic0Z6U=%h0dFd4mlDY(1|QZeu`>lIK6arC{7EaLawCU
zkb>rA>fK4sjXSDaUo2I>K`CbF^iSPZQ?APmX6aK$;vQDSlCOK1pMmZg$I^}a$rnEx
zwJ&Z^*)7sKo)&%fw=(}VXYsM%iabL6+DNTEp#<`jHphY({5w&s)7BXW&)4ntILT19
zhhFZ~stY#zJlRt}+<pJ#C5KJdZvEWSr<;KL0_L$>-lTP+0ET)!a~EpV7SER>V_r?N
znPo1IB_$oCvU*GsCf-ZyZQDQQ5|4I6+>;X>{G}OP)HW$c96EBc|BrMEJzZ^?cClEb
z#1bp-%Y!8)a=YM)@@$KKx;-Ri&K>SoV|?fM^pR9NJe^TP_$d-!DyIfsYcpk<r%2J2
zx7N^?t^%Tlvq)DwLbofJ3|)PB3Sy;EM(V3S?E5cG*J5+%S&xV~bPSC2y9VhZD;4`i
z(D2C>0Zf$7rjZrtXrXT;>I3iKGjv=QC6v%5VOLM+qf1JG&hcFE@J~~|$!Z9@aOr_9
zJq32I3%Et3t>&Q{?8zaGwQ*-%)j8(ETlo*kk@hUwi?H01PclV|x9)jps8GQKjntW&
zrVT;+GTqmI0l>`0@%g$4`WxYA7+8*!PkTK<RU*3iG{6C}1<K1E_6Mk)%Ng{v(1CXA
zl*r=GSGsSMWbrWQPlIm1=;LZkkQ^A}Fsf|06$l)ry|pp4v@mFXJy(cd<AdU6+}D&t
zY8>5QmyBgaWqlv|y60SrLeEusBv*DMV><8HG$jRXTs`W1ol#Q-9+u!rDiOU@*_s;v
zPUOn_7;ML9xPj7II`~ZU8X`5}c4Q3DmRp|c>n}hqbpGJ1>0xY$-{xWc&D09PD=02M
zJ=~sUXDE?+*Ot(593#z<#j1*1TwRT-_0FTOUQ@5H^+;VpLXl^=P&Br8tRGP0#tz`L
zn)qAWlkOb*R^EEQgu~c$T3RLh^yh_j0Cj+SObkW$58tu)&Dz8*sQPc3w+YJm+h>z9
z69tSl7Et9<4+<vNNQa@LE8QBCT==$*rbN{TK8NlxC89!t(<m^$P$gnT_~dl@<wM$e
zjZL*1)%qO6&11C%6U2Z2DwFjry2j{B{ELfLebqIv1K#U5V~g^vUW7gMV>Q9^x%%KW
zPfW9Sw^Ag*EltwOy61<(wT|7DG!yJm5;Y3%Hg07?JLP<Mc+)y_Y}V_*NMtF;WqK04
zBOn_}iYDfZ;oR+d5#8hljDL@$!-7{kzkEn+n5F8wNV&s{h*Vylm&(hans99$zBDSv
z{d5ymPyKR|&y>+P@gSb5!HeeM&8eWE$wg7-ZOGI3l+LDNol-E9Ni>DpA>NOO7^cyu
zACi|&oU<$d=1b}9TdKIt+mSE{BV23*Pue*yZna+m24u9P&s?uL(Lnc_>hZ3!(QQ{I
zPt1Wbe%qihp9qsf0+I6s{j9Wn=%N2EoEDnF@G~J0(AADSv(z=+An+tN?8BMCJn=b?
zf1sOWo}>1Sz)XhjD|{-Ed7DK5x@9DT!z&%bb|$DC{@b1>b-KK~)d??KP@lx_LZ3M6
z5$I}j*5Sa~Ea5Oo;_+%AlccY``jp^>Kqe?D*61(ChDU8SkH!cy^`?t3eo>{T3|~;&
zUVRP6)B9u2(AtpKHbSx}!?}-Hw=7d41BidD$93K%b@eqxtsB=<fN1AjV6sn(%BO_{
zKWo)SgZJU<d&g&k#+hu>tjaYBsRyA@wFO?8HOwOOGpX}dIs=}=`(Ank<=EG*?d{gl
zfV^#5#?kqRL%TwaCE?YK7BFL*VAxl@fMEXtNI_G4y~Z<pkVv#GRY!GMaDgT49-+GJ
zKjiA&%pU5Q$OwCHaP+Gtc?)+1hSB3Um>A|h@w{RBXT`w<cR^iFk&g=9P8#}{%+A16
zP5<oUrk1`vnNU(tbq$bRj|_@H53Y%3jOO!Mehe1BPc<(<9Pun-1oeZf`u^2F#~otL
z2wYmK|7{FynbvXC#TMI^^o~a-)eVUD`_2)1syDTl6Rpl&-Yfd??cvthpg|fK>q&eh
z?8DfbJ9x`@<8_d!juYHg5vXCZ`Q$O`>-?Lo{Tx^q3VcP52c_RJq2LxoHa)<`LqxdE
z=2%jr<%)vm$nVlWBKvZlLnvS<UK(MJIne~SOrrE)4fJo3HRG9$@VFO9NO#^dM=kzL
zy4?0s=c!b1bFFf1<Xy6qf}lG0d6)#M<7o%+xxqY+SAxy&fdp6lCcbd9-pg$MKK}hX
zWMT0=Bfh-C*A%v3JvNCZaztZQlxoi9_1@m`##q$5iw2%>XKSA^T*2?!A8bLJixG|l
zB2Or}dkm#TJD7#W2Tcw7H0--I8#3);gPLh6eNyw)gK!;FT(7yNM9KHdmzXh!IeNp9
z4>YHk{prD@N~eRSiG(*_r}{D8!y4Epzvg^Kmg>^ygR8A0bxT7(8*4tj1rr9Rs?{Jc
zyNHBShZi(!i{*d-hoX}mc$h00)s>wBLJqrifrco$X6|b*yM~Q+RsF6Jaz<y)l;s&8
zz%G#BlD3XDU%V2sM0JX<W-+bOXlZ&WR()q=Bt<pgWe^e;(3ER-+^h*W=bU3vB*HvK
z?8G*8RoFUYMoV@Oho-OuLy*>aX3M%GCc>h+*0|4}7>M-zJd-+ao{m8}u6+OxnUpYL
z$<Mx*KadU=w@^RP59p^Op>7$I!vJOcn?NJavF#T70qCpP-3}F@YD1Dd54ylcd^j}>
zl_iDGoC+et=NGd-Qp<6eKVe}bju;WbR`s0RRP$z}QE#t1ENXhIUq(};LmRRyqixM^
zKs;VD<i#u+MV2Y)p2TP6{A1h1`805MRz)Ef9caKG*^1qDcvTB}ORU-)$@FQb;$y_`
z#{2I79Bcc*COn`<Y9oQ^BST$O{>SLv{G9#XY*S~6UK?Lug$DM*SI@2DBhf@s=b&<z
zr7nDtd*GrkHOAolL{esA?NqvT=5&A!C9!C@{|kH2`ugBy&+Xf&vrxN|iGd$%T3^iv
z7C29Ifw}}+k)ubgtQYbjoKMuvV<y6V^@1$HBkfI%-@{Z-4xshD9^?Jd<<fe@Q#k$s
z-4M4j)*d&;?H(Ze%NfgB`u+tGZjB~}NhbwvNHm>z>)6wO=Go9a+2?XzZExb<v)i4a
zwmAEWQ(I)I*L4ONV&lbPA=muPBFJ>_fw+VGWbI|L!P}cG!36U<&~3-dAstn^WhIff
zR{zYC?jV&Q{MwLa$sMPS&^cMH>58~v$#uo5)g$=ngFYA!^$s{!cfBqsHY?i>Gbm<^
zyZZ=3yO?HwU`3fm_T%#du+Gc&Oe5buBHwb#3__pA`!6C|;<S+O?&q+~Q*e3VdEMgK
zdfae%O<J}`^y>B4;;N0B52S1Un#=2^FWZ}DU3`nLl9`@!WNfgh8T%`9dDH4W_9)J(
z<G@k(?l=SHFF+l7DE9P5>^XmPQ%ak~9?O`)s9jK6yXIAinSggd0rD%dN=Nj3S7;g6
zhP}&w;wZS<;eEvG32zlMBA293>UlfZ!YzF5H}kLMf{f#Y8%*8iD<bC|g)Cgv3!$+4
ztdD8n--jsOX*q`!U=xCtGZ<}QOWw4Nct>KP*19#iaa7m(4wZpXuf@MfS;zDSD`PnV
zm;69Qb)kleDPhoGi!%yC+=%(%$QBM<&#_uMp_aPe_>w7li?2Q!f}vNP&S9j>7>wI>
z)H478f*AhQ#7%TENk;E1bypuwx6TnlYDioaZG{Ngqv`l=yy$7Ip{B-Q$0c+7o$;CG
z{C&dF6dUCnYUq162*1)R;kx;!v2>rq&c1cn<29$7SH(P)KLtohVR&94<$ulRD6L6*
zy^<OXRoND~1UH6giutS$j=-}R2xKEmG~bQ9LmjIV1l7GUw?6kBo!s`%Xq$93@Xri-
zt0YS*FhNr8N%h_3+*<PuNg3tXU32RU``~Drk_?Nr)&4wC(QvJG59ydUBSQ|F<kpP3
zo%%{&3KtY#gp-a)>)#<&xm+S1l*+#hbAM-3v8qU~A&AwxUV?9tD;#Nyk)~2u?+wsz
z=25Fjt2k~+Swq4(b9}V8#8jp9Ue*8(#)J_JB7PT2Qc<AB=eGqSu6wjuoO~KDwk2CN
zdD#2Ulr1;g2c8y7*tNoO@^-iWU#z`jkZfVvu35Hi+qP}nwr$%scDZVoZSAsc?y_x8
zb@!a_bkBSz&WVXxzt(y)GcqzWGU8ov-Pdz#M?j6;)N(W9))bt|r&-C<H%bJnDqjjq
zSjkkvr-B_guP{gJyiaIgP@QI1iFl=n9w`8bQBzioakXpODgiY+U(Z{tkZy|)?&GW#
z*m{sgnwt`jqZ~L&5-l(xnKxfkZwffp9A@D)yh*9i`|a{UCx=nukRRo?cZ&FOyiV4o
ziVFJ1zV3&Tl`6vzZCKK(iC0L4Zb+h4UVn`JzAX658+J`H+7luUpg|sWpdT6QgBzUP
z9xS>>hoEY*voo4WsU3mGyq-Wk%tv(n3<5|g5*vDN3=btjY;!&rf7;cfXw%hQ*eHU^
zzBOapJf_&W@itk{S^K@t<p!9d2_7Sj>obn2%wsg9gsQ??qkA7)I_S-M19)ZUK1rSp
z4|;&BJi*G(nA=YUstWZ-rDQ6!{xfZ=6WMnwYu!uyU~CsS<<Zb^YEkN5eXm}e<Qc$u
zBjO#RZ78HfS?O2hxnwj$1y~~bk{xN3*>!iVF+rFJul_r>)#YpyeKL?|4S6=7`#AYA
zx+&R*`fn5JScV~PWzN@zqZ5ogggbb#+764UDy1Pk*ESUX?c0V;kIYR{t2Z0!x7`t3
zYd<JnQiKC%F#cSp4LnaBH+-qH$^)em-blGe2))x6(FLiylH_owQDchg_25Nw_W-FF
zoxR}x8aMS#x)r#G-x58xCHWn}H5yJwF(C!KQUC>6%NO9BN0B4?8Lg4!A&(>7kUjlE
zVq$3BVJ7!JpNk}tW`lO0*up3rI5c~HPnk+|q2*XS2E=QxXUS9wYi$`NC6V%K-%*Ue
z(oLD`2N;_U)LhdQ9M|kR4oaVm7sAB)wp>6fmrTG)$AdW@&*u6urWou0;#}ZU%>C55
z!;&BwKekF5!v=(_IE%9xZ<^W!W<`lFhYD|L;%qncx%ekbtU7GJL}t5?86OIgMH$Tk
zChzFfZlK9mk%^tg0(0gnH_`xroHMFg(6w23FGB%1mD_PBW$IiV>{)>NM>Uhs;6uaX
znEEw!t%=Szq|=A$L(jKuVG<4DK_$(wFvja4y&0T#;gNVcZ8y!jy*sF6DVrc0`-#3X
z4{+FF;QnrKUzlGa<r-^^Z7Fy@{h0sb!)zA6u(lkIy$7leobvv9Ayic$U8Yaei%wQ!
z=q^RWW$T{j#@cP0oyX+Ue)dv)Z;?}<W)I$#Mflu3Eh0~)C$OVP{hq)m=VWh)vT(Vd
zC=8{&A^+jw1{N&Z{Ck2t1CA61n4J-4o;!Fk*bL}){B=N%N(8Wz@dFdHUL8oUCI69M
zLoTlg9L8*RzdX;jxQFaXZ-CsRKAvijLAQ=33zbX0ht8<X_c>SoblTstZ&maVKtsKU
zMxxx;T<1EcjgT&*hWmTgj1Chcn2K8PP4_^O%QCv7chSjYZ@I-dMyo&U2;}Wy*+4Us
zvv(81`m&zOCxvc81KLZgFxIm3SX;vC`b>+Yw8(K%gOy!ebm*8__@rava(8O8ce5&`
z;dM$Ut`w;{OvX?;&^k{d_bL49<b34?b>Jvi^5dB-7p!)(kq$0+luT2vNOH&Okf*-(
z*N)wJa*umGwF`lBJh}FvZnk*+JPTgb&AX>IMmU}nAaA;r(Xy%M!8vAq-w5a;4*dci
z9wlNb8GTL$*N~l&@2Q2?$xVxL=O~0|EDL7(M~92~4V*F?Ye!{za5XbCqp8aNc=w}%
zTyjo>(5-_Ov~wS_v!9MWZe_h(nRSrKa=!fYD^PdJ8q2zjQ=D3Wj|iyN0w|;lFIrHG
z9#=41U?H_E4Z$Dx6LN4?9g@I>Etw>qXe5|YfGyETecqf{@;=h9s-#*$|CDKj&m0At
z%MRmX8nGaf84c^_RjHKy?)uhA>nQq|)Dt##fE?VGyp>PF1(Ki^U&(G}%2O;01urgW
zSIUe0CtaeS(iXRMC)A#&7%}Q@&@Q}$TPsZ|qS!*Vlm*@{=caF9rAINL|1Jpf6MXnD
zk^Fxb1kq9!k`Yt;M-W8we`V)0{RBZc+5Qm(VPqp<XJ`GNc0N1nk7X}x=<t7g_G}!?
zQ1l|E&c>#8E(ElUoIkk?Nf$#KOJhMha~sp2m7j%w+4(I0Zs#-qboBkt)<Q!kCwupQ
z8T5wEKPJ1Sv$d_Eiv_)rlcBNcKOOsjp8e|?My@tCrY`h9>$X4dNpJq+xc|Jw)X~+@
zhTg{1+4)~*&X%^8Hil0BnJ)Q9mgGOvB`p8;wf$GRgoWwf8U)$?S-1GF$NoKC!p_LR
z_J5o%as4R})ZSoI+M}2!D1g~waFKGCa6e&S?n7W6n4I0BB$y|Vl8}UokdO=}pd}1~
zkSKR4+bKWpI_AFqu6_JI*R;+v-)XP))nCDbLjyJjB&C?gD~%H|01W{ZWD-E-HI}9%
zfFPlQ0Ez_Q;E>tXP+!$I0|>{2i47!1WcsES0EGoMc4Sj!KtnGJ7XfJI)d~DlC}0SQ
zX()+_06_r+B>9aSAu<7=+Mnk@p&*Y?9xKwv004(dG7b*w?(%1E{o@nzAnXPpw4{Xe
z)xQilb<@Ct10)9EEU;dVfn7%&VinaQCZx|$Bi}!DH_iEvks_0jke;4ikO6xP0n#PK
z%mDmDka2E+7y}H<C1fy=KOopPaA*G)0WydKfNaig2EUteG4L{UOc<aem^ueYSl=!a
z&(BBztO#p3mgRZSvm%2C{{9YspqD`ZTv!DSg%|$AukkNch`5(rm@t6^I|_zB;BgKh
zTz$HR0=}-gf>rEk_z+?O{-8m+j1oG0tH?0!pd0%@e%ClKD(XxiL5=|5LE(YF{!-A0
zAppC7f-0V<V{TGb68@r+U}whwiU#~x&!@tH4c?905<i(S(M6D;582U&2Il}eyMYE-
z1SU0!3h4CiYf|}!>dyrDUd1ug_{AZiB_t$+1>p?-r;AWdsCPX3<(ENk?w{UC4iG>;
z3KFQ30AN7q0Pa2q`T%^e3rJw2IlQ``V9DjnGZX<0OvoQXjbFTzh;aZP%DCoXoFAI|
zVew(!z}12rSCGJhy?Nh0YkMc4qC`5rUy$Ft`h>Q$xTes$J^T^h`^su!UcjFqB7y>d
zmX40U>grNB6qJ;(FmA4yzBvPW`@dVT0{;5j!ac}~(uM#dQG9{}U)_8S|9wTLZTn7w
zfxp?oy&`nf2?K2ZF6e=kzyaz1g!uM{-gO`Uj(zU^{n1<gy@Rdv?BM!&dGvMp&A(~D
z-c~uI56D5~ZQ$52E4()t_;Rh_yoyhG-v7%1zV-Xb%CMiqlY$V(5d|Fq4H+cz^Ji~x
z=3tMp2oM*_@kuzpQ`Gn95K3$SCIR!Y5*s`L6!<j}u8cbf>RxH!0Q`O#^jGBZy2lFQ
zXL^FPFVP;700S266JUS?337jZpF9S+GR5ROdvBmn5CjQYI?$jY3;-9wT!6mu(3Xu5
z0teO}RPaX@##8!pt1t+~!O`ES&?1LezqPL=)V)BleU{g3oHp9e?k``!K7(xrh#v<Z
zG7;9@4|Cf`r+V;&^yZQG_L#n!Q8I07Zck&n!rV`;L%oq@crz7WfZ7hW7+WS?EKcr?
z`OXdx-gBerIApTSl5c3Q!j+!V9{S)h|Du`u&y_$TOx!@yDWf!sc07#z&#fX^(~*}A
zpC=yS8exZ4yOPITlxB0%7n}<)8g5c!F{2Sa)nSX@NPMG-;dhX)xutNPKxE7G4Z|E0
zZckr)FgXq-`^ZB63ymhZNl$k6`NZj|9QxMO%4t<nc|>tx+0&BQ7_P)sNttI#(N;kJ
zz6u(@3Z<;9QHlQ*+8C!iz}3~yKE%$aGY)j&SjV^R(L%plFgH{oajwJFOGX<2%b<7b
z(@E|-OB$lY;a<!IkL1<GPTD)SGw-F6=$9%{f3LnwEkl8)z2Ibj%xR)eEfglCC0_8w
zNWa*1shfEb8mYKNQz3x1Lgr-jSCl;D8ly~)RIYg5HY@m`$&yN<Ba|2#L-OcvA1Jxu
zQuy79hr3L+<19h1y==Y)aJ8W4r|xroh2l)n)A54MtqeE<y<6*C24Nxm<)Mt}1;4rL
z#z_xq$dEU{JqWgagKRj%^^i*?b=6})u)lQ!dv{L<dCh3k=K4F&C>xux_76GE(|&og
zb-=EfSgSZ~d2X%ZClef%s<`rgzh6wbE_36jKmc=H+^*4;K!Z?vUgKQk^x^%xqMxeY
zdJg>#dlC>#)Z&^P&ob=4JbBjM-}82*MJL)q<B2}bOKXE8xvYJHaJW+_qP2_KQX`$O
z41MwUdVN%UG**GJK;P)Xj;-A|x{ndek2W_?At~XbNTFP%kWSOmlh;xH)FN<Vct4BX
zujP#kOdWr{Pd1C6$22GEN3(<OWIo*^!VyM|5U)1Wqd+o~OS442SnW_sSM#<Y&fFt(
zni0#bzswtP76YOuzGQ-wn@pR)Ug6u-QzvmUi!2w|(YU!Q?;Z9EpBVvG6}W5G`hNtR
zLDuBxEp<8j6$GdOtb>vbm29`x6xv%Bjlw8DPT4(!n6zTpXE1FnhZ0dtzO<K5^?iE&
zs>3Zi_+zK2y+&51RU14ZVs@f6F@W4kbBnRFXMr9}=oBWzm`P9Rf)buIkcb2xL{gwV
zjzr56hCKHme9Rm|=wyDSpRqn>+>R7i-qQx!4da&j0La;};s>3$#Bzzu<9J!Ndnf@M
z8GCCAt?f+dt+^-78MR&4?9Je4Op#0Uf(r~Or5w0LUiH*BOVB2rvUZCTH4XcnXyPC)
zMkIZ0uC}3i8UatGYc?rJm0ul%BoHKbqCKs(ry`+#xLaoF%Oc_@&@eDNqHBk&Iq~)C
zayXZC?t5jxk56IaGdO)JA^!nU5E!^vpNZ0o<Rhbh&}%N6Z^qDy-m-2|TEFNV%EY4o
zPAnRmvtl(0C&U%LqDhY-Y(eXOzg$hHt$o2&8bB~JyFYjx=_asdoYcoNewRs3Lt0sm
zUisI{ba1cqn)l`Qz<4)Ty$V-Y1~nIK$S{a47CJzji`*k@NDS%ZTrJ;dZQ2{W=lv-s
z%jK$ZRP^?3cAZN8xciO|8z5C<(~``=yw%Fre=-MpjXaZ<u7X<TL+Q*@mYG93gB`J#
z7xMFw$citA`T;snH+t}(?zK-ro9Ym(BDQ2Bt|ak~bABle8tgBQ6k__!z3}Oaz|Vu3
zz2LX>Wss~FCSfcX*Fb@dd%BO^7J@D)-M6aqchiJT@BPQy%3?Lb&>mv;km`7op)_Fy
zH|xje76o(DtMT6gQcN|c8T*=8HQ|c)*NwpxG)eLucpZzc=rtC*h*+gh7>l5_{$1r)
zO2#PBc8$^`)|%EhQxV#2?+f|H(^fghf>k`l%%!vZ!rs-P$32zmD4neLE7d%W;$$eQ
zBS@`9xa9z~o!5R|Zy`5129}ZRj<@Hq9KVIsVPQ|0lpu87<ZlOCVr=+j+T+-IJX1^?
zM$nd7L^b{9fwbgsP1+o2sumhQ$N#!-B>rwD8MveT3?F5xJ0TKeOZA*S58R&V{5>O8
z_QI*pRsS#<k~%ynAJ1UZ_T2=$i~xvn?m8`znwEzQDwPE*BjZv=S>}bG7!vo0d{z-9
z5yO;Ai_Y|-Ld>i`aUcd$FEXTD_nQvTv|@AB?YV)g?Iw$9N)^C`jSpjbgP1NDP|Z|O
z3d>vC(Is{|Urs2^pN3wf6dN~;L0#3Z*KOlS#^<$R<jGqF@edEpgBpFY!D~B*`MoUU
za&*3O{4%_185R7DfT;LF5o;Q^Jms2q>AN|#v%9M0VYgY#w6Irjbzz^qQ$??oD{V8j
zfs~o4N8G+<euZRLx`FrQlH%Sfjz`V}9s{*wO{gIaq3d}XlV>IJ_K@6Uau>rg;;^sh
zmQ(hqv}1jv>f%_(1WQR-{UMgN$%fy{-&hiF2{Yh;vQ$xrzi!9_>pwqpL>Rcf(HMd7
z;OD8-J#N7Th~5xXD_f<pMQR2B=tWt{AtVaw8}8s^R3*Y|Jqc@Kc4M;ET1#zAn)k-b
zTN0NmGmc5kA+&J~<5_Fez*Jy|^Ct-UBAk<&31yk)8D&Tv&Xj~#jj-^Y_Vhd(uHKch
zeEzf|4=$$;pG&wWsW?M*0CVeJDyTWdHBJ5YF5{UAx?grD$rI_3rRm`PxJxD+bbNIE
zA^H-vz5e@hWh2uRTm3xWF%~N_>PuJ<HhweYjd`5T-Sy&nSU6LuXz#HeQ40DKOWOeD
zp4KQzXLKRzTw!;$b7I*f>v;*{@&Ll)FTmWw)Q$%ETn;1O7R{Ej^N&Dp+Xe9a)jTk@
zOtV?Wv*m&A#|BDWwB?UMH0spGKStwt&3IdIV!vnK@<R(EiBNNfv~xB^!b%=^#m)-o
zH+ihD%C1vpr=Xv^euGw;_7u0|cjyKG_4foxl!<)EWHb1c#V0imqD|9;ReH*Q*n`aO
zAenaTi*Y=E1!u~V!V~Cm1t5htcbUA(_TDX5lc0@B_Sk#bCibm6Vsrr40D@Z2h^noC
z%K~^im3Lco!9w0+NPEw-Se`+u%I%N87#tr2aV(ri;huSlkWp`voT1`U)fVO|s?kG5
zwDPBnBRbm6_i-wmUF=}}rBd-SLB3AH;@%xHHPQ3xK&rB6fxcsY{~49?gm&vNA9T*!
z9dR3NTvm5Ne&w4aRaw<7JVTYw`qqWNt!r{saJR&d0ey*9j+bk~rs&>X;r@Pn9T;tR
z6Q4LLTqrw*X6_}B_bSka6m1Kr8V^ez3qQ_~pMNXp8sV;Wfb%v1&2^sJ$G|ft_NKs)
zgn^IV@?(2*n7zIxf>47>qS5abBYUg$hM2DX67caiSpdVWLW|W)|4dHQodl}{(S#<K
z=j*XJ5*W&kIo!^|NxkZkB%=u!Ty|^G`TDjJF1OdLS9!T0cQ`e>Y@7JO5|bP#X<9G0
zuI#T|^jzu{(FRRtRwer84St!fPPZYA9vD{K-z_Q_-r+(tulpkRzHEBdYvj?D8(G(+
zN7J~})6p_=(AOX|@pdAwFl3!{421i6<m6+t!>moamb567+fyteaIm7OZ^F`xa@>9n
zL-e;~=^I>p7!V3RW&lJ|Fm%k2onFMvg(BmOr$G8(9tjK$I{dFn0<LmQ-zFElep4}U
zE3#?!$?$H$8*&fxMK)^KP{j%7f8jAh*6FixS>SR?)3l0`t(;(k?~x=Zks&SV_+Qb+
z^WnR&pjK?8r>S_F#6pnx?T8p^FG+K%DIf;RA>vG_)Uxx~R!-U#VEuNzkJ_F&&t}`*
zhUO6DL2L0s>#|}SUUl}gZNmrg?GW%&nQe88v8ypsJB6)G((R+(2RbnVu2mDxEOHjh
zy6&ap&2(Gw6uC#`>Ye`_?{KUHp$<ALH4vPv3e!DJ9@uubXxFaStI;^R9aX*^jmUMT
z$?DaK3lLuD>eS`U2=)^m8o=U9mvmmum7tNdCvlO@LFs#3nCZQo<2%wvM>5D0(&!W`
za$C_~pADdgocRvE7OOdvxJ+IalJZQnq%uTP!z2@13ej_6zeh2jVZ?U56T|4Bu&bH^
z@t+_tZh=c3oP`aT8Id84!ARbx_`a{X7D;abv&a}P;HD$^9tY6Fnp=bCFUhbSw1Dx<
zTv73h%3K#k18V@jBvOMpk=P$G_u?FSP=w4pUf*KO;;h3}p{|+t*4PO8M1D!RzkB{!
z?MEG+QQq1zeQzQgFTceHNJJnR9+6PW(MvUY5D?ctYf3T}o*C{j_p5$2cBl4N8(_4#
z<cbbIhL*|F$3#7f;H0f_8lEpK>rRn3CdH`jOZ+3r>Kjqiqb7+<|5^`m4$e1w>2;QK
zUQPb@E;CelghZ@z!pTI>cuM|Llh^w;hE9ep3^1L4p<<IRyJo&ZU)F#9rKesh`PS!-
zqVtKc<o8+Gx?SiB=8)?Q&st`78brqlTvc>gI=^+CW*o!n3)V*rN(g%hsWW?VUMOFt
zL>2(MxunvHhRe6c2hH2POcU940aD2r$%|QPu6#!f@cc;1%z5qwh31~r+~cU5OR;46
zqor~w#uOszf-e`_%V}s?$(=3q+8F+Gjx%qkuHC3e2F(;4%HGb0BsR6N#lkVabO*}k
z9E(ho6~hjK-%vh_lOQ*jzocwsD_#u$_3BR@9n_I!(XWFSx~A4U)yE^}&;Uc~LTlIX
zyDdvR&u8ImQa$cFeQhh7MP^Y~cm{|?2tS%Q2kHbTY7b9JB3fs#?FZ~yO8E6>tnG%y
z$x6M5a%s3+`3Y$xQ)K&jSaGG!jD}2d7~ygy3oRkkNBsKzqI@L(-=0cK%k+I=>t~oB
zikUUKVz9+Js>XyBy!+W-lxCT?jv3Mdlf!a<$8&_WKDbU3nISidQpnWldl!|04XK<>
zy6xrkF#3@(jsuP0{Q)VhNB)XV!a9u`K7a)PzJXPTE?d4y<<B<~Q@OA`ln-V(SBR=e
zF$4!FSCdzFaL%4Ft#W8E4Dua_8&8E131}Mq(WW}E<30D(bGmc6`PJK$lChROt-FN|
zZw!^ZIhH}pBR7o&cNLAHn+MMmz&<q&ggz}0g&Q}rNw8g$Xc1QU*z$2)R{s}0oDIX7
zv6ErJok^J;8MK@^HHe@-F?Y#wr}u!05hUppfozAEDGc_sR=7MrMC+c)tSUV9o(5#m
zbN$tHiwFe8#l7i}&(Z?`?@^C5IlP~SxaU|T>8;uMB)bBaR+Ox|0PZER2_o_Y@O3$R
z;{Z@3gx=!fX;Yh0!K{`}CbJwcrF#X*>pf;k46NqYPLhouvzQvvW(3O2{MGhxdy-g{
zGYw`sMK$TH^31u{_E4U^5|%$?4Q}Z`N8RN`vhzh`C*`?dMYx%gJaWkdDS^(Z*?9vH
zKQ)<`vOLIclam{6<<r4iR`*hYG>jx@ZMAoJth!VcT2U`E1(DA9FVLAKchrt)%%K}C
z<uWHz9CwMh<O5?0wre$gvrh-@QFA&B9Et6!h!c3E1P?+<_}p5~eVbXpV;4>LbLWU!
zQ7%}~7Fjqf+Z)RXh@|OtJksG?p`~%OLgDwbBvHx`Clws{PYfhfow||3_@ku#mWW=X
zjc0szf0J~N&)ZQeYPojNbtFE<W^x~0_Y<-(XQvO_0nzgYo0OTsP7iIex7M`DM4c59
z_%{%W9k0TNS}eSJAe%ludOkSYhxHm>wHgeWbk+FwU+Q{XRd&A4J0HR}JsG&~&#&4g
zRjk-+sN+-PVl?{*N*yp>5+`RdVebOliROYBTdEt8ed{$NSzW>N0ZnWhdPb=kS7Sbb
z5AyD0Qn6t<)%=@IkdBbWl;)C%Mx<K#5_~;GII%`ui1_~X`1dBAJ2Cs?bVJa6V=Cvq
zzKVN?!ITMPwP(ahLwld@K}aS&HiH=E+liWJS3UIsQ7^k@Y*R<fGa<9QI{_h7MB}$X
zR#W5^X4lX3!Rohi5p`RwJF$1r4L9v_#~86szCch!p)$om3oAu1{(1cezSWvGc>JRp
zNluK4lu~eChUhOkb(QUG*Eb`Ij9PVANJBkoSdGQ0BXjOwCJlqHLKVz>6E57%k=+5J
ziTr&LtRYqNwYG7?@jE5tLaoZh6?W3^vobR}ArTKzPYp_i!QZx-MAdXsrdfj9+6y9B
zxfq2!UQ6|yen4i~0{vSEr<6<Ps>`}650(k)7xtMEFCOww9OQw4dcj>J&!DcnRoiO1
zm<w3PGuX<PCA39juX8xKbnNgD+2b|W1mn0N@tP=yu~j0ft`KW^G%zpnm_}?^9zh9|
zfSD7XZ$9~5He=W)<z_YBDyjU`70Bb@qm*I0+NsY|wiJ}`_7%Og*n<X{(CVpSKoS=3
zJ<X*O=;Jsu$zLJU>8iPVA`6a&E!@o|C!|29C^+7xrN)ydvWAQ%TFU;mAip*q7Amd7
zj-30j86L%!xevl=b7S1)>`&uf<*~_KWP5fJ+N_@S4rRajo~dXG*%$I)v!!nAyt`Eb
zCusRYpu=2Am5?9L>=vdXl2KrT&st8~96U)bW%uakJ+4@!5>hVm&(`%Sh$S2o;ZzJ@
zEshMT(m-@7xKh-BqW{jt;~}HSgoC|o@-p{o_v{LO-=9<7=6+B{B@42REahl&O|!+5
zXCO(_%D5Pb&k8%TwBC|sBMctE)sHI;T~HtW{pti)x>W!Dcr9M0?G+yDJs(+ChrY8q
z1K>d+3Y}X!0iWig9yI5mRhN6!rTW$FOMB<_YSfu`xBCdwV{s>)G@6AC{vq-eN`K%f
zp|&1=F~zmL=({j~c{lq)xR7{3oIy`E1D?@UeD;NqkQ;QY-Md!R>HC~S;<iA2C|1|M
zx%{T;U2@d@W@OYgc^+kmMq4G(o7ItSKjS453p<Hk3ST^x!+kVbjlTGl$z(M~72Ky}
zPb<%j&yUls=OP`IRYxeHZ8uW8|DYCcv7HN=46SyH{}<uz)V9Q41&)2{ytBp##^)|M
zdgJn#LJVp+71wh#Bgr)kMA)=RP23iCpW3wjPmj^C2K4H0YTcG@{n2hOHGb|A|L=Y9
zZO$3>*^by$_v~wL=oMi!Z+7Do&$Up&B#>vIMZ6q}eaZ%DnJoOcUO4~ewuWG}^yW7z
z<6<#I#}Q<z4<den^JYPk$X0*-o_)yWOVbfAWB1y`Q}Tt@M|)mSSIMZ~aTa8EgVibc
zJ5GK4tE9H4@mXSimE@-83z@-s%ERJA<S0wa{80T$&C^>jBRboYuKamH34bJO=V#0h
zs5{5zqvQ!mut#a*us<=4<`Z<Cr8DWu-V=!6Rh_VeSEyUzwc4isxCNhw2e<KI^7J4&
zA=U8xu8_udV6!M16gavy;*J!NTP8EoCV1aW2iHQp<W`eaDi9pozx-r;R_v;9CFhTo
z*BO=YASF(BomgwoeiqobVHJ%1Hirgorr>`!DlTZXSlu|-=9~~t-57bT>|}Slmda@J
z#T8kLFFUskJMt7)dB3(&=V0f<(ckEvPt$If66rA0bye~kFun#={<Wn+I6<YkXO21V
z{p(ZOochkTV8eGs&<j(UjRJ-}@Pmu9`SDLg|1ZZ9uY1G9ux6zOKkdZTZFzgQdyZe*
z4PFQJ`$m~Sp2v$zym_m`O&+y>nSjl#^lA#pk{}7Ri)wHsZ6WopFBnxTvdhzDUP18G
zH);x$W`xC)moH~z_lQ+xSW@$_e1byL?ODcHA(y4cnig~5v7Ai_)OMhpY?j^lH%q0g
zBFx)4lc&+8hcF4VaYdR*c2AKtnpOHcXh<>HJrg!QQXAJnyPO@PJ?7-W)_uLED$c-{
z;zGl#=Fm|quuT4H_s<xD>z`QGbdpJXoN&?ev5#EMg!}O*m-l(ZQcv8$)Y&dTx}-q&
zEh!xTX;qj0olFti8H4sgot_c=nb}}`$C`nR^}eYnr;N9=$Fmj0FWOUdu?BRM%68#X
zK1GX092&w8O=a`>dx(;0tr+AM_xLgzy%d{8mo}BU1s%cf)cjX*E=r?F-SK-|v!jES
z`16zif}!0$A`ix(D{79C2-5nWVP7xp@C^ms)xPHlE#HC6X8v^V8p6?2%gN)?s$uu_
zu}cp>OkJy@;o8&%eDGFR*jR;}0-=zmPxPxC-5|<t*4qy6nBDp@O60KLEP6V%Y_27?
zSMkG2qbqZhw<g8ppp@XK>W`@%9;g6ftT$8ZR7OD8uo%#7Hc!d;9GeeE?@7hNw~`n8
zi{96_e*QD(i{vnD*gg4ta=#)3TV1#&1kah`%}}M%BH<M*-RgBizk#=-r&<3y;L7@M
z<nVt7T%{zXg*0UT30yh;d-0x`;fJ@fvi|T^4%Q#u`ft<F|G)FrpRwWpCEouhZ#A}b
zGPd~{6Yk*Y<U()jYU5(*VB`6psPrFT^gmH4+rNF_|A|W3f2i)iu9yhen3;a&3j8;y
zl=%lD|MU1i7*(zUs)BscNe(JlTx<<NLBZJ3SuDsUGR$Vc&{7`@2Pvq<m3jpSNh%@)
zlA@xhAk@(*cpY>7e)Zh@oZ~$+z4pY{OTN8x|B;s-zA&pHfEm;jZbg8o0ca@r3r0Y7
zW>pCb03-?uD3Hj6hlh-T2Ip-=X4n`^)Yw2GWYXVRK~z{!BS#iCWa#yj5D~!EokKta
zg@A^Tn1&J<1V|)6;ItPf(E>97YZMp^@B|X5DG`DqEJhqP@=k03C%3MR$^R4dCg>I5
z&-{GSNADlN@frmd7#Jad2LTQ8a_Sl)3=nV(;(i8-<#&BbkHefgjg**#gtD?Sf>BiB
z2oY|on_FNWLk;piT?(+MrvM=Uen{X4!J65fNsuuQ05Um-N&8O5dBAf>Az?rcU>vY8
zB8D~ycMy;$u-z!&m}VD1Fbayj*2XJ)aKFC%ZGaHa&~LeC@;7rLj6HP*2pF-BLIDVS
zj6aCG0Kvk5U|1bN8uT>$03iV%E+G9soRnu27$i9O!V#|54i3nI0vk|(Bgp^6u+IVm
zDjIRHZ<n7E>Qg58ynzd-Fv`U>l&B7)A^%x9xS+s5XQMmRPktFR@ImC|i<6@d0U<n}
zgy4#TP#I)$3io%((+H?J(fh~|#1PP6;5-u2o&dliEWjfWH{_>`{a$R)7f`5wmLtrE
zuc8D>AD}qU8N74Ap5LesBLO@GK(t%<+xwUM?hZMVp&<YW*f7vVK^z0f8vmN3S^2;<
zv)zkD26`V>#6lhez{~ypbFVjtr(gm^+cW=F?-8|$VX2*2vG+UselH^`YB74Bx_}54
z0Rb%?C;}Q<GJu4X6wt?a?$I^ahv--O5(3D(Gv_WpW-|huM8JbRT-(v-(7=8?jmLKt
z4F2v8>O&$2tuSEMx5N$@1suqJPssNU^bPfa|H9`C&3F34_q`C-mBR~t#xwdCzd-^u
z%+==zAi1eKjUDYnLffIhM_DE3!!y;nAR&YM%C|^!9l@p-1u>2z(myF_)Mv{80~P~4
z#UMf$DEBAYyna#NQosO$f-ni#)BRAtC}7}s;NKRwiy&`Rh5-J?puM!S9oHp91CoQ|
zr^pCt009e@(QKFq<jjaRjy`pa)>4G>6?$Lb5D-KO8aj}G{S$Zxfx<DrI8_)3gg>H$
zyo65<9E*VO$e*wf-@qSu2l^0ne^A35Mm}u&JPRH6MT8t`fopu*3Vya-uCjI9Sh2wV
z6_6u;Ywv5(fuSCQg2wp#MS@K6xmI{>cxx2RZc#Km+%3IYc!{PP$aQeMjd?~^B-2TR
zofl<?;FQKV60BY-SZof8@F%LL_66y))#Zaqm99*TZx_9fR9Zlt-sPV3IEB^-CCo+$
z#}*?v&#FC$*`&CwiO#blkML)NNx4PSf1^Rutusoi&07Pg3(OIsnnOXeiE&o?#uAJt
zC3~Z7*joytN(Z;dIkW$O(++}&pdq}9CNbt*j~UI8(r5qAx1+K1Je9dEp4=L%$Z-a(
zQft2T98F-zZW)(b`KD|J4|bJ7cvb1-{TgP}j<Br^qNd~=F6;^KEu<alv+dpTS}YMe
zgqxPRZaln9O2U(Pm2ty^Lkpq{oXXycOqH}f#Px3J6_XCMGD3yVB6-)+9Wn(lUpdZv
zgS_FvM&`cUnrz&Gk$ik(gqI3t0fs$;;XsQAhIj4HCWMo=dF9fjTZ1Q)G!=cDPPzix
zvT~yj!PZIKmQo4Ht-|9FNJ<+e+3EbKXSNxTjv8M`^Th~V<xKmi<1I~D<EPuy7HK-=
ze&S9)$$X7pVi`eSx3jzpr(gZ`V%Ln?Rv%~O(|A@Lr!%r0!uL|8roiE@v^Ze)sTFSQ
zLn68hNuPH9wx`YMdx&m+XH#^4p93Csl_C6Or|d$HTqs%diO4wP2$c&B{GA)5J(h4u
z41A8qB{hd0`X*xag289J)^SRow^s7Vd#7Lp7e&ev&I`|ky3Kaiy)LBaE3<u(t9{ZY
zZ}03=#*;kXHmByfqh14g!IL0w45Oqj8wR9&$*Q1SV<I_z>dzmpJliLt#J?MOE%?ZL
zZ&6C~V>_;$T^x8bsXMnbn4LLnD+g;puEo4k0*D&T`E!kXSEetT;Lh{X^Su?m;jGn|
z_SbKiq!&FT#~<KUnqB@N;to@6Lnj#dpNg7d%SLahin_6)=Y=g{FF8)n>DLTBQMO&Q
zV!zGVFUG|4HGC~PG|e#%%Iw3XZ&b|j6Zrd7Ur3v#nqnkM(n8%`WCxHOg!>v<&nk!%
z^b_T=zT;y1YfsIwlKSi85ppYKNuy>uEPLN)ZW`tQz2#$a)kBw3@PmQ#pK4+I!89#u
zQR}CQSoY^Z`5bg4OcMK^eDj6RHmjX9&k1N;X}6m#nR++2+f_OjNFJgWMV@8Fzfo>3
zy>d6b>$3rzCZoC(D1=&X%n%U%jQUNwRc^QKl$P7(Pjo;RCMAoZ*~g4axNuv8L)*&k
zW`v7E)frtH*J1Xkdc%3hRFvstn5$!pA86SdLfbT!*z@%)DrEDGaAF+EQ?gV^z6A}>
zyOA-Y<Ph)#%;9jeNy&u<T)eRmw=bKYjUr!;m24;!PHzKqZYj$dHp$}5!ju;2CsvNP
zo{Xwh(ok27ckb<Jb>sOuY~ar?uMg%O*S;P(=DcTDJU);t>Uzy0GP|F0ZAB>FC^ePQ
ztdCp=)}4#FL~cS2M)$y6F|fKQDWX;rL)YzZsnkgpBP%naN|SkA5>%9z%{Cx)6Or9>
z&0ztF{eZ;v;;ZEqR~qG0=g^^pym))~bs3tc56X6TTNY~ns&(gm|KWF$hVwTt#C?B(
zO$amF79+#+QYFjNEGG-*)peyEB6>`;ZCKp0lH;(u(JOzP<0Av~?tY=PSv-2jdYce_
zRLpAePjprJCY2W`0u0|@()p!47tDZ4h@)#z7Bxo?s_t#kyPe8XIJ(o3rm59;motF}
z&R+Ag%yB-ZSFiX-6B6#V(8wL7kg`BT7f&0+7>@^_vF_%#xP2wPmp{j#twZY%Ipu$X
zNwP1Cj&r6!0gePb^@~c`-o#W%JpvD;?<p#7gc{ql=fJQrR;t!AU#alJF2J|NCBH(8
ziW|1GX5lE&V*x<n2km68u;XiyZYkK;z^M5ri|nr6Z77?^l5{W7<DN^FF`l=#vY(zq
z5>0G+dpqy;SQ}0Ipt7jMqW7>T><oW}A1-<(yu>|6$c|k-#O!#HGpQ~Pw;SheiG2J`
z_7x;-{`l%76+T&Wy?$z8X9NaWSXkMC1Cz~7V$C>%ThU;8W;w4AbvQ!PI<|V~^wR6T
zCTAqYL{ciL`*LH~@V0!F6&MN&6^)>5kAfN*I2OGOJ4%mt#cGzGTo4|*cDS^b<X(Q#
zn-P&r^@)1A)r2Ts!lGK+S+Hnfk9xz{e^^s#rMILy;JIv55ym=e9b_?lNs)yM3_`7Z
zek=|$3$6Y5CF>iBQ{JRFB3EvroK!*4ZFQ{E<#~7t?V44q{=1U&S-<$WxBg)n%-j_v
zoSzhKdPQ$PIUKtzbt4~z5}O=mwq$D+6d7||TW>QkQ<V#+7pE{+GZEfR>{3G8g^V<Q
z`%DQ0kwc)`;tDY)`l)9nDpAeq7ZZ<}XR1f5%trmdGN?Xyqqex4?-U8ZmnS>1@%LQ1
zviMQ<qm!Os=He#Uizgv{b`yD8L-7lN`|z4bDAU-)V6&+**Xd%#Dnsju-pSU8hC+{b
zO;<6k0ks_cuDqI}9Um@HP&4v7#HGx}U6c59-tSUnH5e$%7}#IJcfh6adtQ8D$k5mN
ziiaI+{-2J;_B+}aPaSDRypUW>uU2xW3Zds~%og^@J<cyvdi1|(?kGBtfkKl6v3sPh
zEXh4LH$njT-k)SqV{4>(BV+)}T$a2JJL$^EVQ^->EfGBqs2a6uSk)@@YM#)802yC>
z_d6bIj8by7F78d46wc$8UFcw~6!&4B3a1F`OO>@<B28?pe_?-KxDLnz*biL?<I)#T
zAokHQIaYKm-)3&PgV(%D+FM=EXJzpX#N~-_J&koSuo-ouiaCn!h)N~ph{sUv$2LxO
z0oC<|+>tzbxTkx~^6!ft=m8En8f3EDW2AM>dKEwBwhCYtB%6~|yd%^fCri@h!+-o;
z?Ki>>CC&@JD}d8#%u!j$v`Ak<ij{d8Nf+c!?h<{@-a6pAj(U49XQde1beI3F8I>Px
z!u{9Hzn)(WyH0orF@f?XIJfp{D)_Ok-@Qc-DTa=h+eD4xg?GtxKH#8f$`De%Ue4Y*
zEo7lROd#sjlctcAnitXRrJHR9)h-DZ2o4=(R~R>cu>NFPIFl%sXPS|(l#g>vEFKK2
zOJKS_@NQ&!6{)e^9GCM{+1Y5l*up5!FX(p*%c#LBQ+FCvrj@I#GAJJG!C}}K*#u`|
z`s|8;RCnYhU$gF$M9QyK$7k)JizMm+w9=Z|CVeiGjH%0Ja9)6NHyusdZkfs1up|zM
zZ-c@PH8d|%^^trPz-@MK{@kJUL-8OT-us-5k3Hhtx^`1Ia4Ie|yD?;axbQO31n!kG
znjF-RA!g$d>!)%%s^D|w7CB-ha^_u!&eAZT8gM}gmfn;8sB+1=0K~?vMtfT9M#e+s
z5wqcVG2mv&poUa@<(pq@Z4{DYdcJ(^_KCPv(>#-hV}osEWYLDkSUAeWK2?}#o9K+k
zxi_WXgaf?A6sM4A-_pifCwIWN_EqxMOEu4YJ*om<r-L~YU9x<N)&Z}w+Csp<R%GC;
zJVz=isI@(sI^7b|VqT(&IIzBtOR)q^MZo<@^T=a{t*)${6dTO^jhjXMF2RaP?bh!B
zk&)jMA{DN~TRwBa51AH)1acAD$H>Wz_V-g^PnLAhcs|GQ_oKJ<pHjiphk^GkX5qG<
zXjMV?1YojLOHl1NEMI)>`gu;?qrTa%c*x~uO9wBVsci)%OjA<wMscq|;>T_sREto}
z9me?XHELUo4HngZ%ptmiLmb$PyD!K6Q`AlMLh`ML016rl_hp!^86etB+Lh#`xyKYe
zoVL|O4YegR$ts_NmMGskmI6z^3s+JTRnSxluyn)RI#SR86EzuICkD?j_&8ZL)erk6
z*!SCD97%*L!Bp4oQZ=z*K&CUy`yIJhD__>y(QJJ++F-5gbw)_KyGq)YwlB<Us!jMt
zvhF!+yBOl7nAKdf8v^18>Niq?Rg)X#`-64tLf2Qm9vjcyd^@DoV?FNW+Hxu3-(5nx
z+^&~-ASmk={4o#^hK5Mb3|$ToV%!ZGbXf8H&v(0wC`@NNlNXyr0Vk&E-+(wxXAxHL
zV*}C9T=`(Sfbv>v-^q|?FV#}~#%sY-wBVvVCKVRMk=lwlN<X>0D@j8xxnBq#*FDu6
zp&wx!@b^m?nD*Dmjp#S*MiO3hf1eeVaaXu@2-MTJFL5zdbR?NX&1Tavl|`J!U?V;!
z8c5{vZbEOjVD_&%HSbcNZdEJpuiJ?ak8N?t0L`<=+9hdeI48-fPy3bnN<}&FCiv2y
z;cXi*n4=DIVpoFb#Z@71w!FC2bJaW?B|8UEp$1QkYgC)o@ta@Syx!a}__X`oRMiVi
z=PK$1$%6-$US6klr}*4`J>)KjAJI1r*w7uVTvhmf%kcV`|9#lKbtkl)myR~BcpXh}
zh@rWYN*7=c&AAq89)S?N(L#d~UDaklT05g7)LhslF+s`S?8W7(mRKzTsjZay78?Ae
z&eZWWPb<gTm2gBgIK7(W^m|fB_)2I*ybc8dGjDoIOjD~Er~bwv5-&%lQ4RUaK~cA#
zxG^AFG35SYT*sy9If{m3deTs}8CI;Z+J6*&Eyxx;2*_Msa&ucg3X@}H4t|QD9q$Dv
z2#a<KDu2O%H$A!rkNApNCu<f|<Ph=6D%h$0aex0ZM@2GFv*wk3trDu%KsgH7O*LVJ
zca?ht7{;_>o-M-+GTN6(M$c4@kMFb;=UU3lNJykF>ht~%qV~l8W$f4wlx2}g$2r9V
z*+KU6(Re84VY1Zl6jgr|bK<38^ddWmzI?M$-QJTFCEr({JgKD|4%_HJBflLfEHZ~W
zfhm7J4Z9x=mxgY70K#-_5?CD1JY@{dzI}u<NX!`N>KMw;DH4~2!I4Hn=?d--dWkAm
zUd)<AW=Kr_&WuZD-nqKvuZZXuF*gQDpTfa25ez#M8gK2u&na1|q~9yo0qGB$q<As!
z4MD^7$n{UUbQCUSxLtj{ihgx==hXW&$mV(ZpOY2JVs5sc7e0<xzP@iwzKoE#Nu^ID
zY3<_}U1=e>DIatly(JvSA*DEkA93J46ZN0L8EJm0b`z~%98PJ8U&Dq38s3{S8p5?{
zp&ozb#yu}DeX;xtXVpZ3#>}HmIh`iU?ej4*l1bX}_-qQ_14XCswyUc0(=#Y>uSY%0
zCZ)o#>}j}c5@z>Sy@j?bF{Rnf9JbUy^5XY0E%kD`x8kEG+&%cWIpZ#Se=ysbKVH@=
z2Y1EdFEeJUxVO}p(vy$N&SVQzXXFp>E4pK1L^vYtriZxAVU(U_IpjtByv`Sw2r%jV
z!+}C_sBw8zxK!ko!1aw2dZe*;1pOJbOLZ!HV<?pMAP#qyscZ^;nTZtI4(;mAP4XER
zXz$eE9<Mnj8RK!b;m;*+4)sO>AxSM;x+|sN5Ny>QYb>MM)~h^QJ9(bI^BR2VKc1=V
zlEZ3o@_CdNimY0^@}s?^o=GnVCKkC8PH&$~O|-?#lPtrl6VBF9334M0rf-6tB7LSM
z(ZYsT4y_SF6B55XUdK)#F&eA0D7qnL(Br(HV##lF*xOkpdQ-)AmTkIJAKS;`R;_#N
z1`%z%2PkvglAF{432{%VQF}{+d_|~b2@K6@oO-V85T`d!dM>*Wh{(taZN$Em`bDDN
z=1s@UICJCMHHq|@cSQeM(rrwJWil&`z?C1w_#n+C+OCTJAn7Qqg3)fS+65Z$Hy^Ls
z4tv|t5s$<kA(A`QzqrLg#^_1h5+&2g8oMoR^Cj?(3j0uwokpPpcPvaJ@?Q#NfhxYt
zPp8>LxpPTE^H_#pJ2&}Z_YfDc-uV2trtF;`oZoV!E{u7I8<#JpCzA|5vaFR&tnr55
zVBM5%1Wn;4Rg!Ia4ArSExtd`V8;(u>3W_&Dj?M>p_b~YpMSJ6C&=1QmD)u2N0Bif$
z^4VOWp5lCih|LM^=K;%+>Z3beBo<?p?=0jRxMR}#JG%F6ZYT!UddEFTjH|Zocp^P`
zu*Tj)Nd%@?G=kidj9u%uT<mPUZ}asG@nT!*`ReaMl3+1bC}lD$dG_%<%Jk<sSie>k
z(3mRp;+~qoSCHH03Z-`&=8r$5K}vUvh{7}~aOR+=z~RstP328}72+o{J?6`gw5sTp
z>)8Hy?D%_YtHOKY{h6A7c*}$yQ<8l+3<ei1NC+Q$hWB*U#OO;N#P{c+x!2&Uo?#J3
z;~Oz1aO#-E12kGBZ4z+RmiOZ*SY_w>T_j7+F*@0PuCAAkKHIB4=n12K%J7>Ym~mE8
z*1kC3cUCpqF3uki6%1!*TWQJ;|CV{PMCA4X;F>AziB(S|#Ni~~6q*~b4?NqaO46v>
zTt-INZv3>FguQSyAB(R3w1txG3a9Z@Cw%L}(Dqr6!-bcJmT7RA%dqqGT?9$^BPNIV
z&rgpSx`|<IDaB4(yZY5u_-!n{)TBl2!Y^h$Aw4l>Q7?JlK&p!OMlp-tPKpl>%oQbD
z|I?IPE|2B+I0};e1Jc7Bt*H@{aaA@d=Icc}!7czcbT#18#wNVK;gl^}fV9^Us5o|L
zBIVnv4}VSu=ZReZ>OH;uh}rR-<-ik>iAF9-X-whLUFt=VQr9~bpkf4)p4*|+cR-+g
zI?|}>q<fD_ILKQyIxT{okEwIm5!UwNSv5wYeL&atO%O_4Cy3t(@iUJ+`>|u}Cy1ts
z=i3OHRD~335CvM!fMhF+<x2R~dYs<GONbAyDoh-P<)})&MH+o#Ek31K4Qg$sgxHSS
zYYh}WUAc=3Nbh#gJvN}pe_9bcO-NK|=x?32P4~oZ|ANpefAT<(F&kN-e7Ge0&`39M
z#*=m=89d=yR3~w|_*$T7LXDQQ7k6%~qq0WDJVeCQu+*Q@*jTtT#`{M2Gi>z4LFq78
zvy5TFQ<QkLB4ePnw-8ES)*@$i{Q=YIxQ;0ZMdy&>QM)cBrO4x8^T?deNSrg=fTwAC
ztJ+Dcc)SzP&8x7dm*e|Ohq*Syd%-w{NZ#d5iCThnvtJl>C`jtAuOEbeFnU)}VNBTl
zy(T+6L8n3q1BuWw_t_vCsPi0q>AQo6sf0mJ`5d+*x5K%#v=kzUWZ|O|yD=w5El$WT
zyeJZpC+(p81rmHZHME)Zp?$0qaxbXK;wj~#U*|LjJr^#yJ8prxvf@qWit#%#w=&r`
zKM(wvX4yaavV=c69rNJAGic_VNv^y=s6@o@*vZPz+e>LORpP>89{I^j0j55YoXkwO
z{%)+I>{gF`t8ViPEm3_-D*&lq#h1s(m7C!*ypq6rC}45|Nm9!&SQ;X297_ABzd)#P
zONpqrEMSVb;Ul)p*72Y^$Z|d!gn{I7^6b^+L)#0@i9_Fr_bHt4dLLanKb<x+gj^ER
z1OQcWc#`>`>K=4M-@*uBFHxuICbPAx7<z{yjr;ffC#I4%x0qM1adPBBGqr?w^wT^F
zsWlm}cBXJc4zpl~-vr=TmVK!{p;EMy)pk!3|5~~G4aBg@NDe+9)P8*e+g9OX!Bwc3
z)|007xaM-Ihd=!1um>yfjRa4wWf#Fgia=b>(X0$bl^vxd(E(pp?H+*)x<5w)!j>cO
z2g$CG*0_pUbK=S45GvDF4xe$G%}JN`n3%Bb7F~%2?Yo9;A$X9NW0O{&`cdAJbMi-R
zQ+o7>JpE#=3z8^)xwm-2l4<pHO3(AjhH!KbL}TaO*tKdA4rC?B(j4BV(sQQ}y@M(8
z&hffW`}i}J@WP>>WFv;2-+_`eXxnb}<?}rP<?l}WpSDGNZg*;ID|oA*=@xgbb8J!X
z{2ro0s+O+1q4$gGc&wS>kU;$&?%+Wt(Yr=-X!vcoSLNSU)Ki;G<4BU8O?Bz(RwcKy
z-*e)XujhGKnekek-Ywwjlg#bc56N9`Bu-A>uv=5dVjoia^z})K?adG%@|b@8Ul4Bi
z+{hQx;G;Qz@cWiNf3`b~9Hdoao{d~O+z0?yd#6Ze<dv+rT!wC{WE5#k{Mw~;m6bHu
z+@1DU=tuWg_&1acilJw}Y4|X!76f<>{NET?t8T;q)0=%2gq~mV#))wqHkc1sp3@f3
z5uut3!4fWjWSUanF+C_-6XNn2X7)wy=7oM+9^7%J1~2U;rx~+(t+iF|f<8(a-P7Ys
z19+#RJNxmHQmA$9B6_09&m&jP{oG0u8}F0wUgXklK+KGO4Bt;SDHH(nEC1P-A2*fv
z&<vO15!&78LDyu3hb9^+`DC?3Ka$O}pN8Y3!JY}XY<g8YBUsF_KAm%d(hrIpp;Lrh
zs&Z51fAzwy_UKCJQ6<ku<)~_uXs$RIc3-e3@SGnrr{8g=UE=ONsk&eaJUC*L7NjD^
zXR{MS5UpvXCG|MNs?0>E8VL_v8<^lrkg-R@54A=g%Yig2J+#g8i_bxM&P7UTejrIH
zy-uFYARx!@n9LeXuB)%Qwl4>(Zp~HKf2z~b3OiLasqFSRBUYH41&jKo=c5#j`S0K_
z`@gZ!{~i3*P!m;C{1^QFUrb&9r)<m0@dJNZ{vq44|NkM|{#d#H3;6p_+1AA9N1`=z
zq5mHj|59shE$v*LfBq{@mi8w95PE;KT9g08!vBE4|A~eF(P{nvY_MkHU}XOvAef1O
ziGhjz#{&L;b73Y%4mOtmLoOWi462a3F%KHG2jULhBIMxybF_leY;NfWb&|A!z}oe1
z<?dqP1c5_3t!$g;=jMEO|Mrgl-DiQj)#yD<k8+)|gdsACD-)Om_f!Kz6EoQbzzFK<
zie?7D%#2OU%uG!L8p<`gg!ZD(Nfb&py9Rn-F!LTBn!!9fh)RRZ?g)xa#jy%(vSkBE
zV*`LDr-w$TXJ!UK&CERG4~v`Q3AjggMc^C&L@cn}k9QIzL3p%#atPDV<OF*BdYuno
z#f$|24G)k0xpM`8$o-3XbaD{ym%*MDXcMqG3v)eaIcG*-K#krva!}UJ_|j5hdgR!^
zK=9n)bm(0Fno>d--Y%e1Gf+8@Zou@FfODTdDKLuc{{>1wwZH6u|CGjvNC8l{0=fK0
zq2^%W>S5vx1b_v$ATyx73)scY-W=!*0A~lN$;bng9D(-#F_!<20X^WK<^Wh2S^f?8
zkN01JK=yw-o0yq7*g2Zmdx7jN0Tv)zAV5h{p3&9Ql^$SXZ~m8|iLHwR*x$t61Y~Ps
z3O4wgx(Ps1L<L|1uJE7wT+Ez7j;=0@E+E^#YGnE=47kk__U2*^c6LB}R~N*;_=$s@
zfo9;odolfUxHk3<9`-)}MHV1?bBn*qFn4og(y#|PxdCOw|FHp!5P!=ofvx}!W@ct?
zHdX-82>|povts%yfx4F?@NXr{Ut(|#em;&4jsOdA89+af1rYoT(Z|Ka9SCrBb_4qP
z{Hgd~gvi1IFbA2r0!)FHAbZ5$(ZOP%#eXpP{GCCb06k{#_^|+(|N8m&F9Yy|nLF6q
zdi^&4`-qvu)ivaGbm;ym`Cp}|sDmfKhk+fyz{<u3;9%ndaPn{h{Qft}zxDsO_P?dd
zCZK<~{Si;b-ogRE^PdvHz58z&?*GgM^*>8P1Nh$ziVomu1_G#m55GP$2eTRYAC~{m
zEAY3=|G&xqE6V@t@c&;%l5V!Pf9t9LWAOi@H?afRdi`Soo@O^!@Mb7DfcL}x|C(w6
z|Fg~tKy#3r-T&3fxSD`BM8w|G_TNT=TqHrBKyzh~tC`i`6Z79=07th4*#ngwTtI)_
zHvk3}X6FB+1FxK!4fsxR0Uy@iDj;~H{x_q9y_tjgU#rK;!3i*Nb~f=sWCqU{D+dR_
zhXuT&=0MNCM;pMzXz$<(b^(B^^8;8oI3xabq?{Z8CXv5H|3RDpCeh!B3&14y8*zi@
z?l<BAFiHH2xR?P<lD`oPfJy2%Vg)cs|3+*8CYj%e9l#{}8-cmV{YGFe^1l(7i^6XN
z=A!r;fw?ICi@3pDlz$^I7nR=#%tiG#0&`LOjlf*ge<Ls#jo%2&Me{cTbJ6;Zz+AL{
zBQO`8e-RIui|%g(_A~hxv4Q<e>>R<n^w%}{Unx77p@|E4{6H=?zncxFX5#ug4%pN5
zHv)T@nwZ(R*qXRl{VQc*1s7-PY+?qq1zNcNA?El;{GY}AHwepr#5O?J|FP#``&a&d
z9Kdy&{fjujjLaNt!9)CS9qfNe?CgHm``2_c{VtRZ+){G~TU!(7KP<p3fWL!+Er9=%
zl=CmWlN<P!_%{leA9%WLP3-=N0&a-K?<imk3y}LCVgFJ)xc!+HY-0I430QCW*V7LC
z(*#`3@8+_Dn`q_bXa%(Y!vZV={Q<$`t^a`Fqqq43g8TKm2u^TgZ2wyF-+FL&|ELIj
z$20v-#Rkq~?`CKE*DY-M2Lte9!{qRrBRB;3bL0;-xGu-vYH&zL@E3vo{~R|v%Rkcp
zJ#6qp!UP^v@XP;)Dd*oZki#Fd3+|(%t=k`UfWMlUoPLJ^H_^$>!4+t3`o{_K{3HC|
zlq@V@<3A371zhy+z#L#F7oZ*Je`k&Z?BW7`OaJ8qru~1mf(6X_cM|Y>#N=w_4E$p_
zz-@8$aQMRk9K-GR)Pq_4{n_MV=HUEiZ^7s3{s#op^Y~-uz(GC#fZ(iNe?aiLc>m@M
zj^qt={ttuyx#`W^oWTd<`uAfDeg^)F|Ni0v0zH9dh|7x(W_+R6b)jGHzlaceFzihV
z%u(!VWzaDAEIW6&J)**=($wSxZ#w@JNge4$U)z<Sz7<{}efBwNZGewxOIB(B<@;om
zs5-OPj<__9J2+KzB2qs<j6lSoF1+vi?BuH%WCPs_*(FPn=j6tXs$76|@6j*iSwB#|
zIv&2br?OweDTnk_KFbiR@j*YRbe<yL^y30P8JsHv@f%vq3s1E9+n;loxl^yCvWawv
zey6c)#Xh=QtZ@$u-s_d>tS;RI<OI6-#L%~x<1>^#qK9d+c;9@=N<M4~eEY;q!w9iC
zK0xX_*7fd;S5ICB6OY-}d6`FQoZPRyo+(JX4;dk6PtDkow$AYc^&38&g@=cvRDn`U
zNx$L#a0J3#q%)nlf_pr?qL<J9_A<<^&dIIZ$ywc$p7*Gp#SQ}E4$fM$=(_lGgp(h(
z0vyrJK6kfok#+WG@>-uh>#0-6InRAhwT{6Gi63sLi10<JnP})(MOjT;6eEeZwOH)>
z2sJ$IrVt4FJv4eF1S^sntBg*U`!DQg`Qy47+;YFXJv@jSy=+saN@O@pF`G%P%@WUa
zaVnuuGo$H~xpIpZN6O`DdGFr~%dVPN3(vGmo<dG2L>%1p7?tP$GRJstNmuuHBG7y&
zcoGdWulq5=B%q}^U5hAM@fJH;nnQ<aWtU&KZm{UEC_iE#84{OJVxm}YY?3lBRtBk;
z{cz##C(4KKaA|1xR*p6n&5??FRXG&6qE92ES2dEoLYAtTa-ZuBq9zjf)#j9gTe@G-
z;wy^<Zp(M{#)<lpmkqwW_s<yDo$JwE@*4FVuK7?GQ9Bf*MJcYAIClh*O^MmK*S5e%
z#zWK|rtJ6N*O|E4)i+~uyJ*d5^gJ1xw=yP8+PH4NA8UAgKj;xZGN;fyN9$?VvVgm<
zQ=N`IE_k|z!sxt*Rhi&u;lnK6?aL`#agclTk<Z|msixD)cWRpPzEo8NW@F@w+&r{h
zj_pswHA=0uK)jgsAs>?Ff{SN6$^N(N=M}^s8`8T7R~9ly*b*e={AF;%i@Ahx)wx90
z6n-fwqee6G#jPiNkRGK2g=^P-;`6IZy8nc|Z#|%+eJk<Jt%CGxA;(a=<7$Ga%-53b
z{Ae`*?$hTyE=QG)SaxPJNa@V^1@;R!y;BqqosPb=tWSF%S-<MJPS1{vrmYlyXvh(=
zy2*@gG0oY2p<O11A`e6~A1pSGCI={oA%|s0@dlxj1?OFUriuIPt1)vv`1$061s>c(
z1(tYYk?TGBym@+WX`Vo3b`HnH_16G8!lt4h)?7UlCm`Y1ble@RVGJ4q$eGA7btZ^u
zr^vS&YtxcPwRZ`vEGe!MQ^B*%>jlCnGX;ToEChXpb&`gvLTL;iQ$JqymN?;t!W-ti
z`}F~emc`QJ?tryBpPi{BFSmuQOSoc3{|lG*s)_gU3=7c_O;j`ER_4WZP>h6Uuntw*
zaQWRaN2H`#fPlSqC+kkf0^Fh*_F;kt3v%q!{rDkUplPm*K}1e;k5@|_Z`}BI6pO|D
z&uhmq$$Q6_CBMonEKe!A9P@wigDy#xNT{iNKF2TAjiGu}yr}J~0kf&N#a}PSW(arc
zS*Iqu_x(ujB&z~N7s?#wV%rkWunb{BdeVRBmgK@Aoh-jnr6yE`qe3eA%t&TL^_gsE
zYYhkex}W0XSkEa^wiB5jrpwd(HtM}TT9}k=hD#cOZ_H+>j*6J8?2btJ_RE*up5w|y
zgF98#>~gVUF&M)dj%S|tHzH@-bV5U-C*%ifLo~<2Np-xX)LK;g^w1FyFQNld<;VmK
z&Pn6y`QfPxxH_Of7_?APdYg~XWe1e#0h1LHWZ7<)?aMk_kDrat1u<p}jY%LHs8g%v
zsNn1}ABmCVsL<qg=1`~vb1`xxwv-j=c@++QqSe@>SXEJlIrVe5gcvRs3~{w%sRCCI
zZv!N88J(;$`e5&L2y?G~*#?`GmwAjYD^_bbPLvu~Ro(1lDe%<7eK*3EwVXBec(;>|
zJpR?CKWJ2aHU*+eyc^myc!XEW>779UXfWTSJ=ys6u;?=k{0FMNUgIq*L%3dKb@h0e
z3VZ1&pzZgj0*sD{q^r2QzC2^h^`DEoK^cKRzpVqP%19g|H&LktU@CN2TK9ieMCEC8
z%~X~#ohm)Tsv}YuQ=Ss}gi_<n4e&CGoq{IhcnZ2ar3V<W+;=3N=Gi@sI7lM*;`5f<
zo}#n*>2E%jn(x?hP??UIt~=4EN&jR7C?$V6<Q~{YP*k+P)9shKf`xZ-n;>Z(s$En_
zf$F^JvHW#3Z_We_#R%_k=1<$-k%qb5)`>RS1;lo|A8SNp`BCTRfEFkRaY~(b?j!Pc
z+C^cP*jNaFf&8g)%#Q%w7xO9y69Ti$aCW7X`qv6emwT=Pr1sO2h;_m{O_=Kt8xDcv
zLCS2M{8JFgvd4A!m@i%#{>N~4v6>iVp!cpMOzlSOk+EdRjx4;=yY7k?@4@_`zCsB`
z8ty`o99Lty^ht-~<>Qn$)m12gA>P==`|o^T0;)`Ct0&A4G>{Tt;At3LWr(bt62HAf
zlF%|k=Q;fdY^x2d5j+T0#2L~+7+@PD9gg4PLhBFYQF3k4WAlx@;BaP!zHW6KTzdJn
z#0|fhs+B?HBrZSD@ioq8?aa6iJ9*gLBZbiSTPTW@?5j6ZbIzeORi`d_Xw))9=90o>
z5qE6SR$Tm`F8X+69AwcOey&ZQ8hOVJjJ`Wf8VW4T?L(W<HMWq@(@b@Tq9-wXKX$=p
z-B;(~FFx(gcs&}ruRIdvxr_5s)!S}~>^v)iTgDCqPMs4>!A0BtN2s1VY4K`dzBv{=
zq~9uqy0tGE-G)UyEu>5(o)|r}_<n_DI?#6`;1!F?+t|o8`oQy8ys@-=qKC9S@~xo!
zN*FkckE$?iLR&ab{xgcW%l-XYli(?r#;Ms+z(ijYd<6tmGZc1&eTyR6bmk<>hpdAV
zt&PEo7x=Q}SIsr#&l!qtaCAJcD`@Oo7%^T_Xw);kFPobvWkD#t-O*%K-lIFH4oba7
zDZZ6vx-UfDqDA{5!p*vYBBbk`4_m?}g+R}=Jwj8VgabjcNt{&3*AaM~?y&P8Ig5Bv
zN9rY(q7Ak%Sj(|I8qbo)vNb|vn0k%|y+|vtqmGkSAC`LZCcBf$F2gmTxC5salD+c}
zHag|Qh3W$zp_1{QcuC}9_^NLo^R-?n#<f#IZ(+!nB6#skM|d1}8zc?QvqUFJ7G+hb
zAvyIJ)M?My-{r*4c<DeGQz<k)Q$r(Fy{S1HRlurH7I{Btkr=2>W<m6U3`Piem;Rz%
zN|~hE*)K74f@Dy}Dj#?p-VebEgM2AuMZ--wj>9Bsf^HG=GcEZYUK?{{<Ap1FvjYD>
z474^gq<06)COIcLPP5;yAT9n)7VACG4j1RaGkGU_EfDfcLp|W&v-}>t`((blo64a>
zEART1z@;B?qeR148TGE(=-31^V{%tb9Loj`lKC&(n2`6^qcU(4eC>hC26&hqBaN4>
zxT=s{FR4$5sckaQ^eJS+LfIdagMRLsisnvD4nKY4_FY?*n)<Y{ey-2gCXI|}Ke!EN
z#u~X<<4LML35T7>@=l0%3uhanHEj<a*Y!O~-xjVAMeEF6s%2=jS{G2yJ1ZAB0TJd0
z7~Qm@Nq-bSMTWL!cno_J+JCAP%ISXV?N*c^Y#M?PGk^f{Y%m*KH+Ejdq;&2+AVNWv
zIX#BMj3<a+X*CkWcoT%eqIRri4toQEzo$d~Bg~klsJH5NrXT+sFKjgKkFHb<1EhOZ
zOoM4jnQC<#&x>qpOVdx%$>whusd*85cFrquh!{UiL#0y`IXZQf-w5IcR0<j%d3(~Y
z_!uTlm>D={?{GR)H-|U2-sTT)RJuwhtFMVYPkjTVD)CJ_?F1QcKagX4wKR)HI><Yt
z1o{YAbT?I!QvoFJ2mEMnLnt0-V^A?4%O73MT?%ls#UK^E&6?V?N`D1&Ncm3$EcLd0
z0s@X%V~FR?D~K>5qjwEVUA(EfhI#j<OteHA$}<Jo@#zqsp-MR1HU(r!)UO_^lBFYY
zAi2wQwwrP7Td=05e4c6<g3!L+Kfd|o`a0{U{B)Y)+K*FED(hLF%KdG4PL*hNn-Ndn
zXVlnj{Z$Vql905q^oHW7m8~Z-{wa*BSAW6TJ~&)ambCHsC1&a>^Epql%BHdosUG&K
zQ^~q3z2j~5=E^=@1H2aS?ZULnXN3jL^cb4*`;4^fo-O!t*-IVG%du?khSP4Ao@Y4o
z_p<UVxI$P84Z}iMWCPizrQ<3+s6^+Ttrlj@({s7b4IMQ|{Fb)<vFGWD96B{}QvyM1
zdzS_$=7_?PMS(`}91GGeTfx6>*Kh`pnXY!)V#aMWiPqirEx!k(Jm<$O{y_JmVz*75
zPT3QFsOd6NlG$wKBFLU%@<}j+@%Caccb>raM6n>u?-(b2#N?9QoLRiQ5B)aReb=D&
z@$)WK?ps1{AA;5s)5hXCGaF^oH~xb`5KD;6`L(@$_azhFgu60xRqsMk)I(ZgRLjZd
z=!;2$%TAf{4n-k~PIrPRB{JZ+d+IZXo)i!~H12Xv+0&O+`ss7FKqV16#+$9Qz$F76
zx>%<hg~TG-=8yhugx%h2ZTuTvK^psF)578DI)&kcx#9+GW1DtvAHyd(fB7jL5lAqq
zn>OlVEVTM(VHmMRYZf&8AjpGjAY@lPB8w-+EvlPtJigdQMMe4S5H6b@r`1E*h12S*
zrAz$~dOR&Hk1!&<A2G>guGCk!KN{+!6%%ffctudNR)zP8*9DIP-kpS&Oo4wKjwHkD
z=-ZApkD`n|Y{no*tkuFg8iRgv^uu>~64QB{9{L(fC3`AvoRO#D@~Y~0cyA@<!>Ra)
zU&2^$0TH|5Iimqo1lK0Ig8<tDqO!TlpIi@Q`IDxB>a)UWWGaLy9*&dN**M7yR@y9g
z^?X#fWz(<?h9ypD{8QBD*8z_`98%p#p+IhQqXd&lLnJ~{BLwqwI@=0$G9MBU=NBV)
zXHBJ5+=B2okO>xWX$QH%<yIk^mQ0zR?fG96XU+!aG+2eMEX96cs{~bGzjs9R(~*If
z(+hqWh@|hFyA#PcS5+P-g;%x#@!x5wL+&LPC7^7b7)QEJDn%wK4`$hp$NyCKq19(r
zN17m^7#3aG%Tvrlt`mY~PDytpcs+ZOzi3F&Zm30B*?167F;Rru6Eq3JbpFczEqOth
z;K97JJiRC7z`ce8yB_MeEr$b$(=TqJO4n>ApbUa~b*Wg@PUI$j$(o<Gvx^DLTIG|d
zs*ykLH^`Cptx`+w{Zbpz40wM#+y&oNdT~ES+)%oA@TI$b5Ut$y0QI(x@F8;-Fnm{b
z6{^OGiu(|~e*rA|(bM&g4KnbJsE)cNdIk*K3S6xpu6n+@NycPkaAa=96qE%{_Pk|o
zN&EcEg?5P%oofn9J}ZsLBC5}3+5P#hS;)69d5INe-paHpW1rL2zNzk+(~=(8DxTpj
z#hYSTk3@XNhVtleGa4v`#wTLl#Ahou>ayz-fMz<}3{bhesduxu*(pH0q>@2Ugehp>
z5gNgAvM#^Oq(K~wJIR`>qx;m9FM>HVLp<$Vu+#pTlTvy*MT}?B&KcIXkIT6Z^IGbG
zf;Zwy*7>E0e}lThQp&KmzoFj0##8wS&M;7JXJeiKuV+i=Q724%ESc=SC50SY$fB8x
zBz+}o(+{IHNqg+O!(rqB+fxqJ4xM@!(3k7hKiBh~N3t_%5pFaYkc#bwIDVdy0@{6J
z1%b$;=!!u=oD{|r-0lW4+jp7xE*t29Xr}Ai)PoI6E;g(;LjsEa2D}JCnkBn~_*J*n
zImq{x%~vGRTXI{6{ihcEk{XfpXol=|>8B$a|5t2^oKjQ0E(|51zRLkVoLp6ph|Sg^
zi7JjlA+k?ed%l5u3)M25KVqR$w4tc<&7%+H9&TLxSU(W>sEE*byp=t6h1cGS!vV3m
zeNe?@gHq3S;z%_0CGkIbOyWOc{ngDyU@GDl7L=R$wpd<!UWxPI7ZW-alrb;q)cj=!
zR6|!oVi`#rB+ezb<p&u^@>?nT)YowZV=q6c<B}Y_Q6U<$q&3)$>$I#_u5CG_cif_*
z9dWjhBaCavP!6{wQ@D)_ZjtVYa?PP?LKDoo{N==7uM`YFIY>ZlFP^8M@M}k!n*ild
z4E=|;ngi(&-l2AG@!LtYy2pB1GV=(QHjt699F?5uR{<w0Wtx!HTrfsNWz?HLEX=!K
zVJ-s?8}msKRNHObZOMJBKzFCiPL3BHh$@~CGtLgX(vK>U^_lM8mj~GcAs4{N<xBQ>
z<FcV~;Ahjjj5eVPO{y9b&kGz9<X+nC30#Dse8_hH7==ACv73UZ>XcjNSu*D=LYM4G
zHL>b_D1qKtuTkV*dh@^L4f)?Ol(3M0;gC<+P{6>DVGw-5T}YAn0l^Ys{yqm&mgfg$
zrc69^*!x<*)PQB&W|J9hyC?=EU0D^*nxvM!dANe#l&X$frl~S?b8wY=)`6Ljk)?tS
zhpt1v!#ehNqO#ojkXehoaA<$L%`>T9!TG19#*>Li`dsw9%8Xr0n-K#j32v-rA?S?U
zC1aCFMJ8iMxKgx($(<usRuFXAqw5#~HIrVy@UAbPSji@Yfr&Hc?t=C$N~l%~J=9mR
zm}m$HCETA;DyLC8#$^Y_vPPmcTO?Mc^G=sXSlr0(_(mA7riKW+X#2b5749mB&JVIp
zyHZLN<OtGDd`unWvSs$|XEZ_`S_nNBEQzn!l6n?meG^qXorr}nE-I)VKcnqKsy!|`
z$Si8=_rtFmfELv)hht*(j5c)}jZUq@iK+`<QaFlg%BU2%J<t>)Dd6cOc-=oBMvsz(
zLbuG7pMMV6PBtm=#O0gPL5?%7w?n?Gog4(lUY(zRCNp)}0yzoN_rKM(A0_T9%Vyi}
z{Q#UEr!KCw99(kvRf6*sAV7Yq`q2EE+xW6p`3PH+O9j0n%WGy`&Q=m^!HTGu)X2*w
zCYHdbgg)v{#vgz?TrVQTBhWP;VUje5KkS43_^2?}9^Lel(ocTh-tA*a0Ddw1YObc<
z_!b>wfl-+0aT&Sm&x!<=W5y=Gk;S389<!OJ8lUeK{QTMOr*~^Do4DiNi#HMck4O^`
zmubV(h3gPJ<IZp!_>DniIi|s`O_`$^<X%V9<%{{6rvL-3H2mJh*?Gaj^<8G@PZ+lW
zq{>C2NzGcc#JXZ5gW1tapN`dn0ufG_>yH8#Oycp{f+rWd+u-)pLTfoDQ|}o3xT1pJ
z%n#x&GxTKB;<d<#Rmc0V47Y}hL}zU34%B~#uO@I-N~D&0gY?!u1Cw)?6#Hv@0S{=G
zW78Bv{8{m?j+BJTL%tUU1#*ubv(B0<^$0@Cz1~Y<($vB|yS3<nh?0IGTpGSQJOq8F
z>8q4putnHeKMrN`XXS3smIg{0O#H0O*OQ`nkN`E>SbE-}H*c_>A0UhT_)%~XX2zf)
z|3Jppz1mCpX8Fnd!<-%obg(N%oA->zcfJ`b2(x0Iw3;JrWHs(nOrMX5s0$x;DrMPi
za9mn1sK}k^vR$*DBlfvyLTNVbC}ZTc##QD~cICN}X{zcie+D)Og@3zFoJedUV~p(n
z`f34)M3=~ZrrkkA`+}!1`ZJi9i>T(-`mJ4d1~hW{yGE)Km@aI#R!{7HLVi7Gz>n`0
zFh8R52u|!1)(taV+D3SD4bv5T`gkOg2b!^Gh@i%~XVyh`UHMnE-k;QKxvV0vCHeKD
z2krYCOeu;y&-kCm4gZpC@3wiJnHgtu8i=HrWA*`VxClL7VKnafi>pWyH0QN`GEdh0
z`bFs-_&-Vp6QdsYA?T^J#iR@=_LJSu@$|CVW}n-!;4q(rYNQ?&<rQi5f<mY*kgqAE
z2oGfui;<>#=rm5>wJuXhr1RyAYl)V0N6Mk0Ox$HMFJ^ZdL*7-Zoisx^_i|gF;Lh;n
zIjPTF)MP8p9c#)2$>OP<238z78%v+Rl$))yV#iqAbXXr(R>{p)MQgCuGII?RCyVg8
z_S#htAz&*l7JW2rC~r{%O#=s_-TBn1d>@*+IxgJ#Il{%!mAOL+B9ESHDAQL0;8R*_
zk}L8BS(AVK()z*=$^Yy9TZpV2`a!Y6{xt>Pg|^3KrS+;|?Z`FQkDWmQ2}d8L2A1F$
z*VcUF9@v}=mJh2RcOx_~i=hW;F(JP%yWc#3(j;t_5g=0*J{@|mM}(cJZ>B8o?(T5B
zHU}nPCBH;ezR%Sh=ZzpN1#&hq01lR!!>t$Xs?(=Z0~j)!Dt@s*tEKTiemHAJYd+Mj
zY2OG_t@UT4*Rgunh8|kJYNs4e7OI6@V$Y`evYC;Fdy|I&l>EGG+DnP-ByBFmLL^R0
z?jn;I$$aJcNp#EH&rF0hs-axj9GeQ|`@`<iM|-c5)GHbMOm!sYs~EIc?TdX8hli!G
z>PZCHw0&%;^#?UwfakiPGX5(p<2Z85SX_dl8^@Hd09C%oFTu*sRGF%;E!ZzAjiaB#
zoOTs9D!J>+s>SYtM~1GCE39~U@U=+{gz1((!HlVxod;Y1U!n}qbe8x0hJQxRoj1=L
z5ZYf_yj3Xm34TPO^mhKnaHvk96xF)#(SNPg<MgZFD+Ah3Gr>ZZNZC*V8{tA*&LveK
zzAdK(OA7)TP%qq?;LhsTBS=k6Fhz~Dn*__RNDx-4;lUvhvkC7iGd^@;I|k{zlUe(d
zgTDl=(HretDcOGqPa;IBQgse}AbtcxHQeV{F&5fbLQ^oG6aPFiI=&|;hQBkO5+K%j
zscWLSxA)xDu0(Jj<|{(;WYjjE0Fy($P*sYhp>DBOfXE~(#=0e!+6R@gN@$(F4Ln{u
zg|vwIG0Xt77+IGb?eg*9fs1yF7A{F<;wwGnglh0hnO1<DukZ8}o_UBo>SL4J#u=c^
zq`owL{G76dor`aYQHRS)*INYvhx4_X(?BT3U!lqNbA{_0{m_O!TLU$r()snulif9~
zxM$f`xL4K_6dukfg$_xJ{u=C?gO@MeQhp_RSNd_hP%W|jKJPR17q^wHSWx%NGsu>p
z3>uEe&1l}fx7Bc1XeS})`_@;w&ANhLqcSucgIe4E5>DMw`22n+3XVO+K`h*u+G*(I
zK)l!h==tMr`dI#qk3|Mrq~6A#-d=(!kLh6Vnn%1<QRMwK^P&ItCM;@7t34`=Hk;$}
zI{F>~)AqsOIRXN_LYk3_`G+Z@a5WK$J<`{{Q!_?=EXE7355gOElGU>sIbnFdES}?(
zrVr4cE<@Ga0}V~TOQmG@kswI5$fB5!MF}#Y2n{mqzE2F$1sD%D#Pe}{u(v!^tWGnY
zm7W7pH%X!plzfeTNV%|LiXoJEO1+U5Itv_U3@obQx684PDaOHhj2%h)bZv+<v2Xxn
zuOl=lQTrSP-!H(qwhyPx!{1IPsc7vxqEJ)v^%)m_`hCZ8BTWKFnLkF#Wa?@50Jky&
z{%Mi;`MVhXJ!cly`YGq1I;9Oax!sw)9;Zwd>;<kEh~(RW({bX=7KUp?5J=fy1jZe2
zjJ`+0sFz5bA{3)MBSjMF#>Z%*T<<mSpRY*YhLewYE5Je+qdutf^Cqs<X+@Qvxdi5C
zP-fUod}RWz6n}wEoefy!SXJ=570wHjAEfVMHfHQ-4>D-Q8Tu~khguG2aHm~p<EgKe
zZTFoa@=F&aUj{B#kivt*a@157i$xalk+!pD75l)m0Pk$r2d$*KeKF!$xtFomMmLi^
zSJVX8*w;XQ>*o-kj-R7)zUNGgAK1RvpEje5CWkc21YGCR;#R_h4>2#JC*Lp%cf1gY
z@5Q~%;CslDO<YgPBx~d1jDEN4&4EEEmbf}l$r;U{*#SY1G;$WP=vQsqNeu`aORN$r
z9{t7|8?N3A#J*vuM0!zGNoVX*8X-AN`QeMBbb>ZPU91s#^yJ!o7(U-&NX4)%THKQ+
zR_T!Iz1Xz3^cv8HX?0<%_u55=XvOzBQ%jQk6KyKYNpUFu)G^y~@OWkUp#34eU(_(j
zT&spSqI40c>zg5GQ=f1h4JB4qsDM>-zeX$eMzf=}OohGLE+>cHCffAUMUa?HU&*f~
z%e!B{x_kD?)A}d>dLm1SIhr(g@dXH|*N+U7d^?4s`*E`dx%=4Bw4SVMGcx47IO!%6
zJ)o7bVB?^6M?1>YE2HQXv9#S}Y9i4i@~P?4p`-73KeSUItIga=lr?kkr&NP!7Tcy<
z6VADCKoFuuJTZv+?4Z9rn<!*hxm&L1u4+_(a~dn|**1PQ!SGbaiXp0;ipGB{@cq0V
zZgv=U=I$ZA#p)cexEgj}?T4CP5%muc7M}Gv`>1{4jnMFhZD7FCcu2XtZ_c*Znwk`M
zcq5PjGU%?kYB856#;JL3HQ?YHC3R0BVz}B>a;@Rqw$wYOx0nP*u(-JHVBp6(pOiEO
zx_(dylTQIcUhxuew~$Gy_D4_*{z$MsOrAT{iptc*t{qdd7qPN(4yti{tQ<oi%{PHk
zj(w10U|DmIm-wW*X86T0cP6UEc}3q`sU*(Zl%eyZr)-1fcRs14l`GCay7m)9{XW~B
zJy&Aa*u^__HjS$WH%C#^DYlY+5VEXwcf3xcF#G9dw_y9_U`|rVNX>)uP;-9zhePUD
z5r!v)^Y?Eb#=m85P~%{cuMSWnfO5$o8j7mxJ$1gSK9dw*uNb6o=>5QM=J|;tF;mD^
zVV7@=rOw&B39@=V4Gv`QXkfpHiaEZ{5gZ+$>AL9Cq`?o1xj|MlS7#`?tLcBcZ{AdB
z2ltqj_d>ulR+OlPGHb9<)QXM99u&wlszk(wjG8C9!VpKL$xT9Ie!Z*PKM!9WUtN6h
zsocdcHtXl`$s0XNQ45pKun$;S6OrXvg10+op+zb97>De=@S@-Io#oDCV9z*%jLX9)
zB9EYcew*{awr}0UT)OYCTd+iUOj5JTu3aX$HQ~h_rf8hBy?;o~WDt5cPJ_`;Z91`3
zvN_7<`PFpe`D!q|*YnLY;_<+q;ql=zX3c8<^4549(1WvF_?Vh3M%MlfRr%rmr=<dc
zyHj3!OL4iN$|bI6DK)iM=+p$Zr&$fgv8+3;Cek;-Sg5t+*!8c6@t{0WnN3zDAYM8t
zOfi{AIw-BMPWD`_N%pjbjc(ar;9^p#8*#bZRg)5Z^EuzQa|yLrbyl$%t{LNa4qN(S
zWi3bD`cUXa57<%uIhu7eEREXi%f*OX%3F>eFSt!pWpAizvCo4$JKfbDaxrd*zDPV_
z8||Nr!k3LRV84@!Q{prla1AT^=ZhOmzXtp7IlJCE<~^0bm579;rkp-m_%%QCmNM2J
z+3b@C!m>GpxM&qJ9B#Vdd@NVZXt&cLwN{*LQ{ZpI7_|T1jZNVo*w7Ia#itbex_VAS
znA>{WyKW02d5*STU;i=k&i1ZYk(R&((O62}8U=Be!=3znbaO%ase|VhkE$-j?9NNf
z;RmRLVto}ukWReGQ3wu4dbL%JzznAi=(>8eOuJnr#r($9R9qtVVrfEnx~*gbRpL-<
z!yRDKKGZ}{?7z&ktFPj)p;V9=DKz2FE~duL>+=oQeij-dDIn}&*qxwv71J$K3N1sw
ziw2OU!yrqauH6Z1pTe!A+#tqBvH4x+bHO0;)M!!X_bl_!)ULpzb$K)U7N+&Gn>i?~
zWKZjcUWUa16``LC9k6pzoUo|+?eE?--!cV;(>+Eq<9ZD_=DCOpuh!LmLwvzSLzKpT
z!%;n{H}QIJ#XBz)L&mD1(irDwwy?<`4C&LA(hovCmIwIII?Cgx=GtK@S3y^!42501
zBymQ@Nj>o&Ck4Z)ZH<y!NjSZ|9DMKcmS>TRb$F(TgqpDMX#AwS1E$=<MQumJ!q1hF
zASTqSUi9j7%CH^L^$g_hPGD5bXp*omYqeOMwAnp5?<O+*pt*NJYPwlYE8bq?G?Z)b
zFQSBqF##KPzU)sRk}@%-d;NXCDTu38XV&-2J^aIhN4dBBm)#RkDO$9dtc%_7AWn|k
z>Dd6iOKkzKVK{upJ|Ek%A843{SE8ajk^FFSI{foqQkBxxuH8vkWX26sd$g>z3jUC`
z_}|{QN8f^kb00KM`E%9ZT;q8C=**$?{1!>NmLr;zXZh}%iv5pk@*D-<MCOqBn|@fo
zDVy@?X#GhpWA)ue*gzISKs(U&PL*(jWsmYs9IGwAPb7A##q_PkTu$S)uvL$#t^`(n
zIO>kV3aqnF7PRwE$^hbUn|9}*kN6{~Zz~#}Zyq$Ljr*6%vbc2sW!)Jo#l1a;o;V8A
zj)~^;HpgFVdYI3&s-pBr>wc2nR3|94T{f$8uPI9_Z!3wK<zqt<=3u`mLM()H8NQXs
zPKr79i|`q9VjWVSGOCt+Q*D>-bjE=lBWESVbR%YpSHuLGh*5rPH)NQ2c6h=p*^a)G
zGIIHR7JjkgUElFZY5$U@hUD`m6c$y{y5MrUPe)e{i;VCd+CIAnTDb)+5eqW~NL%%z
zXsVXK_i;Ff-@9=UL9GWpyreOzSevyzs^K-wO9tz-@Qk{ZvgVXsqbC4h=wMdBtOzoF
zmP}((XM4H#4mHqN(~GOB=i&uTB+eD;-NyWmIt(w<mrsbET6#2xNTmrN+xk$LIj!!q
z!e(5mu_%bZpUV_9%E#N^BDkD#cnu8tMpOA3a0tJ<GF|DX@ZfiTy#FDzWR~gL20J(z
z@mjYJj7}3Pe3A&U>c<>i$}dkR+`hP3c{7#MlYN)tK7VLOni|g+*e-eHcfvl_Wl!tD
z#Dg&=WlPSSQ%}@$MTekEyz4ot?rhB!dPijNW0#-V=)oV=vTyC#(0Z(piu%kwsq(p4
zTLWJ%x5-l8TFv%X$v(`D&E8l>2MCou6*cwQ|6HU1A9iK|-BpX^<x6EPZoDtue&>7K
zjQtOj>w-?(KNoiP<a?8+-vWl5Xgi2xkgeioz8OB#+4TBf<SyTyhPJ#MiyFR5yoV7S
zU0xMixZV`Qz8<ujT5XQ4#7PkGYi>g*3#^~?NDdabVWYhoUp2W&+(*167rtynZ|)z#
z?SkorZ^Drvy`4j<V-1Y>SR0ib@jV2i(V{WCHKRmqK!`S<=;#&suF3Q!_Ua?DGgh&}
z<mHuhu%3f>*_T=Bx)1WC^;L=o-D*d&Y<5RU+Zzu=-STaEaR}-qh}C<d73bnY)+|#W
zm5xS~a@q9Tv4^x>8zwhQ3ZP&QD753l@y>2NqrQ;B`;S|v&2`Pwij;O~wtS2*)~8bC
z#-wk3Yu8SX2g^IsJ46(5mF5#DPvyEy4;M`Fz@1#wgV3afiLz=!`JHgGCrE!%^THoZ
z8P1R<Dlydd5!qUN>AllJ@FjUypPLNzb$c`0?Z(Qd_jSt&^9`C<sD)7cG73?gPXG{u
z0XKU)e$$tNz|b9JJNE@AK^(ahqK&#Gygej!6i)mHo~U1b@lGksOc$9@KCQa)8dc`f
zTC6G%sMKHN*R~khP-q)FcW>b)Ifo6(5fu*i-1afO$I2?zlTJ(0<TCs6IC+aliJ{=X
zwNE^yn&s~q(rNL-s^IT@NY8kkR#4t8kt#>h^i7N9F1PUif}<11ON2FtyJIIsSUYDB
zmYS&wQ{l^#M|xteZadw<-NsHYDA$o(3n3wU=@l(C8B5z<atb#MxS4Ih7*^9Lh9Y1`
zw|Q54LbEe?A4RHecJ_F;X}0mkW0`_jdYJNj^OUP&1g{}YNV{JcQ6x~^wN2`i6ghF8
zzrW2^e=&YBgR2?43D1Zzgo=KGAwB;{ejeiYHnbWatE03GtuXx0M`b1o)+)h<=rJnC
zF~N6gd9U{2UG<0XW3Ur@HXCG|IF(pOW^0&6BwKG4>oPM)N;p?)SE>7uEi1Q*ZK0Y%
zG3skw9o|z()>%WV^Be{&=gtf(Zti=3RJ}bUEXjsBx~9{T9<*%FG~5s=vcXlB)Ob(Y
z_T>!?a(0wO3foJt76#I&7J2!1N=D<V$C}P)oQTq|X~Q1g>s8b*fr2iCTn<b%dF>^1
zZN`>}hQe*f=6#=#zK$BP$GCToO}3VPG3S<D#P${5su~}Zc1DT69#Lu5wBVA*y&5o_
zw08r1E>i8{lZby`dq(Z{0`!`_olO^N+l9{9R}|x%K=QVWs2k3-pb7~mpuCu#xE!xo
zD^tD_|73UGV&Emb0?8}DO^+h8>M^tXd^e}^N_+bGf<~@2t>9axf#F2i+K5r^iV(=D
z5FNd-lBHbX09TH@9zn?#ZmpH2`4PLYfI9ybgeN^vwCJeu{5po_puQlC<vd~aHMQov
zNE|*jU?yM7`*!G5MYcZEuNNbNcW!Ee)p!3MQMNH!Yn4bj<UHSq=`yh85{gU7`B6YP
z7a~r?L7Pg`ZD_EPw+PFoYoBlcxHXNF)0Cwdr1W-NHo!;gZm2|(qRg8km<K4}o^BbZ
zV5Ib{9`19X&#$dBCwNRiP+9#;>eDSQXZf=1Hh&O?i=el~ouA$vekg|d#;r6vK-NPB
z9q;>ajd<GLiZ78J$>1;?Git{OY~*7;);*<jEryDaq~DdrttW0JI>hu_OZVye%$ae6
z;z3yPlev~pWY&Q_x@3Zs5BlRrTHB_L$Z+U7_|TFqro-In4z7-;n$`4Gx%d{(YLAq|
z>K~-BZw7VYWry2GH%Pv?E_KI>hbU*hwM&*lyI+$3I#*B@gG;*vVL3YKjn1UDK-x?N
zz@1#PX~#Zo=T<KSR9*`zCtE!DyjCAdxFD{HOznyD*h<OFDU4e+!;*BQJVR~<gp2<O
zHd<k~PkwtY&9N!;jtYfcizt%_(*rgFw1(gd#Z_FFMU^>1VLNI<xaC_%w4>#&xP2nH
z%QI^SB?oIaSlp6^?EaPP$2>g*vAiA*b<T^GFhml4BgC<T(bxzHN6=k=B^sfGrw4(V
zy_$trcK?YWa_4*n1)GgbaIiRXbHa>Ffk?|q|BvqboZI>arJ$aI8GJg5MdPxax4PMB
z9b<7Jy4DP@kH%YVLabYc3_{Lb!Z`^xC=agvWhb=WU!&s*nIh`&g3hx0vqOe7zKSGI
zK>0T^7OV+&y78Z-kbJBX-4Mr$>VH#R;Cq};Hs?-SeNG*lfTg;jOM?&^Cg6!yse4OG
zfKfb552OFWCy*l#r&6p#11vfT(R<Hf&FkM@(qe0-LR627(P2Q|SJ>!95EqS^6M$!R
zw$A|ATb`RVTyQ-iXXpO8agj01dvLx557OHWNBaqfBZA>zq9eEY(%BrEVC_RpK~4#&
zDrY@GzG$t!k?%#2!{d|GPZZ3Fxd)4O<e1m2s!~;O=vYsE5L-ILp6CtH<FLfi8J)qe
z(;Okuk1uM#FHq*0iKdl2Qj#2RZnnLs6GTRm8jyvYHeoXrRLOecQL>y>{FTmqu65$u
zn-KgS_e$^dn=$Waln1+Op-1xby`g8()d7xf)f^3zGaSeAEZgUdiYaLu$(a{pgMKb+
z>Q7&sR+&;0Y55jOjx2=i@=3B!7c}4AMxWx%O+xs*wl1(pcy~HS8yO1+4-jBfmM|qI
zLD`&<z;P<QJtGuir>{31AsjD1JNn^0?PU59hC~vSw~e6C2c`Os(Z&q>q$R(N+6R;B
zA(C4kY1a+NK6Z|IGLE(B4H44RI@4J*F`x55&%i*ySFpE^hg9V`4AY@D@4;7_)2jL=
z6Zbt|W%1#*pq}%+*xhKM!2CzrBA-=@AP4<Z$7PR(ZPfImUl{};xjkpcwGkTgL1`H9
zH^o#R)_(mW$Sw?AXSO8nSsvoGnNXZ7pcdPP$IFN})BRrlK~;c24ajo^O?OU|X-X31
z4<L>=^V~@zhZ1TMucXkiqWLj@bv<1gEBv&$T>mcYz*3y3#(JNY2_i~E)Io@VO_9y(
zNzRCTWrVpL=dF^e?e}z1>2zvz7;~Q}im`M9P`^YM&w&2uT~JHd<kBZpF8m>+5KM%x
z-RWKQhUv3*2)==!cjzv#C3+_Fba4^I=ebJCgw5hnW~p<_D68)`Ek}wmF&%C8-`iK1
z7=KafKCswv9brtKpGD)5=d{BzGMOM&9N@>6_kF|OAU^lfVkeO7$C`_QuY15O84^<U
z98$Te{L>$!93<*lw$un0r^?Gq&{(OKTm)2WlRV@IJdOZ7laub3X(dBH^z~B*yf>Ko
zu|9u!kdB}fRj%DMmT@lyXp(lq$A$>x90&us0PlsX+K&q_EkO6QcHGjXbihRO<|D}r
z{sEa$Tr5bot5u73Cl!qM&Ry47G3y--_Ey8Ha1l%2h~n4K=5$L7M&&e?j$F}paX?vd
z%&JultEyb8a2l9H3Y_CmeHI`BIIaLY`;INE>cgN2j~8kv&V!o6(gm4M9f>vRDxB_$
zJ$>$WoeCJi1HLbOesEq2wUVekA&e<xwwP|Y!60Xv$NA+<hjUEbf;9p(4rM`c=em#S
zDRB*0L(;dey;IVw&recquj>{uo9@Lot`Iw&A(D`(3j}#upIZUKm%q|#p%>WkFb4*;
zKX+s5);yx%`HxWq*zxWCn02Z9VS36poOCW+gCjAFl<t+Fk`#6F{$LK+X0h5lAKv0q
z`mpa(!9|+3Rk$&JfS@;!ln0k@`wDGkZ_}u^ZyoWopK^t@z)S9<;`Z#D*+L0watGO8
zu}n=yT0^Gy-@?9jQ@F}+?Yi}#k-zXEx{ba|P5odd3Y5~u4k4{4D^qj_N<$V-k<G}U
zFc5fj&jkB_w9}-(&CyW(;dJqqKabQxrlQk-cEtr7!{2bJk-ImBhXuF2+m7k_H4v%u
z)zz?}AtXf3Zbdd=n}gz$iN9+1q#85sx-d*)vucTD1i$;@GWj~qtwLb$xaeK~KA}F@
zhvSP`EUvU;m`Y9`Ww|aTUv3wsLJa7hCl`C*OgxBQrk7AX)p~}ME{agkfg(Ngpi^^w
zv3yiw?pPhJ8bkX?HK24uk{b!&NW<zf`9Kb}0u)5+g&r%*_0e1j7fwp}MM}Q{ptps7
zYt^_k+lGb);|RZL-3`j}-W3F_Z~C){A+UI(DLdZK4r87wB;)@Q4Gv4Z5;$Lb<xJ#(
z>&l0U`2_1;AyYg0f_7g?FpEw0p45IcJ_mwe#mf$aE^iWk@bz`~L>}2wFOZQ7ODXny
z93ZK!iYBA#BmoU2byp)Qo;!rQO-HCe`=&jdQ3BF)0r`<<Bu(vmbq+ZtD~rin>kQ&A
zgNw%v`iM5+wF8ta$Xw`TEZ_4^2rX>L4Ewk2p=I$Fc*(Q8yi9r_Kym}pzK8}SwNlXi
z$l+R%S25Qu`1qRrR5c#Z@~jjz+*D#^xx0zivb`mIlx>0j9ARvMwSh^f>TF*a>)ueK
z^wDC!vf0NGNEg0MRxWixCjHs7j5{D}Y867Z4cSvEl4Y9K<MQJIx}$^6GHl+meYK&j
zrlm8h0c&JT$=*lLhR-B`+XL^IM!SlW|K{q{1HqS>gQreoynSpb(x4D`RkKOZI^ZV%
zz9O5ry&A%gMdN7qT+uR;ROj;PPN(G#BB20fkK(aR1md`5Dq4&Z7o#M8tadc<DJ(#N
zno^E>FbE|TaiT0??Jj-uN6@8_)AwHcj}M@fi31s0z&%dC+Z?8J5bIX^(52<fSkYMY
zKwfU~FOAJNaYU>$GrZIYZO4dcQ33^AA9ncXG?s%Nh&`yOg{O*lPo*YgV8a80q52cn
z@dD@~xPFqUz9}0kfvgl$QLl+l?o+0$E#<rS%6m(Qkln9J<3~!v_}(Lx<zw{XzGU)?
z<0ApGinLNvD|w>C007SNLmlx60X;w3g0W@=1|=KptW<orW_#ybHLFNz)I=r|x_JBZ
z0h~Ll2%76$w6JPteP!Q|Hx7?RYMYr_&15mLv`-<0y#yCqUnzR7FTV0`*r%lMye>_2
zVDWKcaN;DvtnI1ucY0DF{6xBsewb%{ls`vI_nt_Z{H#tLDm(xEC0=AAKB`|}^~Fxb
zE@`a_{l2mkD-aFp0|z|`^TtxL-c_0I32$GwCuc2LKB+n`H)qahFO3O3D%4F@R`VBQ
zZ~BqpS1x}Ak^L{+IW|hP-`$a7`7BEz_JlQmsx5rs^EE2E_0J{!kkdr7QC-KR&+rn0
zk-@F!KyRA`df3Pio#my09HY<VVe%|N_cM<nvFVG2dWsx9vsHceSyM8~2YTdRS(;K5
z(C}?xnD@IHNklDu8%DPd6aQSOG-&_|>Em4da-$8GYd*mglm23_rNqQRSyw<ial{25
z`HY(XsIagT&_O$#XPLoIoI=>duOCIsif!;3tRc-BM<7<gVu%tpa-zsjD!^H0G;@G~
zA7WfHELXRt7@ed-<2WUhHwy3jF$l6`FXjt*!*(9B!c^Nxb))wiL;3^$2afwK3bkpA
z@u8@%8_-uTf@a=d3mQn?*=GT$0I;GuDfxqb!vjz3)fSTCs85q4NEp8LZz>c*?Y(E~
zwb<nad<TC<Jk>UP4u?3GFxsAlRtaoH?=PfQh^84}>U!%7x;KH+19Bnw#M`E#mSIua
z;(`P#Elffc%b}N{c>K2E@r55B1kL*p6#)Wv!z-zs*xyr*dw0M3W<tmjsny^^1@hHA
zkgpI?&Qwl4B_nZsIUN_-?{a+?7{Nd5M3*mB%7z`o!Ihu5vOgZb*m5*@T{VeXRe|1Z
zyv!*1jz5RM!0t{VTIK@hyB`{nWQHdm_Q!EIo8|s*8fc?rbf~*^1-T@N_H6E#*O95x
z`Ve1S7)LQsVZGppM70?gp8Fi%P+>%r2&C`aJ9(7tI%X~K%hI>9`;)1Raz_e8+dVD5
z(G5!#mg}KiSd@e)@K$uylW;W=6oB`+Z)R*}s${>szxM<wo;6Z9R8#@+^WSg4W_uu_
zr|vwMorw}BkHjScF!7^rt)M<1>=8O<j9SdEl2ok3sS9s60?YaqcPbTv_f6!iYOflJ
zOT1nqIWbb#Kal*iyv!WF)%fT&aGScq`z;iZ^VA^f`F-b5r()UmD{Ly4o`=mLe~U$r
zTEjI)ziOr0VcP;#-FQ5knCG{Hjp!uD_)VqB=Y_*0)HNaBdRwH@#Mx~8k!jEa$h`QF
zvSJRfmV8u}Yn?)(uOSR!drl<yYq8PtDxj@ir*Q{D&s#9;@Y~bs;Ut%eXzB^fDx)2$
zKQ`0Fs{suqlFLR^2nn?Yn;G27Tz^L`zzM}P4sfA`WpE=t{I!m*t0km$S+OsIUMSW4
z2vayTH&@#BeThBPcduA9uUD0(a<=9XQnX08K>jrHXD@j%lyB&#$2X=FQAZtIioDOk
zy!gQ!UsaQ#MfPt19Zs_I0r?Z&4XaDLv6-AagglZpiXu^=;?<6&J8mMicpGDP6WC|o
zAKv1MwL4X%`O1ZIo%zAeCawF7y=O9V7#1$@0(zZcVWW-o9HAK-Zw`mmZk|F0WJYN4
zIIp)UaTuhMFsRNk7t*fL$Yrb4ObH;Bn`;n{u{N9)4x{wtRV(L_7m^S^N(o*S4perx
zb`J)$$BNG-P9VWp6FpLUZNKM!SnGTF5P;P@|E5~y+FZO*S)<;wc*M=sgTrLWjHTx*
zm++ES;$FRmEJ6`u(4d;;E-GUA4rcMNS=|^yi>_C^g7TZ7BiWF^rGzwP7=mgB(%j&;
zzgBCo#4ToxuuUJY8B4hdyWSjfxv>Me6fbMwtCy4^sB<Rg{qCgpyA??pCCi1#bcQ#U
z+w+M6X-ka+XkSflY>KjVKd2lCRl1%omzhHX0T$Gggzc5Fw^0Z8*nJh2#(UqWj;+4L
z$9A{qzGx#zac@YNu7Q4j_4V+(GtDQSb=WF|x)-m|B9t)aEcSqDGbckFLUzD3_~CV4
zotAH`1Ix$nLd#a=XvrLdX)sSCd%z+LV5I%H(!1fBz|5b}FkoKhXl7ph&K4n(!-!{m
z{!u_i0?o@fQHHW$3j@M%4Z1>(iRk+}p+>>gU^P&v5ug2aqPk-FxnA1rZO#((aYOmo
z@@&W*@Ar>}WAQm8$YI6o*tQ*V$LJ?7^v7-zM0^6`)0Mzxl`pJBH{=%dvqS``;z#~0
z?ApfAq59GE^uCjWku=`#5KtGrR0>oI{JSxo6-DLzXQ}y1TfU`2(4C?4Ov9e#ZV|r!
zWRuf_@a+(}E4Y@&q`hTuB+Z&;DP~rQnOP~R#LUdhtP(R*2}?{RW@hFRLy4K0nVB(1
z_kKOQ-8MZtW43?NJo1f<a1RSh%XrfL9Lu?~`Guh<;BoQY-knqC4Rp0M7@C@Y7+*Ny
z><nD@JS|&;q1z-$gB|e)XKS*lr3(4C+Y(FPB9~2itw#P!Xh16zo>)y2?_*KmVZH(M
zj#bjEFa`dteVu4d50P{bPsJrR+N-bdTsc1ze^4bJl26DcgX7A{SZTg#(d;zXq|lCT
zdmT7$j@LWA$!89TGkVlH^ig9h^K_PeJKvl$s}#I_zx-n|fe`-F8`G_JuKBQYd=e{)
z$<H*&t9wym`l8_cgvY`07e>*!RVjxFyBU25%%l)WBaY<f2~pH<hiqi0#1?<_22Z?(
zqMH`N+g(f?q3B~ZWnPv?QqaBb6S>B<iQVAR=j4fnaNCVh5w=3sM9k<)Q`;9n&Te2)
zL$qRe#)LnU?RXJ_k%n-LSjhdjYkV;pohyJPl99}$A>>tQNCwAIy3rl(j85l)zEVY1
z;l9h`pC1cuWfO;?AY=9&t2u=e{?k*H;#Ju%%qeQ%q$%<>WxqhN6%EeH_@l|SyaKp~
z8jZH!OqOg04Kt2iQta^7Zxh7+*4gs-NtY<yO<b0KcIU=#etFu$=%i)~CE`d{DRG3=
zjP7gbNql{wyo5bGd9oc4h{EBJezXQqzw5iL{q9m-t8W)#{@Vc}r2w~20m08QpR&6-
z!J(IhwmtY3_iY)bEu}RlnU<)8-jyP}X8OIBvd7mu-|SvWY>?~wC|vo=F+Tw2?NHBZ
zf<^VrgFxO5l@At^F+yQ*LSkO=^4{pPe@hJ_vJm8ViPsJl#Eyl2mpSdY{1x=Yj`W%5
z?ZW(8LXO%*WoCfHjlKf}t=lTWoRgvg&ZO1htY0`4@^P770}&WLtUha0IAPl|5St--
z|A-EQ&cXOe?j1++{2-}sSwh&~sH%EX1Ld_LCU`nksesSifb+Fq7P>;*k*Q4fDuHyX
zL$8sy^A=&Va={5WSKxP<-wyiucNrHB*z={>i(Q(2o^%1GGLs8o?wrRw=y^!{&FgPA
zN!MLkDKy*e!>({PV96s8qz#_p<&s;~XJEOcMSAuQb`<*+RcA0lU@7L(6T_tcD8-#y
z{T|N{(>1Y-aq{4U%f5=_TeHIaduzHEa?GBKYKH5%??sf6rAm+09Fy1KLla@BANb9o
z7Yd{ri|lDbM$~EeiCyY0h4rpBquN}NMX9V!SSo>6fkYo$mJttbCZR0inUtzBs<HRs
zd?PZpln0Y&bDqn5+uX)lt*rf-N&w}O+2q3f=d<u8k*fSMpo8#)Frn%*RZYlIyy2?*
z`WI$#?UkRE;QA9U1Kd3n-ne%;NhpqI(MqXrjpSNNbt!?Hqbnc=*@A5<`i904l~-h^
zydL%Gm?50|x)8AKcqwS30}H#NV9wxWlf}ftxyazWZL9EGuHL;_)0Q$QK$$C&f^D5*
z8?bz1H94fWRCADzlhi95sL`<+o&x=8+1VwsRm@jie6k4jbMzPzl8g0`q%q-#&k<mV
z8kO&|?^}Q_E@Xq*k|2IhrBZ6HwFmcWQZi%WP5%COp0yspoL`9=PnW1tUHcFjE;npV
z*FaaBX&OWeGhsIR<LlS61R@F*@a}|TA}4!4i6z9A_)Bn6?J#r9+cdz^m##(Sr}9P`
z(xJN}Lb&;im~8Nlsb24w2s+zQ@)AejymGXT#z<6=&w?lN-RG%Fd+>SP?zXf#q`7>~
zb-(L4D}&SBNuYCZ#SXG^ce|mf?82w{Qnn!U{(Mg*Z}#B1#21pEVT}f*Bea0mU+l3M
zW>-C4V@6HDR0N|^ma3F%C)2&jVna(fMfL~u{xf>ZY75cF+<woM^+>`%pmYZIL)QDx
zDwVVRGTsV7tBoNdzZ~p)Py^i|24EOORKz+ZK|Hu3gM`aDy$8=7vplZVk32Y+;Wmsl
zL9iW}z5O!0yrAaeBhV~<IX`KWU;E#~e__JeZgBe3`fTQJ=ZKQ_hC{-#bc^Uo&}&*n
zE1K8#E&9gy-xsds_t=>dyo=JG@U{gR_vRO;beJQ{2zhXU%M(^yMNF+x_c|g;IzHN9
zjB<+pMQ94z>zAI^Kbl^n3%#skVN8q#oYxuk)<p8?=w12O5!<qtVjbK)B#+nQC-J>*
zhJQoAS6HzBBEv{P?u*c%c|@$DgZY&<x3NYx*r2GtV!}0Q`x4{9n29v&)lULYIqpj`
zvx_^%cHc{A3O1^e(acmpzp291eCa<-V^xN(ppS9E${lzS13u~)OIG(xdM59eYT&e|
zj5;3poH>uN{T}lv9yprsVg)9<A7t1{U?m>AEH0yUoqo&_kJ_T=orcw*=N65_&jfCp
z<8{ZR(f*at_-h-&p$$i30zu0-_iR*hK7}V;=l?0P>Bo2&*)iK2Q6XMJl6+;oht+rC
z%n44z>W^ZiUe2K7*|^w*B{UmZ<IZoaBr52rMK|Z$biFiX2O^KKg4j-ld*|feR><HA
zW`}WQtXe{ZsTiavvX_@cg>9FXujtJ>YU(Nhho`UKlOFUw8Z}stOa)ft+xY2dqniRs
zY4jm4^_LsqmxCqW9lCx$a$;(Y(e_-e2v6<z9vmh2`uj6%G~mTM$p)RFi6x4fpOKeZ
zOmZ0n3Yy;A#kKm_2$W+iG_9<?A^sJ`kJ^i&3(gyM?-!xe3~Qn>n_2AssH;hcIiHDu
zyfr5e&q+11ZEe?2=P~GR`_qf=zd}d;O(DN)4b^vU1`!O{AjIA*j*+(lgB*oM_8lm^
zwbAxK&ue_Fg};DcBl_naEOqH*(|7i`5M<J)@{V}TG}AE6240$B7ka68_m08BXRwc?
zt-KuSixi*&+I%&Rl#Mas4JUv7A577vqQQRK66=8C)!{n-u8}CO8aY8QJ@<X`Mz6G@
zgi@c$N&$UbXF1XSc4OyP5(=gIi~;oE-GtLlL3V%4VV+1=(Y?xm!<}s(G;3#f^E~kY
zh?=GN?jbGdaLtBJr6$IqkBSf<2+R4z^W=9NO~bwHy$uY&1~x|^>_}qlR_q)^<Zqw@
zoQM-ig8vj!<kwhT<|UPq#3<pf=s<rbK+@U|zvDhaKbZG8cos1SQGCh7nnL(!Km5|h
zZl3GpK7riVwHaV|oRV9Brwczudrt^>J^l$Le=^H!hJJ}=idG1wnSsOM*-<agrbL?j
zaC0vFHaIp)mgVNN*1WH*va>97*%tRCS0-_*LHdByW?>V>W6fW;O}3ON96${RCVWh`
z8!(NP2)RJTNN%^KDy>L+=sigy12p7W6$7PntG$-%ZFpiv?sEO(s9-&*Ca=2Qcch<_
zQ3%!SZhysKISnRkKkx;9HGlF$bZc-Ig9W=PFKE(1Qy+^|Pk0OQ=p_U%&V2sBLf!M%
zEZAIytxsS{aWM4N1jNBRQ0zW3v{G?s3`eE4y?Dg58WWwVe=UZKc7ULZnq??A+=b^j
z&a_o3v1TwlDyhn#+rM1THtUFE%M5SF7x}n6uaB5epWI9IK*e11mlKmce~~D-hF~18
z%yXfGkPolpV+#jW(@Z7{_0(yAIIWe>s9NsLU|#3%qg3rEDEZ7Cc+*olbjmB^Y1TxH
zOpA?O7=PxxM0;jH(USx!79jb>u$yYp`M2{Ahg3hTr6lr_c17<5GDe8`LQy2*JOpBr
zG()gqkKcr0%b)r0@SPgGJZB4X#QK&LJ#1?Z?JXuQM;%bFHJO2c_U`SyH%1i-F=b52
z73G<fwX{!M-ibylN0_U?<JqCK_ke!SuJd|LQrk?+F;Zw(X=&D4kV<^$jr`dd9~6${
zZZEQ}#$@bZxov&u46eKs(Lq6?_oZ514q`-NJZ4{O-RWC25hOc7>KqeATO9HhiL%6V
zs-{eiS%{sEvq>XM4GbP`uHHx3^&2kF1L<ZK(Tgm~mOG1hXUUNNL|&$MZ|wTjG$T%R
z`Q$Ul!)2vDFU@)fqh1|NfQmd}P+TBQl8FQIJmxnq6_NmT;U0q`V7G!Ng*;hEo06|>
z?LUT@$HRio)m>k8ECF){{QBC4l>*!-q#H{G=-gY%277+Z#BfcB-CQ|nTDDn<=)*;|
znY?O9eM<S>1;m0|CrA2C?lknwBc<;YCuE<iveQ9l@(i-J1+n8%(EVv$VS4p$uWfh6
z{7RX8{vf9<j<~Q1qMY)pY@O~dS3R`hEBN=v{-u9hckccJKn7Stn04)jl|p$*8fm*}
z^s$r76bFM*8H?_Axa?I7?X@w<HvzMPPmor!U{al#w~7}S#;Tco8u~chFoj?M=NW+M
zK1!WO<8u&i0&Pse4`V*2p(;VmNjZNZHn8h`O$<O;67mVbE1`vT5&w)nkA)RjTgK|j
zvDq2EDRB*(CRS`y)Ptb$CM$s2mK9=lbYHvQ&&#fJaO&^??mEfHK!!ug>ap-9X-*DN
zcyT>5tde)51riT3g!XKJ=As;$%HqG^j8cj^v%0&7k*VCSDo4z_-O<;{-(ds~6=HR$
zGmO`jQxbyzMm6<I-{KW^0F9@Jw%@v-M~Qwxkz9+DkDT{eu*}DboeJGIheFqP#+pC@
z_1oe<0xdv9=#w>b4j%Xhp`Cwdtsm-(crjK6VOVh-`oWpWP-mId|Io=1wGoIN9amZG
z5~_cbe{d!k-0T*OGIw@miK?p)hjIDsK>WB`f|`5pTUta~?H|T8I0D@~iG+G4VI1*e
zSty;#T+Qc@tnRu&xGH=Rc$CRi&QldyR#<@pnc!a5Xw_@UyHJ(Q<Mnohgvu>NjyU%7
zkQ?&m91u&E-|KI0prixBd^=!zsjEl_9j|M(i?WEknF;d&U9H<w!Y>b$Fm={=Q6S`C
z%tm#^tS)$tY}6ooJ{uxj4Lj+CKXo|h>$wAT2iO&?bfoPf5Z~EVFwK{0lsFFtA?#ue
z-jH}qllMLJ)#D$`!%Kf%F1I-nVdSD2I14oD^z9d#&6`0Kpl8a2zu6Ws?JRlHu%K7~
z!>hkYv@|XEMY@cpsbIm8ZWEm~x>AMd8eyGbaD@}bmcj8_DXNiG{8NAGwf|s&mQJXk
z8jL2_`BFSvB?-)y3K>_Sv|7DR3~M36xJCT{ZV?QxGNv_oQ2NYJj5$`ba>)sZsNjg~
z3mJi&88`S&C>CBgSnT3H3$|mgAgNHq^(&c>Kk|U#`00|+h95Rs(Y^M5a-tMefb(bZ
zzTdf~+9oaqv=S1{oQx#=DFx4RZ9&RfsC#ZL{w{2P7RJ%UvbjjGCplK@OJ;KG&VtB-
ztDMxXUchU>>EfcpT&~MSO<*^ed8e}BN45>A4#q@fd^(y?7m<H^^ixlca)jm1v<vf=
zR`P~JMy4EO{nC--kOPcYT8OGxW7@`;q|j$^W9R1^Zs{V}yr!B}!<T7}D!_7KQto|C
zI3HaraAw_ffx2Qo*ufkzhpdMkH!SsSnF36P{58s&jS+1~Yc%B{FzsI0Ox-a;<Bg7|
z|2^DK&*sxjwMM^fqs2m-g%|PQMteQ(+QHI@x|~}QW6+=h6Eui*ev?lcVr@x#RW_#Z
zt@HP?xNp50@5{v<-c-iecY4{BSK8lej$=fCaZ;LNbwB{>YaqJ!I}Ouo|F>g<G%0pg
zHN=BXLjLf36J7mYYYf-U5t;5PU|niFg6WVrHom4NNKX&F8ajNBvTCsBuj(5JMYS8_
zT-Y&f>H)5i&;lttBusbQHcK{hVNGu8Xz8I!iK;gf%7X2+`9rhc@Ln#+?qFd|<Y9mp
z=ll><`v{;CH!a{ylw)%r8(I1y`0C>l6^lNh5?74_{BRE8Vd;4Ue6hLA2H}Dvz8{nG
zbB%+GSa*wLvXu%X#{Hy0BXn-XN;x0H|IdUJ2-9KBc^#lePjc7YRdnkt#!-|+@;atF
zq~YgoU5iW*N6Dw!R_b~eKQ%0T<Z_p42w8cFmn(5~o_uuqT>*-UMRshfr||iHY7Nvl
zfk<lfE;{vs@71ktwmyp=b%{$bccJn(@H%0k_BiAIGT2rs07IjD!Z{@1@sxH+U}_RI
zMss7z6y5dr@p*x?VzUmrB=PK;7odv0BE3!kq4a=Y!AR^l2pz7F+YZDh3;PFju=(e>
za~Ss495x83T`KF#R$zt*i(M!`u(IDux{@3T{+`UGPvDhs*S!JtlUF57=Jw^CR^J9>
z0x(mx>hE<s;^>MQ%Z9}$wB(eDMWAu&3l+;zBk@yjiHQ4wFApTdQKsUTF3qs6v3S{-
zX<-6#$f<xckwy(Py%M%@I`=rsxACd7zPk^|X(6A!UfNYso}MK`epjpA!f<@Q(y<+V
zla>XdG9hy49fV(V?2|j=%+vl#1N>l%)2e8x;#P=4$Mnk`-`z$gdtjLMqZWGa8eAeY
z4UNS6nBWTI^<qeHg)<6q=R=5V%iFW8lK5pXeitIZ%Mq>gSZo`PT`t#3$Qg9*g7cG{
zk&SWe_P3IR?d_R+aKuGEuJ9lleEoH(tpj-l5wDa)ekJ5G@51Xa%`4aWo)|kp`#A!L
zCYpQNZ<o{g+cNSg*9gMQIV_i_&oERD{0^ZM#de@JaW5H#%N0nT#g24#nnAwqMw=}T
zw@;ak$mPRN4qfQy?e?>T=0Nf8syVbs5d7XH>nTw1Hl~im6VKSMstXMp?~Os>N6@NU
zt6=zjW_T%04)MIwOqD9qclr{th8qU#U8ue-Kc%U&=bu!mpl;9nDw7Msj1V`^7_^Zz
zlHPL(w|H$%UiOimbV?~pT#>LOJ3hBwu2gE@k53r$0yvHbSnr>XM^!=;vYf{_p%~;#
z9bHi7*=OLFv938ncLPIlSVo1^=dF$L#Fl$cBV~&{39t);O$3y+&C{B_U;+mht1J44
zdm!M=kbfkBtY@B${&%cfuKxsh{Xem8MOD>gwY0uOr>d&VUt&;k8zXaP;xG2AxV@c=
zu&})cu?_<(F$2r@@5F51If&W0*#BSDEju$a96-d#LDJ09+`@(U-<zUl&i}&Detm+4
z1r8wPVq{}!B4lT7V@CXSa1|FbTlKGF{BJ?GLI7ca2tX7d1`r2G03-oY0BL{>Ko%ee
zkOwFL6ah*AWq=Am6`%%C2WS8^0a^fUfDypR_KSb(Y-DE&FmnC^;95Fc1B_e%#sFg@
z6KiK1BWDYMv6GRB*}v(z|8D(*&iz-@+RWwudHbvVPr(FWVsB$__w{OPYXmR_nA+Re
z7&!sV0RJ8*z|7Ir$Od5MVPa!s3-|%}Vd-WD_+jtr1TY7fJDC}M^#EW2u<&%SFtY<#
z0;~Ym02_dfnX@y%7GMXkbG0@8>a3-?9l#!7Z)XN@05}*qnc4lj7yoYk(}%AS{p&jZ
z2iX_kVDm-!bp$xN+Pj#2fq?&M{IjPMz!~6dW^4IRpPel|0RN2H1>j=gWM&3%akmG!
z0$lA(zfNmn?_>sW1Gob`0G<FZfR~w*{eOi8{|gQLUtqypO#kU#`iBS1Ow7#A@dYdX
zXN!fHm6`qPKKNf*!E9d?;s0xgtP8Yif=pu*u~>nmO8H-jEAl)k6>jAs4k2Q4P+=)y
zG%G<+A(U^Tg<-7la)n`%s5H{3pfo9)VqRC&FwbC%-ax;qC-p_?`l*$p@$<BX(AcqQ
zQCY|UM?ov&X$a<0PEkU|IS#g8(jX{dg`5zipu2nD#ZcGdx_62Boyn16|K=V5jZ1gp
zn80X?k;vl0Ua?to_fDV$DL_l3c<(!SA;2UBiJiK`urqmck?w>^Kztnqp@YPCg?y~A
zD*;Fc`?ADUcV_@%7%Z?{C#NGZ<S|eTcXA}i&^%CI>|UG`d2LiAXK-02hi~!QZ#9^K
z5t9&L>Vyc7kB^w4t$PE^oF$RM`TiUc+X&cXH<FoHXrTC#P>g-+zXgiIgPuWD%)fm~
z{Ds()ybjrk1EJXt2_&2GP!6ZU4d@3gTmVJ4<?8{76a^BWeGrF%2_QrJM~L|SdDHTp
zhWW)$AJV&n?M{zy5h=_A8bi%NZkSs}C4Ws42FAp?>ks4HXCnE)1i&6eJQO6ww<CbE
zD8~k6ivOGQdhLJ@C({$Tr(_3>+Y4<gIaC=SUTU<y@!JB7(4|(u^&1i_RaQIT`RJf6
zpLx?_7qA~DOooqaO3Yhl_?>mM5527FGeHC{;Ilt74nhzYEMWLc+$xC-asuuSMMQ6S
z8YxCdnFnnY9|A&(83c(h#S#ZfEkS+)E!yAL5eEW#6pldrnf1&8CX@#P21x~iBuVuR
z_r8ROCd%bKK<X)0<OO6AW%gMJbh!8Esk@>OR>W}HuFLN@R0DyXrIv}6<=YF*i@UNK
z#vVAzK(2pkaN##%1`^2OVJUD>;N`6j5O5KyB!Yc<v`OsuYFR+Q?7xl-TiEbM(RX&F
z3-ieujdFd3!Du2m7X<+UMCe8o#Ra4C6#`CSa?-y5<vxW{fUeKLb}~5yjz4`J+sJ=`
z38Ae8>pp%gl5^)$#9SgoBDc`5-Ky~W$UJ9csMHEiz#mPpOz|SB)ADxR$Y{ToL|=$e
zL#V;lUmT2o>VcQF<H1F<_HQdeUm+4h{Sy34Ak#v+l=f1&E*#^QC6S7GeTu-f;DmBC
z<b$=u0SW2r^$|ju^^ua40T-nnDS#=Q+OYsPbYu+?qy?2c1p|>lKyq;?VwnZeJRrUY
z9|1Lt-a6)m#d#O6M**>Uh8B`id4`q(GfWWBD+FPRy2cwF0W%EUf{{+QWx54O(z?FF
z#}C{23C&`z0|n!AdQ!V{AwUK{LzG#_MR94qX2n32wGSD@HGW?=h$ANiVv(~Eze7%e
z8%~Kl*Pw@b?>ZC?9Z!ENJbV6yY`{-G3k?hX@MO3RN5UvZV&8>XUh5qx*DNhL!&)h3
zgWr%kZTOJR4yUfNuAkzAwo;-z$A;1R5pJYU`sQ6$L4n@C;5H8wD&$r1yTOx|2z68f
zx`n{=+<EXRgwt%Cp3*(WTLh!eIi)!kJcT%wpfr>YotnDeo>Pc@*1rQhC&%W&DByRg
zMs%)3l^cdfrL)PA%T%SR$2ENOW>?gTCL#?~?DV4eO1RoTa=WyIU!D7UzALfJFM~g}
za^Y59ir=me^ryMA*t9)f2vTkJ^B^YMyF&(XYz9NXa&PM(T>X%&<4Uhinl$&obqIyn
z8O4mGY+QB9k%<BpG#XiTIPi-uk8uA`&U6!0c+mVjo8k<QH*rUV+@jVFw+#McpyE0T
zY)Q)NH~FO@zeK66UIXXQ_;{b*1P><#B^ZQVq=yyVzLU9AO7*19kxYoC9I{2IvvzW~
z<r%CdKht_&Z`u*=mSy_ZG{KggUR-x?PZkJ#_9=F$l@8`)bQah7>G|$)jQSkmup?Y-
zRwsAlR!Zs{j2!vnYz6YCsIIg^Z76q)4(CkI+h1FrfiSSWx#>1^ZMw}X{<1*|+?(Vo
zWO%>6O7~e^uR)IrN}2&3+8+*0JjCwcOEw(3ZgY>GR0djva;;m6>w_~X!Rf`rX?w?|
z3<cKtSXQSKt?cCk3P`n`)Jth=xfO5*{G&w@2K5QIt=D;Gpx&BzW803gwI1dIv2=_$
zQI_n01>U<6L~g=Zi3#o;yV@7g*Z_aLKOEoiErV_0S_u--A&ZmIydHTNF>gz+PlXXj
zVA5J1rcs6pQDV^pI<K#XD9N0@H{NPALl7pLyJnyI$7b{M>lJmgKUmq5jp6^{Vm<M!
z43z`OO>z%^_y1_!cKoE)wanFbnHH~0`MbG1R}YdZiL%NwJP+Zr?>H2X!s)7LozksQ
zpqbz*V55${)00Y>{_IgJ*gI4}V&p7w1p8rz0&?r7SK0y~b+Zg?7rl}FbZZd;^JoXv
z0dnyThunP>O8c%q96&UCPpHvg#Jm^Wdd(^r%quXcms46LmoH3qdiSRoMhSFc>E68t
zUTcWnl6zqD%HmZHQz&N5UfZ<VTPfZA5_j%Jw^6KdT=l)6t=T?KlTNDaz!L)_;_%I&
zzdUom+L3?@F{G%p9A&u*@w`4waBLJLTB6;$7HN8BGvA2zDEjl;^c1VHIx!RqC-#cT
zfgit^IxCV>Iq~K(?NiAM(qW6Y;080l)C&wD!W*YKFoqP*(eCG%%<tFZ<DuhEDzM)(
z(dHk<ShH4imC&Y|<}~!f_L$ULf6kmOZxu2#Qaf~s)U;n!S5ZgZ7*9Owu2Qn_x|ReC
z;;*m1mQoD}@cM)?s6VS#BR0kDKGcrtz$t0<=G``&tGaW}2a7R097{;VG|!vNt%GlL
zT_v5{j%9+bmYE4#FvXQ1infn`cW5OjPYZ`98@F_V8uuZBa#4U8qtuY;T1!#={^!Io
zY2uMd;^>MeyIaNf$q*m!ae6=f_A^^=fY)%ye%8UJEEz}Fr69PRz%rxX*mT(OoFu?@
zQTzP>PUoemb!_qciv4`>a=s~IBmK_+eZ&UgdPHpoG^j+onyCQWFA9ZqQB55pksVGn
z*KAtExSU0u>?1!*TAH=GA;D_BH~RLS<QPK8W2r6lyd@@wXxB>`xT&?JUGe$O+oR%K
zSy>}FXD;E-s}T$~Yo0aKy-Yq4j#Jf-Y??L;W{Z&Qt?tCvETij;Q?2S2=M{tU;nFg>
zWEQ=It@>UtoJi<-+Q<BgDn5ydj=*a9mY#{dflHi?&ZW$BG~Zu{@zkq*%I5mwGKq$;
zyy-DkLOJQ(c_u6G#<lO5T-@sNHZcu#V#jPbqOjdw6#MGzjHiU<Z)C^{lj93k{8s3l
z*V8kITr;RCMe~)=dN700aV>jpE9cndZ4ir4<!wkA6cf(3rKlM+otF^>`E-3;OWrfw
zHC+6wrMTkKK~;}f((OtVLjAdaG*0NT4<okrB0bOS)(8K*8P-ILwlDak#F|+7m=-e#
zvqRZ3kCI2U+$L_$G0#12lPX*f$)D7&&d^s<RJEJ(RYZkZubrlz0T$JQWpk=EOdnR~
z@_qCh*1F66zTIN7`SRu!-s(?#6l-Mq=>$LCdJPyR{=(T}sQ8JapYTT}sjguP6~7z|
zl%SJ4sEXvxxy^Qb4d>5Z#e^PYPW^C1f)YHv?!^6}&*~35)4`RSee&jXwOF2}lj1L0
z$!S?(`@9OrCGQgqe3{JHrfb3(KC|q@F7kOYc?xGxT?XA{`SRyJj9-o~gC1<c5FKxQ
zg!NBexkdxOx-%u}pA)?^q#xk)C2)L|F>Q8!jK8dl_dbJkQrt*g!Z<(}%O9KHuBfsH
zn{igyIJ|-$zGl->hKGCJ2T2zp1|paVi?<dqNCg>b@6o;W+6?Vq)5JOjq#@HXH}=)X
zj@g;kOF>pkAGshE`rhF~UfQV%IPWYVwXb|ZD6`ZVQ84=Azs04Ok+bol+)XE`Asp&!
zytA)>Hm0p+KgOD&Qc5o`veVh68CRB};}lSctG2$2{_1c_Y|qc6Lcm?8U%}UQ^5c-B
zT%vxv+lPEGq@a>!KJLhpXW1$QyhZn~K6M~$;s@yr6-X=&)sHL}b;w&Z=-i8%Qztqu
z9?+e_DTnK-+&z%0<7hJHgz{T9b<(wh6{vdAFF63{B`#ZLq`^x5I%#y8&-7WW$y@yv
zOy13qX@5N>0t2VVZu1^F-s9zGvm&fhD%x_q>}Y>I`0=)ZYEce>&Lx@`IQZ5D1Xt;P
z6X60czx$k_+AV^V2dz760V}IOKhg5ZK}acwlG&S`?Q!>hA1CwR*cv`@{pC3)%QFhc
zT<$8NG)i|m7PDng_5gqU(o=uSHL0TKS6B)-A|O|*FtxY~nd$1xvJBKw#m+M9z|P{4
zNnJ+roEYKoay`Wb7(*b0<x%srx4hp`T@W=5$!xRk(K~lmadXb9y)%CENGocD{&{UD
zjq{R>PnWk_VJBvu);&P&j|6mZQyEtFSW?)_h7a;%sh<t2uj9;(qZL?A$<CKIOxLJM
zoQc<_jnK0GYiR79RpGFKiz9M_v{ty}D-?U{9o(q7r9r8{maOe5b|AJPd-C>)Wnt0Y
z)9yYHG&!~}fZ{K~WFI?J9vK#Jl3g@IGg(hD5Uu*q^!-lWOd6dq&saI0QW#H*HGSrI
zI$v?=9-Hv@lDV9+m)wVLYH8BsN!BuwY1a0d2WZC9RVpU4{i6otO0%v@=8;E1@#;9!
zx6Prog)Ps}2+l0eV%g0!<P-)<Yt~nxZLtV3s@@WdoA%57F+uc<2$(u?3L)f*6$$xM
zBapR`yH8?BfJ@ll1%N4i(Kj(j@7e-S8Vj{u*^P7_@w0F}^4pDmzM=#xmYW2obUi5_
z?ZABfp=sP;&aCe7<8gAmKKJ4Vu{Z8@{m9$hz$iCvmiy!%QiV`KT2WI&xgmeoQRPoz
zA+)Tv#t5v`ankg1usVM=TrM;w^0wmqS<k;`P!PrHF4)9JSAc6UGH!3-Oi?vgvdS`L
zH&4e`#7A-93L&^yxrtv<fAChEncft%UhX?i2Jqv$-rK4VhA$-~_I3$JIm^hq@M-^^
z_de+xg9S$>WsgP&qpNqS?ug){Iw=*09=T=Jy@_Q>V}wmK_N{b}L8E6Qbi9!}RDG#{
z6YMONZtP|&=3aE#-r)^747&>Ktf?AyR;9P^lY$x@&SLbP-xc%aFUoGK8Ird?xug(4
z<>w)rabTP!{&*mV-=L#jv-LQC8oJHm5eL&fAGF$fM$ishD5zAa;ZuAvz(C9~({}x`
zyzs{O<-2Sme6ULrvE}GuPe&mEb<a(_oPW2^F7jJmi-AqLCrMau|ETs@{#_0um_Qrh
z2APQZ`JzKq$J<`0i!&kxB&?d`>TzHE<EX5{fVi357vHT{RQDvkbi5a1Kv#4iV|*U>
z`Kd&Ac1k#;6ZTgnIi9fu!BolSi$&E&F|DyOr6#8yHJUU~L&Q-4{Y&!+s*ifF)B@}_
zL}C7v|DeSbX0@em-skti{eJji)PS?Q&4l;i)85sRL2D)7cZ%k~$TMrKa{c>*?t+)P
zI@uE){qoyjwCI;Ek4^y*nZlf9rc3Ki8WGQ^F8%c*<a>O0?M?Ag&~?1wFC0-t%Y#JM
zM|n<s3(bAba$s9!XL})eetq!aAs!ChbX3O12fnMWElEk)B`?yUeB;>F1MCkDE1K3s
zR25m0M}|M5l%X$($HJaA)zU&G^0?hQnpryL*?odS?W+&i&7b&}X@8|=@-*HO(!RCS
zY{%WZZNbDS*#W&T7@K?{P2|uC%1jS#MWxyitYMQz_g6iUkeEIoF*Z+ziB%MHV}l(8
z3s9?8A3EXDMfXQx7rvCSHKRj#-Sf`Lwsot_?%8{aQsNs?Uz$fhzMr0MLXWtNQ<TLe
z-FFX8AI59W%$2u`It}lWP0lTi$VC$B=W>s~sP|aB*4l*2|DLc{(0Z(vw~vonUfcPo
z&+&c6napM2{MpLZ6Jh`Jr_hU<r>UxW@{}p4CThOpFRMPkI*6Gq+ae>+gOu@(cRn?<
z=QRXnRjfjILz@6=o%(Aj@f%_b94F~ysN7<ZdP72x4srP2UeZS-M}rZGkFCV4upQhB
zZ#v=J<j++*|7H=xGe~g>+vc8V_uVZjw?y~FvG3=Ns4t-_la^XB@$_X4snP1?ZsiMm
zxcY<xx8}CbzALgsku{kLr0{y((L2K|qhHF#wsg~J>}0yYrGijSC{(B^dgbxvzEYjr
z?(F=l{DtaRgFrv#_K>FEYqQmtV?sDOVR{i4#&K%P`N|NGCk(nMTR|P|I2dtz@0luT
zRVx|RmUXcKiyYy0qX}ie*Af`DS6;vdxk^FH=8Km01D%Jl@ujY&%%-==Uv;PN&zrba
zV-@-+Th&d9^#S^&y27(}%lHQ27eChgsu!~CGA_{I!SSs8mexLNjp_Z_P{@hMvDTb-
zX7%~;tN{~oG{~ad5tUC(>ol{aQd7u<gBBb6`(s;K*Ror_%C>4or}t6kG~mr?HOp>(
zC9qqxWh=1;t-9(QMf^Mjq}#?ugKZ0PP|spNE6II|ct2eQ5+9R(ZllGm``^Ut<2#tJ
z=<2!|tbMfnvFg@)U5P4n{Uqrf%wn^eb2^l5W01ICwrd&7;<ZYINt0d<#fmY}PI0-I
zOw@3eAp95-No#Kq8>Ej_qNq6~?2G@2!Tzmy__vy~T~wymwo$rd=U=b0AeBTy*bt}l
zrBqgVYu-d|11%rr7s(@~#{8E}Lf!#)@f70>+IbTq9kN}jaVDyT6OXFlj2Fxw_Zt+=
zksI*MHb}yCR3_gV86&#tAI9{oM$IxzoKunRoRiY3@86)U!9Tqg145PYcP@;*&mBKG
zsJ?5zA>Bo*HYVM2zH26o<Ldp;zSz~KAax}?b$5G6E3i%e$T&_=04${_;;dt3d+DdD
z;1j;&PnGlY<qd=ncD=@Xhgt;ecA`-rW=9>)kX~%2xM+Xlb51OI9x6#y>|Cf~1IsZ%
zaf>(|&J9GsPlS4_6#8MebtV_;7<UW=Q3U4){x_*O7xRBgGXGDhxRQc~qLSi2srXlp
z{okeH%>PQoS&7+LSicf&w*UXBINSe6DlYaP>9@*1!M52~)NJz=C)@ukJ^r`)?JFDp
zR}Sm~aQ!cN?!VL9|C;Br{HLq%Kl5A`cFwP}|Ia*^nT7Sg4|JKobFnl3U&`O2pCDBe
z;n$$x<~AsZnOUU~Hb|o-IcC{dzxPjXY>=Xnq6NBe%!18s%u*oEx(;?ewLAI&yBE14
zn+&UYYTC2S4Re*0%+WbQQv*$j4XN>{ctTMGm6b$9Kze$6CnkG)fA#j{8X-1;e~=9t
zPJ?lA>4@|Q`(aCR2gv3!r6t0C`%wY|3a;P249eLGn&~5)`7Vr)55kCq)C<HB&I|@o
zrmS8(1QB!Vv4+9kkLo3cb-Z?{Z*C0yJNte`=D%`|<FBF^m-7H{_iy1F9$6iSKu}~2
z&<;2^!kLgg2RTLgZjCa(CqR-aR+*~^O+q|9F%iAdznMm`YfAGPg|Ynu@yq=dkSQcz
zP3+ur6$mO#`Iy^HXEJ06Vxg(U17y$$)fC*2%@hPl?U`IffN%@(Z1JBEk_XZJnyHov
zCb$9$@wukCV-K$0BhU%zQ1ASScS&@$6;Rph$MvOt6FMi5?IBz=l=1)7L;#aZU(kc(
z8d?t~+jH2BD0iMo!sLgpfo{!%u9DhoFfIqevRe*<t{DJKJhC}7y#R6j;ojW*rbl}1
zj;)ze)O#3@;_Z!seHwL>k(1bjK8&#FhP=I%&;%*6-lGo$fw!)09k?PL*TeTuH?)?(
zip!iApkzWFEEq?FfV_5caDZ`e1`&(_iEB^8xOVqIM24O=ke=K!)`aBOM?!)2hpdE&
zi*160d4%p8Tj|Gu@aJyz_v!dleaA9#Z~_~zn#cuFi7R5m-ecyLMW}zq>kGLuy+E4Y
z^S>aZ^51)!>drwhJTpTAUGwBk2eKLDCT0fKx1fz}VE`)<;{02{JC;2_ZFJXlLG%gr
zL2`15dycw=J{f?1ML-HQ=DN>N+#wTQu3AV4;%9fr+Qp9=xFx^jz?YmLPpD^mC<+*x
z1R;>ap@F5N?E|KukKK1*$ERQGJ8<oFOX+h969`0$cdTo8lbLu=`2<SX8r$0*-Nb_0
z{MO5@El>>Zkq3YGlWP+4BdRAIUSA#f1X>;BQr7OFK<li2tchEj<5;`LwMMYbkDu1K
zzHFfw&RkUX5@UolJX}#B`bQ@O?E#Msmwx<!_fF^R6Msm7gWxF!{z!z%5E}P0Aix3;
z_4JR7^n(Y1H|y$lbbtWlmp#*i)`6?mAaMi;0e^KtYEOMYFj1(Yw!jbeAVCKFB0t4_
zKyv%ZcA-Hih<=0EoDlvd2BH1{>>h!Dm=U5<fl$ANwSe?!#D?L4BzlYTf^45jK<tqM
z!whx55u!##?ve#MDtU=AF^&RBL9wxtzKZ#;U&Z-<6<@!KGag^X^jMfdu`|@(77GDV
zY^<!Woo){e$YB~-J<oCIgJAEKV4Yb%upFIwms7dZQUlHfF{cIkJAh%pt^&U-E+FPw
zU@ou@Yyb0K4mZR8@lUai+VvkOJj;t83V-|Vbd2qRl21?EU=u+9+Qi%;nA&9j&xC-h
zKbtd<ZoR++hrZz+x2ehg&sgeOR83y-04+_;_D|HuKe)TQ_o3k9{kP1pvi2`;N${_8
z5Q7`1b!eJHmAv?sg4+W0hAd|S#8Dj+L@4Z(YXZcTT(M%;y={h1ZxBQewd$Wrs8r+I
z%oqTAOl-F{zhfic0N(l_E|JfI5Vig5(hx{z|IWf*U>Ys*(U-1mG7(xm1l)$1*iRPr
z5d-r0nG;mg9AA%EF|PH*L@dw;2m}23X?0C>;^*w(^jG+O##&yJipaYc-q{IiMF;4f
z;^DXEuM5Nm0nmqiKPPwsIBTY4UdU!veZnA^Jfvbi=7R`VWeWP)KvjNM<QOJGs>NpX
z6I`Cx1^P4nu=C4A9bfxEhJnAjfexPNx%-VzA5S%Wa-|CNTupD~><t0=ICF*Z{su$_
zKKz1)aDq-V8kZLu4-T@W4axwF4dk@d*&;aJ82a&qzyB)kWRyE>E5fCwadI8aLT4k}
zWUs?<ZIZ1WQ@d6)uMqibWZkl#g-apIqO%cYW7IZy$|roi>$RlS`^#FTt4VDs70<>_
z%1N_k@$6B?=6wZSedx<(9YvspI`}#piB=(^TD;2VMjxD)O0B943Tj6myp4Gim8={N
zcXJb7<8(hsWi0Y0mB_-QQMU1++flJ~2&c7ErJ<bj`5t~j45ZsDIexo9;&<}gL+#=0
z^9p4>k>hpVt~uNW`Jr+Inj{)b2d7>b))QB*c0v8}gVzdC8_BaQn`M2XzRvykNvl$6
zvs3yNCLFy#{i<|NE^0}C1Xp%sASUZjft0OsT{F~Od3*r&LFMvg(p3E>8fCFM;aUgv
z*HU)o25?=4Ssgw6Q(0QAm7gQ7bK<$`1RH<JR8f3X6Lll0^$t_zT^1B%4RrNW<f5hl
zCW1NP^9$zT-n1Q4$a1l-cJ-!2AFEnNe+AmK2M~6|_vLVQ<>rqvsV64|KT>`wZ*$(W
zU@R4fy7w-=EoVkw99Yzg{g5P8bU@{@<^asiC03)fZy((L@i3oz{+LR)2o*Z0u5ujO
zcz$`QR&4+`3?yT|&Yl@Y>MdF1Zr`CDWj46#liI;_AbECB7IY=YNVTE;rc8d9uV9{x
zFu7f<<6-Xpo@5GBtds*Q<rE}u4TqS|FDf_QhdYyx!YrYgcdG>^R)yh`@Z!BVMBb$S
z5?Ps3VBQ&F%c(y-wbVglHC59si`;;Jdnr6Mn&T`Ed$6^%*EDYYq<tR3abB@r?Mld-
zWZVb3yEg9cEQjHH`|#A?5-U>tn3wbd*sGl^TZw+^6lM|SI!wZU!-#-(FyCo^#Fhr%
z%e7uLnvSk!S=@*^$u=1;HUC>t+}SQ?ZXN~q0L?FrqmooeRd2F*FOwwfwLnTg`Fi6M
zLv3EaKe7~apouPtfiT-;%&gR`vY;xF;!CY^E%MZh<+W(P-_3P85F!Vzsta(I&2ku`
z^nvzXRPJVQ_8MfrgCq=(=j+?Bfcq@>Q-LaT4NcXT)GT;G4}3tiJbge)`;}qIb=r*y
z_n?2>{wvDRlap5X{H~DncmkIo^m>26C9`&Rn`m{yYNT|nW#?;w>K_OS(-u60zBfO9
zE(ATqL|6h?%he<1TgatnViS^bGpT-NztH<GOE1gGJy}zsMnCKRHZfgj?ow}=r&c9=
zrHG0j49t^!<BkifG!-y@gXHiFx`PC>LO--W6}5;Wx(LKob4y1&N3b7lu*0v`p66(w
z><ifx247v9QpYuZtX^Z4fq|lj8}QDaSivm3f@#IGgEAA#-9$5pD0fpnguf2TcP%=1
zUS#y;n33u<+;hUJ-oOe?h3q@uCiqcnNfmpmy2kJD>kZk`t<^1!Wmy_{QoU$TY-7qh
zd_pGU=fDxoI6&5WE~7O8bz1P-I_d<`K+O_fl%;>%&-$)}jcJ22PJM=&!K-AnO%+J*
zvMDG3K4BpxwCt*<&q#Tr^YYTNnRx!a+Bx8sxWnu~<C*_UQ5(st51@h!fyFqnz_A(U
z)%S=NdipTuLh*C47Ol+`QKqDx(@`%=-@_aJDl0`d1tZeV{yW-TZ4I>cRs)%^_WL??
zhWpPf*uXA25|t_ZS=?3~N4P4K797{v)bjLi^5>j<k)9aXKGzgn`K9Jj>&$`yi#J2I
zVmsMc-m`{G-gmT;og~@iI@kV`$8YC?*UjzlJiqThFfZ-I2Pl9gSVaTUBt<=407V6w
zoXupTPC>gkO2<`snMb^|9V4=?6Jpcg%d`kJG?H!P&_D5d1ADYlPTe3LTHX3UUD9iv
z-)#1m=NaUw25BR@P{W`Qzvp}c2itWCFTdmD|GjY8ie8?ZPhS2Rrsf%R9s&#f<OQ-f
zNUDd7m?vPx)+F6Rp#(E2O>%TP7nYlMrfMT1G!t4~6Gxlv6y0CFoI|O)Q@Eo&@7Nmw
zbskTvG|54;ps;f<w<PT!&z#i)x3iclcg9g_Af;g6eb1-0x}gTqzxfKnuRB4X;~w+c
zb>a72{TpAMFtwK0j9E?YLaf>M$g>z{c=A@0w5BCzN;$%kE8dZ^;O?`ZrN&2f%%taz
z6NgY-_8`B0D-9PWAklO-WmZ}TZP6;Xz??Wf7FU}<YhxX*M{g1~7eXFwi>{MLWHVag
zV|?FLU9o*-c)F}%_1jioi|ShLKa4n<bDILthuW{fUBBni_k&YC7YsLyc_2%~`l+3?
z4a3L36bFEk9gpZ?*Zg5ufzeS!I5mfjPm5qWSXxV-<fIwvNL8V-AU*aF-7PKou4x-j
zAUh%IxEFRtYkvP6oX?>Xk={Y-qnQVhS~|hNEo2EzKXv`ah2kKDFLuHqc|dcv>^>Zt
z;Z@^kUYat>^g>hp7ynAEFv_7oc(ajHE1it}@Lij1u;JD>jls|b-Q;&##Lq=1$N4Yt
zL?ck7uX{B(+NAZ3o%$IpT`n6T_p^6UK55Ft3cCzP?{QZ5^Yom}64uHDOZ|%S_6?Pl
zuvm4TRq6s-Uys##y4H?|P$7WkaJvU#u4%&aBfQf!fM7=VFl5mUcZ45&?T>dxkFa+k
zUqCq#LWTD2a#na+hWlf<bep2a{;hnG#&r*E)kXRseTG+9lDd+;fSznXKsuh+YdF}*
zM<n<~utgViB4>9q?`bI5oRlwQC#70EfNF@g%us(*b&kLHA)8+(${lC>C&4DycG~Oj
zJYOcR-@6jkVvDZOZ>mxO<RUzBOrLK#T+YXaR{Pen@5&WFNSAJZT0?v(4FO%Dgp9pe
zPcQ6ySd;T9Ws*}FT{?SXouWP*_cG7vktFOxe^B;>tO<1oAx|onpX$cnxfni;+eP7}
z`+lyd6S(gOFKP*YLRN)3a3$V7oavXP_SHzw1QPXokZC|=c~uM@RIs*56rSV`xM*U!
zL@e!JY(#C{t6v%9`b=O&T6YjGH&hYUQ<1{ZCa($UJ}{ipB1pffE^y)?>eVz97(br`
zIDjY&vQv1fMDN}>KMM*B3r?;5#pMn6ELa<WE2F9MTXkIb5VllBp9*h3ugTMQ(P!3V
z5LM%KXte#selfx2?T|5PI>_Y;w2`8oD{ao_j+Mf~8u7gClq@aK*$&eyC-_8C1<Um`
zn0B3j)Z`x-n@Z5{sfLJSYXOl%E!}c;6fL*yLcTsBel_M|pc;HF|E-MXyGyWkN?QSv
zMW2gy6E0D^E+yJVsCL^G#Pdy#57)romli6zTh#~;Yf~Pub?U?W5)EG_a*a62Js`8i
zUg{$_eyA}B*Dr%pQNF+2AJH}P#CAKR+CS|)I!4$AK#-<>SJ}5}@P6rLbyCyFev8>9
z5d9T%W%QSd(w(GqZ-`hd6tdsZMQJ|XOvcV7QoVzL5%1i}O0i7N&8xO(={^(SpHt!6
zZG`!$pLLs;?h~vP-eu8Cl|)Olc>~(8bL_bs?YJHJgkTUj<!EtPZdIN%pa_xK=yt??
zF&lmTYcT5#V~#kSXarMcooRjJO#LnfUCPQlCQS7e+{7*W6`V|#x-}AsfF2)DU?S|0
z?J5&}GQgf~XzV&{HDmXg+kL6l!$wg6S&a7Mm@bBhtpe#UC1+Du$qC95hGgvb9mn@k
z*jesVZ6dG3A2n!UDbv7m=G%uKBns~VG~W@y`&>9VX$Y(8oBF(09J5%jgocTm^+f-e
z*)^1sr+e}Lnr|aQ(!1pv`^ff0*((wzW;}_2qM0LbN61nu-hj>*yAxyAIz7e`{-FxJ
zQgcS5#;w<IHuODBxNoSo6hF0GI2`5|d&4Q$pt9TFJh2TE6y&n5?>~$e{@}{k2P;p9
zxy#Y*VqK|X=nOQoF-CXw5S*uW(7sz>UaxvQ<2xhr2c#z-Pq3LATD_{Bb3J;RPiV!S
zz26?&vApI*h)noDCFyEPjAr-H?+3P0Yo>45`)N|&NIgougho7Pzd036Wp_ZFICsNP
zvuJ;hU*P-TxP_9C6CDri>^8(@Hmhv3`5aZ>7HOVKSQJXf>H9oT=OLHUyQiSvEdD%V
zH-|?{f%Z8+UI%@W;SB)=D7%IX)?BL>=6qYc@k`C_GJ`J2?3X2$AW2vveB<CzkT*Ob
zo8_RahQg55ScrQOTjvjGU*+Jt!#3pttJZES)hF{t`9?0A^^^5@zCW_u``Ss=AriTR
z*tW-`Y|l+GuvyG7vpVwdWi)uac!R&_3Sq0a6DUGd3)Sh6=%kUU*I*PHRv3*)v#(vG
z)(`IaDG69<QcTJBALelg3}{7F#jq|4YdTR06C9OwiQ(gyM{J4EkYV#VyT>tq^<@!v
z*E}nR-nU4A(OI;~0`3HD^g^j*vQ}wLPFNqhe~Q#0;(3d{VAmT=<giqEseb!;rR4ev
zl{Z%V`wTW>osLW0MHsbhmMJ$Xes`jcxju1GGLL?gMT}S=rTaSjx*3hIo%HAxcJe-o
z{O=mGYU7Lw6)gHUUB4h2bD5?Oia~h@f-)A0si>scwh!|^P;N?u^VPo`@sLE#u~R5s
zaJd{rF+8)YXbLIhVy;t`(|7coC!T-y6Tc~rNmreM^PH;sll^4gu|2`?2-&RXQEzxU
zR5wb*6qpvGd&QqAgFmG`-jXrhb;vajW*_NeR*kR|E)zU*`QT%o6TyF~MVM&p<9Tbp
znB|v?V9AQ7gRKr1$ul}|_+8iy2mePLW{T^B8#%qOru2XcKVVAnFayqOCq?2hoD!Rg
ze^1%DUiZOibmsx~iE^b^UXCoPFtz;hedp%eoP!`Fe01VPeXKCe5IM_%Sp`ZF0xK+E
zL5geh8L89Oz*sb_mAr=S9@9e!R5~B34h_WbWL+)vc&eDcS<+0xsCcMqzAIMDWmXs?
z-#R$$d~)P8H@{swml{W4wvMHDtb0EGP7e{V8%|H1pv8<l7r{1Of&POulC7g1Q}~pg
zvY!RN*xJ5P--7_(P5x6TCI2j~cw}W%T^YQtXWb_2&333}^@M7qY;GW}LcyzPtV_GS
z2O6r9M$y4fYgU6-$n)WGILMie&i5(G&5S(APP%UTLSELKu>Wpa+I4K8jba`<T&w#)
z)!pe2C7KolP`sn~qv+BbFa7$`jM8M=U#aqievvY<;jeUuvRT}5QH;wv{C8(vnr0z{
zz-2WRmbi}4xAJvK{<U8^o&al%E%rjI;Bz%*+>z~a4E?Ix%AXEa-veCSlRgID8uY4O
zOFpjGo{_iG@)`NFyOQZhFG?(J7n`C84tS`=NGhIpC}u+i@cv}$Z%wlD3lbEA&Gw~g
zM_r&02o`q$cI8N#;!iB92T8-1%sxfw#N$YWUj4OJ>}mhvS0wzILyh+U3Ig^Fh7Jyn
zo|>;nCj*|TkH{u>d(qPrEzQ2{yRpt_iQARe_uNaD=k7ae>s-b}8sojBt0vw2uH9os
z6dP7OGelR_M=kclIvliidBX&>e2Co9uBJD;?(Z&<iQMc;T{c=ynVS+!a;_8bEaS`J
zd-D_w4f#YbD2tfV1M|SVwcVoGV?>$p^7z29OQmry*tF?J_t!(}S}YbmDdV1Gf~f*t
zu$fi^wvF?tgf*SC_*-A)0Jp{Y9xBnFs7zTaG2^^p3MduFUY;Xm?b`-{Od$ej*nWx6
zOiZBywv3Ixl!gK7fm?zlPi7DonE!*dd+N?C4AgX;R8(;#so1t{+qP}nwr$(?7u&XN
zTV1^l_E>xL+TFWH_b-?y^JKp7yst;lz-1A^zlbcD*`&R3wUWCCn1VQT*nR%sO-2Xd
zWl9;x0DK(v?3DKHMb9Er#W@?Jf=qneDsZXuAc~6G6(PgB<BYv-|MY`tDt4JVOmV@{
zn473dE1yr0lDUPM2-I>v2Q{D4tMK}hPrg15A_OV^6nA6N?6^gfmWE0tlT)0lb9Xy6
z3bBlWMb9RmO&c|?!6uxL&b*FVbvI1k(2#O^SF&`Z9PM4?gkz?PEuT;(dTy=w^>LK4
z?oRijIUa@ldP$~^PH0B&6i%ISE<5Gpx#CuFl>zJ~;{*4%E6ed8K={i+^N`^S>ixCz
z0ev1-U(Z;?bRqG-1@cPQZNetX4;+#Y3YHo)<~Vz9a<dXQI$TvU0-joqyRM6F%bV8~
zIXLa>vJ)(c83qWp+cVXXQ}I?`SdK3&c`IFxDWYAsb^_I+$jgK67_#U`4;rOTbT<v%
zLgj;$2WYGg;Hicu^JnCO!%uJ(r~-f-Q@ACB&=rUdEB303WpQ%qp)5PvyCD?1UT4Ad
zuMy%IdJ~{}=m7XC^I{M2LdmR2)%)uLp+~B%bV2Z6&ofWzJnhuWlZHLfn?=N-mSjH9
z6(HM1)3#4aLB#C(TnOsVN1oDUHT6}-@J{?}pc6vmqKVz+ps7)fzc3;M8Qz`U-zi~N
zHb1aZr-ATqq&raqA&*SA^>OKmlu@9Fpat2n<}TSo*<WT(f{}_=P<14%<JQgcIQugb
z@R1e#Hg8cKH(@rgGZ5)+5l^QfeDmcnS0GN+=^IPB+wY?gh~i7x-Bn})`PirNDY`X5
zYg6=0^^me3zSI{@H|?G+?QB9tH%lRrxVSCn6tnTNT)a;}TX}|EIO<DMt3lFB_-9t6
z4wY2M*EJ%dl$UKkH4Jde%y)V45|+_h2N-!=y$I$YrPdKGgwdY+9>+^*51a-jQo}gL
zLr+>_O)CVWv81ssn=b~FLm}EZ7Jl_iX(7+qsRNF#+~zx6%4&_YqlGWeQs(~P*=1t(
zBw@_@*h(Mk#no92kB~U)5+{c6C}#2}-1BJ0Gmgbc$w?h|;Fdd&(^l+e^aZqIZ2y#P
z8!~y}a-`npQRqV-cX7EtF<u)}#9~xbbApo}1v@$I3(~^|6Ro)2KzUwSVUIX^y>9$6
z#Z9iLWt~0@C>5K{KbpS*Hd>4%MgQ1Q+GzI=xl}wm<W^<vH3dh=zG1V5$O(o(RB-n+
zl!9l}1$DW~(1Yb^Li<^jkkcNR0~~H91r>f0SwwP9)XPbv6|jMw-vEXOvACJ&mZ8Qp
z$S7xgo?0*iAJ~A#)Z(uq!Y@-fl+CLQ8iJiG@G$A|jZc@l1jsSyS&+ZBuaAO>(oem$
zsRq+#8HX*@o~;3cV@Wfr8{j=Qov5HW-fUE=C{7i8A|gMO>-B2*WbjWf)`I1m&p8^Q
zN^iHs3W%NMn`|^v5*=4rFd?R+Pdx5v=V$?yyCjUP!k$pfMA^;L719hrGx=B28N7V<
ziexC!!OqY|5=5o9hl@sDBQ2^#CG+nX2ZP+diN)@<bo4m2rPfJ-FM7e}>)M)@qSR>y
zR|wQNblE@%rn*mv1WsFAY7BTwFX54#J78F>%JZ+SA2RhPeaxh4Q8vOU){3sS1nln!
zhHc(g#S{o;hY0+lCT#VcSul;+CJy-;En!w4eJkEUXw8E-Y5R)FkIBbN->HTxZoa*A
zd7XiJD7VLkx0dUB9!|lKCDpV!NiE)tGP8^*88&%P@#{r9Y`x-Cr&L=r81ttpYPWTZ
zf|?TU993t<V9^MIs6w3D9rQ2uhqSw{g9S5T5QU~X@thVtx3-hKD(1x7Wivvo;!?9o
z>;5fOb$l?nfUTKReFa4Q(eBT6S4U2xEYijsW<{eGsLN*$^k-&`W*sF`slS%~vnT(o
zlfL35pshPepYap-`i40uFQPrzk&IJs+h<^SjFii#j;PHY!gW`VoAGIU$Rq{0n)^!f
zE$|LtdVs-7qxKyLKtbri{Sn!F6<{TeO{Wu-EbYgQUi?jI*{X*|M*(rwIU$QJ;$F&k
zcx4)uUJ@>P8G<CE32+f{g~6_ViR_LVoXuG63;*^&Ra#_DzNU6QDZKw2a9tl481*tN
zLe=yInlicO{DFc@JkT8{ZR?`so3MhqT0(d{5s9d`UUGdnQ~r0EMi0aW=c>QouFLnn
zf*Q9MZO9`i4X|0vb^b0`?|;D`ywhd?k3q`BNTpmG(>hlPzK$#%;jp0A5=IkdAJsfx
zvfqk$#*}aF?+ojPZ=KpE<Y3YnfRN1-MO^D{7-DJGKaU`%Cj*C9T`ck(Hp8ZkqLs7k
zv6^??8zB6k_H9U|<{)SZ#zGUTrV{y}81%AHs2DC*>)191d||xjk;XK?T(!(jgZ!|0
zeq@`YZoQOrEW+_0ofP$Hh>Y|a<QAnBABO5P1Hd&lO{2Kqk1ARa*-0G^WrIO|)Fa=l
z8`T7!b<fpARgP`!OB~6QxBG*uOH*e4L=nS{xYOcFr=M8e3Moimn#|8%moi<CP-eo6
zs3ZM#4VrqT<)y~cgyM9YU{hQEf*#@~$)vGE$V8wAdvVC~V!t(cG#PYbeqpMEV+_vz
z{3H0gX%IBIuIWaDbtQbh7UV-?WV$)L^ulWqJ$#nmPS@k*ItRot1#m{x!BIuJ6Xq_Y
zSpX#FS4Kjd1tSqk^gd2s#t435bB#cGvj}C~WYmNply%GwRw!uc+E!JB)uem2@ro`;
z)}R5W#IDf`$E9~Lhz_Ud!w&r1ISF+SQgb4=r$?@gBTNH+NfY!mRzLMjYXY=F>3JK|
z3*WYTgayfiJem$7O7f3!@OUD=nR%!-JTq3-LEJ9dRaPyea;msLyyYjZb-0XFZp3w&
zl?HT;{t_Ogk-pI5aPG&PwHlACZq<^?(nz-wF;0<AlrCU^rY1zi)g4I}c5mGnMSs20
z6e|LY7IsQ0(qTv^17Q3Kny}t&LtpeFo_Ji|rQ5zQou6mZ)tx}d$z+jqM}2hMv+{5)
zjJriTFqmf5-JC6?JCc%+P+gK2)G0M)jYohaKyxLeOeN#Xp3T`Xpn3(C57}u;(HS8+
zIyJ6IOl?w6fRnyZU0e;8E5;Z!O)L%#f4o=f^0-j8qH@?<XMxiq-2P6RZxL6SOe<hq
z?0=^i`1{rvdKlOw6`?;uH9J}?s<;=}V8d)VNCt9Jwx7wldy7@9;A3iR?VwA`!vcMg
z4AU^4Ki!*zdF!@8#%fW?p%xiT6b9Sf;QM}dh0-S~$O#Bzgp?_d+ZR&NoBXrCG2iVr
zj?28?qXBGnZq#)%)M{RtdwfF~{loBW)STvVmjbQb@WWMR?z*>Vwi0xkygv{!v4~gw
z&cM%ap^K?FRNZYdbBczY?<>kQ2QdApZ5GqpydDuWm#+mXQS|Jk#GUBm#G>bD0u!^5
zLlfQRJHlesHH`;hN{Q8g@g#)dbD}F=V&IDmLvmYxIf_{(P~_NTHuTr$>;w$Hu41cq
z2H#Uelps?J*CofQZsk<b>Zax~D3bGj_646N7Rm66<tY!mq;5z0wfE0}x-*w{O~2(#
z3b~vV#Lv?TlqwHLgwNwu^!cL}yhwRv(hz0E@aDLS+H~(slmlTC1k2+@4=W6Cet&A*
zR{$J};&vLDY@*2-Zy-Y=6Ov4#4Ug+1<$m+C4p|B}X(nDv2f;jPdL<y{m6j*nnTnBL
z{+K(ar{5+*4)ZXrp~4AmDO<G^z9%ngfD6Nc$0&0b-4U%9_9Gb543TRh?0;j>QaJxQ
zrT~nJ4k}lW$d_9$c)9%DuNv3)9$#agacViQ;WIOGQBs&rg6(m+SN056jkgxg{Iqn%
zq(>fubalIw*f5qCj&{1Gj5<SO^*)|;aNSC=7@z`jGpllmHh%@W5gpP`07gIVqpY*J
z?5s1ah?NY`rl62$f;FXZuUz!AAZ25lBUE0u%b2y=pSDB=%hXxtaC_}6%g^oXu(96L
zYRY}Db8zjr<F|!+I5>D!gdSzzrwdX&gNXk1`nk1@|7R0sE4%?s^r!`nPqPIf?(dac
z`ow`l@uLv0J{k<EJKm6j<pP(XLE>z3tn@(Yu#U0aPmLhU0Z+{1IQucjU_a@&-=UWl
z{y}H8U2E@}x(n~2cam~?6lmsC`&SC-jX664e(ocz$O}ee3#pUkV-o%tnMx*`osF{&
z-aRJDIj6}qtpx>9^P~3fg_+Eu2xfD3p%!9d8$S?;m~=zW_@Fb)x3ge0^G!w><FtVT
zE0A&#gLzFJY?*00a)r27Pr^=e1Rxw-*G={-HU|uVo+MaOza+g@PhXAaj1Wq^N2{m*
zwOEgi2yM92r2o4*`Co#457?x6!@e~S{SLp=KAMKim?zClL43v_IN+#$gL!^^hCyfo
z9MXVcVb;n=9t^H{LQmQ4{YD2GRLKar*4M$*Kk?E|%&CB6o9NtBk0-+um8dDqgE9i%
zXH`Z7IO3(?e!1gyqyQpofIx@up@aiC$3YNB4<pMNN!7K=m$8RMvH<u|*#~n-V}6`}
zqeOZNcQH+ZmE)5a(t~)}Q40hggfP^aZ%pBPbF8;{yXQbfQjJ1Tth52i!c&EPJjJ^g
zcEoRK80h-=OIcf&z&KTX!mD?LaQAchTozqQNY){R>xMXzWUu{G&TaSmqo@sVe}MuO
z584tpHxd3$uuhPV*HXDtEx%H~!6W~xJCGi@zBYASZVLXI-NVgJ;STk=Up}LuYbVLo
z$<E{TUM@)KUN7nhmip4vrIsh5r&zy(Wq&I-HPs$Kp%dK{wNxvAFf|9lD@-Q>t5oWD
zfQ-hB%hGXF#QQAzN}SVOhFvyPp&RcnpVi}}`u#yXr8jrKM+zrQY%}I+8#o}OQc8Ek
z{kwr@-`T5prhx5>|J96vM5KUc+0|y}x95!fH6ThYk{<FPxTSFC{aYXCK@4Xahi%f&
zWeJ)5@28JYhkd4syx_D%Hedz4(T2Bealwei-)gNjcb|9}d+Md5eb|!&CgfY;!)z!e
zdL529-gWFsRY^4{*pu)$eGDDhbSVYrP?1wXvSd&9w0Xg$|GFQ>B5;07X{0AI;T%cH
z<|4t>+bpc0Mkpfi(iiS0sc%ognIWjr<51$QFwi}yYxn%`>}w(#IpI%xo=+N4zaMfh
z*LAjL3&@&un6;dweecXAxB&up!SXH7%*3vh_|HP$1G{nOcd5bjfhLl15lGRpXBhfM
zzZSSL`|wRR_GFX55V?#HFaF>0MH-#g1y^DOTRd^;^R@jfWqWTF*+F`3rU)rF;VJr@
z_mJK-Bz}2Kr;J}QS!E~=w-v*<ay-7VO&(spWj`Pr<powQQS6nW6z@Lpwo9UqksDcK
zF!d^y?uoy4AzO_yB3;NHS{;V>8wHH3#0$rrn#OQBuy9UVG{nLHA8@!B@1gU`##1aP
zaVc|Z0^nj;Di3Uu79OpycFj9%GX2_yUz2_F&kp{+nAFBU*{O;`;Y`$yC}Q<GNmKL3
zMsDB3@P<LbWzD*v<i`A`{@PjuIX7Dbi9%s1Q-Q8fEwtp}#Fl=nDTWbkN|Uc(uGHrB
zbw2eEGV7$sJJ0R507g_d%S%%Jzlx-hMcYwT;R358mB%glr6YngdVvKwG+lQ1QH4*X
zx4fAe;m{!|x$aftpp@M-N4_-W?ZOM9o5<3amW<&K&a=J6WBNRvwImnaGF{`<pXe`R
z28f?Aez}np9OCNfuku}sp^^}e)+4*}I@Pda+;3plTK@+6q+yF9$U@Wfn^g9S&5oF}
znE%id@wEUw5!M)-R%O!!J|L-=C(mK(|AY&VKa7dDHoU;{5lC&H4VS$_yPb8Ld!Mr?
zy$s5&U6MJJjPmi$m*R>F??#gJype3Np%E>gE*m;)sr1nJa_gD}cm$auZInWAMfSG>
zFY7Q`Ws11=suQ`6@|Y9d&P=X~6)3+pOFsV9F3$el&oOMID=To2eO;B$)(5rc*+<f3
z+`#mTFcARv1`grAvhaSul2?8^{cp#wQxAT+N7xmpm2aBC({JgT&AP?@SX|;GH09q<
z8<>hP4`mfF_I0%9hL)4w2uoKn*ekgEz^lQ$;)qAaWnZL-D^ZOO31eW{7i}A)Pw*W`
zZ8x0|9p=Wj?bx1hSh*UMB6+OP^U{w^910-{VLJ=*y<4Cep%rzkYq~;cBn_rO*IZT)
z4wZF-(-rCw2UoTk*y>yp#wTB@tKs+4PNz97TQ+#M?ni*m)jJ+dTvIPKqU`0}bL`n;
zvkd&vU+<H*&n5H|cw;0*?9R)3f@5ZjJ?UGLN_*VO+8ocD_p<S1(ZMBD*&YdjoD{_F
z=DL5a_kL^IjVtFgn_nrs^b($HUg@Y9a4Z;+phP`pD)Jr@kS@P9&GWzuT%}pPt}#ZD
zZR#PsD-iZnEivD6XG7?Y#}j)TZe9W!<CGxZknzSJ|EbsE-dxBrbyS*zEaWw(qyR~9
z&0K#f)J`lZ-;r~tr1V(iX$DSbpJWSYl*vAeJz^i(l*`1#wzobsU3bW-C&!XTrRni)
zbfA%zuuw@Gbsl9K@bG6Qs_yXFcK;uojUBo3FKfNUuQqhVE(HP(UIXTg&c|PvADw5!
z;KA}|Fihqa^ItZ_C9+zChNDK$Ik3he5r6)eb_h-Y$xs}4-xcZ_0BbY!vH6#K>W%$^
z?Hs)>Q7iVnbL_o$PtZ9?>$e%viub&Ij=3j3TQSuL&?U8M^KM}El_^#^Cpo9h7=1w;
zxwMs5;DC^tR|VAw54S{4uK`VOr|X~uEg)u7#6vPUkRnatv3}YuR-S&kiB0ZWXl6Qr
z>JP0gsE^^lk};{RH{e`!ch;)iO!PYmJ6Do3I1?JkrX9vl#KHMI9FH~h)CjPDz0N+B
zC#@Wj*_z_FS_<J!-Qi?8q+`(x&C`R75i1VxF6}?59)RY>F5uMpjO!RB@6C1yi^6~B
zH_sm9Bf2k9<dAm{ZnbX_pp6YWOm~=u;I+;~Do@9^DyALb9Gh(U%gdcw0i#9(Mu$kh
zaSdD*x%L5%n7?Z7kMl77Z)Hp=o2+AkmSa%r63V5{yaZ03?YgVSxIa>bo^}=YwGnRk
zXVzxmnb)&;P03HfEnP~x>xi_lTe;7N|AlI-DVeI-g|!!MOXild#OdU(iN=mKcQ-;@
z8wjaTW+~Z*fbAE{$kfKJi6^*&zkiI8XBq?Ue1D_p$7L9ky+_7&2+@+vF?j6#J|S>A
zm{wC5DDYR+!r^4Y!qg_CMi6tUNh!O97)&23Z~&AWCRQDz?;!MfE9weua0H6H>C2R5
zOwU?oNFa!Hx!ZNK|1Bd92&cRuixC4B3RSlipgE@ZP&c_e=722iQ2czu*o5ckwPFo{
zu=I`=y^mg?ov$0AC>vI{hOVW8#%dbunxq(rrZ(7zt41x0HEL#12J7sIeJ;>c0h``R
z1mK85FTR3Ms!IWwK}j*hA;A6<{@T2>=p1VBLAQS(YIbJJYNzY^R{UzUaO8K_c_RuY
zcgdXf)-OrO+3rr*r}DN1M^#yjIuNj<1UHJNLU1UoxJZakJapk%;Gr)}y>dmV&CjTC
zTg`NiK<gnOyM06Qm;54_h<66x3{D<09WOg^g`+x}bjWeuSsUTI4E-XL_+q`gXDJU@
zbkcyA)5!YOdMckkovN0*GeX3MauPksphmgYTpbXhk@jMbCS4mfVH(tLle8A$WUaZ>
zTH|6bm5*jYvOi0B8`Q3@??hF_%QrYJ`2}o!OL%iVHmZLeL5|2s8?gS3vjCb5>H5Ln
z`2xa)9cCQv7li$U#aK9Y8N5U3lEi3VJ@oWhPp$;RGx|GI8RPJfFu=<-HOz*!+zI2{
zJwki)oZS`LTnxYo$*m@!39G(|iC?1SFA?FCfjrH`H*lLKHW=zGv}mLzd~T!sN3BDI
zK_L9v7OrOijYx`&&zk81S!J9G5_{NNLiGOJI{&j{0_nmsHI>~<VaLBnq^nd=m&Lqx
zTc|Gm`9T>FQRQ#fN~b~?i?a)nfU2_t{p>OogjwO8*I%S)vRqY&^@p>%!h$YVWGDb&
zJfB2@L%;?qg_dbz_44mCCB85Nk+v?)Ls9LV{qI;)9^BHcSs^V%kDGuDniqB*{k3r+
z%}!fuUujfX>+}%WV0n#Erytqr%1ir^WV=EZrRYT6LXD@h6@iA>o6V5_)x_dG6#96)
zT|Q*{+fX9bC#9{vLOQt@`Gk)(%dq^kO&JT<j)vT4kYj0~a7-5kLhG-M<6yUchC}vg
zP^e@D+HeC*eR!9+jiTSA_pvASx1VJp1`bz`ZV))44`*~HxzAq^2c5mH%B~tmp<EAF
z%)<#9l#Q)$wsP$gjZw7LFaH|^Ul^1rZkQxI`%=>(IbHE%CE-Uusq)cw!GfG=O*~Lk
zUj9F3>yNj)Vvl~DWte&nS|0aW2@+3F(#V1zgI%Ox@YD=)^kAIq$A0|p#%t6P2Le+t
z-~bU1LI@fIKG&Zu^K|w%n4$~!?$KDLI*z?7C}n4Ub)?KV;=TP?uW+rD>^}6e+#Mx$
z@`hFmJz|}AO)x_U&ZD>2=s-P1pNpXaeM2vCp857tr!J}BfkCQnF-T;XnSo2ftkB>W
zYT6pa_?DPsfALzO8Scf!rvf}QCs&VD;8n|ERy?~g{wBRPWk&5AcE_gm@B@Lb(c;9B
z6zhfgK}_#Y!BXke+Si-9by?Cg!;ml07E!_>s<&`Dy6t9*fzG^1gHa1#n@AR)i5tZG
zqRNoZA#jVo;=kV=cW`DfP%=cyQJ1=JXIMsLQ}b6r__w5)#yT*Txg6mF4W`VpsjQtZ
ztPpcd{|wBSWkYgbBevp0_SR>L8`<mGI;anOeZ_Z)M{r(Idoe8wz%Uba@USw^n;(VV
zXiBNz`d?d6{-tp=JHvVvEO7~;++FGN6BI%z$@aXk>LG3Wk%n3J%qA&q;T%44M*zMt
zV56zf!d@{Oql>v9*Ch>p*<0jKPTfrfm)5oJD#751QA8;23L#@P3aXBRoywJwxPAWY
zC_)K>AAz$+oo*<M@X-^lJ6yG`l0rk71!#8hAvNr%FtbJP!j|_{DmRLr1;S52={<>%
zkRTR2x(I6yKd@!&G|YI(>@`5g=bvsaxO;BbC*Rr-L5s)f3Moshh^&1lzZ~^u)f|FL
zEO(`n7z(LG7lVfQ7E(JOX?j8|&ZB~JXlOsyzO<2}%|E+si^dBqmmhDGO7IpPh7E`g
zohEJnj*mP=fvtYYIYYQyO!B_;OKoIE@AtqF#59uHRT&-;p6V85@!d9J$2nELH}{{E
z*v>R`$nfIr3vq%#rDjro)xQhY<duXh*rE-&voHAkeG5^X(LKdgUKg;kIN^_-N%$Kn
z=}fJW^24*g>P^>{aA|%rH?GZQ#mA+C@qlg|1uRNu-Gmw8qRr=V5D)7ok01f6<8#>N
z=qBepGnAS%tP`l&clNVt-PX{oV-~W(5O)A;8%mi`RVL%l;CFJFavpXKqt{gZi!lt?
z#tHO70<tmET4NE$Q>zYyvZr()`(CXi(_8HOk`<!o+)DNf<fb+)3C5Wx(l~foC0W-x
zdp}0E8V~o69vh_>ueuYRmTA8WYJ^x87uz}}Z|NJBYP~xT*NOS!h&?L%Js{}hP4#=d
zdYIRZBV)Gj2JO&YOSWg<H!Uh<d6@~rO2v=QJck{!1u#_q?4~QM%ZR&Fu1ny?R^?bL
zGDV2=s^6rH#GbECJt{mz?B0P2mH)#u(BY-RAY`1y!AdiSI54EB*+3K$96=&V9d<qf
zaPb#_u63jopeigR*m6su%0DisB$P(P1o`QoD~V?4q+vRrDoqyhp6xxUw^2>75+>S+
zn*5BuE8=o^F<iUm$L!%GiFVol7Cg#;`xM`9FNa_=rBO80(cK7=^6Zp2LzOoMEZBKD
z4c>e(u=2uaQa=x`T+KYUP0HYVT2mpN6${MeMkznEuDfcBw8T8SX<u3U^zmkiTF{c+
z=73?j?;&D@mMPSIzH4#BYgFg9EDF8WpP8;y9%@#Re0=IdpcBv%?5Q?$QSLv(pF$hw
zcL=La(@srA05YcqwEAR=n<-dXCoO7A>$asZ5L2?kC(Pu94SIrVwna*mR=lPax-0WM
z-peb)j#jwv4h}~WN;lh!S{8YZ;{rPa%@Tcn5+y2&QSKq~C*iMBL!p%5ojEUoc@sSj
zHxO1&T%ZCXK=E;GpjoC}y1YVFRfHAR2PB)$9+To=u^rIcsgU1Px@Ssq;6_K2mOApO
zw7+SQp&m6QLamT?ycX=&czyl1kD_KZsdF$FeWk<|^9uEKh3^!5Pn6N}?;Wpxd(NC{
zB$ct??5f5S;7qrmZhmSZtLI)?lF{dkf7r)`LnI&7dw6Ajt=1teBb;|*c1oy=#;Gth
z-6P|>)!!8h?aCm`^9!M@(WpUCGSsz4AEtag4Ck@TyDJ;2B75cu?vTsSAAmH79NxqC
zSS(q=b<rTaRcgP=2;Kd9VcP~vhXw2We*A9bkb;iGi$J^#5?ohs>?o&R$t7+nC7?Ej
zs#MK+M1K}85;dh69jQO1C7VqKZ%9wg7gc>;DHKOSZm*D?vhXz%_L6~}t{1*C1d61Z
zHDY#I9z8N}s!lGMTw`l0#S4M8*c5)m^=$OY(6!=Jg{EvSfM6R$CdU+v)<BnU;G9je
z2j$5~rY^0uhTs|MxLk;fJIauq&i4$_cR*$y{;Q;ZN_@#NRPFD`vE1(gsQw#Q3UTt*
z#Gj;_-%QE6OfHVo@STGw8jr_)XoPI|yL%_Gbjfp7=;w_7zE@?#foszs6i$I=J5F!I
zsi-5gGNjffTC<5k)q2cpdt)tiB}B}2b=ztnI?*>h(wkYvh#2zHa+9YGc9^iLVc_3#
zMLddN4rEAd`Kq}C=>W4-24j?0Zs<S@Jixe)7afX5%Yn{%x{Z=`p(3eZVVz7$&U4>;
zUwUbs;g<&)PIHnebZw2k<mIa`kYRdw^ncX>GXAFm?tj$*3W-a}^2_{32gvqcXMoKA
zJp*LGV`gIhmjSZ>A7_B9EdP@X@P8M_`$yybhlBTjq~HDDVeicUQFs3<@Xp-U=zlQo
z{=>!l5BBcA)b5O3{vmt+U*_Ha1iEAY?-}I(JB(!dPp`%Q6Gk%7vHs^UlAY;a4EjGx
zcl1mQOl<#u!^l=JIjs#8>WI!xP&2H6&dyFjJL}D@Y&?8EJ6GtMzO60dw$9EI`@@_p
z&!1Y4+Kvj(*fYZ#_LUkEQWd3qB!+s%UlAK?91HYwj1C||hf2!rKv7Xtk5N&Rc87-*
zDAUWmJwtYf=X|21(43mrzUX0WzhJX>j0-}hZ(JN)T7b+^ErF1r{lc>|!vizp5qc*_
zN1t#7IYn*!%JM6NA^Bo(@T^UtodO6^>l<I}n&=o^ePkbUMS+s(nf<~eBLkmhUH#%<
zXXpA?XHoD>Pp#mZ+-J<J%wXl}8C!rjJU>-`Q|LIFntIxRgR`=-dI(~YdT@#|`dDcA
zWd=90^P!)DIyr%50Da@Z#IZI3e-B0B#9-xF*z7%~Y7I><u1(D%{B}cC(?kQY&fezM
zvXb_T@q0N0LMB)D3*er8rZrsafiVO2eF5(knt7CM>*@aaZKVHE+L&KopB>$uUmY4;
zjo!Q1kMUdXPinENi)#rO+Iq&PzM0vT(aW{%sfC%owb`T1t?5w+kUc)J-yUaQ&lx*9
zr^YHL=NzZH+7EyHUM$^>HcBHiLVaUB@Hz))-<N!DSuo-3YxflnjBj>Ta&>lQ^~%n_
zhb#|1!XuK<Ji}PDpuo}ak<ax=ZwuS?7@P2!0$~yo5|TnA0i6N?bzo*Pej?~B4dA?%
z-pk%`g5c76a(eomaYE+B29w`@K}=7K%_9G{b8!NGcl*-3`$rFiL+TmmU&7G+%1~eJ
z|BCsh0nhjj-htnl>&MQUdgbwy0vhJ!`QbVP#HNO4S2vH?KJd<@@~g-Q3(F)Q(+>Zf
z1_fVj0_!+k0&$q2pa5Y~GWumlBJDox@PFt3;GX+RAj49B(?s97a*=B)assXB!fb24
z;G)#{!h<dCKnudY-oV_u+$6$)%6;K@VT-Vc9K6t${G8nVEFAsVp8tg1`(l3nP>XF2
zt-s4MFXg`f#IDZ{_pcw=2EOpS*!$>0v3I^K)qUwI!tUxuQb91)+tt76Rwp&?eK`DH
z%TWJW5P>GOhjz^iF9>d|f6G0%&lPlwwXxQRR%&u=cs(cqo~5T}_>R5y-iWH+wnZv^
zDSTH1^U^K(nI<!_v@*RPS*CBS2Z6!HhUMNx+X;<KkA~eHdIaQ<@V)S!VD$3YTAqAf
z|8g@u0ikPl4Ei|WTU!Hy5%3NB7Ptfg-`fcT>HT*tWDkrs_|1<s2?Rd269(3c_k`K4
zN%0xTh7Jr9(Sv~G$A8A^5nJ$y*)1yn7QJmL{{vy)y<+f+Y5xl?>YHC`@+L+kCS;h;
zGOzrhXVA(w@&}{OH!J-cQ4=`(XCf!^2>0!Wt?PmQ8=}4eQ25e$>dNe!g&8>3v+Mb5
z_XhCgkEOK@@zL7HX}#dv_cCbm6L14L>09bas(-C-`|U&j)8dw@{s|3mJ=mGsnP`54
zP2Z0ENWJ;+9@Kl`eDA#axH#86f0@7Zt=#dx+R;UC`B@@UDd{Qx*&<nf0crHTY2La~
zsRJ-Jwz}BAtA4tM5Aenvad!tsW<UI7{XtsrgvR;-+j;wr>3ipal35OsIsRdAll`)N
zafMFZ8F{(>I<cG{Tb;h!QQFx!ey4raiQMQ3Imzz$=-K5ojol&c>h$=9{d0ccdsSE;
zdEjZ7_0vVSllsK_I-^SP=M;`9<b&PTbP~x*9clAjjyUK>q?L1;RS-}aFC~5Qv`{~F
z1PRIE-@(9bw{<5&reB6>Y^)h|u3{H{x^&T24#&~NE>nKA_96U}dB11_y}3`M@+0(J
zFK3Mld<ns!+{b;R3Ss*MM0rax3uk_B1%WC<G^F(i(W(RavJOZ%nwxp(l<-WxWVodz
zUU1S~B5?i{VKU|z#c*sVmX~$*TaTweyNz&WdwVL+So)J7;addc#PL44xL49Lkp0|#
z+Ob(Gd955N6-E_06zCJh+9{%6@)b`W!?Ulljm{z47C$jQ1IK8th4MvURp=%r-!Cm)
z#=w90rDE0$DyR?z?}twx`_bgrKB!CPzte}2WxGCtomVav$b2>HN#_y~%vyIX?9nht
za?J(%R$XqXLcB%e{*9tE{bM_izhYMTiROUooEbu?2Zf*Va0BAV^_dzHFxcz%t}VIq
zd7!2|xzj8k&~C~>nI3pLY&DC7_=10iQ)iwcVjH*p%)J1j?1&-6)Wk4)Z<`v;RICQ<
z?OX|7qvg9M4JB+1wmJ=3yoPpMntj*m$UUmJ9b}trYudmp$)ho~g*&4b61=M<SkYLA
zXVzetI0X+(h`2EIn_6)wQJ|~PLRFT*c~{sc`90T}8?Q+oqpHizdQ-c|?Lio{*nRb5
z&!iU#h<r{4eSeN*4#P!f2tS%bxC3pDd@3VvdNQ%0JgB4&&rsU;*<8S2icSdshQ27n
z7(aT;&L_gT5*2S6YJmpy<I1Fdy#0jnSQ^5Kswe}<-86PuGeO9C6zNE3=E8E^QmBpg
z51Y;2hUKj`IX1%%5n#=>-C6V-NuM9KU4BfBSCkG1d;x+p?4Q$P*-??qaiPQ`a5@p=
zdHHVy>SI+ziei$`CSdPwF!+>0X6JHSI=_BDmb;$U8jGfo+j~{#+sIuJ4z<Z4T&=Mt
zL&nr*EsYC$4jp=cTi21v!f2B<@gnBul+#?qEEuHkG_@kNG`WmZM@tl8tlt2|eXPOt
z&}6_1eh%H37Au{G`5Zp{@AMVsl^jV-lRX;Rb$@3?=)VC<;$hZ1dkK#OfY!v-L(?5P
zs3g!i7wLDn$rBehQbci7+=6OONkHUwN||S%y>YiXJOKi+3R-MRU0E}Kk#-O+$X}f{
znmJSnlYP6yHn5sy<_Pwa=$%Rj(N2!N5dV*EFGQSW8@ua{nzyMk1UdJ=ve;!VS|zZO
zC=%7r&RgD=ia4_@5{)q^bZRN|g!{V_9ERBzvu6ExG{kkAGNB)!`A6h^AUIefob9vK
zbLitqofX!zd(`-edWky2z%geqs3N2YIVCiOIIYo^U$bmZ?+mral7Ms|(yC5dcjIWr
zqt6f}@f^r-T`BHxab0LZE!Ac%jTQc;XR$@k#let<oXv4uoiiIgUccWhul&{^aD8<?
zpG`oQT$mDbL<5w#`B5%PlqBC2Rwr+Z6u<N!TCkCdoaeLDLqJeNnlCunY3grIAE^xa
z9b21b=u_<WZYTQ-E7|~gL5)d-RL?<=hLJuu{gK`<(4ovEqGdK(QtHoTNho_C*O2ai
zeIueds;^+|q{YK=zo&7_04pO3R*CnO0nnY4w*-N*0|2@xlMN4^jtLk1q_82X&a|Y$
zTu(J}pi;>?ojKQx>p@|=5{*YKZHOK5BTuTCOip;MI49vXPy?`HZzsf4WUa=~-c!w{
zoY7h8^`*kU`|u5D;*e8TC+HwtgL-I(g4xDaYfh5)stXN|_I5A+14zhaOsB<SBZ*7N
z&jv8VtT+HE%bbdAoFA)pa&jL|zfpZ|NUSR99}R1#3F&a90Gt=Pk^t9G=<e<1B(8J7
z1hY;@W_;h+UsXm-U+M1wjHvJ?r{4m~H(&>6Q%|L7{gL>;K02v>U)i5rEWtVnjN+$)
z5OCiQ^;3fU8rNhKQSQM4{Y_4*R`AC~5`eWyS|{HcCcK*ePk}79OG}C8iJ57G4;-t#
zl0%vxJ@t;2+|-c`M%#(EZOjk^t0KP8P%NuWcxEp|rjUAYwB}ix7g<CkiLD~Vrzi<%
zR#)qyhCA~q;FDlO_HlXXmoy<nBV+u+TgGWonWKrg>8ihBpE2g#k0-<uSY3e@+i)rI
z-_nnU33S687Al!)(Z!oUb}DsyBH_BBHc5Uo7UqmhIwnJb+Y~#RlFq!O8TwO>qp+C?
zDh+>nZ-2*i`aIpndchCoTr?mdd^9}4W48v}J>A#6HhYc0J-Y{d>(il4Z#R&sA4LL#
zo4~M=4P2S9^0Z0<;>Jj8%hL7PFtS<Hi$LOM*-CmBNv4LMofyZ$^UEK3rj2lMvgzX-
zWC1@(i!J7xt5F@4TfiIZ%YuaE)z75LD!~YRzRUM3O}LUaCLzW2B>K9cP`f4Ap0$!l
z*fi?jnQ?j>aK6{~(;KJ6<|x`%{>7bgB8{!cdG=yZ?zpRwbB7T}{BOr4U{`yhwjlUv
zqY?kNw^JktQ^>4wQ-^dn=-H@ADo=A@%Vpil$NMS@que&hiTFL^@Unkanvtgof)4`t
zz3`@K1)By>A(2*pkB7hG#^?R`udntP>(a7z^Yj=jG`PC3;lV}2#>XEWmNmpofp-e|
z+SR%atfG+$>eMpFbu_vd4NwnFoM-EufiP~m9!?WI&xT&?c`q7tA4I*er-22lI3DA=
zkBEHtNM)={Gg4+=0@fZqcWiQ<umf}?fg#cmC{z8N&d#}N*yMigXdpbb3kyYE2n;>Y
zt-l*_FcQ=w_|Sxipb%AFTi2CPh6HH{8D-gbF|KA=#C~EU?nfqs9ha6!5Y?_7@9%yz
zg|7CytZt1d3=y;l0$c=Hi2g>ksG)EX(;o?&wNx2b*&qJ>fMawkW;e3%8Jv@T(6_v3
zTjpN@#=QzoFR2%MdIoP#P)J%o^?M*m4SrX7W^>7C0A5lYE4&4vmD*`{leYQaXWse*
zrrJ9sl#d8^$f@frv)frSr^oXNG<ksb?!Z4oQ>FI7ce<`6j>!#-vaI=)Wcs9^Df6Vn
z4e5B2<ay}f8>cjWGNd}22~HTm-yc#-ty8qCz0Bbu$5XVsjd8>pyxpCG<wI?4qpOj!
z&=EO@U_gBu*$f(@?r$}&y=%!w7YM*7eIO%8a6MKgKb&jeQLuAE4@w{qv*0Urh|xUz
z>UPOep0ghgWmO_7er)^EKko=%t758ZE|3OSAPbwaDGB#9Gi|9E29ZfqD*|i_)l)(z
zBDQrgNYxk=H?gXoz_|uvql1Hdma2DK?5<RQBlX%gSaS7R_s}OPo#W3)_2V)p$1@tt
zuzO<7hHG?0SDSeAO%tktut+jIs?Cxu0O_s=NCMi8V7`>&cqq$Dl`#`&bSIsh9=*vy
z=Ljc)Sm#!ePf#iD+A%B`B$aeh|4tGCYW+)ag;&wjn11;SmzJgZorzotR|BRqg=FQu
zz!0#p6*{<VzvrLJoplqWiuwGUcJ`f>kA}M%de8xr2v+qn>|oZ=+JZs%R9i6OYW2+)
zplBxke&w*lEQs~IX9jzF>m;W(J4ZQ8IKU_@g{nZv5mJ?mldqOn_Mx8w+X}l5-u%Di
z*~#wZW@kFpN)+!9f|bMYVUEUoGGq)Lxd`zOn4I5Xq-o8O1uaD*Vkm6^H<m31#k3qY
zUBY%+Rg1yL&nblR<Ou;<%o}JC2ek_Oa!9vSjrCLFzOEX%dg~v?RSTO+_TrlLEd3#i
zvOxtzv5aCJYxsG!%SM00%rK#AtRFKzd4SE<mP9OO6xb+q$OR{{B)w91cS>dR5C(%6
zssdKX_Ml$Afb(}B0Yu$_KCGP6>ZMR@IZjmLGouKeyqHbHO^FGBA$ciV`1DA?mZvHt
zd9`Gi*snHcGC~Ykm>wB0Yev>B_q#RY2f+?LDFq>V084f2-~$k^DjmaFMeM*MJtive
zVX5d6Mv;kc4ANP$2^62<XhuTyqMM-9GE)mi#n=pdZje8bU-#7=J<Yz>uVmyqnwo1y
zXad_MD&qZu*iRQ_+Blc!M5y&&FtzDLcko$)=ki%_ZJenF#ZUNC&p-&8Fw*S{#H!dw
zxZbcGY61`WR|@`9q|YCxE5g3nu|Sxqml>B`oMU%y-x)xfn5y~swV7oa===Ff529r6
zC-`NF8$!uW$!mx$?NXK)K1KCqDskr`mPXIx+Rac?#}7A2M?<mi?azmQW>377X*)bN
zRVMv-unOVCQqC|`QR%W<^G;8RDEAj<kUv_`Wm_i;bpt??e*wHf`$Sx`@6(MRjOy3a
zq`D1Gd#G!ws3-cX7ewKDp0C)llIXgD`cl})-%v&{aK$v`zK_Cc<F4NPxlGH&T}7z`
z8TuDp%+_|kBzx{dJ1R@5)WHeHNq`_=y#Mh8#DDI63aZK2N*I{_?$~Mco3etQ<YBJW
zlR!+r<Y440rKVB=-xnmT74zWczAPobSn%s0Y;mfZ{lgh&D186s;AqG<cKKR_$oZGH
z6-%qJe}Oo^xpp=PJ{4BvKqt3yZ*TV+;SBH)3zUeV$_~?|>p1*6!dI2kWX!SEL9S_4
zDfWIfP2>bb?zIql;Lq-5X9ZYN6KA->Iz}r+x*=bq@*<UGL4=X<nCr|nCpmgi4fPms
zxyBaaAagpRoY@`FGZN_=Tr4fIR5zD|Zn4e2cck)C82>d6L1sKTY{`F5E_n3JUp70S
zY&HusV!t*$LB#8im4Dt85!$h7`3T1Sb><d{yylDq5;v2ny)}EGqR$D6kBfCy?n|V$
zvUNlR({Zt)hXfTwo@^;qPZ!Eps-TW)i%W<obSdo)K}Ll(7fyODdp1w%Qzkbp_jf4j
zB)fVW#am_dPq3BL0`gVOX-Y8BexTl}H2UO~P{&mQyC8ZSb?rE7+722bJcHsqZc4yL
ze^8URi(HkJxeeJ;sRWB=Qg3R0-I=wE&EqAH7x@YVb$y^NogVeQ(NyWjIPidxvrWKd
z(w_sBs)LxaWaJ4*olv*a{KSNcPY@VhSMT>jgMJJnCA*YaUv}z4598WSKnzSl+r2^5
z<HR4IR(#{Y6)U|cVd{?BsOF=^nvUDGBlWCOjcnWjQWDVQENlqCFsmaJT<V3w?{Lhg
zeGoMKuQi$!o%Ym`YUv%D?l`6sZo1)F3_3bUsBQngleJ*bT=T3*h==C)99mA<s6N5B
zVcycpm_nr2_&|Q2O|M2nc*{ba7X`!^+K(dX4w8AwAp`VB@U9qG!?+iS84Hz<Ac;iZ
zC};-TKOVQyM86P)<uy;-L5RxSMNbtQ@pD>*d4vqO0*b8J<CT%9bwEC&fIeBzYTt3F
zJj%A>h7I9)w$(zWSH-OBXm#t|`dZ!6v<$%<m%HSKN+u*AMB@+;6zCl~(G-Sc*vvrV
zZeh<&&DlDXAx-cTL;KMj8oeIU3>}C3JL3^x70g$x7AcU+a(xin$3+)`1LqF}!1aSm
zDLJgTItDzS(vQ`%b>C@Z8*faj6VbL1ef#Q1*?P-uxND@mgeh^a=Ey;^&7ZhpBbvYE
z5Gy2Ylz{YqEK=$P2JwLdWw(yIl#9Fbad?Px7XYuvoOyYwXc=9&?2%$X#}<F&-}f!(
zkSzjXR#2bGH@fOynZtJwDmwx>!=S@viW~@O9UAyex>{fPV*bQ%Mc7M<ENY8YW)MOQ
zVbS9k)V&wvfK2X7x}nJdAFMi!L5t1C!in5F$cjp-5peF2EOj)PnPR|E*UN;y@Lj&;
zpHx3)r7IG1BA9IQ`*V_;V@h`a`uB$n*C*SlHQeA?pStknW4n+<qK8>ZW}E*Qw%BZ1
z=WCdhV5zc8myBhD%D#SvqSXOMX2}K2$1n)LpKV<bhMsC`SS2QIKE8U0igQOkB;Y(y
zx}3~*wrZciDneK!)1D`lj?j~}({Zc!<uJrbal$OH7A}yX6FSojJMI^5Nrm1;ROSc(
zPxRiL$n0Z4+fVSK#w>~ape`CI6^P%~q{7GIF(<mZsINsXZ1z{hl#>M`&E}_sdzSvC
zyA8o8|6zJ@`BmBazJr8OteA;LqJEAeh~VM4&y5b|X!6DVc>jAs!2kq3gS#;IS1!Ik
zNi0OoQnafu(P)J_9>Ka_E8NXKOOZh984!srTvH~I{(ZVGFPs!e_<bGocuzu_lTE__
z8NVcY%a*LEm&iF%;m)D8>R6qa@1=x5u4KMQZejUM5H4}<W#BB+u79*lM428TTylM)
zvblIdE@4zRqbk!5Lbuv`{wFn9XbpE;0i^8Fx!sNAtxqJ7Rj=~iwH5&C<lo#AsJ%Gi
z)h#HBmtFZq5Z7}khV`wAU*Df_x~4rHE7lDzc~<kkHQ}0!kNM#3CC+Z0v?wgBg4%e#
z!7B9db}C&Hk~vHpmhX$EYJLL;eK2s`Ygnqq_(Z7s1HI~%aiDvIG|T=8zk{QYk|LjO
ztDFQYSfM_Cr43=E8>X#xy^=CE-aR`*t?coOdza4(oUi*Us~cxc#mX<e!p2E#nF!`m
z@M|e7nUfldON>4l0+0qrV<1iv>pE(fkez-$ug`ZD3YhIAVIW&MSv{V+YRF&lFcD(*
zj#$q-wm+jufZiwCK1HRg+7K`xptm5>7{R&K&#n8zI(`SJd11ynlW+kp>g3W3Js!yh
zlO%L$BRx!O%{PdF$7k{<nMvH5o;a<|56Ti_^i8LCmMufC4{lC1hExT{q2~zh^ZN&@
z?e~oJx%>O1KVAmG4&gqZ96?lhF<kOVL=g8`sRN!|_UfmYZzh|FokS5^lY&pu|Kc+c
zdavQg3O2Y=xESK4@7mKXwGo6%%m<dQnh*&xH$wB&GA*`E+jBSM`=U=E>s%Yrp-evR
zU(?~=p?X;K+Go4XDnBCl175{|t&eyn9TN?YW&ro@7-QaKzoWX`;pA_S9+Iqz(rsie
zLB5?w%Q~cu1=-E=v)VttWKsx**q4e_QNFj8^VS@%i?+RRo_8s&Y`?88!(Pmt8JEJt
zmv(7%Mj!>JUnL$?#ede^>_Q>UC0ZHw8o5p)SUw4Uihz2g7=x8Tg3m7loDbGAOW1;-
zvYpanS762r%VwO(kfZ6yt4-*cwKQD2qG~w%cQ9;zGaJ1oe7Yn7&M+MEZ)Kb+9_EuL
zfTJI~Rz4rFZXY<rJ)THJP|2$18oS{OvtuTye5A?mzVOb41i8DMv;0@VR?Pzc?8-O>
za*QiXG3kVYV)~rR$X>-a)X;*7a7}i-cRsc1OZ!8cDUWV|MwGpI*LZ3OdA+gVX7?Ii
z?U=*#6^^JXIqZxBF;JoKd2}J{^OYFzhQdeIu0wkJ!}96nxyB(NVg{0K8p=kA0H|oO
z*w5+F6cL9|GRV@2^0Hc~jY9oY$b6F1wn;XE5U?Y=@#j@6)=r%ElTnRjSq^HFWXYcY
zIn6khj^Phuj6wcub5@^DR-YnSiD7(BN0U#h{vC(&A~FT53zxr8P2k7Ky8;QdgK^4T
zFq<_k5;JBJe2giR7)0&GEMLsyk?j<P8}X7*+qhiV=(ol@0CSChO*2(=D>|1N7j7#y
zQyS;Ni{Fmd=C5efr;4wc%Rl`o<BcLZP11jlagD|4%s}}BLh0ElJp(F`&xx-vEheU9
zt8M77TN=I$dX8bN%oG<s`dj8>>MV4{V{#d1Ur=M1QI%woUyYfA^p=w27m&Jz5b@8L
zH{sq#wDHy)jI4rHvClYWgftyGlXd0&%#?Qo+R9!fXyKubbJdjjl~zIEPN#_*IeGr|
zaA^mDkt=_rY4yK^*h8C`o*TTc5;qk!`RwbC*0ai`7X<P*s6!kS597(FY$@F{5;CM_
z+wy^Rz&UIwosH2tgz;1vPf7RChg2<}Nb&A(PZI+cT!dB5o3c<fpOYwk=;Q;_p<k-a
z$7ozyxU$Jfezd@nqeBxg#E;VqMc(B@clPrpDz)22|GBm@u!Hr{_#+(p4u;+z5xdu+
zm(Gq2hT8mp1a!ncxmAs@^{RDYf<{d`7T?;q$?$JvFhZW=D+~7LT|1Md#q-__zBqhV
z-aJh~%Iw$ex^4ob50rG`I^|_YK*~((Fz083Xi{$AtN>(hYt|ET{oVFT&UfLiH-#3O
zF+MD-eb7z5-uI8Og)vMPzEUZ){)5h+aI}Fuumd}AXCL@uPTo{hq!ayFK?-MC!$-D0
zWro&A32_V-f#{S^?I%T1*xR<ff?}+o9TM|OUKj?rP>Y6}JjRywd^=IlQc0PGTKU6`
zK(o}xht+vp7E<yDR1uE#h0?lyu#J;ym7=_qahdOiknjOv`5P7CENQleUK&hD9;0Q9
zaV<*Z2MuT#DItrr1C*oOLT_d&*US4Uk&<Z(4pk+bM+%eM>bdhRW8)YmlHSM>_Q69?
zU(kD_JE>L)^Aq1C!BjtgMBH??86jA1AmtU3_s!(qg`&+jC1>qD$PH$mI{k^3?QCDA
z33_IBhXQxv1a17g6eXVMX5~DI9h>2|UWvlWKl_F;!4TF$&}CB6Vi)N>piK$znVs^A
z5U8lB`obOn+FUa~1wMR;8c9PuU46iNIl~k1D);<!$#mM~Xt!qRbxQAz9RkSq@U@Ss
z;glWsgFaFnVzL(3_TZynOa;|(V{l0)WpV^w<cK+Xgd$gprS5;6$~yuC07!@0=MNt}
zFoJzfMYrr$OZg+`_S>Ej?&K(=%$rnJBd-T}OzxoRB`{iv1jf*~wgd5JC&^FsBK3-o
z#~*v^0eIAdIc5T9`m4A9rdppLOjlNOv(7qH6MhvWqjNYnOay@*)>&7$>p5sH!e&d!
z6`mzC70TlD6G5ZNJ_mlMju$}O{f04T>tSIrF>Zm<<EW4azf&T!zYwrk$~JqC<6`Dh
zL>+>uDUF>elta@Li%=X0iYos66Up2IyZ91mT{l2+>$=Dbb;g&;5l3kl)6+yEiih2}
z;scFRE_7LAkZ0m7Mn(CTLil&{FA)5)HeTZ=e4+`<hwB~Dn&1*R|ML>NMZ5Eg%8q#a
zfO<{d4A}w;FOE3(Tl$_pvHO<E-%SyJ9ySDnA=vztvXm-7#uSD7B!~pJ!(<_ML3shQ
zY^a$(a-nZonWd#?>T`L=j_NBJe4M~zp}@3R=8>>^y+|bD)Vi$cgxWiq-&8vB{@LT;
zwu}Yd+fg#^Gz|2)LTJ|~Kj2^gt+cNItE$-=wnb1eu@H|9sKDuSR76TZr9nYNr9%*;
z#KIOaP_eKBTQD&YMeG(6>{kpByBq!2%-)-`_sn_k{k;Eu?(@93&di!MYu2osnLY5A
z>1LbQy#=cdrC1qN-kw<SuzFD^<Bmgg_nvwx-`eh^V}iG{SAOc}4q3}OE^h4daMa#(
zb-b(j{obc7^&iTMEhB>dskY#isdLj$>IOS!Ol-c#cJrkTjSe<Q?eHri(=TW28JDF_
zF2N~=9TzpSyg8@f=j>*>>nCn<4tdq%-tdT+ox5H7+P~}n&m*7JH9L;$JN%`2zmxiR
z9aDyv544PN+UucwdL?`Dg#(4TgK|%_5RKI9_#|m*^(W8v)$pomcBxkMv^DJ;slA@u
z+L=48*^h}^lUi+VUfRRxYEibrt!uz0`S}>rm*>@yH!rRq*52TY-`lsz?snrt<0g-o
z)*@li#fN7*?r-$)g{%EjqaPD|T6D8sw4osErNO@F8}||~My*=t(eF&pZV`SJ14aiW
ztPSn?t1>U)V?)D`$9>JZtQqmoz+R~Zeugm*P5RsYc7E|WbkkCkk@fVOweeGJHtm)p
zu~#$>w3|Qebf{&wxw}(VTnd_5khWn!?#V)v!OFa?hoAJQt!&b)v0a^vkF(r+pN=hF
zG-Q)JbMH8nTz`{Yv)6^K4urR9zQ4=xj=>XtMT~Zi_;~p0(ECnSw`yJN=X>LeqKEsC
z!V2pp`tm8|t*&o~e|n+UvH?5#R$q6~Vf(hd_Sd~%96a|#r_yrgzRQW8YeOn#Zc05e
zeXhmTKbMB|_kO3^)hRB&Q)b4&ukp(~f)kc3Se<-AtTRRa_HOXvruw%=ei)<sAi=V7
z-^0X?Z#T+qW|m!Dqq0A`L-$C7N1sO7|I^~`yY6*c*GUL?*UGTYsw@2$u9Qu#?bk*=
zBX(fOk0RxA;~dMptWMXIxp8st4(5DU8y5R-($D_1@$<(U-#)E!J=vq_hSXWJbUh}n
zjX0;j#vpEe<CS_J&mG@CsE2xPb@7P1-R};aJh{bLpIF<kJ)=6`ExEC@Y?sI0kxwMz
zEitp4v$`)E5<6ph`Kfnv-}HY}f6c8e>d(2qGds=RIA_7f%ilk*wa)dZGx+DlD~=!g
zxSxpYY4>8x$lkApM;vO||ISUD3CV5RTKut_+cMN&>9MobKc{P&r_}6O=d8iQue!4)
zCB3noG-QC&I-}n;_GSC}B;@=qGyk+S<Bsc{k@Z%u9RJI{;6u0E&QpwZZ60?P9l7M_
zY;hynefp|)OLrD0d6r~#?%J#TZp-28#{MzhXyh!KoR;iupHtwvtGA_V*X#>F9wwyD
zd4IateepJR-xdC!=T?vOZeCd<q-60Ax#PiMXIFop794Uf_t~wYcWW}@%Tu~<A0M;l
zOzUk<))Ru8t-t2J!fk2OlP?y3OxzqX+2p$G=y6AMau=4jc>2C|pWgWyL5_8FboPXI
zD%H<?6lqb<Zq$n%H=n=hHD--l4c}qmuclX?E_(f-_)-56pSO$Lr=5$8TxK=;-k2?I
z$BuE27&qc;-nTYuZvHHczfryam!B8cNd3akPn<U7j<;#k1Gx{nMlA2q%Btz1&i$L8
z>y|z2%WYKy<JU*`cIY$JcvoIoe)l8y+Z*S_)}0mPH*=rEZr5tXX;Vroe<Vj_jd1cG
z(R%Rf(u#1al~?Y0uFGjyyS?`WqfeG=hCb<<y1ev>?psMyX;fiq$V%P92Bne4C(Z^u
zYSnPr*YmX&wn(*3JtiyOyQTJsr~{XOjFgPn*E1y9u#rse7QC}yXTXL_t0QMDd_LRw
zaLVyT5sINx`_=1kezlX;A+ONH?d!z9URcgoPF}fX?B$Y2^;+C)f4;%Jxd&^cwq9ZS
zN-w11%h5j>-x3}^*FDlQ{j*-A(XbX?1#1?T%Isd-FMJeOb7@l3c6F><SaluuF05hK
zPoa*j2c2Ac@P7KJh_@b36(y?`?WP@=kU8<m%6sWMrbIrzbZtXm2m8xr8#?Z9KYing
zsBaBzpZQGgIlb?+(}#?oWlU1P?YQBL@wns7@1C4`Xuy?yIt^W<OV{bup5wGSuX(?m
zR(>fW^V09u5w8<QnmzrVwk@w(QAK3PEb|x7$``Kv;5=f=*tC}u<1&k4c6V#?#x=EF
zv$SsibnpwWALhJaNm$#xSKTLduSghZ5H6R*T$`}+UUKBehsQrX=rLKZzwO-K3zq#j
z^sS~{#gX-+8x*&)%qeR=&!FSCJ-RcjCQ0J<dwAtN{JrGR+a#A>I^I8DDm-VEH>efS
z>G9>*WrLh=j;^`zOquI_gV9+dN*dZK+P$?K(zT=G=9b&qjIq^So>(#AXTq2?n;QG%
z<IScfw%>4gp-Y!LVN-)2&p2!J&!B0(8^^8R^Tqc0Xhr=;!?JB=P8|Qwi>Zr@RJQT9
zPjlvP`uZ(S=@O;dpZ4qQ_3?9W&F(Q}oqpN$dTZN!_p!`$tax<n?DDJ|BX2dV9b7Qg
zXQbVd@4xKy#=9)pa4J#1>BY40=oh^w+;ExPYIciz9_cM5A4<hW?#l9G(^EEndSkeC
za@m5)K-uP9onIY${`6Dj`CFy?)_56o&k{AX-e11DsZZ;?uv6zdkI~i1_~hMpL(Dq;
zR}WpIGT)Yk<)6vEW%zaJua?O<>*LLmpNqb4FGyRw_E?mDZ|`9%_ARh$`D^d;ufq@8
zeZKDau=rlfE7wbhM=K0`&m@bE1g#4ynWKAoQ1Qz|WklQ9`?DULesgc$z3!pk0*<z7
zDb9NI*l>K^uK~eVevf;8==d1L!;=nwM(<vG{!#YOtzDx!8QV7-|7B>qvW2$$JTlME
zHtTZv+U?fe?R#vSImW8*PqW5T;&<=#s?kohV({5F4u>Pt)+s&TtE~6j*_u|T&-3@I
z`y1S!o#c|$t4>mbxF>g8)|>yy^F+DdUhfGXUk&}*Kv%ZRF7&`{7iCvF-A6MjGuAkH
zWN+#xGC$F=)Is)U*RAVuPHk>q|21t!T&7t>;1}`tV>3Ia$C|}l&g}50bhpFp(e9m_
zPq^(=xzan!E7bPYrgF<u!wnX_doXiIqK84cPSdk3)(+~kwz>b_mt|2s{ofYV_f2sq
zJMrdyw-2q%cP}}kwl&|k*Ee^cS-^|H{9Q>QIrH-(?H{dLvNFT_=(IM|el&8leLkb~
za<5OjXUyzgcIWjLzfDUP<bRnxrdz|sCu<dVTzPlCwORh~l~cP#p4>CtqC}+1emT9O
ze@AIj;i{|!AB#ffA58!Dcvn=zLw36!OlW-A>tKE5_wMi36%D%fJTLsjiK25a*WBtD
zlctIr>}x1dmPG&PU+;3`jWIPI%rbl*w<~IJa^(FkGsgyaTL;%1_pNsBgbTXEnoj(6
z{p|Cdif+TE4VvKJdVhA`H;(bkKIKcJ-IiuA_k0lgb@DFLdpQTM<czml|D})jyUR;I
z`?UCeHp9$6yYB8g@!QR91|DBC`+9g{TGX$pJ5r}@`srY0vQKn!>!uB6KkY`(u4OuG
z^<}eT$7dyGr<9H}P}JG5*V1n5iijbnKi#-kx;{%*663yYLfX;Gxs$B(e)eoz*xUL=
zuiQJc@|NdqAMO3UGWGL@7yfpAe4O*<I~IK~A2YFqOOv4?r~0g((bUap!u`$NFAkY<
zD7JK(Z*cuSXU&Yl!cLlRH42RTG^+c%ikBz1kDifJ^X!q~I(Z_U8}8@MF36Tg?44vk
zH(;H~ZQt`VyWh*NJ;2qY@19ZVu^$3YF7eqsexJ{`lLOz~%{_LY$l%APy77C8s@po`
z+SrWlp5gal+wc(&K6N>~cvgG=v3K{rp0;4`_#vOY{M~Q3?lhdaC|i{>BBAZAplGuV
z*H2!$aJc{DbA_Lh1GZer-5)bH?cMIzjcRqv(+QS6k6YxMa=exLuFN8QL+4c!XFXcH
zprLdMq!^mG+%I$rmx>~r)^qmgTT*;@ZC-PO`5RhEXUsg8WIt-1UrDFM8DB;5%TwF8
zb5pboPfwl_P#kyc-91B-KVGlawLCLjJTJheIJIGhyz$1z$M-#YF|g%}dZ`M>qp$K>
z-M^7*y-yTy;dn&q_X&x%p%oYFAC9hl&^P+Ax5EU9N||$glKvc(tLYBWl}-H?e9}8q
zql3ln9P348{fGT5l}@{mr}wbLI?2CI%BYPsrsg&cZnC)`y5iEg@=uFrJYQSU`nbG-
z&hr;P9g>&doMA3HlzHm<@Z?HEFAMi`rdOOMb>1(2d$P`xb$y>D-pSUR-Pp-y+)?Z7
zOPgzj4xJRz-T3pnsF3<zb*;K6)^D5E?ZU2JPud>MYI?QfxmMCIA0r#gwjQ%BW8@2;
zO(*jA9yltw{^FU!ckuKU=Ra@yaiq0xM%s;5b))xJe0!N?lGHk`;lYfHb^S&@d61R1
zeG0q{JJh6B(Ga=*z{i&cdkqWx<-5yVC$0Up>UYF->lGw-ODz|Lo=%CGaMAM4j~+%d
zeoQUAF>mU?y#Xa9eJ$!%Sgy`Ka`|qm-R_5L$B%6KJ$K@!yD`1>?z<g!^J_D8$<ON9
zZP)&4+x4t<=M{NBBMaWls&#pOSx~)SKMJ}n%q|(*wsWG)?EQ+D%ZHqE9qQ2jK|o)_
zE`FoWyOy35m-id6(xjd4gPAX{oS*cgV@^!1^9R(aqaV7tbe;QPc#e0U53!d&bhW*h
zWcsD$>Ip+LnwRBGo)%az<z4jKht?t4h2OsvKCI>Pyp7Y{L7|Or9tpbCu(i>lqWLc#
zJ~pfT-1__O(w8w^)4R^>xx9NoU_HN{C!dC<hUSPJ?E08HetGw5ZMFGLJ7sR~l_tGo
zdV4_2bDO=QPIehNvB4hiviufjDu%7Q((cQ|?_YLR_D&da^vtKl3HN&+UF>?LsHMc&
zZ)3!T>}wI@D<{40HsRKR=X>L=m-Zd|*;=uDz^*PSp4XcWof&g(^3!t*6ZLm3eq0#(
zq5N6Esid4+HRtM#_}=ANy*BatBcIi-R+tgJ-?dG*`xlBMUXN*7V(f6q@34Dr?@fi%
z{0-(EOU&!Pen<YH&Z5EBUa#Hu>}yf$oVk5+E-VcyYuSHgUENnXDRuQyb$wJ#ZX~83
z-8i7#+wT_+_~+^**Sp+#T35RZSs9ySA_Awjxw6sfK!?LVJx|S*&l+*ku+n&HuPJBZ
zQ|7FS@G~@2$2A>Ny`x^2og0$3$lg9q?pryrSN^tP7v7j{yz(IR;DhDotA!PM@0+s3
ze*LxMUZc~KL!O?p?9gdS!{-@kHaiwH^j35@A&ooRAxAwwZ(+|`6MvNHt$q73`;(;g
zwb8SF?pd2KJxUZN)*rsx{b~NGHM?Uww}1C2DYW$A&$B-+UTs_1rJ39NE5@(F3ZBXW
z_8)k+_oL}}pUvrKMQfI|y1zZq#Aal1$<vJ3ye+SP+4M4TEbTLH;>GRzl2&v{9QJCh
zSH*+{oqQ8=2ade<C9Q4ikeO$Kx4gO1{CD+`(GuUO`ZYHW9i;Os<)!De869RcT%5FE
zm{pzi9-*~#*EV#q2)1anct&jeo%ToU?Vd@#=}k2Zcq{gv)8=aQmSdk*bu5UTb6B$K
zY=ar_NT-2M$f2W~ZI(XXeJJ_o?L7?_!KbQJt6OBQ-P0@LrP+KxNtsW<0;89!Y+dVa
zU)K4DVV~Li78vVRyidAsklEh1#hh-9Z&`fUykPveUS0G4iFs#Iy5!K4aUaqai;SFS
zH=5+I=Bi{v-i%K(pDne}O}(WaoBqh;eWL|ojrVoBY|?K^hskft`dsmk+Oy2?b@70t
zOU_el+kY3g`_SU4f%?XtA&q+(_K)fosU8_w&$a)!VMpujvC4wi9GYZo{;uD=<;kSh
z6IbNT3He>m|G~6&^Fni+ruobj`?@V!`AxNaoViH8vAek3BWy>VqcK)34TCzpEx8vn
zXKqAub?XPa^KMzrY@Js_@5AK-bv!%nIMphwI8Ir8&Z14<`rJP|`tCjX_l((t>{egR
zKM^v=Zprwup*KpFJT09PmvX4~NUxlym(`o{w`CN|ml(~^J-Apja?HYiJPiuxYz#S3
zQLuYbg6*%X;xBP0uGEWjFnYCOw$7dRjV`>Y_wz@PX`{pIqZ^;u`FMKLxY%FsH|&)-
zJn8jQ-R@MQw<A6KSas@~teX<=U@+!TmqVtHmWfxaZGZhi*Bf&aT3haI{=4*Q{k3=3
zc#WB!ANEj`V3m9;R`UKypToDz`|Szw8|-3qb;Se|WxW|kckXNbyxQ?s#@TC^n97&#
zGG5h4d~QjNk$UA%ni;On$)A(9x4HT1m%g{E9pCo&(BO_Lzx7^^TczpGe7AP%=Ib?G
zb}i5wa={={d2y%on(nX(I{iB@U1WNz-MZbUN?VPIC{#L~a=l#q!$LGKXTSfdj9ow1
zd#`WO_^ofZ;j4d5{B%SVs0`Y*{Mw*pjk@RCXBB6yY*_bv!;3GM7KUF;tu>;>ORp<!
zPrA7;j-T$*VRH9pvz;4S*(CIdYiDCWKgoHGU-i<V>e>rU&Unt8+;4x+I&z;zu2)R<
z%U<7o-a2qsSrc!AiY3dxiF!A^KBa%ZP_u&C2S&ssq!ccb$rc-0>G+kU4Q^_6f7*^A
zzny<RDJXmX@X3a5M>aJY)avN9mPeXyH*I#MLExTr*8v-sH|hJ?`sLuR#!qe6+;7`B
zWrgRK4?epspXyE+|E{*7^Rc$MKE>ze)qlD<ZL;Z&#2%NdEr(4?^BiyqzGo$^qnEUD
zkL|L(x)a9se`q{+*U7MzgWbpa9QOGd<Xkf*#>vy_zAWs<vK2-}z2)7^^Lmb1ak}K-
z@IB^D>W>H$-O@XI?#|&BzDGV9)O*s#W80LE=WAa+wW0SjtA>M5kDuSX`Dg#dC(j-l
z(ze|2N1wOdX8x1lJpKM0rx7QV6K5SUO?UAfaJ8~;_n<Z_bnNRWGhg=i-nS^eM@lWX
zBW68YZ;hF39C!WhFt<G&d|unTbsUv7^2*Lpje=^acZQglpTC%rFw6B?p`P`=)`{7Q
zwx8Z4E;M}kuJq#cxx?q(HtFsDwr5ArxqGih-Y$D%(6YepR~OwHS7taS-aX>A{Js09
zK5@?mHYjXAdAwuu2hE@R2bRo;%8nG99{WeH^v(C28M7ze?5?V{tlb-%<MFWveT<Ie
z#@S>KYc*t@-QMKgh2<*i!6k?N`>n6Nb#^a*uM4+kS<cO}6}x|F(st+9P4*L0^ps(r
zuFCYSCic6Rb^OnOJx0L|H}6Z67ar|8*x4*y-8{Wk`qg;d;vZXlpA_|wbZG105-w>o
zY<BeFHjm_mD;6H~G#)+dRMF{`(<0V;2b(#4+7#C^vc`lK9|jiO`FZYJZrF{k$!|Ia
zwaV~6l6LxgdU{ctc@LWmf26mgT?gNW8^%n?OwHd_XtVK){@kat6D|~pN>YAwD%Ekm
zoWA!;i?%A)Q*XDl7}a5T-Hg-W5gX68*t9OauTPW3x^b7it!-R>*0qlo4>MMUTMe1l
z{?LKL2WRH4&o$gRG0^(gg3hC^y>_|Xdfu+K6(1isx9^f(SyIxeyj8)M*!IaiwqASh
z?Xv9V^0<0+9O~=r-yB%>dql5iLv9v*HQst8c=@ZwYn%&{Yy0c18l@V1VbOrECB>(c
z^sf{x6<Z$pno%&bthY_wl?o&GIbAy4yn5{Vx6|ecT?fqUyluqnlXEZpEOPC9<5eGr
z!}VXjNqK9qs#nmg7H<>Y`uNBCF6x%_-aEVa&#MHV@4bVX)q2qB*qv1IJ=f-!cPIz6
zZndxVwYv)ruU$TCn~lN2rXNb<etc{yyWhJ1H~U_x&Wb1ZTb*x}x&3V7H{W%yb)I*3
zx|Nym=Gpd36=Q!Mzvy*3zBskI<AH@i^Uf5dS=IP*-fhv%^>dvLx4wVg<LQj6mUUbm
z3YM795QPnm`Ppatz@<A3mc(t}Y#JZ!w%Vkt<-7Sl4!$dU@7y+UX|esmdcCr`bxB$6
z74E&btm5OvE=OgL7d_iOY(_w>z8)@9Ui#g4{u4Oh_s}?N+iJP{r^V+PBsF+xmp|3t
z{|-D+_Im8iWaBP9Jr>K1|CshoE!b>SIppxhZq{Xc<X2p_>3!}e$~?8dX33K>uighP
zZFQdWqV`$6<CaEoUw7DVeq!79!=+stQ<Hk<G-%p9rAFZ5xVQ6~7n-}CXcGCd+99JR
zU6wi)>C`N1kg0BWe@n-Ie6B9)pJ}MvvSNB_r?zXP#yP$Lj%OdsXWhKC?7;Gv+N)E-
zt)en|UG_fF$2n$Ou>YMyS34bg>^`9=G<(IRWzTz-Njj8>g8S-(U0zoINzstv3gxD`
z(y6gWryoppnsuypyLk;1y>lkOg&fBlJDjGK9Pj--q<Kb8_=sidKc=2Fujx(+>vHtr
z%)80I-|t+%#qZmVx{+qmspo?>hTXb8**i}K2E4AvpaDI7%KZ-y>s7D%s^lSA^=i)$
zwJy9}Y*1v{uY*zYius;Lu7CXTtlts+n)AK}1?EPKbobdlwL`0qTjvFe9pjBYcHOc}
zKe+L(VW}&|rngyMBP8(LJH1N}=FLfc9+=cgFKE1<QJ$p1-BWqlb*7G<Rp~xGDJQFm
z|G|5^z8}v2XtnX9Ud&6GQ@D=!ZHGLorNJ?$#S5ZqbvIg*dVG3Gc(>4Nx#3v{l+Q2L
z^}O=(iCawHi_ON@`qO0R$46gQZg{i${;8|gRz8{I==ih$qnoLJ?1D0`yMG%Q*yzij
zYI9#zEY@~!RL;8~_}}i04s`bPboRpcM%CKyjVjQ+Q7POTmB77G)&Kv!QQ3dFH)?P0
zXzs**M$*IF(|nM5fO(*K;9p-2jgAg54>pHOp`#N6qRd0#OO)Z};bUXN;T~oL{wU?B
z|I<Cs*r>#K?y@KEW@l7Dd^q}wrFr70;E=fZpqRK2^Qvo{A#pLnuUn!ko5D++3f>xV
zmpH{z3EbVS`nsi5R`sn*xCzYtAd-mTW10WgJDi?+DzBdgR-=2aN_(TWw69njw_^6#
z)?=TI?0qI*=GZ@E<((jfbo~6~mS;L$_&7bm)T6)uQE!h<t86xH=rrBv#A`?6ip8;}
zZ_YlD8d+YcEEs?6dx7PjKSPe~FaC3|IHr~PyHApkk9+!NPFsJ-WVOw{8b-sCj1pR}
zyO!Ck^*n2d;!D5S#=S>%ZMAF0CP^*ZoPxf$2L$MJjcV9vXN^GP*d=qSt*-aHY|y7?
zgJi8oTn%6Pr0>AU+J@^art9}ME^KZa(CYkcqeaG>92Yb?Sfi;!-?ARn69!y-*5yTP
zp}|Ry>2=o^);uv|cjne})9ecMp0u~v`OCESoqn^PH0fsQz241Dv}f*Ar)g2kGON{|
zs#m?m(&N`HGWER{A2)TZKi@rT!Q3ea8!epI^N2<Dos&0Oi0XHkUN=k9!7lwnfQe{(
zP4gCGb`PnMl{Mn4`{a5pM>bsN`MhbxnGN1)jl9~{f1jM`Q)7MqS`BK{u&Xugmf;v(
zqx6;4be`>)yufj0<M(bYr_Wl~x5m1Zp0!l3<kdQ~On<-V{l;0fmbnMday-3odw<*9
zJB^>+3knp^eJsg%F|pySW@U{h4)!=Zv(3~%U0L9-E`8k!`r1!3U*<N^t&>;7eAV0l
zou(E0dgz>~KIhCvzabXY40RvHu350%^ij^@!pN@vAzf5<{TwaoHEJY#<Y-r|>7<Q1
zt(w)TIn*?@W5<Go2@^YBtC&3hU2G$ry6ufC;*(C)*X#H8)UQpBs@r`#K6R)oyA|^|
zccEL$cDD8xief*{+BVYg$i&yHm(Fuv6MOegK&j}&;?p%Yht{{*GEmxOcc+F6hnFvZ
zUZ@(s|5j6*GsC{x_iN!bdCIe_PP5+3Yg(_3XPvQ8(Mu=q>Xv;%$MWRlU%tP8%M9i@
zOnCn1Ym=?EzgK?o_Wix(*RhF>9n6|+t9<HS_|LBeEgPFSG)|9e+`ZD^k>32Bd+J_t
zc=|ZzsF}(xs!p2*O`LP}7mi*KoB1~FQSZ{N$v2ZSZ>Q}FYU^%vW^fPvX(0_o+nRY~
zgip6=d8%c{=ckswFOA)=s=j1upsI0=PI0}P=vbII$(sbGc<sDk_rtV@tJ#t!y|UIA
zy2t<8In1qP!_=`GH#@W}>S?C3+WGW}{edYba<gUzZ1#KKXWQ_5UuQg7wR@*r{6(?$
zN9Nf@Yj^KbCUu$gBk1nPeqS8j&mH#7UEo}FAheU;<w#TG<=u6nly`<Gqfb8?xjcW>
zwTh(IcYjptazTH^=L4DJ9j+bQ8*n~q(bb3Psmph*ib^rLQW-n!+n|x71N7}3vbuEk
z9Z+;)>b9)LgD+g^etG}JAf;?+UiXZn!+k$?Ni-_IwPo(TuPw97o4XdR7-(}>QvYy%
zV&_qn%O4!5J8bF7m-)4CPO0;x#k2Hrw)yA9m7Pald)n7`e~IoED_e8_@niQn{f^z;
zzGtrG?ut{phg~{f-D>cIk2~rQ&dCh9SU+0FH|mSsj<F98Ji57W{>w107B|1GtXLH>
zTGvs3{E?CO62^B=Z}Q>xnqkc{M;n(fuoz#|`N}!{oG!KpmqmoRW%%A*Gqzd!yrBJk
zZtOQvZOeJ{#^Lyp+=)xyeu*7+;J~?BMKiNL%P%kNQSVKR@8vry!5BVkf5rKp-r}@p
z6E}t3-RY%xaCl{8=D|$QiEr%;3xZ?HUl^BfDf<1P!PQfH)FoCw!rC>Kk2-as=EMz_
z-G92SD1lF|AGMv|^Gk52nOSF@txE3dKYRVFGCgHbzX0=*#^v67zJ&C@`Qv-ddTnd^
zxsD%vCuUc;<@+lOZVd}=F(pagG~3wrWww57W#s1jE${xGY#C#jv_0WPhe7Z{w?a|z
zz1`|Dp)*5fyghb!PAk(>E_?gWs5sfi@8kTTOV4%>_%iVGrJK1%zaD(-+HslZ;F#KV
zy}~<LhBvr6Z$gp(1*f4;_I7UbR;uH;t7r0*l1;YGsuJ~w(XAK6#tbx{ziaQd`I9`0
zQ%2{E8Zf)dXS<9=!!AsA-Z%WKRl@iD)n7k9>+^Qi_)T;5TFyBV-{OxVC{NzmTlz+R
z@WaiE2Moo|Plo#KxB32VtpA(EFZJ}h^mKiZb1+2ZqWDp`y3e!WXD^H^&oHgM-)h|E
zo#mZh6_@uoDl^}+aj^LOee*H<wnW@~k`uGIOOkSkt;g_YF;^e0S+yqA;6tZbq4DKE
z2XA^6z9}h7QnA-}<3`(VIt6=j{YN~1t1K^6mz`OXc<b@w#cNs=f8RT6>y^y5C9@q?
zJZ!tlrPbY_gm(wVB<jdN3_W^s#JzzfrIW12z3@E#&hq7{OaJ(1N>5%cPkdi3H|yab
zmu|-nmrjifoi(P1NlD>jpQxL^7GDiNxU;lHvpoTg|Eb(^<8Iw{2k!pb^XbjepQ-h)
z94cDT?w__1`6HiAt^8T{MC9STwu?=##XenL`CiXfI)BYU#}TJ~CO8jxnO`tEVqe&`
ze4TSU7EU<-GsE*<*P~gRG76>~bUhfVj=r$tSNX?g4gqNkk8Li=KY75n@x4i}S6G@G
zmS*hg<acQ7PqXl`y}!TPvF6M^|83=cA}r^neG1R(Z96qyzeV?Zxpy1quT5WYpoO#U
zmp12by`H`EL6_(|BfO58I%Ot)a(*4%R5`xm=)x^i9S;A#H1+zb<5}^`BMiPp7c1hT
z0=FeTF7Oz$ZE$nr;;&P3b>BM3>KlwJJ9DP2*0K6A!@QO!O<84JR$!H~=jG?M@<;CD
zPJcbN#!2N-Gp@d9VP)tI|C6@8Tg)riJHz_2a{Q&dxqkYy)5DsVFMFoG9@(R0_T`>U
zo7G7Tnp?L+hO<?th+S_gU&p3?$(p`If3`*bz`<J@4?2=A3%_1B>}0#`=ad5?Q)5qg
z|Ezp^_hn_T{FKI{b0(<51|}JJ93A!gs@1}G9~)c$Ir;qdqCx7INuP4V+dS{(c6rU8
z-^tm}R?d9&V*mQ={v#`oUz&MohkX3kQR+VX4o&N^>>u~;E8eeepPv$Wy^EFG(gX4D
zavj{SFS+w`z@$EXqe~*bkMzuRk12kT?)Rtqi0>oHw0WvZo)`4rJXLo)ufets-JCsb
zoW<Y`Y5UT-9NsvQNi0P0xpS#VEwRuzt1<<+owfn7jv*0Y;Rzyz3Y=ZLkocgGQ3=Sk
za!Lq@iU_hE6&4kO{^^+z5<N&HgTH*(4l;!l`iBR^d4?p2jLkiS?j3UG`0kxVrPewz
zz?>MEFg7+sWR7Ml)-|^Yh!5!(14Nknc-q_fxPl{Y>uHPU2T(*|TtsX_Oq@t1Lnu99
z-Vt%}35XIAb`JyCe~2Z3ZE!?zLU=s1MPHET6Qqz?{53&?UF_}b`vXBvUS8S=k~4$w
z2vQ(_wTd7yaEv2Jf}XPB6C}0xdk(n_vU70k2Lw4gX{Co5gGY}XdDB(&NMJcRdZgrv
z$^Hj=tnD3Lot-f~@D>hQMgDq$EE4Ob0uiht2mOPiM#41h(?=9CDmW%EQderBePjG5
z`LXr3ak8<&)Sw4|NNNy5f%FKd5et~3A{I+#p*^DfPvm%5+uFLI_lTUGP+UopLkY7`
zcKE_9VcPP!!w;S`YU4kV<L2UNYhw%KcxV$H1trR8?^msrSPXCLaEm1+$&p$7Jvk1p
z)`Of7cRaL6j)D?onLv=m(1)W&Lavq60zp8CM7?|KVl5-W-2YF^8RY8Z>oEjTqeXHQ
zlpGV(@V6BsWtydu{SSgXaDb1iwF9C?i{L0IIToyxf~N!}Bry^RTIRoFO}~D2_O>WJ
z;_0bXa40zz0|gqvk?;gZMkWCyV5$C=9$QZ*>%n#mJ@CR4xv!M8AoG=uSOR@G(Sdis
zggRLwL2pg{9e=#+eFplV#q#vjB0EZ2l3`7&*rVc8Ls<yf-*2md4$gy|kcIH{)FL}d
zN|MDuKovCt=D<T~1arh_ssD~S_V%uW+}$uaHa^<KM@dODAwGNtktR#}KWSw*8xMv-
zT5YJ3mS-W0c>4>x6nvX5xA=Q`_V98W;LZ}HWyw^OJc|(@HESl&h2#d4ivOOVA)Yn^
z`wc>C=BsVRRFp)6m!l!ag6H!{$^9i&{Jm=K?=-;1XCNj=%l=YP5-nIQJ~0d<`I`Cf
ziSZd^KggXSM%!9Z!pvVO8BcN~q$o?UfB1Ks%g)Ptpsz1*M!R>4J#Y`ukX7!oRIpMK
z@KvicA-&oEo;mKWgWd3E2YO%?NX@LKBv}S#Tq8Mr7ehu`E~x~G_FwWx_#h<VfT?LY
z{#W)$seOe_$KR1d)EP)}C_$F;4TYG$T2PA+s%EJg@$2u%A%jVh9!i#xt*TlrzTuK0
z9m7|H5-p4677!K^4*_IMB7}Od?rs5bDDqUPk(_~)4u}Yj7x_uBzVZ-Bm<=M5C@|+m
z5*6MsBB=!yRFM=rG9LcAn(ZvbWC(fie;ZWF4kl$xo=D2*b&*t#`7DyML`apG2*IE#
zdYk+o2W1qyNXp0|gxYv}h-6}BhB7fTV;K`T@&}bk>DB(f4Ju;<StMgzc!;0b!59};
zBvZ2gR^n9=466E{2URg^D8oYPA(FFeD92F){F|Mz9Qzl7HI-uv^1rO9oLy5n<`oPA
zwL6eA_%D`gkzCFYC0AfLf2MNGg#Y89thc~OuUvuuDUvgOmPoF`J3}OAo-h?DuzTPE
z|D^;;p~dt3*EJOks$kitU}vmgB72bn5<qCi3Ra30Oo+#ysp5a0sRA2vK(1uis9<Lb
z<xxDSQjFD^NJ;NgrSvbhj9{ipR(h4}S}NfoS2RN<E5%B7pDG!m_%l`F-S)q%sgl_W
zA|>PZz<(J5QZnXDq*SvrRbyKDgQ~QI#ey{j16-vB1v6DyFl(p6p{$2U#jdf6RSGH`
zHwm^1qe%b545b>St7hb|O6g*C?IB_`DEt>IH4o4d%v5FUl$qT~jEd;~Porw)NQ}yC
zGY}yx`AC$Q9m>cDtSYM@87>M&Wib7RQI+6oF<X+gFJNb=<xyjeE5zcOm;kSpAVY<{
zCJ|$3vDp*B`@%@vI2#7OvBvl?zg6LiNPsM=gjqaYb8GmMweIY4GeZL^x44ib_>~!k
zj}Addba<;V=vY@uD>^Y3<++3;Csa|bK`zFuAdxdR9B36YYPw1^#hl-UX%#c#&YL;|
z5uy{Tv4TUiia9@vrL_tjrjDV7?>jKRljyK$V04^U%c0{Y%TUKiDz|zZvCR5$W3fI+
z4+(z~LhFXtPt0rsW>n$&Rk4^Il@$|qN@SoR#;Svu5@Qq@<igb?kaGbkgPffpZ#D7n
zBy!Daasf8UFBTmsdCcGhI>zt8v~u|j?UE;DXM&M)V$E(PF*gz9LxNbaO&N!<hm5}%
zIEO>hpb-Ja%4!6Z8<klMHb2JW5QY%w82g2%#HIH*4iV@CYs+Sr_{iy<DrW2ugPd4f
zA#ypZ^_j(>DaXH)lxx;j&MGABlym8K7CE)j_;&)iT(fT(x8dKo%#}VE<m^0nY{9>i
z$TdsIm2$LG&PIRcEOKV=@sQ)+3FM3~fO*cQ^*Gxq7V?~Ni*z~U%=++=Ygte>af#F!
zTUh!R<ZNo3MNUPrV*EQvxn@BbgZpnhXVWPha%w?^&N8FJ*fI~IoQ)bO${8+GNqvTG
z1g&gR5TRp>5*#|AN#pb|gPYUQj7=4D^I#MKm#}3OfxxVaaR!^9flc^h8VI$_L=eC~
zWLCvg$&G=K=&H;r6c8B$&q*bRj+q6OO&1Fjq71_s&kW0<Shzhoo-o8Q4sw;HVrgY9
z6+4txONBcUb70&;W>7{fxMd**Whcl?h@Bo!_QmLT61Qg8;tC=1TG-f~n;+&j=N+-Q
zsZA@agD}j}ct~QxLlV->I!LVKV=4tc1OA<$+d{LhOlYm0ZpJU<)B!sQ4NJ86N@C7?
zwiXetl7MK|QDSMPTs-`<kYtV1MEFcXy!<s}sprv4uOR#V4A?J(@5G_SD@OTs_*Wr;
z8i$BWyRwoX-26hT#%W(>^K)jO+56lEWB0y_=LBf+nKUjFmlS7*=LW<(i*++JEYad1
zG0`g8FNAv}#KdYB%pW%G%|nZR74k>pClPLv5G{WJX=>>OWC9~ZE#dhH&~kM_Of9Q%
zHPmYHfix}<;Q<NJvigKU%aG1POB+j#^TXAYiB-awC|?H;DrBdk$}kd+j!@hLHkXrW
zM%uVKGL|-W1v#av;0c=+S4iUt5snZy92Pd#4bm_~i{Ha!C(!Z{UXK8iz~^CIHw{`X
z9*@T1A^aU7TK@7;)M8&q$aIaXLwGtITD*LeXM=wwFjXxd;noP3kF#|cFDH7;@^P*j
zUOvv9)e<mhaeOp>58?K3!{Oy)b(4nA7$BU6#^oVA9u5;Gob_omnDDPcOd7|9J?_F8
zQXxZFtID8d0|bU2oGQiq;Jhf7AN*)mSWCjDmPzp-Atam}femLy(@a8(gTwQ0xUP&h
z!2Jt51ANU%yJ58E<T)@x|3!e6uQ;_hFO)uJvSw(~EE`!(#3IEUX8kbDoY9ZMb;DUk
zgoT}zR-?z?XPjg(tH!!3Y!ZoWr%j5ffi)fs{G{=_s>B6PfSHVjel<?7b2D%Whz^0e
zim_cxP!G?9fVcz`5CEpKVBF8c)y2!r+QA;dFdC5tW@Z6UG_1(jF*GJ7VN^^)h?&>~
ziX&`G9u%6&*$M)`F|?L5J`J2g92gNDkw8!)Y|Kxcss$b$HwoU55)+Y<trhXHNCC`b
z^!a2%V<}^d_)usoWz*F>6fzSi!C=kBZz~5VV}$rq5z@m$A~z9E1}!AHBAU($gq%rb
z^Kd8;6w5825EVe#Dg&>r8aT}+agCke<R-IFSky!;0M<y#cqQ6r07O)^6cW$ht=vj5
zgCKEWi#u#z#A(Ji<gg(wv&B2M4dyUAF$NFfE~8htUWg@J=|CeoRim<)6a<@*T$5~O
z`XFYqRRgv!VkBE3VIjao8DsreD$rzv0AQkwv9??jV1bOWOnkOh4a%UFhzUaZI!3@!
z35+Rc!YpnCV4$26NQ_qk43x9=6&61*P|odYrVTJq&L-B`HoyWoqlzg?(Wq6egp1Fw
z)d(pC2Fe+W$n`~1$kc*a2wHo_kKzDGQ!rMHgCHfr)?FDAq=*HK(WVK7L91E`2}J^>
zb?v2yfdVO&A_fYiR0<4K2v{lw1}X$Bl>!45oSMW;mI5^jG2XZorD#-^Qk8J=@mgx9
z6d0%w>;oWF%BUR{nG6`HU^WH^0R}1rT$cd@6#^#9fEWeCWSUSIlpz#a30Nvf1_;((
z26QPnJAhdsLkwigaV$2(Kq2jjft;RWdLagKI*x%L7BCS6MJXDU#jPeNMTSZ%OMxt@
zU`RP)pg<brh=BrWkRt{Pbe0@3kO?hV66A;lOgu~x3WG9qLL(tZkw(@|C*mIyo^yQ>
z|KOS<w!oM-a>PHj>d4|p{9|j&ECl$cWKR+>5I`ekSWMGdMJS<2#oFu@ra=MhQ?m7Y
z7C*30$z(TJ$`!yqC6j&OAizE)li=YXz&<5g|7Y<4F-j)=LJ^8at!gFY=)W2#1x#GY
zmQUHfz(1uxw<>^tN`Zn>ApQxY7CK3pV3j37iCDnM6ip}$$_xgrgoOU9ZBilzGNCTl
z7cr3YS1{X^h=FX$oyCtB$T<Q`8^k~+nabfoEMSrc6s2fX78AMn|7x6oTT;Q0z$__S
zwKWcijfJE85y@uO;5TwB8x$T8n-CIbiu@4?$Hgk33_&?zVxg!dc26maVRU0nBO!CO
zSt+h9Fq1tQQPmx|i4P%wZ_Bj;c5-@x0|7rdCBT7zp<+(gaxf4OF?ZlV<En7K!su0F
zC<&3!X0n1Jz|FP^iBEuPC5$E&vrZg_+eL(gM@cL!#DTD|jNt+7s7Jq$xVVsjMEFes
zze(BS0_1N>xYsMghl<h6h5gXtGLZ`TlNfj-WsD<#Q3y&2e9%d-MA)p6st==Zj7f|i
z832^4gVC&TjK`9|{UipVPzvTWFeWrGBrXhkp?$!_f@~+Zn1uMCaD-6|J5`Ex88_YF
znB=ixA)~@$qC?;(E>OT^#Ee3Pe|NVY;jthJw5Lqd%FLMGPdH<KB9Aklp%Gz;aUo_3
zEar@pBQj=zN(5B0XD&2AqzpKpGZqff;l0H9DhvS4lW{^t#tZ~7Nfp#S;*Va{P)M{k
zOUj@GI;8Ab7`89oMyz}?ZIGleiDZrt*hDhUIxuZOs%6}wFc8>5GTM!*8kHT5*gb+A
z2ghEf4~Vji$*^!#fTEJI$D$Z)NGUMs7`BN7cqn5sUK|8?C}U5Fv7MlojL~BJjZigc
zRVyJ&wHZ^~M*<9#u?MKwzQ8~kdz6ZWAO>=3i0OqG$mIPwN`ZlL&fqd_fCX~K_)(Oi
zQCZxCXNq}7?okFGFi<X7G(aL@1dK%{1qRAF&lGo(0t4j&CNr)*8~QPA0FRuBv1meJ
zP=-)&riCokW=t7;z(6^ZX65<<1Ld5JXW9S*<?K;07C&MjXF8cSh=ELPfWw1Wpu!mu
zic%C%u$TzXRFDM}NGWuYu;;9puE@2Qt8vPQMTYF7g0qj9EbQ8IP7MPA1}X$AgPn7R
z?G&L^bV4H`N3~f{W&q%yg0q$k1o)>AXe1f%Pa#l1GT@(r$-Z-w0sbkJtUrtK0AeX4
zwKSdZ8^aYMlY#?Its;sYd!_)xbw%uBR*ZunUCN%pWYGirlw459Pz5|vGI2833y4uN
z+Jhn#jmlyoC>5MW7OcD+_^0IZ3e1?mKc!s28{nT(pnT-OKc!&D0UD`*Q0$y@XOuOr
z2r~q<W(b8=f~6Ovyaj|RfPqSZHo~qw7b{`B3dBGzaA6>bf$X7Xc0|NL_V6zY0cuoC
zm_o4>jat=8Na?>CCj~H2#RV7)6~I6hmo{M_z(AE?r$8roZib1la1-R(t2n=kfdDZo
zCXYfB3jG;E!I>5ksx8jL_<$-Ydy19m3Pee{#8iApP(nn^DDakn4jHGjG*F;L#svWy
zC^AY+xld%Qfx@!L{zVjk20=q1>DsK71}&_uj6Ijh!vX6n=Vqm83u`RrgjxdyrpcK)
z6rT#9NzR2`nzq0kIUQ+fen;v|^Djd63W`FUTvxRQu1UG<l?I4ZFqe<eK!NXarg+F3
z4rl5(Uq{mxsbkLH(LjMv1rvGlrAYHTe;$zD6RwChW2fl~yjO7kuLcUdSJ2Y~-Yb|g
z9)BXhdj%I1XfOcp6`Uchg2L>iOsJ}T9zcv{K?tLb9a~a$YcK=r6_m6f7eYa81ms32
zxO#&I17ba=AT?0Ldd{?KponXXdDl7-<jn9ULaKIc#!`bD@tzVFls!~(CQ{QDc(0@c
zP71tNa?x^CTbPKHv!5Cu5UOOL+9v|UXeI(KhETM$MW9u<fk#q`bAV6ExvW(!VQx|`
zJ5~jRsY#jY503(vnT+#P*_LKv6wxrMTI(EmLWSE3rXYCLf!O8H8YG2N`I_NCP&g&8
z0Rnrud^rb{Ai}tCUjsy{gLTt5pcJ)a)VP)@Fm!OsV1^nC=b$Z8)S!m(i0ug|U=GA?
z0U*qQ^BK7AGDH~{=xTs?2QijU0|cD^DoTJI`c%z91_TJ_pv^XGPy=Oz)R@W4U}3p{
zQqvLyjPuzzpd8JCONDBHKo93GYk)8ZrlzND9b||@RdbM2bI@kCHK?JlnDfRpP?&_6
zOLu9YFblY*D`c?T4EBkb6i2xkY!opmk2pafX2eX}Oyq)@pw`0G_zzYbB_BX*hzVT+
zU|^`2^Ypo?D3GLy2@eI@B4&_MsX!|#CdE==hM2*~rM9Us<W#K+YA&3MwiK_1Jj7Ej
zbEknKo|00pFazx)CM<**&bo+6IaFe1kb<bh%wVE%?Nb42s%C;(3up2lEIH7DSXw3J
ziGkc?KtPcE!`*yVoP<B2MdR|D@tU6i9qv`NLxew}UjM#NFfGOeHVH8qewvuMYr~v8
z@PMt#B!$$hrdP9`xtiT4YC6r&JD$K~r6okV2iKN3?|~bR@!i#IPG8M#I5nF?RI?dF
zHIqZ-FBndkOF&4N!AObYn?-gkcH64i#G9He4XWALQO#~(HRFm=W8n%0P9auhYRL>K
z-Z#Vy*<zrY&8w(c8BnufpPH@ttC?&Ue^f2$N*sWYsT9F^HFmD7IH{RqEbxjH%V*Z1
zRI?^g&D0qAqcTGvC5!)p8jN5A;FEB~6xrIZnoWkPS<|X!jyH+aY}!uE<VC5$P~^w$
zI8!sI;|d4lYjk#YtcFvwj<A}&?5k#ze`;3OshPt8`~le=3B!O!;|9gKT;ZU6<;xDr
zhWlzZZK7sWrxxU_cK$D$4?B_sBiVYanytdBS?68N>SHw<bg7x+i2R}Xn@=-Gl9hbr
z#m<VoS*re5<<7B+R6as}Y0)ICr>tgt|9>U+KjY=Q>F_QUoIX-BF(&Wiry6_T94ncZ
zlxzdwEwq?0bY2|ptb;nkhFsU2yBr7*?j#yMU8QF157XW`COE|0J3fSK4BenGW{o3z
zsCMDxCx3h#PI2Q~Cnki)#2GsVjEWcaj|m70kBAnr3d6+Q3mxP$9v$my?P>2Rf+6C%
z#*3T-M#0;@Cg@eQNJt+To0!`|AqbvJGaeY=72+cr6Oj-uf;WFreJE5E8xS-S=41jF
zBw~USgF@nrV}nDXSr4(9%)$&blKFs;v14N5g5!;&VIEPU*tn432%tSFL=*%M1jNNe
z^#UYuFztl6h#(QXD;yjbkN|(h#>IpII67ifST7M%6A=Y49pmA>xcCHU77{kLmk6kf
z>X8r;9U_W}h5tmvL-&|b@R%H?8~(I|h9YCTo>0GmyYOPD2*Cw$Wj70PR||`-&<+^<
z-<o*Ep=x9>Jo^+76(0iY0&nz~^-GM33Je%EDkM&1>=hmnFM|I&M+`u-XP6+8n5oTV
z$cPzp>@eZ2R5CD7*FvO*2bL_V{)f_@ps7MdRWM{A(EmiESeQzJAEV>L;I_0Hc^rHg
z`WtyHBn){30+?Eb&$97hfKP=an-7D(@kJXFhT>)cObz6LRPtefPl>X8d>H&KK?aqC
zA$7usi7lj1loP@Lp8{1Z`0e0tXh_45TIRPy_)sF54@3BnQRl<pZybXXFno%P4+DJo
z8W0}__~gh8@L}*bK4?e4@Z$x1nAk!ErAuL7z=xl2;J1Un<w!M?FmwTc4@3A+l8+Aq
zd^kzShr!<pWS~hH${X-uVwf#50el$XlaOP;@A%R_za4^+as`*3@L4^6JHRJK=8_MC
zzws@05{B$JABOOeJOF$kFv4~SpB9<K&K`kJiQFVUOrcb(kuT4O!QWW-lQ2}1;ltpb
z8;~P}0X`K<{qftucC$df3JF7%WIhaDIF}%AfDc3Xkc-EM0X}>mjf7F_3uSENRq)#Z
zJ_Sm>@L_;YLhl*Sti;^JQaDXU(}!>A6ZoXa=_O&Let|2GQskcU+W~#}Edf3Z=#y!o
zPo@PQu66SJ0zMhNx4>=?yoWJxKAGPa1XoPzFu+IYSm=wN?cn!CFj96Q{>860@Y_Kn
z2}*zPVbE7e?_UT+30?tlQdD2z_eJ<<-3iys>G`V2aUl{{qOgL%XF<v-&<BGE+aVY_
zm&EUj@R2-FKt+hw_vna+urHVt3*_Mm@j<*sjEnHmwg=t{qIfF7NeKZyc=rUw7<?Gu
zQ=t%w4@2e+1<M2spIYL>#Bk~lMb3N};8T)g06r-S68Y^AjM6V~dWEz-fDb=cOZ3I>
zagZ>S#pc7rV0%b80DM|(j|9Jw!|x09$w^rgt0W3i*8x6Uo+A3<GBF85*%CesUR40?
zB!mGzDQWkC7A1-S`F#<Ll4li|1%d~FPlEhvqAz%2G>p<O5Ym%!1^C3M>P6tg!M+d%
zE)6j*yg?yHp(Vc^;IlyC86Sq+Ar!(9Fr0DZ!vLQGg&%wvlrq$$9tV7&0EO)k3>_fl
z_k~?#LGlIgsYtnkz7kSS5sb8}aAHkD>M+0u5+fWJ`btp(flmv9p^5_^2KeABAVL`6
z!v|sc?VvB^mmnA^2k>&3gqCM0!cejSZWFym_;3k-T&V;WlUO&PPmXe4{B}T#wlVN4
zP5gH72$=<ntNAd%ho1%G!vG&X`b)r+<Qf4!{PGjO9pD2&5RL)(a2=oD4qz%$CSf%w
zI}P~ogKcDAa-Rb}ocZ9lL-<JB1Ng+`xTr6U4~j_yuRvHNWM9AsmY<w2Ja|p)RdD&m
zB<}$qev6t<vjW7A_Vqyg2s;D#B&5F!H9XqChqJxpJWyX!cLEH&N8vo+QV&k$6ZGNZ
zBzzd^OYSXz;fJmH?Es&Iw!d(ehZq<2MR^~7Uj!p*hDY;Y7Yf?}{~&A@!l18~7!2-3
z5$!-qWu)H(`0zP8f))kIdl&<s^yjyef@&w%4e$Y&q7T16#_tRG@R1xo46Z+c2QBQ2
z@R4IcUwj&c-xtA9E=dT3SK|o&fdtX<mP&%(=@Rr+NpSK?00YZ|PLK&;U@l2LX`zx(
zej<X=u@Id2BzOf+c~ScuU~-f!=Z_08d?$^7Y0+0wEjku$bRj+q6+GQY?r9Zl8G=4&
zNBg_rf{?lsvMLgk^ybqCFd&A4;TuatJA8+PfT<`yM<rL2HrYZ2>r3u86&&awbr`sb
zRNSM28#NTo3j9W%kQTUILg9m38kB6n8-5i3;5-~fpAuh66OIAfgT{wnuOr&2NgDvs
zKCWvC+kpU5a|6{$tp{u)Vh;j7djEp&h|Vnt$AGdGg%4akiU&}tpvHi#J0+89d{v4V
z1HWn~fI(>q-K8L4_$VR)qkS{j<)qJLp#oh(%>!=ll4C%qM*6^D7$FiMX@U2=NFG38
zit_8AIE6|qLYgHqbWMqX;ZsKh3^G4dJ4hCgFnHIRTsKHFP%;TiMCAaWvP91pOd}~*
zYDgzi<H{g$M$!Uj*C}4XL#w15K#GFezmWQ(=OLC*wg(ChlstomrTrWWT(%PO6!s;>
zdpKT1@fm_9iU*LGqUHu>i{OhGQq81I0T{l%%%=rl3RI*eVED#10aK%bs}KgaiYVTL
zdrH#`i!SU7Tv5^M3r2uwr%=L2SqNUi;W2!&lurx5@ME<?7!-Gixj{Sh-AKVaU<@f0
zGl51V<pmnycjWkEfK!4?ze1P<9||R45_11SAPJjQNDIKI+!Da#RE!OAH)&^39-Wj)
zSYIe?2*<EcgFzzJ4aR`oAZ!OO$wER_2!jL<R68h`l9oS+a)`BrzF;w_zPN<W=M{{r
zAms%fqmq%i2Y^ZFdBDwVil^}8Ff|Xj&PT`wC@}n9EuR)K{Ev*q5r~v=7*s;)cEAT;
zXd>qe2XKk?fOfbvEEv}UY%MhpaEyt)1-t@*5sm>q9F^yXs0?2R5R3sQpU4;j=7w)(
z6YZ$H543}%n{ZrUi;RqC;K-G>x#4^M!ZBdKkunMCadbX~Xa@;i3WjeE5$*8zt_T=@
zM}iMSd?E9WQWb>aq`W|?nap_rs~}jTXu)S@`C~vke1w^R;WNT~80aAoXcC{C&S!&L
zLXQhCTa)_@mXg|UVtDzQlu3Y*J_CGR2tUrsrw?F|9VY36G6aQBiC<L|_J!0nA*WDh
zqGD`-QFSI57k@N^z=w}L6EHdH%L6|A#5d87if5o57+c}oU~l1D#e%+IJxHGmEDAou
zO|%1NPlylNQ8{yfiP5D1L0@#yl;jnJWu(6geN}XvC!^wIka0QP7eYxw--A|z!T~`G
zRHI1W47NFbYEnQmN)40q0DNS=9qg}ylu3Z$BQkt^pu;4jZi4-RPm>GViKQgXXujwS
z3BMf>Oz#<p-l#nT>1;BFMfFBX?jiL`#wn0y)xs;aiab{U?I<4|U{s9{!6@FtxTL=e
z?I?d7V02y{x#OfvLTVV@3=;4P)DXIiN5H7Hgu@JS(q{!2%|CeWf#w0^poskjN&~++
zDI6Cb&!=gjVk<~VQ}GehO{q8l_Oy!f1>jITnInT!w`$V21$<Q90BDxbYXpG`MKicp
z6rZ7RK)0jT2>G34UI3C2GBS4qFnT>8Ge^lYq%o=W08fC7Ljj+J%qPNpRg_Ny(LS}7
z03-c&<ZX~~58A6_?hs&9T!vuuxS$S5eFdI3nU946q?%fD@KnegC?vS?7Z(Nc2W}Ik
zCn1z1eJ<#W?~V%lLVAPH<7gg~{ea2@8IwTGT201&0E1*bu`j_y(e+<QXOnyZuak^T
zQC*y}Gmya}>tImt7Sk}~krT8)YEBD2e4$Uk7r6|7XoP@Kc{p&d@H-#Eb|B;AK9b9*
zTq*2xH6?#?744V61!BrJf(K0b^59C5wI^_|Ad5`$0P5nT9fVXfwO8ReAj<B8l_uj=
zaP`1}A;*wG0*i#f5pJR#*h{L`Ayz<Hm!c2!0nry+LVUGJz<cOR)>y!rLjX+B0#6E2
zGOhq4K<ouEq(vz{!%9+d8NjHx0#c=9P6o~aP&p%@50n#0pGrmTUwG<=@)=+Z%I?F}
zWXk?RI+pT*As<2QL8uN=@eJfL$lNuI0LMBAngK@oO`uDl07te1g+=v+u!rP5_*axp
zgjkZ)#qf>_2nRI=esWMC8*qJ^ifaKz=>vE;gVJjNBjp~l;}AKL<3jvF`Y)(XN6Is3
zI!a$beUP%#P-UWQ1v&;m?k&iGQaKN(hf=Z&Q8%TVPz{J&4><Ke`mAuKgE~(LSuWUd
z1P`DtMCCys1&>Z=3)Tb1p!5*bPN+Sol2i3JaGGd+4{B0NU;Kifa9qf5korIcuRT!n
zfX_Tpe1;kU#VgRKr0l9x+Qx+wyd)1GT};RZ+B1~T0GUn7|AXUU<bH#K15`<gG2mzv
zWmi>j|AiPA`a)qy*cVlRNL_+zpY*uEdy+5Uc2YVAI7Q`Tz>A>ra1bp}dk{{{lk`Cn
zgtkX;)r8XbaNB^UPfo>ofRC!3AQ-i`z;aVORm1HNa(&fskcX5jH5|Jk^%Wd7C4D5M
zVJZJl4K|coH+b%YkZ0%%orP<OVC3FMvPSMzsMt{TP=LwF^Ns+6gd#Zx9NVPF#Sc>P
z_aOA8&PyQ}=`(;uf$Tat1{{W<#sEW2$R8vRs2VungF}2IJ_WwM!>11<2$E?;J4is2
z@dsq@!SEC9z!M;47ur!a8NtXs4KT`901V%K<I{&=q}@VivdFy!Fv<o1jH=DTKBwxE
zzytiuonTz}q9jQ(B)+I~s^D$luLTPGLLfz~C5(YD;_=%d7<rBx#(<M<LOvrHx#j?)
z&gCNd$h`mpN%?R9!=EG<&;l85(ys%2kdP&5fn%G5yZ}D@NlQLH$Rj~Xnd}Sh6|s*1
z8x_02$vi4{hB2u6FSNstLh{E280BvO48L_kw9_(QaI~rU!f^p&4?<-J-@FpygBx<h
zzJzx8K~G^jNM8|p4V^k9_ZEVYwh>@dZXLnMy#O%kP6I^yR6Y@4)cIoQi(e%X&<wkX
z^iNUtfYhe|qw<buT+;V}NC>iXgls^{jr6^tFLjm^`ck=YfJw-B0rt6;vyCWdC&z^n
zDFuU5ZG^6aR3GL0AQ+jufrK=bXMmua$`3(?2Z{+KEpSAH;tM#{q`aVXA`PSR2QUVe
zw?Ht`zX$p($T$UHP(mW<Q$hBhf`PM4!^oNq;G<;&HYRD`fPZitk-&#asifWj6-LI5
zAXU`e7Qjd49f1}o0#Reo`y2$2(x(6;>%TBAy%zvR@c;ys%*#VxS~dVi%N4jAq?|$y
znLH1I{2Yp>kcFdU2*o(048h3&GTuU`i>dX65<O*)z>6i%BtT#MjbMQ;ff_yOV}f@@
z#jDU4lJ(>o!Q+3VO@UdEdmr^BV`n&U0tW#Ie31Sl>+z6OQIqv#=!-vHE0_ly%Oz{V
z&<;*zkz;^}llueQFsdDd#FUJ~kx$aU0{W;kK;YO~P&|;y#T5TwDM?=n^dx@BPdGPV
zGdT}n3uVvY*f7ll&^Z*JwTuBb9?7@@o&BZkAXNRxvnl8dE~UfZOeH0g@bEHad!T$t
z=Cr^Yms9o#)H7v|AcahxlZL|#P|PCM9M1Go`v{IWkU3fyS3={1Yl|fRAhSuH)rG$J
z>kNW*gQ$X(1JDN&%CCc!Bx4*nHw?)UY97?RRA@)V?C8ufC2Lyl)Id8bZ;N1Lz7t@u
zN+d0CFNU;7a1V|0*Pt>>#$`~grQ#!iQ85mR@~Hg=nHw^;hrYm8f<7>f<e601Td>o~
zcHpa!wg(-xBG(*pOVr+iOfID(;OY`ZALNY4nirg-(4vds>^^DFk&901U8qn|_6X&X
z$n}6FqVy@8qNQveynsU4c+i|$+Cg%i;2-2XDE@&9Ozjysx<IZ6lm{V8LE?ja3Asj)
zlcC}ic+P{`zmRJo^JcK-;5d<E&}WLk@Zg8Z1p60UZ)z_fwM*E2i1w+x9>Bo6AjgFS
zDzT?grhw!Ncv94zB!E%pebBh14M2U#+z4zA%I5+Y9LpEd2Ysp9Eb6NTA5}wxcKAIV
zKK~Gm%q<}p>HEN?4(i?_!0;o>LVWNU48k9Wz!_2!<UBw{5HbYqz&oSbLH3J;L0XiM
zNpOa!JdF$;wx`B`<R-CKp=e6&X>@Xstak&9%40%hf;xu>K`j&}37X-o8o7U=9d)M~
zRuX?*Lcmk-7)UvVL&)I7kz>FC1ae#nTq`1E92_M4kgi}1IDbIdL9i60&wyM)YA=Af
zBz;UM!czJL5<FC18G=Tt#sV<vE*?}!soE#Ns2U#x0n~k1fGNnF5hTp%y$UA-$a)17
zQ>Z(N0K?Ct^7#kKg}M(0FshymRt65L3fB!_R9+8NI!K;^p{4Re;GIGOn!pDhD=7zX
z$&fm$3o!gW8o@l^6-=_$3z=~I%8;NP_y^?qFevp>_oyHtNW~fequyPBN-F;DgJ2BE
z7?E)gWV5N9Ae_XZasaTpR4x)=aMMP}E6_MpEfu7II->-)x2W^V;7w9_LIs#X(&qvg
z^{xgS{D4FzK?{@u$#nxqkID^$y`=8Z!+kI+wg(vH9{`N1*?_cw#wTcj<GW<+2kjt2
zqS{gS%Ap;VI|LXNBf?1+s!j(m$Xt+PfSXOj@TY+I@&bLKl1TNX{1*@^I7UFWgRM`)
zD4zlP!qGmeFMW<tg)BAM4k=0sM%|@A6=5=e3NY&25)?kEb4viD?u-JAx~B;+>MR(*
z@W*U~JcVNb6i=c2LBsGz0EK;F*N`#}>0Qzng=8Ha2dF^j5PA|FkI(|6?!`bmNMw?H
zhWsBzGwm~gX2K6v3g>|?@{#y}snl9R3`WTY98aWZfubuV&-C3dNYqpJVbL)UGKK&c
z6?X#+F4~YZ)3qWvB0`-J0T>)gCHjKrNX-Lc5=tMyRX6hP1K^`#L<A#qxd0Qq3jq&&
zMMQ<f>Gtj|GWU!a7lI$kGWUQlN5ZRXps&#PE1gD##sC~v0{_cS<kwp%wGrFP?Zvio
z8@0^F9ug3CNWj!~5<5FP(42il|GkO7s81i=c=T~g{6-hJBXE{qw{vHE*M7SH4>Uh=
A#{d8T

diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example1.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example1.cpp
deleted file mode 100644
index 1e4b7d0ec35..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/examples/example1.cpp
+++ /dev/null
@@ -1,97 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file examples/example1.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Very simple example for testing qpOASES using the QProblem class.
- */
-
-
-
-#include <qpOASES.hpp>
-
-
-/** Example for qpOASES main function using the QProblem class. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	/* Setup data of first QP. */
-	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
-	real_t A[1*2] = { 1.0, 1.0 };
-	real_t g[2] = { 1.5, 1.0 };
-	real_t lb[2] = { 0.5, -2.0 };
-	real_t ub[2] = { 5.0, 2.0 };
-	real_t lbA[1] = { -1.0 };
-	real_t ubA[1] = { 2.0 };
-
-	/* Setup data of second QP. */
-	real_t g_new[2] = { 1.0, 1.5 };
-	real_t lb_new[2] = { 0.0, -1.0 };
-	real_t ub_new[2] = { 5.0, -0.5 };
-	real_t lbA_new[1] = { -2.0 };
-	real_t ubA_new[1] = { 1.0 };
-
-
-	/* Setting up QProblem object. */
-	QProblem example( 2,1 );
-
-	Options options;
-	example.setOptions( options );
-
-	/* Solve first QP. */
-	int nWSR = 10;
-	example.init( H,g,A,lb,ub,lbA,ubA, nWSR );
-
-	/* Get and print solution of second QP. */
-	real_t xOpt[2];
-	real_t yOpt[2+1];
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-	printf( "\nxOpt = [ %e, %e ];  yOpt = [ %e, %e, %e ];  objVal = %e\n\n", 
-			xOpt[0],xOpt[1],yOpt[0],yOpt[1],yOpt[2],example.getObjVal() );
-	
-	/* Solve second QP. */
-	nWSR = 10;
-	example.hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR );
-
-	/* Get and print solution of second QP. */
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-	printf( "\nxOpt = [ %e, %e ];  yOpt = [ %e, %e, %e ];  objVal = %e\n\n", 
-			xOpt[0],xOpt[1],yOpt[0],yOpt[1],yOpt[2],example.getObjVal() );
-
-	example.printOptions();
-
-	return 0;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example1a.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example1a.cpp
deleted file mode 100644
index 57a28757cc2..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/examples/example1a.cpp
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file examples/example1a.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Very simple example for testing qpOASES using the SQProblem class.
- */
-
-
-
-#include <qpOASES.hpp>
-
-
-/** Example for qpOASES main function using the SQProblem class. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	/* Setup data of first QP. */
-	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
-	real_t A[1*2] = { 1.0, 1.0 };
-	real_t g[2] = { 1.5, 1.0 };
-	real_t lb[2] = { 0.5, -2.0 };
-	real_t ub[2] = { 5.0, 2.0 };
-	real_t lbA[1] = { -1.0 };
-	real_t ubA[1] = { 2.0 };
-
-	/* Setup data of second QP. */
-	real_t H_new[2*2] = { 1.0, 0.5, 0.5, 0.5 };
-	real_t A_new[1*2] = { 1.0, 5.0 };
-	real_t g_new[2] = { 1.0, 1.5 };
-	real_t lb_new[2] = { 0.0, -1.0 };
-	real_t ub_new[2] = { 5.0, -0.5 };
-	real_t lbA_new[1] = { -2.0 };
-	real_t ubA_new[1] = { 1.0 };
-
-
-	/* Setting up SQProblem object. */
-	SQProblem example( 2,1 );
-
-	/* Solve first QP. */
-	int nWSR = 10;
-	example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
-
-	/* Solve second QP. */
-	nWSR = 10;
-	example.hotstart( H_new,g_new,A_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 );
-
-	/* Get and print solution of second QP. */
-	real_t xOpt[2];
-	example.getPrimalSolution( xOpt );
-	printf( "\nxOpt = [ %e, %e ];  objVal = %e\n\n", xOpt[0],xOpt[1],example.getObjVal() );
-
-	return 0;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example1b.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example1b.cpp
deleted file mode 100644
index 102155a666c..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/examples/example1b.cpp
+++ /dev/null
@@ -1,86 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file examples/example1b.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Very simple example for testing qpOASES using the QProblemB class.
- */
-
-
-#include <qpOASES.hpp>
-
-
-/** Example for qpOASES main function using the QProblemB class. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	/* Setup data of first QP. */
-	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
-	real_t g[2] = { 1.5, 1.0 };
-	real_t lb[2] = { 0.5, -2.0 };
-	real_t ub[2] = { 5.0, 2.0 };
-
-	/* Setup data of second QP. */
-	real_t g_new[2] = { 1.0, 1.5 };
-	real_t lb_new[2] = { 0.0, -1.0 };
-	real_t ub_new[2] = { 5.0, -0.5 };
-
-
-	/* Setting up QProblemB object. */
-	QProblemB example( 2 );
-
-	Options options;
-	//options.enableFlippingBounds = BT_FALSE;
-	options.initialStatusBounds = ST_INACTIVE;
-	options.numRefinementSteps = 1;
-	options.enableCholeskyRefactorisation = 1;
-	example.setOptions( options );
-
-	/* Solve first QP. */
-	int nWSR = 10;
-	example.init( H,g,lb,ub, nWSR,0 );
-// 	printf( "\nnWSR = %d\n\n", nWSR );
-	
-	/* Solve second QP. */
-	nWSR = 10;
-	example.hotstart( g_new,lb_new,ub_new, nWSR,0 );
-// 	printf( "\nnWSR = %d\n\n", nWSR );
-
-	/* Get and print solution of second QP. */
-	real_t xOpt[2];
-	example.getPrimalSolution( xOpt );
-	printf( "\nxOpt = [ %e, %e ];  objVal = %e\n\n", xOpt[0],xOpt[1],example.getObjVal() );
-
-	return 0;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example2.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example2.cpp
deleted file mode 100644
index a770dc70efe..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/examples/example2.cpp
+++ /dev/null
@@ -1,124 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file examples/example2.cpp
- *	\author Hans Joachim Ferreau (thanks to Boris Houska)
- *	\version 3.0
- *	\date 2008-2014
- *
- *	Very simple example for testing qpOASES in combination
- *  with the SolutionAnalysis class.
- */
-
-
-
-#include <qpOASES.hpp>
-
-
-/** Example for qpOASES main function using the SolutionAnalysis class. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	/* Setup data of first QP. */
-	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
-	real_t A[1*2] = { 1.0, 1.0 };
-	real_t g[2] = { 1.5, 1.0 };
-	real_t lb[2] = { 0.5, -2.0 };
-	real_t ub[2] = { 5.0, 2.0 };
-	real_t lbA[1] = { -1.0 };
-	real_t ubA[1] = { 2.0 };
-
-	/* Setup data of second QP. */
-	real_t H_new[2*2] = { 1.0, 0.5, 0.5, 0.5 };
-	real_t A_new[1*2] = { 1.0, 5.0 };
-	real_t g_new[2] = { 1.0, 1.5 };
-	real_t lb_new[2] = { 0.0, -1.0 };
-	real_t ub_new[2] = { 5.0, -0.5 };
-	real_t lbA_new[1] = { -2.0 };
-	real_t ubA_new[1] = { 1.0 };
-
-
-	/* Setting up SQProblem object and solution analyser. */
-	SQProblem example( 2,1 );
-	SolutionAnalysis analyser;
-
-	/* Solve first QP ... */
-	int nWSR = 10;
-	example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
-
-	/* ... and analyse it. */
-	real_t maxKKTviolation;
-    analyser.getMaxKKTviolation( &example, maxKKTviolation );
-    printf( "maxKKTviolation: %e\n", maxKKTviolation );
-
-	/* Solve second QP ... */
-	nWSR = 10;
-	example.hotstart( H_new,g_new,A_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 );
-
-	/* ... and analyse it. */
-	analyser.getMaxKKTviolation( &example, maxKKTviolation );
-    printf( "maxKKTviolation: %e\n", maxKKTviolation );
-
-
-//  ------------ VARIANCE-COVARIANCE EVALUATION --------------------
-
-        real_t *Var              = new real_t[5*5];
-        real_t *Primal_Dual_Var  = new real_t[5*5];
-
-        int run1, run2;
-        for( run1 = 0; run1 < 5*5; run1++ )
-            Var[run1] = 0.0;
-
-        Var[0] = 1.0;
-        Var[6] = 1.0;
-
-//                  (  1   0   0   0   0   )
-//                  (  0   1   0   0   0   )
-//     Var     =    (  0   0   0   0   0   )
-//                  (  0   0   0   0   0   )
-//                  (  0   0   0   0   0   )
-
-
-        analyser.getVarianceCovariance( &example, Var,Primal_Dual_Var );
-
-        printf("\nPrimal_Dual_VAR = \n");
-        for( run1 = 0; run1 < 5; run1++ ){
-          for( run2 = 0; run2 < 5; run2++ ){
-            printf(" %10f", Primal_Dual_Var[run1*5+run2]);
-          }
-          printf("\n");
-        }
-
-        delete[] Primal_Dual_Var;
-        delete[] Var;
-
-	return 0;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example3.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example3.cpp
deleted file mode 100644
index 8fb1cea8336..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/examples/example3.cpp
+++ /dev/null
@@ -1,88 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file examples/example3.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2008-2014
- *
- *	Example demonstrating usage of qpOASES for solving a QP sequence of the
- *	Online QP Benchmark Collection. In order to run it, you have to download
- *	"Example 02" from from http://www.qpOASES.org/onlineQP/ and store it into 
- *	the directory bin/chain80w/.
- */
-
-
-
-#include <qpOASES.hpp>
-
-
-/** Example for qpOASES main function using the OQP interface. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	/* 1) Define benchmark arguments. */
-	BooleanType isSparse = BT_FALSE;
-	Options options;
-	options.setToMPC();
-	options.printLevel = PL_NONE;
-
-	int nWSR = 600;
-	real_t maxCPUtime = 10.0; /* seconds */
-	real_t maxStationarity, maxFeasibility, maxComplementarity;
-
-	/* 2) Run benchmark. */
-	if ( runOQPbenchmark(	"./chain80w/",
-							isSparse,
-							options,
-							nWSR,
-							maxCPUtime,
-							maxStationarity,
-							maxFeasibility,
-							maxComplementarity
-							) != SUCCESSFUL_RETURN )
-	{
-		myPrintf( "In order to run this example, you need to download example no. 02\nfrom the Online QP Benchmark Collection website first!\n" );
-		return -1;
-	}
-
-	/* 3) Print results. */
-	printf( "\n\n" );
-	printf( "OQP Benchmark Results:\n" );
-	printf( "======================\n\n" );
-	printf( "maximum violation stationarity:     %.3e\n",maxStationarity );
-	printf( "maximum violation feasibility:      %.3e\n",maxFeasibility );
-	printf( "maximum violation complementarity:  %.3e\n",maxComplementarity );
-	printf( "\n" );
-	printf( "maximum CPU time:             %.3f milliseconds\n\n",1000.0*maxCPUtime );
-
-	return 0;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example3b.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example3b.cpp
deleted file mode 100644
index 58d102250dd..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/examples/example3b.cpp
+++ /dev/null
@@ -1,88 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file examples/example3b.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2008-2014
- *
- *	Example demonstrating usage of qpOASES for solving a QP sequence of the
- *	Online QP Benchmark Collection. In order to run it, you have to download
- *	"Example 02" from http://www.qpOASES.org/onlineQP/ and store it into 
- *	the directory bin/chain80/.
- */
-
-
-
-#include <qpOASES.hpp>
-
-
-/** Example for qpOASES main function using the OQP interface. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	/* 1) Define benchmark arguments. */
-	BooleanType isSparse = BT_FALSE;
-	Options options;
-	options.setToMPC();
-	options.printLevel = PL_NONE;
-	
-	int nWSR = 300;
-	real_t maxCPUtime = 10.0; /* seconds */
-	real_t maxStationarity, maxFeasibility, maxComplementarity;
-
-	/* 2) Run benchmark. */
-	if ( runOQPbenchmark(	"./chain80/",
-							isSparse,
-							options,
-							nWSR,
-							maxCPUtime,
-							maxStationarity,
-							maxFeasibility,
-							maxComplementarity
-							) != SUCCESSFUL_RETURN )
-	{
-		myPrintf( "In order to run this example, you need to download example no. 02\nfrom the Online QP Benchmark Collection website first!\n" );
-		return -1;
-	}
-
-	/* 3) Print results. */
-	printf( "\n\n" );
-	printf( "OQP Benchmark Results:\n" );
-	printf( "======================\n\n" );
-	printf( "maximum violation stationarity:     %.3e\n",maxStationarity );
-	printf( "maximum violation feasibility:      %.3e\n",maxFeasibility );
-	printf( "maximum violation complementarity:  %.3e\n",maxComplementarity );
-	printf( "\n" );
-	printf( "maximum CPU time:             %.3f milliseconds\n\n",1000.0*maxCPUtime );
-
-	return 0;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example4.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example4.cpp
deleted file mode 100644
index ac716ad48dd..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/examples/example4.cpp
+++ /dev/null
@@ -1,172 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file examples/example4.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2009-2014
- *
- *	Very simple example for testing qpOASES (using the possibility to specify 
- *	user-defined constraint product function).
- */
-
-
-
-#include <stdlib.h>
-
-#include <qpOASES.hpp>
-#include "example4CP.cpp"
-
-
-/**	Example for qpOASES main function using the possibility to specify 
- *	user-defined constraint product function. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	int i,j;
-
-	/* Setup data of first QP... */
-	real_t H[7*7];
-	real_t A[50*7];
-	real_t g[7];
-	real_t lbA[50];
-
-	/*	    ( 1.0 0.5 |                    )
-	 *	    ( 0.5 2.0 |                    )
-	 *	    ( --------+------------------- )
-	 *	H = (         | 1e-6               )
-	 *	    (         |      1e-6          )
-	 *	    (         |           ...      )
-	 *	    (         |               1e-6 ) */
-	for( i=0; i<7*7; ++i )
-		H[i] = 0.0;
-	for( i=2; i<7; ++i )
-		H[i*7+i] = 1.0e-6;
-	H[0] = 1.0;
-	H[1] = 0.5;
-	H[7] = 0.5;
-	H[8] = 2.0;
-
-	/*	    ( x.x x.x | 1.0             )
-	 *	    ( x.x x.x | ...             )
-	 *	    ( x.x x.x | 1.0             )
-	 *	    ( x.x x.x |     1.0         )
-	 *	A = ( x.x x.x |     ...         )
-	 *	    ( x.x x.x |     1.0         )
-	 *	    ( x.x x.x |         ...     )
-	 *	    ( x.x x.x |             1.0 )
-	 *	    ( x.x x.x |             ... )
-	 *	    ( x.x x.x |             1.0 ) */
-	for( i=0; i<50*7; ++i )
-		A[i] = 0.0;
-	for( i=0; i<50; ++i )
-	{
-		for( j=0; j<2; ++j )
-			A[i*7+j] = (real_t)rand() / (real_t)RAND_MAX;
-
-		A[i*7 + (i/10)+2] = 1.0;
-	}
-
-	/*	    ( -1.0 )
-	 *	    ( -0.5 )
-	 *	    ( ---- )
-	 *	g = (      )
-	 *	    (      )
-	 *	    (      )
-	 *	    (      ) */
-	for( i=0; i<7; ++i )
-		g[i] = 0.0;
-	g[0] = -1.0;
-	g[1] = -0.5;
-
-	for( i=0; i<50; ++i )
-		lbA[i] = 1.0;
-
-	/* ... and setting up user-defined constraint product function. */
-	MyConstraintProduct myCP( 7,50,A );
-
-
-	/* Setting up QProblem object and set construct product function. */
-	QProblem exampleCP( 7,50 );
-	exampleCP.setPrintLevel( PL_NONE );
-	
-	exampleCP.setConstraintProduct( &myCP );
-
-
-	/* Solve first QP. */
-	real_t cputime = 1.0;
-	int nWSR = 100;
-	exampleCP.init( H,g,A,0,0,lbA,0, nWSR,&cputime );
-
-
-	/* Solve second QP using a modified gradient. */
-	g[0] = -2.0;
-	g[1] =  0.5;
-
-	cputime = 1.0;
-	nWSR = 100;
-	exampleCP.hotstart( g,0,0,lbA,0, nWSR,&cputime );
-
-	/* Get and print solution of second QP. */
-	real_t xOpt[7];
-	exampleCP.getPrimalSolution( xOpt );
-	printf( "\nxOpt = [ %e, %e, %e ... ];  objVal = %e\n", xOpt[0],xOpt[1],xOpt[2],exampleCP.getObjVal() );
-	printf( "CPU time:  %.3f microseconds\n\n", cputime*1.0e6 ); 
-
-	
-	
-	/* Do the same without specifying constraint product. */
-	QProblem example( 7,50 );
-	example.setPrintLevel( PL_NONE );
-	
-	/* Solve first QP. */
-	g[0] = -1.0;
-	g[1] = -0.5;
-	
-	cputime = 1.0;
-	nWSR = 100;
-	example.init( H,g,A,0,0,lbA,0, nWSR,&cputime );
-	
-	/* Solve second QP using a modified gradient. */
-	g[0] = -2.0;
-	g[1] =  0.5;
-
-	cputime = 1.0;
-	nWSR = 100;
-	example.hotstart( g,0,0,lbA,0, nWSR,&cputime );
-
-	/* Get and print solution of second QP. */
-	example.getPrimalSolution( xOpt );
-	printf( "\nxOpt = [ %e, %e, %e ... ];  objVal = %e\n", xOpt[0],xOpt[1],xOpt[2],example.getObjVal() );
-	printf( "CPU time:  %.3f microseconds\n\n", cputime*1.0e6 ); 
-	
-	return 0;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example4CP.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example4CP.cpp
deleted file mode 100644
index 976e4e99381..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/examples/example4CP.cpp
+++ /dev/null
@@ -1,111 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file examples/example4CP.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2009-2014
- *
- *	Sample implementation of the ConstraintProduct class tailored for Example4.
- */
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/** 
- *	\brief Example illustrating the use of the \a ConstraintProduct class.
- *
- *	Example illustrating the use of the \a ConstraintProduct class.
- *
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- */
-class MyConstraintProduct : public ConstraintProduct
-{
-	public:
-		/** Default constructor. */
-		MyConstraintProduct( ) {};
-
-		/** Constructor. */
-		MyConstraintProduct(	int _nV,
-								int _nC,
-								real_t* _A
-								)
-		{
-			nV = _nV;
-			nC = _nC;
-			A  = _A;
-		};
-
-		/** Copy constructor (flat copy). */
-		MyConstraintProduct(	const MyConstraintProduct& rhs
-								)
-		{
-			nV = rhs.nV;
-			nC = rhs.nC;
-			A  = rhs.A;
-		};
-
-		/** Destructor. */
-		virtual ~MyConstraintProduct( ) {};
-		
-		/** Assignment operator (flat copy). */
-		MyConstraintProduct& operator=(	const MyConstraintProduct& rhs
-										)
-		{
-			if ( this != &rhs )
-			{
-				nV = rhs.nV;
-				nC = rhs.nC;
-				A  = rhs.A;
-			}
-			else
-				return *this;
-		};
-
-		virtual int operator() (	int constrIndex,
-									const real_t* const x,
-									real_t* const constrValue
-									) const
-		{
-			constrValue[0] = 1.0 * x[(constrIndex/10)+2];
-
-			for( int i=0; i<2; ++i )
-				constrValue[0] += A[constrIndex*nV + i] * x[i];
-
-			return 0;
-		};
-
-	protected:
-		int nV;			/**< Number of variables. */
-		int nC;			/**< Number of constraints. */
-		real_t* A;		/**< Pointer to full constraint matrix (typically not needed!). */
-};
-
-
-END_NAMESPACE_QPOASES
-
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/example5.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/example5.cpp
deleted file mode 100644
index f5f611f67c3..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/examples/example5.cpp
+++ /dev/null
@@ -1,200 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file examples/example5.cpp
- *	\author Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2011-2014
- *
- *	Very simple example for testing qpOASES (using the possibility to
- *  compute the local linear feedback law)
- */
-
-
-
-#include <stdlib.h>
-
-#include <qpOASES.hpp>
-#include "example4CP.cpp"
-
-
-/**	Example for qpOASES main function using the possibility to specify
- *	user-defined constraint product function. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	int i,j,jj;
-	real_t d = 0.0;
-
-	/* Setup data of first QP... */
-	real_t H[7*7];
-	real_t A[50*7];
-	real_t g[7];
-	real_t lbA[50];
-
-	/*	    ( 1.0 0.5 |                    )
-	 *	    ( 0.5 2.0 |                    )
-	 *	    ( --------+------------------- )
-	 *	H = (         | 1e-6               )
-	 *	    (         |      1e-6          )
-	 *	    (         |           ...      )
-	 *	    (         |               1e-6 ) */
-	for( i=0; i<7*7; ++i )
-		H[i] = 0.0;
-	for( i=2; i<7; ++i )
-		H[i*7+i] = 1.0e-6;
-	H[0] = 1.0;
-	H[1] = 0.5;
-	H[7] = 0.5;
-	H[8] = 2.0;
-
-	/*	    ( x.x x.x | 1.0             )
-	 *	    ( x.x x.x | ...             )
-	 *	    ( x.x x.x | 1.0             )
-	 *	    ( x.x x.x |     1.0         )
-	 *	A = ( x.x x.x |     ...         )
-	 *	    ( x.x x.x |     1.0         )
-	 *	    ( x.x x.x |         ...     )
-	 *	    ( x.x x.x |             1.0 )
-	 *	    ( x.x x.x |             ... )
-	 *	    ( x.x x.x |             1.0 ) */
-	for( i=0; i<50*7; ++i )
-		A[i] = 0.0;
-	for( i=0; i<50; ++i )
-	{
-		for( j=0; j<2; ++j )
-			A[i*7+j] = (real_t)rand() / (real_t)RAND_MAX;
-
-		A[i*7 + (i/10)+2] = 1.0;
-	}
-
-	/*	    ( -1.0 )
-	 *	    ( -0.5 )
-	 *	    ( ---- )
-	 *	g = (      )
-	 *	    (      )
-	 *	    (      )
-	 *	    (      ) */
-	for( i=0; i<7; ++i )
-		g[i] = 0.0;
-	g[0] = -1.0;
-	g[1] = -0.5;
-
-	for( i=0; i<50; ++i )
-		lbA[i] = 1.0;
-
-	/* ... and setting up user-defined constraint product function. */
-	MyConstraintProduct myCP( 7,50,A );
-
-
-	/* Setting up QProblem object and set construct product function. */
-	QProblem example( 7,50 );
-	example.setConstraintProduct( &myCP );
-
-
-	/* Solve first QP. */
-	real_t cputime = 1.0;
-	int nWSR = 100;
-	example.init( H,g,A,0,0,lbA,0, nWSR,&cputime );
-
-	/* Get and print solution of QP. */
-	real_t xOpt[7], yOpt[7+50];
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-
-
-	/* Compute local linear feedback law */
-	const int n_rhs = 7+7+50;
-	real_t g_in[7*n_rhs];
-	real_t b_in[7*n_rhs];
-	real_t bA_in[50*n_rhs];
-	real_t x_out[7*n_rhs];
-	real_t y_out[(7+50)*n_rhs];
-
-	int ii;
-	memset (g_in, 0, sizeof (g_in));
-	memset (b_in, 0, sizeof (b_in));
-	memset (bA_in, 0, sizeof (bA_in));
-
-	for ( ii = 0; ii < 7; ++ii )
-		g_in[ii*7 + ii] = 1.0;
-	for ( ii = 0; ii < 7; ++ii )
-		b_in[(ii+7)*7 + ii] = 1.0;
-	for ( ii = 0; ii < 50; ++ii )
-		bA_in[(ii+14)*50 + ii] = 1.0;
-
-	example.solveCurrentEQP ( n_rhs, g_in, b_in, b_in, bA_in, bA_in, x_out, y_out );
-
-	/* Verify validity of local feedback law by perturbation and hot starts */
-	real_t perturb = 1.0e-6;
-	real_t nrm = 0.0;
-	for ( ii = 0; ii < n_rhs; ++ii )
-	{
-		for ( jj = 0; jj < 7; ++jj )
-			g_in[ii*7 + jj] = g[jj] + g_in[ii*7+jj]*perturb;
-		for ( jj = 0; jj < 50; ++jj )
-			bA_in[ii*50 + jj] = lbA[jj] + bA_in[ii*50+jj]*perturb;
-
-		nWSR = 100;
-		example.hotstart( &g_in[ii*7],0,0,&bA_in[ii*50],0, nWSR, 0 );
-
-		real_t xPer[7], yPer[7+50];
-		example.getPrimalSolution( xPer );
-		example.getDualSolution( yPer );
-
-		for ( jj = 0; jj < 7; ++jj )
-		{
-			d = getAbs (x_out[ii*7+jj]*perturb - (xPer[jj]-xOpt[jj]) );
-			if (nrm < d) nrm=d;
-		}
-		for ( jj = 0; jj < 7+50; ++jj )
-		{
-			d = getAbs (y_out[ii*(7+50)+jj]*perturb - (yPer[jj]-yOpt[jj]) );
-			if (nrm < d) nrm=d;
-		}
-	}
-	printf ("Maximum perturbation over all directions: %e\n", nrm);
-
-	/* // print feedback matrix
-	for (ii = 0; ii < n_rhs; ++ii)
-	{
-		printf ("x: ");
-		for (jj = 0; jj < 7; ++jj )
-			printf ("%8.2e ", x_out[ii*7+jj]);
-		printf (" y: ");
-		for (jj = 0; jj < 7+50; ++jj )
-			printf ("%8.2e ", y_out[ii*(7+50)+jj]);
-		printf("\n");
-	}
-*/
-
-	return 0;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/exampleLP.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/exampleLP.cpp
deleted file mode 100644
index b7e2f3a3c63..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/examples/exampleLP.cpp
+++ /dev/null
@@ -1,87 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file examples/exampleLP.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2008-2014
- *
- *	Very simple example for solving a LP sequence using qpOASES.
- */
-
-
-
-#include <qpOASES.hpp>
-
-
-/** Example for qpOASES main function solving LPs. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	/* Setup data of first LP. */
-	real_t A[1*2] = { 1.0, 1.0 };
-	real_t g[2] = { 1.5, 1.0 };
-	real_t lb[2] = { 0.5, -2.0 };
-	real_t ub[2] = { 5.0, 2.0 };
-	real_t lbA[1] = { -1.0 };
-	real_t ubA[1] = { 2.0 };
-
-	/* Setup data of second LP. */
-	real_t g_new[2] = { 1.0, 1.5 };
-	real_t lb_new[2] = { 0.0, -1.0 };
-	real_t ub_new[2] = { 5.0, -0.5 };
-	real_t lbA_new[1] = { -2.0 };
-	real_t ubA_new[1] = { 1.0 };
-
-
-	/* Setting up QProblem object with zero Hessian matrix. */
-	QProblem example( 2,1,HST_ZERO );
-
-	Options options;
- 	//options.setToMPC();
-	example.setOptions( options );
-
-	/* Solve first LP. */
-	int nWSR = 10;
-	example.init( 0,g,A,lb,ub,lbA,ubA, nWSR,0 );
-
-	/* Solve second LP. */
-	nWSR = 10;
-	example.hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 );
-
-
-	/* Get and print solution of second LP. */
-	real_t xOpt[2];
-	example.getPrimalSolution( xOpt );
-	printf( "\nxOpt = [ %e, %e ];  objVal = %e\n\n", xOpt[0],xOpt[1],example.getObjVal() );
-
-	return 0;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/index.html b/3rdparty/qpOASES/qpOASES-3.0/examples/index.html
deleted file mode 100644
index 9b354532ede..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/examples/index.html
+++ /dev/null
@@ -1,19 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/examples</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/examples</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="Makefile">Makefile</a></li>
-  <li><a href="example1.cpp">example1.cpp</a></li>
-  <li><a href="example1a.cpp">example1a.cpp</a></li>
-  <li><a href="example1b.cpp">example1b.cpp</a></li>
-  <li><a href="example2.cpp">example2.cpp</a></li>
-  <li><a href="example3.cpp">example3.cpp</a></li>
-  <li><a href="example3b.cpp">example3b.cpp</a></li>
-  <li><a href="example4.cpp">example4.cpp</a></li>
-  <li><a href="example4CP.cpp">example4CP.cpp</a></li>
-  <li><a href="example5.cpp">example5.cpp</a></li>
-  <li><a href="exampleLP.cpp">exampleLP.cpp</a></li>
-  <li><a href="qrecipe.cpp">qrecipe.cpp</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/examples/qrecipe.cpp b/3rdparty/qpOASES/qpOASES-3.0/examples/qrecipe.cpp
deleted file mode 100644
index 1304ac739ce..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/examples/qrecipe.cpp
+++ /dev/null
@@ -1,495 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file examples/qrecipe.cpp
- *	\author Andreas Potschka
- *	\version 3.0
- *	\date 2007-2014
- *
- *	QRECIPE example from the CUTEr test set with sparse matrices.
- */
-
-
-
-#include <qpOASES.hpp>
-
-USING_NAMESPACE_QPOASES
-
-
-/** Infinity constant for qrecipe example. */
-const real_t Inf = INFTY;
-
-/** Sparse Hessian matrix data for qrecipe example. */
-sparse_int_t H_jc[] = { 0,  4,  8, 12, 16, 20, 20, 20, 20, 20, 20,
-	                   24, 28, 32, 36, 40, 40, 40, 40, 40, 40,
-					   44, 48, 52, 56, 60, 60, 60, 60, 60, 60, 60, 60, 60, 60,
-					   64, 68, 72, 76, 80, 80, 80, 80, 80, 80,
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80 };
-
-/** Sparse Hessian matrix data for qrecipe example. */
-sparse_int_t H_ir[] = {
-	0, 10, 20, 34, 1, 11, 21, 35, 2, 12, 22, 36, 3, 13, 23, 37, 4, 14, 24, 38, 
-	0, 10, 20, 34, 1, 11, 21, 35, 2, 12, 22, 36, 3, 13, 23, 37, 4, 14, 24, 38, 
-	0, 10, 20, 34, 1, 11, 21, 35, 2, 12, 22, 36, 3, 13, 23, 37, 4, 14, 24, 38, 
-	0, 10, 20, 34, 1, 11, 21, 35, 2, 12, 22, 36, 3, 13, 23, 37, 4, 14, 24, 38};
-
-/** Sparse Hessian matrix data for qrecipe example. */
-real_t H_val[] = {10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1,
-	1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 1,
-	10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 1, 10, 1,
-	1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10};
-
-/** Sparse constraint matrix data for qrecipe example. */
-sparse_int_t A_jc[] = {0, 10,  20,  30,  40,  50,  60,  70,  80,  90, 100, 110, 120,
-	130, 140, 150, 160, 170, 180, 190, 200, 210, 220, 230, 240, 250, 260, 270,
-	280, 290, 300, 301, 302, 303, 304, 305, 306, 307, 308, 309, 310, 311, 312,
-	313, 314, 318, 319, 320, 321, 322, 323, 324, 325, 326, 327, 328, 329, 331,
-	333, 335, 337, 339, 341, 343, 345, 347, 349, 351, 353, 355, 357, 359, 361,
-	363, 365, 367, 369, 371, 373, 383, 393, 403, 405, 408, 410, 413, 415, 418,
-	420, 422, 424, 426, 428, 430, 432, 434, 436, 438, 440, 442, 444, 446, 448,
-	450, 452, 454, 456, 458, 460, 462, 472, 482, 492, 494, 497, 499, 502, 504,
-	507, 509, 511, 513, 515, 517, 519, 521, 523, 525, 527, 529, 531, 533, 535,
-	537, 539, 541, 543, 545, 547, 549, 551, 561, 571, 581, 583, 586, 588, 591,
-	593, 596, 597, 598, 599, 600, 601, 602, 603, 604, 605, 606, 607, 608, 609,
-	610, 611, 612, 613, 614, 615, 616, 617, 618, 628, 638, 648, 650, 653, 655,
-	658, 660, 663};
-
-/** Sparse constraint matrix data for qrecipe example. */
-sparse_int_t A_ir[] = {0, 14, 35, 36, 71, 72, 85, 86, 87, 88, 1, 14, 35, 36, 71, 72, 85,
-	86, 87, 88, 2, 14, 35, 36, 71, 72, 85, 86, 87, 88, 3, 14, 35, 36, 71, 72,
-	85, 86, 87, 88, 4, 14, 35, 36, 71, 72, 85, 86, 87, 88, 5, 14, 35, 36, 71,
-	72, 85, 86, 87, 88, 6, 14, 35, 36, 71, 72, 85, 86, 87, 88, 7, 14, 35, 36,
-	71, 72, 85, 86, 87, 88, 8, 14, 35, 36, 71, 72, 85, 86, 87, 88, 9, 14, 35,
-	36, 71, 72, 85, 86, 87, 88, 0, 15, 37, 38, 69, 70, 79, 80, 81, 82, 1, 15,
-	37, 38, 69, 70, 79, 80, 81, 82, 2, 15, 37, 38, 69, 70, 79, 80, 81, 82, 3,
-	15, 37, 38, 69, 70, 79, 80, 81, 82, 4, 15, 37, 38, 69, 70, 79, 80, 81, 82,
-	5, 15, 37, 38, 69, 70, 79, 80, 81, 82, 6, 15, 37, 38, 69, 70, 79, 80, 81,
-	82, 7, 15, 37, 38, 69, 70, 79, 80, 81, 82, 8, 15, 37, 38, 69, 70, 79, 80,
-	81, 82, 9, 15, 37, 38, 69, 70, 79, 80, 81, 82, 0, 16, 39, 40, 67, 68, 73,
-	74, 75, 76, 1, 16, 39, 40, 67, 68, 73, 74, 75, 76, 2, 16, 39, 40, 67, 68,
-	73, 74, 75, 76, 3, 16, 39, 40, 67, 68, 73, 74, 75, 76, 4, 16, 39, 40, 67,
-	68, 73, 74, 75, 76, 5, 16, 39, 40, 67, 68, 73, 74, 75, 76, 6, 16, 39, 40,
-	67, 68, 73, 74, 75, 76, 7, 16, 39, 40, 67, 68, 73, 74, 75, 76, 8, 16, 39,
-	40, 67, 68, 73, 74, 75, 76, 9, 16, 39, 40, 67, 68, 73, 74, 75, 76, 10, 11,
-	12, 13, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 47, 48, 49, 50, 51,
-	52, 53, 54, 55, 56, 57, 47, 58, 48, 59, 49, 60, 50, 61, 51, 62, 52, 63, 53,
-	64, 54, 65, 55, 66, 46, 56, 45, 57, 47, 58, 48, 59, 49, 60, 50, 61, 51, 62,
-	52, 63, 53, 64, 54, 65, 55, 66, 46, 56, 45, 57, 10, 14, 71, 72, 85, 86, 87,
-	88, 89, 90, 11, 15, 69, 70, 79, 80, 81, 82, 83, 84, 12, 16, 67, 68, 73, 74,
-	75, 76, 77, 78, 35, 90, 36, 89, 90, 37, 84, 38, 83, 84, 39, 78, 40, 77, 78,
-	44, 58, 43, 59, 42, 60, 41, 61, 34, 62, 33, 63, 32, 64, 31, 65, 30, 66, 29,
-	46, 28, 45, 44, 58, 43, 59, 42, 60, 41, 61, 34, 62, 33, 63, 32, 64, 31, 65,
-	30, 66, 29, 46, 28, 45, 10, 14, 71, 72, 85, 86, 87, 88, 89, 90, 11, 15, 69,
-	70, 79, 80, 81, 82, 83, 84, 12, 16, 67, 68, 73, 74, 75, 76, 77, 78, 35, 90,
-	36, 89, 90, 37, 84, 38, 83, 84, 39, 78, 40, 77, 78, 27, 44, 26, 43, 25, 42,
-	24, 41, 23, 34, 22, 33, 21, 32, 20, 31, 19, 30, 18, 29, 17, 28, 27, 44, 26,
-	43, 25, 42, 24, 41, 23, 34, 22, 33, 21, 32, 20, 31, 19, 30, 18, 29, 17, 28,
-	10, 14, 71, 72, 85, 86, 87, 88, 89, 90, 11, 15, 69, 70, 79, 80, 81, 82, 83,
-	84, 12, 16, 67, 68, 73, 74, 75, 76, 77, 78, 35, 90, 36, 89, 90, 37, 84, 38,
-	83, 84, 39, 78, 40, 77, 78, 27, 26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 27,
-	26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 10, 14, 71, 72, 85, 86, 87, 88, 89,
-	90, 11, 15, 69, 70, 79, 80, 81, 82, 83, 84, 12, 16, 67, 68, 73, 74, 75, 76,
-	77, 78, 35, 90, 36, 89, 90, 37, 84, 38, 83, 84, 39, 78, 40, 77, 78};
-
-/** Sparse constraint matrix data for qrecipe example. */
-real_t A_val[] = {
--1.0000000000000000e+00,  1.0000000000000000e+00,  8.8678200000000004e+01,
- 9.3617050000000006e+01,  1.6000000000000000e+01,  8.1999999999999993e+00,
- 9.9000000000000000e+01,  8.0000000000000000e+01,  1.2000000000000000e+01,
- 9.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 8.0062830000000005e+01,  9.9224010000000007e+01,  1.0000000000000000e+02,
- 2.1100000000000001e+01,  1.0000000000000000e+02,  1.0000000000000000e+02,
- 1.1400000000000000e+02,  1.1680000000000000e+02, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  7.4697360000000003e+01,  8.3801220000000001e+01,
--8.1999999999999993e+00,  2.0000000000000000e+00,  9.0000000000000000e+01,
- 2.3999999999999999e+00, -1.2000000000000000e+01, -1.4800000000000001e+01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  7.9194209999999998e+01,
- 9.0175110000000004e+01,  4.3000000000000000e+01,  8.0000000000000000e+00,
- 1.0000000000000000e+02,  9.5000000000000000e+01,  9.0000000000000000e+00,
- 2.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 7.8568219999999997e+01,  8.5996200000000002e+01, -1.2500000000000000e+01,
- 1.0000000000000000e+00,  9.6500000000000000e+01,  4.0000000000000000e+00,
--1.8000000000000000e+01, -2.1899999999999999e+01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  8.2922240000000002e+01,  8.6963380000000001e+01,
- 6.5000000000000000e+01,  1.2500000000000000e+01,  1.0000000000000000e+02,
- 9.8000000000000000e+01,  4.9000000000000000e+01,  3.7000000000000000e+01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  8.2592740000000006e+01,
- 9.3147599999999997e+01, -1.2000000000000000e+01,  1.0000000000000000e+00,
- 9.6500000000000000e+01,  4.0000000000000000e+00, -1.8000000000000000e+01,
--2.1899999999999999e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 7.6506460000000004e+01,  7.8210250000000002e+01,  7.9000000000000000e+01,
- 1.2000000000000000e+01,  1.0000000000000000e+02,  9.5000000000000000e+01,
- 6.8000000000000000e+01,  6.1000000000000000e+01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  8.8357460000000003e+01,  9.4257840000000002e+01,
- 1.2500000000000000e+02,  6.1299999999999997e+01,  1.0000000000000000e+02,
- 1.0000000000000000e+02,  1.4500000000000000e+02,  1.4500000000000000e+02,
--1.0000000000000000e+00,  1.0000000000000000e+00,  9.0590469999999996e+01,
- 1.0582863000000000e+02,  6.2000000000000002e+00,  6.0000000000000000e+00,
- 9.7000000000000000e+01,  2.8500000000000000e+01,  4.0000000000000000e+00,
- 3.6000000000000001e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 8.8678200000000004e+01,  9.3617050000000006e+01,  1.6000000000000000e+01,
- 8.1999999999999993e+00,  9.9000000000000000e+01,  8.0000000000000000e+01,
- 1.2000000000000000e+01,  9.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  8.0062830000000005e+01,  9.9224010000000007e+01,
- 1.0000000000000000e+02,  2.1100000000000001e+01,  1.0000000000000000e+02,
- 1.0000000000000000e+02,  1.1400000000000000e+02,  1.1680000000000000e+02,
--1.0000000000000000e+00,  1.0000000000000000e+00,  7.4697360000000003e+01,
- 8.3801220000000001e+01, -8.1999999999999993e+00,  2.0000000000000000e+00,
- 9.0000000000000000e+01,  2.3999999999999999e+00, -1.2000000000000000e+01,
--1.4800000000000001e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 7.9194209999999998e+01,  9.0175110000000004e+01,  4.3000000000000000e+01,
- 8.0000000000000000e+00,  1.0000000000000000e+02,  9.5000000000000000e+01,
- 9.0000000000000000e+00,  2.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  7.8568219999999997e+01,  8.5996200000000002e+01,
--1.2500000000000000e+01,  1.0000000000000000e+00,  9.6500000000000000e+01,
- 4.0000000000000000e+00, -1.8000000000000000e+01, -2.1899999999999999e+01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  8.2922240000000002e+01,
- 8.6963380000000001e+01,  6.5000000000000000e+01,  1.2500000000000000e+01,
- 1.0000000000000000e+02,  9.8000000000000000e+01,  4.9000000000000000e+01,
- 3.7000000000000000e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 8.2592740000000006e+01,  9.3147599999999997e+01, -1.2000000000000000e+01,
- 1.0000000000000000e+00,  9.6500000000000000e+01,  4.0000000000000000e+00,
--1.8000000000000000e+01, -2.1899999999999999e+01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  7.6506460000000004e+01,  7.8210250000000002e+01,
- 7.9000000000000000e+01,  1.2000000000000000e+01,  1.0000000000000000e+02,
- 9.5000000000000000e+01,  6.8000000000000000e+01,  6.1000000000000000e+01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  8.8357460000000003e+01,
- 9.4257840000000002e+01,  1.2500000000000000e+02,  6.1299999999999997e+01,
- 1.0000000000000000e+02,  1.0000000000000000e+02,  1.4500000000000000e+02,
- 1.4500000000000000e+02, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 9.0590469999999996e+01,  1.0582863000000000e+02,  6.2000000000000002e+00,
- 6.0000000000000000e+00,  9.7000000000000000e+01,  2.8500000000000000e+01,
- 4.0000000000000000e+00,  3.6000000000000001e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  8.8678200000000004e+01,  9.3617050000000006e+01,
- 1.6000000000000000e+01,  8.1999999999999993e+00,  9.9000000000000000e+01,
- 8.0000000000000000e+01,  1.2000000000000000e+01,  9.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00,  8.0062830000000005e+01,
- 9.9224010000000007e+01,  1.0000000000000000e+02,  2.1100000000000001e+01,
- 1.0000000000000000e+02,  1.0000000000000000e+02,  1.1400000000000000e+02,
- 1.1680000000000000e+02, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 7.4697360000000003e+01,  8.3801220000000001e+01, -8.1999999999999993e+00,
- 2.0000000000000000e+00,  9.0000000000000000e+01,  2.3999999999999999e+00,
--1.2000000000000000e+01, -1.4800000000000001e+01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  7.9194209999999998e+01,  9.0175110000000004e+01,
- 4.3000000000000000e+01,  8.0000000000000000e+00,  1.0000000000000000e+02,
- 9.5000000000000000e+01,  9.0000000000000000e+00,  2.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00,  7.8568219999999997e+01,
- 8.5996200000000002e+01, -1.2500000000000000e+01,  1.0000000000000000e+00,
- 9.6500000000000000e+01,  4.0000000000000000e+00, -1.8000000000000000e+01,
--2.1899999999999999e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 8.2922240000000002e+01,  8.6963380000000001e+01,  6.5000000000000000e+01,
- 1.2500000000000000e+01,  1.0000000000000000e+02,  9.8000000000000000e+01,
- 4.9000000000000000e+01,  3.7000000000000000e+01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  8.2592740000000006e+01,  9.3147599999999997e+01,
--1.2000000000000000e+01,  1.0000000000000000e+00,  9.6500000000000000e+01,
- 4.0000000000000000e+00, -1.8000000000000000e+01, -2.1899999999999999e+01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  7.6506460000000004e+01,
- 7.8210250000000002e+01,  7.9000000000000000e+01,  1.2000000000000000e+01,
- 1.0000000000000000e+02,  9.5000000000000000e+01,  6.8000000000000000e+01,
- 6.1000000000000000e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 8.8357460000000003e+01,  9.4257840000000002e+01,  1.2500000000000000e+02,
- 6.1299999999999997e+01,  1.0000000000000000e+02,  1.0000000000000000e+02,
- 1.4500000000000000e+02,  1.4500000000000000e+02, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  9.0590469999999996e+01,  1.0582863000000000e+02,
- 6.2000000000000002e+00,  6.0000000000000000e+00,  9.7000000000000000e+01,
- 2.8500000000000000e+01,  4.0000000000000000e+00,  3.6000000000000001e+00,
--1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
- 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
- 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
- 1.0000000000000000e+00,  1.0000000000000000e+00, -1.2000000000000000e-01,
--3.8000000000000000e-01, -5.0000000000000000e-01,  1.0000000000000000e+00,
- 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
- 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
- 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
- 1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
--4.7000000000000000e+01, -8.6999999999999993e+00, -9.0000000000000000e+01,
--5.0000000000000000e+01, -1.0000000000000000e+01, -1.0000000000000000e+01,
--9.3000000000000000e+01, -8.9000000000000000e+01,  1.0000000000000000e+00,
--1.0000000000000000e+00, -4.7000000000000000e+01, -8.6999999999999993e+00,
--9.0000000000000000e+01, -5.0000000000000000e+01, -1.0000000000000000e+01,
--1.0000000000000000e+01, -8.9000000000000000e+01, -8.5000000000000000e+01,
- 1.0000000000000000e+00, -1.0000000000000000e+00, -4.7000000000000000e+01,
--8.6999999999999993e+00, -9.0000000000000000e+01, -5.0000000000000000e+01,
--1.0000000000000000e+01, -1.0000000000000000e+01, -9.1000000000000000e+01,
--8.8000000000000000e+01, -1.0000000000000000e+00,  5.0000000000000000e-01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  5.0000000000000000e-01,
--1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
- 5.0000000000000000e-01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 5.0000000000000000e-01,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00, -4.7000000000000000e+01,
--8.6999999999999993e+00, -9.0000000000000000e+01, -5.0000000000000000e+01,
--1.0000000000000000e+01, -1.0000000000000000e+01, -9.3000000000000000e+01,
--8.9000000000000000e+01,  1.0000000000000000e+00, -1.0000000000000000e+00,
--4.7000000000000000e+01, -8.6999999999999993e+00, -9.0000000000000000e+01,
--5.0000000000000000e+01, -1.0000000000000000e+01, -1.0000000000000000e+01,
--8.9000000000000000e+01, -8.5000000000000000e+01,  1.0000000000000000e+00,
--1.0000000000000000e+00, -4.7000000000000000e+01, -8.6999999999999993e+00,
--9.0000000000000000e+01, -5.0000000000000000e+01, -1.0000000000000000e+01,
--1.0000000000000000e+01, -9.1000000000000000e+01, -8.8000000000000000e+01,
--1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
- 5.0000000000000000e-01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 5.0000000000000000e-01, -1.0000000000000000e+00,  5.0000000000000000e-01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  5.0000000000000000e-01,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00, -4.7000000000000000e+01, -8.6999999999999993e+00,
--9.0000000000000000e+01, -5.0000000000000000e+01, -1.0000000000000000e+01,
--1.0000000000000000e+01, -9.3000000000000000e+01, -8.9000000000000000e+01,
- 1.0000000000000000e+00, -1.0000000000000000e+00, -4.7000000000000000e+01,
--8.6999999999999993e+00, -9.0000000000000000e+01, -5.0000000000000000e+01,
--1.0000000000000000e+01, -1.0000000000000000e+01, -8.9000000000000000e+01,
--8.5000000000000000e+01,  1.0000000000000000e+00, -1.0000000000000000e+00,
--4.7000000000000000e+01, -8.6999999999999993e+00, -9.0000000000000000e+01,
--5.0000000000000000e+01, -1.0000000000000000e+01, -1.0000000000000000e+01,
--9.1000000000000000e+01, -8.8000000000000000e+01, -1.0000000000000000e+00,
- 5.0000000000000000e-01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 5.0000000000000000e-01, -1.0000000000000000e+00,  5.0000000000000000e-01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  5.0000000000000000e-01,
--1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
--1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
--1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
--1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
--1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
--1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
--1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
--1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00, -4.7000000000000000e+01,
--8.6999999999999993e+00, -9.0000000000000000e+01, -5.0000000000000000e+01,
--1.0000000000000000e+01, -1.0000000000000000e+01, -9.3000000000000000e+01,
--8.9000000000000000e+01,  1.0000000000000000e+00, -1.0000000000000000e+00,
--4.7000000000000000e+01, -8.6999999999999993e+00, -9.0000000000000000e+01,
--5.0000000000000000e+01, -1.0000000000000000e+01, -1.0000000000000000e+01,
--8.9000000000000000e+01, -8.5000000000000000e+01,  1.0000000000000000e+00,
--1.0000000000000000e+00, -4.7000000000000000e+01, -8.6999999999999993e+00,
--9.0000000000000000e+01, -5.0000000000000000e+01, -1.0000000000000000e+01,
--1.0000000000000000e+01, -9.1000000000000000e+01, -8.8000000000000000e+01,
--1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
- 5.0000000000000000e-01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 5.0000000000000000e-01, -1.0000000000000000e+00,  5.0000000000000000e-01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  5.0000000000000000e-01};
-
-/** Gradient vector for qrecipe example. */
-real_t g[] = {+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
-	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
-	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
-	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
-	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
-	+0e+00, +0e+00, -2e+00, -2e+00, -2e+00, -2e+00, -2e+00, -2e+00, -2e+00,
-	-2e+00, +0e+00, -2e+00, +0e+00, +2e-03, +2e-03, +2e-03, +2e-03, +2e-03,
-	+2e-03, +1e-03, +2e-03, +2e-03, +2e-03, +0e+00, -2e-03, -2e-03, -2e-03,
-	-2e-03, -2e-03, -2e-03, -1e-03, -2e-03, -2e-03, -2e-03, +0e+00, +0e+00,
-	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +2e-03,
-	+2e-03, +2e-03, +2e-03, +2e-03, +2e-03, +1e-03, +2e-03, +2e-03, +2e-03,
-	+0e+00, -2e-03, -2e-03, -2e-03, -2e-03, -2e-03, -2e-03, -1e-03, -2e-03,
-	-2e-03, -2e-03, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
-	+0e+00, +0e+00, +0e+00, +2e-03, +2e-03, +2e-03, +2e-03, +2e-03, +2e-03,
-	+1e-03, +2e-03, +2e-03, +2e-03, +0e+00, -2e-03, -2e-03, -2e-03, -2e-03,
-	-2e-03, -2e-03, -1e-03, -2e-03, -2e-03, -2e-03, +0e+00, +0e+00, +0e+00,
-	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +1e-01, +1e-01,
-	+1e-01, +1e-01, +1e-01, +1e-01, +1e-01, +1e-01, +1e-01, +1e-01, +0e+00,
-	-1e-01, -1e-01, -1e-01, -1e-01, -1e-01, -1e-01, -1e-01, -1e-01, -1e-01,
-	-1e-01, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
-	+0e+00};
-
-/** Lower bound vector for qrecipe example. */
-real_t lb[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, -Inf, 0, -Inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 5,
-	10, 5, 0, 10, 0, 2, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 10, 5, 10, 5, 0, 10, 0, 5, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 5, 10, 5, 0, 10, 0, 5, 0, 10, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-
-/** Upper bound vector for qrecipe example. */
-real_t ub[] = {Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
-	Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
-	Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
-	Inf, Inf, 0, 92, 39, 87, 29, 0, 20, 0, 28, 20, 71, Inf, 130, 45, 53, 55, 75,
-	112, 0, 73, 480, 154, 121, 50, 30, 77, 20, 0, 18, 0, 5, 20, 71, Inf, Inf,
-	Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, 130, 55, 93, 60, 75, 115, 0, 67,
-	480, 154, 121, 50, 20, 37, 15, 0, 15, 0, 8, 20, 71, Inf, Inf, Inf, Inf, Inf,
-	Inf, Inf, Inf, Inf, Inf, 130, 55, 93, 60, 75, 105, 0, 67, 4980, 154, 110,
-	50, 20, 37, 15, 0, 25, 0, 8, 20, 71, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
-	Inf, Inf, 20, 20, 20, 20, 0, 20, 0, 20, 0, 20, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf};
-
-/** Lower constraint bound vector for qrecipe example. */
-real_t lbA[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -Inf, -Inf,
-	-Inf, -Inf, -Inf, -Inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0};
-
-/** Upper constraint bound vector for qrecipe example. */
-real_t ubA[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
-	Inf, Inf, Inf, Inf};
-
-/** Number of non-zeros in Hessian matrix for qrecipe example. */
-long H_nnz =  sizeof(H_val) / sizeof(real_t);
-
-/** Number of non-zeros in constraint matrix for qrecipe example. */
-long A_nnz =  sizeof(A_val) / sizeof(real_t);
-
-
-/** Example for calling qpOASES with sparse matrices. */
-int main( )
-{
-	long i;
-	int nWSR;
-	real_t err, tic, toc;
-	real_t *x1 = new real_t[180];
-	real_t *y1 = new real_t[271];
-	real_t *x2 = new real_t[180];
-	real_t *y2 = new real_t[271];
-
-	/* create sparse matrices */
-	SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val);
-	SparseMatrix *A = new SparseMatrix(91, 180, A_ir, A_jc, A_val);
-
-	H->createDiagInfo();
-
-	real_t* H_full = H->full();
-	real_t* A_full = A->full();
-
-	SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full);
-	DenseMatrix *Ad = new DenseMatrix(91,180,180,A_full);
-
-	/* solve with dense matrices */
-	nWSR = 1000;
-	QProblem qrecipeD(180, 91);
-	tic = getCPUtime();
-	qrecipeD.init(Hd, g, Ad, lb, ub, lbA, ubA, nWSR, 0);
-	toc = getCPUtime();
-	qrecipeD.getPrimalSolution(x1);
-	qrecipeD.getDualSolution(y1);
-
-	fprintf(stdFile, "Solved dense problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic);
-
-	/* solve with sparse matrices */
-	nWSR = 1000;
-	QProblem qrecipeS(180, 91);
-	tic = getCPUtime();
-	qrecipeS.init(H, g, A, lb, ub, lbA, ubA, nWSR, 0);
-	toc = getCPUtime();
-	qrecipeS.getPrimalSolution(x2);
-	qrecipeS.getDualSolution(y2);
-
-	fprintf(stdFile, "Solved sparse problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic);
-
-	/* check distance of solutions */
-	err = 0.0;
-	for (i = 0; i < 180; i++)
-		if (getAbs(x1[i] - x2[i]) > err)
-			err = getAbs(x1[i] - x2[i]);
-	fprintf(stdFile, "Primal error: %9.2e\n", err);
-	err = 0.0;
-	for (i = 0; i < 271; i++)
-		if (getAbs(y1[i] - y2[i]) > err)
-			err = getAbs(y1[i] - y2[i]);
-	fprintf(stdFile, "Dual error: %9.2e  (might not be unique)\n", err);
-
-	delete H;
-	delete A;
-	delete[] H_full;
-	delete[] A_full;
-	delete Hd;
-	delete Ad;
-
-	delete[] y2;
-	delete[] x2;
-	delete[] y1;
-	delete[] x1;
-
-	return 0;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/index.html b/3rdparty/qpOASES/qpOASES-3.0/include/index.html
deleted file mode 100644
index 77dd423600b..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/index.html
+++ /dev/null
@@ -1,9 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/include</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/include</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="qpOASES/">qpOASES/</a></li>
-  <li><a href="qpOASES.hpp">qpOASES.hpp</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES.hpp
deleted file mode 100644
index 4570d6031c1..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES.hpp
+++ /dev/null
@@ -1,59 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- */
-
-#ifndef __SINGLE_OBJECT__
-
-#include <qpOASES/QProblemB.hpp>
-#include <qpOASES/QProblem.hpp>
-#include <qpOASES/SQProblem.hpp>
-#include <qpOASES/extras/OQPinterface.hpp>
-#include <qpOASES/extras/SolutionAnalysis.hpp>
-
-#else
-
-#include <MessageHandling.cpp>
-#include <Utils.cpp>
-#include <Indexlist.cpp>
-#include <SubjectTo.cpp>
-#include <Bounds.cpp>
-#include <Constraints.cpp>
-#include <BLASReplacement.cpp>
-#include <LAPACKReplacement.cpp>
-#include <Matrices.cpp>
-#include <Options.cpp>
-#include <QProblemB.cpp>
-#include <Flipper.cpp>
-#include <QProblem.cpp>
-#include <SQProblem.cpp>
-#include <OQPinterface.cpp>
-#include <SolutionAnalysis.cpp>
-
-#endif
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Bounds.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Bounds.hpp
deleted file mode 100644
index c1ea151e804..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Bounds.hpp
+++ /dev/null
@@ -1,256 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/Bounds.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Declaration of the Bounds class designed to manage working sets of
- *	bounds within a QProblem.
- */
-
-
-#ifndef QPOASES_BOUNDS_HPP
-#define QPOASES_BOUNDS_HPP
-
-
-#include <qpOASES/SubjectTo.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/** 
- *	\brief Manages working sets of bounds (= box constraints).
- *
- *	This class manages working sets of bounds (= box constraints) 
- *	by storing index sets and other status information.
- *
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- */
-class Bounds : public SubjectTo
-{
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		Bounds( );
-
-		/** Constructor which takes the number of bounds. */
-		Bounds(	int _n									/**< Number of bounds. */
-				);
-
-		/** Copy constructor (deep copy). */
-		Bounds(	const Bounds& rhs						/**< Rhs object. */
-				);
-
-		/** Destructor. */
-		virtual ~Bounds( );
-
-		/** Assignment operator (deep copy). */
-		Bounds& operator=(	const Bounds& rhs			/**< Rhs object. */
-							);
-
-
-		/** Initialises object with given number of bounds.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INVALID_ARGUMENTS */
-		returnValue init(	int _n = 0					/**< Number of bounds. */
-							);
-
-
-		/** Initially adds number of a new (i.e. not yet in the list) bound to
-		 *  given index set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SETUP_BOUND_FAILED \n
-					RET_INDEX_OUT_OF_BOUNDS \n
-					RET_INVALID_ARGUMENTS */
-		returnValue setupBound(	int number,				/**< Number of new bound. */
-								SubjectToStatus _status	/**< Status of new bound. */
-								);
-
-		/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
-		 *  to the index set of free bounds; the order depends on the SujectToType
-		 *  of each index.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SETUP_BOUND_FAILED */
-		returnValue setupAllFree( );
-
-		/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
-		 *  to the index set of fixed bounds (on their lower bounds);
-		 *  the order depends on the SujectToType of each index.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SETUP_BOUND_FAILED */
-		returnValue setupAllLower( );
-
-		/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
-		 *  to the index set of fixed bounds (on their upper bounds);
-		 *  the order depends on the SujectToType of each index.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SETUP_BOUND_FAILED */
-		returnValue setupAllUpper( );
-
-
-		/** Moves index of a bound from index list of fixed to that of free bounds.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MOVING_BOUND_FAILED \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		returnValue moveFixedToFree(	int number				/**< Number of bound to be freed. */
-										);
-
-		/** Moves index of a bound from index list of free to that of fixed bounds.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MOVING_BOUND_FAILED \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		returnValue moveFreeToFixed(	int number,				/**< Number of bound to be fixed. */
-										SubjectToStatus _status	/**< Status of bound to be fixed. */
-										);
-
-		/** Flip fixed bound.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MOVING_BOUND_FAILED \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		returnValue flipFixed( int number );
-
-		/** Swaps the indices of two free bounds within the index set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SWAPINDEX_FAILED */
-		returnValue swapFree(	int number1,					/**< Number of first constraint or bound. */
-								int number2						/**< Number of second constraint or bound. */
-								);
-
-
-		/** Returns number of variables.
-		 *	\return Number of variables. */
-		inline int getNV( ) const;
-
-		/** Returns number of implicitly fixed variables.
-		 *	\return Number of implicitly fixed variables. */
-		inline int getNFV( ) const;
-
-		/** Returns number of bounded (but possibly free) variables.
-		 *	\return Number of bounded (but possibly free) variables. */
-		inline int getNBV( ) const;
-
-		/** Returns number of unbounded variables.
-		 *	\return Number of unbounded variables. */
-		inline int getNUV( ) const;
-
-		/** Returns number of free variables.
-		 *	\return Number of free variables. */
-		inline int getNFR( ) const;
-
-		/** Returns number of fixed variables.
-		 *	\return Number of fixed variables. */
-		inline int getNFX( ) const;
-
-
-		/** Returns a pointer to free variables index list.
-		 *	\return Pointer to free variables index list. */
-		inline Indexlist* getFree( );
-
-		/** Returns a pointer to fixed variables index list.
-		 *	\return Pointer to fixed variables index list. */
-		inline Indexlist* getFixed( );
-
-
-		/** Shifts forward type and status of all bounds by a given
-		 *  offset. This offset has to lie within the range [0,n/2] and has to
-		 *  be an integer divisor of the total number of bounds n.
-		 *  Type and status of the first \<offset\> bounds is thrown away,
-		 *  type and status of the last \<offset\> bounds is doubled,
-		 *  e.g. for offset = 2: \n
-		 *  shift( {b1,b2,b3,b4,b5,b6} ) = {b3,b4,b5,b6,b5,b6}
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INDEX_OUT_OF_BOUNDS \n
-		 			RET_INVALID_ARGUMENTS \n
-		 			RET_SHIFTING_FAILED */
-		virtual returnValue shift(	int offset		/**< Shift offset within the range [0,n/2] and integer divisor of n. */
-									);
-
-		/** Rotates forward type and status of all bounds by a given
-		 *  offset. This offset has to lie within the range [0,n].
-		 *  Example for offset = 2: \n
-		 *  rotate( {b1,b2,b3,b4,b5,b6} ) = {b3,b4,b5,b6,b1,b2}
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INDEX_OUT_OF_BOUNDS \n
-		 			RET_ROTATING_FAILED */
-		virtual returnValue rotate(	int offset		/**< Rotation offset within the range [0,n]. */
-									);
-
-
-		/** Prints information on bounds object
-		 *  (in particular, lists of free and fixed bounds.
-		 * \return SUCCESSFUL_RETURN \n
-				   RET_INDEXLIST_CORRUPTED */
-		returnValue print( );
-
-
-	/*
-	 *	PROTECTED MEMBER FUNCTIONS
-	 */
-	protected:
-		/** Frees all allocated memory.
-		 *  \return SUCCESSFUL_RETURN */
-		returnValue clear( );
-		
-		/** Copies all members from given rhs object.
-		 *  \return SUCCESSFUL_RETURN */
-		returnValue copy(	const Bounds& rhs	/**< Rhs object. */
-							);
-
-
-		/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
-		 *  to the index set corresponding to the desired status;
-		 *  the order depends on the SujectToType of each index.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SETUP_BOUND_FAILED */
-		returnValue setupAll(	SubjectToStatus _status	/**< Desired initial status for all bounds. */
-								);
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-		Indexlist freee;		/**< Index list of free variables. */
-		Indexlist fixed;		/**< Index list of fixed variables. */
-};
-
-
-END_NAMESPACE_QPOASES
-
-#include <qpOASES/Bounds.ipp>
-
-#endif	/* QPOASES_BOUNDS_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Bounds.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Bounds.ipp
deleted file mode 100644
index 0efdccf73a9..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Bounds.ipp
+++ /dev/null
@@ -1,120 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/Bounds.ipp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of inlined member functions of the Bounds class designed
- *	to manage working sets of bounds within a QProblem.
- */
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-/*
- *	g e t N V
- */
-inline int Bounds::getNV( ) const
-{
- 	return n;
-}
-
-
-/*
- *	g e t N F V
- */
-inline int Bounds::getNFV( ) const
-{
- 	return getNumberOfType( ST_EQUALITY );
-}
-
-
-/*
- *	g e t N B V
- */
-inline int Bounds::getNBV( ) const
-{
- 	return getNumberOfType( ST_BOUNDED );
-}
-
-
-/*
- *	g e t N U V
- */
-inline int Bounds::getNUV( ) const
-{
-	return getNumberOfType( ST_UNBOUNDED );
-}
-
-
-/*
- *	g e t N F R
- */
-inline int Bounds::getNFR( ) const
-{
- 	return freee.getLength( );
-}
-
-
-/*
- *	g e t N F X
- */
-inline int Bounds::getNFX( ) const
-{
-	return fixed.getLength( );
-}
-
-
-/*
- *	g e t F r e e
- */
-inline Indexlist* Bounds::getFree( )
-{
-	return &freee;
-}
-
-
-/*
- *	g e t F i x e d
- */
-inline Indexlist* Bounds::getFixed( )
-{
-	return &fixed;
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constants.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constants.hpp
deleted file mode 100644
index e2ba4e7edca..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constants.hpp
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/Constants.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Definition of all global constants.
- */
-
-
-#ifndef QPOASES_CONSTANTS_HPP
-#define QPOASES_CONSTANTS_HPP
-
-
-#include <qpOASES/Types.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/** Numerical value of machine precision (min eps, s.t. 1+eps > 1).
- *	Note: this value has to be positive! */
-#ifdef __USE_SINGLE_PRECISION__
-const real_t EPS = 1.193e-07;
-#else
-const real_t EPS = 2.221e-16;
-#endif /* __USE_SINGLE_PRECISION__ */
-
-
-/** Numerical value of zero (for situations in which it would be
- *	unreasonable to compare with 0.0).
- *	Note: this value has to be positive! */
-const real_t ZERO = 1.0e-25;
-
-/** Numerical value of infinity (e.g. for non-existing bounds).
-	Note: this value has to be positive! */
-const real_t INFTY = 1.0e20;
-
-
-/** Maximum number of characters within a string.
- *	Note: this value should be at least 41! */
-const unsigned int MAX_STRING_LENGTH = 160;
-
-
-END_NAMESPACE_QPOASES
-
-
-#endif	/* QPOASES_CONSTANTS_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/ConstraintProduct.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/ConstraintProduct.hpp
deleted file mode 100644
index 929f6c91556..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/ConstraintProduct.hpp
+++ /dev/null
@@ -1,91 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/ConstraintProduct.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2009-2014
- *
- *	Declaration of the ConstraintProduct class which allows to specify a
- *	user-defined function for evaluating the constraint product at the 
- *	current iterate to speed-up QP solution in case of a specially structured
- *	constraint matrix.
- */
-
-
-
-#ifndef QPOASES_CONSTRAINT_PRODUCT_HPP
-#define QPOASES_CONSTRAINT_PRODUCT_HPP
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/** 
- *	\brief Interface for specifying user-defined evaluations of constraint products.
- *
- *	A class which allows to specify a user-defined function for evaluating the 
- *	constraint product at the current iterate to speed-up QP solution in case 
- *	of a specially structured constraint matrix.
- *
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2009-2014
- */
-class ConstraintProduct
-{
-	public:
-		/** Default constructor. */
-		ConstraintProduct( ) {};
-
-		/** Copy constructor. */
-		ConstraintProduct(	const ConstraintProduct &toCopy	/**< Rhs object. */
-							) {};
-
-		/** Destructor. */
-		virtual ~ConstraintProduct( ) {};
-		
-		/** Assignment operator. */
-		ConstraintProduct &operator=(	const ConstraintProduct &toCopy	/**< Rhs object. */
-										)
-		{
-			return *this;
-		}
-
-		/** Evaluates the product of a given constraint with the current iterate.
-		 *	This function needs to be implemented in a derived class for the 
-		 *	user-defined constraint product function.
-		 *	\return 0:         successful \n
-					otherwise: not successful */
-		virtual int operator() (	int constrIndex,			/**< Number of constraint to be evaluated. */
-									const real_t* const x,		/**< Array containing current primal iterate. */
-									real_t* const constrValue	/**< Output: Scalar value of the evaluated constraint. */
-									) const = 0;
-};
-
-END_NAMESPACE_QPOASES
-
-
-#endif	/* QPOASES_CONSTRAINT_PRODUCT_HPP */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constraints.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constraints.hpp
deleted file mode 100644
index 8d4556ac720..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constraints.hpp
+++ /dev/null
@@ -1,246 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/Constraints.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Declaration of the Constraints class designed to manage working sets of
- *	constraints within a QProblem.
- */
-
-
-#ifndef QPOASES_CONSTRAINTS_HPP
-#define QPOASES_CONSTRAINTS_HPP
-
-
-#include <qpOASES/SubjectTo.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/** 
- *	\brief Manages working sets of constraints.
- *
- *	This class manages working sets of constraints by storing
- *	index sets and other status information.
- *
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- */
-class Constraints : public SubjectTo
-{
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		Constraints( );
-
-		/** Constructor which takes the number of constraints. */
-		Constraints(	int _n							/**< Number of constraints. */
-						);
-
-		/** Copy constructor (deep copy). */
-		Constraints(	const Constraints& rhs			/**< Rhs object. */
-						);
-
-		/** Destructor. */
-		virtual ~Constraints( );
-
-		/** Assignment operator (deep copy). */
-		Constraints& operator=(	const Constraints& rhs	/**< Rhs object. */
-								);
-
-
-		/** Initialises object with given number of constraints.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INVALID_ARGUMENTS */
-		returnValue init(	int _n = 0					/**< Number of constraints. */
-							);
-
-
-		/** Initially adds number of a new (i.e. not yet in the list) constraint to
-		 *  a given index set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SETUP_CONSTRAINT_FAILED \n
-					RET_INDEX_OUT_OF_BOUNDS \n
-					RET_INVALID_ARGUMENTS */
-		returnValue setupConstraint(	int number,				/**< Number of new constraint. */
-										SubjectToStatus _status	/**< Status of new constraint. */
-										);
-
-		/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to
-		 *  to the index set of inactive constraints; the order depends on the SujectToType
-		 *  of each index. Only disabled constraints are added to index set of disabled constraints!
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SETUP_CONSTRAINT_FAILED */
-		returnValue setupAllInactive( );
-
-		/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to
-		 *  to the index set of active constraints (on their lower bounds); the order depends on the SujectToType
-		 *  of each index. Only disabled constraints are added to index set of disabled constraints!
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SETUP_CONSTRAINT_FAILED */
-		returnValue setupAllLower( );
-
-		/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to
-		 *  to the index set of active constraints (on their upper bounds); the order depends on the SujectToType
-		 *  of each index. Only disabled constraints are added to index set of disabled constraints!
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SETUP_CONSTRAINT_FAILED */
-		returnValue setupAllUpper( );
-
-
-		/** Moves index of a constraint from index list of active to that of inactive constraints.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MOVING_CONSTRAINT_FAILED */
-		returnValue moveActiveToInactive(	int number				/**< Number of constraint to become inactive. */
-											);
-
-		/** Moves index of a constraint from index list of inactive to that of active constraints.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MOVING_CONSTRAINT_FAILED */
-		returnValue moveInactiveToActive(	int number,				/**< Number of constraint to become active. */
-											SubjectToStatus _status	/**< Status of constraint to become active. */
-											);
-
-		/** Flip fixed constraint.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MOVING_CONSTRAINT_FAILED \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		returnValue flipFixed( int number );
-
-
-		/** Returns the number of constraints.
-		 *	\return Number of constraints. */
-		inline int getNC( ) const;
-
-		/** Returns the number of implicit equality constraints.
-		 *	\return Number of implicit equality constraints. */
-		inline int getNEC( ) const;
-
-		/** Returns the number of "real" inequality constraints.
-		 *	\return Number of "real" inequality constraints. */
-		inline int getNIC( ) const;
-
-		/** Returns the number of unbounded constraints (i.e. without any bounds).
-		 *	\return Number of unbounded constraints (i.e. without any bounds). */
-		inline int getNUC( ) const;
-
-		/** Returns the number of active constraints.
-		 *	\return Number of active constraints. */
-		inline int getNAC( ) const;
-
-		/** Returns the number of inactive constraints.
-		 *	\return Number of inactive constraints. */
-		inline int getNIAC( ) const;
-
-
-		/** Returns a pointer to active constraints index list.
-		 *	\return Pointer to active constraints index list. */
-		inline Indexlist* getActive( );
-
-		/** Returns a pointer to inactive constraints index list.
-		 *	\return Pointer to inactive constraints index list. */
-		inline Indexlist* getInactive( );
-
-
-		/** Shifts forward type and status of all constraints by a given
-		 *  offset. This offset has to lie within the range [0,n/2] and has to
-		 *  be an integer divisor of the total number of constraints n.
-		 *  Type and status of the first \<offset\> constraints  is thrown away,
-		 *  type and status of the last \<offset\> constraints is doubled,
-		 *  e.g. for offset = 2: \n
-		 *  shift( {c1,c2,c3,c4,c5,c6} ) = {c3,c4,c5,c6,c5,c6}
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INDEX_OUT_OF_BOUNDS \n
-		 			RET_INVALID_ARGUMENTS \n
-		 			RET_SHIFTING_FAILED */
-		virtual returnValue shift(	int offset		/**< Shift offset within the range [0,n/2] and integer divisor of n. */
-									);
-
-		/** Rotates forward type and status of all constraints by a given
-		 *  offset. This offset has to lie within the range [0,n].
-		 *  Example for offset = 2: \n
-		 *  rotate( {c1,c2,c3,c4,c5,c6} ) = {c3,c4,c5,c6,c1,c2}
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INDEX_OUT_OF_BOUNDS \n
-		 			RET_ROTATING_FAILED */
-		virtual returnValue rotate(	int offset		/**< Rotation offset within the range [0,n]. */
-									);
-
-
-		/** Prints information on constraints object
-		 *  (in particular, lists of inactive and active constraints.
-		 * \return SUCCESSFUL_RETURN \n
-				   RET_INDEXLIST_CORRUPTED */
-		returnValue print( );
-
-
-	/*
-	 *	PROTECTED MEMBER FUNCTIONS
-	 */
-	protected:
-		/** Frees all allocated memory.
-		 *  \return SUCCESSFUL_RETURN */
-		returnValue clear( );
-		
-		/** Copies all members from given rhs object.
-		 *  \return SUCCESSFUL_RETURN */
-		returnValue copy(	const Constraints& rhs	/**< Rhs object. */
-							);
-
-
-		/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
-		 *  to the index set corresponding to the desired status;
-		 *  the order depends on the SujectToType of each index.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SETUP_CONSTRAINT_FAILED */
-		returnValue setupAll(	SubjectToStatus _status	/**< Desired initial status for all bounds. */
-								);
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-		Indexlist active;		/**< Index list of active constraints. */
-		Indexlist inactive;		/**< Index list of inactive constraints. */
-};
-
-END_NAMESPACE_QPOASES
-
-#include <qpOASES/Constraints.ipp>
-
-#endif	/* QPOASES_CONSTRAINTS_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constraints.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constraints.ipp
deleted file mode 100644
index b58e02b2516..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Constraints.ipp
+++ /dev/null
@@ -1,122 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/Constraints.ipp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Declaration of inlined member functions of the Constraints class designed
- *	to manage working sets of constraints within a QProblem.
- */
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	g e t N C
- */
-inline int Constraints::getNC( ) const
-{
- 	return n;
-}
-
-
-/*
- *	g e t N E C
- */
-inline int Constraints::getNEC( ) const
-{
-	return getNumberOfType( ST_EQUALITY );
-}
-
-
-/*
- *	g e t N I C
- */
-inline int Constraints::getNIC( ) const
-{
- 	return getNumberOfType( ST_BOUNDED );
-}
-
-
-/*
- *	g e t N U C
- */
-inline int Constraints::getNUC( ) const
-{
- 	return getNumberOfType( ST_UNBOUNDED );
-}
-
-
-/*
- *	g e t N A C
- */
-inline int Constraints::getNAC( ) const
-{
- 	return active.getLength( );
-}
-
-
-/*
- *	g e t N I A C
- */
-inline int Constraints::getNIAC( ) const
-{
-	return inactive.getLength( );
-}
-
-
-
-/*
- *	g e t A c t i v e
- */
-inline Indexlist* Constraints::getActive( )
-{
-	return &active;
-}
-
-
-/*
- *	g e t I n a c t i v e
- */
-inline Indexlist* Constraints::getInactive( )
-{
-	return &inactive;
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Flipper.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Flipper.hpp
deleted file mode 100644
index 4e6f215d681..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Flipper.hpp
+++ /dev/null
@@ -1,155 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/Flipper.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Declaration of the Options class designed to manage user-specified
- *	options for solving a QProblem.
- */
-
-
-#ifndef QPOASES_FLIPPER_HPP
-#define QPOASES_FLIPPER_HPP
-
-
-#include <qpOASES/Bounds.hpp>
-#include <qpOASES/Constraints.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/** 
- *	\brief Auxiliary class for storing a copy of the current matrix factorisations.
- *
- *	This auxiliary class stores a copy of the current matrix factorisations. It
- *	is used by the classe QProblemB and QProblem in case flipping bounds are enabled.
- *
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- */
-class Flipper
-{
-	friend class QProblemB;
-	friend class QProblem;
-
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		Flipper( );
-
-		/** Constructor which takes the number of bounds and constraints. */
-		Flipper(	unsigned int _nV,		/**< Number of bounds. */
-					unsigned int _nC = 0		/**< Number of constraints. */
-					);
-
-		/** Copy constructor (deep copy). */
-		Flipper(	const Flipper& rhs			/**< Rhs object. */
-					);
-
-		/** Destructor. */
-		~Flipper( );
-
-		/** Assignment operator (deep copy). */
-		Flipper& operator=(	const Flipper& rhs	/**< Rhs object. */
-							);
-
-
-		/** Initialises object with given number of bounds and constraints.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INVALID_ARGUMENTS */
-		returnValue init(	unsigned int _nV = 0,	/**< Number of bounds. */
-							unsigned int _nC = 0		/**< Number of constraints. */
-							);
-
-
-		/** Copies current values to non-null arguments (assumed to be allocated with consistent size).
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue get(	Bounds* const _bounds,					/**< Pointer to new bounds. */
-							real_t* const R,						/**< New matrix R. */
-							Constraints* const _constraints = 0,	/**< Pointer to new constraints. */
-							real_t* const _Q = 0,					/**< New matrix Q. */
-							real_t* const _T = 0					/**< New matrix T. */
-							) const;
-
-		/** Assigns new values to non-null arguments.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue set(	const Bounds* const _bounds,				/**< Pointer to new bounds. */
-							const real_t* const _R,						/**< New matrix R. */
-							const Constraints* const _constraints = 0,	/**< Pointer to new constraints. */
-							const real_t* const _Q = 0,					/**< New matrix Q. */
-							const real_t* const _T = 0					/**< New matrix T. */
-							);
-
-
-	/*
-	 *	PROTECTED MEMBER FUNCTIONS
-	 */
-	protected:
-		/** Frees all allocated memory.
-		 *  \return SUCCESSFUL_RETURN */
-		returnValue clear( );
-		
-		/** Copies all members from given rhs object.
-		 *  \return SUCCESSFUL_RETURN */
-		returnValue copy(	const Flipper& rhs	/**< Rhs object. */
-							);
-
-		/** Returns dimension of matrix T.
-		 *  \return Dimension of matrix T. */
-		unsigned int getDimT( ) const;
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-		unsigned int nV;				/**< Number of variables. */
-		unsigned int nC;				/**< Number of constraints. */
-
-		Bounds      bounds;				/**< Data structure for problem's bounds. */
-		Constraints constraints;		/**< Data structure for problem's constraints. */
-
-		real_t* R;						/**< Cholesky factor of H (i.e. H = R^T*R). */
-		real_t* Q;						/**< Orthonormal quadratic matrix, A = [0 T]*Q'. */
-		real_t* T;						/**< Reverse triangular matrix, A = [0 T]*Q'. */
-};
-
-
-END_NAMESPACE_QPOASES
-
-
-#endif	/* QPOASES_FLIPPER_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Indexlist.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Indexlist.hpp
deleted file mode 100644
index 4a0e40d7fd5..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Indexlist.hpp
+++ /dev/null
@@ -1,199 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/Indexlist.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Declaration of the Indexlist class designed to manage index lists of
- *	constraints and bounds within a SubjectTo object.
- */
-
-
-#ifndef QPOASES_INDEXLIST_HPP
-#define QPOASES_INDEXLIST_HPP
-
-
-#include <qpOASES/Utils.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/** 
- *	\brief Stores and manages index lists.
- *
- *	This class manages index lists of active/inactive bounds/constraints.
- *
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- */
-class Indexlist
-{
-	/*
-	 *  FRIENDS
-	 */
-	friend class DenseMatrix;
-	friend class SymDenseMat;
-	friend class SparseMatrix;
-	friend class SparseMatrixRow;
-	friend class SymSparseMat;
-
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		Indexlist( );
-
-		/** Constructor which takes the desired physical length of the index list. */
-		Indexlist(	int n	/**< Physical length of index list. */
-					);
-
-		/** Copy constructor (deep copy). */
-		Indexlist(	const Indexlist& rhs	/**< Rhs object. */
-					);
-
-		/** Destructor. */
-		~Indexlist( );
-
-		/** Assingment operator (deep copy). */
-		Indexlist& operator=(	const Indexlist& rhs	/**< Rhs object. */
-								);
-
-
-		/** Initialises index list of desired physical length.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INVALID_ARGUMENTS */
-		returnValue init(	int n = 0	/**< Physical length of index list. */
-							);
-
-
-		/** Creates an array of all numbers within the index set in correct order.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INDEXLIST_CORRUPTED */
-		returnValue	getNumberArray(	int** const numberarray	/**< Output: Array of numbers (NULL on error). */
-									) const;
-
-		/** Creates an array of all numbers within the index set in correct order.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INDEXLIST_CORRUPTED */
-		returnValue	getISortArray(	int** const iSortArray	/**< Output: iSort Array. */
-									) const;
-
-
-		/** Determines the index within the index list at which a given number is stored.
-		 *	\return >= 0: Index of given number. \n
-		 			-1: Number not found. */
-		int	getIndex(	int givennumber	/**< Number whose index shall be determined. */
-						) const;
-
-		/** Returns the number stored at a given physical index.
-		 *	\return >= 0: Number stored at given physical index. \n
-		 			-RET_INDEXLIST_OUTOFBOUNDS */
-		int	getNumber(	int physicalindex	/**< Physical index of the number to be returned. */
-						) const;
-
-
-		/** Returns the current length of the index list.
-		 *	\return Current length of the index list. */
-		inline int getLength( ) const;
-
-		/** Returns last number within the index list.
-		 *	\return Last number within the index list. */
-		inline int getLastNumber( ) const;
-
-
-		/** Adds number to index list.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INDEXLIST_MUST_BE_REORDERD \n
-		 			RET_INDEXLIST_EXCEEDS_MAX_LENGTH */
-		returnValue addNumber(	int addnumber			/**< Number to be added. */
-								);
-
-		/** Removes number from index list.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue removeNumber(	int removenumber	/**< Number to be removed. */
-									);
-
-		/** Swaps two numbers within index list.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue swapNumbers(	int number1,		/**< First number for swapping. */
-									int number2			/**< Second number for swapping. */
-									);
-
-		/** Determines if a given number is contained in the index set.
-		 *	\return BT_TRUE iff number is contain in the index set */
-		inline BooleanType isMember(	int _number		/**< Number to be tested for membership. */
-										) const;
-
-
-	/*
-	 *	PROTECTED MEMBER FUNCTIONS
-	 */
-	protected:
-		/** Frees all allocated memory.
-		 *  \return SUCCESSFUL_RETURN */
-		returnValue clear( );
-
-		/** Copies all members from given rhs object.
-		 *  \return SUCCESSFUL_RETURN */
-		returnValue copy(	const Indexlist& rhs	/**< Rhs object. */
-							);
-
-		/** Find first index j between -1 and length in sorted list of indices
-		 *  iSort such that numbers[iSort[j]] <= i < numbers[iSort[j+1]]. Uses
-		 *  bisection.
-		 *  \return j. */
-		int findInsert(	int i
-						) const;
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-		int* number;		/**< Array to store numbers of constraints or bounds. */
-		int* iSort;			/**< Index list to sort vector \a number */
-
-		int	length;			/**< Length of index list. */
-		int	first;			/**< Physical index of first element. */
-		int	last;			/**< Physical index of last element. */
-		int	lastusedindex;	/**< Physical index of last entry in index list. */
-		int	physicallength;	/**< Physical length of index list. */
-};
-
-END_NAMESPACE_QPOASES
-
-#include <qpOASES/Indexlist.ipp>
-
-#endif	/* QPOASES_INDEXLIST_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Indexlist.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Indexlist.ipp
deleted file mode 100644
index 338c8a33bf1..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Indexlist.ipp
+++ /dev/null
@@ -1,92 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/Indexlist.ipp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of inlined member functions of the Indexlist class designed
- *	to manage index lists of constraints and bounds within a QProblem_SubjectTo.
- */
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	g e t N u m b e r
- */
-inline int Indexlist::getNumber( int physicalindex ) const
-{
-	/* consistency check */
-	if ( ( physicalindex < 0 ) || ( physicalindex > length ) )
-		return -RET_INDEXLIST_OUTOFBOUNDS;
-
-	return number[physicalindex];
-}
-
-
-/*
- *	g e t L e n g t h
- */
-inline int Indexlist::getLength( ) const
-{
-	return length;
-}
-
-
-/*
- *	g e t L a s t N u m b e r
- */
-inline int Indexlist::getLastNumber( ) const
-{
-	return number[length-1];
-}
-
-
-/*
- *	g e t L a s t N u m b e r
- */
-inline BooleanType Indexlist::isMember( int _number ) const
-{
-	if ( getIndex( _number ) >= 0 )
-		return BT_TRUE;
-	else
-		return BT_FALSE;
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Matrices.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Matrices.hpp
deleted file mode 100644
index d03ad5f6a91..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Matrices.hpp
+++ /dev/null
@@ -1,867 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/Matrices.hpp
- *	\author Andreas Potschka, Hans Joachim Ferreau, Christian Kirches
- *	\version 3.0
- *	\date 2009-2014
- *
- *  Various matrix classes: Abstract base matrix class, dense and sparse matrices,
- *  including symmetry exploiting specializations.
- */
-
-
-
-#ifndef QPOASES_MATRICES_HPP
-#define QPOASES_MATRICES_HPP
-
-
-#include <qpOASES/Utils.hpp>
-#include <qpOASES/Indexlist.hpp>
-
-
-#ifdef __USE_SINGLE_PRECISION__
-
-	/** Macro for calling level 3 BLAS operation in single precision. */
-	#define GEMM sgemm_
-	/** Macro for calling level 3 BLAS operation in single precision. */
-	#define SYR ssyr_
-	/** Macro for calling level 3 BLAS operation in single precision. */
-	#define SYR2 ssyr2_
-	/** Macro for calling level 3 BLAS operation in single precision. */
-	#define POTRF spotrf_
-
-#else
-
-	/** Macro for calling level 3 BLAS operation in double precision. */
-	#define GEMM dgemm_
-	/** Macro for calling level 3 BLAS operation in double precision. */
-	#define SYR  dsyr_
-	/** Macro for calling level 3 BLAS operation in double precision. */
-	#define SYR2 dsyr2_
-	/** Macro for calling level 3 BLAS operation in double precision. */
-	#define POTRF dpotrf_
-
-#endif /* __USE_SINGLE_PRECISION__ */
-
-
-extern "C"
-{
-	/** Performs one of the matrix-matrix operation in double precision. */
-	void dgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*,
-			const double*, const double*, const unsigned long*, const double*, const unsigned long*,
-			const double*, double*, const unsigned long* );
-	/** Performs one of the matrix-matrix operation in single precision. */
-	void sgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*,
-			const float*, const float*, const unsigned long*, const float*, const unsigned long*,
-			const float*, float*, const unsigned long* );
-
-	/** Performs a symmetric rank 1 operation in double precision. */
-	void dsyr_ ( const char *, const unsigned long *, const double *, const double *,
-				 const unsigned long *, double *, const unsigned long *);
-	/** Performs a symmetric rank 1 operation in single precision. */
-	void ssyr_ ( const char *, const unsigned long *, const float *, const float *,
-				 const unsigned long *, float *, const unsigned long *);
-
-	/** Performs a symmetric rank 2 operation in double precision. */
-	void dsyr2_ ( const char *, const unsigned long *, const double *, const double *,
-				  const unsigned long *, const double *, const unsigned long *, double *, const unsigned long *);
-	/** Performs a symmetric rank 2 operation in single precision. */
-	void ssyr2_ ( const char *, const unsigned long *, const float *, const float *,
-				  const unsigned long *, const float *, const unsigned long *, float *, const unsigned long *);
-
-	/** Calculates the Cholesky factorization of a real symmetric positive definite matrix in double precision. */
-	void dpotrf_ ( const char *, const unsigned long *, double *, const unsigned long *, long * );
-	/** Calculates the Cholesky factorization of a real symmetric positive definite matrix in single precision. */
-	void spotrf_ ( const char *, const unsigned long *, float *, const unsigned long *, long * );
-}
-
-
-BEGIN_NAMESPACE_QPOASES
-
-/**
- * Integer type for sparse matrix row/column entries. Make this "int"
- * for 32 bit entries, and "long" for 64-bit entries on x86_64 platform.
- *
- * Most sparse codes still assume 32-bit entries here (HSL, BQPD, ...)
- */
-typedef int sparse_int_t;
-
-
-/**
- *	\brief Abstract base class for interfacing tailored matrix-vector operations.
- *
- *	Abstract base matrix class. Supplies interface to matrix vector
- *  products, including products with submatrices given by (ordered) working set
- *  index lists (see \a SubjectTo).
- *
- *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2011-2014
- */
-class Matrix
-{
-	public:
-		/** Default constructor. */
-		Matrix( ) { doNotFreeMemory(); };
-
-		/** Destructor. */
-		virtual ~Matrix( ) { };
-
-		/** Frees all internal memory. */
-		virtual void free( ) = 0;
-
-		/** Returns a deep-copy of the Matrix object.
-		 *	\return Deep-copy of Matrix object */
-		virtual Matrix* duplicate( ) const = 0;
-
-		/** Returns i-th diagonal entry.
-		 *	\return i-th diagonal entry */
-		virtual real_t diag(	int i			/**< Index. */
-								) const = 0;
-
-		/** Checks whether matrix is square and diagonal.
-		 *	\return BT_TRUE  iff matrix is square and diagonal; \n
-		 *	        BT_FALSE otherwise. */
-		virtual BooleanType isDiag( ) const = 0;
-
-        /** Get the N-norm of the matrix
-         *  \return N-norm of the matrix
-         */
-        virtual real_t getNorm(	int type = 2	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
-								) const = 0;
-
-        /** Get the N-norm of a row
-         *  \return N-norm of row \a rNum
-         */
-        virtual real_t getRowNorm(	int rNum,		/**< Row number. */
-									int type = 2	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
-									) const = 0;
-
-		/** Retrieve indexed entries of matrix row multiplied by alpha.
-		 *	\return SUCCESSFUL_RETURN */
-		virtual returnValue getRow(	int rNum,						/**< Row number. */
-									const Indexlist* const icols,	/**< Index list specifying columns. */
-									real_t alpha,					/**< Scalar factor. */
-									real_t *row						/**< Output row vector. */
-									) const = 0;
-
-		/** Retrieve indexed entries of matrix column multiplied by alpha.
-		 *	\return SUCCESSFUL_RETURN */
-		virtual returnValue getCol(	int cNum,						/**< Column number. */
-									const Indexlist* const irows,	/**< Index list specifying rows. */
-									real_t alpha,					/**< Scalar factor. */
-									real_t *col						/**< Output column vector. */
-									) const = 0;
-
-		/** Evaluate Y=alpha*A*X + beta*Y.
-		 *	\return SUCCESSFUL_RETURN */
-		virtual returnValue times (	int xN,					/**< Number of vectors to multiply. */
-									real_t alpha,			/**< Scalar factor for matrix vector product. */
-									const real_t *x,		/**< Input vector to be multiplied. */
-									int xLD,				/**< Leading dimension of input x. */
-									real_t beta,			/**< Scalar factor for y. */
-									real_t *y,				/**< Output vector of results. */
-									int yLD					/**< Leading dimension of output y. */
-									) const = 0;
-
-		/** Evaluate Y=alpha*A'*X + beta*Y.
-		 *	\return SUCCESSFUL_RETURN */
-		virtual returnValue transTimes (	int xN,				/**< Number of vectors to multiply. */
-											real_t alpha,		/**< Scalar factor for matrix vector product. */
-											const real_t *x,	/**< Input vector to be multiplied. */
-											int xLD,			/**< Leading dimension of input x. */
-											real_t beta,		/**< Scalar factor for y. */
-											real_t *y,			/**< Output vector of results. */
-											int yLD				/**< Leading dimension of output y. */
-											) const = 0;
-
-		/** Evaluate matrix vector product with submatrix given by Indexlist.
-		 *	\return SUCCESSFUL_RETURN */
-		virtual returnValue times (	const Indexlist* const irows,	/**< Index list specifying rows. */
-									const Indexlist* const icols,	/**< Index list specifying columns. */
-									int xN,							/**< Number of vectors to multiply. */
-									real_t alpha,					/**< Scalar factor for matrix vector product. */
-									const real_t *x,				/**< Input vector to be multiplied. */
-									int xLD,						/**< Leading dimension of input x. */
-									real_t beta,					/**< Scalar factor for y. */
-									real_t *y,						/**< Output vector of results. */
-									int yLD,						/**< Leading dimension of output y. */
-									BooleanType yCompr = BT_TRUE	/**< Compressed storage for y. */
-									) const = 0;
-
-		/** Evaluate matrix transpose vector product.
-		 *	\return SUCCESSFUL_RETURN */
-		virtual returnValue transTimes (	const Indexlist* const irows,	/**< Index list specifying rows. */
-											const Indexlist* const icols,	/**< Index list specifying columns. */
-											int xN,							/**< Number of vectors to multiply. */
-											real_t alpha,					/**< Scalar factor for matrix vector product. */
-											const real_t *x,				/**< Input vector to be multiplied. */
-											int xLD,						/**< Leading dimension of input x. */
-											real_t beta,					/**< Scalar factor for y. */
-											real_t *y,						/**< Output vector of results. */
-											int yLD							/**< Leading dimension of output y. */
-											) const = 0;
-
-		/** Adds given offset to diagonal of matrix.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_NO_DIAGONAL_AVAILABLE */
-		virtual returnValue addToDiag(	real_t alpha		/**< Diagonal offset. */
-										) = 0;
-
-		/** Allocates and creates dense matrix array in row major format.
-		 *
-		 *  Note: Calling function has to free allocated memory!
-		 *
-		 *  \return Pointer to matrix array. 
-		 */
-		virtual real_t* full() const = 0;
-
-
-		/** Prints matrix to screen.
-		 *	\return SUCCESSFUL_RETURN */
-		virtual returnValue print(	const char* name = 0	/** Name of matrix. */
-									) const = 0;
-
-
-		/** Returns whether internal memory needs to be de-allocated.
-		 *	\return BT_TRUE  iff internal memory needs to be de-allocated, \n
-		 			BT_FALSE otherwise */
-		BooleanType needToFreeMemory( ) const { return freeMemory; };
-
-		/** Enables de-allocation of internal memory. */
-		void doFreeMemory( ) { freeMemory = BT_TRUE; };
-
-		/** Disables de-allocation of internal memory. */
-		void doNotFreeMemory( ) { freeMemory = BT_FALSE; };
-
-
-	protected:
-			BooleanType freeMemory;				/**< Indicating whether internal memory needs to be de-allocated. */
-
-};
-
-
-/**
- *	\brief Abstract base class for interfacing matrix-vector operations tailored to symmetric matrices.
- *
- *	Abstract base class for symmetric matrices. Extends Matrix interface with
- *  bilinear form evaluation.
- *
- *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2011-2014
- */
-class SymmetricMatrix : public virtual Matrix
-{
-	public:
-
-		/** Returns a deep-copy of the SymmetricMatrix object.
-		 *	\return Deep-copy of SymmetricMatrix object */
-		virtual SymmetricMatrix* duplicateSym( ) const = 0;
-
-
-		/** Compute bilinear form y = x'*H*x using submatrix given by index list.
-		 *	\return SUCCESSFUL_RETURN */
-		virtual returnValue bilinear(	const Indexlist* const icols,	/**< Index list specifying columns of x. */
-										int xN,							/**< Number of vectors to multiply. */
-										const real_t *x,				/**< Input vector to be multiplied (uncompressed). */
-										int xLD,						/**< Leading dimension of input x. */
-										real_t *y,						/**< Output vector of results (compressed). */
-										int yLD							/**< Leading dimension of output y. */
-										) const = 0;
-
-};
-
-
-/**
- *	\brief Interfaces matrix-vector operations tailored to general dense matrices.
- *
- *	Dense matrix class (row major format).
- *
- *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2011-2014
- */
-class DenseMatrix : public virtual Matrix
-{
-	public:
-		/** Default constructor. */
-		DenseMatrix( ) : nRows(0), nCols(0), leaDim(0), val(0) {};
-
-		/** Constructor from vector of values.
-		 *  Caution: Data pointer must be valid throughout lifetime
-		 */
-		DenseMatrix(	int m,			/**< Number of rows. */
-						int n,			/**< Number of columns. */
-						int lD,			/**< Leading dimension. */
-						real_t *v		/**< Values. */
-						) : nRows(m), nCols(n), leaDim(lD), val(v) {}
-
-
-		/** Destructor. */
-		virtual ~DenseMatrix();
-
-		/** Frees all internal memory. */
-		virtual void free( );
-
-		/** Returns a deep-copy of the Matrix object.
-		 *	\return Deep-copy of Matrix object */
-		virtual Matrix *duplicate( ) const;
-
-		/** Returns i-th diagonal entry.
-		 *	\return i-th diagonal entry */
-		virtual real_t diag(	int i			/**< Index. */
-								) const;
-
-		/** Checks whether matrix is square and diagonal.
-		 *	\return BT_TRUE  iff matrix is square and diagonal; \n
-		 *	        BT_FALSE otherwise. */
-		virtual BooleanType isDiag( ) const;
-
-        /** Get the N-norm of the matrix
-         *  \return N-norm of the matrix
-         */
-        virtual real_t getNorm(	int type = 2	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
-								) const;
-		
-        /** Get the N-norm of a row
-         *  \return N-norm of row \a rNum
-         */
-        virtual real_t getRowNorm(	int rNum,		/**< Row number. */
-									int type = 2	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
-									) const;
-
-        /** Retrieve indexed entries of matrix row multiplied by alpha.
-		 *  \return SUCCESSFUL_RETURN */
-		virtual returnValue getRow(
-				int rNum,						/**< Row number. */
-				const Indexlist* const icols,	/**< Index list specifying columns. */
-				real_t alpha,					/**< Scalar factor. */
-				real_t *row						/**< Output row vector. */
-				) const;
-
-		/** Retrieve indexed entries of matrix column multiplied by alpha.
-		 *  \return SUCCESSFUL_RETURN */
-		virtual returnValue getCol(
-				int cNum,						/**< Column number. */
-				const Indexlist* const irows,	/**< Index list specifying rows. */
-				real_t alpha,					/**< Scalar factor. */
-				real_t *col						/**< Output column vector. */
-				) const;
-
-		/** Evaluate Y=alpha*A*X + beta*Y.
-		 *  \return SUCCESSFUL_RETURN. */
-		returnValue times (	int xN,					/**< Number of vectors to multiply. */
-							real_t alpha,			/**< Scalar factor for matrix vector product. */
-							const real_t *x,		/**< Input vector to be multiplied. */
-							int xLD,				/**< Leading dimension of input x. */
-							real_t beta,			/**< Scalar factor for y. */
-							real_t *y,				/**< Output vector of results. */
-							int yLD					/**< Leading dimension of output y. */
-							) const;
-
-		/** Evaluate Y=alpha*A'*X + beta*Y.
-		 *  \return SUCCESSFUL_RETURN. */
-		returnValue transTimes (	int xN,				/**< Number of vectors to multiply. */
-									real_t alpha,		/**< Scalar factor for matrix vector product. */
-									const real_t *x,	/**< Input vector to be multiplied. */
-									int xLD,			/**< Leading dimension of input x. */
-									real_t beta,		/**< Scalar factor for y. */
-									real_t *y,			/**< Output vector of results. */
-									int yLD				/**< Leading dimension of output y. */
-									) const;
-
-		/** Evaluate matrix vector product with submatrix given by Indexlist.
-		 *	\return SUCCESSFUL_RETURN */
-		virtual returnValue times (	const Indexlist* const irows,	/**< Index list specifying rows. */
-									const Indexlist* const icols,	/**< Index list specifying columns. */
-									int xN,							/**< Number of vectors to multiply. */
-									real_t alpha,					/**< Scalar factor for matrix vector product. */
-									const real_t *x,				/**< Input vector to be multiplied. */
-									int xLD,						/**< Leading dimension of input x. */
-									real_t beta,					/**< Scalar factor for y. */
-									real_t *y,						/**< Output vector of results. */
-									int yLD,						/**< Leading dimension of output y. */
-									BooleanType yCompr = BT_TRUE	/**< Compressed storage for y. */
-									) const;
-
-		/** Evaluate matrix transpose vector product.
-		 *	\return SUCCESSFUL_RETURN */
-		virtual returnValue transTimes (	const Indexlist* const irows,	/**< Index list specifying rows. */
-											const Indexlist* const icols,	/**< Index list specifying columns. */
-											int xN,							/**< Number of vectors to multiply. */
-											real_t alpha,					/**< Scalar factor for matrix vector product. */
-											const real_t *x,				/**< Input vector to be multiplied. */
-											int xLD,						/**< Leading dimension of input x. */
-											real_t beta,					/**< Scalar factor for y. */
-											real_t *y,						/**< Output vector of results. */
-											int yLD							/**< Leading dimension of output y. */
-											) const;
-
-		/** Adds given offset to diagonal of matrix.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_NO_DIAGONAL_AVAILABLE */
-		virtual returnValue addToDiag(	real_t alpha		/**< Diagonal offset. */
-										);
-
-
-		/** Allocates and creates dense matrix array in row major format.
-		 *
-		 *  Note: Calling function has to free allocated memory!
-		 *
-		 *  \return Pointer to matrix array. 
-		 */
-		virtual real_t* full() const;
-
-
-		/** Prints matrix to screen.
-		 *	\return SUCCESSFUL_RETURN */
-		virtual returnValue print( 	const char* name = 0	/** Name of matrix. */
-									) const;
-
-
-	protected:
-		int nRows;			/**< Number of rows. */
-		int nCols;			/**< Number of columns. */
-		int leaDim;			/**< Leading dimension. */
-		real_t *val;		/**< Vector of entries. */
-};
-
-
-/**
- *	\brief Interfaces matrix-vector operations tailored to symmetric dense matrices.
- *
- *	Symmetric dense matrix class.
- *
- *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2011-2014
- */
-class SymDenseMat : public DenseMatrix, public SymmetricMatrix
-{
-	public:
-		/** Default constructor. */
-		SymDenseMat() : DenseMatrix() {}
-
-		/** Constructor from vector of values. */
-		SymDenseMat(	int m,			/**< Number of rows. */
-						int n,			/**< Number of columns. */
-						int lD,			/**< Leading dimension. */
-						real_t *v		/**< Values. */
-						) : DenseMatrix(m, n, lD, v) {}
-
-		/** Returns a deep-copy of the Matrix object.
-		 *	\return Deep-copy of Matrix object */
-		virtual Matrix *duplicate( ) const;
-
-		/** Returns a deep-copy of the SymmetricMatrix object.
-		 *	\return Deep-copy of SymmetricMatrix object */
-		virtual SymmetricMatrix* duplicateSym( ) const;
-
-
-		/** Compute bilinear form y = x'*H*x using submatrix given by index list.
-		 *	\return SUCCESSFUL_RETURN */
-		virtual returnValue bilinear(	const Indexlist* const icols,	/**< Index list specifying columns of x. */
-										int xN,							/**< Number of vectors to multiply. */
-										const real_t *x,				/**< Input vector to be multiplied (uncompressed). */
-										int xLD,						/**< Leading dimension of input x. */
-										real_t *y,						/**< Output vector of results (compressed). */
-										int yLD							/**< Leading dimension of output y. */
-										) const;
-};
-
-
-/**
- *	\brief Interfaces matrix-vector operations tailored to general sparse matrices.
- *
- *	Sparse matrix class (col compressed format).
- *
- *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2011-2014
- */
-class SparseMatrix : public virtual Matrix
-{
-	public:
-		/** Default constructor. */
-		SparseMatrix();
-
-		/** Constructor with arguments. */
-		SparseMatrix(	int nr, 			/**< Number of rows. */
-						int nc, 			/**< Number of columns. */
-						sparse_int_t *r, 	/**< Row indices (length). */
-						sparse_int_t *c, 	/**< Indices to first entry of columns (nCols+1). */
-						real_t *v			/**< Vector of entries (length). */
-						);
-
-		/** Constructor from dense matrix. */
-		SparseMatrix(	int nr, 				/**< Number of rows. */
-						int nc,			 		/**< Number of columns. */
-						int ld,					/**< Leading dimension. */
-						const real_t * const v	/**< Row major stored matrix elements. */
-						);
-
-		/** Destructor. */
-		virtual ~SparseMatrix();
-
-		/** Frees all internal memory. */
-		virtual void free( );
-
-		/** Returns a deep-copy of the Matrix object.
-		 *	\return Deep-copy of Matrix object */
-		virtual Matrix *duplicate( ) const;
-
-		/** Returns i-th diagonal entry.
-		 *	\return i-th diagonal entry (or INFTY if diagonal does not exist)*/
-		virtual real_t diag(	int i			/**< Index. */
-								) const;
-
-		/** Checks whether matrix is square and diagonal.
-		 *	\return BT_TRUE  iff matrix is square and diagonal; \n
-		 *	        BT_FALSE otherwise. */
-		virtual BooleanType isDiag( ) const;
-
-
-        /** Get the N-norm of the matrix
-         *  \return N-norm of the matrix
-         */
-        virtual real_t getNorm(	int type = 2	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
-								) const;
-
-        /** Get the N-norm of a row
-         *  \return N-norm of row \a rNum
-         */
-        virtual real_t getRowNorm(	int rNum,		/**< Row number. */
-									int type = 2	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
-									) const;
-
-		/** Retrieve indexed entries of matrix row multiplied by alpha. */
-		virtual returnValue getRow(	int rNum,						/**< Row number. */
-									const Indexlist* const icols,	/**< Index list specifying columns. */
-									real_t alpha,					/**< Scalar factor. */
-									real_t *row						/**< Output row vector. */
-									) const;
-
-		/** Retrieve indexed entries of matrix column multiplied by alpha. */
-		virtual returnValue getCol(	int cNum,						/**< Column number. */
-									const Indexlist* const irows,	/**< Index list specifying rows. */
-									real_t alpha,					/**< Scalar factor. */
-									real_t *col						/**< Output column vector. */
-									) const;
-
-		/** Evaluate Y=alpha*A*X + beta*Y. */
-		virtual returnValue times (	int xN,					/**< Number of vectors to multiply. */
-									real_t alpha,			/**< Scalar factor for matrix vector product. */
-									const real_t *x,		/**< Input vector to be multiplied. */
-									int xLD,				/**< Leading dimension of input x. */
-									real_t beta,			/**< Scalar factor for y. */
-									real_t *y,				/**< Output vector of results. */
-									int yLD					/**< Leading dimension of output y. */
-									) const;
-
-		/** Evaluate Y=alpha*A'*X + beta*Y. */
-		virtual returnValue transTimes (	int xN,				/**< Number of vectors to multiply. */
-											real_t alpha,		/**< Scalar factor for matrix vector product. */
-											const real_t *x,	/**< Input vector to be multiplied. */
-											int xLD,			/**< Leading dimension of input x. */
-											real_t beta,		/**< Scalar factor for y. */
-											real_t *y,			/**< Output vector of results. */
-											int yLD				/**< Leading dimension of output y. */
-											) const;
-
-		/** Evaluate matrix vector product with submatrix given by Indexlist. */
-		virtual returnValue times (	const Indexlist* const irows,	/**< Index list specifying rows. */
-									const Indexlist* const icols,	/**< Index list specifying columns. */
-									int xN,							/**< Number of vectors to multiply. */
-									real_t alpha,					/**< Scalar factor for matrix vector product. */
-									const real_t *x,				/**< Input vector to be multiplied. */
-									int xLD,						/**< Leading dimension of input x. */
-									real_t beta,					/**< Scalar factor for y. */
-									real_t *y,						/**< Output vector of results. */
-									int yLD,						/**< Leading dimension of output y. */
-									BooleanType yCompr = BT_TRUE	/**< Compressed storage for y. */
-									) const;
-
-		/** Evaluate matrix transpose vector product. */
-		virtual returnValue transTimes (	const Indexlist* const irows,	/**< Index list specifying rows. */
-											const Indexlist* const icols,	/**< Index list specifying columns. */
-											int xN,							/**< Number of vectors to multiply. */
-											real_t alpha,					/**< Scalar factor for matrix vector product. */
-											const real_t *x,				/**< Input vector to be multiplied. */
-											int xLD,						/**< Leading dimension of input x. */
-											real_t beta,					/**< Scalar factor for y. */
-											real_t *y,						/**< Output vector of results. */
-											int yLD							/**< Leading dimension of output y. */
-											) const;
-
-		/** Adds given offset to diagonal of matrix.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_NO_DIAGONAL_AVAILABLE */
-		virtual returnValue addToDiag(	real_t alpha		/**< Diagonal offset. */
-										);
-
-		/** Create jd field from ir and jc.
-		 *  \return Pointer to jd. */
-		sparse_int_t *createDiagInfo();
-
-		/** Allocates and creates dense matrix array in row major format.
-		 *
-		 *  Note: Calling function has to free allocated memory!
-		 *
-		 *  \return Pointer to matrix array. 
-		 */
-		virtual real_t* full() const;
-
-		/** Prints matrix to screen.
-		 *	\return SUCCESSFUL_RETURN */
-		virtual returnValue print( 	const char* name = 0	/** Name of matrix. */
-									) const;
-
-
-	protected:
-		int nRows;			/**< Number of rows. */
-		int nCols;			/**< Number of columns. */
-		sparse_int_t *ir;	/**< Row indices (length). */
-		sparse_int_t *jc;	/**< Indices to first entry of columns (nCols+1). */
-		sparse_int_t *jd;	/**< Indices to first entry of lower triangle (including diagonal) (nCols). */
-		real_t *val;		/**< Vector of entries (length). */
-};
-
-
-/**
- *	\brief Interfaces matrix-vector operations tailored to general sparse matrices.
- *
- *	Sparse matrix class (row compressed format).
- *
- *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2011-2014
- */
-class SparseMatrixRow : public virtual Matrix
-{
-	public:
-		/** Default constructor. */
-		SparseMatrixRow();
-
-		/** Constructor with arguments. */
-		SparseMatrixRow(	int nr, 			/**< Number of rows. */
-							int nc, 			/**< Number of columns. */
-							sparse_int_t *r, 	/**< Indices to first entry of rows (nRows+1). */
-							sparse_int_t *c, 	/**< Column indices (length). */
-							real_t *v			/**< Vector of entries (length). */
-							);
-
-		/** Constructor from dense matrix. */
-		SparseMatrixRow(	int nr, 				/**< Number of rows. */
-							int nc,			 		/**< Number of columns. */
-							int ld,					/**< Leading dimension. */
-							const real_t * const v	/**< Row major stored matrix elements. */
-							);
-
-		/** Destructor. */
-		virtual ~SparseMatrixRow();
-
-		/** Frees all internal memory. */
-		virtual void free( );
-
-		/** Returns a deep-copy of the Matrix object.
-		 *	\return Deep-copy of Matrix object */
-		virtual Matrix *duplicate( ) const;
-
-		/** Returns i-th diagonal entry.
-		 *	\return i-th diagonal entry (or INFTY if diagonal does not exist)*/
-		virtual real_t diag(	int i			/**< Index. */
-								) const;
-
-		/** Checks whether matrix is square and diagonal.
-		 *	\return BT_TRUE  iff matrix is square and diagonal; \n
-		 *	        BT_FALSE otherwise. */
-		virtual BooleanType isDiag( ) const;
-
-		
-        /** Get the N-norm of the matrix
-         *  \return N-norm of the matrix
-         */
-        virtual real_t getNorm(	int type = 2	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
-								) const;
-
-        /** Get the N-norm of a row
-         *  \return N-norm of row \a rNum
-         */
-        virtual real_t getRowNorm(	int rNum,		/**< Row number. */
-									int type = 2	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
-									) const;
-
-		/** Retrieve indexed entries of matrix row multiplied by alpha. */
-		virtual returnValue getRow (
-				int rNum,						/**< Row number. */
-				const Indexlist* const icols,	/**< Index list specifying columns. */
-				real_t alpha,					/**< Scalar factor. */
-				real_t *row						/**< Output row vector. */
-				) const;
-
-		/** Retrieve indexed entries of matrix column multiplied by alpha. */
-		virtual returnValue getCol (
-				int cNum,						/**< Column number. */
-				const Indexlist* const irows,	/**< Index list specifying rows. */
-				real_t alpha,					/**< Scalar factor. */
-				real_t *col						/**< Output column vector. */
-				) const;
-
-		/** Evaluate Y=alpha*A*X + beta*Y. */
-		virtual returnValue times (	int xN,					/**< Number of vectors to multiply. */
-									real_t alpha,			/**< Scalar factor for matrix vector product. */
-									const real_t *x,		/**< Input vector to be multiplied. */
-									int xLD,				/**< Leading dimension of input x. */
-									real_t beta,			/**< Scalar factor for y. */
-									real_t *y,				/**< Output vector of results. */
-									int yLD					/**< Leading dimension of output y. */
-									) const;
-
-		/** Evaluate Y=alpha*A'*X + beta*Y. */
-		virtual returnValue transTimes (	int xN,				/**< Number of vectors to multiply. */
-											real_t alpha,		/**< Scalar factor for matrix vector product. */
-											const real_t *x,	/**< Input vector to be multiplied. */
-											int xLD,			/**< Leading dimension of input x. */
-											real_t beta,		/**< Scalar factor for y. */
-											real_t *y,			/**< Output vector of results. */
-											int yLD				/**< Leading dimension of output y. */
-											) const;
-
-		/** Evaluate matrix vector product with submatrix given by Indexlist. */
-		virtual returnValue times (	const Indexlist* const irows,	/**< Index list specifying rows. */
-									const Indexlist* const icols,	/**< Index list specifying columns. */
-									int xN,							/**< Number of vectors to multiply. */
-									real_t alpha,					/**< Scalar factor for matrix vector product. */
-									const real_t *x,				/**< Input vector to be multiplied. */
-									int xLD,						/**< Leading dimension of input x. */
-									real_t beta,					/**< Scalar factor for y. */
-									real_t *y,						/**< Output vector of results. */
-									int yLD,						/**< Leading dimension of output y. */
-									BooleanType yCompr = BT_TRUE	/**< Compressed storage for y. */
-									) const;
-
-		/** Evaluate matrix transpose vector product. */
-		virtual returnValue transTimes (	const Indexlist* const irows,	/**< Index list specifying rows. */
-											const Indexlist* const icols,	/**< Index list specifying columns. */
-											int xN,							/**< Number of vectors to multiply. */
-											real_t alpha,					/**< Scalar factor for matrix vector product. */
-											const real_t *x,				/**< Input vector to be multiplied. */
-											int xLD,						/**< Leading dimension of input x. */
-											real_t beta,					/**< Scalar factor for y. */
-											real_t *y,						/**< Output vector of results. */
-											int yLD							/**< Leading dimension of output y. */
-											) const;
-
-		/** Adds given offset to diagonal of matrix.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_NO_DIAGONAL_AVAILABLE */
-		virtual returnValue addToDiag(	real_t alpha		/**< Diagonal offset. */
-										);
-
-		/** Create jd field from ir and jc.
-		 *  \return Pointer to jd. */
-		sparse_int_t *createDiagInfo();
-
-		/** Allocates and creates dense matrix array in row major format.
-		 *
-		 *  Note: Calling function has to free allocated memory!
-		 *
-		 *  \return Pointer to matrix array. 
-		 */
-		virtual real_t* full() const;
-
-		/** Prints matrix to screen.
-		 *	\return SUCCESSFUL_RETURN */
-		virtual returnValue print( 	const char* name = 0	/** Name of matrix. */
-									) const;
-
-
-	protected:
-		int nRows;			/**< Number of rows. */
-		int nCols;			/**< Number of columns. */
-		sparse_int_t *jr;	/**< Indices to first entry of row (nRows+1). */
-		sparse_int_t *ic;	/**< Column indices (length). */
-		sparse_int_t *jd;	/**< Indices to first entry of upper triangle (including diagonal) (nRows). */
-		real_t *val;		/**< Vector of entries (length). */
-};
-
-
-/**
- *	\brief Interfaces matrix-vector operations tailored to symmetric sparse matrices.
- *
- *	Symmetric sparse matrix class (column compressed format).
- *
- *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2011-2014
- */
-class SymSparseMat : public SymmetricMatrix, public SparseMatrix
-{
-	public:
-		/** Default constructor. */
-		SymSparseMat() : SparseMatrix() {}
-
-		/** Constructor with arguments. */
-		SymSparseMat(	int nr, 			/**< Number of rows. */
-						int nc, 			/**< Number of columns. */
-						sparse_int_t *r, 	/**< Row indices (length). */
-						sparse_int_t *c, 	/**< Indices to first entry of columns (nCols+1). */
-						real_t *v			/**< Vector of entries (length). */
-						) : SparseMatrix(nr, nc, r, c, v) {}
-
-		/** Constructor from dense matrix. */
-		SymSparseMat(	int nr, 				/**< Number of rows. */
-						int nc,			 		/**< Number of columns. */
-						int ld,					/**< Leading dimension. */
-						const real_t * const v	/**< Row major stored matrix elements. */
-						) : SparseMatrix(nr, nc, ld, v) {}
-
-		/** Returns a deep-copy of the Matrix object.
-		 *	\return Deep-copy of Matrix object */
-		virtual Matrix *duplicate( ) const;
-
-		/** Returns a deep-copy of the SymmetricMatrix object.
-		 *	\return Deep-copy of SymmetricMatrix object */
-		virtual SymmetricMatrix* duplicateSym( ) const;
-
-
-		/** Compute bilinear form y = x'*H*x using submatrix given by index list.
-		*	\return SUCCESSFUL_RETURN */
-		virtual returnValue bilinear(	const Indexlist* const icols,	/**< Index list specifying columns of x. */
-										int xN,							/**< Number of vectors to multiply. */
-										const real_t *x,				/**< Input vector to be multiplied (uncompressed). */
-										int xLD,						/**< Leading dimension of input x. */
-										real_t *y,						/**< Output vector of results (compressed). */
-										int yLD							/**< Leading dimension of output y. */
-										) const;
-};
-
-
-END_NAMESPACE_QPOASES
-
-
-#endif	/* QPOASES_MATRICES_HPP */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/MessageHandling.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/MessageHandling.hpp
deleted file mode 100644
index fcddeb8fd89..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/MessageHandling.hpp
+++ /dev/null
@@ -1,472 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/MessageHandling.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches (thanks to Leonard Wirsching)
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Declaration of the MessageHandling class including global return values.
- */
-
-
-#ifndef QPOASES_MESSAGEHANDLING_HPP
-#define QPOASES_MESSAGEHANDLING_HPP
-
-
-#include <stdio.h>
-#include <string.h>
-
-#ifdef __DEBUG__
-#include <assert.h>
-#endif
-
-#include <qpOASES/Constants.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/** Default file to display messages. */
-static FILE* stdFile = stdout;
-
-
-/**
- *	\brief Defines all symbols for global return values.
- *	
- *  The enumeration returnValueType defines all symbols for global return values.
- *	Important: All return values are assumed to be nonnegative!
- *
- *	\author Hans Joachim Ferreau
- */
-enum returnValue
-{
-TERMINAL_LIST_ELEMENT = -1,						/**< Terminal list element, internal usage only! */
-/* miscellaneous */
-SUCCESSFUL_RETURN = 0,							/**< Successful return. */
-RET_DIV_BY_ZERO,		   						/**< Division by zero. */
-RET_INDEX_OUT_OF_BOUNDS,						/**< Index out of bounds. */
-RET_INVALID_ARGUMENTS,							/**< At least one of the arguments is invalid. */
-RET_ERROR_UNDEFINED,							/**< Error number undefined. */
-RET_WARNING_UNDEFINED,							/**< Warning number undefined. */
-RET_INFO_UNDEFINED,								/**< Info number undefined. */
-RET_EWI_UNDEFINED,								/**< Error/warning/info number undefined. */
-RET_AVAILABLE_WITH_LINUX_ONLY,					/**< This function is available under Linux only. */
-RET_UNKNOWN_BUG,								/**< The error occurred is not yet known. */
-RET_PRINTLEVEL_CHANGED,							/**< Print level changed. */
-RET_NOT_YET_IMPLEMENTED,						/**< Requested function is not yet implemented in this version of qpOASES. */
-/* Indexlist */
-RET_INDEXLIST_MUST_BE_REORDERD,					/**< Index list has to be reordered. (12) */
-RET_INDEXLIST_EXCEEDS_MAX_LENGTH,				/**< Index list exceeds its maximal physical length. */
-RET_INDEXLIST_CORRUPTED,						/**< Index list corrupted. */
-RET_INDEXLIST_OUTOFBOUNDS,						/**< Physical index is out of bounds. */
-RET_INDEXLIST_ADD_FAILED,						/**< Adding indices from another index set failed. */
-RET_INDEXLIST_INTERSECT_FAILED,					/**< Intersection with another index set failed. */
-/* SubjectTo / Bounds / Constraints */
-RET_INDEX_ALREADY_OF_DESIRED_STATUS,			/**< Index is already of desired status. (18) */
-RET_ADDINDEX_FAILED,							/**< Adding index to index set failed. */
-RET_REMOVEINDEX_FAILED,							/**< Removing index from index set failed. (20) */
-RET_SWAPINDEX_FAILED,							/**< Cannot swap between different indexsets. */
-RET_NOTHING_TO_DO,								/**< Nothing to do. */
-RET_SETUP_BOUND_FAILED,							/**< Setting up bound index failed. */
-RET_SETUP_CONSTRAINT_FAILED,					/**< Setting up constraint index failed. */
-RET_MOVING_BOUND_FAILED,						/**< Moving bound between index sets failed. */
-RET_MOVING_CONSTRAINT_FAILED,					/**< Moving constraint between index sets failed. */
-RET_SHIFTING_FAILED,							/**< Shifting of bounds/constraints failed. */
-RET_ROTATING_FAILED,							/**< Rotating of bounds/constraints failed. */
-/* QProblem */
-RET_QPOBJECT_NOT_SETUP,							/**< The QP object has not been setup correctly, use another constructor. (29) */
-RET_QP_ALREADY_INITIALISED,						/**< QProblem has already been initialised. (30) */
-RET_NO_INIT_WITH_STANDARD_SOLVER,				/**< Initialisation via extern QP solver is not yet implemented. */
-RET_RESET_FAILED,								/**< Reset failed. */
-RET_INIT_FAILED,								/**< Initialisation failed. */
-RET_INIT_FAILED_TQ,								/**< Initialisation failed due to TQ factorisation. */
-RET_INIT_FAILED_CHOLESKY,						/**< Initialisation failed due to Cholesky decomposition. */
-RET_INIT_FAILED_HOTSTART,						/**< Initialisation failed! QP could not be solved! */
-RET_INIT_FAILED_INFEASIBILITY,					/**< Initial QP could not be solved due to infeasibility! */
-RET_INIT_FAILED_UNBOUNDEDNESS,					/**< Initial QP could not be solved due to unboundedness! */
-RET_INIT_FAILED_REGULARISATION,					/**< Initialisation failed as Hessian matrix could not be regularised. */
-RET_INIT_SUCCESSFUL,							/**< Initialisation done. (40) */
-RET_OBTAINING_WORKINGSET_FAILED,				/**< Failed to obtain working set for auxiliary QP. */
-RET_SETUP_WORKINGSET_FAILED,					/**< Failed to setup working set for auxiliary QP. */
-RET_SETUP_AUXILIARYQP_FAILED,					/**< Failed to setup auxiliary QP for initialised homotopy. */
-RET_NO_EXTERN_SOLVER,							/**< No extern QP solver available. */
-RET_QP_UNBOUNDED,								/**< QP is unbounded. */
-RET_QP_INFEASIBLE,								/**< QP is infeasible. */
-RET_QP_NOT_SOLVED,								/**< Problems occurred while solving QP with standard solver. */
-RET_QP_SOLVED,									/**< QP successfully solved. */
-RET_UNABLE_TO_SOLVE_QP,							/**< Problems occurred while solving QP. */
-RET_INITIALISATION_STARTED,						/**< Starting problem initialisation... */
-RET_HOTSTART_FAILED,							/**< Unable to perform homotopy due to internal error. (50) */
-RET_HOTSTART_FAILED_TO_INIT,					/**< Unable to initialise problem. */
-RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED,		/**< Unable to perform homotopy as previous QP is not solved. */
-RET_ITERATION_STARTED,							/**< Iteration... */
-RET_SHIFT_DETERMINATION_FAILED,					/**< Determination of shift of the QP data failed. */
-RET_STEPDIRECTION_DETERMINATION_FAILED,			/**< Determination of step direction failed. */
-RET_STEPLENGTH_DETERMINATION_FAILED,			/**< Determination of step direction failed. */
-RET_OPTIMAL_SOLUTION_FOUND,						/**< Optimal solution of neighbouring QP found. */
-RET_HOMOTOPY_STEP_FAILED,						/**< Unable to perform homotopy step. */
-RET_HOTSTART_STOPPED_INFEASIBILITY,				/**< Premature homotopy termination because QP is infeasible. */
-RET_HOTSTART_STOPPED_UNBOUNDEDNESS,				/**< Premature homotopy termination because QP is unbounded. (60) */
-RET_WORKINGSET_UPDATE_FAILED,					/**< Unable to update working sets according to initial guesses. */
-RET_MAX_NWSR_REACHED,							/**< Maximum number of working set recalculations performed. */
-RET_CONSTRAINTS_NOT_SPECIFIED,					/**< Problem does comprise constraints! You also have to specify new constraints' bounds. */
-RET_INVALID_FACTORISATION_FLAG,					/**< Invalid factorisation flag. */
-RET_UNABLE_TO_SAVE_QPDATA,						/**< Unable to save QP data. */
-RET_STEPDIRECTION_FAILED_TQ,					/**< Abnormal termination due to TQ factorisation. */
-RET_STEPDIRECTION_FAILED_CHOLESKY,				/**< Abnormal termination due to Cholesky factorisation. */
-RET_CYCLING_DETECTED,							/**< Cycling detected. */
-RET_CYCLING_NOT_RESOLVED,						/**< Cycling cannot be resolved, QP probably infeasible. */
-RET_CYCLING_RESOLVED,							/**< Cycling probably resolved. (70) */
-RET_STEPSIZE,									/**< For displaying performed stepsize. */
-RET_STEPSIZE_NONPOSITIVE,						/**< For displaying non-positive stepsize. */
-RET_SETUPSUBJECTTOTYPE_FAILED,					/**< Setup of SubjectToTypes failed. */
-RET_ADDCONSTRAINT_FAILED,						/**< Addition of constraint to working set failed. */
-RET_ADDCONSTRAINT_FAILED_INFEASIBILITY,			/**< Addition of constraint to working set failed (due to QP infeasibility). */
-RET_ADDBOUND_FAILED,							/**< Addition of bound to working set failed. */
-RET_ADDBOUND_FAILED_INFEASIBILITY,				/**< Addition of bound to working set failed (due to QP infeasibility). */
-RET_REMOVECONSTRAINT_FAILED,					/**< Removal of constraint from working set failed. */
-RET_REMOVEBOUND_FAILED,							/**< Removal of bound from working set failed. */
-RET_REMOVE_FROM_ACTIVESET,						/**< Removing from active set... (80) */
-RET_ADD_TO_ACTIVESET,							/**< Adding to active set... */
-RET_REMOVE_FROM_ACTIVESET_FAILED,				/**< Removing from active set failed. */
-RET_ADD_TO_ACTIVESET_FAILED,					/**< Adding to active set failed. */
-RET_CONSTRAINT_ALREADY_ACTIVE,					/**< Constraint is already active. */
-RET_ALL_CONSTRAINTS_ACTIVE,						/**< All constraints are active, no further constraint can be added. */
-RET_LINEARLY_DEPENDENT,							/**< New bound/constraint is linearly dependent. */
-RET_LINEARLY_INDEPENDENT,						/**< New bound/constraint is linearly independent. */
-RET_LI_RESOLVED,								/**< Linear independence of active constraint matrix successfully resolved. */
-RET_ENSURELI_FAILED,							/**< Failed to ensure linear independence of active constraint matrix. */
-RET_ENSURELI_FAILED_TQ,							/**< Abnormal termination due to TQ factorisation. (90) */
-RET_ENSURELI_FAILED_NOINDEX,					/**< QP is infeasible. */
-RET_ENSURELI_FAILED_CYCLING,					/**< QP is infeasible. */
-RET_BOUND_ALREADY_ACTIVE,						/**< Bound is already active. */
-RET_ALL_BOUNDS_ACTIVE,							/**< All bounds are active, no further bound can be added. */
-RET_CONSTRAINT_NOT_ACTIVE,						/**< Constraint is not active. */
-RET_BOUND_NOT_ACTIVE,							/**< Bound is not active. */
-RET_HESSIAN_NOT_SPD,							/**< Projected Hessian matrix not positive definite. */
-RET_HESSIAN_INDEFINITE,							/**< Hessian matrix is indefinite. */
-RET_MATRIX_SHIFT_FAILED,						/**< Unable to update matrices or to transform vectors. */
-RET_MATRIX_FACTORISATION_FAILED,				/**< Unable to calculate new matrix factorisations. (100) */
-RET_PRINT_ITERATION_FAILED,						/**< Unable to print information on current iteration. */
-RET_NO_GLOBAL_MESSAGE_OUTPUTFILE,				/**< No global message output file initialised. */
-RET_DISABLECONSTRAINTS_FAILED,					/**< Unable to disbable constraints. */
-RET_ENABLECONSTRAINTS_FAILED,					/**< Unable to enbable constraints. */
-RET_ALREADY_ENABLED,							/**< Bound or constraint is already enabled. */
-RET_ALREADY_DISABLED,							/**< Bound or constraint is already disabled. */
-RET_NO_HESSIAN_SPECIFIED, 						/**< No Hessian matrix has been specified. */
-RET_USING_REGULARISATION,						/**< Using regularisation as Hessian matrix is not positive definite. */
-RET_EPS_MUST_BE_POSITVE,						/**< Eps for regularisation must be sufficiently positive. */
-RET_REGSTEPS_MUST_BE_POSITVE, 					/**< Maximum number of regularisation steps must be non-negative. (110) */
-RET_HESSIAN_ALREADY_REGULARISED,				/**< Hessian has been already regularised. */
-RET_CANNOT_REGULARISE_IDENTITY,					/**< Identity Hessian matrix cannot be regularised. */
-RET_CANNOT_REGULARISE_SPARSE,					/**< Sparse matrix cannot be regularised as diagonal entry is missing. */
-RET_NO_REGSTEP_NWSR,							/**< No additional regularisation step could be performed due to limits. */
-RET_FEWER_REGSTEPS_NWSR,						/**< Fewer additional regularisation steps have been performed due to limits. */
-RET_CHOLESKY_OF_ZERO_HESSIAN, 					/**< Cholesky decomposition of (unregularised) zero Hessian matrix. */
-RET_CONSTRAINTS_ARE_NOT_SCALED, 				/**< (no longer in use) */
-RET_INITIAL_BOUNDS_STATUS_NYI, 					/**< (no longer in use) */
-RET_ERROR_IN_CONSTRAINTPRODUCT,					/**< Error in user-defined constraint product function. */
-RET_FIX_BOUNDS_FOR_LP,							/**< All initial bounds must be fixed when solving an (unregularised) LP. */
-RET_USE_REGULARISATION_FOR_LP,					/**< Set options.enableRegularisation=BT_TRUE for solving LPs. */
-/* SQProblem */
-RET_UPDATEMATRICES_FAILED,						/**< Unable to update QP matrices. */
-RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED,		/**< Unable to update matrices as previous QP is not solved. */
-/* Utils */
-RET_UNABLE_TO_OPEN_FILE,						/**< Unable to open file. (120) */
-RET_UNABLE_TO_WRITE_FILE,						/**< Unable to write into file. */
-RET_UNABLE_TO_READ_FILE,						/**< Unable to read from file. */
-RET_FILEDATA_INCONSISTENT,						/**< File contains inconsistent data. */
-/* Options */
-RET_OPTIONS_ADJUSTED,							/**< Options needed to be adjusted for consistency reasons. */
-/* SolutionAnalysis */
-RET_UNABLE_TO_ANALYSE_QPROBLEM, 				/**< Unable to analyse (S)QProblem(B) object. */
-/* Benchmark */
-RET_NWSR_SET_TO_ONE,							/**< Maximum number of working set changes was set to 1. */
-RET_UNABLE_TO_READ_BENCHMARK,					/**< Unable to read benchmark data. */
-RET_BENCHMARK_ABORTED,							/**< Benchmark aborted. */
-RET_INITIAL_QP_SOLVED,							/**< Initial QP solved. */
-RET_QP_SOLUTION_STARTED,						/**< Solving QP... */
-RET_BENCHMARK_SUCCESSFUL,						/**< Benchmark terminated successfully. (130) */
-/* Sparse matrices */
-RET_NO_DIAGONAL_AVAILABLE,						/**< Sparse matrix does not have entries on full diagonal. */
-RET_DIAGONAL_NOT_INITIALISED,					/**< Diagonal data of sparse matrix has not been initialised. */
-/* Dropping of infeasible constraints */
-RET_ENSURELI_DROPPED,							/**< Linear independence resolved by dropping blocking constraint. */
-/* Simple exitflags */
-RET_SIMPLE_STATUS_P1,							/**< QP problem could not be solved within given number of iterations. */
-RET_SIMPLE_STATUS_P0,							/**< QP problem solved. */
-RET_SIMPLE_STATUS_M1,							/**< QP problem could not be solved due to an internal error. */
-RET_SIMPLE_STATUS_M2,							/**< QP problem is infeasible (and thus could not be solved). */
-RET_SIMPLE_STATUS_M3 							/**< QP problem is unbounded (and thus could not be solved). */
-};
-
-
-/** 
- *	\brief Handles all kind of error messages, warnings and other information.
- *
- *	This class handles all kinds of messages (errors, warnings, infos) initiated
- *  by qpOASES modules and stores the corresponding global preferences.
- *
- *	\author Hans Joachim Ferreau (thanks to Leonard Wirsching)
- *	\version 3.0
- *	\date 2007-2014
- */
-class MessageHandling
-{
-	/*
-	 *	INTERNAL DATA STRUCTURES
-	 */
-	public:
-		/**
-		*	\brief Data structure for entries in global message list.
-		*	
-		*	Data structure for entries in global message list.
-		*
-		*	\author Hans Joachim Ferreau
-		*/
-		typedef struct {
-			returnValue key;							/**< Global return value. */
-			const char* data;							/**< Corresponding message. */
-			VisibilityStatus globalVisibilityStatus; 	/**< Determines if message can be printed.
-														* 	 If this value is set to VS_HIDDEN, no message is printed! */
-		} ReturnValueList;
-
-
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		MessageHandling( );
-
-		/** Constructor which takes the desired output file. */
-		MessageHandling(  FILE* _outputFile						/**< Output file. */
-						  );
-
-		/** Constructor which takes the desired visibility states. */
-		MessageHandling(	VisibilityStatus _errorVisibility,	/**< Visibility status for error messages. */
-							VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */
-							VisibilityStatus _infoVisibility	/**< Visibility status for info messages. */
-							);
-
-		/** Constructor which takes the desired output file and desired visibility states. */
-		MessageHandling(	FILE* _outputFile,					/**< Output file. */
-							VisibilityStatus _errorVisibility,	/**< Visibility status for error messages. */
-							VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */
-							VisibilityStatus _infoVisibility	/**< Visibility status for info messages. */
-							);
-
-		/** Copy constructor (deep copy). */
-		MessageHandling(	const MessageHandling& rhs	/**< Rhs object. */
-							);
-
-		/** Destructor. */
-		~MessageHandling( );
-
-		/** Assignment operator (deep copy). */
-		MessageHandling& operator=(	const MessageHandling& rhs	/**< Rhs object. */
-									);
-
-
-		/** Prints an error message(a simplified macro THROWERROR is also provided). \n
-		 *  Errors are definied as abnormal events which cause an immediate termination of the current (sub) function.
-		 *  Errors of a sub function should be commented by the calling function by means of a warning message
-		 *  (if this error does not cause an error of the calling function, either)!
-		 *  \return Error number returned by sub function call
-		 */
-		returnValue throwError(	returnValue Enumber,					/**< Error number returned by sub function call. */
-								const char* additionaltext,				/**< Additional error text (0, if none). */
-								const char* functionname,				/**< Name of function which caused the error. */
-								const char* filename,					/**< Name of file which caused the error. */
-								const unsigned long linenumber,			/**< Number of line which caused the error.incompatible binary file */
-								VisibilityStatus localVisibilityStatus	/**< Determines (locally) if error message can be printed to stdFile.
-																	*   If GLOBAL visibility status of the message is set to VS_HIDDEN,
-																	*   no message is printed, anyway! */
-								);
-
-		/** Prints a warning message (a simplified macro THROWWARNING is also provided).
-		 *  Warnings are definied as abnormal events which does NOT cause an immediate termination of the current (sub) function.
-		 *  \return Warning number returned by sub function call
-		 */
-		returnValue throwWarning(	returnValue Wnumber,	 				/**< Warning number returned by sub function call. */
-									const char* additionaltext,				/**< Additional warning text (0, if none). */
-									const char* functionname,				/**< Name of function which caused the warning. */
-									const char* filename,   				/**< Name of file which caused the warning. */
-									const unsigned long linenumber,	 		/**< Number of line which caused the warning. */
-									VisibilityStatus localVisibilityStatus	/**< Determines (locally) if warning message can be printed to stdFile.
-																			*   If GLOBAL visibility status of the message is set to VS_HIDDEN,
-																			*   no message is printed, anyway! */
-									);
-
-	   /** Prints a info message (a simplified macro THROWINFO is also provided).
-		 *  \return Info number returned by sub function call
-		 */
-		returnValue throwInfo(	returnValue Inumber,	 				/**< Info number returned by sub function call. */
-								const char* additionaltext,	 			/**< Additional warning text (0, if none). */
-								const char* functionname,				/**< Name of function which submitted the info. */
-								const char* filename,   				/**< Name of file which submitted the info. */
-								const unsigned long linenumber,			/**< Number of line which submitted the info. */
-								VisibilityStatus localVisibilityStatus	/**< Determines (locally) if info message can be printed to stdFile.
-																		*   If GLOBAL visibility status of the message is set to VS_HIDDEN,
-																		*   no message is printed, anyway! */
-								);
-
-
-		/** Resets all preferences to default values.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue reset( );
-
-
-		/** Prints a complete list of all messages to output file.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue listAllMessages( );
-
-
-		/** Returns visibility status for error messages.
-		 *	\return Visibility status for error messages. */
-		inline VisibilityStatus getErrorVisibilityStatus( ) const;
-
-		/** Returns visibility status for warning messages.
-		 *	\return Visibility status for warning messages. */
-		inline VisibilityStatus getWarningVisibilityStatus( ) const;
-
-		/** Returns visibility status for info messages.
-		 *	\return Visibility status for info messages. */
-		inline VisibilityStatus getInfoVisibilityStatus( ) const;
-
-		/** Returns pointer to output file.
-		 *	\return Pointer to output file. */
-		inline FILE* getOutputFile( ) const;
-
-		/** Returns error count value.
-		 *	\return Error count value. */
-		inline int getErrorCount( ) const;
-
-
-		/** Changes visibility status for error messages. */
-		inline void setErrorVisibilityStatus(	VisibilityStatus _errorVisibility	/**< New visibility status for error messages. */
-												);
-
-		/** Changes visibility status for warning messages. */
-		inline void setWarningVisibilityStatus(	VisibilityStatus _warningVisibility	/**< New visibility status for warning messages. */
-												);
-
-		/** Changes visibility status for info messages. */
-		inline void setInfoVisibilityStatus(	VisibilityStatus _infoVisibility	/**< New visibility status for info messages. */
-												);
-
-		/** Changes output file for messages. */
-		inline void setOutputFile(	FILE* _outputFile	/**< New output file for messages. */
-									);
-
-		/** Changes error count.
-		 * \return SUCCESSFUL_RETURN \n
-		 *		   RET_INVALID_ARGUMENT */
-		inline returnValue setErrorCount(	int _errorCount	/**< New error count value. */
-											);
-
-		/** Provides message text corresponding to given \a returnValue.
-		 * \return String containing message text. */
-		static const char* getErrorCodeMessage(	const returnValue _returnValue
-												);
-
-
-	/*
-	 *	PROTECTED MEMBER FUNCTIONS
-	 */
-	protected:
-		/** Prints a info message to stdFile (auxiliary function).
-		 *  \return Error/warning/info number returned by sub function call
-		 */
-		returnValue throwMessage(
-			returnValue RETnumber,	 				/**< Error/warning/info number returned by sub function call. */
-			const char* additionaltext,				/**< Additional warning text (0, if none). */
-			const char* functionname,				/**< Name of function which caused the error/warning/info. */
-			const char* filename,   				/**< Name of file which caused the error/warning/info. */
-			const unsigned long linenumber,			/**< Number of line which caused the error/warning/info. */
-			VisibilityStatus localVisibilityStatus,	/**< Determines (locally) if info message can be printed to stdFile.
-					  								 *   If GLOBAL visibility status of the message is set to VS_HIDDEN,
-				   									 *   no message is printed, anyway! */
-			const char* RETstring					/**< Leading string of error/warning/info message. */
-			);
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-		VisibilityStatus errorVisibility;		/**< Error messages visible? */
-		VisibilityStatus warningVisibility;		/**< Warning messages visible? */
-		VisibilityStatus infoVisibility;		/**< Info messages visible? */
-
-		FILE* outputFile;						/**< Output file for messages. */
-
-		int errorCount; 						/**< Counts number of errors (for nicer output only). */
-};
-
-
-#ifndef __FUNCTION__
-  /** Ensures that __FUNCTION__ macro is defined. */
-  #define __FUNCTION__ 0
-#endif
-
-#ifndef __FILE__
-  /** Ensures that __FILE__ macro is defined. */
-  #define __FILE__ 0
-#endif
-
-#ifndef __LINE__
-  /** Ensures that __LINE__ macro is defined. */
-  #define __LINE__ 0
-#endif
-
-
-/** Short version of throwError with default values, only returnValue is needed */
-#define THROWERROR(retval) ( getGlobalMessageHandler( )->throwError((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
-
-/** Short version of throwWarning with default values, only returnValue is needed */
-#define THROWWARNING(retval) ( getGlobalMessageHandler( )->throwWarning((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
-
-/** Short version of throwInfo with default values, only returnValue is needed */
-#define THROWINFO(retval) ( getGlobalMessageHandler( )->throwInfo((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
-
-
-/** Returns a pointer to global message handler.
- *  \return Pointer to global message handler.
- */
-MessageHandling* getGlobalMessageHandler( );
-
-
-END_NAMESPACE_QPOASES
-
-#include <qpOASES/MessageHandling.ipp>
-
-#endif /* QPOASES_MESSAGEHANDLING_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/MessageHandling.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/MessageHandling.ipp
deleted file mode 100644
index 69b17651cb7..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/MessageHandling.ipp
+++ /dev/null
@@ -1,144 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/MessageHandling.ipp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of inlined member functions of the MessageHandling class.
- */
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	g e t E r r o r V i s i b i l i t y S t a t u s
- */
-inline VisibilityStatus MessageHandling::getErrorVisibilityStatus( ) const
-{
- 	return errorVisibility;
-}
-
-
-/*
- *	g e t W a r n i n g V i s i b i l i t y S t a t u s
- */
-inline VisibilityStatus MessageHandling::getWarningVisibilityStatus( ) const
-{
- 	return warningVisibility;
-}
-
-
-/*
- *	g e t I n f o V i s i b i l i t y S t a t u s
- */
-inline VisibilityStatus MessageHandling::getInfoVisibilityStatus( ) const
-{
- 	return infoVisibility;
-}
-
-
-/*
- *	g e t O u t p u t F i l e
- */
-inline FILE* MessageHandling::getOutputFile( ) const
-{
- 	return outputFile;
-}
-
-
-/*
- *	g e t E r r o r C o u n t
- */
-inline int MessageHandling::getErrorCount( ) const
-{
- 	return errorCount;
-}
-
-
-/*
- *	s e t E r r o r V i s i b i l i t y S t a t u s
- */
-inline void MessageHandling::setErrorVisibilityStatus( VisibilityStatus _errorVisibility )
-{
- 	errorVisibility = _errorVisibility;
-}
-
-
-/*
- *	s e t W a r n i n g V i s i b i l i t y S t a t u s
- */
-inline void MessageHandling::setWarningVisibilityStatus( VisibilityStatus _warningVisibility )
-{
- 	warningVisibility = _warningVisibility;
-}
-
-
-/*
- *	s e t I n f o V i s i b i l i t y S t a t u s
- */
-inline void MessageHandling::setInfoVisibilityStatus( VisibilityStatus _infoVisibility )
-{
- 	infoVisibility = _infoVisibility;
-}
-
-
-/*
- *	s e t O u t p u t F i l e
- */
-inline void MessageHandling::setOutputFile( FILE* _outputFile )
-{
- 	outputFile = _outputFile;
-}
-
-
-/*
- *	s e t E r r o r C o u n t
- */
-inline returnValue MessageHandling::setErrorCount( int _errorCount )
-{
-	if ( _errorCount >= -1 )
-	{
-		errorCount = _errorCount;
-		return SUCCESSFUL_RETURN;
-	}
-	else
-		return RET_INVALID_ARGUMENTS;
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Options.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Options.hpp
deleted file mode 100644
index 83c520cadd0..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Options.hpp
+++ /dev/null
@@ -1,170 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/Options.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Declaration of the Options class designed to manage user-specified
- *	options for solving a QProblem.
- */
-
-
-#ifndef QPOASES_OPTIONS_HPP
-#define QPOASES_OPTIONS_HPP
-
-
-#include <qpOASES/Utils.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/**
- *	\brief Manages all user-specified options for solving QPs.
- *
- *	This class manages all user-specified options used for solving
- *	quadratic programs.
- *
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- */
-class Options
-{
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		Options( );
-
-		/** Copy constructor (deep copy). */
-		Options(	const Options& rhs			/**< Rhs object. */
-					);
-
-		/** Destructor. */
-		~Options( );
-
-		/** Assignment operator (deep copy). */
-		Options& operator=(	const Options& rhs	/**< Rhs object. */
-							);
-
-
-		/** Sets all options to default values.
- 		 *	\return SUCCESSFUL_RETURN */
-		returnValue setToDefault( );
-
-		/** Sets all options to values resulting in maximum reliabilty.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue setToReliable( );
-
-		/** Sets all options to values resulting in minimum solution time.
- 		 *	\return SUCCESSFUL_RETURN */
-		returnValue setToMPC( );
-
-		/** Same as setToMPC( ), for ensuring backwards compatibility.
- 		 *	\return SUCCESSFUL_RETURN */
-		returnValue setToFast( );
-
-
-		/** Ensures that all options have consistent values by automatically
-		 *	adjusting inconsistent ones.
-		 *	Note: This routine cannot (and does not try to) ensure that values
-		 *	      are set to reasonable values that make the QP solution work!
-		 *	\return SUCCESSFUL_RETURN \n 
-		 *          RET_OPTIONS_ADJUSTED */
-		returnValue ensureConsistency( );
-
-
-		/** Prints values of all options.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue print( ) const;
-
-
-	/*
-	 *	PROTECTED MEMBER FUNCTIONS
-	 */
-	protected:
-
-		/** Copies all members from given rhs object.
-		 *  \return SUCCESSFUL_RETURN */
-		returnValue copy(	const Options& rhs	/**< Rhs object. */
-							);
-
-	/*
-	 *	PUBLIC MEMBER VARIABLES
-	 */
-	public:
-		PrintLevel printLevel;					/**< Print level. */
-
-		BooleanType enableRamping;				/**< Specifies whether ramping shall be enabled or not. */
-		BooleanType enableFarBounds;			/**< Specifies whether far bounds shall be used or not. */
-		BooleanType enableFlippingBounds;		/**< Specifies whether flipping bounds shall be used or not. */
-		BooleanType enableRegularisation;		/**< Specifies whether Hessian matrix shall be regularised in case semi-definiteness is detected. */
-		BooleanType enableFullLITests;			/**< Specifies whether condition-hardened LI test shall be used or not. */
-		BooleanType enableNZCTests;				/**< Specifies whether nonzero curvature tests shall be used. */
-		int         enableDriftCorrection;		/**< Specifies the frequency of drift corrections (0 = off). */
-		int enableCholeskyRefactorisation;		/**< Specifies the frequency of full refactorisation of proj. Hessian (otherwise updates). */
-		BooleanType enableEqualities;			/**< Specifies whether equalities shall be always treated as active constraints. */
-
-		real_t terminationTolerance;			/**< Termination tolerance. */
-		real_t boundTolerance;					/**< Lower/upper (constraints') bound tolerance (an inequality constraint whose lower and upper bounds differ by less is regarded to be an equality constraint). */
-		real_t boundRelaxation;					/**< Offset for relaxing (constraints') bounds at beginning of an initial homotopy. It is also as initial value for far bounds. */
-		real_t epsNum;							/**< Numerator tolerance for ratio tests. */
-		real_t epsDen;							/**< Denominator tolerance for ratio tests. */
-		real_t maxPrimalJump;					/**< Maximum allowed jump in primal variables in nonzero curvature tests. */
-		real_t maxDualJump;						/**< Maximum allowed jump in dual variables in linear independence tests. */
-
-		real_t initialRamping;					/**< Start value for Ramping Strategy. */
-		real_t finalRamping;					/**< Final value for Ramping Strategy. */
-		real_t initialFarBounds;				/**< Initial size of Far Bounds. */
-		real_t growFarBounds;					/**< Factor to grow Far Bounds. */
-		SubjectToStatus initialStatusBounds;	/**< Initial status of bounds at first iteration. */
-		real_t epsFlipping;						/**< Tolerance of squared Cholesky diagonal factor which triggers flipping bound. */
-		int numRegularisationSteps;				/**< Maximum number of successive regularisation steps. */
-		real_t epsRegularisation;				/**< Scaling factor of identity matrix used for Hessian regularisation. */
-		int numRefinementSteps;					/**< Maximum number of iterative refinement steps. */
-		real_t epsIterRef;						/**< Early termination tolerance for iterative refinement. */
-		real_t epsLITests;						/**< Tolerance for linear independence tests. */
-		real_t epsNZCTests;						/**< Tolerance for nonzero curvature tests. */
-
-		BooleanType enableDropInfeasibles;		/**< ... */
-		int    dropBoundPriority;				/**< ... */
-        int    dropEqConPriority;				/**< ... */
-        int    dropIneqConPriority;				/**< ... */
-};
-
-
-END_NAMESPACE_QPOASES
-
-
-#endif	/* QPOASES_OPTIONS_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblem.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblem.hpp
deleted file mode 100644
index 6e2232fdcf5..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblem.hpp
+++ /dev/null
@@ -1,1163 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/QProblem.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Declaration of the QProblem class which is able to use the newly
- *	developed online active set strategy for parametric quadratic programming.
- */
-
-
-
-#ifndef QPOASES_QPROBLEM_HPP
-#define QPOASES_QPROBLEM_HPP
-
-
-#include <qpOASES/QProblemB.hpp>
-#include <qpOASES/Constraints.hpp>
-#include <qpOASES/ConstraintProduct.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/**
- *	\brief Implements the online active set strategy for QPs with general constraints.
- *
- *	A class for setting up and solving quadratic programs. The main feature is
- *	the possibily to use the newly developed online active set strategy for
- * 	parametric quadratic programming.
- *
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- */
-class QProblem : public QProblemB
-{
-	/* allow SolutionAnalysis class to access private members */
-	friend class SolutionAnalysis;
-
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		QProblem( );
-
-		/** Constructor which takes the QP dimension and Hessian type
-		 *  information. If the Hessian is the zero (i.e. HST_ZERO) or the
-		 *  identity matrix (i.e. HST_IDENTITY), respectively, no memory
-		 *  is allocated for it and a NULL pointer can be passed for it
-		 *  to the init() functions. */
-		QProblem(	int _nV,	  							/**< Number of variables. */
-					int _nC,		  						/**< Number of constraints. */
-					HessianType _hessianType = HST_UNKNOWN	/**< Type of Hessian matrix. */
-					);
-
-		/** Copy constructor (deep copy). */
-		QProblem(	const QProblem& rhs		/**< Rhs object. */
-					);
-
-		/** Destructor. */
-		virtual ~QProblem( );
-
-		/** Assignment operator (deep copy). */
-		QProblem& operator=(	const QProblem& rhs	/**< Rhs object. */
-								);
-
-
-		/** Clears all data structures of QProblemB except for QP data.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_RESET_FAILED */
-		virtual returnValue reset( );
-
-
-		/** Initialises a QProblem with given QP data and solves it
-		 *	using an initial homotopy with empty working set (at most nWSR iterations).
-		 *  Note: This function internally calls solveInitialQP for initialisation!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_TQ \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED \n
-					RET_INVALID_ARGUMENTS */
-		returnValue init(	SymmetricMatrix *_H,		/**< Hessian matrix (a shallow copy is made). */
-							const real_t* const _g, 	/**< Gradient vector. */
-							Matrix *_A,  				/**< Constraint matrix (a shallow copy is made). */
-							const real_t* const _lb,	/**< Lower bound vector (on variables). \n
-															 If no lower bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ub,	/**< Upper bound vector (on variables). \n
-															 If no upper bounds exist, a NULL pointer can be passed. */
-							const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
-															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ubA,	/**< Upper constraints' bound vector. \n
-															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							int& nWSR,					/**< Input: Maximum number of working set recalculations when using initial homotopy.
-															 Output: Number of performed working set recalculations. */
-							real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP initialisation. \n
-															 Output: CPU time spend for QP initialisation (if pointer passed). */
-							);
-
-
-		/** Initialises a QProblem with given QP data and solves it
-		 *	using an initial homotopy with empty working set (at most nWSR iterations).
-		 *  Note: This function internally calls solveInitialQP for initialisation!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_TQ \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED \n
-					RET_INVALID_ARGUMENTS */
-		returnValue init(	const real_t* const _H, 	/**< Hessian matrix (a shallow copy is made). \n
-															 If Hessian matrix is trivial, a NULL pointer can be passed. */
-							const real_t* const _g, 	/**< Gradient vector. */
-							const real_t* const _A,  	/**< Constraint matrix (a shallow copy is made). */
-							const real_t* const _lb,	/**< Lower bound vector (on variables). \n
-															 If no lower bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ub,	/**< Upper bound vector (on variables). \n
-															 If no upper bounds exist, a NULL pointer can be passed. */
-							const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
-															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ubA,	/**< Upper constraints' bound vector. \n
-															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							int& nWSR,					/**< Input: Maximum number of working set recalculations when using initial homotopy.
-															 Output: Number of performed working set recalculations. */
-							real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP initialisation. \n
-															 Output: CPU time spend for QP initialisation (if pointer passed). */
-							);
-
-		/** Initialises a QProblem with given QP data to be read from files and solves it
-		 *	using an initial homotopy with empty working set (at most nWSR iterations).
-		 *  Note: This function internally calls solveInitialQP for initialisation!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_TQ \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED \n
-					RET_UNABLE_TO_READ_FILE */
-		returnValue init(	const char* const H_file,	/**< Name of file where Hessian matrix is stored. \n
-															 If Hessian matrix is trivial, a NULL pointer can be passed. */
-							const char* const g_file,	/**< Name of file where gradient vector is stored. */
-							const char* const A_file,	/**< Name of file where constraint matrix is stored. */
-							const char* const lb_file,	/**< Name of file where lower bound vector. \n
-															If no lower bounds exist, a NULL pointer can be passed. */
-							const char* const ub_file,	/**< Name of file where upper bound vector. \n
-															If no upper bounds exist, a NULL pointer can be passed. */
-							const char* const lbA_file,	/**< Name of file where lower constraints' bound vector. \n
-															If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							const char* const ubA_file,	/**< Name of file where upper constraints' bound vector. \n
-															 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-							int& nWSR,					/**< Input: Maximum number of working set recalculations when using initial homotopy.
-															 Output: Number of performed working set recalculations. */
-							real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP initialisation. \n
-															 Output: CPU time spend for QP initialisation (if pointer passed). */
-							);
-
-		/** Initialises a QProblem with given QP data and solves it
-		 *  depending on the parameter constellation: \n
-		 *	1. 0,    0,    0 : start with xOpt = 0, yOpt = 0 and IB empty (or all implicit equality bounds), \n
-		 *	2. xOpt, 0,    0 : start with xOpt, yOpt = 0 and obtain IB by "clipping", \n
-		 *	3. 0,    yOpt, 0 : start with xOpt = 0, yOpt and obtain IB from yOpt != 0, \n
-		 *	4. 0,    0,    IB: start with xOpt = 0, yOpt = 0 and IB, \n
-		 *	5. xOpt, yOpt, 0 : start with xOpt, yOpt and obtain IB from yOpt != 0, \n
-		 *	6. xOpt, 0,    IB: start with xOpt, yOpt = 0 and IB, \n
-		 *	7. xOpt, yOpt, IB: start with xOpt, yOpt and IB (assume them to be consistent!)
-		 *  Note: This function internally calls solveInitialQP for initialisation!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_TQ \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED \n
-					RET_INVALID_ARGUMENTS */
-		returnValue init(	SymmetricMatrix *_H, 						/**< Hessian matrix (a shallow copy is made). */
-							const real_t* const _g, 					/**< Gradient vector. */
-							Matrix *_A, 			 					/**< Constraint matrix (a shallow copy is made). */
-							const real_t* const _lb,					/**< Lower bound vector (on variables). \n
-																			 If no lower bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ub,					/**< Upper bound vector (on variables). \n
-																			 If no upper bounds exist, a NULL pointer can be passed. */
-							const real_t* const _lbA,					/**< Lower constraints' bound vector. \n
-																			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ubA,					/**< Upper constraints' bound vector. \n
-																		 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							int& nWSR,									/**< Input: Maximum number of working set recalculations when using initial homotopy.
-																		 *	 Output: Number of performed working set recalculations. */
-							real_t* const cputime,						/**< Input: Maximum CPU time allowed for QP initialisation. \n
-																			 Output: CPU time spend for QP initialisation. */
-							const real_t* const xOpt,					/**< Optimal primal solution vector. \n
-																			 (If a null pointer is passed, the old primal solution is kept!) */
-							const real_t* const yOpt,					/**< Optimal dual solution vector. \n
-																			 (If a null pointer is passed, the old dual solution is kept!) */
-							const Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
-							const Constraints* const guessedConstraints	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
-							);
-
-		/** Initialises a QProblem with given QP data and solves it
-		 *  depending on the parameter constellation: \n
-		 *	1. 0,    0,    0 : start with xOpt = 0, yOpt = 0 and IB empty (or all implicit equality bounds), \n
-		 *	2. xOpt, 0,    0 : start with xOpt, yOpt = 0 and obtain IB by "clipping", \n
-		 *	3. 0,    yOpt, 0 : start with xOpt = 0, yOpt and obtain IB from yOpt != 0, \n
-		 *	4. 0,    0,    IB: start with xOpt = 0, yOpt = 0 and IB, \n
-		 *	5. xOpt, yOpt, 0 : start with xOpt, yOpt and obtain IB from yOpt != 0, \n
-		 *	6. xOpt, 0,    IB: start with xOpt, yOpt = 0 and IB, \n
-		 *	7. xOpt, yOpt, IB: start with xOpt, yOpt and IB (assume them to be consistent!)
-		 *  Note: This function internally calls solveInitialQP for initialisation!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_TQ \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED \n
-					RET_INVALID_ARGUMENTS */
-		returnValue init(	const real_t* const _H, 					/**< Hessian matrix (a shallow copy is made). \n
-																     		 If Hessian matrix is trivial, a NULL pointer can be passed. */
-							const real_t* const _g, 					/**< Gradient vector. */
-							const real_t* const _A,  					/**< Constraint matrix (a shallow copy is made). */
-							const real_t* const _lb,					/**< Lower bound vector (on variables). \n
-																			 If no lower bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ub,					/**< Upper bound vector (on variables). \n
-																			 If no upper bounds exist, a NULL pointer can be passed. */
-							const real_t* const _lbA,					/**< Lower constraints' bound vector. \n
-																			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ubA,					/**< Upper constraints' bound vector. \n
-																		 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							int& nWSR,									/**< Input: Maximum number of working set recalculations when using initial homotopy.
-																		 *	 Output: Number of performed working set recalculations. */
-							real_t* const cputime,						/**< Input: Maximum CPU time allowed for QP initialisation. \n
-																			 Output: CPU time spend for QP initialisation. */
-							const real_t* const xOpt,					/**< Optimal primal solution vector. \n
-																			 (If a null pointer is passed, the old primal solution is kept!) */
-							const real_t* const yOpt,					/**< Optimal dual solution vector. \n
-																			 (If a null pointer is passed, the old dual solution is kept!) */
-							const Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
-							const Constraints* const guessedConstraints	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
-							);
-
-		/** Initialises a QProblem with given QP data to be read from files and solves it
-		 *  depending on the parameter constellation: \n
-		 *  Note: This function internally calls solveInitialQP for initialisation!
-		 *	1. 0,    0,    0 : start with xOpt = 0, yOpt = 0 and IB empty (or all implicit equality bounds), \n
-		 *	2. xOpt, 0,    0 : start with xOpt, yOpt = 0 and obtain IB by "clipping", \n
-		 *	3. 0,    yOpt, 0 : start with xOpt = 0, yOpt and obtain IB from yOpt != 0, \n
-		 *	4. 0,    0,    IB: start with xOpt = 0, yOpt = 0 and IB, \n
-		 *	5. xOpt, yOpt, 0 : start with xOpt, yOpt and obtain IB from yOpt != 0, \n
-		 *	6. xOpt, 0,    IB: start with xOpt, yOpt = 0 and IB, \n
-		 *	7. xOpt, yOpt, IB: start with xOpt, yOpt and IB (assume them to be consistent!)
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_TQ \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED \n
-					RET_UNABLE_TO_READ_FILE */
-		returnValue init(	const char* const H_file,					/**< Name of file where Hessian matrix is stored. \n
-																     		 If Hessian matrix is trivial, a NULL pointer can be passed. */
-							const char* const g_file,					/**< Name of file where gradient vector is stored. */
-							const char* const A_file,					/**< Name of file where constraint matrix is stored. */
-							const char* const lb_file,					/**< Name of file where lower bound vector. \n
-																			 If no lower bounds exist, a NULL pointer can be passed. */
-							const char* const ub_file,					/**< Name of file where upper bound vector. \n
-																			 If no upper bounds exist, a NULL pointer can be passed. */
-							const char* const lbA_file,					/**< Name of file where lower constraints' bound vector. \n
-																			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							const char* const ubA_file,					/**< Name of file where upper constraints' bound vector. \n
-																			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-							int& nWSR,									/**< Input: Maximum number of working set recalculations when using initial homotopy.
-																			 Output: Number of performed working set recalculations. */
-							real_t* const cputime,						/**< Input: Maximum CPU time allowed for QP initialisation. \n
-																			 Output: CPU time spend for QP initialisation. */
-							const real_t* const xOpt,					/**< Optimal primal solution vector. \n
-																			 (If a null pointer is passed, the old primal solution is kept!) */
-							const real_t* const yOpt,					/**< Optimal dual solution vector. \n
-																			 (If a null pointer is passed, the old dual solution is kept!) */
-							const Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
-							const Constraints* const guessedConstraints	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
-							);
-
-
-		/** Solves QProblem using online active set strategy.
-		 *  Note: This function internally calls solveQP/solveRegularisedQP
-		 *        for solving an initialised QP!
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MAX_NWSR_REACHED \n
-		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
-		returnValue hotstart(	const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
-								const real_t* const lb_new,		/**< Lower bounds of neighbouring QP to be solved. \n
-													 			 	 If no lower bounds exist, a NULL pointer can be passed. */
-								const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
-													 			 	 If no upper bounds exist, a NULL pointer can be passed. */
-								const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
-													 			 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
-													 			 	 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int& nWSR,						/**< Input: Maximum number of working set recalculations; \n
-															 		 Output: Number of performed working set recalculations. */
-								real_t* const cputime = 0		/**< Input: Maximum CPU time allowed for QP solution. \n
-																 	 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								);
-
-		/** Solves QProblem using online active set strategy
-		 *  reading QP data from files.
-		 *  Note: This function internally calls solveQP/solveRegularisedQP
-		 *        for solving an initialised QP!
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MAX_NWSR_REACHED \n
-		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-					RET_UNABLE_TO_READ_FILE \n
-					RET_INVALID_ARGUMENTS */
-		returnValue hotstart(	const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-								const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-													 			 If no lower bounds exist, a NULL pointer can be passed. */
-								const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-													 			 If no upper bounds exist, a NULL pointer can be passed. */
-								const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
-													 			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
-													 			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int& nWSR, 					/**< Input: Maximum number of working set recalculations; \n
-																 Output: Number of performed working set recalculations. */
-								real_t* const cputime = 0		/**< Input: Maximum CPU time allowed for QP solution. \n
-																 	 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								);
-
-		/** Solves an initialised QProblem using online active set strategy (using an initialised homotopy).
-		 *  Note: This function internally calls solveQP/solveRegularisedQP
-		 *        for solving an initialised QP!
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MAX_NWSR_REACHED \n
-		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-					RET_SETUP_AUXILIARYQP_FAILED */
-		returnValue hotstart(	const real_t* const g_new,					/**< Gradient of neighbouring QP to be solved. */
-								const real_t* const lb_new,					/**< Lower bounds of neighbouring QP to be solved. \n
-													 						 	 If no lower bounds exist, a NULL pointer can be passed. */
-								const real_t* const ub_new,					/**< Upper bounds of neighbouring QP to be solved. \n
-													 						 	 If no upper bounds exist, a NULL pointer can be passed. */
-								const real_t* const lbA_new,				/**< Lower constraints' bounds of neighbouring QP to be solved. \n
-													 						 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const real_t* const ubA_new,				/**< Upper constraints' bounds of neighbouring QP to be solved. \n
-													 							 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int& nWSR,									/**< Input: Maximum number of working set recalculations; \n
-																				 Output: Number of performed working set recalculations. */
-								real_t* const cputime,						/**< Input: Maximum CPU time allowed for QP solution. \n
-																			 	 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								const Bounds* const guessedBounds,			/**< Initial guess for working set of bounds.
-																			 *   A null pointer corresponds to an empty working set! */
-								const Constraints* const guessedConstraints	/**< Initial guess for working set of constraints.
-																			 *   A null pointer corresponds to an empty working set! */
-								);
-
-		/** Solves an initialised QProblem using online active set strategy (using an initialised homotopy)
-		 *  reading QP data from files.
-		 *  Note: This function internally calls solveQP/solveRegularisedQP
-		 *        for solving an initialised QP!
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MAX_NWSR_REACHED \n
-		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-					RET_SETUP_AUXILIARYQP_FAILED \n
-					RET_UNABLE_TO_READ_FILE \n
-					RET_INVALID_ARGUMENTS */
-		returnValue hotstart(	const char* const g_file, 					/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-								const char* const lb_file, 					/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-													 							 If no lower bounds exist, a NULL pointer can be passed. */
-								const char* const ub_file, 					/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-													 							 If no upper bounds exist, a NULL pointer can be passed. */
-								const char* const lbA_file, 				/**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
-													 							 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const char* const ubA_file, 				/**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
-													 							 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int& nWSR,									/**< Input: Maximum number of working set recalculations; \n
-																		 		 Output: Number of performed working set recalculations. */
-								real_t* const cputime,						/**< Input: Maximum CPU time allowed for QP solution. \n
-																			 	 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								const Bounds* const guessedBounds,			/**< Initial guess for working set of bounds.
-																			 *   A null pointer corresponds to an empty working set! */
-								const Constraints* const guessedConstraints	/**< Initial guess for working set of constraints.
-																			 *   A null pointer corresponds to an empty working set! */
-								);
-
-        /** Solves using the current working set
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_STEPDIRECTION_FAILED_TQ \n
-		 *			RET_STEPDIRECTION_FAILED_CHOLESKY */
-        returnValue solveCurrentEQP (	const int n_rhs,			/**< Number of consecutive right hand sides */
-										const real_t* g_in,			/**< Gradient of neighbouring QP to be solved. */
-										const real_t* lb_in,		/**< Lower bounds of neighbouring QP to be solved. \n
-																		 If no lower bounds exist, a NULL pointer can be passed. */
-										const real_t* ub_in,		/**< Upper bounds of neighbouring QP to be solved. \n
-																		 If no upper bounds exist, a NULL pointer can be passed. */
-										const real_t* lbA_in,		/**< Lower constraints' bounds of neighbouring QP to be solved. \n
-																		 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-										const real_t* ubA_in,		/**< Upper constraints' bounds of neighbouring QP to be solved. \n */
-										real_t* x_out,				/**< Primal solution */
-										real_t* y_out				/**< Dual solution */
-										);
-
-		/** Writes a vector with the state of the (constraints') bounds
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue getWorkingSet(	real_t* workingSet				/** Output: array containing state of the working set. */
-									);
-
-		/** Returns current constraints object of the QP (deep copy).
-		  *	\return SUCCESSFUL_RETURN \n
-		  			RET_QPOBJECT_NOT_SETUP */
-		inline returnValue getConstraints(	Constraints& _constraints	/** Output: Constraints object. */
-											) const;
-
-
-		/** Returns the number of constraints.
-		 *	\return Number of constraints. */
-		inline int getNC( ) const;
-
-		/** Returns the number of (implicitly defined) equality constraints.
-		 *	\return Number of (implicitly defined) equality constraints. */
-		inline int getNEC( ) const;
-
-		/** Returns the number of active constraints.
-		 *	\return Number of active constraints. */
-		inline int getNAC( ) const;
-
-		/** Returns the number of inactive constraints.
-		 *	\return Number of inactive constraints. */
-		inline int getNIAC( ) const;
-
-		/** Returns the dimension of null space.
-		 *	\return Dimension of null space. */
-		virtual int getNZ( ) const;
-
-
-		/** Returns the dual solution vector (deep copy).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_QP_NOT_SOLVED */
-		virtual returnValue getDualSolution(	real_t* const yOpt	/**< Output: Dual solution vector (if QP has been solved). */
-												) const;
-
-
-		/** Defines user-defined routine for calculating the constraint product A*x
-		 *	\return  SUCCESSFUL_RETURN \n */
-		returnValue setConstraintProduct(	ConstraintProduct* const _constraintProduct
-											);
-
-
-		/** Prints concise list of properties of the current QP.
-		 *	\return  SUCCESSFUL_RETURN \n */
-		virtual returnValue printProperties( );
-
-
-	/*
-	 *	PROTECTED MEMBER FUNCTIONS
-	 */
-	protected:
-		/** Frees all allocated memory.
-		 *  \return SUCCESSFUL_RETURN */
-		returnValue clear( );
-
-		/** Copies all members from given rhs object.
-		 *  \return SUCCESSFUL_RETURN */
-		returnValue copy(	const QProblem& rhs	/**< Rhs object. */
-							);
-
-		/** Solves a QProblem whose QP data is assumed to be stored in the member variables.
-		 *  A guess for its primal/dual optimal solution vectors and the corresponding
-		 *  working sets of bounds and constraints can be provided.
-		 *  Note: This function is internally called by all init functions!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_TQ \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED */
-		returnValue solveInitialQP(	const real_t* const xOpt,						/**< Optimal primal solution vector.
-																					 *	 A NULL pointer can be passed. */
-									const real_t* const yOpt,						/**< Optimal dual solution vector.
-																					 *	 A NULL pointer can be passed. */
-									const Bounds* const guessedBounds,				/**< Guessed working set of bounds for solution (xOpt,yOpt).
-																		 			 *	 A NULL pointer can be passed. */
-									const Constraints* const guessedConstraints,	/**< Optimal working set of constraints for solution (xOpt,yOpt).
-																		 			 *	 A NULL pointer can be passed. */
-									int& nWSR, 										/**< Input: Maximum number of working set recalculations; \n
-																 					 *	 Output: Number of performed working set recalculations. */
-									real_t* const cputime							/**< Input: Maximum CPU time allowed for QP solution. \n
-																			 		 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-									);
-
-		/** Solves QProblem using online active set strategy.
-		 *  Note: This function is internally called by all hotstart functions!
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MAX_NWSR_REACHED \n
-		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
-		returnValue solveQP(	const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
-								const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
-													 			 	 If no lower bounds exist, a NULL pointer can be passed. */
-								const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
-													 			 	 If no upper bounds exist, a NULL pointer can be passed. */
-								const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
-													 			 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
-													 			 	 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int& nWSR,						/**< Input: Maximum number of working set recalculations; \n
-															 		 Output: Number of performed working set recalculations. */
-								real_t* const cputime,			/**< Input: Maximum CPU time allowed for QP solution. \n
-																 	 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								int  nWSRperformed = 0			/**< Number of working set recalculations already performed to solve
-																	 this QP within previous solveQP() calls. This number is
-																	 always zero, except for successive calls from solveRegularisedQP()
-																	 or when using the far bound strategy. */
-								);
-
-
-		/** Solves QProblem using online active set strategy.
-		 *  Note: This function is internally called by all hotstart functions!
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MAX_NWSR_REACHED \n
-		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
-		returnValue solveRegularisedQP(	const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
-										const real_t* const lb_new,		/**< Lower bounds of neighbouring QP to be solved. \n
-															 			 	 If no lower bounds exist, a NULL pointer can be passed. */
-										const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
-															 			 	 If no upper bounds exist, a NULL pointer can be passed. */
-										const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
-															 			 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-										const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
-															 			 	 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-										int& nWSR,						/**< Input: Maximum number of working set recalculations; \n
-																	 		 Output: Number of performed working set recalculations. */
-										real_t* const cputime,			/**< Input: Maximum CPU time allowed for QP solution. \n
-																		 	 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-										int  nWSRperformed = 0			/**< Number of working set recalculations already performed to solve
-																			 this QP within previous solveRegularisedQP() calls. This number is
-																			 always zero, except for successive calls when using the far bound strategy. */
-										);
-
-
-		/** Determines type of existing constraints and bounds (i.e. implicitly fixed, unbounded etc.).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_SETUPSUBJECTTOTYPE_FAILED */
-		virtual returnValue setupSubjectToType( );
-
-		/** Determines type of new constraints and bounds (i.e. implicitly fixed, unbounded etc.).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_SETUPSUBJECTTOTYPE_FAILED */
-		virtual returnValue setupSubjectToType(	const real_t* const lb_new,		/**< New lower bounds. */
-												const real_t* const ub_new,		/**< New upper bounds. */
-												const real_t* const lbA_new,	/**< New lower constraints' bounds. */
-												const real_t* const ubA_new		/**< New upper constraints' bounds. */
-												);
-
-		/** Computes the Cholesky decomposition of the projected Hessian (i.e. R^T*R = Z^T*H*Z).
-		 *  Note: If Hessian turns out not to be positive definite, the Hessian type
-		 *		  is set to HST_SEMIDEF accordingly.
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_HESSIAN_NOT_SPD \n
-		 *			RET_INDEXLIST_CORRUPTED */
-		returnValue computeProjectedCholesky( );
-
-		/** Computes initial Cholesky decomposition of the projected Hessian making
-		 *  use of the function computeCholesky() or computeProjectedCholesky().
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_HESSIAN_NOT_SPD \n
-		 *			RET_INDEXLIST_CORRUPTED */
-		virtual returnValue setupInitialCholesky( );
-
-		/** Initialises TQ factorisation of A (i.e. A*Q = [0 T]) if NO constraint is active.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INDEXLIST_CORRUPTED */
-		returnValue setupTQfactorisation( );
-
-
-		/** Obtains the desired working set for the auxiliary initial QP in
-		 *  accordance with the user specifications
-		 *  (assumes that member AX has already been initialised!)
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_OBTAINING_WORKINGSET_FAILED \n
-					RET_INVALID_ARGUMENTS */
-		returnValue obtainAuxiliaryWorkingSet(	const real_t* const xOpt,						/**< Optimal primal solution vector.
-																								 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
-												const real_t* const yOpt,						/**< Optimal dual solution vector.
-																								 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
-												const Bounds* const guessedBounds,				/**< Guessed working set of bounds for solution (xOpt,yOpt). */
-												const Constraints* const guessedConstraints,	/**< Guessed working set for solution (xOpt,yOpt). */
-												Bounds* auxiliaryBounds,						/**< Input: Allocated bound object. \n
-																								 *	 Ouput: Working set of constraints for auxiliary QP. */
-												Constraints* auxiliaryConstraints				/**< Input: Allocated bound object. \n
-																								 *	 Ouput: Working set for auxiliary QP. */
-												) const;
-
-		/** Sets up bound and constraints data structures according to auxiliaryBounds/Constraints.
-		 *  (If the working set shall be setup afresh, make sure that
-		 *  bounds and constraints data structure have been resetted
-		 *  and the TQ factorisation has been initialised!)
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_SETUP_WORKINGSET_FAILED \n
-					RET_INVALID_ARGUMENTS \n
-					RET_UNKNOWN_BUG */
-		returnValue setupAuxiliaryWorkingSet(	const Bounds* const auxiliaryBounds,			/**< Working set of bounds for auxiliary QP. */
-												const Constraints* const auxiliaryConstraints,	/**< Working set of constraints for auxiliary QP. */
-												BooleanType setupAfresh							/**< Flag indicating if given working set shall be
-																								 *    setup afresh or by updating the current one. */
-												);
-
-		/** Sets up the optimal primal/dual solution of the auxiliary initial QP.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue setupAuxiliaryQPsolution(	const real_t* const xOpt,			/**< Optimal primal solution vector.
-																				 	*	 If a NULL pointer is passed, all entries are set to zero. */
-												const real_t* const yOpt			/**< Optimal dual solution vector.
-																					 *	 If a NULL pointer is passed, all entries are set to zero. */
-												);
-
-		/** Sets up gradient of the auxiliary initial QP for given
-		 *  optimal primal/dual solution and given initial working set
-		 *  (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue setupAuxiliaryQPgradient( );
-
-		/** Sets up (constraints') bounds of the auxiliary initial QP for given
-		 *  optimal primal/dual solution and given initial working set
-		 *  (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_UNKNOWN_BUG */
-		returnValue setupAuxiliaryQPbounds(	const Bounds* const auxiliaryBounds,			/**< Working set of bounds for auxiliary QP. */
-											const Constraints* const auxiliaryConstraints,	/**< Working set of constraints for auxiliary QP. */
-											BooleanType useRelaxation						/**< Flag indicating if inactive (constraints') bounds shall be relaxed. */
-											);
-
-
-		/** Adds a constraint to active set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_ADDCONSTRAINT_FAILED \n
-					RET_ADDCONSTRAINT_FAILED_INFEASIBILITY \n
-					RET_ENSURELI_FAILED */
-		returnValue addConstraint(	int number,					/**< Number of constraint to be added to active set. */
-									SubjectToStatus C_status,	/**< Status of new active constraint. */
-									BooleanType updateCholesky,	/**< Flag indicating if Cholesky decomposition shall be updated. */
-									BooleanType ensureLI = BT_TRUE	/**< Ensure linear independence by exchange rules by default. */
-									);
-
-		/** Checks if new active constraint to be added is linearly dependent from
-		 *	from row of the active constraints matrix.
-		 *	\return	 RET_LINEARLY_DEPENDENT \n
-		 			 RET_LINEARLY_INDEPENDENT \n
-					 RET_INDEXLIST_CORRUPTED */
-		returnValue addConstraint_checkLI(	int number			/**< Number of constraint to be added to active set. */
-											);
-
-		/** Ensures linear independence of constraint matrix when a new constraint is added.
-		 * 	To this end a bound or constraint is removed simultaneously if necessary.
-		 *	\return	 SUCCESSFUL_RETURN \n
-		 			 RET_LI_RESOLVED \n
-					 RET_ENSURELI_FAILED \n
-					 RET_ENSURELI_FAILED_TQ \n
-					 RET_ENSURELI_FAILED_NOINDEX \n
-					 RET_REMOVE_FROM_ACTIVESET */
-		returnValue addConstraint_ensureLI(	int number,					/**< Number of constraint to be added to active set. */
-											SubjectToStatus C_status	/**< Status of new active bound. */
-											);
-
-		/** Adds a bound to active set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_ADDBOUND_FAILED \n
-					RET_ADDBOUND_FAILED_INFEASIBILITY \n
-					RET_ENSURELI_FAILED */
-		returnValue addBound(	int number,					/**< Number of bound to be added to active set. */
-								SubjectToStatus B_status,	/**< Status of new active bound. */
-								BooleanType updateCholesky,	/**< Flag indicating if Cholesky decomposition shall be updated. */
-								BooleanType ensureLI = BT_TRUE	/**< Ensure linear independence by exchange rules by default. */
-								);
-
-		/** Checks if new active bound to be added is linearly dependent from
-		 *	from row of the active constraints matrix.
-		 *	\return	 RET_LINEARLY_DEPENDENT \n
-		 			 RET_LINEARLY_INDEPENDENT */
-		returnValue addBound_checkLI(	int number			/**< Number of bound to be added to active set. */
-										);
-
-		/** Ensures linear independence of constraint matrix when a new bound is added.
-		 *	To this end a bound or constraint is removed simultaneously if necessary.
-		 *	\return	 SUCCESSFUL_RETURN \n
-		 			 RET_LI_RESOLVED \n
-					 RET_ENSURELI_FAILED \n
-					 RET_ENSURELI_FAILED_TQ \n
-					 RET_ENSURELI_FAILED_NOINDEX \n
-					 RET_REMOVE_FROM_ACTIVESET */
-		returnValue addBound_ensureLI(	int number,					/**< Number of bound to be added to active set. */
-										SubjectToStatus B_status	/**< Status of new active bound. */
-										);
-
-		/** Removes a constraint from active set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_CONSTRAINT_NOT_ACTIVE \n
-					RET_REMOVECONSTRAINT_FAILED \n
-					RET_HESSIAN_NOT_SPD */
-		returnValue removeConstraint(	int number,								/**< Number of constraint to be removed from active set. */
-										BooleanType updateCholesky,				/**< Flag indicating if Cholesky decomposition shall be updated. */
-										BooleanType allowFlipping = BT_FALSE,	/**< Flag indicating if flipping bounds are allowed. */
-										BooleanType ensureNZC = BT_FALSE		/**< Flag indicating if non-zero curvature is ensured by exchange rules. */
-										);
-
-		/** Removes a bounds from active set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_BOUND_NOT_ACTIVE \n
-					RET_HESSIAN_NOT_SPD \n
-					RET_REMOVEBOUND_FAILED */
-		returnValue removeBound(	int number,								/**< Number of bound to be removed from active set. */
-									BooleanType updateCholesky,				/**< Flag indicating if Cholesky decomposition shall be updated. */
-									BooleanType allowFlipping = BT_FALSE,	/**< Flag indicating if flipping bounds are allowed. */
-									BooleanType ensureNZC = BT_FALSE		/**< Flag indicating if non-zero curvature is ensured by exchange rules. */
-									);
-
-
-		/** Performs robustified ratio test yield the maximum possible step length
-		 *  along the homotopy path.
-		 *	\return  SUCCESSFUL_RETURN */
-		returnValue performPlainRatioTest(	int nIdx, 							/**< Number of ratios to be checked. */
-											const int* const idxList, 			/**< Array containing the indices of all ratios to be checked. */
-											const real_t* const num,	 		/**< Array containing all numerators for performing the ratio test. */
-											const real_t* const den,		 	/**< Array containing all denominators for performing the ratio test. */
-											real_t epsNum,						/**< Numerator tolerance. */
-											real_t epsDen,						/**< Denominator tolerance. */
-											real_t& t,							/**< Output: Maximum possible step length along the homotopy path. */
-											int& BC_idx 						/**< Output: Index of blocking constraint. */
-											) const;
-
-
-		/** Ensure non-zero curvature by primal jump.
-		 *  \return SUCCESSFUL_RETURN \n
-		 *          RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
-		returnValue ensureNonzeroCurvature(
-				BooleanType removeBoundNotConstraint,	/**< SubjectTo to be removed is a bound. */
-				int remIdx,								/**< Index of bound/constraint to be removed. */
-				BooleanType &exchangeHappened,			/**< Output: Exchange was necessary to ensure. */
-				BooleanType &addBoundNotConstraint,		/**< SubjectTo to be added is a bound. */
-				int &addIdx,							/**< Index of bound/constraint to be added. */
-				SubjectToStatus &addStatus				/**< Status of bound/constraint to be added. */
-				);
-
-
-		/** Solves the system Ta = b or T^Ta = b where T is a reverse upper triangular matrix.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_DIV_BY_ZERO */
-		returnValue backsolveT(	const real_t* const b,	/**< Right hand side vector. */
-								BooleanType transposed,	/**< Indicates if the transposed system shall be solved. */
-								real_t* const a 		/**< Output: Solution vector */
-								) const;
-
-
-		/** Determines step direction of the shift of the QP data.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue determineDataShift(	const real_t* const g_new,	/**< New gradient vector. */
-										const real_t* const lbA_new,/**< New lower constraints' bounds. */
-										const real_t* const ubA_new,/**< New upper constraints' bounds. */
-										const real_t* const lb_new,	/**< New lower bounds. */
-										const real_t* const ub_new,	/**< New upper bounds. */
-										real_t* const delta_g,	 	/**< Output: Step direction of gradient vector. */
-										real_t* const delta_lbA,	/**< Output: Step direction of lower constraints' bounds. */
-										real_t* const delta_ubA,	/**< Output: Step direction of upper constraints' bounds. */
-										real_t* const delta_lb,	 	/**< Output: Step direction of lower bounds. */
-										real_t* const delta_ub,	 	/**< Output: Step direction of upper bounds. */
-										BooleanType& Delta_bC_isZero,/**< Output: Indicates if active constraints' bounds are to be shifted. */
-										BooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */
-										);
-
-		/** Determines step direction of the homotopy path.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_STEPDIRECTION_FAILED_TQ \n
-					RET_STEPDIRECTION_FAILED_CHOLESKY */
-		returnValue determineStepDirection(	const real_t* const delta_g,	/**< Step direction of gradient vector. */
-											const real_t* const delta_lbA,	/**< Step direction of lower constraints' bounds. */
-											const real_t* const delta_ubA,	/**< Step direction of upper constraints' bounds. */
-											const real_t* const delta_lb,	/**< Step direction of lower bounds. */
-											const real_t* const delta_ub,	/**< Step direction of upper bounds. */
-											BooleanType Delta_bC_isZero, 	/**< Indicates if active constraints' bounds are to be shifted. */
-											BooleanType Delta_bB_isZero,	/**< Indicates if active bounds are to be shifted. */
-											real_t* const delta_xFX, 		/**< Output: Primal homotopy step direction of fixed variables. */
-											real_t* const delta_xFR,	 	/**< Output: Primal homotopy step direction of free variables. */
-											real_t* const delta_yAC, 		/**< Output: Dual homotopy step direction of active constraints' multiplier. */
-											real_t* const delta_yFX 		/**< Output: Dual homotopy step direction of fixed variables' multiplier. */
-											);
-
-		/** Determines the maximum possible step length along the homotopy path
-		 *  and performs this step (without changing working set).
-		 *	\return SUCCESSFUL_RETURN \n
-		 * 			RET_ERROR_IN_CONSTRAINTPRODUCT \n
-		 * 			RET_QP_INFEASIBLE */
-		returnValue performStep(	const real_t* const delta_g,		/**< Step direction of gradient. */
-									const real_t* const delta_lbA,		/**< Step direction of lower constraints' bounds. */
-									const real_t* const delta_ubA,		/**< Step direction of upper constraints' bounds. */
-									const real_t* const delta_lb,	 	/**< Step direction of lower bounds. */
-									const real_t* const delta_ub,	 	/**< Step direction of upper bounds. */
-									const real_t* const delta_xFX, 		/**< Primal homotopy step direction of fixed variables. */
-									const real_t* const delta_xFR,		/**< Primal homotopy step direction of free variables. */
-									const real_t* const delta_yAC,		/**< Dual homotopy step direction of active constraints' multiplier. */
-									const real_t* const delta_yFX,		/**< Dual homotopy step direction of fixed variables' multiplier. */
-									int& BC_idx, 						/**< Output: Index of blocking constraint. */
-									SubjectToStatus& BC_status,			/**< Output: Status of blocking constraint. */
-									BooleanType& BC_isBound 			/**< Output: Indicates if blocking constraint is a bound. */
-									);
-
-		/** Updates the active set.
-		 *	\return  SUCCESSFUL_RETURN \n
-		 			 RET_REMOVE_FROM_ACTIVESET_FAILED \n
-					 RET_ADD_TO_ACTIVESET_FAILED */
-		returnValue changeActiveSet(	int BC_idx, 						/**< Index of blocking constraint. */
-										SubjectToStatus BC_status,			/**< Status of blocking constraint. */
-										BooleanType BC_isBound 				/**< Indicates if blocking constraint is a bound. */
-										);
-
-
-		/** Compute relative length of homotopy in data space for termination
-		 *  criterion.
-		 *  \return Relative length in data space. */
-		real_t getRelativeHomotopyLength(	const real_t* const g_new,		/**< Final gradient. */
-											const real_t* const lb_new,		/**< Final lower variable bounds. */
-											const real_t* const ub_new,		/**< Final upper variable bounds. */
-											const real_t* const lbA_new,	/**< Final lower constraint bounds. */
-											const real_t* const ubA_new		/**< Final upper constraint bounds. */
-											);
-
-
-		/** Ramping Strategy to avoid ties. Modifies homotopy start without
-		 *  changing current active set.
-		 *  \return SUCCESSFUL_RETURN */
-		virtual returnValue performRamping( );
-
-
-		/** ... */
-		returnValue updateFarBounds(	real_t curFarBound,					/**< ... */
-                                        int nRamp,							/**< ... */
-                                        const real_t* const lb_new,			/**< ... */
-                                        real_t* const lb_new_far,			/**< ... */
-                                        const real_t* const ub_new,			/**< ... */
-                                        real_t* const ub_new_far,			/**< ... */
-                                        const real_t* const lbA_new,		/**< ... */
-                                        real_t* const lbA_new_far,			/**< ... */
-                                        const real_t* const ubA_new,		/**< ... */
-                                        real_t* const ubA_new_far			/**< ... */
-                                        ) const;
-
-
-		/** Drift correction at end of each active set iteration
-		 *  \return SUCCESSFUL_RETURN */
-		virtual returnValue performDriftCorrection( );
-
-
-		/** Updates QP vectors, working sets and internal data structures in order to
-			start from an optimal solution corresponding to initial guesses of the working
-			set for bounds and constraints.
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_SETUP_AUXILIARYQP_FAILED \n
-					RET_INVALID_ARGUMENTS */
-		virtual returnValue setupAuxiliaryQP(	const Bounds* const guessedBounds,			/**< Initial guess for working set of bounds. */
-												const Constraints* const guessedConstraints	/**< Initial guess for working set of constraints. */
-												);
-
-		/** Determines if it is more efficient to refactorise the matrices when
-		 *  hotstarting or not (i.e. better to update the existing factorisations).
-		 *	\return BT_TRUE iff matrices shall be refactorised afresh
-		 */
-		BooleanType shallRefactorise(	const Bounds* const guessedBounds,			/**< Guessed new working set of bounds. */
-										const Constraints* const guessedConstraints	/**< Guessed new working set of constraints. */
-										) const;
-
-		/** Sets up internal QP data.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INVALID_ARGUMENTS \n
-					RET_UNKNONW_BUG */
-		returnValue setupQPdata(	SymmetricMatrix *_H, 		/**< Hessian matrix. \n
-																     If Hessian matrix is trivial,a NULL pointer can be passed. */
-									const real_t* const _g, 	/**< Gradient vector. */
-									Matrix *_A, 			 	/**< Constraint matrix. */
-									const real_t* const _lb,	/**< Lower bound vector (on variables). \n
-																	 If no lower bounds exist, a NULL pointer can be passed. */
-									const real_t* const _ub,	/**< Upper bound vector (on variables). \n
-																	 If no upper bounds exist, a NULL pointer can be passed. */
-									const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
-																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-									const real_t* const _ubA	/**< Upper constraints' bound vector. \n
-																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-									);
-
-
-		/** Sets up dense internal QP data. If the current Hessian is trivial
-		 *  (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
-		 *  memory for Hessian is allocated and it is set to the given one.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INVALID_ARGUMENTS \n
-					RET_UNKNONW_BUG */
-		returnValue setupQPdata(	const real_t* const _H, 	/**< Hessian matrix. \n
-																     If Hessian matrix is trivial,a NULL pointer can be passed. */
-									const real_t* const _g, 	/**< Gradient vector. */
-									const real_t* const _A,  	/**< Constraint matrix. */
-									const real_t* const _lb,	/**< Lower bound vector (on variables). \n
-																	 If no lower bounds exist, a NULL pointer can be passed. */
-									const real_t* const _ub,	/**< Upper bound vector (on variables). \n
-																	 If no upper bounds exist, a NULL pointer can be passed. */
-									const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
-																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-									const real_t* const _ubA	/**< Upper constraints' bound vector. \n
-																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-									);
-
-		/** Sets up internal QP data by loading it from files. If the current Hessian
-		 *  is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
-		 *  memory for Hessian is allocated and it is set to the given one.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_UNABLE_TO_OPEN_FILE \n
-					RET_UNABLE_TO_READ_FILE \n
-					RET_INVALID_ARGUMENTS \n
-					RET_UNKNONW_BUG */
-		returnValue setupQPdataFromFile(	const char* const H_file, 	/**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n
-																     		 If Hessian matrix is trivial,a NULL pointer can be passed. */
-											const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-											const char* const A_file,	/**< Name of file where constraint matrix, of neighbouring QP to be solved, is stored. */
-											const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-												 			 				 If no lower bounds exist, a NULL pointer can be passed. */
-											const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-												 			 				 If no upper bounds exist, a NULL pointer can be passed. */
-											const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
-												 			 				 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-											const char* const ubA_file	/**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
-												 			 				 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-											);
-
-		/** Loads new QP vectors from files (internal members are not affected!).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_UNABLE_TO_OPEN_FILE \n
-					RET_UNABLE_TO_READ_FILE \n
-					RET_INVALID_ARGUMENTS */
-		returnValue loadQPvectorsFromFile(	const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-											const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-												 			 				 If no lower bounds exist, a NULL pointer can be passed. */
-											const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-												 			 				 If no upper bounds exist, a NULL pointer can be passed. */
-											const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
-												 			 				 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-											const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
-												 			 				 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-											real_t* const g_new,		/**< Output: Gradient of neighbouring QP to be solved. */
-											real_t* const lb_new,		/**< Output: Lower bounds of neighbouring QP to be solved */
-											real_t* const ub_new,		/**< Output: Upper bounds of neighbouring QP to be solved */
-											real_t* const lbA_new,		/**< Output: Lower constraints' bounds of neighbouring QP to be solved */
-											real_t* const ubA_new		/**< Output: Upper constraints' bounds of neighbouring QP to be solved */
-											) const;
-
-
-		/** Prints concise information on the current iteration.
-		 *	\return  SUCCESSFUL_RETURN \n */
-		returnValue printIteration(	int iter,					/**< Number of current iteration. */
-									int BC_idx, 				/**< Index of blocking constraint. */
-									SubjectToStatus BC_status,	/**< Status of blocking constraint. */
-									BooleanType BC_isBound, 	/**< Indicates if blocking constraint is a bound. */
-									real_t homotopyLength		/**< Current homotopy distance. */
-		 							);
-
-
-		/** Sets constraint matrix of the QP. \n
-			Note: Also internal vector Ax is recomputed!
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_INVALID_ARGUMENTS */
-		inline returnValue setA(	Matrix *A_new	/**< New constraint matrix (a shallow copy is made). */
-									);
-
-		/** Sets dense constraint matrix of the QP. \n
-			Note: Also internal vector Ax is recomputed!
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_INVALID_ARGUMENTS */
-		inline returnValue setA(	const real_t* const A_new	/**< New dense constraint matrix (with correct dimension!), a shallow copy is made. */
-									);
-
-
-		/** Sets constraints' lower bound vector of the QP.
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_QPOBJECT_NOT_SETUP */
-		inline returnValue setLBA(	const real_t* const lbA_new	/**< New constraints' lower bound vector (with correct dimension!). */
-									);
-
-		/** Changes single entry of lower constraints' bound vector of the QP.
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_QPOBJECT_NOT_SETUP \n
-		 *			RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue setLBA(	int number,		/**< Number of entry to be changed. */
-									real_t value	/**< New value for entry of lower constraints' bound vector (with correct dimension!). */
-									);
-
-		/** Sets constraints' upper bound vector of the QP.
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_QPOBJECT_NOT_SETUP */
-		inline returnValue setUBA(	const real_t* const ubA_new	/**< New constraints' upper bound vector (with correct dimension!). */
-									);
-
-		/** Changes single entry of upper constraints' bound vector of the QP.
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_QPOBJECT_NOT_SETUP \n
-		 *			RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue setUBA(	int number,		/**< Number of entry to be changed. */
-									real_t value	/**< New value for entry of upper constraints' bound vector (with correct dimension!). */
-									);
-
-
-		/** Drops the blocking bound/constraint that led to infeasibility, or finds another
-		 *  bound/constraint to drop according to drop priorities.
-		 *  \return SUCCESSFUL_RETURN \n
-		 */
-		returnValue dropInfeasibles ( 	int BC_number,				/**< Number of the bound or constraint to be added. */
-										SubjectToStatus BC_status, 	/**< New status of the bound or constraint to be added. */
-										BooleanType BC_isBound,		/**< Whether a bound or a constraint is to be added. */
-										real_t *xiB,				/**< (not yet documented) */
-										real_t *xiC					/**< (not yet documented) */
-										);
-
-		/** Decides if lower bounds are smaller than upper bounds
-		 *
-		 * \return SUCCESSFUL_RETURN \n
-		 * 		   RET_QP_INFEASIBLE */
-
-				returnValue areBoundsConsistent(const real_t* const lb,  /**< Vector of lower bounds*/
-												const real_t* const ub,  /**< Vector of upper bounds*/
-												const real_t* const lbA, /**< Vector of lower constraints*/
-												const real_t* const ubA  /**< Vector of upper constraints*/
-												) const;
-
-
-	public:
-		/** ...
-		 *	\return SUCCESSFUL_RETURN  \n
-					RET_UNABLE_TO_OPEN_FILE */
-		returnValue writeQpDataIntoMatFile(	const char* const filename	/**< Mat file name. */
-											) const;
-
-		/** ...
-		 *	\return SUCCESSFUL_RETURN  \n
-					RET_UNABLE_TO_OPEN_FILE */
-		returnValue writeQpWorkspaceIntoMatFile(	const char* const filename	/**< Mat file name. */
-													);
-
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-		BooleanType freeConstraintMatrix; 		/**< Flag indicating whether the constraint matrix needs to be de-allocated. */
-		Matrix* A;								/**< Constraint matrix. */
-
-		real_t* lbA;							/**< Lower constraints' bound vector. */
-		real_t* ubA;							/**< Upper constraints' bound vector. */
-
-		Constraints constraints;				/**< Data structure for problem's constraints. */
-
-		real_t* T;								/**< Reverse triangular matrix, A = [0 T]*Q'. */
-		real_t* Q;								/**< Orthonormal quadratic matrix, A = [0 T]*Q'. */
-		int sizeT;								/**< Matrix T is stored in a (sizeT x sizeT) array. */
-
-		real_t* Ax;								/**< Stores the current A*x \n
-												 *	 (for increased efficiency only). */
-		real_t* Ax_l;							/**< Stores the current distance to lower constraints' bounds A*x-lbA \n
-												 *	 (for increased efficiency only). */
-		real_t* Ax_u;							/**< Stores the current distance to lower constraints' bounds ubA-A*x \n
-												 *	 (for increased efficiency only). */
-
-		ConstraintProduct* constraintProduct;	/**< Pointer to user-defined constraint product function. */
-
-		real_t* tempA;							/**< Temporary for determineStepDirection. */
-		real_t* tempB;							/**< Temporary for determineStepDirection. */
-		real_t* ZFR_delta_xFRz;					/**< Temporary for determineStepDirection. */
-		real_t* delta_xFRy;						/**< Temporary for determineStepDirection. */
-		real_t* delta_xFRz;						/**< Temporary for determineStepDirection. */
-		real_t* delta_yAC_TMP;					/**< Temporary for determineStepDirection. */
-};
-
-
-END_NAMESPACE_QPOASES
-
-#include <qpOASES/QProblem.ipp>
-
-#endif	/* QPOASES_QPROBLEM_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblem.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblem.ipp
deleted file mode 100644
index 84662a069d3..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblem.ipp
+++ /dev/null
@@ -1,283 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/QProblem.ipp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of inlined member functions of the QProblem class which
- *	is able to use the newly developed online active set strategy for
- *	parametric quadratic programming.
- */
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	g e t C o n s t r a i n t s
- */
-inline returnValue QProblem::getConstraints( Constraints& _constraints ) const
-{
-	int nV = getNV( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	_constraints = constraints;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	g e t N C
- */
-inline int QProblem::getNC( ) const
-{
-	return constraints.getNC( );
-}
-
-
-/*
- *	g e t N E C
- */
-inline int QProblem::getNEC( ) const
-{
-	return constraints.getNEC( );
-}
-
-
-/*
- *	g e t N A C
- */
-inline int QProblem::getNAC( ) const
-{
-	return constraints.getNAC( );
-}
-
-
-/*
- *	g e t N I A C
- */
-inline int QProblem::getNIAC( ) const
-{
-	return constraints.getNIAC( );
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-
-/*
- *	s e t A
- */
-inline returnValue QProblem::setA( Matrix *A_new )
-{
-	int j;
-	int nV = getNV( );
-	int nC = getNC( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( A_new == 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* Set constraint matrix AND update member AX. */
-	if ( ( freeConstraintMatrix == BT_TRUE ) && ( A != 0 ) )
-	{
-		delete A;
-		A = 0;
-	}
-	A = A_new;
-	freeConstraintMatrix = BT_FALSE;
-
-	A->times(1, 1.0, x, nV, 0.0, Ax, nC);
-
-	for( j=0; j<nC; ++j )
-	{
-		Ax_u[j] = ubA[j] - Ax[j];
-		Ax_l[j] = Ax[j] - lbA[j];
-		
-		// (ckirches) disable constraints with empty rows	
-		if ( isZero( A->getRowNorm (j) ) == BT_TRUE )
-			constraints.setType ( j, ST_DISABLED );
-	}
-
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t A
- */
-inline returnValue QProblem::setA( const real_t* const A_new )
-{
-	int j;
-	int nV = getNV( );
-	int nC = getNC( );
-	DenseMatrix* dA;
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( A_new == 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* Set constraint matrix AND update member AX. */
-	if ( ( freeConstraintMatrix == BT_TRUE ) && ( A != 0 ) )
-	{
-		delete A;
-		A = 0;
-	}
-	A = dA = new DenseMatrix(nC, nV, nV, (real_t*) A_new);
-	freeConstraintMatrix = BT_TRUE;
-
-	A->times(1, 1.0, x, nV, 0.0, Ax, nC);
-
-	for( j=0; j<nC; ++j )
-	{
-		Ax_u[j] = ubA[j] - Ax[j];
-		Ax_l[j] = Ax[j] - lbA[j];
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t L B A
- */
-inline returnValue QProblem::setLBA( const real_t* const lbA_new )
-{
-	unsigned int i;
-	unsigned int nV = (unsigned int)getNV( );
-	unsigned int nC = (unsigned int)getNC( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( lbA_new != 0 )
-	{	
-		memcpy( lbA,lbA_new,nC*sizeof(real_t) );
-	}
-	else
-	{
-		/* if no lower constraints' bounds are specified, set them to -infinity */
-		for( i=0; i<nC; ++i )
-			lbA[i] = -INFTY;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t L B A
- */
-inline returnValue QProblem::setLBA( int number, real_t value )
-{
-	int nV = getNV( );
-	int nC = getNC( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( ( number >= 0 ) && ( number < nC ) )
-	{
-		lbA[number] = value;
-		return SUCCESSFUL_RETURN;
-	}
-	else
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-}
-
-
-/*
- *	s e t U B A
- */
-inline returnValue QProblem::setUBA( const real_t* const ubA_new )
-{
-	unsigned int i;
-	unsigned int nV = (unsigned int)getNV( );
-	unsigned int nC = (unsigned int)getNC( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( ubA_new != 0 )
-	{
-		memcpy( ubA,ubA_new,nC*sizeof(real_t) );
-	}
-	else
-	{
-		/* if no upper constraints' bounds are specified, set them to infinity */
-		for( i=0; i<nC; ++i )
-			ubA[i] = INFTY;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t U B A
- */
-inline returnValue QProblem::setUBA( int number, real_t value )
-{
-	int nV = getNV( );
-	int nC = getNC( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( ( number >= 0 ) && ( number < nC ) )
-	{
-		ubA[number] = value;
-		return SUCCESSFUL_RETURN;
-	}
-	else
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblemB.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblemB.hpp
deleted file mode 100644
index d62244e130e..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblemB.hpp
+++ /dev/null
@@ -1,1091 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/QProblemB.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Declaration of the QProblemB class which is able to use the newly
- *	developed online active set strategy for parametric quadratic programming
- *	for problems with (simple) bounds only.
- */
-
-
-
-#ifndef QPOASES_QPROBLEMB_HPP
-#define QPOASES_QPROBLEMB_HPP
-
-
-#include <qpOASES/Flipper.hpp>
-#include <qpOASES/Options.hpp>
-#include <qpOASES/Matrices.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-class SolutionAnalysis;
-
-/** 
- *	\brief Implements the online active set strategy for box-constrained QPs.
- *
- *	Class for setting up and solving quadratic programs with bounds (= box constraints) only.
- *	The main feature is the possibily to use the newly developed online active set strategy
- *	for parametric quadratic programming.
- *
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- */
-class QProblemB
-{
-	/* allow SolutionAnalysis class to access private members */
-	friend class SolutionAnalysis;
-
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		QProblemB( );
-
-		/** Constructor which takes the QP dimension and Hessian type
-		 *  information. If the Hessian is the zero (i.e. HST_ZERO) or the
-		 *  identity matrix (i.e. HST_IDENTITY), respectively, no memory
-		 *  is allocated for it and a NULL pointer can be passed for it
-		 *  to the init() functions. */
-		QProblemB(	int _nV,								/**< Number of variables. */
-					HessianType _hessianType = HST_UNKNOWN	/**< Type of Hessian matrix. */
-					);
-
-		/** Copy constructor (deep copy). */
-		QProblemB(	const QProblemB& rhs	/**< Rhs object. */
-					);
-
-		/** Destructor. */
-		virtual ~QProblemB( );
-
-		/** Assignment operator (deep copy). */
-		QProblemB& operator=(	const QProblemB& rhs	/**< Rhs object. */
-								);
-
-
-		/** Clears all data structures of QProblemB except for QP data.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_RESET_FAILED */
-		virtual returnValue reset( );
-
-
-		/** Initialises a QProblemB with given QP data and solves it
-		 *	using an initial homotopy with empty working set (at most nWSR iterations).
-		 *  Note: This function internally calls solveInitialQP for initialisation!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED \n
-					RET_INVALID_ARGUMENTS */
-		returnValue init(	SymmetricMatrix *_H,	 	/**< Hessian matrix (a shallow copy is made). */
-							const real_t* const _g,		/**< Gradient vector. */
-							const real_t* const _lb,	/**< Lower bounds (on variables). \n
-															 If no lower bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ub,	/**< Upper bounds (on variables). \n
-															 If no upper bounds exist, a NULL pointer can be passed. */
-							int& nWSR, 					/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
-															 Output: Number of performed working set recalculations. */
-				 			real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP initialisation. \n
-															 Output: CPU time spend for QP initialisation (if pointer passed). */
-							);
-
-		/** Initialises a QProblemB with given QP data and solves it
-		 *	using an initial homotopy with empty working set (at most nWSR iterations).
-		 *  Note: This function internally calls solveInitialQP for initialisation!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED \n
-					RET_INVALID_ARGUMENTS */
-		returnValue init(	const real_t* const _H, 	/**< Hessian matrix (a shallow copy is made). \n
-															 If Hessian matrix is trivial, a NULL pointer can be passed. */
-							const real_t* const _g,		/**< Gradient vector. */
-							const real_t* const _lb,	/**< Lower bounds (on variables). \n
-															 If no lower bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ub,	/**< Upper bounds (on variables). \n
-															 If no upper bounds exist, a NULL pointer can be passed. */
-							int& nWSR, 					/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
-															 Output: Number of performed working set recalculations. */
-				 			real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP initialisation. \n
-															 Output: CPU time spend for QP initialisation (if pointer passed). */
-							);
-
-		/** Initialises a QProblemB with given QP data to be read from files and solves it
-		 *	using an initial homotopy with empty working set (at most nWSR iterations).
-		 *  Note: This function internally calls solveInitialQP for initialisation!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED \n
-					RET_UNABLE_TO_READ_FILE */
-		returnValue init(	const char* const H_file, 	/**< Name of file where Hessian matrix is stored. \n
-															 If Hessian matrix is trivial, a NULL pointer can be passed. */
-							const char* const g_file,  	/**< Name of file where gradient vector is stored. */
-							const char* const lb_file, 	/**< Name of file where lower bound vector. \n
-															 If no lower bounds exist, a NULL pointer can be passed. */
-							const char* const ub_file, 	/**< Name of file where upper bound vector. \n
-															 If no upper bounds exist, a NULL pointer can be passed. */
-							int& nWSR, 					/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
-															 Output: Number of performed working set recalculations. */
-				 			real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP initialisation. \n
-															 Output: CPU time spend for QP initialisation (if pointer passed). */
-							);
-
-		/** Initialises a QProblemB with given QP data and solves it
-		 *  depending on the parameter constellation: \n
-		 *	1. 0,    0,    0 : start with xOpt = 0, yOpt = 0 and IB empty (or all implicit equality bounds), \n
-		 *	2. xOpt, 0,    0 : start with xOpt, yOpt = 0 and obtain IB by "clipping", \n
-		 *	3. 0,    yOpt, 0 : start with xOpt = 0, yOpt and obtain IB from yOpt != 0, \n
-		 *	4. 0,    0,    IB: start with xOpt = 0, yOpt = 0 and IB, \n
-		 *	5. xOpt, yOpt, 0 : start with xOpt, yOpt and obtain IB from yOpt != 0, \n
-		 *	6. xOpt, 0,    IB: start with xOpt, yOpt = 0 and IB, \n
-		 *	7. xOpt, yOpt, IB: start with xOpt, yOpt and IB (assume them to be consistent!)
-		 *  Note: This function internally calls solveInitialQP for initialisation!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED \n
-					RET_INVALID_ARGUMENTS */
-		returnValue init(	SymmetricMatrix *_H,				/**< Hessian matrix (a shallow copy is made). */
-							const real_t* const _g,				/**< Gradient vector. */
-							const real_t* const _lb,			/**< Lower bounds (on variables). \n
-																	 If no lower bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ub,			/**< Upper bounds (on variables). \n
-																	 If no upper bounds exist, a NULL pointer can be passed. */
-							int& nWSR, 							/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
-																	 Output: Number of performed working set recalculations. */
-				 			real_t* const cputime,				/**< Input: Maximum CPU time allowed for QP initialisation. \n
-															 		 Output: CPU time spend for QP initialisation. */
-							const real_t* const xOpt,			/**< Optimal primal solution vector.
-																	 A NULL pointer can be passed. */
-							const real_t* const yOpt,			/**< Optimal dual solution vector.
-																	 A NULL pointer can be passed. */
-							const Bounds* const guessedBounds	/**< Optimal working set for solution (xOpt,yOpt).
-																	 A NULL pointer can be passed. */
-							);
-
-		/** Initialises a QProblemB with given QP data and solves it
-		 *  depending on the parameter constellation: \n
-		 *	1. 0,    0,    0 : start with xOpt = 0, yOpt = 0 and IB empty (or all implicit equality bounds), \n
-		 *	2. xOpt, 0,    0 : start with xOpt, yOpt = 0 and obtain IB by "clipping", \n
-		 *	3. 0,    yOpt, 0 : start with xOpt = 0, yOpt and obtain IB from yOpt != 0, \n
-		 *	4. 0,    0,    IB: start with xOpt = 0, yOpt = 0 and IB, \n
-		 *	5. xOpt, yOpt, 0 : start with xOpt, yOpt and obtain IB from yOpt != 0, \n
-		 *	6. xOpt, 0,    IB: start with xOpt, yOpt = 0 and IB, \n
-		 *	7. xOpt, yOpt, IB: start with xOpt, yOpt and IB (assume them to be consistent!)
-		 *  Note: This function internally calls solveInitialQP for initialisation!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED \n
-					RET_INVALID_ARGUMENTS */
-		returnValue init(	const real_t* const _H, 			/**< Hessian matrix (a shallow copy is made). \n
-																     If Hessian matrix is trivial, a NULL pointer can be passed. */
-							const real_t* const _g,				/**< Gradient vector. */
-							const real_t* const _lb,			/**< Lower bounds (on variables). \n
-																	 If no lower bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ub,			/**< Upper bounds (on variables). \n
-																	 If no upper bounds exist, a NULL pointer can be passed. */
-							int& nWSR, 							/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
-																	 Output: Number of performed working set recalculations. */
-				 			real_t* const cputime,				/**< Input: Maximum CPU time allowed for QP initialisation. \n
-															 		 Output: CPU time spend for QP initialisation. */
-							const real_t* const xOpt,			/**< Optimal primal solution vector.
-																	 A NULL pointer can be passed. */
-							const real_t* const yOpt,			/**< Optimal dual solution vector.
-																	 A NULL pointer can be passed. */
-							const Bounds* const guessedBounds	/**< Optimal working set for solution (xOpt,yOpt).
-																	 A NULL pointer can be passed. */
-							);
-
-		/** Initialises a QProblemB with given QP data to be read from files and solves it
-		 *  depending on the parameter constellation: \n
-		 *	1. 0,    0,    0 : start with xOpt = 0, yOpt = 0 and IB empty (or all implicit equality bounds), \n
-		 *	2. xOpt, 0,    0 : start with xOpt, yOpt = 0 and obtain IB by "clipping", \n
-		 *	3. 0,    yOpt, 0 : start with xOpt = 0, yOpt and obtain IB from yOpt != 0, \n
-		 *	4. 0,    0,    IB: start with xOpt = 0, yOpt = 0 and IB, \n
-		 *	5. xOpt, yOpt, 0 : start with xOpt, yOpt and obtain IB from yOpt != 0, \n
-		 *	6. xOpt, 0,    IB: start with xOpt, yOpt = 0 and IB, \n
-		 *	7. xOpt, yOpt, IB: start with xOpt, yOpt and IB (assume them to be consistent!)
-		 *  Note: This function internally calls solveInitialQP for initialisation!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED \n
-					RET_UNABLE_TO_READ_FILE */
-		returnValue init(	const char* const H_file, 			/**< Name of file where Hessian matrix is stored. \n
-																     If Hessian matrix is trivial, a NULL pointer can be passed. */
-							const char* const g_file,  			/**< Name of file where gradient vector is stored. */
-							const char* const lb_file, 			/**< Name of file where lower bound vector. \n
-																	 If no lower bounds exist, a NULL pointer can be passed. */
-							const char* const ub_file, 			/**< Name of file where upper bound vector. \n
-																	 If no upper bounds exist, a NULL pointer can be passed. */
-							int& nWSR, 							/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
-																	 Output: Number of performed working set recalculations. */
-				 			real_t* const cputime,				/**< Input: Maximum CPU time allowed for QP initialisation. \n
-															 		 Output: CPU time spend for QP initialisation. */
-							const real_t* const xOpt,			/**< Optimal primal solution vector.
-																	 A NULL pointer can be passed. */
-							const real_t* const yOpt,			/**< Optimal dual solution vector.
-																	 A NULL pointer can be passed. */
-							const Bounds* const guessedBounds	/**< Optimal working set for solution (xOpt,yOpt).
-																	 A NULL pointer can be passed. */
-							);
-
-
-		/** Solves an initialised QProblemB using online active set strategy.
-		 *  Note: This function internally calls solveQP/solveRegularisedQP
-		 *        for solving an initialised QP!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_MAX_NWSR_REACHED \n
-					RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
-		returnValue hotstart(	const real_t* const g_new,	/**< Gradient of neighbouring QP to be solved. */
-								const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
-													 			 If no lower bounds exist, a NULL pointer can be passed. */
-								const real_t* const ub_new,	/**< Upper bounds of neighbouring QP to be solved. \n
-													 			 If no upper bounds exist, a NULL pointer can be passed. */
-								int& nWSR,					/**< Input: Maximum number of working set recalculations; \n
-																 Output: Number of performed working set recalculations. */
-								real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP solution. \n
-																 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								);
-
-		/** Solves an initialised QProblemB online active set strategy
-		 *  reading QP data from files.
-		 *  Note: This function internally calls solveQP/solveRegularisedQP
-		 *        for solving an initialised QP!
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MAX_NWSR_REACHED \n
-		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-					RET_UNABLE_TO_READ_FILE \n
-					RET_INVALID_ARGUMENTS */
-		returnValue hotstart(	const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-								const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-													 			 If no lower bounds exist, a NULL pointer can be passed. */
-								const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-													 			 If no upper bounds exist, a NULL pointer can be passed. */
-								int& nWSR, 					/**< Input: Maximum number of working set recalculations; \n
-																 Output: Number of performed working set recalculations. */
-								real_t* const cputime = 0 	/**< Input: Maximum CPU time allowed for QP solution. \n
-																 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								);
-
-		/** Solves an initialised QProblemB using online active set strategy (using an initialised homotopy).
-		 *  Note: This function internally calls solveQP/solveRegularisedQP
-		 *        for solving an initialised QP!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_MAX_NWSR_REACHED \n
-					RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-					RET_SETUP_AUXILIARYQP_FAILED */
-		returnValue hotstart(	const real_t* const g_new,			/**< Gradient of neighbouring QP to be solved. */
-								const real_t* const lb_new,			/**< Lower bounds of neighbouring QP to be solved. \n
-													 					 If no lower bounds exist, a NULL pointer can be passed. */
-								const real_t* const ub_new,			/**< Upper bounds of neighbouring QP to be solved. \n
-													 					 If no upper bounds exist, a NULL pointer can be passed. */
-								int& nWSR,							/**< Input: Maximum number of working set recalculations; \n
-																		 Output: Number of performed working set recalculations. */
-								real_t* const cputime,				/**< Input: Maximum CPU time allowed for QP solution. \n
-																 		 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								const Bounds* const guessedBounds	/**< Initial guess for working set of bounds.
-																	 *   A null pointer corresponds to an empty working set! */
-								);
-
-		/** Solves an initialised QProblemB using online active set strategy (using an initialised homotopy)
-		 *  reading QP data from files.
-		 *  Note: This function internally calls solveQP/solveRegularisedQP
-		 *        for solving an initialised QP!
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MAX_NWSR_REACHED \n
-		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-					RET_UNABLE_TO_READ_FILE \n
-					RET_INVALID_ARGUMENTS \n
-					RET_SETUP_AUXILIARYQP_FAILED */
-		returnValue hotstart(	const char* const g_file,			/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-								const char* const lb_file,			/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-													 					 If no lower bounds exist, a NULL pointer can be passed. */
-								const char* const ub_file, 			/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-													 					 If no upper bounds exist, a NULL pointer can be passed. */
-								int& nWSR,							/**< Input: Maximum number of working set recalculations; \n
-																		 Output: Number of performed working set recalculations. */
-								real_t* const cputime,				/**< Input: Maximum CPU time allowed for QP solution. \n
-																 		 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								const Bounds* const guessedBounds	/**< Initial guess for working set of bounds.
-																	 *   A null pointer corresponds to an empty working set! */
-								);
-
-
-		/** Writes a vector with the state of the bounds
-		   *	\return SUCCESSFUL_RETURN */
-		virtual returnValue getWorkingSet(	real_t* workingSet		/** Output: array containing state of the working set. */
-											);
-
-		/** Returns current bounds object of the QP (deep copy).
-		  *	\return SUCCESSFUL_RETURN \n
-		  			RET_QPOBJECT_NOT_SETUP */
-		inline returnValue getBounds(	Bounds& _bounds	/** Output: Bounds object. */
-										) const;
-
-
-		/** Returns the number of variables.
-		 *	\return Number of variables. */
-		inline int getNV( ) const;
-
-		/** Returns the number of free variables.
-		 *	\return Number of free variables. */
-		inline int getNFR( ) const;
-
-		/** Returns the number of fixed variables.
-		 *	\return Number of fixed variables. */
-		inline int getNFX( ) const;
-
-		/** Returns the number of implicitly fixed variables.
-		 *	\return Number of implicitly fixed variables. */
-		inline int getNFV( ) const;
-
-		/** Returns the dimension of null space.
-		 *	\return Dimension of null space. */
-		virtual int getNZ( ) const;
-
-
-		/** Returns the optimal objective function value.
-		 *	\return finite value: Optimal objective function value (QP was solved) \n
-		 			+infinity:	  QP was not yet solved */
-		real_t getObjVal( ) const;
-
-		/** Returns the objective function value at an arbitrary point x.
-		 *	\return Objective function value at point x */
-		real_t getObjVal(	const real_t* const _x	/**< Point at which the objective function shall be evaluated. */
-							) const;
-
-		/** Returns the primal solution vector.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_QP_NOT_SOLVED */
-		returnValue getPrimalSolution(	real_t* const xOpt			/**< Output: Primal solution vector (if QP has been solved). */
-										) const;
-
-		/** Returns the dual solution vector.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_QP_NOT_SOLVED */
-		virtual returnValue getDualSolution(	real_t* const yOpt	/**< Output: Dual solution vector (if QP has been solved). */
-												) const;
-
-
-		/** Returns status of the solution process.
-		 *	\return Status of solution process. */
-		inline QProblemStatus getStatus( ) const;
-
-
-		/** Returns if the QProblem object is initialised.
-		 *	\return BT_TRUE:  QProblemB initialised \n
-		 			BT_FALSE: QProblemB not initialised */
-		inline BooleanType isInitialised( ) const;
-
-		/** Returns if the QP has been solved.
-		 *	\return BT_TRUE:  QProblemB solved \n
-		 			BT_FALSE: QProblemB not solved */
-		inline BooleanType isSolved( ) const;
-
-		/** Returns if the QP is infeasible.
-		 *	\return BT_TRUE:  QP infeasible \n
-		 			BT_FALSE: QP feasible (or not known to be infeasible!) */
-		inline BooleanType isInfeasible( ) const;
-
-		/** Returns if the QP is unbounded.
-		 *	\return BT_TRUE:  QP unbounded \n
-		 			BT_FALSE: QP unbounded (or not known to be unbounded!) */
-		inline BooleanType isUnbounded( ) const;
-
-
-		/** Returns Hessian type flag (type is not determined due to this call!).
-		 *	\return Hessian type. */
-		inline HessianType getHessianType( ) const;
-
-		/** Changes the print level.
- 		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setHessianType(	HessianType _hessianType /**< New Hessian type. */
-											);
-
-		/** Returns if the QP has been internally regularised.
-		 *	\return BT_TRUE:  Hessian is internally regularised for QP solution \n
-		 			BT_FALSE: No internal Hessian regularisation is used for QP solution */
-		inline BooleanType usingRegularisation( ) const;
-
-		/** Returns current options struct.
-		 *	\return Current options struct. */
-		inline Options getOptions( ) const;
-
-		/** Overrides current options with given ones.
- 		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setOptions(	const Options& _options	/**< New options. */
-										);
-
-		/** Returns the print level.
-		 *	\return Print level. */
-		inline PrintLevel getPrintLevel( ) const;
-
-		/** Changes the print level.
- 		 *	\return SUCCESSFUL_RETURN */
-		returnValue setPrintLevel(	PrintLevel _printlevel	/**< New print level. */
-									);
-
-
-		/** Prints concise list of properties of the current QP.
-		 *	\return  SUCCESSFUL_RETURN \n */
-		virtual returnValue printProperties( );
-
-		/** Prints a list of all options and their current values.
-		 *	\return  SUCCESSFUL_RETURN \n */
-		returnValue printOptions( ) const;
-
-
-	/*
-	 *	PROTECTED MEMBER FUNCTIONS
-	 */
-	protected:
-		/** Frees all allocated memory.
-		 *  \return SUCCESSFUL_RETURN */
-		returnValue clear( );
-		
-		/** Copies all members from given rhs object.
-		 *  \return SUCCESSFUL_RETURN */
-		returnValue copy(	const QProblemB& rhs	/**< Rhs object. */
-							);
-		
-		/** If Hessian type has been set by the user, nothing is done.
-		 *  Otherwise the Hessian type is set to HST_IDENTITY, HST_ZERO, or
-		 *  HST_POSDEF (default), respectively.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_HESSIAN_INDEFINITE */
-		returnValue determineHessianType( );
-
-		/** Determines type of existing constraints and bounds (i.e. implicitly fixed, unbounded etc.).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_SETUPSUBJECTTOTYPE_FAILED */
-		virtual returnValue setupSubjectToType( );
-
-		/** Determines type of new constraints and bounds (i.e. implicitly fixed, unbounded etc.).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_SETUPSUBJECTTOTYPE_FAILED */
-		virtual returnValue setupSubjectToType(	const real_t* const lb_new,	/**< New lower bounds. */
-												const real_t* const ub_new	/**< New upper bounds. */
-												);
-
-		/** Computes the Cholesky decomposition of the (simply projected) Hessian
-		 *  (i.e. R^T*R = Z^T*H*Z). It only works in the case where Z is a simple
-		 *  projection matrix!
-		 *  Note: If Hessian turns out not to be positive definite, the Hessian type
-		 *		  is set to HST_SEMIDEF accordingly.
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_HESSIAN_NOT_SPD \n
-		 *			RET_INDEXLIST_CORRUPTED */
-		returnValue computeCholesky( );
-
-
-		/** Computes initial Cholesky decomposition of the (simply projected) Hessian 
-		 *  making use of the function computeCholesky().
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_HESSIAN_NOT_SPD \n
-		 *			RET_INDEXLIST_CORRUPTED */
-		virtual returnValue setupInitialCholesky( );
-
-		/** Obtains the desired working set for the auxiliary initial QP in
-		 *  accordance with the user specifications
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_OBTAINING_WORKINGSET_FAILED \n
-					RET_INVALID_ARGUMENTS */
-		returnValue obtainAuxiliaryWorkingSet(	const real_t* const xOpt,			/**< Optimal primal solution vector.
-																					 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
-												const real_t* const yOpt,			/**< Optimal dual solution vector.
-																					 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
-												const Bounds* const guessedBounds,	/**< Guessed working set for solution (xOpt,yOpt). */
-												Bounds* auxiliaryBounds				/**< Input: Allocated bound object. \n
-																					 *	 Output: Working set for auxiliary QP. */
-												) const;
-
-		/** Decides if lower bounds are smaller than upper bounds
-		 *
-		 * \return SUCCESSFUL_RETURN \n
-		 * 		   RET_QP_INFEASIBLE */
-
-		returnValue areBoundsConsistent(const real_t* const lb, /**< Vector of lower bounds*/
-										const real_t* const ub  /**< Vector of upper bounds*/
-										) const;
-
-		/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_DIV_BY_ZERO */
-		returnValue backsolveR(	const real_t* const b,	/**< Right hand side vector. */
-								BooleanType transposed,	/**< Indicates if the transposed system shall be solved. */
-								real_t* const a 		/**< Output: Solution vector */
-								) const;
-
-		/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n
-		 *  Special variant for the case that this function is called from within "removeBound()".
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_DIV_BY_ZERO */
-		returnValue backsolveR(	const real_t* const b,		/**< Right hand side vector. */
-								BooleanType transposed,		/**< Indicates if the transposed system shall be solved. */
-								BooleanType removingBound,	/**< Indicates if function is called from "removeBound()". */
-								real_t* const a 			/**< Output: Solution vector */
-								) const;
-
-
-		/** Determines step direction of the shift of the QP data.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue determineDataShift(	const real_t* const g_new,	/**< New gradient vector. */
-										const real_t* const lb_new,	/**< New lower bounds. */
-										const real_t* const ub_new,	/**< New upper bounds. */
-										real_t* const delta_g,	 	/**< Output: Step direction of gradient vector. */
-										real_t* const delta_lb,	 	/**< Output: Step direction of lower bounds. */
-										real_t* const delta_ub,	 	/**< Output: Step direction of upper bounds. */
-										BooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */
-										);
-
-
-		/** Sets up internal QP data. 
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INVALID_ARGUMENTS */
-		returnValue setupQPdata(	SymmetricMatrix *_H,	 	/**< Hessian matrix.*/
-									const real_t* const _g,		/**< Gradient vector. */
-									const real_t* const _lb,	/**< Lower bounds (on variables). \n
-																	 If no lower bounds exist, a NULL pointer can be passed. */
-									const real_t* const _ub		/**< Upper bounds (on variables). \n
-																	 If no upper bounds exist, a NULL pointer can be passed. */
-									);
-
-		/** Sets up internal QP data. If the current Hessian is trivial
-		 *  (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
-		 *  memory for Hessian is allocated and it is set to the given one.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INVALID_ARGUMENTS \n
-					RET_NO_HESSIAN_SPECIFIED */
-		returnValue setupQPdata(	const real_t* const _H, 	/**< Hessian matrix. \n
-																     If Hessian matrix is trivial,a NULL pointer can be passed. */
-									const real_t* const _g,		/**< Gradient vector. */
-									const real_t* const _lb,	/**< Lower bounds (on variables). \n
-																	 If no lower bounds exist, a NULL pointer can be passed. */
-									const real_t* const _ub		/**< Upper bounds (on variables). \n
-																	 If no upper bounds exist, a NULL pointer can be passed. */
-									);
-
-		/** Sets up internal QP data by loading it from files. If the current Hessian
-		 *  is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
-		 *  memory for Hessian is allocated and it is set to the given one.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_UNABLE_TO_OPEN_FILE \n
-					RET_UNABLE_TO_READ_FILE \n
-					RET_INVALID_ARGUMENTS \n
-					RET_NO_HESSIAN_SPECIFIED */
-		returnValue setupQPdataFromFile(	const char* const H_file, 	/**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n
-																     		 If Hessian matrix is trivial,a NULL pointer can be passed. */
-											const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-											const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-												 			 				 If no lower bounds exist, a NULL pointer can be passed. */
-											const char* const ub_file 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-												 			 				 If no upper bounds exist, a NULL pointer can be passed. */
-											);
-
-		/** Loads new QP vectors from files (internal members are not affected!).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_UNABLE_TO_OPEN_FILE \n
-					RET_UNABLE_TO_READ_FILE \n
-					RET_INVALID_ARGUMENTS */
-		returnValue loadQPvectorsFromFile(	const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-											const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-												 			 				 If no lower bounds exist, a NULL pointer can be passed. */
-											const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-												 			 				 If no upper bounds exist, a NULL pointer can be passed. */
-											real_t* const g_new,		/**< Output: Gradient of neighbouring QP to be solved. */
-											real_t* const lb_new,		/**< Output: Lower bounds of neighbouring QP to be solved */
-											real_t* const ub_new		/**< Output: Upper bounds of neighbouring QP to be solved */
-											) const;
-
-
-		/** Sets internal infeasibility flag and throws given error in case the far bound
-		 *	strategy is not enabled (as QP might actually not be infeasible in this case).
-		 *	\return RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_ENSURELI_FAILED_CYCLING \n
-					RET_ENSURELI_FAILED_NOINDEX */
-		returnValue setInfeasibilityFlag(	returnValue returnvalue,			/**< Returnvalue to be tunneled. */
-											BooleanType doThrowError = BT_FALSE	/**< Flag forcing to throw an error. */
-											);
-
-
-		/** Determines if next QP iteration can be performed within given CPU time limit.
-		 *	\return BT_TRUE: CPU time limit is exceeded, stop QP solution. \n
-					BT_FALSE: Sufficient CPU time for next QP iteration. */
-		BooleanType isCPUtimeLimitExceeded(	const real_t* const cputime,	/**< Maximum CPU time allowed for QP solution. */
-											real_t starttime,				/**< Start time of current QP solution. */
-											int nWSR						/**< Number of working set recalculations performed so far. */
-											) const;
-
-
-		/** Regularise Hessian matrix by adding a scaled identity matrix to it.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_HESSIAN_ALREADY_REGULARISED */
-		returnValue regulariseHessian( );
-
-
-		/** Sets Hessian matrix of the QP.
-		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setH(	SymmetricMatrix* H_new	/**< New Hessian matrix (a shallow copy is made). */
-									);
-									
-		/** Sets dense Hessian matrix of the QP.
-		 *  If a null pointer is passed and
-		 *  a) hessianType is HST_IDENTITY, nothing is done,
-		 *  b) hessianType is not HST_IDENTITY, Hessian matrix is set to zero.
-		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setH(	const real_t* const H_new	/**< New dense Hessian matrix (with correct dimension!), a shallow copy is made. */
-									);
-
-		/** Changes gradient vector of the QP.
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_INVALID_ARGUMENTS */
-		inline returnValue setG(	const real_t* const g_new	/**< New gradient vector (with correct dimension!). */
-									);
-
-		/** Changes lower bound vector of the QP.
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_QPOBJECT_NOT_SETUP */
-		inline returnValue setLB(	const real_t* const lb_new	/**< New lower bound vector (with correct dimension!). */
-									);
-
-		/** Changes single entry of lower bound vector of the QP.
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_QPOBJECT_NOT_SETUP \n
-		 *			RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue setLB(	int number,		/**< Number of entry to be changed. */
-									real_t value	/**< New value for entry of lower bound vector. */
-									);
-
-		/** Changes upper bound vector of the QP.
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_QPOBJECT_NOT_SETUP */
-		inline returnValue setUB(	const real_t* const ub_new	/**< New upper bound vector (with correct dimension!). */
-									);
-
-		/** Changes single entry of upper bound vector of the QP.
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_QPOBJECT_NOT_SETUP \n
-		 *			RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue setUB(	int number,		/**< Number of entry to be changed. */
-									real_t value	/**< New value for entry of upper bound vector. */
-									);
-
-
-		/** Computes parameters for the Givens matrix G for which [x,y]*G = [z,0]
-		 *	\return SUCCESSFUL_RETURN */
-		inline void computeGivens(	real_t xold,	/**< Matrix entry to be normalised. */
-									real_t yold,	/**< Matrix entry to be annihilated. */
-									real_t& xnew,	/**< Output: Normalised matrix entry. */
-									real_t& ynew,	/**< Output: Annihilated matrix entry. */
-									real_t& c,		/**< Output: Cosine entry of Givens matrix. */
-									real_t& s 		/**< Output: Sine entry of Givens matrix. */
-									) const;
-
-		/** Applies Givens matrix determined by c and s (cf. computeGivens).
-		 *	\return SUCCESSFUL_RETURN */
-		inline void applyGivens(	real_t c,		/**< Cosine entry of Givens matrix. */
-									real_t s,		/**< Sine entry of Givens matrix. */
-									real_t nu, 		/**< Further factor: s/(1+c). */
-									real_t xold,	/**< Matrix entry to be transformed corresponding to
-													 *	 the normalised entry of the original matrix. */
-									real_t yold, 	/**< Matrix entry to be transformed corresponding to
-													 *	 the annihilated entry of the original matrix. */
-									real_t& xnew,	/**< Output: Transformed matrix entry corresponding to
-													 *	 the normalised entry of the original matrix. */
-									real_t& ynew	/**< Output: Transformed matrix entry corresponding to
-													 *	 the annihilated entry of the original matrix. */
-									) const;
-
-
-
-		/** Compute relative length of homotopy in data space for termination
-		 *  criterion.
-		 *  \return Relative length in data space. */
-		real_t getRelativeHomotopyLength(	const real_t* const g_new,		/**< Final gradient. */
-											const real_t* const lb_new,		/**< Final lower variable bounds. */
-											const real_t* const ub_new		/**< Final upper variable bounds. */
-											);
-
-		/** Ramping Strategy to avoid ties. Modifies homotopy start without
-		 *  changing current active set.
-		 *  \return SUCCESSFUL_RETURN */
-		virtual returnValue performRamping( );
-
-
-		/** ... */
-		returnValue updateFarBounds(	real_t curFarBound,					/**< ... */
-                                        int nRamp,							/**< ... */
-                                        const real_t* const lb_new,			/**< ... */
-                                        real_t* const lb_new_far,			/**< ... */
-                                        const real_t* const ub_new,			/**< ... */
-                                        real_t* const ub_new_far			/**< ... */
-                                        ) const;
-
-
-		/** Performs robustified ratio test yield the maximum possible step length
-		 *  along the homotopy path.
-		 *	\return  SUCCESSFUL_RETURN */
-		returnValue performRatioTest(	int nIdx, 							/**< Number of ratios to be checked. */
-										const int* const idxList, 			/**< Array containing the indices of all ratios to be checked. */
-										const SubjectTo* const subjectTo,	/**< Bound/Constraint object corresponding to ratios to be checked. */
-										const real_t* const num,	 		/**< Array containing all numerators for performing the ratio test. */
-										const real_t* const den,		 	/**< Array containing all denominators for performing the ratio test. */
-										real_t epsNum,						/**< Numerator tolerance. */
-										real_t epsDen,						/**< Denominator tolerance. */
-										real_t& t,							/**< Output: Maximum possible step length along the homotopy path. */
-										int& BC_idx 						/**< Output: Index of blocking constraint. */
-										) const;
-
-		/** Checks whether given ratio is blocking, i.e. limits the maximum step length
-		 *  along the homotopy path to a value lower than given one.
-		 *	\return  SUCCESSFUL_RETURN */
-		inline BooleanType isBlocking(	real_t num,					 		/**< Numerator for performing the ratio test. */
-										real_t den,		 					/**< Denominator for performing the ratio test. */
-										real_t epsNum,						/**< Numerator tolerance. */
-										real_t epsDen,						/**< Denominator tolerance. */
-										real_t& t							/**< Input: Current maximum step length along the homotopy path,
-																			 *   Output: Updated maximum possible step length along the homotopy path. */
-										) const;
-
-
-		/** Creates a sparse diagonal (square-)matrix which is a given 
-		 *  multiple of the identity matrix.
-		 *  \return Diagonal matrix \n
-		 */
-		SymSparseMat* createDiagSparseMat(	int n,					/**< Row/column dimension of matrix to be created. */
-											real_t diagVal = 1.0	/**< Value of all diagonal entries. */
-											);
-
-
-	/*
-	 *	PRIVATE MEMBER FUNCTIONS
-	 */
-	private:
-		/** Solves a QProblemB whose QP data is assumed to be stored in the member variables.
-		 *  A guess for its primal/dual optimal solution vectors and the corresponding
-		 *  optimal working set can be provided.
-		 *  Note: This function is internally called by all init functions!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED */
-		returnValue solveInitialQP(	const real_t* const xOpt,			/**< Optimal primal solution vector.
-																		 *	 A NULL pointer can be passed. */
-									const real_t* const yOpt,			/**< Optimal dual solution vector.
-																		 *	 A NULL pointer can be passed. */
-									const Bounds* const guessedBounds,	/**< Guessed working set for solution (xOpt,yOpt).
-																		 *	 A NULL pointer can be passed. */
-									int& nWSR, 							/**< Input: Maximum number of working set recalculations; \n
-																 		 *	 Output: Number of performed working set recalculations. */
-									real_t* const cputime				/**< Input: Maximum CPU time allowed for QP solution. \n
-																 			 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-									);
-
-		/** Solves an initialised QProblemB using online active set strategy.
-		 *  Note: This function is internally called by all hotstart functions!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_MAX_NWSR_REACHED \n
-					RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
-		returnValue solveQP(	const real_t* const g_new,	/**< Gradient of neighbouring QP to be solved. */
-								const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
-													 			 If no lower bounds exist, a NULL pointer can be passed. */
-								const real_t* const ub_new,	/**< Upper bounds of neighbouring QP to be solved. \n
-													 			 If no upper bounds exist, a NULL pointer can be passed. */
-								int& nWSR,					/**< Input: Maximum number of working set recalculations; \n
-																 Output: Number of performed working set recalculations. */
-								real_t* const cputime,		/**< Input: Maximum CPU time allowed for QP solution. \n
-																 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								int  nWSRperformed = 0		/**< Number of working set recalculations already performed to solve
-																 this QP within previous solveQP() calls. This number is
-																 always zero, except for successive calls from solveRegularisedQP()
-																 or when using the far bound strategy. */
-								);
-
-
-		/** Solves an initialised QProblemB using online active set strategy.
-		 *  Note: This function is internally called by all hotstart functions!
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_MAX_NWSR_REACHED \n
-					RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
-		returnValue solveRegularisedQP(	const real_t* const g_new,	/**< Gradient of neighbouring QP to be solved. */
-										const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
-															 			 If no lower bounds exist, a NULL pointer can be passed. */
-										const real_t* const ub_new,	/**< Upper bounds of neighbouring QP to be solved. \n
-															 			 If no upper bounds exist, a NULL pointer can be passed. */
-										int& nWSR,					/**< Input: Maximum number of working set recalculations; \n
-																		 Output: Number of performed working set recalculations. */
-										real_t* const cputime,		/**< Input: Maximum CPU time allowed for QP solution. \n
-																		 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-										int  nWSRperformed = 0		/**< Number of working set recalculations already performed to solve
-																		 this QP within previous solveRegularisedQP() calls. This number is
-																		 always zero, except for successive calls when using the far bound strategy. */
-										);
-
-
-		/** Sets up bound data structure according to auxiliaryBounds.
-		 *  (If the working set shall be setup afresh, make sure that
-		 *  bounds data structure has been resetted!)
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_SETUP_WORKINGSET_FAILED \n
-					RET_INVALID_ARGUMENTS \n
-					RET_UNKNOWN_BUG */
-		returnValue setupAuxiliaryWorkingSet(	const Bounds* const auxiliaryBounds,	/**< Working set for auxiliary QP. */
-												BooleanType setupAfresh					/**< Flag indicating if given working set shall be
-																						 *    setup afresh or by updating the current one. */
-												);
-
-		/** Sets up the optimal primal/dual solution of the auxiliary initial QP.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue setupAuxiliaryQPsolution(	const real_t* const xOpt,			/**< Optimal primal solution vector.
-																				 	*	 If a NULL pointer is passed, all entries are set to zero. */
-												const real_t* const yOpt			/**< Optimal dual solution vector.
-																					 *	 If a NULL pointer is passed, all entries are set to zero. */
-												);
-
-		/** Sets up gradient of the auxiliary initial QP for given
-		 *  optimal primal/dual solution and given initial working set
-		 *  (assumes that members X, Y and BOUNDS have already been initialised!).
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue setupAuxiliaryQPgradient( );
-
-		/** Sets up bounds of the auxiliary initial QP for given
-		 *  optimal primal/dual solution and given initial working set
-		 *  (assumes that members X, Y and BOUNDS have already been initialised!).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_UNKNOWN_BUG */
-		returnValue setupAuxiliaryQPbounds( BooleanType useRelaxation	/**< Flag indicating if inactive bounds shall be relaxed. */
-											);
-
-
-		/** Updates QP vectors, working sets and internal data structures in order to
-			start from an optimal solution corresponding to initial guesses of the working
-			set for bounds
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_SETUP_AUXILIARYQP_FAILED */
-		returnValue setupAuxiliaryQP(	const Bounds* const guessedBounds	/**< Initial guess for working set of bounds. */
-										);
-
-		/** Determines step direction of the homotopy path.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_STEPDIRECTION_FAILED_CHOLESKY */
-		returnValue determineStepDirection(	const real_t* const delta_g,	/**< Step direction of gradient vector. */
-											const real_t* const delta_lb,	/**< Step direction of lower bounds. */
-											const real_t* const delta_ub,	/**< Step direction of upper bounds. */
-											BooleanType Delta_bB_isZero,	/**< Indicates if active bounds are to be shifted. */
-											real_t* const delta_xFX, 		/**< Output: Primal homotopy step direction of fixed variables. */
-											real_t* const delta_xFR,	 	/**< Output: Primal homotopy step direction of free variables. */
-											real_t* const delta_yFX 		/**< Output: Dual homotopy step direction of fixed variables' multiplier. */
-											);
-
-		/** Determines the maximum possible step length along the homotopy path
-		 *  and performs this step (without changing working set).
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_QP_INFEASIBLE \n
-		 */
-		returnValue performStep(	const real_t* const delta_g,	/**< Step direction of gradient. */
-									const real_t* const delta_lb,	/**< Step direction of lower bounds. */
-									const real_t* const delta_ub,	/**< Step direction of upper bounds. */
-									const real_t* const delta_xFX, 	/**< Primal homotopy step direction of fixed variables. */
-									const real_t* const delta_xFR,	/**< Primal homotopy step direction of free variables. */
-									const real_t* const delta_yFX,	/**< Dual homotopy step direction of fixed variables' multiplier. */
-									int& BC_idx, 					/**< Output: Index of blocking constraint. */
-									SubjectToStatus& BC_status		/**< Output: Status of blocking constraint. */
-									);
-
-		/** Updates active set.
-		 *	\return  SUCCESSFUL_RETURN \n
-		 			 RET_REMOVE_FROM_ACTIVESET_FAILED \n
-					 RET_ADD_TO_ACTIVESET_FAILED */
-		returnValue changeActiveSet(	int BC_idx, 						/**< Index of blocking constraint. */
-										SubjectToStatus BC_status 			/**< Status of blocking constraint. */
-										);
-
-		/** Drift correction at end of each active set iteration
-		 *  \return SUCCESSFUL_RETURN */
-		virtual returnValue performDriftCorrection( );
-
-		/** Determines if it is more efficient to refactorise the matrices when
-		 *  hotstarting or not (i.e. better to update the existing factorisations).
-		 *	\return BT_TRUE iff matrices shall be refactorised afresh
-		 */
-		BooleanType shallRefactorise(	const Bounds* const guessedBounds	/**< Guessed new working set. */
-										) const;
-
-
-		/** Adds a bound to active set (specialised version for the case where no constraints exist).
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_ADDBOUND_FAILED */
-		returnValue addBound(	int number,					/**< Number of bound to be added to active set. */
-								SubjectToStatus B_status,	/**< Status of new active bound. */
-								BooleanType updateCholesky	/**< Flag indicating if Cholesky decomposition shall be updated. */
-								);
-
-		/** Removes a bounds from active set (specialised version for the case where no constraints exist).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_HESSIAN_NOT_SPD \n
-					RET_REMOVEBOUND_FAILED */
-		returnValue removeBound(	int number,					/**< Number of bound to be removed from active set. */
-									BooleanType updateCholesky	/**< Flag indicating if Cholesky decomposition shall be updated. */
-									);
-
-
-		/** Prints concise information on the current iteration.
-		 *	\return  SUCCESSFUL_RETURN \n */
-		returnValue printIteration(	int iter,					/**< Number of current iteration. */
-									int BC_idx, 				/**< Index of blocking bound. */
-									SubjectToStatus BC_status,	/**< Status of blocking bound. */
-									real_t homotopyLength		/**< Current homotopy distance. */
-									);
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-		BooleanType freeHessian;	/**< Flag indicating whether the Hessian matrix needs to be de-allocated. */
-		SymmetricMatrix* H;			/**< Hessian matrix. */
-
-		real_t* g;					/**< Gradient. */
-		real_t* lb;					/**< Lower bound vector (on variables). */
-		real_t* ub;					/**< Upper bound vector (on variables). */
-
-		Bounds bounds;				/**< Data structure for problem's bounds. */
-
-		real_t* R;					/**< Cholesky factor of H (i.e. H = R^T*R). */
-		BooleanType haveCholesky;	/**< Flag indicating whether Cholesky decomposition has already been setup. */
-
-		real_t* x;					/**< Primal solution vector. */
-		real_t* y;					/**< Dual solution vector. */
-
-		real_t tau;					/**< Last homotopy step length. */
-
-		QProblemStatus status;		/**< Current status of the solution process. */
-
-		BooleanType infeasible;		/**< QP infeasible? */
-		BooleanType unbounded;		/**< QP unbounded? */
-
-		HessianType hessianType;	/**< Type of Hessian matrix. */
-		real_t regVal;				/**< Holds the offset used to regularise Hessian matrix (zero by default). */
-
-		int count;					/**< Counts the number of hotstart function calls (internal usage only!). */
-
-		real_t *delta_xFR_TMP;		/**< Temporary for determineStepDirection */
-
-		real_t ramp0;				/**< Start value for Ramping Strategy. */
-		real_t ramp1;				/**< Final value for Ramping Strategy. */
-		int rampOffset;				/**< Offset index for Ramping. */
-
-		Options options;			/**< Struct containing all user-defined options for solving QPs. */
-		
-		Flipper flipper;			/**< Struct for making a temporary copy of the matrix factorisations. */
-
-		TabularOutput tabularOutput;	/**< Struct storing information for tabular output (printLevel == PL_TABULAR). */
-};
-
-
-END_NAMESPACE_QPOASES
-
-#include <qpOASES/QProblemB.ipp>
-
-#endif	/* QPOASES_QPROBLEMB_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblemB.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblemB.ipp
deleted file mode 100644
index 2a0922cae5e..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/QProblemB.ipp
+++ /dev/null
@@ -1,477 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/QProblemB.ipp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of inlined member functions of the QProblemB class which
- *	is able to use the newly developed online active set strategy for
- *	parametric quadratic programming.
- */
-
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-/*
- *	g e t B o u n d s
- */
-inline returnValue QProblemB::getBounds( Bounds& _bounds ) const
-{
-	int nV = getNV( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	_bounds = bounds;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t N V
- */
-inline int QProblemB::getNV( ) const
-{
-	return bounds.getNV( );
-}
-
-
-/*
- *	g e t N F R
- */
-inline int QProblemB::getNFR( ) const
-{
-	return bounds.getNFR( );
-}
-
-
-/*
- *	g e t N F X
- */
-inline int QProblemB::getNFX( ) const
-{
-	return bounds.getNFX( );
-}
-
-
-/*
- *	g e t N F V
- */
-inline int QProblemB::getNFV( ) const
-{
-	return bounds.getNFV( );
-}
-
-
-/*
- *	g e t S t a t u s
- */
-inline QProblemStatus QProblemB::getStatus( ) const
-{
-	return status;
-}
-
-
-/*
- *	i s I n i t i a l i s e d
- */
-inline BooleanType QProblemB::isInitialised( ) const
-{
-	if ( status == QPS_NOTINITIALISED )
-		return BT_FALSE;
-	else
-		return BT_TRUE;
-}
-
-
-/*
- *	i s S o l v e d
- */
-inline BooleanType QProblemB::isSolved( ) const
-{
-	if ( status == QPS_SOLVED )
-		return BT_TRUE;
-	else
-		return BT_FALSE;
-}
-
-
-/*
- *	i s I n f e a s i b l e
- */
-inline BooleanType QProblemB::isInfeasible( ) const
-{
-	return infeasible;
-}
-
-
-/*
- *	i s U n b o u n d e d
- */
-inline BooleanType QProblemB::isUnbounded( ) const
-{
-	return unbounded;
-}
-
-
-/*
- *	g e t H e s s i a n T y p e
- */
-inline HessianType QProblemB::getHessianType( ) const
-{
-	return hessianType;
-}
-
-
-/*
- *	s e t H e s s i a n T y p e
- */
-inline returnValue QProblemB::setHessianType( HessianType _hessianType )
-{
-	hessianType = _hessianType;
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	u s i n g R e g u l a r i s a t i o n
- */
-inline BooleanType QProblemB::usingRegularisation( ) const
-{
-	if ( regVal > ZERO )
-		return BT_TRUE;
-	else
-		return BT_FALSE;
-}
-
-
-/*
- *	g e t O p t i o n s
- */
-inline Options QProblemB::getOptions( ) const
-{
-	return options;
-}
-
-
-/*
- *	s e t O p t i o n s
- */
-inline returnValue QProblemB::setOptions(	const Options& _options
-											)
-{
-	options = _options;
-	options.ensureConsistency( );
-
-	setPrintLevel( options.printLevel );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t P r i n t L e v e l
- */
-inline PrintLevel QProblemB::getPrintLevel( ) const
-{
-	return options.printLevel;
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-/*
- *	s e t H
- */
-inline returnValue QProblemB::setH( SymmetricMatrix* H_new )
-{
-	if ( ( freeHessian == BT_TRUE ) && ( H != 0 ) )
-	{
-		delete H;
-		H = 0;
-	}
-
-	H = H_new;
-	freeHessian = BT_FALSE;
-	
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t H
- */
-inline returnValue QProblemB::setH( const real_t* const H_new )
-{
-	int nV = getNV();
-	SymDenseMat* dH;
-
-	/* if null pointer is passed, Hessian is set to zero matrix
-	 *                            (or stays identity matrix) */
-	if ( H_new == 0 )
-	{
-		if ( hessianType == HST_IDENTITY )
-			return SUCCESSFUL_RETURN;
-
-		hessianType = HST_ZERO;
-
-		if ( ( freeHessian == BT_TRUE ) && ( H != 0 ) )
-			delete H;
-
-		H = 0;
-		freeHessian = BT_FALSE;
-	}
-	else
-	{
-		if ( ( freeHessian == BT_TRUE ) && ( H != 0 ) )
-			delete H;
-
-		H = dH = new SymDenseMat( nV, nV, nV, (real_t*) H_new );
-		freeHessian = BT_TRUE;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t G
- */
-inline returnValue QProblemB::setG( const real_t* const g_new )
-{
-	unsigned int nV = getNV( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( g_new == 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	memcpy( g,g_new,nV*sizeof(real_t) );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t L B
- */
-inline returnValue QProblemB::setLB( const real_t* const lb_new )
-{
-	unsigned int i;
-	unsigned int nV = getNV( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( lb_new != 0 )
-	{
-		memcpy( lb,lb_new,nV*sizeof(real_t) );
-	}
-	else
-	{
-		/* if no lower bounds are specified, set them to -infinity */
-		for( i=0; i<nV; ++i )
-			lb[i] = -INFTY;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t L B
- */
-inline returnValue QProblemB::setLB( int number, real_t value )
-{
-	int nV = getNV( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( ( number >= 0 ) && ( number < nV ) )
-	{
-		lb[number] = value;
-		return SUCCESSFUL_RETURN;
-	}
-	else
-	{
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-	}
-}
-
-
-/*
- *	s e t U B
- */
-inline returnValue QProblemB::setUB( const real_t* const ub_new )
-{
-	unsigned int i;
-	unsigned int nV = getNV( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( ub_new != 0 )
-	{
-		memcpy( ub,ub_new,nV*sizeof(real_t) );
-	}
-	else
-	{
-		/* if no upper bounds are specified, set them to infinity */
-		for( i=0; i<nV; ++i )
-			ub[i] = INFTY;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t U B
- */
-inline returnValue QProblemB::setUB( int number, real_t value )
-{
-	int nV = getNV( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( ( number >= 0 ) && ( number < nV ) )
-	{
-		ub[number] = value;
-
-		return SUCCESSFUL_RETURN;
-	}
-	else
-	{
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-	}
-}
-
-
-/*
- *	c o m p u t e G i v e n s
- */
-inline void QProblemB::computeGivens(	real_t xold, real_t yold, real_t& xnew, real_t& ynew,
-										real_t& c, real_t& s
-										) const
-{
-	real_t t, mu;
-
-	if ( isZero( yold ) == BT_TRUE )
-	{
-		c = 1.0;
-		s = 0.0;
-
-		xnew = xold;
-		ynew = yold;
-	}
-	else
-	{
-		mu = getAbs( xold );
-		if ( getAbs( yold ) > mu )
-			mu = getAbs( yold );
-
-		t = mu * getSqrt( (xold/mu)*(xold/mu) + (yold/mu)*(yold/mu) );
-
-		if ( xold < 0.0 )
-		t = -t;
-
-		c = xold/t;
-		s = yold/t;
-		xnew = t;
-		ynew = 0.0;
-	}
-
-	return;
-}
-
-
-/*
- *	a p p l y G i v e n s
- */
-inline void QProblemB::applyGivens(	real_t c, real_t s, real_t nu, real_t xold, real_t yold,
-									real_t& xnew, real_t& ynew
-									) const
-{
-	#ifdef __USE_THREE_MULTS_GIVENS__
-
-	/* Givens plane rotation requiring only three multiplications,
-	 * cf. Hammarling, S.: A note on modifications to the givens plane rotation.
-	 * J. Inst. Maths Applics, 13:215-218, 1974. */
-	xnew = xold*c + yold*s;
-	ynew = (xnew+xold)*nu - yold;
-
-	#else
-
-	/* Usual Givens plane rotation requiring four multiplications. */
-	xnew =  c*xold + s*yold;
-	ynew = -s*xold + c*yold;
-
-	#endif
-
-	return;
-}
-
-
-/*
- * i s B l o c k i n g
- */
-inline BooleanType QProblemB::isBlocking(	real_t num,
-											real_t den,
-											real_t epsNum,
-											real_t epsDen,
-											real_t& t
-											) const
-{
-	if ( ( den >= epsDen ) && ( num >= epsNum ) )
-	{
-		if ( num < t*den )
-			return BT_TRUE;
-	}
-
-	return BT_FALSE;
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SQProblem.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SQProblem.hpp
deleted file mode 100644
index aec5adab23f..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SQProblem.hpp
+++ /dev/null
@@ -1,394 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/SQProblem.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Declaration of the SQProblem class which is able to use the newly
- *	developed online active set strategy for parametric quadratic programming
- *	with varying matrices.
- */
-
-
-
-#ifndef QPOASES_SQPROBLEM_HPP
-#define QPOASES_SQPROBLEM_HPP
-
-
-#include <qpOASES/QProblem.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/** 
- *	\brief Implements the online active set strategy for QPs with varying matrices.
- *
- *	A class for setting up and solving quadratic programs with varying QP matrices.
- *	The main feature is the possibily to use the newly developed online active set strategy
- *	for parametric quadratic programming.
- *
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- */
-class SQProblem : public QProblem
-{
-	/* allow SolutionAnalysis class to access private members */
-	friend class SolutionAnalysis;
-
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		SQProblem( );
-
-		/** Constructor which takes the QP dimension and Hessian type
-		 *  information. If the Hessian is the zero (i.e. HST_ZERO) or the
-		 *  identity matrix (i.e. HST_IDENTITY), respectively, no memory
-		 *  is allocated for it and a NULL pointer can be passed for it
-		 *  to the init() functions. */
-		SQProblem(	int _nV,	  							/**< Number of variables. */
-					int _nC,  								/**< Number of constraints. */
-					HessianType _hessianType = HST_UNKNOWN	/**< Type of Hessian matrix. */
-					);
-
-		/** Copy constructor (deep copy). */
-		SQProblem(	const SQProblem& rhs	/**< Rhs object. */
-					);
-
-		/** Destructor. */
-		virtual ~SQProblem( );
-
-		/** Assignment operator (deep copy). */
-		SQProblem& operator=(	const SQProblem& rhs	/**< Rhs object. */
-								);
-
-
-		/** Solves an initialised SQProblem using online active set strategy.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MAX_NWSR_REACHED \n
-		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_MATRIX_SHIFT_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-					RET_SETUP_AUXILIARYQP_FAILED */
-		returnValue hotstart(	const real_t* const H_new,	/**< Hessian matrix of neighbouring QP to be solved (a shallow copy is made). \n
-																 If Hessian matrix is trivial, a NULL pointer can be passed. */
-								const real_t* const g_new,	/**< Gradient of neighbouring QP to be solved. */
-								const real_t* const A_new,	/**< Constraint matrix of neighbouring QP to be solved (a shallow copy is made). \n
-																 If QP sequence does not involve constraints, a NULL pointer can be passed. */
-								const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
-													 		 	 If no lower bounds exist, a NULL pointer can be passed. */
-								const real_t* const ub_new,	/**< Upper bounds of neighbouring QP to be solved. \n
-													 		 	 If no upper bounds exist, a NULL pointer can be passed. */
-								const real_t* const lbA_new,/**< Lower constraints' bounds of neighbouring QP to be solved. \n
-													 		 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const real_t* const ubA_new,/**< Upper constraints' bounds of neighbouring QP to be solved. \n
-												 			 	 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int& nWSR,					/**< Input: Maximum number of working set recalculations; \n
-																 Output: Number of performed working set recalculations. */
-								real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP solution. \n
-																 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								);
-
-		/** Solves an initialised SQProblem using online active set strategy
-		 *  reading QP data from files.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MAX_NWSR_REACHED \n
-		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_MATRIX_SHIFT_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-					RET_SETUP_AUXILIARYQP_FAILED \n
-					RET_UNABLE_TO_READ_FILE \n
-					RET_INVALID_ARGUMENTS */
-		returnValue hotstart(	const char* const H_file,	/**< Name of file where Hessian matrix is stored. \n
-																 If Hessian matrix is trivial, a NULL pointer can be passed. */
-								const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-								const char* const A_file,	/**< Name of file where constraint matrix is stored. \n
-																 If QP sequence does not involve constraints, a NULL pointer can be passed. */
-								const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-													 			 If no lower bounds exist, a NULL pointer can be passed. */
-								const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-													 			 If no upper bounds exist, a NULL pointer can be passed. */
-								const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
-													 			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
-													 			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int& nWSR,					/**< Input: Maximum number of working set recalculations; \n
-																 Output: Number of performed working set recalculations. */
-								real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP solution. \n
-																 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								);
-
-		/** Solves an initialised SQProblem using online active set strategy.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MAX_NWSR_REACHED \n
-		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_MATRIX_SHIFT_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-					RET_SETUP_AUXILIARYQP_FAILED */
-		returnValue hotstart(	SymmetricMatrix *H_new,		/**< Hessian matrix of neighbouring QP to be solved (a shallow copy is made). \n
-																 If Hessian matrix is trivial, a NULL pointer can be passed. */
-								const real_t* const g_new,	/**< Gradient of neighbouring QP to be solved. */
-								Matrix *A_new,				/**< Constraint matrix of neighbouring QP to be solved (a shallow copy is made). \n
-																 If QP sequence does not involve constraints, a NULL pointer can be passed. */
-								const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
-													 		 	 If no lower bounds exist, a NULL pointer can be passed. */
-								const real_t* const ub_new,	/**< Upper bounds of neighbouring QP to be solved. \n
-													 		 	 If no upper bounds exist, a NULL pointer can be passed. */
-								const real_t* const lbA_new,/**< Lower constraints' bounds of neighbouring QP to be solved. \n
-													 		 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const real_t* const ubA_new,/**< Upper constraints' bounds of neighbouring QP to be solved. \n
-												 			 	 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int& nWSR,					/**< Input: Maximum number of working set recalculations; \n
-																 Output: Number of performed working set recalculations. */
-								real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP solution. \n
-																 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								);
-
-		/** Solves an initialised SQProblem (without matrix shift) using online active set strategy.
-		 *  Note: This functions just forwards to the corresponding
-		 *  	  QProblem::hotstart member function.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MAX_NWSR_REACHED \n
-		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
-		returnValue hotstart(	const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
-								const real_t* const lb_new,		/**< Lower bounds of neighbouring QP to be solved. \n
-													 			 	 If no lower bounds exist, a NULL pointer can be passed. */
-								const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
-													 			 	 If no upper bounds exist, a NULL pointer can be passed. */
-								const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
-													 			 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
-													 			 	 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int& nWSR,						/**< Input: Maximum number of working set recalculations; \n
-																	 Output: Number of performed working set recalculations. */
-								real_t* const cputime = 0		/**< Input: Maximum CPU time allowed for QP solution. \n
-																	 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								);
-
-		/** Solves an initialised SQProblem (without matrix shift) using online active set strategy
-		 *  reading QP data from files.
-		 *  Note: This functions just forwards to the corresponding
-		 *  	  QProblem::hotstart member function.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MAX_NWSR_REACHED \n
-		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-					RET_UNABLE_TO_READ_FILE */
-		returnValue hotstart(	const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-								const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-													 			 If no lower bounds exist, a NULL pointer can be passed. */
-								const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-													 			 If no upper bounds exist, a NULL pointer can be passed. */
-								const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
-													 			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
-													 			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int& nWSR, 					/**< Input: Maximum number of working set recalculations; \n
-																 Output: Number of performed working set recalculations. */
-								real_t* const cputime = 0	/**< Input: Maximum CPU time allowed for QP solution. \n
-																 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								);
-
-		/** Solves an initialised SQProblem (without matrix shift) using online active set strategy
-		 *  (using an initialised homotopy).
-		 *  Note: This functions just forwards to the corresponding
-		 *  	  QProblem::hotstart member function.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MAX_NWSR_REACHED \n
-		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-					RET_WORKINGSET_UPDATE_FAILED */
-		returnValue hotstart(	const real_t* const g_new,					/**< Gradient of neighbouring QP to be solved. */
-								const real_t* const lb_new,					/**< Lower bounds of neighbouring QP to be solved. \n
-													 						 	 If no lower bounds exist, a NULL pointer can be passed. */
-								const real_t* const ub_new,					/**< Upper bounds of neighbouring QP to be solved. \n
-													 						 	 If no upper bounds exist, a NULL pointer can be passed. */
-								const real_t* const lbA_new,				/**< Lower constraints' bounds of neighbouring QP to be solved. \n
-													 						 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const real_t* const ubA_new,				/**< Upper constraints' bounds of neighbouring QP to be solved. \n
-													 							 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int& nWSR,									/**< Input: Maximum number of working set recalculations; \n
-																				 Output: Number of performed working set recalculations. */
-								real_t* const cputime,						/**< Input: Maximum CPU time allowed for QP solution. \n
-																 	 			 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								const Bounds* const guessedBounds,			/**< Initial guess for working set of bounds.
-																			 *   A null pointer corresponds to an empty working set! */
-								const Constraints* const guessedConstraints	/**< Initial guess for working set of constraints.
-																			 *   A null pointer corresponds to an empty working set! */
-								);
-
-		/** Solves an initialised SQProblem (without matrix shift) using online active set strategy
-		 * (using an initialised homotopy) reading QP data from files.
-		 *  Note: This functions just forwards to the corresponding
-		 *  	  QProblem::hotstart member function.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MAX_NWSR_REACHED \n
-		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-					RET_WORKINGSET_UPDATE_FAILED \n
-					RET_UNABLE_TO_READ_FILE */
-		returnValue hotstart(	const char* const g_file, 					/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-								const char* const lb_file, 					/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-													 							 If no lower bounds exist, a NULL pointer can be passed. */
-								const char* const ub_file, 					/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-													 							 If no upper bounds exist, a NULL pointer can be passed. */
-								const char* const lbA_file, 				/**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
-													 							 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const char* const ubA_file, 				/**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
-													 							 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int& nWSR,									/**< Input: Maximum number of working set recalculations; \n
-																		 		 Output: Number of performed working set recalculations. */
-								real_t* const cputime,						/**< Input: Maximum CPU time allowed for QP solution. \n
-																 	 			 Output: CPU time spend for QP solution (or to perform nWSR iterations). */
-								const Bounds* const guessedBounds,			/**< Initial guess for working set of bounds.
-																			 *   A null pointer corresponds to an empty working set! */
-								const Constraints* const guessedConstraints	/**< Initial guess for working set of constraints.
-																			 *   A null pointer corresponds to an empty working set! */
-								);
-
-
-
-		#ifdef __MATLAB__
-		/** Sets pointer of Hessian and constraint matrix to zero.
-		 *  QUICK HACK FOR MAKING THE MATLAB INTERFACE RUN! TO BE REMOVED!
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue resetMatrixPointers( );
-		#endif
-
-
-	/*
-	 *	PROTECTED MEMBER FUNCTIONS
-	 */
-	protected:
-		/** Sets new matrices and calculates their factorisations. If the
-		 *  current Hessian is trivial (i.e. HST_ZERO or HST_IDENTITY) but a
-		 *  non-trivial one is given, memory for Hessian is allocated and
-		 *  it is set to the given one. Afterwards, all QP vectors are
-		 *  transformed in order to start from an optimal solution.
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_MATRIX_FACTORISATION_FAILED \n
-		 * 			RET_NO_HESSIAN_SPECIFIED */
-		virtual returnValue setupAuxiliaryQP(	const real_t* const H_new,	/**< New Hessian matrix. \n
-																     		 	 If Hessian matrix is trivial, a NULL pointer can be passed. */
-												const real_t* const A_new,	/**< New constraint matrix. \n
-																     		 	 If QP sequence does not involve constraints, a NULL pointer can be passed. */
-												const real_t *lb_new,		/**< New lower bounds. \n
-													 						 	 If no lower bounds exist, a NULL pointer can be passed. */
-												const real_t *ub_new,		/**< New upper bounds. \n
-													 						 	 If no lower bounds exist, a NULL pointer can be passed. */
-												const real_t *lbA_new,		/**< New lower constraints' bounds. \n
-													 						 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-												const real_t *ubA_new		/**< New lower constraints' bounds. \n
-													 						 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-												);
-
-		/** Sets new matrices and calculates their factorisations. If the
-		 *  current Hessian is trivial (i.e. HST_ZERO or HST_IDENTITY) but a
-		 *  non-trivial one is given, memory for Hessian is allocated and
-		 *  it is set to the given one. Afterwards, all QP vectors are
-		 *  transformed in order to start from an optimal solution.
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_MATRIX_FACTORISATION_FAILED \n
-		 * 			RET_NO_HESSIAN_SPECIFIED */
-		virtual returnValue setupAuxiliaryQP(	SymmetricMatrix *H_new,		/**< New Hessian matrix. \n
-																				 If Hessian matrix is trivial, a NULL pointer can be passed. */
-												Matrix *A_new,				/**< New constraint matrix. \n
-																				 If QP sequence does not involve constraints, a NULL pointer can be passed. */
-												const real_t *lb_new,		/**< New lower bounds. \n
-													 						 	 If no lower bounds exist, a NULL pointer can be passed. */
-												const real_t *ub_new,		/**< New upper bounds. \n
-													 						 	 If no lower bounds exist, a NULL pointer can be passed. */
-												const real_t *lbA_new,		/**< New lower constraints' bounds. \n
-													 						 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-												const real_t *ubA_new		/**< New lower constraints' bounds. \n
-													 						 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-												);
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-
-};
-
-
-END_NAMESPACE_QPOASES
-
-#include <qpOASES/SQProblem.ipp>
-
-#endif	/* QPOASES_SQPROBLEM_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SQProblem.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SQProblem.ipp
deleted file mode 100644
index 61a88faeb32..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SQProblem.ipp
+++ /dev/null
@@ -1,51 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/SQProblem.ipp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of inlined member functions of the SQProblem class which
- *	is able to use the newly developed online active set strategy for
- *	parametric quadratic programming with varying matrices.
- */
-
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SubjectTo.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SubjectTo.hpp
deleted file mode 100644
index 9d3d4bea812..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SubjectTo.hpp
+++ /dev/null
@@ -1,229 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/SubjectTo.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Declaration of the SubjectTo class designed to manage working sets of
- *	constraints and bounds within a QProblem.
- */
-
-
-#ifndef QPOASES_SUBJECTTO_HPP
-#define QPOASES_SUBJECTTO_HPP
-
-
-#include <qpOASES/Indexlist.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/** 
- *	\brief Base class for managing working sets of bounds and constraints.
- *
- *	This class manages working sets of bounds and constraints by storing
- *	index sets and other status information.
- *
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- */
-class SubjectTo
-{
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		SubjectTo( );
-
-		/** Constructor which takes the number of constraints or bounds. */
-		SubjectTo(	int _n 								/**< Number of constraints or bounds. */
-					);
-
-		/** Copy constructor (deep copy). */
-		SubjectTo(	const SubjectTo& rhs				/**< Rhs object. */
-					);
-
-		/** Destructor. */
-		virtual ~SubjectTo( );
-
-		/** Assignment operator (deep copy). */
-		SubjectTo& operator=(	const SubjectTo& rhs	/**< Rhs object. */
-								);
-
-
-		/** Initialises object with given number of constraints or bounds.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INVALID_ARGUMENTS */
-		returnValue init(	int _n = 0					/**< Number of constraints or bounds. */
-							);
-
-
-		/** Returns number of constraints/bounds with given SubjectTo type.
-		 *	\return Number of constraints/bounds with given type. */
-		inline int getNumberOfType(	SubjectToType _type	/**< Type of (constraints') bound. */
-									) const;
-
-
-		/** Returns type of (constraints') bound.
-		 *	\return Type of (constraints') bound \n
-		 			RET_INDEX_OUT_OF_BOUNDS */
-		inline SubjectToType getType(	int i			/**< Number of (constraints') bound. */
-										) const;
-
-		/** Returns status of (constraints') bound.
-		 *	\return Status of (constraints') bound \n
-		 			ST_UNDEFINED */
-		inline SubjectToStatus getStatus(	int i		/**< Number of (constraints') bound. */
-											) const;
-
-
-		/** Sets type of (constraints') bound.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue setType(	int i,				/**< Number of (constraints') bound. */
-									SubjectToType value	/**< Type of (constraints') bound. */
-									);
-
-		/** Sets status of (constraints') bound.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue setStatus(	int i,					/**< Number of (constraints') bound. */
-										SubjectToStatus value	/**< Status of (constraints') bound. */
-										);
-
-
-		/** Sets status of lower (constraints') bounds. */
-		inline void setNoLower(	BooleanType _status		/**< Status of lower (constraints') bounds. */
-								);
-
-		/** Sets status of upper (constraints') bounds. */
-		inline void setNoUpper(	BooleanType _status		/**< Status of upper (constraints') bounds. */
-								);
-
-
-		/** Returns status of lower (constraints') bounds.
-		 *	\return BT_TRUE if there is no lower (constraints') bound on any variable. */
-		inline BooleanType hasNoLower( ) const;
-
-		/** Returns status of upper bounds.
-		 *	\return BT_TRUE if there is no upper (constraints') bound on any variable. */
-		inline BooleanType hasNoUpper( ) const;
-
-
-		/** Shifts forward type and status of all constraints/bounds by a given
-		 *  offset. This offset has to lie within the range [0,n/2] and has to
-		 *  be an integer divisor of the total number of constraints/bounds n.
-		 *  Type and status of the first \<offset\> constraints/bounds is thrown away,
-		 *  type and status of the last \<offset\> constraints/bounds is doubled,
-		 *  e.g. for offset = 2: \n
-		 *  shift( {c/b1,c/b2,c/b3,c/b4,c/b5,c/b6} ) = {c/b3,c/b4,c/b5,c/b6,c/b5,c/b6}
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INDEX_OUT_OF_BOUNDS \n
-		 			RET_INVALID_ARGUMENTS \n
-		 			RET_SHIFTING_FAILED */
-		virtual returnValue shift(	int offset		/**< Shift offset within the range [0,n/2] and integer divisor of n. */
-									) = 0;
-
-		/** Rotates forward type and status of all constraints/bounds by a given
-		 *  offset. This offset has to lie within the range [0,n].
-		 *  Example for offset = 2: \n
-		 *  rotate( {c/b1,c/b2,c/b3,c/b4,c/b5,c/b6} ) = {c/b3,c/b4,c/b5,c/b6,c/b1,c/b2}
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INDEX_OUT_OF_BOUNDS \n
-		 			RET_ROTATING_FAILED */
-		virtual returnValue rotate(	int offset		/**< Rotation offset within the range [0,n]. */
-									) = 0;
-
-
-	/*
-	 *	PROTECTED MEMBER FUNCTIONS
-	 */
-	protected:
-		/** Frees all allocated memory.
-		 *  \return SUCCESSFUL_RETURN */
-		returnValue clear( );
-		
-		/** Copies all members from given rhs object.
-		 *  \return SUCCESSFUL_RETURN */
-		returnValue copy(	const SubjectTo& rhs	/**< Rhs object. */
-							);
-
-
-		/** Adds the index of a new constraint or bound to index set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_ADDINDEX_FAILED \n
-					RET_INVALID_ARGUMENTS */
-		returnValue addIndex(	Indexlist* const indexlist,	/**< Index list to which the new index shall be added. */
-								int newnumber,				/**< Number of new constraint or bound. */
-								SubjectToStatus newstatus	/**< Status of new constraint or bound. */
-								);
-
-		/** Removes the index of a constraint or bound from index set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_REMOVEINDEX_FAILED \n
-					RET_INVALID_ARGUMENTS */
-		returnValue removeIndex(	Indexlist* const indexlist,	/**< Index list from which the new index shall be removed. */
-									int removenumber			/**< Number of constraint or bound to be removed. */
-									);
-
-		/** Swaps the indices of two constraints or bounds within the index set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SWAPINDEX_FAILED \n
-					RET_INVALID_ARGUMENTS */
-		returnValue swapIndex(	Indexlist* const indexlist,	/**< Index list in which the indices shold be swapped. */
-								int number1,				/**< Number of first constraint or bound. */
-								int number2					/**< Number of second constraint or bound. */
-								);
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-		int n;						/**< Total number of constraints/bounds. */
-
-		SubjectToType* type; 		/**< Type of constraints/bounds. */
-		SubjectToStatus* status;	/**< Status of constraints/bounds. */
-
-		BooleanType noLower;	 	/**< This flag indicates if there is no lower bound on any variable. */
-		BooleanType noUpper;	 	/**< This flag indicates if there is no upper bound on any variable. */
-};
-
-
-END_NAMESPACE_QPOASES
-
-#include <qpOASES/SubjectTo.ipp>
-
-#endif	/* QPOASES_SUBJECTTO_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SubjectTo.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SubjectTo.ipp
deleted file mode 100644
index 086f54ca528..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/SubjectTo.ipp
+++ /dev/null
@@ -1,158 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/SubjectTo.ipp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of the inlined member functions of the SubjectTo class
- *	designed to manage working sets of constraints and bounds within a QProblem.
- */
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	g e t N u m b e r O f T y p e
- */
-inline int SubjectTo::getNumberOfType( SubjectToType _type ) const
-{
-	int i;
-	int numberOfType = 0;
-
-	if ( type != 0 )
-	{
-		for( i=0; i<n; ++i )
-			if ( type[i] == _type )
-				++numberOfType;
-	}
-
-	return numberOfType;
-}
-
-
-/*
- *	g e t T y p e
- */
-inline SubjectToType SubjectTo::getType( int i ) const
-{
-	if ( ( i >= 0 ) && ( i < n ) )
-		return type[i];
-
-	return ST_UNKNOWN;
-}
-
-
-/*
- *	g e t S t a t u s
- */
-inline SubjectToStatus SubjectTo::getStatus( int i ) const
-{
-	if ( ( i >= 0 ) && ( i < n ) )
-		return status[i];
-
-	return ST_UNDEFINED;
-}
-
-
-/*
- *	s e t T y p e
- */
-inline returnValue SubjectTo::setType( int i, SubjectToType value )
-{
-	if ( ( i >= 0 ) && ( i < n ) )
-	{
-		type[i] = value;
-		return SUCCESSFUL_RETURN;
-	}
-	else
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-}
-
-
-/*
- *	s e t S t a t u s
- */
-inline returnValue SubjectTo::setStatus( int i, SubjectToStatus value )
-{
-	if ( ( i >= 0 ) && ( i < n ) )
-	{
-		status[i] = value;
-		return SUCCESSFUL_RETURN;
-	}
-	else
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-}
-
-
-/*
- *	s e t N o L o w e r
- */
-inline void SubjectTo::setNoLower( BooleanType _status )
-{
-	noLower = _status;
-}
-
-
-/*
- *	s e t N o U p p e r
- */
-inline void SubjectTo::setNoUpper( BooleanType _status )
-{
-	noUpper = _status;
-}
-
-
-/*
- *	h a s N o L o w e r
- */
-inline BooleanType SubjectTo::hasNoLower( ) const
-{
-	return noLower;
-}
-
-
-/*
- *	h a s N o U p p p e r
- */
-inline BooleanType SubjectTo::hasNoUpper( ) const
-{
-	return noUpper;
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Types.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Types.hpp
deleted file mode 100644
index fc42d1288c7..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Types.hpp
+++ /dev/null
@@ -1,281 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/Types.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Declaration of all non-built-in types (except for classes).
- */
-
-
-#ifndef QPOASES_TYPES_HPP
-#define QPOASES_TYPES_HPP
-
-
-/* If your compiler does not support the snprintf() function,
- * uncomment the following line and try to compile again. */
-/* #define __NO_SNPRINTF__ */
-
-
-/* Uncomment the following line for setting the __DSPACE__ flag. */
-/* #define __DSPACE__ */
-
-/* Uncomment the following line for setting the __XPCTARGET__ flag. */
-/* #define __XPCTARGET__ */
-
-
-/* Uncomment the following line for setting the __NO_FMATH__ flag. */
-/* #define __NO_FMATH__ */
-
-/* Uncomment the following line to enable debug information. */
-/* #define __DEBUG__ */
-
-/* Uncomment the following line to enable suppress any kind of console output. */
-/* #define __SUPPRESSANYOUTPUT__ */
-
-
-/** Forces to always include all implicitly fixed bounds and all equality constraints
- *  into the initial working set when setting up an auxiliary QP. */
-#define __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
-
-
-/* Uncomment the following line to activate the use of an alternative Givens 
- * plane rotation requiring only three multiplications. */
-/* #define __USE_THREE_MULTS_GIVENS__ */
-
-/* Uncomment the following line to activate the use of single precision arithmetic. */
-/* #define __USE_SINGLE_PRECISION__ */
-
-
-
-/* Work-around for Borland BCC 5.5 compiler. */
-#ifdef __BORLANDC__
-#if __BORLANDC__ < 0x0561
-  #define __STDC__ 1
-#endif
-#endif
-
-
-/* Work-around for Microsoft compilers. */
-#ifdef _MSC_VER
-  #define __NO_SNPRINTF__
-  #pragma warning( disable : 4061 4100 4250 4514 4996 )
-#endif
-
-
-#ifdef __DSPACE__
-
-	#define __NO_SNPRINTF__
-
-	/** Macro for switching on/off the beginning of the qpOASES namespace definition. */
-	#define BEGIN_NAMESPACE_QPOASES
-    
-	/** Macro for switching on/off the end of the qpOASES namespace definition. */
-	#define END_NAMESPACE_QPOASES
-
-	/** Macro for switching on/off the use of the qpOASES namespace. */
-	#define USING_NAMESPACE_QPOASES
-
-	/** Macro for switching on/off references to the qpOASES namespace. */
-	#define REFER_NAMESPACE_QPOASES ::
-
-#else
-
-	/** Macro for switching on/off the beginning of the qpOASES namespace definition. */
-	#define BEGIN_NAMESPACE_QPOASES  namespace qpOASES {
-
-	/** Macro for switching on/off the end of the qpOASES namespace definition. */
-	#define END_NAMESPACE_QPOASES    }
-	
-	/** Macro for switching on/off the use of the qpOASES namespace. */
-	#define USING_NAMESPACE_QPOASES  using namespace qpOASES;
-	
-	/** Macro for switching on/off references to the qpOASES namespace. */
-	#define REFER_NAMESPACE_QPOASES  qpOASES::
-
-#endif
-
-
-/** Macro for accessing the Cholesky factor R. */
-#define RR( I,J )  R[(I)+nV*(J)]
-
-/** Macro for accessing the orthonormal matrix Q of the QT factorisation. */
-#define QQ( I,J )  Q[(I)+nV*(J)]
-
-/** Macro for accessing the triangular matrix T of the QT factorisation. */
-#define TT( I,J )  T[(I)*sizeT+(J)]
-
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/** Defines real_t for facilitating switching between double and float. */
-#ifdef __USE_SINGLE_PRECISION__
-typedef float real_t;
-#else
-typedef double real_t;
-#endif /* __USE_SINGLE_PRECISION__ */
-
-
-/** Summarises all possible logical values. */
-enum BooleanType
-{
-	BT_FALSE,					/**< Logical value for "false". */
-	BT_TRUE						/**< Logical value for "true". */
-};
-
-
-/** Summarises all possible print levels. Print levels are used to describe
- *	the desired amount of output during runtime of qpOASES. */
-enum PrintLevel
-{
-	PL_DEBUG_ITER = -2,			/**< Full tabular debugging output. */
-	PL_TABULAR,					/**< Normal tabular output. */
-	PL_NONE,					/**< No output. */
-	PL_LOW,						/**< Print error messages only. */
-	PL_MEDIUM,					/**< Print error and warning messages as well as concise info messages. */
-	PL_HIGH						/**< Print all messages with full details. */
-};
-
-
-/** Defines visibility status of a message. */
-enum VisibilityStatus
-{
-	VS_HIDDEN,					/**< Message not visible. */
-	VS_VISIBLE					/**< Message visible. */
-};
-
-
-/** Summarises all possible states of the (S)QProblem(B) object during the
-solution process of a QP sequence. */
-enum QProblemStatus
-{
-	QPS_NOTINITIALISED,			/**< QProblem object is freshly instantiated or reset. */
-	QPS_PREPARINGAUXILIARYQP,	/**< An auxiliary problem is currently setup, either at the very beginning
-								 *   via an initial homotopy or after changing the QP matrices. */
-	QPS_AUXILIARYQPSOLVED,		/**< An auxilary problem was solved, either at the very beginning
-								 *   via an initial homotopy or after changing the QP matrices. */
-	QPS_PERFORMINGHOMOTOPY,		/**< A homotopy according to the main idea of the online active
-								 *   set strategy is performed. */
-	QPS_HOMOTOPYQPSOLVED,		/**< An intermediate QP along the homotopy path was solved. */
-	QPS_SOLVED					/**< The solution of the actual QP was found. */
-};
-
-
-/** Summarises all possible types of the QP's Hessian matrix. */
-enum HessianType
-{
-	HST_ZERO,				/**< Hessian is zero matrix (i.e. LP formulation). */
-	HST_IDENTITY,			/**< Hessian is identity matrix. */
-	HST_POSDEF,				/**< Hessian is (strictly) positive definite. */
-	HST_POSDEF_NULLSPACE,	/**< Hessian is positive definite on null space of active bounds/constraints. */
-	HST_SEMIDEF,			/**< Hessian is positive semi-definite. */
-	HST_INDEF,				/**< Hessian is indefinite. */
-	HST_UNKNOWN				/**< Hessian type is unknown. */
-};
-
-
-/** Summarises all possible types of bounds and constraints. */
-enum SubjectToType
-{
-	ST_UNBOUNDED,		/**< Bound/constraint is unbounded. */
-	ST_BOUNDED,			/**< Bound/constraint is bounded but not fixed. */
-	ST_EQUALITY,		/**< Bound/constraint is fixed (implicit equality bound/constraint). */
-	ST_DISABLED,		/**< Bound/constraint is disabled (i.e. ignored when solving QP). */ 
-	ST_UNKNOWN			/**< Type of bound/constraint unknown. */
-};
-
-
-/** Summarises all possible states of bounds and constraints. */
-enum SubjectToStatus
-{
-	ST_LOWER = -1,			/**< Bound/constraint is at its lower bound. */
-	ST_INACTIVE,			/**< Bound/constraint is inactive. */
-	ST_UPPER,				/**< Bound/constraint is at its upper bound. */
-	ST_INFEASIBLE_LOWER,	/**< (to be documented) */
-	ST_INFEASIBLE_UPPER,	/**< (to be documented) */
-	ST_UNDEFINED			/**< Status of bound/constraint undefined. */
-};
-
-/**
- *	\brief Stores internal information for tabular (debugging) output.
- *
- *	Struct storing internal information for tabular (debugging) output 
- *	when using the (S)QProblem(B) objects.
- *
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2013-2014
- */
-struct TabularOutput {
-	int idxAddB;		/**< Index of bound that has been added to working set. */
-	int idxRemB;		/**< Index of bound that has been removed from working set. */
-	int idxAddC;		/**< Index of constraint that has been added to working set. */
-	int idxRemC;		/**< Index of constraint that has been removed from working set. */
-	int excAddB;		/**< Flag indicating whether a bound has been added to working set to keep a regular projected Hessian. */
-	int excRemB;		/**< Flag indicating whether a bound has been removed from working set to keep a regular projected Hessian. */
-	int excAddC;		/**< Flag indicating whether a constraint has been added to working set to keep a regular projected Hessian. */
-	int excRemC;		/**< Flag indicating whether a constraint has been removed from working set to keep a regular projected Hessian. */
-};
-
-
-
-/**
- *	\brief Struct containing the variable header for mat file.
- *
- *	Struct storing the header of a variable to be stored in 
- *	Matlab's binary format (using the outdated Level 4 variant 
- *  for simplictiy).
- *
- *  Note, this code snippet has been inspired from the document
- *  "Matlab(R) MAT-file Format, R2013b" by MathWorks
- *
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2013-2014
- */ 
-typedef struct {
-	long numericFormat;		/**< Flag indicating numerical format. */
-	long nRows;				/**< Number of rows. */
-	long nCols;				/**< Number of rows. */
-	long imaginaryPart;		/**< (to be documented) */
-	long nCharName;			/**< Number of character in name. */
-} MatMatrixHeader;
-
-
-
-
-END_NAMESPACE_QPOASES
-
-
-#endif	/* QPOASES_TYPES_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/UnitTesting.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/UnitTesting.hpp
deleted file mode 100644
index 96ade4b8bb2..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/UnitTesting.hpp
+++ /dev/null
@@ -1,79 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/UnitTesting.hpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2014
- *
- *	Definition of auxiliary functions/macros for unit testing.
- */
-
-
-#ifndef QPOASES_UNIT_TESTING_HPP
-#define QPOASES_UNIT_TESTING_HPP
-
-
-#ifndef TEST_TOL_FACTOR
-#define TEST_TOL_FACTOR 1
-#endif
-
-
-/** Return value for tests that passed. */
-#define TEST_PASSED 0
-
-/** Return value for tests that failed. */
-#define TEST_FAILED 1
-
-/** Return value for tests that could not run due to missing external data. */
-#define TEST_DATA_NOT_FOUND 99
-
-
-/** Macro verifying that two numerical values are equal in order to pass unit test. */
-#define QPOASES_TEST_FOR_EQUAL( x,y ) if ( REFER_NAMESPACE_QPOASES isEqual( (x),(y) ) == BT_FALSE ) { return TEST_FAILED; }
-
-/** Macro verifying that two numerical values are close to each other in order to pass unit test. */
-#define QPOASES_TEST_FOR_NEAR( x,y )  if ( REFER_NAMESPACE_QPOASES getAbs((x)-(y)) / REFER_NAMESPACE_QPOASES getMax( 1.0,REFER_NAMESPACE_QPOASES getAbs(x) ) >= 1e-10 ) { return TEST_FAILED; }
-
-/** Macro verifying that first quantity is lower or equal than second one in order to pass unit test. */
-#define QPOASES_TEST_FOR_TOL( x,tol )  if ( (x) > (tol)*(TEST_TOL_FACTOR) ) { return TEST_FAILED; }
-
-/** Macro verifying that a logical expression holds in order to pass unit test. */
-#define QPOASES_TEST_FOR_TRUE( x )  if ( (x) == false ) { return TEST_FAILED; }
-
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-END_NAMESPACE_QPOASES
-
-
-#endif	/* UNIT_TESTING */
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Utils.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Utils.hpp
deleted file mode 100644
index ddef94320b6..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Utils.hpp
+++ /dev/null
@@ -1,370 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/Utils.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Declaration of some utility functions for working with qpOASES.
- */
-
-
-#ifndef QPOASES_UTILS_HPP
-#define QPOASES_UTILS_HPP
-
-
-#include <qpOASES/MessageHandling.hpp>
-
-
-#ifdef __NO_SNPRINTF__
-  #if (!defined(_MSC_VER)) || defined(__DSPACE__)
-    /* If snprintf is not available, provide an empty implementation... */
-    int snprintf( char* s, size_t n, const char* format, ... );
-  #else
-	/* ... or substitute snprintf by _snprintf for Microsoft compilers. */
-    #define snprintf _snprintf    
-  #endif
-#endif /* __NO_SNPRINTF__ */
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/** Prints a (possibly named) vector.
- * \return SUCCESSFUL_RETURN */
-returnValue print(	const real_t* const v,	/**< Vector to be printed. */
-					int n,					/**< Length of vector. */
-					const char* name = 0	/**< Name of vector. */
-					);
-
-/** Prints a (possibly named) permuted vector.
- * \return SUCCESSFUL_RETURN */
-returnValue print(	const real_t* const v,		/**< Vector to be printed. */
-					int n,						/**< Length of vector. */
-					const int* const V_idx,		/**< Pemutation vector. */
-					const char* name = 0		/**< Name of vector. */
-					);
-
-/** Prints a (possibly named) matrix.
- * \return SUCCESSFUL_RETURN */
-returnValue print(	const real_t* const M,	/**< Matrix to be printed. */
-					int nrow,				/**< Row number of matrix. */
-					int ncol,				/**< Column number of matrix. */
-					const char* name = 0	/**< Name of matrix. */
-					);
-
-/** Prints a (possibly named) permuted matrix.
- * \return SUCCESSFUL_RETURN */
-returnValue print(	const real_t* const M,		/**< Matrix to be printed. */
-					int nrow,					/**< Row number of matrix. */
-					int ncol	,				/**< Column number of matrix. */
-					const int* const ROW_idx,	/**< Row pemutation vector. */
-					const int* const COL_idx,	/**< Column pemutation vector. */
-					const char* name = 0		/**< Name of matrix. */
-					);
-
-/** Prints a (possibly named) index array.
- * \return SUCCESSFUL_RETURN */
-returnValue print(	const int* const index,	/**< Index array to be printed. */
-					int n,					/**< Length of index array. */
-					const char* name = 0	/**< Name of index array. */
-					);
-
-
-/** Prints a string to desired output target (useful also for MATLAB output!).
- * \return SUCCESSFUL_RETURN */
-returnValue myPrintf(	const char* s	/**< String to be written. */
-						);
-
-
-/** Prints qpOASES copyright notice.
- * \return SUCCESSFUL_RETURN */
-returnValue printCopyrightNotice( );
-
-
-/** Reads a real_t matrix from file.
- * \return SUCCESSFUL_RETURN \n
- 		   RET_UNABLE_TO_OPEN_FILE \n
-		   RET_UNABLE_TO_READ_FILE */
-returnValue readFromFile(	real_t* data,				/**< Matrix to be read from file. */
-							int nrow,					/**< Row number of matrix. */
-							int ncol,					/**< Column number of matrix. */
-							const char* datafilename	/**< Data file name. */
-							);
-
-/** Reads a real_t vector from file.
- * \return SUCCESSFUL_RETURN \n
- 		   RET_UNABLE_TO_OPEN_FILE \n
-		   RET_UNABLE_TO_READ_FILE */
-returnValue readFromFile(	real_t* data,				/**< Vector to be read from file. */
-							int n,						/**< Length of vector. */
-							const char* datafilename	/**< Data file name. */
-							);
-
-/** Reads an integer (column) vector from file.
- * \return SUCCESSFUL_RETURN \n
- 		   RET_UNABLE_TO_OPEN_FILE \n
-		   RET_UNABLE_TO_READ_FILE */
-returnValue readFromFile(	int* data,					/**< Vector to be read from file. */
-							int n,						/**< Length of vector. */
-							const char* datafilename	/**< Data file name. */
-							);
-
-
-/** Writes a real_t matrix into a file.
- * \return SUCCESSFUL_RETURN \n
- 		   RET_UNABLE_TO_OPEN_FILE  */
-returnValue writeIntoFile(	const real_t* const data,		/**< Matrix to be written into file. */
-							int nrow,						/**< Row number of matrix. */
-							int ncol,						/**< Column number of matrix. */
-							const char* datafilename,		/**< Data file name. */
-							BooleanType append = BT_FALSE	/**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */
-							);
-
-/** Writes a real_t vector into a file.
- * \return SUCCESSFUL_RETURN \n
- 		   RET_UNABLE_TO_OPEN_FILE  */
-returnValue writeIntoFile(	const real_t* const data,		/**< Vector to be written into file. */
-							int n,							/**< Length of vector. */
-							const char* datafilename,		/**< Data file name. */
-							BooleanType append = BT_FALSE	/**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */
-							);
-
-/** Writes an integer (column) vector into a file.
- * \return SUCCESSFUL_RETURN \n
- 		   RET_UNABLE_TO_OPEN_FILE */
-returnValue writeIntoFile(	const int* const integer,		/**< Integer vector to be written into file. */
-							int n,							/**< Length of vector. */
-							const char* datafilename,		/**< Data file name. */
-							BooleanType append = BT_FALSE	/**< Indicates if integer shall be appended if the file already exists (otherwise it is overwritten). */
-							);
-
-/** Writes a real_t matrix/vector into a Matlab binary file.
- * \return SUCCESSFUL_RETURN \n
-		   RET_INVALID_ARGUMENTS
- 		   RET_UNABLE_TO_WRITE_FILE */
-returnValue writeIntoMatFile(	FILE* const matFile,		/**< Pointer to Matlab binary file. */
-								const real_t* const data,	/**< Data to be written into file. */
-								int nRows,					/**< Row number of matrix. */
-								int nCols, 					/**< Column number of matrix. */
-								const char* name			/**< Matlab name of matrix/vector to be stored. */
-								);
-
-/** Writes in integer matrix/vector into a Matlab binary file.
- * \return SUCCESSFUL_RETURN \n
-		   RET_INVALID_ARGUMENTS
- 		   RET_UNABLE_TO_WRITE_FILE */
-returnValue writeIntoMatFile(	FILE* const matFile,		/**< Pointer to Matlab binary file. */
-								const int* const data,		/**< Data to be written into file. */
-								int nRows,					/**< Row number of matrix. */
-								int nCols,					/**< Column number of matrix. */
-								const char* name			/**< Matlab name of matrix/vector to be stored. */
-								);
-
-
-/** Returns the current system time.
- * \return current system time */
-real_t getCPUtime( );
-
-
-/** Returns the N-norm of a vector.
- * \return >= 0.0: successful */
-real_t getNorm(	const real_t* const v,	/**< Vector. */
-				int n,					/**< Vector's dimension. */
-				int type = 2			/**< Norm type, 1: one-norm, 2: Euclidean norm. */
-				);
-
-
-/** Tests whether two real_t-valued arguments are (numerically) equal.
- * \return	BT_TRUE:  arguments differ not more than TOL \n
-		 	BT_FALSE: arguments differ more than TOL */
-inline BooleanType isEqual(	real_t x,			/**< First real number. */
-							real_t y,			/**< Second real number. */
-							real_t TOL = ZERO	/**< Tolerance for comparison. */
-							);
-
-
-/** Tests whether a real-valued argument is (numerically) zero.
- * \return	BT_TRUE:  argument differs from 0.0 not more than TOL \n
-		 	BT_FALSE: argument differs from 0.0 more than TOL */
-inline BooleanType isZero(	real_t x,			/**< Real number. */
-							real_t TOL = ZERO	/**< Tolerance for comparison. */
-							);
-
-
-/** Returns sign of a real-valued argument.
- * \return	 1.0: argument is non-negative \n
-		 	-1.0: argument is negative */
-inline real_t getSign(	real_t arg	/**< real-valued argument whose sign is to be determined. */
-						);
-
-
-/** Returns maximum of two integers.
- * \return	Maximum of two integers */
-inline int getMax(	int x,	/**< First integer. */
-					int y	/**< Second integer. */
-					);
-					
-/** Returns minimum of two integers.
- * \return	Minimum of two integers */
-inline int getMin(	int x,	/**< First integer. */
-					int y	/**< Second integer. */
-					);
-
-	
-/** Returns maximum of two reals.
- * \return	Maximum of two reals */
-inline real_t getMax(	real_t x,	/**< First real number. */
-						real_t y	/**< Second real number. */
-						);
-
-/** Returns minimum of two reals.
- * \return	Minimum of two reals */
-inline real_t getMin(	real_t x,	/**< First real number. */
-						real_t y	/**< Second real number. */
-						);
-
-/** Returns the absolute value of a real number.
- * \return	Absolute value of a real number */
-inline real_t getAbs(	real_t x	/**< Real number. */
-						);
-
-/** Returns the square-root of a real number.
- * \return	Square-root of a real number */
-inline real_t getSqrt(	real_t x	/**< Non-negative real number. */
-						);
-
-
-/** Computes "residual" of KKT system.  */
-void getKKTResidual(	int nV,						/**< Number of variables. */
-						int nC,						/**< Number of constraints. */
-						const real_t* const H,		/**< Hessian matrix. */
-						const real_t* const g,		/**< Sequence of gradient vectors. */
-						const real_t* const A,		/**< Constraint matrix. */
-						const real_t* const lb,		/**< Sequence of lower bound vectors (on variables). */
-						const real_t* const ub,		/**< Sequence of upper bound vectors (on variables). */
-						const real_t* const lbA,	/**< Sequence of lower constraints' bound vectors. */
-						const real_t* const ubA,	/**< Sequence of upper constraints' bound vectors. */
-						const real_t* const x,		/**< Sequence of primal trial vectors. */
-						const real_t* const y,		/**< Sequence of dual trial vectors. */
-						real_t& stat,				/**< Maximum value of stationarity condition residual. */
-						real_t& feas,				/**< Maximum value of primal feasibility violation. */
-						real_t& cmpl				/**< Maximum value of complementarity residual. */
-						);
-
-/** Computes "residual" of KKT system (for simply bounded QPs).  */
-void getKKTResidual(	int nV,						/**< Number of variables. */
-						const real_t* const H,		/**< Hessian matrix. */
-						const real_t* const g,		/**< Sequence of gradient vectors. */
-						const real_t* const lb,		/**< Sequence of lower bound vectors (on variables). */
-						const real_t* const ub,		/**< Sequence of upper bound vectors (on variables). */
-						const real_t* const x,		/**< Sequence of primal trial vectors. */
-						const real_t* const y,		/**< Sequence of dual trial vectors. */
-						real_t& stat,				/**< Maximum value of stationarity condition residual. */
-						real_t& feas,				/**< Maximum value of primal feasibility violation. */
-						real_t& cmpl				/**< Maximum value of complementarity residual. */
-						);
-
-
-/** Writes a value of BooleanType into a string.
- * \return SUCCESSFUL_RETURN */
-returnValue convertBooleanTypeToString(	BooleanType value, 		/**< Value to be written. */
-										char* const string		/**< Input: String of sufficient size, \n
-																	 Output: String containing value. */
-										);
-
-/** Writes a value of SubjectToStatus into a string.
- * \return SUCCESSFUL_RETURN */
-returnValue convertSubjectToStatusToString(	SubjectToStatus value,	/**< Value to be written. */
-											char* const string		/**< Input: String of sufficient size, \n
-																		 Output: String containing value. */
-											);
-
-/** Writes a value of PrintLevel into a string.
- * \return SUCCESSFUL_RETURN */
-returnValue convertPrintLevelToString(	PrintLevel value, 		/**< Value to be written. */
-										char* const string		/**< Input: String of sufficient size, \n
-																	 Output: String containing value. */
-										);
-
-
-/** Converts a returnValue from an (S)QProblem(B) object into a more 
- *	simple status flag.
- *
- * \return  0: QP problem solved
- *          1: QP could not be solved within given number of iterations
- *         -1: QP could not be solved due to an internal error
- *         -2: QP is infeasible (and thus could not be solved)
- *         -3: QP is unbounded (and thus could not be solved)
- */
-int getSimpleStatus(	returnValue returnvalue, 				/**< ReturnValue to be analysed. */
-						BooleanType doPrintStatus = BT_FALSE	/**< Flag indicating whether simple status shall be printed to screen. */
-						);
-
-
-/** Normalises QP constraints.
- * \return SUCCESSFUL_RETURN \n
- *		   RET_INVALID_ARGUMENTS */
-returnValue normaliseConstraints(	int nV,			/**< Number of variables. */
-									int nC, 		/**< Number of constraints. */
-									real_t* A,		/**< Input:  Constraint matrix, \n
-														 Output: Normalised constraint matrix. */
-									real_t* lbA,	/**< Input:  Constraints' lower bound vector, \n
-														 Output: Normalised constraints' lower bound vector. */
-									real_t* ubA,	/**< Input:  Constraints' upper bound vector, \n
-														 Output: Normalised constraints' upper bound vector. */
-									int type = 1	/**< Norm type, 1: one-norm, 2: Euclidean norm. */
-									);
-
-
-#ifdef __DEBUG__
-/** Writes matrix with given dimension into specified file. */
-extern "C" void gdb_printmat(	const char *fname,			/**< File name. */
-								real_t *M,					/**< Matrix to be written. */
-								int n,						/**< Number of rows. */
-								int m,						/**< Number of columns. */
-								int ldim					/**< Leading dimension. */
-								);
-#endif /* __DEBUG__ */
-
-
-#if defined(__DSPACE__) || defined(__XPCTARGET__) 
-extern "C" void __cxa_pure_virtual( void );
-#endif /* __DSPACE__ || __XPCTARGET__*/ 
-
-
-
-END_NAMESPACE_QPOASES
-
-
-#include <qpOASES/Utils.ipp>
-
-#endif	/* QPOASES_UTILS_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Utils.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Utils.ipp
deleted file mode 100644
index 3d89822b8b9..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/Utils.ipp
+++ /dev/null
@@ -1,174 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-
-/**
- *	\file include/qpOASES/Utils.ipp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of some inlined utilities for working with the different QProblem classes.
- */
-
-
-#include <math.h>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*
- *   i s E q u a l
- */
-inline BooleanType isEqual(	real_t x,
-							real_t y,
-							real_t TOL
-							)
-{
-    if ( getAbs(x-y) <= TOL )
-		return BT_TRUE;
-	else
-		return BT_FALSE;
-}
-
-
-/*
- *   i s Z e r o
- */
-inline BooleanType isZero(	real_t x,
-							real_t TOL
-							)
-{
-    if ( getAbs(x) <= TOL )
-		return BT_TRUE;
-	else
-		return BT_FALSE;
-}
-
-
-/*
- *   g e t S i g n
- */
-inline real_t getSign(	real_t arg
-						)
-{
-	if ( arg >= 0.0 )
-		return 1.0;
-	else
-		return -1.0;
-}
-
-
-
-/*
- *   g e t M a x
- */
-inline int getMax(	int x,
-					int y
-					)
-{
-    return (y<x) ? x : y;
-}
-
-
-/*
- *   g e t M i n
- */
-inline int getMin(	int x,
-					int y
-					)
-{
-    return (y>x) ? x : y;
-}
-
-
-
-/*
- *   g e t M a x
- */
-inline real_t getMax(	real_t x,
-						real_t y
-						)
-{
-	#ifdef __NO_FMATH__
-    return (y<x) ? x : y;
-	#else
-	return (y<x) ? x : y;
-	//return fmax(x,y); /* seems to be slower */
-	#endif
-}
-
-
-/*
- *   g e t M i n
- */
-inline real_t getMin(	real_t x,
-						real_t y
-						)
-{
-	#ifdef __NO_FMATH__
-    return (y>x) ? x : y;
-	#else
-	return (y>x) ? x : y;
-	//return fmin(x,y); /* seems to be slower */
-	#endif
-}
-
-
-/*
- *   g e t A b s
- */
-inline real_t getAbs(	real_t x
-						)
-{
-	#ifdef __NO_FMATH__
-	return (x>=0.0) ? x : -x;
-	#else
-	return fabs(x);
-	#endif
-}
-
-
-/*
- *   g e t S q r t
- */
-inline real_t getSqrt(	real_t x
-						)
-{
-	#ifdef __NO_FMATH__
-	return sqrt(x); /* put your custom sqrt-replacement here */
-	#else
-	return sqrt(x);
-	#endif
-}
-
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/OQPinterface.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/OQPinterface.hpp
deleted file mode 100644
index 4623aa32840..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/OQPinterface.hpp
+++ /dev/null
@@ -1,250 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/extras/OQPinterface.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Declaration of an interface comprising several utility functions
- *	for solving test problems from the Online QP Benchmark Collection
- *	(This collection is no longer maintained, see 
- *	http://www.qpOASES.org/onlineQP for a backup).
- */
-
-
-#ifndef QPOASES_OQPINTERFACE_HPP
-#define QPOASES_OQPINTERFACE_HPP
-
-
-#include <qpOASES/Options.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/** Reads dimensions of an Online QP Benchmark problem from file.
- *
- * \return SUCCESSFUL_RETURN \n
-		   RET_UNABLE_TO_READ_FILE \n
-		   RET_FILEDATA_INCONSISTENT */
-returnValue readOQPdimensions(	const char* path,	/**< Full path of the data files (without trailing slash!). */
-								int& nQP,			/**< Output: Number of QPs. */
-								int& nV,			/**< Output: Number of variables. */
-								int& nC,			/**< Output: Number of constraints. */
-								int& nEC			/**< Output: Number of equality constraints. */
-								);
-
-/** Reads data of an Online QP Benchmark problem from file.
- *  This function allocates the required memory for all data; after successfully calling it,
- *  you have to free this memory yourself!
- *
- * \return SUCCESSFUL_RETURN \n
-		   RET_INVALID_ARGUMENTS \n
-		   RET_UNABLE_TO_READ_FILE \n
-		   RET_FILEDATA_INCONSISTENT */
-returnValue readOQPdata(	const char* path,	/**< Full path of the data files (without trailing slash!). */
-							int& nQP,			/**< Output: Number of QPs. */
-							int& nV,			/**< Output: Number of variables. */
-							int& nC,			/**< Output: Number of constraints. */
-							int& nEC,			/**< Output: Number of equality constraints. */
-							real_t** H,		 	/**< Output: Hessian matrix. */
-							real_t** g,		 	/**< Output: Sequence of gradient vectors. */
-							real_t** A,		 	/**< Output: Constraint matrix. */
-							real_t** lb,		/**< Output: Sequence of lower bound vectors (on variables). */
-							real_t** ub,		/**< Output: Sequence of upper bound vectors (on variables). */
-							real_t** lbA,		/**< Output: Sequence of lower constraints' bound vectors. */
-							real_t** ubA,		/**< Output: Sequence of upper constraints' bound vectors. */
-							real_t** xOpt,		/**< Output: Sequence of primal solution vectors
-												 *           (not read if a null pointer is passed). */
-							real_t** yOpt,		/**< Output: Sequence of dual solution vectors
-												 *           (not read if a null pointer is passed). */
-							real_t** objOpt		/**< Output: Sequence of optimal objective function values
-												 *           (not read if a null pointer is passed). */
-							);
-
-
-/** Solves an Online QP Benchmark problem as specified by the arguments.
- *  The maximum deviations from the given optimal solution as well as the
- *  maximum CPU time to solve each QP are determined.
- *
- *	Note: This variant is outdated and only kept to ensure 
- *		  backwards-compatibility!
- *
- * \return SUCCESSFUL_RETURN \n
- 		   RET_BENCHMARK_ABORTED */
-returnValue solveOQPbenchmark(	int nQP,					/**< Number of QPs. */
-								int nV,						/**< Number of variables. */
-								int nC,						/**< Number of constraints. */
-								int nEC,					/**< Number of equality constraints. */
-								const real_t* const _H,		/**< Hessian matrix. */
-								const real_t* const g,		/**< Sequence of gradient vectors. */
-								const real_t* const _A,		/**< Constraint matrix. */
-								const real_t* const lb,		/**< Sequence of lower bound vectors (on variables). */
-								const real_t* const ub,		/**< Sequence of upper bound vectors (on variables). */
-								const real_t* const lbA,	/**< Sequence of lower constraints' bound vectors. */
-								const real_t* const ubA,	/**< Sequence of upper constraints' bound vectors. */
-								BooleanType isSparse,		/**< Shall convert matrices to sparse format before solution? */
-								const Options& options,		/**< QP solver options to be used while solving benchmark problems. */
-								int& nWSR, 					/**< Input: Maximum number of working set recalculations; \n
-																 Output: Maximum number of performed working set recalculations. */
-								real_t& maxCPUtime,			/**< Output: Maximum CPU time required for solving each QP. */
-								real_t& maxStationarity,	/**< Output: Maximum residual of stationarity condition. */
-								real_t& maxFeasibility,		/**< Output: Maximum residual of primal feasibility condition. */
-								real_t& maxComplementarity	/**< Output: Maximum residual of complementarity condition. */
-								);
-
-/** Solves an Online QP Benchmark problem as specified by the arguments.
- *  The maximum deviations from the given optimal solution as well as the
- *  maximum CPU time to solve each QP are determined.
- *
- * \return SUCCESSFUL_RETURN \n
- 		   RET_BENCHMARK_ABORTED */
-returnValue solveOQPbenchmark(	int nQP,					/**< Number of QPs. */
-								int nV,						/**< Number of variables. */
-								int nC,						/**< Number of constraints. */
-								int nEC,					/**< Number of equality constraints. */
-								const real_t* const _H,		/**< Hessian matrix. */
-								const real_t* const g,		/**< Sequence of gradient vectors. */
-								const real_t* const _A,		/**< Constraint matrix. */
-								const real_t* const lb,		/**< Sequence of lower bound vectors (on variables). */
-								const real_t* const ub,		/**< Sequence of upper bound vectors (on variables). */
-								const real_t* const lbA,	/**< Sequence of lower constraints' bound vectors. */
-								const real_t* const ubA,	/**< Sequence of upper constraints' bound vectors. */
-								BooleanType isSparse,		/**< Shall convert matrices to sparse format before solution? */
-								BooleanType useHotstarts,	/**< Shall QP solution be hotstarted? */
-								const Options& options,		/**< QP solver options to be used while solving benchmark problems. */
-								int maxAllowedNWSR, 		/**< Maximum number of working set recalculations to be performed. */
-								real_t& maxNWSR,			/**< Output: Maximum number of performed working set recalculations. */
-								real_t& avgNWSR,			/**< Output: Average number of performed working set recalculations. */
-								real_t& maxCPUtime,			/**< Output: Maximum CPU time required for solving each QP. */
-								real_t& avgCPUtime,			/**< Output: Average CPU time required for solving each QP. */
-								real_t& maxStationarity,	/**< Output: Maximum residual of stationarity condition. */
-								real_t& maxFeasibility,		/**< Output: Maximum residual of primal feasibility condition. */
-								real_t& maxComplementarity	/**< Output: Maximum residual of complementarity condition. */
-								);
-
-
-/** Solves an Online QP Benchmark problem (without constraints) as specified
- *  by the arguments. The maximum deviations from the given optimal solution
- *  as well as the maximum CPU time to solve each QP are determined.
- *
- *	Note: This variant is outdated and only kept to ensure 
- *		  backwards-compatibility!
- *
- * \return SUCCESSFUL_RETURN \n
- 		   RET_BENCHMARK_ABORTED */
-returnValue solveOQPbenchmark(	int nQP,					/**< Number of QPs. */
-								int nV,						/**< Number of variables. */
-								const real_t* const _H,		/**< Hessian matrix. */
-								const real_t* const g,		/**< Sequence of gradient vectors. */
-								const real_t* const lb,		/**< Sequence of lower bound vectors (on variables). */
-								const real_t* const ub,		/**< Sequence of upper bound vectors (on variables). */
-								BooleanType isSparse,		/**< Shall convert matrices to sparse format before solution? */
-								const Options& options,		/**< QP solver options to be used while solving benchmark problems. */
-								int& nWSR, 					/**< Input: Maximum number of working set recalculations; \n
-																 Output: Maximum number of performed working set recalculations. */
-								real_t& maxCPUtime,			/**< Output: Maximum CPU time required for solving each QP. */
-								real_t& maxStationarity,	/**< Output: Maximum residual of stationarity condition. */
-								real_t& maxFeasibility,		/**< Output: Maximum residual of primal feasibility condition. */
-								real_t& maxComplementarity	/**< Output: Maximum residual of complementarity condition. */
-								);
-
-/** Solves an Online QP Benchmark problem (without constraints) as specified
- *  by the arguments. The maximum deviations from the given optimal solution
- *  as well as the maximum CPU time to solve each QP are determined.
- *
- * \return SUCCESSFUL_RETURN \n
- 		   RET_BENCHMARK_ABORTED */
-returnValue solveOQPbenchmark(	int nQP,					/**< Number of QPs. */
-								int nV,						/**< Number of variables. */
-								const real_t* const _H,		/**< Hessian matrix. */
-								const real_t* const g,		/**< Sequence of gradient vectors. */
-								const real_t* const lb,		/**< Sequence of lower bound vectors (on variables). */
-								const real_t* const ub,		/**< Sequence of upper bound vectors (on variables). */
-								BooleanType isSparse,		/**< Shall convert matrices to sparse format before solution? */
-								BooleanType useHotstarts,	/**< Shall QP solution be hotstarted? */
-								const Options& options,		/**< QP solver options to be used while solving benchmark problems. */
-								int maxAllowedNWSR, 		/**< Maximum number of working set recalculations to be performed. */
-								real_t& maxNWSR,			/**< Output: Maximum number of performed working set recalculations. */
-								real_t& avgNWSR,			/**< Output: Average number of performed working set recalculations. */
-								real_t& maxCPUtime,			/**< Output: Maximum CPU time required for solving each QP. */
-								real_t& avgCPUtime,			/**< Output: Average CPU time required for solving each QP. */
-								real_t& maxStationarity,	/**< Output: Maximum residual of stationarity condition. */
-								real_t& maxFeasibility,		/**< Output: Maximum residual of primal feasibility condition. */
-								real_t& maxComplementarity	/**< Output: Maximum residual of complementarity condition. */
-								);
-
-
-/** Runs an Online QP Benchmark problem and determines the maximum
- *  violation of the KKT optimality conditions as well as the 
- *  maximum CPU time to solve each QP.
- *
- * \return SUCCESSFUL_RETURN \n
-		   RET_UNABLE_TO_READ_BENCHMARK \n
- 		   RET_BENCHMARK_ABORTED */
-returnValue runOQPbenchmark(	const char* path,			/**< Full path of the benchmark files (without trailing slash!). */
-								BooleanType isSparse,		/**< Shall convert matrices to sparse format before solution? */
-								const Options& options,		/**< QP solver options to be used while solving benchmark problems. */
-								int& nWSR, 					/**< Input: Maximum number of working set recalculations; \n
-																 Output: Maximum number of performed working set recalculations. */
-								real_t& maxCPUtime,			/**< Output: Maximum CPU time required for solving each QP. */
-								real_t& maxStationarity,	/**< Output: Maximum residual of stationarity condition. */
-								real_t& maxFeasibility,		/**< Output: Maximum residual of primal feasibility condition. */
-								real_t& maxComplementarity	/**< Output: Maximum residual of complementarity condition. */
-								);
-
-
-/** Runs an Online QP Benchmark problem and determines the maximum
- *  violation of the KKT optimality conditions as well as the 
- *  maximum and average number of iterations and CPU time to solve 
- *	each QP.
- *
- * \return SUCCESSFUL_RETURN \n
-		   RET_UNABLE_TO_READ_BENCHMARK \n
- 		   RET_BENCHMARK_ABORTED */
-returnValue runOQPbenchmark(	const char* path,			/**< Full path of the benchmark files (without trailing slash!). */
-								BooleanType isSparse,		/**< Shall convert matrices to sparse format before solution? */
-								BooleanType useHotstarts,	/**< Shall QP solution be hotstarted? */
-								const Options& options,		/**< QP solver options to be used while solving benchmark problems. */
-								int maxAllowedNWSR, 		/**< Maximum number of working set recalculations to be performed. */
-								real_t& maxNWSR,			/**< Output: Maximum number of performed working set recalculations. */
-								real_t& avgNWSR,			/**< Output: Average number of performed working set recalculations. */
-								real_t& maxCPUtime,			/**< Output: Maximum CPU time required for solving each QP. */
-								real_t& avgCPUtime,			/**< Output: Average CPU time required for solving each QP. */
-								real_t& maxStationarity,	/**< Output: Maximum residual of stationarity condition. */
-								real_t& maxFeasibility,		/**< Output: Maximum residual of primal feasibility condition. */
-								real_t& maxComplementarity	/**< Output: Maximum residual of complementarity condition. */
-								);
-
-END_NAMESPACE_QPOASES
-
-
-#endif	/* QPOASES_OQPINTERFACE_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/SolutionAnalysis.hpp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/SolutionAnalysis.hpp
deleted file mode 100644
index ad625807495..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/SolutionAnalysis.hpp
+++ /dev/null
@@ -1,155 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/extras/SolutionAnalysis.hpp
- *	\author Hans Joachim Ferreau (thanks to Boris Houska)
- *	\version 3.0
- *	\date 2008-2014
- *
- *	Declaration of the SolutionAnalysis class designed to perform
- *	additional analysis after solving a QP with qpOASES.
- */
-
-
-#ifndef QPOASES_SOLUTIONANALYSIS_HPP
-#define QPOASES_SOLUTIONANALYSIS_HPP
-
-
-#include <qpOASES/SQProblem.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/**
- *	\brief Provides additional tools for analysing QP solutions.
- *
- *	This class is intended to provide additional tools for analysing
- *  a QP solution obtained with qpOASES.
- *
- *	\author Hans Joachim Ferreau (thanks to Boris Houska)
- *	\version 3.0
- *	\date 2007-2014
- */
-class SolutionAnalysis
-{
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		SolutionAnalysis( );
-
-		/** Copy constructor (deep copy). */
-		SolutionAnalysis(	const SolutionAnalysis& rhs		/**< Rhs object. */
-							);
-
-		/** Destructor. */
-		~SolutionAnalysis( );
-
-		/** Assignment operator (deep copy). */
-		SolutionAnalysis& operator=(	const SolutionAnalysis& rhs		/**< Rhs object. */
-										);
-
-
-		/** Determines the maximum violation of the KKT optimality conditions
-		 *  of the current iterate within the QProblemB object.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_UNABLE_TO_ANALYSE_QPROBLEM */
-		returnValue getMaxKKTviolation(	QProblemB* qp,			/**< QProblemB to be analysed. */
-										real_t& maxKKTviolation	/**< OUTPUT: maximum violation of the KKT conditions. */
-										) const;
-
-		/** Determines the maximum violation of the KKT optimality conditions
-		 *  of the current iterate within the QProblem object.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_UNABLE_TO_ANALYSE_QPROBLEM */
-		returnValue getMaxKKTviolation(	QProblem* qp,			/**< QProblem to be analysed. */
-										real_t& maxKKTviolation	/**< OUTPUT: maximum violation of the KKT conditions. */
-										) const;
-
-		/** Determines the maximum violation of the KKT optimality conditions
-		 *  of the current iterate within the SQProblem object.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_UNABLE_TO_ANALYSE_QPROBLEM */
-		returnValue getMaxKKTviolation(	SQProblem* qp,			/**< SQProblem to be analysed. */
-										real_t& maxKKTviolation	/**< OUTPUT: maximum violation of the KKT conditions. */
-										) const;
-
-
-		/** Computes the variance-covariance matrix of the QP output for uncertain 
-			inputs.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_HOTSTART_FAILED \n
-		 			RET_STEPDIRECTION_FAILED_TQ \n
-					RET_STEPDIRECTION_FAILED_CHOLESKY */
-		returnValue getVarianceCovariance(	QProblemB* qp,			/**< QProblemB to be analysed. */
-											real_t* g_b_bA_VAR,		/**< INPUT : Variance-covariance of g, the bounds lb and ub, and lbA and ubA respectively. Dimension: 2nV x 2nV */
-											real_t* Primal_Dual_VAR	/**< OUTPUT: The result for the variance-covariance of the primal and dual variables. Dimension: 2nV x 2nV */
-											) const;
-
-		/** Computes the variance-covariance matrix of the QP output for uncertain 
-			inputs.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_HOTSTART_FAILED \n
-		 			RET_STEPDIRECTION_FAILED_TQ \n
-					RET_STEPDIRECTION_FAILED_CHOLESKY */
-		returnValue getVarianceCovariance(	QProblem* qp,			/**< QProblem to be analysed. */
-											real_t* g_b_bA_VAR,		/**< INPUT : Variance-covariance of g, the bounds lb and ub, and lbA and ubA respectively. Dimension:  (2nV+nC) x (2nV+nC) */
-											real_t* Primal_Dual_VAR	/**< OUTPUT: The result for the variance-covariance of the primal and dual variables. Dimension:  (2nV+nC) x (2nV+nC) */
-											) const;
-
-		/** Computes the variance-covariance matrix of the QP output for uncertain 
-			inputs.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_HOTSTART_FAILED \n
-		 			RET_STEPDIRECTION_FAILED_TQ \n
-					RET_STEPDIRECTION_FAILED_CHOLESKY */
-		returnValue getVarianceCovariance(	SQProblem* qp,			/**< SQProblem to be analysed. */
-											real_t* g_b_bA_VAR,		/**< INPUT : Variance-covariance of g, the bounds lb and ub, and lbA and ubA respectively. Dimension:  (2nV+nC) x (2nV+nC) */
-											real_t* Primal_Dual_VAR	/**< OUTPUT: The result for the variance-covariance of the primal and dual variables. Dimension:  (2nV+nC) x (2nV+nC) */
-											) const;
-
-		/** Checks if a direction of negative curvature shows up if we remove all bounds that just recently became active */
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-
-};
-
-
-END_NAMESPACE_QPOASES
-
-#include <qpOASES/extras/SolutionAnalysis.ipp>
-
-#endif	/* QPOASES_SOLUTIONANALYSIS_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/SolutionAnalysis.ipp b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/SolutionAnalysis.ipp
deleted file mode 100644
index dc59a7e3d60..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/SolutionAnalysis.ipp
+++ /dev/null
@@ -1,51 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES/extras/SolutionAnalysis.ipp
- *	\author Hans Joachim Ferreau (thanks to Boris Houska)
- *	\version 3.0
- *	\date 2008-2014
- *
- *	Implementation of inlined member functions of the SolutionAnalysis class
- *	designed to perform additional analysis after solving a QP with qpOASES.
- *
- */
-
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/index.html b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/index.html
deleted file mode 100644
index 6cb3040f5aa..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/extras/index.html
+++ /dev/null
@@ -1,10 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/include/qpOASES/extras</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/include/qpOASES/extras</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="OQPinterface.hpp">OQPinterface.hpp</a></li>
-  <li><a href="SolutionAnalysis.hpp">SolutionAnalysis.hpp</a></li>
-  <li><a href="SolutionAnalysis.ipp">SolutionAnalysis.ipp</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/index.html b/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/index.html
deleted file mode 100644
index 7bc668d65bf..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/include/qpOASES/index.html
+++ /dev/null
@@ -1,33 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/include/qpOASES</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/include/qpOASES</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="Bounds.hpp">Bounds.hpp</a></li>
-  <li><a href="Bounds.ipp">Bounds.ipp</a></li>
-  <li><a href="Constants.hpp">Constants.hpp</a></li>
-  <li><a href="ConstraintProduct.hpp">ConstraintProduct.hpp</a></li>
-  <li><a href="Constraints.hpp">Constraints.hpp</a></li>
-  <li><a href="Constraints.ipp">Constraints.ipp</a></li>
-  <li><a href="Flipper.hpp">Flipper.hpp</a></li>
-  <li><a href="Indexlist.hpp">Indexlist.hpp</a></li>
-  <li><a href="Indexlist.ipp">Indexlist.ipp</a></li>
-  <li><a href="Matrices.hpp">Matrices.hpp</a></li>
-  <li><a href="MessageHandling.hpp">MessageHandling.hpp</a></li>
-  <li><a href="MessageHandling.ipp">MessageHandling.ipp</a></li>
-  <li><a href="Options.hpp">Options.hpp</a></li>
-  <li><a href="QProblem.hpp">QProblem.hpp</a></li>
-  <li><a href="QProblem.ipp">QProblem.ipp</a></li>
-  <li><a href="QProblemB.hpp">QProblemB.hpp</a></li>
-  <li><a href="QProblemB.ipp">QProblemB.ipp</a></li>
-  <li><a href="SQProblem.hpp">SQProblem.hpp</a></li>
-  <li><a href="SQProblem.ipp">SQProblem.ipp</a></li>
-  <li><a href="SubjectTo.hpp">SubjectTo.hpp</a></li>
-  <li><a href="SubjectTo.ipp">SubjectTo.ipp</a></li>
-  <li><a href="Types.hpp">Types.hpp</a></li>
-  <li><a href="UnitTesting.hpp">UnitTesting.hpp</a></li>
-  <li><a href="Utils.hpp">Utils.hpp</a></li>
-  <li><a href="Utils.ipp">Utils.ipp</a></li>
-  <li><a href="extras/">extras/</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/index.html b/3rdparty/qpOASES/qpOASES-3.0/index.html
deleted file mode 100644
index f03255769ac..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/index.html
+++ /dev/null
@@ -1,31 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="AUTHORS">AUTHORS</a></li>
-  <li><a href="AUTHORS.txt">AUTHORS.txt</a></li>
-  <li><a href="CMakeLists.txt">CMakeLists.txt</a></li>
-  <li><a href="INSTALL">INSTALL</a></li>
-  <li><a href="INSTALL.txt">INSTALL.txt</a></li>
-  <li><a href="LICENSE">LICENSE</a></li>
-  <li><a href="LICENSE.txt">LICENSE.txt</a></li>
-  <li><a href="Makefile">Makefile</a></li>
-  <li><a href="README">README</a></li>
-  <li><a href="README.txt">README.txt</a></li>
-  <li><a href="VERSIONS">VERSIONS</a></li>
-  <li><a href="VERSIONS.txt">VERSIONS.txt</a></li>
-  <li><a href="bin/">bin/</a></li>
-  <li><a href="doc/">doc/</a></li>
-  <li><a href="examples/">examples/</a></li>
-  <li><a href="include/">include/</a></li>
-  <li><a href="interfaces/">interfaces/</a></li>
-  <li><a href="make.mk">make.mk</a></li>
-  <li><a href="make_cygwin.mk">make_cygwin.mk</a></li>
-  <li><a href="make_linux.mk">make_linux.mk</a></li>
-  <li><a href="make_osx.mk">make_osx.mk</a></li>
-  <li><a href="make_windows.mk">make_windows.mk</a></li>
-  <li><a href="src/">src/</a></li>
-  <li><a href="testing/">testing/</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/index.html
deleted file mode 100644
index 70331be2a27..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/index.html
+++ /dev/null
@@ -1,12 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/interfaces</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="matlab/">matlab/</a></li>
-  <li><a href="octave/">octave/</a></li>
-  <li><a href="python/">python/</a></li>
-  <li><a href="scilab/">scilab/</a></li>
-  <li><a href="simulink/">simulink/</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1.mat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1.mat
deleted file mode 100644
index c0bd0d2ed4364ebaf2b16eeb1791b99510164d60..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 790
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i?A@$QE)CwO)N=GQOM7;
zQV7W?Rq#(PQBW{ZFtoHXwy-iZQZO<wFjpWIFfe-h@-r|n=m2rWoX5!t2^<VbW(v<5
zE-4(~dTQ)ws4P&$km;uomeA11|BWZ&AagZCObxRuNRJv^j{&kCCPU#FX(<IPn`cT$
zG&V7awlb^%sn&(7E<jezXs9e<Y_x<KXonck4*72aznX0sEPR-wL7MdNXfk}FVrWpn
zbd!n4;KKpAQx*&i4u6?#K)Tf7x=<Vjw#smdTH_BNo;41R3^D<X9(eq5$k5n1bCNQ%
z@Pwl*Pna3xb}%;MF&w1YZ<6wal%ya3=QlDksCqM3;5D4vIj*8W0BATX&~U}|jQ2pQ
z4dE{TfEGw1%||Xh{v33=(L`N<+u*<f_G*s5hvgUwC0GSO8WEwp0Yl@HqDhZ{Iwd7$
zNJcd5`Tw8MMu6oVNRu%h+n#t$hG?>A%wc4c5GZW8!2gYzq2v=QA4sbvTq}xwN{M9(
rdcH+H=e>2)6PUUhjxboKF`MIYAV|YGPpF2pgrpzeL4oIZl~o4-!Xx*U

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1a.mat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1a.mat
deleted file mode 100644
index 48c784fcb7c53eedc3b3124e3577f8d0822e1cb2..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 896
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i?A@$QE)CwO)N=GQOM7;
zQV7W?Rq#(PQBW{ZFtoHXwzM)dRWLFzFjpWIFfe-h@-r|n=m2rWoX5!t2^<VbW(v<5
zE-4(~dTQ)ws4P&$km;uomeA11|BWZ&AagZCObxRuNRJv^j{&kCCPU#FX(<IPn`cT$
zG&V7awlb^%sn&(7E<jezXs9e<Y_x<KXonck4*72aznX0sEPR-wL7MdNXfk}FVrWpn
zbd!n4;KKpAQx*&i4u6?#K)Tf7x=<Vjw#smdTH_BNo;41R3^D<X9(eq5$k5n1bCNQ%
z@Pwl*Pna3xb}%;MF&w1YZ<6wal%ya3=QlDksCqM3;5D4vIj*8W0BATX&~U}|jQ2pQ
z4dE{TfEGw1%||Xh{v33=(L`N<+u*<f_G*s5hvgUwC0GSO8WEwp0Yl@HqDhZ{Iwd7$
zNJcd5`Tw8MMu6oVNRu%h+n#t$hG?>A%wc4c5GZW8!2gYzq2v=QA4sbvTq}xwN{M9(
zdcH+H=e>2)6PUUhjxboKF`MIYAV|YGPpF2pgrpzeL4oIZl~o6%$pCIk2ePk#sWRk@
zvYq}`8INM7jZDluH5=M5@Ec?>oIk~G0n(=h*N5UdV9K1r)@ga{`O<04M`r+2CliB_
HDN`x{YE&BX

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1b.mat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/example1b.mat
deleted file mode 100644
index b45b4f74c6a935c350dac6de0673396f20801338..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 549
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i?A@$QE)CwO)N=GQOM7;
zQV7W?Rq#(PQBW{ZFtoHXwy-iXRWLFzFjpWIFfe-h@-r|n=m2rWoX5!t2^<VbW(v<5
zE-4(~dTQ)ws4P&$km;uomeA11|BWZ&AagZCObxRuNRKXDPXV$XMnh!@W1}U^KwV-$
zUGm=qel^=NSoknUgEZ;k(Pa2U#n7OD=_V78!G{BKrz{v49R4!ffOM(Dbs1n-WjIBx
z@rMu38V5%PnE*x)ka9Kb$`2WGJI7TN2nbI&%KC(vL2*6fJ&<ZcxatpRhKn>Gx%BvR
z(CJ1KbpdXJ0|(fvIsP7&V<?nh6#!{O1i%IijZcauJp$^Kl$aqI(Xi+Le?}VtmUkdc
f#&~Rd;x!qf$)YibkxfFNu;Bv#H)e*CPpo_Z46>$n

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/index.html
deleted file mode 100644
index 2e1a48f8c63..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/index.html
+++ /dev/null
@@ -1,20 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces/matlab</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/interfaces/matlab</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="Makefile">Makefile</a></li>
-  <li><a href="example1.mat">example1.mat</a></li>
-  <li><a href="example1a.mat">example1a.mat</a></li>
-  <li><a href="example1b.mat">example1b.mat</a></li>
-  <li><a href="make.m">make.m</a></li>
-  <li><a href="qpOASES.cpp">qpOASES.cpp</a></li>
-  <li><a href="qpOASES.m">qpOASES.m</a></li>
-  <li><a href="qpOASES_auxInput.m">qpOASES_auxInput.m</a></li>
-  <li><a href="qpOASES_matlab_utils.cpp">qpOASES_matlab_utils.cpp</a></li>
-  <li><a href="qpOASES_matlab_utils.hpp">qpOASES_matlab_utils.hpp</a></li>
-  <li><a href="qpOASES_options.m">qpOASES_options.m</a></li>
-  <li><a href="qpOASES_sequence.cpp">qpOASES_sequence.cpp</a></li>
-  <li><a href="qpOASES_sequence.m">qpOASES_sequence.m</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/make.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/make.m
deleted file mode 100644
index 0970f19b2ff..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/make.m
+++ /dev/null
@@ -1,238 +0,0 @@
-function [] = make( varargin )
-%MAKE Compiles the Matlab interface of qpOASES.
-%
-%Type  make            to compile all interfaces that 
-%                      have been modified,
-%type  make clean      to delete all compiled interfaces,
-%type  make clean all  to first delete and then compile 
-%                      all interfaces,
-%type  make 'name'     to compile only the interface with
-%                      the given name (if it has been modified),
-%type  make 'opt'      to compile all interfaces using the 
-%                      given compiler options.
-%
-%Copyright (C) 2013-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%Christian Kirches et al. All rights reserved.
-
-%%
-%%	This file is part of qpOASES.
-%%
-%%	qpOASES -- An Implementation of the Online Active Set Strategy.
-%%	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%%	Christian Kirches et al. All rights reserved.
-%%
-%%	qpOASES 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.
-%%
-%%	qpOASES 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 qpOASES; if not, write to the Free Software
-%%	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-%%
-
-%%
-%%	Filename:  interfaces/matlab/make.m
-%%	Author:    Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
-%%	Version:   3.0
-%%	Date:      2007-2014
-%%
-
-       
-    %% consistency check
-    if ( exist( [pwd, '/make.m'],'file' ) == 0 )
-        error( ['ERROR (',mfilename '.m): Run this make script directly within the directory', ...
-                '<qpOASES-inst-dir>/interfaces/matlab, please.'] );
-    end
-
-
-    if ( nargin > 2 )
-        error( ['ERROR (',mfilename '.m): At most two make arguments supported!'] );
-    else
-        [ doClean,fcnNames,userFlags ] = analyseMakeArguments( nargin,varargin );
-    end
-
-    
-    %% define compiler settings
-    QPOASESPATH = '../../';
-    
-    DEBUGFLAGS = ' ';
-    %DEBUGFLAGS = ' -g CXXDEBUGFLAGS=''$CXXDEBUGFLAGS -Wall -pedantic -Wshadow'' ';
-
-    IFLAGS = [ '-I. -I',QPOASESPATH,'include',' -I',QPOASESPATH,'src',' ' ];
-    CPPFLAGS = [ IFLAGS, DEBUGFLAGS, '-largeArrayDims -D__cpluplus -D__MATLAB__ -D__SINGLE_OBJECT__',' ' ];
-    defaultFlags = '-O -D__NO_COPYRIGHT__ '; %% -D__SUPPRESSANYOUTPUT__
-
-    if ( ispc == 0 )
-        CPPFLAGS  = [ CPPFLAGS, '-DLINUX ',' ' ]; 
-    else
-        CPPFLAGS  = [ CPPFLAGS, '-DWIN32 ',' ' ];
-    end
-
-    if ( isempty(userFlags) > 0 )
-        CPPFLAGS = [ CPPFLAGS, defaultFlags,' ' ];
-    else
-        CPPFLAGS = [ CPPFLAGS, userFlags,' ' ];
-    end
-
-    mexExt = eval('mexext');
-    
-    
-    %% ensure copyright notice is displayed
-    if ~isempty( strfind( CPPFLAGS,'-D__NO_COPYRIGHT__' ) )
-        printCopyrightNotice( );
-    end
-    
-    
-    %% clean if desired
-    if ( doClean > 0 )
-        
-        eval( 'delete *.o;' );
-        eval( ['delete *.',mexExt,'*;'] );
-        disp( [ 'INFO (',mfilename '.m): Cleaned all compiled files.'] );
-        pause( 0.2 );
-        
-    end
-    
-    
-    if ( ~isempty(userFlags) )
-        disp( [ 'INFO (',mfilename '.m): Compiling all files with user-defined compiler flags (''',userFlags,''')...'] );
-    end
-    
-    
-    %% call mex compiler
-    for ii=1:length(fcnNames)
-        
-        cmd = [ 'mex -output ', fcnNames{ii}, ' ', CPPFLAGS, [fcnNames{ii},'.cpp'] ];
-        
-        if ( exist( [fcnNames{ii},'.',mexExt],'file' ) == 0 )
-            
-            eval( cmd );
-            disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' successfully created.'] );
-            
-        else
-            
-            % check modification time of source/Make files and compiled mex file
-            cppFile = dir( [pwd,'/',fcnNames{ii},'.cpp'] );
-            cppFileTimestamp = getTimestamp( cppFile );
-            
-            utilsFile = dir( [pwd,'/qpOASES_matlab_utils.cpp'] );
-            utilsFileTimestamp = getTimestamp( utilsFile );
-            
-            makeFile = dir( [pwd,'/make.m'] );
-            makeFileTimestamp = getTimestamp( makeFile );
-            
-            mexFile = dir( [pwd,'/',fcnNames{ii},'.',mexExt] );
-            if ( isempty(mexFile) == 0 )
-                mexFileTimestamp = getTimestamp( mexFile );
-            else
-                mexFileTimestamp = 0;
-            end
-            
-            if ( ( cppFileTimestamp   >= mexFileTimestamp ) || ...
-                 ( utilsFileTimestamp >= mexFileTimestamp ) || ...
-                 ( makeFileTimestamp  >= mexFileTimestamp ) )
-                eval( cmd );
-                disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' successfully created.'] );
-            else            
-                disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' already exists.'] );
-            end
-            
-        end
-        
-    end
-
-    %% add qpOASES directory to path
-    path( path,pwd );
-
-end
-
-
-function [ doClean,fcnNames,userIFlags ] = analyseMakeArguments( nArgs,args )
-
-    doClean = 0;
-    fcnNames = [];
-    userIFlags = [];
-
-    switch ( nArgs )
-        
-        case 1
-            if ( strcmp( args{1},'all' ) > 0 )
-                fcnNames = { 'qpOASES','qpOASES_sequence' };
-            elseif ( strcmp( args{1},'qpOASES' ) > 0 )
-                fcnNames = { 'qpOASES' };
-            elseif ( strcmp( args{1},'qpOASES_sequence' ) > 0 )
-                fcnNames = { 'qpOASES_sequence' };
-            elseif ( strcmp( args{1},'clean' ) > 0 )
-                doClean = 1;
-            elseif ( strcmp( args{1}(1),'-' ) > 0 )
-                % make clean all with user-specified compiler flags
-                userIFlags = args{1};
-                doClean = 1;
-                fcnNames = { 'qpOASES','qpOASES_sequence' };
-            else
-                error( ['ERROR (',mfilename '.m): Invalid first argument (''',args{1},''')!'] );
-            end
-
-        case 2
-            if ( strcmp( args{1},'clean' ) > 0 )
-                doClean = 1;
-            else
-                error( ['ERROR (',mfilename '.m): First argument must be ''clean'' if two arguments are provided!'] );
-            end
-            
-            if ( strcmp( args{2},'all' ) > 0 )
-                fcnNames = { 'qpOASES','qpOASES_sequence' };
-            elseif ( strcmp( args{2},'qpOASES' ) > 0 )
-                fcnNames = { 'qpOASES' };
-            elseif ( strcmp( args{2},'qpOASES_sequence' ) > 0 )
-                fcnNames = { 'qpOASES_sequence' };
-            else
-                error( ['ERROR (',mfilename '.m): Invalid second argument (''',args{2},''')!'] );
-            end
-            
-        otherwise
-            fcnNames = { 'qpOASES','qpOASES_sequence' };
-            
-    end
-    
-end
-
-
-function [ timestamp ] = getTimestamp( dateString )
-
-    try 
-        timestamp = dateString.datenum;
-    catch
-        timestamp = Inf;
-    end
-
-end
-
-
-function [ ] = printCopyrightNotice( )
-
-    disp( ' ' );
-    disp( 'qpOASES -- An Implementation of the Online Active Set Strategy.' );
-    disp( 'Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,' );
-    disp( 'Christian Kirches et al. All rights reserved.' );
-    disp( ' ' );
-    disp( 'qpOASES is distributed under the terms of the' );
-    disp( 'GNU Lesser General Public License 2.1 in the hope that it will be' );
-    disp( 'useful, but WITHOUT ANY WARRANTY; without even the implied warranty' );
-    disp( 'of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.' );
-    disp( 'See the GNU Lesser General Public License for more details.' );
-    disp( ' ' );
-    disp( ' ' );
-
-end
-
-
-%%
-%%	end of file
-%%
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES.cpp
deleted file mode 100644
index 0f1175b754e..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES.cpp
+++ /dev/null
@@ -1,557 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file interfaces/matlab/qpOASES.cpp
- *	\author Hans Joachim Ferreau, Alexander Buchner (thanks to Aude Perrin)
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Interface for Matlab(R) that enables to call qpOASES as a MEX function.
- *
- */
-
-
-#include <qpOASES.hpp>
-
-
-USING_NAMESPACE_QPOASES
-
-
-#include "qpOASES_matlab_utils.hpp"
-
-/** initialise handle counter of QPInstance class */
-int QPInstance::s_nexthandle = 1;
-
-/** global pointer to QP objects */
-static std::vector<QPInstance *> g_instances;
-
-#include "qpOASES_matlab_utils.cpp"
-
-
-/*
- *	q p O A S E S m e x _ c o n s t r a i n t s
- */
-int qpOASESmex_constraints(		int nV, int nC, int nP,
-								SymmetricMatrix *H, real_t* g, Matrix *A,
-								real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
-								int nWSRin, real_t maxCpuTimeIn,
-								real_t* x0, Options* options,
-								int nOutputs, mxArray* plhs[],
-								double* guessedBounds, double* guessedConstraints
-								)
-{
-	int nWSRout;
-	real_t maxCpuTimeOut;
-	
-	/* 1) Setup initial QP. */
-	QProblem QP( nV,nC );
-	QP.setOptions( *options );
-
-	/* 2) Solve initial QP. */
-	returnValue returnvalue;
-
-	Bounds bounds(nV);
-	Constraints constraints(nC);
-	if (guessedBounds != 0) {
-		for (int i = 0; i < nV; i++) {
-			if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_LOWER);
-			} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_UPPER);
-			} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_INACTIVE);
-			} else {
-				char msg[MAX_STRING_LENGTH];
-				snprintf(msg, MAX_STRING_LENGTH,
-						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
-				myMexErrMsgTxt(msg);
-				return -1;
-			}
-		}
-	}
-
-	if (guessedConstraints != 0) {
-		for (int i = 0; i < nC; i++) {
-			if ( isEqual(guessedConstraints[i],-1.0) == BT_TRUE ) {
-				constraints.setupConstraint(i, ST_LOWER);
-			} else if ( isEqual(guessedConstraints[i],1.0) == BT_TRUE ) {
-				constraints.setupConstraint(i, ST_UPPER);
-			} else if ( isEqual(guessedConstraints[i],0.0) == BT_TRUE ) {
-				constraints.setupConstraint(i, ST_INACTIVE);
-			} else {
-				char msg[MAX_STRING_LENGTH];
-				snprintf(msg, MAX_STRING_LENGTH,
-						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of constraints!");
-				myMexErrMsgTxt(msg);
-				return -1;
-			}
-		}
-	}
-
-	nWSRout = nWSRin;
-	maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-	if (x0 == 0 && guessedBounds == 0 && guessedConstraints == 0)
-		returnvalue = QP.init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut);
-	else
-		returnvalue = QP.init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut, x0, 0,
-				guessedBounds != 0 ? &bounds : 0,
-				guessedConstraints != 0 ? &constraints : 0);
-
-	/* 3) Solve remaining QPs and assign lhs arguments. */
-	/*    Set up pointers to the current QP vectors */
-	real_t* g_current   = g;
-	real_t* lb_current  = lb;
-	real_t* ub_current  = ub;
-	real_t* lbA_current = lbA;
-	real_t* ubA_current = ubA;
-
-	/* Loop through QP sequence. */
-	for ( int k=0; k<nP; ++k )
-	{
-		if ( k > 0 )
-		{
-			/* update pointers to the current QP vectors */
-			g_current = &(g[k*nV]);
-			if ( lb != 0 )
-				lb_current = &(lb[k*nV]);
-			if ( ub != 0 )
-				ub_current = &(ub[k*nV]);
-			if ( lbA != 0 )
-				lbA_current = &(lbA[k*nC]);
-			if ( ubA != 0 )
-				ubA_current = &(ubA[k*nC]);
-
-			nWSRout = nWSRin;
-			maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-			returnvalue = QP.hotstart( g_current,lb_current,ub_current,lbA_current,ubA_current, nWSRout,&maxCpuTimeOut );
-		}
-
-		/* write results into output vectors */
-		obtainOutputs(	k,&QP,returnvalue,nWSRout,maxCpuTimeOut,
-						nOutputs,plhs,nV,nC );
-	}
-
-	//QP.writeQpDataIntoMatFile( "qpDataMat0.mat" );
-
-	return 0;
-}
-
-
-
-/*
- *	q p O A S E S m e x _ b o u n d s
- */
-int qpOASESmex_bounds(	int nV, int nP,
-						SymmetricMatrix *H, real_t* g,
-						real_t* lb, real_t* ub,
-						int nWSRin, real_t maxCpuTimeIn,
-						real_t* x0, Options* options,
-						int nOutputs, mxArray* plhs[],
-						double* guessedBounds
-						)
-{
-	int nWSRout;
-	real_t maxCpuTimeOut;
-
-	/* 1) Setup initial QP. */
-	QProblemB QP( nV );
-	QP.setOptions( *options );
-
-	/* 2) Solve initial QP. */
-	returnValue returnvalue;
-
-	Bounds bounds(nV);
-	if (guessedBounds != 0) {
-		for (int i = 0; i < nV; i++) {
-			if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_LOWER);
-			} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_UPPER);
-			} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_INACTIVE);
-			} else {
-				char msg[MAX_STRING_LENGTH];
-				snprintf(msg, MAX_STRING_LENGTH,
-						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
-				myMexErrMsgTxt(msg);
-				return -1;
-			}
-		}
-	}
-
-	nWSRout = nWSRin;
-	maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-	if (x0 == 0 && guessedBounds == 0)
-		returnvalue = QP.init( H,g,lb,ub, nWSRout,&maxCpuTimeOut );
-	else
-		returnvalue = QP.init( H,g,lb,ub, nWSRout,&maxCpuTimeOut, x0, 0,
-				guessedBounds != 0 ? &bounds : 0);
-
-	/* 3) Solve remaining QPs and assign lhs arguments. */
-	/*    Set up pointers to the current QP vectors */
-	real_t* g_current  = g;
-	real_t* lb_current = lb;
-	real_t* ub_current = ub;
-
-	/* Loop through QP sequence. */
-	for ( int k=0; k<nP; ++k )
-	{
-		if ( k > 0 )
-		{
-			/* update pointers to the current QP vectors */
-			g_current = &(g[k*nV]);
-			if ( lb != 0 )
-				lb_current = &(lb[k*nV]);
-			if ( ub != 0 )
-				ub_current = &(ub[k*nV]);
-
-            nWSRout = nWSRin;
-			maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-			returnvalue = QP.hotstart( g_current,lb_current,ub_current, nWSRout,&maxCpuTimeOut );
-		}
-
-		/* write results into output vectors */
-		obtainOutputs(	k,&QP,returnvalue,nWSRout,maxCpuTimeOut,
-						nOutputs,plhs,nV );
-	}
-
-	return 0;
-}
-
-
-
-/*
- *	m e x F u n c t i o n
- */
-void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
-{
-	/* inputs */
-	SymmetricMatrix *H=0;
-	Matrix *A=0;
-	real_t *g=0, *lb=0, *ub=0, *lbA=0, *ubA=0, *x0=0;
-	int H_idx, g_idx, A_idx, lb_idx, ub_idx, lbA_idx, ubA_idx, options_idx=-1, x0_idx=-1, auxInput_idx=-1;
-
-	double *guessedBoundsAndConstraints = 0;
-	double *guessedBounds = 0, *guessedConstraints = 0;
-
-    /* Setup default options */
-	Options options;
-	options.printLevel = PL_LOW;
-	#ifdef __DEBUG__
-	options.printLevel = PL_HIGH;
-	#endif
-	#ifdef __SUPPRESSANYOUTPUT__
-	options.printLevel = PL_NONE;
-	#endif
-
-	/* dimensions */
-	unsigned int nV=0, nC=0, nP=0;
-	BooleanType isSimplyBoundedQp = BT_FALSE;
-
-	/* sparse matrix indices and values */
-	sparse_int_t *Hir = 0, *Hjc = 0, *Air = 0, *Ajc = 0;
-	real_t *Hv = 0, *Av = 0;
-
-	/* I) CONSISTENCY CHECKS: */
-	/* 1a) Ensure that qpOASES is called with a feasible number of input arguments. */
-	if ( ( nrhs < 4 ) || ( nrhs > 9 ) )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES' for further information." );
-		return;
-	}
-    
-	/* 2) Check for proper number of output arguments. */
-	if ( nlhs > 6 )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): At most six output arguments are allowed: \n    [x,fval,exitflag,iter,lambda,workingSet]!" );
-		return;
-	}
-	if ( nlhs < 1 )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): At least one output argument is required: [x,...]!" );
-		return;
-	}
-
-
-	/* II) PREPARE RESPECTIVE QPOASES FUNCTION CALL: */
-	/*     Choose between QProblem and QProblemB object and assign the corresponding
-	 *     indices of the input pointer array in to order to access QP data correctly. */
-	H_idx = 0;
-	g_idx = 1;
-	nV = (int)mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
-	nP = (int)mxGetN( prhs[ g_idx ] ); /* number of columns of the gradient matrix (vectors series have to be stored columnwise!) */
-
-	if ( nrhs <= 6 )
-        isSimplyBoundedQp = BT_TRUE;
-	else
-		isSimplyBoundedQp = BT_FALSE;
-
-
-	/* 0) Check whether options are specified .*/
-	if ( isSimplyBoundedQp == BT_TRUE )
-	{
-		if ( ( nrhs >= 5 ) && ( !mxIsEmpty(prhs[4]) ) && ( mxIsStruct(prhs[4]) ) )
-			options_idx = 4;
-	}
-	else
-	{
-		/* Consistency check */
-		if ( ( !mxIsEmpty(prhs[4]) ) && ( mxIsStruct(prhs[4]) ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Fifth input argument must not be a struct when solving QP with general constraints!\nType 'help qpOASES' for further information." );
-			return;
-		}
-
-		if ( ( nrhs >= 8 ) && ( !mxIsEmpty(prhs[7]) ) && ( mxIsStruct(prhs[7]) ) )
-			options_idx = 7;
-	}
-
-	// Is the third argument constraint Matrix A?
-	int numberOfColumns = (int)mxGetN(prhs[2]);
-
-	/* 1) Simply bounded QP. */
-	if ( ( isSimplyBoundedQp == BT_TRUE ) ||
-		 ( ( numberOfColumns == 1 ) && ( nV != 1 ) ) )
-	{
-		lb_idx   = 2;
-		ub_idx   = 3;
-
-		if ( ( nrhs >= 6 ) && ( !mxIsEmpty(prhs[5]) ) )
-		{ 
-			/* auxInput specified */
-			if ( mxIsStruct(prhs[5]) )
-			{
-				auxInput_idx = 5;
-				x0_idx = -1;
-			}
-			else
-			{
-				auxInput_idx = -1;
-				x0_idx = 5;
-			}
-		}
-		else
-		{
-			auxInput_idx = -1;
-			x0_idx = -1;
-		}
-	}
-	else
-	{
-		A_idx = 2;
-
-		/* If constraint matrix is empty, use a QProblemB object! */
-		if ( mxIsEmpty( prhs[ A_idx ] ) )
-		{
-			lb_idx   = 3;
-			ub_idx   = 4;
-
-			nC = 0;
-		}
-		else
-		{
-			lb_idx   = 3;
-			ub_idx   = 4;
-			lbA_idx  = 5;
-			ubA_idx  = 6;
-
-			nC = (int)mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */
-		}
-
-		if ( ( nrhs >= 9 ) && ( !mxIsEmpty(prhs[8]) ) )
-		{ 
-			/* auxInput specified */
-			if ( mxIsStruct(prhs[8]) )
-			{
-				auxInput_idx = 8;
-				x0_idx = -1;
-			}
-			else
-			{
-				auxInput_idx = -1;
-				x0_idx = 8;
-			}
-		}
-		else
-		{
-			auxInput_idx = -1;
-			x0_idx = -1;
-		}
-	}
-
-
-	/* ensure that data is given in real_t precision */
-	if ( ( mxIsDouble( prhs[ H_idx ] ) == 0 ) ||
-		 ( mxIsDouble( prhs[ g_idx ] ) == 0 ) )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
-		return;
-	}
-
-	/* check if supplied data contains 'NaN' or 'Inf' */
-	if (containsNaNorInf(prhs, nV * nV, H_idx, 0) == BT_TRUE) {
-		return;
-	}
-
-	if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
-		return;
-	}
-
-	if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
-		return;
-	}
-	if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
-		return;
-	}
-
-	/* Check inputs dimensions and assign pointers to inputs. */
-	if ( mxGetN( prhs[ H_idx ] ) != nV )
-	{
-		char msg[MAX_STRING_LENGTH]; 
-		snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Hessian matrix input dimension mismatch (%ld != %d)!", 
-				(long int)mxGetN(prhs[H_idx]), nV);
-		myMexErrMsgTxt(msg);
-		return;
-	}
-
-	if ( nC > 0 )
-	{
-		/* ensure that data is given in real_t precision */
-		if ( mxIsDouble( prhs[ A_idx ] ) == 0 )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
-			return;
-		}
-
-		/* Check inputs dimensions and assign pointers to inputs. */
-		if ( mxGetN( prhs[ A_idx ] ) != nV )
-		{
-			char msg[MAX_STRING_LENGTH]; 
-			snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Constraint matrix input dimension mismatch (%ld != %d)!", 
-					(long int)mxGetN(prhs[A_idx]), nV);
-			myMexErrMsgTxt(msg);
-			return;
-		}
-
-		if (containsNaNorInf(prhs, nV * nC, A_idx, 0) == BT_TRUE) {
-			return;
-		}
-		if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
-			return;
-		}
-		if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
-			return;
-		}
-	}
-
-	/* check dimensions and copy auxInputs */
-	if ( smartDimensionCheck( &g,nV,nP, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
-		return;
-
-	if ( smartDimensionCheck( &lb,nV,nP, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
-		return;
-
-	if ( smartDimensionCheck( &ub,nV,nP, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
-		return;
-
-	if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,x0_idx ) != SUCCESSFUL_RETURN )
-		return;
-
-	if ( nC > 0 )
-	{
-		if ( smartDimensionCheck( &lbA,nC,nP, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &ubA,nC,nP, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
-			return;
-	}
-
-	if ( auxInput_idx >= 0 )
-		setupAuxiliaryInputs( prhs[auxInput_idx],nV,nC, &x0,&guessedBoundsAndConstraints,&guessedBounds,&guessedConstraints );
-
-	
-	/* III) ACTUALLY PERFORM QPOASES FUNCTION CALL: */
-	int nWSRin = 5*(nV+nC);
-	real_t maxCpuTimeIn = -1.0;
-
-	if ( options_idx > 0 )
-		setupOptions( &options,prhs[options_idx],nWSRin,maxCpuTimeIn );
-
-	/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
-	setupHessianMatrix(	prhs[H_idx],nV, &H,&Hir,&Hjc,&Hv );
-
-	/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
-	if ( nC > 0 )
-		setupConstraintMatrix( prhs[A_idx],nV,nC, &A,&Air,&Ajc,&Av );
-
-	allocateOutputs( nlhs,plhs,nV,nC,nP );
-
-	if ( nC == 0 )
-	{
-		/* Call qpOASES (using QProblemB class). */
-		qpOASESmex_bounds(	nV,nP,
-							H,g,
-							lb,ub,
-							nWSRin,maxCpuTimeIn,
-							x0,&options,
-							nlhs,plhs,
-							guessedBounds
-							);
-
-		deleteAuxiliaryInputs( &guessedBounds,0 );
-        delete H;
-		if (Hv != 0) delete[] Hv;
-		if (Hjc != 0) delete[] Hjc;
-		if (Hir != 0) delete[] Hir;
-		return;
-	}
-	else
-	{
-		/* Call qpOASES (using QProblem class). */
-		qpOASESmex_constraints(	nV,nC,nP,
-								H,g,A,
-								lb,ub,lbA,ubA,
-								nWSRin,maxCpuTimeIn,
-								x0,&options,
-								nlhs,plhs,
-								guessedBounds, guessedConstraints
-								);
-		
-		deleteAuxiliaryInputs( &guessedBounds,&guessedConstraints );
-		delete A;
-		delete H;
-		if (Av != 0) delete[] Av;
-		if (Ajc != 0) delete[] Ajc;
-		if (Air != 0) delete[] Air;
-		if (Hv != 0) delete[] Hv;
-		if (Hjc != 0) delete[] Hjc;
-		if (Hir != 0) delete[] Hir;
-		return;
-	}
-}
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES.m
deleted file mode 100644
index e5ab89631e9..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES.m
+++ /dev/null
@@ -1,77 +0,0 @@
-%qpOASES -- An Implementation of the Online Active Set Strategy.
-%Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%Christian Kirches et al. All rights reserved.
-%
-%qpOASES is distributed under the terms of the
-%GNU Lesser General Public License 2.1 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.
-%
-%---------------------------------------------------------------------------------
-%
-%qpOASES solves (a series) of quadratic programming (QP) problems of the
-%following form:
-%
-%                min   1/2*x'Hx + x'g
-%                s.t.  lb  <=  x <= ub
-%                      lbA <= Ax <= ubA  {optional}
-%
-%Call
-%
-%    [x,fval,exitflag,iter,lambda,auxOutput] = 
-%                     qpOASES( H,g,A,lb,ub,lbA,ubA{,options{,auxInput}} )
-%
-%for solving the above-mentioned QP. H must be a symmetric (but possibly 
-%indefinite) matrix and all vectors g, lb, ub, lbA, ubA have to be given
-%as column vectors. Options can be generated using the qpOASES_options command, 
-%otherwise default values are used. Optionally, further auxiliary inputs
-%may be generated using qpOASES_auxInput command and passed to the solver.
-%Both matrices H or A may be passed in sparse matrix format.
-%
-%Call
-%
-%    [x,fval,exitflag,iter,lambda,auxOutput] =
-%                     qpOASES( H,g,lb,ub{,options{,auxInput}} )
-%
-%for solving the above-mentioned QP without general constraints.
-%
-%
-%Optional outputs (only x is mandatory):
-%    x          -  Optimal primal solution vector (if exitflag==0).
-%    fval       -  Optimal objective function value (if exitflag==0).
-%    exitflag   -   0: QP problem solved,
-%                   1: QP could not be solved within given number of iterations,
-%                  -1: QP could not be solved due to an internal error,
-%                  -2: QP is infeasible (and thus could not be solved),
-%                  -3: QP is unbounded (and thus could not be solved).
-%    iter       -  Number of active set iterations actually performed.
-%    lambda     -  Optimal dual solution vector (if status==0).
-%    auxOutput  -  Struct containing auxiliary outputs as described below.
-%
-%The auxOutput struct contains the following entries:
-%    workingSet -  The working set at point x. The working set is a subset
-%                  of the active set (which is the set of all indices
-%                  corresponding to bounds/constraints that hold with 
-%                  equality). The working set corresponds to bound/
-%                  constraint row vectors forming a linear independent set.
-%                  The first nV elements correspond to the bounds, the last
-%                  nC elements to the constraints.
-%                  The working set is encoded as follows:
-%                   1: bound/constraint at its upper bound
-%                   0: bound/constraint not at any bound
-%                  -1: bound/constraint at its lower bound
-%    cpuTime    -  Internally measured CPU time for solving QP.
-%
-%
-%If not a single QP but a sequence of QPs with varying vectors is to be solved,
-%the i-th QP is given by the i-th columns of the QP vectors g, lb, ub, lbA, ubA
-%(i.e. they are matrices in this case). Both matrices H and A remain constant.
-%
-%See also QPOASES_OPTIONS, QPOASES_AUXINPUT, QPOASES_SEQUENCE
-%
-%
-%For additional information see the qpOASES User's Manual or
-%visit http://www.qpOASES.org/.
-%
-%Please send remarks and questions to support@qpOASES.org!
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_auxInput.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_auxInput.m
deleted file mode 100644
index 6d46efe149d..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_auxInput.m
+++ /dev/null
@@ -1,103 +0,0 @@
-%qpOASES -- An Implementation of the Online Active Set Strategy.
-%Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%Christian Kirches et al. All rights reserved.
-%
-%qpOASES is distributed under the terms of the
-%GNU Lesser General Public License 2.1 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.
-%
-%---------------------------------------------------------------------------------
-%
-%Returns a struct containing all possible auxiliary inputs to be passed 
-%to qpOASES.
-%
-%Call
-%    auxInput = qpOASES_auxInput();
-%to obtain a struct with all auxiliary inputs empty.
-%
-%Call
-%    auxInput = qpOASES_auxInput( 'input1',value1,'input2',value2,... )
-%to obtain a struct with 'input1' set to value1 etc. and all remaining
-%auxiliary inputs empty.
-%
-%Call
-%    auxInput = qpOASES_auxInput( oldInputs,'input1',value1,... )
-%to obtain a copy of the options struct oldInputs but with 'input1' set to 
-%value1 etc.
-%
-%
-%qpOASES features the following auxiliary inputs:
-%  x0                   -  Initial guess for optimal primal solution.
-%  guessedWorkingSet    -  Initial guess for working set at optimal
-%                          solution. The first nV elements correspond 
-%                          to the bounds, the last nC elements to the 
-%                          constraints.
-%                          The working set needs to be encoded as follows:
-%                          1: bound/constraint at its upper bound
-%                          0: bound/constraint not at any bound
-%                         -1: bound/constraint at its lower bound
-%
-%
-%See also QPOASES, QPOASES_SEQUENCE, QPOASES_OPTIONS
-%
-%
-%For additional information see the qpOASES User's Manual or
-%visit http://www.qpOASES.org/.
-%
-%Please send remarks and questions to support@qpOASES.org!
-function [ auxInput ] = qpOASES_auxInput( varargin )
-
-	firstIsStruct = 0;
-
-	if ( nargin == 0 ) 
-		auxInput = qpOASES_emptyAuxInput();
-	else
-		if ( isstruct( varargin{1} ) )
-			if ( mod( nargin,2 ) ~= 1 )
-				error('ERROR (qpOASES_auxInput): Auxiliary inputs must be specified in pairs!');
-			end
-			auxInput = varargin{1};
-			firstIsStruct = 1;
-        else
-            if ( mod( nargin,2 ) ~= 0 )
-				error('ERROR (qpOASES_auxInput): Auxiliary inputs must be specified in pairs!');
-            end
-            auxInput = qpOASES_emptyAuxInput();
-		end
-	end
-
-	% set options to user-defined values  
-	for i=(1+firstIsStruct):2:nargin
-
-		argName  = varargin{i};
-		argValue = varargin{i+1};
-
-        if ( ( isempty( argName ) ) || ( ~ischar( argName ) ) )
-			error('ERROR (qpOASES_auxInput): Argmument no. %d has to be a non-empty string!',i );
-        end
-			
-        if ( ( ischar(argValue) ) || ( ~isnumeric( argValue ) ) )
-			error('ERROR (qpOASES_auxInput): Argmument no. %d has to be a numerical constant!',i+1 );
-        end
-
-        if ( ~isfield( auxInput,argName ) )
-			error('ERROR (qpOASES_auxInput): Argmument no. %d is not a valid auxiliary input!',i );
-        end
-
-		eval( ['auxInput.',argName,' = argValue;'] );
-
-	end
-
-end
-
-
-function [ auxInput ] = qpOASES_emptyAuxInput( )
-
-	% setup auxiliary input struct with all entries empty
-	auxInput = struct(	'x0',                 [], ...
-						'guessedWorkingSet',  []  ...
-                        );
-
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_matlab_utils.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_matlab_utils.cpp
deleted file mode 100644
index 7362704f46a..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_matlab_utils.cpp
+++ /dev/null
@@ -1,885 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file interfaces/matlab/qpOASES_matlab_utils.cpp
- *	\author Hans Joachim Ferreau, Alexander Buchner
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Collects utility functions for Interface to Matlab(R) that
- *	enables to call qpOASES as a MEX function.
- *
- */
-
-
-
-QPInstance::QPInstance(	int _nV, int _nC, BooleanType _isSimplyBounded
-						)
-{
-	handle = s_nexthandle++;
-
-	if ( _nC > 0 )
-		isSimplyBounded = BT_FALSE;
-	else
-		isSimplyBounded = _isSimplyBounded;
-	
-	if ( isSimplyBounded == BT_TRUE )
-	{
-		sqp = 0;
-		qpb = new QProblemB( _nV );
-	}
-	else
-	{
-		sqp = new SQProblem( _nV,_nC );
-		qpb = 0;
-	}
-
-	H = 0;
-	A = 0;
-	Hir = 0; 
-	Hjc = 0; 
-	Air = 0; 
-	Ajc = 0;
-	Hv = 0;
-	Av = 0;
-}	
-
-
-QPInstance::~QPInstance( )
-{		
-	deleteQPMatrices();
-
-	if ( sqp != 0 )
-	{
-		delete sqp;
-		sqp = 0;
-	}
-
-	if ( qpb != 0 )
-	{
-		delete qpb;
-		qpb = 0;
-	}
-}
-
-
-returnValue QPInstance::deleteQPMatrices( )
-{
-	if ( H != 0 )
-	{
-		delete H;
-		H = 0;
-	}
-
-	if (Hv != 0)
-	{
-		delete[] Hv;
-		Hv = 0;
-	}
-	
-	if (Hjc != 0)
-	{
-		delete[] Hjc;
-		Hjc = 0;
-	}
-	
-	if (Hir != 0)
-	{
-		delete[] Hir;
-		Hir = 0;
-	}
-	
-	if ( A != 0 )
-	{
-		delete A;
-		A = 0;
-	}
-
-	if (Av != 0)
-	{
-		delete[] Av;
-		Av = 0;
-	}
-	
-	if (Ajc != 0)
-	{
-		delete[] Ajc;
-		Ajc = 0;
-	}
-	
-	if (Air != 0)
-	{
-		delete[] Air;
-		Air = 0;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-int QPInstance::getNV() const
-{
-    if ( sqp != 0 )
-        return sqp->getNV();
-    
-    if ( qpb != 0 )
-        return qpb->getNV();
-    
-    return 0;
-}
-
-
-int QPInstance::getNC() const
-{
-    if ( sqp != 0 )
-        return sqp->getNC();
-   
-    return 0;
-}
-
-
-/*
- *	a l l o c a t e Q P r o b l e m I n s t a n c e
- */
-int allocateQPInstance(	int nV, int nC, BooleanType isSimplyBounded, const Options *options
-						)
-{
-	QPInstance* inst = new QPInstance( nV,nC,isSimplyBounded );
-
-	if ( inst->sqp != 0 )
-		inst->sqp->setOptions ( *options );
-	
-	if ( inst->qpb != 0 )
-		inst->qpb->setOptions ( *options );
-
-	g_instances.push_back (inst);
-	return inst->handle;
-}
-
-
-/*
- *  g e t Q P r o b l e m I n s t a n c e
- */
-QPInstance* getQPInstance( int handle )
-{
-	unsigned int ii;
-	// TODO: this may become slow ...
-	for (ii = 0; ii < g_instances.size (); ++ii)
-		if (g_instances[ii]->handle == handle)
-			return g_instances[ii];
-	return 0;
-}
-
-
-/*
- *	d e l e t e Q P r o b l e m I n s t a n c e
- */
-void deleteQPInstance( int handle )
-{
-	QPInstance *instance = getQPInstance (handle);
-	if (instance != 0) {
-		for (std::vector<QPInstance*>::iterator itor = g_instances.begin ();
-		     itor != g_instances.end (); ++itor)
-		     if ((*itor)->handle == handle) {
-				g_instances.erase (itor);
-				break;
-			}
-		delete instance;
-	}
-}
-
-
-
-/*
- *	s m a r t D i m e n s i o n C h e c k
- */
-returnValue smartDimensionCheck(	real_t** input, unsigned int m, unsigned int n, BooleanType emptyAllowed,
-									const mxArray* prhs[], int idx
-									)
-{
-	/* If index is negative, the input does not exist. */
-	if ( idx < 0 )
-	{
-		*input = 0;
-		return SUCCESSFUL_RETURN;
-	}
-
-	/* Otherwise the input has been passed by the user. */
-	if ( mxIsEmpty( prhs[ idx ] ) )
-	{
-		/* input is empty */
-		if ( ( emptyAllowed == BT_TRUE ) || ( idx == 0 ) ) /* idx==0 used for auxInput */
-		{
-			*input = 0;
-			return SUCCESSFUL_RETURN;
-		}
-		else
-		{
-			char msg[MAX_STRING_LENGTH];
-			if ( idx > 0 )
-				snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Empty argument %d not allowed!", idx+1);
-			myMexErrMsgTxt( msg );
-			return RET_INVALID_ARGUMENTS;
-		}
-	}
-	else
-	{
-		/* input is non-empty */
-        if ( mxIsSparse( prhs[ idx ] ) == 0 )
-        {
-            if ( ( mxGetM( prhs[ idx ] ) == m ) && ( mxGetN( prhs[ idx ] ) == n ) )
-            {
-                *input = (real_t*) mxGetPr( prhs[ idx ] );
-                return SUCCESSFUL_RETURN;
-            }
-            else
-            {
-                char msg[MAX_STRING_LENGTH];
-				if ( idx > 0 )
-					snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Input dimension mismatch for argument %d ([%ld,%ld] ~= [%d,%d]).",
-							 idx+1, (long int)mxGetM(prhs[idx]), (long int)mxGetN(prhs[idx]), m, n);
-				else /* idx==0 used for auxInput */
-					snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Input dimension mismatch for some auxInput entry ([%ld,%ld] ~= [%d,%d]).",
-							 (long int)mxGetM(prhs[idx]), (long int)mxGetN(prhs[idx]), m, n);
-                myMexErrMsgTxt( msg );
-                return RET_INVALID_ARGUMENTS;
-            }
-        }
-        else
-        {
-            char msg[MAX_STRING_LENGTH];
-			if ( idx > 0 )
-				snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Vector argument %d must not be in sparse format!", idx+1);
-			else /* idx==0 used for auxInput */
-				snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): auxInput entries must not be in sparse format!" );
-			myMexErrMsgTxt( msg );
-			return RET_INVALID_ARGUMENTS;
-        }
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	c o n t a i n s N a N
- */
-BooleanType containsNaN( const real_t* const data, int dim )
-{
-	int i;
-
-	if ( data == 0 )
-		return BT_FALSE;
-
-	for ( i = 0; i < dim; ++i )
-		if ( mxIsNaN(data[i]) == 1 )
-			return BT_TRUE;
-
-	return BT_FALSE;
-}
-
-
-/*
- *	c o n t a i n s I n f
- */
-BooleanType containsInf( const real_t* const data, int dim )
-{
-	int i;
-
-	if ( data == 0 )
-		return BT_FALSE;
-
-	for ( i = 0; i < dim; ++i )
-		if ( mxIsInf(data[i]) == 1 )
-			return BT_TRUE;
-
-	return BT_FALSE;
-}
-
-
-/*
- *	c o n t a i n s N a N o r I n f
- */
-BooleanType containsNaNorInf(const mxArray* prhs[], int dim, int rhs_index,
-		bool mayContainInf) {
-
-	char msg[MAX_STRING_LENGTH];
-
-	// overwrite dim for sparse matrices
-	if (mxIsSparse(prhs[rhs_index]) == 1) {
-		dim = mxGetNzmax(prhs[rhs_index]);
-	}
-
-	if (containsNaN((real_t*) mxGetPr(prhs[rhs_index]), dim) == BT_TRUE) {
-		snprintf(msg, MAX_STRING_LENGTH,
-				"ERROR (qpOASES): Argument %d contains 'NaN' !", rhs_index + 1);
-		myMexErrMsgTxt(msg);
-		return BT_TRUE;
-	}
-
-	if (mayContainInf == 0) {
-		if (containsInf((real_t*) mxGetPr(prhs[rhs_index]), dim) == BT_TRUE) {
-			snprintf(msg, MAX_STRING_LENGTH,
-					"ERROR (qpOASES): Argument %d contains 'Inf' !",
-					rhs_index + 1);
-			myMexErrMsgTxt(msg);
-			return BT_TRUE;
-		}
-	}
-
-	return BT_FALSE;
-}
-
-
-/*
- *	c o n v e r t F o r t r a n T o C
- */
-returnValue convertFortranToC( const real_t* const A_for, int nV, int nC, real_t* const A )
-{
-	int i,j;
-
-	for ( i=0; i<nC; ++i )
-		for ( j=0; j<nV; ++j )
-			A[i*nV + j] = A_for[j*nC + i];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	h a s O p t i o n s V a l u e
- */
-BooleanType hasOptionsValue( const mxArray* optionsPtr, const char* const optionString, double** optionValue )
-{
-	mxArray* optionName = mxGetField( optionsPtr,0,optionString );
-
-	if ( optionName == 0 )
-	{
-		char msg[MAX_STRING_LENGTH];
-		snprintf(msg, MAX_STRING_LENGTH, "Option struct does not contain entry '%s', using default value instead!", optionString );
-		mexWarnMsgTxt( msg );
-		return BT_FALSE;
-	}
-
-	if ( ( mxIsEmpty(optionName) == false ) && ( mxGetM( optionName ) == 1 ) && ( mxGetN( optionName ) == 1 ) )
-	{
-		*optionValue = mxGetPr( optionName );
-		return BT_TRUE;
-	}
-	else
-	{
-		char msg[MAX_STRING_LENGTH];
-		snprintf(msg, MAX_STRING_LENGTH, "Option '%s' is not a scalar, using default value instead!", optionString );
-		mexWarnMsgTxt( msg );
-		return BT_FALSE;
-	}
-}
-
-
-/*
- *	s e t u p O p t i o n s
- */
-returnValue setupOptions( Options* options, const mxArray* optionsPtr, int& nWSRin, real_t& maxCpuTime )
-{
-	double* optionValue;
-	int optionValueInt;
-
-	/* Check for correct number of option entries;
-	 * may occur, e.g., if user types options.<misspelledName> = <someValue>; */
-	if ( mxGetNumberOfFields(optionsPtr) != 31 )
-		mexWarnMsgTxt( "Options might be set incorrectly as struct has wrong number of entries!\n         Type 'help qpOASES_options' for further information." );
-
-
-	if ( hasOptionsValue( optionsPtr,"maxIter",&optionValue ) == BT_TRUE )
-		if ( *optionValue >= 0.0 )
-			nWSRin = (int)*optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"maxCpuTime",&optionValue ) == BT_TRUE )
-		if ( *optionValue >= 0.0 )
-			maxCpuTime = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"printLevel",&optionValue ) == BT_TRUE )
-	{
-        #ifdef __SUPPRESSANYOUTPUT__
-        options->printLevel = PL_NONE;
-        #else
-		optionValueInt = (int)*optionValue;
-		options->printLevel = (REFER_NAMESPACE_QPOASES PrintLevel)optionValueInt;
-        if ( options->printLevel < PL_DEBUG_ITER )
-            options->printLevel = PL_DEBUG_ITER;
-        if ( options->printLevel > PL_HIGH )
-            options->printLevel = PL_HIGH;       
-        #endif
-	}
-
-	if ( hasOptionsValue( optionsPtr,"enableRamping",&optionValue ) == BT_TRUE )
-	{
-		optionValueInt = (int)*optionValue;
-		options->enableRamping = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
-	}
-
-	if ( hasOptionsValue( optionsPtr,"enableFarBounds",&optionValue ) == BT_TRUE )
-	{
-		optionValueInt = (int)*optionValue;
-		options->enableFarBounds = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
-	}
-
-	if ( hasOptionsValue( optionsPtr,"enableFlippingBounds",&optionValue ) == BT_TRUE )
-	{
-		optionValueInt = (int)*optionValue;
-		options->enableFlippingBounds = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
-	}
-
-	if ( hasOptionsValue( optionsPtr,"enableRegularisation",&optionValue ) == BT_TRUE )
-	{
-		optionValueInt = (int)*optionValue;
-		options->enableRegularisation = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
-	}
-
-	if ( hasOptionsValue( optionsPtr,"enableFullLITests",&optionValue ) == BT_TRUE )
-	{
-		optionValueInt = (int)*optionValue;
-		options->enableFullLITests = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
-	}
-
-	if ( hasOptionsValue( optionsPtr,"enableNZCTests",&optionValue ) == BT_TRUE )
-	{
-		optionValueInt = (int)*optionValue;
-		options->enableNZCTests = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
-	}
-
-	if ( hasOptionsValue( optionsPtr,"enableDriftCorrection",&optionValue ) == BT_TRUE )
-		options->enableDriftCorrection = (int)*optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"enableCholeskyRefactorisation",&optionValue ) == BT_TRUE )
-		options->enableCholeskyRefactorisation = (int)*optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"enableEqualities",&optionValue ) == BT_TRUE )
-	{
-		optionValueInt = (int)*optionValue;
-		options->enableEqualities = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
-	}
-
-
-	if ( hasOptionsValue( optionsPtr,"terminationTolerance",&optionValue ) == BT_TRUE )
-		options->terminationTolerance = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"boundTolerance",&optionValue ) == BT_TRUE )
-		options->boundTolerance = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"boundRelaxation",&optionValue ) == BT_TRUE )
-		options->boundRelaxation = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"epsNum",&optionValue ) == BT_TRUE )
-		options->epsNum = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"epsDen",&optionValue ) == BT_TRUE )
-		options->epsDen = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"maxPrimalJump",&optionValue ) == BT_TRUE )
-		options->maxPrimalJump = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"maxDualJump",&optionValue ) == BT_TRUE )
-		options->maxDualJump = *optionValue;
-
-
-	if ( hasOptionsValue( optionsPtr,"initialRamping",&optionValue ) == BT_TRUE )
-		options->initialRamping = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"finalRamping",&optionValue ) == BT_TRUE )
-		options->finalRamping = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"initialFarBounds",&optionValue ) == BT_TRUE )
-		options->initialFarBounds = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"growFarBounds",&optionValue ) == BT_TRUE )
-		options->growFarBounds = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"initialStatusBounds",&optionValue ) == BT_TRUE )
-	{
-		optionValueInt = (int)*optionValue;
-		if ( optionValueInt < -1 ) 
-			optionValueInt = -1;
-		if ( optionValueInt > 1 ) 
-			optionValueInt = 1;
-		options->initialStatusBounds = (REFER_NAMESPACE_QPOASES SubjectToStatus)optionValueInt;
-	}
-
-	if ( hasOptionsValue( optionsPtr,"epsFlipping",&optionValue ) == BT_TRUE )
-		options->epsFlipping = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"numRegularisationSteps",&optionValue ) == BT_TRUE )
-		options->numRegularisationSteps = (int)*optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"epsRegularisation",&optionValue ) == BT_TRUE )
-		options->epsRegularisation = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"numRefinementSteps",&optionValue ) == BT_TRUE )
-		options->numRefinementSteps = (int)*optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"epsIterRef",&optionValue ) == BT_TRUE )
-		options->epsIterRef = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"epsLITests",&optionValue ) == BT_TRUE )
-		options->epsLITests = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"epsNZCTests",&optionValue ) == BT_TRUE )
-		options->epsNZCTests = *optionValue;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	s e t u p A u x i l i a r y I n p u t s
- */
-returnValue setupAuxiliaryInputs(	const mxArray* auxInput, unsigned int nV, unsigned int nC,
-									double** x0, double** guessedBoundsAndConstraints, double** guessedBounds, double** guessedConstraints
-									)
-{
-	unsigned int i;
-	mxArray* curField = 0;
-
-	/* x0 */
-	curField = mxGetField( auxInput,0,"x0" );
-	if ( curField == NULL )
-		mexWarnMsgTxt( "auxInput struct does not contain entry 'x0'!\n         Type 'help qpOASES_auxInput' for further information." );
-	else
-	{
-		*x0 = mxGetPr(curField);
-		if ( smartDimensionCheck( x0,nV,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
-			return RET_INVALID_ARGUMENTS;
-	}
-
-	/* guessedWorkingSet */
-	curField = mxGetField( auxInput,0,"guessedWorkingSet" );
-	if ( curField == NULL )
-		mexWarnMsgTxt( "auxInput struct does not contain entry 'guessedWorkingSet'!\n         Type 'help qpOASES_auxInput' for further information." );
-	else
-	{
-		*guessedBoundsAndConstraints = mxGetPr(curField);
-		if ( smartDimensionCheck( guessedBoundsAndConstraints,nV+nC,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
-			return RET_INVALID_ARGUMENTS;
-
-		if ( *guessedBoundsAndConstraints != 0 )
-		{
-			*guessedBounds = new double[nV];
-			for (i = 0; i < nV; i++)
-				(*guessedBounds)[i] = (*guessedBoundsAndConstraints)[i];
-
-			if ( nC > 0 )
-			{
-				*guessedConstraints = new double[nC];
-				for (i = 0; i < nC; i++)
-					(*guessedConstraints)[i] = (*guessedBoundsAndConstraints)[i + nV];
-			}
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	d e l e t e A u x i l i a r y I n p u t s
- */
-returnValue deleteAuxiliaryInputs(	double** guessedBounds, double** guessedConstraints
-									)
-{
-	if ( ( guessedBounds != 0 ) && ( *guessedBounds != 0 ) )
-	{
-		delete[] (*guessedBounds);
-		*guessedBounds = 0;
-	}
-
-	if ( ( guessedConstraints != 0 ) && ( guessedConstraints != 0 ) )
-	{
-		delete[] (*guessedConstraints);
-		*guessedConstraints = 0;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	a l l o c a t e O u t p u t s
- */
-returnValue allocateOutputs(	int nlhs, mxArray* plhs[], int nV, int nC = 0, int nP = 1, int handle = -1
-								)
-{
-	/* Create output vectors and assign pointers to them. */
-	int curIdx = 0;
-
-	/* handle */
-	if ( handle >= 0 )
-		plhs[curIdx++] = mxCreateDoubleMatrix( 1, 1, mxREAL );
-
-	/* x */
-	plhs[curIdx++] = mxCreateDoubleMatrix( nV, nP, mxREAL );
-
-	if ( nlhs > curIdx )
-	{
-		/* fval */
-		plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
-
-		if ( nlhs > curIdx )
-		{
-			/* exitflag */
-			plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
-
-			if ( nlhs > curIdx )
-			{
-				/* iter */
-				plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
-
-				if ( nlhs > curIdx )
-				{
-					/* lambda */
-					plhs[curIdx++] = mxCreateDoubleMatrix( nV+nC, nP, mxREAL );
-
-					if ( nlhs > curIdx )
-					{
-						/* setup auxiliary output struct */
-						mxArray* auxOutput = mxCreateStructMatrix( 1,1,0,0 );
-						int curFieldNum;
-						
-						/* working set */
-						curFieldNum = mxAddField( auxOutput,"workingSet" );
-						if ( curFieldNum >= 0 )
-							mxSetFieldByNumber( auxOutput,0,curFieldNum,mxCreateDoubleMatrix( nV+nC, nP, mxREAL ) );
-
-						curFieldNum = mxAddField( auxOutput,"cpuTime" );
-						if ( curFieldNum >= 0 )
-							mxSetFieldByNumber( auxOutput,0,curFieldNum,mxCreateDoubleMatrix( 1, nP, mxREAL ) );
-
-						plhs[curIdx] = auxOutput;
-					}
-				}
-			}
-		}
-	}
-	
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	o b t a i n O u t p u t s
- */
-returnValue obtainOutputs(	int k, QProblemB* qp, returnValue returnvalue, int _nWSRout, real_t _cpuTime,
-							int nlhs, mxArray* plhs[], int nV, int nC = 0, int handle = -1
-							)
-{
-	/* Create output vectors and assign pointers to them. */
-	int curIdx = 0;
-
-	/* handle */
-	if ( handle >= 0 )
-		plhs[curIdx++] = mxCreateDoubleScalar( handle );
-
-	/* x */
-	double* x = mxGetPr( plhs[curIdx++] );
-	qp->getPrimalSolution( &(x[k*nV]) );
-
-	if ( nlhs > curIdx )
-	{
-		/* fval */
-		double* obj = mxGetPr( plhs[curIdx++] );
-		obj[k] = qp->getObjVal( );
-
-		if ( nlhs > curIdx )
-		{
-			/* exitflag */
-			double* status = mxGetPr( plhs[curIdx++] );
-			status[k] = (real_t)getSimpleStatus( returnvalue );
-
-			if ( nlhs > curIdx )
-			{
-				/* iter */
-				double* nWSRout = mxGetPr( plhs[curIdx++] );
-				nWSRout[k] = (real_t) _nWSRout;
-
-				if ( nlhs > curIdx )
-				{
-					/* lambda */
-					double* y = mxGetPr( plhs[curIdx++] );
-					qp->getDualSolution( &(y[k*(nV+nC)]) );
-
-					if ( nlhs > curIdx )
-					{
-						
-						mxArray* auxOutput = plhs[curIdx];
-						mxArray* curField = 0;
-
-						/* working set */
-						curField = mxGetField( auxOutput,0,"workingSet" );
-						double* workingSet = mxGetPr(curField);
-
-						QProblem* problemPointer;
-						problemPointer = dynamic_cast<QProblem*>(qp);
-
-						// cast successful?
-						if (problemPointer != NULL) {
-							problemPointer->getWorkingSet( &(workingSet[k*(nV+nC)]) );
-						} else {
-							qp->getWorkingSet( &(workingSet[k*(nV+nC)]) );
-						}
-
-						/* cpu time */
-						curField = mxGetField( auxOutput,0,"cpuTime" );
-						double* cpuTime = mxGetPr(curField);
-						cpuTime[0] = (real_t) _cpuTime;
-					}
-				}
-			}
-		}
-	}
-	
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	s e t u p H e s s i a n M a t r i x
- */
-returnValue setupHessianMatrix(	const mxArray* prhsH, int nV,
-								SymmetricMatrix** H, sparse_int_t** Hir, sparse_int_t** Hjc, real_t** Hv
-								)
-{
-	if ( mxIsSparse( prhsH ) != 0 )
-	{
-		mwIndex *mat_ir = mxGetIr( prhsH );
-		mwIndex *mat_jc = mxGetJc( prhsH );
-		double *v = (double*)mxGetPr( prhsH );
-		sparse_int_t nfill = 0;
-		mwIndex i, j;
-		BooleanType needInsertDiag;
-
-		/* copy indices to avoid 64/32-bit integer confusion */
-		/* also add explicit zeros on diagonal for regularization strategy */
-		/* copy values, too */
-		*Hir = new sparse_int_t[mat_jc[nV] + nV];
-		*Hjc = new sparse_int_t[nV+1];
-		*Hv = new real_t[mat_jc[nV] + nV];
-        for (j = 0; j < nV; j++) 
-		{
-            needInsertDiag = BT_TRUE;
-                
-            (*Hjc)[j] = (sparse_int_t)(mat_jc[j]) + nfill;
-            /* fill up to diagonal */
-            for (i = mat_jc[j]; i < mat_jc[j+1]; i++) 
-			{
-                if ( mat_ir[i] == j )
-                    needInsertDiag = BT_FALSE;
-                    
-                /* add zero diagonal element if not present */
-                if ( ( mat_ir[i] > j ) && ( needInsertDiag == BT_TRUE ) )
-                {
-                    (*Hir)[i + nfill] = (sparse_int_t)j;
-                    (*Hv)[i + nfill] = 0.0;
-                    nfill++;
-                    /* only add diag once */
-                    needInsertDiag = BT_FALSE;
-                }
-                        
-				(*Hir)[i + nfill] = (sparse_int_t)(mat_ir[i]);
-				(*Hv)[i + nfill] = (real_t)(v[i]);
-			}
-		}
-		(*Hjc)[nV] = (sparse_int_t)(mat_jc[nV]) + nfill;
-
-		SymSparseMat *sH;
-		*H = sH = new SymSparseMat(nV, nV, *Hir, *Hjc, *Hv);
-		sH->createDiagInfo();
-	}
-	else
-	{
-		/* make a deep-copy in order to avoid modifying input data when regularising */
-		real_t* H_for = (real_t*) mxGetPr( prhsH );
-		real_t* H_mem = new real_t[nV*nV];
-		memcpy( H_mem,H_for, nV*nV*sizeof(real_t) );
-
-		*H = new SymDenseMat( nV,nV,nV, H_mem );
-		(*H)->doFreeMemory( );
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p C o n s t r a i n t M a t r i x
- */
-returnValue setupConstraintMatrix(	const mxArray* prhsA, int nV, int nC,
-									Matrix** A, sparse_int_t** Air, sparse_int_t** Ajc, real_t** Av
-									)
-{
-	if ( mxIsSparse( prhsA ) != 0 )
-	{
-		mwIndex i;
-		long j;
-
-		mwIndex *mat_ir = mxGetIr( prhsA );
-		mwIndex *mat_jc = mxGetJc( prhsA );
-		double *v = (double*)mxGetPr( prhsA );
-
-		/* copy indices to avoid 64/32-bit integer confusion */
-		*Air = new sparse_int_t[mat_jc[nV]];
-		*Ajc = new sparse_int_t[nV+1];
-		for (i = 0; i < mat_jc[nV]; i++)
-			(*Air)[i] = (sparse_int_t)(mat_ir[i]);
-		for (i = 0; i < nV + 1; i++)
-			(*Ajc)[i] = (sparse_int_t)(mat_jc[i]);
-		
-		/* copy values, too */
-		*Av = new real_t[(*Ajc)[nV]];
-		for (j = 0; j < (*Ajc)[nV]; j++)
-			(*Av)[j] = (real_t)(v[j]);
-
-		*A = new SparseMatrix(nC, nV, *Air, *Ajc, *Av);
-	}
-	else
-	{
-		/* Convert constraint matrix A from FORTRAN to C style
-		* (not necessary for H as it should be symmetric!). */
-		real_t* A_for = (real_t*) mxGetPr( prhsA );
-		real_t* A_mem = new real_t[nC*nV];
-		convertFortranToC( A_for,nV,nC, A_mem );
-		*A = new DenseMatrix(nC, nV, nV, A_mem );
-		(*A)->doFreeMemory();
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_matlab_utils.hpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_matlab_utils.hpp
deleted file mode 100644
index ad588e8ddf9..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_matlab_utils.hpp
+++ /dev/null
@@ -1,92 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file interfaces/matlab/qpOASES_matlab_utils.hpp
- *	\author Hans Joachim Ferreau, Alexander Buchner
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Collects utility functions for Interface to Matlab(R) that
- *	enables to call qpOASES as a MEX function.
- *
- */
-
-
-
-/* Work-around for settings where mexErrMsgTxt causes unexpected behaviour. */
-#ifdef __AVOID_MEXERRMSGTXT__
-	#define myMexErrMsgTxt( TEXT ) mexPrintf( "%s\n\n",(TEXT) );
-#else
-	#define myMexErrMsgTxt mexErrMsgTxt
-#endif
-
-
-#include "mex.h"
-#include "matrix.h"
-#include "string.h"
-#include <vector>
-
-
-/*
- * QProblem instance class
- */
-class QPInstance
-{
-	private:
-		static int s_nexthandle;
-
-	public:
-		QPInstance(	int _nV = 0,
-					int _nC = 0,
-					BooleanType _isSimplyBounded = BT_FALSE
-					);
-
-		~QPInstance( );
-	
-		returnValue deleteQPMatrices();
-		
-		int getNV() const;
-		int getNC() const;
-
-		int handle;
-
-		SQProblem* sqp;
-		QProblemB* qpb;
-		BooleanType isSimplyBounded;
-
-		SymmetricMatrix* H;
-		Matrix* A;
-		sparse_int_t* Hir; 
-		sparse_int_t* Hjc; 
-		sparse_int_t* Air; 
-		sparse_int_t* Ajc;
-		real_t* Hv;
-		real_t* Av;
-};
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_options.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_options.m
deleted file mode 100644
index 01b0e17e7c4..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_options.m
+++ /dev/null
@@ -1,251 +0,0 @@
-%qpOASES -- An Implementation of the Online Active Set Strategy.
-%Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%Christian Kirches et al. All rights reserved.
-%
-%qpOASES is distributed under the terms of the
-%GNU Lesser General Public License 2.1 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.
-%
-%---------------------------------------------------------------------------------
-%
-%Returns a struct containing values for all options to be used within qpOASES.
-%
-%Call
-%    options = qpOASES_options( 'default' );
-%    options = qpOASES_options( 'reliable' );
-%    options = qpOASES_options( 'MPC' );
-%to obtain a set of default options or a pre-defined set of options tuned
-%for reliable or fast QP solution, respectively.
-%
-%Call
-%    options = qpOASES_options( 'option1',value1,'option2',value2,... )
-%to obtain a set of default options but with 'option1' set to value1 etc.
-%
-%Call
-%    options = qpOASES_options( oldOptions,'option1',value1,... )
-%to obtain a copy of the options struct oldOptions but with 'option1' set
-%to value1 etc.
-%
-%Call
-%    options = qpOASES_options( 'default', 'option1',value1,... )
-%    options = qpOASES_options( 'reliable','option1',value1,... )
-%    options = qpOASES_options( 'MPC',     'option1',value1,... )
-%to obtain a set of default options or a pre-defined set of options tuned
-%for reliable or fast QP solution, respectively, but with 'option1' set to 
-%value1 etc.
-%
-%
-%qpOASES features the following options:
-%  maxIter                    -  Maximum number of iterations (if set
-%                                to -1, a value is chosen heuristically)
-%  maxCpuTime                 -  Maximum CPU time in seconds (if set
-%                                to -1, only iteration limit is used)
-%  printLevel                 -  0: no printed output,
-%                                1: only error messages are printed,
-%                                2: iterations and error messages are printed,
-%                                3: all available messages are printed.
-%
-%  enableRamping              -  Enables (1) or disables (0) ramping.
-%  enableFarBounds            -  Enables (1) or disables (0) the use of 
-%                                far bounds.
-%  enableFlippingBounds       -  Enables (1) or disables (0) the use of 
-%                                flipping bounds.
-%  enableRegularisation       -  Enables (1) or disables (0) automatic 
-%                                Hessian regularisation.
-%  enableFullLITests          -  Enables (1) or disables (0) condition-hardened 
-%                                (but more expensive) LI test.
-%  enableNZCTests             -  Enables (1) or disables (0) nonzero curvature 
-%                                tests.
-%  enableDriftCorrection      -  Specifies the frequency of drift corrections:
-%                                0: turns them off, 
-%                                1: uses them at each iteration etc.
-%  enableCholeskyRefactorisation - Specifies the frequency of a full re-
-%                                factorisation of projected Hessian matrix:
-%                                0: turns them off, 
-%                                1: uses them at each iteration etc.
-%  enableEqualities           -  Specifies whether equalities should be treated 
-%                                as always active (1) or not (0)
-%
-%  terminationTolerance       -  Relative termination tolerance to stop homotopy.
-%  boundTolerance             -  If upper and lower bounds differ less than this
-%                                tolerance, they are regarded equal, i.e. as 
-%                                equality constraint.
-%  boundRelaxation            -  Initial relaxation of bounds to start homotopy 
-%                                and initial value for far bounds.
-%  epsNum                     -  Numerator tolerance for ratio tests.
-%  epsDen                     -  Denominator tolerance for ratio tests.
-%  maxPrimalJump              -  Maximum allowed jump in primal variables in 
-%                                nonzero curvature tests.
-%  maxDualJump                -  Maximum allowed jump in dual variables in 
-%                                linear independence tests.
-%
-%  initialRamping             -  Start value for ramping strategy.
-%  finalRamping               -  Final value for ramping strategy.
-%  initialFarBounds           -  Initial size for far bounds.
-%  growFarBounds              -  Factor to grow far bounds.
-%  initialStatusBounds        -  Initial status of bounds at first iteration:
-%                                 0: all bounds inactive,
-%                                -1: all bounds active at their lower bound,
-%                                +1: all bounds active at their upper bound.
-%  epsFlipping                -  Tolerance of squared Cholesky diagonal factor 
-%                                which triggers flipping bound.
-%  numRegularisationSteps     -  Maximum number of successive regularisation steps.
-%  epsRegularisation          -  Scaling factor of identity matrix used for 
-%                                Hessian regularisation.
-%  numRefinementSteps         -  Maximum number of iterative refinement steps.
-%  epsIterRef                 -  Early termination tolerance for iterative 
-%                                refinement.
-%  epsLITests                 -  Tolerance for linear independence tests.
-%  epsNZCTests                -  Tolerance for nonzero curvature tests.
-%
-%
-%See also QPOASES, QPOASES_SEQUENCE
-%
-%
-%For additional information see the qpOASES User's Manual or
-%visit http://www.qpOASES.org/.
-%
-%Please send remarks and questions to support@qpOASES.org!
-function [ options ] = qpOASES_options( varargin )
-
-	firstIsStructOrScheme = 0;
-
-	if ( nargin == 0 ) 
-		options = qpOASES_default_options();
-	else
-		if ( isstruct( varargin{1} ) )
-			if ( mod( nargin,2 ) ~= 1 )
-				error('ERROR (qpOASES_options): Options must be specified in pairs!');
-			end
-			options = varargin{1};
-			firstIsStructOrScheme = 1;
-		else
-			if ( ischar( varargin{1} ) )
-				if ( mod( nargin,2 ) == 0 )
-					options = qpOASES_default_options();
-				else
-					if ( ( nargin > 1 ) && ( ischar( varargin{nargin} ) ) )
-						error('ERROR (qpOASES_options): Options must be specified in pairs!');
-					end
-
-					switch ( varargin{1} )
-						case 'default'
-							options = qpOASES_default_options();
-						case 'reliable'
-							options = qpOASES_reliable_options();
-						case {'MPC','mpc','fast'}
-							options = qpOASES_MPC_options();
-						otherwise
-							error( ['ERROR (qpOASES_options): Only the following option schemes are defined: ''default'', ''reliable'', ''MPC''!'] );
-							
-					end
-					firstIsStructOrScheme = 1;
-				end
-			else
-				error('ERROR (qpOASES_options): First argument needs to be a string or an options struct!');
-			end
-		end
-	end
-
-	% set options to user-defined values  
-	for i=(1+firstIsStructOrScheme):2:nargin
-
-		argName  = varargin{i};
-		argValue = varargin{i+1};
-
-		if ( ( isempty( argName ) ) || ( ~ischar( argName ) ) )
-			error('ERROR (qpOASES_options): Argmument no. %d has to be a non-empty string!',i );
-        end
-			
-		if ( ( ischar(argValue) ) || ( ~isscalar( argValue ) ) )
-			error('ERROR (qpOASES_options): Argmument no. %d has to be a scalar constant!',i+1 );
-        end
-
-		if ( ~isfield( options,argName ) )
-			error('ERROR (qpOASES_options): Argmument no. %d is an invalid option!',i );
-		end
-
-		eval( ['options.',argName,' = ',num2str(argValue),';'] );
-
-	end
-
-end
-
-
-function [ options ] = qpOASES_default_options( )
-
-	% setup options struct with default values
-	options = struct(	'maxIter',                       -1, ...
-						'maxCpuTime',                    -1, ...
-						'printLevel',                     1, ...
-						...
-						'enableRamping',                  1, ...
-						'enableFarBounds',                1, ...
-						'enableFlippingBounds',           1, ...
-						'enableRegularisation',           0, ...
-						'enableFullLITests',              0, ...
-						'enableNZCTests',                 1, ...
-						'enableDriftCorrection',          1, ...
-						'enableCholeskyRefactorisation',  0, ...
-						'enableEqualities',               0, ...
-						...
-						'terminationTolerance',           5.0e6*eps, ...
-						'boundTolerance',                 1.0e6*eps, ...
-						'boundRelaxation',                1.0e4, ...
-						'epsNum',                        -1.0e3*eps, ...
-						'epsDen',                         1.0e3*eps, ...
-						'maxPrimalJump',                  1.0e8, ...
-						'maxDualJump',                    1.0e8, ...
-						...
-    					'initialRamping',                 0.5, ...
-						'finalRamping',                   1.0, ...
-						'initialFarBounds',               1.0e6, ...
-						'growFarBounds',                  1.0e3, ...
-						'initialStatusBounds',            -1, ...
-						'epsFlipping',                    1.0e3*eps, ...
-						'numRegularisationSteps',         0, ...
-						'epsRegularisation',              1.0e3*eps, ...
-						'numRefinementSteps',             1, ...
-						'epsIterRef',                     1.0e2*eps, ...
-						'epsLITests',                     1.0e5*eps, ...
-						'epsNZCTests',                    3.0e3*eps );
-
-end
-
-
-
-function [ options ] = qpOASES_reliable_options( )
-
-	% setup options struct with values for most reliable QP solution
-	options = qpOASES_default_options( );
-
-	options.enableFullLITests             =  1;
-	options.enableCholeskyRefactorisation =  1;
-
-	options.numRefinementSteps            =  2;
-
-end
-
-
-function [ options ] = qpOASES_MPC_options( )
-
-	% setup options struct with values for most reliable QP solution
-	options = qpOASES_default_options( );
-
-	options.enableRamping                 =  0;
-	options.enableFarBounds               =  1;
-	options.enableFlippingBounds          =  0;
-	options.enableRegularisation          =  1;
-	options.enableNZCTests                =  0;
-	options.enableDriftCorrection         =  0;
-	options.enableEqualities              =  1;
-
-	options.terminationTolerance          =  1.0e9*eps;
-	
-	options.initialStatusBounds           =  0;
-	options.numRegularisationSteps        =  1;
-	options.numRefinementSteps            =  0;
-
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_sequence.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_sequence.cpp
deleted file mode 100644
index 943e529e8eb..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_sequence.cpp
+++ /dev/null
@@ -1,1061 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file interfaces/matlab/qpOASES_sequence.cpp
- *	\author Hans Joachim Ferreau, Christian Kirches, Andreas Potschka, Alexander Buchner
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Interface for Matlab(R) that enables to call qpOASES as a MEX function
- *  (variant for solving QP sequences).
- *
- */
-
-
-
-#include <qpOASES.hpp>
-
-
-USING_NAMESPACE_QPOASES
-
-
-#include "qpOASES_matlab_utils.hpp"
-
-/** initialise handle counter of QPInstance class */
-int QPInstance::s_nexthandle = 1;
-
-/** global pointer to QP objects */
-static std::vector<QPInstance *> g_instances;
-
-#include "qpOASES_matlab_utils.cpp"
-
-
-/*
- *	i n i t S B
- */
-int initSB(	int handle, 
-			SymmetricMatrix *H, real_t* g,
-			const real_t* const lb, const real_t* const ub,
-			int nWSRin, real_t maxCpuTimeIn,
-			const real_t* const x0, Options* options,
-			int nOutputs, mxArray* plhs[],
-			double* guessedBounds
-			)
-{
-	int nWSRout = nWSRin;
-	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-
-	/* 1) setup initial QP. */
-	QProblemB* globalQPB = getQPInstance(handle)->qpb;
-
-	if ( globalQPB == 0 )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
-		return -1;
-	}
-
-	globalQPB->setOptions( *options );
-	
-	/* 2) Solve initial QP. */
-	returnValue returnvalue;
-	int nV = globalQPB->getNV();
-	
-	/* 3) Fill the working set. */
-	Bounds bounds(nV);
-	if (guessedBounds != 0) {
-		for (int i = 0; i < nV; i++) {
-			if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_LOWER);
-			} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_UPPER);
-			} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_INACTIVE);
-			} else {
-				char msg[MAX_STRING_LENGTH];
-				snprintf(msg, MAX_STRING_LENGTH,
-						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
-				myMexErrMsgTxt(msg);
-				return -1;
-			}
-		}
-	}
-
-	if (x0 == 0 && guessedBounds == 0)
-		returnvalue = globalQPB->init( H,g,lb,ub, nWSRout,&maxCpuTimeOut );
-	else
-		returnvalue = globalQPB->init( H,g,lb,ub, nWSRout,&maxCpuTimeOut, x0, 0,
-				guessedBounds != 0 ? &bounds : 0);
-
-	/* 3) Assign lhs arguments. */
-	obtainOutputs(	0,globalQPB,returnvalue,nWSRout,maxCpuTimeOut,
-					nOutputs,plhs,nV,0,handle );
-
-	return 0;
-}
-
-
-/*
- *	i n i t
- */
-int init(	int handle, 
-			SymmetricMatrix *H, real_t* g, Matrix *A,
-			const real_t* const lb, const real_t* const ub, const real_t* const lbA, const real_t* const ubA,
-			int nWSRin, real_t maxCpuTimeIn,
-			const real_t* const x0, Options* options,
-			int nOutputs, mxArray* plhs[],
-			double* guessedBounds, double* guessedConstraints
-			)
-{
-	int nWSRout = nWSRin;
-	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-
-	/* 1) setup initial QP. */
-	SQProblem* globalSQP = getQPInstance(handle)->sqp;
-
-	if ( globalSQP == 0 )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
-		return -1;
-	}
-
-	globalSQP->setOptions( *options );
-	
-	/* 2) Solve initial QP. */
-	returnValue returnvalue;
-	int nV = globalSQP->getNV();
-	int nC = globalSQP->getNC();
-	
-	/* 3) Fill the working set. */
-	Bounds bounds(nV);
-	Constraints constraints(nC);
-	if (guessedBounds != 0) {
-		for (int i = 0; i < nV; i++) {
-			if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_LOWER);
-			} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_UPPER);
-			} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_INACTIVE);
-			} else {
-				char msg[MAX_STRING_LENGTH];
-				snprintf(msg, MAX_STRING_LENGTH,
-						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
-				myMexErrMsgTxt(msg);
-				return -1;
-			}
-		}
-	}
-
-	if (guessedConstraints != 0) {
-		for (int i = 0; i < nC; i++) {
-			if ( isEqual(guessedConstraints[i],-1.0) == BT_TRUE ) {
-				constraints.setupConstraint(i, ST_LOWER);
-			} else if ( isEqual(guessedConstraints[i],1.0) == BT_TRUE ) {
-				constraints.setupConstraint(i, ST_UPPER);
-			} else if ( isEqual(guessedConstraints[i],0.0) == BT_TRUE ) {
-				constraints.setupConstraint(i, ST_INACTIVE);
-			} else {
-				char msg[MAX_STRING_LENGTH];
-				snprintf(msg, MAX_STRING_LENGTH,
-						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of constraints!");
-				myMexErrMsgTxt(msg);
-				return -1;
-			}
-		}
-	}
-
-	if (x0 == 0 && guessedBounds == 0 && guessedConstraints == 0)
-		returnvalue = globalSQP->init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut);
-	else
-		returnvalue = globalSQP->init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut, x0, 0,
-				guessedBounds != 0 ? &bounds : 0,
-				guessedConstraints != 0 ? &constraints : 0);
-
-	/* 3) Assign lhs arguments. */
-	obtainOutputs(	0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
-					nOutputs,plhs,nV,nC,handle );
-
-	return 0;
-}
-
-
-
-/*
- *	h o t s t a r t S B
- */
-int hotstartSB(	int handle,
-                const real_t* const g,
-				const real_t* const lb, const real_t* const ub,
-				int nWSRin, real_t maxCpuTimeIn,
-				Options* options,
-				int nOutputs, mxArray* plhs[]
-				)
-{
-	int nWSRout = nWSRin;
-	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-
-	QProblemB* globalQPB = getQPInstance(handle)->qpb;
-
-	if ( globalQPB == 0 )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
-		return -1;
-	}
-
-	/* 1) Solve QP with given options. */
-	globalQPB->setOptions( *options );
-	returnValue returnvalue = globalQPB->hotstart( g,lb,ub, nWSRout,&maxCpuTimeOut );
-
-	/* 2) Assign lhs arguments. */
-	obtainOutputs(	0,globalQPB,returnvalue,nWSRout,maxCpuTimeOut,
-					nOutputs,plhs,0,0 );
-
-	return 0;
-}
-
-
-/*
- *	h o t s t a r t
- */
-int hotstart(	int handle,
-                const real_t* const g,
-				const real_t* const lb, const real_t* const ub,
-				const real_t* const lbA, const real_t* const ubA,
-				int nWSRin, real_t maxCpuTimeIn,
-				Options* options,
-				int nOutputs, mxArray* plhs[]
-				)
-{
-	int nWSRout = nWSRin;
-	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-
-	QProblem* globalSQP = getQPInstance(handle)->sqp;
-
-	if ( globalSQP == 0 )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
-		return -1;
-	}
-
-	/* 1) Solve QP with given options. */
-	globalSQP->setOptions( *options );
-	returnValue returnvalue = globalSQP->hotstart( g,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut );
-
-	/* 2) Assign lhs arguments. */
-	obtainOutputs(	0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
-					nOutputs,plhs,0,0 );
-
-	return 0;
-}
-
-
-/*
- *	h o t s t a r t V M
- */
-int hotstartVM(	int handle,
-                    SymmetricMatrix *H, real_t* g, Matrix *A,
-					const real_t* const lb, const real_t* const ub, const real_t* const lbA, const real_t* const ubA,
-					int nWSRin, real_t maxCpuTimeIn,
-					Options* options,
-					int nOutputs, mxArray* plhs[]
-					)
-{
-	int nWSRout = nWSRin;
-	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-
-	SQProblem* globalSQP = getQPInstance(handle)->sqp;
-
-	if ( globalSQP == 0 )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
-		return -1;
-	}
-
-	/* 1) Solve QP. */
-	globalSQP->setOptions( *options );
-	returnValue returnvalue = globalSQP->hotstart( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut );
-
-	switch (returnvalue)
-	{
-		case SUCCESSFUL_RETURN:
-		case RET_QP_UNBOUNDED:
-		case RET_QP_INFEASIBLE:
-			break;
-		otherwise:
-			myMexErrMsgTxt( "ERROR (qpOASES): Hotstart failed." );
-			return -1;
-	}
-
-	/* 2) Assign lhs arguments. */
-	obtainOutputs(	0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
-					nOutputs,plhs,0,0 );
-
-	return 0;
-}
-
-
-
-/*
- *	m e x F u n c t i o n
- */
-void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
-{
-	/* inputs */
-	char typeString[2];
-	real_t *g=0, *lb=0, *ub=0, *lbA=0, *ubA=0, *x0=0;
-	int H_idx=-1, g_idx=-1, A_idx=-1, lb_idx=-1, ub_idx=-1, lbA_idx=-1, ubA_idx=-1;
-	int x0_idx=-1, auxInput_idx=-1;
-
-	double *guessedBoundsAndConstraints = 0;
-	double *guessedBounds = 0, *guessedConstraints = 0;
-
-	BooleanType isSimplyBoundedQp = BT_FALSE;
-
-	Options options;
-	options.printLevel = PL_LOW;
-	#ifdef __DEBUG__
-	options.printLevel = PL_HIGH;
-	#endif
-	#ifdef __SUPPRESSANYOUTPUT__
-	options.printLevel = PL_NONE;
-	#endif
-
-	/* dimensions */
-	unsigned int nV=0, nC=0, handle=0;
-	int nWSRin;
-	real_t maxCpuTimeIn = -1.0;
-	QPInstance* globalQP = 0;
-
-	/* I) CONSISTENCY CHECKS: */
-	/* 1) Ensure that qpOASES is called with a feasible number of input arguments. */
-	if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
-	{
-		if ( nrhs != 2 )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-	}
-	
-	/* 2) Ensure that first input is a string ... */
-	if ( mxIsChar( prhs[0] ) != 1 )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): First input argument must be a string!" );
-		return;
-	}
-
-	mxGetString( prhs[0], typeString, 2 );
-
-	/*    ... and if so, check if it is an allowed one. */
-	if ( ( strcmp( typeString,"i" ) != 0 ) && ( strcmp( typeString,"I" ) != 0 ) &&
-		 ( strcmp( typeString,"h" ) != 0 ) && ( strcmp( typeString,"H" ) != 0 ) &&
-		 ( strcmp( typeString,"m" ) != 0 ) && ( strcmp( typeString,"M" ) != 0 ) &&
-		 ( strcmp( typeString,"e" ) != 0 ) && ( strcmp( typeString,"E" ) != 0 ) &&
-		 ( strcmp( typeString,"c" ) != 0 ) && ( strcmp( typeString,"C" ) != 0 ) )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): Undefined first input argument!\nType 'help qpOASES_sequence' for further information." );
-		return;
-	}
-
-
-	/* II) SELECT RESPECTIVE QPOASES FUNCTION CALL: */
-	/* 1) Init (without or with initial guess for primal solution). */
-	if ( ( strcmp( typeString,"i" ) == 0 ) || ( strcmp( typeString,"I" ) == 0 ) )
-	{
-		/* consistency checks */
-		if ( ( nlhs < 2 ) || ( nlhs > 7 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		/* ensure that data is given in double precision */
-		if ((mxIsDouble(prhs[1]) == 0) || (mxIsDouble(prhs[2]) == 0)) {
-			myMexErrMsgTxt(
-					"ERROR (qpOASES): All data has to be provided in double precision!");
-			return;
-		}
-
-        nV = (unsigned int)mxGetM( prhs[1] ); /* row number of Hessian matrix */
-
-		/* Check for 'Inf' and 'Nan' in Hessian */
-		H_idx = 1;
-		g_idx = 2;
-		if (containsNaNorInf(prhs, nV * nV, H_idx, 0) == BT_TRUE) {
-			return;
-		}
-
-		/* Check for 'Inf' and 'Nan' in gradient */
-		if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
-			return;
-		}
-
-		/* determine whether is it a simply bounded QP */
-		if ( nrhs <= 7 )
-			isSimplyBoundedQp = BT_TRUE;
-		else
-			isSimplyBoundedQp = BT_FALSE;
-
-		if ( isSimplyBoundedQp == BT_TRUE )
-		{
-			lb_idx = 3;
-			ub_idx = 4;
-
-			if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
-				return;
-			}
-			if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
-				return;
-			}
-
-			/* Check inputs dimensions and assign pointers to inputs. */
-			nC = 0; /* row number of constraint matrix */
-
-			if ( mxGetN( prhs[1] ) != nV )
-			{
-				myMexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );
-				return;
-			}
-
-			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
-				return;
-
-			/* default value for nWSR */
-			nWSRin = 5*nV;
-
-			/* Check whether x0 and options are specified .*/
-			if ( nrhs >= 6 )
-			{
-				if ((!mxIsEmpty(prhs[5])) && (mxIsStruct(prhs[5])))
-					setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );
-
-				if ( ( nrhs >= 7 ) && ( !mxIsEmpty(prhs[6]) ) )
-				{ 
-					/* auxInput specified */
-					if ( mxIsStruct(prhs[6]) )
-					{
-						auxInput_idx = 6;
-						x0_idx = -1;
-					}
-					else
-					{
-						auxInput_idx = -1;
-						x0_idx = 6;
-					}
-				}
-				else
-				{
-					auxInput_idx = -1;
-					x0_idx = -1;
-				}
-			}
-		}
-		else
-		{
-			/* ensure that data is given in double precision */
-			if ( ( mxIsDouble( prhs[1] ) == 0 ) ||
-				 ( mxIsDouble( prhs[2] ) == 0 ) ||
-				 ( mxIsDouble( prhs[3] ) == 0 ) )
-			{
-				myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
-				return;
-			}
-		
-			/* Check inputs dimensions and assign pointers to inputs. */
-			nC = (unsigned int)mxGetM( prhs[3] ); /* row number of constraint matrix */
-
-			A_idx = 3;
-			lb_idx = 4;
-			ub_idx = 5;
-			lbA_idx = 6;
-			ubA_idx = 7;
-
-			if (containsNaNorInf(prhs, nV * nC, A_idx, 0) == BT_TRUE) {
-				return;
-			}
-			if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
-				return;
-			}
-			if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
-				return;
-			}
-			if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
-				return;
-			}
-			if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
-				return;
-			}
-
-			if ( ( mxGetN( prhs[1] ) != nV ) || ( ( mxGetN( prhs[3] ) != 0 ) && ( mxGetN( prhs[3] ) != nV ) ) )
-			{
-				myMexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );
-				return;
-			}
-		
-			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
-				return;
-			
-			if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,7 ) != SUCCESSFUL_RETURN )
-				return;
-
-			/* default value for nWSR */
-			nWSRin = 5*(nV+nC);
-
-			/* Check whether x0 and options are specified .*/
-			if ( nrhs >= 9 )
-			{
-				if ((!mxIsEmpty(prhs[8])) && (mxIsStruct(prhs[8])))
-					setupOptions( &options,prhs[8],nWSRin,maxCpuTimeIn );
-
-				if ( ( nrhs >= 10 ) && ( !mxIsEmpty(prhs[9]) ) )
-				{ 
-					/* auxInput specified */
-					if ( mxIsStruct(prhs[9]) )
-					{
-						auxInput_idx = 9;
-						x0_idx = -1;
-					}
-					else
-					{
-						auxInput_idx = -1;
-						x0_idx = 9;
-					}
-				}
-				else
-				{
-					auxInput_idx = -1;
-					x0_idx = -1;
-				}
-			}
-		}
-
-
-		/* check dimensions and copy auxInputs */
-		if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,x0_idx ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( auxInput_idx >= 0 )
-			setupAuxiliaryInputs( prhs[auxInput_idx],nV,nC, &x0,&guessedBoundsAndConstraints,&guessedBounds,&guessedConstraints );
-
-
-		/* allocate instance */
-		handle = allocateQPInstance( nV,nC,isSimplyBoundedQp, &options );	
-		globalQP = getQPInstance( handle );
-
-		/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
-		setupHessianMatrix(	prhs[1],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );
-
-		/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
-		if ( nC > 0 )
-			setupConstraintMatrix( prhs[3],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );
-
-		/* Create output vectors and assign pointers to them. */
-		allocateOutputs( nlhs,plhs, nV,nC,1,handle );
-
-		/* Call qpOASES. */
-		if ( isSimplyBoundedQp == BT_TRUE )
-		{
-			initSB(	handle,
-					globalQP->H,g,
-					lb,ub,
-					nWSRin,maxCpuTimeIn,
-					x0,&options,
-					nlhs,plhs,
-					guessedBounds
-					);
-
-			deleteAuxiliaryInputs( &guessedBounds,0 );
-		}
-		else
-		{
-			init(	handle,
-					globalQP->H,g,globalQP->A,
-					lb,ub,lbA,ubA,
-					nWSRin,maxCpuTimeIn,
-					x0,&options,
-					nlhs,plhs,
-					guessedBounds, guessedConstraints
-					);
-
-			deleteAuxiliaryInputs( &guessedBounds,&guessedConstraints );
-		}
-
-		return;
-	}
-
-	/* 2) Hotstart. */
-	if ( ( strcmp( typeString,"h" ) == 0 ) || ( strcmp( typeString,"H" ) == 0 ) )
-	{
-		/* consistency checks */
-		if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		if ( ( nrhs < 5 ) || ( nrhs > 8 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		/* determine whether is it a simply bounded QP */
-		if ( nrhs < 7 )
-			isSimplyBoundedQp = BT_TRUE;
-		else
-			isSimplyBoundedQp = BT_FALSE;
-
-
-		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxGetM( prhs[1] ) != 1 ) || ( mxGetN( prhs[1] ) != 1 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		/* get QP instance */
-		handle = (unsigned int)mxGetScalar( prhs[1] );
-		globalQP = getQPInstance ( handle );
-		if ( globalQP == 0 )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
-			return;
-		}
-
-		nV = globalQP->getNV();
-
-		g_idx = 2;
-		lb_idx = 3;
-		ub_idx = 4;
-
-		if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
-			return;
-		}
-
-		if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
-			return;
-		}
-		if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
-			return;
-		}
-
-
-		/* Check inputs dimensions and assign pointers to inputs. */
-		if ( isSimplyBoundedQp == BT_TRUE )
-		{
-			nC = 0;
-
-			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
-				return;
-
-			/* default value for nWSR */
-			nWSRin = 5*nV;
-
-			/* Check whether options are specified .*/
-			if ( nrhs == 6 )
-				if ( ( !mxIsEmpty( prhs[5] ) ) && ( mxIsStruct( prhs[5] ) ) )
-					setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );
-		}
-		else
-		{
-			nC = globalQP->getNC( );
-
-			lbA_idx = 5;
-			ubA_idx = 6;
-
-			if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
-				return;
-			}
-			if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
-				return;
-			}
-
-			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
-				return;
-
-			/* default value for nWSR */
-			nWSRin = 5*(nV+nC);
-
-			/* Check whether options are specified .*/
-			if ( nrhs == 8 )
-				if ( ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
-					setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
-		}
-
-		/* Create output vectors and assign pointers to them. */
-		allocateOutputs( nlhs,plhs, nV,nC );
-
-		/* call qpOASES */
-		if ( isSimplyBoundedQp == BT_TRUE )
-		{
-			hotstartSB(	handle, g,
-						lb,ub,
-						nWSRin,maxCpuTimeIn,
-						&options,
-						nlhs,plhs
-						);
-		}
-		else
-		{
-			hotstart(	handle, g,
-						lb,ub,lbA,ubA,
-						nWSRin,maxCpuTimeIn,
-						&options,
-						nlhs,plhs
-						);
-		}
-
-		return;
-	}
-
-	/* 3) Modify matrices. */
-	if ( ( strcmp( typeString,"m" ) == 0 ) || ( strcmp( typeString,"M" ) == 0 ) )
-	{
-		/* consistency checks */
-		if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		if ( ( nrhs < 9 ) || ( nrhs > 10 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxGetM( prhs[1] ) != 1 ) || ( mxGetN( prhs[1] ) != 1 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		/* ensure that data is given in double precision */
-		if ( ( mxIsDouble( prhs[2] ) == 0 ) ||
-			 ( mxIsDouble( prhs[3] ) == 0 ) ||
-			 ( mxIsDouble( prhs[4] ) == 0 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
-			return;
-		}
-
-		/* get QP instance */
-		handle = (unsigned int)mxGetScalar( prhs[1]);
-		globalQP = getQPInstance ( handle );
-		if ( globalQP == 0 )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
-			return;
-		}
-
-		/* Check inputs dimensions and assign pointers to inputs. */
-		nV = (unsigned int)mxGetM( prhs[2] ); /* row number of Hessian matrix */
-		nC = (unsigned int)mxGetM( prhs[4] ); /* row number of constraint matrix */
-		
-		H_idx = 2;
-		g_idx = 3;
-		A_idx = 4;
-		lb_idx = 5;
-		ub_idx = 6;
-		lbA_idx = 7;
-		ubA_idx = 8;
-
-		/* check if supplied data contains 'NaN' or 'Inf' */
-		if (containsNaNorInf(prhs, nV * nV, H_idx, 0) == BT_TRUE) {
-			return;
-		}
-
-		if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
-			return;
-		}
-
-		if (containsNaNorInf(prhs, nV * nC, A_idx, 0) == BT_TRUE) {
-			return;
-		}
-
-		if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
-			return;
-		}
-		if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
-			return;
-		}
-
-		if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
-			return;
-		}
-		if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
-			return;
-		}
-
-		/* Check that dimensions are consistent with existing QP instance */
-		if (nV != (unsigned int) globalQP->getNV () || nC != (unsigned int) globalQP->getNC ())
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): QP dimensions must be constant during a sequence! Try creating a new QP instance instead." );
-			return;
-		}
-
-		if ( ( mxGetN( prhs[2] ) != nV ) || ( ( mxGetN( prhs[4] ) != 0 ) && ( mxGetN( prhs[4] ) != nV ) ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );
-			return;
-		}
-
-		if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,3 ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,7 ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,8 ) != SUCCESSFUL_RETURN )
-			return;
-
-		/* default value for nWSR */
-		nWSRin = 5*(nV+nC);
-
-		/* Check whether options are specified .*/
-		if ( nrhs > 9 )
-			if ( ( !mxIsEmpty( prhs[9] ) ) && ( mxIsStruct( prhs[9] ) ) )
-				setupOptions( &options,prhs[9],nWSRin,maxCpuTimeIn );
-
-		globalQP->deleteQPMatrices( );
-
-		/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
-		setupHessianMatrix(	prhs[2],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );
-
-		/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
-		if ( nC > 0 )
-			setupConstraintMatrix( prhs[4],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );
-
-		/* Create output vectors and assign pointers to them. */
-		allocateOutputs( nlhs,plhs, nV,nC );
-
-		/* Call qpOASES */
-		hotstartVM(	handle, globalQP->H,g,globalQP->A,
-					lb,ub,lbA,ubA,
-					nWSRin,maxCpuTimeIn,
-					&options,
-					nlhs,plhs
-					);
-
-		return;
-	}
-
-	/* 4) Solve current equality constrained QP. */
-	if ( ( strcmp( typeString,"e" ) == 0 ) || ( strcmp( typeString,"E" ) == 0 ) )
-	{
-		/* consistency checks */
-		if ( ( nlhs < 1 ) || ( nlhs > 3 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		if ( ( nrhs < 7 ) || ( nrhs > 8 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxGetM( prhs[1] ) != 1 ) || ( mxGetN( prhs[1] ) != 1 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		/* get QP instance */
-		handle = (unsigned int)mxGetScalar( prhs[1] );
-		globalQP = getQPInstance ( handle );
-		if ( globalQP == 0 )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
-			return;
-		}
-
-		/* Check inputs dimensions and assign pointers to inputs. */
-		int nRHS = (int)mxGetN(prhs[2]);
-		nV = globalQP->getNV( );
-		nC = globalQP->getNC( );
-		real_t *x_out, *y_out;
-
-		g_idx = 2;
-		lb_idx = 3;
-		ub_idx = 4;
-		lbA_idx = 5;
-		ubA_idx = 6;
-
-		/* check if supplied data contains 'NaN' or 'Inf' */
-		if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
-			return;
-		}
-		if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
-			return;
-		}
-		if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
-			return;
-		}
-
-		if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
-			return;
-		}
-		if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
-			return;
-		}
-
-		if ( smartDimensionCheck( &g,nV,nRHS, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &lb,nV,nRHS, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &ub,nV,nRHS, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &lbA,nC,nRHS, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &ubA,nC,nRHS, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
-			return;
-
-		/* Check whether options are specified .*/
-		if ( ( nrhs == 8 ) && ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
-		{
-			nWSRin = 5*(nV+nC);
-			setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
-			globalQP->sqp->setOptions( options );
-		}
-
-		/* Create output vectors and assign pointers to them. */
-		plhs[0] = mxCreateDoubleMatrix( nV, nRHS, mxREAL );
-		x_out = mxGetPr(plhs[0]);
-		if (nlhs >= 2)
-		{
-			plhs[1] = mxCreateDoubleMatrix( nV+nC, nRHS, mxREAL );
-			y_out = mxGetPr(plhs[1]);
-
-			if (nlhs >= 3) {
-				plhs[2] = mxCreateDoubleMatrix( nV+nC, nRHS, mxREAL );
-				double* workingSet = mxGetPr(plhs[2]);
-
-				globalQP->sqp->getWorkingSet(workingSet);
-			}
-		}
-		else
-			y_out = new real_t[nV+nC];
-
-		/* Solve equality constrained QP */
-		returnValue returnvalue = globalQP->sqp->solveCurrentEQP( nRHS,g,lb,ub,lbA,ubA, x_out,y_out );
-
-		if (nlhs < 2)
-			delete[] y_out;
-
-		if (returnvalue != SUCCESSFUL_RETURN)
-		{
-			char msg[MAX_STRING_LENGTH];
-			snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Couldn't solve current EQP (code %d)!", returnvalue);
-			myMexErrMsgTxt(msg);
-			return;
-		}
-
-		return;
-	}
-
-	/* 5) Cleanup. */
-	if ( ( strcmp( typeString,"c" ) == 0 ) || ( strcmp( typeString,"C" ) == 0 ) )
-	{		
-		/* consistency checks */
-		if ( nlhs != 0 )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		if ( nrhs != 2 )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxGetM( prhs[1] ) != 1 ) || ( mxGetN( prhs[1] ) != 1 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		/* Cleanup SQProblem instance. */
-		handle =(unsigned int)mxGetScalar( prhs[1] );
-		deleteQPInstance ( handle );
-		
-		return;
-	}
-
-}
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_sequence.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_sequence.m
deleted file mode 100644
index e5d97b6229e..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/matlab/qpOASES_sequence.m
+++ /dev/null
@@ -1,112 +0,0 @@
-%qpOASES -- An Implementation of the Online Active Set Strategy.
-%Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%Christian Kirches et al. All rights reserved.
-%
-%qpOASES is distributed under the terms of the
-%GNU Lesser General Public License 2.1 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.
-%
-%---------------------------------------------------------------------------------
-%
-%qpOASES_sequence is intended to solve a sequence of quadratic
-%programming (QP) problems of the following form:
-%
-%                min   1/2*x'Hx + x'g
-%                s.t.  lb  <=  x <= ub
-%                      lbA <= Ax <= ubA  {optional}
-%
-%I) Call
-%
-%    [QP,x,fval,exitflag,iter,lambda,auxOutput] = ...
-%               qpOASES_sequence( 'i',H,g,A,lb,ub,lbA,ubA{,options{,auxInput}} )
-%or 
-%    [QP,x,fval,exitflag,iter,lambda,auxOutput] = ...
-%               qpOASES_sequence( 'i',H,g,lb,ub{,options{,auxInput}} )
-%
-%for initialising and solving the first above-mentioned QP of the sequence
-%starting from an initial guess x0. H must be a symmetric (possibly indefinite) 
-%matrix and all vectors g, lb, ub, lbA, ubA have to be given as column vectors. 
-%Options can be generated using the qpOASES_options command, otherwise default
-%values are used. Optionally, further auxiliary inputs may be generated 
-%using qpOASES_auxInput command and passed to the solver.
-%Both matrices H or A may be passed in sparse matrix format.
-%
-%II) Call
-%
-%     [x,fval,exitflag,iter,lambda,auxOutput] = ...
-%                      qpOASES_sequence( 'h',QP,g,lb,ub,lbA,ubA{,options} )
-%or
-%     [x,fval,exitflag,iter,lambda,auxOutput] = ...
-%                      qpOASES_sequence( 'h',QP,g,lb,ub{,options} )
-%
-%for hotstarting from the previous QP solution to the one of the next QP
-%given by the vectors g, lb, ub, lbA, ubA. Options can be generated using the 
-%qpOASES_options command, otherwise default values are used.
-%
-%III) Call
-%
-%     [x,fval,exitflag,iter,lambda,auxOutput] = ...
-%                     qpOASES_sequence( 'm',QP,H,g,A,lb,ub,lbA,ubA{,options} )
-%
-%for hotstarting from the previous QP solution to the one of the next QP
-%given by the matrices H, A and the vectors g, lb, ub, lbA, ubA. The previous
-%active set serves as a starting guess. If the new projected Hessian matrix
-%turns out to be not positive definite, qpOASES recedes to a safe initial active
-%set guess automatically. This can result in a high number of iterations iter.
-%Options can be generated using the qpOASES_options command, otherwise default
-%values are used.
-%
-%IV) Call
-%
-%     [x,lambda,workingSet] = qpOASES_sequence( 'e',QP,g,lb,ub,lbA,ubA{,options} )
-%
-%for solving the equality constrained QP with constraints determined by the
-%current active set. All inequalities and bounds which were not active in the
-%previous solution might be violated. This command does not alter the internal
-%state of qpOASES. Instead of calling this command multiple times, it is
-%possible to supply several columns simultaneously in g, lb, ub, lbA, and ubA.
-%Options can be generated using the qpOASES_options command, otherwise default
-%values are used.
-%
-%V) Having solved the last QP of your sequence, call
-%
-%     qpOASES_sequence( 'c',QP )
-%
-%in order to cleanup the internal memory.
-%
-%
-%Optional outputs (only x is mandatory):
-%    x          -  Optimal primal solution vector (if exitflag==0).
-%    fval       -  Optimal objective function value (if exitflag==0).
-%    exitflag   -   0: QP solved,
-%                   1: QP could not be solved within given number of iterations,
-%                  -1: QP could not be solved due to an internal error,
-%                  -2: QP is infeasible (and thus could not be solved),
-%                  -3: QP is unbounded (and thus could not be solved).
-%    iter       -  Number of active set iterations actually performed.
-%    lambda     -  Optimal dual solution vector (if status==0).
-%    auxOutput  -  Struct containing auxiliary outputs as described below.
-%
-%The auxOutput struct contains the following entries:
-%    workingSet -  The working set at point x. The working set is a subset
-%                  of the active set (which is the set of all indices
-%                  corresponding to bounds/constraints that hold with 
-%                  equality). The working set corresponds to bound/
-%                  constraint row vectors forming a linear independent set.
-%                  The first nV elements correspond to the bounds, the last
-%                  nC elements to the constraints.
-%                  The working set is encoded as follows:
-%                   1: bound/constraint at its upper bound
-%                   0: bound/constraint not at any bound
-%                  -1: bound/constraint at its lower bound
-%    cpuTime    -  Internally measured CPU time for solving QP.
-%
-%See also QPOASES_OPTIONS, QPOASES_AUXINPUT, QPOASES
-%
-%
-%For additional information see the qpOASES User's Manual or
-%visit http://www.qpOASES.org/.
-%
-%Please send remarks and questions to support@qpOASES.org!
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/clean b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/clean
deleted file mode 100644
index 2957397008a..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/clean
+++ /dev/null
@@ -1,3 +0,0 @@
-rm *.*~
-rm *.o
-rm qpOASES qpOASES_sequence
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/clean.sh b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/clean.sh
deleted file mode 100644
index 2957397008a..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/clean.sh
+++ /dev/null
@@ -1,3 +0,0 @@
-rm *.*~
-rm *.o
-rm qpOASES qpOASES_sequence
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1.mat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1.mat
deleted file mode 100644
index c0bd0d2ed4364ebaf2b16eeb1791b99510164d60..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 790
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i?A@$QE)CwO)N=GQOM7;
zQV7W?Rq#(PQBW{ZFtoHXwy-iZQZO<wFjpWIFfe-h@-r|n=m2rWoX5!t2^<VbW(v<5
zE-4(~dTQ)ws4P&$km;uomeA11|BWZ&AagZCObxRuNRJv^j{&kCCPU#FX(<IPn`cT$
zG&V7awlb^%sn&(7E<jezXs9e<Y_x<KXonck4*72aznX0sEPR-wL7MdNXfk}FVrWpn
zbd!n4;KKpAQx*&i4u6?#K)Tf7x=<Vjw#smdTH_BNo;41R3^D<X9(eq5$k5n1bCNQ%
z@Pwl*Pna3xb}%;MF&w1YZ<6wal%ya3=QlDksCqM3;5D4vIj*8W0BATX&~U}|jQ2pQ
z4dE{TfEGw1%||Xh{v33=(L`N<+u*<f_G*s5hvgUwC0GSO8WEwp0Yl@HqDhZ{Iwd7$
zNJcd5`Tw8MMu6oVNRu%h+n#t$hG?>A%wc4c5GZW8!2gYzq2v=QA4sbvTq}xwN{M9(
rdcH+H=e>2)6PUUhjxboKF`MIYAV|YGPpF2pgrpzeL4oIZl~o4-!Xx*U

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1a.mat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1a.mat
deleted file mode 100644
index 48c784fcb7c53eedc3b3124e3577f8d0822e1cb2..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 896
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i?A@$QE)CwO)N=GQOM7;
zQV7W?Rq#(PQBW{ZFtoHXwzM)dRWLFzFjpWIFfe-h@-r|n=m2rWoX5!t2^<VbW(v<5
zE-4(~dTQ)ws4P&$km;uomeA11|BWZ&AagZCObxRuNRJv^j{&kCCPU#FX(<IPn`cT$
zG&V7awlb^%sn&(7E<jezXs9e<Y_x<KXonck4*72aznX0sEPR-wL7MdNXfk}FVrWpn
zbd!n4;KKpAQx*&i4u6?#K)Tf7x=<Vjw#smdTH_BNo;41R3^D<X9(eq5$k5n1bCNQ%
z@Pwl*Pna3xb}%;MF&w1YZ<6wal%ya3=QlDksCqM3;5D4vIj*8W0BATX&~U}|jQ2pQ
z4dE{TfEGw1%||Xh{v33=(L`N<+u*<f_G*s5hvgUwC0GSO8WEwp0Yl@HqDhZ{Iwd7$
zNJcd5`Tw8MMu6oVNRu%h+n#t$hG?>A%wc4c5GZW8!2gYzq2v=QA4sbvTq}xwN{M9(
zdcH+H=e>2)6PUUhjxboKF`MIYAV|YGPpF2pgrpzeL4oIZl~o6%$pCIk2ePk#sWRk@
zvYq}`8INM7jZDluH5=M5@Ec?>oIk~G0n(=h*N5UdV9K1r)@ga{`O<04M`r+2CliB_
HDN`x{YE&BX

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1b.mat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/example1b.mat
deleted file mode 100644
index b45b4f74c6a935c350dac6de0673396f20801338..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 549
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i?A@$QE)CwO)N=GQOM7;
zQV7W?Rq#(PQBW{ZFtoHXwy-iXRWLFzFjpWIFfe-h@-r|n=m2rWoX5!t2^<VbW(v<5
zE-4(~dTQ)ws4P&$km;uomeA11|BWZ&AagZCObxRuNRKXDPXV$XMnh!@W1}U^KwV-$
zUGm=qel^=NSoknUgEZ;k(Pa2U#n7OD=_V78!G{BKrz{v49R4!ffOM(Dbs1n-WjIBx
z@rMu38V5%PnE*x)ka9Kb$`2WGJI7TN2nbI&%KC(vL2*6fJ&<ZcxatpRhKn>Gx%BvR
z(CJ1KbpdXJ0|(fvIsP7&V<?nh6#!{O1i%IijZcauJp$^Kl$aqI(Xi+Le?}VtmUkdc
f#&~Rd;x!qf$)YibkxfFNu;Bv#H)e*CPpo_Z46>$n

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/index.html
deleted file mode 100644
index 6383f5f1115..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/index.html
+++ /dev/null
@@ -1,21 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces/octave</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/interfaces/octave</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="clean">clean</a></li>
-  <li><a href="clean.sh">clean.sh</a></li>
-  <li><a href="example1.mat">example1.mat</a></li>
-  <li><a href="example1a.mat">example1a.mat</a></li>
-  <li><a href="example1b.mat">example1b.mat</a></li>
-  <li><a href="make.m">make.m</a></li>
-  <li><a href="qpOASES.cpp">qpOASES.cpp</a></li>
-  <li><a href="qpOASES.m">qpOASES.m</a></li>
-  <li><a href="qpOASES_auxInput.m">qpOASES_auxInput.m</a></li>
-  <li><a href="qpOASES_octave_utils.cpp">qpOASES_octave_utils.cpp</a></li>
-  <li><a href="qpOASES_octave_utils.hpp">qpOASES_octave_utils.hpp</a></li>
-  <li><a href="qpOASES_options.m">qpOASES_options.m</a></li>
-  <li><a href="qpOASES_sequence.cpp">qpOASES_sequence.cpp</a></li>
-  <li><a href="qpOASES_sequence.m">qpOASES_sequence.m</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/make.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/make.m
deleted file mode 100644
index 28cef5fc907..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/make.m
+++ /dev/null
@@ -1,238 +0,0 @@
-function [] = make( varargin )
-%MAKE Compiles the octave interface of qpOASES.
-%
-%Type  make            to compile all interfaces that 
-%                      have been modified,
-%type  make clean      to delete all compiled interfaces,
-%type  make clean all  to first delete and then compile 
-%                      all interfaces,
-%type  make 'name'     to compile only the interface with
-%                      the given name (if it has been modified),
-%type  make 'opt'      to compile all interfaces using the 
-%                      given compiler options.
-%
-%Copyright (C) 2013-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%Christian Kirches et al. All rights reserved.
-
-%%
-%%	This file is part of qpOASES.
-%%
-%%	qpOASES -- An Implementation of the Online Active Set Strategy.
-%%	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%%	Christian Kirches et al. All rights reserved.
-%%
-%%	qpOASES 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.
-%%
-%%	qpOASES 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 qpOASES; if not, write to the Free Software
-%%	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-%%
-
-%%
-%%	Filename:  interfaces/octave/make.m
-%%	Author:    Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
-%%	Version:   3.0
-%%	Date:      2007-2014
-%%
-
-       
-    %% consistency check
-    if ( exist( [pwd, '/make.m'],'file' ) == 0 )
-        error( ['ERROR (',mfilename '.m): Run this make script directly within the directory', ...
-                '<qpOASES-inst-dir>/interfaces/octave, please.'] );
-    end
-
-
-    if ( nargin > 2 )
-        error( ['ERROR (',mfilename '.m): At most two make arguments supported!'] );
-    else
-        [ doClean,fcnNames,userFlags ] = analyseMakeArguments( nargin,varargin );
-    end
-
-    
-    %% define compiler settings
-    QPOASESPATH = '../../';
-    
-    DEBUGFLAGS = ' ';
-    %DEBUGFLAGS = ' -g CXXDEBUGFLAGS=''$CXXDEBUGFLAGS -Wall -pedantic -Wshadow'' ';
-
-    IFLAGS = [ '-I. -I',QPOASESPATH,'include',' -I',QPOASESPATH,'src',' ' ];
-    CPPFLAGS = [ IFLAGS, DEBUGFLAGS, '-D__cpluplus -D__MATLAB__ -D__SINGLE_OBJECT__',' ' ];  %%removed: -largeArrayDims
-    defaultFlags = '-D__NO_COPYRIGHT__ '; %% -D__NO_COPYRIGHT__ -D__SUPPRESSANYOUTPUT__                        %%removed: -O
-
-    if ( ispc == 0 )
-        CPPFLAGS  = [ CPPFLAGS, '-DLINUX ',' ' ]; 
-    else
-        CPPFLAGS  = [ CPPFLAGS, '-DWIN32 ',' ' ];
-    end
-
-    if ( isempty(userFlags) > 0 )
-        CPPFLAGS = [ CPPFLAGS, defaultFlags,' ' ];
-    else
-        CPPFLAGS = [ CPPFLAGS, userFlags,' ' ];
-    end
-
-    mexExt = mexext();
-    
-    
-    %% ensure copyright notice is displayed
-    if ~isempty( strfind( CPPFLAGS,'-D__NO_COPYRIGHT__' ) )
-        printCopyrightNotice( );
-    end
-    
-    
-    %% clean if desired
-    if ( doClean > 0 )
-        
-        eval( 'delete *.o;' );
-        eval( ['delete *.',mexExt,'*;'] );
-        disp( [ 'INFO (',mfilename '.m): Cleaned all compiled files.'] );
-        pause( 0.2 );
-        
-    end
-    
-    
-    if ( ~isempty(userFlags) )
-        disp( [ 'INFO (',mfilename '.m): Compiling all files with user-defined compiler flags (''',userFlags,''')...'] );
-    end
-    
-
-    %% call mex compiler
-    for ii=1:length(fcnNames)
-        
-        cmd = [ 'mkoctfile --mex --output ', fcnNames{ii}, '.', mexext(), ' ', CPPFLAGS, [fcnNames{ii},'.cpp'] ];
-        
-        if ( exist( [fcnNames{ii},'.',mexExt],'file' ) == 0 )
-            
-            eval( cmd );
-            disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' successfully created.'] );
-            
-        else
-            
-            % check modification time of source/Make files and compiled mex file
-            cppFile = dir( [pwd,'/',fcnNames{ii},'.cpp'] );
-            cppFileTimestamp = getTimestamp( cppFile.date ); 
-            
-            utilsFile = dir( [pwd,'/qpOASES_octave_utils.cpp'] );
-            utilsFileTimestamp = getTimestamp( utilsFile.date );
-            
-            makeFile = dir( [pwd,'/make.m'] );
-            makeFileTimestamp = getTimestamp( makeFile.date );
-            
-            mexFile = dir( [pwd,'/',fcnNames{ii},'.',mexExt] );
-            if ( isempty(mexFile) == 0 )
-                mexFileTimestamp = getTimestamp( mexFile.date );
-            else
-                mexFileTimestamp = 0;
-            end
-            
-            if ( ( cppFileTimestamp   >= mexFileTimestamp ) || ...
-                 ( utilsFileTimestamp >= mexFileTimestamp ) || ...
-                 ( makeFileTimestamp  >= mexFileTimestamp ) )
-                eval( cmd );
-                disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' successfully created.'] );
-            else            
-                disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' already exists.'] );
-            end
-            
-        end
-        
-    end
-
-    %% add qpOASES directory to path
-    path( path,pwd );
-
-end
-
-
-function [ doClean,fcnNames,userIFlags ] = analyseMakeArguments( nArgs,args )
-
-    doClean = 0;
-    fcnNames = [];
-    userIFlags = [];
-
-    switch ( nArgs )
-        
-        case 1
-            if ( strcmp( args{1},'all' ) > 0 )
-                fcnNames = { 'qpOASES','qpOASES_sequence' };
-            elseif ( strcmp( args{1},'qpOASES' ) > 0 )
-                fcnNames = { 'qpOASES' };
-            elseif ( strcmp( args{1},'qpOASES_sequence' ) > 0 )
-                fcnNames = { 'qpOASES_sequence' };
-            elseif ( strcmp( args{1},'clean' ) > 0 )
-                doClean = 1;
-            elseif ( strcmp( args{1}(1),'-' ) > 0 )
-                % make clean all with user-specified compiler flags
-                userIFlags = args{1};
-                doClean = 1;
-                fcnNames = { 'qpOASES','qpOASES_sequence' };
-            else
-                error( ['ERROR (',mfilename '.m): Invalid first argument (''',args{1},''')!'] );
-            end
-
-        case 2
-            if ( strcmp( args{1},'clean' ) > 0 )
-                doClean = 1;
-            else
-                error( ['ERROR (',mfilename '.m): First argument must be ''clean'' if two arguments are provided!'] );
-            end
-            
-            if ( strcmp( args{2},'all' ) > 0 )
-                fcnNames = { 'qpOASES','qpOASES_sequence' };
-            elseif ( strcmp( args{2},'qpOASES' ) > 0 )
-                fcnNames = { 'qpOASES' };
-            elseif ( strcmp( args{2},'qpOASES_sequence' ) > 0 )
-                fcnNames = { 'qpOASES_sequence' };
-            else
-                error( ['ERROR (',mfilename '.m): Invalid second argument (''',args{2},''')!'] );
-            end
-            
-        otherwise
-            fcnNames = { 'qpOASES','qpOASES_sequence' };
-            
-    end
-    
-end
-
-
-function [ timestamp ] = getTimestamp( dateString )
-
-    try 
-        timestamp = datenum( dateString );
-    catch
-        timestamp = Inf;
-    end
-
-end
-
-
-function [ ] = printCopyrightNotice( )
-
-    disp( ' ' );
-    disp( 'qpOASES -- An Implementation of the Online Active Set Strategy.' );
-    disp( 'Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,' );
-    disp( 'Christian Kirches et al. All rights reserved.' );
-    disp( ' ' );
-    disp( 'qpOASES is distributed under the terms of the' );
-    disp( 'GNU Lesser General Public License 2.1 in the hope that it will be' );
-    disp( 'useful, but WITHOUT ANY WARRANTY; without even the implied warranty' );
-    disp( 'of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.' );
-    disp( 'See the GNU Lesser General Public License for more details.' );
-    disp( ' ' );
-    disp( ' ' );
-
-end
-
-
-%%
-%%	end of file
-%%
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES.cpp
deleted file mode 100644
index 32aaf7ed9f7..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES.cpp
+++ /dev/null
@@ -1,557 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file interfaces/octave/qpOASES.cpp
- *	\author Hans Joachim Ferreau, Alexander Buchner, Andreas Potschka
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Interface for Matlab(R) that enables to call qpOASES as a MEX function.
- *
- */
-
-
-#include <qpOASES.hpp>
-
-
-USING_NAMESPACE_QPOASES
-
-
-#include "qpOASES_octave_utils.hpp"
-
-/** initialise handle counter of QPInstance class */
-int QPInstance::s_nexthandle = 1;
-
-/** global pointer to QP objects */
-static std::vector<QPInstance *> g_instances;
-
-#include "qpOASES_octave_utils.cpp"
-
-
-/*
- *	q p O A S E S m e x _ c o n s t r a i n t s
- */
-int qpOASESmex_constraints(		int nV, int nC, int nP,
-								SymmetricMatrix *H, real_t* g, Matrix *A,
-								real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
-								int nWSRin, real_t maxCpuTimeIn,
-								real_t* x0, Options* options,
-								int nOutputs, mxArray* plhs[],
-								double* guessedBounds, double* guessedConstraints
-								)
-{
-	int nWSRout;
-	real_t maxCpuTimeOut;
-	
-	/* 1) Setup initial QP. */
-	QProblem QP( nV,nC );
-	QP.setOptions( *options );
-
-	/* 2) Solve initial QP. */
-	returnValue returnvalue;
-
-	Bounds bounds(nV);
-	Constraints constraints(nC);
-	if (guessedBounds != 0) {
-		for (int i = 0; i < nV; i++) {
-			if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_LOWER);
-			} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_UPPER);
-			} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_INACTIVE);
-			} else {
-				char msg[MAX_STRING_LENGTH];
-				snprintf(msg, MAX_STRING_LENGTH,
-						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
-				myMexErrMsgTxt(msg);
-				return -1;
-			}
-		}
-	}
-
-	if (guessedConstraints != 0) {
-		for (int i = 0; i < nC; i++) {
-			if ( isEqual(guessedConstraints[i],-1.0) == BT_TRUE ) {
-				constraints.setupConstraint(i, ST_LOWER);
-			} else if ( isEqual(guessedConstraints[i],1.0) == BT_TRUE ) {
-				constraints.setupConstraint(i, ST_UPPER);
-			} else if ( isEqual(guessedConstraints[i],0.0) == BT_TRUE ) {
-				constraints.setupConstraint(i, ST_INACTIVE);
-			} else {
-				char msg[MAX_STRING_LENGTH];
-				snprintf(msg, MAX_STRING_LENGTH,
-						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of constraints!");
-				myMexErrMsgTxt(msg);
-				return -1;
-			}
-		}
-	}
-
-	nWSRout = nWSRin;
-	maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-	if (x0 == 0 && guessedBounds == 0 && guessedConstraints == 0)
-		returnvalue = QP.init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut);
-	else
-		returnvalue = QP.init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut, x0, 0,
-				guessedBounds != 0 ? &bounds : 0,
-				guessedConstraints != 0 ? &constraints : 0);
-
-	/* 3) Solve remaining QPs and assign lhs arguments. */
-	/*    Set up pointers to the current QP vectors */
-	real_t* g_current   = g;
-	real_t* lb_current  = lb;
-	real_t* ub_current  = ub;
-	real_t* lbA_current = lbA;
-	real_t* ubA_current = ubA;
-
-	/* Loop through QP sequence. */
-	for ( int k=0; k<nP; ++k )
-	{
-		if ( k > 0 )
-		{
-			/* update pointers to the current QP vectors */
-			g_current = &(g[k*nV]);
-			if ( lb != 0 )
-				lb_current = &(lb[k*nV]);
-			if ( ub != 0 )
-				ub_current = &(ub[k*nV]);
-			if ( lbA != 0 )
-				lbA_current = &(lbA[k*nC]);
-			if ( ubA != 0 )
-				ubA_current = &(ubA[k*nC]);
-
-			nWSRout = nWSRin;
-			maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-			returnvalue = QP.hotstart( g_current,lb_current,ub_current,lbA_current,ubA_current, nWSRout,&maxCpuTimeOut );
-		}
-
-		/* write results into output vectors */
-		obtainOutputs(	k,&QP,returnvalue,nWSRout,maxCpuTimeOut,
-						nOutputs,plhs,nV,nC );
-	}
-
-	//QP.writeQpDataIntoMatFile( "qpDataMat0.mat" );
-
-	return 0;
-}
-
-
-
-/*
- *	q p O A S E S m e x _ b o u n d s
- */
-int qpOASESmex_bounds(	int nV, int nP,
-						SymmetricMatrix *H, real_t* g,
-						real_t* lb, real_t* ub,
-						int nWSRin, real_t maxCpuTimeIn,
-						real_t* x0, Options* options,
-						int nOutputs, mxArray* plhs[],
-						double* guessedBounds
-						)
-{
-	int nWSRout;
-	real_t maxCpuTimeOut;
-
-	/* 1) Setup initial QP. */
-	QProblemB QP( nV );
-	QP.setOptions( *options );
-
-	/* 2) Solve initial QP. */
-	returnValue returnvalue;
-
-	Bounds bounds(nV);
-	if (guessedBounds != 0) {
-		for (int i = 0; i < nV; i++) {
-			if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_LOWER);
-			} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_UPPER);
-			} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_INACTIVE);
-			} else {
-				char msg[MAX_STRING_LENGTH];
-				snprintf(msg, MAX_STRING_LENGTH,
-						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
-				myMexErrMsgTxt(msg);
-				return -1;
-			}
-		}
-	}
-
-	nWSRout = nWSRin;
-	maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-	if (x0 == 0 && guessedBounds == 0)
-		returnvalue = QP.init( H,g,lb,ub, nWSRout,&maxCpuTimeOut );
-	else
-		returnvalue = QP.init( H,g,lb,ub, nWSRout,&maxCpuTimeOut, x0, 0,
-				guessedBounds != 0 ? &bounds : 0);
-
-	/* 3) Solve remaining QPs and assign lhs arguments. */
-	/*    Set up pointers to the current QP vectors */
-	real_t* g_current  = g;
-	real_t* lb_current = lb;
-	real_t* ub_current = ub;
-
-	/* Loop through QP sequence. */
-	for ( int k=0; k<nP; ++k )
-	{
-		if ( k > 0 )
-		{
-			/* update pointers to the current QP vectors */
-			g_current = &(g[k*nV]);
-			if ( lb != 0 )
-				lb_current = &(lb[k*nV]);
-			if ( ub != 0 )
-				ub_current = &(ub[k*nV]);
-
-            nWSRout = nWSRin;
-			maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-			returnvalue = QP.hotstart( g_current,lb_current,ub_current, nWSRout,&maxCpuTimeOut );
-		}
-
-		/* write results into output vectors */
-		obtainOutputs(	k,&QP,returnvalue,nWSRout,maxCpuTimeOut,
-						nOutputs,plhs,nV );
-	}
-
-	return 0;
-}
-
-
-
-/*
- *	m e x F u n c t i o n
- */
-void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
-{
-	/* inputs */
-	SymmetricMatrix *H=0;
-	Matrix *A=0;
-	real_t *g=0, *lb=0, *ub=0, *lbA=0, *ubA=0, *x0=0;
-	int H_idx, g_idx, A_idx, lb_idx, ub_idx, lbA_idx, ubA_idx, options_idx=-1, x0_idx=-1, auxInput_idx=-1;
-
-	double *guessedBoundsAndConstraints = 0;
-	double *guessedBounds = 0, *guessedConstraints = 0;
-
-    /* Setup default options */
-	Options options;
-	options.printLevel = PL_LOW;
-	#ifdef __DEBUG__
-	options.printLevel = PL_HIGH;
-	#endif
-	#ifdef __SUPPRESSANYOUTPUT__
-	options.printLevel = PL_NONE;
-	#endif
-
-	/* dimensions */
-	unsigned int nV=0, nC=0, nP=0;
-	BooleanType isSimplyBoundedQp = BT_FALSE;
-
-	/* sparse matrix indices and values */
-	sparse_int_t *Hir = 0, *Hjc = 0, *Air = 0, *Ajc = 0;
-	real_t *Hv = 0, *Av = 0;
-
-	/* I) CONSISTENCY CHECKS: */
-	/* 1a) Ensure that qpOASES is called with a feasible number of input arguments. */
-	if ( ( nrhs < 4 ) || ( nrhs > 9 ) )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES' for further information." );
-		return;
-	}
-    
-	/* 2) Check for proper number of output arguments. */
-	if ( nlhs > 6 )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): At most six output arguments are allowed: \n    [x,fval,exitflag,iter,lambda,workingSet]!" );
-		return;
-	}
-	if ( nlhs < 1 )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): At least one output argument is required: [x,...]!" );
-		return;
-	}
-
-
-	/* II) PREPARE RESPECTIVE QPOASES FUNCTION CALL: */
-	/*     Choose between QProblem and QProblemB object and assign the corresponding
-	 *     indices of the input pointer array in to order to access QP data correctly. */
-	H_idx = 0;
-	g_idx = 1;
-	nV = (int)mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
-	nP = (int)mxGetN( prhs[ g_idx ] ); /* number of columns of the gradient matrix (vectors series have to be stored columnwise!) */
-
-	if ( nrhs <= 6 )
-        isSimplyBoundedQp = BT_TRUE;
-	else
-		isSimplyBoundedQp = BT_FALSE;
-
-
-	/* 0) Check whether options are specified .*/
-	if ( isSimplyBoundedQp == BT_TRUE )
-	{
-		if ( ( nrhs >= 5 ) && ( !mxIsEmpty(prhs[4]) ) && ( mxIsStruct(prhs[4]) ) )
-			options_idx = 4;
-	}
-	else
-	{
-		/* Consistency check */
-		if ( ( !mxIsEmpty(prhs[4]) ) && ( mxIsStruct(prhs[4]) ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Fifth input argument must not be a struct when solving QP with general constraints!\nType 'help qpOASES' for further information." );
-			return;
-		}
-
-		if ( ( nrhs >= 8 ) && ( !mxIsEmpty(prhs[7]) ) && ( mxIsStruct(prhs[7]) ) )
-			options_idx = 7;
-	}
-
-	// Is the third argument constraint Matrix A?
-	int numberOfColumns = (int)mxGetN(prhs[2]);
-
-	/* 1) Simply bounded QP. */
-	if ( ( isSimplyBoundedQp == BT_TRUE ) ||
-		 ( ( numberOfColumns == 1 ) && ( nV != 1 ) ) )
-	{
-		lb_idx   = 2;
-		ub_idx   = 3;
-
-		if ( ( nrhs >= 6 ) && ( !mxIsEmpty(prhs[5]) ) )
-		{ 
-			/* auxInput specified */
-			if ( mxIsStruct(prhs[5]) )
-			{
-				auxInput_idx = 5;
-				x0_idx = -1;
-			}
-			else
-			{
-				auxInput_idx = -1;
-				x0_idx = 5;
-			}
-		}
-		else
-		{
-			auxInput_idx = -1;
-			x0_idx = -1;
-		}
-	}
-	else
-	{
-		A_idx = 2;
-
-		/* If constraint matrix is empty, use a QProblemB object! */
-		if ( mxIsEmpty( prhs[ A_idx ] ) )
-		{
-			lb_idx   = 3;
-			ub_idx   = 4;
-
-			nC = 0;
-		}
-		else
-		{
-			lb_idx   = 3;
-			ub_idx   = 4;
-			lbA_idx  = 5;
-			ubA_idx  = 6;
-
-			nC = (int)mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */
-		}
-
-		if ( ( nrhs >= 9 ) && ( !mxIsEmpty(prhs[8]) ) )
-		{ 
-			/* auxInput specified */
-			if ( mxIsStruct(prhs[8]) )
-			{
-				auxInput_idx = 8;
-				x0_idx = -1;
-			}
-			else
-			{
-				auxInput_idx = -1;
-				x0_idx = 8;
-			}
-		}
-		else
-		{
-			auxInput_idx = -1;
-			x0_idx = -1;
-		}
-	}
-
-
-	/* ensure that data is given in real_t precision */
-	if ( ( mxIsDouble( prhs[ H_idx ] ) == 0 ) ||
-		 ( mxIsDouble( prhs[ g_idx ] ) == 0 ) )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
-		return;
-	}
-
-	/* check if supplied data contains 'NaN' or 'Inf' */
-	if (containsNaNorInf(prhs, nV * nV, H_idx, 0) == BT_TRUE) {
-		return;
-	}
-
-	if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
-		return;
-	}
-
-	if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
-		return;
-	}
-	if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
-		return;
-	}
-
-	/* Check inputs dimensions and assign pointers to inputs. */
-	if ( mxGetN( prhs[ H_idx ] ) != nV )
-	{
-		char msg[MAX_STRING_LENGTH]; 
-		snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Hessian matrix input dimension mismatch (%ld != %d)!", 
-				(long int)mxGetN(prhs[H_idx]), nV);
-		myMexErrMsgTxt(msg);
-		return;
-	}
-
-	if ( nC > 0 )
-	{
-		/* ensure that data is given in real_t precision */
-		if ( mxIsDouble( prhs[ A_idx ] ) == 0 )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
-			return;
-		}
-
-		/* Check inputs dimensions and assign pointers to inputs. */
-		if ( mxGetN( prhs[ A_idx ] ) != nV )
-		{
-			char msg[MAX_STRING_LENGTH]; 
-			snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Constraint matrix input dimension mismatch (%ld != %d)!", 
-					(long int)mxGetN(prhs[A_idx]), nV);
-			myMexErrMsgTxt(msg);
-			return;
-		}
-
-		if (containsNaNorInf(prhs, nV * nC, A_idx, 0) == BT_TRUE) {
-			return;
-		}
-		if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
-			return;
-		}
-		if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
-			return;
-		}
-	}
-
-	/* check dimensions and copy auxInputs */
-	if ( smartDimensionCheck( &g,nV,nP, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
-		return;
-
-	if ( smartDimensionCheck( &lb,nV,nP, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
-		return;
-
-	if ( smartDimensionCheck( &ub,nV,nP, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
-		return;
-
-	if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,x0_idx ) != SUCCESSFUL_RETURN )
-		return;
-
-	if ( nC > 0 )
-	{
-		if ( smartDimensionCheck( &lbA,nC,nP, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &ubA,nC,nP, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
-			return;
-	}
-
-	if ( auxInput_idx >= 0 )
-		setupAuxiliaryInputs( prhs[auxInput_idx],nV,nC, &x0,&guessedBoundsAndConstraints,&guessedBounds,&guessedConstraints );
-
-	
-	/* III) ACTUALLY PERFORM QPOASES FUNCTION CALL: */
-	int nWSRin = 5*(nV+nC);
-	real_t maxCpuTimeIn = -1.0;
-
-	if ( options_idx > 0 )
-		setupOptions( &options,prhs[options_idx],nWSRin,maxCpuTimeIn );
-
-	/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
-	setupHessianMatrix(	prhs[H_idx],nV, &H,&Hir,&Hjc,&Hv );
-
-	/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
-	if ( nC > 0 )
-		setupConstraintMatrix( prhs[A_idx],nV,nC, &A,&Air,&Ajc,&Av );
-
-	allocateOutputs( nlhs,plhs,nV,nC,nP );
-
-	if ( nC == 0 )
-	{
-		/* Call qpOASES (using QProblemB class). */
-		qpOASESmex_bounds(	nV,nP,
-							H,g,
-							lb,ub,
-							nWSRin,maxCpuTimeIn,
-							x0,&options,
-							nlhs,plhs,
-							guessedBounds
-							);
-
-		deleteAuxiliaryInputs( &guessedBounds,0 );
-        delete H;
-		if (Hv != 0) delete[] Hv;
-		if (Hjc != 0) delete[] Hjc;
-		if (Hir != 0) delete[] Hir;
-		return;
-	}
-	else
-	{
-		/* Call qpOASES (using QProblem class). */
-		qpOASESmex_constraints(	nV,nC,nP,
-								H,g,A,
-								lb,ub,lbA,ubA,
-								nWSRin,maxCpuTimeIn,
-								x0,&options,
-								nlhs,plhs,
-								guessedBounds, guessedConstraints
-								);
-		
-		deleteAuxiliaryInputs( &guessedBounds,&guessedConstraints );
-		delete A;
-		delete H;
-		if (Av != 0) delete[] Av;
-		if (Ajc != 0) delete[] Ajc;
-		if (Air != 0) delete[] Air;
-		if (Hv != 0) delete[] Hv;
-		if (Hjc != 0) delete[] Hjc;
-		if (Hir != 0) delete[] Hir;
-		return;
-	}
-}
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES.m
deleted file mode 100644
index 970a446414c..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES.m
+++ /dev/null
@@ -1,77 +0,0 @@
-%qpOASES -- An Implementation of the Online Active Set Strategy.
-%Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%Christian Kirches et al. All rights reserved.
-%
-%qpOASES is distributed under the terms of the
-%GNU Lesser General Public License 2.1 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.
-%
-%---------------------------------------------------------------------------------
-%
-%qpOASES solves (a series) of quadratic programming (QP) problems of the
-%following form:
-%
-%                min   1/2*x'Hx + x'g
-%                s.t.  lb  <=  x <= ub
-%                      lbA <= Ax <= ubA  {optional}
-%
-%Call
-%
-%    [x,fval,exitflag,iter,lambda,auxOutput] = 
-%                     qpOASES( H,g,A,lb,ub,lbA,ubA{,options{,auxInput} )
-%
-%for solving the above-mentioned QP. H must be a symmetric (but possibly 
-%indefinite) matrix and all vectors g, lb, ub, lbA, ubA have to be given
-%as column vectors. Options can be generated using the qpOASES_options command, 
-%otherwise default values are used. Optionally, further auxiliary inputs
-%may be generated using qpOASES_auxInput command and passed to the solver.
-%Both matrices H or A may be passed in sparse matrix format.
-%
-%Call
-%
-%    [x,fval,exitflag,iter,lambda,auxOutput] =
-%                     qpOASES( H,g,lb,ub{,options{,auxInput} )
-%
-%for solving the above-mentioned QP without general constraints.
-%
-%
-%Optional outputs (only x is mandatory):
-%    x          -  Optimal primal solution vector (if exitflag==0).
-%    fval       -  Optimal objective function value (if exitflag==0).
-%    exitflag   -   0: QP problem solved,
-%                   1: QP could not be solved within given number of iterations,
-%                  -1: QP could not be solved due to an internal error,
-%                  -2: QP is infeasible (and thus could not be solved),
-%                  -3: QP is unbounded (and thus could not be solved).
-%    iter       -  Number of active set iterations actually performed.
-%    lambda     -  Optimal dual solution vector (if status==0).
-%    auxOutput  -  Struct containing auxiliary outputs as described below.
-%
-%The auxOutput struct contains the following entries:
-%    workingSet -  The working set at point x. The working set is a subset
-%                  of the active set (which is the set of all indices
-%                  corresponding to bounds/constraints that hold with 
-%                  equality). The working set corresponds to bound/
-%                  constraint row vectors forming a linear independent set.
-%                  The first nV elements correspond to the bounds, the last
-%                  nC elements to the constraints.
-%                  The working set is encoded as follows:
-%                   1: bound/constraint at its upper bound
-%                   0: bound/constraint not at any bound
-%                  -1: bound/constraint at its lower bound
-%    cpuTime    -  Internally measured CPU time for solving QP.
-%
-%
-%If not a single QP but a sequence of QPs with varying vectors is to be solved,
-%the i-th QP is given by the i-th columns of the QP vectors g, lb, ub, lbA, ubA
-%(i.e. they are matrices in this case). Both matrices H and A remain constant.
-%
-%See also QPOASES_OPTIONS, QPOASES_AUXINPUT, QPOASES_SEQUENCE
-%
-%
-%For additional information see the qpOASES User's Manual or
-%visit http://www.qpOASES.org/.
-%
-%Please send remarks and questions to support@qpOASES.org!
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_auxInput.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_auxInput.m
deleted file mode 100644
index cd27e656909..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_auxInput.m
+++ /dev/null
@@ -1,103 +0,0 @@
-%qpOASES -- An Implementation of the Online Active Set Strategy.
-%Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%Christian Kirches et al. All rights reserved.
-%
-%qpOASES is distributed under the terms of the
-%GNU Lesser General Public License 2.1 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.
-%
-%---------------------------------------------------------------------------------
-%
-%Returns a struct containing all possible auxiliary inputs to be passed 
-%to qpOASES.
-%
-%Call
-%    auxInput = qpOASES_auxInput();
-%to obtain a struct with all auxiliary inputs empty.
-%
-%Call
-%    options = qpOASES_auxInput( 'input1',value1,'input2',value2,... )
-%to obtain a struct with 'input1' set to value1 etc. and all remaining
-%auxiliary inputs empty.
-%
-%Call
-%    options = qpOASES_auxInput( oldInputs,'input1',value1,... )
-%to obtain a copy of the options struct oldInputs but with 'input1' set to 
-%value1 etc.
-%
-%
-%qpOASES features the following auxiliary inputs:
-%  x0                   -  Initial guess for optimal primal solution.
-%  guessedWorkingSet    -  Initial guess for working set at optimal
-%                          solution. The first nV elements correspond 
-%                          to the bounds, the last nC elements to the 
-%                          constraints.
-%                          The working set needs to be encoded as follows:
-%                          1: bound/constraint at its upper bound
-%                          0: bound/constraint not at any bound
-%                         -1: bound/constraint at its lower bound
-%
-%
-%See also QPOASES, QPOASES_SEQUENCE, QPOASES_OPTIONS
-%
-%
-%For additional information see the qpOASES User's Manual or
-%visit http://www.qpOASES.org/.
-%
-%Please send remarks and questions to support@qpOASES.org!
-function [ auxInput ] = qpOASES_auxInput( varargin )
-
-	firstIsStruct = 0;
-
-	if ( nargin == 0 ) 
-		auxInput = qpOASES_emptyAuxInput();
-	else
-		if ( isstruct( varargin{1} ) )
-			if ( mod( nargin,2 ) ~= 1 )
-				error('ERROR (qpOASES_auxInput): Auxiliary inputs must be specified in pairs!');
-			end
-			auxInput = varargin{1};
-			firstIsStruct = 1;
-        else
-            if ( mod( nargin,2 ) ~= 0 )
-				error('ERROR (qpOASES_auxInput): Auxiliary inputs must be specified in pairs!');
-            end
-            auxInput = qpOASES_emptyAuxInput();
-		end
-	end
-
-	% set options to user-defined values  
-	for i=(1+firstIsStruct):2:nargin
-
-		argName  = varargin{i};
-		argValue = varargin{i+1};
-
-        if ( ( isempty( argName ) ) || ( ~ischar( argName ) ) )
-			error('ERROR (qpOASES_auxInput): Argmument no. %d has to be a non-empty string!',i );
-        end
-			
-        if ( ( ischar(argValue) ) || ( ~isnumeric( argValue ) ) )
-			error('ERROR (qpOASES_auxInput): Argmument no. %d has to be a numerical constant!',i+1 );
-        end
-
-        if ( ~isfield( auxInput,argName ) )
-			error('ERROR (qpOASES_auxInput): Argmument no. %d is not a valid auxiliary input!',i );
-        end
-
-		eval( ['auxInput.',argName,' = argValue;'] );
-
-	end
-
-end
-
-
-function [ auxInput ] = qpOASES_emptyAuxInput( )
-
-	% setup auxiliary input struct with all entries empty
-	auxInput = struct(	'x0',                 [], ...
-						'guessedWorkingSet',  []  ...
-                        );
-
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_octave_utils.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_octave_utils.cpp
deleted file mode 100644
index 7f75b721c8e..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_octave_utils.cpp
+++ /dev/null
@@ -1,884 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file interfaces/octave/qpOASES_octave_utils.cpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Alexander Buchner
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Collects utility functions for Interface to octave that
- *	enables to call qpOASES as a MEX function.
- *
- */
-
-
-
-QPInstance::QPInstance(	int _nV, int _nC, BooleanType _isSimplyBounded
-						)
-{
-	handle = s_nexthandle++;
-
-	if ( _nC > 0 )
-		isSimplyBounded = BT_FALSE;
-	else
-		isSimplyBounded = _isSimplyBounded;
-	
-	if ( isSimplyBounded == BT_TRUE )
-	{
-		sqp = 0;
-		qpb = new QProblemB( _nV );
-	}
-	else
-	{
-		sqp = new SQProblem( _nV,_nC );
-		qpb = 0;
-	}
-
-	H = 0;
-	A = 0;
-	Hir = 0; 
-	Hjc = 0; 
-	Air = 0; 
-	Ajc = 0;
-	Hv = 0;
-	Av = 0;
-}	
-
-
-QPInstance::~QPInstance( )
-{		
-	deleteQPMatrices();
-
-	if ( sqp != 0 )
-	{
-		delete sqp;
-		sqp = 0;
-	}
-
-	if ( qpb != 0 )
-	{
-		delete qpb;
-		qpb = 0;
-	}
-}
-
-
-returnValue QPInstance::deleteQPMatrices( )
-{
-	if ( H != 0 )
-	{
-		delete H;
-		H = 0;
-	}
-
-	if (Hv != 0)
-	{
-		delete[] Hv;
-		Hv = 0;
-	}
-	
-	if (Hjc != 0)
-	{
-		delete[] Hjc;
-		Hjc = 0;
-	}
-	
-	if (Hir != 0)
-	{
-		delete[] Hir;
-		Hir = 0;
-	}
-	
-	if ( A != 0 )
-	{
-		delete A;
-		A = 0;
-	}
-
-	if (Av != 0)
-	{
-		delete[] Av;
-		Av = 0;
-	}
-	
-	if (Ajc != 0)
-	{
-		delete[] Ajc;
-		Ajc = 0;
-	}
-	
-	if (Air != 0)
-	{
-		delete[] Air;
-		Air = 0;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-int QPInstance::getNV() const
-{
-    if ( sqp != 0 )
-        return sqp->getNV();
-    
-    if ( qpb != 0 )
-        return qpb->getNV();
-    
-    return 0;
-}
-
-
-int QPInstance::getNC() const
-{
-    if ( sqp != 0 )
-        return sqp->getNC();
-   
-    return 0;
-}
-
-
-/*
- *	a l l o c a t e Q P r o b l e m I n s t a n c e
- */
-int allocateQPInstance(	int nV, int nC, BooleanType isSimplyBounded, const Options *options
-						)
-{
-	QPInstance* inst = new QPInstance( nV,nC,isSimplyBounded );
-
-	if ( inst->sqp != 0 )
-		inst->sqp->setOptions ( *options );
-	
-	if ( inst->qpb != 0 )
-		inst->qpb->setOptions ( *options );
-
-	g_instances.push_back (inst);
-	return inst->handle;
-}
-
-
-/*
- *  g e t Q P r o b l e m I n s t a n c e
- */
-QPInstance* getQPInstance( int handle )
-{
-	unsigned int ii;
-	// TODO: this may become slow ...
-	for (ii = 0; ii < g_instances.size (); ++ii)
-		if (g_instances[ii]->handle == handle)
-			return g_instances[ii];
-	return 0;
-}
-
-
-/*
- *	d e l e t e Q P r o b l e m I n s t a n c e
- */
-void deleteQPInstance( int handle )
-{
-	QPInstance *instance = getQPInstance (handle);
-	if (instance != 0) {
-		for (std::vector<QPInstance*>::iterator itor = g_instances.begin ();
-		     itor != g_instances.end (); ++itor)
-		     if ((*itor)->handle == handle) {
-				g_instances.erase (itor);
-				break;
-			}
-		delete instance;
-	}
-}
-
-
-
-/*
- *	s m a r t D i m e n s i o n C h e c k
- */
-returnValue smartDimensionCheck(	real_t** input, unsigned int m, unsigned int n, BooleanType emptyAllowed,
-									const mxArray* prhs[], int idx
-									)
-{
-	/* If index is negative, the input does not exist. */
-	if ( idx < 0 )
-	{
-		*input = 0;
-		return SUCCESSFUL_RETURN;
-	}
-
-	/* Otherwise the input has been passed by the user. */
-	if ( mxIsEmpty( prhs[ idx ] ) )
-	{
-		/* input is empty */
-		if ( ( emptyAllowed == BT_TRUE ) || ( idx == 0 ) ) /* idx==0 used for auxInput */
-		{
-			*input = 0;
-			return SUCCESSFUL_RETURN;
-		}
-		else
-		{
-			char msg[MAX_STRING_LENGTH];
-			if ( idx > 0 )
-				snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Empty argument %d not allowed!", idx+1);
-			myMexErrMsgTxt( msg );
-			return RET_INVALID_ARGUMENTS;
-		}
-	}
-	else
-	{
-		/* input is non-empty */
-        if ( mxIsSparse( prhs[ idx ] ) == 0 )
-        {
-            if ( ( mxGetM( prhs[ idx ] ) == m ) && ( mxGetN( prhs[ idx ] ) == n ) )
-            {
-                *input = (real_t*) mxGetPr( prhs[ idx ] );
-                return SUCCESSFUL_RETURN;
-            }
-            else
-            {
-                char msg[MAX_STRING_LENGTH];
-				if ( idx > 0 )
-					snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Input dimension mismatch for argument %d ([%ld,%ld] ~= [%d,%d]).",
-							 idx+1, (long int)mxGetM(prhs[idx]), (long int)mxGetN(prhs[idx]), m, n);
-				else /* idx==0 used for auxInput */
-					snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Input dimension mismatch for some auxInput entry ([%ld,%ld] ~= [%d,%d]).",
-							 (long int)mxGetM(prhs[idx]), (long int)mxGetN(prhs[idx]), m, n);
-                myMexErrMsgTxt( msg );
-                return RET_INVALID_ARGUMENTS;
-            }
-        }
-        else
-        {
-            char msg[MAX_STRING_LENGTH];
-			if ( idx > 0 )
-				snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Vector argument %d must not be in sparse format!", idx+1);
-			else /* idx==0 used for auxInput */
-				snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): auxInput entries must not be in sparse format!" );
-			myMexErrMsgTxt( msg );
-			return RET_INVALID_ARGUMENTS;
-        }
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	c o n t a i n s N a N
- */
-BooleanType containsNaN( const real_t* const data, int dim )
-{
-	int i;
-
-	if ( data == 0 )
-		return BT_FALSE;
-
-	for ( i = 0; i < dim; ++i )
-		if ( mxIsNaN(data[i]) == 1 )
-			return BT_TRUE;
-
-	return BT_FALSE;
-}
-
-
-/*
- *	c o n t a i n s I n f
- */
-BooleanType containsInf( const real_t* const data, int dim )
-{
-	int i;
-
-	if ( data == 0 )
-		return BT_FALSE;
-
-	for ( i = 0; i < dim; ++i )
-		if ( mxIsInf(data[i]) == 1 )
-			return BT_TRUE;
-
-	return BT_FALSE;
-}
-
-
-/*
- *	c o n t a i n s N a N o r I n f
- */
-BooleanType containsNaNorInf(const mxArray* prhs[], int dim, int rhs_index,
-		bool mayContainInf) {
-
-	char msg[MAX_STRING_LENGTH];
-
-	// overwrite dim for sparse matrices
-	if (mxIsSparse(prhs[rhs_index]) == 1) {
-		dim = mxGetNzmax(prhs[rhs_index]);
-	}
-
-	if (containsNaN((real_t*) mxGetPr(prhs[rhs_index]), dim) == BT_TRUE) {
-		snprintf(msg, MAX_STRING_LENGTH,
-				"ERROR (qpOASES): Argument %d contains 'NaN' !", rhs_index + 1);
-		myMexErrMsgTxt(msg);
-		return BT_TRUE;
-	}
-
-	if (mayContainInf == 0) {
-		if (containsInf((real_t*) mxGetPr(prhs[rhs_index]), dim) == BT_TRUE) {
-			snprintf(msg, MAX_STRING_LENGTH,
-					"ERROR (qpOASES): Argument %d contains 'Inf' !",
-					rhs_index + 1);
-			myMexErrMsgTxt(msg);
-			return BT_TRUE;
-		}
-	}
-
-	return BT_FALSE;
-}
-
-
-/*
- *	c o n v e r t F o r t r a n T o C
- */
-returnValue convertFortranToC( const real_t* const A_for, int nV, int nC, real_t* const A )
-{
-	int i,j;
-
-	for ( i=0; i<nC; ++i )
-		for ( j=0; j<nV; ++j )
-			A[i*nV + j] = A_for[j*nC + i];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	h a s O p t i o n s V a l u e
- */
-BooleanType hasOptionsValue( const mxArray* optionsPtr, const char* const optionString, double** optionValue )
-{
-	mxArray* optionName = mxGetField( optionsPtr,0,optionString );
-
-	if ( optionName == 0 )
-	{
-		char msg[MAX_STRING_LENGTH];
-		snprintf(msg, MAX_STRING_LENGTH, "Option struct does not contain entry '%s', using default value instead!", optionString );
-		mexWarnMsgTxt( msg );
-		return BT_FALSE;
-	}
-
-	if ( ( mxIsEmpty(optionName) == false ) && ( mxGetM( optionName ) == 1 ) && ( mxGetN( optionName ) == 1 ) )
-	{
-		*optionValue = mxGetPr( optionName );
-		return BT_TRUE;
-	}
-	else
-	{
-		char msg[MAX_STRING_LENGTH];
-		snprintf(msg, MAX_STRING_LENGTH, "Option '%s' is not a scalar, using default value instead!", optionString );
-		mexWarnMsgTxt( msg );
-		return BT_FALSE;
-	}
-}
-
-
-/*
- *	s e t u p O p t i o n s
- */
-returnValue setupOptions( Options* options, const mxArray* optionsPtr, int& nWSRin, real_t& maxCpuTime )
-{
-	double* optionValue;
-	int optionValueInt;
-
-	/* Check for correct number of option entries;
-	 * may occur, e.g., if user types options.<misspelledName> = <someValue>; */
-	if ( mxGetNumberOfFields(optionsPtr) != 31 )
-		mexWarnMsgTxt( "Options might be set incorrectly as struct has wrong number of entries!\n         Type 'help qpOASES_options' for further information." );
-
-
-	if ( hasOptionsValue( optionsPtr,"maxIter",&optionValue ) == BT_TRUE )
-		if ( *optionValue >= 0.0 )
-			nWSRin = (int)*optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"maxCpuTime",&optionValue ) == BT_TRUE )
-		if ( *optionValue >= 0.0 )
-			maxCpuTime = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"printLevel",&optionValue ) == BT_TRUE )
-	{
-        #ifdef __SUPPRESSANYOUTPUT__
-        options->printLevel = PL_NONE;
-        #else
-		optionValueInt = (int)*optionValue;
-		options->printLevel = (REFER_NAMESPACE_QPOASES PrintLevel)optionValueInt;
-        if ( options->printLevel < PL_DEBUG_ITER )
-            options->printLevel = PL_DEBUG_ITER;
-        if ( options->printLevel > PL_HIGH )
-            options->printLevel = PL_HIGH;       
-        #endif
-	}
-
-	if ( hasOptionsValue( optionsPtr,"enableRamping",&optionValue ) == BT_TRUE )
-	{
-		optionValueInt = (int)*optionValue;
-		options->enableRamping = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
-	}
-
-	if ( hasOptionsValue( optionsPtr,"enableFarBounds",&optionValue ) == BT_TRUE )
-	{
-		optionValueInt = (int)*optionValue;
-		options->enableFarBounds = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
-	}
-
-	if ( hasOptionsValue( optionsPtr,"enableFlippingBounds",&optionValue ) == BT_TRUE )
-	{
-		optionValueInt = (int)*optionValue;
-		options->enableFlippingBounds = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
-	}
-
-	if ( hasOptionsValue( optionsPtr,"enableRegularisation",&optionValue ) == BT_TRUE )
-	{
-		optionValueInt = (int)*optionValue;
-		options->enableRegularisation = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
-	}
-
-	if ( hasOptionsValue( optionsPtr,"enableFullLITests",&optionValue ) == BT_TRUE )
-	{
-		optionValueInt = (int)*optionValue;
-		options->enableFullLITests = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
-	}
-
-	if ( hasOptionsValue( optionsPtr,"enableNZCTests",&optionValue ) == BT_TRUE )
-	{
-		optionValueInt = (int)*optionValue;
-		options->enableNZCTests = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
-	}
-
-	if ( hasOptionsValue( optionsPtr,"enableDriftCorrection",&optionValue ) == BT_TRUE )
-		options->enableDriftCorrection = (int)*optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"enableCholeskyRefactorisation",&optionValue ) == BT_TRUE )
-		options->enableCholeskyRefactorisation = (int)*optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"enableEqualities",&optionValue ) == BT_TRUE )
-	{
-		optionValueInt = (int)*optionValue;
-		options->enableEqualities = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
-	}
-
-
-	if ( hasOptionsValue( optionsPtr,"terminationTolerance",&optionValue ) == BT_TRUE )
-		options->terminationTolerance = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"boundTolerance",&optionValue ) == BT_TRUE )
-		options->boundTolerance = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"boundRelaxation",&optionValue ) == BT_TRUE )
-		options->boundRelaxation = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"epsNum",&optionValue ) == BT_TRUE )
-		options->epsNum = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"epsDen",&optionValue ) == BT_TRUE )
-		options->epsDen = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"maxPrimalJump",&optionValue ) == BT_TRUE )
-		options->maxPrimalJump = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"maxDualJump",&optionValue ) == BT_TRUE )
-		options->maxDualJump = *optionValue;
-
-
-	if ( hasOptionsValue( optionsPtr,"initialRamping",&optionValue ) == BT_TRUE )
-		options->initialRamping = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"finalRamping",&optionValue ) == BT_TRUE )
-		options->finalRamping = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"initialFarBounds",&optionValue ) == BT_TRUE )
-		options->initialFarBounds = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"growFarBounds",&optionValue ) == BT_TRUE )
-		options->growFarBounds = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"initialStatusBounds",&optionValue ) == BT_TRUE )
-	{
-		optionValueInt = (int)*optionValue;
-		if ( optionValueInt < -1 ) 
-			optionValueInt = -1;
-		if ( optionValueInt > 1 ) 
-			optionValueInt = 1;
-		options->initialStatusBounds = (REFER_NAMESPACE_QPOASES SubjectToStatus)optionValueInt;
-	}
-
-	if ( hasOptionsValue( optionsPtr,"epsFlipping",&optionValue ) == BT_TRUE )
-		options->epsFlipping = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"numRegularisationSteps",&optionValue ) == BT_TRUE )
-		options->numRegularisationSteps = (int)*optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"epsRegularisation",&optionValue ) == BT_TRUE )
-		options->epsRegularisation = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"numRefinementSteps",&optionValue ) == BT_TRUE )
-		options->numRefinementSteps = (int)*optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"epsIterRef",&optionValue ) == BT_TRUE )
-		options->epsIterRef = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"epsLITests",&optionValue ) == BT_TRUE )
-		options->epsLITests = *optionValue;
-
-	if ( hasOptionsValue( optionsPtr,"epsNZCTests",&optionValue ) == BT_TRUE )
-		options->epsNZCTests = *optionValue;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	s e t u p A u x i l i a r y I n p u t s
- */
-returnValue setupAuxiliaryInputs(	const mxArray* auxInput, unsigned int nV, unsigned int nC,
-									double** x0, double** guessedBoundsAndConstraints, double** guessedBounds, double** guessedConstraints
-									)
-{
-	unsigned int i;
-	mxArray* curField = 0;
-
-	/* x0 */
-	curField = mxGetField( auxInput,0,"x0" );
-	if ( curField == NULL )
-		mexWarnMsgTxt( "auxInput struct does not contain entry 'x0'!\n         Type 'help qpOASES_auxInput' for further information." );
-	else
-	{
-		*x0 = mxGetPr(curField);
-		if ( smartDimensionCheck( x0,nV,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
-			return RET_INVALID_ARGUMENTS;
-	}
-
-	/* guessedWorkingSet */
-	curField = mxGetField( auxInput,0,"guessedWorkingSet" );
-	if ( curField == NULL )
-		mexWarnMsgTxt( "auxInput struct does not contain entry 'guessedWorkingSet'!\n         Type 'help qpOASES_auxInput' for further information." );
-	else
-	{
-		*guessedBoundsAndConstraints = mxGetPr(curField);
-		if ( smartDimensionCheck( guessedBoundsAndConstraints,nV+nC,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
-			return RET_INVALID_ARGUMENTS;
-
-		if ( *guessedBoundsAndConstraints != 0 )
-		{
-			*guessedBounds = new double[nV];
-			for (i = 0; i < nV; i++)
-				(*guessedBounds)[i] = (*guessedBoundsAndConstraints)[i];
-
-			if ( nC > 0 )
-			{
-				*guessedConstraints = new double[nC];
-				for (i = 0; i < nC; i++)
-					(*guessedConstraints)[i] = (*guessedBoundsAndConstraints)[i + nV];
-			}
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	d e l e t e A u x i l i a r y I n p u t s
- */
-returnValue deleteAuxiliaryInputs(	double** guessedBounds, double** guessedConstraints
-									)
-{
-	if ( ( guessedBounds != 0 ) && ( *guessedBounds != 0 ) )
-	{
-		delete[] (*guessedBounds);
-		*guessedBounds = 0;
-	}
-
-	if ( ( guessedConstraints != 0 ) && ( guessedConstraints != 0 ) )
-	{
-		delete[] (*guessedConstraints);
-		*guessedConstraints = 0;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	a l l o c a t e O u t p u t s
- */
-returnValue allocateOutputs(	int nlhs, mxArray* plhs[], int nV, int nC = 0, int nP = 1, int handle = -1
-								)
-{
-	/* Create output vectors and assign pointers to them. */
-	int curIdx = 0;
-
-	/* handle */
-	if ( handle >= 0 )
-		plhs[curIdx++] = mxCreateDoubleMatrix( 1, 1, mxREAL );
-
-	/* x */
-	plhs[curIdx++] = mxCreateDoubleMatrix( nV, nP, mxREAL );
-
-	if ( nlhs > curIdx )
-	{
-		/* fval */
-		plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
-
-		if ( nlhs > curIdx )
-		{
-			/* exitflag */
-			plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
-
-			if ( nlhs > curIdx )
-			{
-				/* iter */
-				plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
-
-				if ( nlhs > curIdx )
-				{
-					/* lambda */
-					plhs[curIdx++] = mxCreateDoubleMatrix( nV+nC, nP, mxREAL );
-
-					if ( nlhs > curIdx )
-					{
-						/* setup auxiliary output struct */
-						mxArray* auxOutput = mxCreateStructMatrix( 1,1,0,0 );
-						int curFieldNum;
-						
-						/* working set */
-						curFieldNum = mxAddField( auxOutput,"workingSet" );
-						if ( curFieldNum >= 0 )
-							mxSetFieldByNumber( auxOutput,0,curFieldNum,mxCreateDoubleMatrix( nV+nC, nP, mxREAL ) );
-
-						curFieldNum = mxAddField( auxOutput,"cpuTime" );
-						if ( curFieldNum >= 0 )
-							mxSetFieldByNumber( auxOutput,0,curFieldNum,mxCreateDoubleMatrix( 1, nP, mxREAL ) );
-
-						plhs[curIdx] = auxOutput;
-					}
-				}
-			}
-		}
-	}
-	
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	o b t a i n O u t p u t s
- */
-returnValue obtainOutputs(	int k, QProblemB* qp, returnValue returnvalue, int _nWSRout, real_t _cpuTime,
-							int nlhs, mxArray* plhs[], int nV, int nC = 0, int handle = -1
-							)
-{
-	/* Create output vectors and assign pointers to them. */
-	int curIdx = 0;
-
-	/* handle */
-	if ( handle >= 0 )
-		plhs[curIdx++] = mxCreateDoubleScalar( handle );
-
-	/* x */
-	double* x = mxGetPr( plhs[curIdx++] );
-	qp->getPrimalSolution( &(x[k*nV]) );
-
-	if ( nlhs > curIdx )
-	{
-		/* fval */
-		double* obj = mxGetPr( plhs[curIdx++] );
-		obj[k] = qp->getObjVal( );
-
-		if ( nlhs > curIdx )
-		{
-			/* exitflag */
-			double* status = mxGetPr( plhs[curIdx++] );
-			status[k] = (real_t)getSimpleStatus( returnvalue );
-
-			if ( nlhs > curIdx )
-			{
-				/* iter */
-				double* nWSRout = mxGetPr( plhs[curIdx++] );
-				nWSRout[k] = (real_t) _nWSRout;
-
-				if ( nlhs > curIdx )
-				{
-					/* lambda */
-					double* y = mxGetPr( plhs[curIdx++] );
-					qp->getDualSolution( &(y[k*(nV+nC)]) );
-
-					if ( nlhs > curIdx )
-					{
-						
-						mxArray* auxOutput = plhs[curIdx];
-						mxArray* curField = 0;
-
-						/* working set */
-						curField = mxGetField( auxOutput,0,"workingSet" );
-						double* workingSet = mxGetPr(curField);
-
-						QProblem* problemPointer;
-						problemPointer = dynamic_cast<QProblem*>(qp);
-
-						// cast successful?
-						if (problemPointer != NULL) {
-							problemPointer->getWorkingSet( &(workingSet[k*(nV+nC)]) );
-						} else {
-							qp->getWorkingSet( &(workingSet[k*(nV+nC)]) );
-						}
-
-						/* cpu time */
-						curField = mxGetField( auxOutput,0,"cpuTime" );
-						double* cpuTime = mxGetPr(curField);
-						cpuTime[0] = (real_t) _cpuTime;
-					}
-				}
-			}
-		}
-	}
-	
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	s e t u p H e s s i a n M a t r i x
- */
-returnValue setupHessianMatrix(	const mxArray* prhsH, int nV,
-								SymmetricMatrix** H, sparse_int_t** Hir, sparse_int_t** Hjc, real_t** Hv
-								)
-{
-	if ( mxIsSparse( prhsH ) != 0 )
-	{
-		mwIndex *oct_ir = mxGetIr( prhsH );
-		mwIndex *oct_jc = mxGetJc( prhsH );
-		double *v = (double*)mxGetPr( prhsH );
-		long nfill = 0;
-		long i, j;
-		BooleanType needInsertDiag;
-
-		/* copy indices to avoid 64/32-bit integer confusion */
-		/* also add explicit zeros on diagonal for regularization strategy */
-		/* copy values, too */
-		*Hir = new sparse_int_t[oct_jc[nV] + nV];
-		*Hjc = new sparse_int_t[nV+1];
-		*Hv = new real_t[oct_jc[nV] + nV];
-        for (j = 0; j < nV; j++) 
-		{
-            needInsertDiag = BT_TRUE;
-                
-            (*Hjc)[j] = (sparse_int_t)(oct_jc[j]) + nfill;
-            /* fill up to diagonal */
-            for (i = oct_jc[j]; i < oct_jc[j+1]; i++) 
-			{
-                if ( oct_ir[i] == j )
-                    needInsertDiag = BT_FALSE;
-                    
-                /* add zero diagonal element if not present */
-                if ( ( oct_ir[i] > j ) && ( needInsertDiag == BT_TRUE ) )
-                {
-                    (*Hir)[i + nfill] = (sparse_int_t)j;
-                    (*Hv)[i + nfill] = 0.0;
-                    nfill++;
-                    /* only add diag once */
-                    needInsertDiag = BT_FALSE;
-                }
-                        
-				(*Hir)[i + nfill] = (sparse_int_t)(oct_ir[i]);
-				(*Hv)[i + nfill] = (real_t)(v[i]);
-			}
-		}
-		(*Hjc)[nV] = (sparse_int_t)(oct_jc[nV]) + nfill;
-
-		SymSparseMat *sH;
-		*H = sH = new SymSparseMat(nV, nV, *Hir, *Hjc, *Hv);
-		sH->createDiagInfo();
-	}
-	else
-	{
-		/* make a deep-copy in order to avoid modifying input data when regularising */
-		real_t* H_for = (real_t*) mxGetPr( prhsH );
-		real_t* H_mem = new real_t[nV*nV];
-		memcpy( H_mem,H_for, nV*nV*sizeof(real_t) );
-
-		*H = new SymDenseMat( nV,nV,nV, H_mem );
-		(*H)->doFreeMemory( );
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p C o n s t r a i n t M a t r i x
- */
-returnValue setupConstraintMatrix(	const mxArray* prhsA, int nV, int nC,
-									Matrix** A, sparse_int_t** Air, sparse_int_t** Ajc, real_t** Av
-									)
-{
-	if ( mxIsSparse( prhsA ) != 0 )
-	{
-		long i;
-
-		mwIndex *oct_ir = mxGetIr( prhsA );
-		mwIndex *oct_jc = mxGetJc( prhsA );
-		double *v = (double*)mxGetPr( prhsA );
-
-		/* copy indices to avoid 64/32-bit integer confusion */
-		*Air = new sparse_int_t[oct_jc[nV]];
-		*Ajc = new sparse_int_t[nV+1];
-		for (i = 0; i < oct_jc[nV]; i++)
-			(*Air)[i] = (sparse_int_t)(oct_ir[i]);
-		for (i = 0; i < nV + 1; i++)
-			(*Ajc)[i] = (sparse_int_t)(oct_jc[i]);
-
-		/* copy values, too */
-		*Av = new real_t[(*Ajc)[nV]];
-		for (i = 0; i < (*Ajc)[nV]; i++)
-			(*Av)[i] = (real_t)(v[i]);
-
-		*A = new SparseMatrix(nC, nV, *Air, *Ajc, *Av);
-	}
-	else
-	{
-		/* Convert constraint matrix A from FORTRAN to C style
-		* (not necessary for H as it should be symmetric!). */
-		real_t* A_for = (real_t*) mxGetPr( prhsA );
-		real_t* A_mem = new real_t[nC*nV];
-		convertFortranToC( A_for,nV,nC, A_mem );
-		*A = new DenseMatrix(nC, nV, nV, A_mem );
-		(*A)->doFreeMemory();
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_octave_utils.hpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_octave_utils.hpp
deleted file mode 100644
index 5430fdeac44..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_octave_utils.hpp
+++ /dev/null
@@ -1,92 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file interfaces/octave/qpOASES_octave_utils.hpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Alexander Buchner
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Collects utility functions for Interface to octave that
- *	enables to call qpOASES as a MEX function.
- *
- */
-
-
-
-/* Work-around for settings where mexErrMsgTxt causes unexpected behaviour. */
-#ifdef __AVOID_MEXERRMSGTXT__
-	#define myMexErrMsgTxt( TEXT ) mexPrintf( "%s\n\n",(TEXT) );
-#else
-	#define myMexErrMsgTxt mexErrMsgTxt
-#endif
-
-
-#include "mex.h"
-//#include "matrix.h"
-#include "string.h"
-#include <vector>
-
-
-/*
- * QProblem instance class
- */
-class QPInstance
-{
-	private:
-		static int s_nexthandle;
-
-	public:
-		QPInstance(	int _nV = 0,
-					int _nC = 0,
-					BooleanType _isSimplyBounded = BT_FALSE
-					);
-
-		~QPInstance( );
-	
-		returnValue deleteQPMatrices();
-		
-		int getNV() const;
-		int getNC() const;
-
-		int handle;
-
-		SQProblem* sqp;
-		QProblemB* qpb;
-		BooleanType isSimplyBounded;
-
-		SymmetricMatrix* H;
-		Matrix* A;
-		sparse_int_t* Hir; 
-		sparse_int_t* Hjc; 
-		sparse_int_t* Air; 
-		sparse_int_t* Ajc;
-		real_t* Hv;
-		real_t* Av;
-};
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_options.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_options.m
deleted file mode 100644
index f3cd4175129..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_options.m
+++ /dev/null
@@ -1,251 +0,0 @@
-%qpOASES -- An Implementation of the Online Active Set Strategy.
-%Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%Christian Kirches et al. All rights reserved.
-%
-%qpOASES is distributed under the terms of the
-%GNU Lesser General Public License 2.1 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.
-%
-%---------------------------------------------------------------------------------
-%
-%Returns a struct containing values for all options to be used within qpOASES.
-%
-%Call
-%    options = qpOASES_options( 'default' );
-%    options = qpOASES_options( 'reliable' );
-%    options = qpOASES_options( 'MPC' );
-%to obtain a set of default options or a pre-defined set of options tuned
-%for reliable or fast QP solution, respectively.
-%
-%Call
-%    options = qpOASES_options( 'option1',value1,'option2',value2,... )
-%to obtain a set of default options but with 'option1' set to value1 etc.
-%
-%Call
-%    options = qpOASES_options( oldopts,'option1',value1,... )
-%to obtain a copy of the options struct oldopts but with 'option1' set to 
-%value1 etc.
-%
-%Call
-%    options = qpOASES_options( 'default', 'option1',value1,... )
-%    options = qpOASES_options( 'reliable','option1',value1,... )
-%    options = qpOASES_options( 'MPC',     'option1',value1,... )
-%to obtain a set of default options or a pre-defined set of options tuned
-%for reliable or fast QP solution, respectively, but with 'option1' set to 
-%value1 etc.
-%
-%
-%qpOASES features the following options:
-%  maxIter                    -  Maximum number of iterations (if set
-%                                to -1, a value is chosen heuristically)
-%  maxCpuTime                 -  Maximum CPU time in seconds (if set
-%                                to -1, only iteration limit is used)
-%  printLevel                 -  0: no printed output,
-%                                1: only error messages are printed,
-%                                2: iterations and error messages are printed,
-%                                3: all available messages are printed.
-%
-%  enableRamping              -  Enables (1) or disables (0) ramping.
-%  enableFarBounds            -  Enables (1) or disables (0) the use of 
-%                                far bounds.
-%  enableFlippingBounds       -  Enables (1) or disables (0) the use of 
-%                                flipping bounds.
-%  enableRegularisation       -  Enables (1) or disables (0) automatic 
-%                                Hessian regularisation.
-%  enableFullLITests          -  Enables (1) or disables (0) condition-hardened 
-%                                (but more expensive) LI test.
-%  enableNZCTests             -  Enables (1) or disables (0) nonzero curvature 
-%                                tests.
-%  enableDriftCorrection      -  Specifies the frequency of drift corrections:
-%                                0: turns them off, 
-%                                1: uses them at each iteration etc.
-%  enableCholeskyRefactorisation - Specifies the frequency of a full re-
-%                                factorisation of projected Hessian matrix:
-%                                0: turns them off, 
-%                                1: uses them at each iteration etc.
-%  enableEqualities           -  Specifies whether equalities should be treated 
-%                                as always active (1) or not (0)
-%
-%  terminationTolerance       -  Relative termination tolerance to stop homotopy.
-%  boundTolerance             -  If upper and lower bounds differ less than this
-%                                tolerance, they are regarded equal, i.e. as 
-%                                equality constraint.
-%  boundRelaxation            -  Initial relaxation of bounds to start homotopy 
-%                                and initial value for far bounds.
-%  epsNum                     -  Numerator tolerance for ratio tests.
-%  epsDen                     -  Denominator tolerance for ratio tests.
-%  maxPrimalJump              -  Maximum allowed jump in primal variables in 
-%                                nonzero curvature tests.
-%  maxDualJump                -  Maximum allowed jump in dual variables in 
-%                                linear independence tests.
-%
-%  initialRamping             -  Start value for ramping strategy.
-%  finalRamping               -  Final value for ramping strategy.
-%  initialFarBounds           -  Initial size for far bounds.
-%  growFarBounds              -  Factor to grow far bounds.
-%  initialStatusBounds        -  Initial status of bounds at first iteration:
-%                                 0: all bounds inactive,
-%                                -1: all bounds active at their lower bound,
-%                                +1: all bounds active at their upper bound.
-%  epsFlipping                -  Tolerance of squared Cholesky diagonal factor 
-%                                which triggers flipping bound.
-%  numRegularisationSteps     -  Maximum number of successive regularisation steps.
-%  epsRegularisation          -  Scaling factor of identity matrix used for 
-%                                Hessian regularisation.
-%  numRefinementSteps         -  Maximum number of iterative refinement steps.
-%  epsIterRef                 -  Early termination tolerance for iterative 
-%                                refinement.
-%  epsLITests                 -  Tolerance for linear independence tests.
-%  epsNZCTests                -  Tolerance for nonzero curvature tests.
-%
-%
-%See also QPOASES, QPOASES_SEQUENCE
-%
-%
-%For additional information see the qpOASES User's Manual or
-%visit http://www.qpOASES.org/.
-%
-%Please send remarks and questions to support@qpOASES.org!
-function [ options ] = qpOASES_options( varargin )
-
-	firstIsStructOrScheme = 0;
-
-	if ( nargin == 0 ) 
-		options = qpOASES_default_options();
-	else
-		if ( isstruct( varargin{1} ) )
-			if ( mod( nargin,2 ) ~= 1 )
-				error('ERROR (qpOASES_options): Options must be specified in pairs!');
-			end
-			options = varargin{1};
-			firstIsStructOrScheme = 1;
-		else
-			if ( ischar( varargin{1} ) )
-				if ( mod( nargin,2 ) == 0 )
-					options = qpOASES_default_options();
-				else
-					if ( ( nargin > 1 ) && ( ischar( varargin{nargin} ) ) )
-						error('ERROR (qpOASES_options): Options must be specified in pairs!');
-					end
-
-					switch ( varargin{1} )
-						case 'default'
-							options = qpOASES_default_options();
-						case 'reliable'
-							options = qpOASES_reliable_options();
-						case {'MPC','mpc','fast'}
-							options = qpOASES_MPC_options();
-						otherwise
-							error( ['ERROR (qpOASES_options): Only the following option schemes are defined: ''default'', ''reliable'', ''MPC''!'] );
-							
-					end
-					firstIsStructOrScheme = 1;
-				end
-			else
-				error('ERROR (qpOASES_options): First argument needs to be a string or an options struct!');
-			end
-		end
-	end
-
-	% set options to user-defined values  
-	for i=(1+firstIsStructOrScheme):2:nargin
-
-		argName  = varargin{i};
-		argValue = varargin{i+1};
-
-		if ( ( isempty( argName ) ) || ( ~ischar( argName ) ) )
-			error('ERROR (qpOASES_options): Argmument no. %d has to be a non-empty string!',i );
-        end
-			
-		if ( ( ischar(argValue) ) || ( ~isscalar( argValue ) ) )
-			error('ERROR (qpOASES_options): Argmument no. %d has to be a numerical constant!',i+1 );
-        end
-
-		if ( ~isfield( options,argName ) )
-			error('ERROR (qpOASES_options): Argmument no. %d is an invalid option!',i );
-		end
-
-		eval( ['options.',argName,' = ',num2str(argValue),';'] );
-
-	end
-
-end
-
-
-function [ options ] = qpOASES_default_options( )
-
-	% setup options struct with default values
-	options = struct(	'maxIter',                       -1, ...
-						'maxCpuTime',                    -1, ...
-						'printLevel',                     1, ...
-						...
-						'enableRamping',                  1, ...
-						'enableFarBounds',                1, ...
-						'enableFlippingBounds',           1, ...
-						'enableRegularisation',           0, ...
-						'enableFullLITests',              0, ...
-						'enableNZCTests',                 1, ...
-						'enableDriftCorrection',          1, ...
-						'enableCholeskyRefactorisation',  0, ...
-						'enableEqualities',               0, ...
-						...
-						'terminationTolerance',           5.0e6*eps, ...
-						'boundTolerance',                 1.0e6*eps, ...
-						'boundRelaxation',                1.0e4, ...
-						'epsNum',                        -1.0e3*eps, ...
-						'epsDen',                         1.0e3*eps, ...
-						'maxPrimalJump',                  1.0e8, ...
-						'maxDualJump',                    1.0e8, ...
-						...
-    					'initialRamping',                 0.5, ...
-						'finalRamping',                   1.0, ...
-						'initialFarBounds',               1.0e6, ...
-						'growFarBounds',                  1.0e3, ...
-						'initialStatusBounds',            -1, ...
-						'epsFlipping',                    1.0e3*eps, ...
-						'numRegularisationSteps',         0, ...
-						'epsRegularisation',              1.0e3*eps, ...
-						'numRefinementSteps',             1, ...
-						'epsIterRef',                     1.0e2*eps, ...
-						'epsLITests',                     1.0e5*eps, ...
-						'epsNZCTests',                    3.0e3*eps );
-
-end
-
-
-
-function [ options ] = qpOASES_reliable_options( )
-
-	% setup options struct with values for most reliable QP solution
-	options = qpOASES_default_options( );
-
-	options.enableFullLITests             =  1;
-	options.enableCholeskyRefactorisation =  1;
-
-	options.numRefinementSteps            =  2;
-
-end
-
-
-function [ options ] = qpOASES_MPC_options( )
-
-	% setup options struct with values for most reliable QP solution
-	options = qpOASES_default_options( );
-
-	options.enableRamping                 =  0;
-	options.enableFarBounds               =  1;
-	options.enableFlippingBounds          =  0;
-	options.enableRegularisation          =  1;
-	options.enableNZCTests                =  0;
-	options.enableDriftCorrection         =  0;
-	options.enableEqualities              =  1;
-
-	options.terminationTolerance          =  1.0e9*eps;
-	
-	options.initialStatusBounds           =  0;
-	options.numRegularisationSteps        =  1;
-	options.numRefinementSteps            =  0;
-
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_sequence.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_sequence.cpp
deleted file mode 100644
index 35ea59741be..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_sequence.cpp
+++ /dev/null
@@ -1,1061 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file interfaces/octave/qpOASES_sequence.cpp
- *	\author Hans Joachim Ferreau, Christian Kirches, Andreas Potschka, Alexander Buchner
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Interface for octave that enables to call qpOASES as a MEX function
- *  (variant for solving QP sequences).
- *
- */
-
-
-
-#include <qpOASES.hpp>
-
-
-USING_NAMESPACE_QPOASES
-
-
-#include "qpOASES_octave_utils.hpp"
-
-/** initialise handle counter of QPInstance class */
-int QPInstance::s_nexthandle = 1;
-
-/** global pointer to QP objects */
-static std::vector<QPInstance *> g_instances;
-
-#include "qpOASES_octave_utils.cpp"
-
-
-/*
- *	i n i t S B
- */
-int initSB(	int handle, 
-			SymmetricMatrix *H, real_t* g,
-			const real_t* const lb, const real_t* const ub,
-			int nWSRin, real_t maxCpuTimeIn,
-			const real_t* const x0, Options* options,
-			int nOutputs, mxArray* plhs[],
-			double* guessedBounds
-			)
-{
-	int nWSRout = nWSRin;
-	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-
-	/* 1) setup initial QP. */
-	QProblemB* globalQPB = getQPInstance(handle)->qpb;
-
-	if ( globalQPB == 0 )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
-		return -1;
-	}
-
-	globalQPB->setOptions( *options );
-	
-	/* 2) Solve initial QP. */
-	returnValue returnvalue;
-	int nV = globalQPB->getNV();
-	
-	/* 3) Fill the working set. */
-	Bounds bounds(nV);
-	if (guessedBounds != 0) {
-		for (int i = 0; i < nV; i++) {
-			if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_LOWER);
-			} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_UPPER);
-			} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_INACTIVE);
-			} else {
-				char msg[MAX_STRING_LENGTH];
-				snprintf(msg, MAX_STRING_LENGTH,
-						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
-				myMexErrMsgTxt(msg);
-				return -1;
-			}
-		}
-	}
-
-	if (x0 == 0 && guessedBounds == 0)
-		returnvalue = globalQPB->init( H,g,lb,ub, nWSRout,&maxCpuTimeOut );
-	else
-		returnvalue = globalQPB->init( H,g,lb,ub, nWSRout,&maxCpuTimeOut, x0, 0,
-				guessedBounds != 0 ? &bounds : 0);
-
-	/* 3) Assign lhs arguments. */
-	obtainOutputs(	0,globalQPB,returnvalue,nWSRout,maxCpuTimeOut,
-					nOutputs,plhs,nV,0,handle );
-
-	return 0;
-}
-
-
-/*
- *	i n i t
- */
-int init(	int handle, 
-			SymmetricMatrix *H, real_t* g, Matrix *A,
-			const real_t* const lb, const real_t* const ub, const real_t* const lbA, const real_t* const ubA,
-			int nWSRin, real_t maxCpuTimeIn,
-			const real_t* const x0, Options* options,
-			int nOutputs, mxArray* plhs[],
-			double* guessedBounds, double* guessedConstraints
-			)
-{
-	int nWSRout = nWSRin;
-	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-
-	/* 1) setup initial QP. */
-	SQProblem* globalSQP = getQPInstance(handle)->sqp;
-
-	if ( globalSQP == 0 )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
-		return -1;
-	}
-
-	globalSQP->setOptions( *options );
-	
-	/* 2) Solve initial QP. */
-	returnValue returnvalue;
-	int nV = globalSQP->getNV();
-	int nC = globalSQP->getNC();
-	
-	/* 3) Fill the working set. */
-	Bounds bounds(nV);
-	Constraints constraints(nC);
-	if (guessedBounds != 0) {
-		for (int i = 0; i < nV; i++) {
-			if ( isEqual(guessedBounds[i],-1.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_LOWER);
-			} else if ( isEqual(guessedBounds[i],1.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_UPPER);
-			} else if ( isEqual(guessedBounds[i],0.0) == BT_TRUE ) {
-				bounds.setupBound(i, ST_INACTIVE);
-			} else {
-				char msg[MAX_STRING_LENGTH];
-				snprintf(msg, MAX_STRING_LENGTH,
-						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of bounds!");
-				myMexErrMsgTxt(msg);
-				return -1;
-			}
-		}
-	}
-
-	if (guessedConstraints != 0) {
-		for (int i = 0; i < nC; i++) {
-			if ( isEqual(guessedConstraints[i],-1.0) == BT_TRUE ) {
-				constraints.setupConstraint(i, ST_LOWER);
-			} else if ( isEqual(guessedConstraints[i],1.0) == BT_TRUE ) {
-				constraints.setupConstraint(i, ST_UPPER);
-			} else if ( isEqual(guessedConstraints[i],0.0) == BT_TRUE ) {
-				constraints.setupConstraint(i, ST_INACTIVE);
-			} else {
-				char msg[MAX_STRING_LENGTH];
-				snprintf(msg, MAX_STRING_LENGTH,
-						"ERROR (qpOASES): Only {-1, 0, 1} allowed for status of constraints!");
-				myMexErrMsgTxt(msg);
-				return -1;
-			}
-		}
-	}
-
-	if (x0 == 0 && guessedBounds == 0 && guessedConstraints == 0)
-		returnvalue = globalSQP->init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut);
-	else
-		returnvalue = globalSQP->init( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut, x0, 0,
-				guessedBounds != 0 ? &bounds : 0,
-				guessedConstraints != 0 ? &constraints : 0);
-
-	/* 3) Assign lhs arguments. */
-	obtainOutputs(	0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
-					nOutputs,plhs,nV,nC,handle );
-
-	return 0;
-}
-
-
-
-/*
- *	h o t s t a r t S B
- */
-int hotstartSB(	int handle,
-                const real_t* const g,
-				const real_t* const lb, const real_t* const ub,
-				int nWSRin, real_t maxCpuTimeIn,
-				Options* options,
-				int nOutputs, mxArray* plhs[]
-				)
-{
-	int nWSRout = nWSRin;
-	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-
-	QProblemB* globalQPB = getQPInstance(handle)->qpb;
-
-	if ( globalQPB == 0 )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
-		return -1;
-	}
-
-	/* 1) Solve QP with given options. */
-	globalQPB->setOptions( *options );
-	returnValue returnvalue = globalQPB->hotstart( g,lb,ub, nWSRout,&maxCpuTimeOut );
-
-	/* 2) Assign lhs arguments. */
-	obtainOutputs(	0,globalQPB,returnvalue,nWSRout,maxCpuTimeOut,
-					nOutputs,plhs,0,0 );
-
-	return 0;
-}
-
-
-/*
- *	h o t s t a r t
- */
-int hotstart(	int handle,
-                const real_t* const g,
-				const real_t* const lb, const real_t* const ub,
-				const real_t* const lbA, const real_t* const ubA,
-				int nWSRin, real_t maxCpuTimeIn,
-				Options* options,
-				int nOutputs, mxArray* plhs[]
-				)
-{
-	int nWSRout = nWSRin;
-	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-
-	QProblem* globalSQP = getQPInstance(handle)->sqp;
-
-	if ( globalSQP == 0 )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
-		return -1;
-	}
-
-	/* 1) Solve QP with given options. */
-	globalSQP->setOptions( *options );
-	returnValue returnvalue = globalSQP->hotstart( g,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut );
-
-	/* 2) Assign lhs arguments. */
-	obtainOutputs(	0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
-					nOutputs,plhs,0,0 );
-
-	return 0;
-}
-
-
-/*
- *	h o t s t a r t V M
- */
-int hotstartVM(	int handle,
-                    SymmetricMatrix *H, real_t* g, Matrix *A,
-					const real_t* const lb, const real_t* const ub, const real_t* const lbA, const real_t* const ubA,
-					int nWSRin, real_t maxCpuTimeIn,
-					Options* options,
-					int nOutputs, mxArray* plhs[]
-					)
-{
-	int nWSRout = nWSRin;
-	real_t maxCpuTimeOut = (maxCpuTimeIn >= 0.0) ? maxCpuTimeIn : INFTY;
-
-	SQProblem* globalSQP = getQPInstance(handle)->sqp;
-
-	if ( globalSQP == 0 )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): QP needs to be initialised first!" );
-		return -1;
-	}
-
-	/* 1) Solve QP. */
-	globalSQP->setOptions( *options );
-	returnValue returnvalue = globalSQP->hotstart( H,g,A,lb,ub,lbA,ubA, nWSRout,&maxCpuTimeOut );
-
-	switch (returnvalue)
-	{
-		case SUCCESSFUL_RETURN:
-		case RET_QP_UNBOUNDED:
-		case RET_QP_INFEASIBLE:
-			break;
-		otherwise:
-			myMexErrMsgTxt( "ERROR (qpOASES): Hotstart failed." );
-			return -1;
-	}
-
-	/* 2) Assign lhs arguments. */
-	obtainOutputs(	0,globalSQP,returnvalue,nWSRout,maxCpuTimeOut,
-					nOutputs,plhs,0,0 );
-
-	return 0;
-}
-
-
-
-/*
- *	m e x F u n c t i o n
- */
-void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
-{
-	/* inputs */
-	char typeString[2];
-	real_t *g=0, *lb=0, *ub=0, *lbA=0, *ubA=0, *x0=0;
-	int H_idx=-1, g_idx=-1, A_idx=-1, lb_idx=-1, ub_idx=-1, lbA_idx=-1, ubA_idx=-1;
-	int x0_idx=-1, auxInput_idx=-1;
-
-	double *guessedBoundsAndConstraints = 0;
-	double *guessedBounds = 0, *guessedConstraints = 0;
-
-	BooleanType isSimplyBoundedQp = BT_FALSE;
-
-	Options options;
-	options.printLevel = PL_LOW;
-	#ifdef __DEBUG__
-	options.printLevel = PL_HIGH;
-	#endif
-	#ifdef __SUPPRESSANYOUTPUT__
-	options.printLevel = PL_NONE;
-	#endif
-
-	/* dimensions */
-	unsigned int nV=0, nC=0, handle=0;
-	int nWSRin;
-	real_t maxCpuTimeIn = -1.0;
-	QPInstance* globalQP = 0;
-
-	/* I) CONSISTENCY CHECKS: */
-	/* 1) Ensure that qpOASES is called with a feasible number of input arguments. */
-	if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
-	{
-		if ( nrhs != 2 )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-	}
-	
-	/* 2) Ensure that first input is a string ... */
-	if ( mxIsChar( prhs[0] ) != 1 )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): First input argument must be a string!" );
-		return;
-	}
-
-	mxGetString( prhs[0], typeString, 2 );
-
-	/*    ... and if so, check if it is an allowed one. */
-	if ( ( strcmp( typeString,"i" ) != 0 ) && ( strcmp( typeString,"I" ) != 0 ) &&
-		 ( strcmp( typeString,"h" ) != 0 ) && ( strcmp( typeString,"H" ) != 0 ) &&
-		 ( strcmp( typeString,"m" ) != 0 ) && ( strcmp( typeString,"M" ) != 0 ) &&
-		 ( strcmp( typeString,"e" ) != 0 ) && ( strcmp( typeString,"E" ) != 0 ) &&
-		 ( strcmp( typeString,"c" ) != 0 ) && ( strcmp( typeString,"C" ) != 0 ) )
-	{
-		myMexErrMsgTxt( "ERROR (qpOASES): Undefined first input argument!\nType 'help qpOASES_sequence' for further information." );
-		return;
-	}
-
-
-	/* II) SELECT RESPECTIVE QPOASES FUNCTION CALL: */
-	/* 1) Init (without or with initial guess for primal solution). */
-	if ( ( strcmp( typeString,"i" ) == 0 ) || ( strcmp( typeString,"I" ) == 0 ) )
-	{
-		/* consistency checks */
-		if ( ( nlhs < 2 ) || ( nlhs > 7 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		/* ensure that data is given in double precision */
-		if ((mxIsDouble(prhs[1]) == 0) || (mxIsDouble(prhs[2]) == 0)) {
-			myMexErrMsgTxt(
-					"ERROR (qpOASES): All data has to be provided in double precision!");
-			return;
-		}
-
-        nV = (unsigned int)mxGetM( prhs[1] ); /* row number of Hessian matrix */
-
-		/* Check for 'Inf' and 'Nan' in Hessian */
-		H_idx = 1;
-		g_idx = 2;
-		if (containsNaNorInf(prhs, nV * nV, H_idx, 0) == BT_TRUE) {
-			return;
-		}
-
-		/* Check for 'Inf' and 'Nan' in gradient */
-		if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
-			return;
-		}
-
-		/* determine whether is it a simply bounded QP */
-		if ( nrhs <= 7 )
-			isSimplyBoundedQp = BT_TRUE;
-		else
-			isSimplyBoundedQp = BT_FALSE;
-
-		if ( isSimplyBoundedQp == BT_TRUE )
-		{
-			lb_idx = 3;
-			ub_idx = 4;
-
-			if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
-				return;
-			}
-			if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
-				return;
-			}
-
-			/* Check inputs dimensions and assign pointers to inputs. */
-			nC = 0; /* row number of constraint matrix */
-
-			if ( mxGetN( prhs[1] ) != nV )
-			{
-				myMexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );
-				return;
-			}
-
-			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
-				return;
-
-			/* default value for nWSR */
-			nWSRin = 5*nV;
-
-			/* Check whether x0 and options are specified .*/
-			if ( nrhs >= 6 )
-			{
-				if ((!mxIsEmpty(prhs[5])) && (mxIsStruct(prhs[5])))
-					setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );
-
-				if ( ( nrhs >= 7 ) && ( !mxIsEmpty(prhs[6]) ) )
-				{ 
-					/* auxInput specified */
-					if ( mxIsStruct(prhs[6]) )
-					{
-						auxInput_idx = 6;
-						x0_idx = -1;
-					}
-					else
-					{
-						auxInput_idx = -1;
-						x0_idx = 6;
-					}
-				}
-				else
-				{
-					auxInput_idx = -1;
-					x0_idx = -1;
-				}
-			}
-		}
-		else
-		{
-			/* ensure that data is given in double precision */
-			if ( ( mxIsDouble( prhs[1] ) == 0 ) ||
-				 ( mxIsDouble( prhs[2] ) == 0 ) ||
-				 ( mxIsDouble( prhs[3] ) == 0 ) )
-			{
-				myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
-				return;
-			}
-		
-			/* Check inputs dimensions and assign pointers to inputs. */
-			nC = (unsigned int)mxGetM( prhs[3] ); /* row number of constraint matrix */
-
-			A_idx = 3;
-			lb_idx = 4;
-			ub_idx = 5;
-			lbA_idx = 6;
-			ubA_idx = 7;
-
-			if (containsNaNorInf(prhs, nV * nC, A_idx, 0) == BT_TRUE) {
-				return;
-			}
-			if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
-				return;
-			}
-			if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
-				return;
-			}
-			if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
-				return;
-			}
-			if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
-				return;
-			}
-
-			if ( ( mxGetN( prhs[1] ) != nV ) || ( ( mxGetN( prhs[3] ) != 0 ) && ( mxGetN( prhs[3] ) != nV ) ) )
-			{
-				myMexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );
-				return;
-			}
-		
-			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
-				return;
-			
-			if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,7 ) != SUCCESSFUL_RETURN )
-				return;
-
-			/* default value for nWSR */
-			nWSRin = 5*(nV+nC);
-
-			/* Check whether x0 and options are specified .*/
-			if ( nrhs >= 9 )
-			{
-				if ((!mxIsEmpty(prhs[8])) && (mxIsStruct(prhs[8])))
-					setupOptions( &options,prhs[8],nWSRin,maxCpuTimeIn );
-
-				if ( ( nrhs >= 10 ) && ( !mxIsEmpty(prhs[9]) ) )
-				{ 
-					/* auxInput specified */
-					if ( mxIsStruct(prhs[9]) )
-					{
-						auxInput_idx = 9;
-						x0_idx = -1;
-					}
-					else
-					{
-						auxInput_idx = -1;
-						x0_idx = 9;
-					}
-				}
-				else
-				{
-					auxInput_idx = -1;
-					x0_idx = -1;
-				}
-			}
-		}
-
-
-		/* check dimensions and copy auxInputs */
-		if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,x0_idx ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( auxInput_idx >= 0 )
-			setupAuxiliaryInputs( prhs[auxInput_idx],nV,nC, &x0,&guessedBoundsAndConstraints,&guessedBounds,&guessedConstraints );
-
-
-		/* allocate instance */
-		handle = allocateQPInstance( nV,nC,isSimplyBoundedQp, &options );	
-		globalQP = getQPInstance( handle );
-
-		/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
-		setupHessianMatrix(	prhs[1],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );
-
-		/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
-		if ( nC > 0 )
-			setupConstraintMatrix( prhs[3],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );
-
-		/* Create output vectors and assign pointers to them. */
-		allocateOutputs( nlhs,plhs, nV,nC,1,handle );
-
-		/* Call qpOASES. */
-		if ( isSimplyBoundedQp == BT_TRUE )
-		{
-			initSB(	handle,
-					globalQP->H,g,
-					lb,ub,
-					nWSRin,maxCpuTimeIn,
-					x0,&options,
-					nlhs,plhs,
-					guessedBounds
-					);
-
-			deleteAuxiliaryInputs( &guessedBounds,0 );
-		}
-		else
-		{
-			init(	handle,
-					globalQP->H,g,globalQP->A,
-					lb,ub,lbA,ubA,
-					nWSRin,maxCpuTimeIn,
-					x0,&options,
-					nlhs,plhs,
-					guessedBounds, guessedConstraints
-					);
-
-			deleteAuxiliaryInputs( &guessedBounds,&guessedConstraints );
-		}
-
-		return;
-	}
-
-	/* 2) Hotstart. */
-	if ( ( strcmp( typeString,"h" ) == 0 ) || ( strcmp( typeString,"H" ) == 0 ) )
-	{
-		/* consistency checks */
-		if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		if ( ( nrhs < 5 ) || ( nrhs > 8 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		/* determine whether is it a simply bounded QP */
-		if ( nrhs < 7 )
-			isSimplyBoundedQp = BT_TRUE;
-		else
-			isSimplyBoundedQp = BT_FALSE;
-
-
-		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxGetM( prhs[1] ) != 1 ) || ( mxGetN( prhs[1] ) != 1 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		/* get QP instance */
-		handle = (unsigned int)mxGetScalar( prhs[1] );
-		globalQP = getQPInstance ( handle );
-		if ( globalQP == 0 )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
-			return;
-		}
-
-		nV = globalQP->getNV();
-
-		g_idx = 2;
-		lb_idx = 3;
-		ub_idx = 4;
-
-		if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
-			return;
-		}
-
-		if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
-			return;
-		}
-		if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
-			return;
-		}
-
-
-		/* Check inputs dimensions and assign pointers to inputs. */
-		if ( isSimplyBoundedQp == BT_TRUE )
-		{
-			nC = 0;
-
-			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
-				return;
-
-			/* default value for nWSR */
-			nWSRin = 5*nV;
-
-			/* Check whether options are specified .*/
-			if ( nrhs == 6 )
-				if ( ( !mxIsEmpty( prhs[5] ) ) && ( mxIsStruct( prhs[5] ) ) )
-					setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );
-		}
-		else
-		{
-			nC = globalQP->getNC( );
-
-			lbA_idx = 5;
-			ubA_idx = 6;
-
-			if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
-				return;
-			}
-			if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
-				return;
-			}
-
-			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
-				return;
-
-			if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
-				return;
-
-			/* default value for nWSR */
-			nWSRin = 5*(nV+nC);
-
-			/* Check whether options are specified .*/
-			if ( nrhs == 8 )
-				if ( ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
-					setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
-		}
-
-		/* Create output vectors and assign pointers to them. */
-		allocateOutputs( nlhs,plhs, nV,nC );
-
-		/* call qpOASES */
-		if ( isSimplyBoundedQp == BT_TRUE )
-		{
-			hotstartSB(	handle, g,
-						lb,ub,
-						nWSRin,maxCpuTimeIn,
-						&options,
-						nlhs,plhs
-						);
-		}
-		else
-		{
-			hotstart(	handle, g,
-						lb,ub,lbA,ubA,
-						nWSRin,maxCpuTimeIn,
-						&options,
-						nlhs,plhs
-						);
-		}
-
-		return;
-	}
-
-	/* 3) Modify matrices. */
-	if ( ( strcmp( typeString,"m" ) == 0 ) || ( strcmp( typeString,"M" ) == 0 ) )
-	{
-		/* consistency checks */
-		if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		if ( ( nrhs < 9 ) || ( nrhs > 10 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxGetM( prhs[1] ) != 1 ) || ( mxGetN( prhs[1] ) != 1 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		/* ensure that data is given in double precision */
-		if ( ( mxIsDouble( prhs[2] ) == 0 ) ||
-			 ( mxIsDouble( prhs[3] ) == 0 ) ||
-			 ( mxIsDouble( prhs[4] ) == 0 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
-			return;
-		}
-
-		/* get QP instance */
-		handle = (unsigned int)mxGetScalar( prhs[1]);
-		globalQP = getQPInstance ( handle );
-		if ( globalQP == 0 )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
-			return;
-		}
-
-		/* Check inputs dimensions and assign pointers to inputs. */
-		nV = (unsigned int)mxGetM( prhs[2] ); /* row number of Hessian matrix */
-		nC = (unsigned int)mxGetM( prhs[4] ); /* row number of constraint matrix */
-		
-		H_idx = 2;
-		g_idx = 3;
-		A_idx = 4;
-		lb_idx = 5;
-		ub_idx = 6;
-		lbA_idx = 7;
-		ubA_idx = 8;
-
-		/* check if supplied data contains 'NaN' or 'Inf' */
-		if (containsNaNorInf(prhs, nV * nV, H_idx, 0) == BT_TRUE) {
-			return;
-		}
-
-		if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
-			return;
-		}
-
-		if (containsNaNorInf(prhs, nV * nC, A_idx, 0) == BT_TRUE) {
-			return;
-		}
-
-		if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
-			return;
-		}
-		if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
-			return;
-		}
-
-		if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
-			return;
-		}
-		if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
-			return;
-		}
-
-		/* Check that dimensions are consistent with existing QP instance */
-		if (nV != (unsigned int) globalQP->getNV () || nC != (unsigned int) globalQP->getNC ())
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): QP dimensions must be constant during a sequence! Try creating a new QP instance instead." );
-			return;
-		}
-
-		if ( ( mxGetN( prhs[2] ) != nV ) || ( ( mxGetN( prhs[4] ) != 0 ) && ( mxGetN( prhs[4] ) != nV ) ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Input dimension mismatch!" );
-			return;
-		}
-
-		if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,3 ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,7 ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,8 ) != SUCCESSFUL_RETURN )
-			return;
-
-		/* default value for nWSR */
-		nWSRin = 5*(nV+nC);
-
-		/* Check whether options are specified .*/
-		if ( nrhs > 9 )
-			if ( ( !mxIsEmpty( prhs[9] ) ) && ( mxIsStruct( prhs[9] ) ) )
-				setupOptions( &options,prhs[9],nWSRin,maxCpuTimeIn );
-
-		globalQP->deleteQPMatrices( );
-
-		/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
-		setupHessianMatrix(	prhs[2],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );
-
-		/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
-		if ( nC > 0 )
-			setupConstraintMatrix( prhs[4],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );
-
-		/* Create output vectors and assign pointers to them. */
-		allocateOutputs( nlhs,plhs, nV,nC );
-
-		/* Call qpOASES */
-		hotstartVM(	handle, globalQP->H,g,globalQP->A,
-					lb,ub,lbA,ubA,
-					nWSRin,maxCpuTimeIn,
-					&options,
-					nlhs,plhs
-					);
-
-		return;
-	}
-
-	/* 4) Solve current equality constrained QP. */
-	if ( ( strcmp( typeString,"e" ) == 0 ) || ( strcmp( typeString,"E" ) == 0 ) )
-	{
-		/* consistency checks */
-		if ( ( nlhs < 1 ) || ( nlhs > 3 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		if ( ( nrhs < 7 ) || ( nrhs > 8 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxGetM( prhs[1] ) != 1 ) || ( mxGetN( prhs[1] ) != 1 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		/* get QP instance */
-		handle = (unsigned int)mxGetScalar( prhs[1] );
-		globalQP = getQPInstance ( handle );
-		if ( globalQP == 0 )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
-			return;
-		}
-
-		/* Check inputs dimensions and assign pointers to inputs. */
-		int nRHS = (int)mxGetN(prhs[2]);
-		nV = globalQP->getNV( );
-		nC = globalQP->getNC( );
-		real_t *x_out, *y_out;
-
-		g_idx = 2;
-		lb_idx = 3;
-		ub_idx = 4;
-		lbA_idx = 5;
-		ubA_idx = 6;
-
-		/* check if supplied data contains 'NaN' or 'Inf' */
-		if (containsNaNorInf(prhs, nV, g_idx, 0) == BT_TRUE) {
-			return;
-		}
-		if (containsNaNorInf(prhs, nV, lb_idx, 1) == BT_TRUE) {
-			return;
-		}
-		if (containsNaNorInf(prhs, nV, ub_idx, 1) == BT_TRUE) {
-			return;
-		}
-
-		if (containsNaNorInf(prhs, nC, lbA_idx, 1) == BT_TRUE) {
-			return;
-		}
-		if (containsNaNorInf(prhs, nC, ubA_idx, 1) == BT_TRUE) {
-			return;
-		}
-
-		if ( smartDimensionCheck( &g,nV,nRHS, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &lb,nV,nRHS, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &ub,nV,nRHS, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &lbA,nC,nRHS, BT_TRUE,prhs,5 ) != SUCCESSFUL_RETURN )
-			return;
-
-		if ( smartDimensionCheck( &ubA,nC,nRHS, BT_TRUE,prhs,6 ) != SUCCESSFUL_RETURN )
-			return;
-
-		/* Check whether options are specified .*/
-		if ( ( nrhs == 8 ) && ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
-		{
-			nWSRin = 5*(nV+nC);
-			setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
-			globalQP->sqp->setOptions( options );
-		}
-
-		/* Create output vectors and assign pointers to them. */
-		plhs[0] = mxCreateDoubleMatrix( nV, nRHS, mxREAL );
-		x_out = mxGetPr(plhs[0]);
-		if (nlhs >= 2)
-		{
-			plhs[1] = mxCreateDoubleMatrix( nV+nC, nRHS, mxREAL );
-			y_out = mxGetPr(plhs[1]);
-
-			if (nlhs >= 3) {
-				plhs[2] = mxCreateDoubleMatrix( nV+nC, nRHS, mxREAL );
-				double* workingSet = mxGetPr(plhs[2]);
-
-				globalQP->sqp->getWorkingSet(workingSet);
-			}
-		}
-		else
-			y_out = new real_t[nV+nC];
-
-		/* Solve equality constrained QP */
-		returnValue returnvalue = globalQP->sqp->solveCurrentEQP( nRHS,g,lb,ub,lbA,ubA, x_out,y_out );
-
-		if (nlhs < 2)
-			delete[] y_out;
-
-		if (returnvalue != SUCCESSFUL_RETURN)
-		{
-			char msg[MAX_STRING_LENGTH];
-			snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Couldn't solve current EQP (code %d)!", returnvalue);
-			myMexErrMsgTxt(msg);
-			return;
-		}
-
-		return;
-	}
-
-	/* 5) Cleanup. */
-	if ( ( strcmp( typeString,"c" ) == 0 ) || ( strcmp( typeString,"C" ) == 0 ) )
-	{		
-		/* consistency checks */
-		if ( nlhs != 0 )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		if ( nrhs != 2 )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxGetM( prhs[1] ) != 1 ) || ( mxGetN( prhs[1] ) != 1 ) )
-		{
-			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
-			return;
-		}
-
-		/* Cleanup SQProblem instance. */
-		handle =(unsigned int)mxGetScalar( prhs[1] );
-		deleteQPInstance ( handle );
-		
-		return;
-	}
-
-}
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_sequence.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_sequence.m
deleted file mode 100644
index 6dc57dc767f..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/octave/qpOASES_sequence.m
+++ /dev/null
@@ -1,112 +0,0 @@
-%qpOASES -- An Implementation of the Online Active Set Strategy.
-%Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%Christian Kirches et al. All rights reserved.
-%
-%qpOASES is distributed under the terms of the
-%GNU Lesser General Public License 2.1 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.
-%
-%---------------------------------------------------------------------------------
-%
-%qpOASES_sequence is intended to solve a sequence of quadratic
-%programming (QP) problems of the following form:
-%
-%                min   1/2*x'Hx + x'g
-%                s.t.  lb  <=  x <= ub
-%                      lbA <= Ax <= ubA  {optional}
-%
-%I) Call
-%
-%    [QP,x,fval,exitflag,iter,lambda,auxOutput] = ...
-%               qpOASES_sequence( 'i',H,g,A,lb,ub,lbA,ubA{,options{,auxInput} )
-%or 
-%    [QP,x,fval,exitflag,iter,lambda,auxOutput] = ...
-%               qpOASES_sequence( 'i',H,g,lb,ub{,options{,auxInput} )
-%
-%for initialising and solving the first above-mentioned QP of the sequence
-%starting from an initial guess x0. H must be a symmetric (possibly indefinite) 
-%matrix and all vectors g, lb, ub, lbA, ubA have to be given as column vectors. 
-%Options can be generated using the qpOASES_options command, otherwise default
-%values are used. Optionally, further auxiliary inputs may be generated 
-%using qpOASES_auxInput command and passed to the solver.
-%Both matrices H or A may be passed in sparse matrix format.
-%
-%II) Call
-%
-%     [x,fval,exitflag,iter,lambda,auxOutput] = ...
-%                      qpOASES_sequence( 'h',QP,g,lb,ub,lbA,ubA{,options} )
-%or
-%     [x,fval,exitflag,iter,lambda,auxOutput] = ...
-%                      qpOASES_sequence( 'h',QP,g,lb,ub{,options} )
-%
-%for hotstarting from the previous QP solution to the one of the next QP
-%given by the vectors g, lb, ub, lbA, ubA. Options can be generated using the 
-%qpOASES_options command, otherwise default values are used.
-%
-%III) Call
-%
-%     [x,fval,exitflag,iter,lambda,auxOutput] = ...
-%                     qpOASES_sequence( 'm',QP,H,g,A,lb,ub,lbA,ubA{,options} )
-%
-%for hotstarting from the previous QP solution to the one of the next QP
-%given by the matrices H, A and the vectors g, lb, ub, lbA, ubA. The previous
-%active set serves as a starting guess. If the new projected Hessian matrix
-%turns out to be not positive definite, qpOASES recedes to a safe initial active
-%set guess automatically. This can result in a high number of iterations iter.
-%Options can be generated using the qpOASES_options command, otherwise default
-%values are used.
-%
-%IV) Call
-%
-%     [x,lambda,workingSet] = qpOASES_sequence( 'e',QP,g,lb,ub,lbA,ubA{,options} )
-%
-%for solving the equality constrained QP with constraints determined by the
-%current active set. All inequalities and bounds which were not active in the
-%previous solution might be violated. This command does not alter the internal
-%state of qpOASES. Instead of calling this command multiple times, it is
-%possible to supply several columns simultaneously in g, lb, ub, lbA, and ubA.
-%Options can be generated using the qpOASES_options command, otherwise default
-%values are used.
-%
-%V) Having solved the last QP of your sequence, call
-%
-%     qpOASES_sequence( 'c',QP )
-%
-%in order to cleanup the internal memory.
-%
-%
-%Optional outputs (only x is mandatory):
-%    x          -  Optimal primal solution vector (if exitflag==0).
-%    fval       -  Optimal objective function value (if exitflag==0).
-%    exitflag   -   0: QP solved,
-%                   1: QP could not be solved within given number of iterations,
-%                  -1: QP could not be solved due to an internal error,
-%                  -2: QP is infeasible (and thus could not be solved),
-%                  -3: QP is unbounded (and thus could not be solved).
-%    iter       -  Number of active set iterations actually performed.
-%    lambda     -  Optimal dual solution vector (if status==0).
-%    auxOutput  -  Struct containing auxiliary outputs as described below.
-%
-%The auxOutput struct contains the following entries:
-%    workingSet -  The working set at point x. The working set is a subset
-%                  of the active set (which is the set of all indices
-%                  corresponding to bounds/constraints that hold with 
-%                  equality). The working set corresponds to bound/
-%                  constraint row vectors forming a linear independent set.
-%                  The first nV elements correspond to the bounds, the last
-%                  nC elements to the constraints.
-%                  The working set is encoded as follows:
-%                   1: bound/constraint at its upper bound
-%                   0: bound/constraint not at any bound
-%                  -1: bound/constraint at its lower bound
-%    cpuTime    -  Internally measured CPU time for solving QP.
-%
-%See also QPOASES_OPTIONS, QPOASES_AUXINPUT, QPOASES
-%
-%
-%For additional information see the qpOASES User's Manual or
-%visit http://www.qpOASES.org/.
-%
-%Please send remarks and questions to support@qpOASES.org!
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/README.rst b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/README.rst
deleted file mode 100644
index 64911d9f4cb..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/README.rst
+++ /dev/null
@@ -1,68 +0,0 @@
-pyqpOASES: a Python interface to qpOASES
-
-:Author: Sebastian F. Walter, Manuel Kudruss
-
-
-Installation
-------------
-
-
-Requirements:
-
-  You'll need numpy and cython. Install for instance with::
-
-      sudo pip install cython
-      sudo pip install numpy
-
-Method 1:
-
-  This is a local installation and creates `./interfaces/python/qpoases.so`::
-
-    make python
-
-  Then, you'll have to update your PYTHONPATH, e.g., on LINUX you have to add::
-
-    export PYTHONPATH=$PYTHONPATH:/home/swalter/projects/qpOASES/interfaces/python
-
-  to your ``~/.bashrc``.
-
-Method 2:
-
-  global installation::
-
-    sudo make pythoninstall
-
-Method 3::
-
-   cd ./interfaces/python/
-   python setup.py build_ext --inplace
-   # or python setup.py install
-
-
-Testing your installation
--------------------------
-
-For a quick test run::
-
-  cd ./interfaces/python
-  python example1.py
-
-
-To run a complete unit test you need ``nose``. Install for instance with::
-
-    sudo pip install nose
-
-Then::
-
-   cd ./interfaces/python/
-   nosestests ./tests
-
-The results of the tests can be found in `./interfaces/python/tests/results`.
-
-Tested setups
--------------
-
-The Python interface is known to work on
-
-* Windows, Python 3
-* Linux (Ubuntu 12.04) using Python 2.7.3, Python 3.2.3. NumPy 1.8, Cython 0.19
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/example1.pyx b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/example1.pyx
deleted file mode 100644
index 1cdfbc8f5b9..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/example1.pyx
+++ /dev/null
@@ -1,73 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-## Example adapted from examples/example1.cpp.
-## author of this file: Sebastian F. Walter
-
-import numpy as np
-from qpoases import PyQProblem as QProblem
-from qpoases import PyPrintLevel as PrintLevel
-from qpoases import PyOptions as Options
-cimport numpy as np
-
-def run():
-
-    #Setup data of QP.
-
-    cdef np.ndarray[np.double_t, ndim=2] H
-    cdef np.ndarray[np.double_t, ndim=2] A
-    cdef np.ndarray[np.double_t, ndim=1] g
-    cdef np.ndarray[np.double_t, ndim=1] lb
-    cdef np.ndarray[np.double_t, ndim=1] ub
-    cdef np.ndarray[np.double_t, ndim=1] lbA
-    cdef np.ndarray[np.double_t, ndim=1] ubA
-
-    H   = np.array([1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
-    A   = np.array([1.0, 1.0 ]).reshape((2,1))
-    g   = np.array([1.5, 1.0 ])
-    lb  = np.array([0.5, -2.0])
-    ub  = np.array([5.0, 2.0 ])
-    lbA = np.array([-1.0 ])
-    ubA = np.array([2.0])
-
-    # Setting up QProblem object.
-
-    cdef example = QProblem(2, 1)
-    cdef options = Options()
-    options.printLevel = PrintLevel.NONE
-    example.setOptions(options)
-
-    # Solve first QP.
-
-    cdef int nWSR = 10
-    example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
-
-    # Solve subsequent QPs
-
-    cdef int i,j
-    for i in range(100000):
-        for j in range(1, 100):
-            g[0] = i%j
-        example.hotstart(g, lb, ub, lbA, ubA, nWSR)
-
-run()
-
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/index.html
deleted file mode 100644
index 8054f6c1c37..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/index.html
+++ /dev/null
@@ -1,9 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces/python/examples/cython</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/interfaces/python/examples/cython</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="example1.pyx">example1.pyx</a></li>
-  <li><a href="setup.py">setup.py</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/setup.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/setup.py
deleted file mode 100644
index d605d2c33a1..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/cython/setup.py
+++ /dev/null
@@ -1,16 +0,0 @@
-from distutils.core import setup
-from distutils.extension import Extension
-from Cython.Distutils import build_ext
-
-ext_module = Extension(
-    "example1",
-    ["example1.pyx"],
-    extra_compile_args=['-fopenmp'],
-    extra_link_args=['-fopenmp'],
-)
-
-setup(
-    name = 'Hello world app',
-    cmdclass = {'build_ext': build_ext},
-    ext_modules = [ext_module],
-)
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example1.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example1.py
deleted file mode 100644
index b3da19a52fc..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example1.py
+++ /dev/null
@@ -1,76 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-## Example adapted from examples/example1.cpp.
-## author of this file: Sebastian F. Walter
-
-import numpy as np
-from qpoases import PyQProblem as QProblem
-from qpoases import PyOptions as Options
-from qpoases import PyPrintLevel as PrintLevel
-
-#Setup data of first QP.
-
-H   = np.array([1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
-A   = np.array([1.0, 1.0 ]).reshape((2,1))
-g   = np.array([1.5, 1.0 ])
-lb  = np.array([0.5, -2.0])
-ub  = np.array([5.0, 2.0 ])
-lbA = np.array([-1.0 ])
-ubA = np.array([2.0])
-
-
-# Setup data of second QP.
-
-g_new   = np.array([1.0, 1.5])
-lb_new  = np.array([0.0, -1.0])
-ub_new  = np.array([5.0, -0.5])
-lbA_new = np.array([-2.0])
-ubA_new = np.array([1.0])
-
-
-# Setting up QProblem object.
-
-example = QProblem(2, 1)
-options = Options()
-options.printLevel = PrintLevel.NONE
-example.setOptions(options)
-
-# Solve first QP.
-nWSR = 10;
-example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
-
-
-# Solve second QP.
-nWSR = 10
-
-for i in range(100000):
-    for j in range(1, 100):
-        g_new[0] = i%j
-    example.hotstart( g_new, lb_new, ub_new, lbA_new, ubA_new, nWSR)
-
-# Get and print solution of second QP.
-
-xOpt = np.zeros(2)
-example.getPrimalSolution(xOpt);
-print("\nxOpt = [ %e, %e ];  objVal = %e\n\n"%(xOpt[0],xOpt[1],example.getObjVal()))
-example.printOptions();
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example1b.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example1b.py
deleted file mode 100644
index 7fb519ad11c..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example1b.py
+++ /dev/null
@@ -1,72 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-## Example adapted from examples/example1b.cpp.
-## author of this file: Sebastian F. Walter
-
-import numpy as np
-from qpoases import PyQProblemB as QProblemB
-from qpoases import PyBooleanType as BooleanType
-from qpoases import PySubjectToStatus as SubjectToStatus
-from qpoases import PyOptions as Options
-
-# Example for qpOASES main function using the QProblemB class.
-
-#Setup data of first QP.
-
-H   = np.array([1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
-g   = np.array([1.5, 1.0 ])
-lb  = np.array([0.5, -2.0])
-ub  = np.array([5.0, 2.0 ])
-
-# Setup data of second QP.
-
-g_new   = np.array([1.0, 1.5])
-lb_new  = np.array([0.0, -1.0])
-ub_new  = np.array([5.0, -0.5])
-
-
-# Setting up QProblemB object.
-example = QProblemB(2)
-
-options = Options()
-options.enableFlippingBounds = BooleanType.FALSE
-options.initialStatusBounds  = SubjectToStatus.INACTIVE
-options.numRefinementSteps   = 1
-
-example.setOptions(options)
-
-# Solve first QP.
-nWSR = 10
-example.init(H, g, lb, ub, nWSR);
-print("\nnWSR = %d\n\n"%nWSR)
-
-# Solve second QP.
-nWSR = 10;
-example.hotstart(g_new, lb_new, ub_new, nWSR)
-print("\nnWSR = %d\n\n"% nWSR)
-
-# Get and print solution of second QP.
-xOpt = np.zeros(2)
-example.getPrimalSolution(xOpt)
-print("\nxOpt = [ %e, %e ];  objVal = %e\n\n" %(xOpt[0], xOpt[1],
-	                                            example.getObjVal()))
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example2.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example2.py
deleted file mode 100644
index d093e2e17fb..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/example2.py
+++ /dev/null
@@ -1,90 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-## Example adapted from examples/example2.cpp.
-## author of this file: Sebastian F. Walter
-
-import numpy as np
-from qpoases import PySQProblem as SQProblem
-from qpoases import PySolutionAnalysis as SolutionAnalysis
-
-
-#  Setup data of first QP.
-H = np.array([ 1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
-A = np.array([ 1.0, 1.0 ]).reshape((2,1))
-g = np.array([ 1.5, 1.0 ])
-lb = np.array([ 0.5, -2.0 ])
-ub = np.array([ 5.0, 2.0 ])
-lbA = np.array([ -1.0 ])
-ubA = np.array([ 2.0 ])
-
-#  Setup data of second QP.
-H_new = np.array([ 1.0, 0.5, 0.5, 0.5 ]).reshape((2,2))
-A_new = np.array([ 1.0, 5.0 ]).reshape((2,1))
-g_new = np.array([ 1.0, 1.5 ])
-lb_new = np.array([ 0.0, -1.0 ])
-ub_new = np.array([ 5.0, -0.5 ])
-lbA_new = np.array([ -2.0 ])
-ubA_new = np.array([ 1.0 ])
-
-#  Setting up SQProblem object and solution analyser.
-example = SQProblem(2, 1)
-analyser = SolutionAnalysis()
-
-#  Solve first QP ...
-nWSR = 10
-example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
-
-#  ... and analyse it.
-maxKKTviolation = np.zeros(1)
-analyser.getMaxKKTviolation(example, maxKKTviolation)
-print("maxKKTviolation: %e\n"%maxKKTviolation)
-
-#  Solve second QP ...
-nWSR = 10;
-example.hotstart(H_new, g_new, A_new, lb_new, ub_new,
-                 lbA_new, ubA_new, nWSR)
-
-#  ... and analyse it.
-analyser.getMaxKKTviolation(example, maxKKTviolation)
-print("maxKKTviolation: %e\n"%maxKKTviolation)
-
-
-#  ------------ VARIANCE-COVARIANCE EVALUATION --------------------
-
-Var             = np.zeros(5*5)
-Primal_Dual_Var = np.zeros(5*5)
-
-Var.reshape((5,5))[0,0] = 1.
-Var.reshape((5,5))[1,1] = 1.
-
-#                  (  1   0   0   0   0   )
-#                  (  0   1   0   0   0   )
-#     Var     =    (  0   0   0   0   0   )
-#                  (  0   0   0   0   0   )
-#                  (  0   0   0   0   0   )
-
-
-analyser.getVarianceCovariance(example, Var, Primal_Dual_Var)
-print('Primal_Dual_Var=\n', Primal_Dual_Var.reshape((5,5)))
-
-print(maxKKTviolation)
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/index.html
deleted file mode 100644
index af694d2f6b3..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/examples/index.html
+++ /dev/null
@@ -1,11 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces/python/examples</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/interfaces/python/examples</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="cython/">cython/</a></li>
-  <li><a href="example1.py">example1.py</a></li>
-  <li><a href="example1b.py">example1b.py</a></li>
-  <li><a href="example2.py">example2.py</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/index.html
deleted file mode 100644
index 75e2a483c3d..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/index.html
+++ /dev/null
@@ -1,13 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces/python</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/interfaces/python</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="README.rst">README.rst</a></li>
-  <li><a href="examples/">examples/</a></li>
-  <li><a href="qpoases.pxd">qpoases.pxd</a></li>
-  <li><a href="qpoases.pyx">qpoases.pyx</a></li>
-  <li><a href="setup.py">setup.py</a></li>
-  <li><a href="tests/">tests/</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/qpoases.pxd b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/qpoases.pxd
deleted file mode 100644
index 60fcf99426a..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/qpoases.pxd
+++ /dev/null
@@ -1,476 +0,0 @@
-##
-##  This file is part of qpOASES.
-##
-##  qpOASES -- An Implementation of the Online Active Set Strategy.
-##  Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##  Christian Kirches et al. All rights reserved.
-##
-##  qpOASES 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.
-##
-##  qpOASES 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 qpOASES; if not, write to the Free Software
-##  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-##
-##    Filename:  qpoases.pxd
-##    Author:    Sebastian F. Walter, Manuel Kudruss
-##    Version:   3.0
-##    Date:      2013-2014
-##
-
-cdef extern from "qpOASES.hpp" namespace "qpOASES":
-
-    ctypedef double real_t
-
-    cdef enum BooleanType:
-
-        BT_FALSE
-        BT_TRUE
-
-    cdef enum PrintLevel:
-
-        PL_DEBUG_ITER = -2
-        PL_TABULAR
-        PL_NONE
-        PL_LOW
-        PL_MEDIUM
-        PL_HIGH
-
-    cdef enum VisibilityStatus:
-
-        VS_HIDDEN
-        VS_VISIBLE
-
-    cdef enum QProblemStatus:
-
-        QPS_NOTINITIALISED
-        QPS_PREPARINGAUXILIARYQP
-
-        QPS_AUXILIARYQPSOLVED
-
-        QPS_PERFORMINGHOMOTOPY
-
-        QPS_HOMOTOPYQPSOLVED
-        QPS_SOLVED
-
-    cdef enum HessianType:
-        HST_ZERO
-        HST_IDENTITY
-        HST_POSDEF
-        HST_POSDEF_NULLSPACE
-        HST_SEMIDEF
-        HST_INDEF
-        HST_UNKNOWN
-
-    cdef enum SubjectToType:
-
-        ST_UNBOUNDED
-        ST_BOUNDED
-        ST_EQUALITY
-        ST_DISABLED
-        ST_UNKNOWN
-
-    cdef enum SubjectToStatus:
-
-        ST_LOWER = -1
-        ST_INACTIVE
-        ST_UPPER
-        ST_INFEASIBLE_LOWER
-        ST_INFEASIBLE_UPPER
-        ST_UNDEFINED
-
-    cdef enum  returnValue:
-        TERMINAL_LIST_ELEMENT = -1
-        SUCCESSFUL_RETURN = 0
-        RET_DIV_BY_ZERO
-        RET_INDEX_OUT_OF_BOUNDS
-        RET_INVALID_ARGUMENTS
-        RET_ERROR_UNDEFINED
-        RET_WARNING_UNDEFINED
-        RET_INFO_UNDEFINED
-        RET_EWI_UNDEFINED
-        RET_AVAILABLE_WITH_LINUX_ONLY
-        RET_UNKNOWN_BUG
-        RET_PRINTLEVEL_CHANGED
-        RET_NOT_YET_IMPLEMENTED
-        RET_INDEXLIST_MUST_BE_REORDERD
-        RET_INDEXLIST_EXCEEDS_MAX_LENGTH
-        RET_INDEXLIST_CORRUPTED
-        RET_INDEXLIST_OUTOFBOUNDS
-        RET_INDEXLIST_ADD_FAILED
-        RET_INDEXLIST_INTERSECT_FAILED
-        RET_INDEX_ALREADY_OF_DESIRED_STATUS
-        RET_ADDINDEX_FAILED
-        RET_REMOVEINDEX_FAILED
-        RET_SWAPINDEX_FAILED
-        RET_NOTHING_TO_DO
-        RET_SETUP_BOUND_FAILED
-        RET_SETUP_CONSTRAINT_FAILED
-        RET_MOVING_BOUND_FAILED
-        RET_MOVING_CONSTRAINT_FAILED
-        RET_SHIFTING_FAILED
-        RET_ROTATING_FAILED
-        RET_QPOBJECT_NOT_SETUP
-        RET_QP_ALREADY_INITIALISED
-        RET_NO_INIT_WITH_STANDARD_SOLVER
-        RET_RESET_FAILED
-        RET_INIT_FAILED
-        RET_INIT_FAILED_TQ
-        RET_INIT_FAILED_CHOLESKY
-        RET_INIT_FAILED_HOTSTART
-        RET_INIT_FAILED_INFEASIBILITY
-        RET_INIT_FAILED_UNBOUNDEDNESS
-        RET_INIT_FAILED_REGULARISATION
-        RET_INIT_SUCCESSFUL
-        RET_OBTAINING_WORKINGSET_FAILED
-        RET_SETUP_WORKINGSET_FAILED
-        RET_SETUP_AUXILIARYQP_FAILED
-        RET_NO_EXTERN_SOLVER
-        RET_QP_UNBOUNDED
-        RET_QP_INFEASIBLE
-        RET_QP_NOT_SOLVED
-        RET_QP_SOLVED
-        RET_UNABLE_TO_SOLVE_QP
-        RET_INITIALISATION_STARTED
-        RET_HOTSTART_FAILED
-        RET_HOTSTART_FAILED_TO_INIT
-        RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED
-        RET_ITERATION_STARTED
-        RET_SHIFT_DETERMINATION_FAILED
-        RET_STEPDIRECTION_DETERMINATION_FAILED
-        RET_STEPLENGTH_DETERMINATION_FAILED
-        RET_OPTIMAL_SOLUTION_FOUND
-        RET_HOMOTOPY_STEP_FAILED
-        RET_HOTSTART_STOPPED_INFEASIBILITY
-        RET_HOTSTART_STOPPED_UNBOUNDEDNESS
-        RET_WORKINGSET_UPDATE_FAILED
-        RET_MAX_NWSR_REACHED
-        RET_CONSTRAINTS_NOT_SPECIFIED
-        RET_INVALID_FACTORISATION_FLAG
-        RET_UNABLE_TO_SAVE_QPDATA
-        RET_STEPDIRECTION_FAILED_TQ
-        RET_STEPDIRECTION_FAILED_CHOLESKY
-        RET_CYCLING_DETECTED
-        RET_CYCLING_NOT_RESOLVED
-        RET_CYCLING_RESOLVED
-        RET_STEPSIZE
-        RET_STEPSIZE_NONPOSITIVE
-        RET_SETUPSUBJECTTOTYPE_FAILED
-        RET_ADDCONSTRAINT_FAILED
-        RET_ADDCONSTRAINT_FAILED_INFEASIBILITY
-        RET_ADDBOUND_FAILED
-        RET_ADDBOUND_FAILED_INFEASIBILITY
-        RET_REMOVECONSTRAINT_FAILED
-        RET_REMOVEBOUND_FAILED
-        RET_REMOVE_FROM_ACTIVESET
-        RET_ADD_TO_ACTIVESET
-        RET_REMOVE_FROM_ACTIVESET_FAILED
-        RET_ADD_TO_ACTIVESET_FAILED
-        RET_CONSTRAINT_ALREADY_ACTIVE
-        RET_ALL_CONSTRAINTS_ACTIVE
-        RET_LINEARLY_DEPENDENT
-        RET_LINEARLY_INDEPENDENT
-        RET_LI_RESOLVED
-        RET_ENSURELI_FAILED
-        RET_ENSURELI_FAILED_TQ
-        RET_ENSURELI_FAILED_NOINDEX
-        RET_ENSURELI_FAILED_CYCLING
-        RET_BOUND_ALREADY_ACTIVE
-        RET_ALL_BOUNDS_ACTIVE
-        RET_CONSTRAINT_NOT_ACTIVE
-        RET_BOUND_NOT_ACTIVE
-        RET_HESSIAN_NOT_SPD
-        RET_HESSIAN_INDEFINITE
-        RET_MATRIX_SHIFT_FAILED
-        RET_MATRIX_FACTORISATION_FAILED
-        RET_PRINT_ITERATION_FAILED
-        RET_NO_GLOBAL_MESSAGE_OUTPUTFILE
-        RET_DISABLECONSTRAINTS_FAILED
-        RET_ENABLECONSTRAINTS_FAILED
-        RET_ALREADY_ENABLED
-        RET_ALREADY_DISABLED
-        RET_NO_HESSIAN_SPECIFIED
-        RET_USING_REGULARISATION
-        RET_EPS_MUST_BE_POSITVE
-        RET_REGSTEPS_MUST_BE_POSITVE
-        RET_HESSIAN_ALREADY_REGULARISED
-        RET_CANNOT_REGULARISE_IDENTITY
-        RET_CANNOT_REGULARISE_SPARSE
-        RET_NO_REGSTEP_NWSR
-        RET_FEWER_REGSTEPS_NWSR
-        RET_CHOLESKY_OF_ZERO_HESSIAN
-        RET_CONSTRAINTS_ARE_NOT_SCALED
-        RET_INITIAL_BOUNDS_STATUS_NYI
-        RET_ERROR_IN_CONSTRAINTPRODUCT
-        RET_FIX_BOUNDS_FOR_LP
-        RET_USE_REGULARISATION_FOR_LP
-        RET_UPDATEMATRICES_FAILED
-        RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED
-        RET_UNABLE_TO_OPEN_FILE
-        RET_UNABLE_TO_WRITE_FILE
-        RET_UNABLE_TO_READ_FILE
-        RET_FILEDATA_INCONSISTENT
-        RET_UNABLE_TO_ANALYSE_QPROBLEM
-        RET_OPTIONS_ADJUSTED
-        RET_NWSR_SET_TO_ONE
-        RET_UNABLE_TO_READ_BENCHMARK
-        RET_BENCHMARK_ABORTED
-        RET_INITIAL_QP_SOLVED
-        RET_QP_SOLUTION_STARTED
-        RET_BENCHMARK_SUCCESSFUL
-        RET_NO_DIAGONAL_AVAILABLE
-        RET_DIAGONAL_NOT_INITIALISED
-        RET_ENSURELI_DROPPED
-        RET_SIMPLE_STATUS_P1
-        RET_SIMPLE_STATUS_P0
-        RET_SIMPLE_STATUS_M1
-        RET_SIMPLE_STATUS_M2
-        RET_SIMPLE_STATUS_M3
-
-
-    cdef cppclass Options:
-
-        Options()
-        Options(const Options&)
-        # Options& operator=( const Options&)  # equality operator cannot be overloaded in Python
-        returnValue setToDefault()
-        returnValue setToReliable()
-        returnValue setToMPC()
-        returnValue setToFast()
-        returnValue ensureConsistency()
-        # returnValue print() # print is a reserved keyword in Python
-        returnValue copy(const Options& )
-
-        PrintLevel printLevel
-
-        BooleanType enableRamping
-        BooleanType enableFarBounds
-        BooleanType enableFlippingBounds
-        BooleanType enableRegularisation
-        BooleanType enableFullLITests
-        BooleanType enableNZCTests
-        int         enableDriftCorrection
-        int         enableCholeskyRefactorisation
-        BooleanType enableEqualities
-
-        real_t terminationTolerance
-        real_t boundTolerance
-        real_t boundRelaxation
-        real_t epsNum
-        real_t epsDen
-        real_t maxPrimalJump
-        real_t maxDualJump
-
-        real_t initialRamping
-        real_t finalRamping
-        real_t initialFarBounds
-        real_t growFarBounds
-        SubjectToStatus initialStatusBounds
-        real_t epsFlipping
-        int numRegularisationSteps
-        real_t epsRegularisation
-        int numRefinementSteps
-        real_t epsIterRef
-        real_t epsLITests
-        real_t epsNZCTests
-
-        BooleanType enableDropInfeasibles
-        int    dropBoundPriority
-        int    dropEqConPriority
-        int    dropIneqConPriority
-
-    cdef cppclass QProblemB:
-        QProblemB()
-        QProblemB(int, HessianType)
-
-        QProblemB(const QProblemB&)
-
-        returnValue init(real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         int&)
-
-        returnValue init(real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         int&,
-                         real_t*)
-
-        returnValue hotstart(real_t*,
-                             real_t*,
-                             real_t*,
-                             int&)
-
-        returnValue hotstart(real_t*,
-                             real_t*,
-                             real_t*,
-                             int&,
-                             real_t*)
-
-
-        returnValue getPrimalSolution(real_t*)
-        returnValue getDualSolution(real_t*)
-        returnValue printOptions()
-        real_t getObjVal()
-
-        Options getOptions()
-        returnValue setOptions(Options&)
-
-    cdef cppclass QProblem:
-        QProblem()
-        QProblem(int, int, HessianType)
-
-        QProblem(const QProblem&)
-
-        returnValue init(real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         int&)
-
-        returnValue init(real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         int&,
-                         real_t*)
-
-        returnValue hotstart(real_t*,
-                             real_t*,
-                             real_t*,
-                             real_t*,
-                             real_t*,
-                             int&)
-
-        returnValue hotstart(real_t*,
-                             real_t*,
-                             real_t*,
-                             real_t*,
-                             real_t*,
-                             int&,
-                             real_t*)
-
-        returnValue getPrimalSolution(real_t*)
-        returnValue getDualSolution(real_t*)
-        returnValue printOptions()
-        real_t getObjVal()
-
-        Options getOptions()
-        returnValue setOptions(Options&)
-
-
-    cdef cppclass SQProblem:
-        SQProblem()
-        SQProblem(int, int, HessianType)
-
-        SQProblem(const QProblem&)
-
-        returnValue init(real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         int&)
-
-        returnValue init(real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         real_t*,
-                         int&,
-                         real_t*)
-
-        returnValue hotstart(real_t*,
-                             real_t*,
-                             real_t*,
-                             real_t*,
-                             real_t*,
-                             real_t*,
-                             real_t*,
-                             int&)
-
-        returnValue hotstart(real_t*,
-                             real_t*,
-                             real_t*,
-                             real_t*,
-                             real_t*,
-                             real_t*,
-                             real_t*,
-                             int&,
-                             real_t*)
-
-        returnValue getPrimalSolution(real_t*)
-        returnValue getDualSolution(real_t*)
-        returnValue printOptions()
-        real_t getObjVal()
-
-        Options getOptions()
-        returnValue setOptions(Options&)
-
-
-    cdef cppclass SolutionAnalysis:
-        SolutionAnalysis()
-        SolutionAnalysis(const SolutionAnalysis&)
-        # ~SolutionAnalysis()
-        # SolutionAnalysis& operator=(const SolutionAnalysis&)
-        returnValue getMaxKKTviolation(QProblem*, real_t& )
-        returnValue getMaxKKTviolation(QProblemB*, real_t&)
-        returnValue getMaxKKTviolation(SQProblem*, real_t&)
-        returnValue getVarianceCovariance(QProblem*, real_t*, real_t*)
-        returnValue getVarianceCovariance(QProblemB*, real_t*, real_t*)
-        returnValue getVarianceCovariance(SQProblem*, real_t*, real_t*)
-
-
-cdef extern from "qpOASES/Utils.hpp" namespace "qpOASES":
-    void getKKTResidual(int nV,                  # Number of variables.
-                        int nC,                  # Number of constraints.
-                        const real_t* const H,   # Hessian matrix.
-                        const real_t* const g,   # Sequence of gradient vectors.
-                        const real_t* const A,   # Constraint matrix.
-                        const real_t* const lb,  # Sequence of lower bound vectors (on variables).
-                        const real_t* const ub,  # Sequence of upper bound vectors (on variables).
-                        const real_t* const lbA, # Sequence of lower constraints' bound vectors.
-                        const real_t* const ubA, # Sequence of upper constraints' bound vectors.
-                        const real_t* const x,   # Sequence of primal trial vectors.
-                        const real_t* const y,   # Sequence of dual trial vectors.
-                        real_t& stat,            # Maximum value of stationarity condition residual.
-                        real_t& feas,            # Maximum value of primal feasibility violation.
-                        real_t& cmpl             # Maximum value of complementarity residual.
-                        )
-
-
-cdef extern from "qpOASES/extras/OQPinterface.hpp" namespace "qpOASES":
-    returnValue runOQPbenchmark(const char* path,           # Full path of the benchmark files (without trailing slash!).
-                                BooleanType isSparse,       # Shall convert matrices to sparse format before solution?
-                                BooleanType useHotstarts,   # Shall QP solution be hotstarted?
-                                const Options& options,     # QP solver options to be used while solving benchmark problems.
-                                int maxAllowedNWSR,         # Maximum number of working set recalculations to be performed.
-                                real_t& maxNWSR,            # Output: Maximum number of performed working set recalculations.
-                                real_t& avgNWSR,            # Output: Average number of performed working set recalculations.
-                                real_t& maxCPUtime,         # Output: Maximum CPU time required for solving each QP.
-                                real_t& avgCPUtime,         # Output: Average CPU time required for solving each QP.
-                                real_t& maxStationarity,    # Output: Maximum residual of stationarity condition.
-                                real_t& maxFeasibility,     # Output: Maximum residual of primal feasibility condition.
-                                real_t& maxComplementarity  # Output: Maximum residual of complementarity condition.
-                                )
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/qpoases.pyx b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/qpoases.pyx
deleted file mode 100644
index bb011c393a2..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/qpoases.pyx
+++ /dev/null
@@ -1,779 +0,0 @@
-##
-##  This file is part of qpOASES.
-##
-##  qpOASES -- An Implementation of the Online Active Set Strategy.
-##  Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##  Christian Kirches et al. All rights reserved.
-##
-##  qpOASES 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.
-##
-##  qpOASES 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 qpOASES; if not, write to the Free Software
-##  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-## author of this file: Sebastian F. Walter
-
-"""
-Python interface to qpOASES
-using Cython
-:author: Sebastian F. Walter, Manuel Kudruss
-"""
-
-import numpy as np
-cimport numpy as np
-
-from cython.operator cimport dereference as deref
-
-cimport qpoases
-
-cdef class PyBooleanType:
-    FALSE = BT_FALSE
-    TRUE  = BT_TRUE
-
-cdef class PyPrintLevel:
-    DEBUG_ITER = PL_DEBUG_ITER
-    TABULAR    = PL_TABULAR
-    NONE       = PL_NONE
-    LOW        = PL_LOW
-    MEDIUM     = PL_MEDIUM
-    HIGH       = PL_HIGH
-
-cdef class PyHessianType:
-    ZERO               = HST_ZERO
-    IDENTITY           = HST_IDENTITY
-    POSDEF             = HST_POSDEF
-    POSDEF_NULLSPACE   = HST_POSDEF_NULLSPACE
-    SEMIDEF            = HST_SEMIDEF
-    INDEF              = HST_INDEF
-    UNKNOWN            = HST_UNKNOWN
-
-cdef class PySubjectToStatus:
-    LOWER              = ST_LOWER
-    INACTIVE           = ST_INACTIVE
-    UPPER              = ST_UPPER
-    INFEASIBLE_LOWER   = ST_INFEASIBLE_LOWER
-    INFEASIBLE_UPPER   = ST_INFEASIBLE_UPPER
-    UNDEFINED          = ST_UNDEFINED
-
-cdef class PyReturnValue:
-    TERMINAL_LIST_ELEMENT                 = -1
-    SUCCESSFUL_RETURN                     = 0
-    DIV_BY_ZERO                           = RET_DIV_BY_ZERO
-    INDEX_OUT_OF_BOUNDS                   = RET_INDEX_OUT_OF_BOUNDS
-    INVALID_ARGUMENTS                     = RET_INVALID_ARGUMENTS
-    ERROR_UNDEFINED                       = RET_ERROR_UNDEFINED
-    WARNING_UNDEFINED                     = RET_WARNING_UNDEFINED
-    INFO_UNDEFINED                        = RET_INFO_UNDEFINED
-    EWI_UNDEFINED                         = RET_EWI_UNDEFINED
-    AVAILABLE_WITH_LINUX_ONLY             = RET_AVAILABLE_WITH_LINUX_ONLY
-    UNKNOWN_BUG                           = RET_UNKNOWN_BUG
-    PRINTLEVEL_CHANGED                    = RET_PRINTLEVEL_CHANGED
-    NOT_YET_IMPLEMENTED                   = RET_NOT_YET_IMPLEMENTED
-    INDEXLIST_MUST_BE_REORDERD            = RET_INDEXLIST_MUST_BE_REORDERD
-    INDEXLIST_EXCEEDS_MAX_LENGTH          = RET_INDEXLIST_EXCEEDS_MAX_LENGTH
-    INDEXLIST_CORRUPTED                   = RET_INDEXLIST_CORRUPTED
-    INDEXLIST_OUTOFBOUNDS                 = RET_INDEXLIST_OUTOFBOUNDS
-    INDEXLIST_ADD_FAILED                  = RET_INDEXLIST_ADD_FAILED
-    INDEXLIST_INTERSECT_FAILED            = RET_INDEXLIST_INTERSECT_FAILED
-    INDEX_ALREADY_OF_DESIRED_STATUS       = RET_INDEX_ALREADY_OF_DESIRED_STATUS
-    ADDINDEX_FAILED                       = RET_ADDINDEX_FAILED
-    REMOVEINDEX_FAILED                    = RET_REMOVEINDEX_FAILED
-    SWAPINDEX_FAILED                      = RET_SWAPINDEX_FAILED
-    NOTHING_TO_DO                         = RET_NOTHING_TO_DO
-    SETUP_BOUND_FAILED                    = RET_SETUP_BOUND_FAILED
-    SETUP_CONSTRAINT_FAILED               = RET_SETUP_CONSTRAINT_FAILED
-    MOVING_BOUND_FAILED                   = RET_MOVING_BOUND_FAILED
-    MOVING_CONSTRAINT_FAILED              = RET_MOVING_CONSTRAINT_FAILED
-    SHIFTING_FAILED                       = RET_SHIFTING_FAILED
-    ROTATING_FAILED                       = RET_ROTATING_FAILED
-    QPOBJECT_NOT_SETUP                    = RET_QPOBJECT_NOT_SETUP
-    QP_ALREADY_INITIALISED                = RET_QP_ALREADY_INITIALISED
-    NO_INIT_WITH_STANDARD_SOLVER          = RET_NO_INIT_WITH_STANDARD_SOLVER
-    RESET_FAILED                          = RET_RESET_FAILED
-    INIT_FAILED                           = RET_INIT_FAILED
-    INIT_FAILED_TQ                        = RET_INIT_FAILED_TQ
-    INIT_FAILED_CHOLESKY                  = RET_INIT_FAILED_CHOLESKY
-    INIT_FAILED_HOTSTART                  = RET_INIT_FAILED_HOTSTART
-    INIT_FAILED_INFEASIBILITY             = RET_INIT_FAILED_INFEASIBILITY
-    INIT_FAILED_UNBOUNDEDNESS             = RET_INIT_FAILED_UNBOUNDEDNESS
-    INIT_FAILED_REGULARISATION            = RET_INIT_FAILED_REGULARISATION
-    INIT_SUCCESSFUL                       = RET_INIT_SUCCESSFUL
-    OBTAINING_WORKINGSET_FAILED           = RET_OBTAINING_WORKINGSET_FAILED
-    SETUP_WORKINGSET_FAILED               = RET_SETUP_WORKINGSET_FAILED
-    SETUP_AUXILIARYQP_FAILED              = RET_SETUP_AUXILIARYQP_FAILED
-    NO_EXTERN_SOLVER                      = RET_NO_EXTERN_SOLVER
-    QP_UNBOUNDED                          = RET_QP_UNBOUNDED
-    QP_INFEASIBLE                         = RET_QP_INFEASIBLE
-    QP_NOT_SOLVED                         = RET_QP_NOT_SOLVED
-    QP_SOLVED                             = RET_QP_SOLVED
-    UNABLE_TO_SOLVE_QP                    = RET_UNABLE_TO_SOLVE_QP
-    INITIALISATION_STARTED                = RET_INITIALISATION_STARTED
-    HOTSTART_FAILED                       = RET_HOTSTART_FAILED
-    HOTSTART_FAILED_TO_INIT               = RET_HOTSTART_FAILED_TO_INIT
-    HOTSTART_FAILED_AS_QP_NOT_INITIALISED = RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED
-    ITERATION_STARTED                     = RET_ITERATION_STARTED
-    SHIFT_DETERMINATION_FAILED            = RET_SHIFT_DETERMINATION_FAILED
-    STEPDIRECTION_DETERMINATION_FAILED    = RET_STEPDIRECTION_DETERMINATION_FAILED
-    STEPLENGTH_DETERMINATION_FAILED       = RET_STEPLENGTH_DETERMINATION_FAILED
-    OPTIMAL_SOLUTION_FOUND                = RET_OPTIMAL_SOLUTION_FOUND
-    HOMOTOPY_STEP_FAILED                  = RET_HOMOTOPY_STEP_FAILED
-    HOTSTART_STOPPED_INFEASIBILITY        = RET_HOTSTART_STOPPED_INFEASIBILITY
-    HOTSTART_STOPPED_UNBOUNDEDNESS        = RET_HOTSTART_STOPPED_UNBOUNDEDNESS
-    WORKINGSET_UPDATE_FAILED              = RET_WORKINGSET_UPDATE_FAILED
-    MAX_NWSR_REACHED                      = RET_MAX_NWSR_REACHED
-    CONSTRAINTS_NOT_SPECIFIED             = RET_CONSTRAINTS_NOT_SPECIFIED
-    INVALID_FACTORISATION_FLAG            = RET_INVALID_FACTORISATION_FLAG
-    UNABLE_TO_SAVE_QPDATA                 = RET_UNABLE_TO_SAVE_QPDATA
-    STEPDIRECTION_FAILED_TQ               = RET_STEPDIRECTION_FAILED_TQ
-    STEPDIRECTION_FAILED_CHOLESKY         = RET_STEPDIRECTION_FAILED_CHOLESKY
-    CYCLING_DETECTED                      = RET_CYCLING_DETECTED
-    CYCLING_NOT_RESOLVED                  = RET_CYCLING_NOT_RESOLVED
-    CYCLING_RESOLVED                      = RET_CYCLING_RESOLVED
-    STEPSIZE                              = RET_STEPSIZE
-    STEPSIZE_NONPOSITIVE                  = RET_STEPSIZE_NONPOSITIVE
-    SETUPSUBJECTTOTYPE_FAILED             = RET_SETUPSUBJECTTOTYPE_FAILED
-    ADDCONSTRAINT_FAILED                  = RET_ADDCONSTRAINT_FAILED
-    ADDCONSTRAINT_FAILED_INFEASIBILITY    = RET_ADDCONSTRAINT_FAILED_INFEASIBILITY
-    ADDBOUND_FAILED                       = RET_ADDBOUND_FAILED
-    ADDBOUND_FAILED_INFEASIBILITY         = RET_ADDBOUND_FAILED_INFEASIBILITY
-    REMOVECONSTRAINT_FAILED               = RET_REMOVECONSTRAINT_FAILED
-    REMOVEBOUND_FAILED                    = RET_REMOVEBOUND_FAILED
-    REMOVE_FROM_ACTIVESET                 = RET_REMOVE_FROM_ACTIVESET
-    ADD_TO_ACTIVESET                      = RET_ADD_TO_ACTIVESET
-    REMOVE_FROM_ACTIVESET_FAILED          = RET_REMOVE_FROM_ACTIVESET_FAILED
-    ADD_TO_ACTIVESET_FAILED               = RET_ADD_TO_ACTIVESET_FAILED
-    CONSTRAINT_ALREADY_ACTIVE             = RET_CONSTRAINT_ALREADY_ACTIVE
-    ALL_CONSTRAINTS_ACTIVE                = RET_ALL_CONSTRAINTS_ACTIVE
-    LINEARLY_DEPENDENT                    = RET_LINEARLY_DEPENDENT
-    LINEARLY_INDEPENDENT                  = RET_LINEARLY_INDEPENDENT
-    LI_RESOLVED                           = RET_LI_RESOLVED
-    ENSURELI_FAILED                       = RET_ENSURELI_FAILED
-    ENSURELI_FAILED_TQ                    = RET_ENSURELI_FAILED_TQ
-    ENSURELI_FAILED_NOINDEX               = RET_ENSURELI_FAILED_NOINDEX
-    ENSURELI_FAILED_CYCLING               = RET_ENSURELI_FAILED_CYCLING
-    BOUND_ALREADY_ACTIVE                  = RET_BOUND_ALREADY_ACTIVE
-    ALL_BOUNDS_ACTIVE                     = RET_ALL_BOUNDS_ACTIVE
-    CONSTRAINT_NOT_ACTIVE                 = RET_CONSTRAINT_NOT_ACTIVE
-    BOUND_NOT_ACTIVE                      = RET_BOUND_NOT_ACTIVE
-    HESSIAN_NOT_SPD                       = RET_HESSIAN_NOT_SPD
-    HESSIAN_INDEFINITE                    = RET_HESSIAN_INDEFINITE
-    MATRIX_SHIFT_FAILED                   = RET_MATRIX_SHIFT_FAILED
-    MATRIX_FACTORISATION_FAILED           = RET_MATRIX_FACTORISATION_FAILED
-    PRINT_ITERATION_FAILED                = RET_PRINT_ITERATION_FAILED
-    NO_GLOBAL_MESSAGE_OUTPUTFILE          = RET_NO_GLOBAL_MESSAGE_OUTPUTFILE
-    DISABLECONSTRAINTS_FAILED             = RET_DISABLECONSTRAINTS_FAILED
-    ENABLECONSTRAINTS_FAILED              = RET_ENABLECONSTRAINTS_FAILED
-    ALREADY_ENABLED                       = RET_ALREADY_ENABLED
-    ALREADY_DISABLED                      = RET_ALREADY_DISABLED
-    NO_HESSIAN_SPECIFIED                  = RET_NO_HESSIAN_SPECIFIED
-    USING_REGULARISATION                  = RET_USING_REGULARISATION
-    EPS_MUST_BE_POSITVE                   = RET_EPS_MUST_BE_POSITVE
-    REGSTEPS_MUST_BE_POSITVE              = RET_REGSTEPS_MUST_BE_POSITVE
-    HESSIAN_ALREADY_REGULARISED           = RET_HESSIAN_ALREADY_REGULARISED
-    CANNOT_REGULARISE_IDENTITY            = RET_CANNOT_REGULARISE_IDENTITY
-    CANNOT_REGULARISE_SPARSE              = RET_CANNOT_REGULARISE_SPARSE
-    NO_REGSTEP_NWSR                       = RET_NO_REGSTEP_NWSR
-    FEWER_REGSTEPS_NWSR                   = RET_FEWER_REGSTEPS_NWSR
-    CHOLESKY_OF_ZERO_HESSIAN              = RET_CHOLESKY_OF_ZERO_HESSIAN
-    CONSTRAINTS_ARE_NOT_SCALED            = RET_CONSTRAINTS_ARE_NOT_SCALED
-    INITIAL_BOUNDS_STATUS_NYI             = RET_INITIAL_BOUNDS_STATUS_NYI
-    ERROR_IN_CONSTRAINTPRODUCT            = RET_ERROR_IN_CONSTRAINTPRODUCT
-    FIX_BOUNDS_FOR_LP                     = RET_FIX_BOUNDS_FOR_LP
-    USE_REGULARISATION_FOR_LP             = RET_USE_REGULARISATION_FOR_LP
-    UPDATEMATRICES_FAILED                 = RET_UPDATEMATRICES_FAILED
-    UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED= RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED
-    UNABLE_TO_OPEN_FILE                   = RET_UNABLE_TO_OPEN_FILE
-    UNABLE_TO_WRITE_FILE                  = RET_UNABLE_TO_WRITE_FILE
-    UNABLE_TO_READ_FILE                   = RET_UNABLE_TO_READ_FILE
-    FILEDATA_INCONSISTENT                 = RET_FILEDATA_INCONSISTENT
-    UNABLE_TO_ANALYSE_QPROBLEM            = RET_UNABLE_TO_ANALYSE_QPROBLEM
-    OPTIONS_ADJUSTED                      = RET_OPTIONS_ADJUSTED
-    NWSR_SET_TO_ONE                       = RET_NWSR_SET_TO_ONE
-    UNABLE_TO_READ_BENCHMARK              = RET_UNABLE_TO_READ_BENCHMARK
-    BENCHMARK_ABORTED                     = RET_BENCHMARK_ABORTED
-    INITIAL_QP_SOLVED                     = RET_INITIAL_QP_SOLVED
-    QP_SOLUTION_STARTED                   = RET_QP_SOLUTION_STARTED
-    BENCHMARK_SUCCESSFUL                  = RET_BENCHMARK_SUCCESSFUL
-    NO_DIAGONAL_AVAILABLE                 = RET_NO_DIAGONAL_AVAILABLE
-    DIAGONAL_NOT_INITIALISED              = RET_DIAGONAL_NOT_INITIALISED
-    ENSURELI_DROPPED                      = RET_ENSURELI_DROPPED
-    SIMPLE_STATUS_P1                      = RET_SIMPLE_STATUS_P1
-    SIMPLE_STATUS_P0                      = RET_SIMPLE_STATUS_P0
-    SIMPLE_STATUS_M1                      = RET_SIMPLE_STATUS_M1
-    SIMPLE_STATUS_M2                      = RET_SIMPLE_STATUS_M2
-    SIMPLE_STATUS_M3                      = RET_SIMPLE_STATUS_M3
-
-
-
-cdef class PyOptions:
-    cdef Options *thisptr      # hold a C++ instance which we're wrapping
-    def __cinit__(self):
-        # FIXME: add support for the other constructors
-        self.thisptr = new Options()
-
-    def __dealloc__(self):
-        del self.thisptr
-
-    def setToDefault(self):
-        return self.thisptr.setToDefault()
-
-    def setToReliable(self):
-        return self.thisptr.setToReliable()
-
-    def setToMPC(self):
-        return self.thisptr.setToMPC()
-
-    def setToFast(self):
-        return self.thisptr.setToFast()
-
-    def ensureConsistency(self):
-        return self.thisptr.ensureConsistency()
-
-    property printLevel:
-        def __get__(self): return self.thisptr.printLevel
-        def __set__(self, printLevel): self.thisptr.printLevel = printLevel
-
-    property enableRamping:
-        def __get__(self): return self.thisptr.enableRamping
-        def __set__(self, enableRamping): self.thisptr.enableRamping = enableRamping
-
-    property enableFarBounds:
-        def __get__(self): return self.thisptr.enableFarBounds
-        def __set__(self, enableFarBounds): self.thisptr.enableFarBounds = enableFarBounds
-
-    property enableFlippingBounds:
-        def __get__(self): return self.thisptr.enableFlippingBounds
-        def __set__(self, enableFlippingBounds): self.thisptr.enableFlippingBounds = enableFlippingBounds
-
-    property enableRegularisation:
-        def __get__(self): return self.thisptr.enableRegularisation
-        def __set__(self, enableRegularisation): self.thisptr.enableRegularisation = enableRegularisation
-
-    property enableFullLITests:
-        def __get__(self): return self.thisptr.enableFullLITests
-        def __set__(self, enableFullLITests): self.thisptr.enableFullLITests = enableFullLITests
-
-    property enableNZCTests:
-        def __get__(self): return self.thisptr.enableNZCTests
-        def __set__(self, enableNZCTests): self.thisptr.enableNZCTests = enableNZCTests
-
-    property enableDriftCorrection:
-        def __get__(self): return self.thisptr.enableDriftCorrection
-        def __set__(self, enableDriftCorrection): self.thisptr.enableDriftCorrection = enableDriftCorrection
-
-    property enableCholeskyRefactorisation:
-        def __get__(self): return self.thisptr.enableCholeskyRefactorisation
-        def __set__(self, enableCholeskyRefactorisation): self.thisptr.enableCholeskyRefactorisation = enableCholeskyRefactorisation
-
-    property enableEqualities:
-        def __get__(self): return self.thisptr.enableEqualities
-        def __set__(self, enableEqualities): self.thisptr.enableEqualities = enableEqualities
-
-    property terminationTolerance:
-        def __get__(self): return self.thisptr.terminationTolerance
-        def __set__(self, terminationTolerance): self.thisptr.terminationTolerance = terminationTolerance
-
-    property boundTolerance:
-        def __get__(self): return self.thisptr.boundTolerance
-        def __set__(self, boundTolerance): self.thisptr.boundTolerance = boundTolerance
-
-    property boundRelaxation:
-        def __get__(self): return self.thisptr.boundRelaxation
-        def __set__(self, boundRelaxation): self.thisptr.boundRelaxation = boundRelaxation
-
-    property epsNum:
-        def __get__(self): return self.thisptr.epsNum
-        def __set__(self, epsNum): self.thisptr.epsNum = epsNum
-
-    property epsDen:
-        def __get__(self): return self.thisptr.epsDen
-        def __set__(self, epsDen): self.thisptr.epsDen = epsDen
-
-    property maxPrimalJump:
-        def __get__(self): return self.thisptr.maxPrimalJump
-        def __set__(self, maxPrimalJump): self.thisptr.maxPrimalJump = maxPrimalJump
-
-    property maxDualJump:
-        def __get__(self): return self.thisptr.maxDualJump
-        def __set__(self, maxDualJump): self.thisptr.maxDualJump = maxDualJump
-
-    property initialRamping:
-        def __get__(self): return self.thisptr.initialRamping
-        def __set__(self, initialRamping): self.thisptr.initialRamping = initialRamping
-
-    property finalRamping:
-        def __get__(self): return self.thisptr.finalRamping
-        def __set__(self, finalRamping): self.thisptr.finalRamping = finalRamping
-
-    property initialFarBounds:
-        def __get__(self): return self.thisptr.initialFarBounds
-        def __set__(self, initialFarBounds): self.thisptr.initialFarBounds = initialFarBounds
-
-    property growFarBounds:
-        def __get__(self): return self.thisptr.growFarBounds
-        def __set__(self, growFarBounds): self.thisptr.growFarBounds = growFarBounds
-
-    property initialStatusBounds:
-        def __get__(self): return self.thisptr.initialStatusBounds
-        def __set__(self, initialStatusBounds): self.thisptr.initialStatusBounds = initialStatusBounds
-
-    property epsFlipping:
-        def __get__(self): return self.thisptr.epsFlipping
-        def __set__(self, epsFlipping): self.thisptr.epsFlipping = epsFlipping
-
-    property numRegularisationSteps:
-        def __get__(self): return self.thisptr.numRegularisationSteps
-        def __set__(self, numRegularisationSteps): self.thisptr.numRegularisationSteps = numRegularisationSteps
-
-    property epsRegularisation:
-        def __get__(self): return self.thisptr.epsRegularisation
-        def __set__(self, epsRegularisation): self.thisptr.epsRegularisation = epsRegularisation
-
-    property numRefinementSteps:
-        def __get__(self): return self.thisptr.numRefinementSteps
-        def __set__(self, numRefinementSteps): self.thisptr.numRefinementSteps = numRefinementSteps
-
-    property epsIterRef:
-        def __get__(self): return self.thisptr.epsIterRef
-        def __set__(self, epsIterRef): self.thisptr.epsIterRef = epsIterRef
-
-    property epsLITests:
-        def __get__(self): return self.thisptr.epsLITests
-        def __set__(self, epsLITests): self.thisptr.epsLITests = epsLITests
-
-    property epsNZCTests:
-        def __get__(self): return self.thisptr.epsNZCTests
-        def __set__(self, epsNZCTests): self.thisptr.epsNZCTests = epsNZCTests
-
-    property dropBoundPriority:
-        def __get__(self): return self.thisptr.dropBoundPriority
-        def __set__(self, dropBoundPriority): self.thisptr.dropBoundPriority = dropBoundPriority
-
-    property dropEqConPriority:
-        def __get__(self): return self.thisptr.dropEqConPriority
-        def __set__(self, dropEqConPriority): self.thisptr.dropEqConPriority = dropEqConPriority
-
-    property dropIneqConPriority:
-        def __get__(self): return self.thisptr.dropIneqConPriority
-        def __set__(self, dropIneqConPriority): self.thisptr.dropIneqConPriority = dropIneqConPriority
-
-
-
-cdef class PyQProblemB:
-    cdef QProblemB *thisptr      # hold a C++ instance which we're wrapping
-    def __cinit__(self, int nV):
-        # FIXME: allow other HessianTypes!
-        self.thisptr = new QProblemB(nV, HST_UNKNOWN)
-    def __dealloc__(self):
-        del self.thisptr
-
-    def init(self,
-             np.ndarray[np.double_t, ndim=2] H,
-             np.ndarray[np.double_t, ndim=1] g,
-             np.ndarray[np.double_t, ndim=1] lb,
-             np.ndarray[np.double_t, ndim=1] ub,
-             nWSR,
-             double cputime=0):
-
-        # FIXME: add asserts
-
-        if cputime > 1.e-16:
-            return self.thisptr.init(
-                    <double*> H.data,
-                    <double*> g.data,
-                    <double*> lb.data,
-                    <double*> ub.data,
-                    nWSR,
-                    &cputime
-                    )
-
-        return self.thisptr.init(
-                    <double*> H.data,
-                    <double*> g.data,
-                    <double*> lb.data,
-                    <double*> ub.data,
-                    nWSR)
-
-    def hotstart(self,
-             np.ndarray[np.double_t, ndim=1] g,
-             np.ndarray[np.double_t, ndim=1] lb,
-             np.ndarray[np.double_t, ndim=1] ub,
-             nWSR,
-             double cputime=0):
-
-        # FIXME: add asserts
-
-        if cputime > 1.e-16:
-            return self.thisptr.hotstart(
-                    <double*> g.data,
-                    <double*> lb.data,
-                    <double*> ub.data,
-                    nWSR,
-                    &cputime
-                    )
-
-        return self.thisptr.hotstart(
-                    <double*> g.data,
-                    <double*> lb.data,
-                    <double*> ub.data,
-                    nWSR)
-
-    def getPrimalSolution(self, np.ndarray[np.double_t, ndim=1] xOpt):
-        return self.thisptr.getPrimalSolution(<double*> xOpt.data)
-
-    def getDualSolution(self, np.ndarray[np.double_t, ndim=1] yOpt):
-        return self.thisptr.getDualSolution(<double*> yOpt.data)
-
-    def getObjVal(self):
-        return self.thisptr.getObjVal()
-
-    def printOptions(self):
-        return self.thisptr.printOptions()
-
-    def getOptions(self):
-        # FIXME: memory management? who deallocates o
-        cdef Options *o = new Options(self.thisptr.getOptions())
-        retval = PyOptions()
-        retval.thisptr = o
-        return retval
-
-    def setOptions(self, PyOptions options):
-        self.thisptr.setOptions(deref(options.thisptr))
-
-
-cdef class PyQProblem:
-    cdef QProblem *thisptr      # hold a C++ instance which we're wrapping
-    def __cinit__(self, int nV, int nC):
-        self.thisptr = new QProblem(nV, nC, HST_UNKNOWN)
-    def __dealloc__(self):
-        del self.thisptr
-
-    cpdef init(self,
-             np.ndarray[np.double_t, ndim=2] H,
-             np.ndarray[np.double_t, ndim=1] g,
-             np.ndarray[np.double_t, ndim=2] A,
-             np.ndarray[np.double_t, ndim=1] lb,
-             np.ndarray[np.double_t, ndim=1] ub,
-             np.ndarray[np.double_t, ndim=1] lbA,
-             np.ndarray[np.double_t, ndim=1] ubA,
-             nWSR,
-             double cputime=0):
-
-        # FIXME: add asserts
-
-        if cputime > 1.e-16:
-            return self.thisptr.init(
-                    <double*> H.data,
-                    <double*> g.data,
-                    <double*> A.data,
-                    <double*> lb.data,
-                    <double*> ub.data,
-                    <double*> lbA.data,
-                    <double*> ubA.data,
-                    nWSR,
-                    &cputime)
-
-        return self.thisptr.init(
-                    <double*> H.data,
-                    <double*> g.data,
-                    <double*> A.data,
-                    <double*> lb.data,
-                    <double*> ub.data,
-                    <double*> lbA.data,
-                    <double*> ubA.data,
-                    nWSR)
-
-    cpdef hotstart(self,
-             np.ndarray[np.double_t, ndim=1] g,
-             np.ndarray[np.double_t, ndim=1] lb,
-             np.ndarray[np.double_t, ndim=1] ub,
-             np.ndarray[np.double_t, ndim=1] lbA,
-             np.ndarray[np.double_t, ndim=1] ubA,
-             nWSR,
-             double cputime=0):
-
-        # FIXME: add asserts
-
-        if cputime > 1.e-16:
-            return self.thisptr.hotstart(
-                    <double*> g.data,
-                    <double*> lb.data,
-                    <double*> ub.data,
-                    <double*> lbA.data,
-                    <double*> ubA.data,
-                    nWSR,
-                    &cputime
-                    )
-
-        return self.thisptr.hotstart(
-                    <double*> g.data,
-                    <double*> lb.data,
-                    <double*> ub.data,
-                    <double*> lbA.data,
-                    <double*> ubA.data,
-                    nWSR)
-
-    cpdef getPrimalSolution(self, np.ndarray[np.double_t, ndim=1] xOpt):
-        return self.thisptr.getPrimalSolution(<double*> xOpt.data)
-
-    cpdef getDualSolution(self, np.ndarray[np.double_t, ndim=1] yOpt):
-        return self.thisptr.getDualSolution(<double*> yOpt.data)
-
-    cpdef getObjVal(self):
-        return self.thisptr.getObjVal()
-
-    cpdef printOptions(self):
-        return self.thisptr.printOptions()
-
-    cpdef setOptions(self, PyOptions options):
-        self.thisptr.setOptions(deref(options.thisptr))
-
-cdef class PySQProblem:
-    cdef SQProblem *thisptr      # hold a C++ instance which we're wrapping
-    def __cinit__(self, int nV, int nC):
-        self.thisptr = new SQProblem(nV, nC, HST_UNKNOWN)
-    def __dealloc__(self):
-        del self.thisptr
-
-    cpdef init(self,
-             np.ndarray[np.double_t, ndim=2] H,
-             np.ndarray[np.double_t, ndim=1] g,
-             np.ndarray[np.double_t, ndim=2] A,
-             np.ndarray[np.double_t, ndim=1] lb,
-             np.ndarray[np.double_t, ndim=1] ub,
-             np.ndarray[np.double_t, ndim=1] lbA,
-             np.ndarray[np.double_t, ndim=1] ubA,
-             int    nWSR,
-             double cputime=0):
-
-        # FIXME: add asserts
-
-        if cputime > 1.e-16:
-            return self.thisptr.init(
-                        <double*> H.data,
-                        <double*> g.data,
-                        <double*> A.data,
-                        <double*> lb.data,
-                        <double*> ub.data,
-                        <double*> lbA.data,
-                        <double*> ubA.data,
-                        nWSR,
-                        &cputime
-            )
-
-        return self.thisptr.init(
-                    <double*> H.data,
-                    <double*> g.data,
-                    <double*> A.data,
-                    <double*> lb.data,
-                    <double*> ub.data,
-                    <double*> lbA.data,
-                    <double*> ubA.data,
-                    nWSR
-        )
-
-    cpdef hotstart(self,
-             np.ndarray[np.double_t, ndim=2] H,
-             np.ndarray[np.double_t, ndim=1] g,
-             np.ndarray[np.double_t, ndim=2] A,
-             np.ndarray[np.double_t, ndim=1] lb,
-             np.ndarray[np.double_t, ndim=1] ub,
-             np.ndarray[np.double_t, ndim=1] lbA,
-             np.ndarray[np.double_t, ndim=1] ubA,
-             int    nWSR,
-             double cputime=0):
-
-        # FIXME: add asserts
-
-        if cputime > 1.e-16:
-            return self.thisptr.hotstart(
-                    <double*> H.data,
-                    <double*> g.data,
-                    <double*> A.data,
-                    <double*> lb.data,
-                    <double*> ub.data,
-                    <double*> lbA.data,
-                    <double*> ubA.data,
-                    nWSR,
-                    &cputime
-            )
-
-        return self.thisptr.hotstart(
-                    <double*> H.data,
-                    <double*> g.data,
-                    <double*> A.data,
-                    <double*> lb.data,
-                    <double*> ub.data,
-                    <double*> lbA.data,
-                    <double*> ubA.data,
-                    nWSR
-        )
-
-    cpdef getPrimalSolution(self, np.ndarray[np.double_t, ndim=1] xOpt):
-        return self.thisptr.getPrimalSolution(<double*> xOpt.data)
-
-    cpdef getDualSolution(self, np.ndarray[np.double_t, ndim=1] yOpt):
-        return self.thisptr.getDualSolution(<double*> yOpt.data)
-
-    cpdef getObjVal(self):
-        return self.thisptr.getObjVal()
-
-    cpdef printOptions(self):
-        return self.thisptr.printOptions()
-
-    cpdef setOptions(self, PyOptions options):
-        self.thisptr.setOptions(deref(options.thisptr))
-
-
-cdef class PySolutionAnalysis:
-    cdef SolutionAnalysis *thisptr      # hold a C++ instance which we're wrapping
-    def __cinit__(self):
-        self.thisptr = new SolutionAnalysis()
-    def __dealloc__(self):
-        del self.thisptr
-
-    cpdef getMaxKKTviolation(self, qp, np.ndarray[np.double_t, ndim=1] maxKKTviolation):
-
-        if isinstance(qp, PyQProblemB):
-            return self._getMaxKKTviolation_QProblemB(qp, maxKKTviolation)
-
-        elif isinstance(qp, PyQProblem):
-            return self._getMaxKKTviolation_QProblem(qp, maxKKTviolation)
-
-        elif isinstance(qp, PySQProblem):
-            return self._getMaxKKTviolation_SQProblem(qp, maxKKTviolation)
-
-        else:
-            raise ValueError('argument 1 must be QProblemB, QProblem or SQProblem')
-
-
-    cpdef _getMaxKKTviolation_QProblemB(self, PyQProblemB qp, np.ndarray[np.double_t, ndim=1] maxKKTviolation):
-        return self.thisptr.getMaxKKTviolation(qp.thisptr, <double&> maxKKTviolation.data[0])
-
-    cpdef _getMaxKKTviolation_QProblem(self, PyQProblem qp, np.ndarray[np.double_t, ndim=1] maxKKTviolation):
-        return self.thisptr.getMaxKKTviolation(qp.thisptr, <double&> maxKKTviolation.data[0])
-
-    cpdef _getMaxKKTviolation_SQProblem(self, PySQProblem qp, np.ndarray[np.double_t, ndim=1] maxKKTviolation):
-        return self.thisptr.getMaxKKTviolation(qp.thisptr, <double&> maxKKTviolation.data[0])
-
-
-
-    cpdef getVarianceCovariance(self,
-                              qp,
-                              np.ndarray[np.double_t, ndim=1] g_b_bA_VAR,
-                              np.ndarray[np.double_t, ndim=1] Primal_Dual_VAR ):
-
-        if isinstance(qp, PyQProblemB):
-            return self._getVarianceCovariance_QProblemB(qp, g_b_bA_VAR, Primal_Dual_VAR)
-
-        elif isinstance(qp, PyQProblem):
-            return self._getVarianceCovariance_QProblem(qp, g_b_bA_VAR, Primal_Dual_VAR)
-
-        elif isinstance(qp, PySQProblem):
-            return self._getVarianceCovariance_SQProblem(qp, g_b_bA_VAR, Primal_Dual_VAR)
-
-        else:
-            raise ValueError('argument 1 must be QProblemB, QProblem or SQProblem')
-
-    cpdef _getVarianceCovariance_QProblemB(self,
-                              PyQProblemB qp,
-                              np.ndarray[np.double_t, ndim=1] g_b_bA_VAR,
-                              np.ndarray[np.double_t, ndim=1] Primal_Dual_VAR ):
-        return self.thisptr.getVarianceCovariance(qp.thisptr,
-                                                  <double*> g_b_bA_VAR.data,
-                                                  <double*> Primal_Dual_VAR.data)
-
-    cpdef _getVarianceCovariance_QProblem(self,
-                              PyQProblem qp,
-                              np.ndarray[np.double_t, ndim=1] g_b_bA_VAR,
-                              np.ndarray[np.double_t, ndim=1] Primal_Dual_VAR ):
-        return self.thisptr.getVarianceCovariance(qp.thisptr,
-                                                  <double*> g_b_bA_VAR.data,
-                                                  <double*> Primal_Dual_VAR.data)
-
-    cpdef _getVarianceCovariance_SQProblem(self,
-                              PySQProblem qp,
-                              np.ndarray[np.double_t, ndim=1] g_b_bA_VAR,
-                              np.ndarray[np.double_t, ndim=1] Primal_Dual_VAR ):
-        return self.thisptr.getVarianceCovariance(qp.thisptr,
-                                                  <double*> g_b_bA_VAR.data,
-                                                  <double*> Primal_Dual_VAR.data)
-
-# Wrapped some utility functions for unit testing
-cpdef py_runOQPbenchmark(path,               # Full path of the benchmark files (without trailing slash!).
-                       isSparse,           # Shall convert matrices to sparse format before solution?
-                       useHotstarts,       # Shall QP solution be hotstarted?
-                       PyOptions options,  # QP solver options to be used while solving benchmark problems.
-                       int maxAllowedNWSR, # Maximum number of working set recalculations to be performed.
-                       double maxCPUTime,  # Maximum allowed CPU time for qp solving.
-                       ):
-    """run a QP benchmark example"""
-    maxNWSR            = 0.0 # Output: Maximum number of performed working set recalculations.
-    avgNWSR            = 0.0 # Output: Average number of performed working set recalculations.
-    maxCPUtime         = 0.0 # Output: Maximum CPU time required for solving each QP.
-    avgCPUtime         = 0.0 # Output: Average CPU time required for solving each QP.
-    maxStationarity    = 0.0 # Output: Maximum residual of stationarity condition.
-    maxFeasibility     = 0.0 # Output: Maximum residual of primal feasibility condition.
-    maxComplementarity = 0.0 # Output: Maximum residual of complementarity condition.
-
-    maxCPUtime = maxCPUTime
-
-    p = path.encode()
-    returnValue = runOQPbenchmark(p,
-                                  isSparse,
-                                  useHotstarts,
-                                  deref(options.thisptr),
-                                  maxAllowedNWSR,
-                                  maxNWSR,
-                                  avgNWSR,
-                                  maxCPUtime,
-                                  avgCPUtime,
-                                  maxStationarity,
-                                  maxFeasibility,
-                                  maxComplementarity)
-
-    return returnValue, maxNWSR, avgNWSR, maxCPUtime, avgCPUtime, \
-           maxStationarity, maxFeasibility, maxComplementarity
-
-
-def py_getKKTResidual(int nV,                              # Number of variables.
-                      int nC,                              # Number of constraints.
-                      np.ndarray[np.double_t, ndim=2] H,   # Hessian matrix.
-                      np.ndarray[np.double_t, ndim=1] g,   # Sequence of gradient vectors.
-                      np.ndarray[np.double_t, ndim=2] A,   # Constraint matrix.
-                      np.ndarray[np.double_t, ndim=1] lb,  # Sequence of lower bound vectors (on variables).
-                      np.ndarray[np.double_t, ndim=1] ub,  # Sequence of upper bound vectors (on variables).
-                      np.ndarray[np.double_t, ndim=1] lbA, # Sequence of lower constraints' bound vectors.
-                      np.ndarray[np.double_t, ndim=1] ubA, # Sequence of upper constraints' bound vectors.
-                      np.ndarray[np.double_t, ndim=1] x,   # Sequence of primal trial vectors.
-                      np.ndarray[np.double_t, ndim=1] y,   # Sequence of dual trial vectors.
-                      ):
-    stat = 0.0 # Maximum value of stationarity condition residual.
-    feas = 0.0 # Maximum value of primal feasibility violation.
-    cmpl = 0.0 # Maximum value of complementarity residual.
-    getKKTResidual(nV,
-                   nC,
-                   <double*> H.data,
-                   <double*> g.data,
-                   <double*> A.data,
-                   <double*> lb.data,
-                   <double*> ub.data,
-                   <double*> lbA.data,
-                   <double*> ubA.data,
-                   <double*> x.data,
-                   <double*> y.data,
-                   stat,
-                   feas,
-                   cmpl
-                   )
-    return stat, feas, cmpl
-
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/setup.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/setup.py
deleted file mode 100644
index e338fc993ad..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/setup.py
+++ /dev/null
@@ -1,72 +0,0 @@
-#!/usr/bin/env python
-
-##
-##  This file is part of qpOASES.
-##
-##  qpOASES -- An Implementation of the Online Active Set Strategy.
-##  Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##  Christian Kirches et al. All rights reserved.
-##
-##  qpOASES 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.
-##
-##  qpOASES 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 qpOASES; if not, write to the Free Software
-##  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-##
-##	Filename:  setup.py
-##	Author:    Sebastian F. Walter, Manuel Kudruss
-##	Version:   3.0
-##	Date:      2013-2014
-##
-
-
-from distutils.core import setup
-from distutils.extension import Extension
-from Cython.Distutils import build_ext
-from Cython.Build import cythonize
-
-import os
-import numpy as np
-
-BASEDIR = os.path.dirname(os.path.abspath(__file__))
-BASEDIR = os.path.dirname(BASEDIR)
-BASEDIR = os.path.dirname(BASEDIR)
-print(('BASEDIR=', BASEDIR))
-
-extra_params = {}
-extra_params['include_dirs'] = [
-    '/usr/include',
-    os.path.join(BASEDIR, 'include'),
-    os.path.join(BASEDIR, 'include', 'qpOASES'),
-    np.get_include()]
-extra_params['extra_compile_args'] = ["-O2", "-Wno-unused-variable"]
-extra_params['extra_link_args'] = ["-Wl,-O1", "-Wl,--as-needed"]
-
-extra_params = extra_params.copy()
-extra_params['libraries'] = ['qpOASES']
-
-extra_params['library_dirs'] = ['/usr/lib', os.path.join(BASEDIR, 'bin')]
-extra_params['language'] = 'c++'
-
-if os.name == 'posix':
-	extra_params['runtime_library_dirs'] = extra_params['library_dirs']
-
-ext_modules = [
-    Extension("qpoases",  ["qpoases.pyx", "qpoases.pxd"],   **extra_params),
-]
-
-setup(
-    name='qpOASES interface',
-    cmdclass={'build_ext': build_ext},
-    ext_modules=cythonize(ext_modules),
-)
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/__init__.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/__init__.py
deleted file mode 100644
index 8b137891791..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/__init__.py
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/index.html
deleted file mode 100644
index 2adccecf074..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/index.html
+++ /dev/null
@@ -1,11 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces/python/tests</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/interfaces/python/tests</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="__init__.py">__init__.py</a></li>
-  <li><a href="test_examples.py">test_examples.py</a></li>
-  <li><a href="test_idhessian.py">test_idhessian.py</a></li>
-  <li><a href="test_testbench.py">test_testbench.py</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_examples.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_examples.py
deleted file mode 100644
index b826ef08868..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_examples.py
+++ /dev/null
@@ -1,328 +0,0 @@
-"""
-This file is part of qpOASES.
-
-qpOASES -- An Implementation of the Online Active Set Strategy.
-Copyright (C) 2007-2009 by Hans Joachim Ferreau et al. All rights reserved.
-
-qpOASES 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.
-
-qpOASES 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 qpOASES; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-
-author Manuel Kudruss
-version 3.0
-date 2013-2014
-"""
-
-import os
-import re
-import numpy as np
-from numpy.testing import *
-from subprocess import Popen, PIPE, STDOUT
-
-from qpoases import PyQProblem as QProblem
-from qpoases import PyQProblemB as QProblemB
-from qpoases import PySQProblem as SQProblem
-from qpoases import PySolutionAnalysis as SolutionAnalysis
-from qpoases import PyBooleanType as BooleanType
-from qpoases import PySubjectToStatus as SubjectToStatus
-from qpoases import PyOptions as Options
-from qpoases import PyPrintLevel as PrintLevel
-
-# get qpOASES path
-qpoases_path = os.path.dirname(os.path.abspath(__file__))
-qpoases_path = os.path.dirname(qpoases_path)
-qpoases_path = os.path.dirname(qpoases_path)
-qpoases_path = os.path.dirname(qpoases_path)
-
-# set qpOASES binary path
-bin_path = os.path.join(qpoases_path, "bin")
-
-class TestExamples(TestCase):
-
-    def test_example1(self):
-        return 0
-        # Example for qpOASES main function using the QProblem class.
-        #Setup data of first QP.
-
-        H   = np.array([1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
-        A   = np.array([1.0, 1.0 ]).reshape((2,1))
-        g   = np.array([1.5, 1.0 ])
-        lb  = np.array([0.5, -2.0])
-        ub  = np.array([5.0, 2.0 ])
-        lbA = np.array([-1.0 ])
-        ubA = np.array([2.0])
-
-        # Setup data of second QP.
-
-        g_new   = np.array([1.0, 1.5])
-        lb_new  = np.array([0.0, -1.0])
-        ub_new  = np.array([5.0, -0.5])
-        lbA_new = np.array([-2.0])
-        ubA_new = np.array([1.0])
-
-        # Setting up QProblemB object.
-        qp = QProblem(2, 1)
-        options = Options()
-        options.printLevel = PrintLevel.NONE
-        qp.setOptions(options)
-
-        # Solve first QP.
-        nWSR = 10
-        qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
-
-        # Solve second QP.
-        nWSR = 10
-        qp.hotstart(g_new, lb_new, ub_new, lbA_new, ubA_new, nWSR)
-
-        # Get and print solution of second QP.
-        xOpt_actual = np.zeros(2)
-        qp.getPrimalSolution(xOpt_actual)
-        xOpt_actual = np.asarray(xOpt_actual, dtype=float)
-        objVal_actual = qp.getObjVal()
-        objVal_actual = np.asarray(objVal_actual, dtype=float)
-
-        cmd = os.path.join(bin_path, "example1")
-        p = Popen(cmd, shell=True, stdout=PIPE)
-        stdout, stderr = p.communicate()
-        stdout = str(stdout).replace('\\n', '\n')
-        stdout = stdout.replace("'", '')
-        print(stdout)
-
-        # get c++ solution from std
-        pattern = re.compile(r'xOpt\s*=\s*\[\s+(?P<xOpt>([0-9., e+-])*)\];')
-        match = pattern.search(stdout)
-        xOpt_expected = match.group('xOpt')
-        xOpt_expected = xOpt_expected.split(",")
-        xOpt_expected = np.asarray(xOpt_expected, dtype=float)
-
-        pattern = re.compile(r'objVal = (?P<objVal>[0-9-+e.]*)')
-        match = pattern.search(stdout)
-        objVal_expected = match.group('objVal')
-        objVal_expected = np.asarray(objVal_expected, dtype=float)
-
-        print("xOpt_actual =", xOpt_actual)
-        print("xOpt_expected =", xOpt_expected)
-        print("objVal_actual = ", objVal_actual)
-        print("objVal_expected = ", objVal_expected)
-
-        assert_almost_equal(xOpt_actual, xOpt_expected, decimal=7)
-        assert_almost_equal(objVal_actual, objVal_expected, decimal=7)
-
-
-    def test_example1b(self):
-        # Example for qpOASES main function using the QProblemB class.
-        #Setup data of first QP.
-
-        H   = np.array([1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
-        g   = np.array([1.5, 1.0 ])
-        lb  = np.array([0.5, -2.0])
-        ub  = np.array([5.0, 2.0 ])
-
-        # Setup data of second QP.
-
-        g_new   = np.array([1.0, 1.5])
-        lb_new  = np.array([0.0, -1.0])
-        ub_new  = np.array([5.0, -0.5])
-
-        # Setting up QProblemB object.
-        qp = QProblemB(2)
-
-        options = Options()
-        options.enableFlippingBounds = BooleanType.FALSE
-        options.initialStatusBounds  = SubjectToStatus.INACTIVE
-        options.numRefinementSteps   = 1
-        options.printLevel = PrintLevel.NONE
-        qp.setOptions(options)
-
-        # Solve first QP.
-        nWSR = 10
-        qp.init(H, g, lb, ub, nWSR)
-
-        # Solve second QP.
-        nWSR = 10;
-        qp.hotstart(g_new, lb_new, ub_new, nWSR)
-
-        # Get and print solution of second QP.
-        xOpt_actual = np.zeros(2)
-        qp.getPrimalSolution(xOpt_actual)
-        xOpt_actual = np.asarray(xOpt_actual, dtype=float)
-        objVal_actual = qp.getObjVal()
-        objVal_actual = np.asarray(objVal_actual, dtype=float)
-
-        cmd = os.path.join(bin_path, "example1b")
-        p = Popen(cmd, shell=True, stdout=PIPE)
-        stdout, stderr = p.communicate()
-        stdout = str(stdout).replace('\\n', '\n')
-        stdout = stdout.replace("'", '')
-        print(stdout)
-
-        # get c++ solution from std
-        pattern = re.compile(r'xOpt\s*=\s*\[\s+(?P<xOpt>([0-9., e+-])*)\];')
-        match = pattern.search(stdout)
-        xOpt_expected = match.group('xOpt')
-        xOpt_expected = xOpt_expected.split(",")
-        xOpt_expected = np.asarray(xOpt_expected, dtype=float)
-
-        pattern = re.compile(r'objVal = (?P<objVal>[0-9-+e.]*)')
-        match = pattern.search(stdout)
-        objVal_expected = match.group('objVal')
-        objVal_expected = np.asarray(objVal_expected, dtype=float)
-
-        print("xOpt_actual =", xOpt_actual)
-        print("xOpt_expected =", xOpt_expected)
-        print("objVal_actual = ", objVal_actual)
-        print("objVal_expected = ", objVal_expected)
-
-        assert_almost_equal(xOpt_actual, xOpt_expected, decimal=7)
-        assert_almost_equal(objVal_actual, objVal_expected, decimal=7)
-
-
-    def test_example2(self):
-        # Example for qpOASES main function using the SQProblem class.
-        #  Setup data of first QP.
-        H = np.array([ 1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
-        A = np.array([ 1.0, 1.0 ]).reshape((2,1))
-        g = np.array([ 1.5, 1.0 ])
-        lb = np.array([ 0.5, -2.0 ])
-        ub = np.array([ 5.0, 2.0 ])
-        lbA = np.array([ -1.0 ])
-        ubA = np.array([ 2.0 ])
-
-        #  Setup data of second QP.
-        H_new = np.array([ 1.0, 0.5, 0.5, 0.5 ]).reshape((2,2))
-        A_new = np.array([ 1.0, 5.0 ]).reshape((2,1))
-        g_new = np.array([ 1.0, 1.5 ])
-        lb_new = np.array([ 0.0, -1.0 ])
-        ub_new = np.array([ 5.0, -0.5 ])
-        lbA_new = np.array([ -2.0 ])
-        ubA_new = np.array([ 1.0 ])
-
-        #  Setting up SQProblem object and solution analyser.
-        qp = SQProblem(2, 1)
-        options = Options()
-        options.printLevel = PrintLevel.NONE
-        qp.setOptions(options)
-
-        analyser = SolutionAnalysis()
-
-        # get c++ solution from std
-        cmd = os.path.join(bin_path, "example2")
-        p = Popen(cmd, shell=True, stdout=PIPE)
-        stdout, stderr = p.communicate()
-        stdout = str(stdout).replace('\\n', '\n')
-        stdout = stdout.replace("'", '')
-        print(stdout)
-
-        #  Solve first QP ...
-        nWSR = 10
-        qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
-
-        #  ... and analyse it.
-        maxKKTviolation = np.zeros(1)
-        analyser.getMaxKKTviolation(qp, maxKKTviolation)
-        print("maxKKTviolation: %e\n"%maxKKTviolation)
-        actual = np.asarray(maxKKTviolation)
-
-        pattern = re.compile(r'maxKKTviolation: (?P<maxKKTviolation>[0-9+-e.]*)')
-
-        match = pattern.findall(stdout)
-        expected = np.asarray(match[0], dtype=float)
-
-        assert_almost_equal(actual, expected, decimal=7)
-
-        #  Solve second QP ...
-        nWSR = 10
-        qp.hotstart(H_new, g_new, A_new,
-                              lb_new, ub_new,
-                              lbA_new, ubA_new, nWSR)
-
-        #  ... and analyse it.
-        analyser.getMaxKKTviolation(qp, maxKKTviolation)
-        print("maxKKTviolation: %e\n"%maxKKTviolation)
-        actual = np.asarray(maxKKTviolation)
-
-        expected = np.asarray(match[1], dtype=float)
-
-        assert_almost_equal(actual, expected, decimal=7)
-
-
-        #  ------------ VARIANCE-COVARIANCE EVALUATION --------------------
-
-        Var             = np.zeros(5*5)
-        Primal_Dual_Var = np.zeros(5*5)
-
-        Var.reshape((5,5))[0,0] = 1.
-        Var.reshape((5,5))[1,1] = 1.
-
-        #                  (  1   0   0   0   0   )
-        #                  (  0   1   0   0   0   )
-        #     Var     =    (  0   0   0   0   0   )
-        #                  (  0   0   0   0   0   )
-        #                  (  0   0   0   0   0   )
-
-
-        analyser.getVarianceCovariance(qp, Var, Primal_Dual_Var)
-        print('Primal_Dual_Var=\n', Primal_Dual_Var.reshape((5,5)))
-        actual = Primal_Dual_Var.reshape((5,5))
-
-        pattern = re.compile(r'Primal_Dual_VAR = (?P<VAR>.*)',
-                             re.DOTALL)
-
-        print(stdout)
-        match = pattern.search(stdout)
-        expected = match.group('VAR').strip().split("\n")
-        expected = [x.strip().split() for x in expected]
-        print(expected)
-        expected = np.asarray(expected, dtype=float)
-
-        assert_almost_equal(actual, expected, decimal=7)
-
-
-    def test_example7(self):
-        H   = np.array([ 0.8514828085899353, -0.15739890933036804, -0.081726007163524628, -0.530426025390625, 0.16773293912410736,
-                        -0.15739890933036804, 1.1552412509918213, 0.57780224084854126, -0.0072606131434440613, 0.010559185408055782,
-                        -0.081726007163524628, 0.57780224084854126, 0.28925251960754395, 5.324830453901086e-006, -3.0256599075073609e-006,
-                        -0.530426025390625, -0.0072606131434440613, 5.324830453901086e-006, 0.35609596967697144, -0.15124998986721039,
-                         0.16773293912410736, 0.010559185408055782, -3.0256599075073609e-006, -0.15124998986721039,
-                         0.15129712224006653], dtype=float).reshape((5, 5))
-        g   = np.array([0.30908384919166565, 0.99325823783874512, 0.49822014570236206, -0.26309865713119507, 0.024296050891280174], dtype=float).reshape((5,))
-        A   = np.array([1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1], dtype=float).reshape((5, 5))
-        lb  = np.array([-0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359938621520996], dtype=float).reshape((5,))
-        ub  = np.array([ 0.052359879016876221, 0.052359879016876221, 0.052359879016876221, 0, 0], dtype=float).reshape((5,))
-        lbA = np.array([-0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359938621520996], dtype=float).reshape((5,))
-        ubA = np.array([0.052359879016876221, 0.052359879016876221, 0.052359879016876221, 0, 0], dtype=float).reshape((5,))
-
-        # Setting up QProblem object.
-        qp = QProblem(5, 5)
-        options = Options()
-        options.printLevel = PrintLevel.NONE
-        qp.setOptions(options)
-
-        # Solve first QP.
-        nWSR = 100
-        qp.init(H, g, A, lb, ub, lbA, ubA,  nWSR)
-
-        result = np.zeros((5,))
-        qp.getPrimalSolution(result)
-
-        # TODO check against what?
-        # Where can I find solution?
-
-if __name__=="__main__":
-    try:
-        import nose
-        nose.runmodule()
-
-    except ImportError:
-        sys.stderr.write("Please install nosestests for python unittesting.\n")
-
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_idhessian.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_idhessian.py
deleted file mode 100644
index 4bddb2dc4b0..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_idhessian.py
+++ /dev/null
@@ -1,94 +0,0 @@
-"""
-This file is part of qpOASES.
-
-qpOASES -- An Implementation of the Online Active Set Strategy.
-Copyright (C) 2007-2009 by Hans Joachim Ferreau et al. All rights reserved.
-
-qpOASES 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.
-
-qpOASES 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 qpOASES; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-"""
-
-#TODO add doxygen support
-# \author Manuel Kudruss
-# \version 3.0
-# \date 2013-2014
-
-import os
-import numpy as np
-from numpy.testing import *
-from qpoases import PyQProblem as QProblem
-from qpoases import PyBooleanType as BooleanType
-from qpoases import PyOptions as Options
-from qpoases import PyPrintLevel as PrintLevel
-
-# get qpOASES path
-qpoases_path = os.path.dirname(os.path.abspath(__file__))
-qpoases_path = os.path.dirname(qpoases_path)
-qpoases_path = os.path.dirname(qpoases_path)
-qpoases_path = os.path.dirname(qpoases_path)
-
-# set qpOASES testing path
-testing_path = os.path.join(qpoases_path, "testing")
-
-class TestIdHessian(TestCase):
-
-    def test_id_hessian(self):
-        """Very simple example for testing qpOASES (using QProblem class)."""
-
-        path = os.path.join(testing_path, "dev_idhessian_data")
-
-        #Setup data for QP.
-        H   = np.loadtxt(os.path.join(path, "H.txt"))
-        g   = np.loadtxt(os.path.join(path, "g.txt"))
-        A   = np.loadtxt(os.path.join(path, "A.txt"))
-        lb  = np.loadtxt(os.path.join(path, "lb.txt"))
-        ub  = np.loadtxt(os.path.join(path, "ub.txt"))
-        lbA = np.loadtxt(os.path.join(path, "lbA.txt"))
-        ubA = np.loadtxt(os.path.join(path, "ubA.txt"))
-
-        #Setting up QProblem object.
-        qp = QProblem(72,144)
-
-        options = Options()
-        options.numRefinementSteps   = 1
-        options.printLevel = PrintLevel.NONE
-
-        #options.setToMPC()
-        #options.setToReliable()
-        #options.enableFlippingBounds = BooleanType.FALSE
-        options.enableRamping = BooleanType.FALSE
-        #options.enableRamping = BooleanType.TRUE
-        #options.enableFarBounds = BooleanType.FALSE
-        #options.enableRamping = BooleanType.FALSE
-        #options.printLevel = PL_LOW
-        #options.enableFullLITests = BooleanType.FALSE
-        #options.boundRelaxation = 1.0e-1
-        qp.setOptions( options )
-
-        #Solve QP.
-        nWSR = 1200
-
-        qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
-
-        # FIXME check against what?
-        # Where can I find solution?
-
-if __name__=="__main__":
-    try:
-        import nose
-        nose.runmodule()
-
-    except ImportError:
-        sys.stderr.write("Please install nosestests for python unittesting.\n")
-
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_testbench.py b/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_testbench.py
deleted file mode 100644
index 0333ff7e42b..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/python/tests/test_testbench.py
+++ /dev/null
@@ -1,332 +0,0 @@
-"""
-This file is part of qpOASES.
-
-qpOASES -- An Implementation of the Online Active Set Strategy.
-Copyright (C) 2007-2009 by Hans Joachim Ferreau et al. All rights reserved.
-
-qpOASES 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.
-
-qpOASES 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 qpOASES; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-
-author Manuel Kudruss
-version 3.0
-date 2013-2014
-"""
-
-import os
-import numpy as np
-from numpy.testing import *
-from qpoases import py_runOQPbenchmark as runOQPbenchmark
-from qpoases import PyQProblem as QProblem
-from qpoases import PyBooleanType as BooleanType
-from qpoases import PyReturnValue as ReturnValue
-from qpoases import PyOptions as Options
-from qpoases import PyPrintLevel as PrintLevel
-
-# get qpOASES path
-qpoases_path = os.path.dirname(os.path.abspath(__file__))
-qpoases_path = os.path.dirname(qpoases_path)
-qpoases_path = os.path.dirname(qpoases_path)
-qpoases_path = os.path.dirname(qpoases_path)
-
-# set qpOASES testing path
-testing_path = os.path.join(qpoases_path, 'testing')
-
-
-benchmarks = ('CVXQP1_S',
-              'CVXQP2_S',
-              'CVXQP3_S',
-              'DPKLO1',
-              'DUAL1',
-              'DUAL2',
-              'DUAL3',
-              'DUAL4',
-              'DUALC1',
-              'DUALC2',
-              'DUALC5',
-              'DUALC8',
-              'GENHS28',
-              'HS118',
-              'HS21',
-              'HS268',
-              'HS35',
-              'HS35MOD',
-              'HS51',
-              'HS52',
-              'HS53',
-              'HS76',
-              'LOTSCHD',
-              'PRIMALC1',
-              'PRIMALC2',
-              'PRIMALC5',
-              'QADLITTL',
-              'QAFIRO',
-              'QBEACONF',
-              'QBRANDY',
-              'QE226',
-              'QISRAEL',
-              'QPCBLEND',
-              'QPCBOEI2',
-              'QPTEST',
-              'QRECIPE',
-              'QSC205',
-              'QSCAGR7',
-              'QSHARE1B',
-              'QSHARE2B',
-              'S268',
-              'TAME',
-              'VALUES',
-              'ZECEVIC2',
-              )
-
-def results2str(results):
-    """converts results dictionary to pretty string"""
-    hline = '{0:->11}|{0:-<12}|{0:-<12}|{0:-<12}|{0:-<8}|{0:-<8}\n'.format("")
-
-    npass = 0
-    nfail = 0
-
-    string = ""
-    string += '{:<10} | {: <10} | {: <10} | {: <10} | {: <6} | {:<6}\n'.format('problem', 'stat', 'feas', 'compl', 'nWSR', 'result')
-    string += hline
-    for key in results:
-        line = '{name:<10} | {stat: >10.4e} | {feas: >10.4e} | {comp: >10.4e} | {nwsr: >6d} | {pass!s:<6}\n'.format(**results[key])
-        string += line
-        if results[key]['pass']:
-            npass += 1
-        else:
-            nfail +=1
-
-    string += hline
-    string += '\n'
-    string += 'Testbench results:\n'
-    string += '==================\n'
-    string += 'Pass:  {: >10d}\n'.format(npass)
-    string += 'Fail:  {: >10d}\n'.format(nfail)
-    string += '------------------\n'
-    string += 'Ratio: {: >10.2%}\n'.format(npass/float(len(results)))
-
-    return string
-
-def write_results(name, string):
-    """writes results into results dictionary"""
-    path = os.path.dirname(os.path.abspath(__file__))
-
-    directory = os.path.join(path, 'results')
-
-    if not os.path.exists(directory):
-        os.makedirs(directory)
-
-    with open(os.path.join(directory, name), 'w') as f:
-        f.write(string)
-
-def get_nfail(results):
-    """get nfail from results dictionary"""
-    nfail = 0
-    for key in results:
-        if not results[key]['pass']:
-            nfail +=1
-
-    return nfail
-
-def run_benchmarks(benchmarks, options, isSparse, useHotstarts,
-                   nWSR, cpu_time, TOL):
-    """run all benchmarks and return results as dictionary"""
-    results = {}
-    for item in benchmarks:
-        # NOTE: c/c++ function epects trailing slash in path!
-        path = os.path.join(testing_path, 'problems', item, '')
-
-        # Run QP benchmark
-        returnvalue, maxNWSR, avgNWSR, maxCPUtime, avgCPUtime, \
-        maxStationarity, maxFeasibility, maxComplementarity \
-        = runOQPbenchmark(path, isSparse, useHotstarts,
-                          options, nWSR, cpu_time )
-
-        if (returnvalue == ReturnValue.SUCCESSFUL_RETURN
-            and maxStationarity    < TOL
-            and maxFeasibility     < TOL
-            and maxComplementarity < TOL):
-            ret_val = True
-
-        else:
-            ret_val = False
-
-        tmp_d = {'name': item,
-                 'stat': maxStationarity,
-                 'feas': maxFeasibility,
-                 'comp': maxComplementarity,
-                 'nwsr': int(maxNWSR),
-                 'pass': bool(ret_val),
-                }
-        results[item] = tmp_d
-
-    return results
-
-
-class Testbench(TestCase):
-
-    def setUp(self):
-        # Setup global options for every problem
-        self.TOL = 1e-5
-        self.nWSR = 3500
-        self.cpu_time = 300
-        self.decimal = 7 # number of decimals for assert
-
-    def test_m44_default_dense(self):
-        test_name = 'mm44_default_dense.txt'
-        print("Test: ", test_name)
-
-        # QP Options
-        options = Options()
-        options.setToDefault()
-        options.printLevel = PrintLevel.NONE
-
-        isSparse = False
-        useHotstarts = False
-
-        # run QP benchmarks
-        results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
-                                 self.nWSR, self.cpu_time, self.TOL)
-
-        # print and write results
-        string = results2str(results)
-        print(string)
-        write_results(test_name, string)
-
-        assert get_nfail(results) <= 0, 'One ore more tests failed.'
-
-    def test_m44_default_sparse(self):
-        test_name = 'mm44_default_sparse.txt'
-        print("Test: ", test_name)
-
-        # QP Options
-        options = Options()
-        options.setToDefault()
-        options.printLevel = PrintLevel.NONE
-
-        isSparse = True
-        useHotstarts = False
-
-        # run QP benchmarks
-        results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
-                                 self.nWSR, self.cpu_time, self.TOL)
-
-        # print and write results
-        string = results2str(results)
-        print(string)
-        write_results(test_name, string)
-
-        assert get_nfail(results) <= 0, 'One ore more tests failed.'
-
-    def test_m44_mpc_dense(self):
-        test_name ='mm44_mpc_dense.txt'
-        print("Test: ", test_name)
-
-        # QP Options
-        options = Options()
-        options.setToMPC()
-        options.printLevel = PrintLevel.NONE
-
-        isSparse = False
-        useHotstarts = False
-
-        # run QP benchmarks
-        results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
-                                 self.nWSR, self.cpu_time, self.TOL)
-
-        # print and write results
-        string = results2str(results)
-        print(string)
-        write_results(test_name, string)
-
-        assert get_nfail(results) <= 2, 'One ore more tests failed.'
-
-    def test_m44_mpc_sparse(self):
-        test_name ='mm44_mpc_sparse.txt'
-        print("Test: ", test_name)
-
-        # QP Options
-        options = Options()
-        options.setToMPC()
-        options.printLevel = PrintLevel.NONE
-
-        isSparse = True
-        useHotstarts = False
-
-        # run QP benchmarks
-        results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
-                                 self.nWSR, self.cpu_time, self.TOL)
-
-        # print and write results
-        string = results2str(results)
-        print(string)
-        write_results(test_name, string)
-
-        assert get_nfail(results) <= 19, 'One ore more tests failed.'
-
-    def test_m44_reliable_dense(self):
-        test_name = 'mm44_reliable_dense.txt'
-        print("Test: ", test_name)
-
-        # QP Options
-        options = Options()
-        options.setToReliable()
-        options.printLevel = PrintLevel.NONE
-
-        isSparse = False
-        useHotstarts = False
-
-        # run QP benchmarks
-        results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
-                                 self.nWSR, self.cpu_time, self.TOL)
-
-        # print and write results
-        string = results2str(results)
-        print(string)
-        write_results(test_name, string)
-
-        assert get_nfail(results) <= 0, 'One ore more tests failed.'
-
-    def test_m44_reliable_sparse(self):
-        test_name = 'mm44_reliable_sparse.txt'
-        print(test_name)
-
-        # QP Options
-        options = Options()
-        options.setToReliable()
-        options.printLevel = PrintLevel.NONE
-
-        isSparse = True
-        useHotstarts = False
-
-        # run QP benchmarks
-        results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
-                                 self.nWSR, self.cpu_time, self.TOL)
-
-        # print and write results
-        string = results2str(results)
-        print(string)
-        write_results(test_name, string)
-
-        assert get_nfail(results) <= 0, 'One ore more tests failed.'
-
-
-if __name__=='__main__':
-    try:
-        import nose
-        nose.runmodule(argv=['', '-s', '-v'])
-
-    except ImportError:
-        sys.stderr.write('Please install nosestests for python unittesting.\n')
-
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1.dat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1.dat
deleted file mode 100644
index 1c9e594b6da775ef2a23e6db6ce75a676f91f100..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 656
zcmb7AK?;O05bJss77vOy5mvCjnEqU!(jPb{@wiiH&_WOz7}7&C$>dl)#m70<<T1)n
z!K>sb9o^zR_9VV><vl9ri$Aot4*n`b%MGC^{UUPjQ<bg{**gp^Pnvu%kDS|7_1}eV
o8NV|*iQj`BF6)5{xef`{^3~|jL)@%~A4`f35<g&l*Zf!c0|-Py!2kdN

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1a.dat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1a.dat
deleted file mode 100644
index 4e67850dc07986750160308909887ba9fd3a9eef..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 784
zcmb7>O$x*y427N97KSb=-AQ2z^%n8wouhaIvnpLq1Rst}5DbL4@VzfT>F{bij4{uW
zQc5u4)z(O}>f$|oa;<R}AEv9rhqmg#XR?zHgr?<1^!;%|x+N5Lu#?_t<{*FiJl)WL
z6ViKqZOTZPC*Pt5_JFx+o%g;^yBGM}banXX;h(?AUUVQdEe{SX`x(*U4)K&bsGVjG
M5)`R&?~(sZUuq^%egFUf

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1b.dat b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/example1b.dat
deleted file mode 100644
index b591a7ddcf2ea8854c8bfdf5ba1193068bc1cce2..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 408
zcmaE_u7MRWGB7YOK{1emf)DmE7ApO~UI42p8X7QjKr{mUfSU8cUX&ZVM(p|?!0bOD
tN0L4f2dLc+0xIHs3RtYfZXe9QKY;GUt`WOFm`^|Kmm^6Z%$*PR0|49CCeQ!?

diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/index.html
deleted file mode 100644
index 8b1f81fa0f5..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/index.html
+++ /dev/null
@@ -1,14 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces/scilab</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/interfaces/scilab</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="Makefile">Makefile</a></li>
-  <li><a href="example1.dat">example1.dat</a></li>
-  <li><a href="example1a.dat">example1a.dat</a></li>
-  <li><a href="example1b.dat">example1b.dat</a></li>
-  <li><a href="qpOASESinterface.c">qpOASESinterface.c</a></li>
-  <li><a href="qpOASESinterface.sce">qpOASESinterface.sce</a></li>
-  <li><a href="qpOASESroutines.cpp">qpOASESroutines.cpp</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESinterface.c b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESinterface.c
deleted file mode 100644
index 9be0a6f223c..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESinterface.c
+++ /dev/null
@@ -1,1061 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file interfaces/scilab/qpOASESinterface.c
- *	\author Holger Diedam, Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Interface that enables to call qpOASES from scilab.
- *  (Please excuse a lot of copy and paste...)
- *
- */
-
-
-#include <stdio.h>
-#include <string.h>
-
-#include <scilab/stack-c.h>
-#include <scilab/Scierror.h>
-
-
-extern int int_qpOASES( char* fname );
-
-extern int int_init(   char* fname );
-extern int int_initSB( char* fname );
-extern int int_initVM( char* fname );
-
-extern int int_solve(   char* fname );
-extern int int_solveSB( char* fname );
-extern int int_solveVM( char* fname );
-
-extern int int_cleanup(   char* fname );
-extern int int_cleanupSB( char* fname );
-extern int int_cleanupVM( char* fname );
-
-
-typedef int (*gate_function) ( char* );
-extern int sci_gateway( char* name, gate_function f );
-extern int C2F(qpOASESgateway)();
-
-
-/* forward declaration of C++ routines */
-void qpoases(	double* H, double* g, double* A, double* lb, double* ub, double* lbA, double* ubA,
-				int *nV, int* nC, int* nWSR,
-				double* x, double* obj, int* status, int* nWSRout, double* y
-				);
-
-void init(		double* H, double* g, double* A, double* lb, double* ub, double* lbA, double* ubA,
-				int *nV, int* nC, int* nWSR,
-				double* x, double* obj, int* status, int* nWSRout, double* y
-				);
-void initSB(	double* H, double* g, double* lb, double* ub,
-				int *nV, int* nWSR,
-				double* x, double* obj, int* status, int* nWSRout, double* y
-				);
-void initVM(	double* H, double* g, double* A, double* lb, double* ub, double* lbA, double* ubA,
-				int *nV, int* nC, int* nWSR,
-				double* x, double* obj, int* status, int* nWSRout, double* y
-				);
-
-void hotstart(		double* g, double* lb, double* ub, double* lbA, double* ubA,
-					int* nWSR,
-					double* x, double* obj, int* status, int* nWSRout, double* y
-					);
-void hotstartSB(	double* g, double* lb, double* ub,
-					int* nWSR,
-					double* x, double* obj, int* status, int* nWSRout, double* y
-					);
-void hotstartVM(	double* H, double* g, double* A, double* lb, double* ub, double* lbA, double* ubA,
-					int* nWSR,
-					double* x, double* obj, int* status, int* nWSRout, double* y
-					);
-
-void cleanupp( );
-void cleanupSB( );
-void cleanupVM( );
-
-
-/* global variables containing dimensions of matrices
- * (also used to check whether qpOASES object were initialised) */
-static int qp_rowsH = -1;
-static int qp_rowsA = -1;
-static int qpb_rowsH = -1;
-static int sqp_rowsH = -1;
-static int sqp_rowsA = -1;
-
-
-/*
- *	i n t _ q p O A S E S
- */
-int int_qpOASES( char* fname )
-{
-	int H, H_rows, H_cols;
-	int g, g_rows, g_cols;
-	int A, A_rows, A_cols;
-	int lb, lb_rows, lb_cols;
-	int ub, ub_rows, ub_cols;
-	int lbA, lbA_rows, lbA_cols;
-	int ubA, ubA_rows, ubA_cols;
-	int nWSR, nWSR_rows, nWSR_cols;
-
-	int x, obj, status, nWSRout, y;
-
-
-	int minlhs = 1, maxlhs = 5, minrhs = 8, maxrhs = 8, one = 1, y_size;
-
-	CheckRhs( minrhs,maxrhs );
-	CheckLhs( minlhs,maxlhs );
-
-
-	/* check dimensions */
-	GetRhsVar( 1,"d", &H_rows,&H_cols,&H );
-	if ( ( H_rows != H_cols ) || ( H_rows < 1 ) )
-	{
-		Scierror( 111,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 2,"d", &g_rows,&g_cols,&g );
-	if ( !( ( ( g_rows == H_rows ) && ( g_cols == 1 ) ) || ( ( g_rows == 1 ) && ( g_cols == H_rows ) ) ) )
-	{
-		Scierror( 112,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 3,"d", &A_rows,&A_cols,&A );
-	if ( ( A_cols != H_rows ) || ( A_rows < 1 ) )
-	{
-		Scierror( 113,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 4,"d", &lb_rows,&lb_cols,&lb);
-	if ( !( ( ( lb_rows == H_rows ) && ( lb_cols == 1 ) ) || ( ( lb_rows == 0 ) && ( lb_cols == 0 ) ) ) )
-	{
-		Scierror( 114,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 5,"d", &ub_rows,&ub_cols,&ub);
-	if ( !( ( ( ub_rows == H_rows ) && ( ub_cols == 1 ) ) || ( ( ub_rows == 0 ) && ( ub_cols == 0 ) ) ) )
-	{
-		Scierror( 115,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 6,"d", &lbA_rows,&lbA_cols,&lbA);
-	if ( ( lbA_rows != A_rows ) || ( lbA_cols != 1 ) )
-	{
-		Scierror( 116,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 7,"d", &ubA_rows,&ubA_cols,&ubA);
-	if ( ( ubA_rows != A_rows ) || ( ubA_cols != 1 ) )
-	{
-		Scierror( 117,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 8,"i", &nWSR_rows,&nWSR_cols,&nWSR);
-	if ( ( nWSR_rows != nWSR_cols ) || ( nWSR_cols != 1 ) )
-	{
-		Scierror( 118,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-
-	y_size = H_rows + A_rows;
-
-	CreateVar(  9,"d", &H_rows,&one,&x );
-	CreateVar( 10,"d", &one,&one,&obj );
-	CreateVar( 11,"i", &one,&one,&status );
-	CreateVar( 12,"i", &one,&one,&nWSRout );
-	CreateVar( 13,"d", &y_size,&one,&y );
-
-
-	/* call interfaced qpOASES routines with appropriate arguments */
-	if ( ( lb_rows != 0 ) && ( ub_rows != 0 ) )
-	{
-		qpoases(	stk(H),stk(g),stk(A),stk(lb),stk(ub),stk(lbA),stk(ubA),
-					&H_rows,&A_rows,istk(nWSR),
-					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-					);
-	}
-	else if ( ( lb_rows == 0 ) && ( ub_rows != 0 ) )
-	{
-		qpoases(	stk(H),stk(g),stk(A),0,stk(ub),stk(lbA),stk(ubA),
-					&H_rows,&A_rows,istk(nWSR),
-					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-					);
-	}
-	else if ( ( lb_rows != 0 ) && ( ub_rows == 0 ) )
-	{
-		qpoases(	stk(H),stk(g),stk(A),stk(lb),0,stk(lbA),stk(ubA),
-					&H_rows,&A_rows,istk(nWSR),
-					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-					);
-	}
-	else
-	{
-		qpoases(	stk(H),stk(g),stk(A),0,0,stk(lbA),stk(ubA),
-					&H_rows,&A_rows,istk(nWSR),
-					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-					);
-	}
-
-	LhsVar(1) = 9;
-	LhsVar(2) = 10;
-	LhsVar(3) = 11;
-	LhsVar(4) = 12;
-	LhsVar(5) = 13;
-
-	return 0;
-}
-
-
-/*
- *	i n t _ i n i t
- */
-int int_init( char* fname )
-{
-	int H, H_rows, H_cols;
-	int g, g_rows, g_cols;
-	int A, A_rows, A_cols;
-	int lb, lb_rows, lb_cols;
-	int ub, ub_rows, ub_cols;
-	int lbA, lbA_rows, lbA_cols;
-	int ubA, ubA_rows, ubA_cols;
-	int nWSR, nWSR_rows, nWSR_cols;
-
-	int x, obj, status, nWSRout, y;
-
-
-	int minlhs = 1, maxlhs = 5, minrhs = 8, maxrhs = 8, one = 1, y_size;
-
-	CheckRhs( minrhs,maxrhs );
-	CheckLhs( minlhs,maxlhs );
-
-
-	/* check dimensions */
-	GetRhsVar( 1,"d", &H_rows,&H_cols,&H );
-	if ( ( H_rows != H_cols ) || ( H_rows < 1 ) )
-	{
-		Scierror( 211,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 2,"d", &g_rows,&g_cols,&g );
-	if ( !( ( ( g_rows == H_rows ) && ( g_cols == 1 ) ) || ( ( g_rows == 1 ) && ( g_cols == H_rows ) ) ) )
-	{
-		Scierror( 212,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 3,"d", &A_rows,&A_cols,&A );
-	if ( ( A_cols != H_rows ) || ( A_rows < 1 ) )
-	{
-		Scierror( 213,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 4,"d", &lb_rows,&lb_cols,&lb);
-	if ( !( ( ( lb_rows == H_rows ) && ( lb_cols == 1 ) ) || ( ( lb_rows == 0 ) && ( lb_cols == 0 ) ) ) )
-	{
-		Scierror( 214,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 5,"d", &ub_rows,&ub_cols,&ub);
-	if ( !( ( ( ub_rows == H_rows ) && ( ub_cols == 1 ) ) || ( ( ub_rows == 0 ) && ( ub_cols == 0 ) ) ) )
-	{
-		Scierror( 215,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 6,"d", &lbA_rows,&lbA_cols,&lbA);
-	if ( ( lbA_rows != A_rows ) || ( lbA_cols != 1 ) )
-	{
-		Scierror( 216,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 7,"d", &ubA_rows,&ubA_cols,&ubA);
-	if ( ( ubA_rows != A_rows ) || ( ubA_cols != 1 ) )
-	{
-		Scierror( 217,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 8,"i", &nWSR_rows,&nWSR_cols,&nWSR);
-	if ( ( nWSR_rows != nWSR_cols ) || ( nWSR_cols != 1 ) )
-	{
-		Scierror( 218,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-
-	y_size = H_rows + A_rows;
-
-	CreateVar(  9,"d", &H_rows,&one,&x );
-	CreateVar( 10,"d", &one,&one,&obj );
-	CreateVar( 11,"i", &one,&one,&status );
-	CreateVar( 12,"i", &one,&one,&nWSRout );
-	CreateVar( 13,"d", &y_size,&one,&y );
-
-
-	qp_rowsH = H_rows;
-	qp_rowsA = A_rows;
-
-
-	/* call interfaced qpOASES routines with appropriate arguments */
-	if ( ( lb_rows != 0 ) && ( ub_rows != 0 ) )
-	{
-		init(	stk(H),stk(g),stk(A),stk(lb),stk(ub),stk(lbA),stk(ubA),
-				&H_rows,&A_rows,istk(nWSR),
-				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-				);
-	}
-	else if ( ( lb_rows == 0 ) && ( ub_rows != 0 ) )
-	{
-		init(	stk(H),stk(g),stk(A),0,stk(ub),stk(lbA),stk(ubA),
-				&H_rows,&A_rows,istk(nWSR),
-				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-				);
-	}
-	else if ( ( lb_rows != 0 ) && ( ub_rows == 0 ) )
-	{
-		init(	stk(H),stk(g),stk(A),stk(lb),0,stk(lbA),stk(ubA),
-				&H_rows,&A_rows,istk(nWSR),
-				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-				);
-	}
-	else
-	{
-		init(	stk(H),stk(g),stk(A),0,0,stk(lbA),stk(ubA),
-				&H_rows,&A_rows,istk(nWSR),
-				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-				);
-	}
-
-	LhsVar(1) = 9;
-	LhsVar(2) = 10;
-	LhsVar(3) = 11;
-	LhsVar(4) = 12;
-	LhsVar(5) = 13;
-
-	return 0;
-}
-
-
-/*
- *	i n t _ i n i t S B
- */
-int int_initSB( char* fname )
-{
-	int H, H_rows, H_cols;
-	int g, g_rows, g_cols;
-	int lb, lb_rows, lb_cols;
-	int ub, ub_rows, ub_cols;
-	int nWSR, nWSR_rows, nWSR_cols;
-
-	int x, obj, status, nWSRout, y;
-
-
-	int minlhs = 1, maxlhs = 5, minrhs = 5, maxrhs = 5, one = 1;
-
-	CheckRhs( minrhs,maxrhs );
-	CheckLhs( minlhs,maxlhs );
-
-
-	/* check dimensions */
-	GetRhsVar( 1,"d", &H_rows,&H_cols,&H );
-	if ( ( H_rows != H_cols ) || ( H_rows < 1 ) )
-	{
-		Scierror( 221,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 2,"d", &g_rows,&g_cols,&g );
-	if ( !( ( ( g_rows == H_rows ) && ( g_cols == 1 ) ) || ( ( g_rows == 1 ) && ( g_cols == H_rows ) ) ) )
-	{
-		Scierror( 222,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 3,"d", &lb_rows,&lb_cols,&lb);
-	if ( !( ( ( lb_rows == H_rows ) && ( lb_cols == 1 ) ) || ( ( lb_rows == 0 ) && ( lb_cols == 0 ) ) ) )
-	{
-		Scierror( 223,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 4,"d", &ub_rows,&ub_cols,&ub);
-	if ( !( ( ( ub_rows == H_rows ) && ( ub_cols == 1 ) ) || ( ( ub_rows == 0 ) && ( ub_cols == 0 ) ) ) )
-	{
-		Scierror( 224,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 5,"i", &nWSR_rows,&nWSR_cols,&nWSR);
-	if ( ( nWSR_rows != nWSR_cols ) || ( nWSR_cols != 1 ) )
-	{
-		Scierror( 225,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-
-	CreateVar(  9,"d", &H_rows,&one,&x );
-	CreateVar( 10,"d", &one,&one,&obj );
-	CreateVar( 11,"i", &one,&one,&status );
-	CreateVar( 12,"i", &one,&one,&nWSRout );
-	CreateVar( 13,"d", &H_rows,&one,&y );
-
-
-	qpb_rowsH = H_rows;
-
-
-	/* call interfaced qpOASES routines with appropriate arguments */
-	if ( ( lb_rows != 0 ) && ( ub_rows != 0 ) )
-	{
-		initSB(	stk(H),stk(g),stk(lb),stk(ub),
-				&H_rows,istk(nWSR),
-				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-				);
-	}
-	else if ( ( lb_rows == 0 ) && ( ub_rows != 0 ) )
-	{
-		initSB(	stk(H),stk(g),0,stk(ub),
-				&H_rows,istk(nWSR),
-				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-				);
-	}
-	else if ( ( lb_rows != 0 ) && ( ub_rows == 0 ) )
-	{
-		initSB(	stk(H),stk(g),stk(lb),0,
-				&H_rows,istk(nWSR),
-				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-				);
-	}
-	else
-	{
-		initSB(	stk(H),stk(g),0,0,
-				&H_rows,istk(nWSR),
-				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-				);
-	}
-
-	LhsVar(1) = 9;
-	LhsVar(2) = 10;
-	LhsVar(3) = 11;
-	LhsVar(4) = 12;
-	LhsVar(5) = 13;
-
-	return 0;
-}
-
-
-/*
- *	i n t _ i n i t V M
- */
-int int_initVM( char* fname )
-{
-	int H, H_rows, H_cols;
-	int g, g_rows, g_cols;
-	int A, A_rows, A_cols;
-	int lb, lb_rows, lb_cols;
-	int ub, ub_rows, ub_cols;
-	int lbA, lbA_rows, lbA_cols;
-	int ubA, ubA_rows, ubA_cols;
-	int nWSR, nWSR_rows, nWSR_cols;
-
-	int x, obj, status, nWSRout, y;
-
-
-	int minlhs = 1, maxlhs = 5, minrhs = 8, maxrhs = 8, one = 1, y_size;
-
-	CheckRhs( minrhs,maxrhs );
-	CheckLhs( minlhs,maxlhs );
-
-
-	/* check dimensions */
-	GetRhsVar( 1,"d", &H_rows,&H_cols,&H );
-	if ( ( H_rows != H_cols ) || ( H_rows < 1 ) )
-	{
-		Scierror( 231,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 2,"d", &g_rows,&g_cols,&g );
-	if ( !( ( ( g_rows == H_rows ) && ( g_cols == 1 ) ) || ( ( g_rows == 1 ) && ( g_cols == H_rows ) ) ) )
-	{
-		Scierror( 232,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 3,"d", &A_rows,&A_cols,&A );
-	if ( ( A_cols != H_rows ) || ( A_rows < 1 ) )
-	{
-		Scierror( 233,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 4,"d", &lb_rows,&lb_cols,&lb );
-	if ( !( ( ( lb_rows == H_rows ) && ( lb_cols == 1 ) ) || ( ( lb_rows == 0 ) && ( lb_cols == 0 ) ) ) )
-	{
-		Scierror( 234,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 5,"d", &ub_rows,&ub_cols,&ub );
-	if ( !( ( ( ub_rows == H_rows ) && ( ub_cols == 1 ) ) || ( ( ub_rows == 0 ) && ( ub_cols == 0 ) ) ) )
-	{
-		Scierror( 235,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 6,"d", &lbA_rows,&lbA_cols,&lbA );
-	if ( ( lbA_rows != A_rows ) || ( lbA_cols != 1 ) )
-	{
-		Scierror( 236,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 7,"d", &ubA_rows,&ubA_cols,&ubA );
-	if ( ( ubA_rows != A_rows ) || ( ubA_cols != 1 ) )
-	{
-		Scierror( 237,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 8,"i", &nWSR_rows,&nWSR_cols,&nWSR) ;
-	if ( ( nWSR_rows != nWSR_cols ) || ( nWSR_cols != 1 ) )
-	{
-		Scierror( 238,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-
-	y_size = H_rows + A_rows;
-
-	CreateVar(  9,"d", &H_rows,&one,&x );
-	CreateVar( 10,"d", &one,&one,&obj );
-	CreateVar( 11,"i", &one,&one,&status );
-	CreateVar( 12,"i", &one,&one,&nWSRout );
-	CreateVar( 13,"d", &y_size,&one,&y );
-
-
-	sqp_rowsH = H_rows;
-	sqp_rowsA = A_rows;
-
-
-	/* call interfaced qpOASES routines with appropriate arguments */
-	if ( ( lb_rows != 0 ) && ( ub_rows != 0 ) )
-	{
-		initVM(	stk(H),stk(g),stk(A),stk(lb),stk(ub),stk(lbA),stk(ubA),
-				&H_rows,&A_rows,istk(nWSR),
-				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-				);
-	}
-	else if ( ( lb_rows == 0 ) && ( ub_rows != 0 ) )
-	{
-		initVM(	stk(H),stk(g),stk(A),0,stk(ub),stk(lbA),stk(ubA),
-				&H_rows,&A_rows,istk(nWSR),
-				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-				);
-	}
-	else if ( ( lb_rows != 0 ) && ( ub_rows == 0 ) )
-	{
-		initVM(	stk(H),stk(g),stk(A),stk(lb),0,stk(lbA),stk(ubA),
-				&H_rows,&A_rows,istk(nWSR),
-				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-				);
-	}
-	else
-	{
-		initVM(	stk(H),stk(g),stk(A),0,0,stk(lbA),stk(ubA),
-				&H_rows,&A_rows,istk(nWSR),
-				stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-				);
-	}
-
-	LhsVar(1) = 9;
-	LhsVar(2) = 10;
-	LhsVar(3) = 11;
-	LhsVar(4) = 12;
-	LhsVar(5) = 13;
-
-	return 0;
-}
-
-
-/*
- *	i n t _ h o t s t a r t
- */
-int int_hotstart( char* fname )
-{
-	int g, g_rows, g_cols;
-	int lb, lb_rows, lb_cols;
-	int ub, ub_rows, ub_cols;
-	int lbA, lbA_rows, lbA_cols;
-	int ubA, ubA_rows, ubA_cols;
-	int nWSR, nWSR_rows, nWSR_cols;
-
-	int x, obj, status, nWSRout, y;
-
-
-	int minlhs = 1, maxlhs = 5, minrhs = 6, maxrhs = 6, one = 1, y_size;
-
-	CheckRhs( minrhs,maxrhs );
-	CheckLhs( minlhs,maxlhs );
-
-
-	if ( ( qp_rowsH == -1 ) || ( qp_rowsA == -1 ) )
-	{
-		Scierror( 311,"ERROR (qpOASES): Need to call qpOASES_init first!\n" );
-		return 0;
-	}
-
-	/* check dimensions */
-	GetRhsVar( 1,"d", &g_rows,&g_cols,&g );
-	if ( !( ( ( g_rows == qp_rowsH ) && ( g_cols == 1 ) ) || ( ( g_rows == 1 ) && ( g_cols == qp_rowsH ) ) ) )
-	{
-		Scierror( 312,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 2,"d", &lb_rows,&lb_cols,&lb );
-	if ( !( ( ( lb_rows == qp_rowsH ) && ( lb_cols == 1 ) ) || ( ( lb_rows == 0 ) && ( lb_cols == 0 ) ) ) )
-	{
-		Scierror( 313,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 3,"d", &ub_rows,&ub_cols,&ub );
-	if ( !( ( ( ub_rows == qp_rowsH ) && ( ub_cols == 1 ) ) || ( ( ub_rows == 0 ) && ( ub_cols == 0 ) ) ) )
-	{
-		Scierror( 314,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 4,"d", &lbA_rows,&lbA_cols,&lbA );
-	if ( ( lbA_rows != qp_rowsA ) || ( lbA_cols != 1 ) )
-	{
-		Scierror( 315,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 5,"d", &ubA_rows,&ubA_cols,&ubA );
-	if ( ( ubA_rows != qp_rowsA ) || ( ubA_cols != 1 ) )
-	{
-		Scierror( 316,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 6,"i", &nWSR_rows,&nWSR_cols,&nWSR );
-	if ( ( nWSR_rows != nWSR_cols ) || ( nWSR_cols != 1 ) )
-	{
-		Scierror( 317,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-
-	y_size = qp_rowsH + qp_rowsA;
-
-	CreateVar(  7,"d", &qp_rowsH,&one,&x );
-	CreateVar(  8,"d", &one,&one,&obj );
-	CreateVar(  9,"i", &one,&one,&status );
-	CreateVar( 10,"i", &one,&one,&nWSRout );
-	CreateVar( 11,"d", &y_size,&one,&y );
-
-
-	/* call interfaced qpOASES routines with appropriate arguments */
-	if ( ( lb_rows != 0 ) && ( ub_rows != 0 ) )
-	{
-		hotstart(	stk(g),stk(lb),stk(ub),stk(lbA),stk(ubA),
-					istk(nWSR),
-					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-					);
-	}
-	else if ( ( lb_rows == 0 ) && ( ub_rows != 0 ) )
-	{
-		hotstart(	stk(g),0,stk(ub),stk(lbA),stk(ubA),
-					istk(nWSR),
-					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-					);
-	}
-	else if ( ( lb_rows != 0 ) && ( ub_rows == 0 ) )
-	{
-		hotstart(	stk(g),stk(lb),0,stk(lbA),stk(ubA),
-					istk(nWSR),
-					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-					);
-	}
-	else
-	{
-		hotstart(	stk(g),0,0,stk(lbA),stk(ubA),
-					istk(nWSR),
-					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-					);
-	}
-
-
-	LhsVar(1) = 7;
-	LhsVar(2) = 8;
-	LhsVar(3) = 9;
-	LhsVar(4) = 10;
-	LhsVar(5) = 11;
-
-	return 0;
-}
-
-
-/*
- *	i n t _ h o t s t a r t S B
- */
-int int_hotstartSB( char* fname )
-{
-	int g, g_rows, g_cols;
-	int lb, lb_rows, lb_cols;
-	int ub, ub_rows, ub_cols;
-	int nWSR, nWSR_rows, nWSR_cols;
-
-	int x, obj, status, nWSRout, y;
-
-
-	int minlhs = 1, maxlhs = 5, minrhs = 4, maxrhs = 4, one = 1;
-
-	CheckRhs( minrhs,maxrhs );
-	CheckLhs( minlhs,maxlhs );
-
-
-	if ( qpb_rowsH == -1 )
-	{
-		Scierror( 321,"ERROR (qpOASES): Need to call qpOASES_init first!\n" );
-		return 0;
-	}
-
-	/* check dimensions */
-	GetRhsVar( 1,"d", &g_rows,&g_cols,&g );
-	if ( !( ( ( g_rows == qpb_rowsH ) && ( g_cols == 1 ) ) || ( ( g_rows == 1 ) && ( g_cols == qpb_rowsH ) ) ) )
-	{
-		Scierror( 322,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 2,"d", &lb_rows,&lb_cols,&lb );
-	if ( !( ( ( lb_rows == qpb_rowsH ) && ( lb_cols == 1 ) ) || ( ( lb_rows == 0 ) && ( lb_cols == 0 ) ) ) )
-	{
-		Scierror( 323,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 3,"d", &ub_rows,&ub_cols,&ub );
-	if ( !( ( ( ub_rows == qpb_rowsH ) && ( ub_cols == 1 ) ) || ( ( ub_rows == 0 ) && ( ub_cols == 0 ) ) ) )
-	{
-		Scierror( 324,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 4,"i", &nWSR_rows,&nWSR_cols,&nWSR );
-	if ( ( nWSR_rows != nWSR_cols ) || ( nWSR_cols != 1 ) )
-	{
-		Scierror( 325,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-
-	CreateVar( 5,"d", &qpb_rowsH,&one,&x );
-	CreateVar( 6,"d", &one,&one,&obj );
-	CreateVar( 7,"i", &one,&one,&status );
-	CreateVar( 8,"i", &one,&one,&nWSRout );
-	CreateVar( 9,"d", &qpb_rowsH,&one,&y );
-
-
-	/* call interfaced qpOASES routines with appropriate arguments */
-	if ( ( lb_rows != 0 ) && ( ub_rows != 0 ) )
-	{
-		hotstartSB(	stk(g),stk(lb),stk(ub),
-					istk(nWSR),
-					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-					);
-	}
-	else if ( ( lb_rows == 0 ) && ( ub_rows != 0 ) )
-	{
-		hotstartSB(	stk(g),0,stk(ub),
-					istk(nWSR),
-					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-					);
-	}
-	else if ( ( lb_rows != 0 ) && ( ub_rows == 0 ) )
-	{
-		hotstartSB(	stk(g),stk(lb),0,
-					istk(nWSR),
-					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-					);
-	}
-	else
-	{
-		hotstartSB(	stk(g),0,0,
-					istk(nWSR),
-					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-					);
-	}
-
-
-	LhsVar(1) = 5;
-	LhsVar(2) = 6;
-	LhsVar(3) = 7;
-	LhsVar(4) = 8;
-	LhsVar(5) = 9;
-
-	return 0;
-}
-
-
-/*
- *	i n t _ h o t s t a r t V M
- */
-int int_hotstartVM( char* fname )
-{
-	int H, H_rows, H_cols;
-	int g, g_rows, g_cols;
-	int A, A_rows, A_cols;
-	int lb, lb_rows, lb_cols;
-	int ub, ub_rows, ub_cols;
-	int lbA, lbA_rows, lbA_cols;
-	int ubA, ubA_rows, ubA_cols;
-	int nWSR, nWSR_rows, nWSR_cols;
-
-	int obj, x, y, status, nWSRout;
-
-
-	int minlhs = 1, maxlhs = 5, minrhs = 8, maxrhs = 8, one = 1, y_size;
-
-	CheckRhs( minrhs,maxrhs );
-	CheckLhs( minlhs,maxlhs );
-
-
-	if ( ( sqp_rowsH == -1 ) || ( sqp_rowsA == -1 ) )
-	{
-		Scierror( 331,"ERROR (qpOASES): Need to call qpOASES_init first!\n" );
-		return 0;
-	}
-
-	/* check dimensions */
-	GetRhsVar( 1,"d", &H_rows,&H_cols,&H );
-	if ( ( H_rows != H_cols ) || ( H_rows < 1 ) )
-	{
-		Scierror( 332,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 2,"d", &g_rows,&g_cols,&g );
-	if ( !( ( ( g_rows == H_rows ) && ( g_cols == 1 ) ) || ( ( g_rows == 1 ) && ( g_cols == H_rows ) ) ) )
-	{
-		Scierror( 333,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 3,"d", &A_rows,&A_cols,&A );
-	if ( ( A_cols != H_rows ) || ( A_rows < 1 ) )
-	{
-		Scierror( 334,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 4,"d", &lb_rows,&lb_cols,&lb);
-	if ( !( ( ( lb_rows == H_rows ) && ( lb_cols == 1 ) ) || ( ( lb_rows == 0 ) && ( lb_cols == 0 ) ) ) )
-	{
-		Scierror( 335,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 5,"d", &ub_rows,&ub_cols,&ub);
-	if ( !( ( ( ub_rows == H_rows ) && ( ub_cols == 1 ) ) || ( ( ub_rows == 0 ) && ( ub_cols == 0 ) ) ) )
-	{
-		Scierror( 399,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 6,"d", &lbA_rows,&lbA_cols,&lbA);
-	if ( ( lbA_rows != A_rows ) || ( lbA_cols != 1 ) )
-	{
-		Scierror( 336,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 7,"d", &ubA_rows,&ubA_cols,&ubA);
-	if ( ( ubA_rows != A_rows ) || ( ubA_cols != 1 ) )
-	{
-		Scierror( 337,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	GetRhsVar( 8,"i", &nWSR_rows,&nWSR_cols,&nWSR);
-	if ( ( nWSR_rows != nWSR_cols ) || ( nWSR_cols != 1 ) )
-	{
-		Scierror( 338,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-	/* have matrices same dimension as last QP? */
-	if ( ( sqp_rowsH != H_rows ) || ( sqp_rowsA != A_rows ) )
-	{
-		Scierror( 339,"ERROR (qpOASES): Dimension mismatch!\n" );
-		return 0;
-	}
-
-
-	y_size = H_rows + A_rows;
-
-	CreateVar(  9,"d", &H_rows,&one,&x );
-	CreateVar( 10,"d", &one,&one,&obj );
-	CreateVar( 11,"i", &one,&one,&status );
-	CreateVar( 12,"i", &one,&one,&nWSRout );
-	CreateVar( 13,"d", &y_size,&one,&y );
-
-
-	/* call interfaced qpOASES routines with appropriate arguments */
-	if ( ( lb_rows != 0 ) && ( ub_rows != 0 ) )
-	{
-		hotstartVM(	stk(H),stk(g),stk(A),stk(lb),stk(ub),stk(lbA),stk(ubA),
-					istk(nWSR),
-					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-					);
-	}
-	else if ( ( lb_rows == 0 ) && ( ub_rows != 0 ) )
-	{
-		hotstartVM(	stk(H),stk(g),stk(A),0,stk(ub),stk(lbA),stk(ubA),
-					istk(nWSR),
-					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-					);
-	}
-	else if ( ( lb_rows != 0 ) && ( ub_rows == 0 ) )
-	{
-		hotstartVM(	stk(H),stk(g),stk(A),stk(lb),0,stk(lbA),stk(ubA),
-					istk(nWSR),
-					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-					);
-	}
-	else
-	{
-		hotstartVM(	stk(H),stk(g),stk(A),0,0,stk(lbA),stk(ubA),
-					istk(nWSR),
-					stk(x),stk(obj),istk(status),istk(nWSRout),stk(y)
-					);
-	}
-
-
-	LhsVar(1) = 9;
-	LhsVar(2) = 10;
-	LhsVar(3) = 11;
-	LhsVar(4) = 12;
-	LhsVar(5) = 13;
-
-	return 0;
-}
-
-
-/*
- *	i n t _ c l e a n u p
- */
-int int_cleanup( char* fname )
-{
-	const int minlhs = 0, maxlhs = 1, minrhs = 0, maxrhs = 0;
-
-	CheckRhs( minrhs,maxrhs );
-	CheckLhs( minlhs,maxlhs );
-
-	cleanupp( );
-	qp_rowsH = -1;
-	qp_rowsA = -1;
-
-	return 0;
-}
-
-
-/*
- *	i n t _ c l e a n u p S B
- */
-int int_cleanupSB( char* fname )
-{
-	const int minlhs = 0, maxlhs = 1, minrhs = 0, maxrhs = 0;
-
-	CheckRhs( minrhs,maxrhs );
-	CheckLhs( minlhs,maxlhs );
-
-	cleanupSB( );
-	qpb_rowsH = -1;
-
-	return 0;
-}
-
-
-/*
- *	i n t _ c l e a n u p V M
- */
-int int_cleanupVM( char* fname )
-{
-	const int minlhs = 0, maxlhs = 1, minrhs = 0, maxrhs = 0;
-
-	CheckRhs( minrhs,maxrhs );
-	CheckLhs( minlhs,maxlhs );
-
-	cleanupVM( );
-	sqp_rowsH = -1;
-	sqp_rowsA = -1;
-
-	return 0;
-}
-
-
-/*
- *	q p O A S E S g a t e w a y
- */
-int C2F(qpOASESgateway)( )
-{
-	gate_function function[] = {	int_qpOASES,
-									int_init, int_initSB, int_initVM,
-									int_hotstart, int_hotstartSB, int_hotstartVM,
-									int_cleanup, int_cleanupSB, int_cleanupVM
-									};
-	char* name[] = {	"qpOASES",
-						"qpOASES_init", "qpOASES_initSB", "qpOASES_initVM",
-						"qpOASES_hotstart", "qpOASES_hotstartSB", "qpOASES_hotstartVM",
-						"qpOASES_cleanup", "qpOASES_cleanupSB", "qpOASES_cleanupVM"
-						};
-
-	Rhs = Max( 0,Rhs );
-	sci_gateway( name[Fin-1],function[Fin-1] );
-
-	return 0;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESinterface.sce b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESinterface.sce
deleted file mode 100644
index bbe36140e74..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESinterface.sce
+++ /dev/null
@@ -1,41 +0,0 @@
-//
-//	This file is part of qpOASES.
-//
-//	qpOASES -- An Implementation of the Online Active Set Strategy.
-//	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-//	Christian Kirches et al. All rights reserved.
-//
-//	qpOASES 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.
-//
-//	qpOASES 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 qpOASES; if not, write to the Free Software
-//	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-//
-
-
-
-//
-//	Filename:  interfaces/scilab/qpOASESinterface.sci
-//	Author:    Holger Diedam, Hans Joachim Ferreau
-//	Version:   3.0
-//	Date:      2007-2014
-//
-
-
-
-sharedlib = link( './libqpOASESinterface.so' );
-addinter( './libqpOASESinterface.so', 'qpOASESgateway', ["qpOASES","qpOASES_init","qpOASES_initSB","qpOASES_initVM","qpOASES_hotstart","qpOASES_hotstartSB","qpOASES_hotstartVM","qpOASES_cleanup","qpOASES_cleanupSB","qpOASES_cleanupVM"] );
-
-
-
-//
-//	end of file
-//
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESroutines.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESroutines.cpp
deleted file mode 100644
index d84c2e1dd01..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/scilab/qpOASESroutines.cpp
+++ /dev/null
@@ -1,365 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file interfaces/scilab/qpOASESroutines.cpp
- *	\author Holger Diedam, Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Interface that enables to call qpOASES from scilab
- *  (C++ file to provide an interface between the files that
- *  have to be compiled with gcc and the qpOASES library).
- *
- */
-
-
-#include <scilab/Scierror.h>
-
-#include <qpOASES.hpp>
-
-
-USING_NAMESPACE_QPOASES
-
-/* global pointers to qpOASES objects */
-static QProblem*  qp  = 0;
-static QProblemB* qpb = 0;
-static SQProblem* sqp = 0;
-
-
-extern "C"
-{
-	void qpoases(	real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
-					int *nV, int* nC, int* nWSR,
-					real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
-					);
-
-	void init(		real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
-					int* nV, int* nC, int* nWSR,
-					real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
-					);
-	void initSB(	real_t* H, real_t* g, real_t* lb, real_t* ub,
-					int* nV, int* nWSR,
-					real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
-					);
-	void initVM(	real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
-					int* nV, int* nC, int* nWSR,
-					real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
-					);
-
-	void hotstart(		real_t* g, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
-						int* nWSR,
-						real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
-						);
-	void hotstartSB(	real_t* g, real_t* lb, real_t* ub,
-						int* nWSR,
-						real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
-						);
-	void hotstartVM(	real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
-						int* nWSR,
-						real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
-						);
-
-	void cleanupp( );
-	void cleanupSB( );
-	void cleanupVM( );
-} /* extern "C" */
-
-
-
-/*
- *	t r a n s f o r m A
- */
-void transformA( real_t* A, int nV, int nC )
-{
-	int i, j;
-
-	real_t* A_tmp = new real_t[nC*nV];
-
-	for( i=0; i<nV*nC; ++i )
-		A_tmp[i] = A[i];
-
-	for( i=0; i<nC; ++i )
-		for( j=0; j<nV; ++j )
-			A[i*nV + j] = A_tmp[j*nC + i];
-
-	delete[] A_tmp;
-
-	return;
-}
-
-
-/*
- *	q p o a s e s
- */
-void qpoases(	real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
-				int *nV, int* nC, int* nWSR,
-				real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
-				)
-{
-	/* transform A into C style matrix */
-	transformA( A, *nV,*nC );
-
-	/* setup and solve initial QP */
-	QProblem single_qp( *nV,*nC );
-	single_qp.setPrintLevel( PL_LOW );
-	returnValue returnvalue = single_qp.init( H,g,A,lb,ub,lbA,ubA, *nWSR,0 );
-
-	/* assign lhs arguments */
-	single_qp.getPrimalSolution( x );
-	*obj = single_qp.getObjVal( );
-	*status = getSimpleStatus( returnvalue );
-	*nWSRout = *nWSR;
-	single_qp.getDualSolution( y );
-
-	return;
-}
-
-
-/*
- *	i n i t
- */
-void init(	real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
-			int* nV, int* nC, int* nWSR,
-			real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
-			)
-{
-	cleanupp( );
-
-	/* transform A into C style matrix */
-	transformA( A, *nV,*nC );
-
-	/* setup and solve initial QP */
-	qp = new QProblem( *nV,*nC );
-	qp->setPrintLevel( PL_LOW );
-	returnValue returnvalue = qp->init( H,g,A,lb,ub,lbA,ubA, *nWSR,0 );
-
-	/* assign lhs arguments */
-	qp->getPrimalSolution( x );
-	*obj = qp->getObjVal( );
-	*status = getSimpleStatus( returnvalue );
-	*nWSRout = *nWSR;
-	qp->getDualSolution( y );
-
-	return;
-}
-
-
-/*
- *	i n i t S B
- */
-void initSB(	real_t* H, real_t* g, real_t* lb, real_t* ub,
-				int* nV, int* nWSR,
-				real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
-				)
-{
-	cleanupSB( );
-
-	/* setup and solve initial QP */
-	qpb = new QProblemB( *nV );
-	qpb->setPrintLevel( PL_LOW );
-	returnValue returnvalue = qpb->init( H,g,lb,ub, *nWSR,0 );
-
-	/* assign lhs arguments */
-	qpb->getPrimalSolution( x );
-	*obj = qpb->getObjVal( );
-	*status = getSimpleStatus( returnvalue );
-	*nWSRout = *nWSR;
-	qpb->getDualSolution( y );
-
-	return;
-}
-
-
-/*
- *	i n i t V M
- */
-void initVM(	real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
-				int* nV, int* nC, int* nWSR,
-				real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
-				)
-{
-	cleanupVM( );
-
-	/* transform A into C style matrix */
-	transformA( A, *nV,*nC );
-
-	/* setup and solve initial QP */
-	sqp = new SQProblem( *nV,*nC );
-	sqp->setPrintLevel( PL_LOW );
-	returnValue returnvalue = sqp->init( H,g,A,lb,ub,lbA,ubA, *nWSR,0 );
-
-	/* assign lhs arguments */
-	sqp->getPrimalSolution( x );
-	*obj = sqp->getObjVal( );
-	*status = getSimpleStatus( returnvalue );
-	*nWSRout = *nWSR;
-	sqp->getDualSolution( y );
-
-	return;
-}
-
-
-/*
- *	h o t s t a r t
- */
-void hotstart(	real_t* g, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
-				int* nWSR,
-				real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
-				)
-{
-	/* has QP been initialised? */
-	if ( qp == 0 )
-	{
-		*status = -1;
-		Scierror( 999,"ERROR (qpOASES): Need to call qpOASES_init first!\n" );
-		return;
-	}
-
-	/* solve QP */
-	returnValue returnvalue = qp->hotstart( g,lb,ub,lbA,ubA, *nWSR,0 );
-
-	/* assign lhs arguments */
-	qp->getPrimalSolution( x );
-	*obj = qp->getObjVal( );
-	*status = getSimpleStatus( returnvalue );
-	*nWSRout = *nWSR;
-	qp->getDualSolution( y );
-
-	return;
-}
-
-
-/*
- *	h o t s t a r t S B
- */
-void hotstartSB(	real_t* g, real_t* lb, real_t* ub,
-					int* nWSR,
-					real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
-					)
-{
-	/* has QP been initialised? */
-	if ( qpb == 0 )
-	{
-		*status = -1;
-		Scierror( 999,"ERROR (qpOASES): Need to call qpOASES_init first!\n" );
-		return;
-	}
-
-	/* solve QP */
-	returnValue returnvalue = qpb->hotstart( g,lb,ub, *nWSR,0 );
-
-	/* assign lhs arguments */
-	qpb->getPrimalSolution( x );
-	*obj = qpb->getObjVal( );
-	*status = getSimpleStatus( returnvalue );
-	*nWSRout = *nWSR;
-	qpb->getDualSolution( y );
-
-	return;
-}
-
-
-/*
- *	h o t s t a r t V M
- */
-void hotstartVM(	real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
-					int* nWSR,
-					real_t* x, real_t* obj, int* status, int* nWSRout, real_t* y
-					)
-{
-	/* has QP been initialised? */
-	if ( sqp == 0 )
-	{
-		*status = -1;
-		Scierror( 999,"ERROR (qpOASES): Need to call qpOASES_init first!\n" );
-		return;
-	}
-
-	/* transform A into C style matrix */
-	transformA( A, sqp->getNV( ),sqp->getNC( ) );
-
-	/* solve QP */
-	returnValue returnvalue = sqp->hotstart( H,g,A,lb,ub,lbA,ubA, *nWSR,0 );
-
-	/* assign lhs arguments */
-	sqp->getPrimalSolution( x );
-	*obj = sqp->getObjVal( );
-	*status = getSimpleStatus( returnvalue );
-	*nWSRout = *nWSR;
-	sqp->getDualSolution( y );
-
-	return;
-}
-
-
-/*
- *	c l e a n u p p
- */
-void cleanupp( )
-{
-	/* Remark: A function cleanup already exists! */
-	if ( qp != 0 )
-	{
-		delete qp;
-		qp = 0;
-	}
-
-	return;
-}
-
-
-/*
- *	c l e a n u p S B
- */
-void cleanupSB( )
-{
-	if ( qpb != 0 )
-	{
-		delete qpb;
-		qpb = 0;
-	}
-
-	return;
-}
-
-
-/*
- *	c l e a n u p V M
- */
-void cleanupVM( )
-{
-	if ( sqp != 0 )
-	{
-		delete sqp;
-		sqp = 0;
-	}
-
-	return;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_QProblem.mdl b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_QProblem.mdl
deleted file mode 100644
index 6b284674b4d..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_QProblem.mdl
+++ /dev/null
@@ -1,803 +0,0 @@
-Model {
-  Name			  "example_QProblem"
-  Version		  7.4
-  MdlSubVersion		  0
-  GraphicalInterface {
-    NumRootInports	    0
-    NumRootOutports	    0
-    ParameterArgumentNames  ""
-    ComputedModelVersion    "1.65"
-    NumModelReferences	    0
-    NumTestPointedSignals   0
-  }
-  SavedCharacterEncoding  "UTF-8"
-  SaveDefaultBlockParams  on
-  ScopeRefreshTime	  0.035000
-  OverrideScopeRefreshTime on
-  DisableAllScopes	  off
-  DataTypeOverride	  "UseLocalSettings"
-  MinMaxOverflowLogging	  "UseLocalSettings"
-  MinMaxOverflowArchiveMode "Overwrite"
-  MaxMDLFileLineLength	  120
-  Created		  "Fri Apr 13 11:08:51 2007"
-  Creator		  "jferreau"
-  UpdateHistory		  "UpdateHistoryNever"
-  ModifiedByFormat	  "%<Auto>"
-  LastModifiedBy	  "chjofer2"
-  ModifiedDateFormat	  "%<Auto>"
-  LastModifiedDate	  "Thu Feb 20 15:11:24 2014"
-  RTWModifiedTimeStamp	  314809883
-  ModelVersionFormat	  "1.%<AutoIncrement:65>"
-  ConfigurationManager	  "None"
-  SampleTimeColors	  off
-  SampleTimeAnnotations	  off
-  LibraryLinkDisplay	  "none"
-  WideLines		  off
-  ShowLineDimensions	  off
-  ShowPortDataTypes	  off
-  ShowLoopsOnError	  on
-  IgnoreBidirectionalLines off
-  ShowStorageClass	  off
-  ShowTestPointIcons	  on
-  ShowSignalResolutionIcons on
-  ShowViewerIcons	  on
-  SortedOrder		  off
-  ExecutionContextIcon	  off
-  ShowLinearizationAnnotations on
-  BlockNameDataTip	  off
-  BlockParametersDataTip  off
-  BlockDescriptionStringDataTip	off
-  ToolBar		  on
-  StatusBar		  on
-  BrowserShowLibraryLinks off
-  BrowserLookUnderMasks	  off
-  SimulationMode	  "normal"
-  LinearizationMsg	  "none"
-  Profile		  off
-  ParamWorkspaceSource	  "MATLABWorkspace"
-  AccelSystemTargetFile	  "accel.tlc"
-  AccelTemplateMakefile	  "accel_default_tmf"
-  AccelMakeCommand	  "make_rtw"
-  TryForcingSFcnDF	  off
-  RecordCoverage	  off
-  CovPath		  "/"
-  CovSaveName		  "covdata"
-  CovMetricSettings	  "dw"
-  CovNameIncrementing	  off
-  CovHtmlReporting	  on
-  CovForceBlockReductionOff on
-  covSaveCumulativeToWorkspaceVar on
-  CovSaveSingleToWorkspaceVar on
-  CovCumulativeVarName	  "covCumulativeData"
-  CovCumulativeReport	  off
-  CovReportOnPause	  on
-  CovModelRefEnable	  "Off"
-  CovExternalEMLEnable	  off
-  ExtModeBatchMode	  off
-  ExtModeEnableFloating	  on
-  ExtModeTrigType	  "manual"
-  ExtModeTrigMode	  "normal"
-  ExtModeTrigPort	  "1"
-  ExtModeTrigElement	  "any"
-  ExtModeTrigDuration	  1000
-  ExtModeTrigDurationFloating "auto"
-  ExtModeTrigHoldOff	  0
-  ExtModeTrigDelay	  0
-  ExtModeTrigDirection	  "rising"
-  ExtModeTrigLevel	  0
-  ExtModeArchiveMode	  "off"
-  ExtModeAutoIncOneShot	  off
-  ExtModeIncDirWhenArm	  off
-  ExtModeAddSuffixToVar	  off
-  ExtModeWriteAllDataToWs off
-  ExtModeArmWhenConnect	  on
-  ExtModeSkipDownloadWhenConnect off
-  ExtModeLogAll		  on
-  ExtModeAutoUpdateStatusClock off
-  BufferReuse		  on
-  ShowModelReferenceBlockVersion off
-  ShowModelReferenceBlockIO off
-  Array {
-    Type		    "Handle"
-    Dimension		    1
-    Simulink.ConfigSet {
-      $ObjectID		      1
-      Version		      "1.6.0"
-      Array {
-	Type			"Handle"
-	Dimension		8
-	Simulink.SolverCC {
-	  $ObjectID		  2
-	  Version		  "1.6.0"
-	  StartTime		  "0.0"
-	  StopTime		  "0.5"
-	  AbsTol		  "auto"
-	  FixedStep		  "auto"
-	  InitialStep		  "auto"
-	  MaxNumMinSteps	  "-1"
-	  MaxOrder		  5
-	  ZcThreshold		  "auto"
-	  ConsecutiveZCsStepRelTol "10*128*eps"
-	  MaxConsecutiveZCs	  "1000"
-	  ExtrapolationOrder	  4
-	  NumberNewtonIterations  1
-	  MaxStep		  "auto"
-	  MinStep		  "auto"
-	  MaxConsecutiveMinStep	  "1"
-	  RelTol		  "1e-3"
-	  SolverMode		  "Auto"
-	  Solver		  "FixedStepDiscrete"
-	  SolverName		  "FixedStepDiscrete"
-	  ShapePreserveControl	  "DisableAll"
-	  ZeroCrossControl	  "UseLocalSettings"
-	  ZeroCrossAlgorithm	  "Nonadaptive"
-	  AlgebraicLoopSolver	  "TrustRegion"
-	  SolverResetMethod	  "Fast"
-	  PositivePriorityOrder	  off
-	  AutoInsertRateTranBlk	  off
-	  SampleTimeConstraint	  "Unconstrained"
-	  InsertRTBMode		  "Whenever possible"
-	}
-	Simulink.DataIOCC {
-	  $ObjectID		  3
-	  Version		  "1.6.0"
-	  Decimation		  "1"
-	  ExternalInput		  "[t, u]"
-	  FinalStateName	  "xFinal"
-	  InitialState		  "xInitial"
-	  LimitDataPoints	  on
-	  MaxDataPoints		  "1000"
-	  LoadExternalInput	  off
-	  LoadInitialState	  off
-	  SaveFinalState	  off
-	  SaveCompleteFinalSimState off
-	  SaveFormat		  "Array"
-	  SaveOutput		  on
-	  SaveState		  off
-	  SignalLogging		  on
-	  InspectSignalLogs	  off
-	  SaveTime		  on
-	  ReturnWorkspaceOutputs  off
-	  StateSaveName		  "xout"
-	  TimeSaveName		  "tout"
-	  OutputSaveName	  "yout"
-	  SignalLoggingName	  "logsout"
-	  OutputOption		  "RefineOutputTimes"
-	  OutputTimes		  "[]"
-	  ReturnWorkspaceOutputsName "out"
-	  Refine		  "1"
-	}
-	Simulink.OptimizationCC {
-	  $ObjectID		  4
-	  Version		  "1.6.0"
-	  Array {
-	    Type		    "Cell"
-	    Dimension		    4
-	    Cell		    "ZeroExternalMemoryAtStartup"
-	    Cell		    "ZeroInternalMemoryAtStartup"
-	    Cell		    "NoFixptDivByZeroProtection"
-	    Cell		    "OptimizeModelRefInitCode"
-	    PropName		    "DisabledProps"
-	  }
-	  BlockReduction	  on
-	  BooleanDataType	  on
-	  ConditionallyExecuteInputs on
-	  InlineParams		  off
-	  UseIntDivNetSlope	  off
-	  InlineInvariantSignals  off
-	  OptimizeBlockIOStorage  on
-	  BufferReuse		  on
-	  EnhancedBackFolding	  off
-	  StrengthReduction	  off
-	  EnforceIntegerDowncast  on
-	  ExpressionFolding	  on
-	  BooleansAsBitfields	  off
-	  EnableMemcpy		  on
-	  MemcpyThreshold	  64
-	  PassReuseOutputArgsAs	  "Structure reference"
-	  ExpressionDepthLimit	  2147483647
-	  FoldNonRolledExpr	  on
-	  LocalBlockOutputs	  on
-	  RollThreshold		  5
-	  SystemCodeInlineAuto	  off
-	  StateBitsets		  off
-	  DataBitsets		  off
-	  UseTempVars		  off
-	  ZeroExternalMemoryAtStartup on
-	  ZeroInternalMemoryAtStartup on
-	  InitFltsAndDblsToZero	  on
-	  NoFixptDivByZeroProtection off
-	  EfficientFloat2IntCast  off
-	  EfficientMapNaN2IntZero on
-	  OptimizeModelRefInitCode off
-	  LifeSpan		  "inf"
-	  BufferReusableBoundary  on
-	  SimCompilerOptimization "Off"
-	  AccelVerboseBuild	  off
-	}
-	Simulink.DebuggingCC {
-	  $ObjectID		  5
-	  Version		  "1.6.0"
-	  RTPrefix		  "error"
-	  ConsistencyChecking	  "none"
-	  ArrayBoundsChecking	  "none"
-	  SignalInfNanChecking	  "none"
-	  SignalRangeChecking	  "none"
-	  ReadBeforeWriteMsg	  "UseLocalSettings"
-	  WriteAfterWriteMsg	  "UseLocalSettings"
-	  WriteAfterReadMsg	  "UseLocalSettings"
-	  AlgebraicLoopMsg	  "warning"
-	  ArtificialAlgebraicLoopMsg "warning"
-	  SaveWithDisabledLinksMsg "warning"
-	  SaveWithParameterizedLinksMsg	"none"
-	  CheckSSInitialOutputMsg on
-	  UnderspecifiedInitializationDetection	"Classic"
-	  MergeDetectMultiDrivingBlocksExec "none"
-	  CheckExecutionContextPreStartOutputMsg off
-	  CheckExecutionContextRuntimeOutputMsg	off
-	  SignalResolutionControl "TryResolveAllWithWarning"
-	  BlockPriorityViolationMsg "warning"
-	  MinStepSizeMsg	  "warning"
-	  TimeAdjustmentMsg	  "none"
-	  MaxConsecutiveZCsMsg	  "error"
-	  SolverPrmCheckMsg	  "warning"
-	  InheritedTsInSrcMsg	  "warning"
-	  DiscreteInheritContinuousMsg "warning"
-	  MultiTaskDSMMsg	  "error"
-	  MultiTaskCondExecSysMsg "error"
-	  MultiTaskRateTransMsg	  "error"
-	  SingleTaskRateTransMsg  "none"
-	  TasksWithSamePriorityMsg "warning"
-	  SigSpecEnsureSampleTimeMsg "warning"
-	  CheckMatrixSingularityMsg "none"
-	  IntegerOverflowMsg	  "warning"
-	  Int32ToFloatConvMsg	  "warning"
-	  ParameterDowncastMsg	  "error"
-	  ParameterOverflowMsg	  "error"
-	  ParameterUnderflowMsg	  "none"
-	  ParameterPrecisionLossMsg "warning"
-	  ParameterTunabilityLossMsg "warning"
-	  FixptConstUnderflowMsg  "none"
-	  FixptConstOverflowMsg	  "none"
-	  FixptConstPrecisionLossMsg "none"
-	  UnderSpecifiedDataTypeMsg "none"
-	  UnnecessaryDatatypeConvMsg "none"
-	  VectorMatrixConversionMsg "none"
-	  InvalidFcnCallConnMsg	  "error"
-	  FcnCallInpInsideContextMsg "Use local settings"
-	  SignalLabelMismatchMsg  "none"
-	  UnconnectedInputMsg	  "warning"
-	  UnconnectedOutputMsg	  "warning"
-	  UnconnectedLineMsg	  "warning"
-	  SFcnCompatibilityMsg	  "none"
-	  UniqueDataStoreMsg	  "none"
-	  BusObjectLabelMismatch  "warning"
-	  RootOutportRequireBusObject "warning"
-	  AssertControl		  "UseLocalSettings"
-	  EnableOverflowDetection off
-	  ModelReferenceIOMsg	  "none"
-	  ModelReferenceVersionMismatchMessage "none"
-	  ModelReferenceIOMismatchMessage "none"
-	  ModelReferenceCSMismatchMessage "none"
-	  UnknownTsInhSupMsg	  "warning"
-	  ModelReferenceDataLoggingMessage "warning"
-	  ModelReferenceSymbolNameMessage "warning"
-	  ModelReferenceExtraNoncontSigs "error"
-	  StateNameClashWarn	  "warning"
-	  SimStateInterfaceChecksumMismatchMsg "warning"
-	  StrictBusMsg		  "Warning"
-	  LoggingUnavailableSignals "error"
-	  BlockIODiagnostic	  "none"
-	}
-	Simulink.HardwareCC {
-	  $ObjectID		  6
-	  Version		  "1.6.0"
-	  ProdBitPerChar	  8
-	  ProdBitPerShort	  16
-	  ProdBitPerInt		  32
-	  ProdBitPerLong	  32
-	  ProdIntDivRoundTo	  "Undefined"
-	  ProdEndianess		  "Unspecified"
-	  ProdWordSize		  32
-	  ProdShiftRightIntArith  on
-	  ProdHWDeviceType	  "32-bit Generic"
-	  TargetBitPerChar	  8
-	  TargetBitPerShort	  16
-	  TargetBitPerInt	  32
-	  TargetBitPerLong	  32
-	  TargetShiftRightIntArith on
-	  TargetIntDivRoundTo	  "Undefined"
-	  TargetEndianess	  "Unspecified"
-	  TargetWordSize	  32
-	  TargetTypeEmulationWarnSuppressLevel 0
-	  TargetPreprocMaxBitsSint 32
-	  TargetPreprocMaxBitsUint 32
-	  TargetHWDeviceType	  "Specified"
-	  TargetUnknown		  off
-	  ProdEqTarget		  on
-	}
-	Simulink.ModelReferenceCC {
-	  $ObjectID		  7
-	  Version		  "1.6.0"
-	  UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange"
-	  CheckModelReferenceTargetMessage "error"
-	  ModelReferenceNumInstancesAllowed "Multi"
-	  ModelReferencePassRootInputsByReference on
-	  ModelReferenceMinAlgLoopOccurrences off
-	}
-	Simulink.SFSimCC {
-	  $ObjectID		  8
-	  Version		  "1.6.0"
-	  SFSimEnableDebug	  on
-	  SFSimOverflowDetection  on
-	  SFSimEcho		  on
-	  SimBlas		  on
-	  SimCtrlC		  on
-	  SimExtrinsic		  on
-	  SimIntegrity		  on
-	  SimUseLocalCustomCode	  off
-	  SimBuildMode		  "sf_incremental_build"
-	}
-	Simulink.RTWCC {
-	  $BackupClass		  "Simulink.RTWCC"
-	  $ObjectID		  9
-	  Version		  "1.6.0"
-	  Array {
-	    Type		    "Cell"
-	    Dimension		    1
-	    Cell		    "IncludeHyperlinkInReport"
-	    PropName		    "DisabledProps"
-	  }
-	  SystemTargetFile	  "grt.tlc"
-	  GenCodeOnly		  off
-	  MakeCommand		  "make_rtw"
-	  GenerateMakefile	  on
-	  TemplateMakefile	  "grt_default_tmf"
-	  GenerateReport	  off
-	  SaveLog		  off
-	  RTWVerbose		  on
-	  RetainRTWFile		  off
-	  ProfileTLC		  off
-	  TLCDebug		  off
-	  TLCCoverage		  off
-	  TLCAssert		  off
-	  ProcessScriptMode	  "Default"
-	  ConfigurationMode	  "Optimized"
-	  ConfigAtBuild		  off
-	  RTWUseLocalCustomCode	  off
-	  RTWUseSimCustomCode	  off
-	  IncludeHyperlinkInReport off
-	  LaunchReport		  off
-	  TargetLang		  "C"
-	  IncludeBusHierarchyInRTWFileBlockHierarchyMap	off
-	  IncludeERTFirstTime	  off
-	  GenerateTraceInfo	  off
-	  GenerateTraceReport	  off
-	  GenerateTraceReportSl	  off
-	  GenerateTraceReportSf	  off
-	  GenerateTraceReportEml  off
-	  GenerateCodeInfo	  off
-	  RTWCompilerOptimization "Off"
-	  CheckMdlBeforeBuild	  "Off"
-	  Array {
-	    Type		    "Handle"
-	    Dimension		    2
-	    Simulink.CodeAppCC {
-	      $ObjectID		      10
-	      Version		      "1.6.0"
-	      Array {
-		Type			"Cell"
-		Dimension		16
-		Cell			"IgnoreCustomStorageClasses"
-		Cell			"InsertBlockDesc"
-		Cell			"SFDataObjDesc"
-		Cell			"SimulinkDataObjDesc"
-		Cell			"DefineNamingRule"
-		Cell			"SignalNamingRule"
-		Cell			"ParamNamingRule"
-		Cell			"InlinedPrmAccess"
-		Cell			"CustomSymbolStr"
-		Cell			"CustomSymbolStrGlobalVar"
-		Cell			"CustomSymbolStrType"
-		Cell			"CustomSymbolStrField"
-		Cell			"CustomSymbolStrFcn"
-		Cell			"CustomSymbolStrBlkIO"
-		Cell			"CustomSymbolStrTmpVar"
-		Cell			"CustomSymbolStrMacro"
-		PropName		"DisabledProps"
-	      }
-	      ForceParamTrailComments off
-	      GenerateComments	      on
-	      IgnoreCustomStorageClasses on
-	      IgnoreTestpoints	      off
-	      IncHierarchyInIds	      off
-	      MaxIdLength	      31
-	      PreserveName	      off
-	      PreserveNameWithParent  off
-	      ShowEliminatedStatement off
-	      IncAutoGenComments      off
-	      SimulinkDataObjDesc     off
-	      SFDataObjDesc	      off
-	      IncDataTypeInIds	      off
-	      MangleLength	      1
-	      CustomSymbolStrGlobalVar "$R$N$M"
-	      CustomSymbolStrType     "$N$R$M"
-	      CustomSymbolStrField    "$N$M"
-	      CustomSymbolStrFcn      "$R$N$M$F"
-	      CustomSymbolStrFcnArg   "rt$I$N$M"
-	      CustomSymbolStrBlkIO    "rtb_$N$M"
-	      CustomSymbolStrTmpVar   "$N$M"
-	      CustomSymbolStrMacro    "$R$N$M"
-	      DefineNamingRule	      "None"
-	      ParamNamingRule	      "None"
-	      SignalNamingRule	      "None"
-	      InsertBlockDesc	      off
-	      SimulinkBlockComments   on
-	      EnableCustomComments    off
-	      InlinedPrmAccess	      "Literals"
-	      ReqsInCode	      off
-	      UseSimReservedNames     off
-	    }
-	    Simulink.GRTTargetCC {
-	      $BackupClass	      "Simulink.TargetCC"
-	      $ObjectID		      11
-	      Version		      "1.6.0"
-	      Array {
-		Type			"Cell"
-		Dimension		13
-		Cell			"IncludeMdlTerminateFcn"
-		Cell			"CombineOutputUpdateFcns"
-		Cell			"SuppressErrorStatus"
-		Cell			"ERTCustomFileBanners"
-		Cell			"GenerateSampleERTMain"
-		Cell			"GenerateTestInterfaces"
-		Cell			"MultiInstanceERTCode"
-		Cell			"PurelyIntegerCode"
-		Cell			"SupportNonFinite"
-		Cell			"SupportComplex"
-		Cell			"SupportAbsoluteTime"
-		Cell			"SupportContinuousTime"
-		Cell			"SupportNonInlinedSFcns"
-		PropName		"DisabledProps"
-	      }
-	      TargetFcnLib	      "ansi_tfl_tmw.mat"
-	      TargetLibSuffix	      ""
-	      TargetPreCompLibLocation ""
-	      TargetFunctionLibrary   "ANSI_C"
-	      UtilityFuncGeneration   "Auto"
-	      ERTMultiwordTypeDef     "System defined"
-	      ERTMultiwordLength      256
-	      MultiwordLength	      2048
-	      GenerateFullHeader      on
-	      GenerateSampleERTMain   off
-	      GenerateTestInterfaces  off
-	      IsPILTarget	      off
-	      ModelReferenceCompliant on
-	      ParMdlRefBuildCompliant on
-	      CompOptLevelCompliant   on
-	      IncludeMdlTerminateFcn  on
-	      GeneratePreprocessorConditionals "Disable all"
-	      CombineOutputUpdateFcns off
-	      SuppressErrorStatus     off
-	      ERTFirstTimeCompliant   off
-	      IncludeFileDelimiter    "Auto"
-	      ERTCustomFileBanners    off
-	      SupportAbsoluteTime     on
-	      LogVarNameModifier      "rt_"
-	      MatFileLogging	      on
-	      MultiInstanceERTCode    off
-	      SupportNonFinite	      on
-	      SupportComplex	      on
-	      PurelyIntegerCode	      off
-	      SupportContinuousTime   on
-	      SupportNonInlinedSFcns  on
-	      SupportVariableSizeSignals off
-	      EnableShiftOperators    on
-	      ParenthesesLevel	      "Nominal"
-	      PortableWordSizes	      off
-	      ModelStepFunctionPrototypeControlCompliant off
-	      CPPClassGenCompliant    off
-	      AutosarCompliant	      off
-	      UseMalloc		      off
-	      ExtMode		      off
-	      ExtModeStaticAlloc      off
-	      ExtModeTesting	      off
-	      ExtModeStaticAllocSize  1000000
-	      ExtModeTransport	      0
-	      ExtModeMexFile	      "ext_comm"
-	      ExtModeIntrfLevel	      "Level1"
-	      RTWCAPISignals	      off
-	      RTWCAPIParams	      off
-	      RTWCAPIStates	      off
-	      GenerateASAP2	      off
-	    }
-	    PropName		    "Components"
-	  }
-	}
-	PropName		"Components"
-      }
-      Name		      "Configuration"
-      CurrentDlgPage	      "Solver"
-      ConfigPrmDlgPosition    " [ 200, 197, 1080, 827 ] "
-    }
-    PropName		    "ConfigurationSets"
-  }
-  Simulink.ConfigSet {
-    $PropName		    "ActiveConfigurationSet"
-    $ObjectID		    1
-  }
-  BlockDefaults {
-    ForegroundColor	    "black"
-    BackgroundColor	    "white"
-    DropShadow		    off
-    NamePlacement	    "normal"
-    FontName		    "Courier"
-    FontSize		    10
-    FontWeight		    "normal"
-    FontAngle		    "normal"
-    ShowName		    on
-    BlockRotation	    0
-    BlockMirror		    off
-  }
-  AnnotationDefaults {
-    HorizontalAlignment	    "center"
-    VerticalAlignment	    "middle"
-    ForegroundColor	    "black"
-    BackgroundColor	    "white"
-    DropShadow		    off
-    FontName		    "Courier"
-    FontSize		    10
-    FontWeight		    "normal"
-    FontAngle		    "normal"
-    UseDisplayTextAsClickCallback off
-  }
-  LineDefaults {
-    FontName		    "Courier"
-    FontSize		    9
-    FontWeight		    "normal"
-    FontAngle		    "normal"
-  }
-  BlockParameterDefaults {
-    Block {
-      BlockType		      FromWorkspace
-      VariableName	      "simulink_input"
-      SampleTime	      "-1"
-      Interpolate	      on
-      ZeroCross		      off
-      OutputAfterFinalValue   "Extrapolation"
-    }
-    Block {
-      BlockType		      "S-Function"
-      FunctionName	      "system"
-      SFunctionModules	      "''"
-      PortCounts	      "[]"
-      SFunctionDeploymentMode off
-    }
-    Block {
-      BlockType		      Scope
-      ModelBased	      off
-      TickLabels	      "OneTimeTick"
-      ZoomMode		      "on"
-      Grid		      "on"
-      TimeRange		      "auto"
-      YMin		      "-5"
-      YMax		      "5"
-      SaveToWorkspace	      off
-      SaveName		      "ScopeData"
-      LimitDataPoints	      on
-      MaxDataPoints	      "5000"
-      Decimation	      "1"
-      SampleInput	      off
-      SampleTime	      "-1"
-    }
-  }
-  System {
-    Name		    "example_QProblem"
-    Location		    [2, 82, 1918, 1147]
-    Open		    on
-    ModelBrowserVisibility  off
-    ModelBrowserWidth	    200
-    ScreenColor		    "white"
-    PaperOrientation	    "landscape"
-    PaperPositionMode	    "auto"
-    PaperType		    "usletter"
-    PaperUnits		    "inches"
-    TiledPaperMargins	    [0.500000, 0.500000, 0.500000, 0.500000]
-    TiledPageScale	    1
-    ShowPageBoundaries	    off
-    ZoomFactor		    "125"
-    ReportName		    "simulink-default.rpt"
-    SIDHighWatermark	    9
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace1"
-      SID		      1
-      Position		      [85, 53, 150, 77]
-      VariableName	      "H"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace2"
-      SID		      2
-      Position		      [85, 113, 150, 137]
-      VariableName	      "g"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace3"
-      SID		      3
-      Position		      [85, 173, 150, 197]
-      VariableName	      "A"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace4"
-      SID		      4
-      Position		      [85, 233, 150, 257]
-      VariableName	      "lb"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace5"
-      SID		      5
-      Position		      [85, 293, 150, 317]
-      VariableName	      "ub"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace6"
-      SID		      6
-      Position		      [85, 353, 150, 377]
-      VariableName	      "lbA"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace7"
-      SID		      7
-      Position		      [85, 418, 150, 442]
-      VariableName	      "ubA"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      Scope
-      Name		      "fval,\nx,\nexitflag,\niter"
-      SID		      8
-      Ports		      [4]
-      Position		      [915, 229, 945, 261]
-      Floating		      off
-      Location		      [6, 78, 1276, 993]
-      Open		      off
-      NumInputPorts	      "4"
-      List {
-	ListType		AxesTitles
-	axes1			"%<SignalLabel>"
-	axes2			"%<SignalLabel>"
-	axes3			"%<SignalLabel>"
-	axes4			"%<SignalLabel>"
-      }
-      YMin		      "-5~-5~-5~-5"
-      YMax		      "5~5~5~5"
-      SaveName		      "ScopeData1"
-      DataFormat	      "StructureWithTime"
-      SampleTime	      "0"
-    }
-    Block {
-      BlockType		      "S-Function"
-      Name		      "qpOASES"
-      SID		      9
-      Ports		      [7, 4]
-      Position		      [595, 226, 790, 264]
-      BackgroundColor	      "[1.000000, 0.915850, 0.439000]"
-      FunctionName	      "qpOASES_QProblem"
-      EnableBusSupport	      off
-      MaskHelp		      "file://../../DOC/manual.pdf"
-      MaskIconFrame	      on
-      MaskIconOpaque	      on
-      MaskIconRotate	      "none"
-      MaskPortRotate	      "default"
-      MaskIconUnits	      "autoscale"
-    }
-    Line {
-      SrcBlock		      "From\nWorkspace1"
-      SrcPort		      1
-      Points		      [180, 0; 0, 165]
-      DstBlock		      "qpOASES"
-      DstPort		      1
-    }
-    Line {
-      SrcBlock		      "From\nWorkspace2"
-      SrcPort		      1
-      Points		      [115, 0; 0, 110]
-      DstBlock		      "qpOASES"
-      DstPort		      2
-    }
-    Line {
-      Labels		      [3, 0]
-      SrcBlock		      "From\nWorkspace7"
-      SrcPort		      1
-      Points		      [180, 0; 0, -170]
-      DstBlock		      "qpOASES"
-      DstPort		      7
-    }
-    Line {
-      SrcBlock		      "From\nWorkspace5"
-      SrcPort		      1
-      Points		      [55, 0; 0, -55]
-      DstBlock		      "qpOASES"
-      DstPort		      5
-    }
-    Line {
-      SrcBlock		      "From\nWorkspace4"
-      SrcPort		      1
-      DstBlock		      "qpOASES"
-      DstPort		      4
-    }
-    Line {
-      SrcBlock		      "From\nWorkspace3"
-      SrcPort		      1
-      Points		      [55, 0; 0, 55]
-      DstBlock		      "qpOASES"
-      DstPort		      3
-    }
-    Line {
-      Labels		      [3, 0]
-      SrcBlock		      "From\nWorkspace6"
-      SrcPort		      1
-      Points		      [115, 0; 0, -110]
-      DstBlock		      "qpOASES"
-      DstPort		      6
-    }
-    Line {
-      SrcBlock		      "qpOASES"
-      SrcPort		      1
-      DstBlock		      "fval,\nx,\nexitflag,\niter"
-      DstPort		      1
-    }
-    Line {
-      SrcBlock		      "qpOASES"
-      SrcPort		      2
-      DstBlock		      "fval,\nx,\nexitflag,\niter"
-      DstPort		      2
-    }
-    Line {
-      SrcBlock		      "qpOASES"
-      SrcPort		      3
-      DstBlock		      "fval,\nx,\nexitflag,\niter"
-      DstPort		      3
-    }
-    Line {
-      SrcBlock		      "qpOASES"
-      SrcPort		      4
-      DstBlock		      "fval,\nx,\nexitflag,\niter"
-      DstPort		      4
-    }
-    Annotation {
-      Position		      [335, 96]
-    }
-    Annotation {
-      Name		      "This file is part of qpOASES.\n\nqpOASES -- An Implementation of the Online Active Set Strategy.\nC"
-      "opyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka, Christian Kirches et al.\nAll rights reserved."
-      Position		      [693, 128]
-    }
-  }
-}
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_QProblemB.mdl b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_QProblemB.mdl
deleted file mode 100644
index e86ed8e85aa..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_QProblemB.mdl
+++ /dev/null
@@ -1,745 +0,0 @@
-Model {
-  Name			  "example_QProblemB"
-  Version		  7.4
-  MdlSubVersion		  0
-  GraphicalInterface {
-    NumRootInports	    0
-    NumRootOutports	    0
-    ParameterArgumentNames  ""
-    ComputedModelVersion    "1.64"
-    NumModelReferences	    0
-    NumTestPointedSignals   0
-  }
-  SavedCharacterEncoding  "UTF-8"
-  SaveDefaultBlockParams  on
-  ScopeRefreshTime	  0.035000
-  OverrideScopeRefreshTime on
-  DisableAllScopes	  off
-  DataTypeOverride	  "UseLocalSettings"
-  MinMaxOverflowLogging	  "UseLocalSettings"
-  MinMaxOverflowArchiveMode "Overwrite"
-  MaxMDLFileLineLength	  120
-  Created		  "Fri Apr 13 11:08:51 2007"
-  Creator		  "jferreau"
-  UpdateHistory		  "UpdateHistoryNever"
-  ModifiedByFormat	  "%<Auto>"
-  LastModifiedBy	  "chjofer2"
-  ModifiedDateFormat	  "%<Auto>"
-  LastModifiedDate	  "Thu Feb 20 15:12:19 2014"
-  RTWModifiedTimeStamp	  314809937
-  ModelVersionFormat	  "1.%<AutoIncrement:64>"
-  ConfigurationManager	  "None"
-  SampleTimeColors	  off
-  SampleTimeAnnotations	  off
-  LibraryLinkDisplay	  "none"
-  WideLines		  off
-  ShowLineDimensions	  off
-  ShowPortDataTypes	  off
-  ShowLoopsOnError	  on
-  IgnoreBidirectionalLines off
-  ShowStorageClass	  off
-  ShowTestPointIcons	  on
-  ShowSignalResolutionIcons on
-  ShowViewerIcons	  on
-  SortedOrder		  off
-  ExecutionContextIcon	  off
-  ShowLinearizationAnnotations on
-  BlockNameDataTip	  off
-  BlockParametersDataTip  off
-  BlockDescriptionStringDataTip	off
-  ToolBar		  on
-  StatusBar		  on
-  BrowserShowLibraryLinks off
-  BrowserLookUnderMasks	  off
-  SimulationMode	  "normal"
-  LinearizationMsg	  "none"
-  Profile		  off
-  ParamWorkspaceSource	  "MATLABWorkspace"
-  AccelSystemTargetFile	  "accel.tlc"
-  AccelTemplateMakefile	  "accel_default_tmf"
-  AccelMakeCommand	  "make_rtw"
-  TryForcingSFcnDF	  off
-  RecordCoverage	  off
-  CovPath		  "/"
-  CovSaveName		  "covdata"
-  CovMetricSettings	  "dw"
-  CovNameIncrementing	  off
-  CovHtmlReporting	  on
-  CovForceBlockReductionOff on
-  covSaveCumulativeToWorkspaceVar on
-  CovSaveSingleToWorkspaceVar on
-  CovCumulativeVarName	  "covCumulativeData"
-  CovCumulativeReport	  off
-  CovReportOnPause	  on
-  CovModelRefEnable	  "Off"
-  CovExternalEMLEnable	  off
-  ExtModeBatchMode	  off
-  ExtModeEnableFloating	  on
-  ExtModeTrigType	  "manual"
-  ExtModeTrigMode	  "normal"
-  ExtModeTrigPort	  "1"
-  ExtModeTrigElement	  "any"
-  ExtModeTrigDuration	  1000
-  ExtModeTrigDurationFloating "auto"
-  ExtModeTrigHoldOff	  0
-  ExtModeTrigDelay	  0
-  ExtModeTrigDirection	  "rising"
-  ExtModeTrigLevel	  0
-  ExtModeArchiveMode	  "off"
-  ExtModeAutoIncOneShot	  off
-  ExtModeIncDirWhenArm	  off
-  ExtModeAddSuffixToVar	  off
-  ExtModeWriteAllDataToWs off
-  ExtModeArmWhenConnect	  on
-  ExtModeSkipDownloadWhenConnect off
-  ExtModeLogAll		  on
-  ExtModeAutoUpdateStatusClock off
-  BufferReuse		  on
-  ShowModelReferenceBlockVersion off
-  ShowModelReferenceBlockIO off
-  Array {
-    Type		    "Handle"
-    Dimension		    1
-    Simulink.ConfigSet {
-      $ObjectID		      1
-      Version		      "1.6.0"
-      Array {
-	Type			"Handle"
-	Dimension		8
-	Simulink.SolverCC {
-	  $ObjectID		  2
-	  Version		  "1.6.0"
-	  StartTime		  "0.0"
-	  StopTime		  "0.5"
-	  AbsTol		  "auto"
-	  FixedStep		  "auto"
-	  InitialStep		  "auto"
-	  MaxNumMinSteps	  "-1"
-	  MaxOrder		  5
-	  ZcThreshold		  "auto"
-	  ConsecutiveZCsStepRelTol "10*128*eps"
-	  MaxConsecutiveZCs	  "1000"
-	  ExtrapolationOrder	  4
-	  NumberNewtonIterations  1
-	  MaxStep		  "auto"
-	  MinStep		  "auto"
-	  MaxConsecutiveMinStep	  "1"
-	  RelTol		  "1e-3"
-	  SolverMode		  "Auto"
-	  Solver		  "VariableStepDiscrete"
-	  SolverName		  "VariableStepDiscrete"
-	  ShapePreserveControl	  "DisableAll"
-	  ZeroCrossControl	  "UseLocalSettings"
-	  ZeroCrossAlgorithm	  "Nonadaptive"
-	  AlgebraicLoopSolver	  "TrustRegion"
-	  SolverResetMethod	  "Fast"
-	  PositivePriorityOrder	  off
-	  AutoInsertRateTranBlk	  off
-	  SampleTimeConstraint	  "Unconstrained"
-	  InsertRTBMode		  "Whenever possible"
-	}
-	Simulink.DataIOCC {
-	  $ObjectID		  3
-	  Version		  "1.6.0"
-	  Decimation		  "1"
-	  ExternalInput		  "[t, u]"
-	  FinalStateName	  "xFinal"
-	  InitialState		  "xInitial"
-	  LimitDataPoints	  on
-	  MaxDataPoints		  "1000"
-	  LoadExternalInput	  off
-	  LoadInitialState	  off
-	  SaveFinalState	  off
-	  SaveCompleteFinalSimState off
-	  SaveFormat		  "Array"
-	  SaveOutput		  on
-	  SaveState		  off
-	  SignalLogging		  on
-	  InspectSignalLogs	  off
-	  SaveTime		  on
-	  ReturnWorkspaceOutputs  off
-	  StateSaveName		  "xout"
-	  TimeSaveName		  "tout"
-	  OutputSaveName	  "yout"
-	  SignalLoggingName	  "logsout"
-	  OutputOption		  "RefineOutputTimes"
-	  OutputTimes		  "[]"
-	  ReturnWorkspaceOutputsName "out"
-	  Refine		  "1"
-	}
-	Simulink.OptimizationCC {
-	  $ObjectID		  4
-	  Version		  "1.6.0"
-	  Array {
-	    Type		    "Cell"
-	    Dimension		    4
-	    Cell		    "ZeroExternalMemoryAtStartup"
-	    Cell		    "ZeroInternalMemoryAtStartup"
-	    Cell		    "NoFixptDivByZeroProtection"
-	    Cell		    "OptimizeModelRefInitCode"
-	    PropName		    "DisabledProps"
-	  }
-	  BlockReduction	  on
-	  BooleanDataType	  on
-	  ConditionallyExecuteInputs on
-	  InlineParams		  off
-	  UseIntDivNetSlope	  off
-	  InlineInvariantSignals  off
-	  OptimizeBlockIOStorage  on
-	  BufferReuse		  on
-	  EnhancedBackFolding	  off
-	  StrengthReduction	  off
-	  EnforceIntegerDowncast  on
-	  ExpressionFolding	  on
-	  BooleansAsBitfields	  off
-	  EnableMemcpy		  on
-	  MemcpyThreshold	  64
-	  PassReuseOutputArgsAs	  "Structure reference"
-	  ExpressionDepthLimit	  2147483647
-	  FoldNonRolledExpr	  on
-	  LocalBlockOutputs	  on
-	  RollThreshold		  5
-	  SystemCodeInlineAuto	  off
-	  StateBitsets		  off
-	  DataBitsets		  off
-	  UseTempVars		  off
-	  ZeroExternalMemoryAtStartup on
-	  ZeroInternalMemoryAtStartup on
-	  InitFltsAndDblsToZero	  on
-	  NoFixptDivByZeroProtection off
-	  EfficientFloat2IntCast  off
-	  EfficientMapNaN2IntZero on
-	  OptimizeModelRefInitCode off
-	  LifeSpan		  "inf"
-	  BufferReusableBoundary  on
-	  SimCompilerOptimization "Off"
-	  AccelVerboseBuild	  off
-	}
-	Simulink.DebuggingCC {
-	  $ObjectID		  5
-	  Version		  "1.6.0"
-	  RTPrefix		  "error"
-	  ConsistencyChecking	  "none"
-	  ArrayBoundsChecking	  "none"
-	  SignalInfNanChecking	  "none"
-	  SignalRangeChecking	  "none"
-	  ReadBeforeWriteMsg	  "UseLocalSettings"
-	  WriteAfterWriteMsg	  "UseLocalSettings"
-	  WriteAfterReadMsg	  "UseLocalSettings"
-	  AlgebraicLoopMsg	  "warning"
-	  ArtificialAlgebraicLoopMsg "warning"
-	  SaveWithDisabledLinksMsg "warning"
-	  SaveWithParameterizedLinksMsg	"none"
-	  CheckSSInitialOutputMsg on
-	  UnderspecifiedInitializationDetection	"Classic"
-	  MergeDetectMultiDrivingBlocksExec "none"
-	  CheckExecutionContextPreStartOutputMsg off
-	  CheckExecutionContextRuntimeOutputMsg	off
-	  SignalResolutionControl "TryResolveAllWithWarning"
-	  BlockPriorityViolationMsg "warning"
-	  MinStepSizeMsg	  "warning"
-	  TimeAdjustmentMsg	  "none"
-	  MaxConsecutiveZCsMsg	  "error"
-	  SolverPrmCheckMsg	  "warning"
-	  InheritedTsInSrcMsg	  "warning"
-	  DiscreteInheritContinuousMsg "warning"
-	  MultiTaskDSMMsg	  "error"
-	  MultiTaskCondExecSysMsg "error"
-	  MultiTaskRateTransMsg	  "error"
-	  SingleTaskRateTransMsg  "none"
-	  TasksWithSamePriorityMsg "warning"
-	  SigSpecEnsureSampleTimeMsg "warning"
-	  CheckMatrixSingularityMsg "none"
-	  IntegerOverflowMsg	  "warning"
-	  Int32ToFloatConvMsg	  "warning"
-	  ParameterDowncastMsg	  "error"
-	  ParameterOverflowMsg	  "error"
-	  ParameterUnderflowMsg	  "none"
-	  ParameterPrecisionLossMsg "warning"
-	  ParameterTunabilityLossMsg "warning"
-	  FixptConstUnderflowMsg  "none"
-	  FixptConstOverflowMsg	  "none"
-	  FixptConstPrecisionLossMsg "none"
-	  UnderSpecifiedDataTypeMsg "none"
-	  UnnecessaryDatatypeConvMsg "none"
-	  VectorMatrixConversionMsg "none"
-	  InvalidFcnCallConnMsg	  "error"
-	  FcnCallInpInsideContextMsg "Use local settings"
-	  SignalLabelMismatchMsg  "none"
-	  UnconnectedInputMsg	  "warning"
-	  UnconnectedOutputMsg	  "warning"
-	  UnconnectedLineMsg	  "warning"
-	  SFcnCompatibilityMsg	  "none"
-	  UniqueDataStoreMsg	  "none"
-	  BusObjectLabelMismatch  "warning"
-	  RootOutportRequireBusObject "warning"
-	  AssertControl		  "UseLocalSettings"
-	  EnableOverflowDetection off
-	  ModelReferenceIOMsg	  "none"
-	  ModelReferenceVersionMismatchMessage "none"
-	  ModelReferenceIOMismatchMessage "none"
-	  ModelReferenceCSMismatchMessage "none"
-	  UnknownTsInhSupMsg	  "warning"
-	  ModelReferenceDataLoggingMessage "warning"
-	  ModelReferenceSymbolNameMessage "warning"
-	  ModelReferenceExtraNoncontSigs "error"
-	  StateNameClashWarn	  "warning"
-	  SimStateInterfaceChecksumMismatchMsg "warning"
-	  StrictBusMsg		  "Warning"
-	  LoggingUnavailableSignals "error"
-	  BlockIODiagnostic	  "none"
-	}
-	Simulink.HardwareCC {
-	  $ObjectID		  6
-	  Version		  "1.6.0"
-	  ProdBitPerChar	  8
-	  ProdBitPerShort	  16
-	  ProdBitPerInt		  32
-	  ProdBitPerLong	  32
-	  ProdIntDivRoundTo	  "Undefined"
-	  ProdEndianess		  "Unspecified"
-	  ProdWordSize		  32
-	  ProdShiftRightIntArith  on
-	  ProdHWDeviceType	  "32-bit Generic"
-	  TargetBitPerChar	  8
-	  TargetBitPerShort	  16
-	  TargetBitPerInt	  32
-	  TargetBitPerLong	  32
-	  TargetShiftRightIntArith on
-	  TargetIntDivRoundTo	  "Undefined"
-	  TargetEndianess	  "Unspecified"
-	  TargetWordSize	  32
-	  TargetTypeEmulationWarnSuppressLevel 0
-	  TargetPreprocMaxBitsSint 32
-	  TargetPreprocMaxBitsUint 32
-	  TargetHWDeviceType	  "Specified"
-	  TargetUnknown		  off
-	  ProdEqTarget		  on
-	}
-	Simulink.ModelReferenceCC {
-	  $ObjectID		  7
-	  Version		  "1.6.0"
-	  UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange"
-	  CheckModelReferenceTargetMessage "error"
-	  ModelReferenceNumInstancesAllowed "Multi"
-	  ModelReferencePassRootInputsByReference on
-	  ModelReferenceMinAlgLoopOccurrences off
-	}
-	Simulink.SFSimCC {
-	  $ObjectID		  8
-	  Version		  "1.6.0"
-	  SFSimEnableDebug	  on
-	  SFSimOverflowDetection  on
-	  SFSimEcho		  on
-	  SimBlas		  on
-	  SimCtrlC		  on
-	  SimExtrinsic		  on
-	  SimIntegrity		  on
-	  SimUseLocalCustomCode	  off
-	  SimBuildMode		  "sf_incremental_build"
-	}
-	Simulink.RTWCC {
-	  $BackupClass		  "Simulink.RTWCC"
-	  $ObjectID		  9
-	  Version		  "1.6.0"
-	  Array {
-	    Type		    "Cell"
-	    Dimension		    1
-	    Cell		    "IncludeHyperlinkInReport"
-	    PropName		    "DisabledProps"
-	  }
-	  SystemTargetFile	  "grt.tlc"
-	  GenCodeOnly		  off
-	  MakeCommand		  "make_rtw"
-	  GenerateMakefile	  on
-	  TemplateMakefile	  "grt_default_tmf"
-	  GenerateReport	  off
-	  SaveLog		  off
-	  RTWVerbose		  on
-	  RetainRTWFile		  off
-	  ProfileTLC		  off
-	  TLCDebug		  off
-	  TLCCoverage		  off
-	  TLCAssert		  off
-	  ProcessScriptMode	  "Default"
-	  ConfigurationMode	  "Optimized"
-	  ConfigAtBuild		  off
-	  RTWUseLocalCustomCode	  off
-	  RTWUseSimCustomCode	  off
-	  IncludeHyperlinkInReport off
-	  LaunchReport		  off
-	  TargetLang		  "C"
-	  IncludeBusHierarchyInRTWFileBlockHierarchyMap	off
-	  IncludeERTFirstTime	  off
-	  GenerateTraceInfo	  off
-	  GenerateTraceReport	  off
-	  GenerateTraceReportSl	  off
-	  GenerateTraceReportSf	  off
-	  GenerateTraceReportEml  off
-	  GenerateCodeInfo	  off
-	  RTWCompilerOptimization "Off"
-	  CheckMdlBeforeBuild	  "Off"
-	  Array {
-	    Type		    "Handle"
-	    Dimension		    2
-	    Simulink.CodeAppCC {
-	      $ObjectID		      10
-	      Version		      "1.6.0"
-	      Array {
-		Type			"Cell"
-		Dimension		16
-		Cell			"IgnoreCustomStorageClasses"
-		Cell			"InsertBlockDesc"
-		Cell			"SFDataObjDesc"
-		Cell			"SimulinkDataObjDesc"
-		Cell			"DefineNamingRule"
-		Cell			"SignalNamingRule"
-		Cell			"ParamNamingRule"
-		Cell			"InlinedPrmAccess"
-		Cell			"CustomSymbolStr"
-		Cell			"CustomSymbolStrGlobalVar"
-		Cell			"CustomSymbolStrType"
-		Cell			"CustomSymbolStrField"
-		Cell			"CustomSymbolStrFcn"
-		Cell			"CustomSymbolStrBlkIO"
-		Cell			"CustomSymbolStrTmpVar"
-		Cell			"CustomSymbolStrMacro"
-		PropName		"DisabledProps"
-	      }
-	      ForceParamTrailComments off
-	      GenerateComments	      on
-	      IgnoreCustomStorageClasses on
-	      IgnoreTestpoints	      off
-	      IncHierarchyInIds	      off
-	      MaxIdLength	      31
-	      PreserveName	      off
-	      PreserveNameWithParent  off
-	      ShowEliminatedStatement off
-	      IncAutoGenComments      off
-	      SimulinkDataObjDesc     off
-	      SFDataObjDesc	      off
-	      IncDataTypeInIds	      off
-	      MangleLength	      1
-	      CustomSymbolStrGlobalVar "$R$N$M"
-	      CustomSymbolStrType     "$N$R$M"
-	      CustomSymbolStrField    "$N$M"
-	      CustomSymbolStrFcn      "$R$N$M$F"
-	      CustomSymbolStrFcnArg   "rt$I$N$M"
-	      CustomSymbolStrBlkIO    "rtb_$N$M"
-	      CustomSymbolStrTmpVar   "$N$M"
-	      CustomSymbolStrMacro    "$R$N$M"
-	      DefineNamingRule	      "None"
-	      ParamNamingRule	      "None"
-	      SignalNamingRule	      "None"
-	      InsertBlockDesc	      off
-	      SimulinkBlockComments   on
-	      EnableCustomComments    off
-	      InlinedPrmAccess	      "Literals"
-	      ReqsInCode	      off
-	      UseSimReservedNames     off
-	    }
-	    Simulink.GRTTargetCC {
-	      $BackupClass	      "Simulink.TargetCC"
-	      $ObjectID		      11
-	      Version		      "1.6.0"
-	      Array {
-		Type			"Cell"
-		Dimension		13
-		Cell			"IncludeMdlTerminateFcn"
-		Cell			"CombineOutputUpdateFcns"
-		Cell			"SuppressErrorStatus"
-		Cell			"ERTCustomFileBanners"
-		Cell			"GenerateSampleERTMain"
-		Cell			"GenerateTestInterfaces"
-		Cell			"MultiInstanceERTCode"
-		Cell			"PurelyIntegerCode"
-		Cell			"SupportNonFinite"
-		Cell			"SupportComplex"
-		Cell			"SupportAbsoluteTime"
-		Cell			"SupportContinuousTime"
-		Cell			"SupportNonInlinedSFcns"
-		PropName		"DisabledProps"
-	      }
-	      TargetFcnLib	      "ansi_tfl_tmw.mat"
-	      TargetLibSuffix	      ""
-	      TargetPreCompLibLocation ""
-	      TargetFunctionLibrary   "ANSI_C"
-	      UtilityFuncGeneration   "Auto"
-	      ERTMultiwordTypeDef     "System defined"
-	      ERTMultiwordLength      256
-	      MultiwordLength	      2048
-	      GenerateFullHeader      on
-	      GenerateSampleERTMain   off
-	      GenerateTestInterfaces  off
-	      IsPILTarget	      off
-	      ModelReferenceCompliant on
-	      ParMdlRefBuildCompliant on
-	      CompOptLevelCompliant   on
-	      IncludeMdlTerminateFcn  on
-	      GeneratePreprocessorConditionals "Disable all"
-	      CombineOutputUpdateFcns off
-	      SuppressErrorStatus     off
-	      ERTFirstTimeCompliant   off
-	      IncludeFileDelimiter    "Auto"
-	      ERTCustomFileBanners    off
-	      SupportAbsoluteTime     on
-	      LogVarNameModifier      "rt_"
-	      MatFileLogging	      on
-	      MultiInstanceERTCode    off
-	      SupportNonFinite	      on
-	      SupportComplex	      on
-	      PurelyIntegerCode	      off
-	      SupportContinuousTime   on
-	      SupportNonInlinedSFcns  on
-	      SupportVariableSizeSignals off
-	      EnableShiftOperators    on
-	      ParenthesesLevel	      "Nominal"
-	      PortableWordSizes	      off
-	      ModelStepFunctionPrototypeControlCompliant off
-	      CPPClassGenCompliant    off
-	      AutosarCompliant	      off
-	      UseMalloc		      off
-	      ExtMode		      off
-	      ExtModeStaticAlloc      off
-	      ExtModeTesting	      off
-	      ExtModeStaticAllocSize  1000000
-	      ExtModeTransport	      0
-	      ExtModeMexFile	      "ext_comm"
-	      ExtModeIntrfLevel	      "Level1"
-	      RTWCAPISignals	      off
-	      RTWCAPIParams	      off
-	      RTWCAPIStates	      off
-	      GenerateASAP2	      off
-	    }
-	    PropName		    "Components"
-	  }
-	}
-	PropName		"Components"
-      }
-      Name		      "Configuration"
-      CurrentDlgPage	      "Solver"
-      ConfigPrmDlgPosition    " [ 200, 197, 1080, 827 ] "
-    }
-    PropName		    "ConfigurationSets"
-  }
-  Simulink.ConfigSet {
-    $PropName		    "ActiveConfigurationSet"
-    $ObjectID		    1
-  }
-  BlockDefaults {
-    ForegroundColor	    "black"
-    BackgroundColor	    "white"
-    DropShadow		    off
-    NamePlacement	    "normal"
-    FontName		    "Courier"
-    FontSize		    10
-    FontWeight		    "normal"
-    FontAngle		    "normal"
-    ShowName		    on
-    BlockRotation	    0
-    BlockMirror		    off
-  }
-  AnnotationDefaults {
-    HorizontalAlignment	    "center"
-    VerticalAlignment	    "middle"
-    ForegroundColor	    "black"
-    BackgroundColor	    "white"
-    DropShadow		    off
-    FontName		    "Courier"
-    FontSize		    10
-    FontWeight		    "normal"
-    FontAngle		    "normal"
-    UseDisplayTextAsClickCallback off
-  }
-  LineDefaults {
-    FontName		    "Courier"
-    FontSize		    9
-    FontWeight		    "normal"
-    FontAngle		    "normal"
-  }
-  BlockParameterDefaults {
-    Block {
-      BlockType		      FromWorkspace
-      VariableName	      "simulink_input"
-      SampleTime	      "-1"
-      Interpolate	      on
-      ZeroCross		      off
-      OutputAfterFinalValue   "Extrapolation"
-    }
-    Block {
-      BlockType		      "S-Function"
-      FunctionName	      "system"
-      SFunctionModules	      "''"
-      PortCounts	      "[]"
-      SFunctionDeploymentMode off
-    }
-    Block {
-      BlockType		      Scope
-      ModelBased	      off
-      TickLabels	      "OneTimeTick"
-      ZoomMode		      "on"
-      Grid		      "on"
-      TimeRange		      "auto"
-      YMin		      "-5"
-      YMax		      "5"
-      SaveToWorkspace	      off
-      SaveName		      "ScopeData"
-      LimitDataPoints	      on
-      MaxDataPoints	      "5000"
-      Decimation	      "1"
-      SampleInput	      off
-      SampleTime	      "-1"
-    }
-  }
-  System {
-    Name		    "example_QProblemB"
-    Location		    [214, 137, 1484, 1054]
-    Open		    on
-    ModelBrowserVisibility  off
-    ModelBrowserWidth	    200
-    ScreenColor		    "white"
-    PaperOrientation	    "landscape"
-    PaperPositionMode	    "auto"
-    PaperType		    "usletter"
-    PaperUnits		    "inches"
-    TiledPaperMargins	    [0.500000, 0.500000, 0.500000, 0.500000]
-    TiledPageScale	    1
-    ShowPageBoundaries	    off
-    ZoomFactor		    "125"
-    ReportName		    "simulink-default.rpt"
-    SIDHighWatermark	    6
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace1"
-      SID		      1
-      Position		      [85, 93, 150, 117]
-      VariableName	      "H"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace2"
-      SID		      2
-      Position		      [85, 158, 150, 182]
-      VariableName	      "g"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace3"
-      SID		      3
-      Position		      [85, 218, 150, 242]
-      VariableName	      "lb"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace4"
-      SID		      4
-      Position		      [85, 283, 150, 307]
-      VariableName	      "ub"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      Scope
-      Name		      "fval,\nx,\nexitflag,\niter"
-      SID		      5
-      Ports		      [4]
-      Position		      [840, 184, 870, 216]
-      Floating		      off
-      Location		      [6, 78, 1276, 993]
-      Open		      off
-      NumInputPorts	      "4"
-      List {
-	ListType		AxesTitles
-	axes1			"%<SignalLabel>"
-	axes2			"%<SignalLabel>"
-	axes3			"%<SignalLabel>"
-	axes4			"%<SignalLabel>"
-      }
-      YMin		      "-5~-5~-5~-5"
-      YMax		      "5~5~5~5"
-      SaveName		      "ScopeData1"
-      DataFormat	      "StructureWithTime"
-      SampleTime	      "0"
-    }
-    Block {
-      BlockType		      "S-Function"
-      Name		      "qpOASES"
-      SID		      6
-      Ports		      [4, 4]
-      Position		      [510, 181, 705, 219]
-      BackgroundColor	      "yellow"
-      FunctionName	      "qpOASES_QProblemB"
-      EnableBusSupport	      off
-    }
-    Line {
-      SrcBlock		      "From\nWorkspace1"
-      SrcPort		      1
-      Points		      [130, 0; 0, 80]
-      DstBlock		      "qpOASES"
-      DstPort		      1
-    }
-    Line {
-      SrcBlock		      "From\nWorkspace2"
-      SrcPort		      1
-      Points		      [80, 0; 0, 26; 260, 0]
-      DstBlock		      "qpOASES"
-      DstPort		      2
-    }
-    Line {
-      SrcBlock		      "qpOASES"
-      SrcPort		      1
-      DstBlock		      "fval,\nx,\nexitflag,\niter"
-      DstPort		      1
-    }
-    Line {
-      SrcBlock		      "qpOASES"
-      SrcPort		      2
-      DstBlock		      "fval,\nx,\nexitflag,\niter"
-      DstPort		      2
-    }
-    Line {
-      SrcBlock		      "qpOASES"
-      SrcPort		      3
-      DstBlock		      "fval,\nx,\nexitflag,\niter"
-      DstPort		      3
-    }
-    Line {
-      SrcBlock		      "qpOASES"
-      SrcPort		      4
-      DstBlock		      "fval,\nx,\nexitflag,\niter"
-      DstPort		      4
-    }
-    Line {
-      SrcBlock		      "From\nWorkspace3"
-      SrcPort		      1
-      Points		      [80, 0; 0, -25]
-      DstBlock		      "qpOASES"
-      DstPort		      3
-    }
-    Line {
-      SrcBlock		      "From\nWorkspace4"
-      SrcPort		      1
-      Points		      [130, 0; 0, -79; 210, 0]
-      DstBlock		      "qpOASES"
-      DstPort		      4
-    }
-    Annotation {
-      Position		      [335, 96]
-    }
-    Annotation {
-      Name		      "This file is part of qpOASES.\n\nqpOASES -- An Implementation of the Online Active Set Strategy.\nC"
-      "opyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka, Christian Kirches et al.\nAll rights reserved."
-      Position		      [608, 108]
-    }
-  }
-}
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_SQProblem.mdl b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_SQProblem.mdl
deleted file mode 100644
index 106f19a56ed..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/example_SQProblem.mdl
+++ /dev/null
@@ -1,797 +0,0 @@
-Model {
-  Name			  "example_SQProblem"
-  Version		  7.4
-  MdlSubVersion		  0
-  GraphicalInterface {
-    NumRootInports	    0
-    NumRootOutports	    0
-    ParameterArgumentNames  ""
-    ComputedModelVersion    "1.62"
-    NumModelReferences	    0
-    NumTestPointedSignals   0
-  }
-  SavedCharacterEncoding  "UTF-8"
-  SaveDefaultBlockParams  on
-  ScopeRefreshTime	  0.035000
-  OverrideScopeRefreshTime on
-  DisableAllScopes	  off
-  DataTypeOverride	  "UseLocalSettings"
-  MinMaxOverflowLogging	  "UseLocalSettings"
-  MinMaxOverflowArchiveMode "Overwrite"
-  MaxMDLFileLineLength	  120
-  Created		  "Fri Apr 13 11:08:51 2007"
-  Creator		  "jferreau"
-  UpdateHistory		  "UpdateHistoryNever"
-  ModifiedByFormat	  "%<Auto>"
-  LastModifiedBy	  "chjofer2"
-  ModifiedDateFormat	  "%<Auto>"
-  LastModifiedDate	  "Thu Feb 20 15:12:07 2014"
-  RTWModifiedTimeStamp	  314809925
-  ModelVersionFormat	  "1.%<AutoIncrement:62>"
-  ConfigurationManager	  "None"
-  SampleTimeColors	  off
-  SampleTimeAnnotations	  off
-  LibraryLinkDisplay	  "none"
-  WideLines		  off
-  ShowLineDimensions	  off
-  ShowPortDataTypes	  off
-  ShowLoopsOnError	  on
-  IgnoreBidirectionalLines off
-  ShowStorageClass	  off
-  ShowTestPointIcons	  on
-  ShowSignalResolutionIcons on
-  ShowViewerIcons	  on
-  SortedOrder		  off
-  ExecutionContextIcon	  off
-  ShowLinearizationAnnotations on
-  BlockNameDataTip	  off
-  BlockParametersDataTip  off
-  BlockDescriptionStringDataTip	off
-  ToolBar		  on
-  StatusBar		  on
-  BrowserShowLibraryLinks off
-  BrowserLookUnderMasks	  off
-  SimulationMode	  "normal"
-  LinearizationMsg	  "none"
-  Profile		  off
-  ParamWorkspaceSource	  "MATLABWorkspace"
-  AccelSystemTargetFile	  "accel.tlc"
-  AccelTemplateMakefile	  "accel_default_tmf"
-  AccelMakeCommand	  "make_rtw"
-  TryForcingSFcnDF	  off
-  RecordCoverage	  off
-  CovPath		  "/"
-  CovSaveName		  "covdata"
-  CovMetricSettings	  "dw"
-  CovNameIncrementing	  off
-  CovHtmlReporting	  on
-  CovForceBlockReductionOff on
-  covSaveCumulativeToWorkspaceVar on
-  CovSaveSingleToWorkspaceVar on
-  CovCumulativeVarName	  "covCumulativeData"
-  CovCumulativeReport	  off
-  CovReportOnPause	  on
-  CovModelRefEnable	  "Off"
-  CovExternalEMLEnable	  off
-  ExtModeBatchMode	  off
-  ExtModeEnableFloating	  on
-  ExtModeTrigType	  "manual"
-  ExtModeTrigMode	  "normal"
-  ExtModeTrigPort	  "1"
-  ExtModeTrigElement	  "any"
-  ExtModeTrigDuration	  1000
-  ExtModeTrigDurationFloating "auto"
-  ExtModeTrigHoldOff	  0
-  ExtModeTrigDelay	  0
-  ExtModeTrigDirection	  "rising"
-  ExtModeTrigLevel	  0
-  ExtModeArchiveMode	  "off"
-  ExtModeAutoIncOneShot	  off
-  ExtModeIncDirWhenArm	  off
-  ExtModeAddSuffixToVar	  off
-  ExtModeWriteAllDataToWs off
-  ExtModeArmWhenConnect	  on
-  ExtModeSkipDownloadWhenConnect off
-  ExtModeLogAll		  on
-  ExtModeAutoUpdateStatusClock off
-  BufferReuse		  on
-  ShowModelReferenceBlockVersion off
-  ShowModelReferenceBlockIO off
-  Array {
-    Type		    "Handle"
-    Dimension		    1
-    Simulink.ConfigSet {
-      $ObjectID		      1
-      Version		      "1.6.0"
-      Array {
-	Type			"Handle"
-	Dimension		8
-	Simulink.SolverCC {
-	  $ObjectID		  2
-	  Version		  "1.6.0"
-	  StartTime		  "0.0"
-	  StopTime		  "0.5"
-	  AbsTol		  "auto"
-	  FixedStep		  "auto"
-	  InitialStep		  "auto"
-	  MaxNumMinSteps	  "-1"
-	  MaxOrder		  5
-	  ZcThreshold		  "auto"
-	  ConsecutiveZCsStepRelTol "10*128*eps"
-	  MaxConsecutiveZCs	  "1000"
-	  ExtrapolationOrder	  4
-	  NumberNewtonIterations  1
-	  MaxStep		  "auto"
-	  MinStep		  "auto"
-	  MaxConsecutiveMinStep	  "1"
-	  RelTol		  "1e-3"
-	  SolverMode		  "Auto"
-	  Solver		  "VariableStepDiscrete"
-	  SolverName		  "VariableStepDiscrete"
-	  ShapePreserveControl	  "DisableAll"
-	  ZeroCrossControl	  "UseLocalSettings"
-	  ZeroCrossAlgorithm	  "Nonadaptive"
-	  AlgebraicLoopSolver	  "TrustRegion"
-	  SolverResetMethod	  "Fast"
-	  PositivePriorityOrder	  off
-	  AutoInsertRateTranBlk	  off
-	  SampleTimeConstraint	  "Unconstrained"
-	  InsertRTBMode		  "Whenever possible"
-	}
-	Simulink.DataIOCC {
-	  $ObjectID		  3
-	  Version		  "1.6.0"
-	  Decimation		  "1"
-	  ExternalInput		  "[t, u]"
-	  FinalStateName	  "xFinal"
-	  InitialState		  "xInitial"
-	  LimitDataPoints	  on
-	  MaxDataPoints		  "1000"
-	  LoadExternalInput	  off
-	  LoadInitialState	  off
-	  SaveFinalState	  off
-	  SaveCompleteFinalSimState off
-	  SaveFormat		  "Array"
-	  SaveOutput		  on
-	  SaveState		  off
-	  SignalLogging		  on
-	  InspectSignalLogs	  off
-	  SaveTime		  on
-	  ReturnWorkspaceOutputs  off
-	  StateSaveName		  "xout"
-	  TimeSaveName		  "tout"
-	  OutputSaveName	  "yout"
-	  SignalLoggingName	  "logsout"
-	  OutputOption		  "RefineOutputTimes"
-	  OutputTimes		  "[]"
-	  ReturnWorkspaceOutputsName "out"
-	  Refine		  "1"
-	}
-	Simulink.OptimizationCC {
-	  $ObjectID		  4
-	  Version		  "1.6.0"
-	  Array {
-	    Type		    "Cell"
-	    Dimension		    4
-	    Cell		    "ZeroExternalMemoryAtStartup"
-	    Cell		    "ZeroInternalMemoryAtStartup"
-	    Cell		    "NoFixptDivByZeroProtection"
-	    Cell		    "OptimizeModelRefInitCode"
-	    PropName		    "DisabledProps"
-	  }
-	  BlockReduction	  on
-	  BooleanDataType	  on
-	  ConditionallyExecuteInputs on
-	  InlineParams		  off
-	  UseIntDivNetSlope	  off
-	  InlineInvariantSignals  off
-	  OptimizeBlockIOStorage  on
-	  BufferReuse		  on
-	  EnhancedBackFolding	  off
-	  StrengthReduction	  off
-	  EnforceIntegerDowncast  on
-	  ExpressionFolding	  on
-	  BooleansAsBitfields	  off
-	  EnableMemcpy		  on
-	  MemcpyThreshold	  64
-	  PassReuseOutputArgsAs	  "Structure reference"
-	  ExpressionDepthLimit	  2147483647
-	  FoldNonRolledExpr	  on
-	  LocalBlockOutputs	  on
-	  RollThreshold		  5
-	  SystemCodeInlineAuto	  off
-	  StateBitsets		  off
-	  DataBitsets		  off
-	  UseTempVars		  off
-	  ZeroExternalMemoryAtStartup on
-	  ZeroInternalMemoryAtStartup on
-	  InitFltsAndDblsToZero	  on
-	  NoFixptDivByZeroProtection off
-	  EfficientFloat2IntCast  off
-	  EfficientMapNaN2IntZero on
-	  OptimizeModelRefInitCode off
-	  LifeSpan		  "inf"
-	  BufferReusableBoundary  on
-	  SimCompilerOptimization "Off"
-	  AccelVerboseBuild	  off
-	}
-	Simulink.DebuggingCC {
-	  $ObjectID		  5
-	  Version		  "1.6.0"
-	  RTPrefix		  "error"
-	  ConsistencyChecking	  "none"
-	  ArrayBoundsChecking	  "none"
-	  SignalInfNanChecking	  "none"
-	  SignalRangeChecking	  "none"
-	  ReadBeforeWriteMsg	  "UseLocalSettings"
-	  WriteAfterWriteMsg	  "UseLocalSettings"
-	  WriteAfterReadMsg	  "UseLocalSettings"
-	  AlgebraicLoopMsg	  "warning"
-	  ArtificialAlgebraicLoopMsg "warning"
-	  SaveWithDisabledLinksMsg "warning"
-	  SaveWithParameterizedLinksMsg	"none"
-	  CheckSSInitialOutputMsg on
-	  UnderspecifiedInitializationDetection	"Classic"
-	  MergeDetectMultiDrivingBlocksExec "none"
-	  CheckExecutionContextPreStartOutputMsg off
-	  CheckExecutionContextRuntimeOutputMsg	off
-	  SignalResolutionControl "TryResolveAllWithWarning"
-	  BlockPriorityViolationMsg "warning"
-	  MinStepSizeMsg	  "warning"
-	  TimeAdjustmentMsg	  "none"
-	  MaxConsecutiveZCsMsg	  "error"
-	  SolverPrmCheckMsg	  "warning"
-	  InheritedTsInSrcMsg	  "warning"
-	  DiscreteInheritContinuousMsg "warning"
-	  MultiTaskDSMMsg	  "error"
-	  MultiTaskCondExecSysMsg "error"
-	  MultiTaskRateTransMsg	  "error"
-	  SingleTaskRateTransMsg  "none"
-	  TasksWithSamePriorityMsg "warning"
-	  SigSpecEnsureSampleTimeMsg "warning"
-	  CheckMatrixSingularityMsg "none"
-	  IntegerOverflowMsg	  "warning"
-	  Int32ToFloatConvMsg	  "warning"
-	  ParameterDowncastMsg	  "error"
-	  ParameterOverflowMsg	  "error"
-	  ParameterUnderflowMsg	  "none"
-	  ParameterPrecisionLossMsg "warning"
-	  ParameterTunabilityLossMsg "warning"
-	  FixptConstUnderflowMsg  "none"
-	  FixptConstOverflowMsg	  "none"
-	  FixptConstPrecisionLossMsg "none"
-	  UnderSpecifiedDataTypeMsg "none"
-	  UnnecessaryDatatypeConvMsg "none"
-	  VectorMatrixConversionMsg "none"
-	  InvalidFcnCallConnMsg	  "error"
-	  FcnCallInpInsideContextMsg "Use local settings"
-	  SignalLabelMismatchMsg  "none"
-	  UnconnectedInputMsg	  "warning"
-	  UnconnectedOutputMsg	  "warning"
-	  UnconnectedLineMsg	  "warning"
-	  SFcnCompatibilityMsg	  "none"
-	  UniqueDataStoreMsg	  "none"
-	  BusObjectLabelMismatch  "warning"
-	  RootOutportRequireBusObject "warning"
-	  AssertControl		  "UseLocalSettings"
-	  EnableOverflowDetection off
-	  ModelReferenceIOMsg	  "none"
-	  ModelReferenceVersionMismatchMessage "none"
-	  ModelReferenceIOMismatchMessage "none"
-	  ModelReferenceCSMismatchMessage "none"
-	  UnknownTsInhSupMsg	  "warning"
-	  ModelReferenceDataLoggingMessage "warning"
-	  ModelReferenceSymbolNameMessage "warning"
-	  ModelReferenceExtraNoncontSigs "error"
-	  StateNameClashWarn	  "warning"
-	  SimStateInterfaceChecksumMismatchMsg "warning"
-	  StrictBusMsg		  "Warning"
-	  LoggingUnavailableSignals "error"
-	  BlockIODiagnostic	  "none"
-	}
-	Simulink.HardwareCC {
-	  $ObjectID		  6
-	  Version		  "1.6.0"
-	  ProdBitPerChar	  8
-	  ProdBitPerShort	  16
-	  ProdBitPerInt		  32
-	  ProdBitPerLong	  32
-	  ProdIntDivRoundTo	  "Undefined"
-	  ProdEndianess		  "Unspecified"
-	  ProdWordSize		  32
-	  ProdShiftRightIntArith  on
-	  ProdHWDeviceType	  "32-bit Generic"
-	  TargetBitPerChar	  8
-	  TargetBitPerShort	  16
-	  TargetBitPerInt	  32
-	  TargetBitPerLong	  32
-	  TargetShiftRightIntArith on
-	  TargetIntDivRoundTo	  "Undefined"
-	  TargetEndianess	  "Unspecified"
-	  TargetWordSize	  32
-	  TargetTypeEmulationWarnSuppressLevel 0
-	  TargetPreprocMaxBitsSint 32
-	  TargetPreprocMaxBitsUint 32
-	  TargetHWDeviceType	  "Specified"
-	  TargetUnknown		  off
-	  ProdEqTarget		  on
-	}
-	Simulink.ModelReferenceCC {
-	  $ObjectID		  7
-	  Version		  "1.6.0"
-	  UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange"
-	  CheckModelReferenceTargetMessage "error"
-	  ModelReferenceNumInstancesAllowed "Multi"
-	  ModelReferencePassRootInputsByReference on
-	  ModelReferenceMinAlgLoopOccurrences off
-	}
-	Simulink.SFSimCC {
-	  $ObjectID		  8
-	  Version		  "1.6.0"
-	  SFSimEnableDebug	  on
-	  SFSimOverflowDetection  on
-	  SFSimEcho		  on
-	  SimBlas		  on
-	  SimCtrlC		  on
-	  SimExtrinsic		  on
-	  SimIntegrity		  on
-	  SimUseLocalCustomCode	  off
-	  SimBuildMode		  "sf_incremental_build"
-	}
-	Simulink.RTWCC {
-	  $BackupClass		  "Simulink.RTWCC"
-	  $ObjectID		  9
-	  Version		  "1.6.0"
-	  Array {
-	    Type		    "Cell"
-	    Dimension		    1
-	    Cell		    "IncludeHyperlinkInReport"
-	    PropName		    "DisabledProps"
-	  }
-	  SystemTargetFile	  "grt.tlc"
-	  GenCodeOnly		  off
-	  MakeCommand		  "make_rtw"
-	  GenerateMakefile	  on
-	  TemplateMakefile	  "grt_default_tmf"
-	  GenerateReport	  off
-	  SaveLog		  off
-	  RTWVerbose		  on
-	  RetainRTWFile		  off
-	  ProfileTLC		  off
-	  TLCDebug		  off
-	  TLCCoverage		  off
-	  TLCAssert		  off
-	  ProcessScriptMode	  "Default"
-	  ConfigurationMode	  "Optimized"
-	  ConfigAtBuild		  off
-	  RTWUseLocalCustomCode	  off
-	  RTWUseSimCustomCode	  off
-	  IncludeHyperlinkInReport off
-	  LaunchReport		  off
-	  TargetLang		  "C"
-	  IncludeBusHierarchyInRTWFileBlockHierarchyMap	off
-	  IncludeERTFirstTime	  off
-	  GenerateTraceInfo	  off
-	  GenerateTraceReport	  off
-	  GenerateTraceReportSl	  off
-	  GenerateTraceReportSf	  off
-	  GenerateTraceReportEml  off
-	  GenerateCodeInfo	  off
-	  RTWCompilerOptimization "Off"
-	  CheckMdlBeforeBuild	  "Off"
-	  Array {
-	    Type		    "Handle"
-	    Dimension		    2
-	    Simulink.CodeAppCC {
-	      $ObjectID		      10
-	      Version		      "1.6.0"
-	      Array {
-		Type			"Cell"
-		Dimension		16
-		Cell			"IgnoreCustomStorageClasses"
-		Cell			"InsertBlockDesc"
-		Cell			"SFDataObjDesc"
-		Cell			"SimulinkDataObjDesc"
-		Cell			"DefineNamingRule"
-		Cell			"SignalNamingRule"
-		Cell			"ParamNamingRule"
-		Cell			"InlinedPrmAccess"
-		Cell			"CustomSymbolStr"
-		Cell			"CustomSymbolStrGlobalVar"
-		Cell			"CustomSymbolStrType"
-		Cell			"CustomSymbolStrField"
-		Cell			"CustomSymbolStrFcn"
-		Cell			"CustomSymbolStrBlkIO"
-		Cell			"CustomSymbolStrTmpVar"
-		Cell			"CustomSymbolStrMacro"
-		PropName		"DisabledProps"
-	      }
-	      ForceParamTrailComments off
-	      GenerateComments	      on
-	      IgnoreCustomStorageClasses on
-	      IgnoreTestpoints	      off
-	      IncHierarchyInIds	      off
-	      MaxIdLength	      31
-	      PreserveName	      off
-	      PreserveNameWithParent  off
-	      ShowEliminatedStatement off
-	      IncAutoGenComments      off
-	      SimulinkDataObjDesc     off
-	      SFDataObjDesc	      off
-	      IncDataTypeInIds	      off
-	      MangleLength	      1
-	      CustomSymbolStrGlobalVar "$R$N$M"
-	      CustomSymbolStrType     "$N$R$M"
-	      CustomSymbolStrField    "$N$M"
-	      CustomSymbolStrFcn      "$R$N$M$F"
-	      CustomSymbolStrFcnArg   "rt$I$N$M"
-	      CustomSymbolStrBlkIO    "rtb_$N$M"
-	      CustomSymbolStrTmpVar   "$N$M"
-	      CustomSymbolStrMacro    "$R$N$M"
-	      DefineNamingRule	      "None"
-	      ParamNamingRule	      "None"
-	      SignalNamingRule	      "None"
-	      InsertBlockDesc	      off
-	      SimulinkBlockComments   on
-	      EnableCustomComments    off
-	      InlinedPrmAccess	      "Literals"
-	      ReqsInCode	      off
-	      UseSimReservedNames     off
-	    }
-	    Simulink.GRTTargetCC {
-	      $BackupClass	      "Simulink.TargetCC"
-	      $ObjectID		      11
-	      Version		      "1.6.0"
-	      Array {
-		Type			"Cell"
-		Dimension		13
-		Cell			"IncludeMdlTerminateFcn"
-		Cell			"CombineOutputUpdateFcns"
-		Cell			"SuppressErrorStatus"
-		Cell			"ERTCustomFileBanners"
-		Cell			"GenerateSampleERTMain"
-		Cell			"GenerateTestInterfaces"
-		Cell			"MultiInstanceERTCode"
-		Cell			"PurelyIntegerCode"
-		Cell			"SupportNonFinite"
-		Cell			"SupportComplex"
-		Cell			"SupportAbsoluteTime"
-		Cell			"SupportContinuousTime"
-		Cell			"SupportNonInlinedSFcns"
-		PropName		"DisabledProps"
-	      }
-	      TargetFcnLib	      "ansi_tfl_tmw.mat"
-	      TargetLibSuffix	      ""
-	      TargetPreCompLibLocation ""
-	      TargetFunctionLibrary   "ANSI_C"
-	      UtilityFuncGeneration   "Auto"
-	      ERTMultiwordTypeDef     "System defined"
-	      ERTMultiwordLength      256
-	      MultiwordLength	      2048
-	      GenerateFullHeader      on
-	      GenerateSampleERTMain   off
-	      GenerateTestInterfaces  off
-	      IsPILTarget	      off
-	      ModelReferenceCompliant on
-	      ParMdlRefBuildCompliant on
-	      CompOptLevelCompliant   on
-	      IncludeMdlTerminateFcn  on
-	      GeneratePreprocessorConditionals "Disable all"
-	      CombineOutputUpdateFcns off
-	      SuppressErrorStatus     off
-	      ERTFirstTimeCompliant   off
-	      IncludeFileDelimiter    "Auto"
-	      ERTCustomFileBanners    off
-	      SupportAbsoluteTime     on
-	      LogVarNameModifier      "rt_"
-	      MatFileLogging	      on
-	      MultiInstanceERTCode    off
-	      SupportNonFinite	      on
-	      SupportComplex	      on
-	      PurelyIntegerCode	      off
-	      SupportContinuousTime   on
-	      SupportNonInlinedSFcns  on
-	      SupportVariableSizeSignals off
-	      EnableShiftOperators    on
-	      ParenthesesLevel	      "Nominal"
-	      PortableWordSizes	      off
-	      ModelStepFunctionPrototypeControlCompliant off
-	      CPPClassGenCompliant    off
-	      AutosarCompliant	      off
-	      UseMalloc		      off
-	      ExtMode		      off
-	      ExtModeStaticAlloc      off
-	      ExtModeTesting	      off
-	      ExtModeStaticAllocSize  1000000
-	      ExtModeTransport	      0
-	      ExtModeMexFile	      "ext_comm"
-	      ExtModeIntrfLevel	      "Level1"
-	      RTWCAPISignals	      off
-	      RTWCAPIParams	      off
-	      RTWCAPIStates	      off
-	      GenerateASAP2	      off
-	    }
-	    PropName		    "Components"
-	  }
-	}
-	PropName		"Components"
-      }
-      Name		      "Configuration"
-      CurrentDlgPage	      "Solver"
-      ConfigPrmDlgPosition    " [ 200, 197, 1080, 827 ] "
-    }
-    PropName		    "ConfigurationSets"
-  }
-  Simulink.ConfigSet {
-    $PropName		    "ActiveConfigurationSet"
-    $ObjectID		    1
-  }
-  BlockDefaults {
-    ForegroundColor	    "black"
-    BackgroundColor	    "white"
-    DropShadow		    off
-    NamePlacement	    "normal"
-    FontName		    "Courier"
-    FontSize		    10
-    FontWeight		    "normal"
-    FontAngle		    "normal"
-    ShowName		    on
-    BlockRotation	    0
-    BlockMirror		    off
-  }
-  AnnotationDefaults {
-    HorizontalAlignment	    "center"
-    VerticalAlignment	    "middle"
-    ForegroundColor	    "black"
-    BackgroundColor	    "white"
-    DropShadow		    off
-    FontName		    "Courier"
-    FontSize		    10
-    FontWeight		    "normal"
-    FontAngle		    "normal"
-    UseDisplayTextAsClickCallback off
-  }
-  LineDefaults {
-    FontName		    "Courier"
-    FontSize		    9
-    FontWeight		    "normal"
-    FontAngle		    "normal"
-  }
-  BlockParameterDefaults {
-    Block {
-      BlockType		      FromWorkspace
-      VariableName	      "simulink_input"
-      SampleTime	      "-1"
-      Interpolate	      on
-      ZeroCross		      off
-      OutputAfterFinalValue   "Extrapolation"
-    }
-    Block {
-      BlockType		      "S-Function"
-      FunctionName	      "system"
-      SFunctionModules	      "''"
-      PortCounts	      "[]"
-      SFunctionDeploymentMode off
-    }
-    Block {
-      BlockType		      Scope
-      ModelBased	      off
-      TickLabels	      "OneTimeTick"
-      ZoomMode		      "on"
-      Grid		      "on"
-      TimeRange		      "auto"
-      YMin		      "-5"
-      YMax		      "5"
-      SaveToWorkspace	      off
-      SaveName		      "ScopeData"
-      LimitDataPoints	      on
-      MaxDataPoints	      "5000"
-      Decimation	      "1"
-      SampleInput	      off
-      SampleTime	      "-1"
-    }
-  }
-  System {
-    Name		    "example_SQProblem"
-    Location		    [2, 74, 1918, 1139]
-    Open		    on
-    ModelBrowserVisibility  off
-    ModelBrowserWidth	    200
-    ScreenColor		    "white"
-    PaperOrientation	    "landscape"
-    PaperPositionMode	    "auto"
-    PaperType		    "usletter"
-    PaperUnits		    "inches"
-    TiledPaperMargins	    [0.500000, 0.500000, 0.500000, 0.500000]
-    TiledPageScale	    1
-    ShowPageBoundaries	    off
-    ZoomFactor		    "125"
-    ReportName		    "simulink-default.rpt"
-    SIDHighWatermark	    9
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace1"
-      SID		      1
-      Position		      [85, 53, 150, 77]
-      VariableName	      "H"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace2"
-      SID		      2
-      Position		      [85, 113, 150, 137]
-      VariableName	      "g"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace3"
-      SID		      3
-      Position		      [85, 173, 150, 197]
-      VariableName	      "A"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace4"
-      SID		      4
-      Position		      [85, 233, 150, 257]
-      VariableName	      "lb"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace5"
-      SID		      5
-      Position		      [85, 293, 150, 317]
-      VariableName	      "ub"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace6"
-      SID		      6
-      Position		      [85, 353, 150, 377]
-      VariableName	      "lbA"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      FromWorkspace
-      Name		      "From\nWorkspace7"
-      SID		      7
-      Position		      [85, 418, 150, 442]
-      VariableName	      "ubA"
-      SampleTime	      "0.1"
-      ZeroCross		      on
-      OutputAfterFinalValue   "Holding final value"
-    }
-    Block {
-      BlockType		      Scope
-      Name		      "fval,\nx,\nexitflag,\niter"
-      SID		      8
-      Ports		      [4]
-      Position		      [935, 229, 965, 261]
-      Floating		      off
-      Location		      [6, 78, 1276, 993]
-      Open		      off
-      NumInputPorts	      "4"
-      List {
-	ListType		AxesTitles
-	axes1			"%<SignalLabel>"
-	axes2			"%<SignalLabel>"
-	axes3			"%<SignalLabel>"
-	axes4			"%<SignalLabel>"
-      }
-      YMin		      "-5~-5~-5~-5"
-      YMax		      "5~5~5~5"
-      SaveName		      "ScopeData1"
-      DataFormat	      "StructureWithTime"
-      SampleTime	      "0"
-    }
-    Block {
-      BlockType		      "S-Function"
-      Name		      "qpOASES"
-      SID		      9
-      Ports		      [7, 4]
-      Position		      [600, 226, 795, 264]
-      BackgroundColor	      "yellow"
-      FunctionName	      "qpOASES_SQProblem"
-      EnableBusSupport	      off
-    }
-    Line {
-      SrcBlock		      "From\nWorkspace1"
-      SrcPort		      1
-      Points		      [180, 0; 0, 165]
-      DstBlock		      "qpOASES"
-      DstPort		      1
-    }
-    Line {
-      SrcBlock		      "From\nWorkspace2"
-      SrcPort		      1
-      Points		      [115, 0; 0, 110]
-      DstBlock		      "qpOASES"
-      DstPort		      2
-    }
-    Line {
-      Labels		      [3, 0]
-      SrcBlock		      "From\nWorkspace7"
-      SrcPort		      1
-      Points		      [180, 0; 0, -170]
-      DstBlock		      "qpOASES"
-      DstPort		      7
-    }
-    Line {
-      SrcBlock		      "From\nWorkspace5"
-      SrcPort		      1
-      Points		      [55, 0; 0, -55]
-      DstBlock		      "qpOASES"
-      DstPort		      5
-    }
-    Line {
-      SrcBlock		      "From\nWorkspace4"
-      SrcPort		      1
-      DstBlock		      "qpOASES"
-      DstPort		      4
-    }
-    Line {
-      SrcBlock		      "From\nWorkspace3"
-      SrcPort		      1
-      Points		      [55, 0; 0, 55]
-      DstBlock		      "qpOASES"
-      DstPort		      3
-    }
-    Line {
-      Labels		      [3, 0]
-      SrcBlock		      "From\nWorkspace6"
-      SrcPort		      1
-      Points		      [115, 0; 0, -110]
-      DstBlock		      "qpOASES"
-      DstPort		      6
-    }
-    Line {
-      SrcBlock		      "qpOASES"
-      SrcPort		      1
-      DstBlock		      "fval,\nx,\nexitflag,\niter"
-      DstPort		      1
-    }
-    Line {
-      SrcBlock		      "qpOASES"
-      SrcPort		      2
-      DstBlock		      "fval,\nx,\nexitflag,\niter"
-      DstPort		      2
-    }
-    Line {
-      SrcBlock		      "qpOASES"
-      SrcPort		      3
-      DstBlock		      "fval,\nx,\nexitflag,\niter"
-      DstPort		      3
-    }
-    Line {
-      SrcBlock		      "qpOASES"
-      SrcPort		      4
-      DstBlock		      "fval,\nx,\nexitflag,\niter"
-      DstPort		      4
-    }
-    Annotation {
-      Position		      [335, 96]
-    }
-    Annotation {
-      Name		      "This file is part of qpOASES.\n\nqpOASES -- An Implementation of the Online Active Set Strategy.\nC"
-      "opyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka, Christian Kirches et al.\nAll rights reserved."
-      Position		      [698, 138]
-    }
-  }
-}
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/index.html b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/index.html
deleted file mode 100644
index e3ce9152edb..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/index.html
+++ /dev/null
@@ -1,17 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/interfaces/simulink</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/interfaces/simulink</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="example_QProblem.mdl">example_QProblem.mdl</a></li>
-  <li><a href="example_QProblemB.mdl">example_QProblemB.mdl</a></li>
-  <li><a href="example_SQProblem.mdl">example_SQProblem.mdl</a></li>
-  <li><a href="load_example_QProblem.m">load_example_QProblem.m</a></li>
-  <li><a href="load_example_QProblemB.m">load_example_QProblemB.m</a></li>
-  <li><a href="load_example_SQProblem.m">load_example_SQProblem.m</a></li>
-  <li><a href="make.m">make.m</a></li>
-  <li><a href="qpOASES_QProblem.cpp">qpOASES_QProblem.cpp</a></li>
-  <li><a href="qpOASES_QProblemB.cpp">qpOASES_QProblemB.cpp</a></li>
-  <li><a href="qpOASES_SQProblem.cpp">qpOASES_SQProblem.cpp</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_QProblem.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_QProblem.m
deleted file mode 100644
index af2a81d91c9..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_QProblem.m
+++ /dev/null
@@ -1,93 +0,0 @@
-%%
-%%	This file is part of qpOASES.
-%%
-%%	qpOASES -- An Implementation of the Online Active Set Strategy.
-%%	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%%	Christian Kirches et al. All rights reserved.
-%%
-%%	qpOASES 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.
-%%
-%%	qpOASES 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 qpOASES; if not, write to the Free Software
-%%	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-%%
-
-
-
-%%
-%%	Filename:  interfaces/simulink/load_example_QProblem.m
-%%	Author:    Hans Joachim Ferreau (thanks to Aude Perrin)
-%%	Version:   3.0
-%%	Date:      2007-2014
-%%
-
-
-
-clear all;
-
-
-%% setup QP data
-simulationTime  =  [0;0.1];
-
-H.time = simulationTime;
-data1 = [1.0,0.0,0.0,0.5];
-data2 = [1.0,0.0,0.0,0.5];
-H.signals.values = [data1; data2];
-H.signals.dimensions = length(data1);
-
-g.time = simulationTime;
-data1 = [1.5,1.0];
-data2 = [1.0,1.5];
-g.signals.values = [data1; data2];
-g.signals.dimensions = length(data1);
-
-A.time = simulationTime;
-data1 = [1.0,1.0];
-data2 = [1.0,1.0];
-A.signals.values = [data1; data2];
-A.signals.dimensions = length(data1);
-
-lb.time = simulationTime;
-data1 = [0.5,-2.0];
-data2 = [0.0,-1.0];
-lb.signals.values = [data1; data2];
-lb.signals.dimensions = length(data1);
-
-ub.time = simulationTime;
-data1 = [5.0,2.0];
-data2 = [5.0,-0.5];
-ub.signals.values = [data1; data2];
-ub.signals.dimensions = length(data1);
-
-lbA.time = simulationTime;
-data1 = [-1.0];
-data2 = [-2.0];
-lbA.signals.values = [data1; data2];
-lbA.signals.dimensions = length(data1);
-
-ubA.time = simulationTime;
-data1 = [2.0];
-data2 = [1.0];
-ubA.signals.values = [data1; data2];
-ubA.signals.dimensions = length(data1);
-
-
-clear simulationTime data1 data2
-
-
-%% open corresponding simulink example
-open( 'example_QProblem.mdl' );
-
-
-
-%%
-%%	end of file
-%%
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_QProblemB.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_QProblemB.m
deleted file mode 100644
index 8c3900c9922..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_QProblemB.m
+++ /dev/null
@@ -1,75 +0,0 @@
-%%
-%%	This file is part of qpOASES.
-%%
-%%	qpOASES -- An Implementation of the Online Active Set Strategy.
-%%	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%%	Christian Kirches et al. All rights reserved.
-%%
-%%	qpOASES 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.
-%%
-%%	qpOASES 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 qpOASES; if not, write to the Free Software
-%%	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-%%
-
-
-
-%%
-%%	Filename:  interfaces/simulink/load_example_QProblemB.m
-%%	Author:    Hans Joachim Ferreau (thanks to Aude Perrin)
-%%	Version:   3.0
-%%	Date:      2007-2014
-%%
-
-
-
-clear all;
-
-
-%% setup QP data
-simulationTime  =  [0;0.1];
-
-H.time = simulationTime;
-data1 = [1.0,0.0,0.0,0.5];
-data2 = [1.0,0.0,0.0,0.5];
-H.signals.values = [data1; data2];
-H.signals.dimensions = length(data1);
-
-g.time = simulationTime;
-data1 = [1.5,1.0];
-data2 = [1.0,1.5];
-g.signals.values = [data1; data2];
-g.signals.dimensions = length(data1);
-
-lb.time = simulationTime;
-data1 = [0.5,-2.0];
-data2 = [0.0,-1.0];
-lb.signals.values = [data1; data2];
-lb.signals.dimensions = length(data1);
-
-ub.time = simulationTime;
-data1 = [5.0,2.0];
-data2 = [5.0,-0.5];
-ub.signals.values = [data1; data2];
-ub.signals.dimensions = length(data1);
-
-
-clear simulationTime data1 data2
-
-
-%% open corresponding simulink example
-open( 'example_QProblemB.mdl' );
-
-
-
-%%
-%%	end of file
-%%
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_SQProblem.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_SQProblem.m
deleted file mode 100644
index afde11103d5..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/load_example_SQProblem.m
+++ /dev/null
@@ -1,93 +0,0 @@
-%%
-%%	This file is part of qpOASES.
-%%
-%%	qpOASES -- An Implementation of the Online Active Set Strategy.
-%%	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%%	Christian Kirches et al. All rights reserved.
-%%
-%%	qpOASES 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.
-%%
-%%	qpOASES 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 qpOASES; if not, write to the Free Software
-%%	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-%%
-
-
-
-%%
-%%	Filename:  interfaces/simulink/load_example_SQProblem.m
-%%	Author:    Hans Joachim Ferreau (thanks to Aude Perrin)
-%%	Version:   3.0
-%%	Date:      2007-2014
-%%
-
-
-
-clear all;
-
-
-%% setup QP data
-simulationTime  =  [0;0.1];
-
-H.time = simulationTime;
-data1 = [1.0,0.0,0.0,0.5];
-data2 = [1.0,0.5,0.5,0.5];
-H.signals.values = [data1; data2];
-H.signals.dimensions = length(data1);
-
-g.time = simulationTime;
-data1 = [1.5,1.0];
-data2 = [1.0,1.5];
-g.signals.values = [data1; data2];
-g.signals.dimensions = length(data1);
-
-A.time = simulationTime;
-data1 = [1.0,1.0];
-data2 = [1.0,5.0];
-A.signals.values = [data1; data2];
-A.signals.dimensions = length(data1);
-
-lb.time = simulationTime;
-data1 = [0.5,-2.0];
-data2 = [0.0,-1.0];
-lb.signals.values = [data1; data2];
-lb.signals.dimensions = length(data1);
-
-ub.time = simulationTime;
-data1 = [5.0,2.0];
-data2 = [5.0,-0.5];
-ub.signals.values = [data1; data2];
-ub.signals.dimensions = length(data1);
-
-lbA.time = simulationTime;
-data1 = [-1.0];
-data2 = [-2.0];
-lbA.signals.values = [data1; data2];
-lbA.signals.dimensions = length(data1);
-
-ubA.time = simulationTime;
-data1 = [2.0];
-data2 = [1.0];
-ubA.signals.values = [data1; data2];
-ubA.signals.dimensions = length(data1);
-
-
-clear simulationTime data1 data2
-
-
-%% open corresponding simulink example
-open( 'example_SQProblem.mdl' );
-
-
-
-%%
-%%	end of file
-%%
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/make.m b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/make.m
deleted file mode 100644
index 96352251986..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/make.m
+++ /dev/null
@@ -1,239 +0,0 @@
-function [] = make( varargin )
-%MAKE Compiles the Simulink interface of qpOASES.
-%
-%Type  make            to compile all interfaces that 
-%                      have been modified,
-%type  make clean      to delete all compiled interfaces,
-%type  make clean all  to first delete and then compile 
-%                      all interfaces,
-%type  make 'name'     to compile only the interface with
-%                      the given name (if it has been modified),
-%type  make 'opt'      to compile all interfaces using the 
-%                      given compiler options.
-%
-%Copyright (C) 2013-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%Christian Kirches et al. All rights reserved.
-
-%%
-%%	This file is part of qpOASES.
-%%
-%%	qpOASES -- An Implementation of the Online Active Set Strategy.
-%%	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-%%	Christian Kirches et al. All rights reserved.
-%%
-%%	qpOASES 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.
-%%
-%%	qpOASES 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 qpOASES; if not, write to the Free Software
-%%	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-%%
-
-%%
-%%	Filename:  interfaces/simulink/make.m
-%%	Author:    Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
-%%	Version:   3.0
-%%	Date:      2007-2014
-%%
-
-       
-    %% consistency check
-    if ( exist( [pwd, '/make.m'],'file' ) == 0 )
-        error( ['ERROR (',mfilename '.m): Run this make script directly within the directory', ...
-                '<qpOASES-inst-dir>/interfaces/simulink, please.'] );
-    end
-
-
-    if ( nargin > 2 )
-        error( ['ERROR (',mfilename '.m): At most two make arguments supported!'] );
-    else
-        [ doClean,fcnNames,userFlags ] = analyseMakeArguments( nargin,varargin );
-    end
-
-    
-    %% define compiler settings
-    QPOASESPATH = '../../';
-    
-    DEBUGFLAGS = ' ';
-    %DEBUGFLAGS = ' -g CXXDEBUGFLAGS=''$CXXDEBUGFLAGS -Wall -pedantic -Wshadow'' ';
-
-    IFLAGS = [ '-I. -I',QPOASESPATH,'include',' -I',QPOASESPATH,'src',' ' ];
-    CPPFLAGS = [ IFLAGS, DEBUGFLAGS, '-largeArrayDims -D__cpluplus -D__MATLAB__ -D__SINGLE_OBJECT__',' ' ];
-    defaultFlags = '-O '; %% -D__NO_COPYRIGHT__ -D__SUPPRESSANYOUTPUT__
-
-    if ( ispc == 0 )
-        CPPFLAGS  = [ CPPFLAGS, '-DLINUX ',' ' ]; 
-    else
-        CPPFLAGS  = [ CPPFLAGS, '-DWIN32 ',' ' ];
-    end
-
-    if ( isempty(userFlags) > 0 )
-        CPPFLAGS = [ CPPFLAGS, defaultFlags,' ' ];
-    else
-        CPPFLAGS = [ CPPFLAGS, userFlags,' ' ];
-    end
-
-    mexExt = eval('mexext');
-    
-    
-    %% ensure copyright notice is displayed
-    if ~isempty( strfind( CPPFLAGS,'-D__NO_COPYRIGHT__' ) )
-        printCopyrightNotice( );
-    end
-    
-    
-    %% clean if desired
-    if ( doClean > 0 )
-        
-        eval( 'delete *.o;' );
-        eval( ['delete *.',mexExt,'*;'] );
-        disp( [ 'INFO (',mfilename '.m): Cleaned all compiled files.'] );
-        pause( 0.2 );
-        
-    end
-    
-    
-    if ( ~isempty(userFlags) )
-        disp( [ 'INFO (',mfilename '.m): Compiling all files with user-defined compiler flags (''',userFlags,''')...'] );
-    end
-    
-
-    %% call mex compiler
-    for ii=1:length(fcnNames)
-        
-        cmd = [ 'mex -output ', fcnNames{ii}, ' ', CPPFLAGS, [fcnNames{ii},'.cpp'] ];
-        
-        if ( exist( [fcnNames{ii},'.',mexExt],'file' ) == 0 )
-            
-            eval( cmd );
-            disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' successfully created.'] );
-            
-        else
-            
-            % check modification time of source/Make files and compiled mex file
-            cppFile = dir( [pwd,'/',fcnNames{ii},'.cpp'] );
-            cppFileTimestamp = getTimestamp( cppFile );
-
-            makeFile = dir( [pwd,'/make.m'] );
-            makeFileTimestamp = getTimestamp( makeFile );
-            
-            mexFile = dir( [pwd,'/',fcnNames{ii},'.',mexExt] );
-            if ( isempty(mexFile) == 0 )
-                mexFileTimestamp = getTimestamp( mexFile );
-            else
-                mexFileTimestamp = 0;
-            end
-            
-            if ( ( cppFileTimestamp   >= mexFileTimestamp ) || ...
-                 ( makeFileTimestamp  >= mexFileTimestamp ) )
-                eval( cmd );
-                disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' successfully created.'] );
-            else            
-                disp( [ 'INFO (',mfilename '.m): ', fcnNames{ii},'.',mexExt, ' already exists.'] );
-            end
-            
-        end
-        
-    end
-
-    %% add qpOASES directory to path
-    path( path,pwd );
-
-end
-
-
-function [ doClean,fcnNames,userIFlags ] = analyseMakeArguments( nArgs,args )
-
-    doClean = 0;
-    fcnNames = [];
-    userIFlags = [];
-
-    switch ( nArgs )
-        
-        case 1
-            if ( strcmp( args{1},'all' ) > 0 )
-                fcnNames = { 'qpOASES_QProblemB','qpOASES_QProblem','qpOASES_SQProblem' };
-            elseif ( strcmp( args{1},'qpOASES_QProblemB' ) > 0 )
-                fcnNames = { 'qpOASES_QProblemB' };
-            elseif ( strcmp( args{1},'qpOASES_QProblem' ) > 0 )
-                fcnNames = { 'qpOASES_QProblem' };
-            elseif ( strcmp( args{1},'qpOASES_SQProblem' ) > 0 )
-                fcnNames = { 'qpOASES_SQProblem' };
-            elseif ( strcmp( args{1},'clean' ) > 0 )
-                doClean = 1;
-            elseif ( strcmp( args{1}(1),'-' ) > 0 )
-                % make clean all with user-specified compiler flags
-                userIFlags = args{1};
-                doClean = 1;
-                fcnNames = { 'qpOASES_QProblemB','qpOASES_QProblem','qpOASES_SQProblem' };
-            else
-                error( ['ERROR (',mfilename '.m): Invalid first argument (''',args{1},''')!'] );
-            end
-
-        case 2
-            if ( strcmp( args{1},'clean' ) > 0 )
-                doClean = 1;
-            else
-                error( ['ERROR (',mfilename '.m): First argument must be ''clean'' if two arguments are provided!'] );
-            end
-            
-            if ( strcmp( args{2},'all' ) > 0 )
-                fcnNames = { 'qpOASES_QProblemB','qpOASES_QProblem','qpOASES_SQProblem' };
-            elseif ( strcmp( args{2},'qpOASES_QProblemB' ) > 0 )
-                fcnNames = { 'qpOASES_QProblemB' };
-            elseif ( strcmp( args{2},'qpOASES_QProblem' ) > 0 )
-                fcnNames = { 'qpOASES_QProblem' };
-            elseif ( strcmp( args{2},'qpOASES_SQProblem' ) > 0 )
-                fcnNames = { 'qpOASES_SQProblem' };
-            else
-                error( ['ERROR (',mfilename '.m): Invalid second argument (''',args{2},''')!'] );
-            end
-            
-        otherwise
-            doClean = 0;
-            fcnNames = { 'qpOASES_QProblemB','qpOASES_QProblem','qpOASES_SQProblem' };
-            userIFlags = [];            
-    end
-    
-end
-
-
-function [ timestamp ] = getTimestamp( dateString )
-
-    try 
-        timestamp = dateString.datenum;
-    catch
-        timestamp = Inf;
-    end
-
-end
-
-
-function [ ] = printCopyrightNotice( )
-
-    disp( ' ' );
-    disp( 'qpOASES -- An Implementation of the Online Active Set Strategy.' );
-    disp( 'Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,' );
-    disp( 'Christian Kirches et al. All rights reserved.' );
-    disp( ' ' );
-    disp( 'qpOASES is distributed under the terms of the' );
-    disp( 'GNU Lesser General Public License 2.1 in the hope that it will be' );
-    disp( 'useful, but WITHOUT ANY WARRANTY; without even the implied warranty' );
-    disp( 'of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.' );
-    disp( 'See the GNU Lesser General Public License for more details.' );
-    disp( ' ' );
-    disp( ' ' );
-
-end
-
-
-%%
-%%	end of file
-%%
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_QProblem.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_QProblem.cpp
deleted file mode 100644
index e5cba8ffd18..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_QProblem.cpp
+++ /dev/null
@@ -1,447 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file interfaces/simulink/qpOASES_QProblem.cpp
- *	\author Hans Joachim Ferreau (thanks to Aude Perrin)
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Interface for Simulink(R) that enables to call qpOASES as a S function
- *  (variant for QPs with fixed matrices).
- *
- */
-
-
-#include <stdlib.h>
-
-#include <qpOASES.hpp>
-
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-#define S_FUNCTION_NAME   qpOASES_QProblem		/**< Name of the S function. */
-#define S_FUNCTION_LEVEL  2						/**< S function level. */
-
-#define MDL_START								/**< Activate call to mdlStart. */
-
-#include "simstruc.h"
-
-
-/* SETTINGS: */
-#define SAMPLINGTIME    -1						/**< Sampling time. */
-#define NCONTROLINPUTS  2						/**< Number of control inputs. */
-#define NWSR            100						/**< Maximum number of working set recalculations. */
-
-
-static void mdlInitializeSizes (SimStruct *S)   /* Init sizes array */
-{
-	int nU = NCONTROLINPUTS;
-
-	/* Specify the number of continuous and discrete states */
-	ssSetNumContStates(S, 0);
-	ssSetNumDiscStates(S, 0);
-
-	/* Specify the number of intput ports */
-	if ( !ssSetNumInputPorts(S, 7) )
-		return;
-
-	/* Specify the number of output ports */
-	if ( !ssSetNumOutputPorts(S, 4) )
-		return;
-
-	/* Specify dimension information for the input ports */
-	ssSetInputPortVectorDimension(S, 0, DYNAMICALLY_SIZED);	/* H */
-	ssSetInputPortVectorDimension(S, 1, DYNAMICALLY_SIZED); /* g */
-	ssSetInputPortVectorDimension(S, 2, DYNAMICALLY_SIZED); /* A */
-	ssSetInputPortVectorDimension(S, 3, DYNAMICALLY_SIZED); /* lb */
-	ssSetInputPortVectorDimension(S, 4, DYNAMICALLY_SIZED); /* ub */
-	ssSetInputPortVectorDimension(S, 5, DYNAMICALLY_SIZED); /* lbA */
-	ssSetInputPortVectorDimension(S, 6, DYNAMICALLY_SIZED); /* ubA */
-
-	/* Specify dimension information for the output ports */
-	ssSetOutputPortVectorDimension(S, 0, 1 );   /* fval */
-	ssSetOutputPortVectorDimension(S, 1, nU );  /* uOpt */
-	ssSetOutputPortVectorDimension(S, 2, 1 );   /* exitflag */
-	ssSetOutputPortVectorDimension(S, 3, 1 );   /* iter */
-
-	/* Specify the direct feedthrough status */
-	ssSetInputPortDirectFeedThrough(S, 0, 1);
-	ssSetInputPortDirectFeedThrough(S, 1, 1);
-	ssSetInputPortDirectFeedThrough(S, 2, 1);
-	ssSetInputPortDirectFeedThrough(S, 3, 1);
-	ssSetInputPortDirectFeedThrough(S, 4, 1);
-	ssSetInputPortDirectFeedThrough(S, 5, 1);
-	ssSetInputPortDirectFeedThrough(S, 6, 1);
-
-	/* One sample time */
-	ssSetNumSampleTimes(S, 1);
-
-	/* global variables:
-     * 0: problem
-     * 1: H
-     * 2: g
-     * 3: A
-     * 4: lb
-     * 5: ub
-     * 6: lbA
-     * 7: ubA
-     * 8: count
-     */
-
-	/* Specify the size of the block's pointer work vector */
-    ssSetNumPWork(S, 9);
-}
-
-
-#if defined(MATLAB_MEX_FILE)
-
-#define MDL_SET_INPUT_PORT_DIMENSION_INFO
-#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
-
-static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
-{
-	if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
-		return;
-}
-
-static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
-{
-	if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
-		return;
-}
-
-#endif
-
-
-static void mdlInitializeSampleTimes(SimStruct *S)
-{
-	ssSetSampleTime(S, 0, SAMPLINGTIME);
-	ssSetOffsetTime(S, 0, 0.0);
-}
-
-
-static void mdlStart(SimStruct *S)
-{
-	USING_NAMESPACE_QPOASES
-
-	int nU = NCONTROLINPUTS;
-	int size_H, size_g, size_A, size_lb, size_ub, size_lbA, size_ubA;
-	int nV, nC;
-
-	QProblem* problem;
-	real_t* count;
-
-
-	/* get block inputs dimensions */
-	size_H   = ssGetInputPortWidth(S, 0);
-	size_g   = ssGetInputPortWidth(S, 1);
-	size_A   = ssGetInputPortWidth(S, 2);
-	size_lb  = ssGetInputPortWidth(S, 3);
-	size_ub  = ssGetInputPortWidth(S, 4);
-	size_lbA = ssGetInputPortWidth(S, 5);
-	size_ubA = ssGetInputPortWidth(S, 6);
-
-
-	/* dimension checks */
-	nV = size_g;
-	nC = (int) ( ((real_t) size_A) / ((real_t) nV) );
-
-
-	if ( NWSR < 0 )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Maximum number of iterations must not be negative!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( nV <= 0 )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( size_H != nV*nV )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( ( nU < 1 ) || ( nU > nV ) )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Invalid number of control inputs!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( size_lb != nV )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( size_ub != nV )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( size_lbA != nC )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( size_ubA != nC )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-
-	/* allocate QProblem object */
-	problem = new QProblem( nV,nC );
-	if ( problem == 0 )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Unable to create QProblem object!" );
-		#endif
-		#endif
-		return;
-	}
-	
-	Options problemOptions;
-	problemOptions.setToMPC();
-	problem->setOptions( problemOptions );
-
-	#ifndef __DEBUG__
-	problem->setPrintLevel( PL_LOW );
-	#endif
-	#ifdef __SUPPRESSANYOUTPUT__
-	problem->setPrintLevel( PL_NONE );
-	#endif
-	#ifdef __DSPACE__
-	problem->setPrintLevel( PL_NONE );
-	#endif
-
-	ssGetPWork(S)[0] = (void *) problem;
-
-	/* allocate memory for QP data ... */
-	ssGetPWork(S)[1] = (void *) calloc( size_H, sizeof(real_t) );	/* H */
-	ssGetPWork(S)[2] = (void *) calloc( size_g, sizeof(real_t) );	/* g */
-	ssGetPWork(S)[3] = (void *) calloc( size_A, sizeof(real_t) );	/* A */
-	ssGetPWork(S)[4] = (void *) calloc( size_lb, sizeof(real_t) );	/* lb */
-	ssGetPWork(S)[5] = (void *) calloc( size_ub, sizeof(real_t) );	/* ub */
-	ssGetPWork(S)[6] = (void *) calloc( size_lbA, sizeof(real_t) );	/* lbA */
-	ssGetPWork(S)[7] = (void *) calloc( size_ubA, sizeof(real_t) );	/* ubA */
-	ssGetPWork(S)[8] = (void *) calloc( 1, sizeof(real_t) ); /* count */
-
-	/* reset counter */
-	count = (real_t *) ssGetPWork(S)[8];
-	count[0] = 0.0;
-}
-
-
-static void mdlOutputs(SimStruct *S, int_T tid)
-{
-	USING_NAMESPACE_QPOASES
-
-	int i;
-	int nV, nC;
-	returnValue status;
-
-	int nWSR = NWSR;
-	int nU   = NCONTROLINPUTS;
-
-	InputRealPtrsType in_H, in_g, in_A, in_lb, in_ub, in_lbA, in_ubA;
-
-	QProblem* problem;
-	real_t *H, *g, *A, *lb, *ub, *lbA, *ubA, *count;
-
-	real_t *xOpt;
-
-	real_T *out_objVal, *out_uOpt, *out_status, *out_nWSR;
-
-
-	/* get pointers to block inputs ... */
-	in_H   = ssGetInputPortRealSignalPtrs(S, 0);
-	in_g   = ssGetInputPortRealSignalPtrs(S, 1);
-	in_A   = ssGetInputPortRealSignalPtrs(S, 2);
-	in_lb  = ssGetInputPortRealSignalPtrs(S, 3);
-	in_ub  = ssGetInputPortRealSignalPtrs(S, 4);
-	in_lbA = ssGetInputPortRealSignalPtrs(S, 5);
-	in_ubA = ssGetInputPortRealSignalPtrs(S, 6);
-
-
-	/* ... and to the QP data */
-	problem = (QProblem *) ssGetPWork(S)[0];
-
-	H   = (real_t *) ssGetPWork(S)[1];
-	g   = (real_t *) ssGetPWork(S)[2];
-	A   = (real_t *) ssGetPWork(S)[3];
-	lb  = (real_t *) ssGetPWork(S)[4];
-	ub  = (real_t *) ssGetPWork(S)[5];
-	lbA = (real_t *) ssGetPWork(S)[6];
-	ubA = (real_t *) ssGetPWork(S)[7];
-
-	count = (real_t *) ssGetPWork(S)[8];
-
-
-	/* setup QP data */
-	nV = ssGetInputPortWidth(S, 1); /* nV = size_g */
-	nC = (int) ( ((real_t) ssGetInputPortWidth(S, 2)) / ((real_t) nV) ); /* nC = size_A / size_g */
-
-	for ( i=0; i<nV*nV; ++i )
-		H[i] = (*in_H)[i];
-
-	for ( i=0; i<nC*nV; ++i )
-		A[i] = (*in_A)[i];
-
-	for ( i=0; i<nV; ++i )
-	{
-		g[i] = (*in_g)[i];
-		lb[i] = (*in_lb)[i];
-		ub[i] = (*in_ub)[i];
-	}
-
-	for ( i=0; i<nC; ++i )
-	{
-		lbA[i] = (*in_lbA)[i];
-		ubA[i] = (*in_ubA)[i];
-	}
-
-	xOpt = new real_t[nV];
-
-	if ( count[0] == 0 )
-	{
-		/* initialise and solve first QP */
-		status = problem->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
-		problem->getPrimalSolution( xOpt );
-	}
-	else
-	{
-		/* solve neighbouring QP using hotstart technique */
-		status = problem->hotstart( g,lb,ub,lbA,ubA, nWSR,0 );
-		if ( ( status != SUCCESSFUL_RETURN ) && ( status != RET_MAX_NWSR_REACHED ) )
-		{
-			/* if an error occurs, reset problem data structures ... */
-			problem->reset( );
-            
-            /* ... and initialise/solve again with remaining number of iterations. */
-            int nWSR_retry = NWSR-nWSR;
-			status = problem->init( H,g,A,lb,ub,lbA,ubA, nWSR_retry,0 );
-			nWSR += nWSR_retry;
-			
-		}
-
-		/* obtain optimal solution */
-		problem->getPrimalSolution( xOpt );
-	}
-
-	/* generate block output: status information ... */
-	out_objVal = ssGetOutputPortRealSignal(S, 0);
-	out_uOpt   = ssGetOutputPortRealSignal(S, 1);
-	out_status = ssGetOutputPortRealSignal(S, 2);
-	out_nWSR   = ssGetOutputPortRealSignal(S, 3);
-
-	out_objVal[0] = ((real_T) problem->getObjVal( ));
-
-	for ( i=0; i<nU; ++i )
-		out_uOpt[i] = ((real_T) xOpt[i]);
-
-	out_status[0] = (real_t)getSimpleStatus( status );
-	out_nWSR[0] = ((real_T) nWSR);
-
-	/* increase counter */
-	count[0] = count[0] + 1;
-
-	delete[] xOpt;
-}
-
-
-static void mdlTerminate(SimStruct *S)
-{
-	USING_NAMESPACE_QPOASES
-
-	/* reset global message handler */
-	getGlobalMessageHandler( )->reset( );
-
-	int i;
-	for ( i=0; i<9; ++i )
-	{
-		if ( ssGetPWork(S)[i] != 0 )
-			free( ssGetPWork(S)[i] );
-	}
-}
-
-
-#ifdef  MATLAB_MEX_FILE
-#include "simulink.c"
-#else
-#include "cg_sfun.h"
-#endif
-
-
-#ifdef __cplusplus
-}
-#endif
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_QProblemB.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_QProblemB.cpp
deleted file mode 100644
index 404cb5e47a3..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_QProblemB.cpp
+++ /dev/null
@@ -1,392 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file interfaces/simulink/qpOASES_QProblemB.cpp
- *	\author Hans Joachim Ferreau (thanks to Aude Perrin)
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Interface for Simulink(R) that enables to call qpOASES as a S function
- *  (variant for simply bounded QPs with fixed matrices).
- *
- */
-
-
-#include <stdlib.h>
-
-#include <qpOASES.hpp>
-
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-#define S_FUNCTION_NAME   qpOASES_QProblemB		/**< Name of the S function. */
-#define S_FUNCTION_LEVEL  2						/**< S function level. */
-
-#define MDL_START								/**< Activate call to mdlStart. */
-
-#include "simstruc.h"
-
-
-/* SETTINGS */
-#define SAMPLINGTIME    -1						/**< Sampling time. */
-#define NCONTROLINPUTS  2						/**< Number of control inputs. */
-#define NWSR            100						/**< Maximum number of working set recalculations. */
-
-
-static void mdlInitializeSizes (SimStruct *S)   /* Init sizes array */
-{
-	int nU = NCONTROLINPUTS;
-
-	/* Specify the number of continuous and discrete states */
-	ssSetNumContStates(S, 0);
-	ssSetNumDiscStates(S, 0);
-
-	/* Specify the number of intput ports */
-	if ( !ssSetNumInputPorts(S, 4) )
-		return;
-
-	/* Specify the number of output ports */
-	if ( !ssSetNumOutputPorts(S, 4) )
-		return;
-
-	/* Specify dimension information for the input ports */
-	ssSetInputPortVectorDimension(S, 0, DYNAMICALLY_SIZED);	/* H */
-	ssSetInputPortVectorDimension(S, 1, DYNAMICALLY_SIZED);	/* g */
-	ssSetInputPortVectorDimension(S, 2, DYNAMICALLY_SIZED);	/* lb */
-	ssSetInputPortVectorDimension(S, 3, DYNAMICALLY_SIZED);	/* ub */
-
-	/* Specify dimension information for the output ports */
-	ssSetOutputPortVectorDimension(S, 0, 1 );   /* fval */
-	ssSetOutputPortVectorDimension(S, 1, nU );  /* uOpt */
-	ssSetOutputPortVectorDimension(S, 2, 1 );   /* exitflag */
-	ssSetOutputPortVectorDimension(S, 3, 1 );   /* iter */
-
-	/* Specify the direct feedthrough status */
-	ssSetInputPortDirectFeedThrough(S, 0, 1);
-	ssSetInputPortDirectFeedThrough(S, 1, 1);
-	ssSetInputPortDirectFeedThrough(S, 2, 1);
-	ssSetInputPortDirectFeedThrough(S, 3, 1);
-
-	/* One sample time */
-	ssSetNumSampleTimes(S, 1);
-
-	/* global variables:
-     * 0: problem
-     * 1: H
-     * 2: g
-     * 3: lb
-     * 4: ub
-     * 5: count
-     */
-
-	/* Specify the size of the block's pointer work vector */
-    ssSetNumPWork(S, 6);
-}
-
-
-#if defined(MATLAB_MEX_FILE)
-
-#define MDL_SET_INPUT_PORT_DIMENSION_INFO
-#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
-
-static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
-{
-	if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
-		return;
-}
-
-static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
-{
-	if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
-		return;
-}
-
-#endif
-
-
-static void mdlInitializeSampleTimes(SimStruct *S)
-{
-	ssSetSampleTime(S, 0, SAMPLINGTIME);
-	ssSetOffsetTime(S, 0, 0.0);
-}
-
-
-static void mdlStart(SimStruct *S)
-{
-	USING_NAMESPACE_QPOASES
-
-	int nU = NCONTROLINPUTS;
-	int size_H, size_g, size_lb, size_ub;
-	int nV;
-
-	QProblemB* problem;
-	real_t* count;
-
-
-	/* get block inputs dimensions */
-	size_H   = ssGetInputPortWidth(S, 0);
-	size_g   = ssGetInputPortWidth(S, 1);
-	size_lb  = ssGetInputPortWidth(S, 2);
-	size_ub  = ssGetInputPortWidth(S, 3);
-
-
-	/* dimension checks */
-	nV = size_g;
-
-	if ( NWSR < 0 )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Maximum number of iterations must not be negative!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( nV <= 0 )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( size_H != nV*nV )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( ( nU < 1 ) || ( nU > nV ) )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( size_lb != nV )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( size_ub != nV )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-
-	/* allocate QProblemB object */
-	problem = new QProblemB( nV );
-	if ( problem == 0 )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Unable to create QProblemB object!" );
-		#endif
-		#endif
-		return;
-	}
-
-	Options problemOptions;
-	problemOptions.setToMPC();
-	problem->setOptions( problemOptions );
-	
-	#ifndef __DEBUG__
-	problem->setPrintLevel( PL_LOW );
-	#endif
-	#ifdef __SUPPRESSANYOUTPUT__
-	problem->setPrintLevel( PL_NONE );
-	#endif
-	#ifdef __DSPACE__
-	problem->setPrintLevel( PL_NONE );
-	#endif
-
-	ssGetPWork(S)[0] = (void *) problem;
-
-	/* allocate memory for QP data ... */
-	ssGetPWork(S)[1] = (void *) calloc( size_H, sizeof(real_t) );	/* H */
-	ssGetPWork(S)[2] = (void *) calloc( size_g, sizeof(real_t) );	/* g */
-	ssGetPWork(S)[3] = (void *) calloc( size_lb, sizeof(real_t) );	/* lb */
-	ssGetPWork(S)[4] = (void *) calloc( size_ub, sizeof(real_t) );	/* ub */
-	ssGetPWork(S)[5] = (void *) calloc( 1, sizeof(real_t) );		/* count */
-
-	/* reset counter */
-	count = (real_t *) ssGetPWork(S)[5];
-	count[0] = 0.0;
-}
-
-
-static void mdlOutputs(SimStruct *S, int_T tid)
-{
-	USING_NAMESPACE_QPOASES
-
-	int i;
-	int nV;
-	returnValue status;
-
-	int nWSR = NWSR;
-	int nU   = NCONTROLINPUTS;
-
-	InputRealPtrsType in_H, in_g, in_lb, in_ub;
-
-	QProblemB* problem;
-	real_t *H, *g, *lb, *ub, *count;
-
-	real_t *xOpt;
-
-	real_T *out_objVal, *out_uOpt, *out_status, *out_nWSR;
-
-
-	/* get pointers to block inputs ... */
-	in_H  = ssGetInputPortRealSignalPtrs(S, 0);
-	in_g  = ssGetInputPortRealSignalPtrs(S, 1);
-	in_lb = ssGetInputPortRealSignalPtrs(S, 2);
-	in_ub = ssGetInputPortRealSignalPtrs(S, 3);
-
-	/* ... and to the QP data */
-	problem = (QProblemB *) ssGetPWork(S)[0];
-
-	H  = (real_t *) ssGetPWork(S)[1];
-	g  = (real_t *) ssGetPWork(S)[2];
-	lb = (real_t *) ssGetPWork(S)[3];
-	ub = (real_t *) ssGetPWork(S)[4];
-
-	count = (real_t *) ssGetPWork(S)[5];
-
-
-	/* setup QP data */
-	nV = ssGetInputPortWidth(S, 1); /* nV = size_g */
-
-	for ( i=0; i<nV*nV; ++i )
-		H[i] = (*in_H)[i];
-
-	for ( i=0; i<nV; ++i )
-	{
-		g[i] = (*in_g)[i];
-		lb[i] = (*in_lb)[i];
-		ub[i] = (*in_ub)[i];
-	}
-
-	xOpt = new real_t[nV];
-
-	if ( count[0] == 0 )
-	{
-		/* initialise and solve first QP */
-		status = problem->init( H,g,lb,ub, nWSR,0 );
-		problem->getPrimalSolution( xOpt );
-	}
-	else
-	{
-		/* solve neighbouring QP using hotstart technique */
-		status = problem->hotstart( g,lb,ub, nWSR,0 );
-		if ( ( status != SUCCESSFUL_RETURN ) && ( status != RET_MAX_NWSR_REACHED ) )
-		{
-			/* if an error occurs, reset problem data structures ... */
-			problem->reset( );
-            
-            /* ... and initialise/solve again with remaining number of iterations. */
-            int nWSR_retry = NWSR-nWSR;
-			status = problem->init( H,g,lb,ub, nWSR_retry,0 );
-            nWSR += nWSR_retry;
-		}
-
-		/* obtain optimal solution */
-		problem->getPrimalSolution( xOpt );
-	}
-
-	/* generate block output: status information ... */
-	out_objVal = ssGetOutputPortRealSignal(S, 0);
-	out_uOpt   = ssGetOutputPortRealSignal(S, 1);
-	out_status = ssGetOutputPortRealSignal(S, 2);
-	out_nWSR   = ssGetOutputPortRealSignal(S, 3);
-
-	out_objVal[0] = ((real_T) problem->getObjVal( ));
-
-	for ( i=0; i<nU; ++i )
-		out_uOpt[i] = ((real_T) xOpt[i]);
-
-	out_status[0] = (real_t)getSimpleStatus( status );
-	out_nWSR[0] = ((real_T) nWSR);
-
-	/* increase counter */
-	count[0] = count[0] + 1;
-
-	delete[] xOpt;
-}
-
-
-static void mdlTerminate(SimStruct *S)
-{
-	USING_NAMESPACE_QPOASES
-
-	/* reset global message handler */
-	getGlobalMessageHandler( )->reset( );
-
-	int i;
-	for ( i=0; i<6; ++i )
-	{
-		if ( ssGetPWork(S)[i] != 0 )
-			free( ssGetPWork(S)[i] );
-	}
-}
-
-
-#ifdef  MATLAB_MEX_FILE
-#include "simulink.c"
-#else
-#include "cg_sfun.h"
-#endif
-
-
-#ifdef __cplusplus
-}
-#endif
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_SQProblem.cpp b/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_SQProblem.cpp
deleted file mode 100644
index 533d07a4cda..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/interfaces/simulink/qpOASES_SQProblem.cpp
+++ /dev/null
@@ -1,460 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file interfaces/simulink/qpOASES_SQProblem.cpp
- *	\author Hans Joachim Ferreau (thanks to Aude Perrin)
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Interface for Simulink(R) that enables to call qpOASES as a S function
- *  (variant for QPs with varying matrices).
- *
- */
-
-
-#include <stdlib.h>
-
-#include <qpOASES.hpp>
-
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-#define S_FUNCTION_NAME   qpOASES_SQProblem		/**< Name of the S function. */
-#define S_FUNCTION_LEVEL  2						/**< S function level. */
-
-#define MDL_START								/**< Activate call to mdlStart. */
-
-#include "simstruc.h"
-
-
-/* SETTINGS */
-#define SAMPLINGTIME    -1						/**< Sampling time. */
-#define NCONTROLINPUTS  2						/**< Number of control inputs. */
-#define NWSR            100	    				/**< Maximum number of working set recalculations. */
-
-
-static void mdlInitializeSizes (SimStruct *S)   /* Init sizes array */
-{
-	int nU = NCONTROLINPUTS;
-
-	/* Specify the number of continuous and discrete states */
-	ssSetNumContStates(S, 0);
-	ssSetNumDiscStates(S, 0);
-
-	/* Specify the number of intput ports */
-	if ( !ssSetNumInputPorts(S, 7) )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Invalid number of input ports!" );
-		#endif
-		#endif
-		return;
-	}
-
-	/* Specify the number of output ports */
-	if ( !ssSetNumOutputPorts(S, 4) )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Invalid number of output ports!" );
-		#endif
-		#endif
-		return;
-	}
-
-	/* Specify dimension information for the input ports */
-	ssSetInputPortVectorDimension(S, 0, DYNAMICALLY_SIZED);	/* H */
-	ssSetInputPortVectorDimension(S, 1, DYNAMICALLY_SIZED); /* g */
-	ssSetInputPortVectorDimension(S, 2, DYNAMICALLY_SIZED); /* A */
-	ssSetInputPortVectorDimension(S, 3, DYNAMICALLY_SIZED); /* lb */
-	ssSetInputPortVectorDimension(S, 4, DYNAMICALLY_SIZED); /* ub */
-	ssSetInputPortVectorDimension(S, 5, DYNAMICALLY_SIZED); /* lbA */
-	ssSetInputPortVectorDimension(S, 6, DYNAMICALLY_SIZED); /* ubA */
-
-	/* Specify dimension information for the output ports */
-	ssSetOutputPortVectorDimension(S, 0, 1 );   /* fval */
-	ssSetOutputPortVectorDimension(S, 1, nU );  /* uOpt */
-	ssSetOutputPortVectorDimension(S, 2, 1 );   /* exitflag */
-	ssSetOutputPortVectorDimension(S, 3, 1 );   /* iter */
-
-	/* Specify the direct feedthrough status */
-	ssSetInputPortDirectFeedThrough(S, 0, 1);
-	ssSetInputPortDirectFeedThrough(S, 1, 1);
-	ssSetInputPortDirectFeedThrough(S, 2, 1);
-	ssSetInputPortDirectFeedThrough(S, 3, 1);
-	ssSetInputPortDirectFeedThrough(S, 4, 1);
-	ssSetInputPortDirectFeedThrough(S, 5, 1);
-	ssSetInputPortDirectFeedThrough(S, 6, 1);
-
-	/* One sample time */
-	ssSetNumSampleTimes(S, 1);
-
-	/* global variables:
-     * 0: problem
-     * 1: H
-     * 2: g
-     * 3: A
-     * 4: lb
-     * 5: ub
-     * 6: lbA
-     * 7: ubA
-     * 8: count
-     */
-
-	/* Specify the size of the block's pointer work vector */
-    ssSetNumPWork(S, 9);
-}
-
-
-#if defined(MATLAB_MEX_FILE)
-
-#define MDL_SET_INPUT_PORT_DIMENSION_INFO
-#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
-
-static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
-{
-	if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
-		return;
-}
-
-static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
-{
-	if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
-		return;
-}
-
-#endif
-
-
-static void mdlInitializeSampleTimes(SimStruct *S)
-{
-	ssSetSampleTime(S, 0, SAMPLINGTIME);
-	ssSetOffsetTime(S, 0, 0.0);
-}
-
-
-static void mdlStart(SimStruct *S)
-{
-	USING_NAMESPACE_QPOASES
-
-	int nU = NCONTROLINPUTS;
-	int size_H, size_g, size_A, size_lb, size_ub, size_lbA, size_ubA;
-	int nV, nC;
-
-	SQProblem* problem;
-	real_t* count;
-
-
-	/* get block inputs dimensions */
-	size_H   = ssGetInputPortWidth(S, 0);
-	size_g   = ssGetInputPortWidth(S, 1);
-	size_A   = ssGetInputPortWidth(S, 2);
-	size_lb  = ssGetInputPortWidth(S, 3);
-	size_ub  = ssGetInputPortWidth(S, 4);
-	size_lbA = ssGetInputPortWidth(S, 5);
-	size_ubA = ssGetInputPortWidth(S, 6);
-
-
-	/* dimension checks */
-	nV = size_g;
-	nC = (int) ( ((real_t) size_A) / ((real_t) nV) );
-
-	if ( NWSR < 0 )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Maximum number of iterations must not be negative!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( nV <= 0 )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( size_H != nV*nV )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( ( nU < 1 ) || ( nU > nV ) )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Invalid number of control inputs!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( size_lb != nV )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( size_ub != nV )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( size_lbA != nC )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-	if ( size_ubA != nC )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Dimension mismatch!" );
-		#endif
-		#endif
-		return;
-	}
-
-
-	/* allocate QProblem object */
-	problem = new SQProblem( nV,nC );
-	if ( problem == 0 )
-	{
-		#ifndef __DSPACE__
-		#ifndef __XPCTARGET__
-		mexErrMsgTxt( "ERROR (qpOASES): Unable to create QProblem object!" );
-		#endif
-		#endif
-		return;
-	}
-
-	Options problemOptions;
-	problemOptions.setToMPC();
-	problem->setOptions( problemOptions );
-
-	#ifndef __DEBUG__
-	problem->setPrintLevel( PL_LOW );
-	#endif
-	#ifdef __SUPPRESSANYOUTPUT__
-	problem->setPrintLevel( PL_NONE );
-	#endif
-	#ifdef __DSPACE__
-	problem->setPrintLevel( PL_NONE );
-	#endif
-
-	ssGetPWork(S)[0] = (void *) problem;
-
-	/* allocate memory for QP data ... */
-	ssGetPWork(S)[1] = (void *) calloc( size_H, sizeof(real_t) );	/* H */
-	ssGetPWork(S)[2] = (void *) calloc( size_g, sizeof(real_t) );	/* g */
-	ssGetPWork(S)[3] = (void *) calloc( size_A, sizeof(real_t) );	/* A */
-	ssGetPWork(S)[4] = (void *) calloc( size_lb, sizeof(real_t) );	/* lb */
-	ssGetPWork(S)[5] = (void *) calloc( size_ub, sizeof(real_t) );	/* ub */
-	ssGetPWork(S)[6] = (void *) calloc( size_lbA, sizeof(real_t) );	/* lbA */
-	ssGetPWork(S)[7] = (void *) calloc( size_ubA, sizeof(real_t) );	/* ubA */
-	ssGetPWork(S)[8] = (void *) calloc( 1, sizeof(real_t) ); /* count */
-
-	/* reset counter */
-	count = (real_t *) ssGetPWork(S)[8];
-	count[0] = 0.0;
-}
-
-
-
-static void mdlOutputs(SimStruct *S, int_T tid)
-{
-	USING_NAMESPACE_QPOASES
-
-	int i;
-	int nV, nC;
-	returnValue status;
-
-	int nWSR  = NWSR;
-	int nU   = NCONTROLINPUTS;
-
-	InputRealPtrsType in_H, in_g, in_A, in_lb, in_ub, in_lbA, in_ubA;
-
-	SQProblem* problem;
-	real_t *H, *g, *A, *lb, *ub, *lbA, *ubA, *count;
-
-	real_t *xOpt;
-
-	real_T *out_objVal, *out_uOpt, *out_status, *out_nWSR;
-
-
-	/* get pointers to block inputs ... */
-	in_H   = ssGetInputPortRealSignalPtrs(S, 0);
-	in_g   = ssGetInputPortRealSignalPtrs(S, 1);
-	in_A   = ssGetInputPortRealSignalPtrs(S, 2);
-	in_lb  = ssGetInputPortRealSignalPtrs(S, 3);
-	in_ub  = ssGetInputPortRealSignalPtrs(S, 4);
-	in_lbA = ssGetInputPortRealSignalPtrs(S, 5);
-	in_ubA = ssGetInputPortRealSignalPtrs(S, 6);
-
-
-	/* ... and to the QP data */
-	problem = (SQProblem *) ssGetPWork(S)[0];
-
-	H   = (real_t *) ssGetPWork(S)[1];
-	g   = (real_t *) ssGetPWork(S)[2];
-	A   = (real_t *) ssGetPWork(S)[3];
-	lb  = (real_t *) ssGetPWork(S)[4];
-	ub  = (real_t *) ssGetPWork(S)[5];
-	lbA = (real_t *) ssGetPWork(S)[6];
-	ubA = (real_t *) ssGetPWork(S)[7];
-
-	count = (real_t *) ssGetPWork(S)[8];
-
-
-	/* setup QP data */
-	nV = ssGetInputPortWidth(S, 1); /* nV = size_g */
-	nC = (int) ( ((real_t) ssGetInputPortWidth(S, 2)) / ((real_t) nV) ); /* nC = size_A / size_g */
-
-	for ( i=0; i<nV*nV; ++i )
-		H[i] = (*in_H)[i];
-
-	for ( i=0; i<nC*nV; ++i )
-		A[i] = (*in_A)[i];
-
-	for ( i=0; i<nV; ++i )
-	{
-		g[i] = (*in_g)[i];
-		lb[i] = (*in_lb)[i];
-		ub[i] = (*in_ub)[i];
-	}
-
-	for ( i=0; i<nC; ++i )
-	{
-		lbA[i] = (*in_lbA)[i];
-		ubA[i] = (*in_ubA)[i];
-	}
-
-	xOpt = new real_t[nV];
-
-	if ( count[0] == 0 )
-	{
-		/* initialise and solve first QP */
-		status = problem->init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
-		problem->getPrimalSolution( xOpt );
-	}
-	else
-	{
-		/* solve neighbouring QP using hotstart technique */
-		status = problem->hotstart( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
-		if ( ( status != SUCCESSFUL_RETURN ) && ( status != RET_MAX_NWSR_REACHED ) )
-		{
-			/* if an error occurs, reset problem data structures ... */
-			problem->reset( );
-            
-            /* ... and initialise/solve again with remaining number of iterations. */
-            int nWSR_retry = NWSR-nWSR;
-			status = problem->init( H,g,A,lb,ub,lbA,ubA, nWSR_retry,0 );
-            nWSR += nWSR_retry;
-		}
-	        
-        /* obtain optimal solution */
-        problem->getPrimalSolution( xOpt );
-	}
-
-	/* generate block output: status information ... */
-	out_objVal = ssGetOutputPortRealSignal(S, 0);
-	out_uOpt   = ssGetOutputPortRealSignal(S, 1);
-	out_status = ssGetOutputPortRealSignal(S, 2);
-	out_nWSR   = ssGetOutputPortRealSignal(S, 3);
-
-	out_objVal[0] = ((real_T) problem->getObjVal( ));
-
-	for ( i=0; i<nU; ++i )
-		out_uOpt[i] = ((real_T) xOpt[i]);
-
-	out_status[0] = (real_t)getSimpleStatus( status );
-	out_nWSR[0] = ((real_T) nWSR);
-
-	/* increase counter */
-	count[0] = count[0] + 1;
-
-	delete[] xOpt;
-}
-
-
-static void mdlTerminate(SimStruct *S)
-{
-	USING_NAMESPACE_QPOASES
-
-	/* reset global message handler */
-	getGlobalMessageHandler( )->reset( );
-
-	int i;
-	for ( i=0; i<9; ++i )
-	{
-		if ( ssGetPWork(S)[i] != 0 )
-			free( ssGetPWork(S)[i] );
-	}
-}
-
-
-#ifdef  MATLAB_MEX_FILE
-#include "simulink.c"
-#else
-#include "cg_sfun.h"
-#endif
-
-
-#ifdef __cplusplus
-}
-#endif
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/libs/libqpOASES.so b/3rdparty/qpOASES/qpOASES-3.0/libs/libqpOASES.so
deleted file mode 120000
index 6b42ca6417a..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/libs/libqpOASES.so
+++ /dev/null
@@ -1 +0,0 @@
-libqpOASES.so.3.0
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/libs/libqpOASES.so.3.0 b/3rdparty/qpOASES/qpOASES-3.0/libs/libqpOASES.so.3.0
deleted file mode 100755
index 363fa77a0367dd7857020474fa0ccfd668b2b2a4..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 1269403
zcmeFad3+O9_dh;`q6HBGqE<wWtOXI1vb7+Z0)?^%TA(V>l(x`fm(T^vB8E*{a0!Zt
zxE0qbqICf)K57KS3Mv5+tEfa=XaO;xXhnqIIrpAP&P*}|pXd9}@Adjzz0ADdbLM@|
zUFXi7ne?v2(ZiZVL?~)#s@$X?GF>J)8yzTpwXJHR*pxWsLio3f(oxtp3mggEnfh*|
zAl0p+m<2}l3m{f~D34ct*je!ER<Q`EcxbDh*M!b9e=E;3e=E;Zw<a`zpl+iiRL~k@
zjm%caR;nn9x<y+I_MeI~aldqoWc1rrt%RAPZq@U~!+Ds$^9?34oIvRrm5aT-G*q{g
z9g(+oaHuK7;bSK%&6|whe|G+-GgptTe#k#6(edP;z2@VY&7pic!qx_{B}7xxrb@J}
z?Yila-AqwcO3MhPh1v9xsX|#_<{o@7s@mJc(yV8sD>d`!+0RFGj8v8`vqU~;Ge>;9
zrR|VtS6h?S8WmwzB8Qrn4z)I^a+#n(Btc0>CVf216mxH^5}6$F;Hq-dP?OT6MU>L#
z*!UKawthclN12<&fCCX`lj5n`(lxT3&9tO#XGgoiF2!b2TCTK3DG^uLE{t|v5b@&R
zb#rY=Wlh{oqm%@j>F$bFwnfbqTg1A>N~CMsMT#xS6k&2X97C08rNservmwixc4^mc
z?zM^|(tK64Vv6Z}fnw?r=`$(oE@>T69%+tJT%DIF0~A}lBSMLCMA}B0hAK-Hm$@<`
zqJ_CRl+VQ}Wr`)cnLpCAc4)^-FK_u$=cOY%46Ru1UcR(V)X;?H9ij)D7P&W?FGx47
zu{|B7nA|OGvCo;KOam&LI!*0NWlcJoZIMZe$KDo-+oUveMYt|*>5fWhGC?ul+rj*3
zl(OUA<LjE3;ploLBFY)LXpyZ-aYZPzBO;dCToI1;$|h6f=nrj!AG!SQWlGZ@ZN*CD
z^-Da1?|u5OsEZZl%D%BtWiE5uHcdK3Emo9!Yb<RSxxO90acPqa+VzO&d`YRx8`Y$&
zqo?0x*E(F$X2llkQXsXCwk~msYmt&V7@Sg~f3!!f>j-V26U5FCF?500RqO@-=?1${
zTS^az*Fn4iq7`Bvh<zdUgV-Np9K-<-2SU6NB8HpfM*Mp-{F?wV5#lh2!y&2x8jUgn
z;z)?2RT8%2Af^zW#Q0R&zZJGqXnz`P(`erT+v&8Q3EM1)GYHS5Z8mLlXgdqGxeyBo
zJ7GJU_KRqXn_}88f$bdHFQqMR=0cnYaRCAfchYttY+ba!6t;KM{ynrUqis2Dm&5j6
zi1$N$5aL4+S3z6@aV^BhAwCK5DToyiF|3mt<!RVH1F;g~dWf$;d=+99#MdCc0g;D|
z3~Yk^w;)zS+zjy@i2s539z+ksEfBXt<Y60t4><w*9~0gQ+fQkK7i_%{cSHP~xV^O9
z2iq?p)<XOm;(mzVK>QZscMuOi{2n5PgAjiZdx~-hwsjEeAs!~~C~S|>K5l-6=!bY5
z;t7bqLsY{bu=6K@zhL_}?VqOY8Qj8P8wv43h!~<^+XCW65HE(<3L*v*Y}?R&Ti9Mg
z`|W6ZDQ(gAa@fBDVta^J65j!~SJOV)T*LOwuzxMYju1OR>;f?cVh@OF=n4D17`C$g
zK5QTD`vUIIxH#C4hj=5zn;_aCVn~4PVA_|Aq6`5%jJPD)j-c%*+N!pEI|h!8g*Xo4
z_%L?K#HGM?BE(6AC&M<C_EGN^*uNFxZNyE1?No?q5FHTHA!b0#gs6rYus;*xEaGm5
zZ7#$-h_fLULM(z<46y_vhB>sw{ZiVWOWXOhT|nDAX^XZCVgD||i(tD1;xdT$KrDk;
z4skiedm-Kr@d1c#h-z2~`>P1AW;`BWL-<kHK2G~j!1gKHuYm13+JBn1&%*Y3h%Z81
z5AkJ)YS;k#uMn<+?duRX5`K%eZ_^gf*$n&d5`GW1??d!J+yZec#O)9<d<5H%A?_sn
z32Z-w_!-1q5O+hwum`rEL;Mor*AVwZ{1)PO5Ha{@D-ra4hJS?ppCKM1uAUt`0{cfH
z{sQqB<5A~Vz<!9wA^ryOcZg~@N&A1m_D_fn5dVgF8sb@qN)z}Wq8ggQelv&{LX3jg
z0^&swFNW9(VjGAUE`jZ35HBYj4cqp#-vPE)LA(Z{nYe3V+Yw?Xh@ByJgNUI!Y<tkY
zWZ>FNxF>9{r~MmX8%z7WY1;?3eIfRP7zZ&P;y{RMxDobmf_O8;1c-TO%I7_kyd$xV
zZ%5*5pKd#R#fF!U<Scl(bKzfK?ETbpV8i~%4JF48=V#aSODo^}{Czt|tlUuIdgv4H
zFP8b;UxD*dET`_+X;~lNrj>JL{RnHum(4zySKjRJX+J-3-IV_K-FjiSX3ZA;vt!qd
zJ(|YvIoUP#wtCRpyx@s>%d)1uxqYmoGB)O`?aBq2Rd3&Syw$Q_pZsIQ!u4||Z~m?4
zfk%c$7W~sADd`>W+ykyDU)}wEuZ?rsPAI&1@sHDN-tAWnzOVb4CciD-Uc2ql$rqhI
zyMNWS|M~Jo-<%G6FS1s4db{812Y+~Oz-`Bmo;f&qN8-#o_bjmgxFfOkGyBUn{89ew
z@!7?erDwl5mH2dOL~Qpxrt!U3=Z)KQY1~!KH<YY+ctgo?IHz^%sHK1J8=Li=Z*14Q
z_NA`9ao4UJ=lt|mP5(Q)cs4xW^6l<mTebPUJv#?I=ejuNfgAEpeD?EQDPy~SGkDjH
z&rJJh*R{{~n|H_(|JCWkdC$*xxXX)%qz)~=EOv6wk})%{&pCP~`@s5PG3&2hy7uJJ
zGslxHA3Zo`f5+zQ#-5ruKJtynT3D8+PknRkRR5C~IahpV%`=@$9suX;-<<wd?N4uv
zi@nOcYS@6~gAW}jTHon|6*v4eclKBPq8H~ZEi(=K?#ZpMT=Hm#2@ASDYI&#Q{MUD!
zcz*8OH_kqDxa`}g7w`S1d3vuqtk?bodHQ_UjeY+(^33H=Wn?Xnc>d=pe=V84_-e~P
zQ?|VE^R9d64BES5QO=JE53jxC%a7h|`^CP`qL#GVKXdMZhu*!c`ML{74!`czo$uaZ
zp8V;;_fyAq?J#`Tjh(hW?0bLmKUcljYje~O<7!<EV>-;-@Ji1=-l;Adz0rH~oB4T1
z&&(`aeg8Lq9X&Jrsyi|(k~$q<^l0mQFWopHHRroco`+|*zi<1A4mX|Hbi>+)Cq9cG
zlK92Zd2r5E@29^tq3X*^PVTd0pMIm2d*WJ8yMbBjA8Pt(O25*6mo%?;f7ATJb(eY{
ztXQ1ft?uQ-l2b>Xxp~j%asFE#*s?LBXXT6cw{g$?=FrOC-`zES{mw~KHV*6NTYJHv
zbs4qY?|Oek?`J-4mi0-+i(_y5@z}IT-?a6<A0FR1efB*^lD>Fm>B1wAr?u+*{q2Pj
zpEaGBy7$R122SWObNsb&DUaN};rb7rXnpT(W3mgMo;Gp8+0!kac>Un<nZHc^vG4vD
z+naA%c6#~)r%v>pacslN_0?a$^Vn_eliOKm^gQxr`o|sbeXYr3i++9WS^sa>d~)dE
z=)&EfeY`#`<@N98+h0EM-cOs}n)1lz$(sg_aveRnY5b}$@4vook#)w=mlK!l%D?NG
zi0=+<I&<WiJEuG`w)@s~w{C>HfJ2YF|M+Fsq$y3C|KnW!_NLoE`rrZc!Pw{j{^6UY
z&rLkKe(9JU_uo5k_KoXbdFwUniXoGx%%9X@=7BMX1|_@_|4j0V3DaJC;H!7`)n*^q
zJ>j0M&yWAPN%YpQmLBR;cTsGUs9%>2dgRF;?_9a9S;6og-=wuq?&N;<NUv#*CD9WP
zuXtmB&+LN<t-st<mR(eE<ifKT|1k5NIR`rKd}z!Mtv0?8H}8>c5l>D@Uw7rtsn>5A
zSiJkaS1h|HWp%sZBikowrc0MS{mS)wCr#<T=_}tq@2%RjI`Ybm$NZI+gzgVddj7c=
zvkP7v)wS8|)W1F%K55FRoR_D&KK*lW^3H^d$Df+~-S<~k?yD?`h<I*bMDLoM$69?j
z>;0D}_V4uEf#U-<ZoKLHqBgc+J0F|BviH{5tnqJ5T6+2HuUo%-{pkhC2S)s!GAG{l
z*8I}l@6GPpXKm5Q6|=p49;=UcRX(+A<;Jh7Hq2hWd+e&OZyVD8*y^k^TSgzB-Q>dy
z>RuiGRFh9Hxp+bShTcE4>Dcq^r-P$*J=P-s{oi_T`|081gEqEY^zPhlmyBrc-L>}D
zZ>}hxo4w=0wXbx`oE15_@3gaZb0Z?gq#XOa`MpPazj1O_tA|$H-)-ehGw=H3=vv>O
zl~d3DShIdy`7cVulvS5E^c~Xmo&P*P^3mp3eSg!RscjSfKJ&`4k1n|5^9gJHdy@bD
z^!?Q2hjNCOp7^2Z$a|h0U%V{$o^IFJnhZM_bM>WfzB1<P=4br3k9cbCnK>_RoAJrk
z3m4_AKhW>xaXpXr%V_!6{dW(teRprs_z`VSmGtks@!2ME=^Ot1A^pz7FFo|hJ-wcu
z_Sxo>3*(+>`FPpNN8_iR9q_`%9j{)p^sAioC~M7^hsR#GFd`x&-t+6_KR@?Uw+T-F
z=@BbqthRf4-TlmV)2N&gbx+@a)AgnMO4~oY<K<HWc5J?(|Awh&YmO@sf17q>48P*G
zt0I5-^P0t<Ww*Yi<9(ACxt{;?<nKGTUOuw4{n3HNpJZfqm_0Z3$-5k{zA*ppH*YFG
zo8=t2|K?|^&p3a|9r18Nmzvl=z8}6UXZ^Rgm$evj;n@{i+gP1n9K7*H|JZ?#<Q!_a
z=#7aZ+76u8zi;$l*R}VL|L|~=xF)^MX18zOYIAwvo4pqIYqobo{GG#=7v^qxC+BnT
zJ-sF`cz$Jd|LCjJb6$LY-iATLbH41;{q^oASBx0D@V-^!ulxN<IIn2vJ-za$H~%2{
z+v6Wse_h)CtFKlr9sfYL$z5V2_TF>VZCA~nvFgruzD=6b_phB>Q^%IJf9l1j4{pfW
z{l>bxkF6VD@n+X|KP}z9f9|6ruU<8$^v9=H-`f43Urvp?<LBziOYVMYZ|=^miynFZ
z?RS27a6z9tSGnH0E3y5>Z@quy^rjzX?4J1H;j!<P7v^1&G4kD!%L|vj-C@Yd9$kjc
z|E*h#O+TKU{mA^O3qGwLn71$X?pnw3C$8!L-h*3Prd`%_S<d(;KHI+|aqVK$>xuTC
ze9JQ4x}o6XZHM<P**jq>=q*^^`JVW02RB^t;@eHWS+%-%^1w}_<LCcyck}x;l;mBw
zIq$%8ul(|l=Y#q8zq@qT<L&>GH}3hOVam?#mp@@jFWUL?YcJpN&(5s}=Fg7!YIbUe
zV`raf_#h&)>hR~kw%YNI`J&^mzc=QE$1mD)>Fk|buN}I#sA<V-H{WEdE*kz=w@m|!
z_Ov(M`r+a7<4MCZ{`hl$L)@7DZ=_bI4a)rD!AY;2dh(xr&5NcU7%^F$ml$oED$Nzg
z1vn`ShQpUBN^r$8$fK1x2m^cR20YGV!|8k%8J^#Fd3Zj}z@M`T;q5yZ_|ssZ^N2yZ
zaRxdQfbR%VAEGaTxvHK7yyepHem)8I;mYUq1>tq_49e{lgL1(8g>d@o`-j)hH7K7f
z1OBC(!s}dT;OCPD<@~ULpQ#sx_y2AK`+p4dd-V*j^RYoWADxQ!7b%WbkP!%3kRFCv
z%_Q&YCW!@-R;omPFH)RcC4W2dXJLU4b-aH`K7sh(NT&)HA0YH2zN<~zd+??O0yG;D
zb`O`lk}7#ztHCe=8tAo3JH=PiSzv6h7}mieBBtx48z<GzDLtgUYp=AgmdPs%dP{pV
z-poP3`!EdaCzH;grUECn78_+VG%8U(^q@e^@5&U(yN*hM#nN1P=xWKk=|;aL@ngXZ
zbxhaFbRjtrru3J*4Q~`7;2Iu=yRVnL|0ikRT++&0#ETnQ;GnsR@QYR2d+=rzf|{=R
z8p*p?X!-ZX6aSG+7w`2j*rrI{yjzyT2)R+3cappvZ~7sq`dv+uw={u<0pSs{Zz|gJ
zMT&2bv{(D)lp87Cb~3>h;w)HOA%AS!CBK^ZRs*EH6K|3ssO8p@%HMQE@-bw;xC8k^
zi!#vsL|8wJ_|ei{9lJh)=7II%rbUZm<o}}XlK0`_0t7X`kX^73y^j1kQrg?+Nc|jf
zmZCfctMEcULF!B=hgMyQ?ek*Aa<wcswf)bzf^_HxekSRBN&ef(|K?IoDW&?YQ0tfU
zAA<&i>6)*R8S+wwqp6+yEs}qf@;m2RY45`U3POa;hSEM+@|G_pLCsMqF=+pfLpq|J
z!}B5x?}&e4-G}<G<73$$)%qGLt{2c)ev|r#$e)WRk^k)^v7B^1r26$!{aT4X9WCu$
zG=Sk6Foq?h<7+Q<)c)QfR@!^1zq~EYmBKEPcYh$uL9O>`?Imx+1waUDdya?l#{BBX
z$xSxNiv=05SM&QQR5#k|$IrW|UsUM&#aZe{j+<q=&^$!An8tw$su0zm$7wwGHk0k@
znhT|Ura`}W;5zA#nHI`u*i({KKPSp`KcjTVQT_U9Je*DZQiE}LB=HX2_=#(HqMhi*
z!`)*gZ>g8<;Vsg?MdepZ{w%fg*WkJ=bm%$))hfcvR9_YO(*G}ruZIB#?L9Ou!L&q#
zmV+tX7^&Y~o~HbkDtTX;Y(M+R{xr4c3Tn>)Mff6K+Uv)wfuki~ai`SZL-roKyiO_K
z$bR7@{s7gBUso?VG@evYM_CdjPxy_>!A|ASjdXsZ_G8nvpJVN$KjxM)U(;lBQHD}K
z^5cRygf!BLx{CbVA^o&d`CI}CiSopSSO{u=`Mj^>Z8RUamvnAc$FWhe9Oe+8bDOmH
zmPvo+(1l?YKvBP4q<$vZKS=fKr26edymyea_u=9xgj=PYvIrVB>Nx(9#1=Kbq+_Oj
zt+t2$7SgGc=?)<Mf7J0gNAkFqjiD#ZuTbAl^Bi?Oeh`(j{}<`c1=McysJ{HTa0@|A
z*9TCPXGbagrZ@`EJCjI%nzY|2H_Bg7;ArnYBZ+=wf4Y_AEq<BbYbd{^lwTW_8w__M
zZ0{lMZ3kq!>U`rV8s99GuZ3ivNA1)8y|mv#{QbSipP#ilZN`y5k4ryaA^(qFD|z)n
z#V=G}3#c91>7h(d(jSr{?frD!UrPK!XsCESwbT0-Q0yY~Cw(tn*VOje7X-1~>_139
zRsIPmK;&(>fDZwNWf9(`@^{aWjt?XM|D<-}#RmrvRGm?9VL}}TJxow}8}xVNt2Rl2
z{*>Q=z41N^(rqGnwfv)QC4cgy;c6P7%MHf8Ck)27&2Rz2bUoilKh^6+bVteiAJh7O
zOS0t6UrB$c8z_I!_~X4@`a|7D$r~x{-6hf<s#^prwFkSdJ$R;2x<5#rXQ>>f7}Ref
z4D9H?8z00#P|I@wv}@!m#!Eg^9EE4sG!A%a9H4He^fF6((@JTtmh(!R<b5=L!n8_+
zx1c=nzQctRMF?*3ucBn%OzD#SmBjx#T=IUpf4Lm|!*Er1$=hi@siwQfVBCJ)VBDTg
z`t}rA&UoL8A+4v>cTjt7O8k|HlK0VgJ6M`4`)!i9-~%HF;GPI~Lw>Oxx~M$Wap|1|
zY44$VDGb-r9t^O2oOJzyZY#pCJ*2&z#uMrWik-$2{kYwo`iqbH(R|Xu`x2~Q{rG$d
zba3RIG_Fn{`xiP%zG|Z^0JWWbN$uZ9?O!ebQ`E10*|HoU`y%{sxzwphlR7(yKSJZD
zj~@2!Cw@E#qCZu=rF{b>5D)VKEFb$$*?vr9zX2Ky+KY$Cz^Une1ml<q-cYJKDecKz
zISjIBuOGJ;P&u2ZoYnmHCw*U6nXj27^aNbUP{)T493gBY{_T#EH&sag2auy@X@2fY
zmi&8Uzn8`V`y|Ozvr|f-AEUm#v*b6D{UK_H78*D&qYk+b`Zcz<stD;1Tt-E34W|6!
z14sz-qr^t}fa=}RL=G6QN?Q3D<{_w~kPc)=gi@+^JJtIn;y(aA^wUB0MeZmACQ4pu
zkm<f6v!RR~PyTF|{A$u^Am012<k!iKG7ntAdbfFHJ*x9VH~DWFFAXoFaqn5uank*`
zIzBhI$aK9l0IT&op2iK|wNgj*e`i-|@7yOHPoM)Gzzxi=Ws~I9_8-wj@~(F!uj<e5
zL;Oj}L$eX#I%+@Oi)6Y(<zZza+1tooy>40(aNP_>g*sUdRnTrQ%%J-e^WU-nW=mT6
z4^$BP>F*$U8eWyZXgs&kL-0guD49_2Xz!)@r%Xjr7SnyMGoA`KQYLU6WDxD0e@gy2
zI$Qu95ZjNF#yz^sD%}T4dpq5~kh$_0WM8CPAPeAk^8aCKZ&kW+W*5yHEi<J(KHtZ1
zu{xgQN*;<)gpb=uefKt*BD@MH!bBQRTzh0r)$!yt7>Cgx^J!^6P|7LSfj+iFGp(CJ
za}wdTWXb#YYjxVveOARv>EJz7ZqFKA@9w02RCT>f@Of%)RWSb-*AaZ-0fO4z)>@^X
ze%-ui8qJ^G^n%ib;w*Rvgv!T7<*Z(hJ{&>$&6D~xO;K*A`J3gm^ha&a8?L2vAJOu;
z)Sk^W9==KGnx{#7Z);fqw5*_<?JarNZ&GK2+$cqtN#2$!32NrbP2e!jODj4^9kpLa
zQM+=smi~W1>9$CfI-b$89&@RJk93#3nO<~(<}Jb-&>pbfeHUo!>k!=UWBGe&UGZwN
zfA1=3@0_bGhwHAByzP6*tMzpUjL)cFLGvycE=2eeGJ?Ekj!btPmBSl7CGWzQZ6I7s
zd?xsh?XZH{t-2mNHbL5pmsY@jwv<zf5+(1W@%C<^2JZ~fe6oV(le>sNP5sVK{Z1{P
z`)Qu(+$+nokUI8fP%r4e{R&wx>UiFa+MDTi={UZZguzYYu!UY4+E0FZq1|Hrny=K>
z?{;d3E^3F+%tWZZjq>$^^rxnk<eSs@Z`vY-X?mdyHW-)MQor+4f03yx%4*QVe0h_k
z<J29MmjEK~>L7WwUuVIE3H^7{4I&IvB8=`R?bVmY&PsQcrZBLhP6fWP3<0t!!eXHV
z<BV>7?-uYM>)p4#v#?u3_Wv0vb$oQckWc($)IKXprJo-VKgBHV%`|V9YKrpgXvzEU
zlK!ldrz!ntymicw#HXY)g2p9Z6KSuu^HwzeRM0pB-9dyr(SO@19`Zk$_;qx>^U(Ef
z38m{8NBMeE>L<yK;+#PI9!b!0h7xmw<X!Yq+D_8Hp2|llmiA?&-<;aNh1x$1J0c`d
zy(`0|q1tYnfWuf{PKV@+s2&Tc+*}K#emd!YOa0eP>xydm>`0b6-tVRT8&F;t7LSm;
z>2}Gh>#Mm`&VDNA1=3uJ1T$<8E-Fv;dQ=7N3frOEEL*Z#&L300e7g4hM~Y0>|EEkB
znwbd8sUPXbja-|w*Wce+rb@ozq%4@7R1R0c{0+-Zyxa=PR^o3QBYE30neLC&!K~Ly
z-lHqG6*NClXk4OVQ+Ckx#ea#+ueu)c9Nl-gr^y6&%WNsn+oisKd{{R`^7cv69~d@8
zD1m>`f6En8XE@dSBJfw_D@O9r{6u&IjwA1$E&0pkMu{bVJhw@Lnvt@E%1xp9hI(Cl
zNlljquu9S&29BY=|842#L@KxaEhS%Zv($&~Cc*<_$)47MRex%3A%3RJ(07!tlhh6s
zr_?{8^8IDIve5XZwyOjhpPi><x^%m!d_ndmvhP6t;D<Ble-*y?3t<TH^)z0&=z6N&
z2Th`U>DTQmVPb<iUK+QhxA6WhbY$f1G=HFKP?}LcvQt08dd2X~Q1WNDR8Ys0Pv98p
zm}wmwE)yc$Naf~n$bxx^%4ZvmWA;B~zUcN(`EsJv@kdF+hvi}A#V(Ss_&^fsdR?zJ
zlrCKt7E-!>Z<4(JKIQs2Sub{7y>x-{!STUF^HrMuC@Tj^9S5!F<dFU+qsgB|vK-Xw
z=EXfFuTX`IBKtE_Byay+=1aYQ*#!w<x!I;lo#!aOkJGqq+N&++0fVKz17EO(@C6JN
z7%HgV_4hRgFO~NG2~zlbX|8mk`G9$m^yg)o_!e{}{e99NWJRc^@!XXu`7^{9Qag0h
zbyFR;M^iiT-X#63paG~I&F5VmC2uAjKebPvu6=$sMEX-f_iMM3{ix28w{4RCSY<Yp
z6Y;VhJ#>Gej`Np7{}S!|jI>wV+d-NSn|1968iTO+(RE*aKKm54AGfZbI>>*I&i~Cc
z?&<F%zE!Uyf5>w9oAUK3IE;Rpj!Hk@kQ*hM+M%bNw4X@)8S1|kG)}7H;bm|mfI1!;
z-_+}Jo8gjojFCDssNDX9ibMM<dOk(Nq;iu%eN{olp}m93X9oHI8r8ddtJHr+Zj|v<
zkN%dC!ZM5eA5ZPaN$m%ks|cs5f0>s^fi|S`6Epx!*Zz=nyq+4U3pyI|&Uw=C57NnM
zCwcELlD~%dmq@2-lhio?<%J=Z`km_&Y2T3wVgl)VN+nOPBPczfz%gAfJqMxgq`XW0
z(M9DC(?SuJ_oe%Nx<6I>S9`S{Q)I!^kbNPwX9vAcqK->(G!Iv<llIR`x0D+4-}Z$h
zXj-6L7$eiQL`Yukr+>vt-b4IMN_QHJD>xoHX#KF7>Z=2_XGfkS)Oo^{RKFE8A5h0P
zh1zWuwOeviISb7|w4WlWvy#&Nk>;iDI?1c|FPpEE_7-|B`8L^qW-xC204Jh;RV$g|
zMzZfSLh^oEcO|!!lf#IwllE7W{j#2tchY^OT3=q6_@TajTyoz+_L;I>&6gXcGqoQR
zwVzi>e*pDIuU)1K%~FKcJ*19{?z`3cN}=*J)AdO0r;82R+hl6zz6H{e8)f%XI>Wpd
z{rAzhr#?44ZkN1?`U^~lMEJFz<UMp>sor<2rgq|@^+vkwS8k$u(T|6pfIX(`*NuCh
zx0bvzLe{&QugiK<yVcz{T}szAmr3RepVwj-09({|b&@*UBn>YpOWsx?4cCHy7*>yy
zyk(s%|84R#<%RB&_tSF|=;k7PMdjl>BJGQbKiNszduT$Zj)%=gOWwIa+EaH`?xA`&
zQ-6VGCc-x~pEv8CTg|0(?e9ySjdG*xru!o|wOh!h2)EI=RMk%AcPka>{xQ-YGp(}~
zk-d+`dHs8F&CQfAw=9@MvhPIWn3w8B?H9YD<A{E-K-&A^J_<vLmCAwE-{wnN*=v@(
zm#!mfJAa7A0SAo(YWd$nyq$P8-3#DEOxH>4>d<UNh=J=1u1EQ)VXFG8Xxy{WxaT1I
z4qeIr52gQa(}4RN^+z*3?@;~u@=9rMqHzplMYx8_!8u>*sD8TPDEetGmONaZM0ktx
z<<jNrJ}7XJuS8kluaW&&I30OArK|Rr_h>%jrwb;?icm-WT|W+=>_z&t4xsw^0u-Pa
z|G(-i&WCA_2>*c#D5hIQ?LoEA0{4WU*UEgU<NtK(7Y@2$4kkxCjhFWNdBPfK&v-v>
zNs<E<baN3dyNT@0vK-WPKU|i_bj1geU<sE|5pFaXC*SHO?M-xFthR?m7f8N>=5H{(
zi?D&_Y5MWdKUmu9*YgWgC2yH3``vNUxq`0aK3b=uW~IDF?XZH{q3X}Vj#5XViK<%8
z+o)bDs9qXmaVx{ROM3^6SF<VI>&Sooc=ZyEKmN(GJXQT2be(d}m;6c6Sq$SPmb3o;
zF_+5OLf4}%WIqIU(B4hYchq&fWzd|EcfBngSJOQ-MDm_NGF^53HgOW!(|dH#JVfZ)
zNAi}FTAehD<ju5Rs_K6T<$&$Ef}R(<NauSfPxQy~o77RSccvuC+i0A>j_kiq!{?+|
zD9%4*zaUz9j@nO^uKoN3%^3BSZc<;Zm&p?)U-g&F?^m+9E4^Av-k&7x)qXvY#veD$
z2UMNz9i+W}Jo!#7e;UWsayUujm44h9Nb^tIB$=*kz3_e!m4k`yPt|hRJC5>26Z}q5
zvLp`B{ZSR&A6-EOw2;cd(N(Ko105X8$3*w1d&u4rL;0e93fUAPWt!w|ZDfI|>sRSC
zjydSM*^%r!Lw%us#bwg|MOoZRHn@Sjtwj2x=IbN!(?@=)<NPNuev0-(_m$D4zmdw#
zdyCZBDmTh+a2)krU1fsM{6sMKlf0SMf7JH&3p8v|4z%tBvLdvm`qhtb_tACSvO?<r
zLJcM55;=Z)=(&$Ne%8Wq)YrdHRYCerDu+3g?l`y~LVIVL%ur9_3))EDMH6Qj7DSjy
z*BviCuT;yeiSVbLQbp^o>bN(9*7wX)rT%m(Hzh&pJ37gHJwf`5&62n4<_T@6J$vqu
z0?(6u5w+W@bgA>LtbWB#?aHPb|9^r8FY2p{)cJyR=282!d@6gykJK>^(fH%10eA%2
z-#bj|ct^=}DOu$+8aMRsR~(~uYoZ4`>iYJFG@hGu^M7m?ZQ(7XI(mLY$to$6WxDP@
z(!p9v_;vMqG)?l)P&t=EMTquB^X;kBf2Z{(|NYW1ffBgHE_us(nXiQ~q+$3O#yv4l
z&^@OZLhZ^%^PD<quCx*31B}D8&IZ*X!h>*qM1S0LU#zYh52SS)|5@peTFxI?rM>t_
z9u(dtoeLnFsN;1?`!|VS0Xo7@UAq5JIk;QM@}X&sGL`1xUb=x+`}<Uy*STn3*IFi{
zoP>6c={o4XNu593F_HYC_tLLX2lTYGnRx~IX+_29g~e%UN?J~SPO*|U12#(9h?FsD
znOTKdGjocHvkFtj49P9X&q_(3o|`3)g+G#(QJRhd>A5-cz;NW8v<ajzBsaaND62?G
zyLD{;+0JnZ$%)DR1{aj%XBPD>%FdZloCs+K55<qj&&(>#g|vrQjgG{pXJ(Eq$(x>4
zsI`dgJu|C#cy7V;^xQF7MMdc|vy#&DGjqXg4o||`mmkf^pJ}xeXBQUSkyuz*P?$J2
zIn8P<%qlJ^%%7B=TauM%AC-~ZI}QHN!$bWh<rL*i&&kaxo|{~pUR(l?Y{Pm#)T&K1
zn6LEY#0jI4El}zc^Y6&X&xC?1D#?Q~OPg6*n&!+ZEGocKES{S-$HKL#(E1e?K)P8X
z=lSV*V3C<_*W`~3hs5eUNG_Q^D=VWorGSp4Oww0~L0+IHGzr89l!ofkP?Of3K(y6T
z1m)@!m7kb{rP(*26s|A>2jif^MY~9}!&$+iJP#2y>AradbFzlzlxAh76bviOlI0i3
ze=tW__h=eExiIx%mC7Iq!l-}#PQ}m2&2iE>VU**F?nrk^U$tE%P%iOk5#CeH8B_>;
zorO91#j-9(seLh6n$|whePq8%C@f5$n`pO#N?K9AIBiB+M)vL8%ecI`cGR1JJ%k^i
zWbC7Ia>NlFW-@Na)6-_8=j3uz)v=7+taM#3r9%cSOy9j!Ym3NTW^ULX(Fcb)?T``7
zeYk^xf(~<#QiQ&wDe7QL(jQSz&<$%u_sJ-5&Q%A7;Bcs(6dQU-lM;<6p(k2HPbAIY
zI21f9s2T1kofs#2aYAmc7%Lm?!gPi;Ry{>DXU$-un)FspMi<<nAF)GhCOTo9gsLto
z&MYW_E7z?l$$`F1#yFOxj0nq48f&$r!3C={eR|FuYb@*+<)mfcWh$+Bt}_d+hBFFu
zrw0p0Ge%nbB^TtD6z3G=C%~9Jw<xEG#@0Z(VJvimV;GAtwH?MPOx1>3>HAL@i*x$J
z+d^R62o<u%!T2#Iy>!&5lsP#Cx#?Iea=;#MFD!uhL*C%T37Oda{~M7wRpfurh)+I$
zkytC_W>R`#PI`Vu){ufZw5QLIJriaI=g3oIQo(Eml+=+qa40r5GYh6-c{%x6$;DaD
zp*e+c85e~+Dl<7YE%3jXx(qHTfWb6B1sZZ9xCW|y(&YdDgO{w}Lfq^(CcU^Yr<58R
zpH#>Lnhv8H>)M=3%i%lFd4kD?st1p05(pHUzTpS$_16w|#4vqpKRG;=l)}_6y>RY$
zJ6!cNg%7m=1)P5e&d69HSgk{|@{6*9MUj#c$ez`jJU350EC!t5j4b3>;H;p#`mrXE
zcl`l&imu?<5x9_vIhi#H=FZUSMGL|<6Ve{>%GS6klZDIOdE15=_jsN{E^?Y=8ac`b
zu#nt@I~Wv>%PuGuw@G2@8>)Olj$Mpss;bojqs9>X#NwR1EbXK@kP$;gy;gW9G6LN_
zu%kcG8e5o^o;j?rAa7VsF5JLm(nO36ya8`wt+@s1nd9wqU`Qz_EDD|kcRg%q5xrjC
z2GMw@<~XdEa&A92FE0y*h74-_&|z@;PJKNwFfPH4J_lh%wf*h<PSIpSe@dVbc$GAI
z#(DZ>ABFjpH@VPm#JD`9Ait=%FdZh$+FQo3PH09Jbo<{ZhIcX~U;jojtdswfV$jKO
zLu>f<!x|3`zWvhV|9p&!S1<5`V=aM;CLho=bI(AwV*BWa_nC$1nK@bN-IChatyWfz
za9bQQ=4cGU+%;&9t2xij9g&|7SM)hXLq@E;aZVXO1I!8vbBg5L>iqIDxuEcNSma2~
zDwa*Lk#;E>DxW~`qGg4ZQIO{>Db9jf9WE$jWe&-PIY-g$a}7qkSepI^FP_uI`p^I7
zkESR?uIYhxVC@eJRwcPGeJjeMh9KPJ_Xh0hti3m|_J{j~FgFJ5ZiAg0WP`0+T-JlP
zA%+J6r3)vBwKQi*VOH9loWkOgbj>Yuz_4#&7BpIeBDdmF7_WKlygMiYW@B%aB7s?X
z;Hb4P93Bb|sCPMVEeYEstn!{88U);z73X9X8M!6z9Rv5(VveC{#=#D34QDGSU15Eo
zPBH@S@pIC1&(S2UeZbKf1%-JN((|0qI}H4djU}_8g|JdNq@Ylpd24(QmY1>1@?sj+
zadj97c8_y*3}puNKh2VAV9_O0F1iG#ZJHyE9Blkh4&F;>*Lh+sVqs_qEL;@k%*-wx
zTTq;nfkW5~Xa)JojG~P6{29uOjNF2vES~Yu>4{EQZxEQi1cu?>cNFFn!x~*NEV<zf
zXOx`cgc-lmX0X*OEH%wil{JhII1@%ESe~Kh2>%*Cr(eKf7%!yHacLvQr41V~I^aF5
zW)$XP!3kSUi8X$Djw~SPt2pf}n30)2m!I7q`#3apnUGR6yAW4Q0@Hf2><9Pkune7(
z34I;r>)6YzX<`4LKvR>(6+#8Vofq3Lqac3{3>fT^lTwfjQ%2~JA?0b;)Ld^1of{ZS
z<O;Ip{E)RNtwwBL(h%#IqqF8@<!Vm11o=emnPOl}&dGD;!X3GI3LxC&j}1cn*OZw4
z5k$Or0x<#?o<fwMLCTr;c|3rKwd6xh!{dV@ShEODIH5{2;gJLUpQB_t3yKS8q$x$T
zWlbTt%GEcxBFk=OgU9i*7HC01I^;4OsGXr9x7LBD5h1sv?1;hIs=;b$(2;O+v*21U
z4luaLAMPO%v$Epyy0j^RUcs{*`V1*n)Zpaw!u&w%<;~$dDv_;W1~VcGI&fuX&fx`q
zL~=o4F<Yx=RU)1W;A1palfeoL91U3|2^<MoL*Pd=Wf64Bs9N}`Ayeky>AcdyW!VDH
z7vVWj5sVme(LIb2u0e|Aw9E4Fq)B@>N-dQ&?hq3!`jk1AwEWmUX@Q~(u~M6Sz!%Mm
zPoK=3^qImHq0}cXjV*rirJ<mqfkD5VqM@jTGY_HFx1W&~H#Jg&VwwPtJYlGZ@g`UW
ziJ4-CClZLW7$(s8*^Q(*qyVljjiqT=K>Z-iM$R@!voBr|i{yNY<r2oz9Jr^>gnu&S
z>J+;K1?t^ujWsHP|5n$)O?U!bn4Vvha&BkU+Q-Kh|1ZRX4V$+{e8QemAle&F_(gND
z#zNf$W?+q2bGO(6XMARfGdCv#pIryqa35_ZjIEhwY_c=GP+d{wO+8GnFtjz)YrJKL
z=@;508b7_UR2ymAGF45_2uZb((~VN4HXQ5-YE8#$Mixy{;m$GA*EH(TRIC;lR)eFa
zCK=;Xjn4RQJEUyd+K#Sq<-Zj+a#WKw{Ya=8m(=Reb#;w*|4NOy)R%W6xDcG9A2nv)
zm}=wQAxv)yNvLs!bEMr!rwG%h!lm7aLL+I1>m?#VZO_st81A%2Qqk1?Is2DBG2^ov
zN%Ft#4B>j2-l6|iyAgLadHiqsn%>8MCCD6$g)3X2$LTMG+4;Jmy%AfSj%qKOjY$VD
z*CEqic{?WRrSa*l?!HYsyPbh=QU$K&y<xf(c7o6+U>E+lIvEd{Z1Y)$I?)cB0^MGQ
z=0~HT&x>X#4LT`QG?_6l4CY2-pk{|X&D9(aOfWSiuQ3V9toEo;?S-=nI;n5Xjpf+b
z)>to0zV$NUv#z%a$$f}{Q2`hjvaZOVxyRv)ocZ|bnSOA<B`{bs!Ta2l?0jqs!I{dx
zR$%99DyYwl5_pL!(0#1EjptoKQzoX*Y|u!Kw^}`8YbMFNiz=6wm6zef6~%C>;ZA_1
zAy!)I3l8v(h<@Y?9nH9$&YEko8wG1@;JyM_S(=U2SQvY*%)AY(n~GL|zV_41Sh=qn
zTLV4uZUD@+#9FLaP7(#$U)#8t1ED7{Wlc-cBrjC8t{NLNMSTM~muRE*jp&AED@-})
z#Y0bGDe#&IokV5M%*xBdmvcli+9_D$XBswS%{<_oQ!k!~@tH>X5zjARfWqZ(kppWV
z!)6$G)+5%+gPl-Q{~DukipJK=M|}mIQ?=0(n6PF-b6z8~=47@w5ZY$KG&D_SL(eeM
z4zAY;P3^*-Ml)#1wDg4$GzueUY#h86Ymz+o@=asrK~a6e=hi*P89`~@C-jTZns?vS
zNk!<~kX-~+BRTK~XNaLD%maSVXg<NO_39k6u|yi3VX8~&ykD}pewZs0e_2q|FT^}b
zGrwd)crchxO;xL(a&Q+|kI`LnWn0v&(P`9l=0<7a>w@+ic$}zxd%&vE0?*FJ3bk-@
z)@T;bWfIchG-azcrjQmF)=J#H1zKD<6J9K=mel5Sj%281HkM4Ijn3L91KtKK&cel9
z@q7{In?aZK9ae3lfgFVx(Bryb3({NYT1A*fbdEC`^)7rP(JLCZrVzuh4JyP68)rzf
z(_4i!E4@M3x?nFNg*;rMZm#)U!J3vfColZ>3c(kU!Yi@keEb!u1m988+~SL4{5&>l
zhMcEA#?I69b^VE<2ickWLe!re_BcCV(<SvMhaP0FXX_VD#L4V<NExzYp;=-F!?_%6
z^!y#eaGywOUVP)91BREN9?hS0@)R&r1}Ews%xluM_7!hT246BbkI&vT0<ji+0-qpW
zdrT<^ya1_rvK;0F)i9+%p89W-HTvdGlN8NmwId}jv~UmAH3}g^0Y6*+E`;Vxs4n9&
zz+ug>z6_qe>(xUZz-tV`y$!E1_!nP)Y0k4Y_JNmyrTUo{-1x8#&5KgspY_KAExZ3X
zCw?+hq_3C&uWRfoFFsv^N1<6W(o1r6Z(FK1`fFx@H`2tSNGX_*1)q|@XHjC!BE(TF
z=ahmm_95X9$BV<m(u)jyvi671PT&<%?4bA|cYX%Fog27rt6qj{?CLRtCZ!&RjEA(Q
zz_BoIN2|xeHZuJyQKF{c1HY`oLQRtovD1}u$g4T}>IpSC4+VSRb-hNNfew1Uma*bZ
zuh8$w<!g86N(J8%`|r+;WgZ%+h|k^6<v}o?|Gi3>_7kplLK<0!(RsC~aBob9C|UdB
zTT}3LEB#1-?p0<hhy;EcU^G~2ub=wHX_c>qk=DO|w+|i&!~5W{0t?+;{{eQW>+$fk
zRQ!s;kb=xC!!+pybGRoc#b143SATd_K0R~Xcv$z&gV6(rOI&xFkRxAShJQ8I@TI)8
zPA^W^+1c4=hx|LA;D?BT`-}cX1-WywAkpbCMrG%v7v7FjZJcX|zJH1h&C1`@0hcD=
z3ry<3l9&VUnX2D~1a`RBfp@jV9fA<#pNDW$_9{9K*z9FcJdM8`%FhZUAiw^=FP=gt
z<B+>^eF_yAr<ohLA`5Q7`<>@{AwT2^%Kuc%+A^Mih1@?){?D^+4VwtBNm1VaTLlJ-
z18eX+EASk3A)BT8d04oTKgWj<L8thqWb~@Es;f5npz-<DQlkZUzE#t=uq0p8$)IEZ
zKd22D4$njP98QKQa4c{2yOhv!3;St^X0{$I<8Xt1|6%aM8}Ma^Y!<U-@kSgx^Bf<w
zFvG?_wh0>I_&jd<Ecjg+7%#P7<b*X6Sqf7+)>!dSfTpzC31Uzv+)YaGsL|ApiYD9z
zHF$F4N3HN)z4$;(`w>o<jKuaWm|hIuL?)=;zVJ_$#P^=!!EE4@HE}s)PZxt8SuNAy
zM@Gc8WC9j$Xxqkb;jyB_xN**twF{Rl)wsY%hC|^hm@IzJ2>Yw}bXNUnc0!0V1`W;%
zmzfeLTz|zC70(J;f<STcLH~J4sGjI9-7EumAE720_TyiJB0J}I!)oTk`zb!wg5}7p
zboJMihULP`&7z>#1WOY9f??o}H9oyCOMY-0xZ}a^9kC`1PKk|$U+a)xYLCvzgCB+|
z&B)5igx~Lhv&4h{z%5y@xX$Mrv!Jy#KDhK6c>5<Y$FLRl3w)CXx2h>wFt>zh5d06N
z!afMVO!ZI8%$$%pL9-SROg9!sPq@2;uSDl$B^Bfq6vHQ4qqFj77H3nD7?+B*Z;2C{
zU)HenLi&CgKPZFOq_IiMU%Ch#k3&D7g^pOlexkyl5uE2&x51n>{@Gs8?)<*n<MyH(
z1co7)jmj@n&$H*A`xiPwgwz>w_@c2|sV(3SegrEGD?$7Zb1+m4`M}Abt-~jV+3C5t
z6JQlXen5=h%z=P9u!eU6e|AeW0)s`ikUWK#QGZg4NogkW{4Dj#j1z9Hi`WR!WzAAi
zVshU!_+OLjfCO{`@oOz=CgCgk(DOrXby=g(HBZg8i51@Y7DKq75MeUB7=li~wKO|D
ze<m%ICPS9QoHuO6hjvWpFZdH(BjDF#<R|0uw>U!1);vxNCNAgm{I^bUE^9FAz@!ZC
z<ltxj=xb~ERf%BBZRA8gWv1U0FrG5gVh&r)4!Wzk6<3=9+$CschAOY0dk1CoXRsr%
zHY;lT+_#M3md2P+n8mT+QT-YvJED8k7d*lXk==KkPcLV`aKeiJJkGTC5j9R<TnB1f
zbEa`k(mffQqMX|{_>&Xow8eTdo;~5|avr7eHhdop@2MP!|HbKA=kYS$4?d~ZjsR?(
zJ6N~6MQ(neNPV{{Fj-OCH=q3nro8GwTu2z2la~^{Yn<cOK);@<iUO~_l?oiN8XX9;
z{>M%VS@u(pQ3qjzKw#BHGX?}$eU}a#5EHuKLoE0KgUpf)dD*b`59N9L2x#z<R)^rg
zL0wq}Y9e6pZ*PG@uYq=$m)W5$g>~+{re>jDH8!7?4IfhrJug<Bx7c&xwr&Dmbl5}|
zyIgSM8hSRn4@<#IhZt-5>|c8y7AQk&qqkwyQSpb`0{@5JlF>9GqyphVCtlWul)!I4
zz|X<JC@0n?)rlC0i#wh0g>Qu!RInAn0&Z5u?W0F%pWy{_(NCRkox?CTRzBMqTaZ65
ztFQoEn*-mV7iNXtE#*wWMO6HmD&y&5XpYr7G?;tJ@@zykv~a_h&iUM?$i%`owsYL8
z;HgmM;$H)Sb4-iH=&YS^hRuof{BGQhmVD1glU*idk-)#<&pYQG{W;D*&naU_-_*<@
zgN?dxCj2OjdZ!P+5@axc4UseMRp+{&2z`JS^roNu@ITlmX+p;bnMTf!jn#H&*$=Tt
zqyHh(>hKlRTccrcYDjq))%U-?Ni!0%_74m{|5dim^F}2kV_^#^^v*-JTAa6O?mvQ6
z+vx3w?(EP@Pi(CC)s^v1T*t#7mI?e$i}puv0_nx}rxWnDu*jL7A%2eLTxY>VBxF_-
zkkr2y5;$-^6YVf#m41#K&<mZ=%JPAK&v{O87JJ_3$iAWd{RO<d8;yqslk+e+4BK|j
zkvr+OX5#&Cl?}=u%#=R7GMnWG>LgLg&6y6bJ7ipUU9X~oUi}c}1@>lUWTX`#vLc@$
z`LrxtD2B(m)8Utk#g9}fX<7N1XhA=;k9NI#S(E|)e+<g-(IW;ANsH|j+pDiKe8`YA
z%*l|{)P%t!(yYCD%fqRuK=%olT7$d6Jv0zO{`H8aN>lj5w)meC__rDCHG}^yfWOq$
z6t?ns6TvoBE>xnF=CB{}fB*eo3H)CP{9g(D4<+!N_**=RVusPDq<v0v1;&;a8~`BT
z`0^?{Ztpr5{?g5b${P-G7>+hkR?~3{P@lhgJN!kLX39Ho9R8GyQr!JP_)9uXmF<8r
zM8KH!F4-eqwQ@E5#hm8K9*4AVmy-j3Y3TyxTf)+xNa2sry8``dq8x(b7^*((EVlIL
zdK7=O`~x)AI^J}d<UKmxLVS&mw-fKx@lN7vb-bH+pN_90zD~z`iTCSxKk*GZ-lYEF
zb^5zPYJM%mN9lMw@g^N#L4351_YiN^@jl{Xbi6|4Y|-&%;^TCD74b>*cTrXU*L)`H
z&#vREc1b=}$H#ptd54a#BR-q{4xp+ZT`TRKI=+VZQXQYVPujb5yz@)R`wq#isvp30
zT7E^>NYO6MI{pdb<8=Hh#M^azO%JJ)t>d?ly-UY`Mf@5ae}ecb9p6CuH9FpWolMuK
z<L$&Z==i8^(%w|xS<@d&$v#HMR}yd2@ioM!>i7<%@6_=PWM8J^qkBq!Ds+4j@zpxM
zlz6X>uOPlo$JY?A9M<O7PkgkFx03%B9e*?NNjg6Idg;GI$J>Z6)$!THyLJ2;;wyE$
zhj@>UuOq%z$6I<yfBZT=mH4P5+H#mm`7-PHS;WWb_%)<s*YP#PXX|+72I-GW$6JVB
zqvNxQuhQ`q#MkI}7y0Sa@pWY1pyQ)0(jU`NZ8;<nAEV<}k-kmGzfOFrj^9GOQ^&hV
zzf8w}PWBZ#zMAZ-b-a&wua1wh%6!%7_`{^H{G!dTjqIa!{Dm=6$D-p)$v#QPR}$|q
z;7fJ9m+aj-UWt|IR_b^&@g5zYN_?%3cN6c|@m}Jij%mvws<%wntmE5IImGGsp2XXA
zd@AW=>-aL_T{?aU>8#Q59<r~}@hN0qqvJ<kCH?mq@C`b?f%HwkYRe(IkJO1V;B7iS
ziR@Dic&Cncl6{$uuOPlc$LCXit95)C@m?M8C7n7Q|0LNfer<j?6CbVP{iI{j@s_@_
ze3Ep$gLsFIcN1T#<7<g`>v*;ORO)zjJn<OtwL1Pw%9mfqoBK)sqmFCK;a9RZ>-ebd
z(mqbdr;?6c$9Ew6Y#m=l_AVXoA%2aH_Y+^G<74_ue`<7mJf-W?@so*f(DA9HV>+QN
z2N&@%I=+&4n~wJqpQ__canetxj!z=KOvjfIU!mixh_BZ1KH|MPJ|<qKTc_il#4EpP
z^IJ`Pw2p5e-eSNf>G+rdGF^v`cMxBy<12}G>-b`-uSy+XNA?~azmn{0b^LS0`*r;L
z#7F(EEeG>J>AzXW+lY_T@ukGub$kW!**d<K(sk+hKZswW<D+^=f2wqRd*W+!d@ZHx
z)A7oUQl~-3#}RKjsVxU5@i98Sig=rj?@s=t>i7n-cj|b{An8w;j?X5(LdUDuHOF@8
zC$0+M-)6?O1q>bozG8oPd*?Ud`D~`sUM@WdeJA7bZ*Hq0rY5Mv`6`w!=gXK*U%7}Z
zq}+^`iZX>YjK750S1^7Q^QV&W*D}6}@$jeF1LbC7{&U{X{O5c%)471@*D(G{=BJnO
zo7F;tZ6zyz&et*fYne_d^ON({27KJd!Fu8Lr3Sp8rTYu>vw`u|j8{Gi>cgKB4}>TK
z-ekZ>8}McWKE{Bz81Qihyv=}5GT`k7e5wKOFyONdc&7nxWA)3+Imv*J-VwfD%m#d|
z0q-;5>kN3m0pDQ2D?7vc8D+qm4ESgR-fY0f81NPYKF)x*8SqI4yt@8}Lk6!$+o$2n
zp}~MxJ_~OjWx$&Z_-F&(Y{17D@D>9;&VaWW@JR-|-GEOu;2j2hwgIoMv+C=`TpPZe
zYYljx0bggp`wjR8177(mygyL}yvcx%HsH+$e2f8aG2r72c$)#AWWblP@vs-W&bk?&
z8@TEy${NNGWPAnVPcq)auD_hGWcKt3L7Y{?>^Wb>>}~9N?Pd0yuV(h|Gk<ECJ?A~l
z{%&UPWA>b{VfHIoxz#ay&U=~tH%#Bo>^bi@;A@%AUZ&H)>^bja_IEIQ<=bF8=X@Qr
z-^}tA#q2rnXZ91AzKPj$zJb|yXZ0A(>^ZNn>+cZm594FG4&#S2e=Lk&%=kFQ-@<qY
z<1YhVo&Q%D@IC|H%;tey$6>%%81UIlKb4h3CF3VCzK-!T8Q;M8{;b@b?0U-ivQL8T
zg!67T&U3zUS9tp<maiLGy441}iOpxYy~lvJu<JOt_ZjdeHvV&aj{zUU{OrieGn?^u
zF}{-V7qR@-Fun`3ui6tVH_q359-g=F3(seN8J_pCbmu~Qj;6L=r{imguR0)g;NiQ1
z_G*Y><$n?LKZ%ty=VMrTa^AvpzEH(sYiBx~w=f;f*D#&k%pdi6FH)RWpX=&*FVlIP
z>G+vHocA(+IB#eD=t<B~%Q;oYTPTf6R?cXzhEis~PbFciJ`YDz&byfXI%Z$N`YGqj
zn0+hOJ}a5MhW7^B73bYdXCq6uis^8E4YR+G<+qyIYj~C}=PQ^_4Ab#29SzTPIA6(h
z;I)%Ls9`!9p6PJDis{_M%G1kqG(6Mcd^OX#mF26J>1cSS!+8(WxtaOnV>%k1>2SV=
z>9k}1)G-|m&vZENWje7;$Io;$Jk#NPEz{Y_bQ+kBhG#mQ_c5K3fm(+hcKy=uyMyhY
z^L0$;W9Cm3)6wuuhx2}>b34;9F&z!hbU5F@bUtG`(M(6fGab&m*>&H~%13?g2BkT_
zhS`6>?5kP2oUdT^-B|ndFni8NvFjt}E1AxVEWb5Ohx1j;zJ%F(nLX#DS-PCBW;!jI
zPA${nyqW26-otbfnU0U?a6X3VaK47=^k(H>$8<PvVLF`mGM#<QA3xLKd>qr^d@a+l
zFn=1D4(Dx5hx0zB^DXm7Vf~u(Nlb_Hbxda$(}`j_oVPO_&ik28Kb9{O)8Tw7)8Tvr
z(>cKWiDo*ScQ75!D{Ox9GV{mGbU2^QbT}WybRJ<kF-(W^PNu_o6Vs_>`LZw_&X+PB
z&fA#%M$l2`QQ11al+tkOcy%5d$L3e4qXsv#@2--twXyM#^L_(<4b$1f^eY(u1mmk1
z-<|Phrf*_=4CCKnekL(Kj@jE8e=*}z8NY|=I2iu~;~N<N3)4|peZ9?i6XQFx@!ZAu
zHej#TW0{Uu=fgHuUuds}YG&_ey!t*SQk?fN`}df=gWWfAzJ}Rf%Ive5J?Epo4$cQS
z?`1lXOvlM|IA6=`-&HdU+fruF`Dm6d=Y33PIZM~YbU1HjI-IX#I`j#O@T!dIa6X3V
zaNf^!mN0+ZOo#Ipro;ILrn8XgtYJEwk7GKVSJ-{@7*@^|Oo#I}ro;Itrt=`vsbo5w
zPhvWpH!+<~EMHYjhx2x(!}(~Y^C|PEn(1&pmFaNa%yhnI{&<)U=N(Lk^D#_k02>Et
zm=5Q&nGWYIOy>cXFE7*Kyp!p0K91=;!}3+jbU0tibU5#2IzyO0>U-ZP&G}kp-;0$)
zDO;c6ypP$x#`Imxp7Un*+??}uOy?BKZyD3!yr0=W#?o~&d(K-}x}0xdI-jz1*DxK<
z$1xqwD|>_E>Q<&x!E`upV>+CVVmfPCzABjx=aZNY=S@s!HPfkLI-IvN9nMEHod}k%
zYNo^aRHnmuGt+sR`Qu?aoOduC&c`sFW6Yl#ro;Jcro(v)(}`#Pc$p68olJ-GaZJa<
zbZVIn=S!Im=WR@<FVpcc9nQO$4(F4Y&UB_z$8<Pf#&kGuXFA_89Y53IyqoE8-obRP
z108i9m968|c~mN!N1?qM+{}KwO2W4Ci(tRw{2FE-*(7*E1>^U!_EX9D=8Ugm{A9*g
zGyW6Cdl>Iwd=2CKFy6~}6Z5l{@$;CykMV<p*;48l{|B@8Gd`2?4UGSs@d_(XAJdOw
z{A9+P82>EOiDvu~W?#kl9gMGL{C$k~F#Z*$U&DC#_%0B<j6WS<6{VK(-!tCF_~#j4
z$9Om6{fz&UrQ5*xgUnuG*XvZqM=|~aR_`Xp7cu>4#?NCqX2!qB_!!0)G2X)XznQ*+
zJ!k(Fc(tFF>3Aod<JR$N-YQuCMIAL%G5Z3QgstCzk7DBqx36aQJ(-S&@mH{PYZyO`
z@m|IcWPB~-4>SD+#`k3Q3Y&NR&FoE#AI9uWEMGeqAI<nj7$3v<LyWIv{2<2HFuomY
zS6;^VVt&>#-puTMjE`e{9phWDbQ>7|1LLFE^Zh?rx-J9W#P~~?PBi1cWjbcYk6?TZ
z<2NuJ3*)b2`HExwCZ=O!{5?!3iSY@{K9%t=GkXW)doVtm@v|6T%J_$weih^AGW%-A
zk7K-r^~YIECyDV*8E<ENB;y^7|AX<_jQ@_M>tuWjW?#zq{fu`p{uRcTG5%u4yBXh#
z@oN}=BlD-4@uwN@VSFLesbTyTjQ29WE7PfE{LjqZ$M|a*U&r|QOsDQpa6Z$4*+=^%
ze<5&hG2YC0E8}Are-q;^jCU|Tj`1UyKQ_j9WA;gm=l#^q_#~#2%J?4{?_hi(<FgrG
z!FVU*<Cs6Cj9<X)U5w}bx{UGHvvI)9cpuYQ!}us>U%~it=1(Q#KV*Cr<DX{w)r|j%
z@gBy%&iER}*D`%C<G*C~wT$n_cpu|kOs9_V+nK$e@vIn>2FCOE!IbZV?RGZPk7E3K
zrf*_=e`X)e_??V5Gd_>$#4w(ZZx+VG4>|-w9OEBgIyS~nWqcCjlL9L69th*{9TYXB
zGXAVe!q&m~u8hxS`~b!~86U&=QpPu772{(331(l$cst|WjK{G?4Qm+x1}ldO#`j?9
zRx*B(DgxUo#y`&ZYQ`rr-otnc)30HC7sh)T{~pt+W&Co+`xqa^_&UZfWW1m8zp-*?
zVElN-D+hz^zZ27mV*D}2n;4(M_-MwbF?}=R-(h?V<L_m>h4HsD{W!+&XZAM6H)r-q
zjQ@o3cE(@E>Lr!&In3U{_*E?3Y{okn?__*GreDhVBTV1L_?FDRjPaSw-p%+&8NY_{
zA2EL_7$3p(D;eK|=~OYkEz_xHd<VvR7=IDdsbTzAjQ2ABYo=4n__rAEWBkWVr;hQL
zvV8d&znIxKFuo<@l^=rbzYo)iVtg*MH!(hs@zIQL#r!ujeiXBhVf=2!TNr;m(}`m|
ze^1QDc>cUBiSgGneLLgp7@x{`K3{b(-p2gNW_&5raWeiOvoB@*!;E(^{!OM+#`tt*
z?`Hhx%zh2ypJjXn<2$hWtz`TSOuvfpTbWKZ<A*YP595Di_BD*>_d#C9KhJb(86VC3
z^fCT&#@8`Ef$@IEH)Hw@jQ27>l^=uc|4zn7G5#LLn;3sT(~oBSbIjh%_$L`3!*~<p
zEsVc~>BljC1+%v?{wKyKG2YE|?2IpC_Nk12joCXGKa%m;jDL#pPR7q>d@18QGv3Ac
zWX6{<{x8P68UF&~*D(GA##b<Y7t3!Y<5QS@72_u{`)bBpS$p#^{wl`TF#a;8?`3=x
z<7*jz4b$;4zL@z_$M_7!`x)P!=`=7tpV=!v1>64@%%3R6zrgHGjBn2LqZ$7y(=jvt
zKTIcv@x7V7h4D+7eH`PrG2X`bOBkQT_yJ7c&iLP$eJbPIvV1uh-;UX5GkzlDos3tX
znc(44#`EVLF2*-uewH!*L&m!q|0_#(4dee|d<Ekh7+=YFKhv*bd<NsI8Q+_gkB9L`
znSKr9k1!oC<5x2KTE^Ei-pBa<Os9_VBbdFP@x2(|!1$XPulyWr|Ccg8it(dax+ccI
z&-iG@&tbfo@t-mM7{)JR_7=uJ$oM$MU(R%FjK7=NCo#S?<L!*!$8=H|U(D<sjK7-I
zdp6^%n2wY21x%-u@q?I-i}Be^r;PE-81H8Mf0+In#xG@j1>>WcP9@_{F}{lN7c)Pr
z882ImJjuiO_DrXS@eeV7yo`T|@wJT4VZ4v=S&Xk^{20dj8UGUVr-AY7n7u;lnYfm4
zC9{uW{0e4oV*FvoM>GC?rekJ&3s#;nj8A6v7RGO3d>rEkG94S^=QH~x#`EXJcE)#O
z{--kjAI3Wv&!1CdGyW=;u9NXynV+SMKg0B0jK7-clrjEYX76VFHOzhu<G*G06^u7C
z`%1=FGW#mVKg8^-8LvKL#KRuO+nG)c<2$l)@G^cPvrk%bG%9IDvu|rVDM`yb#m(xl
zl9QHfi=vy5vwatT)k!(qWhwmI`WhSTAr>P>z3(i9F7pthV!c=3Ld2MIy+`0#h?^p=
z5_me|NW>Ka--@^yVz<B(5aSYIy-VPch_Rd0I|Uw$I0~^t-~ou6Ben}1ix?lR)Y}B^
zff$zv>Ma6yM2xrM^=5(FBgRK4^(KMaBF0<MdPU$Ch+8A}p8=vv6U2DyQ127?6ck-{
z8^m6LPatlK*dy>^#5hH&uM+qmVw}>{R|xzyV!Q;_y9M5Z_%g&Ufp;LrOHaL1;4O%8
zNUe7Wycsb*imA5?{5oPB66<XOzl0cvn0kxAPb0=hDD`H6*CNKQRBsaaLB!Z4>J@>P
zBgQ6Q@Bat$zZkI@u}|Q6h_6NL6}S*_N5me1XCdx{xJuyZh;b;duMqfF#9a`(1)hKy
zmwfA80*^%84Y5<;!H8oJI|LqpxI1FIz_EyNX{p{Oa1X@SA+`wI5phq%W`Wxy#-)mS
zlfZ2e;}oV|5x5278xZ?Xi~2`wLF^Ow)R%y*h`j=zKpczMBk*Cwy%ARld=POT#1#U6
zjkqsjx4?T4_e1Owcn4x!@~?LayajO_Vu!$+5yvC83;a4_TpF*p3H%b`fru>vKaKcC
z#AbolA|8a;B=CcXZ$hjHyc{u3-Rk{+i~2`wL+lfH9^wSVUV#e{4@T?}coyOzh^qvi
zj(8~I3W0A$oQT*h@C3xe5W55(iFi0-r@(^|Cn0tSJOD9HMeFSX$08ny*d}lf#JB`j
zZxOg7;?aoB0=Gvz2C+%twur|fRs?Q=cpPH?DN+B3?TCE>pV|j_JYuiFClF6S>=F1d
zV!YJWR|$L&aSGxJfxkwKw=(r^f%hQBTi|+^z&j96M(h-L3*uD74uLl#z6G&e;MWn~
zir6ObONehnY!Ucr#8VKP1zwAIDq@qs4<eq1SP^(R;xxqmzeN2bb|Cf%JP&a?Vz0o3
zh^Hg=2s{gM2I4A#rz6fpTp{qSh_ev81)hL-24a`MBN5L;>=bw~V!Qy=I|LqpI0vy^
z;8?`75ZeUqf%taB7J)k=&P8k%xIN-L#3q5;BF;yw2;2g30b+lHsDH#x#6E#feF1nj
zVz0m_5Eml$2z(fE5#lO=4<ar`Tp{q+h)WQ=1>S>r4q}(UI}qQ2*eUQ9#HENG0&hk<
z7qMO7*AdS{Y!mn;#5hH(w+Q?+;suD!0<T4UCt{Pp4<cTOSP^(R;=2(0{}lC)*oD|9
z@I1te5PJnKM7$WWN8nkAmmsbZcsk;xh${rX74b5}Zh<Etz8kSi;E{;$LF^QGFyb=A
z4uJ<CE=Oz^I2Q48#5RF@AYOskB5+5<_aZh6+#d0Lh)n{wMSMSEMc@{QA3*H?L)1TF
zH)5Z_r}hF~iP$Uf3B(T~_6U3!@k5BK1U`s(72*nkzefBpVz<D15U)n;5_kvVM-V#&
z-hy}yVu!$+5kHF9F7WG!*CMtF{1W2F5L*O(8u8<Z%>u7Q`~+f?zz-sR60suia>P#|
z_Ma5>kGKM{PvCio*CF-_T!{E-#2$fXA$|sNmB7;xKa03R;9C(thuAIf1jNrHb_qNZ
z@e7EZ0uM%9iP$0V0K_jMwhJ7K_$9<PfqNickJut`N5n59HVfPy@dm^uf!iW}1+gM<
z3&gJ?_Wv&GA8{38pTMU+2mBghufQh|zmC`=@L|MnAg&VlAmTR>R|xzy;*E&i0`Eb*
z39(Dy9f;pT>=bwl;%dYWfj1+58?jyB*AZ_<Y!mn;#P1-s2>dkS{~$ICycY4hh)n`N
zi1<CkionYezmM4eo2Y-p9>hL@=OO+8u~*<i#9I)11fGR>E8;4Frz75mxI*At5r2r-
zE${@y+Y!429*OuP#7=<+Bd$U05O@IMj}hAijzzo!u}$C}h<75k2;33zCy31gw@3Ub
zVw1pa5r2kQ5x527U5NcBMExW7BK8S<Y7gMuh`j=zK)eUBN8rPVKSx|8@Il0T5myNO
zHR3N2y9M5ZcpqYyz&j9siP$Od7R0rP9RhDg{1sxmz^^0z8nI2_mk{qqY!Ucr#NQw`
z3%nNbw}?#wKZy7{#EQVn5g$P8KQ8JYu@A9N;CYC@N9+~25b;689)V{e{sD27z|#@`
zh`2)FTM_?+*e&n`#6Kf;2|N<<A;eCB2P3XS>=1YW;(Ek(fnyOLMr;$f2jU}$EdqB$
zd=#-+;P!}rL2MGZE#hN{6@gnI{uQy`FX|t$AF)s1Q@a5lN9+~&1mY8jJpvy_{2Ssb
zfe#}79dU)gUn4$=*e&oL#D5@m3A_XGpNO4+;a>gYkKtZ@g_2a>?1clJl&v#*wY~-~
zr;4~ghvP2>jz19gczNLX9bu2>1dgYLJw76Ee0bR7-2=yO2z$JF;P~ZXkJs0z`8)ev
zxcq$_IDRPX@fQQfcZE4F*E^P>&3%L31B1;GP;?2^`0v~JPtpoYQhDEPaHKBnS1j;B
zl^>z;rQ^N0>AM_`-~vf~0t^psDDU_{dXI!jZ$+~!0>|$Pd;Bky7xg(a?D4Mx$H#>|
z{&wJa|1ihzYv%kG{T?(OHWQP&Hzbw+l(eLw)g4WyZUyIC;>ob^b7JaQAFl5t-PbH$
z+Q%~c4;q>#m2XM<;PA~!5#FTT4JG2_OAl?wllQ>MN#)zm9>nuqgPOy6N=dh*C4+Xs
zVMwz4r{b1Sifwm6i4KBGN?kT|qp91Q`M&9-MEtgO28KWzhSQQZz-h%@!MSll-&5Rv
zBKRK*G-#dmPOYB|0<EucL1iiRqhLRHx;tTV!lZ<W2`LHXzfVkt#jVn1f3#kTSC18U
zL?xB~v*b)f>-+KInYir7)@Ar0q1?Z_At8QS>j&^H!gBwk34fkmwx{*7;wFg3WE{2b
zAKKkF;)9=>Ec3K}0Eg8Xt*;rXD5LMo?Q9x-UvX#K=<;9dCLKF_c120l=<+}7M&WK^
z6j<M%4jPHpAFcb7RwPCbUJ+>qa_JICW=TUt>!rAgyQJZ4>t&B&!r$S!zrqbC+Ta7+
zx>y)}K8h62Y!lwbS2F5xeI3M>Y%49Vy=`j3l!U1X(-P9Q3V)JTOf;28u1#EawmK4=
zENw9~{?4e@58!nT92*+HZ{eL_5)CHh{)8plBa)VEjlhg8`7m;5yubDRJ)syA%MT@#
zf1ObNT|&eGsnTp{{F%EJz}a^_HMBgXbIZCXU<ew0U$gCJn<xR_rb8OmkF9&&gX}^L
zeN>kOMnhMCd3_#Mj_|K8L!NmroGA<gr_ZL-@yu3W1n$<S2F`<`GeObC)ZYPA>uaE+
zh@y<C?<#TKMN)U~8Rk(NEGX{LCg{lJTkAi71ubefSm3|bErdH<DDY)?xrG9WpBmNr
zew-?fSh6j0+5Xn$L*RtE7vbc9xA=}I6hIavCsMeQ@|+@4z)Bp64uSSy*w&8|DMEXZ
z72HasSG=ljBG-%s%_A>SISqs(kU%8QPdha~O)0gXp^-s;j>7^~(tUTvj4nS`_XG4S
z*td@^|D*0p*thPl&xazaI|8Nw?_)ryeh|<}<y+CmCn2$<@&k2$z?~lW4s6{DppN|D
zR<9{)f0|f+GC66{QM~WM9)+)bt{4tI>ThT~OUD7iHhupv*nz`k+riQJ?KmE^eqb}~
zkIp!TT?msrazqqQQu)x%QFY$|u<pTq(~2QcM>@@b@wB)zEJ!ctY<e5p@kf!XaZ}#%
zk)$PCBI3VT*dyM%5c<<7Q_yfl%a!FVR|6L<_Q>+CeF^2;6S^Nu1b^<w=_K^TBT1k?
z#5B0PT}9%uJ)6~zk_;VX3N&7DLG%TD4+iDHZXJp&`6vP!L<ERocS(#tvv6)g`QgO!
zpP|DfMjVtn%@g9cw7wT#xlD-PzF-Y@mkxFLzlh3w<TPuX)4>-_<Gcclu)B1qpDEh3
z3k<NIFWQEst?BAB++#9+9`V7CO~B8)uC2$rNNUeXE0UwJUk@&C*%4bW7%VIuTK)$X
zWmNa=<^G5|D2b#sD<bb{U3M1EgA<?(p_~VoM|Q-Vm!IgqCE_qRpOmp@MaxGPY^bX`
zD!l*dZ`sxZtq6Op=)=%uF-=UbE*Xqv0p3-Xv;=gO;ub=`?oqf-3OACLorMPs%K@$^
zio&tejH4*D*gxwot(PP;kQTT*L=2!+(424p{RL_*Fo5FJS`47^qSe6wij%$aW7xP}
zfjd?lKs~_J9f!sh@ELzsq#kGCR9<zTV>`UB+38b_Rn0;$s{bAOFm%qPa5AVqCePc(
z&-?$ldl&d9s<VGQ3maH*VHOP<D{5L-8x<(gSXn`vg$>N&2B_Q>FLfzWwQ4Z|l#l?M
zfaACrFI9W7YOAfUwy$`{YBd+aa#I5+a<Kx6GRulwR0Krx|9;PznVsDvfUWKC{rvrW
zBs()_&Ux<7bDr~@GnNUnh++&yBjAvH4GJptT4In_n0x@YgdH0VyOwu$@=bY7_`fth
z#f;OoN6oIl^-g=VQ_%>DaS}SW@!}db(0J#7X^(mXMq1J(J7^(DlfM&S^!+UUCjTNv
z3t9B4663)hHKV!R=WO(8+oPxO1Ot>~0K35mG{_L)abmP&7`?0US+Ga@jL|TE_zmuU
zY7E0~%N||s9O)Kv<s!=-eQERnjI>8nV88%7?a^0g@3co_;MTRT)cEy<Mh;u+xxadi
zfx#{{R_{}_&c<&)mIE7i;|JYjld3hd(`O8JzRNfUorkq0{P_nWr+t*#V%s}}4EeSD
zW{w&-!!!F5gf`W&gYea~AF+evn^JQJrQp{PQc&f}{SK<!j|YM(mX$6+BY5k4c<3A>
zD&Oeid}FTl2CRnQS`X_DTi(?8u)?C{+Fk@$#?3Izln$DOqI%JD9iETUR?Fw3#PjlP
z#+Sg<W>y@nQ$?2iiuQYDpo=a~k*_>sEOEYa4_*OwbG|f%>O>Q~n@B+-ln%HI-T5LR
zOEo*=xv`_Q&rF(nKvRTZmBq$*yo3G=(7z2PGYlCr)yW*}@+(L`H9ljMw$b<pf}+~`
zim)25AXzq&i4)=B5W%iBa-5)$Wf<eteI^bknK(SPUE(0REy*!f;@SqHiSRFX!XJdZ
z2@HIY3JZe^jbGy}@*ln1FLO?xfkBBJ$|r=-veWSyg+c9Ypt2WA0aQi+l~EyWtHdP^
zUjST|8^unrID5xvyT8c7Wiv%RwnPpf?Tqmi?%5zQO_0eClNSRh-{-0NuzZ#Faz5Ir
z(S2$>Z;{@KL1|4T?!d}c{3Ou{>e*_F+mt%vcjzCmOa?6WFa{cDU_Mj=mfuuoTrb-f
zJKJ+ME!ta5Il*pURG(VPxFI2mvMec2%bN+mg5V>tbhf8<%}jTor8;1^<uGr2?f~l>
zYZyztp8lwf{8GQP)f8%>L&;YmOxgUWy_x$7Q$CYb-F-$OeyCcFe;K>42Oy+i3>P8&
z+5!4aSF7-BJ~dulXtYv<VFP(5-`Inzkba3rjZdQI%iRN5Uw#Ddi5ci?`~@?m-Qb5W
z_WO(KjV%xc8m<9=Nw`u>LigjPkoK{W2g}8K`M9?O_h4mRxCMggcM8ZRtLJ$Is1;sB
z5(Hj|p`oKyU7DUn^-Xn7e5G}jljQuDVOWJ#?P|QeO`>94Pa49;8`nr;LZvrfenoQ3
zhw?dWt)W^?e!?F>-~v8`rrt$FwB}Tb|NIj+w<L$g+}MuTf!!o~d?2Xrm=8MaM=^|t
z5dj>vo61jM*sHy7JhTOGBI?D@d-#)ld>=yrqP@@{$ZHB?gV@jSbqL0FpoWk?%TnJ@
z0P!x20;%B`pL0nO`)XfB;?Y8gpU3dfO6$1?UOGJ|qHVYQ&JQqtBz`>{-Y(+={MJ4t
z>hH;(J#xNgxuP?HCkax;_>g|C**G4Ep*Q1L{7r2lf%cF-m_C1@#-|rbr&lcjpna#=
zpllgCc!7LGQ9Yp-@Zue|w*kIZ2urS-3GlBrrvd&seybAVgVG0OL0rC7LcEGGp@jHn
zq%0twh`%PpQ|bt>fR!Q?__F+D<F^7=L5y7_h8*QSa}<^Ft;3(t-U4*c*{6?(dMgXL
zG=kZD(N1%oJH)5EpP<Sy=ZzpMJ{FT%#7ZsIAJ*1aU1zr&?TZb1626;rcB@Te<#x0g
zTC_9mUl9hQT_>f6ai!I+rJC(dE!rvO3m7kEbcQvY{{48V7;NAWPbIP_OME0cB8;>?
zg|rVOf=&Mvmi$fQI#ABGl=~Q3oeh6xxBH!Fr_`JaD{EZrY`Y$9A-M;4WqD!N-EeF%
zvfa;U(N9Go!_MyL%k($)^Yi5M-hAGRW353qHeB2?k-Eot&3W$=+)K_vQ>Q)@?MrTe
zz(IFqJp3A-w<ee3mh?Zb;ia79i}rPPe&<Nns<xjQAv+%w9IKMg_-~7%CK4PiI3|qb
zj;g)G{EkPza)26bg6HBALXH|(rYb`Z8nwiBQy76?qLlY@s{CO+<j*fHS|j1yJ6Q3#
zTlNl7dWPP?bidIR3aqW(5!7q^OpU_@|BBu(fF!^|t_6kl`}|BB71f(`-5XSVq2`>u
z$^>^Ke)q(07y?(IsrsYTh2U?PQ>(p-ulpvj&w4wkZ79<WZNn6$utC+k3R<b1XBzn3
zI%-tyrmCS~{ds>q#=)Dtp23@Gc!yxTcX9ysB=<TK>?z8a#wkj_(wynJdzGH0HX3Wo
zYIl|+8MVte6<(1jbOBcgQ9&5qLE=#caui~iQp|`jAHc7?2K?hq>{TYMK(EWD_dus*
zNOu+TJ{|^@NqO~n(lY%RJQ>R;jV8_wMfF8%oUniKX(x7h#@G0pDg-^)br%3EyC%G4
zAXp=`sooP<TQwEWOEGgE;~!A<c9|aWtEHIC2V)0X%dfz{u8P+qac>tA{TRZAp*J|;
z!~K)^(2H><d0l&zi7TzKX55z^rusK^mZfC&jB%ya?Mdho?~GrSjk@R>J)_e}-mcD`
zmH7iH`$hQ2A+O_A(ii`2QDi2P!U~hNmXV8o<A9KU{{Xn1Nc4*^G9>eR=jP;$h!0}^
zKgX1BnQ3d<2-fWs5=<|J-~tIXO7R$fei7JU>L2KqDS*w@pQNQ&scFEgl;rB#oyx-3
z-I<b1z5~DZ<hZ%|qp<#}KcAp#2g9B36Ji<CRtxBYS`vbcvAZAoq!UsJpo=f}0DcOQ
zZXGp2rvJDqZ|aM!4=d41^pREy_N67IHt1a`aA~Cw9UT=U)6$UK<bb4hr=;9ZI9@#f
zBGlv1fT~=Q#($DcMo3Nrq@eS3H>9+)knrZ}cf--fHkQa?bp%Sn^mN;R>7_|J3B8uE
za4pmJYe`7iatql~Z8b^vVyaUPEP!Z^%v9CSsHX*>Jzp%uUxdX!H1)J+XCdj-Q!mwj
znSk?H!if}A)tS~)pPPDnJem=k#u?4j*VJ;89~1{)8>_I!vI?CY@|8yrz3-wUsl44m
zmSf2!+{QDSg%*s^GTnm*$8!{!tE+CxvI^Zd?~qnuJO0utn0AMDVYTRi6hGe9*LSLZ
zg&{gitFSJ^DkwE419)K>{$Z(bPj-RfH%=#n0ua(d7=}C=hCCXEwbo!W?oAI={eXop
zsIM#>Pr@{O+_`8?QN6>ebwAC7|A&MhE{gEekNmi;$e?deIJVTotl+|~gf&p(r{Y(=
zLEftPrF-YXz9}`op^C!9JwH6|{jp@3&qbC2m-W-Vz@;;OwcyD~e3jyW-|n8Iy!%9;
z)MO6dIK4Jm32Q{TwunNz29(FYE$Ev?81Ro~Esti+ri(Iv7P3sqY4`{B(S+$_j0pM`
z0;cgx&{dNS*Lps&#Fg|WNRt4aG7*i1CN(MZlk9Q}{;^$CY%?!f>8Dza>9~MphGCO&
za(Z23xM>^1T5Iw{WX~<lI;>H;*07DzWc<uDO6Q@fF>SAtmmuVE%!g>DjhEhKWSqNr
zyJOD4%}ip>a4guRBExnK8WHBq`IDV57xBs5PdGK!s|M)v$$M2R_us5)(gh>wU93hE
zTBUyi6W}w^XWDhBfrPT)>aKVt-ur8G!I`14=xv72n>qJ<5Vwp+tm}|}lyR@P2>B5W
zv11m1;6DUSffi-K2-%Yik<inW^qZ<N(R{N+MM!Y78h>8QAN8420oo*eaUHtBudDUN
zQ-K10O;+PGCm5&GKOq@)IlH!A<TO)_FP?)xMklkut}c!S>t%xp=*id$Ls?r7WLS)&
z)b3_SfPIqs3a;@VV2=+nqq_BKq8^gnsw%_w8&9ld0x!YFS4ZOhtR@j<*cMJp=80Cw
zW`_uIyB%F6nluA?IIKzjV!Y<654xHF(-Qz7q)qZqH7-OWKAno3VMyB;(thusgNH%o
z>9uOK5o!LZ9rW(Pi}2P{k;RnLas~z=UA_(@-<x`>CfrT9&A$!eFN4U&Y9#9sw*7<t
zI@%OLFUaNp-v0<2Hf-r!rhORE9}_}AVIYUpsDwFD<G-E#;6F89*_?yiIaoF|qP-jb
za-(`y0`WC_;A@M}rVh(36KeNWMN|PP+&u3R+z2EI5#uN;-ld&eL#qZI@9ACwtslG}
zrpXOIWd3Rpu5xMh+jzH_T>$sm<yFP(s|4fVN2Oo2`cr(JL~reOu{FZ+BVY`s(;tft
z*qzEAZjOL0NHO1lBnnW=k3gw>39Kd&XH01LC!*cwpCkTG^3OCTyhA~YfQK_9@TYvB
z)V>7ThA_&cQoz{9%9;SoNem2f^t`#km?emmYlm1BA-Zd;KMDb?t4lFI<cwm13Rh3D
zM^{(J%X39o0l%Q<EfDW74aMDOyY^yDr7T(xXVKH$B<?o}V!?&7%QZl2a&2qX#h89j
z@KnqKs42W1kFZZW-qZ8u^L=x=;aU-Z8my7MY{T1+;H{Vx0T&??qTzx7xSHXAC>cua
zF*L*4dqJ(q9FAcJY><?%qMeZJrcz4xgAnYdvgn$Sw#k$@N^qt;g&RJAFxfICMxb8=
z=o5g^Wwu=Li7i*p3&13Q;nE%?T&K;!ztW!`2NZjX@THUZRSy~Kq`LMJPbp=I>UEST
zOUm|Doo@<>>=~mMvv;+QZ+6(n2Wc^L%GmQ#v(U-6m$gsUv?FVMVD2btkhFf8l2(@?
zXCS&IY9zqh)my=tt6?feQzA?$n}D1n%&|!?Qy$z)gIbGgMF^}zCqb71X>;x1Jkh;`
z0z$tmMJ^$z@pko6kY_T4xP$-+VS8T4p6GqDL*9fQSn{@4f+&Peg7`jiFKR60pXv$x
zQ>o2`Y^gADkARhsF^V9WD_8#N=rIBND0w;8yc{4er<s?kyo{QcCGv6%F70q(n7G)-
z*x^g#LUCbEc>9WrZG5ubD8PjZJ6iJ)L4ey+a3e{TNc(?t&Si{w18rq~!S=5u&U-LL
z$p^4ZZqnU)uXkn}HTgHRF{Z!e$gSMyf<cPsUborI<D4g;Nf_TpBh&jq5$Obxs5S7%
z5d+m3=Qz8%9$lrLv#1IJBf%jSRnK3SLE1CTOOv!GnwKVNJ?5oJ+5<3;HfcW*7ba;p
ziwl#qYs7^~+GbpUwB^kH8jEpZ#NKqE%=IOpJmY;6<pCzj{m@AJtH3qsfjc+KoNux0
zCAmgOkQ5ssK~k73L?awHEkhTXmnNe9%u5r|)67c~Q6DaCL_3QM6VZJLf$YdMDK1Pz
z-(zrOA^H|BjNgLecHXR&ARh-ztocLotL4t{68Xd2mmjs$=*hkyeMY{W$xdSdr~pDD
z|Gg3~^S^ZdYkXLHH=Rg83Y=N5!jL`#nS`eqRKDIH#El?=geQz(;Q{}6j4#MyttnAb
zh==eexsw1yNV^_O3%7V*Cp+-N7tv;6{XorEbNd4iRU1;EYR4Kc<K3{<m?&Y#e0TRF
zr{{#@(+X%UeKf22$X%=YDAZ}Fs3Ex22y5QsaGkH}B?YD0G!Opw;(zh1OuGj6;T*$S
z=(qy(b`5${<D>G!+Hq>Uj_dMj?lKG%)&}D-vRK-sg{pqM+W9g=)x3Vb3n|;!I%;NF
z?Y8o+iBfTVL!#CP-0?E9NJ|Qx8k0#sLEpqG3QcZ*jx1M+FGtia=5ONp3>a-Z_jUAq
zm{6G9-4C~YA31Jzfx3okR*>ZD*>eW}g5$#4HzF-h&LM;`12#mC`HbS`0EdYFSK*u^
zmc0Z!3-krF_(<L;^I47^+uvEKc>zBl!m73+!qxpZl$v7&=a4t!<f6Yp{}a}`z*8c<
z7t#9o%e1Xw4V2tKPdoqylZ7+=TSwi@;Qi0&&UhKGV9LgHAl!t)U>=OXid7l2JX=&P
z3G(Abv{^>yp)l+MOWb~=iYBbdApK+y+IqI=3ZAGAo*<qwq96}U!l8|rQ`PwAU|=#Y
z__Dngb3QKl9_B&mSCfaJPK=X{)A4@Bixp^(4xd7YVqSI*u2Zie7X^%oxWrl@T7hgU
zk#Yvd+D3sRumg_3l$3!ro`YU8p-=t>+G*#z9rHV(i8EDH)kLpCtXv}!t5n4PRK!B^
zauIrqME@e^ej$GeTh(&}w7QxX6ntJxRcij&2|sJsgtc`_&Hea|3dOLt0`*h)t*sCL
z11<627EmA28vH!@JxauAVgeKi7|uCT%~DDurh^)$P7RzoA*@@H*<2ZRz?j5#VD=~)
z)d3(1-T@2tU_tHsFmw!?Ib7`$iW!=y5mXys67;drNjz3+F9m#&KuTHA9|{?X_xUHm
zWb)a?u2BipUtl;vsqrA><^!WyJ}49qMkzJp@up+$Yr5e;6Y2%xgG7JE^LPizP{_Vg
zlaJ=%=#s;w<3uKI4#&|D_Eh>yP`H5#7Nj%W9}zys{8)|_4!n(b_8SrL4upjjA`qHU
zG<x707>eW_bqLkLlW1%AkyJ2&oC9dPI3I!Sg-&dQ#BzD+GEj^bON1i7h78g5bhmv8
z{UsqQ%qj0~yPg}(oBeajp@(*)G@iF{IYBym)x0#lT)@0Eec;)+G`;}sA?=husb(wi
zE0vG+&`sG-9>WvpQS@C?kE%%?)R<=KVL4tCdicBS_nE%J`=?v)gE`Pc%O9$>%az)T
zz6MCwWWV@(`Qnas=D^~`S~I>f&dYB87c@_f7Ooca3(|e6m_P_$garfgW&#wM(hNH{
znI?*^3JtGTwcdVS`HVXuwcv`@Ecx;^JVPd5QuGzsd|7AS%;L-I!mrqTS!7<Ce0kKo
zH2HFm=mWL8M%uhVHs5MB);Ds<w=$Hab{*1G)=el1Cb*+O08HQZ@Bxr2EalvuVd-|<
zvFMF5D=(;R$M0yAOqg##$05e0Ma!iaqsszzNx7CH&64IUxfUYb73*$v7Z;-EBM_c|
zdf01w#8*H9tvRVP#1kZTpmiL8O!~pzO#e-L```4Re?<g#RQ_|O?LTkDEB~VZym*=L
zpBLCYbnHJ*Zxa6V)wNmvlRHm@|7+iWz6#kAdix=MWcbfRcoBMgJvuzB{{*7{p8uQ(
z>9PH1@)BO9<J)8k2G@E03i5o;>vJ-%ZU4)t3spHP%$p39SbZacU%Ft(am1f517s0F
z73Z1})H(RUb+s=NBD|t?R2K3CG2wgHC^ef!zVa2WzNq?h%kYO;hI^TIS9QGwe~?q?
z?qAahDT@-3iC>DOxX9!%7sj+jPq$ncZLZ$JeFimGpv%}RB4AZ&J_P__9ktN+(vM*!
zEfM1r{)b@};}5FAB6_SIH@Q@VK6CG1ZDz5N$15$`hoz#_5E;K3$HKXTjm0gFT=g~R
zMDP@7K@k(iiy)$Gk2MJ_b$_8O{F4|@muQK>M78j}L{QaVA#7r8V-H;BZXiy2K+1Wu
zr+Y6TC1n5`8h6FLN$x%J3BwhvWevciUMGS`SOdu24&b={Fl}xq@En67Wnl=KuIjN8
zTntXQ7vT?<uO(=A{ahIN8r%{jmz=nl^dKjmBgnRWGbzqhc)$hP``k<AqC75_iX>Ze
z^+u4>#EVcPoo3Ik%oohnKX*N^(bXi$Mv?ssG*@qchgNDPB7TX@@}Q>V8}t&<uJ<K|
zn6_?nBF4G_@HxjwzACNd9yIz=+~zp~GX6mKu`Or>YUCaORg?uEQuHv1j+!?1dc38^
z5i3UGxAc$1U+K?lEn(W_LPH0?)wm2JBic83L`z#>e1`WC6v6x!qkoYm=7RwU)tDt2
z^ExaIYK&Wr@8OrKca9+XoCE2{*mMi<lVcFTz;}9DM_o<Zb_}rPo86$GyuMakw;H#>
z5b^qCTodpWCc5Y0x~RSuwc40xR3D$5M>MhCUXS!pqbJZ@RX$M0yR%bPHVe^RErE><
zn%}A7oq;im#_TsBY!r*wpn$8QUhVv<%uXp>E2}+)gX(%c`&Pzp0vUVvoc=p&kOUXn
za=@=&+wxIV4-G`q`syCZIe5sX*zQv`BY-%!YL5(k<K8qF?+1nqsKR7vbV+PbH&?}5
zY%u`~y**+VwpL)4`$|1}!MJWL&|F3Ox%R{W=wF}_^2zv}V6s@5ePjCF94_Gk`&%(+
zwa)zwV=xgxpM_=(?q=g8LC~PqtkiBYg<%|c1RB?LB^{8<bSbr+F;?x)Iwm@WQ8I0j
zuuW7AD}O5GSExc-0g^?_v0x@-XtS~5Wq5)Z=ay7e#+3!eJAi?|3XRLb?i3a@t$O#T
zz+Tvw&>If>G7qo_`%=jcU|+_e5fDKBiuO2^5#mU@qb6co(3t;GXnHM?2zFv*sN!uq
zqr$_*QnT#wWgTd;6d{9{uY;Ra7h*NsXXW8cex7OZ6G0%jHBawWyS(B9U9UrTCeO8Q
z)f=fExRaCo=#c3bSH9vHJb`CGcKj-bPb7ZR(Fph@PX;4ReDqm9MB_@$7f^0dzmuQb
zfuF`KG_{f8p0d>cn&wnA?}Rl>H0LC)pmmgov#WySP8>9$#%B32N!i0bE<hguqh+Oo
z=8fn|(EJ8L!i4d`-z7{7KpG=gzVVIojcI5o-Z;g4V-em+J_P`bb@+t=ZgMui%NhW}
zB$zta9N<h2@E9&^w6;q!^~dXC08YJ0rjh0wPmrtAoNw$TAyyezI`4IZ_@x#b!=3l`
zu-#+EkDd3pQ7%=7h_{$3F&KR(KLhtTvl9~gKMbECQ$>)eIxM6swF^MRw3XOKo6h2Z
zCCdC?;z^<_^)Y#kXawO$N5(sFkf}w;Qy>jAaiNvw>DmN+%?+oolfQfk0$@82w#Rxg
z7`s>!4SA9B?-?_pV5)vItSR#(wfoACGd_ZdLshw4Eak@-jYeR>p%o4u+p*lWCPBYh
z46w55(dEuY5&Up^mr;`Z1a4p$>{~bseIG#IVH7uO+h2YJ57K4E#(X*vWXRvgUusl1
z2#<q1A4TGmn>=P*(-~d-16^djjQN(>EQ=k49^)d>(JtfH_!|j)Sg{NnH46|?8a22<
z+jOT`RLCqwCfJ+<H5uRhqfNX&HmH5U3co|LXlm{!jELGra=Ab<PawIx9V8zGPl@CK
z{4K5BS@qLkyj0?NL(Fa2H#JH$*DY@kO<?F|O3b}PAZ&~h9quxoz+YrMDo}NJ(Rm1Q
zj0bRo$lhlmn<p!;d%z$Pd^zE3G87Blv<QjRuJ(@Huqv{<b3f^jtrFuO0NAkB9>!0@
zZ4l$GfTt|_MbRb}v5>#tx9ur++wBTzpJKOT#Px-71>OjOZw64AiXhWig&Xbh%?DI0
zzH#4alQ9?~Jw@)f)%FF)mTAqBZLG`GRw6X-fN^ED%gfIM%`abuB8|49nR-p)Fa&^L
z$68gtV0<VPrJO7aA=d9qSGEH+wqTC(bhG+kI6i2J(F+D668NlQ3-EXZah2gV@yKS$
zaTzQTbtcA47*9(ctewM0qhG&CbfjNrnn+aPFX><E&@UoVf~DtUL4zsip{;s>{y@&y
z3TXkei~u^JZmj}tOWp!*g}Bikk67q$@u<o8BS<o(4)y(<P}?Rei1h^Y`WEDlTnke4
zplNeI*$+8IM$wNQ?(27xGe{L=sGWfZ*p#3@2dHZ6VKkK*9w03CSDuO=;rKA5>u&H5
zfRFW}Z4Ukx@GrwqpVU3%W1xx3LhiR^VFvbvV@aVbUYt-n;~9!M8d9eUKEq}scx43@
z6UsyPmjymnY98im@$)ZS3cmvgcF^k2f*4|k`WAk3V?~MaTX2&}yF$?SShQ?I-_5|H
z7H=6XKn0C%ZFZR8B4C09iMH`Ikq3iQyNw9LjuC&`jw!Ww6PCG4NH#}4rw%Kj1+I|m
z2EP{$%a5R>0FiVn8M)cW0b~eWo)ypuU@`hsjNUGYAE33-SAw{1i37v}8^qa!0U*Bn
zEd1F7jAHx^4kLr#s<v9#{p4tPF8Hh9*sy7BK*(LF&ZU|CO4Z(l2rLsAI(E2FgF&nZ
zpD|h9tkksN79?6($RZsf)o>XxcPh`0$C}&_R;A{5c#a|N1?1$F*s1jvL=wEh?uK1N
z-)DR(o#13NLgubrGHk*-#+g76w0f(_L5AyWGFY5yA)MfpG(6V!ms`OlVrCj)3r;B3
z%@`g0fL;L>bpKCu-w_yN2*$%00PR#K7@Px0fw8y|Myq5wxp5k!1@WIl|EPKExE@3w
zr)qCd`PKuz3}qw0MfE|XE})ZMrG|UUK+P~lhVmL+7MUD@67~TAkToyX@~gtprOXP7
z0w1AtLoru$H+%Rtv=I4Un$!!K!^7&%_Tlk91=hR)ey%I_5+xhWOalZ~c)f5e;W1wP
z8zz^5T@@Qt?fuugp|>Db{f!cmqw{3HFEkMHUhx=Zyf*|;+9oMYCKr_22Es6R2{Co(
z3vjv|GYjH$?$b6-wSA~D;7S3gJq!?qI}nc7Fq&};h+&7OaZm#<J>J03lxl2o<vDbp
zob2d#7n*d`?-cg?4BoK&eE|KY{%n-JkfmQiZ5dQd+be1elT`pEi{G}sNk!{W%NV%U
zRFO#W^REGDP)Xyo|20Vh#pCuAx{@EIm5zQyKNd{uM{Mh5O)*r9&cppz@CHs}Gd;+H
z7bR2V9tIdO8J{S%+**;T9vFQE!g`MWD3~rReRIWpOGW-(-?54mLE4Q<fNT@(Ey;|0
zyp+j|Gtnw}gUJl4-Zh8mcLSg|{LX$iNM_W-IA-PpW1mb#FkbOJ8IH%6Om&EwId7}o
zhl2d__q7)7od)Byn)-^h+OnYbmi(P4tF0IBmN!ONVAvRmVquWWJaA_HK`fz0SY5aQ
zYeKlC@|N)g*%Ze@m&ikC8)TqeXto|@JO~6pNUm0RD!MHulap5wc})IBVP#e@4&xa&
z$%f_cA?LP}UE??pu4J&htv9b>YyE}PWKa3eu*c()h>ny!5`w7{?Dz#qv`A;{Aq)mS
zEkB3RYbjthL82W%VT@cHHe7PFidD&902nx~-lCAlheHr5{k<z5E~-zy{7oBq9;da0
zwSCId4K!iugV@VzRl&oUeQlP-j79*M-nN6iT;{$UK}*B<9}yK{IZu$JY6Ld$lxd%p
z!J)_VL?#B(0vks8#-^C>W5vRbUs2L@wp$m4O)J8&2IMkVR;Tbc>{=UkHHHFPE8fsH
zVAB(}om2Av1{{&Q)0(*4v&-l~UmjcHOCPErkDp1$N1V?UVQFu>(dkK~^;P9;lppYq
z+mDLo?z!0F&siK&j1Aa6QX=MmVvBeTw2d9?RB<tKVEd48T2plrpEDOEvxrDbPDC<y
z!s_bOUgf8(+^?U7ByM4&@gR&!YJfQ}9Qc&*G232sxVoQVrYt7f*3|rC!iKzX)juRS
zoK)6OKydm7lU-Fi_-_%jnZ2%i1<6<2|2W`5W_AVfs7^WNrNQxdb6ahzEIEPAg3#U^
z(I<1U4GY8!3wKlMN+3i9e0X&P$t;yY8e&I;U8DFwS?Gp}#)m<ABmVu2cXmgjn~Qd`
za$rEX;ls|BR}z{O(Ke}_Ct>lf7NI>lqsXJ$7CW^H--D14iN}#v#>zNhm_TT*R%*Dp
zWJ;ZwpPhw&C%J!AhN<?ctlq>tf%m;g<WBT9P%8sS%>&aU^t;>-7g!LE=M^Fk%=*A?
z5!XgGl=X37)1-!K1H>VVnx=EpQBSuYTMRvAu8olZ^1xGp8(`%Sz5Pn<t<WBTx<mqz
zoX$&|-aLK8Lo*oNtfBz*=7`ov{E5@*muX8kMmi&91rf)s^Irt7@V77jN_halO3ijM
zwpCfUnJ6oTs~q*9Xw7`}M5`_mXfCVX!3E`$pd`M~8|V)kK!QkE`Yl{Yr+oC&fEJV7
zZ)QK-S$-W+`ySALfBIEcJJHj`CcZ0nRPb>Md$7=j_f64AZbTr}%&jxjPi(F8fWD!?
zo624NK^ccZ7JR~+OK{eNO)p%p5+_2KaS3Gy0tB%sf&qox32_qwiKd^ESaTs8G^G=8
zaY`jFm{hwtHB!m}=p(kSF`+-Pfor|rLpzb5-7sAt`Q&2=$V6<E0oOsRPS{ZgwxdFs
zqN6PQG3+4ctpaoQg!O@JvbExAy0`47%gZ^IObaUurbv-lDtusOye>+FLaf+EQXbR%
zhz8+<{r7?Qu;i=xn|2((_m(oUX30PGkX8#$ioYlD7mBE`ILu!K#?n6tyQenQ0slO8
z@!&C6jk$CzJwq2v5QQD%y`jO#=+dP_J&yZ0`zF?!t1OTKXzuF8pmj8FF^&N<&kI{T
zddyYf5kpIZiJ`txrI+$mb@Ax%;7I9JV=?+r>`TS~DDObKu_HpILlZ+i;Sr^wr9-{;
zFwj@=JTh{8>7|+Di~Hdb!69S#-Z%?n+uq53M`7f4-;;2mS0`MNkxsY<{o=;yKfB=<
zlLg=f_*wDui_uyDKBX>}=p^ueo)nMgCSPiU|ES1SWu>9;*q6%$Q@TviMo66J;2L=0
zxf&j(n#hXlQ0d^Y!+_6K;W4EccJZZXI7J)k1*ydIF~PxOBYzL#_R^u<_~I08oJ~jc
zH)7-nix97m9FLiH7u<|29ohxGRJl3jxAjR~e0cm|;o<ROt!I{>uPo&JI@Wqo`Ps47
zpNNkpmiLIY4pM4<i~E?3B1rhgRemD9h1j)Cxxt-RkdbYzNE&k=!+wXU9^+R0O_b2+
z<ru#dzw`O7@iY8JsMqa9%rN43XoiVry&rt}1~@i4I8J5{K8m*Iqpdd8V-IEXyJ!=W
z?wno4TF+2wsYtItIa`xI!#|wg^87bPz1@J?R=AYB5VXmsEaaD3XpyV%7g<uJrb3wJ
zT~N(a&V9ZU%Y#JR71mZov}ROZnl%=%mZpD&Y4Ks96botabCXbMdGeoW{rlw}3$)M;
zZcU#R(e`8Gu4R8bL35w$UggSVcuU=1iUZX?W^Is<Hro4%cz{}M?<Xn@_i<czjqF5k
zXpYrZ{KUCai0ZJ}Ar!d5pQqeK9|PLJCT81HtQPrP<-dUSI{V|IO$aowp_Tp+AR>q=
zY*6mt#2vrZqWcyaxxCGoFknsW{pw8w3A=uMeb0OW$N-=|0-(9Lb^dw01!&55=7?k6
z((}f%mx@5+qCL-&0$1)w=Lwr$9KkIDAe<~!y(jg1^F3qqAB8j=dd?B&%r(W}H7m~f
z2C_gr9ay4wY(4?$3-NEnUx0rngAHRa-x1`;ZJ)U9!|mkpAXHizEcrm>D&!ua#KjT)
z7!`{#C>Y?Jm|m7=?a_An5D9G>5%2v5&27=naKqNl;rPhbaAFBMMKRhAjN}_0FV4pv
zHBVXmdQ{OM4(qp86d{B)Z0_YIEGyPi?)U;%DrU+!`l{4Lx^#(X4N47bJAqV-svQJ;
zU;(=I?wEOMMB9RETwuot{5vWN48-UAS!%MZ!T_OHsPL~q-@7B?+7S-CRo)ey`c6o_
z3L!#>jyF%3c^V}8SSt=S)YwjR@in?&BsytzMKa=gKjL~T;z|*4h<us$HV{)545fgP
z2IwE_7$6NMFy%kwK?|yeLnxL^aInOF-c?Ni8DN?D9N0B-*9ZbT)Y5VL4ypslK~b`*
zawm^*A0k?=m#B33aPnxwRnq>z0uALd+3Qer$9ymLN&Ecc0-u0gvM=riM1p;tGTHZ;
z;`5MwO23sR2M=*g2{3vD_nPpYlDAyB4F3M7+~K-Lxnn=>O;PgJ*n9*Frzn@L`3&ql
zK^{qA^AY`fpS1Xh3wP`@@Nu5S$Ik>GKQsCG8Tk05#Ygm(<|E$hk>(@&d4haIUq};w
z-730C^AX56*m#k}MszPq75^PJrdf#ICi(jaMmkuy`U#VTl;$ip0&Knk>sgR|m6J-#
zo{H?FUd^@C^6tiskE4sk5E<*}C{7o5C&;_`pgqzYmuH+0f%Un-G<&@++4uLX9&+Tn
z*}w7SAJ8r9>;Lz67&;@~8mmS>6)q5I>D%wZ0BpY<%`j=a-jf)@WNS`hh&YC`d@2eS
z{zfQ_GtdI{@;)^>#AD5L>rb;bkeIVr>oAJ_zlCGOxj)*FeCz-^UDa>#(L2mxPv!xK
z33naE6qORL!0PH9P$01v7Uw6O^N_HUuRiUtllFOYmYrM&wE0fk^J_q#lYRyIB!&s1
zB1lXO6NE%)>r=I^$(4^})LWr_^5%5=?A8y7)(-lL!gj}*FZ#FwH9FLzjW5v0`L$vA
z;n&A`=xXl(epVGTNM)9OnAQ~4%VA~MA>)nTIOu9Nv7SAHXZkEj#?~qb(<QzLP6tKQ
zpjYHu@)Xp)bwSO0x9+`P^P>6$^n6#)6Y-NYS%^j%Z<+*2Be=xF=SaHp44`!Uq~~t9
z$su3hdxm^vpLgRko)jpE^KM$fc(4T^^N<_Vz?F!6fqm@OTO^6LTaE6;e0Wj7qtq}i
z!jw<*l%I;(E~26LAp2TlQGM)S_wpfRv9*4DJRm_d_1NDWa`7JC&d3wICIJvH)!-o{
zcN1>e3AzoqgHEnOC*r;1am^TPzV%+6@f3cd$sp%Agj=*f&TKypjgnWw*wh)boc(jw
zkotpDAH{mPb?lhbw>`wab~&oRr~>6>+B&9#*FG%P3KWU6%@5;bXa8I<2XGJn)r3FK
zgl8#vKw{ziC;UW{N;I+2{v6FR=yZR&{VVusH~a(*4+H-$JPvDnu=S28L|5y1b%DG^
z73Ty4%Vf~ZIb)f&BBU*i0HuNl1xhyv$gDW%66bjQaxQ><>nRhTUM4>K@TQ5+$r7Kx
zqKOSAKXjUb&r-BcJxF;oQut{%JRS{Ge-QWA;y#9&?Ci&D^>czbZjbw!I2LCtTk!tQ
z*&l;^c>=Sge=K)676mStv~V5*4a9!>YvouoontM)5E(EZhei^|40#c!%!<B*-kSab
zWfk-n7eh$kFXVh!AIO_t<1BHLrqS{i4tte67WUMk-wyjfa|msJ3Z}Gnr|E;V>kG<%
zuHxic<`Rp3jlqy!#~G7nik~q3mYuf!nKA1DLDxH<bnxfqY+TPoBfbl(VA9p@iPpsd
zubSveL|~=_k~n_p$$On4ZPxSToqXze-)W!pp*}TsagH_&Zfj_PHmpFYp*M5*FUxUR
zJ22#7N(1f#w9!infI0majBbwYzi4mcx4CF**@E`=0PL-K`~dB|n>Gyypgs}Z1Xhb}
z79RmjeKdN+L9f^e4-4*Lmsrmbb~`>CacZM$;3Wl>Sm-y;HZ$#Z20i%j@}hc@QQEiU
zu@Kn|{$`#><}<np*DdVxX0kEUKI<cW(Ai%^aQIk6SHjUw!9elc>WON!Ash|OI}{vP
zJa_hxSkfg@aPT9ccn}2?^|K4&c^>WEhFzz*4z50^Mh}LggZ_@jFIV=^TJXGb%V*N>
zXsc7Jh$mjdjgp+jq>5v{Zk{y<wec8RXqEKQ9_-rDkJa<$iMFsbfj-YDA<PHpvBYjO
z2fu}WFyS5@<{LPyVD`1Dexg=9&$thL!C}TnVO#htPhfS`1!{CpEZIqI__)(q4QhPM
z1&44-;RV>py|t5i77}{ae7Bn#+u_!R<1n|q6?=K^@`GArfjc(D2M&qz&m>+(3_=2X
za^GJZa{nY4FLb;+B>;Y<A0-K|U2gNsK3&h)OZ~^ez-ko1<MAj3_%-DQ@&Lmc4D`7t
z67MbUlzT6~0$zHO92W;F`mlpP99>;?j;ei$j2J)%U-T0Lt7jh*+u_nqWRoBgd1@@-
zM!V8LL-kf%h62lGpBiy}Y_=cg7EPAAwQ+~k=x84fVMxUXw+0$!pMFU^^gR@9%n|VT
zFzG_<<km**XS4XU*1((9$zURM&mpohcD)a>!AL^-d&WmtBT2J0-#7+{qAC{wX(VbG
zU(pnh$@;OFj6A7EFOJ0BF9-DjSZ?jf{aTnYM16YR-J!q-<pHBN6bEGP`)3E4w*W9f
z<{tP#GAHg4WQKV$kGU#AojF9prwzJW_vj~RJ#b~3F3}<#@8~cN`uKd%LV0?r`XHyD
zjR?4H{~OUy40pb*CH1bke=-C`DXI?*9Pcf6<7_vqFRuxTbz@~=9iYMcy^`43hQHmD
zkKsHsoKRFn*;MtB`9ZA#=L{?>i!Nuz!;Soe$hkMn7$>e}Hdg7^Fk`41FK$L}IT2Sq
zmWUdL$3x3KBjXWQ>+pDFIYe_=`HwIHW@cswy>P|ktp|q&SU0DyheKNAY7)>jg@K_s
zAYGW-Z2XpqohZ~#%w?7#Q-uCb`M$toQukvdKD`2e64mW1Wquy>!`0J*A$>5!v7XWC
zNJQ|;(=&i6&x6Bx5aJXFPSM>sP{};VAQ-5x-bP1h=^MvmCzzghf8tR&farQCyM2e<
zl9rF-k=eOC$11abWG*mib&RnEV+g%qFLz}CXUe|=J}=;#$V`=7J0Xnr`vR5=_3>VW
z^b3bT;3X6|N0!IO7Voz74?waI-vj~5^r<*?5K>dP3~v*?<&QYf8_fPb!_3g?k66Br
z7VY($fgbwaAr>mx!!wQ^>^I3rbm!#?TpBI$-dwcL@V8RF7bV^7$mBoXJ;e`D2((xH
z_4OL$<jr7<?M3h#`4PPv4pFE^g);0=5C=RcPrsYyy*R9+0kVmdTLxXzd|&jigD@k=
zu?^0CNtwsdq=Wu5h{G}7bIKQc)1AGUb}buDFW__#%j&02(&vffHj_TV@c`$HE&}4h
z)xmM3RRL&(JzG$`x6N{+v7?cfR9)s0V!7Wyl4Tonvip^qnTXUNRU4Ig4<VXFs%aZQ
zyo3oHgU9Y{D_V|!ORzwOidt+-$Ku`TVI3vlw%n4II0%L3)`t^rekm!Z+6cl-yEc>t
zHdMR`kou1O14&xA3=3o6;u`>Ml~S`6Ar(g6pv+?}z8qOu*bTqM=tVoR)Uu4-;qPw#
z<?7^mv4*La_>eE$f_JuHfo|bTc*y0w_2NYwSs&DA`}E5(Kb?s%qCYZTEhe2WiMs!S
zqYdZd&1E6iG91_u@wechlwUC0wI0=s`W2oseJqBW?8k!FmHE6s0GcI}tUXfQB;G=g
z@?ykYjjnL*DAoOUfB>bgRe?k}x=NUtmH2m}yM-&C(8Mm)E4ge|AI7H)LV?NtEy`Ut
z(P05M7oaIY(ZGHepduYmx3*iUJr$3o3khBck9!rYR3S{mq+ZHHLO^+@7}!<?1|oBZ
zMPt}}h=rg*BnqZse+oIsWjnFH+3VBaiz6-dnc(CSu7d*ezGb=>ryV=w5DfE*tve9q
z71j|qNU(X|t=__1x{K+qU|?fKgZP9kNkP@T6`hl@f~B#^glY_+f>pQxh)Af&K*3j&
zpIBFvtXu{JwWT58u?$5Q%0f{Y15y}*ttLmwPH+_;>j6i%aA~cD{A81sknW8QiMxvg
zN4Eg4>yM73C!|EZHA$-<o}b>8LM6ddFL{c!`<^|j6B#^RrrdSC#nVt{e4K_%O*8bA
zwhRRWV_~6zlC{OZbFy|mHZ+AGG}~BtG|pt~7&7(%nAGbuG%nglZsHUmuJ*^hqSg%9
zpMnK;B-X(gA7)AQ9<mYutNU>tS;Y<(skl}TEcb7#z&ZrhyMNn`dY)mcC+bmipa$Ex
zbaVaJCNDG8^AOe3QpyOFa=EgQa}WVCl@T0N^$Ccw5FmkDLH$Z(zaZtfSWi38hAm{|
zZXOD?et0zR5!|7Edhc~nL9e!S^UC}%>Q$+uSdpfd0)ZYWB-!T>hVN1@54ES4hp_W1
zn~fylEx!>Yq<+3b%PVv<=J|?;mq880xrPT1nzW-~g-S}JiljG;D!#Wgu&5sA5iM}q
z(|F#y==?Yc9%Gi{sDG%rp<!KJu_E~-`lEEw1g^zMg4!8;IlS3?OmOJ1X0s!EdBxFb
zSC1p;l-stEINO5y^j3yG!A8!q^a+TzV<U^Qc|plL%aTzwT1jO&2qnQuDGl2>f9^$F
zhxxF(k@7-fV?weU8lk!l3XRy5OkyfwS+zes;IL}*c#tmz6#T?ES6Tebpm~NajDaq|
z0&*sHRJw3PJwVNu^il2V0p#Z>ebN!=-I45{19~T~J>Zn;iMaZ<l4sk7Z1WBS5kh`m
z*ml~5OQLzwz6)!h(->gR^5*{|`wn@u?R)$3*v2-<0B4XRz&UHblV0_d!jOJ7rY+m5
zW|}fG!VA+-N*f8A8PR>=Xfw81)DMrk^`P#ahuSF~5(i<vm_e65J)he!^4?{b0&{`z
zH^Uy{7Uu#J<vMTx!H5V*I#qwb3Zc@#s_H)@3aNL&S(Stw9*dh!$?ln7inZOob@#Pr
z;+BB@h%mNsU%_Ha6e3Zv()bOcMtY~M(mUl@-sv<zd#<YO7WH443fFr~3x`R4pb`G5
z0siUDfzY#x)yXJ2wY0I90KtkeR)KVuUs}7*q$)}Vu=tOV(Meg99H6a}y21mYTq2_9
zo75HY{P1Wfj!A<%HaMQ=I}DAF1C6`FjxcTjeXY68M6=kCG^W4pR{g$3<HrS!L#Q^(
zCh{Z_`FOxj!$P~#)q#sJElyObZgQe=G(vKIhLBJ|WISum%d%uc7=~`uA6sxg-H~kE
zOt2??LFO=qr#ZF4(4n}SHVGNfR~!xW#q;80a(Tq(XOQs=Q3s5<Bt!=1I1lCYoA6_%
zo9Z$}%x8}*-~ZDQ4%ZQf2S4Xrvj*_X_<_OfU+;F9{g}WFku(nly$-Df4iL1q%(?B`
zqNhfeRt+*4<j~=7FJF&!gyYYN<MW@<WCI0pu<4(1mBs<;Vj3W2q1ej-(`E$q3?n20
zHw0w1CCby|p<X!%;p*AZGQGIYWET>fyxAVZ%zBDUTa+ztk8z4&o1)M_1W!us)x=@<
zUgX|H{;MwO-Xrq^=KQx#S|;qR5>w#5r%3i;{u>+v@!Wwl{5wd^e#R=3^_cg5&dKi~
zmOl?r=J6X|RJ-4}VImkY(8GU@m|-}oQt;JH5jLD4^E~ax4JfayK(UYSlwaN@0O5Ry
zC*X4~lvH9r?IbgC<I_)Zl(+eG-kF>!h5Vm0isN^5@r68V!V3|@PZm9p(%UTc`V+2I
zd`LpX)uQasu;f>8wYC+XY+O`zGmmkLF6)diJE+gVWccTp+FphH$XMjAitk{}CT6@M
z9>>f9i=5z3IO?roOcd06-w}+D=?;RdaeW5CY~e|#jMjXd2Z=*HCmsVdIkoLny-du(
z<Ovd(C@6Q8gJBTJ{S2WG(W#VRZo8XkVs5)%scjRtq0e3yEELq+fNnQ}KJS5gDJ4dc
zByD@jr+e`9Gd%qqPd`;+tMQbA{kafq9C(<c-{y-%#U!brXdh-poE$l3MyCTA&Wz+d
z9vx@<AA5cj3cO$OX0k7))RYHG$c)VH#kd6g{x~OMZO8hdHI|*fMeFSO82z1?k8!*e
z$rlj3n(i(wLi3t&mlFw~iEy%!Fm)&<=>YW)?Fq^p+?1}+l#?6Ma^jQ`AsD(TCs1yr
zQXl*_aw21U7d&AnAtS4@Wn|_NWTY6+Gx8tApXfJ%7#*W;MFbDEMV{wBz_$J8^#iFp
z<O!f36S|2g;9KhlQzjX>Qv(#eJ?ruUW-G~uF#V%Hm-g}Y<TgxAc&(50z=aOfXLvEk
zEH+mcLh%wTlHiEiFm`z`RK$C(#=!1!e67IOjWZ$`unC-yb*uQ8*~{$w{G8NZ%=HH@
zl=1k5y4y)@)Ts4Pb)VXKu)*aqtVh60f8ksYmGG@kCG7Q!ktn+j4IJsQ)-RSb`G_oT
zw39yE9gN@X3hb%=NR1E10%hvtI{1cAU`2UhBwBJvrzTlVdIa`Ve1KIA-{3&r+;Xlp
zh<Hw}He|!UEVaNKpPCjNOX4<aNOp6jVN&<M;Ae0QZuWZT4(`x|wZSTb56TalvwB)}
zBzhgf^_^9}L{?>aME`-RUk4&wCBJS{xSI4F5uNmFPF{YmM&0*DqM>`SE3<y~naKS6
znCe9VYR}{rkW5_cRpPAFpxs5Q%G8LsTYRI2-^GC(^~@;??5O-3#X*$ESi!IdU=lO}
zrOaD|9wO0e#5@Aohw;l*{p^Sd81dVCyZJdA0+=(B@C}cSc?bqR4WJTd2?Bvk9WU*p
z36m1%7t~G2Q-O6RgpEp#f%^_PKd5}%hLdz~G_CCmnFebM0gbycc^a-vLMjUjZ~>Ut
zv?1n3Gq2M=J%a*NFYqzpoc$vc#$EQnE6dNZ;Pa4<FC?FX^|bt@iO7l+&ax48NfJJ>
zb!Ay<X`RK2WWE!4fZCMoB4CC*Ov3?`UWSI3p<^i$wk#lja_>l!{R)=VP3rct#<%JF
zBRope7c2&CSqUp(S9)U2v%wP%Y^?m><maY?P@}!i@PYc2*stfOMPb@B!ou&GI$)s$
z>=um*S7*ZXI^~8O?@4~y@H~;hZwZh5G&5w<0nf9^$ry<vPLd;nhZgo5r_?#enjH}1
zba#w1`^+@IFviOA-X^}sSopq{bW40!q>w=r!ecTAX|}?K<a>hV_z`t7DA|nLwyX(u
zaj|TQxqgE6B^XV@fj*WA^>3;bYh5Jf6ZlExUk`d=1J*F?i+8^OazIsz7o`>MQQR3G
zy&=aPi4OK*CrIE^AP6PDETRv`wDKw>F+Pjv$CX9BbBF19ca#S5o+}?!HIx)a+u;fK
za`st?as;eX@x||}`ZOe?=kT^@6&*AG5FQ}rWR#Orxx*~AKzkJ-L+VcAGrmx?#WjO7
z_eK_pyO*0a;sQS#$i?i17`|EK5SOcgYxlQtNAM-^sS@Y91OY<t9btm=Qqh;P@J{@j
zjnR^1KcN~zpO{!<pG=IPPbEHpKRWEU{M4#d+5Ig`zAu<8=#+)8ms`g~nZ9><>VDQc
zEp;X7JWV>br4NLm!K9HwFvWj%d}gn&LFh;iF~2zF&oSY6<@(wg^^$sP&B?_FsAcCC
z{xN<-nRg*IJh~6tw3LMl5!iEmzAU=I+;&!kBf|C~f~z{oT0e8nC)B|1O6(p^Aoqcd
zZeLceqi~X4CyL>Pq2H>+W?=}htnV=rD7N5&8r{HFWnL6sN{r4)LP($G4M!W8e8>D@
zH>zm(cYip#3yUmOGZ+l*XRY^cZ1}|F!iebC=ZA4lZJF})dlAgHr_`b9CXPu#dE6b?
zJ^Lcoe4>7(FzrvnQGBQYa{{r&fH9AE1)v8eoHF16LO(f0sCb9v@@gPi`B$!hl?6Um
z?wkSoX9qcb&z&b}jTf0n$9Hl1eW1kpNu<yBnomJvGysKUgIk6J=c>{UsQrCTZRGqO
zAmwqt9@FW0sNtb5?ygKasLsuFq$&PdG`%0<Culkn0NXSbr+?ZsEsJg^O+|7hgQlva
zX)|d0PeD`ZXGmAoq$`%KO}f?rJ~tK+^X3vX($k$mPZ>(%8~|p>0$VE=b7x8!&J(<|
zI-8c36tYGzO&9dv%b0`pVJ!aH(wIpfZjuv=Mc(%5ll(b!Ly)5cbU8dZ%G-!oHaRSN
z=cLE^8T9a`=>b)DNSBZ{E5w%aMVdTs)8w{nnpg_8wA!|RP^ephPx49O5X|`b5!*kQ
z`Y+m>>+5IKgMJjewEi=#0Oh~Hq5OXm%72qrDF0HD8)=rZXfxx+lhg7D{a*@2wft`7
zF8V(#ua*V&Rw9Zs^*;mL8J7M_pgPk3hlKuDd!h8^E~?!y1w#K#S^bq@j#zZ#YL21m
zH&OeW@joQhy|ic_>}fRg0t{N@1$@zd+3X*~trfzg`sixByJfy$p71+j_pa*N5aHYb
zQVR{@9H^&vH_jV@QH}&Yu6)+!%<OE=KrZ_ZsA^C1e{5?0apu!(%~z@ULqhsZILrn}
zfZmzJ?k(~f)O+Tggx-tAZ8)RKq=VWpG}(Ip2TSjN3UI{;q$Q4a`bd~li=1TXt?kn?
z0a|W0X=&-bgPs>kI$Bgs<ziM+h#Z@@Bbp*{51P7nSG;S{^nY<c2rR&t%AFP2G_@>1
zzOVpk`p^b!tCTh%EtEE$ygW~p$!l2z(1uQF{l8G?|6xQ-SvZffO*eBmF=PCmA!kD^
zijY2eyHni}oJ;!+`tXx?K5rALB#b1Cf-QGmz>UPal)JSSjs9rM-PCLvSt|DYET={I
z87;ztun1<n_+avLbAB!JjrWVzR=v$4&=U4QBpdBYo5?%`8_7m&yl{(oBPumU3j2Uf
z@H?yUt-<L?Qz6-itx?hUZTxsX%sJQs7SXYj9xmfu(>|6(-C|*{%+;a>)?bovFQL__
z#55Z_69YRddGu}HUAGAZk_B^Cl}(>x#PG<_@5b;dVpY3hhE_|}2O~g%`@ae$WJuHD
zG>b?SXVx)X&{21dLW|+pgU8WOh>dRzF?|Pqfv0GI3L>mjVO?qwb|uf{^_ED~eRm{!
z!`*IPt&hY9H874U3mCHpqU_lhMn&hjsy+@A=3&0!Q7_VvC*b`#1KoEhH7DdUrp&>=
zTz$YhjE?7UY^D|?Se`X1YcnDSArB~tIdrS!0ip#EdM@6GL=TAVgdajknUXFc#bF64
z{U4(>w^MBiM^T83OaYJ4syYwf!4!0lVBu$EJa37xZ-Zh&-h?ltVnkdCHDCx4J5;$v
zO4iqvuQPtyLMB{?zLV22C@-!MV2RQWM^(TCJdH&62p<{|I&@=N7XB&h1Z~RsN0ft+
zaeN63+TatG4N~u`d|yh$7nT1=?tv6jzR7c#<L>Kc3L41*op~!jBTB;GNS_WQL4{x(
zRuV)$4RSDsa)6m1XkZG%Jj{C@D2v|k0OSC9X2?OKl!K%t2aRbt7!h^fBjw=s!2{j%
zl$xKzdnyY@;$K?mGDM9CZyEo0BnQOgL*UyHCPyLDb3l-PNv_Yc<ofLP<+}XBszOVi
z-)<w%@eFxpU99swhB8bRe!_`t7_3Iw5B9@0;v35Le1+V{w?{cA$215jN0>sGKo86}
zif@fbU`^$F?33ds8NLbyiH(3MuvDjEx>I%j2g;{Lli0&rr9zGI<+v+-_(BJ@W)T{e
z7Hx_|*M?yUa54<y7@-^W!eP+p*3%+K0xgx>!`ReZ<}$*8ROLI=gwbLYvzXViP5i<l
z0D^7$0j4dMZ8{l-w*x*xlaC;greU%KvRnw{Vpt~%<oyuH`z?VihCsqDC1>L^khDvU
zv(HiWp_sX|I+`{K#T4^~1oCcIYAy!3l!b-(*AmB7bi0v(m@`$zlh?sHNSfiJuAs4l
zZW;7C9-lK!xa)<SnRG(DXVdAQ$+gf$OU_Xroe)XfIc!fkIIpc7i1SxEoBT%w3we*p
zVNV>3EO-z5Wb%HN=nL>e4rYj0SM-aGU;G4_lrMaV0P?k>1NrjtP?w3GuxmYxNrO3v
z8dy=eiJg)#$y3p>1wY1l80c8>J;uQY2(58;o8vg(N8dT3ZyN#jz1i;jQbC)vHS1RH
zv}W9bMH{f06$tHHv+r7VBwrvueJuIG*3$Omr~GW3cVn8dx6^WSck)~SYukecSL#{B
z+nM~Z=+vIR9|b?IgfNmS%UN>LK2`AboFizIj9aC@x76yW?7V3g8)w5Vwo8}6RsC(c
z*mSV`e5PHr;y;Q*>W^vrmqo9`$uI1qEx)#b%#>elyO;Ke@`Qa7IMq*CsH6Tx#6~3H
z77;8VCy1Jt^;TqrpvfeoA%Y|j%YZ)|5%n$_5v^S0PQzOkI4~Ra7}DPKvHV3F|MzF%
z{~oaY-+{`N^n8dmJdA?2RQ^8s1&TyySP$MzJ;)9*?uNQsCaD&`J2FWHoE^ch(vIiV
zTVX_AyLP-KIS@U`h+>%7LNH&9ZI<KA<7y-6bvYmt<2mW2c0ez>658y4k6t)1JO|%v
z#lKRZOnULuD2rars7WDo|6pwj9h(!nZ^ca+#~hg5*W#nU13s1)Rh?$Rvo@1|PbSX?
z9LTRxo}W$2N0vMrhq1>;A<yG1d5%2;-Z(k8>PDyaL_$Mw?orgX{Ck|Ed>`4LHVc3H
zqU}!)RKlMkQEap8FUb>FW@9Q~xBn7Y8$l#x;Y$2#ajgwl)u@~y|H-%Hj~J+Mc{T0W
zEQVS3ih3pe^O5}92M8iWug5TqMXv#?SA0oLiUHyk_GiX>ZS8-=Wn|j3*8rgC^AMb8
zM}5u#zM|iD;WYb9`(bYJ5q<VNOrPHp&ZR(^y7_%p_+;zmx3V0Rza#qNLdzc?sDwXu
z%H7k+KZwF2!Qa%gBpFU?l~Id>4JA(e+Uj8&K1>mMDAzBzf3Gh2{4U4(!Wl!amo~gi
zOUNxeSKJ`f6DbA8>*BZ^PGOaKJX|Fb-HKysl!becSP1Et`FO0y0PJO?DP=ZclbFwL
zs>;J|Ok@fg$zKr%2G(<Z0SRN*j!596N>(iOedRhaw3S6PCx;BjML`XB)}Ti^y?7t7
z2T-I0RQ)m!=F(zTHHk?CWDk%kz)bLa{JY<BjADL>$9A6J2`f*40|}WYm@M-I10wNa
zGf%L^njeZh!BFH0Mw;>X?TE*@y06qQCx}I3VLzBB*y6|&ARmH5eztUwCwTi8lCUJ}
zEtMp|-^HD5|M?JHuSMs(@%wOe_Jhv0zT-eCbAbWpH{~^2bl>iv`%3WzF01!*vU&$h
zy@moN2RGrL!HlZj7)KmfB@^^Ku&Uku*CtPgYFcqP<`I9vr5Rsw9$}g*FFtC+__ED>
z@~Gw$qs0(1KV3al5Y?2ObHElMJG0QFW7!b_)8S;tjyoYgR@_<c-;IeGg~*)FO_tL+
z%8!{}*gE@&ai^G%Gp(?-GCd!+<R&xjq@{Dt$7vD7e5ow%HtF|o%$G*ME(lVMf&uK9
zR7X@ZNBqE0>RalYGk?*ZzNPaQTaJQW9p*1Ol(%n{zo5!?ozh7tWO4|6W_#<mw<jMH
z)7=s@Yrnn-`L=(pz9|3O)fb_nNU2d^O&NRv_ERqZxhyW21sqU#Fsb{6xPfX}78>9r
z=5`7VVyyq<>uuNZyWm}OnGzL1T#LsL-95N#*&|jZ4B=Y-WOL0Ob~?K%L8%!k?5X$&
zG4ch@>ICFK=b+nvsXl>YCpb2;s-ljdow3Vrta@9n!a6D<Qg_36)o|FX0t+Qx4vk@#
zzdlTtmsADYbeSB87vUGP>j9kg1I}|+xO9+;AEv(IM%f?RB_BxJ*X;h?C}g$k4Q@)e
z3+cm9_lDiT;b=?M$qxNZxEqMKM4+W|mnAo+V_~1-a;*64>ost4n)TBLAx7~*h)~P*
zEEbiY<zoFANMseNdNqz_g8zWUT)^1TEoOFD^G5#0y}H5>dy|n=k?(VWXRo0=n*CV9
zHSK)#xYp{CLiQ&#GviV8iQXl|y9K(U?`^3+;-x=WFG+ty7?34Qx0iX#p5E0J+mbhd
zrtQ{aEJ{8vQdT8G4`jXGafDOW>wWn%t6uL6Q0IvCde?YR8i$Yts{uUYSk&u{jP|~-
zO*x#X*UL96l>YsCy$Jx*B51qyde<TOC;+3O7>w37fH`LUTSxVJ`Tx)A^|FX*)$6rS
zPuA<@r~WV2>shkktXH$^_3~5Sb-iBMRcTRZQ?Ex@C;=s+Q3v&U`QK%|o{4>X^?LcK
z{{!`UPh{0&oNy({XVqi$m&81(dJI{N^>5Z=EV#nv9?D0odW>7eT@ewtSC3`ZW8i$9
zZ&8ooJGy#|d|_6<>w1hQCP^y8D>y2#IQN2do<Glg+F?Bgtn7cd9z)XdXzMXZ%WqYW
zk^i5q$FS)GYW5m)`FB^3(XohiRF9D*X7KCn*JB7d`>)hv_%Cy+Sav<eBboIWr*sjm
zEqhP^TEfpd?7?B{Gi8aE_$+{35%=xtGiOY60&CT0{zBaC&>s8=Q67rtJm0cDldL*i
zeI^bp`Ons8-ZP;+t6<e<-e5k>whDHAW_#4mtUvts>oX-ykG?)ri2o7lGbLUBbM=`f
zU1$r`@!xHI=3&UuZhdA3In4PV>_NI-mGzlvdi)3KGtV67RH(M~nGa^h17~y*?F(1i
z`O{NCLYRQek}MqL!)>3RD>ayV<1s6RLhxo*CaxRusi<}k325dvnMXz1&5y5CMRe@{
zgoM0-IxJCjR_0n35p`Iidef@I`l#|PFtG2nV}*)j)?ux(>#(L{9B!C+h4Z{<lU$6#
zc|D(VMXH5sr82e3<BkUK-g;H<BGwgAMJiUUm@D89L*!TS7<E^u!x9cx=EuBqkzZYi
z{OUqchn1aQ9c<NM4Tcw*0bX{)Yq#s(xq}1V`AQA9q@t7o|1$s8UVgQugZygw7)f|i
zciL=xz6r%{%ECpUx2VAac~Oi`A&;UVOR!qhXe!JSEIHo?y^B$9V$mCASl*>#y(KzG
z)I`c7(O7TdsKfe(by%z^|3ubdwb=Dn{~RqifvQjEsVrv!9E<6v;CBYo<vbN|{MgBN
z3%*SLqyB0sL$9{=S0ADN>Hv~@Lds(NNFC!<fYIwx0g`z@%)E<G0i#AYsFa7l;lLds
z3?Y34YHlz^$2n(cjT(|Nl;k11k5fmg#20qN`h&Ngbinx8bvI$w-E6AFaK65qE^S8^
zuj(e`A9XiZKo-SWCjCURwxNAtM6UKIgptd*kVPy{V*<nP#28W*i|3>C=Ebt;<QE}}
z5NYIU!Bfd^<Y2j8NO=_XH-lM!19?0buh!|s^M?eAQGc@!nxZV+f`8@0+*Ym^%7kr8
z<?YF7Tro3=I8_i7%ey8KGl^&W^>*uUOuDU0Hh~x<-XE}Y8^R$L`lMVipPzh5BqQbk
z98s5peELY#XBC_I;0AmyE2CZm`E-6LU}6{5^?pWu&T7i7S)UUXeF}X`Zcpclm~TdQ
zXGUQ<-~1-(va|aan}6EY&%EiVpScH}%48Du|BMn^Y9{Q@hICT?&H)G=+M}Zr0m~j8
zr3|nxROIU=|B?G>kN-0N+<v~krShoq^@pV&^3R|j&aZ?jwXJV@(^22Vd_5`U5(QuX
zmgTnms??cpWxtWHm-buGtF8S$T)w_7z1q#!A6A}0FCovYH|!y0I$dwLx=p>ITqC-C
zgj3{BgvbfA^Abv+SVu1weY1Vw(aGGmF*|jXH>?j!c|*NcyYgn&YhisTZD$@xcI&WS
z>pPOKD<NNkERUdIt3$G2Ip`>?v9wnvasK}+Uo_KSUecbIp8^@PyuxMTcSl~~bjSKF
zym^OpP^N!am3#{9foF&R<Y|?(2)lS>PqaxMJb}ZH(y?5&5c4<+o`{qg$e}V+TWT_E
zl2t^Fl7DQ@GmtHhA(D$>3(g0#XJ8%v9eN(sJdZhi*4tbk9l%P{cpof+&Jj=HH{h3s
z;S**?r1P*se5E1k7ANOLG#nNp;s-1;h`Qk}IBUuBOA+obSV6zkB+7C4G1D@9$Mk8|
zNwAcvMY(e<q%`>r%9lv{S;M&&H^P^E09A69f4LDi5q_Kg<p2O8jaG`vtN#-;!pgyN
z)>R*tJ~&>0^nqW1of9;YWv?cUP*<Hvqo=ZIv=MbnM0|~?W%@55KK%&7A?r(z5^qsY
zF7cM-l1GC#E6$Q1ph!xnN9+`+hb0sfRi{w2;)yVIFQ73@U8d$BjKoSbicWC+O^c9c
zmR@2S<kTL+ng;Es;@~L`mV~Z6$SXWU845OR*Fo-|gZ4ZJ?b$NB!meV8_pW8XW$`}i
z!e=dk@=zj5JCX<(QyH%^+W)q4@wkeUa9VKiJHulfe*+b|gk*XJ)lPy0&*H~_jRZG#
zOakWpzH7VD7WYeu`|BvP61cyJA8l|Kli+{FaAL-hw@69@<`2SX>cz28;N#h|aL^XH
zY*jIt{0HyZ9CMOBI3l>@+t`JFNk0Vgjl1o79P2!*;*5H*AD2ppCX2yF4Hb%G9cHz0
z>}YwAGb_U69<zBn?HO&=h5B_!VHjoFK~X`3?Hbrol!NUYy-+?>=d1@Sll5TSe}H<h
z4`n^rQz0>QRy`O!P6*$xz=E~RTZE(Q+t!3#nOPHds#y}olMoy=Va~c9)bq&wV^642
z?|jY<_dD_yI4oGs6**53bzw5@#s0DO^A_Vo-lF6%d5d=H!f@XEP)y(_bzeNhX`r}2
zil=qAbcm|lC?`soIzScnR@x#P&qW>L*#~ptqs4P6*e$avZSynIO=sgN7}p7!*C95J
zV!jTr5&DsZ&v(POf4!d1Jy_}n^DMHSZ%0rFaCXWEDFQp?gWqkHM8?a|lQW<vX?x#J
zJs<T3^?bJ8tW2)N%-QOl$pO1}L=U}g1wghcw%5P8;oh$2Yui7Q0}?)wf5@umgFokd
zjrC~zP|w#U|8Vs4wRGM=jI#j4n(?H$|0sJLYyZxX^BV(2U&wDDe{v!i;ON%@|EGt6
zU&di#zG-hiiUgeV&DTk$!`FjZ=T+f@clG7RWt>$d>=Mqa+JheK{E8ae<w>nbUt8x_
zy=0yrWquDyd?_#2==WpH0un>S2j2LNl@jwjfi3p;I`DnBpLRYS$I9Tukoc&4HJ3$K
zvFbv6)^S*Ys-0MV0$Pkm3zXBqYz`@4Hz9mCY)F56QtCrh?{y&`YB+DB5zj{#7=Oos
zD%tQEsRtbJ85a;Net)lDACo{fei9$UhR?)Dd=D_!xbl3w$nO9q%x?nX15dezD(@Cr
znDVnvxyHHTZfD#qWMHN0B}O9D87a-29DFj+il5|oTWt7%?FLsiJwS{8paqJT3gmaD
z))fo77O=C}tO2faWC(MMI@wg}Pb$sfQ=n-S{s^RcAEZ*)!X{OJ<1QQw)2@6NN!FE-
zU&P;d={%EO-<<vJ=+%#KwxL(PyqiU@PU7yrK(B_JBhu^7L?AJwfE2Om<pP~esmi36
z2jU^<wHZA->2(zz91XpGc&<sWH_ti}z4-l5e4B#brT8K^rc_&klR4jrXv_Hx$imt+
z{6eXx3JID@@j-1Iy=m@G!Z{fY#D-O7_;QFmulqn?UBza6{EeS*#eZ8A^D9?4tgzIF
z^LX<3{T{EZS`X~Thp!G8pA_O7rur2=%$V~y1II^V7<J@p8<eM4Aole+59Hv%;GDl5
z$GM7L<tx*FnNt>BRfYqXS#ysK^I!3FP|oJDsxeHT^f+?$^*En;vn}tJL_@VXwQB~)
zy=W~yS?_(b>WBE$Q!_pp6wzAXAi5x%CT5otv8mG9{J0+nynjez845I2|DCHF$b5T3
zfe))6<f#Ey<I_kyDLubvA401<L`mXkWz=?td0gol9NY6yPSr?4=@Ek=^c8jlD;6h2
zw5=Ff`V)>1nj(KU5!*-cRe=d5`%xNLvL8j36(0nJlRF6Lf`PRaP4syH#S_4g)xT9^
z_#kplc{lES&o|C1f(*o#6k4Bo3|`3(R^U9QT?NUm@GGW08f$&+Y>OgH4UP}Q7v_|w
zhk30pl!0Y=Cd=gK&jLT>;TxBG@olQ2HR6z2)H%Lcb%Kl|6K**_#8jP$i(TPpihGwY
zr5YCP!|X5bB^<x^5@vsmvx_78XzZ4_#BqKRzMpmv=XCDXob03bm7I0D_Cx|WC8(!+
zSq1jfixaFWU*L&`_~2JN{^jp8Ii>kN{;3O&Nh3IRwD@@5SBJ&!@-rDaM7?VCdZ35z
zQYlY=Q7aHc%^Bt++4vE;5kL;>Xk4GfeCOfe(ej7gfxWZ)m1+1;&1lHxkV33g;^>4D
z?A<W&YaxCS*K&MEOd|FX(-+*H{VF%0TlQ1fXNeCO+FB=2;Kw+GehEzAxy+BrwPDr5
zGZ6dmTUxiE)Hk6#J=&`x+hvT0eK7r4y8qN;gioHGB@dCL%ZOEW^|;C>zTO|>8BbHD
z^zH?RegL!7$X@j!UX)BE1`8Jxi3ju5xecOhS(}!x4fbk-eK;wyAfn;mis1#?ZJ4P`
zp4tA69LYqdBU~E&ZE(?=1v<9_ED2x_31IbC0ze2T;8V5h@&O=B22ymlS^#R(JZbn;
z?Lxw50q2Z2q<>s*f8VDg`V1g_?m|oe!f;S5$Yc1#VA3te97)feevp6-&&TL$Tm?^X
za`x>r0h*e-jPPlL3rH_CDKvpOdiwnF^p5B=6=1zC0Q?l5SOO@%ze~c9TWJ8_Iq-uV
ze%$WpfMb4w_Jr$WrajU2;%HNxO$(W;&QJD$b#F&MZ>zWC8+`2*C8**ZETKby2Q?(u
z!@y+5CyVX&YIG!sJ1k$@ivbGt;@gu?qIGt>mF^$y0m?96y8Ug3Z_nLLDBPxziH4pz
zq!&NKXr6GLwKRD#-{pU{{7U&)Y@eU1&i&+eaxyy1gG0VxMQ|K6zNU^33i_w=+t8z!
zASjH)L;kDf(Q|x^58{UW6U5P|Y;(1$jM!a^FOv5R`BmPSif<F*D#AUod*D9&GtmOy
z<-taAd=i{D@F3#9IgDlDctpMfK`Qs(tSTeL5mPf&j5tBX!%=((;oA6oHGcl+3$W-P
zzi0&iXnB5!hH)&8{)7*nyM$;ondj|1Vt@bb=<%EIU&rt2Hu&B0V<PsYh1-u#bK)la
zmE_-pY5cT3V4H5rZ@>2A&A;CaQ=Gz9C7ee%Cair9qrfk`e$_f^raqc}QPrB^)M0Xs
zPY{8@MQBasxVg)C2$%f$F$}!)_tO4~{+>UY{(hg?U)d4+Lwv{J?rX7U5ub?HYy5S>
z)I|KS%Mm}n+ZBO^6k-BKF&v=OT#Iamwwi%hr61p1u)?qeUX$@2!Z4(0@LA@N->ocU
z#SJpSIKDWdPw|Y150cRWGR0!v#ZEn8y|jQoN?3IgvBM)IFCdDO{GOtnjQUDAVnI0S
zu0fJmthEf#%jdiC9@GG8qxX*C`;Q5&7?K+D_b0su_V&*%#{NS0?0#yrneQV_{TaK^
zy$diZqC9*_dYMvlFQCLxN7X1(z&n*V)DmO!H1vFAfZfVnWC8Z#g(ZL};DJN0$N7LF
zjtj?c+aJ;1L|h!w%>DU_&xL3ZE3aH?MJ2&E{C*xkt6Ap4=Q&#{;NO=_$OyiKyursO
zRSad>Hw>;SHB5bD@abW_tiPavDA=H(0CBJj$qTI?ln9?yU#DsZBHH?pw$7Yii7!JY
z<a#;24J`vl+??obh=}l!2b4|3s)-dx2KH93<ca=euFvqj$Y5Yi#a{svsFLJHVgRuR
zRO`<`6$B8cQ1-_zD3mhGe7mZBgIG1BEw{!;td@g-*&Mnn%PJzSZye*TsNNJrgEH4%
zG2ZHmSIzN)7|)mll}t#H6doEx!{<Dgp4%zMI)*2BRq*9%6p@H8dfq`<(-O%qPKC!I
z|1nL;j~#R=HJ$N#>|iIQmb8QlqZDTi&n9OyN{7K~eEu{NALYgZNMl*N_dR@H_l0y7
z!RU(F7oyMwroRyp=$yctRaHTJ)t0lH;90A~u7=bJQeTlIz*+o}x<i3IvoBY*796iv
zk>ZL<Szx1>K1^6$el?~B?$qPP8~_Pq4iq`QP5KygE8{cI=b#aS{@kYiFjX=8d*=9n
z!T-QW7ouA{^)`yWC&DL8SY7eRUgf9Qa@wzc7SG0PG^WDKrc8SxF|P+7pEa?r2iEA~
zlxh7s;al1Q`xas^@#zr%p-001&8pKK_^%LvPG|s}N)2Yx*pdz!JX!}lwh|A41UK8H
z*0tw9lf@7Q!B2e{at!t57ceYPqwAo)h%Cm}4-DPyo_!2@@&$_Ll^=uf*px@vH$TcM
z=O6sOcGpt`;pc#)_NTtFB{oBZjAwgG!e93zlkg6I&|>EwKtvlJNJm?E@{a<Z85jzJ
zup1-U@bu2jg69V&JR89$AqXA7GXwDKE1wO5c7M2!+`IUCUy*dhClvavzGXrLArE2$
z%r>Lry*%7@s2V+i!i~@9)YaBkpnR#iuxNP-6E@68@B!L0F)5pmiAkLrXs9?97M9;Q
zgIN<N&Lb8K;smK>mG5wCTHRs|`h<xmsQl>30;}&iv0C8H!YYDBCN^{MrkQ`6lcb19
zemn4a>l}&CF5m@OIRZY|2=<-gv-*1ipW&dt&81Va43ANXMu*3z3rw`ySv6FxD~}<G
z-ReOuv1fS~e(<^^CwVN`?D!rxD@oa~1Pi1%gHOxrYJS-~G0aPP;X~&>^RM)#iD3oi
zwU~2dfA5;!ULH>{9{H57D88?&cp(z?*BQGoML=khC@B$Lz|MaB1H^x`F>WdT=3cuU
zms;rnB=ZjK<dd88jmr@a;@bb|4|2k0UZ?)F;qgI_vx{(|8GLLNnNw{nUZ}g7lbKlk
z1ayPRmk-AZhm~PptJ>HBTB%PTlBbpC8T(Hl7^Rpz=Fx>R=%>-fp4Q!LY^=v~jPMcf
zp<RKxn7^1$3I~>m7B8X&j$FXQ{`^?_Yy=Uv<QL(C>GFH&4{&ZB4!m8x2OnY2t6e@5
zeiYs4vkKT|4*Sx*4+P~$+Vj{L0k#@gHyhQl+p11OXZ^Vok$GNdf_oh&=wiOkK6k3I
zS%o>}Q;zse_nrV!9}O;)B9T^g5_(0|hy0#9!4ks;R--?BKVD{Mb9pu}P&O>z*Hot_
zN(G-&8*nZnSd{k>S+onC;+@>rKEahpOqUKYw)P@fHyeQ;2;XSg$J}GkC^3xlKm75?
zKkdtwodAc_6|kX5{3eD82vqGv=)Xh0Ftet7<-cBnmO{SnLdu2tO0C(LjGy=-PFs<o
zL`bP2Cxw)FwNkI~VV)2Q$cVRHp=iQ$hfw6V6^cjELJCD8c{tk#iu)`+3c6>=h1Zk|
z$PO}7ZRNt-o?L)~hm(tfobnrwC>IBgr(A$~r3H`+95R245KGA~TR!CQwtRG+Y9vbA
zhzIg{iP8d)KS#<3W*D2$JEh<}=q>cY%3p+R|Jo(29gK7yCGFF6Pgpxw8;>b6W}(_h
zZ$!hUcMYD`7J%`E!Ng!8HNt*D=w43R@}$l1Xqe<`Bk>ZC<+m14(ytHekM0U1lpUc@
z$Tw^6BITS|O2ZKxTc*8%Db%Y(At$H)w6WS7W*QP~=@LSekKQxPBfL>atM+J@AVFyg
zsl$M5flz&etl=)>-s4DzA&4P%;%pUV!Cv$aL-RL|V@&76_@JG<KqH^=bKZ!K_Tt<y
zZ3wKzE%}Tce~-tkgAu_bEWs)L&Oi*MeNm<*i}oSk$Kn^$o@f<0NVI{U<1k6Umr>3%
z?=v$Pi7v(0T@j-X*76p>*aSv<#DVwqRcE177o$f;M)cm(-U<aamajk~yn%Mr?_2!<
zIA(uwY>69d%*Z@%tkzY%b5JY3-&)!+18DOz_afmN(Y`dap73j&1P}2gAsp~Nj{HRs
zU$M>I4!a-J_K_RS!7)K?m6ehsL*5bAU95|NHN*PHOX1H>gzlR*MSW1a#iQXg<9MF1
z;x=0X%dEVlGW4L)8Bl;RINlXJ`TuBp7x*ZP>;FH2M56|GsZe7@O>NM4qoiJ%6wqv0
zU{e!~1qCV!6>G8j{Yo_ftdM}aOL)32#CmUSrLC>nTD4Uxiek71yimj&UMi?mo;84Y
zL%fmS`!n<Gv(E-fzrXMQ_viH@`^?OlnKNh3oH=vm%#5C!<X^{8S%}HoP&-WD#A;q-
zV_=v2eAx6~I19^WLb}Lym}epM0RJSwJ-Y5TikHgYF!=>KOnrCdH+mItp{)AMbe`qM
zV81Qhqu<v0{k9;`Z>#^;{nlaoO`X;`yjuO`OM14)c4#&ZY>!PvRbUjD0g*olayxPL
zhXLd5EWr-eWZN5)?S6K@6H#z}<}SM3v(M>Ti*gBWbnIT|<1vk)-?P8wdL!GD^q{&f
z7V-qva0#rZjJ(|G^QC9c$3Q=8fbNOUo==8_i8=ti^7>fK`!lyYI0Dg4)_ng#>nabw
z|Ht|d++4*J`Txg!hnA%IE}Wb1?vOHQ=evZ2dT_q;COLnCBXO_uT2~)tts2}@M}$Ld
z2g4Fs-O7EAl;Di#b_mpHJ(Vp-<|a5JS9{EFsWUab4GgMAf%)YY^pEWPGHaW3p2IO%
zYi?_ozJseq4}GV+fNQhi*4X6XzPbM2vddwh`TuNwt7l4ET`B=4ou@4GNp>E~&U=_Q
zm&>yDHb9#9hA<!k@+~|6<<^(~pXa~l-tzhDe>eaABQXC(oEIZw8k}d&jXeQ<`PXsO
zf0zIx2;iu5#olK?Zo<|K$d>1O?)3+WBIw|gx7F2b3$^W_boj~6fK4P4$j*Re?qoqv
z9$k{10WS+{29y}pSpa_qJn=|L(XIyPO$pKUIs|3sb?0RIJU9Oxo0ShJSImF1lLaTh
zQf~s3T_Aq~WM^}I(;BJdWT_cl+G_9L7HTqEu}M}`SD1;`tZ%&zc^7e-Zt|38gr1Pa
zgp!r1Y<Qaa@4)Q*SC&CV-pl;gy-D-mORSunk8`t~`i1Rq?q2o<q@4gq?qvduI@ABt
z3|I&rbc|*|e^&Dc5uNlsZw9RMCbs7eF#YGxmYYIA1ZTi3;N1e;qw8*l8fFG`y3|XW
z0<RdL&f8>q&g-8@px-qA<@ej#T))`~(DvKvmj7kHxu)Mtr|oV2+t_3N(}Lzd&3{td
z{?q*T2rGksJOBM>ffg%~Ktufd^Iy2?-_C#j{TzFr|JVW5_H|(XD}Ln-vxs9i5Jk7R
z%Z9?JT6(X~$}jwZ{P9$LPagc@0DR_+{rA2<<#u)E^1uJjqpiPFyu;L)F5dXG2#;Qn
zE>1jSJ}bfG0F|7cTH=;TG~g%Y*EGBm%^}slRuR6wQt5Uu2WIiTC0if+gIzz*?Uzx`
zPxq*QVy^z8T>TDkrv6(QIOKayVP~UV?}9>R$9{_M|EP)2I$%}0HK2iVXXm>tK5YSf
zGJ5AI8%`D1W${V3V(elG<BZ*vXUR~vY<*pok7ctBo5-P%8CuPIQE}*8#it1c>mXKb
zNHb6o={}bj2o}V;QjIPhUG4q~hTUU+%K1CVzb*6QJpAsVYBMM73%~jxe$z#mbg}z_
z@Y`rnyg>k26f+MO8`8BXGTrA%1DJ>AN2u$vS9$cy*+YBaC;uT?oFZlyEWULH4{as1
zQ=~04=gau+LS7*!$ZP7Mc~2stogL5ZiqxzKE%=)W+!p!d3nrRmllGi;G)&%F^;-0}
zO}J<F!n1jSE`YUqdVTtGZG^B#B#mHaZ$@NuiKKg%K1Y<u51I>_@b}q_Q#t!SytrFV
zWFk&A@m*{XylXzXL&;yM;x|;x0vaippAzG)hiEywA2FE(e<&=zVuszai(FvVRA#=5
zrOpjIMZe^UsYOK~Ipe@+&8(8pf@jt8X0-g5T4n)K28gpGq9M=PO$}rD7OmM4nvdgO
z_XuZ8#CbL1yb&qfXiF)M*6ePbN$%D=qU;Noy2C&U%kd@sd*y7B7g2}VLZG?yZ>Q_h
znR<)1)I2h0>E-seIZq_#5&`;6`>B^*IqxGU=w`-&jm-z?!jP`g8zaoEhLn|=V+>Cg
zxPK-K7)8Gpz^G7!V%2LZQn}{=<SfrDHOks-Dl`QD#66O=8U#<iL<|I}xu7I8-#%iv
zq6o(>XE+($Ix_p%9z)So_u^qZ=&KqIC#P!os7N)40p1ltbmm+9X3jP>MzbA!A1fCY
zIybr>1MZi3G-Yy!r0GZZHBzXci3)sxqHKF`TsO7NHMN-sJTRAH&e-n8C6v;$*w5eT
zPUpMowT!Nyb5p^Z^wLje7||%H!tr!)l#+V$!NQp(`EvIJqPpH@FU!iS?07oY`FOOq
z*RBzD0R_nw#Q7ZNN$A}g@$`J%6G-yBRUlfkO5-)O;4!}ICvVpfQkAJfLmq9Imbfhh
zP}=y$&Q{X(4eUp3yJG!eqxVtqac9xZ`f*B)sjTM3mg8&IwJ@Ia99kAIReHTsiqQcN
z7kzn<tuIBKRgvSi^KgsZ5@sZJ#VCJx7_-Mh_8}fq$r>o587LFrNl-9jp^R@82i$K}
z2roIv!H&W-Tt?LFZn-+*ygR~qhXFI9@GV=;0g;+zq3f{66hvxP&Ae5^Wq^Ay!hp{F
z_E!R(c@eedG3TSW-D|iE2+CQ;xpjHS8|s&|H%xw~Zt3P;;Mw~1%#9S~&l{u5G_Jqv
zlpYPT8q_T_qRvjU1AW{oXGft6C1SUxirPZ+y2%&A)y>fPyH3&3YPs{lamxzd1$kcd
z#u`2IFK#1<f&JBeEwB6Rs1chTTB=j^xgXM%mVmAE<oOr&JpaOOjnqN25pMi|k0*#r
zf&^%}1SlN)p8N3b-6oRp^Cx`{Hv;ql*hvBulk{+3fUc)@BS32m*C9aPCC1sVV$NdG
zB>~z&jC(y8`8ZDjIP*Nw9=+VF#E0qP+r#_nnj-<qe8d=LaN6<U4zc+z4(7{>uq;vB
zcM1$KGD~n|&&n~ipkHO?7USD)$WQqAjNW0zE-L&#ap$Ai`0{9FVa;Vhd9*F$2o?8H
z3isQkse)#VK&%?38bA{zk1htt%A+3$VQBMxwdF2TTQp0rBTaqCqaOv!{4dJ%<<Xg>
zP{HPY0XpfqlROsQ*}YrxwT{wb>1&KU`cR@JeT|VvTlmh(qt5~Lv7fA78J3er_feVT
z(S8UpPage>sGvL=V?Ex|msPq81)VDXq0tt&xLOYJz1{C{m$@o-tN75;i{<!A6?zVg
zrIq<TFkqdtLZ7Fran8TGHu>^SJBi7iGa83XFPU{_a(z>)+02(qN&GYQleIgzr_u_j
zvi6VwRc->vP$es+H>pBxp_9+;ZFAfn%rQHg1F2zp?Y?XdpWkBXQH@n=oDX8oDmM%n
z#U1mMt;3YuPI{&DoK4?>sAqQB?A}5#K8<zjYgbj4e+~Qxk3x4UT;)Dtv;5V)IGE*n
zQfb`|#FlB0cKXCrAs;>8-^@KBn6XWDtqj_8&mHZ2=+U$UnxcZuJjMOwOGCZ(pjgyi
zULH^V4j{XF7X4!B>6Ol_?gePNY#G<Z(#;~vtLns05wbM<d3PZ>{ARDTZ8$O8hQquz
zoEa>(w!pnCm}!~tUlYV%dzYpKVYnjOg73Jel1u%MX!L88bjfSMCUIhXMc6Q5R!*m<
zH%6QnXv!~8h-k`LG^H}?ylLBVl5NWx_XYC$B}?t3wn?5KUXpD~Yp?<<XWU-}Grp|$
z<n@Qw7uDi#-V5R?70JuFz{xc*-S2@^ulTO+P+}%ANZIByI&~;a9(cwVq}?4t`QY+G
zQ2uQJIIrdJeC~06yL*s@zcUEGJq!Oe)eB?&-G}FV%_`{)oxQ}OSuW45*UVlrcF#Fm
z<w$dJELAj6yMZw$QL)-&Kr2y;;sm0pA5ThMQ7y+q93ER6-4J6?@<}meZXuzl{7%yo
zG!qs75ELI>NWRBZ^%8v{Hk`{Vm>Vl9syf^qbdCf`j6q|j&RDvC6Pa+zb}Ni5!qJ_$
zONm1x>r0fJ)~+*{T^u{P)S%j2%FHh04O_U9Z-M=HoEq}>LjwN2#WVhqT{7+Oe)Oqn
zN~3#?NJpF8D=iHfVjIuDAak*ouD#E3S&*~!K?6o9k&$zU8se<%>e8Qo8vi9N?A#wS
zRj8QEk-K*H@GlLF=TjKW?&TDfe2r5{sG-gq?q7Ban^}!~{!X7Tjq?Uofl)66#U<_T
z=VXA`Hy#`qIhR5W!#`Qc`9~pWEN6T86UduDx~TJceR?up$y>1ps9RIT<JTU|W*7(W
z>^$%->aR~<X4H#g_;xUKZFC>ECF7_cnGm@kJ2jj}J4KNB(${3!UFm+~GqO8d+{5|p
zdQAj+Uwp}N6ijl{D!m1;MPPr!SIv#|L!<kd6_O?LwOih9ydTJuhwya6EX8*}n}_h6
zk8=n&^BuDo|3tXO<)$9QIDz3=7{4wZ&0_phQ5K9Z<=4abl6K*1aFq*v%_mKVyN{Wi
zh8W!<hOMQ@t0g9VbNQa*Ext9(OVu*LYMNtpUA4_ycMcDyitg5slT{MMEq$&w1q*A|
zwZ6^i*q``tqr>r`e`4J$VU~qiHggLwBdfOM<~!6<;Ch7;((|%#>skeOWYspKzf)h_
z?TvJr_H}LkQQ45;B{Tm8k$7g^jH(>|QRgk&z?11mW{I`#&uG6dS>Iu(cOB3C;=>(e
z;6jOSA@$s|g0QQBg>zp$sK>1MsbzF4=KXS(LtDgqB^1f90g<)u`02m5=>wE5E3o?(
z#rSxw7hb<`*9PNWCoXdhpuzd6YG-Erhr8u_X!X-2q<3dV5@qLuC1fhdoaTo=>~TH?
zGi?7d&0_mY2a!?sD+-O$l2gIr<s}LlTQ<QyMJBb>ruH3M>A3@Sj_L<I8Tw8f+&N0i
zOfn7d${5&*K;&r(ZUdP`Y5bnLk{EBhoZztY_EWIJ77^}$zf#*~6I5#eIN}e)Q239l
z`Ex}rf2L33&!`xG&TiD7ar|kW!k@8I`7?0_f7+xcb+)yuRQ^%Sz)MUq;gh`IQ}x@~
zwvbe>L2bA4-PzW`U;XQBTftv1?XJ5`jHVgIbhbT{OS}I8zNzyO*_M!5zJ8REeh~<3
zEV^T*a}4iiN*S(KN}x_>d?CN;g!6;>4g9gdgMRUoA<!LO1Pni>OFD-e&l$2058L9T
zs|l0gm7T-27yz=uq{L?{#J|qCNMhp_DuREi(~RfLkbHclFK`z<kN@g~_`HE+OHD7=
z8ecbh$`cYdfl-yAxE{6##|0u1QA_b6cqJ77k_IVOWc6aX5xJ9CWg8rd-);fUuJ%%&
z3&nZc#H2Qc;-`C`<3e%tW)nM!jU!X@pixsoZO_`zu~V=73jTrpkAaWOu>}YqMhN%U
zJVViAXG}X#aWhOR5r#QJaV>RH3q+6Pn$WyawzL(YHr-2-x@Do_*N5hPVzFONefFzO
z-93G>%29_;zY1a0)Z%F?hK>-40YF;81BFn_S#K7RV>>*i&!Pe4u3beEsGf9baSr(_
zDl_*h2fbpe0N~UZ0OB!KJ}pelu+wbJlS0D%?FX7aioc=(TV(n?^5}ol4l}dP0YaMA
z>+hM_L~D}SM5%L5+EJ9N_<r?YhCNG9e%R3=NP}{QdCAyeRr{hqz9-egnKh=B@>9!=
zJDcwhxqPS24&-}ep7LGo=fhiLO0KrA2jeH@;&-#qwEQak2M5Z#VqE4Wo0t%|+BYm5
zNN;V-{JBSZ{~f(^?H^m6ncXA()OCUM>4P&3J<<n05J<nv(BUWx0v)LTi1tAG9frP{
zFS7Wf&7ekc=4COgD+^mAJ(j1|#?*x!?(>?f)59h+`_!lJtXN3f+$V`k%|hvU$Gwx8
zg3f<}QrHXKLyg@+2~{cq{lKv$9Y#s+EK@>}O87tuPrCT``zYZ(qb6-J`eSMq=I(dg
z$8D~g_L1vRFIQhv7cO1zxN~i;wteJ^d%0fY7vDMMYP7ko-bbzmFV`PTu71k(HJj_Q
zedIdS%XNjxRi<2<K^AIUu#a4C8bxZ0TSKmBs$l@&n#Za-_N2c1um{`6!fLtkePH*5
zq@NrQR^_K=RS28&EH<O|fz4<y*Pr#99ya6vdiM^#VZafb?<ziduhp5s=^pCky~2a%
zghlZQ`+(mfjR#IT!-MB`oZ@fn1O5>YKIFmI0)O~E;M+X-jr<D#81RSe1AeRrf4>Lc
z2>e0&fIrrQZ}s4H#H05<;6E|i_!jqU<x3CSh_2)g_wr66?|X>hJ-gD<cCnZD5D$I|
z@LTo)pYq_}=2vZ>3jD@>z@P8IKj^{F0RH8Dz@Om3$N5b?F&i-=1{;B8k&LUz`0vEW
z2Y{&4zh^5lRw%M8-bY<edRY!7OEi_%G)s=Z?llLD-|m6C{L=KKL@Dt1>=oWp_X01=
z!~9aiLYw1`z2=~XK_2*AuLcQYYPe;u@V17}QJvN7(Owo!zGS&>A6Zs=S<1aEnw`lq
zhb+dlU|7YoFKn^3+~j51rr-3inSi=)@-6$pjTM>#upjix*$*^JSo^{GJM#3))P7KI
zWTmrzYGjc!s)$?C*tbuOEOkbeazqYq+SJH?&ZvH=k!3i@rbZ5MMh!@fEN8DhHL?QT
zVw4;yMh@23U}qFD;gM3gNAV{-vQ}UG36GTCE#*5rvQc0B36GQpK5ATQ?rd7C@Fa!#
z6CNqGd=!7eBd6+%KjD!x9HZZ-=87p4i^d+FJCA^4G<Kqlik6zY(3Uq>+@M(fI&F$g
z&F$c?@~Iz{W`DzT)kzB5-|$@ZfYa8&-_+a{UYd<lC^dIAf1Nh{P0f{7?-<=)wwTNb
z|H`8;YiOzD+-LF?60}+lES6=bOtqT3QI5qR(eVWKnjzIqII|Vs?tO<czof%*>w})_
zF+elVAI)-%4rWPYmSNuVi**%oXmO*$%l-A<-T-6H^sonvTSW0~l;?cg8C&F>UDBhB
z+5+cL=j@^=JQB<lSg)k^%V#C--IFT_Y$OKaJU|V*hP#@MOnuckG<9APwQy|2IB=f_
z+}LpHyfSBOna-M6eGGgK_|NS#RxqPS8++fJtsOKxPb*UIX>b&ee*EZycy)JELo7Xx
zO{I$~!m>Ef54{<3zv;!pPfn(XY&J%s<z?6nVos3^dxz2`vYcGOZ~U5F-OWqwxKkzh
z<4$<kargFC@kP76uLM^u??@}Q;yGJq(0NhLo5wiZs_|_$IP=h-LTpvX@O1Qy-C9=N
zOfbHy^07*ae?osk^KT$Zik@n;0;8NSVR;WVuS%S#1e$r$_ZJ^sz=;j4woL<THZ=V-
zmToA+7hG<y=$|-Dl@U87RVZ(5fSR{Km$}Fne~qwJxM(kXe^bGGZsj|0cIu%DAxZO3
zF=EHr5K8Lh3XGR(p0!39{{t$^@pHrE*Un5AogbgMGF;gFSp4yd2LLW?egMaW(7Z$V
zu1|BMw&_%mC@tLEWK-`T^y;~^q|BYYUER{{ZqP3WPs<uuNhKD5rK->sw_FC!!C1;#
z%gPy0Lny9PTUipc;hH`^*()r?uP$8+&#oTPG&F)c0hT3P<%S(V5$=DH8tEKi23iJP
zjos<0ZJ8#sc$rPbSjpX(^KRxMVj}SoWrdmN`7-Eum<$PEVvsQCc+%cra?du~tU~I_
z{FIdzm0!nom!{E%JnlPyWYI7!&tQ@5cCUC>Jg0xYeZkuEwS8RPDh>2r)*2C0Z{*Q&
zklS=@5s1gnD0Fsk=v!udv!8P6AE`6y=#)K#WH0Fb%}_?k|EBzwljE&LDg@^t|6C}C
z_Al;H&OO<3jK5C0_%Kl7@U^jXL>`@*MXFfTq@c{BZ__GYKA8PC)1DdBSF)G-zSy0Q
zcTJD_?qSg9>YJCXZ@+APb$RurPVKvh0kogSPeskHCN>8~mT?;KV7tD=np{@%SyNdm
z%$Z@f0Gqet$kYl3%d0qYTnj(&@NIv|8IA+g(bua=*ingvqRXJN#vkWxG4uNJFLGXk
zvx<J2%!X~9^Qq*<vxaMRaQ_9<aWdH&e$jGpov==`L6IjuG*cm>uxC%`qSF{s2-{be
zsGr^MZIa<Jl$3uoY!T)h96$Xc^fbR^5gQ&5alXpZG1=YHFOoW?BDu4rG?Hq-3NoW8
zx&G?o-7#;$r0L?5K+_i#BUP&e#cV!gOz3`7W2xu<Mr~k!LoQ0Roqe|6w!qhe`^ZDH
z1-5zx-baC5e{tKmz{9go`2HuO>z${eM;YBIu`1ry^APwj+*=`Ss2+he$wb0xf{^r*
z;5AZMT&Dnjgoz5tF9ilGFxv}MD<Ec3O05FzULcl7`VwM!>La;^i;a@onu?K0%|CDl
zf;i;b9o*d=pKV=sS7RML6Jakpb$f+ML9`AqqBY8m%gK`tr0O{<)V_dh<$fgglnR=D
zWA>ZsoTuxYPb4QAVHURlk|sMO9~wi+A=t+vse?FYTcJGV>{QnhrSD3<Pw~D7^Id5`
ziz>2y#G67Ya$M;C4s-TVJEsLPotEi9=yb<IqjtG3{!=0tS<4Q4EX`#hH1^MIts2&~
zV$SKT(>{B&qR}uf(`tuHdtt6vYDm44aXxUbV5C}v$2E;4)?N&E7`^Ro-Wafc{}%{Q
z!2?ua_$|vG3jl(Ena6VSIFi~wcOu6$gF`(uO=qyYhO7%Vq|`Lza=cDFdA$vTc8t@D
zxt4h7%V%OYsPVjGMI`lYTEOWA{LPRaUK>KmuhLupw;25Vb&)cF!Bt>T=j^vv1ZL$S
zP?tL0Jt0tkVl9Ls($eucDWsriL(5IbBk|t}xO3hJkXR(m+?u1KJ2{wop;CWFYP)+T
z@$^N7?EYD+;$)KZ1i?<Is*d@(Z}-X}@|_?eCkc^VK>KA)Q&l0Vvhw!-mYPFfv()5L
zE2{|%!lbXk(%6HZb@VYjc_O+^ornZ@qt~_W%Z%P+2My3z8rO7=^aI33wWZ|MaIH_|
z%Qe~fM8433JR+Y%z#R~5+dQ@HWM~pe^+qJm?-IKh_I!*K{&^$s-*W4Puc|iPjN-L!
zJUalUb!N<YT1BC0DvdiE>Y65~0p)=vveRGfzHcT5+r-HpgxAEIVXFK_Cy4PIJ>4W{
zIqOmr@)|vXfO{+~?RTi0(lV!G;8a&?Pmq6JJ866GU|aJ<%$2VQhcUWi#P(z}ru$q^
zdTs4x8ctg$Lv6pUh;`GQ-;SgXQB(KJH8qqt68Wpf0pvCJE(B*j0dDeQa=jiW#wWlu
zlbj{M6ch3Ya6SR|7`V_Uz;9@9X4vOQPSXpWh5{CW-1p#^_l<n6MkebTgFJ$HClL+1
z7`Z0G<zIYPyTNitb(Ugd`|OK5s!YFeqfUS^xYnZD-M|p{MOtrB3#4j6N?pmN1{IOs
zy>mSYM*Dln{TsaQe%azxo5kzt(tY7|8x-_=vQ}f#@6Fz(H>-pA9SDB9*)V1*=d4&L
zq1P7bCI^@;dy2CMy~5ySP%>PFP&)&>>YSpA9-;#7DmGngdCG8HU215Bdq0Dlah&)L
z6>23m5jIm<G!(fZX<=s4usf0j#TgGv!*pK4N!f%*d`LmdY2pgB5RWaxrnn%Z?q<Tv
zXVk4;OpZ}sCB|peXH9aJQJ*v+k5T_dz?}ul_>B6R81+WlvR9#-H&XNF(A6&rt1%Va
z%40|BiwEv&q(17kd7RhUypcKq(UIl7#Cu4Me3E=g*DEGBcDuY7#G6O9SG<@UNjiz~
zN%ELU&XVL_6Y@xM2LbmdaD`8jyG4>Yyi}oXtrm-g-K&8zor!SaKv|DYJKq!-#IVkb
z$zk|yFD8fKKw^9h%S>_>!vjso!|+Qu&RzXd0K><H;l`b;27d_YEcY2S*8^&?HjD=|
z9_TJ1&O@CEs3+<RO^zVy^SziH>NCBV9O^$I#z*}Elbl8UY!mWO|1JTyPZ0H|g?a_3
z2iC9X_s%Dg^LP$2a_$J-3WjssxiNbo!|2#za`2C{)^oO27NhIbaBVL(6+Y4Zn>aA|
zvC?&i-HdwYx(ZFW=&N6{qMeEZ_T|_HK|E1$HyN-8-d-_V09h%VsCXb7zau|h6$HRK
z@`J1MgX{8x8*)MSH*EV#B_0=bc14|wD=v=~ZVFC-8-%Pm9NFYSatq(eoa$=Q-Em68
z-5r1KcUgAnj!FDi-LEsC85)-(kUJwp2UK30uiWHgSp-H==QBU+O7|Uj4BN~+67a~W
z<;UK~c%k9F{&(DcbU|nBU}#cc?y-cSrp8iS!<f1fiB^XN<P^N-V&%@b5@%egGmce(
zT?7IIy}w^x?@Z!XfMZJFrTzy~U@T3VL%yGz_1BzbTExbj#`2gGt8_OT*6Dw)sku@$
zv+|~bt<Ja|_0E)CCik3WgVCU3j&A3@oNSvMT^o_8FpFew9SWVMn&cD@YUqmTLant5
z#03I-RAzwAS{s*6RD?}|95uofjCG%TuNhPiRotpxOfOclI}aw#exJAwqCZqouH_WV
z@00#Pa7;y$9HZtiGf_)BIg=e6jWnzox-n59Ia{?2kA6^<fYFt1myf9GjxF8PTX72&
z2QG#Q3E%34x9XR0yB8MH3P0e5<-?({%#_N%%Y*OWH}gjlW#^$)W(cSdwsz{_n3&e=
z>jVtyjk5HauFym#aQ0edS#wUTaMwNupX@q=I$j4nlq|-=AWj@-<c@ok2t#m^4bDaj
zlCUOtokfzp3xgb0bWniii&#B05=W}4a)FQ2U6IwgcXF^yQHXkG2%$EEqAv%lsiqOa
z3e<4Ntj8VGy<x2?%MR`1ud5KX@Rlf<R>Vk$RdEQV79@Zr3`qqhi3Y$5*(A|0KS|@)
z<Py2_lOz^Q(hgf!pg>6y9TbW6zTNh5?|Um20u>#CoWeDwB#V?}31R;CMZ;>K4&gHo
zP)feBVm2Rj&g*r~w))<8R!jxBo|0zpTgMTB-VasG<}+5ff$TSW48w|>eTZ0ZOdh0R
zXcf6s=t6>Yona$_VyjT1i{;4d#|=-?q5mW#a6>Q4BJYdzum~qJxSqnBVBU;5MHTKh
znfL?jka(Zm(uXoCCH1(+Q~P%}QrzPnQxSsyxRKf}rcN}v;Y_P76}v|ebX^9KKLcM0
zMPL{48yebQFsx~%DQna9fH6$Prf#!D+ng1ZYN5Hl(mM;+u6C<0J_sFOU%~6O7AZ(e
z%!S;a-Pe%Yc0w?2pGZ$Pd=Ao#VeHt688XIRM5H-lW$b+BQY-gJtzjIl0D$>u0NWGy
z{vJ^psU<*jt}bB&%0CrTb<BxWlU_l3EHw^-O~FTbDx?~XNINX%oY<Fcm>O}`InTMH
z7>2+OHn3LGKB*$@>CnD;heIPJG3T6W_eK;w6`&!cZe)INmQ&R2Pf-*{+syrwY?8!!
zk0bGR({-}7my(YsS(_MS@42n?x6!cg_{=K2|4l^Z7X;MnADSi3|ESkbGa94c>&Fsw
zKX^9K>r?3UOfgn~7?&?fH0fXFGMS&OSX%C0i{zEaeb;c1WiK7$NDZd}M?o+hiyihm
z$O(bjqN;W$J&pE*n!ZRm>=}>-qU1LF2ck^Bjg|`=+QmZZ6Ym%qi-FE147X%QCLbCd
zCPw2(V`hfntqhK8e#{EsDEH&0Jt+>@B&A4TD9<>hs3ZmO7q;aqKwn_A*8EmLs8*Ra
zd2rQyZ?N%|iWhy9rCb5SX9VD!gb}hz2`gYEF9CSVn=>?qpGIyP8D>S!iR+w07)T2#
zQWu~U$lhv{F>m9|Wl~fAMkGHf*C_~Lq%_aTNocbMqZO29Dz61ZH@IpecmgfRsaY{n
zh`ilS3b}X{8UP!YADomQoRS}$8VIVT{L_7Gt3qvaNcmoRfSG>9Fo)rl#;@p7>6V%R
zv`cB_<(ntG6(jEq-@idmoc35bzO<Sm7)sR+Cr*q4CKvIAurXRxIrX7M9nX)|qRI@n
z_aA?UuSKypS*}rLdcKDu#cT&fr<6Id5+_#b#OOP#L<JS7XMgh_6ewOia%EUn4fSa|
zC_#AeW72mR*m`P1yyY%t!%9>BV9>2gKWP-4yBLbu?=w4%g0l%=$%A^l6OkjQNWtkq
z>46n0$(gU<{EC9BUgMY^B%Ug!zLy9ECMocM7ciqD%dRt&(qU2(6|?nA*mA5L6Pa_t
zK<7ajPSP7D@!+xWHVx##ZRaf`MB(TI=u$f?-A*~7WTRnO6ba!!lcuTXP)WPp`&Zhw
zi}j^t%rIrzNmmt>tD;H-4jqCDWO`hDj^4^fiY&lR5&l?jxu;2tKnk%uGtG6T!THLE
zxYvv`H7Z|?5GR|Yjy;mZ)h0=F%Fm_2Vv<D9{3MMp?Pm8zPv`F8Q-b=2231bq_>#P8
zseCuaRMaN7F)VL1Jyf5T2w^z$(wX_dDe9Cm+E`5i%xCJ-P~yK3(P$6qa40>IkVr-c
z(bx6P`}NMdcoT`#r@%&mPDxw_s3wiclJU&S>c&_qs~m8JsGf)kF~dvVbN91?1q9uv
zDacn2*gTSQpngU<X#cFxR}Pv0(|N~k_Xo=<WX=i+t~}Mi+s~s#BA3w*PRU{dHHgXS
z2bO5TxE`4P$Y`HnYBYkTHg^mWnOV$N*>hg@QZV=S6=36C5GDbX`5Ch&a@Oh#+nLkq
zQ^VSR#pt$bgXrSx3zu>mUZyUsEATjzV)TWNerffE`qZSLzJN+BePOWG7Y>z>OBENW
zSX6O4WZv6{w(wQWt(x^UH#%m>c+*rWocS#ntLyGI)|r3Pbz@8>zw1U2bi>FFzw0m&
z6=Z6Iy|0}xGk1gh{%WO}wEwK`1B>2fx;H&KZ>gd-x;x+pJ733F%+~PhFidoN#X^2z
zWF&?8D=#BW&(OP3VA+W<rhrC|Qfgyq52jktcYD!;6+Kk|mHL{ZuW~ar8}UiV%yqYV
z;9;_gyvPN2&NF6aHCUKwHNJH=-a+P{zh562+u(5Dl6NVpImot){PpRFDjqVw_j;r8
zPAf#y_Zrlkhbpd9H8<MDA<J#m8f9FXvEXEoWC$h>^PoSeE&9i&My>wwsfOzxpQ=g!
z+$9J^x;FU^ofAtZjMh`1zRsiAM!l|CD?oiz>q7$5(#BIyq_Zdi;b`kO1XEcXh*h9W
zfpuO0p3SK&*aEV10Ob1snaUOJ%R|X_jdClM)HT}b)%5dMAxVKV6xpcx3Eu8d-GmqM
zOHeICKxF5isRE4wio_9B>QP~0Dr1TgYDrLFk^<FUV4MO{!<5p<w+PH<t>Q&=#Y-c*
z)y5Cz({znEhHuvcDOX^V7ce}NB}iB)S%NUhWC;Qd2ATeFR)!KQloPDgE-MW`FoW7#
zRvG!Y$%`>nnwYg-jH%beJnqGq%1z9lycmOliMiE_>G12CON@KYvcT}21@RHNM|^BW
zuFh1k|Iff=MK))}E^_u1wh7{TwqcN0(`u!dx(9eMxi%c=#cc9nb{XyKam!IA*E?QJ
zt_?4EF&#EnC~=9_XiJFAOyCRQZ}D-j0cRRA=A**ge&!?^XZACb%1y(LF%8Qcos;M8
zaGP&HFyE!wd@5V{ws(ji8QgUIc|csGKF%_%IzkL<Q1=*EKAxtd0X9O)u&ia{0OyvA
z^$Z_oIk#FRXE}GU0$I+j<lEz1!+h-qLskaF-^w>7mGEge7sgx3^vU3Z@y_%_w86&^
z`)bm}in&hvu@|GZ*ls-6i^+A!a4)9Ahp8gQ{o=_0Q_Y8dnLlDY$oVr_`Hrhcd@E#Q
zplU@?rdLF;#)GpF?bYUQt;kVnJ|UWhG2!x9S~TPfQ6p+OMHNv@fzESg8^`^E9%8m<
z2!9Z=LV87G(1kNKYD%8}Q)+BAHh(-g7pNiT5L=PV2t`~WXI>iHp1MViuIk{j|E9k2
zcZ>KvtmBh)@lD^<dTLMqPL`=-krZ4ro~o_w-qgdND|_CzjDyR`ZJr!Rw+`k!aTTO<
z-VNRVA_$Q=b+@V3*EjOtKckKDsgh#p#Zu)v;|~C>f3eQEI#U0f?KC95i?7c3>-^PH
zf#gm6=%0#^skSrzf{oxyJ+6OM>y@>@q^kM)*BLicBwy~$8jbN4Jf3#`1yCVMRPQBb
z+zov{Fz%iM){MI&_|W*d?+KfaT;6#!OOH8Q#f}%p0;j!g>r-uEaNwNfvGUlw&Ioz@
zYV1ktQ1U1(1?H?!W`jsA^cZ#+D7Xo96TbUx;Clw&90adMENs|S*wYgP=WjLyKhn|x
zTY}a@@oO4c@BI?yrGz0W?KBfRhS<w(Y_np^Z5<E1%y&OQEEbiELv7OmO5LCU?qSz{
zT8KrsUwf=@9o#^P$9(|&X6lpQRD7|BPr;wlP7<erpxa4ee6jk1gr{u6V&%F|Sf+=a
zt{Q)5fYdp~(~nEtpbu(0F<6`I!|7qye+YDwS6?bFg2p?z1)zBP(YUr9Vlot68!KGa
z{TA!7^sv6n0dCZTC%o<@!lw|<%m~Q6^sw6ymF@uIyI#$WZ{Yu``id04%#>FpxR212
zyZ&avZz_GW!oM@&N7b?y2xlhd<T2%3s`$qhKPoT2uj21iJg-anayUI~&JN<QQ~X_d
zc;2JFYEpc6Ui=!BGePlN^5P3sPE_$)|76Esdf1|!#Gk16QF-y#zejvO#eXv|{--ME
z3)J1rp?UF36~9^WReAB;;Zxu~qxgcn_&2u`|B&JrAWC!iT&?)q75~G$_`_6ALh*MH
z?|u!5nCqX^+10p^)oL`g#<~t~=lYZ8AxMHg55i}R`BE$QmbM&`;_hhtA>Sae_%}Au
z6N*b)-`3j<X543=OYGuGX~}`e*gA9Hwr+PeCMHy1UUuXOi6|1Ub5tYTSI~6M2xQ;<
z=NKlcy80Pa9o~;c@6u4)tA^i4mPFF$!t4FkbscK_Fyne^CS1GA0zE7sUcGTMf8$Gh
zo?PEKYK#gGB^L2xD!88yq5sL#g$0*HLWB07&NI2AawYeH|0tItYj;iWqw;oi^|9^f
z96pA!3XDhc6pE?Z#zjR!Hbq%oBYk~CZwC&zf)X}d`Q22i49Mh~=IUt8<ZA3G6u=Uw
zwzH^GWm)=g(Uk7g9#ONk=_+OJ$9wG2leFM}>Z-q`Za76HtZ$mi{ZveS;ologAT_$2
z?*V#N<-}gRRQ)>8UDN7q-Cp?lX+u@3-Ub|A8=)3zg}zuzO=ZnDc>NXc@kFD?PbM$#
z;Tmkgp&;{p2AA&VWvNxhUpxWaqZSz%AL|Ur_=|uwGTu~%jDPA8GXnz;y5!Ap9Zya_
z>m1%l2I$HGY1>`NAHeNCJ>_nBF{#ZHO%FDZw`yD0*Zp*hp(Y2ltT4hwv-5e5rn}~{
z>ejd27V1XI-?eIA2p$Je*D^XZxvjN4(`Ck**H3q=<$k%WP`z@GvE`n<&vKvpyUGnP
zVsZfmWR6$!bL+!%WssIL@E1&dI76o<R>wbwX=SR!F*r2;*WiE(VIB}N`bcyoOgz45
znNdiVW9wPhnu(y>X@1eV{R}d2J~@<}r-;{D{u{dYi9UxS-(l`op0H9T^THZNY$e(C
z%IJ7o%v<81JIyKvjvDCFuhRxwJ~2;VGoC4HZ~0gFWxSy$fusvquU52y*`UFUw|o!l
zYryIXLyf@zr`x`N@Syw+@btyKG~&F%W(E{m<M95(yXJBOtVkeHd<e9U^`7Axqf%q?
zHR33t<wP}h>^d6DiwYLs=!>G)Qrb42Pra&8^Sq(j+uHcx%;<Zzc6avUhEpjfpAMU-
zDm1r(e$(Tx1Nc=XAN-@RAoDN7t7F$^t}$#=Taaqg#2eq$tEqq7Ez+G8eS~ciQbLMa
zdeU4Okm&~pDZCf>Mc<@^EZuVYsFa$$>7(ZCjL%OxUF==K6gS4+-pD_4I=>E!EtdC+
ze_j-7(}g@J2WK!7qKe#VR1w?H?wJpvuuIV^aBKA`lPBM&nC}vO2Nb>90{8G7eAvR9
zyhZ;GzKia)I|A+|eR}1;V7~V(Kfb$n({T6iq%w}*d&jOueIwIv(D#<$NNtqKb9g{3
zubFdotli^3_bOAb!EK7I*L+W~_3GPiSKfJuIh`V0rXN1Y>aWo{qric%P(m)9x{6*4
zqCRevbnXEzBT}*DHg=GWE%#!LowJWyW@9UD?BctXg+jt^_oFH#t_b~$r53B}Y}(s4
zO%eJ>T8Dm2zD}E_2>l~%hki`jpKY2V^bZZ)<3n?9EtIH%MxJ4oJ0%3-UA39ecY$}G
z4JX!H2XheaL+-*EEHM>L{db8QdVtM>;U#dnGomDvSSanA`xax|CC?)@Wk|6GqDDAd
zxx%owcIH~{C&RhyCGjqved8kWXXXjCfl%US%8jdw)&-|A5-Fja5|)#$ti%1R<$AFM
z15sj8HtQqwP&Vu5i%nMfz^^7+Sxfw^ti!IVRMsz*L$#u6_*pwlR?{Ek{v9c}>TKg8
zOK^YBU3MP@J4w@9sR#9g^;^g~Hk;|0lv#H<$p=UdC0ae)+iX_TnW+c$V{B)g?pZc#
z2Qh$Gd*I_NxME|e2leBD9|d^IEZvYPW?4t`5MIgEz&J@w04cM0Q}QMgr`1HtEaL>H
zIHm0}X&X%1I`gf+skqBu!6{vEk^>6s*GVeXUHliwkbY2uB9?x_{5Z)SN;!w2lzQ04
zV9&kRz-_W{`f<bJdvBF!?lwYCt6F2_4z=~EU1n9z)f{3gta)mBmwD+Zl&nU!#^SbL
zvhNXvl9MEQd4I<A2=y0arV|q=uYcH<*Uv96bENW^dBL3wQy@|jZa_6SA1w3Mrw0R+
zk+%bx?09fbu=(oACs1D$9ZKFMM}US{;wAN<;ooGf(ZyM3k0>`tPO@S5Z9u5MC|Lgq
zJ?ozjP-alDeg+dom8mEVs?19m>CL>-ne0#`|1z(bDlERxXOWFA$oztMWK&qf-3-#q
zdF1x!G1}tOv?V}~uVwLJXqtBIrt>qC$>g8k>UrN(*!`M1-rm8~{jlk9b{TZZ6nSb3
zJX5%4Da1%P{-l_8zi4WDbyGhUA#r<qo_CBWssZ51Dc!aQ;+=-e87R5QzmjX7R&d#A
z<Z;mDB&7}vMxM|&ry|FC@*n={Kkr@C{?MRxp|-!1sAq4a)r=|L05Do}pALQr_gQUz
z2lZL~BnQtzqtHJF6+dpjH|ZMXnh!ZO#vK*hbcV}&RtG5;*slS*@`gn^i)B`$?UDGG
zh0O<Atc6k}{so%ZTvNaF6PrS?AeAl$fNN=zAHk)aNrjZd(=j~$OWmYfmTWHEUSudw
zvefK$Z-DIZ(=!_b{Z|fz;pfRfiZ4$GD07P)1eQNihMw9JXi;=~rkVL5a30v6cirtR
z-r`%UIp<o9(G34WEN!?VzKnJ1YnaO<5UV;EkNI|~NNTQnSo-K(iH2yZqC$6Z&%ca7
zUHU>CByjT@p}pjd?nK5`-Q-TW%E|T@OP{!syli`Hio3j=S&=LvL`qRkf38clz|Us4
z_S3rF^MwUnUHtLl!h*WO_voMRrcOCt*Z6W?&hM&NL(RXMSMs!DMP2G<Wr<;LWdBFo
z#uyRV!5H=x#({Q0%t*6_Mof-yGA2m(5#~L6P%AY5IJF}+SEYEjBNc?&HWG?-nwAS?
z*t&YrMSKKKdU<?_DQl**&AFRM76)Ag7f)k7+}%<?B7H@n{0Q}c29r=%De}^Xt#h{W
z4Cm^G-hZ@J4_XuY`A*7!LF)^*0oJ#)B3ko#Gf#Ai&xC@#EY0(r^6FPbZ%5K+7kZRn
z406J8>6^WC<K~FFF{QF#U?TWhtnjI5&93H5*Zl^OyJ@^e*3CdLa@K*-Q#4}D&l(v|
zq+B4ayYF0qWwyd!@u3x1u=LbzmhkbFT%3naUgFh9JuzJu1;&N%4xy`?w`FR9*Q&zp
zW`^-;UAfvvq3e2gGf5iJnJ=g&kALgz{+oGP%3o#G)vT7x2_<ykzCo*|&s8O(FYzs6
zM0F%e>$ft`5N+hvkclPc;^&zc*t^@W`hzU!%@;#&EvXR-p~M{kM}|zOsGha14sNN9
z53TIgyz(*7@3xsvH<_BAcK>xZs&DpqReubzT_@Ros?*!zx*VRngWZq?zuo<N5d3h!
z8R>7mW@n%!#|d)=_YVQm^crBY*)uCb1x*KyNN{Vij(y9%VXd^9f4$q}XOq9c9ZhaZ
zy^d0S#P+8qBR_okk0q@~6<)uw%$0WNycoJMS`jj;l~%9@fl%T;KF!74!$E@K-F0Sm
zJ{6rR%_G9QU>UtCyE+t?J~skK%IeT{ud6rKOaJP1r`@}}3+|}H^OAeHL-RK(KoRBQ
zg&WNdp9(AeJnHPumO4(QF1_+5(+?_X(eG7K(_`*Ln&UV2I2xOw4yIF$m*i`a(2bp;
z<gK92R=GWgZrXrC&F#irk?i{Ni0aK|UfB?8`wiGeYG8oSg5^Hqb*4I;X~9+3@JDLi
z2+hBULKr#Ge{wUuhW!Mt=$-aAAI1I_#dYrEe+-~lFBA*O34Mi~N8L(d)WwOT^nq48
z|Eqj};<4OUz*bW`sHoi?zq1s)fv}H={6h`TI&aKcZ@4y9bgKr$?`Syd*ybk!{M%wm
zXj<kD45D?l(E5o{I_!lf5MT8*+WHb_fsOkiyzJ9@uyRzBqutc_-T2T&yqw$*I8&LI
zKkVf{Sot3e=C4uyUD89U*4vS_`M0KR39ZuIwjk`4f_=uQN%Sj%iYdgrzVKHs%M=wq
zCYWWkvfL)!$l@0tdMoG+%A#kpU+HB(PTBhhvrka=ACTSM4L|vfK10#N6}=@GeUYM1
zRP?%FbgQEK6YX=N$RJKkwn8{#6ZY<-@SYCRLxxz9bOC5RyR`NqKXwfFk1}bMb~p0M
zqqLEFywOWTH!eGF9lbpNPUs<)Nod6qF;YI<P|z@WT>};+%`8hw_@hVbB<JsYsIZ_R
z{XK?ic;HjmTqPXNOb;!|&Bw@K$^0<psxgea$}UVVEBJmFCU-;Ej-)yqZ=RNduWhx`
zLqdtK6PM&=>2Uq21uaM9>^6D^czn_G#$dyfU=voivfTQhp>SP&;XioArljkJNIF&+
zO;2G?xY6J$6b}N$$fFXu5$7LulRrj9c{cLN#)ionZUg&+bluV|y_w|d(*vH0I!ix!
z#eARgEa#%Souz=Uu9KfWvtZ%-dMpRM<0;-@=T__GC6}~wBtw{fyeoR#Q#?&@T-150
z9v|Pr4>-i}e7(aqS(y1?SxJ4N%xd4=6|4D7s5fuP66Wx|5$4LDh$s=L(opz$L*Ykt
z@prkoVSAxw&jUx)WSZygA-CP%F*<zSS_8Krz1P0z%?aXi%?S+v^$^(ZrNNBy7v#oh
z-+ol*tgc#<xfu{UAIW~tdhf~E6v(q-&RX4I@UblUI-hU!!Lt{ou3+<3p8VEr4y{Qi
z_NGSJ$}M$&&d`YBG*)WVG#-Yw&qmaxrUM0NRfpzR@r*HSPAFQXRu`cZ@b-Dl($;@5
zwS*Gm4WE+t5I)FTu565F=v}l73X_XWaJ|js8biHYLVZ1FP0PSKc4D*ns84w&kXzNQ
z$mV=UTgx+&+pG;NG+zfb6(#F?|NH)Gb|(7V_n-`CVO9RT?ax6=r;k+|ieB);qqJcW
z@6dDaZ}I<?Sh}>m>+q~zWngXc7zW6o<<k%441UuQJ&T?8qCv=WqV1{0i8jg22nMw`
zc_=KL8Ok3u6y*9G44#(0hW++zt!wV81-x9~PqKO9(I*EVBx}Y@kU{&HxzNJgWpQr3
zvr<<jN$|)>7D{Y@bmqOltHPbbB}TAue8Jz;X!h7ci67FF#(;)!(Vb+GKVd(xN!O4h
z<7~v3(oFhx_v^WIW4|Q*Ow!@H?K1wjg`@|EiD&0J6IRLbH_a^GV0y7{(Ngt!%`)r^
z$!*OKYlp?ySNnMBrMdKg(v;oj%C1Wfhum5Q%7N4zec+cydku~`SA|1KUS06*kX^qq
zdRE2yfSxsA{~mhQ%>d<wEe$DfAGG;iV5`?3_2)w-qn~B%4i|d)vYP(#cJ~^a?{@NW
zx6K+qZ=Sv47=SG4lSxfyBW<Q1n@oNdYo73p9Pj_yWXj0Z)3*oY@AuX5uwZ~KP+PR?
z7fKw1Dr9-2o+el?>h;nsQ^O9qGUr9j4b-vJ*{tv6Jn?-u=9Kcy<G|_0@~AU~Yucxj
z@*i(Woukm{vnHKB>(l;I{2<dm#fx`vEdAFRVzW6*SHpN5AuEqLQ{kA=;z$y-@f1p6
z$<H#SR@XVE-0j{ARvuHHMiM>S6wNW^JMMM4^xvumn39_=a!h%-S`}a)Pv@o%!<X&i
z#~AyPl3qD}gC8Hu@uQc%cYZVl=J?yUmr$=~*Uj9{XI9^KFC!#9pP9n%*gg2@s{ri<
zYviqag3VvAndaZ}(@Vd~QZ4zE_-Q9$Dm~O>@~iE^PrtR9GAyn&`6M7GTw)AF2$Rvo
zie)<6WKYn)rt|4MZ0=zNvH~V~ifL?t8@E+HzBbU#Q+kfGK>+2(S#Uk%(Xr`v#x}#c
z>qgH%#o9yI%;p{+EA~87mbSFnyOYyWIF#TT9USEL*zpS8zkJVQ{g>Vfah&&^S9Nnf
zm#TES-@Zf2EKd<N#b~0==5)Jnqi(wQ{nF5J%d>$-o$ED9kN=SOd_ZVYWndMk`IT}~
z4U9bbtHftXI)4v#HY1N&dVo6y-IIfxH~qIi`k1nL+WtyL;LjL!(->Vt2NPzbeZM?+
zc7ao&O00Kg$Z38sf&P6dBbEkG!|!i)_yrDsYWOvTJH5xmImVDZvT05pod|x|*2=JW
z^QORBm`#LO5lU)(t|{R>Qp>Ttllnj|6~<yo!=@oqj9vo+2ooJ<A;261CP&kwNHIpn
zAa$&sqn*8Olkwa==8L=kY{#d!<JyW{S~WHQGNY>>Y<}4T*>DLZ?t%Z{84(q}5fL*t
z8<y<B&%D6ZgP%i*`za3Vu4U*EX2#6C<MH!@aEU*1E}_pEPaB_R5Tb(fAceYq5+I(h
z2>ZUF`MfR%D(5n*<>Z5=`CWf9?J4!!Q`O<Qywn!BXNnuKi&L~;-8CMI83uJf#ekT(
z%`Eq7A+%?ojoNvTHfOY@<d5GcG+be~oB0vr<qV!pSN3RY)tXqU-$39BGMi2Dk~fQ~
z%}3<FKqSMP4LSMX{)%+)eF2XYAM7Fz-z8bQ#Ymz2juun4o0^h#SZ97_ux^0C`(~z6
zr^iFd$N7dT&+*gKpWAol)(Z=E%%kkqh7xl?98*%$OspXFI)C?p^9E9)!CC9>$DkA+
zV^8=MP?{ymjfQ%JSWe8W*Pu4tCMcXTldDWo+nNTLF|mn|AUJEW#a(QHmN(C0lw&*V
zG0xo-+=kt<mV#La{l+rN-`7}1u>>I>?2^lvIhH)0Uh1Bim;W}Kf0~z{g?X3NyWMZ(
z!7Z|IXL)eW8(p{Aap8V`QxKoKE!;6#xb5DF$^!TKJh&%-%M7P;+{JnEe^LAi0sitH
z>%ncQv(0$U(d?!JQ*Flc1~)<X8uG&*53LUH$3G}V<HqvyaX>)JFq;(1+v(A!AD~q?
zotqj}%@qn~F<|;Xd6!Z9akU8_jOT!{QcWM0j_JBqOr>2FdE+bEOP)#f`>}$PkCF#|
zFxq|do4%jaqCp_a`>OJHWNEpTZRDNk0GODY>Z>}E>zx-{s@?mMTqxq}q?R7PQiaMP
zcEWSSJ;j1?H8Cv9$M<;>=ME~8>2+!77p$CQ1Vn3O{ODUulJ&0~4a!DG52tR?4g)Ry
zdg>NqqW_FhDtkMnwr%29W~qPu0khOWqK>wWDG&IW=r>{e^jTp0gSp`v7DVIYGWJR#
zW?zH`#Z{6SY*UA6DTN;;f8BrGV0z_9+#>u@`*j4K(PniWnB>{)>utw?Kb0qlMNPX3
zi^er&k1<UYxZi{avsGs6>iWIG(~a929>;gXuc1%220h@W8U_KBIWkY4oN4nVg87cj
z=DX6%xAPa0CrIXYt#~jb*QbWprdp?H_%p6s>Z&?CJ4V%-NPOt*UZER}a##3N&8pU|
z&aUIu)$C|GAih{VP}uYh?R^YifRE>{i=1UmLzC+leS?acz5ze-B=S1V-vXJ66mga<
zI+irL;VE-|(>nTUun&ex*mr@81>sO6iTc-kW$pR{_m{@J{lHBjE6M6QS3rPn9-8K=
zL;E1N9aNREnw31}{e@x6u)7Qdamh&!FU+bn9Y?_`s5_`MHLF8yeZfMTG~j}{af^FB
z87u&yI!6n1fZd-FU4<tNijGZf`I_C==o|gAAmR*N!v7~@7<g+w$FIp5aGyCk#mD{b
z{ZiL-jHr0TB}a2M<^5Rt+>*`_wW_hf*~NHcTk~aOg}}uCMk#*yN8E+`Du<^~W2T1d
zn2u3Zegrjn5Q;y2P*kR(Yn)i&yy6y04y1}7Ba05DVbW*>=3GG9r;4ZJ9pfk!(RpsQ
z`=YcgZeg`oP86G^1fvAi#?8r9m{xi{jW(Mfx^n^q%4G8Nu;7$eeKCa;H>*;E{)lS#
zRys?awu4H}MHnC$!UKo@wvhJ%Qaqv<<9d#=&JpIVl-5vJvp3FJ%C2gek{Wt#vU|q+
zL#{5FwY5GqK3qTf<>B$6H}q<HgHYM1RKFV{^5PsJTSY%(sT<J2K!5JaCH&oR<szy&
zfgvSxUDvcfA!J?!ElQ>xhdZ+hTJP}YiSw&EE?CZ^noWPFyu#+$DsO-(??lS0#40it
zOUR(-Lf2kohF9gs#C$}|j?lG@#8Af}gkB2G{jLdJh@r6W3Dl6$Lp(Wo%3CDWdWmHI
z+u|w~f7)aN$v)p$$dsn9fd}yAuNR2uw+aNd^8Zzkn$XV-Jd850L}~z^*u^K-#0y{K
zdmTW_TAz`)Kb^8s{InkHC%ffG>8W}bK2SuohH}TeLbeaRB3{ZCF>?(&M@=!-snWGf
zf2bV2iSk9d(Ut=y#+5c&L-D4V&%I(Wb6QMhKE>$)niqTzhIPjJ(eWqyNl=)ja~wJ(
zx3x@H%fER?VL^HXQ^eR!@}-l)sQN2k=sGnkF9|(`RDu!{OPRm=mKHXhNSyZ0E-01N
zjTm~Ex1!cHeWM|{zIm;nLw)-dHgE9ka(#=k5Y2D2n|_C{LE)qH$nMk~VtsA|2Av~j
zv;DzDh1d@dypiw1e_qn=Y@oKMZ~J67Ws}B#QGNo`*Z5ls$ja}6UEt01^|Yoi!mG)_
z6gT#Tru{w`9-7$iYr^QepmdmtxS+I*O%zi4mL66YUtUtv)%;6?mGLi-Ms_yqoEr_h
zv`N<0A!`TA?Dz+dK3ThAz|e@DaT{#V+^6e1o<0vxoPe`l2YF&OFEt&8mvn#2AF?-Q
z&x0&&-OiLQdaP;L8B5(Fa9Pk$<BOK@twD{YyLE?w()_um<?EggU=~iwo)4Fq2KyVU
zUaQ7Id)LBz{S#H8BxRPs^4#tSB^HyJ-Sy^?l>DuFJ@99dW}Z*oqUQVj7&LcwczC&H
zImAQnS$;{bQ5h}Y2()E3h0s5;v-*ZvL$B1RaKu?-oH9x7Gv{`rB^B#8mI}vuf7kU;
zs3+ufM!LV>n61vYH<Ph#6Xhcdif8;9W#cUMln!(4tu0-sKNg<fQu<-MbRg3($sW&Y
z400R1irQ6LnMjJcInXArJ^B5J054(e>7S3^XbB+Zcq4=Q!?0C5t02+X+7an-n%(-R
zeq~yH#P9jd=p3S@cf1?N%XWfV6VBtecU&U{B#&Rto8!CilZ)}rHSIb6DXt=xHV50&
zq6ykJNO?oUIBGF0mWC3qQ3gwjSnnIuspvrE^g0L}NEV3DxAfa~ixTKwq~a7KF8y|M
zD3*$MC`V_}V18h<r3~j)C6SudEwM;y=+B+i_36^IlH%R$a!Kiqaq6>o+}1O?Tsf!+
zlJzb6mOj9}QUM$RO*yTLG+XsTTZ@}U7FvaEw#u}y;R6}Z_!##sMNKs5nEaV5#N&4U
z3RX$!VJI`sv`bYodk-()Gy7n&_vkNwzL38Vm9CK{rXD-HR$@iFwg#{G?q1CUc@Z_%
z+xX+sqm&=s&oB{kGQ6LJxscq?&}&(dgk-d4X;TX~-1g;J`Q|5ph^0>Xsy5H$PefeP
zrSaX}&1>tNW%a$y*aM{8%TLK+3Z_6)s|q*OO;n_fv%<#KV^TvCVTiu_HD8zG$IP!#
zpO7!6j<Myr7I@`fV49HM0t?guj<!_1GAK3k4N=b`Q=UWFL+gSZT0K$qgkonATSu(M
zUPlei;isjH-v@GofwD3601t|#N)L?0KV=D^XG%l!>lhD&3!Bd8Z|~-Th7S;z(Hg|s
zoq)i2kbPN6vg2xB|9Ah~W>yZH*~s<v`rjYqtsD+eF7*2I0Mg25d3&AnTz&5aV&(eO
zL=6hM!ECD)9Wo<CR4wZHLA*Yw`shLKyoV|Vh{SS>4I`{AW(RNIqC-N-ivh{XOCx8v
zLOmNcWqcceWQWQ>Tjd`>`Ku{E50`R_NnX)H(Tg4zs4;jR$wTdIW$#<mH}i}!57mm0
z_;|cQD8H8c79Zrdxak;!X_^dPkyIVZtM#X(wP))SUaD`x9yJ&UlX^%`;BTsge0j?H
zBRy&ydy3jjG-rai+1i|Zn39?}C7QZlGE<w0jr61L;=s}$)mc#8n!?rJ3zbYv%>9<b
zdL-R8+k;0p2Oh(Rdl(5(bZqp0vP*C$cJ$V?%mEMC1P8Yq9^lu&JRPZ7+4>#_CeARI
zVb!G;?4WGBCA9J;jsTeT^yGeX)y$%o5H1==SnWI6xYR#0r!H@OJ(7Cz0oa8DLJ0qP
zSMV9-8ndCg1Ukm>>OaF2X4kW_(=95}K(L1HX{EY$Ml$6ombi7PM}&iMTEGZ=N$9vS
z=>N|9?x`#hk{w7i!lC3JXr2cuUzq@0pFFszfxE>v%gSBwG25==j8?;^*N82VRGS(h
z)%$L>4YZiO+hdbUZTs;n8!PFQJ%`fzFLKC^Afd}L_$;~nC1u}I6E881F*RGEoE<OJ
zubL+qBq$7>xHxtr3WW3~W{inaqF1+W5Y5{bQ5uTE(&>jqIM6g=$X&t^`oU(|@}dxV
zs5vThJ-9?t3yf*wQ@js1gMv6>TrBmF@fNW9*W`Y4Y=5dPMne}HxOK8?dVsj*7}441
z4m@W#l2WH@(#)M{`-9w4&#f@#oKk_|Ei7R5H*;1~MQwNEB<kdhT_~X!4G~IH@@#gh
z2O9K=Qnc+}TY;&wo}Ep5+T|pYy?U~Ba0zHhUCNL&=Jey5ns=!yTinOLz;%_1VY+C=
z;A{hm-yzx-y6?i&JfWU^&0{g6JA1WdJ8(7))r^wm0CioO3j9ucJ2R4=v3w?<&*Vv%
zQ?nS32GTaT2cEe`R|E1t!_#7!c@_(=u*<BaBEtlOjLnw|43X2i=cnt)?EX7=vkdH8
z)G1D*%R}>f36|lVTtDN@$dIwsvtDaRjfx?^+q;|B86J7TWaxU<J!Ve8D1CA%lMC;=
z;do-qS5e5KN$9!RW)hkbFbKHYi1VkdZ<4WVWsuWGI2kd+x38z0{EmYKda#+{tSlc`
zUUNR6yE4C~-AbgnCO?JsKU!4uZWZV{LG4(u2@DxWPfZIE9ADazRkd=o7^8wji#55y
z(b=R0z_=p7AzRn;Mjl2|w-_<8KD6Kq?v2UJR2KyFXYH3^0nZ)&oU>I^X*4w%VgF|%
z>~S=jN0e+blVa(IDrSQnN56*-tmGr+JZ>&LsOnB#gC36bmH`MkUl}7`yUFL>6)=U%
zJ}P4AYsS^n+Sw*0%BwO5qLZoEc`7ZvSf#r=uO$uwbC5CIf6W32<X_MZS!XhRqmlQ$
zVKuOxp{26`A&t*RlqzcQAitcTe}2u{oNC=WW~x|*j=}L|cL8D8uug`b8w~=QTg-_L
zb@Nmz&e{u*tJe}&w3d1$3pK@b!pnA9<N39$Y~2t`n{yvDlg+iJikaj|jiap%4wi{<
zywNn<TzX=rP@EBB&ZjX4PiM!C8n1kNiMN?4kAe(PVWzpx8ij@Pp4{l;aAN_a>7>2b
zThDAnb|oL?lOe2T%;vKN#|wKliK94b!dNtH1P%5_uO0tDrmHa%HAiL}B&$Q#zsGCR
ziq>aUTVu8zej^ZGF&bsiW_Jt2xZt4dMy~-y74AfaA(8(Me|2MO^ccCw>;-@J=<QjY
z9RAh+0smD7f3?DQHTqxTPdmW>4x~XA|Jpt2PhYa3^qTg6H@a_&joCM%Ki_j-Qh=P|
zUymQ|HJ8<-umu!_5+j*vs6Zf<h^Elr_j1d@d(l*hCe#AA1fLIgG`tGvb@(C#yhL^)
zqIB@G4mioQZC4wcuBlT?_LXTJjclbDxT6JkorfxB3pACI%3amb70St5i>TF~#%HJ6
z6$~Qe@Hj0udbPg7XldbgF>me3jm4%EZ_Jph$M!6*js6&q8io&0T^bLO!Vf5q6>DGq
zTk05qtZ?%ydmQRiu*Jizp;uuWkUs{uaaJ+}D#Xl_R2^5IbN3<Zt-PWnFs;KGcF{l;
z@cKA=TL@o)coZC~UwKnyXX^v1%Stw`udt#nlzc>_o3la!GB9aaz0<p2Z0HPnK6}5|
z%FJ!ZT$4`MHPOp3$D<1=#b}I6^kutvAPnP9{h8_Fd;StwI+gX&(n(z$16W=Br|Il%
zT)@!DmV3ZnxyP3KED|Ks!DQhsZ#D0uSsgHnamqXXdLABsEn}<Jk6<BAo4I6Qfb5Dx
zN)HX)w3Mlr1zu6;?`tB3tE0HTl?@qL@~9;3EY8-PRhBC4+x4QsV?-g@qeDhc3f*Ds
zZKnF=6x!u#h2h(4G~Z;;Eb2Wav7tR<t5qpWjC(+~yQw<je4L*81#6&bCrMFF6ft60
zi-r;@ks&>6aMbzB4&soR?%mKFSN|xL9q*pN2!PlZ-y0y}!-j}Fl$2e6zNB8t-a`^_
zwXZF3yGZgifVap<VTV$fQ5_n*KDZg6t|g*V+}l5&I|a1Ci<`x_GYgh!RTOr&(^pZP
zU~v`UYOsam*1|{EWtT;2UJ0e;H5p0Q7Dk;l4U^xHZ|fx^B&#Q{`yh&u_C4lZx%S3V
z58q8*(wE}*+K|3z*IDVIhrz<9J^=*3!~W^OTa$OyO<pSR+4|!)j7VSDE5;|~RlYL9
zS<6Yu4Jxz$HRMp_3SNe*D_mZ8+|zYS-|p?Nk7Mbg)9VX&Q|6KUuSBtV7QK<h#Gp?@
z*G>do?q#U#(w|o&IN!=w-dtUKNi<Z+>doMJX1eI8sPn#>;>HSh82+Fzz1YQ%u7s$3
z3nLnP5XR0uo}JV2|Kh(4%o>W%ARA~^-`KAD!c8@=G%v3&e7T|U<;)~YV!D32`_MG?
zc_>kW2J3zsGW&h@1bt?1{D`I}GrMnY!OwLya!=GxpSc?%cGOMYCED=0BX(<jdO`_K
z#NJaLlXr?7vEx>Ga*S@8bxggpF4jA#E}*0Qby<OFPv4>)vBF(oxvdUUNJGu5&Ch5-
zH6AvO;&_}g+0?W0SWcI&_WU>V&S5QY-I1vz19A+SV64#o%ot?XL>V0+&xP6%L1vA|
zK0jX4Z5=0hw9Vb|vy-6wASgF}_4LD`8T;QCzT(~C%4a5Xo>_dU+r~cAWe)#Gxvfry
zP%f{tO-fYUNf7Sf>dD!B)%(}l*~<l$GC?;#-EQPPwQzkj)gEVV=HpP7zAvt5WVyhK
zO3-M0Kp~#_I{{TtgMrV>QTJX$4n?^cNEmo-;ZZb2VNg%_R(>UUrFqt;yb9r4i<!7g
zm7+qIYw`?>r9n;c27yW;5DJ5zQVbgW1r4BN(F21F_u2*hA$@-lv+W+s3Zqs|(*4*<
zqflZ$7~jgv(A>|!EdHeY5DR1JQQYvjGvd5xTG)JKEOmoOiW<9#^bu8SBvp~#tK1cm
zING(0IuCwi<kn8NkvQv5@{Y<}ypF%#9={F(gc99E*z?4&dh#y@$W$CkE+kN|^7dY3
zy|ddr2aN%zGdZW7#sJ|I!Hff=L$2b`P6#`L+p8%sXf8dN;?l)~aHKGT?l6kiWb>D+
z*q2_}b*e8P+^2~FvIt0j$bHU`Gug88b(+b>VtSALRyReGpRw3h`^X=?Y8$<3PqWo7
zxuo51enKxUG8Hu46HAXarxL5{?6VXxtKL_-9ms@8I$Ggxfb&j;EL3vo%RWA~njP~m
z-J7SXH<Sctmeoy0%gG0527t|UTf5Y6UZv#GHnG93LM<UGISxZN%4oGYXs@d4ELu(@
zjC~|hv!a!|E5gP5$F;0~gR3&kO5aTWD9Vh8s4l{#ba%y-;ssL^If&+1wOEMK&p`q9
z=TB6naQ(_`@eF$oW=vSx%G%*GEcf0l@j`6!di1t++7GkvRAt@Cgi}|1vE`zKxKwK5
zZ$L2AmL%lsvxhK``WKJyN5I}mrn%qk#PkRc6ySW~cjb7x((u4VULRfw5KoYOf?T6T
z@l8b28b13BrnV14%T}}TI%a8T{xCyFPu}i9zyCnD>!>B4ZV9eB1#?d=uV;+;%{YUA
zdmuS{tiJ`;nV*3o8yK;M$uF_x)2^2%kCxJr++eH|>}+skt!SOuDq)|qXkF%Lo5LQA
zQ-VO0aDYj;2AAhoOuwr4513QPyut2c?ksXGJ3vT+yts1H`I#3eK@S(`@Tk|L%-vtm
zOQ;ClXQeB4g?|1aw4`G>LF%{jVn#Ua<iZ3rP95$VO+@BN0_2R}O3zNeXCLw+*SQ95
ziI1;9EKhuV0&^lh41zQElMI<z<=6Z!qt2)BKMA;ZUlE}2nWC_+tVMT^)lFWN>US*;
z4ow5%?-n*MCaxS3p?0I#!=pvZ%=J4qWTQz}^dXFXJ5qir1y3VV>O<yZI{QDc$fcl@
z&m!iTh0IQRw>@Cywio>1CA!mW<o^)1WVJI*+eq=#i<)?kv!w(M@2Kf)t#McUw;2Lo
z;`QM((_7qhk|zeQ00<!%&Mp@4|7l2#EW>0~Qq=UgEU0eNS6Ns7RXZv1p$8W=|Gw)F
zR=>b$Bb1m<w`q0!2)`V`u6m6dkiMSzDIs4T*KBC5XS-b|sB5}H39X|eOpMhv8(KEk
zF-^ueyEmx2`J_nAg(a;Q8ObA8#VzjJ|0Sw~l9MSdo8#{~Pv_++Y`)v%85ziPzsb|I
zz+J_M-|RwmcQU`FV(G(mJTOg_$Fr;YL%OUhogLS-oA}DY=KFK#CeH!Hw%5N9;43z)
z^N%(E49!odK{~PNywTDn&zu-a)USywNDaM}Yo}folIGxVYg1Hr-fnSkWAebj{e)W{
z#PVRUL}t}5^ro8QDMB;c)0g+`BfFld+Qw*FpcfBJSlyU24CnTf>S$N0Uy07k)B($<
zIic_Ok8M}igYFH~<CE-5IK_BYx-*Fet%vax@l)T)@)6Ut!SPstG7lmcI4^o`efo;Q
z@@)UeUd70MUR}+{hGUa}cbfZ+=+gps4!I=8%+W#ZJ7A{S=)Ptqnox2yF!(NQYwgqZ
zE%`%;8&|d*TQj`6<wFfs#wq?5F6u>xun26!V1DPar;YAky^@xaUWIS_Mz<S&XCZb7
z#4=8e<`NOxpY9DDL!qJ1;CJFT^lkdY444yC1)Xs^KwV44M?9{A5L-)Fk776*yBp<A
zk$e_FCH%pN{ZR4)!iGV;)_v*4Z#M;QbkjdJT%Py}edC5H&98f{xK>)Ghu?=kG5DPb
zenKho=q3P`c!+tq+}{9Xo7&GbHI$GKcjhSmczo{u*_N{zUkSgi7bq&Po#z3RX^|f4
zUc;vkxEw%;bQhWP;Ew@FM74rsxjQnLX%(5e*0|pyj=YxzD}JhH#s9pNir=Q<?0$^a
z;^tK9<n5!K=UI@ih9p>bi#+SDy~$Y~uUt4zQv9=`2fouZ0})=IzPtkek_>g;iz3jp
zKOc|q(Zjw$Z*g7c2zbS9r*eSS;D3VK<)fp}8rc<D@_o}+omm<mPnQ`ri31lf!4`Lo
zk>8=@J%VH9Ad^AhPSH;lJ!ggR_Q)gt(HqTri2MXF+VjPE7z(5wfLYwnIbIChDIQX%
zfs~$ZK1ZcGK2kSbvb);~LW9yD$poW+Uk6(|1ld{iE<Xk4Zet@a+~3=BU&rml@7^=B
zdh!_Mf#1&pD5LA}e0kwsX7k+{%=evazR_O3uPWaTy3yT=jLWmH$hu+=fYO;A4D$TW
zw4>g-GS`OYorI^gy|a~LG8~V=x<0A=N0*@GBEYpPtDDk>#CPouCDv27EJP1g$hJcV
zoMKocl)Q^F=m6D~o9MFq#X)%IrHJ#4`eSM+F|ZhM9{&Op=plF~#2$-8qvhW{D_Rg+
z^!*VH<_jG$78-#WAZ%_;($hWKvP&h+iZES#1NS~f*@`G(FQx~p(W_c$#?m%R%qcRc
zqOZb1l21pW)pKf~+6-6CETFLOd+*GRcT+B=xABKv5t^@~^s!U}=E!6RxpnQX*{7ON
zq#dZfMK74s(9gFbH)pZp4fiQ!4v(d763?2;@-{iIptmeV88m)b-u78{Ba7#;o6TPK
zNttQdd8>TE@R9U)d-zrN0gk~IFj4N0>smPnJF7gFN~>g9$SYsB@POmDJHb1wrX5<@
z6E+CGxiWz7OBT~$x@i(fpL)d%MyP*3Uny>wgVs*_nwp9)<OJ=@jc41}Aj8qv%1vIQ
zxwS8R>eKX3)q0<wi8A@$xA}3&+(&*M-hS-IaI;~SPu?ZCbk|W!KkIeevUJfe3Olq5
zm^c}xq%YA5FZN!xY2=&TUN>S@va3+Z_R1TK_$7=MF~U~?ji%BvAc7#7`OB-_{H0JG
zfu6U5Y9u}=Y)>wuq$aPVXZYQ-2+GzXe@`S$*?J0tvj2f9ve&Z9yt1e6wQOojOW0Au
zH<_$V3B<AYVg!^iD$_>2jaJ}3iw>X$52Pr)lD?V8Qc-&mtBjOdcMvloI#q3EAd!ZD
zoE^a)aCYQ%0I&I&dswKt56J&JDv$FjeSlw&%BJ0SLz39!WPxpd7b*{^^`Jm#NwjDZ
z0~Zqa^@_RF79)b1Vss5H#T40M?)*>1JS80h5*$r20oq+dK1;S97>IV%P~cwgWz+#$
zq((2TrP{fb@e8P4pYF5K{1hMDvwmYz0PGdgz0NQ~sdya`O61<8;x*>mH1l|`nrr#Z
zaIW9>Lo~I(wVnJEW~Mz+gg);}DWG9>Dj+@V=fJtc?DrCVSE;}SpQ*rF=ELHX4Z?kn
zH9y5qGr{apEL(&Ch7EGw5EI5+y;NLCB>ZefiH4iv%ZcBPzL9P6JKi@UxcDvaTdjS~
ze4Ew|1!>4`#;!NyC@gb)mjCR1S3nr)4~Hc?tmua)LJ6sLFyGj{l0?%|f%}>?oDi+H
zdL4vGQTv`F(f!KaF^L#Vpx3Z-$mecVaV@CcS^e&5?zIDjB79){dA)@-uF=<ENr=(u
zP6H_O95+_LFT)N}Id@XdUfTW5Y`cMbQ=K+b^_`omkE*KVx!Q+0D?6twn)*MVQ{*5b
zL;Kqqy&2~|@gwa5?Xe$Ir<Yz?Pt4gm4kE@TPpr<<8noe6)nP58as#0Ajh_>ja_Kzn
zB=gD`OO2=ux4?$xJ^>}-yKu36hz}jw8&?oYG$NARYf$g#4L$jxsf&MD>1vjT+Wv-V
zDJI)HEWLArVQul%U{^kEL@H42zMc!zy8o~N&aSvCa`9#EBLtYP>&yWVY<0$7j3V0I
z$Sy7l#dXvgO1=p?G9pikbS7Ugm@r3oCO3GW(IM5L^z%Y0ov2tyWF$SYF!IUEQ9K^z
z5p2*KKr%*?d?R56W+-sG2_!0J^V?PC@r|j7HEncAWhnh)?4<Il5FqRAahPb0D4G)K
zZ2JP)Ltl6ExhwZ6;iuU2W!>q1Wu$59^1w>Pb^If?B=;%t=*3Fl>YsZ*bTKQ!mxz(O
z+f>-A^_P3okgGZ<P6lb^)ru>Q2JEiAz%c**_`gVlUqw5l!Jh_9Kv|x0J{<;OXK07E
z%)IHP>)WuJmeW$TIHo5<MX(b0=^vUETJi=_noPCIgg@b%+7&Q#)oxk6v16MY0>a6J
z?fFd;ZPpUgk(S_gQ`o+%ii2TZR>V^78Nd=#nJK5_B3q7L_movQA5}L)bEJkXsD`ys
zg}4A~Cy=QA>S^@hTMdaUS2w!vLMS<RGb1;KlJ}x8;^j7&tn9P)aopQJ`BZBu-D?I+
z|03KE*_#=3i>CA|55~=~A1N%_d(**WJdseu`6qcf|Hy}z{YONz6fy|dO%rFk*J03+
zPU&`k!BCgc`dWmE+n=7Nug}7x#g|%VWz7tikV8_dgN9~C8`)FfzD<A}1A;l`(cebS
zo$BS750E#W^w^!`1Pg-!W??}0*>Uz2yFQR)$*vC~6LA=xO(DY6NP|ZAt+4^RB{ne;
zYQLmbx8(MLQ_9^rbQ67Fu4wbrxQ+fvAbM~PdbAg<8zC^qO5Z7WPY=M_i-CzQbC0#r
zWnNP<Z$d*4zdHH%A*~ugw7YSf?bZx_E!p|S+xL%-gXgpR{*qcJ=SNh#N9RY>x_xsI
zl3=}ZLGgK6=2R38>~XpJ>3$Xp9tyi{Ke24!Y>&3*GWR0j+jaO14@#t312P>Em3+;q
zYznzWG!fk!u?sXC${uBl-4hspK1GY&0bbBhve@lIP!z<)Ws6(jr5VZ<yYDl}ahOtR
zpSYU|*t)U2huoV0%~cnPn~P|B^VF4C;tc+Wl26lsSo-#gh2nyAaW_u4Bb-n7P)W2<
z-yh`N?~gYOnYu2(!V?V(YrU>A#lzRJ-<j>kOU3zz+iYWk+5Vj6<CDEytDt>0*U99{
z?Y9_8={0ZIu-M&=#0e1VJum1HOY+Dk*7IJPN30bl%_G($CN0}h*)IAaB+Svwy$GD3
z%db*__HM``{%sHOJ^*P#5?M1>qfF%EdzOcU87{@{8D7xC_x~{WF7Q!RSO0ec6O95+
zP}EqlqDC991&fvxuo<1`L?;#%1uH5QYiYHu)dZ;*f+hi{;{dka`e@Zw+uBE;YHPJ9
zUcxniH@u@*!7FD>xp*m}Re8U^wf8wQ6TsR&|Nr~G`6zSFKKruv+H0@1_S$Q&J=kFK
zgS(Ia=HVS+-#onA+P9wY{)yBgc>m1&1>WwT6~Wsb!aK~vdpemSc$e6&0F)Tv_vSN7
z@H^ALdU&Vu8sYas|INdDwte&Pj<Ih&;VmzPx0i!=crm<XAxo_D@cs|v6v4Y2@OmZ-
z|CdfFp7J1B==b}0V2C#+9@w5r9{Gl=z%$$6<k9f19{k!SvLs@#8pvebJRKHUn7|=j
zG}4;_o^5~~HJ*<&E~3Wo_*aju6M2oW)!@H*Y#najaAH3io}uZxFA4cNUaWd!9iHU-
z_>!K4K9QaU_wbFZ;WcVxmH*}&dD_1DAu;_d;!R=KenjPkuE8dxhavnFnvyrEV&%<0
z4aS^oD4vDuN+i7w4ToyD!*tM#OwmyN=p(<5N@q4UWY)2jo3CW_f_|>1ql;_m7uFQY
z!PyK}p55_H!K*wmw!0Q*|7w_hq;EJwrjU7TK<7t?l;R{gjLXpoexyIW54nZ8mXRws
z^xIhOIPY!u4>n~MMS<Hm&BiI6v6%Fskm;)^Ur@IZs?KJ8NX0wo0P$(88Bh}jH+3vg
zG~5)K%TUmV#@nw)e<)IBa=2?siP;u)4Bbgi+<pT(71V+h<j6Bbz{o0o0t>Qu$3)TM
zgoUX`RHZ5DUb=;zGIi^j0E*y(j%Q}F#0jQ#XWK9hNVnk|L_K3ErR@?hDT56|o@wH+
zo_6ff=Gk{(hwV`I7-o~n{QW7HUB!!FwKE5&>KGx1?qprFAaHCyyv*jKizB0Ky^(f9
z)vdnx1)A>oHYM0n3^ln#hVxr1{eG-Kwwap>1~YIWoVKG_)cy6qpl#S>-5hl%KJz5B
z?WW915j$5>tag15)*xM66&9%K4D}pHcO|&zZe~v@Kmpc+M^1tW@T^@hw&Zp`E7G>{
zo0<77Zv;O$(P-|rjv`|JWk_=u0+z(wZ@<Mx$zQ>r`cB7jH7T?NF5GsAb(!9KVC!jp
zEB$S+QujLEq=(G|!|qixMpeP~bdgp-8Y5b*ct;flc;g+q(<!*0p=X|~YcGO%K?rlR
zhxy&(nO0h~6d(m`&|>h49)p|dyQf0bXQ*(sDOzJNvyNSs{~?lj$cp^Kq?N=o<E)bR
zUvK;SM7#*KL>;U(Dd)-P0(OIQSj7<aC6!r9#53a25q~pyhWTgTlktxG`JQN-9sp^%
zP6zRuC2~jAVLxPhR5UhoDkIwg_WHA2qt1HlXiV@#_OI3Y04HtZ)w!QXuIl6_Ac)-2
zryVQ+ysEexc%W`Rq4xPimo@mT$=E6E%4LYR51}Bdm>QyZPa$pRSI$Dr^za9XDL&z{
zNtM`g?KF<<<;`n-Q|`!(Dy)KS6y$f`8^>D(p?#htrWxhuP1*%B0mkVux~*vWq)OiK
zJ{$A6uk8hvZ8fCLC9FeJW|dvOOu!qVL{|+BU@k!=<QTKb9XBVnA@eeU!O3x{j9#&Y
zl|~7+hnqx86$|J--q~qrvtJn?Cb2A3#oJDXdwpJm)xm8LbCS-9E@3Q;UL_+@4h;1f
z^jjIN*aX&Bx{clob`aN`J%B@K2|Bnjvpkv6xg2*(a@dhe7y=4$K8UrLi6f2wbR+=v
zj6F{_XWFYuW_Oa$9hH(Z-aZntCcpT8MZDuypcgV7?$21x(Ry5<Z>gt__>4AV%p3vG
zsmoc6k?K$GT-FT658-#xmpLjXehApHU2;E>mrTX;a25?q{20#vzo`<jU7GvqcfbJB
zmu<1ot%X8Z=pn?9TXf#Kz1+Y%q~9z^p=i+<Z(9d|Tfw@<rP_~mEwfap=Uu4CPh~=e
z^c36){$K@fGGQWF{Ves`x^WL90M37siP4ucY!Yr*d;}+47y8)p5}m=ZR4E}YZ*Hbd
z{z<c%O{Fro`8&j%+^!zwD((IQNR79r;T8;b4LzRuAqDcoNtk8wI3f=YXX7g<(ikQe
z7S@UDw6zrMB5LMA%Nr6uw8V0ssM)Sl*p~QE+U2Cn@3t0o`8aT_E{j=DB#(M)M!b@B
zC7rQ#C8U}t%=zC*Q<*>^D!r+=(sw)_?Vw0kP!-mDpACTF!$@01UJHFQ0K$<1;3_!P
zrr>5UnOT(ohM-?$zB7!!X<1}@L<&uQmAGqR{Zr(x{k)GJTAP|M1_KQu!R`W;HRH<w
zhTjY)M&XO1Z_zJ*tKvj=Pu0zfPrr*C1fO|csZwWbK^bpEnbi3kLdK93Z$Fr_NFU1(
zyqgj;lp!zPE;YyU6lxAv+XM)Oxs0%ygNIc{-IY`c(mGm0$K{-0wxb{ijmQ_sTDt>S
z2z#Lg?2ExZ5VaU)zet_!PfFz26yqg$N`#oJsGL$X(3AZ@KhkPF>k2L=6Et00V(Tai
ze&NbKepyl3Z--^Wd2z&+%1)(XtaUIs9eV)jPn$OLIyzihm<+~(M(a{$e$<`ONaWJ)
z97zh@$t;hzpU(J;)W=cyK$5AJU|1y}r`n#v=&IU(aE$uHThTz-9dMkVmD1BO9g0Ey
z@Wcl8!F+`FV0SbXt!-{4SD_R1fJ6To^&$8rW5y-D#{<+kvx0NgnOw!~mlomaj1W%<
zLsNftA(P#c5yJ3K=C<iu`3tC~n0|GuVG5TS5pOzooA#0wql1_}SML$V^x0NtcFs!D
zFjkIZ5zpz|ovTE!Bp(RHxJx(@Cl$d#4bVXs5Nl%ZNf9_m8}|6JtX;X&<>KguQ7`Ex
zyv3qKM3+x>9W_V;s&bPBOxIDhdM~<HN=3@}RB}nIpHa%!sr>x$xY#|B^%b75H7apc
zlqmV7zrsbaU2T$4`s->7+yhS%o%*A&6kq)XuODS}T<M)EQs}Lq<2=wouYwPx@yP-{
zKiG{^q(a=&eCl37O82`Z{6qIkhHgtlKM$`zV+7&@qY)6QR~<>+xegm5cUZhK5PwbQ
z?sKE@Vel&kCu=!GQ%?c5ob8|EH#k&u%Aov=TGA8V=xV4`H^LBx;SE<eF=RxEs<tia
zq`t=tA$|*cy*e`4<rzdC-Iuc2_p$CfyXl*Mcd_Sp0hTWsUUYk_t>X@W?drHkF!Luc
z-qiqP?0TRfY{s|Shb_Ug*k$Y_gECH4#-8~gw+@jh_YH+Klg!xJ6;f8I1b?&&?b+>8
zg)Nc7j)>|I$-4Ee3Ut|>Q+iT4quGv-mVzOG72Y>xH&BUwu1c_(YIg#B7?g>YBCjTk
z?m8=Yj0}wF%!5p(l?A6U;ot|?&~T(roJWJEPlWtn8#2uSva<X?U<3$k(dFer3#N+7
z;Om;wG=4W~hluy<@FdYGa$<h>+ok(qr$Cn%?Y1|;AT}gUM)$2M;vcCDPEL-6P}`7t
z)w(*^%{Y6+Fg`vGhptjK;3QBm3tA4P0+Y$+(?Ac>?G8l2y+i~|y_fT8v-uP{{fU7s
ztB$5mkfc!XRU@UYVEQ-dpF4vPTw;@IjwV&`RW;VO0C9QA;L}de-wHfF8wX;86N++H
zC?~msOO=Pt#(<AZ-GumG*!@yrev7RJ7pc9>%G2^w7(8|tH``xy{)(FWY%z+`3CSll
z>Ik61bkUf3p2+o0nJ!}1$J>9+cXX@jU=)~4wLMg`iUbiD!VjgaI^KQ*?{Y-a2LX2s
z;z0c1n^cEsDc-&d4{lu%^q~@40B9-Dhun?2vvOmo_RCZtExu2%Bi#oJD;Q0eKNFDN
z8_y;I9FCjqT!?RkaLI5&KCN0h+!-)!Q8@Nh9XtU-M1$4A`^XGnY=4Z(f@!oBQS%>=
zZczB2Tbuzg^f?TMXgpL_s#fqma7p~<<&s*!e=wX}ly0VURO7XwybSe<w*kWfx6NY3
zUEF(NZ#RA1nOfy15OniKeKp1P{U)sMKtp&}aGi?0E=^JTX0>XW56eSB?iu_y-y%rv
zqNI0^^!&?^u^*kuqfxp3iRcHAqN#Zk!?h*)L2!in=P>D?QsK{+63W&f=Qwn`4NJaw
z`#>WBr-|x5-GaUtYNBjtm1v?URI7uIBWONSa0n2s!ySB(3RD$*kFTL7iau_dXjP%+
zEo!1)Q<<%5qE$g}q^8qEX>A60ja0W^N|2RkqSHx<G|`KB2^z$Eauri9WYj?fFcC~P
z9}bhLqHPN>gcL`SeT`W<E9gVXLXpSR&0NLd)Dtbxx);bF82r{sUmJwJDZP_HUg;Dz
zDV<f)u!HzWpg@<dDpEXG(63VHmyi-c|ByZdT8aHDR5q)EPe<r;v9igux(GrE6}{Ve
z-nz2+M;M?OflNe&9td*vAk;Vw_LFD{ty3Q8M)&VCUIn~7<?!<53p`#VX!3t3f@1*B
z`3p_g?7{EsKC4EY9tjPoi#XDmCv4&57$4k@b7gM%6mLJAfeXG5N{qWz7L+qhg{OzR
z&X|L1u@kiTk(jf~uCWiU^&Q`tw{+$`XWk9I<!e}N1>j3A5VfXCb#SN+o=u>S3PnD#
zX&i5VQ?;%nOEi`DXD<g*>BI2@BbB{Wsq`%}g>0%aNYfc`UD-X@^WPhb>!@~hbV0Bs
zbsViaw!moNN2e8+A3%9`?lxk(mH9;s1&ulW5*%2Zzef3I*!b>LoIZ$jH+sgu%yCXA
ze*~35^rLb4&{S%c;&{T2<h^Oa2ZbmUe##%4i~R#M5W?_<B4s^(^u}Q5H)IU&DwGJb
z`jV8uXudJ%Yxx%b%Y1Lv=?Ko7*g)bljuPfe&cqkw0YZ+G?=qHm>Cx=wW=B^TA0_(`
zItyB{hOP|OAU!0NZlua+FSi;jL=(1;y`YfT{wLXkB~VpF6~_vbZ`tZ=Ds_|vcNTv=
zL|?o3s<uvW$Zfxu@e10>1~onWqav|2l-x!ZU!fwA)>p}tzm?*TfPYd}2ESsKB4v(<
zU_B3rB3RF~7Z2-+f_0B#XhsQ|IzhwrF}4n3mX|f}7OW<#$$Tt9+^*zRf+iN-2;4@U
zDseW#&ji9dj0DXS^xo}!zk|GsAo!cTcnFs9(l*Czi1GHH(^T6Wv$fzA#S3oG-)uQX
ztg<RNrwD-l4Q0VOPzBhz<%dPsxgf+&t>L08C?BQ`uT}Y0j9ZXK^Iwe44|GM(E-rd*
zShU6$eStpYkD*-f2~V_iSaJ5V!|eO|?7vs`YGr@IW#6VayS$lf*!J_;Co6lNnPY50
z(*1DyC){a%iCq@xHL@>x<5D%(SL&SFD8*%5NrhP(C@#$+4w*%3pGUYFeD9BF$}GbO
zZ8z`Q-oblD8ZAH4ih{P-R0wsprp!lzFLCCO4f)3z6X8j!LuK$8D5UpaKMzCb16saV
zKfdhx;b}amJ6sG-ercOVRhgZoyRE=)=C~?8IM*ccC=qJn0i74Nsw6iYK*f@H3}yT}
zEx3ed<c=@+Px1rM@2JhnOM&Ztq}ZMj#!vWP<LmXqG?o4pzHUNYDB$bKuN%I$1-!%q
zyN8|Zfv@eTc<cDex+)g@pcr3!g;i}CUtgh2h_B;#F5pXwKKPnMZU49N^&2SXU*qdM
zl&k{2ZiaLK@5S#6UlJ-?qOX^}_Ida^%~f#?RTR2CBCN`5dJudueSOFXg!tN>@`Zv!
z9KMdGw$H~`coQcfIYM0<Q@>{BWQF;=61wcb)`R+>I8Re~H3KXD`Ly6XMOi`H!ft6(
zX1;{#<;o<>{1P5ywn-^#QziQ#ozZ+EHtdMtd%zFOUadXT!Tj@y9%fG?J+#~2)@S5m
zOLg0bU1H4->OdQ|&NtL5F-!<j$>GXbn)k#bK(7tTDZ$`;!`GX5;1n8qqz5sNI21Xa
z)rR?MW<TJI0SwY^F(<$kN3toiTpF^OnYEHQ7yb*Gj48Ht=AG#pwU-<rSK^Fq;>`RA
z)Dq*{7Y;4%^9iocH-E2aobub!TyR`TKE|;uzY>9JmRMcfDreiuvUt0KU<O|<$t<VA
z{GZ9Z1%C4X>3Cnn3@D5@o?4pIzd7e~$9pHr*gEA4k9U7cQ17w6-oyhOL*Hz?(}Vxb
z@xGA)|HgQyQAljO+aRn$pV?7RpZ~}ZMf1{)_h}{huG0{1eY_7T$xL9mFFxLWQ^qFw
zd0eo!B8`D%swR)0?l#%E=ZQ+g`TZVi9h=-O<d4tjB=;8Q^YufD`>NB-^mX3ZpF6%^
zqs->x+kB><%fIjbgW3E2d~Rkw51|D0{(yQD58Oi|TVl579a22Lg~^;B&U~RgG`W@o
zrCshP&1|n(!3xH=FisB%rQfuyp*@2=65|0ZqFxXKxR?=!AWkV_08ISTQg%3*h1uyk
z70ZH4_RjoBR3+<2aA8S1cRC#1jQc@>9$u8<d|XMsPM2>|m{0N`e>eHU^-y9AD(xQE
z)!XL(&i5z44S`*`FC5k^mw#bktQJA0b=3=<>!r4tvk-x*&r_VOvx%@`-T<Q?W3&0`
z3r<UCcQY3pb9%^bPm~iJdlX}Tzoz)zob!OAd-;<tp9dqxH_7(r$};m(bxSXJC($;q
zydkr!sc!iN>;jFe!O~W>?`xRvy5;k<?~6RCK{nhUXZPD0joZ!ysyWUMqdC&XHSt&c
z9#N3WmWwh2?S*fbn0f^M1l(}yv&Gx*Ma6fSr7DOy1~A9}0!>ZX7D?1qJnC@0#{4I;
z8LY>BVF=XEjn_#nik38gE1e0NGVcs|rcs?smoKN2dvX!^LK;VNQ_Gd<_}!cjKWEml
zxxUru@@4dojwrz1sj)5;)jIXX0@GDq@CwHO5}6fEb?;yB>ox%3*u;4?tJ1^I!=Q47
zu6GY51h&Qy(J#l;Q*BrGdl{o?0pSNn2zTpkkMG>*=e5Jsi)Av%=-<oX{q-We&ojK|
z7eXt5_8Lfi13Wz!ze2uuA)d6Ewgo)>bjx@e^TqKL#WVRFd|{Vm!CHc^Uv7r4AC}@P
z1pEQIrQ;tBLpCtRpJ<FlC9Cv(6$Egp5;WM{BR&L~UKP$4S>Z;fGwYi&@B8sil`m)9
zw=muuao2gUzSSZCE_GjSgMMn!pf4*7`iqISg@pO)P7NPdv+4r%;}gccx#pAf@Mews
zgwlRElKn;z$*yMIy7eA4xB4MrBC0!)KBfL(f?D60-IFdE0TT+Kyfd9s6q%5Gw}X7?
zj`(e6uIouEe)nT`Np3L}M;+Skf)~`G2KLy8gVqaf*sMd+KZ*!BmEFxO^Zl8R5V8^Q
z3UQ1A0Xu%~_(e#+p!U>W1p<C8!Q5%cEN_Hy%u9)J<YO#L<@y+CjKw_uYdFWH*0TC6
zCF_SZ;(q$yNSV*U<t3%K^nBp|FdiLs|7$#IPa*$~7SkU1S9uojX~L=2mk{@mPFb8h
zg-b&(^n=(foJ2BywxangrNip~2_<A}@sRa=4WbxbL=@XY0-|v%2VEGMGaDdt3OwO%
zP%fhO<sr3if5FoUV#77yhG@i*^~7<hs2v6zzhX=e;t0nte>g74J?sOF=^iiCIr+7L
z8Sw?>*ZWM$BKZ~nHXV^en-^i|=T3x8b)2=pQTL8Gs;%RU9=np)ioCq|>MmlVPekHt
zZ(wxdtD2|H#n-!(59N}Z-ggv<FM(^~EB<XF%oR(pKSdI3ry_QsEwC+c<{$eZ4<*>=
z%C8Q`0Mzsr<k$PiuNm(nzowh~dLQ{k&QgB7x%|4QbYw#;d-+A3{T@U=D`5Me2-`j6
z*R^G2?uGbzKN4SW6ybgT*2I^*FT<%K`}`J=ba4riT)UF<Ta#Z~z|$7w*Jk({w>iEx
zmtW_V;wyyw|BC$T(T_-ei2%I((s&mJeOY19|9>dIin_CP`Bge9QHTCD-~HdpFGs(i
z_S9Yl3f_YJ>fKX*y-bw3{F?t4<=20PM@QZN8jt@k%deu&^psyl<|Xp0l-iev)c(Ja
zUx!J4Ma%(x%FMLcM>mQq;W}Rx%-J7nUzfRrkp>5zBfGQvc8mL_1piIWH11oQ`=$i_
zxhs6?%Mzm#+|QBuhuR5#T&o>{39-z9E>soq#<JUH$HeNEU9>G>PjwGgW?B61dGiY(
zCQdQGO?E4rG7IIVp$oy?hGpi%=Iv6s3oC=gduoT{&E|M2ckfi%)++bSeQg({c@JVg
zyUuNB?w{QW*PHm=&)9YEJL18keG%nwUGzEDg*nzA2r9_&l~mgt>lb<1BxtOWJ@j6s
zux!D{Oqa(mAg@y=IhlOM{DS|#gP#EQwxDgwEC}Hj@U4|W9|L}afS==l-w=GofH%<Q
zgd_7K0lzi`?PQu$mjON+pYxRC?J&pJHUs`V0e{s#0^YhK;MW@P+m-?^y&^k2e)lE?
zp$)eGG9@z3M=;Z6(Zd+0!uNOfu<wRKzDvNGp1v{|L|Z6)s$`+dB%E0d5q@-4RQ5XS
zWNwD_KYOKerL$A5#@wXJd@TqM*FX4K7%iEPWZj1l?3eaa`?kfhPLJ{*((b*+K=dc?
z%1R^i%sRZI)JCw&?wDerk}vaHOzd6e*GLW=)kwEN*Na6Ddlw4V5QnZ&<xsOwnjxLa
zw2yQK0Cc}mvVU}n;tTnZM_Yj8sxLW6uHkpfkR&tj6@g-|)hmPVdRP*tLu|Ax50{^O
z#liCtaCvy1gphi`<9Gcx2SpeSKHbfMQ?XTWv~^#=jQamm8WGkV`G?TFVZ*XAIJW>z
zeF#m_{#xWO7M!SQ<?mP6#aEfiT|%|_>s6QgBuB7~mh+Ozpw-$~7i?=If%6yq<d}&E
zPM?V4iU~&RiR_qRy{{j}I|o$uO=V|xYB-C}r`RTRxbIvyuXSzqdO1`M>1_MBS9AZi
zcVhhR*7@PGwpCvhf8vJ468&FMGqW=XnWj93gZUR>d!Jrwul3_Mt+M-h|A|xCZ0gKj
zUj=2CM6S5Qnj(<hR}{MHCJT5RoluW=JSycsi~~;d^@{tEVJ|L~c`JBVNXm`8{Og3}
zc#G#`_PAl#7aPWIA#loSPAPv!`62oT_-(sn6vr4>3(nlgJE$<Y%~D6}aHC6oU8y;h
z$&K7YZCuWCGMg1LY38<5eKU{TN*}z3f<mc%tkHoSPgK!tww~<SdsT?lbiDm49`LN2
z(ax`|a!b4&ZpbrjBQEEfTf0*{QU0{<xuumK(uy_kK#x|g@vZz_zu9}$iiZt(5pO??
zcLn2_agenmx6^jOq|GI5Bc>Iu(@n6SqjnCXoy7i67h17K=>{Fz-#2wRzu7jUrOqjb
zw#^wu)u8KTSqyR`Pojq446?M%8SOs2<Uagd9~x9gZshZa@myz_UICbj`1Hrgl>LQX
zh(oh-93wknz^qjHvQ4Y}x;8hmQ3Da9kj6jSum2~cM2)l96be?uXrsORrS8KDm3mU#
zJ~#3VH9OBTiMA^)+CTdXJ<w!(ag*ZPxsiWjb0@fl+$IOws;IB6vsc^3ii`G6<#s;T
z3dF8RmCxJsy6j6MhhTCK&i46vUyp(qjON|P44+C%s`r5tNgqLa_sa$OPWpG0UZeDR
zR$qU;577Ic?cLys>HRnM{*rq3!QRw=rpcPHe$9*E6{R0jl0HP~3zV+2fS%rH_qA`4
zewWgBElDr8{wsZx8LGqje{wweFID<QCFy%9{cNS5S(3hdH0f!j<Fi!M?w^`SAFA}P
zmZV>;dIl<e*OK(tlpa$$8wf@9{6Xn|-;4CJlJs)b|D4i~M0YRr_m^qXA5!{&LOL@r
ze~rER`SDDC4X+TNdk0UABC&Rr+Y1|Cp}vgL&r`a>4Hwe$YJarSzeRc={h%=3i81DX
zX6GT(nT6g<pf;KPt0q=(@Xp$t;H(j$GnpCe8g{kWJcERqPa?)|d`5<RvNNA}fIIrg
z<wo3EK0Qm0#@v|71h>j^mTzA70B09!n<uB4BMIuyy<gwwtWV3|yU#1YgMQc5W)oH|
ze4iGkoK60U0n|>B2EtCi#ur@_9o4*}<_Y!k{m6~+>2vj6_2%{D@!-Jb9ky50Ld$Lb
zyX}XbCJw=lbH|B1sOj$vS)=PMk|bx_#M8QBk`T1EkgE*VPD#|=DKxdd7QDF&f|@XA
z`?(F6$lvKKa=W|LWolbk9%RW#(bk2YpR_HgC2n1j<<q4*B&aPxW+!kOGw<NlIAo1$
zGZVyT{+Z7nei_^Pt~)L_K!-OEUQ}z?%Ezy`i!Ay^m|X3E{EFA?z16S~hOT2KZ*|5&
z<1>#V4Lq7-AwIHlW36(1s@u27t4RH^9VRe@)awf7Kq1-`lDo8a_vvF}+k%)bH=vvG
z8M`x=BZz05%p1qIg-W9P?%lijsl4usE{uOR3%4TlP4gK@5y@?;y|JDoMAsnqI$#&l
zYWiavBT&^LuTL6k{qwI&C++KdG-HUhWy5x4S8R{~F5H6>X6jp-iH6Eb;yng^)Iaz#
zOf?$RRwC3G5{~sqxF7dUCP|*n!Z6N@qZc(yp0fO#d>OI^nrf}hR~mcp^Esj`HSOst
zK0_XIp0@rLTuJSK@TYx>K-g>?zDB0*M@xu1-f=V47?+6j`+|P=Ok9$>;@)5{X49O(
z;8v#>QkmYt4SeZpt0F~zXqVN6M~#6`)#&K2t8I26@s7gdJ%z{4!sFt?<Fdlz^1|b)
z!sGhF<Ho|HMM|Ju01FDKl`#W`PDt}b>*ZFlmBC0xPs&dVOwZOIsmx#17S}hsdB$Db
z>L`1H|8()QS&XTVZV`QiQ@f*1XXmKX?%Uq(n-cU#xjUk7+qiE^@ZW?CSMD8DdO%l#
z|0e9XZ%?{!O7PznN8kSJzA3?fTNZu0&V5sY|F$ancCq`W1pjS)^z97yO$q*+@a$kY
z(tV2%&%zcIyKnosZ_!9(=LAu??cFye_&PN-uH4#c>w7p>&L}a}WqY*RRS|FhDf0t8
ze$%MRBHGJO(A*;ax``@QovGaYgDGJWEBB}#Sut_W`j3?meKLDaWpF-&-!`X;FQ|nR
z_~9fN{}RzGW7HjQ|B)cec38_9BlA?-!-L4fmfw5)Bw0pkxqRIYsG7R3u1z1azZk~9
zTwbFWap&vxDmC{<Fd41j#Tbv&TDI-sTE0=+biRbOrBvH-s!boG+Vmooe7CT+RYkRZ
zAc|^xxKp@_x1XXf**SU>Vk?4cQBjV`4LzuF?%TZ;pXIH7WY5^iKj2h8JO~8UU9O%x
z1XZ?UHa{7gzVY@GdB*6I(ta*~MA`EJJh%<RUv^Ws*aCN4N#%s#5SW=Z)kMX#qNmW>
z(cqf7ha|vV4Y&=NwcJbaj}XA515g<#svi$vAKy_|@>f>!?Xc7izFV$TXQ9;dBD%JR
z7rRnpN=q&8Ysk<?+R~42>lRmPnJcBO#)eGP)^A)XeWa9re5voNRJLO|DUEaA0^OWy
zY|~rtV~V)fReBvCy{^~m3B2NWOJ>a*cWx;>W_Xxo#b!YeM6&J|y#{*Sl~+F5JZdY6
zD?e+<5}l{M29kID4AEGzc#w9#`o|XQrti9?7j;Ku*zr>`hGfL#s-<@Q#XJ7U2V2h;
zj*HsW5@ZrY`r}d1=#RxQHV1m4KR9mT#sa9anoLk-ygi5Fy7@f5Na}24@^tV^VU6JL
zSL@*K8^NFN6@3pbfdmkS&0j7OhU<YMa(fY#??)!PJ#<!ZoXejq&fnQH{{|48{}zC8
ze$%En8OriU2!0&DeW&x=iZ5}7HhiBeWP-2o{Sx<nPxyYAd%q)m?>2lqZ1@p-!wb`R
zPJTMp`&q94J?&|`2gh!w!8d!_Q~jgIYqsYxGOZQx8Tm)s;W@$rF#xz^ZW=WR2Xq%1
zwRguvAd(k9R)n(5tMR+vN7dynB0Ezm3qVqX9a}6U7zsVux*o!9*@<B%6iu<NIxC&~
zp<S-Sw(z>($5JVB#L7tJ9v({@!5JjD2(4;v8O<r2WQXnx7oqMsu$=ZKpRets;}45c
zSqzOisn|rS79Qbc3B}#e!SBq#8bm^4b{i7Y6W$m8ve#N(1uIg!^*GrOts^(G<`^zQ
zo3KFp{KKpl_;S>zIV6}09;*`KhzLk%jl}@uyN%-u8@`9IyttIat`1oMMBnQ0?6Y_%
z>b1=ifF2J)^gzh0Y>3>BKTr=?1JWtzvJYEtCcG~%uiE4NF_0k}6!2lA>SG?Pn6e}5
z>0-P;*1A5C?Rzz-+T(*SVTxcU9aoI<cgtT*)NyAISNZfG_*CnHY{Nv*J7X>6N%)jw
za|7&*Ny8AWyE@c~A$Z=}87vg^a9v2eae0~I+xk+fx1n4kl2bF;ZI8?j{RS?ojcr}A
zDB^f3)mtI**^sYpnQIXZ1viB{R?!5wtGE&m@A!0V*J5+6mGnISZC--KP%u+Akt&+9
zAA~A2hXHIIaUq%PzFXCJ2e{*R=N$ltEqL5WPFP9{OX5Gfl<cVb{BO73Th4qFt#CY|
zJU-*wnsSaR@D%z-CAkxAe=ir|&ci_qdySmSVDvOvzBF04s%33{KPz(?&dP=c=*<(K
zBRkOHC@+7YRx0t@dBHI|G4O7>xP5yEab8Y2J?=-Bdcxt=boQwFZ0p1(&Z6KrHl>kk
zzA%9?&j^d!HezbQ046(IX5;?L>ee@J*ER=6NI<iFq{-gKgm2q;R%TxFh>mAw??VTg
z_c?9;tRc^IFLU&WeIL@!%$u##FnIASJx%secg%=t$_96y#{Q0~+Q_Vo^0hpuqy}ow
z%){T=Zg*fKWGo8>)T!*5tFk{P4r()pAFgJOW?qip{Vb;2L5)PG6G=z@9$m4yGy;v{
zMm?`}J#o5*HsR^5JaQK_uWjJTa@{y|wl7OiWi5ZY{N=iZB4wR5THLxSL73FpltGc8
znZ4AE_-EuF$owd2hxzC4S@HV|*fWabJ5>|ksj-gRY|pM4#clM%s0&_D8E?nyN6rrM
z_Cxhhw*ccuyj{V|*v4`9;xuIzr*MJFyb$aP-<3zYJj-PtX#WupxQgKn#)T+f&x@b=
zd6lTUMQyZ>bhqYQ234!1W+bjijcto!N!`2iTi)wVcNfu2dCV6|WgDuG%nlelWqxCJ
zdOfwUL>}C$<-P|Yt4*%HgRQ>izXZ+On42l4G@T#0213u@&f~1$G(Irdo=Y;>qGKYL
z@!bR@m4bI2cq#G!ulQ`!)JtpPWzALf?U+CMkS*jBWG>ueQ;5SFyib`z0~gZ(t)4-v
zQ9Iq&G2X#rCHel#<vTpgw?FXM3FXkf72H^ozn%QuPf`!FCVow_CN8HilT$p@$FIRI
zm0&jOYx^)B?2_;l=u&8{${{P7azht&Cnzs&GX6-jig4TWb)YRV+(s&X)dRffx|`L(
zQ!sRm)@{_J3xyK8QRoeri!2PWS2J9Pcu2(+yOx(ty2r*uWp1=G&C7z<W5o!048Zfc
zMaJ+NZ*St!fXk}2HQgtB{Bj3-E!bwjm+=FXv3Fo^v4EC{8$3m$JKU%?e$}sead3Xc
z!}$hvIg`Pf_>64;n%bm|P}?AWGHV6*Oj%D#E50BTf&U8R0#kY**rx>iI|Tko^xQTQ
z+`TG)a#?2t8+#Y`^loKk!w2;de&#8+ZNweD;vEXn;)vI7K{?+la2Fp1_m)&LTa~;4
zBX;fCjZ+Dq{Q~L^+Do!OqwIenJMnI9rnHT?f%Z7M;@h(d%6tVf(*{SCRIox7Tqwrx
z$jFb<4y0%Xv{&$8!}SR|8^QVZ3F*@f!0#YZe728|h~#;)d4kHkdfCfGP_;Y};re_l
z)x0$LYp-Giz9(2#Q$cV`N%}^mKc)2PCFzeS{jW;@aY_1XO238lXlm>S{xvl^){>g9
z=8gNHEOtAG<BY*qvTP-AWY$fE_9w+2ve#>KELHbj^FX+`xp~$b0TE7%tJRFdFKk`g
zm^DpSx0G<X3FL+fwZ!hbuQ+B$c8Q=msS-pWMza~(n9VIV%Dhim@jT34D}tXT!nAsE
zJN8emh54;S3xX5hNi}5d<R@MCdh_1p-BdR)k_R?37Zd=S-?|3=RvEIa`Sf6*?~(IY
zExnj(J(b<i$O_qDGY})#LOpm9O-mo1>wo9jujJhP)>VSBV;q?E^I70=?F$JWXtm0`
zpnP$;^(8<SpRZwmoUj+}+GP~?WcK%xCvc_q*Ju~@x#c{yAYjtjV^D#w@8nUtl`}PL
z!lD9Tx|6)IP8+THx?jddi_iW3P=)NB@E8N<#wa%c-O8?*;R0G>R-bk{E)(OteN5*%
zj;WrqPt=a@r)x(-J8Xj6e5F!`SJ$YeZ_<+8V_z|nr;s8djdD~Pfkz2-Ab*<SQ1DM*
z#k)A|u>Nq`;~AkBx75?_zQy_{<@!QGim>MpU?-U);FjnAX&j1kWK%LFwQapW`Nb3o
zLq#Tat}XLVuR=6*tT-k&;J~AD2OV&9F1ZIw2^~VB>5=2&NI)VE5MnNSTzx}sJZ{BR
z`yNiJE|~4t#DgxM?dm?tt@pSusfEb)i&MS*9qD$#AyxH{$$Os}bX2WgDshlyEk@X_
zE;I-(QAXD$oMfy{6!BovxxX+O4)}nvWIj~gxe+WTa<RjCzOjtVzf63%F&8@|S-$?g
z_yN!8jk;6W7$+zWSW2YJRBwx_%1ut(1I)cUXK)x{9HewA*YxPh<*~X)=sD7KosTXM
zex9_ab+hy0=C9Jo0qN|8gMfxyPx1fX(A!rMVe%Dn;^{lC8{qmMv_ri8SmYGhzeEI=
zbngpmX-`hJ<m4J>4d2+ul#`4nkWtIU_C0x!SpJCdU>~NFju|#soOH!tddw~`Bdm11
zW24M`Gv@uMAC#_s<s8R<ESXu^IQO;QZg6g_XIh2ha~&CMSZt8);H|r<Fg^#k@ez+;
z?_v-?z+XH>W=C<O)Ws;dk!m!UUW~Wbxske8BXv*q&PoB*bD(lDp)eXJ#_iDZpfFBt
zDQonArP%@ZaFyD>GeDSAX|T`(&P1ns->FV;Wu(T6c5AmbUO&W4^$z$Y4=Upmsor-=
zSpkC0hAiH{iM+aR#crZ}ln1;h;{H0oW4mHRcXfCpNs<5*jB9X6B*iN~O3>O4;?Mty
zp>*q&I}qJ%=KRA#E?kFu{^Y61lZ9@rhCHi!iG;SH9<EoMkL)YhN9^uZWzd}RrCdHa
z`K!IKmmc)6N<js9T-!Qe`}|Otm7@=zi#FAxLJp~Z;!mI(+<S~O$wfpqXYzMg|I!nV
zfyd7jk8gm+XXy051gRE?#2?9ysb_4S?mjZqKiWR))v_-aM5lUN%+GZ8%t5rhjoOYW
z0_`@qycomS2HT&`z99gZ8grYcB(g(yZ2PRd<zZb=njN}}ROVM94bm{x*~6w5;@!?q
z_rApx0DNbB9n@Y$<^5yCMW$snU`?K>M3?s0P5}39*;noz)5n%89iGhjV^3O9=Njj(
z>K%6Q`g&o6;HTX`u=P3pYjph+s?yn8M5=`@l3;p7SBzx$A(j2BsM5ig{i{d`zM$B4
zx^X?kL8IY;^`6dhg}AtyvMKU=bj%FVv5QB?qhA~nhd2YoVe3pAl->aBh<hA%D-ByG
z?H7hi_o)d_KCnz#eh$t9tXHhMGY1t$+K<8MuGW%~_CTs$jf=C4Y?D`vL+DupHmESH
z9+jsThOXyuX5CnN6q=e{m_M!}jY$DDrJ(r{q8;{W$1PJ=2hf-(bF*=x=Y5OOQVc$U
z1elNiVSBFF=Z*&86ovu@WdxI73erma1?{#==Na)>@FauNw($^7#BgiHX_x|LHJ_Zo
z%HdSBDUEb<=d-r6vnOI@RehY;JY?}jU(b!=QZVfeb${9GEy;>XvZQ+>(>r;Rd?n)!
z#D8vi&G9+xHK(0E`w#{;(#Oz8bnz0mxF&I>a|VqDwg6_lL9SHx%#<CKfPEaD9oQ$)
z_D`5{o-gqkgkZoF_RsR>@A7Z&malld2Q@ZH!0C1w50F^w+jmxS^qo6Z+eyKM+h@b4
zh3G9mgNw7wzlc40TjqzxyZZ^FC$oYv=QCt?;+U`JBkAn-YPsLWCdE<kkl5rnN%cpy
zjp*Ae-u@76w0(q4SZ6ORlUSOA3i6qb&9Lk}73=NPY^*OcR*Cwuw`k(pK~lnWJVZ9r
zv#qYLP#4Vf%KDf%$eC)ZPcI;HxEOwPW&Em}Xx9T+cXTBH4f0f6)MP9^^Xs$`LIu|w
zTLe#jE(AI&{5Z$~nZJufjW0+h)afI<m0)5}DJJ?vm}ox2Apao+6o3e@&#4xQs6;*!
z5iT$Ca0(^cC<CaI>KwvR>*2sXtxzcJoT0yG&W}a$(SA(AG_Kznv2Y((hpgdg=TO<0
z;k+25I(;!n0ibYp^(}AY5Mb|_#^xAiDQ9(S89Y<L9r|c=Tsi;rN7n3xI){6RNhXBQ
zmAlcz(b?EeG+ox*8lS1f6VLQ~6N5ynzDd`LSWm|Wb)r)BZ8v#G7*0KjAY&&8N_S!u
zX1^izbdMiqq#r;E_Er_!6(G+i$aT&vE6|5WLAD=|;y$X{i7HG3958DCJ`D`N022DG
zwG={^?)`g##ursgQ=X7$iks-Jy4x~`Xet1x%pCpg0axD|i@N|F1ljkB=HFw?zx*Xq
zRKodL=s=-PhaN+B@5wv(4jA;!d(pq<3)5MHwSZbd(KnVSA`ZShW;-(M@K67~MI(^M
zi-K($&FpaSHN2bS(xPfY!Fk!^ZC+Y^Vcqd@Gi_e?to{+pJ(W>sV(0H<SF8Y{$FJ(K
zHe)qg>bgF=j_V0Do=cVK?6I0l{h_F|b)d(X71gP1<;}m%pJ13cwz_+5>GV}rgjq3@
z$iGqoCxlW0YM9sPApa5Il*A9^res!%!~~eq;P6C*_PGdxWs?q8TLU|JereKPdftom
z(Hp$JK50i50MBMeTtRC8{)x;tpc7l?FKX??v8AN`WcK=rkY#rIZ2mPtu@*{n5(Kf)
z(FLwq+uy<0)P#*}AEhQNZ=5hMl{@HDE*-&=oSwTekvo#32i&wbcXbSjwZ~+ZlbmCg
z#b)Q`Ub@^erpsP;NaxN#mdz(zX!AEVrn=^-f+1c{RtKc!tzzY1orq6U%n~}X*lx{u
zQGMAi-cEJ8H-X%JrhI8gr`d;4X-<uy$OVL?mu+KlOTMu@Xr!gawtQu({MA(3JC)tP
zN=;an$jnV-I#VCM+_3+$50}%4=e0ac<WAhwG@(<VKMYF|n92CvO9lRY<tD6mNacn;
zo$8t^h?^!XX)2!!sd}8q0pwe`zKap8gc}uPS~f-fm^4m(NFcEq>XxC;q<5%nu1A%3
zUD%(yrV#u4FAe7_jrK~Ff2<yD<9f8%dbGN+Jg*+5=u!87NtQ|lm>*oVwrKj_-e1sH
zY&^(|UvOo6XJ03i6TkC=(~1jROM&huL=$2A+8-vKLn`;^RAI)Qg3&1+=@cobC4lE8
zvyaZ^NB3gb-p^o*nqh%6<rIG<B^BQv!`fYTy-YCaT>xN{Xjo(Jk+G0c@LeTn`-iM)
z_Hb^{FWa^xP32KHdJJ;gkL(TMbcV5B*>BaiOspn&i}DQ}c5A@E_>8}joX8!6No|IB
zEI91<CIczI6pinY&hEX#{t4~L3bEf9Y!$UZ$`vXVktT@L@fsrs+4lI>@@7H)$@(|L
zm>Z4ULL=RKM*J%V^}C|9;Od>%{ADbFrS%p*w!AS*y}AP!nR;Ea!k~%$jDK~aRG}+&
zKIl`|pM%t&2~{M;@M;__U2yJ2C-X`mjOe{VluW}hy74;yzr-w@r7I)rg%fhRZ=(r$
zamf6Oc1_hSZXVRL(n-&UAZhB;XxoBRVO7lrPw9CIPcQDi6s|!Dk4_vjEc@e$X@~*C
z9z%9j%~a|Vh16@Sc`^e!K0AGurH#||i(G?(?M$pF&845Rfskc{*@xws(3m?KHv)Gx
zqP&UtI4Bsj?$b?;xoN6d-)_~f`<4n~?wg2B--KfStu_dl0??ccU<$l6SbOgE+Uk3E
z2RPyAni0C5e#$^S>3(<(OJv@aS=Vh}4o_trJP^bxnvse1(webeZ{}FQGz}IPT=$R7
zZS0B}Ho<>6_YJ*{S}H|#6Zf$D{N$JqJjSQFm%4QwbRoAD8Fp8btucFD;2J416kEnb
zcG^8aD1G)ztWhAG@$9<|w#p_fPsHzDAl?kZ&Q9H++s`#S6S-s!EPEA>XoST|<Qk%v
zi5?z-SHT=_%-%0BB6~WBaQ1H36Z}5bOc_D!Vj&Y%b0ZM&rXsw(!yABaG{C(*q$l{e
z0!-PRO!B6HpHu%l3VS>(XkAD5<ZFT238?k)X(zL$&{dvlFv-9ThL<Kdhav+)`!47|
zf~dNo9ocm;ZxEBA5qG0=?c<yA83%(d`lS{%&(0r$2H-BeWGL?e*d{lkc#CXP8II3{
z*n{wsxtxCs<?!}4#mir2nCmyfS5EEw5%VAR(Z7CUx~{Y36>PEPb)U8_NM+u&Z7VT2
zVocNr-$Q3ptX`hu9g`qB4~;gDTo#H13Ck7lh?5}%uqiuDVEa+A8XB{YtamUA^dX$~
zw|%w=v7dp<r-Vy0sQ^2CP@DmQA{q0tKz0W3MBM^ZOhF!ds`YO?DhfmY*puem^F-^t
zMy!W`wM5$^8jSMzw2v8D4c*C&*(p^(B@hxB0+SIriTeAU&G=YnwR;Mh@%m>Aq4#7q
z8n%Tz#TCjjTy?|9Yx1Nr1F&+xw`tUP->3UU?HZkl3d_MxfEyrY*x7XFnUsbG{Ebg$
z&K65~SGRho-yLgGU8=im%F&FX8_a2LdIG`H?mm7zg@;6Tz!l{mrzYq&ohojjkML0^
zbGFb?-uj|U$K@;2b+5JD4t2<<EgZ`vJ_Gf-UT>5e)vLRUM)g~L1k05D>0inGv8D*6
zI@X6z#p&Nz!fKK2LF85o<}}f+UfAj=h6O0hKVSJZ@83>j@3(o+5sST<sG6`HNV@`8
zGQ@hW6s-qR6U@OPhFQ0;SND#eWMVeUX<uu+j+aDDovQY5CQc+jZTlnvJanQ^LjV<4
zQ@|ouLDnLH?`(iX+=ky7JcfiI62YrzK#hi9GJepeQAi33d0No;M*N0~!YxI_T*V$#
zgh@<W4a-{JfH~XbX6DC1fstu$TTPD{j#-+&)Lh{)MB6$KsD^A5Js9m^y$=Tr$d^#&
zG+1#rZ_ud$m&)|5F^~=ZkaUv*9%}Wqsk;|YNDOI&lo-hjE1>GeTXpJnf%dHeI&5Sv
z;s+5atI(2!k5NN9DGh*Lemguc_CkbwxFSTnE~=U~n>G>b^Df$j_$Y&QbJgI)aFY$N
zypOrO2!YB7&sZjOfVx}xWh{_(Cb$H)4t^lHE~i~2mzE@*MxTpX^S-Al;0Cqd<saC1
ztA|r~496Ey&anQ))umpgGD95(8Rr6_b&P{p4enLI0o)ber%RI#7c|#c;qL9C&#F%-
zm0G;WO@@g0=vC_rlnResDjb^K{b1xUr9z!`3P3~_68Mu-h=!>&-MxE=Uni?QrNn1E
z!&*#YyyKb4uO)_GHw^N=Y&onh#RE=Rv#^E>Ko|$G%<b+H!Zm{~U<T2Yq#k_Q2r=U8
z!Fj}8idg^Ipdi$KJxeE?KJwg(KB5}L_$0Bz`Pay4d@$bkNrHkk<_2`a(-BeiAh5vK
zL6Wf*NX7H9*psa+f^hbIqV<vd&sfEXYa6eI=&7#XeeZd8%?t<!k}5-vt?3GT4yivP
z{wY#O`%R|s=9F*6A(ug#NuSU73$!mz9ZI!oo{RZOf12_9WCtEYFt^B0I@XAbSWWSn
zx3d7xFXNwxo<wdRQ^%wm#NSSVM!;h~qXW&PRUn+aps@$7Eo@zx-;s*q?XLh_RGlAY
z)p|R%#;3h%nv++2is)GMt8I@kYW6}d)su`1rC70;lXw|#!q0?V3iBl6jFe~q=i7PV
zHl!1B5z@N1cXD5J@P*}CvV3)_?xmJH^Lul>sW?(3XE|nQ_qoqP#vg|&G37YVoFQyc
z=kdANQc_HXVx^zT9a(v7ZbWUj8SMmLs{Ex?otUY%+D^rMZHiH^RAyewE7*}SDnHSB
z8(ptgu)0x9n+{IK8AfLa`=GMlsqYGv^0fkp_>7A_RPY^Bf5flOPh)P(jUHC%&}*e=
zPK=(`&Twj&*~;@TZ@^gxg|Pj#e>ln~U%&b59L0~)O10jIsf321+5!cCA@Vhzt$Xb1
z^OJo;z$*b&tQZ~V+jUKbt$cn11;r>ooSF?C6X}#kNB$!cy-F-hw#wk-<Z(d-Q=gw&
z+hd}VSTT>ptqCTkuZikVI@yHE?@9edex1>Jz?LKV17Vw<Sn{_sW<`(`3EU*D76k4Q
zr7*bB5SIxhWHBJ<3&nEx<25}xWu?`#fG>_a{A;;Oeg@8lF#ktmkjA-bUtCz~h}XwV
z|0$lqYw~p%Ty0FA`v9k_k6-yz4we_<fDs)CoU|i#5HkFca6{y`WXd-(>tOGu!mwQV
z+I!xo>;N7@nMJ8iFeoV^GvMUVLef1@+$Xk7%p@+6B|Vh^h|l=@yOG#>B@|oF!Y;hn
zx_VQ~LMy6UW@7Q~`Ji?L#_SpUtDIL0B!nL_iQB!aWi}b-bX6`4_-ws3ak-O>rwqGn
zMN{_Zl#_`-go#3RcXdDI$G(`0ANTi&i>G+}TrM8*@cfQYdvNQDB3sugPzsv+e#zUq
zj$)p%!ViukQ+^$ECndK1Hc}<)P^U&9+CsfVai>#W^gY-uDnSip5*Rc^at|f3QtC@6
z_=vG=#5bi1cU!OK(cgZPs3Vbi*NUezACs+gVp}~JL!@7Yb)_;ouP{EJ0M=0D5#B>h
z$J~_0I_Ed%@+M7ZWxhZ8gKJ+dLdDM=D#~~Bs7QKL)RQUyrZ9I-arP6)ZktoVw~CWz
zlH5HzF@^&NjqYSu=-cFNI&yK!-Z9H6zf|iotR~Jg0nc?=b%rJB=h?Carex_EmB@TX
zOu_TBvnN8m-2F`2*rMZOg;QgwjLqa6pU&QH>-xs{F)zxHXqFe<?L4|>aB%30SbH)H
z8z(F(u4BYP;K3PjF=aLdJ6N7Ya_$|!^<2RVGCopgqpU?Q2Fvvh)Pvo6Gw01rxyfUL
zhnc5Q>aA)iH+w2S!R^H#?r<N*(#;^N4-@EV5}G@ENGI(yYvvA4<c9W%-~9qc^%=X8
z->*YiqtEBf)+aJ7L72^~x;DjU*!Izc`KbwtI6Bz!c!SMk&TTwl37B)H^1wT~Gr!)v
zBYQthjuP|ZXaT2Rbn`^LX1AD)#OFjE;pR}zrMi%ZE0@x6+bSADBh@MP8vXd;B%p6_
z?Mt6>%^Hhq$n#b{sYkAdQ`oMC;8<5)Cr)8T#(Cv+&-2nh_IT@RT0g&I$&|0E<PDUR
z=(fx996t@q#a__ql{&ZRc)T?zZfwe41^a81{~61Vk6HiNW373>8IR5GV6~#rpq24M
zI6=XA0s||?I(}YxvizmG7u~rS3ZZ%tecskesd5hdsO(hA>ZLx_+u5=8l4>mfBvsxF
zKFj}N2&7zd9e38pJFaCPNFh<HxbjmJ6dd#_wnZK|m9IqI(>gfVHA?i?zbdg;l;|&t
zRpOezx#F9zX8ifD;`3WNTcr!ogjV=XlB+Hl%KGe?6YV(HqHM=7O}t#Ql%;ja>~A%9
z0cfl{gv8UZ;1vco+ff5w($eK<@;6TCYJ8ZC{Bg!J=O$RXE=YBCviq?q6+dQU@VjT-
zj2WdvRDK?pWLd_%(zG2PT;j8)Qqc02Ya2@z9qwY*ayV;?4IZXiG{~B#FdObyRSI2W
zGCVvLSoL9i(2wq<n823e51D<m+@)z5rOcY8Z;{3@jZM~VOvalw(&w!-{1B@^xQ-0A
zSP3yWiSj4LXY4{{(#IRS2-~bbw`}ZcUF$t*?a~n$^u}>Xv@ju!<oLI2$<0jMmb+v(
zjjC5|RB3sT8OAx8jZK++kGW{K&vHqCs$$0G)m`X|>+{wrsXloB`A-!dEy1OC)JetE
zsjJud>>;QRW+EG1W6Hw+!^Dl6(Xf%UC>jEB&YOCxecJOU&FO(o{9tq$ji=_BFZ21?
z*0<4krktb_8A=rFHOlXeRBSU86?jYI$5w({r%=#Ui|x^8<F4@9$6DXU1Gh%i9%|KE
z`*z1r9UgsFozKTw>e^1|cJ^4X&w62L63Qfsc=L{F-7H4uf?JSp!UmTy`&)T)q~P)H
zZrYj3i+DiFoss^De;wss&*C*W7%D@{_#K|cUYSc0?EHZZSofsQN#R(!j9M;Vph+0|
z9<oa!YTxXnbV1_TIqC5#>HMy>E37LYUQ^F&--xIL5|KCe0_&oN$PG=st(0O5M09mn
z2Tx__#x84pM=?n*2oYneBrEX}%Q-6EcjfDm+yTqu{lRLXUA%HCjleIj7s_MLiC>n_
zt$Wtkr3IlLySSD8vC%|OKV<1e2Pd?ME8o~q_ioF*iATp!)ZFoxYGXXJI{z@gs#uvP
zJ)3w`gVc@B?-)g+eL8;<oFsbrGb@HrzgE1Nk0ZsS?eCMlhH=b9xij5j91PVWJ5wwx
zky*qvU@KM%M#mDKhsE1><T=F-b%BRTwFsxTS8Y;t0j%;osAQrk4$nWSX}*9_W3$?;
z6hTw`7;cc{KNv-DujMfH!?P&ru(H3jFQx1&{B{}NBzwndzSdJ&a4sp3<V4&g*f8d4
zd};!aASOp4z^d2>(qKpWK%LxG_ub_+2CnJp%_(ZayhO%iX3Ym7A4V4v^}}>9AsU@+
zd;&Tv;$f`v`95VA@h^_RZpOb1QsXr*7IXuyt?I#&8g(tRT}}x%f_C4IIL)ZwyHCr^
z1v8p0%1FkO8$kl92ug`DAcW0~nCCm<3-$D0`<)ddR*9L2MtbmxI=mi=xXEe=_;?lQ
z;|qlAvxxsh{KE4qu_!lh#w~Q{EoCTg0Y;i3_=;-1Z>8gDHqQn+{uA=QkpE!U`1xiG
z2%o>^9gQiEg8_XR%~Jh^0j&_vSsC;--BSvN#bx-^xVGm<>m3VM#&y9P_VT7)RtGQe
z68u`Ul&km>-si=<=y`SrLtb>m<BW9Mg2_dUDAY4KrP^Fm4`HN@H=-eQbWZucaYqqg
zy?XuVt6DAnOa>k?0qY&<ecuK3i*4E?#i@bG^gP3grt}^=%ujR1bhmK+k{#R6M^S-Y
zrPa-A2^@>af1P=6`l90qd3oYRd|-cghV3VJEh2f759DEYylUa3<%iAmWcGiI4>Z>#
zCF>yoq(5q=ChA0}UK&vLQRzNuzww;8rbdB|y4q`I^P|dne5ml)=^rx>IqmV`SrjIV
zrlN9F@p!n%h*a(}X7WR3kqv$g`)-`DF!d;z_>-~}#=#&0`Y|tf6=+m28K+S#H4q7k
zChhZvD;HFycGHY0^I4F#yf7lIV78Roa_vVhr{M!xO6t%6rSLL}3$Lfrf)KM<zgpt7
zJ1>m&Pmzc8W%k{jz<<TkI;0fo)AF!LvSxD7sc+2NiCW4eWf2&~FgP~_rJv6+moBzj
zl%~IU`y{3`AL_jpo|#v-DBko!O2=mT149$$F%-A*9P;B--FoZM=p_#RvFdq!QHhAz
zx_k$b51WaqKxGsMMZ00v?&|5hcpqKsDYM7fDy$wC3*)bmy~@^G?M4}*{s&e7)`Vpp
zm=*iJFnPP-4a4GBJwb=m)lsZ7%A0g;-`QF#U0PGceQ+T^OlMw7XI>We+2LV4{PM||
zW7^^uq%$`e$Uzem6#av&W91y7ZfKeCVekPAGfat?#Yz%kjN{qbo}oMInJLeBElL1a
z0Wh2r-y!%~s82<F(y=MqS8Os`QIWsB9P+g=zuQleUg!B*flm$$cM(cB?^{wxeuM&~
z`f^cy;=5iSDarn})RD#IrlLI^pMDolQcS#(^{J^qkqWk!SJVi{^j@=<O?ufBJjV;T
z9gC(VUQoo~oSgU<Wb=<j5qY&cXOIF-_Mot(WR)ej-snrR$!HA!0>xk0oW5Lrkk~a=
zG42RczEoJRl7DOT*7#B}y;Tmk)!L;;7m*ueRUo(JA~(7YQ-ciO+cLT3uQFY#9vpYb
z;Ah)Z|D%z5mZ{x+8~#QODJ8}`_9dr2JjjQ9H7}wrkyD;27U^S}^*7L{g*m!=p%}t7
zHLH}6y{uM?w(Nbgd5iRa*5_AHtM-@2;YA=LfY;tz-z2?bJUvL~uBqu%6*=d|L3(a1
z>C7>g(3Dw4CzV|cM}C|cwu==AM6zo<j9JCSm_yzfEO_kGO+@WTmP?|I=ejcZ5*NyO
zU@gLi;N&{DL4sBGimk$#J5Gq{H}^&1dR1dC{dgrr)=qIFdMOVi*hM(?8&S7fIFeoH
znKeVt0++kcGbQ}%o~d|A&)$Kv7x(PF3iWKtLAjF*%H2aKM?c3#peAKc_?ET26D@gi
zQUwv=BZ_XDuA3L1z9yz&8ER5p>}cV9jkF6zlUc-U7$JHaFHy}FoLN$@F`>drmL@A&
zUcj=DcE(a~(q(B5#hRlBW=>xjgENAIrSk6^FC34g95_j3wUBo^E{t%cHr=h7um%mJ
z*6-9L@b&{dn8*ktz-vrub!?h)fRDqZFX1{f9Oq(E>}u7XXGDq;@*+i98-auUdwOcL
zs1yhhx+A|0B($iaB1_Sv$Z>L!o)8+~g2N=H&`(5-lRGxFRHr65LDSJWxv!h)@vBZ^
zBn`E6ZaF}}VTc4{0jOLTis-Zl{Q54W84{}*w%oUv3+q$4^pj9Q@{7=^)E{+wJ-^mv
z_1(H{DPN@9p#~Lo`-sKHrG2+Mx7_V6Xf4D)zs|u9?Q(8>nu}Z3ZhA&&jw|RJ6P=oY
z-CaYL!R@!o?Vi{o^APzhYGA^G;$>ZKE-|8o!h+ft-pJ$U6{a?-v%-3$flhvWVa?~y
zD{Qm4q_F08W*f>7x<U8eVd#ZL*@<WYy^7CMYD0O3pN<ydCR352bL%jl^~EW9Y`Jui
z)>qAkP{kIj*a4xQFaI8uGuf?j2>6nMe+#Fgi(YM~uG57R`VeG2h^@Ho8FhVICaXK4
z|3g2yh<#IwV9`#+wVga5AMOBvBL2wHa(I7Xo4bzr=E<7EgRHPGb<QlB3eTfQqQdf{
zUb>Nwc-PYqmsE%G7Ch{)+6H-qr2PNz6!GV-4$UMD7OZ3JNPffny=xcekr4>CsU))Q
z?b7%JuW+WoBLrrmDbRz_d2VK0ZsyaBpSib$%&&;ar87gdpA%vimI?0YUcXZ*WjxNY
zuo7Me%M4kZH569jpv&*XPHRXHA^(V9`5F6$(oS>#;31Vg8G(KU4qK~vqrH)N^m`C3
zVoNi>r8AIeQ0Qo40}cr}FI&gkVn|rzEu)soxuudMw<eeuUj>#Ie0u>nzl65D-M;|W
zRqUG@TpZ2`;X=`9;f<OOJHg;;sx1Izl;0Dszc`4E=l(lz%|j;Sx3iY7b*j%5G=39~
zPf+SIs7?(@hJ@_{)4mVmAn6g}y6W&@44zpNp+n3boZW7)1!L6Ik*;Rn6!@=YuxM)W
zmYX{G#DA};yIfO$pGQ+q(o~5WJ?#N@njfvCGr{f;iGZC>1_kG@XTPsFUPQ_L&X@mP
zns|D`xomc{*Hp2jkbXB<zeuw4wThmc_RCSNoo}TDG54wZr9`#ml=UL>7h{E5GfG^X
zoyv~bw^q4W9nYo>f{Rg3=+0g;QAHJBS66_R*)PP$FYBn9?3`I-lI;xz!@#O_Uj}98
zOazst=ejWNWYE$JQx=8z#B+5jcgfgbS0e2R60!5}N`l^&Hb;AM`XrWMu#qKL-KqR@
z!bPvm5a3>{Z;$C4T)P(X*!gC**^+f@3JY;$J)71hR3L|6s6>M0o>)njoMF%_7dFPz
z3voP&Pp@Z@#2FK_uR&MA`43WcW*vPEzN<$wr4WZ!GnGG<$5O>!#@jNLi#Y>}b7vnM
z9H#v6K0M`7PJ;c4Und5;TE*-i$sAFzI!ES+#bKz<_|YE+?>ykvH=K`}uSE@eW^P+B
z+gwvqq{rh07|D^kMdAmsXIq0Ls2QX+4&v6i&{ND*>UQgdc@L_b)B2-wz%I5u;|#1-
zB3B!a?7f{7V(G-k(6Iru_dDkC$WEQ@dquK5Z$@Zc94IIj7+jrR1J|vYvVwQDKBsZk
z=%jLO8W<n7m<qF6U0}fEvCT9Dm^gMEI61&$KKV$LafgazVl`sb+CaF;>9^o}RPZ)Z
z0vcB3TBM6&QRKMHg>AzOexi_z(@peP@6{A@eY?hp60<yOaO*8aK$m)ZCAFxxr#>b<
z!mlq%_0_F)6$Sdznz3QeqHg7m8m!Tvr`Cx}<%p@JwiZxfcifqqRAvP8Xl2mv-|QH8
z1l@C%GpjK7+zf%EFI3Au@kdhVJjM}cVA63YfPrA$8K6|=Z%H0CFln$YDV@%&XbZ5a
zu=nQT5VFxGR-ySwgu!7Q8&VS%p)TJiQy%fHSkfU~)ixHIMVy>UXLn6(I<ZPVPf2Et
z!Vxyrt!tUnm|2nJ=#ddYrdwPG(L-4Wi4K^k`E1)Ls#0MV;y5slhMPc{YF(zH6B#h^
za;<rU#=#<ePF-m~{roMASNzPmMdZnT%Q$ng_Vk&k>uh~i)Uo-fF^@!hAgekYIad10
z%g7ZGBFCQ2fZ6{`IutW1qC>9P7)>|qe8l~rkgil~Cf+WT_`wl*2nOeWA`HmpF<4ps
zmA&Y|)^S_VbRQP=pOJXh%|EEH90ZIre^}fAY?k6=<j(cRmQC5uJJUWs+~Ol&wjjK1
zr0>-oOIq&tv-su0FgcjA7mmU`sRLV==&MFUB#8qHI)z}e;+-i1%V@nfW{HM~{UCRq
z%<Cs5>-?UBBzST1>|ZoWPi%FR?hyzu$$vFUvth^o&M0Y8Wse%I4fumu5*BYEj0#pf
z?6wd_$u7p$=4#v52^JKR{m#OlNfGE0T%P~kWy|j@{`|3cyH0Gt%#5(=*+)3^A9!Eg
zO4D|p;SoI{oU&z^7&t~0C$p6j<YmE2@Q9%1e%pNb6^`CRmiX<5!~+KoP@#7oX|%#!
z!6Wy6Dpk*#XPHxR9jq`ut(KPY@~n}st?b32z)5E<rV;!fq{{s^X$;W;FE4a+!8N*A
zgs<~5);_%$t6r@y%OH&uS}_H^!<{|y+;vE&Om~AquW4Ifg!N6g`W7k10#sYY=0&)e
z3PH9Ew%N@+Gf`^~i8Z9e5<(uX4oHB7lIyCPsUV;P{;UUouE0;0zf*_#pHnLX2%O|Q
z@x1w`Un<OHFJzh>2MwpPC)KJ)our0jtiIgJyITiDpoZSbxWi*&i;{&hfv$h>N-s%g
zNv2~Yr29TDY`sV5N=pqm-Cv+Uz?xdvg$+tteD8eC`^lkviYg4--$FaqexV&gmcl?S
z>@x$M#(t!!YResJtN6wOVL-c6wiKeLy#7K<OD%3~exrO3d4{zW(4a(U*UI^ZrCnek
zyezU=0S!U9a&yGEAqg!up}bU)J$6Fu7v6J7TFP7~p)=x9zzh$AB^Xk>TAvY5mT#m?
zxEdF6MkfKBG!Qo&FIG9RySa3HSUfllj^Q`5dODSPGS&ND!N)MLv{GOkr5LEavs$4b
zBqIE3AmN@aXGto~mCgn*<xn{4beu4M<~iyW#T+%>alQlr?MwDNAFg}B;b6D-csYuo
z26&=-y}-YYCmOKrO@4<2#k#%86#AlDuHrGUaruJ9AY*eoeD>OUA$`CVxR$j4faO>|
z#Xpg}2-n?b-%aB_fV`=+CzU2l%<qR&v(E3hb^6f4XY}X%9M(TJ-^B2XM8Zj_>?O>g
zdqp-qM8d5kQD3Ruw^SZ<F;e4Vi0=Qh^5704QEny=;#Ur^1!S=X2|^(=(;)Sjf6rFg
zQ+Y%^LHQ`CC!JI1>Aj+vEvYAy3-liLmjiu~egfS;wWXf^@AVV$|1I=1gM~(vR?wA;
zdaB;#zef%mPXyCV-C!ZJu7_-LvZ!e`vgihAfqeaO8jmE=xk#cROC*a@)F;23XYOL<
z+|MjN$QAMrq23;q(u}M3AAcqDu4YbgN+K#`D2H_hqdGWA0+5+rT`Y&IgArk}sfe05
zaS7<^V4va-HpTph?TbIimKBzG_r9nFFVa_%68!fct+)HEDeCQ<ozz?W$tU~Ho=m39
z{CpFh=pqscr~DdoBais5Ibpk1fM~{_q<M&aFwcFxr3;;1CMgYz9Gr$Sy_}6vws)Ap
zEI#BrnSWk<=YP!n4*tT(Yai0b4{fr0Ke;Z7OA1}^(BM7BL%~#U!GZiCfsLoc7{Y#v
zN5kKCJ`zay-S+@~wG3?+1WMsz`P?YQRQNmpV^JH&c@wSp@aq`O;oCF2`Nb}J#uGf_
zrBFNf(xvt78=6nkK%GJZk_>9#wTE2;OZbh}xy*yAz6)Ia3XND)zs}jK{`Y<TnnG58
z>r#9yxI{)L%`BQm(fx>tF;ucVk$KPgFg0dAaj`TAGfmV4Eo9t@QKS+pf&K%*?s*a+
z&3G;(Yx?RiN@cL?pX5Cg@3<NSBsfkzfZ;?gRSm3>DpDD5pTP$l5~uux#DjOEKZ<_9
zsCLT*p^}g0Li0rTSMeI<5Ld?<Kqvegt7n1_G#^MJPGhGl4uTfc+kj4sfv`7Wt6H8=
zc9PQ-Wi2l#eo_B{VTP8Kv(Qo6d>kit%FfFbLGy;f4=~O9D&&<!CD84A8e=W*vjg5c
zIydl}xuG(o9()doiE_n(%B%;N$B8!iF>NcLAp?nyG)_nBPIZK=CueFv@DT)ZEQ5V2
zj%BrY1dUnPT%o@8@jxg0l{F(<Pfzz#lpz7%cL$w}Aoyd;HN=ehCL*yU{)r2uD%W;m
zTWEONf|yPYOuN@myyB!OBZn-(eNMXBnW^sGlfz?u67g$0AwcSlHN@}lY$%_btXtl^
z2XUhkvk{GDm$5KCy1F47tL$DzQR8b;9}}vnQ~HC&rc$6UqBoD|Qy;&UTOzE!`uP2u
z66G5SxkY_PR_Z><vdh>bxVSnyBHsNB#XY{WM-J<K^!SGI#SP`1ry=0uw{!x^{8`4%
zO77z=xY1-DOK0XPYPsEmV{v0ec=bBzl?r;@Bcf~BPbXP!>0~rGRNrnfjKc`(c6q69
zKiKKNIe~zv5+_eQa4!C^5JnYj_ToST9x4X#D3B_Kod44wKHW5nV^|%xQl{;)VaT-K
z+REPCMlkbZP~Zzoe6a)zuD?ObdQdI-*~9=@cAfK|hKUeo*LeH?xB{BuJ{rFYtcDr+
z+>}#+x76grRb=B($URnw*ayF=CZQqoT9@7X1DG^!HtC+5BG7Eb&oQjA9%nbi&icHC
zvl4-y4~q1v4(UBzu^afetVsS^nDAHPGwvh5-#cM%zLQ*RXgkBS-!h<)7!f;Ss9c^Y
zx=@#_h|Q?Xci?0GjEljg`o~^=5G=4GI*boHAB3+Ee5&FTG#^%8%PyEU{8xtDtImSt
zgYVv9LPU{LB9VC(rA>&a8cED5GUbmWJ9HU*jhDjs{)q9-b77I4b86S11C)j-+-C)b
zJ<bOP@if1>Sa1>h&P0nJM=^%Mov#$`+7+>z@*1_G^81c*<!cJ%^9u53^T*=|aT#w{
zqGVROrY{$MusZ3RKRC=6T=IMCX)}0;dU`?F(>;7o_ajq&A9RBt?XowJy=YzAsRGYD
z5@!GKcf|ylzqrUhBIx5Ph?Q0#3q?@{_ftWEAoD+<0z*xL2T+s$4!>^w5Q>e*-Bd+*
zWd5o7>LU4RrZQbIS_T*1Zqs#7FnmiVqR!3MQJRE>a4qv^1?SRqVH|%}n5WOYsV;9}
zo^D5pNMF}{ov6uk$*{7lU1ms>jd1q;-sr&YLz4rjyPWz5@R4Np++i-a7P;-ZZ^`EO
z5w3s+UIpCruA_j<tYTJRgIWx4=DM7hM+bw@;1(?cvO8B@ycfpZ$%FiIPufgZeJvRz
za4O!pkIE+!Wa`-J)aCOAQ#hHK*f*6KHHc*;qoiYpsZ1q_Rmn^~of%(kvptnDP08JB
zN1?LdYFY#O8GnGbXiCa9%UDOW(F)Zq-`w4U-VaL#CGCGov~~i~X8W59TVLuiOa*=s
z%wb#$<CW9AJI;vP=|e*2>qh^6neO?I=3f=FJDPuW3oq)<-%U<OS1L!~NqS%biX%Bf
zxt(X<Z{-Hqyi+-iRmW(`YCyWLQNR2G-sazrt^J<&uboP2@ab(f`!@WgX!do2Oye`*
zbh?8~1q#W(<cQz(FPP(M_)BpOH}$OH95UtmQ*Ks}b@|(i^V{r`dK|WRIGGA0mcN0L
z*2nXC5G47Ts>}3O+b>fPWD}JZOw(pybu#-sA5YVcc4!uk4gU5&HVb#-nh&;NH@ga>
zUKY2$!Rp`8d~;1-q~o~PzzmjMQCQ-EyBS`gMn&Pk;bcslnQy0v;4vwO6^#ct-`hMe
z5@Ng@Gt8_Fe)U_mkDqnC<Ir%DalkkvRQ$xqG}&ib6z>>GrX(92g-s&2zK-9$kmx`~
z8%EkG>b!+T1vfL~5v@SS?qZOGH*|FBDirBQ;85Oq9|ngb+JkaHE_7`<U8BF}t#Uog
zthHNvX9X*MU7YLBF4xgMm+1<5T~xFsI;~=iq3@7RTOFPDM_`fL%}e<c>{03R`C}^!
zTf6hyk;l!0WOhQtnUF*|#=$t#y*xx5hufh%$2s^;<2X0Bd>+S{{HM)1&iE~Hobhm+
z*V38ux5jbyq%{uspZJBijq8v2%NOA{S-VNBt6pk(S^fmb-IWiHyR~>c=D6{=|5Z0?
z9Cpc%SKcW8XRv!o{v|H|v?%|E{NR%O#e74I6uI5w-o{^$T2%iWhz`2>%RNOLWb^q?
zrhKgwo2y**j^gaM&_!c7OcPUVzOL+w4$u9NW7uz!kZsC55k|^&3sf0i#>>`;XD$;|
zHv+Nc!P&V~hMkw&P26S{ww!_0Ii=Hmdn0tunH6vU8I5Q%+La>xm;*LHI_8ti<ZT{k
z<n=e+lmnsnVGu9{$TYv@;>}pbtYFT~*8icDC`_jJL9=><h~11+e47GA_`8JHLc!j<
z+AMtgl_HFE&yjw2Qc1oYTs}p2vUXnWzRuIffhGAqbK`wom~XAWoTn`4U6OAn>g%31
zOZYa8Ci4$N_T_)qwm(Mun0FSy-xGqw4{8Gwn?0uZl{@vtdN{p`)D$;k{TQv^7Anc?
zEfX0|_TO&>m4m)}4x1V<#2kxVu1IJe*0%ApcsqwSs7v+=)r!S(s5KMs*fvgemoHG`
z1s?0-X>8yCO(OGTLk3@uyDT0d_jR(Fm}vVf7H?nUp2{CQ>RZQ)ojuR6>?WaZn+p5(
zFW(M!in#}CN<|`>9F!os{R4s!=Lhll*+(9qEVI3pc*8ubO}5v<x6ypdw#_1A5-)~I
zqy&K!T@jI+GfItzZ&N0i%`a<Lc^P28<lb8LK5>sK_8V9fpLQg%1>^$gjjG{U;Urso
zQ)wr7@0WH8K_@l~+`ow9`*LFZU1aT81HVsV?@{3?*i?_h{P>GbUZgv~hcTWP^c)u&
zV4?02xulA3{m#BiU2~-==k+cv+Rb@%+f3eRT`uqii>i=OVBnZ64XwAeMP^v0D%^<*
zT@mMT+J|>8Bk5QZ!Wf^i9lix9gB$E1W54(A)LZQfo=+Bh85BPW(0vf8x3%-)AvCMW
ztG3=K^v`Tf3(3!Mq1xrG%3muKIDJL=*f!ukmCf^+LJ3{}#cXE6#+9It1cw$Do*W|J
zl^#on)sn_xg-R>-0erXC<c%Es>x!wruAVAaaeT(M%o(~MwE!63rW+BzcJRrpZ4Mb0
z>FrETT-L7A`R7Q7bi1`|d&mXMwhR`~W1mk-@Qr=&F(pi3HY*aK{90-rc^!kGD={iM
z{u>A)+moY@zS!&zd1}&rkOT-elf0m@1uQn2Q|EHe!D!~vnG8s6K&!GrwE#oa$QND^
znOV&TQgcy#A6b2&iT{h#XRGyx0NMpT6h~JRE5L6Ead^*zdNSt^O7FW~L!8uTRr!8!
z#f|MJsm*Or?#J<OQ70DB32yCioxt8zHL`!96X#HAB(JT$k0z-;DJ}oHzIiQ6*z1hX
z*dAt-nqb%Qs{SRMz2LXsb`Ii&m|+z{-fn8SgNj4=d&#z>%M6!0!auzWP&c-`#kKMe
zevfFLA>aG(9a83Q+;i^7z4o7%i~YR3Q}IOFA7uAdirjNDmE?`NmFyU6w&6^8gClK6
zlBC=f68r5nQ{AU=B4UXB{9rmdgc#dOesND&4_l@qeBocqRpkn?FG)&RlP7mlTx;<w
z&tx+h&|Y|UONRX+oxz^Ios0P0VgsL3J8*qqaBa131|2SeJ-Zjba=L@-h^@gj9eu?5
z&+mT#*T(-8uHU<%{2i{nTN=k;9Wq>S9rK^T^~rw<*HaFz9kvG77!TLo{QkT6B7b%<
z1PGnXhJ>x(q@in+P_ZtUD+|{C$|2xr8j2P-r`{y-YwNFHvrXlKI!p|XBpF3<V*aXP
z|KrU19&;+QzGO;qAg*1~9dN#cU+`9DRPbF7$iw{l86pRsEB^+HVF6`KZ)(`MjJM5p
z%Hof-$8M!J=n>3;-F;0Lx|&4OR?|`PG?~yAb2a_(3)J*99Ts)$?`jeiT1~ITtfop=
z)7f93rs=+>mHg5>(T&w~5mf+FUsuzYzCcYQeNET$TQt&Z@wIh>yg8lErnRyl<_nJF
z7u{+xo6W{AwF)>tI-H1ZHaY(w9<?xUC+}0aiZqp{bN#+*KNUZMs7l5_x>+sR_UF6z
zs?j0oT*WsmXOeB)T*cS?Gg@CoB^gx+lT}5zJ=?&Yb)!S~gZ$<>w&URAE}#JTljNTW
z1?;i4DO2N1U#j2S$OiHS=ey_2^?WwZ$?T2NskNsxG2hoPP?pWULiIA4U%{u@CyXj9
zThv~2A{1M8j@M~HtZBbed}Z+eDfKyBc8k>Pde&37ZT~OVyI1%H|EX|4A6WN)68IL7
zo=;n^r_y#=aNE_kE*c0?M0VY;u*xV|=p9d{{3q|1j@O@nkMU}K!sg5+RInLQ=wM;4
zB1cAsG7@JRlu@J~OGy%Yh&Fvq@%3~{@wHeE=u<tPc69|2Y)TnMN*_}6JeX(2@X@n~
z<&($Vv*=_;1H;itAO9>mS>^MHPF}KSqmyr-tZQ2SoL`0}l*9#T8OhHOL@#3jE58p)
zgQNT0$UjkMkfxUWpV>wZ_cd}OZzMg!$>w<I(RW@Y;$!~_G*=olHr~*|K7!{VY<2&3
zug)*{O;FE@$Ec_IKdEO{2|QP;o`a|-pTg6>fS;c#eSp&6#-pQ<K0)c9T>l@ypDMur
zV7g=mUSq4u{7)GmurhKH#GD^${Hb<ren)xCp#SHOV>W3a<d-$1u?nhvHUE8PMzH-g
z#r6W$=b5M6rbD>bz=oL*#g0!_H5<|cvv<#83KEDMHo$uPIet<(xi{y`zh1$<IK&JG
z3vx_AQZ<CiQ5#%JLUzt_O312GDf>!QqVAJRe{9}wx5H!HYAty)27@^MIkxy9jNMVN
z>AE&;VNK~qQ?BPBog2_W#T;s#)~n?ORB)D{QfXOcw2h^ON%O?L2PHC}4B2%2grQaW
z?Wn_#M>6}1R|I`#Q=^M6&`|f>rDqusyGB4f`YV9g!OaH;$CPh+7}iBF3=Cl?Z&3*C
zo-iyj7#cFq84T6Q;kOB0m!2ZKp0bzO+MdDN`2<?y*PPSo2H_<RnLFu#{4D@ow0=_y
zohfrra9QHhrOu=>%LoGf8UbJ8S1#urgVeZW-G#&AGk;EV+Gbv2`c0Be7W};b@rzG4
z&8DB6W+2ZD-QR<+Vd}_C?9aBNKdi(o!n%iH7QXTWu$svZog99z+fMu^m26i1?=oTa
zK3k3poV6rP(WV-~Y&EQ~EoIk4dwDu{RWF@RuT4)F+DpC394=dJw0<4Z$=TO|It5>C
zyYA%K)O@T|p}lEg_;H+q+Rb++H6fpg{rBuF0Ot3n5*x?DfO`sH+~c`^^eX=y{+Xqk
zgumfgbq{-9ZYggb^5Pm_G(_;l?pkb|@PZ-1p*MTbEJJp1^Oc;{#c<!Tj@+hb<lT*v
zlshW5l|rZf+!s16Ds&1@-u`yy&rI$;_sb%=_Y*)gxhGXdJGZBhX$vT39D{RR`D;G6
z{4Tz{A+LxOz+V0v2;l-2Exe}$wxOfGtT-OoNtySFMmCJ$NOGlpk_#Y8EPb_nz^!z;
zcnO@P2F`*P<?p3}z&i$+Wq6RC^5%7DaF%enk$XPRVs$WNMcc-)@%B^ca4Oe;u;%D1
z9XGKWROiHWI;IL4e3<Az)CZe54KI5a`^x&$0h?v<Y2Q>O-mUO&(<tgKU$j~kJgx+u
zY86mmP-9VSk1#G7H`jDRT`myXuYC4uoxF)cmz89>*s{$3f2eyG_^OJl|NBHkjTSx8
zqK#GCV;gE{6`=)96g3ADJVz6ZfF&qZLJ~-7NMbG!S_N!Qg!9A$snw^nwXL>&TAtpZ
zPj6T+MJ@&}RlHPD+d`|=eHtlft0IDVzrUHi_u09C_WAq%Kku6lhuM4fT-K~vv(}n5
zYi4wA;qVD&8yHX~RI1JivHPxJWie=foEW`Xf(5Ede%vR6rL-Az@nJ+Z{Tk~oF6K)5
zY@W2Wn)9wOa^&}KH<ph^u+HlQ`KJ{a6Nq^5T*Si@+zs(YS!zx%+pXX7^jkQhcYr<{
zjPG4f=NPNlLw!e^@q`@F`NB7H@(btY!<#rXdv>;WDm~6QSk~B`y2Ci#WW7<w@xnjR
zZtvU}Kopo!B4wqPo{_&h@+)*N5Qd`nT*d#gtDEB<{<ey4(3v@6UOV&x_<3@c8{s_Y
zz82Wm-@324_VvfSI*V4AL9=J89VfoR`16tI<g=*=5-CZ(;2hU=(A?+UHBLlmd|gGQ
z0bke6!s?A%MAU(C5lfv)(JE%A^NoZkW~YcIH+>I-r7ZQ+N{SOHvYBp=vqXc-_NmvO
zBg(%<0-Z3gp8K0pi^Hk!if!L{M1V6pc0wV~eg);n=8##^6r36w2=P8(^F2%PoV8B(
ztH2H!Z-2EBXz9ovv!CkK0sE=kQ$DsVdm)yB#L7|)*5_MxT(kW%mP!dfymrZ^Edwr%
zrJv5sFG;?n?tB3LPXidkOeJt8_iCimA~K{EGp)1xFzgmCYcAG4Uuv8qg3P0ZkxYf<
z<A$qERb53@a$28bXN-*H653vAOv8rY-_h`<;tt$sY^x2fTs@$Bl(qrnj>L@($L#QI
z_VSeACsxFXJ95w0X<J8N1r-$^f&Vu=fQ=?xgRB!Xd1L>Nrc05`d=L-p!39Q(SW6gG
z)H9~*!-+TO2g+eHDJBTnyN<o`HrWPe-Ot6#u{BhieLpv@mZiR*znrAuL?8b%$r}B@
zkZB8hL73m45A$<wf(A*{ze)5Sy=ygOUY7jpeV3CLN_WZMiG0~CLsw?Rw-EaG8_iJc
zQbjbaQr<~CmX3e1w@Wi|jn47AL6R`ko2?NWO3y$6b#iU3Now^+*e|GBqR9|OuX8Wb
z5WmzrNuFtSrln~u-hzY?6K|!^3sI|S#WtAkpc$HNY6VQg`6eZjXq4O3Wy3dhB8`Hg
z1~qjLBT-!EMw;Mqof&FP7Zp2`ZNYcnA85@7-PYW9CCR{~vl|rBp2x``BCHe<CT05U
zymvc@%4)kDl;U_WhbX54#y);`^d7yWPbZ67dg)hrISzbi`{w{;%M@dg?W6e>CKN_x
zwzT&k{i7XzY~9=Av`~8IzB$q47W@ad(yZGyp1w8xTjNx22tR!x%!s=$Us7L<LYi9t
zhU8vf-Df$D3OSAlpI@Z$gpOmoe<L+0o;+Bz_xh>M*|Zj1uiY3x$zL!+8{OjyS|yY`
z2dXprnpW8;Gsr8ex5v6yND=Iw)YH}9{@<nDDarhJ_q|f;ag{9{^N{f?`8a|e4RDpN
zG1B$Fg12+himc8j@GZpQ_km@vMvK^0kj3|Ccb6y;zXKG##C*84=eT*Lp1CpfRv2Ws
zOAeV)IdpdYj3@9n0sM^<Tb_J_@b~%Trts>|Da-yj8_Fm#oGPM7&Wk*{bN}pcS6uls
z=cPuwOADjjpRc2Ua?S#a_iiDvjt7gxyGvQGVe;_+$v4ojSjuilw)VogkbmJA?cM!?
z<VE$Njsly>CcyB;2b_|n5*oa*+c}^*i18Mq#h5e^(y$pq2SI?f4*|S6pJ)pY24PV!
z#(gL5DK5A=ajjhLh=<U>gae~*ak?aXsH$Pp(bQB(&7QwA{=s;vZA*^M{Gn*BoLIT^
z+<5nGQU+jbn>ZB5=YNz~@_W_Bq5HG#&2^RBhNL@?INGay7kWTe(ZkC&&=hmn=r4hd
z+CgH<Ac;XRH{RW~mZHP!9z`y(Qo-E=sqF1m|K<E7O?Rk)avoi_7kJyu7B3Ydcl3RH
zEVr=5tKUjZ>n~L~Ku$riRN3|%Ot;m#6e*!%INf$sXIa)m@zey4X5D1=hnc;9n*{#{
zVp;E?!b}$YlY##s!4KhWUbao%>L??7oJ$#Y2H!VB<|WC!osKT1$`2<d^Ik@CB3iaO
zEwS2gbu?$r4F*XQtBDxD+bn4<rw7LOU&0=58J!h%;zT7c<-AOtC_|s4fa>)HW0o!*
zPdBr^WY(jmgmce4z$LD^a}IC_QNiz-IX=qO^qijZ<y{)g&l9;vH;6wMUL}_xU0c5!
z;=^0cI#I{Ff2EErO<j{k6|>SMsqDadX0&_cjcRjh)R+>E|KuFg2aRML`6xY)W4883
z<(?od5UDp?D|dedacoJN{V>13fjw43<)wZ6(fp2Bwr)9RrG>&>TY!!Ixmjgdw684d
znX)5M2TO|P<fB4t4X+-}C@P^#D>Qp9W}C%a3DuG<=GaK|R5PMXQK^|na;1PF7^8t6
zpS?ZY^-&rscYkz}m!1D+fKkdg4fc5IUa2n8WS7j<I53^csFw(sDVBd3FA_L+2l!j_
z2VMSlDSadUw*Pu8JEKW%@b8>r^V}ND)93L-mf%gE2bAYkSn>FcH}3ByGMspUH|Nj9
zQ{k*&<{Ym=PsSvb`IjV@VpEt-Qgw#Ef^e-vYi|N8rDi_@YtvVwx9G}?*{I~RH_`#y
zUkhI@gTlt}f80U)d3u{K+rld}{^V$X^|QJNxa>gArM=k)h(o)(Ivrm9DE!o*Srj7@
z5p_V@{(LmrdG8-djf4DH;$RAEc{4O7VtNJl<^7kW%0820dDj?moLqhy&X!ULE$LWt
z5!v`8Gjh9SB!+oxYu{{d+qUzLXn5Ac>B*4E_$ShDU>|dC`I5oq2btXY+Q*AtM|=*b
z)Oi<>C|yVL4BSuAL|T_GDp&<onu#6?7^0fZB5F=~@&ygb(HrTxXz~?&Zl$hGypvZ=
z;k-dRV#Ce0={w{2)Tgf5XGZe1q|(rRWc(XZb3#XH@->cgo|ZbZDB8IN#cslQk@iBf
z;{H*vkr}})EgH9dX7EOfUI`~O`!u+Xr?NkW6P_8bv8-)&Jv^7AHG!*m-+MtV{d;_D
zUo2}!(cjxQ$Fut3bm=;<N#1{pp1!!iqou{C(T?Mf6Dh@eOMz5+!#0tT*=0d1W<ApP
z$I|4}<;g$dY$cS3*FjaWHTK>jlpJ(8Pztv(k}7>3ib^>{o5()V3xeimcLS(Bbw0+y
z0q{<-%;Rs(U(S;@uCFI7;aJI)g!;m6OprE4yYvZa@+;<t$LN4lJ|Ta>0~^u!zf3!&
zNu0<34b1#+k^Tpd+(0q0Znr}Ae5yhT8dD8>t5i+AJ1HpD;T8_{5)~@KR$pN1EwJ^X
z>Qis7sW;cuODgKkrCxI*xEia*IjW1M?6QT^NI|1!eBT^lpr@PjH@b25{772ou1wHC
zd%6QHvS*Z<W;XNx?fJ{eWLjG1tVONUZdOwpE!w3|rYUZ11%7P>er*L_Z3TX9xn6B7
zItVK?NI38uRZRQp(#r?%FV9|NH%3_nw0p&Na=Rv>31WJ#TK5>k(^Km<^Q=u&h(@eN
z>!d+qU`i(JDG%~0jFd1-%P*xt(k$Ysd-5v@Nk6G{46Y6DXm3@4^W#P4=Gf$BT*>}L
zwffVPu0T61$90?_I*p#6$S))>=YLXbwCGRaRg(6}9rrsQORz6%8830ol|3ELLZ$Ly
zIz~rxI=`rSDn&{sC@?B$;$ak5@UCi6gu~j}bt*-%CFLMrN>lCqB}M-X-<%*0JIvAv
zQLioNWzvukgij;%%Ca<7-ERD_296@`4P~A=fda2ZZ&g!-1}gy*`C4a11WLZZ{JQ#7
z=lC}OLI9y5rwFKV8$nz(5;9dZ+X5L6hFX#@!*s&yIK?k88zk*3HtSvj75znuc1~Y%
zus;fxbAX%{h5E+9x18tO4cnfHAoAJv4~}zXt|vclA`zOjJDEVs3d|?09^`zLBz^(+
zQUG$b{}*mKbE!`3|3X_%->-fBj{{!Swiqy#bdThEs|Ukf+SlSt36}R;%Io`CYKm@J
zT)H&*+?!97rb<r4miR=xdRr{(@r*n!B|&+q*aju0nvsZ{B%`iMZl<?h<sRv6+~9!*
zdP!>3(dAhWmyLNep7k)@_sk`!>v!h#Fi5aB9H0Ffo6_-r+TZ>Uu|t$fE_&vYET{e3
zqJ;B&N~qrC5<x6$lj$VtvczEVGK3zu-1VI?JHjhpl-RRp9HD1%hu<DhQJNZgM08AV
zY3j->F@O^vMM+M+jWFE(=JVy%&qq7=WU<Iwko;#n>&dLm+_{+*&w3)fZYNNWzJ;{m
zb@(3VPF&Yz8OX7X#7X#Vk__9gp*n_RM9jR?T+hrAslrWo9S1S<FgZ0cXL70}hfBpD
zl>7NeG<Lk{faGL~%~c`n^D`&TzZ(AX7>G!G_D_nxzqE(}y?<$k_8O|JIDMePRefKq
zdaq=qLruN0`(HXTwubBSD4TO|o-G^0-3H=Sc~*Xo+@?ZbKpnw9s=(X=`uo>Pxk{aw
z?I<U@(!260`e+ow?Rc&e-&<1jTKn(ll|`m5GsXcz6OlOMM9MWK&f1fjbmaJr=+n!s
zK7Df5Tb#r<Se-^o&i#mAuMdf-+X9gR;CSjkX4y}psL@fV<4AfY-r1KG@BBM=i|+|{
zt$~iy2aKkQ#z!YG2>^Za<l82kvF8)D&nk+jfJRZ=fue{4AVu*g1}#q<W;%P@;xkl$
z+DmU-{j6&eR^D`{bU}Z8hH_^;)c1r~nX4Dx1OR2#yEERYalFV@+y?K7-~YEGHS?}(
zf(1Z-$<5B^d6cK=)}@o_Kg)UUF(`z)t<$liU{sj(=5O7jZem{NnVwNVyz`&9N-qxo
z$0mx*#eozeevnHuK<|hhH)2o{2a?4#`Pwj-5xRFV$zPi(#2#mUhnP2KY_i%EUUddN
zT3-ET972-&_J23>F}WY0xvx0h3jq34Cv%`QC)B>_tRRK)pIurPtG1h`p%M-j^=4qU
ziHvd%m)*)d`z0vFj$h`dH!}O>;Qhbb`5w9=WpB!fXYHk9%vG>M=BHbiZR7ktV_e7d
zt2xjJq>mKbBAV^n(R&sLyqY@Ai`(8syf{&I!}uKaqhZ5QfBG|*qh6*a!ckwL!{E&6
zD%qJ*_ViGsXMQYovk@a@gho>EO+sNj%a7R{+xD`RzTdh|a8ZDfis8gRSYc0}NLh%j
zLPXF$-s8w1$)0j|M8_#oBCMQ2ron}6f|JgWYnt!cVt?HVMM=)4e?tMzhiQo<Pl(j&
z_w+9`VW<LhzkRi|FMSFMu(uNQWCKc(w-U5DJx}b?%0ldXlRS9{Rl)xrm+&(ZSgL%s
zTtCGdtm~|Xk4}%7^Z2sA$cJnc6v0P;bHeBz5G0SHG(O1JL?7Nj_}CAymB}uf_LzxA
zf;sNhQjet*Ey0+pi?L-hEuTHoEWwxyKObNAS9SR)7A4%`zvdV3s~l}tgI%MDAON2<
z9&BSze0doIZH3lTU2{#Bj2fU_0~7cz_&^dNSCQpm!&3yPX43X_%9JweiF6@oHT~KC
zO-8S&g#BcV`Wy|9<sxSa4X^kg%8aLOH(fip%ZKf17Pr)?VlyZ=A?JQtWmG;IihAc<
zPOmdwOVOh>+4ocr9zz|+m7af(tJ3{cd9PS#w2()C9MD3hLK4QZc`_+P>md?}*4u&H
z=RY1YF5fg55#pkTFizbb{*NEgM$mF(dNJM^1HgU`LPJ-X5v+Q)SM@x%>Mjzg>iy)_
z1T|k6lG4J#DxLeWcvezAdkw-}<3YAQMl>?SLka1sV~@PSgxP2bx8@g^NC;Ff+ieQ@
zPM9;Svp2z<@}sEIQV`Q=>qW9zQ>z_-qxp#R^AaXwj}Rgdj(2AT;W02W12MWVvTKap
zouQ{&&Af@ZiCxD46|9Ewe}g6n%)RP#)?|!F?0+heyGoo~hKFWard9!}T4|DMWr(?T
z0W4;7GD}$63Z*e*CMsUl%wm?46J(gGnQT!tGn6|1x3w}0t4JRWZ;-Haz5+d&b!`u0
zLZ^$YV}JRj06K+hK)2|05gtINnbP&xaVr0sH+_RUI|;o7a<j9g2l$3rBS%iTv!OgY
z)6wLHI|Y+FQK<@?4PS6q#`lb{8oZXqUxFaC<Ty8`6ER01fj9<H?N=n&cBdt;K+}Av
zqy_qbW19I8bDq?P>Vp}v9V_Y$-=tL%k3;@^fzKhe74W;fvRbY6bx!QiAM5MhS^B-3
zAj^I%z&d5I&|Kih#bNuMg(^d)TiJ)kX{MBGR?bHnA!^b;!<{u8$jB_vxyYm@AwAYv
z2PhJ%(n`3Ug2`4Tzd7Mm^<V*ZlSjd13MY<-J3u4_WIFcX5~FUScWpPrD25u?{Z&a3
z`1dB{H;J%XKV*i2Htc#ESvY3R6R5(w%(4Z}I&h@Rn|Tf=Hqqj{fhlzFRs84QF!gD;
zkd_PG<>*s+^;fmS=EdDJc?2)c@9PXRnEFG@dVKM8GZPSRp*qSI&f570`xDQHyLK}|
zbt2P%Rchu(;jYuc!qA24p0d=WgMh~<?sxXqQW*|V@RbWu;*5UbZ@X#3FH75@X`A|{
zn5`*Np*+0_JdIe>K^4gfSZr%_j2yerS!Gt@W}KroQi+bX5N=qv8RpzhcYE=&7x6h@
zbv9+@&)wrJH!@yO<;m^pX}ieGB?p|20)^zT>rFktOu}|U<+Wn;W^tQESt>M#6)w&9
zp+LVMxZdpTX<>g7)tUuoJSs00T@9|-EL>8lt=;YTml$zo(b-gIR-S)MPh?h}wMFKu
zqAGeGY;;>}0Ef5UY<Axur#VVZr@=vxLzkPMq)|)H_1Bu&$FK9MhO94lXtjE{_2oR|
zf^508iUDHioyTL}S|s*qS-n|XrjPTq>0>!*!--Ep!^op8kl<Z+5M$KH`}Cxx%#F@-
zuo7o9^hcs^e_wRUaUYPU&RQC@!2G-Fp{uStog}e#)Z~S`R9nfB9Dd{Oxf#E^*Z6yD
zAdP3vx%-mdZ0V1o^pCIx&hK%4ETT<~$fst1bgZI!Noo;BuQQtzl9ex#0em6LECR%#
z?@xaaKBRsSUF#cD!aoT4SvU)Jx{Sb>QWC0FxD!8+Y18Kged#^2VVUs)C<S*O0#oZ@
z56;^)2Hv*sSxj9uJf?0#zhnRnSfAeMT!z&5FqQZ#&{+Or)~LT}=XK6EZTX`p-^WM#
zSc?yTnR+UTgc7Y&Tevf;)nW&ocbnDfa00B-kvrWLYqR(?{`q_75bN9m^A)-0r6Z<l
zi=RRKLgVw0{GzT+g%8Is%rHv-i*nsn@Xn1?DO&Is!JqdR!S7^xU|cu^48Vdn4(atS
z+ofNf1%S#@cyg;LH+<V>rar8OZ%)tGEX9t-^mnujer!EU@^6@PvOM`bU_Cq!IyCFy
znpea+j=_%&OC^ipQRm~3F}?pydf!L<mXR*vuQqt>b$(C3TE05uo*Idu?u$7mTZ`a7
z*IczY%0icp1`8*ClB0S#l$B6R-vDRvJ!t*A=t(ZPW06Bv52;74p|p{yyJsksyPUiJ
z9SL`B<jt;s|C%Q-D85~yDdKmH7Gc)EuT~b;zo$`RI2C2Y`0L-7D?ied1zTXHx6R>B
zb+9_hFygzL<F6|sJeK*U7-nKhIxa3<t$SwAfufoDC2=^fk@(GRqXdJ7?xS->@Zqiw
z41clIFZ1O(5KnQmajd$}i88LS+3W_WP{4)Houd`~0X<=#bLuse@VpEN=f}W_3efGl
zBcZFc2Q*JHL;U`~9BJokp1jxi_ACdRm;D_$%b1O`DVx>D@T&2YQl?U*Se|wl?w0V+
z-K^eX_WCAo(cjWdVigmA)glc{Yq`HSQKUbyUg@zyv6hQplb&8M!vtF^>&+L#oL>)_
z1>DzjWR>AaG0(#FF=RLkxahT8^@SDOR$^3w1B4^BJZ~_>tV+!)O<ir5a8r|5!FB1`
zw>OQ!-t-=pZ&{&pmv33m-RCaGaeGf-{WcN`cVz=eoFlc$vS?Fz)?;NyGVrv1Tbw)?
zUj0c(2HMK);yTe*Ak(xgH()YZ^m!dAGYjo)GnNxz_3cN4?^B9ZbHa`A{MlxU;Td2v
zhT*6@b^Tc~x}TMUaV}SWZ+U~T*93^_uwyqcL=6&2blUSx!s*f_LZHT+j4(kYhS%+5
zjq|7g{@nHNqW7@=%@nHOT@d&)p#sHp+@-0K&w}6FoYbuTF>fJ;>^cPy&(_NA!!m2V
zrm4qT5D2xtMp`Yj%N{ygN1kowq4GglBRA4Lq12TABFL7=<Bx%C3&N>aAX`y#YQ{U|
zskXCnahc$2?uq*vUBo=bgD!fm;GFI1y(OuRtTE{%it!UNOOAPMj1$XxRMf^=)T<29
zk@3`|Cxv7#G@p|XBm8|l`DA$AKY;yB)cSE_8qmz?jH~y_404iE2F$h7X@(_cYp@Nk
z`W`V9J5E(_D)+6XAH1nPJWm!|ZHLb*NtI1x$jRR!+CAYGo!6QgIkqJ0QC&yMq2Rcu
zOxVlGtr%T$S%Azv88lhLt-9Tqa>3Vj*NU4lpq?bMUiOpo<r$P>otDyVCm4vt84D?3
z)}FHJP2qLh<Jv-B{gBrsWgJ6~g<51j=DCur{bgCtF@t&<r;z>Dp8OG+>44^F9|#|;
zVeQG&p@!p+F3Y+<CVMi2D@T@H*ifu<PfomZD~8>^EL5eDz?E0jMh~xEh1#w&pv)zj
znM);qgJ|+?mp|f#C43Fq2hmX>2rX~LjAp7yzINL(ecODMVZDhNGL220V`SU!z7ddZ
z^F5`aT>KYi{VF-q`6QQq?|jH+xjC5SQBNFt>p1r*%d42=0)7xO&^=8&vh=cG<~}d;
zm2Q1ED)TQ%4y}z$cbSTz3V*%Yw`(&T4(9-|E?fM2Y#$_n1H`8pBmoDA*KvS=AM1~?
za`pY;)hToagFkT$u~@ikI1w^BP%*O9H+1hNp&<_Pihvvu@GFP{c|e@N1POJeY|MU3
z$IwMCKjQ~sI0g-I{U9vg^&KI^dj~l}Y)aoqU1mIx?ccTQzir!Z?fMs~0NO9BzJ%a$
z@!qMjuOrRW_|KyxnIV{dh9~HnL5<(V!?O9?`i;H+w>V*3XUu*7c5l266YzRN-09dW
zzkt%QqJeHP9YE`I;`UZaKzdd<-IhL|<_9oeAmc1^REe7=GD@)@DfyxTM;}drR#xw+
zw>Gi0rhT~+kGj^VEzF6H6Nkp>pOodqUy~3|k2p>o>d8kC)zNz;(92|X`BU=AX{kwL
zqH^EJdR6?!h@e5@XEcB)^Jc|A7cKgu^=?;`6YE|r7$m-!G#<u}<K0PvjxvrMPSJDi
zo8noo$Ftr@Uyb;6zFcX>)Z70V7{k5Zn7W4AHHOb3QF^W{eDvdk>HnZ#ow&`oDVXsc
zFQYg3|22z@v#~~^4ok+MwXM37F`;gdnx*E+96S<HS7s--%&LN`kwy0}+na7A3l!+N
zoV>Ro{KZds1hbfRK`AD3Ibn~zOET%zB2OChNZ&4zw>&BC;G-_7k%f&e#*<=~iIGNT
zoG6yNA23BIqDEUH<Sar3hynhWaLag!L{tcJ!@0If_8qqZ6fw5~{6JKJNb2VN<vbaO
zei4_gpukkX|5SiIk_xa#QUUfz8LBwWUTLWyol9q`6|%cJ6eK4Ysrt4Rckm>5zn{O+
zeY)A0U=`;b?whf>{?Y(t8q;AKGnfA{dfLVmI^RbBmLN9u$l6Jng2O#=p2qm~(5&^g
z#saUNjDgjwhc;0UZK56-S8baL(>E}F)h24UNqxhxG-HeH0RWmalr5A6&OUT}%JXu%
z)yh*44^YpMW|(>l(qDDSO%x$6C!6+O!lZjWBYo;NOQvkKUU;;(yy?k1c^{1S@X>(3
z+&=uoLoF5^;(arAo)ZULQHZIq+g8X35stxuUk$eFjVcOsXHe@^NE>a1w0gNx=@^V%
z_Q6z`v0&$=KW2o3oiED2rmXsn%%QK=-i*g86WR(UBr{n;k<*F(I*uthe=)b*Ud&hu
z_*1uE;!K=rn90v>2{4nddy5pFbl)`tXL$BR&=DN9ICzhYl^1qay69*4P0LA!`THIY
z!qof`FKQyNejC4ObG|w69oBi8r$`CIF=FPfiQSr@-s}3xJOZ?A0yA?-%t*`@OOn6R
zxdv>;bIRaTW*UdBso5qpUOEwp32W=Fb6)<O?I9Ccy_rk+s1LK9ag6QGUcuX3?0L+y
zX<AJ*xv{JoFAgGc=hz&#lS%F#wsy`HTKQ$JARW3l`%&RF)(bRHTh>Fk4*#j-NOG#1
zPrR&}Nn6*8qHAVP<<%Qsn~GZj=V5DgyewsIWY#pe0AOXXy$CqVtG8U@?HdEGan7eI
z1iW$TX4)&KZi?1qEcpgU^|5py`J+h#Ja0Kr^jAo+t%;^e@{Ji~oYJ56;ymH2E&qC)
zpxG&`CCc2hen;e+EE?us@FyTe6E6U}9g<)V8_A}ew$PH)q@$8=f{u5XNW{k&Gk<X`
z)smImbXjWVR-iA@#Y!mq)J;$RwIp@r5m|eQEfgT{kaOpFnW#8#SF}KU@-59G$_^q%
zo*A=G<~h&F7bC!SfXZX^$nW(JNQ3?1FMnLqJ(LAvACM+9=b)68f`?L3ymq#5Vi9e(
zrE|NX=2Q%5Bysg?G-XyA{a!s9G2S6`P_bmz7#t+G%R3Q^`j~B8n<O3+i)9hL&8*ay
zCAYi#h=3=Tj+egwaUjr6;Mspr4KJc*{)OMOAZzw6=F?B{sq&9N`Vn*E$nh2C%Glp^
zJ>Mb0Q{ZSqY6#Ji-^I9kG8xre$oS2;{@9aKqYg14#Vh1JgCOyF!VN!;G0!MDlJ1Q&
zV~L%AJuW(3t;kU35M|s2f4y~rsA4Fwu2R7w<3~^iySudVs;v}>4@yfEY4FpSxL(89
zY!B$#2sJ$)O^v*W;Q(Nx9twAz%rG=}M$=&>sY|mmwj7|rsL%g+&Bv?nmpQ$_Y|&gp
zQg7MmymD)Mmzb1yy#&iRDBt~V%P?7OLC(=+aL007yCZa4ix4amsFk&nyA)-{@2DG@
z%hCBnWou74_lp@<j?FqB>xNvK!@*pJJFtvqDx(Ym%oST^afFN`7NK7$@5n6!TucO&
zf4`%)%bEKjZ(y%HJNmGZDeL)UWsJ;KM(;2*RVaDpHMc<8)~_I8Y={PgLY<3kz<>)r
zqvf>%=Y%pfn<p`|i0*_B5ko|Wgd~@OA$>s|+ptZ^<1By1{<4W~_1stXs^%CpP@_v#
z5-wqyZJ<m{83WJ@-YNn6O?5VFOe1^E$Y8z>7IH^EWk&{93rN4rTm?1z=(tqp%Gf@@
z#u1rYlS#EHtLVvP_n4aro;JwzHi+6*+7?O|D3ED7;{1cIwFdHt8l~Q}tAn&FoY3)p
z+6C}oOp``|aK0@u1`14SZf@W>Mw~88SvJ89gAzQiE0)ZnPo7idek0!#8WGPpml5N9
z1Q>h+@P~6*srU%Q<=Jax;$@$AiRWARUz+Z|Oh+)?or}rA&v}a{O{;``RP$R%M9<}=
ze@d2EDv>W+wA^YW_T*NRYr=%N4WK0Xnmr6JnQb<Ml}_`;UgAmer|bAn^bK#4k?Jq-
zO`T!4ooBg${gof|Fk9RTdZ>g^UCwE5as$);DW2%8+1nRKZ2IGKN&is1w?vBvV!Kyw
zO5ZA-u19M3lYLwr@7k-Kw=WH#=a)2+hA;RJ*GyjFw)F=jGHtz|NhCaGCOl>g5SKu5
zYAzQHhP(0%9+2~PlR+8I`3wP=xm~p}ds1Vmdv^&<_DIA^=cku3O>rnx(u2wv@*VN&
zXRPs8_z-<&Yx=Pb;u_?z^Q;_po@o0NU2W)(<wiqe8tu-E=^`adbB1YYbe7|16M<k%
za+fbSV@PR+nkCN&=zSu_Ou;hwl%SXHVD&XKUbf)X2D%?|Igtj<Ry!i;tJxzTr-Wyh
z{XwLS&(O=JvWT<ka<>|w@STYm=+1xpAAFVRL7~D@5?8I&)4iv?&8l^H#c>Q7#>Y%%
z(p_6Yq}YXvI+}37Nwu&|mp#hG=L9BzWvTqF6o-g^+4=UxASL-S+;|9cO;<x7f#L$5
z#3E$rfu>+@nTMVqB>-+Ubg%wDomc0NjEdf_9B{^4DE$?tt}eZtDVwu(pk_hkHrn(p
z>sWN2iusj<72a3J^ct}RO*12#qh&|4W5wOlJea0jV*0aF3l80*GcExF)Ms=|S<ZU9
z<W$CkQJ&jTp5Nrz84))leCdk72#>)V2KW=LkGWL;usQ8$IT-=!2F8{sY_Jf3ZbV{`
z>JEtDIsMYVGGhdtRElW&dmzL4G&NhG41$^|I@4rlAyqN}U%Dv(SEUEnJk<~9$@Ad)
zESc41@(FdRfKC!yFf)4jHR<3|;5{;Z6lu}jW0SkH*PEkQV|+0iEp_$<Um~wfW~lAu
zseq^Bv-hT-G6ypiY|%-WBT20puDaEmomriAFHmXjmB`=46O&(A>lI<Ar+T_Qf1?tG
z$Na?1NrA^+-^qjBx4{E$W~7NR(_2nR#LeCm&K{rXd{<q}^vpm4*%?UOX`2)?TR)>l
z^=oZO<>-SC2NU6qiVGLGa2WMi@Y8i3L`>lBurU@QK6#MjT1e8X?Cf~669%O8w=MdF
zG>bkVQs}#l`+VH<bmQ)My5?l^!dTZEnS*k}iAL#LEVVqd^owOkqiAPgcH0NL7v+q9
zV*EySgkIoOP7Sc=6Pe9HAzNTahFHDH-H{}Ub#57WFibHnW;f(@PWo_m+g+XbyE<(j
z<{s3aA;`0%os)92+rQIyr`0RWae3zfiF_KaWYsvvu<5)gyN&h3#U3ahaeiMK03|}*
zX<b@|-Lbs;;yhDi+w)XZQnah>L~43WHL3dc`};V)w?v$jqt{PLAEb}WyV2b^K1gdJ
z(W26vrT>U`U%}b!r+%3uG~53&9!vE2Cx9uu>aWZxIiHxR!s?4m;)`>_tL1vq-I>q-
zETJNy&SYtp!A_?E1!#S+*L<WyMN;Tl;jXci!!0kE3@LWm==AlZ%jie!Y`Xs1C<|wC
z-4;{yQaCZkR8a^zN{aR_-RYuca_6LQe|xO7XfaUqQ+!Exr>21ej`7igVvbWZ*;wwo
z_Kpy&Do9(zbExTt|Lh4Am({+)7PsxN#r-{Mbt;MoC#K7iSXdJ1lxr+~eE-WXeSC_-
z#57)*0<AueA=dV{dpuI-q?3-|$WTZ)S_D2<t>-D;y_gOdbrM(LguA{&1gKO$(y3sB
zM!@WoZN#U)%zy6uD&9RS51bss+W1}iwA*|V{avj19A+UuAgU+*2}>`(16QH+$B43h
z0abiF{Usv3^Ph_LF01zXAzJj0aH12L&^@J4o$%K(PUo3Isr~I!N{ia*fz)A9_QOkT
z4|Lfg4Ej~hcjotaC1thWqLP+fbLf7oGNqWz;Y5`X|2ultR}dRC8Z1Vjb5XI24|#m1
zuQAOQK1w}&e3Yo3zSYp`S4FECO6|p+iS~$M5h!>Vw?M4!IagZKEh&0^=`}hSYU!&S
zQ&dbf>yK#XoZ~66sW5kRvG!*Tfo%|ue@iW%Xk2F~vF&c>13|!-3GfG@4ArYF(!1Q@
zabj9-zl?9yTUzw5@T%|fUQ%QZms<LFTylz`=Sxx}e?@f2Lh=-`;vSQ`C#^@)Z?$q+
z9)cXF&(fGvSS<GsL_0Bvue@;LV~p7%bIa_~liV_UvkfeLH(7dhW<d7vz3VFil5rk&
z8YW0~L*(yJa{6)_Tsr=F=nA5Wq@UnhV0^R48t>ek9W6?S6Tjh8NztW+OEJS3JmwwI
z#w9;f5RwzPN3#8i(xUR5W!IN*@1u2S%hL=o*Lm4CKsr>ev1qohRm$H&UZL>nuYg10
zpKMXn1p_mG%8cJk-f%+uI-OR^^l9k_>XLJqwf7i$=^m*on-j&POE>W!{ZTp|{{}Iz
zg7jT#pr@Zki<)wleWu7+Hq|UhZe`T|Eu3Jfz-2u}&eA{PQK?>|w?1`5+m60R)Zb?O
zc8~fL`t$>Rf%SgBFSD7{w*T0;_Bo3=se%q{zg@Mtt$tNih^?6>+jNgv&X>4O4ZP)|
zKm#|Zfmdif0-ntX;4B)by<6iTX*jk-*?<GcGGzxTdZAkb{tErhxL^%mQ4Rkzl3H@d
z;&D`7dLAst`9Lt`$)tp`T5fQO3Dg*3`Z=vk|2G<(H!jlG(Ifs#9WREvb&o1YAE6em
zCxhRG`IOqX*10_xca>ne2AG_#V04|LV?@)ho&8L$KE>K8=ec0rNwtn!c5+%8n#X5Z
z)KZtOdY-tFqH3(PFKHZ3i7^QO3&=xLyGP!E$7A~+N{ec3=PWx$n_Rwp!NLB%jlx6s
zsGPptJ$78#{hb5!6OL_qBXfC5II)D$73+Ee1tpwV$a7h0@wj*qw}0J1Z}oO=r%rJV
zEStJgh!d8!(J9g_6JH~q!y)_+*C2y5Q}`OC=oE|>mmS}Ct}BlaGRXF&1w~T|m%gU6
z?z$&@nIZTx#JZ8vgDX>{Tec%!{S>Am%qr3+`bs_emp*~8%3+(|c=ss4SMVqkKf*Ex
zS{UEmra&d^%|4#8&3+wD=X*@Re-I5vFlzs*?>y(7hab4X$R2Y);!GCYFa?>(J;%Sp
ztcwt>+o(=}6Ud%rE{nD8gw}QbyuAY0KHYD1pk*(Lm1+OYPG^3RfM`E~QNRgCfz6aX
z&ZUzKAKdcA03RHGl1sXuR$~zMZ=6qGZ_>DqrB;j+Fh!ffT@{c_-%l(Ykd269A*IDq
zol0VyGi8g)an4X|Hy5sPds?B8aT3T-^09Wfbx8q}B|pcx9k!(uQN;lHD+cm#S1Iqt
zLX_$KD#Q4~T^G7Z?}Q-9ns`lPw+)E>v6D-C(rh?I-*c|w1EcdP9K-$Ze?QS}FJPPY
zet<;6RRB}&cyUg#`EL#8|KwnNJ;0%P>El38kJHB^_0<H^pENlAe=0pc(%((s19lL=
z$b1Hwja>5jV?K!n!RLHAm@z~ReOsNzVBC?4`?+&vFzzVDeao3j9QFLJBY>0-4hHg7
zx*~0EYYE<Wc|~+)S@la-#F7VRB%fq=gH<-03b6=h`<Hxd0Iz83oZa&XPo4X-+s}|i
zMA3m;I<$tvI+9ATh~GTF2Q^#=lI3y;nicEpegV=H<l1%=4?p2yklQjAKHQc+0ckmn
zfJ~y77Yl#UyY3w&vitje)2RWncG2wQoQt=t5i^FulFu0*@IB64$hLD|SvYYqfQcyA
zk&m+v<sj+t2jGjP=D6_f;XBnW8444m3{0z`Sm(Jy1Jeu_rZvD6OFlQ)mAdU5%}%>C
z1pt}>w$ak8SIVnjbUs50#`H8S8XKt>zv;gcgwo%G>@xa^b1l{R^t43?NR4=AkxNfA
z$)NFl5{c4ZF!XwUF#YW|{SUqL@<dkpm%v#1Jf)|!ytW}-V4HgfW7+8=DT!)K5z)JD
zro>^I+cnW|Zl<knHxwaUHLgD-nRq+J`OWE}u=E_FGwrqHac!$h9*cMwmOQcr+hrEE
zZS9skeyY|EC6Dxk;mPCY<nG)zt?e8E_csQw_@nbS-QolIO99xe4)N&4oH3Q|N7fr}
z+w|)^Ig25*fc%Un-!bdAGZ;>5A%L>#1%<Kd?`WncJH#%-@66sES?jQnET>z_uym|6
zp*1ha^0Ah8<#0?6yH{$!xLm^r9my$~D+znkv07-|cVsw`%|i@dr#tu(3MX!3Chz2;
zWFTtnsSlZrLUE?hD@sUkN$SkMDLJ-1C-GT0l=k?t_USC<C4R3yxtoOkq^DT#R=Tdg
ztf)_^V(bq(=#-PPyyz80R4mR37WmmrftAiDGcU!?$(ffzXCyCL91?5cPe*=k4zIXL
zt-1dNNg(I%pAlp`SsHS7@@Nj+w5W>q-t?5s#^%ABcDRq-p(32-KvXz&=dnnA`;w~5
zdUHD{zK2vY*30T6sO$8y%e>h2eZ9*Tss0dW>#H?(3Q+6tmd$0DbSqN)#7izlKCwUY
zKb5NGxM=5nvWKGe91ZZ%?p0^<hSt7nGU<~}vhV9}z9okGO(xLlWA!yEdQo0@)kIPd
zV?cw;-oY1yEp^zQqix#gC*1R>t&;Ae_^%#QpYKnND&U(8p6)lD1gJk^b25+e)^}rN
z7S*5g4UlTIwIv3!>&-d!Uuc$t)w6?!BHQD8ZxZb(uX7!vz_~}A$s|sP+mCCp<8n3#
zxbGa21bFt$@k(b=tQFMde5R4wCx8Ll9RgVORw^5&)n;`LO9{3U<OCZjV}YBcxEl5{
z@r_*!EJEukPfx|MB)5LO!_XJwf0UV+V$IcHg&gOjB8$tCZ*j^Zwfgxh^b}FbxqeA3
zgX7ji#k&B7#~dC-5GwydxBUBrs+8R^ttJ$j@~OvGp%OST-ujUE_3{3qTIW?+=~rpM
z$o0X%716B5Q{h?gEH&s`pj2q<p#ZJ(2~8^4#iV)wG=es#4|$+PidBWsAEw}K;m-Ap
zbrQZp9x&|KtPq-@G5lc-56%%|Rh6MSy@$K*%rSI4&d~AFW0N~v3VxLcE&Q1B?C$a8
zpD9?yx%2`01)g&pVnW>ZK5@;jKmt(7F_4y&BcJI$Cvurj4oRv>B=W~v*RABU`f8Fx
z#e;)p^}~FU3R+>0k*%TG#<#@2Ug5sR?CVtbb*_Cqmsj8a_EYXR%(6PI_7wBYJ=XR(
z^WvUUmgOcawh3R8FmfHsv}5o?&K)FRAGsa1^uh5*!4$N3a^!}K3`Ub`BpbQ*3O%V2
zBNq-g$l$`L9;48NoNLAzjP1Y;+{f1IU~Bw?+RUXgg|e@og|a{1(|12@^L#&;=R}i-
zO+ypiEKi>;Y~5~lb_Wj6y`3IMp_2?7ph=gQ-pr87%XCtZR1&+9rhaD}3xQWSPa;<#
z;|jl1Z1g>WXwx~0jyeBjqhm^KI>|<_a--E*4E;SO?UF!R(`iau=*+Xxcv<%=t=20G
zopL|&MMY`{OrcZgMyfJ(X1MFSjOTQp^rLw92OwN7Dof9j?V!i=YN$CAjM=R$NTT$0
z#=9B3c;8Dh-!-h~nu85wH#6lW`<NS=34`1%K;-RaAtb#3>w=|ki#?abfhmbU)EQtk
zt^X*;eCLoF(cW=XP~o%~6%406eX3M^ST}rBIJ1Kv435o*m{aV2$Y9z1aEA9mcxFL;
zj|z%8xn8ttFqk&!3!OJ%gC(ra8C;Vd`ywySu1xfcDuy#86#bwVtzyjJaf?}>`Cx~3
z5>|eD{v9Gx7HUtm*)IVKe^i-T!QYYg+l7!m1L-&T3Ht-&T94Szb%{Mq*O+`I$sOqr
zxO(4Ee16=8W(Yoy4t@yYb1$+_1rNdJGv0@R_}u74560&Y-4BEC*^`M5;&X`?Js6*J
zGan9v&;6u26h8m)X@k!Ncx3zdT<YPo(8cH1N#x`6*Vyokoq@is7(ky-pjRl_5PaSh
z{1C+F9qxxA_*~(A7>LhiFM2ROXSp8+;j=gs9mMAtFM2ROb2A?fgU=t4>QMOn_NNR!
zKU^Qc=Pe#Si(GuhN#x^GJ0OKmSCGP&B%;w%o$0o$hwH5PjNbJf3cBw+TZr3R&TiPn
zp=2|nopL0W7c*?6ReS<{(G#l!$KDCflkj-ISsHP|jw@{6M8@dVaGep3<Gz}{9N|3c
zz8XG0!g+{S^&1{Jdz^dSZ-$$UaDHaKxx)5N^WqBITit}k*w3`#avBWRFz+w%0l89`
zo<t1%c?jKINrGYNE*kt0q`Lz5!w|X)dmjeU-9DtJYB1@8d~v7yVG!MI%R~q1?&n_g
zV7gnA`EVGz`wYPO9c1W7y@bZ0IT6#4^#r_WT9nmxt>u@;6&QM4F+V_$4|w!=z9l8+
zE<;0bZt=^f@y&6bAqF;E5zO{oFB_3Y4f!&O(m#W}I%{q6(}T%3d&w_wlh07{4oZjP
zYqpj?+Z?=NrWaBl(N-oAh+H>7`RQ^2@(!$=8xVu`EcgCzdta?tU+-XX9a>+pi|qC!
z3l?n(<>6Y4K0M-o;A2{tjY)YElFD4m7Q%NUui=}t3K8u-m&1T!So;}oDF1EwGsB5L
zGhU=yf54nvfwh}5SpcG}SV}_+x-6C|j+39cUjUMyl<yAT8E{R6{}Y_1)E&k_h$&OJ
zYc)0LhS<!39PVye|DM;qWM>f&cnjD+yWjcaCo~1#(%mWJJFV0LA<l+>21Ww|z!b3f
zRn95hx|)M2kkv0ROn(N8^aj$&nMql@ck!a;u(4KdOPHlOM?d3HxpmodG-xI+R?*~9
z;e_sQQEIM#(ZnkGl9s}3<K<8_2M1uGIe-J^P8NEh!5jtQ;^JaD)Pvn@0S}<$1R>un
za{fY146oK1vgMpgn(?Nh%}yJXV2d#KHR}=`fqv5X=ljkS|1^NPzyOx%71LYvr0EuC
z(-^#aIE86TM{-9%+cl+h?z@QVo?meOoNow!R&`*!oXu>?7<VyM8NxsL<LKYNarJK>
zDIh3-5a_|sg;tNqCZ0|!e7ral0p<`GN$7fYS`QB74JmgroJMI&#A$-2z<0#t<cz8}
z+D2r+4|hetoO6n8!maZHO}Nr)LYv!!&z;T)dzE@zkTp7z{|NTwP9aaj!GJHoof}~$
zE;UQAgu5OvhO=3?=2mV`kA@SBe?EP?;)aY@bGfuRTxSgsa7<|T?~JvK&Z#iF0p>8*
zpBqkGsd;wt5A0+uPj0~rw>-HGgI~-nGnFSFb?$=-O#17?UAHMMV|Y6yc6K`apT%8;
z8sMO3;`f>Q5Poktgwiea|I*B%Stz4nHQ~f)P{PuG9-rmRn3&JI9M0zq@P4krSfDFu
zKA^hnP<1Yen6A?&iYjoUseu^hEIzV{+H6J$mXym!c`J@naVxHeIck#|u)>=9oi{%Q
z*&$%Y0Wk@21twMl2rgz~P1%o=3c-XyERa^i3D??F0Fb9ST~91sOd-eq*933e+wm^L
zz#>Mcy)xJ=KyBha5yZ6Ulq+j;kDX2y!qdL+8JjgB=oACHAp8A`PQsDuDOegl{$sZ<
zl3)KW-d=DEW5BXR_)}Op1<nPICKY;G5Dp#0HVhaqYN$t*OF16?tOetR@dBeq94K3!
zukTtv17nVMa_9(2aYf*RgtW&K+QNi{*Dz=3lqGdIg(H7d?s{k6tuS0{dasb~>~OvW
zA*(2wX8NR97>)s1O@1N6VVscC>2R>~@z+BE?oLHA%#Be^9baOpY_oe&)X`j)+*dAJ
zCeBb6pR(CXMEj(RK2wvV2QS6OCUVwCYBT1hmF9U)iCHYSMj(<SZ5Wp2$;UnFH~!iC
zHIxfv6RI$+5jeS~)$9O_C9`43nw(9dc@&R%O<SPM*tz>Ds-Z!-oh!vv!<GJpgf;oG
z?u$fh$k*Asgot8Fg-Y3c7YQfEAs#uYgxn)A!SMph9{~!;>FRB|8hCbhF(rmqE#jM$
z9wRXe#><y~#6Qm#TkU}a)y1Pe+y+67oq1wt(MMTNmDobWj5;ZGT^63vCq6)8K0g$`
z_)$iH$&lN2tim}b>cex3J~A{HzSJg%mWOq6>#Z(feo3G-kLUV|6623<iu(}tW857z
z%G9rg++h7vgY~~*GMvyhQsJqL3aS^-gEJkU$>f3X6}feMADobBbQTnDm(v+Ep>StD
zl^V*W4?CdI00GaD3^UY=MKd3da(JTgf)A3fvk}2$s8wB743Z()&dwaF%StDtsP&&_
z#9POlfNa>!XCvND0|=RyencV&?0RVWdW=J2*a2}FWLw3jP+SsYJ#5g_7D>O(bjIuv
zu<P3YfE}u=zW?#)Uk%S;5QRIZLlpl<&3zmpW*ceRYg(Qji42rJz%t<DSPaKMnErso
z9OpkJu9Ex7_3+64@am&@PG7=*@Sbquetl_!YrWAEUfrrs>-djDFq1pVlW%JU!W*@1
zCj`bQ41l3vlz&E7fML^B_<y6h2_~}#(rmMm^ZG|{UN=nLr1`c<1B<72<E##U+G)Nd
z_@YSBYJrSB$C~JcZnSu<uA?)N1*h7ysO;amGSIclz_7Yjz4{b=rJhKS7CNjbGkhZA
zEIBz~>e>{LE$Jmpx$q0H>7Y2as_Af0ohgi%3McNyoaOvTRFE3+iD1niaBDV);`KVS
zyvo-?g&qP8tKys>m})LG#;N|hv_H;}w&8FddlU0=5~8FB(%Ez`z0%t$HVNgD>+SnA
z#h{bYx|NTQW*%YtEj@*(o<r*+ov+ks@vf}esw1%9J19&0;tDinFq{^9hMkq+zaqFQ
z!-;38S$Dq`gu;n#@on)d{&t1q$)|M;M-h^TLv=^<jdP{{rf=>(phD-EJc)7KKCpv^
z6Xr@~QHeGL?fI~g@cL97PCP5F>6h&G8AWT|ZIUU|xsMD=&7>3p&m}{=x;J*0>N9I=
zgJ&1p%$#DSasmE0u(l<f(CXtQsqBNwlhIlH`gGpkK4+#|6Us@iCInEm@B@h!#FLIz
zX^^ulhrA|ma+Lz1V<$y=VUdMS+zA~>+3KYx|Ioc}(6drRj+Gh!n|s+=NsMOTVG+M;
zIcPI+Z`e3vR2<>VRZfz)c8JS)bxwttp~l;0F%E2Rib|q)>9R+|=9a0Iom=V{6dAUb
z{uP~K<bm@%PXluHkdqRpP@>N#`|kDlnHpDVv}iR~5>hycl_%eVo`s7XrKm8?Z-%qd
z?Br%Ov){QBeQsdGS6dBlK*PUe;s!Q+fpTUVUa41_G+<ui&QEYFvt7-89gTc|VRA&T
z;VvolJ_0(>LFqozjkX6(zj>YLHo^AbX&<qcq)XkhhG{wl4{droEQ))%j%E0|_HXJg
zRNZNOE`a{Ae&^LuYQ|%k!O_(R2Zs;@XR*?TyN)5T76*$JXY*$fmGLidPJvJd^xv(v
z|2nU5`;RdhO7ADrz>?QeSKs%0+@nBomx;05mYO)XG8CHO{Q5*dOK#q919>r3Gt|TZ
z*cwvOUCTw9KebE$nGJ#+5X=~Ia+D)}W1J{qHbCukE<|K$+D8As>D&o9#Jbz_NIMs$
z*Z5VKq&uCHNayY$#@VdDLgl>x0ea;j{!w6eI+Cw`d6(PrBBWhM+F*GPn32+6z^AVb
z`t)P-=~odcAGq6QzDmV#Qub?s{W@>Z*Cu@>)y34Daa-Fw9KYpQ3X9M%WpI;m=PQNI
zTy=O5tKDEVs#Q;_^N#)js6C(4Ya+jnXdSw|mXK;oIcSGt&wgUg4Tf8fOg>)9pr|V)
zNe>;R!RI`}*zgW&rl=oKR7o;jVx)|-&iyFOjgQ@YG+47=zv6z~X1_KM{Mzb%-EP0m
z8u;}JzUuteTyt8Qa}rENuX#EadpWQ<*=|fkF_o0zye1AyngWwXF}oDQSk)!&2OwX!
zNB0Zm*}3+csORO(H`wn^#|fhk%(O><iW(mCzU$+|_Txyy1o-&#;K!fwQHd78s4>KL
z4%lAOW9-zQlj}<~`7e2tag(Mz32`h-K17e^IzQ7m=^pVhCjw8|!_1HwNNLD9ieizT
z;Z?_xn&GqZYR07H1L4H=8lTnw?Cr|sqyA#OU3u=KcclB%YHhMhL^}RK!>4iO+-4Hn
z`^)xfK&`i_M6A;8m)pX!H9>*po4u@ilYf?3`kW65&U-iN1G?1+=S28gX==i$@#=SU
zH<?pwi&6qrSPW2^`l4KPwx^`%g%UWtFKffoHoGD-X)ePGuiL0A6Gm;N2;4EP<UP(7
zD*_&nnG?IVxVeXBdKT>I2n4gQGMtcqJNWWYbGPZTE<?iL;3xLg&@8Xtvacf2^6Gzq
z*&RIT&&z(jO|Xk-p>Uyn-Rr(y&8w+eCwhkylAVF7i)~`ZO+4Pd?sH#1ZeMfA>FxqG
zj(g62XbZ~9^`x)eCj@9}g8q0E`0k@X*I#;)WC!cBU8+xiJgP4DQT6Dr?-nb^KXklN
z1a7`8pb+a6NF&3QO<M7N^oJ!pX$ik$UJ`PigA`)jCtmuuNT|2CfEZ|JW2CoO7A9ik
z1;I(Ey~TU=5bG`8XTsDa9I>%Uz7w$|L?1);8!84!QLRbS+g0E`rrVJ}7S3w{fnoUK
zv;dq)j)8EVM55?Df-!wBatfv?AFjv=8WlU8N+8y7OKx_~f@So*(36peSl(jE*Js3%
zZ<nRMH;(F^---siM||nU2tEF?9ivA&H6H{Mow+`!o!$_%#y981V-NPL$EbL#>ij<F
z6%2orzC|&~t<KL+vUS7$Gxd9y)}scE$e~JG-%-wlk>pR^Z6N9(BNQPU%~hXIU!s!j
z7nORIK2qUl-3qVOY$dt1ug~%iwaj_heA{_d`X7|4>NEAbj(TRHfL%@t3cJ@8(nxWm
z#4hmZ^ue+;so5ItlF)E#=1Augpx^GhH;_2IuD`2)>1mkL*Yf7`gNH~?d${-Tv1NNp
zk}ore54h>pA2J=~?-G$PgCULnlH7gtlJlbE0UpgINyiQh_CU)%;aqy0p^q(P0s45B
zY=({_Jcy3ppm&s;W+*p>L_crGmGeQHKM~9?gO9JvD*vyC%WwI;S#QbMZ({(@&EbXJ
zD}+YPl%RHuY})P<(V9yE_u*)zMXrmyV%d62?PUTj>k{*|N5hFdJUH_}JTpsqPVuov
zAEV*hwxA&EzSPphi{Y-F#Q7g}VdzESgwBT}O*DLKuSxSH?;Hnr5z1i&lEkmbT2_dG
zZ~cbAc@1p?n~v*cCSV{dnY=NY0Xs8MsqcA4;4;C*SUP7L@IjSE15V`2lx+0)wU8TQ
z?am<ul3e~Y!qFgI9BC&zIhOo!o)Vk@v{0xx!Ne^O*2X*=*Lite3l)II+1;8e9YE7|
zrb5>+iQiokAOeR*nNIYa&R=B+qho(be9}L^)0F>+v~~QDHd0}UjOlxvd=e`xgNIN}
z*T4AWf?%xeZtZ6YbKArlvbE`wiPS%d0h9@VGNATWTOfET0x#*;sZw&&=_a1^*+I~s
z_dq{QwL*XAkO<H>noP@y43#EW1kMk@jUL1td5?3qt;hKgY)tq$1+bh?66r(uwuSJP
z_#g-$XYxP^KzJ05PS2ol_8asO>u23d*7Qg1B*pfRf#Zjob);(Yi-kNFkD93NJlYsu
z3|ya3JBfSSh3gkI!dVq8{3r$tnAjg11Xqy{R}+M#GTsz?ztFs<N5%&co+VUGF`-pM
zsD@xQ{Fn*l2xW#gN5Zwh<RVYLm~9>G7aB-bZFSB+f!%Mn?&MBy10g*^@dj81p6fGh
zUM+07)Yw+(+!M?ur_=P90raN)g#y14IVC?P-q(N<@7?)Quz;ggfTLa}*>xmS>)!$k
z=?O;68JRZQjsJ_~@=m>!x8&wN+>(tLEaJN)#N{?#Mg7`MdLdDR<iP-b*7-_DS@q(e
z{b#*FB<CWTLt8ZVE7S!Wh~|(1y))%?J%@RjtCwviPYFw6iE<F>&bJe3Ju4%BvJCwm
zn^3&yf%4=d;T4^H!!q=`zWJ7+y(4IdF)OJaj6+YT1kX711Yb2}<x_DuF=0UAzOm*3
zV;nNYI(Mo#Qd{HDKOv&`VjQ}bTud{KU349J-qkX6p2=X1LISp+T&L%NL9M%BF2Xp(
z#K)NSb_$$f?Fw6=imr`N@;K?3ZA)Ye!XV^&iY$g8T}z2OcdjL7u!-n$r84LnU?n<F
z(HUdOr+J0xkAuLg5t$vCd293<_Or-9QfKTrm~hmbVzmW(&R$4N;3FW+VvpO9O2q|i
zJg0&UV#cM<V^~1{PB$KC1{<jnp;Fkl^%e!O@X}e3znG}tfDL*YarBz>XmYvopQizt
zq%y%qet~k}%kYdnio>qc*D;_x`xN_n)8hi~8N10#un1#!S#qTC_SgW@>q_x;pn>Ek
z#0)aMe9v&1<+LT_e2<0?fOCR{^Mf9oyoS;zfR%y8mQzOG579L&zt#T4a_;E9g7Ffa
z8Badtsjuv{kX+RjlkVa%T7x(vS}ZIPoR118zRkOHG|G%oEb^=lAn?Q=t~<htx+)SG
z*5>VY%TYw?OXF;E73oZbn@cR-2GpLKdZ(dLt{f)?e&w!We7>8q)1+i<(%D??WNvL4
z=iDpYrf$yPNTJ3U&YnY->73WIMU*@lGb+^O7Z^*9WHBM_%s9$I@FmK9vr`V~EJ@(H
z`T|dC%|C6ui6?n?K1p%V#CIkK^n)Lba0%oLlcU#p99Vj=-fJ)vI7gCSPve{<%0=G<
z&!6~tQu%V>X3C)4iz(O8+UaglH<3ssSzV_jxi3Syp5FS^U^y!(r|;L&2Yfv!FMYM-
zjdN`<-Pe?Em6>nT#2!q0>V%khoj8kOE5-^U3}m_e(X7uLOFn9d@w&HD>|B2=%I-Fy
zdn|odh}J_hg@HU?64z+%ro4)j@WXda_gST@-lM%Ozcp2AZ_DsS=9nV4ylc8$Y(rwI
z1HT#7wQ=divF=uw&!rLVFr5J|u#M|f#!-e(ps{9i;WU?G=*hbE@UG3LxXG}D&%}!j
zw;Pu|%@tfr3)!QpX(>}0O-aq+>UR8Pa&NJ@1DSi5{Y7|x*5W-8*07PIpCs;6Ibz7J
z0l(M0xGsRdH7_pT{wXg8a%*<^Hm^=S-Rx7`c1v-8xM&c?jUrL{q5!}7Y%tfY<Vw%r
zYmakEFkYK)(&rP;WXiVBc^l+I(rr}WSGta5hQ2>8Qb8a60g24|VUP1LkAA}Msl3lK
z4xL{Hqkp03sYDM$rQHV)K*ViV`k49=1KY>cx6F%+sUGv<Vycsu0H)e_buJ24`m`#2
zoH5jSx7mmkoba@%Ydrqg_A%!Ta+|pi1e7pblg`b|aNIZzFHVt9+m$%8mSaSo5pNNr
zUj9^kt7|u7-*)?-kWPMMwwo8XF`Lbc+n8VT5@^g1dF`}r2jRrm!FlK1JNO?b#s>+a
zo&6WJU!DQ~!_K+GfPZ|*8OQjbEC2g~Kv%vER!mnGxLx`1fdl>Nqv4&<ecO+EoVVXG
znc9Mx{^e!LcQgHfMCr#_%XS{I$xDODcYDc4yU7<T`FBa~iIs}<QaZ+2Z8PTtGwW0s
z1K=m!%;za{t;wvVUZy8?h*Du|obwOijUl&4!v>W%?Vc~>&Bw8#cQ0cwC-Rq5npT^e
zwSL>tMQWp<Sf>%Aw{S)}RMBnr@5i+#3OB1q%9C4}k)sRE(xNoW5~Fl=3(f~R0#P0^
zx?IaS3c3>`mTo5XCr?s~dH+rh4O3gR*smiP2iS|#r@wW8{ihULAV-^sb0Z^x?%I#v
zY4omlD9WJWqbf7A*Y}?N@H}@bI^KrlQ-cc3pfmHy^KB`=q!iS}W|~B4*#xFEm>Gn>
zg&A2;n|cNs@Z{UuJZCb#MS5VGPiZFqX}tH{7qp)ZnlJoc!S~m|+`o_SpT9W}-^bHZ
z+V&r~j7RU%8HmH-yNs$1i|>cuHl>tMN(OBaue9Ouy?{I(zPowOWIU0K!1PAZ|EKW1
z07mlf<2(AsKz!c`>C(23(6;DZ=Xe+zMBjg;lMajTt86Jx6$bG=$t%r_z#;VgCY|Wv
z`xNqL@NI6&NsYLP(hf)8@#I0nQt*f9zQ#3?imQ0D;XKWTkpL*HUC+zP$K+04wmcOp
zoo2Z@r~D2R-flL#$_Tj8yxZIAl{luvnl8%+TP$CZx$GCDeRh{Ktmt|KN;vT_Uv(<&
zX2K}!#+*quzlT%mCpqWR-T`a8Qt3hccg}U2>5^sLKsK;_-vQPFnT&D@mU}?jj<)%%
z3}tt1T=Ijywc;=0SC!&d70%-%r_f8>LZf$8Qp<gV`g0^iz!@v#+tXi>3*_Qi72;Wy
z;Vvy%xc#D4+8H=yUl#42W}4#0)1FhE1=KX4J@wR-{xrGSpSH+?`yBck=edPlaC`gg
zIv8{0b}lx-*Dus|f)LkrF9@TyaMv$+5TiCe+H}|+GvSbm5|N~5O73!e=L~2D#9u4k
zj+=a$w$7RpaC6CJCMSpi*EgnfOI=3UPhap)QC^AWe-QS7Hq5Yn^1*Wgq?cY`rlLyU
zIyilc+I+6WX64{djrw#dpEwWB@Z<lV(d!C`WB_h-P!6QEXTo9WHDYM%J=5#yB&X1C
zxP?aVT172`>2(er_J2&Tzu{Db0qyyYI_Uo%y)J(3e@m~ay&k>(^IwD7khFbr6|NXQ
zs|?WV=E3RLs?CO8*AD(956=PgDoe%z<Mr_W27mpD@#8%le|;KOkm0Xlt6GAIc9*-X
zs5k^4Dsgr~90U6GVT+TC<r0pbci-@QwZkw0JHLwK&|=u@|A=4h2q$i$B9~`<v+r9*
zA9VTab!3BH|Bf4M^sX(`G@O3&6FMy3E$`)*;jbCynm*U?){yh*!7%HjL5lsV{Ko~e
zRJ!!Vq=x%m?r~p#K4|#w%h1z+c5hb)9{IoEzq`X-YpBog-_<+>`R~hz<i9U7a^b({
z1o<ziXUbij+1OsAh7Zes@7;Z%-zgXj{tk<Ro^dV;q&1{V#99x&I;f>jsii0L$vfW!
z?~XX$9a>k#r6b2KmzGsG6*@mhlLLoWT$&p7%J_|ad4})1S1HK4KribUu8Hn3Tz|_P
z!`1m#f7@72;%Vok<lJM^ROzM1VsXhnj^cV4r9H<p#j<jV_jT@wZ6qYY5q)<m!z(#T
zBXuD7cXnOV!rHUzTkGl<w6`>!U0v07ZGGc4i(AjGuB)$UYN<baUVD8*&DnJ=&8<sX
z&;D#nO><RC+mf@dYrZTxqjZKBp7q(W<IZkvsXlvhQ+s1g>)7h%X19{}vHbeR>W223
zTCdKtXSC1zLTz>1%%-vResv)WK%~ihic~i(Y;LJ<t#4cq8C?@uRMk>nHLszzHB!La
z{Fd6<GZj6*zN5Bg%z4Lz6cY)VS06>w&UKr5jxD+L{co4>HlwMby{*2fG1^$wu%xv<
zh(T4j7i)_Nf5({8pC~f;x7>SqAMtJAy1b&L36w4Ly7YZ4e{g^LRRy{;l)-;_Ra;Aa
zhlhW!z5e&28D4&q<k=gZdwu(B_nr4X!Yh1S(N^DJJ7;uj<budCp~&d5<Ikx*ny}{R
zP{HWdF`>~7^CO|rHOGV&hs?J~C~@(@{@eN8!|y46llax}n;ARU-^TA&e&6Sp&##1^
z+Lqe=t2^F&VEu)~H~*^Rv~?$Lxn{yEyWfdkc7Yq-d+PM$nh{y+C%NBC-sg5^##Ux1
z8<Yp|?F~PJ@AtX>cw<d%M?-yUo1wdDm(83Woi;-NywCL*qN=XVj5oi1<0sBovT$K7
zG*=y@yv+CUCAE#MwZU(KFYulGUXl#eX1;%B2CQ^Q0h#X;gWsn&Ew*K3zJG@D{Um+`
z?=ya;G&Q!iwN%wNwz((@L=4Ylt1~$ao2_rGs&1=aWLd3!6tDH7LpCZB5%T!Uv~PT%
zs?ztn-!E$xx6O2W=$KCo{fk5*F7uB>&N?d+xx6BBS;frwWz%NxWoYta1V7@Yv8}9j
zQEdZhhW=d;iHvSl!rDejqUlu&n<bov{0&MswW`GxY=cq`PS*hMq`)DG2c?@{yP&-R
zY1mpNGz7{Vlx}K!Lql17W^HR*YakJigVIgAa>_7?hNdfNsh{6ArKzO_xjmp_l?Ti8
z@G_;Y2{C=`lIgYcp%2s$570sBO0R3LYN&6kN2Cw@^Wkf&ZCMDdskWJv*;3V5T^mTM
zbhWB?o)nkiQrN@;(@n2!sOlJuI+M^(SKHh=t$ksjrh$*abTEm59|LcGx`kC86)p7(
zs~Rq8U)UT-;64V^m4K7sQ`vqq?X7PFZB-42B0#^q`5^C*sfLz!C_?qiThP+9_)v*x
zi=WPgZ$?{HTYKveat_d6bL&)>=!aq@P+nvE!hu9Tqm3@|v#4H6f6c7})A>ovgAbo6
z59HJ?tZi%?Se6n7%Zo#U5c0r++&8~GfEjk64U_JWgUzj%{yoO<BFH-Q<`;gm<ET*|
zx-kF8mH%qF=^qy;F0^uUv4X@iUyCZly~gO5%M)Ymd-3YxvYyY~w<hc43;w=x*O=AU
zZn)s$Gp_s0vg;OH@bRyGdve)hsSCgHb^X115on8?1y4g28r^!<h5UD(WpVC<VRxm|
zr(ZTbG%GrNT725Yq4>0^mxZoqytc7vabu*mrKPC_J-wzjG^4#5N!&WWy&=+4+t%LF
z7%Hh>gz^r9nztnK#oCspkWmXFP3>)wruh*wWLra}rUd$9UTsT+L0qd|sR_-hYH1W}
z9r7XGIKOFVEQ71DEz)2ZcXeG=;{uAXwMh#Bz?PQw<~AayHMP~L+_t7jO;cz_ZCjh(
z+M6vnk$NdEk@;2i4Wyl3+gjV^y~OQ^^H`aiWKe8PQ)4YvL~1)Annp>S)~1F<G^}b7
zMM`9Zh^ud<p{Ds%V&Cf<=hs%X*1HK>eN;9qu_e@iab*|W?%1ktt&KEW?LH(LH4KeL
zFM_2qD;mIf=QmU>2$axTwWwCW)l{`rg{Ca2ZV(P?YTIDzK;%WJaOzw_cbZlRAq$%p
zDee50riBq#^n;`GLeZKU7pw0U>)|t_9ta;*4K1}*HA~zw?KE(h_<@-ko7#qC2*kyp
z#`>zp$U-yoP-*I2$g94sHh_#8x>T4AXg-0jR?r{fV|z0gG?+AXK>mT8P0a{~`4Cbd
zZgC5B0|vb~=!=?f(K_6Rd9{tzbqlLnuC-k@*?+B?*VLjubLr5hxS9}CQqxs*b7Rw3
zk(o;@9zhw-@jmOD<)FN3L2az6u|_oI%{tKwn<5MCx5#`L24k=_$U8z&4c;n7Dg#K&
zp{=eqQq{7cUF?~DQ`Z@~HbC>A;TmTzwoKw|M$a%jL1|~IqWSHOR$t-SABcw6v_#74
z8{0dmwPA@VdM5Q+p4L=d-HxJDqsm28OJD_t=b3cVYp-iZt>SaAa4%i`!e-{&fL>cO
z)+7(|qPnV9jpBK=jH{-Wnp*0>HPP@L!>ejLs%vX&XfX{(ZIIFsX|BVnP|ZU_ZR3Kr
zx=^UXi?9p}7zc7ycRcd79;$T5E8x|^i)@wvYXihMuriymXj)v~Ru`!{Oll8a;nN+s
z^jS@9YrPQGYJ|RRcLomFs^MMYuLJ8=N}sb>;0%8m4$di6jY4wk;wm_HZQJ5n##l}L
z{P|!J{%>+ncx#ZcXU4VLa+h@*0Lv{uGtAg8(61Q8SxFFb(*?@%Y9A&sCC{isQx-?`
z1>M=1yqRJ{)0;rndq^j`WC*)<F_DnaFi*{=y@)SKIJ&Q4$(fOM1cTRmw$gSf;ikQw
zktL!cyuLHVWTXn@=<yRz+AXTD8Y<HQ10z82rkcU|EgB;=?M8sjynLW|8d~4L^xkv=
z3X?^!sTJL~i32|mmG5++s=BGYp~l6M%kob5+jWQp9)O-Su4i(xgk0}6pD)!L;dQ}$
z9v<Cxf%`$iX#lh>-t<nK!TwD(bf^*3G_S3yzA>`6sRc{#0y~u8s8#J9KwH(agr<=t
zlirfYyCffyK3JUL*a7m0U~_J2Yibt9bj^#fACFxwu0!M*gbO@6!f+FKLSs!;3(Udj
zL+>G<Z8sB$7BfCP;pM|&)Sa<oUH+QUj+$#*kT45<ATxa`l?)bY*t;#At+>8cH6$OM
z)Z9|LsJ^MaRn2gz-PMFjP?r3esRlU~`1wYymE<D1R4bo*;QTryJ@Xehmp7BPgb@nK
zupx#|HO8Q<q8n@L7u3O_P;m`H=WDL(HsCPuBb}PTb1kY#drNHwA3?z9)mB%DUD)<{
zWRJ@7Z{_pJK8R0OHwq9Q45u14<td78Pev9XNL$gI%wM?#9usd@?WgsD0uY$Uz-mk%
z8vk^U?P`q@mk!bCk#_E~!2p%+^vLX{cDNpjFN3JHxwg7~{t{5TIH=m4;Y~_H(Rqy!
z5gif4)}h(gVblBUYk*E}+w1At)mE+<rNENdnQo_u@QoH8=pE#XVfvuQxAe|j(kwym
z$}7oTW1=BRM)0WVL410tS%D0sx59Avq()AImVw0@aXoCof%LZ6Lnk!|@%4Z~GYqd`
z(T0XVd!Q&N+s-)7gx8~twHOsA&}1+xU8}05MszZb4mW@V#Ic4p+G=f83v->CTGaL$
zlnL8E@0woXKac};l5&iqVPi2@fh%`X6adSelnj{Z`SYwG|HewK;cEeW&jnlP=~wV&
z+IvV;*=`w#B{QD1IEOYjNKi@buo9S&<j<x8eAFHH-h?P3ZNDjk5xTL}@a07m+tlI?
z_dq^xWY?NO>7r-Ah*6#ZzcVv_Y5t5;OX`ggqq0j8WZ0TW4-d>g7Bn!B8vJPp`f@Xd
z2+g*@s<EaqqxSgfWbs(R9_X@wwrgjFBz~84?%Yylhrx)H0%r9$!;9)|)mNCsPG14A
z4I8R1xWZ^?N0ed|(*$c!2@BhqS}_V*+vm@(udau@A$2dea5R+f4k|&?g!zbDPGeK!
zS&g*|0B=#PL9s_Aeycp)(l5rgFRr15QZ*PGLH_63woL}RLgG!~hfNg$JvH@J3!02@
zZ)`&a0zC`snfx~{2=qT3z_3JUzz53WS8b!Cr7wabU#fVTLqq*S=3=&_skMt`%{gpQ
ztyavK&OKCNzDyXP(u!3Sv^V<Tv94_lXM?Z>C<U!`LH)L^4!dllVSx=z7{A1UG!Gc#
zwclFVY-Gz>u8}QB+=!VPrV0LZ*3BsX;R$M&k9ag-7TP0=>of&%Z4I#6!Qhuk8#VGO
z(&R4MxN9e#l56_lg2^+lnK}K6QbW96ab*=An+S~i!Tja0YFcQjhBsqaI<t%j8qOF5
zHCZ&qFLxmco>Pl_5T`P-;tSBZ=JqZFFgdDa2&-zP0NRt`LsgL75>%*y8DqT3Rl(#j
z?o4f%aTOGk)>7FTSxh6)(^WlgJNnC5csvC}6YGFk&?XBF93Mve5r;N<4vlQl(h3Gx
zjIs^>AW_y5Z@?Hm6|!qJtC`+F8!*8Pa>wE1n<utp=B;UIGAme-c@0g~W`dGopsE)|
z)6?V!OnI!%W9AAF=>phFV?YP=*||>-?^2GdsX>p}+Jb6-ARkO3qf$i*n4e&ELf0P#
zt}#Kl$>%G)@0M+JYfb&a*0D|3HDg(ix#0pgY?$Xhw0jRx|BLt3KF@pVxQ6lLeqR!J
z#w6gsz3=Pmmt9_A5QrSEHO9R6*<R?4n8}`{aOgTNt0IF}aVw@@R#7^AX1sI;ABQaD
znzfV9`MyHZ5RI)w5?y0jrt5jN4NZ&3l5$`=!{LUc5#0_<H>5nwu#0Nnr93~4%JbwU
z#H5uGZ`>8skF6amZILxVBsBC}W@estc#KgE_Rq`j>PZw+z(Rk$3_xRi!S3s4hs)<_
z0e)ifi$?JaSi&K+*H*v4p>X)+wbm}Iw~~h8>tz_4UJA6e$t%sE@8U!4?ov}56CAG?
zjYly}GZl8Zc4jb>YMAetK|7EH3_?lYT3fr&v&<fn5bT1c<TN>~HSRn}s<yR`?n1<1
z=3re`vco80Fm(&^u=(iT0d^L8(N$n6%-C)G_8vCBwfa~oBJ)a(pDi?ezKk)zs6N!|
z>nU#5;q$rdbYU&}mR<L#lDc86^`22E(p-i0*hsVhEW?%uRr^K`nN2L+3$uRTdMydA
z?*)-BRnL{2=-SN<RScR)Th%<dN5~{@RD8OkrgmQYf(0mqZh`*?<(U!=mmk)0X2c9+
zEi1Uo31UOdnzGp5Ib43Bw|${ogk6k~aB>$r>_je5l%e0lm4|O=om(1QC9neG;z!+|
zfz0xk_mR&Z%1>}|(C>4GrWf^JWGdDwm%NF!r$-9vYdTnnA6;{%yP{xTT#fda(Gy8H
z!Tl8x4*YqHa&*zy&(!j9*#Aj%%zF|z(=Hnu8I6x@&2aMO6I!k3LnMx-L2$jE)t)l5
zR--o)MHtsQmHksx<J3yM2v!`FyuVL-51H_=Q8quT_7lgB7l0~x^!Tyo$WO*pj0l-=
zS=r}GONcO0qbJyu7VbmlV;uf(@{Lxf_@H=?$AqTYC4NS)FHFy~DUb2lKD}S>%@D0<
zW3&z8&E7&eU#@_^4W^r+Q6UB-J!5z}ssLTds$6?BEN@Vp)XdPlY8LN8^HfWymBpD*
zTUC1q=BA~$5Z2VmA!dJ*L!&3w;3hhO|12q<L#y?(WQaG##G297k&r=ogp?Ty<XcTd
zhU<dHSu>`)L|vKlPJc1K0)D%WdZ&LS$2K>+&+B<#!*4miUBvGrEaX?oFNa@@UxeR2
zzJ<c?^l#^8-*IlJcRQl(|Gs}-k*|6g-$U2m_P4HI>OD5~zvNf4+&<g?XR=<^5YWu0
zFdcFo3~?OMpXqRswE4?)y6fw;Br+(=q0+}2Sz?uU*q|H((+3JOw!k5pnXl<tcm}6G
zl%OA+Zb-QUw7F1daQXq|2GYKdd{zF;UMG6$^HAa+z(?z?E7l>oD*SK&Up+K`^$1;|
zIy*q$L=DSrgYiCn48wm>=pm!QI{)EWFnO%clqm3j$%hR`j!sbt_=Uem=IbZRu;|Q_
z$~g>QhEWdBUx6gUHZ9#ArkvsV?VvI&3#|4>>lqjyjfvCahqfA*ExvoYnRsP}stv2G
zHmnlPI7OUs${>C?m@j6E_F3`bPlsZmhIyg(dHf$Gh=yi{E)PXJ_;*c%{%a3Soqi4K
zKxk^E3F6TyCIsmJdZ*v{K0J@#27cqXUOJDsW}Ykg74s|Lx03jkgdu)=d0x+N7r$-%
z?%?M(KjXh#=nXBy9^rz!&Rw!OI>k-5JnNJr&%gD!Q0VcjP$=iF1O4lHUbF2$f6j+O
zp$&v<M}<N=2seH>6xv6a%QmdM2M_e`BP=A`@v%^-lCY2?H#!J&*`d6KFpo>1HV`hy
z({G1DIB>y32l~eqGdCh^o)!w#DV&KLAmMtBuU)IKmVDe1x(_&V9%GMnnDU-H(0>Qv
zKEmyUu|FK>59RXxY3e21LAac7%`@az{0`C)I)pjLlaDY$SoiFK{tCjCgw2FG&(S`;
z6FP(&2y;)MUC)z_u!FFJFz-*apRj;%hrSc;C5#b9z*8k*Az>Y11z`ta9pOsC<%EU1
z$WQ3}jq<_gKEeXR!j}&87b|3MoUYi85$+|dBg{FG{DcLB+t}>YLFf>!A<SW)*akxF
z7u!J?Biu)r$Nt5RlgP)m>Rp7DZ1kE-zts_T5Oxsu5UwO#OSpz`2jM=B?%7DaIUEYI
zgRp>bTpr&ED+oI{$6_U69j91qBP=-TV1Mq(<RhF*xGm>k{~E#_gzNRrL3p8$@J^UV
zSjoXem4pQx*|nB1M!1o1Eoa<?P62LCd8;7YN7zGHz`1j4^-j2rQ0I!~o=SOy1%x>q
zj#{jcvo<RU9l~b94IC-DmT(6r(-uIVYdFrjhp>)AoO9Xly_Rqs;Wi!WOz05S={<6=
z{|>@^gc}G8PCwYcoiK-cI3mz@0b!x&oN%tfPk;}?ynNbASUCD%|6anCggFuFB}7#U
z?I0{BbO<X6SAG&a5w0a%Pq>Y68{vjCz{Bah6Xp@_BP=HDC?Fr<2Eyfpbz{g!82c1w
z$Pn%$T+X=5`!wYe77(r{+()>LFz3vJ{k!yj*1`ULdLIiOK0*4k!4F{pVX@x1Z`{#4
zVJM&WOgY%!K^P<KAzVYamT)cMM#3G0y9gb^eT0=Ilsg*u2nz@+WAumK30D$!TmoE#
zD+#yheH!(0Y|2W)N`=$GH{lvWhj7O{>gTj8hp>mRunxR(4or+NLb!pjkZ>De1z}DD
zcp%IpT%-49;L|(dK80<-!+h)iVeehwtEjHN|Cu=#PDpYbG0})o!$k-P5+Gub03it^
zT*RP&QPH9zMx_=NF}7$?6j4#pBBIhtt0G!ku_DHnDpkbvDOTD?5v#3fOD(nfh&P(H
zkn?`mo;Bw(IiP*|KJD}V|IX)=vu6L++H0@9_uAK)Jt>7QffhBB&;Ep=MbKSK$RD&F
zx(Ql;8~XVgXCt%$x&%5Gx)Qnsx)!<(+6vtb-2sgaz|PyrKeQfN1#N=Pgf>H)p*x@}
zp}U~#gujgTCVc2_=t`(N5dF|nXzULB6j}tG30(<YB7Ep-Xe)FRbO&^Ygug+&;|N2m
zplg@o-_RE5LTK|{qzhdFZH4ZD?t<=uT7$^{3i1t&-A%rsMJq`US_)kat%tTi8=>2v
z&Cp%YB~Yu3^q@u17HAoC2ecl#3px{8bPweKErrUN)^_MRXzX6>f$o6rg6@J^gTe1Z
zAG8Qs25p4Sgf>B&p=+UQp)Jr&(3RgJ|3i>lO}fw~=uGG?Xfw1Ox(wR<ZQ?<fKsQ0_
zzeD*!8=<?QMGsJ)L$MQD0&Rrug06*H<)rrz`k}Ek=$G)r=!3RGHwnHLeb9R7Zs?9j
zsE^}G_q&uQv=!RO=M)+r!!GDr=rU+KbS<>y`^1B`LSrYO?{Vya)<Y|xJJz8G+WZ6R
z0on@P4Q+gqdZV2;Lz|#WpsVrE7HEt3E3`uLwH~{n70_mAJ#-ng1=<2_g>HlHfUd>f
z#-|8Fo1nWSA5cr|g_c5N8^|BD2wGZ!+%xEfw*Q!T(8gzxA5K1>qaLA6o3IaB@e}GF
z+74}l?s%U1K9Tx@mO&d|z)om0bRo1I+9Ki2v}@>EXvqlj^CEK4Cg>7q>?P8L7D0Cj
zA8J*S9<&Tv53Pr;Z6zMG1=<YV1zibk{3+=`o1ojEty@VCTJ)d9XFk*PD)FJs(3#Mc
z(3Q{~&~?z*Yxpy?6xs%@hqgnTpzbK@1zHMifmT6xK^q0%PCY<lZx9dK1hqz^7g`Ff
z{}uHMT?=i3wm?@xcfE-`wDphJRfXJc>K)qnA$CC9|3W#9LB1V6bjLp8K^r^BC$tH=
zP597V!iQSb=uO}+(8f?lq6yjpT?pO5`g|L7E$e!>hOn&np`|&@Q=!dWI}*#FOQ35d
zoXh+ax{@_~tQI?=Wzgn>I}**%iUQ`a&?3&S&m4<AM<5SvhPFXh9*O)o(mNV`&?P-d
zrw-bOFtn%?JL-uCErafW)<f(2VJ~!Lf9A;&J{En@_5sL^hYn<JE%Z3%+0arxu(=u<
z8_XOW+B}3gx!~p4J%RL(XMY0P%GV<5p)DiPD|ij_?up3NGG~Xj)KgwUC!+`2G==h+
zM3_$%H$#_91DEhw#5;-nU)YgofHuz|edvx$ncG8G&SS1Wnf%SCJfN-3=sy{KS5Z#T
zcIYx_(O0np+PDCH4cPy6(uY=D-;vk^ZMuQ<r=V{!^3c{B*<*pW+=QM}u>ThHLs#BP
zxtxlg<<u{<=swb)ie1pP(3Q{~&=%-!!B-(a4g8ywAG8&^1lkU5g*JZ+d!}R0{gfAU
zC3FdN7qkW14&4TATunJco1xZeln?8kGHCN7)Su8t$)DiRR%pdz$U}>sKyM@Qp>v_T
z)?*iR<x|wt>EIhES7`H(NDo@{W9sn?^7}0NPSDs3)Q5z(P%hAlm(VZx%gCKcxj{>y
z&96|N(6zrnkA!~@e+K&gi2p#_|4jJ^{eW_X)+ey*EXwJiL}H~-H<8!|ZQ`q`C1=Aw
zB$23swjT-~+T1;nh<york4hxwLR*hcBz8cX1|<@uGf8)7BC!eDI3kg7&%r;&CK5BD
zt*0as>!3|%BofxS;zx-@1GMPML}E3x<?HA@kNArci6zkX#fd~Kw0T7$Q8WvGy*H6)
zg0_4ok!XSLSc84%gRezDwEYp}p=-ZKy0gK*&zJF_OQ6lrwa}H&U5_Uc?a-F>q<;Z+
zwxAE%{2cL`;BP`6+W34T(FSdO0X-MO--7+n(w}0_MWp|;L}DRSt^$u(_E%4_>}J<K
zBCktKKB(RsyqLd+M_J1p#7*2C7cZJ{Q0|qnh1TfqBl-{R-BY59j9m6t8zvbIUGBT#
zmJ;g-Uk}aC=xW)xH&GX|tniPh*y^}@yHg!6x-C>4FS#{b9WPxPsg0K{j*f{}Eb3Al
zUuQe7b&Xfl#LLFSOA$eoDAn=UxLlfpHIKiR=k_LU45RO{{pcGb`f5a9U3`-ry2Q?^
zuqGE>at+{bD}TFR*qgxRt?*r*-lfi%c+uj}n0U#e@SLvkqA|vfF}WgNNja5l-kYd}
zTD9>Kr=e>+R-1bXzY}uL=C?Z6xdJkgM9LAFgIv{%dlT!qw@|cLbT0~xi<f=X(>z9^
zJ5!Q@vAI?)q7M?Um3W&a9tDwZlNY%JMV@8#xz4n%@l?1@Gbr}Hhn|KldlOHI9_+me
z+iHxt)%rU%9uppy)N8TBNkQai2_X3!V=^;Agd{(e@XKD>n|Mw50lOx|ciYa7(=8ce
zEGa;A9`SY%4|hmjV&5e2ZQvJkAMQs7kt(CQc*%tnOKsexj3^dk-$vwJ_DZ^m7GxJW
zlals}eQzoMQst-0`yzCWGrH<>FEK@jT9K0Ji}fn;8qvFe`|vY#Xyoly@z<r|2aCfs
z@zO<+iSaV(wiJM}_q<bb*qb<0$=FKb58~d-L%r1V#CXZ{cu|c>)}&L3+-l^mk#sKX
zoKCH#Gk-tnY$Sdg@rNbTsg-mnt66?g8UNXdoa`wr7e5WuQ(e5wd5~(V@f~Plu5)qc
zYBT-BfH3~U-cc7xPcNy@T5#EG>gt0~!DoP1Wx?lxFL_GqP2-6CQt%SwDgNXo_=Dg@
zS@4bEZWjDi@EABGc}cy#Cjj^wNx;<UsZytS_TsQ9Yx<zjP>0g4NvqXb<F}KDG9bcv
z|E<J_O8ndLxBc{Cb@7H!NhE8RGtTIfd@VrFHva3TsPiwu<?Fny;4BSHySoJ6l=L41
z-wocLL%^gT=tE7PSL^k8tDR6;3<JOdooKZfAnEKy-|9Czr(Y1I{ML7_p8?=yAMimi
z;`*29sRb_uKg0*2g3kbNKz}+e{mMM>M(}LyR`R)A`SLMFFP~$xl(WfaZG2|vAUj(w
zi7J<*w-bH!Z|zMSE`aGLPD}O^!Vlxn2jchs41TThO+M+O7CDoX`WK*omhxXp=Ev0A
z*yPw}>P^SKg|_pjtkqVVORrUdiw3H5e6BNh{{=9M^mmf}O7=+46-PU;-^kb-dV?mA
zHK9;-_ED6MPx3j5E_C(p_+Zw*A)n%JTS<TS?>qbRPVik>@V($|S?~hd*N!as0C4w-
z%yu!Abi?2+$e)G2@H3|#po~o&PlzuJg_cR@(77`G;oA7^D)jE=e>zGX%J}<bhF@Qx
z9B3o_s`vILekpwFb*V%DuXB|N@ol#Abmxw|646hHw~Khc)p)l$v@h)kajr#?3Gp`D
zc`nPSgBiprr=wrVht!_c__xW}cxwoMUMl_QVwuY=iq?sem0gX$GD;$XiOk`=d4RO@
z1?X}AYj2`e_2`_?w2QIHIiZ=und!tmS=)uDPs(E}`j(;Z=c=z$c|6&<Jj8FpbcoV_
zF-#^ev9B1s1pN0&e`XxNP4anb*w~k9w{`J#PUr+_9S7)d=b&#B&l>WRsd)2^>%m*V
zFV%FkKA0~o4x9QoNhVXj3e-nEv1oJrZRW21!@Y?Yl6D6A7wThiRL6y7j`MC-Gre+@
z@+in-&A?{@P5&&3aEadx0AC0`Gt*8peyZPK=Qjd&rs{hR@mKTg<lIa<Q)__q@?33C
z&T6JM0I_Ey`Zn=t&0mOtIM;2?1o1=kEe%f=KV*)!DB2LOa&C{8F=<QpLdoY|;?Lw$
zf&(Nzxj2tryO`SoDySL!KNNSaqv1`+y-GTD<NxqW`R3~7;xMK^HGYvTAE7DqrNJIs
zW&o1@0`$z}Q?e{UCH)5HjCf4&<=|_<ja|ay68tgnP2e2{@qPr~Eb=^K`b`>M&iz~9
zbNQ(C%Yvuo(s;wA%zwR!=-6E6++<)vt|fg<L7ee_ALF*plhWIzS*xY$sWv{>`8D;F
zZK>vL&su>gKU+XLZQsrGGf7{bldk0}@0&i`Pio`K?9hnp?QTpiE1@jy(HJ1<>?Iw`
z;@=f~&UDP^*SUQkFdeC%SxlIU0{Ij_TL9i*?@LHsdGnIXlJf?Mw+jAT_`&(pdhllO
zZ1X44^{VnsyD;@`)&<nNb4RkSS@NYXd?dX*m;TP#mpF!d{}NpO-*^RhFCT<X;;$0C
z0o-5zrLU39d|DP4_XeiZEaB5uO|^c8C7q?{YeQdpetUyI2)+yaSnfSP_SZS5C)YV5
zw-veF$bBMmwCAhMc<C+2<yR)YBKHY$t)YG1IvhD#18a0oB3-WdVA^MRU*a(F59FL$
z1nX5$(%=%iYmsYwB-4)tp8;MK*_Wsh!Kv{Qna2x04_u!0GF&Au@rR}0bHRrQYUZhP
z{o?W3Km29zWg6#S(hqJ1UjlxIfEo483SZm#1TkCo2w3RW=Td&U4i$UCIMOcSA1d+n
z61*5({>PTw`&T(P<>2k$y97+7$GX$`VKTvSxh$?=)_7SE9dn4cE}Ch-)W`MUYrzjx
zzY_m3jX$4z|C0Ed!Hc@^A9w{c_OO~Y>)`YJESvqE_mC??Zmr5Ka>V2dx*8+L(H^95
zgvTZMEXEPnA^)ZTW`1S%f><0b3V+2bP5gIUu5-6ZhPjQ`E~XN%F}5$UM&oI_z=L$}
zppGGlMRvyK!Pwksnd?r8?D^M&=qq}-vwv-rhQc$+ql~2YBjxlexIEKjnwPu;e-C^v
zb`>U3Pk$I@Gk8$G7+mCk5|A$kFU{GPVE9a4qJI)UCE#Yhq#VI#@zdV5v-|?^-Qe<%
zar{f<m-DkLH&gyW@W;U0z;{c(ZN}ZnG9R0gWj+>qhA|;y+@+?*<c40%4*w`Syf8a_
zeQ-=Sy1g8T{ZlF6((ak|&jX(WULtsM9X3VmpPXC=j!n+fLU&|Ox+rVX&hBiPH3o@2
zJ4v_Ys7!kV-wVDDe5{f5egrR|BiIF=?hn#G4FGQgmo`U#=$=k@=x?A(yh-qP2jU4n
zOZCl`czOxG0KAHKL(<c!1z!$c0sdbyzhay2R%e>5PsS(b!_JM~L>?b7MenRm56kwT
z=`Y_xPg~E-e2eX$fbRgeWqvbO_F-p8f8pHb_XHZh_%P;My))y_gI5mT0{(t-{%8C!
zP>*J<7plx&k5$>>SXP)Mz5btc%St=jvl+Y;+$>;H?INo^!?LCjs?QD=WrclvDvR)+
zV>9hp3Uey>TJRGYXVcng%AVBxF?4R$6heEm!p_EQejdtWEgfv@<HZTM#LsqOPveM8
zJEb1?f;WIyXK9b)Wc`_D=g_PMg$878kD=aKrG0zmu@O~Tm1&RIxfHwvJlIYi1aAgU
zZzr-g*a$use6aZ8+2Y6SN8-mXc71^%ZHo*|p%ObDdtr-L4(}nquqHEIX>Z}~=m-C2
z+uJ1cl+<PFmvWv3UIhNH_RqIScQx-qrH@l$&nMt3!T;6mcg_*)cTCQ-N7~Ev;4$!L
zlI<lhE~NS4X<0ohv>@EM2{7Dc_ruWCtX_*Ho}U!Z*;kyMX{VIi0Pr&KG0FC_|9)>&
zRy&=yvYF{@&W0LuO#6M1bYtgb+9Tu9M)?8v$0L?|$^9{DPjA6@;YU4H=2!4fR9|{~
z5<Krn#)}*GC3*(rdxN)wr^`#ymEhaX-ItJiZ@$g~UgL_s8OjgFD+g~S-gIQbPmgBv
zS@w+C-KD`wyyfsEzrlEqflGdw#%g-0=UozStMY^Kc7nH)-f78r45(_T#0xWltO%qh
zW%?HJ%D^ubnIn8Vohq{m5?x#pzn1t*kV|i8QYJIN7lQB4-UZ4J+PfUQnRxrNcRl<T
z`04S)-lgE{z>U3Ej?!}NZ0{$?#b$N3H}7bafbY-V0m@HLcar2Eyq$FaD{~(qwNGgF
z-|D<)+cQI(d0x?ZzXUyUnfgcH%JVzt?=kRY;LnSoS?5gDb&m6xztEBRuOe45n|Za!
z=`{fQ9(Wn}GlH4*4Dofp*m*d$h7q>tDK26D6VM~|S`OX<K9PIR@161K{xlW-F8JyG
zP>$#v@HX&}l#{XFJpYzO*cr*iDbF95`I$jX?0ArPEf;jww-J0Dcz$Pnvy%G8<U02T
z^nF6SB~6*-E9vC*z+b^l{S%f;@ZR9r<SSL4X+-i8`KkMnpQG|jKa-cpU%wyuRr`@&
zzaROnDj)2>c7m@A<X7r(FZeR>SHzk+cG8@e{A%>-A4OFcW|q6DfA9+MpC#kHkP&Yx
z{9S=~lFvEdZQ#N3y*`V)`0FZ_KT*;#{+fEe@s&(>HTG^res>^U(X&(ae_lNWy=cz?
zJ+sg=09^FH>3H_sp7st);CX=Am;5qgV3wB7tgIJZKz-(%)$AV7=1{$ybeiVuOFU^4
zkbS3@Qv;in<9hh9OET+4@U7q$_}3s7&w@(kd;7^dMPC;Az2Gym$QRI=wq}tZ0KN&_
z^dHX6W<yKTtp#uTN@sg!fX@UE%FhFrcgNHF2hp<>+yXxa;$H&I;%CW1-T~!4{N0N5
ze%;$l1~>UfZWD6p{j=aZ!CS!7=K-R3BR}iFFA%+7O6sklw9Oh4*lYh7)Sg9eCHU?v
z_*8Hyk1?V^uUhB;@YUd5@SpJCPcS~M&(6#<3!b4YmNmzsXy`dyC*ygAd1#Lg$<KPx
zf9bx&WYsN1^e+|t;NQb<!rwoj{%eBz)7S9@eW;(yJJ-(ua0|Rr64XoVsRb_uH|<OD
zNzfVK4d9E!kIlN&JjZ7pug}kTK9hV$t08ovG#9c@F;E<V=Y^8LIp|w9k9Te*e{I9G
z{8;novq$sH=Oj~prv~%qJBqZgPtddL3jX~x_p*<%$hp!~IeQ6afB03&&)4@Qu7Dk0
zOgd(M=1fG$S7Yj-vM=KWazQ_y3f>H!?uWRVH2{1r_yW;;Vp8wqpua6gt~HC^$G|rQ
z^h$m<gEx_%rQCbt@kz#1XMwhV_&eaI`=iv`UT}AQXFqxsydL~1Dd%q;e;_}V*XlC-
zG~JJ8lw$9doy%t)cnf&YkCuYBfd~EQLGW1f0sP3!uO@o$y)=Y|f0iGuM_>I_>POqk
z)BWh=<TLEa#*a=3=FfKkDfh~Lw70Kj_aouYfWHm?{_14`{F!%T?(0gsnF_w^>V4k(
zlA}EP(#M*A^nJ+(MIZf?d0%qSe)O3&=0Enn<X-fa+~CdA!$qFn^mh}3?@P|<Pk(^?
z=Y3yt74mZz>vNjaeDO2imwc6Y?Zn$mS$gvZvv1Gi-$mhn<bBEFV`<;4`<@a#45tTr
zUy|HglaP0pq@IVRm&JkJhrFJ6EyN2+I(iL&&H!HrelhpqtwREH=Faa!Zbg1K^4Zoe
zV&8kpm-SKF{F@><@cWQEiC1xx_FFTsgT6fdS>I>94>^m>G!uVNGTrR&Lry|&A#&Fs
z7tZUP&S$+3xs~{vh)>U|`SYI>&PqNflyT$}<aQwU&%O^?Igs_#EsW=qo?Zpeso<sH
z>Cg8Bp95Z!1-~A=`PR&SK;&0}Tgc0F)4v2?58i(BzQi;cSM?HnD|pq?%;#&;5A6i6
z0B3rj<Ac2E;9bHGA4h)(|3ZOiXNw%?0$)J%_J+SQpjXDTO7LaiXNcp{K0?RQTgvk=
zw&Quo2<SaW<Len#>+L*ehys{-ylIcT-?AuN7vJrK`b0aum}Bx!ytcdcB~A|Hb3J&`
z-JSEf72E}v$70^No_Y^Y^t}haY-#6m4&#_|CJ&EGULs!%UV=V`!{jA+Id}zlNfPzu
zWs|_Gz}e1AULrpWydJzTiF)!2z#G6>mM1TfUk=^~&M=j{1b+;CE;!w8@+#+UGx!ql
zZwi_!U(?^2x3HFUDqi#aelPKM6YtW@^-OBtBDL>0KE5vGynTTAnfTG9GRo^-{?DD*
zfj`W}4Qg}GG_EB6G6()l_!lVO+pjwlsj<1|s>6xga^%*2hMbhgM&x!Qcc$pmOYm30
zD>$p@`?#<Ke-FIj=DmrN1z_IijKe-|!#p^avyc76&nUm!oReivIYH(E<7G}hUgiRI
z@r91OQ77}$^f!sTrNShlPC}2hivC|*A3dGM>{NO3dY<F7Wi3x{rZ4ucLf`J2GW~Tb
z_&o6Dhgs*5!SG%>Gn1B?Tc_&I%!%q{PBbOn)-_Z>6U(v|^_LZ9-YZ@1>c><2DT|zm
zaug$x{#;zr|Ah3Ha5i!^?K%9cpMKz-vs7N|;;Un!XV{?5lD_{sqe*`$>2KSxFYyFv
zc>PelY)Y(2PhZM;J#tM?^S(laKq~Lq-oKh4@8!L~c6XMvdE3cbZ-<BCFB|tI<`eeP
zofx0#G?`?j9(u!H2|p}+y#%iWUj|<7gHXYzg13SnoQ9WkKL>mpc#im4YCa=x$-J2M
z-bBWGIh3yTAo136#&fpxv#EIUiYKqQ=t~{CVwLn?MSj;N`ep9@OUms%@G{PT9x8fL
z<z{>)@LpFP?^WL_^XSeSm0rhQiHb_e)aHGOt=evGr9ox9M_S8!4nGaN=b+<)*tvlC
z%U;~q={>jQ;A_FhBI93T|6|~7;AZ}4%E659%wL=t$?_YU%L^Sv{>yFY_YQkT`qqnA
zPkTk*mdy2u^kW4qd3J#BlEl67@Ojfo5tMwF!(aOn?*)lNP>$(yp?dGkMpdX1r^|AV
zK>D|N#9#8W1HbPg{n9GrmLd00vR|T&>HCdluYfkbi1#|HPVL+)d8Z5*d(g8JJ?&eW
z$7bp01NF}Pe4z@dcM3JI4(DVM6Z-u*!~)Wp#Kf!Nm3@g{Yq{Jm?-lDAuhjdz=DE1M
z?Z~sBtjqoB`$>ftn6k*LKS@5flK#5?+?N=aC7-GE^*z{z&@0*fdyKp(l_eo><ySyD
z<!r62d@XZ+C;mAJd>ME_GJSJCB-JOU_la{um+mipRGBkwtI#*|b#K4O^Rv`@+WLN7
zl|v7Z;by$hIX(c@<vOzit<aoO%ZR5V!E(|rU@NSB$G*fH$$X^7mektQFGuI^S?h~~
zY-6%A*x+Z!Ba0s~am@b4s8q4k?_=0i_2$0BN6G#xHRc5BmvbS`?OCgFY%T{zvU1E=
zB<U0kr~UtiGw#~%4ls^O4|i?W+?akS%Nzcaym_(oGYd$^YGZy2@%ou@Z1yrS;hj10
zvsLiR;9nwqz38&6_25gu{rS3{Ym;&0<m7lTF6|{h;>bAi9`QCwdOH3r;;~H$FS5D3
z_D12)^Im`P-kcEmVm4yhe!DO6IQQBQo<%=col9@a#7;Ju{OukypUgvk=I=P0kN)uQ
zdg~Zt+CXY-X4`vlbdt<V#xcgoYZX}vsWz9`)++RE`u)Dd9!-C-bCP+#;TF$O(;%gs
zHY2xk=f1?JBBz(w{}%YdKhV!=eizI62;Pra>cz)HZ_ju?F>eI&@9gu=Q>5wX4PO7p
zeTj2KkIWlSA?Fivo%11hvv<-{U)7X)or?UDUHcN>PL{Jd8eo=0V(&coMej1-5WZfL
z-=&iNpP4sEyJP<B+)M(TA*Jz>oF^#$xgNRZ_xB}sCg(rnljjmr?Nj^N)plrC_W2L*
z2mC&JOQ2+>KP#5O?qlAQNT!o&Z-H^coN4JHXIkjv0t1H`iX`82(6^dzBV5kCe@Qym
zgU9~r?N5305|d8KXH37T{S7A!=<>28lbVOWioSZjy<lFHHS?To`b_ySskcKXODS}o
z=$M0lbFi=XNcuykBk_Ih4{wukO~+ySVcoNrt{}@mT$3BR2mQfSLtXAvz`&sdDp|^7
zIq6kIINPr2b(&Yn`=fVt>f}S?NGIc+QtDBvaHr_wdykvN-%|4xJWS6$j+gT#ErG~h
zdDcq)d6eE7>qzvH1r_nr&-<8p2=ycDqun8yAp~>kd9LJZ4tmzP9f@nimjm?@cz(vy
z$WZ7ESu=F@Co|%UzK!T>JE9|TLUKGvjZ>ZV*`eQLuV>Sbh`zkh%+HVNNK8xmyU`c$
z!&JL^Dztz57NBni`r3MR1m|4=ed^aULw&@&&e=6XbrziY7#&a+MzUXgOY-xz%=M%A
z!+P)--}I3FCp`J+be~{hyI9UPEsC)J9qQlNp`@JTE1&g!I}#7zpW$P@_~~=K)Vp&g
zZ{s)%+f}-t&UvfO75}QOVm{3`L{@MgZa+$W%skg&KwK(kpcY4XH+E6fsq>ta_Kg1|
z50epZ0Dnu-WAP1=#gOn%(UV#$@s7a)hDKiYWULd<E=FcA@-va|#=SSbrQWBJe7>dp
za>?hNo?V^h0%o19b;yCYC$i>qT(0!nbH*?~=9?(r)^^=V7X_3&r!_a!Py7M(ow`$p
z{hTcQm8tisq_doKR`RWtp7?P%aiq4jY;D8XEvNlLJ+eEWt}4Cx*MMsFpN4iMj?sQ3
zyZzd)HaKIkk~vAb`Mf5=fXS32OTN5`k(ASN(y8EEF#pr(>?IwGZ_WHqr^7cctya?c
z!~V+6_>lH1bLGTLmg1qg-`|-rHXiflYf^4oNvDBt=KN2mQ%<AZQNg!iK3{&A#bnP<
zD-2wIg8#gi)=880#*0<tXBFwpJh3D3r0yHv?wpdG=LGuq&|Ua%hO3#*!Stz}VdvRw
z!p=+C(9j`tU7b~keU)RGuT}l;v`^++Z(-l=D>D0k>GwVXUox&EF@t-Z*C$_Glzy~e
z9R3QQ9?$e0dXCndd*RJgdQ}QncwCZBE%GhMA8HWqrxG#)d=vQ70-1RO&wV<b6Lwal
z?Wf8kcymeoRm88V>+sHB?LWRAIQ7N^6qr8C<e&H@d?TvVQ)zyC^ZN;S1^BUor1p^*
zBU0zdBz|!n<LA80@+;T)rNp1defYT|EhF<xgSGymuv71?!qUelT-h3czPZG^UgF*7
z#nZVY@iNA_3Hux8q+I5qN4|Y^o#?s1)1zmGQ{(rPc$sVs0AN^Zu$1~;PyE>U4)gv<
zcm(k?#z}VN)5b|Dhu-x(&q6-<JgXAC1pE|a{7dAgf;WLPGe};N?i}#B;8Dh<v^2Bs
zW6Jou3i$@U?e)jZ`Q85JPv&*2&f}%G#VqMRhQ8fXIud{9K786?={Cqai0ns+Z!<w)
zB$a#=kH>#c$;@{#csY0r`1hs%BgP^p^mV!)v%Z@FG!=gR)Q-d(VFJc)@y+yoJ9931
za=a?EQ~H*`B7g<ojb4wF{*=KC?x++0U620O#*PC%=Ujl?TgbH`cWtu#vaN&696;Vz
zlVxp|Ia*DwvlB+=4Z*Rwld?uIJ4sUBGbT{pXLfw9{R5F-iu~Le9f==tFXgt>p})}g
zQ>j<yp|sD`%tB-%^43}SnfMd(>2uJ)`jq{~(4pDqQ)6>^0+nSt<!vmAANHO|eV@~j
zxJ$>oPTxxq<6ft+rGGYt_fA)3B53mFsq@emJCARnsXl#Afb|%C2aQ&|kbcZKuC(_D
zk#CySkvMyQ{=^{4J7YJq9LabxB?`okcM^X|Q%7RC_<>&K(7oU;-@?0DFjL;_k1Uq&
z;a~?3@3g0{$)yLuq&n%F#h!8srHObiXt|s9pne;P+1?_)|4`*D4|G0eHSO6mkNEAE
zb|g-fboHu*E(Nc;ti!w?XPuV(J{F}L`WAIpRKu~BF&t|#JG4b(k|DlYB}ZJ$CH3_x
zKe2hte;oqGFO1($O1^i7#^krgSpWaU&Q|{^eZ5bT_r>^zo;=V==`;Jf0Y8)1JVIB+
zvbGD;gvGvj=xe&MBXLi%T?Y2Z%sN2l(dfH8l&xK+?AwgKmYexzp!!GldAWHOSjRJ?
z&<)wwtx5X|Cewf2&Nl*+`VO!TUT24n&#o`IPM?Lo_B%TgV-LzGKl2`7s{C{fwap3T
zWncH3_8{fAUi5vtBQeL#&=(jNO!=`d6%I|z%UT~spR|K88?Z$`=tz9Xz0_}d`!@UR
zrhR9QrD8`V@!E*@Kyn^z_Tf@?r2AQe^Yg4$c-@Ryk1j=D72j6eE_R$_-otp%f3tcj
z&|~l|zPb3Q`c0>IM@er{^rUzd4={qGN`3BWfD<x1h=yEi4l2V9yl>Fbk@#8`y~NP{
zEjFEGcc;^6l|^r3Zs<&zj|XJsJ9x85ZyxD2@vX?;B>g`%E=|llcgJ_MWRs)w=G@r*
zehrBPxFnsoNT-c&P<}aCKPf*BtRLig)HPWXs?MDSPRXXPW0~2VOlG*m4{E2-KRn0&
z#X$s8{e$s?KstPj&~))x##wKvy&M(G(O1E@Fe^X1{nhcj?h_f;=rRHZvZK#-WR{{E
zpBtJiQ92hYXW+p*kM#5mlFtE5^xA&ek$9|2=X?g*F*}M*=(X&dKFNM&0s5+b#XeM5
zUmxR7`Zpod`@v0)!;Wc|d>VDezOCq+yR#$lr#xR@%CC%lsrE=cIH3};uXD{N`<Y?}
zDEGsT#J$P<ru-+BU+rhe?^)UNYvw~zesj<_vz>3B#(nz^kYC;p$v(YG<#!|c+M<cX
z+xfmeli$F&r2Xn{$7#x1ekQx7z2wo!%q>VH?qQt&Oh2CM@GK}xb?|&`WA=AVjNvjb
zoku!rixUa?F5zdU(-68XyR%m3hK|ba`D~Tkmen>akon6eq+e8$NQ~F?_4$2ZKfF3H
zf$<)7Nj;WNV?ER>k+>|gy$8x&$M3dKXtR_@wg!qW(RV%i8iynj4}PBVV7x68KkM9j
zP5FwxSJBsUd?K+ZOFee-Tjm2#WPfgB^hrP3dphIo$VB4kWPcQBSLyAP=S}TWEuGsH
zPs}!EHC%>3sh6dsvu;end%uHzwA1+Jy_4P7&K$*v>6PPF;%}->Bz~6c?*n!cKRNH4
z8+ypjRx3uEH%~1-jrGf{%=LdQ{7*=yv^J4=i24fm$T2>gZmW4FWY&1~tnmg?ThceD
zuja<(mZPT<{mbeS-g(6jsVwrGu@)GYoP(RX5Yq<mH;;Hbh<66};g^ZW<UIKdxa2${
z@sKiYpL-|rvHC>TeeM~eqtWA!&~i(Ep9RAgMdW$Y12hL(NOCiQ52vOhY(&ZzR8k@U
zsOBH@;6Lc!Mf%U<KjFb%KC{nJ{W)96oLtW0<GPCQ#`lfroq19sad(z>6Yz7s&t`|L
z?CpjHE^l^cby&~-ousp}0sm(l2>+g+)EIBtycy%o*rNOC3qzsh+1r5E*-|Fbt`?k*
z{ih`o=kL#7)7#bD{k+e>Fn?uM>r?ybJ4vUh@qa#@NjT)()BoqwSx-96e2d$E9-Hn<
zf%#T#e6=md=d#o;166JI@k4f?rGARfq(7PQKcCL^q!as6BJoJ(dNunxg8aOgH9sfi
zhGu2YkIXEke|?X18qP_W=WOD`>EoWZ^D=pYo3W3PY;l&ANZ2E<V9meKjyxQ-Hrm~O
z7`1-mY{YxN7e1N$Ya@c+6hY*hkvPDnNPk9-$dTMWC7XVtpy57xfe*uxKgO)@MY{bh
zX6=ZaYgz9{Pv-U~T?GDV7es&EB@VDKhJu#Zk=%Y7^Axzb&>oR;tAC`<k}r3lwBWEv
zrBxD{ZS{+sW1RptL-9kP{URq@$49QTMw8B2iWflpN2XZAB3D^sBD1V7d3gUDoP}7o
zG8(zrwVsYH3;*1;ws)04Ti4rR{3U)l$i1#79jl0pwx;O)sgX0RIa%(9M#fubDn2yQ
zV4dsTA5;u&i2N#KJz(#o+Q~#*puUlNLssK!9@4ex?3+T?eIbor9+}GL!HjIIJaVRW
zrFXCKz7w)$`tdGGkN5xWDjW4{`)RuW2O>+meA~4)$fuQV%0HOfm-D%OFdo_9TCc~s
z{U9FM<yyBN#O;3^RF1$C?gUeo6~DB<L3w`?YK`2QZ~Y_(fqS}!xP7H-FWxQ6jr=ap
z`c$0m(Y!vOOGNK|@<W}u!{r^Zd!5MN9cznok--k{2XAoXrw-qK`_QpY{=nut<gYtk
zgu{=8j*fgQVoi?R6R|G5HvCDL?>&3W!v4@9kqdFjJ8ZmWfnUXihjnYP$|Bd=Rwdv>
zq_pmPjA$kOCC&-=hpkrUPVRSw;@saAM&@z(!N}qv3oPq7`-EG&S_%7G$ox3;vnYXY
zcJ=sfA@oR`YeM7#-#=a1Q|(L3m`=;NOy!P(_MsJ>61l>6L^mhW&nm>nd&8^s<l<6<
zy&^mv++%x4!I9AN$PHbrD{hFc>|%XGTDTP7!O*_6!@5)(bPtORBla|kP+EwQyX5pm
z)bZP)$&cq)KM0>k3yqw>$8leka%+o3KvzlkvNjsIJBKgKoxq2E-!u20M639hdtLGn
z{<upI;3wt(y={v{xP3P!(jQ5;a;I4QsQmm`ez5HLYHGlG$Bx_^r+82Mdp;i^l>3hy
zIm~)X_>M5p+8EE)+hQvs+}|ZW_Qmx@34BokUzETXCGbTFd{F{ll)(Rm64+hDe1pq9
z)T3oe8<fsfx=iUhrQ4M5R_dCM7;~OLX@k<$N;fHOQ)+3tB}&&R{lAc<FI4>htOPvI
z^)emWl`z*oQUw1@d{%ar{P|b<y<kp>&)GlyXVMp~|4jVPwBPKq?%(zx=N}XgM*o+G
z+dX$l`1D_Hz(1H^$v-InuNH69^89zxe|5e;Z+c&}kI!3D|2?_V|6a6zS*#t}KK|YG
zUzXwjW<o1}-rK}n_lid=UiRoNrTbI+zrFs0cKiK--(Lp#`wRcO{l`y#fA;-v#moM;
z8m4w_)BfS#P5+k{(|*nSqJBLG`Yiw1Zvp<B8DG@j|J(If@o)B@e>C}4?QebhkH-IZ
z$rtVL=0mMY8@GGmxvzV4e|G(|_2akCb}hI4rStE?KdaySzs2|c#n<PDefsap{}<{0
z?=As<uWrScJUjjHLxJ$)f$)ztEU$LU^|prP6=AtP)Nq~`v}Q&<&GPEDT)Per8+y3;
z<vrLJy?){d4KGpJU1`M@k6)}|f1UDaN8&32?N-D6w-R>nvWr|%k63F9yqm5X{&=Q$
z<DF{Y{b<E`@4~y<y!8D2)2hQfSk+kaio9G3^M7-_u#dY>cwyg8ca#_A?OAjA>2)ML
z`y1AIINg}Zk5@Y6OT$~g>+%1p;qQ6DFMdCx1WJDG)nia>h5F+fr5luPQMz5}J4*K`
zjdt-I?NFtClnzrmPU&=|vz5+Qx>)H7rE8RKP`X9wcBStq-J>)b)AW`0Q94ZNIHl8-
z&Q>~K>0+fTl&(>_LFpEy+m*hfbdS=g`Iw_+9jdgC(qT%+DV?r#w$k}Z7b{(%bdAyt
zO1CK8uJj$Hdz41IYW|h>Q94ZNIHl8-&Q>~K>0+fTl&(>_LFpEy+m*hfbdS<#uBNZF
zkJ4dE$0?nzbhgs@N*60#p>&PX4NA8t-LCW<rF)b{^E7>>eUuJUI!@_yrL&dJSGri~
z3Z-k5Zcw^K>2{^>DBYtpny=|A?W1&<(s4?sE1j)$zS6}?S14Vhbc50@O1CS0N9i7=
z(YU6sw2#tZO2;Xku5`B2`AQcnU7>W1(hW+tDBZ5~9i@AeM)jX}9;&pD(qT%+DV?r#
zw$k}Z7b{(%bdAytO1CK8uJj$Hdz409&A-w<N{1;Or*yi~*-GatU95D4(ltsqDBYrT
zyV7@*?ok>&SkqV9N9i!7<CIQUI$P;{rHhrWP`XCx2Blk+Zddw_(mhI}-86lreUuJU
zI!@_yrL&dJSGri~3Z-k5Zcw^K>2{^>DBYtpdWfd4w2#tZO2;Xku5`B2`AQcnU7>W1
z(hW+tDBZ5~9i@AeMhi53rG1nRQ#wxRbfvSE&R4ov=?bN5lx|SEMd@~>?<n1)G+L<X
zEA69nn9^}drz@SUbiUHXN>?adqjZDPElRg5eMjjYrBQvq!ynJv{+NAy_xV1(rH?nG
z@~lriqyA<9_vd@x58(d1u(f}t9xr5B&j;lFdFUGf+@Ftr6u|v?V|zfqKYuJa)=$aH
zkw1@YP~6X#KfjIj&9u{>=QaoA{rUOY0PfGzTLZX1-!Bg6_viiX0eM#|plCp*JqIgZ
zAHcgQ-W<RWQG9g(_vhoS0i4&K&9ys#AFBA^fc^eFeslo$=jlZQGwt`+EmZ;BU#~O;
zaDQF1GJyN*nN0zlH?7UJD}bAC6u~JwF4LZ)RlYHRmneQ#AYVNczbJtBRD5MXPcOw=
z19)%6twEXg9HV$y0PmxCQvmO)`04;&s`$cyo&6Nw6_D?*cu`rVJ;y3uAHd~&p<K%Y
z`UiTj^@9L@oZ`<1@Ii_%4Cwdwr~We_KUn2k0`fx?Zwuf<6^{+h%vZVM?*{ZAuXt5J
zewgB2N;CDGpm=jY-rt{F8^DLFd=NiT@ofS55sF(wGSiiFT5^>Ha6aK_uEqea%V=v^
z03WUL+X8r%;<2Hb>5ftS;6OQ4D?T8A*C^f?&{M1Uk^nwd@tT01af)vW$k!=8H6UNF
z_@pB<?H|7%T(>=}_JID0DqmEdsb|uD@RJm;56Dm64}P-Za|7}XimwjfeDc9umjv?5
zCk@QCB!F}D-(1@QdZsG=t$;j7!p-%=0M1cnbL|f3IZg4h<1@>rQSq4poFk^@S{A@L
zs%fqr0i2_V<_h}H48?=}$yxis&sO~PK)!Ul%_<p|Y5z=>{~#cLj^eR?neyi<-Wbqx
zp5ny;`B{oD3CN$X_@)3pTk+ii`~t;GPsp^VN%7%<bT3r=v;cmQ;*9}47c0InfX`8U
zZ2<qW;@blFC5pQhnRc2@J9za0e6Gqb3E-D1zAk`Yrg&Qbzg+Rs;hE{qQ+#d!ze4er
z06t&w_5gn6e(+|+OHa&9_bSC_2Jo*czBYh=P4Rhw`dXm)u7Ld2ir*2Czee$g0{GVz
ze=dM8RJ>$Frv29{UKPNvQ@lBVFH(GU0KZ=G9f5RjP<&4SU#xg6FwehH@$Lb9iLXD9
z?oEnY=A${O^~}wRR|N1|6rUTwmnyz4fZwWkzkofrDc&BCzg_XNk(uSZO!1}wE|2Sk
z<&SGBX4TcyoLEvi?c6IanRi7=`EeD;l@A<pg$WJ5W^nm&W#z{W?dRd<h__?n#msNn
z;kKbS>)OKArJ}#2@RRVVOg$WH4Kph*{-*w3{7*GP*=NL;uG_K3(35KXkzV}o9pa@h
zcZwH2NE2MBVP#tlpP}Eg@6HE&{i`+p1G-k3`?=!(>J2}B!-bwbH-<cemVK`HZ@lft
zZ=2)Ae<|X{U-!A<|L$*o{B@Uk@i%DvZJ!Z;2I7wO6E=Rl@}1l4#h<Yxd--nD7vKFQ
z(ryjs?5F%K+w-2D$}iKfFJGnMnabx&1m@1?uhVc;`I|KC4`pRerhHW(+@N8<tL6(x
zMpK^ZYYWKpMI*!UyUe+vO!<Wx_U+sK8RhHw*NS`n@?EC!yZh}&kH#Fh{q%X+kH60R
z9~jFzHQ&pCfAr?S@!LQ2<GVe*{1j<{`bT#BsihW6l6dX!##~4JqpZ}6zoM5X@>%WA
zKMJ%z`#k-(&hYhbo9OAU(7x`o^!rDQ_NU(~|2s#9)PC3epKbo*u$G^HH0i+cAF21_
zFWcqW_j9iY=9#9~UiKH?%iqN_{P;89_u?-e;Klo_{P{<v_NV_l^sJsKF?$nL5`H;u
zS&tws-;+qcoQhl?)V?Z9*-)&@dBqSrHj*8Xkke)L(U5SLCId!C8!++#JzzK-x$u&C
zM2HM!kH*<XtM9Vtd{j7_dvbfd+@ftb5hJl^><$Uq(Qznn9z2{bsf>Qr>hcPOup{Tw
zUgSLXBK=~{9y7s?^>exZy9}DK*iodGvsdoj*mtosXP?{`#iC>-r$g>bVqZgVPD1WW
zb0|<N$Fe0*mh(Es<k%)qk#h*i<Txf!m2-zg51T-JPA!c#Ct?B(IZt*W5H*3uoJT3b
zoS1zkQD^47+=D<@`y2vIIR|$okY|g*b8{*s9oGcr<y=9gbGq3ww{6am|2Cg<s9jB9
zVa`j11P-%hRd!uYCH0(BWM6=`ML7i$r`W!dz>=J+Byfx^lfq>=gCx#z_Duv<<_wd-
zAQM=fGg1O&Ca^ZARsw@fU|r5c2@J9O5vL{Rr;LC(<;J#6IX@S3hS^t>&StBtd<rP%
z-Y$nx+(kL7C6lqM>Hl)REw@%|rWEUUB)hR#Un!L}l3Uk`#`=RrV^W8?Qng}s-gVpv
zDz$0QTYdBpQ2FRZzQ{&n-;!I=6yHfkyDq+lA^D)SG`n!uJ70q57RxrF`v`twTL%rs
z{BYNY+l0{_dCR?p+`%}J#YVX6)?Dh_-ELxR#ItR;o({}$XQMRa-qIHt>!6inB;57&
zN)dgNjC|ca<|xrLm|xrdh#uE*Zy<8WeG-@m*Uo@Ms-nocr{_w9pGjMCzu8?PT!%@H
zyT?TM0K3AjeCHzK{uzs-?kZC0;{F0W<{l?r=6)Zft1AO{uKQ3=ips4fBl+&LG@ZEn
z7CH}dLt>kIGx<2!Jp@1N=E}!(4{>{tdVyO9x6sX_DIDs)jh@5YOWD#aa-X72AMWl%
zXLt7y?vHToUVKT&-9g5VbO&L>QSNJ$?9uMK*k9sqL4OZ-8uvZjLTu~hevBo(-M$pT
zG451Ku8-TD-@a}wc`0?njQ#!G`$@OI`v`H5b^nM}1KiWFd7x`!=W*`ql*S;p38c)u
zoklU(<x?Zp5cdph80xOax60ikiFLfYmi!KL$5B=%xXtLSaDT?{aCZ~Me4;BpIl{f2
z)GOT&@xqaASF9T4E+coN-Ph2($#U;P-A^p{14`w2%e{<Be!+5AA-CCbr_zdFwA@B4
z-vUVJCCeQ{4q7eu?-bWhE%)~&e2)bYz@J&}+1R<&a{o-|W!#$U=azdLI$yC|`EtrO
zB9Q4{SniD|`=#YJ(H;J$<u0W}U$xv<F!eRd-2`vD<(^1(U$@*pQ5rie_eW&x4a<F)
zeEiCCTabLyaxX#ITb3)I5&E^|c7y!Ja`#YhZ(HtYa@1zI1r*V5E%#Q;{hj4rLB@V>
zxzCZKotE2%7yrR>|BC8&_|MOPf3(~V8pAHMVA7u~cOJQT*K)r~Vg1>1=Tg$|S?&#F
z?0w6<5+nZ0avvpsACPwv+HJW{lA{kT_m_~5EO!7E@i8SqRz9)ZZ)3!#mU}-b?6KT~
zA%C&lFG1QZ_g=F3SId1Loqx03zfc5!x7=sQ!(PihpG@zgw<gqKxvf~8u-u7&3vIVI
z?sl#1UWPwkXS-V|`9-$7f||eHcBf$c4Yu120~g!wlQ`*(wmXe7TVlH}0p4W06S4DV
z+ue&h-D0~pk-w$3i+@<R!Xu&EZ1)%<+-|$G2`#g&gRdjOlF0j8D3NY|n?Wmm$d*3F
za=P7dHlZ~p6n46uH=NMJCKPkJ)tyD?5nFmD*XdR>iqLmW4_f3LA{rxIZ2LkA>nr>f
zT#uCH!Ch^8A<!*8Tqqip7q{(I;7=rZVn<=}N2LmE`*|eZN=k`_l$5zs(Iag8eZ<<6
zqBonwt`+-^w(T4Ww?O{Hz5+5!=TS$&MS9v}@vCK^_CWX*zD%Kse`p)w$N>9Nh;*3t
z6mG6a$_P{_J$u?uIjTLw4lTlUA}88^#Jf!7+lcss&l~wUoGMah?<ENn`DaA@;PXbF
zPdkcCwZ8{3kz@E|oIkI64m}!^Bd6P2ecgRw4NS2YNbIxiZMdM+h&>M0Bw??RGCj1f
zSaz<RgA*I}`LHfdu@}<yMrPZgp31%+*3BvQ`!uA;MRo@*R%+9J0M;WZb`zc+xx`+B
zi%Koo&%<g>vJaa@{YEacPY~9xfp-Z@>eMUp!_JU8oo`=GXKe3-7eh}vzpxkluiD4b
zZP-V`D-H1fM4ZSq_C3^`JrZ7hl5d>%&}zy1_4c#)j49Z2;Ll0Q7{4ldovcM}vJ2>i
z#5e34;TMXub@(ZC7?I_6VTl*rS(S_`{4_5zPL*^e!(fn<A^6~lwCU2^6YfEM_a3B}
zyIy)R%jy0+tvvTB3B;W4&ybPar^O)G>3*khS|m{5bpIA4_ZbNkIo;nBu^&pH*y;X+
z1b!re66Xj>Cb!O(FX+lgLJQVOA|5)yZUi~cM++r}vL@S?1IvFIGZ6|mm{=1;*D3aG
z!1wqpuMrkYBg~z)p3Z9{qV^N8H~J!8?BZ|HhI40(rl?GjzY6>9q=-qocpcS}dtR~c
z&wqjK;GPA~qqDG8Y$$$$TFJd|px+|8!#*Y;GUX^l=1#$PM5{d#c70OBOK1>%Ztna%
zevQaX*cS&xen}F!3-Ty(BeDqgO}+^K*IcS%;dcDMaK8;l#zkYPReZ@&aBsYxYBpJY
z4$glFR|?-N=Hd%zmAOj~_e+B>q*?C=M0OHC_l^aWtkDv|QwsS@X;D@Cq+0H-q<fOj
zIobn}sSr6+YGR&M9HpV;e!DMCT2wg=u&4W~tm4ZU@p2z(#%RO61kP1H*Q>lE?;u+4
zV`ou6qR`<hZt421{GXALqke;@H|JH%qE*S?(eh*2-QT1p@;(xYQm4B^o#uTkfd=OY
ziC_=0<?G9T5RE^VD)!J5?N320EH<b*r-@KwyD}|IvR;)~B^S~m*^}&V5z8)vH^S$q
zd8j>YJ?vj0Z=VYLY+t0%s4yOC&prWjO@YsceQi?2E69t;lD*(Z)RYmq2lgWwB84({
zw~FN-_}Z)g1DlN8Pmp=Zm$QoVC~*6cD&IfehVzFM*TFUHABe-5xclIQD0r`QihnEC
z|L6l;*Km)3)5qtkLo5{|Ul~h0BQgs1<O~rtvL7yE?>wI#-pI{H=F$u~t9S(-ZvS-@
z(`qAf6YP63L^R2jH15!Gx#UR7$$kQvr$w$!>ZW)Pc<6-rl!@WK0_U|9_f-l!RP_KF
z4EKFFAE&q<q9HVXFePcYxfIU9{Hf+EXdR(bN|7;KS!54Nara5OXMS*$a_iunl;WO6
z#zV7KQcTh=?epPWEL`J-#lID+zB~g*HQYsTZc1`{>_)R4v-A84c3j7J-lK>-<%@Y`
z(_;lLZ;!PbaXnLrjvF$v+mZWirtG2bN!2a0C;NK;2Hyhnj5<`>vHi5&!}e4;hr>EL
z#g>L_Z?Z3B0FacN3RpEh+jH{63n(%B=XQcQh>>Yb@{Q+OJ=aw+R8!%_?UagBJ(gct
zsM<FmxJ-m2XwYiEA3108FEK|V+iPJz84!`mcFvf929x=p!G2RjPVu!Aigv5`1<KpG
z@Uaq=`xu!J+SBB`mUR<7Z~oKYA+|C92xQ8Va>))>J^We9F@Mup@QrXiH;tWzt>R5M
zUw-Qqnz%C;nHxIGX{Kc$%b(g?3()>1f@_n);)d3dG92cg^duR(XDH>eG0DE4AS|i>
zeCZyYo>XwxTOXj1dyQ>4)rt@K5ZPGqDL1W**y}3ue@Uo!*HSvd;fC?X)xY|5+egoy
z*!xq;JU-H3RZE|0KJxLx+GB>(Da5NghVVaP`>bar7CByKJALPoSY)gH72Hs!Sf$U>
zrVl!=wx?GpX9IZ?lYZiS2c7vJWKT+~a3$uOEbF`_8e56&(U?qM`p0DT(cj!h&3z~w
zzLS`tSooVH6pcwiL}Sv?_m9aWr@y(6n)_%>rZ?=G7+jpKzqyab<o;M0irvc49%N+0
zV8FvA-i?(nLs~TUGC~8tEDmzGuzeA^IqpKssWx?xFK#+e95ytXJ8?Io$d{UIq@ppY
zg@NL<q1lLB?~BQeC_6*M22H<RoY**OzGxaKE*!d>yQhK?E=)zZ(0J@X>2g9ZAosE_
zD>qiY^fLpc#|gC&em8?TPzD~m9ju7jGKNc}K{8Xc7nme|_+`2El8_!T)Cb8jUs`Ug
zeCd7%N{1M#A$(E>vup|q!y`pkXd#^AzVxNb3Ex7P+e^6Z$L*k3sH5;mnaB>dhFl{f
z#sdfs{g{fiJIwlV=uYY2ynCx$I!XVznQZaMWv_akII^8`yws)1KJc)w;xP20V#2fh
z$#TFt6{(5~ExW6MZ@|rmUqfX3Xj^<KbVu7Y!`DdXTcY7*Uq5j$8F^73)s>;b?rndC
zInfBI_tP*ubyE)_t+LV=XGk*FNRc%^BShu|WB7@U+?IC71g@F<+1>3zdf*Y_DQgi)
z-i&Mp8Sxc<J_N43P2-#>5xETKWYRKnBPF`|8TAbnJf3<Q-J$_myjIy$?IBh66uYd-
zK1F^Sb3!w_d)8Ljr#rE8tDGw>doapP_6KR)WdQaO_PJuEWa%=Zq;4c@KLQ!E39&$y
z<x{sPy9tpRRkj-dWicv!g!uZ2h$L@DJ_0ggH9zu2<;qsyc+tXTI6s18<VH$#^E2vY
z6*O7?$dC1TvV3VO%eknVqj5{9A1Y>=SSkI%Z^X*vO;3;lhQ%=rh>aB3{EQI%TyQ=`
z|ERjog=na<r#V-HJ*C*yid{pge5jaA^yPmKitIH~Q_kh=>2)Yq@-^z1BRr>(K+OaV
zNWJbh@jBP7m8#oN*Mn`%Y0bGBRg#Re^6|{`le{rKmB-Rj8E-_V+7kmF<fmd)Z&2y8
zR4MtZepazdlL=Whzf?>NxIHQB8}O!bO`bnZ3)FNk_6k7qyi=_ix*u!s=TO&zWS$3V
zR9Rv=bOw^=`K6L16;m@jjoZWHhURZGwLl(5ja9Z-RDG@ns#Oc|Yp+nut2|oWMG6!R
z^^&?AmnfGh+(KpC=(RdJu^Z7du}4}bwh{;OI<c{)x7tj{bizV%J`5@8#Oe(E1cfm6
z69z4svc#9&h;fqv?Ne=8rVejlj$~hy>fn};ynS(I2lqEMXC3P8M!S}ojcM4AqAYbI
zE&LP;Zj6WpTDageVre@E9|@bQO_ZHx-yzDRZ7e|~bt7%$7otqW0&Pcdlt~ZSpvqi2
zR{Jdb1UhxoF?PXck~gUyvH`gA#~NpZXyGzF<oDqOd&uWiP%3ovA2smb=^-06Zk1Sh
zwtc-=DLv#b#LDE2lwTJ{yfJeT3zVO%n#r>CkaJYoYEM~z=A@=$T#QKaCe=f31g>0m
zv{&X8qJ_)!kX;z&gFWP3DrmC&Lq8VoLK`^npb+RG?^Y|fc@=Y|SShvKpF~qPQp=Zv
zjIkKH0<}C8$J8vB9_5)|hLk<aE<8$edLbev%jQM~g&zna1B1*;#w?R@QbN}g3TAbo
zDm7XCo(4Ww?WSc_d~l-1-6YnXXTKoU$uPT-L{m31N=KNv8E=y(ER~IZ@k&0q4Cf%4
z>Ijh?!cE#}eO7wKT<6F9JQWidt)m*(rE9UjViz-SH!e6$5*A-HH{y#=fK>Y8+7Si)
zQVNfr@ipxrj-?|jN251ZST(;(!zZQTFKLlgBR|{2!*^`ECTi7ePvb1~IjwN2|E!$l
zsz-*6J&LVO(>+8pD8t5<BrbjUd^+jTY2{e+QJUz7NwvP{VKTaNNgit|H6RW$pu}qr
zllC*HSk>cHy2?m;!}?^!rYeU0HRmcOwqBN$^$oaKxnjVm8`1(bk7_`yc}cAaPTBkc
z*9>u!lSAB(HL478f6yc^H$$8l^wTtMxQD68(B<9`#|Sp+wKOBDKhnT4o*eTL{)d2U
z4{)giSfl@@Lg_<XG}cD15RJ+DVW7<Mt@Baw75<Wo4Xf@nf`7;BT-A7*(TmTnx)V-e
zPrOxPt)XnHOH}4@07?0BuwjbH+Fmd!zk0M{J3LHsW>uf8;CrdaHK!>iR)<MS71W-q
zxQ4AdSvkp$++1bU+Mb#AstTg*?1h!>h;-*~gw0aoFLIN|O+Jf+6RODHYLO8{wo;AE
zW|=@stS6YTjk-@Y8!z0X0jc+Aja^gi159n4*b09(=l1k0lYNc%yc~r-wx{805lAl_
zU<%=>X2x&Tl%+`*V<7oWmu^T&15dF=4Oe+%<aue4Pdi{_))1rSq({9&1CqZxP1LD&
z-TqqHsE5*`TBBZ0E2z;Qr4<x;>@;+xH4Z0strn71E_<DuRO|Yr!-aZNJNzXVkmob2
ztnYC)&S)NWyiah{8)?SWbfo2DHZecbgy;NvI;59ZPqMFci-{?&OMGvpIYoTCrmqS~
zWv)*bGLu!2@#=9@nF^YEnWh0L+z-^K@%t^@^V6bQ<+9DYP?62af+;?R4d6a$1#<<n
zRIgxOp>(W$5;S`SuSi?JfvMi8`_lB*Y)UifHk5s4Ilbza({D7Ul+zDPOf9Em5m+@J
ztBe%WuhV5xWl1?X3$SW$x>Y@TdvzcMRYdm0s{I$#@hUB4Y?aGqcY`8NB@1dSTHZ@9
zsC=e?UP0}Yf|6(8qwY#mRP)0$8`i1~2k7CQ*e)%ZQ7@*c8U1dWngeCStm{8WwgcIU
zcaQN(VOBC1xqbOREBqzf!OtZZcTvgqUnLi1O!De4qN2UvLj*?+P%Xv-r)WS5_G%P;
zW_5R{MdRmbOxZyVbVg}im+6eGa@p8ks1aUETK`iLJe6&wr1jm8vaA!9g;^hg4&RGr
z`)*q#LU*jYrs9%ur&#tqW(_qPLBfreeXj}MM0mJMBK9g<HWNd4tiERWgN&#4{Tg0-
z%>>!-dPR+ptrEHH@7NEQ(z#y_?=h5&XyRsv+?>VDK5j-^eY{ei#>gY1j5Ot`PHeTb
z)4}4ca?6IyHh<C1Rtz3YImKj%vub{#Y0C4>EhJ`?$oMW+S~b2&!hXkN>i{3RV=erc
zrY~FX9e#Aw7Ar1Ww!J7Lv5I8vDLJ9MBPc_;Mh{985&`MB5a{L$Sfi$?#I<D4d#mj{
zl$m$k?zhV2X~RsFz>UmhueuJ2+xVOKaEZ5nXHT;R$|E>y73kyqC8rGo`qIV7`*2pB
z#E{*DxvKACqfP4Umv9#Kr_d$(Fs6MqOI1c}`ItYi;H~Pr6qo;FCSGUe3*S<(*aMx*
zORY(dDK=OI8B(q476r$p$Nq_eXQX0R|5UL{e2k*}g<}4^N$S?*=_U+rQzkBJE_>A)
zqQ1eO$&<{6tN)}jFQk-?|46aLo^#2NPM0N*BCWoj+g+N}*iO8y$EG!xE+y>ZvNvnb
zJ{zIi3Dp#6EGf5Rk(F}8(3)O~%VYTQK5piim%}|SjoajL{cRN3lSe6A9)c`Uwl`D3
z-l=S}C|`P%(HChzdK5hFz>}ei{QhmAJn^-Dh4TO6FX=tUN$yUR+*x&!^>5(U3pD`-
z33J)24j#bUYW$6m_=ol2=J(tz<)(V23Xg>C`H<HNtm^M8XNq!ASN%i9&R2{HcFktR
zq%IeA(()_i-ks#I#8ONg<#ENSgX;Gce<7v4_EW{gHpx)AJioI_y{aGNWsnB)F;ONC
zOso1+nJ}JwS<~ef5BP*}KjE6gHJ&Uajsr9qHkCZT)EeC<jSI)`W7sZZ?kkVH2g>tx
z>ue%k&R?nyOfofsd2)3P+zz9&EmMuxB<;Kp&e_LN0ixy4*fin-Ze}uXtRAnzF9BrM
z#Fv!wrgEevE>`RV#iS<YD<)N)Pa>IGmMXVLl4I2@S4?VRxZ+Y1_bYyqVrZ{@STWIR
zYC>KaSf}8yWDc(;%Dem}$E?1+X@sVTrU^cmV9gedCav%ZK+jUE<`;^~1HxC5xO(L1
zx0EaS%gCEo?B(+8cDKqb_dEpGh~p(|2B#Uc5etTyDc;X*nd8+m&l<Cdh9g^$+stse
z6=;I&{JH%+J#fuszh>WuKC>Ov!5k-bBl|*ijMtUHD1kj9kx}IpLFI4T?Y{Cm5i`oo
zjVOOpln0{(l#7fipBYsCCwp3%>Kx7}X_T8AQGORlWiU!WxyY#Ug+b*X+duG?f9xwa
zH=?{BwOkpD5>PHOIL41vu`E)zqcOdj8JNZKMr~0UGe7%7T7)jta<&K`rAIhS51C2#
zTWum7V7*o|P^HD5+1b_CoRlVWb9R|gXRD0ZJnFKv06m(#5wlF8E>a3y_No`r^ge&d
zb~KL?8h^LSec&|?84hI(p`^;?g_+gL?&@hlP2Lbx$e%wG`JSkGG^-jXCxIS$wPsx>
zJyX%KAYIQpqUVQM^{_^^8rCwEb^`~X>{ZXAVv0OI?ZoP^Pm#B05{Xl9hS2J!j?LrI
z{#rSEF!gH6CH9EiPWyGYv<Lpv_DINo%0I0ur5p2~@8^%F5T+jmwEDFE{N{vOKc(e!
zHU1s-!=p?9hO#YllR&3Gz|RkWBqUpWh2;V_4$ff}R7|od$#;A3YBYZ`=*pb1@D^^2
zxcBs5BAQnK23u!rXFH_v#6y`k9^$>*;HB2zJVx(tp3GZ!(}Rbtq6ntjn}{1;hm~?l
zrv!8SYXtX>_pU%l4(sH+Dvtsj@34;J9oBKY!#a+4SjVx=VI9XwKdj?;hjl`7Sf}fv
zIj9J2W)SLnz1*VBKdiG-g5F`BkQ~;@EqfHiJFJuUsT}<<hjsGyoDXc7!#a6?zf*3_
zVV%6aa_=41$=fIQ-eH}*4!QRZ>*OWm-aD+5XW0_)4(sIECg2^`$#YD=JFJrzHUaOj
zPF}<Wyu&(qQ4{bE>*U4kbFt7ntdrN(ZX)0v*2&AW#bEESPM&K5-eH}*ZuaGHyu&(q
zhuY#h-eH}*!))2d^A79e71@`d%{#1<S8QKHz&osycZ@Bgymwe9?>KuI0q?L*-XIh3
z4(sHVnSgg#CvUI`c!zcJhDa;c!#a88#y0P;PTny4deYIuI(d3nCvUZ6(j3;w`?lPA
zhjsG4BiZ#1>*TGG+{$5{!C<^aLMT_NR?N;beh(_OY0%|9Du;FMC6F(&`J%%d)5%}_
z4BLF(F`fK7TM@`|Oeg>0-wGq+n9h(Pcy<1BMU1s>R0a`uD!;ZnkZF(OHXnl*x*zpJ
z#ytl-;@-?C7S$s=`LB<cXrKScPORz>I*Tuk?0j)#=Zhmd|8F1JiM{`y%wx=vo!CRR
z^f!8BC$`3f^vF)^VH46LJF!P>>813@PVBp;AJro}F^=rar*l}qU%_)od8cS%9ND=Q
zi2oDq!G)qhc^uh!1pL_~PwXg6{-_j3c3wf^Pf01!kdiWYD$0?azabW3`e|$^e9<I!
zEt8BGM|KVc@8R<br0?~P?8LkyJ91jiJ`w&{U&f1Xj_ky|BRkSz+GoSRE-52Wq4e)*
zKQe*v3^PY|V&0J*6Zt+OKKFSe%aNU!cVx#z-a$nE+qLHIGg6i#J2CIbj)`2zeCr7Q
zyy`JWc4FR<9m6h%bz+JwM|NV~ksYZK`&3w`3;P#Rrsl{_%saAU*q6a-PO;_4PRu*9
zW7v1V;vaPTvE|54%sa9pwP~+|wIRipBRetg$d1&K{R*ttl5BHiC#FYs-Ua?tSmJYD
zk((nsF+H-=6*n*BFP$$(c4B&Dr$4;o1AIBM6VoF*6W~ot@{RMFBRetg$c`zwOW<FX
zlrer~j_ky|BRk?7_Hy`zBJCa7iFrqM1aKZnMwMWi7a6BYx{_fqXOJP-JF*i$VNE^{
z6MHaQj<5fXREu|HC;pTK^vF*9X)#ET?8I9nphtG%&qzRz?8JX40X?!4|B(dr$WFY@
zKABnZH2w;9N+RaSPJDuWKFC}jEtD9_nrtrwz9q?e*Tj+|JMmNO`+(Q@EUyuoBRg?(
zWM?;`_Oq~m>WdVb*ycb)+#K04MgCjZ?<GY{+UCek+#K04B4L&|2k}?12c3m`#RhX^
zCvJ}H7?HlPhXh3A$WGiG*%7Vw1lZG(B3?prWG8Np>==<R!)^|U$dR46IkIC!mcd@(
zi+D$N;^xSX;mXTK>r-4gvJ*E)c8rl*;k+(fDSWS(&5@nBIkIC!K7jqVfQTH~iJK!k
zMx+~_(v!cG7F8wBnd9cj&g0^zCn7Ue<V>k~`IT4w<L1bYsB+GNeV(t%JF*iuM|KSN
zYjEV<BQGISdFIGY+#J~vh0eXgPU*MuchCaNk)1lajbb^#5j6)mOzCVz`j-+z4)0pU
za?B!Xj#(J)yKr`=xN^)QYK~bLZVY{K{xq%}vxu5w7E+dWA2<VqE3cJWNB)+DXVe_C
zFx;vn$2w{#TSpHQE-q&M*kw@L0Nzm$?`Q(MMC3}0ZD)daIkoOs<t~P^G|4@(rXSqd
zgGzZ3KvG{VoVdiZLh|Z;{;lw>{O^*Pd`T-`3gh5jBxoO0QGvh_a(~ch``6&Vm&ko2
zvWHgNAA@xGXyE|E>M0S%*>OgZ!}&82(wf{s<L6?`5JYYH3_ztX;>9kG^(FQcc|v78
zXe#VkB2qzw!cp>5E2(&Lr(f1b<rW~bG$}VBAa_o|F)H^UGV7Ca4Jo+-QaotRxs;_z
zcPlc#P0E>c9TAovtJnx%xxr8TuL#B|E6+%icuM$4BYfQo%Gqd^QuhQ+>M21hUrtFK
z)jprMIglwnd>~$X_e$zj7GU<-V6!CdM=akxe|4(PM8^2U0yx*DxZ<zQ`1z!1xc9<Y
zo#Y-V{_32v(2py}`8}=HQFB<?oq90(#pfbpjw6dk!&}JwPUKFfkFkoy=OU-p^ipo}
zl#byZ{u69=r-Xi~h3*uhu)q{ccPYbcKhg~pnxABWn6mZ?+&cPV>?%2qRAUDZmlHdO
zcVkvnO86Y95c>#W9X!%r1nVYA?NWg~w938*<Ut=Tyi!;_@99s3T3dA4&+xO^=cl>q
z!FAsy3-SuI{TA#$`XZi+UNxfPl&dKyQ$!qrGTO|kokc`4eVvzE<2F{l@OyuRrxkT8
zw2vYSh3q07BR|n@(fZ<OPuuSC9&9gDeMHipj@ZTg72JeMV_d=QqI?xUmM3x*?3+4^
zSbhIWW9oK<eG&B`v2BOX03c^S%FjB9@EuXocejX@*ef8$MIF}|Xa53`Pkph%WZ&s!
zN%HJfk=$9Ozs1?SXkqdbuvRJ$cXPWfkj!?_`}Zg%?B%oD+xStps4dkWbNYy&JqfYX
zM2~!5qVW4-<`Kf^w$Q!+_{%=a>RCYfcUxqOUi&(JZuMDSMfN+0LhQD9J9#7fjuCqV
zk;i>8tN&1nr`ydt_?5(v3K^+aka$g`)=TRB`-`62s_--ECy@#psZWuxXl+Ky8{PUZ
zlJZ=>8gtF)=C~0fT7p;~U)1VviPSyMrxZqw)Ho#MWJ<ciBN6I$|7844)9zwKFGcLy
z3{f>8kGkmg&>?=8<G3*+|7~P{lqqj2Rg`)St^a8f^LwQ!PslMb-z3(j2afqwshy|V
zDMj_6t|n$7d%vUjOJ%3fBxgDS<3q{R)%fp8W;g91v#Kc9C^{P@H~vG4j72{y$3Ik2
zo>BBLO5RQ?G6j%w?E~6oIw-5ZG4PGA;IyhY-{?)Ca|oX8<@6uXYxN%~F8IgOk4dVG
z8<nS^?krJhT7lSR_5aC2M0Yn*L(-|>hX)zas}Wn|r*8F^qio$iy)~uKHBt{C@qJ&)
zo7)$!5XatoDdlavRX!)P)fcgjJwm)R(IJhy4fGx1h&9%Lze%fg?Bk`m{v`=p`O?Ff
z5;<yJ6^EvnbsQ<jB*W&Iq-+`5$AA^gU>Ngi+{%vu<d0-{8Q2YNq1z>=4PeGqtb7>`
zkCW4ZhYWcdPV#i%AUPO#$Z&)Cj|>LM!N5aC8q9xiP$mZh52-!_-Kz)7!NAz@vZfj$
z2Lof?!9cTl5RKhW#-cG<><o>`Ud2#zA2s)mcfc<iJKqS&^l@lR)<Q#LavwGK(U`1)
zqA|%qG$zxkXspiM|G@l;UD4P^^DD#s(3niJhQ{PRYVM;kDTtw}L9!l=$%tGo$x4TL
zJirCeVKIjcP3JBelYwPuOa{fF=00lfqcN$AXzVT{BS&^l7#fq#H>$tl6GLMmW3l{J
zNWU6tMet4jhCf`QH}Xcy{q)XOzEu5*GA1y9g+3&ZPc<4rbK?sRuMp>uiAuf<Joz$W
zRmcz=ItH-9k7I89I99$4+9S-OrZRLAGN<`+a%1I7lVayv>^zvC&|J_(zKGmZUWk`T
zF<JTIT@`ZJJ+y+m)xMP6Sot!iuV|peLr)U^vCotnD_<tn6*Bb?Z6o|EpJ`%N$fQ5C
z3rr?qCff#&#)gSi<EX4qlnIi{pW(`ll`p~-GGh<*A}rr-FiewPh0N(fBf!S`xQR4!
zgk(|XQ<cryBFMZnii*t=<I8Za^kc}4l`p~-G6@Xb%-wRIY1CE7^e^-su*WiSnbd`T
z2)5bBjj{?^z=UK$DW9@3=E@CCRf<Q}FQE?z|22bIA?uD%7o6o_{!ApfvGQfXG0L2s
z98P$&IXgMTk(wH9&Q5yw)(H8WojueVkI<>U2D!2FWz|t7k-vvy`JWXPkCe+^^))z)
z_!}cpd7)^8ELH4E>l*~u@@H%jO_k=og-XG!d|9tlNwi5!bGPY}&y><_aNg!`jA*f^
zjaemRsx?9uJoZ@Y1A-2##-#4yNl&hlXg%mr+d3&dh8P3+8za%>4J}zC*^_sxVAx~`
zWYuJZF5s_5TA`S3PpdV?PZktAjQQ2FezMQ9j-}V*TBFw6t8PW;Qr7fiCA!=kjXC2k
zor|!nW*Q0Ay-QizXU&KCG{O3+2*VF=OngZy=1>{PFPU!HGbMBtWAgZUUncgsGU~`X
zJ`==Wi}k~XNwcrX<tg#RlVP<Z;|aB@P!51x_P6CFO(RrJ4`xj`m-g|#3gsCQgqAH8
zA!*}hA(Xng3Phf;P7202No38>L|;Y?7(UP|ourkkp70H5GX_Xjz7Z6<Yr8M>_2V#V
z@(3;Cc|<9q%ucqa4Y=HjmGq94^cmVaC*j}sTIgSP*Q&PIpx#Fh;1!lb53w5ltQyWV
zv8URDPqxp={iI*a%ku8gC-Z9M<>);(6Kv$*-^rYLy_|_*wHDJY7=ZzDBoIfI#c{r@
zi>qa=Y(KtE^Rk8-mCIhWiX<QJndS~3ktgF0kITYr>LA}89v7)C1nU>7J3Ju{P*JAt
z@B<0W8zP|^%YITqk4nfm$5Y}QM-L@9rrEOJkWdpL<0_XA3RJ>>%4?6t3!Xs6Iz=j>
zt;`cjx=Prd;Y+<QQpQ(8gFUIFuYBAhLQ?;SmZ#r{A2fhW3dRWdipZ#$r>O3vuOxo$
z8*q<E88cU_P*;ief7p8u=qQWzfBc<yH=9j13o9fcAYkYnLVy$?5PB#Hp_fRDAW}t|
zpfp9X3$a}bDp)A?-WAkqZ(sx46??hrMX%`nTHycrJTvp|o27{N`#b-0zUR2-?Cw1C
zl$m+vnexue^A0|$p$cT1!W48t#BiGQ3x*1#rF>LWP0lvin*u7yumqUTYY{UfktHFY
zq%m1&mYbl?x@rxDv*~?KGEw@>M5_PXYKgM*tf<EmiE62xmg-VcjriTz6HUMT!S*`k
zrj2NV2U+npSaDL-V=FHU9@dZ-LyBX}Vfvw^NLyHZ__jb1y^M(44=v*8GE(+{k;1a=
z6G@|TCHwJ~?E??rxW6@Wx}p6XLM?|X5DCY+EAcx{Ys@M8z|;lXx14y?ZM^JjU;Hdd
zVH6mhkwZEuRL~()Ji79mj$;%T&s2Lg*~mlNmycTiHd1Sjfx;U|UV%xB=0MV3aRn=s
zkBOR$B<&UHdIBCh&4@W&_me!;1fAe-<WTYP8D^8^TAV6S^9((kI6rEADMY>MPj(69
zO0ort1K8b6b|N233KaX9uL<9VVa&iy3ZTHHM(mb4=sK@605{rLLEVrQl>=+#4^Fce
zm=oftjX-KwHPpOcU=)`DRWEgyVgHZmo7DQf$lg$la-L?6N1BqwUW%RS<p6aSdr21e
z5tb}!__adxvfH8QaU5s+%f@Dht_0Du4iNArf>USAaa4)G;ckvI{vo8PuoD73-Ai~D
zo`MYgP7&b8<M^_kfCCuH)lkX8Q-o(wYvlqrE(TnQazGW-;+ceBq!xz@{2JlM2#*lB
zLodMPYXOf^eE3~PcmW;})o5A1*-!X2${XXs<HU<$lH@n9H;YFv1`S^$a4(cs!>W(t
zRESr@Ns13r@CR5~Azlp@zH4Pg>yeOe;fGd^FJ*a_NdBgSe|&0Fz)J;wn(!vVrwM#q
zU%)QuPZ#)fz{d}E)N+B;h_YB`-JaMRywQw?BA$F@*}nLH#^Xn{^Tn5q_W?0XDGl?8
z>Av`~YYo7Sq+(4JqT6Ycv1}nIuT%Dfk7TMQNfwe-s7BD-1)2|iH0Szhc87%KA<$%X
zi5KQ=O;b?y*ow=7{;tgfeO=oE#|N0%OIZ(}wC}Vup582*;*0MNQ$VQ;s{ZbY<<!Ur
z`UD>`l{G$TBegV1Wt%Vl2Aj(B)l-Rsf0Rl|zSaz#J<`y8kZ1Azh3b$RI))$|QJ~1-
zK{%}7e{2x$IQSnIthy5a>>6_L8<yV~yiNgRu_t#=&E{;)J+)=9YBxNF%kC?Tr?jMc
zO2MlCQ0D);w03mWX>stl#HNKbU@bn(Z5D$PIc4AZFm%-sO`wcU+3!Bg9*coioU*iT
z@pOK%80g6<tLwwmLNj9vz%W(T+!x>4#IHbnCtrMT6OSovS+OsEyosl+4fDm%H}MM)
zKikB!*`9CWarU)rl`sAVU;ItJ_($ULBOWpF95Bj0Gyn&TpM(fbP@N7lYB(d3x@)Zn
zZyN)!DjJ~w6tzlKboF8KEk>$hpbs<5Vx%g@`Y^LBMyg`E4|BG~NL4KKVXoB}sfsgv
z@%Ni}R>j4>_*YCktKu48{AVVfRk6hv|GSAtRgAdZ#ETkUHvm;wh+&BBD_n3`6@KF9
zO)rXD*<|5G8x<8c@nKH07*S!a4|AEthzfi8Fn3stsIc6JdER0~g%f<3JsKk_oau|_
zf;X+4Dm>K}AHyKbc&hL$Up)7$7*7>e`Qo{;z<8?g3KLHi;vfLG7F8SfE)$ChgM8Hb
zQ^2zuUvC2PcMNAT0cqoMYW6!oJppOsa&*t@XD1*UJ{j=X`;Aymwvd5?r)ua05jh;s
z;Wbg#oP$n)*Mo^q%)|o4L8x4JQd`6yI50UIf2hqVJOLira*~T6Pk=|ZGFUkZ<@6SH
zD`yb0%ej@y3E73*XgNxLLKUJmOuZJra6vRDz^B~^43B%S!#^8c24+S);h&<9duK2n
z%LABi1f7fsWUB3egFAEd(QGG`X#KeY>pT9V+2Q~F(d?A?(d?8zI+~s8JDMGdAI*;V
zj%G)EN3$coquCMP(d>xtXm-SRG&|xynjP^U&5rnwW=H%-vm^ea*%ANI?1=wpcEo=)
zJK{f@9q}K{j`)veNBl>#BmSe=5&zNbi2rDI#D6q9;y;=l@gL2O_>X2s{717R{-fCu
z|IzG7wWHY)b2K{=Kbjr!9nFsTj%G*x-yY3QTN7=70||12LfYYuW~XgF_-J+|7Ab;h
zchw4D6}msy0b-YTMB9tqfnPVa2un4A*m-!&3dZsgljPKtv)pOhIGg`7XSuu1WI8V|
zB-_#4)R4C8W|AKLvG1f}zKH#=W8Z%r`~K_L_y6r<-$~zgz<_ISYT(IS$GfS4Q2eF_
zLh+j#*aOCIYM2H#-%Sm#qjT7eZ+dM|>66$=IQGp0+dtaq3`#IK9Q#hfB(xsB!g9#7
z+(|h0-3An#u4*buI5a7aeUAp&6pys7Bz8u9^r|@aJs<JQElwXr@!!;N9EjBwz;CoP
z8N%<ssev7)dIb1?cr=VU>{(sMe^Y}n{|rVsd`%@~`~kD?rUqf=(d@DKYDN43v+t$`
zVLk<nr&?yme^Y~?*8z1!9PPWQfhM5t1nPdGn^C9!n;HcDDo}66(LDBTZfX$pw?O?n
zj`rQuz}i%4nCjKS*9iCB)IbwcZGh_Np{w20&>zvmh!S-&EvMQ|4JQJ3N&?=0Q^OkI
zsuJ-2n;JF&cejTZ<MrRvAews-__sV7=GgJy)F8*c_X01qUG1g@vb#xWiAPbP_SiRv
zV14X6n(so*a_l>LUoNXfAN!8pPrw}ej{1*%NBzgXqyA&xQU9^;sQ=h^bb#W0V7xvd
zy?}|xvG3?0^(10mwz2xyh(?vE_YwVthw33*JoX(OuDlBrC`&g&IrbfuW8Zxdsk{pm
zNRlDka_l=Q$G%17-USLmA}W_--%&aCEhOFr3WF%696$QlcT|pj3klz6e=)x4J`x`L
zj>eCDZvy&mk3=WLW8cyEvF{gvelvlDyY$icvF~qy{=t&yW8cyEvG3F<I#qln8~Ub(
zs2uy2Otu836S2Oc@}`EU9QziM!9dI12|f}Y`;N-7Zz16Y3M;Bf(KS||$G)TSW8YVR
zW+UlDCAz#k_8paD-xTFO4D>cDN+0`<%CT?3?gHjxi`A7U$G)TSW8Xg!9hdJ!JHu4u
z*!OJMvmE=b5kL0b5^?SEO+QJhkZ(G#IWoVI<4C#s12c@+<5|m+-v%E9K{A~e9SF%Z
zi$)?mzr7Q%oTZJ$&u?FZxJya5&`ac)ksI0@3AY0C0I`+vM83ha{g~;0&o?A5S~Mo}
zjZr&k`Ve<AqN7YO%A(1U*;uenx684)n2kNiZ2sV=ERWK<om6dXYsmTR)0hh#+fB7X
zEbmK5XRH)a|L}Rj5cN@fztlL7XchSz&7{=9>Yqqi%>{0$#rv9_JEA3?NY;Q}RRO);
zl4OX8`tA9MG9?)gFrrFs4SnH}%w>L#{KEqS8KapSE$YGuHe5v10f@<kqI9Xm!{x|7
ziK0G!uY(aa2x69dqIe^X&efG(C?S(^QJGS}+G?pS;T4dx&6AbD7Y7q1G*VU85_W@I
z9rK3;OO((-t;jLu;nk$2o`iU!2_a8H23tmkSgtNl55<=*h?-A?9G<oQ12rE?R^u0;
zV0WV8yHZ4!8>>NEavy=be|Rzza#bC4nuw5vs;8|i9F7zrzj^|s!VXtgXtO;3epk9_
zv%&cJ_g-i_1Mp3MJe&5F1N8-GZG|;6v+g8dW)k}mVlpV+k#VNh_^u#~^hPC}3G_uC
z$uWp>M1Qr0sCSJRwF&swJeul-^Vz<qKPU4%?8Vmlci_TYlaVjMon6~xGjXCAH+eP^
zX(<UmCQGx_^^hyoPzYg_6euN&OGpU$vP2`Vb*>uF-bgqd+;?~q{<u(y23G4fwcX0%
zjd8ztvJ&zT2ff_*WnI?^`FhB0U`poJMlJJYz)~_Na~D)t>pb@r<X);Y7pQ_bdONu{
zxjClcxjdoG_v>oCcx^(T5YGICI#}-Z1(OI{3CgoAwNrO5g>7?p+N@k{QStn0K~9wB
zt33u(hT>fdw`IU%fIptcKy!KwY^BYq7UD7REy!F@q|?}?`%T};3jYUbG)WBT?1M?1
z#`G9a+GBv+I9@C_Jg8nE9zcdqDWowy29)*~5Ho?t05hlfF1ZUWS%x?4YUAaAsSQqs
zPVF%uY}b&bdT#NO8SxlU%UD>BVa@3=pvx&91Il^~v=EPhPeAq^<w)!Pn%NYO0kul)
zM|3dLpqxhZ7*MMf<*K@fZfa3_sA^4*0p)oN2w5>m`dc!*Wuy%##AASo(PKcUP6Wkl
zQtf5xZRjze#AASiGCc-_>OxTb%~I)EL>qbxsA?VqLi!-c9=D`U8+r^V@fa`yLqhcd
zD895*Mj$-~lz0pn(qthGV|JB=ujDX3ygO~^F`&d_z|?10=nFtQygI#9DuwEywhcW7
zlz0pn&J^LC53UQ3%*pflN<0P(XR2`C2ClcNb4qg3b%+glshh&`K)w=>0V60Pg7!g3
zLu`Ba3i}6wBuC;gU<9R!Af7i_><JPD#7+B%B6TxvBN-Hr0V6jma<72Q7yc``P8)g*
zDDfCDB5R1quORM6ij-Esyg6;2Tm@3`7+}9oj{zY~N1LjLud!Bo3@Gs!Fal#j)fE)|
zES1k=K*c=<c!$CXmc(gGj{&7U22Mrna$;miY)6j)WjqEh_M)}R+h{z*)Tf_7U}__D
zkK*$70B9Z~UH5F1qyA@|fxV?7&p0Z)2@H>C8*CShw1M|C+M6zyXRd!I_Ehj@uy4_m
z{XpDjE(1v(i?0@t*$N{-?K<x1U5tdzKy@XW5}dkMU{X>03HLT+f(mkZ&`zV!Q^5ek
z>4hvq9Xts%=O&--sl){hJDFzy#p@H&A7)N;Y_a+qVt6K7Vl%c8)rgL1>3tPtsR!}%
zl*Rid(&^%u_6+FNyFh<tNwkRebWBUH=9nhZ6tb575}0(um-ZynWhY8}lwGkHW(Rs<
zB(J6QbOTR@n2~sM>;fqFVugD1<(%UGay;UuQ}ji6n$;~*Dl#MvAKV~ND}g>gfrR&B
zh2&l=v9}w5zK105S}EFby}{FX`U=UtSVH$~bsCCCu9?k-(@UU!JhA04po*0GfwY_P
z0PPzDr2~oonDTRciSqfV;g9`sB8Wm5P;V=_&LMqI_8$xBup8!`+?`G|iy=U?rd8LG
zzAG8IA3>kqHfJt<S27AE&Te+*?!z8wMv=tXEzeH$UCHSETrcc-cBb!2QWq`^bxA`r
z$b6y}jFPpJ=ey&Ii&Ob!M9<q|Cc^PiS64l!O3<otci|#WL6~c_1!0Cm5>9Y;;bU__
zm}@`<VTMBz4ux}(S}4psgu;9P4Tbse84ABIzbr~9{JH#cDpL^V<hmfN!>oo-m}VD}
z;iixgY&TsIaVP$}iHpm1k_1h(Ak5}f5N0?e;ZS&&U_Zcb_kwVJphNOo5N;%4{uXiY
z4+LRYb@1)6Iokxh^ZHTtPdzW>gaB?W?Laip5=y`l_Mlr!8Bk7?JxY|*sv<Vvz!XF-
zv^)~9JWiBNzPLH^5nv1HwZt?nWR%URh&P!8HUe{><!7MR3@6l^`hbfzz#_J=z;g((
zeMk{3mG*cEh1mv**vbOmfrNK^3K;_wHlD(I^CGX%c(z+7N=Zd*bAjVPQ<TUsBhD~G
znpqLAoec20=24cG0Vm4oZ_y$md71Wni)6rw@=>vfgFxVHz-uj%HQ^L-!WOs|acret
z#XvLfvoN1AZ;t}wWE-pmtY)P)l7Q-i^N~ezzOWBop4vl)X7_bePaRzBsA458J<4>X
z+In(^%-TK{0`jf=gKL{8%>jc$vo5-hMJY$@c+13q6YY-QB92ypIS8GKuarlSaKQqi
zh@(~D0$?^+ECapk<RSWf8KR=(E8@5nxEG<vEuE24pBZSyL2@TB?^!GZPL#x<a3hkm
z#!ui!z=e%WHY89@aXo;d;@<WYw=1r=-Jas!F^by*X`;Ad_Ktx{aMO~zv*q_1Idq7V
zTqMIp-xZjBsI!6YM>ik0skr&{qnnQjJN<uxNxXwWe%>&JjAQ*we!;eW3;6EBcK{nJ
zykmq^b-}CK3@HX3`Ka}8fUHhqoS-G?mVRim6l>{_D^0gr`DvU%ml{DI^}=C!ecS~K
z<7$@E43xYvLF!EcR$)$*%VGVQ+!SPHyE!rw&&*_CPRF-|GCo3PhP+{DyflRS`+WE~
z7!0aWXL0>?VA^T9>tWm}{mRfhidabCsh~kWn7C>=FXMWP(?f}*j`|OJ+zY<7&2XTH
zeI1Lk6jIA+Xjo$*;u*rfVlHFgV+H`esy*OIiW7qgglp$|@a2U6hx8i%3gBU<#9_C}
z8D1X3*(DrhcBhE-Bw-XDfuqcBrGPC7PVMEmQxzNGV1f%u9Cw=9gFMa!IFbj8-RVlk
z`SMY>vr8}~TLsZz%)yge$v(tafxP5v1YTrdj}5$yz_*NHrx6>7g=*<<jF4ne66~s%
zDq)|~9|U7gMZl)w5v&@^z6zgFpJzMHX@>tUW@4IZ0ri!%Zv<Nde@PPr9%i7f4b-#&
zHo*6R8pljnAzm8qs)tw_W~A02laYgnY(_*GXPA6AkfGjH0p^3s7y@=t#@P%|84{o}
z7#RBs0-}*EfN(q-+1hGEm1|LB+aa!mJ>fq{;#71q_#b;=Ttqsw!ElaKajGwVjEygF
zj!)~U$R~7*@yqKU2J`gnmX)EwJU#mm|8aWuF8t&4Y@o`4GMy+*t%w(1I5$ArZTNbP
z&*^^|z%iKB7aQT|73HJWzXr@#_?D1M4aSUZpf3YxEV##sLVpIjadV&0UXy5_-KGJD
zAZ$~Dy~8y8ZnDPRpK^P^@FyRXDlbr2i*E_VmLk;>2hddvL=bqLflLG<Ly(~v)iN|#
zJ3l}%I7&ARn$aifin!G7TnnCi@b!w`i++A?AI3RRu8sF+vVl%8xq~wGE-=9sNSRXl
zBIS}z2&`ik&O1~VFn{6F{04)IcVSH+qyk^B%Xk4YCP+?3FuqnPj``hT*tpoK`z2E{
zlhHD+>2Ehg&uN=LceE1xlUn&?4~DQw(j84<Z`7p>;1X1|E8sSQ(m4ru+)#khH&v2&
zD?#a+1RP(Ex{{vh1Y*$<O_qMiK;g;7?4_nMdABMyc^vw0cj{DuvWBlqMvwkDo_5^1
z9K9mL07@^kf}POm04qBe>teisVXt(&0hYDY<zE3UjMP@mbKGL(^x=ce<0bU4c%1X3
zz4I7o<#eB<iW>~THQ}ArrCrIkVp@<$8A~aUwOnN3NV+tlFwde^ZuJSRe8m8}k=GM~
zy>!NT!k{v00Afn}7$6r~U@fu~hs*V#a||&v3SyoQl``vG0vxN&c^r+EE3Z+`w92@1
zv(B(qCIN?!T7NE5ysU%I5dIDoPdbG)e<9=V7!L4E7`g_383FLwjuw1kB*1J|;6)_<
za1_8S51c+4hVXY1Ura0w;Xz6=kTHa{R|(D-!dhzx4wN*NeZwWwkkNlZ)Kqp4ml8T<
zjJ1l(X%(*!6x-dvl2(odD8{;x;5h`vly4#UCPA^?TL~TyElm;Iy^Y`}1jTl5C%ASz
zKr!As2&Pw{MRxD4Yq3sm9Heb6RZy_NS>QB``*8R`pWnmsL-=r1O!Y}CZDXY^t1it0
z|47<OBkfBV)#+MVu<Be|FRujkY*-A|skqKZz132S718IyM~yMVso3VrDc>hVJ6#TK
z@Ak!4ruB&{r=r*gaQPD)ZI}7{b8XmHs_Oo^6$^b5MraAy%N#neEb}Wff$IUo#hsf6
zG?%p6k<zZvE3BeMFc#wMI{Sr{$Qee$b<&6(Jw(El#5U`gZsu9kpbKx3WiW!5OcL#3
z1h1MT82^DU{&N$*0P#Qg;`f_)D6b-=uhtjkWTDrCicIBX`7pT_BNfrYhZ$lqs5Yk}
z&xbkDVo?1~WjBLiZ&NwS2b^%B0i;v9+=r=r)erpc11d)L)486^Q3xY7O@W;&I$kk0
zI?xyui#cJs!5-4<FtsMbx+{*^IY^dt*G|O=MmlxU+DNBv7y06Qn|SK>d|&){6Hnc4
z^u;gK@kY0I`7jq-jOcco53|K$M7OW{Fi%+wbzAYJ!LUPkS7Z2^_looSYc0_X3(?1U
zS~`*$svM3^e`JGrkM+czMm#IQ6aD8ZKoU#TKgJ8LM#;!g#yxsxfG$UT_V74La~=A1
zZ)S2fIz~Nv_?S=4Cm#<`y4+7V1Na|;(jkA!Wu0*o07?)18L^)blpgqVf|pISvxhIL
z%^tqgkQp?-V)EYX;cGi+Ova$0XAk>0d+0X_ZvvCqLy|nh+z+^Np(&0`AFejQqZsh0
z#6LBC*zA*3@v#9oD#wsHpEPL$M>~D^$|tlkTB21i(}%tWkd|8N!<4Qzz$w0T4?BH$
z&JZ8<^r4KWD|Q&JB-jH#xIi6=t;zKn!G37BTxz|xJkjP)wokC>0?#8X6|Lc|gk`MF
z5d6o4Ww6!ou~YCwD|5w6#e^__7$%d&Or`5PJm{@MmIYIwk<};m=;igiXdM2=4TdB>
z@8CZis0xRyvGe4|o`cHCmnqD}w8b;_k;qrMOo<vP&TB3rER&$d0zXSwW<eT0Kv<?h
zO$1+130NjV8a@TEE@xB4guHS#^~(8~$rVo$z_3sVPWk@Za`Jr2-h;|{36eewGjp$;
zw=kblpB<!rJ|QgaHb>z4CjgfE)bJ?6QlA|Ke=%XHPYpi@SeG+bF(I#<xwf3cb}_n>
z;vyy*_pr8{8Fo@!wsfnq2JJI>uKZtV0ks|rRK-~PS%Io7ul9N7#u=#aLoW>d!TJ|+
zbTwwQo>rWxD3#QH52?*U!czM^1)fh>YG1>*5|-L87W}7#rS>)4V4BrRFU5pBt@Ns{
zmEP60(%aKYlF2iVbo}?WaTu+f*bn6|Kgh%EBV=@RWut>pSW%q51n~zBoHQGM&}Jyi
zU7R8w)d}naTPrMi$#XRWy*VlbXK_@RF>|I9oC9SzGcoHA&efqlPVi)g&_tQ$EFL_H
znF3EhXU`8_O#o5n^zv2GXz=L8wS|fU(vO5?1X`$=Sf}ZL#grGx;#$E;fW?$Gyq&O^
zvWBmmfl3ln)-Wn*_6{UJABm2b;kZj>L2celKzw95O~ABS1o6yrx`4d_>cME43`R=;
zkKDl;e_3&#CQ!9~`N(?g=U!3Vu>rhdnv3~_s~3cy1pTNlK)>CT-$?=PO7&3zmKgRU
z{s0I{sN(Zx<BR~Yb5fz0*8$C`jX?i72=KJcS_J0q!6I$;AXE@O#h?wI_cNA4OB}Sd
z-N2V{%OW<{ANUzDLl9Hh%&>7(nmEV+9F@=~sa*8Dm{;&64(N4L0+Y>^gq;0_D>=F0
z-8BRIG{P#KVa=3GAPeXAXIVxAbtY3TMc_pQ#w<n3Pa0w*BcJ5rlGTVN9uLr22D=&p
zn2U~stG~#HkJEgl@x920gyS%6bxLhM)@x=xJyW?(Wt(!XA1P?L0c=4_6AGGBKPVZK
zg3h-Ebw(}E#&h>zEodIA><*Pth|*nv_-|QEFK`)R@=k`3agz{w&G1<5xDTps<eLNB
zN>a_<0BK)!r3JzSe-tCT0f=#zLN6~5(J9siAwTXubr-oe0;lHr1GgY%IATstWEo3Z
z`6;n9M)*IC@S0jSg-4MO5q^yoE&(#`qVPnP1mUEC@P|R}l<J(PTH!ybnH0{gw&*GT
z0I%2?fS8jLS;mr9eo8D2gdYRbZfAs}*O!m`v$~bSx!<;&!o5I8#N5D`mIyqHfHRr`
zMws1-h!yZZN_f7q%n~4TjroST69TBM9jL7zyPMkDFSRuq7^$ssCR^yj<>MYu6T5+(
zHE=)WcmXOjxBz8NWJypcX^imsMmW-!kL$RPTH&Q26X6n|@N+0UktIPmX-sXMktiJR
z?LZ-=wjQT&R)qvu6F*aUB1=MTk;Vw$XoLqCgO3|<7g^!$78>Capzz^{IXRIfK{#n3
zoHhBh5$>5n(0zcyX#tBU+zYTauVxHQVH+igDZn|K!f^YK{<y9yJRe(T36ME?kzsCw
z0Cw48hO^vo-dkkM;(alTGEhlVXl3ZUcJ_f<1X9uI)s#u)N`U3xhM1ERSrW=m8k7CO
zM!0J-;r>Hg@;(Y?*(Jd2*Fz^MWiLVi*^id#f}`QcXSdCMJ=t2EOsa|pP4~s~JbOXF
z=ONT=#kVG&r$)m)y!g@#)2B4mad-k=F3VOc&>^%lxMGXjpm8f9Z-~LsNnoOpCr@#*
zPBYqIdAfg@!E(8Wx6nGJiw*WpgW(bx*Dxp@YZ&YJk7c4O`9D^6NGjGCmSGV1oR^$a
zafOMeqrit=LQcgd6VHRKKYPhI6<-+ZD?ohHo>mQ#yc8VCOCg72w5<5W5c7O%caPYq
z7-K4eCt0U?@hlxRFiaM<Fb+GVvkc+snn+|jrHf2#m5oKpPUYDqmV;i!Ee7DA_pXrP
zbrH)FM!p^9Jxv(-0;y*zGxSxwXLxyJ>jy7)PUV-r1S+Q)E0omEHvm)XR!pTJwKj)I
z&8fWK$6NV7KA@tZZ>+Aef@U8^kYxKQo9)W+zCpiYrBBd1T9E2=Bspt6IT!ln-0hQ-
z(d!RO_JohO@^wG(xgYq&2b8ujBhw@ik7Y#G=JB&kESjILb!TD+6V<D?H9M@R^F0u$
z@*ap&_#TK<c@M;V9-HzGNX<W=kUbhTpRPF7-vhDW9z<<e_ym4a0=vz7AQsuX`M7hb
zU;40kQ70S)%*C__9R<(g>sLb*$ddw&41s^~jX>GAM{prflPt1tk6_-luv33UXT+NZ
zHR$xs!vN`|)I#Reb_fZ!#8}6>{`R6}+D~C71`~bPpE*?(2qw*#yU+=S`+~;Y%XjEY
z|Llc;{=5HNed%8kyW!Ak_OVI4^e=Q4gZ9$DchUSq7YOo71}|a|XS8m^yeyX7@NGch
zwm>9jUxlC(?uq_2xzSAwI?0*nLc`(wUkP1}d1|;2vvvFpXSgfFL1dmXlJw<;lNsd2
zf~h%mpdo#+U}WFlT+ly*0X6cI7SIaASL9bDYc3Xy{Kl}pSTOQC!}?;u$bN?P#e$Io
z4C{*p@%9P=`eH%6y@G(gSP*ZoAfPW6#M>(f=!*sM_6h>}VnMvUf`GnQ5O1$|4LQ^o
z3*zk+ZvoI33*zk+%&@*#5O1#_pf480+bfm<&=(8h?G?iT=!*sM_6j`cnTrMS_KNo)
zO<ydCw^#fVfWBA|Z?8B9Wcp%3yuIR10QzD<yuE^ezE}`%uOOf=7R1{t2<VFi@%9P@
z9Y2)vLQN>Mv*b-*EQq&Pe2;X@#e(s-SFn(Bv0!8)gZg5@$PFy6zF07_i3N<IPI+;8
zFw8oP&^DRvv`Y|RENfF@6>3ysYa=!pK$Nt>Fn7?SlqA;+rmfkB4BB^CbOWyX^@5M#
z2{M>=&1@pxfc342ZG>&EjQtx;$cZ(D398rv7@QmHh4_F|llO%L(>9+?hHCc_@*z8z
zw(9{Rj^+&$$F#)#l4(dLDTvNv0@6rJR%{<mN{uoAVFAJ^(+2`b5g?W_09nN4lO)SZ
zIR&{(iwKbI)I8&Oq@Ps$8oIK-URd$h3oHJ5Va5N|7gp3d6SINrq;J>afxGrEXw^wK
zEA~&0Tf1r^piKe=-P$LhM@rf(P}r?KU;?16irrnzt=$I$Owz5=EoZsul$eBj3Ue{b
z<Q3H!zk<rq(Rg9SKtzqS(V3KBaCl+GG{m3g;h2w1@5fN#g%uZo;%1ME65>=6GNgE6
z#RDLF(j!%TluSQ^`NIn<UPt@~7MH<J+^I7FGyNoePhm2M)la}HJlIPaGKIg+9>gW-
zdkWc`s(Qfp@Msv7$>GBHGZ~_F#_I4r+DZDJLSY^Q#?z0M+4sT<VZIEE4;?M@Vmu}!
z>3a%=c_$cmS!SosaTw~7^gV@w-V4-EakTG+6|50e6U#>#{6?wMIxSEMN&22bLAM90
za~$n^VTGVeff^A<`(9YV+ElZFnjc5|URc3eQWpTlw@T==v!-GYN;3Bp+80Xb`eIs6
z)_B&bxu@`1u<ax(ZBkcBmj8tnp9A-O0^a|^iV&K~G5BhJG2S}8S$2I-p=ho(@LfC_
zv8%efFi}j>_Y~4LR5|dOq;~2J&j3o_Q^*LH_x)+{1btQ{Ml~5M+S&<X^1&ha*e4)E
zeMI`L6fCHuxTE~B|3U)|TuU4;{L5s(Iac31!tg)5$ok?GHRN0j_LQdZ)RIYG-%J)m
z%Z-vl)6II@%qZGdr%OZDG_j^Te<3>v$QxaqE-qBlZ92oqJNFQDA~aF*Q!lbHL-MWU
z5Nup%g8~#oIjYo@rXhVqR4qj(5%PRW9$qV<OBZL<g#Fbj_IV%-N2?KvdNs{(mEK*`
zv?UE4ntZBi*cTHmylVFrLo=Q<admnLW;SMBYU+E{4Mqv8t5Zw$`v{*<n~YX(BVqNj
z8;N%^{k>`wCUPvtyCjtU@(`JDj@!;@VVdHkvY~J`c54{UbUfT?&UiXr_MHaw%~1s|
zopWd%4V@N%%UlPm5`;_0T|G%ByAyJ+X1Y_Lo9l@eaq=USNBgs-HF&)rBlb1CI}$I-
z@WdWjn8z)0oQC6=vya>_joB!66;ymC;!(Kn$T6Nec0gNHYBtfFp&N966j~h{xXNg*
zC((QqFzA0FUiAyS0MlY&ClXCDrpC@@jV%d$Xe@?rjqqtkj5Yp40Zt?OT19i&RK2By
z#`Lv{7D}95P-skFt7wtLSzoIr^tFoiezY^5!J2AcE1E|GpL3NA-obDhn1f+<HqFAE
zD>RdEP{PgG(+7(KO)wO+!?(reY#nf-937gorw<Mc^hS6{HFR@!#lbm&;}Ls`#W2va
z66y)gnGLl$QRbjIJKf;Qz#1U0sYclwl@eSjnrM{{E_V{cfu|{DEpYB6mZ;@xFnyfl
zhrrfOo68_feF_R(XQ4h~WV`kkvtIrsJ-XA1&hBoJ2&!`o$R<s~@#G>_KZ0`+<m0H8
zS!8712bqC3B*yJE($zX`USM2L6StNv!Ra90B9(~Cp|^;7BfwO0<Wd+Nb~-ZeeH)`n
z%tF#sQ1GMH(~fgr0s6MZ*kmn-fUh8_^De}Ef|xE;&9jCpNi#X>hxb5}_W)42x8kSn
zsj$444AEPt`2rkxs5%KNJcSRL*iWG`r|WJ5wKameF=4mrq=OLMnSS91KIVLPI242r
z=S~G&Wt?8XZOZ~~TNZHJa*x@~E8w=}_XTF#5>*+dffjK@iWwQNoAlNCD5|9ge4B<a
z&>iLI0q>_FRN%P)dag!;RYO=GZvrgtZ1~SL{0Fci4!<x}PKN9y*d6~3>QuvOK^R^!
zzM=~)qR=okvWbo!1h(8%sIV_sau0xJ9s@rja5)0`;|<66T9qh?6nFy*+z|zK!w90p
zg$>Zk@j;1kuw)o&i|H*f+-+=n6YjO?J!I4S)TWoR6zScVkly0Mq*rXxgVR%`VWDBY
z1o5bGWYg($t1tc&6Av@Wf6NyT+LXYqW;*ZC&&LMWpbK&wr9sL9Nn<Ec7D%plibANt
zy@u=~Tdwu*V2;Ado5jj&i;}SNX0a|V0MK)50qSBFiFOf`x}3$jY;qdvvhE0NPwR}h
zmq5q5Jem9_gF@HkJYAO?bX_(yOkQ0s$^@I#<)vVey4=P9>+(|s@_QPNKUbH-O<kT~
zxZBw>FI#ETOFrGC*NFk9Hx_~XbqVPmMO_XsEUe4FLA<TYD}C{Int0abR$qK#UG6kE
zU6&l$i#U_pW1^q1<yhaT4yvmZZ-8LkosOLMJdSPcd}{wWf+<}s*nF9-z4KXnX9Fyb
zjMTL^!wBScc&xpP$-fg6y7tyqtG$?E@@nt0H#4C-w#1y}Mw`boKy99ZKz>KV@uzFA
zPod$YQolheR;eeL_-Qcn6l9t$dpUm5l{xtfeN2g(z0Tl7vqhZtZZI*=drG}6ODlB~
zm3k8jpp_~MlYtdhsR9lqIEyW06I;j~1fMLl;4cKF!EB<+$E}3QUpL9kG|63u#HjLH
zsPaodp;dmXR{1*JV0Ia%hBl9P4FH>{`~k2?gZY>NsyulWRKCw}{5h46L*Mhe;iSqZ
zAr-6gHlwtyutPah;<=Xym5buJ+m_$mkdQS|@h2IaRy?00=9!pRZK2lFnY!mE7@5>l
z)MjfGgniLdRLx=lT9;2zmrnsKzRXBC3wVA?z)xL1ONJU}SY19>Z6LYIFnQ+t@}+f+
zE_1;mx}3-Wb-5aW{6`GOpVOrsNOl`;%)I5J-uTU?x5=jWx=rtArgtDAy~A}rCGmme
zDZ@g|r$Y<2(vn9jNtE$zO*}iFEMI(LP4zH1sj0qvek(RH(7P_e`e2xorlLoIEH}tc
zbV@vx)&M7vf1ZqLtfe?7u6HJ2>6dEBQq)qy(lKfH8^Wg-fxfn4-40%jIn*l5p?X44
zf7E9kVOx#p{xdDK6VQON9$?B+t&pog)wwH5xY4ZY%IeoX;EUfJqcb&L=iB-ISTJ?y
z38IF|Hn4#l?JZ$zL#U2Zw}4h!*i@L8Sj9R9XchM(kiXx^wpOtKO2>R^L*n$JG(Nq6
z;{~=Rox;Faoh?r1@>?0en~^PNo1(NsOvwVN*?64ON6Cs?nH)?ondVlqE?oz&bc19z
zZ3uTjrz%IC1v)9&ceZ5p&Ne0Mi9mkEe_ygZ!`iqDiaFT;Qp`05@QS(eh{f#a6|>Lf
z#=!C+^8m6Adlf}O-6?_c0YP2*KFv%5!$H;>hQlTxb&?Vo?p47s9)HN+;mf5aLvA~n
zde9vdmmrEw44zQrqt<g7L0fizvFu8yYcP7K{$ill0Md3mK<xNUz(Y!nkOjK+@dPt6
z&J)bmQ1qlT7-pwc#1)Nl!*Yt(q84V`bcZYEA$yHZcetc`IUt?x2uU~f9JC(??Xw%$
z*nY+c*$hPM7BW^^$VGsQr;gK!jFl2%4edB<LlcY*ZCD{K#1i89RX%E>dJ)5s*wJs0
zBrT*Rru5Q6N)gDv(8&H1Ed*l;s-ns;is3wH0MBra+&(6GhEx2O(PM?pa<(noNnY8c
zee5=bT>Mir)D6&qlxq!(!v~9dY`J!^Tv&}2iGQ|SeLgnnuys6wL}+`k$HG*9d2<rW
zd*p^P(<^VEbB%a5l;$Wa#dA{RZzlPZjd<q4r0?|EX*k(-PBfe{>ZAn5_}b0_<n>9z
zubT-M*LIkgr#*9B*TkCZa>Y5+FVLDE0G5j-o_QW%82~i=F<}`1R)_(QJRk5tPNP;R
zuJ61LxYH_B&Y2KQgFaKl^teC|2pYN;kT&ntV%|>zF78~RGi3*WFB(}BxZLX^#k_|b
z!j@9ANcjr26|`ao4K6fhFpvQb0CN$@Kh3cG88bj_JAFzGBQ5$JBqHO+VXfLJTxsa#
z>A(1w@w!M|O-|3pf`sn%FQ{H5OXwBpLbVLEQl!ncNN?C8{f9+r{9hKSu!TR11G(C9
zlF24gl#jYzeaO_g+|ltO1i0ie0fGGU4dq;8k<w}N34K^fV1hTr8APFL4e55+g4(E>
zz#K&2Dl70d3fy3*F4h87S1K5`>-4E-Fl-?uVAc&lT1bUWsn30eh%IEYAp&>)cRpqx
zW3QHG1ab;5HCoVZgNw==Obi}Vv=-LaZ3^3#$<e*iHeN%HIfUFRCLO-mwuJk1OCT&Q
z;XY{z8vqZv#|T}REhR8RifBxaEA;6fhP4g+;Ssoq>($#0&tz>pn5FG(4$;dCpvWf_
zXWQREG&AyqWaRv{HY3|4BemAqjBJyPoCUbkUR1-=5W^<<w6J}SXx#{(kw%zWg+}<~
z@j6l42xm957Q4&qhoupIXb3%v-L2Y#RxI{HNRqzzAqLn8KR_Tqd4iVxXBr{Lm_F~C
zwActIB9T98glQ%r-3ZSy+_n+cgcfuoe5HDkETLDVSCs;-6e;r(Q>5Muut?Jp$iLIb
z{<B3Yyw5O77Nrqpn;6WR<fA@Ryv0x2%myaPM))!l?QSS-Bb46XG(tNBKx@j7u>qY2
zNxmT_-hc)gA~v9nhKSRyl|E)4qptyF8zt)o#O)IsQ|Z}l4XCmo56fczgyDZ7`?^7W
zFAb{fQouYrqrMlDRF?ssKG1_F6PAba@5QpW0WQr>!;1nIPnqwiUlliF0&t4%5AOmM
zaXBrUaydsDJbP&HYJdZBI6kGht$@a}$I3oiOx|xnsjq4wstHzoiL2=PJZ~`Dfq7bE
zC|@kDrJV(`gX;6WkMc{O43O$2OZG#PCTd+!9M!mtwyO&Kg0KL{1@ofMQIvIte5W=6
zwETg{v(_N?k)XkyD2~=(6PG<fgDU`w2AfFfXAl+*YWRM_qQNFEpV!pipuuuzuq0Xp
zsJY9Bz5+yw23v}Zxdf>}4c-i}gbu2zy~|CO?*aGOZF2ez3YLOAqHwv(s=r{s2OidW
zzE7V?T0!jLI%zqwQel>eL}Bz2b@GQB%0Ql0C3^;^h<j@D45qE8wHqO+bPr2;A8Wp=
z%O~KjSHcns2={jH2B}*$Ie&#=%``$~-5JXxklYlKHyHGbo@8!w4aNd&naj!2Qix<f
zTqb(^4p6^nJ0zgt6Rtuo_||;(x#dE#8&M@+vq21(Jmp;tHzV$jyk%0VZBrWO79*+L
zM!;<cQQ?<{AP+fp262z1+>8&!NI^|-D@oo3@t=U)Dcoo1WnNy-keYe9>O=!P^hQD;
zb&6}BTCqVTO9v%XvJtTWM#PU=KOFiV3QscZKjt%H)X=%Ok<|br@uns<OO({?8YpSH
zg{}rPv=@#i2oA3SnO$jt#_K`2$OBgsoLvdpIYRrYMtgAGYk|jw;d2E$mC#ZPT?44!
zloOGwlU?RBdL7`G<_SCo@Jhht!=?*-JYct~pmZMeIIKSHV}UChe3LqV^X>j#LyK5_
zi=>P%f{n`>8d`EapaqNYHnhc3nO^}eY26b7G`wpADwj32WQsO&JNZx-xxDC4YU_fH
z(4A1-Vfsyv>Nz(TRC%mZXE^1|Jt0BP7FT0YKpJNJr--}3dky&{@bjbAuYhq5W|h4r
z#km7%vz~T{2EQPLdeTs*8vwCxb_#MOLDr21Um^(Izb?qE8&Rjwy@ud-ST3r6!)1E#
zUoIETzu~SHFs2p%hI_7nS^0ye>SEo2BA_3!b0pX&-+P2-Z#ze#$#UOkI1Lp4WATs6
zRPh^=-z>z91C&iS2>iFC<23$r=t_(K0^>@Xpet^1mzz95;qp=Io3=+RUwnvNY(Chi
zQRSm12t<q;DYKxFhAnE@zHtA8Qsr@hU1mVjpc<JjoQHrjoGUyh86H+EKk74A=tuDB
zHN6|>7HT9a6TO5}#C`KshGD;u1H1m&4lrlQ+xISa&2NKnoxAT{U+y*-M$F~qqauMT
zkoS1*KC`(?xd{YPqUQ}ymgNZqUaZE`7Nk<5a*yZB#1j4L?yttP!1Daf<8h|xN=Gj$
zAN8}Fi&Dk&_K4;AhsTp`cs$J=a8Inpqi!~NYleU>Pl4g_bdVglx*AW3<(W+$siDb+
z2dj$mQ8fdHs-YsDT3BUh|6ys@pU}}(&anZWSy&6Xx^j*U_$$XSW?IHpflfJo1w3yv
z_6;*8m9yROc;?nFFufX2%}quDoe<DGI}A?)n@*R&HPv`VTb|QAp1p<#a|8LPj)B*z
z@my<pp7eNPC+Z6I^42I2fF|M<`l;m!Z-!7QPqyLlM!jYM-Y*#EX#-L*wxQ&a$|*KH
zUgZ=Xx^j4CWU`@sMHd4t=gVAFj!b6y1vr_R54rv3v(xV%U@LzDaOobDq2xeUL=Kdb
z1*uyQnNtIiTZ<4mJ-{;+k07#S6gg)J(FcGB@@U3`CedV@=t+TOq?*eE8YNp1n1;X#
z1e}435qdh2d`2LT<UFNu1IgDS@QjdC`mcr@IgpQ<8<=LLe{H2lZvn3GPD9EmK%vc*
z05(klUh|Rl`N;r0PPrR$F<ngxU$nrff20}1>;O7fx=a_^+)*$-MLe04GhG`-ZCBz@
z35A_8dT<z8q7QB$w}Ux%(BMA^Vw7JR;M}ywt$<_}py3UK7jagg;cmB~0%g*5nh1Lv
zaOq^!l}u=tOI+UV9JVoTYw$9FC2Lr{D*~L~)Vl*SDo(;mPSQEu4t8~I;N?!xJ||!&
zfFQr^42|RjtUyf(Rb9FAr*JDwg>N?H<D~Q7VBtJ}B^3t;H)t&huNAs1h@%}wI8~GH
z8evB@Av?pW7iWHd?S`pXrp`$%txqWQbSQK<&&V8vdLuAu6ygsY7%>ok;2W)-sOZEo
zT02oOIEH7WypvC3ZYE^UJdJsnkUi}b9fwrMzJjO?<KDq9PN5hls_}W4`V`~K*&rW?
z^|4Di4K)7iMsO^~mpDE#%0aHo$$<39ED7-erL|M>HpZS~Z$L{+{g&i>e$_{ukFV&C
z$e65FaQRawKrZL_E<|mWr${~*>vMKm;ISW}uH8|T#bqXqA|(8+o$>XMY5F%Zj*o;o
zOOo;9Z-m?$YsGbj4{`cL=JB6ka87;6<q4+G7%M-7GUZ2DgW-PyHDUaXkV-GHJMqtl
zm(0lsI1{ax7#}?{iF1x^gEEH)Fc&$Dl%HRT35M^+@1%0{HZ|A7%$!XApfgAU&S_Hz
zY-&5;;AtoY{m8P>Q`pa#hA4e?KQek>1>fuq3iy4UP#=FPT!aC_b(1$7hreL*NfPOI
z9}8;dGvh@Tpch%v>ID!Hpch%vSqxe)vao(hxN%Vn(8DW~!y;%8ujG9lVRPc)mHg9e
zL_1-6cqRXO6@%f1C{OZl3~LXs<lh<A9$v}&8P*<N$p;wL9$v|~;sSv7@WQ!y0@}kX
z*%d&0cqIn~&>mjNNdjmOujG&b+QTb3tY#pG+QTb36%P^C!z(#WF~i!!D>){B_V7xs
zrA`Azdw3;hs&W9@!z;Otnh4;`l<qK)<SaZrSP!q{`s!=|+QTcknK~O}+QTckJ)hK#
zhgWh30knr#a*hDn!z;O?0NTSVIakbIdw3;xmb_^XujD*+0n#xZUdiTu;^Yl1q<DBG
zZ)8w=cqQM!;%X1C<V`GX3?6_5hzp4sMy^K`vz@jM0mia6CH5K{TN|+(0YphFZe8Iu
zt<g^9;<2z?JXVwEsoiVu2Gve{V+=UQ@Em4J?q>*ly-}2yT0D%IQYZ-9G1b}!eWnyi
zob8@!^Dt&g?>EpeHl*_~W}pkxMRqt*$#@K+0(}vYxx}RH)RDu4(-4;>hY3$*iL>M^
zp#+_}b9$oZY+i1<?CnUDAMBlD{!nj`AX)G`FiISYgN>>=5nm9R0$?nFY_#usACj^K
zabpRdB;Z7VRRHUAF4;gHCR?*@zks*~c<yY&X4UL202sam7)W520cI0Gk~o72k!WEJ
zW=}<;?FcRd*f0m>deY>49+FT6$8|;2^T1_)GgBAkTc`=2P<R9~6YVWQ<{=cOn7Wgh
z(el_VP5hi!{2^e|#V=>^_d}!<znn$Pzeg9poK*7w=;99}ZVN#v{xBB*BY^ccc*P%?
z!$j{zQBeF*%t0jq6kmWf1W<edwh}<`N0V&KeE_8RV+cMBP>O%H$@yLuzfy{ybU*84
zc0%!)aHCIA+I0sFu$Q6~jcGRrp-ycw4$%h=G%dp)1UBRN8rs4p)Qsb6Xe)!wY2uzm
zH~*TDHRI;{30VV<jzKP3&?Rz1%ZB((34CeB*H-NQQ>FWl>Fz%XbKs!Uvo6?m_dlt(
z?*8wJL5TS@X@q9R4nOcF#$+d;JN$s|@B_NT59kg*prpeOxJmy)7i>EGfOZ>p+2IFQ
zPlE{e?~M^Wi$O>$#7L2Z4!??^3ianO!VW(v=SIZn4nMT73Cs8l$`ksDeWC8~L%&|Y
zpmg}5-x$^%e&~0Gb%!6?&#><BLkAev9exNdEC6(eAA&y<0Nvq-Tmf{49|{VfJN!_R
z0J_5ug#^$YekiOekVD<!hf>uP0NSNDlqQ|G?(jn~0d$8SswF+F?(jpIsyA|=JN!@`
z>HT$wAIehGAx*pVhU%+R0q71t)J&ZU9^K)G+N<*bXqVnl2LW`4AIcFxcle=>0_YAu
zlq<(+bcY}6EP2x%eke~ZM>?j%519@>w1I_`4nMS!LEYhpZeVeBhacL+>S2f98gZd8
z>oCHqWwz7KM1ZlZO^Ln8#<Ih|6hM@;QOb~BKY7iG$5?0H<V&6eL3L+d@y$)%{4NpI
zd~@ZZ_~e(9U@=E^QGD{QJdz%b8?W}eb&L5z^uPRW|MI*2%kTF8s^9G~;&<zQyFC>X
zMEc#jH!Jorj=m_~-6W8?DBj&Hkhv({-Ky9#nTz7xTcz(b7sb1HemDW0!wh`WXM@VA
z&4gU|-JXW1vu$(+B^Vt1Zm&T6tsai~$nbs)75r`=1I61O6(z)}BxFe8cl!m%e(*@=
zOJel9b>Vj#2&0F^SMrxGL#kX9?`pr>B_LKUfT#bk&}0a|TomtWzgu>gYB2C8do+y7
zV4v>$;iN%lOfHIdwco8Up9{v@j+U8zx9$M-8<G&_XTbQ?(K6HT*42Ku!tADCz`|Eo
zk6aY*YQI}SHv+0f98JGl+*q;$vm(}rDh8@Q(JQD^xhNjjjig}WCg@2(O^c)Hck7<0
z0*wuN1yE<j(e%4@XR7^}@~}45wLsk%N7L`tovY|m%UV*80rixJmW$%u`Fiu4j>I1m
zMVr$qmy6<E<9GWra4zKd@$|dJs~oumpy~kEH~~+;TX%)Jsv+?C!1ea<V!U!uysQ0g
zMROIvpXkwuUCBl9uJ*g7ZKyMV&mgtFDBjh6w~TPF@K_1@tVoQ?GbKS_p1>hkUlbqC
zzZ8qEa#4KvzDrmw`l9&o{RGTK@!<!Vp_p5bugeTSL?GR*cOCx14-?38>%E8ah94nN
z->r8yfkz2syK?V)c!262ffDddzmkdQ*g<MIVkX$w400G$nVN&>B_8TJ;o?Q{;o<6B
zL|<xAx)I6+zG1m2{w9#hz4Q-Qk__RNi{is_QM{=9RiNMTNF;5!C_XF~#S6*zK>tRP
zTOc#zF6N>B-JJHzMe#zC4(mM*Uq1;iiVw?0@sz6af$r^*=!7~P2NJm`UP#6RJuQKR
z7sZF=qIe-$4m5A2mxT01@nN|rUa;2yvoVgH+Yr2RQM_dGVPO7FES0a-T%Q-khvlMp
zA$cF@FA_+2QG8e~iWiatK-Yw|#HAQfzq4B8qWCSe(-xq~C7o!E*>mdiqWG{}6i-p^
zAfU^wD1A|UST2ee>`A~ZuvlGra#4I(E{dl>_Z*_*@}1~2XaRERVat?)D}nqL#XIhn
zV)PxXHI+TrNqL=|Flnw@r@Xln@i6H}=Ky$X7XVmsS}y>*Scq)5UVAX7>?Y9EJ@zgD
zDp$Galfw9>e?X}^wuj0>OcNVxtXLQZsZNOQ;h{buSEFBgL)HkzT&fYkO|*EQDXZh}
zMZ$EDREvQ=!;)l(2r*?ft0}s1shO*QzR4re8Xt;OQag-9z6q<xfPRi7|ANd6PW<)N
zkg0MtBw_U-&|iBbTFUjcAVsc*<RgJ{F_=X0)lxJGuZB#Ot05&X%>Z^G2~{M@FsVI_
z2jbKjw`H3=mx5+Obvh=2$9-me$xWyVbqesyJQ`Mo(|C6=&}TdgBDD^vYpSE`uLNJ}
z`5o~qws<elTt}A-JM}*y^Q9^H73@x6c6rz)dtvV>e|rR2DTy0%rH+0Eg$r|vm&j>y
z1$u>)jgyg#Aq@yAUt7?wI;ohxs`}1Ib^8H5*pg_|SDP|X913zz0A{Ymn#w*8y{>xh
zhIB(>-H3H|ycV4y+N=KrhDG(+1XP>IyBD(P*pfi(G}w*_jrx62wxOdJYyyooWGBjk
zxhz=nMHiq-n{ufzl6wq--oi@s2cVmAsW4I~2)oA8j7x=)B8jsrEzP-980mdKy0;B2
zxK!wN;h5Nxe%pB6@yXDvO#VV)oa@2kEeESsVa_#MNjN0oKrna-iV)BX?4dAiF%)Jm
z&??L!t(AoFaEx#$%)zWRN!S~-LBw<TZ!7mzK7ddt%mxq&Q-w%Y*x7-!l5i-@a67g^
z6@X$J<J*2SG{-;(t~}17=$wvNIGZbN!im!Ob7h5d5QsX-3TJ(p>~@kRP91hS(^&SH
z1u+iO{HXOaz*B{97n@2uES%;2VCY_h=JLXGpjmtfIPKWQIBcu;;OphBJ#WOjlC376
z($*EDUo&jA5y_8Q{}wPm;#)xWrAVi+lc8bGyXtcxM#A~9_hN$uQa);Z9SEbNu}HWF
z61ojEG@TI(2|PzuINZd2j10%6?t`A#jZHS#ou1S=;4O#E7f|KLJb*wN0&5WHUJj-0
z1iGjWVro2y9(Ji=XO_3d6YX)0iQk7rpVslg#ZGQhC+~S5#_8$I)!J-rl9xosL$V}#
zDFO$P6f^O>B*VzfzFw^jz{!7CoHy*$mB+oh(n+Z+y{Mlz=cl{UZ~T<5dhZq~w)+7V
z=S)OlPr}!$J|~~$|BX5~f}45Txd50u@hu?Nhsa;yG($s;8vQa4IS|hg%Xu7xJMs0z
zi{<tZd(Ee)kCD(kBczF!(2u}0$v{Hn&w2=fXAu~NK;fl_Op!;2;MUJBZi69s%ahg{
zl6Y~eXV~XMP;XvarZzGDH-LRsfweE%(pKiJ;$dj{-!>&$e$A70U9nCv8gW2u$I;ih
z9s=&eH(o*!Jcx0cF24%Fyke{u8W}JN`Ka~p0~5hKrhrm5Bhi8_2<$<i@KQreHN-V*
z%E`fdgCS_+Nox&BOY!x}S@#iJ&f^(h39!clhK)u$5s{{)^m?*-dT2eUNIJpjLUiW%
zu*1;&=p}hAB)nS3m*gEtqu*{+s(Oy`m5%~^jKk*h)N8B90FEa1dG>lA0PGQ*r!~uo
z<j*=oEG1IM-!Lbyfx*+*d-OH{jlBot|DnOZZwx+flwZtgJ~2y;m_CPTH%{K!e%4!j
ztlJH1@58W4lY7nP;C`PN3~(q;XSz=8FwysUO8o(1U&-<*wd!%J)X%8Y`cDANr&2$s
zQXc`><98#46Ye7LYLkg;F~j(*i59yWfLdH4#3NPT!<50v>*r^k>0{kySdZA_{Ai26
z$S20>%;~{86H`YU0EUHcAh8o#m}kNLw%L;Z&XPCY4)AgisQoN?ttSD_gdu6reF}=$
zVTAIrq7+X})fIt4!m5Tld7t@m(<61h&H?ptuJDXh{SK>-3`4C|>0{ktSdXBOKx!rq
zD9iDgyna48c@um<k24LxwVGxqEv>x&Ve$hT3#6ubLNE8par&qC!V5tU+TU6Pkpo(w
z{U3y?uS$v#f8fBtj`)L8262NcY0DH0=YzPxm9&+?!M~&Sym{E*OqjB_!8N!wAv^7g
z4n#E#>4B&XrGxOB64+}txXSF22oB8|=Yh;AvoGdlC#$1|-iUafryxN_XHq%up_2$Y
zyz$#?gstwQH^R0f0sDzG#k}sXjj+C06!~i-?En9bu)8p~(r>l>QyXFOl32I+CK&u*
zn^k{pR{gbE^?!Y{YATin{`_VYdym6zRwZK?j^dmC0H5Ey&8mi6u&~h?ln~#n>WcWG
z9*)}{@olL1X4Pa+aLY>si-b6p-k{KlVYBK2kZ}i0lRhSi+0ClWh`-0;4&JPK3dHIq
z;ALNoWylbI|7Mje4E_r=wJ`zGX)x-rI}DD0vr3rTg0cK)nSGm8!aM_v7ac9LZ?j66
zH-S;M$)tMxn^l75M%YVMwv(B-S@kJUUlaX2b$ZZdRRF_z3cd!NxLMU0sFrcGZ?lT_
zpn3r{AdXJlteOnei5|MzW)%;JuOUk6iw5LW+pM|<xJ?Oo|7O*rz&)FQ_it8lBj<Au
zFUITNtP;%~06rDXN#}vnf9Ighs%F6Jh2Oe-NvzqdBD>qwV<mW4Q?i>?9D?m;RX&tw
zHmlxewb;!n0{;&;tM(v=e4B3i$4ta-R{ew+h1E%kHOu^iF#`^lDZb&czK8l!xXfl%
zTSVtuly8M!Z&rN=QZ*Fl@s=b*xP6;dqVoAbFZW19<-W}-A-Npr4J7#yGBbW*9(<cs
zLh=yMPbZKhY*xJo^yeOlPAFlsYCq82wABe|l7!8wMnJc)BnNF)^#q1bGa|sLAK$E!
zOilpiBx0$2t!Ce5m5{6g`ho<K_-2)m+z9l&)kut}gw3j#K=U@~L~FV*@y#lVa=!!m
zM=Q#|StZys<gyOFI=fPNiJMhzh>pv5qBCGgW=Z`m;2P@Fw-pF_k8;uOfR59rZv|nO
z)Q{7rZzaw?+hyz1x7WcKH#E{q>TJ1<eM{=n2ihfdHmN3Ij)+ad42L9q=p}XLwMm!_
zuSuBUkc2~FHo2y>S~Xk6un5K_xMrKPb)Y#G#b=8$qpZaivJyTR6fI>XysNB)x0ID|
z9d=sLy46{CnTKXPN6JU7pAD{;@oi1n41D+$mhgvyK%K>Ve6ya$O8yIm?Rq5E?mGi;
zSZmK^*!En8ZO>)c_FRT7X^67{__XIT>^betGHjF;6lJq%#a5&)aTe&M2$WeqYW>eh
zE{4sZ4mK-lR8F8TX_*l$d$yt_ssF?bXRgP|^LFF#l|oQc!<g&FhVCKMTibje;521t
zqDD9~z{3qNPqEFsCH;<+cMQ}}ZWytXV)A)CFi+r{OZID#Qm0XdhPiKVm-IV2$Vg8V
zZ?eHwc@jPX-HljT$YssG4GFn*5m@+lER@}4h+7-tiI(g)V1=y9GbqmehD>VYT>~6d
zjm)z((q)lhW3K74g`9IMq03qm{}S|ev58-dioe<yPg=w|P347NxehnN9>>?yq|=cF
z>;eTk3w1?9ntasyJ-`I9gpo_GGf<pPnTCccGYX_><sbnL6u6}(9rP{m^%7tk(97!`
zDQgB&$~IEcy_C2TvlQQ4vOj{9avLD<ZzkFKkV)!g=REdDDyoVN&6l3QYry^mzMiGE
zeHP=7EQCiD5!V4OU;zuU#IDcP{*I`&=NeH|YTNgG09plmwWb{U)F9_K$T$W|Qt@)?
zRd&W{%8uWFqz2kW&LXYYBrkV`AUX-(TuM3PIRugbJ%T`|E{2%eh-+9Ew-pCab$qcQ
zY2(Q|58S))^-BA1i0UdCI-c>}pGOssHf%Jb#}R4FsNIz6DZ;c+rFxy61<qryu3D&h
zhK$z4k6M2KLMpIz;B`soBAISbXA9}q)X46ZXOU)c3Qt_H&{6FL{U(SDXUE}=F94o1
zW2(h31B_0H<FvYVuSvY45v0~HL&9(R6Tb)aZIWPJ>RR@=`vG?U)v#fwRX#=X#>nf5
zAM_$xinCbP%m<!2Y|))o9GTVT97D0iNKotdA<@fVxt^})KvJ7FOn5T|e+;l7hT7`^
zP29$qzApiQ1Azdi1L!ium+nx=(p7=9rJmc~ZqU+cylw#7X%ro)8^6VN8cr+D9n_<H
z4EK9p{_jV64KNk;@_!w2)J7KX<*ZWemy!Q+lmEw=|8o)3W5!hY+do0@Gk`^VSl(>}
zb6(+C<13I?7_lLz6=x{wo$aS;Rd4b#(gT^Ok5=YoWCSF&k+t-9DEN7TvX(Aj(W}VU
zD&(uEf@!};l6w$WR7voCf?opcv2e~jM}15%@)}gV%cRRHei$iM%UzexeAr#pu*=Z=
z)0gNxOFa}Cz}7>LBh|wmuO51Sj_mEo)Pj%)`Ka~W=qhfBUi2VTSJjK#*ar^u&cz@2
z`ovef`@~ng`wl>}^rlIDClj)BqrRN*RA2G#cP^qf^uG+hDS`i(74HFd#haa-UfwRb
z5gANa-rjjGm$&b31et0((82o$n&s`zQX2WmX@R+%K7|nADtGA3ZixS1`S&HSo(&P=
z-<NzAgFgShD+y|M5Ap8{=iGuA?e3AXkIn<)?ve7-G(>B6kCb08VNl#XQhsAtyL+Vk
z&aig(NZHS@cK1j*z_51rNZ~{Xww|)9F%+l-(C!{7t^nHIBPA$+cK1k05<t6qq=W>}
z?j9*&Rf!yGcaM}*brJyW?vauvx0`BrkCd1I+TA0imRf)uXm^j4Oob{i?j9+1)EEHT
z-6JInFC?+<9x3(J3IN*OBc+*I0W$6Gk<wnR1EAeKQaT8r-91ur1kmmtDIEpS?j9+*
z9OumqZYiB5Z`$1>B~P7&bd0-4igEWy*}y`IyGP1K2DQ6K$_*^8cK1lx#Ok5DM@Phk
z!mPsxtCrbLI}ZWIvNk35G8;>GkE;PhNgJgMara1Fv#p6;<xaiic@P}J-6M7LM?_R}
z_b^wX-5LU79E^8m?4=-t#;(P-kBXg%0n?5BgyB07`xkhFvFp$*lVbnG?pi3eJ^;Im
ziGx+FeJxl=><+}F#-0OxBzALS6eQLgM|7gGC$N`TBlbRI){F%}6N{aXc6UsyHd<D#
zn1g<~cB~OnPmc`%HY1jX{l(1Khmcbzb_$NLWX0}Bk5D)EDP-1*)kgT(m}-JfG`71T
z`t(=_<gH<hPg2LlzDEADW6wf<qu3~f8^<z`w<fWFBPUH`GC_-tL~)zP>fyIVtS`#a
zG8PPD#QkfP`>$2*|J$qFN#D*&HLKjX3XT1Mamh&9B#?2*NZKrramm0{XzV@ADmSh|
zW4B^lGI$kQjbyY;exCuAUZYNeOGYz9<=E&9N-#LMWb{J(Xb;DHWOzS@3N9JbL2-sh
zMG0{#2^mtXa$f?nYdzAJBr&dX^D4AE5dW~nr8D!6xMY~C&~}1Yy$<{bmL@~^WtH1p
zg~pCi{RDhH^aE1P3`S+J-}U`)VAmOwRc><?nlN_)<Cvpm<|=oRcF7Rtd0@QkXqkOi
zp$YTtV7%8dJF?1cu0j*^%Rs#zM{|`sNxNjQM$|un`kv@r)Tyj;o2$?Sotgr}z}KL;
z%AKTLG6dZUs19+o?<zFbrYZrdERObFg~nP^(}9}fp=FiZUWIlhqAwzfHm6lCtK9Y~
zv>SlCJpu2(3hgQ2UP-`nl{?9}WPFV9w;o=MS5~>rRcNBQ5Sr34`06}}UCAo9xeAT8
zq1pm3rlMUk%vES)cYArP1btQ{M&+53ATUUB2-d6Ik^CMQ$z+u~a^I({7QM<Hxu1Yp
z<&Hea3>lY<$U_8-OGe~j0>&jH@(2Ork`Z~7fN{x)3{YPnhu<=nUo#O|<&F$ezau8h
z!I-fb<S?i*m4)ag9%`>}ag{qVTy;Wp4~x=`P+T%1;*#+bNY!Ydr&y8<;g(hIh`3~k
z%9jFtmPaCK%PMz7Trz}YJ<yv;@*A_~HbwSil{+FX8A7rR=ob@6xXK+7mkdf(9|QfZ
zN1_wrDtAO&GK3@vPj4}NbwZkitK1QB$q<s3K=aNKA<?Vc5pl^7Y=2;e#IaoEj)+T!
zWO6DnClec2v#fGQ#3e&W&H?(81QM=tN5myVNNxf8k!mEm#_DsGJ0dO_chXK@2h9hh
z6RqjOaFshEE*TW%?gRQ)D@w0&N5myVu(e^54e{04mCBP<?ufW#P@pR=A949kbRI0J
zVd_A~DN%WW=h1cWdmMipbsYP^u*xP{&4b{xDLjno6yr&+fl$V=tlio|(lwy20L7+*
zsSH6;Ks^W~Um=w!hPe~9WHh2w`Dqas-7lcN2UiC2b$CvZ(T+@wI<-MzM<stT0E<?L
zP;M;P2mC4z0=iR3p^*Jg3y@VLA2SGRteVUbvI!tLAx^fJWJ{AD=<hgEqi#$I*;ydj
zV#zW*wPr96x-cw)uI<Jt)S|FGNS5kqL?!&t5<P;-P}DsY-()4`+@V^blKP>>N;Ulk
z=yGOSWs_1lAj!Ztof-Gl#}?9Pk|scRI+(<1#A2&s)l4YK7MrQ6ksuyVhC3*v5sR&|
z)fM=&#TK&DLBi#0A^W3Zf7A*>gzXNpRL?Dn{aRnFHK<<{i#!P>PAK*ZK<i@Hk~g`Y
z?Xs3JE1FJqt`R8BZ8igMygkRI)x_LpGf!~TxdLPXz?$880%W_*7EhUtONCt4N=rAJ
zNvjg9vQEG^eLf<6dRzj~sZA0$%~d?kyW(IfLr~LId=c402NP&35W`HgYY|=BRWE>x
zz6*&Nh_{HiT{kc8hy1j2MO!L>^`_eRCU80}YQjL=uI^1RYgoQZ)v6p2=2>FXK>E97
zkY^f5K*+{`WCF=nu>6f$Q_@s-4wQW|Du?Yz$mkq;o+Z-;-e@&j$tmvq0XX%}{>+Uf
z2`xRq=$2<h^6Ex+joTfWGa^GmMqjV@EtwJ7pe<-Gc5f;7Xp@EZchE)g$S5M?I(8bR
z!P?wg+)IafCL0z8KKqtbXOQN~9Arp8(`XD?KXjiP;<2U(EB!oHADQ*Ec3=$!7D1V&
zp`;4yO<;SaI;$iY?;pJMO@VJ`f!hX_m3opQBI!Fwp$mtvz<(e~@-#f~b39LJBB=mU
z=6RAt3Gt5Wh%z%+t4<@yV%OkJgS{+9MQ{~_{{6oa>@+H3cYS7XEzHa)u!hL|4D$9;
zCL3o4bLV7!0NuEk1pC9x*rZenVr$|nG)|)@R{?!<@KI2)USWkbMN|$*@<=9z-ICiM
z?)|}?aYKm-*$7Kw=X~t319w20nmY~2%S)P4<t0r~=BHKrcIb1|GIrJ{VF{&aOTebX
z`j9iKkfPecZZ-+k10Z{nNwCpn+=n0MSV62(^!wOlQBLc0W`4DzT=flpezqv-qntKu
z<7&;@XiRM4LRK4V#C7o%;Z9qQf$IF-__Zxus0u;RlT?o|^|ox`>f&P99gD*TE>x31
zG1F3M<7~?Yt}feva@h7Qr00Wdoh5bJvVE(oo{9?$3DsSoc+^rEfo$ID`m)wu+Q~xt
z9?1S#jnvSyb*r0e<JVY8Sm+~|M>WG&N+hIuP-1~}<8I5wt!{6LtQpP};p_*lNk`^n
zJ5u-m2U@gETR2yOYfE)bN$#*syDi&|+P)iwHG(1{=y?eF{!a;#9KF;TwPysSiJ)UJ
zRUC+~*214}*gAvnoD8Eia-$-524t@PujD#y*{;<W<C=LQYlz6lAnrMelvY5!Ic?c)
z)!wnFA=_+)^b3%EYg2dHve~MA7sUm}ger=bmxZq`J0ISh`d84fe>(;G7HiD`I?s|g
z?T)3D9@tN_7=+kzVwfAJ{V!~_uIj>ipW;P34bw|FQTdz8RvyM_`h<x}4WRuKm8Jo9
z7j~l3Odw2DW@4h!JYXg&GcZwUA%K~vv=qQjR9XehM1@JK?~!f@3!&+gkxk!3<wWMX
zHc8y=K~)D7Ee@tK1WyN50gzJ;CeY1>Sw3h2E~1|cs`=o$;)tBWE)($P8R%HuJwZin
zsoTKy*uk7;qVjF<O^omC#f11B5PoQh%|zvw;3Grh-I$O$c+3mon?9T6*Atbjqy<oe
z?Z<?y1xRu&neImPM5S3$+TeIcCbT0!H_p;Jjbx(IHR%GRY($2H>@<*^ZOM#CnW*$h
zx_XF5n=G`OK=(j3TEid{mBC3{hkH#UEDXCq{C#zXLrzrAO8N@<G7U9FSZiSwzAe6@
z5nZ2&tTHKhJn5(SsFy0NrC?iJo%P7QnVtoFlJq9@ZM!p(^fyS^{--2Ko>D?%U<0;4
z6G<OIN?j};>f-$Q{!C}_mC(jAPjFNOcZJY7|CL}fQOQc~7?)W?WL^w;S5YP#r+=c-
zGI<!xOeQM#0RNDsF%uOWH69S}@kG=+Ao-AFV%U13GAMatU#}D~A^Xje*og{bWhU0}
zM?9w%sMpj4tY+|g9DndQ&!@0#l})tbm4qYlO+O16_Vp&`vo+Ngk~fr^28u-oQyGG{
zm0APjEe8{5>yz$4e)J4m9?G9rZBZ0EBdG;m^%8z|Qph?cE>EZKt7<>ml6g8kM#z2u
z$$#Qx@=*SXx-xEowS=rL3R{G)Nc*Fw(|=PJ!Zw5~)s=y#)9H|&qdJQ!S2jMV)1Lyg
zzti6)>+-hK-!4#^TWc1IqMj58kEikw`jnzhvfON@pk6`ZA2S12AktSG>#301LQ<gA
zzd;ee0$2i-A;6X4=|C17OmN8BXuTd?kDDVUQ>9><a4@5uWx9Fo(bcejrX8uJpj&Ba
z^(@mZ8U<mtPKD}vP~1$a8(0LHWxC~Gq1o8pS*V@?#STlQdq$aMx}#Ts)Xp-6bT7z$
zvZQ*J>7KX>ZOit}LY0{f1HxC5H3Fq?p7Uxyk2G0Gdx30NHBv(_UGw~wrCx&z3;ije
zy`VaMLiZ}OOt-2AYTI_q!g(vWo;fn7bb>dwj*kYZ!uc7vYG4C4UL5ILd<#s{P1BT2
z=H=<^?%{pCEJQ?5dk7ixrvyokw$&Nv2}%<|r$We`o}mA>S3Xq@jSoRl5xfIJ`5Gx*
z)ITHG^vZXt_v59mAu^jnUI)r#v(mk?em;Cy&4fu)Z7}PaLR${H(KdZYce_uj)c7D9
z6S4)C#CE&vJfs9scCRvJ?ywQI78uK(?{<(&&FJdsPz$rlz*<(?Q^YXfbhsBoK%gD1
z{UaNxt68IWV?r9}PV}$U&`yVc^#JBtQpG@{rRDIPa$4?bi1ZHNbmS4`v_grq$8b9C
zQ-~{)ID14nmq(P-dOvawPUCdq5#^*V+;-{AnNaj;yk0>X+Zv`7f6iesoz9$?M+YuM
zhG_bC989A1)rE(DlJpBiWUErY3_BF&)=FNOJEM6L4oTS6FAxcZnMf$i#p=8;cV6-&
z9FlM-%srP-m{01VFrS-4;d=PZ&kIk27I1D_eyOHVnB@)0V`Io*S#1TB&yKth$kF)k
zDz6>sC$c!O=7l-s51C)C6y$}Y5~F`<1$kkP5D+fEw3fW^SrX=NUidr-^EWSiv4r{C
zjnDjnSCP_3_;%l%Z2~+)e7%coMcJ<wU5_NOI2`yHK<#FRPy&{)djYhN1(Xxzuo>m*
ze2*8=%z`=V5E&Q>RL}7kZ8Y4o6DC~29IC75qcFf=P7bJ?Ze09R6F|Gtrouq+3@!ss
z13|CeEy3u{768d`Ii(vX;_4F6-DjB?AX8S7k11OKFmY<r=TekD@HXiBE-|g8?@}i)
zj`U6+Hk`oM1}P23>BA-ysMTCEv}1jep}hn~4jtkIIxy(;VJixBG<4nL=psRP<06&n
z3G@_vMd1we8wu554T?0q`XlFO1!prSPLyj;-8f-X3qf~<Wnw@xWhHw|^^y$}=Yf6L
zUITZ6jwO_K>U85{YT$9iJ!b_m;6%9^(~S?Pfp-CaYLPSwryCzn13w_{x9ag+N(!W+
zb*AI%*^bkVvxGon#7(#H5~yY>vw<qc{Y*tNFDd3hXdRl}7wXZ$#f~bLiHZ|t7b-SY
z#9arP;+YU|ot1yEDH5eQx^W5_xEt_KmRthWvhyoY%+A|hc6P<Hv)jwgJ0?4vsY!MQ
zh}%LFba4IXw&329Oatmiw*?b+O16XeolaQy9ELs*9~3}7YJGq3mEk*(4N#48N<PQ`
z?foz-aa<umD>kI3fb1HQvI(kDgDCVLhAL*MGL%RPEDB!_zkg1E=9!b3SkE$o9)XaZ
z_zu~etpjRQDeSGB`G$W&lryHmoE2>_tWRoA9BSSNwn<pC@UfQdMS~p*ei?6iy#`>W
z0j>v7&IY#42+FPnrA#V@fMI(O{WS2-a1#7v2qp`GAkW+(sSX~N?KrEJYQ4p;MqdFL
zS#2C=ge^n%PCQRE!~Dga&vHCLa0%vc?tGH{{dIuSz~-~5Y<vSRo0xy{sldt@JiKfj
z5@sP5l6l^n$T!=*O?MBob!v;&$ae@0;LEoV_#OD1w-NXefh7n?LC!Hb%|;s!<Vr!l
z2hPabYp~_Ox~H9P<=wf<$a{|g%1hquIHRZ-=QLgNAZ6fVWR3zJ(*@>nouLRZ1|N6z
zV~{(B`Sgywj$vlQ8o#%vq~N{-a5x3*FalOFQ)`aExJ?*O2cu-|!$W871q#L|$|=;~
z3$mfb(~Z^ZKbTQ>R7q)DU+pb^7cbubbSN@VH4VIK<jBxq0<1gro~^qo*4>2wOHVj?
zHn67d?n1^{cbAei>3yRYn=14mANTU_A?ygn(|VzJsmZfUuDSegwEhBQ8>M||u-V9w
zd*u<NYNb8)fl=Cj4B!99-g}2<QFMRfJA2>h39L#0BPH}8E%X`^Ak-ivG^r9)Ac!D>
zBB(S00Tl%+Y95IdtPd#IMeGd)yJEv$QPfAVA-~V(%-Ov+H&OY1-}m>&dtJ|7*PYCn
zQ|HW?va>V0arK${zBRIlbL5J1t)uo~V!#W>o8YTg+J^jfALy-%9;J(V?(w5tz>`13
zIue54-A<3(`myi7?eyQnpTK|kz`}OUpv>qo^9C0xVdKw0*qVc@5Z3z~Qsx7on{&Sa
z*ZhDK-;Eo6*<esU#%ZCu)*HgNP_Yu8pvq&vw7q};)xHKWT`|vm3qV?);BNgMpj4)S
zi+=(*4LSw-<7Yy%dntI%?`TCFeQ!|zIOx*cXSQHQThavCOaDZo4ib@rT7^DgF$g0C
z(in-uGiHkUOvR)!2bWvVeC*4*9AqLZ7Z19facm|<3v8_{rq-6GOKVWDk$n#e=?uE!
z>oU?pmLMT5WR$YRRrcrn42?6ELQ9n*u%XDz@&zw-Rq*FbuvaLGfKm8XFSN#B+9Ma<
zEc~3$_avyqp!BHnjX**Sx?0)d47ve$72If1oZ!)>VH+*!&;%8H6%#b*10+On8)b_V
z+$xdMq?;;R94GjNvwgX{K_vz^Oj5b0AR%(kP_8(+_XDq8>Q_p!oz?RCTafuTjnOvo
z$;m4Hr>dpD0K8g#P$`1?`A_v+^~Tf`wff&o&>Q=au+`J#JX=_Q@OGP;GM{f&s`8aH
zLB3~^kkWQkwz$%M4ZI5eb*U8`)Vnc{`GSX~so;f7Q1ER?h+ynkg9Oa6<!{iR{Q|re
zG<k)k2rS-tH2NC}@SD?B?#oP2?g1o3Zf)g?lbfAEVG^h-lp-*=lbP)cUI8+x_j{P2
z;17@x!IxDnII9{3k7Uc)x5^qE2>#%2>fLt2H-lTG|Dn8bX73T{Xl7hrsJPI|PfxHu
zGv(jI?u8vfo$&PP#u+c;)LDkT@Hrk}nR8gU<q^i2z;1Y+%pQ9j3AD^X<{5Ux2LVq&
zM?BB4C!Ti{A$G8qdK2LB(ivOo(`o>Zo^hVcKU-uP=LG4FEj9Y%Ax=%q&Cmm%E!s{5
z#Y9ZREc7`c^uZRYjyHHslup>f*|h;nFKpqoqm468dRleDVu*InHI3V&b+M}rCKl31
z(li+|Z&31GAdlGomN@M|+h1fo>fmV_-dKGmgh_m@hc*d=NS21dR3t=TcV#I6i;lrR
z0;jQ6qF88?3$G7&$rn297!|si3C7m1NJ#CEQMS0+?_VbnJZ-hLG-wQqO*2%ySo<i*
z#M-Z!px|0{Rq%Xeixa#6c$}zX(5{FVyv&HuA@x+~l}u3R`$&kjS5+;vWqoU65tppC
zudzU1b%ZJZ7tDPVKbfcV*2|16mC*aQgr1o+$>-^<JdB*p5;<>+=pwGFXRoz_5ub9I
z@}D6zv!gZwCcJFpoY+d~PV!F;XK*FGh#PcD6*VOg(G^@{@Dtm3Z*N8pS&Ry1w9b*c
z`hk!Mc5blQqwL>Cd<*7*wN2+r?4>gAzfE|}2-^g2bJ-*pLy~QCx4CR{=QltrlE}YJ
zHes!6Xq+M2sT{?>!{xp6;6}FLSm;ASXgC&X*8~lN73rJBhq~@2w^t{Pj{86$(Q%Lo
zR%HLC8q1$)Nh)|NZSM@;0#0N3@=L9!gYN7;_Zna5gUwW^o1;PpAtBLGp=@y>w+VO^
zT>o+_IMMpgai4YHh2X#GEbSn<q&fIbW?8RN3A<R<ms|<hBac;+<}$%@?nXjPx?RcR
zOd8$7+EK(^P<yVhfU)<unBfjVjh@k^Qmu}c`+*OnR*!3mdX%pA1F6^Z2}?kKAocnR
z;1i?=`#|c|3>)GcZEN*IsnwHO*;=*Gn$~_jsv*T7c0YBGf(8HR8Pi5<^iyT70JGNU
zrNC)h-+rYnqu+Miz3$(Lwj<PvTy^4ZWsY;=Uf_8ZaX*-|&H{Ubrg0g}|Dc;TPPVLV
zZ8@l51WJrd2E%FXe6J-&*c$q^Cz@D}K#vN<5y(<M-VxL?0woD`X=jc(j+-RA0?g<M
zGX5lqWPE|t8N&A|vK^&F*p43TYCUeD%evWiWTDmFb%1fT48@3hH!ZTfYn1~GHOqM5
zlUIN|Vh0%Fbbwi)!gf(u?Z_JtCc%1b4-M9XOfXna%2R=>l*KzHsKR%F)3&o-vB3aS
zH_`^~6`>FJRH4y)6*?FRX#pFQEw1ve0bT{qyxLY>Fu+ugtn>vRS)hW)GC^x^KtcpB
zRJJ(5$-O8|2AGSK0)4k!?oko3_HB?zzm(8hg|<aPgs!hz=&8W-D&qb@r`!SugHZbu
z<_^fK)LtdfT1q(V2eWOSEhThb>VwchptY1hGbMJSV`=*r`eD{eXg}?1CpxVp%&zZ8
zh(Xy(B;PaufmV##VxYB=K<hoo2AYMADYAiPq0GU4plJzqfz@pr`L=tNfkxdn(3CtI
ztF93nXvFz}rr2U&-JX&B9vTykj#DqMV1iZmH4-9lm9ng%K->GDJcI&emb_lEL7?@C
ztn-By4ppHSF+riPAz=ee+2Z1%VR0b1>|eIxf<WsS`Q8_N4ag)WK4OA`k1bKb^OP-4
z@EYJX(9TziAkeznK$|p7g|23TLLWdvti8Hwq5lD%R}r_DdKDJv=1Cp~hN%d$^_Zl{
zLLd9IMZ=9#Dnr4rh%683fN!DgBecgGF5&X&C_fN~OK`0@-u8IIB@jD}A;jRa)Sbrq
zp=hbcjc4!?%O#K*6Z|>1rN*4QRMtR7MEpgNU8r3K<|qlwf)i|DT4?M<8<-Xvav~tC
zVOgloWI!@}S$ObN4cjBtAEofKJ2q0$6OqOVc!x50munrnkdOfLDJwu3u@Org0*n*+
z{wgQ9k9{t()0Z`Wn#yW>3g~5evsuY^QSF<N#`zlH4X1;YYl-JG!P@-_38~$Ol_f4@
z&N)?V;5_2#cpDO;<9)#sW~kuDn4sV$Gr=g0qvCpsz+or(3xi&aZ1wr>1(l6qCdgOq
zG&SfBWs5UtJn$;G`wdob&^TU>R4dSurp!{o9hsouNk~XZi<B)+@G{`lecP47gJd@|
zdXg{nYhP&l(^cp>NQlr~RSW$Pc(wMOo2<2g8%~Hk><hkowp#le6STJX92NYzvc*}u
z26(mhxSOru0$N)ZIii=kAnyznx||6LeGv(<cC@m@2|eLVN|H)DQ7M9eyD~D+7hEt`
z1+Qd+f_EWdBk$r36oH95jkzUqozFArEah3v1bH4nLX5diS>lXInny`u%yqX}p^zz;
zdtzk2FL(>cB!c!ZLBZ+sRq$QP7ALq3@M`Umw_CxRXzgc_!RV)CxV;i2BJ_PGD75L>
zDzu@p#0k9!c%5(WxWfwFL!oEcZgs*rD)43|C~z+lQv7$7JWk;7bFGx3S?E?vH(8)}
zf^~o?-v(0$_pLO3+fKDxG~B6LsPO{rRJ%tfL%`Pn?daZJo>pGD&~~cbqnt)|TjY1D
zmb&v|zf-l;e*)$-vU`-Bs=HR#F)T6HF}(V=6U~$7pJy$xov4LgC4}2C3+*}2cA^&g
z7Hea+6SeRi%d`_cQA@uDg5CV+SAD7+`)QZ-q(1o`kVkA!N}R^hrz$KMss~5=6l%5S
zEZ16^#RO~Vek7!p_9|OkEe$+B5d7?BTStN5KGAYtaElcx_;@BLcqtMh_%&sV6TA(0
zwfKFd2%^7#^e12Nz?CZaOeQG!dL%^fw^a*%5_p{h)V;@A9P~)zqZ0isu)HSGe>@Xn
zEfONMxw6Dr`!evninvpD`&J7~kt{3^luz%Aolp45ioq5bi4_Ac+eS$<d8EJa0`e4b
zr)$Bzmc4B-SG^6aS^X=q+ZBT%;WZ;lWznuEK_b7zE|e4{@D9mLIzd(oiW2l$Q!&)?
z;rww4yvLc0B@3G}BV^zlsLD5i%pIOEupeI0$%h0}IA~6xD<s5@Q!=LkXM<DtEL1CN
zrDE`$RW5g2!lj1_5ntY*q%mKH1uePXx&g|qpfeNRJXDZavs_7Y!`8^H|Hq0c1;(A1
z5LXnj;-HcoVg;&$`-s;(VC{%4;B-Y*I0GwJhurEf)9SD{`pcv`wxVKfU$aaq<e>}E
zyU0vwnN-P(7vX-O0_Jhrb-q-}lGV1yuu#p5ZI5B0sh0qnGy4pm_8Y)balV;eLzMJ_
zgd+!NlWx6MV|gwUtiWwZh}*8zk}U>{PU(kU%EHQ+Q~01QchER4O_=TrZF`vtozDb?
zK8A$2vsl?;?v%$?%`OiF?|jG#PPFxkvqlMbLh#?LE!YK}UnoLqwg1Cb!X}pW_5^9O
ztFBO!_A)_}O0HCs#wmH6Njrh(Rm3gdYaX#c>;|zuYIs^vrCOF7#7Cr(pS;Si)<>jT
zuU%(v5Fbg9r;>VuXlZL)?GGT^g!b-h5CRxLwn+fYyp|B>@Y_W4>Hk7N>_N#G5Zfgn
zs;{>JVWBSwVF<C%M-@6qT&g7~Z3L%0o=|;Im4gIzOXd6In?N41g9LFpFUZ(t-LZ$l
zb|#cUn0R^L_3GsjnC^*}??*xe9<40dje}@vdn2VtG__G|FgU!H@PIG$2+Z)LyB)&>
ztL}OvB$_%aTU^zB2fPaYMJa;8;pv1UQEOuDRLtQ-@V}X$;QdI5;N(Yb0pkQuxP{rW
z5>Znra0elm`@9Xb)*DplSSBd+d?Z9@^QwhD20X7K9uTN_%mT4Ruaz+Owd3>#aSP-P
z-pC0E7QH@}fH(lq_8=ci_`G&2(Zn*;j>8<!-U3?cV$7!OqSq%v{q3E0w(^M`0Cdso
z6Onw|T?n;O2w|vwCZRR}!#vlqE%YKGgqnqpxm!c+1}(!Lu)1F)oISY8P@`sv44*s~
zWsBHQBThrD?&H=WrNFvBCA<J(Qh6L{rSeuYK|g<wga~Y|EEN=J*CI~YLMakz`HBrf
z?YD%+aIXj*grQl4UdjZ8?nXi?ufMXzMMRDaQX;t86V}oo)V@hr;0vw*nF#)Z2@1}=
zPX*Ugwm8980<WRgQYnH^+h;><DuyNzdKnWG`X~}&ZGP24e*>OZ5l@g*JY|6gg0+Yr
zVeaw6L!q`DLx3HvQW80N_!oL_`}o1akE839P)tpn46)y%kFlYcn#kG3<7j*g#neQG
zV%c^-6fJdEc+`fXrCtS_+3%$$axQZ1lm2Rir4Cjj(h~jE2u>>)ni+|ltDO3@4NVJO
z^o$Kn3oY3R2&)kmI_nicGDuqZ_SXQ*G|0kNz3vCHdT16V8SW8@J+XgBRwurC6aMDv
z#QAUeF|3rEkQuQtOdLXvs}s8w8{90Oo;bso)%$Ig^&1j)tnP}E@Y+MQixM9vDJNN<
z?FK0)TCLwfg7rNY390WwWr+)-?||2^AEFe24eJtr^9A4et_nWlJr#T=61KVIz>&Oy
z=5l7@@M1M6@qOiM#{>;J0SPgvgR;dLbO-P%_#veTn#=seJAA=oK2X8SnV{g!NQmGU
zsuuhj@G5xFv)1BZW|EnhP@)!h-=l(;GC{#FAR&TBDO;SyM}J6ZGKYFfDYEgr(XF3&
zGKBu)tmJ91i}+nti_iZkuzR;s1U1w>vD~-&0LY~3Cw;7TKZS&?p_iVIEAPg{pM1Vi
zpD5pXOt8FnAt46sR<^kE{sFuS?)74v;K7MQhN&ZO{Zs|N&IARU&s6Yn$`&VhDDW!y
zdNfB~LDe6fxWO0v2gszPIiIWG5lD!|o0Tn2@L9mC;MOn4S-c|AL4PK#=Y7g!>q-4W
z1$RP11b0)mIKj(+*Dkovt8s#J5=Cg+FIDJ#Cg_I8kPx9ml_gGS^{=d?A|Ab)^@as{
zCD0wb6`Tumj~G>HT4_7r7o`J!7~J-5&cb}ufSJHc(*4f*+V6f}lJ2+lH@5qINxI)h
z1>!`~Qup8McfXeUcfjm^Uy|;3(zky1YpJ<#QHr={f4fWi+l=qfYho&3p_>U|Dqx|x
z`v9T8wa~2Z{r=U$$NT_T`d<ql04&=vW~)2)uz|gjc+ZF`Ck`}J#(AGS7dMj;+vyUg
zvG9`$3p%b364OU&EX@B=tL-r+SZyi)(P}%OY;o0gC-B-0>_P9(s}yS8eTkENp__kF
zp_xCc(D_J6EbLR3IH5lPuhy<(?~g06_Km~`eZimZSHVa8qJr~~5WzPoTb$r|z-#2*
zr4&J1+ifE^@~aAM!vtM04ha#uO<CfEE(D%e5nmFtOGXyFjzO{*xw(kiq4>#z3?@DW
z{fTcYU2NwBUkoH0#tk+)EH1G!v4$s?DL)IG58+qB4deBGGtQ{(NceQnZ0VNX9Hw!m
zUZ#RxTY;ya{w@vKY~G|lIP^h0ZksH7`TJn+dpy)|v-$B*GnO0EeL8=pg<be<!bGws
zy_yQE?hDH_V(c17!GVB=imqfy5fA@mW0>WIhA)>XKN-Ro;U_-hHkKD5qpY3MeU7p?
z-Fe>7Sq-9F@slG6haNQ`%I?-5KyhE<mZ4aMpqz4<@|S_hAB`eY*wMeMXJ;#&Um7>}
zAkb2vd7!cGzeI^@sW3OUF$CG2_;a$?J7uetfG#b&MS<UFzZi5S>R`L(@^LFKQ+^_3
zT<sgQ70RYPr!;p#%u--blYH0x)K@gWvQPPHo_&e|gIuQke()VNMt#bICpG>+?ee@w
zS59hhrORJ5-ahWvRR05Tr66+3T&1Ar%W8*#)*-;@&esV!4WJ|szu<&Rk4on)h|)Tt
zIZF$nM1`EL2AJ~6!0g4Zgu))Ap^Wx*N*B~gEBA5|u}K_#0EP<HgGv=tN-Ot4P-(5?
zqE;?Y3KnPD{R+_IAAnAMTlTW%+uEY3C*U?<pXSDD%NvWwZ1{<(qYj|9epQ;@;Asnt
zUt6wB@^IG*wLx1f;`<4*6%(tiDd2kyKWPCx@v{|@rM9LiZBSc3N3W)O)|UB`wZ&N{
zFR85oVSv+6ALDfAI~AOr;QbmusVzBURH?Rz(%Paqk0D^!sE}A~9W@U3CHR$4*tkEn
zww_hGptklzdy|N@^&p6BZGELwL2d1cmVqi%TMsIQ)YcaY(BrE?=hv2zd9LHw+M=nm
zaYvl3xv|>X2A-d21x?)#E2cG9ntjOlASQ8YJI$|LTRaC|tQbrM<TB+o#-op)ptZ%_
zaib1m5+SS0AIlce8vx6U<P+Iy_pN}A`-yA;?SaEQb}hnUR}#jI;}hBP`5GX*<<nyE
z<YdSi46zfZarHsOrK4+|cnX&v1e{KAI@ccre4n5!om$v+0fYB*!5&WttQLBK5I8MV
zD?-ZI*obf8A%uJ3#&xe)b~?aQ3ienp^s>Hij)fa0mO1Wz*>LzWNSrcz6M{He={VC{
z$&G`JPSjp>?fr`q!CO=vH^#+^dt8N?@|h5KpYK_21NaXl4%sE;ciau~sHh-ni(sJ#
z0ZpFkxEtj;lPNA+p+c!0diW-UT)Dfzv&2Bp8_-ez1g*g>(DOaS3~_MUh_31(r|a?(
ztR4$p3y6C1lH?IVaoJ=Qs(N_-*Wt|wE{r>JnR0&f<1Swh4`Y1`F++GVY9eGkB!2Cd
z=sM%-!iJ8@J_~5~VY425C4G=T0R$yokXXw^D<sHLHc-hMv!G?|6jF0|a=uivB0=E3
z2$83kYO!g~RtOqW0ez=_gr@H1^Yq=ZNiIIN-VE>KRsKD+XVC14FlRinR{~#Bg2e4i
z^!F2aNPNVchDanNyUrjUBD_LnBw3F*rk5$<LBbpmjO1u^2*Khh?Amb6$UYX`ODSwi
zMB8@z(C?*SE2giE+@B$YK;ss^Oxx(H+kh8Wz0U+)MULsUm71<%7cv*(rHBm`v6y1|
z)_^i8qDcWai`az;7Lkd8ETS191w0L~GinDc?W7`-Ad{EduWKp9@O0(PBr5w;T_DZD
zSJ+D}NzO$Z@f{t`bc*<@ip45CBdEeP5dQp$p}yxmC>gQ~_Ro|meA)2G;;>Z2)QvEg
z_qn`0EL*wnn)4>Xw6BrB2?Qm)!T6jBzTQg`JCLZChMdhv^g?1RE72E{B%<Jh-KHA_
zu_8|fkyPX%M1Dn{%vjp0b`-!4u#1;hQ4zhooe6qb67=$Z<V@*+gg84Z9e#9X%mVKg
z6n&<|!|T#`@_>@Y(*QT%yee4GH_XShj}fr~1QHPsF~Nu+%k(Zv9*zicSdk)jQO=v@
zROl0jy-#*JEHeWMI!qFD*kI%gq{B>!l+_pEwTzWj*jZNLVp%1@vXWiOI*=^~-&S|V
zGOmWIAxTm!81t676vhl>U`0;P!MvuPQb=I+Rv?I>E(oma)$8F*r--k&s8I6QQ28Z<
zA9GTuZ&(VuMr5>t<|&h5!@~fl-mPpW;fcpd^3iVGoEA06#+{I2YOA@Xyc>j_6Bd|U
z4FY8k_Y4Z`MNs4mxEkQpFAk9pcNFohj*hIG#O@4D`8AOJu`i!v#nX^5UIg0rbshvf
zq!^Vj^MMqUVwLO=E~*YSHC0%pVmL*7DO2ttdfby?0)8?#<5;m1BCMWn@_-=OgPz5K
zo;-O(FanuV`&F%nZ*?kFRKvhuc!Q|3Gpy$=h!`*a>LYrNIKtO!;fa7J#mli}?JG8{
z1Hshu4zU3)b&8(KMi3}!XJEs(&_8raC<y8v>Dn`fH?qn`0kY?7Z{))EeS&n{8@aCi
zod8@|5z@S)Kq}jcg>)`Jo-(|Vz4%=M1f+ie9I^@iJ8dFYac=GoP43!)Ss6p{{}u}k
zm^Eb}pbZu(o^{G#KpQPIde+9ifNr%=>8wUrKXq@jP+9Q<sIix{p!=Z2-S`2DQnCZt
z18TU==}#bGoavKg`3%QXZO5Dv*-$SkZaK+}NQvAW0446)0VkPZu<s-@+Fe_G5<c{f
zIn7D#S;_9&(fH@#^F<~2U)x<<ihtRWwVQ~|K{nC3?%FcQEvnW4y}RmsK@`D^@|p1c
z&Z!~)Plt(e#&a`_c5VV>{lA(1Psk+FaW~Wdc>>U>LV6)U>wh7Al34hEGyVUE00HTs
zn(%)v8oKrWJ=SyT|1B0$|8KRB`u|=FssHb@koy0A3t9i)r3IbE-sIkiD4_WNN+|aI
z-%C~*L;jDsJl_BCdEkH4#sAL#d<RRZ>Wp=Q-@>#8s2^$ToFF{_6UQ&jgV~Q6zVGh`
zz*A={AKy>{TkLnqiul&gHHzsL)Mx>uJ?~3oFV(Y_FL7&>5-$KeWD^`Sksobx?~{4#
z8_1k_&rqnnKl&kv(rdfU><SfrCB*5C(6lWfQ?_XSTpHz`i~j!!#qwUtpZ7RLeCyYq
zLk#^A+;gUdy19|iX^TE6iH2Sec*vf>(7`eh-iu6Y=y6Fjw9(Npbe{^(hdAHRC>Dfy
z(PtAhKW6ArimhxYUyYrsn0FwYmnpv#+#60&L%G6sDs<YrulY%QJeQQk8!YrBXUjVp
zrU^)h&Ey7w82c9M9%Jvm&X(ew49MPnDHeBMUTb?-aIQ#u6BN9Gve3FZfN%q4p{%-q
zu-<E-lL_&QVG8dcEYH_0-mM2%ZqzK?ramV1uVL|;7vF3%H4SUky!f>WbVF|P1sHQp
zE^3#5@b$_~LANv}lDY^~P7z;i>?z+pmK{EO8vc56y84V!crKLLvE^cyJ<IomW$d)@
zlx$RXsg`_=B|#QTZlU8EpyI#ki3nXQM{||~9(yh3CJD0kjKwEfj?2BqJsz4%ZU(Ml
zLlm*L5<I>V*l~ROK_|^w1AXp#cOl3J@~LL>EWGUN%$&rAPT3|c68$p$^~!!R?f=d#
zH@WC_9n%)d2Jqc9e8==2Rd@~!#AV7qhtAcf!psRE=?gPUrXaBpi5c5rRXR{}PDjpt
z$Z;k$*0q3kmfdpg_PLH~gmF2Z<P36`HCIL{#t;P#uX@X3<!}ZpgD}2>a+ZpS6=V}E
z9W?_5;cIdR!;+F~kywtzjD0A`>nzAE$k~P*XVRAVf{^P&pDU{|3KCCp2D!@~2p8o2
z|4~8eBm@=W4B+c?E<uercyPiic8fkAg_t@M1>q}nhQpGQ0Z42`Vn%iwOb3CUGZH!b
zkTZynms3;(CRcK~tD^Fl`nV?G<zwpkNXQctpRyQoBKC<1akwU3ph9pzC6{O~rvz$$
z9f?Co&Y%@#mnfsu>=p$MuVz<7Lk^h&VSMf5E)@}T$OUl5ywl(iz8=VH3X>KfF&l{)
zi&2pMK+m}VId>vQ9P+k`2pm#BNgR^eOdZk*32}%|xhdul;&4qGi)UE80*6rh7)qe_
zOOZH)<P2I-Hc=VHA<GmvJcrax3OR%;`6~utvZ^8weR7%dYvGKC=n$^tzetnTBaxWn
zI%htm#9WAX^787TiSIP-4iJBVpDcbJ`f4Debn%m7f59}ez=#)|Qc=l{!kdK`+Td40
zVfoM{PfUg>T}_{Erzr%{loVJi&sMGjrCkA?r=-<mq^D3G*GUta`&6rxs#c6@4XAXT
z6nB5qd^rq>tw7m<R%WXMc0m@evI-Sr*GXBb8i=j!nv2PfT&Dap2-tDDmZ~?b7)M&4
zZciKW-_2nHDfTNWC{*n3Ac_@x3kx)ki}}AODPRAEx|l#&EOPF#evv&FDO{l<F9lVs
z$kTlpEOJ%F#fO1a#%q}HYy&f>FgqLRlr>X?IHEg0#Ng}({%8FD;=u(`8V&SJ*J89u
zjjCQW6PGEkHyhvm#BU&lnUMp(;lB}1cYeXd?GD88_=y5ZR9b5v50sO^?{mltri%Ez
zkZe37=9Oe!0S${mwdst|Z09^UY(^=(cr9|dT6u!3R_;f(z4x>5jV%DniqOO)>UbOQ
zIGS3hDk>ntotX469xRp6(uOT{wZ|vmbb^=Z7RezIS9=zMNLG6eA@Wyy#+?qbBGuuv
z751TEpK>|!OU}cek08St$LEZE;>|19lAS>^wix^Bt5LSSq~X~#uj=ww<Xr<o8y0iB
ztaqlLsoH*r^BER%lcg2RdmAd`lJ%4~Rav@bx^t2~VzIcM(z6vzn$}C@aJA$&peQSO
zfHUn`&F4BxCpaDXP0w}OHD2JH>oi*IU-+<O2xysOoSH{DWnYANr=_Y16g1xo%03rU
zoH#+L1$&a)G_LQ9_;-kFdVTc<16<0A(oAornG9}b!ggl>gSLGUoU2H*d$~;ct5BVF
zW)QTpPTN_jl9UP`BN@mFkDQsiUMf7E!1xvJ6!CK(i<SRQTUiL)uF$Xw3$1c1#VO}`
zaPo{&RP`CiW#A5w1)2TGwt;Km8(aH<JJ97T@!ke375KaCJwV&=O`PD~2pv*^)!VQF
zuU8{jfe!&C6<FX*yF>HYj$YM#*f8xq&1Zd7N5=o8Vh@CPr`1=}C}@}!bQl%;Oo(fG
z6KtE{RjFcIX=b=$yYu@n&Q;LzIevf9c@b4GbGGAFm#KTf%#(toWHx|f3nWiccotDz
z4%BOh$9h}S4<xF;hnR-8S6=-rv{C#9(f~8E<W~G^2j`dEjl^^$20lW>uz}C=-{^s_
z@ZXq$@8TbxWDVkq{~Q&XqC)Y)-+}Tpb1b5knkQkjU#vv28|hWhx<96QAQhSsR$Ylz
zH_WOVE$Yk|3G5XrD2N(ps}$Jr1ZJ^3TfKo1g?$J7*p8Dh4~vNS{e`!Q5^o<B^R^~W
zJ=t+J2`3-r&H<^s_OYgr_QU;@re@AO+fVC=kiq-a0W5QW)Wvo!$kL#NT?<)1%ts5L
zdC#yCY9Y}y7l~{nCNj|iiET`DLSjD?c%bLBX^XqN3N*zzg8^L(XwE1kzGtEoiJW$5
zbDUGZDqC}K+|sYB25v8E<l)qbAd-3Y1|($1j8C1c)H=5&UKh1%U=I(kpc8KHPNh6{
z!tar6S}WDarnD6(w8<_%nqiGha(3h7?56D+FLXj+sK>3;7i-@+ZBR(sPAe@s7hShg
zHZFvj-dW9}Qg#{(X`^`lW5i;yVk-jAXP%93tK*(aJfG4YexSq4n4rU-VS)}nssj>q
z_-rI*IPEZ#1(!JeW#ov{J9Jd1Pe($WexYnzs^s*0p-`ND1Bk@wUm+n*_o?HYPP{t(
zIh7YQf#&YJlt-s$ccRlD36;MdEQAkQ&J8@Ca?nqQ+?~33duWiFwl^eUgq4s@M}s#!
zELP^4UQ;bozk{RwAsm$kL;F1HLRA(NsD}GIYlH220tzGpRy<KKV9BnLw^eg3)-*1+
zhPw|ULi-T*DTSRnb3@2ehy_2Fx2(KOc>^r8jlxehgggoDCG9~wxHDEdYUoxmu4+x~
zssJ~hz`rsrb@Dc%W#Nl>?o>!vir-(%6^YWmnYtSGMO5mH7UP**ru=rwSO7~XY&i^&
zIr=K4OZDkGMLqyNHb?&ily<6hnUV%mtxk~y%Huq}ZWmC=O#UXNDn&TM^k$$?w_!3S
zDsNW`OoFXxog&4c3eD|?JJWo-*pAKwmhEm%+{~@)DV1a^fkhvSdCWe|1!(1$-vR-f
z(Q2jWV`1tz()x63tfd!JBKQo2Y93)D!FrG-*zX%Dw`@CDVhI-1kC27@QkSymm)c!@
zzZ5Bz*IxXR4~jUy3{#4bU&=sL$uHA=o9LISeB#XY6>+L?Bl!JjDmzu>i?@pSM#};X
zx{JhqhU+dcWGvEY*(CG;r=S2s3Scv*WS7ehiw}^+l|BpM3vW)@JQc*2icygth;fdA
zn9le~%pSUzh$xL&Dl~}MT`Hv)h2S#f6M>nD-#`k(4OA=s8{rHf$2Z5Jf0EsVczLd6
z0LNNj-oj5gWRP#BR6&QZJz9B?k0%H^1FVLlD6^D*YtX~*hlUzU)J=To*A-rvu^4VT
zxY)Yt4{_C9$hNar3y<yY&t4BiOA#Jx0L$$49quB;6_|KL!X!wNPWWjMIn&mv5uClw
z2a1)ST;NPA*L=EZt>$CyQ}(*%vr49Y7Xt3LL{Z+OIAxA1CRy2r5HRhR5b%`>Mhlo;
zmzw}_Rho)6(acIyQGNl)X$UvX#_un}fGC+9Jt3#cWh$ykOhuoF`cqL&VV3lSJOa_s
zVI{N}01>LchnU9Dxdxc;@f%13Fcs~9e_z6rC3#3R?18E103wDB9L|5E2TtI>F$1UK
zUoaI_p}|!2$!KgUO3h76Fcnpz*i^K}dAOUwPc;vuLQF+PU5QmU%&HqL>insw3JO91
z+A2>);|O#rItFFI)t#SHa@IiaulW6iw}}#OKNIt|COH+=B%J(AG?<D$C#0N;YD%V}
zHtn}y7Wb5Xn1!l|u~}$?rC63+hDfGm3t?)>4M@Dl#O+Ab%tPW{B<3>l7!t2Du>*;W
zp6GeDqe;zq6VR!E=6s06PA0xaA~9blM6Y2pBrlAWa=EWXpMh4H5aoeLCPd4TkO`4b
zovhS43ngABL^ZGxkXJAZeK)$7@;D)Sh-A~+s75xL?kF7EB4(i_m1m&=7@8JUn(g?r
z(7_>TBdxUP0%@ERLYV2D)hsGy<3~@8StvbUI2hx-Vq>$=Yawdc`Re)4=?6NzUjY(y
z_@zwH;ombshv)S|AC}z#o(GpWy&O5>^n*;$>79D3(=WoNMP4DN_gJn@p8+Cq`c@>w
z={|Lw(}`E7Kd177=zlkQBIVKPUz2RwqoMNO1QdMGazSVo>Ze2Qt~{Td_FYKAZYv?1
zj!rqa;BRK3wbe59J2*HCt;|uyEc7B(78K~2XfbPpO**HK&O+mff`P}*LRE7x3w<U!
z7b3#5P^GXFAZ~O$XBZ|nnj4#iZUEc6)Xt5rmq2^The*^a<Sf)-9bma&1!tiOa6>Kp
zYiFTF{G?QYVq#N;-yyFHn1W5o#Sk}SG2(|)g=1ye@hN0+s$ii%s1FZ3MIYViIA;wR
z3VUnXSUOS==q$_6hTK6;&Y>v0On+WY<R3hEI=?0YzWII8PLns5LF)VkNl4zx<k|cb
zN$};7vl|ogzd3R?KT(nFe{CeA7+TL6jI4@tN8@iw#QC|g7_y*SO?>tm(<Hhka}Dzr
zw8dEP4ctM8YZiyeu8C+hD3Hxscq2&fMizOvg)Fl>nHTXh7|h}jE|i)4?nUM*A{X*a
zs0T37BmsAsSf0hE@c<Na;4RF#90jv~1dRRJOsxEoytvfE#1et8g4}_B8MG!b`93C_
zOOk(=nBuNz&gD#J@(*6J4s)VTPXNhgJK*RkBXPh2wUjO8Aqmb-kHLW1B@Gf^L)ANx
z<fN0}pNL7$tBZebQbm3IizbyxW+FfM6>$^3Lr6!X3Daht0fvP3wIO0xJ<NWVPKF5g
z$%aTSV-nK3f-xaEF?lUPlh_Xe+?z1eCFj!1Njt}5zL)Y-Be*8{sR*j?=M#|aBscev
z{{2EGlj}iS${$R7$+yGOlmkrHPEJHoQvPH*JNX>QO*zPPPRe-7aSVanls92aiV>ho
z%2800;tEiZvX<;o0S2TLrT|C~pg83o#9>OJ0HafGivmbC<G@;)@?rx3sb&&@vXmom
z)0L8LXz;9*9@OIra7N0RD0Iq^=2T$jr7R<+rs)k}amq890BV_H0OzIjK<K5^Hd7&O
zX-W<9)G=oPSeY`PKofHo$kwE^Ax~?w3c$LQP6XNrP?3^HAXk8mDTM^u3a}}qh(J5T
zK)65UIo#T$bP(INr@TaSI++E~^Q4nHnI>G5R15B|ol?O<CNDsPOu3#(Cz-D-OSyr?
zO-^n`uiV1YdI&#0*hx%g9j38rX?FT~NHCYRDY>it+@p|t8GsDZCMI9UBqde*1O-h!
zKL@R}`bOM1MN=<*1~{({ny%yBia5b1%G<*HXzGSNL}Y>9@h(K^farD#5j)aQ-`*R-
z@o)kV-T<^+*P8;N5%0ogpmD0NLqVdcZ}uSRJ1EE^uSq>hYKy<d`{4*OEC+MMy9e1|
zSTY{}5(*MQ>y1ex!wU!m$NN_n8P0=AuJ^Mrd<VOt-gR(fg7-BnPV~y5CCPgk`N<x?
z8J*(Yg`8CH8PKPBH#dZ<yxu5ChPMq(r<(UJWLEbgw9UH!<v7AS3UPL%=hVO=lh**+
zYk2*E&GgdI6l!`OKu#_1^kYzG-d2p@M|+<@W|nsp(#LqF5l&xuZ=qmyy*99+p7%OD
zTi^Q%_Gf!fLw*BqB+?DNOxV`Q`w^Bj_L{*3O}r8CTvIO#f17!QC`*nPO~#G1w;p<r
z^=<`k3-5DS)zUj2Hn;K&>}>7532(IV%8--mU4ll@*1H-Ow)4iqhW6eT#8wBdF1R{+
z8&Td)UO)J%vo{YiyLd0)Z&z<S+}zD$Om_D!f%YEW_lUwgFBMkx^wyws`QGc0yxsAx
zfVjsT?>l(qamSmDN`Aue%0c&}<BdQoe#-Gi!}6z*3FsNe>xB|L>v+GzUC%k*$JyAi
z1rlVw;CLs%&K-{THJ}#}*0^4Byo(`oC-zhWw+jp?^vjO70>WN#yfSo$uR7jE@aSuf
zw-ctm?s(gQd&BX%p}224-k0#kTaNb-3ih_+Z9qA8JKp`Ee8=%-LfE^G*BheWbG#$*
z?|sMn8IJqF@$ylkJ&socF8a{%R>Rzn9PdmN>|@7!6eaq^@%A8!KXtrcA^J1NTZzoi
z9q&&xhA$umCVlC6XP^{cIo{cD*4K_V3!eVQ@s^`tdmZmA81b#+-G=gghq6P_KF7NU
zCHmg+UctW~9IquR;zxJ_Mfs28T@NFEa=i7>@U!C`fq(lQj~{jZ#qq8|F@JTuy^#5v
z<L!qFes{bFP=-GoZ!!vf0KGM!KOOH`Sbfm(ijcY3c#RQmON=)g@qC`~cEIyXjdvw#
zewpz~VEl699SH-^H{Lx6(iO%V37@Sr-ZRKtWxOKTd4ci%Ksa4!yj3XQMaDyXIIDq!
zqKl2kuQ*>~yeWXz80U!dpfEdO@6+(ek-v>cE4|sUk8#{1*Pa0A7J;JfktcNpv_T+z
zd$IonK(`w9OrCpWpPqnjmmajXdlV%mBpEXe&N>ah8m&R)*z8ne79;CIKRc5W6j#ld
za^!CgaI_;c_@h)cjCmXs?*>$q5TlZmk{)BsUXc9~kaiGdOK4wxV^ZMU8vN0|8YnC}
zkN$96LPOIRan%jErWNpAe40$*uek?sLQ8Wx{;|U}B}kkZ&@d~LJ$vX+2Z*+Y)vS%s
zN$6%iM|27E#bCVqaGCi{w1ocV52z647r^-4;WE!fJ4zT~?!-S~PDV^t!_QVv&H6Ao
zVT{?~%Wei#s~GxRa-U#!Ap}_?rXNs)i7uc|Yc`{0Cz=!lv7jddH9dx2jIK9fiitE-
z^fI6>h@tnQAtjt*{zQvqZJHZ_x;2I_L!>9nG`Ao`Sxe?|pq>rTwN6I;Cd@XSiFyy&
zUl7GQwJxtUmUTMU%t2>t4gi-7IbnQeBjC?AEzoV4y1?bc;lBh=!a{R3YR=>VHz2@E
z@Yd|jvM)2+5HsS~Nx+{P&`4a>eiKDYSY>LU7h-If6~Jeb+Btd{I*f$NO=h-bcgq7-
z;zLm+K^26OATUUB2tJ|+Z8|5d^VJZa)c_jPwv1r4xLJ>*m8WebknCphBtY7IG{|$a
zE+yuE0yW&M>+mn_0Rpw%tanKEAb~n=)@A|^5y*Crp_;V*rZ+mS0r=GzOC>gUkQt4f
zll<IFawuwunS<<w0jgBE22s{9b1|~7_EEMG)?p(|8@UCY7bhuZGtdwFB$>ip=Q_0E
zwDI|HmAL#hpg#ymM0cG{sFt*o>iF@zA7~fhS>sg5%;do6)Y*($Nt@QnZxLBQH;E%D
zsRxo-C5RnLHF-b}2uQ4=Ht2KH=BDGXkdy*FJ&xoRs7O0E9WED=r9iLpNt`-uSQU%k
zKpY76dSEy%ilt7Sne~8Ou?*ELMST>Q=ZU5Bt()siMXOA^=xE;?9|65Lj^q>Yr>#8~
zo)swxh?GqHVp3Gp0anXZJ<vVf#W-yRnl7XhuhBfGP9hpg+V#!Qq$$cR26~h)%BeFO
zBVO9g^I)`KX96?d$J)xPyB2I|cT7P2P@ub-=$L#b;{_C?Ui&t<J4o-+9j%JL`uyXV
ztin;49)3xR95*X>B!I676uZZe!L&0UK-#C2IFD6qbGw<Jki*p+$<^Q#hCasQv@k#|
zBv<w}XfcD$b>K3&z;*ZWp$Ii2KOTl*8T4j2&?opLnIb|W)J(2utqz|H^pb$YI`Z`6
z!Fx{ma3#4K=vyn1WODBA)cK|z=+-}p4o>RsG0;5Y(>Zn0;c&C@9at>b4}keJhIJ7d
zX7hAk#{pm>aJ=<SokvFi`(RhNQFxC5rm2tBfM^L;^V0FY_?|!ysYIeiHb=;qkK00^
zh?)YL(<{+Ab?P*R@-}-~YMieE`Vx{{03SMaPKKgLmtOcw@0bn1+!@2pgWyQ7MffY&
zr+|4OhP{HkgNDGLg8dj6&NEf=IB1Lv@83eP1};g$PqF>L6&e4UFS$N2O=8$S&4HcL
zCI<_7u+roK(}!4zt~y0<OXRdDv<r3|Fp~mogMDao=9f2p?_#fq1k@5xT;x+(FE_jr
zy_4x^M)~e@y@=4>2|Dh=2*qtpeg@VoHV1sUF9ZLkPvg{N^E4aGzqH2OeL(#jL$iIF
zO(p@QW<|NxAuS6(t3hJ-=o&RZv(5aX*0^l~yac4vXj1_md*Jq{&M$C^+q*CR^8V9|
z2jNr_{~I|PK<h#04tWK=IhoBOpf89c*@?Px#}0x7iJcpOzMCX}_);<{-Kp~gx?y+P
z9ob6vJZN@Trn3!Z6*|<6`))ufq=>wKO@b9xUeIr9;BLd@BV+pn;Dxw85^WA8cBDaP
zjA)GMmKfO|G{XWqY0z~!aAh2K4{Tl94n>_3pjQC|idCAyj?8U{3Q1jk9Lm?2-x99Y
z?pn|WD|#8{0&2n~z-nCu;V<sVMyYCflIJh+HRT6|tM&e(E!Oy&*$)bHUXl~~3NR-_
z&9`OZHu|K(hSvUMx|TBv=UB#5^>NQ*L}kG;*CA7?PG=a2$?HIsn9PYri)7v>w2*Y7
zq$Bq0!I9+XzYxNS$#g_wGP~p!$()X~kaVJ?6O%a+!TN^eGk#h~Ix(5)mK-#_9+3*r
z@E)wyhHRU_r)-7oED*IWgt3t)0KegrFwq8Uvc07aG8lQS8H158kzxdiAfIZ8P$ed_
z7Fsh7BOWTPHGV?I1ci+yS?;Jy7?%=z8I;tTQ5oq48h-S+62r6@!!!xP*6b!CGeCEi
zPs@ap!JeZvdzi=tfG@9vY|SCUFmN9a=jU=xXU-8756)oHs$ee`*#*koJ~b0g20PT&
z>;NNs0q?JbY+C{_nnziYV-XT<CQh6K_(H%)&O{PDo|C&1brj8GfV6YkFJ#|-9Edyc
z^RMPRxzp?L3HvOYb~>`#TN7bCVaFCnEnN7hBX~T_eLAr&S^WBaU8nQe2sQHyEqD)L
z7q1g;jxiJKI?XY^G54l^uG8($;UKDR7)g<}dlq(k0sWcr6kfBa`#UIOlc-f#012js
zsZ|pp%vzt@-wh>xK+Y@3$@9r^J0Vw3(#oG6q!~4U(z?4abEOKNMZu}&5ejCUJOh#-
z(X%0PdK`^E-attIR(ZOqbivF7M(BEyUH;_%sDvLNo(pOx*dp*IvzHlR+Tk4mCbt()
zIrZWxr_wBIZ7(y%O+K-gf0{>%e~AjJpp|JR6&=46e=U^85-i$o<n-PJvbZ8QVxQ}D
zZx7pURAEkM5w?lK80lj`5+r&KM^2AH_;U%e^KyLS{UQ^y8yKPMNp|^@-%1Ik$isZD
z!zuFVu_A9pL3XH>xwJCX9H5o#4Gz%CAh82E?Tb+4Qjqmzw*2YA{3Xc87@n$vjzDPl
z!t=%j$jj8cCYrYp4<Ac3j}!7f0WE5Z8;$c%P%O)pzdTgP!rMZHTqk^o*6ptPLET2M
zFrp<bd|X+n^4`D{U*+dvE$NA|==ol?6vai5uf^$8Q>hcs*jQ$iuTMSAJ6iMLhr)K6
zM*}(s`Mv?&6)R;P83GE=R)Dd5k?@T$?f=3W#B(Gy8kBjpvhr5&HdQ${hbn2%)gkP2
z78{wni|vJF$iF#cM4!hM_$n~m>HE6oX`!8h=QYD^XFY+sbUJcQwnq^ww2v_5ZP0!F
zm>ufE?n40elP;{I^n)J{Mjy5Yf%ZPA`lAo)EZMt`bDaVIL0p*LhWt~9VpGNtc*gWG
zzXI;|+(@Ly#yYs}qX3Vq+`)}avh};2;Xc5816!nFKL=s41Y7v&#Rzwj#kFweqlL5W
zoUX$5QCNS|74sfxG8;e=OR$aXqcD=iwH@Yb54lN&Wp{zF<ID#X#xAaD355BHSPyv`
zvh(UD+tPH05MI(lz6eZw57|%&rMu{_!2hO)ykD&>rj<iXOU`lF>F=YJL4y8kIt)c#
z46-=?orZkvA$O>-O;*?|6vi&@W{?DlSP%IQvh(((*dl{PF6ld)4nMSq{7DI=$XTh%
z^}pyL_p6nAXytLHKBmG_xdWjzmS8Pkik#kF*F$P~*odIWE7kV}pu*tv2!*l8D?t)V
za8P)eIqd#(M?z`^=mokFQ2b!9CtSN3zJvaUMdc`cy9(Mx>xP@@^`MlY_YRcC5*(!)
zk3tSd>W7fXyWiL8R|+w$j|T~9OOObS){lfM<|Ci;aIB3Rt*0n&HYNyWqIr_mG6c_{
z;vm6T+=ZMxpZqT-D<ej;k_tO&ZD*n*dlwRQC@bt8%KuL&znL1*67<z@-vIjP+Fc71
zox)BbOpcFv7MMOois@l<WvCpbdBvKy5Vl;Zc^o!o2DI#?=c1EduUH0nAz#tMi=<Zv
zB7D-b6f30Vp|<dvkd82e|6)*a`rM+_Uj#y(K6h!}AAwCy;p3V|1G2Jxzxf8dpjaA^
z|7i#)Oihcm%+rMLABOdw!4OwhSvkb5R+V#tA+C7{dxynF=ER1$j*6y5eTFIUVqmz_
zce3UML)=)+2oG_I$-I|IOy=D~YtHbU4BS!I!cXpH<o;yPK}by>frs`g;CPVn!b@#H
z_y~SI$n_Kgu+L(j1{u^m2i9GwdDArymEUK*<}J2)ESb~i-<o-OjJfbW&7;+K#02zx
zT=S^}0-Zs;Z{o_?E=3d?F%nkp0oz55VC4-U={g3D>vkk+jz!{LBwFIpeV1ai!2zUU
zPMz^e^Dz_T+BLy-@_$#!5{236v3j4<kcIu*sg0X_g+H5{rJP)-xLi18;}HZT;BaQ6
zVuUi^p&>JVr*KV(`pHA75iUM8m`a@dtCe1id?Lhr@Nb#%Mu7YsVb<>z;2_%#j$!!*
z{8cN<Psnh^+tljhw+aa=7#0#lA@TP!{%ln4XBPXD`;J_b%vPyeY)zsnZ-*r}HxA9c
zmYsmxN4Xi6=M(7@QBPe%#uT2Q0LxJgod0OTHou<cC?^++uyt6%c`o@~&O=KW+J!5W
zhIKhUOk-!GB(n9Gzb0(d){s$GsZj&}(z#ECSe=esj@_d<PX>;85>j4j5^~Jt@Pu{D
zODBM;PANcHDR_x({U(6<zlY=&X5;lxyy&M+5cH3Jde!$+PvxYa#tA1|B=}=Ka0-Vj
z5goNWOcXd1e&SnN^2aNs1n`oORom67zjo9WAy%g&7o|68&Q>WKyR5yCa(UB`qgrCO
zj&;<>bky|#^M48nD$J@L^Gq=~|Iss_t7r0?C?|_jFeoI3A_o0MeM>v|HfRT2>z$>n
zuLq^L2X^G;gi5g&W_PJTC&q>8Zht_bSz$<l7cMOv@C)xOR>@sR*Arc*|IA)}9CLxd
z;{kVTJ_7iQ3>V`fYbzFZJC^Vz3U6Fk#08^A)rhvf5to{^&Cp@i&BSIO2<dSo61Ovv
zgv2f+3ic^WemmQG*ckBMmI{`<E$f>}ENSFsM@1+LjTpecWa-ljH>&_X$=eKN=<Ib2
zq?t~kZxivmEZ&t7G_rOh(7!1AcW}oSJ{qky-Lo~yCC?1_h*m+ZDM^XUQ&XIR`XM3`
zFr-1S#3yj_`zpnB6wmJ8Iu63p&jYmH>Bt56Vx>TflMCx#pqPiBeASD?D~{v0=EElE
zeB@q@Uoh~t;`w;){Bj17qdA7%QeUGLhj4xvm>F1~>p}LElU%3pT&1Bc@8Rc<!pk-P
z2b*U{+>13c5h8>z$2qQC^Nz83vMILDM$OFCOc}2?Yi41LyUzofH`>ob=s%`;{>($h
zYnJH-#Mv%IqK@P;<!iwD1b$Mcb5N!}Z!686F`+{~(!5zVxa{8Dj(q60Pcf$l5<ddp
zy~l={%Wg;=inUEMnQi{Wli|1M5jH63w@lFL#G>#h&F2H>_I|$1(l`Lr3t@*@tiKQe
zba87%^ZCJSMPrahJaLG3@;fSCdWf+KunPvo->sCm$<zEEqBWmwI!{9QTlfXh(}tz%
z#?m>1SoC_M(2uABbhC1q^522k)&j03|5tEBj~9`cI2k@)sl?5IwlQQ|fz#(Y#dKE;
z#PzvN^M-037iqR=9_w<(AyS@K?2-WE6uzQ)43w)ipR>jHH2=<+^uC{J9&KZRI`Wa$
zKFw?sl<+;UbZi-_`leH`9ej4#96s|RVm9Eylp}5Bb1GgNnYJxRfpeUKnjtJOeiwuV
zC-fhDK-OB$1q=Y=1pH!kAZq$B&(aO*hU9z0F|$#L3j;fs12do%YJyU>!KNN1NVK1d
zn&_&;n~_nuCWb2JDaEiR#%bQ$n#Y=$qIs<9{f9_7SFveOZ{<6M%QTNQ(Ma=I6PIXy
zXU&83zE^1;d8H=!aJ)h@I|U`23YORK3zq(WL*a&si$*fPPZ=uWUS(q|yjFFvojs=c
ze8BcVkgpM0utTvdUnLJ&w|C6KtYeSTTprvP`~o>^azh4P0t@=dBx#%BEUDHstX%BE
zpvpzO?J``hM^+Kn&YYuc#MqGw{rFR6d23W3*Op&C1=SZMxU!Np9XIgtEOBdTq`}KV
zC_E=#_)hbYFMKk{M7Sg<{4NTQXNeO|8Wmm~FZ^wjkM<%m(@$05lA!Q1<mAP(#0e*j
z3ZE4({9`Pbs_>_M;gX>6KPWt&B~CbL5CC_oVBY3!hs1C-V_;_B<=?9`GIe}C#PI!J
zFnkndNUA|ab_U!MVZko=FYkB?>nb(vDHdwp?Am}-dkXVHG-p&zlRrdhXmkFFApqSL
z_kJwatR%4waaR6sNO}{$pdIy}frBV3l<sZYI5-@#0HEv0ts!d^of?Sw8L0Z0%mrc=
zQ_MA0ig8m8LJl{PR2(9wCuB?s%XxruZmUub-#a&K2_`aLE!zrFXt_-JMxciC@#7(=
z@63n-<R3gZvIG8sZ<IX(!(H~H>=76yk6!^{!2|Q-D+u{d$&cSc$bUYbF%UXOKZ~r2
zF|XloN+em2z>M_|w{Z2$9#k23BZzspWflH&ku0+VEgsu8#h#4e+*T>S!aBo$@Z+G%
zIbr67NC>%odQL`pgx<~>|3+=d@DJQ@Z-AB2LLNi6NamAmhX<;o&N4K7J&%$<&Ynlv
z?yr}F==_OzBF*zC(f{-FC<#j^L!z8VN%*JpD2bu-C`qyNC`qC7C`rH50rosfQs_KN
z(gEh%^C(H7^C(H7^C(H-^C;LlOb&Y<B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!
zB`JI!B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!B`JI!
zC8^SRlq5Zmk`z0Sk`y|Rk`y|RlJtM$JWA{&O7i)4qff9WQIaox46U&8NtB1DBQBG#
z{eg&AF^(?uuE)y!BJW2Au-7;b&DooS#}cmB3;7W}x{|zsgKy=dD{>ws`G0yIB~tL;
z^C-BP`0sg?|DH$rfBZa3Bz7Jpa<gHFqUTW}w+N)?Q6d`z((@>hTMhdsJ&zKpbRH!V
zK96!gsO%9DZo62Fxt5$qVRj}ZC@y>+MR2quGx(!a;qxeggcy~ilr(%EMM$@avL$uU
z8HUfJ5EnX+60zq|o<wf!JW7mT&Z9)CI*%eWvZrq~Esy<?sfl=1&Z9)CI*)QV%sh`0
zsp>q+;V|<&N~EguC}eizJW8ah^C%Y0^C%H}9)$&ookw|IxOpBWV$Y)pIyjGF(L9e5
zvFA|)9h^t8Xr4!j*z+iihuC=(i{^QhNLA-i1TE)LBJt-@#3SNzIgb*FKaUc^^E^r<
z{ya(u&+{me`12@&m*AE2D3PkpqX><x5y^R!$X}mFiNv2r5!SyACEYD*&!e1l8dmyJ
zx;&GCd*M6I#GLzV&K9tJzsG0*=WYZLHCcC!0kD8TvdMY`v)z=11kz2`qhkRqBH)><
z@#I-dpoYoHCD{@JwM|wQf%90RIwor$$(9nxHd$}st|Mg`$r=Mg;CvdB1JNW~K_J&;
z{hNAL66j*Gz9iWy0tF`PTH1C2fdM8fpTLC#icQu_6mSuN(I#sYF&7gkHCfltoJ$Cl
znXGe(Swmo!$r?^QYYEIVS=)%Yl#`ytChG<Qml3nlWbspQDVMVdYfRRi)N=(f>rB>D
zwCze_DooY}>bZ)TjV9}E0_zBDGFj)5=V}7?o2>a%dM$mh-DC|SrknzvG+D<I_!og^
zP1Z~T*AduZj$y^6<eSUW9p@VSYP`?ZWOI9)+mN%>&&~WqP&^=?($74F>|H)8bVwp)
z;42{bijC+Cpnvp9EO(ucse5QD43$0AX)yc<{H)qKt0{YUYaiPjm^Lx&Zt@<V4I1I?
z56p2fYyz?4F7bIM12a8_twzZwzvg3?0<$WH<%Z~#X$O4l^}uW(b{|VzmmBC)=3ET<
z)ayRv^VuTVt=-A1-vq5a$in^IsqMZ7S#Z0W!5XU1{oScu1>xVh*8d4Nys3GT=ik3(
zbANYg@AGkcSkZv{yCWUh&Kq)ncVve-Ee(|oxDoevrv>}FM<dHN$;SQif#_ha?q)4M
zix@UXxnXr0kLd|?Q}YF;=Pb~bL^EhCx=D{7K({ixx>&SqnztxyBv=QB*qz}`WjA=!
zzu6hioxDwxUxC$4bx%0c&A1)ewcC-Kqtnjec4Rk?H-JdaM6x-O&AC~@mF>vKIxS>7
z@?a3Rl<mlkT?TJU*^X?}PAi6~DRjnz=rsIVZ^+gJpR#a=yOY7tZbK<|qHS%`1q@<%
znewZES&v^XW#F+*TgI*#w_OQGqM|I}cHnp5*Ur}g;#SK@+|GFy`Jdqz=y%$XbP_@<
zyQbBJ3ROWV9?Fbgdvg7NWt9$1l!kV6^j}!gmMz5Go~t-K;8gyB;GK@2Eb*|^EoS0b
zXV=Oy_#E45%VuOAEmhRCKm@ly-W3*c7DY^}N(9@Q*|SSgu~O^=`ww9ePe4S6*OiX;
z#Y(~!X};Q{xUq;nUZ$L1^*jf^pd^1lMwjC5*w0ac$}-<DJ_nqJN12z@wq9(5%Iv&F
z0UpNf$VZP$I&w30M{cI>$S02*^=V#5Za$r9I^qGOd>f%V8^1Zr6l7UxebL0NCI5mI
zuQgOlm?)WzCDS#q<Y@qT^5pd#^ET3vwQCmSm7%Gba~0mSu;<S(>mn7}$jWrgd8R+s
zdvfcbhEgNN4eXNg`urmRSF}&W-%Zfb`QOS`fp!m%!Bi<y1&h=cMZ(imDU&Y%Ws>E1
z(fLxm&Xwk&n9g}s==_5^zduZ!zJN+PqsO>X!l0ZNnKoGT)pAx;p>x0-=xnBJ|5Q$2
zKqZ~s;&rYuH^y{6TZPWcsdK_%>huLv(m5+$=jEmr`Y<i$zAAL?q0Y+=Q>QPWlFnP=
zb*?iP#&p(hcxVi@JOetPJ4~IvfJ!>wjn`Rj4#spARH1VobsjuSoxXrdI+N<y7(&d;
zW!9S+7}nIcWmV{Wk~&)`+dsvSF92I!*)rS5YrDa`6VtY)3T@t*(02S`YV!rCwo{ZX
zXdkzjp*4fD-Cu>a@zl2ZFtzytD*5wzWyVay`tx=<%cB0=Q-#ir)cM?D>huL@315!a
zc9$HgS8d6S4vo_PP}{GEsm&Ll+Du&=D?!;do2zRD{>-gHTftmtYp!hn6f3>})z&s%
z+ZK~lJJ2?|3T+or+nB@D<_l15lj61AXU>diTU>><x2SE^VQTXQsJ6B7+8#6?$Fyy%
zLR;grpzVRf)aDCNZI8ukd&HC+9e8qA720M|+n0x_%@?5B4#aDF-0*EN8k4_Nq3vO6
zYf{e!*FQyxFF>`mQnnx_pE7*AfNINbd}s?uo(F9s4^x{jK($SX*Y>QrHl}Sr71~Bn
z+w#NI<_l15tK+r3U{Wxd*0Rm2LR$s3Z9PnFz5vzsg0cm@<xaC8rfppn+P<N-Zx2(O
zFQ8Iy`Li;&@O#Tw%-1oU+pEyoWj=H^;Hy(`{Zp*?0<?t9l?^-9<T9_DF<1#ye|}Mg
zwq?{-e3;sN0jh1ZvIS*(%RCd)<~2DqGIvqi(!<o|3s7wr#+U6K)3I()wk}m@t8+HA
z-F=wad;zNM!T7ShZ*GifE3HCX8MS?SnA&^+s%>w)whzq_^@6gktU}usYO9@ncv0dD
zP;J@D7S!7(W>HMrrYf{K=RjN0VQTXQV9jfWPX6;TGrA5caMOWD<qd3LI0Z{W=tq3?
zo2uU_@b7ZC?(n*xxyjNgxG%)E*RtVO#wmCrl+RbEh_;wG{|1JO6^#XL#r+y$<9QT&
zXXF(4Hy@-Q5u$ISvr!i6)DXRY<3RdlA^OQ7`tlIHKj|m^mJt25A^II5dVj)B`cFdi
zFNf%P9>urcpKO!9MmF<pdHo47?Qbdhyc2W^{HZJR3q$!6buPvH@u7Tw@=5t;gydfl
zqQ59a?@t^l=XRT)5{XrhKhq=ni4dDVCu9CQp?rV##QYya`TjhI`Dxfo;k(11kubke
zDBqtuFu!{!|9c(fnLj*~?~ne>pKbHQ#qft~qAv)s`2#ESZwTf4Lm=}X4COzf0|)b8
z3FZ5P7}wOeUBs_df1n|~+b~vae@I|{Rw&=^yqVuFl<#-5%pVlW_d7)9pBT#byDsLR
z9m@AR8RlOa${(KXI=q;FTP(lx#8AF8+&BDw!0EPJaXT7=F@sB{@~+D-!Jd8|verA@
zxtMD9n>9)~)KCYq%F{5~H^DEol-5gj4DL6~#unNPt~$5k(zNL=2UUh5KUiJM5#sL5
zvy^R)Me<uL_U>t5OA=eBIt!Rv@N3WGE`!f;o%Vf@SdK)8PnDQ9qj=n0g_@HsyiQxL
za+|^Xlmv?na+&gHz<m(EV9jb9MD-Sf1~WhDTy)ESR+fy#xInO!tWWjP7)cJ<E!7Xc
zEad9wA*jY~-+bgBJec1e|KP}e+%1*3aT1E#kGrK3Z)LLon{aus{M>&ZA%D%ee-tjQ
zyjyBOZDdspY>dAt5l?qZ4GM2~V(tFrhNoklB5rtcF2qDT>g;F-@n$>V=%y2NM-$eR
z{cFj|Xan5Q^xrq^{`U>L;}EO=|G!~3wxcQa{9;6&|8CvKpuO^rCVjW=ZXznZTUXzI
zdlMFl|NH*i|GxkBzwf{OfBpTpH{j{yk+l<kGdm~X&BAEbM@^qR-%0qLP2Wl0Uy65h
z{=p42$;lnZBYi;n3a5s?F_#cNqor@mB@}gQ=o@nhCA&5Bjk)Y^Jhw)lo)F^iP|6$+
zoe2jwL940hzdg4v2Ja&LYK;PiW3#baX*{x~`q{N9O>x+*G!OZ!1040%4*n<=b}L;6
ziu(g9N{CTON=dO>={b<S7Lbk?W!$aAx95I}e7@jL?5l-=9j`Lw`_J_4xhH}+c*CyH
z)E54uLvPQeZ)Hal-=J%0m{ps84*lr>(blk|YvT@5-<~VXr+|@f&^;_>?pD&b=L+)%
zF!Bw$hs7Lvd#*5l0>-a=X6I<G3FzB%1s%a2ofP~;wsUmo?YXQG(-5c}qRZ&hqeE}c
z6?8A4`p3|rx91AF6sRdNbm;B5tWC2BsO2$q{M&P{1L~##ofUq2E?*G)BvFhx>+-Cz
ztW({s^fqvx#Noql&;1p+C>m0jKm7JwzM-^nfS2GsI`sBj@tpsLT`NQ4>X`Vq=bi)^
zwMp&N3B5g+8ScD5Ji$;DNl*o$BnaFjZ~)IBWnFHkz#~*{r(o&z4Si=KS&ut7(%QWP
z$-xvl<6%IugCni0ApF_1?BGbtle|hhIMRA=!kpOd;7I7mvNv?mlb8<WX#CFu)#yPa
z%y5?`HRfiHblJ=i6oWf0)}hFW$)sqK%pR+Wq!T4w`AvLHS;g+S$VE_eJ$}tLWZOi}
zuh8m_V<0&hENN5LxO;NsUZh{BM7ajucTX0|rmS{%M&un3?5jj{Vpk$85$oQa8{xNw
zGVzP)?1!ScbEU$YG638~5x!`*TP332J&BMSZ_ZsI5xXm728fR3u8@fA3PExvlFea$
z4tIq_WLHQFr}-7o5goh=`H}Tt$r(GqhjY3^Xml_o=RDM(Npd?aWgp4oplv1lNG8fY
zl2)>h#HO9r?EOq@<Gc#0&+rS1)GXLY(uNXV8HaAYA$^LwSNRLR@)X#YjbAR=3vdJ2
zwv*EQ!uzA99Vq={Cl@5$@C$@HZOFA66;-VC%>tPdfZ=YYcI3iVlcpRb%s%H#fEVBw
z2+5(-%|3B@8*<zQd5cwEb|CLgU>?S=9R-+??H2Oi2&X9r8FRqd0dPNlF|mvvBjaBV
z1H}Bvy3b)4n1d1qZ741k$}3bE*3jiL<yp9+?2KPKiiNV*`ENwKJ^WW9d!)=Tr!~hg
zlVO|zpga%1KzA!CQifrfT8SOi3{|!gL#o@fSmH_AX+vd$VC8l-1<guLxfz(}@M}jc
zFr|e5Mzov2e<kgv@!zm^XY$`@r!~h*lWm;cfVrd*Skzu_@Ux9<4a?vN*M>p|!$*5m
zC06-VWo-;V?CRT53{-y1e<Pd@@}Agk#%W~ZCnsPS2;!6Q>*zOLGqRH?>}HtF^$@D;
zz){RxVRBV~L^p((@(U<{TW(?kI<xuul>54a>RPi6WbN2$&B$E7!hJqM)#-9B2s^7t
z^f+>vtIQW9X18KScD;iCoG!QEU!GFJQ<id_!6!PLcI;L_`KYC22U8eNd9~?5O7<<F
zl=kOzV+T?cPg!o-vEKkyYy892fn%__*_0~fza$q~<;x**6n5`)_ltfX3e{tw(qTzR
zt^uIP>%cJHC<i|GfvBhK>bcdBo9|QT%x-F(GG_Mx?OmolaJhWb_lXN!yz&Z(i<uaN
z#63vlKd0mwSOO`KgR%wOkW^8?-8%)cd#8Xqk_wte#f}0DJ#!afrR`;9>;`98MjTB%
zpqX7E%`T@+fqq?1>%h^_JZ_xEHhfOE|KNg^AbJ@cY2bo4U|R12mi7t5ab?^>z^Ai4
zKWR8VzDM{*;-3=u#6^G?<pF+L;3o;6!EWUlL%~fK<Gk`B!p};3!>0xN3?ulQkh^P>
z3ya~eCqQbQ{E|3%CD^Q!cdC>3o!c{qYF|-!&BDA-Y1XSk|2gQpa2S}NMZ`l+xy<|K
zCT?rG47kE2@c&9A%9!9+dR8Nmf0nZJtXvV<whj>GtlTb5tj~g$_q(aVZA7f#dnk?-
z`~efJ;OKdfTdFL61usHvRaL=7p$a}1MvK0)mASoNj!bv8udf@Vi@qsLP~U~rcjsa1
z8xzwPUAyPp!~(uuW1+7&rKv4wYhQ7)d(c;Wm#=uYulOJpcUG2vDrj+>Vw@CzOMTCe
zY!n~{7|#pnC)1hjAQyGOKWqla2F)On`i+6n+?dz5ObgA%Fj@tmk@Vdf%AXa=e>IeU
ziOml!_xH`$qEOt17+e0>P(Ex7u{ga>SDZAxj1KTy2Nnl!)?2KY5xy_We}T*;7-53;
z)^I7>OEv?0h;46#rM(T6268!JX>Y{>zerfxTZzDl%K%Gz8z${7AFyq2!~OP_zf;X%
zkHE@KiA>g-fT_H(GLy|7rI{L-dLzu7jon9qsq?7nH_T$p6ft!-VKH^8m@;%ZU@^5!
z;LU`^)M)}Abv|G*^%ODnOv2JNOczt%1l+e)GaG(lE3_jNxT?<Tj;?hsk6Qbu`}X(i
zuk>$7WsNW$|1NC!F`o{+88iz_u`<RAd^yyegzZ5=fq$ogr?RtJC<UId0<aW#kre0^
z!cySH0(V%+0_A~ziNIG9mI9wA1^z|wl($q0Ja!cdyh80@f!jiR)dijt6!<=+_Y2(V
z?+UyU1@6y}k(gX}yE38z<uc`WLtUT6>_l_j4I~;lz$Y53M7B@vAk*H3Fc%+RyaSZk
zIkuMIO1Vt=pd%rtSRYFa4YJ2$5?jQ%_6S9-=>a_x-LAJH&kWv*zXuN<w?vU`KrO8+
zTOTO+V<88`(P-}d)+^=7wy(ychlbV?xGg2(vkk#}&m3ncpK~P3bB>|0Q{h8@gaeXq
zn*_GvKTsFE8z^}OWeRTGO7;-)@77BGL&(2Rlau1Z_*tfk;Wh9V{gRrRmV;@{ZWJK>
zJV;#Y46iR}=L<Xr!8N?KfWCzzZa|nM??t<J*lXHVnvvnhC+tmXUkH3{_9(SUX;U19
zTR5Zm_+(@XvX8=#g`i7nGZCUCt8@HYs2cJwA%*-AGXBA>;tJ%B7Dn7#!EaG#Luc4#
zwgVe&H_1MZ@%oY2UcaKr$m5u#|MLUp$xA0gs2ng){-*=xDWL=Asj&m*si6brsi6br
zsi6brsi6brsi6brsi6brso?|WSVp7o?E&-D@BwqYk(e0!!Nb(>0rS-G0rS-G0rS-G
z0rS-G0rS-G0rS-G0rS-G0rS-G0rS-G0rS-G0rS-G0rS-G0rS-G0rS-G0rS-G0rS-G
z0rS-G0rS*K2h3CTfO%@{fO%@@fO%@@fO+cw-2>)n=TEBZKaNTJs{`g~|I-8JX;mCB
zZ-<-oXxgK_Be-o(%t4iVBk<RFt?;DQ_2xCf%-{Q=IcU5|$WQPtXol9L-$_h+a}L=K
z|2v7%f;@B||9wvM-{(aCeNOcM_H&}>-UFC0$afN>mCmI{E1gS^Ryvm+z16T=({t%j
z|EZ6B5HX5#=@sZ4Zo#j{R8ZNY;!zAy_aTd4I+W~8N>Ci0^t_7vF9IBo6~|7TD;1vf
z{0fSrFpG;xh*5>krAP6kho7}+6OhgjWqi^T#gm=_<QMt4(7E)eJ(oTQ#O7q+PxEOq
zg<sC4NA0<Ec9>=*@OK0>%nBVz=fuapJcp%m<Xn2xo=X?zr@;8-;WG0{Pt=}E7iKpF
zYZdreNBo0kKIw_tbLqmI55~Sevpts{wdc|WeF9J?$IyJz6Ahh9UjWonqUY16@}0zJ
z=v;a^P&dTTe9{vQolAcRsK;VxKIw_tbLor+vm2;AF*Kj_L__D&`8eiafR=OVQ9YMl
z17TbrKkE^3xtvRn>XV)h!13#PVLYGoMD<+y2;e5h;rXN|s^`+r0B%u$m*ACi=}~(w
zT|BoA_!|Qn&S4!nmmUqBOWy{3CaLXr5~KE9Iy2l?16G268A|pLtdqeZ_y{?dp5En8
ztT)TK^z`t#^z`t#^z`t#^z`t#^z`t#^z`t#^z`t#^z`t#^z@}HUba1#o*q7zo_;=!
z$+73s(^nA4wdc~)R}$!A&!wlYB2Zw@rKev&V1PZBo_-;LVtXz<{UQRR?YZ>yiwTt4
zbLr`q5Gb?f($m)vm}SqUr>`Y2&z?(9zm!v=#r9l!`enqdwCB>(!{^e|uOMcfJ(r#y
zK9`<;6)_v_x%BjP1oT{b`qc#Px98H+!{^e|!{^e|!{^e|!{^e|^UVvm>3bQ!8mrlw
z<Xn1sZ}TB?zVUN2FB25crKk5Ze;_*vYK0+mE<Ih&rC-fPR3GSLeG<zp=hD;VT)ISR
zZ=eVGB#xX*PnUD)f-MDRN({?$>FIJVU9by*Sr)_cTza~kOBZZ8FgL`oJeQs>=h6lH
zATZlwSe{EymviZYeG8cPiCxbU%enM)IhRhX`-9JCi)0^Yrma37v>7gLSM4`LGdjPE
zb#nP;XvUVCDaU>@G-E3P{bp!J_?w{_;ctd!gufY@A@AhYZ-!=szZsg*-z-PZb|HQ>
zZl@CYW@yGBb1iai@pJ9>;w);2xfj{|(xEV9ZWb<nGc*HBVaR^hM};aiL%tb$A4tuA
zfIjGxSZ@0PH~D6$xUUuliH7)D1=9V<H$yYzo1sF|4d}ik;fF6WpP&u$&Cm?_W~h+x
zE0$$(B>ZM*M(mrRi-Eo{AhC-0&Crb4H$!g(`mQ(<els*9_RY|zf#%t9vBiEfG$Z!S
z&`*H*Du(4ZLo?)?p<*OI0-1qdxTySQXoh?<R7m(ivfMZlels*fz8NYc1Arb~iA2Bf
z$8Uya$TvebV(d1vKywc1#A~*^{3v0Dd^41y+{=Kz+81TN8JZ#A3>EBVU>^3dw({hg
zp&7AnhVCLdCf~`ZhRV)h2UD+gO9XDUuAhNo0~$>I9KifCxMPkXs#=~o0@=0jtFe#S
zHn+euLryzCH}hwrvadcC426cWOn=~p`S?)ORqKB{ZWsRmy*UMFev(`STM-TVP{gnW
zSSX?=3_b!W($<p9MK#%%1?~iU=5y5PWDwtw?`^JaGR+;JyAjVg8u3r!k%<H9BsMgr
z{x?9|LNz9(`2=LYQe-kjig7inQTQ?V<5&{Du)P+3aU@QYUz<U|G3JyO@EgcsqC>5w
z3y6D?p#g<7*+;T$b0z*sIJlk|XC{JVsZaL*vG?8KSruEqyQI7c2?SC=j2e3Ap?3%n
zAoL`(gc3q3HH6*-B8UY+5-fm54T_2d#g3v!?AXzRf{2Qpqo~+XQSR@zX6Ai&LecZx
z`_FxzFVB-?tyyK(teII;_UzfQ{yK@3B4tBT%B7l0(~p}&iD=(Qk*e0R@>*k}Y+ua6
zu0yN3C=s|SXPu84K{Ui>6Z1KZ$!H$}+vn8N3>L<=rkUppq165v$$uD<l@J5}V`G+j
z?I=9)I3K?vr7E&nEjsbCcUeT#kg#<DOE1G_C3b*&?6n8+mzjaf6|PC($Rk$==APJs
zdah1JJ7Ze}SCDY62FKNgOSvL(H8Z=m66KIy<3t4u>wRE**sxlO7P+=O8&DV`T>HWC
zk>Sz`F94H$%LueLop-3P@&QjRY@myUs<maqR4Xxpb#Z$%{_6Vl34dGg4z9{Cl}e?m
zi6)Mt<c`EZO9&ImX^>idZpo#rwnt9Di#pG6k-QC3uT_<l>;i4bpSv#X4i<I~@4(iI
zYKWp^P!fx08=k`cjUut=)sAQ%T2X{3>I5Zx^;{_u2Lzh-IaSK#DOREw_}*+ZTCF!y
z^sa}_J^v%UR$?zU!7sBAZM4doqVfpTeNC0p3uv2__)IBSk7uHW7*x;>YYA%&^r_nT
z)vc8i3j62v0fkY*)eamz4VRif$6wEeeR2-`Ev<Dl$m0!*)%<+6(o=u2Srj96J}I=u
zYVkXJtz&m$y|3{nTak<b;*pj`shHD&(u#oH%eL<&TM!0FPaI>54YXYyDO>)8jq3=E
zEUgIG@7ng46m5|3_oA)u&c!g)?&2Kf%WhDjRuFLw09E~CwLT6uXXJ*-ih1+7m}A3H
z;Q#86)iQjrW&NAQAYc!0?fZbleAwn(#bD3Cqk<C)aUu^73*ryJ#E_>y)F#?LLBlC(
zNo@;rWJ(<O30P{Ku!~S~X+QNK*9gCg=NZIREAHR<;Camc(eUs?;DgBXRT}ij044LN
zT61#qw3^8vulB29L|36-`mX#DcZzJ23`o^_%03BGbQ=z4$?a`WaDUZ&&TG>Syv^;q
z4e#^*OaiK_>Bom$!R-Xu8}Vp^85H69ho{{Hno%_sXF}P~)lEqBpVG37?mXuotL4o0
z*lob%O&k?f($v_01#H=k-<Z$n%YdV#+n&jQYJQ9Q$xbv*$7sQ#*@yAxOX@uCuOIPS
z)G+%{C+*{t{yfi=iKGQ`vRaD=zi@O5xE2NZzjC1XPV}F_ZMwc5vZHmw{i|wrXEOMI
zSS_k$Y_dN`3unt1jlC34!Ooh~-`KwrhC69aoYs0fwaQ-B|I|!T*v(m@6@F4x;km78
z%af|L+kcD+r2m0uQaB=a?QW809(yM7nz7owB+WehY{zTHYWIsrJF0BYYsP{)aoeW#
zh|bvXTpfcayYyNn_5@?OG*qr*Nv#e%Ivb>JBI6dO5-Hh^u0oMAim>;4ye)Dq-%P=M
za-g2~!EiL?`Od&W_^Ji+x9>w@%!h#B+xti>X2*?Wq=;m*@7SR?j0&-*^t7x7RW+Zf
z9?zOqwMAN;L~UJrXD?&yStW%~P27Jzms?GpecUPT>?0f^aMkm<AwDj`JNv@0Zs{y=
zh`{Cx+FZ|c_Hl9E*+)1;;1C~uAL8R6AL3*Eb)jcCL+*-%FYv#c&uNWUu!W5)iOxQ*
zf;#&MhX@?v<KP?Oi@A`0>*H_t&OXkzL-b!x0y_K3C53-`a3bvv0^@l6_T+&&0sKc`
zBN@Vat>)x9)^<yPtTT)P7{;Dm8{z3WNLD1nUnG|(J-Fs_w*h(#zoJ|KBZGHHQI;N@
zY`Cw0W53~|304nIHQY}C9;=$pWr6!U(m0Xun~Wk}&jQ^-sd}JHGE@Ofo?>S%NWEuq
zMv{$;dgJ>ly%jVw#j^V-aG_-<+cL66G9M{WFRsJrhs8*_*62Uo4<spxKAzLge2F|c
zaM~H5IqlpxaN4=6)lbeYe+TCNJiF{{kwYB)d3I96R-aeW@#He;O61};NWdvV9*9<W
z-wtU6*#P?TKy+|Yb8x{qoQL%|MCWvZ#8Ui<>jbFcoRuKaIq&+Nb0FZHgMR0{r=8OP
z?Bbl1Zpib!4%mE5M+m|X_AKPbu3)peAHM_G&~UI>SZxeqrJ5D%82Pa)-Ub_ol!0V@
z87Ef;QLW)aZ&7@!EiaFidW}}p8Koa=uG8x*pR6CkX=G9&!x?THRHoqs<+}~>=;Nkb
zIJF%^mb2Eeh83r$Sx$RWMhH3<*!#eC2-Y9i5nvHlk#zvk&w<VWI{Y}o)n3Z+7?IQY
zD}em4)reX-K)e4gXxWO9{2*p0ZAu9_yVM0~z|%+$cW>zxBsSiX#%l;F2i}FhuRwYF
zrCMUJDjZ%d9xMkE2lK?~U^#I*m`Cl5g2A{04=*-jPvd$m*$Gj%2Xdlo%F9+j-0lcz
zXtG8f);x^=8Fq6=Y~TZ*)gpkCkEP5g2^sMZ@`>uL3`IVUZ{T2cqHk1dElNG<86G`i
z?8<ZL=&5yBqq4Nz0u<GXWJ9R-N8UK6KO<Iw;})w%HvVy9F1U+^&OSSYv;G{d>mBqQ
z{@~PX8&l^aS?6)<Or58Z(g<n8ic#lj>w(=P7%Dk!Bd|4Bn>tV11hm-(psALV&YZ6V
zGP+13I43*0uOIe}o|)@MqDC(WAoV~A@Fjp0z!JxNn~S!QLFG<)(wM^@tm~MM{g_+6
z#~OYF&1fl1QY~+cYI(Y;ecb}3#7{zRD4MpiF}f9-TJ17?9fxhKoB*5I&j8XBS}JbS
z%$A>}ncc11DXKrHe9!U_oY-Q!eprjAF|)bHOlEhK>&(J8>ddkLq<``cGkfsNnRRZZ
zY%OJG%eZcql5J-mELp@gI<pA?((^R;-<9l%GiP>XYn9nfW_IabliA(OgPHwcGHZ1$
zGJ9Ba>&$}6OIL)PxxYSl=In~vsO*j~yA55M>arC=xny@A0oK}4W>>Dc|316d&Yaz-
zwko@*KFDs%9VWYm*Xiu06JU1R0HiP1+<%|l{xfH{CP`(N%<LYGZKg~2p~<f4^*Xy8
z0O|K@?!V9O=$W&-pq<LDgxNiRsmbmJ=EA!B#AFx03E2(O+<%|lPiM|<r&dtO?Ed|u
z$!;u^OLnUW(3y8KyZ8Tbc2*$ddi&g;?N#aaF}vGxu^f<Y@S4f)gvsuL&B(4<2gUvO
zrTg%-(v7O2V~GqCNgClWaTjvsfYRYy5k5;t_>5958IRC@KqT>%Y6KDXkd@HG?>}Lr
zkzQksG68HUhJ%cDI%G`60CQ4AGo0lZzy|5`axje=rV%l7jz(za0L+8VITBf;OaznM
ze)i-xU1U{mCh`c=0v%Xr(gqLH2AwV@-Q-2?_9FLdgvO3~-HUwgK{CR1L}4-NVVh2h
zQF<<S=b1sdsTLJOI(U(;8j-v&)rb`8WsR^<g_51AjxW6me)1xYjzW@8EiV$I5y>Z4
zBa+Yk8eu+fRLSQRuY$u~<Ud~IS1%Hz1Cv+~rV+_>ghnLO8#Ka9`(x8J(AFOJDtOh4
zeBedC@**d^NU)AoVnMh@#DY?dhy{B!LJMA~VnIC}4uoy0M#y%o3Y&>B!uGv~jo&+D
ztKghab)XDe2MF4fBxL!rUq*dT`Vf^KRDK*|c}52hCErkvf-)-2Lm8FpMdo{v%e=@n
zUSz8mxzCF{q!IDf35`hkQgqCs8-5gXOk>Q`6mj<gFLJpT+2Td+@FEX;k!QTfiyEOh
z8DDxR3$Xsi96G3691`47#Z$4Mt`}+UMS6IVR4+2ti{yEcsTvUruJ%xVH5N!^-Rhy#
z#w0Lc+a3>P)Z1R<GcWRs7jZjz%2V5mT<Ap_X@s^}qgHASXPvQ+eKHrTnYwD>g{8av
z*cIFZAIBwebrKf2nV%s2)Ty+0@egu%IU3XX9$WFp`2k|xu>B;9R{^RlUVDSoyLc|Q
z+Ov^S@8UdsKSY5u^YNfpfmqw8y8Xe5tKy+gmamcA_X+WFBdW(?{Bb9OKF8!lpzk}A
z!SVD$k>lojB)9B3%Z|+u?$jr1$1Fpf=8s(w+0(MJWK|g?3+H|qhO@ZDQ0P)YvBps7
zK|#5&RCuE;;GT?SMzBp|ClbXvLt)nf8$TI~28CkXF@7eN7a_um*Yn0N@L*A(fi+YU
z>w)M&6DE9;2iN-K0jyJ=IH6C02ThvrF3RnM*({7x-gUzFWDb+^a0N0Ufq1xG2V}Q=
zuQlOT)Hm?Dg732?Y*qAl!4Fy!noSXU1Hq426GHNV*RY!ke$<+9FZpZOEd@VrO?Xq`
zNrIoShFi&4YxjUZW>`*5G2=D(V$!Qr6Rpi!w<voz#bRylr^Vd>dNJv7f@2N5XwsBI
z;1_Cq^P)+wmU!^WMU&EtJoxrS*)LMQfz4(z0{XHS<^XRf_)g+qGrvZH*SQh+UgC`f
zpFn&uz1T$XZNMikwwy%SL-~f-63b~WSd$81sPon~_t7R1Lshl0xexa=u<WCBZ<6rV
zya~8PyV?t027L0I63ZE6b1xNdxGu_0+W_(io7=UgK+fUD($vll+ox5T3|c$;iuRZ{
z#wJ+S<u_w?8V=wa0;j;q9MV^6_H7tM5MgA7Hae($^Fqwp(!-Zq5vTA|ze8gzxv9NU
zvmc@M4)*6pdy`wV_BRMnJMB$>qMCMDcDm)3C|~JS-VBz#t+^BWLU|wiF!q}U&PIB0
zC3NNkNF*@c0BHu`rq^(ga>9|XQIZBF_91%#m1H1&EoBD**g>F|0Xk@4N(%r3xwrT>
z=vFTGCEwHbgj(tc>UL2b_a|#@b*wyYYM#baI)(e1I9*x<Wi`9#+ECU^S<76w!#EuB
z@F^`Grg`?U*va;kj@sfiU=fSA6QIQc(BjvbLSugbFpjp4(n|cc_O->%7Po3UPh&84
zqSN)mhEQj8x>S6_ql?Y3nG3O;&j4J|QxzZ?JWf~pPHgNAY=eOHJQxNx0+4VUO))^E
z0T|0?gOo$#jFM6)u?M?<vV?TvXJ8N~HrBcLtvANy(22b?b&nEn3>yNDoVE=H=1zvT
zg}ngb2m7G%gPcZ^brom>xx4+UuyW6Foo1~SU|qnHabwcG9cl`u={(sdWbG1MUbgVs
zAk4lDkZAz*0f&q_TH+b>q^F^blD<?cqr#JZz@#Tzg%9iW2zZ{nvhx>QzD{yUH_8Lq
zaXpl=Ukh!C$N3;FxDx3f>vT?6dg^rY|KLd<t<x_?dZl&%_f(5jIwn2~L#*_NJQQUX
z-1{W_QlVQ|g!Y-!-@Xfy1G&53mp;oO*LKZyydT<2iv6nL*h!Au5ojL!H3vZXaZ>F;
zU7)7`38%m{kc0EkI~*(HsA<y(^XGvD=D73994wLC@~)W9{-o(ViP=6tx6<9N$P7Px
zly1t9(NOnhPCZvE?xriO4viODE38JP=Et7rMOr1=R$R1|G0Y?TmXhtd(z@7c=u2qq
zOK3_Z9`x_DnGj7&&g&{BwqI@qR2lg1^q62wJyW!vl^#6?{yRM;WU}QhQF<I-SCm@&
zm+9puJ;^FGHTa}X(=Au^542p>gfr~UR>mi~CQ2}5#dnjU;{Emo+!B!XhCv2v6XKZq
zALpQxiFD%ZRb1(!+2bEyhR46Cpz@tyau(GKD&K|HnqhZSHO&JNy{qd#u>c%VnuCw<
zbE<QcQpa-5@e2CX=ZeGhzR{*Iuu_IZT1o3N)gXpVQW{G$#af@THcD9wBVhul30Sd-
z*@-B;07*gR2L<RFRKAZ!eBWIagr@>Jbg$+p$r2`rV=`on&SXw-lDNe(Zt*F%6xQvb
zv<)_z;99G&iB6qtQrY1%I_gx0jwOa7j_L2ATw^G7>ZnvrktNJ*jc^O&ZDE7kPO30=
zrbi1_;W908)F?%ZunJ$(sehSNG5U~BjlrxCeOX$KmGK{)N~7BtiWvQuhmvY2G`cWS
zN49K}D*DgYsq;;$IHiS7rT(i8Mf7*`P_`S2IHk9y&?yDQ8WEFOJS)SDPrpFt5hclo
zG#T49E7MQtbj0ZN-JWztv4x0evMh<@6YO*U8N!n+d$a=F$s<2@MgQKIZpfPDi)<wA
zK$MlIB7%J(7*;5PU38~;Dk5kmQLb0M<Z`8)7?vx7yaHr2S1W??Aqf{NN6Ck^iaZ$6
zNR)ao@*hR2ryh)GEMbf+RKD7dmWy@D7+IKnO@sNEBpFi%=j&9#x{le(>G#*efpL*C
zMiv>zG6dagjgd9Rv2lX#v&P60<Jd$&4_ZYD{V~UvWzBb7J~-u>BW}&b|KkeF86iuU
z?-e$6giNu2<P6(ROMj%d>Q<MaXs8iBh~r>{yATk^@Gl)<4F571AAYnm(LeD|wnrsg
zAdI=?J_dv+w>(P1m|I>?0)$(h!2gI3iI`+Ki;}R><1FgxmM4V_b{FlXnfSjC|8wyF
zAUgaWOrqUIN02fV|BoVNucRCYv`?TDfDYpISJP_biQ5^P&JZ3J+NX<FNq+p;6>|oe
z_3cAy;f0!!^{qnlh_XREEDO))h@wR(tkGS-*a{vI;%cG`{OG4Z)w1+ay)3oVveckH
zwJd$irVUPclv<WPCT2M0_0+0#m%UJ^32ITg%U&jEqFR&gvP%U`QcKcZ_8LLEsuk(u
z_FAB_!j;zYxV=GetZ=2dJZ@hr_&#fR4%$l!5Uxm{6bt&_ZC0euD94nqTcTE^&&i5&
zJ?XL{ecr4{m#7u#3uZ;SM6E~-Ub#fANDaPyiCU4qDJ#lvpiiwx_X*zl9^kSfeM|6_
zz`5ewFYCm;#IWLgN3g_u)r#{yS#d4^rq-B;gn9tDtSOJ#+#A7~a*0|~{vd104m&V^
z=c@0kKC0dCfiIl*?Ze5KuEi$cSR5E+O__fm0IvF|G2Kd5>}E~55S@i<%EcHs)5GU^
zmVLQg_SMtuC1`g1alWwM$5Kd^eh*MFmwx*Rj5h$6ev;A&z;7VQLdqzzQqX=6*(FrM
zrC<F0kmb^^4*@Qu1mHr7Kw#;Y4@@onF48uITIvVt9-=y~S}q5dtXfWEGOLzxT(ykS
zDsZesexM?-YPpv=S;LcHX(^m4E3c1a<@F{gvhwPeqP&;`9_NVN0Bo}I3f+l9G(rUG
zMSv?W0l4xKfGe*>Aj!(hDA`GuAWrntWjFu^DZ^-;i@%_z46^c?rK#2c5x*JYfim=e
z00zp+tNj3FYMd$RVE1LR`d40M!pfCbs%F*OXf*0Ou*%A7FS!b~=sY>(agM{K&|<Ul
z5`Zf&0dV;ia?CJ=<UF+-+(b#Pyk60oxL8Wp>0A^Y(CHl9r|EPkN*|+b(+5p3)mw~c
zD9f5FbY&*N5zc??TUi-g_=G$NlVw@+h~_{w$dB`leIcAK%bKC!l4Z>%02wcvwuRy6
zxXRvLsh2fTn2^1r>0H)?K?O$|(5>{(G{G!uR<iJS>b}WEKxf4*M?W6+q-U58Or|x8
zsntCHN+Il&fjujQ@Z=-Dwv8_`D<IRi=;NDoHdrGRnqoh#)3{c~^iS1KI+p;K_l1k_
z!}G`v+{fAxF7YqwSLU;P0Ec0d^I181fLMW9H3l_QLB=+%g0uXFRaIO;`U9HI^UVFK
z(o0GIUDF$2fA2O$HzxYy(wVG)a%Bay0Kn|}r~p`o91#FB{DaIOzP~CX&q9CTH9)kc
zOW87{Y?&CjA<8ncXodZ><c~^9wYiLRoyry(IZ%9tB?NQI@lcu>3Y{>jKvQH+zD^^Y
zlNSmb=j769&UoC%D!g7xELBQTMONW4omOTtVu#51L#NWXyA4H*i|}-;=M9C%Wi-|l
zHo(I=hp<yt;h#GF2g5BnMCgvl93n9GqRHsupYNeGH54g+f~Lsqd#XlMTSG6Iq!T#D
zo~Yy)JJL%$>14)y{GZRUdFt6HctF}XqGUz#U^8#pB-4YihcNl!a{-0+Bg%QxW|<l-
zBZ_&G!X73z6BDH^Li~m(=S>Q2`mmWd-Qb@$$u#*!o0(#=JwfKlH`)UPjZ#yg8|@*2
z)>Ctz3VWoW32HOG0+UzDCaRfF1?H<nlZtZCA}7g2`4*YBU<x|v)?CmkHJUR?rpjA2
zI(3rFm2Z=&118J02C6<WrMC)U|9rA6mx{5!$s`zaWtlT4a1M+qGN-_pAnTcPS2h3L
zgUJhK(3~o}<%tCL0Z0n*SLazLzdTT$q)m+rR<&qXOdf%q^qW}PG-o}^F&VC(Dhre}
zhbxr*#N|<n!Xq95E_IRaP&S3QJWNTKBc2x%7iVTT`Jg`xT+LfEoN2(ZYEqSz;S>w%
zQ}fmgXP%%@s_sTPiv_Kx>TZ;?T+jqnccYwDf+iMKqV6WkI&Z9G@0OofF_~@9peKQ1
zrDxF6m6PSrqrop-$rhUF@VuQ{e#OekXrTrxTgkPa!8Wg)%=T)~%9WGZUJbgPR1fz~
zl0EpYm1@2@!Ql!k`caII&+5A1A(Ey;)o?M-nua4Jc(Ru?O@<5p=YYZg95U=AD`UhU
z)!d>QSyrWA4ucWo&N#@hTxdurUm5>nX||dF(cCKezn)vZo&OUsE9U=1w|p1>Ct*ti
zD>M9c7wwUpGg4K~Tou+E28+QQTz+_kTF3>LA5{=`dXC_<7^Q;Ck1<Uab{Gm(W0f`A
zL70Qq&V`*U5xKIHmhLV(4nUgt2>^)*_{BP3M#)j7USy*e+2%za^&)${$RRKCkw#=J
zu?MU2aV)u?<ug7#<L1t&_L^PHO7S9Ny~tEAveb*L^CGu;kw-KlW*yQ9%^EdandMu9
zsy>rhT#AS1IH(%SZfA}9#;d5#kU;q=jG{}=rD&Q_l<}&TWss|d&c_V=b?@`nJtqvP
zd$cND^=Ma|1WB_(#Y;SsD2F9Kb_Uo;bhR^}NwqVevD&}XS&Dyu^CC5ds)7jR0x#0m
zi*(coz4%{@c6>)BO^b5c%^$nsUn5PR>?VQoI`m-`D7#6Ze1o_I#%>OILmvY!qf~e2
zIpCv+Z_Psl?C$IZehYEg(COj44*VP7D%kaK-UiM<DNFPo&H+JvMMwMtRc}Xc>8OFK
zk4E)Cm8?-cQ1z3TgMsQ{U8rmfe3%1Z%f_>-gCR)vbkfm+)Bpq%H~{%%hX?V30}#W7
z+UZHqJ3R;&zP}%Rrt><)Fu4wK`f@D6!sN40Gx+w9|4pD7zYsI;0L=(TtyR7HaOQ<b
zs+`pVf5Y5Vef42+_c+`+9E}noI>cVd%9`!H%8*Ao&iW;&u2u=JGMs%Iq#i^mD9%PD
zH~m<}?ZR+u3Au57uT;<BOAF-U@<5Ve`IbWLDkNF6`3&|?JSipvUldYmLVSFQD8$DX
zjY52UrXAwjBk&9OJNFM8i#g=2GO+BA@lJ>}8{JIbg1Bj_x&<*|7V}U)@$=C@d_gzW
zz^D}zJgs=X6~s?e)!5$;bjKH(2gQ!<>xEIxaY8GxNOyuldHTq)L;6T!@ORh;aeToY
zQ2d~MC_=>QnUD>7z9GO%2|`UbNCf-Twf>MC2+5#rus<^CZZL$tIT6LI`Ev_&M&An<
z)@uHegk;OtJPPn18wvU@L^d@~5{~lS0~2fhNw}Ubqz>S}2q*ZKL2u1dgcHLiQjcX5
zNeVjvV{n@k5p@}2O-GRAu<|;f;5AB6)Mbb@g9S+odkC$oW{4n}VRr`s@!4}>VNTfV
z7-(yT+lzrr4XYgvBm(0pX0~BHnNO4;^TQUP&^7De@fa?h3A>b(I2(0om4>|(3#6{h
zbytP;M77ndXRm;^)nPG|iMOu?az)q@B2DZSkSPmmOPMxy8Ia9kor$y+q%y1*kt9L3
zhoul{C&<pQp+wr-T_Ce7?B!ZOI*M(3!rq`co$YIp&+}IJW-#z+N)5I*eAS?UCh5x&
zYgV$*@~x_xTM1ge9J={d7T@P<MlapL(np~+a@82(W1Wgl7DIG0l^ICtHj^4nULcXo
zAB~$JHP!qK1+R74U2X7pJC^i8wKjeTTGcOBJvoDf8*8DOqyCNOA1k7^;z`uXs2_XK
zmd3phYNO_LMeB>|i*(njC7-yeb!Q?4s(j)q7dz!awRUEaFt8J)uWLu?`|#I^TF@TS
zQCFfv21Rv2O0ZQ+?o_PxbQuNyi#rv&PiH=_?WWkfh&5K!lbAi*QK{&&PE;Wxj~jJW
zGw|r^7i%3jM%I6F{bJ}%3()>+eVv77>RYf3emh1-TE0l6*ZPKVkZ(EDza{MR#WMXn
z!V$i_C4i3;j`EEl&-a95d^4E-1L1nUu}uGwaJ*GZz6lnd+z8W%|Gn$+fA4zy-@6|F
z|Gw)n{F_fPp2}U1;kVlisFqWQU-$~YL$DyH4!`gfey3nQr_P{B!0xgcpY>gj;rGb6
zQqPgQ9>a0h<7EgA>+l<MKe*IQjkrJ9z6nX&O>!(XXc|W6$C3VqpGG@k{eLtU?t1(P
z9KZRws3E{5kY>eQkKveg#o||Nh<!vdy97f^IPQAnEs30!2`z?!&5Gk!y~5R9kGsKa
z4*`9Q;ZgE(*JHT4>yg3ME(U$0pNC1Y9N#>D`a!iaOzwINS9d*%@I4TG^PdXyuE%h7
z*P{p@g<u3GAF6bJlQZvn3|DtOif|JMwlu=(uE%h7*Q1bAKpGMt^RCBmb=M<n#O8-M
zrjh&%eJXc7hO4_Cg}e%+D*|NR^%$=1dKB_jka$tMmgZfL;p(nO)~3A&q!$BZ-t`!+
z?s{Y`*&l)Qxt}a|J%;PM9)Cx2Fq*RR2=lV!uE%hF*JFLqnpC6nuE%hF*CRidFrXTp
zcRhydyB^1bR^X>g<CVJ}!_{4n;<*)|U+L$Ob|rT`hO4_C**5HLpvRJ1-Srr*?s{Z`
z^SED>sHYW4qw+T;Az-$|Az0mS7uALL$jSY7Q4hbtYEk#wMLj}9-)|T77!8VY>Tkph
zE@~H%7^nWtOnaP2J*WQrC~wpgMD+c3QM-vGIC5QG)F68@)?WqqjoHskRO%3W0a8j$
zYAhu*X@p&d<Qx5@Ln6id?V>X5dyxF7A*oIn&rTR6_uCx-tNjMZ?-`a@kyh7@$o+QW
z^6x?B^@n1S%3JQYi<0~8ge3;ENM3&!^96Lq9-|F%zg?8vZzn8WK<-zKh4<S<$^CXL
zkv$${eqTyykwV6|#gZsW?za<`<se^CjfMByMalhk!cqzH9fn2SZx<!^+X?kwpgbR-
z@_xH0x!+EVJPgX0q|*7is6|m=k^Ak0<rK)_HLJ4le!D2S-%eN>g516ei&n+^?V{v<
zyBF9_hk$1c`NV59&x+^$c2RP_9aTBAK%Qq*sr&7s<bFG$@*YHf5?q-pl_&SxMalhk
zROsAIazMWoc@#b1!oky#i(R|h%_!SN*oCcq`8ek8T!byy+E)+`a_U={x7S`v7Buga
z<v^~=z-q44xhTV`y`B$sPdEwbR_&_^;%=sd(N^sZgnhPiK69wu-)@bd$`4A%oFI=%
z9ccGQ$|#c>%k1QjrVo{GQWg8@e~D-+6^^u5Ao)r|@-*q%8U1I#>b5XsE6Dd6mROON
zCS5x#ay}k}z^(T4An*6H1Po|UR50JFo%3>$<~s(S8gM{Ncw5*Q#Xl@yY@8pGvNxDV
zv1tWZii-M15a~xTmO3_`%nkV`(GL+-m0f$$n3+n<84r;hBc>|5_OkP4Y3gE7RvW4+
z)&&`J;gr&sXXE7-=yNuaOjGet@<Y@ft1EspM&z(IHPA$pkZwN>o)^eRv#j_92nS&q
ziC7#7^#f2o4N(7tRM>>?&`E^)2Pk%Jzw9rhc3B9A3iUitE(%a5H3T((b4_>}Ua-4>
z(u-7j(Te{VYy7ZT6EUI)bqpvI0@RyfbDhy$z$5;g2g(XV)qRO^tj_R<(Hqx5p?y8b
zTm396jxZ12qF5(;H$s<ac^v4=eioV>%eA5vzm9yX2f$L{`xxl=XX0y)Etoo26`)+g
z7Y6Uf@K@Re^`TgBWSxt7LPc0wf!w7k%LO06Uv-LJ!Kf@OLk$^M0(3x0f*QifwbnO>
zw#eNuDUuy9avZoKS)J<U;pnb&WA)9$(M!2@x9s{oGcnh@n1Ln8u76(^kmW>tcKxR&
z09ipK!mj`Hcpxi@MA`KxQl^whj9ov8Y?lzJXV<S!WEBe)Z`VIgw$(%u)Xl@umy)e9
zC}?-lm(jXJs3zOxM3U6a!_ij|>8frXj=qvevbuRVdM%Mub@OoaRYcO%&BM{_h-9jp
zhoi41lA~@Oj^03Is=9eNx{OGPx_LOdoXBEz^KkS=)@rG`c{utSQm#-p4@Y0iB9z(n
zchJ!5NZG7z9*(}AluEn)oh<eyQnss`hod(W*=g5bMVT!`cBz|(qi>`S_Sp4@kWxVf
z&)fA|5V?uSUb}uVk(-IUX3HJJ(S7VbXtx9K8^d=N)Xl@u{p>MF$uX(19fic3hocAD
z{4UipL-IttXt{Z~I~)HsAm40Ql(gJD94$8wOEY^E<R=Y_x_LNSZXOoueozhtsJwYN
zT5cW|>JOmslT^y&c-}l5EjJGfH3}^;4!@epn}?(2=3$|>0i|Pr%8LS{<>q0b4g-ZB
zUX4j+iRI?uXt{ZqRHx9$sUqQ?TX?@QZ>o!OWBM!twO3QjIDbGwI_2#;Z!CkZ+Ci)s
zpVKrI-7{zkC{a$+A}b~)h{3v^(-e;fV`2qKaGEXv5+_KaQ-?!dOuZm>xg@7)h80s^
zDBGPndl95#&KJ!)ou-Sem<xom%W1mAin&nWJx<g4Qx;e;2|;Wnd!0JF5xQd<260Dh
zpHt_vIlvl=hW$>RCDVX45$vE-Ck%$gGz;Pu*<q(nTnVs5(Q?G8Gid>^7C~%9N1Zyu
z3V^ke{Ej<yE?xqxjc}fDnyn(%Rxk|5&Bl{8Ndz$>H{&CZnD#;RVHBxzfp!dHh&clE
z2QqgK;*QvH$2p%)#=Ba_Fk$%tzcEGhuS(6aow|r%_?1)%$Fa0r)3@0TL2K=&&!T8z
z9!lZtv{`9(KhTC7x~D}uPre8r%>%z(0CI_8i4_&ixyEu{#p5g$IB~tIXe$HT^@dg5
zmFN7No2R*W1LE6;8q3C@hGH3y<<AnP$uJ(rVF$O-T7_EpYY5h=X9^k+2|(3_TiPEi
z)xwDo9Of6UwrCZ!y6`}|&3r9f0Kv_E;d7epDUS^4suc6=Q&Y9B2cU;9jH>Di*dBoV
zhMZYfrF{Wfq;%Lnp(p7)qwAc7I!za^VRT-x@6OS>21CzOzpj8a0{Km{;zTaP*@6Zu
zHt^e2m43NuHdL2e=RCAT%RLU65Bzdx%TK0jI@4#JIi@y$fna?!K;^`<WvYZ*3_+I8
zDd%%DAH*HM(o}mu!9c$fX>;*~s3te*=xl9dHptV+vXae`QyVpdbCV~u*7K}OK)&3t
zs2Q9)cp)YX3=#G%pxhpyat7ySlp5KmKzS}e<qXc9kc{>&vhRcPv7g%FPT226yN@r`
zWL!cl#c<9?c*U>SXvGbM?al>mIVR`~e0Bqn_?onr+{tYMp9nb_q@ji^jYQoR>J+&@
z=4vnGgFf5OBaUjp8$X>=_oQh)xVcjGfAHN{l{fwt*x}|k=zze3z+^uJ^5cfZiZ4Yh
za%bIx5XXVSehZWX0qQ&$>MnT^JyEFNf%0>JIv<5~*OZ{rgc^y65shC}*7)&om3wvK
zGEHp>O1l8{Jk)}_X>&^yl2#1{Wn_T*HvH#SUX2JSt-J`7xui1ms`wiJIff_q?g^c>
z`n4eQS*6ZAho(LHBDx7<yuBS1x&M$YN!{z}%Dt|l{zXvVqO7Qw2}k_nSV6gOgcjjz
z0QAd$K>m&_q9E=QINsUf7MVKoVUHskze<BlLR;L%hX0CtT(4+wU2urhQAhA~H@sF{
zS2nCsL4Tqi#F$YaWs$6M;u_eQ67(<}Lb5X#q{RX0xqO~;L(o1g?UaL5VaRH%h-=;+
zJii7l>!ejY0{U*lW3}i6)lSdg3yc9yR4DCq4uj>BYHS07zkpxFPAAGIY%Uf=(fCzG
z_eT;dZUJh~DGzRlnh?udfvLS=v|7|5`GepaH5u*+6Mdt=lI3TME63cx`7?O69)_H`
zAgv==VuC+AwXl@sR9prcq;oKIlaABQ?O=c8bP+4=7nXc|NLNfv#KJeg^l4Q_Hf%j0
z3ip<fZI~Pg_epTZVR7TJM7TX7zoX+g`$J}EpE=yX$)t!hw1Dvfv1h9lwLn*~rN(`a
zT5+O7*Ps_krgI>Dm0!ad#zHO9GIYMNa~s(I<rk@zDN!qKFmvf2x<kik=K$Eh_lr~u
z@I<Y+E79?st3n@HqzwyCz^h*PRo<v3QnhGS+{bkM-q0DvT6hoy$5B}IZ?yARdeMse
z8S0(?gg(Al^>U{KOl!y(sC~v!wG58C9))p&efJ_Vh_h}6=QeUvtrg!1aos%}3pWe(
zNl;!gRGAHFmTzxFuNRh&K>o_lqI`88Vv1YG{x>3~@VHo**Tiq2POZ4Ts4lmOjoUC3
zPczWlpNXfB-NpD|DCiUXJRE{zSu`u|2wIOj#(v%Ofcaor>1R|%3D0ah%rukDpl|o{
z1eDdihe7!=`!nR~JOu`BJt$|2ALUOuycJvZk5zX$%In^1Ux*?(zd-1shNlY#R8-Yt
z#dU^9aVV!ZIx|U5S4a=1hBI^otHsr9j+MR#!A%z<G=xiND2Bv*BW1-Uv2ncQn_~L;
zdXOF<SsI6!ASIV3FHO)7S#hHob_e<9nEj?#Ao!<WSmf?^;D=3ghRDgEKoP}3oMBmU
z(^=gw*%#@7(1~k=yC(7LF(e=-5QVL{*HE)gE8nw-KQ|%483CDW%86G3wbrr#ec7?X
z;!$~Y$U^X3=jRI;C!1@5NngvhtmS24c>74ic0lB<Gl+=-znJPvEuWxlari#%-2Xr>
zvavBOkmVU=_h7E!yc>Q<*IyIJb@I!Kk<z<cUJ1jT^J>i1@pm|QCi(gNc~~uProMhP
z#_Ddl06fbKpVjg>snctu>k;xAP;NF<6-HXVLhs&J<0^z>`tMPYpYgNELZRg|Ex~fE
zMl0Q|-vilca+dP?BYYrSTP-KEGOvkfrspj6A)I<P1^%+BmL<NwjZLEEUGRm|II^y8
zmO0Q+;@2WALbmBz-ORQ>F7l*~Fz6yR*hR{~d%fYeT7J#mx;FA#9X{^^<#AG_gQ#+k
zVjbKOnXKdZTOfa76i8E$eL?NVL1aD_8Hu9OkAH!&W>e!Nu}&(s^%Lyw3u_+N72B{X
zkG~iyz*%j~XIZ#|(Xko6nYSSkXLQVGS(@;(Ea~))og|-SSpc@RRLS8_mdr7C0g8Q;
zJc?yA$lFh6v08XO%R*`RP4-Rfm%;od1)ijm7M{<t=m=!fIQuwRV4<+Z`X42zYLs$G
zoFQbjlh3k<SR;y5wHDop6&BeWq=q|_G5bX+<45+xQ^mHUfl`G%8N}(7l_<nBKvw*T
zXt4AffWO4;RUohTvq;7=;}0J3wvA~XS#Ae;XEl~LYeCC+9#56>?FIQDSu)$fkx~%J
zt$hj&TD+|<)Eoo-S3i$bbo+y_H29Ji!DQEf{c-qJDuf)r5^}+7+u<*9yA8-4$-?4T
z7ko<LYr^nXsHuL6)sWw8U?b=tA8+X8+65#sNP6m9%#=UPOJKG9q5F}E{^AW)EN+Eu
zF@EtzH6ioG8)oTqlH|iSFM{nYX7DUBh}}!$%olGyM)H@2WF_h^-cYZ75`RLQYbmvJ
z)2S8-mzgi#2wNktG&gKk^S~Exm>JriD_n!YF_K(wF!$zxFWzvt09TN3%?8I(!=(as
z^S~Ex#EV!73hNDEt2C@u^S~Exw89YK+5?VP4VP9J_~MOb4Hed}!S+WLR?Q#y;*IVq
zKH)#V1)@HFr9`SpNq>>?PpYYT;EOj}GE5}LKx+26B?Di)(URdJxdBqUs!B?B5^trY
z_;c6B2flct71a<$`~;4T5y4~p-zW-v@kT3(5Je53B+IWz91w^a=TxaeZu7twZ?xV>
z(YpjX@BJU?wVDUMc%xO;6qT<*-2tkUUO?Nd=FgPE5Awwu))?AhEn)o?Y=4^Et>)aw
zR$si)3ZsOJ4T;|gQ%;m+gXfDk(pvdVr-6pWYVY~t&3L5dkV0#$&c9>+VZL}X$DfQ3
zP$D3UN4nHO7x&3ikxihhp`{_ee)Bfa4(2mBbZ5-;_V~l*pvDbp7=b@3gf#qVAT&c~
z2Lk*I&QXd!h_%JviFP>%V|_hYz-CY&rYL*5eAgS4o9Nq~hGKPTTZF`wOdM+xHT%{9
zpv)$vq$(vyeZYZ6cjf0PV%yIIlmAe9H-4TXwwt7xqv_rFd5YLxl4d?n(Vd^Ci0${*
zM~D?Y_<4%(PCRMSli!{QSKprCk*Z$*!blaDm(oy;7Q6^i9SZ5i?^wjiM=4nI7lBtz
zi2~k~YFB(qOS>rr{i^Z_OK)DZ6!)(>sBM<F7Xwz@b5<XP&Zk767gN2}kYBpk0)qET
z7aRoajYy37_6V7JckKl|(}v?OEyT&+TjVbfv-I)tELtBQ;ShnHprDP|<O%UHgAgB&
z2lVmrtV<t(Lj(@-@svx5k7kDW_{^Y>FBfnhfkOli@$s=qh>uSxae7|<((@s{Ir5jA
zQhj`UZrR62I7HxNX3Jr!FA~P#e?R%`#6UnHKDP7_*>dUQ<0Pq%hLx!*g_-v8vElX6
z=5+&Sh;JmC1I)u@AjEgSXnO>I`}gs2a17CZ*=PIsmP-o%rf}SIKLO*<_#JR(f(E#q
z$>Y;jB)jH7PKWqmh8i%W8GeOP0K+(-EBZq#+8N0?P$Va7gLteeWP;ll$zzQafz;n7
zk{}1FBH8!`1)~;1^4%FAmR8{`%O*iM2Q?wnEVm2{w^w1xMVkwmB}{`?p;Rsp9}QWo
zLxnYn2WaZ{bYDj5zl|UPd@&H2rwl^r+%JIF#>7a9Mj$l;T_CtEr$|GGO|ydAQDFEL
z*zgN(PY}|a;-qly<~D`^4|Dsahd`pAu%&S3<_-Xn^R6lkBZMKu$N6CjXK(IIu&gy~
z1gK{`*;2=DK+fZ3#uiCUDV)o>HvxLkE0FrcEbuL6`>;|tm2>xi{Lbkt^5EM_;dIXZ
z6y$FV3kz?ha1Q7G0Wi3|$7(C(&M2fuBkcl{PQZ%fN+X3~-c17D-4NNVtP}=$Hx+3a
zRns{kb90c!Xf3W0{tzF-drBpxR)F%Hp$br?$?XMcgnVm+P8uQK8WBi+YeYc2wU}&6
z#E)b?(l!Tjalt12AyR%f`cH4tBq@m$KBaIYu@J3`Uon<|70HEG3ZLECEr1U;L;`F$
z&g(Qn?)Z6w5G1(tAIVRb1odKydkI*F@Y5why*2^|SPZZWz*K&^B&ZjoXJ0FYyU+Fp
zFyC1vleCj{uoQdSWh!F&X8d^!pu8w?i<QE|FYXISc+*HpjivCBiu(c5zBK7lo)jL3
zaeqeIDU+`F?b=9>#MoFDzfyjc&Sw#B6Qs2@=}HeDK)Ah-HmGVkA1=6KkT$t$Iv*>z
zvyir^YC0bZxNDHMp=$cJ1f<`DG>+F|4_%&G7h{Zjm_Z0pkMn|*_t}nK1XGPpno)o%
z?I;Q)wxf6D+%enH0R^!g9aJFN(R;cbF-vJjDKF8AMwB+n16F*?w4tneS6+u0mY-_5
z-%}RS9O*{#rLEAUGT`nS@TT%hGYU`bkVcS^JazXVf@}{~%2x^Cy#~CGdh<(PoP^{$
zwq%&W3}ftZ<hN3|Z|Ob{=I^Tra8Sker4pECxy^)ML=%lDw<_IJ5NidW`aPV^-jB^n
zzv#GtXr>k&%vqH?6k-g+JvodH89j2C<?BVz8q7J9J6V%uu4JWfFVmfcw6{%eY)9z_
zu_1=tOv<HCED6BF=L$^b^p6V!J^*+`p5<O{A0{-9(8qv=X3V$TwW1<p)&c{f4!A{E
z?N17KOWmI!H5?B2cXO(}S2R$qhKiv}fyzn^i<KG{D|gt`rL5dxXRsmzxu$SO(k(#Z
zT~#tHUNDOrjx6_x`~rN-Xs`0Yf5w6RWd+}u1^5-kgCf3aQ-#u<!joz4lfYjzY6;+L
z#whb(&P?5-z*#*qEyW{TU#V113ZJIBzmU1B#{*UhPqVo-kyg*7i^dc_?Xa67E!(8C
z3yk3V>(Qa?h$Hy&dUPlo%m{f`Jx0Sgi{dEQO5qa^dnPn&J55VV%?>RF#E}-Kl}C$$
zt&}zi(DEQO?DuL(PuqY9xi^&4!}Q%V0lyroM#wef{{+0py!=)QpBcNqBP|GX3x6xH
zQuuJou7k7|CSBruTB|}R{U})GpVnZ`@!dh-pnGL*PxqFHdq>F!sHTIME+3$(T?<_o
zH_Kph<0#k~y&q0H^S7SXL+(2Kj^UgQA0Zmc$JhJx>$9Bv^2e@t7%Y46JB~9tb|bR#
z0rL^f+D{nC`VrV7yH{b&Je&(-N?j(~qqA}b;<IHK$X2<uw8t(CMph)p)3JQQm^6$y
zCQtI08rwlE4ZjnZIo)$PL?-ste3R7sz4(qGOKGD`n9ol|`!&pi^kewVW-0eTdNO$p
zUrocedaIYm$}yj*`sO2SGW)ZMhXp)Dlco&_vz5GEaR}nOdLSDP+`l24DU~Q{FQnvl
zL`w1Fj+NUBz!L!SW@~Yt+KdMaGH~W4O_De9t^E4Kl^1vpms!s{;wsW4z73CK-iew4
zn-20Lc^-%2(<p_^yO8-7wbnv!pgrYhXoPQ&K({?yR8Qh}qbBjYQ6>5#CX?`*wC}A+
z>OFhpHJZ~4KB3Oh3JNi$<&RzQKFV=fG9183Me`@{-uFt)gPFVhoB~JT7ErCJJ2k1c
z7GIpK1+Rc$L3Qdh3gP0071Kum*sEn5E3J5R>u&7{@%f<L1M%!>01gny25=HU{xL0L
zA5p$u#6mm1N|w4EN-ZFYea}GC?DL+~A|JwCd!QZ_5$5jGZn5%=5hQ<P$OoEe@w}fr
z(!VM)n@wK$2vxNdi^CdIv?lj4VhJ8{CqpjtlUsSnqYXKb=|m5Co*|be8|6zp<c)^>
znJIDJMi2P`Lr(2$q;K|+_Ze~^@7Fx!?+iJ!pOHT4A=kv7hLqcvV#pCA18!+;$N}Yz
zJ>+4AysN*F?&%@VFyy2DyoY(nYYlm^UwWd4e3v12^^<3M$a@Vr!LR&M5BW<&KI->c
zg@+u{N0)oMpZu_g+{BQR{5AHPhn!-_C4TA8Jmfq>-tIT;q=&r9kWcs<Q%qXG=UWZ=
zC4X~h<st7e<XwL06c70$L*D7Pe4>Z^mm$~lTRz`Iz5rccn!^dd^g0i@ry)=E=e^BC
zo?yuJ3_1Tv4_WnEqZm=}MNMpjNMMcSp|pKEWjqr2V^`dsjMJj{^^Z9h?RKopt1(B!
zxipSd7{~S@xX-}c*s*0Gvoj=xBU_LpWI_Z#V|gGZDP4Iw&L1m7`{D+M6i*Hxfqf>Y
z(awNiPHlQW37m6djh_UbWnt>h90mn*?7a3Vd{u%Lqzbz0Uye0>Lp#ihbZ(){A3x5p
z;8n;jo2&b8!8Gd*0Jc@kmHs`NHw~Jd@xeRD$0dHRrx8702Cx-?HJz*{s;iw6eAuwR
zXW0KDdvP|{J7=gemr?WV;I;jrnHv%f_dv5BSb>y5Oj!V627rQyQA!TZksqO4O&0P#
z0AMzy<xf#<%`8J&XN{O4t$np2EvLSU7c!*f`iC^J_Fir69$LFF_$6cQJIt8*2|x>e
zX9}&o@EI67okcyWg&S*=oyEbv{<^qh!6ga=pn_#gp@LgP0c$KxTkfy1QNdlPfHn3k
z*;!*>l3k1~)9lA->?OgUo4jf~tMd|oc_lH0d5s28utUpX1sibfTC$LL2LO+2AJoj^
z+5;N-FI`(A-qErNSdBWD1&cG@Vy1Mf0L=XtrqI&*&%x5*(W((tu`~@7v2-2)kEP+7
zSuAa<k^j<CbX2b3)3o$!w(^i&!HYKE#iqK10?vImA@eztaXo7%m;E&UdGyOP6pj6|
zi}d`hNRocpRnRF|8_W%vfx`E4IcMJle9BHpJPNhrQ=w_N%a}6iMa&Be$EbpLJc1oQ
z&=J1=yNey?ZPFU}B!Ck;%x<RC_^G*173g>$bPC*{#VOA`{LKxJ$j)2H**)fAhIm9P
z8U|Bf1}J!_z}=Li+DX=6lxe)1&E1p=7>}Q^JlL8rwrb_s6+gq47KoRACw>n0tR%>%
ziDNDj$K(vtf((qvkr($+Cy%?PX%ZC4k6kes`WE5Wvna{@HWwBZ`Bxgy&sv!#)o|fU
zv}!DmWL9crDgIi%9*Uj=yFcJh2lvc-7C_w=mxnV`_9D1))An;M8=?(ycDkQHIJ+-s
zyTOz_1i*&?tmzW~9oOvsp8bGZ8@)E0d(+O#V4n}52>@$4x2jK8XMfO5COdbj2a|mZ
zfSJNhV`9g#OxT(-#ysZECOeHOBRh?GNZ4sif@b#{v&+5N7;}K^H0F0<r!lRnvp?>>
zZj8C`6>Usk0Ln81svC2cd(s$FKz15)g|O3@wCdXLc3YxTh-dC4JB@i$*lEnf>g@Np
z+};w;{780sChk>Z%v05kdCI+v?DR}`aLuAIwyLAO)m8t?eb87kiM+G~yfkBfb>64l
z4~-e6<fR$5Rb-k8V^Wc61cI0<7Q|GbAf|c*G0qpnxK<G3I6;hC1X-$$Ba(NEM)(H*
zjcD481>LKSh@M5B8-BB^hr}Y|JW*=ABZw(J5o;{ZBp=t=MCc}HSA2rZd_z2){Wka|
z=cMN2+f3-&A?VwU%1~SLd2-pxRPFevUr944C^Jg9>i+>sisCgNLsC(DFOup-rg)L%
zUSx|GdBlTQ^La%2xK637CJLAd-VYnPr+ETH`fCXPbEo3NIyVH7uoHMz8Hzk!?qj?u
zXdKE=>vhK}<`!(fELE^0U~ujUYB&r{umb6sV94ga>sA0$l90)rL$ObA7gD}L%JfkH
za<!~aJH?H+bHS9o1hheKK)x8jH3aeiJPM#_x)#ZSf2^Y5i7IneR(=bOm~pZCdOuSy
z#Ss17@Vd5{9TgnZa@aAEpE0!B9y^Pruuh{EvIIpRcr$h;DJx9I9jdj#DjC=JXN;*1
z_oSEW5|zM3&O<?MhQk4`BmF&=oBPlW--HAAfO;2vn|lZ;Gmw(|8GzdWETAWEs_x0Q
zws;Zml_@U{BS0_C22gan*Nav`sxA%oF62kD-AD-<@(6&_SPE%JA#Erc?)Al7jTm2?
zvl3c*eZlSaUD_@St-#qG)O`ee;ltMQLE6HN^_p+N7s;SL0ypKxf|P=k+=~EQ3_yHw
zu(~fMD_^__25C`s-qyb81fb|hRbT9#pehqvnaUT;Hj@%GWEp^h>MVt{qmVWfy{@ID
zLH?kTf8>h^9$!f32p+Et!F)=7oacfrqd(X=Zl*o#9M2G7wSEC0zpfTx=inrW^BK{V
zTwT}eL2nyfVgJ^;S`nbGbO8Ao)pT8Kbj_}=>z$wrM{2v~8(o`>t|zJMk~8SatF8Uc
z_qKaN0ViM!Wq)v9l9gw|3TKI<H5Df0ncl+mnVxjha+rRpC;cy-3)3rfI^2@iO`F8@
zhdumLJ?XD`(t~wflK(SL`b9dOFW?3tSaMRgu*|_w#P2>?d95{_^SF3L$K2G)PuA(Y
zsz799|C8IG7B-9dKI2%v0U4!3OG~8k$F9hRk{$T<&*MIVhZjn?ev;{l`;lu?Eyh7=
zqt106a&4#6IquNkoFDVHS)Jv4z9o~ad2aVl9p8e6f2Vu0oR2)RO3@2B_xw#>ApHVK
zvLg6h(4=>;BETo6Eawk--L#N+;Zoqy!F-VLEO8zQa$*FJco#zg4=L<3mP>I#PYljL
z*0b*iO>rEl&e0@zF*vfxF&1j9dFN-Vp0f_gEFVf<bdye#j`^%cbjNgjdDS9zm03eV
zuo)_>v3#lLxR!oI<$^l-0U5nL%HyD?U`N&-?VxL9?_$OQIA}UfLkm`PP`m3|L9Y^Y
zV20&fCwSxc80HY}S3>Kes3`d1kzk&%adFe{qN&(ya-IsF36Jo`-$l>N2mY_%euD2@
zIYWkNgYQ}?ehr(Vtc^8(-5Y#ghW6SH<YpD~;p2sxs}xx~dxJk9#~7sl4vwNrH1X0S
z&@uhhE|@;+6g`)qNCXVd{t&b=hY(HP0k9Z=l{aOw%Ku~p4s?UZ-}+c0Yu<~RJVHq6
z(lD#6>8$*)9Hn6kvWIk-`<LFwV_(CC$5`+2zGJNL=oDDUh1?`96mE)rYw+!~kPEsS
zX;=x1y-suOWU+4#mLh)!YEgx^$Y@@C2WzKJ<24QZu`9lZ#$Pja^>YDT|9vd2zRiWQ
zKV_$@*J0p?1NgKLfFFPf&R@Yi6Hxde@PtKz^Ks*}kI;OM!x;82JA_St-4Wej96@J)
zg6S6z(45fL<X}8>>WAzYjl%Kl%&TAs03(oMe+s2!n@zg>$u{ZmRvZ!=<SG}Vj^xLw
z6B0fK3gxs8!~;NUa8E=^L3NSY<dr|!h6inn%BSTi<(0IwbI3TO_)eo(01Ewrlq|AU
zQ^s_ytYN-VwvWoPLbe%Y1)pgv1)#Erk&;EWYRZ_di~WvPR#TN4uXu#mV|B6rGztZv
z!YQAl*wsa<DJKsaeWdp1*4nt102<lox%<G7#E8=o<D7>j^>h{oUmPeJs(Bc@^5%F1
z=2a80ik5m=Z_x<D-2K9e=RwWS66!Fg^~yTBwkZ3nlC`VPYpX=b&=4IZ^Hyk4mO5r!
zpwxV>m$%nL?CK$o#@*7Kvaq~wX&xzW7hO5jmg^A^Z8JQ?%REHwV9_>T^N6-R9)WxO
z0#=?=pxP62dDEZnuPISfL-UBDB#*$UDgsgt+}K^z%gbNl;ZH)W2sDL2xtDtdDm(%?
zRRmPqNwSLW^76mw;V(M_KMXqT<*!*7DDg9=^Gk`(*E}q7QF|{k*o)+N5CloSEn23N
z2C7UDB;&HMosZ`a{tdFHKF2Z>rxLb=aFen97YKnT3(=dmguDQuCx~CS1o#btxBU{_
zQ!MAk5I$tz1}w|M43+`%W{HUXkE-!iVTXwE!NDy;?EN327+xy{T}X5u&Wkv=hH$@f
zKk(w)5oQyLl)X7n?YtPW4>rrZqW@P&7mn7NqfBx9U^v$KIl4_%I`)v`v5**)S#&(>
z=NPCtjw_DNhNH&Ukev6VcHIypvz1`hTXeOSkf!&LM%Xpi2$O7qqC=eiY}+b&$s>x&
z;XA)abdnhq?TUw>CVqmaa{9s)R?d2<oTb22<*b*=c!*R~#??|8$AIO0rFC)~gw@>G
z<hYFQ%%NA~U%IgkFlH5F`I7K)Efb?0hDOo@HcXr7nMid#hMaj;3vUdwnQS61&yW=U
z3vqddr0_Q10Ka|__}>xR))SvG+mF8hJTq*LAIIJ7nQw9$`fdpG#rH_E5=N?K^`119
z#^0i;OhYSE==|?=yE-IPT!n&mH89h1-WR+p@I|~`(D@)lP1km<lxFk$G*!kDD8~if
z&5@Thied<;h7~_uaqLkXLk&mL_ejt0tmQd<qI*}C+A{6gg^7+8$=9M6^N<l=iH@3~
z6osmP@M<}9jm<%hJahCO^kugN@X!wcx&vtbBY*(_;(h{<4#52x{eh<kn`&LhX;i(?
zA7~U$5PkrL0B{FV9{UB^$%ashmPt;7%!Q%5CP9W<DXo8n47XNZ1&~FmYz9p`fxNhd
zF;dHwfP#&nxs>Bt_aQLITK5zIu1_2OhBf76&92tEquEWYqDN+`LbIRbAJT}P<)3!`
zqsq2SU_VIh${6*PEJcIR=dxi9H$w&@Z`mIC48W8lp!Nevc4b!mu6Jdg08qjr&DJ*J
z5<JV*J2LJcx;RM$Se#J+@>f<X4%VpHeEM7$0dqw88N>Y`d(;?OVoy4a?Aet0%Ik?*
zvy@x@kta^ulv$-sOG7D~hM{jyhAX%ivWNxX?uc^|t|$X_I7qT7vmPn3Df0pVam5C$
zWDi}TH)Lx5sa?^709`R3K>jV&T(OWvFIcP#)Ixbe<-D1KwB;=Tr?GhUWtMy0ajQoD
zkvqKmG6l)ijbM)o{|iR2NA)Mb9yN^sd(?UY>`^ZQC>r23VtO?r&N;b?5A`oW7xT~{
z?=eWwm5QaA{MZ#sa<EPSC7Y)LPciES0Ny(Vpp+?=Yh_;rpos%u6M&(vYZb@B@4d9H
z<E-MO(8jq?(hBKKg234bz|dgVnv#RS{l^qkaaWQ;Tr0O9fNKFv--Qs6t(6q%8gfJJ
zFTkB`f$1?YW&aGOLj=Ayz{dvQY2!aZDlS1&H;Q&5KYW64C6(+#dO|2<cL3;5;5Gwn
z(ZK970NFhMUI@B1ooD9@v`wMfCa8Os>Uf6!N^mXV!TZygN@w%zJr;+r<KKL(!on&K
z;<Jw<Et{v{?_(}jzOnY*BWk?%!LeD%Tx8ONZ_!kGFwEzw<U)F{ke<VjM9lYe4y6!s
zI){Em?=xcdfrv!R4=78jM9gJj@Dsz!XIgG2D2SNbD94ET0vIG>)(!{2e$^2`{!g0S
zL`)7HMZvRG^O1<zLL>jEV^oirM*R_%qFd;1d9a3YDw9SqZv6sa3hn^4=7S^=^V%9{
zL)=4t4S<A+pkifC4%ke@jE&I6=|_OYDFBckV_1*S?`i<B3OeZ`RN2bDl7h75UI3@D
zc%o)^uQw+DH{LicYSz`}m7yd~_fQ}EB_q|Fw1rV~d?ehl57gE9I%<B76p5M_)YR^1
ztd&?}0#UONM5*0t2+$p`0m$!I%^fOg?$N1LqULc5(w63U1*o7pizjNnprxg8e5H|p
z)Htd{&F$Keej^y4a*?;hCw@Uf;?sTtj87*BFg~@5a;>5r-a2`^TAiFTYIY1n&H23S
z#VS#a>j(B*{MZ%upjH-6_sln^BgdI@l2B#+LrLh3!<#c#b@1a(9pDM@xsxLFmeIiL
z1@KeAvyxC-T$l_Bg(B<n7*{PtQUz@k>soW=c=ZrDUcD0dlrl&>Iu%kp1D+Pjvl3->
zb!^_Ht+vJ@*KwhH5MCv+)TswAHA2?K0H)-Cx(OtS&95LuVsq|!IyN8HN|1;AAU3a}
zBF5%5V3OFpod9FA0F2E7FgCvqlEh}CXeaVRG(4Y57@Ln%ma#dyJ^;pM0T`PJ1Y&bn
zU@A6$s%;9@HbGrB)iE|t0GGt()0nEn=D&<@umZ0Vn@gFCm2a%A8k@l^9=r`q;=%U;
zWF;fHN^G9q5QdDLquPvrODr?=3mVCnwrZb`O!CHQ8#*4rWk2vQrswVFS@Wz5eF=?y
z2~9gR=C?}y=vhBknm0}B;hUtHN{{S`n7s@x`}sjmJOk03!;m8{8sjPv!MtddPM7X_
zyGC?(h2U8>e&liJ7=KrpQUX8po2J|1L}`oNQP2|36O!jDZ;;2C6e?aMQM@=8z*NH#
zsVlV}G)bF$15`EYWmTFAs2{IIGXO|(l?t%(=4$WrCE6Tl3T((=x}R6(p{}Sx#q2rn
zF%R`-FEt38#UWPFa$OdBE3Z-`^wuHGeDT>lhO-Y|+0c2)DU@x5nBh0_Obc@H_A=w`
zq6J!(MigD=LGqvQXobdeZ@%Rkg;)%3gEeD$QI$1c*>#1j>cXzL0oMII!?Vo44}MqS
z?J5apQ5Tr-rtmeyCA=xT^M$}=IleBG`!N3^F3WL+CtT!OnX(*LIF{F0`%v4m6|+I&
z-*&L5_*X9RFDSva^15qNS<h)OQ0*XO`aQ{g1S>Dc!!=)VNwYRRWFabYjfZEm=3zw|
z9<+?2+cgiJzVtnfu=a{R_fU!=7byQp-E{IIlf1|R4}xcvd>e;%c$Mj7teoY?uBbN?
z_s-)to3Go{Xn>}_8$b$ymjJ8>P_R==HPceU`=VhvWrVsm#K8E7C*%AB8Zq5nZxWha
zmnv^NxZ!UG+RIp89Ct)Z!)|Tt3oz84RrTzdRP;f4iO3$Us##;{S=`g)nD@R^axv^f
zD;$@A>RhP2h)yLxb_G9wR*ql)PT)px&&)!2!L4T8<oS^JQ>Yy0t59ywr8NRBAK?31
zwBG<ci+iGTv$5ad{31-hA#v_w3xNMBczk2HXpYXDFV|y>gzq@<BD6g^d8EH*JPdn&
zLg};lYSR*kW>rGJV0IIXp?sF~sg{C8@`KC%zK57ROUeP0HLw06Rn{PAVxMh~y?dP`
zQKm#AJg_ue@i@L&=Zeb>d}dd@T+61TDET6rPW?N)`rp<F8^I2vf6lq|M;iS_pL-PL
zhc8wIF|Elr_gqRYG)mC?`TEz5v#|@MHQcYAWmnWiDsLjl=G$Oz!(UT)Yxs*uEsRG>
zF<&UVP)j9XTh;kA_yG{J`BK^HrmnS&uh)7R+)3GdyS70y&<<jz<)wvn%l-mMr1^na
z{M?q4Mcui70DK+9+@N-NZlB233b&T(?AcB@sLn%TRvP%l=3KA|mP>mN0Lz=C1^9IQ
zcNnXM^0s>v)hY3(=kNCLn+*8cEpM_&$xMWKy}-R5zXR?}&_I>fZa0E70$1qSz1$=i
zH=;PtvU@8KiG5r;7_VO1$qK?7xP9g1trf|8g5(X{vAl(Ck2YkdzXv=D$?+(#|6IYq
z=B_oTn0K);r<K5FzK%8poPF{q;8V)rngNiRdvrbsZG4=!&S`;u*qnG<AFX+U_!XEk
zIZ1-MEioM)ZaF=Cv}hQxDHe90`_5tfKmchk*{wplRooWxmo8CFF$qi%nmZS@u?p$O
zz>&QeK*Lr5iUDL2$OCXSflLF?qsNg_%>4kPtrXhqe(oE{Rpy(+TI>1dMx9+K%FG|z
z%r}d<7jUnp?o;AFLp-n_uoF70;@?rK-b<Cu7PhmIOMTcp`+_7q6R&|kc(gd3Z|9BH
zOtn--WK0I5yr}3d?0SoJ@kcs0v=o`MpbyG}Lv^k>Q)Y)*90Bp#l{)Lg8EY-A*FR(J
z<=YIsGGqOXCg%O6E5G9reD4V)=ucQ^b!dFCwa_~I{IspMhEiWBDQ>#v4fLe{<w?Ip
zr@ssSI5ZNrcVDWNS3}#k9E~MOrK=fP{!~rTuOrI2l>8E%_!HEaE;IIlhM4(k!Y`Kk
z!TuMtEyoDpA6R;|!F1#$%mQ$!Av|wk)Vd9m1Si!;Hak7D-fUoleZ0ZJ&REVjh@Bxm
z4ncP2a=t_C3{_ZGCf_1<hDp=6r)2UyVrRI*a_7v)Erq^S(6|u!&AbOAoioyRIq?0!
zv-s5nj*J;%Li@I8{%q$8Ee9%(-fz>TiPEKU#{1Tw6Uh_zLtqL3d<IRif;+W{spx9Y
zpc^HjuC3`jB=Cgh^cm$7eV<S{4+3O?EdcO9zy_ogR2QjsXn;IWKCTN2sg>6^%Cmi)
zQ1)!Tq5T4t-wfbu0viF;Y6svl0PP7Z1u%v{F@VJYtP-khwM_M&61dJO@{OUYAUsjJ
zWejy;|C^fZ+%eQ+PZ2Gz8!gAF1)=VKs8BDB^CrQu7qP`?wLfc1vHpjlA&c}Zhi7o@
zF=~3dNb0>)d(@}QdKA71xSIDU+|DSNuXP^RI$6_8rKUdxv%EdEOtrnt@%ff>v9y^x
zI-n8Ye67O1BQ}$*P2qDoB3SWh<Xu{KCEaj|FB?r)9vZy{26YzdS<T@;fV0|H3NCe8
zN^~649Q&wat?yQ&<NQuWNAShU*?t}Ce9u?a(NS|)c>LpB<NLzsm`)uFXz8%(I<E66
z8{ql*^t`6G^ba>p;Ps0aSDjye%TePc)5l`4vIPe1=(nmC_DZchj%+hCmw>w0CvRM5
zs5*IB_uS0dOF?^C(Xz6VU^Q#yHBlAY8N<<DRJ-%KWOOO;@#w$`yOS6n8^0mUUji-W
zi|aeJ{U!9ke|+D=`0QRtAJ7GDw<Ca=0MwbN=QV4%ZV=A5z66w0&O~h>JI_QtD(uu;
zyj*$T|9-<E-$=4k`+G*a)s>FsiK~^G9TPnHaX#{i=0@NU%|plvuk&uvO3nPX;>CW~
zjZl5mcwM*mZ*x(m^J*~a&4RExr=;+X-8d1}{)^|}WtAdHTJ;5j%ClVFPg=S{m1By(
zhC`QNiygoI3B|SDq3yWVmLlRukR<*dpwJ$As-3w~Y4baAXpQkuHHOEw@W#x=eKehu
z>}qfSJYD6DpW}V#jNuJ21<)c}{RVD_#>HqVegorra)O8Fj$+cS(gYSDZEg<FB`EwJ
z;A~9_>(fiMp4+4?9RQ}<mcpOujp8u=rk1MW_~Y+`zFcH1VK=-FYL0<e0LI@Gq!d&a
zsTP09g9+<}mng-oQ!_YPV|h7hrKT+O7k3kKOuN`q+@;KCo93IV`B=6r`3UffAd4f$
z3)(dqeSuAIhn6UXqnsI$f3hGv@K+25X@WNZP~B#?X5C5F!bo0wFAe)Gva;L!Nmg-D
zja8~7{wOphk`J@RGadVC7YRTY6(gmfx=1w_kq64z$wp}9!ARhbQxN&EvE^|pr!60l
zUye22T-}ywk)M*CrHtvPOW75GV*k9l_TtDo@UyVbHtg33JB|6RI{WO%zGP>Qe}(Kc
z=3BBaU87l9y=ca~-#1~k@>6Z~4}E$zgnM9H(BF)k^hYlq%c=VG8azrf2YA*IHfz-F
zOzCJ(AxY|XmZ0@hbVHr5wVNr0Gq1)Mw2kdE4NL)mwQvhk)Y-m&X}JVE3wN%q(Puei
zco4A30Lb_O&RixVrQmhVZYsOl%;j$Iz_&a!*!)sePJho?T%*D$KVp;%K;`uZLV0T~
zau(%j;L%p^)SPLw`tlk-8^xD_MXVNpitnM~r_ZhU4N+XQMwg8X{`|W&qSw)Ow{tC{
zZ>!;d$UF8KRCV$iRrb$xv$ZS6qndZG@>F%uAXK#+cR3~j{9592+(qH>sles9i^9u^
zzu5@I{YIKb_+a4S$jkY*28Ysqz%z$8_v1Vs;}HE$$UGiX3TLq){U9x8DFkFXtI)!s
zK<8S{k3zJDv6b;u^G|}WRya<}{4Druz}4aAUu&q-GCNnQ(=#VDz6;NOC!^(;VvSj!
z^MRfbTzrllj`qrVN6zKSMgI1h9&ssl!ey3p-Uzl%az$OKxRRmDnHX`0;V34@yj!kN
z)RRc&J|Pci^3@4$ot-|mWMWmKULUPC&Y<pmT%9?KqHy@{;%~K9zs4Y(a}GI+&Y<!#
z(z1ffD^^|`bWX)8T7IfaR1-~^KTeH^he~xn-HB33KNW!e^edzke5pmukXkKTCXAGB
z!F{f~)+&dT&|Qln-l1X+y#2|NceQSnJc;xRNEmj4aNZi`%PDLt?@kXx-AgoslpPE>
zuatMyWlBF!19nsl>hLH0miLf{m|lgLWX<cKo%)avMr)U*>-<lvF1!eQuIj?!c^?n0
z`dHbj+U8xay%e>|(-{6ARb5fIcC73Lw$%tj!!VtjFXb%p!7A$Sk-hjPjlkytADp>)
z(Fdk^Nu&0>4vjaZv2$>`7tq0Y&3n>Ss-C{_$Dc#6bNA(*_)~}`q2kXq5u8B2OkCp6
zwSwDez$N}Dyc=<eH`fV$4snS$*9(3Za20Si3C{Q<>*>uA%op(|^P463zd2%$pixsQ
z(MWdyW=Og*f-_WKx(-PS9h<=*geiT6P)`cVm~@li4Ms_fLQJ|@@NvLZOsb4fF{yNg
zib-2Fe#MGKKQR>DE&Oiz$>rl^<KW&1Zhx(Z{0!MT*de$*VbQG^y`AqOc-x>`UbaGJ
zByH9!-<m<nc_m`trRZmz1iT9sG6`@-qr(aNx7FD{h*)6Qhl5Mlmyo@fw+C&wT4_Jd
z9{fYZ`()+KLC=$wHwPUjz?*|&$KdEdbh%=;k4SHo2zRENgRDWuT)Cs@Zk-ECEkDjr
z5d+rfoVr4<<djc<IbF$|4r-CV%W1E<HOG>iHkT;_8=0KyM&567y4U1%(B$+dbK0#%
z{w}9zf0^<g)$N^;?M<jhWOL^o#6QHX(iK+TD<1lHhOW<HUIBW8b;`<)M{wHbG9CE#
zEXSk9b{<OJTN>eJ>qNySyLxKm$p|XnyBxt(2d<#<gTT;BgUX?a!78YHA5`X*>lQ&R
zw^q@@r(OS4OS-NPYWe8zwNx>oXt@?1hVtgE^&mm$k6evlpP=Xioh~uY-Jm?g=N}`4
z3C(OUhg-R>;q-gC-0)u_SbB~Om+Bg&dkb|Ad|P0TPL~PS7aHLg;W_mBQqQTqUZmD;
z6<we^K^v&IBDrT2$*)h0<(DH8G!0&oAG_i+7_jyV9SQl$Pr_J4E_tPAZ6sgm!Sy)_
zIVvozll9X~Fv!Ary`Xm1+>d6WcU~vhM<6cXN-;<0biU5X*&Hcjd5dxAMx2EUz(IXE
zQVOcGR~yvH!=JJIz(u82eYx($cEw#NfV<8U&OZSE%m$nd-yvq*mkIoFJmUhs8>%9+
zfOEs&xigY0i+6!%<>aVh-Yp_)$0MHNH8+KIouGs7Jwkj^PzK+71#dl(4-p{W@hVL4
zx-d@e6+#{e<MZ&PF!`?SbHuP+rm)|L;U-Cqww;7^DR0KyyHPc+J<#X88Trs!grZqU
zF91XKP5?I&xC6lZ1a1Iuel~!s0W2Y~3cvvZ3jqwCjG|Fh?rvar0?T~?z$*Zz^LEdX
z*Qneu8<ih-ot?-W`+%0110|gSP}>sDKn=G~bNiBIib}ekqwcv@>5H0zI=S8+LS1~L
zu>`u*OKL|oM>08Xu;nGS-Js^*ew`A%^a$JhlrH7FR{Y{?UcQHa&#!`9Siap$@*)>D
zq-oQ?fYsY{(XfD#ALrkZ^-w?AyyNrnDJWWPtb-2|NCr?R7eGe<od_fXC?*gOU>AW<
z08So&KOx=(fR(>d*Hphqi4*QZ39^`-AdZ~St(4zk&OojbN93(wkc)z~q(qYz8sQA;
z24Nk2?giE(rneUj@amcF(etj-<M^t-w{G5bvht2;_t3(hPP6bgO{InCc>HWk()Fr-
zP6dlQv1D(EPWM^4r{kr<FRJ4mllFfnap`E^NJl#_AFZ(j70+qTabY=vB$?NIFX*cU
zMA@}|5InaKU27*Krj?mDNTX_+R}#j{>%uWLIbrz>j9jwC3O1pLSd!(`t;vPfHlXtC
zyK;*vgK5Cd`8B^mA!J?9XDTXHULSq=2E}FNqRvOvj9(89zCOAD97SK;sEBN)>fOaG
zosvfOK{cn5oiXknum=DJyM0K>Dg|4$0QV!a#>l)$sb)*q3@sA*)6O=R&a*Zvb`JE9
z3%h3t!xc&k&Cn61x>rjh?0B(l&T}od4D5MW#?_q%ZIx(SanV-ZP>-hJMw8{yq+yTf
zSu{PVHSq<T<w}#9HUyP#=ITpb^2ik{Z#crO47$dKp$vKBJ;<$Em`u}jy7Z^3H9{|b
zEle0cbI;;MImeV$(+ti(X<1r#{sz@;c?1RPlQUWuqg#qt*T91$YhhZ~RHxIrd3S1L
zu`rC5gFjW?5SCdFGPCCG)~xINtX9!UPab)Zy5TYR2Lc)9jnL`)YhgO1(p`NqDc{ml
z7l(3|lEqgUP4nfa;Q!QG$VS_&yoH`T>Y@+II+^C>t@cpbDvH$D6*`^8&97Ub+9(~$
z7<dk6{vY<<JGzRh3mc!An{sbR!VpM83DUbjK#(G61VRx+x-@Bt1PRhYuTjJTqF_Y@
zMMME>EZ7Tn#R4iS))(t*2Rkat_dNUTGj|dYeZSvdzqP)sb#KnDXYYOXsZ*}$Of|I+
zl%bb+XQtl|#nOkGou!>g3hYb;z>L@nwhEo;8sI@)r^8hxAJ=(}c^U-WM^*?!ney|O
z?vvK2qjH{M0#R8FV^G@}`cZ;v-C$4$8E(A`5<x9$rMe_v2Zwt7Dq$Vsaxr<N+wPpA
zGsB`QLZWY~=uyw+{s^Xyn?rgq!1MRta<#4QG);y9K5`j;k@Bk00KXHlHBWfImBQuW
zEt3I%ekPjg?G&!078j#U`~ow;znhYah8Q_T2l)2{Y<V^=E|@7nBrXChPk;~7+Djqr
zeU$PDs+NX-5)API8~*sISTb66yOpbu1Y5wP!*hWuE;EXwm#gBM(^Rn(sQ64^Mv$$7
zGU8R)epR-Y$_}LrwaUt@GAU45qv=e~!<7+_6$)3$k@Y6h=|J$$`y=HZt8gvY#BC{1
zVfz_yyP`-1OUa{_R#)YIp(s9N6>kBHD3$^hC(UG|fvtiv;#FBcRhCC(hf?}op)ouS
zETT*bRJNST$W}oa@#^+&Rh9(=|GY|-wpxX|tU@VJVXYFlT~VZha`LF9->dTJw6s#C
zU##Lnu!yBnpyF$R89}xR%7|BGKdZ7bDod->{YrIvmsKVOD*KVj$W}oa@#yeVG;O5A
zuTg*#)JmwtP<!+R-^VF-erl-(al3P;i1NW&j@9NyN$fAjhge=aB1_R^Wf5zVuQy4^
zZQ#KW{~#I6eWeO(dj)8CM+^(BH`@GfhpP1<*`oVGK;b(fMfCotk3W2Fy~i+heM8*j
zslvU3Vd_@c^42w>VJd4Dh9%j>-dTym)C+(|>_zKv-B~7w_5)_?uynV8cLS6@nRkOx
z3m4_jd#_T@tF_sG2b1KG6xi%_<{&vMid0Ae@?buytcst-mb(xdgV|8-K5C>?E~hW3
zK$QUFMdZrDt7ta_EKb2WiswbqM*}>=if8NBD^c3-DFxVmiEFj}TKrA>$;#*xdo8En
zb(Q67&>M)XeFwxGtZLp<Y6qp1z~QegE(06KK*A}AY%md~t1|<B=90Nn9XVW8S$IF<
zih#u_s;hW<Q`9vC6iiTnuI&{Gq~=jw+Z(txMP(VlKZDvdfXl<R+#0H-ClZcEAH9~{
zww9u8VMXtT6rKA|ikzaaLb64Fg@D2aH=1D5?|-&thqVS>i9cYPtJ2T7CfV`+PZ)Fe
zx=>dditIJ_f;E&4+4F?u@VcR#rF5E$4cY*#0B(YmIU{b8X3mH=n0LTW)=IE$l0UCU
zTG6!_D0z}Qji<QNc&=(rNEa$^UBl{lJ=2~6uble6jhf^vHZl(d$}~lx@H!Q7_nJOs
zUQAN%eetS1y3xd{sa4)St=)C1yf1aqn9Hs5SExK!MXYjJzIx@9W1|v_oYX5pl%mt{
zxX>xupjJu0drkqK7cN1K(oloY?=ZyL<#>fV+k4rCTEW{Y%<*}%2-_i_hmCqCB@Pyg
z+N-Tn<tPPM<@dJAfpLB;{>%S#KqTe(pvuWA%vq)ynw3M9nPsF(R<<f!**ywK5(+=M
z1z(tOq$RA3D}8NUMbCy56@C>~#J)B7Xi7R;B~H;#D%c81;0)vM);c$t#^w*e@y}hi
z3zogQIh5Kp<|CE6&BN3_C!H(R2@7nh&lUK?g*Mgart{da=VHKbP6hv5IaPRj2~xil
zslOdsIkS|CSld#aS<Vr#^*LDZj-Lprb44oaT);b~2k_N|2NVTx{bhjPoC0{69A)GK
zo_i2usJA@b^sCavb9v{ycYZqEiLBYbXwn_(?JV&Xw=rFh&M5EK`b~v4ERjnpmT?!?
z><82LuRpva3f~K=(4pZ@<zncQPPhivNI!Xu1$M%ZQ7Ejj$wd2~`pLZDT~{NR`*PWC
z!xH?Da<l0a)>A$F%>^?-VkO2)LC<Y9739*MAJXr$_9ULC_OxMv_6$S83??6|Oe+nF
z_fvY68`O{aR_<miw*!U3#9RN-4;aLw&|PZjbQ-iK{WvP-LFczpJvq+0+2}GO1Gv_v
zkFZ>=m(xgcZLP?4P5M&HRV-X||B!Muu&!)O-)CLf3@&MsO6O|`Mxszy|CWDrMaF>n
zp`i<>6()53RJg9KZ*%&H4H~+eSPh5DowmN8Sl^U?S>F<^Pixvzv8KDzpSCq+U!XO0
zXMv4735CKJ{_kq~SjE~{pB_%{aHIOP($@5Vt?7N%RCV*ei(%b-JN%DNTC-~r4y@I?
zGgU#*ZZD*7wsjt|brxKx-ra*j;f4RQ&Wi0O-DI;0Z(vrwlAg9v{riJeu+5rWq;>T}
zp>XHFtm~Sf&7_v^RBRV(c{{zt)-sLCS<7{{mhG%1Ys<fjV!Rf)T6&C%1?}~p^#9tL
zKCv}bUZMW9L!t1re_7L(Am=Nz*Em%$%;v~5=~vyPF+81B(Ys}~&MmC-{(o8LvY^fp
zvB;?wzz7(N%!)(N7~Tu6$%K13Vv(XaR6Q0cP)O731+BY@HTS>i&L%bYC2Q`l*4&(x
zFn8a-G&fIET5GDVW9b%K)Ai}^*qTm)UI|Pw3+$$=S<@yJarfG>T-H4sIHluwH~1l+
z1M&A6|9=k*rjoj`$e4K0n#Ll-<4|raGAIaP!EhbJ+rh$GH;a}3<173hQVnB7`mmdL
zao5UWFYo7j{fEnPN?zVyV25^2d2@^r*v#YbUWY5<FA3pih48n8@Vi3z$3ys9+F7aR
ztq{Jg;(2)do#Gdv-qj)gq+4v$IR$Tr@W+JkwwqG_@ge-_nrOuL58)pV;U|XhzlQKj
zLimWL5%sJ$_*mpZo1F!fL*z#-`GtVoN+)zmzh}uCZ3+|&36X!d<PDZwbf%JJNxeb=
zu5!C!;e~8I`oYDL;IhKHP&MO46z~0vTk%YYTx!XI6^qhtGvg;KtZ*mazeq5rsDaAM
z;yA52lnuKQ6H>0CmsB!+8t;Ea-F>%(R>=3FADahB7RGpA)E%n;%Y!6~1a5x`V0lj3
zz)LR$EKj&Dj!{waYQXYD?BW>j%e)lu@MULV)>s_F+6N0KJT+H@_x6ByrmVzE1)sgf
zE};dicNvz%@*wTSF<#DFOjvHWx+2DFe+icZHn(F5d<kK>-}5Sgt6u?FZZBOc@J7ON
zd)GArH@p(C+}?Gqz<UVGyDiqm_{)C1uEG+0Cqm|}kMWc{vgW)cbLD=n8^pfI8r)1J
zd*Tf-ZWJPGE?zQIHVPZX-Z@u8UT&YdIcApdtIcYB@a?8$^AH2?+1OKCb?k1wR!7w5
zSm20y(lr=S&$z>|yXdl99Uoz<D@(d<cEHo!s7$AK-fe_inAUc@F)3|t)%qTUb{WAx
zWr6j@u0?(CsEDoa?{>eBhU!}i^+h*pYVmpHf*lHQlsz3a>bkz*?GS!i2>)pae?bWU
zeF%R;2!Akye;|%8ifM3JZ%JJRzEB1U9WDg%9wpp(xt8}L)wAI}8xP^$W(<`JVR|nU
z)(U$TV{HeC=$ZqIuSh6y>N`afR7%E(3l-qDdy=TYCiZv;{QXY<xtKlN-KN5PgW>(<
z>wi$~6nk-36zt_J!k1x;NLX{B+){<-uDGMZomF4ZoBgVd_tYN@YZEE>OesuH+}K0M
z7NZ0AVM+m7%BZEIUJQEVTwKSxJrt88Xu%Dd)Ix1W3GViS657Nbg8zQPn*G#zx@x_6
zw&R=|)|#;92%b6x=PN%Cd9Mxloq}{e9fv=jSUnivxy-;JOi?YRO3UOaz|q^igN~qO
zCUPx;qmU!~lwu^Mzdd&@IyJ0lW=PQ~R#E$-DY`Id;m1{$=b95lR^E70@E^tZL#i$^
zc#Z!rA<9*jf+*vqyk*9B%9L3O;_ejG4^dvwS}?O0bqe82#wzfoGN9iUEK(eMjC-eP
zY}}ewuquSF5yD>|!nX<G?+W4jgz%4t@Z;n7l8+TXTXC?ZC`ZR08A94CKwsAxCXG*z
z5aoVLk@yS_QFd91#AjTH@`YB(mCv*g-n&cOmtKOvgeWx)h36GTD-};8i#}99(k<a0
z(>lC?Z<a_5NyII-yer-w*&X2sZfuZpnJO>JIxE23eatBHqDQ@Ea$Asc6I7O=d)qoC
zU3*4|^1Y=<=t@JBN@W@d3Eip?rGcSf0Kvuf5WcIGxRA-XHAESz5)9oFikHy6s(^U=
zqXP8yHjzM2?QpaXwkPm5<KB3uc-Gn>-qs6I{$nZPZElG2o27`iT|<;Acbon|Z~KMt
zEeu{0WkiV5T_x!4G{uXzOB4`qZ&HBXULz9lw&T&f{W|dWsZeeAT06ws*Fuz6EJeKC
z8>0NzQpDSDLzF{?B5ivxgs*W=xLd^Ti$}YSp-8u=qj>SQl>*}JU<K&y1tI}&k9x;~
z6Lg6=VMW&|z=?L3Rdm!JO-iwf3La8f&Kd{QY;(g4kGC<)ujVZ86uqr{bfaLu0_;b1
z?$szrtPh1Koh(ISoxUv|pTU+Qv91-O6dQ`fx_Jn{+)7A4Iw3?^uM)_tf}tV&16D%p
zDhyFxvlOwbBt-ezQpB#(5G8V7JmW77;cFYbX8gJkrM*fp<L^<t<iI-$NDlm^0CQla
zNFWDJI9kTHwM|#l;C>TPQPf)jD%x!o9d$}i3lt3sE1DZpR2y3nSsQdcnnkUxqQdnm
z%l9RP*5qA88gaN&>8+4A=6>v9uVE)qrsxjXF7OY}KceC%9}I22x?u@!HeXXyxcO>(
z9bnmfO%wP5!m{}?@JZ_c%jRo(3Kcz1ST<kNQ@Ht>x!!KRrpui;*nEvxjg1GdT$BiA
z=JnY3xW(9u82E9(a&P7=AxCep+oHuO+!mckc=-&lFBSTmgkzHcmkNB!2EePhwK+G1
z+qTI!VjHxN{L2L9V^nkBodS463b%3Y=&5{e>lF>nE=tZzS#>`)DSU40sf~zLLB|J7
z2E4HuFC_)Z>?YV^T+(A%E4OTMiOjnRb%`uLsDY!lgf-3;2~NRM)zNM*-s<;&=s0#x
za5rP@kS#YTwSR6(-ri{IhIQ=ykhu@Wtr%)Fm|NRqIbGh0C1OS5noaUd9GBX>d#*$^
za6CC%HE?d(7}C5}@a45}vtZmPdQMrTGkmQ87aSjn9C}sB(Q^12o5KZlABuZZ7nu`}
zsa%mS7t;#f@CL$RDo$5AsyJLJTA+Bb?*;{E-#Nkr`(_=@zGtj`g-?a-i`#aT_Qsk%
z2$St~xi#>-kSZFQh%Sdecim?&W#D6>UU$z;=r!_cxE(HgU58D8rPsYA@YRH+*BLl=
zGhpd;FS}H<kg)W+mtFR{Zviju!%mU!HG15{d4vO)-EJqshgWFbf__pu3oHiy6R>o<
zT|%C=8Du$)coSPITy`NW-R?u7|F8w{1#=<(AAwhIMK{BP1-OgOWw&GU&-`{G;Ex5K
ze=GRQ<^ldx;AywvGG-nudjAm&67tfCpIr9G%WnsxyrAM|fxSDxxEhT2JPh+^O`C!@
zihHbK1_*L7>y1Z^gHeetYG~y31Fu{d+YBbd6@9D%R}*ih<@$?Ug_D$n?=)e9b9TnO
zyq|adqpK&bIZyhM{drVof<02j+=FK~A2v0xgYST9>fkdrPlfOWiykpdtfVGf|KBSr
z_%@{CjPfcD{5x9<@-<`wF#KN~Qh_^;WhB#+1KDm5moWw-h}?g>M}tHs?@~!zz$-XK
zvy9#G`8d9CYzR+ZB~q<4B7C8r^JqK>+(Dyra#2#WTq}}(e5(SSbbE`yki*_cZyz#{
zOZ{)yAA~D7q@tWECx`3e7JMM@Dww4vN=<7OU`_X%niAK1u|&2|f4gb#iytwR=d4N|
zQ1Kl>P54>6u>Soa{Z-L(gE{1Hm49VjDXjm5$sM+u&6{|OF?8l&C1|k=F&oh^_IG(I
zYuF@=M*~!iu81%m&4wX6wui=}p?6|DkvI1a^7tm-)^`DxH}?(}xJDUZd9|g1cMz8G
zXoyEeJMIQ7<Ixb0<I#e9a4Xy{jz4^u^mLB~HNO`fUB<i#@|)?;5|%NqK;T~Y0hTeZ
zP~e9M%ZN0^<A~I48{Q#zxB9|#Jn?Z8mPN>(ogsWtWP7}07Bo;`A>6uJCCm|{yyR6f
zaFFzU`(wv)z=i2gX3(xG&OE={h%4o^5aq?VDFvfL_>UBi9#b@4@sf7uE5NjC{)9=3
z=+dK2RP$`k6|D~|dMc!7rcreCo-)$vDL9~_&tZ1DQ$+1=+{vUTiwrDr<Z*SJx|MOM
zgJI!8E-X@?G$CeKUNEv67Wo-;G&d=GRV!MkpnXW<fZ{oj@ORMp#}@Usn-=A9(3Og3
zyY*H4VeQ6FRKD4e`vniEC>=YmoM^}~5iRdk(X%SZCMx_&0X7kO?a{BojImMGj#uhV
z#9X*N+uZVo*-QT1byc6l13tYv9ZvG9R6fT&1B63|&K`(=5bW`O7NqJyq6I$ro9p61
zyW@ZvLM7)+L%5zYp^Lw^raveRp2A#6sYB>6B_}FRF%TR>=HQsRr{b0>4$V_CRB@Lp
zju~61ICj#avlL)#ie@N~nt~SW|FkiKzuNe^ikCoTq2fP}S6whgaX;;KoWDZ6a}}Qf
z<7p<UU$j(lgkhr7;|Ip04LC;Q6U5^b`31jm9AbJ$VW~H61ZMZX{jl<xKqLajkfdTQ
zvRXCX<Ov=l;ZjiEc?JQa+V}6rb&1X>40`~Dd=wHN#A2lGS<nog2?K#C1g7XO)g!S;
zdd3rN5{rfkFq=OFyV_pRJB0rwgr5|`r#~I%U!i#32z#vJnY_FO+bOs|BsI%W|F7Pn
zy)&fe;!y1eLikM~{H44ug+F@tXb8Va@yvkTibtXqlqvr9%J^M0#dFlPH<IJl;b)aa
za_mh7r2qY{z`y8!UUZ61$WF<2ZJW_(G0mis-HMxOaApiE`9!hjE9VmAY~|;Shu4O1
zH56A?PP(3ApHWU^Y+*CSF}qne+REvXk&W;Fg6kkY@vN1n3O5mFe!+AI)9?@MTk}-O
zRiN<CUDxng?6&Y3LoFK~LT>IyVe!N0-qV!%6p&dXyZ2nhjZqxC_xXyOsW^_j8x+Ui
zH(jlGJHk38_bGK#Tyw#rilcvz1`<xm^NQaU*S=_{;<N`iJz6pj`@rEn7m7o^ePgRL
zgf9L%AG3x^o=@-b=dNo6iJ$QqL-F_FcfmpB;cvF~1Vy7=<UViOPkLrd0rt$2Oa*2e
zZa!7zl+;t?3X5FrIp?~$irr|iiTrJHyinyNML%GfsiChTU$#hSC>f&2y$0#wH+l<4
zE6(NyTFSV55lYjQ7@sJrKaVjCA8~mAuJ@MASDyC3s=MRPRh+&A6mR|>iqUIB4K%1y
z+L?GDh?5~Vct>c_cNespP3XlQH=(^A1uP4)RRZ5aSQcak9`YDqS@d1vQIYpJU|IBC
zB8$E<z~-s`OFcdhfuo=C12A@;2wqtSUMB0n@lRk~C)e4pk$v40gk_z#P1bo+w)2Kp
z$Q^nCx0m8p89YGmUGH;?xqV5=<u4eAx`K!(perxIa%8<?J0O6khbcw3DuoGAuu}mJ
zDVGZyjtBnk{(Yz917+VD)>yFkMPmUUS$ok?WD$v#Ic+IAU+Ju|<W>c~39DH2sA2;L
zoT6RYUfnQ*75t_Et7!3}?b#SNBWoPgkg0YF?}-X<u`@NyYYvIbC1Kszafd7S0~O_m
zR22QJfW)Y#+Qt|?E&Bdp>vg4F`V@3fQJQk#-<X2MT2a0VvGT&np~^W;{B7gNrf;LI
z*(rQR1#Ry_*60rC19XO6O1&}Y40Dl_{ay)mhK<NNvjcicc07Zh1S~tCrv>i%6kyo_
z8TeVk(ixudsOZ_J0ZV6iMmodnXKZJ9)^>&wWoKzectJWsqi2!$vLSjyHay!2OaI#~
zaJT0GOaFUQ;QI(mNBKZHO4sKx5T3BZ_|$GMo_~4CqzVT7q7fl{(W($oaEAh1SZ`4Y
zJM42aZuXT-mGUcVTyN*f|9KVX`u-^+u9SB|ly~E%6nq}S|Dbp-FTYkiTYf>Emy91w
zyP`%4NJ7p~Kt{ut?byfc2(9MD1X!%w6>L+uU6JEC*{>{DCA4~;QKD8~9ipsO^$1|W
zrVxIM;yG(Rt#}4}yW+WD@m@9#Fsy$l{;;qfdB4(9Md@Kz<dPZPD|o2;71@ndbg!tZ
zYGA-@;*V*7JM<M_KaqG&rPHDR={G`&|2neB{QR;bcU$#(0r2}1FzOn(-HU+bDK|I9
zQN7*{z>@f0jEd4<0xXH|$=z2|0gv|)ggZgx@nfRgz4QvuG6^IJSmR~nvP=Rga`Vyz
zz;oqCotwn$SU+<p_BdW=xmoHj@;8H4?*xG#dIkIwW&-Xausg=|j5Fj`D(_V+(jQRY
zWt7~hz&!(S|KqDR#7@ax#jl9t3jR<WCzFj9k8Nd@S4<1@O}x)pJQ~X;BG-?tl;>kZ
zmnnf9%%~np<By+L+i7hG%kw}-I-X7?164x$!5jtHLk5UA#>aYpb7bI@oTuy)O--ut
z%CN?VLmD^!qsF2)L$XDQubN=cw4&M}prD%qpRU0zpJ6vO7(9b3xLBFm?ZuBh?~+*5
zU4pl&#||wy%3Ia%3rUUq*HZ6?q*niHspEDU2l*i679%BluilF1NU~k=X!(MnA^aZ2
zbG(=k!tYo7QpBuC@e;u0A)w#}1$KAD`}$wAdEyk^9%gz?0cJz7Fd-Y7{7p8<Y}qcj
zqW_7?@=?SKg9?y{Y?+^wNdGn{k^Z^6OgxBxQ1SFHJ%rz-c=}gW@#0@61*9)dRDjEb
z`mbC6*bkWd<&L5;j4$D-6ZbU*b3!$BRykU>EQBAVcv`kPgrBT<TK2HwX-v@$1*D7Z
zQ-Ig|w~GXV+Vp5aooRzwl=xb_LliVufUOh5I7sKB4UeX%I8f9stY}0?(IBg+(a{tY
z28s&8iY^Q(y3Q(Ud^ANxR?(tsRW`m{E!?Q6pO6h^y0X*GRoLd0DYcR7$e+9Jzo_!`
zH$&4-M-0((_ycY~k8kZbZwOQbh#Qgfc9J#)zo`5R!S=fd&bHv^0GGKk1N};s=f4(l
z&U0l3`juzT*mjI<jVrmYsS#NJ$)ESv%-z^ebp`2^*CWV}zm$Q$1-uTV__S-UN^m?z
zCM|axzHS<KAZE`u3@i6q>@spy0bl0sK43?%x5+xatx8F5ovr{cP1b_hl<j}C_&fGG
zK*=~2Z7pJQtWY#Nq<g_u1!VEGEkw~Z@at%ky^4;v$wH{x|5m6?wjwMu*e1SQgzWJK
zK{Sd1e-@CfQbowO>?WDbV&K!?v<rg_mui0nJY1S3L#|PxT_&?#+&4@tgLbLmRsy>;
z%p-5HVOl8%f4sFlNJnXyA<D{znPOP~MZ-)`DQTDs6kx;LCo2A-VQK{pvsy)82#myC
z$syef-cdjr=Ccq*8>Vt~oZ{{_@rXA}wYPE4%AQce)O;JEl^<Gc;qvm#Ho|hb#=sNz
z0G7?lu|m&&2e52b4EzFN=`k%`DxUPNZJL(lnx>7K5;RRa_ZHY9O>^vfY??93!C%hZ
z1=3NP<~(I((<Gx(u>OmtxlE;`X&z93O>>&4_=l!BK4_ZlDq3J_=G0yEW=Qvf*qbKo
z(lnV0uvTrFe${alr|8duaqKRr`|#aR)2u{Tq|fwpIgpKgU;9i?=_AK~K#+Z=m%wQs
z+CF38I{=%8>FrYOssFJJ)7v)8@b4yIOzR`!e5I=_%`NDg^OjRwbKX*Y!~kRm7u~1I
zHNZEaP8Qw$1+0SuE9rvE)vua%XdL2R@{YFiBrutG<!o23U0imLao-~c-#AwFBc6`i
z0$)(k2bd9lF`n`zTE4~*zZrPM{l!pp7yPM!#J%-frV*sIhbX|-?uuai;|Mt{Xzej7
zI#9&qazVjz#c_C_9pE_=n+tz3u-|6|k)+!UlP0OW=~wRB=XI|^ku}xv5Bp1bb1*hw
zygFF)n2OW>qR+xW#@lhvY)Y*~O64j(o>Ja>xTx_#D5ZWt)Fh=!q|u-ISX0V?>p!t6
zHA~2|KD8-j;A)@Ql$z~Q?R|jFFg&|_O3f*sQWn&dnj5549@>>DwLo&H%jcR>3k5tM
zprq7n^{b{$sdL<q;Ft^@JHcd9s!X}IL#MaWZS}sU)PnzNN=bpKQwGckXUbtImHvgK
z)I*{0F8EdfNvS$};(;&dp#W2AH7fhZlv);~RDTuSBw{9|W-E>p(+dH<LP~8?CXRbi
zG(ddZ`&TJ-hl)!|y%`3)cjBJel=>7YRafy}1f$eYRC~dHLMb%@Q8N?B2ALpQehFA6
z5CgwLSSFAgg+BBvz%qdt_}H%j%LKB~rQ(+Xo21%UKB;ahpHvpqq}miDRW4eVNwrzB
zXvjC3R9ghR9-t)End(_pn^bqY$A73v^<OZVq*|?9Wzgx}?=B+;$Dzx<)ufUFlj;Rv
zM(jU4sTzMLNp(#qzzbecK$7ZEh*Hq-UDFOss?Si_KPFXKkW{Ty^fwWcq$(P!I2i-a
zSAg^Du?RHhSG_$>ZZ+ij0M?`TDjRdG*szr!wjyi%ql{Xi8jC&)0h(3kyZNao%J1@>
z?=DV8dFwp*e1W?V<6Jan&_!z#-LfthxR(-qXYso~@A~2K&KI~h5zM=I+q~L$W%fur
z9gTLO`v7noE^?p7zgT3zFAb$7SGb+C@T;C^R(CI6tG$}YmF`#d@QzS?asjm>{xTFY
zyYY)CQ`!VOPIfMK-$LTAbZ-Lwo+R{b{O870c$^h(8#K)~IgT@qI8ndK-7(PcEwQ-F
zyD(vAUIbD6czjU??!p_;$#JT$YX-zMWMaX&#69~Y*g5VAVpw~oly(r5(k_&or0BEw
zmqufg_*_1|k!G-P_HlUKPg_&-cVICVCPimq^LeTJ2fA!0*l%ofs#d~dhR&xw5k_}A
z#1s5Qcy|LVE1L#L|K3}VG>;~%2XCY!1$LhRiWf~TfWw{}?OGfEyyOja@Gl{Gk|2{#
z5@h1OZ15)}o;4Le;uK9h4l#`EItG`ym(keBGxbqCk40$f1i{2;QgkgrH>oESL^joc
zW4X5ile}}Z<LpWPxj6z7#ksYU{0lM`<z_z0f3W4E(PL0s@<EpU=xwkx`A?RsN0Z=D
z@?R|1jV^@V<U=esjg6)r$0d*(dmF|iy8?8KRYf()o&b5VH55+}pm(fC3_zj)17i0g
zRg;qh7#_Pd0YKE90MT)=SCCi9DefcylVVj;0HnG!czUc0>+uDc6`KvGlQUc_7M*#q
z^GL~Z`v547J)a36+htOp8|#8*ORnxtg|=m}YLv-wmjJjhHlIKvcL~^5$68aSmCO8F
z8#|srYXLUIx)R70U`wnAfi?n^#rhIxE5qP}u@@@?XeYL9kG)EBj(5*PJ<m8PXVQdg
zld}=->d71EWOOk)ZSsvQI?-_q>qfd8jW%IaZlSk6(y|S3Nl~_8B{nV1PQ3^PV%eI4
zy~1Lv0(&ihH1ei@3h$!l&qgy<-jaq)ieAP`=zb15v*T|?a=1>UJ}io^Y6eV|40L+$
zQnGx4PljA>ud=5BkX>#C;!lfI%DufnX-P^BY7k`n%c+__V6YQwn+sAf0;N%9wUnk9
zTRI8Bjz!geMtEXf1!o7O%ueVLv2F*^_#3kBOGG-irzddm27G%7375gUY@QWH@FcEN
zeV*FxWYhY{HpO*eoz;h&*3a>}ibvV>BZ7s#h_n>)e8+1Q=W8u|HI|^exqpGL=8v$!
zUCkD$`6>E^DLS<p_C~p}{~q+=?_P`x{BzeeMg!&~EFzH&GXt|#&invezc1p{KGkuX
zknJr3{S}z?J%GLn6#f9I5=u2~1IE*S1np!6vVH>4uLnUYIh9~HrS<wb;vCbj2SL)j
zHPv$}QA;ywSps71?f|*~X#R_6AxO@Kzeb!o)6YgHKg)4j41g!c9Edn|Co53>Hvq*7
zT=09usmG{isPoQLRMD~zTB$y6G>%U)4K@I3d@$lPJU7Ih3vMs3!Np3$YcbhcPpU2q
zaPsH2xe@vr;JC=S4H_EYLWk4hPZXXY28G7d`I<^$^pHQVt>~@IhBJEqqa1dr>$d;R
z>diQ$dPlNAy&F*wy`LOe@3)}iq6+Qx-Zu?Dj=rC8Dw<zxU4SV;Z2gc0>g?tw7@a99
zQNh+{Kv!F<DF+fx{@lLvptlKQ(1iXsp+4hQyAMqD7+nqPfL+#v4W63tG7A(rfP%Qy
zQKc%F&?-ViW4cvoZw0-1;?}v~5V!7RfjZwsL3CDDi3&RX1cOM59*gu!<E)ny?Z6^O
z8#C?J`g0+IH+t7crp|aDnv4jTJ~Wzrs1UkL9};kNVuJLcGnlw95<njkpePvt`j7yp
zM<FGBXbkmri-CqtBS0(#fb^j;RFX#!oknP#D<x<j5?auQ#!|}+5No5;jGYajcB*J0
zNG;iE2~M5W$OrbJ@$5rY(i5Z)2~epr0Q4aN7WfIK552C=*Ttg2Em##jVjrrJkzo3e
z<sJZT?L(w#Qj~sd0vYt7qR*gD8gK%*qyax>fekpJsy5(im8#HyYe84N*DFWRhl;z#
zRByd%s`o4wsP}FZBr!G~S?_MpwGTa`9HsPq#u}@$MyBdq%mQ_Ogo5aN`N%p8vI1M*
zQVuka{JG~m3cW|{LqC2}A=l5Z1}O>GN!e;bF$*-|Y81qTUsb9?xLyHW`_N0DSI|30
zbk?h`I!|SRI_IMxI(Mo>1)X<+&OiK$HVjK!2$7^Hzni{`j<ff$`&AzWS7Xj?hmlq{
z!-RsnRhwg@j(Oi{!97FL7A`qjaLbUimBnK@l?U_nvF!=jN&476gzWU3#_Vo+22dMX
zorQ0_QRkP2Vyp8wHVO$&`##`yw*!?>iV29z;<_^o1HZ4{g#0z@VH`3$5wjkSnDubP
zll5@KtcN46tcN3Bcs(33>tWBVhdDQUW<AUzwAuCW8iHm$?3wlOO~Baoa0B?P>)|m#
z+x75D7Ii($vRx0eY}dmq+x0NZ!FrfLupTB5tcM8%>tO=HdYC}49wrd1heyG}U_Cq@
zK(HRB!NGc%K(HR>mLOOUcLNZthuLVsdbkMMg7xrR0Ks~AF4%(g@I?TE^)P{8Jxm~2
z4-*L1!_5E$>tWg!tcMq)p2OC|bW+#DEZX%j-L>msD&cyVYu=<N+prRwmS(4(ivqE1
zO~J0R*s4w%1$8CdG{)a(#v(l-y7_*v_%(At^jBgH?>d#w84W@tuN1SWKcW(Tj@EyP
zozGv7LC^Kig_!66f|WzW-v`+Q|9bSIME`5<vHW%D0m(iuP(}S#$n=<hCon1g^Wd-K
z-&`L?`rS}jn*TUfmFfQb&{^4!fXDY&U^!mJuUZeQDc`Av^IE?iYOm(^0yWc5#ilIF
z{}1$J`!i}G{rqj|j#I<`6gq4ARZ*_xyA3c|`g@MSeB`%=4afLzV!2<({|ffk^`C?O
zdj3$9>-(9ot%3g|ENSRBse-}X9|Ct9`!%`B@O!|QrhWoujAs4~sJFSl6|yb-|H7(c
z{nKG{OW%c^t^BtUjn@7oU~>ISG3K=KuYrYa{gJTYIR8E$_Z<7RA=TdB0)LP9dm>gH
z{CUvX(SHfwo&4<>b2|IXh7<ftQF|Bvdkjcj{S;VrqQ4s6o#ej>&D$OSN~n9%@%JGr
zPdWZfj4n?*{yOkI<M>0+q|ZA3a9I8vkbs_d{FC9q3y%K>!u6u#e^M6@ynqGBmmGf#
z?0nhrzXtRQ5(<B>I{qclxzq7mfVK+)aQZdJzW~Z!cl=4%hP>hU7bBv(9e*cGeG?Bp
zfcBQ-cZR!fJN}o5#vaFi1dhGq_?zL!yN>@LINx*psZjR5<9CDV4;()O|2}m5Ul6$e
zIQ~iSXs_c}Lx?_d{8cdbW5=Hj$3AiVC*aYij=vWh!_OT5H>m#H@h=4OzmESG67~yd
zfk|IF{w#R$mE$i!u)cQu>4@|<j=vm^ee3vhVZ?Wie=Gdk2j5ZAe#gHT9)0ilujAhj
zj?cx}kB9_Z`N{Ebgb_bG{tc+%7ss!Hf4@5ZSo}NS_}9YC-yHv2==|OBe?<uXaQuhh
z!$HSC6Hfn$aT3s9j{gFzKIHg)fh=|XhDf(_Tz@9=`CQk38IfP+`d6Xl&vX5OFn+n~
zXTZSoUH@Jr=>@Jo6fwKd^`8gwBG>N=J6E{=L8Q}4*S`q<UF`bE4`&r<sOS>cZv=r$
zU4J5=)vi;e&t&u~H`b{DRb}9brq~62*w9|a)b38~LY5P}s!ls}(-mZidR2QY2C(vC
z0Ht2WASbq}3%onvRU7EUE@9Dey=p_8*rhB-T`!Y0#JambAQT7i$?8TPgYD@?5z*@S
z2sXP9DDtKBzHU>{jtl6$MYJ~+p5}H3y1yl5je)3By%!vh4eDMD3RvBVAkVNY*&<#2
zdPF!jB5f9)i9=Z3i$K06V2K-0qv*_8PHfzZMap+Kcy^JGy(oJmzvy80xS%Ay*C?Ub
zS!b50*cT8<MyQN`6UjV<Uo(ReL8p2Wk~TJX=oBO7)qzMOE9O+6iMYiU*1(37HhWz`
z>20aTt(t>of~EAuX}Dqqecou2X)4CN57ByTu9}Qa%BlCED=b60xTWL~Pc8xo#^YM!
zN|9x(bm}LAkSS!`N=9+?N@4WOK}dV3PvaDj#>_~gWE$(q;c-T4+Z`wd%cI&n$w=)a
zIJ-!y&6AAOu7a~mrDJ%Kk=m^j>e*1ooMdn&!i2RJ-w|_?5iv)ENl{jr6r~AuquWug
zE9E39Cq-#hQj`|q2vfdURZ?^(zU%SG)XTkyN^eC4^+)!$aQ)*3uqfC^da>9HT#zdR
zUxS8n)Ug}IMsn1V>N4{h$ytXfJB``k?nkS?KwIH&w0r}z?q^6f$I#Pcb6r!2)JSle
z-mH9s;d9d`!@vh200*#;Zp>_UXYE%4#v%E0*A0N$)nV1AL3J}eKIG2o1D@*)h|Sk*
zf}aCy?tW<uq%$q_k&a`TS8l}hm4?AW{@iu<K>OqPw50JY;C}oxW>{l(ANTTHm3SbK
z_!LazF|3KZlsK+tg41dMh7b30;+Fz!Hg=HXUNI5SOC)wD@ycR=*|kU<=(tyrSRl|4
z$Gw`+U4UB8L)A;Q&b>%`_r_8TsJYhz{Ril|m!mKt2Zak!SdK#TOO$&e0!@FHz*13j
zqZ0h-P_yZIt7Z$OsitymRnr!Q=4BPsz+|K5c_rjg&E|9%D%K38G}T;b)jUi!?^jSW
z2Wr|3SL775LQ5!snYj2dqy?_*mPDO4Q<Quq$o&i%mry{Bl$5$ob87$-!8T|bJ9rw$
zrZo28#vH@kvi+(^H$}+JHBdJYE2JPdKY)>@vC4?=a*Q_C-NOLQ24ouRo>2gI3lWWV
zFNw$1AuAf|K0=oPk|=li%A{K<!}C;2j6xEg??IRF46mzEUWh{TzRGPHi}#^wl*cHc
zjB0kwvTClUG^3nYPt`O<q50Vr)GV@U&Qro(s(JkptEMldsb-~Bb0^hYQ$dY1R;D7^
zSnooCggnaDK$AIbnkku$^_wBHnb?-dyf)SVHdYnHNgJ%`{VH}Hhl965K%4yvP@;QC
z;*SuMlg2qFjgv|m=a|NvMcjk_*CzH&;XeP|b>~3AbNB@9{RaGO#wWMjgT$xSPjK3H
zRI%wnDh`GDcxY%y1Nb>8i3KE{lg6pFF(+fUFi&M#n3_=4H&Ay1)<Z#6!=c+$RY?4e
z0NV^xF%Cv`fz)s`AFudvP-1WlAmcBU9@mtKQ5`nm4-0nrbJuYP((Z1kI*gR+wtpV@
zLx+xk1pgq{;bTl6Zd>Ovh^)h>qaB=`PSi3S`lqW6STm^pCqOyPM+Q^sTiD-uh4Q=)
z1nq={oi9_|e#IgA+B~7S)@VfRVd6fyxh?oEJ*V?d<!G!_XlnDe;_g9GoES(sCw#1U
z*2vWEz!jEzk~0=OOYsSu=^|6XNqxYBBBp^n7M)IX;#WxSS4HO$i$8bWMo@ZUeVt2P
zH#R`Gn1sU6hA5Pva32btt9@hA>NOy&(>aR!OmTF&uj1%*3_9Ztxftbv><^vCDo<^t
z)9oU~(e3tuoO8lV#RqP43F-EChJmLTpSatS(EW)s`O=9ly$730RmHjDE*-ucl%`0b
zTxzM>2!4)5;dvA~-=xeB0b%{TO>wU%j()zaI40}YiZ6kL3}2nURqCO*mNq{tjvi*C
zauXTn1m{~5*}y|Cg56P0NAUE;C-AT%A2Q?~-swc+eWa_evi$?srh@VtKDiWc+!)>M
z80BGiyA(*Rw{vsFZ!$QUSGsmm<Rb<t4-H10&b<}AE5zG*pdvrDN~2Em5sIUe8b0Sl
zbEd{)ixbV68jC0G#&8uZ8c+IykX<gGbdZo;D5f!g^L&`U;p7~A$0FnOOs!iRfV@J^
zsU!O$2BAAwdn|F?E2wNP@3PM&v92r>>&g<bt}GYp$`Y~1O(60LJO!nFu#U##o%X`E
zQSDqX#dQ*#zxu&8cRT0_!@<uJJm*1t^so08tRwZpwz)=~!1KJc-QB?O@2_v`h3y0$
z<fY4n?F1g=r61+Ob^;IbDu*s?`|%6g{x7+-PcCfxp$pr7=)$%iy0GnsE^Pau3)_D9
z!nPm2u<eI0Z2RF0+kW`MwjaK*?ML02NCk6{=cl;5J;NO2`KcHmF&xW<ZQmEb9OU`o
z3)_B{I{<9vAkWX11-d!N^Q+6XY;%z3=eU<ZwK>T1!xy&w@P%zZd|}%UU)c7;7q<Pj
zZU<Cq4)XkVVw*Y0^N)9VSq2Ar+`@E5u@W;X;-vCIx33qv{S9<l4)**TSu_WG{zkfQ
z4)*+8=sgeiP6p0g=oX!HLv#|$3Ixj=It0rL-M0de3*Fc>u}DpF(pTmD>yAT$$TjaO
zmwgCYxoh5)A1y{oSGle-4kY|HI^(jae`6=kEkB+>TN-x7sN|P)L=yWa10T^d!z!Db
zQlQ+K;RJM_geqktNa)#~(pR>j^qu(j{Ml_G?Oz0268sLpBs!Jb0+mqZiPaQHYD$42
z_;&r44Ja_L5e0r|MuG9bC;BUzz<;N5=gAa%`4Nh}l|`{fagyZvz0s9DpMyfgUkTO(
zr}Em`WPSS^vc87_cB$V8J<IXi;M?_os6v6|5RUlw0u2E<_e{;ZAH4tX-1GmPd;Y(3
z&;Q?^d!CC0LfzDF=U^7j_<c0?J2$%=<8bh~W(=TP1WNES3NWgrZWbu&W%N1&&{mg2
zrtf8RKM~MvGSpZ1s#0SrZtJQM#q|Vys@(`K$DmVOHy5ZAEIN}Kl$P$g{eT}I&}c_y
z@TFYUTz4ioc$y`OMMIoRO3GTxb=QM!OTfBWDm#bv)p6a2fPco)s&OK4vUoC>THnpb
zh<68=-48(L;>%Pe@>zQUrylFhz&{R{t{cO-7e0oED02^?+(;;2-JseSmQ@`iYHDZq
zztvUvL<mm!x5C_QrS@`hV~eSIAp~#ux59I9{UCLSdk6lB*$+bSaVzX()xnIHI>LR~
z>V6xf58~uSlpf>m@-au!>0dxPNb<dmX;u?jHo=Wye=OuoY;bDfqvTTT15zitk@`w*
z2U6!a`CBaAQzyHBVTHzga0i1lB2J!!6aLhx?k&KumE4&q%nQibg_wj=XUeL>T?O=7
zl9+QQ<k=(HrgPkqdJw$>v<EBDGaG=uz-@t(XZID*_EeyM2{%)hxYuwl{1&tW0bP<e
zs~dek&wU&@BY~}gu9Sn1u|e{x`rB|V^&+<#jz5?YZU@ja$?en_gbO99SGbvVjkwn@
z5GDO@Q%ML|z;Ft#(ihWv(~J&%5ayco&}}pB+s<b3YCeUDJ!2bzs8{nLIFj)I4f4I3
zmyz-yfofjO>+vt+Ap+ICn(vYAVFEc`O`aWOJVKzZSBuqT^m2cIg$HQmbF9Q*PjRDY
z`>OZ|R_^9iQh&Dr(5(W}%OZ6Obq#V)1iFtUnNFC)PM9(DJ}e4f2dm2?(kYfDQ>1fv
z@gigNNoWZP`FS8;8nDn>C#MX}l2O2W6NH6V*zO_A9_Y;ckT&GpjaJDxtEKG`J3xN3
z0?R<OddBpD$Q^2RzXka~z+x(Djg$I}IjQ&-79VX~3m;RFVR;=@WGqTW$c5!NkULox
zC#N-=qVz4~flvp6a(bLPl@?rZ9-3MFm<GxMQW<;`=A0rQEEm_X(YOrc^%Yn?g?z@E
zMTo3u*#`2n<ychJpKO+^yI^>FmU;RCc)lQ?M9nzEOEeibHbJgYmFJ+4gpbLF9A2Wy
zxOpCo7V0seG_zFGc(vC+EaUbw&^}b?btO5jA9t)Ew~jH_<EnOQ1K;Z4_`d1|r-Rw?
z>P+qKRK1Yp1g|Eq$5mZH7Nj??$5maq7u~Sbt24-{y6RIz=zv#upi}h{7I8hU?hvQy
zr7YumTrJj6wY$3=)$^dQ+E?T;*q-iNz<h48nXFE}l-}3<1vD2Ph*ai&5#{x`s;9YC
zfX=a`P)b*o>v6w;)olmzNtVS(OG;Oj>v7UTr-NJ+u*40h$Ln!b<$9d(Ed|f@<U0t(
znH~a?Ne45dK}meC$Ln!b<$9cm-4BsB1NG@-uEwvLL5ZM~!^W;E*W)PWeG8Evt(YT?
zT~)5f3AGX$A`72TP-^pfTvfRqM<2ZAB-2z}YU1@cHdoGC^yXNrN=VUM;N3ys8BRW$
z<>bt+2I`=uRh2p&lsR$gpOA`;+J}70rR*i3ToI@KLTZOOm_USjJ1F<Wsb^q5iWRO!
zQ?fI;J3!$CSB+70ay~+*kDWaly-KKGfbw0O`aSH4^=X5dE0^^q!H-mY)aI+unX9cn
z5vLy#?S`PVBURdm0@~d!CURLmED*ka07sYS<Mz$TX@(A2ZRMG;SNLXuXK6V;m<xld
zE!u}+NLa20d1D0@j+)hmbbw#N@-WEHmS?H`0YX)+Xb0R9mUk=}&Bd#tJpReaX^76(
zdhfC7x(hFp@u|k#adHX=gWB<Ad`n>JfYK;V9R;a`!M$22wJRvy<J6wWk%ZB^hbVOv
zDC6VQ?#)1**t)4U$b3+ilFC$fa{3Mfby@=ImFBt{l=X4ym9*-V{?IR0Z3Bg;cxp3e
zoJ_gSI{>=eH$eH&%4R-QjF~8B21GN*tb-Z}%r77(Rn^L+F*8j|9Se@kb54gb!gmaK
zS{;dxEt+{z8@MWbJ;5`u9G{c(1c;dnI}KHqvp}9zfrTxaIb=gCWw{vStIM<0{-!Nh
zik|79EVo%Q;=#+;RaPOYPFjDYU>XO~G&XS>yJdqZkn|I>x?GC(=BFXQ9JkSZ7onRM
zCUAoABVOZ^ovbqx>LPB{0Zf?yuqc6JPF;Yr2~HBQX~b*9Av$Yv0=4IYI>gDEBD5_4
z20B@13s45eXP1C+Y69EpLC{W~=VVP6;&u>Mt^#qUfG+@ioZu`0cLCf_(YXnYVbC6c
z?_UUTzEJl9{FvYZ0lxs4dI`XV0`3PmWd*=R0v-V9le$<y%r=c~SPgKAfYAi67Q^GU
zbsco|BzE^>QKMt91KccNT3D>f4?2^{Qy*p8;+_D8o&i%^$~Q^86k6|dUqdUnqd=Qz
z>7kj#d#p2*bp*e=1mue>OQxu3%9XzN%EF7a#Xa8>ZCk;1mt}QizVUu&JWjdz@XPO(
znt390n+bspYf3Ud#J_rwAahF=v(QU-S1ngvRk4WZiI0Tjh{AC-QmUw{x!VyfAsu5Z
z^jsF`itnlZwp3A9Pj}Zjs_QoB;R{BMUH^-ZAtz+6l%dY}y~xd2s`@KsJI)|{{&&BR
zqW>QEGux8I(0xy!|8FA@Cj_xLvi??W&~6{Q@3vR_d6(T!f&L0@6bA#XPL^mm-@D;>
z)sTi!?Rd^r><kt6E=RCSHSF?w*&*ySNS_;Mh*w&n6xfg%Y7eodQDnQ-bshA)8t95w
zU!inFUE45>cw-`0o~NF1$DWD(g7HlHas`v(pe(?oYcxA$KGbZERI*Lz`4Q=i$3xZ0
zfjZ&MnZFVvN$#Rjt#Jg%g=9H}vE;;LrrxBJTQ$~cY!S%kS{5_)CY{m=!!zx3uLXtg
zi4+A+4yWFv!QC4u^+8Y`k5f7ICXId+!=uQ)1<LyYwb|wpxbbXcuqAtbg1GS~SRxp1
zOcl)r6@YDLq$&PM=k@$VVQUDM)|O2Njn_EPeHnQOQ-bjJ27NGjBy~7^JI!vI51yYQ
zt1-f>z9i8%3w#SKuam_^s<%ENQ^z0ga*(cxles1Feov?Z<Eh2F2c*X=*^HT4XPybd
z^u$FN@rCCt(0>nj;!PW5TV{|oPFCxw5Kc%s{d^U!isjSE`252-5NI%q!K{ZxP4a7+
za^6U&xbS~g)2uzVA>NeaTI!1H`N<5Lw-u`Hvg#beHapq5Ac$?Mu<ZiNSC%a_ZUixK
zvVP~d@ltXRO!5+}2-ojiTgnvJEMo@9t)lzS4K&9@b91od25ebxbN(C<eFe%yOCOL<
zi<8%!1@hEr{nbk5Q?3`2EZrfnCf@(e=0_HTSv>LZ@EW=fDtt2pAGW$fB3#2bSr^R%
z_Y1K%&~e0)H^BKxz#UZOWL;0SZc1$(?!1Fw^00e1j%V#BxqHfMmRt*@TuVNz*2mZZ
z_N3gs%=!<`f`D5_7%nL@X`_5OSq@UeTUcqHPB`8ouwPJ4#L0RGVfX&5Gy~(8II{ut
zvT{6TrJ6OEUO$pLeX%mW2&PXCXAIYWIWpXvmey8BGw&eSlaH~9PLrLi50H1>`)PA^
z0{7~Io?DJb>rX|?dRL{7*97$XfT^H7W8k{_--{-nTlqo^l;YbWuwQj_k%^@cneC52
zR7B)1uzx@iNh68!T?}y0%9&gMo3CLUc-c#_!lhR${o8Ehe}Rgc*lU?CE_RBp(G-y{
z>D|26hmgQUZ()@`ZKRKbU~dY?Q&}{~G?4ZCEO3`q*<?qP@!-5Es48w}g%X9$*1tk2
zFv-b!68kRihbpf((qKFawZHwd>Uc9$P;CqzTMVszswV3)%gd<iI79IHhvBb+{Q?F-
zlNdT#ThE5-dDSMMt4Qme4YlV6swLt<A91pdF9P?jYKt(?vk|@P!1-{%eWdz`bDCW;
z8_YjcyGL^bBW8*WSnomhBP;J@<<13LkIdS(!~PCZTWqFHGfLWsMN*MO>Zm2B+4WRd
zni;ceUC*y1^$v#85mao((;cnB_HpJkjW5hg6Sf6lSz+1Y!;6#k^-Ktykd=#W!lv_X
z0@J-@lza=OFemGKIzA!mecM-G2Gh@h#(!QQtA=y3*4hyvy*_Tf!sp+%y_404Hf+fH
z&CbpPAU!|OP+_Td1Q5eF##`^Rtcx+9NRnIz=^fM{?jAwJEKHA(%epjqn0ZtuyOy1Z
zKY=o@fptpSxP0o#aN*?S0-IF4wY}O=6C#ZQF>a)k<%-^=%F-3&-T{kbW6pe3oAl84
z7&FA1aUd5TiDmcww%L|}e0jhkrZF3xoF}m&O8T;??LD`F=l+0CYReLq1h=QI7QCIH
z|0m#y>&hzS+~ZUqvrO$e0H*AQ<$@A)#rQbnWDS}GxmGFpx`y*|A#)1l;)9=bli<tA
z+Oq^A1=U~HD7gh-pBjj;L7f`?;hMLq`b#<$xXVZnb{#mbIxq)4yE%ezLk7-!3+rKc
z<h>Yf85r*;^SxLcI60+Azv$Vwftg*!eHE15aq6sTpe~;d7lir^C_luhBNu>rNz;W&
zO~J70<D;5CLxV?e9fjl+*`}bh3aHt)QTBZI)2VQyJJ3azRB`^1NhtVovKPbD=<V(?
z?Bk0dbn_7e<5QTF;?i=|8qFKk5_2-VcOQfB(^i2wYK``p(@d%Ff$~wD%A?lkpi(RQ
z8z_h3R35cPN99da+3d!6vJ4*+3|Vbv*YiHdRKcvq7#8Jel6N9l`jc%APTAs<up<|N
zvPVCMrQ}{v+=<|s5%7Hul#}%`lx25|JcT*p$J3!}1?blXJfYN`4K3L%z4~ZO=~DNC
z=^@MLWL<^vHT!YzcC?p}Uk7PVoXmx2b{B8iL?wR@Ql%#9S=?kZ0L#&_<b3ui-qq7p
zq#@Yb9WH{E^epHu@QO7o-l<@kL&oyqFtt<|usE+fS-(t%hJjv31VN0y6w;f@SH<+x
zk!?r`xSK>~Oi}K~z_~x*mXOJ6Xa&ZwkZbXL3N48^+h$jeoQUv{?!}tovJE~*vfWIc
z*|j3$ZAdzRa}>GR4NcBV^)d{UpFGVhlRe#Aj-Dn?&Vt}2K~?d2Dl~?0YRg{Y%{o&>
zHiP}iKqO#{J85R3>`T2n5F@GOU9kTYh#X-aF;h?WdT$#hauG>G*J*=~@v4GI`Sx?N
zK4R0|;T2;D5aClGIFiEB_(bWlSK4@_x5@fa0wx~Ynp(scY2#2s{|t+=w|Ng?!Az`o
z3)mkiFCvY~m^*S}h%{6$&^bMJPWL?sd_*A{<YYC&1e=}hEwuC7?;trCVGX%Ck2<nf
zx}RzIpd}(&YFieQgHDT97(sk@5eMgtR(MLZDc}+&vzx)0$4zo+a@1={Mt2<8im5_Q
zBJU($Eg_V;3xPhzlAI>hX!A0cdflt=b)zMjqtjNE(U$J{Zz0|3p|Cv$mZvS7)AYC&
zpsm=0Z&pSRgzFP<d`YhRSbNjv)N@JRu_o`ql^|R(h*jf<TxLt(bQvqVVgvl3OJGeD
z)^=d)Y+0SA4!N#*Dy}d|xJH8GOv|MTYlF$Xp+Aga^Fd*<ur3GNmE~BKKNW4{-dq*m
z%qQ?ig?}4(cb4auE3lFi%u}c7>6E;qDNIwzm`Lt}R18MMz09mAxq!{K?En~6GDRet
zL26KWNvTfqSNeWXyBdE7o!!0%&Z?qHqG%@2*Zoh5#G;qlAwN`6swjE{N`4F!i5tOn
zXfq`Joo%LLHWdTknTDiQy=kJiDgw|G9}}bhMX%GeE5hdfcLe5DRhcd-Cqmr}s+3;9
zK;ap=Qn2nHjuv8zp=~P*>l(0KZEJU$ep3aspI5~d`ohJRwmoIJ%*`Y@S23{<&Op2+
zTi*qFuVrys*J73)`ip7t6R^LNB8%GG-`Q(Dw>>a!CG=>6Mn^|%qt!dHlfNMy9kA)>
zwMeSo)$Iy&cS^{4pTXw2gMk@iv6;)*>6%;vtE%^Ksmm?F*Ahz)T^Fg|>o&M~5xeeH
zAm3<NjEXkhsbbJ#WC61c@$y95V`RJ3_SyJUi_2i#X0`nRQK{}`x_iL46HA5T7{8>9
zq~1A6^<DSaZ$LIqwIrwOgQXEZ)i^DQan-J*@{Ra%3=6NMc0ZiOY4#gKT+5w|9H2Bz
zbXcof2<FKY*g_@EIAgnY-K+3VlSGVjSAvBvyc0IlE#q^llw=BG$!W)d-H1I-k@D8U
zcg7}-wM$O7=Zn5lE@k5V2zImKsb7|d;Tz|7Ij42QjmNEr0F=<u?K=2WlXV1dTy}B}
zKs2#iKE4UN9YF3Du%MtT^~C<KW3J&&!W{|hnH5-eR)&_5r(iG@&liKdiYz{s*)nQm
zn%bVmE;jM3+v_Ud&EVNqo{v>Ht?|+;Cvo1UlTqD^pzjWN*aMu_d-6a&=Q%LBUx4&u
zd2-HD$R)05gKz0Dl@N+5_=ww1?N2Ctc?{n|Z4^+PG{!KEvt#=`4G`BZWEcQ?eDUwJ
z-*+M?>lyJgEybx{)EoER0j$b`+tqcK0=NmEYFBi|7wmIPy0K&eA|)BqjZjC|y$=G<
z94_P>|LSrL*z2SY!y_F=hm6dBHyFZ;)<v*&DY>$+NC<mzV?HR;NXb1CWjL)ng_OWx
z)mA`6VR=f2ls>rb$;YZ|w0#~bgC}G<@v-U}odjo}lj+3As%vx=oPAoRGasw2(XA@h
zGaF9eW7Vncc@tlk$1`!9SJ0!r@%~NLD%;Gie6BL}Wp`R73@2brV^ehLk7TR4ptO_W
zP6OU-c*O;gGc41$pqZJc?aQ;o`A$rws>d_H#}IY7t0TazL(!BJn?W(!X9lGpLdLd}
zg!#U7Wj2hmdauBU#~yA#ER&+VS1>QiTYdASoFrv^EnZTTVt7-Kka;U%o|KcKEGI>I
zq@EO|l}S<FEsz&|4COp2CrKGE9`s4&<^81SQ^LvUCq-YDZy7hE99NQ}99i?CoE7q<
zoD}6ylczQqJoV&Bjd@Xaki01S?8!8f18z5{nUDY7u~Nj7F(~ClIX>rU6)Y!3uLC|Q
zdLzDj<VAN2ir;zBccjeke6ArQFM#Dee0pxKtA*fa>(V&Q_2S5iX&~}FfHbUug;5HY
zv1dng<_wUWG!Ca}9QX4%kwqE-+0n{K!OA#kocely2<4H1;OH|?hgs*8TFAXhT_WRw
zEw-{OU|2p5orZT1R?3f*0>fce%n>qPo`x*VKS-6=gYu%Kvd}jb!Afk+QL^8uXE}*&
z7|#9|V(p&TmPKf%wtNoXk@r#B7nbe=369!e;J?V9AadxH%Bh|V{e>YZ%0Vul19+q%
zSh`s@7Rbio-RV2*Qq(+B2JJLz%I9z%8H!R#MS-*80%ytSozEdXat`>e3iI}T_-yE$
z$CPpMIig24f&9?nEaw;&j_Q%;LFR><z!N8*LwV#sz<pUB-`NNLSKxThvGB{y$7wuR
z&HoZf8>t4uF_y@Jlg2v+^0}Idv;lmAB{Jije6FP;eSjNW9?yMGWIS-i<?*~bE;1ju
zWfm_M<nz9`$Qt0TE04c|_^rTkvmLh>9hQl*7V}W#c~D-nR2G~xGUxNCEb=Kz-<2cg
z^N=iZ5V#}^S)x*e@_9TKsRrCJ<?%ciiyRBw@#XNnk7tmF0>{Mf!r`S4U%Z*pl|`pF
zC&kEFN|wpc$>%9rWG-;;hgF@nX8|UJ^IgiFHhOxzeCkXolliod&Xn>;C=V!bq8GTI
zvsA*;*C_QHILnEy5ETQ@o^6W=1&=){eC6}RF7g+oPRB$LM1X2<7Y$UarK!+mipEMU
zi<MdyEAMcr%UF4bI~wIst@%9Zi%bJ@ez-y>pEn6aE&y(g#k1g~@qjg-cjiSl0lw1`
zB_9V3tOk77T<}Fc0;SK`u_b^n1B~KS6w$O!-<@gX;*{3S$>*I0k$qtP&8m`m@_E(X
zO~y1-2OkLp3;hNkMAhCOk%j<Z4bNufdAxRj<$1gtoP6FH80kiCd_+{N%jcP7WF&Bf
z7H_hPw=+a$0=LNGjS0MmFtP$Tu7cuvlA@fi^QSV2wt(`jrAomL!7^<B0MdZ7xujxD
z226F_6HRGK563;p6zAY^aaUGSX{^VL?>ym;B%XkI2%o_FzJI{oq$`N)cj{y(=_+bH
z756bGU2RI;ousu=Dm>Xqx`yhYkSnY~d`34pcQxS10*LqJ8(9q8_qYKpa!Tv_NG}%I
zNKQVlAVe+%@1x~R_>Cq!WK1~ANqXdT@QMkKni5QSj3sE`)q}_mNdHw%!whQpGSKjK
zpy8W9!?&t|=fsiJE?5BKBVjNt#@lTp4S{QA@hmuLJSffQ&7F~x0r#~;7MwI*#P7{z
zZscsh92sP7*=N3!QYzy@J`bKF3qf3NB~5j_4Z*z}xZ5n=<l?b7z4daVTvb6e7pajK
zz(LirIHl_8Mm28%j=TlpUaLYZ&gZ?Rk?(*zVDVyXK5wFlBz46^h>v(KIoX@*-AGH|
zSc$B2S;>f8T9D5>i6bX~m~Vy52+mtpBZGlE!{SYc;1zVY7`T_PD+sdeG(PNsYv|N@
zT7C)2SEqAMJMG2`P+n`w*Pn~>HB#<35bxt(C+q_!2c@$@$N)Q7euZ*<IwLmtlGRXg
zy{H(79QJNCDyH@v<aoDf`Q+}7ce|EHI^G>p9PC~U$-7Ls!#tGBOnJsql<zU+eV3qo
zuV^1Uc{$4WrE_{8{KbVRZ!>rw@$$B(a}he^5`<u2f#dB>XM+ta2}&4d6^W6&_X5W|
zl+NBd5GAKFOM|n{gBrIo<@yaANtRS044mnBm8Hbi87l8K`VQ5c#&;Wek5!id^W8>T
zcJjIBiW~$>^^>$g4j)rUGNVLqZgC==0W%q7Yr|wHq+Vy(!$|jcVBYkB*s<WRvV5b#
z*PQ{0Gx0f{t5dvaXgKf58mF%jfNApQt~(DbtMM7ZwJWoath_I4y0Q)mSnmSc2jy8u
z%J0qWj1cV(88u)AZbC;&ycVH#*WxV+aDver&YO{Tsm3~iMmG<~rTCn|%GujKhsc<H
z$~RWt+%r_Ybg@J%RWH#7^5?GWo`>65@EJ!h>oiGl#xo+8FWs==Xm>`K$C+@sDr$}0
zT)Y}iPP}1kzZ#H~Xyn~>w?dqQ={U|+!%<DaYPk9ou!U7P{uXQp*v~S&0)|n<oJnE?
zzfw1mQ)sX6(7*UMlbMvz`()_b4g7wvk9!e?oTex|j>5?(6fITu?#4#E4rMIff)U*?
z7VkJ|GZw!+#^Uf;<hGs+Gyf*BmM2uKiNQHuM#4R|rb1T0nr^o>ea@Ob|CcpQ?`><E
z$cg_C70abRnF$@c!5_}?In5HBv#zSZkkoG+mlA_WTaBg+3nz3Q$IZqipv4QDf!P7f
zD5{@)FCedZf;0J16gr_0_<QU=6V<xb-^_%Kw1km84~*jP%PcUmNiE=SKNb0>$Zja_
z?*tVaM}Mm)H0`ebc7aauH_V{^&LoZghH2_AY2vRG=<l7tjG}t+_j5qvZ|!44{$8P0
zG_d|wPq>Vh(BDa56o1QDpub<CQ1rmR^mkX_Z}Dl`wvV>O3znfCog^07b)vR^@u(1a
zmnFZA_q@no@dY9B7lzEY`4q1Y;gfr)*1VIf*u5cgV@qBgkZoqurhH3|SNdH@`V2#s
zN{6WT6q_BwKM}&O3gJHv;cvIq<n~EW-~I@ZU$W#219I|yla8$ID@%?W*(F4d<!b~=
z^Q`g_A#zho{?vBx;+Y|GFH7!yvXx#EB2TpBc-^;$$jdExcsDEkeu%uulA}E=`I`{=
zX-kePPxwCW_s5p}V0SB>6C%4k)$e%$`PdM-t|fO2$X!C@lPvj*!18_}@)%358;}b_
z<i(c!MG&<`A@Vv)-V%_n2$3JL<mmzV_7M49OWqRH{Y;4bn<eK4?frg;Tpi1D>1GFl
zjQ=S_?qJE!2kDUdLp<igEcwAex?zYs+mg$Iy1RtPS6FiOz_B4A^1YUPAdsFCBEN3Q
zlY+X>3z5IK<mv(W(Gc0kv?+c&K_}WBBDc2W;g($VO^9p;CTkWR_x(kQ(|O@}IPY3^
zel%g?tH}B1uA2{?pZ5!`JL<MfaL$$=SD&5CJ<C$U^5g0TevmLXMcy2te@~cO9EAt8
z!oa<Hrdj>WPv+{UYwHAO^i-^D7YO)AZh|w3=TdFd8oLDZ&P(R|TF3D$s!baV+bd95
zhJrJZM^l$8`z|Q=E=%r4R-Q@iB<me0bZ84!(fmb4%`1~<Sj|_1OEf=5_K8^#Z2FT4
zi(dlGk0jSQ1)6!<_7Pcw!tuZ)9|ufJ6k4D#wU^4_a8mw+GM+5tJr9Lxl$Nh4{gj#K
z;zhF+;A;`EmT}v1DU*w(48O3F8He{*@^37D(l<I`EN&`r+W?%zIILCKAZ*tp_ou~-
z#THi1SiFjYGx1f-r1vU&9-Z5id>vVNB>4kbgTiOPv}%W@Bil5h<!hY9Yoy$tO{At%
z*{#Wa`f8-Ugs4PH3iQ5Fdtex;Gf<dXOXaZql0Tu`LKgDAk3uL?b(L8nm9N168mTPk
zeM4m%X{}ya@(OFQbG%wC1zORV7+QP^3eF@dIG_reS_Stczho7ZS_M*|f?KSD9aJEh
zRr-sG*9OLGd-8nrGzpW_!Pd@#)Xsv`j>6QNR1Oz-<xeQ3miHzULh;(7%o49x6!;(G
zRop<k3~x8P0cK(}@k*GReE%e}6aNM}QyPV<I;DJ7Ui6d-SzM;$6tXyyvNSnJ<NW>A
z;MGs4g>O0wHsa1OjzqJTV;`WJ|4{ecb@figaTPw}DD^!2pZ=HfoCvHj)|p=YR}=2R
z1{WzQOLC@vqR65+W6{@&TNLalFhcU-n*B<!oy%Q!9mKZbBilmm%-o?P_bowlei`94
z2zQ7xgHO@mA$c+OPK>qubJy*L(8~Uyp|x?x1ZPTp<y(!Kr<@oDMuveoVc@bbaAz3U
z5e7aF1Kw}O9;xS;Fwi*+3=0D@Lx3}b5C4=Y<{dK}!K+*0(TE`-ua?8BnS3)Mo(>`*
ztrJF(UC8&*d`qPFX9CZ*t^^V==77Hh{t)m)9?UiW-I$p-3lYsoOc;PBUIToS&gc$2
zjhl`_!3Izlo`%i9dSD&`X7Vj4B%Xjm8443nDCv#DFx7@pRsNEm88;Q^?Vyh<Kq0LQ
z3d2#zMPbSq<=({k(V4pUPh*N4CjcSU52geSnuEe&EQPeEkhT=kjwzq13h9X{e;IwY
zC!SiNcjV@npvoA(J?I?HL>@rSQ``14E;SQ#2Ev{<fWL<J@QiUM3I%zyF)bowlm7!u
zVpoLY3ls*j@B<2Wq9AeER530;NnCydUE<Oc+Zl<=Stv}oy?k8CD#nG?UrY%abQcO!
zE3y>Qo<iDENIRx%3&-VM1^!iB_Q&Hgi7ouG+LmD&7<ZH>PGW4>%&*fXHuKLcu$gOO
znN{>t$Q?9+ys3F2b>;t|T?A3|S|)C_db&bN^xVP%^}K|_lu^nq=_1A~Q%qn?`^2BE
zlJBgNDp-+;lH*Y*x-z818Y8+Ms;H}L;>m;5u@O{3#}-*#H&WM2f2V6-MP2!ctE{eP
ztgioBU8z{;7P$vaEa+T0yBaAW@bT2dcdV|~kQBQHvA{shrLGP~&{e!wM-sj;;5R71
zkj>$l6U-cp(#3WlcmW2~7%cRZMNaA={M9_Jp;K%-1nHd)r%!oFTbbis@p}r8d5mFZ
zvrp-$fg!$lQ3zNTXDYrYgugC?|00CHH-tY%-KEl(Lim9p{3nWEjIIB;5dM!4ep(1$
z6$2?JHS(_v;aezv5%AZB@ZID1qOBo3b$}NmAD?O;&gbA)>#&3ci~PCk7DM<ee1fyb
zYtf3cWJrIE_)h>9-=boi3hxF|r`+PZ70>(6cLsQ#&&|O)_fe&C9yAh4F$T@xGm^WM
z=bT_ul0FnW6-*m}L9MZi9JL+Ylt-$s$S&d+phn}M-9WEkgFKqk&8tGJj=lQmsW?F2
zDc~Ih`xN3dHkoRB<R>_@<t%(}GS9*zYt}BE-5j=ipD4Ux>Fh~3$MvenkCR658`g8i
zQe8&08)AlIOv0$m!VfMj7yx&FfZe0zJU?0P!B~Zg=C*K+U+h(j5rpxTerl%k(|*)3
ziI;eLD^I$%iT71<^<n7zywv+FlsJ>Rw#O6v%2myB<wQ>;S1-{1AlF1*0=`ALN@0+f
z6Fr+8V_63_%Tqde#+YxAtInhWxCZl}vg-s-k#!U)0PQA-j2nr<bKGc8|4JLG40JXo
zDkvU!$c!6&anw%5)5{?Ys#826MBZn}kvV&$*W(Amk=sh=QwjVbT%(pbQi2NaR%&OZ
zETLiv;y`3IHcH^){6Oa7pOx9g{Z><rKrHf?o)CyZWlOQaimZ7M|Aw<oo#H&lgo6(}
zoF7#0OqrxX;XGcvN&(JSuL{!;w-Z`3yk9G~(*4P<bmz9vaR0s8IERh${;XlV1h@ZO
z1YSk%(nfaI<qXHxYy>F0BKIX6DkwwbUjnvpJunk_b#JOl?FGea7rh_EalEj1Z7&?U
z@;-(zL-dSsyp5qF&N$^jHcX3kl5<$ng2Fr=$a<rwTTWv?1yN38Yw7yFXqQ(15aOr<
zgEPvXt2WOEn?RFk?{|QT?@)n#=yiu+tO^w$2r0^j&x$Xq#9wT}ANg$s7|-}1wEUP8
zNRJ2u7v85r@OgZK;p2=`V88&F!{K9)bj__mjpj8w0}k)2<Hum3UMnCk%ncFvF~DZT
z7$&!Hd25CjwN{gu`7kN6Xb=7k0R?spKMjWMQieK-#xi%^=P>G#F=`mEFztojv%i~w
z=uVCDu;s>nfZvb9znlvA?RKC~i}KQD-_rme+ge~=(tL|>T{z}VkMdgZz5xl&$fs8c
z%*(&t8fR3g`duQPEEuSc&Ju9#V1ihd&la%mkOXJ`XOu6M+r-WSEQbgK5={K}vaU7J
zy(7`IJS4aQ4DkXF3*G}}6xk{qA`p*c<&97aRaxD6P<BbQ<tQlQJy4y7LRnBqIvzTJ
z8AY~fM9bGWi-$5EZ~UHUEE_;&d!h@djF!DZWkG?K)fvVtptADE8^o)!jY&pXDV1%D
zK5vyx0*fe<0+l^XWn`<MjChQ!w5)H`D8qtB{=9wBhNIQGW~Zw{DNx~=z)Y<uQb9R+
zFd#KcFpBwj)i7vehgd4f#|=9dxE$wxbO&*q`_UcY6c?xj-^VmdB{+b}Fk5_Yh`P1{
zmHJ|boGFhg4@bl)Z-#&cF>L@2k5jrTAZ%xbfh$8maTSd-?TMUWJfJ=N$%5ki5V1*!
zNMo%z{gp=s=!ZfA149C$Eg{(ii&f1I5xG9G+R~Iqw6zWi+<dqI&f%tX4D(M9@xOIA
zzeI0wSm1_`KoUj+HlJ+_h5!}Z9_HU0;_q-czf|yTSfH~e9$g+E5|DUI4-uD#h~enY
zR~~7_HDO?D7<e)SV9evIA@(ck%HR~_UDR>T*wA2n9g^6Lc`w8`<~0}&SjId9F9$rT
z3_Xj3^-D4C#?wY%l)_q1p^-X-y)48l1;h~cihwTxoLd+(jgAcG1>$l`E?zHY(@%)6
z!`KERxQKukd}YPz6eEtGG?PDf-3EBDU|h%xcNDDLuU1A<Xyy4p$r?1lfTMM2O%(7^
zfU?$GTgfQ5Crn;iiU-Z7bFGIv4yIRC`bj|ko&8Lgkp25lQv2sFR^cNEK0(LXgN`J~
z=BtZ45K@h8V>r&>&s}%TINZrm5OVwn_%SMPK29090RBmCmX{gC6bGJx8D!i<44($R
zoABqC2JpE5L))9c`CNVf<FEI77&Bv;k=fcbg;L2{+C!1F+8Z;nGkCp?w-~#bLAFpv
z3yDlZw2=};rCp`cq9_$nsZ`pP%K!O1_uTjU&YMA>-}nE1JdFFi_uO;NJ@?*ox7X{w
zz?v0nQmb}HYHI&Tst??CSX(dnTPmL}xu3~<rXbni);*cr(!%Y4qc+coBroWV<e|NL
z+Vm=YFnV#~e61EM47G!aX+wL$3v<%k9|T^14oQD2MI|XVs1njPlZwO6LQq8m9DcST
zCO<Yy%n(HyFy|9g-KrX^s!^%JK{9_1X|?-;`Dz402$*U9EwSWDX>~9q5RGpaDl(mV
zC{;&Nk?AXzY1MvUDkj*e<fSyh>1o3)!-hgZ6D+P~wA?|8I%(@j!Q$tEA`%$P;^&ei
zT3m_KqE_R@#rQ>vdV1QoR;vfdh(-O5GG#?`ek}PJX(?!IQq*dBq+(GoK9s5%saVvO
zpc1AtNS?P$N$GJS^Tcf4FHxLgG`rLMho(IYJXWBkMy81?z#@OT(oIu3Hr5I9Uc~_<
zOaGoI@Xr2(@H@8%{28zx7gTpDgOt&*<6UXrK#DA;>JEUWTugOD!0X3?EmG3vB%PI(
z+D8jDoustX8j?x_{Y6QE<rkcuHiYD?p&v*d39yFh4n#>}vqWo5=1`<j17B3#6jmpy
zUhwv`QZlE3hk{IWnL~O}d$N*;srJ)pZ(7o~E$Lq*6}1m4X{2ARN~_yf)o$dg+7h7J
zgAkJ+n<YkV(x}?m#~HO5!C!Dq+KpsRwI3jJn(#H!%S!jLSn@m4CX<{i-G52W$~i4$
z$$wMwh*9RIt+wP{Kqchkgq(GmSR=-U_oVH%$~;MODzi1hy^WGbbYGuV18q^1Ngkx!
zn;>9xFNr1JkaoExA85(%j*vecOa4;Y5KI1+CI3mtsry&4<gcWalAMODo3FZGi-0%!
zF0~3f7zT}u+88uaP04R%41R(6ZzFTBJndOr=S)OibEsTDiDUkLVmVYcxY1x>IaF>S
z=>v)7P}$(mfyZ2}a<Bm<zh;oY-g%ykuN}isFR!uE@M?j`yvjYn^0Il=)Ji3LNh=p$
zFaxt+e8(A#1V6L1x*~~7ukVRvY1J}`ORqPE0C&zut810Sr$w9HjB&9PJ%TD-B`Q6M
zC~+8V1U@uW9mdrH#|?wSpyPc@mD$Yh)HdlV42RO2p8!EL!2YqQ0R6*~+xbIG|G0iQ
z>|mBg{nAXucw@kLOww2~=F;bDkVFGqe4IK0nM9ejnx`T2lq7Aj%7j26%1D69TsRWS
zSn^D*lNe<-g9a^_tD$Ym7^`LZ1v@1*9-s>4jZ%dqK!tW7CV!`9ArJmyRzo+8mXgwh
zzbIu7YTZ4lh>YnF9|lP@K&^j8OfJb{G-bS+ATHgQZ5EmJN!nzYT{=dYNr24mK};^m
zVwnAgc&&r5GTT6A`AObDE%Ie!wa5}+k$WS?)Z6L|qrg7qS&`Jm=9z5sNPu}>L`?om
zN?wUJ!Y{^5`^neT!SR`fak9{X`Q1rRS?2Y}sX`JU^J2v0pQ>5PVg5R3)ND<aGHWat
zmnOxbGl}UBk})eo0yNusg)ke*V$8;PW!6@iwI#D9No_4NA0)y|0%Z0inUO4p**}IE
zo8AY?teDK!CoLy4_JbxxT4V{Z$b%7+|HYAuyaY6uuGP^dCu_#56>tr_8|*p#E^mv_
z3oTIr%nBdMyVr_OFg4SGG57`M9~Z)u4F6<yLLd4G<mS8v=D3KB%Z)(CoCk&j4NoCi
zpxxN~f<azzL<;?Dp#9iBee-eYmqFP%-`ohwQ7N2M2YTk*+XZN}LA}NfgL6n(P99wb
zJ&eX49GmhOP--y@yAmd&Ln{)I*B}b3u@!K4<~<(XG&Y1cxb0B=D<O%$!2I{XV*H>o
zPg9TW1xeLKzm7S7V~hZz>xAx@vyZ7Rk50GG-2s_=#C}|L7L$+IYbd{?7c7)W^F(}+
zAsEVXkFU<M$)s}7iN=<w3;6|SSMLc$26M*mjk^?o&*KLT@QYB~U)`jlHh87nK21L@
zQkqgQm^0WJ9_GLlgGNF@Tr$TJ?IM;D%-|Ep1Iq|za2~NdR`s~hKS3-bpuyh(=iH5L
zv*0SR19t9mUZ3W`gq&%U9hj2iL!ICgN}QRKIK+WjIYTi32A`}>or5KHb9f;Y;Cz7%
zbDNJd)w)J3SZe|XChmnAm`lis?OI9wS|DFccv^7MMC^qwt_*(bs&8mwPD9M;$$G_W
zi~7;ZV`~~K7eiQZclB>DR?5Wwaxn0O;y$I=Y$z&2JE5p>3n=`nPd53v!Fo25FrLR1
z76;I`cmU)1xlZ~uHeJsa))skr_i8%t4B}isP7f-S%qqh1L%Ryt|HuFBE8rLc|KmKs
z!+%}kN;2blq7YY-#s3rHN*czap;YBHLGma20kn^-Q;cStt4lLNYu^C*&tUwHtR)^r
z5e7+p{w(!feG;%#sKITBrA7@NPb^jXi_kw!EOlyd{mEz_QmMa6pJDruO0_T{C#g2<
z{F^q2l$`6%bYN!AQl|c{saZKerdn7x*DEv);!mmK2O*66Ee7?Sq0Pg<S7$OYlKR<g
zfOzr4B(A(2h#Lcc38T8R2)U1bK=3ewv%TQof*%shw)KzTKLq2T?O%cCl-O-|?>N)c
zmKB+%7f`%X5AK4BaS#_qQFtBRs1q=E3C7o;Ul<NX(&CEin3~%GqF_Reap*MC;(E`v
zZF1`Ah9ad16g6usv=mE7A#0krx`ravRK6EXsqr_dIL3Vqs%U`YQ>{BNq*T%@DVil_
ztm*<9Fy~3&hi4k*c6Jy1?YPr!26OHXjU`z)Pm3@T(dY{mO4b&d2ECI1w~yWL!nRL#
zg_6xo>mo;IWw(FO#(9nxK1GZD-m!C)J?Ad5duiB<B2SzFVl2X5W;m>z8xD=pcWb%&
z#+9T{-HIif3h%5CyJ1z9`$=lATP=670=oC%t$~fQ>?7)pok7(*q@4G}az4D?<a|`U
zqukdh_sfRGSmp3~A7d@fImfh~xyR1+h}MYK;vVJvdo1U}TZ8%@)fy4*BgQo_djFrb
zXRjYSQ@yhc+ve=cYmf8c6{I6AWJi+jHsWkU5jl7tbX<w0+7hA4JHt@H^~*2VxW*bw
zRryZq7oMqas3Lx$-f^kuTH+4sgDM)JUl@p({O2@FIewv(yp7X2SuGS%H08J<mibGT
zxdh1kA2L5xvshOW<8+$bMdsmKbS#?$Zf2D9{8EkM$h*|UYlp(n%X9Pf4#8`dKw5&1
z&08IUJ(gfCJ3I(5IQv~rGsfaQeHAh7u>&-ZKl~ym@w76D4ZZM?TAEW5;Qx-=<ID3=
zk>2PxsBL&Yfl2EfWdy>3-JrF<*9w$!z{Ql?^bhZ7F{!94lM3V4Jinuk3Y3~S(zOAK
zd3HUcQl2C@#XP*!|LA^hRqpFzxgXyBfcsI67_O!+;&|*1m+8od*Z9A=h)`4QDze_n
zRlxODPb03}^%iG!NyCqWTjw+Mp;_81gwJXP<g>u=2_{^@LpbMIeEV#N7fx+x>f~P~
z<)P~lhZ>nqmSeg|bKs$jjyXM5Krc)sVM6iE?d0zv<N7gFIG>9F=L8)`jf~K}Dnksg
zK>-bLp9)QAS)KtxZ#a2FI~{<96TcC$PowTJcEZGX8<<{($yQ`y+!=0XQ1T==<Zf}R
zvA(*WI7A2CJ&tegZzH~E7H|*27v2rr^>*N%g6|>Taw~8z!G97rnhM-oaF;p2mAF*u
z6UX;$7ZY!!yuN~e0L~3V-Y9uDyMNqX=z76i%u~4n8X&LhP9)?M*TAax=%jX_q&_VW
zb5>vAPXal|4GF${p3HeM#~mbiv|!G0^93&e9wl@2uqgB*u}s#72;NUDv-O(=*S{B7
zrt3om_W?G~q708KJ1HKa*iMQ^DqdUU-`2>~2%k|Lg7r1rZH1Bzg?R3Am6P&>NvT)p
zC1ll3$U41A?IJ7;Ot@CD{RNVm??`A+`;>Zh173O!&)@2BIAAz5F4>2fTa<|d%jdJl
z;gg0HVN}Ps#zuRNM;Dls_>%pF@wKvLJmH9Rl0(wfkW>;9XTf-$L&HTW1Q?p=qS3G1
z#E7~d3%c*D5)dU0Z6p6jhu<MHSF>b{Os>s%KrhpfolgFyn0as}*Pg)<S?tNd5p^q+
z?1AM&=bmds$rGz>9FT(k!toG|)bHlR*VTCZmp9ajCoNP(AMAZH4zD$3D~EUd@cuh8
zEWO3)gY(gZ2A$dsePT>yY+-l@fO9uT`oy@(>=VQ0vrk~_M@W(vuuE8U7ZLl5N&K9U
ztG;5P{qsIT_K5L<J1r!}s&0b7=>pLoCJOvOAp6HHf}55~ZuFl?f+qr-_CC3?Y44>)
zy_*+fQ8y7!bjU`;9h!jEp*+TEF*?GT9Z^b)*A|VECHY;I=@~YGvFV6+3qJmS+Y#po
z?nf*gajxJM#L^M(5&SoB*FFQV`Ls|JJ$F$BZ+Lz|2)mz>9=K^#EBd8JD{~2YBT`2#
zrJs97@G9WmVHBe_{&mIv*Gh@rg6Kl-o_Ob*)=`RNq{%9EKN7J~yctnB_{UtUCD%m{
zXs(8u%Nvhet4*$(kceCj5j9J5JuA5u+FaS1s|RyEYjVA4a?L{2tD5UY$yH&o$|}}e
z8<^`wlj{s5qSc;oo>89erW%sbLK_!FoJ!(B8+M$*IL`srwX|~h)HQr$1IP3GJ1Jq6
zfUAPkZ6zMGVTb8qB}{ktJZJb^fbJP}UWo@Ccr<Pc9By07==+4G(1q<cDRINqH_?B+
z=A>s%5Q{|~l;vjECr<hWHodr#_&?64D>RK-yx64wFBT7XS3ZvmkB$|^bL)KIkjH4~
zpdQr>beL)Ak#t6Uyv9ym_%LvL*?7p7zcM$D@sbtjso@!|pR53>#PF0;i!esLt1{>*
z&run0c%c`x1<E^4?>oTX;ehU%gKqOjH=bi41fy^4LygqC1#^N~_!HbF-FE<*a#z_~
z-Y)Ha6)`?n7c{Wy5<E`*ufxvoaY3{S{xLG^*=E?J_F<c3*^zX_1mythMGP-SJH+xB
zX1(y!>gk(9X=8TJ->q?MXWccPRe)tzX^|I-Qwf~><(;m8pU#d{*x_crGKK!VGHS%U
zhMK(#<4pKMQ(UhCbipq${}sr49(&f2xn6(R$c!_Ct8$!)Uy5<2AzBv4l_3&Uf+!hD
z4BSY_k>qAc-TNRRN0Om}doCm9STRiS2ZFD}SWzJOPr*nYF0jFJ_*|Lhji|~YZ5eQH
z_Kvfmr5OsQ$fYa^4}nnTWK#vVBbGVYG{N(Tm)1txxK;2jVi^ji3vT!@^pc^VL<El^
zmZ4y_q(2TkY9P!Lyjv9ci&zGS1@fuLhL4ax2Zx6QPY3S38H%>RM3!#8RH7e4v<wc#
z7pv{j%#bF7!@EeNn@7|L&9y{w)p%5Mt<+pSm}`m2)f9=yH49OXYOY5m*GQXdv*y~s
zT#uSu_nKV$5VcElt&&`y+Fafxao~%sau#}($>lvpS^jt0a5y+Lnj%g20_+JyU2c)*
zK*!|{Kim*e^&%sP+^#X)WN+W|&u~)iFe#1gY_^=4?O`Qc;qcjM^_^N)-zYqqzMULy
z2g>NX(Mh=i6Nh6MG!Luq#adzX$PXGmddksJo-=JH{@FYPnJr;i+sF81`W@FJ^Vmc@
zGR{pr|8lKfdfw-a3~=&79i4c;w*$Viihz&jhn<vvH9Oq1KhB9yM<1nEgQL&8)rn^u
z-CysvXj&sdBvZ>brQcnS7S@lmr*AYvF`M7pRr`WWI!RwP!4aQmDp;8j#tYr9QQ{a^
zDL6LAxTR{;G4i8W;Y&@6Y?aIoK*#tQi!#S?jNd_xLCLp6x9KT70lj_&a78Y9XUIY8
zWMa9wYi0^F_F4%nH+<ce(g%1RvE2T3d&&Ud_keTc)~{J9`M`Yce*=n6KYNF8OnMw1
zi8FQscdx>vdI&5Zyi=0nFv%&nz{PtD3p$`mBI)}I3szELOaE)3nS0-rVkYJL3(efy
z;$oe9TRgSMc;dTL=!y3~Ne2l(KUc)hSxrbke~;kJg6Zey2|oTQrUTw9@G^mP`||~l
z1Rk}&6SP|(_%UF!WLT)$%_=hP{ecwD{11>`-1}m|E!WWf)<*s%f@czodtWN}72<1Y
zoe$*D{2$^fbk{ot7p{f7ZiqdGH)g=*G7hW~MLtG?83&#aMrS;YB|={xN3j9}-Q<~@
zpPQKm(}TMdPp4c8CbCyN4Ajv8_jMmbOl~R2V)k;YJ%eWL`5|RQUpNU|=s|Gw6ZNcx
zo&z0ERdybdcc;Tgc<dWjhDQ{Z%LBs~)J(pO-IGhAm!!a5pr@4oWw-pjE;r@(Fq8ye
zPq}oamcInlQho`r{A&=C+Z<#u<<D3r<?qnkFrw$X30&wZWG*TPV6cy-(`yxII@)+>
zyc6%g*}^cP5#KOG*v|OR2FUx};Uh-eTDBsh@CZf>qpYoHM9cv`lD5_hMU^?gGF7^i
zR{JPL+}k9vL+{Oq8l$<kORm>!uC1DjU#tvnH@Utwxf&wsam}?`a@AX}vi57P9?Z4d
z<hlWgoH-!sYt8kI<SMbb>Z%1cFxNLG*OMmKK198I7uN5~Fv$u2B>Chu%)w~35scR`
z(~BL9_5BL+vh*v9My-qD<Y&>iMs~ju=i1u3y0*Zr1huNO%YTZ*f_O5oD>%?i&T{Ks
zV=6lhMd2^#Ro@6NC#`P8bK3kQz~(m}G5HNOOI4F2wkbXj8d$%zQspUCJvjH_gY)}(
z!8mzRg{L5Sk0}u!JdCte9^kf%E7@MibrYXiz(E>cS?p4wg!@kzrm+O(sznMg>*|;S
zq*k8>QBr^}Yyl*|0#te)1(>T@$|=BEpg{pvD3vY1_mKi*L<^Aq|5N~Rer>Nb1<0a%
zZCw4L5>@^>s3E100F|GHnB2A?i|PE&8--t1T`&yDZ!S0)i=P)&`1os$wDK$-we?zS
z>t(dHsM@-<$y`iTTUGzks`{H%RRUDC(FUlx@=#Ut5D(inP%W&gSIoAmUKLgKmj6*z
zzxowMO**jd;Ea|P&!*D^Z_z)ZX)OOdlV&_5j|yyilUZVHbMq&+jGAuCf6My5W~JZ#
zUbAw#cR=H*X~jECA3%Kgs#ZpnFL5~gLLJD)D|W08^N0hFLgbL-kaIV%wEYDE9E9wJ
zA4f<YOqi`w-i?*Qsz3BW3X#GWZ}_Irp${dPUU`U=&^(pGGWr*5W2gCR-f0Rf1C(lJ
zYe0IqbonyP&~L0Qkomj)_x$1bHpZ~`N#2gJ&x)Xk?XmGd8@X=@QBQe2%D-Lnvm&2T
z`K-uAPW)yQZ=96BTT@v4J(>gW6NVe9d$|^a0QMgKU#r+`u*X#JaFww#R>uEU!H1KP
zw?uoXbhbdJzy+|zDVG^5a}VZAk&R`B$iyzpENs?Yr3B1$&=M5G&-lZflnd{c22$Jf
zT}<83hs&KR`172c<4w-S7$?fi+oG_1K94wj?lpWWl!$JdHHF1mZ&Rd4qFDP2vG?Q<
zY&QgvJ<MVh+tX<!8T?}zNY@5~sUYaB69zV~3@l^dxm5PePD&%2vVR;bR^+6#F)3Ug
z=FM>8IksZ9?mov?m5;7g?S((t{C4jKt;SG_j2&3B%?7VK7CCFS1@PVGiM}>9_{8TM
zF96FEeOC((z6dN&^cj2+u{_atjild9EKl?q{1C8tX75^)A5Zkjn#95c^UU6Lnwnys
z*|RV+N1oNaz6O;HmSp98k3;F;^)==StedmCSl|MI4SVmvU^EF~t_a)JXoymN0r@=9
zG3csSfw(?2a48{AKn)zUk?R;N*V;?godU5O>mYEz>lN@eAXjf41tW~5Stkjb-MY>a
z-VQpmOuJF=nQz!-T9+DT5xH-iSybjI-apPPDsvSV7n%c|u8OA?7W~>D<?Ju~;^5w~
z+Q5O#z#5$Mm4bg)c@Ekaoct!_Pr{TwI7skA#A}K31)s7B_+Q#>u;3!#f|sTP4-p*5
zEm(x-C4#k6c^f~va-qC|KDM3d?pavn2Upkl`yR|-_;~pyWRC{;c==UtaRvjjm`C&r
z5s&#CU&T4Iy<x@({(`U8IC~x#@>QHCK_;&e?I69pRnfMCA+KSmz2M#&Q%O$wC%vum
zB|!Pvh{^A$S&}tNj4ro<Ms;~xDI>c4U1N>a<#96SD@~u0UM_WhqNCAe7VU6G>dE&)
z4_?Jv_Z^V(s^yLh@G9O>2;_a2Ysimw^|064%9l%>-&QGLBfsF8sZ*?^`Bu_KE9rYm
zs?_P&B&A#(k;K;q7G=lSwp;3MN}_F3HmhwVK-*r6nEa)hr5xK%0FB!Avo40Yl^9G(
ztubHgc?p@*w(pW&n#JomG3487TyDuvdRNJ>LcrX79ah`cvPK%1`UuJ8CTNngI#x2k
z>ezxn-W|tS9W^4=;V-|=C`Et13GCr!<A%yD7;%_Hc>c3a`d*u^XI}e}zRgLmx<K9i
z+HshCVLT|tfbyF|P~Q@4RD!7>XnVcUfCK53hCsF<bDem9x&sDTE-S}k?3m}IOmR~}
z%bgVeLkE0l$m)%A&KhmF{6RITt<jU;ls)-ADXB~%am=%EEN5GI4t*KRm2KwUoRiDM
zb@N!R_!97J0rlPGu<28lO{rzG@~CXWi`8L`6c+8EiRaH&ze5e5FUxwaWqk}9UZA~`
zY~mM|Dd=L$=I9#wldLO2z1CDV!=dGTC%&o1kHrSgV@`aiigtH-z^SnUZspFD9Jn~X
z4Hgr!?6KFa+DE)F&y7nuJC(NQqsudIG}X4#WW*$n#?^<z4!u|4@XHTAe#N45)q&eB
zK35mPInDu71U$mX+Wo;vNh{Uvu!kuJos<SPWd~EbsvBVkXl+t>AA~>1iSO>j&v4>L
zIPq7he__2nr}13npn?9EdTXpsglsC_U#9y1Ln{i+RW?$C&nc*b8oWVxq6V)zVh#GX
z2K}mCO%2l8d8!Z_Qf()`NaN9v{7aqqdrZ96PG=|O37aCdGuTOa%cih)LgSs3&}0Qt
zsD~Bsxw~4Jmht#CVPvFjnM;7nw90igv{jC%c9SWSEStm|mfIrnaMCO`*~7O?Pl`tW
zQl^P|p?WGI46Aw3nX-8SW<nOO&s2h+Jo5|8e*uL!bFnjB-j6}WydbqXmDA;x?*YpT
zPzH}9mKUH5eu`LLpn6y6e<YR{s0_aNeT=VdRPI|Um$TUq<O+%@NR|_VEdrB2z+Pa8
zufTmMH%SC~cE$B?A4#||(z;?}(4hW6{di<FQ+aRT^ksYM+y^jT@|CfdL111iTdWko
z@(X^J`V6Sdt7F3Bd!^V%9$%(zw>)ZWu{<hti<7(&9$%;a8{v_6z0HsQ!$pfZxsP7J
zi-Cgo`hY|B&MZ`eGJ62k#9v_kRf{ppT&mT;w+4Gb@}M7vL7kTJLCF`0<;IPca&5t>
z9|B9Yv`S+&bs^3k1^QNLd>?N%u~gHQlKv)f_0dSbQqn7YgjI}GTWd+@dr3*fNNz33
zxGO40D)A~!P086d1F2VOYG!U09_3^;UL)#U3U;G57J%YfQMUlND_&4EXfBXkJ93>6
zOMPE2!L7_EC;e^H_QJx|@bsJ9Rj@%#)Pqlq4o(}fMCVv7K_GK19|7}@{T3xn5?g|N
zSlUvO^L~KaNY48KRtq_2>JM}`HCfS;_ek4fxo@@H4@S5@spRlt@(cD&tAL6U?x$^2
z?rjk8@<yp5{0=-@0XLm`;vxCp6V4Qm%(HU13bcfF+90?|{7V2&Lw3P`UBEy4)>H9P
zri%PfMF!^QL1Ny6WnRk6k?X}SPUk!Bc%@V>7rP{#Z@35A<<5fD_$p?zbgmf1^vyMh
z)n5w<7Sx;!TeXpJpi99EQ-InTlr3A-XZJ9MN>eq0*QTEY9iqFqa)C&GELluCUftxI
z-eZ(mWWm!hJ%%tpmLw*fTC@t*!UC=US}e!ED<90l?O+$-#;ya{4~X~qp-TDQ*dUf#
zKK7MwLipI%V21G4>k}~W236)EQ$X~;jbIo5sN)7hsL`9dUxlZY)A=>aUj^n7vbUY{
zF;iO*QfNPcA-pErTaq(AVRAtP_Xf^+o4%!wq_eZ#M(bOckh_oFDIb-SCsi;XIpm(@
z?4Cx)wp`8*O&_-$O^??a?+1ZgbX<Qs&M-@n`s*^iXSokz<euf18Q}fKwLis;fxJ2Q
zX63d6^zr77<-d?7cfsEH83Mcub~FNcQ<c)(E89^HG}^fryW(oX&wCr)T8L$#*i__R
zNH)~)MFb8ZiM@Dvx3ZAq`@}wm9|u=-n81I!czL!e+>mZ+So+e3pbzgk{*wCe=HvRG
z^C2Rno{S{9czFb3<l^Oz82A}9Rd=YN9#C$)r6}H9x;zkRVyM>`prO7+An)lIL(%oe
z8Y)W*2EQ&p(I<H)*igg92;|3-#NNbQM_GuWu2yhNhH`FV?nfW;k}90S4U`e-Qyzh#
z3K8Fl`mk#~i9jw>2i^+w5759n5a{{^0t*n>gg_B**{|En)K_!R#!p-DFc|T(32D2)
zC=%cu`Ry2U4g%8<$bVI{HMZ7Z%JU>4?H33*cjUjR#8U2k3T(NL@#_mc`PI$Enm1jg
z#!uZ#h1(!0em7({N8ow{hH_5vh!UeA$}g$8OevU-V~$i!Jrky*pO*>uVfN&mH#SMw
zh5MUftH<S%S#NI(G{#-qZddqNfi39~yTZ*n!^AxJhaRR%axnz0Q}yJa0^^JQ8h{yl
zFzA>|216%ngP@6v12-t5<Ufp#fhOflaJ|q!Y6|9sbNvm@HQdLjFvk1`=UNAj^~c8@
z=Q;#-6z4i#+~H|o62iF}m@5#@)xbLi(z#9$>Q-M_=W1{XuyL*v%Q{yJ6LQ07pyFI7
zi*tPgImEdp^ffx}KzeX?`p1u<LC|%b{xuo|o!M6i^lpe#@^(m)OY9r(K`-FN@v|AA
zbNv|sajvH-w=n3{xo-RhjCzBp&bJ8AXLLm%uc=a6=W2Y$@aV1CaIW?9jBb}p1B7DK
zXFS<Z!^H?3LK5p-n<xwEJJ|}3sYPvwv?z0{eREZK1Bw>BBRy^<^r7qeg!<6AUh^Fc
zm4y@d(JR!sK8qM}t|#wBfX=l80%EB4%B>jm>Rew!niwkUdo|P*2;}9(7>dX0?UKqG
zs#FVB#<}(<8)`Tmf&5sKSm(OLHPjpb#Zazu{R1<RA=XeRTJY}lFk1cKYEYvGzmfVF
zbjjU-lz}GzW&HrV)IuP{z&QwPWZ--RYX1liI}hwil$|#VmU{3#n0@dUi0<(b{<H>B
zkGE{#B?JaChF^M}iNH91P5D91Ur8HLphciZPsFbT!BBpN`4EEQ!Tb_4e!i7`8SZ8N
z7Lw`daM`SXP<g)G2+!l9d>p9{-BU4-jvwLyhoiU!uC?d=7i-UtG)sR$AJaW<gQHt4
zTBsjBf}?vy^B70>haz@}9a@g^9bG{`BTpUO`KRHfIaU2LlLALqZ51}lpK=`C9@u%%
zkcMarrRhuv?*pEIBM!yOiC={w6@NjTg#jpdzt9i-2{<!~p90RI;e(4LofFA5WM^SQ
zZVxmSuFIB)tNRfd<N#=?z#D$n)%Jq|9|kn5ujT1neVy<Ndhup3r~qBNa(?@MIuB$f
z{0fK3=Kb*iQ`Uwhkoj@?t|!qJ*}Ojkfi&^d-@u^<Qge_b&HQS_NHf2i0XFkZ46vC8
zf7fOnH_))1g^ckJKKlf?vYFRGx-|8x7+_PE0Gqnc7&i6W5Rj&Ryz+=`>W_m!n))FG
z(WY+Yv#D=^blcR2C{J2E9R(_{smEOFNkTrFi>B_If{6#7r{I`8aHOf{FVt)ev=84%
zR}$zjl6LnFN$o?|(uZ!P4{hCtb`C9e)p$Kdjo3C{=JO^CBR$URUki@mHMPj`s!Z^c
z^u*Puu0}}v2A1|lkL_=Y%u@OfDEJoBgL6R#1Iqp_-;MSBjZSL&GO2|~^|Y;Vi*%l(
zc4|D<8}8MNzAWUx^7rs-D4(im$S)f{vYN4)u=41w@F@$W<fh~H2&ETlpz*B5(3fff
z0cnn5J-?Qg=LPtr3($iibHgL}q&AvI*mZIQVKKrlk7tj9r`fDx1_g}S1M^3Meb!oM
zzv$FItY`UBJVOq#coKu}AQn$z@FrsMBwq=A#r?qIN(`O}oFn%neVxwzBzhXzZn7{T
zM|PJiOfj2Ddz3iS>>gQ|mAemJTc%*&iRVfC(|Vro#q$gS_N#<U)A<$lo4Q8q2g`Lr
zqdCNYaz(bi`X@o2uH&^8p5H`yu*E%Mc*s7CPPC=nWji#Rl}0WZl9@ce_JE$NG_04=
zOBJw9hgLf&p-){<aZt3kht6>Ts__j}V{?tVJK0uaGgRSsuQ>z8eSe`EE*}YdpOS%L
z!@J}Fuskr3B{=jqusku4m9Y-eKKKWCbus9(GWbCHB?p1G<O}B8i+6Yx@xc9T9QQ&B
zhlMkRP19gSGc24X@Xk0wY(AVVa8?Dv7QpocZv!6Hvmcn&&mhxqMc_BW5&RQzfcqIH
z{W^SgcvRcG2;NEjCTBz@{gO(+RipG{h_{50-XMd+&9A^+Ti{WIh5~;`K=R2Lm4b~#
z{6&etxicbo8F0bh#{oCVFbDkm#+uK7oU3?NVL>0-<h%?TJFaBMSULEP%Qy8^3i||i
zWo&&K(<`2|4(2KAV4kp=1M*5r2Wxu4JsI9J<<M18x*nw4n=uA-a&o&53d+gtNl6Hh
zZwmzSik19m2kCpR;i{9}!M=mXM;PFk6Xf9-*}&d7*c4?4MPHt=fTFnx-UourHu-5o
z3`NSFptv^Uc}uZ0LNQh;nv>$XjGdNZXM|#}QiMryeMa0mm2y&LE2YKFYEn|%kWt4{
zbdFGLPzpY77VMDG+EUCSg*+(`Kh*H(Af*OX&x|#sV%59`Dlz5RRn(ND5y<PJS&nAP
zQLSB5s=WI(bC#9YFXPnpD(@wVVRQJ=%1f;Zc`qEJyh4?yk}e!(N*0k6&X{f`oe#bu
zsW$^`BvUD=i)J}m$zmkE8!Ksa#&1^A!&cHqR?^>;^wa+)X+S|tnQqDG{;XQ>lxkY0
zYZzde!U*I|(=7k7OfP6QOvdCFoSE^ym2`)d^t_d{o05J$MoCkzw(X*4e95x0*s#|c
z%N}k@SIf%0H{*)uv~-n{wRBS$VClX?Aa9IjIa=vr+oHInd79a~3+=u%W4V>pIz?p-
zWq`8gB9QmWG0M6p(hfz+Zq3$^QXa}keqKwo&Pw^(N~uyEQj$j)BOI+nF;ePlwjPx7
zWJZCN(f~ZArROrhvJ_KFm;X)5NX<5jQl88B#7cR<N_odh`IS;0_|H<rCq1m$B5m*0
zj2kwn)iRFL@?60H%hMl$yzR#*X<5W4#k9SYk;d#8x#Sm|n{mPmDs#G(xyH)ePML+8
z<!Ci^$F;V-cS(%9&MJVXClcd^MNvXx+~6omNsPPAX2T1HiE-1SC@V28FG^WAF|M~F
z?MAayuX?s9Z)e<Mi*nEw<?I@2zP1SDtv|-1Y|yT&lK$0fX;#uF8EYX)#&v0QL#>oK
z2$%t<#wcUxHa5?v84X`lLqAJ$4glZSeAQB+U02O=G((pjcI;NU8<fdcR_;w19bQto
zjUYwj-oyYK)db32I{MhUV%vyA`~SIuF%4;$7}qUY`4=R{Wm^<CVgp*#MB7f{Mr;IZ
zA5Gm1I4+vH6>y|NHBs?cQ{EG-iMC}-fr+Bkxx||1bp$+rs?MqS?fQiv<PwYv@wYXe
zi%I`a2mD}&>Wy`t3$x)`#r79-bxwGIPPZDpOc96Q|Khg0v;W0!eb~1hpi#I~SqwWo
zi|4s0joDX-Wl<b9##wcE7JMr}R@=SeOEz;%qXJy#$U%hPQ|B-=)f>XWd>QU_C;kg3
zzNgL)NdK1;|ECjw+$#~2`z1Q|lKw0w{x2u~awoo$4oX~o=V(0l0ctw&H*0(m;wNf6
z`QPgBdCQ4k<iuyFKPLS%PJB-%{yit&x_r`q>%{MN=!36D%R5;;59v>E;$L>+8$0ov
zo%nVpJ~1vu{e|amcap<4nV&n8pWjD)2c=DQxQ=z=AGFy@waa;-nVQUHt-neEm$g3%
zZO3wtdeCa(lFrD`?I8a#^+Ob232Q3tXa28F{OKk>F)kwBzeEp(n5~sfUSzfNZ*h|Q
z9GbkzNuGFUa;o|YGGAnqqq6Hc$uAs|oY&GxMrjh`B5HY|4>ip^S9KQF+1a1h^1K}y
zeTh~|V1DvOJS6>=v!4GEUTM$)E`7aBuIFD*1D5r>m&x_}nd!i?e)lrDexC+>^T+|0
zd%-{^m%=MD1Y;>~aB^lv93fz;!?k>zupEpivs$kxxYhB*e3Cz2@RPu!Hu2!5lHjf<
z0B_}qSAyWR#976_iK4>QCj#%K%1M$w>Le_$<$9(nnH!)IFL5?c`zI^bd!ze|Gw!Iz
z-N>7fdFtyJ%D4t?dU8d0b3U0pPAM`>Bld#nnT4d{liA%t6%Fvo?0XTDADboSsdv)o
z0{0tbK8xb(WR_a-??%K23XOsh@wGGmvEu)-;w3=wSv4U(HcO0n(x~|Bl=%jVZ=89`
z8(R9#B<Y<p2m5(%qH&C);hRa_e}EKuKk*UG+8jhk8^^5wK!eKo0%0UzzTiWWL!OK2
z-?}OG2$jik*0wD&pCY49NNYt#HZV93F@w9|kLSOsnR?g6g&?4g>{y!TJ$SBT9<{|2
zuz7jK#(X3`5+R<lfR;Tr+0wR>(hh_UQrd(f!)60Z+bYwCRiu#j+tM;%N=uSMO52a6
zZJ<oNxnR;dGZDh1fWO<4G7v3kezEF)mRd*3Z_;U(!Tb}Nq(pR%c@HVDyY)wW)?i3v
zo7_K3HM{9jw0x*_BunKrRv{!l5}_xBo}#Q9vd|qeli+Bi(C1}o^-6%%JD)ME;*3+#
zh&QMdTX;$7hm=?YyrP6+fNvH2M*~P>+@YG*vW>A*=Ecxf%xwNRjj)M!T)vKw3n~}_
zuOpbZ32=p0UP~uk()3jXT%QF;Jmq9`)$yiDJyOZY3(TJjKHIlAj(9qXYpyi<M=pIk
za2oIrqhQE?D$^0?*Q)4^547ozh=;1=Pc-~`hhgcr!G_-UpyXQ$I=+2n(5%`(eEZ74
z*9iIcm4Som&=ceBE3XQD{|bS8-6|mXOM~I4gMwR}PI7L$#tFVnFs|RLAh6aM@XOsW
zBMK%|*$e%0fz4$Z=~awhwr^n_uXz8st|JG4-APqAl$0=k`efukS#W$^h?AkFrr`d-
z<{qt61i!&_c`GYR@P%h0T^`juP2}GNT<~f!_|>k$U17fZQ6S?=9nlw8lZ-34astv%
zSG;$e8E4L@!lCZQv+TIiT=m6UVe+0<uPTxEv>wC4XT)Sv8>MXag;kz?2hE;)fz8fF
zBj?g=3Ikjsy?{X8tSN@vF3mWJ#zWere1?VoOoG{oan0<s!z=<Psgx|!gz$mWDram~
zNmc8qqzf6Kq;3f0EzvATlk|9GO%o1lOT8GZ+2M!72^VS{uUqXBiKo&-k-AJ%**iv?
zR9q;)hr{u*4d2#z!6CTU5^PoizOJ)N3ApxoLizL4pualteys_{Y9y-mt|)gw$x7$;
z4BV#|h0e;T=x~=iUFmS^Y`8VHZ`<%nH5)z}pFD@peaa_#6;$Fu8$Jr3@OtI*lJLOG
z5l_my(zp^-EXnUH;=2^u>v^s7J|j$)Pk6~$-ydP~!IeyWeRYfybG4H2o~cZ(N~SvT
zSsG8~bDa2#OuW20gb%bkDIIMJ1bY5zCneveP<rSYO_6E*7Yg{Q%o-s>bMI2lBU>_$
zf4qvF$De*nwA$Y>BE%N0HJ+{R2aPumwU^-it?)=qPJ5pvckL++&%71oh1V%rfgzK}
z;$d?`6Z_BzR=jxa0mU;5)Y!!{HmA(HR`S{dn<5TuCmHSe+R7S{q4&nf%DH{q@ReYu
zi*CajT&M+j&^>wnT@Hr>E{BTJ+GPdJmpX9L0=29HQ6788d7+2Y4M=5dR=~<=^?_I?
zn9vTDVN~KReegYH-VnJxQX5}sW=;n}lG&8UI4;SHiC8F0J)8)<LIH>A)hdt=Bp&uE
zY^nVxth~@&E~9k{sQJNT)4a%X^550WoEx5w0n_+4>vAKS|KJL&G1b%_H6JW_&~9^%
zq<Q6tyylv>(V2J>&yfcXU4?^}cb}5%GiyE5l_NdBV#x+^0cLuuZ}Fjm*}c5bCe14g
zzU+pWVr<r!;YntE2+ThKvBN%drWkWDCYUM4R{2oekaO%5V`~zp7`usE4aFd~O};$W
zvOe%vT$p_<_|yhi+ik`m$JxfG!s1^<akgR5szyMZZ5a4|V-C=mUR0Ht$)F~LoL*ED
zyi0HkjQ`1k>z&JVOeazVE&&`Bm;vd>3BLY3JDo_=TY1+Ob?r%ZnR2b>Ye*kH0@;r@
zm*jTs+S?0WBsXmSgCxF~V^FARMZ9rPV4=+Ucd7N--UE3%lJY*nz{(ZQC!mc6cmqq}
z`7pvPkVWVH^3}_q5Ra8sEA))KX{Oz5AhTUbZ;@FC#9wkjMQ^|?aCHePT-)P5L@_NA
z82KP#u#86jflna#BN7L$N8sWMDFoMwBBk?ItY5C;GWgddPPd*%n*W)K<{Cc*1Z?Kw
z7VYgD?JAc1O4k%>f16QKrqUx6=v4YBtvn%eVQ0Sq7kFNi8HQ8VJm_*o<%Qc&+!2UB
z@gfxI83g(;uo8jw3@k<<^<o6(A~1u283<Il1c71%CL)kaRs&xF`WtBAI|wwsw4&Fa
zk4Z03X*FmF{1RpaQYn$oNzVe2JSR<V@|?7#Mh})>l6g*=>AEbQrA1!}kqLJPnvxec
z1%3rDudoxiu2qU1q?i}z(H(KkF2nR{>ugi-y%=KgBF*`@_Z*v;_-q9Jo>&Id#6>es
zk_&2{M#cK?1Jm@Gh96vp{DN&Ndmlrcf01@+GpvSAyb$PW{sHQ783Gd-XpF#822Mv{
z9|P49xTrY-l@PcI0VANz?M6TYE1+%Vo2-ChP(=kiKtut;{1dVw^Exe*`7j2^d=3Jh
zG=o2sdCI+L2Hh$z{RElW44xq=n?dH~2(TGsGr(prmjO0|w;5nFsBi@WYzD&-$R#Uj
z1|I-PGx!4mX$IYAnW7|XQP2!dq(nA@PA$QtxpI?cV5wu80i|g(*rEJ-KqQ*MrR2qC
zumHSFGe{^g5@(S@o53EW=KY`vk!G-U9P4@y|DIUlf|{6FI7u#Wn!#dK<s4f}y((AQ
z4t3ZJzG~&xQsR{;C~K)618fFk8DLGl$^e@|N^1mIQ%w-?LIHIUGBlsAfZe_=M94Y>
zk294&H1AB(iCMVet;QUEB%j=->37yRPRb@~Jm)1?2=FTDv6{>&{97jZ+~_^VV?7&o
zCd)9Ndo_){zPr@nl>Di<xq7@yrb+Fw9{)cohwu*N&_Z%!f@m%$GT+2F<W^*^g3{FS
za6;Y1$duL58tt`=AENPNLH~iq$HP<2H}P@fINys3v%Ywzl1in-JHGf!qg^oJnj=+8
z*!ts8n&uTo7ded1!8`zxkNho@?`-9U?$f?3gTqS-B7;NcBPp}nGQkMNX=2YQXj^xf
zw(<?y1%HA0bD&S**J#u}NIJ8L4nyFKs}LwgU<v~>5crybxd^mwgXhFHpzHacV?bGg
zm_94`=V|_V8Gj}O-oan|_X+>&^EKmt=AVE0C+RZ$N#mbW`R5$|xsZQa@lQMcxe0$l
zo$fN4O2J1f(1IV?cuT9<Ew<c!P*art21-jYdR?srTfqQ}`ZohCe4A_F=$K<53-uY$
zKo;!PYY|}Kwp@<@3t0UI`alFOVt}Q*i2)XLIRh+gQd<OA;N}dl(4R2Cg4b<_01ID$
zKwlbwUtH=-BgB<FQq-4*;Ldqp8Y8ac$)dhA2s^G!khW+QX3d9pf4bV#9Vj9vNbiDM
zG{6bc)b=P)50J%7kWxC(+4i1=l(k==xcOjz6XFJrMBokvCLr)S0y3O@bGNC94HUS$
z+HX4{ki*GA5Xf+HUPlBtoD?v?;bb)f98PL<LV&}`5C%A$>_Q-ytYkQ8(HYV>tLTG(
z3@2mf7-`O|Vjv}QIC&97GMtc`3@4Tv3laJ0tb*w}oUEN|_@yk6;baDRaX3l3(GDj&
zm7+N*bU5jR)V%OQlQlA&l*-gW<_ss6xS%GUvg2@~NiJ}PligahhN^6EPqoglzYHfG
zvQae7Duy$_nZ+XrL<jzN7|r42_$~-=IO)#-hm+|Dc;h&2>8mBFX9av;O_)DMQZj#+
z0Wz<Yg8;{?vl!rbbsYl~FoyvO*vS9|RLGU~5{7-g{t^{;J6gvXi0eBKfxt}z7vsO>
zz`a+3V$&|@@wg=PeU|@q^E2idJ94y1L^7wsh43F2I`OAE@#{369}c@(<IUEby!~Xh
z=Ez{P64K%KQbH`6{7;?u(GKHpo%q=rKMua;M~xR}-EMxIn2$`?L^J0NbmGlq9xaUT
zE3a5!GVDO<gUynU-;F9|hfM4W`$YomknItZn|0qI9r9kpgBho|8TT1xZ9x^hA^9pY
z<1}|SNJ2k44bt<o?=hq_YY)(=QlZ{XeE53>ZwmqTr!XdC(9r)6`^hO;rX2DuF!1M&
z2=)v3G^t!(!z&j3zf=It^;J|0|NDER+Wo8Xytd?Z$BgM{P1Krowot8@hk4}1CO&TL
zjyO()#_~&DLS#){AKsx$Br|ESp%(f?Sx7Z{YDf+l_X}yq0563p_u~gwChM>f>a1C1
z=o#bip`e6bM`F^&GV#KXsGmNddX7aQt?%A~;rE2Er0{WL<@`dPHox>vV>8a$E;h+p
zQ95t?ND11Z{5RSBI+NKg`&`X*q~wW4v7ylIB<x7?nGP?sz!_Qn7oCxHz0jkGJ<2Jf
z7y91Ko2pez6YsZqZzwOX-$BXw{fLV(<6zB8w`P@KHOvB_qB-J0RSt#RgTm0rdNJPv
zen?r|hep!Vuqe|=;>NDTmgmz-wen<?_8kSRO#HGd)x?-wYV16(lEh&av;fQw{s5ys
zm}Tw>IN>I{L$h73oqwtu@Y93PDYsW$hurPDM-G7|SF5|o@atWQUi%Z+a9idx$v3?R
zHaxfyW8k))+*!d!%;%C?)QgY@@H+&5BA6R7I|Wzo&2;Rfd?D~@Ky$A1m3(8ORUf+%
z^J~=&O*lue=Q@8Vwzup3rMS4zob&u6->eve{N|kJU%@GTAx_SD4hk+JmUA7i8qbA(
z2R7$8!D{9l2j?(yjuTgngIe?)r-EX8j#IH3&vDlDv*$vUs&S)aVIKG`8cDZVt;{*j
z*2Tu4`{)I8s!jR^UXbTFz52tY@*L+?2A)G8Z2$)9hD!~(JzbI4n2&yr^OZ_LmCG+U
zu-Yz2ig~a<FMi3fad*ycrfCEF1MIAWo2zk4Z-Lg|+IaO{{yI%%*FVpsmbqx{11G(+
zN!Q(RxCS2GeD4s9umo7ML4dhwE%|;^3=S8!t9+jA)O6zg4i4C3h??4G+?v$JMrgc6
z?!pcyBM#K1VQ9R=%df6ogeN97zbjjMdJH*kqjx$_2|1qlEgjIx<kCZxc23Go%8!RC
z-JJLWjVIgQ8ZTqRbOqeMeolzs^p1E8GQ+MZ{LPw|X8bTJIDF~@#=K4TvTzt^#r8)$
z!q6w|VxIm(cKADc29W?4>G^Y&IZrP}hyZ<sRko_z#|@R!3q5@5LcYQR)rEkbUnFU3
zCy(`Ic+d%BeC2tAWnslbj|dYUU{uz=AZBc&fM$&UUW%;;7%b9ZFJAK_c5JPgxiURR
zGMggB9AHdx1wN>N1I}<2NKxev>bF^Wq4U&7h~~KpsQHFt(>&L5@`q?<9$<W3zUFti
z0-sSp%^PBXkeS304=}#5pB(Z3)x12w$dSCA64<(uvg1x7AF%+ZN8xr4nl`w`NccZ(
zgL%u9xjiJ&yW{kDv?4Ayl;N8_IeIvUuIGi`*PLN=884fo@(zvJSj~9e!2Fs&;?dFr
z&H(%r9J@IpStri~%pYh6;B{3v04MvvazwITo)H*KEJq~I3N8q7uD#HV#T$ghm_ZoM
za71FzfWbf<nGI|_gkv2>=AVT2Re?Oy_*w8-H#3<d^Dlz$5sZ=fSAiD|wMQhs3x1rq
z#|(^K2LwMh%#PFn9f{W#4aao4f{e3)n|MN!ET30;7nD39F=$4Co{$(Q>y2O3c%>*@
z@TDr#_IYNRF=%*2C&2Z_X(M2R9UzO@0JsbBSZ|zzuGedsX)-CLVD@U2AIa=u#P0x!
zoRGK#iPy-%2-6|~Jt1L!IU$)i5-WP1ki3Hce!ZI`W#muxgybW6Af&@6o{*?$?mjgJ
zL1aE?HsfSYxL;04a#k26*?04b6tF8Dy&HXJ&jG!DVJLO(a>FSMrBF2wKx~g<#IJxt
zk$~Cu_*Y3wNm`@o-k(9rZI51~LCRh9=NRC&NBuDfaNFZP2Dt6fa4Z7c_SnP#w>_>H
zhpHee+4fiuB-<W8BOu!z-5xTEVPY>o?4tLiL~eVuEd&$pqLZ6!dsu3?FZt=V2h(-i
zW0qQ};r+7hF_FBu?XelW%r5%VO3{N9y6tgd5oS|gJZV&kY<m>T-iB;yoQU-!H#!Wc
zX|~oCl>yGS#}+NxS*mRC_^Q|a0(H2HetfaJi+%}^>`HWJfV&bC7~rnNCI+}Gkv<*)
z?n+#U05-9p{nS4&91I&2_JVaycgmH0tAdA3IkFa^0vl9a@~c*0S8`wl{=@()uw()P
ztiVPS5nu(jyafSP;Nz1JU<KBiOn(AaQi0=vqypC<(2u8KR$4JgP<ba%8Y?et3K+2R
z$Z5c(r~*s55|mnTOw)?1`MA+~2l?C`xP*LIac_Z-skqikk@7&a;xebA;-;=X_KGuy
zAzG&{a4IhS5yLN1apzTy|4l0{eVSWw7XnGeb!C7RSHu7-?sW!OaW!s5fECvi0k0qT
zt9q({<|s+<f~vWec^6PbPrw#2BloICPDeDE3*+H4fXG;w-UDR%?eogCm`pFNy7+fc
zu-~o+g*pA2i2$p8Dg&(c{S2_$hunq$tNjZGSnVBeN3|D&l~nsGAgT5}2uQy@`%z;f
zr{5k=iLCl7XMqW;p4_D0TI!g7%XHgsR~mlaVyXU%$&1zhDtKwXRf>kB(0*IJ1l523
zQzmPq-@4n7Wi}{l;#nqVyV3<t^)J<;)wBKfimDaS)1==f&vt95F_6?yCk9wUBN<@z
zzsLY<smdJ)u=*PyU=Bggeat8v*$-`9wU-sp9#qjLP(Z{cAk62I6`6m*0GZdjQ<>j@
zfahPWJ)d**(cs9Dg^PgYDv)anqe44tOk2jRs$W_aZU(QY3U?7jRCtR5sY1qGszN^o
zSiWfpn3njMcGMoIso;%O>;DP;*%BWiDIHMK-3YKH_F{l7@fil#6659|z?RsP0k*{V
z5y&MgX^D;IYD?^hfV9LDR~^z4AJ`9xY>BHtBrTELq$OJFn3l+NZHXP8F#J}UmiP*J
zu_dP80}?roAFC8QoR-)Vsd;I!BZxU)6*qIDp1c}R6Uz&>L``ym(-K2kwEDJ|a;x4>
zV{!!PKF_VC2|&^kmoUJV_!|SPsb=>gz{WC-0oK$b2za4tIt`J9K@$a>Tx|VQCm7w1
zw8D7VPB6lCb;7V$^2rwr{azYZr8;J;e@RQk(u!NKID3@GKl~yk;mW^(A;F#?r#9xS
zcCn_jG50m;S;IX(8d>JkkY&H`=0D{$^>HQ__I0r}uJPwWq2-5D$eDJ1tF&U7WM|s-
zvt-1YIrgr(G7qcuvn!a;$(mOfUG6Zt2s3^Z^vLIVZ(9q89@BOpeBM{Us+wr{1QWU+
zsj6O!F#6hIlvhQYab!Yy#8UlhnP4fy72rzsx}R4yjWaO+8z}Q+#Xxi~@g1}?vy_>T
zOkelJe7kg+nEXl*EBXb%_fLV7xJB?s#P>}Ao+P;YedzfqOUzPcs<1d~Ayx@k${2J;
zDG--327XJ(XUh%DzTfV;O-tsk+Y`VA@@2_eg%>{?Tp(YboG#+xN){It44ee<Gm^Q|
z;udUy`BJ5(KT;$sr3I4xxJ8gA50l*|c&K2mlokrE_5jndQYsZV3(!1Fwn*@&z~<TX
zr;^PcCO-K#hlPJe(*4EyGRSLc=wcWmSdv`)4;%nbSI$|2?i>kl*^|5!_Syilm}O7L
z2P?w*z4syICJ&znlsA=LRjGJ{O{S2qCv&IsR*-T{wmF&moPQEwO=i$F%UB>hwPxS~
zK$&m8zt%X+hD)K~2gxTNgo1SU?}I?x{YlFapt~Q&0Nwp22I%f@dk6u#`@Ia%-CzAM
z3PD!l?w0|{#N$l_#NE$$+DJQ2eIn+Zv;T!ey8AO90TWI<$W7e6rAA+opUyj(uI_%r
zbB13rL}K3gBze)@&jc^y?)NCgN>b>=V=GegK6}YzjZ8e;qtTdi(z)(AsS6x;U;7y~
zq$(TSnp{X@(%pafD2m3T%s&wrEKgu@P;T%TqUrA2GeCDgp8>l2uNa`aPh5eL$m3lP
zs({GM=CkBoRzN*aiGUjzpnypXP{=X{DCA8BC?stq0u<7n0ScLbfaj0bra+$_L{sAg
z#CiPYZcV03uW6FwxzE<5wikXy)2@<uy-im#<DLJt94xt62UnhVP)ott3<V(0>%9Y&
zlUpI+bsgOp7t2WNT>_OxrQZTgz8Aa?xOc+`aYITFbXSe`f`3Sxud@n|Z368V6zJN!
z7&Kb~pt%_v74p5s@g%vqKe|Fi^Zmsf|FQL^vRIZqrYy3F==mF*lpxqZM@^aitjUvu
z9j$XsitJ)x*K{R!taT62>?Ez_^X)Hc4wm&=&B2Y)T~2%-jfWjV2c7s(b#1uJ97<Op
zV@S^PMiILHXCylYwYKGqAzrBJ21724&Ub+4Po}5Dk7oE@72)ps8Ye#7PAzgh43A+C
zpQe}6>i>s7jOFI6^KLkr8joS+!F2h=-EieO!SIYcKBY_#d4P&myU$U5p5!yLl{HTM
zDvf7LJMD#N;n>o^!yJ9bzi1LRprr(xr=-ROj4OT`?L>UC1n83!o`5^^mL1}g+aVsz
zc=Y|7GHVE`VB3@{$c#td^FR_B^0JXcALIGAs6ssD><c<7W%iueX7~+`14_Z;<mrYY
zZn!y4=EkKw@bCFn|E`=3DIeL_l;y(Ns*p65whGu(eiG87oF`wE!kXeYM#gB?1oTUp
z`0)uhFSJCXdF~i`$&r<}$B~5$M>%uOwt~FS0UaYn<E%BtWYqWpBl^e_t$Xa@b-2G~
zT_f4dyGKuIToG*bk%_N>Egw#0X)S-7rqk!`x9QTrF_Fc~d7(CL&&yXp<yv5}B*pAm
zPS2Bf5oTNM{D(C!50b}AUVWE?d(Z87`oU9V9v|Zrd9gV~F4a@yWl-ci?e(-$-V+LV
zq>N^yjYLaknh9olGfu)Y&DsR-fy(>^=D!c&_t$V{dYjQy2W5>w`;AZJZTs0z0>3m9
zxKbkT+s|GN{8eAzghbB#PI?NxqIj8^*;P*D<nYxsoLyiax{AQ}o*~3!s;a;i>)^{L
z%mRaI!eAk>?1HxveC>K*K1&e1R`4F;D<=bAm&lpS{AYDG^_&)F2ZRSx6F-jEp4<01
z?YR<Q&wT|k`Ifwbl>CrheE##&i}^InJIXj^xiCJXk}_Tn5^)I<Amd*VlS{IgX&U3z
zzg+o>X~NCP?3Bb}%dFW3WhMbKyB{(6mb}v8wXE?%gmI2C4jaaAS;mt<B3ep-jNe5}
zF3DoFWIU!5Eb@9~mZV~XXC|JNpv6AvMP(=fGHiyJ{Malp%t-?i@J09?%Dj}CHcA{p
z#(W9BfQ%ynzJ>l8Vsdv-Q%1|59*nQ^Ql!}5D6^D@z^rlN%T~)UNTk>jpq5J!lS{G~
z%^0th)BIJf95TBpv3jBw`6tUv0%SJeWiV@gC^N>Z4Ld6{JDm-7NzAtlUjvzFCjl}X
z^9pPjn<Yk5(x|3`m3f$&c1c`k8Se#&FqQxrU-K$!`%q08ugsoPW~HXs2}xS)J3%7M
zBtT|wkQvEhG-JFn+xVKX;bt-$nApiO^IlVC5+Jhz#N?7Jh8g44hMy@jIAHk&`z5Zj
z4EI=u5+K7#8)3uPEHTVUqniGt%)N)9X}`pSmhpa&NU<eA#+_e>rX-7D#&~6R@kUeZ
zhGaG{@!HC2!-XIbW)dK?Eo4Tr7-oz|vDrhuQD#X}XfPa|xSR~x3o5?>hLHe!Lqo*m
z$7YG?F{DwO{-w;V;{)@5t+xHhoGVWT%ok2#$)j3Jx@z3_byNIItMRRgxm8r->10Zc
zB_RD8G5N7sVl;jYG^+6}%G^4^;H`;k$()L&yr~*XfcCu>G5N7sqFT!fRipuPe#hY%
zWqzI2{O-hoRl%I!aF{^mkpMr?u#7S1Ah3y2wrbu6npaYOBnfGc+XNdjxBLlxs>Cuz
z2j4LE;$#66g}@OnSm>qGF12+d%^PVQrHQXmW16@rWJUwDbr>=Eu~}klO&YcJ<;pzL
zI!Y63S5sSWCUY8H0<`ssZ-IGimKa-;Mmx#%%De|VNonE`%Xkb(gs}w3_;JMKk}QT9
z<CWP+Wj2e<mLxuBnN@#VnMr`mS|cWxWHHPbugttRO?_-2vlWSzleNgREi(y_*$MA}
z8OdUpF<zO)E3<uMwkk2(GV2WzDW(L->=iO2SqwAAYi*yP%rHllAGBLdhOF&so53&=
zU~ONGnEco*F||z^)iht3XFUQ<*CYm0wAjnYm^Cc{YWf*sa!D4WDdV;2e80)qFwH3H
z1ustQZW*S&s|+PThSwq{KQ>DYbJD1$mESVV+fvg@5|@GTjZ`pjll~|DdKoh0k|c%^
z<FO>!sxvuRO#GvOU2qrJu^V1k&U775TfJiBOV&9$VDd?y%<5*bW*1rHfJr-^74rPA
z9J=P^(}ngcoiIOHhb8JYRx)9^bXIwa`Nyj}=c6Wz%a`GIcjz{iPZt`ZbfU|t>a)0@
zt9;xc1;tkzQPPSfbLHD!*;pBd&T_n*zf1uycW4@u-SY=%2j=60H@c}6v3<)o+?Of=
zrB6PTAXfCVN-mlY(3VZnkGpxXs>bIXG?lCEPh(PJ+1{tL!uA&@<4IU2ahdMa{9z2@
zX|G`R&{+=fZ&1LB?jyX)zk$pP6+2l&ce>zV2jtb%e!|lbDu?1@k=Er@OcOL^w`s^&
z>}`P1ewof<?<aJVLGo#}FVeW!TmK%gd|d5|G%og@ejnYd7~P9c1Mik^*gfz8R&&^x
zH>mj*UFv-)i8BOZsrQw@J%F;jctKT3q;mKLPfnkfs;;Nyhw3RLKu<9MG5N7sVmdf!
z)D2a6+Y}J1U-<=3P5;U=Uuc<2Ks+#+$7YFP{)#a7r>fTcRAEw@Tlex3hdl3oztZru
zq{(4v)HSqHEys7q6g@)dg^M(Ps>F4qD>=-DSL_Cw`Qn2AhGy=Shv$DHOzGJ8Ji5Lo
zfLr{POj>l(JH}jWh5L=rV8Yo^a%?%oT-nelUFRcy6V1B_jV(RhSj5~V&po*|nmP=t
z^rj{iyQM|4y@E~{Zm;QAYtEs_(@W!C0mohuj-EeE<L7|aScexj&NQWd7tg!X;j~cW
zSrgAj;<+hfYl}63*F9D!;g20LH;)i{q4gRs9e0ZZ{J#{?WM^ft*yVRz+35K;O)VBm
z+w2S@7bzg0p@z><On<sHeW;boXrRMr{V_30k1#558O?JT?LQhLe8Jes8+y?NAG+XM
z2jnHc8*Oi_jU%_U%2rP}v5r}um!-Me7$$IHli<KQoa{KU1L&>hs{teBtE!)W1T41>
zj!fm&_4cj6tBc`vMy7Jd>gR1ZJlF@P)(Tu?pK7$^d*@^ODF3)r^C*95VQ$u=c<fK^
zfm)Kf7tF?dg54+H(=<^PYYT$lL#byRABcVG-rTfwr_x2%GS8%TD35l5(xTk*3$9OH
z30iq?;9Cfm_Xg6pBk&wFxEO)FXElp`Z-8wbHZHZZ5(F`!qQbS_GbSvi!qZdJPf*6)
z$d4~1+-e!GAmirm8*<Co40>f4i>HXWSgMgq$ucGIf^$+QTS@O(Nhf@&Md*k?UOUZl
zG)ea51oSZ3zL%@2*Q=Tld5@&-vhr@F7?$iQEAJD^+a6Q0!<sDgq8e6w<{kxn*!r9k
z#Q_8p@Kp|eJwyEap-%JJ$k&}cEg$C|xsqA;dgaw2e%M?b6IgIQ)mf=U?pOtsyTr<E
zN4X{wpLdUO)^j6r@6*hjw7%xZwMSE4=oMGiw+iT-;!e^s@+*3Wd)>?n{pRL9K~2iM
z&1~Kd<>qar<Wfdo0n0ee<~?#5XGF?4MKg0U{J3N;V_P#ZhgI)%B!+f6z_0XyY3CH5
z0EZym=g3w2Xhi%enwb;Urjogg_?W6aPpMgK%nb)94D&oF`>zu)LTb#-I+Y2`zZpyy
z)N`EOzi7ecgVhz{uecaTKA!>KQy*QrqToiK1IxE+41StezI+@n=`D5u%eRgVejPZc
z`MJnnDV~D(<(n>8W&xTH;Uwft93YT8`@xcw-YelhZU^P4%SR$2DW37|cIq^+vcOrp
zbjQDnz#6-;gD*a`T0DK|EyT~BgWSi*b4P#Km%!JZ37i$rCl?xg1)NkH_*B796E7t`
zP4G)!0|!Z8OYmEJfbSjwTt{%rZ-D*5z^4mdN1QkW_zc0Xd<*>gG~l{|&;1T~8c!I`
z6#Nu$!O$~+&yMGS5m)l}xPo_TIT&~U$!BWLk@Cfr+)`K|15W*T^Rx-xDU$)Gf##o4
zI7&WK(@4r&XRjS-&W$hg&RFdaO<UYS7o8pd!YS&ae?SItWJ|wS$9e7#aAbo&GUVzw
zE0jz%M_GqD>D8^x_6}VqW?g1vy8mO7{cJ?=7yLE8QI<-~{81&gWPqd100i<jX%_3M
zd0-hc+K5M5A0sc6vc<?c57p~mqJRg@ogvX2ed2V@iT8&(V4)#8eJGw88;+YjoHw<`
zxxn7&G1JMrCMv+6=fuC`#6Rf7f9=FS>BJv&;y-laGfp)HfLTL3op`^g*3L{c)Ep&}
zxw3g5DS&4o<e5Tv$%K;aI8Ghzo#usxX)bY%^A*rHEfp3pc2i`~sy^)dNnPaoP@L!n
zw;H35AA}b|Pcv$&2D9|kei{F7ug@&totL@9=3+3;U@PF`LGu0;hpOO+6h6d(uOZ;A
z9Er{{hxm2PQ6uig2R$Ek=%s@1M;{zlRxtV5>*3^)5C|nRG)xSo5CLAo8$ybqOmIY2
zg_3;|12Ox@g8Tc*edrv|`{loJM}Z9Y1j`-r40p%_cgkaT#{YHWaQPqpZx~lHqtNp!
zZZpPQv<SD~)H<v&#dt7gDUFD7iE&{}JP_jfmuY^kzpshp_x#x!pS2c!EE3Od#TK<t
zQ>8h)tbn8JDj`L4=vU4t3ya1jeQeu8=x@!-=RW+(+Kmae8JTL^6P%P^Yzj=~`DZ#Q
zRclN6#fF~W)JdshQc#zk-^z(^W^<sfJ-@w^a--(Jfv?})i65--Tv+Bi@zX3Pv?Oej
zXbSyG=v@V3tUnagLEqUSd0?z&Y<uP@i^EKvev4SE;wPrEh0|FIX!bMfu;k!$-jU3n
zW*PYdG%wehEhMjbjC-cWOLZ)EKz~DIsw1?{N%7xv!0na{eZ=#>c2XWxDX4+azfQbg
zW4kF6MQ&0e8ojO)zgy#B^l%%E->-3eK%%S0(Q~JqZaM*PL>TMD*Nw)9@74HAG!8sM
zKWLm3z0#*qrFt78Eqa!d;$P^1TP>Lsy{(h7NTsmo1Dtrj*a5F85sN;@iT_;V+1^$<
z@qb!z(%wGQ6lrfiE09)_@tG+JTgeS)Xe&AYNUg-Ttt51r<`t9N=z!tE2+y3+@rA-p
zia*T(3oMzKq|`}SZBp1DLr**L{$>Y!tVA@)*G~M;8c&mWpGPaIN?q;#Vv^G}MND$J
z0x?OR0-B_k<Uz-|;7BIHQ|0D?qtI=RJpVxlOtZqpBpaL*f13kVS~4-oJ||_PNfDDI
z?ueGk&vL-`N<@>K@5EO;(=>XTq^%Qwiiwx;(hK1!Etye?NoFe$ldMrdlN3lEn54Wz
z2yx-FBPRLGWfZs5h~U)VIm_sxBiVMLW#pf!c{${Kr3Es(cwALtUNV)>XnLXRl#a#l
z2Pj}0t$LO=01Ta;KhjA#!=|uy{3%XK3!5UN$=yy$u1%4udca8;YEoo0S>?pfv^iun
zdEQA`qB&S^o1FM(H6HaA`qGIH#ed<nfKwdsla(Ru>;fmnZ|8u_v(*%$e19jUp-GW;
zHr|Q%=Q^O15>ek3PW&K^XFJ>I#80u}r0e~lDbmhP+hs~3?X0x|wzE$q58BzqM`|9g
z+U61J;xa097@dw)pA3FSoO3LSFq-NzdemVw*fP5GNE*Fl8Rc!&ynKD!oN<U6mq>2Z
z=ogwE_OX(#+7heT>=q~D_DYRjYz%|F!xtcUdP8S9y$GusT~1fxa(ebYV6&VSobeN|
zSxyUHNNkqVg3tIF*es_7uK?ENbR`OkE~hJ%T~1fh<#cY=W9D=vNn+0b1xwL@kHwh5
z6M<!mva%$<#bjAYr&i)hI__8CNnF{cOZq(G#Qwk;hTU(#GjTW&JiZcF+FgFf%JWCk
zpCFj8=M;P#0&ZG~EAK$pel;(^)mnOF6oziW<VydX6YzS}L;T}^;LycipsC!1?NHzN
zL$f=`%`mJ@in-;FdGJ*Jk9qJc{*QU^T>Q@`e`OJYD;?JIfksTgQ+W2bTe)&-ahKs*
zZ@BZBDjRmzelhP0E&tNgGxge4MlU{qe9U@b-d+UXwO6^(-yKkH?2Yxmib`qOfTcfX
z&U~yhQs7W~WhDJ|gaXc1X9`#J*P+tB%h_LSP)UbMf1RVzR>^CRDF0Krd4BzqvSOk4
zDJ#0nw69H}Mc0cIP}d95#-%R~JCd$BR@c12nwNDHJxcvg?FQSC5hL2(zAD4>N|p6Q
zxIF#>^H)H*Wlf#le*b>-){*FW!7l>r?cSfj(%W|l?m;ZQ-Qf3#rMK^v^p<}COK&&$
zbzsxmzYI`Nq_=+=(B7^e*%%q@_k*!u?n>?Ndth(u;U7Q;EglAq3|{!R_V;fjxz0bj
zk@l^?Nr0yN{}^ER|BG0<{~wZm#X;aH+z8t*_zU0yyyoKt{|d1Cv&(g;Uo4p2ebo2;
z5I!JjRRZzesFfpu{}FsvFdnBQQ1<xqu-8|8G!jyR%+xaugxRBjs|)^)c-su%;{<oB
z0Gvc2H3UBg+`H`x+)CX9$1TGKhwxx;klLJ6G2ZKUDfSv~`qtQHC5VD;gQ3Q7H+*kr
zI0y{I^6w1AK2o#~-exJ@B1M1l=tX<rr<ISFRfA7js@n0EYECRwm*B^iY7D9JQoc8Q
z&RlGU8l4gC;X6NQ=(r<99(~YNq}jp15HJ31tC_x<y%Ov&>=T?~x;^(73L(KCzQ;(Q
zqw5Hhup96-i#wcn|8)n9QX<R|LO(k3ekZjXb^T6>j;ycI374oHR%7Fs%j)Z&tEOLs
zq4VN%onEVc-du7EZ_m=x+wbB3@b;$kXmcLDeoDoX9fkDs#cE&{zk~AQ?B}3k__dB4
zOh^CEjPKv4y^#KGb5wZfMJGPA-2whD4yb^hfZCFxdU>sXjMk&pq38dsPH06xT*hk1
z<q*JLaYMVCFCChT>>6pQ#j$o)z)4$)WbPO^;)|T$2kaQINV9T=^n@$dJP3djkOWm%
zB-K+uNe7LjiY1#-KS%Q7MM0Go4r}%r@Oj3Id!flL|K$$<u0q7?4Ufb;-ZIbIt9jYr
z40H6mNMn=D8)?4Ta(rfQiqd9=Sw(_0H}QXFD)=h~hlcx1Wu0R7!7(^&f?6kE><kX?
zq5lt>00$SG9PIcM4tOg8XTh}urwDG60Gt`cS0)0#+z9DYCA~`$@EYbf_{z%hIyg)V
zQcz@Ym=@H*A-Ca5%>DSH#Ec*ZgWr)uUX++AuzeLcQW>r0iX*+3SVpUR1XrsHETh#t
z!FLhMXmzjPQ>p>WXf<E(=fpC3To$As9GQ%1&Qod_cE{c4Yx~AT4ZCShN+|eKw6Xga
zIH36jlBZsgGgY(U@~3Jchr@8gA*)DU2ru)2otO#U<C?1rYG<3tWjdEmHTAt2Z5?bW
za6_DY%1$<BbqK&Mmr4NhPBS0PH2yjZ4b<~rRS|R$KdA`DZ*}4mFEkC4v%$Sid@U0%
ze6eBjv#}UCU20Px+Vhj0lpAdd=bWKbO<^A$eT@S4(NHHR#b0R|V1Bbnm5~X!OkL5-
zOc^-^XA$A<IT7q)+&i|wS6WM~r8>`BZfpU{jYWRfiT_*US>!LB`1FgM7UTcvq%^cC
zQsjhRqQ<z|rpPpVsHU(^mz$KM)u~JXQH-NiC|rR5uoVnb{a%(mvzKQoE26BW-ce@k
zE7j1W)$51Sel?|Iwdss>$knxR>5;~_#Hy6P!2A}-HRe)vIwd$&tC|9*QFJMsM(<2I
zoi4z+v)~p+)xHdod71aZp)5h9xQRjXiONp|;vu_yf&T)IYBm$pVfl#Wsnvlu6ax<z
zd?T<wP*v#~3V0tG)#H8%;B>!ZV%*F2gI)eAt1yICyTlaGtn9cwq#yN>#)PL1UW;UV
zUbC^OpRU=sTl}%ZuCv3=ejNypx`EGGb=4>P!QYG>TSELyhu*$H><pfXaqmXNCu=r}
zU*d>A%VD?0VOLiC>k&S!9Qul=Bs=(&rQZ~x@9of^SB72`<+BGU-a8Tc@eY08a_Qf<
z^m)^iekAHlM_Fua<fCDgYRa3DhBXiT-fQlRW;0RireV#LhP4eicXOm+l}N*?bDTD;
zInt7H1)^ci6<8>c_s88M_<n<N)9XCJFAHt~`g;X`4LoXpCp5hIf~(Ys*J}|T)K914
zl7_{Vk!~?AWsnODEXJo+&%{AVBz<j>zd?)2hCAyrF~ngSZtdTrEoZf%5-$S#e`M|H
zw{VEQa)|UH9y4qt#%+sa=<Z}lZ)Pij9l;zSnF{_SLO;%-zp`BV&m#2qI`qTKrQa2y
ze@N;1dg|SVK61@UDL#p@-bsJTrt9Y;@zSXOwv+y$O^<$h(%<2v|8CQZE5S*aYh3=;
z^dab<r=zh&I|%1??X|)0iRhd8hp83PcP>LucVg*(jL@Iw&`)#e?E~jF%O8;}?VT*^
z%VwD`3pJbNcbg?IN3)daRucn_CwX30rs+_}iMyu5PHz$L#=Qww9Get;HuILiP9z*W
zIC2aAL&jF|X~<U_Idbst$_KN|HOy_gbB5QJ6FdabTO@c*LVYy!wq0PMYp=t@)$N%M
zGf`mEDL8&pEteP*LX~ky8Q2O-V@kLZeXGYg2&`k^0tC)ZMc@hqrXtY0`Qv!F7vs*r
z8xZpXW5S53csa8F4Q(f-#e3sAoR6jVUxvSZIx%oTphr){KMR7P7axwGcyJp;<L7^?
zDsnhH8JyW13K5^O-|%Goc~1OA8b7Rt=XKC{dP;NBeT}Bli~G3_i(*5XWWRoh8z%f!
zPI}4}B6{>tPe1x1*ZZ1BK@JyJGa;*XLe}ZLedut>hl|rJ<<DoocJ#?M`ZOL3eH>X1
zET2!sKcl7#8$M#vR40C_6JN`TFLmNuI`M0r_zq6|W+%Rv6TioaFLvTRG$&idw>$Bf
zPW*jNd_yPxDUENI%nvFFpE$kBK=vr%ti905+7@MndbtA5e&<V$BR+B+Oz38p!ksne
zWf(D@R2>rV9l=)|_Ickr?9l>`^db0QLbkH@!n3u>ACG=WTW}n}<^K5F4x>_gFT7j{
z&QSuKyN103rczr(GI@T2#*Ia5@1pUr!H60fPv*$$wK|(VWh5wO;$PIK$OR_L80S@O
zjPs7p^bAeW`?8b)_W0!&n7<k+ZLdVfCzs#QuX~I~U|@Q@7jCM=I}t&NX0A6JI$x_P
zziA35{vnN<em!nN1jDj9#%k&rkrX_tuW>A8GmQuL5w~kRIe@d*>PnWf8z_e0U!;^*
zo`FN0)p!8U>&ybI8IPHd=FE}8(L>*fl$xzkJZ4YGz+88|(yYD?xe;mf^uil8{tXis
zha0E8Nr4YFc8A9P66oZGztqUz9MbUj8kq=b$}{gbjbjNJhte5kf5C*@``c7}bU0~&
zti!CbqoHg#QE9qGnf5wP;~E&IXjzz4Wlw{RvNQ!Vc=-k9-+_G3T;)vswt;KzN;vz;
z0Sqhea9A0U84tu!XfxqK12Gc4OL*1suvsf*ybk+E-$YgMyJ<Xqb^^qpjI<kDJ&NjY
z(q#7Qb8PYs^Ti9$a-xLuRwa^to%D~f3Hx<{<iJ5k7D%~Kb(rVbf(hHS_l4?cR`SWa
z+Tp_ve1v_nV8REs1AEy`Gd;ZMXmof{7aoE6+n~v7*E?1$fl<s!!R8=O3a&p9SoU7t
z75pKw?7bM=?Id8?dwEaNzao~s7lZRpwwpok2Pr7B8T7v049bz$us#U#H7q=4n<H;w
zeGueZSizFa-YKi_b}}-`I`AXeA3CX~ZWnDe+eLupsoKwFd;Mc#+3v}V<91KaQ@9Tg
zHcPL@#bNc}ZGt}p8>R46U_)W}l7q%3d)wHC%sPe(@>^;O-_WUmtlG@Oxf)+v<E-;w
zwPXJv)L+x3+Gi?YwfB$=sP^(+I`YDElzdoJUZ|C4s*C)>a{~t46(tyPLeRw7e%QMd
zg`IN^3VYsU6!y>H4wOI&`<t@g9?6ZuR>aj*{~vqb0UuSd{lE84vJ8Zp1OywQij;(=
zQWA=lY(P;#l7J#536TIX*+2*+flUBA0W8l#K*b&e3zmTOSpqiRQzLfNSf2&ff}Q{O
zoSE5scM}kOzTfZt{vXlYy=Tgqe%hRwO9tv+bR?ZT`X-dXlh?vk{_G?TP<#5L2<ohZ
zv}dSJeljOzGLb)6Hz7-hasP=`ZOJOZ>n24f;0E8Wf#nW{1<fKGyax@A9cM;2O6buo
zRUHs5LLIPF*XIq5jW`&OcK+o$W?!frSJzynBWWtM8LOrO=y%>tI-H04Sq)Ivq;aBy
zcFjL@@~IjhC`SJ4I;_wsV|Kdk02-D{U1?|Q@Vj)FnubcMAnN0H^<v%@7m)vw23~hz
zdrgOZ;gp>Ju@1A)vwChV_)3QnOdKrlQB70W6(jLiwU`K)U95pBn4n5D3a%t<ljJB0
zYhBZhqiWyCY@o1Z+ml!BLY@D%25xtwHB*Sz;Ac7k4@?`^8r;h{4l*#X7m6027^8gN
zEjlkF+TlVp6f_Al<Y4qyw0P6t$%)tfMx8-@_eVN{O#8F<X+*?$uj4upG8k9&nxOD|
zo$#Elu5eYb**gw^BlOp?*QjxWSn)69J^L&dN4N&$Q*96Q5M-(AoB)`&2YNF6A;G*o
zpx~>z0Osw1V>tfyu7G)aK*3jaGaR9ppn^gip_ee8BFqtv6=59VSfM$B-dRo)uL(u=
zCXc#L?<VvW#Mf`{uGvFB2BUg_9TX$NK0(kn?l^*(qYYvBae|qn4P|&xPr%I4hA}*c
zVCHDU8Ge9ZW-lX{y?hRM_D<c8r1}jU&qK_%d3b%gV)8ocDpRuA6J5X+D$noSO<3;2
z@RY)E2)hZ->0D%aKIY7@>BtT&SeMwarR_RWQqw__$%YQX!n+A-?*O|AYJUN{2^X<d
zy`x&SYB%9iT?Fa0BXtpETK~xvK1qj@=G-Pq4JD<&P=%}Ro88kDbE%0z(Vp4IxngcH
zF{qtq_LVw@#KUOJFxPtHZ2>fpD(`seP}N=A1ZtlIO;m<<G*Owuv6D?32YI(>{6rH<
z-qZk5{k#udF|$vLR*j;;K3P|T_Pa0F;gsF1#Yb=mYNrnGtZfq34GWNK$Yj@AoO%q_
z(wEaQm-C}ZI{Ss-w;cGhUVwS^To_HO=e1)2vp{uWG%cT}dIRP|PX*D0_?|vki+t*-
zAX+Ux&y4k0P2CVjC*O;rX+gE?tF5KQ3|`A1r09hV{s2&kUuDtc-7v8qte<?3T6J`Y
z5A!{0>!J-GUN~_c+VD^`t;c7Pe&Mkf0DhR^_(Z_d3jo(Ld@jMyVV2li8NL;8(eNdJ
zw=ry;qgLZb(Io7Bh1k5qysGD!4JB#dJxKV^oo!}=Cx4U<f7c0{Jz0klpWkP~v70ha
zhZ7HOJArYq7BzdBj@Wu0Zo^a&A!U-p8L-Iwn{}GARlGiP0<r5#61(!Z>gahUnom#g
zSMg@Qt>d1}z-7fvqIZ@l&tZjJZTpdf{4W~8!##1mWe5Rex@M|}n5`vgfTnP}iPSpq
z$74|a1E?mV#huYwvB*!+d5__Ia1lW5J^82U&=eC&CqeQix(h9F6}r$A+T{R+dh*t~
z^5#A50%q^l03D0JOO?kAala0yWAVE};b@y408jyS2*|>Tq2bb5tF6!B0X~F_y0cul
zPCn#Zm%DP69CEIQT)8$Ma<1L3Tpt~Bt_96iZ!Dea@%*ZCahG1E!--6Hn532xM9taP
zy22B6IA;FrTV3I&>TuGm?sJ8or^AU2Jn0HAR^jT_*}P|U4A0lkH9+(Aeolt@df=(^
ziz{zl`xef`&pXZq%+AyRVLdEeuY#@zn2;<bZZfMNFUy4}Z?OxQeTxR@qKfkvYe@FP
zngDk9EPOyGrv9C;6I1{G-4%X?4yXRz=?cG9hg1K)?FxTXhg1K4?h4<f!gc@d)iKn+
zdA(y)A2InX)Bp*t!!y*FYvJ`FZU8e+BP)>l!Ji*x60{x(_#0*(kfI8M2#q^iBcjGt
zxx#1baBAFIS9paEr^ems3cp^5Q{x_Xg>O;ex^bWC7*<g{Eu9H4yN3qoFhNYF9`jxY
z8}pw+V}6_)QKklH@=P#@x*rVDHz7nb-H0xBAzEt?^*9)!FASpmYjoawI3KG8c{jR|
zKjT9Fqe0&DV957`knhrY+d?#eK&1%%g7<YOhY*qX<b0-sjAgJeB;;&@A#ab4%?cUv
zrU0RJu1m-nlH~RHr;fqbyGFpk`!Yj0fJoq}>`!r{0VN>rxB&#^P50PeQ{+bs78&rG
zfnLuF>&1&9DSSiGP&8IpSCB<SgK~w%ve36wJWNk3EEa{nRgzH0Y4wnp`nOWkr4w2~
z|LmV*r@-dgfT%Hp(A?R%I>8;cAp}t&VN>~ivGi;l-$<S&zb(%aYn{%P4iT?RWg~MZ
zX_T%TY#@FU2yY(pCdu2RGcgr>MFZrG{4`^jV4Z&8v9#r{hG)O6bG{y`E^ak&p)NVL
zwQ4lY=Dg!HK!xfv20vmT&)31uK=wc{{^LB?K;DK9DxiUE%}&H84ki#Islcxp#84`5
z!*LW#W7&pNPduJr8cYRyPk^Y(gV~njNp0l8G@w)t$?Ytv0~r!|FxzQ|<c_mwG}r4&
zheorb^&V=?!}%Zzz)i?Hg?A&k01zGGaJFfqdW?p1oXYmw!`WG9;^91512mjhF@}Q<
zr)Y7w@qx@Ar?cJ>s;?(6%Z0n-S`F}c-r|bU<9TWaXdgP*8P9KlZo~Pm@$4`J+QWsU
zN%ywM5^xW}>^P|4=|cfOGawWnHw-Xa3i@#RrwC?Cfr5*MV@TOj(3j(BNZC+eKs}`W
zEUHFmNc)8iY1_6D3v{q`|5QDsJ(FojQ+0~3?(uj&L)0M-X}QiuLz;l{k?psKbfwP3
zLwdgkXh>%<hTj;{<3mIGh|aoL6-?4s-m5O$CC%HpmS}qoxR&T@4CyExd^hbR;h%Lw
z9nd{6!!@MK&_ldLPqm2HW~AsPdMdB6_ZY+yJ=G#|n>)fR(bG73FT=D%6})hyS)yaY
zm#6{tc&1uZhtPPYn(@qJ&DN*$CS<Z=YrtM=pCHXrR-yiCpCHXrR-qx2ci*Ru6P?Zu
zu+}K8vYx@<G6u;7fx#C6GCkO#dnk0l=sDKjX?pS;HX4(MKiarQry6xXruO;Pi0Mc{
zH!?nn6wV9XvGzSeCNDv<h<nw}IT5%Nvd3f9z-=aEv#d)EvKx_vk?{*5dkP_w_cTV<
z^&~=;_k(NDOZsc9yuznyfG1(4251s~fx?Kx9E44X7U`i$c!kbtVYFz`K>Ett?#7)O
zr`kfi@AOb2R<YQE1mTt~N@r<5)$k)x;DQwz0j;si6#~7+#Hs`(9g{abT?yYGKe!Tp
z2!0GZyj<s|cIG|e26njs!?c1}5qosFvm)L=sdd?|6)^|B%PV3r&%@Ct>lLBE?F?c?
zDDccv%!(-G)UPv4D?-83PBkl{EPO>6P_Kw`i|P<s5#`}4VoCUlFrZ!$OT$;h($I=X
z*rHZMCC~X!Q5D`pTgKp=)3lLvIfD-X<P}j8uZA?VA}+H8SORm%{Ya!H@p_$V35u|<
zw#HKmS`l}S(UX{8XcF&6$mGY7EMi5BNDZxs{PwEIJ%p^<T5FJ9j3kVeUkF(ZLMHcr
zpb@ebK3)-b=vvWyDA}z6UJ-3{5xgP>Yk*e7eiZhbE8>dKiWs4@#$mL~idg8z{euQb
zd^kUp=y$A$K^;_Q(27{65d4l6k)m_+ikRaDE^+~8MVxgQBqAMtM~Fv!j#A&wb*+er
z=v~G3?`1ZfoCcWLzJi}3nAyI9$BqTeZ2vw^{|dp(_7yy3oMHR-hqHYH>P4}|qH2T|
z1;CM*AgekmHt|6CqA;Ld6c2_kiU&iB;(W{=vTWA!v~NIFcu{O+aMtO1QEX%I9)P?k
zdLI_CD4w)B&eDtGM<h~<VvJ7pIEt`$SXq>U7R5tn=y}X9G>^YT$Yjrhjpp&#@u5XA
zs-r5hKT_E*S@#-b*B}XF<rhL$i;&6rjgfUclNZIAx>md>F46!mipN|rvl}!(iy|J~
z{+o;9snDYMQfKYO*?3zpFIrchCt|7#n7v&CB#i|^Iey0q5uKbB@`OV0J64F)xw&G4
zT!2{=?V%ft*Wp)_)P#T54b}yg=SBRTds~8XZk&PMopVJ22In^36?ziz<n<T@KF4wg
z?>u!mOV6%58Jq-g#!e);7l>#MZsy?25nM{2qFAF#g-Z$lv+of+()*FXd7&X6ezw;$
zH!^tyfu%I~morae_4Ibm^^xZm{#|690*Bf*w-;mWUu2bH+o8=J^nS6G3koWaJnQ1W
zMO&MbFSbeve7*FIZ+8YC9d@y`ioj8qZ7Du#Q~Q%!O#-G%tXmMa{!;56{4^J{zUxl-
zJ(pR-I>4|3qdDE1)xL_tF1J4G0{;U1(bhsfg|Eg-`xEIwLF#P->T#my3acKQlwP?K
z_%5@4g3SObhKI~!3fX1W0L<6VkHm%0p|F+Kjx&)4n4UCY?XMpUycZ+LGfF;>>RfK!
zjQ4v97k+nBIw~KrRMIFpU;vB@<A>tf_ZyByu$6}d^x~<q=70!U=O05MRLA!GHjqGm
z8-h1fxF3F65lS!alb|P#nxoXBLD;iF7g4F_P#!MTD?dcoDr<5wMxd=lCm+a1SK+5Z
zJgcpnQV{nA_YcLR4(+TSt>a=u>myoEJglA7_ApGv4j$2ZSj_O4R5Vd^_r%1EKs=mR
z^eeRa=r#yNedvc$HH))E>zEkIWC4_|LKuc1S#u3Wr^XT3!r;f4T2^Wt<!IT(lNy(V
z$W}e^g3}X2fzY696BUNNZ7*OyvK2NN?6B}ycQFHjD=yI**@@9a4~$9fG?Pn;k46E5
zu{zt+3$EdH63uO$8s{WD%pKtbcXS7<^wfY(je}<*_;7bmP@RqlrHgTJ9_a*z*Wsw9
zB0Qd>orWL9jp}?P+A;(fJ)Q(7%h;HsT<z%+iioB{#>R~6=#K76IFmb3Te=PQ2&pDO
za;GC9!gWW0o)IdbM_2$#?dcA{esH44^gNZ7jPC9gDn3R9QAx)}2I79;wt7zWNwiLj
zqgo`ysFCX%jasO&>ql`WJQ3l#y14t05~;d_G&tx}k$f<>ZU9JuXJ8^)JGfH<PuD>@
z59TMf-w1U(E4JCFjx*z~Igzs*XG*rQJdSr#6L><5HSsWN%Mjgbz(V0eb&n8;5{DgS
z4Tw7nNz-F8qAB_CXwLvnm>h#O7z?VILLnZ{h>HW3ryWvzJR|XnNO~Q02}13Tt4;*2
z(NrYpnu;Z4Cz*r<oU8!2g}SPM^rt!?KJkzUv~ycWG<1y9d(?(hh3TpjJ5d#U)C9_4
znvfRi7F23%C@s}t9HR7?5TesVkpp<Xo}r_4UyWCzp(>;M{7iRg_^2aw&XKr2<7|Gx
zl$)TN0|GR%6V0%RgFUCk(R4kBUyx)Hy-@F`tBEidL$0QHh6$kgq=GWloF(u8bkQM&
z!jLeUF@MoT8u9-cx~N98vkf{xBbg9ME4vU$CTcmj6Vc1A4vh$>m))F*NO}p17{VH@
zC~yy^z#Ydj4eqJ3QUQkT923IUB=yzQk!oOaMNr2`dfhvOkm+e;G^nShz=(FL(Ki%~
zYN%j%9)|+4mQ?p8VsVABfFx(NDHhN_EC3xjz#Tvowh;>$WJvqq_L}V<hqdWBo}>sG
z5)jj#fQPt=`%pt;KteS`h&F_OxDE($h-8QAfSf8cG&e`Msr^X82oAy2epHiWa5U3q
z)a^u5LIj;W$<&I7%gIiu1fCL#Bbw`q19%$6DegpMaf~A5sZM88sGH6*e*&FVtO6g&
zqKYtiC5o)cs-~$f4l!QQTWb56>Xnk8<tC{VcQ)cQtC^sZfr^@5j%e~Zx+R)qPtq;X
zV7kh#IYowUkdDmMGed)uT_E~viYqa|bDQ7==ea8m{loBr^9?VUrcs7?!F0n5W*A;D
z)9`{UHQQ-T8`7U`g8m}@|Hp-5L;OiGh^B?6sB?lNA3I`jOgO!F7(tU1+F=KcqJNVe
zgmWCt3Jjg6h1fLt5Ahh?iZGIYP=eoBymttH7(Z}Gzl$FvI{ct9=Sy-T4YP2>Y^`T0
zW)Q|daLCF~KE*!<g$00r40Z=VZw~PftuKI4go1boh`XQQ)JAC)U>}Cd3^iP47|4oe
zxWi?ViOXpApj3+yM9q|HG1AaK!)3rCNY|iY0$(s(5xhZDKvsZ>Ay9ik;=mBx?1tHa
zi8-}Nb_1>e`i@FM{8ofM)kW~QWM3G4O>>cYNA3n&P^qY@r&FyXNH|H|+)Cmy<8}2l
zQ9jcJq9$i4&U7}$L^q4_7?GRuAkjpsrCX;t$5m~J>XTd`1~{Eshs{A(C~HEQZZ!DB
zgKC6gYEu*dwxBy0y-hVa&m>}QU}hG@fG31N)X~KmD0VvXX^uBTy=s;P_F#sV(q-w%
zsCx$tg49LTpISl5VdjHo&f*s|FgFH9n+qI7*fe&J!qrNd%rgq(oWXju=ejz8M|+;(
z^ZDw91{AL2rd>+iDKIRq(6BhaVR1!neny$+n*fLBEigfc#`7H8Pn0qGM7xzBHU<5!
z8G_<0Ha>wYP-|ly!bxCUq+KXJL^ND>AvOn*U8sX%fnnw!w_PYq(Qzc}CJaDS_~A}j
zM4*hw?J@%yDjHz~Qk?9l#w73PaEzMc>9Jt?<CuWZAu7I$Gb<JO?dnV?6+lKG$Hs%%
z9A@;vCxi`Ig>n;ampO)3oFbmRoW&QWzBtyG-dK|!Hti^qN=6;cD4>YY0wUETWWfQq
zaqKkxHFj>~05>VsOLCy0pM%UA15;5H^*9&R;WY*`4pE~M96~b0MOBA7bYhrUjLKqz
z`X>`o%|XIQ0<jU@7$PD@)6g_)l(U#eJHtT<Pjs40bT!<XlL#9|s1&VUD1Jg&$*AL^
z(5GoknnI6pfk^CTD@=M>)o@dAGU>R`In0F`JWer%(_KZPwr*PvKsR3jc&1YWGR=gk
zAZ}w$69jgmA+WILm>Ho+AKP^ZUAiHC&^VN(C)1EMhtf?}Ye><#DbDIK3-4SK2hKD3
zj7~5$ln}%Bf5#*gUTdO2Z52Ihr$>!jLwy(P^s1r&abW?V0P*es7|b0(2sF}6bojTq
z*Jww*Mp2C0C>|4<!AumLL^OwwaIg`0q?%?&ITfbRquoprn`dUJCpaq=qJ1MS-POJB
zVcIrxsyn}+kuFoVnp`ktD>LFTW^(m1lgnkUKFFMKv&Fx*PaqQ;OA`%!829|I***zZ
z&=LS@GOqSi4nb!O)2bX@k#(0yRgE~S#;;bMYYwV_u<ev1F-tKQ4GAP#)T^;raIE3a
zy^Y$_2jth&*HL>KZJKDNO<N|qI@mHH7EGqRP{_ZtRpP8oC~8gYHw-bWnD{MhTUr<&
z=HkS|RSeA??3AdjmK4L)X|IGgNGN)wVbt)(CU!O2L@HfPH;8D-Ns7Xp>@<@?ji#pA
zHFUK`)Z{c5HG$;M`WkIYq#A1CbH+(glQct3#u{oe&QKGmXk?;wHYLt50g88F^w74y
zc4ze#t|LCvV26q4juif1L0+6`rB-uiXHuvDc)>cWnU*(+=ON*n_9PnF&byiA-91wL
z?m^5{iQhflD;Z`cM|3uVIg8F<V@iToOgXg8+1p^E)zF6kvY;u2yDt`#R-F4e>nhQ;
zP>GCiJxGK#qB`XU7$&TT%5%hZg;B5m!H%4KoGFTi{CHjM=!j~cpj%BcFDgDnEpFO>
zr6sRJhv`JRIgsT;v8bQ32kQ(3;es=>A%q0NHq?kDj0z_Symde%0p=lWSYm8Xa!@Lg
z0J5V54D8S11xX;pE9n)xvf(6Qj0rd>N%(7H^1q!Vge($zyRfmthT9zM$Qx|vjFca=
zr8<1Gu$y}WvU|8d(?bh1SPM#^fevr{Gs2DFVcV3E<vK1Ljj9E0Kh(s=*6Q#w!nP(O
z%LubmH#TEC_UbT!%duF8giW}Dgc^xOugoB{X{qG^MTtq$2@zh>io)&GSes#XYIYBC
z_Eim9$5zc9LlWBYhF+^JO(T;HC7EoPBa`tKrm-~B_NAueY@H_RW#X`fY07nsDJ}y8
zJVYFl#x5d`S%HcENsjjB+OTw_9!})4Ca{Z$!$3fYnvmaE?jZv2-m?VpOQXUf$n;p#
zI!f>M4*__(qS<FS!$ahDyj$g?6%He{vgWWELE9}bM(_#ItOvO!5a2K_Hd0en;I)FJ
zZWA@zfv}MYI!(3&S3XoJ1E5P}WFxgk!={tTuF7%16rEo)9AmK7;CUJyfxtdhw^YX(
zqqUBmW+YiMS3?H$cxJd5?M!#c%2aI_9WZB8d*HAc#!_bi(%d`O%~CILSnX^RjzbX6
zqM%`zW5n*cI$T$Jo@=yFaK0-SzzYrkD^Q4O{9vh8;|EJMRkBD`OY{BtDj`FZdO;K1
zwwSSEhAnhTQ(U#gaMeYIt1dQNwbU?CW7{b+0gjQVTm?A+ODt!_DdMrz74nz*|9^a=
zSUIcy5%0++Rr=rBJSOI-X3>8}`FG~3+6JZd6SiHfCTt(WmB41hjrjeDdMPae%+ztN
z3^xvF5(j?DGJRu?G!QG_GsvxZ40gyOZR$4W3oyhx#G7P!7x89Up4wuhc`d#Vb5SCc
zINVJUDLFW)A~Y$6lEO4k6o))2H6a|O$PHTxT-5bM#fr2Io{(vOO0^+*ipr%td#I+G
z7?R=LP+`!_NW;5RU+~H%S!o)z!OdX8`DKVUQjOg1l#T&j_l@@b36W9S*>%<ItR|=`
zz9!|A%^XzuS}F@Xam`3-dnguoLv1HFrnXbuCi%at9RDYM(Dj68*_!01KgG(?6di-b
z!ze2baqAHo2p@Dq+#Dqal{0SNWC79g9F!G@%z#c3&4N<m2)pcQuGThtM~0J1@;BF{
z5-LU$QZbAlpgzC~vqzJL78pJ>$wMM|pDS85IJCxzCLQOZN#P{Yp<6Cfhl^+qLjq0X
zjE7{T$;53*WqPXx&k=4yK$Hd=hO%OWs3nCOKNy{hJYjU#lDr5_qmh;5WH(|~IuwN=
zg+o_eiE*H#$~XcFs*EG>j#S1G&`p{LGzSc6N&Eknj%betxD(H1p9XSb(0xp2XyQr1
zS2I*;^zEHpC-Q1`Q545GdKk%U<l=x$y728eR$HBfVNMH9u;a84N>8)Faa;%m)6tiv
zTo?X*nqlS(%>p%JjN<|(BPrevwO~Pwg8nSm1!tOrUJ%iM6y*>NmY@vLU>S-4qrV@F
zQee#tP-|9eLdr0zXoKP1`eX{>j8Lqd(<#V7q{47}fG5zpb8qkuaW6y~Jvdma70xGA
zi~z#tQyj5O+eaxq_Fl})qs2}ZQX>YTyaSzBaY&MJo6~X=ds3@jBZ_&l(KWT4rG&0i
z70J`R10yL@RoxHT_XEDvRC(;hL^w=<Y&0TAENq!>!Pyn=189|Q2RcQ6Azqm&XC~Zu
z3Lt=}5PLW%iglKuSZ5oGH6g^Lm`hUk!M!??h@ImmVv|(7L*mlgL;e3tL~;={TKW!A
z`fuI{iZqFK4mZm>ep(T($d3KDk+O-+p%7MWOAfdEs`=x#CP#=$krr2N3l6s?yF{qD
zts`?dcHl<Uqbct4l5p?*JL)Ldgc?7PQAaU$EWaTy;xKEkF}8-;dQBi5PtojA8Fs^u
zr$9UnxAcNVhFN+^X>GLUhNFOv;Hk8CD6sAzx1HCRc!z+Dd4@VB-eKfkbm(?ohko;Z
zlP=9A$QnB@f|Mkt<tNyC-R#IkSlJ`F2{{!T-bzlzDHSDxy7#x)Iyxqd@}|0EQjk?+
z;?)$Gj{|~~qhiDvNPl%nh3q^=(}Uc{bd`zXpW)(ATqGRG6hm^KTv63f+`xTm#Z?Ay
zL0@&FKBo!F@AgRs!H3aT38Kc_tJzYfGYnM8$r`a1c-==<(9&~ti*)RHE)cP9Q!h}R
z?`kjGji*r+;F%oBm}j^GFkK_9zN$xH?^Q3<vpLFiG0Rn1l%E?KwACk~eDY7$-LK@Q
z+03@ELSXh)5;q)^mBc+)&34B_VV((a_*1@{dtIpFo!A1CLGj8a`Iu|>QorJad6Xm1
z{=3J`%^C9;bH==-!Tr~9^Kf2zlo~Bs!%l=bZqCPPXqpih3#lS}<lGei+6{LG#=~hG
zJe1Xa$lM&zR7WXYhjnPT5-i>v(Cn$PV|6K2=NO7tBI6J)&c%<ycN)VuQRB_TFpktX
z+S#XMy~dp6bnz)_oMMh9(&1R4Jn(3a<k(5>y;^;G#?7%|Hz2wn&XVDWWtf5CpVXx=
zwi=FPnCjqAd3r0>;aV8iNN#?-gTUYoJ6B^^tU2G#qAVGF$R?{PiQ>7&Dyvdp9t?M<
zfK|3Ji^V>t(<oEYh)_D7h9qQ<G`5{lhRu!!E%Th{u-TJ{&En*ZW4dO4E~W6omrJP(
zMjbOcMCj-!T)9kRNQGcD)V)5<2$<WZk#igzxy<y3!r-(JoK2_2Oa)(aTILv`SancS
zmv=MGJceVm#>#omb~AD)5l!%JHq8=i0+Ta*IOJG^Jx#~HWcD>Rcgj#qg3e^p0;Nze
z31)EAR|M!oH&D00+d|v}KGH@^nCfOt)Jx}wi3!-gRDOCmg{kBUI%^a1)0=4k>|GkA
zEn95}(ypa8UCnZ~CUY4L@h(@n%saumLaji2o*qQqs5wTGa38SYeDf5~%_n@08i*~+
z5H@r}fgW3ptx#2)n(KG>xjxoY<gOb|+-O`{7+T=Mg^Cu3kEuqn(1il4r$lvz)_NDY
z>Waw4sx}@^sjJe!ToxKH=Jw?+G_9mfWZKefqzW&!xR)Rc1uTUV1yUm`5Mh=C&5BAl
zjFOf)$(3BT+|rFio0Vv>0wGE_zDT{H;urH9&D=}$eySs0Tp9{-B#6r_w}5fEPT|B~
zp+ksiDH(lb2+N@f9Yz|URr}v}mxG&(sP@Gw>m9l4Yc{ONZ<xD25$N{LcpwG(zJoh}
zeJIVzMpy(5GIrZ{%vy&#vmosD8fApiqCXto+*9Q!2OG_tqg^1zy|bb~<OHefPl>v?
z=Qqv2t~@W%4EEWlB;DOfVAgXl7s}Do9RQzcwa6hIG@A6V|0}T{E{{st=;L&an!A0?
z)a_@)VjN6TVsVpFta}#y_j~<^ggLr2Nsoe(uFPpUEMgTvre>3Z#jW<vlyuB69gJ<6
zjm&O8f0rmpgQU*MF~V@!&@no@a$w3i7J?(48KY)A9T5qWK#mRzz>E*ul)_~oMvUm<
zw87!dm~jRjW*x~BFeW=IfgNoP^M=)=$*rBxVK|zwy%Yr}^^SI&T(iBmBDECLCq%K?
zpzJnnh8TKicb@1b?>`Z_Q>;75y(*}O)%FvLC!=zR>kbSNcPdnG(T6SK!LH>4b^x=o
zS|czit2J_hX|s`{hERto32SILN7HnHwxUo2s+IQR=e>E;6R;+SYDH;`F*(FQMw&$k
zmwu(2jy8)B#txDk9p-^;Ckk*l1B5}9!U;OX1RWY@_)8t7>29<F8lnFR|9Vz$LKN)J
zQb)1U-B(6*cdm?Jg;XtkrEt1cQYMKmb7{B+>QYS|WznIUj)KbSVx($z5Cv76PTg)y
zeMv8N^k%J!y7fxOblQX-h#e|N6?3ZSy4=A`1W*=tTB8FS^>biI{nQ5PL2h+d8&$(~
zTbF`rlt8VP(%LnMPcSBfKSLFDHP#fF64E=h@=iAg8LP0PiiWqHw9`?Bt&BF>DOuV)
zCpp^b$;3@SeH`tSZU)k{(<q~%&B8G%AuTv~*gDjbZLUQ54FPtUT%nkfc1jAV<}+cY
zR^=w7%_i!UE+vC*qt57t0Qd))Y2a{1>}WA(<m3}@Ya?<^bn`}Np-m{M&_k7hIh`7$
zm`#YMk;RoefOs0&TXp3KoUBB|NIPpN2bi0_0fLp(sV)`@S6ZcWPIIv{HY{srTCGpb
zFiPr7rF!DV2UXW>7e8fXQM=L-+nnXX$#J=^#1J=NTvdgKylJVY^1$S@RtuFh)J`;F
zuF-#uLhA5Km{OfC(+Xk%a_fO2@uJXG8x-!>v05%Da>k6hbiSS#q|E|cph|)&YPh@c
zsMYFjg!buZ<BMID0HRV?B_K7FxjF=3xeMe$SQ4o<F16e$Y5>}y5;iKJ#Wj-KNe5Lz
ziA(#WE@s_R&wJ{c6%ooR>7PcnQ}VsiLrG1#gf&vqk1pjm>K<sFT6iEyfl4r%Cy{Xz
z2Hlg%gOVnU-s!|wX)Ls46_!;_2>Mxtt#-mNbVN@)^bHYQ*oyxx>!;SO@U(ar7;w_8
zd2_OoatmkW<`fmoOPVpwKRb8s)CGk}GiK$^oL7*WG`%P{Z)Vc0g8agTg-L@7X68>T
z@GnfdF#n9vXP<Jm`Rz9-v42uw!3@uzh%R^TjJ%?mS*8e2Qc+<+k_jvxa{Sce2lva%
zom*7gFK2F1()3(^p-PgMJ3T39#*Cy!Nd_klG5N!g!R|M&Xzt9y#2NYd9`wuEMbj_H
zn&F=?FEJMZ=rkRzp+f)6+<5~NXL;~lzj0ZGh0}7f#!Q<#6IIDcoTV}#$;_;5B*C4~
z{)PEjg(@}Y#49E;Dw#V^y-rXGRL<17GqZ~Gatr+il`dN&cB-ZWPMSBj&_8XiA0;RR
z6aBe)A;7_=>yFEF?f#u;>~R(EMYbh7c<d<48}NS2zg6Dn==V)|U&f>gdb`_O6J>kF
zrm}S0nftQklRIS>F->NR0f|wzSXU;~MS*|hL~%rIB@*0a`4L)ECaWHqC_4H#h&J{x
zOH8nbSfXVR@sC?U(E*S=0?F<!qo9wh0GDj{j`fNQ62;L0Q798$6VqjtIL01eZQUj=
z@QV(BrUyk&nJx!%@_<+3rfRPFV;>IBkOMP-@uyO%Wu={7bRnRxN_}YnSKB^{`-0<E
z+sSzUu{0SGD{TLT;{jEaNuMYPa3i)M{7Wl<&`(QcHZ`JV1A;!a?BSLenMfsyNwOC4
zmzBv_0N+?X27LS;$OQ01X+3}yT+$Dv>9YVVw<*<+Tm|*MqO2O>KUh?q6=m6j0qn8t
z9u~l=vH(MY^Hc-zw5#d0+B>--Y*ZQ;VYSVpf}Ed~$~ts8di#8O_^E$4+ClE^4@wET
z$8Nf}&qV(YiR|rXOJ%x`>tJ_{^{!qbp7+?tT4OoH=ba*AWx(#;LF5EPCwzHF%&A9+
zKS>m1%M`g;=GeUt7jqlLVA)Wqvs8!{lyG77$1=t4-CmT`h}N=UeMD?fc9Urw-5^@Y
z22<YT?K1a*BvIrSF+egMC1;h+PWsVAe~5q&a#BT=CDZF=Qj`!+mez>oG%k<Z$*)I6
zi5q!b-nY@rO4M+$Xo<IYvAQ%^wd)+NLlvI3cMUyHqsK^k^zo*9?ZL4LBSj}!Q?Vd(
z?8eQh8U4h3RS$6rrX<e`udMcNTlccKNZ5&1S}Oj_cuxGhxLPIyi^Z`UUlwt8Uu$b-
z#>UNJk1dloW6*+&B~`LY`0k{HqD8$Ilf^4OS?mXNo$&ptf_*CZoy9c>-Vho5%3_+*
zcMG398lOr@O%f#yGL9CiOcO`T1i4UTSBVy~K`f8~{3eSnrFJ(f?nnG*w*Wf;S!mlq
zlZ9V|6%{QWTx{8~h?#`VQ`_^*{|Bt&ck_kD9K>NVnh*U4`CU?0vA<wiA=uHpydr<@
zyt$+2PRm<Zm>XhKXO1tJH$5+F&WVOiDblYgy~8PVfJtsrhch+iBL&lPiIeCIzs1|Y
z{tRyeoBJcYtx?N}he1>cRn^VpxN>On%%T|~<~VMezaV!8u~)_X8gauW8Q|l+^-LdV
ziRMB2zeUhGY3IP7u^eX-@lHsVvyV+#PRsvmdhDM}YfyIb1_|=6qNmKdiURFH9k8a7
zMNAb)PA5sLFxOXf=~RmIrt@=|SNgm%UZ(8YDoQ}0r9VpSK)RMyIU=tPl<CB(B7h^)
zQ4N{&8t^P^h_cQU1@+>Hba6pY<{%8!@`)p4V9>Rw<mz&n5EY$*OhJzyWeBfq;Gc8p
zc`Q9r=uzyg#gC1zzTmlc9G;duQ#YcB!ekamq&=>+ozlThju%BB4z03Lfs<t-kdN@?
zl!_KUyI<V1DfTh(cDMHHH;R%#rrq;!yBFHC1MR65ZK*wCT{-G3zVc9;YSE^5%L3G>
zoA7ZYkQ<q_9cixSG@p6EZtKP2wQ?WAmT}m{RyFbkk?-lUs&&Y>%_1@tD>Jqt&Cg{r
z8)22`fpZ&V4xw6GPUG|eP;~@#TSciV$~o092#clOK=*rDhQaSLg2m<K5LY?|#ryQZ
ztvHN3*i4)}1|7Pj3?s)Ky5GtM@mpF}hxC^*+y0uRh0ATl3MI2*FHH+;I6b(C3<C;!
z6YB`{N*Wr=Nh&g;yc7%UVE+7ewVkfRJ}V{8EJSmER5N}lHfgT1t7Uunt=;Qz420b?
zPMi+PEe2JIamiwkjLke}ggv06DDj<$Y_{aNNp-Rx5^cE3_N?|^!foSw4)nt_nrdjj
zd<H%3?s4?9ogS}<7@vJ+C%jrBj%IBFg?d-h6TKE?_ov4alEG}d2j`=SW;41;^v301
zPj3V0F>w$*?cVY9vxgoEgYXfvwM3oAUS>i5q!_Q=pD}JFfH8ihD%g{QIb;u2@SMXr
zFI7-2$oZ5*9!*b1e+5@ieoX;fKdx0j0@LV0183ulk5M*<@!Qh`a-C<WI``x{OMEBN
zU=_YZ@q|a#AjI<4d3n&i)xYuUOhPjm!+)#1Vk74F*20R6LK(lcP#jq;9<(5uh-V;2
zi@$q}0Fm$;1(}NcR)Oi>O7V<GCWzV6FIJS-iV90+bCYD%pIAgcSs!mlE779A5iRbC
zvZAD@(4xgUcvst=%KxxvVKhdqvHqDHn-yNtO8+CGg(r-=fn}W?;)|rHqT39tBmkr^
z0nEYyH^ssUQJ8{@07r5F5K>-$AA?6uhuacAMu1Dka3CEQDRCfyJ4mMrwI~8Uky(CN
z9=B{@B0`!m0vyd4fYX;6F=@v6XCj6)W9%|nb{jQ$mxKkzAu!5D+Q(CL+8qdy4GAN}
z8e7(Yg1-;VF)AtzT;)dFKj9M)x$5$O7)hFOcjA##(&<eG@Vb^G#I>w8UtUgnKxUno
z5EOj_VnVIxOIl-33cb5LT|8}3M({A=d|!Jg4q0SoM$wJpX=r{4GV^NvC)!lS4Npkv
zKLtEALB@j1<;Y%g<CGy6*gX#aMlM;sZ&RPiVkT(7k#<i@%=C#Plf}$x2r+iAcF2=$
z_d8q^*NeD%e3wdyJIhH;eF;TIiF?as6@V8#e)+cS_Lx09&h8ffP^Qe4S27J@vhMw!
z-bD$MmGg+aT3)3FG0n`PfD9EZNk*~*-ilRrpSZ1dj}8+?*~8+PUladUG!`!*qN!dC
zv1|OBDKbGeVJQ%CK{3ZC;*!OjYH>!D-9LU6RL3Ms93va-<n}{})?41hKT^hIF;2!_
zlZpnOmzydUW?zH9jN!8DUeP9CkB$?u0WlLFALW=~GVpJWKs-z*@uHRf8xWKuF(P%P
z*%*W^kWZq-^Q8@V-)3h+TTT!Q({GtYJ!&6+82!9PkC%~Zcd751okJH4TOZ{W)9pbP
zf)eb(*6KHg6z8uiT(@cHBT>!7Lzs`RdII9E@&J+@nY{z4x-0cozJgf~Tw<-QgaVc&
zWFI3Uh~_M6GRu{HAT^?v_m)*{pmp%P<sZK81q6HtBZMukvt|1Cs|vps?V${BywUE}
z{@d?>+WO9mspa*Gor`^|kZiLEq7pAH&aPm<58$oE)a+Y?lEFCfzQsHrSBPxUqHgkO
zNc7C&U=C7k6?FifTAWSRpRK|N;2)fQvruysk{6W)N!k|o3N<}B>@$UO6;Q_b?XHI{
z7pIVXVu2Uec)2%h7V2!-Z|t{Nn$_YJPqlcD%t-4kyPqYlDVNoEe~kj~DJ5GG=l#kK
zdKvpSi)Av^?=n%9a|6<>f_MpIm7NHFoQOWUrVO^59SHiTRfFz-rc@?>1L#>>`ZfaI
zTxt&_E74k;rt&TiAkmFSgal5nm=O?1K^@7#-$MK);;%XWCTSxD_u03qfxGP*rg2Qp
z#v2xWGgx%ASfyF?;jH~P`wz3|5Fa(Xx{>KtDc*mcsoE425(*A#TZZM^!`A2D&F2l*
z(UxUalF4+84t02L6Lw@$!J7ysUghG-z~axHlcn(KH>HXz>oe@kMlFX{ZA{L1bxh8{
z7OpfSpb$zm*|JF$pEJ!rYsR!ZGAUD~iA%0G4Dmh7Iz43)R2EvULAGdKW2GmF1vTO=
zn#IuHjzi2GvdVUlB&u7>0cT4JILhv0m7n{VJ-8DrXq4=93O!CIla&ZLg|z8ptSCx%
zLMQ3xbeqa4;VT}Ma<fcXnlmZ$F}rUkS<mrnIDU^u)^UL{Nv=gSicz)M$ENJdET=wu
zV8sT;HI{u0l_YxFy(|$!rh%zqVY2j{DHi(WB0?q)SL6;$ZHH)u<f1Tmm0W<Tt|_Cs
ze!(ny4VS>?Tv%iKfWMN}GhX_sO)JWXpTBESP=ehbrO1Rh5fj`XO8n=El4QReY)WeI
zu9owW<qcLqZ-uHz?^M-0Y*GPR5$<ACmhkQ@CA`%()xD#_n=Gqf<t*`yhfa@Tm}o*1
z^HwWL4&q1_QItm9!|qB{<tavAYX|WD3}dXdt1-ZpgT)lsei8amsU*ww^IQ<53k2N>
z;R8YS9MpjcPJ)#JF`|}6R<x?3!Zr7(EG5eVm-#cqr71u>O_{yFTg>YF6@prz5P!0m
z5&w;Y+7aS(d02+%mM*49rHwOP_OjAzO%g2<ho+2}uqq>C)#Qoeo}T<6TbAvv9l1j{
zdZEsKl(RQyx9kNQj4!EQ%J8&)DNEA|Q`V*xW=t6}ijvt~<Gh60_OcebchVbQ4^N^9
z2nx&OOW<qk<msoLK9v+x>vc-V9CjtWQ964mNci)M$$IjDkcpQ-^kVBbq$zJ8N>B=~
zcz7{PCaM#j6|fsoC<0B#)CsOZ>hYpg&GF+zQQ{?{I9;ABimT2S#o6bNvyX58mE2VE
z7-ie4wm~vfrBQ-cT*}zR_rfRU$;34ui(_Hc=~M%jP$SwWFBh}xk<nVEb#7+kuM#S*
ze(JysnKUaycA1^A@uQ55Xw1$<ByZju<%7KF6GvBJTR~hH6sJST>MCQ$Etlmg-B*i9
zxLt#E7X+6nBuTR~-V-Ot^fT;p;^xGojc=FGB)v1b4)66PWZ%9sTE;G-s255k<$onw
zO~jfKviQ6bjgG>GQb|Aou?$c?#1Px8DXDE76M><6D&fCb;r}%nYuy%s5}L%nazz3q
zv~&?c+d3hDgxmmEA|e&-dj(2p!XpdTM*0Pq;|tS8`<f}DRk|pw6UE8r_pbn}UQt48
z^~Y#G-ml~LPow=0XA~yMt_WGmAr;<aajs10BU;K>%<O>d&|AsT^>&X~uk0vkf5VnH
z(KD4EB>vhfC*w)>EfTlrSNKttZZ)v`&kgGNw6Y{L$$pHcA-84cDLOgLS<Y6-c0*g<
zNbzeiHAD{(CXL~jg$?#Vr32*~>ENHuNIH1GSnUnefg{+SyP*jEU*A^#-HiQE_69s-
zM~@$UQrcNr`FYc3WX;K%>o+^h4(~?~Hl?H^_z$Y#$QjPi5#VOxkPnE7kH;s9>7VFj
zy`C~;!y38%207@`(U{zyduZuZm)DC0pxlA)hGo*^;CZ(;^p-k6EF{K=-P+n=^C{Rd
z0dmr%n=Z_~VAVR&*8l#xB4Dle(DL3~ZjaUFt-fJ}Ja#lm!cTC4kD<WkKCz%yG_QK}
zcbCa#JzgN{ZPSdf()(gh4z@nt=c+Pu`8yA0x_Go)&E*F4lkIurueluVOkwu9Ts9*u
z%l_%pHr#rI;c(1UB=?2|=q;*7Go7aPZu-_SP3={dl|bfSYc)MG*r=dhmy7$#gVrv2
zG`4jQlQnBLo!2|1XnubgXCE6sOp#o+{_-Zt9cO{A&h|lFh0IoiJp^n1vcBaDf3W6P
zlrIz?!j$^m@U)a+BSze99~&p<>-<!ZS~b^;Rz7(|#d2|Rx%6ARL_3*)E#S?_*^u__
z=O+y6|F|egoO%audDq*$JBs{lyZ>Pj7#q^Q9yO%6|J&HiMeqztP?SyKV`cxhuFjO5
zu+<{F0iS<{-M<a=JwL>af52Ud=lNzc&#$*haJ#ll&GRl0@NLiaf6em)%yVZJ{*TS`
zCTH>H0zHc>;T<`7mMBRdYxh1(9uv1fyzi-MG_P68%qSMmm&t^9(J8oU!MS~kM)co0
zJmpHv-hx`u8nah+P7?QH?=ZfoQg*@vmKhoQ$jH-D=Fh@zPAYGe$T*s~Yqud_IMY%V
zaG!-&!9R+Yi(BCK5--{Xc>@34YT#Ak^?Sc2S=jn0bL+Exx6vCo!&1>YU3^qlvoS-i
zl2a?L%pqHzZ4YMrD=Txxp<ddX<L&Gb84HGP!c30k%!Rey2Hs`0yxHEB-%vhPE#A1{
zMM-bga*koal{a!!%-yTUJt4bfWThx6Ny1ks4$2Z1Z*fCPZoj0Y0jWuLGqp%|nnm(3
zFOr{1)gtMH51-qf-T&c5(#Qz$=h-(7)uQr;HZLHuxcA-BOCgxn3<$L&-yl+%69&P6
z0MgSG*WfgN$YBmSo%N~>`QfVxDZ=djZczx;IiS^lCp82A#Fe%MpRlrevD%Uec5_S{
zsIu59C<bPV|A)tlle888vqcKTin3t)w0@#Fo4%oZL&6wB_E}$G3C-9{maQ#uJZa7p
zAbizGay;2JebsjNb^uNw4aKLmMUJ9rE_@BsPeS@{V7UOa7T$A0JXTJm{qL~M3pg$C
ze!>SHiawu09w`S}F60?cwwKHyTCK&le}{A=*icSY-p<({Ecb~IJOSXq&H`uJj!e9V
zZF_l#RIU|qWUaSEe!xpMRNJeJz-vWYg)N|S{*!X56f6wBin3rQMOm+MS$CBuN0jv{
z<zOv^_w1poP(f?FyfbOa?~J0~hJP3E+{MjbU#@%rXdeabcGbNB_<~$TmP=9f7F5O6
zFl^i6W3I%tjT-zhGAV}%*@g1TSEo`f9?*eXRoE2NVkf)({gW$kHfe<YYJvUNGFA@Z
zKPbf6q@MQkt(hnX0Q#xSjKsTe<J0Ea550b_p;GDSl_}X>{g7cz8SEtRB}6#_bBel)
zHhiwuu)%hn4Qxy^y>_=auT1yKyDN%#wCocc3FbXQ&U@Q(#%*`OAW#pikCfH{Yptu(
z_EO!b*I?hu(e?q}IhRabxpph5b}Lc4h`jexO?Y;A<vOxN%hlV{R7H+!vLe@`B5ioX
z>^W4VpgIEIbyOF&RCr~It_$<0{gY6EkH}xchIa{d&pV;+8L2?iJAWq}s&`}sJK=nk
zc|%&-r2f<$c4Cv&qW)Q8xjM)_tG*?KG-#H0qaHl2(@H|f-6Q)^S6O19g!UdjFk{WY
z^f_R`N2(T!m&@u<T`p&pHb*h@d^i=&{R}KPqO*2Vy|fpiYUtgrhITzrd0f+0c6SW7
z%53J)`mvMJQj?hb+Xo)Kk36QE^s@iS>F?LP^<Fu)B1Pfo#XR4!GA*4(gSf0r2GJ0A
z2Uin@05?1|>fCZ3_4gaMe5JDhWEU(fOuJQSJSN($g$ao^%}txcEuIepkr^Br=dP$^
zStqidKa~>2<*XYbTR1i~O*K(D(L`p28T?oxQSMdtkb^^)PDuJG;Y$%zB7PBdmO(Q}
zTU9gDDil2;6ObKVt|c~EY|XlWc*G4%l^VdEgm|_zoi_NebQ*Z+JPR8yOpmsYh$H#?
z>UKV2gB&MNlf7H@1mv~M6L1S`xM&lz3jD$l=<tjWFdoYJ3KT>pdcVo&=blDP>f}*W
zWkrrIzvC__U=1jJIg=xv43S7*ra{)TBF;^h_Va*#>9xD-Rc>P0nThAjJ*70q+-z+Z
z{HZ!n)8q`I2_OAN4JV?8UD-?qkLtRx#;yO2tkk%cx^b|bnrc|$Up7<sxFD}guiZyg
zpL)t7BZftD-dZ{kgruk*19Wtdqr5~9g}0~>;qk=fue4a$Xr+XW`MykfL#6CSd135^
zpS5V2n1!_K%PH|C7R`ngi%4SlA&MyFXN!o(TpLCFs&Ww#)F1eL1&6D5lZMh)E;5vL
ze^mCByGJddYQk-+8&|)UC3aDUFmXCPCWt;Xt?Emv&~*0-SRP${rJO#hfU;YZy^fi_
z_|lcNd%DQr4IF0UmKT1}&+i^}r^+I$t5l8$oE)+{V+*3)L4K<vWB7L^Awlb!=<G+z
zM{@VVT~ZhMAR!n>k8)J?Q<#MzT18W1KPU-=MocaKwb9wj%11uT$Yo7q1Qf9WDY8@w
zzpi6BVOK?vnUz!;;cvK_UPTEBN!D9<+pRd--8@MDETKWVo9nS<5$&<O7!^RBUV^Eb
z!h_ua+`Gt4Iq#2VhGD}PoFVT{I&~C{gC#oozh0TLbfCzsE<=_bFoL5|X2ps}qj}~$
zgrv3?&R^IcK}OM<J08NT4qv4Z?vT5`1j_AY*j_?4UgD}=&U5AKD55S8l_YBF@-@$|
zhampiqAuD;``Qn|h_Cwb`DiU2THXwHk+qe~w{#_v-3pIVJzHEg<+gktU9BgBOoDzW
z6){m1lL&!w_1oKTrHH}Ui;LlFtGNqda!<hbsRoMWnHsQ|_$t+)nrq;u7#~Mh6O|=4
ziDDJmEusRYQ|!Yiwyo;7oH{QKOmKz>>=XvS%Gj`GFs+gb-*CdJI6>!rD_;z?LRH=p
zS48oS+n3z+AD8g*ji#<&9qRgPdDg7vuK!YX{c3DAsICt>U7w@7o*T|xzvRGOzZ!0i
zYE_}@L2bQE)RjN!{!pDiFkSz_!UJ~wMVS6tv@tbcD#Bg={X!aich_I;t^S63P4?qr
zdB`<q4=>l%_nH|*pl{swcCT3WceL%kET2fC4XXFi66$=_4%7L6iy|_5sZC_`aqj!a
zx$pnA&`kTsqkXv(Z$$Jb3k{{bq#1aZnhdXb$#c~9@_=($+>6ebFD2RflVTh!vc-mG
zM1<ZdHj@7nIQ~Jf!TSo~4SG+uySG|L7%aG9G4^!tDtRvjczJDu9QwA10pytW(*hb5
z_d2+NkSIA50<x=iYY7$Cu8`h%MrKj%8j7*e6fU!^caW@e<7lddvNIvsT}(`^fm?V(
zu`goC8wg*m_hk7~<AyyR-mu$57pk-+=0RRY7Q0syol(Mh)N1F>Xjk(T1%074V#$I#
zuVgi=CuO3JRmDvxB^$8}v;+7$Ef#U+y+k)&hQAG(wS@=jxrJun{>kg{xzL38kZ@q&
z<QKYmN<0VMZZvLZbK6da<fn!*tjwh=#Y5V0*R*6K(%Y_vbk2ki+(Da~l_@IA6(u|n
zl77@1s}|Ch?YG>tOBY6t*tgUq8Zpx(<vPtxis=2b;iSfmDLqi{XVG?(^`aWjo`*7?
zORO4li{yE1TG41wv#1$+i_r}3)LSvh=*ZzoTQYd{NY)9t#u0OD0`k344CjOJS$_|;
zIeb`{g!5E*X(6xSEj+`YYc#{RP~klfsc^1r8<`xIeg{H{(XWyhQM@-aw<kc{WF7qA
z-EbdugLuYER)E?@qxlS4=C0l9P-zilWdc|3LaLmrCl8!l{Nz!}^QC6&*~;RYu>CSc
zEb~y4Vk?#HN`U#gtUN3QF=ls)LqI{*e(?ZJjF_&}p*+abbd{NQhgAsF=P7b(vBf8F
zSQO&*VtT)=SuNfxi;cQ<8&8Ewj{CWpk7zj}^<-Qk!4sK~c`GzmW{S&PIY_6A!kOar
zIBF;xnx>EuW28y{{SN5|F8Ibe8r#)`EWB{0BYVlHVf`G^t=73~ws$2Qy0~KwIMX`Q
zf_u{`SGV;pU3dGCmUfpKoRFe=9I%)Wxj=u|eGY|O90K{bpc=t97327H^dV_j7cU~w
z;0h#0qdzDjg~x|J4j}Nl79_cUR76tzx)y;GdH{NAzR`1Gd{JW|YFe;^)*hPASle0_
zr?!iBRV2iV_(X{D9H1rhkf;xoFBXNA_nTS}K~MrcYXTy+TKM5sRYuzAmy97EASF_A
z=Tabe+^Mxg>cmqmXq0Vrl!mPgJToVW7?SDlDI$*dEIa!?Ttu!q&vI*Tn6GUDfK`6B
z9ys||l0IZBE2}FMZGE|f?Yj>|(^}eCL}TzW<FA@e>+0*~)SGv5xi52B8;dAhy_+<&
zyzVSAqSEUvvafn~^b%(7W;$zSwG`D5XVBwpkx0tw5B^AH^}QC=SIKFk8!4+_xU%+4
z7n69J8f7)6{)Kl(-$^AxX{gbP;1(x`?7>k`R*QrD28G2dt}6;DtKYQvO=b16B2rd=
z<jz@M6o{Y^KeqS}Dyz4bu&DD14^ll3z@06K;4LjOqVSzuucbw#0q$?X8sO3*L+~nM
zLK>iv(j7JAD}OE8!eT0Zjjl?)ByNuRU24cb{RdP-uB93%$=B4tQA0lF8o1Sv>X`pc
zHAJ<;s39BC>$X_#r3JE?`+g_){hNN%_dB`o-}F=8-x9-p|E52n3Gf|G-y7|ewJK&d
z+Yj9L+ueQt+uC={f&2b3s)3SOObvMSSp|HeQQtom^S`Hkt5%qve=wT0@0+>nFXpb_
z$6bHF-|+c;Ewl!BkKd4=ds~9Rs^bL}UZUT6JF6-Eh&8~+3t4)6z9o^d`wNXC@o9{L
z4c?ClZymVaF->s2rp16y3L-VYr&?kxRORG$UAxz)j%(@Blm@u1kgdON+f$v>LTE_b
zLdMZl3zY`=OksWFEuRL=FlA`<8exOiB#YEH_?hH<&Yf_j-*n2w8aADB300-uE1D;Z
z5*%&H$QV9}sjzfp=7_F=V+Cq#iF2T4`ROfGxLy{pvcl7wVh$*TK+(FboC)~>ldu}~
zFL;NBYvB71Hi!|z=oV<Zhb#`!#OT+=#At-`5X2!NzG^gau0K#m&*JeX<s_mgbTqE}
zV4?B1l|M6teO*Ji`oNfL;k8&JZc?hj0mpz9`|Ugi?&<pp&AafSXL`i*?r(ltj5PY<
zMKswyIo6w*q2`pG636^rp9XSi1)cX##L~UD5T5N3<6R$#W9Wf!Oxiz@N|h&vmpSAh
z=~3oQg{&=vN%;HFKDxohKBhe_OUpjq;E=q&Tos?RUnzA9Af<<8IbbzA>m0F^<CIwM
zWYamxrjSSt4P~G+pIAs|0>oR*h>bl{NHSb)OA;aW78o{G%LH(50kN@_t(cANEojh#
zv8Gjsfmdi#3{xv+WA7F;(wXjVh5Es8=}?AYiZJX7&8kL(p;ingyIFMssmqVGBJ%r0
zfe$0<77ZQ)Vp*f{cEfZ%6ujM(*=jD#Rt-99k3-BFtE*%t7JkD<`wps4fW{en)Fk1U
zqYPS~(;NKy#DG5Luu=^D4Rvl(CT^2qMq!p$6V|5qgVR8)KiuL_j7@*|153T9Mf?JL
zT6ZDpy9GfKZ9ZEVRPJr7GAa;yxeekjJ}C_bc8r-1_#YmcEccW*&^Fjkk6%2gJYxP;
z8cZ04pjC7X*B0Md8VfcyGE<3`lgGRBHhCD*K2aJ#(kE>?MQ{e?Iy@MVBPcBq8{USq
zmm%&&J~?>5Xjlp0g~iq4aslpw^hX5~L<>4Ukimt%6MYDUUQtYiehUkMB_3WxrEZRD
zy4dR%P}4t_T<moVs>PjAs@VIYOu2tw#05W87L<?no<fx-<EBlt*M2zlCVOD~l1zH%
zRrv7fbm7IgjDBp!VB-q&Xx7d?D1Qc5A{}G(Bcg!4P9}-AXkAO$<<d!V=3I)vU12vO
zdMcYd9*Yiky`j?X9?xkM!(5|wWYn{eB9zM5?<!>5u?V*vVKFri_P?S3uo3bb&ZvZc
zcgFn@`CeH-o=WRVgI)Hg$gMa&u@4s>+2UeBhb#6i4&eP#LB}l?kQRDzX>g5bPUlwk
zSwv+nE~7Ie4_S5U7&h+iqH`$f?yk$rs&Vg0teqGKO!Ek7JAY~ly;DVF3Fps@q>_>R
zY*oQ!+$Wy&P(i>%gKscVDZg4Qs8m?ND^(S4#qAC#7pJ_l#m8mzi=1*{lyyp!JsuvA
zs^qu1JvR$g_S=i8@D1D|_<JOpB3(#OQ-dVRT94GrOM_MtDiE}AYKl&zg@L!(!MLUU
z5xo`;%!t0qtKsE~7+w+`jFU-A2|?u|9@{q-1?Q)Y!Lg(2^9A-rh1kN!!2jXF$x>34
zAHX>~TRi8X_bvQh=gGdH4DS1K!gASXvfVEp$adfqD?AL!B5>WtxZX9mZs+{3Gp^hC
zy*?b*v4rbc3)EkX2wLQBw}3qyevS?n4NpnQO4;Zf(?aA^i>M2pYu;q!iwh|7Eg8g#
zK(a$vymcH}A*aNzf2d-mVh;0Z8iH)hsu*4*S~GQHnu`7CdC9klBWXk460;<)5pg(y
z>X=Ruy%nZDCgy`U7jed$*@OstKpDk-Pme5m@an+1`_zvC!Ev@mr^7vXL~m`;hO#Rg
zC!>|Ass9pLHKJEgr29lKzeukZRr6)d>6Bk~Be#pYAEqpHZy9eeCwnu-%o-zF2k1DO
zxKLU9l=eG@GBL&5gK@kZhtO=X4=2|sc$v3>%o5)(qN#pKRPC#gNC68%AP9jdAQ>B;
z2T_85#dnJ^iRl%2X&GICodzHp5{$bhl^SBf&`j=S)=F4G#@PcqBF#qj68sgD@xW=W
zUPNp9Il;!ZmBm$bTwNbqHwEs1MYT%b)3lc9neNoRS0)V=3$j564@YUc7ZFvtvpHk>
zp`a;DyCG*B0;W%yKHXG;#TP$jM1^=A%TVlWfqRB3$BamuL>Cd>To@2>esN2bU%a`<
zhhLh^cSt_<_!_Qd19Tgp5lH$PQ?c8mFC}M{+_*GNW~xI4*!5<zY8X6xY-oIOeS+*i
zKjg4YI}?L|<+4H|BKum_{vE)ELYnkDS`vwR06OKDIERWw(5eAOHk@cc=y#<&%(h<)
zw@<VxQd3ZE8XY3hYKF-7>6PZIMI6K8!ILBhSWD@l)(o*5YZyJoDrGQmNR=kvEvofR
z%#z=+{UdJ^bLp7f={#$SgK>BdmeSP)xcI`ZQy^9O3fpfFY`<{c!fQ7U&-i4>Co(RT
zYsPMy7H_w5{RGudOr{5MJ<*pQ%jBy`yHe!1b;D1)V0sdkz&DG1eE)$h-V@oA<WaDh
z`bbpR#ja9!*|lO`L&b0bGi6fY3)iNs?Q;5%;fVf<kMUn^`+Hwdwww_24wNd0%T=4q
z!tT*NvI2XPRv)wiEtp%G@@W8HUrfgn?-VlIC%XcjBdu7=c4eh>7H>z@b~|mPh)Eao
z6ZuGsBn}PQ1E-W#LwAbK*_J&%mPY-_V(P+2n;m%6f2EwxpsVp7PJwE?X$RERzcl=U
zhF|F6_lJhxt9IpZb&~8)XJyp+0(NJkk^gc5t%23jKT^!CK^M2GQxo99MXV~UjN&Qq
zXtA0CPc-8x@F>^+$!0VKit{&L7myd6`uk_VHVnW<GXtJjU}nIT&9f){f(ejJ6W|uw
zuMzJwQxia*ZFlc*UBc-@5VMtb<;C`9{@%sEckG`{T7R&jadn!jWE%Qg7tqkZ4MK5%
zp@00~LvP#a;6vkEZQ#`Iu^KzQH^@ZxdPnpq6aAvV^=!{TaH@qjHio*rxWAN`RC}BY
zZ@_yZer0yh_NuZTZ!#a{K{`KvlkpqCZ#Ly?2%7T7qq6K{zs1Y9#Kwwu7vW+o6ch{k
zRn5QmMK|E=a3Z|({RpcQ_u<HJ2*DmV0+m_Z2>Sc(A|imvNKBct-I-%mW-zYi_8xOM
z<c>CU>h;nK)!~ria4mxEX@k1`UqAI~4xk+1x{W`0ID`*i=&L0487k)_pZnlm_;r(^
zyD<)MN(B8KIuiTuU#sk{WTVrCIw1Tc3Ced)k^pcHp6E8wS&}M&82l68<5H*&n4!OY
zxAJ3rUBut5h`W_HMcl1?Jzo;F?SOYHZ+73U{3ze8T>mHDt$Zoh@*7Ltt-N2|tsGEy
zEC0f`Mg;iA<4gD=jD426TN%d~!tPdnhwoPY!F{*#>y7VLzCqosOfJec+?PpLYq0ok
z<%`RdQpdVq8Q-n^9jCg4tEb+t;hRW4<-3)yQFklv)^{tf=ew16Tfcp`@;BV3D?)cG
z|JcOc%4_xA%3t%{$~XR=yOm#yyj%IX|GvAG#Y!uvu2OyrS1B)`VR%SgrF;)xrTovx
ztCVj#<g1iBHg%QqsgYMHe~&{Np{tZHE3Gyg6}V_Q>?-Bm`YPo>DN9OABCk^Zp085A
zB=XMWFZn9vD<gv&_$uXUzDhZUu2NnYewA{G$O^wo`KiWNDIcZ}hkeOcDL<nxQ`!fz
zWqURw?mx~r4tkx@|Hx%Zzxzf7d{HEyt~n0fZyaI>AgN6q8w8(diXk91=fru#1KqQa
zoTe-y^#H?3Jpvq34}d89=6Frn|L(yJJ3(={MK_-xo-Hm6h{OEif&ebO7r6n^(l3hA
zi*R$8n6G|tmtRKWMPhb9bnvTZ<yzb~f9`o_XJ$;!$T+w1))f_^IIvc<46GCLaS0!O
z7FJi_>IHbU)y9kOJ+LWe7U342-Z#CJkV2P?6a|WkM0>x;ODtN1ZIz<M>u$Q~8oJPy
zZcr%p9e?$`pULy&dH2e}_uhB!z4I$3P5CHe@>_3hcx%Ik4U>~_4!wFv(SVCiEt-6H
z#Uva<uPyto|2{fho>vRFL?)sF^<i&wWnu;V>QT?23O~pbH!mgT;Wndy9Go$KNhV16
z<E3<G`IB^MQROzkwG7uW4DbO4aib8T0p82tP6OV};422alfhjEyp_SX2t?Km&>eBP
z60Wo*=thQc;d^(+djlu^$mIN6DcN8DMVA&K?NyxiOAW1NXs?D=GW3mxE@Nn)hAw94
zM-44!=of;hB}=)809{`M=vP=NEwM~PKQV;M;yLbnhAt(DGJd_7*xX8?TD6xWR|{PC
zgt#vmx=O%O3+P`A;f5rR`-GwEb=-#xRqMF-8M;wJ?=rN3pvuQ^IC=o?LvEQU{OSF3
z^uG}q*N<9<e;cObkuhsVW<}PrtgmNJ|N867oEgh<wmu@RTkMM$ZG-Bj$<S4=hV<Qx
zd*t9as!5+8tH}1e@IOKN{=7rfMm#>WOX8fn;r{^Xi==M{xO+fLnjy-FUj5C})rube
z!OxL|(?y_i=pvItd^Vt~ri&YGi%c69%w@6&`!lk>V}4H2g2J>7@i|5F(=HOtt^A~v
zk%eh_9g|YVrpe>u({g52i1<L<@Ra`adI7E&CClFXrIyI@;|MiKo`?rICB64H+jmQi
z94t@8q`H=6Gbndh^zhW?>(RcnYsy87Y+TZDtR-<_mQQ}3x$HzTTu0$640NN-r9xIM
z`-lj2hx##tPQig-F@iQ|H<Z&oYTcCoR=s!qTfkYzH+XHO_4u6VP=$~hSmbfXnSk+O
z`GyKye2yqFo6O=5bIf-7?1PwHCo}NHiv*lcmdRuLW+ur^#pN;=``)pD1}?t??+dGN
zj~Kje+t)V=y3?vZY**=<V+Nf-wS&E|ZMM7`k9b=uO?tdpUGMNMjrFjw;;Nn~Ki$c+
zh%JfLQ8@JlrzyXWS7hgwwC)lyME^;VqtQb7^VZ=1@I?k^oH-t!K*%b{#)VCMSrdBB
zz1kwzslIa{Y!R9(i$k|EHzlR;%Efnt4kD$1Sh}SYfR4(k%XPHah|7<iI_Q)pAne-Y
z1Ky5&z-tj2n~m5~(!ZA`0<g{sNGMpvh4fW`R%FpDq#Ys^#&4w6sW2y&ZM-D`xu&S1
zqL`@&PLT6QVhX|Nz@Yl#O2%55Y*$!HtH#B@Vg>GWV#gfueHq?X*gX!@S?%r}QD%m+
z6dpsRW{}EwEK<qUS}V`Mc|)w!tILU#3NaqH;UY<YxO)JgIu69mz5fDmnu;09F{crp
z;fNu>0ex{jnZPVI#PeGKRhug7@XeK2T!P8T!XsN@>xwR(vWHTMPkQ)>C^i)VJmvB8
zo+$2@M@C!W3%!M*k-3|I<-T(Jc*6362S!o?seUhcYVo}WF4<gD|J&#;K=wPzQDY!@
zO<}u(vAqs#e1X5aQiqHehy1g*ZhhntV82Ya_#1`&GRFQLTs@m(SDO<2T4pCx@d1k)
z^DAr#Ha`1Mi|+;{T6~ehS7l0IT7`>ikl`!d#aN@PkQO_6Srw9Yw=L|I(6+KV2Bq#S
zwwz8l^y;4mvPz$~P#yE0s`@&vI$XJ$=$O!G9da2|P27FMu2Fh_hq=)oscv_LM&$6+
z`H&AuTI#L>b)fr?Abrw<BlF@2zsO1!-4eyQ$>NA?V$nw=h^%S~pOXv?MjYW2S$Nio
zEZ-$#m#i+r{~zL?h-p|S?*q%b1#Ajm+ahE6ued2SS_F82f3=cRIfh1iKSwOZ)c^pN
z;3Cp!e4K+Kme`zXDFXo;pA~_Oyk3l^?TZnR5Hheg4zHh&%Y1mL5?jjxe2cmd_okD@
z{G&2b3Xm$40XI|I-P$c%UWU%!#^YIsZ!G|LNN;A-jnhAs;nvxdZF(Q`QmesB_cDsU
zu!W0Rm=gAhao8D-&HnHcdro_q!#g547sTfyvEAZ&87p(fiZKB(L<TCvX+hDt-afT`
znP^u}e7+SZTOu&23G1z-ImD;EE#-y5gK6$MxBJF&I&n1b(yoKX=6k(-EBme3>eQc;
z%l6WSj7@u8B$?qZce;nKj_h&)n9QGrHT=O6zt|q_!*8;<3g1|{KdM1Ij~&YUqiE}X
z&mvGOe4h$ixC{^tLd$9iE4@eY_D+;`u%XZZrBKO4@45rFMVtx02oM_jjM_3B--1P+
zJrTB84PV2dK>E6*k``SQs93e?Z}y{B+{fZkkb+J=k(0a=(@oCB-Kc@{bBoH?@HG3J
ziZzbK<nF@XXHg$yxj8Bt0XUqq7@7_brVzKoyF!KTqFrx}zn$*^7nqJZIc6Te-6_(Q
zH^-aU^%(6JyU=$Zm(_|1xWeLT&>~!nrIgMuaL)p+K~<-@zks>ZE9y(rohh`*)>EiA
zq*qL#!Ei2BQ)v71cIqKaAzwfoUt1wkgQ8_kZXfyT8#H<3x~Aq1?T{!Ia)7B*fvHm!
zF?B+j!e$Px7~N1VWAS;Cmb53kzLW&@-8M<)H<i^K!Zi9DU4?FmPuNHdpi1q^^ThUw
z9=IHtDy!yvs0z3#BLkT(B@Y@sJKn}d(LZr#$meB2njb!zA2~U5ZGYS&;&uzO2wyLN
z(?tV*>-aKBNDe#gN~`t2-lAppV$7RM><_H?zWryRyn{0|7o`k!q$><u9yj^Px$=R3
z6BEvZ<9h;$;DY#QUuPmJcm3`PqB5JxC3OjOCY)BxnFG5}9zGcK2yen+7USM-qdfdA
zny&x9VUgO-+IlRh2!X#;gadpVG<3IFgd}~U(GQGh?UwgL9|#TK`GO1|sX>rRkKiy3
zLMhogCj1*Jk=a8E$e(t9+NpUcIIF-Y3c7e*PJ}|@G+oDs+Y8Vx$6gSozv#9d(rs)!
zd3UC14P%Yhicb-rd2nJ1PQ)ZFKFQw}Q6I2+^;BGT-iMYedo_=uw+-YeksK$kSipkz
zcV#3GTwx_I-$b$M8i|KRTi*&X*MFmEQ*$hc#Jc<Op|f?Od3Epdn{J$pO@QeEaWq{a
z_*Qg%tsE$pTGco?4b6dk5kD^`H=@5aBSYhj+iu+UC_eMpoj!T`+_vW!=xY6lYabeZ
zfTN+G94mkPQp^d;e3>sMBcKcZa67y>4Wb5x(R*vW&lBnV>;?FF@JW380sdIxNxmH9
z$>`+mnZKk}{0furrQ*@3pqzuaZ*ed#S_~mg;^pRkqD%c(+r!%u(T*0k&nG!*mkOV?
z+V0&Jfw#^lzn~96HO^y}jW4J8w<7$jqH1y~ysSlVhwQWtS1usz<D%d!4)X!{hL3h#
z)}jWd5&-@KO`9d3$KCTM)0n)6@0htt6x|xAGV8rnQPE!UP7#%PQ;P;>v+zPHsydJP
zES#iT(7DTK@p=(?N8J`m@){=*KQvd-p{Br<O8N$!f~H?qLek3j(d?uAm_CL=Zc6qC
z^dK)4JC!aSvgAejLN}7u)Ik@H-7DtMp~iiD3CzvW>~#m9x$s1CI8up@K9MIAAhJzL
zfgcN{(6^DWJFP0KKa~aFLEM|3Y|v7W*c@FiW`I*ot!p=jvQC2KU*NidCp<wD5ED>N
zbYce`yysk-yvh1@1xEksIMK6y({p)Hjc{{6z99ymOk2F-&Bpr{et+Ie0_-FF{;JoH
zo5=y*%HW%v>~4O4nUg&bN|r^*?q~1?PPUETpXOvwnPgn0jJgcyKY6&oDG_<rbKHBJ
z=XQR7o%3uqdFb1$KHS6!E|ro;2v`$$U(35JB6J(U8Jp;Nu-DC$K%7A%Ww8%p4(?a<
zEjw-8X=!6KM+_U0TX{DI;Im?yQdc)iP6P0-V&Aeg_~OkqtU&%E1C-)BcFev+rPzlQ
ztC6CTQ!H!FDXJF+fa85H<5;&)rFexs!!B<g*oEYK=d*ru4L<9zWZNtRd_13uT?d$=
z-(~0yKr`{JAaP-GW$sMa(`mD(Brt2+tctH@XTCB&V;3vC+lqsi>__IxX1@EER9=J{
zUb7G(VnG8G7Nw6~+YCgBESBFElfqQdj6PJ{8<VRTL}LP-t@sXah`E_E@U5-)$%fAm
z{?vR5-i8gMm_*SpQH+Hxq*dK;)|ua?>vcNPrzz6#xwfzP<ZTsB-b%4&K6SvV=7C4V
zoZ3p6v`)m<<g6DHgX>?zml*w7>nhmjA$BXH#%6d{M>Q9*Y}$aId~>mlkM2Izf?ZqH
z0pN#E7l#F(vrp<^AK&iWvGT!HA5GpW&xCj@icq<9(8UA+S4JVOpAX8-WBq;eYJCHx
ztca2bf<{S*`7MCsk0uOl<glfDx>|knFG~!<P5F%H9C#h_I#(a`SxRZyAGZ)~{RFqu
zwq|$rTUt~Dev~QRlP%bB_8oP6RNv<EETUg7s-f&Seujs5y9I1Sd8qU@KT+MMVtk)c
ziC2QQJ>i!L<0c|-2YMd&dqs;a{vh3v{z6RpEPT2+8hQAmM%!_?4Ikq5)5_i+le|ha
zz+fd8iy000#!mv6(h}U|i`xo`FI@+>ZYsd;+Md+B=0xzX@v~Ryk1;j$&6U;k&CWP{
zLzJp%$;sl}D$y78SbT#IZli>?=x|CVyCPLFr0AZ^K;#H0#8u)APqvtgqXyApS78->
z@8yn`>U*W|1IKqpiV$%XI2$7N#-!hk4S*q}IE(XTjm$w@u#l^AWy|#SG6&QzTXe(U
zQTRIrf3M?XE$FIS3fZUo9X|JSQz7-+J1x^s%$+!Sg@~`CE-ti35_h==DWrG_Mnrn2
zdHE9ix*{A~mp**9B|Wzv(|&RUb={7a+K;}8VHd=6xa6VUOG)@G6^P#BqewS#0q<}D
zH*o>)Z~+L<!9Cm6;wXHB#3#BZ-iAAwMP7qAN7h**@L9z=e8YtdpPv<A;xvav1T|dR
z3WpKN2KgDCUJ^I7N}obS;!`8^BPWqZs<m1f{JdlWz=x$^|EJJz^ek7Q(09@^`_7t$
zYOdj`eZ^H<RY;ugtCm#=UsXufm9JXTd|kyZzh6;P2q|DJMztKiYFR&h3MZjXM%G#P
z;si@9K1s{wQJeV^YLUm2%7#+G)#{9^L#ZKkg{-MRf?|Z&UI=Q4U0k>p<NSOf9GIKo
zc3Lnnd^X*OcOtQ%PgY>WGC<4WsDm%Matx7Hft{Tiih!03pav>sGodJIuob*75O$Hj
z;mQ5tSl|6b3!jMbap1#+bvpoV#dqa#uUTPoA%F*4`nE%hq7PVz2mEx$&<^@mIHwQ7
z%F+(NhzbN(v;>(%qGvJT#0%sI2(w3<Y&zF6jMaM_*O&nNk&m8FBaW;}_$(ipl5i^l
zsruC9yST~k6;}OeO}?bHYVsZ|$+JRjeqFa2)kOpAvA>SO-w!qIjl!UAniz|Ay)cFZ
zw+CC2Ec1L}GPU=<mi|?yV;;lU(fZxqvO)e0Nxr#|ra)!u<nw32MR{#XM!yszxm1Y$
zWNj5<PXW|Z)lbQ$pX$jBLj44z8(bWe&CSPsMm9GZOx;oXP|o`;YuCKiFXe(vJVZ?P
zwy*moVZ%<*;W@a7ku6H<Jlg&<ko$5RG~O->WHlO;ORe|_H9m$`1!QSl&cx|O8~)jU
zIzBpuJWC64H{Et}VL+w<l<A9>Sgl1Y_+M9uPo`1Rf5fFOq7|&l()Tu1lY8L#*2z<M
zNqiLumC9Vb9hHkQzlrp{A<<bT?mK1lwPH#wlyq$V<<p}*zk$~tEgSH0J*pkP6toUU
zEw72wBHSnWu$6GNuWgf@W~a70@3M16O!Desvyukn;s(IuQr9iV5fRbGPv6yPlXy$F
zKJ#xvwboxq)Z&NMAo2M%<7MWU%7RMS0iU@C;m38k^Qq)^WHNYs5gMI<91UPK&BX#(
zXNhl|pPad44=`+uQ71hD-Yb_49dLZoq(O+>%3SGQ_?k=-alUib;VO3Dx=G`pNYo{*
z%_;11S5aY(GU=($XsIvFtfXcT>9jKGF@_!^@UZWu2NNllsmsQXJ7+A$>FNA{crx0L
z8sZO@#z$dcd=i$%M{&0zSsFJY?Bzv?_+=08ADe@$SYr8nqU!*v$@uf+g+xQ2Xie6m
zS1(i^5x2G`|A;p(Brje{`%s$((V?z~Xorsv1rlzYFXPd~&*vr6U|-diG?9Jth&5f>
zHZTkQw5OnU8=$II^<q9gGo6?wiqL0OVm54i@F^7HW1JsFf@@n5efzi|Knv>HR`nIw
z%b<SVK988tBW-<8KO*~Q5H-%HI)2%PxYBAGZt+<g-$-{vS}FCFXr2h>Jc6=Zl1~-d
z*T#1pfXnl#xbHasC5#IZ?kts*WhrM_($;qwfJ)A?pR+9GEQm-erSDW6?GpoP&qY0h
zbE%9g+L5e<8$+`{-7<&@n@5!K+y6)1o4{99UG2j+XJdLq>p+;r7H0-YNB{)^HH09!
z8OZ<%sMnC(Bo{J~+%VV{B!HrINI<1(381#MN)YvJH36+-0k!I@1#xO?1+~rwwA#t<
zdDcGD9fG!h-~N7|r8j5TXAf(yy{5g^+9$BHKbC;ELnl<t#m#LEq$}^90Nm=Bd&$I$
zs}cHz)h-_gTIsdFIVJ0YNvq6vR-*JKDgE{dRp*f(-7R;ooY1D4JJczDbyuH`q3FS;
z1svF$1E$olmK}+$_+w>zd<%-J4qmV`%nA?QizBvJw)__?dMSUp^7%kDv|9QPEr&+e
zjnI=jNGJXlBFnpSpdRJ1yCp@dotk_y^m_SvagX&}IR9ID#`wYNQrSa#Z$YcIKJOTS
z$QjzyA;gg`8Rk7|fd0G&AvXGrw7!=b;KNK+qtuoLN>6`0!JiI-(l7fekgQ~LC~$5|
z0~hcgPiW1Zlx=c_*eUrW3R@y5WT)yCzM{<{VIHKD*10AK@o{QE>6#^~Wvemb1ap(R
z<7n($qOJFW#JpRFa_;V~q8(%35MJL}MWlOo$Znh%57}$}?!##RP>==MhWgE196$EM
zO(@r1C8ENR(rmSO9|KoS)mBB^{RC$9#eQ{Gx2mEzwGo0Kp1w82U$zIgw^b2k4i52W
zbFP0<B|`DLs0DXdNtIvX=Ail;&Hz*D<q&JUcc9-KYn(wF!yiMkoL(A;?Xp2?ArDx;
zFT@(3ADArHTT|r5e}B80-KIuWsoDF~Ir~yn9);uQLTzNW4-E7_U>;HfX*RzE1UIPz
z?;eqA9+zq!2}w1N5A<K&ZvILkFPXVYeePrVqm${rdbE1026X&rqU1)n>@8`9hEuf(
zvqrQ`fUGYwtb^ja0(fSaoEt+U!;?T*<Lm(p?qTHGBgSa%L>tDrgQZLsLkS@%$^M&R
zRurhkVGEa^gF3YzpxRYK9qqwE-8hvGPJ_wn!UJj-L|^K`1}rD1a<kYlM_p=UCp`)(
z@84?VCmHzz@M(7q)qEsv8$>CuyM`-u_aJ?yfXd(94o+~HY&EOwHe|H^5)ORp0H0ko
z{!J*^J;>jp&e{zjo~pv_D-KZN#)-d136bhisqIN5^uo=~5RvBDK{TQ#-<Ui4-tjnO
z04zupx}#NhHCX4-oZpAx?szmz>=g9wsuq!C|6m$AaKpKM6DtIPG)M0)GcTOI_NU8N
zwJgBs610Us(IlTppp8yKi<Fq~RN^Igpw8vbeL8EVkw}|>agaae#DFoY|I;lG;3i=k
zVwn=30W7vwv$fw3_E($50ponUzc1qbE%N>y&wDW!z%XNNStYK83Gam};`{a?J#x)O
z@~a^{Givl8R%XuANm4);QrVN2_;Ue&9vshKW8?t-k#h+3;qt88Fo*G1Y>>;(k#cAE
zm*-lX*S~i`J4R?ESon@+G=5VxZDTJFA*1V~epzKq;IzLngxvl^U+W+M`2HXX-g85R
zGF^><nEfjkS7Nrif~6Ofa49`A6taBFZgqUAS~6VqgVHz|-`$GuEM5E$L#J%K2FE?C
zliRXd)TP~A`>DVUvl#tLzyZNy6P`8mGw;AxFI2PXCSxZ1@yK9OyvM4^R~{S89WPV;
zHh4e+=8&G)=$L~SPYy1{#sgxgk<7HZG&?JM&Z?~JEWFq-xK%AU0IhJS-lR4%le|P(
zY-hp*R<6_<l1{4LMT`&9Wnv8uJT}$eVQ+z`XZZ7Qb2T<-arb3H$LySOGjp3B#Iq|A
zsSO++YP1^(U*YBNE1iMgd6NI0$6MY?F`u2&VxImx^^&i{+>EsMfZm4{oc3FW9l$9O
z#<>XU_R;C&<`q~dWAxTJ#K7HzPjJ<|^^2ZCEbW{!b3Gm?d)HBYcc09!D0W>)k|u=3
zIA^eGpgM<ea*<Z=;k6#9&LL?ert{IK^B~1wjbfEZarwG5arGv3(+cccs?=5t=DHy1
z*C#_;*|J_3FkY7$=Mc3bKU{@TZK@P|)Q!W!=4s|im0`BRma9fU-!|0?m0ZZ%AuaNn
zqqnaKzwOxLm+WocN?}?k-+ia~)3;gokL8=cpF|6aw~+2UExlLj);~Wf{EQhyE3Z~c
zwt{O<f<AHgGB|sfOow+sq=cUi5Ds30ia%Yzj<pS-82j-8E{JslG=k#6BMZn@@v<2=
zpDbWk+Xr9^ecuLl<4Qbyd4Y`im-79~3l2~Wyn6s04)#j%tpmt{e!oEaa_;~!U(1W_
z1FFRS^1=d<NAH#j_AL<ZwGBPMM)^{3kimEW+Pa{X6?YGyspY8!VKM1EJRtcXncT%X
zf?5roqWbQJ0?KZSm8Dzi{Otlld-DLQ+^s4Enz|<eGdwQyHGt##5NEh&;FQqqYA_Kh
z!JLIiY=HCg8b437z6RvnJUjohSF=){!nv$t6SkNGJ~x*h!b+z@3IvedssWGHF(BNq
z%Bjn?nis0qz^Y(?HOFrsr3NR1xco~nsdeZ9xh6-sawv{QA3eY%Mf^0V4}ATMq=yev
z52@oiP%ku&fjoQqmj>eJhns1m7C=)%x4SFIIi4IgWi?!O%&HVs$}aQ;XXv9|oKN}W
z(UXl@bgET-wfulFV&EvDj95GH7qv_jYbu4#V9bX4$4M|^$kpmfbk2H)LtIJsjKq79
z{7Np6ka}c4xmtqXgv71#6(aQiSpm+#M{3nD{Y|$lzr@bSa{qA3t+vZ0<`^Req2F67
z#1QxAVIaq=asSf_&4%9@W*#s%;K4fU!KZkD+Z%NGzYXJZ{a;@z++ZnFh)?W|cz;C&
zJWesTwI{Q}!+3IdJ`woZlVKj?O)GA$J2`wcU%e-<Zq~2flUEx~X1<HR$n~a^*~r=a
z?pN~(`qd{Jv&l66F<&t47bjzTYBl$#_RSZ8ZTrc_T{yi6%x*pz$o7+YzW59CIq}<1
zZgZZ$Ka8HNXL6o#*j-pn<TUb3WRy5KUrc<@59?O{^dU-I^}v>ixqqTg*2u&C{N|Wy
zb1lv>Ui-n0`(@;MA&6ct4jKAv14_}J+qt|wk3Vwdz9p5^Ur=7w&K&9l2yTF1QRjAa
z>H0d3eN}%f`9A8O3%PWzO)6C*cEiCACfH>!8EHwz+5PeI>N-jaE3pxWo3CoQ0@oz9
z8l#7RTd97n#<(OnBO4bc!7vXS<p4<Wo$4f6e~HNN^IakR#8#2lA!L2=$+knPdpS>F
zM|>IPVip$I%lp}5k#fX6CP6qt8z1hojx-bUC^S>vC0+})5)re@%>z4{%gUO|<~GeO
zYo1!xthOe=vjUIR*q%AaZ;^sbWU`H>H@8?#LWHY?w5*A5<h#<uHJkpzh1bF<e;X3G
zp*s50?fKc|WOZl2ok>2H74>gbeC3=n^z3cbLdkNVhSC3HErY+6T9S5~{Xq<kGY9G(
z%J@9s!hNJ~OcdY})kpyZt?A{uN1QY38}~|5$a}e;+eT@khR!>0+^#}|PZQ?yRMO7N
z;k4PQ&WCBLFQu0k7Q$ouJs)xRw@GC3-L*8;s+W@XO*`+}g1WcLx~k{pjhwmwo7*^$
z2llTis;*5X30ffl$*UNfUlsghPR`!!pPoP2OoG>4^1P`bRaFI+FYJ}#;`!}!;Rn`6
z=c_4dhS?9nMFY^QY;#vTj+}0s8mbXsLFL_6Hn&q{u*b)myV3r9^{8*R`l^mb*TX-Q
zK3=P)+>X0XBy&ic^51MeOiWuEPF=HZWJO`!`&d_~PKkY6CDPcfCh>>zGfV<KnlvPK
zCwAq`H7DK6r?@e1$X6*)dey9M)gMIDJO@~c+l<!gO`jo&LhTjTE+|i*Q46P@{W&vp
z&R!QR8zU0A*b%M3*BzP_sE?UkhZU=K)hc4QKrDgm04Lm7r9N~@tyitTxa_JcwlN0b
z))m-uP+$5$6s3l{u-(CnL;MjXekyi&hvSSGk;mJy!7^N>ly2H2bqi9-S}#rfOrCTn
zA_WgTe-DnCy2F?_81BEIubpM;RKoOw9|#@2v5$YtmA{eVdRHgW%;Vie{#>ZpSF61C
zP)hE1!2eCqk1vhHOZjUad`4bC-;u%|#b>n~3JDADa5yvqJ&~)%e786)esS3~O8iV+
zeBiaLi5LDdeS^7&vderBVk2cxy4v#txIBmDTV$ntXVW}5g05BNRW0g!*xQ@}!`v!0
zuoVGtt5wogRo}X~bKAB}+cs_9eEa5YWopho%=Y*M4O+=3{+Cy;{@tV&@d}e9#gd0a
zlJr<thiPq9iP{?y8N_(UM|~_Slbjf-%fcjbT#pL3ZfgO4PsG`jn|}7A&0AJ&o?E$i
z^$s<&XWqTWs1sF72i&hIlgj)u0sY%8t2-4=#W)sSYui8X2^k%c#z=gWzt#~c1^PM}
zpfxV+H+-6UO+mCodSr%V80y77Q`VZRF245m8aRxF558@FZ}rl<(sE@N?DDod>&;W}
zztdQUxd=NhJ!&{PZXIaF(YWB2osmyH{qH*K?wX_g?~I<e;J<+$Ctz|_qhFmcMKyM)
z`Bmz4%7P;Y_J$+iGit8c&qa{qTGeG#qelQi?m5=%7JvE&>q+PyKhYnkzW4Ue&7qlh
zJZoNTW&@wjvh=LeA$GvsunkmaIK(L2ZNl~@@G}h65l%92c?_FKuhnC|v;}ZevP-5C
zDJ&plN63wF2K6DGh~#|DjuWBL^)a}5j2-;>6Rf>_<R#uN6QCD=B~W_dAvFX9&`O}<
zzIU%!5@sJD`JE}|8gtDg(jlFdU;Dy(i_UxI*@b0?)k3bf@3yw9WvJUo8Gr}}5ZN^i
z0JxbnxeT-Y8Me@P;2&j}tm>65W7Lu!W^cR7IBnpJ3vX2|aD^50kkYS8>HEH6>EB3c
z9RAvS-6x@L^y2d$36a0Xm|`v%WuA^Fn|PF=df!?l>mY7JF**DrDrq+ueoz#k43Z=s
zHwBv%P<wh163Ki$ei|qa!+{A}1{4R3WD5;$=28>hAS<ViVNms8V3*loW`p2MWY-?u
z_=9faJwIaOKQZ(YwHn_gjqi-p_*2q&w{CowH2%OhZTzRY@dIf5!ck@(M*fsE{uIyd
zI!fU+1^0K*>UTNhX8fH3<2PR2Dy<&WtupGAp0cJNLxwh_$!JGnwfT;D<EA<7bEamg
znhx1VGDheiuU=wo9<|*!+rF33r?0wmzINYyNV1V^gm>NlWrUZ6VSRg1T`>V;|52)u
zlq2LVv0Cu?rsbp;6$Tq?5zti^tPhLRrsJ1LH$?aq#&fE~V<&D4!CK*EjlpIFYaxVR
z;=zs(w}mew-5~LR0aYiwCKo<6k}BZ)zGN|*C4e*TGuO}`<b7X<enk4A>Z`;!mXKI&
zIkO4nqQs$XM>k;0+q_-5M_lcblLTPxHiy#;x73e!t*Gc{5?O>CMOuIvYUfbbxtft6
z%Mu@vK#Y|)J1uPF6@>=lhB5ch6$Y0Hs&uP;@QK|qiaOftNeXOy#g3JtaUOwy07d!g
z`@Ob0=7_Bxty{gz%B+9LK1}IQWT(5>u(a;&^j@r610A;d(AVz6AuAgkkBk(Xzjdlt
z(5dAsURgumi_Kj<^ozGfO^IwKxl>|(zZnK1Sdz_)qlSNcbki??qm~Ie^Cmk}J-geF
zj}l+DyhV)B0zB8C7}@XsFjif>75-qfzD%(3ZMfju7CcIu;=#Amqw!5_*zVv~(#0YV
zDoZTJT>y;Z9!>fb3+3j~emBj=xEFl(=xvnijGVpyXO#Ptee=g4-QJjn?K8Nb@^glJ
zYxrBhy4zRSX!oeoi~n=H!~8@qyr*JX|1YARahzspf3kVk4KP5#(QAd~U7eB@;Hm!x
z@B05`LKNE--^vEy#t49s#N$~ys5mCnAEm#JXW>h`F?#aj*5mh&J7H*6zAmdkB#%QG
z$$#~(#pB*gNYF`~camWB;={XLtz6OCcHz7%xV<3HNn6tTsTZmBD|%8Uo5QP9)|I_2
z#I6(?I(7h7B!l^UBu0{TD;Rk6iI0Lcq<49LgS`J#-rpea4>|9-2YM||<Kd=`m|Lan
zVJUkpS(N&#uN2D06gE7S4bu#1JZxE=Kg9{U<YJI;nN&)yK9y|1>k>*?vX3qa%dAzh
zkJ@0B<TG~mR$Vd?WEUSKsy88mv(7}diHW_eXSy9e)9tVvyLU%IDR<63Fh5Yq5Rwz7
zGM;ga_UFw!&}y#hy!Y&FaDBu!2(0!`&m4ZfS<`v%dD}V_PE_P0bNAD{M-rEPxF4Oc
zZZ=v>c$hyI@h6u*qZ3PwGxUP>4aTCxe>m=t3-*`G^@2S&-v@cA8*}`BDQw0_hu@yy
z`Cm>aV8_SUp+d3@(;z!WEh#G;CMH<O`Q9qvQUFw7zjWe`tR-uejR+!gGY432<B<<c
zxg+vI&b38eH1&1l17Er=SfM~(dV=>JdV;%XdV)vxaO}V%`+l1J!2KpF*+C>FLRAq#
zN8dcg=&}zj;`P*b68g9QU;x$x%xC=F%kz|vjtwmO@pl+lbk#ERz{3@0Di*n4y}umW
zE?6>8N@CKT-<d*X{t1Z?dd!Is!lMV|(Tm?<=|1Wg2t7DTUA}vrc|P+n!|~ftd%q()
z6?ftl8mAZAqVPI1=g)jkZ-xEef#SJ(GlGmBX2cN=8jl}wWHx^E5nKxyTVzESWz&e_
z-04^&?3N(p2#s{=fPoLtI#L+W6!Q#&kKR7&0J`zaA~9+0I4=Apihd@ZlRrL=Hb+Jp
zO+us*>;>4Cl^1Q+i@C-)x&a+rB({WGj-!(7lu&1gxy?K^5gW9KGF`;&oY&#?Xaxt|
zzNmC92YVcMrH<iV?IVjs_1=ix$kJ}yKf6eLl`Q>)hcO())Bjnt&%7BQK6;#y&WNJh
z7qz{N+dm!$uN=#$iW4D5ap%8t(Se=j6+7{E%W>l3{>Mew{Su;_px!)Ac6&F$p9lqT
zXLoGlqBeHyzT<RAk~<(4Z8P_8+Sa*kQ|Go$*f~4pKkE4peFq^hOVsv+wo{sQ5X_PI
zqZq^gYX!D*7NHP`i9!rx)$kZHhu_|q*O=XsGxxcg^J+r-_X7*INw(`30g*(tRdQAD
z1zI2oX-QaOW^`JH1B{44__GwgTMGYP3U^B3y;kAhNPx`;D9rX@`Y(eAc)Jdx`73l;
zgwbG{f_Qk9gslAS=nh<8Dc4^eUCOAMuUcfA_Nu-*$mXwd7M{Any8T$+Zm@3O(YKp%
zyR>m>+2j`dn|t2knjLbbaG+D7`qSc`L-aO(&h-D`ifunrQ+qn^8>QiMi#}vV!v{qV
zFEQH=NlZ=wO=eiOCjP3)J;;^j{+rjM9zggZ0&qIFt2IC=lU`ki#EJ8|<EJd-(9H|2
zSQOcz=*la@!b?_N2)i~1m;<o6Uxicwd*H01&g?cHn%urw!eox_fTn*L5~m-4GfS(Q
zBbyET5rrbq6q|u4a$PxQ#RV(i0Q6ELcYOcn_$fwek}>f_HKPYT`NN`CJlu8MzCAE4
zZCONPB=Mg9$NTW)J@GKT`FK%-U$CP8CBKHT3uuiJ8y0DIfoqS)=!c`m^%z^MGxEQ5
z6*xrVX4e_{UGYL)bpoA{2Y8&3k7qOmol`z-5C<OiXVnQ)wqRs-*e2x`!{o-lVUFgn
z_-9==K^_R{Ms!hMj_`#Bp*|17mc5L4`@apPeN3v251!zE$DCzej87yyr>jAHANRtS
zwTX*eJ=M^2L5afYYTrc=T`T>oo;@MF*i3=nMk|si(E7K`pMjixYEdbV#gwEGYB=OH
z^#}OsL-ItF!O5Vbr)llooU1O`r_LrR`>Y-<ASC3(-EQd3Y`u%JO~QCs=qC-@>EO!~
zbSWhCHj~WD#Z%?x1__z@<b*KeYKDM>Guq}*y+qrrKT)2B(DWH$4$q3^`8`suaNizb
zmi29&X#egL5xa>hu^Gr-hOspfrkQptOzZ&N$F%_1RFrE=s*<fi+hdnoPR8C`Y_kfy
zxA9kuK`+6nd>vcdA^9i_or?c0%O6tJ?6&lOv9NUMNu;oE2&&4@uM7G8-?YWU&~Y(c
z`Q{IPhz>ne3u?m-?MjkyDR})_h8(fTAV~9gKYUhLg9_Uo!SV{8zf+S`1)W^)s%y1q
z{K0-5mLr-eLo#VlNqEqDroU4M2hjEM`PukL-8a$?6nbGcG4i32;YrV61>8D^w&}M=
zz&W8K`)c@~md?ZB+~)Zkc64R0Tzp;*!2Qf@^7UVh4Cg+PlXckS=9YbW@2Wn1rMEwx
zGe!QEBcxa^){m4$_cCwHB&*alb}7^+rEb)v_EfX8IxP`AVCpq=`sqLQz(OwOf6f+<
ziILb_IeKgM^XO^6dB&vguG`SrHFf3cv)R#Y($N=3Qs~gvdq++=mwi58>KCVQBxtg_
z^AQ0d{b&bZ^guqIE&W#IHjuY_2V#Hu;cRQT_CNqxR%8#vpZK;}h8=;v;Wgsjqw@6X
zFh-sz?J*{s7si<<!lxB(449(F;MJN)bJP=~!WixqGTbXhp=0Wk5?aF6joRn+lipK8
zcyy2Qn`^Y6bYJyO39a_GjB@;>`>MA~ICr;>(tgsds$Uy6F;V575^jBTje^^Isk!Ok
zAvO1axfo!(U21)ER2W-An+~GjZ%VlN@z^MTS?9)T+&?T8J~7Iltxj!K4?;cds}7Y2
z?YRzxNQgfzp~uP%C{lyFk4nUiXB~QlyEmkZci`rrI^_T!JY6D-{@3`{%s#XOPkt+(
zV_?~farcCN|FFEr{e9A(=h+{`dZ>46%zhnHxAaLAxZXhb7`f_6YRxvLpBO-dNE@Ru
z`*Ich9hvV0#6Rv&SGrZTgy27uI>s7jQPOLz72eU^zZ*f#@Dclgx@!epxu0Ih@%*|!
zH&C8lNDx2RKl#0OmD8riSE-zRD9!2~U&uAqia2=Oe7dlWaXfAP{d3R5?cc?DWnF&;
zE`GF-%KW<i^v3&Wp$Miw>tAI)u8KguPNtOkvNZThY4Bxf@b3P}_v6JI^5Xjb{!W-`
z5%^dQj$DRAuy(5{-0q)-+<>L+Dji;T>M<$$SpQbgaJG8$LK)n9Qt8bLJID~;>mPn#
zW4pQ(G#@3l>n^|4Kg=F?OS>;gk2@C%6M9Lyj2m*|{rwRPi4A7e2yy=?b+xYkuTuTh
zy86HN4_|J6gacuE5M6r?STiMzix{0s{&3Gi&3d2bXtt||>8|~l4=J_OYsbLRp#RLd
z6e!>rhX9W3$z`_|;=CgIIlpoV#g)5H4KGwUL<OEDIfKl1ff%bUr9A!5Q<#U=UnjAY
zLZ{_tFRHSlCDGxnNQ#h#ENlbu+2Kq0*|n#(B0|#t1GTIwb+UY+gM2^re0S^tHP5fk
zNXCw-F>0_{7)DJ~cz<jM61336ZPO(^psy0Xa_H3XWeP_x@S!lvH%~3Srpz2-TsVl1
zW<8IXGv>`h#Mf|mQHB3(-6X?vb@h3Ol<cb>kZwJHDwo%Nmk{TkKebJ+Nv@wimBPz?
zmk=+XKULE%kaym*!+6F(NtPg~$Cov)L&e#LCGhmcKsdNl^ZA{pF_HpL+VDhTK5sjX
z?r3jZOmcI_X<<A*crlTB%V}_sS!H6LI3<|CRyJdW9L$pI&-hDnZ~VGI9-PA8F0n}A
z1JZJ%>k?x&xtk^Wbmhrg&GC#bHCT3>wfHkn4FhgJg2J?vnM;lIf&5TAR_wiQ76OL(
zPAxV33Gh!|O?AUaBoQ_eWj8@~Rc=THX;jDg*QpWvIICc*Mm>|3^^9p@QR6h-ofW)E
z<<E*0pccm1LA=(1wq{n2+-Me?H4irl@7NB|pUJC}Ib!Z$$tX4EfSLCN(KWKU>v`)u
zQrn)>IfK{F<MKItdg~Qw^3`__Sn_i=>*|_k<bhqPFXtK|;`!P$_{|q{IbYYD(R#%=
z1g0KPpXk(a1_Jh^17mWH8U5AM*eW*F{g;5M@hQF)aoR99A|8s7K?&Gw7|sX;vp`)g
zkrt25_3uH+-<%O{Q9GCKe%J^N5-~>l;v)YI6{<6#4UX@|#RH8C`m0081Mg`H=VhJV
z;q_lJF!~*E)!KtyclHPYB+{cBPPd{(_DXM%@ze2wl{2jj(=!v-V;`==yi^~z60`GR
zPOb>38=z%G^y7)s#5qxWQy5>t^|##!4a7e6T$~?-<Xi_)`VSW)=)~&Kji(DHcd6HL
z9HCK?M3?mjvSwoofA*?r9qsUjg<`=DzYVc25vffPsL3#upIt1#d+oGviwq|Kac)ZD
zW5~R|8gC!JSb|u7a~gtJ4kH8DJR=vGSbeB!gE0%C>PDg44rb_8<fHlx8-jmbmD-O}
z3*qO%bKy+&+Oh-o0=-7^RJ^{d0}oClC0L)xHT;?+tN0mg5l5>J@v;x&@^6!}FF|cU
zG-V>fTEK_<sE?LWAXtZcByV7(OY0%H)<Zp;Gxd4MEUQGkGprrDu3Tg0K%=NXVPWpz
z?_2zlD6`;Lc`8eu(;4xU0PzH{&(UQCwFIlAhLx#QJ&rG(cf}>k=bcxk>RQzht^bwk
zz)h_DL#T)p!2bK0di{9lFULX^@UO+l9~H0j?tzSx+Ak4Y+XhlkdQ8IWo*GDEw>Lyn
zR23<-q;z;-V8=Y<;R8xxnC2n#;GDA656vwzr<$+m6XSJOlwCLrZV*=EWRenb?%Up#
zy%DSN-H?b`14)D532`<5bfBMzn=meMcb~qA>dz95)FB1_EFIi}0x13G5QV9qBQ<Yd
z^{Tx1rSoMszuYU;-6LP>>uo47Vb#+X7=*SE9t5TP*GiP8&ghs2)K~DN#-HY?<}#GB
z$h2%QYq0avRGwdyDn@jiy++%Kk@yUM-{g;w!Ut<{<e0i`MK{9HwB!t*(x%pe5Oc@p
z?N(1HAXK7yDTp;F*k~%aNP&|O{#8WIUnAo0X!RNbcmyHu97^u;dXNyib10l`s*H(A
z$R}X7o2xJdGbyB7M7OKvUb7-(&4v)8U6-hUk(HoMNmhX=>Z~?%j+s3MVEiITW_9&Y
zg7mLa^|eFo$q<nC1UXyR59Mt2NP%_~kegP#xOwPST)!tTZgIZs;ba^PQZ%?tzC0)e
zI#6IjdoPn=XRC{z9OTj&*ve^v1;-pqA$XVcxoaqy`>r6P>$--vnK$5WHv)!uw>1b?
z*D!R09+kUQeRkAt1_>h=*i_pC6oQff=z5g=)o_SYJgY?BkSYMfdCDK&JwLqaZCu|Q
zKvK#+NOy@xzaGvRcq{-7nN;BM;eK-f!sq!91fc9`^JOf8Z5NlUteHEhy2f0>C2|@N
zNhanL{&op7<Z8qvsN9BtNH>2OPT_b%0JY+kQXdW{N~vQ0&<9i@(r-NJoprz%_vhtc
zGsYy^Yx}CMb}p~S0-TpehjT_B4G>wL7;euf8pFiw2ykMak`g-t9P4hBK=B>&>F<Vf
zt{;#WFF2pdlx~wRUz7sdr0%^aFhP9<c^_S3Z$wUT{<@QSrFwq3xfSzhoC#Wq!iJRx
zMvDhpY#~Ul(jfU#KysA>l79`Sy;&9UCm+RD1&pcW@Lvq4zOXhRBKEt({qV%CvfOeF
zW2{I$=AwetX45)zZ>L1w!n|S2ACyMme<{E3@%O8F-jZL+i+242zoZyf$zQFHYS1_H
zi4Zx)vgx=5<GdtZ8~y?KW5+oXH2Q2f+Y{npJb?)d-MeC)`IYV-Xy8%&Hc@q4+KO`(
zi4nMY?-aO4la3gP>-AUsHO?5!KkRnA8h)&6Kvc{r-J0Yfk^y#Ae8W_^NM#l0itH-k
zXROxTB){M1uh70Oz7p!Dg;!Hv$!vC|X_s$<e!NJR%5M@ok*=7ZI20Erd_(`)q?YW%
z(d#>!r)HG_Qbs1Bl)&U#dgy8=`rVqyK&vNC@o!nxvNHR(>n4{?E?X(srK;&EC_qv#
zI3@38$?TOac1d8xvzN-DsBfMUzTFJ2!fBiBczD~Tq<DwjXK+mDFh{6Y`!Gt!R6S*)
z>0N&&-5M`~TVpH8!s@x=P4ka4NTG3qC|FW}LuZgdUN@H%VksZ|9S;)Kp)zVN{h%$Y
zdI}9EaTQ*1W=A{7L#KJ)?E8Ou*Q!;|3Sipg_Yi+&Q4^Mma=n_(A8mNCqG>$gvjPYC
znZshd5;thu5muf~wfu#IF};K$nlV+3<E8lye?3o;5`j;QiF(b*q?+dz8khDr$F|U-
zoN&M8XP#(oVM$rC=EPY>j+B5|iG^N=Z$D&Q0J^=bJ7qF~bml&Yd_C#}>_D@`ew<0(
zFYz<bnC)e>AiQuUqIhQnj7+I!JBz6PJ=hX3rVU2O^!P(KgJdk~d$rV0mw;cK6}}5v
zpIDF(4~*dbC8+9erBvpwJWB@z>#FX9&pB%Y_Bo8-xmXLLdRr+8;lZ=$gnt_x#tik|
zS<sz5@xP=wi2oH^r3<SES?hRx9w*)sdyg0fJb?ueMK0xp+Q=GuQ$L@e<wLC?wMdz}
z0`kRJ@0(v17&DUOafVd1g4cR2%3QrT-bfn+Rs{V$JPl@zWo9X%_RbuE+ASl(Z>s*?
z_y$CpUy2una}|~Lbsj!^X%3>B0FqD63?mnm*(za5zuwWzD&U#`VI82)1?b-v(BJBU
zzDA=9(v=|<GUpoG9(~nDne4aR$wo22{ST#L-@1DQ*R)dm=IGexFU~|7cj>KYwVJ&+
z^LT(GVCN+?D*6gc2cgU7BNIu2QG>laUmF(UC$q)wvVLUM#W)_j9UXdT4p;1+5&mm&
zvttg|=<X4;7dkK8WE^Hs;yQkFuYB{O{^ox9=6~fI=LNEHAn^dU0BAA$q6F3~u~}a=
zD*SUAH2944r)`vf(;5L~fB0h$LXM8U>Z;ikGX73us#+sf<}XK<(ujA{Y<BLWkyeh5
zrBj)s<2Xwv4VVB6cmvXO<W5>;oIYSBzPLrc_+Vt|dHnuPx%=Zt$<r~sRQ(n?vj?Lq
z|0)8tqRn*W*<tqOG3ijVHqL%`IC9!NeZ}G?;KG{X(hDkR^7`9of%-LEQe*YIe->Hs
z=dEYA-SNn<8&xP-ozi0toCu)5QA7~mad!Cj^^C=6grC|8*c+kSH2Tu|DT=&U+&Xh{
z2-|s|B131QYKLFUcaY(oLU23$VR%iEY-hp&=8Jkv3Oy!;ej|k*lS151OjPF|xcY@j
zNXUaQ0Cf6d(D6iUWp7>g>ZqlS?al4?PTB%sJmj1k%+2><Rga>F*Pac777D*sjJ=c=
zq+lvF&q8b6I8ZMU2T-?u#o*P$l3wpQs-Q@bl%8Nr>W@#?k2b3w;aDFjA~AdX>?)4+
zfNBK6^9&W6PKoLPjI}0qtjwbrHCARRj2tTqdB^&$@4$xNVI|&*W6BfZ7!=OM&3@lj
z+}tIOP#Yu-^<S7?I{}VKETx^2=%mDJe@V;cxkhULL*{67{G-u+m>Q|+eNYUOBnQu}
z|6K`fU#Wrv0u;a6j>DV}SF4kH5P*z0(BCtPNrT~jfI%JIjXXratBxXcgLw7k*?8Dd
zmaCHessMWDSlBP@ZL_kro7b61$h>q+mHij;@;yjJm7*ijxQyq*pou^u;H3c@H2-{+
zuW$6u&sS*ea3MOHjj>m)zwz6ENQyut0UN7C-kqb-Z<<~Kj8}^A7vsm~FQPZ_@|m+q
zBk`h8WQJ{Hs$jkNS^Joj)ZDq-yxF91%F<jIE6+rD&*gp|P@@_T5I@XJPev)2GGVl1
zXf<pTNYhh@1;I1v`;jdCX-s$<5P%`pMGT2U{t}SO_X|i&wx$?3JqPi3k1wY^2H{bY
zD1RwYb0HFtqpMN;xzGxv#`0}3CM6l?>b)fd9Tw7xO<5DxjxkUG<m#nD5FYDUYz+1$
zv)y7zjDRvu(=wk(B_;y3Mc3F<NR8~lF|rAYL)B3k&W300kZJc6a`S@MC?Rw{v0E$v
zdz^M38l!oZ^UZGen}_Ugq_#4w?Olb~8A5wqV;H+>$O#B;bGr(;&mzrr*`G>tyPW2_
z$B?x!oZRZ-Z{2UY?Qf*SyhPE6o+#92_=m@kYCnM~gE?@F|B4BioOfqI;=%ovWTOay
z<Vw9Ui+feDU?d~re&P^3dC7XR-FdRddeZGY`Hl7DNj$+PJ7<ZTiuzqLa8GY0LPdVp
z*Q#!D!_J>Y!L74oO4V!0^cXsfCtGF_lm3`2sIz4j1)e`8Gwd8WM&;FC^s5c>>OJe#
zb@J+ves!I^I&8gKC9nReU#*f?|4L?TYwIjbthzFV0n(|h0`kX~GnvO#g%||3Kaycc
z#_e6RME|`$1@1JfcbM<t(O(KE$!$zAE=^EFTjAo+ty<xwgg3Vq>XC0uHr`H1P>?@C
zgAkdCM9Rie1TrFv0Au;^%a}0;>G<ZMSvrSFdolvcF5i7EQm?58alV2&`hb~^we?&9
zS!-tsjrFe;u+w*^Xlo6BRpRCR6?s?bU@$Wm>5QR7UondxUXjuRCUS3q>{h*)f*m;-
z>%R(Mzmcofl#b2w&Kr~a2>pNt3zwaTs3+jcbyC6gDH2$;rJ$7iU4L?F_{6E9P1i7s
z)vd>O2MS1*Hl~!~`mF*Aup3io33*F;zcGb)_*Q`kRU1?G?SdJ7J-WC~zWO9ZBtA1+
zv?rzRE-2kB&9=f97Y!i0Au^w!_?`lqRX$IVW#kmUv!L{8JpMRkt6GHAiOEP+U9|zZ
zCTP6)Nda-|-4uTiH@6g&A{!g>@TGL9ZQPj}OeOu90$6b7%425F_-j6``k$q6%kIYo
zM8?ljrfkB^H7JL>k1;9GBIM1-`ptGQ5xjXng@JU^1(di+`YO+6&Q%v}g~q0E*8Qk%
z^=viLLZ~NeP@R<m=k7r?sr|7=WWg<iXwG@L1`6H}QR^=T`4<%D&Akz6INQv5=4zPM
zDC}zGY7?)F6bj%<>}tyV9j|}PA7q!}pGN+Oxu%F$RJir&^D}w1kU!WlF~$#)V;c;~
zB7>Y!c&^h5t+);eK#kM;t4}S1p=&+S%6qhRr6X~!F-EeQCa8CpTk6#z)R!<BwoKWz
zN0yo%HY}zJWn>JpT`RQv#iC1%sah=&B@w0wq?Fk&BwQT}CgEr2B=><~mN*?1ryE*p
z<*l{#Fv)rS@q~NLcL0^&R@1iBJ-Ah_Y0-RcFbE&Aiad|gC}EfLP1%f(u*?y$BJt)A
zK+izopVSaD9?^xVeC$?F!&Un#eDj)*BxT~hpM0e{aEsilAbZ`Q1ygpaLAz0ea{qNW
zr{k_4a@+Rs0-5P+2M|RMGv^!9#vD-V6JSomlQ%*_er_1R1LHnhfb3vI;td0cc>lYg
zYTe<xaoah-zkcyOJcw%X;q%6-qubOH%JI)GfN{Y>tDOV<({k|aZb>Nf+yMXPrFT67
z*#(v^^JT(?h9{%zQdr2}(Hk;W=J1Wz74nTSQ6`~_(8x(N(x5VtvKur6VE;)9ImV}(
z(9r9(vcT_55;fy@`tE_GedgO({Vy)qEfGE-zyrAe_S7ab5z|9SrAQ;Nsv;GSdXVEa
zdro%Nsw|S7Ym)rqVP4&e6a&fTy(REzP0}<FxeT)JX*merCT>*^rR2LP8T*B3<hq4M
z2F)eR&eT`3Ga*+YvorP8*_ljaXWBP22esdwL}})g+T@&;D{%YsB!4Z(Nu>n_I=p(n
zwoT`Y;$GVNBm(!jTGG{<<iL^p2MF0zKy1B_^cwK8j|p+Owi|lh$b>F@_FU4w{b>3@
zBmt7K!)M0`BOs0~A)^9Nd3fT>$WFzMc(YUe9GMsTs{cWDp(78aV`k2{*|~RLho+uP
z())trI5}PT8cBZ03%)qXcv&TwknXA;zX#QAuIL6<q5h+pS6afK-yaqZ%~`b~XDY&X
zk3s>Gxjzk?G7dMlNg}Ebh9UU)5-4%l4V{9mvQu`$V%l{H*!$Mp6>HR>12}Z)wRg>7
z=GeteSL5^#oSCim@jx_a3$RcfrEaXyJo4RPTlc^eI)$547bfkR^EqsNUDy&sPj&?9
zq_#+lBKw9?@5WYl1-%RISs~s|I-}PsLraYdn9&O<n1iKd$mrFc#h8kFhKA?ips`gY
zM5O0}M5MB;_AFZKwqaX>p>QxCSIE|4eC(jPU9clHCu{B^^K5+i`q1!uAh?sKUNSNr
zLpGapjP#_Z_nV7%<*XTffpK2{HRi@^VRSP$<K!IX&l`>)2@sQW`=5ROUprxsfI?OV
z1*RW!VprkBnqi1p>)W&GhM8k$!Z0$Bb<Ro2N;Oxjp<Ca6btmKsCi+TkGv|I`j6Go|
z=i{dNWO^T*jO@8bsrB<IxPE>zPS-WgBZPi0iFvl3OmzRfl(er&Gq#=#O{a}#D11>)
zT@nh_$-B*bbuz)+S3N#o?9<5J^Djs)xBbY-;o3RKL|K;az!fKj&2i@fVZNwr#n;!J
zRCVEeFe7Y(x4i;qB8MA^flycrQrrLo)kgl1Vk^m8sKoS#eMvr1E#&HAMO3seX?8TL
z2A760X(<1gS$WJJ^`R3s6Uk~^7PK0yM&2R)Di99eTG=p^oT7;&h(;-&!k%7WOxH|B
zY>ejUOEG`QnaWS#9$;tH)5dq<D%y-SngzyLaj)y8s!9C4fInm^_hLlv!HW;uPQzN|
znoQ+xWMKkRF-{wTaG`GXfiGEel_A&zo=&XZ!PD|s#}@uDH{AV|6hd#+A2O{zyg7dh
z4s-R2-E=5J5~PXyUihjEEC%G{DW6fm{E6Mz<xUnZlSI4l=gUiRrZEMXcAW_EW8~N3
z4f}U^aJTvyG*>d2cZ0%YdLRQGt=3jU^kh=9%aC(DhwP<oi2twA+y&{Js^l!RO+%RJ
z>CGxK(oIAB+xi@%{ujAAk!W*Bsa#VQ{MC@Q?2RdA`qSsl#bJme(>Y4468<s2UkE(A
z7S5n3cX%N4zx}g@D;_?zgZ7!{12tQ#_sP0vR#7Gp|FR~Tfy8SEl^Wx$MSgoobBlK2
z7OxJpxJCcn4@*<p5wxoSmAze6YO$V=(R1DlJ=RpkWX1R*LESm1H4*w^;%d!}u8gSv
z^MQUfJsBxdT9M4D8-J70t5u;kCPDiLcB*+}`oX=}%_}4|=E@Mq_tij>w?EfFVV8J<
zdj|m{(TV+4TdfgZfz3#820g<2F~V~!e)t7sM3;=M9^@Zy6epqR;hJuj@LhxSp-3uC
zmm`h>HZZZ^@vrK?HT#BD2PuV(i(-^_S2OK+Z`{Qph3Nm^u-%T_h9NM@>v8+}z*37*
z!p&nM8RczAiYjE7jPfmvwy*lUs&~%i)SZgrTOj$G>Q6PvWo0Xz+czi(lh1+<L2E#m
z+}HIp_l5;!*^8`cyl>Dx0^%?DYB!RsqS}Y7Or_XL;q2@hgw%8WQS~ELSd3ooB53gJ
zAP|E#i!cAFM)-2O&KkLCP^<b77ARBg3vn44>dk>2$VUp=icbx_hb7$iL1e@|a<9p|
zktFgq^g}0!JaDpcMIx8xo^tUYeDHfy*5dAo@~vp+$?tX7EWuH4j6As$Tb8K2lkTkQ
z58vZj{8c%&fE<SYhwu5973I7GBbRvza!X!luIU`36UW-VMcM4`tp)qfgD6^KW;f&L
zn4zfhnF<<iel^Tkn~*RU9eb>t?C2i&gPQ7pjgFbQjd;ChSgTP%G<>U~TVp3TKyDib
zALFz7uWn4kn>Q-Fh}$s?{1=8Lz3tJFcqPHy$)DiO=Dbwco*rJf26<-j1d0K%7~6Hp
zdc&tVH>=(JwAwrj>#4bzULAam<0JT|3yGkOl)=0t&V#(1a2>cYwtjFY8!TimxZq2j
z?m}M<=VLGtfVcZHNfG<#_riPNQ8W%Bc>l9=F!T?~dq-g+q(};B*)wym|0PDhVOS3k
z)k+z&0jHlKUmI*TlFH0rAQF2(ZOyoWKUBSpv2ryYzg7>iMadi3O&aw!gP{g#%^3EB
zK1LdUuAQLE!{OP!xDeTmw!ON8w>(<qG@@ceN~piU4QjhNY(IrFDRt2SbMC8~HXCCZ
zadhKl#IFy(KZWOJth<Z|@WuDLVZnfa0c8kdC{v3LY=UnS4%F?dUb>7cs`L94I9`&{
z_BDxJ=lAI}yYey`oId`(-@KH5+3NMnC{k|zzVUM-0V!91yQs>{00{4c^eNjA&z;Cg
zPpHACkBFn?6DKm==FWO5Tu;GCg2^LqhkXPOZ>STo7A2_CKP<(cJhAlyRzUx)2kO}t
zYQ*(@Qo##S*?sj;O87!@b>0tuX*g4yNO=T4e)RET#MfQ*t<`wG|3v?~RD6GP{Q(?9
zfd~IMu><lmWN3fg+EG6h`nLe@zpRrwuj)tEiJN{5U|0*oD9i|T1jjYlZ9PhTUPpT@
zq^Ex5%ZKF6_5C{Fr^MXHjD5JSPIJExrQ03#9h7L_JkkFo-rZc^!($btW<uo)o!{_U
zr0T`N=2PH(#x_2v6YIo{(iPnFqXu;o{6>ya?@5Cjq$_`^0|CLCTl>*l_icG|TfZuF
z1-A$5Kv(Rq-q+3Cf@UiAt;W{QS^?ErRjS0VM$?1(g}PD#>sQjq%Tm=Z`}uD~?j*l;
z@atr300~?lt&?+Fp6W-h;$2d-t6%s^+&ovu%#h~AsAy+DEt&kh4t5mQ{F>B!pA>&y
zuD3HxLcP?FNY+`0oWGLjbT0;w$^qO{ClUq93dD|5cjyOi$k;d3^$@J@$^6`2N8I{@
z)Ox!f>YM$-Yf#B;bqA!@cluRfTf8=V?*gR1&f5w#052b{lNguX2$CUAo`E~Kd1%yn
z2@lW3gnSpmMS+zSh%lqNoG_D?XJFXP^#Y0aPUMF-)wfAgADk%suAXae-RK^38WzVZ
z#aJBNt@!!qD)aPbZpYDVqWzjbKzIpj`k&%b${x>-W&&1%ayxcNIO{Kr4!4+<`JZV?
z+>kSztx%X*=IL1}vQJF$5<=)PX1Ehpr2(Irr+;qVfq(%>DNjpg?k4PtHL^%Gf08g*
zhF|6!FfRZ<xln!dLoGk-nIjzb!4ZCQg4zSS$0a)C^tL&~*n=Zlp|nG^T`RX=jG*yz
zt!z;KV}v%kw5+=dxySLiON#6oK@qx3;#Xd_ii9fGV~=9DByq*dpUpv!Q28_ZHFF3L
zfmjLyjnwe2H1ZQ^<O92rxjR7IVJvtQ`je@;u%n@Y``cr}MoIrVbHqWIa6pGGpN2!c
znT#K;!jNhz3B=L&?Ywth+hH?@`O$g&kr@|K%R+IpjEc@8Rv3?mh5EkeM?Yfq<d14Q
zM)hU04!PQN>~1DHJu6{9>qJkj<evw)&vwAP$(&?m7ezY6W*Vz+JXS38AIOnBKRFhO
zu!k5`l3lbk2?hWoB-h9U)gQ+YoypUfg#9qV=yAKvrfr9eQ3K2iAUq}`2Y>|_VMpYl
z-fW+^J!1MwDKCsy$gO+TkvMOUIZl#+mSUtYG*NN-ALay5o5i;C7dcNVA2bUgVVbE^
zwu{MT>OPB~*OYs73DRzldzFYV#wqHl<?#5yk$3ocG-0>uTHd3cqb2@5C{i~kwBqj+
z{7qK>ST0hfWvG<>W-$2J{eJ7DZ)r@+18y<TL(k=b#4VeUb#I%Mx73;iEi(MywKM3y
z)XqSJj7sv;<N=%Z=tDN*xq5i<=}2iz3GzrR@;Zqy)o!^=@&DaCh2D5qXHae4txf^Q
z!hzBJ{mj?hiHxMcsAE8%L1S=ki_&>i-;krR?r5fVZ(j?ieec*R=2WdMGtNG-dS31*
zWJisgRCTL2L=j0<bIv=zw#=NIZBDMv>g+wQD&%(hro*f@LlUbFq84~8Oj0pvRi|?V
z$AGM&3l-{u-N;NTgLv(SB$SM9yi-+Of>G^5YE|J%zf}JBjMD~P!9did;WhAxO8p6s
zt~thE_oXpGi}EzY>)qZ2>eXC%+BfkYUQ&Lwven0=Vl~U)uO$Q&629bOrI92-a<S&{
zs@-{S<!YrhM!%=R|M3s8w~1c2D{P1IPHbN{E3@AXFW2l?`93T`!K<5=-&j6jb`FAy
zvTFhNr<uBSHHz>0A<0meqQB`bNhSL;)vDx77egi@K@bp`Ui7xwlTNPIoe>#bIecbx
z)ukSHN<E_dzu>C?oC$a0B5tJ*JH?-fsI)tx(j88zr*xXw%?LJv$!k_Zi~1`F7<Lq_
zRIzQ=nS2gsgH%6%E6;N6#$n<>Xv9+B;MIT#;_W7V8xq<no_2TI&S~tbak@++6lC&1
zOK*_{w@(~EIR(g?19+RKG$Up?SyIcU4u&5D4rAT3O3kBu0jbGU5B>ngN5Q4teZuCe
z6EC*Yh-x*_EMNl&%i^C)wH~M8?qRI@u|62CHE~lr?5V-2)o?$>A$QXdTx(lGR%u*<
z#-}f^W#wGRIIw4ha5jXg)%y`frRtH+p#zC%;YTud;KVd3l5Br`q*~_TBzc(9raWd$
zPg2MYj*YB@-*3pmFfOe@toH_lqLsnzFpOmgCdUU)jc$8p(P?Yu+zgPsm=Av;%6y$;
z!+YTA&t*h4cD1=V@^+KC$=qW;y?VS^_{yTtnmMolJe^NP<*~7}2K+joq~>W{o9d2y
zc)gV2;YqFXo;!`d%Et-cbOx0}MxP#wD9xRyZ;$!xK}i66Dg@t7bBDQe^>}r2K1}}I
z=6yH6zHri}ZRn8pDRh$Aez!;Ln#`3TFL#(BkR@DVZF9b?Y`8`;*|$2IKZ|R`Pq&W*
z7xhVOQO6uOZ=2XBoJT0uXElHLL7$ipuuYzXILYCiojms(zZXJ2MIz6HO+cAlI1kxR
z?dxO7cC-uU`$4%_&SScsOyHJC*K)IGaVIjWA`j+?%t8d@9FY))g#mdocpT2LuzIxZ
zpN*zHX^DiwAT1F+iHBc+gJx-iIlLT4)N;n!5~^<c{1tp*xm;R-<Sl&CHEYqmTaZL|
z3NoW$hq%rUPk&W&069i+SA#Pgz$g^04IeP8u2i@Fs4DY9`fq)BIj3#IQT|%<l<nrD
zU@oAsJuvLSW#G<gbF6v&PUI~O0RUCz<?GB&WAcFYX3A$0I}YLLc#c8$TrEVTF^tIT
z&8deZ#czVRnA>lW!}Ub-!1`4ySM4}tp25$|2L94+n=lgZrl=rn0Z3om*9<?5j04qh
zo<&cN=`rVDt(NV60IQvb$Yh+hNrUc&%kAa;?onLc=Bo=HVO7Y9qW{nYl@R1lnFQur
zR8`icBR0bjBos$kVig5BI1)*!L7Zfu??w#fUt^kZofKZL#6hxpfjf)Uxxm_w+)TjQ
zhi9f1aFLLzK<p<v0cC>t&WojGqblnV7WwI?zi;1IRz0~rSY~FzDR*+aLe_3ZtUbrG
z{e)ZY><duP0Qg)Btq~9R^;d7ZL(&UB1MTTzgk&u-zgl}(h%xmo(1LqbOo4x92czGT
zVapv(b5e)8W(9ueH!`FL7#IHBeBCS|E5zcG&3##PwtKbivsO%myMi1FWJdJ2q7ls;
z+-qHs`i1dPfWIbsU?n4d^D@z5w8bn^&eHO8)H49nJQD_SsN#sM?hi9`nCjvQP=SX7
zBQ<FdSvTs#c+Xf_?V>HUo#rcWWIICY;`kpkopGmG14C6x0^)ut(lqiXX`i}RM6^FZ
zJ^dTb21iz7^OP~>DVU$XN(SK@k$o8cXy&H9R$}A(+{DI1F&@tcugM47zxfBhkq?;P
zxVILDp#^m1GbZ^J?77CMG8c|CW(~sAjq>@2A|CT#R%-q_^Kd)T{SF|@!ZCXTrYJ<g
zBa;yhE@kfH(n{R)NV?y(2DV$7;rM86l9-za2U9p&%FA1@O^&UgABi-&!4Q+0ezVcg
z%M_Ne(_xE6z|dZGX$MZ51T=2NffVH6zj1o}sHey8NNVCcwEMHw<DK$sSDYSqOOKz@
zJ>DsudL(|2ACQ;NOONl@J^r2Uv3~Q?H|X)zPLDtL^!PsM-75wmxb{aN!Ekp*S5G8x
z8}N6EYT&CYCAagJI+yZV$>4lgXK)5b->QzsDn_Z+ABjQa3Ps4S)cyRI?q{ovV3mr2
zY>6@YIQ4r)JwH+1wydWQ4k&^@QKEkyX00F2*hW>Gm-)pPD>3{#GzUK?zY6#4FbrQp
z0`mFxlBapNojPwP43NjFFY<Kiyu!@lK>GCQ)ACF5rUz!EXXRv04`k)d$V@NFnwFK5
zRb1v{iK49Bf}G4iQE_^4Nl~C+tS>M(@7zU=`RPTOMX3{_lD4IdZw`hto0=M$W`~+X
z^Fy_v@Uo(CdAOxHGp{I+dQQw!X`-YkEAOH}Vdh08Iq8L2Md`&^`FXzD^5(Fwp)uH0
z9&TtFULRaCeBLF#qT&ELn>iyZFLS!@qP&vf8KXxJAD=uidHnD(b>WtJh<(BOU{k09
zx8+S0HB*s~dCWO@Sv`DA{y6+UZTOg~P<?HvJ~*bTrM@B@YN&6<qk^mq{5FTnD;ADv
zY-$JxE6}KSr03{laT6+PgXK+`i+zFSnuezE{FW*WqnTMViUZR#i!%#zv+|_>7C?Q0
zto%T2L&ZXf)P!bUeqd&1QBhWUUZAKTGb3w876uXr%&GElPI_S;LF10u1N=!&e!KFb
z%);!Vu>p2C+n$?APU^l%)xoee2_YXQ7!w-^%x?+RhC}s%mii^3`pQ64u(_o!D80p?
z0>S!9Uu~#4oL*aN&2Y1HXjVZWJ*O};eR>%%C960qJtwOO1Mv3BY4%*8LNRhwXNEZS
z<DBR6Olq5%pPOHtUr+|*$}F$}MqrfWrB4IS73Tvf(x(TeW#(nf%uO%Mj^B_oe14if
z8;C4l&&evD8OX`XD=7`+=jCW*&MYj<FZ6(uUt|HHN-+*u40bOfAn5=)bv!_s*HSk>
z*p%MXRK6^;pdh4YZ%%rF)Z=`QX<yP53I_$0_3}xY6m`dzo|m3eR+Jf-RZy6Z;pX~)
znUz`f&A}#(E17e$qPkj8n3Y$YlQ}yxCy+5SJ?|odzN)soy4eeum_?a7A!;g#zeiZa
zlfpGk4NJ1>s~Tj{rZxq`Elu^a%WGSLnFZMuMdJeaS9gT<Ow_Od5h$MJ1I&aZWHhuO
z{f`&2)~Y&(P@OS5vRl@>jI_*<FM!3oFxceaU{QWf35f)$QeiQmP*jqUfn_zLBnKE%
zTvC|l1M;;r0)1yR1%m?j!ptH}wkC7l;k)fDmU5aL+E|{RQJgh9)2bjhe|DyqTFPU<
zsH8A6C(9miMp*`s?jm^y>Y9<CS5#b>j(M{SM3Pvbo&5k9Us6zzSty@6%S%%ed!R^r
zIU`gXBq5ZZSkxmgzc^4vTqdo~&CDZAh}#%)Nr5hD4_%}DY^>brf%L+QN?5wc0as4L
zl3<h50lUjlq&ZNciAJF&8t6;rtP)gcaSZSDm*iR9%)|8fDjFJ>N#&UZMS<KB4k9y9
zkYAKlY{7VDezA-c6PpiEMDwG--1_o5T|pGofo2?-2ikFFump=OawvfrAO|@G@sM%?
zky3E7u|XpqXg9tVT9r4as4xx^rH-@FPT&ZH?u{KXe`!ODtOkc*J7i{3d1a-hGQj=j
znow0Zh*@76tQ7j(T)tRS6%IJRpg1cxJtyF-h#BPOM30>OIY6c6CFPB}s%BjiS0mP%
z{!=PI%b!*Z{3M^5lV6yPzwsA}%TUIL>O*04O)w0kyC`dJCI&<{UtHHHkK<v!7hthO
zYOGKlAr${1+2N!R<H<Mj^D=#Ig?5)B7O#f{S~OsyJ$urD2@c1yn5Qd^R0eB<;UEPP
zviJFr8d@6hr&&`Pivox|kinH0+tPTh1dC}CiU1dTt*H%$ARV^r0p-finT>iHYnC;K
zD#~kXgZ0(n8a=2SumXp~6j?Qh@YK>AtOP>|E+rD?w}cy8!a_ROUP;09^y19i^y0!S
z&`p~(1gbz<>KnZMb%l)7w5DKP!{VTWn4VF&rEJvjytIJ}#1kaOs*oXDa>R59eBgeX
zUb)Osh78uo&NwnnEY^%etuAx1s`dhel+l&60s*3KsJu3Gc`(ojhRa1I0C6OaIfYrp
znSmLQhUG<JCaHb|Z*S(unnk-UL9)X%YHnePfjN*qNE<ap_Chs{-M*-{VSc$SUd=48
zudEGHvV`<m9uE3I9p;11&nOLqDwhhHH3iEnT_lS;gMsuSfK0k#p-dpq*c57jAfoB6
z2fpdEDR}@5(vJ+{G;-%T<xTaW`sx_*9Rw~6BojyxWULPSj9|gg!C9h!n_rNbr(qkY
z4=#-$e-@g!B($Uv!oM}6QB2kgOZ&UAvUo*I*Ai#!`sGB&aF|FA<-yxnWTPN$AzNk8
zl8a+$Rh-V9qbC(4vzK`nFhvsqds$9vXn@vHPq{fx-H!Ad2vpT+tOTPH9xM_t_-Wqs
z^up<65wk7fB2%#4MP7#j5JE*%X2=a<WhHl=yNZx2NZK<ofn<VmS8;=0@S*_3P6uE?
zAu}4O;*9ZJ6lH}LHef}ve-YF?<y+0jo?_(&=P?IpyBG9#xVx5Y^(0raqcTwr*>b^_
zI@Xo(Ee5JdOMVG>`3yazB5OI7TDP{4f{tk7E4qhfZv-I3<A%y0J0mE<IgBI%d;*y?
zGrcf}dfW6&titJBoHoCcrRJMa=+GK1Z~H)UYC-A9<b?V(hT|#48EGC)=ICmH^lAAb
zn|s-Y#&e7EYb=~@Q9Mh+VPCWIw0JrL^fcc#2n1&2q-&|#S;Cgh0*sUBPP}nI9<zAU
zsJhAGjm9?%sXVfermZ&q>mIopjV-Qg6UmVXhO;zX(j)+|EdZPfm@>d`d{IkAic!*>
zo>iC$rUF&R8f?bQ{G809Y^d<=!iyQK5K#|X4EewXpnHKE*}aFdp(R94uGUl?0Y->N
zbbACtNnUnd{+ztPw33Uw?b{rl!lo?K5L7m|d#>P^3SE*T6-hU<04k)rG_V}l6_IYG
zCzvlSc=^7)wJj$IP)4m=RzQeU6@~Gq8ojNoEzoia2M@ajmypdvJ$PjeRcow;KpijA
zxdSD^(zXx6JVL~YJFAxZqJ<5YE_+eXQa$49O`!PGv(hihheg4)fM}$mhyy}QHJAjX
zL_~jwP=Mz|$~KnTtnt*=#w=WLr3(v4V*IgfbW?rJ^@3(qq$|CVO!i?$gzn^`Kx(Q-
zUCHbXi6%ljijP1vNyM@lA}-*QSqiZX76UIAv^4kWg`#ZAnC<Bo=2B#vJ@d-4U^8nh
zuT0f{()6Ek`p<a%XM!flmS7;YX?s&vo<~N9v1fWo257IP*9o_nT@;v|1+y!JPJ0o_
z_bw_xv@guml3hkR#bq0VeSyW{a)^q7hN>#SJrW@ul`@qQXAVrNmiVr5-9k)nD|)JZ
z(`c^9Dbg}<qzO1TEm{%sEqB_HUb{TNwaaL5L}~4L`GS1%POd~?hOH#Cun1(&i~ib@
zkzP^?vYTF5hSh2HG>SZPy*VPCRIC?OU`Annu1CQNHRlFt$McCrR|-OcpGmm_{A7B*
z%k*dq^-!}IVw*;#Kb{~XqG2y4??@^T*K*8(=}~)FlV{K{IXRl4lm)xU$;`V5R?oQg
ziN$(2*#Jw6DKP-nQy5{c<|{2%dGwj8+EAmmKh!qVS4$NUWY3xXi8fR%*~Vscwk^gn
zzECL+&|(m(a5XaAHPi>72n8yGI98@btn-1f8ateh+Lkc()Ww;M{KCSL0#UYEFYN6V
zH46h(<)PXm&RuGn7zr$56)Vesb8}}DrrWwZq3Fz#2a7B|GCj?bK0u%@%E_Mw@|_Dq
zS^7np0Z4=eCB>qLVlfsMmSmECXBA=FLCb#xfN1cgT4c8u1WH9KDH?=%Gx9z5WbX{d
z;Ixnn*JcpS=bQ<ba&Dg-dpMS7<RWyrDRvq_rR+{d3uC@|F_r7>kLg*n1JlX^b2AIE
z99?-LrneKMPIQtq&b5uW*a2AP&(IWIK#>lsiACTomEBXlH8m52KvOCKh60<KC?&W8
zrdJRsiEp}|Alwk$TCTMTX<Dynm*U#9#@V!&3z4R%KwCs?d5_uw0bv({Z0<->Uf@{$
zN=*Zl9Rs)qjXR&ZCzj<7OeIfjurzebUX>5*XlB-Qs0dP!HbyA)FjDzpvc=M+0VGUq
zG;$4QmS$vTPKSV!UK-GZ$S3rorM@{-T_3E}grlLh(q>i5qjnE{RkaP}f*X0WeKgec
z3h`-#I60+YqIdij&<nCdAuL{>wNZ%@mE|4*&w{#R4TxyS6-cP?YlO0p%VD5CXHXS2
z<=S8##U&vrG&}o)5qqe)5E~q!n<|wp(7+zmgiX&VyQ9yh41wmB`GKr}M>P$R8ptfT
zh8@{4bIey3fr!$iWUp7gn~@1Cv2E5a3W#0XOA@upU@a1|R%e64Q7Bz?6427~A;=sl
zJaUj)_VCV@3u>=L)Uz04*1D)RE+%IdOe8F2yaD2sWtf{)kUz~&5kl4ycR?bi*t>dX
zlwxwdOWW-f=id^T#B^f~fC!FF9m)z8o^!yC0L~`80y1C56+-N_?-{SvvFT%p-`rvz
z?jV}Yxiix9G*_{<%QFL6ROap7m)O@u1?do{ykNJ9gtpYU8Um@^5$LSNEqk?meI2*N
zwxPFvS6!OqEae!bFa)7G6tTbQ{mfPok7V%^id$47=un(rTvh<<uDbxhm|T_EC1}nz
zH^}bv%pz#vFrbMGgm(lM96d&F#{l6X(L{S$d<}Hd&H>5pMQ!R$l*AX7jtmY}4nr&v
zcG_%Tm1q^X(PEps-4ChDQqm|OaV$=cW0sm&spAT^STC~e3^m9_mx;DU!cAg!M08G1
z0Sj7_9C4BWsc2~mR5aAK)M+C!mzM0~fQ)6#v~~qtMsDvJ7G~xbPR}f)6p1x|&UhNB
zTI#vufi8g_(1*b>XISRK_&m2TAG7MQb^@aDhU}XO;s-7XgALkT91cSnY^sZZ-$eU(
zKrs%aXJ7?mpD0`&0M)K&h$J!R<d(zmurzgSxT(Cpxj0l8YzEXK?IBn#p~`~nO8itt
zuFG?Z05A&6>GkEc*b-{apcpw_8k;{Y5Ne(qY-$i3fR5A9w9HpmzBD_#cyXwqR%C2!
zF61@H^jPn8KbOuZ^)1&O6FkcBEst&}V!+lQCWODf`_#-+J$7uNMlR-^8*`9iMQtYd
zH%4y3a9MiBw`c}R!J(Te(PNVnF(Vd444hNBmUAP4sVP>GJ^7i15!#{!Bkt%HH%t$e
zS7%mwP$`gCl9L1SoDm5R8?ah_<ZAM)qSRPmId(=*F$Wvd(kii6*;EIYr=oDMaeAl;
z&P~7_;9F%;TEO`iGey9^g36-t0sSvx=AFuAlbo3DX*o3pY(URg|D@Pry+YS%&H}EB
zI5AD7rDBF=mp6sV>nnm84U4TSnIyLwj0>y?CG#4vh-2pz3xveWS{+`Tu%?DfJflHy
zG;+}rW8;(`U)54u>#orZdyR&gi4%emmTfiSYgn_E)zt+-Dk}84oJg0tdb*%RQwy$^
zP!yrCIy;`+0FB$<`YKx*Yq5_U4x)BxTQn?|A}nH|=c{BVy;Gd(F4$%YN$EhT#cYFQ
z(h*1GhBj_rQ%ijWuZW#|2Z!r}w!JbA_+!886=^2fD{`7(&0+}J$x+a>X$$Bc#!%R>
zBm(%Jsm3dkqu8l-hHh206sAo)18VrH1M>s((*v{93$1sxv24KYV|;^h>~N2S-;-P-
zq!E01^msWTMVYynAaF64b6X=#P4lAt*DY!>UD&qY5hLS)E_JL1D_5Ci&UKcq-R3#b
z^Nn_wyVzx&tLuZcz7Bg)(_8T4g0uwyCTrFFtJb=Uv;HC&t%s}X534nQwhS<qniZEl
zI4g(hj3($215foX3|9{D_9$je1MHslsKM@l$;A-yJ)9YAZU$+StpyG{9$}B@kOk&R
zP7_h}TkkC%?tzRub1*P-?ZnQBcT`qA<4GwX*5T_saBOdhe-oG8)vIyY6_iL!QuzZl
z+9SSNJ>brZ#<z&K5fkHbEl6_nYXcZwK<pX#Hhq3Gd+!0XHGWGlLX)<`mepeFgeY+;
z3dHUdIxb}*F-7#xEu<+yv`%WZb&h5jXaPpfrZDSaHFl3&3Xn==0`{MlrC=E4P0MB#
zMDu$veqVhc4CydX<6>1rZO$Cft9z#K&=~BE&?_AdJF%uAEIo{FwGcc?2&ttEXg(G(
zJDEOEO89$wyAn~rqi)#)kK1sCBirEO8#U|!rO>X70-W1jN4{3w0vH7=-9-?klG>n(
z2X{(V(TrSxX-%rY=$t^2{DWPux}_FkOLNe|!3a3O39B3)1`Ty|<;n!V7b{ovTzfxp
zal$hx@w)_h8!zl$Iiepf2tDrVrSoT|w|WQi4g>c9(QR8al$qjS2F@5YyKXU07bB(9
zZ!~@1-afHb5bQ}6&A`TJoC=R^LG6$icc|Vm{QD@#+6xLZVX}cz>e=p+W<B>-56JWA
zwHJ3v(zH)h@vN$H#xsPPMf(FbS2WbY8!!mwx&X=m(7Fa5M$HSC`Jk$DZeXMJV!Fd;
zBB)|KvV&<3iMLOxr)VbhNE?(g4EQ+1?-r2~7ugX2?7qYE9F$5+g*~`(R>5Kk$7puZ
z9!}^U%|#dl1bY3C7~0YpHD>(~?%-MV00l%<G+4hx^mm(txJVdRU2zFC<y~!b(S+Wv
z!0k3{nepL^4{V8v5iM8=GIyMF8xpwIxD~yotDDLzLqY7`QRtD+B1_C^Xj%wYh9bQM
zY<J39%p$BsmY-EHAO852z0ff)5=W?s8t_67>ct5<N@}tvTGotxW7(+K^Z#t=d(qpS
ziPUprh^0%atv#2PW&oworm5Qe!9^3*4;xOjyJ8Ute8GmImzIkp4!Psx!5-^1_*P(X
z^~#xLD!hcl2GX>&vGEpKFGxm~oF2hNxN5CId#ZP^vey}vxdcvI%+5D68a7T965SOc
zRM<#~PI?(r#B%n`*Q7YJfluSEa_^KoJ@5`9f(FPS;te~^+6Z$6A&vH0@Hm1LLKYcg
zva*+z)1hdPxU2C0tpOL&zVVPFWF%V-aVcIT9=Y|)R)Ys}?wej)_Rs+F9LuT?!_yN?
zD13RKvRO-|*vDlY-VCUI*wQP^pCi771>E1Bo>h{I?Y(fY8txbMEP(hV7;EaW=?@!r
zxS939UIh--yab*AzS;7v(-b6eX$^n1P(Bkf5vuYGV%*q@hWf<_q>9>0D{jyZUt&*>
zaTO}4@KsqJOqEbHwNaxOtPw`qil7gs>gna-a%@NhD;Uz}!)Rt^T{P2I4o_t`i8L?5
zW*ma05GiHvbcG`PTVYm$9$W92>|G8@=lV#wj#fX^Uz|RTL6JVvlf~t=>_ZcnKtSkA
zMt+X=*z{CH9iu2z*H{Z{pA&Z(5yBOV*!3{Ld!d(lPBg4N+icuf6`?b{H~@otg%65(
zb*LUXMgcUEf1?^pcEyOq3fCYJ3ef~NZGttC0=S&u;$UsW9FBFLXJYyQvFVxcP87!$
z80xL*l)a89z+e~Kg_6%vp|A@QiB_54w%r*jMQyWfY_<uCvu!O<2|<*JHv5iq=7h*k
zoq@9uJDHwU;Hea|W?>X|`=PT%a_zWXipEJj>s8Ga<@HsNAdj@Xd{q#0>LYN0?zh;3
zK*(5#YYSo`?a!%!xc>?*kpo(86CzR~DvAS#uLVTf*mz`yeW*m(EdGVazH0UA#!9=q
zp+4xVM-U_t3U0Er2yn$UR8^KQ)4sGB2;gZ7RoB4S04Jsh`8#c_5Rs@7puhDQ@D<1_
zht6IJIzm)OI9tT05i<sA1UiGDqQ%YJl8S&r3^-$<=>XA#5ozaG6X)5BiC9k&91jbs
z&Z4zg>|E=SljsWs8S1UQ-^F($z=7Gw5vL9B7(8PY+@wPoW&uJuEO2P70m3Qw^*qEa
zX6^+i<E#OpkNCJs7L70&_PvM{+J?QiX2)v8WrfW)Cp@|eqFe=LyrI&Ev7&=f@0#tw
zx^&2fr?0uOA>33I0IS2~Ra!SN^)-Ck6eTAeTS(c3!N%J1ieMesgQhOEmF3Y3y_b?6
z<;&e&PaRuR-kjH9hvC%gD=*aCvhh_Whxw+8<x4!f@~gyn$vKM-3HQvjTeB9Iwe`Mb
zKGoIYh?U@$bR4!I8C%`<xg&_*ZzqhUzClO6MiEA51U9{PlvKA}3r=>#Clm`<z7Pr-
zzeP6`Ncv{A>f*RtZ>(Tuk3W@qeV*>I_F42SwM^g}g8mrt9lr<E(R+Opv6JWa#p|7;
z9nT=&(R<eI2-lYud-N24+ejWc6@aqUgSc2@K}h3nEPYt+ROYLOF`zz9RgR0JARw&$
zM=Ar5g1QSt%9$5HPeH2w6S48-x`oIze9IOqr_+M2p&|(#jAZrEMI3ihFC3jhao2Tf
zD)yD82a%`%qEfFr3#qAgk!VK;&n|<f(bTjexKv1;_+Waf)>oYM1`Dd^H8$iN_JQ4T
zO&&b&z4#r`k%*0WZ|l+3%anQxq+!rb7o>a3om+I6ExMhJ7)$RUiYPXL5=L{T2Rc|$
za5nG-mgd0G#U&9j#^hLFMZzBS0APAoM3#Op9kqH8z0nC_)a_L4V)FJZVnD=j5%Z!t
z@0llbD}JfiiV8vy6{B|7fis>-`39Tg+!3|WGIGYE=ilD3_l_>wh#yU%BT+bYd<Ddr
zaF9Fvl;5HeI%0_K#*Vi^?=(tdq2|m*ke(rgM2p4I{TH0zu3zLZBLEhOm@$h8ljEzb
z4%XF0c<se3M43Q)cFJoTYan%q2fwv0ZFt8llISkmn}>jai=bXw^9?3A;@jv3dfNpi
zRyb=p5?<aJ_z#yyJ-h)#7;py_G4-Bd+uC3xB1O=wZ<%9=0i`17=%d~~dVq4oJ?hBb
zI-+!hkP%rXBbB<ByP%zMd%+!TD2=Sbc53l1mk6*P`E%(3RZw53sbNXf^hD!L{KXm3
z8tRJ>onltW*HHuS$Rk1Moe~e}j6miilI#fb>upD91guK7!PjlcTk7k4ot(*w2Ad0G
zP^<Fkp@{947yt=<Tb6;H6S3sEeTlC2NM0q8IP!HpciZq%nIq$RG-N;|<GTj+j+oqI
zh@evum=rOuF5iVVN$Ci&C@}sV;d7gBMe?2q6o>|>#@%|1BtmD25c6VKU34W8L@ILZ
z9y+3#7|e}PD=q@6j*mkGkfIxmfYaCFzh|mF{K!Lk;}`G>nH~Ym+g*ncMatQ|6V}L&
zwO`;7N9rL5+PWM${n3Dl;OZ55Es<WzG3)Ngi)0x@PZ$*7B{Dw<%MMI4O_AQH5x8p$
z%`PybJNF;*P_IM~(PA88&?`N1X39I09)U1csHX(v#pUSH2!-?TF?ZbFN&ZhUHDaKg
zre0}HU9heKnYALYDB|l#Ja)SoJ)l?}q$vi0yhDpEAJGSI(Fn9W5>SP4FZZm{$Z<t?
z`G|n=w&Qipb9QfgE09V*V%u2&^jgQ-Y2Sh32E8*B*)MPP>{DJQ{i5|n1QK}*raB43
zY%?1_C=t&&2ah8@m;j4QOJjz!S=L+ROK$_i64k!gR{Xg;l(_B5iq1-h!!olFc^8h`
zh33eJj$R76hA$2=E6O?0J4S&X@=`?#RNfN8MmQz*7M(p)y-$e{Q4Hh=wSHatFKlwz
zz9a5-k+)lEW8?J+*0yh1X!n3#3_VujnXj2j$39vQF!VZ?7NzJN_Eh)mlr0X}F{RFR
zZ&;$CV;8d*(Z7qoj>XpFBy6R&3nDGV*UXT+-fVMiZ@u+eJbNo6luEEU+Sk>K6@tzs
zM7*e_8*QAAo>uJ{onE=1CDN38ZhoWWt(E3sbu0`bsgT(H8Q{?je$9LkVkkhavmhQc
zHj|2LXJK)HaW}QmCu9d<uH7ipx7COsSY=Z~W9B0C54#*XnV4>)u)MC3=~MV#Tu-9j
z&wzA;w2;`tZ<aDy^})zm(6PB0`n?Cp_5i(efH6dQ@rWOXI}pS;(L3DVg6u9bsN8}K
zeK9>)Ro+q?w%}s>x&dlH7RM_NZ<|}1#0vqI^I(02uv42R+C3tPC~iQ_m`yZ@;lg=A
znp|$d3pQU#FlU2C=Ew5r6wvEAvP#V;Z?ar=q~gLLoOPM@4Wk7E<ewSHy&y2w5O+VY
zYS6p<RwZ#@7?0E0qj7jaobM!n93Ji(c9B%m{$5s7af97!6sm4&SmJguPPM{M^iZY)
zX<!!%mLpL#ybrw;$!=hhgPo{b<{NF5FV$XL779kXcExEj_1u`L)x&YbAiFt-_Sqdo
z=FN--I9y1;l!ZSI!hXv{-Mikfp37@(3<45lVd(;2i4XR2ve3c8j@bE+iRKh)foy%)
z#`MhrR&w{oli4>%_KTu)82CJwR|h3GA(pw2f@;ZH7?ZZa1NWGg9W=u(3f4vg#uk?3
zf1c*OvmdX5NPZNZb{s84#DP~VEf2u*x)d1%^hooo=F}h=GjwR!x~y$$cMM<GIBnjs
zd0e#Tf-j&QSHvR;k>1!MCPgfSlxF2j7RY8<5NGt3ec6Cio^B^BA4!PvK>ElYdEms#
zkO(AB>c|KemdJ@qoe<R>IDo)Wt07P$StoI0X9Euebo=5klWBEyi3fBGgUgr>H-G{@
zjMgggHM8%|O)roX-(6o|NpzM;pPi#!{3O2R1s9ckLa^IAkaL?GY8MCd;U^1jP{WLP
zQBEr{TOkZHV33$iPclRB!LS|soeMhfX0%}OwbXknkC<{eY?W8yEqxW>>Qhk|avYuu
z;Im=}WyW3r9kp=at<K|t$(>`1(m)UJ_7y>Xbr7VY$Ly`g@p{w|7z`GGIUzDak&a16
zj3TI=&%s9k+8!+SzmO^fs!i08y`|l;c-<2%e<AS#bw^0{4;EdI93Jo+ql=FXQ`YE^
zf455UQ=k-K2Y(^L!dJey+IEfomN5_$jLrp{AczFN8T!d~TCB^|e~O6_vk)`&+`WtM
zr4`d&Bq|jW@6QeBfAJ?zL<e_n#EZe}%jat>M}}j7UNVx{Gv^>kjIbIF$z?!tU&H(b
z{4+B^<H%B8_A>2M4z(=jop=a%(z^E}X9>hb6;$KJp7=xmn#H5nv|utiU}GLGXNaUE
zM{4-2$2p-ousFE0b4jr@tmn0&2_S$7DSt!FdRq%5$0~sk2xNG#lyM<;5^%w1WIbfd
z@{JcKCPdLFuFIIY|Ati0>T>kr|3*z53B9UWBSR4?@Hb{2y;G3-?^E<6H4GoMnO2k=
zv>Qpjfbicyac+UH33^e!PRA#!Dog7GI-9kvQ+UWuDhO3eqeJf9fQ#M`b}Vy_OVtP!
z%C3x^6UOfQmNCdds!m66cXN#oi2@m0rqkzmLF4wt!cI^0m7t;)L9kcU60{YlUiv2L
zt{mntpMFr$E7gg1unArqGMqM{VSX5%;7;^ngjYSG6B$AX2Zi{9p|PxF8X@GfVluGR
z6|6+gZwV)bB*UEg@mT>ACVB{znDTOUh7~Fy3?iD@P}dNKAE-_d<(prD)fc}b(P09!
z3Otb$xq%SU5bBtbKp5%2d#S{V!o*-2$<(M6XidmFf^$V-3^qw(bYa9=HvlISe(kkP
zWCC!69aoRla6PpSUsHWlZW9+jQpbjxr_uWzQf_7?{zJZts`94bl3;DE4}W!QG-Zey
zE!n(W?Ig}*r>2GI-fF3V(9FJNCL4)x)5MY)2xE{QLgx-IKKg6nD+1}_f1XB1$N1<$
zEY$TNci4P}UIF(Ad1N`SLJ$P*dc}I5?soh(Q^#91SPG+7ZeyuTG!8~ADo_wR7AA(6
zFdi3n5<2<UYI5SZ&(W9Dx7<Rx_}%L@04KH`FCM#7!g#y<`3n$L(c8!zn;@3EC+?7a
zwJ}TB<_sK_<8vt|yvu`)6cSK%{Jz`yiUonSr82@n+WVEg@En7SH7m6Dc+0s1OP%P%
zdrgO&|Nk-fE?{yNRsMg6#1LRrL`0MTF-8PMNy1G;Ny0^fg7kHm877lV0uqo*AY8&F
z89)?V1wo>Kx^xspRK$RwAgmw)in@ZXE~p5oh`5SG0Yw+&cj|r5=Tvo7b<ae1|IeR?
zC-1jToqL_Syl=nV({Mis>#KxsHP+KSb*xO4V-Ju=^Zw&r{~}AV_;oWaohL2p7)LeT
zMPOI?eW_!Q&<E}f`H9IcKk6RLDl%fjlONw%yIK9pDa1VtbRVe8{}Q-=1GVsO_l0h~
zsUIm{q-napQB?fSrM%>Ovfq^4x>`BnAbDT=up{5~uEXWaGE4MNR&)!jSWM-su;PV#
zTH0A;#1KA;>=MRUdvf{i+Sv25R+WwYBzGWVM55GJ_AY)pHG0F?*11`GWQsvvE9?^g
zE}ff+9Qs|sCEtybuQ$0{!)`|yx6>ZDx3eP0qVOTziY1NCAC33*Uwl0|nV{th&A}s&
z%p1<-$%iL@hs#%li=U`*^H}~2K)5G&8LOT38@l_uw||$*J|;ITd>5s%ocFA|WK6!2
zRD8XcgOF5=Em887URx?1QE$fiff6~96L9BuwtZHLC#1nF@;&->Zaqu}*25qvSa(Y!
zuiP}ta?b3#ZdoliX8BoczxvBv@)2FvLv~01)k}9cbQ{}lokz0Dsq$r1m%fbQS-YOJ
zOrC2TZqJGLbKhi=A5L<+V|kLxN$rt2TrfcvyW>Nb(Rx!_EGj1Uocpqx=+Cs3n_HCM
zE^225%e&WCZt#WTV%<)r5nsJ$^vbiB+=F#7Q=Iwy&vTS>A=u;ONuK|_)t8EGLPy%k
z%1)T>J}E0?&uL{Y7r*bH8-`i(WyhmWwRuw<0A=b_R=9f&tvUG}SNYIFo-fNcsckl7
zP3p4Y4(O+3zrXH73Lp1q{S|j6U5d>8^pMUznF7m~%3n?LpZIiq=gOOyJzEEL-xE5X
z{ELFak6Cu&sd|nr7EES#S<5z@s-W%;=58wsGtJa=pZQD41e4B{ziW4Vg?GQkt_$V=
z;uV6yP>*+bmivH&8^`R=BXp5-cL(A5uVZt-&vLdObyVe`g-3O~^y3}S1)<zA(g%V5
zSw@LEg037}eqb%@Ko>7_zhM((t+Mdg@-GQw>QsJ-yy5n%!({9ZT6nT-3{v*2IR1x|
z?A%q6_Z#YmAG5b!J3b%x;m#^w5$HTN>F2uT-8+LI^W6Qh!&SB`+w_yK&&aK<`;5Tt
z1?3;3a<`#YIps&H&CXl(qUOrSnC@!=)x%Hlw}0+wL-!>+_)B9!vAB$9(l%>$Slp63
z$y)IXb=lcF_D>5IH64CAiHFzjJJT#9`Oz<56+CQ3`N34)+nqa7DDGz6bZhATZjII#
z(*2lZ+3nZFNBGSEo%hkUl{e=8Iz@21kZb@+M#>#PM_=kdf0B%Ph?b%*<2L3mrEoVP
zMHfCh+;7E_@_9%$6S5&>e$1u(%%^xENIp_?A0oQ9UW%CAucDP8tFL_i7XB1fx&Ib*
zX%Bn+`L`n9hjAZ3$_diV7kSLE*xfgG%Yi&d$i?PHzWf2zT*2=aZw;L*a~E5|J*lg!
zj@49uzDoZ1zzLPZS1g$)Dbedb?tyHvDiu!%XBPj?vX3Z>o9QB(5^b5rVmA7FlP)F4
zPZPOF%M(a>EDP64d4k{`$921DC_Wu7fBmO$JUf1~b}7zMx@y@tcC6~fo_yr;mObK-
zDSDW^LFyhyXO(u@*E^JQYATN%UB;yNj_IH*0xR_FnciyU&#!sE^x0u~-;}GISwWOy
zRff9QK+5Yr6?sOuOxE?LtkyE@JTi0pOHmQ!wc9?^`9RpM8M)8d%1zasS`Xa2__<#_
z7{<?B_dal5BmQ>q^74y4@~3Iv*_c<^dq4Rzfd9)6-f~vw9ax<|uTmJ@`+wV>K851B
zGNaFF#!k0E6$X8B%*k{V(;|8?s>99_dl6IaKZ@0<<31!X(6Kq$$4KkFUC8_n9W_P8
z&7psTOxBFU<PWHnzuA}mVsLTWU`1_8X8C6r^2Z@EpDT1e!!S9^t*rm8z2%Qs%M+=N
zPYe!U^6mo<$ooB8WW3+<=d#^vSO*@^?fFu%0_{`&3d(+R-|zkwMbmu*=I=C$@BG)6
zxwG8?^2;>xCVug9hWyAUzc8cOshl#he4+bg-Xn`&2xw#-WX;f()$Z_$o4oR-HU2yz
zJ<7{t0r}`aB6XPw>dZ1Ydsb%}mdJ0+NXq0@%hTL%d1pmtm$`c9amP9y$;rvGzMpV-
z`n=@Gx_h|c?smJ}l<E&+6)V5|t&U?x+#ps?bw9!*zf`PS5uZ@V4&`29@cGU;r$0~S
zKC37nZ&#0&Q*Xy@QKtOduUhz8SYAAs=)5N?o`kDM^W4ytUb+OO`ex70mx7e`5<O?l
z%MZt_F>ddz3c95<^VBw%CVr-glV-7}2HSS#t}t5*<vQh`1Xx{-af8<DO^wVgTh5)b
zO7u{ep3lsa*`&0y67*TGd#6R8?H((4b<1Yy`<U{!olJ45$OBzPW>c7c{Lwj;SW&Xl
z<9eoMF^uu(%2kDYqGpe=*n$Fi_ol3W0w~}1F86kxIF>Jgr8BrVzGnlNwR1<b<5eh5
zy5-1EHhH0|T%=D}dfe>1rExwfb`f=aN+S0(%MT0QKr0U{^Yo2iKKWZ-nI}*5^Okpw
zR()4A9qVzA{PcYOSI*<|F67vSCoJvQylvS02N<32M{*-s965BUaZgo>*9^n0M>1WG
zY&JaEJ@bcyF}5C;$JXBSWCfI05JhwI<e3YP-D9_fekJVq>4WsUI!lVjrYj2ZraXf@
zPS*R%iX-<a-eHJ8J}Gtqd6(Qh2=isLy!brrq-Fd0H&b(yG`;f+Gtcwad3OJ3)tFP(
z`*X8WG55RWC*0|3>>jM;br(0IolkheV#`Xo8|BQMa@N3Z<^8fxq26PMPf5zpA}aFC
zQeLO#1@LlX-p8F`$|W)Rgv;wJhozV`&EhUoo(~m&0V!FM+%vWV_m*FhbdN`J8%Xf{
z$nE;&G3LO+4R@G1<#6{SSjG9Nzw@^x{PdAs{hjW{Os0Wb#=D+@#`EEMIlRu*biWV2
z<8z|a={)(y!!gT>oA2VY%lXCc^vNTZT%2P+N-ZtRzj@>SJ753H_fZ|IjQb-Eewd3d
znXb^Ij2z;!k;R|;`{yhAydW>t{aJV}eJoY}lOao2@kF4U;_`k}$5Q|+ky*P-Z4@ia
zm^T19>lCrvth=ME)M$=N{=BSzTHfW?%w-yMI}_!HT~2V(XA6?mF-~xur`#^Smdiwu
zlkO{orZuZ#_U=8lq~ZvCqWj&)c`Ftkl9ez1wqw^13H;FQW<%$1THJ0@Ia<C|<my#l
z5-*Q&<@%Ky9((SZ>x{;(bNAOZL7#)~Til(<$5HWvEOU8`Vs*=X^m%6P;5)y#{Sar<
zg=kif4=dH#@?qRUyA4^`@d}E$I;;Fnihl$s%V;_1GHxeT>=CtJ6PD|Jn`T{qxW32n
zqj=ChC~q#>96c>x5BU#2a--$uqnu3K>j3$C)VzUlJYLKUM6tds)>qQ1d_m2M(vJU7
z<Odslw)`hF;dAUxmR=FMZ4(Q@6A$;vXSY>ooNv-3$7r{Gwrt;(Mi9l4;?+aRx~K7s
zV~MLYtqFOYET6~9lUR8gto0$wO!>6NJ;e9Ym8LrjW{Z1$-_A^*hE(NLR+TAMIizvq
z^5zLgo_wTxabu+XuAA)7^1hiEFUwK>@K<pho%xCh75CqV^*W)_0&e=svTH)654f3;
zTEJH;X;S6M39UA*t+z5+;0z<Rfd6fzKHyVEY5`A~YPt$ngMaY;K=Zx{{HBqHfJEV3
zZMJC<$b`xekSIz<S~YWR2#D7uBdu4N!$Uw~C>etY3V22|2pd#pwr^0{oqBaNFs@Ii
zG;pqNBhvu(D`{e7`Ghu&qm6I)1ceNcXd|v`EY!v~6%w3q&DHo0u{axXM5(8JsqVg1
zcVDWzZ*1M>_c7jY;K@poER42k{2#>E%ZIZ=AJU-5lTgOlk6+(8I(~n%A?<si8uCM$
zWDi9{Ce;9<dtznPXgk&3NVO;9et~Me-D{7BZzt`-nk+AHuU{moqJ^oVg{jjEV~_Sw
zkM=RA$0G%I?>J&xNRe}yPCyhD=}#j`ay<?f1{>9q>vYz4EfFuZ5UCZKqRExf%JRze
zHf>*UNp?q7go2t-S;#muHbRQ?bf&H?_u3lM0{(Gp_JaoSZ$@eXU$jlh<yPEQo7P^_
zOcvNsQprf`YR?F~wu6z@cReHU-VR1uzwwN~zjZLus=Ug41a7FL(#LMO`jO?T2_!)>
z@IVqYq0$7BopN`gHF;b7=mXZ2R5H@~wPysL7DU!;(>lje&;*i3aS#{=+ib_d{#Jwh
zgI?Xf2;Rubpay)Qk|tJ;pU{4ZHg@)Wz}-W>J+*PLUY!l_nI<~~JlAB;n9!ziiSaG`
zH-(IVoHWSsr?pBaREB|HR8p}F(0I`E{aGPn;+z9BB9cwuxi&b%Kn6+t05TawkQu<B
z2`Me%WX<@w4#zL72CG2Ao>;l)+p7p1?FBI&`GOb^{AA4{gD5NK4oYHdgw(GjpZ-Rg
zoCcZru_dRGW}=a1qLF4IS(fpW%9~KB;U`5Vq&PZCIPg0y8}l1yCE#v~0`p|7hj}s>
znE!H8wPrp@TWlUhA5dwjF(A4pRz^qLsrI3CP=;dd|DO1iuw)7#Rh(oEDW>#<${<p!
ztwsiKSLov(j6Gbe9v*G+k4Fk_QY7Pwd2D8e8YvRoE!U~4Bx~bfVPpdz|FH^15jmG!
z1BqmE<>bm4a=f68%_%}3txTUawT<a3G|!VOrxg;kzv1%_4A!9sRuM2miACCeh9yJ%
zc~wmbi~r-0dX+BQZ?%=YajL=_Jn3w$Fx<<Gda9_7qSH-LJy!H`$=s^S^yO1lPntSq
z+KegFwwpcUqI*|Oo*ox4>Svm+VIZy-I}<k9wt^jucnZGQ5#ftX;`UgoVmVyJFG*G-
zO=-FjCbJ(!Z&K;WmF1gGminWOfhkJgWh!=#v@BJDM=R+CmE&DS(j-W9<JOQZFV$}G
zk$*`Y5RZx!<IGt3!=x{+WGL2yrHA909hdp!4;ugI_Nw{9Q#qcX`#DQxA8@jsI44$4
z9&OX`KKz4+oix5)K5Q2Hke(^VbGq78&;Q%ntw9So*GLWEJS9oc{cRfiDzUtUpml_o
zj;n8(c74oPTEIIybkX{rX9WJGgOS#Yr*mV`0&c6M(mPr^c}C#;4n|swJtOe?4#u&A
zYvzJ$w$tF6x!{^@g4@JgX#xpSj(tEPEN`S}z23_LiCbiW1YOF~8uqe4QX#UyFe~Z3
zRXXIgTvlqCtkiN@saaO0n0GZGLAx+&ZCcxVMj&xdsMLY;m8+Dc)#qh_VZ62EUUH?D
zCqYW0fQ)--fx(^PEdX)+|5ssjDQ3c;Fy#>d@WnndU>I4`28hBGSs<hG|4EIeW7cI^
zB3QzaPd0#IWG#!#htUF3S#EG!ZCXqw(FhFdtd-Q+ZJONsEy*q53tnBWg{1{rn<-T+
z01q`%A8@Ecmez@$F<$jjKbrV4(WDwcoN=x-+q4EfBM{3aBQ5Voa^5;qb$rZpwSadk
zsmuqhpL<5&YiF>D^Z{R|q*9jFzMe78N9sotKkhV(4Is{xVba>p)Hi@wE*WWgKgN-d
z8h&8L>8iD9{lvVh0e3JzhJaWuWodaolDojF>R4^MTEJVC<aD*#wC?bXz$ZEwX}#bz
zj8-cigw$vgqez#Dq~&!rV_lo7nb&)Bz&<6F0nl3R8G$EuFph1ZnX}MrW8pZn&;(*h
z)=OY$G~JieuxdG@H8Xm&8La`aB)cduG@5RXXR3}{OjiqdzmXci)^_1K7H4g(3hbm@
z@}weW`EtUNJY&2kV%fMd@!nTXDikG4Zp=HeGG$FyCW4q$xoGXGHZAXLGGUi#93QZF
zTEHuf)Bt|LNG;&iN-C?7*3XPFvG}ldJ!UK|;NwPW0RL#D7Vu9>DlNv@ue`P#zL)A%
zHXh&}M(P8;)krPiUP>y})7sZF0*~)t9NUj3e!R(aHGmkGvj`B&&W~oB)^T1I82XXU
zxHDDY5)*6z?>15c_<bc!s;pc)+NO1nQQeYB>sc=z2cH^kV)Q#E)dxgj87ZwFdPd+c
z13#K=T90`~;9oizY5mPJ0*RqC9fwibzO1E9^(wUle1nl1z@3y-7EU)^Q%tZ4e4~;2
zfT6B*OlPXVd=qQ|Pd8Enc&3udF{O2uQQer*y2gvg0i;Ho7(K(J`hX}b6HDtH&j?%{
z_|a_B`j}?~ey)R&*43U7NDTQgU8@3jnqUj~h>;q=M~##`(@GUJQS@t*>I0%E@6<XK
z*klJgTmV01qz3RxC6$FnYwGKeZ2+e!sbr*ej%SR=IQ656A0IbAhJaWuW1_XKsUHGj
zxn!i}{YbXZ*bn!~AdbAo9O(nT!ALdW&PsBU-~)`U4_Gr&4R}Z(2|nJ~`hcezsRkSk
zB*7mxwmu-~n^36&$({2S{JfV;GnSkv$POCi3395akt#~IXB6$=%>&7!d;&e%{;~E4
zZpPIFf5s=I#m#Z}grbAIPe4Yf@Cj9WSPAwGC_KiKVmDFI+f8($P&`McFXSYf=swAN
z4x~Z~&r|JvsrF>hQPJD;I9{Ya>U{#DXkumAs1>3gtHgl|HLD-B?6!cHD5+d~VjtFM
z@8`TA@bf(d*J|%gUJ&@To`RLv>$y;`?p&zw$~Tl<c~=p9vk5kVgDNOD0l)=;u4bDS
z7BY7WsNc!d*MR+|z7L2(=V~pZJ{h6;s<y6Ix59uAH<N?FBb79<a@vIUs5UMNWWirB
z*(&f$CcA7x`*+%SB9NW3W0~h2^(r4X0%t0zu)DW5mgv>_3~rlj6?lcoirqW4@qqEw
zfDap=yg>7~HukX`*MR#gsd&+bg!>UpUU-<HAs%9?8mUyWGR#$}gY_!A0dU9^El3qD
zh^Nw2jbxfdJOU)U8_Gw@JI`2INj@SaTlcB#hLtmJn=pA<cgBM)H)qUD%5P#th_QQ^
zAp7#DfZUU4A*FF9X?~rW-b1fjR8~~P(?3ty;RL--eR}g9wrSJ<R?FgI;5)_;rJpEY
zS(<p$RE!j;G!w`9Ue$fE*}$!nI^3iG67L?68r!0h7mZD>RTWQ~suu8nW`|1}ttmT~
zR9X9geM-u&eOP$bEHr^nsRdcmfS7T*nr&K}n$ac@qnYI}wOGUAAI)L|h>dbNq=gx&
zE1<WSOvH`eSSID=dUdM}ct@ql<OI%Cl5CL^+6QRk&_EWv+{*$_31x55#y0}lc{`Q9
z4C&SR0zOM=;tTM-N-BJLTpOFrEOj_p@D557S>PL#RLIWOM$Pj9S18G;1K(h5HQ-H3
zDi+P3Y2!)H2mDLOH*uDRt5+GYA%%<dTIgG<jgRZq`2xOPX%ZUnMw68xx?3B&SoKta
zD~(hG-fN_4TsqEZJOh8A^Wsk?)<+Tgic9w7`&Ej<mVFTTQLB)PZW*OAei{<dk9#5v
ze7(*zdFk+$(b&}uR0*Xm`2zmgykCGS?|ri0+@*E)kgrGJ?~T+C{F~`s7<;r_xlb{v
z5g-M=p)8%*jf)C<Ld&V^#swoKRP0f40c1BWB&k>_PU@B76jH28NO7F|Xa&~w+HK>a
zzfbulRF(h_8$*<SqFkSii;1*WZIr2$H>~0~U?k*8l{3~(ZpSmHq;`{KYD)EGCT6Tl
zxMD>w(t5&)Eh}eSHF@&!#W`S9-cxetf&2zNq>@MJwc9RB|FKrNOMv*-O*D4)N>#zC
zvZcK8SGst{8l0Z`^h@Q?5=$=9seG7io-N?vO3LrpsbDiFm_AcX1#3CM^wX7P>dy-A
zD)5S)uBL*`oM7r|Dp<=2j>pw{POzQ~$65{KTRukMRc|f#u>5c!_~KnkE_uEW?Ax;>
z$!dM?DjM%rqVNGRQ1HcdlIpLafA8He*$2c*d0e7}Az8YBkD42Oz~_}z%F^2WEoKDx
zppr^PT2FgMAc2*PV@FmeG8~tTTslev*X2_|AO;G)SpTneC?EGj3dGI^l@s3t#*FB?
zF&Qn#m@#_es%}QnI<a!%jgx5vCRQgV3eJp;)m?26%_fzTDzj#Yoz;`3w;^Klh2r_@
zN!AS+S6<|zb^RBumdo@ubR3DqA(Qx6JWj`5yO!1SfL`4Z3%pKgvIhX4Qc`hbU8v=K
zmR_AK_-ZTtDsZim3fb$m@q<7X{DhYUJ{ig`*uC`mB)vMj;P)y`e7;!WmjgEN&pq3t
z3O9UHsnux%Z=*EP_GN{?4cNe&?ZFhR0=H075$r3qF-Nb?1h~)30tZ9cyS4F9APaun
z%RaC06>s)YfU8QAC<Yat60m_U@N99F#MyO+!r3!Us7xo_7m-AHYFTvLtb}h_qH92M
zUn)7NG97}AM4?1F;$bKyPOMC4TwbSxAsu2?I^xWjqonFN%~L5M5xiQXexpTS1I{*5
zKaf<KClE-ic@k&UJR!|%RJ(Z+C;b_nYfLj&HDIby*22eD(fGRg+s97uHC^)yK34sW
zb-$FionxC@=_a|YZI)Fa3y-LJX4UDs6^+_1RRyy06nwG%?N#rNUOzC<zjoCJRqsV!
zKQPe0Hd%RRtKQwceqf;g(p4E{mnO;<tCDwlWxzn$RjbyjlQ(#0fq}DECHjw6y~lg~
zz(D_+RT*V#5@qvsqA^uvvR|*Y&ZH0$b@4ho3siv|6Qn9Al$TneRDlGPsU5<>cUP~m
z>y8%J#F-lY`z#GD;3t$c*&Wa(zj`TcOh}F0l1tAmNqYWOHC^b908zSOY2?BQ5CSW=
z<*eM6SXr&wFZEV{C{-(uO@<Iyxg%%gj>O7Vx-d-h`!f(_@+D}7+K=l(#?=pE@<d>s
z>U^8m4fMK)Q{BU<?%{Z#4^y2>y>6h_y&%=SAl02b61hNiuJO8o)9g4g1Y{YVP&qVq
za;?_<^;Y{W;0;Q0r{(m$+*Gic6HM==Qo&kIFuj*b1?xG%^j_-BT6swLuwLb1H1J|2
z<zu`}Gw?&79N>TUOwPmF`<NF5K5nE2@Q+4n0so|=ykC{Q%79+0R|yEX{T$9_eZW~t
z>aFX|rmG2jo#`3^hPu+B#V-`d{T*<o=~@U3b)}=7>Z+p)L0Jy38jTA&mFv&Q-8c@p
zfsEYRL~bk1+Z!#wWFeTYEIS%YGM`73<vAOL<Wqw`XxApDCHZ#cUdlp-WmyE0Ggky@
zvJvbN1S~y*faO37q6+L0S1cvB5YyVRocsdKw01nyr>Px^Q$E}QhT*1lieQg$VL8vo
z0_-sWv6RHJeLI$uPn@?;Vma#{jRnCTvA}{Oaxen+h~<Ook!ML>8K0}AINwUL1?)Fc
z12~|h{5rHqdzYGE3%J5a4dAKksjG>uP%tfw)QMV7Fr7fDU_B?8*_$+w^DM>|@KPf+
zfS2_Q`^-dF6J4QT8ul6${GwTG0k1Ps1NdblCC|!JMNJeT<wigC;5OB{^IW#C7Vvf>
z4Fm5~Qdvo~zGsXr;B!VA22Pq+S}0{{%~Y!Uny|vXjU+!CSjwDfq+#IujMM_2tE7CL
zr;#>^lt5%(TRTdFP~`b65D&{#(!xn$1csh;&-p7XNi`tG#gj`jp49LJ0|j5aJg1S?
zGm+ky(O*YDvAIa=ZCZpU#|t2il#FA0(vK&fw%{8;f_J+5+q7_LLZu%_@sy0TPPK|?
z0I3Xd7#KR-pX>%{>IQK57IU}{h>OmTfsDfgIfn<@IDDEp+y}%<d9)1-9Ue#=-d4SO
zt6n#*Tt0<SfHq*Bj{xz!ILR@*SPFHZiz{PwmFcVkZ#Gf`h#A@VP{8#<4St8cZQ%iq
z?hC08_(3JPOt;#!RvTjrc%zZ}fd65n7I0ucvjBWZNu>o^|M@m!1m3Ttl9AS*JR=cu
zCDJ`D(kAd%N|Iq%GfLx~L9ER-t#>K4V5LDKoG;BxTAFcMO2#xZO)~Sx4&$_5s>vyg
z(;!EsPFkC4A_`U-WT|AO^_!5D28k?L$syM?mo;*EvJLYvFicM^lb%{GJ+*e4o*L<S
zzzSy=NZ`)lTALPOWPUj$jiF8q-|_Wu)hG=rz!|9L>fx%<Ts_n?0oQW@*E0dv3785f
zjnkq8WRR~Kr9tQ=E3Loy(z$As2H7cD8C<Sug8E4iBPxc^9Hl`}vO@qF!h(+mF$o_K
zM+!ddb4~pi$oTQhs??8xj33XeO8pqf`0-5g9PTPzD)-rk#i&6|F!1t<I$kgwLShVN
z+v3#NUEzFBVmguw5hMd#?^X}aFb{@O4~9|?hVa1aPQC#071jA2(_JNmt!=hdS*90z
z67au_)Ca_@>^tw*KtE~$SL2mDvpc;-C*nLmA<r8n8(dG)&Q*F9JwR3<sjEvznKdM#
zAfkjTv;97=3`nX)E}d-!k$JV14-&}e7JM}DBojG)up7B2>EI{5Z@}l3B<k1L^xj_G
zaI$H^Rbwx559@K`#ab1P2TVRDR2EPvKeZZIKrTWjz&In4Kqi7S0LDRK1Cp^`zA`@@
z57_(Kz}Fv}uMK>~aNdc=`Z&Q9b*jK;`Kbl`u91fDYl-<agkQa)uIyi~M$7do&q9G4
z%x9q*2EIT^ZpCl4Y5j{awt#On(lBt}4p~~uJR|UIBej6H8EF`Jmyue)Zz-wtjwxx^
zB+@345{Q)l+EE&WA~%FUJah({ZCW@fjKI*7^v;o-4_MA?K#Yqgmu5Vv;Ryx`K8p~L
zL|V^8dSga^9sR^sZtb)PPj1A3I8rj=fL#+$`tjru3%&s)c&DpBQ(gUdNAZ-5v~IGB
zXaK1UaTpjnoZdMyG<Z3oG5~zm9PR^#4i97;9>_U7(8l4fnZtcRyp%gfVCZmq=eSV4
zTB+CYt`FK}=J^N^&!v8@8fAE~6zZ^%RKQq0W;(0D|1nYnh?&d|b*~2hBTGUH_+urx
z;Y~j|O9h)b!F1ZCg0-Ar`gAfCtmg#Nr<18*e@-wh!&Gn}CzzJQ!<xEBd{%+K?wQq8
zu$dD~vziLla)N1AQ^9&pFwJTz*q;+jvziJH<OI{KuG7>#<+BQWx@T5X!Ddb{&1x!G
z%L%4gO$F;Y!8EI>V1G_9&1xz*kP}R^S~;LRUrf@g96f+9pewrqq=L<yV4Brbu$B`{
zvziLlbAoABQ^Ee6V4Brba3Cj`W_43d-AbQT;EnI(K-U1?uB39QrM0zv(%%5?WS{hp
z-g|1B*1J3-@R$z9c>BP+bIiLYaNdD<w*Yub;9av#>l_QC3B++1Mzc-pFUGh4ICQYq
zT){}|e5J~r0`S8fjI>@=H6MZ7DXBC~tK}Ji14e2B&o<Hmpif`!a~{%nzNN1Q{4XO7
z0!f&2t(Hk&jr83S1Y2v<deE{m2>h**<k+xgB@JRO&Cq(C#Wn~e6D1=pk|Ad_V3>q-
zGLnSV7I7U&#Nyw1D`^m=oT-4=F8F9*UHE`FQt)-FWzsDVq=1+fxeGIX^y3GqF8FAW
zY|#fKsRbVm(kgsFLMixY5RC8vgSakSId)tFnYb>^#5ItK>(Z5JTmzZ7E?t?%HIRwx
z(v@jk1DUukP2yUk^>U-tO%-^Hl8X1QpLu&Z$zT4C^6Kuv!Fwr94z$3%l~l+s(#ELg
z1HLEZi*x=)b>L=OvW8g27^}>8Z}(BDn@p-Nl}g^){o6rhTrV`K>Lxm`Uv5*rx{<Eq
zFSI2-`S520Re+)?m5Z)lxq8y8C$!PW0=q$FR`QwGZR$L=CEtq#o~x-U7mBog^6ODr
zKV4RyIQe*jS$TAdP1ZOep`P-~JYd~YE9aGZb(I5tyVB&N1YlE1@=Mwi+6%PtJI@Dv
zBIFy}`JvSLq15^032vH&zEn|Pswk<iIN}W|JGbxA5SUwz7Dp1xO{y%HuPerqC3P%W
z$ufduFY_4=7RkN4@%_olpFVx>pT7R)n<9_HzSugvciY&F#4HzTN&ngwh6YphFE$Sw
zoHIy#rp8i53sOZ3Qbox-bN8ek#ZF(NN3yT$)zvfj4y6^#-uJYzgDx$yJaR-F?!JB?
z^*d7~rwSUotEzImV{Q2=h<y_`u(5ug{WDG=yM7ic8ZKeW4!&k(9CbmA2MNjbv-g^u
zy;yb3lv~E(3Czc88d?IBLN<()X`|<pB_d|ZJXa!tRSnS9EV!w(qGkuQ@lCyoE@mRz
zS4KTjCzz^>u3Aas`$k+ovmGmwcQDg$GsK>WMJ%kJU;nrZNUoo~vous6vg9ERS%`5p
zAh~|_KAE!@tA;619kbjz9z7{jrWRr9VQgIah?#PeEau4+CN=vCJ;J%V2hXtEHu-&x
z9kemupY`G-A$PFJWyTREl#5T}P`$6J?5R3tk~^~4MOb=7!Lf7)-Ctcsz;kTRtpfK^
zQn5J1=GN#E_Ep=>TEIzlZWAtA8jIbc?B6l5<_{G<;7PI6U8z*evZ)$<n_lJF8Sq3S
zHGscVk~>eQ57g1sGQlPg1#<5J40WYX*w9rqT{Yl<kp_UFuJk^Cu?k?>jbEIHJ(LGt
z`CaBcw(jz~%rS5FcbQ|R36(=)oA|R<ZHvvh-E79ZS(`Ca=G$`X)Yj`8WwFV41MfD{
z5b%3SD#x4F4?JTW66)V<EKOj)krn_^<kY7nfUaFlum(hd9BYB0uCxTuHPdv}fqNNg
z02t~@AI0vk0$9$K(>BTjZ?AHSd9y<sGiAyNfBsEv_bR8DH)}Iy%9PW?YHRDJl@oA(
zBlQ6<S5kiYK-Z2Y*aWUN(h%^nKv%O(3k#WN9H@V->8b%gYNQ1~6gpRH8TH9s5bCF!
zt~zk0kp_VH>C~qcwucIkH&<ccLrqx&xI{_CpNFQg)bquYbW`oZOt0dMd9#HQGi8c%
zjoSQ$xzz#=wWysT;1VU3g+gnoXN*Ha{W??M1a4wd!$1@{_365UuHTui8gS5b^#O6N
zEEHN;$V?g3KWgghK>U+s4Tz$Ap`hzQ)71|gHdhw_an9-LZ_~npd}h%tt52Ew0U%ON
zSNhq}b1Fa-xh3*G<$<?X6~?^TDvX&jRk%!(a-}7w1^khb8o<5!%EELprl%cr-C%-E
zAPQs^0*1QMl0(;8(^UiBWTXLLs4Ly-ZdU;;=W>6U^1$0G_c3oa_c2o@_xSTKnpLqm
zzuRdi=FQrSnKE;&s<!&|DyJRbHHWb3EChZ@N#%Ic+PU8t<N1O56{fxk{DF~%fj?69
z^3>v~l{DUH+J}J{Ecm*aTy6Smz|UJm3xLGnBC54%VIVW(i3pu?!vsW1+<GRt7yqQX
zL#(do!0+>MuUm-^-wug!kVrnMT5~1*aqYXt5>W-tvP3k1yBev52aJF$w!ps{SMm`B
zEDVxV??GLwSJl9quU=((P_OcLTP|GHriEzcx$SN$Kw5iw67yy~iJ3As=W3N6Xw}*R
z9;BrFBAE&{bAsuLh~Rv)SOcO!Y7-bbk*<hU)p4ZhY5_lIqz3S?fpFEQb)-@yIlymp
zFvc~E(Gj!I1b)^?LqN>9EH>M;ZZV@nz|d%#HH@BX7HYt%=~@7+1p(CBw6Ks_9agJa
zQk9#R7bwrC%!MlOKy#rFV?-)zH}G8Jssg=B$!A1Jk)K|(A?D4_2ACLSnU`x;wP|4{
zGe>@@0nb$bdwCM`W<80SGB)p3TQ3`Ak7@zmX`~_GLrN;ADy`odV+(k}p{N@IUaX}2
zG{=w4Oji?#8QC_TT}k6n&j-Xn!52>^^uNO7YQP7KGz`Rw^Hsirr3{Z7V-1MS%qM!O
z#X1&Ar_?ME8_r@qW3i4!(kZe)oG4|-j=Uc~LZkf|qx~2SEu=4qZKru57B{KEAGe$>
z0IpHe#LCLiHjS%2UmO6cMwBJDW=zw**O{6s@GC}YJf(1JtIld&;Ypqp+lPg8_nMY5
zZ+2S3bcoFew{g?LOlGqFP5s|R1$%iC^JYDP>3LOL9wBI9CgTZX@gWryPZo4L%*4DI
zb*P~VMBRO_>GJF}=FO;Eqxrek2DJse&T^i+BSY|uCfEd`K;8`lhPu+76kS)Dt{M;p
za{d5@y3+Fpx~?=`bs!4ll`3GUD}6L{o9cMhbhUs}hgh{6z-3A*S8ZA!^o+oVIv8m^
z;u(R{7KEpxc(UQ$U(LHF5SQeh7>Ga4JNXuj8au^|Hi28IQIQ3Pex##^9~IM810H9j
z1wbr2KWgU3Dr2kxF`79Vr55X04As{&>g%!k{7&#f^@r@`7Saus=jG;m75Hu=^}Sc&
z=Z#baGV-!N0XMMZSAjkc$$1PZ#;MmL7xQKpIhZKG@;pWhGnr-NT#fD<dhO*&%$xND
zCgx1(2`$WIJYmG<c~1`Mw%o?NG7#}3Bl~;Wz_fsKjnn|XT}kB<K<n&bTPwy^-$Xsq
z<%Fg6Su@%M{?JH6zy|_9nr&JyH9v-cuT+v81lO&ifxo5Yv0bj^T&}fKmut9;g>fak
zo>O1XsIRB$7mn?He@=aWMtwi($+9Hvu~jrkvP)XOrF=`D-^W(bAo`Lu_VCX-c8rEw
zdcdc9b<ZYWRflPNWpaYxBbAmpd|#+tUon;zaBv}o+Xoy`lJh-%OpC7jOji@Q$aD<@
zLtW{%g03%^t{QO2bPWMRUFo3!UEefab>Mu{)c}UN(nA5d2uw~BKm<3aTr@u()(6$}
zW9C8?IBYH?9||D#tV+2t1@CNP$$J*CY^W?w3_R7u27uF)G_i8(gyh3|vQjg-DsaF^
z4InAVyz8<|xj(K~|AobMw?g}bX&i|YxJJ8vV=OJ;e=DghN?K2OM&N4~g$F8HM=4bv
zSdVSAnKRmKr$(DOqs?||H2H<OG}2nmXf0#3mNQz*7)}0s%5|EpTlDHm27I?=vWoj}
z)0{ar@POqpd7?{5X!K)WsX+U&Sy@6hr>n-0J@z2%!xKNENf}{<jLM&Xi3c4m#N&R}
zd*>PQTjdZ??|#<1=!i8eQ@^(vVF9fHzhb0e;9nQ}s;sqXZLtK|Vc@Tn<mx<K<S=@j
z4MQEcqZ*YR>&8(Un3r!$1F>E3(RhJ5G62Mpg0EZfcbgvrK<vl@ITwr>7u0}7j|Ios
z&A@-E0pY2DUux-H2n<xknkwY%5wB_rc%rFV01Q;wFMXjCRZn?Un}T0ts)m7qs!dVB
z6;%hTm;|^b7`e>bb%-&*A~p?7w&mARfh69sfj73XUI@gXW5YqNSUk@=x;+>(avurA
zim2Kiyp>rT0tObh#}Zd8{?%uBCU{HB@(?gkH4_zFzoIJObxRqZ{>=iK*#_UD(^TdQ
z@HQnC8|TloL1e<Wrv|smf~*41R#Nfe;=gJGcQd=^o7AKIEy!w9;c(zAc$sJ0Nd0=b
zUR|8vH!3Y-kjmM|G>rfcQqq*l(=XZK>CJYS-iC6tX9k`eF;8tnIm0twtC0LpsEm+y
zTn*QGXjhv1i^=|@o&;n-LJbrF*W*>}LcNB(pI6#7dX=y@-$5eZrX3p%r5O8zb!@}F
zqm*{6UPa?(rN+CWN(iO!mFo&kT98M&xUN%m2OP#`(gGf=r2I)9!5vMo2}FTB-UEiZ
z(vv*8wl-ZgAPQtP1BSZNO%7czF<o^a3S`~^LtW`f9$nK+S3eL18&poIfJ0^JlYNv;
zFl7Tk6v(nS9~>%6U+HP8lEd`6L1o|MTpsht57{v4T+dYT_n1#D;Hty@nIpZATcZMZ
z_OKuGWb7mUs~&1%o{SpApY5S0=E<l*{N);YE~zn3Mh)V-^iUJ?WYi$Opof~6C!^*}
z6@QOj<sJq&{H~rutO7Unu)ika$=FBy-X3aVo{SpA*Y!{n^JLT@{z45sH^ebdMh)WA
zdZ>wcGHMXtt%sVJC!=POieIBw8DijuM}$L6>kUfH+|Q=EnmJv~cB(5`UC?!b`Zcj4
z-?yRhF>|&7{DhIJz^lCIb8Q+gF!vikypdxH@YTvySfxSaLi~HOU|y%jCY%1`Y43NG
z<q=pbQ_f#rncAlPw9;jifVenWS!v^^qc0N`tjS!zs;X``mDT@IILRDL?xT@Hd+uvT
z-&8f<_nI-v6&BPKNu-nOINX7yM@naEFy~rOE#US`Ig|_oU$3O{P(temo)P$fky^kP
z9ck)-FHusdj@CY&kyyE=-Zt?T%W}{A=}{WkmVyAHz2HMH*HnKk*6)0+wNnGN*g(mM
zX0EA)Iu>SF&W3<w*y*abX<=M$-GCUEgBp-@ma?=6Uu1EKYwB7*u1z=B`hYm&)c0px
z>q}kh&$-s$PF+hTe(Kr)u3<*Tb4_O42e3)t1s@@EU8B`M+2ULP+*nESj>m*dm<v+x
zlc&VDCE5q~R;`Db!3J=t8C*W0eVaD!GQKMCJH{v9KTN$yeyH{{i5DY`A>PPs-DRrv
zGiGpvByoki&sYw}{OfJ%9u5rhe(5MGxK@?_DwAsgH+^@wcOt+Q!B3lD6Br7nH#-P^
z!vt$U1f@dG?CR-<O|A|^uAFFi$Tc<CpD~#Jg6<kMbdMQa@{Gc%N3oJF-b>-9jkM$r
zg}?J8eBpYSD#8)>$bmkt4=c~(W<U8D0~b+KY*RS-oXHIVUt}Itfm<l4c+5g$OV3Aq
zH6!%_7ntbD@<SimIL`AC2-gvH=n7+52qb&*hvA-)zi~IA4dI)fm$Wk4@}v&UTuJK%
z7Wx2?TG^m-LIv#8CW|{dxY8L6KOXcLSe!dbCyyL4f%hHVYd#^k!~~nbP%xcO2#%Uy
z4TxZNJ|R~#xjGQJ{Cq+XEAsgs5J54R&ZmWH=xj5%_%?-)c+zCe(!2FKp|Y3};W|UP
zv6`DaS18X7ra2j4{6S4IdGP2?lN$p5myxQ#2bEMz9vTmOK7x6L1=9z7jglr-Wb)A1
z&GQi~*H@|5ql{%CkW|^^f$*=M_X7$Uq0HnVtp}O^13(JHO&+jMTW0dm$yiU8=XW#z
zvO~JYsaubkr!C-1k712%05?~XtIPD6%rxztZGtV}&OHU^YVUk62t3G04PebkEnr<q
zdA}BE?+Pyne9lM>;PV~2taU8OXiZW$ORwWK+QjJJ&1fIc%t&IJZEKm)CPrr(sSg<X
zk*-ensIi~u)y-A#AIy;&@J~vbSUEoZpgr|#oXJQ9|M7t4Y6cFoOtgT9D=9xB%e42y
zUJ!WkdI~mkf@$v8sM?3UMc_flQBMtELrJCgw2twNz>|!Wz73$3{$v)Lz;~Kd9}pYP
zVzW)lt4|)NY@s@~*Q+Zg@Ls0B2Haao*<w0VJ3nl4E#SpU%8z3z*vtuLhC|if;4K30
zHc|ulT_YvWIMyVJnkf2?N%a9yRA!m&gzLR3@Vp7OfSWA~&nvX1Dpd|Pz|jsyTIYC1
z;FmfWX<hFbfp>H;;wx8t{*C$E1Wq(v{XiTq<EMr1a(Dsac*#f$--Qu~<0T_4e3vI3
zz%Y!o9tZ<R<>MkCUOEf4HZ2?#Mj&36jI?l67=d_MGSb3PVFZRgr=KpRKG$<T*E2rX
zb3WJGsn7MC&-Hfdb3NyC`YG1ES`*J$ZM1-Mjt@(m*1k%W5(ggF!AR>w&j>uHgOQ5m
ziYvb{SDL_QjZ_EXPg!TQa7i9C0r96~q=idTkigKpOhKv(*q5pRV!SLLTG$sxAjV5Z
zTG$sxVCYI(XBTQ3zw0v${7FZKX+7c@fiFHG99&wPD^&&;*z91WHRKtA@9$uwwc0ZR
zKik2`=yN5w&zsLp;5|kf01{z2xU>jTKFb9XVaZ5qbMtNh*bKaDwrLThXao{rsgc%d
z)7S)t@uqb}ykE57YCwXN)fGsDWd+e9NMQsLVaZ5qbMvkr*bL&WW#aA6#anBq@z!$j
zb}wEcl9>u59OrYrO^b+x5lA>CBP}8lMqn6RJri6#7hF9PTs;?Dx|*)lWPaQ7+yXus
z<T<@PO9h)b!L)ofU0%-PE%hoh7dX#I4dC8Na*0cwsA_N11Y5vXPr*gnd!iQve#S@*
z;AcB@(YoFkTfkp*Fw%O$7#qMpb}+J%a812y;@!EXs}G1jE=A2YEnITv7nOO~%K}3`
z(xFOqb%_+mxu$}>oFGqS0O+vmiDkXKOs`Tez%7)NFOF&2yR#Pr?y{bO&75GGueqxB
z3~v#*(Me?#@(mu~CQ2&3r!~Vf0=G9(a)*Y+bIoECxUos~0kKggnwD3e+@XC))n2Ao
zcZUZ4teLC<zhb1;FBSeikOcqL*lNHH%-Q6*-c0S<#aLRvU6qs{|5UJ<6U+>ws%?3T
zz$1;+0KVHu$(`NYL{Sq(N10R~5JlzaSotehrUD-`!4~jGN-EPp>ru}b>-tEf%MI(N
zm3D`TRe^UKsRqPI<~_<wBjvKWq5FrXs18KY#L9~F3DFJ8k5xHuepgAz2`P?low7V{
z)>^=APqtYIe3g>&HI(XVqHCJz>H~(l(z22#!-cCaF~Ou@W@^_vjHLzql#v?1E0k2a
zPU}9;2)w_8G0s<Nw24u4mL*B+YO~M;h8EJu4_1LiCRhiaqNEKf%QuZ3J5c*RX<|vj
z9@ehMjim*A*(o;Gz^#;2CXCisJtOeu4#wE8)Myi<TbQmsAZE%O(z?+z0z;!|4iTKK
zf^N=&4>4u^g9=v~sR~5DOvpH*KWO^4^itSEq0DsPV&#&LNk)^8048eZ`%P71?KbWD
zFJox|_gZ0t2HaOk<)G18<QakQ>R^mROpP`%I>&VN0Wnh!8m$q}2n>y;gO(btVRWA9
z8UkX*8LhQxE%A)N&}e$U^020L9}`@FuERYk&d;w^YLc-elMO8`V`%~LD6<@|RqlKA
z>IxTp@Tm-UKd`PO#|9qqY_Z!vQmr!rv6$yk<=IwwHmsZ~-)}mhG93cKn^jKQboHba
zm2IX>n=xhDb_EZ8JE-tWW1f??oIIV*Ip)Q(3iv=zThXtWfU?SB_x~J)xM5{Q={~OQ
zu3D!Q9nK#pdz<a!E8w>&P2Nq3-SG-=aKx*2-~1BQv3F-G$DO9O1^kVY%9%@R`e}@L
z3-}Htm5j6&dq&^~Iv8m!Io&jN^X_)@t_j4Sa*EU1){HiR_){{{TId;p_){{{`iy4;
z-V=n;Y}0z$GXh_)k(C;0?QZclfux{h?3M)vMrJ9HxSe3FO>2g^QUelq$w-TV5sg6N
zE*WWE<~0I|yJVzwpJxPy!KKSxT7V;QaFZ)fPL%JjWWpPX!-JIouQPW>fOrZk?Tu|%
zMu3bMth9+p=3qQlUyA0FTL+2fXI8~k;6q9(URl^!oh2wI3!Y<vRDt^_sgNaLhSAA_
zN4+fYJ)!JJwLylQEckXW3%o0o{jD}gnUe)?V;QXirz@%OnRM=}L?;XG^RmFfP&Ur{
z=QNRz=+%7@iNCVLw4j*7zVYZOdDhLz>7lQi9>!iu1&E7O6>1r3VFfa(u+k=yOaYPz
ztZnMP^!C&a`u)#!bEaB3;>Tv?^y=V=eM+^&F6Okv?xtGev|*P3dks~NSBK-)ZhG<h
zpUW|pD_Z<fT(`>W^V%~>ukzj$5EoJ*dPrN6Qc+AF2}-rZp~K0Hme7GLeUuk%Ca|yq
znX9nU?y(g9-)MWNYtHHUmCrelDAH`vLp-^h(?_hSmN@5U>QZ&Htzs?UuaxAr%~qS%
zOV6+~5O9Abm5j7TJR|T!9gJhUQp>qgGgr1U3pF6d%dMUk_T}&Z3|+}=^+{GnVpk|H
z@`49ai9pQ5N*mk48t353$gtaJuza5lQ5E<zB^8_02AU4cJ6Z6amZ~anj*<#lTq8YB
z7W^JB3p_WJjbossR;zPS5z4ww&l-zpF_2(WA$mx7QvafuKEh75&_lpEE%Y%?sg^k8
z*zP}91ZaCEpFmk#>3P;C5J-TL(;`F<;U%F94}D!i7mhgeHChTUS!wG6@Ny#!1J7Jl
zauwg@pmDwu%k_iS7nCX=!vK#R#l|pj_{=~djVqNXqQGfqn<(%#N-E9LdY3V_fIl|U
zF!0*<1ZHV`Q;A|0c<_78EO6fY0#O=M&NV*ZzD8;RFZ_UUbsL5kS<ah4hC?2#0rxSk
zK_F?Dky$%R<2D<ECXgH#d^E1Ggg1fYv*4q##4_0glDvYC#<wk#O(1D0_-M?r95sRD
zsNn0CCGsE}0+3>qUqu9xg>o~Y^^oPH20X;VUjQUCr7W!*jIjpX#!|8XNVZB@T2!O>
z4E(&MX#tQ7ma??&GR7M4&02;MFOXs^WoaGd8G)p~WTe%w!KeXmFwz3xG3S*vT*}fq
zOR3@waK`&XMp_3cRTzPfbuiM}=3kjB3xMBKQfZpjFFhlW8Y&sP711P{M0FsOX+q^t
zAY~)<c(u)|?^q4hfs{zWN8>W9mpYJgDEMeBup+7h8Tx{c#&@j_>Oe-X;G;3m#;pz{
zCk0=(l(5Xp{s$y}4neecK8x`g0TK<Yv{`gTE0BO-r9I~y(+YfrF2S(UrkF)5knmxp
z{UNK)5g_#lD{YEWv;wI}SZV*x3U35Rl40$Z=S{5i27u&MehXtRnDt3s%>q(cjt$IG
zBy2!Z@7TbLtjGp{q}{QB|7tZh03?r&4g3}>u>l~_JGO3-Gp*z{6-cy2(l*<nO`DK~
z6-dOe(k5(S1rjx^v_ENCSPUd`SZNc!Xay2IthCRyY%B(n4OqKn<V{)&lC=t$$tT~|
z0g@HR2IdeYY(O&N*uXO^<b^=8;n={;P0<D<6OIkcIY`)mY&DJzO!&eUNB(P#cr#r^
z-Ob8W#jm!Nwfa>(Ek2@G$M$>0PufYc`aWHVKBrg5cD>@8ZIP<JOv`$%UL6~Fe=GUw
z1-bxzNw1C#{8d|=s+Z`ZbcbFY+xHazr(Wf}_6gm4Z_=w{yF>90ZC|cdbbZ=dua0eo
z;@xcRs(wS^0|6WO51#Ez-A^yqtJC&r#n<Uod^=2+ua$arZ09Kcs4Z>Po%GClkX{`d
zc-YRf$wz=3%U)^KEkD+(__cZ!C#deb12!<_AzzrUYgP2=bz<d=32hdI-zw3027b<$
z5i?^=P)1-%OH6F1TlMSoT9{xlxlM`A1iRf~LA`;gLDBc$+WiN;7Wz2&9j7zW=>wl)
zv$V?es|2+Qre?*&$=ZFIUJDcVXyeC$EcjuiN#cQzn5=x4oh^4+Q2byff!KXiyZ>9S
zg<aNyGlCfbW|oNTWUcy*^;*cXf1j^Jmlp7YHcbYAzcg9-RaTCw!@2@H**g^fE}((;
zwuv$Te4CPrAFJbla9uFTz<=|yz`uvGoEVk`GY|ZAn|TAk+m%$<-BG99!NGI{f6Aug
z0PspB6|x*ZULMR*@KH8L2Y|;asgR8q=XKHZ0GCfDGds2%G>*~<%LI08VD7)<sCnb)
zXLTkrTOA|#)wYTczEvT!)v<xu#|nLC>r`ZFI!5p>wNGAU-AJb$v(>SI+3;k$+IJtF
zb<9l12j+Yr;R2bNjt$JlShz=nY3LZi93sTMm+I7E#yK`H2VCbKnAzp{!0fbz-FN7m
zU@AFAFozUjJa2S6ofFI(#|Y-=A}{`YQrVbFjt$IBU!ji%^TsiPIaC*nZ`5jMk~l^%
zN9uxcV=ZlFgkuDA(^oLkp!^*p*dIRz;|jlDOP6Da3@?x^XY<M#mDw{!E7Mm`D&O#!
zJ!4g6I=$>hTey5qPWis5wafuV3;^%au8e`@mFe`dEp3syanYrtlc&?qp0#Dpk%IGd
zod$kmssY)cHc{b%e~NAgoR-9OARAA{b;%9A>`C2n^BygHzn#>8Y%3iOouk`{-$H6Y
zwvvr}^dHe#z_}|pg0X3AF7Ncro*~KhLwj0fI=#P91;RC&a{W~&1?M$U4`dH1)t?-y
zKfN-Y-goG3QL2x*zNWpL^F%$6Z6@zMy%Tkc<-I>qd(YLYs0XqWxfh;_upxX)d8MPw
zM?)tkr$<o&WJ@w75Wb)?rerJKfaciVR0Fa%nGy)gmDiNS)0)}IorL=h>LQkHbqCts
z&V)4}Gue2VS=?t>#}hhnpV6y4#RD=6jh7k5U5IsjRcGUEdKD!=W~1@$qcib9JG0e*
z%rWErvQDa7^eTFROe*7L_Hegi9rx%=yWehKYCvY1@y5A-NSD-qu@qN<49nK(S|-cJ
z>PZ(}IJ$b$^fqja$hLk``-vI~T&n}|pbbbB$RJKlO%{vX>Pe!g4Hv_=P0nJ#&wb|Q
zHh5Ja11>9(i{^OI4vBHyyzDAB4AnZl{bC*5x7Z+8fh?$F<PA1zV$8g8gL|dbN)^aL
z9E1UhGRlW>v6jr&t#qnD$}P|diK^NxOii)pPH$hT>+l!#DisZ6seUDEm9Gz3$X8F2
zRkaNd%enAomh+PLIbGCWVu$-0kY&3x6))GTCyBi_JS^>{sXXt$tBD_aQ$UvRUQxlr
zGT$qzGj!p;z!v)&@WV!`0$J`~SUK5eb$S~zyFk%RBB!?x)ph-Ny~-c~S^6V|kl7I;
zg|F5%etX*&sz8?i;@}rAKOEmiZ27SR*RFO;Fu(-<t4&}#+q~E2-vIODX`3GdJnVUy
z72^O0jISE03Z(YBjpXT-ZN^=nj+Yb0R#r4sAhlPTN^5Sph{H2heL7xF8$48!%^gVX
z^@<9fvFg)B#Y3u(Sur+&R9!Dq@KBk;OI>ncFBSu5iu1|oR;E=T_1Mc4Jk)G2Q#>qt
zhZS%W$T?=JpI~?@o5o~WxSX$WQSQPW#}#nL^MD&=Zg9hQ+PE?0t}*Q629f)}u#bB<
zZm7b(ZuYq!@9{Vucl<rEJ!8j19-PPCiPqRDSYwAC)rT28kqDjVAv#G5`$!;OhJCn)
zTVWpw#J{kwn{WQ1ihUx2yM%?SJU5XH0P!AHJm-OhumTAN);JL8aS+hs0Al$t*jPu{
zHy%AD&$>A|J@j?c!()#g9}J?F2Nl8!Wax^h<yG*ptVHkW%$b@zc1hmT5uVtAR7>nn
zPD||DB0YvvKVjB+)<(gEeQ||^edDoF@~oSavq4|Z27TRZ@IX<92S{iPPi#_HvBdK^
zAptw4KxS~6XlM-+5znl!FAgB5M}xgJr-#07dYJ9O#K%QuXRKCOyJ@>jk4aK=n*}dy
z%gbT({F@!5mH=@f6`}`^ib{<X$KIt{VrS9z&(>>B59y{-C<`~F^o`}uKUZjYwUgex
zM)O~Wf}Wl2{&q2tl%zrgM7^gR^bm5gtcdA2^rd=+=J9%1`Rsx$Xkp)Y^pNhH9{RfJ
z$s99qky#O|&CCj-J5i%dOOu`zb}z96hzqF@J$RIx6|r}zmN;~FCmx(j6#{vN&dNxe
zC*r~i<SYm4SXQDklKm?-nv{|7#15odVt;a4V&9(EL10A*3y#AiCzG&mJbFr=b#roh
z=<BA350@m(KwKovsO5tmVFmIz53IC_L|9|PXlKs%3O2{vEufe;Gi6}eRX;q7>Q=^{
zo;~bC?Il3`O@-nJ$rnlJ6=9B7BfPbnUXuUMjRTfnpbre9GMDv~o{j90!eSsUq(byi
z3CY|L#q^P&R7)IsyyEmqX^vM@I%h1snW916mioLc9e;YJDu?_9N+8=yDinJHCo5&I
z2y?s|VNx{kaxRym0b+{D+s&nmedOU6<zyh~xx^x048%w}w)9{=S0S-0@tQ1A#EWpZ
zC=h*CpV*`h(NnPpEsKE|NrmWPPa01YYxMET)ApF91zdVQFOV(-9;qbvHgKy=>nq0C
z0#3fbWPvX;QVV#pkro0!W~3JIgC8<o!1I+<`a$cw3yl%@ekGNRw5~SB<j0-J$Fr7?
zCh+_ZqP_tn15SO@@^QVz&;a6M$w=$V7Fh#`izOp1?{M-LF>rVT^R5OQwsGkLh7Q-v
z;d_j+24vLaucrg|w6JPGMoqYYjG8>b2A*wU_5sO&3$xaywaUh&4@d?|Mp`}}$uAL+
zkLPUw>%iAriUxq>L!O1-GD_p?mX11*929&sHnxP+fn=fJqd^|zT@WAv6nyc@LI_lu
ztSmrWWM!dEoe3+Dq{2!Y)56-#@GmWQ{XnF|;WeY(<o;%I13(;fZ{UEbF?k~bNJ<?W
zm?9H4AQ^RRU{We<K+43ifeAp^y17qL$c_ud<symlLN#q15>_Doz)JfxtCGb)e1ets
zN38xA192PHZoW^}+Ln+jU}WV7KVz|*mHlxOo7o0GW7DMiEA{<lI)mM7LEvdNtE)d#
z_(JvHv4OX=n35M;@$+8-C%{{JCxBCxRD4U532{{*3x0#m-(lb^B~7fXn$V``eyT+0
zE~EH1jo8tcNk0f^;F?803~VW>u)%u%eI+`5;DanB!@!!73fXwD9@A+>zML-b0hXuc
z5ei9-V*?Las29!sjAGK@_`u9Pd9MygW*i%s+7`ACYH~=HV*|6~NKwuOlNiSbrv4ot
znBsMOaYCrxZ&~nFAOpQoWtF^sBj4YfJRQ<oG;ZmV4}yv=>iY=|LO!JfGMa%RNQ-n#
z14Yb$-&k2xfedY+2-1*_dY~u{^(hVU#p=B5AwX)oe3j&k%Jivfe0OuA4x~nncXKtp
zlhtA!NKG1V?9<Nb6N_PZUqEFrjA7qJYKx%_`^ICV<XJZ-XM?_+4f?v-V3PFMXE8}-
zk7J)D1Hu|RmDzg9Df2lx-OF<TTws$+Ibt&@N1TD0jwH)NI3i3~;t2c3qeruTYfcY+
z-SlKe1Q&~qP(E*n)n;-^bZCq6AvcTkpgecS6dOvl#8EIW7^z;F%kg&04!o?w|JdOJ
zuezNmuyU%dI1+F42?RYWZB051h-;}3J(O{g>PRttoRd;5^bkZ&OFSIss+(+R;c#FP
zePQ2t^ypC8k`(HpZ!A4BjJQE~LIR?(+i(%xCHdi!g@c}t+i)ELBqdRzgVhE%QVx1>
zFc}Kb5{G`9?f{SI)ot#EFXHw(IVeukt{sh~1>9Fj?*1_St@35s`ystLd*GW*S#l5g
zuy*b8VO**KS1YN!AEotU&j=j8xRfm!X`Q81xjP1KyE<f~HCL&^2)v|&k=BnqBk;8!
z2{qE%OR1s}`27yXc(BqejKo=BF;4R^66XO{GV#1+VgyL+u+k<0vcv<49#-0<Kv;pq
z4=e3YSUyI8<O9}j8To(>cO6Iu+;G?1v~X8WZ9qIN8EN6HFaq(jWTeH=2_q1%OUBse
z{nS?+l_Tz>O4`GQp!ybtjE-=9L`jT-koMJD!)qbkuO$3)T$)(?5>o8cTh)X2>2=e}
z>2g|^BP=hVwtucYZ&WWzj^Mq#cI<Mh<9wC<eus|G%Kh3y1*SUaAq%M=uh9%Jz;2BJ
z`$P@ipll4UV*~r-4FVZl#|HKZO}>cpGEEl~)QN%p$PZqlY)nqa2KGZg2xN(JY+w>G
zp^`Z9V%3hpa5H5_ARhL`)sfRv@~oSa(?ef3JtRUF1t2am(@{$@gcV3KV5LoE2rH1_
zU>(aL*;K<%FJ!}v{Yws{!V~+NYKdLXX^F!~wZu{7w8RnO8<R5}W-OAWux~tiN}hGM
z=Je3lO%E9f#uXPCEYuSAdW<Vkuh9t*<?`04ZF)Yf9CE+|;#w+151u9iAc|wJQ!T_p
z+H+dsw86_dCb<Bzq>(n-tYyLqWG#c0HruVR0$JK%jUB=Y{9)~UM)#X*^cwcv675A{
z-*{}4JnL@F*`P0HgT8JyNYVObEB1nH#U4QKX8jAg7p28Q&zF=#>J*5}sSrJkZ?3}V
zV*pbvad0^;aiBS#IOvD91YYt{4)aamFD~&1<@AT-5j@@mYru<@<c?gmHZ9c23A>vU
zuQOdEz<*Z~gGT%Jo)!3J^%qv!*C<`835YMS#txB!s#$3Qml&x5ByM@$OR%5tT)@kX
z)C68_qy}&sbG8ZG*GLWE2}WuHKVhT>kZ8p(;Psvhc$1Nu!0#EU0VHFht6Q=dGIt&V
zUt|$hNev??2VWo~=p@13U~)cYbTA?V3q&IWOB=1i3Zy(>rA?~-fpU47MY|Y?W6_M3
zg9<&kn9Nq;p%0%^E%e}hP78fRmTIAgr02B6!yfzgf0J6Q{8UpL`7V1rJrDZi14&LQ
zL=UM+0v5&ek(^WuJzdi#TH>@-E-hEps$S(cAc2c7V-0Qqk5!UeN?UDO*L=(vfnQTn
z$w=$=j~gTK`${SqX+7>4yE)u2hnv8aMj8g<qEp{AhreNwHG!L148uUYE@f%`!Wf&t
zEzR3uAc2&!w0u~}s>L|mZ!wGjiGh`Y_KxPs2oNV>rR@!mbj#=4&2SCKsBcg?Whr=x
zrcP*!z=wNUKllhwtAda8wE5s;JZ&zR04G*Xxp9=njmjwdHIQO+{(--wNpb#xul2Nk
z@RvQU3ckV9=7VqYw7FoiTKGrfUS$;jfYiA25ByE773UxLPEYFxf6LRV;JZC-KKMU8
zZ7!H{EBvG3OCUM#q_tP4_D1~htN=42uD0torzk;r)&*otB-RyR#zbh#z>JB@6PPh^
zc>*&gLhA=JCPJ%%84{t*2k)tQ68C@%iLk8zGbBP=24+ZvwiL{e2yGFVArV?Xm?05b
z70ieTZ9dqKPV&c~7#)^0IpqTxiDJ6T3oo?)Vsm9NkUYRjn>9?d0`Un}+CQ`uE(YQ@
ztlbtD%1Pem0a8tpg=Jv;ca=3@(NInz45XTzFqmo*+EOspB(z0fs!3@5V5&)IRWQ{g
zwE1ANCbYR=%1PV<Qcc3P3`{i%Z7G;)651j#)g-ijFx4cqDwt{#+I%o0CA7I<hA8tY
z)eE#tw$STFm1ULkySZ&hZ?H<L0uNA<yxB9_UZAVqv3d<uKw4=FV-@%jB^4@;(Xv{l
z*FXiN^R4o#z|SeEP!X3M2ZBM})7(KGe7x;*RZ8*i0UNk!f!DvG@Y{NI+Q7dtwkow*
z*Wer*c#Y+t&MbQ@U;}UH<D`^El;pI5uQ#@2vDigJAp=efe2GQgx<MfsaBN^Eqx@*y
z=QKJp;Ml-VTI8*bG!!!6*ucy%`Q2kC3>k22V9rf)`~;E##|CCXh_<-C9a~%n@j4kk
zP{Mz_3j1(@xfAw{$41GsZcfeyeK{NSb+f?`$-NDbC9_z8<xLArl7C?Z5-F^)2g2IT
z@cXr1qI(XxC8XzkE3U;r>Ma$*Nlw}+2c!E_E%lUxo(F9*ECw=csZbnWTfL^{!+|C}
zxDxh_M^DMK?$(?h`nu_1bc2D$#d!2^FEe^XhqfpmayLj1%5(RfvHz)-I0|Z#;SDoK
z5u!Eh8;>3`m(xREH$9w%<g5e4MGi2iWnBu6MR*KrH!G`jVU#>YZ_Lnhwk@4YfcTRN
z(SxVO5Jie(uTw3=Ll8MFaX=qbH?iDnsB*kI9CNzqC9)pF(M`0S&#|mW^jzU{48&fP
z9TB33NR#XcPwYzN6Xmmv{q-tOB7rN7Gz?t&ZzXs6Jb>1<N)=h)i!Lu2Csc-kw<xKU
zrS$`2Yy!7zn=J5QC6%(Yo;SwyIfVLtzr`>D#A{X{+B3|P5g<;&O4}PA>6TBd$&DKD
zP)(B@YnOpB?gFc~X<=W4fos(}Ck(#U)0Tq2>}iX@H+Wh<_^X~)1>fvx^TCAd&f?%b
zE$?;UkCf!x1OM35mV)o|v_;^bcv?UBXP#CCKjdli!IXk?59~)LIagBc)U(W4Ah~B<
zr2T{y_hKM<fR*;6nm^GB#3xv3KccY+D-gF~?N(VeE9rjVkgu$z;02zx2)xkK`oSZf
zRs}EdwE5s8JZ&zR92HM!Xnb86<r^SC3RXsGDVTy4+9EIoE3|$v1uL{Fn1U7Bd@uzo
zw7FmgrtpsjLnHnHsaTO+3Z`O(wg^nc3auYZ#R{zorecLQA56sxZ7!H<DEy;A9b~>i
zL;13zxaSaB4J;_4eg&8jaf{19W^ozFEG{cD<FhO?K1(y>vnVq@{h9Hp#^dAi#PIBC
zGh+bAkhs{u42jT|ff*8^Ed?_qLR$o8NQBl8W=Moq1v4T-n-BJ*lYFJ?1zPx9>os@+
z0O^f3H>$vWlvF$cXz2=aoL&PJCo1V|TV$%hOO#YThtT|;tJgrqY9)QjQd|YztfcI7
z2$q?Ctu=R#3O?ReqbjBN_ka!jP7A!rqHw2@oHp=7##W^^->$(qHt?q`2Ti8%;{h9Z
zLmwxlbhwh7Ht=1>mK-ByX((jCiGeS%$OmswNCq4mn8_&j`d4UlWWce3e`k>oPS8-u
zfMWwQbL7u*Fk#4mV*_(;k{9NHWWce3nGoVzT;Gl@-htw+GCWD+KVF4>xWL>A`^ICV
z<XJZ-XM?_+4f?v-V2Ir9sg58^X0Zv&a|le5e_;g@DXg&vnLAk$_<pq}NBHPnJbKQz
z;#v%(-cli)<fNT)FuK3jQcpSPS!a`BF_2+Ph2q4v)oW@#9B9&mD`DSw^ypyTn$tsH
zH$9AQFwnRdj~?!2Mvv&w7Ujb|hlt|OK8Mf*V<^=UM?p<8ykX`jLb4L}jYkie%ju!7
zn;wqCa)t%sB4=3CvMvQDV?2hno0S#1FiM`H#|8A9VN2%{ApWF6^x$c7k1WF&d!1?_
z9)ieei356{x{2jpLzUyz;YglCz)NI3hNGKHm*sOTj|=GeH=ko5_M+^F5IsbiWJh>n
zS2kT!-t=FqS9yXCT=U7Y%N=7|ZCbxpsxSig{8Y&(zZY=TD2>~cD15*ld^%J}>x3&y
zsxSg)e>P;K^{3AnBk=Oihm5o~`+_k7Z@)TZq_z1q#t6J!NoCNq-oDltyCv{tmX{ik
zlsR2BOW?LwpsNPFN=YRnt>1b^AUP`;X<hpn(+J!_lUg#;BERyI0q}w#)wMRQ$CWCK
zz}v4Xjg%T`O|&to0U4W;k=Cv{Qlb&aaFvX-4!6Op0l%-Ll9AR#9a7N<WL(upW!UzM
zmLA|rC6$b{zTz2yM|~-bnpRt>Vj4(=lp4n_k~&3lvDM8WFsz%p)eSWu52t_>fE+i0
zjH_?~|HCHOAP{ebYiw6Xa23DUyJ=I#vUdZiVOVKXpTY{HIANuY`@#yOJYl6xjS4G}
z0)>?}#V4#lDi2oLl%KEysXbU}Q&++Yq^Mw}O=Srykg|f6wjZUDZliRBE!+J-My2@m
z78)$o@^AylLS69DSZ=F!Kak2Q_-IVAC8r<AxD|Xf=34Fb14(beN8>+jdFTfceZki)
zeo9lG$N&l1&BOtlJrt%e0;$Q8krs1H7=Z*`GSZ@cg%L>9B_l1WR~Ug5ZplcC+7(71
zWm_`RVyP8IAnR<&NQ)8|Mj#n48EH}A!U&|cOGa8Owa&Dzui>gY(5A&a5Jn(Vpwvi<
zSs;u+CP2wZi=|c=fz(LJ*sVx7(8vi5NRbp%PF{VY&EybPAX5WY+MH^H6-a5oO8b>I
zTNVQ;4p?bZA)*z?7{N-LvyHF<$p@_6GO|XO4fhK|;7e>H8Um7)f=#>Ib}AVHGIbmq
z_y|4g$!j3MWlC~v;88n23<1wml4AoCmuQPa+D-jp^OO~CFLe=Lq+nj8wiyxOx<?0v
z%nKJ8XPXo*Aahi>Hqnz1vr$Ofsn5(iA<fejjnNX)@p_J7E(z(y(Hs)e>-7}E{1DRZ
zN@6Am=?i)U@tH}Ui?Sh{WrHo>>{QI4%hwi2;J%tj_XG;;lRCh3crIWAPqSU4%C7N#
zB{^;2>pUAnbf6BEV*~qP8)Vp~TItB2yiJC3fJrq?htz2U?_qPV%836wU<3Oq7<{|#
zSyjC{ZQv2x&#Gy$45Y;}NU^*^i^eGf`vMxIfOfIsm+$l@MKwrKy-kbDX#@L08~nL;
z|0-YuPq6cKm3ll`NlqKs*XSTMy1CY>V*|6=%L$aaovFp@*ucJ)2dU+61#Dnn--Fcm
zTCH=Z4eV<^`C4*K>zmqlV&KndpZtK~*R+T!YR3j<eV2!`16r^YwPORb=8Lv7wIV+n
zR4MolrOB<r(+a6prwzQHRq3HrB4y~<z|`D?%Ax0JC0-s>Dfl+4(%Qoc{~WM^U*qrc
z<|`c5t1}8d%d;)hDt=#3&EOl9CXueu%Kb`E&EQ`vP1tVH@}yjyQ7}82M7m7t?tMZ1
zfp1WnXnU33x$mx5rwu$}H~O_x6rLZjfp73^-&OdFfDOEnzx~@*;jTfMf){wUzbo8a
zpCdTmz;Cr@3(1@H_iDv*v~^-&zcdd&sAbB**0F)Fu<d%7b(LeSV*|h2%5<2m*>9D@
zM{9L*&UKPt%2hrYp*neBBQK!<{r0)=YAq*@xK0_^uZRnQ9B>^Q_&rub!<-QO&O7|9
z_HyENl3>3`4$st8kMpi$1N+r)7|3b2@I$N4uYSY-tt8I6jt%TrzhNLJUB?FYtKTq?
zbFO0p`_*qaJ_}JBY~P!eKa6){@pqE+)ocob-8RQVRj}3Oco=^+(9J5N?nn4KY%#2C
zcsU-%l-(|8D~^-Wq~O9wx>Otm5%w|qEG}UmW66WSjy_!kNPXDX%{NwdIh_NUDxA(~
z6Na$H-oqMuF08S`u*T0Up~sHyuZs%{Nf;Cde-a(`;VrI)eIy-s!#<LZ-(ep~$M>+W
zo8R2ZNh*MZLdb-{)-J3-c6nH7`)$4JU0s|2RK&qUj{}Du2Ms+AIMy2`C)OSI#rng(
z@z^MN*3HS;pf6{GzHT;Hi+enR!zAnRI3B@KR>B%PRU|g^XNyQI4#_7f(Z|B{P|wK+
zi}Hy|>}#rpDA-DJTIeH;R7)IHPD`BfyL!xVDt&$D_;_qk0y!J>#nZ@@Cw<*)u))b7
z13Tut3^FF!=Y$o=0f>5_O$LNDb_&+mAyyWw^_p8bUJdltZhBd!)^Ek?rtNCI-HARj
zyme}up0&y$e>(z*3#kx2teMH@jG~x65|nC*osA`Wr8LK@DNR17jd>-d-Aunj57Lpd
zGRf&V*dAdl0d|Q{gkm#rQg2~|JUYnjs^ih4(chZWLti&N%#Zb(DzT+*Q-!GS^~nua
zS*aCzeq_mA48%1Vkw}OhCULSHiemamc&a51BBv!z+t2h~iF@{N7;U4_n~Sh-JbD<a
zoF4kR>EW0u1rEf;Vr=9wcdRxu7l`gR8f7#WB&+oN&h8nP0I{12(St|1H6!*e)e?u!
zZpq#lrkk_mcn<A3J(})0IX(1s^M^Y>H>0A*xlEJzPEc665hNN|@qoKR=(LGYRE*_N
zGy`Ojiv3Gwfbhh=rdndxb6Vmsay)TJsiio=#rkZ5o2jq_SWG$Sg?;1EQ}V2vlhZ?A
zH$B`R%b);pk?}(<4@aQW=Jr`s0C_3`D{UgolmOab>6L^WuLhbNRp7<7{~a$c*QZ0#
zz{|2j&!_Bh<YFK$q(bzNr=;dYF?}Q`)e?_!yioN@X^yvB?Zmv9<r$W1^x3I68@`?=
zla-$9ltYemKyFo1p*TV~Ss23zkJYCk%;&IgJbE<xTXTBo>!ycWDJeD}cTP+R+PE&P
zu_ai$S(&c8cUlSbY^NMDLO}dUh3LVvToKZT*Qu5`h@6%<pzrBZNfz*MsJ75&((E2#
z-+1(tJnL@F>7lQi9yT2r8z3$+HmGGsfKD5?MMXC&4`}phF6nt#Ib;}t_>&6JgQvLx
zh`rA7#1W*H;(!+EM+9@OE#F3yPdk9$R+4)IqSmJMwd;%#xb>GyQprf`bft<$;Nt5;
zMp{=YRTzO6-4HU;`jk?IaqKYagmJS)HV7mZ=Wsm}Sv`(SK7%AIT$K0QfcPg|K<{eu
z7F?R~5nRPD3X(SNNI?Q|2UgmoT3CVP8CKff$B}O7yum&|>IZ(y(zy_bn=YOGmd>rM
zQ2K#`Mp_7bkCFO;&s*Uv1XhB=>2K58$?9PtaAr_A{cT!|ihPg+^uZ^8)G8gIfp~yi
zqyrWyV<PWP02z*wkrpE&j6epXWTZu92_q24OGa8gjN}~+!uYw3;bI^IR#b%CyU}I@
zWX%F%2v*wOaPr7wSSy6tCa=20X%W)pI?9ABp?+US85_bC7baDBn+D}Ry#oKh0`13z
zqtzA12KH_ZP?y_l5jr;T0cl}g7_fmq=h@OA2MF?N4b~X|`@jbY`~gjXV*~qSBqO|9
z112d>4E$?LM&GlV1fq3p;6Zc056H+nHZY;d8wvNS#G^qnz?<64t!=8IY!`$CZYfRH
z@wqx*`}FFJg3qz(T_XvX1#EF?{EtS=Ty?6ze#{pT3UgB4#n@flW==Xu@Lw&Y1uQ#$
z;1&Rxq)rmNgSo#TU56IL-s34%x>;o<e|XIPm;pXfK{+0*Q$9JK*fzDoG7uiuNEagz
z_KinR$+K=wP7i(E^pF$T%YeAZUWQsm6FO~DHjeF(2!(YlhoWPZ+!@9GC5JQNiG9uS
z#D1rih=|h5X`zphQY~?W<P{%#4RelHhcda7hnIyU*jX^e&eF}LAL;TXL4>!Xa(7M7
z{mLP?qd@GXLiAw1$X}#5cI81eFsN%@uk7S_H9N`Y2QjZ?r<>_N<g*}UMbDpn7J&F0
zWkG~uGt?aw(<{OpuSS@RKfF}!KQ|B2wu3IpQC&+((DMdcwig3&Ar+#BX_HJTQA{5R
zO0~o(y;s-&CxSf-ywz9A`<u+2%!N0VU61HhSb+O}t?Y6~z*?KudzC7Tz!%*dGSZr*
zRAB_()4@n<oo58@e@m#5);UTQjllal7-{{%GXhurd#Eu^UYdoGI14QGX&y%6JitmO
zzGMN80Erz|+9W^<7fAH5(k2DM3M775X&<P;3oDR(z}hV%d)jc<fn>ndMcwKGcjZPE
zh^HkZEu0lbAbysNv=};J1mbnc82kLR)-aCBE8!ni_kU|)RDZ9K(GjlwbbuHIAw8ue
zycW`VT2=VxxRiunLW-UGy*jm<noa){)A`!-U<b#_;Ll3ME~h%)qVBHJYszt+_E3Q-
z2R&q=<Ou!vvFc!e-T4#j6E%3aW`*H(Y+#?fK_G+c*uXxa$&1l<Mk|&RW3u{@9~{<f
zGC3U^*bn_6kR{5ofk}isXo*J^gR^YRPK*8&7(IU6Pm{mj8~zzDy!lCoj_{9#(c>$_
z=?FQ}%1HdsF+0^NBj%vT*G}wdT060hw00av=xLn6TcgPu9syWk7=W;EJbFr=b#roh
z=<BA3k(IR)NL8^`qLu*^Rv_gBD{b;4tU!W;bu5RXBY>Q$V*iqZgz&_^rdndxb6Vms
zQY~>*IW2L7_(l<j!;D3;6!wirPsy|H)|?*ty6GV!!MNfggN0hcUXO7l>Z(oq_=OO9
zn#v*PbRe!p10W|xdhj$E0O5(fPPGsZY0qhi(*`d)f#d?nra;<gvl|F2klg@Q+8kJf
z70BiQYwQqK;16r3rAMJ*y@q{9D-Hc&-*{}4JnL@F*`P0HgT8JyNYVObEB1nH#U4QK
zX8lmz&C_C`XQ6UPodR(=6{3gn%~cqE3}C7y4lbu94m8IT2YsQIz<qj^A0!37@ayFr
zgnSnS_>7X=akSZ{HSbm!o50JBGz>iOHh)9XY||Q1s@MfyWu#%?vTua4v_7O%k?j`N
zQj1{(h}V<>?d#3u5g<;&O4}PA8QX9j!@pBs<hSvbfiYg5?`dIQZUlgHH7QOQJkQgX
zg7@*XMd1BBtslI<r&Yo4@U;10LKeGo!QWLraSwQy4ux|Me7L7A1s~yQi@-;ET0i*R
zo>m1P=V|l7l!9{)>_;d0Y>;ZFo@D|9$$c@_@|g|oQ7enZK=J@9?UOApi-GtAEA10C
z7I6TG+pu=4tY6qn?FateSJqPS<DRw%{Di0Vga7DhRq&IZHXr;KPn!!SN5zkE(s-vO
ztRG0h$|x-bQ?No?1g2nx)(@s&g;oVqutJ*;reK9O7tFvE{?TA)<kK%86)Un!!Bni!
z7J;c)q4k5QSfN$HRIJeEgQ-}d%>`2pg?}`tgUqLMlrJlae5MCvK@s&Uz>J7nTn4P5
zSy4n7$e1``Fk>RLWnjicXiLG2iO?2-855!PgBcT{Rly92(B^}`Ym;c;|FQQT;C5Bj
z-uF%+1PDk6AwUR05~PFx5~KyCBe3gXA2=n2UL>>-DLFx@O1*&K1>NxqO0j@~f*?&n
zx-@AQAqXg9i%63q-<WHS-<a#nIrqr{a_{$j@AGo+<ND9B{-e$@=PG-j9UxO8wAo;$
zL}W9;Oo_-20y8Bdn+j%1MAif|B_b<>nGuog0`{{LeO!2vu8<$-vG6mZh;G$W^4bLW
zn2NHW5q(+L&Ifudm>8~gS>EbX2Ck=~%*6XTSVec3f{Ae|nq-?#8MwQOl0Rn3HuH+^
zJnoeNc!<x4Ry;>Vjs||tXmvJ)hUVyK;3I7BDASwI6lmZ{R)acA_#_oMYv7BGR;HES
zDA2$iR*vX4;ttK23OMWF^(>`*c2r0O91YB3l+Qso)l{f}qk&Jb%=bB8Ar){mFe^vC
zX~BY_0*(gezDZu>0I7hZfmsj|TiD-@7G8mt)->5yijND#PgKQoguvP<o*RshoU>rN
zln>6Od~mLv52i>SGk~O)U4-RNl5k1=g$g87s9^*`ZRhxtTFBrwU*2ADW({ku=|K7|
zRw795v||Rdd!n{_%;3yiTMW~IOk1oJ=C`p%P0tr6n(`1z@!VkS<eUY!rR;F7ogHSk
zFwuk<&K}_<W{>Q!7SuytH*f~ysRyty{@6;G1wG017Ar>+Vzqc~Fm|Y1$`0q++2M9r
z?qPw1$UQ7(*_VV0Bx0!Tyo}V{Q0f$Xip-f&wsp<`5>Kqe86wT5C@>sG9a{;LNLdLJ
zT3N%y^FULT;_7rnA9BMbvjL`~T}TtsHJ1H|GaLFE1MwGBN0c~2rcrf-6NWNR_nif{
z|1^L@f0;l04=L4v%c#iR**02?_WYF*f%~W^Co-CTml1(qR8dZ3bhan93;3TFa2>eD
zotW<d5~4F-w}7WvW_93?P1FM<>Rgx6$(C##c%zAWfFzRZGV*CfcRb9)Y|CL9kQ~@D
z89!hlPXiJpRL0)%w08A;)#_ORGV5|5eh_$=Iu+Se@M2!p1TXGoW$+SSwhMS^FPj1;
z!R+yjK~stHnLLnYbMb-q(z$T)fh%6t1lPQ*3~qSYF5pftn*ye)S$qs;DpBGC(&H{Z
z@C@y27aw?kFKdDi^s+MeU@zMRe7KiQ0n=_-d<=XGL|-zGd#_6G4e`@68_bNj-mco3
zq6OvU6p%TQTxWxs6OqjXGbgT2VCKZt3Cx^`tO;gLL{<hfB_i7e{NFY`RUlI$wAo;$
zL}W9;Oo_-20y8Bdn+j%1MAif|B_b<>nGuog0`{{Ly*p!e*wW-W17s$+mNUNC*2;7s
zb%4s4Jxr_ui3uv>qqNn83M6c(?KT%{r&6au-&r$Loz<D>tj<Jd%}jLGL5a?qn&_-%
zqO;10&e|o>SyQMW?IiDYfOM10@Juk>B(j6Rbd$)Yg6SraHNkX~$jV^4No2c#nJJM?
z0W(DsUN}2U#kcgD<DNaiuiJIVmh)>iV-s88De<xzVms2r7PxM$6SX%N?|1b2<AMM$
zXF-&&Q20|l7GAF)deF0he^OEQdWB2<L4`cPr`w7tzpro^O|9Sw(HfR@8MwKMGEc0e
zGgRnu0)NlzF0EBrQ;!8th&Hssm4UmeDEZ3hajha%b)LanSbZ7|g>-<Ufmw|5!=bxs
zDX5&IfiJL%HLg;~>U1<PD@<N>uwbZ;qk*|^l3%n3vX~qV%z_YWVShVX*x=zlS-kZp
zexfR#BLvn?@!Vj1<eUZDrF?KM<%4tWeApC~+TI-ElKRW@G>}ZQ&05~Egb@g}o#P4G
zFY?S${7nh>prU6swC0))q~BsCg5*v+W-z<QYqQ4;&djkDG9AdY#Y$m*+Yz;PaB-q3
z51|y#4aQE+S#Vp*4(Hn0NlY}$GMqiaOUxeGVJ)b~ZKI<#pL%f;#vfY=v*0#_=`B`{
zCd6v-++gfbxs)BwwX?&$klerk36UEZ%(5>PZi|Q*YCA7$YA;Ehg7+Jo8LJHWUO$j{
zVkOQHY4lJp(-=k_TOl8kNLdLJ+Cam^^FULT;_7rnFHYc+*#OhgE~M?!HJ1H|GoSZ0
z2I4QMjwo@4Orz=uCk$nt?mL^@%}a*{aQv@xk$XUDv=|+vQn|zdZ&s1~UjHS143@ml
z3<3{VQErgYL!JoS{=uSWMyow+7J+L#QY13kNTm`YaLdPvL`I!Yn8kK=X<1$BKq`^H
z6k)W%z1XS)uToJ?WJEROYbQYR&WVgRzTYeYmw&*gQExHoRH;}5QnTD5qwi>4g$O+A
zp`t}bKUb*`f#3d3k;v$9o0B?_S;$>8I^X7`4&46HqD4kuRjIfJGF`bvM&H#b6(Vrz
z<3)>%u2QKGfnC2X5*dA0rOu*4IwW`9u1Qw6HBkZ57EZBZi-#J?+Z7-M%887qkr06t
zC?_(aMnVKqpq$8v8VM0dfpQ`vY9vG;1<HwxsFAanC|9NJUc*9?cPv24CZ9M0ixsR|
z1z9qp3uGe^T_DRqSU}c*uz<9-uz>Wnuz<9Hu-eT(S;)-`km$M3Va(njR3IAzRL0~d
zR3KXeRL1NNLItupKxNFr7b=id50x=3DpVl-36(KTDpVjH3Y9V4CsZJ<2bD4XCsZKK
z2bHm(rD^SE>3zG!ZUWb{h1&~cqFk>xt?dpnViS0?iF$#rnWzbza!<a~$S3Z=6HU|v
zGUdVo-eK31Ch#2-^#W-O`L+-6M|Roh1zxM7JSRra+hwB{IKOaxX|@<`r(3^72b@y4
z8Z=vsSTaHcK54p5AUz{2AeU)L1L!BTci|J-$%Ha{aw~F69|LC0ecwrEh#AXx445<F
z0hzOm$KXpgft^5RE#on`)#j@c$h>7d2F#rJ1ya6@$KY|RWhamVW;_NH>{{Ciq?#G8
zT|K$+lRH@;CCrwae65i&brdR)Dney^j#X<qkODzvO!i_GNbXP>a|b9?An8M8Oh*Y7
zNI9UktH^l0`*Z&y4VWEXzSj(-Dj7{XjNM&mK+<$HFnJ0M$foXSV0Lw(0ol+U4a~((
zXh1G@js|Aq7aEW~-_gKa6om$){Ei0Z+8{I_rFS$i*9D;gxg=z<>3tt}F+u}!3*%^D
z?l^=7WKtb1{O?ws$Omjf<=vj*)os`A+)Y<(Q;+U#9GDrCTLWf_+YMQ`KqgFBd+M&+
zrAKEB%!CPTj_$c<>Cw@^Oqe*hL+{;xqen*rGht%wIlXiLiyj>f%!G-xus-ac%NFVb
zrt#gk4x^IU<?s}(qH_qo(kfeKLmjERfun&Z*$z==hq$Of17GK9%#)v!&ii%B9xY5A
z_)jX64C1M4##7hH)cI-ZWSabxG?@}#{!Ys8OWk}yckNg7=rRW{VUJJcGZg-p9+xf6
zl)nf(y6{)6oViXJ%V&)9uPy!?51e~jIh*GH4#d0yNB*gXEBg<N7EUOQZK3?08rl3K
z7+GgX3-u-Ra`cg6`MxUOt;e`5ocXOXQf1-XXx(H!QYa{oQ6nF*EU0hzabmebm2cBy
zTo%qeq>NNqIJcrcN`IuXpuDOY`G{pfecg`}%iXFxUysX{jtLeKXI@ptav9_NDI><(
z@Az<PGktHcnR~4@V&G*XXA36WB0_tzTKUXJu_CJo{T)A6H213Zi+YR;!<oM-BT*Pm
zP1L9Tk5(AdRx2O1Fq^6V_8%*nyHxuxdR#*KLI1rRM4Rbb!jH5%P@b$tKH};?eaDXz
zOL)<LNgs&0_`53;m<Is)NwID9D$b3*qk*}}mrq%rRvK4&M+5WZB-ZxRt2kGBM+4tw
zSNMIovU6E?G%(Li@-Kt|hw4jYM;oK~GX)uVs=aKk&Q<vJ0u6kJ(dxJB-P;p|yEyP-
z_5@vDpm0Rt_6)q4y>_jR(c7}m6mHMJ`>RY|KbG}g;ef(D1NcIf$@{3=6mpB<vIp~2
zBt_z;;J11`cQi29dU=V*jrzKJA<bxt-zdnyTn|N+3&~&gV&hc7TpxtS&bqknF^&di
zzZ4q#%+k6h9SzLxAvD(K(z+%xnqron(88{7>OCwEZ%z&D9|gK!(q81@&C$R#r+j|T
z{iMI|?|wy_hX*yM3BF3ZNHlp3<L@!Mfjql8P4Ls!Yu&t;@wcMg`)Z5vIOjCM{`RyR
z$m5)&fw#22>W<&%bdS^C;R(-af@vIi7TZPpgeN>l1M{>eG$2oSjt1rlPiWnGk>)+2
zqk(xxC~q__>0`iKLPrGihA<;C;F&SAsBOmcqc{$GhKc7{P=B_scw24d=AkUbq1HT+
zr8p$WVp%bD<6zmbR?;leWU1&svDPo{Fjv+BkH@JPKc^2=JLxgM{XC}du)-N%G50vr
z@5#d!g*_DKKdRU-I@+Hn;#4#@Y4iU=k8w1d@pBVL^Mr1{&*(9?a#SHF&iL7jt*|E8
zbJt$zvy-h49#RIO!dhY99fZpDx|V3G<x5Ap2`N7NwSK1iGEJ4@Tp!#pDW~{I8GZ1>
z<)5o{@gw-~>AQH24UTJB@f@2Duk?!N*g{y*#dBf16C3v+@|F<DHqTo^#-t(CFnXv&
z&U*mljLAq$go%hKj34bV_Gk14j>}dtPu@wAU-29X5Pk6+7cw#^o+D~PE}ml^h`o5O
zUEKaA(_T1}3MrEYH=I%(Aore78I!S4NtCO!h=Amdc9=KvFlXdpzR1H|L;Yf1LL0?%
zgRzry7HpTY!?|{L*r*2ha1XOwzz6Z+9<wjhg*le*uLNJhBP7}@`Wlyfe<h45wnDVr
zW2CHbj&NfuVf1?jZ$67t6}D9IT-a8{bA$1ba~5ou^1->356-po!Bt)+3fOPy$rmd_
zmsy949CzH)iU}Y$uuvJZnIzhR1<tA+s6Z*MCVE?#C|q`n54Ul&v$mN&<OW~9x@~le
zGn4I|_Y5E*#7dlD>x#ayC5AaifnqCRu%X64B~5X)q}wgbu9UPL<^FV?WC3&LH@;55
zHW`Xi=w>&4IQxYjiy4NO*y6dt*vUByZcEwWTsu45IY?!Igvj+FG@B?Q*>z}k@r-k3
zA7#i_FoF1ul{iC0sb&kKi>-vIvxV~5B1;^|QyIGfW1h@}3go#ADr4^85*tV8SV9f1
zCt)0^D?u1jY$XgaWhD$Zwi1@@8J#)qDT`gotqB`#@f<rT6IDDn7#}%j!FDMhoJ;xO
zTst2;A<IMoNsT!TT_VRF?<>RvkoOf(8IygYEhrjR2P#mCtBFR}6S!RJ#5Its98oeR
zgaM*_M&A_$4I|q=XP&cHk<)?rjg>e<1W~VwVa^d<Y$cpym{fhBlBT%rN(z_R{&*9Q
zwLR=DK)?M!GUrT18M5^P2_Y^9XK1}tNjXP>Vk==uVe)h^P)So<EopR#4!Mb|3zX~h
zeV+td{DQw+Z8>v;y%C%NB;Hsl%#a&(w&{T~OmW*a4P0*XrD=fN)w2#66GCDgV(qi~
z-ZGv!&TOj;sVES?u@Yy9Ak{RSBf8j1SklkyD;%y_#gc|spyIi~*rD7hJDh80hwH4=
z7f6UqMQAoL6=b)UW*OImGc{$%Q~>cCD{+R1Qd1E|7h4HapQ)cS)Sl$)ops=R6}c~4
z)?19WeaeWydsLJY8I5|{h`>u!loJ_!^%)}qulw&@loJ;obCsAMvo!Vr61cNfO{7r`
z(~vJ8#W_^N9E24oPz@6h*21HoM)ZVAvoR(@X*M7cLS_69E9NvH#e~`}pu4mc<h!dt
zGL(-T=Jhdn)h4zHTwlFrJO*V8r3oaIjMpv>sv+O*0#XP!hn<Nz><r726DbOFCl-Mu
zlM@*+eL@5ha!za)^IxrY(}CE`$}ZnHWlSw((F5@TwVfBH#eFdl{JeF1FOX_E8u-_?
zY<q!J$I-wS+uZg7iQCb@1T5ct4r86A@tmSZ_rVmHB`M|Y(RdEiqoaWdMra#pJX`D0
z(ZB?gm@E;6aZ#_|=uA0F;1_K6nuPB|Z4#;vt4UZs1X~Jk6(R+HGF}OLtH^}`{*tGK
z-Iw?g%xg4g5_6Wo^q~Ay&;uF|OV!c9tW==^X-P)|6O7R2X^1~9gb#kn!mp0h{d-M4
zI&0u5DwB`>rz)IPpn)&<v~Vhy($->kaMr<o-n##-H1-BZ1OL*NLl4u+&fsX^^R;;L
zDK|yf!{X`&`qdD9-g>acaFib7PxLs$8qu$}-B|c$W2>Uk?xV-p3TIeCDJyGhGVALx
zw!#@!SjtK`85G{fAOC}LRNY56C;G^al6&8=ozV9ix&&B;#d}5Oi^(aT8;qTtvtYZF
z9nQ70Lrvs*4P+nYBFvc54C2yGmO>>cF7YA)k}r-H7GrRWC{Imc$kC0aaKi9oD<sD9
zN?G9?nZ;JZL}Nyncgjy#1ER-ou{T1G#dG0;FP<BWkDRmMwv-RfrF?L%oeygLv70D#
z-mZx#-~!v$!dDcWd0iQDw*w^FU@qmo250DtXfB1rIp!|55>_{6glVSygtf*W)(4vV
z6t~?R!sTLHxGlt0yW2ukFSiZ7-wO~a7H6)s>(~q+{-ip^w<e;*8Qf>p2{>VOe#o^&
z?*Rs?PKvA5iCzPRT&Yew*SDstu-)huXYTY>0Isj`22~KH(9Oeo-G9oi{|(^lDsqnq
zjYx5BC4ZLv4?W5s^8+tXQChJyWM~$|+Ikd4;8+!<6_*JVhn0>U*$P#qsfIMx85xB;
z=Gi>l@p=@mz^zPF18%LNbRcCtdz2|QfL}II4fvIQwitca6T>M9Z4E0OJ*E{}%kvx)
z<$1f>y~pe~fDf9e27JUs4dA0HN~gX+&%W&yfjj+<_Z>Ch&MHdV+EUMMqsPTdN0(+E
zG-7NE0cJ+H6Y`aP#+Y5)&9yrbR&c6XoU2EfY~Yn9ssXQ3Q643uM?5h!e~F&LfK1TD
z76bJ235tI#YJn=hZT1_$_f1qY_#8!T3{f~#kLjY0(X->dB5;<8YQW<Lv~{qaJzS4A
zK?pFz1Tn^J+X*^PEncffnIPa(CaM9SR#BcYqan{znp!wP_oxB}vI$~<e!B@8qjtxe
z{RVJD6V-s5n5Y4qq@wf$mG$fiUJ<zJd@||*Zu>$m8rarJW~&Zd&TMr9i?-^~njx;o
z%vJ^XU9}~zx32GFP_!2RlmyleGHX@f=9-fHtpTuTt2&6tnnd<#%d!htwACDhtxjyA
zC|O?F7j_i+oUHk{r)ltiS&sD^G@hj`#~N@s6-h4+iNeCvR|@9A=b8C3@In>IZ}Sal
z-J(A4*P{y!jC1)TgD@i~&lJ=`PH0;GJ`*+4aeKm~aq^F9B3NDXloRrAJIaaB=nESB
zKMJ{m|K)Q9zF*9B3k~)pJ<9td;K?c)R+=5xcZi<+v>siJz^9q5F5p+q)`_vLx}Ka`
zumvWV#BY%hIxUiz5-RheuYy5~==>Y|!ffOZQb?xI#5YDwOyo>a;;yr-vv;I5a09sP
z@7bMdz~xnx-knZY8<*<Q6&-w?*{}Rc;S+jXLcaK{f8mAbZ=MY-sq0~-Bm1J|g!S80
z&v^zfWobk|jy+LN?PZh(@MRO#fPYd^I{E$e?1_4GA%M>}V-?^x3pDUcMvE5j<$CHC
zJr-()=poat0H0P-vSy_h^XVO}N4X>ecQa89xVwtd$xqU=yO?71&j^Q<<x5A8ym0u@
zqb?l2=BN+kztuJ!7foxZGqiN{s^RW3Rb8HFt{cE#nWzT5TSa*qjHbP0oerF#qIAAE
zy++Ns$p&Lo{?ZBf88zl;U>qd=AnGEuin=WKFl#6`Sc+vJT0=`mj%ud{N9KItl;{t(
zLg&(6V_Knmxpai4KcQt<#mZd<PB&2pkOD~_(WHl#2GdNTPNJ%lGzvnHI~L$T(V*6_
zB+9^oS~M9OD;;&I7!b8YbI?C{$coRv>qW(%tKKfNAREBHnWzR_prX9}8GY?#TPMJ?
zRg_*QIDJ9Qxdi~msN@KoqsAN!jDtieeyCO-wD_w)ET)<VwL49%3@oTc(}fzGME}H;
zXzhd)nZEJSzIVe%OAIx#k%j^ibZS#T4NfAlm=YznvCiEV)+1%$StjZLGOMZi2`vp~
zafdpI9Cp_z2tgJ%aG+>Vn_)?mfd#dwTTna1)T+RO+Q@K=oTv03m|6q)V-=-W8j4q#
zVjXy$i7LRNt@z3CVO4n16dS--RFn<^#ivcN4*Z>o%D|$n_Cfs76f3}@V*Dgp`a`}+
zEu%-d#sODWQ961QmovpWa3vE}fJIwz^w=7%ik~bUD|b5iUGAiD$F5rVbqgp2xm@We
zdBEyN;S|1C%qZ@?XVFmDOB2=iwibK?xRZ*~g+Ot8Q>+7bG*Jauv=tWuTbrq(6hdzM
ztPsl<3IT;6wGc%Lr||6yQDlXMn5w={u;3fOFRLhB2ow)B#X9h46%8vL)7N59v=)~F
zYbTqv3b3dccl1nEz|9KTPo!jj2s^VhS|6P_d6JSx3CW*Dio-GTCbgbxt}|rPIM+&9
z1-(XC#CdAB^N-vEG=N8_C|yJpAF{OTz|X0od?jZJct4XJRRR|s#C631>>;ITKosQ_
zPyBR@-Yphb1&F@r&Fy1=F_&gMNmyIT>Qe<472~&EDB?yQ(t#*S7}rJ*_vqbk;WvTk
zi{2&4@Z<MnSX<Y^?*uY4@{R*YV6GIM(e2C28WCY6zC1Wr-E97fbrW!R6{Wig#UEOn
zb>LW4bln7=VzQ%3;G%=Ln{WVIx(SG4vYXJm%mS+b(a*Yx0mjnZgtb4LwJNZv7<Utj
zxRGuGqL}O^^sX_7O(6PNH!;AN3qS5AtSx5K+X-Z5q?>?6TX8qtr4ivd(M?m;&Ee**
z0X#}Y>25;tU{kCEk2FygShN*4&UvcPGQ|e)CKJ_w_o*lyS8S_}t$WQ@7qDn6j_WE_
z_=zc&fxk9UN4VV+=bc6=1MfCb4M+ruhyMA>{<~>4fJ^*|#a{!irlNFNVq0}=ebQ`o
z0gJZcviwWq8D@%Q;3_7H%0isW8>I|f*+exU5hThoQQ1eDRs;CcS1C(3@Maa|WnuK2
zKN}JFn28#|1t#hSF7=mOF<oUsd(c9w199UN>n%ojl7}u}(L-E&JiwM`dtgy9zR03@
zh=o=K5{&Fp=k+nbs%$nu9Avz(>#<M1@^J$YlP>jUBK2nIASbr-aE4{j3B;2;umEu?
z!Ol%2)fxKCc%jRK)YWV~y0<IfxfVtlc#4X$&zicm@s8J{(*<8`ecF6R;Rgj8xNiNo
zPp`t+ddwnGOsa{mJmc^y-XYM(P{!X9-XGA1P{;L|#_9z62%_JWFRABIh5W#jNZu#J
z3p3h4!pU|eV77Nv>oSXH^fA~=g(IEt!r}4*grS#=9!@$D`{-(+o_q~D)Y?#;;at`Y
zTEEv*A_D$VMXQx&=?6HI#+~SYo<|;HOJm{eqxBx$j(h&wb4HAXMWYD2k7vbxmNPai
z=2Suh-Zv<Bu$cLQ-G#+^TNlc3t4tY4)n#gK?4yEs$%LV?Wpzvbksf6e0RFz;Zp~<w
zzq0u^fUBt}-_{xZ(i0c%w4QQWZ^cgQDW~;T>@@o5f=nk^rWN2dg-k0gMua3+2_SA<
zrWJF#j%8W_l6+1K^RKGE<9#T=i%e9ms@dbcdEiARir%+>N)3ELk8Yt8_HL@?WMNE{
z2R-)<L^#N(jQq*~@OSE_NN3g-vnXeT5fte}Ma|^N2ge+-#N<l=oZ>iNosIvS^(Sy&
z73KYj0>@afo>GkOCSF&MtC`1WGB|}nSzW+^gNhXg8PrS-=0*#u0sQlTWr!8)DaE)9
zr>NPh&0|ysPGL~W030}|SaFa+mHwW0!7h3fkHFJZly?K8b374vQ$Hf@Vn;m-Te=pA
zqI~gdn0&80T59X4>KF9rmLqt&%H-A^ILky;;0Y#*YQ2P>BC&zKNgQ$$D-Sc|CWQo&
zW_nr0ii57t!<xdX3v4L^J5-eWWYpt{z%%<1ZPFCr0k$L#M6uoE{ZoS*tq$DefhSoU
zW#CRGsseX6Q8al^t8o$=X!1gCV)8IUZZdg5(o9cYtT^cU%+u_CWmz_Wcd01tGghpp
z6yptOihf=DI6X?1z%xu#1D>g(JP1bfJTdH*s-D88d>edBivd=%uTC?-X#T~|F)hY8
z$;hFb+3E%(LK@#<fQ8&*{K4(*s{EFv(Eu(rH2)U7+;sy#VWI|bSrz3~VYHqRqdWFT
z1CLo%LZ4Zpq1TKV`lVE%=VYngR^M38O2q)n;%0n{0cP9PZ_GdPoo2EgrG<che2q=0
z2K>8<@?tT1-xGoB{WI51H~ey{u!bJpP6FQ8j8%Z!7ii#KPXiuOpn<>QX+KqXdw~Xi
z%+tc|+*%9ss!_Vgn<V9zzeBC>tJW)~)d56Xu5-Y5JS#M`gBq&Z{FH$hl*eEo1`D=*
zkkONIXj|StOm2nNhm~ecZiV(UCeHAXF~j(G{~wH>S$k2-x1>Dj&MJ-AWwiW(1F}Ep
zxe;<E^>++&wNWq8qx3&;#2eQCz-3gFw>6_RJQ28IKVn#vPraU7o}@?D|KOcfCeL2L
z&H@d5oTmZ5T%dt3^|Xf+K2@NB|LJLAte0tFmartd$eScn{jXZ@n&A$#Rx?9ULoTML
zpeEkpPEkt5tki)gs>n@F;##R^yG*eHL~*EmQXZ!NQ>Fi0kFvSOPms6k$u-`jV;aD9
z-$GOaZmgobi5Pv}6T=uVzk!*r1JO$7jN<z~XW&NWt`|ro&Q_(x2oH%LY`{Fh$lD+w
zTFzGdky)(R3>D>-%`MRfu(4hzdh1(0-9R#MF?1&K=??3Bsn+)@J-THH{*~3f0(?+K
zi<f4*uaA_*BKU{rhA#HkQ|If^xd1P1DMVjJ9-LbA;)mdWquz?|9v)K}!4}VjQHOTq
zhC5VG+=H*E`Ha!ylJa*9N9*<+dMGRBdaFSL_{iI~f`Ct}$jxk{#b|*iwljZ=nXd!K
znN|-FgU)<CVLtk;2BJUC%vXSSn5Y9toX&jwfeW@~nqn3BxP{sUEQVT5gc{wLV4n2l
z^A_L}X1*I(G~Y~^kCsqPZA{an>u>OhX1@YFRYi-JzLYN^1Xp=(IG1De)NXooF2Hx0
zTC{`)rxq<Cf<Ij?7njM`R7SAHb79n>9a%4-dSVIPs~%{4x!<4IV!-6&xAyT>iK%8G
zWtT1n7#Ywa9_ZMN9A?JJCI7`bZ2&)Qq8e}m73E8p(Po|qoNS^7aLazW?c!L$6zjmE
zVti*%)@Qw5d**8Zzhj~r@NyO92{5|L6T`U9)KeIc$59~Kxvfy~f<RHKKe-jE=951;
zdegQ*l|Qh+8o-@~*>f>)Hxo60yQ?UVjnQF7jGl`p=qYSUo<MBM%{35%>C`_H*ve85
ztw}#mZiV*R)nkfw#U9oj4d9+8ssZ;>Q63$m`#ll(KtEzQgBR-AJ4`Y95_g}TI#Z8s
zHv?ll@zv^?@8pZ^3_VI^fq!_Hbyx%bO+|S+j8=Njh`@DKl%AEv)yxWdbg_chGp`lk
z<N^&`^E6;jfd)R>(|)M%#sUren5TuMKT8YqC!=(cH%TTQTP{<r|2C}-w3fGOMUO2n
zD;sL+u|>5jW~B~1u~74PbDE-OyG*eHL^1i;qV%8ZG5y%0C)awPMrZ)n{Wqc-aAOta
zy}{`7o)}KS?P`7_GhYXy<!r?{qxf$%AdkYp_03i{5S#L+Y9ql#`|&Ln_F>C&3veS#
zrx!@3F4k&`5gt;HMN!^O%%hclx{35U(Hn1JbpZ*)g(XinO29#)w=dPwU8P62dck*D
zSu4PYRAf&#2>$80;fx=sr!LT=a{(S@DMY_(IXJcG){NjER&T}Iug6tJu*Gv>)S(@@
z4-3^3w`MD9K4bJa;M0wAerh#n03Z3l78>wr73B+!(E?8l6DX_MTg`kO_)8OY12L3d
zd)V6F6f3}|EP@^&!8u!%79%_){uMFI&oT2=;Fe~-1BfAKK7NaUEpnAd8Q|k)s|!eQ
z&XzpNC;<<N^-@#HG(Eaz2A^d1E5OrKWREfkuJPP(;>PN!UG?Z(fPZCb(Rvx2TC`pW
z{tUHTToY%hj9`oB!l*+#vN%HZ#Co|`J<#R(gNF*4j<%07+Age`>~h9nP!{pPgr^^6
ztTNmoH<m2r@Y8yfyISA|DsmmwXffK%6M>UW)BtYTPdBVEL9Sqmbzo63zH&}cg*{BM
z0o>C>HQ-(<$`fF8zb678=tm5r?bEYo>d|#E7|p~j?1f6dLyvNm4=takj{&dQQ%3_h
zb2wAf0X)h?4d7Q)lxN83q{U1Zc#4S{z}-d|3s^T%1NfwgqT52E|Gq_E2d-nGb^*!4
zg<5YhA`#i2fMk&q8C_=ybODJvCo($O0`3BmLrx4Q;n`pk-1M%b@vo~#Cj%3@tQ3O#
zv87uB-l!s3heKKn9xz_dqY9ZIu|{6YSO&Fq6q1~<=Bemso)u;kN7@nF?uc!7#I~bJ
zk8NAr584#ffKRAMs?gVB@IR2>Kr#Hj7(<-&U<%4S`}ym5N#}1LOS<E5g?>t->S5>>
zYlIpQ+rvt8`dYE|T5LUfL-9AYI@V%}DCa5#Z}QnH><^boY^XOj)Efug8#e1VG@|cV
zNd}_@d*D1mUuaQtSx!I<W#x}Ei3&Lg3X_d#sdc>S+qQ_;>P71*EzyWlpM20Tw#C>t
zTvXlXRfUF<$2sPixk)_X&DXtU;^JH)8o<g(L^a?(CTak`s3N!HHd>6nwWR3+f1;wC
z$Y|73Mg%UWqMXR+)1C;N*pJ9)XHNv~(~rn#swV;u>_-fHidt=EwW<RtkW4&~0?GXX
zkm}@DCq@S>K?yp5*QqEcGTPCK+5vo~P}F*h(Mv|G11X~f2rMQW-+%3@N$#mfcU=S@
zYz3$Q4_A@=B*l={kvh1*^MDr@d6(+oM|yNV!1GPF3;0LVl^@z-@Rsp<-&e>C$eji?
z9&R=60WPhgY#U(kdC%)m$eg&~z|4qL6Zl8331pHa4j_v`6j=dGnuy{Sj`oagG{@~N
z?J|(G<&Dfeb4VQPnJ}FBOc-|g0cx2^l%4ZT6)`s=x=}^G{I$3^H7c>WHDoQWL@lmF
zEv`hgEfXi5CpX>1Ns}e+!)f6{+;Q7j+ypBLhh@T0!95ilz&#Z#xL-jxP^M3OCCV1M
z$Izdx54wQZ9#-n>YsJ>PV(ZaIt7}9tMU+GX#D=4)p~aGxzsE)Ebz4CDjMaAWZ4<pX
zxKs0f!15oA7HmpXs5L$lQ;ik{mh}w8P*!U)@us4x4T8eVVp;<FXdQ|nYA&?~62AK?
znfz%~i!nY&ioCBhc9YR9To2QZ7*RSt(_p-|%EdU4MwZ>>**PT4RN|4bUt!S{FR7+!
zVR11Ct&>&jn-*B@X@x6Uht+<sknj?-9viA+=)cW&H8iw}RBldbtowO7{c#QXXW1W?
zzfKDmFy_xQTRlKR&#p}P<gz7iB7x+S@xnk7Z!8HL{dT6>g-Tpfte?asjW2}J>L=$*
zR9k)tc-axse~fW3QstYC#?QT0r84jV6)h&e04X*Yk|eeX>lT~*=nMOVMg65NKqAUY
zOfU<pA1-}!MPIY|5R?G|Ph4?XPyR;Zqh4OMUPLOlC)n<`t9b|T02Rp>XZu<Vd<-A)
zW+%>XpcsxS#t`3A4Cd}?rr!Ve*`q-NIL|~i;I~yINjF;zE>~gx^up-ZUOQY}59_IS
zjM4zEJ1T!N$Zav&K&A3QIdJcOL`Fw=BCxL?k<q!H2)wc%k<p!=2wZXLVvGw<uAWM+
z-ini}r;@9;NbXPzr4A%Xx$OgzVg4Y+=xbgVNZz6gB<Wn2(XCz=NGU`YSgcC?Y!y#=
zB~_J5qAHbCRVr4MgDkoVkfdE2l@_BjJP}CV@)<JlN@eA`jDG5MfyI2|PpN+wwRoF+
zncRcC2Oxp}e=3bO%}g3Jrn~|G9^o?s7Bj2c1ksqH3uIRQS9&y_vo_lj$&!w2OTc1g
z4a>}@(E!p}ZgLwfMl2?=2rTwl{I86R(Y<K09z_wjf4^;t(NUfVyxBw@z+3w1GJ4z-
z!(A_qqfQ(%OsfVY7?*s##pvgr2*h(vWaQ(B?ziTvjd#pe12|!s;<n9b8<k4ofTx?N
z1NhZ`x{NOO#IPQ59ChMY*R*Otg2~flbebmu@thMG`8Wnqj|y?%#@VX07;R}zE5I{N
z)CI(IuFJ^B5j_Q7t2SOYTMgh^pD4}?qj4&gc>(U#kH~0$PYfp^c3Q_N+Ib-vd0Wxv
z?=#iS_q{vdJtnFFcU-oZQyAAgRai?|@^l?iz7tWR_hPpNlqE~UCXN}N8H&D88ghq~
zM$T(XB8lPhGkSAcjC`<BlTFn;POy9$z^|LA2E5cn4d7)ea<kEBF}mM~k;jMi)Kf-j
z0Dotq8t?@ZHGscYQSLFU{sKKaY`J_Ir3rx(Ow<9~&_oU3Mk>n9Guq4(fz$dC7am8Q
zIL4c;8W87l{RHAUZ*xYyUKd!5Bffi^sS2l?Vgq=SiE6-GRW!VG(zSgpMz@*N%`v0D
zc=a&(*l8W7KR2xoAO`bH8QtlLz()&l)LV?6@<iYt`w<zv>WM&d$X$nNEKr5g^43eh
zHB3|kuBD>9aoX8>+k&VA*ECxlz@n{qPRFRiWK(Pa4>eH@I8#OWoH9Dnq;5_beaowd
z3B*q8I6cg?I)E6=3(M#jPXwM)h@;+O^i@v;exo0e(Pf?pB!~2z&QpaOOtArc+(b3t
z6DEq@!^DQ_82YVgbpSDx4r;zCylaXL;D4+@@6~``S5e+*j7F_!MBuV2%887Q@x*YB
z<2dTX@imL13y9}DCq|zz^IbqZ=R`(6j_48^#^FBAB9N6VkPhG)CaM6}QIXRGZ)vm+
z;Pxh}0Cz5Ef~Og+19-5BD!?NOn&2-RtpiB;<lE{%YUiQ_|J`fG6^rgDs16q8t!Heg
z78{B#&lviocMqhF@~LTG>n=TC2s7*^_=%X%mM{kq6Na|&F#(yOEGA4%uomptRbjUm
zg<)c1;4ra?L5UojzLBG9VtaodIgk#?B9E<i#MYxp$Hc&qhxwv)nvV&HA^DksK5Ilj
zS5W|SwW_CCbsNCbRg~{NVGQT#*>8A7;C~LNI8V=B<rRTf52!e1<$QY_r$_g2qHtY3
zrbC^mXD53_VBJJDU}HdAQ}ygYrq}@9W1<@H-T`ertY;rL#Rl-V11iqfv*)bJ1)~AH
zNkw@j7=3m%BZl*ct$(Ubw}F8#UIn%CWeTtLBH#@w8d}<<1zyG?E3cq%tR9^M@Hmrg
z(gJVcRe@U<XyCf1bt>#B(7>}j4S0NkMu8smwBIUxra%L~>S@5&3pDUzpR~Fyt#Elg
zx*~zcdKz$p0u8*4r|qb4w*n2^^fch~0u4OV(}2epXyCIw?LQP=T%dt};AuZr__G2H
z{Gg{js_@AI4g6P6drje61sZs1>*n%G3RlykD=&CMPupA}>%h^%g)l`qdze}Sc+wc=
zvjaF+Mfo~obgCzY=Fd{ICm3aKAlmZCf8I$gM&8!mp{<7&p{?Po=eZHgkkUtw>$8i{
z*33m{>${84*0YPy*5acVUF|nrgtiV@gtorD2yH#G2yG2nW6{-qlSOE2+9I@d{vxz>
z$0D@#{vx!s;hKwX9=jKzt+N-Qt@{_Dtv45;tqE%_y4p7vp{=hiLR&vygtq>=2yKng
z`-nwx-JZX8K94WyQJ$=TFRLj1WECscQ;PBX&jo7seeV%CbZj1sJe~uGnJ9XP7aOW$
z2rU<S9Ko_`b49b=0DewI>6~|8Cy%eGM|lYZ>{QVbrD0Rv92efTW#g$zxyZB|!1q;@
zb~`6fbdOPJ-fG4=fcKcF6ZnXVvhPeWc+z;$s{yP-k&n2he5%byTp>645m!h`eDYQr
zmlyXaJxV76|81gf;IMUbc3v??^Hn;e)D3*sL=E7PpQ6p8Ut%JNBg|tR_zx4+fVh=D
z03K$xYrvT%Dg%84(Mu;HsG7S9uw<$90CDTeP-!tDclmZ9@b{)$0s0`K?+g;eAr?dx
zh(YJE+G0d$<qJJP%;&^tMX2Rj=AjA1rc-PtJTy}t;(yzT955^YU<a`1p)=v3Gvy)v
z7oWD#*e}(iTbJOUSYG=wD<gCY9E}KY<YZxqKBp$S%~TmRrcfFW_@{z(FxG_@8c(S$
zj3X<1OcZ5k$UVu<iCFt;bcgFvHk94lZ73l(xuHPnRfDoy0P&Id6SP^%zQKpl*)EKb
zn+zi)C44TX%YO$wO6uiyK0|KOXGlu;oU6VbvaP29d|X9tb;Vz6Lh&k7tOJXR@ii62
z%T2KYEGoud*hBHZOtA_qD#kx#f#QXx*aQ|8<Bb8ud8XJ2EGow9eZDHJydKw&25_{B
z@~Sf0$rFJ)_alZY4iAf)VjYMlS<67&IH&a%qiww|u;?_NI-FuwzVipfrt?rS4_lgt
z3b5!Qp5xd<HRYjd9=0$KRbbIW+zhdYX39g;JgjFPn!uumxNBk$ohc8U=3#U5&<QMh
zXy0BNtD^k-kYRt6%6jfJGu{B6rJ}qA8U54~fj{d<3|-*iI8&?x@gyw>#7*9UjDF~K
zfkmfr3*r>B(t<#2<}JwRD)UeQ7Cpo*7<;IuJXBkF_?~&F0*fBv7K}YKQy!Y;;bQa9
z1QtESEf{;~OnK-u4_BCnPGHeP+=6&$sbZ=HZ`X6r7-wP&{F*tezN7HH0xj&dxt089
zoc=?PauEk!prX8+L&b^X^BuTPkD>_tiitXbofC@B3hiulnPMHdhAPS*qyWbiY}H$g
z@F2u?=4)oY0$j_?cL6b&cRM3*KKil+mDttHSAlDr`EDQvo%w1a`sjHG^IMzwCU6}y
z-vca~Zzjw~&lxAG+0kaJ2K2VhpEoCAD|&F6sy3Ef->%NUO;sd+CV2Zv40=_VcNn9?
zy*9A()49P+o57b<m}@gyX;Oj8;4T&BR7Stwv_NIhw^=UCsf^B4X%+y3%{MPn!>)^q
zQKuMW>U`8&jQT9^I`DXPEBBW`BFc3c5lZ4lG>)T^ildT<qe2{%T;3S~ske)x(qeS{
zCV5GO2&CFMkrC-Qq9#c$IgydiELwPRX4O<?)kJ1hGF#Di*4#M^?%2RjY_-K``3-Za
z^uXLX3<%E!9bb^*2%D)0n~4aUL|F6?Uyx!Cop{*6`l$x&EiAZBbIJtBJ3k=RcWHID
z7!isP!*2hlTBbeZ5$o!0^mP@@K_QFhs3VSx=R&u|a|`pIJ+_68Ghyg46NavmPkrS0
zwpGzRXERz|J4tTMfNPnk0i-_idW`69GFCT`O7&|sN|XuXfTbzJqTH*NF((E5F_RLO
zp?EGdUOcxjA0M;EVaOk|0=wx-E>z&>RA4t<&(u}2v~!6H*txI*p^vNrq4B|YS!_;b
zHZ~{1FvzS{VRS!;v-~9$2n|RD?A*fa3_hVSg2DIisT<`x?&yj6QQ~`m*tw8Ar5Ewy
zxiI$7P8NG;AQLWJQ|caTCf44O8%MD}K0xetE{q_wGx*qJ&B4b09j*6oecfLncIsVN
zHFhqHJ+za>9vVp2J=T=iW6ea}x6(V1WA!L~@j9_5`rXcjv4?iDFhc{$*kerzGuBMR
zewW5fgGubAZQFe26LOQG#H57R)pZ3?Uh(=I6$`5pa+6+TQo`#Ly?H)WkMjHoym=eD
z(YkO}7y)`~e<ml%U;F@fHc>b5t0w9N7ObB)2<sKBA8)3*fCW=$4Z>6vQ|P5{8siL`
zp`vV|mkh#TXF~7FNN?EWyprG4qx&EPeB@SCx%aKDb6D2dNjz%&MCTH>LrVJrNm(MF
z7e#=#&wn;|A*2Pzo~DB{^(gmMK%&ZcVO*Io?3#p!SC0LLTAt@U01F;oIX2<pm17s?
zK^|<dk1-L2rbM)GL*22V?$}WD;vmdfK8lPD^~8pvR|M;6=dW)opbRWb>)TODD$1{W
zJzzm^ZqyE+-$L6!J5FZwm01_);8H!x$DF`fe!qBS)|EOacz7l9aAxdb!K?>@26v4#
zYnqxX_*f8`V?UVV69qB}GSS1-J{1=cYY8tR!qLgL%(}vq5>0c1dcMg=0Q3=b#}RbL
z5p)v)6_iJnut)!*TSaM0wudsXu!i58^{%e06>S?S16dSO<@aV$k|izaGLQf>UKmGQ
z_zH!mB1@G{DvkO;{#$L+aV><TwxWDeI_tvW+5fC*tc822;{`(!UA!UVjhT>F>OdNB
zDf!_o^NWyalIYHgqvPzx+D`pUASAA|i7>%9&^m!)DD@ipQEfKjPV{9+kvhc}T}Zp@
zs+64`i0P4T3m7qS%;@N2?fDB2BBUgPfQ07{6-1bU{Qb}P7M93i@hpdRMn)+tqj@iH
z`B#DHO33jp_66lbN-Wfnl1x28if?Bi^;nQ)ApRxmsZnyFo{+}0LQN@l*pmb7_v{u~
zQP>~YLNj5wV<yYD#{b}c9NNz6o(T24S>bM*F~eeDZk&cJ^#bl^qAGCV^+`C5+beV`
zGdH;vx*b+JYH}-dterl&6=rNF!T{_<nE5uE9xmjv4_riPa-nV6wWk53jgswimhvHI
z|Ed=QG~4FInz>S!(=yrS;5NDZp8-bBZZ9D$u*)Q6&4e^QCC!9k85gm+$2DrAldHpI
zri0*|ER4;`!qBj_f!<Td%Vi+uC8hbPyYTtPh7K}j=;3yqsq44pZlwWyP(|)ZEdDA^
ztXNMe#vi7~ij|aN{26(ySWPL$pESgZ&6HyN0Ya?UnNo~DsD4;W_g~vtRe{Uxpf~WP
z^DmsuU@aBOgFbNPjs;#gYuNv;*{=gJDYJc^*<Z)(*MSB5=gnTY{Yt|AB?<eLg#Akf
zVZWNNe`UgcHDUkCLD+95>|dL(-%QxQb`bVE6ZUUR*zZi(zcI4^(v*Did$!My?zIE>
zoX_Vn`7r;FI<1qmR}!D^ucjSBc=Fl-Sm=~_iAeWLL^>~ul&F64Iil)%P@xaBD-JYz
zw2uvSKM@(~jtxc6U$LQ{=OaTsv7zYE{zqE6pV%TR0}G`)WA;Ryh0Se|m4Sstc1BdX
zo7C<vynSH7{@mFKV{;>8XK2S1G#AXiLnFG^BC7&v8FxjT-L3so2zNo`;av^$10ODM
zX>C*KKZ2#cBEr=+?Ys1RHSirXR0h&m66kxgKc&6Dovnc~kZ#L(cz?$dF9V4t<AwWP
zoMVL?*Vh6sCI1SAJ(VJ)>h76&wtHrl|MAbv)MU0s)6WD#YM(Z-@IdPXN-UD#C8^$t
zcjHwxHF_wqjf5029$z&=QsT+wQww($Qj)GhQo>ajP4?Uw)^So4PxR^i0E>t=lMv(e
zQrFprG`N)zYGv8ev#ir(NzgpN_dM`I6IFpkm4%+L-##CVW{*4!Fq=K{Fu;f0=r5Q(
zVx7sY(2t!6r{!=B7)$bG1H|ay183i<+y2u4777|Cyo>T7XAdz9&}^Fmi|GpOuqCo0
z9C^@)u`NW`>nVAGBlVdnzdj$<;Ra=}rsWx7SqeZgos4MMM4YUht*dPB>;+z}BKOOF
zUurSJseG6b`V>*<GVzfI(JZqQ_XZ8%+A2yvTf~a>lw$mB5i3?wit)2WtXNGc#?Kb9
zVl$-}KU>6#ohilm*<zxW?hap7V0C8-T?2k^*Ibkrn$fc=mFG#|PP-S0Ve{bhd~;d{
zq9_aU+2~s~=$&JF6<|T{m9~0km|hiF(3{^@?`x*l1QzuE(pHa3%3osvSGHR90#_*%
zwX?+t2Z@!?u}fau!}TbaE8s9Q-+`e`OjHIkadHv)&2G7^Kj=|x0eu7=;RKd-dViuv
zdC~;lJ>bm8iuIIYycSXXk$J2DF(BJIuoy(VJh64P*{TBXGEog!v=y&dY<<rZn?Qn*
zR=%>2LD5>gjInm8S?dI1K&;)Ev=*=J`D$f>9^G>v_#N|52L4+`aw#4XT{|XzA@7qZ
zdXyOlZm**B?)#Cy_Fh)A2YHXcwfD$_kqrm9j*9Xi813zez|MZeu(6L&XD92?l?;5I
z1yTlHsG?-ay7c5&3!;O#nqCw}0EKuZN8rGrPgR>|n(YSgY7^Cf-&avyFGhElt*~KS
zQ0{0?xkqh6k*~L0vs0Td)r8#S*IPo8{AN!W*25ak?<}kaaQ#Z2Ou8>0Q-$aC=w=%H
zni(qt-&9dH)9>iu6Y4=WP2dVD%6O~kfJhS06%&<mhnZ>spRC$s0AEm1o)V*->P8I9
zgZWj=Rvq}9iF$w-bmr?VMmyIq-vd0|M0McPCh7scY@#}F6AQHm=%bH*4x8xbSaB-A
zD@@c2q;B%zFL1u)*b5{pmt)0pBpA8c0)1T3KRUd(IzL5^u6J%!{9HjcQr%N>#{=JI
zp>+`w)s;_okJr*sU8f1Y)XLfcY^g|o@?c2oN*yo_a+L=%4bB|+pWYnst)jV-P8oF(
zb3jVu%!Qq|r`F+F%d?w2QA>P^iWU=?{VU$oWp4}X=!vZ`hUCB29kxbh?rn|i13hM%
z*go(h`yfj9n`>Aj)70;<CBq1-7?M(z{)|?5cReO65t0UDDDf?WQCc379Rwxi>H8Wg
ziy#?tNE%w26m=P#pIeu80ln=L`yxGB@!td0EF`67G2|o)Lak_uWU?VCRkhQ#_^jrn
zp^!9?A+`-!(uqwY?8P4h?nP^1l$3M!9Tsphi;$EmJ)E~RE*DR{SA`)i_Tech=VdkX
znjYnT7`Rb`>tz>k6BXsxZAM=(VgtBBhv@=Go2U`y5(iNyh)<dA4j^vxt2?9pjaUcb
zSqcjDK|~knnOcFldUSIRKG&*KzE<I_1seE1Pb2<!3N-MN7Hauoh1clOIR)Qkw93B~
z5`L;5_ft=wHnkeHK~3(#!_tRYFIoE5NH}DFv86SG?3s%pr5SMw=kG!a&s!`rfPYYt
zyo}og%<PCYAR!myAuf(zR8yy#moo6{CTaq4mbhBKq6so-g+(yd40SC~NQj9oa*X!)
z?!MQ7Ur<rLbVz~Y!FqPC9<%0C{8c^5T{rL?6(yh7=IP1%Os@f4YHyZh2XK^%^0*jn
z<%z(p`w<x(>WRR^`Vkp@$%qZ$izey-zSK{b>TrxRtCJaS@=bx!mFA%ayiP@Oi+SEq
z21U2=JDZoakbk#=M&CVyGD@wdw%P@%{%(6SP`sJ=hV(sEU&3;WqJc7ynUz$1t@dW1
zFxRQ1#;E$I+na$hkeLlsearS{pm;NJZFW@kz1o|Ba+p$5TWML(?c3fAl!452srtd~
z%|IE*%r8{^*X_;Rqo*EgPkCHVz0jVrKu^78lm_r!6%8*PyWpf2qxVdj`Yu$dnQu1B
z=uvi6;4e*71O7@y`98$x6;EUn;J83fy=RmLaF=~;n*;8lqTD>A#k-A&S&o<=-U+1+
z+}X_6fEdiTA4cAMbln}LW?wV&-Dq9dg~#sO6?#L_7d!S)0|%R-`dj2`hU$+heAbId
zlp}@?F+&{~+T64{FyteOZk{nT-3(QL1Ua-cYcj@%oAC<9eU#BFiyk$K#iit5_q8t`
zAjHkm;^xBPcA88%Vhg(bVHl@4#*}EYJwqe9+|sQxle4W+>N{$dU$AT=Ul_W=40XhY
zIxuvc8Hy&Lsc!c-_f_D*Dw0p6<9Fi(@J$P#0wg&%adA);g7Vo#6ZZ`@ip72thY&Xl
znK*1gPfT1)i6)Mq2dE?zpI0<%ztp^*x1!bOX^|hW`nT2kn`w2R#kxp5mR_j_Z!p7E
z;7uw@PAoA#W!@@4!gdo&1U~L)79Up&sP~&igxFljEMg0KVisdcG>i9W;scZoiq9*W
zkHz-M_uN(Vn0QElva(X-dH>4%myp;j<QS3?Z_1~re$`w>-}-}6@vd%+T!o~Bt0PqZ
z<o2$h%=NBrk6eYMgsWxsO1iFHQ5(Q5O;iJJWuj<4V?%Weeb%%(fEdcJyM>9!;1pHp
zGQ|dPKNEET4^>fqi@@j@BQ}69_i%xZw%$vWeVrcN-6i-lrdB&n;jg_YjC~)qaESTu
zAtdj=C)C2%Q>HZyINwCmS68<ioBL@*beR{0w&UD3N#xdzC7)Y#&oVemU9qKTrQD}U
zk>s+aH<p|5`UHK2v0%H=EzXd>FqZGn;LIz&Kw;_&v?^V_%zOj5(^Nz~z+F|8Hxi?h
zjMxC)ZK59Fz5R3<Einxz4d4|fike^<jdCqLx+VmVw+za_4OEo9OgK>e6Qa`v|JcH<
z5!jzp<Y-_blwD$54YE^@js~7#36z0{s7NB|YcZJZc^516xkvZa(=?gW3b}`~xtN|i
zP>=Fq8Spq0m4Ulzit>epOZpi2QuN-d=Y86}K;q3*8TjCP!@$CclL|6#TUw!yD}f0{
z%%5kG-BKWe2{9uwAdaD><H9&fBrx74WX_etn1~Z?LZ$)d7I~qy(Adb57<+T%gip3Y
zi;KC{`^i>l)hmRiH&DOy*^tuoc9e-e>X6c{C8N*fK^5rdxhI~#XyIL~hQ6)GA*E?(
zUE1FLwR-Axqcniuo6frL0{&1%`R2mtK2HQb(vQgKStB-pOU^JSz~xkwJ7Kho5gWi4
zP1FTkd_T-JfTx+L3wXAP8o;v;Kr!0p;}X>=5sA4_>n%p)Cij#;vdD>yHns%nz$GlS
z8jz@ST}D@XB5;lU^Bi&_qjOa%@85wNYO*<z(GFIE8t|xoL`J?=(GD2bszR;k7U#6m
zVsxvuL<f*E=0rwRQofh~q>MR{5#1p~Al)HPH9)#USU@T(EFj$>ETB(5y8nukuaf-f
zmS!)I#9Z{%MDJ8db6M+>ULd*Vx{OFyzQYG3-JHmXjD-j!<DAHdq=g71|C|`^W^oCc
zl;A2WK{t>laJHJ(5)XJHuvn{RqE=06HOgw$4Wv4`6GjwhNT~^=I`a4hq&mU^QW{|a
zDUGmzR4MT&7naD~mQW*lL)I!O6P=@hPqHlQz|&0B3;e2zhL!q;w64*?uZ>p+lC0c`
z0GHCu#W@9U=QkYS=T#(6qeEJC9Z(g?0Z3Jxxo<1&hkA4}@I5LMRp0|E%3M6F1GYBd
z0ms`eR|oE8qF&%ZCaMG36D51#hJGUivLOl!$ZjY>gsb2oy_jBOo!mpKKjcNs@G>?f
zJ#?pUpXf4At1M@RdiPQ2yC=H2i?wR8R+I-?7#>zSG5**C(ux*P4Y;<7Wb*o23^10c
z+re51Iym#k+%Y?uFr1fUWSC=($n_YA2boXo(-6V};!;?lLlK3uK32Pv)se;YH|;p|
zGa)UsuQpmT`p}%zD>Npe&{U$TtgheMQj%E=mr@o2`QEHSXF`Ic-;=@OgXJn=gdRi`
z+80r1JrVzTTBVz9SvG)g9%y%Mz%vi>H$jaSqf1pPADaW8=tm4EGj>|X>0Gnb0mO~F
z#?;N}O`Zt6v*5JeVnis3x2|y<72<f#lJ5Z)<EU61WGug^1;nEZIR0*b?6iv0i_B>^
zu;{d!aN3=68vhXhPOr0RZUX;pPHVu~T1eSiX15p=BWWfgsSyc<&+iEsQIyPP*bu+d
zuvXSs+;wX=#b;Z5Rf<BKasvRIsv(ao%`APXG<wqL7Lud&%;F*+<NoN3i0+P5!m$33
z2&-4anNbK8Jj+6@ETP87gx*6RH>nSLK<;yZzGUa$(iiI8qIz@GfxJ;*hR*h)Fjrxb
zwzqy#{DBdf?`#=OCviU?(@C<+98Qm8m>$P4JxpeTy4ld&O#>DpNZ4*?$OpM!thHaL
z^*N1yC(C0Ru#nmy^1u*lMt*%D){5NESGRvM_YL5yDspQn{!Nltv7S<lzxNU=R#J-b
z=7r+R7HSoU0cl`hG5UB<TA()GGg}SdIfroR=>?vvqP!9|I@E~aq+))JgQ3)cUp4dH
zKr@(KWNc*Sqc`1{-`vbsfWuWwUdqjj)@`g;Mei&Ns|RR-NLbaDjV!F_4mLL5#C##F
zE8B*(lBLrLJk`Rg0WA;-t1}T+bhG;zHCxxC+u*^6n9DNoiz>=qT%V|euN8E`Ew2mw
zPEq$p9sIJO3x3+`0)JQ3{ksmf*Zf_~Bz}EC2L83xsSLbdMOhxd)xnDeUGSS;7x+$5
zH*D^AG`3C6*31%^c;&B~Z|obvs9IoxyS(f~@HCa>+Ki6&%rKPFCK?NUCVxu`EVSHZ
zQOlusn%b5dG|G!!;+}13WpAv<#pP=vn~poWG!_A6_=MaETsVB>m~kV=d@3Vwf{HAc
z)u$hYQxwCDBiK*9CUiMPsZ2L4*yv;iCoU`lAuKEdp`<gA6J$VmVFp4816@|PnTfXD
zG=SeVQ4RQg73CW=qxU=!IP|c5L(Ykej!>yQqX3WXM`U!BCjx)ekH~11!`b+w?TE~F
zu;}VQ;&IW{Ta4B+r*+_#Dso?U2IDeUZdXrS$%{|mRTfAWkP13ml@_B9JP}BYIgt@z
ziWA^bmP8kjqUO4czVC@ZikcG{k#FJ)({YKbBzukJ&<!L#XTEC54zW18fuxrc8Ihs9
zN(CO{b%CUp>oWSGCjv<?Co=NMMmGa-vQ3ixo+aA>EGFBuWZ(BhAnCa{n%0XmE#M9y
zx#UDfS9>CmTyi2KpIP+UeXK^s!k16bfGm7jvA|+x4a<y$FGOH5vxa4Mga!P+um_MC
z6D6;vfW_z*@g9H+R=KPkV6k`~V!<=ptW>cGWPaUjS6YnxY)7BQ5gqgS@u!yP*s8@M
zka%2ljYM?OuD!GN;@-AWD!?8U4J{o#8N1h8H$`7|+bgit@5^qX&85ZW0N*vm)*%*E
zR8}a3C|3<H4!x?*jwtUofW^l65St^VXIB%8KuY9FUu`|3h4PINZE!^Q|3YKL(M64M
zyVmYOn}Ra%5fx>3;9<?D>&fFxuR^V#@uJ^pdH&SCpN<F&_3Nk5E95Vmi>TaC<|swl
zdQ4~rb*?IMuM8xKe3$>N8sVxUL|}2FYqn?@E*3%r78@r1Q4m5|Xm=0_mw~od1VUl2
zXuEBMRhh5J{?&?G2L3}u$yOkR|1`Y{!BQl-lOn7W+qc5GY5+q6wgO}(uI(I#X`=Wo
z^(GB6KI9PWhpr)tO#YzzfZO0BtUPV5<6)>M;@LHB{MIUDxd-xgmbmAWO%GT!9>0yk
zIJZ=iG_a@`H~$!Y%&6PrMgzFj7wriIxTA{liyou1JQ4Wyendw9@I>Id{fLa7Ji=T9
zUr<r*n$f>JF|1{rS)I&2XL0lZ$t9mVMi+P@kX&*iqaPcw4t(20JwVdSbr~IKIrIR}
zHc=h;oQZmX6d^aq=uIQmfxb%78@ISh6{@s@CD03`F3w}6#pohW1X7or$cWO&UIcu{
z>jJ4uuFHth$fv77N+afg^DX&aAa%*jF`_i03-nd$U3f25snWKVKo^j@IFD7U(z%`p
zq%Jv;5v7sGLE!6N7f4-lT}G5fZli&e#(hyotMiOisS8M5a&wF*jpzb>m7*KPxR;t#
zX=_WM16Zt5)2ei~CjzO93$58=^qMCEDMC(U^pqvr0VKVg$jE0FJx6XC?UMg@FDXTP
zNnEA>Q?4aD==@Kyrt7AC&-Wr)^Gz?J37@ql?2g+lx`Dn!-92m<<>=2*dLl|R9Wf<x
z6jLHchb`PuOo<%Dl*mySi`?jrk8Y68h@y#}j!)83U!g~LpB2_Tb?+7G$xVBxCZh1l
z@U#ZR6;duwKsv<jG9Th%d82j@mqW1#q?>Y!jM(TC-#a3@t*j$!z+!X_+kUx!lT3kh
zwR75N5gqq$LIe^|PGsbxi)#0**6uHQlxs2YpDJ>1M6fj6)=)I(F9nwRJ+_247xG2~
zTOU|d(d__~LX^=Pk=Iq7n*zBa1B<2q5cda^o;v`s2(%JqcO2_$p?vA1OEuAL^}pX3
zadc5*lr^eui>?furlRCy@LO8*QKna+)`xh}hFYF2+xOFOWn-vcKZRQW<W<Z?RBk9M
zC?)kqWb<g&qPoPC$P+<umn9E^KuY1(XS21xhQeKz5P`)N-?SCaU6v4m#kPwdEeU0z
zJw_<(I3IuW38!eLCVP@iQ5kr$ijoaQ3LBeVg<vU?Y+5YLUwKv-z&JgHp#d8TG85N!
z4%>OzRK2C%s}L-^cH0d+G>#&ZKj=Q-Ld=uAcm#5R%`X|DM-+M9l`km*i;D5978GHn
zF86SF0FvDlh?<Kke$|2^4---cU{Nt%52cy;OO0*yC?7@vuQ5>v@Ol;HrD1gMk-Qk`
z06wasoXBYMQAPw#Gf@LL>S$vDS2R%r_?n5L2P1OoumtMBFPf+aNF*-QdW#Xk$VXVf
zV=dGkAo1n8jK*4Mb>Mgt^#DmA*A3f@9Co*WE5IvE)CFWFoUKZW(Q!5}UBKlvhn&cW
zq@~?}BrQ7$kfenLB;(v1Ba#+f;Gvd(7w~iy<+_ZX_e5Z^Q1RF2DAX2Kl`8N@Ch7)K
z8t1fXg?i8m)eWR&b0Q<EBd<V#R7c)M0I7qpfYd=)K++OcyUC|(<@<j?+{zc>uI*!h
zW8ndDobedoT6jRB$arBKr2Pl2kK3}q?_15v2DQ1etJW1d_^BS9F8D!}$yaBAj}&#o
znNMw%V{~#^0<wWKxi?#AH8ObCJk}@%nTukWH(SGr)<9*Tch`fvhdc|ouSL+q$~)IY
zW#C07>H>bxL}j4QrVIE>&muvlMM?@}b<27Mj<+(Df%K4E$5_=TdKQq`6kEV+Jqzfo
z+y#8evw%#16rR0s4_ozRAlrn*1w6*HfNUEQ*KK<Ce$N6vW}+V8Urkg7`i6*ZVE5B0
zKFCis&^JUc4dG|Mm%{rh_mVSN$rCc6)Axyo=Ko#W9dX<paoino+|jl8R;?H-LpDHc
zZ)B}e1yUy2jc)0~Asr#AIDF7%ugZiz=|w=w>3*&!T;fx73GL;VQb>7LL-&;EnLDIp
zaWC9X^qT3_g}VwVNmn6Bc7ugQlYFDKLR6h<XlEO(H*M_pXr_lbN$uOAp4_R1Y9b1~
zF$XNI#Y;z(vVRCEB+^0bWiA9Z92zPW7J6Ji<vtjQDG3Wp+=L1Xi19@5q3xliV<(4}
zoD5T3n^S$ehb|g|8f<>(k<_BhuXId)!@j;A#WwIV6}j7}MvKwqo(MeS*xW`=3}+P&
z8=GPsxTT3IK-@T6^@P)U%4z(-hSLqb2jG4t>Hrp<RuWDtIDJ}mU38TeqhXrAv=4A*
zp(K?SBMO!1GXka#vS9)*wt%~V#el1cfUBv1s}``2BffoAXE>MJO<>V!GvTy}Q_Q<6
zC7xO%51n}UhGp6VEIRE>IPJvg)2i#7b|x~7KFwHB70xuLHJts@i!jT?%8k$ZI=7^m
zxCz}(4fL9!GH^c=^#V^ZQJG-Q_aY*A$%}wKs9xYG>%}sVcnhI#=2^goi8_E?CMwgx
z$9NIYmm#{uo~P{VO{)QXUPW%Y;=kmI73(NIXSQlUY$dLkv7riv(8||xxNBY8KJ+IC
z3_Vsfk444!VK`Q_oKZ}4+N&DO%Cqu4ICXt2>$#bFlsz7Jjf(PVV04owhINT;)v<Mz
zY1M!jbmr?VMnCgJp!XR4VqxrY8Xhqye{;|TdrQ+69(x5#S6fIOKn&*n!N{ABJ~KL1
zZCq})W`q$yIa?`9mEKrk!mQF;6W^M!dJEw<On*8Mll}Oi@!M7J4byJ`-!V}Q_^yib
znP+s|alF5)0cWcyCo=lPY$JwoJRZlfD9yjP2+V)Sr}?D90TNnPb6wMNZUDb%q8jiB
z73K9}be1Oq&+bQL^i59$eybmm(QTd>&cOe_qLV*?pV$hkH^N;P)|uD}>z9$k3PKKR
zn32OOLJsSgk;6)^qecF-9^Li|-qt1{dQf8mpcd5{rdA=jV+z*6eV!Hug#~IRmjxge
z^X3cdA;hp6IWeq7;zBcB<2lsgDgzHUQ3a0}l-J0>?-{H73x&RgqYB=xl$TAb0i1dK
zK#v<JzHf?k;5w=(*#S2!*s8Y}ZR?4^qSN?sBX&9sr|W$wk2ZTr%K%5xi^JrX0L}4d
z)Nw{;fNRKMI}A||!}Tb)LBR2rNH;FAmUwl&l^WR58v?FxhI+PA=nX};)_bUdx!w?P
z12fbc8;WiqhbnuVX>|a}Cvnq<`-ZxAs}Fpjh138Zsv`Nf#)h;Q9HGKdrDOXhjg#NW
zxN!J7@$W*MqO3xl<E)}*EVMpcQPKKv6-0}MZ`=FCO~VTj$V|JvBJO=We9bCb1~OOj
zybCN$8y+#Acq$HU%L}i`t<bvs11FPPq5X^*ddQgJ^7+2TjfIaK>{8mKtLt&e(k%JE
z5xb0*fAbsJSUopF&ZPduZ)$OYHGBhjzzG9wdngW3mfRATz)Pr1rWCkzfd<Bb(83xe
ztdGR{D6`%H#G(W-66~!<F9^@oWXaU6SMX(eF8f?+o(|r$>Xc*i(WL}aXv^czcQ9bF
zH)2E4zupxas>X(@v7zYw>-RpA`=3WzgYgw>4JOsc17G=VLf94|%Qap2U&u-Pa&1hB
z9^GO}^lSy?H9{}#2c%yXmo>NPxY?z#2;Nh!oaoC=gwb!H4e|wRl*sQmJ++S><+puT
zp0HMnVULPOneUNuHuN)7S<mZHzPbRMdZJ(V^%kT3R4PQ^{{4vUJd8EP3J{xak}Ku`
zb8-t8+B!y~yjYKJ!GeEg8I&JaNG6U3e#g^*D`{lIO2_oIR?z`5I$iMgmO>fWWx7Z9
zMHx8VFopZn|3enXjHT7aa%O%;U18IULPIyJfjiC6en4^^QaWIW8d%H>?FS^&A*BO%
z(X(IhEFg0<q;yaiK{y-5e@Tv6@#@onv`vv7n!ZAl`iUhs14!QTn@&LT%~#Vg@h@n+
ztY+Ty<_!9jsTGCE%^ew=+fa43G2IzJDk;$c$vZc9RBUdhnwe+jD!|1~%GcnK(mueA
zOjH40YNCC@LY}HNuhgS!$<UIMVd@MuwM6OY(y?a@y>R&G7NT?X)ROs$G4fk4QFrgB
zoKr2L>0v}W=&7<%W&p{<eW%d9nv(r&^~gn^W-{FLJAfqSJ{JR%nA}|fapP!Vbkyli
z^|Dy$cz3_lj`l01$-k=@QaJUvS(=@)1nqZbDY^!IQ8PV8k1nPY;+R5OBBnmI!1OFu
znw_!~(h`@>d1~pu^(dDE;O|wG-?cOPt0w|inp3>MFdD5=u?XC%ACb}So(SB(ACb{f
zo)}INX{=+E3J^E>#fg!(6<wTe)C}*`qgz(s$1M9Y@JSVAH<<s@!3PCh@RAl=8Mus!
zGF=L{t_q!B@I<c*+^nceT6+|9!A-9V>?-Pp<#m3;`p#8_t@_x<8o-@Yl;7bodchMz
zTmPxHeym654}6cx<e?n+fQqsj&DX&{3cBE7=CTZ2Tt%7gbREpqqw{-);(2-$zrf2?
zl<8ipgS+(Tbit2$UEmW%-7x>NHTG>yuNqe^y5(P>?6Grg27u#FMpOe%P*FZ9jL!GO
zFsL`w{2E4S##+r#>o^VOCJU+zyhBBcmyW)tG~!F8v6$P;Jd}Y|6&V-7zMcyt2jhk@
zkqOmtQw846#|b1(`Mex>m$I^dA?$G-yy8`XZ<(k{azrQA!npQT_MxUV9XMA-%jos;
z1pPn<If1P%Uwk>*UChVMF0HuY>T8a67gxxHK<x%3LpneU9C}7hJ_8yHGo>r8uuP{B
z+}1Xs(+B|<@=t)YGerK`8+J^gmwfF`2x!WDgnEf7ny1m7@?{oO19+;6@&(0c<*yhq
zjQw)8b%!2Z8-h1C`&Hm!Dw3Y-YcZJRd9lZ+!SG1P^397_D_X*Ls*m63aiF_-xOZDz
z(}4G>XlUt}uo9&(oq=4yo#|Zwj~T$lQhJVx$*b6jtuXsc7)FOC6CrH@^u}aPLSu>c
z*;9kq+X6j4^j3mXQ;u{!(5uBI{Fc(cXQtb8{>z&NdbK#BmzDlEGu@u^sy7YvYH>vW
z!lW*fp>o3;{s-sZ-ZXHisl}$p=($t;3IzU0Mfp-?w8^P$muhUQp0ZVM#kT5UI^)&(
zR(f>H9*j}B$-vUcQ;4d(ib5Y~^db`D-cS=mUMpJRxSy>dWy=6P0?3462+0M5xfyCs
zvH>1u9^_voW8j0etpDLvfj$mV4dW0OVV&<+6JAdi{Fk(Z7wRz?Kbm+J<3Mjrf(VTz
zf_PAac-H4L2(1Yk9n>t!;o6?eV>0wmQ$h;$5>alb{<hZR;Iw|BY#by53P}m?x2c``
zy!XEcrW0CIv>b=~RSVqf43<nUu&kny?gyerjGQ~Vg=Cwr8X35)iaZ&~xUU&GY&YUL
z#Nw#~_po?+fOvO1P~BqL%3|pO;yEWW@^M5Tj1tG$7DolR(P>2A1-$yqT;$AGT8tL6
zIJ$s%%!%!i$CF(8fV-$u`Mh~<9|IgYYt_hYMol@6|FAL6wuxgR7HgOEEj-}Pgx-0P
z-p{qFck9t@-r%RK;$`5oD$1S^U(mt31zqrHo0>9kj1_L?kk-RGc)|04FBf@l>)-=D
zy7<5=oX+%@fvcz}i*HjM?4n1f3qH*2KCJM0F9N=(qRiaiwCk797IWsnpH!Kw>u)Q(
zqd)^c>S@3yRFt`RUI+Awn7dtD_c?3fGVu2*%2fZXgQaXmM;8$0W(BRKywxpD8Yl0Z
z!-Cq0ur&Dzc@IoN1S}tTycoq^A*S~<HnSUc?>LQ!<&@2o%C4|!>0*;%waH&WQK>Dg
zJYDQ5RWG6_EFn=0L+H^a-pTg18tyhTLp5xBL(%8H`>UZT8ozvL1V}2ff-mdCI>}^0
z2Bam*K+?&0VM3WOOeyUD;zJ1QV3t*PI&ij$rU5T8(e!W?6pgYPK5cD29XQEE(^w$n
zTC@<RFjXBNWC2VAvVvVxZ8}aq>OeryazZ|!One4Hm@HbefPi4zS_s=;{t35<t#AQk
z!Z4vwGre#^tv|+VE2P}3P0Hqw_?x<kx-}ecTSNnR@Of;dHQ>=IlADHmP8u<C;^-EV
zHO@D37zHj4G#7OsTGFcXPim)kgXvX(1-)wrp;t}l-8cxnW<u|kLFjd&H`7Yk1EgT?
zuAnnfzUW?Oi?i~|R`uxiLGW=ZlWPL-1Qlf$fOmDU=GnOorwiUqWugn*Q$?BX%{q9l
zpbK8|9A>)=Tt-Ei?kCU9&u^kf=XZ+YJ@qJldljBspn-4mv<DQvQJ{gRScS{L9aNM>
z5;hf;J5Vd**7+p2AyAgdny#H7=eNTXVM4ivTsq0HTjO<GMs`glw;%Wu;{4ds*f2_#
z;5==8Ss#~2eFy0<D?t~{#rjBG)6n{&X?4e1-C;lzv|JPQD3b?lSub>)qc$%v+tK~T
zMq0(19tYa!;Z9Pnyd(lLODoH(;7wfx<)4RkNUASrbo9Z^-_*nk8tg!B;lAeG0_lOl
zy8W{jVpZK8mn;n{?L0gyZWLn39ab7SZ=s~bcm1x`6rZwGhuyP78=oOJ=`$oHd@|wW
zAbGCPF3*sg^cj*8KF4THKWA5-2JloBxjnYgVswEghV4B`Pwk*bw_kxPrd9^-t0LK9
z`dSQ5@Vs*r;?bG=FfP5R=3q<_=~Q*gH}e@EZIRW1cz_y4he<M)^$x^pmOKOO3J-{j
zj2GIUry;#;p*4V||KQrv0UU0k25?gobpW?kQ9kR84)a9d$tG$5=U-q>fiIb;0bK9v
z#%h<)LzYk-NEYs!;_8{;T&VRHBX2(1f8%;p!g@Jd^4D+_!E&rnGSbh9jL!E&;1-&>
zTy?IT!+^5n+Km2eMXCVFLgNVv6(>?nB_c0DqC~2xM5?Vgkt&H0r3<CnVnm?w{svf#
zIez~#PeY*;@-I;VkFflEfVde}n$y=>>cYHZcGaWX2~JV`oq`N}t;%E_0k2n4c02P6
z9T2emGDuM15^-GMLj$%<R0UqHqU-}V2BaVrBJk-JSSRoV6GflF-5M?LW;~zGc;=eS
z9QBnJoHvJXm@WB4FHHIlbvVhoxfZXg=&_uM{i$h{fvZ}ndVqu>F<slo#4-_?FpTO@
zHG-ymi*B)t@`hMRk0M&Q)&a{#S_dplYeo0<**8|4@cM+2Id#EyW8-h1?G;7X5w}?M
zMe|T=MCr9>UY5K<Rpeo;3H=A!=x6eF`qWoe3$9<YrYx_eMZ4TeT?Q6rZQh)PTd&8~
z>#_BE!uqvstsh_wb^wrchRQQZ?D2ru;{geeS4JLpS5KIimW!?L7hB&iVg1I)`ny{2
zHLS|h!-77bl!t9@rUCtQO^*YZo(SOfD1fk;+^zl7Cq47V!>`@?S@LAQi|4{9;rft1
za!sDqMCa>qS@-A3^8ahf->Av|uQ|5GnU|EYYDt#r8_Q2vT@RiK;}vC&iI}p?onn3v
z+c141GveCL2x~i8{@KDg^6Yf}?e2e9?>AUe?gI*^iUaW%7BaqyL>ov*8Qf-t)+4rb
zqQ}dSvWi@U^M9*xPANZ*-qigSTHBz%Z?<OuZ!*#Tz<X8XR%-YkA>3eU(}2G)(RAQL
z-qP%Lmi*--+VyefDbeL*qTaNgWjC)4-~|`w7bE#gCEzzz<ZhW7Ek+w(X2dW^%wKHg
z>%hIud=GFx6V-wEa31Rkk3H=?o@yQ|z(-8f4a9SPJIV;>@~RbBjH6<4{MZ8S20m0M
zQKiM`pPmS$YPst$jUz6}YdTAh?$!`|hRWo^20Tke*=^u&bnrw$7yM_hyX-e|7o+tk
z=73{Vl$pCu2RG@_g#y0E>jEDr>b`zy?t^4g&$_$ov@j>~9-4_b%gWZv1Wf)<nyvRM
z3h(tI;8QA+H|l+@<u!=4^(YR)xDxNa_lm;mio@!P!|Ece7cHzVB6!v!=!zqVK498R
z=V}W-XTWc$NNy}I>AOfP{&%aq2#MlctAq$~y1cH~Og(J9J=J#Iy9N5<cVnNKP5h<v
zkJZ4<W@s9a7L@?z^)Xo2nrRw6_o^2Gi6K)Zp`~pLng%4Wj2GtkO|`qY%~1_g=q3I@
zlNtS!X-#8BU$&WzzM$Eur=TXkiy4v<k8U&6&cWuY=RX6}39Z$132mjv%sVw?5|bf?
znqoZEOZZz${Y<c>I~c8rI)A-3j}72AOw<MZmWuMr5Tk9r#m>+L{G5t%BBSY^2t2SK
zk<pJl5%?1mHGt2VC|bd!)nPf*f!{Py50FG$wDlIF?ah1-kVJAKqXRq<NFq6r5xL0h
z0m()F4h87bim$O6`=ORY<^SRCO~CG)uKxd%K@t=(wWexBv?v<1q-ttwtf6Oi9-cuG
zMNJJsLz6sFUedRNr08p?o*=C#^`>fwIfj-RDvBCQsG=>J7DWesYv1?!tbJ$ieeyhS
zfA9ag{{Mfji~GCRz2>#{aPBiaK)iMqD;C4co)L)GIU}Q*#ZUpBrKFsZ(fOVcNGv%c
zBf^or97tq2W4JUNsJ>3ttJ|``M_3$X0=?6do>WMPP7-|E#WZK_PK6KXRVLq%_R~7p
z$>OT*r;tQAb(D}a$RZVnpIEo{QOEwvd~M?XXUtxc8XW3Lm?L9}2kj55P68ZS+O~~%
z)WFfgG*?xQX`X#_jsf`|R+^qlelOl%V*&L7sYfQ-$td~<sO{CJz6``n_S~8QHsu8M
zzi;t+bt1FKU<DS5&G+iRovv7wwLSTekb(G=IU44HdGMx1BbD7rn|@cD2}0ac2m2R^
z45lklmae%9ukoZ#M&GtN)_}uI`_z8(!)vdh{YR#~2E?Gu+y1DnlW}5fJu@nCG9U>O
z5^(VOz!wpY*MVr=#yOH8O9c=Yv!ECdvG4&gnDNCXqlc)2jgr)p_rV2(b`mCz#)<~B
zLjHUG6-)iweZ%_ymtu=B_@^~tFDo)jtmG-|`fz`dq@ex7wzSt`MN#ekJ@ZKY&usOp
zXDf&+W{Esrs#;cWv)eX+-!f7SI9Ex-N;ArB2H#bpI~+9HjGmi^!W!^JC1uhKey_w_
zn$hN$m~>dI*yB1LpKTs@198WxubXo(m~-7gEa!}jydTj?b!#>DDZRR#5xld~vZn#N
z>!44svH<`m7x}_K|E{I^w#hY#W~uq#j6IE>4&v1jW=($BF0=*{u8WsAj+cqedc4-@
zT&q{Z;H8wXdx*l}T(eGLw1qJ?fNvTpN;?+kn#DTseIqr2*vPBO=%c2-4)hkI^kMN^
zroIAv&PX*NHk`#uo6#m_u>w3@NjW2<8;r3A#I>9;tlQVrg|F*XJ}UC*P9GHsd6OR%
z37Ha)DToTE#pZ6<#Opt6AjNy3@1dT|pHM;4NhDXZB+C1mZASPd>v*V5NTC;{%koA&
zRIl<oOu)00<Wk>gGrG((0ym#evp0cz7^wmL%;m-f{F0Fxz}^MM1)OH22JkK;HG!`f
zsR8WzvFQRHW~2u2Y$fGkGTQh`lLc<7q?~c!MAk{<8J4mhAlY=f>WSRe<J|U;+k-5(
zbs!0qb_J49d5I1rp~3|`$Ff!jl2G9S{@!x|U$}z&^Z<ujZo7b_U339SyZ8nCr3v-`
zpD|Jwko1eLfpcFW_r*k45{a(FiS8xQ`&*(bK(g!VR%tWZ+xoZyBy~9>qotO}3UD1u
zWH0b$CFQb=R<V@y0?BF4$cVJMw<A=BwC0SA$gCU+fh0L+95~O_RGzDeJXhm9ca!H`
zEYDRSnRR|tEzdNve3S%8Bj=2aW>}u9KuVA^GJ4uG0;z(0+!aU(a#==HK^TFQAZKJm
z6~t{ICCC{WQ3cTmEEXjGFi}ZY7OEg0Y6a5x{6HyV>L9E@>L8E4mrQ5C(jt7ot+mZF
zJ_b|@JB%ri$OEZPCePqHtH4Ac#mV>>yzcpc<TvAEK!(LGkOe&BV?bi@JM8ryZOSQn
zbthc#ENj>b@C+plD;+<ieWMQUFUW$QGTHJ!6t4b5*3$AAh1(dZvY*0}JPCM;k}{)n
zba0*L1KwEVqq9ha3oHzNx2Ugb^v}h%nbm;wk^9Xk+UGv=qDDvTZ>wYtNM|@n8iGCE
z(ZFYE@!cPAp&{7i9Uqv^aC~8$iNjA=r{V{>sn;u3ugO5_CM^cM)xw>;njRC#Z&7KO
zXT;Yt@xy?d8EGPLf{_jb(j4Ln(7STj|HPG8{7a#?VOO}$p)HtxjuxJwQXTe$w$b{U
zUO6dG+pEI|%R}Dep*UnplsjBz#pb9^HbdT|&5$W!lf%&;bWazXyLYk~@+NJDObMH`
zE=!I3F3I-erkBP-cw3Y1zDttb2rGSvb{Y%K2J$01d^5r!`GThVW`ulHXya(@xJYGL
zd~(_RyA%V-zOS+@8IrA2cx}ZVyQiLMiudTapC#^vv4u9WEQiW7VyHD)k5HY|BUC1&
zP*tL8`~N841<ur~#5a@J!x%!|q-P;h!sY_C^}--*hP+9eAydL8whA>KvuB5yAMz$`
zhD-^Y>@(~!#onQxSiy?-=qDz6@m^Sy&_?EKs5~QvT9Z+Q>LjXAnUF$NiP-1qw)?nV
zC58(+^;O85^ekjb*nC-SZLItDifo3wNt+>4!X~x~HD1su&X6~0Gh|BG#1>}~iSzVM
zHbdT|&5$W!6I;AE5u3Yrnm-|L(q_n%u=!=(z3in=Y1liRz6KWZCT)gH37g~f95vsb
z#590&uj0Vm1Kjh!A@u?GH&O$5ijjJNS18H75^J;>9ktNZ0Ut6_5AZc3^#R{EQUf^p
zr=|<I*ENuOfGs2S0ZTv2xpLPSjZ&(71`W7yIYvf{JR|VXYl|8g?ROpC^Z{4B9#RkR
zgGTBDZfvA7@BkzA0k1Gp5AbRwX=K4t8dttf<*BW_lv^}|0o9d9N<hky@iCwv!Ux2W
zj4yDb6#G$0_%SEpM+HCVGSNS01_PSRMPIR&drBKIV`Z?fwo}f^h=4Ly281M!eSpVn
zxidZnyKA*`m5iug#>#*S=B$h;V#dmVMCPmmXR?~e<oStAR>>q`iq-RHFd$HQvI4}B
zjBj8+S_wavB>ZS4{8*Ckqm}SuN%U>&xU;$ve%zbzqbuRZy$L_M5`Nqp`SAr^NGI8X
z+6QD|b$>npJkQu_z$=W@^MXRArF{B_PG=1jQoMFHarL9NP&R?1{i?`9$$BKZfb07e
z21pCbUl$qDW<awhK9_We?lO1UZc`%;rmgHz#6b{3id!*ykL6s|d8J;9PXwT0OxpH|
z8Ke!}8Ib|mDr$j7fSEwp{J&Q7zp)U@z=HL8W}RR~*}NIEbs_qe?Qdluj%9oaPv%8!
z_ndn0JMRgw;K_oNCktj=pdMW8JpmRxSui8v$%4p}SJi`M-V<QKlZ7cy7S8ywdT_P(
z1X%E7Vak*EheqC35B}jj0pf`~r>8ThjgW3s>$iF@fOsKXVK#&m_Q!CYzoTqQmw|X7
zC7x}$_@o-lh-^(bmJ#n$4nJMWKwQiCaBfT8nld6gQ=zA4NA>%F`a0VC48&)7m>y@X
zJM=@i@B)KE3R865Px52YoqClI1uRoox|$PHd4j^GC!MMA5>L8K_xoq<Oj-v1u6Pt+
z-J57jVHwDg(0v_24|{9cp|1?&JSbeiJ3JSV!=P}H`MRA8%Rp9K;R=rf&WYgPK^pCU
zSkx1NS1CzKz1TYHMj!X$88~#39%AGXC$P{bb7tTzu4Nio@o`d!b7nB0z!_)Yvh8(j
zZ1VEzI$qK>wAQp%@m|<t#d~4174K1AoGRWM$hYjJRp?Yk486&Sp&J=7w4V_NHY`7}
z6dM*|Y*>h~VIjtbg%}#XT8{*r10*}QP>q3jlaZkCdR07J8e?Xg5oQuEM;GcDW9u83
z=MCUD7D1{3zo{g59B#B3&G(GJ>y~3=bh~E+zP21Aqp#d(rh)U7l$&Psf@cK2yc}bg
zP2zpq&Q^6G;mAAjz)Q^GI&h(pdVqxIT&uSk;YZ?)eVo|Ql3oE~+0E>J1{a#m3XlY3
zd<?F(Y*c`Fk@0m(2FB&}1`yLCcfR>?p7~J)VmjkvaE19%1s42>e}ps6M=SJ0Hg;U1
z67mxVz-!F0ULa*~MQK@4@KDqNZ!l+DK%7mSCUA8lD_s|Gfsy)vKQYoFK%#Jdb=edm
z5c$~*AX7;;AYd`3E{lmtBaA>|%Z)JlsYTiaTxz5~Ai?LdVapK4ts0qo6Ab*UMOy~G
zprq_WlcBD!>rHezUEo8lq88NWysx3A2X!`IWl~LYML3BUM-$Y3+55eND)8pZDEhhS
zE(1xotgBnAa}9H{3_MgxGDT+f->Cz#o%sCaNOfmRbE^)dc|_IxejKB`8Ii$F=0_b!
zCNjP-gBWGriC@6asvqtpGkB)?Rs~WyQ9rw%!5WsdD)2)}8d~~lY4<4IRiYxtvL=z{
z_q{UUx*DcaHeNA#7fG^lhh?J-yvHQxmcYI!(XX|TpS?Bk;>%QMWJ3N56OcSeQf66F
zK4Ec82JWS#jE}(yo)36Rk&jf}<oQU|M=apUz#Wv7nPA{^H5piF*V+9f@Ie(Wh7Ij$
zy(homu%`bhm6H5`0C#*|qd%F&%-z}=>i^&8k7b&yt#9Il(*RDp6;dDY3?nswi;dI=
zyhBND#cs42?RmSY3nx2HeAJw%1F<Y!GN+#bw&e*k5bYUXC;ekge+7t2S&o6IlqbAE
zVsH^v5)nn8=O!X@DRUo)8F}3c#EftOix#U1i_vT5*kUWTSc@&TQWjeYi_!D8*kV^`
zQK~UJQME2?7OFP8KdjoLTCJD#>b7I>@SAC-GH{fVvaNkh9ehl$P8MACvcP6hcHS+y
z?*G=SlU@C`oc0mDI$G%OHLB#Dg3Xf>y3kv-nNvmWW(D=*b-s`zX^zn1I+_{ME}JBf
zLQ4hCNK~MlsK6Of1-4NA#}z6-l&|_M?x{f@VF8u@pm3!^Kw*&LE%ENFY%-yN2P#Pd
zcxC2f9ZdCnz)OmJ3~o`PD--wulPv?^HB!$On(fcnDpdwj0C}OiOH}kDgA_?nWUEku
zdHu|15+h;F>nA(7oe>#aY0i~_*IQUqOJMJvC=43}?<ktf2iO<zU?80<ek_|=(yH#E
zSNZ#&M@NM|n3@-YeP?DEtTRijbb+IRskxXSdg|fWz=SGn0|i<<7sLS{PQXmhtjQP<
zlKkmfU?I9!qUegY!YGUP;%E;Kqb;(>QAfWdM0x2r`6HPung`nSoK#i#HBSPPnylaq
zNRDU&78>K7L}N^%M@V6&j{!*(eZWFKm(3(+z6&Oi7n)o|fy~*gWO0rrg`KfbC+Hn|
zl?e*O3wc@ntgcr~PvOEX)*j)C%|%c5?^QEwRH6vz&Gn$0mJlwWH)m^-@B5xGI`%3O
zE58#L?;Fw2OWm#Wc!^%ua3|&dMMFl&Z)&>_9Jaaff-<aAnjs%eboYu}p)>C)_ez?~
zNG&%$qzvnnW)4DaS(oIiY-w%)&s@wgpawioN%`TE(LJ6K`0#R!jNbQ*z<ut>jpP~`
z-KbP~tqr_wIYve+-Dy6C2_v$F7FiwmUGt+CNG#5edYjSrjj;|SmYk8%p5|~bkXUj?
zMh#=E1BpfE0Pt?l1@w_d(}GBkv`8z!Z&?6MAh9{WDi-Ozo)JiFIU}Rp%(W(v*m6cj
z-r?u~kHgeMPG3MeO`dN7iw;-KVd^2WKwNZwRNIX9Hi!FwxR^6Cq8_3VNIk?GAoUO~
z;Gfk(u8xt9zi;5yY!N@f$_l)&p8)~NLr@@2XM7BBTlj#uo$)ciap42vZN@jSzg-D`
z7bX1dO8C2|KlZmP;qRjU*x#;%zl-9NnN9?ploy-8`}CH?xgg<hC3dZXYhTqeXQHud
zm9X#`Uzo}p)c&t*u9kuHyS#@$?|$BTx93oWcp<wQ5MM+v^g*~fdGMHe;0}S{KYFi#
ze=|~d=z^0B4LVvUi^Ft{aXjF$w(gXHyC^BkADMWgNW>8WAFLyrs*;_(QS`s<No-B4
zTJp=lEsRtHe$q%~;I2lh0coqe?ezZj##at^xkTMY>sI$^+wvN~+FiB_0$WNNUYap)
zdYe&~QQgBMM%`W<c#JVlUpAx7=mgIQJk1!L9Y$w(M&OytF@`NNNAKQjf(_vHKPR{*
z@D?QvFU=IAjBZ!z%JR9&{t>H;9<osz()PbF9i1%SZkFr7>&$WuMGtxsaJcGrzxcF;
z;;qbH4Y-Yx+$K?PGuqZO0`WCx9N66o?%rm;^#O6)>8d2M5$#Ny-<>CFOTD_81)iWZ
zSxtdED=GW(-u^l`upkR=d0Ak0QT7-e99NJ9PxG?C=|$P!=-`QhEcg{K3w*UG`#$$I
zcP`!`ibq>ME5LP>l*JpCW_PXSQC8Ci@Dd}{fIm`FK0z5>VbtuC+&a3-ivw>lMmIqj
z-RT*D4;!PKpo|{%jKJS4$Ji;j<4mv)TwtVLU@?q(n-LZg@3bye9se+04d9mdFn4Rf
zPbf)#K&{+nFkXqS*Bfm{hkNNTq$zr9nz1y1GmKONo@S&5@asy-Er#mn=-r3BAaM74
z38@L3tfX8QqtlGB0es0wP2ihGY5?#4rCI3Y*N@GwI<VUmbp!Fl`BiT-^6H~U<#E^*
z!mgP59v}*x`iiM1R#|C)#42YTAh8M;@FN;_u8xuSHhPI4ds~gY?M-=GO?X?4z3q*?
zt){%KwqtLj7kaU`t=QYXl((&fx2@RQzS!GV%G*}L+h{L|z3qy<t);x}GH>xm9s~gK
zM?O6PBz3OrUFO;iHV0}zIw5BaX9m*%FXe$b@DDm!<ovd1CQVEJgfDh3`U~K9sEX(G
zT6`P{4KwmNK_K}N%L~o&*K{TobwKl8O$Op!rh{aWi;Nhif@NtBo3dpf4kbQgu|zYz
z!+oqi4d9yhL#hEkq9j+0Mw`(z&j`G3IYvgGc)&CQKdGc#BctOzV;Ck5?_>_wfh(II
zy+B-a>g(q4IAg2>_bfPEZ!@~a7<+-ll51pir!m%n#3EZKaAymv4)l>mO9zp@Vw0i*
zT+7TgfyGEG7U@-<5s1q!fJ&Q@_al1rgCC?>9-{zBvpj487X6SFQtgvwkp+@t=SS5t
zeyus&2gJpkkr8PYjX=^Y-T+CnZ~+f3bX>K~$j2WoigElc;wL<rkPG@55THD&0pfJV
z#{jp54~W|t9|Ig0J|Ny^d;`yru7tk}6aIE3{9V`|``eZ9cVU0*Z&$+Kh5ezwG&N4j
z;Q(0INiK2DOZZ!fU8~^Qi`qz;XzW@gEPTcny7pPM-?GJ~45Z)XbVDaEwBGG`P$6E(
z!vP?^h+ybL;t>KK^r;8#)eiVX?-g*Ck-9?{oMdRw(K^Na?Se({Ri?fSyhlk{{>a3U
z#pL4%fe+S^pQz?vc%$e)z%pHC$2r53fL9u+2Bcvm(?fL~Si=_Oa=0la>M={XuQ#a%
z@XiNqcLP44B)4=m+KkS7*cgGADk*0SOA;HcW3<<d_5lC0V6@(5bdzTU;*T?0H}78b
zjKH^zR0rPhD`MyYp7Kym%GELYjZ&rEfyH3!QUArkR^niLQ^8ge!S<$ttt5i&jf1Ta
zY>x%o3nV-jY^BYJ2xZR)7K4>PZ5aHEgRREF_N9WYCW7rt1zSx7+ZP90CD^kq*ghcP
zxnQe_VEa<R#y=Yt2iqc8{E<~;;Y<d2BpV-)Y-W5xE=#fgF7)4J9oz&KyT8kFgn`6S
z`2|hP-}LI%G4NRH@-pzFN=kmT`f^=9PBy=~<B8ghi^nWsvPG}mQFN;+EWW&e)~`k+
zX$xkCW-=m%$b3d*K<<mGp^>gkAgl-NbBT3S8Q5d_>H|(RQUgfe$$^dzTiH_42VBia
z4d6G8)CWA<NDbh(Z5{3lU1v@ErFR|Zow5bZD~g_0P`Z=Md>QDaqN*-b&+ax48^G0m
zO)u1dn;NMB9H%5VV;gNo7a3y>c*Syzj2`ohVP<gckLFq(xVlL-fmqJ>1V-B$V;y*o
zk(xl9$z>T`Ym9Xu-sX%0J6yrx(Wa{#h>K2F#S({?ax?|vrF?7^h>N)_BfJz@VA0`t
zPlz*I#o@Kg;T~Yo;cCL+YV2@N%He7|cDR~yI9}Idhg&#Y^sbfgu7!6;Xk@aGEb3=~
zM=qhQgnzA+f30Yfjs5GwzjZ93y+Edr>^i_=6kUlZx`=|r%Ki^5#?Y0Bp(_<bR}{lb
zI^CG_@^lS|m+~9Rz@o!``s0$whLdEe=HX4tM+3OaBeviGYes4S>q>H^YP1>MZ;UnI
zlglwOTIo?U(#f?AOjjMatC5;OEIU8y=Ep>1tOM^dQWJ<Xxh$g>jj;~I+njM=hbuU|
zyXoo%79Fl69InI;cc&b#v}1=WDTm_)hvMU<JS+iz&K&Lm79Fl89InO=_oN)ICLFG&
z9F7;9*x?oq7rkpGyldgze`#ddg2R9my27>+{<Tv6#S2dCUl;!EVF~R8GKFNp0T!d^
zN<`5`6eLy_9AGhqu0#x7sTkq~=Y#4fb6yq=AYRIX11vi1r#~(w7M#&K;m6s;Zvc-q
zQVn>zks81=l;ld)XfyhUG1h=9Kb9{oIU}P_DRl*|eZh3qfyWuC3B+=~7BHG?jCJ4}
zMrs0aCYNQjj=5F`;%&|tE;!^8FXf~GoMJ}1fw<^&RV;CMDY8JkloJjR7js!gcqy{L
zqQmim6FXeR;S<c^9$?YoYQo`a>~K%Y;c7c}xSDb}UT|WETR2?wu9fhvg?IO8WZ8nl
z0FPWkTM7SKDgWXHC-$!k|4y`o_5ztgvfu!VQFJAu=pqUdD+>;=7(-VghOSf$@q)9L
zI?9}vnF+*8S#W?whyC=&rNpe>U1!w-n_UgyUyala{FjnkT^emhANdWZly2ZQMrr_m
zWTbB3&y3UnUaO?s0;5|zBXG0F&1g9B<CyBi^s>d&3nU6>v7U&jH;$=JOg}cidV#o{
zt7C-M@&+q#X(7^jn~@JIdgCh&s}hIRlM1Vn2&*R!t3p@{EUX?Nfw-_LZAQc*OB|3u
zaz;iztmsid99EUEaM9_KH_g;5yp;18@OZr?i^#%$1~{2ZGr~`i2I5@KIB++$QvS&Y
z2_pYm3I7%){c9ys+DiEsFLH7Ix<daXE%H85<X#u<k(-Q<0oLWy2|yB+@r8N9=nXo>
z+;c$iy*BO2OgG#~yqQo}?mwGU4XMjC2wBPS6Q>f3+kdG7T$6<o$TX2%9Npfd|LtWg
zo(0jPEi(1E#oUbjir#}pDv{sSEK_CRx=NBqul;QXKI-`M)#}UL=95(ql~O$*iZbnC
zbBx#3`g~ip8^9Ng)D3)1Np3<m+KkqC!d86X#ztxYzh|Uw;AKW?0Ou<yx4`Hs&j?)i
zNi#ZdOm$*<++ykl5{0u^PsG$4$5bb#3(c=yATH<X7~!>SGr%Q<Nb7AzKCEb)iAz<9
z!|F+eRY`=^6NgnHtn)3b9w33Zuqtgv#3I`akU(-qMn0@)n~B4!5*98xUDY-typ)Xs
zI8hzWwiyOEnM*UmPmu=VT+TRfH?>m!wG#fd68<gfPx;qMq_matFWzS2{B?!?W!plW
zzAoG&HyIxTtjjh7BvHw22BVkj6m#1Qc#%!JGSdxr65Grk%Ke;4)sQ;JI<tnKIF;CD
z&Qb-qCff{<X(HPUy1hr;KoTT^K-Ml11hRGs7ZB%MOEnUcJKAQ*)IAn+GxjUmW{^tc
z_g|){3|z@(iEJ|r2sKd$v|sC0;S(xF3W%cQHZw+7>uI)XH-L8<sT=s9lJe{^`jck_
zzGtKc@P|*?z5u+&NDbi6l$2Xwbc<&Mj(XbEg$qL*Q=ORb&#AAs8R4RA0>E9=VzzKG
zz@1#05gxf$Dk_C@Ipe_oRYL#7?D_q%dllTncE-m5>+ZEh;7G<dupd?YxWVez2c+6A
zrfQoJKFT|*Kzx*i9C)?WuMddRxjIJp?OtoB%u>DPjEu;z*c~{m7GdF{)746pzlFn8
zT=KT4p8-zh(v0v^9)bXIE@vFrzpl_fF*`3&{x00ZcE-m5>+a=8T>khsEHHYO*4ph0
z;Pb8i<+#$(vG<e8eY;83kh;JsU&Bwblh_wdR0X&u3pbE9l;s`WR8vSm5+s5^78~IL
zve*b05a(RU8tu5tqJ4o(U2ZWqW51$(0jWfOe`|`$z~_~et=tR<HL)+C{amjKpHL}M
zKolkSh0(ga?rY0z1GvpIth~Lzhm@3Ok5Tit#t3|0NjW2<wVpLb;5|yp85zCk8G+|N
zm&@jijK(iDjlidrlru6K^1LwuuTxUa$mlN57*1$%wUgzl4n)f7ia()-;MYvB0=&aW
zJ-~O3Q~`e4O3?!(`Oadc&FEEQtN<rkb$Wo*BbR0L5leaxkb2~djQ-*ofz%^sWb_lQ
znQT<Rw~W*ST-9n=0q$a?9w3FxEij^%@?k+Bh0GZlQ6O=pQ&~^4yjOvg#OabB5mN@L
zBAXYG{O62}p0#RJfuts9WOTV@r3xfFIU^$yB&LBxm@_i+@kT#-MK?~fcw0as%(g=Y
z#3&mdkN`7227j<<TR`H=_!#`yf^7i_FXLlCfMOR&bQvE5A8M;pX_+*#WdiZqrLD{6
z4>Lp<flQB_kr9nAj6mYc85z;P!U&{!b4Er!jIK^$%-0ONAMONuvFO`K6dG0$8wLKA
z6{`<OMjRXXJzG5bfMme2fq!8uLLZRD!m)w>%PQXoBu$PD%uJRuACOu*Hn5L8IvOS-
zA5G+5tkEgYbiP;{1E%%mx3hpG$+3Z1b%hN`SdI-$Lkk;_upArM`x^aV7a{&gXR(NF
z4*rD|w+X~Q#|HkJ6}JhbppFfEn-#YSq@a!se6|(02_#y_2ENMn*Cvo?9UIt39-XZc
zk&h?xdoA*AAkm8U@!)qX@@^o}IyUfPi@Y01w2lot*COu*60KtcUu}_h1BuqLfqmrB
zS#&>*Zl+${Nghml%LX@9qx+Fw9UJ&ji@Y*HqdQKojtzW)MP3=H(e0vF#|A#iBCphR
z%l;3&IyUe%ww+ft)Ff2(>e#?%S%RWHn&Z+ptlP)8!PH)kye#e{%4rAxx3xx%nfdoZ
z9>E)1g39aw<kM*bKWA(;4ol?Iv4JnQJod0nkWa@3o@ROM;h=PLA>H8TjIGRGLfV}+
z@M2519g0Z1V*{UO>5iUsAFgR8?M@8Lf+$PZ#+pCU?%2S0>pj`37xXjlNW5bNf6_8}
z@Lmc@yki41Co_EvNWEhOGb?3I?eVatpWHh>Fw@fUfl0pO19Kc1QaS|4lI_?&tjVSz
zjs|9HcP7B>XpRre)OUPfmI22X?o$V9d48u?`6@Axy=iFa)KZvrVT%JjwG>7rY_Suk
zhCY<&11!2v`#(A3skfH=^|qa`Ol^zzZ~?Q$d!gOpz0iS_{hVjn?J4`*OWEgM%0Bn<
ztN%at$^8Fa5kkwZe&IhXKCES5{`a|;?KZZ}oyV*(CIN9X7UB+tTuV&L4;8+-#i-l?
zr<jB?X=5#<m1&dG5(b&#34?x4L&3XZOJFnAN`uiud?`KL>!gRQbtpS3Nk^)lgodgx
zsT;Mq^Rm@*B9MH<Iozln8c*?ralNM|sB|&UOk?yG@39tA+v2^!*dWg-8{A9T;9e&i
z>_hU)r>JHt5mLB;2#I}z{Xt0BWjz;CxQq*FAP=OhYZYhHM(xm{m?QKr<_O)5IYRGa
zjxd;*Ba98tvA&{er+Bq$w|COJ;t~xc75YE6Jy0IUMtdt+8%#f^csoT1@BgfqLa8{y
zfod2>xJ1u=E5E=a(FSmtk$Qn=|1Rfp4=WmNM(e(aY%lP3CFP8ae(f27XZ^mYk<omm
z%I8FZt(S_7jFu@?7=ahQTx4W)hf;+Rc+x9HMn<PART#q#BUgX7vvM7Xl#4fh<cr|0
zmeC4umXUgZi;PqO{=-N;K$7n)R;(0PX@TU0A8?E%y$47=a#=>tc}5`h$Qc>^)T+}1
zq#ijVqhqZ)72tJ7>H*$wqzdp~M(P1l$lL-WYAKI=ffO=lWJG}yk9?`l1WR%iNJ*To
zYMT*Nk>{?!Vl}E(jUQMwsz6ecYh-kU#ajiEot%*o2@=ylBFq^X`FNv8zQp?(i?;<N
z!t9YM17eg%uRy}h_!wMa(YAoZm+>(;%7SeH2`}SgK!9QwNOTz=10QPi$d^!=H1gaP
zh}SM{T~;w>h%f?~9yuc;8ebTJ#FsNNqJM=ENb}~5jC>f;BVWQeOfxJm4NG7z7CrJ!
zh>b$*J65bdAQ=(uqrkV>n$QO%1C9-RrmYBlKo$$f20q9t-v=a3jt$IAmPfuoYVFv-
zKJw_1ZzA&1MDE3+N4|*B`C@GhnAVpk9YB)g*ubp1!UiNP#|EaMg$+npjt%U6jUM^p
z>z8yEi`eGiGp)EyApSWv@by;QCXj+UHt<X<ZWBmB9UJ(IR@^3#XdN5)7~5Z)K%#YQ
zU>|w($Tt!Bcp{%|k#_@$R<w@?FS5wHfkf-rz_Tp!ZXnS*HgLrv?*<aBV*?*&k#_@$
z*0F(o<k2JF)ik<Y_39o6foX5q;I`A~4%e$=1D|V=SKd*$jYjF%z+D!3<sOCqDzJgK
zw8$%K>j7&|y*h2+6YTI+d0OFGnheJV-rEusJ@Vzaw7+%x_%@i@%OhVFcM|2agTG>}
zQDbJ_RLCRvx5idx2OytL8@O$GtUafYd^$Gp5tc_gDv?je2A*Jf?BSsFH6=MVaNF3*
z>?Ne#v4Lk<y6sRz+8rCXVd;)ex?5<PNxKsRvmnaSwN%+iyJG{Np?Q?8dO<()j>J1g
z@Eex!gICf_l6c1kW=>}M7?66$2tLdDbnYH=6|+S<J}}c#=H4D)lJEGy97p7lFOVhM
zvHezSLO~o2%+~HqfZ5R;ADF4{_`oa!jxT&6vZ3bvQoYJ!r!d3rxvhGJw?YaPQA>8T
z;wyMGQlH{I@`Z`wy)b~{y}{Ved6wOtvd_JgeeR{~b1%R8{}ca;6Y2SLgwz~Ozq}4F
z{Aon@gp@nyT7f13F%=6DFcTx@2p#!>s!x?Kl_H+V4@OVUvuwAN9`1G0LyU=z!(~{)
zAUA4r=SP;isBQ1oTV$-*VzeuM7w<6>Xp!Q*FqV`J;!fG%UdjgdI@w^&l;?-2X4w-`
zxY!AaHIB7RNZ4iV5mLD12x%Y>q#VKH$<U#gBlIuk2;Gf2LhoaaFqoJlj1AARzM^WU
zc(rQL<4Jf|T%u0iAQk#QwmncD<A---Z7}_u;_Vb6y#KRa3Z>!*2dZHl;T%1qTx4gJ
z2JlZtssUeBl6zs@h<43odbjk4{JEce5E!_Mk!rxzl{8pgb#$$2x|%?A4Rd~ljS;(5
zLDg!esvC%^m7J<ghlk3>tHc2&*#Q35NHyTgN=nCwu9^wffd?6>0W9i@pMatZZh4gg
ze93h60*kuh-+zd8RnfK7baexZy5gT^h;_Bl^@8c@0Ty*7zWSvCD3B*Uz-LWY6Ij$0
z|C~X;3jBv&SC_x+c6@2$54<n`HrRe#8~Qi&{#vD}rE`b7|2z-(zHP>%msObYEYU+G
zSiWOgqE`!-=&j538afO8O1+<QmV5tgW+G=X<5?nSVOeNeqAxh!qqiQ{>#C(GB}*&B
zr%gWc0(n@5m8Q<l*OS6rgy|X66FtRPrng4_(N-hig+{6Y=PN0nsf<Sc3EAi;xlq53
z>8b;fa=PM~8VgnsY@4ngAf7l~@<mi-z=AxLFC-;{vHB|NFEjPMKomOl)r9(}6=U@+
z)L(4syMaaZ@}1Af`{)5otiB8N7n%Ad5QWbBu7vuiCw5jHpVMnNk)ZFT_fr#zdtWer
zq87%?e#R1&2^Oy<+P8XD`Y^qQ&O$#@@28yQ-ZAD$<Sb^6HI}F<uy`$zvuCLExq4l0
z)<ZnsOhjHFe}S<?#e)Sc$%SFG&e$Do<~D#6l;pZ4KD;2fy$RNVD3Ir0z@n~ritnyE
z8m6lO{IZd1z@v>6eFZF5R7cTKCe;L@$ORZXHAMwxnqULC*hn?tT}m2Wnm(()&FF5U
zx)d_H$BP3WH%2GU=(nB`_~LSm(PE}|N4?5A+yL%xq#E#xO3Hm^R5z;gnbARB9C(B=
zI-eOG>luM3F2~r(=e13+4y+id2Z%p;%@|=JwZ3ag&?yH!V9nI`0#WGH%g+}ogI6D|
z@3WNU9MjbRK4YXB@Hr*r8E5o@QC-Fv{oacM-!ev*aYp~}jKHC<<@L|)gc--@OHHs2
zeBMYsK>W!w&Ik*MjE`31AJ(h9tO0Isq#AI7lH@OGO=~mwv=W`oMw?NumkvXkqPNb}
zt9wWZzR<+Vz#k|{zH~SwdRewaZ@zAF4d9=Zl=~GrF{?5pBYjt|5;*W2C8Y(IsoM2l
zw`mL9%1AZf&PvL4G5X72k*xv$rlhpdgkTuZA!-SQtCWUaxmW%YFUnfTJFGNn-hfPr
zpC!LtZFZ^iVspwqYV#XPEBIsgLf=C@nLnX|q?4hfI2p<%{C+*~TfSEuOGT&RP-=K-
z>cSE2P%x*45&k2v_<tiiw6tAh)zLzm!~dLj*_wKF^9;P1(q!8Y#c&nYqS$^W)<xXs
zYn@YXHEgBqh5ndFe;`-r54#t-6za)ZAykm;k5E#YA(Tt>$E3h->5o_{>W@$=uW=}t
z?vGH`)iab#)%a-jbedjW6~NzCTB637gS9BOz{DzeaQO;qe6F$=YP^IRBUh-g-3x0R
z>dDF(DoEBil$3&oa)}z3-pFUiSiQ;y0G$1%EziL3Dk)!{8NK{BV+_NI)z?vvl+zU-
zWn;lgN-%!16$@4ooNEE~0SUo5QEfBAwZv~^#Ohn9-^A26fhct9Tc-YJo)LJ5kq!o~
zr&-Nq8C5OUP2e%hF)}*NGXfu4j**cMHhMX>i#Em4dUf-ot@!5!+6L<Vrh0XJ;2kWo
zCU7Su4J(~Iq`jLCrg*+972d2@rw;s)x9GJpuxz9zu&$)c!m&D->iK{#8L13>!$?iw
zs+P(!kaEb3q6~eW3rH=53ph==vZY4xcTBPg{H~I+04~x26%%zpis#gYb8lm<(Ak#R
zUiuNS#0PY@QK|WQ%@^wDb@bQesG%3>MIDW&S~<71sX!jx-+?BPIOk|#OmmXn;nfhe
zko!Tfmo7&goX^-HZs!YD{x{-4FFMni3%OENW}e|yT2siiB27$BX|u%S3@fs?X16eZ
zJF(zm%6Ph1p{9^4rHPe-bu>>%xCo@vge*&ooEd;DDUy{h{l8&B2~#sfomx$=d4S9*
zmY;Okp{z@5D49xiC@1Mcj3rP=Kzxu4hR$!OPLu3H-dWODAO%ax#Ya>#Yk4{WAy?{*
z2@e~E_y|OK!bfhg(53B%T&blG9=0H{4@7ywKKl*JUfO=hm9h`dcl~aa*qFk~UKa0N
z1F@uxFH<%DGUE;4Z{FraPy;@pBzJUbv>E-$Glnjd-pO^WsaH7#182Q!>Vc;zDOb<v
zYR?#|U#PbpFqQ`JhJTuR;O$Dv)ic`XU&aV5D=BAW^o{q7v6CNnm>+fEBJ-mO#IjRg
zZ!_B0{AdEPoHH`=eniv#akc+BGungtF{ZvJRusL(^0X>C-4u1BXg^cb9V?37{`l<Q
z^Q86YReq8XMQ@ny`g5xDMbjO<dy1l7Q&fu;)ljrX=UO3kpPF}z8~C^8QWf~Tl7^K|
zh#wE)RF^r`021)f($oo<^bt33)JGh>Erjx>DX#!gE^p&38WC%+#M+}bjjE~@m8;5I
z`1V#aMC`2Y?3_E??uY%4GWey+1G7X=hVXg?#e>DCEi!br26m;Tt$wn$)wP}53#pq-
zs)iIc6Tc1!%YUk3*Gu4w%~%z9iIS4Nga_X5D(b)PRRMo&7N<@C`|zT#RiS*og;xP$
zFY6^NdR0;5%u^M}FV{E_Q94lLpbY**<HRgc<B)(AR0<X!SkyQ#Xkc$zj-q#XV0lAX
z<W=P2{5gN2-9sihPZaKEaMyq2t728J;oH;Dn|gm8nSUeYBQ)-R{xdY(Jy^LCUmwKK
zp2{K)FBsX$8?PsF_;i&&&rC(Rf#qCpYGGt5WJ=`bMwP$6vng2a^QIO@rb4EKsXt@F
zmi;IySW31HoP2L&Dr8ES+Dzql>}(2_9lWU}k*Sa=VQP-<eLu84umQZ=NHyRuj1(0;
zR#ZpP&rPZcL{WZN343gb3jDzY8^BM!&$+4z+*L{W0gTbdOT#j?avBGILP^6*GZyr>
z8GX{I?wHDGFE0-K;&P0PDnrdq7{(BFY;C=|1s^=d;-~`0DJfg<&roCE(W{dMpKrcZ
zf!|kBCi}}Fxl6C=)yab2P@1#?@b5~>en^$UNDC{fbg$aNMW-IT&|ItBqwp_!b!=gV
zUe*F{ZP}<%zgs<tn(S%HYh?drPXg|25%vPVYor=+jkoxsw@zMDr{1vcD+9fs-T1k+
zdC-lY`*;$5zGNPB1My^7Y5I^h1K&;2;rKh6khzwaGVmrN^#X4(Qgl?LFuyi8%Rp)+
zEB`BDtxEf742M`O&B+RVf}>X%+`3j--KH{(?0?d#FcCOGNf}@0L#S+&Qzk#^vwl0&
zxRU!O%lhq5s}~4WFH--QJc+Luz;cTvAo@PXq3Zo{7FYv#n~`e3+m)2BT#TOajKJS6
z$H?e4&j@^dIYvfn3}cOon*IM<-pR3Zd^^nE2={zp)A8*ve;IR_L72lFX3Sw0VGi?{
zF^3H_Ra5^py}I=d{2i-Qw3<=?Sc_smG_eZ7-CfWQe#o<hPN9LE$;%QT8uR80^C663
zHgd)=7l{`-*Qh@?n_p$%twyR~5ruLf1-@-u<<+!xeG5k!9IY(tTbDP0_Zg`hc)ybJ
zXu^DbLhpV~udW*4vMDPA>q^Q#4%E`Yu?1Ohzn7h(@QMN(c#&rV-lC-BCyqk(BjqFD
z+l@Q1G!}c`QE9o+pWO%p9;*!ZnB{1R*i3KTYb?<szeI0sKAeT70er|veZWVJ)Bp}&
z3Bf+#%1X)$#b~@|1a7+=Bcq*-u>t(^C^HhKAr7EU0K1vDH6VdF*XnIX#3C<CfjE;h
zGQuBW1kNtRP;WDO&SIzmCyvNt$Tc#WGm^+^z>)@=Gcp=yDX0PGFUQE}7S9NLbU8*w
zKB-aT#7V7?)Lks8y+G3F9F|{xR|YaEs{@cU=8TNUq%Z<WW6sFvcb3#%AZg4Q8IehO
zdlpC<b4EsFQW$}xF=u2%CWR458goWQKB>`b%s8o4l1k2;u4*EK)l>$n?Kp!~GKe3!
z#!i!FV=HD0_=b_1K+HH@t%T7QMtcf#q19$I)ymccQqf!^qs5*PNJVo-MqkybB(D&G
zR5WK~G}7im6Ie`4{Qar8MqQ~I$?vmAUEY<dQCA{eU8Jk%O8oUJT-jGGxeX1Bb=ks!
z6w9$uFe>0^U`imO<o)-WcgF@^-%?xyeppFzh8oh|MhBEc)B#V?TTUH#k*TW#?=Vsm
z_;V%6D~KWOXBpTESs$iwjFFnaj~S^B+|Ni&;E_hE16d%Xmw<GSJ0NKT(LFAv4VAXJ
zUL6g*t2JB=_&FtIX{+mibwx}7Sy#jnAd8A~B>4Asv~QQgM$LYpiuRy6<>>*CF3I>9
z(3`>sq)juvz>N}qlx*(yu;FjzNoYq^;v?j=_9R<>D(5P^!jnStNOdX8@KV2gm=?kB
zEl2Gndz`7S0l%gssad~iBb8E(#ANEE-AgrNspvRHJUAk@fXn&Bz-u3jPk+~nFr)Kq
z-L8{@UeBpv59ZV`%21>D8>$mhs7pwpqDytQvlQg!n77Q#oEpxUv}<T&y^%8;5F;|x
zu)uO4TtJKpS7=d4VF0WE`{@kNoeJlB+Erv(DkMLlU?OG9)Y4~H<_OsUK4+vJ;8G>I
z4O0Hjl#cdY#bkkJ7^w&NA4<w)!^rWY*NoPIm~n#ghqIMoRkKhB7A?ddX2rt#W}yQ7
zt&zHcm~locX7o?SSOI?B{OAT2{fIvsyRqsRr&qV~gSRvNW#A4<%3c8eR0lT}WWo1)
zS>QuOSt5F^APauS%L4ydl%=ebHCX2}c#0*U3_MXu$rr7Msp9^EAK-JnAHegAew0-4
zh6O*s6TBb5omb??2vz*F*=&-cS*9wQp&wB0pP5t*sj5ju3)39E^@_1Hfa|Wx<Y@vo
zP*Ofq7=6(g8^CWHsR<nO0n|kgey>&IOZ4iB2wrNAm4PoRDJvpQY^xTXEco;0MH#ra
zk}}y#b#O~T_FlzH^(rR?;EPJiWS4PY!*X_qC|=t_EdxKKq-5jG(}31NXQ@V5K5J3w
zE?24bO{zPVik{Y^uxUOvfY+=+g=)Z?l#~~o(ah1t7_OE(sM-xyhouL2x$2Tr$D$Fa
zTHRFj0MVZDh5C12E!TU4UZp(1+mtl2H1*!nh?7cVAv~ftR_4YhH^Tg{EaWoqB_){}
z2yc2`ATAqk=<f5HhQm#^0{nrJRxX_=kH)6nJ7UCG)NNu(sQ`CUQh^)N-kuxS9dkn>
zyNS2i_tM&(s#n+2U~(u22q1NJY~+&&ge^=3+Gzv%%~_<pPtniIuBWvdXZ4;8{GyW9
zEzOh^j-6IoXPpf<-stV|Z*TF|hql<T&F!0%Z9VrZ66X$2aKd4MiMot;?6w<)T5r^X
z{@nU|GH`1x=(?qoN+-wuaGScXCv3Tu&<1HCnVmg{V{NT>2h-f92RfUEgNk%e7}j<x
z%`q92UnP4`6Lg$){zT$>+>@AF?|2e1ZE0;ikxt*wlR`%mzk`NT6RdS60qKqPq|#EV
znQ^t?pf@(mIXe_A2>-*@K%zYEAyV_C_0S|BO`?vS8aoCDZM32M^$_Xl4vuMB$>|EC
zfQfGD>;oJWVTC@xL`!v68+H|Iz3-~Ytf#)KCeu}qSPM<2t3GP|GMN^_iy@`Sp}9#~
z#lx*%4*QnEIYyeamYVsXnLBJZg`YRlq)_*Ms^BP7bU1K|l7`Ah7sG#H24qW&*V>HO
zNs|7R1sO;>hm}t3AI!qts}7#5SN9`txw&P#k^Fl!^y0nHCG?QzA*I7YmQ&RPE(!~^
zq&pK*vjeH36QOwVUT7UXR6X%d`v{G511t66z%fc1DsOd#|7cq#ZT>NPLZT<rAHH8*
z^?p>Z#R$-_fi`;aUg!gQXy8O5w^S2;b{)pNVHPK(J`+e4oe0H?_d*BJ!*oidf0o91
z?pmBK8^H6Fl%G@?t+S3XhUUJhx)$iw9b>?otch3^*iw>wkg31TV3OwpeoIMC9r)Yk
zZZ&o+`jqP#>d+~syagoKp{0{1py{J)<7*2@)Q+uF*r<}9P(u@9LJFmRtr0w@S6|1F
zvs8ljHx~6o;8RK(DqB|gFU;wROu*Z1DVhj8)bbyjctbNwt>hOv#<z){<YmOrIV9-@
z`Qs2kuS^;vR3?0(s-4uq&zqxP3t39=?B?SGda*dZS(;w@ZAB@B_Y7Yp;OrGt>3#JG
z6T?d<$rEXEM+fE9Fsx8kk`oFh^3_srCa%CMc)EQsKra@j`D$gqVFjw;xyGvoda+pb
zTgv{PsqW14cdr`g#o{WjtP_4My$&tKs^MAP7Ke#IFBYrbSJ|tkx--uKUNz8*#U9O7
z_8;hVMZF2{dENx@0wtwZkQ?>horB=H)vE@2vDo{Ul>N^uPz}!?y=tHri&c-&Eoxi8
zRRJ$HQaA7iO3Jr6Mpt-7;E$JM45wmjw2slDVEn8)7ObQM<A=+!U=_g&&HEaVz+9Ns
zHls^CBe3Xg{M)0k(H2H8GF?p|W}MNM8NJjq0*glDH`za@0w?O#odm-CFVuVE)@Ac<
z02@lmE6iyAdd3L6K}k6yqp2Gh<G@DiDWml^Mt@=6)q(dHysNhvjrb5o>%fE8&q>a^
zdYjQ?rOL|@;3n#D&d6wQ3!@GsvYc_?;3}!$Di$1Rlm}rza_B5n+KjN9cw!cpv6?bk
zO&G1Fj8+pyqXWd<+B;7c`U{M8c`Qm17g`a^K+5Ji6HMJi5{QRVEOJ2|g#@IIA_$!8
ztK3#dT}2Q`T}2S+Q(X`1ds{SP4kp68Ey9C=MCDGvV8W8WssSWI#}<a@XmN}O6C+VL
zHZWm`1t1YQwlE|LSvLQhgoIe)omfi!t|lZquYN#Ptf$wEh%(Ba5y{3IMNS5re$eLe
zZVJ!SYbMO#UeEW8LQ<OX#d));e@-WM#>aqcNq2-vBHUq?q%x48<vo@DHiN_rxWJ17
zeM&@>ln{b6WE?;_E^_pN-@*DyVem!_0{@?JY^r9!YnK;3RJdNOaF~81@voDsp)%nM
zRgvB=>SmF@NcgIbcCg*dy`W%pv2A;maDL|EOh=ZSoXV<mvu#37>5I`?RH5;XvrWh`
zGV-%9`u-@v9OQRCAQ5HH*BG$Uq}S3g3gIFI+D=F~>)ncG%L?2&OFhlpV&L<eTH^oL
z+<H>`kM$+Fm}JC3EhiZ#i6JW)G3>d}u(V~ohLuC3!u9Vh0M7?;{@<7jLrJXBaVUvh
zIqH#EQ7R{i1cV-aSQieeDlK}ek_sC{tf)SSWk1$kj&-N16zh%_qJLX=n1C4?Ep<q?
zV91nM8Sl`kx%zTz;C8xPk-+prl~7iS5DE$@^!zTh_?Tb51|hY+`p2rV{9?<5%&AOE
z7OS-Pp+<4_-&^=LQP+;K4I`x}Oat4Dm#QQ-iGii+u~d}u!-M3>%ZW8~*CBI48pPHU
zE2_te>an7Z9{K1~io)oZY8>=SUf6J}NS8a5m2!uIiDrF4EiUu5T~p@<P9&seX>z}*
zSNYK%;3Kx%yAKP70V4Qw<&r-cR{|p`C;U!g7;!JIz=+G-2R@+MlOG6Lwo!f#e^0OC
zJ8+GUa76C~uB9aRvNa9|T_620f^{H*^3mrx{b4-F?Q3!s;A-YW4-i%POXq=gRnfJo
z>FNd+b;ZwgV!vAGLQvvaFo<}%5_0n*xr;RZd-dw-di=(DF7MWB>JiU$y?4FwOeukp
z8(x~bsDA{bUl%yR&v@E&@bgN`r5U~9Im7gqHd5c$v}E^%Ji90hIgt(r7Sj6tC_QJZ
z*=zJVQr^bie%#d3SO~<thP*F&?(k7##*G^D(Tsr`guiB%jOAFkMdYR&!i#D(A<HfD
zVzOc6KS)L(pWIk+1i~;-1j3SzKyDBL?u8KuOX%nrt={QY#RhQE$C%u`z?+q{vZQt9
zh*es{Hflp!Hr}`gwo%6hQZl<<nK`grB_TIEYM(f!YC<kvonpBba*M|hT@85XHaW?y
zPOUbhT|Z%rKw``p2M)3eqkmDOlAI+o8DPk1>x#@~tSQ6sm!<xq6(6Zq+<F6E!@?~C
z*EVP5j{$s02iq28!3TO-V6`aQ)WO7pEO?5S1)f-xovnkp1zGR~UKV(9QFinud1cnq
ztL&S=jg*w_n`IreJRh*T$Tv|3U(u_}A9$LV1x_!@jvAZ$vbJ8G?4^pYDbT?GQkuM#
zX>ikAMfM{oC+px!y*hn6kINbM)vKd{Cn`;H0Q{1YvK)L_2VX77g6Dc!;CV&aD|B#A
zK^FY1mj%91lzmSJqc+QZcRqv1C{26@j#E<R^D#O&r63FbhL^oc;cW#r@V%Z5_<)i!
z6H`8#yLE<MoqOQ9N)z|~3bX~ZJVfD2dd<|Gse_9>AMlbQA4}|uN_1wyfAzA!zZGTQ
z*TKg(&s`Vy<|w?+NLApkl$15q4O`?Q59-yKU8?xi0_|PJD{pBgfU7Af3-?<(__1D{
zzH1dfs8?}s<i~S0<Mry;{zLKn0&Rz_bG~=<>iAe3FVwZ!(ZD}cnwS7yp`^^c>viyg
z=K~JaMct_buVxE<8MvmBGIi_gfW=tM0>4>MH$}J2vuyh;1HY-HOkKEbu!8JtD?<ag
zyOQL|!I1XdI#{At7s8{8f3H_@?M7WV9xf~(&nSLNufj&_%-23~+OAT3i?vhvK&@uW
zR~6W;q%5e3I-oP0x{{W-u2=cs5U{1BVWm@sw8v=KH`l9p2i!_YnT^p}&JBE7fg39+
znP9fFy>t(;Uz-RsI<j<veAZq*RU0mHCc#`~F3L;tVVW#IEh}MebG@gO#)fkw*DD5@
zG>>Z<^WVLY8?-zvEe3DWSZQe?(aChGkUktWZS;_u#$)RmDIYn{!VlZU+@ycr*FG{g
zndw_vsmnl0J+gF4t{uX?$}2bIH$EiRbGOQufh+WCH#XOZKc!c@5l*E`wZebVtLsAW
z{koLPxoYuD)?rqejL4+E$tJ&Weov<^N570Sw0npiz$V(!yA0$6=-%N27j!L)j>_n|
z#<qhd-XfQH+i#{?KFxao#HuVJbA~WLm4x#^<O0e_oqS=LM3JM!cbLyo)8F(i0KKB_
zSW)yHq;UH$ein#hz(uydPX=CWq>16~E>SL-$@V^<J-sv;_#-1t1kx_@N$_y@Ec)|`
zE>LV;nWY)pK<o}hfv|N5h;xmbYuk@IzBCpBVY#`M-3T=h7snRU0D+J?Y6w@g;iZ}K
zHdnZ?<<!u6D4SjpL#dJRq8YOMM$aN2w4EpVO_9y?7MqX!KFNAo4~`T<3Kf4<*B#a$
zVPS!xITKPx=ybf?re&l!T&wehh0Aq1jiGFMoex8~E#3-3mT+qrR+>4!9c~U8aUjXT
zZ7Q1#yzj8~Y`!h0%m#3OBb9;Q_+-u{GwYpbr9#!WO|A|s$ej^AS3qu($yE$)Z}#q;
zIYbAm=v5vh0}J}^?daELroU>?>tB-4-%99T(osLb%D3eVdi_r%^mirnKQRdXT?ze5
z6Z$I&{YyLQC%^6?Iv54=3K>YQ9b5R1Qcl?}ubc3U0&i+%>t0ve=<9kNQi}GSgr?E4
zrW%_1O;hyUTQpr~KFPO#)fdFw*#x|)sfyl>{FJKbDyRY<sWdq-1CLQsb`Zw-lp8Pk
zHd#C~^h4&_;;3e;ssu@O0f2Y3L{~nm@F2Z9Ht=ztZJNR}3vA#Yc(%(G-dbP-Kj_)Q
zrr1vBoi%Og5Fp8r7~hOyEUVnXe%=C+AL-Vi5t}X=0imEUe(;IH>r7!Ukfs(#W<?7W
za<i;6qHpxWa;&NrJ@ILBuHHP??3aOOnCj>+BjDmqUJ7`oN%e+FU#i?Mdnw>-lj;kl
zUQ_PBdMV(yteF$-I7d}3GSv;>Cnm75G=US8<es%Q+Ki6;v@wQm#p>&*U$BGi5X)vV
zK&3NKHv@;Lfvh$Ru$r@WGW;{MGU}i<gC$Co84ujig6+eHkDFByWw5iw+6TlT`HO%D
zwHe@D;?LK8T}_><S2wl6mzo>phZX*zzy|)8u~qP5FLlgmJ5VvsI9iyXt5pSN91nOi
z3$hFKs7!oqX{2UzBfUCJ;4OTnfm<snTU(#j!Mg=n@G3jfR^<&8ZlPCa0=$!Ft16u6
zNhc_rrB^2j{!h;qCf2==wcR-Rc>^-NzNSQO$jfZGL2fCuQyk{_zjac*^eG~VnoIhF
z6z^|Y?<3n7Np#Wms(Tx2g&K{EONrn6xkaV!wCP?39&JwcqR5+%R;kBS!P8z5@OMg*
zUtn7_g2A3v^|`bAL;W(Z<2S9IQ#ELi$qKwh3ny}~%tRScXT-1)D0|mR-i_Q*mcVY}
zMOET-^AXL`Sj$!!h;DIUR)44}BZgjmL6scfl>rON-kF(D79ZlN-xRM5h%$LW7OG0T
zMmtXxeBUbqqDZ1SKNaHn(GGmID!Ilh1Af7`MEuqxQ)X9XO3Z+8?I=DUU=6v{>ODD(
zyvQCFhJ|FYiAP^HAFa36F_!3y);PgJmOoOEM|2Qa*78RRawGKPv#Na0&PKvzFgNnG
z(pc6-V#$rjjnK%Es{EMFM$o~+nj4YFD-c*_b0cyiG%`n(pWE3;xGd*J<YyyTr!Q35
z+=$!=jWCt_ZAzDcrx~dVq#+WsoQ;req1Y?X*0$w(5-T`+aFIRqG2H!&yqp5qo8_rl
zxDyoFL)BB%qb2591NcWH9Sr=>o$dU!u%E#lO3V*<jGj`ed_oF1c^8wO-Opf(CoYUu
z0=&b%JOlt@U6z$wlCIR-u`Bh2D|1q=+!DF+w8pc!`s?;?@IFeD=QLq{yLZZCo}yPL
z2A-xg*`<Kfm6RQH?$^OH1zGSPye#mwqU^q($?cx1S7#Ue4W)_Q3lv^nU<2Re*&a~%
zv?u*Z;lK3i41ia$+LyyJJGL-K-_}N>w30?(p~{Q;<9b#S_56OSo{OS-&R3&9wk|IN
zx3t{$0t<e8-~1pp*$;t*ZjL{Zg~^3#Qd&1Y!oYNc<*-KFC{LWxkYeu2I}VFRFd&)3
zN~bOw!Qgi5mbvpRBGQ?OGaxo`H~tVFhUvHL1r3A67DFE~cxCd!hk*~QFYJxiHCKP}
zWdc$YS&L&u(IckO8t>Xx+%nKB>WLLauh40UPk2Q@uPADW#F^;;RZ#befGCobCzq%A
z?87?K>y-iTG14Kx9dy#BORD_Wm|O+8yUqWoP%Kstnp7D`H1g(IoZsl-3O4pMTV)`9
zl{naOi#4ve=Rw=l+HrE|N0B{jK^DKyn)sxU<!tr!9P>Fk*x|%Cl*Nui5Lk*k9)}jI
z<lUW(giCjBM9y!l<;0R3NgV8+SLMI&Yy=%_2Dy<LrLhp$8ge6YBTUz5&Ez_k(I{Qv
zR+1Z$^V=#avw>c7BZ-3@8#bku-7fUw9n~>T_wSMN>nGcfJE1fd0vo*halh@yow{)3
zh_R4%v~$1-CGcnL95Cs#3OO1SG=yHT`rT&I4d4Zz&3AE`IlxPmG|D|F9x-axMr|n9
z?QhJTOk8dz>OiDqDR^^6C%KJxMRf)E&KGi$$UQTY!Mgk9ME8?f;5kam7#aMezz80x
z{%4E~)>ER>2tJ|6$l&w>Be)RTGtmQQ(oR6~rU$SfH*XMft%Th9gOKY=$SsWI#(X}{
z=@xo*Cn)e9N|W9O?xUpa1bK@N9xKR#U-h!UWkuQV?UCF4iC&%EOZLoZH|f>Uzz-=+
zd;vb9q|BE?_sV5w=+()B&sLhq0?$!WCfl=5E_;Groh<lFrHSl?3a==zfp7C{BlgX;
zZLU|R4ZNGuL>ut)O3I_q!B;#VaB7inp|&Hf<ZOhq>j%5#eq5nf#{*uZH1Pv?i;^-w
z7VBV1K^FXkmjymkls#?t-0pn6I=kSXDoyO(q42Q+8~Az8_9nM&ttzjra5KF+1K{mE
z+j|OE|6J~mV*{_JH1Pwtk&?3b$LL^(f-HC+FALniDElWJyi<@3XO!D-wsRi_38#zQ
z2>ae{giUQXzA=cN7@;fe!DLl(0zTR&9mv`*Yv$~k42aL=6MU03r!X?O+lZrfX)~Z>
zGnHY>;vOs4(9+2hzy-l&(N2P3-O69X-REs))-G3vP?dP4gQ8V5QR3#DXw8HP*RsdT
z3{WnT@!Ri6UTl8$;3sm44`8jX&a7qoWCMs*abeC#2B;DjUX4<<pDH=fl$C*(m{)zV
zSJ9FDtEynSR|LGs6m`dnqSsFIRl${B5wH-+J5eM*RsKa@4p@+z6_qbspo*t+7AoIJ
zljw2gbCDg|Xxr^3F%9SIbWHr5A#2(fEKQSv-tJ+s?!(YM*K|(~r_dg{6d!G?bOpG(
zPH<_9B{Lt=)rsgbVyNtRRdTW^D+3G4W~Y?Rj?(^)D)Evhk8j6r2r+a6$zgtl0(xa~
zPDI(G8k?-{z(SbuS?%xY8A?U&-&M6lOZtSBwhTn2WI9gHk?Jx`!%HX2C-uTaz;%KT
z5O}SUMEf%{8QkvqfEdX5Lj6Z6KT0JL|E{VVv{pA;&7v1!%ry=LZYRq=2nm5Bg4{@d
zLh=<={%U6<;gQ0OaK>OVb~LiGmU1;KX%q=MhAK;Lq`x#40!NnIh};M>KS3pT?rbDH
z;^aoAm&S7DVeQC`$c=CXu^XZFzP<A!(g}K%U)2U)X{1TOCyg{2_==Gx0Y7N_?quL*
zMw$ey8EG=`FGiZghRxwY)>7cZx|b(@0DN_QU9hQt$pGBNNOj=eN*Y@FY6*;TnIv;&
zzO9y4)|aF*DS%%$Qt!|7B_~UCCK~2>b7g{AcI|kzgNYZeVlUJ)UiGYiWsbVGp|U1T
z!NiN{gsJmY&k9(|`dW&YVENibj2r7-m0r<8S@8ftUXLf7ss|PaC$+aW=|OtUJ;FZ<
zm-aK16^jV+I3VHYeu2foNiEily<)q31K3x|C#sxWfm4(u-}hZGW5g=s<@>&n9x?7F
z@adQv(pxRl4cxLTC&|;Gg)>Hs8oyB+%HDcM9vuOf9b)ppcZ}2oUeld(72JjNu^v+Z
z+)7E_fp992tdAXla2<%SOvS~~;Ssq7CRYI#<d#kEWDjO}t^+K{y)y{8RzmLlLHOB~
zkee6D?eoPvAE)Tmy;=j$SDLgVaDkGtSHC-zbJ-*G>SV!ZD@|m9=O`(Y-9#IV9(1yQ
zP}+y8xn4&D@1Zo&2i!+VnZ94^;6t^7tm4h}Dzf`4Jj#=R$0;dO_iY{AtXJm>_>W!|
z_<B)xpGNM&(Ry{V(-fbnS8?Tg3U4d0f#3FQ!<)IbkL%TG1MjIc(RPT!|17Y9ANOp)
zr<IfiyHp3G4)VDJ@1`^{0sOp@@<i$27X?}H6DC`EPvMrjc(^FQd)R_f{(-{9o&>yG
zNtxA)bXob8UY$Dd-)+e$1K(3pCVSe!d6d`d)yaZiRGLH!d__r_EKAVsx;i=84R!5d
zEpoJQHF);G+~!~O>Uh8-{45)-aC5ynHt?>VE!56@Jy%)Wi7$I<#ANK(nP)eq>rA^$
zukwioCffEok&Kra#nh4a#mn06U$>c1VRAg7BohxiKffXGZZH+j)n1ny^7%x$!6f*-
zzmb2UF>HHN#XRWJMmAMpC*~iHlsBAc&DXUr^AAVL4VscplxQ#2&ikb`WhLI;>a^c0
z+H1LU^5~A<dPEz^ZiHnF8^Xe)Lss|o%`+ADhDp#Ydxa9A&7E{xV;PpGN<j8|*%t4e
z5eA$Q+0mJeBFv7?M9KIVFjEqbuD+*!uxH5x1n#4AMvf4`nKqy2J}{ku_o4|b?6eO=
zJ1r)^Z##Pfh-vxLDX)xRP*8Snq->rlneUYWkr!oiMlyKcR<pV1PbayfbXAe0oj;wV
zv8-gouzXF;&B2zVGH_E{2D{NursSgv-_yHfO7aaP&cuhyCaalay*c3iwvse~g$A0F
zYM?nYXdv%cGtoeEW{?ZtK+*g4=c!8<c-Mefkqj=KexwF|jHRax#EXnC%tl5G(?4I0
zE$~Kx1*5M_KShmUA(3s-|4P(VSF7Hidi}tH{)N*M&Mu6UvF3Bc&tJeWDimu$RIHqp
z(r`r2sf?(R1hODHWn#Oq-_Vo<B3n?H-qW?G&bHpF4?=1S-M|jeYw<a3XaU}>l~<k;
z0R29Af;?@KKcN-MhWldv=Xt`dG5tq-LfQ158d|LD-g<)Hc0-n5>8&^Px}xCW{k5r^
z2xJ%kp!^`h_U=0ae;c)hra{m~(fQ^seE~;{tWp|Q+Ix8B+<?6DS*>{kGRfi}g<8+g
zR3r76AT~#Tp@Yp|1-wa{AydL;csj_d8y*nSH4kOeCyOx3AH|1{kmXrjU54oaV?{NG
z_XS)2CIUIQ47TRKRCBKl!YsTGDzD5lpg$IMm}f_8Z_#5?=e0ZaR>+&IbI6pa^LRbe
zA7GCf8o)8VdE3b$2Dpim@{b;e<%xCG(Nz?T|CUB9SV;-Se@i14tfmCx-%gALTL^As
zVK;$A@8dsn5$ozg*T$x+8(7p8|GwFQT83$Qb+3!S7g~wRz-yH>Ouq5dzFr4U7G%MH
z_p-os)VEAFEakq+zo7*Xee?G!Wm#ydCw)M<N1Igi$-Q~XJ>R4b1LFUX(%~glFx(U!
z2K=CrjtHGTQ+0mNbWZ|eR{m-m5OeZULf%VB{L0`<s^)KA9T5NAAAwVU@GDm*?=^(p
zjaSBL7C-~IK_4@%8@P#*@|niy5zjcV`iZFjph~$1`d~E5A!cDe1Jpal@L%kH9sP)j
zuK07`v0NpTb1^37c9gGbN-#0EBf(ZmFfoxM!LF2GJZmph!Q1ufW-U4GvE;`OOD@oR
zH(6Pu`ErupI^9?%14;X;@=-neW4~e9zM?LTG5J0`Lq758-cwZSO1;W49;sbSQS>GZ
z;Xk5ME0w0o^O<NO!1kCakD}(*?@iOmDNV4wZknPuA}>>S7wFabBu`$#G=*%5H_@(D
z4HRo7`GdI$O(9$2(+-Od&8PcodX=y209TpFO3(zZsib@*V6>BG1b%TjMn*>+W*Wnw
zV?XNnfs`{E@7J+lB_$ZoX#^j($g9Ai7EldHtj>vQo6)B|Bam2gMn)f*MC3K#TT03q
z84+ya(@!}3s5#sM60yAB0Ys5gA1^QHf?K|(4J-=A%gc*uoV>{M3X-w2C8L|LCwmeB
zj-E^=y2Bo5DL1V0y$QH+D)Ci{SgILIMXzIE8LRHNWfJ@;bFmEkw34#t(OufcIOSwJ
z#d4O$<UTAH+dO%~bE^1v=EunsV*Mvaf2N4ce$!_d_+2GsAADnQqvr$ORpevfbKi}-
zAFx^54O~x2nF$78@O(fb$@mzMw#1{Z6V#>C^}1?ldg%l2%fAh_AJ>LRO68>-kSLeq
z4@-h_m=nDO3wp)xc_TtSl#I9&uViAD-cSo(_=NZDsy%?Tyx75p&sopFikh*aW~?Zh
z5@j_%$y#xI8+?+|Wb*}{qNHp}d}$C<f`~7)R;=RHIBT8g533<{nMrjM-s$F4H;O8z
zs5@-8)GS5mwW)rCxX3J5fQwW7VPQ~ym&r%9g9W|PZwsP+BQ^p`^jpjlnZgUUAK74l
zv|zpykyYP?wXnL0%?b7B5+`Ft%~(;?h3n|HxQSk~^Q+<=^eXm&dnies^ABkspabtj
z^hyywpRl4-@$(XErz-GDCCRT_?GL`zv&G>=?TbC{QaAQ|g1ciy(dI}#2t0odd{Yo{
zP7OOTlpR{yz8y+B+CXxW_fRa+3cm<^8U)LVj8jzifbGc{PrbF|uea?qKQK|e7X|_E
zwJMW1PEXNQ<NLOHG=Mi4sRq1VNp89{+Kis}jKH^-V`S8OI0v8_@NgyN8X5iR2xAOm
ziXE=wFj7ued|HkLD=ESFw2a_%3$qHm$w=Kml9I0|j9&DNK$4O(GMZ>9>IRaOoRJa1
zCXNy~+;2u(Kq8jYG7v>heSBI*7u<4M1{MY5)ABiLe4#nfBpEMw5@E0PB?{goz}c3G
zXrUaX+&h_64-jeh!+mJDQ#Co7Ve7D-p$fOrt78Lqdp6*c1vc<Yo()KLoVM^E{xb<=
znE>Lyt45<u1Sm_aCj|7Q1v$PAe$7%?2L44!+0g~}QJ*>|;OnO}*F#HFC&V_VP9S63
zS$d{Ui1km6PV-cr0?O_Q++9t|2DfMggCnfwP2llGJ_dajdy_0uK`{Y5LcPsQFu2dE
z*#r_v#>arPB~B%qs7u@GbyayvVW)YBq*PAxK%!iZKWrbAlP5VW1CdX49b(v$SmF~t
zWh57gj)R39ig_SXDf77YB`ZuXkfs;0Q51_Flb0%QsRV|Fgdtyo=v4;xm{|i(GH<aK
zTdc(vqXQPMU!wlH=>)#cqAmk(R8lq-<7Fd$VNFE)+7#$%YpXtLEgPMFk*b=MO$Z-x
zlZc0#q9(eRcv9H;sRaW&lTxh@@o;md0z5XwAGSWqndONWvaq06+B|+?L2Lw+X!DpQ
zYB9VRu*Jx#?_ldE%!mhJ4<wv=Uz4;|p~b*o^eqOg6<aLKCEiaqxt`e19{S$<*%SL2
zH66MOt8lEl8S8Gwx})9yMa{u`)<e-F@VR=c5Ly^LECmlcu?)h{_tl`x<uPprUak?U
zUZ!(u?2(*%8o=$0R0Hm;r2J68=s3>^oW2|*qhB43k!Z<^{ix#yQqE|6nvMl4DZ%(O
zjo?3RRjC4Zw19em#Oj==wi!+Jj6h<|85t3SY*C#m@V@!c0uqQEl7T34>f=K)F~Kc|
zWMEM+J|v&^<-9WI=~bRF0WVNewmU4;!L6PTct?>h403f%)W<Dh-Q@6iPa<27n)dFe
zj>;X|!CEJJRlZK7yNT|@O?MOBA2Qv|FDksxlVWqxtlu;;SH|2vX08mx19>m>mVSJ=
z)blM+c!MWBsPGX_!s#u{(=w1C<m?Bmd#-5;sjt+PXy5f*ME#;BTeyHDEbo25b&OO7
z`s(xoYo6<Hg(rGaTj6D%6t#@{x0d%4NT9Om5)B~;2@mM_#Mb=3B~^=UMJwC!n$!#R
z>W(Vl+kL`;cPS~GZedQC7(UwW*n?<-Jf#`*Qq5Q@>hq>LGR6BCx-(2~-D51#$s}fp
z+>2QvUtk%8FRQCB*#^kK%S9U?zCUJiQ_mm4z-KJl*_UYNu6hi6S_8Ptu{p`D(ea<1
zjRor|!FZ>O1uH4Rc&Cd6t0}?wlM@KOYqPWkJkG-I1AawG`PRhf63+<y(Q=H8$Xep#
z2l(-(`OyVD%GCD&vF!ZlYBRdnGXk-kGcxjiL|gSA)qc0>s-b?4C!y#uPx`w;uRB^M
zMk~v9CRGORXrw0aAS0E5-l-<=FwcdbCwdalyWYh0YdsgxN7Dp8<GFwYDz6xT!*w>u
zrUKl|NKN2YMk)h+vZD21cjf+>Ni~4aC@G(Oj7~d+Sy}^r%Sa924NA&o89hARWP!g?
zQqCAIi?MfgynEan?g8SW(^YRXde<21z_X9Tj~?Lnj8q4H>}2Btwv1E<j+|y(z|R}0
z4*aH(dVtp$sSYHea<&DMQ2A&(kd!4(z;V7R<coB;<Wv&Lsl>_YBRLORaw<SFlB;91
zqvfXp{L@$S{N#*`NR>P%1Cp1Vk<m{qOBLYJ)^2@3vX#p+B4J_#cuJw&Ds4s=T3_}7
zNph}n;J&P;@?1^ixf<uWH_mf4mFH@kJkxygat26dox{~OqpdA{RUnzo85u3`j6jkr
zr#>K=&1D&pT=51-W^+bHBv&*7i+PT>&A6XisXVt5d2WSycCNJAajILXRJRhTZlzM)
zvQ%$o8EgSbZf=^<Wu6guvyl!4zNRGiU6|<%$at=h5lNRn-~=qDHGXj$r*&eO))A#Q
zhP}1s7Ky}83=<1$oZ5+`c70tJvS1j{%*Zn)&GObJkao`G8O*SxO$3tSjE?~s6@5VZ
z8Pkl(XeQ5qoQgb<hR*mH(9$Sn%vP5%GoU~)hbzo`y5z3=m3;YhuXVtiD^1P;z^#;&
zz1EqhgF6ed;9q;$Hx!PT!VXp*qi`Efnx*i1y*fw04|%p%6u#|A!;jDPeoC)S@=nG7
zDA3M2A?LeUuZ|CVkJ2Pm;FC&{Q2W~qp7wlU+MT*EyQk@yfUcL9{Opf!*m0o=q~YEB
zWHkAo%wQ8ZVrm|T)Oq1_nt3<d5t~4o+Nq<L*#yNA;EvkfjxB6uN8?bn_o)T5VepCf
zv1UiHcN04uFK<AjoqkQcCw+FL=nxc<u_2{HNEl0jkbrw=14_acTf)eW@R1y5w~)d_
z2q~;>*!{(?GBFF5+ANq1WH!hw2kvCOJo!dFA&`!u($E3KN1OOzz|D*_5jep}hXK9a
ziJ|UrHWvM0Rz77KzXW{UNE3lWbp{S8eF?~fl@u)0c!|4cC-ez1=6R798uMPhfpWR3
z#zc`lRE-$RMUfX8%g2yX^tBaM1!m^x(wke1!p2+cZLwjSHvgecqL_m_zf+F2i#p~$
zF<rjPxM8%ugj2(<LClSZb!B%n@S}FXE(2LW<V8G?>FFeK9JhoG_^x{D*uX>l2^YAn
zk{lbDtxU85Sy3Dtm|2i`z_6moQ~smMsi!hrsVtj8R{dE^cRb~;&wd^n8^Dm#kwB_0
zZH&Y8Tu}p7xF)UX&7>X#gX<w$OBO!)uHh^Db@)zc$eaAGVaSwN(^=Qp-QABkY(GwZ
z`whaMb-~NOLzVbZxX<fy@MXQOUz$2*gnY$B+I#AvA&NG7YppFdj>>e7irnZ$Hy@<v
zs=AAAqu2G^=ND9Vru$xs`=)Tx=piaG(@QQGoFtudpGvyl;|4#hG-;ins-?Q_?o380
z>nVJNG@NC{d*Re7-W$k9)()Zaj2LQ7HcsfWG)|~YNTI4kdtIWjzpGb?VJ5MMF@(HH
z&qAhz%}<}0A8Y37bwxHq-lWZtDPi+rwY9}bx$PC%40)3_L#BkyFRQIP24OSgP1+2Z
z5;n0_=;hg+zV;sSCT)gH37eejIMfyALHP2kcrV=5iub~;tavZ%%g}xnM`$=Bh88no
zXeyb8(1xTTv>>EVzmP)hiTs_TFC-q+tHgRir#Tq%CY=qL5;lLUwmziK@K$6q<W1TP
znG!ZXrnV*y!e+>uv>7rbY#y(+t{jBTkT+>FWJ=h?R$)FY=v2>;H)%6uO4#J3C9k5S
zp6Be};b;=_CT)gH37g;7m$5v;5}S)Vtu-NU(q_n%u=$?aTw9+Mr#_G$@(g#szZ#uw
z(7-en@8@+o%!Ry3??a}9_q?EIoe-PlP8kb%lQu)9gw6Q0AAf%$<Vk02slKcjHzVIe
z<tw<rKg@(w1HPdo_dv7JW^`b`F#=mk${88`be1s!Z&p&y$mmVa2z+ljMn)T*W*WmC
zgvdT@k=21X?jG^=GZ=1B)PZQv_&Vv|&h%G+s1&)Y68b9%{a5u5tiOu>XU$|E@MR-4
zfPc06^#O6rWxHzi!yS1;0C<okqYp?Cb6G|=S*7}b6ftLH^e@i{q=-2qqajw(29P==
zUcgdiQX>!5fdng6I6qN^7O|o|;~TgNT?zg368gIm`r|)i*QIqkPOt8THTXoOWlxD`
z=-_91l}D`9{*8hx_@71DA=(K$T1S-kRd}*qon3I-vr*>5tRKqMiL|921^i1_mDf#E
z57nrdrXG}EZ>`Zox4i2~p}oZWsH<wQ_nWyU=C(9*P0U^GNjEDjnYrd_3ODzpJr(wN
z5`MnrNwJ^NyZ76upV#YEp3>f-kVz{foVw5a>_+#xHaDUV6QTPSRXD7)^80NDSXoJ4
zq|0CKWr)#?_seSTWWBB=FSO+)edtZb8~T$G!>NNu%okZ+fcPa78i-G>oxn7>umN|r
z5;t~Nc$FtH<u<d_HK_Wro&+S)p`~rxVVGi){6FU<X^zdT><v=ES|uAO&{wMu$fOaj
zPU%WC$4(YucN}y#L08Pr?l|b~FlhNu{-eW)+K1Rph%~)4Bq4g|qwc#<yN=Z<f1-6t
z{QDxTZIn3w;k3|?^v|gw%ep8_U}9Aa^SZGnkHxQ}Yb>3lIPC;n|Fk(=PAspX69=dh
zEU`mN(@Nlr^j_}Rw9;4ztiz6(in0(JbJ$sqIsA9MT3~$~TKZ(0#q3mT<!HHLV!f#1
z(lX=Q^aXMmF*LYDC6<#6nby^QWr=oPvFc(aarUL%Icnx*Vnm)sx)bWA@`qBweoj~Q
z6Y2|U22dJ?GFQdv<18smDm=)DY#bbGGa>^fn!L*_&c{#K{||L<0;lD;|BpW<l|4z8
zl1{elSx=5FBq7K1JoC()8D`PU6j6jYNwXa5IEqOj>oXCjtb=l*kR@_#W6vJqWS<Bj
z{66=6z2Bev?z*nqq~rVgzy7_vyx-U7{#>8!TJGz)pJnLgeqHHlQhM$rUvmq82%6qI
z>-eTg8IbBSs3#)bwlkEYGkwrtcuuA<CKC#6bE~D)8+W_Y^sZM(deZqk_ZN|V29f+h
zfEM|S1oHPODd~QJQ4Y?Tg?Y9*U#uobkAf&NLCL$3GU+FdG9`yLsAbY)Ac`zga#o~F
zdY+?9$&5&ube5w`$y1Rs>9Y`-O4x(q);q^~U!p@$K+0@od}r0Lv8nnjAf?YxuWgqZ
zx{UR6|3<@H4C(X0;M73A1TwijkS>KNJW20#p&6cmz@RVr-yoB|q{l-P_PrC0r(=D|
z---JE0=n5n##P{gd^gC%1xIQ=surl(75ec!V`4o_??9&;_|CctoKI=n#R%>f;l5yJ
zsu%1u%mw)&Smw17Ny!EIEpoYFXQVxt;t@zyd)Ud@i5LrVY+z>lB12=rbDqdxVs4^?
zUd{#13FP@21bdyT1uSE)zo?<IDy;C)y7<dD&$rZh?tE7IWJEMdmrR<JQ(OMZd1E^^
z7^bX`<{G;|G!_L~%0TOM9+h&JiUKWVpo2BgQtmj%MI!Di(<2e*B)1IDCrQphW;IcU
zq_Je4aFVHo{#8*whWUk-*qO;+LHnDi7b!P*DP2>|ZWy?|bO6(&^b+cE=$K$AwCnx@
zoc$6xC5<_qQ{(Y4r^e%9j#G@jSl2rPWji2^d6?nzFvI7;{RQqLVR=a8f%F}n_XW(L
zhmJr2rxVL2pJ}&K@_aG&Ot`6y+>tUNc~M1ok?jiCc7;70w9RD@Y=Y#G@lqORE`b{P
z;Zc61%u}W~DZ>hSGOj(aI06|t*OENIEzPsR85Jnn8B9i17<Hh-aA(ABCgi-)1g}lW
zuhSKuW4zp=<n!jF+ynv-<QHl3Ic9ZSEGupUT>C*+a^9Q71L<$&Nsn*9qdW~inD3L&
zCEdlZ+4ZvRgip$C1K!8yc25~y_8SpxbEJ&L|8)F9RGgDYBEBc-9T;2J8yuBZ>QI)F
z(uY5i7KFZ1Qz#@o^b&JIFo!y}j)hd7G?3ml(J0CjG}84hi}z(~aHA@cj-C`(Wa}JA
z#UknOE8>c54VtVL?U80?-)d$#Qf4T7BPr99UAwi0jUk$Anv@Phy(E`3D&IIDrAg_x
zT<ssO_A{h0`|e+_XG$sUmnEfr^URyAcOpenj)hTVs~g5ko~4nFj}2iq&(=keBIy;E
zM<bKXvvmWcVv&@&8P`e0V@C4Cos{VW9gFj9F(XkVWjaQYEoLN&q)f*svc-%<k(B8e
zMYfocD3ZpLb3bZ-^FK|E?g9ooA^9B0WHyq{gD8A$uo#WhrN&0kmwW(ZQf1PC5UrIu
zqg}ocji+LL$x{=!cG9FBAPW7?MPn&2@JqfT>iY#|#JU*pK&=<}fSBwxaIXA7k36j#
zLn-MXYyoSf#@FSKMB_MM(3iYY_lh*>aP2#~UEbbPg1+{q5<0ObWGB>Yvsmrw!5e^5
zchU(Eg>`3*-;MR{g_`{{R!{OFx`kGdj#km}_GFav&RA6J%BXUrc@<?yr$ZF}6g?Y@
zBfSAA7>h3VHg|`0PEU4$WCF8RoP?d^7o7LpR-QdlFc<}q3-V%5F4&baFV*DFXDvsW
ze$lCxC1r^cU%l;!C9`X^v`BY{sC}xYKRGuF*_;>G*u@To3o&`Q%0r<&Vd(W79k~MA
zn4E9j!a84Fr6Jx;#oG#BTq|lA7Z*~fY0vmjXuA!Cb_RQ)IDd)iM#`BguX;&2L&a2M
zEgOtko=7dJnbKjKc6qx7p*jO+Sdls<u~IkI+JR;eAiasrrdp4qrdm3RRj8@fQ=z6>
zkK-oC@da>vaa1AFV|AiBPrSL6vPkl04c^4Ic^+n#h`6Ssg;~8F7Uq7s$yIZ$a!(BY
zM1u9tbvK;5bmVTh>u&hoj=SNmyWy6%)N43ltpIjN)yM;Kws;fol$uoBF?e!r-@o+&
zmmzP`dcnRw>jnFMtQV3(<Oxt-b&&Fe_D3HEnSabaGv{GaT$09Sd#m%B{zM$(INMEu
z*<#uBtqBS^<&8R9ocpRy0dI;%oh_~kRi}Uv8+E(jqY?6WU~tzZzgSPDY0^s}3J)H(
zEvC=@aa%8`tqjxV^pVrIJ#j?DDRWKiXhP1joT%9`d<Z|w)L)u(Bi#xz2V+qmqNn6E
zX+}kk?HjOhM7u+oy_LxeA-nFL&nwu5I;OrJxtDyf&$dRnZIW_3d5_)l1iPW!V>gs1
z*-hhy**R4w-&c@7?UJX2C6O!gH)Mh<b}msL+iP^CEx+s1?>3zs&vMOpmTR`}ct6{F
z{92O#`}K}7ob*wMIx!^rd{XDnaJqUOFNx*bRHR&#azC8m+`Bl8ygkUBgOqO9q1$ab
zyN;_}$JHLk^Bu>}Bld4Jhf3auChG0a`5PtIL%T_P4JD0=QR&81>4~Y?iHXJF3=6&!
z#9v}&>f9^h*XWOxj$gCKnRcRZ*SLgtpa{*tE~#%f*-`%MCEw1L|9bVw^U5q}Y@YhI
z+m7<3nI3>hlNxiN+1t_VZaNwe+tiE_sbU>i>~^UtQkJH_e4(>ezPdB1Irx8%RT{ly
ze#S1v8{*F{WOr;3JotO|K3B!rXI&NNm(8iNgC60lNOxD!Fw*@Y3cfpJ|LcltTWhzW
zq&usqk~Fe(hHWTCOHuO93CvY-&V)x|p>g4PN=TJ6bxo#8*-P%vW1Z1qzl`>)A}v?Z
zFw)Z?YA^2`+W%<jIPEr+v{FTtq_h<LDdxSl_52|unq&0AA3n4GxhhWOD)2g)=Vkc~
zK%UUM>ao0p=cO+q5h<67Y|1N~eTde1oaa55_Sl(nEB=(He|WF;^!MI_?fx<^?5>`v
z!d{1Iw&iZG@}yYL)F%(aNm*QZS)OvMJDkQ@QW;577D%ew;+&irC(X+(&$V9dTCet4
zU*uTd64@W5Rd8-%pCi>)O?fD3G<hoB04hBJEOi24dYnA*n-OiYKJjbz!2E$VS&2kb
zsEK={;*}22NWM`~qcb$yzEM##s%;hPz=Fz+ij<|18`Ywk;BK@u_<xU88ogvz#V*Ag
zo*R|>Y^}tuHC0~J8R5J&ZL91ie=(?Y?`=Ex-hO;Lc|tX%Lod_n6E?PKH@+PNJ;QC#
z*`R6f4s}^Y`hbdtkv<KPZ-b_#+1hO==|d{2Bz-=z<a~ILmN@l1d$X(J?9FMs|8u*(
z4^&9{mn->ryOi`G6%~>mqoPvMoQeubFI7<~=@k$KZwwUX*_w5oIwhT>qC(RBnpDf4
z9d1H7CdBw;qgztL1|yUQ8l<!y>a}e@<FUWoV?X1u@4iLl#-H`rU*WNz_1ItG?0ByI
zV%pz%im9sP-CDy2>tx=)VT-Qh{Tot_Al~S~=&l5YHrZf-jjCPYE1fC%dm>~Q^z}bE
z_LrV6n1;A;>+ELXpC=w;?xb*y`6so<*b-+3xW?9e$<ra%s=V((%C#!*bCa@QqGb>I
zub3xST*Z~0&R=cldq8=pM7ji`@a-5jwwTJU8PWj|1rNN)2di3ot7}b<t$~3W`Hqmu
zBSz9)Aqt<dos7o0@xIp^dwT+<ubt;utg#z_flKlkkV!YvJ9J6P8*#l+n+mN_@p+)P
z0Rwa7b5t!&IuD{Sg9T_j1?(uVC(cau%(LA^>z*bZsG<z%G>C%dExl{x`4R=M>V?zI
z1qvo(0JH}MSK~rwIl|VZdhXdXPst9~n8Iq8lOSjS26>ZDhfK;yIs>9G?}IR69KoP3
zc@wTlU()Fig?&e3WUl}QeaY{NM*X`RO}roHt+?&!Fc2vpqmwdA%Bq<6=;Vz)yDwwr
z3^Z^2c|M9+j2q}ldLwnF<vh%@_hQrXKTv-k807j95@UkFr=aY(ZpHpRQ;!sBQr>CW
zr_Sn6&RdG?q78N2Pq<Tr!XyXmDc?n~`+2^ez_6IAyeQLNI%CTA16@x?y|!JxCn~_L
zO}eruD{*P9l<4h#gBkArz);9iaV~gX3@$*WUWqD3${eK-D{C}f9DhQgU7Z%k*l{%0
zPj?eJsMA>NM(MsjdK1RJjUpqx2gc+bS(;j|J5GUHGUrX&k6@E?u0v|PytzeNthJGH
zE<TCM8Yx?yF)Jk$((@rQifnN#M3HnkmZ(u=i}Ozu?fNq8DE|u5*kCQu`o5(_sUT%>
zWc-%Yu)&1ovk9b(BGj|vScdYg6%Fc=nx}9ZeG4f27-?*r-7~>8*sN#kJ~VV=tc32m
zIGo?|aeIw)dyTXMw|jL<ecLm51gpO()_QOs_ou+dBSPK^<P5<fj8>L&tgw5@L!h+}
zls9zju9Vxcdit+Cz~4WVC-rB}@^KfI_$w`SmeHJsxhPts%uS|1gY)Q%(M8tNuJyEQ
zJ?*hRHPQMMRK8tmBq<%Xmrtj-9!I(!M|wQEA4+iZsdBAXxz?*Z*4<AYe2;=}rxV(F
z*z^Qc7V6jxC5?uw(hZ=}6M*~n5!2%ojz3VMO^$c`nmwuhz?!^@MBf0)!zOvpqg(7<
z5ng!*8uGA7-Zu%}6)_OKD?*L;p(ziWEKOPFK}Q4f@Q0dFZL3)a7F3=$lCm`Nu&F70
z>mz*6qsJ<ZUh+Yj;9(Ok#T%Z7P3|iPX9KZSR&TRnuWl%XY{T{-QsoO>UDc#Nsc01G
z1~BA%q(n>KX}3z!UsY5^N=v~9%e>G0X^yX<WZ5Uh9;rCDhG7Vpr&!NRK3iq=N^AG>
zeAK{Iao(n)%HMT>s!8irG>UYhR^NS_YACLqs@*C{&s9+sX=KTLn~IjAk~xKURh+_?
zV|T06T`o<!QeWH@e2-%*kFPR8Nr&Tz6+2Ar8=bEX2jz*%b3ob}uY=P1dZ>!@ZxG4d
zY-tT&OD&A_NMDHS*|X1c>SHwAJtV5tILWz&DqKSquAvG$qJ=2gYg)QBG{iz5parku
zuJ@&$4XSMS1MqU7L7$2il5TU8c|{@xA)O4-I>9shj_Ve$)iY1gBe-N9$+C6oEoy`G
zZir$wC_4XE)h3+?QB0en<&ie&&nhY;?ffreGiHFIakwN2l3wg;Q*?i%P5PLtP0{m_
zHtEX{nF_J>W~A8m*yK1iTTEl6Do9y!Ss$}%*jT7zSV7A2g?el}riH5@WniIR+o5F`
zT3@)7elu#=U_3$YnLJyIrkgB6RW>*oj4E5}BEV3U4MuNN+4?%J$_B^WsIqkeiXU2K
zg9C0<+2SaMs%&r&jVfEQ%DG?QTa1xgrX!dp{T8C|Y0;l<HnrIs7(78C9|D=|yrgM}
z!oE|{m>ug&{#4YL^qF|y9WW*b0t3I~$3P~2M}gMH)W|Q3)LOCFf7X2<tyG6e9#3X4
z5FCTRtG$#0nLQ8fikRnEr>FYZ>FNbL7jr?r_{`)^LG*ILd?VUUOM5WSf7w0kD9^&?
z%GDr`akvCZv@m6y>YQAnWzQ(j^B>xtqbt3XwmA<y=ckLDh>F;2v5sC5BjS3KCu3Y>
zKj?;0WG^Ps;-a}rH@6~tG)2qJ&iU;2WW;&3-k#E=>qq;{9Nq8UiVGK@whUjE?wlGX
zz-=xRGKFoU{lNWl?^ML_U=$0DP0?yNXLWARTSe=g^dcRy^4CBazpOCQTAd-~91psc
zuLCfRXwsCsWpzFa@+SndZ4?wK@8|L;i4jEZ^yUz`t8m>pXPip785X%Cr8^mhrW)H}
zD6}&Sg|^dBXfM0Ta5pz{OWH+Ce;g?nr<j_fku3Mkrx$fZ(xjYX?NY~aF7?;BpC(PK
z=r~etc%fH2foTYwp#?i2Yt#qJG?0;V?+%`<ldsg}oa6N2x{$Z6Ng01&jeL8Z)H#j;
zqYc!!#<{Qq8F@54a`s3CYO9kcbLIq*{er1;MFghEnVYDQa@vN0Ar~G80vS06Ed1Ix
z8_Xb3T%8nndSN{AuR*h*3@Cn$6=gN!*X%OcKEf{}+r~qoZPoj=Wt!uLZ@#0<R;5nz
zB2wmN6zRA@r%4eh6E%tq=2V^iMWoEyDBAfhMu&A~@UUA*dK5&#!K*OORxVN`ee^E&
zEhX)Cw-L#=o|<YqcJ9|RPstbes50rydu?S(Hoi}lNza~TD^oJ#epOD&vb_eMBW1R7
zHy~xIvR^E(VI!km)1-75>Lt0PQF#tRN|VxWh1x$x?Po}1_TA4faNsu3epyo54+bu)
z1NTm(NXjuWifr}7*vV6F(hDFmifm1c6iGM3=o&?~dg+*FNjbtsk*zy16T~$si(nL!
zs?5~o^Dd<EDinLFQ0%KhvCdPbE*43dcMwW(o-L*>ilofjD6+-WMUj+w8%4I5x+s#y
ztKfe5dge?s^Irf48!Gv`kjeZe{Q#oybxh4|rtfXQpfC9x$fR%PcB57V4AjU^f=tv%
zM?w_3Sb)Z-vA*OhqP|DnX<Up01}?}OAd^+}3g~va>!e#T5!Y`v7J{BzgC81`eF6S+
zOm+?CW+zO&pfCAGIvq<%IY!|Zs<|h1!!{KP**FNoz##b`-OtjbM`>SqiB^CHw>0n9
zwm-)3{WUgX<m+oW%Sm@q(ea}}Z@t6RNj9L?8a8^Pt|DAn!^Q{{c`f;pP@av`wel6D
z*Q?&7g*99Tm!iyageGO>LwjtjtJ_04Dc4M>$JO#r=!IeT$JmrbV?=tQiZY}lAqqc`
z#0JMwCe<Qb|3LMmGWPJwYdj9jQL{}2B{kB=APS>ngX=cT!`{4RVE`Y}WtJu7;F%Z5
z^5K%@7`k6{G_$0KVl<7ee7@uYjO1myXmX@nJo1eKdEqU;XluvR6_blM1IcEd4rnwg
zBIKyBilHX!809URNj`X3kY|IOGV~xK+Fz!#qkz^qFrvi)p+gbb?*4?7wgbvGMY;n-
zvd)%vWP@QzLi2s8&)3wsPlTFwaG}r+FBIAV?t&=xjshf&4Z})bEGspZ*f6ZDVJxhQ
zxLxTC!+~&jC~zJ5e3@iCr9;m=MMH6E{nYq5ou+iaRZ4qeeytz;s(@bQP;$wydevU@
zt6t@pbEU6(m05B4SG~$obLp*m)voy<WX4=PZ&UHk7;T`{ypwTzly^UQ*WwN+e^Ti!
z+>TfQ(X#J6qE(N1&0IyRKXxCErHGg-?<yjiikrkty_q<l8fWO|>&`NqG|FZKvl)&J
zq+MzRvq|gLF`P6?a3t@yQGz4f1V_3Fj&Ktk;U+j@x|84td-#^(Wy{())XD<xmuG_~
zz>cX&>5lSiusjL!z(+Y37teQ;+b289xjAu`QqJMxVU6;np!w26o(+)lrGe~Cq>NhD
z?wY>}VM%CI?9(n^#fXSBwq3ZNajrzEqwP^j&IU^<!8X;h!Pj@;;IP4Bg_>+|5`>y;
za6yNfY(yc7ce}u+AjH1|gQFMu#gIw5q?bYz9#U;vOqc!Rwq8<O8Rp74B>RXxeMDr>
zoWK}!=dAJUKp(=-GWC}x<$YRu9f9rUNPRh$CM{8s^IcKy{v1L1eBGjyGoJE$6Y~0f
zo4$GO>deHbb0u(ajg&2Bs7CA!+^BQBIkc*74@o(8vuFz|WmwTpRBGR0yr(wjb0oHH
z`M}v_!4KBi4k)+1P|kVEQz7MT<sy3Z3I0O4$6qM-_zUGp{<`4&*f~`zA1?_$4^9cI
zDp%z90D>!a?ol5*n&^rhPjscNh}LC9@;d}}OmfY{B-iYy;{EKn;@9lR;@6U58>{!v
z5v072c4D}U=P6xlBk1aVJgbr$D=FuNY-;YOc{vGr`<9zMDc!C^x7&1f9ap=Kt38hE
zosSZ}fY`s&94cALwe|Szd>Ze1Xg6uEp`=kUD&3eWJu$f-He@k4FXNxMW9nS?@oNm6
zFD2vG7z;-se$Ac(^bw`9#^t>e1-b|5er1$bW<w+SE2GqS0GjSsMz=7Z;xizhs-tF<
zNEPeAfsn6^lCm`NDZcvfqk7?I^m?q)=w-e~YCg-?W0i*IQ+%&t5%D#x=dDs##kpU(
zD$a+-sPeI{+A7koRCE&QPZ0Tj+u%i9dsn*+CH+)Im86j+_d{c}6eaJRWnC5LJbyiQ
z?dWw!>CQNzN5}F~t_H3EDKr-WZ`&f`Jv(~cQOf%=pMHoIrvY!uo<U<!Gx6+)w>mov
z%I#Pvw*#Wwj*4<SG|KG-?1?YDHG1L;<(~LLxhK9*?ujpyd*Tb_N%6g>FTW~CH^ks>
z96X?#(!qQg+T4_RiQ~S>sw%QhJNC)jDSDOce7#C`^759oowtaXGGBX(h?En^YdxtX
zd}S(!b<#zsQt;)ovr~QKAV3K#VtJ4gj41IU%eASeT%u+dEJ4jKS%MmiR;c&RH0eJf
zilt#adW^2Hsu%0S#xyCb8y+LrV5t+LN-Fk4`bcpEDU04-zUSvAGr|pQgeR~?&J!oD
zNSc@PjcdKywO;M9?jB;woChkEoT~4j#_dwhA!aA^+D*r5C@IHGdbu%GdSY@9G0d2I
zWc<Kk&$;+Dd!og!v1FV|@oP+&J9+#XbKo?IU$f^1eMBW#<6+q!g*_PP9)5V`FlZzn
zeyDK-G}}J>P%}!Tign-s%HfBUE9tinKjx=2a4w2BJcpm|*tYnP-g6&yRh+|*tKuAf
zsIr6JO{z%yspurq10eDpe!Ak?*4k|->CP&uB#kV&haXysl6MY2u8MQ`se{wl;pZmY
zANh8MRCRBhLii??r_eO?Z_Oe@`W8g%rp5$6&^M!#+(apRAJ=-M&Z-L^gZDZk)n|{M
zyeS)HDz!soTdQ*TZ2YWL&pxYmpdufl?_i$2@6pq^h&@raI@(K>S>2#nrAg;O)H!vo
zylm{#zh|D3jS;5YGlM7o3{8HOSd@4%cF&9}$$7tHpl0u1a@+B#*;`qH8kZh-rv$a6
z?4!w1)wYT=#n$*RZAa~yb+&CDSMBmqZr6))+ZE+@x*mVw@zdiklzaSza*sdnF_!*D
z>07|HWAkiq3duLEM>`Wngk$rz`%q~64uwgH(wDp!OB!=K)^RI(wo?(=E=82&@Ng_1
z`Q{ZV-y~@-DY4K`>D%8UX?$P(h|)+>&UZP<Ep!e{tS!yUt;)4t<yx=uSa%O6FCf5g
zG|);GW>ef?JqM=iq1~jthLT1ZSGqA(dSY_FgUO6Jg7HHNQ|Ig3_%#O37rOCljD@9+
zU$e)UKB83CxV(3wK=%OM?{4zSY-l8Zcas_qK(p=dZc;N!q?&c$K*;f%l%<h_Py9QX
zt2BDa1dV-nGv4qVd~S7b(RQs}73Wmrs^pTa%+Xa@P5P*cMv*=bQSi-Z`yWpgcWbvw
z(s?SXB8@DKu??kYDLS7xUlnv!oUaP5#*v09)!2OHgL8H;R5?&ps!5Ml(J0a~h~!}C
z{Gu5x4bpCvq{phLiZrt1R+^Tg!<thcSH-DM28)4f)^qcv%6O>C0|ip{l6U{yU;Jdh
z675$-dZLO>A{`5n@7uq$RH@yDlAfZXN>W-%{KZe2WAyUYf<54_igVuSf!D+R^>uL}
zKYDhNeyX6b8MK?ejZsL-2gq{QYH{98pmo0g$CC=*=;LP@D5q;0rQA-;V>djJc<hF9
zk6rHx#<nX@uWXYdvaN|IDZr?#S@w#`nsv*X^_0~;3^2^SbkIhS(qZtt#`QSD^*F-g
zalUi>=<ZCDYS((TYrWcI-Tmp(%P_s}(i!JGoT<R2DpeUu8WptC4WQB!fcrR#1?4=A
zp9|~>5WmJ6aK6T`*;9hLQMhYduhA&N7@+$gg;&NxBl$rJHO51;?FT8;j1sAG>t1Eo
zUCtBc2SvljBt2GX^pcm0!Fd9g;tkJv!u`;u^M~N6a;+{9=bK`-$|^rRI!|5+>kp3x
zjkbSyl=jA`%PP|IRCE&Q6%hIE6SP#L-G-80q@qgFt0PO!ZJL%itv$C0SH-zSG~=bp
zkNT3OkaUlK^TV%2q<?`Z_#k^>o~;5^EF|swu=XV#tfE5Fx<^%u^n4W+k~Tf2TBOre
zR7iUH6RKq|PB+IKbKF)#FDGSIL5?}i@kCY3k)H4XJ(iO)SJRg*rYj!=Bb}$kC?{nC
zrf*UXpXfMcNa;py(u-@@__I35kkUb@mt>!XkrO5<3nKlN)Ud%Kh#o0jhk9%b);O}H
zu{hjcz;ef`*p4G8P_buViW$g1F;)`Dj2bo=km=19W0LP+kp3MJ8%4J6fi$$q1~W0L
zc1eFg7(M5hc@lgYjC=>k<e)km(`d0yr!?s=5QTq=t>i&tW;`%3N8SLLn0t1fQF{{@
zsF5#)Ow>p}hba7TT<?cW-<^R$-<C&=>_edRy#lL`t1(cslYbrw{x!;ql#4}5*;4c9
zTx0%iU|@@UIb`CJbOl7Kgj<Sm<wJ6$CqN`$1ZbB(Y`*bP3=Djb*Fq*f>=f*=(l4uX
z@b*ED<`enyz4r?+443P0WJzCzNI$)Ad$VgFyfg^^u$&#K31dvE4@18JDHzjo!8Xk`
z#T_olpX->*Bam@XaC!$aa_&-|It|4<AEsMmnv^Rt_@PO1dXeE@Uc=?Zl@to?`00Q>
z<-`B%*P^$J0@4!9PBB$q%f^O}n<*e&Nz)L8dTgAecaZ|pSZMB>kqm8`PSsL|cBn3x
z(kjq7k*F&I-%=AOWvpCQ@;cWI%lSm(>8SeOqw15gUXo!$EyIeX<NlIeJ&YWo%S`^}
zI(sr9G1pvUrxXgA!O77OG}qX>MW|^f77Fcg=LWh~v{{mlhe&>kMm`C#=eDx=D5}S$
z^>ArYb6v+i6vj%@TEjWL1BOL{XszKGG3-#tMl>{O(pWv+KX~?zb-I#2Y~L@>`x%$x
zTB)<6^~eKMsL4hYwP><2Sno(;lMNoDLYr*xAQjqVgYk!&Z16A@YO)b!DBkTcVunmc
zo~M&?iwW+I(Lsv4zbKOaioH=vJ)xEjrfGV!#UYd4q_NSPP|F7As_2n&#u|fcakxd1
zl!I*)*<y)Ak(86zD6++UBg}>k*3hWhMSKs_geN-j!GyW^L}d4B!bHC>S{|fNs3=Ry
zy2|gK*p(BJ9lYn)QEtSY{9a)2a@d5_dN5_b=#8&(eJrY8y5~7B`;#rEQm)0JTR0Y-
zM}>AT`7Jy7vr=8#_)XIZ$S5jNkz1l7w?xiQtk{O+30I%vI;2vzHOg(1l-ncivD+=d
zZYcNI4dqF8S+*_p?{loPqSyF3zFpi$zCf5~4&Pzh`7K@ge+ReN-_oTHS4z)>7$!B!
zHtuXctPC@d<B1)pi0pVoWV@Bs>b`U4TCaAkSG(4$J=WbHc0JQtQ1T|6YrWF7Ug@#k
z<UF1I1>>}xuD>dK_@9MJ3sAnTM#@o^cOu;YsyqR>Z}G8sTvhSMXUvn^b^IFraYDqe
z*%{DI6z&?=>unTaF;E`6ZMviU7yYm*uPlLvJa+3E{OP(LfJl=XOQ9)`-7HP{b=RPy
z0eK8Y%_xy!tOJYPE;WplrIE*Ozwr~SQ{z9u8gF<WyKRe&n=dluw_og9+bXNKx3br<
zdS`Gx^+c6@Rb?3I!73U>Is_u$N1A%#+OFEIiu5lkswRyrjdmWc(Na_o=U(HgI5)U|
z!6{$-d8%rwtX@^^UY;+Nxhl?EhE#b#Ycq`WF%^v>eE}k0;cv#ZX6;r*`jCpMNh3?{
zTZXh0mCPx;tKt;?5WIfj>loR__}XNgK02%-<q=Qr0nN^vBQ$ZLzQU^}{a5rVX?_i#
zldP`~6sk#kK_vF)*YIHkU$x7-^Q6-wz2=&S(D;+SK&&P`2%^v)8$8#Gy~()#W7Ksb
zvSzBH#ZxSg`68MHt;e)R)yqJ;=?my;?mnaSwX*Y6`C3%wel1#>lm}ZmVe-JuR7KPm
zAu}t_<AynSV)aP$FpG4eKK1bo<d(>U6Cr%zvlY_fW*Z96Mq)g)heCThw9Ux@-8Lp7
z+mz=R-xG0duSH)&LwvCD`-Z9!$-bbYSxDM`fj%@LT_2+0bxdKNtrwnAMbbAQGKy^N
z_?#-*^<*5+XdF4xpH);wO3&u?6kBUQMe}8(eIPQ5Y<>10RU~DIMv<-Uo>fKC9aWSg
zJyk_zq~}6p%&`^aUzU{rbDC_1lu655(WJ~cFrU$JSft}nLCQpoB3sNwERr%8xs{OS
zVmV~;Y(+UZZ;W#cU)LP6r15~W8gQk~uq^3Wu^E=lv$gM&9EU6^$1=z)n`i5EodQ|X
zTVhinn`i6ZNRjm3g~o}o$kuC+$~zsTteH_{i)9l<Qr65UvNcqvOO})q#3-^=7b%iH
z6dRLlo~<_`MN$rvv1re>c^DyG>hh?U{AC@zG)HYu9jEjmpvM7&hs5ONks1f?x=2KN
zd#oq<EXcy=`t#6uG3rbDZmci)H<8-*m?oSfL2%^U5o8N?CrjF%ETY9}@hE0WV2qrT
zL)2cv{NUsW)ZT(@2~a-R+AjYI8V5Yhxm-?K2vN|NycDvq?=Unv>Bdz~+8v^x@3!D4
zsagr?G>GKT&{X#V8XrY^q~Ajnm?Ll2eWi#yNLQV4<)mB(p$ATE?uSw+QqFEU4POo`
zoX4W|G(?=oK^){ua7}(cqFsJD8e8c-B}dv@MWv)%4)PwXy%ui6eCOUOadF;9bEb%M
z<_j&E)`E#iN9Go}b6!W?19xldv@NB(g<9uQ*7;dvDQ^alvKnip&YaWHPKMT*Oz``q
zG|xp8$jGC`Q^*Xt6yz@k+M8w2lf%s2ERZp6j%Og_m@-^3X9wF6%avsgy3spb2Qsci
zuDVc$)x_l&%o8rBfx4Xb(AyRVDd@{E8DbzyijCWcoJ2`kD4FQ<>g;$!Ap_=shC&W0
z3m6JnMXncdL&~CtdUnX6(2lvcwf)Hh9Z$^7zL=Zq7&D739-8Y|-B=>cb)H0;>)b?!
zdJ<`_^CZ$-XD9Nob$bfmk8tl#Wt`c6(zR4Z%3GGK68BvT+kP;4T=RPS+e*!+N$Eyz
zSao?e=+pD6<$w3M9fAlwvhf=p-F()#T&}6#Dh*=_1GPtX5VGKrp+o9S`D=4*MF)rq
z9w08&+E<YBAYuBp<sleb*M6GzFH-wyQu+xKV1rIQV{7O1`#oMBA6qtl!=szen%B#<
z^;@-JP~O449cl39;&~T|Dm?H>IX%Zcdra-4o*8-W!-wesKbT9^XmMlq&l20B#6fd^
z(^m#*QrZl&W`hnqGvfDIry*}mKS52S8+FBgR)(IBD7(GV?8TfUY|`HhfOn9?adR>C
zybZ0pSCRf3t%p~Uvc=oH#5(~g@GkF3fqsvKEjb&K7C|IO<GMT}iL95q)}1#`X?@kK
zX!x{M;%AGVBv{flOrHghrO*4~t$N^dUS2b1mLaJk%pz71*I~Kq&?(YtCr<M$yi~|F
z@8m=CETZJ&8d{w=*KC5hy;f)9@D<>5n8D%sVn-ZCchlE<a_HC_js4?7HqM7ow#e&2
zdGPY=nsoZ2ZcXK+?`j;f78pllz1+24&T|=$#Njfsw_LhhB~LnArcNbKHt1a9Ctdw|
z#FFO<;OYE-s0ZVXtoxI9T{>%Fb+csYS$B!vweCFqqjeTcmV#@j+%@Ecxn}dr?d6*{
z_7r&w6PGW$yhm*+@ll9m_KDR`M7HT4Efue6J}Kdo5OZhZs}nw!@ZbO0mrq$F2G27V
z+m<KN3#=zIzb6R0ub5vAj}Ll1+7-05e6MkB9Z!0TEN82s;`79ZRD8-5jweSbdO_@Y
zskR56x9~YuyVPnw(sJ#~qc6Kohr<3}fnj89FkD&0wlUAHL{tA)p{`j}?Dc;Y)mRjc
z&lFd$R_t{Pddskq(vUoWXsUC|USf|RpHg<zr)L$U>q2A}<=$uwiWEstT}6?t=(%SF
z@0ENg<2kT04my>`f%F&|Dsl}uPYC_d|B<2E9wbzn|9zn{XZA9MwsZ7v!R)(%4o4B`
zmJrE1uXTAg=tF|F9f-)D6>BzzZm-qx#YfTW;kk3Thp-Vn3U==Ld^F1E+VYT>ruZmZ
zo>!CdQMPE2Mz)=&+H}v}Yiy_=k~&`>suz;J`aIvsC?I_sA~{p0^K85aVenp5VV<pD
zU(nv9>$F7gz!m1%%0nt2-Y1<2k$nDpPF=@!`^jHVrKsDBYSx|;Or>5^$&tRPsg#j2
z&LHZXM!mj9T}IkRMLE*vRa8d$ii&ci9W=2rQs!)&vNb(YBxSlrk*z3sXR<N*8cjY!
z`mQElK^jj!<4L~4O+Mo$U*RU7Ve-r^NFbwQJwr3CAZ2bwk*z3G=XK>wWOy$ycpgVS
z4>Eby(k}lf8jFGQNQ87rT<;S!RsaKo<Q;Uvi^28L*i{Sae5<Gqm#zf{w#a8i-ALy^
z6vk`M=c5tNBJEeoaci%2EM=i@ibQS^o$r-oVEZ+-T}0a{HB>~~O_9j8?d;7Q4A#|q
zx~CFHOa_PJ#li7BdE>Agh_L;)9%^y$#L#LCU2p6xCAz~DlAfcYBGOujf@xcrXX}1d
zEF|6hC2m+nq(`Z!kaX*pRf}{#6%~@+s-hy&S5;I<I^q?zWlt$L#~gEH3TDS)>po2=
zN6IYZMn!srCX^$+N=2olOw4#-YhO(uN6Os91L;eV7HPGnSxU;xjfb|A&-jwh=xE-h
z$!AEJvD~ak57kUFr1wJ<#F5Ff^_40Xkg^Cyk*z&6*$gS`U=-PUE>a|ADI`GBcvUj_
zwyTo$RVAxcnW<IDlCl&*blE&x2Wq}qQf6or+1f?Z$dWQcqsZ36NRc$2T-Hf$2x4VM
z^6G^%yFt?^Bju6}zRyTL4{GgGXZL4f98j^wfn<atBIR-ntdYNk1OjXJK%`KvO|)!j
zI(i}!ZH-yKx6c0b5uk;U=s3`dNMyUa0f|iq%HPvW2Y<Uj{yAl|b-jNj(^isF*giSn
zEYWZM4NqIw>(f?}S#SH?1GT#eC_e9Do*cdQ7?$|8w(NxQ*ao~o+7U>Qc0W(t^AOYH
zQEdAikuW~Ct~U%@$umBqF<mx#m3NRr(jVTWsGPLJTND+Nrd3o<S^!b7k_+=}HNDNg
z<)rtjsF3u74^_(^4mZ@C9jZKGXsmIA%`sRy4)xfeYk5IM$|yp;w&Tcn;+X1*Bjbr<
zYK<F5#uLZXnzrM}dg5sE#F6#H(d3CE>xrXjHR34t#4*DYN3kc48J;+bJ#oxfjW|j?
zaWs44DDlM6?1`hq6Nmc+gZ8hPDO><-pPJN#{94Fl^^@KLQTS+tjX9Ct_n=?A!Ie?K
zC3Uc_p90dqLL?{fy8J{mrbc?CoOGdAruEc2>VouDh=QNKA>a00qZamMW3%_PFX?s=
z1%1iKKo<68<HW_<m-H-%g1+QWKo<68<FNO&FX<47g1+QmLKgOA<Gl~GFX<N$1%1hL
z*ptG(Y@DwfRRQU}aXmI3hA@}~<nP24?Pa|LJM!DQBbUCl$i&$JQ$T*Lv_E&bopk>#
z<=79`;VwM|lrurj*Bp5cx`>iOcwR#&utSi?L328~F%&R{$YFu&(79u`M}ivx<>gx?
z=oyjdWl)wuv<^Z=N_BChN1z}Rv@mJXNf61l-a7tOl;#VqR+{u1h(bM1@B=aH#VhGy
zx)8)3C-X#I2x-#EI_X7^lW%REd}-44APW83gR&CyVhapP^mXl2iZTv}7-*^CF2VYT
zLN+-1aVwNX(^IV2VGM-(!8$k6q%rUH>YX!Cyw}$>IMXrBiNe4_Jx?U{P9!HIf-|B>
zNMn&K9nU)4AC3Of@!T(C;Vm8SiDl_{#?k{jp#->eJSW8|(F|CsGhl8s+)KxEpL$z2
zAnDFIuvawTOUK(gOX$enMna)ICg-986QW9x#wyXOm1u}6(dw&2tFIERT8U45%VWQ&
z646??B&tNKuM*K}XctwY)mI7ej!l!Bs8phLyj_VaQGsisN|44Xu|g{m8~zo(q*rLt
zu|=`MaeEWo-5R+ijk#UybGz8*cCpXxV#n>>aCcwimNe#ek-GgM8uvxMabM(%Ws$~G
z9j$;xx&mfJ<Gx5&KnJ~3Pg>*~_eHt_#z*76$T#ld-5vKuP9^@03OpKBf|Qkz7eCwy
zIO-y@m%NC$;5p)+S53S(FOJ-iM(#@Kj`u{l)AH6FMc{0JqUA<VYESef@celc0qNQL
zx~;;Epu$bK!gc37fnOW<*8}xFJCyW9h-5v^(OthfnrU-txCOqW$GoAWv0bsDhPxs+
z!q6fcJY<Qmp@t34jZl*t1IH~C+I8bWoCj=q{Xd(pu}^;89J6@Rj$gAY8NX(iG=7bz
z3`QEiW(OR<)>ho%>wP=mP-sUQ3hfX>p&eT&w1WzTZAT-6;zlDvHyRPT(TI@Iyb<jk
z4K-|VYIF#G)g#Xq=SDau*x=kSs%)|Tp{kps__uSMhGoPBEu%A!Go>wEZ_#LQCJdVZ
z*`gcIJKwF?0WVp?mn(&&?LVfdfV2Zd=IIMt{USxuLsn5_>*z?4^qf@`*;@C%>YDUm
zh>UBtE{_yRr>vrASCjc3t@-9inU3tqq`PXsIno1ER7T45g3xk#wit(Ji)RqmYY-Vy
zdX~9VU&lsI^_d}M0YW`CinJOTQbrN#B^86t<;$p~bREoTjbk&7BTGuxp&lCpHI6K4
zERKe{w&N)F#L=j6bk#VDN$FbRXslx+qj40I#^PvPjW|j?aZJ@XHqbaqNa<SQn5uCc
zsd1E$#^RXj#L@L5GyS&%25<Y44}wfyDv}-rk@s!i|6-2OW?+l_4YgIymG_RO<b2Gs
z6D;*tl_GbpJ8$3KfL^n8uBY#Y+sJ*IhB9h9P0GmRvDQLdZH=@@e}YKr-dcMa>OE2G
z?!2>pJi<9u!^)AK0a5V7>EyBHFujh!#>RTOj`a}rnIkQOC@@GK8~^Ff_%pci8eE$6
zRt;`a7xE8PElbJ~mU679Wusn)J4-r4r_Q9swGTMsl4YtK4A~(#9+PwsvZN12gRr>v
z88kRT(v<}}Rcn+c9iugx)P+16KIhkHSfM+Rdi+rXWvqq#o(m}}5zZ1eSOd``eIzPN
zYb_g}Y5A&1zmDs%R)e%KRjd^YBQ8i^jB3?d%SKeID$>}*UQx>ejzhKLX;apKYx0{^
z8%Muomy%y)<CUncRrXHIHO@1Dd=HnDX8>7;q+AvM|2+;gp;!+AC129vU+5WxbTmXJ
zU$(YsRYkj;H2+^UpCjD`wqz>S*Rj!0edb7MJ=9CGzeMe4NNG~ee55ofzuG}MI+jr;
z&lY`p&Wp_GUG<nHr5ic$kkXB;eNy@eYMAwSEKADqef3yOx=ck`QhJtGx}@|hKif?j
z52Dx;g!7V^Y155-0-2O<<iqZybR$}%agQY)kIvn8y-&=n+6oxlx5@W`OwItL`#}_*
z0S-l@IM$c^yr?hfMe)9q(YPhnmwaB-mvnx-@6%|!7wfD1Q?8P9d(e)+Fy@;-GuIyh
z2KnytxsmM;3}jn>X=K|21KIB217flx!H<v0?5Q=*<CFfE!KX~!8CWFUVy1jyeDjqF
zt&465SyHa3AX+<$g_y;hP%^5doKUj(CwMlAG-nshhkCaCXJP+ST`h&A^ds-+&FRQS
z%xR;?X`#oddzAhH&JI{=Y7!*AH_E+{?sDnLpe%8qM*d5rW>;HE#Bn_VgCZi*8&s4d
zy#pdCP)qIiXmrAO$(!?}`#==x4MpQDU=SSnh1xewdasHqI1|28Q96Z5!5JYh;}6WU
zv5C$Y=b*YT3dllAo}{sXno`FNh82|zrqr>N%p?@DQ5^+GdKyHcm+HtyY;sL;j;aiA
z2G(*ofZ$^=-ePTWEgN@1D0f=YSRRXO?L6W$;!~Xwg`}Jrp$|5=H;4~XPM5$O`IXV6
zSX|2nXG~ZEP6v)>(3_lN7kXxcwGz+vGPEldKfAGd+>pd(OD!AGav4q<t5}Ouu~iJC
zV$oU}PRe41zSv;7+NFk*vRZMol(AT%ZkH;q&XPsd8J<)f&TaVtQ{Dk<^{z9FcdTC`
z5pO0rOgiaE%Bo6+OKaKKOXuG(QWjeDR(PsD%u{u-=TvqWE6e*t=$DPCRKvIcxxgf3
zQtlz)ptxllW|!?B*d49_%I-iKn<I;A8xSpb2hn2!+zrL$qFRn3Lkxv>9HG#T<SImP
zT@(rFLKPiPx&$IIwOE}F(hcc&(pVgeYi*aW!^oFvKTSGBj}|iY_DmM1OKUT5Mh{XE
z+y1w(%VU?Qklv!Rt%8&jOtk2b=MfQ+a^-lw62=n{r<?r5v%4%SIT+k=Jb#V#WlZt+
zfE%Uc_k+)z)2`#`sXp?@wR+}xVHp&<$%6@VEv{vkV;!7`HU&Cnd0`tUbW4q04YLK%
zu2{wF{7xDzFPyGs&!yRzmK|*aw5KGQ<%L_-?6}o1+Yfi@JM@lSNP6Jc=BOhtkV%h*
z$h=@=Yj&hay2Ur9uTf;{PDsTf>AkBcvUTiob#2#y0ngTebEJ$%)<Z)b8#xUwNBSgu
zhI(u~ud(GwJ0glukBx2`T8@;_hI&cy&eV7_r2A-S<)n-{s9Pq_76X?Tx};-cBaz9o
z)m@V)CuQx8MYfo=yvQSE*5ZVeIg1u4a~3Vq{ZSF)fh`ur^POpT2(uhQ)<Ir#ld=eb
z`K+fTSzk%AT9R&Bk}^^j!B}LAbr9F2tb@E}B4r&!i<ET`Em9W2cwj3kiStmvElIIk
zk|JM8iajMM_LZbqOVUM4Qbfuk1OXT4*<yzB+=P@F%EJdzrf2%H6(#FDLU)rbVX_=-
zSxSwbC0xS9I6|Qw8yq8fOhL-9Lp?Saxag5G+ECA4!iQt&9<q$PQYqu)!js1{EZR0&
z-BNcua~{MDf$cGByTY|yLE8+|bGLmMhFWx=Nt1HgNOCjk7%__$3g1G^tdQK`-Enlz
z6Lk3oTn4Wu$Ulj~ApI61@z_+y(l8A1Xa^CPv!e)P_V6%_PAFlZLmo?gs-{lg63A31
zkf8Vf8VM&#V2k_+9pE%6gOka_QRx~@9*#NZQ(%mo83dnyK@S%4kBFa>F|b8`P82`s
zlPaoU{Ez4@P3P<%5!gNOv_L(k>G4AKn4S&#a3mt#Nb6ca%KFO8I}=yyRV!_uPejX}
zcWK=B%Jf7~NILOb?i59&Qy>a<$wFri(^j?G%8`yxQ6XvER?e|C3LVbSj%m^f8kh62
z;`5<>sj4`8)ju5-=RP(KD)&UbNtda}c{Z$<)OWfaXcUgqs|hN1X|F=k>hD?qQqof(
z^4FiXW~;3n=_4vCB#qm0>;E`9EYyx^(xDocQ~%U=Cgf68af;j7QE_gqn>Z>J_O$Kc
zs5q6S%GOW~H_9YC+o;m`#!hT73+Z`%o!xUZQk<lz7Lv~Ufdf}WIv=7AshXB^@@ze>
z(qK8UwdqgVy=|vCI(=B3meSDNA8DwR^jnC6U-%(k5ve8lyF`=BkoH?a;{}_*0G$Lo
zE_r0!nJjJvvrL912<Ex_jn8F><{Mz(k36MeWl1|g6#fm?&Cu8a81yCYt9>)1yJ%ne
zE2NcZRLA;~kJi3f(lOfijCRicR)m_Jq{UB@vUKte%n5Dw6Q$mHyQgQL?~O^dk4~;a
z(orfZARP@+htznPu56vI(qOu>H7V+DPaoH5j!sY2UPYud7z}z&&5wx`NiT@Sk;}97
z&q$G!p&5&IbWP|mS34Gxwy3Cp^i>rVlD-C!*^SxyQ5BsTpIT|wOlP1xkS5(gMFpfA
ztEiB)D@4YloltLF9T;^aeMLnDq;IZbi><Cd>#`>80g-XaRyI;3J$@BMJGyfC8v_i_
zW#IKddG$$p0Yu@dZ9wCCV9=L*M%0(|j(A@?jy-Vkb?sNm<TurvN||P1B(e?t83tZe
zLq#;SiS{a@p@K+c8#)1(9#WM;(s94&aFE^tkr@uQHvUx=NxMU26xq5WQY5{86-Bn5
zjTA{=Tt$(sy;J5Z%kn^s^dN|gYqp+^6iMG*MUk!U?bIS^4~Q@c(fS}#Bwe|RB3r*k
ziliI0H%^R2wz@(p$&mJ0MX~L{%W?2HHgbMwtYPEdI-)sJRy@>WW4e|<N6J7#y|&}X
zc;cAqi6i5QV`_~XN5&I}`z2*}GG{$;G<o94dg5sE#F6#H;eJWkjicBT#|%###hy53
zc;YDb#NmEP*^Q%waZJ*wP)^EO5=_dHJX;J%p7oG&c;v|>DZ>#hQZ7+rjxEk3&r{8@
z*n*lgv@!+}^>WVi)0_<~%^cZV=Ik6#t<!csp`M#jni(;QP|sci_aTlafbq9GDPyI@
zK(iB2yn`pQ<{G<%p%ps}?$otf#55_r%l2aT>W6LSN!@`8Ne6b&{gCvp5SiVJt>+>|
z(zjMoWb3K5)nZZ{FK8S&(pT3qy(H`UIyPuBh$NS1Yl%jZBRvQ{!`^HR(m-;g|A_0c
zF(J|;We%aeq%0oSEHb1FQTnZ@WrIP=TLPpEG1Oy&L5dzJLk#uUV349m8jE*@^R_8R
zpQV<Q5b2Rx$}&=pcTn?eo~;FuA}P}|ifl1MaYEV|^)-rYF>86FfRtHFUsC2QTBOWb
zv`Ft;#RFR`iu6qyLe@c^){`EtnHG_<2tfkHo{|*#N>c2Zuf@KS6z5qIW+-nbkTOFF
zkd*0}XxNI9b)I>+<6pvLkI-aGN#n_uc#<vkC0pW2w#1igi6+ZImq)au9CW$Mkg^V<
zManve7AcDml%yojR#Xz_N!{UCy2aRa<aWgg%)t=R=9pI7=mt^V7c{c%JRBMV+n1>A
z3fkU84OP%~WXO3C^%e|$q5Djllw%<K!qhrO%rb_;BDiAJ<aN7S7w3`fcW~8FV^5QI
zjW+1UI+lSh#d@jbFnAqhdkkduQrs4HV<|OgO7tTsx!)q<;2o`#xSW%h#$*s#9Twbk
zGT}%aqlPM6L(cF#gTP;pYDLQ45_WwZ8-;2;Jr(qZNJPrW!>)F%T2SNe9eIl_ByH$K
zQ32@{5CyBEFwfS@>#HK^rw|!Mw!VlINxxb}vF+$`jBa<0u88!=Sai8OTQ!j)DFY9p
z%V`>SM2e)3s3=FeerG0IM7nGpBQoaLx@=ulB#kHQKE!s1BI71o>Pt4`Nw(CNY{rvp
zshezu$<EbeOG%ktkZdN;7BiGhl{B8L`y+pDvROCTGGDS;PqJmcWV4=R%iLtMOm?y+
zTSm(Cf@HIvWXpWXx{rX}WQ*No%YDffdy*~pC0p!Cw%kp&n8}``$(EDGlP&fnTkcD?
z*x7yEWJ{PV<B^@HsfG<kBsX7DPMuKC-V;7V<ZEHo1$O}Q9U+sO2kEX5h2Id^6OAKc
zeaVMMeMwJ__qF3#A3G+K6Q86^P2!*8$usTAb4pE;eNGGc0D`-vRd6diXFcvV6_;U4
zy<T@)=SIWWw?Yo_SCW*)l9=iw5%1v1#(mH-0Hc=Gd6y=AMXM|?x;e+5LGV(-IZkOf
zW5-3tNNG&&$+juaZJO)+mdFrkWJnh|WAdE-xjQ}=86u4g>DCw-a*mN4$MbYN)1(|%
znFt)($dEI{t-P^rz==`V0d$gz3P^8MQ6Xs_qG0bU%(K;FLz*if-4P<A$W~#bXwPhh
zcBDGZkzT8!3Q~Fw%;z+YyHqhpx`}$MAZ0M7FI#)7Vvdy28pXB)&M@FBG>#(Dc)%G?
zz!^8-B45Cnyc=-F7qEMGbt{}@z*98fQqp+9Sx>-OH{eoVz*$egSzo~J9nuZBm;qm<
z0hf`+11|OiT<iv1<_oyk6L7IFVD~QP23*2`r)t3Er15}DJOP)u0hjv%F7X6h;tROM
zxh)@#sJP<gnI0)uyu86j8V@*H{0zzSJn#gpsWG~?3YBi8TShz34OLV~x)DTX7h&sC
zRV*OAW)(%Y7DS5n>SbsTXlOaob+uOoDLtE=fURDtm?J%3MHQqB#`I-tx+>;K8LfZ2
zaRbgU;Pz^(h?F4)wlaCP7^OVXBW0BGEQpjLn!ao?O6f})57^yq+<>zTxPt~<O3Dxe
zr&&+HSvTNPU%=VC8*tVau)E#30T(mijv8<oX*}R!Pr$`)z-7LGi#-7s`vP{i8#mw*
z2HaT#E+>r#T;d71#0|LI7jTIu;1XZJ?shYqi%p{|_}z=2A$hi&7MNY4<_k$r@1olc
zX|0M1N$VgACShTot)En}fOMUWjX9&p);^H_Mrb>#tsLn%6;+VZb6`HFaa^W~Inr-b
zR6)vMOkcLTX=pi8Mr#!9{gJg~l(Ote&rqjDr15|=o`5rMz(u}*<xj96P(~?!N#g;#
z+l?D=mI05^fJ;f^0cSk{XWf8HeF0}Z0cU*yyW5Q$a4`cuTLUg5jR#!p3Aoq|xXc%D
zu_xeSU%>8m;|5&9fX8aU<)ra|OFRLWxB-{@0xt0cT;dDZ-EOu+R9x}0E=jrK<#a<D
z4>(%<49T;m2Vkncqid^>bce3oZpujeL1eZJwuVHCq~$6qBptVjnj<|=MTMm2Lu5R#
zH6c<Y{Zd8F8&+;kIp)Op<i+=jS~eJ=eAS(ljzc{*=vwqh8AYgPZ$EAv8BZMbb#5FP
zPaO4iZX6j;9QAc=$C35K(cp<A>xrYm6Gzq)hx;CcTaRK-9F3khial{Odg3Vd#Nqz@
zf*VJPCyuF}I7&QmO!dT3;)%okB{Mqx5^FDb-bcPd*JIjUX6ZYzydD4sJ;@j9(o4Sw
z`covb=SB{G`lw0){hg&Ntbj9*)7`U?90x<&sG%YnI$I4DxrUr~VE+R{td8t%q+6=(
zGTP?aktbKA93$yS$_+=fNV(yN7Ab=>d(VA{I?CDEXgFBwYFwpmT+V|q_VNtzL24*X
zdMHHll(#O=22+(Mv-W_}`f_&y(WLYurJ>NeZHhB0nm*3^w^Z>=x-X(!NSDQPVI#8c
z<U;FDM5Z{UX;N|_r6F%Ftr*RT-FO!&Bz;{)Wu%KCGBbg#m60OphMTH6(w(|fR7Sd|
ziV8{hg2;Ga>wrj+^hOmq)6dN*$D9~ncwb_J5z76Gl#W9^Ht1UPNEwCqKIg`f@x)PI
z=f;uo#8F@8#*y*F;oj%mII^BN8a#1iJ#jR6;>dd9aPM<&9L1hE8a;6od*W#H#8K>t
z!@bYBag=!CnCgk6#1qF<PaGwlINbXjo!*G`6Wp!HXXu(ryUQ$HhUGN^81y6`t4l9^
zIq2<?$ll93z|TwSuYmrx(-l_0naAnwxzDW&123zgA{yFW4HdbDocr837+`heMnw7_
zwOvNrTsv|HBjp%LM^bJ$qD9ILN3=*8oY{M-5OtKZv(a#{9@Mx>-MF0l9D8|&_)9gE
zCjA<saQ9||se0~nv_3sD#h7SPdXdtQ_ujx66-^)KK1UVLq`M)SbfnWD3caxrS$FPp
zv_2*>#VJjbk_#yfd2?yPXzslk??Q#7H7Y74ZGgzk1h(#u6iFXeQ6cHiDk>%Iu(>)V
zU7?~<()L@Z7U^0LnK;-wQWc%K<))cqnhZI-Rk6XK<(5XuphG=27`Et<G79gl&y6GF
ziKD*GjU(fUqrT3KBjbs~z4f_qWIb^-c;d)<;%M;1k@dvk-um1)ial{Odg3Vd#L?)9
zqu3LNd+T%KDDlKG)e}dFCyuF}I7&QmxVJt!9f-ve+`!1wx`5K|%1iHzh4W`%(35<y
zE~<0|=(&-|zV*$*mM~8J70}<e+N;1_bj~N^AB3TE)ld-)eW!+sTtm*SZzmXFb>!Ye
zdWPCArERVq`2-dz$4EMoa^;8?OUDgIM5Nqsq$4TgUG2Sx<5#Jnmbsxi_fYoojQR~~
zC{20`MByIK2DA6v4QYKyWQsA-WN-(;RT}c%K{&~x8Rgs!sp6T3qjVyqNl%9;^u|VH
z-MJgm`oPE(XE{wuE~GT%%_WUH$f<fKDI|SCMFpg<LS$wFTOUV?_R4b2=V<;V?Nv%j
zgMs;6o~_8dbCVkm8!c+9$nE7Uu9KntiuNjVdpXndG-#g-47MBci`7z^^iqhV>h1EA
z(U=?QxgMQiruBvBW&Xan-O>M7$Jv)*$b49{AP(80T{x&+w=~!n`1hWsZIAc@=syh{
z*Td+Y2ekERZ+F<e0Json>r?MAT<-$Ay%X&32ffwRr(V{MBf<Vi=rQgT^ca^Nf8FoY
zpNu;HxRt5nX1xq{`jfx@KSKXk;Ew3m4`|1;G2~kSw*hXSpr3}k7<eCWMS?!{M&kOy
ztxepEfOb5U&|6)7>eZuPBka<TZU1WMF>c1OG$Eebey4t4jL$B>e!%{~(ZDl-=K!04
zGl0!NJN|Qjr~c8<|0}QnSO%;F4hI&a4y>c?zay@%OYCjNvn$ZnufaT>&-vQN=x+(^
z4V(mQ0?q>3_NHxT`riS(54ijGMsDl>)Yr(b+tFYiI1Shg+-xUvy*Kazplz=o_O%Cb
zy-Rn~uMg1HZ@;s-z6o$!V0u?0KLL0v(C&XP<nw@wfS>MZ`lt3XSOm2DzYF>Mz>k0<
z_cr}b2c83*0{nP?*gwGFj04fO9Lz@@X5)H4)Zx&Cdh|KS*x3QNFR;(Se)|XEdJb3u
zJOgNjlNty41mHEm1;GCRzXW~_{0?Z_`v>F=K-!y$_C3IdfR6&{&$icAKgRb0?AZO=
z4KQ`O8+Z@!e&9mjm%y)q-vWOHjyc5GIS+U-(DpYC^69{(z_SOM{^Ni(z>9&;A8xL{
z2mE@FY1{S>Il{=l0xko72kbo9T;C12CvYF&!N6I-$AC`(ZGVRyY3$wkSAz?IqcWy#
z>(7UN@1xE2Cyq7v32^(gX%_<R{=JVf*K@!ZfL{S^{YLn?1vnLcnt`_dT|<oh0mm5Z
zhdjy?@}XV}^i#0gCBgoDtZUY1H0;&^>w%MjX{`Iog!MlidUJsDfX@N#{0fUqUd6!E
zfn$M<z>EH7^qPUY6`QtgZx>wO1Gq2nc%ZF+9PA7Q4hN0^o)7F-f;fTmfVRCw(0>Q`
z0dVaE{huLkcbu`mHt>Gnd%(+%_vd>#u3rtj4tNXDj_(uLIl9!?8x8DH=C{8G<og5D
z!1K!e`d2_c1$Z^^THuYqCg9D$+ktlj?*~2rd>r@`@Oj|Nz-)y-o-vS523`%kE<t}f
z<b6&s_WA*b0quIGAuj<Q53ESgKLhfyz<S_?z-xdv0sjTO6L>GMrP9Q=82Dk8KmXyw
zjeIh&33xZ~Uf?glE+-oOU4eGKx3$sV);|Px4g(GWZh4aN(+k)e*bmr#gt@*k@Gzil
ze=zimfNkkFVIQKs(cO)o^MJPfa`>$Vo(jD3WYcdta5nG>V3$+Ob=%&f=>Gz+E&X1w
zvn_BpVChI>{}kYO;BCOyfwun((SI_qE&Vj&q`gMe>86BwjTmL(`VROba3#==r}<Q)
zKj<`r#lVw*w*F$|^{+FGely~DEFqpDV~k$=vkZ0tZVj~U_dna{T?fnqrvYaGUjo|w
zKY_g4-;JH#z*#_B|0T%x8*B6j151EqzzU$<zZrEHj_WPmjhzpGwtf@z?f^Cehn{2l
z+4_&*`cuG<fvvzD(Ql80{@+0_h3kI?4oc9khkOaJ6*zdDi7yRo05$@v&h^Jndl$gY
z#lV|^Gl6#l?fj{KIj-k{>~HHc-sR9+0bCbx+WIY!zXE(6_(_6(J@Q-(TnZdG9`ylE
z0NVa8fc^yF^}r^etxx-(<9aKweS-a)AioRP44ea;3w#8)5cnK$F|fYY<TDYNsW<I%
z;O6I<_IAMO=bQFDz`ZXr?L&Z#mzuVn-|vl={!c<2ZP~N)we^p#F>xIW%mR-CGXA5H
zhu!~E#PKC?8Sn?-wwD<Dy8-R~H=!<f0q+Ao0(>0!6z~IJ=|p391hD>c)1C+%-)P#C
zfOi1@DE~_lM_cyhLXY{_`g=_<ab|#LUuN19fNjOUKl&XEtVO@`fY%`J8xrzAVUqDT
z5;z+8AaFkLNuVA7V#t>OKL-8){0X?$6~@onz)OKsfO}0g*9QQfztXf<*WZ`$vuri}
zuLHju0XGHO{+JK#Gk*5B^{<{{;=T@eGjKN0)?bV~mjFLv-d7nr?SPj7rvl%<I-x!Z
z_P&6;)v~`a?Ck`!<6}JRPyJ03^xIE0acv0f0_+Lw4cr#E8*u0~rr)W+JFhkE2Z4`X
zXWGvLzX00tuBrdM5l370HisVbvGr?kJrDc_`K*IH+KT@Y^qUC$3H^TszKOiwP0Igz
z)F0Rd*b~?rxGivJ;PE$@ekTF%zR|Sj0bkCW_F~|#Ks*2Tu(u&_b^Y%F{kH6F4n5{$
z>(}6V9{3sZX@@-8ivJSyn+W^{{l5o(j=WnF^6%PY>eLI^8@LN_PvD<{hX5x}GyR%?
z*WGN|Gk{goO?x!(+?l4`2t2CUwC#Gd70-*X_ZG0N{(TWI^Re~s!S&g|Il%eAuMzJ`
zpxuA|O(u^gfD3`|0&V?cZZY~9U=COceC!T${aK*h|FVCX>sJ7;0^R|%^}F6~^t%JM
z1oi{k`t;iZ|E~Z)O^Bxw<9I6YOkf@GAHWNNmjUPAW8zu}{N_H>PR%m7&1{4HfHwi{
zc-Peb6vWY%y>p?*d~E&si0fHk3vkQ(P5ixq+XL<X10XL0mIFrsPX_MxZ)2}NaPb_|
z{t$Tj1Ey`;KN9_I`@<n02W$Xd0{fQ&ZF|Q+uh8mWi?*$QA>x||ya6~3XzMed|Kj?W
z!0&*+0@q2%w-NFwz^i~Wfwu$i1I`BS|DdVgK;VbKR$%Qz{`w~QyAgJ$0o(Hb0Q4RK
zE&$r~VLr6a_}SmqKVqJV`)FVRa2U|mzXEyQ2y6o00c-}&0?q|~_OS7@0(kIz(;fn>
zdepQ>11|^uQT`uA9BtW~0X^np>u-SgcL$#Ph>3d=u&wyFK)<bk=cC`n!12htJ|X{`
zAioRP44em?4}1dnJh0v4Ce96k$2?)$#{q{fFzr);6M=UAYwG_M#L<?$InZN1w*FR+
znK%yyPDVa6fo;XVBl_(Mybk^Hz;VdCHYtC|?*iTjd<3`v_#faaz#dPTxcdTce#W$$
zfiFL6+KYjEy<pk{fF-Y)wq1{#U_U9I0SisPqkwJop9_1;$JXEPNn`Io-~ixY;Fc}s
z`nEv3{{qDEH1JvAE5Q9;HvJC++WpT!9cqE~zzM)c;1u93z?WY)b{7NZE;8+fz)RmS
z?Ww?f-ZJe!%Kz<%qb++4&|^NfKI314JeC4~0owYb-ZOce1{?$Y2e1LS>-+wEGmsw-
ztOBn0fnR@5$PWM>1Uv*d5V!<r$J_2hW49}CQ{a(6TYn$O4*(8qLw^LWj|7eeJ_2k3
zmM-z<_YYie0A2~a2RIu@dv?6EpMm{CV3NOfxZWAK32<|uZNCrXI|BCv?h8B+xVru?
zf&K(wBk&VoEAXO^{PnsE*WU+z1Z00Z{@J)b7x)<PX`ro7|F!Uc9xy4MKjC^`;9kJ~
zK->P&kRJ;y0<NyVr2eI_Qw1CjwEd?(Hg)X`To>3KXzTZbyg%?Sz(EQ6|APE8;4<J3
z!1n(&{)Yi=f6qhjMc`Y&#lVk%v~Tyn7<Qilz6g8+_yO>cr3v#3*Y5$&1<nU90Mfqg
z|7zI17I*{j@dSVWh5S?C7r;$E@yAyL`E8$?>+^x{eP-Gp0zU;V2d?+I-(EM!-vKTK
zKK_MYe_zNC1<nCJl%T&D@=jkGdtHFNfp+~yLw+@|3D}&VpZdz!tpU~pF9O>3*MWRv
z;1<BG67*XjKc?0ANdt?3w*5Dt|32^&;1@t!e}236q39{#Gr(Vg?c1B{>jJv~cL3V<
zR<zOI*6+K_<h2WMci@L#8+j{mhi^>V?mrOv<AGNK?*QHpd=U6E>~(;>PQWb_>>rJO
zSzr-x7|@O<$^Lo?_8TCd0K6Qy_O~YAF2G%Zw!fz!e;)WM@I7F=?~I)ez)gU?zc=#T
zfrkNY`@<n04LlEcA@B;|6yUYMn}D|gTUPkTZ#w4D?Z7#}1qt)374q+aKLPu#W#;K#
zz{7z@14jbw{C0%g?!Rw>KJ8_3y#knI|BxR{-iHGR1CIe73;Ykzj&JObrvG?g9q^-{
zjJy@N-AaFbpMv~(;2XdXf$e`b{W=0S1@`{M$ae=G2DIZF19>g*eBedE$-pat*8`^m
z?*O*^>W}}Blo{8<frEj^0FMR!2WZDPdaSYgEUw$1Uu}W>OJFPT2Vi~1^ylYWQ`q17
z0Q&*=0}cQ-SvV8zX5jrm?!Vkm`(Zxs2OI!A5|{>ZeYBwcHgGZU6Cl?M=jUk5*YUu5
z;3dG;?k3;%oB79M8uZTSV8*2Z_<Bdvw(GwguI~tZb8Vyd0npZ`z00A$1lTu0pL#b#
zZ#4Ak6ZEfv-s<X8?_Mj86B6t{3q8h7J;r6{``+)={|0qFuCuA*-+?Et=db^9os7H!
zcnPo(XvcFp<mUj-2VRt*-wgR2;3kNt7--w4-m}o}yRM0^KhV~H40@}pPrb$H*Se0e
zPd~Q(@2$8Q$KZr`)<s^-vn~Aw=uZGP0;d97fG-2z0HzRkXJ8kg9sgUuQ~!48-vzu6
z_z>_h;FG{P@XI>d{)glG$wbucjD-5VgL&F}eKRg60LKCAfy;oY4UAqlpzVLsM&|nU
zz?*?vcQJBX|Ff<}e(5F#F9%KnHUT?zGuOKS#{g}6jo1%wqdxY{J_-BhlJ3UthMODQ
z95@R&ANZFo&2`(~Ajppao&>Du?bmNcTx<0)*ZU!^Llffbw~f*JGcXG*0^0FRg?ujX
zVc^pV`de&k>~94;5V#1q)^_Il$n6c<{&I->D&V!id_q25kayp{#$LJ&{e7T!0B|7i
zNZ?<A1;D=nOM%tE5y0`lb~_sXeStZko!|eJ{|gX@?eBZUvDZ!}z7v4s_BQQ0;6D49
zw%z|T=(hsD0ruY6uRja&1;D3)A0+7SyNj`N1n?+e70{07l3k7dOyHfsSwLI=#NCYk
zdB6*SlYq8<!R|)?RN(2r8lbIzVLzka1iTq|H_+DKY7e8|AGkm82%xRs4EdwL1;Dow
z^nc#d*x!6FgFS#d0B!q6VBC)a4gnquECiMU%YY{WPX(R>tN~sKyaIR?@J8Ud`<wV0
zfpZTuZ9Be`_BHZRz|p`nf#ZO6K)Zhv<THS`1MdRP20jS1`@aPFV&F%>BmV4<m-=s5
z`ahukGq5B2cL#2nU~j_&y=@Zohd_QDupIc01pUqjm^{7*rZEmffoB7g#%C+^8w;!f
zUJYdb>k`J7{db1`p1^&9`vDIG+T%m{5L{0Kk5AB_fjTq;X9MRY)aOyey#V+u@D<?O
zz$Xs!&xcnae;v34_&IQ`gH6AUfZc#s9cJXsz|n{M?SBgWuYk*d%Yi=uQv>|{H-~&{
z;GV#O1pWR4jh+7j_e5MLB*b?p^o|7PfHQ!%0~Y{kFa1019gqH%z~R6Vz)?VZ{!l(1
z*Xx0oCg?YyE>nOrfXxZ@IvshO1w04%58x%h9)tYja|Glk0xtxvb)?bn4IFf|zkdeu
zLSQLyeu92KoFksZb^CdW{k)_J=dF7a&SxLOPWvGyzRtj&z}~<<Kzn>jA>Z;C)4vz6
zH_+C9Ic@Yd%NXnp+zDvw9|8GMz`p`JXVD+n0$d6llrz_Dd$*(iy};uNjNVY--srb~
zLjQeXKMh<T{n9|&-yP7qANU|}0npZOgq=Ka2Jnpp`%5AJp4Sog9trVhAuj?}1K$NM
z1#Vwx>e3(hF|ZZbv&diHX3O3RwB_f_!c_YJlkD5|U_7)#|G&VFt-n3u+6y=UI3yw7
zBFIMr&jenXpueKn<gpT%DlzR2z|Oz{#~XPX*s094?RXD`{^7u5fF}b}rKaCbz(v46
z%AOrh4gA&v8-TX}na?QXH5l=n5By*4+5P9h?_A*Hz{S8O#KAhPY5#?^V;MLW<5mPb
z4p<Hx4g9FwKfawSjC?)dhQNJJF!KF?2LaQ-9Iy;H2RIM-Ft7!9%TT}nW7_by4*Ff4
z(7zYteSu@!(4T;K8i7{>r?HLxN%1~~>;C~h56oAZ{O$tI2R;FO61WK1zRKut4xBjL
zU!N7QzZUwP0IUX%0FDG&(<3zk_QwFz!;GIopsnAh+Faii*b2RG6ZG#v+_QiW0LPwW
z?2HH20xtpH2Yd|p8qkiX_ld^Nw!j^My8>-}%J;zaeSrHV=<f%=2LcZT4oZlJ@}qG5
zufS}A{<n~?1a3aU<g*oU4`A)dMt(W4!zre1=l3J@+e2>yV0YkNz&``4fun%7y`GS7
z4;&0EO3>dParFc41w07Yd8CPZJ78bnUO?ObK=i8sHbd{t1pQ;7R{;E*)!(*HyXQe~
z7VuRd{cJYMU+>lJ|4~A`r@_yez`p|-j~y@NdFV0DZ_yqLKQ|@#YpcKQ?{e6^3V1D$
z{%!pOQKtdGfxs4E)2XJ8^MRvJGwq4MMqnPe6!-)1kMdWOV6Pl;4g-z^jscDZ)&m=W
zQ-OATN&POc`b|sd&p6k#K5xM8a@t4UJ0;}51bRz>pWFSd1Zj6OjOP&GFd+SKoUQJu
z)$PAeLcAT|r!#PUAmg$9QNB0y80Up(b6$Rm_V4X)>u(D?I|FwI(!Z_0<!G}WwgPSk
zyahN1I2ZUZ@bS}4zeT{W&oph@{?5>E%ii&@v-ugu&H!LP*c}ag1o&QpzodAN{r}kh
zfeG=xjrx2D{1o^Lup{E$2)G%r2hgs!-EWhGe!Ua=GtSkmH^*r##;XhR<ha@K9Rt19
z)!%T8nGYKSw*X!WYyv(Gd<ys<;48p}v;6b(3&;;X+gu+EJO*gTUjlh4uo5^OI16|`
z(C+^V<lpl8-;LkVz%jtHf%U+R#`@#$1$iNGH1N_k^aon{V-xggcNDHS0<Q-)0UrU{
z`922uv%r^tZvfu}egym+_zmy}V2^Po|AT;|fwsRv=NP@CfX4t&1KRo*K>jiCGhj!{
z-w?Eq1y&{ar@c#I=L+C8!0Ulcz!|_hfOdS8--qk7fh`I8tdAXUE+O92A+G^806U*+
z>bL=LIM9x77s&So9t12((4PtUoxpzs=O*ZX4tXo^XJEVW{`Igo<o$sIfrAtD&w_kB
zu#S2)#(rnuRX{ucX6VlX{u}rZ?N}X}VCQDwt-!9e#^2_^{=fmiGk})4)CG|L6F3pr
z0$dC{xX#!w2F?IB1K$MN_E$i@-9JqKe!xjUTmMeT?*Tq`p6T}_(AJ*|y@kM$&>Nkg
z-}iiD?>1mFu<Hdz-W#~bg{E!$dl>rjfzJS61lsz|uy-%;A>iXcTmMk_9RwT(90j}p
zcyNP>?=axuz%+38f12wD0p|ms25x()Kflz)Mt(7n_WEC9<kNt+0qyv^K)waA2XLDN
z{h0~zy#TpwzXEZe2y6mQ2i^l5+-Tw*3*3B?Ki|tQGx9vJwGI8=u(LgIC*VZjWx&Zm
zJN`!@-{}hDcK~pQtNiiR;QHdp=K6Y53?}I>g#M<`?+F|LJQjEgaDE&1c>M|JEd)Lb
zwDVgE`Paa$ul((Jk44@St~Pp2z$>pY?Hho*UvJvBzk8wI0{k$cfB$Pu|MP)K_MXP|
z=YWfVZv#IBegb^|I%9V^(2lPJ@)5vmfzuN7tA3~cmdJNUAoDyJ@!9cg{X6v!Mcoev
z4hBvFHUam!(bQouuozehEC-GTz6-SD%fem}@Lu49KwG~G@)LoKXL^EuE9A?8f4RZL
zeK^pzzd7W+fc3x&67+{ao&`<?UZ0@94DxS*KLS?*S+6k(^|JfbL9a1E{{zTB1}+DF
z5Bw2$Vcyi`65yr4slYdz%=M3eeW#hWUB6<;-vf5O$>@CrwDq^U*<9ZNxGV4g;K9HX
zfQ`UjxBLCyiR%l2Ex<)U+kcYY7SsLqpPXsrF9Kf%z71@-&0K#QX!~n{-8X@60~aUw
z``5pWz4^d}z!!kF{VyT!G{f}U2-qEH>yJYnYk(I5&$-+5ZwAi1-?Z)i1$P+v$-q&-
zGk`nYX|5jyECiMTZF|qb&#S<Xfz#(0JF|iReZU`2Qvc-%@o)H`vC{?E4S3f>M!pbu
z_#=LQHPElO^jpzh`(e|s3fK>JlI&eP*XU0MUNX<LX}_)hXTbhN^G(00z{h}#fcrga
zt`7#T{g`Rn<MH9+M&7)@;Cx_*C;j@jJ!RyXg$7RoKJm0){~x&C3LFi+B>nGk{RiML
zz;&K6_BQ}_0d5JL4x9}<{aJrJO_1LVyaV{)b4I@fc+-pi{-?ZZ<fDLf!2be21AYly
z0le-t(=QLa33wauPT<|Z?Oyl$pAUOa08`NK0Net&6Y&4l_9gI9RoDLyQA9-zsEB)T
zS1itiy>$soSY#WLsJJm1G7~b8WQNHiRFqh3-D^~=Xtl=HU8`|lt5H#?RU1)J>lUpG
zYSmbGMfjil&iA}`@16GuvA_R(lrQsr=bn4+z2~00&ckl+?$X=xC&7+WfEnN`z}JCq
z0oMV~f0gaO2zUwbO5oMNYk<4F7F^yo=(`8l1^f!Q4btxpwC(RMy)8c(b{qqY1Mdeu
z415&08o2ylZ2uL&KLc+B-U7TG*!T6|^0tURU<dFe;QPQIfVTbJrMKlfrm`JkPj~m>
z+HntVO~`$?0VvnLz$w5Pz}di+Z*ac<1b+1<f8Pjv<SqWb3V7k${Cx%RmUsC3y}&1d
zcKK|3B>yVd5hlO8Wp71@{1&i(2cX#d0_?W+uYsNK0=IdW%drD+5b&&jGk+2Ap7;3s
zW56Zv^Y^oX(?8(vvw`#1@%KjHGGKT1?1B6b0fx!{3-*irej)NJ!FK?~-iu+kt-k>E
zEcSN7&;A?YZ%aSoa{dN*F7Wq2TmQA-uLIr+YzvWJ1^#(p2k=GUE5J8^c78{E%=R4(
zoCur>wB_f3PXMcdwIT9nfj=L3A+QCw9C!uL&aVZ2G3QgZ(;39Sn;ihO<+G6cGw>$h
zBfxgx6F@utdhlC(#`*06+zn{U7lZ!{_#?3Vb51`O_{D!=zolRj<c|TK1Y8QV#2wLp
z81%M4Z)=Et$$vTIMXoJGe(x{Y&gQQeR{(DZ-VOZlo8a<Xiu6|lzXomuu0gstLed`$
z{l5er4QvG3^&$4G#`DzIY}c=WwtO$}Cj)20j+zjAn!#TJ{3Gz6A@UD>&-p(Nd<yt1
zumjk(p5@wsBY)!Wqk-drG2poy__<x4Lw{iY7r^1bUjoMh=Kv+Y_mOWW^cVbp(r??l
z2zHzX{0;CN;6=dOfX%Q^?EJsd*9N_|zLFogK4$`t1D*u5<=;WQy$Addc=$%vQw%J0
zdb>&Y2Oa_(4J-!6fGdHk<asYnKN&b{3;u4~GpC#Mw)_pycMmWEy<z(6@cepUJMeX&
ztzYDqTk@+z<VEk_@VpcFA7B@7$KJu^-v#_`zypBoA@c8m{}A{s@XIY(|F^&&fLm_G
zd?9c&@bkXG_Qa6x1mJIh7XX(7uLj!ncN+Mxbhi8tuxIN&Y)|{vjH`gQ{GTCr2QUV?
z<3r?~ZCGD#U_aoCz)s+zZG-FMa`0CJZv^hN9m^L2TYxKs?-#6ZB=|da<man^XA}g>
zPlJ3puylKtn*+4#=N<500ypfy@>?LEKENG-cKYiee=G0~;9WplemU%30bDbH^IHqF
z<^K%1HsI|3EPpc4mj5027GN{<o)w~h<<6|{m|YlW1M7F^@3#KuApZ(*XUG=<ZTTwj
z)uI>J7$W}w_zvLGUD=MafwulHA-5j5Vj#<31+?XRgYN^}3%Fm1d=`8Tcsg)ti2S{<
z^M2stz-L12A32EaJsSAlZv1^6(6&DYxiVlCum)($zXX0QuoL)Eh<r2J{Vd?Qz>W}q
z_z?Uz!1cgC?8)W05_lEx8sOi79l#HOw*B9N-vAuE7wbI`SOV;|5A$~VC3`b}7VskA
z3gCUfhk)+^KLy(Q{sjIe;A-I75cxA;*PMgcj(NbFfe!<pJA|LF1@3t$e?JUZH-x{R
z3havTciX<3_h<gz0~tmCkc0XAD?mHHfyigvFIcY6P{yMUXFMNh=hu7yKR*|EKJZeY
zEq@N|x)`_|cooo=m-5emeS=_MA<&lp8uhXsI3>#VO#{Y&DPSXTG4M>_slz#)t^X?U
zt-zarw*v12J_39K_zduE;DMuA|7c+KnBe^W3;FLy@$-`)7bfpOZd+hK;2@w~o-p}>
z5cvrBUjmN=PU)fi*bw>I;1j?kFxx}<>Ja&6@aF+90Iukvd`pPDw9^JWm-amX?KT7`
z?P#~h<#>KI@D|{0z&7ArN3cJv24;^8_MiJ8|Epi|^CN-9z$w7<pr;ag!t^ym{#@Ys
zz{`PldBWr`3XyLE{}AvI;OZXAw};4gfPWqMCh(&k%D)pL@1P#H1@;3D0^0QxCSMRD
zFYP@U?KB%G?Plx$2ztH%_JaJjzzFbHKs$W{_|t&b0B;072mB29Be2g=>~A{&FBr?;
zZvx&sE!bZt9L;=jF=NL##!rB@egB30SHSOpy{+^wg{1$Xhw@_Anb0rg5j$=B<|5xD
zuo{>HJ^*|SXs4gwL;0Iv*PXz-fsX-g{bBO=hseJQ{vF_Zz^)$3uM3g?4*VAIldXXL
zfwui&^4o;S9}K<-I2>5qL;2Am^3Hhn*K^_De*}j4du$@hR{-yx%HJOX+T|_pq5Pyt
ztnYZ>-mvF@kp4Hdhw}SOXZ;5OTY(#AGaoqt&re~r%d=z_^A`a{eiYK#@@GKqOyIH}
zroX$K^LrAw`Xv7TBhb$8LfCyV@G4+yNO?X5{{?Wp$ju2Z|H<GJzzlFeoaGM&z6iAK
zYlr+w;40v=KwG{OdOiStA$lt~zh1!WfY$?W2etw41KRpu0RIy3Bj6SZ);|b11b8@b
zG_V*rKN(zKv+?{AU_m9z9R;iaRs-J#7XBaUv&)-<9gBfWfJ=ePfMTz0pRG^io<zE^
za-Cz9E3CeHs_z8UQ<%QLz^+?>e+9My?*WSacKL+=ZZ5Z*ltbFdmOmJB6Mzo^CA}@b
z1okflt^l?H+kr0vUjcRk*8z{OV*9edPv!;tW8W0>#lUMIe-m&u@N3{u=*j*c>9fl}
z7<L>CJQVl~U=$c;udPqy79w2>%F$gp`=WfUz_9xJU-iG%js8;DeFE?#;2hvwpp?%p
zzwqJ@QZBn4rJQ#9i=lr7@J8V6!25vD0@niH1a<=71KRr6qx>6zdsnkR4gpR8RsrV&
zYk{W%JJOu)U%=-V1p9kf`ex`k7Z`({rNGVAFZr*UkA4L^!t4o?zb3@~-q5=(a5rEf
z@L=Fkz{$Xwz*69GK)XFYM!w$zd)II|<^Z$67T|K=<-lu!N7e?{|2*(%U<P;w@FL*d
zz)s)`OM><Fu48^{;7-6@fO`S=1s)1K95@<yG|;XO(c8~TH_S>m0pF(prvb}>Cjo8!
zqIYxcmVCwT&CRztB>&YJuD><Fmw>MVZF~1!$nwL0Cj%2eTYd@nbAY!2?+lUu5BTqa
zgL9nUfk0dTSnx66LSR#fd^`A+z~_K3gvejn#QEP1d=&T;(6;A2@IL|jp33QW1lsaL
z!A}B~0#68$Uj+VQ;8noCgvft|e)A1*yVKd8_s(Ga4mfEkf1e7R4om`XY3Ap*1KWV@
zz>k5S0lx%p0PcAvr?<<Wggp(wD}mPmZTXMDuLtf8`ws@%^3%bW0*?pAf%AY1fX%?)
z18se`fqxe`8RcsRhRNS&$<KuRiNI4r()UIEOZyZ+&dyib$If5U?*cuNes}0O0C;dn
z{)b!nOF6^xI~(%n1OEWL6KMO3ZSQ>e$tTO$Uk0AVxayo>|2g${%r6CI&*twfKwJO6
zq33<zr@+X$oNgSj4443}0A2&!@jRB_6?g#f5a7g1`T6GNzaDl-{?m|;o&IykZLa)0
z=(z}Z$wh3}6+m16^H%we{yobt0owAzF5u_WfpdXb;MKr3;9bCnfi2Mc5^yc>Q{XwU
zQ|zv~g!2olU(vT5=~n=+0^0UJ0RCxUcly7DJ-=*WJL14az)s-oi~0Ftpy;*p7y0*)
zZXNJHKwG{q^bOcd{Y~)03xToA*sjxow*DI|xfPHb)5_^=`IjO8D)4<^@8vAtf%LD3
z<o87n<<~>cL07Q;Z~nyi!8O75ee?(Br(Mpt1b7ATffdYeu0D}}1nJs=D}i?XyaWDo
zV0ZS8M7h>q$#%W-N5=Q9a)z}>U!;@tJ6^@<vq0OvJFe#EcLARUt_3bbzAZpI{pudd
z?+QJ80EYk%2aW_D0hD(ADx`fI;g^emzXM+SXD(Me@cA40`-{MT0qyd<dL7Gk0t;^7
z?-8Ia|0VQ%4g3i>`X)|y3~)AZF7R^THNXKkv;6MBy?}#(H{Hk2H#h%npjYyrh3|Iy
zjj(rf<;DJspl{bZ*{;2Tw*Kd>@=dyf<<A1z^21TyF~B2%6M?q;2JqY6!g_}Sj{wdA
zE(F#CPY1TZj+cS20K3FKl&b*PbPwlq%)N}ZJ)&<p(ystsCHkxc4}xC>>`wp4TRGox
zx4{nJalqNYn{VgmEp4#VDu>-}=i&LQcd`7tKq+UK|24v%-vX}!J_QuN=uZC=A@v!i
z{{iT?s?!nq(=B_h50Mx9_C@&*1Re^s^-ltSEKuqv%>D%0V?J;x@Jisdz#D*eevg4K
z{42N15a3b3V&E}AY44ZO?ymqp1&SX_`-|OS?JN40BmD~CRY2R`2f?obcBlV#)Z_iY
zH-O&&_kbUc2A18;^&tJk&R?D%54n?oaiA@~1pM!SZNM=7s}P5u0j>dl1pFMh|NU%d
zF>o?41}p<6fETS~dD}iazc;P?B9C&qQ9wKW+dY(@{{ZXT_z+_|^hO@z@3AKtt3&L4
z5$R8QkmW7_-V5xhK9OGuJ5~dq4Jq$B@ZSKtvv;?L*-oj)aVTdc@Uf6`hqc$t&D8g|
zcFuRe<BUfE?e_cqBb#Sm1@zhJ`#izvw*l@BJP^1S+H)|_PQPal<-dmf_rTuJyESkJ
z;Lbqtqoxplx(@t}z&n9I0Slhu@~!>{<Fml$fv*7%d5)jk<$Dx*o&XMpzC(bv{68W0
zqLu&7A@UQ!PY0e4yfj4qMDTNfi-1c)<U7HC1Uz^Zm-BF761WIxm#4e*w!Gx?CGr~s
zy^})p7eCGRlmKr5-Ve0(XTUE8irq^>^gjx@)xZ|WEf0}j4gNXcA*<QG;XvD-FCh0L
z@Epip5F-C8@MD3k!0STfXFS9CmjgS1F9B_P7J)w-IOthUe*n;yci{hhfTMv&0Bw1Z
z+ZNAbz~e*Y>%gB5tb-k=h1jzl`r$xeAuz0ePQdd?z*)fOpXYL~0loz61pe?(eqPYQ
zcqQ;=;La~FZ<l`>_!EH5z~oE8^76d@8h%~~43l35edh!3ev#8X1hnnh5Bi3Pe#jMv
z$bSqw4}G2W&IZ2u?_m4)U(0+Hc=Rj$Jr0}?%m8iswtkQ0cLeSYJP<ez82NzZ#sZgr
z$lrgizRA#Qr+)%^R|1~}p7<%}mj&+E6`cRHb<7`T$zO`^ZNP6oW4RHZGbVuF0K@d3
z@-fTT0<QtK0WbW7pZ^Wm4lG2zqJO6l`@7TsF7$m2wCnF?@OK061&Td`{=@d|3AEEM
zfL(RKdf<rva=J0VBY+cueZS=Ag}|9#@poJQDd3a96mTi<JmC3X2dBRs{Jp@3fiD1G
z1Fi#h0e=Mc`-b)I0Ne+7^0&eH-3<P|@A>&^;CDX+%O3|m39JS-0T%(U0NV94YCY?H
z7T5{g_9y0d0QLtK0*?XC0L}v1`Y!>08Sq-*#T!`vO~8G(*wRkwem`U*%e4ZF9hR#A
zw)EofD}V!_CrsZvmYzbSive4KHvu;SMXzmS8g_I7MUU9khV-uh->~%B`WC?cI^c!C
zBe&#o#(+-&{|<Z>xE8o-tKj<l0etU1{JbCV^sP~kz|Z>yr(f8g`Fh|=;O+yM-y1j>
zI0Ts8iJzYV{0wN9zYF@l0!EO21aKnoL|{+T+w$$uvl7?|d=L0F&<U}(yYzN`gLme7
z*bmqMd<OVD@CD%Oz`}u?ZYc0iKwEz+<gWvE0^bLI1Kf5O*4NYYwtN<P8h|ap<-qHK
zZNQ$Ux8+|zxnBhCzbls`2W$c^1}+134Pv<sz$v>0mwz(krvY1l%Yioo9|GF;b(h|j
zFNQr6fEB<buo2h{>}h&iem=^*8u%*kgOK{j^iaM6@0$^Q-M!zc5bwPy3VHvHGqiqj
zZ9`%%VMC6wTK-m-X-Ey7SJyanZezM8`HOVYalch3>Z_fh$;EY)lCdGjPnuG>`gEqw
z_!6gQxl~O;CBWaZH4V<tbX}VMY)CDlKda~qJ<B8$4GCvxsybekOVp;~)k#V#-$-d?
zWjwX0GL>zJlc6;!))7zSa*4&PL;gO$k`gLqDreH`TA8R#SCWzp<wt)Eb&cQ*=hoLd
zLn|}2wW&IKm`u%WoJX1xb@Nih^Vf7;mD0kG=FZKfn&ewex{fU2Csc9r*P0Av#w;}1
zp7Jxk??Gv{rvK&DF01+7MvmbAmQK414yEVcY}}ZU^j%0Vc+UV<z*7Rp>1*j5P2abp
z|0VrRJF|dbWF&uA`6?MnKZt3km{94K^sgSn4+Oug;P3J$%#*LqWcu4i&HiU^<wt^N
zp}=Cl<Rf@2iEq%-$J#i(U^{<xry5F?*bmxC(qE)(iYvI2pLlo_f0z6vy_CO>9_#4~
zKWBNt;$8jp+tOd{^oxj7b<&5$oQghNLxQ`57f^ZWpL+VDr*B4j!Im9$X(}JS3Z7x5
zFBr)A3$E=gCFg{aPPCq7rC*2OC%BCW0&V-xLwadXXLm>d7Y#EN*!f>#rC&6i(+e)&
z!Ax)G-)f~VIGH5`4?EdRFYPDnEmry?9^mwXgSQe%My_3-dxZX~Oq5z~duo68VcS!z
z=Nw<u{hOWtYGPFrOZw4IbNbOwb9!6e{{9lBRW?cbVx%wL%=E8W=_`=F0_ocal7RLp
z)c&si`T_l~d|&b}e3R`je3R`jfMf7CzyF9IsP3lQA9<V8N8aWX?etv#Ncjc7qKA5V
zr<2n=ot*wv6`Zw?q!av!p6Tg3KjieCA9DJR?X|@8*Fy8F)0+}2T~hw`uQ`3&*POo1
zlDE_MrSvNQh$ScKhgiIlRUg6rNYAYQ=j+J7wHyB}3`u|5k8J-ps~JVJ<SX9~3rW9l
zB}a}wr~t2h?DW43Nx${(D%v{EvTwD-^jACmM5MRt@4WtMRCJubwz%N^A?XEYA-yMn
zzl{1br$4JqB0E2jbRvIpNcxAr?(d4k9uUdk^h!#6N0;30b{_cWTNzqk-TLriucqLE
z2RQw!M>DCsGEM)B-^=)?S}7?1mhU+Ic2%5S{<QNKJ_vi0butgqpZBKOhRknFzPBUu
z7L(u7k@<+pZ{^55!{qxoGJi1nt-bky$@g_+yf^u6yz$)Rw{>KEHu>!w8Fx*-pCjX`
z$y0EV@lt!xn?@8F|1_G~X-7xKHIpxJWV|x@{*H`CCO^QDamM6#a%B84`JEjZ7fgN^
zN8-K74|F7sn><aXBtB~odOL$0iMtw2<=xGZcxv+YtcsuZcGPS<|I^#qGoK`;x3gCW
zzjp|~PY7Qa!Vk{lRsY%2`C_B}pdS%i&`e(ZhkKvn97udGs)yAWSET<Q5u#@n^h7WZ
zwDojYdKyFYEQ6j{h@Mw1J*^>n)ci`7D;uJx)6(-~h#oahQF>ZJ^sKY=YzWcQj~YPf
zX$#TQW$8JD<b&F46!dh2==sjlQy!ux0X<zIdYo@m<o4tC=^=V9gq{Mt#Mo~4zLuWb
zLi9WUJ&_PS1(u!{L-dH>7F+TklDzWgLhyxd>1Yj0gP3ZU81kJ7ekk!l^?x+vTSM%e
zY~>pd(NhaO^3rj;-eQ)X^Fs9e33}vZ(zc%2mY&B$^gIVW-b<2E4^p2MmLBzP<)Hde
z@5WX2DVt5UJ=K<;{xk^)(z7S=y`21U;Sl1L{$gtr>nQM9i=PF)&En^|`u#Xh?%{r#
zTt0uCx*B@AEIsOuYcD5%ybCLrx@*vj#?#SQ?;%@ft;^?^S3M7^pLbpPLC!j?8zerf
zGoD_~&Q9@K?!OP9V29Imx|fqb4wkt5Ag2v+L+qIa-X0(4xctse^Yi<=qN4vo@P*GY
zA49&6gLhsvd9_UF<>Zg!8;Bo3?Hk#T9Em=MxPLm%ezcxYe8FH=d_4GH62C9;UE~$&
z!?S<Ksf2v<K`g%n^1lY(x`g>Itn>d4zT;5lS)1d04nFoK^P+!HAGW9SK<35I4<~*w
z)qm^jTwa;)PKA6i?7Tqi0q;y=J*ya;e}Zp5jfow>Z$;}$w;uLo3yYxVK=2Og?O^az
z!Dm&VqmR^EHTVu3Qtd<E)#m~51y8Vi6Y^andOl;~G!}Qh1>X*Pb^(7RIi#}Fxr*g+
z7U*Qacj6ym`5olX1>d@e<sS!s8~Bzv%)bEsP2mgK&YAq!Sr5JtKUTX9<tnCzQ|;2+
z!R>-(a84#(+1Yv=>zM#Omw+$CWsoV<uhgfLc;#=AEm)qux}U9R*{Jl$%M-7F{8;dj
z^{iOUx74Q|d~5^rt;qKZ@NMuz`s#lE2EG&jsKq$&>%hxDE^s&bjrx2JK8yY$<vM_x
zMA?~r&$OoueDOQXOSvu+{y64QEaySu2Rqw4ZTnD%rO$5epN_K@@?CSde(q&(HiB;(
z#(KU(22-d)TtE4MdD-@<17A_k{Ke3JzQ{M2^4Ej!+Q|GA$UhCfa{wE%oF6+IM9+83
z-wu8cS{f;P3ZWm{+Rg~@*$Nh{M@5_pzH=b!IT3toh@OXtcjLnv_CtxEuYhmEK3D<l
z{}g(%JF@<l!S6tEOW7H_nC+4FIs$y#&TPn&6u;Exk0QU0<*E4Y=VjuRJza=blJ94(
zJcKmZhBghAe8o#_zsMg#yjySRM~5NJH1LsGZ2v&i&*|XhABkN9`Fp@;lPq`|<o^S{
z1^-CXa8~CWNCv2UI}c=H9`q~&-+m<X+oOHgfiFIV`Rl<Cqzw^OuGl}!aupF@2%S2N
z4dIU^Ud94lzS`v}ZXC=Rs{M2ISq8rOKWxu$n0BrKU-%mHXg239@Xk@JCkp#_qfFg;
zfIr|4loJErT)-8%9QC=BcvaqJw4?apHIR>8%7P!kQQj6k8P0Gg*!h#IXM3jwaX?nQ
zdy#<apR-whA?!Q~eCK6M^y1<<jUs;#6Vi^)gKtHA_$TuH-sSN{gYBuqx_+o8*z`g5
zIY)wTL;d4;%9#y5g3BM5qke87Ue!<GYW9Z+@^}*R1#htZ((nEY-kFW|f_woDh{~Qe
z{NoL;ZOIQJ;Ip5wd;{bsgYT?i{lmdmfp2eNz6AXF;0qS9o;$!lC_K(#9tQsb__iZi
z55j{JrNHa@&tTRg{B)N`Vh!d&z5s{NGw^IF@oIeRzyK>d8m*A;`i}FJ`248L=kHUk
zgnY+j)6Q?f7h^nC`w{9>NK;!kE}^{+X4+BbXsTTHyspZX-`<Jr%?(1;;(nGwz5<u0
zo<MuO06vECNb2Vk@NM^Ty^VtZ>`xOvm2Vc~-7x6SfR8n?d>Z_9;M?xy@=AREFZd3O
zFOu&78gP~V80K9ELC-MottYV_>Ynar9`UMOVwkryg8wb#TZXgYucEwngD*as_0U)M
z^Bnm0Q<=X3{FmShYni9+>3+7SEi=_F?HJdnIPT{_;+6d^_(v~Bf*<el_@cpakZ;Dk
zMdEog_>R{(-~Ey1M(_m(FrNhfH28M(U#XvegU_DE?eaC`_ol5q*RNh<&7;Bpig;yz
z*Mls7EE=X7yuI#ND!f&nSA&n?zzXi;JW9N3uTE>8`3~f}8o69i*wdQ=uWJwD$ywm{
z1mA{#^bghHoCLo4Yu58i$e$_ve$1}~{|NYOe=hG5@UMydo-BX%mi*vr@Ljmvmg2`w
z32l8UJF|<JkbYMMzO|0|m!bb`@C7e%zGDB~;2m6|ly-a;d>8CI9D2G$|M{%HAKGg;
z1t>`iQ5R;oJo)EXmY)mxv%wc)oMo?|^B&?|KlwW|5`Uf(`7c<{)?5tdJMfX`xW(np
zbTMu9s`@Fm#+PdFo!helX_wo<N9M8qouK~>@LiTad=1_~9Fz6MK82ibGv-m{sEl8O
z?|PT@(=^Qe+yp-I2hMOK>gO%+?HG^RIGNLLFzYWonB{4i=6;5OkL<>Rlh6TXfG@^8
zd>!O7;Im^O2!C4vzWFWIBjfT)@D*66OaFQsd}klFNBp+ezHDa~`r|_6J6QNXvVMur
z#}Z!%866fvzNKmlm!8Pr+z!5NXC^>9PlIp8W#)fyHqLtR74WwzWKy^v+u8oPY0pgX
zG2Ey41LT{)cVN6GH*i1Kf|q|Rz8?Im;0vt!+(5iq_r-8uf~Gz0XCS$~@>>V{EmCh0
z;#C}L?#(5Yb}56r9Z#~NXMdJo$l$1DyVBozV{bRV)wDiQpWO~%J~E93X?pB_N?aaa
zG?)$fc8rf$&N&x=FMgW!NWI+x-X8Z?f^T`;tp6X0S8<>i`wTK(?0O*E+4T+U{}DDt
zz!xL#QF8Zl4Dm`&^C~JheOlZ<X<rNS9Vc+UPr^@DfVaoryIdZLHFz5Gkz3iGGpOm*
z=S$*Md$spsUgqHiG{GozCCon#C0?~lJLZ!z?oS|I+24tHNXstwlZ1Q(aX_7OsLv(f
zTX8@u^>7FH!hN{iJJ7(}QGj**VKCdd6{~R$0v|z#8;A!>!M9@lG6eEh3y*me4X^I!
zHSmRq!!(?^pYOpJpg)Q~A9M)(d=?i->ftEjm7QJCb3F9K!OP3=OAuHe1>d=b%O&I8
z$D&7VjME3^Iuj3N{S}B;3!uLed^^@tCxO2aygjdboOsn<h1U4>7V-NctqwmxkG*ak
zNEPe03;NOTkSGGatCH&{4!#<EGYV7){u1ylPjWr{g~9oo@Q5ceUc3dq1M^92|2aF-
zMuf7bVlC$@aeD#ySUD?x7Jk?w{5woM0ekKO-)W7@uYhl_;C8u%!5Q)khfW3ZKc^6{
z>?t0>`JUF7Wu61y0{`!aq#MC^EMUc$UOUGOWj*$OLJE94=5@<Zu5-XQf62sO!QTPC
z3;T^*BHx$6+xx(ugYUSE6^NZfX`<!EE9`4Yf1gUcDsL+;k>8AVydHdZ6%#TCdrb6T
zy&&uEmm%M2tq1#(fZM)XaQ)l?`$vFpeTwaT5C)wCzH2GxdoAjr4ZOWi{+#HEv*0t(
z{~__JyscOlU|H*I8Abc{=bF47<^47I3Tys<1NdfibeZ=)E_!~;3f@B;8&JghJJ8>y
z-VPUjJC>LI-YobC){hcbF9Y8Sdn8`n4Zi(eE^ige`!Dc?Z!<pt{EwpNN3Q=9!S6hb
z?QGkD?YS5H81M!7M`dX_?|x>BJoY!ludWAQ_ygxV1M*LU@4|eh59;SL@EzFq*a!R!
z+DK7;((yj)k#*>);EV5LK8pT(r|{Un+8+6?Azrn61@;La;AGBckdN)pgp_yt5y<x(
zR@}5LD>@l`7sjuhpnnngPU{@%O7MltxIVGGasCCq<!_v?oU?303u;wfdw*sy_?AZ2
zGZXn{!57!EoqyPoA3Wyr5Yk``<lAzb?|3}>348=UF2uH=bNDE>rx<bR2*@7`KKpC7
z=Ns_XfUl@x{m+4a8+^-y%*#IKZZy$S<t?<<DHFgqTm5b>_z2`5g#NRMSMBRO!}G2f
zXXD%mdFS6uOojYv@CB{R|FVD|3>w4p0r^n|s?#}~cx7k%zAS$a^c)MmMb@X_{|dfy
z3Af7;5};|puJkxq9}R^3?%+E<W&P5Rrh#w%kokW={&euM<GB7yz~2JCU<%8hf^n+X
zuQ=akY)Fg#q2MbpK)nDxQ^9wYvYvOqSA%cGxeU3D`#BeU$KTnWYZ;s;!Dq3KTLAfg
ziGIWrT+49wBnMOVBmd}J8RSoNd3@1eG4X0$6M@4H!n4bu$Bx^NgYT+l1F@WQc07{v
zt-yZuzR+_7`0NsvAHvn)<iK}b&V<zGo#10QcZ1tGFN(anz(Ak37@VDs;(Q~ouzrL&
zrxbk0JIu@Z;0fSkU$FuiFU}`k)kD@g*SiVwUDo*fBltq>!~6>68gw+<(*i@bK|M?c
z-?=;IEBhg5g728hc8Z&~f{)B$`M*H_JK$reXVKrknDfmxvS1VP9SOb#4TUi5REYdb
ztUrg2dI|XUY0TH4U;hBU;xn_o_8-gj7PI`Pn0V!ft&g)jek<-Yf^UXBTRZ&uGx!MN
zr1--N!egA0@;c*Kf1y<mzW`s+#`%uJIG6+9`XQHV8T$2g;IqfF{PVEqQSg!Tw{V$H
zp{L(?&R71?kF}6L7JR`1)+6^BP66MJeOf8k?IMr;D4I^VpJ%~$;pGU|^=5wC37oHk
zd3Yblj|5+UbG;$p%fT07{sZ%!8t_@HyZ3|q??eygIb!D>#H)VMQn;lnDe>VS#23)%
zw|#%+73hheT_iql1Rr~h>r>9h52L`V*4@Run6H36Q;6p{&-^)Dt<JgNJFx%rAmr}?
zA4{`fJLDe{Ju8?HKm36B{z$9C9uwJ~R@fu{R!qFIr?ZLmypBXw;G5SoPs4}%xya@5
zMT1)*-)_Z+SHL?6kT-C#ojoUUzFk(on*zRI0Sihzxd432N_Lzp;OC!$FGhS2KiO`w
z(x3l1z~$-O1OZt5VJ7%?Yo4|cd<Diw(Rms8<`mb@Tgd94;5*etKKe{XdAFLv<!!<K
z+d{}62)+>ez%qY0UgX!Yf|DU%558hQ=C20-7x10fw-G!42EGIPBws;(i(}ZHj_=w2
zvEcUuFF!_=xH^`2)nD58<o>b?<WGcr3+ioa@TY^%p2qs6o*x!@E53DsZ@rkyCHuFd
zrm~$CZ?Yb#w`0M#EHU+83f|to`YZV2ms!7zryId{^<}@hjMX_Krg6Rz_|;Y@R~h(<
z9k?FQ{Lb0nJFwm#3;FB8cVa(eAMnqB&z7(~cY@!4I``8o_CxxCKZbbKPqW{$o!2us
zjo>SWvEgFpL*P5CdEGkj&X=4oh7D(LT8JtAkyBXl7sz)G_*U4z2ly*R{wmfZ>*9~V
z7w^t|NgqzohZZJEfAbm4Cs5C$!N;)QyAAl`z}x#^_29d{Wj))%p8LTUe8T*};MWTO
zA69%f^gA(SPyT0T;@y3KH}-cQHbdkvSAILkIhYk34*nR(7vP+_0DL9nyR3M)9K3x`
z;5P7W)_&9r;5&}ua<!s9N6re4^EJe)dTvF3l=^7_-&w}?KMFf<1>cPLAnVq@gSYq3
zKP28=PoaLY(BqVHzJ(vLe(~EV_?AV?Zv*)g!N<O39?LD~bmG<co5j6iQU4p_)p**5
z@e9@9Tn{~M_p#r;y*;Psba_eb<5!Tcn9lWCg?_XbEhtpIb)vt>cvk|x9qUb*|C}Lw
z6YH7D#c*yWUd72)#K{53`Z37I(7wlkf5VlheJAUl!*`H(togv+<!onjk~7=@J#&dy
z_2Zy^_65HT@?CfdhxF4o!B=1$><#;SAIti)aV8Ffp1p*(=2xS@cj2DQt{fJf+2AV<
z;re-*OjDmffp0~>kotK7ynX-S-{9@|*_SF%wO7lw+<@3-a>js<VV*30cnSCj)|L0O
zInEm5-Tsb!a$5eopU)v**pKzAd(!GN@OaL*3vpmM>TNRk;#QWw5(DAc;M=h-z;xWX
z6@15z<~Z`U%X41(pI<`W!TMd|{KVO8kA08&6!2}oU;}SJf@{GyW4}e}=ij0S>-sgw
z{3qi3>xSU({ZHV0?eXz&@b<cXqRZzinhW{nXW38og`KU$D}V0Vg83y#`Y7bvF&^y#
ze#;Zto>pt$axd@|STCd@e+2QWpGGEg{fnMsA#blQYK6!C#u?D_d+;p_xt?Xeq8)rI
z&X1Cie+|5SUi<~|s=N{OUx|~upQOr_{~7A?<o{N@DkEO`b0_xOS0HH`@`aZD=ZT&t
zxt{;R;M@zowVcbf8hX}(x91_fXoBjF@7T8#zZy?`0sSt|UQZnZ`7HK(OQF93yaPk_
z>&p-B24A={_pkFH{|R_|-oE`QY)>clb7UNx1imB7<+=xYP6J<z`S1ncZ*+Nl(O@;?
zn-{Ylgi&V$czeIMa1Ps3i1|NtXZJG!d?)sA>%gB1z6<kS@qx>TSM8g{_&W&le}#NI
z)@#eyEayeYcVc`Q4f)R@ANi66Y1-y~wvV%&9WizUa!dC!!sYQrgUOI@OR)T#h?AFq
zFUEXq2-4gRK8E!;mVeI2B9Hw>DesmQY)=I9ir3+vV_jY|1b;n2<gaDHm8gfy!FOUG
z^)$#o2)@HQ|9u~PGvZ7k<abPP+$h|G3o3RVK)mw*Ec`+IKj!jCtifF3Revu=e3SnE
zTksv=MbE9!-)il5y$n8!^OSpgaV8sySAHVDw-rCxWiIT*zOm?^4Bo*+O!#Hs3s4V5
zD8qH&Tb|+!MgCov*9wZiehv99tY`NI-@lUWvF}R^0pE4}7Ote|F9qL;ej)rK@G;Db
z4uk%Cz!zd%SP1?F(f<wW{}KGhqGu}eP2fjS09Wm0@BhyrUbVZee*y9RDegJz_IEY3
zVP1MJ<Xg{#ezezv;3L?Fxe58c4!-ad7Mu(Deksn^$+8~2+tHaoe9(A38}h~Icj9kn
zy8JeIzfV7#c;!Fs_#Ht6_WZ@=w|5G#FS;lAC!s&PmJJ-c6&{SsG}a~ShgN2@PJGg|
znd8Pzi^uC5=f<bR^(QCJ7_~4vb8J~j*|bsh4ate=npBBXmZ@oMpqEaKtxMD_u1^Ol
z36v?D70YGlHBz-f*_scJZAjMyD)t{t8yg!terjndTa&0vsTWBFX$=-CYn(ejRoPIU
z2})u<oKlxeEviY^2WmGTPER!C(v_(|vyErtGj;Udszkc3AyC$SJTt3mJ5W@AG(A;c
zpO}}LoTy9E8?xqc(X<9Eip;CY%uUqja;aR2-TeAvQ8RN^jEqg?s;H)esx0rhH1xP2
zY4hQ@X=BT}X~ZCri8bkLHkAv~uRoH!g5->6mAQuchQ_L@p_NWNe$4pN__Qfy<?*<a
zOyyGZsQFX5ctdTxvL;hUuP6(k;_+lA&J`R_He_=3@kHYyhhA}(tw}Ycl0!$188wVF
z2MER0yV&?WYH{-7++v5{#g=TWtzAq>v~O|s3O7SE9-mk`c6v#?WX1$CmM!PMPdJ`l
z#a1;m9-p^pQJiY3K11C#-LN>`6mjAwmXG&(cC@H$acx;Pk*iOUmzIo<&nSx%Cqyp2
z$oG+%6DO9Hl*h}*j+<7ZvMZlH-pe{!pNUs1=Sx}oqojXAs*cr1T=h};&e{!g4Dn$_
zuKWm6QJYfT%k4<YA|s7i1kNKM&4d$XjGaDZJSFx^Kf-eO5xG=-s-dJw7aGYiaeUOJ
z%bOzcx}xE6O^0RP1QzrJBz;jyTB)5XK{cw^!-$4-ZK}Q`HkImsj2Br-%7(?$$=K8+
z{gF(oriqTrWNK20y7I+Y3dVX-M%qz#d{Ig1)UpV*)QPUqsy4m6jV2yml}n`p3q2vi
zg`VQ4jA{|ev|xnpHl>+Uqr)q6sYF9+LOL;T3cb+J%wdcd^G8=_sQV>y4J9#E#Jr}X
z@~a0U(fVb><KBOz=~yznQb*29HH^>Hl%(xyrP?&=&UOR438S_^qRgw?;F^KAcn?QQ
zyonC4&(t)frchr_Cu(NJsL7LMMRC_ccnW(#)Tn4Fs5Iah(Kn8o*>zXnuk3aho}>m~
z=ab}l1tNQ6qvKO%#wSjhR-#&A<Z<cx^xU)x8f6WMhQ@m3i}asa+lCuOBQ&nn)t9@T
z?7OXA)4thK#lG2kcNm$jS6*X!OmyU_xpV_nT|<Uk!JUryHpix>)6~us!2Rw$3M1t3
z%1m}~euH^!QdE>kCVjW2KwRILOHG?n;&!<a-kbmxL0Oa^=e2Cq2w8geeqp1-D5}JA
zG>lG5HKl6Y3XW*s^;Di!(GistQRk(`RyL%YQe_mDg3SpE3;9t>w-UYDD}t7#>zv~o
zb2%D}O2|8@N|k$h|HYE*18xSxDc1V!XS>~)WMj4_T}c7JYDQa-e9vt=qfeqQxHZ?9
z9a~d#tQzqxSJ!HI1oia^jfonGf+f_&lk-xwwXVmC-;34=wMS#t4eTMcsv22^db5nW
zk6Ps`udY)O)~^q}#z(83RF_KR@*=#dU<zXH&>&V8MY76Q#*^3P((|etW@H*@YU!4}
zyv%DgYv|T}NUs+)3u$afjjyJ0qkh3+s|KTa!$wh*YVx>5E}f{WOpVVp;csqi_8f(3
zchj+86e*dJ$<>-+#~;~-tI15*LpylVkf2RSXk?jym?srx#LCfb+mgq)-7Kj0dR6F^
zXGA3ps_-sR>Km&ZRIlFXa4Ox@spX}q`gD?fj@(yulW08fzfyD{U4ulX=qQdgD)^{*
zW?dy3D8LAwKj@Lih`#Qu$RDR=wA6Dos%eDlW9(D`j;>AHEJx3;y`mzn^2SBv<%!(l
zS+Tk90+XG66Qf#a=!!;YhNX2SZN?Zr%9Q)1_d5VpOT5Z0YE`_ldO>mVq-j&ejgJ=%
z9X510)lGT1SDZXAb7wc65x&YX)cP~$&OeSC-k2!*5>fU+Z{ToeYaB`S_V5!F4YzBM
z0<pW!#{57}=6P*2>ZY0%Q}0!Vpckq6FM2vZJ8i}#;ayEIj7f-|GH<#NEo!LFWllXl
zk*lL+qTA7;ISQ${I;xe%6xmYA(}mjLxrCv7lso9qGIB2Ye|2plw?K^t!CflPV@lPG
zS-Mk6X_|W5s5t+pXX2N=D3YW#S+17C@nl*wQ<!sGN42Ex40@zjJ~Xd3Qx|t<scNZp
zOjEoRE5q?MiTZk47RFDkOVq->X3vhZdwI@<PN2=ny+@<4WDR=eo6P}J)vJug++xYH
zk>=#lD781oj*he)EWlq>QqSMbRiK(mQ{e4(D((4ZQ4tN-RWt%j$fc`jD5Dt#&0{ik
z9B{PhlO5aLUMJztQx_eXOVwtYXh4$P6CSsX^$>?&vjSwZhhn(58q&tlP39ggqV-F1
zVlGq5lTB}exrs{pS?i54Zf|neNUnqTv@7}c=x&QOr7;sv*2J1Zoz(ZX@W`UqE}S}O
zG2}Iz9%_8wB`3}cIBxVYhA8C<+ITm{i>K~`{6Wju2nZXs{6Yo|`kvh(YX+^enz7%r
zEl8`e)E`5Ojq;4UyfJ3HzF|c?J;fT)oVn%o3<#IcqWwAAdP>(Pyl$p-<{nM+{H3`+
z&y##5)Oyj10)yJS$W$d0i~Z5HXy^#tQ>5Bt)hF8W7W;Vu#vF&|eeOSBpe^%!b9HAk
zmM!-3Id80|;!y8SQv`9h2DGU}o{2_)%AY?62O86Nz1Z)$+a@hH#^vB{;%W3}t@b3@
z_b-~Bs67Z8!5V95dS9PPdixOQnAT2J&%DRmn()2H3sAlhMMdfQ@iE%buT4!$*V1Bu
z{;7#nGL=;0lDiyomtWB$V~uHegE13ytHm$8xpQW?A~pra(12K;nV6`zw)^ttI^-Hg
zSC0<!wjp>}Pz$)Jm3cE0JyUb7W;?m(1w~PFBcif8Rk=XzL+HAEcegF`i%;?U)I@eh
zW9?kp`z6md)619$57C_6*KK&YXP_7At1{VCU0!$hn%USCi57V~0ko>%%Jd~SD`;b+
z8Ldh+QEgV5MKa>6y}O~goZ2KPrGFenZKzDn)MgrJg*`1*H?N`EpAAt}>GMFtJfl*4
zy#bxq4mdLZvsRLReS7g#3v9+<sEsSM)j%8dUJNx%6yNeH*msK2RC;%7+4Lug%@nxi
z&o>;!>&TdVDi;sho#pLVyU6AQQmvu20P9(giiwU;vpdza)F;!6C=3K|cIoyR)uRHq
z{t$Xc<}$Q<N&ccg^W3?ca{atr=YZ{2Kl+DED|}xHp63TzZ#%kCA67=5I~ZN1ClNGW
zxhN5*s$7(A$m{pwioQGRUBa05@$yA{MKddW!$FHrdE>U*BYiDKR|twbVyWi>feRyY
zbodX&1vjw!>yCPrr8_na(+^biiFd$xiqSSj5#{Vt>S)47fv7ygJG`6W@kNn3T2awS
zUcI&uQt_R(<05hT&v0|EV;7-gd-deUgy=986v{HWhOz43bt;L)3>A5UA$g2i_?P5z
zncUA>_VcJdY^1)uV=uynS2fb^oiXV89VAK{4XXOI&0Av;sCN}D)foRQ%G*pVNgEUJ
zB2^cYf{w`zQ{WjR>Z@sCYX%m-UajsI;C3|9yG5W~DSwd9_pAK62|KIP1_&>@8g3UD
z3k?YlKFR}JL&g}bWX!z+)|OMsr^o#Rb2=PW$K~bjzZ|CTD`;&JnVYCwz*A8*{^re~
zXbZ_asFTHqJ+<|EbI~YwZZ<1>d?vS`K1(+ijME}M9`d?@IYQ}YPVA*c<|wP}L5Hp3
zJVV@mrHr8Mu`)WkG0WkL1#J|%-sTyp)z`@S>O@UVX-ZalYM)IDRnlz6>9o;ma-8dL
zfOwV|QyaLr<_?>haqt;&u+OPsU+<Cm0fl!GaSq?}kb+}~ybd;6`d3I;&3CpU<D^Kd
z_TV<EqEnU1(2+2Ik>G9~<>ji^jh|)EWYCW-nBoO=Q9D=!^ipGTs~=Gr4&b>(-V(#x
z=fD$Ve?}&ZhOYdoNNz@5c=@=cr5jT<0qYHEb-(6#FF9~IE5gRwgnNVvU2?6*4qTGV
zpW8_N8%GEES5*8CF-mPC(*Aam*1B}gLtB{CaVtyHJQt!rbuTa`XyJjb$lJv(Ye;1$
zq-A@RPZ>PTc@F2sRPX+Tp0Na1Z3DwE!iZdBov}k7wBn^)&Fy%#8$U8`{io;YPmH~l
zrM~{LuU(MOsR9MuO>ocNbj?OJ`V+E%z^>2S!s0qk)ADBAMs)Ra@#@X%M@2<^&^IGf
z_v=(HLnrJ_bg3ej;((`L>PeTXZEUqaTB^#r$@5F!?`7HG?|R7;Tm08EB|6%3oe6Yq
zROU`&EVprwcliW|{&Wp7CRt^4^;~Z#{jQ4bv-u%k9iX{0$nlwEDm3!z8Tg%#yi<*j
z`kKIK1PVRwN`*EFX`cM+RA_|uX2<@)fY)q(z2+4r@90<6oO$vabl{s;2~-aM@`_PZ
z38Ly|58wIH&#81#W9}9-ANAV2h~}g2H2}JUt1iE$lE$tw_ll@-eIaBl+2lT3UbIox
z2Fzr%6I7}`L&%(y`6~g}L}O;7_glMTdIh%*GPp-sv6@T>Tj`T&G{8k8%BL13=BAsX
z(NXkwb>tUspeuNECzI~A6&H`6J$vl9De>q~y5{NMKZ^cOC!3ocO#@<@-FS9f^V-C#
zsG?|1CPDkaP2?SPfz?0s(oP)7A~P`Pg~gmPaP%;GsB&vH0oYW#haKMJZ?^}H8-Dkd
zXra!EMu+btcXPUF?gwhi0ArrxRi(t@kbMsoFsw7%;7x|0aD@Aq{`slwQ|L|MN6XM{
zn9$K-Q)f?}*W_)(V4~Uc;2@)u-(=C@I5;j%)MnM?0%P%Qggi_^y?XL49Z?m#8^qe=
z)ZBsJWW)6|NT|Bx8I`dd>#k9~vw62exS=yF(94#Ce`UujvtMyV-A)Pf!n4?|JC9Mr
zc3d?cMn|}J3jD3HkfoNcT_%}PbtAGoL!0qwwL@SwvhTlo?+*7qedX>|OrDS0W%2r}
zY?+HM8igEFMZYz`j<hO>XNAlkl+#T}*Ov3NL<Jh_%t{}pda~iFjU#qAM_BX9>J=r*
zx4LMQr%u$6rW@tvW`ZYJG$J#%fqsNCR_<)+dll}TpzeG*rdARZ$c#!cX5G;URSjJl
z;d_AYHh@=qd9Bk;2=<C<-^p;#-pB^MtLrsx^mOziyG^Cxz1oQmGtS{c{5r2Y(fAnT
z>$($5R`~;&?S*>BcKeyvRNnQvJOj+Ou)QW|H^8e`ac6tNtC^v0CD0v}R6;I>PpqLU
z+O84$g_S_7q}+ZL4|jvv6uOGV7Z%dN2R?b)l8)MWsn46&d#>egl(_da`4BX4ciS^&
zvxiA@cIpOh|I&aT8~jR(j`T(|x|UI%qSI!Y`p|z=E`RZ>c0cLPS~6LhEVZ`Fs0)>)
z)$fRK`F!)L+pCPgezu7_N<5bw7QXT8&&WO3@x!$~`3pC}SD|;>yn{v4ne{&HEAXq_
zY?=RgJ|YW#>+Bx^1gDkioRW?!-ScP_g52P5uA+3a|7W8-e~pevQiPYjNcXxoH$u77
zcRfb=oduIAw-BR@m7p<u-(*nNOKS{u#vL@j(X@V|hS69qO@~`$-pyCzbjJ6xpd&lg
zE%H1_U)Axfo42C8wi+IK%NVz#X_w2WEpJ!S-x~M2ftN%LN%WtQ(wI{O4G4Z!1Oy{B
z!!*WY-6;5^9k?Z+tBIC*PC!)=FgK+w9qj6OJ=gQMq9~$RtcK<cr7B=+p-fGFC3Lxa
zFIn2yiZ`Wm4Rkl&S{v)XU(mh97^eNErhvlZLNsh@?AP0-_qAl<n!lH&>e4H6UcB6t
zziWAqXw1?j?9{|W&OOwtFHufFKQp5#0nL-d;YQPelAo2)uQvO)YSx`p-b>g0!E3ds
zy5iKB<?G>L3wmXsIc@PvB@0<|Q;}1c?pW{r(#9*EXGT#G&g9f?WBzfR?h!_fgt&y>
zJ$%E+sJu=R;N*ezG$QBy=yQ`l;t9G>kYC)Wf2vuo#`U}j3?Bu03H|C;x07a^7&c>%
zZra6EtI!Yh40ju8gg1LS=&hE`nYlkAL?g6`nw&zv-KC#qn$r?}zt>&Lxj)gGmN`{^
zqLnw_%RiW5KT@q`==S_3DnUmbaJVxh*Ht;0{%6=ud0<y}$G73({m{R?qdR8aL7CTl
z0Rf}iUqKp!fsAG~HF6=uUrCxjJ~MiUwL|6>YlJavv)fx=CA;FLdl=e7e8Ze%86&*g
z*?iv#zklhq?<PC4-ch}Q)jc-UpZI;JJ7)>=5v9#*OV2cFlTDtq2St|pY^EVs<%N1{
z6TohapI3K&{RUn3@Xg=k{a&-*@m)f%3aEv(_W@W==1q0{dJme)$R3ZqOX06~)WV5s
z-t|IbPv5R4IcoJUEEs)<hKTrF+KP97mt!<#Ubi$oTEdfG5ZxQGajQa;?rrA5F@ELm
z)ts-Y+pTn&ix|r7q8nTGNzb`_Z#6q((Bj@Yx}dAn@%nUkrx*E+q6fdcT?+4A1!`kP
z-OJ+jwl!YZg>&b1-pX0EU*!Vz^SpRfB2809`Hi0Y3q7@}GDDR<aWvc2YmNN<m8iP@
zIz6$7ez)0_&Zs-U^kXEoLhu`|q*S%HTLI<>VVmy{5&>OeGmo5j$5?j(um9S=1~m5O
z$YRX+c~)<Bh`iZ`o`D}e{B9esP1VvgG4QvM?t0eT3(%`xTZT)2%CB|_0)ETrD!kBc
zES8E!RA%a$Xx2dsLi}XDJX1z50KrcU^`FkijFf&`%CIW?o9>kmt?u-C$qN>?mm77=
zw>SCDW_iXNw2euOEWU!mbly>LbcA}v5dB~oN8#ACl{)oTL-r1~A5hh8C%)S4dbHOm
zL-qwWS$J#&_<_drOKpOxO^$r~j2XLZmEgC*H8t)&iF<ah{K`Cy(|w|wonW_nni~{$
z_Y7FGd6ns%uJ`zYm{DoQY)M8bw_L{5(k|B~cHaC*;vEdxOM2hCu`*M=J%87O_7dG!
zq^R*L;9N>Chu5@r$e9(Rdzp<KBi-WwJ(V9v$XDc7m7e{c&iv)0H*3!uT8;V?50pmB
z>oGyuMgtGq)wYk^&d7_6=4B=Ox5N1Zp)ohIJEK=@GA?A#!jFL?Ws^%@qZU3odETIn
zPEm8?%BZ~Hkm^~tiJzu(>*x*br7GH-U(V#G&u_TsW?$g)o!$1$u;DPTeN|DsI+s}~
z>c==$4$o^=Y+f#rOslt{grv}R54<j5j2HG`;#aCVQ1MSs>Ci6OSc#V)jmqcshLQ2b
zx@PVb)|?yW$HQbI|7XD3X1r%&P((NL)GlKA!;wlB5U2HMm~WF;dr~+|9rF93uE(z3
zpAiPgZ_P*$c5Ts~Pe<eNrrK^N)Lyw{L{vMnVf!Us@6F%W^Q$)~{P`|S)h01nYf}HY
z*Em)bu}4wQ6g9(g8^N3u>#YzlAvRsmo$#wIQHS2H<j%7Bbva&$-_#<*bI0y}`@x~!
zsiHUa)ra5w^Q)q}<MO<Tp;0GZ!}>EIeHCeJPI7Fthds>_x+5&V#>G}QZv*)&-{3d8
zXm3W5X^8(C6yrGF=*wp77;8-mn$jx6BAAGK=N^0tY0Sp+Zm615^1KO3=)l>{e1WUb
z*mn-vhtXrcQU0*a3^x+tm&jI-_lm9DSC7Ab!<o_0t2yPVeCL~A&7$A-j>)u)Cui25
zDjPqJ>z&M8>Pj00jj_g16(k@5!n4=wV)kHb4S;SOh?-}d=Jey|zR1ukvxI;3Yb<wx
z?k#uSYnsN`>c5mu8;8iZwLL7u?Y&}d`j#1T{5fxuijI;Wo@~Zz<h<&a$w^eLmgwaW
z^b?=P8f(ho*)IJg<bI)3RbQE?tMYeItamX4M=L{_ic_1@(ZmN)Uovm^nlgVoAatJV
zl}?-HnrlRReClSM=+&(kv-zqJy~#M0%Z1mDw|3UXQBM~N%8@o+U+3Oo34F1Xm&6Yh
z(am`Eqdh{HzHEoi{H|T3-6~h};eek?c&~Ak*7K^~UT=z5s(w(OnI0Q&4@rL6j9w<`
z#2+Lhqw5A$uRp~ZqkichyrW?R;?cE>-A$q@+6*&xg2V<H6xG_?+G;khfxC5E<sMb$
zIfw7iW~_6YA31Att>BT`pV8NMJAvEuy?@Q4)l~y^5zj2V*P`yU%Uu^)=jU!5H;<!(
z`d9dVMM}MM%owY@OrvVCXupe5n^pK;Ls3}m@O66$%-x3f8o|$KOd`j>1Flz<>%Krw
z-F5J1Uiz4wAB@C5J<XbB{wZ#$(wMilG>-;+$CW8+u<si77`_zrvfO~TH(S>!LS_n^
z8yfSg#0~*@GhppUPC?bSi53YjJHnbagnLbF#Fac_r~$p75@@N7gBnwVQRmb<jG2<w
z{A%){?=vV5tUERT{cZtu(9^56d_Tk+FJ$`TZ_yj`LG3jOJ$tFB@1in_c($8^%;u;)
zw(it)U6S5`SKnBx7Fy%S(+ls^tHr%yXmz=n@3B>>b(6H;S@_FKH*T5d+(wh?huoem
zn;s@9Uu`#ACYy>?V}_ZRJ{|Y7s$ZDPdnX2NlK4ZP)^OtQd1I0>an0M=2)=X?^j_S&
zj$*BSw7z1O*Zj@DUf#eHMpXpsWt~QQ80XyPEkLho>|c2z2JuqEd=aQUMun-TL#@iJ
z_ByhtI!W~WLAn@Eq^+0U=xaVVI(R;*xpa{`ZsAQ*JSz%lOwEJs#w;3%m9_TMPh(YW
z{M^!9QG3BLXbPo2G=f`nvwp&BZiLxe;$98P^;j!_`k4ou3y=Z+8$zeiU~NQZzj8PE
z=19*1{|rKHUIZRGZ~ArN0UK=I*--a4=Nwu_WE%5(eV!NC9?N}Sz5Q5482@)%#@txd
ej24rqJDYP=x8^**%<Jt&uW;9Ker;J7bN(N5U8tV`

diff --git a/3rdparty/qpOASES/qpOASES-3.0/make.mk b/3rdparty/qpOASES/qpOASES-3.0/make.mk
deleted file mode 100644
index ae907b4a1ea..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/make.mk
+++ /dev/null
@@ -1,38 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-##
-##	Filename:  make.mk
-##	Author:    Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
-##	Version:   3.0
-##	Date:      2007-2014
-##
-
-
-TOP = $(realpath $(dir $(lastword $(MAKEFILE_LIST))))
-
-include ${TOP}/make_linux.mk
-#include ${TOP}/make_cygwin.mk
-#include ${TOP}/make_windows.mk
-#include ${TOP}/make_osx.mk
diff --git a/3rdparty/qpOASES/qpOASES-3.0/make_cygwin.mk b/3rdparty/qpOASES/qpOASES-3.0/make_cygwin.mk
deleted file mode 100644
index 2ffdd18b57c..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/make_cygwin.mk
+++ /dev/null
@@ -1,106 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-##
-##	Filename:  make_cygwin.mk
-##	Author:    Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
-##	Version:   3.0
-##	Date:      2007-2014
-##
-
-################################################################################
-# user configuration
-
-# include directories, relative
-IDIR =   ${TOP}/include
-SRCDIR = ${TOP}/src
-BINDIR = ${TOP}/bin
-
-# Matlab include directory (ADAPT TO YOUR LOCAL SETTINGS!)
-MATLAB_IDIR   = /cygdrive/c/progra~2/matlab/r2009b/extern/include/
-MATLAB_LIBDIR = /cygdrive/c/progra~2/matlab/r2009b/extern/lib/win32/microsoft
-
-
-# system or replacement BLAS/LAPACK
-REPLACE_LINALG = 1
-
-ifeq ($(REPLACE_LINALG), 1)
-	LIB_BLAS =   ${SRCDIR}/BLASReplacement.o
-	LIB_LAPACK = ${SRCDIR}/LAPACKReplacement.o
-else	
-	LIB_BLAS =   /usr/lib/lapack/cygblas-0.dll
-	LIB_LAPACK = /usr/lib/lapack/cyglapack-0.dll
-endif	
-
-################################################################################
-# do not touch this
-
-CPP = g++
-AR  = ar
-RM  = rm
-F77 = gfortran
-ECHO = echo
-CD = cd
-CP = cp
-
-# file extensions
-OBJEXT = o
-LIBEXT = a
-DLLEXT = so
-EXE = .exe
-MEXOCTEXT = mex
-DEF_TARGET = -o $@
-SHARED = -shared
-
-# 32 or 64 depending on target platform
-BITS = $(shell getconf LONG_BIT)
-
-# decide on MEX interface extension
-ifeq ($(BITS), 32)
-	MEXEXT = mexw32
-else
-	MEXEXT = mexw64
-endif
-
-CPPFLAGS = -Wall -pedantic -Wshadow -Wfloat-equal -Wconversion -Wsign-conversion -O3 -finline-functions -DWIN32 
-#          -g -D__DEBUG__ -D__NO_COPYRIGHT__ -D__SUPPRESSANYOUTPUT__ -D__USE_SINGLE_PRECISION__ 
-
-FFLAGS = -Wall -O3 -fPIC -DLINUX -Wno-uninitialized
-#        -g 
-
-# libraries to link against when building qpOASES .so files
-LINK_LIBRARIES = ${LIB_LAPACK} ${LIB_BLAS} -lm 
-LINK_LIBRARIES_AW = ${LIB_LAPACK} ${LIB_BLAS} -lm -lgfortran 
-
-# how to link against the qpOASES shared library
-QPOASES_LINK = -L${BINDIR} -Wl,-rpath=${BINDIR} -lqpOASES 
-QPOASES_AW_LINK = -L${BINDIR} -Wl,-rpath=${BINDIR} -lqpOASES_aw
-
-# link dependencies when creating executables
-LINK_DEPENDS = ${LIB_LAPACK} ${LIB_BLAS} ${BINDIR}/libqpOASES.${LIBEXT} ${BINDIR}/libqpOASES.${DLLEXT}
-
-
-##
-##	end of file
-##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/make_linux.mk b/3rdparty/qpOASES/qpOASES-3.0/make_linux.mk
deleted file mode 100644
index b36f90b8791..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/make_linux.mk
+++ /dev/null
@@ -1,107 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-##
-##	Filename:  make_linux.mk
-##	Author:    Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
-##	Version:   3.0
-##	Date:      2007-2014
-##
-
-################################################################################
-# user configuration
-
-# include directories, relative
-IDIR =   ${TOP}/include
-SRCDIR = ${TOP}/src
-BINDIR = ${TOP}/bin
-
-# Matlab include directory (ADAPT TO YOUR LOCAL SETTINGS!)
-#MATLAB_IDIR   = ${HOME}/Programs/matlab/extern/include/
-MATLAB_IDIR   = /usr/local/matlab/extern/include/
-MATLAB_LIBDIR = /usr/local/matlab/bin/glnxa64/
-
-
-# system or replacement BLAS/LAPACK
-REPLACE_LINALG = 1
-
-ifeq ($(REPLACE_LINALG), 1)
-	LIB_BLAS =   ${SRCDIR}/BLASReplacement.o
-	LIB_LAPACK = ${SRCDIR}/LAPACKReplacement.o
-else
-	LIB_BLAS =   /usr/lib/libblas.so
-	LIB_LAPACK = /usr/lib/liblapack.so
-endif
-
-################################################################################
-# do not touch this
-
-CPP = g++
-AR  = ar
-RM  = rm
-F77 = gfortran
-ECHO = echo
-CD = cd
-CP = cp
-
-# file extensions
-OBJEXT = o
-LIBEXT = a
-DLLEXT = so
-EXE = 
-MEXOCTEXT = mex
-DEF_TARGET = -o $@
-SHARED = -shared
-
-# 32 or 64 depending on target platform
-BITS = $(shell getconf LONG_BIT)
-
-# decide on MEX interface extension
-ifeq ($(BITS), 32)
-	MEXEXT = mexglx
-else
-	MEXEXT = mexa64
-endif
-
-CPPFLAGS = -Wall -pedantic -Wshadow -Wfloat-equal -O3 -finline-functions -fPIC -DLINUX -D__NO_COPYRIGHT__
-#          -g -D__DEBUG__ -D__NO_COPYRIGHT__ -D__SUPPRESSANYOUTPUT__ -D__USE_SINGLE_PRECISION__ 
-
-FFLAGS = -Wall -O3 -fPIC -DLINUX -Wno-uninitialized
-#        -g
-
-# libraries to link against when building qpOASES .so files
-LINK_LIBRARIES = ${LIB_LAPACK} ${LIB_BLAS} -lm
-LINK_LIBRARIES_AW = ${LIB_LAPACK} ${LIB_BLAS} -lm -lgfortran -lhsl_ma57 -lfakemetis
-
-# how to link against the qpOASES shared library
-QPOASES_LINK = -L${BINDIR} -Wl,-rpath=${BINDIR} -lqpOASES
-QPOASES_AW_LINK = -L${BINDIR} -Wl,-rpath=${BINDIR} -lqpOASES_aw
-
-# link dependencies when creating executables
-LINK_DEPENDS = ${LIB_LAPACK} ${LIB_BLAS} ${BINDIR}/libqpOASES.${LIBEXT} ${BINDIR}/libqpOASES.${DLLEXT}
-
-
-##
-##	end of file
-##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/make_osx.mk b/3rdparty/qpOASES/qpOASES-3.0/make_osx.mk
deleted file mode 100644
index 71982da7f29..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/make_osx.mk
+++ /dev/null
@@ -1,99 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-##
-##	Filename:  make_osx.mk
-##	Author:    Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
-##	Version:   3.0
-##	Date:      2007-2014
-##
-
-################################################################################
-# user configuration
-
-# include directories, relative
-IDIR =   ${TOP}/include
-SRCDIR = ${TOP}/src
-BINDIR = ${TOP}/bin
-
-# Matlab include directory (ADAPT TO YOUR LOCAL SETTINGS!)
-MATLAB_IDIR   = /Applications/MATLAB_R2011a.app/extern/include/
-MATLAB_LIBDIR = 
-
-## system BLAS or qpOASES replacement BLAS
-#LIB_BLAS = "-framework Accelerate"
-
-## system LAPACK or qpOASES replacement LAPACK
-#LIB_LAPACK = "-framework Accelerate"
-
-################################################################################
-# do not touch this
-
-CPP = g++
-AR  = ar
-RM  = rm
-F77 = gfortran
-ECHO = echo
-CD = cd
-
-# file extensions
-OBJEXT = o
-LIBEXT = a
-DLLEXT = dylib
-EXE = 
-MEXOCTEXT = mex
-DEF_TARGET = -o $@
-SHARED = -shared
-
-# 32 or 64 depending on target platform
-BITS = $(shell getconf LONG_BIT)
-
-# decide on MEX interface extension
-ifeq ($(BITS), 32)
-	MEXEXT = mexglx
-else
-	MEXEXT = mexa64
-endif
-
-CPPFLAGS = -Wall -pedantic -Wshadow -Wfloat-equal -Wconversion -Wsign-conversion -O3 -finline-functions -fPIC -DLINUX
-#          -g -D__DEBUG__ -D__NO_COPYRIGHT__ -D__SUPPRESSANYOUTPUT__ -D__USE_SINGLE_PRECISION__
-
-FFLAGS = -Wall -O3 -fPIC -DLINUX -Wno-uninitialized
-#        -g 
-
-# libraries to link against when building qpOASES .so files
-LINK_LIBRARIES = -framework Accelerate -lm
-LINK_LIBRARIES_AW = -framework Accelerate ${LIB_LAPACK} ${LIB_BLAS} -lm -lgfortran -lhsl_ma57 -lfakemetis
-
-# how to link against the qpOASES shared library
-QPOASES_LINK = -L${BINDIR}  -lqpOASES 
-QPOASES_AW_LINK = -L${BINDIR}  -lqpOASES_aw
-
-# link dependencies when creating executables
-LINK_DEPENDS = ${LIB_LAPACK} ${LIB_BLAS} ${BINDIR}/libqpOASES.${LIBEXT} ${BINDIR}/libqpOASES.${DLLEXT}
-
-
-##
-##	end of file
-##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/make_windows.mk b/3rdparty/qpOASES/qpOASES-3.0/make_windows.mk
deleted file mode 100644
index 95ae0786831..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/make_windows.mk
+++ /dev/null
@@ -1,114 +0,0 @@
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-##
-##	Filename:  make_windows.mk
-##	Author:    Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
-##	Version:   3.0
-##	Date:      2007-2014
-##
-
-
-##
-##	definitions for compiling with Visual Studio under Windows
-##
-
-
-################################################################################
-# user configuration
-
-# include directories, relative
-IDIR =   ${TOP}/include
-SRCDIR = ${TOP}/src
-BINDIR = ${TOP}/bin
-
-# Matlab include directory (ADAPT TO YOUR LOCAL SETTINGS!)
-#MATLAB_IDIR   = ${HOME}/Programs/matlab/extern/include/
-MATLAB_IDIR = /usr/local/matlab/extern/include/
-MATLAB_LIBDIR = /usr/local/matlab/bin/glnxa64/
-
-
-# system or replacement BLAS/LAPACK
-REPLACE_LINALG = 1
-
-ifeq ($(REPLACE_LINALG), 1)
-	LIB_BLAS =   ${SRCDIR}/BLASReplacement.o
-	LIB_LAPACK = ${SRCDIR}/LAPACKReplacement.o
-else
-	LIB_BLAS =   /usr/lib/libblas.so
-	LIB_LAPACK = /usr/lib/liblapack.so
-endif
-
-
-################################################################################
-# do not touch this
-
-CPP = cl
-AR  = ar
-RM  = rm
-F77 = gfortran
-ECHO = echo
-CD = cd
-CP = copy
-
-# file extensions
-OBJEXT = obj
-LIBEXT = lib
-DLLEXT = so
-EXE = .exe
-MEXOCTEXT = mex
-DEF_TARGET = 
-SHARED = /LD
-
-# 32 or 64 depending on target platform
-BITS = $(shell getconf LONG_BIT)
-
-# decide on MEX interface extension
-ifeq ($(BITS), 32)
-	MEXEXT = mexglx
-else
-	MEXEXT = mexa64
-endif
-
-CPPFLAGS = -nologo -EHsc -DWIN32 -Dsnprintf=_snprintf
-#-g -D__DEBUG__ -D__NO_COPYRIGHT__ -D__SUPPRESSANYOUTPUT__
-
-FFLAGS = -Wall -O3 -fPIC -DWIN32 -Wno-uninitialized
-#        -g
-
-# libraries to link against when building qpOASES .so files
-LINK_LIBRARIES = ${LIB_LAPACK} ${LIB_BLAS}
-LINK_LIBRARIES_AW = ${LIB_LAPACK} ${LIB_BLAS} -lm -lgfortran -lhsl_ma57 -lfakemetis
-
-# how to link against the qpOASES shared library
-QPOASES_LINK = /I${BINDIR} /WL /link ${BINDIR}/libqpOASES.lib
-QPOASES_AW_LINK = /I${BINDIR} /WL /link ${BINDIR}/libqpOASES_aw.lib
-
-# link dependencies when creating executables
-LINK_DEPENDS = ${LIB_LAPACK} ${LIB_BLAS} ${BINDIR}/libqpOASES.${LIBEXT} ${BINDIR}/libqpOASES.${DLLEXT}
-
-
-##
-##	end of file
-##
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/BLASReplacement.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/BLASReplacement.cpp
deleted file mode 100644
index d882eba8766..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/BLASReplacement.cpp
+++ /dev/null
@@ -1,145 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file src/BLASReplacement.cpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	BLAS Level 3 replacement routines.
- */
-
-
-#include <qpOASES/Utils.hpp>
-
-
-extern "C" void dgemm_ ( const char *TRANSA, const char *TRANSB,
-		const unsigned long *M, const unsigned long *N, const unsigned long *K,
-		const double *ALPHA, const double *A, const unsigned long *LDA, const double *B, const unsigned long *LDB,
-		const double *BETA, double *C, const unsigned long *LDC)
-{
-	unsigned int i, j, k;
-
-	if ( REFER_NAMESPACE_QPOASES isZero(*BETA) == REFER_NAMESPACE_QPOASES BT_TRUE )
-		for (k = 0; k < *N; k++)
-			for (j = 0; j < *M; j++)
-				C[j+(*LDC)*k] = 0.0;
-	else if ( REFER_NAMESPACE_QPOASES isEqual(*BETA,-1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
-		for (k = 0; k < *N; k++)
-			for (j = 0; j < *M; j++)
-				C[j+(*LDC)*k] = -C[j+(*LDC)*k];
-	else if ( REFER_NAMESPACE_QPOASES isEqual(*BETA,1.0) == REFER_NAMESPACE_QPOASES BT_FALSE )
-		for (k = 0; k < *N; k++)
-			for (j = 0; j < *M; j++)
-				C[j+(*LDC)*k] *= *BETA;
-
-	if (TRANSA[0] == 'N')
-		if ( REFER_NAMESPACE_QPOASES isEqual(*ALPHA,1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
-			for (k = 0; k < *N; k++)
-				for (j = 0; j < *M; j++)
-					for (i = 0; i < *K; i++)
-						C[j+(*LDC)*k] += A[j+(*LDA)*i] * B[i+(*LDB)*k];
-		else if ( REFER_NAMESPACE_QPOASES isEqual(*ALPHA,-1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
-			for (k = 0; k < *N; k++)
-				for (j = 0; j < *M; j++)
-					for (i = 0; i < *K; i++)
-						C[j+(*LDC)*k] -= A[j+(*LDA)*i] * B[i+(*LDB)*k];
-		else
-			for (k = 0; k < *N; k++)
-				for (j = 0; j < *M; j++)
-					for (i = 0; i < *K; i++)
-						C[j+(*LDC)*k] += *ALPHA * A[j+(*LDA)*i] * B[i+(*LDB)*k];
-	else
-		if ( REFER_NAMESPACE_QPOASES isEqual(*ALPHA,1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
-			for (k = 0; k < *N; k++)
-				for (j = 0; j < *M; j++)
-					for (i = 0; i < *K; i++)
-						C[j+(*LDC)*k] += A[i+(*LDA)*j] * B[i+(*LDB)*k];
-		else if ( REFER_NAMESPACE_QPOASES isEqual(*ALPHA,-1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
-			for (k = 0; k < *N; k++)
-				for (j = 0; j < *M; j++)
-					for (i = 0; i < *K; i++)
-						C[j+(*LDC)*k] -= A[i+(*LDA)*j] * B[i+(*LDB)*k];
-		else
-			for (k = 0; k < *N; k++)
-				for (j = 0; j < *M; j++)
-					for (i = 0; i < *K; i++)
-						C[j+(*LDC)*k] += *ALPHA * A[i+(*LDA)*j] * B[i+(*LDB)*k];
-}
-
-extern "C" void sgemm_ ( const char *TRANSA, const char *TRANSB,
-		const unsigned long *M, const unsigned long *N, const unsigned long *K,
-		const float *ALPHA, const float *A, const unsigned long *LDA, const float *B, const unsigned long *LDB,
-		const float *BETA, float *C, const unsigned long *LDC)
-{
-	unsigned int i, j, k;
-
-	if ( REFER_NAMESPACE_QPOASES isZero(*BETA) == REFER_NAMESPACE_QPOASES BT_TRUE )
-		for (k = 0; k < *N; k++)
-			for (j = 0; j < *M; j++)
-				C[j+(*LDC)*k] = 0.0;
-	else if ( REFER_NAMESPACE_QPOASES isEqual(*BETA,-1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
-		for (k = 0; k < *N; k++)
-			for (j = 0; j < *M; j++)
-				C[j+(*LDC)*k] = -C[j+(*LDC)*k];
-	else if ( REFER_NAMESPACE_QPOASES isEqual(*BETA,1.0) == REFER_NAMESPACE_QPOASES BT_FALSE )
-		for (k = 0; k < *N; k++)
-			for (j = 0; j < *M; j++)
-				C[j+(*LDC)*k] *= *BETA;
-
-	if (TRANSA[0] == 'N')
-		if ( REFER_NAMESPACE_QPOASES isEqual(*ALPHA,1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
-			for (k = 0; k < *N; k++)
-				for (j = 0; j < *M; j++)
-					for (i = 0; i < *K; i++)
-						C[j+(*LDC)*k] += A[j+(*LDA)*i] * B[i+(*LDB)*k];
-		else if ( REFER_NAMESPACE_QPOASES isEqual(*ALPHA,-1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
-			for (k = 0; k < *N; k++)
-				for (j = 0; j < *M; j++)
-					for (i = 0; i < *K; i++)
-						C[j+(*LDC)*k] -= A[j+(*LDA)*i] * B[i+(*LDB)*k];
-		else
-			for (k = 0; k < *N; k++)
-				for (j = 0; j < *M; j++)
-					for (i = 0; i < *K; i++)
-						C[j+(*LDC)*k] += *ALPHA * A[j+(*LDA)*i] * B[i+(*LDB)*k];
-	else
-		if ( REFER_NAMESPACE_QPOASES isEqual(*ALPHA,1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
-			for (k = 0; k < *N; k++)
-				for (j = 0; j < *M; j++)
-					for (i = 0; i < *K; i++)
-						C[j+(*LDC)*k] += A[i+(*LDA)*j] * B[i+(*LDB)*k];
-		else if ( REFER_NAMESPACE_QPOASES isEqual(*ALPHA,-1.0) == REFER_NAMESPACE_QPOASES BT_TRUE )
-			for (k = 0; k < *N; k++)
-				for (j = 0; j < *M; j++)
-					for (i = 0; i < *K; i++)
-						C[j+(*LDC)*k] -= A[i+(*LDA)*j] * B[i+(*LDB)*k];
-		else
-			for (k = 0; k < *N; k++)
-				for (j = 0; j < *M; j++)
-					for (i = 0; i < *K; i++)
-						C[j+(*LDC)*k] += *ALPHA * A[i+(*LDA)*j] * B[i+(*LDB)*k];
-}
-
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/Bounds.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/Bounds.cpp
deleted file mode 100644
index 59a5a945f4b..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/Bounds.cpp
+++ /dev/null
@@ -1,515 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file src/Bounds.cpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of the Bounds class designed to manage working sets of
- *	bounds within a QProblem.
- */
-
-
-#include <qpOASES/Bounds.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	B o u n d s
- */
-Bounds::Bounds( ) : SubjectTo( )
-{
-}
-
-
-/*
- *	B o u n d s
- */
-Bounds::Bounds( int _n ) : SubjectTo( _n )
-{
-	init( _n );
-}
-
-
-/*
- *	B o u n d s
- */
-Bounds::Bounds( const Bounds& rhs ) : SubjectTo( rhs )
-{
-	copy( rhs );
-}
-
-
-/*
- *	~ B o u n d s
- */
-Bounds::~Bounds( )
-{
-	clear( );
-}
-
-
-/*
- *	o p e r a t o r =
- */
-Bounds& Bounds::operator=( const Bounds& rhs )
-{
-	if ( this != &rhs )
-	{
-		clear( );
-		SubjectTo::operator=( rhs );
-		copy( rhs );
-	}
-
-	return *this;
-}
-
-
-
-/*
- *	i n i t
- */
-returnValue Bounds::init(	int _n
-							)
-{
-	if ( _n < 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	clear( );
-
-	if ( _n > 0 )
-	{
-		freee.init( _n );
-		fixed.init( _n );
-	}
-
-	return SubjectTo::init( _n );
-}
-
-
-
-/*
- *	s e t u p B o u n d
- */
-returnValue Bounds::setupBound(	int number, SubjectToStatus _status
-								)
-{
-	/* consistency check */
-	if ( ( number < 0 ) || ( number >= n ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-	/* Add bound index to respective index list. */
-	switch ( _status )
-	{
-		case ST_INACTIVE:
-			if ( this->addIndex( this->getFree( ),number,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_BOUND_FAILED );
-			break;
-
-		case ST_LOWER:
-			if ( this->addIndex( this->getFixed( ),number,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_BOUND_FAILED );
-			break;
-
-		case ST_UPPER:
-			if ( this->addIndex( this->getFixed( ),number,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_BOUND_FAILED );
-			break;
-
-		default:
-			return THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A l l F r e e
- */
-returnValue Bounds::setupAllFree( )
-{
-	return setupAll( ST_INACTIVE );
-}
-
-
-/*
- *	s e t u p A l l L o w e r
- */
-returnValue Bounds::setupAllLower( )
-{
-	return setupAll( ST_LOWER );
-}
-
-
-/*
- *	s e t u p A l l U p p e r
- */
-returnValue Bounds::setupAllUpper( )
-{
-	return setupAll( ST_UPPER );
-}
-
-
-/*
- *	m o v e F i x e d T o F r e e
- */
-returnValue Bounds::moveFixedToFree( int number )
-{
-	/* consistency check */
-	if ( ( number < 0 ) || ( number >= n ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-	/* Move index from indexlist of fixed variables to that of free ones. */
-	if ( this->removeIndex( this->getFixed( ),number ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_MOVING_BOUND_FAILED );
-
-	if ( this->addIndex( this->getFree( ),number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_MOVING_BOUND_FAILED );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	m o v e F r e e T o F i x e d
- */
-returnValue Bounds::moveFreeToFixed(	int number, SubjectToStatus _status
-										)
-{
-	/* consistency check */
-	if ( ( number < 0 ) || ( number >= n ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-	/* Move index from indexlist of free variables to that of fixed ones. */
-	if ( this->removeIndex( this->getFree( ),number ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_MOVING_BOUND_FAILED );
-
-	if ( this->addIndex( this->getFixed( ),number,_status ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_MOVING_BOUND_FAILED );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	f l i p F i x e d
- */
-returnValue Bounds::flipFixed( int number )
-{
-	/* consistency check */
-	if ( ( number < 0 ) || ( number >= n ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-	if ( status != 0 )
-		switch (status[number])
-		{
-			case ST_LOWER: status[number] = ST_UPPER; break;
-			case ST_UPPER: status[number] = ST_LOWER; break;
-			default: return THROWERROR( RET_MOVING_BOUND_FAILED );
-		}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s w a p F r e e
- */
-returnValue Bounds::swapFree(	int number1, int number2
-								)
-{
-	/* consistency check */
-	if ( ( number1 < 0 ) || ( number1 >= n ) || ( number2 < 0 ) || ( number2 >= n ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-	/* Swap index within indexlist of free variables. */
-	return this->swapIndex( this->getFree( ),number1,number2 );
-}
-
-
-/*
- *	s h i f t
- */
-returnValue Bounds::shift(	int offset )
-{
-	int i;
-
-	/* consistency check */
-	if ( ( offset == 0 ) || ( n <= 1 ) )
-		return SUCCESSFUL_RETURN;
-
-	if ( ( offset < 0 ) || ( offset > n/2 ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-	if ( ( n % offset ) != 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-
-	/* 1) Shift types and status. */
-	for( i=0; i<n-offset; ++i )
-	{
-		setType( i,getType( i+offset ) );
-		setStatus( i,getStatus( i+offset ) );
-	}
-
-	/* 2) Construct shifted index lists of free and fixed variables. */
-	Indexlist shiftedFreee( n );
-	Indexlist shiftedFixed( n );
-
-	for( i=0; i<n; ++i )
-	{
-		switch ( getStatus( i ) )
-		{
-			case ST_INACTIVE:
-				if ( shiftedFreee.addNumber( i ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SHIFTING_FAILED );
-				break;
-
-			case ST_LOWER:
-				if ( shiftedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SHIFTING_FAILED );
-				break;
-
-			case ST_UPPER:
-				if ( shiftedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SHIFTING_FAILED );
-				break;
-
-			default:
-				return THROWERROR( RET_SHIFTING_FAILED );
-		}
-	}
-
-	/* 3) Assign shifted index list. */
-	freee = shiftedFreee;
-	fixed = shiftedFixed;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	r o t a t e
- */
-returnValue Bounds::rotate( int offset )
-{
-	int i;
-
-	/* consistency check */
-	if ( ( offset == 0 ) || ( offset == n ) || ( n <= 1 ) )
-		return SUCCESSFUL_RETURN;
-
-	if ( ( offset < 0 ) || ( offset > n ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-
-	/* 1) Rotate types and status. */
-	SubjectToType*   typeTmp   = new SubjectToType[offset];
-	SubjectToStatus* statusTmp = new SubjectToStatus[offset];
-
-	for( i=0; i<offset; ++i )
-	{
-		typeTmp[i] = getType( i );
-		statusTmp[i] = getStatus( i );
-	}
-
-	for( i=0; i<n-offset; ++i )
-	{
-		setType( i,getType( i+offset ) );
-		setStatus( i,getStatus( i+offset ) );
-	}
-
-	for( i=n-offset; i<n; ++i )
-	{
-		setType( i,typeTmp[i-n+offset] );
-		setStatus( i,statusTmp[i-n+offset] );
-	}
-
-	delete[] statusTmp; delete[] typeTmp;
-
-	/* 2) Construct shifted index lists of free and fixed variables. */
-	Indexlist rotatedFreee( n );
-	Indexlist rotatedFixed( n );
-
-	for( i=0; i<n; ++i )
-	{
-		switch ( getStatus( i ) )
-		{
-			case ST_INACTIVE:
-				if ( rotatedFreee.addNumber( i ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_ROTATING_FAILED );
-				break;
-
-			case ST_LOWER:
-				if ( rotatedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_ROTATING_FAILED );
-				break;
-
-			case ST_UPPER:
-				if ( rotatedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_ROTATING_FAILED );
-				break;
-
-			default:
-				return THROWERROR( RET_ROTATING_FAILED );
-		}
-	}
-
-	/* 3) Assign shifted index list. */
-	freee = rotatedFreee;
-	fixed = rotatedFixed;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t
- */
-returnValue Bounds::print( )
-{
-	if ( n == 0 )
-		return SUCCESSFUL_RETURN;
-
-	#ifndef __XPCTARGET__
-	#ifndef __DSPACE__
-	char myPrintfString[MAX_STRING_LENGTH];
-
-	int nFR = getNFR( );
-	int nFX = getNFX( );
-
-	int* FR_idx;
-	getFree( )->getNumberArray( &FR_idx );
-
-	int* FX_idx;
-	getFixed( )->getNumberArray( &FX_idx );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"Bounds object comprising %d variables (%d free, %d fixed):\n",n,nFR,nFX );
-	myPrintf( myPrintfString );
-
-	REFER_NAMESPACE_QPOASES print( FR_idx,nFR,"free " );
-	REFER_NAMESPACE_QPOASES print( FX_idx,nFX,"fixed" );
-
-	#endif
-	#endif
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-/*
- *	c l e a r
- */
-returnValue Bounds::clear( )
-{
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	c o p y
- */
-returnValue Bounds::copy(	const Bounds& rhs
-							)
-{
-	freee = rhs.freee;
-	fixed = rhs.fixed;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	s e t u p A l l
- */
-returnValue Bounds::setupAll( SubjectToStatus _status )
-{
-	int i;
-
-	/* 1) Place unbounded variables at the beginning of the index list of free variables. */
-	for( i=0; i<n; ++i )
-	{
-		if ( getType( i ) == ST_UNBOUNDED )
-		{
-			if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SETUP_BOUND_FAILED );
-		}
-	}
-
-	/* 2) Add remaining (i.e. bounded but possibly free) variables to the index list of free variables. */
-	for( i=0; i<n; ++i )
-	{
-		if ( getType( i ) == ST_BOUNDED )
-		{
-			if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_BOUND_FAILED );
-		}
-	}
-
-	/* 3) Place implicitly fixed variables at the end of the index list of free variables. */
-	for( i=0; i<n; ++i )
-	{
-		if ( getType( i ) == ST_EQUALITY )
-		{
-			if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_BOUND_FAILED );
-		}
-	}
-
-	/* 4) Moreover, add all bounds of unknown type. */
-	for( i=0; i<n; ++i )
-	{
-		if ( getType( i ) == ST_UNKNOWN || getType( i ) == ST_DISABLED )
-		{
-			if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_BOUND_FAILED );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/Constraints.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/Constraints.cpp
deleted file mode 100644
index c0ec04590d2..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/Constraints.cpp
+++ /dev/null
@@ -1,500 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file src/Constraints.cpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of the Constraints class designed to manage working sets of
- *	constraints within a QProblem.
- */
-
-
-#include <qpOASES/Constraints.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	C o n s t r a i n t s
- */
-Constraints::Constraints( ) : SubjectTo( )
-{
-}
-
-
-/*
- *	C o n s t r a i n t s
- */
-Constraints::Constraints( int _n ) : SubjectTo( _n )
-{
-	init( _n );
-}
-
-
-/*
- *	C o n s t r a i n t s
- */
-Constraints::Constraints( const Constraints& rhs ) : SubjectTo( rhs )
-{
-	copy( rhs );
-}
-
-
-/*
- *	~ C o n s t r a i n t s
- */
-Constraints::~Constraints( )
-{
-	clear( );
-}
-
-
-/*
- *	o p e r a t o r =
- */
-Constraints& Constraints::operator=( const Constraints& rhs )
-{
-	if ( this != &rhs )
-	{
-		clear( );
-		SubjectTo::operator=( rhs );
-		copy( rhs );
-	}
-
-	return *this;
-}
-
-
-/*
- *	i n i t
- */
-returnValue Constraints::init(	int _n
-								)
-{
-	if ( _n < 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	clear( );
-
-	if ( _n > 0 )
-	{
-		active.init(   _n );
-		inactive.init( _n );
-	}
-
-	return SubjectTo::init( _n );
-}
-
-
-
-/*
- *	s e t u p C o n s t r a i n t
- */
-returnValue Constraints::setupConstraint(	int number, SubjectToStatus _status
-											)
-{
-	/* consistency check */
-	if ( ( number < 0 ) || ( number >= n ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-	/* Add constraint index to respective index list. */
-	switch ( _status )
-	{
-		case ST_INACTIVE:
-			if ( this->addIndex( this->getInactive( ),number,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
-			break;
-
-		case ST_LOWER:
-			if ( this->addIndex( this->getActive( ),number,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
-			break;
-
-		case ST_UPPER:
-			if ( this->addIndex( this->getActive( ),number,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
-			break;
-
-		default:
-			return THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A l l I n a c t i v e
- */
-returnValue Constraints::setupAllInactive( )
-{
-	return setupAll( ST_INACTIVE );
-}
-
-
-/*
- *	s e t u p A l l L o w e r
- */
-returnValue Constraints::setupAllLower( )
-{
-	return setupAll( ST_LOWER );
-}
-
-
-/*
- *	s e t u p A l l U p p e r
- */
-returnValue Constraints::setupAllUpper( )
-{
-	return setupAll( ST_UPPER );
-}
-
-
-/*
- *	m o v e A c t i v e T o I n a c t i v e
- */
-returnValue Constraints::moveActiveToInactive( int number )
-{
-	/* consistency check */
-	if ( ( number < 0 ) || ( number >= n ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-	/* Move index from indexlist of active constraints to that of inactive ones. */
-	if ( this->removeIndex( this->getActive( ),number ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_MOVING_BOUND_FAILED );
-
-	if ( this->addIndex( this->getInactive( ),number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_MOVING_BOUND_FAILED );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	m o v e I n a c t i v e T o A c t i v e
- */
-returnValue Constraints::moveInactiveToActive(	int number, SubjectToStatus _status
-												)
-{
-	/* consistency check */
-	if ( ( number < 0 ) || ( number >= n ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-	/* Move index from indexlist of inactive constraints to that of active ones. */
-	if ( this->removeIndex( this->getInactive( ),number ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_MOVING_BOUND_FAILED );
-
-	if ( this->addIndex( this->getActive( ),number,_status ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_MOVING_BOUND_FAILED );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	f l i p F i x e d
- */
-returnValue Constraints::flipFixed( int number )
-{
-	/* consistency check */
-	if ( ( number < 0 ) || ( number >= n ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-	if ( status != 0 )
-		switch (status[number])
-		{
-			case ST_LOWER: status[number] = ST_UPPER; break;
-			case ST_UPPER: status[number] = ST_LOWER; break;
-			default: return THROWERROR( RET_MOVING_CONSTRAINT_FAILED );
-		}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s h i f t
- */
-returnValue Constraints::shift( int offset )
-{
-	int i;
-
-	/* consistency check */
-	if ( ( offset == 0 ) || ( n <= 1 ) )
-		return SUCCESSFUL_RETURN;
-
-	if ( ( offset < 0 ) || ( offset > n/2 ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-	if ( ( n % offset ) != 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-
-	/* 1) Shift types and status. */
-	for( i=0; i<n-offset; ++i )
-	{
-		setType( i,getType( i+offset ) );
-		setStatus( i,getStatus( i+offset ) );
-	}
-
-	/* 2) Construct shifted index lists of free and fixed variables. */
-	Indexlist shiftedActive( n );
-	Indexlist shiftedInactive( n );
-
-	for( i=0; i<n; ++i )
-	{
-		switch ( getStatus( i ) )
-		{
-			case ST_INACTIVE:
-				if ( shiftedInactive.addNumber( i ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SHIFTING_FAILED );
-				break;
-
-			case ST_LOWER:
-				if ( shiftedActive.addNumber( i ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SHIFTING_FAILED );
-				break;
-
-			case ST_UPPER:
-				if ( shiftedActive.addNumber( i ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SHIFTING_FAILED );
-				break;
-
-			default:
-				return THROWERROR( RET_SHIFTING_FAILED );
-		}
-	}
-
-	/* 3) Assign shifted index list. */
-	active = shiftedActive;
-	inactive = shiftedInactive;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	r o t a t e
- */
-returnValue Constraints::rotate( int offset )
-{
-	int i;
-
-	/* consistency check */
-	if ( ( offset == 0 ) || ( offset == n ) || ( n <= 1 ) )
-		return SUCCESSFUL_RETURN;
-
-	if ( ( offset < 0 ) || ( offset > n ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-
-	/* 1) Rotate types and status. */
-	SubjectToType*   typeTmp   = new SubjectToType[offset];
-	SubjectToStatus* statusTmp = new SubjectToStatus[offset];
-
-	for( i=0; i<offset; ++i )
-	{
-		typeTmp[i] = getType( i );
-		statusTmp[i] = getStatus( i );
-	}
-
-	for( i=0; i<n-offset; ++i )
-	{
-		setType( i,getType( i+offset ) );
-		setStatus( i,getStatus( i+offset ) );
-	}
-
-	for( i=n-offset; i<n; ++i )
-	{
-		setType( i,typeTmp[i-n+offset] );
-		setStatus( i,statusTmp[i-n+offset] );
-	}
-
-	delete[] statusTmp; delete[] typeTmp;
-
-	/* 2) Construct shifted index lists of free and fixed variables. */
-	Indexlist rotatedActive( n );
-	Indexlist rotatedInactive( n );
-
-	for( i=0; i<n; ++i )
-	{
-		switch ( getStatus( i ) )
-		{
-			case ST_INACTIVE:
-				if ( rotatedInactive.addNumber( i ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_ROTATING_FAILED );
-				break;
-
-			case ST_LOWER:
-				if ( rotatedActive.addNumber( i ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_ROTATING_FAILED );
-				break;
-
-			case ST_UPPER:
-				if ( rotatedActive.addNumber( i ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_ROTATING_FAILED );
-				break;
-
-			default:
-				return THROWERROR( RET_ROTATING_FAILED );
-		}
-	}
-
-	/* 3) Assign shifted index list. */
-	active = rotatedActive;
-	inactive = rotatedInactive;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t
- */
-returnValue Constraints::print( )
-{
-	if ( n == 0 )
-		return SUCCESSFUL_RETURN;
-
-	#ifndef __XPCTARGET__
-	#ifndef __DSPACE__
-	char myPrintfString[MAX_STRING_LENGTH];
-
-	int nIAC = getNIAC( );
-	int nAC  = getNAC( );
-
-	int* IAC_idx;
-	getInactive( )->getNumberArray( &IAC_idx );
-
-	int* AC_idx;
-	getActive( )->getNumberArray( &AC_idx );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"Constraints object comprising %d constraints (%d inactive, %d active):\n",n,nIAC,nAC );
-	myPrintf( myPrintfString );
-
-	REFER_NAMESPACE_QPOASES print( IAC_idx,nIAC,"inactive" );
-	REFER_NAMESPACE_QPOASES print( AC_idx, nAC, "active  " );
-
-	#endif
-	#endif
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-/*
- *	c l e a r
- */
-returnValue Constraints::clear( )
-{
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	c o p y
- */
-returnValue Constraints::copy(	const Constraints& rhs
-								)
-{
-	active   = rhs.active;
-	inactive = rhs.inactive;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	s e t u p A l l
- */
-returnValue Constraints::setupAll( SubjectToStatus _status )
-{
-	int i;
-
-	/* 1) Place unbounded constraints at the beginning of the index list of inactive constraints. */
-	for( i=0; i<n; ++i )
-	{
-		if ( getType( i ) == ST_UNBOUNDED )
-		{
-			if ( setupConstraint( i,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
-		}
-	}
-
-	/* 2) Add remaining (i.e. "real" inequality) constraints to the index list of inactive constraints. */
-	for( i=0; i<n; ++i )
-	{
-		if ( getType( i ) == ST_BOUNDED )
-		{
-			if ( setupConstraint( i,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
-		}
-	}
-
-	/* 3) Place implicit equality constraints at the end of the index list of inactive constraints. */
-	for( i=0; i<n; ++i )
-	{
-		if ( getType( i ) == ST_EQUALITY )
-		{
-			if ( setupConstraint( i,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
-		}
-	}
-
-	/* 4) Moreover, add all constraints of unknown type. */
-	for( i=0; i<n; ++i )
-	{
-		if ( getType( i ) == ST_UNKNOWN || getType( i ) == ST_DISABLED )
-		{
-			if ( setupConstraint( i,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
-		}
-	}
-
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/Flipper.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/Flipper.cpp
deleted file mode 100644
index 879d4b054d8..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/Flipper.cpp
+++ /dev/null
@@ -1,273 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file src/Flipper.cpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of the Flipper class designed to manage working sets of
- *	constraints and bounds within a QProblem.
- */
-
-
-#include <qpOASES/Flipper.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	F l i p p e r
- */
-Flipper::Flipper( )
-{
-	R = 0;
-	Q = 0;
-	T = 0;
-	
-	init( );
-}
-
-
-/*
- *	F l i p p e r
- */
-Flipper::Flipper(	unsigned int _nV,
-					unsigned int _nC
-					)
-{
-	R = 0;
-	Q = 0;
-	T = 0;
-	
-	init( _nV,_nC );
-}
-
-
-/*
- *	F l i p p e r
- */
-Flipper::Flipper( const Flipper& rhs )
-{
-	R = 0;
-	Q = 0;
-	T = 0;
-
-	copy( rhs );
-}
-
-
-/*
- *	~ F l i p p e r
- */
-Flipper::~Flipper( )
-{
-	clear( );
-}
-
-
-/*
- *	o p e r a t o r =
- */
-Flipper& Flipper::operator=( const Flipper& rhs )
-{
-	if ( this != &rhs )
-	{
-		clear( );
-		copy( rhs );
-	}
-
-	return *this;
-}
-
-
-
-/*
- *	i n i t
- */
-returnValue Flipper::init(	unsigned int _nV,
-							unsigned int _nC
-							)
-{
-	if ( ( _nV < 0 ) || ( _nC < 0 ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	clear( );
-
-	nV = _nV;
-	nC = _nC;
-
-	/*if ( nV > 0 )
-	{
-		R = new real_t[nV*nV];
-	
-		if ( nC > 0 )
-		{
-			Q = new real_t[nV*nV];
-			T = new real_t[getDimT()];
-		}
-	}*/
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	g e t
- */
-returnValue Flipper::get(	Bounds* const _bounds,
-							real_t* const _R,
-							Constraints* const _constraints,
-							real_t* const _Q,
-							real_t* const _T 
-							) const
-{
-	if ( _bounds != 0 )
-		*_bounds = bounds;
-
-	if ( _constraints != 0 )
-		*_constraints = constraints;
-
-	if ( ( _R != 0 ) && ( R != 0 ) )
-		memcpy( _R,R, nV*nV*sizeof(real_t) );
-
-	if ( ( _Q != 0 ) && ( Q != 0 ) )
-		memcpy( _Q,Q, nV*nV*sizeof(real_t) );
-
-	if ( ( _T != 0 ) && ( T != 0 ) )
-		memcpy( _T,T, getDimT()*sizeof(real_t) );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t
- */
-returnValue Flipper::set(	const Bounds* const _bounds,
-							const real_t* const _R,
-							const Constraints* const _constraints,
-							const real_t* const _Q,
-							const real_t* const _T
-							)
-{
-	if ( _bounds != 0 )
-		bounds = *_bounds;
-
-	if ( _constraints != 0 )
-		constraints = *_constraints;
-
-	if ( _R != 0 )
-	{
-		if ( R == 0 )
-			R = new real_t[nV*nV];
-
-		memcpy( R,_R, nV*nV*sizeof(real_t) );
-	}
-
-	if ( _Q != 0 )
-	{
-		if ( Q == 0 )
-			Q = new real_t[nV*nV];
-
-		memcpy( Q,_Q, nV*nV*sizeof(real_t) );
-	}
-
-	if ( _T != 0 )
-	{
-		if ( T == 0 )
-			T = new real_t[getDimT()];
-
-		memcpy( T,_T, getDimT()*sizeof(real_t) );
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-/*
- *	c l e a r
- */
-returnValue Flipper::clear( )
-{
-	if ( R != 0 )
-	{
-		delete[] R;
-		R = 0;
-	}
-	
-	if ( Q != 0 )
-	{
-		delete[] Q;
-		Q = 0;
-	}
-	
-	if ( T != 0 )
-	{
-		delete[] T;
-		T = 0;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	c o p y
- */
-returnValue Flipper::copy(	const Flipper& rhs
-							)
-{
-	return set( &(rhs.bounds),rhs.R, &(rhs.constraints),rhs.Q,rhs.T );
-}
-
-
-unsigned int Flipper::getDimT( ) const
-{
-	if ( nV > nC )
-		return nC*nC;
-	else
-		return nV*nV;
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/Indexlist.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/Indexlist.cpp
deleted file mode 100644
index dc2bac3179b..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/Indexlist.cpp
+++ /dev/null
@@ -1,319 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file src/Indexlist.cpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of the Indexlist class designed to manage index lists of
- *	constraints and bounds within a QProblem_SubjectTo.
- */
-
-
-#include <qpOASES/Indexlist.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	I n d e x l i s t
- */
-Indexlist::Indexlist( )
-{
-	number = 0;
-	iSort  = 0;
-
-	init( );
-}
-
-
-/*
- *	I n d e x l i s t
- */
-Indexlist::Indexlist( int n )
-{
-	number = 0;
-	iSort  = 0;
-
-	init( n );
-}
-
-
-/*
- *	I n d e x l i s t
- */
-Indexlist::Indexlist( const Indexlist& rhs )
-{
-	copy( rhs );
-}
-
-
-/*
- *	~ I n d e x l i s t
- */
-Indexlist::~Indexlist( )
-{
-	clear( );
-}
-
-
-/*
- *	o p e r a t o r =
- */
-Indexlist& Indexlist::operator=( const Indexlist& rhs )
-{
-	if ( this != &rhs )
-	{
-		clear( );
-		copy( rhs );
-	}
-
-	return *this;
-}
-
-
-
-/*
- *	i n i t
- */
-returnValue Indexlist::init(	int n
-								)
-{
-	if ( n < 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	clear( );
-
-	length = 0;
-	physicallength = n;
-
-	if ( n > 0 )
-	{
-		number = new int[n];
-		iSort  = new int[n];
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t N u m b e r A r r a y
- */
-returnValue Indexlist::getNumberArray( int** const numberarray ) const
-{
-	if (numberarray == 0)
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	*numberarray = number;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t I S o r t A r r a y
- */
-returnValue Indexlist::getISortArray( int** const iSortArray ) const
-{
-	*iSortArray = iSort;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t I n d e x
- */
-int Indexlist::getIndex( int givennumber ) const
-{
-	int index = findInsert(givennumber);
-	return number[iSort[index]] == givennumber ? iSort[index] : -1;
-}
-
-
-/*
- *	a d d N u m b e r
- */
-returnValue Indexlist::addNumber( int addnumber )
-{
-	if ( length >= physicallength )
-		return THROWERROR( RET_INDEXLIST_EXCEEDS_MAX_LENGTH );
-
-	int i, j;
-	number[length] = addnumber;
-	j = findInsert(addnumber);
-	for (i = length; i > j+1; i--)
-		iSort[i] = iSort[i-1];
-	iSort[j+1] = length;
-	++length;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	r e m o v e N u m b e r
- */
-returnValue Indexlist::removeNumber( int removenumber )
-{
-	int i;
-	int idx = findInsert( removenumber );
-	int iSidx = iSort[idx];
-
-	/* nothing to be done if number is not contained in index set */
-	if ( number[iSidx] != removenumber )
-		return SUCCESSFUL_RETURN;
-
-	/* update sorted indices iSort first */
-	for (i = 0; i < length; i++)
-		if (iSort[i] > iSidx) iSort[i]--;
-	for (i = idx+1; i < length; i++)
-		iSort[i-1] = iSort[i];
-
-	/* remove from numbers list */
-	for( i=iSidx; i<length-1; ++i )
-		number[i] = number[i+1];
-	number[length-1] = -1;
-
-	--length;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s w a p N u m b e r s
- */
-returnValue Indexlist::swapNumbers( int number1, int number2 )
-{
-	int index1 = findInsert( number1 );
-	int index2 = findInsert( number2 );
-
-	/* consistency check */
-	if ( ( number[iSort[index1]] != number1 ) || ( number[iSort[index2]] != number2 ) )
-		return THROWERROR( RET_INDEXLIST_CORRUPTED );
-
-	int tmp;
-	/* swap numbers */
-	tmp = number[iSort[index1]];
-	number[iSort[index1]] = number[iSort[index2]];
-	number[iSort[index2]] = tmp;
-	/* swap sorting indices */
-	tmp = iSort[index1];
-	iSort[index1] = iSort[index2];
-	iSort[index2] = tmp;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-/*
- *	c l e a r
- */
-returnValue Indexlist::clear( )
-{
-	if ( iSort != 0 )
-	{
-		delete[] iSort;
-		iSort = 0;
-	}
-
-	if ( number != 0 )
-	{
-		delete[] number;
-		number = 0;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	c o p y
- */
-returnValue Indexlist::copy(	const Indexlist& rhs
-								)
-{
-	int i;
-
-	length = rhs.length;
-	physicallength = rhs.physicallength;
-
-	if ( rhs.number != 0 )
-	{
-		number = new int[physicallength];
-		for( i=0; i<physicallength; ++i )
-			number[i] = rhs.number[i];
-		iSort = new int[physicallength];
-		for( i=0; i<physicallength; ++i )
-			iSort[i] = rhs.iSort[i];
-	}
-	else
-	{
-		number = 0;
-		iSort = 0;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-int Indexlist::findInsert(int i) const
-{
-	/* quick check if index can be appended */
-	if (length == 0 || i < number[iSort[0]]) return -1;
-	if (i >= number[iSort[length-1]]) return length-1;
-
-	/* otherwise, perform bisection search */
-	int fst = 0, lst = length-1, mid;
-
-	while (fst < lst - 1)
-	{
-		mid = (fst + lst) / 2;
-		if (i >= number[iSort[mid]]) fst = mid;
-		else lst = mid;
-	}
-
-	return fst;
-}
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/LAPACKReplacement.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/LAPACKReplacement.cpp
deleted file mode 100644
index e7bac2a37e3..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/LAPACKReplacement.cpp
+++ /dev/null
@@ -1,120 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file src/LAPACKReplacement.cpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *  LAPACK replacement routines.
- */
-
-
-#include <qpOASES/Utils.hpp>
-
-
-extern "C" void dpotrf_(	const char *uplo, const unsigned long *_n, double *a,
-							const unsigned long *_lda, long *info
-							)
-{
-	double sum;
-	long i, j, k;
-	long n = (long)(*_n);
-	long lda = (long)(*_lda);
-
-	for( i=0; i<n; ++i )
-	{
-		/* j == i */
-		sum = a[i + lda*i];
-
-		for( k=(i-1); k>=0; --k )
-			sum -= a[k+lda*i] * a[k+lda*i];
-
-		if ( sum > 0.0 )
-			a[i+lda*i] = REFER_NAMESPACE_QPOASES getSqrt( sum );
-		else
-		{
-			a[0] = sum; /* tunnel negative diagonal element to caller */
-			if (info != 0)
-				*info = (long)i+1;
-			return;
-		}
-
-		for( j=(i+1); j<n; ++j )
-		{
-			sum = a[j*lda + i];
-
-			for( k=(i-1); k>=0; --k )
-				sum -= a[k+lda*i] * a[k+lda*j];
-
-			a[i+lda*j] = sum / a[i+lda*i];
-		}
-	}
-	if (info != 0)
-		*info = 0;
-}
-
-
-extern "C" void spotrf_(	const char *uplo, const unsigned long *_n, float *a,
-							const unsigned long *_lda, long *info
-							)
-{
-	float sum;
-	long i, j, k;
-	long n = (long)(*_n);
-	long lda = (long)(*_lda);
-
-	for( i=0; i<n; ++i )
-	{
-		/* j == i */
-		sum = a[i + lda*i];
-
-		for( k=(i-1); k>=0; --k )
-			sum -= a[k+lda*i] * a[k+lda*i];
-
-		if ( sum > 0.0 )
-			a[i+lda*i] = (float)(REFER_NAMESPACE_QPOASES getSqrt( sum ));
-		else
-		{
-			a[0] = sum; /* tunnel negative diagonal element to caller */
-			if (info != 0)
-				*info = (long)i+1;
-			return;
-		}
-
-		for( j=(i+1); j<n; ++j )
-		{
-			sum = a[j*lda + i];
-
-			for( k=(i-1); k>=0; --k )
-				sum -= a[k+lda*i] * a[k+lda*j];
-
-			a[i+lda*j] = sum / a[i+lda*i];
-		}
-	}
-	if (info != 0)
-		*info = 0;
-}
-
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/Matrices.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/Matrices.cpp
deleted file mode 100644
index e962e383da1..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/Matrices.cpp
+++ /dev/null
@@ -1,1967 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file src/Matrices.cpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of the matrix classes.
- */
-
-
-#include <qpOASES/Matrices.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-
-DenseMatrix::~DenseMatrix()
-{
-	if ( needToFreeMemory( ) == BT_TRUE )
-		free( );
-}
-
-void DenseMatrix::free( )
-{
-	if (val != 0)
-		delete[] val;
-	val = 0;
-}
-
-Matrix *DenseMatrix::duplicate( ) const
-{
-	DenseMatrix *dupl = 0;
-
-	if ( needToFreeMemory( ) == BT_TRUE )
-	{
-		real_t* val_new = new real_t[nRows*nCols];
-		memcpy( val_new,val, ((unsigned int)(nRows*nCols))*sizeof(real_t) );
-		dupl = new DenseMatrix(nRows, nCols, nCols, val_new);
-		dupl->doFreeMemory( );
-	}
-	else
-	{
-		dupl = new DenseMatrix(nRows, nCols, nCols, val);
-	}
-
-	return dupl;
-}
-
-real_t DenseMatrix::diag(	int i
-							) const
-{
-	return val[i*(leaDim+1)];
-}
-
-BooleanType DenseMatrix::isDiag( ) const
-{
-	int i, j;
-
-	if (nRows != nCols)
-		return BT_FALSE;
-
-	for ( i=0; i<nRows; ++i )
-		for ( j=0; j<i; ++j )
-			if ( ( getAbs( val[i*leaDim+j] ) > EPS ) || ( getAbs( val[j*leaDim+i] ) > EPS ) )
-				return BT_FALSE;
-
-	return BT_TRUE;
-}
-
-
-real_t DenseMatrix::getNorm(	int type
-								) const
-{
-	return REFER_NAMESPACE_QPOASES getNorm( val,nCols*nRows,type ); 
-}
-
-
-real_t DenseMatrix::getRowNorm( int rNum, int type ) const
-{
-	return REFER_NAMESPACE_QPOASES getNorm( &(val[rNum*leaDim]),nCols,type ); 
-}
-
-returnValue DenseMatrix::getRow(int rNum, const Indexlist* const icols, real_t alpha, real_t *row) const
-{
-	int i;
-    if (icols != 0)
-    {
-	    if ( isEqual(alpha,1.0) == BT_TRUE )
-		    for (i = 0; i < icols->length; i++)
-			    row[i] = val[rNum*leaDim+icols->number[i]];
-	    else if ( isEqual(alpha,-1.0) == BT_TRUE )
-		    for (i = 0; i < icols->length; i++)
-			    row[i] = -val[rNum*leaDim+icols->number[i]];
-	    else
-		    for (i = 0; i < icols->length; i++)
-			    row[i] = alpha*val[rNum*leaDim+icols->number[i]];
-    }
-    else
-    {
-	    if ( isEqual(alpha,1.0) == BT_TRUE )
-		    for (i = 0; i < nCols; i++)
-			    row[i] = val[rNum*leaDim+i];
-	    else if ( isEqual(alpha,-1.0) == BT_TRUE )
-		    for (i = 0; i < nCols; i++)
-			    row[i] = -val[rNum*leaDim+i];
-	    else
-		    for (i = 0; i < nCols; i++)
-			    row[i] = alpha*val[rNum*leaDim+i];
-    }
-	return SUCCESSFUL_RETURN;
-}
-
-
-returnValue DenseMatrix::getCol(int cNum, const Indexlist* const irows, real_t alpha, real_t *col) const
-{
-	int i;
-
-	if ( isEqual(alpha,1.0) == BT_TRUE )
-		for (i = 0; i < irows->length; i++)
-			col[i] = val[irows->number[i]*leaDim+cNum];
-	else if ( isEqual(alpha,-1.0) == BT_TRUE )
-		for (i = 0; i < irows->length; i++)
-			col[i] = -val[irows->number[i]*leaDim+cNum];
-	else
-		for (i = 0; i < irows->length; i++)
-			col[i] = alpha*val[irows->number[i]*leaDim+cNum];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-returnValue DenseMatrix::times(	int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
-{
-	unsigned long _xN     = (unsigned long)xN;
-	unsigned long _nRows  = (unsigned long)nRows;
-	unsigned long _nCols  = (unsigned long)nCols;
-	unsigned long _leaDim = (unsigned long)getMax(1,nCols);
-	unsigned long _xLD    = (unsigned long)getMax(1,xLD);
-	unsigned long _yLD    = (unsigned long)getMax(1,yLD);
-
-	/* Call BLAS. Mind row major format! */
-	GEMM("TRANS", "NOTRANS", &_nRows, &_xN, &_nCols, &alpha, val, &_leaDim, x, &_xLD, &beta, y, &_yLD);
-	return SUCCESSFUL_RETURN;
-}
-
-returnValue DenseMatrix::transTimes( int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
-{
-	unsigned long _xN     = (unsigned long)xN;
-	unsigned long _nRows  = (unsigned long)nRows;
-	unsigned long _nCols  = (unsigned long)nCols;
-	unsigned long _leaDim = (unsigned long)getMax(1,nCols);
-	unsigned long _xLD    = (unsigned long)getMax(1,xLD);
-	unsigned long _yLD    = (unsigned long)getMax(1,yLD);
-
-	/* Call BLAS. Mind row major format! */
-	GEMM("NOTRANS", "NOTRANS", &_nCols, &_xN, &_nRows, &alpha, val, &_leaDim, x, &_xLD, &beta, y, &_yLD);
-	return SUCCESSFUL_RETURN;
-}
-
-returnValue DenseMatrix::times(	const Indexlist* const irows, const Indexlist* const icols,
-								int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD,
-								BooleanType yCompr) const
-{
-	int i, j, k, row, col, iy, irA;
-
-	if (yCompr == BT_TRUE)
-	{
-		if ( isZero(beta) == BT_TRUE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[j+k*yLD] = 0.0;
-		else if ( isEqual(beta,-1.0) == BT_TRUE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[j+k*yLD] = -y[j+k*yLD];
-		else if ( isEqual(beta,1.0) == BT_FALSE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[j+k*yLD] *= beta;
-
-		if (icols == 0)
-			if ( isEqual(alpha,1.0) == BT_TRUE )
-				for (k = 0; k < xN; k++)
-					for (j = 0; j < irows->length; j++)
-					{
-						row = irows->iSort[j];
-						iy = row + k * yLD;
-						irA = irows->number[row] * leaDim;
-						for (i = 0; i < nCols; i++)
-							y[iy] += val[irA+i] * x[k*xLD+i];
-					}
-			else if ( isEqual(alpha,-1.0) == BT_TRUE )
-				for (k = 0; k < xN; k++)
-					for (j = 0; j < irows->length; j++)
-					{
-						row = irows->iSort[j];
-						iy = row + k * yLD;
-						irA = irows->number[row] * leaDim;
-						for (i = 0; i < nCols; i++)
-							y[iy] -= val[irA+i] * x[k*xLD+i];
-					}
-			else
-				for (k = 0; k < xN; k++)
-					for (j = 0; j < irows->length; j++)
-					{
-						row = irows->iSort[j];
-						iy = row + k * yLD;
-						irA = irows->number[row] * leaDim;
-						for (i = 0; i < nCols; i++)
-							y[iy] += alpha * val[irA+i] * x[k*xLD+i];
-					}
-		else /* icols != 0 */
-			if ( isEqual(alpha,1.0) == BT_TRUE )
-				for (k = 0; k < xN; k++)
-					for (j = 0; j < irows->length; j++)
-					{
-						row = irows->iSort[j];
-						iy = row + k * yLD;
-						irA = irows->number[row] * leaDim;
-						for (i = 0; i < icols->length; i++)
-						{
-							col = icols->iSort[i];
-							y[iy] += val[irA+icols->number[col]] * x[k*xLD+col];
-						}
-					}
-			else if ( isEqual(alpha,-1.0) == BT_TRUE )
-				for (k = 0; k < xN; k++)
-					for (j = 0; j < irows->length; j++)
-					{
-						row = irows->iSort[j];
-						iy = row + k * yLD;
-						irA = irows->number[row] * leaDim;
-						for (i = 0; i < icols->length; i++)
-						{
-							col = icols->iSort[i];
-							y[iy] -= val[irA+icols->number[col]] * x[k*xLD+col];
-						}
-					}
-			else
-				for (k = 0; k < xN; k++)
-					for (j = 0; j < irows->length; j++)
-					{
-						row = irows->iSort[j];
-						iy = row + k * yLD;
-						irA = irows->number[row] * leaDim;
-						for (i = 0; i < icols->length; i++)
-						{
-							col = icols->iSort[i];
-							y[iy] += alpha * val[irA+icols->number[col]] * x[k*xLD+col];
-						}
-					}
-	}
-	else /* y not compressed */
-	{
-		if ( isZero(beta) == BT_TRUE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[irows->number[j]+k*yLD] = 0.0;
-		else if ( isEqual(beta,-1.0) == BT_TRUE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[irows->number[j]+k*yLD] = -y[j+k*yLD];
-		else if ( isEqual(beta,1.0) == BT_FALSE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[irows->number[j]+k*yLD] *= beta;
-
-		if (icols == 0)
-			if ( isEqual(alpha,1.0) == BT_TRUE )
-				for (k = 0; k < xN; k++)
-					for (j = 0; j < irows->length; j++)
-					{
-						row = irows->number[irows->iSort[j]];
-						iy = row + k * yLD;
-						irA = row * leaDim;
-						for (i = 0; i < nCols; i++)
-							y[iy] += val[irA+i] * x[k*xLD+i];
-					}
-			else if ( isEqual(alpha,-1.0) == BT_TRUE )
-				for (k = 0; k < xN; k++)
-					for (j = 0; j < irows->length; j++)
-					{
-						row = irows->number[irows->iSort[j]];
-						iy = row + k * yLD;
-						irA = row * leaDim;
-						for (i = 0; i < nCols; i++)
-							y[iy] -= val[irA+i] * x[k*xLD+i];
-					}
-			else
-				for (k = 0; k < xN; k++)
-					for (j = 0; j < irows->length; j++)
-					{
-						row = irows->number[irows->iSort[j]];
-						iy = row + k * yLD;
-						irA = row * leaDim;
-						for (i = 0; i < nCols; i++)
-							y[iy] += alpha * val[irA+i] * x[k*xLD+i];
-					}
-		else /* icols != 0 */
-			if ( isEqual(alpha,1.0) == BT_TRUE )
-				for (k = 0; k < xN; k++)
-					for (j = 0; j < irows->length; j++)
-					{
-						row = irows->number[irows->iSort[j]];
-						iy = row + k * yLD;
-						irA = row * leaDim;
-						for (i = 0; i < icols->length; i++)
-						{
-							col = icols->iSort[i];
-							y[iy] += val[irA+icols->number[col]] * x[k*xLD+col];
-						}
-					}
-			else if ( isEqual(alpha,-1.0) == BT_TRUE )
-				for (k = 0; k < xN; k++)
-					for (j = 0; j < irows->length; j++)
-					{
-						row = irows->number[irows->iSort[j]];
-						iy = row + k * yLD;
-						irA = row * leaDim;
-						for (i = 0; i < icols->length; i++)
-						{
-							col = icols->iSort[i];
-							y[iy] -= val[irA+icols->number[col]] * x[k*xLD+col];
-						}
-					}
-			else
-				for (k = 0; k < xN; k++)
-					for (j = 0; j < irows->length; j++)
-					{
-						row = irows->number[irows->iSort[j]];
-						iy = row + k * yLD;
-						irA = row * leaDim;
-						for (i = 0; i < icols->length; i++)
-						{
-							col = icols->iSort[i];
-							y[iy] += alpha * val[irA+icols->number[col]] * x[k*xLD+col];
-						}
-					}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-returnValue DenseMatrix::transTimes(	const Indexlist* const irows, const Indexlist* const icols,
-										int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
-{
-	int i, j, k, row, col;
-
-	if ( isZero(beta) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < icols->length; j++)
-				y[j+k*yLD] = 0.0;
-	else if ( isEqual(beta,-1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < icols->length; j++)
-				y[j+k*yLD] = -y[j+k*yLD];
-	else if ( isEqual(beta,1.0) == BT_FALSE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < icols->length; j++)
-				y[j+k*yLD] *= beta;
-
-	if ( isEqual(alpha,1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < irows->length; j++)
-			{
-				row = irows->iSort[j];
-				for (i = 0; i < icols->length; i++)
-				{
-					col = icols->iSort[i];
-					y[col+k*yLD] += val[irows->number[row]*leaDim+icols->number[col]] * x[row+k*xLD];
-				}
-			}
-	else if ( isEqual(alpha,-1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < irows->length; j++)
-			{
-				row = irows->iSort[j];
-				for (i = 0; i < icols->length; i++)
-				{
-					col = icols->iSort[i];
-					y[col+k*yLD] -= val[irows->number[row]*leaDim+icols->number[col]] * x[row+k*xLD];
-				}
-			}
-	else
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < irows->length; j++)
-			{
-				row = irows->iSort[j];
-				for (i = 0; i < icols->length; i++)
-				{
-					col = icols->iSort[i];
-					y[col+k*yLD] += alpha * val[irows->number[row]*leaDim+icols->number[col]] * x[row+k*xLD];
-				}
-			}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-returnValue DenseMatrix::addToDiag( real_t alpha )
-{
-	int i;
-	for (i = 0; i < nRows && i < nCols; i++)
-		val[i*(leaDim+1)] += alpha;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-real_t* DenseMatrix::full() const
-{
-	real_t* v = new real_t[nRows*nCols];
-	memcpy( v,val, ((unsigned int)(nRows*nCols))*sizeof(real_t) );
-	return v;
-}
-
-
-returnValue DenseMatrix::print( const char* name ) const
-{
-	return REFER_NAMESPACE_QPOASES print( val,nRows,nCols,name );
-}
-
-
-
-Matrix *SymDenseMat::duplicate( ) const
-{
-	return duplicateSym();
-}
-
-
-SymmetricMatrix *SymDenseMat::duplicateSym( ) const
-{
-	/* "same" as duplicate() in DenseMatrix */
-	SymDenseMat *dupl = 0;
-
-	if ( needToFreeMemory( ) == BT_TRUE )
-	{
-		real_t* val_new = new real_t[nRows*nCols];
-		memcpy( val_new,val, ((unsigned int)(nRows*nCols))*sizeof(real_t) );
-		dupl = new SymDenseMat(nRows, nCols, nCols, val_new);
-		dupl->doFreeMemory( );
-	}
-	else
-	{
-		dupl = new SymDenseMat(nRows, nCols, nCols, val);
-	}
-
-	return dupl;
-}
-
-
-returnValue SymDenseMat::bilinear(	const Indexlist* const icols,
-									int xN, const real_t *x, int xLD, real_t *y, int yLD) const
-{
-	int ii, jj, kk, col;
-	int i,j,k,irA;
-
-	for (ii = 0; ii < xN; ii++)
-		for (jj = 0; jj < xN; jj++)
-			y[ii*yLD+jj] = 0.0;
-
-	real_t *Ax = new real_t[icols->length * xN];
-
-	for (i=0;i<icols->length * xN;++i)
-		Ax[i]=0.0;
-
-	/* exploit symmetry of A ! */
-	for (j = 0; j < icols->length; j++) {
-		irA = icols->number[j] * leaDim;
-		for (i = 0; i < icols->length; i++)
-		{
-			real_t h = val[irA+icols->number[i]];
-			for (k = 0; k < xN; k++)
-				Ax[j + k * icols->length] += h * x[k*xLD+icols->number[i]];
-		}
-	}
-
-	for (ii = 0; ii < icols->length; ++ii) {
-		col = icols->number[ii];
-		for (jj = 0; jj < xN; ++jj) {
-			for (kk = 0; kk < xN; ++kk) {
-				y[kk + jj*yLD] += x[col + jj*xLD] * Ax[ii + kk*icols->length];
-			}
-		}
-	}
-	delete[] Ax;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-SparseMatrix::SparseMatrix() : nRows(0), nCols(0), ir(0), jc(0), jd(0), val(0) {}
-
-
-SparseMatrix::SparseMatrix( int nr, int nc, sparse_int_t *r, sparse_int_t *c, real_t *v )
-								: nRows(nr), nCols(nc), ir(r), jc(c), jd(0), val(v) { doNotFreeMemory(); }
-
-
-SparseMatrix::SparseMatrix( int nr, int nc, int ld, const real_t * const v ) 
-								: nRows(nr), nCols(nc), jd(0)
-{
-	int i, j, nnz;
-
-	jc = new sparse_int_t[nc+1];
-	ir = new sparse_int_t[nr*nc];
-	val = new real_t[nr*nc];
-
-	nnz = 0;
-	for (j = 0; j < nCols; j++)
-	{
-		jc[j] = nnz;
-		for (i = 0; i < nRows; i++)
-			if ( ( isZero( v[i*ld+j],0.0 ) == BT_FALSE ) || ( i == j ) ) /* also include zero diagonal elemets! */
-			{
-				ir[nnz] = i;
-				val[nnz++] = v[i*ld+j];
-			}
-	}
-	jc[nCols] = nnz;
-
-	doFreeMemory( );
-}
-
-
-SparseMatrix::~SparseMatrix()
-{
-	if (jd != 0)
-	{
-		delete[] jd;
-		jd = 0;
-	}
-
-	if ( needToFreeMemory() == BT_TRUE )
-		free( );
-}
-
-
-void SparseMatrix::free( )
-{
-	if (ir != 0) delete[] ir;
-	ir = 0;
-	if (jc != 0) delete[] jc;
-	jc = 0;
-	if (val != 0) delete[] val;
-	val = 0;
-
-	doNotFreeMemory( );
-}
-
-Matrix *SparseMatrix::duplicate() const
-{
-	long i, length = jc[nCols];
-	SparseMatrix *dupl = new SparseMatrix;
-
-	dupl->nRows = nRows;
-	dupl->nCols = nCols;
-	dupl->ir = new sparse_int_t[length];
-	dupl->jc = new sparse_int_t[nCols+1];
-	dupl->val = new real_t[length];
-
-	for (i = 0; i < length; i++) dupl->ir[i] = ir[i];
-	for (i = 0; i <= nCols; i++) dupl->jc[i] = jc[i];
-	for (i = 0; i < length; i++) dupl->val[i] = val[i];
-
-	if ( jd != 0 )
-	{
-		dupl->jd = new sparse_int_t[nCols];
-		for (i = 0; i < nCols; i++) dupl->jd[i] = jd[i];
-	}
-	else
-		dupl->jd = 0;
-	
-	dupl->doFreeMemory( );
-
-	return dupl;
-}
-
-
-
-real_t SparseMatrix::diag(int i) const
-{
-	if ( jd == 0 )
-	{
-		THROWERROR( RET_DIAGONAL_NOT_INITIALISED );
-		return INFTY;
-	}
-
-	int entry = jd[i];
-	return (entry < jc[i+1] && ir[entry] == i) ? val[entry] : 0.0;
-}
-
-
-BooleanType SparseMatrix::isDiag() const
-{
-	int j;
-	
-	if ( nCols != nRows )
-		return BT_FALSE;
-	
-	for (j = 0; j < nCols; ++j)
-	{
-		if ( jc[j+1] > jc[j]+1 )
-			return BT_FALSE;
-		
-		if ( ( jc[j+1] == jc[j]+1 ) && ( ir[jc[j]] != j ) )
-			return BT_FALSE;
-	}
-	
-	return BT_TRUE;
-}
-
-
-
-real_t SparseMatrix::getNorm(	int type
-								) const
-{
-	long length = jc[nCols];
-	return REFER_NAMESPACE_QPOASES getNorm( val,length,type );
-}
-
-
-real_t SparseMatrix::getRowNorm( int rNum, int type ) const
-{
-    int i,j;
-    real_t norm = 0.0;
-
-	switch( type )
-	{
-		case 2:
-			for ( j=0; j < nCols; ++j ) {
-				for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++) {};
-				norm += (i < jc[j+1] && ir[i] == rNum) ? val[i]*val[i] : 0.0;
-			}
-			return getSqrt(norm);
-
-		case 1:
-			for ( j=0; j < nCols; ++j ) {
-				for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++) {};
-				norm += (i < jc[j+1] && ir[i] == rNum) ? REFER_NAMESPACE_QPOASES getAbs( val[i] ) : 0.0;
-			}
-			return norm;
-
-		default:
-			THROWERROR( RET_INVALID_ARGUMENTS );
-			return -INFTY;
-	}
-}
-
-
-returnValue SparseMatrix::getRow(int rNum, const Indexlist* const icols, real_t alpha, real_t *row) const
-{
-	long i, j, k;
-
-    if (icols != 0)
-    {
-	    if ( isEqual(alpha,1.0) == BT_TRUE )
-		    for (k = 0; k < icols->length; k++)
-		    {
-			    j = icols->number[icols->iSort[k]];
-			    for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
-			    row[icols->iSort[k]] = (i < jc[j+1] && ir[i] == rNum) ? val[i] : 0.0;
-		    }
-	    else if ( isEqual(alpha,-1.0) == BT_TRUE )
-		    for (k = 0; k < icols->length; k++)
-		    {
-			    j = icols->number[icols->iSort[k]];
-			    for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
-			    row[icols->iSort[k]] = (i < jc[j+1] && ir[i] == rNum) ? -val[i] : 0.0;
-		    }
-	    else
-		    for (k = 0; k < icols->length; k++)
-		    {
-			    j = icols->number[icols->iSort[k]];
-			    for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
-			    row[icols->iSort[k]] = (i < jc[j+1] && ir[i] == rNum) ? alpha*val[i] : 0.0;
-		    }
-    }
-    else
-    {
-	    if ( isEqual(alpha,1.0) == BT_TRUE )
-		    for (j = 0; j < nCols; j++)
-		    {
-			    for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
-			    row[j] = (i < jc[j+1] && ir[i] == rNum) ? val[i] : 0.0;
-		    }
-	    else if ( isEqual(alpha,-1.0) == BT_TRUE )
-		    for (j = 0; j < icols->length; j++)
-		    {
-			    for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
-			    row[j] = (i < jc[j+1] && ir[i] == rNum) ? -val[i] : 0.0;
-		    }
-	    else
-		    for (j = 0; j < icols->length; j++)
-		    {
-			    for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
-			    row[j] = (i < jc[j+1] && ir[i] == rNum) ? alpha*val[i] : 0.0;
-		    }
-    }
-	return SUCCESSFUL_RETURN;
-}
-
-
-returnValue SparseMatrix::getCol(int cNum, const Indexlist* const irows, real_t alpha, real_t *col) const
-{
-	long i, j;
-
-	i = jc[cNum];
-	j = 0;
-	if ( isEqual(alpha,1.0) == BT_TRUE )
-		while (i < jc[cNum+1] && j < irows->length)
-			if (ir[i] == irows->number[irows->iSort[j]])
-				col[irows->iSort[j++]] = val[i++];
-			else if (ir[i] > irows->number[irows->iSort[j]])
-				col[irows->iSort[j++]] = 0.0;
-			else
-				i++;
-	else if ( isEqual(alpha,-1.0) == BT_TRUE )
-		while (i < jc[cNum+1] && j < irows->length)
-			if (ir[i] == irows->number[irows->iSort[j]])
-				col[irows->iSort[j++]] = -val[i++];
-			else if (ir[i] > irows->number[irows->iSort[j]])
-				col[irows->iSort[j++]] = 0.0;
-			else
-				i++;
-	else
-		while (i < jc[cNum+1] && j < irows->length)
-			if (ir[i] == irows->number[irows->iSort[j]])
-				col[irows->iSort[j++]] = alpha * val[i++];
-			else if (ir[i] > irows->number[irows->iSort[j]])
-				col[irows->iSort[j++]] = 0.0;
-			else
-				i++;
-
-	/* fill in remaining zeros */
-	while (j < irows->length)
-		col[irows->iSort[j++]] = 0.0;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-returnValue SparseMatrix::times(int xN, real_t alpha, const real_t *x, int xLD,
-		real_t beta, real_t *y, int yLD) const
-{
-	long i, j, k;
-
-	if ( isZero(beta) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nRows; j++)
-				y[j+k*yLD] = 0.0;
-	else if ( isEqual(beta,-1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nRows; j++)
-				y[j+k*yLD] = -y[j+k*yLD];
-	else if ( isEqual(beta,1.0) == BT_FALSE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nRows; j++)
-				y[j+k*yLD] *= beta;
-
-	if ( isEqual(alpha,1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nCols; j++)
-				for (i = jc[j]; i < jc[j+1]; i++)
-					y[ir[i]+k*yLD] += val[i] * x[j+k*xLD];
-	else if ( isEqual(alpha,-1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nCols; j++)
-				for (i = jc[j]; i < jc[j+1]; i++)
-					y[ir[i]+k*yLD] -= val[i] * x[j+k*xLD];
-	else
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nCols; j++)
-				for (i = jc[j]; i < jc[j+1]; i++)
-					y[ir[i]+k*yLD] += alpha * val[i] * x[j+k*xLD];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-returnValue SparseMatrix::transTimes(int xN, real_t alpha, const real_t *x, int xLD,
-		real_t beta, real_t *y, int yLD) const
-{
-	long i, j, k;
-
-	if ( isZero(beta) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nCols; j++)
-				y[j+k*yLD] = 0.0;
-	else if ( isEqual(beta,-1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nCols; j++)
-				y[j+k*yLD] = -y[j+k*yLD];
-	else if ( isEqual(beta,1.0) == BT_FALSE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nCols; j++)
-				y[j+k*yLD] *= beta;
-
-	if ( isEqual(alpha,1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nCols; j++)
-				for (i = jc[j]; i < jc[j+1]; i++)
-					y[j+k*yLD] += val[i] * x[ir[i]+k*xLD];
-	else if ( isEqual(alpha,-1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nCols; j++)
-				for (i = jc[j]; i < jc[j+1]; i++)
-					y[j+k*yLD] -= val[i] * x[ir[i]+k*xLD];
-	else
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nCols; j++)
-				for (i = jc[j]; i < jc[j+1]; i++)
-					y[j+k*yLD] += alpha * val[i] * x[ir[i]+k*xLD];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-returnValue SparseMatrix::times(const Indexlist* const irows, const Indexlist* const icols,
-		int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD,
-		BooleanType yCompr) const
-{
-	long i, j, k, l, col;
-
-	if (yCompr == BT_TRUE)
-	{
-		if ( isZero(beta) == BT_TRUE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[j+k*yLD] = 0.0;
-		else if ( isEqual(beta,-1.0) == BT_TRUE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[j+k*yLD] = -y[j+k*yLD];
-		else if ( isEqual(beta,1.0) == BT_FALSE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[j+k*yLD] *= beta;
-
-		if (icols == 0)
-			if ( isEqual(alpha,1.0) == BT_TRUE )
-				for (l = 0; l < nCols; l++)
-				{
-					i = jc[l];
-					j = 0;
-					while (i < jc[l+1] && j < irows->length)
-						if (ir[i] == irows->number[irows->iSort[j]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+irows->iSort[j]] += val[i] * x[k*xLD+l];
-							i++, j++;
-						}
-						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
-						else i++;
-				}
-			else if ( isEqual(alpha,-1.0) == BT_TRUE )
-				for (l = 0; l < nCols; l++)
-				{
-					i = jc[l];
-					j = 0;
-					while (i < jc[l+1] && j < irows->length)
-						if (ir[i] == irows->number[irows->iSort[j]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+irows->iSort[j]] -= val[i] * x[k*xLD+l];
-							i++, j++;
-						}
-						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
-						else i++;
-				}
-			else
-				for (l = 0; l < nCols; l++)
-				{
-					i = jc[l];
-					j = 0;
-					while (i < jc[l+1] && j < irows->length)
-						if (ir[i] == irows->number[irows->iSort[j]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+irows->iSort[j]] += alpha * val[i] * x[k*xLD+l];
-							i++, j++;
-						}
-						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
-						else i++;
-				}
-		else /* icols != 0 */
-			if ( isEqual(alpha,1.0) == BT_TRUE )
-				for (l = 0; l < icols->length; l++)
-				{
-					col = icols->iSort[l];
-					i = jc[icols->number[col]];
-					j = 0;
-					while (i < jc[icols->number[col]+1] && j < irows->length)
-						if (ir[i] == irows->number[irows->iSort[j]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+irows->iSort[j]] += val[i] * x[k*xLD+col];
-							i++, j++;
-						}
-						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
-						else i++;
-				}
-			else if ( isEqual(alpha,-1.0) == BT_TRUE )
-				for (l = 0; l < icols->length; l++)
-				{
-					col = icols->iSort[l];
-					i = jc[icols->number[col]];
-					j = 0;
-					while (i < jc[icols->number[col]+1] && j < irows->length)
-						if (ir[i] == irows->number[irows->iSort[j]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+irows->iSort[j]] -= val[i] * x[k*xLD+col];
-							i++, j++;
-						}
-						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
-						else i++;
-				}
-			else
-				for (l = 0; l < icols->length; l++)
-				{
-					col = icols->iSort[l];
-					i = jc[icols->number[col]];
-					j = 0;
-					while (i < jc[icols->number[col]+1] && j < irows->length)
-						if (ir[i] == irows->number[irows->iSort[j]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+irows->iSort[j]] += alpha * val[i] * x[k*xLD+col];
-							i++, j++;
-						}
-						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
-						else i++;
-				}
-	}
-	else /* y not compressed */
-	{
-		if ( isZero(beta) == BT_TRUE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[irows->number[j]+k*yLD] = 0.0;
-		else if ( isEqual(beta,-1.0) == BT_TRUE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[irows->number[j]+k*yLD] = -y[j+k*yLD];
-		else if ( isEqual(beta,1.0) == BT_FALSE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[irows->number[j]+k*yLD] *= beta;
-
-		if (icols == 0)
-			if ( isEqual(alpha,1.0) == BT_TRUE )
-				for (l = 0; l < nCols; l++)
-				{
-					i = jc[l];
-					j = 0;
-					while (i < jc[l+1] && j < irows->length)
-						if (ir[i] == irows->number[irows->iSort[j]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+irows->number[irows->iSort[j]]] += val[i] * x[k*xLD+l];
-							i++, j++;
-						}
-						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
-						else i++;
-				}
-			else if ( isEqual(alpha,-1.0) == BT_TRUE )
-				for (l = 0; l < nCols; l++)
-				{
-					i = jc[l];
-					j = 0;
-					while (i < jc[l+1] && j < irows->length)
-						if (ir[i] == irows->number[irows->iSort[j]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+irows->number[irows->iSort[j]]] -= val[i] * x[k*xLD+l];
-							i++, j++;
-						}
-						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
-						else i++;
-				}
-			else
-				for (l = 0; l < nCols; l++)
-				{
-					i = jc[l];
-					j = 0;
-					while (i < jc[l+1] && j < irows->length)
-						if (ir[i] == irows->number[irows->iSort[j]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+irows->number[irows->iSort[j]]] += alpha * val[i] * x[k*xLD+l];
-							i++, j++;
-						}
-						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
-						else i++;
-				}
-		else /* icols != 0 */
-			if ( isEqual(alpha,1.0) == BT_TRUE )
-				for (l = 0; l < icols->length; l++)
-				{
-					col = icols->iSort[l];
-					i = jc[icols->number[col]];
-					j = 0;
-					while (i < jc[icols->number[col]+1] && j < irows->length)
-						if (ir[i] == irows->number[irows->iSort[j]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+irows->number[irows->iSort[j]]] += val[i] * x[k*xLD+col];
-							i++, j++;
-						}
-						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
-						else i++;
-				}
-			else if ( isEqual(alpha,-1.0) == BT_TRUE )
-				for (l = 0; l < icols->length; l++)
-				{
-					col = icols->iSort[l];
-					i = jc[icols->number[col]];
-					j = 0;
-					while (i < jc[icols->number[col]+1] && j < irows->length)
-						if (ir[i] == irows->number[irows->iSort[j]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+irows->number[irows->iSort[j]]] -= val[i] * x[k*xLD+col];
-							i++, j++;
-						}
-						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
-						else i++;
-				}
-			else
-				for (l = 0; l < icols->length; l++)
-				{
-					col = icols->iSort[l];
-					i = jc[icols->number[col]];
-					j = 0;
-					while (i < jc[icols->number[col]+1] && j < irows->length)
-						if (ir[i] == irows->number[irows->iSort[j]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+irows->number[irows->iSort[j]]] += alpha * val[i] * x[k*xLD+col];
-							i++, j++;
-						}
-						else if (ir[i] > irows->number[irows->iSort[j]]) j++;
-						else i++;
-				}
-	}
-	return SUCCESSFUL_RETURN;
-}
-
-
-returnValue SparseMatrix::transTimes(const Indexlist* const irows, const Indexlist* const icols,
-		int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
-{
-	long i, j, k, l, col;
-
-	if ( isZero(beta) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < icols->length; j++)
-				y[j+k*yLD] = 0.0;
-	else if ( isEqual(beta,-1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < icols->length; j++)
-				y[j+k*yLD] = -y[j+k*yLD];
-	else if ( isEqual(beta,1.0) == BT_FALSE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < icols->length; j++)
-				y[j+k*yLD] *= beta;
-
-	if ( isEqual(alpha,1.0) == BT_TRUE )
-		for (l = 0; l < icols->length; l++)
-		{
-			col = icols->iSort[l];
-			i = jc[icols->number[col]];
-			j = 0;
-			while (i < jc[icols->number[col]+1] && j < irows->length)
-				if (ir[i] == irows->number[irows->iSort[j]])
-				{
-					for (k = 0; k < xN; k++)
-						y[k*yLD+col] += val[i] * x[k*xLD+irows->iSort[j]];
-					i++, j++;
-				}
-				else if (ir[i] > irows->number[irows->iSort[j]]) j++;
-				else i++;
-		}
-	else if ( isEqual(alpha,-1.0) == BT_TRUE )
-		for (l = 0; l < icols->length; l++)
-		{
-			col = icols->iSort[l];
-			i = jc[icols->number[col]];
-			j = 0;
-			while (i < jc[icols->number[col]+1] && j < irows->length)
-				if (ir[i] == irows->number[irows->iSort[j]])
-				{
-					for (k = 0; k < xN; k++)
-						y[k*yLD+col] -= val[i] * x[k*xLD+irows->iSort[j]];
-					i++, j++;
-				}
-				else if (ir[i] > irows->number[irows->iSort[j]]) j++;
-				else i++;
-		}
-	else
-		for (l = 0; l < icols->length; l++)
-		{
-			col = icols->iSort[l];
-			i = jc[icols->number[col]];
-			j = 0;
-			while (i < jc[icols->number[col]+1] && j < irows->length)
-				if (ir[i] == irows->number[irows->iSort[j]])
-				{
-					for (k = 0; k < xN; k++)
-						y[k*yLD+col] += alpha * val[i] * x[k*xLD+irows->iSort[j]];
-					i++, j++;
-				}
-				else if (ir[i] > irows->number[irows->iSort[j]]) j++;
-				else i++;
-		}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-returnValue SparseMatrix::addToDiag(real_t alpha)
-{
-	long i;
-	
-	if ( jd == 0 )
-		return THROWERROR( RET_DIAGONAL_NOT_INITIALISED );
-
-	if ( isZero( alpha ) == BT_FALSE )
-		for (i = 0; i < nRows && i < nCols; i++)
-			if (ir[jd[i]] == i) val[jd[i]] += alpha;
-			else return RET_NO_DIAGONAL_AVAILABLE;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-sparse_int_t *SparseMatrix::createDiagInfo()
-{
-	long i, j;
-
-	if (jd == 0) {
-		jd = new sparse_int_t[nCols];
-
-		for (j = 0; j < nCols; j++)
-		{
-			for (i = jc[j]; i < jc[j+1] && ir[i] < j; i++);
-			jd[j] = i;
-		}
-	}
-
-	return jd;
-}
-
-
-
-real_t *SparseMatrix::full() const
-{
-	long i, j;
-	real_t *v = new real_t[nRows*nCols];
-
-	for (i = 0; i < nCols*nRows; i++)
-		v[i] = 0.0;
-
-	for (j = 0; j < nCols; j++)
-		for (i = jc[j]; i < jc[j+1]; i++)
-			v[ir[i] * nCols + j] = val[i];
-
-	return v;
-}
-
-
-returnValue SparseMatrix::print( const char* name ) const
-{
-	real_t* tmp = this->full();
-	returnValue retVal = REFER_NAMESPACE_QPOASES print( tmp,nRows,nCols,name );
-	delete[] tmp;
-
-	return retVal;
-}
-
-
-
-SparseMatrixRow::SparseMatrixRow() : nRows(0), nCols(0), jr(0), ic(0), jd(0), val(0) {}
-
-
-SparseMatrixRow::SparseMatrixRow(int nr, int nc, sparse_int_t *r, sparse_int_t *c, real_t *v)
-	: nRows(nr), nCols(nc), jr(r), ic(c), jd(0), val(v) { doNotFreeMemory(); }
-
-
-SparseMatrixRow::SparseMatrixRow(int nr, int nc, int ld, const real_t * const v) : nRows(nr), nCols(nc), jd(0)
-{
-	int i, j, nnz;
-
-	jr = new sparse_int_t[nr+1];
-	ic = new sparse_int_t[nr*nc];
-	val = new real_t[nr*nc];
-
-	nnz = 0;
-	for (j = 0; j < nRows; j++)
-	{
-		jr[j] = nnz;
-		for (i = 0; i < nCols; i++)
-			if ( ( isZero( v[j*ld+i],0.0 ) == BT_FALSE ) || ( j == i ) )
-			{
-				ic[nnz] = i;
-				val[nnz++] = v[j*ld+i];
-			}
-	}
-	jr[nRows] = nnz;
-
-	doFreeMemory( );
-}
-
-
-SparseMatrixRow::~SparseMatrixRow()
-{
-	if (jd != 0)
-	{
-		delete[] jd;
-		jd = 0;
-	}
-
-	if ( needToFreeMemory() == BT_TRUE )
-		free( );
-}
-
-
-void SparseMatrixRow::free( )
-{
-	if (jr != 0) delete[] jr;
-	jr = 0;
-	if (ic != 0) delete[] ic;
-	ic = 0;
-	if (val != 0) delete[] val;
-	val = 0;
-
-	doNotFreeMemory( );
-}
-
-
-Matrix *SparseMatrixRow::duplicate() const
-{
-	long i, length = jr[nRows];
-	SparseMatrixRow *dupl = new SparseMatrixRow;
-
-	dupl->nRows = nRows;
-	dupl->nCols = nCols;
-	dupl->jr = new sparse_int_t[nRows+1];
-	dupl->ic = new sparse_int_t[length];
-	dupl->val = new real_t[length];
-
-	for (i = 0; i < length; i++) dupl->jr[i] = jr[i];
-	for (i = 0; i <= nCols; i++) dupl->ic[i] = ic[i];
-	for (i = 0; i < length; i++) dupl->val[i] = val[i];
-
-	if ( jd != 0 )
-	{
-		dupl->jd = new sparse_int_t[nRows];
-		for (i = 0; i < nCols; i++) dupl->jd[i] = jd[i];
-	}
-	else
-		dupl->jd = 0;
-
-	dupl->doFreeMemory( );
-
-	return dupl;
-}
-
-
-
-real_t SparseMatrixRow::diag(int i) const
-{
-	if ( jd == 0 )
-	{
-		THROWERROR( RET_DIAGONAL_NOT_INITIALISED );
-		return INFTY;
-	}
-
-	int entry = jd[i];
-	return (entry < jr[i+1] && ic[entry] == i) ? val[entry] : 0.0;
-}
-
-
-BooleanType SparseMatrixRow::isDiag() const
-{
-	int i;
-	
-	if ( nCols != nRows )
-		return BT_FALSE;
-	
-	for (i = 0; i < nRows; ++i)
-	{
-		if ( jr[i+1] > jr[i]+1 )
-			return BT_FALSE;
-		
-		if ( ( jr[i+1] == jr[i]+1 ) && ( ic[jr[i]] != i ) )
-			return BT_FALSE;
-	}
-	
-	return BT_TRUE;
-}
-
-
-
-real_t SparseMatrixRow::getNorm(	int type
-									) const
-{
-	long length = jr[nRows];
-	return REFER_NAMESPACE_QPOASES getNorm( val,length,type );
-
-}
-
-
-real_t SparseMatrixRow::getRowNorm( int rNum, int type ) const
-{
-	long length = jr[rNum+1] - jr[rNum];
-	return REFER_NAMESPACE_QPOASES getNorm( &(val[jr[rNum]]),length,type );
-}
-
-
-
-returnValue SparseMatrixRow::getRow(int rNum, const Indexlist* const icols, real_t alpha, real_t *row) const
-{
-	long i, j;
-
-	if (icols != 0)
-	{
-		j = jr[rNum];
-		i = 0;
-		if ( isEqual(alpha,1.0) == BT_TRUE )
-			while (j < jr[rNum+1] && i < icols->length)
-				if (ic[j] == icols->number[icols->iSort[i]])
-					row[icols->iSort[i++]] = val[j++];
-				else if (ic[j] > icols->number[icols->iSort[i]])
-					row[icols->iSort[i++]] = 0.0;
-				else
-					j++;
-		else if ( isEqual(alpha,-1.0) == BT_TRUE )
-			while (j < jr[rNum+1] && i < icols->length)
-				if (ic[j] == icols->number[icols->iSort[i]])
-					row[icols->iSort[i++]] = -val[j++];
-				else if (ic[j] > icols->number[icols->iSort[i]])
-					row[icols->iSort[i++]] = 0.0;
-				else
-					j++;
-		else
-			while (j < jr[rNum+1] && i < icols->length)
-				if (ic[j] == icols->number[icols->iSort[i]])
-					row[icols->iSort[i++]] = alpha * val[j++];
-				else if (ic[j] > icols->number[icols->iSort[i]])
-					row[icols->iSort[i++]] = 0.0;
-				else
-					j++;
-
-		/* fill in remaining zeros */
-		while (i < icols->length)
-			row[icols->iSort[i++]] = 0.0;
-	}
-	else
-	{
-		for (i = 0; i < nCols; i++)
-			row[i] = 0;
-
-		if ( isEqual(alpha,1.0) == BT_TRUE )
-			for (j = jr[rNum]; j < jr[rNum+1]; j++)
-				row[ic[j]] = val[j];
-		else if ( isEqual(alpha,-1.0) == BT_TRUE )
-			for (j = jr[rNum]; j < jr[rNum+1]; j++)
-				row[ic[j]] = -val[j];
-		else
-			for (j = jr[rNum]; j < jr[rNum+1]; j++)
-				row[ic[j]] = alpha * val[j];
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-returnValue SparseMatrixRow::getCol(int cNum, const Indexlist* const irows, real_t alpha, real_t *col) const
-{
-	long i, j, k, srt;
-
-    if (irows != 0)
-    {
-	    if ( isEqual(alpha,1.0) == BT_TRUE )
-		    for (k = 0; k < irows->length; k++)
-		    {
-				srt = irows->iSort[k];
-			    j = irows->number[srt];
-			    for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
-			    col[srt] = (i < jr[j+1] && ic[i] == cNum) ? val[i] : 0.0;
-		    }
-	    else if ( isEqual(alpha,-1.0) == BT_TRUE )
-		    for (k = 0; k < irows->length; k++)
-		    {
-				srt = irows->iSort[k];
-			    j = irows->number[srt];
-			    for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
-			    col[srt] = (i < jr[j+1] && ic[i] == cNum) ? -val[i] : 0.0;
-		    }
-	    else
-		    for (k = 0; k < irows->length; k++)
-		    {
-				srt = irows->iSort[k];
-			    j = irows->number[srt];
-			    for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
-			    col[srt] = (i < jr[j+1] && ic[i] == cNum) ? alpha*val[i] : 0.0;
-		    }
-    }
-    else
-    {
-	    if ( isEqual(alpha,1.0) == BT_TRUE )
-		    for (j = 0; j < nCols; j++)
-		    {
-			    for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
-			    col[j] = (i < jr[j+1] && ic[i] == cNum) ? val[i] : 0.0;
-		    }
-	    else if ( isEqual(alpha,-1.0) == BT_TRUE )
-		    for (j = 0; j < irows->length; j++)
-		    {
-			    for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
-			    col[j] = (i < jr[j+1] && ic[i] == cNum) ? -val[i] : 0.0;
-		    }
-	    else
-		    for (j = 0; j < irows->length; j++)
-		    {
-			    for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
-			    col[j] = (i < jr[j+1] && ic[i] == cNum) ? alpha*val[i] : 0.0;
-		    }
-    }
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-returnValue SparseMatrixRow::times(int xN, real_t alpha, const real_t *x, int xLD,
-		real_t beta, real_t *y, int yLD) const
-{
-	long i, j, k;
-
-	if ( isZero(beta) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nRows; j++)
-				y[j+k*yLD] = 0.0;
-	else if ( isEqual(beta,-1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nRows; j++)
-				y[j+k*yLD] = -y[j+k*yLD];
-	else if ( isEqual(beta,1.0) == BT_FALSE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nRows; j++)
-				y[j+k*yLD] *= beta;
-
-	if ( isEqual(alpha,1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nRows; j++)
-				for (i = jr[j]; i < jr[j+1]; i++)
-					y[j+k*yLD] += val[i] * x[ic[i]+k*xLD];
-	else if ( isEqual(alpha,-1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nRows; j++)
-				for (i = jr[j]; i < jr[j+1]; i++)
-					y[j+k*yLD] -= val[i] * x[ic[i]+k*xLD];
-	else
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nRows; j++)
-				for (i = jr[j]; i < jr[j+1]; i++)
-					y[j+k*yLD] += alpha * val[i] * x[ic[i]+k*xLD];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-returnValue SparseMatrixRow::transTimes(int xN, real_t alpha, const real_t *x, int xLD,
-		real_t beta, real_t *y, int yLD) const
-{
-	long i, j, k;
-
-	if ( isZero(beta) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nCols; j++)
-				y[j+k*yLD] = 0.0;
-	else if ( isEqual(beta,-1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nCols; j++)
-				y[j+k*yLD] = -y[j+k*yLD];
-	else if ( isEqual(beta,1.0) == BT_FALSE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < nCols; j++)
-				y[j+k*yLD] *= beta;
-
-	if ( isEqual(alpha,1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (i = 0; i < nRows; i++)
-				for (j = jr[i]; j < jr[i+1]; j++)
-					y[ic[j]+k*yLD] += val[j] * x[i+k*xLD];
-	else if ( isEqual(alpha,-1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (i = 0; i < nRows; i++)
-				for (j = jr[i]; j < jr[i+1]; j++)
-					y[ic[j]+k*yLD] -= val[j] * x[i+k*xLD];
-	else
-		for (k = 0; k < xN; k++)
-			for (i = 0; i < nRows; i++)
-				for (j = jr[i]; j < jr[i+1]; j++)
-					y[ic[j]+k*yLD] += alpha * val[j] * x[i+k*xLD];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-returnValue SparseMatrixRow::times(const Indexlist* const irows, const Indexlist* const icols,
-		int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD,
-		BooleanType yCompr) const
-{
-	long i, j, k, l, srt, row;
-
-	if (yCompr == BT_TRUE)
-	{
-		if ( isZero(beta) == BT_TRUE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[j+k*yLD] = 0.0;
-		else if ( isEqual(beta,-1.0) == BT_TRUE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[j+k*yLD] = -y[j+k*yLD];
-		else if ( isEqual(beta,1.0) == BT_FALSE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[j+k*yLD] *= beta;
-
-		if (icols == 0)
-			if ( isEqual(alpha,1.0) == BT_TRUE )
-				for (l = 0; l < irows->length; l++)
-				{
-					srt = irows->iSort[l];
-					row = irows->number[srt];
-					for (j = jr[row]; j < jr[row+1]; j++)
-						for (k = 0; k < xN; k++)
-							y[k*yLD+srt] += val[j] * x[k*xLD+ic[j]];
-				}
-			else if ( isEqual(alpha,-1.0) == BT_TRUE )
-				for (l = 0; l < irows->length; l++)
-				{
-					srt = irows->iSort[l];
-					row = irows->number[srt];
-					for (j = jr[row]; j < jr[row+1]; j++)
-						for (k = 0; k < xN; k++)
-							y[k*yLD+srt] -= val[j] * x[k*xLD+ic[j]];
-				}
-			else
-				for (l = 0; l < irows->length; l++)
-				{
-					srt = irows->iSort[l];
-					row = irows->number[srt];
-					for (j = jr[row]; j < jr[row+1]; j++)
-						for (k = 0; k < xN; k++)
-							y[k*yLD+srt] += alpha * val[j] * x[k*xLD+ic[j]];
-				}
-		else /* icols != 0 */
-			if ( isEqual(alpha,1.0) == BT_TRUE )
-				for (l = 0; l < irows->length; l++)
-				{
-					srt = irows->iSort[l];
-					row = irows->number[srt];
-					j = jr[row];
-					i = 0;
-					while (j < jr[row+1] && i < icols->length)
-						if (ic[j] == icols->number[icols->iSort[i]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+srt] += val[j] * x[k*xLD+icols->iSort[i]];
-							j++, i++;
-						}
-						else if (ic[j] > icols->number[icols->iSort[i]]) i++;
-						else j++;
-				}
-			else if ( isEqual(alpha,-1.0) == BT_TRUE )
-				for (l = 0; l < irows->length; l++)
-				{
-					srt = irows->iSort[l];
-					row = irows->number[srt];
-					j = jr[row];
-					i = 0;
-					while (j < jr[row+1] && i < icols->length)
-						if (ic[j] == icols->number[icols->iSort[i]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+srt] -= val[j] * x[k*xLD+icols->iSort[i]];
-							j++, i++;
-						}
-						else if (ic[j] > icols->number[icols->iSort[i]]) i++;
-						else j++;
-				}
-			else
-				for (l = 0; l < irows->length; l++)
-				{
-					srt = irows->iSort[l];
-					row = irows->number[srt];
-					j = jr[row];
-					i = 0;
-					while (j < jr[row+1] && i < icols->length)
-						if (ic[j] == icols->number[icols->iSort[i]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+srt] += alpha * val[j] * x[k*xLD+icols->iSort[i]];
-							j++, i++;
-						}
-						else if (ic[j] > icols->number[icols->iSort[i]]) i++;
-						else j++;
-				}
-	}
-	else /* y not compressed */
-	{
-		if ( isZero(beta) == BT_TRUE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[irows->number[j]+k*yLD] = 0.0;
-		else if ( isEqual(beta,-1.0) == BT_TRUE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[irows->number[j]+k*yLD] = -y[j+k*yLD];
-		else if ( isEqual(beta,1.0) == BT_FALSE )
-			for (k = 0; k < xN; k++)
-				for (j = 0; j < irows->length; j++)
-					y[irows->number[j]+k*yLD] *= beta;
-
-		if (icols == 0)
-			if ( isEqual(alpha,1.0) == BT_TRUE )
-				for (l = 0; l < irows->length; l++)
-				{
-					row = irows->number[irows->iSort[l]];
-					for (j = jr[row]; j < jr[row+1]; j++)
-						for (k = 0; k < xN; k++)
-							y[k*yLD+row] += val[j] * x[k*xLD+ic[j]];
-				}
-			else if ( isEqual(alpha,-1.0) == BT_TRUE )
-				for (l = 0; l < irows->length; l++)
-				{
-					row = irows->number[irows->iSort[l]];
-					for (j = jr[row]; j < jr[row+1]; j++)
-						for (k = 0; k < xN; k++)
-							y[k*yLD+row] -= val[j] * x[k*xLD+ic[j]];
-				}
-			else
-				for (l = 0; l < irows->length; l++)
-				{
-					row = irows->number[irows->iSort[l]];
-					for (j = jr[row]; j < jr[row+1]; j++)
-						for (k = 0; k < xN; k++)
-							y[k*yLD+row] += alpha * val[j] * x[k*xLD+ic[j]];
-				}
-		else /* icols != 0 */
-			if ( isEqual(alpha,1.0) == BT_TRUE )
-				for (l = 0; l < irows->length; l++)
-				{
-					row = irows->iSort[l];
-					j = jr[irows->number[row]];
-					i = 0;
-					while (j < jr[irows->number[row]+1] && i < icols->length)
-						if (ic[j] == icols->number[icols->iSort[i]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+row] += val[j] * x[k*xLD+icols->iSort[i]];
-							j++, i++;
-						}
-						else if (ic[j] > icols->number[icols->iSort[i]]) i++;
-						else j++;
-				}
-			else if ( isEqual(alpha,-1.0) == BT_TRUE )
-				for (l = 0; l < irows->length; l++)
-				{
-					row = irows->iSort[l];
-					j = jr[irows->number[row]];
-					i = 0;
-					while (j < jr[irows->number[row]+1] && i < icols->length)
-						if (ic[j] == icols->number[icols->iSort[i]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+row] -= val[j] * x[k*xLD+icols->iSort[i]];
-							j++, i++;
-						}
-						else if (ic[j] > icols->number[icols->iSort[i]]) i++;
-						else j++;
-				}
-			else
-				for (l = 0; l < irows->length; l++)
-				{
-					row = irows->iSort[l];
-					j = jr[irows->number[row]];
-					i = 0;
-					while (j < jr[irows->number[row]+1] && i < icols->length)
-						if (ic[j] == icols->number[icols->iSort[i]])
-						{
-							for (k = 0; k < xN; k++)
-								y[k*yLD+row] += alpha * val[j] * x[k*xLD+icols->iSort[i]];
-							j++, i++;
-						}
-						else if (ic[j] > icols->number[icols->iSort[i]]) i++;
-						else j++;
-				}
-	}
-	return SUCCESSFUL_RETURN;
-}
-
-
-returnValue SparseMatrixRow::transTimes(const Indexlist* const irows, const Indexlist* const icols,
-		int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
-{
-	long i, j, k, l, row, srt;
-
-	if ( isZero(beta) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < icols->length; j++)
-				y[j+k*yLD] = 0.0;
-	else if ( isEqual(beta,-1.0) == BT_TRUE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < icols->length; j++)
-				y[j+k*yLD] = -y[j+k*yLD];
-	else if ( isEqual(beta,1.0) == BT_FALSE )
-		for (k = 0; k < xN; k++)
-			for (j = 0; j < icols->length; j++)
-				y[j+k*yLD] *= beta;
-
-	if ( isEqual(alpha,1.0) == BT_TRUE )
-		for (l = 0; l < irows->length; l++)
-		{
-			srt = irows->iSort[l];
-			row = irows->number[srt];
-			j = jr[row];
-			i = 0;
-			while (j < jr[row+1] && i < icols->length)
-				if (ic[j] == icols->number[icols->iSort[i]])
-				{
-					for (k = 0; k < xN; k++)
-						y[k*yLD+icols->iSort[i]] += val[j] * x[k*xLD+srt];
-					j++, i++;
-				}
-				else if (ic[j] > icols->number[icols->iSort[i]]) i++;
-				else j++;
-		}
-	else if ( isEqual(alpha,-1.0) == BT_TRUE )
-		for (l = 0; l < irows->length; l++)
-		{
-			srt = irows->iSort[l];
-			row = irows->number[srt];
-			j = jr[row];
-			i = 0;
-			while (j < jr[row+1] && i < icols->length)
-				if (ic[j] == icols->number[icols->iSort[i]])
-				{
-					for (k = 0; k < xN; k++)
-						y[k*yLD+icols->iSort[i]] -= val[j] * x[k*xLD+srt];
-					j++, i++;
-				}
-				else if (ic[j] > icols->number[icols->iSort[i]]) i++;
-				else j++;
-		}
-	else
-		for (l = 0; l < irows->length; l++)
-		{
-			srt = irows->iSort[l];
-			row = irows->number[srt];
-			j = jr[row];
-			i = 0;
-			while (j < jr[row+1] && i < icols->length)
-				if (ic[j] == icols->number[icols->iSort[i]])
-				{
-					for (k = 0; k < xN; k++)
-						y[k*yLD+icols->iSort[i]] += alpha * val[j] * x[k*xLD+srt];
-					j++, i++;
-				}
-				else if (ic[j] > icols->number[icols->iSort[i]]) i++;
-				else j++;
-		}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-returnValue SparseMatrixRow::addToDiag(real_t alpha)
-{
-	long i;
-	
-	if ( jd == 0 )
-		return THROWERROR( RET_DIAGONAL_NOT_INITIALISED );
-
-	if ( isZero(alpha) == BT_FALSE )
-		for (i = 0; i < nRows && i < nCols; i++)
-			if (ic[jd[i]] == i) val[jd[i]] += alpha;
-			else return RET_NO_DIAGONAL_AVAILABLE;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-sparse_int_t *SparseMatrixRow::createDiagInfo()
-{
-	long i, j;
-
-	if (jd == 0) {
-		jd = new sparse_int_t[nRows];
-
-		for (i = 0; i < nRows; i++)
-		{
-			for (j = jr[i]; j < jr[i+1] && ic[j] < i; j++);
-			jd[i] = j;
-		}
-	}
-
-	return jd;
-}
-
-
-real_t *SparseMatrixRow::full() const
-{
-	long i, j;
-	real_t *v = new real_t[nRows*nCols];
-
-	for (i = 0; i < nCols*nRows; i++)
-		v[i] = 0.0;
-
-	for (i = 0; i < nRows; i++)
-		for (j = jr[i]; j < jr[i+1]; j++)
-			v[ic[j] + i * nCols] = val[j];
-
-	return v;
-}
-
-
-returnValue SparseMatrixRow::print( const char* name ) const
-{
-	real_t* tmp = this->full();
-	returnValue retVal = REFER_NAMESPACE_QPOASES print( tmp,nRows,nCols,name );
-	delete[] tmp;
-
-	return retVal;
-}
-
-
-
-Matrix *SymSparseMat::duplicate() const
-{
-	return duplicateSym();
-}
-
-
-SymmetricMatrix *SymSparseMat::duplicateSym() const
-{
-	/* "same" as duplicate() in SparseMatrix */
-	long i, length = jc[nCols];
-	SymSparseMat *dupl = new SymSparseMat;
-
-	dupl->nRows = nRows;
-	dupl->nCols = nCols;
-	dupl->ir = new sparse_int_t[length];
-	dupl->jc = new sparse_int_t[nCols+1];
-	dupl->val = new real_t[length];
-
-	for (i = 0; i < length; i++) dupl->ir[i] = ir[i];
-	for (i = 0; i <= nCols; i++) dupl->jc[i] = jc[i];
-	for (i = 0; i < length; i++) dupl->val[i] = val[i];
-
-	if ( jd != 0 )
-	{
-		dupl->jd = new sparse_int_t[nCols];
-		for (i = 0; i < nCols; i++) dupl->jd[i] = jd[i];
-	}
-	else
-		dupl->jd = 0;
-
-	dupl->doFreeMemory( );
-
-	return dupl;
-}
-
-
-returnValue SymSparseMat::bilinear(const Indexlist* const icols,
-		int xN, const real_t *x, int xLD, real_t *y, int yLD) const
-{
-	int i, j, k, l, idx, row, col;
-
-	if ( jd == 0 )
-		return THROWERROR( RET_DIAGONAL_NOT_INITIALISED );
-
-	/* clear output */
-	for (i = 0; i < xN*xN; i++)
-		y[i] = 0.0;
-
-	/* compute lower triangle */
-	for (l = 0; l < icols->length; l++)
-	{
-		col = icols->number[icols->iSort[l]];
-		idx = jd[col];
-		k = 0;
-		while (idx < jc[col+1] && k < icols->length)
-		{
-			row = icols->number[icols->iSort[k]];
-			if (ir[idx] == row)
-			{
-				/* TODO: It is possible to formulate this as DSYR and DSYR2
-				 * operations. */
-				if (row == col) /* diagonal element */
-					for (i = 0; i < xN; i++)
-						for (j = i; j < xN; j++)
-							y[i*yLD+j] += val[idx] * x[i*xLD+col] * x[j*xLD+col];
-				else /* subdiagonal elements */
-					for (i = 0; i < xN; i++)
-						for (j = i; j < xN; j++)
-							y[i*yLD+j] += val[idx] * (x[i*xLD+col] * x[j*xLD+row] + x[i*xLD+row] * x[j*xLD+col]);
-				idx++, k++;
-			}
-			else if (ir[idx] > row) k++;
-			else idx++;
-		}
-	}
-
-	/* fill upper triangle */
-	for (i = 0; i < xN; i++)
-		for (j = i; j < xN; j++)
-			y[j*yLD+i] = y[i*yLD+j];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/MessageHandling.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/MessageHandling.cpp
deleted file mode 100644
index 897b2d7f80c..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/MessageHandling.cpp
+++ /dev/null
@@ -1,612 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file src/MessageHandling.cpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of the MessageHandling class including global return values.
- *
- */
-
-
-#include <stdio.h>
-
-#ifdef __MATLAB__
-  #include "mex.h"
-#endif
-
-#include <qpOASES/MessageHandling.hpp>
-#include <qpOASES/Utils.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-
-#ifndef __XPCTARGET__
-/** Defines pairs of global return values and messages. */
-MessageHandling::ReturnValueList returnValueList[] =
-{
-/* miscellaneous */
-{ SUCCESSFUL_RETURN, "Successful return", VS_VISIBLE },
-{ RET_DIV_BY_ZERO, "Division by zero", VS_VISIBLE },
-{ RET_INDEX_OUT_OF_BOUNDS, "Index out of bounds", VS_VISIBLE },
-{ RET_INVALID_ARGUMENTS, "At least one of the arguments is invalid", VS_VISIBLE },
-{ RET_ERROR_UNDEFINED, "Error number undefined", VS_VISIBLE },
-{ RET_WARNING_UNDEFINED, "Warning number undefined", VS_VISIBLE },
-{ RET_INFO_UNDEFINED, "Info number undefined", VS_VISIBLE },
-{ RET_EWI_UNDEFINED, "Error/warning/info number undefined", VS_VISIBLE },
-{ RET_AVAILABLE_WITH_LINUX_ONLY, "This function is available under Linux only", VS_HIDDEN },
-{ RET_UNKNOWN_BUG, "The error occurred is not yet known", VS_VISIBLE },
-{ RET_PRINTLEVEL_CHANGED, "Print level changed", VS_VISIBLE },
-{ RET_NOT_YET_IMPLEMENTED, "Requested function is not yet implemented.", VS_VISIBLE },
-/* Indexlist */
-{ RET_INDEXLIST_MUST_BE_REORDERD, "Index list has to be reordered", VS_VISIBLE },
-{ RET_INDEXLIST_EXCEEDS_MAX_LENGTH, "Index list exceeds its maximal physical length", VS_VISIBLE },
-{ RET_INDEXLIST_CORRUPTED, "Index list corrupted", VS_VISIBLE },
-{ RET_INDEXLIST_OUTOFBOUNDS, "Physical index is out of bounds", VS_VISIBLE },
-{ RET_INDEXLIST_ADD_FAILED, "Adding indices from another index set failed", VS_VISIBLE },
-{ RET_INDEXLIST_INTERSECT_FAILED, "Intersection with another index set failed", VS_VISIBLE },
-/* SubjectTo / Bounds / Constraints */
-{ RET_INDEX_ALREADY_OF_DESIRED_STATUS, "Index is already of desired status", VS_VISIBLE },
-{ RET_ADDINDEX_FAILED, "Adding index to index set failed", VS_VISIBLE },
-{ RET_REMOVEINDEX_FAILED, "Removing index from index set failed", VS_VISIBLE },
-{ RET_SWAPINDEX_FAILED, "Cannot swap between different indexsets", VS_VISIBLE },
-{ RET_NOTHING_TO_DO, "Nothing to do", VS_VISIBLE },
-{ RET_SETUP_BOUND_FAILED, "Setting up bound index failed", VS_VISIBLE },
-{ RET_SETUP_CONSTRAINT_FAILED, "Setting up constraint index failed", VS_VISIBLE },
-{ RET_MOVING_BOUND_FAILED, "Moving bound between index sets failed", VS_VISIBLE },
-{ RET_MOVING_CONSTRAINT_FAILED, "Moving constraint between index sets failed", VS_VISIBLE },
-{ RET_SHIFTING_FAILED, "Shifting of bounds/constraints failed", VS_VISIBLE },
-{ RET_ROTATING_FAILED, "Rotating of bounds/constraints failed", VS_VISIBLE },
-/* QProblem */
-{ RET_QPOBJECT_NOT_SETUP, "The QP object has not been setup correctly, use another constructor", VS_VISIBLE },
-{ RET_QP_ALREADY_INITIALISED, "QProblem has already been initialised", VS_VISIBLE },
-{ RET_NO_INIT_WITH_STANDARD_SOLVER, "Initialisation via extern QP solver is not yet implemented", VS_VISIBLE },
-{ RET_RESET_FAILED, "Reset failed", VS_VISIBLE },
-{ RET_INIT_FAILED, "Initialisation failed", VS_VISIBLE },
-{ RET_INIT_FAILED_TQ, "Initialisation failed due to TQ factorisation", VS_VISIBLE },
-{ RET_INIT_FAILED_CHOLESKY, "Initialisation failed due to Cholesky decomposition", VS_VISIBLE },
-{ RET_INIT_FAILED_HOTSTART, "Initialisation failed! QP could not be solved!", VS_VISIBLE },
-{ RET_INIT_FAILED_INFEASIBILITY, "Initial QP could not be solved due to infeasibility!", VS_VISIBLE },
-{ RET_INIT_FAILED_UNBOUNDEDNESS, "Initial QP could not be solved due to unboundedness!", VS_VISIBLE },
-{ RET_INIT_FAILED_REGULARISATION, "Initialisation failed as Hessian matrix could not be regularised", VS_VISIBLE },
-{ RET_INIT_SUCCESSFUL, "Initialisation done", VS_VISIBLE },
-{ RET_OBTAINING_WORKINGSET_FAILED, "Failed to obtain working set for auxiliary QP", VS_VISIBLE },
-{ RET_SETUP_WORKINGSET_FAILED, "Failed to setup working set for auxiliary QP", VS_VISIBLE },
-{ RET_SETUP_AUXILIARYQP_FAILED, "Failed to setup auxiliary QP for initialised homotopy", VS_VISIBLE },
-{ RET_NO_EXTERN_SOLVER, "No extern QP solver available", VS_VISIBLE },
-{ RET_QP_UNBOUNDED, "QP is unbounded", VS_VISIBLE },
-{ RET_QP_INFEASIBLE, "QP is infeasible", VS_VISIBLE },
-{ RET_QP_NOT_SOLVED, "Problems occurred while solving QP with standard solver", VS_VISIBLE },
-{ RET_QP_SOLVED, "QP successfully solved", VS_VISIBLE },
-{ RET_UNABLE_TO_SOLVE_QP, "Problems occurred while solving QP", VS_VISIBLE },
-{ RET_INITIALISATION_STARTED, "Starting problem initialisation...", VS_VISIBLE },
-{ RET_HOTSTART_FAILED, "Unable to perform homotopy due to internal error", VS_VISIBLE },
-{ RET_HOTSTART_FAILED_TO_INIT, "Unable to initialise problem", VS_VISIBLE },
-{ RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED, "Unable to perform homotopy as previous QP is not solved", VS_VISIBLE },
-{ RET_ITERATION_STARTED, "Iteration", VS_VISIBLE },
-{ RET_SHIFT_DETERMINATION_FAILED, "Determination of shift of the QP data failed", VS_VISIBLE },
-{ RET_STEPDIRECTION_DETERMINATION_FAILED, "Determination of step direction failed", VS_VISIBLE },
-{ RET_STEPLENGTH_DETERMINATION_FAILED, "Determination of step direction failed", VS_VISIBLE },
-{ RET_OPTIMAL_SOLUTION_FOUND, "Optimal solution of neighbouring QP found", VS_VISIBLE },
-{ RET_HOMOTOPY_STEP_FAILED, "Unable to perform homotopy step", VS_VISIBLE },
-{ RET_HOTSTART_STOPPED_INFEASIBILITY, "Premature homotopy termination because QP is infeasible", VS_VISIBLE },
-{ RET_HOTSTART_STOPPED_UNBOUNDEDNESS, "Premature homotopy termination because QP is unbounded", VS_VISIBLE },
-{ RET_WORKINGSET_UPDATE_FAILED, "Unable to update working sets according to initial guesses", VS_VISIBLE },
-{ RET_MAX_NWSR_REACHED, "Maximum number of working set recalculations performed", VS_VISIBLE },
-{ RET_CONSTRAINTS_NOT_SPECIFIED, "Problem does comprise constraints! You have to specify new constraints' bounds", VS_VISIBLE },
-{ RET_INVALID_FACTORISATION_FLAG, "Invalid factorisation flag", VS_VISIBLE },
-{ RET_UNABLE_TO_SAVE_QPDATA, "Unable to save QP data", VS_VISIBLE },
-{ RET_STEPDIRECTION_FAILED_TQ, "Abnormal termination due to TQ factorisation", VS_VISIBLE },
-{ RET_STEPDIRECTION_FAILED_CHOLESKY, "Abnormal termination due to Cholesky factorisation", VS_VISIBLE },
-{ RET_CYCLING_DETECTED, "Cycling detected", VS_VISIBLE },
-{ RET_CYCLING_NOT_RESOLVED, "Cycling cannot be resolved, QP is probably infeasible", VS_VISIBLE },
-{ RET_CYCLING_RESOLVED, "Cycling probably resolved", VS_VISIBLE },
-{ RET_STEPSIZE, "", VS_VISIBLE },
-{ RET_STEPSIZE_NONPOSITIVE, "", VS_VISIBLE },
-{ RET_SETUPSUBJECTTOTYPE_FAILED, "Setup of SubjectToTypes failed", VS_VISIBLE },
-{ RET_ADDCONSTRAINT_FAILED, "Addition of constraint to working set failed", VS_VISIBLE },
-{ RET_ADDCONSTRAINT_FAILED_INFEASIBILITY, "Addition of constraint to working set failed", VS_VISIBLE },
-{ RET_ADDBOUND_FAILED, "Addition of bound to working set failed", VS_VISIBLE },
-{ RET_ADDBOUND_FAILED_INFEASIBILITY, "Addition of bound to working set failed", VS_VISIBLE },
-{ RET_REMOVECONSTRAINT_FAILED, "Removal of constraint from working set failed", VS_VISIBLE },
-{ RET_REMOVEBOUND_FAILED, "Removal of bound from working set failed", VS_VISIBLE },
-{ RET_REMOVE_FROM_ACTIVESET, "Removing from active set:", VS_VISIBLE },
-{ RET_ADD_TO_ACTIVESET, "Adding to active set:", VS_VISIBLE },
-{ RET_REMOVE_FROM_ACTIVESET_FAILED, "Removing from active set failed", VS_VISIBLE },
-{ RET_ADD_TO_ACTIVESET_FAILED, "Adding to active set failed", VS_VISIBLE },
-{ RET_CONSTRAINT_ALREADY_ACTIVE, "Constraint is already active", VS_VISIBLE },
-{ RET_ALL_CONSTRAINTS_ACTIVE, "All constraints are active, no further constraint can be added", VS_VISIBLE },
-{ RET_LINEARLY_DEPENDENT, "New bound/constraint is linearly dependent", VS_VISIBLE },
-{ RET_LINEARLY_INDEPENDENT, "New bound/constraint is linearly independent", VS_VISIBLE },
-{ RET_LI_RESOLVED, "Linear independence of active constraint matrix successfully resolved", VS_VISIBLE },
-{ RET_ENSURELI_FAILED, "Failed to ensure linear independence of active constraint matrix", VS_VISIBLE },
-{ RET_ENSURELI_FAILED_TQ, "Abnormal termination due to TQ factorisation", VS_VISIBLE },
-{ RET_ENSURELI_FAILED_NOINDEX, "QP is infeasible", VS_VISIBLE },
-{ RET_ENSURELI_FAILED_CYCLING, "QP is infeasible", VS_VISIBLE },
-{ RET_BOUND_ALREADY_ACTIVE, "Bound is already active", VS_VISIBLE },
-{ RET_ALL_BOUNDS_ACTIVE, "All bounds are active, no further bound can be added", VS_VISIBLE },
-{ RET_CONSTRAINT_NOT_ACTIVE, "Constraint is not active", VS_VISIBLE },
-{ RET_BOUND_NOT_ACTIVE, "Bound is not active", VS_VISIBLE },
-{ RET_HESSIAN_NOT_SPD, "Projected Hessian matrix not positive definite", VS_VISIBLE },
-{ RET_HESSIAN_INDEFINITE, "Hessian matrix is indefinite", VS_VISIBLE },
-{ RET_MATRIX_SHIFT_FAILED, "Unable to update matrices or to transform vectors", VS_VISIBLE },
-{ RET_MATRIX_FACTORISATION_FAILED, "Unable to calculate new matrix factorisations", VS_VISIBLE },
-{ RET_PRINT_ITERATION_FAILED, "Unable to print information on current iteration", VS_VISIBLE },
-{ RET_NO_GLOBAL_MESSAGE_OUTPUTFILE, "No global message output file initialised", VS_VISIBLE },
-{ RET_DISABLECONSTRAINTS_FAILED, "Unable to disable constraints", VS_VISIBLE },
-{ RET_ENABLECONSTRAINTS_FAILED, "Unable to enable constraints", VS_VISIBLE },
-{ RET_ALREADY_ENABLED, "Bound or constraint is already enabled", VS_VISIBLE },
-{ RET_ALREADY_DISABLED, "Bound or constraint is already disabled", VS_VISIBLE },
-{ RET_NO_HESSIAN_SPECIFIED, "No Hessian matrix has been specified", VS_VISIBLE },
-{ RET_USING_REGULARISATION, "Using regularisation as Hessian matrix is not positive definite", VS_VISIBLE },
-{ RET_EPS_MUST_BE_POSITVE, "Eps for regularisation must be sufficiently positive", VS_VISIBLE },
-{ RET_REGSTEPS_MUST_BE_POSITVE, "Maximum number of regularisation steps must be non-negative", VS_VISIBLE },
-{ RET_HESSIAN_ALREADY_REGULARISED, "Hessian has been already regularised", VS_VISIBLE },
-{ RET_CANNOT_REGULARISE_IDENTITY, "Identity Hessian matrix cannot be regularised", VS_VISIBLE },
-{ RET_CANNOT_REGULARISE_SPARSE, "Sparse matrix cannot be regularised as diagonal entry is missing", VS_VISIBLE },
-{ RET_NO_REGSTEP_NWSR, "No additional regularisation step could be performed due to limits", VS_VISIBLE },
-{ RET_FEWER_REGSTEPS_NWSR, "Fewer additional regularisation steps have been performed due to limits", VS_VISIBLE },
-{ RET_CHOLESKY_OF_ZERO_HESSIAN, "Cholesky decomposition of (unregularised) zero Hessian matrix", VS_VISIBLE },
-{ RET_CONSTRAINTS_ARE_NOT_SCALED, "(should not be thrown, no longer in use)", VS_VISIBLE },
-{ RET_INITIAL_BOUNDS_STATUS_NYI, "(should not be thrown, no longer in use)", VS_VISIBLE },
-{ RET_ERROR_IN_CONSTRAINTPRODUCT, "Error in user-defined constraint product function", VS_VISIBLE },
-{ RET_FIX_BOUNDS_FOR_LP, "All initial bounds must be fixed when solving an (unregularised) LP", VS_VISIBLE },
-{ RET_USE_REGULARISATION_FOR_LP, "Set options.enableRegularisation=BT_TRUE for solving LPs", VS_VISIBLE },
-/* SQProblem */
-{ RET_UPDATEMATRICES_FAILED, "Unable to update QP matrices", VS_VISIBLE },
-{ RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED, "Unable to update matrices as previous QP is not solved", VS_VISIBLE },
-/* Utils */
-{ RET_UNABLE_TO_OPEN_FILE, "Unable to open file", VS_VISIBLE },
-{ RET_UNABLE_TO_WRITE_FILE, "Unable to write into file", VS_VISIBLE },
-{ RET_UNABLE_TO_READ_FILE, "Unable to read from file", VS_VISIBLE },
-{ RET_FILEDATA_INCONSISTENT, "File contains inconsistent data", VS_VISIBLE },
-/* Options */
-{ RET_OPTIONS_ADJUSTED,	"Options needed to be adjusted for consistency reasons", VS_VISIBLE },
-/* SolutionAnalysis */
-{ RET_UNABLE_TO_ANALYSE_QPROBLEM, "Unable to analyse (S)QProblem(B) object", VS_VISIBLE },
-/* Benchmark */
-{ RET_NWSR_SET_TO_ONE, "Maximum number of working set changes was set to 1", VS_VISIBLE },
-{ RET_UNABLE_TO_READ_BENCHMARK, "Unable to read benchmark data", VS_VISIBLE },
-{ RET_BENCHMARK_ABORTED, "Benchmark aborted", VS_VISIBLE },
-{ RET_INITIAL_QP_SOLVED, "Initial QP solved", VS_VISIBLE },
-{ RET_QP_SOLUTION_STARTED, "Solving QP no.", VS_VISIBLE },
-{ RET_BENCHMARK_SUCCESSFUL, "Benchmark terminated successfully", VS_VISIBLE },
-/* Sparse matrices */
-{ RET_NO_DIAGONAL_AVAILABLE, "Sparse matrix does not have entries on full diagonal", VS_VISIBLE },
-{ RET_DIAGONAL_NOT_INITIALISED, "Diagonal data of sparse matrix has not been initialised", VS_VISIBLE },
-/* Dropping of infeasible constraints */
-{ RET_ENSURELI_DROPPED, "Linear independence resolved by dropping blocking constraint", VS_VISIBLE },
-/* Simple exitflags */
-{ RET_SIMPLE_STATUS_P1, "QP problem could not be solved within given number of iterations", VS_VISIBLE },
-{ RET_SIMPLE_STATUS_P0, "QP problem solved", VS_VISIBLE },
-{ RET_SIMPLE_STATUS_M1, "QP problem could not be solved due to an internal error", VS_VISIBLE },
-{ RET_SIMPLE_STATUS_M2, "QP problem is infeasible (and thus could not be solved)", VS_VISIBLE },
-{ RET_SIMPLE_STATUS_M3, "QP problem is unbounded (and thus could not be solved)", VS_VISIBLE },
-/* IMPORTANT: Terminal list element! */
-{ TERMINAL_LIST_ELEMENT, "", VS_HIDDEN }
-};
-#else
-MessageHandling::ReturnValueList returnValueList[1]; /* Do not use messages for embedded platforms! */
-#endif
-
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-/*
- *	M e s s a g e H a n d l i n g
- */
-MessageHandling::MessageHandling( )
-{
-	errorVisibility   = VS_VISIBLE;
-	warningVisibility = VS_VISIBLE;
-	infoVisibility    = VS_VISIBLE;
-
-	outputFile = stdFile;
-	errorCount = 0;
-}
-
-/*
- *	M e s s a g e H a n d l i n g
- */
-MessageHandling::MessageHandling( FILE* _outputFile )
-{
-	errorVisibility   = VS_VISIBLE;
-	warningVisibility = VS_VISIBLE;
-	infoVisibility    = VS_VISIBLE;
-
-	outputFile = _outputFile;
-	errorCount = 0;
-}
-
-/*
- *	M e s s a g e H a n d l i n g
- */
-MessageHandling::MessageHandling(	VisibilityStatus _errorVisibility,
-									VisibilityStatus _warningVisibility,
-		 							VisibilityStatus _infoVisibility
-									)
-{
-	errorVisibility   = _errorVisibility;
-	warningVisibility = _warningVisibility;
-	infoVisibility    = _infoVisibility;
-
-	outputFile = stdFile;
-	errorCount = 0;
-}
-
-/*
- *	M e s s a g e H a n d l i n g
- */
-MessageHandling::MessageHandling( 	FILE* _outputFile,
-									VisibilityStatus _errorVisibility,
-									VisibilityStatus _warningVisibility,
-		 							VisibilityStatus _infoVisibility
-									)
-{
-	errorVisibility   = _errorVisibility;
-	warningVisibility = _warningVisibility;
-	infoVisibility    = _infoVisibility;
-
-	outputFile = _outputFile;
-	errorCount = 0;
-}
-
-
-
-/*
- *	M e s s a g e H a n d l i n g
- */
-MessageHandling::MessageHandling( const MessageHandling& rhs )
-{
-	errorVisibility   = rhs.errorVisibility;
-	warningVisibility = rhs.warningVisibility;
-	infoVisibility    = rhs.infoVisibility;
-
-	outputFile = rhs.outputFile;
-	errorCount = rhs.errorCount;
-}
-
-
-/*
- *	~ M e s s a g e H a n d l i n g
- */
-MessageHandling::~MessageHandling( )
-{
-	#ifndef __XPCTARGET__
-	if ( ( outputFile != 0 ) && ( outputFile != stdout ) && ( outputFile != stderr ) )
-		fclose( outputFile );
-	#endif /* __XPCTARGET__ */
-}
-
-
-/*
- *	o p e r a t o r =
- */
-MessageHandling& MessageHandling::operator=( const MessageHandling& rhs )
-{
-	if ( this != &rhs )
-	{
-		errorVisibility   = rhs.errorVisibility;
-		warningVisibility = rhs.warningVisibility;
-		infoVisibility    = rhs.infoVisibility;
-
-		outputFile = rhs.outputFile;
-		errorCount = rhs.errorCount;
-	}
-
-	return *this;
-}
-
-
-/*
- *	t h r o w E r r o r
- */
-returnValue MessageHandling::throwError(
-	returnValue Enumber,
-	const char* additionaltext,
-	const char* functionname,
-	const char* filename,
-	const unsigned long linenumber,
-	VisibilityStatus localVisibilityStatus
-	)
-{
-	/* consistency check */
-	if ( Enumber <= SUCCESSFUL_RETURN )
-		return throwError( RET_ERROR_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-
-	/* Call to common throwMessage function if error shall be displayed. */
-	if ( errorVisibility == VS_VISIBLE )
-		return throwMessage( Enumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"ERROR" );
-	else
-		return Enumber;
-}
-
-
-/*
- *	t h r o w W a r n i n g
- */
-returnValue MessageHandling::throwWarning(
-	returnValue Wnumber,
-	const char* additionaltext,
-	const char* functionname,
-	const char* filename,
-	const unsigned long linenumber,
-	VisibilityStatus localVisibilityStatus
-  	)
-{
-	/* consistency check */
-  	if ( Wnumber <= SUCCESSFUL_RETURN )
-		return throwError( RET_WARNING_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-
-	/* Call to common throwMessage function if warning shall be displayed. */
-	if ( warningVisibility == VS_VISIBLE )
-		return throwMessage( Wnumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"WARNING" );
-  	else
-  		return Wnumber;
-}
-
-
-/*
- *	t h r o w I n f o
- */
-returnValue MessageHandling::throwInfo(
-  	returnValue Inumber,
-	const char* additionaltext,
-  	const char* functionname,
-	const char* filename,
-	const unsigned long linenumber,
-	VisibilityStatus localVisibilityStatus
- 	)
-{
-	/* consistency check */
-	if ( Inumber < SUCCESSFUL_RETURN )
-		return throwError( RET_INFO_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-
-	/* Call to common throwMessage function if info shall be displayed. */
-	if ( infoVisibility == VS_VISIBLE )
-		return throwMessage( Inumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"INFO" );
-	else
-		return Inumber;
-}
-
-
-/*
- *	r e s e t
- */
-returnValue MessageHandling::reset( )
-{
-	setErrorVisibilityStatus(   VS_VISIBLE );
-	setWarningVisibilityStatus( VS_VISIBLE );
-	setInfoVisibilityStatus(    VS_VISIBLE );
-
-	setOutputFile( stdFile );
-	setErrorCount( 0 );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	l i s t A l l M e s s a g e s
- */
-returnValue MessageHandling::listAllMessages( )
-{
-	#ifndef __XPCTARGET__
-	int keypos = 0;
-	char myPrintfString[MAX_STRING_LENGTH];
-
-	/* Run through whole returnValueList and print each item. */
-	while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
-	{
-		snprintf( myPrintfString,MAX_STRING_LENGTH," %d - %s \n",keypos,returnValueList[keypos].data );
-		myPrintf( myPrintfString );
-
-		++keypos;
-	}
-	#endif
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-
-/*
- *	t h r o w M e s s a g e
- */
-returnValue MessageHandling::throwMessage(
-	returnValue RETnumber,
-	const char* additionaltext,
-	const char* functionname,
-	const char* filename,
-	const unsigned long linenumber,
-	VisibilityStatus localVisibilityStatus,
-	const char* RETstring
- 	)
-{
-	#ifndef __SUPPRESSANYOUTPUT__
-	#ifndef __XPCTARGET__
-	int keypos = 0;
-	char myPrintfString[MAX_STRING_LENGTH];
-
-	/* 1) Determine number of whitespace for output. */
-	char whitespaces[MAX_STRING_LENGTH];
-	int numberOfWhitespaces = (errorCount-1)*2;
-
-	if ( numberOfWhitespaces < 0 )
-		numberOfWhitespaces = 0;
-
-	if ( numberOfWhitespaces > 40 )
-		numberOfWhitespaces = 40;
-
-	if ( numberOfWhitespaces >= (int)MAX_STRING_LENGTH )
-		numberOfWhitespaces = (int)MAX_STRING_LENGTH-1;
-
-	memset( whitespaces, ' ', (size_t) numberOfWhitespaces );
-	whitespaces[numberOfWhitespaces] = '\0';
-
-	/* 2) Find error/warning/info in list. */
-	while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
-	{
-		if ( returnValueList[keypos].key == RETnumber )
-			break;
-		else
-			++keypos;
-	}
-
-	if ( returnValueList[keypos].key == TERMINAL_LIST_ELEMENT )
-	{
-		throwError( RET_EWI_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		return RETnumber;
-	}
-
-	/* 3) Print error/warning/info. */
-	if ( ( returnValueList[keypos].globalVisibilityStatus == VS_VISIBLE ) && ( localVisibilityStatus == VS_VISIBLE ) )
-	{
-		if ( errorCount < 0 )
-		{
-			myPrintf( "\n" );
-			errorCount = 0;
-		}
-
-		if ( errorCount > 0 )
-		{
-			snprintf( myPrintfString,MAX_STRING_LENGTH,"%s->", whitespaces );
-			myPrintf( myPrintfString );
-		}
-
-		if ( additionaltext == 0 )
-		{
-			#ifdef __DEBUG__
-			snprintf(	myPrintfString,MAX_STRING_LENGTH,"%s (%s, %s:%d): \t%s\n",
-						RETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data
-						);
-			#else
-			snprintf(	myPrintfString,MAX_STRING_LENGTH,"%s:  %s\n",
-						RETstring,returnValueList[keypos].data
-						);
-			#endif
-			myPrintf( myPrintfString );
-		}
-		else
-		{
-			#ifdef __DEBUG__
-			snprintf(	myPrintfString,MAX_STRING_LENGTH,"%s (%s, %s:%d): \t%s %s\n",
-						RETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data,additionaltext
-						);
-			#else
-			snprintf(	myPrintfString,MAX_STRING_LENGTH,"%s:  %s %s\n",
-						RETstring,returnValueList[keypos].data,additionaltext
-						);
-			#endif
-			myPrintf( myPrintfString );
-		}
-
-		/* take care of proper indention for subsequent error messages */
-		if ( RETstring[0] == 'E' )
-		{
-			++errorCount;
-		}
-		else
-		{
-			if ( errorCount > 0 )
-				myPrintf( "\n" );
-			errorCount = 0;
-		}
-	}
-	#endif /* __XPCTARGET__ */
-	#endif /* __SUPPRESSANYOUTPUT__ */
-
-	return RETnumber;
-}
-
-/****************************************************************************/
-/* S T A T I C  P U B L I C */
-/****************************************************************************/
-
-const char* MessageHandling::getErrorCodeMessage(	const returnValue _returnValue
-													)
-{
-	#ifndef __XPCTARGET__
-	int keypos = 0;
-	
-	/* 2) Find error/warning/info in list. */
-	while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
-	{
-		if ( returnValueList[keypos].key == _returnValue )
-			break;
-		else
-			++keypos;
-	}
-
-	if ( returnValueList[keypos].key == TERMINAL_LIST_ELEMENT )
-	{
-		return "Unknown error code";
-	}
-
-	return (returnValueList[keypos].data != 0) ? returnValueList[keypos].data : "No message for this error code";
-	#else /* __XPCTARGET__ */
-
-	return "No message for this error code";
-
-	#endif /* __XPCTARGET__ */
-}
-
-
-/*****************************************************************************
- *  G L O B A L  M E S S A G E  H A N D L E R                                *
- *****************************************************************************/
-
-
-/** Global message handler for all qpOASES modules.*/
-static MessageHandling globalMessageHandler( stdFile,VS_VISIBLE,VS_VISIBLE,VS_VISIBLE );
-
-
-/*
- *	g e t G l o b a l M e s s a g e H a n d l e r
- */
-MessageHandling* getGlobalMessageHandler( )
-{
-	return &globalMessageHandler;
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/OQPinterface.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/OQPinterface.cpp
deleted file mode 100644
index e172d6da99f..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/OQPinterface.cpp
+++ /dev/null
@@ -1,670 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file src/OQPinterface.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2008-2014
- *
- *	Implementation of an interface comprising several utility functions
- *	for solving test problems from the Online QP Benchmark Collection
- *	(This collection is no longer maintained, see 
- *	http://www.qpOASES.org/onlineQP for a backup).
- *
- */
-
-
-#include <qpOASES/extras/OQPinterface.hpp>
-#include <qpOASES/QProblem.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*
- *	r e a d O Q P d i m e n s i o n s
- */
-returnValue readOQPdimensions(	const char* path,
-								int& nQP, int& nV, int& nC, int& nEC
-								)
-{
-	/* 1) Setup file name where dimensions are stored. */
-	char filename[MAX_STRING_LENGTH];
-	snprintf( filename,MAX_STRING_LENGTH,"%sdims.oqp",path );
-
-	/* 2) Load dimensions from file. */
-	int dims[4];
-	if ( readFromFile( dims,4,filename ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_UNABLE_TO_READ_FILE );
-
-	nQP = dims[0];
-	nV  = dims[1];
-	nC  = dims[2];
-	nEC = dims[3];
-
-
-	/* consistency check */
-	if ( ( nQP <= 0 ) || ( nV <= 0 ) || ( nC < 0 ) || ( nEC < 0 ) )
-		return THROWERROR( RET_FILEDATA_INCONSISTENT );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	r e a d O Q P d a t a
- */
-returnValue readOQPdata(	const char* path,
-							int& nQP, int& nV, int& nC, int& nEC,
-							real_t** H, real_t** g, real_t** A, real_t** lb, real_t** ub, real_t** lbA, real_t** ubA,
-							real_t** xOpt, real_t** yOpt, real_t** objOpt
-							)
-{
-	char filename[MAX_STRING_LENGTH];
-
-	/* consistency check */
-	if ( ( H == 0 ) || ( g == 0 ) || ( lb == 0 ) || ( ub == 0 ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-
-	/* 1) Obtain OQP dimensions. */
-	if ( readOQPdimensions( path, nQP,nV,nC,nEC ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_UNABLE_TO_READ_FILE );
-
-
-	/* another consistency check */
-	if ( ( nC > 0 ) && ( ( A == 0 ) || ( lbA == 0 ) || ( ubA == 0 ) ) )
-		return THROWERROR( RET_FILEDATA_INCONSISTENT );
-
-
-	/* 2) Allocate memory and load OQP data: */
-	/* Hessian matrix */
-	*H  = new real_t[nV*nV];
-	snprintf( filename,MAX_STRING_LENGTH,"%sH.oqp",path );
-	if ( readFromFile( *H,nV,nV,filename ) != SUCCESSFUL_RETURN )
-	{
-		delete[] *H;
-		return THROWERROR( RET_UNABLE_TO_READ_FILE );
-	}
-
-	/* gradient vector sequence */
-	*g  = new real_t[nQP*nV];
-	snprintf( filename,MAX_STRING_LENGTH,"%sg.oqp",path );
-	if ( readFromFile( *g,nQP,nV,filename ) != SUCCESSFUL_RETURN )
-	{
-		delete[] *g; delete[] *H;
-		return THROWERROR( RET_UNABLE_TO_READ_FILE );
-	}
-
-	/* lower bound vector sequence */
-	*lb  = new real_t[nQP*nV];
-	snprintf( filename,MAX_STRING_LENGTH,"%slb.oqp",path );
-	if ( readFromFile( *lb,nQP,nV,filename ) != SUCCESSFUL_RETURN )
-	{
-		delete[] *lb; delete[] *g; delete[] *H;
-		return THROWERROR( RET_UNABLE_TO_READ_FILE );
-	}
-
-	/* upper bound vector sequence */
-	*ub  = new real_t[nQP*nV];
-	snprintf( filename,MAX_STRING_LENGTH,"%sub.oqp",path );
-	if ( readFromFile( *ub,nQP,nV,filename ) != SUCCESSFUL_RETURN )
-	{
-		delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
-		return THROWERROR( RET_UNABLE_TO_READ_FILE );
-	}
-
-	if ( nC > 0 )
-	{
-		/* Constraint matrix */
-		*A   = new real_t[nC*nV];
-		snprintf( filename,MAX_STRING_LENGTH,"%sA.oqp",path );
-		if ( readFromFile( *A,nC,nV,filename ) != SUCCESSFUL_RETURN )
-		{
-			delete[] *A;
-			delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
-			return THROWERROR( RET_UNABLE_TO_READ_FILE );
-		}
-
-		/* lower constraints' bound vector sequence */
-		*lbA = new real_t[nQP*nC];
-		snprintf( filename,MAX_STRING_LENGTH,"%slbA.oqp",path );
-		if ( readFromFile( *lbA,nQP,nC,filename ) != SUCCESSFUL_RETURN )
-		{
-			delete[] *lbA; delete[] *A;
-			delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
-			return THROWERROR( RET_UNABLE_TO_READ_FILE );
-		}
-
-		/* upper constraints' bound vector sequence */
-		*ubA = new real_t[nQP*nC];
-		snprintf( filename,MAX_STRING_LENGTH,"%subA.oqp",path );
-		if ( readFromFile( *ubA,nQP,nC,filename ) != SUCCESSFUL_RETURN )
-		{
-			delete[] *ubA; delete[] *lbA; delete[] *A;
-			delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
-			return THROWERROR( RET_UNABLE_TO_READ_FILE );
-		}
-	}
-	else
-	{
-		*A = 0;
-		*lbA = 0;
-		*ubA = 0;
-	}
-
-	if ( xOpt != 0 )
-	{
-		/* primal solution vector sequence */
-		*xOpt = new real_t[nQP*nV];
-		snprintf( filename,MAX_STRING_LENGTH,"%sx_opt.oqp",path );
-		if ( readFromFile( *xOpt,nQP,nV,filename ) != SUCCESSFUL_RETURN )
-		{
-			delete[] xOpt;
-			if ( nC > 0 ) { delete[] *ubA; delete[] *lbA; delete[] *A; };
-			delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
-			return THROWERROR( RET_UNABLE_TO_READ_FILE );
-		}
-	}
-
-	if ( yOpt != 0 )
-	{
-		/* dual solution vector sequence */
-		*yOpt = new real_t[nQP*(nV+nC)];
-		snprintf( filename,MAX_STRING_LENGTH,"%sy_opt.oqp",path );
-		if ( readFromFile( *yOpt,nQP,nV+nC,filename ) != SUCCESSFUL_RETURN )
-		{
-			delete[] yOpt;
-			if ( xOpt != 0 ) { delete[] xOpt; };
-			if ( nC > 0 ) { delete[] *ubA; delete[] *lbA; delete[] *A; };
-			delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
-			return THROWERROR( RET_UNABLE_TO_READ_FILE );
-		}
-	}
-
-	if ( objOpt != 0 )
-	{
-		/* dual solution vector sequence */
-		*objOpt = new real_t[nQP];
-		snprintf( filename,MAX_STRING_LENGTH,"%sobj_opt.oqp",path );
-		if ( readFromFile( *objOpt,nQP,1,filename ) != SUCCESSFUL_RETURN )
-		{
-			delete[] objOpt;
-			if ( yOpt != 0 ) { delete[] yOpt; };
-			if ( xOpt != 0 ) { delete[] xOpt; };
-			if ( nC > 0 ) { delete[] *ubA; delete[] *lbA; delete[] *A; };
-			delete[] *ub; delete[] *lb; delete[] *g; delete[] *H;
-			return THROWERROR( RET_UNABLE_TO_READ_FILE );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s o l v e O Q P b e n c h m a r k
- */
-returnValue solveOQPbenchmark(	int nQP, int nV, int nC, int nEC,
-								const real_t* const _H, const real_t* const g, const real_t* const _A,
-								const real_t* const lb, const real_t* const ub,
-								const real_t* const lbA, const real_t* const ubA,
-								BooleanType isSparse, 
-								const Options& options, int& nWSR, real_t& maxCPUtime,
-								real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
-								)
-{
-	real_t maxNWSR = 0.0;
-	real_t avgNWSR = 0.0;
-	real_t avgCPUtime = 0.0;
-
-	returnValue returnvalue = solveOQPbenchmark(	nQP,nV,nC,nEC,
-													_H,g,_A,lb,ub,lbA,ubA,
-													isSparse,BT_TRUE,
-													options,nWSR,
-													maxNWSR,avgNWSR,maxCPUtime,avgCPUtime, 
-													maxStationarity,maxFeasibility,maxComplementarity
-													);
-	nWSR = (int)maxNWSR;
-
-	return returnvalue;
-}
-
-
-
-/*
- *	s o l v e O Q P b e n c h m a r k
- */
-returnValue solveOQPbenchmark(	int nQP, int nV, int nC, int nEC,
-								const real_t* const _H, const real_t* const g, const real_t* const _A,
-								const real_t* const lb, const real_t* const ub,
-								const real_t* const lbA, const real_t* const ubA,
-								BooleanType isSparse, BooleanType useHotstarts, 
-								const Options& options, int maxAllowedNWSR,
-								real_t& maxNWSR, real_t& avgNWSR, real_t& maxCPUtime, real_t& avgCPUtime,
-								real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
-								)
-{
-	int k;
-
-	/* I) SETUP AUXILIARY VARIABLES: */
-	/* 1) Keep nWSR and store current and maximum number of
-	 *    working set recalculations in temporary variables */
-	int nWSRcur;
-
-	real_t CPUtimeLimit = maxCPUtime;
-	real_t CPUtimeCur = CPUtimeLimit;
-	maxNWSR = 0.0;
-	avgNWSR = 0.0;
-	maxCPUtime = 0.0;
-	avgCPUtime = 0.0;
-	maxStationarity    = 0.0;
-	maxFeasibility     = 0.0;
-	maxComplementarity = 0.0;
-	real_t stat, feas, cmpl;
-
-	/* 2) Pointers to data of current QP ... */
-	const real_t* gCur;
-	const real_t* lbCur;
-	const real_t* ubCur;
-	const real_t* lbACur;
-	const real_t* ubACur;
-
-	/* 3) Vectors for solution obtained by qpOASES. */
-	real_t* x = new real_t[nV];
-	real_t* y = new real_t[nV+nC];
-	//real_t obj;
-
-	/* 4) Prepare matrix objects */
-	SymmetricMatrix *H; 
-	Matrix *A;
-
-	real_t* H_cpy = new real_t[nV*nV];
-	memcpy( H_cpy,_H, ((unsigned int)(nV*nV))*sizeof(real_t) );
-	real_t* A_cpy = new real_t[nC*nV];
-	memcpy( A_cpy,_A, ((unsigned int)(nC*nV))*sizeof(real_t) );
-
-	if ( isSparse == BT_TRUE )
-	{
-		SymSparseMat *Hs;
-		H = Hs = new SymSparseMat(nV, nV, nV, H_cpy);
-		A = new SparseMatrixRow(nC, nV, nV, A_cpy);
-		Hs->createDiagInfo();
-		delete[] A_cpy; delete[] H_cpy;
-	}
-	else
-	{
-		H = new SymDenseMat(nV, nV, nV, const_cast<real_t *>(H_cpy));
-		A = new DenseMatrix(nC, nV, nV, const_cast<real_t *>(A_cpy));
-	}
-
-	H->doFreeMemory( );
-	A->doFreeMemory( );
-
-	/* II) SETUP QPROBLEM OBJECT */
-	QProblem qp( nV,nC );
-	qp.setOptions( options );
-	//qp.setPrintLevel( PL_LOW );
-
-	//qp.printOptions();
-
-	/* III) RUN BENCHMARK SEQUENCE: */
-	returnValue returnvalue;
-
-	for( k=0; k<nQP; ++k )
-	{
-		//if ( k%50 == 0 )
-		//	printf( "%d\n",k );
-
-		/* 1) Update pointers to current QP data. */
-		gCur   = &( g[k*nV] );
-		lbCur  = &( lb[k*nV] );
-		ubCur  = &( ub[k*nV] );
-		lbACur = &( lbA[k*nC] );
-		ubACur = &( ubA[k*nC] );
-
-		/* 2) Set nWSR and maximum CPU time. */
-		nWSRcur = maxAllowedNWSR;
-		CPUtimeCur = CPUtimeLimit;
-
-		/* 3) Solve current QP. */
-		if ( ( k == 0 ) || ( useHotstarts == BT_FALSE ) )
-		{
-			/* initialise */
-			returnvalue = qp.init( H,gCur,A,lbCur,ubCur,lbACur,ubACur, nWSRcur,&CPUtimeCur );
-			if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
-			{
-				delete A; delete H; delete[] y; delete[] x;
-				return THROWERROR( returnvalue );
-			}
-		}
-		else
-		{
-			/* hotstart */
-			returnvalue = qp.hotstart( gCur,lbCur,ubCur,lbACur,ubACur, nWSRcur,&CPUtimeCur );
-			if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
-			{
-				delete A; delete H; delete[] y; delete[] x;
-				return THROWERROR( returnvalue );
-			}
-		}
-
-		/* 4) Obtain solution vectors and objective function value */
-		qp.getPrimalSolution( x );
-		qp.getDualSolution( y );
-		//obj = qp.getObjVal( );
-
-		/* 5) Compute KKT residuals */
-		getKKTResidual( nV, nC, _H,gCur,_A,lbCur,ubCur,lbACur,ubACur, x, y, stat, feas, cmpl );
-		
-		/* 6) Update maximum and average values. */
-		if ( ((double)nWSRcur) > maxNWSR )
-			maxNWSR = ((double)nWSRcur);
-		if (stat > maxStationarity) maxStationarity = stat;
-		if (feas > maxFeasibility) maxFeasibility = feas;
-		if (cmpl > maxComplementarity) maxComplementarity = cmpl;
-
-		if ( CPUtimeCur > maxCPUtime )
-			maxCPUtime = CPUtimeCur;
-
-		avgNWSR += ((double)nWSRcur);
-		avgCPUtime += CPUtimeCur;
-	}
-	avgNWSR /= ((double)nQP);
-	avgCPUtime /= ((double)nQP);
-
-	delete A; delete H; delete[] y; delete[] x;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s o l v e O Q P b e n c h m a r k
- */
-returnValue solveOQPbenchmark(	int nQP, int nV,
-								const real_t* const _H, const real_t* const g,
-								const real_t* const lb, const real_t* const ub,
-								BooleanType isSparse, 
-								const Options& options, int& nWSR, real_t& maxCPUtime,
-								real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
-								)
-{
-	real_t maxNWSR = 0.0;
-	real_t avgNWSR = 0.0;
-	real_t avgCPUtime = 0.0;
-
-	returnValue returnvalue = solveOQPbenchmark(	nQP,nV,
-													_H,g,lb,ub,
-													isSparse,BT_TRUE,
-													options,nWSR,
-													maxNWSR,avgNWSR,maxCPUtime,avgCPUtime, 
-													maxStationarity,maxFeasibility,maxComplementarity
-													);
-	nWSR = (int)maxNWSR;
-
-	return returnvalue;
-}
-
-
-/*
- *	s o l v e O Q P b e n c h m a r k
- */
-returnValue solveOQPbenchmark(	int nQP, int nV,
-								const real_t* const _H, const real_t* const g,
-								const real_t* const lb, const real_t* const ub,
-								BooleanType isSparse, BooleanType useHotstarts, 
-								const Options& options, int maxAllowedNWSR,
-								real_t& maxNWSR, real_t& avgNWSR, real_t& maxCPUtime, real_t& avgCPUtime,
-								real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
-								)
-{
-	int k;
-
-	/* I) SETUP AUXILIARY VARIABLES: */
-	/* 1) Keep nWSR and store current and maximum number of
-	 *    working set recalculations in temporary variables */
-	int nWSRcur;
-
-	real_t CPUtimeLimit = maxCPUtime;
-	real_t CPUtimeCur = CPUtimeLimit;
-	real_t stat, feas, cmpl;
-	maxNWSR = 0;
-	avgNWSR = 0;
-	maxCPUtime = 0.0;
-	avgCPUtime = 0.0;
-	maxStationarity = 0.0;
-	maxFeasibility = 0.0;
-	maxComplementarity = 0.0;
-
-	/* 2) Pointers to data of current QP ... */
-	const real_t* gCur;
-	const real_t* lbCur;
-	const real_t* ubCur;
-
-	/* 3) Vectors for solution obtained by qpOASES. */
-	real_t* x = new real_t[nV];
-	real_t* y = new real_t[nV];
-	//real_t  obj;
-
-	/* 4) Prepare matrix objects */
-	SymmetricMatrix *H; 
-	real_t* H_cpy = new real_t[nV*nV];
-	memcpy( H_cpy,_H, ((unsigned int)(nV*nV))*sizeof(real_t) );
-
-	if ( isSparse == BT_TRUE )
-	{
-		SymSparseMat *Hs;
-		H = Hs = new SymSparseMat(nV, nV, nV, H_cpy);
-		Hs->createDiagInfo();
-		delete[] H_cpy;
-	}
-	else
-	{
-		H = new SymDenseMat(nV, nV, nV, const_cast<real_t *>(H_cpy));
-	}
-	
-	H->doFreeMemory( );
-
-	/* II) SETUP QPROBLEM OBJECT */
-	QProblemB qp( nV );
-	qp.setOptions( options );
-	//qp.setPrintLevel( PL_LOW );
-
-
-	/* III) RUN BENCHMARK SEQUENCE: */
-	returnValue returnvalue;
-
-	for( k=0; k<nQP; ++k )
-	{
-		//if ( k%50 == 0 )
-		//	printf( "%d\n",k );
-
-		/* 1) Update pointers to current QP data. */
-		gCur   = &( g[k*nV] );
-		lbCur  = &( lb[k*nV] );
-		ubCur  = &( ub[k*nV] );
-
-		/* 2) Set nWSR and maximum CPU time. */
-		nWSRcur = maxAllowedNWSR;
-		CPUtimeCur = CPUtimeLimit;
-
-		/* 3) Solve current QP. */
-		if ( ( k == 0 ) || ( useHotstarts == BT_FALSE ) )
-		{
-			/* initialise */
-			returnvalue = qp.init( H,gCur,lbCur,ubCur, nWSRcur,&CPUtimeCur );
-			if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
-			{
-				delete H; delete[] y; delete[] x;
-				return THROWERROR( returnvalue );
-			}
-		}
-		else
-		{
-			/* hotstart */
-			returnvalue = qp.hotstart( gCur,lbCur,ubCur, nWSRcur,&CPUtimeCur );
-			if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
-			{
-				delete H; delete[] y; delete[] x;
-				return THROWERROR( returnvalue );
-			}
-		}
-
-		/* 4) Obtain solution vectors and objective function value ... */
-		qp.getPrimalSolution( x );
-		qp.getDualSolution( y );
-		//obj = qp.getObjVal( );
-
-		/* 5) Compute KKT residuals */
-		getKKTResidual( nV, _H,gCur,lbCur,ubCur, x,y, stat,feas,cmpl );
-
-		/* 6) update maximum values. */
-		if ( nWSRcur > maxNWSR )
-			maxNWSR = nWSRcur;
-		if (stat > maxStationarity) maxStationarity = stat;
-		if (feas > maxFeasibility) maxFeasibility = feas;
-		if (cmpl > maxComplementarity) maxComplementarity = cmpl;
-
-		if ( CPUtimeCur > maxCPUtime )
-			maxCPUtime = CPUtimeCur;
-
-		avgNWSR += nWSRcur;
-		avgCPUtime += CPUtimeCur;
-	}
-	avgNWSR /= nQP;
-	avgCPUtime /= ((double)nQP);
-
-	delete H; delete[] y; delete[] x;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	r u n O Q P b e n c h m a r k
- */
-returnValue runOQPbenchmark(	const char* path, BooleanType isSparse, const Options& options,
-								int& nWSR, real_t& maxCPUtime,
-								real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
-								)
-{
-	real_t maxNWSR = 0.0;
-	real_t avgNWSR = 0.0;
-	real_t avgCPUtime = 0.0;
-
-	returnValue returnvalue = runOQPbenchmark(	path,isSparse,BT_TRUE,
-												options,nWSR,
-												maxNWSR,avgNWSR,maxCPUtime,avgCPUtime, 
-												maxStationarity,maxFeasibility,maxComplementarity
-												);
-	nWSR = (int)maxNWSR;
-
-	return returnvalue;
-}
-
-
-/*
- *	r u n O Q P b e n c h m a r k
- */
-returnValue runOQPbenchmark(	const char* path, BooleanType isSparse, BooleanType useHotstarts, 
-								const Options& options, int maxAllowedNWSR,
-								real_t& maxNWSR, real_t& avgNWSR, real_t& maxCPUtime, real_t& avgCPUtime,
-								real_t& maxStationarity, real_t& maxFeasibility, real_t& maxComplementarity
-								)
-{
-	int nQP=0, nV=0, nC=0, nEC=0;
-
-	real_t *H, *g, *A, *lb, *ub, *lbA, *ubA;
-
-
-	returnValue returnvalue;
-
-	/* I) SETUP BENCHMARK: */
-	/* 1) Obtain QP sequence dimensions. */
-	if ( readOQPdimensions( path, nQP,nV,nC,nEC ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_BENCHMARK_ABORTED );
-
-	/* 2) Read OQP benchmark data. */
-	if ( readOQPdata(	path,
-						nQP,nV,nC,nEC,
-						&H,&g,&A,&lb,&ub,&lbA,&ubA,
-						0,0,0
-						) != SUCCESSFUL_RETURN )
-	{
-		return THROWERROR( RET_UNABLE_TO_READ_BENCHMARK );
-	}
-
-	// normaliseConstraints( nV,nC,A,lbA,ubA ); //only works when nP==1
-
-	/* II) SOLVE BENCHMARK */
-	if ( nC > 0 )
-	{
-		returnvalue = solveOQPbenchmark(	nQP,nV,nC,nEC,
-											H,g,A,lb,ub,lbA,ubA,
-											isSparse,useHotstarts,
-											options,maxAllowedNWSR,
-											maxNWSR,avgNWSR,maxCPUtime,avgCPUtime,
-											maxStationarity,maxFeasibility,maxComplementarity
-											);
-
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			delete[] H; delete[] A;
-			delete[] ubA; delete[] lbA; delete[] ub; delete[] lb; delete[] g;
-			return THROWERROR( returnvalue );
-		}
-	}
-	else
-	{
-		returnvalue = solveOQPbenchmark(	nQP,nV,
-											H,g,lb,ub,
-											isSparse,useHotstarts,
-											options,maxAllowedNWSR,
-											maxNWSR,avgNWSR,maxCPUtime,avgCPUtime,
-											maxStationarity,maxFeasibility,maxComplementarity
-											);
-
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			delete[] H; delete[] A;
-			delete[] ub; delete[] lb; delete[] g;
-			return THROWERROR( returnvalue );
-		}
-	}
-
-	delete[] H; delete[] A;
-	delete[] ubA; delete[] lbA; delete[] ub; delete[] lb; delete[] g;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/Options.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/Options.cpp
deleted file mode 100644
index 14bc5bf6cf3..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/Options.cpp
+++ /dev/null
@@ -1,553 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file src/Options.cpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of the Options class designed to manage working sets of
- *	constraints and bounds within a QProblem.
- */
-
-
-#include <qpOASES/Options.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	O p t i o n s
- */
-Options::Options( )
-{
-	setToDefault( );
-}
-
-
-/*
- *	O p t i o n s
- */
-Options::Options( const Options& rhs )
-{
-	copy( rhs );
-}
-
-
-/*
- *	~ O p t i o n s
- */
-Options::~Options( )
-{
-}
-
-
-/*
- *	o p e r a t o r =
- */
-Options& Options::operator=( const Options& rhs )
-{
-	if ( this != &rhs )
-	{
-		copy( rhs );
-	}
-
-	return *this;
-}
-
-
-
-/*
- *	s e t T o D e f a u l t
- */
-returnValue Options::setToDefault( )
-{
-	printLevel = PL_MEDIUM;
-	#ifdef __DEBUG__
-	printLevel = PL_HIGH;
-	#endif
-	#ifdef __XPCTARGET__
-	printLevel = PL_NONE;
-	#endif
-	#ifdef __SUPPRESSANYOUTPUT__
-	printLevel = PL_NONE;
-	#endif
-
-	enableRamping                 =  BT_TRUE;
-	enableFarBounds               =  BT_TRUE;
-	enableFlippingBounds          =  BT_TRUE;
-	enableRegularisation          =  BT_FALSE;
-	enableFullLITests             =  BT_FALSE;
-	enableNZCTests                =  BT_TRUE;
-	enableDriftCorrection         =  1;
-	enableCholeskyRefactorisation =  0;
-	enableEqualities              =  BT_FALSE;
-
-	#ifdef __USE_SINGLE_PRECISION__
-	terminationTolerance          =  1.0e2 * EPS;
-	boundTolerance                =  1.0e2 * EPS;
-	#else
-	terminationTolerance          =  5.0e6 * EPS;
-	boundTolerance                =  1.0e6 * EPS;
-	#endif
-	boundRelaxation               =  1.0e4;
-	#ifdef __USE_SINGLE_PRECISION__
-	epsNum                        = -1.0e2 * EPS;
-	epsDen                        =  1.0e2 * EPS;
-	#else
-	epsNum                        = -1.0e3 * EPS;
-	epsDen                        =  1.0e3 * EPS;
-	#endif
-	maxPrimalJump                 =  1.0e8;
-	maxDualJump                   =  1.0e8;
-
-	initialRamping                =  0.5;
-	finalRamping                  =  1.0;
-	initialFarBounds              =  1.0e6;
-	growFarBounds                 =  1.0e3;
- 	initialStatusBounds           =  ST_LOWER;
-	#ifdef __USE_SINGLE_PRECISION__
-	epsFlipping                   =  5.0e1 * EPS;
-	#else
-	epsFlipping                   =  1.0e3 * EPS;
-	#endif
-	numRegularisationSteps        =  0;
-	#ifdef __USE_SINGLE_PRECISION__
-	epsRegularisation             =  2.0e1 * EPS;
-	numRefinementSteps            =  2;
-	#else
-	epsRegularisation             =  1.0e3 * EPS;
-	numRefinementSteps            =  1;
-	#endif
-	epsIterRef                    =  1.0e2 * EPS;
-	#ifdef __USE_SINGLE_PRECISION__
-	epsLITests                    =  5.0e1 * EPS;
-	epsNZCTests                   =  1.0e2 * EPS;
-	#else
-	epsLITests                    =  1.0e5 * EPS;
-	epsNZCTests                   =  3.0e3 * EPS;
-	#endif
-
-	enableDropInfeasibles         =  BT_FALSE;
-    dropBoundPriority             =  1;
-    dropEqConPriority             =  1;
-    dropIneqConPriority           =  1;
-    
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t T o R e l i a b l e
- */
-returnValue Options::setToReliable( )
-{
-	setToDefault( );
-
-	enableFullLITests             =  BT_TRUE;
-	enableCholeskyRefactorisation =  1;
-
-	#ifdef __USE_SINGLE_PRECISION__
-	numRefinementSteps            =  3;
-	#else
-	numRefinementSteps            =  2;
-	#endif
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t T o M P C
- */
-returnValue Options::setToMPC( )
-{
-	setToDefault( );
-
-	enableRamping                 =  BT_FALSE;
-	enableFarBounds               =  BT_TRUE;
-	enableFlippingBounds          =  BT_FALSE;
-	enableRegularisation          =  BT_TRUE;
-	enableNZCTests                =  BT_FALSE;
-	enableDriftCorrection         =  0;
-	enableEqualities              =  BT_TRUE;
-
-	#ifdef __USE_SINGLE_PRECISION__
-	terminationTolerance          =  1.0e3 * EPS;
-	#else
-	terminationTolerance          =  1.0e9 * EPS;
-	#endif
-
-	initialStatusBounds           =  ST_INACTIVE;
-	numRegularisationSteps        =  1;
-	#ifdef __USE_SINGLE_PRECISION__
-	numRefinementSteps            =  2;
-	#else
-	numRefinementSteps            =  0;
-	#endif
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t T o F a s t
- */
-returnValue Options::setToFast( )
-{
-	return setToMPC( );
-}
-
-
-
-/*
- *	e n s u r e C o n s i s t e n c y
- */
-returnValue Options::ensureConsistency( )
-{
-	BooleanType needToAdjust = BT_FALSE;
-
-	/* flipping bounds require far bounds */
-    /* (ckirches) Removed this as per filter's trust region
-	if( enableFlippingBounds == BT_TRUE )
-		enableFarBounds = BT_TRUE;
-    */
-	
-	if( enableDriftCorrection < 0 )
-	{
-		enableDriftCorrection = 0;
-		needToAdjust = BT_TRUE;
-	}
-	
-	if( enableCholeskyRefactorisation < 0 )
-	{
-		enableCholeskyRefactorisation = 0;
-		needToAdjust = BT_TRUE;
-	}
-
-	if ( terminationTolerance <= 0.0 )
-	{
-		terminationTolerance = EPS;
-		needToAdjust = BT_TRUE;
-	}
-
-	if ( epsIterRef <= 0.0 )
-	{
-		epsIterRef = EPS;
-		needToAdjust = BT_TRUE;
-	}
-
-	if ( epsRegularisation <= 0.0 )
-	{
-		epsRegularisation = EPS;
-		needToAdjust = BT_TRUE;
-	}
-
-	if ( boundTolerance <= 0.0 )
-	{
-		boundTolerance = EPS;
-		needToAdjust = BT_TRUE;
-	}
-
-	if ( boundRelaxation <= 0.0 )
-	{
-		boundRelaxation = EPS;
-		needToAdjust = BT_TRUE;
-	}
-	
-	if ( maxPrimalJump <= 0.0 )
-	{
-		maxPrimalJump = EPS;
-		needToAdjust = BT_TRUE;
-	}
-
-	if ( maxDualJump <= 0.0 )
-	{
-		maxDualJump = EPS;
-		needToAdjust = BT_TRUE;
-	}
-
-
-	if ( initialRamping < 0.0 )
-	{
-		initialRamping = 0.0;
-		needToAdjust = BT_TRUE;
-	}
-
-	if ( finalRamping < 0.0 )
-	{
-		finalRamping = 0.0;
-		needToAdjust = BT_TRUE;
-	}
-
-	if ( initialFarBounds <= boundRelaxation )
-	{
-		initialFarBounds = boundRelaxation+EPS;
-		needToAdjust = BT_TRUE;
-	}
-	
-	if ( growFarBounds < 1.1 )
-	{
-		growFarBounds = 1.1;
-		needToAdjust = BT_TRUE;
-	}
-
-	if ( epsFlipping <= 0.0 )
-	{
-		epsFlipping = EPS;
-		needToAdjust = BT_TRUE;
-	}
-
-	if ( numRegularisationSteps < 0 )
-	{
-		numRegularisationSteps = 0;
-		needToAdjust = BT_TRUE;
-	}
-
-	if ( epsRegularisation < 0.0 )
-	{
-		epsRegularisation = EPS;
-		needToAdjust = BT_TRUE;
-	}
-
-	if ( numRefinementSteps < 0 )
-	{
-		numRefinementSteps = 0;
-		needToAdjust = BT_TRUE;
-	}
-
-	if ( epsIterRef < 0.0 )
-	{
-		epsIterRef = EPS;
-		needToAdjust = BT_TRUE;
-	}
-
-	if ( epsLITests < 0.0 )
-	{
-		epsLITests = EPS;
-		needToAdjust = BT_TRUE;
-	}
-
-	if ( epsNZCTests < 0.0 )
-	{
-		epsNZCTests = EPS;
-		needToAdjust = BT_TRUE;
-	}
-
-	if ( needToAdjust == BT_TRUE)
-		return THROWWARNING( RET_OPTIONS_ADJUSTED );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t
- */
-returnValue Options::print( ) const
-{
-	#ifndef __XPCTARGET__
-	#ifndef __DSPACE__
-	char myPrintfString[MAX_STRING_LENGTH];
-	char info[MAX_STRING_LENGTH];
-
-	myPrintf( "\n###################   qpOASES  --  QP OPTIONS   ##################\n" );
-	myPrintf( "\n" );
-
-	convertPrintLevelToString( printLevel,info );
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"printLevel                     =  %s\n",info );
-	myPrintf( myPrintfString );
-
-	myPrintf( "\n" );
-
-	convertBooleanTypeToString( enableRamping,info );
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableRamping                  =  %s\n",info );
-	myPrintf( myPrintfString );
-
-	convertBooleanTypeToString( enableFarBounds,info );
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableFarBounds                =  %s\n",info );
-	myPrintf( myPrintfString );
-
-	convertBooleanTypeToString( enableFlippingBounds,info );
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableFlippingBounds           =  %s\n",info );
-	myPrintf( myPrintfString );
-
-	convertBooleanTypeToString( enableRegularisation,info );
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableRegularisation           =  %s\n",info );
-	myPrintf( myPrintfString );
-
-	convertBooleanTypeToString( enableFullLITests,info );
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableFullLITests              =  %s\n",info );
-	myPrintf( myPrintfString );
-
-	convertBooleanTypeToString( enableNZCTests,info );
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableNZCTests                 =  %s\n",info );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableDriftCorrection          =  %d\n",enableDriftCorrection );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableCholeskyRefactorisation  =  %d\n",enableCholeskyRefactorisation );
-	myPrintf( myPrintfString );
-
-	convertBooleanTypeToString( enableEqualities,info );
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"enableEqualities               =  %s\n",info );
-	myPrintf( myPrintfString );
-
-	myPrintf( "\n" );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"terminationTolerance           =  %e\n",terminationTolerance );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"boundTolerance                 =  %e\n",boundTolerance );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"boundRelaxation                =  %e\n",boundRelaxation );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"epsNum                         =  %e\n",epsNum );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"epsDen                         =  %e\n",epsDen );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"maxPrimalJump                  =  %e\n",maxPrimalJump );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"maxDualJump                    =  %e\n",maxDualJump );
-	myPrintf( myPrintfString );
-
-	myPrintf( "\n" );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"initialRamping                 =  %e\n",initialRamping );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"finalRamping                   =  %e\n",finalRamping );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"initialFarBounds               =  %e\n",initialFarBounds );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"growFarBounds                  =  %e\n",growFarBounds );
-	myPrintf( myPrintfString );
-
-	convertSubjectToStatusToString( initialStatusBounds,info );
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"initialStatusBounds            =  %s\n",info );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"epsFlipping                    =  %e\n",epsFlipping );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"numRegularisationSteps         =  %d\n",numRegularisationSteps );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"epsRegularisation              =  %e\n",epsRegularisation );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"numRefinementSteps             =  %d\n",numRefinementSteps );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"epsIterRef                     =  %e\n",epsIterRef );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"epsLITests                     =  %e\n",epsLITests );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,"epsNZCTests                    =  %e\n",epsNZCTests );
-	myPrintf( myPrintfString );
-
-	myPrintf( "\n\n" );
-	#endif
-	#endif
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-/*
- *	c o p y
- */
-returnValue Options::copy(	const Options& rhs
-							)
-{
-	printLevel             = rhs.printLevel;
-
-	enableRamping                 =  rhs.enableRamping;
-	enableFarBounds               =  rhs.enableFarBounds;
-	enableFlippingBounds          =  rhs.enableFlippingBounds;
-	enableRegularisation          =  rhs.enableRegularisation;
-	enableFullLITests             =  rhs.enableFullLITests;
-	enableNZCTests                =  rhs.enableNZCTests;
-	enableDriftCorrection         =  rhs.enableDriftCorrection;
-	enableCholeskyRefactorisation =  rhs.enableCholeskyRefactorisation;
-	enableEqualities              =  rhs.enableEqualities;
-
-	terminationTolerance          =  rhs.terminationTolerance;
-	boundTolerance                =  rhs.boundTolerance;
-	boundRelaxation               =  rhs.boundRelaxation;
-	epsNum                        =  rhs.epsNum;
-	epsDen                        =  rhs.epsDen;
-	maxPrimalJump                 =  rhs.maxPrimalJump;
-	maxDualJump                   =  rhs.maxDualJump;
-
-	initialRamping                =  rhs.initialRamping;
-	finalRamping                  =  rhs.finalRamping;
-	initialFarBounds              =  rhs.initialFarBounds;
-	growFarBounds                 =  rhs.growFarBounds;
- 	initialStatusBounds           =  rhs.initialStatusBounds;
-	epsFlipping                   =  rhs.epsFlipping;
-	numRegularisationSteps        =  rhs.numRegularisationSteps;
-	epsRegularisation             =  rhs.epsRegularisation;
-	numRefinementSteps            =  rhs.numRefinementSteps;
-	epsIterRef                    =  rhs.epsIterRef;
-	epsLITests                    =  rhs.epsLITests;
-	epsNZCTests                   =  rhs.epsNZCTests;
-
-	enableDropInfeasibles         =  rhs.enableDropInfeasibles;
-    dropBoundPriority             =  rhs.dropBoundPriority;
-    dropEqConPriority             =  rhs.dropEqConPriority;
-    dropIneqConPriority           =  rhs.dropIneqConPriority;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/QProblem.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/QProblem.cpp
deleted file mode 100644
index 14dc07cb962..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/QProblem.cpp
+++ /dev/null
@@ -1,6338 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file src/QProblem.cpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of the QProblem class which is able to use the newly
- *	developed online active set strategy for parametric quadratic programming.
- */
-
-
-#include <qpOASES/QProblem.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	Q P r o b l e m
- */
-QProblem::QProblem( ) : QProblemB( )
-{
-	freeConstraintMatrix = BT_FALSE;
-	A = 0;
-
-	lbA = 0;
-	ubA = 0;
-
-	sizeT = 0;
-	T = 0;
-	Q = 0;
-
-	Ax = 0;
-	Ax_l = 0;
-	Ax_u = 0;
-
-	constraintProduct = 0;
-
-	tempA = 0;
-	ZFR_delta_xFRz = 0;
-	delta_xFRy = 0;
-	delta_xFRz = 0;
-	tempB = 0;
-	delta_yAC_TMP = 0;
-}
-
-
-/*
- *	Q P r o b l e m
- */
-QProblem::QProblem( int _nV, int _nC, HessianType _hessianType ) : QProblemB( _nV,_hessianType )
-{
-	int i;
-
-	/* consistency checks */
-	if ( _nV <= 0 )
-	{
-		_nV = 1;
-		THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	if ( _nC < 0 )
-	{
-		_nC = 0;
-		THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	if ( _nC > 0 )
-	{
-		freeConstraintMatrix = BT_FALSE;
-		A = 0;
-
-		lbA = new real_t[_nC];
-		for( i=0; i<_nC; ++i ) lbA[i] = 0.0;
-
-		ubA = new real_t[_nC];
-		for( i=0; i<_nC; ++i ) ubA[i] = 0.0;
-	}
-	else
-	{
-		/* prevent segmentation faults in case nC == 0
-		 * (avoiding checks for A!=0 around all calls to A->... */
-		freeConstraintMatrix = BT_TRUE;
-		A = new DenseMatrix( );
-
-		lbA = 0;
-		ubA = 0;
-	}
-
-	constraints.init( _nC );
-
-	delete[] y; /* y of no constraints version too short! */
-	y = new real_t[_nV+_nC];
-	for( i=0; i<_nV+_nC; ++i ) y[i] = 0.0;
-
-	sizeT = getMin( _nV,_nC );
-	T = new real_t[sizeT*sizeT];
-	Q = new real_t[_nV*_nV];
-
-	if ( _nC > 0 )
-	{
-		Ax = new real_t[_nC];
-		Ax_l = new real_t[_nC];
-		Ax_u = new real_t[_nC];
-	}
-	else
-	{
-		Ax = 0;
-		Ax_l = 0;
-		Ax_u = 0;
-	}
-
-	constraintProduct = 0;
-
-	tempA = new real_t[_nV];			/* nFR */
-	ZFR_delta_xFRz = new real_t[_nV];	/* nFR */
-	delta_xFRz = new real_t[_nV];		/* nZ */
-
-	if ( _nC > 0 )
-	{
-		tempB = new real_t[_nC];			/* nAC */
-		delta_xFRy = new real_t[_nC];		/* nAC */
-		delta_yAC_TMP = new real_t[_nC];   /* nAC */
-	}
-	else
-	{
-		tempB = 0;
-		delta_xFRy = 0;
-		delta_yAC_TMP = 0;
-	}
-
-	flipper.init( (unsigned int)_nV,(unsigned int)_nC );
-}
-
-
-/*
- *	Q P r o b l e m
- */
-QProblem::QProblem( const QProblem& rhs ) : QProblemB( rhs )
-{
-	freeConstraintMatrix = BT_FALSE;
-	A = 0;
-
-	copy( rhs );
-}
-
-
-/*
- *	~ Q P r o b l e m
- */
-QProblem::~QProblem( )
-{
-	clear( );
-}
-
-
-/*
- *	o p e r a t o r =
- */
-QProblem& QProblem::operator=( const QProblem& rhs )
-{
-	if ( this != &rhs )
-	{
-		clear( );
-		QProblemB::operator=( rhs );
-		copy( rhs );
-	}
-
-	return *this;
-}
-
-
-/*
- *	r e s e t
- */
-returnValue QProblem::reset( )
-{
-	int i;
-	int nV = getNV( );
-	int nC = getNC( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-
-	/* 1) Reset bounds, Cholesky decomposition and status flags. */
-	if ( QProblemB::reset( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_RESET_FAILED );
-
-	/* 2) Reset constraints. */
-	constraints.init( nC );
-
-	/* 3) Reset TQ factorisation. */
-	for( i=0; i<sizeT*sizeT; ++i )
-		T[i] = 0.0;
-
-	for( i=0; i<nV*nV; ++i )
-		Q[i] = 0.0;
-
-	/* 4) Reset constraint product pointer. */
-	constraintProduct = 0;
-
-	/* 5) Reset flipper object */
-	flipper.init( (unsigned int)nV,(unsigned int)nC );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	i n i t
- */
-returnValue QProblem::init(	SymmetricMatrix *_H, const real_t* const _g, Matrix *_A,
-							const real_t* const _lb, const real_t* const _ub,
-							const real_t* const _lbA, const real_t* const _ubA,
-							int& nWSR, real_t* const cputime
-							)
-{
-	if ( getNV( ) == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* 1) Consistency check. */
-	if ( isInitialised( ) == BT_TRUE )
-	{
-		THROWWARNING( RET_QP_ALREADY_INITIALISED );
-		reset( );
-	}
-
-	/* 2) Setup QP data. */
-	if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 3) Call to main initialisation routine (without any additional information). */
-	return solveInitialQP( 0,0,0,0, nWSR,cputime );
-}
-
-
-/*
- *	i n i t
- */
-returnValue QProblem::init(	const real_t* const _H, const real_t* const _g, const real_t* const _A,
-							const real_t* const _lb, const real_t* const _ub,
-							const real_t* const _lbA, const real_t* const _ubA,
-							int& nWSR, real_t* const cputime
-							)
-{
-	if ( getNV( ) == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* 1) Consistency check. */
-	if ( isInitialised( ) == BT_TRUE )
-	{
-		THROWWARNING( RET_QP_ALREADY_INITIALISED );
-		reset( );
-	}
-
-	/* 2) Setup QP data. */
-	if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 3) Call to main initialisation routine (without any additional information). */
-	return solveInitialQP( 0,0,0,0, nWSR,cputime );
-}
-
-
-/*
- *	i n i t
- */
-returnValue QProblem::init(	const char* const H_file, const char* const g_file, const char* const A_file,
-							const char* const lb_file, const char* const ub_file,
-							const char* const lbA_file, const char* const ubA_file,
-							int& nWSR, real_t* const cputime
-							)
-{
-	if ( getNV( ) == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* 1) Consistency check. */
-	if ( isInitialised( ) == BT_TRUE )
-	{
-		THROWWARNING( RET_QP_ALREADY_INITIALISED );
-		reset( );
-	}
-
-	/* 2) Setup QP data from files. */
-	if ( setupQPdataFromFile( H_file,g_file,A_file,lb_file,ub_file,lbA_file,ubA_file ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_UNABLE_TO_READ_FILE );
-
-	/* 3) Call to main initialisation routine (without any additional information). */
-	return solveInitialQP( 0,0,0,0, nWSR,cputime );
-}
-
-
-/*
- *	i n i t
- */
-returnValue QProblem::init(	SymmetricMatrix *_H, const real_t* const _g, Matrix *_A,
-							const real_t* const _lb, const real_t* const _ub,
-							const real_t* const _lbA, const real_t* const _ubA,
-							int& nWSR, real_t* const cputime,
-							const real_t* const xOpt, const real_t* const yOpt,
-							const Bounds* const guessedBounds, const Constraints* const guessedConstraints
-							)
-{
-	int i;
-	int nV = getNV( );
-	int nC = getNC( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* 1) Consistency checks. */
-	if ( isInitialised( ) == BT_TRUE )
-	{
-		THROWWARNING( RET_QP_ALREADY_INITIALISED );
-		reset( );
-	}
-
-	if ( guessedBounds != 0 )
-	{
-		for( i=0; i<nV; ++i )
-		{
-			if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
-				return THROWERROR( RET_INVALID_ARGUMENTS );
-		}
-	}
-
-	if ( guessedConstraints != 0 )
-	{
-		for( i=0; i<nC; ++i )
-			if ( guessedConstraints->getStatus( i ) == ST_UNDEFINED )
-				return THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	/* exclude these possibilities in order to avoid inconsistencies */
-	if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 2) Setup QP data. */
-	if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 3) Call to main initialisation routine. */
-	return solveInitialQP( xOpt,yOpt,guessedBounds,guessedConstraints, nWSR,cputime );
-}
-
-
-/*
- *	i n i t
- */
-returnValue QProblem::init(	const real_t* const _H, const real_t* const _g, const real_t* const _A,
-							const real_t* const _lb, const real_t* const _ub,
-							const real_t* const _lbA, const real_t* const _ubA,
-							int& nWSR, real_t* const cputime,
-							const real_t* const xOpt, const real_t* const yOpt,
-							const Bounds* const guessedBounds, const Constraints* const guessedConstraints
-							)
-{
-	int i;
-	int nV = getNV( );
-	int nC = getNC( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* 1) Consistency checks. */
-	if ( isInitialised( ) == BT_TRUE )
-	{
-		THROWWARNING( RET_QP_ALREADY_INITIALISED );
-		reset( );
-	}
-
-	if ( guessedBounds != 0 )
-	{
-		for( i=0; i<nV; ++i )
-		{
-			if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
-				return THROWERROR( RET_INVALID_ARGUMENTS );
-		}
-	}
-
-	if ( guessedConstraints != 0 )
-	{
-		for( i=0; i<nC; ++i )
-			if ( guessedConstraints->getStatus( i ) == ST_UNDEFINED )
-				return THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	/* exclude these possibilities in order to avoid inconsistencies */
-	if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 2) Setup QP data. */
-	if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 3) Call to main initialisation routine. */
-	return solveInitialQP( xOpt,yOpt,guessedBounds,guessedConstraints, nWSR,cputime );
-}
-
-
-/*
- *	i n i t
- */
-returnValue QProblem::init(	const char* const H_file, const char* const g_file, const char* const A_file,
-							const char* const lb_file, const char* const ub_file,
-							const char* const lbA_file, const char* const ubA_file,
-							int& nWSR, real_t* const cputime,
-							const real_t* const xOpt, const real_t* const yOpt,
-							const Bounds* const guessedBounds, const Constraints* const guessedConstraints
-							)
-{
-	int i;
-	int nV = getNV( );
-	int nC = getNC( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* 1) Consistency checks. */
-	if ( isInitialised( ) == BT_TRUE )
-	{
-		THROWWARNING( RET_QP_ALREADY_INITIALISED );
-		reset( );
-	}
-
-	for( i=0; i<nV; ++i )
-	{
-		if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
-			return THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	for( i=0; i<nC; ++i )
-		if ( guessedConstraints->getStatus( i ) == ST_UNDEFINED )
-			return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* exclude these possibilities in order to avoid inconsistencies */
-	if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 2) Setup QP data from files. */
-	if ( setupQPdataFromFile( H_file,g_file,A_file,lb_file,ub_file,lbA_file,ubA_file ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_UNABLE_TO_READ_FILE );
-
-	/* 3) Call to main initialisation routine. */
-	return solveInitialQP( xOpt,yOpt,guessedBounds,guessedConstraints, nWSR,cputime );
-}
-
-
-/*
- *	h o t s t a r t
- */
-returnValue QProblem::hotstart(	const real_t* const g_new,
-								const real_t* const lb_new, const real_t* const ub_new,
-								const real_t* const lbA_new, const real_t* const ubA_new,
-								int& nWSR, real_t* const cputime
-								)
-{
-	int i, nActiveFar;
-	int nV = getNV ();
-	int nC = getNC ();
-
-	if (nV == 0)
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	returnValue returnvalue = SUCCESSFUL_RETURN;
-
-	/* Simple check for consistency of bounds and constraints. */
-	if ( areBoundsConsistent(lb_new, ub_new, lbA_new, ubA_new) != SUCCESSFUL_RETURN )
-		return setInfeasibilityFlag(returnvalue,BT_TRUE);
-
-	++count;
-
-	int nWSR_max = nWSR;
-	int nWSR_performed = 0;
-
-	real_t cputime_remaining = INFTY, *pcputime_rem;
-	real_t cputime_needed = 0.0;
-
-	real_t farbound = options.initialFarBounds;
-	
-	/* writeQpDataIntoMatFile( "qpData.mat" ); */
-	/* writeQpWorkspaceIntoMatFile( "qpWorkspace.mat" ); */
-	
-	if ( haveCholesky == BT_FALSE )
-	{
-		returnvalue = setupInitialCholesky( );
-		if (returnvalue != SUCCESSFUL_RETURN)
-			return THROWERROR(returnvalue);
-	}
-
-	if ( options.enableFarBounds == BT_FALSE )
-	{
-		/* Automatically call standard solveQP if regularisation is not active. */
-		returnvalue = solveRegularisedQP( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime,0 );
-	}
-	else
-	{
-		real_t *ub_new_far = new real_t[nV];
-		real_t *lb_new_far = new real_t[nV];
-		real_t *ubA_new_far = new real_t[nC];
-		real_t *lbA_new_far = new real_t[nC];
-
-		/* possibly extend initial far bounds to largest bound/constraint data */
-		if (ub_new)
-			for (i = 0; i < nV; i++)
-				if ((ub_new[i] < INFTY) && (ub_new[i] > farbound)) farbound = ub_new[i];
-		if (lb_new)
-			for (i = 0; i < nV; i++)
-				if ((lb_new[i] > -INFTY) && (lb_new[i] < -farbound)) farbound = -lb_new[i];
-		if (ubA_new)
-			for (i = 0; i < nC; i++)
-				if ((ubA_new[i] < INFTY) && (ubA_new[i] > farbound)) farbound = ubA_new[i];
-		if (lbA_new)
-			for (i = 0; i < nC; i++)
-				if ((lbA_new[i] > -INFTY) && (lbA_new[i] < -farbound)) farbound = -lbA_new[i];
-
-		updateFarBounds(	farbound,nV+nC,
-							lb_new,lb_new_far, ub_new,ub_new_far,
-							lbA_new,lbA_new_far, ubA_new,ubA_new_far
-							);
-
-		for ( ;; )
-		{
-			nWSR = nWSR_max;
-			if ( cputime != 0 )
-			{
-				cputime_remaining = *cputime - cputime_needed;
-				pcputime_rem = &cputime_remaining;
-			}
-			else
-				pcputime_rem = 0;
-
-			/* Automatically call standard solveQP if regularisation is not active. */
-			returnvalue = solveRegularisedQP( g_new,lb_new_far,ub_new_far,lbA_new_far,ubA_new_far, nWSR,pcputime_rem,nWSR_performed );
-
-			nWSR_performed  = nWSR;
-			cputime_needed += cputime_remaining;
-
-			/* Check for active far-bounds and move them away */
-			nActiveFar = 0;
-			farbound *= options.growFarBounds;
-
-			if ( infeasible == BT_TRUE )
-			{
-				if ( farbound >= INFTY )
-				{
-					returnvalue = RET_HOTSTART_STOPPED_INFEASIBILITY;
-					break; // goto farewell;   
-				}
-
-				updateFarBounds(	farbound,nV+nC,
-									lb_new,lb_new_far, ub_new,ub_new_far,
-									lbA_new,lbA_new_far, ubA_new,ubA_new_far
-									);
-			}
-			else if ( status == QPS_SOLVED )
-			{
-				real_t tol = farbound/options.growFarBounds * options.boundTolerance;
-
-				for ( i=0; i<nV; ++i )
-				{
-					if ( ( ( lb_new == 0 ) || ( lb_new_far[i] > lb_new[i] ) ) && ( getAbs ( lb_new_far[i] - x[i] ) < tol ) )
-						++nActiveFar;
-					if ( ( ( ub_new == 0 ) || ( ub_new_far[i] < ub_new[i] ) ) && ( getAbs ( ub_new_far[i] - x[i] ) < tol ) )
-						++nActiveFar;
-				}
-				for ( i=0; i<nC; ++i )
-				{
-					if ( ( ( lbA_new == 0 ) || ( lbA_new_far[i] > lbA_new[i] ) ) && ( getAbs ( lbA_new_far[i] - Ax[i] ) < tol ) )
-						++nActiveFar;
-					if ( ( ( ubA_new == 0 ) || ( ubA_new_far[i] < ubA_new[i] ) ) && ( getAbs ( ubA_new_far[i] - Ax[i] ) < tol ) )
-						++nActiveFar;
-				}
-
-				if ( nActiveFar == 0 )
-					break;
-
-				status = QPS_HOMOTOPYQPSOLVED;
-
-				if ( farbound >= INFTY )
-				{
-					unbounded = BT_TRUE;
-					returnvalue = RET_HOTSTART_STOPPED_UNBOUNDEDNESS;
-					goto farewell;
-				}
-
-				updateFarBounds(	farbound,nV+nC,
-									lb_new,lb_new_far, ub_new,ub_new_far,
-									lbA_new,lbA_new_far, ubA_new,ubA_new_far
-									);
-			}
-			else
-			{
-				/* some other error when solving QP */
-				break;
-			}
-
-			/* advance ramp offset to avoid Ramping cycles */
-			rampOffset++;
-		}
-
-		farewell:
-			if ( cputime != 0 )
-				*cputime = cputime_needed;
-			delete[] lbA_new_far; delete[] ubA_new_far;
-			delete[] lb_new_far; delete[] ub_new_far;
-	}
-
-	return ( returnvalue != SUCCESSFUL_RETURN ) ? THROWERROR( returnvalue ) : returnvalue;
-}
-
-
-/*
- *	h o t s t a r t
- */
-returnValue QProblem::hotstart(	const char* const g_file,
-								const char* const lb_file, const char* const ub_file,
-								const char* const lbA_file, const char* const ubA_file,
-								int& nWSR, real_t* const cputime
-								)
-{
-	int nV = getNV( );
-	int nC = getNC( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* consistency check */
-	if ( g_file == 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-
-	/* 1) Allocate memory (if bounds exist). */
-	real_t* g_new  = new real_t[nV];
-	real_t* lb_new = 0;
-	real_t* ub_new = 0;
-	real_t* lbA_new = 0;
-	real_t* ubA_new = 0;
-
-	if ( lb_file != 0 )
-		lb_new = new real_t[nV];
-	if ( ub_file != 0 )
-		ub_new = new real_t[nV];
-	if ( lbA_file != 0 )
-		lbA_new = new real_t[nC];
-	if ( ubA_file != 0 )
-		ubA_new = new real_t[nC];
-
-	/* 2) Load new QP vectors from file. */
-	returnValue returnvalue;
-	returnvalue = loadQPvectorsFromFile(	g_file,lb_file,ub_file,lbA_file,ubA_file,
-											g_new,lb_new,ub_new,lbA_new,ubA_new
-											);
-	if ( returnvalue != SUCCESSFUL_RETURN )
-	{
-		if ( ubA_file != 0 )
-			delete[] ubA_new;
-		if ( lbA_file != 0 )
-			delete[] lbA_new;
-		if ( ub_file != 0 )
-			delete[] ub_new;
-		if ( lb_file != 0 )
-			delete[] lb_new;
-		delete[] g_new;
-
-		return THROWERROR( RET_UNABLE_TO_READ_FILE );
-	}
-
-	/* 3) Actually perform hotstart. */
-	returnvalue = hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
-
-	/* 4) Free memory. */
-	if ( ubA_file != 0 )
-		delete[] ubA_new;
-	if ( lbA_file != 0 )
-		delete[] lbA_new;
-	if ( ub_file != 0 )
-		delete[] ub_new;
-	if ( lb_file != 0 )
-		delete[] lb_new;
-	delete[] g_new;
-
-	return returnvalue;
-}
-
-
-/*
- *	h o t s t a r t
- */
-returnValue QProblem::hotstart(	const real_t* const g_new,
-								const real_t* const lb_new, const real_t* const ub_new,
-								const real_t* const lbA_new, const real_t* const ubA_new,
-								int& nWSR, real_t* const cputime,
-								const Bounds* const guessedBounds, const Constraints* const guessedConstraints
-								)
-{
-	int nV = getNV( );
-	int nC = getNC( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-
-	/* start runtime measurement */
-	real_t starttime = 0.0;
-	if ( cputime != 0 )
-		starttime = getCPUtime( );
-
-
-	/* 1) Update working sets according to guesses for working sets of bounds and constraints. */
-	if ( ( guessedBounds != 0 ) && ( guessedConstraints != 0 ) )
-	{
-		if ( setupAuxiliaryQP( guessedBounds,guessedConstraints ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-	}
-
-	if ( ( guessedBounds == 0 ) && ( guessedConstraints != 0 ) )
-	{
-		/* create empty bounds for setting up auxiliary QP */
-		Bounds emptyBounds( nV );
-		if ( emptyBounds.setupAllFree( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-		if ( setupAuxiliaryQP( &emptyBounds,guessedConstraints ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-	}
-
-	if ( ( guessedBounds != 0 ) && ( guessedConstraints == 0 ) )
-	{
-		/* create empty constraints for setting up auxiliary QP */
-		Constraints emptyConstraints( nC );
-		if ( emptyConstraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-		if ( setupAuxiliaryQP( guessedBounds,&emptyConstraints ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-	}
-
-	if ( ( guessedBounds == 0 ) && ( guessedConstraints == 0 ) )
-	{
-		/* create empty bounds and constraints for setting up auxiliary QP */
-		Bounds emptyBounds( nV );
-		Constraints emptyConstraints( nC );
-		if ( emptyBounds.setupAllFree( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-		if ( emptyConstraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-		if ( setupAuxiliaryQP( &emptyBounds,&emptyConstraints ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-	}
-
-	status = QPS_AUXILIARYQPSOLVED;
-
-
-	/* 2) Perform usual homotopy. */
-
-	/* Allow only remaining CPU time for usual hotstart. */
-	if ( cputime != 0 )
-		*cputime -= getCPUtime( ) - starttime;
-
-	returnValue returnvalue = hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
-
-	/* stop runtime measurement */
-	if ( cputime != 0 )
-		*cputime = getCPUtime( ) - starttime;
-
-	return returnvalue;
-}
-
-
-/*
- *	h o t s t a r t
- */
-returnValue QProblem::hotstart(	const char* const g_file,
-								const char* const lb_file, const char* const ub_file,
-								const char* const lbA_file, const char* const ubA_file,
-								int& nWSR, real_t* const cputime,
-								const Bounds* const guessedBounds, const Constraints* const guessedConstraints
-								)
-{
-	int nV = getNV( );
-	int nC = getNC( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* consistency check */
-	if ( g_file == 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 1) Allocate memory (if bounds exist). */
-	real_t* g_new   = new real_t[nV];
-	real_t* lb_new  = ( lb_file != 0 ) ? new real_t[nV] : 0;
-	real_t* ub_new  = ( ub_file != 0 ) ? new real_t[nV] : 0;
-	real_t* lbA_new = ( lbA_file != 0 ) ? new real_t[nC] : 0;
-	real_t* ubA_new = ( ubA_file != 0 ) ? new real_t[nC] : 0;
-
-	/* 2) Load new QP vectors from file. */
-	returnValue returnvalue;
-	returnvalue = loadQPvectorsFromFile(	g_file,lb_file,ub_file,lbA_file,ubA_file,
-											g_new,lb_new,ub_new,lbA_new,ubA_new
-											);
-	if (returnvalue != SUCCESSFUL_RETURN) {
-		returnvalue = RET_UNABLE_TO_READ_FILE;
-		goto farewell;
-	}
-
-	/* 3) Actually perform hotstart using initialised homotopy. */
-	returnvalue = hotstart(	g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime,
-							guessedBounds,guessedConstraints
-							);
-farewell:
-	/* 4) Free memory. */
-	if ( ubA_file != 0 )
-		delete[] ubA_new;
-	if ( lbA_file != 0 )
-		delete[] lbA_new;
-	if ( ub_file != 0 )
-		delete[] ub_new;
-	if ( lb_file != 0 )
-		delete[] lb_new;
-	delete[] g_new;
-
-	return ( returnvalue != SUCCESSFUL_RETURN ) ? THROWERROR ( returnvalue ) : returnvalue;
-}
-
-
-/*
- *
- */
-returnValue QProblem::solveCurrentEQP(	const int n_rhs,
-										const real_t* g_in,
-										const real_t* lb_in,
-										const real_t* ub_in,
-										const real_t* lbA_in,
-										const real_t* ubA_in,
-										real_t* x_out,
-										real_t* y_out
-										)
-{
-	returnValue returnvalue = SUCCESSFUL_RETURN;
-	int ii, jj;
-	int nV  = getNV( );
-	int nC  = getNC( );
-	int nFR = getNFR( );
-	int nFX = getNFX( );
-	int nAC = getNAC( );
-
-	real_t *delta_xFX = new real_t[nFX];
-	real_t *delta_xFR = new real_t[nFR];
-	real_t *delta_yAC = new real_t[nAC];
-	real_t *delta_yFX = new real_t[nFX];
-
-	/* 1) Determine index arrays. */
-	int* FR_idx;
-	int* FX_idx;
-	int* AC_idx;
-
-	bounds.getFree( )->getNumberArray( &FR_idx );
-	bounds.getFixed( )->getNumberArray( &FX_idx );
-	constraints.getActive( )->getNumberArray( &AC_idx );
-
-	for ( ii = 0 ; ii < (nV+nC)*n_rhs; ++ii )
-		y_out[ii] = 0.0;
-	
-	for ( ii = 0 ; ii < n_rhs; ++ii )
-	{
-		returnvalue = determineStepDirection(
-			g_in, lbA_in, ubA_in, lb_in, ub_in, BT_FALSE, BT_FALSE,
-			delta_xFX, delta_xFR, delta_yAC, delta_yFX );
-
-		for ( jj = 0; jj < nFX; ++jj )
-			x_out[FX_idx[jj]] = delta_xFX[jj];
-		for ( jj = 0; jj < nFR; ++jj )
-			x_out[FR_idx[jj]] = delta_xFR[jj];
-		for ( jj = 0; jj < nFX; ++jj )
-			y_out[FX_idx[jj]] = delta_yFX[jj];
-		for ( jj = 0; jj < nAC; ++jj )
-			y_out[nV+AC_idx[jj]] = delta_yAC[jj];
-
-		g_in += nV;
-		lb_in += nV;
-		ub_in += nV;
-		lbA_in += nC;
-		ubA_in += nC;
-		x_out += nV;
-		y_out += nV+nC;
-	}
-
-
-	delete[] delta_yFX;
-	delete[] delta_yAC;
-	delete[] delta_xFR;
-	delete[] delta_xFX;
-
-	return returnvalue;
-}
-
-
-
-/*
- *	g e t W o r k i n g S e t
- */
-returnValue QProblem::getWorkingSet( real_t* workingSet )
-{
-	int nC = this->getNC();
-	int nV = this->getNV();
-
-	/* At which limit are the bounds active? */
-	QProblemB::getWorkingSet( workingSet );
-
-	/* At which limit are the contraints active? */
-	for (int i = 0; i < nC; i++) {
-		switch (constraints.getStatus(i)) {
-		case ST_LOWER:
-			workingSet[i + nV] = -1.0;
-			break;
-		case ST_UPPER:
-			workingSet[i + nV] = +1.0;
-			break;
-		default:
-			workingSet[i + nV] =  0.0;
-			break;
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t N Z
- */
-int QProblem::getNZ( ) const
-{
-	/* nZ = nFR - nAC */
-	return getNFR( ) - getNAC( );
-}
-
-
-/*
- *	g e t D u a l S o l u t i o n
- */
-returnValue QProblem::getDualSolution( real_t* const yOpt ) const
-{
-	int i;
-
-	for( i=0; i<getNV( )+getNC( ); ++i )
-		yOpt[i] = y[i];
-
-	/* return optimal dual solution vector
-	 * only if current QP has been solved */
-	if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
-		 ( getStatus( ) == QPS_SOLVED ) )
-	{
-		return SUCCESSFUL_RETURN;
-	}
-	else
-	{
-		return RET_QP_NOT_SOLVED;
-	}
-}
-
-
-
-/*
- *	s e t C o n s t r a i n t P r o d u c t
- */
-returnValue QProblem::setConstraintProduct( ConstraintProduct* const _constraintProduct )
-{
-	constraintProduct = _constraintProduct;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t P r o p e r t i e s
- */
-returnValue QProblem::printProperties( )
-{
-	#ifndef __XPCTARGET__
-	/* Do not print properties if print level is set to none! */
-	if ( options.printLevel == PL_NONE )
-		return SUCCESSFUL_RETURN;
-
-	char myPrintfString[MAX_STRING_LENGTH];
-
-	myPrintf( "\n#################   qpOASES  --  QP PROPERTIES   #################\n" );
-	myPrintf( "\n" );
-
-	/* 1) Variables properties. */
-	snprintf( myPrintfString,MAX_STRING_LENGTH,  "Number of Variables: %4.1d\n",getNV( ) );
-	myPrintf( myPrintfString );
-
-	if ( bounds.hasNoLower( ) == BT_TRUE )
-			myPrintf( "Variables are not bounded from below.\n" );
-		else
-			myPrintf( "Variables are bounded from below.\n" );
-
-	if ( bounds.hasNoUpper( ) == BT_TRUE )
-			myPrintf( "Variables are not bounded from above.\n" );
-		else
-			myPrintf( "Variables are bounded from above.\n" );
-
-	myPrintf( "\n" );
-
-
-	/* 2) Constraints properties. */
-	snprintf( myPrintfString,MAX_STRING_LENGTH,  "Total number of Constraints:      %4.1d\n",getNC( ) );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,  "Number of Equality Constraints:   %4.1d\n",getNEC( ) );
-	myPrintf( myPrintfString );
-
-	snprintf( myPrintfString,MAX_STRING_LENGTH,  "Number of Inequality Constraints: %4.1d\n",getNC( )-getNEC( ) );
-	myPrintf( myPrintfString );
-
-	if ( getNC( ) > 0 )
-	{
-		if ( constraints.hasNoLower( ) == BT_TRUE )
-				myPrintf( "Constraints are not bounded from below.\n" );
-			else
-				myPrintf( "Constraints are bounded from below.\n" );
-
-		if ( constraints.hasNoUpper( ) == BT_TRUE )
-				myPrintf( "Constraints are not bounded from above.\n" );
-			else
-				myPrintf( "Constraints are bounded from above.\n" );
-	}
-
-	myPrintf( "\n" );
-
-
-	/* 3) Further properties. */
-	switch ( hessianType )
-	{
-		case HST_ZERO:
-			myPrintf( "Hessian is zero matrix (i.e. actually an LP is solved).\n" );
-			break;
-
-		case HST_IDENTITY:
-			myPrintf( "Hessian is identity matrix.\n" );
-			break;
-
-		case HST_POSDEF:
-			myPrintf( "Hessian matrix is (strictly) positive definite.\n" );
-			break;
-
-		case HST_POSDEF_NULLSPACE:
-			myPrintf( "Hessian matrix is positive definite on null space of active constraints.\n" );
-			break;
-
-		case HST_SEMIDEF:
-			myPrintf( "Hessian matrix is positive semi-definite.\n" );
-			break;
-
-		case HST_INDEF:
-			myPrintf( "Hessian matrix is indefinite.\n" );
-			break;
-
-		default:
-			myPrintf( "Hessian matrix has unknown type.\n" );
-			break;
-	}
-
-	if ( infeasible == BT_TRUE )
-		myPrintf( "QP was found to be infeasible.\n" );
-	else
-		myPrintf( "QP seems to be feasible.\n" );
-
-	if ( unbounded == BT_TRUE )
-		myPrintf( "QP was found to be unbounded from below.\n" );
-	else
-		myPrintf( "QP seems to be bounded from below.\n" );
-
-	myPrintf( "\n" );
-
-
-	/* 4) QP object properties. */
-	switch ( status )
-	{
-		case QPS_NOTINITIALISED:
-			myPrintf( "Status of QP object: freshly instantiated or reset.\n" );
-			break;
-
-		case QPS_PREPARINGAUXILIARYQP:
-			myPrintf( "Status of QP object: an auxiliary QP is currently setup.\n" );
-			break;
-
-		case QPS_AUXILIARYQPSOLVED:
-			myPrintf( "Status of QP object: an auxilary QP was solved.\n" );
-			break;
-
-		case QPS_PERFORMINGHOMOTOPY:
-			myPrintf( "Status of QP object: a homotopy step is performed.\n" );
-			break;
-
-		case QPS_HOMOTOPYQPSOLVED:
-			myPrintf( "Status of QP object: an intermediate QP along the homotopy path was solved.\n" );
-			break;
-
-		case QPS_SOLVED:
-			myPrintf( "Status of QP object: solution of the actual QP was found.\n" );
-			break;
-	}
-
-	switch ( options.printLevel )
-	{
-		case PL_DEBUG_ITER:
-			myPrintf( "Print level of QP object is set to display a tabular output for debugging.\n" );
-			break;
-
-		case PL_TABULAR:
-			myPrintf( "Print level of QP object is set to display a tabular output.\n" );
-			break;
-
-		case PL_LOW:
-			myPrintf( "Print level of QP object is low, i.e. only error are printed.\n" );
-			break;
-
-		case PL_MEDIUM:
-			myPrintf( "Print level of QP object is medium, i.e. error and warnings are printed.\n" );
-			break;
-
-		case PL_HIGH:
-			myPrintf( "Print level of QP object is high, i.e. all available output is printed.\n" );
-			break;
-
-		default:
-			break;
-	}
-
-	myPrintf( "\n" );
-	#endif
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-/*
- *	c l e a r
- */
-returnValue QProblem::clear( )
-{
-	if ( ( freeConstraintMatrix == BT_TRUE ) && ( A != 0 ) )
-	{
-		delete A;
-		A = 0;
-	}
-
-	if ( lbA != 0 )
-	{
-		delete[] lbA;
-		lbA = 0;
-	}
-
-	if ( ubA != 0 )
-	{
-		delete[] ubA;
-		ubA = 0;
-	}
-
-	if ( T != 0 )
-	{
-		delete[] T;
-		T = 0;
-	}
-
-	if ( Q != 0 )
-	{
-		delete[] Q;
-		Q = 0;
-	}
-
-	if ( Ax != 0 )
-	{
-		delete[] Ax;
-		Ax = 0;
-	}
-
-	if ( Ax_l != 0 )
-	{
-		delete[] Ax_l;
-		Ax_l = 0;
-	}
-
-	if ( Ax_u != 0 )
-	{
-		delete[] Ax_u;
-		Ax_u = 0;
-	}
-
-	if ( tempA != 0 )
-	{
-		delete[] tempA;
-		tempA = 0;
-	}
-
-	if ( ZFR_delta_xFRz != 0 )
-	{
-		delete[] ZFR_delta_xFRz;
-		ZFR_delta_xFRz = 0;
-	}
-
-	if ( delta_xFRy != 0 )
-	{
-		delete[] delta_xFRy;
-		delta_xFRy = 0;
-	}
-
-	if ( delta_xFRz != 0 )
-	{
-		delete[] delta_xFRz;
-		delta_xFRz = 0;
-	}
-
-	if ( tempB != 0 )
-	{
-		delete[] tempB;
-		tempB = 0;
-	}
-
-	if ( delta_yAC_TMP != 0 )
-	{
-		delete[] delta_yAC_TMP;
-		delta_yAC_TMP = 0;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	c o p y
- */
-returnValue QProblem::copy(	const QProblem& rhs
-							)
-{
-	unsigned int _nV = (unsigned int)rhs.getNV( );
-	unsigned int _nC = (unsigned int)rhs.getNC( );
-
-	constraints = rhs.constraints;
-
-	if ( ( freeConstraintMatrix == BT_TRUE ) && ( A != 0 ) )
-	{
-		delete A;
-		A = 0;
-	}
-
-	freeConstraintMatrix = rhs.freeConstraintMatrix;
-
-	if ( freeConstraintMatrix == BT_TRUE )
-		A = rhs.A->duplicate();
-	else
-		A = rhs.A;
-
-	if ( rhs.lbA != 0 )
-	{
-		lbA = new real_t[_nC];
-		setLBA( rhs.lbA );
-	}
-	else
-		lbA = 0;
-
-	if ( rhs.ubA != 0 )
-	{
-		ubA = new real_t[_nC];
-		setUBA( rhs.ubA );
-	}
-	else
-		ubA = 0;
-
-	if ( rhs.y != 0 )
-	{
-		delete[] y; /* y of no constraints version too short! */
-		y = new real_t[_nV+_nC];
-		memcpy( y,rhs.y,(_nV+_nC)*sizeof(real_t) );
-	}
-	else
-		y = 0;
-
-	sizeT = rhs.sizeT;
-
-	if ( rhs.T != 0 )
-	{
-		T = new real_t[sizeT*sizeT];
-		memcpy( T,rhs.T,((unsigned int)(sizeT*sizeT))*sizeof(real_t) );
-	}
-	else
-		T = 0;
-
-	if ( rhs.Q != 0 )
-	{
-		Q = new real_t[_nV*_nV];
-		memcpy( Q,rhs.Q,_nV*_nV*sizeof(real_t) );
-	}
-	else
-		Q = 0;
-
-	if ( rhs.Ax != 0 )
-	{
-		Ax = new real_t[_nC];
-		memcpy( Ax,rhs.Ax,_nC*sizeof(real_t) );
-	}
-	else
-		Ax = 0;
-
-	if ( rhs.Ax_l != 0 )
-	{
-		Ax_l = new real_t[_nC];
-		memcpy( Ax_l,rhs.Ax_l,_nC*sizeof(real_t) );
-	}
-	else
-		Ax_l = 0;
-
-	if ( rhs.Ax_u != 0 )
-	{
-		Ax_u = new real_t[_nC];
-		memcpy( Ax_u,rhs.Ax_u,_nC*sizeof(real_t) );
-	}
-	else
-		Ax_u = 0;
-
-	if ( rhs.constraintProduct != 0 )
-		constraintProduct = rhs.constraintProduct;
-	else
-		constraintProduct = 0;
-
-	tempA = new real_t[_nV];			/* nFR */
-	ZFR_delta_xFRz = new real_t[_nV];	/* nFR */
-	delta_xFRz = new real_t[_nV];		/* nZ */
-
-	if ( _nC > 0 )
-	{
-		delta_xFRy = new real_t[_nC];		/* nAC */
-		tempB = new real_t[_nC];			/* nAC */
-		delta_yAC_TMP = new real_t[_nC];   /* nAC */
-	}
-	else
-	{
-		delta_xFRy = 0;
-		tempB = 0;
-		delta_yAC_TMP = 0;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	s o l v e I n i t i a l Q P
- */
-returnValue QProblem::solveInitialQP(	const real_t* const xOpt, const real_t* const yOpt,
-										const Bounds* const guessedBounds, const Constraints* const guessedConstraints,
-										int& nWSR, real_t* const cputime
-										)
-{
-	int i;
-
-	/* some definitions */
-	int nV = getNV( );
-	int nC = getNC( );
-
-	//writeQpDataIntoMatFile( "qpData.mat" );
-
-	/* start runtime measurement */
-	real_t starttime = 0.0;
-	if ( cputime != 0 )
-		starttime = getCPUtime( );
-
-	status = QPS_NOTINITIALISED;
-
-
-	/* I) ANALYSE QP DATA: */
-	/* 1) Check if Hessian happens to be the identity matrix. */
-	if ( determineHessianType( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 2) Setup type of bounds and constraints (i.e. unbounded, implicitly fixed etc.). */
-	if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	status = QPS_PREPARINGAUXILIARYQP;
-
-
-	/* II) SETUP AUXILIARY QP WITH GIVEN OPTIMAL SOLUTION: */
-	/* 1) Setup bounds and constraints data structure. */
-	if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 2) Setup optimal primal/dual solution for auxiliary QP. */
-	if ( setupAuxiliaryQPsolution( xOpt,yOpt ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 3) Obtain linear independent working set for auxiliary QP. */
-	Bounds auxiliaryBounds( nV );
-	Constraints auxiliaryConstraints( nC );
-
-	if ( obtainAuxiliaryWorkingSet(	xOpt,yOpt,guessedBounds,guessedConstraints,
-									&auxiliaryBounds,&auxiliaryConstraints ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 4) Setup working set of auxiliary QP and setup matrix factorisations. */
-	/* a) Regularise Hessian if necessary. */
-	if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_SEMIDEF ) )
-	{
-		if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_INIT_FAILED_REGULARISATION );
-	}
-
-	/* b) TQ factorisation. */
-	if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED_TQ );
-
-	/* c) Working set of auxiliary QP. */
-	if ( setupAuxiliaryWorkingSet( &auxiliaryBounds,&auxiliaryConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	haveCholesky = BT_FALSE;
-
-	/* 5) Store original QP formulation... */
-	real_t* g_original = new real_t[nV];
-	real_t* lb_original = new real_t[nV];
-	real_t* ub_original = new real_t[nV];
-	real_t* lbA_original = new real_t[nC];
-	real_t* ubA_original = new real_t[nC];
-
-	for( i=0; i<nV; ++i )
-	{
-		g_original[i] = g[i];
-		lb_original[i] = lb[i];
-		ub_original[i] = ub[i];
-	}
-
-	for( i=0; i<nC; ++i )
-	{
-		lbA_original[i] = lbA[i];
-		ubA_original[i] = ubA[i];
-	}
-
-	/* ... and setup QP data of an auxiliary QP having an optimal solution
-	 * as specified by the user (or xOpt = yOpt = 0, by default). */
-	if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
-	{
-		delete[] ubA_original; delete[] lbA_original; delete[] ub_original; delete[] lb_original; delete[] g_original;
-		return THROWERROR( RET_INIT_FAILED );
-	}
-
-	if ( setupAuxiliaryQPbounds( &auxiliaryBounds,&auxiliaryConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
-	{
-		delete[] ubA_original; delete[] lbA_original; delete[] ub_original; delete[] lb_original; delete[] g_original;
-		return THROWERROR( RET_INIT_FAILED );
-	}
-
-	status = QPS_AUXILIARYQPSOLVED;
-
-
-	if ( options.enableRamping == BT_TRUE )
-		performRamping( );
-
-
-	/* III) SOLVE ACTUAL INITIAL QP: */
-	/* Allow only remaining CPU time for usual hotstart. */
-	if ( cputime != 0 )
-		*cputime -= getCPUtime( ) - starttime;
-
-	/* Use hotstart method to find the solution of the original initial QP,... */
-	returnValue returnvalue = hotstart( g_original,lb_original,ub_original,lbA_original,ubA_original, nWSR,cputime );
-
-	/* ... deallocate memory,... */
-	delete[] ubA_original; delete[] lbA_original; delete[] ub_original; delete[] lb_original; delete[] g_original;
-
-	/* ... check for infeasibility and unboundedness... */
-	if ( isInfeasible( ) == BT_TRUE )
-		return THROWERROR( RET_INIT_FAILED_INFEASIBILITY );
-
-	if ( isUnbounded( ) == BT_TRUE )
-		return THROWERROR( RET_INIT_FAILED_UNBOUNDEDNESS );
-
-	/* ... and internal errors. */
-	if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
-		return THROWERROR( RET_INIT_FAILED_HOTSTART );
-
-
-	/* stop runtime measurement */
-	if ( cputime != 0 )
-		*cputime = getCPUtime( ) - starttime;
-
-	THROWINFO( RET_INIT_SUCCESSFUL );
-
-	return returnvalue;
-}
-
-
-/*
- *	s o l v e Q P
- */
-returnValue QProblem::solveQP(	const real_t* const g_new,
-								const real_t* const lb_new, const real_t* const ub_new,
-								const real_t* const lbA_new, const real_t* const ubA_new,
-								int& nWSR, real_t* const cputime, int nWSRperformed
-								)
-{
-	int iter;
-	int nV  = getNV( );
-	int nC  = getNC( );
-
-	returnValue returnvalue;
-
-	/* consistency check */
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
-		 ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
-		 ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
-	{
-		return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
-	}
-
-	/* start runtime measurement */
-	real_t starttime = 0.0;
-	if ( cputime != 0 )
-		starttime = getCPUtime( );
-
-	/* I) PREPARATIONS */
-	/* 1) Allocate delta vectors of gradient and (constraints') bounds,
-	 *    index arrays and step direction arrays. */
-	real_t* delta_xFR = new real_t[nV];
-	real_t* delta_xFX = new real_t[nV];
-	real_t* delta_yAC = new real_t[nC];
-	real_t* delta_yFX = new real_t[nV];
-
-	real_t* delta_g   = new real_t[nV];
-	real_t* delta_lb  = new real_t[nV];
-	real_t* delta_ub  = new real_t[nV];
-	real_t* delta_lbA = new real_t[nC];
-	real_t* delta_ubA = new real_t[nC];
-
-	BooleanType Delta_bC_isZero, Delta_bB_isZero;
-
-	int BC_idx;
-	SubjectToStatus BC_status;
-	BooleanType BC_isBound;
-
-	real_t homotopyLength;
-
-	#ifndef __XPCTARGET__
-	char messageString[MAX_STRING_LENGTH];
-	#endif
-
-
-	/* 2) Update type of bounds and constraints, e.g.
-	 *    a former equality constraint might have become a normal one etc. */
-
-  // (ckirches) disabled this, as inactive but tight bounds may become inactive equalities
-    //            which would then never become active again!
-/*
-	if ( setupSubjectToType( lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_HOTSTART_FAILED );
-*/
-
-	/* 3) Reset status flags. */
-	infeasible = BT_FALSE;
-	unbounded  = BT_FALSE;
-
-
-	/* II) MAIN HOMOTOPY LOOP */
-	for( iter=nWSRperformed; iter<nWSR; ++iter )
-	{
-		tabularOutput.idxAddB = tabularOutput.idxRemB = tabularOutput.idxAddC = tabularOutput.idxRemC = -1;
-		tabularOutput.excAddB = tabularOutput.excRemB = tabularOutput.excAddC = tabularOutput.excRemC = 0;
-
-		if ( isCPUtimeLimitExceeded( cputime,starttime,iter-nWSRperformed ) == BT_TRUE )
-		{
-			/* If CPU time limit is exceeded, stop homotopy loop immediately!
-			* Assign number of working set recalculations (runtime measurement is stopped later). */
-			nWSR = iter;
-			break;
-		}
-
-		status = QPS_PERFORMINGHOMOTOPY;
-
-		#ifndef __XPCTARGET__
-		snprintf( messageString,MAX_STRING_LENGTH,"%d ...",iter );
-		getGlobalMessageHandler( )->throwInfo( RET_ITERATION_STARTED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		#endif
-
-		/* 2) Determination of shift direction of the gradient and the (constraints') bounds. */
-		returnvalue = determineDataShift(	g_new,lbA_new,ubA_new,lb_new,ub_new,
-											delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,
-											Delta_bC_isZero, Delta_bB_isZero
-											);
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
-			delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
-
-			/* Assign number of working set recalculations and stop runtime measurement. */
-			nWSR = iter;
-			if ( cputime != 0 )
-				*cputime = getCPUtime( ) - starttime;
-
-			THROWERROR( RET_SHIFT_DETERMINATION_FAILED );
-			return returnvalue;
-		}
-
-		/* 3) Determination of step direction of X and Y. */
-		returnvalue = determineStepDirection(	delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,
-												Delta_bC_isZero, Delta_bB_isZero,
-												delta_xFX,delta_xFR,delta_yAC,delta_yFX
-												);
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
-			delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
-
-			/* Assign number of working set recalculations and stop runtime measurement. */
-			nWSR = iter;
-			if ( cputime != 0 )
-				*cputime = getCPUtime( ) - starttime;
-
-			THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
-			return returnvalue;
-		}
-
-		/* 4) Determination of step length TAU.
-		 *    This step along the homotopy path is also taken (without changing working set). */
-		returnvalue = performStep(	delta_g, delta_lbA,delta_ubA,delta_lb,delta_ub,
-									delta_xFX,delta_xFR,delta_yAC,delta_yFX,
-									BC_idx,BC_status,BC_isBound
-									);
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
-			delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
-
-			/* Assign number of working set recalculations and stop runtime measurement. */
-			nWSR = iter;
-			if ( cputime != 0 )
-				*cputime = getCPUtime( ) - starttime;
-
-			THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );
-			return returnvalue;
-		}
-
-		/* 5) Termination criterion. */
-		nV = getNV( );
-		nC = getNC( );
-
-		homotopyLength = getRelativeHomotopyLength( g_new,lb_new,ub_new,lbA_new,ubA_new );
-		if ( homotopyLength <= options.terminationTolerance )
-		{
-			status = QPS_SOLVED;
-
-			THROWINFO( RET_OPTIMAL_SOLUTION_FOUND );
-
-			if ( printIteration( iter,BC_idx,BC_status,BC_isBound,homotopyLength ) != SUCCESSFUL_RETURN )
-				THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
-
-			nWSR = iter;
-			if ( cputime != 0 )
-				*cputime = getCPUtime( ) - starttime;
-
-			delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
-			delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
-
-			return SUCCESSFUL_RETURN;
-		}
-
-		/* 6) Change active set. */
-		returnvalue = changeActiveSet( BC_idx,BC_status,BC_isBound );
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
-			delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
-
-			/* Assign number of working set recalculations and stop runtime measurement. */
-			nWSR = iter;
-			if ( cputime != 0 )
-				*cputime = getCPUtime( ) - starttime;
-
-			/* Checks for infeasibility... */
-			if ( isInfeasible( ) == BT_TRUE )
-			{
-				status = QPS_HOMOTOPYQPSOLVED;
-				return setInfeasibilityFlag( RET_HOTSTART_STOPPED_INFEASIBILITY );
-			}
-
-			/* ...unboundedness... */
-			if ( unbounded == BT_TRUE ) /* not necessary since objective function convex! */
-				return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );
-
-			/* ... and throw unspecific error otherwise */
-			THROWERROR( RET_HOMOTOPY_STEP_FAILED );
-			return returnvalue;
-		}
-
-		/* 6a) Possibly refactorise projected Hessian from scratch. */
-		if ( ( options.enableCholeskyRefactorisation > 0 ) && ( (iter % options.enableCholeskyRefactorisation) == 0 ) )
-		{
-			returnvalue = computeProjectedCholesky( );
-			if (returnvalue != SUCCESSFUL_RETURN)
-			{
-				delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
-				delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
-				return returnvalue;
-			}
-		}
-
-		/* 7) Output information of successful QP iteration. */
-		status = QPS_HOMOTOPYQPSOLVED;
-
-		if ( printIteration( iter,BC_idx,BC_status,BC_isBound,homotopyLength ) != SUCCESSFUL_RETURN )
-			THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
-
-		/* 8) Perform Ramping Strategy on zero homotopy step or drift correction (if desired). */
-		if (BC_status != ST_UNDEFINED)
-		{
-			if ( ( tau <= EPS ) && ( options.enableRamping == BT_TRUE ) )
-				performRamping( );
-			else
-			if ( (options.enableDriftCorrection > 0) && ((iter+1) % options.enableDriftCorrection == 0) )
-				performDriftCorrection( );  /* always returns SUCCESSFUL_RETURN */
-		}
-	}
-
-	delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
-	delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
-
-	/* stop runtime measurement */
-	if ( cputime != 0 )
-		*cputime = getCPUtime( ) - starttime;
-
-
-	/* if program gets to here, output information that QP could not be solved
-	 * within the given maximum numbers of working set changes */
-	if ( options.printLevel == PL_HIGH )
-	{
-		#ifndef __XPCTARGET__
-		snprintf( messageString,MAX_STRING_LENGTH,"(nWSR = %d)",iter );
-		return getGlobalMessageHandler( )->throwWarning( RET_MAX_NWSR_REACHED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		#else
-		return RET_MAX_NWSR_REACHED;
-		#endif
-	}
-	else
-	{
-		return RET_MAX_NWSR_REACHED;
-	}
-}
-
-
-/*
- *	s o l v e R e g u l a r i s e d Q P
- */
-returnValue QProblem::solveRegularisedQP(	const real_t* const g_new,
-											const real_t* const lb_new, const real_t* const ub_new,
-											const real_t* const lbA_new, const real_t* const ubA_new,
-											int& nWSR, real_t* const cputime, int nWSRperformed
-											)
-{
-	int i, step;
-	int nV = getNV( );
-
-
-	/* Perform normal QP solution if QP has not been regularised. */
-	if ( usingRegularisation( ) == BT_FALSE )
-		return solveQP( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime,nWSRperformed );
-
-
-	/* I) SOLVE USUAL REGULARISED QP */
-	returnValue returnvalue;
-
-	int nWSR_max   = nWSR;
-	int nWSR_total = nWSRperformed;
-
-	real_t cputime_total = 0.0;
-	real_t cputime_cur   = 0.0;
-
-	if ( cputime == 0 )
-	{
-		returnvalue = solveQP( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0,nWSRperformed );
-	}
-	else
-	{
-		cputime_cur = *cputime;
-		returnvalue = solveQP( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,&cputime_cur,nWSRperformed );
-	}
-	nWSR_total     = nWSR;
-	cputime_total += cputime_cur;
-
-	/* Only continue if QP solution has been successful. */
-	if ( returnvalue != SUCCESSFUL_RETURN )
-	{
-		if ( cputime != 0 )
-			*cputime = cputime_total;
-
-		if ( returnvalue == RET_MAX_NWSR_REACHED )
-			THROWWARNING( RET_NO_REGSTEP_NWSR );
-
-		return returnvalue;
-	}
-
-
-	/* II) PERFORM SUCCESSIVE REGULARISATION STEPS */
-	real_t* gMod = new real_t[nV];
-
-	for( step=0; step<options.numRegularisationSteps; ++step )
-	{
-		/* 1) Modify gradient: gMod = g - eps*xOpt
-		 *    (assuming regularisation matrix to be regVal*Id). */
-		for( i=0; i<nV; ++i )
-			gMod[i] = g_new[i] - regVal*x[i];
-
-		/* 2) Solve regularised QP with modified gradient allowing
-		 *    only as many working set recalculations and CPU time
-		 *    as have been left from previous QP solutions. */
-		nWSR = nWSR_max;
-
-		if ( cputime == 0 )
-		{
-			returnvalue = solveQP( gMod,lb_new,ub_new,lbA_new,ubA_new, nWSR,0,nWSR_total );
-		}
-		else
-		{
-			cputime_cur = *cputime - cputime_total;
-			returnvalue = solveQP( gMod,lb_new,ub_new,lbA_new,ubA_new, nWSR,&cputime_cur,nWSR_total );
-		}
-
-		nWSR_total     = nWSR;
-		cputime_total += cputime_cur;
-
-		/* Only continue if QP solution has been successful. */
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			delete[] gMod;
-
-			if ( cputime != 0 )
-				*cputime = cputime_total;
-
-			if ( returnvalue == RET_MAX_NWSR_REACHED )
-				THROWWARNING( RET_FEWER_REGSTEPS_NWSR );
-
-			return returnvalue;
-		}
-	}
-
-	delete[] gMod;
-
-	if ( cputime != 0 )
-		*cputime = cputime_total;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p S u b j e c t T o T y p e
- */
-returnValue QProblem::setupSubjectToType( )
-{
-	return setupSubjectToType( lb,ub,lbA,ubA );
-}
-
-
-/*
- *	s e t u p S u b j e c t T o T y p e
- */
-returnValue QProblem::setupSubjectToType(	const real_t* const lb_new, const real_t* const ub_new,
-											const real_t* const lbA_new, const real_t* const ubA_new
-											)
-{
-	int i;
-	int nC = getNC( );
-
-
-	/* I) SETUP SUBJECTTOTYPE FOR BOUNDS */
-	if ( QProblemB::setupSubjectToType( lb_new,ub_new ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_SETUPSUBJECTTOTYPE_FAILED );
-
-
-	/* II) SETUP SUBJECTTOTYPE FOR CONSTRAINTS */
-	/* 1) Check if lower constraints' bounds are present. */
-	constraints.setNoLower( BT_TRUE );
-	if ( lbA_new != 0 )
-	{
-		for( i=0; i<nC; ++i )
-		{
-			if ( lbA_new[i] > -INFTY )
-			{
-				constraints.setNoLower( BT_FALSE );
-				break;
-			}
-		}
-	}
-
-	/* 2) Check if upper constraints' bounds are present. */
-	constraints.setNoUpper( BT_TRUE );
-	if ( ubA_new != 0 )
-	{
-		for( i=0; i<nC; ++i )
-		{
-			if ( ubA_new[i] < INFTY )
-			{
-				constraints.setNoUpper( BT_FALSE );
-				break;
-			}
-		}
-	}
-
-	/* 3) Determine implicit equality constraints and unbounded constraints. */
-	if ( ( lbA_new != 0 ) && ( ubA_new != 0 ) )
-	{
-		for( i=0; i<nC; ++i )
-		{
-			if (constraints.getType (i) == ST_DISABLED)
-				continue;
-
-			if ( ( lbA_new[i] <= -INFTY ) && ( ubA_new[i] >= INFTY )
-					&& (options.enableFarBounds == BT_FALSE))
-			{
-				constraints.setType( i,ST_UNBOUNDED );
-			}
-			else
-			{
-				if ( options.enableEqualities && lbA[i] > ubA[i] - options.boundTolerance 
-				                              && lbA_new[i] > ubA_new[i] - options.boundTolerance)
-					constraints.setType( i,ST_EQUALITY );
-				else
-					constraints.setType( i,ST_BOUNDED );
-			}
-		}
-	}
-	else
-	{
-		if ( ( lbA_new == 0 ) && ( ubA_new == 0 ) )
-		{
-			for( i=0; i<nC; ++i )
-				constraints.setType( i,ST_UNBOUNDED );
-		}
-		else
-		{
-			for( i=0; i<nC; ++i )
-				constraints.setType( i,ST_BOUNDED );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	c o m p u t e P r o j e c t e d C h o l e s k y
- */
-returnValue QProblem::computeProjectedCholesky( )
-{
-	int i, j;
-	int nV  = getNV( );
-	int nZ  = getNZ( );
-
-	SymSparseMat* Id;
-
-	/* Revert to unprotected Cholesky decomposition */
-	if ( getNFX() + getNAC() == 0 )
-		return QProblemB::computeCholesky( );
-
-	/* 1) Initialises R with all zeros. */
-	for( i=0; i<nV*nV; ++i )
-		R[i] = 0.0;
-
-	/* Do not do anything for empty null spaces (important for LP case, HST_ZERO !)*/
-	if ( nZ == 0 ) // nZ == nV - getNFX() - getNAC()
-		return SUCCESSFUL_RETURN;
-	
-	/* 2) Calculate Cholesky decomposition of projected Hessian Z'*H*Z. */
-	int* FR_idx;
-	bounds.getFree( )->getNumberArray( &FR_idx );
-
-	int* AC_idx;
-	constraints.getActive( )->getNumberArray( &AC_idx );
-
-	/* calculate Z'*H*Z */
-	switch ( hessianType )
-	{
-		case HST_ZERO:
-			if ( usingRegularisation() == BT_TRUE )
-			{
-				Id = createDiagSparseMat( nV, regVal );
-				Id->bilinear(bounds.getFree(), nZ, Q, nV, R, nV);
-				delete Id;
-			}
-			else
-			{
-				/* Code should not get here, as  nZ == 0  always holds for an LP (without regularisation)! */
-				if ( nZ > 0 )
-					return THROWERROR( RET_UNKNOWN_BUG );
-			}
-			break;
-			
-		case HST_IDENTITY:
-			Id = createDiagSparseMat( nV, 1.0 );
-			Id->bilinear(bounds.getFree(), nZ, Q, nV, R, nV);
-			delete Id;
-			break;
-
-		default:
-			if ( getNAC() == 0 ) {
-				/* make Z trivial */
-				for ( j=0; j < nZ; ++j ) {
-					for ( i=0; i < nV; ++i )
-						QQ(i,j) = 0.0;
-					QQ(FR_idx[j],j) = 1.0;
-				}
-				/* now Z is trivial, and so is Z'HZ */
-				int nFR = getNFR ();
-				for ( j=0; j < nFR; ++j )
-					H->getCol (FR_idx[j], bounds.getFree (), 1.0, &R[j*nV]);
-			} else {
-				/* this is expensive if Z is large! */
-				H->bilinear(bounds.getFree(), nZ, Q, nV, R, nV);
-			}
-	}
-
-	/* R'*R = Z'*H*Z */
-	long info = 0;
-	unsigned long _nZ = (unsigned long)nZ, _nV = (unsigned long)nV;
-
-	POTRF( "U", &_nZ, R, &_nV, &info );
-
-	/* <0 = invalid call, =0 ok, >0 not spd */
-	if (info > 0) {
-		if ( R[0] < 0.0 )
-		{
-			/* Cholesky decomposition has tunneled a negative
-			 * diagonal element. */ 
-			options.epsRegularisation = getMin( -R[0]+options.epsRegularisation,getSqrt(getAbs(options.epsRegularisation)) );
-		}
-
-		hessianType = HST_SEMIDEF;
-		return RET_HESSIAN_NOT_SPD;
-	}
-
-	/* zero first subdiagonal to make givens updates work */
-	for (i=0;i<nZ-1;++i)
-		RR(i+1,i) = 0.0;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p I n i t i a l C h o l e s k y
- */
-returnValue QProblem::setupInitialCholesky( )
-{
-	returnValue returnvalueCholesky;
-	
-	/* If regularisation shall be used, always regularise at beginning 
-	 * if initial working set is not empty. */
-	if ( ( getNV() != getNFR()-getNFV() ) && ( options.enableRegularisation == BT_TRUE ) )
-		if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
-			return RET_INIT_FAILED_REGULARISATION;
-
-	/* Factorise projected Hessian 
-	 * now handles all special cases (no active bounds/constraints, no nullspace) */
-	returnvalueCholesky = computeProjectedCholesky( );
-
-	/* If Hessian is not positive definite, regularise and try again. */
-	if ( returnvalueCholesky == RET_HESSIAN_NOT_SPD )
-	{
-		if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
-			return RET_INIT_FAILED_REGULARISATION;
-
-		returnvalueCholesky = computeProjectedCholesky( );
-	}
-
-	if ( returnvalueCholesky != SUCCESSFUL_RETURN )
-		return RET_INIT_FAILED_CHOLESKY;
-
-	haveCholesky = BT_TRUE;
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p T Q f a c t o r i s a t i o n
- */
-returnValue QProblem::setupTQfactorisation( )
-{
-	int i, ii;
-	int nV  = getNV( );
-	int nFR = getNFR( );
-
-	int* FR_idx;
-	bounds.getFree( )->getNumberArray( &FR_idx );
-
-	/* 1) Set Q to unity matrix. */
-	for( i=0; i<nV*nV; ++i )
-		Q[i] = 0.0;
-
-	for( i=0; i<nFR; ++i )
-	{
-		ii = FR_idx[i];
-		QQ(ii,i) = 1.0;
-	}
-
- 	/* 2) Set T to zero matrix. */
-	for( i=0; i<sizeT*sizeT; ++i )
-		T[i] = 0.0;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	o b t a i n A u x i l i a r y W o r k i n g S e t
- */
-returnValue QProblem::obtainAuxiliaryWorkingSet(	const real_t* const xOpt, const real_t* const yOpt,
-													const Bounds* const guessedBounds, const Constraints* const guessedConstraints,
-													Bounds* auxiliaryBounds, Constraints* auxiliaryConstraints
-													) const
-{
-	int i = 0;
-	int nV = getNV( );
-	int nC = getNC( );
-
-
-	/* 1) Ensure that desiredBounds is allocated (and different from guessedBounds). */
-	if ( ( auxiliaryBounds == 0 ) || ( auxiliaryBounds == guessedBounds ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	if ( ( auxiliaryConstraints == 0 ) || ( auxiliaryConstraints == guessedConstraints ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-
-	SubjectToStatus guessedStatus;
-
-	/* 2) Setup working set of bounds for auxiliary initial QP. */
-	if ( QProblemB::obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds, auxiliaryBounds ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-
-	/* 3) Setup working set of constraints for auxiliary initial QP. */
-	if ( guessedConstraints != 0 )
-	{
-		/* If an initial working set is specific, use it!
-		 * Moreover, add all equality constraints if specified. */
-		for( i=0; i<nC; ++i )
-		{
-			/* Add constraint only if it is not (going to be) disabled! */
-			guessedStatus = guessedConstraints->getStatus( i );
-
-			#ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
-			if ( constraints.getType( i ) == ST_EQUALITY )
-			{
-				if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-			}
-			else
-			#endif
-			{
-				if ( auxiliaryConstraints->setupConstraint( i,guessedStatus ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-			}
-		}
-	}
-	else	/* No initial working set specified. */
-	{
-		/* Obtain initial working set by "clipping". */
-		if ( ( xOpt != 0 ) && ( yOpt == 0 ) )
-		{
-			for( i=0; i<nC; ++i )
-			{
-				if ( Ax[i] - lbA[i] <= options.boundTolerance )
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-					continue;
-				}
-
-				if ( ubA[i] - Ax_u[i] <= options.boundTolerance )
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_UPPER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-					continue;
-				}
-
-				/* Moreover, add all equality constraints if specified. */
-				#ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
-				if ( constraints.getType( i ) == ST_EQUALITY )
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-				else
-				#endif
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-			}
-		}
-
-		/* Obtain initial working set in accordance to sign of dual solution vector. */
-		if ( ( xOpt == 0 ) && ( yOpt != 0 ) )
-		{
-			for( i=0; i<nC; ++i )
-			{
-				if ( yOpt[nV+i] > EPS )
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-					continue;
-				}
-
-				if ( yOpt[nV+i] < -EPS )
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_UPPER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-					continue;
-				}
-
-				/* Moreover, add all equality constraints if specified. */
-				#ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
-				if ( constraints.getType( i ) == ST_EQUALITY )
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-				else
-				#endif
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-			}
-		}
-
-		/* If xOpt and yOpt are null pointer and no initial working is specified,
-		 * start with empty working set (or implicitly fixed bounds and equality constraints only)
-		 * for auxiliary QP. */
-		if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
-		{
-			for( i=0; i<nC; ++i )
-			{
-				/* Only add all equality constraints if specified. */
-				#ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
-				if ( constraints.getType( i ) == ST_EQUALITY )
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-				else
-				#endif
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-			}
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	s e t u p A u x i l i a r y W o r k i n g S e t
- */
-returnValue QProblem::setupAuxiliaryWorkingSet(	const Bounds* const auxiliaryBounds,
-												const Constraints* const auxiliaryConstraints,
-												BooleanType setupAfresh
-												)
-{
-	int i;
-	int nV = getNV( );
-	int nC = getNC( );
-	BooleanType WSisTrivial = BT_TRUE;
-
-	/* consistency checks */
-	if ( auxiliaryBounds != 0 )
-	{
-		for( i=0; i<nV; ++i )
-			if ( ( bounds.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryBounds->getStatus( i ) == ST_UNDEFINED ) )
-				return THROWERROR( RET_UNKNOWN_BUG );
-	}
-	else
-	{
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	if ( auxiliaryConstraints != 0 )
-	{
-		for( i=0; i<nC; ++i )
-			if ( ( constraints.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryConstraints->getStatus( i ) == ST_UNDEFINED ) )
-				return THROWERROR( RET_UNKNOWN_BUG );
-	}
-	else
-	{
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	/* Check for trivial working set (all and only bounds active) */
-	for (i = 0; i < nV; i++)
-		if (auxiliaryBounds->getStatus(i) == ST_INACTIVE)
-		{
-			WSisTrivial = BT_FALSE;
-			break;
-		}
-	for (i = 0; i < nC; i++)
-		// (ckirches) here we chose to ignore an invalid ST_INACTIVE on
-		//            constraints that are ST_EQUALITies or may just have become equalities
-		if ( (constraints.getType(i) == ST_EQUALITY) // NOT auxiliaryConstraints here
-			|| (auxiliaryConstraints->getStatus(i) != ST_INACTIVE) )
-		{
-			WSisTrivial = BT_FALSE;
-			break;
-		}
-
-	if (WSisTrivial == BT_TRUE)
-	{
-		for (i = 0; i < nV; i++)
-			if (bounds.getStatus(i) == ST_INACTIVE)
-				bounds.moveFreeToFixed(i, auxiliaryBounds->getStatus(i));
-
-		return SUCCESSFUL_RETURN;
-	}
-
-
-	/* I) SETUP CHOLESKY FLAG:
-	 *    Cholesky decomposition shall only be updated if working set
-	 *    shall be updated (i.e. NOT setup afresh!) */
-	BooleanType updateCholesky;
-	if ( setupAfresh == BT_TRUE )
-		updateCholesky = BT_FALSE;
-	else
-		updateCholesky = BT_TRUE;
-
-
-	BooleanType was_fulli = options.enableFullLITests;
-	real_t backupEpsLITests = options.epsLITests;
-
-	options.enableFullLITests = BT_FALSE;
-	/* options.epsLITests = 1e-1; */
-
-	/* II) REMOVE FORMERLY ACTIVE (CONSTRAINTS') BOUNDS (IF NECESSARY): */
-	if ( setupAfresh == BT_FALSE )
-	{
-		/* 1) Remove all active constraints that shall be inactive or disabled AND
-		*    all active constraints that are active at the wrong bound. */
-		for( i=0; i<nC; ++i )
-		{
-			if ( ( constraints.getStatus( i ) == ST_LOWER ) && ( auxiliaryConstraints->getStatus( i ) != ST_LOWER ) )
-				if ( removeConstraint( i,updateCholesky,BT_FALSE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-
-			if ( ( constraints.getStatus( i ) == ST_UPPER ) && ( auxiliaryConstraints->getStatus( i ) != ST_UPPER ) )
-				if ( removeConstraint( i,updateCholesky,BT_FALSE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-		}
-
-		/* 2) Remove all active bounds that shall be inactive AND
-		*    all active bounds that are active at the wrong bound. */
-		for( i=0; i<nV; ++i )
-		{
-			if ( ( bounds.getStatus( i ) == ST_LOWER ) && ( auxiliaryBounds->getStatus( i ) != ST_LOWER ) )
-				if ( removeBound( i,updateCholesky,BT_FALSE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-
-			if ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) )
-				if ( removeBound( i,updateCholesky,BT_FALSE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-		}
-	}
-
-
-	/* III) ADD NEWLY ACTIVE (CONSTRAINTS') BOUNDS: */
-
-	/* 1) Add all equality bounds. */
-	for( i=0; i<nV; ++i )
-	{
-		//if ( ( bounds.getType( i ) == ST_EQUALITY ) && ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) ) )
-
-		// (ckirches) force equalities active
-
-		if ( ( bounds.getType( i ) == ST_EQUALITY ) && ( bounds.getStatus( i ) == ST_INACTIVE ) )
-		{
-            // assert ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE );
-			/* No check for linear independence necessary. */
-			if ( addBound( i,ST_LOWER,updateCholesky ) != SUCCESSFUL_RETURN ) // was auxiliaryBounds->getStatus( i )
-				return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-		}
-	}
-
-	/* 2) Add all equality constraints. */
-	for( i=0; i<nC; ++i )
-	{
-        //if ( ( constraints.getType( i ) == ST_EQUALITY ) && ( ( constraints.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryConstraints->getStatus( i ) != ST_INACTIVE ) ) )
-
-		// (ckirches) force equalities active
-
-		if ( ( constraints.getType( i ) == ST_EQUALITY ) && ( constraints.getStatus( i ) == ST_INACTIVE ) )
-		{
-            // assert ( auxiliaryConstraints->getStatus( i ) != ST_INACTIVE );
-			/* Add constraint only if it is linearly independent from the current working set. */
-			if ( addConstraint_checkLI( i ) == RET_LINEARLY_INDEPENDENT )
-			{
-				if ( addConstraint( i,ST_LOWER,updateCholesky ) != SUCCESSFUL_RETURN )  // was auxiliaryConstraints->getStatus( i )
-					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-			}
-			else
-			{
-				/* Equalities are not linearly independent! */
-				constraints.setType(i, ST_BOUNDED);
-			}
-		}
-	}
-
-
-	/* 3) Add all inactive bounds that shall be active AND
-	 *    all formerly active bounds that have been active at the wrong bound. */
-	for( i=0; i<nV; ++i )
-	{
-		if ( ( bounds.getType( i ) != ST_EQUALITY ) && ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) ) )
-		{
-			/* Add bound only if it is linearly independent from the current working set. */
-			if ( addBound_checkLI( i ) == RET_LINEARLY_INDEPENDENT )
-			{
-				if ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-			}
-		}
-	}
-
-	/* 4) Add all inactive constraints that shall be active AND
-	 *    all formerly active constraints that have been active at the wrong bound. */
-	for( i=0; i<nC; ++i )
-	{
-		if ( ( constraints.getType( i ) != ST_EQUALITY ) && ( auxiliaryConstraints->getStatus( i ) != ST_INACTIVE ) )
-		{
-			/* formerly inactive */
-			if ( constraints.getStatus( i ) == ST_INACTIVE )
-			{
-				/* Add constraint only if it is linearly independent from the current working set. */
-				if ( addConstraint_checkLI( i ) == RET_LINEARLY_INDEPENDENT )
-				{
-					if ( addConstraint( i,auxiliaryConstraints->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-				}
-			}
-		}
-	}
-
-	options.enableFullLITests = was_fulli;
-	options.epsLITests = backupEpsLITests;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A u x i l i a r y Q P s o l u t i o n
- */
-returnValue QProblem::setupAuxiliaryQPsolution(	const real_t* const xOpt, const real_t* const yOpt
-												)
-{
-	int i, j;
-	int nV = getNV( );
-	int nC = getNC( );
-
-
-	/* Setup primal/dual solution vector for auxiliary initial QP:
-	 * if a null pointer is passed, a zero vector is assigned;
-	 *  old solution vector is kept if pointer to internal solution vevtor is passed. */
-	if ( xOpt != 0 )
-	{
-		if ( xOpt != x )
-			for( i=0; i<nV; ++i )
-				x[i] = xOpt[i];
-
-		A->times(1, 1.0, x, nV, 0.0, Ax, nC);
-
-		for ( j=0; j<nC; ++j )
-		{
-			Ax_l[j] = Ax[j];
-			Ax_u[j] = Ax[j];
-		}
-	}
-	else
-	{
-		for( i=0; i<nV; ++i )
-			x[i] = 0.0;
-
-		for ( j=0; j<nC; ++j )
-		{
-			Ax[j] = 0.0;
-			Ax_l[j] = 0.0;
-			Ax_u[j] = 0.0;
-		}
-	}
-
-	if ( yOpt != 0 )
-	{
-		if ( yOpt != y )
-			for( i=0; i<nV+nC; ++i )
-				y[i] = yOpt[i];
-	}
-	else
-	{
-		for( i=0; i<nV+nC; ++i )
-			y[i] = 0.0;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A u x i l i a r y Q P g r a d i e n t
- */
-returnValue QProblem::setupAuxiliaryQPgradient( )
-{
-	int i;
-	int nV = getNV( );
-	int nC = getNC( );
-
-
-	/* Setup gradient vector: g = -H*x + [Id A]'*[yB yC]
-	 *                          = yB - H*x + A'*yC. */
-	switch ( hessianType )
-	{
-		case HST_ZERO:
-			if ( usingRegularisation( ) == BT_FALSE )
-				for ( i=0; i<nV; ++i )
-					g[i] = y[i];
-			else
-				for ( i=0; i<nV; ++i )
-					g[i] = y[i] - regVal*x[i];
-			break;
-
-		case HST_IDENTITY:
-			for ( i=0; i<nV; ++i )
-				g[i] = y[i] - x[i];
-			break;
-
-		default:
-			/* y'*Id */
-			for ( i=0; i<nV; ++i )
-				g[i] = y[i];
-
-			/* - H*x */
-			H->times(1, -1.0, x, nV, 1.0, g, nV);
-			break;
-	}
-
-	/* + A'*yC */
-	A->transTimes(1, 1.0, y + nV, nC, 1.0, g, nV);
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	a r e B o u n d s C o n s i s t e n t
- */
-returnValue QProblem::areBoundsConsistent(	const real_t* const lb_new, const real_t* const ub_new,
-											const real_t* const lbA_new, const real_t* const ubA_new) const
-{
-	if (QProblemB::areBoundsConsistent(lb_new, ub_new) == RET_QP_INFEASIBLE)
-		return RET_QP_INFEASIBLE;
-
-	if (lbA_new && ubA_new) {
-		for (int i = 0; i < getNC(); ++i) {
-			if (lbA_new[i] > ubA_new[i]+EPS) {
-				return RET_QP_INFEASIBLE;
-			}
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A u x i l i a r y Q P b o u n d s
- */
-returnValue QProblem::setupAuxiliaryQPbounds(	const Bounds* const auxiliaryBounds,
-												const Constraints* const auxiliaryConstraints,
-												BooleanType useRelaxation
-												)
-{
-	int i;
-	int nV = getNV( );
-	int nC = getNC( );
-
-
-	/* 1) Setup bound vectors. */
-	for ( i=0; i<nV; ++i )
-	{
-		switch ( bounds.getStatus( i ) )
-		{
-			case ST_INACTIVE:
-				if ( useRelaxation == BT_TRUE )
-				{
-					if ( bounds.getType( i ) == ST_EQUALITY )
-					{
-						lb[i] = x[i];
-						ub[i] = x[i];
-					}
-					else
-					{
-						/* If a bound is inactive although it was supposed to be
-						* active by the auxiliaryBounds, it could not be added
-						* due to linear dependence. Thus set it "strongly inactive". */
-						if ( auxiliaryBounds->getStatus( i ) == ST_LOWER )
-							lb[i] = x[i];
-						else
-							lb[i] = x[i] - options.boundRelaxation;
-
-						if ( auxiliaryBounds->getStatus( i ) == ST_UPPER )
-							ub[i] = x[i];
-						else
-							ub[i] = x[i] + options.boundRelaxation;
-					}
-				}
-				break;
-
-			case ST_LOWER:
-				lb[i] = x[i];
-				if ( bounds.getType( i ) == ST_EQUALITY )
-				{
-					ub[i] = x[i];
-				}
-				else
-				{
-					if ( useRelaxation == BT_TRUE )
-						ub[i] = x[i] + options.boundRelaxation;
-				}
-				break;
-
-			case ST_UPPER:
-				ub[i] = x[i];
-				if ( bounds.getType( i ) == ST_EQUALITY )
-				{
-					lb[i] = x[i];
-				}
-				else
-				{
-					if ( useRelaxation == BT_TRUE )
-						lb[i] = x[i] - options.boundRelaxation;
-				}
-				break;
-
-            case ST_DISABLED:
-                break;
-
-			default:
-				return THROWERROR( RET_UNKNOWN_BUG );
-		}
-	}
-
-	/* 2) Setup constraints vectors. */
-	for ( i=0; i<nC; ++i )
-	{
-		switch ( constraints.getStatus( i ) )
-		{
-			case ST_INACTIVE:
-				if ( useRelaxation == BT_TRUE )
-				{
-					if ( constraints.getType( i ) == ST_EQUALITY )
-					{
-						lbA[i] = Ax_l[i];
-						ubA[i] = Ax_u[i];
-					}
-					else
-					{
-						/* If a constraint is inactive although it was supposed to be
-						* active by the auxiliaryConstraints, it could not be added
-						* due to linear dependence. Thus set it "strongly inactive". */
-						if ( auxiliaryConstraints->getStatus( i ) == ST_LOWER )
-							lbA[i] = Ax_l[i];
-						else
-							lbA[i] = Ax_l[i] - options.boundRelaxation;
-
-						if ( auxiliaryConstraints->getStatus( i ) == ST_UPPER )
-							ubA[i] = Ax_u[i];
-						else
-							ubA[i] = Ax_u[i] + options.boundRelaxation;
-					}
-				}
-				break;
-
-			case ST_LOWER:
-				lbA[i] = Ax_l[i];
-				if ( constraints.getType( i ) == ST_EQUALITY )
-				{
-					ubA[i] = Ax_l[i];
-				}
-				else
-				{
-					if ( useRelaxation == BT_TRUE )
-						ubA[i] = Ax_l[i] + options.boundRelaxation;
-				}
-				break;
-
-			case ST_UPPER:
-				ubA[i] = Ax_u[i];
-				if ( constraints.getType( i ) == ST_EQUALITY )
-				{
-					lbA[i] = Ax_u[i];
-				}
-				else
-				{
-					if ( useRelaxation == BT_TRUE )
-						lbA[i] = Ax_u[i] - options.boundRelaxation;
-				}
-				break;
-
-            case ST_DISABLED:
-                break;
-
-			default:
-				return THROWERROR( RET_UNKNOWN_BUG );
-		}
-		Ax_l[i] = Ax_l[i] - lbA[i];
-		Ax_u[i] = ubA[i] - Ax_u[i];
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	a d d C o n s t r a i n t
- */
-returnValue QProblem::addConstraint(	int number, SubjectToStatus C_status,
-										BooleanType updateCholesky,
-										BooleanType ensureLI
-										)
-{
-	int i, j, ii;
-
-	/* consistency checks */
-	if ( constraints.getStatus( number ) != ST_INACTIVE )
-		return THROWERROR( RET_CONSTRAINT_ALREADY_ACTIVE );
-
-	if ( ( constraints.getNC( ) - getNAC( ) ) == constraints.getNUC( ) )
-		return THROWERROR( RET_ALL_CONSTRAINTS_ACTIVE );
-
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
-		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
-		 ( getStatus( ) == QPS_SOLVED )            )
-	{
-		return THROWERROR( RET_UNKNOWN_BUG );
-	}
-
-
-	/* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,
-	 *    i.e. remove a constraint or bound if linear dependence occurs. */
-	/* check for LI only if Cholesky decomposition shall be updated! */
-	if ( updateCholesky == BT_TRUE && ensureLI == BT_TRUE )
-	{
-		returnValue ensureLIreturnvalue = addConstraint_ensureLI( number,C_status );
-
-		switch ( ensureLIreturnvalue )
-		{
-			case SUCCESSFUL_RETURN:
-				break;
-
-			case RET_LI_RESOLVED:
-				break;
-
-			case RET_ENSURELI_FAILED_NOINDEX:
-				return RET_ADDCONSTRAINT_FAILED_INFEASIBILITY;
-
-			case RET_ENSURELI_FAILED_CYCLING:
-				return RET_ADDCONSTRAINT_FAILED_INFEASIBILITY;
-
-			case RET_ENSURELI_DROPPED:
-				return SUCCESSFUL_RETURN;
-
-			default:
-				return THROWERROR( RET_ENSURELI_FAILED );
-		}
-	}
-
-	/* some definitions */
-	int nV  = getNV( );
-	int nFR = getNFR( );
-	int nAC = getNAC( );
-	int nZ  = getNZ( );
-
-	int tcol = sizeT - nAC;
-
-
-	int* FR_idx;
-	bounds.getFree( )->getNumberArray( &FR_idx );
-
-	real_t* aFR = new real_t[nFR];
-	real_t* wZ = new real_t[nZ];
-	for( i=0; i<nZ; ++i )
-		wZ[i] = 0.0;
-
-
-	/* II) ADD NEW ACTIVE CONSTRAINT TO MATRIX T: */
-	/* 1) Add row [wZ wY] = aFR'*[Z Y] to the end of T: assign aFR. */
-	A->getRow(number, bounds.getFree(), 1.0, aFR);
-
-	/* calculate wZ */
-	for( i=0; i<nFR; ++i )
-	{
-		ii = FR_idx[i];
-		for( j=0; j<nZ; ++j )
-			wZ[j] += aFR[i] * QQ(ii,j);
-	}
-
-	/* 2) Calculate wY and store it directly into T. */
-	if ( nAC > 0 )
-	{
-		for( j=0; j<nAC; ++j )
-			TT(nAC,tcol+j) = 0.0;
-		for( i=0; i<nFR; ++i )
-		{
-			ii = FR_idx[i];
-			for( j=0; j<nAC; ++j )
-				TT(nAC,tcol+j) += aFR[i] * QQ(ii,nZ+j);
-		}
-	}
-
-	delete[] aFR;
-
-
-	real_t c, s, nu;
-
-	if ( nZ > 0 )
-	{
-		/* II) RESTORE TRIANGULAR FORM OF T: */
-		/*     Use column-wise Givens rotations to restore reverse triangular form
-		*      of T, simultanenous change of Q (i.e. Z) and R. */
-		for( j=0; j<nZ-1; ++j )
-		{
-			computeGivens( wZ[j+1],wZ[j], wZ[j+1],wZ[j],c,s );
-			nu = s/(1.0+c);
-
-			for( i=0; i<nFR; ++i )
-			{
-				ii = FR_idx[i];
-				applyGivens( c,s,nu,QQ(ii,1+j),QQ(ii,j), QQ(ii,1+j),QQ(ii,j) );
-			}
-
-			if ( ( updateCholesky == BT_TRUE ) &&
-				 ( hessianType != HST_ZERO )   && ( hessianType != HST_IDENTITY ) )
-			{
-				for( i=0; i<=j+1; ++i )
-					applyGivens( c,s,nu,RR(i,1+j),RR(i,j), RR(i,1+j),RR(i,j) );
-			}
-		}
-
-		TT(nAC,tcol-1) = wZ[nZ-1];
-
-
-		if ( ( updateCholesky == BT_TRUE ) &&
-			 ( hessianType != HST_ZERO )   && ( hessianType != HST_IDENTITY ) )
-		{
-			/* III) RESTORE TRIANGULAR FORM OF R:
-			 *      Use row-wise Givens rotations to restore upper triangular form of R. */
-			for( i=0; i<nZ-1; ++i )
-			{
-				computeGivens( RR(i,i),RR(1+i,i), RR(i,i),RR(1+i,i),c,s );
-				nu = s/(1.0+c);
-
-				for( j=(1+i); j<(nZ-1); ++j ) /* last column of R is thrown away */
-					applyGivens( c,s,nu,RR(i,j),RR(1+i,j), RR(i,j),RR(1+i,j) );
-			}
-			/* last column of R is thrown away */
-			for( i=0; i<nZ; ++i )
-				RR(i,nZ-1) = 0.0;
-		}
-	}
-
-	delete[] wZ;
-
-
-	/* IV) UPDATE INDICES */
-	tabularOutput.idxAddC = number;
-	if ( constraints.moveInactiveToActive( number,C_status ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_ADDCONSTRAINT_FAILED );
-
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	a d d C o n s t r a i n t _ c h e c k L I
- */
-returnValue QProblem::addConstraint_checkLI( int number )
-{
-	returnValue returnvalue = RET_LINEARLY_DEPENDENT;
-
-	int i, j, ii;
-	int nV  = getNV( );
-	int nFR = getNFR( );
-	int nZ  = getNZ( );
-	int nC  = getNC( );
-	int nAC = getNAC();
-	int nFX = getNFX();
-	int *FR_idx;
-
-	bounds.getFree( )->getNumberArray( &FR_idx );
-
-
-	if (options.enableFullLITests)
-	{
-		/*
-		 * expensive LI test. Backsolve with refinement using special right
-		 * hand side. This gives an estimate for what should be considered
-		 * "zero". We then check linear independence relative to this estimate.
-		 */
-
-		int *FX_idx, *AC_idx, *IAC_idx;
-
-		real_t *delta_g   = new real_t[nV];
-		real_t *delta_xFX = new real_t[nFX];
-		real_t *delta_xFR = new real_t[nFR];
-		real_t *delta_yAC = new real_t[nAC];
-		real_t *delta_yFX = new real_t[nFX];
-
-		bounds.getFixed( )->getNumberArray( &FX_idx );
-		constraints.getActive( )->getNumberArray( &AC_idx );
-		constraints.getInactive( )->getNumberArray( &IAC_idx );
-
-		int dim = (nC>nV)?nC:nV;
-		real_t *nul = new real_t[dim];
-		for (ii = 0; ii < dim; ++ii)
-			nul[ii]=0.0;
-
-		A->getRow (number, 0, 1.0, delta_g);
-
-		returnvalue = determineStepDirection ( delta_g,
-											  nul, nul, nul, nul,
-											  BT_FALSE, BT_FALSE,
-											  delta_xFX, delta_xFR, delta_yAC, delta_yFX);
-		delete[] nul;
-
-		/* compute the weight in inf-norm */
-		real_t weight = 0.0;
-		for (ii = 0; ii < nAC; ++ii)
-		{
-			real_t a = getAbs (delta_yAC[ii]);
-			if (weight < a) weight = a;
-		}
-		for (ii = 0; ii < nFX; ++ii)
-		{
-			real_t a = getAbs (delta_yFX[ii]);
-			if (weight < a) weight = a;
-		}
-
-		/* look at the "zero" in a relative inf-norm */
-		real_t zero = 0.0;
-		for (ii = 0; ii < nFX; ++ii)
-		{
-			real_t a = getAbs (delta_xFX[ii]);
-			if (zero < a) zero = a;
-		}
-		for (ii = 0; ii < nFR; ++ii)
-		{
-			real_t a = getAbs (delta_xFR[ii]);
-			if (zero < a) zero = a;
-		}
-
-		/* relative test against zero in inf-norm */
-		if (zero > options.epsLITests * weight)
-			returnvalue = RET_LINEARLY_INDEPENDENT;
-
-		delete[] delta_yFX;
-		delete[] delta_yAC;
-		delete[] delta_xFR;
-		delete[] delta_xFX;
-		delete[] delta_g;
-
-	}
-	else
-	{
-		/*
-		 * cheap LI test for constraint. Check if constraint <number> is
-		 * linearly independent from the the active ones (<=> is element of null
-		 * space of Afr).
-		 */
-
-		real_t *Arow = new real_t[nFR];
-		A->getRow(number, bounds.getFree(), 1.0, Arow);
-
-		real_t sum, l2;
-
-		l2  = 0.0;
-		for (i = 0; i < nFR; i++)
-			l2  += Arow[i]*Arow[i];
-
-		for( j=0; j<nZ; ++j )
-		{
-			sum = 0.0;
-			for( i=0; i<nFR; ++i )
-			{
-				ii = FR_idx[i];
-				sum += Arow[i] * QQ(ii,j);
-			}
-
-			if ( getAbs( sum ) > options.epsLITests*l2 )
-			{
-				/*fprintf(stdFile, "LI test: |sum| = %9.2e, l2 = %9.2e, var = %d\n", getAbs(sum), l2, jj+1); */
-				returnvalue = RET_LINEARLY_INDEPENDENT;
-				break;
-			}
-		}
-
-		delete[] Arow;
-	}
-
-	return THROWINFO( returnvalue );
-}
-
-
-/*
- *	a d d C o n s t r a i n t _ e n s u r e L I
- */
-returnValue QProblem::addConstraint_ensureLI( int number, SubjectToStatus C_status )
-{
-	int i, j, ii, jj;
-	int nV  = getNV( );
-	int nFR = getNFR( );
-	int nFX = getNFX( );
-	int nAC = getNAC( );
-	int nZ  = getNZ( );
-
-
-	/* I) Check if new constraint is linearly independent from the active ones. */
-	returnValue returnvalueCheckLI = addConstraint_checkLI( number );
-
-	if ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED )
-		return THROWERROR( RET_ENSURELI_FAILED );
-
-	if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
-		return SUCCESSFUL_RETURN;
-
-
- 	/* II) NEW CONSTRAINT IS LINEARLY DEPENDENT: */
-	/* 1) Determine coefficients of linear combination,
-	 *    cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter:
-	 *    An Algorithm for the Solution of the Parametric Quadratic Programming
-	 *    Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */
-	int* FR_idx;
-	bounds.getFree( )->getNumberArray( &FR_idx );
-
-	int* FX_idx;
-	bounds.getFixed( )->getNumberArray( &FX_idx );
-
-	real_t* xiC = new real_t[nAC];
-	real_t* xiC_TMP = new real_t[nAC];
-	real_t* xiB = new real_t[nFX];
-	real_t* Arow = new real_t[nFR];
-	real_t* num = new real_t[nV];
-
-	returnValue returnvalue = SUCCESSFUL_RETURN;
-
-	real_t y_min = options.maxDualJump;
-	int y_min_number = -1;
-	int y_min_number_bound = -1;
-	BooleanType y_min_isBound = BT_FALSE;
-
-	A->getRow(number, bounds.getFree(), C_status == ST_LOWER ? 1.0 : -1.0, Arow);
-
-	/* 2) Calculate xiC */
-	if ( nAC > 0 )
-	{
-		for( i=0; i<nAC; ++i )
-		{
-			xiC_TMP[i] = 0.0;
-			for( j=0; j<nFR; ++j )
-			{
-				jj = FR_idx[j];
-				xiC_TMP[i] += QQ(jj,nZ+i) * Arow[j];
-			}
-		}
-
-		if ( backsolveT( xiC_TMP, BT_TRUE, xiC ) != SUCCESSFUL_RETURN )
-		{
-			returnvalue = RET_ENSURELI_FAILED_TQ;
-			goto farewell;
-		}
-	}
-
-	/* 3) Calculate xiB. */
-	int* AC_idx;
-	constraints.getActive( )->getNumberArray( &AC_idx );
-
-	A->getRow(number, bounds.getFixed(), C_status == ST_LOWER ? 1.0 : -1.0, xiB);
-	A->transTimes(constraints.getActive(), bounds.getFixed(), 1, -1.0, xiC, nAC, 1.0, xiB, nFX);
-
-	/* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */
-
-	/* 1) Constraints. */
-	for( i=0; i<nAC; ++i )
-	{
-		ii = AC_idx[i];
-		num[i] = y[nV+ii];
-	}
-
-	performRatioTest (nAC, AC_idx, &constraints, num, xiC, options.epsNum, options.epsDen, y_min, y_min_number);
-
-	/* 2) Bounds. */
-	for( i=0; i<nFX; ++i )
-	{
-		ii = FX_idx[i];
-		num[i] = y[ii];
-	}
-
-	performRatioTest (nFX, FX_idx, &bounds, num, xiB, options.epsNum, options.epsDen, y_min, y_min_number_bound);
-
-	if ( y_min_number_bound >= 0 )
-	{
-		y_min_number = y_min_number_bound;
-		y_min_isBound = BT_TRUE;
-	}
-
-	#ifndef __XPCTARGET__
-	/* setup output preferences */
-	char messageString[MAX_STRING_LENGTH];
-	#endif
-
-	/* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
-	if ( y_min_number >= 0 )
-	{
-		/* Update Lagrange multiplier... */
-		for( i=0; i<nAC; ++i )
-		{
-			ii = AC_idx[i];
-			y[nV+ii] -= y_min * xiC[i];
-		}
-		for( i=0; i<nFX; ++i )
-		{
-			ii = FX_idx[i];
-			y[ii] -= y_min * xiB[i];
-		}
-
-		/* ... also for newly active constraint... */
-		if ( C_status == ST_LOWER )
-			y[nV+number] = y_min;
-		else
-			y[nV+number] = -y_min;
-
-		/* ... and for constraint to be removed. */
-		if ( y_min_isBound == BT_TRUE )
-		{
-			#ifndef __XPCTARGET__
-			snprintf( messageString,MAX_STRING_LENGTH,"bound no. %d.",y_min_number );
-			getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-			#endif
-
-			if ( removeBound( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
-			{
-				returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
-				goto farewell;
-			}
-			tabularOutput.excRemB = 1;
-
-			y[y_min_number] = 0.0;
-		}
-		else
-		{
-			#ifndef __XPCTARGET__
-			snprintf( messageString,MAX_STRING_LENGTH,"constraint no. %d.",y_min_number );
-			getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-			#endif
-
-			if ( removeConstraint( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
-			{
-				returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
-				goto farewell;
-			}
-			tabularOutput.excRemC = 1;
-
-			y[nV+y_min_number] = 0.0;
-		}
-	}
-	else
-	{
-		if (options.enableDropInfeasibles == BT_TRUE) {
-			/* dropping of infeasible constraints according to drop priorities */
-			returnvalue = dropInfeasibles (number, C_status, BT_FALSE, xiB, xiC);
-		}
-		else
-		{
-			/* no constraint/bound can be removed => QP is infeasible! */
-			returnvalue = RET_ENSURELI_FAILED_NOINDEX;
-			setInfeasibilityFlag( returnvalue );
-		}
-	}
-
-farewell:
-	delete[] num;
-	delete[] Arow;
-	delete[] xiB;
-	delete[] xiC_TMP;
-	delete[] xiC;
-
-	getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-
-	return ( (returnvalue != SUCCESSFUL_RETURN) && (returnvalue != RET_ENSURELI_FAILED_NOINDEX ) ) ? THROWERROR (returnvalue) : returnvalue;
-}
-
-
-
-/*
- *	a d d B o u n d
- */
-returnValue QProblem::addBound(	int number, SubjectToStatus B_status,
-								BooleanType updateCholesky,
-								BooleanType ensureLI
-								)
-{
-	int i, j, ii;
-
-	/* consistency checks */
-	if ( bounds.getStatus( number ) != ST_INACTIVE )
-		return THROWERROR( RET_BOUND_ALREADY_ACTIVE );
-
-	if ( getNFR( ) == bounds.getNUV( ) )
-		return THROWERROR( RET_ALL_BOUNDS_ACTIVE );
-
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
-		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
- 		 ( getStatus( ) == QPS_SOLVED )            )
-	{
-		return THROWERROR( RET_UNKNOWN_BUG );
-	}
-
-
-	/* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,
-	 *    i.e. remove a constraint or bound if linear dependence occurs. */
-	/* check for LI only if Cholesky decomposition shall be updated! */
-	if ( updateCholesky == BT_TRUE && ensureLI == BT_TRUE )
-	{
-		returnValue ensureLIreturnvalue = addBound_ensureLI( number,B_status );
-
-		switch ( ensureLIreturnvalue )
-		{
-			case SUCCESSFUL_RETURN:
-				break;
-
-			case RET_LI_RESOLVED:
-				break;
-
-			case RET_ENSURELI_FAILED_NOINDEX:
-				return RET_ADDBOUND_FAILED_INFEASIBILITY;
-
-			case RET_ENSURELI_FAILED_CYCLING:
-				return RET_ADDBOUND_FAILED_INFEASIBILITY;
-
-			case RET_ENSURELI_DROPPED:
-				return SUCCESSFUL_RETURN;
-
-			default:
-				return THROWERROR( RET_ENSURELI_FAILED );
-		}
-	}
-
-
-	/* some definitions */
-	int nV  = getNV( );
-	int nFR = getNFR( );
-	int nAC = getNAC( );
-	int nZ  = getNZ( );
-
-	int tcol = sizeT - nAC;
-
-
-	/* II) SWAP INDEXLIST OF FREE VARIABLES:
-	 *     move the variable to be fixed to the end of the list of free variables. */
-	int lastfreenumber = bounds.getFree( )->getLastNumber( );
-	if ( lastfreenumber != number )
-		if ( bounds.swapFree( number,lastfreenumber ) != SUCCESSFUL_RETURN )
-			THROWERROR( RET_ADDBOUND_FAILED );
-
-
-	int* FR_idx;
-	bounds.getFree( )->getNumberArray( &FR_idx );
-
-	real_t* w = new real_t[nFR];
-
-
-	/* III) ADD NEW ACTIVE BOUND TO TOP OF MATRIX T: */
-	/* 1) add row [wZ wY] = [Z Y](number) at the top of T: assign w */
-	for( i=0; i<nFR; ++i )
-		w[i] = QQ(FR_idx[nFR-1],i);
-
-
-	/* 2) Use column-wise Givens rotations to restore reverse triangular form
-	 *    of the first row of T, simultanenous change of Q (i.e. Z) and R. */
-	real_t c, s, nu;
-
-	for( j=0; j<nZ-1; ++j )
-	{
-		computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );
-		nu = s/(1.0+c);
-
-		for( i=0; i<nFR; ++i )
-		{
-			ii = FR_idx[i];
-			applyGivens( c,s,nu,QQ(ii,1+j),QQ(ii,j), QQ(ii,1+j),QQ(ii,j) );
-		}
-
-		if ( ( updateCholesky == BT_TRUE ) &&
-			 ( hessianType != HST_ZERO )   && ( hessianType != HST_IDENTITY ) )
-		{
-			for( i=0; i<=j+1; ++i )
-				applyGivens( c,s,nu,RR(i,1+j),RR(i,j), RR(i,1+j),RR(i,j) );
-		}
-	}
-
-
-	if ( nAC > 0 )	  /* ( nAC == 0 ) <=> ( nZ == nFR ) <=> Y and T are empty => nothing to do */
-	{
-		/* store new column a in a temporary vector instead of shifting T one column to the left */
-		real_t* tmp = new real_t[nAC];
-		for( i=0; i<nAC; ++i )
-			tmp[i] = 0.0;
-
-		{
-			j = nZ-1;
-
-			computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );
-			nu = s/(1.0+c);
-
-			for( i=0; i<nFR; ++i )
-			{
-				ii = FR_idx[i];
-				applyGivens( c,s,nu,QQ(ii,1+j),QQ(ii,j), QQ(ii,1+j),QQ(ii,j) );
-			}
-
-			applyGivens( c,s,nu,TT(nAC-1,tcol),tmp[nAC-1], tmp[nAC-1],TT(nAC-1,tcol) );
-		}
-
-		for( j=nZ; j<nFR-1; ++j )
-		{
-			computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );
-			nu = s/(1.0+c);
-
-			for( i=0; i<nFR; ++i )
-			{
-				ii = FR_idx[i];
-				applyGivens( c,s,nu,QQ(ii,1+j),QQ(ii,j), QQ(ii,1+j),QQ(ii,j) );
-			}
-
-			for( i=(nFR-2-j); i<nAC; ++i )
-				applyGivens( c,s,nu,TT(i,1+tcol-nZ+j),tmp[i], tmp[i],TT(i,1+tcol-nZ+j) );
-		}
-
-		delete[] tmp;
-	}
-
-	delete[] w;
-
-
-	if ( ( updateCholesky == BT_TRUE ) &&
-		 ( hessianType != HST_ZERO )   && ( hessianType != HST_IDENTITY ) )
-	{
-		/* IV) RESTORE TRIANGULAR FORM OF R:
-		 *     use row-wise Givens rotations to restore upper triangular form of R */
-		for( i=0; i<nZ-1; ++i )
-		{
-			computeGivens( RR(i,i),RR(1+i,i), RR(i,i),RR(1+i,i),c,s );
-			nu = s/(1.0+c);
-
-			for( j=(1+i); j<nZ-1; ++j ) /* last column of R is thrown away */
-				applyGivens( c,s,nu,RR(i,j),RR(1+i,j), RR(i,j),RR(1+i,j) );
-		}
-		/* last column of R is thrown away */
-		for( i=0; i<nZ; ++i )
-			RR(i,nZ-1) = 0.0;
-	}
-
-
-	/* V) UPDATE INDICES */
-	tabularOutput.idxAddB = number;
-	if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_ADDBOUND_FAILED );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	a d d B o u n d _ c h e c k L I
- */
-returnValue QProblem::addBound_checkLI( int number )
-{
-	int i, ii;
-	int nV  = getNV( );  /* for QQ() macro */
-	int nFR = getNFR( );
-	int nAC = getNAC();
-	int nFX = getNFX();
-	int nC  = getNC( );
-	returnValue returnvalue = RET_LINEARLY_DEPENDENT;
-
-	if (options.enableFullLITests)
-	{
-		/*
-		 * expensive LI test. Backsolve with refinement using special right
-		 * hand side. This gives an estimate for what should be considered
-		 * "zero". We then check linear independence relative to this estimate.
-		 */
-
-		/*
-		 * expensive LI test. Backsolve with refinement using special right
-		 * hand side. This gives an estimate for what should be considered
-		 * "zero". We then check linear independence relative to this estimate.
-		 */
-
-		real_t *delta_g   = new real_t[nV];
-		real_t *delta_xFX = new real_t[nFX];
-		real_t *delta_xFR = new real_t[nFR];
-		real_t *delta_yAC = new real_t[nAC];
-		real_t *delta_yFX = new real_t[nFX];
-
-		for (ii = 0; ii < nV; ++ii)
-			delta_g[ii] = 0.0;
-		delta_g[number] = 1.0;	/* sign doesn't matter here */
-
-		int dim = (nC>nV)?nC:nV;
-		real_t *nul = new real_t[dim];
-		for (ii = 0; ii < dim; ++ii)
-			nul[ii]=0.0;
-
-		returnValue dsdReturnValue = determineStepDirection (
-				delta_g, nul, nul, nul, nul, BT_FALSE, BT_FALSE,
-				delta_xFX, delta_xFR, delta_yAC, delta_yFX);
-		if (dsdReturnValue != SUCCESSFUL_RETURN)
-			returnvalue = dsdReturnValue;
-
-		/* compute the weight in inf-norm */
-		real_t weight = 0.0;
-		for (ii = 0; ii < nAC; ++ii)
-		{
-			real_t a = getAbs (delta_yAC[ii]);
-			if (weight < a) weight = a;
-		}
-		for (ii = 0; ii < nFX; ++ii)
-		{
-			real_t a = getAbs (delta_yFX[ii]);
-			if (weight < a) weight = a;
-		}
-
-		/* look at the "zero" in a relative inf-norm */
-		real_t zero = 0.0;
-		for (ii = 0; ii < nFX; ++ii)
-		{
-			real_t a = getAbs (delta_xFX[ii]);
-			if (zero < a) zero = a;
-		}
-		for (ii = 0; ii < nFR; ++ii)
-		{
-			real_t a = getAbs (delta_xFR[ii]);
-			if (zero < a) zero = a;
-		}
-
-		/* relative test against zero in inf-norm */
-		if (zero > options.epsLITests * weight)
-			returnvalue = RET_LINEARLY_INDEPENDENT;
-
-		delete[] nul;
-		delete[] delta_yFX;
-		delete[] delta_yAC;
-		delete[] delta_xFR;
-		delete[] delta_xFX;
-		delete[] delta_g;
-
-	}
-	else
-	{
-		/*
-		 * cheap LI test for simple bound. Check if constraint <number> is
-		 * linearly independent from the the active ones (<=> is element of null
-		 * space of Afr).
-		 */
-
-		/* some definitions */
-		int nZ  = getNZ( );
-
-		for( i=0; i<nZ; ++i )
-			if ( getAbs( QQ(number,i) ) > options.epsLITests )
-			{
-				returnvalue = RET_LINEARLY_INDEPENDENT;
-				break;
-			}
-	}
-
-	return THROWINFO( returnvalue );
-}
-
-
-/*
- *	a d d B o u n d _ e n s u r e L I
- */
-returnValue QProblem::addBound_ensureLI( int number, SubjectToStatus B_status )
-{
-	int i, ii;
-	int nV  = getNV( );
-	int nFX = getNFX( );
-	int nAC = getNAC( );
-	int nZ  = getNZ( );
-
-
-	/* I) Check if new constraint is linearly independent from the active ones. */
-	returnValue returnvalueCheckLI = addBound_checkLI( number );
-
-	if ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED )
-		return THROWERROR( RET_ENSURELI_FAILED );
-
-	if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
-		return SUCCESSFUL_RETURN;
-
-
- 	/* II) NEW BOUND IS LINEARLY DEPENDENT: */
-	/* 1) Determine coefficients of linear combination,
-	 *    cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter:
-	 *    An Algorithm for the Solution of the Parametric Quadratic Programming
-	 *    Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */
-	int* FR_idx;
-	bounds.getFree( )->getNumberArray( &FR_idx );
-
-	int* FX_idx;
-	bounds.getFixed( )->getNumberArray( &FX_idx );
-
-	int* AC_idx;
-	constraints.getActive( )->getNumberArray( &AC_idx );
-
-	real_t* xiC = new real_t[nAC];
-	real_t* xiC_TMP = new real_t[nAC];
-	real_t* xiB = new real_t[nFX];
-	real_t* num = new real_t[nV];
-
-	real_t y_min = options.maxDualJump;
-	int y_min_number = -1;
-	int y_min_number_bound = -1;
-	BooleanType y_min_isBound = BT_FALSE;
-
-	returnValue returnvalue = SUCCESSFUL_RETURN;
-
-
-	/* 2) Calculate xiC. */
-	if ( nAC > 0 )
-	{
-		if ( B_status == ST_LOWER )
-		{
-			for( i=0; i<nAC; ++i )
-				xiC_TMP[i] = QQ(number,nZ+i);
-		}
-		else
-		{
-			for( i=0; i<nAC; ++i )
-				xiC_TMP[i] = -QQ(number,nZ+i);
-		}
-
-		if ( backsolveT( xiC_TMP, BT_TRUE, xiC ) != SUCCESSFUL_RETURN )
-		{
-			returnvalue = RET_ENSURELI_FAILED_TQ;
-			goto farewell;
-		}
-	}
-
-	/* 3) Calculate xiB. */
-	A->transTimes(constraints.getActive(), bounds.getFixed(), 1, -1.0, xiC, nAC, 0.0, xiB, nFX);
-
-
-	/* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */
-
-	/* 1) Constraints. */
-	for( i=0; i<nAC; ++i )
-	{
-		ii = AC_idx[i];
-		num[i] = y[nV+ii];
-	}
-
-	performRatioTest( nAC,AC_idx,&constraints, num,xiC, options.epsNum,options.epsDen, y_min,y_min_number );
-
-	/* 2) Bounds. */
-	for( i=0; i<nFX; ++i )
-	{
-		ii = FX_idx[i];
-		num[i] = y[ii];
-	}
-
-	performRatioTest( nFX,FX_idx,&bounds, num,xiB, options.epsNum,options.epsDen, y_min,y_min_number_bound );
-
-	if ( y_min_number_bound >= 0 )
-	{
-		y_min_number = y_min_number_bound;
-		y_min_isBound = BT_TRUE;
-	}
-
-	/* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
-	#ifndef __XPCTARGET__
-	char messageString[MAX_STRING_LENGTH];
-	#endif
-
-	if ( y_min_number >= 0 )
-	{
-		/* Update Lagrange multiplier... */
-		for( i=0; i<nAC; ++i )
-		{
-			ii = AC_idx[i];
-			y[nV+ii] -= y_min * xiC[i];
-		}
-		for( i=0; i<nFX; ++i )
-		{
-			ii = FX_idx[i];
-			y[ii] -= y_min * xiB[i];
-		}
-
-		/* ... also for newly active bound ... */
-		if ( B_status == ST_LOWER )
-			y[number] = y_min;
-		else
-			y[number] = -y_min;
-
-		/* ... and for bound to be removed. */
-		if ( y_min_isBound == BT_TRUE )
-		{
-			#ifndef __XPCTARGET__
-			snprintf( messageString,MAX_STRING_LENGTH,"bound no. %d.",y_min_number );
-			getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-			#endif
-
-			if ( removeBound( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
-			{
-				returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
-				goto farewell;
-			}
-			tabularOutput.excRemB = 1;
-
-			y[y_min_number] = 0.0;
-		}
-		else
-		{
-			#ifndef __XPCTARGET__
-			snprintf( messageString,MAX_STRING_LENGTH,"constraint no. %d.",y_min_number );
-			getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-			#endif
-
-			if ( removeConstraint( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
-			{
-				returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
-				goto farewell;
-			}
-			tabularOutput.excRemC = 1;
-
-			y[nV+y_min_number] = 0.0;
-		}
-	}
-	else
-	{
-		if (options.enableDropInfeasibles == BT_TRUE) {
-			/* dropping of infeasible constraints according to drop priorities */
-			returnvalue = dropInfeasibles (number, B_status, BT_TRUE, xiB, xiC);
-		}
-		else
-		{
-			/* no constraint/bound can be removed => QP is infeasible! */
-			returnvalue = RET_ENSURELI_FAILED_NOINDEX;
-			setInfeasibilityFlag( returnvalue );
-		}
-	}
-
-farewell:
-	delete[] num;
-	delete[] xiB;
-	delete[] xiC_TMP;
-	delete[] xiC;
-
-	getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-
-	return ( (returnvalue != SUCCESSFUL_RETURN) && (returnvalue != RET_ENSURELI_FAILED_NOINDEX ) ) ? THROWERROR (returnvalue) : returnvalue;
-}
-
-
-
-/*
- *	r e m o v e C o n s t r a i n t
- */
-returnValue QProblem::removeConstraint(	int number,
-										BooleanType updateCholesky,
-										BooleanType allowFlipping,
-										BooleanType ensureNZC
-										)
-{
-	int i, j, ii, jj;
-	returnValue returnvalue = SUCCESSFUL_RETURN;
-	BooleanType hasFlipped = BT_FALSE;
-
-	/* consistency check */
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
-		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
- 		 ( getStatus( ) == QPS_SOLVED )            )
-	{
-		return THROWERROR( RET_UNKNOWN_BUG );
-	}
-
-	/* some definitions */
-	int nV  = getNV( );
-	int nFR = getNFR( );
-	int nAC = getNAC( );
-	int nZ  = getNZ( );
-
-	int tcol = sizeT - nAC;
-	int number_idx = constraints.getActive( )->getIndex( number );
-
-	int addIdx;
-	BooleanType addBoundNotConstraint;
-	SubjectToStatus addStatus;
-	BooleanType exchangeHappened = BT_FALSE;
-
-
-	/* consistency checks */
-	if ( constraints.getStatus( number ) == ST_INACTIVE )
-		return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );
-
-	if ( ( number_idx < 0 ) || ( number_idx >= nAC ) )
-		return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );
-
-
-	int* FR_idx;
-	bounds.getFree( )->getNumberArray( &FR_idx );
-
-	/* N) PERFORM ZERO CURVATURE TEST. */
-	if (ensureNZC == BT_TRUE)
-	{
-		returnvalue = ensureNonzeroCurvature(BT_FALSE, number, exchangeHappened, addBoundNotConstraint, addIdx, addStatus);
-
-		if (returnvalue != SUCCESSFUL_RETURN)
-			return returnvalue;
-	}
-
-	/* save index sets and decompositions for flipping bounds strategy */
-	if ( ( exchangeHappened == BT_FALSE ) && ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) )
-		flipper.set( &bounds,R,&constraints,Q,T );
-
-	/* I) REMOVE <number>th ROW FROM T,
-	 *    i.e. shift rows number+1 through nAC  upwards (instead of the actual
-	 *    constraint number its corresponding index within matrix A is used). */
-	if ( number_idx < nAC-1 )
-	{
-		for( i=(number_idx+1); i<nAC; ++i )
-			for( j=(nAC-i-1); j<nAC; ++j )
-				TT(i-1,tcol+j) = TT(i,tcol+j);
-		/* gimmick: write zeros into the last row of T */
-		for( j=0; j<nAC; ++j )
-			TT(nAC-1,tcol+j) = 0.0;
-
-
-		/* II) RESTORE TRIANGULAR FORM OF T,
-		 *     use column-wise Givens rotations to restore reverse triangular form
-		 *     of T simultanenous change of Q (i.e. Y). */
-		real_t c, s, nu;
-
-		for( j=(nAC-2-number_idx); j>=0; --j )
-		{
-			computeGivens( TT(nAC-2-j,tcol+1+j),TT(nAC-2-j,tcol+j), TT(nAC-2-j,tcol+1+j),TT(nAC-2-j,tcol+j),c,s );
-			nu = s/(1.0+c);
-
-			for( i=(nAC-j-1); i<(nAC-1); ++i )
-				applyGivens( c,s,nu,TT(i,tcol+1+j),TT(i,tcol+j), TT(i,tcol+1+j),TT(i,tcol+j) );
-
-			for( i=0; i<nFR; ++i )
-			{
-				ii = FR_idx[i];
-				applyGivens( c,s,nu,QQ(ii,nZ+1+j),QQ(ii,nZ+j), QQ(ii,nZ+1+j),QQ(ii,nZ+j) );
-			}
-		}
-	}
-	else
-	{
-		/* gimmick: write zeros into the last row of T */
-		for( j=0; j<nAC; ++j )
-			TT(nAC-1,tcol+j) = 0.0;
-	}
-
-
-	if ( ( updateCholesky == BT_TRUE ) &&
-		 ( hessianType != HST_ZERO )   && ( hessianType != HST_IDENTITY ) )
-	{
-		/* III) UPDATE CHOLESKY DECOMPOSITION,
-		 *      calculate new additional column (i.e. [r sqrt(rho2)]')
-		 *      of the Cholesky factor R. */
-		real_t* Hz = new real_t[nFR];
-		real_t* z = new real_t[nFR];
-		real_t rho2 = 0.0;
-
-		/* 1) Calculate Hz = H*z, where z is the new rightmost column of Z
-		 *    (i.e. the old leftmost column of Y).  */
-		for( j=0; j<nFR; ++j )
-			z[j] = QQ(FR_idx[j],nZ);
-		H->times(bounds.getFree(), bounds.getFree(), 1, 1.0, z, nFR, 0.0, Hz, nFR);
-		delete[] z;
-
-		if ( nZ > 0 )
-		{
-			real_t* ZHz = new real_t[nZ];
-			for ( i=0; i<nZ; ++i )
-				ZHz[i] = 0.0;
-			real_t* r = new real_t[nZ];
-
-			/* 2) Calculate ZHz = Z'*Hz (old Z). */
-			for( j=0; j<nFR; ++j )
-			{
-				jj = FR_idx[j];
-
-				for( i=0; i<nZ; ++i )
-					ZHz[i] += QQ(jj,i) * Hz[j];
-			}
-
-			/* 3) Calculate r = R^-T * ZHz. */
-			if ( backsolveR( ZHz,BT_TRUE,r ) != SUCCESSFUL_RETURN )
-			{
-				delete[] Hz; delete[] r; delete[] ZHz;
-				return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
-			}
-
-			/* 4) Calculate rho2 = rho^2 = z'*Hz - r'*r
-			 *    and store r into R. */
-			for( i=0; i<nZ; ++i )
-			{
-				rho2 -= r[i]*r[i];
-				RR(i,nZ) = r[i];
-			}
-
-			delete[] r; delete[] ZHz;
-		}
-
-		/* 5) Store rho into R. */
-		for( j=0; j<nFR; ++j )
-			rho2 += QQ(FR_idx[j],nZ) * Hz[j];
-
-		delete[] Hz;
-
-		if ( ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
-		{
-			if ( rho2 > options.epsFlipping )
-				RR(nZ,nZ) = getSqrt( rho2 );
-			else
-			{
-				hessianType = HST_SEMIDEF;
-
-				flipper.get( &bounds,R,&constraints,Q,T );
-				constraints.flipFixed(number);
-				tabularOutput.idxAddC = number;
-				tabularOutput.excAddC = 2;
-
-				switch (constraints.getStatus(number))
-				{
-					case ST_LOWER:
-						lbA[number] = ubA[number]; Ax_l[number] = -Ax_u[number]; break;
-					case ST_UPPER:
-						ubA[number] = lbA[number]; Ax_u[number] = -Ax_l[number]; break;
-					default:
-						return THROWERROR( RET_MOVING_BOUND_FAILED );
-				}
-
-				hasFlipped = BT_TRUE;
-			}
-		}
-		else if ( exchangeHappened == BT_FALSE )
-		{
-			if ( rho2 > ZERO )
-				RR(nZ,nZ) = getSqrt( rho2 );
-			else
-			{
-				if ( allowFlipping == BT_FALSE )
-				{
-					RR(nZ,nZ) = 100.0*EPS;
-				}
-				else
-				{
-					hessianType = HST_SEMIDEF;
-					return THROWERROR( RET_HESSIAN_NOT_SPD );
-				}
-			}
-		}
-	}
-
-
-	/* IV) UPDATE INDICES */
-	tabularOutput.idxRemC = number;
-	if ( hasFlipped == BT_FALSE )
-	{
-		if ( constraints.moveActiveToInactive( number ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
-	}
-
-	if (exchangeHappened == BT_TRUE)
-	{
-		/* add bound or constraint */
-
-		/* hessianType = HST_SEMIDEF; */
-		RR(nZ,nZ) = 0.0;
-
-		if ( addBoundNotConstraint )
-		{
-			addBound(addIdx, addStatus, BT_TRUE, BT_FALSE);
-			tabularOutput.excAddB = 1;
-		}
-		else
-		{
-			addConstraint(addIdx, addStatus, BT_TRUE, BT_FALSE);
-			tabularOutput.excAddC = 1;
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	r e m o v e B o u n d
- */
-returnValue QProblem::removeBound(	int number,
-									BooleanType updateCholesky,
-									BooleanType allowFlipping,
-									BooleanType ensureNZC
-									)
-{
-	int i, j, ii, jj;
-	returnValue returnvalue = SUCCESSFUL_RETURN;
-	int addIdx;
-	BooleanType addBoundNotConstraint;
-	SubjectToStatus addStatus;
-	BooleanType exchangeHappened = BT_FALSE;
-
-
-	/* consistency checks */
-	if ( bounds.getStatus( number ) == ST_INACTIVE )
-		return THROWERROR( RET_BOUND_NOT_ACTIVE );
-
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
-		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
- 		 ( getStatus( ) == QPS_SOLVED )            )
-	{
-		return THROWERROR( RET_UNKNOWN_BUG );
-	}
-
-	/* some definitions */
-	int nV  = getNV( );
-	int nFR = getNFR( );
-	int nAC = getNAC( );
-	int nZ  = getNZ( );
-
-	int tcol = sizeT - nAC;
-
-	/* 0) PERFORM ZERO CURVATURE TEST. */
-	if (ensureNZC == BT_TRUE)
-	{
-		returnvalue = ensureNonzeroCurvature(BT_TRUE, number, exchangeHappened, addBoundNotConstraint, addIdx, addStatus);
-
-		if (returnvalue != SUCCESSFUL_RETURN)
-			return returnvalue;
-	}
-
-	/* save index sets and decompositions for flipping bounds strategy */
-	if ( ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
-		flipper.set( &bounds,R,&constraints,Q,T );
-
-	/* I) UPDATE INDICES */
-	tabularOutput.idxRemB = number;
-	if ( bounds.moveFixedToFree( number ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_REMOVEBOUND_FAILED );
-
-	int* FR_idx;
-	bounds.getFree( )->getNumberArray( &FR_idx );
-
-	/* I) APPEND <nFR+1>th UNITY VECTOR TO Q. */
-	int nnFRp1 = FR_idx[nFR];
-	for( i=0; i<nFR; ++i )
-	{
-		ii = FR_idx[i];
-		QQ(ii,nFR) = 0.0;
-		QQ(nnFRp1,i) = 0.0;
-	}
-	QQ(nnFRp1,nFR) = 1.0;
-
-	if ( nAC > 0 )
-	{
-		/* store new column a in a temporary vector instead of shifting T one column to the left and appending a */
-		int* AC_idx;
-		constraints.getActive( )->getNumberArray( &AC_idx );
-
-		real_t* tmp = new real_t[nAC];
-		A->getCol(number, constraints.getActive(), 1.0, tmp);
-
-
-		/* II) RESTORE TRIANGULAR FORM OF T,
-		 *     use column-wise Givens rotations to restore reverse triangular form
-		 *     of T = [T A(:,number)], simultanenous change of Q (i.e. Y and Z). */
-		real_t c, s, nu;
-
-		for( j=(nAC-1); j>=0; --j )
-		{
-			computeGivens( tmp[nAC-1-j],TT(nAC-1-j,tcol+j),TT(nAC-1-j,tcol+j),tmp[nAC-1-j],c,s );
-			nu = s/(1.0+c);
-
-			for( i=(nAC-j); i<nAC; ++i )
-				applyGivens( c,s,nu,tmp[i],TT(i,tcol+j),TT(i,tcol+j),tmp[i] );
-
-			for( i=0; i<=nFR; ++i )
-			{
-				ii = FR_idx[i];
-				/* nZ+1+nAC = nFR+1  /  nZ+(1) = nZ+1 */
-				applyGivens( c,s,nu,QQ(ii,nZ+1+j),QQ(ii,nZ+j),QQ(ii,nZ+1+j),QQ(ii,nZ+j) );
-			}
-		}
-
-		delete[] tmp;
-	}
-
-
-	if ( ( updateCholesky == BT_TRUE ) &&
-		 /*( hessianType != HST_ZERO )   &&*/ ( hessianType != HST_IDENTITY ) )
-	{
-		/* III) UPDATE CHOLESKY DECOMPOSITION,
-		 *      calculate new additional column (i.e. [r sqrt(rho2)]')
-		 *      of the Cholesky factor R: */
-		real_t z2 = QQ(nnFRp1,nZ);
-		real_t rho2;
-
-		if ( hessianType != HST_ZERO )
-			rho2 = H->diag(nnFRp1)*z2*z2; /* rho2 = h2*z2*z2 */
-		else
-			rho2 = 0.0;
-
-		if ( nFR > 0 )
-		{
-			/* Attention: Index list of free variables has already grown by one! */
-			real_t* Hz = new real_t[nFR+1];
-			real_t* z = new real_t[nFR+1];
-			/* 1) Calculate R'*r = Zfr'*Hfr*z1 + z2*Zfr'*h1 =: Zfr'*Hz + z2*Zfr'*h1 =: rhs and
-			 *    rho2 = z1'*Hfr*z1 + 2*z2*h1'*z1 + h2*z2^2 - r'*r =: z1'*Hz + 2*z2*h1'*z1 + h2*z2^2 - r'r */
-			for( j=0; j<nFR; ++j )
-				z[j] = QQ(FR_idx[j],nZ);
-			z[nFR] = 0.0;
-			H->times(bounds.getFree(), bounds.getFree(), 1, 1.0, z, nFR+1, 0.0, Hz, nFR+1);
-
-			H->getCol(nnFRp1, bounds.getFree(), 1.0, z);
-
-			if ( nZ > 0 )
-			{
-				real_t* r = new real_t[nZ];
-				real_t* rhs = new real_t[nZ];
-				for( i=0; i<nZ; ++i )
-					rhs[i] = 0.0;
-
-				/* 2) Calculate rhs. */
-				for( j=0; j<nFR; ++j )
-				{
-					jj = FR_idx[j];
-					for( i=0; i<nZ; ++i )
-										/* Zfr' * ( Hz + z2*h1 ) */
-						rhs[i] += QQ(jj,i) * ( Hz[j] + z2 * z[j] );
-				}
-
-				/* 3) Calculate r = R^-T * rhs. */
-				if ( backsolveR( rhs,BT_TRUE,BT_TRUE,r ) != SUCCESSFUL_RETURN )
-				{
-					delete[] z;
-					delete[] Hz; delete[] r; delete[] rhs;
-					return THROWERROR( RET_REMOVEBOUND_FAILED );
-				}
-
-
-				/* 4) Calculate rho2 = rho^2 = z'*Hz - r'*r
-				 *    and store r into R. */
-				for( i=0; i<nZ; ++i )
-				{
-					rho2 -= r[i]*r[i];
-					RR(i,nZ) = r[i];
-				}
-
-				delete[] rhs; delete[] r;
-			}
-
-			for( j=0; j<nFR; ++j )
-			{
-				jj = FR_idx[j];
-							/* z1' * ( Hz + 2*z2*h1 ) */
-				rho2 += QQ(jj,nZ) * ( Hz[j] + 2.0*z2*z[j] );
-			}
-
-			delete[] z;
-			delete[] Hz;
-		}
-
-		/* 5) Store rho into R. */
-		if ( ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
-		{
-			if ( rho2 > options.epsFlipping )
-				RR(nZ,nZ) = getSqrt( rho2 );
-			else
-			{
-				if ( hessianType != HST_ZERO )
-					hessianType = HST_SEMIDEF;
-
-				flipper.get( &bounds,R,&constraints,Q,T );
-				bounds.flipFixed(number);
-				tabularOutput.idxAddB = number;
-				tabularOutput.excAddB = 2;
-
-				switch (bounds.getStatus(number))
-				{
-					case ST_LOWER: 
-						lb[number] = ub[number]; 
-						break;
-					case ST_UPPER: 
-						ub[number] = lb[number]; 
-						break;
-					default: return THROWERROR( RET_MOVING_BOUND_FAILED );
-				}
-
-			}
-		}
-		else if ( exchangeHappened == BT_FALSE )
-		{
-			if ( rho2 > ZERO )
-				RR(nZ,nZ) = getSqrt( rho2 );
-			else
-			{
-				if ( allowFlipping == BT_FALSE )
-					RR(nZ,nZ) = 100.0*EPS;
-				else
-				{
-					hessianType = HST_SEMIDEF;
-					return THROWERROR( RET_HESSIAN_NOT_SPD );
-				}
-			}
-		}
-		else
-		{
-			/* add bound or constraint */
-
-			/* hessianType = HST_SEMIDEF; */
-			RR(nZ,nZ) = 0.0;
-
-			if ( addBoundNotConstraint )
-			{
-				addBound(addIdx, addStatus, BT_TRUE, BT_FALSE);
-				tabularOutput.excAddB = 1;
-			}
-			else
-			{
-				addConstraint(addIdx, addStatus, BT_TRUE, BT_FALSE);
-				tabularOutput.excAddC = 1;
-			}
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-returnValue QProblem::performPlainRatioTest(	int nIdx,
-												const int* const idxList,
-												const real_t* const num,
-												const real_t* const den,
-												real_t epsNum,
-												real_t epsDen,
-												real_t& t,
-												int& BC_idx
-												) const
-{
-	int i;
-	for (i = 0; i < nIdx; i++)
-		if ( (num[i] > epsNum) && (den[i] > epsDen) && (t * den[i] > num[i]) )
-		{
-			t = num[i] / den[i];
-			BC_idx = idxList[i];
-		}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-returnValue QProblem::ensureNonzeroCurvature(	BooleanType removeBoundNotConstraint,
-												int remIdx,
-												BooleanType &exchangeHappened,
-												BooleanType &addBoundNotConstraint,
-												int &addIdx,
-												SubjectToStatus &addStatus
-												)
-{
-	int i, ii;
-	int addLBndIdx = -1, addLCnstrIdx = -1, addUBndIdx = -1, addUCnstrIdx = -1; /* exchange indices */
-	int *FX_idx, *AC_idx, *IAC_idx;
-	returnValue returnvalue = SUCCESSFUL_RETURN;
-
-	int nV  = getNV( );
-	int nFR = getNFR( );
-	int nAC = getNAC( );
-	int nC  = getNC( );
-	int nFX = getNFX( );
-	int nIAC = getNIAC( );
-
-	int* FR_idx;
-	bounds.getFree( )->getNumberArray( &FR_idx );
-
-// 	real_t *delta_g   = new real_t[nV];
-	real_t *delta_xFX = new real_t[nFX];
-	real_t *delta_xFR = new real_t[nFR];
-	real_t *delta_yAC = new real_t[nAC];
-	real_t *delta_yFX = new real_t[nFX];
-
-	bounds.getFixed( )->getNumberArray( &FX_idx );
-	constraints.getActive( )->getNumberArray( &AC_idx );
-	constraints.getInactive( )->getNumberArray( &IAC_idx );
-
-	addBoundNotConstraint = BT_TRUE;
-	addStatus = ST_INACTIVE;
-	exchangeHappened = BT_FALSE;
-
-	if (removeBoundNotConstraint)
-	{
-		int dim = nV < nC ? nC : nV;
-		real_t *nul = new real_t[dim];
-		real_t *ek = new real_t[nV]; /* minus e_k (bound k is removed) */
-		for (ii = 0; ii < dim; ++ii)
-			nul[ii]=0.0;
-		for (ii = 0; ii < nV; ++ii)
-			ek[ii]=0.0;
-		ek[remIdx] = bounds.getStatus(remIdx) == ST_LOWER ? 1.0 : -1.0;
-
-		returnvalue = determineStepDirection (nul, nul, nul, ek, ek,
-											  BT_FALSE, BT_FALSE,
-											  delta_xFX, delta_xFR, delta_yAC, delta_yFX);
-		delete[] ek;
-		delete[] nul;
-	}
-	else
-	{
-		real_t *nul = new real_t[nV];
-		real_t *ek = new real_t[nC]; /* minus e_k (constraint k is removed) */
-		for (ii = 0; ii < nV; ++ii)
-			nul[ii]=0.0;
-		for (ii = 0; ii < nC; ++ii)
-			ek[ii]=0.0;
-		ek[remIdx] = constraints.getStatus(remIdx) == ST_LOWER ? 1.0 : -1.0;
-
-		returnvalue = determineStepDirection (nul,
-											  ek, ek, nul, nul,
-											  BT_FALSE, BT_TRUE,
-											  delta_xFX, delta_xFR, delta_yAC, delta_yFX);
-		delete[] ek;
-		delete[] nul;
-	}
-
-	/* compute the weight in inf-norm */
-	real_t normXi = 0.0;
-	for (ii = 0; ii < nAC; ++ii)
-	{
-		real_t a = getAbs (delta_yAC[ii]);
-		if (normXi < a) normXi = a;
-	}
-	for (ii = 0; ii < nFX; ++ii)
-	{
-		real_t a = getAbs (delta_yFX[ii]);
-		if (normXi < a) normXi = a;
-	}
-
-	/* look at the "zero" in a relative inf-norm */
-	real_t normS = 0.0;
-	for (ii = 0; ii < nFX; ++ii)
-	{
-		real_t a = getAbs (delta_xFX[ii]);
-		if (normS < a) normS = a;
-	}
-	for (ii = 0; ii < nFR; ++ii)
-	{
-		real_t a = getAbs (delta_xFR[ii]);
-		if (normS < a) normS = a;
-	}
-
-	/* relative test against zero in inf-norm */
-	if (normXi < options.epsNZCTests * normS)
-	{
-		/* determine jump in x via ratio tests */
-		real_t sigmaLBnd, sigmaLCnstr, sigmaUBnd, sigmaUCnstr, sigma;
-
-		/* bounds */
-
-		/* compress x-u */
-		real_t *x_W = new real_t[getMax(1,nFR)];
-		for (i = 0; i < nFR; i++)
-		{
-			ii = FR_idx[i];
-			x_W[i] = ub[ii] - x[ii];
-		}
-		/* performRatioTest( nFR,FR_idx,&bounds, x_W,delta_xFR, options.epsNum,options.epsDen, sigmaUBnd,addUBndIdx ); */
-		sigmaUBnd = options.maxPrimalJump;
-		addUBndIdx = -1;
-		performPlainRatioTest(nFR, FR_idx, x_W, delta_xFR, options.epsNum, options.epsDen, sigmaUBnd, addUBndIdx);
-		if (removeBoundNotConstraint == BT_TRUE && bounds.getStatus(remIdx) == ST_LOWER)
-		{
-			/* also consider bound which is to be removed */
-			real_t one = 1.0;
-			x_W[0] = ub[remIdx] - x[remIdx];
-			performPlainRatioTest(1, &remIdx, x_W, &one, options.epsNum, options.epsDen, sigmaUBnd, addUBndIdx);
-		}
-
-		/* compress x-l */
-		for (i = 0; i < nFR; i++)
-		{
-			ii = FR_idx[i];
-			x_W[i] = x[ii] - lb[ii];
-		}
-		for (i = 0; i < nFR; i++)
-			delta_xFR[i] = -delta_xFR[i];
-		/* performRatioTest( nFR,FR_idx,&bounds, x_W,delta_xFR, options.epsNum,options.epsDen, sigmaLBnd,addLBndIdx ); */
-		sigmaLBnd = options.maxPrimalJump;
-		addLBndIdx = -1;
-		performPlainRatioTest(nFR, FR_idx, x_W, delta_xFR, options.epsNum, options.epsDen, sigmaLBnd, addLBndIdx);
-		if (removeBoundNotConstraint == BT_TRUE && bounds.getStatus(remIdx) == ST_UPPER)
-		{
-			/* also consider bound which is to be removed */
-			real_t one = 1.0;
-			x_W[0] = x[remIdx] - lb[remIdx];
-			performPlainRatioTest(1, &remIdx, x_W, &one, options.epsNum, options.epsDen, sigmaLBnd, addLBndIdx);
-		}
-		for (i = 0; i < nFR; i++)
-			delta_xFR[i] = -delta_xFR[i];
-
-		delete[] x_W;
-
-		/* constraints */
-
-		/* compute As (compressed to inactive constraints) */
-		real_t *As = new real_t[nIAC];
-		A->times(constraints.getInactive(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 0.0, As, nIAC);
-		A->times(constraints.getInactive(), bounds.getFree(), 1, 1.0, delta_xFR, nFR, 1.0, As, nIAC);
-
-		/* compress Ax_u */
-		real_t *Ax_W = new real_t[nIAC];
-		for (i = 0; i < nIAC; i++)
-		{
-			ii = IAC_idx[i];
-			Ax_W[i] = Ax_u[ii];
-		}
-		/* performRatioTest( nIAC,IAC_idx,&constraints, Ax_W,As, options.epsNum,options.epsDen, sigmaUCnstr,addUCnstrIdx ); */
-		sigmaUCnstr = options.maxPrimalJump;
-		addUCnstrIdx = -1;
-		performPlainRatioTest(nIAC, IAC_idx, Ax_W, As, options.epsNum, options.epsDen, sigmaUCnstr, addUCnstrIdx);
-		if (removeBoundNotConstraint == BT_FALSE && constraints.getStatus(remIdx) == ST_LOWER)
-		{
-			/* also consider constraint which is to be removed */
-			real_t one = 1.0;
-			performPlainRatioTest(1, &remIdx, &Ax_u[remIdx], &one, options.epsNum, options.epsDen, sigmaUCnstr, addUCnstrIdx);
-		}
-
-		/* compress Ax_l */
-		for (i = 0; i < nIAC; i++)
-		{
-			ii = IAC_idx[i];
-			Ax_W[i] = Ax_l[ii];
-		}
-		for (i = 0; i < nIAC; i++)
-			As[i] = -As[i];
-		/* performRatioTest( nIAC,IAC_idx,&constraints, Ax_W,As, options.epsNum,options.epsDen, sigmaLCnstr,addLCnstrIdx ); */
-		sigmaLCnstr = options.maxPrimalJump;
-		addLCnstrIdx = -1;
-		performPlainRatioTest(nIAC, IAC_idx, Ax_W, As, options.epsNum, options.epsDen, sigmaLCnstr, addLCnstrIdx);
-		if (removeBoundNotConstraint == BT_FALSE && constraints.getStatus(remIdx) == ST_UPPER)
-		{
-			/* also consider constraint which is to be removed */
-			real_t one = 1.0;
-			performPlainRatioTest(1, &remIdx, &Ax_l[remIdx], &one, options.epsNum, options.epsDen, sigmaLCnstr, addLCnstrIdx);
-		}
-
-		/* perform primal jump */
-		sigma = options.maxPrimalJump;
-		if (sigmaUCnstr < sigma) { sigma = sigmaUCnstr; addStatus = ST_UPPER; addBoundNotConstraint = BT_FALSE; addIdx = addUCnstrIdx; }
-		if (sigmaLCnstr < sigma) { sigma = sigmaLCnstr; addStatus = ST_LOWER; addBoundNotConstraint = BT_FALSE; addIdx = addLCnstrIdx; }
-		if (sigmaUBnd < sigma) { sigma = sigmaUBnd; addStatus = ST_UPPER; addBoundNotConstraint = BT_TRUE; addIdx = addUBndIdx; }
-		if (sigmaLBnd < sigma) { sigma = sigmaLBnd; addStatus = ST_LOWER; addBoundNotConstraint = BT_TRUE; addIdx = addLBndIdx; }
-
-		if (sigma >= options.maxPrimalJump)
-		{
-			unbounded = BT_TRUE;
-			returnvalue = RET_HOTSTART_STOPPED_UNBOUNDEDNESS;
-		}
-		else
-		{
-			for (i = 0; i < nFR; i++)
-				x[FR_idx[i]] += sigma * delta_xFR[i];
-
-			for (i = 0; i < nFX; i++)
-				x[FX_idx[i]] += sigma * delta_xFX[i];
-
-			/* update Ax, Ax_u, and Ax_l */
-			A->times(1, 1.0, x, nV, 0.0, Ax, nC);
-			for (i = 0; i < nC; i++) Ax_u[i] = ubA[i] - Ax[i];
-			for (i = 0; i < nC; i++) Ax_l[i] = Ax[i] - lbA[i];
-
-			/* change working set later */
-			exchangeHappened = BT_TRUE;
-		}
-
-		delete[] Ax_W;
-		delete[] As;
-	}
-
-	delete[] delta_yFX;
-	delete[] delta_yAC;
-	delete[] delta_xFR;
-	delete[] delta_xFX;
-// 	delete[] delta_g;
-
-	return returnvalue;
-}
-
-
-
-/*
- *	b a c k s o l v e T
- */
-returnValue QProblem::backsolveT( const real_t* const b, BooleanType transposed, real_t* const a ) const
-{
-	int i, j;
-	int nT = getNAC( );
-	int tcol = sizeT - nT;
-
-	real_t sum;
-
-	/* nothing to do */
-	if ( nT <= 0 )
-		return SUCCESSFUL_RETURN;
-
-
-	/* Solve Ta = b, where T might be transposed. */
-	if ( transposed == BT_FALSE )
-	{
-		/* solve Ta = b */
-		for( i=0; i<nT; ++i )
-		{
-			sum = b[i];
-			for( j=0; j<i; ++j )
-				sum -= TT(i,sizeT-1-j) * a[nT-1-j];
-
-			if ( getAbs( TT(i,sizeT-1-i) ) > EPS )
-				a[nT-1-i] = sum / TT(i,sizeT-1-i);
-			else
-				return THROWERROR( RET_DIV_BY_ZERO );
-		}
-	}
-	else
-	{
-		/* solve T^T*a = b */
-		for( i=0; i<nT; ++i )
-		{
-			sum = b[i];
-			for( j=0; j<i; ++j )
-				sum -= TT(nT-1-j,tcol+i) * a[nT-1-j];
-
-			if ( getAbs( TT(nT-1-i,tcol+i) ) > EPS )
-				a[nT-1-i] = sum / TT(nT-1-i,tcol+i);
-			else
-				return THROWERROR( RET_DIV_BY_ZERO );
-		}
-	}
-
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	d e t e r m i n e D a t a S h i f t
- */
-returnValue QProblem::determineDataShift(	const real_t* const g_new, const real_t* const lbA_new, const real_t* const ubA_new,
-											const real_t* const lb_new, const real_t* const ub_new,
-											real_t* const delta_g, real_t* const delta_lbA, real_t* const delta_ubA,
-											real_t* const delta_lb, real_t* const delta_ub,
-											BooleanType& Delta_bC_isZero, BooleanType& Delta_bB_isZero
-											)
-{
-	int i, ii;
-	int nC  = getNC( );
-	int nAC = getNAC( );
-
-	int* FX_idx;
-	int* AC_idx;
-
-	bounds.getFixed( )->getNumberArray( &FX_idx );
-	constraints.getActive( )->getNumberArray( &AC_idx );
-
-
-
-	/* I) DETERMINE DATA SHIFT FOR BOUNDS */
-	QProblemB::determineDataShift(	g_new,lb_new,ub_new,
-									delta_g,delta_lb,delta_ub,
-									Delta_bB_isZero );
-
-
-	/* II) DETERMINE DATA SHIFT FOR CONSTRAINTS */
-	/* 1) Calculate shift directions. */
-	for( i=0; i<nC; ++i )
-	{
-		/* if lower constraints' bounds are to be disabled or do not exist, shift them to -infinity */
-		if ( lbA_new != 0 )
-			delta_lbA[i] = lbA_new[i] - lbA[i];
-		else
-			delta_lbA[i] = -INFTY - lbA[i];
-	}
-
-	for( i=0; i<nC; ++i )
-	{
-		/* if upper constraints' bounds are to be disabled or do not exist, shift them to infinity */
-		if ( ubA_new != 0 )
-			delta_ubA[i] = ubA_new[i] - ubA[i];
-		else
-			delta_ubA[i] = INFTY - ubA[i];
-	}
-
-	/* 2) Determine if active constraints' bounds are to be shifted. */
-	Delta_bC_isZero = BT_TRUE;
-
-	for ( i=0; i<nAC; ++i )
-	{
-		ii = AC_idx[i];
-
-		if ( ( getAbs( delta_lbA[ii] ) > EPS ) || ( getAbs( delta_ubA[ii] ) > EPS ) )
-		{
-			Delta_bC_isZero = BT_FALSE;
-			break;
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	d e t e r m i n e S t e p D i r e c t i o n
- */
-returnValue QProblem::determineStepDirection(	const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA,
-												const real_t* const delta_lb, const real_t* const delta_ub,
-												BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero,
-												real_t* const delta_xFX, real_t* const delta_xFR,
-												real_t* const delta_yAC, real_t* const delta_yFX
-												)
-{
-	int i, j, ii, jj, r;
-	int nV  = getNV( );
-	int nFR = getNFR( );
-	int nFX = getNFX( );
-	int nAC = getNAC( );
-	int nZ  = getNZ( );
-	
-	int* FR_idx;
-	int* FX_idx;
-	int* AC_idx;
-
-	bounds.getFree( )->getNumberArray( &FR_idx );
-	bounds.getFixed( )->getNumberArray( &FX_idx );
-	constraints.getActive( )->getNumberArray( &AC_idx );
-
-
-	/* I) DETERMINE delta_xFX (this is exact, does not need refinement) */
-	if ( Delta_bB_isZero == BT_FALSE )
-	{
-		for( i=0; i<nFX; ++i )
-		{
-			ii = FX_idx[i];
-
-			if ( bounds.getStatus( ii ) == ST_LOWER )
-				delta_xFX[i] = delta_lb[ii];
-			else
-				delta_xFX[i] = delta_ub[ii];
-		}
-	}
-	else
-	{
-		for( i=0; i<nFX; ++i )
-			delta_xFX[i] = 0.0;
-	}
-
-
-	/* tempA and tempB hold the residuals in gFR and bA (= lbA or ubA)
-	 * delta_xFR, delta_yAC hold the steps that get refined */
-	for ( i=0; i<nFR; ++i )
-	{
-		ii = FR_idx[i];
-		tempA[i] = delta_g[ii];
-		delta_xFR[i] = 0.0;
-	}
-	for ( i=0; i<nAC; ++i )
-		delta_yAC[i] = 0.0;
-	if ( Delta_bC_isZero == BT_FALSE )
-	{
-		for ( i=0; i<nAC; ++i )
-		{
-			ii = AC_idx[i];
-			if ( constraints.getStatus( ii ) == ST_LOWER )
-				tempB[i] = delta_lbA[ii];
-			else
-				tempB[i] = delta_ubA[ii];
-		}
-	}
-	else
-	{
-		for ( i=0; i<nAC; ++i )
-			tempB[i] = 0.0;
-	}
-
-	/* Iterative refinement loop for delta_xFRz, delta_xFRy, delta_yAC */
-	for ( r=0; r<=options.numRefinementSteps; ++r )
-	{
-		/* II) DETERMINE delta_xFR */
-		if ( nFR > 0 )
-		{
-			for( i=0; i<nFR; ++i )
-				delta_xFR_TMP[i] = 0.0;
-
-			/* 1) Determine delta_xFRy. */
-			if ( nAC > 0 )
-			{
-				if ( ( Delta_bC_isZero == BT_TRUE ) && ( Delta_bB_isZero == BT_TRUE ) )
-				{
-					for( i=0; i<nAC; ++i )
-						delta_xFRy[i] = 0.0;
-				}
-				else
-				{
-					/* compute bA - A * delta_xFX. tempB already holds bA->
-					 * in refinements r>=1, delta_xFX is exactly zero */
-					if ( ( Delta_bB_isZero == BT_FALSE ) && ( r == 0 ) )
-						A->times(constraints.getActive(), bounds.getFixed(), 1, -1.0, delta_xFX, nFX, 1.0, tempB, nAC);
-
-					if ( backsolveT( tempB, BT_FALSE, delta_xFRy ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_STEPDIRECTION_FAILED_TQ );
-
-					for( i=0; i<nFR; ++i )
-					{
-						ii = FR_idx[i];
-						for( j=0; j<nAC; ++j )
-							delta_xFR_TMP[i] += QQ(ii,nZ+j) * delta_xFRy[j];
-					}
-				}
-			}
-
-
-			/* 2) Determine delta_xFRz. */
-			for( i=0; i<nZ; ++i )
-				delta_xFRz[i] = 0.0;
-
-			if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_IDENTITY ) )
-			{
-				/* compute Z*delta_gFR [/eps] (delta_gFR is stored in tempA) */
-				for( j=0; j<nFR; ++j )
-				{
-					jj = FR_idx[j];
-					for( i=0; i<nZ; ++i )
-						delta_xFRz[i] -= QQ(jj,i) * tempA[j];
-				}
-
-				if ( hessianType == HST_ZERO )
-				{
-					if ( usingRegularisation( ) == BT_TRUE )
-					{
-						for( i=0; i<nZ; ++i )
-							delta_xFRz[i] /= regVal;
-					}
-					else
-					{
-						/* When solving LPs without regularisation, iterates must always ne at a vertex. */
-						if ( nZ > 0 )
-							return THROWERROR( RET_UNKNOWN_BUG );
-					}
-				}
-			}
-			else
-			{
-				/* compute HMX*delta_xFX. DESTROY delta_gFR that was in tempA */
-				if ( ( Delta_bB_isZero == BT_FALSE ) && ( r == 0 ) )
-					H->times(bounds.getFree(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, tempA, nFR);
-
-				/* compute HFR*delta_xFRy */
-				if ( ( nAC > 0 ) && ( ( Delta_bC_isZero == BT_FALSE ) || ( Delta_bB_isZero == BT_FALSE ) ) )
-					H->times(bounds.getFree(), bounds.getFree(), 1, 1.0, delta_xFR_TMP, nFR, 1.0, tempA, nFR);
-
-				/* compute ZFR_delta_xFRz = (Z'*HFR*Z) \ Z * (HFR*delta_xFR + HMX*delta_xFX + delta_gFR) */
-				if ( nZ > 0 )
-				{
-					for( j=0; j<nFR; ++j )
-					{
-						jj = FR_idx[j];
-						for( i=0; i<nZ; ++i )
-							delta_xFRz[i] -= QQ(jj,i) * tempA[j];
-					}
-
-					if ( backsolveR( delta_xFRz,BT_TRUE,delta_xFRz ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
-
-					if ( backsolveR( delta_xFRz,BT_FALSE,delta_xFRz ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
-				}
-			}
-
-			/* compute Z * ZFR_delta_xFRz */
-			if ( nZ > 0 )
-			{
-				for( i=0; i<nFR; ++i )
-				{
-					ZFR_delta_xFRz[i] = 0.0;
-
-					ii = FR_idx[i];
-					for( j=0; j<nZ; ++j )
-						ZFR_delta_xFRz[i] += QQ(ii,j) * delta_xFRz[j];
-
-					delta_xFR_TMP[i] += ZFR_delta_xFRz[i];
-				}
-			}
-		}
-
-		/* III) DETERMINE delta_yAC */
-		if ( nAC > 0 ) /* => ( nFR = nZ + nAC > 0 ) */
-		{
-			if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_IDENTITY ) )
-			{
-				/* if zero:     delta_yAC = (T')^-1 * ( Yfr*delta_gFR + eps*delta_xFRy ),
-				 * if identity: delta_yAC = (T')^-1 * ( Yfr*delta_gFR +     delta_xFRy )
-				 *
-				 * DESTROY residual_bA that was stored in tempB
-				 * If we come here, residual_gFR in tempA is STILL VALID
-				 */
-				if ( hessianType == HST_IDENTITY )
-				{
-					for( j=0; j<nAC; ++j )
-						tempB[j] = delta_xFRy[j];
-				}
-				else /* hessianType == HST_ZERO */
-				{
-					if ( usingRegularisation( ) == BT_TRUE )
-					{
-						for( j=0; j<nAC; ++j )
-							tempB[j] = regVal*delta_xFRy[j];
-					}
-					else
-					{
-						for( j=0; j<nAC; ++j )
-							tempB[j] = 0.0;
-					}
-				}
-
-				for( j=0; j<nAC; ++j )
-				{
-					for( i=0; i<nFR; ++i )
-					{
-						ii = FR_idx[i];
-						tempB[j] += QQ(ii,nZ+j) * tempA[i];
-					}
-				}
-			}
-			else
-			{
-				/* Compute HFR * delta_xFR + HMX*delta_xFX
-				 * Here, tempA holds (HFR*delta_xFRy + HMX*delta_xFX) */
-				if ( nZ > 0 )
-					H->times(bounds.getFree(), bounds.getFree(), 1, 1.0, ZFR_delta_xFRz, nFR, 1.0, tempA, nFR);
-
-				for( i=0; i<nAC; ++i)
-				{
-					tempB[i] = 0.0;
-					for( j=0; j<nFR; ++j )
-					{
-						jj = FR_idx[j];
-						tempB[i] += QQ(jj,nZ+i) * tempA[j];
-					}
-				}
-			}
-
-			if ( backsolveT( tempB,BT_TRUE,delta_yAC_TMP ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_STEPDIRECTION_FAILED_TQ );
-		}
-
-		/* refine the solution found so far */
-		for ( i=0; i<nFR; ++i )
-			delta_xFR[i] += delta_xFR_TMP[i];
-		for ( i=0; i<nAC; ++i )
-			delta_yAC[i] += delta_yAC_TMP[i];
-
-		if ( options.numRefinementSteps > 0 )
-		{
-			/* compute residuals in tempA and tempB, and max-norm */
-			for ( i=0; i<nFR; ++i )
-			{
-				ii = FR_idx[i];
-				tempA[i] = delta_g[ii];
-			}
-
-			switch ( hessianType )
-			{
-				case HST_ZERO:
-					if ( usingRegularisation( ) == BT_TRUE )
-						for ( i=0; i<nFR; ++i )
-							tempA[i] += regVal*delta_xFR[i];
-					break;
-
-				case HST_IDENTITY:
-					for ( i=0; i<nFR; ++i )
-						tempA[i] += delta_xFR[i];
-					break;
-
-				default:
-					H->times(bounds.getFree(), bounds.getFree(),  1, 1.0, delta_xFR, nFR, 1.0, tempA, nFR);
-					H->times(bounds.getFree(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, tempA, nFR);
-					break;
-			}
-
-			A->transTimes(constraints.getActive(), bounds.getFree(), 1, -1.0, delta_yAC, nAC, 1.0, tempA, nFR);
-			real_t rnrm = 0.0;
-			for ( i=0; i<nFR; ++i )
-				if (rnrm < getAbs (tempA[i]))
-					rnrm = getAbs (tempA[i]);
-
-			if (!Delta_bC_isZero)
-			{
-				for ( i=0; i<nAC; ++i )
-				{
-					ii = AC_idx[i];
-					if ( constraints.getStatus( ii ) == ST_LOWER )
-						tempB[i] = delta_lbA[ii];
-					else
-						tempB[i] = delta_ubA[ii];
-				}
-			}
-			else
-			{
-				for ( i=0; i<nAC; ++i )
-					tempB[i] = 0.0;
-			}
-			A->times(constraints.getActive(), bounds.getFree(), 1, -1.0, delta_xFR, nFR, 1.0, tempB, nAC);
-			A->times(constraints.getActive(), bounds.getFixed(), 1, -1.0, delta_xFX, nFX, 1.0, tempB, nAC);
-			for ( i=0; i<nAC; ++i )
-				if (rnrm < getAbs (tempB[i]))
-					rnrm = getAbs (tempB[i]);
-
-			/* early termination of residual norm small enough */
-			if ( rnrm < options.epsIterRef )
-				break;
-		}
-	} /* end of refinement loop for delta_xFRz, delta_xFRy, delta_yAC */
-
-
-	/* IV) DETERMINE delta_yFX */
-	if ( nFX > 0 )
-	{
-		for( i=0; i<nFX; ++i )
-			delta_yFX[i] = delta_g[FX_idx[i]];
-
-		A->transTimes(constraints.getActive(), bounds.getFixed(), 1, -1.0, delta_yAC, nAC, 1.0, delta_yFX, nFX);
-
-		if ( hessianType == HST_ZERO )
-		{
-			if ( usingRegularisation( ) == BT_TRUE )
-				for( i=0; i<nFX; ++i )
-					delta_yFX[i] += regVal*delta_xFX[i];
-		}
-		else if ( hessianType == HST_IDENTITY )
-		{
-			for( i=0; i<nFX; ++i )
-				delta_yFX[i] += delta_xFX[i];
-		}
-		else
-		{
-			H->times(bounds.getFixed(), bounds.getFree(), 1, 1.0, delta_xFR, nFR, 1.0, delta_yFX, nFX);
-			H->times(bounds.getFixed(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, delta_yFX, nFX);
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p e r f o r m S t e p
- */
-returnValue QProblem::performStep(	const real_t* const delta_g,
-									const real_t* const delta_lbA, const real_t* const delta_ubA,
-									const real_t* const delta_lb, const real_t* const delta_ub,
-									const real_t* const delta_xFX, const real_t* const delta_xFR,
-									const real_t* const delta_yAC, const real_t* const delta_yFX,
-									int& BC_idx, SubjectToStatus& BC_status, BooleanType& BC_isBound
-									)
-{
-	int i, j, ii, jj;
-	int nV  = getNV( );
-	int nC  = getNC( );
-	int nFR = getNFR( );
-	int nFX = getNFX( );
-	int nAC = getNAC( );
-	int nIAC = getNIAC( );
-
-	int* FR_idx;
-	int* FX_idx;
-	int* AC_idx;
-	int* IAC_idx;
-
-	bounds.getFree( )->getNumberArray( &FR_idx );
-	bounds.getFixed( )->getNumberArray( &FX_idx );
-	constraints.getActive( )->getNumberArray( &AC_idx );
-	constraints.getInactive( )->getNumberArray( &IAC_idx );
-
-	/* initialise maximum steplength array */
-	tau = 1.0;
-	BC_idx = -1;
-	BC_status = ST_UNDEFINED;
-
-	int BC_idx_tmp = -1;
-
-	real_t* num = new real_t[ getMax( nV,nC ) ];
-	real_t* den = new real_t[ getMax( nV,nC ) ];
-
-	real_t* delta_Ax_l = new real_t[nC];
-	real_t* delta_Ax_u = new real_t[nC];
-	real_t* delta_Ax   = new real_t[nC];
-
-	real_t* delta_x = new real_t[nV];
-	for( j=0; j<nFR; ++j )
-	{
-		jj = FR_idx[j];
-		delta_x[jj] = delta_xFR[j];
-	}
-	for( j=0; j<nFX; ++j )
-	{
-		jj = FX_idx[j];
-		delta_x[jj] = delta_xFX[j];
-	}
-
-
-	/* I) DETERMINE MAXIMUM DUAL STEPLENGTH: */
-	/* 1) Ensure that active dual constraints' bounds remain valid
-	 *    (ignoring inequality constraints).  */
-	for( i=0; i<nAC; ++i )
-	{
-		ii = AC_idx[i];
-
-		num[i] = y[nV+ii];
-		den[i] = -delta_yAC[i];
-	}
-
-	performRatioTest( nAC,AC_idx,&constraints, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
-
-	if ( BC_idx_tmp >= 0 )
-	{
-		BC_idx = BC_idx_tmp;
-		BC_status = ST_INACTIVE;
-		BC_isBound = BT_FALSE;
-	}
-
-
-	/* 2) Ensure that active dual bounds remain valid
-	 *    (ignoring implicitly fixed variables). */
-	for( i=0; i<nFX; ++i )
-	{
-		ii = FX_idx[i];
-		num[i] = y[ii];
-		den[i] = -delta_yFX[i];
-	}
-
-	performRatioTest( nFX,FX_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
-
-	if ( BC_idx_tmp >= 0 )
-	{
-		BC_idx = BC_idx_tmp;
-		BC_status = ST_INACTIVE;
-		BC_isBound = BT_TRUE;
-	}
-
-
- 	/* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH */
-	/* 1) Ensure that inactive constraints' bounds remain valid
-	 *    (ignoring unbounded constraints). */
-
-	/* calculate product A*x */
-	if ( constraintProduct == 0 )
-	{
-		A->times(constraints.getInactive(), 0, 1, 1.0, delta_x, nV, 0.0, delta_Ax, nC, BT_FALSE);
-	}
-	else
-	{
-		for( i=0; i<nIAC; ++i )
-		{
-			ii = IAC_idx[i];
-
-			if ( constraints.getType( ii ) != ST_UNBOUNDED )
-			{
-				if ( (*constraintProduct)( ii,delta_x, &(delta_Ax[ii]) ) != 0 )
-				{
-					delete[] den; delete[] num;
-					delete[] delta_Ax; delete[] delta_Ax_u; delete[] delta_Ax_l; delete[] delta_x;
-					return THROWERROR( RET_ERROR_IN_CONSTRAINTPRODUCT );
-				}
-			}
-		}
-	}
-
-	if ( constraints.hasNoLower( ) == BT_FALSE )
-	{
-		for( i=0; i<nIAC; ++i )
-		{
-			ii = IAC_idx[i];
-			num[i] = getMax( Ax_l[ii],0.0 );
-			den[i] = delta_lbA[ii] - delta_Ax[ii];
-		}
-
-		performRatioTest( nIAC,IAC_idx,&constraints, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
-
-		if ( BC_idx_tmp >= 0 )
-		{
-			BC_idx = BC_idx_tmp;
-			BC_status = ST_LOWER;
-			BC_isBound = BT_FALSE;
-		}
-	}
-
-	if ( constraints.hasNoUpper( ) == BT_FALSE )
-	{
-		for( i=0; i<nIAC; ++i )
-		{
-			ii = IAC_idx[i];
-			num[i] = getMax( Ax_u[ii],0.0 );
-			den[i] = delta_Ax[ii] - delta_ubA[ii];
-		}
-
-		performRatioTest( nIAC,IAC_idx,&constraints, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
-
-		if ( BC_idx_tmp >= 0 )
-		{
-			BC_idx = BC_idx_tmp;
-			BC_status = ST_UPPER;
-			BC_isBound = BT_FALSE;
-		}
-	}
-
-
-	for( i=0; i<nIAC; ++i )
-	{
-		ii = IAC_idx[i];
-
-		if ( constraints.getType( ii ) != ST_UNBOUNDED )
-		{
-			delta_Ax_l[ii] = delta_Ax[ii] - delta_lbA[ii];
-			delta_Ax_u[ii] = delta_ubA[ii] - delta_Ax[ii];
-		}
-	}
-
-
-	/* 2) Ensure that inactive bounds remain valid
-	 *    (ignoring unbounded variables). */
-	/* inactive lower bounds */
-	if ( bounds.hasNoLower( ) == BT_FALSE )
-	{
-		for( i=0; i<nFR; ++i )
-		{
-			ii = FR_idx[i];
-			num[i] = getMax( x[ii] - lb[ii],0.0 );
-			den[i] = delta_lb[ii] - delta_xFR[i];
-		}
-
-		performRatioTest( nFR,FR_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
-
-		if ( BC_idx_tmp >= 0 )
-		{
-			BC_idx = BC_idx_tmp;
-			BC_status = ST_LOWER;
-			BC_isBound = BT_TRUE;
-		}
-	}
-
-	/* inactive upper bounds */
-	if ( bounds.hasNoUpper( ) == BT_FALSE )
-	{
-		for( i=0; i<nFR; ++i )
-		{
-			ii = FR_idx[i];
-			num[i] = getMax( ub[ii] - x[ii],0.0 );
-			den[i] = delta_xFR[i] - delta_ub[ii];
-		}
-
-		performRatioTest( nFR,FR_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
-
-		if ( BC_idx_tmp >= 0 )
-		{
-			BC_idx = BC_idx_tmp;
-			BC_status = ST_UPPER;
-			BC_isBound = BT_TRUE;
-		}
-	}
-
-	delete[] den;
-	delete[] num;
-	delete[] delta_x;
-
-
-	#ifndef __XPCTARGET__
-	char messageString[MAX_STRING_LENGTH];
-
-	if ( BC_status == ST_UNDEFINED )
-		snprintf( messageString,MAX_STRING_LENGTH,"Stepsize is %.15e!",tau );
-	else
-		snprintf( messageString,MAX_STRING_LENGTH,"Stepsize is %.15e! (idx = %d, isBound = %d, status = %d)",tau,BC_idx,BC_isBound,BC_status );
-
-	getGlobalMessageHandler( )->throwInfo( RET_STEPSIZE_NONPOSITIVE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-	#endif
-
-
-	/* III) PERFORM STEP ALONG HOMOTOPY PATH */
-	if ( tau > ZERO )
-	{
-		/* 1) Perform step in primal and dual space... */
-		for( i=0; i<nFR; ++i )
-		{
-			ii = FR_idx[i];
-			x[ii] += tau*delta_xFR[i];
-		}
-
-		for( i=0; i<nFX; ++i )
-		{
-			ii = FX_idx[i];
-			x[ii] += tau*delta_xFX[i];
-			y[ii] += tau*delta_yFX[i];
-		}
-
-		for( i=0; i<nAC; ++i )
-		{
-			ii = AC_idx[i];
-			y[nV+ii] += tau*delta_yAC[i];
-		}
-
-		/* 2) Shift QP data. */
-		for( i=0; i<nV; ++i )
-		{
-			g[i]  += tau*delta_g[i];
-			lb[i] += tau*delta_lb[i];
-			ub[i] += tau*delta_ub[i];
-		}
-
-		for( i=0; i<nC; ++i )
-		{
-			lbA[i] += tau*delta_lbA[i];
-			ubA[i] += tau*delta_ubA[i];
-		}
-
-		A->times( constraints.getActive(),0, 1, 1.0, x, nV, 0.0, Ax, nC, BT_FALSE );
-		for( i=0; i<nAC; ++i )
-		{
-			ii = AC_idx[i];
-			Ax_u[ii] = ubA[ii] - Ax[ii];
-			Ax_l[ii] = Ax[ii] - lbA[ii];
-		}
-		for( i=0; i<nIAC; ++i )
-		{
-			ii = IAC_idx[i];
-			if ( constraints.getType( ii ) != ST_UNBOUNDED )
-			{
-				Ax[ii]   += tau*delta_Ax[ii];
-				Ax_l[ii] += tau*delta_Ax_l[ii];
-				Ax_u[ii] += tau*delta_Ax_u[ii];
-			}
-		}
-	}
-	else
-	{
-		/* print a stepsize warning if stepsize is zero */
-		#ifndef __XPCTARGET__
-		snprintf( messageString,MAX_STRING_LENGTH,"Stepsize is %.15e",tau );
-		getGlobalMessageHandler( )->throwWarning( RET_STEPSIZE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		#endif
-	}
-
-	delete[] delta_Ax; delete[] delta_Ax_u; delete[] delta_Ax_l;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	c h a n g e A c t i v e S e t
- */
-returnValue QProblem::changeActiveSet( int BC_idx, SubjectToStatus BC_status, BooleanType BC_isBound )
-{
-	int nV = getNV( );
-
-	#ifndef __XPCTARGET__
-	char messageString[MAX_STRING_LENGTH];
-	#endif
-
-	switch ( BC_status )
-	{
-		/* Optimal solution found as no working set change detected. */
-		case ST_UNDEFINED:
-			return SUCCESSFUL_RETURN;
-
-		/* Remove one variable from active set. */
-		case ST_INACTIVE:
-			if ( BC_isBound == BT_TRUE )
-			{
-				#ifndef __XPCTARGET__
-				snprintf( messageString,MAX_STRING_LENGTH,"bound no. %d.", BC_idx );
-				getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-				#endif
-
-				if ( removeBound( BC_idx,BT_TRUE,BT_TRUE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
-
-				y[BC_idx] = 0.0;
-			}
-			else
-			{
-				#ifndef __XPCTARGET__
-				snprintf( messageString,MAX_STRING_LENGTH,"constraint no. %d.", BC_idx );
-				getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-				#endif
-
-				if ( removeConstraint( BC_idx,BT_TRUE,BT_TRUE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
-
-				y[nV+BC_idx] = 0.0;
-			}
-			break;
-
-
-		/* Add one variable to active set. */
-		default:
-			returnValue returnvalue;
-			if ( BC_isBound == BT_TRUE )
-			{
-				#ifndef __XPCTARGET__
-				if ( BC_status == ST_LOWER )
-					snprintf( messageString,MAX_STRING_LENGTH,"lower bound no. %d.", BC_idx );
-				else
-					snprintf( messageString,MAX_STRING_LENGTH,"upper bound no. %d.", BC_idx );
-				getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-				#endif
-
-				returnvalue = addBound( BC_idx,BC_status,BT_TRUE );
-				if ( returnvalue == RET_ADDBOUND_FAILED_INFEASIBILITY )
-					return returnvalue;
-				if ( returnvalue != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
-			}
-			else
-			{
-				#ifndef __XPCTARGET__
-				if ( BC_status == ST_LOWER )
-					snprintf( messageString,MAX_STRING_LENGTH,"lower constraint's bound no. %d.", BC_idx );
-				else
-					snprintf( messageString,MAX_STRING_LENGTH,"upper constraint's bound no. %d.", BC_idx );
-				getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-				#endif
-
-				returnvalue = addConstraint( BC_idx,BC_status,BT_TRUE );
-				if ( returnvalue == RET_ADDCONSTRAINT_FAILED_INFEASIBILITY )
-					return returnvalue;
-				if ( returnvalue != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
-			}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- * g e t R e l a t i v e H o m o t o p y L e n g t h
- */
-real_t QProblem::getRelativeHomotopyLength(	const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
-											const real_t* const lbA_new, const real_t* const ubA_new
-											)
-{
-	int i;
-	int nC = getNC( );
-	real_t len = QProblemB::getRelativeHomotopyLength( g_new,lb_new,ub_new );
-	real_t d, s;
-
-	/* lower constraint bounds */
-	for (i = 0; i < nC && lbA_new; i++)
-	{
-		s = getAbs(lbA_new[i]);
-		if (s < 1.0) s = 1.0;
-		d = getAbs(lbA_new[i] - lbA[i]) / s;
-		if (d > len) len = d;
-	}
-
-	/* upper constraint bounds */
-	for (i = 0; i < nC && ubA_new; i++)
-	{
-		s = getAbs(ubA_new[i]);
-		if (s < 1.0) s = 1.0;
-		d = getAbs(ubA_new[i] - ubA[i]) / s;
-		if (d > len) len = d;
-	}
-
-	return len;
-}
-
-
-/*
- * p e r f o r m R a m p i n g
- */
-returnValue QProblem::performRamping( )
-{
-	int nV = getNV( ), nC = getNC( ), bstat, cstat, i, nRamp;
-	real_t tP, rampValP, tD, rampValD, sca;
-
-	/* compute number of values in ramp */
-	nRamp = nV + nC + nC + nV;
-
-	/* ramp inactive variable bounds and active dual bound variables */
-	for (i = 0; i < nV; i++)
-	{
-		switch (bounds.getType(i))
-		{
-			case ST_EQUALITY:
-				lb[i] = x[i]; ub[i] = x[i];  /* reestablish exact feasibility */
-				continue;
-
-			case ST_BOUNDED:
-				tP = static_cast<real_t>((i+rampOffset) % nRamp) / static_cast<real_t>(nRamp-1);
-				rampValP = (1.0-tP) * ramp0 + tP * ramp1;
-				tD = static_cast<real_t>((nV+nC+nC+i+rampOffset) % nRamp) / static_cast<real_t>(nRamp-1);
-				rampValD = (1.0-tD) * ramp0 + tD * ramp1;
-				bstat = bounds.getStatus(i);
-				if (bstat != ST_LOWER) { sca = getMax(getAbs(x[i]), 1.0); lb[i] = x[i] - sca * rampValP; }
-				if (bstat != ST_UPPER) { sca = getMax(getAbs(x[i]), 1.0); ub[i] = x[i] + sca * rampValP; }
-				if (bstat == ST_LOWER) { lb[i] = x[i]; y[i] = +rampValD; }
-				if (bstat == ST_UPPER) { ub[i] = x[i]; y[i] = -rampValD; }
-				if (bstat == ST_INACTIVE) y[i] = 0.0; /* reestablish exact complementarity */
-				break;
-
-			case ST_UNBOUNDED:
-			case ST_DISABLED:
-			default:
-				 continue;
-		}
-	}
-
-	/* ramp inactive constraints and active dual constraint variables */
-	for (i = 0; i < nC; i++)
-	{
-		switch (constraints.getType(i))
-		{
-			case ST_EQUALITY:
-				lbA[i] = Ax[i]; ubA[i] = Ax[i];  /* reestablish exact feasibility */
-				continue;
-
-			case ST_BOUNDED:
-				tP = static_cast<real_t>((nV+i+rampOffset) % nRamp) / static_cast<real_t>(nRamp-1);
-				rampValP = (1.0-tP) * ramp0 + tP * ramp1;
-				tD = static_cast<real_t>((nV+nC+i+rampOffset) % nRamp) / static_cast<real_t>(nRamp-1);
-				rampValD = (1.0-tD) * ramp0 + tD * ramp1;
-				cstat = constraints.getStatus(i);
-				if (cstat != ST_LOWER) { sca = getMax(getAbs(Ax[i]), 1.0); lbA[i] = Ax[i] - sca * rampValP; }
-				if (cstat != ST_UPPER) { sca = getMax(getAbs(Ax[i]), 1.0); ubA[i] = Ax[i] + sca * rampValP; }
-				if (cstat == ST_LOWER) { lbA[i] = Ax[i]; y[nV+i] = +rampValD; }
-				if (cstat == ST_UPPER) { ubA[i] = Ax[i]; y[nV+i] = -rampValD; }
-				if (cstat == ST_INACTIVE) y[nV+i] = 0.0; /* reestablish exact complementarity */
-
-				Ax_l[i] = Ax[i] - lbA[i];
-				Ax_u[i] = ubA[i] - Ax[i];
-				break;
-
-			case ST_UNBOUNDED:
-			case ST_DISABLED:
-			default:
-				continue;
-		}
-	}
-
-	/* reestablish exact stationarity */
-	setupAuxiliaryQPgradient( );
-
-	/* advance ramp offset to avoid Ramping cycles */
-	rampOffset++;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- * u p d a t e F a r B o u n d s
- */
-returnValue QProblem::updateFarBounds(	real_t curFarBound, int nRamp,
-										const real_t* const lb_new, real_t* const lb_new_far,
-										const real_t* const ub_new, real_t* const ub_new_far,
-										const real_t* const lbA_new, real_t* const lbA_new_far,
-										const real_t* const ubA_new, real_t* const ubA_new_far
-										) const
-{
-	int i;
-	real_t rampVal, t;
-	int nV = getNV( );
-	int nC = getNC( );
-
-    returnValue returnvalue = QProblemB::updateFarBounds(	curFarBound,nRamp,
-															lb_new,lb_new_far, ub_new,ub_new_far
-															);
-    if ( returnvalue != SUCCESSFUL_RETURN )
-        return returnvalue;
-
-	if ( options.enableRamping == BT_TRUE )
-	{
-		for ( i=0; i<nC; ++i )
-		{
-			t = static_cast<real_t>((nV+i + rampOffset) % nRamp) / static_cast<real_t>(nRamp-1);
-			rampVal = curFarBound * (1.0 + (1.0-t)*ramp0 + t*ramp1);
-
-			if ( lbA_new == 0 )
-				lbA_new_far[i] = -rampVal;
-			else
-				lbA_new_far[i] = getMax( -rampVal,lbA_new[i] );
-
-			if ( ubA_new == 0 )
-				ubA_new_far[i] = rampVal;
-			else
-				ubA_new_far[i] = getMin( rampVal,ubA_new[i] );
-		}
-	}
-	else
-	{
-		for ( i=0; i<nC; ++i )
-		{
-			if ( lbA_new == 0 )
-				lbA_new_far[i] = -curFarBound;
-			else
-				lbA_new_far[i] = getMax( -curFarBound,lbA_new[i] );
-
-			if ( ubA_new == 0 )
-				ubA_new_far[i] = curFarBound;
-			else
-				ubA_new_far[i] = getMin( curFarBound,ubA_new[i] );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- * p e r f o r m D r i f t C o r r e c t i o n
- */
-returnValue QProblem::performDriftCorrection( )
-{
-	int i;
-	int nV = getNV ();
-	int nC = getNC ();
-
-	for ( i=0; i<nV; ++i )
-	{
-		switch ( bounds.getType ( i ) )
-		{
-			case ST_BOUNDED:
-				switch ( bounds.getStatus ( i ) )
-				{
-					case ST_LOWER:
-						lb[i] = x[i];
-						ub[i] = getMax (ub[i], x[i]);
-						y[i] = getMax (y[i], 0.0);
-						break;
-					case ST_UPPER:
-						lb[i] = getMin (lb[i], x[i]);
-						ub[i] = x[i];
-						y[i] = getMin (y[i], 0.0);
-						break;
-					case ST_INACTIVE:
-						lb[i] = getMin (lb[i], x[i]);
-						ub[i] = getMax (ub[i], x[i]);
-						y[i] = 0.0;
-						break;
-					case ST_UNDEFINED:
-					case ST_INFEASIBLE_LOWER:
-					case ST_INFEASIBLE_UPPER:
-						break;
-				}
-				break;
-			case ST_EQUALITY:
-				lb[i] = x[i];
-				ub[i] = x[i];
-				break;
-			case ST_UNBOUNDED:
-			case ST_UNKNOWN:
-            case ST_DISABLED:
-				break;
-		}
-	}
-
-	for ( i=0; i<nC; ++i )
-	{
-		switch ( constraints.getType ( i ) )
-		{
-			case ST_BOUNDED:
-				switch ( constraints.getStatus ( i ) )
-				{
-					case ST_LOWER:
-						lbA[i] = Ax[i];
-						Ax_l[i] = 0.0;
-						ubA[i] = getMax (ubA[i], Ax[i]);
-						Ax_u[i] = ubA[i] - Ax[i];
-						y[i+nV] = getMax (y[i+nV], 0.0);
-						break;
-					case ST_UPPER:
-						lbA[i] = getMin (lbA[i], Ax[i]);
-						Ax_l[i] = Ax[i] - lbA[i];
-						ubA[i] = Ax[i];
-						Ax_u[i] = 0.0;
-						y[i+nV] = getMin (y[i+nV], 0.0);
-						break;
-					case ST_INACTIVE:
-						lbA[i] = getMin (lbA[i], Ax[i]);
-						Ax_l[i] = Ax[i] - lbA[i];
-						ubA[i] = getMax (ubA[i], Ax[i]);
-						Ax_u[i] = ubA[i] - Ax[i];
-						y[i+nV] = 0.0;
-						break;
-					case ST_UNDEFINED:
-					case ST_INFEASIBLE_LOWER:
-					case ST_INFEASIBLE_UPPER:
-						break;
-				}
-				break;
-			case ST_EQUALITY:
-				lbA[i] = Ax[i];
-				Ax_l[i] = 0.0;
-				ubA[i] = Ax[i];
-				Ax_u[i] = 0.0;
-				break;
-			case ST_UNBOUNDED:
-			case ST_UNKNOWN:
-            case ST_DISABLED:
-				break;
-		}
-	}
-
-	return setupAuxiliaryQPgradient( );
-}
-
-/*
- *	s e t u p A u x i l i a r y Q P
- */
-returnValue QProblem::setupAuxiliaryQP( const Bounds* const guessedBounds, const Constraints* const guessedConstraints )
-{
-	int i, j;
-	int nV = getNV( );
-	int nC = getNC( );
-
-	/* consistency check */
-	if ( ( guessedBounds == 0 ) || ( guessedConstraints == 0 ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* nothing to do */
-	if ( ( guessedBounds == &bounds ) && ( guessedConstraints == &constraints ) )
-		return SUCCESSFUL_RETURN;
-
-	status = QPS_PREPARINGAUXILIARYQP;
-
-
-	/* I) SETUP WORKING SET ... */
-	if ( shallRefactorise( guessedBounds,guessedConstraints ) == BT_TRUE )
-	{
-		/* ... WITH REFACTORISATION: */
-		/* 1) Reset bounds/constraints ... */
-		bounds.init( nV );
-		constraints.init( nC );
-
-		/*    ... and set them up afresh. */
-		if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-		if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-		if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-		/* 2) Setup TQ factorisation. */
-		if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-		/* 3) Setup guessed working sets afresh. */
-		if ( setupAuxiliaryWorkingSet( guessedBounds,guessedConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
-			THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-		/* 4) Computes Cholesky decomposition of projected Hessian
-		 *    This now handles all special cases (no active bounds/constraints, no nullspace) */
-		if ( computeProjectedCholesky( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-	}
-	else
-	{
-		/* ... WITHOUT REFACTORISATION: */
-		if ( setupAuxiliaryWorkingSet( guessedBounds,guessedConstraints,BT_FALSE ) != SUCCESSFUL_RETURN )
-			THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-	}
-
-
-	/* II) SETUP AUXILIARY QP DATA: */
-	/* 1) Ensure that dual variable is zero for free bounds and inactive constraints. */
-	for ( i=0; i<nV; ++i )
-		if ( bounds.getStatus( i ) == ST_INACTIVE )
-			y[i] = 0.0;
-
-	for ( i=0; i<nC; ++i )
-		if ( constraints.getStatus( i ) == ST_INACTIVE )
-			y[nV+i] = 0.0;
-
-	/* 2) Setup gradient and (constraints') bound vectors. */
-	if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
-		THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-	A->times(1, 1.0, x, nV, 0.0, Ax, nC);
-	for ( j=0; j<nC; ++j )
-	{
-		Ax_l[j] = Ax[j];
-		Ax_u[j] = Ax[j];
-	}
-
-	/* (also sets Ax_l and Ax_u) */
-	if ( setupAuxiliaryQPbounds( 0,0,BT_FALSE ) != SUCCESSFUL_RETURN )
-		THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s h a l l R e f a c t o r i s e
- */
-
-BooleanType QProblem::shallRefactorise(	const Bounds* const guessedBounds,
-										const Constraints* const guessedConstraints
-										) const
-{
-	int i;
-	int nV = getNV( );
-	int nC = getNC( );
-
-	/* always refactorise if Hessian is not known to be positive definite */
-	if ( ( hessianType == HST_SEMIDEF ) || ( hessianType == HST_INDEF ) )
-		return BT_TRUE;
-
-	/* 1) Determine number of bounds that have same status
-	 *    in guessed AND current bounds.*/
-	int differenceNumberBounds = 0;
-
-	for( i=0; i<nV; ++i )
-		if ( guessedBounds->getStatus( i ) != bounds.getStatus( i ) )
-			++differenceNumberBounds;
-
-	/* 2) Determine number of constraints that have same status
-	 *    in guessed AND current constraints.*/
-	int differenceNumberConstraints = 0;
-
-	for( i=0; i<nC; ++i )
-		if ( guessedConstraints->getStatus( i ) != constraints.getStatus( i ) )
-			++differenceNumberConstraints;
-
-	/* 3) Decide wheter to refactorise or not. */
-	if ( 2*(differenceNumberBounds+differenceNumberConstraints) > guessedConstraints->getNAC( )+guessedBounds->getNFX( ) )
-		return BT_TRUE;
-	else
-		return BT_FALSE;
-}
-
-
-/*
- *	s e t u p Q P d a t a
- */
-returnValue QProblem::setupQPdata(	SymmetricMatrix *_H, const real_t* const _g, Matrix *_A,
-									const real_t* const _lb, const real_t* const _ub,
-									const real_t* const _lbA, const real_t* const _ubA
-									)
-{
-	int nC = getNC( );
-
-	/* 1) Load Hessian matrix as well as lower and upper bounds vectors. */
-	if ( QProblemB::setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	if ( ( nC > 0 ) && ( _A == 0 ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	if ( nC > 0 )
-	{
-		/* 2) Setup lower/upper constraints' bounds vector. */
-		setLBA( _lbA );
-		setUBA( _ubA );
-
-		/* 3) Only load constraint matrix after setting up vectors! */
-		setA( _A );
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p Q P d a t a
- */
-returnValue QProblem::setupQPdata(	const real_t* const _H, const real_t* const _g, const real_t* const _A,
-									const real_t* const _lb, const real_t* const _ub,
-									const real_t* const _lbA, const real_t* const _ubA
-									)
-{
-	int nC = getNC( );
-
-	/* 1) Load Hessian matrix as well as lower and upper bounds vectors. */
-	if ( QProblemB::setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	if ( ( nC > 0 ) && ( _A == 0 ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	if ( nC > 0 )
-	{
-		/* 2) Setup lower/upper constraints' bounds vector. */
-		setLBA( _lbA );
-		setUBA( _ubA );
-
-		/* 3) Only load constraint matrix after setting up vectors! */
-		setA( _A );
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p Q P d a t a F r o m F i l e
- */
-returnValue QProblem::setupQPdataFromFile(	const char* const H_file, const char* const g_file, const char* const A_file,
-											const char* const lb_file, const char* const ub_file,
-											const char* const lbA_file, const char* const ubA_file
-											)
-{
-	int i;
-	int nV = getNV( );
-	int nC = getNC( );
-
-	returnValue returnvalue;
-
-
-	/* 1) Load Hessian matrix as well as lower and upper bounds vectors from files. */
-	returnvalue = QProblemB::setupQPdataFromFile( H_file,g_file,lb_file,ub_file );
-	if ( returnvalue != SUCCESSFUL_RETURN )
-		return THROWERROR( returnvalue );
-
-	if ( ( nC > 0 ) && ( A_file == 0 ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	if ( nC > 0 )
-	{
-		/* 2) Load lower constraints' bounds vector from file. */
-		if ( lbA_file != 0 )
-		{
-			returnvalue = readFromFile( lbA, nC, lbA_file );
-			if ( returnvalue != SUCCESSFUL_RETURN )
-				return THROWERROR( returnvalue );
-		}
-		else
-		{
-			/* if no lower constraints' bounds are specified, set them to -infinity */
-			for( i=0; i<nC; ++i )
-				lbA[i] = -INFTY;
-		}
-
-		/* 3) Load upper constraints' bounds vector from file. */
-		if ( ubA_file != 0 )
-		{
-			returnvalue = readFromFile( ubA, nC, ubA_file );
-			if ( returnvalue != SUCCESSFUL_RETURN )
-				return THROWERROR( returnvalue );
-		}
-		else
-		{
-			/* if no upper constraints' bounds are specified, set them to infinity */
-			for( i=0; i<nC; ++i )
-				ubA[i] = INFTY;
-		}
-
-		/* 4) Only load constraint matrix from file after setting up vectors! */
-		real_t* _A = new real_t[nC * nV];
-		returnvalue = readFromFile( _A, nC,nV, A_file );
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			delete[] _A;
-			return THROWERROR( returnvalue );
-		}
-		setA( _A );
-		A->doFreeMemory( );
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	l o a d Q P v e c t o r s F r o m F i l e
- */
-returnValue QProblem::loadQPvectorsFromFile(	const char* const g_file, const char* const lb_file, const char* const ub_file,
-												const char* const lbA_file, const char* const ubA_file,
-												real_t* const g_new, real_t* const lb_new, real_t* const ub_new,
-												real_t* const lbA_new, real_t* const ubA_new
-												) const
-{
-	int nC = getNC( );
-
-	returnValue returnvalue;
-
-
-	/* 1) Load gradient vector as well as lower and upper bounds vectors from files. */
-	returnvalue = QProblemB::loadQPvectorsFromFile( g_file,lb_file,ub_file, g_new,lb_new,ub_new );
-	if ( returnvalue != SUCCESSFUL_RETURN )
-		return THROWERROR( returnvalue );
-
-	if ( nC > 0 )
-	{
-		/* 2) Load lower constraints' bounds vector from file. */
-		if ( lbA_file != 0 )
-		{
-			if ( lbA_new != 0 )
-			{
-				returnvalue = readFromFile( lbA_new, nC, lbA_file );
-				if ( returnvalue != SUCCESSFUL_RETURN )
-					return THROWERROR( returnvalue );
-			}
-			else
-			{
-				/* If filename is given, storage must be provided! */
-				return THROWERROR( RET_INVALID_ARGUMENTS );
-			}
-		}
-
-		/* 3) Load upper constraints' bounds vector from file. */
-		if ( ubA_file != 0 )
-		{
-			if ( ubA_new != 0 )
-			{
-				returnvalue = readFromFile( ubA_new, nC, ubA_file );
-				if ( returnvalue != SUCCESSFUL_RETURN )
-					return THROWERROR( returnvalue );
-			}
-			else
-			{
-				/* If filename is given, storage must be provided! */
-				return THROWERROR( RET_INVALID_ARGUMENTS );
-			}
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t I t e r a t i o n
- */
-returnValue QProblem::printIteration( 	int iter,
-										int BC_idx,	SubjectToStatus BC_status, BooleanType BC_isBound, real_t homotopyLength
-		  								)
-{
-	#ifndef __XPCTARGET__
-
-	/* consistency check */
-	if ( iter < 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	int i;
-	int nV = getNV();
-	int nC = getNC();
-	int nAC = getNAC();
-
-	real_t stat, bfeas, cfeas, bcmpl, ccmpl, Tmaxomin;
-	real_t *grad = 0;
-	real_t *AX = 0;
-	real_t Tmin, Tmax;
-
-	char myPrintfString[MAX_STRING_LENGTH];
-	char info[MAX_STRING_LENGTH];
-	const char excStr[] = " ef";
-
-	switch ( options.printLevel )
-	{
-		case PL_DEBUG_ITER:
-			grad = new real_t[nV];
-			AX = new real_t[nC];
-			stat = bfeas = cfeas = bcmpl = ccmpl = Tmaxomin = 0.0;
-
-			/* stationarity */
-			for (i = 0; i < nV; i++) grad[i] = g[i] - y[i];
-			H->times(1, 1.0, x, nV, 1.0, grad, nV);
-			A->transTimes(1, -1.0, y+nV, nC, 1.0, grad, nV);
-			for (i = 0; i < nV; i++) if (getAbs(grad[i]) > stat) stat = getAbs(grad[i]);
-
-			/* feasibility */
-			for (i = 0; i < nV; i++) if (lb[i] - x[i] > bfeas) bfeas = lb[i] - x[i];
-			for (i = 0; i < nV; i++) if (x[i] - ub[i] > bfeas) bfeas = x[i] - ub[i];
-			A->times(1, 1.0, x, nV, 0.0, AX, nC);
-			for (i = 0; i < nC; i++) if (lbA[i] - AX[i] > cfeas) cfeas = lbA[i] - AX[i];
-			for (i = 0; i < nC; i++) if (AX[i] - ubA[i] > cfeas) cfeas = AX[i] - ubA[i];
-
-			/* complementarity */
-			for (i = 0; i < nV; i++) if (y[i] > +EPS && getAbs((lb[i] - x[i])*y[i]) > bcmpl) bcmpl = getAbs((lb[i] - x[i])*y[i]);
-			for (i = 0; i < nV; i++) if (y[i] < -EPS && getAbs((ub[i] - x[i])*y[i]) > bcmpl) bcmpl = getAbs((ub[i] - x[i])*y[i]);
-			for (i = 0; i < nC; i++) if (y[nV+i] > +EPS && getAbs((lbA[i]-AX[i])*y[nV+i]) > ccmpl) ccmpl = getAbs((lbA[i]-AX[i])*y[nV+i]);
-			for (i = 0; i < nC; i++) if (y[nV+i] < -EPS && getAbs((ubA[i]-AX[i])*y[nV+i]) > ccmpl) ccmpl = getAbs((ubA[i]-AX[i])*y[nV+i]);
-
-			Tmin = 1.0e16; Tmax = 0.0;
-			for (i = 0; i < nAC; i++)
-				if (getAbs(TT(i,sizeT-i-1)) < Tmin)
-					Tmin = getAbs(TT(i,sizeT-i-1));
-				else if (getAbs(TT(i,sizeT-i-1)) > Tmax)
-					Tmax = getAbs(TT(i,sizeT-i-1));
-			Tmaxomin = Tmax/Tmin;
-
-			if (iter % 10 == 0)
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH, "\n%5s %4s %4s %4s %4s %9s %9s %9s %9s %9s %9s %9s %9s\n",
-						"iter", "addB", "remB", "addC", "remC", "hom len", "tau", "stat",
-						"bfeas", "cfeas", "bcmpl", "ccmpl", "Tmin");
-				myPrintf( myPrintfString );
-			}
-
-			snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d ", iter);
-			myPrintf( myPrintfString );
-
-			if (tabularOutput.idxAddB >= 0)
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH, "%4d ", tabularOutput.idxAddB);
-				myPrintf( myPrintfString );
-			}
-			else
-			{
-				myPrintf( "     " );
-			}
-
-			if (tabularOutput.idxRemB >= 0)
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH, "%4d ", tabularOutput.idxRemB);
-				myPrintf( myPrintfString );
-			}
-			else
-			{
-				myPrintf( "     " );
-			}
-
-			if (tabularOutput.idxAddC >= 0)
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH, "%4d ", tabularOutput.idxAddC);
-				myPrintf( myPrintfString );
-			}
-			else
-			{
-				myPrintf( "     " );
-			}
-
-			if (tabularOutput.idxRemC >= 0)
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH, "%4d ", tabularOutput.idxRemC);
-				myPrintf( myPrintfString );
-			}
-			else
-			{
-				myPrintf( "     " );
-			}
-
-			snprintf( myPrintfString,MAX_STRING_LENGTH, "%9.2e %9.2e %9.2e %9.2e %9.2e %9.2e %9.2e %9.2e\n",
-					homotopyLength, tau, stat, bfeas, cfeas, bcmpl, ccmpl, Tmin);
-			myPrintf( myPrintfString );
-
-			delete[] AX;
-			delete[] grad;
-			break;
-
-		case PL_TABULAR:
-			if (iter % 10 == 0)
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH, "\n%5s %6s %6s %6s %6s %9s %9s\n",
-						"iter", "addB", "remB", "addC", "remC", "hom len", "tau" );
-				myPrintf( myPrintfString );
-			}
-			snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d ",iter);
-			myPrintf( myPrintfString );
-
-			if (tabularOutput.idxAddB >= 0)
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d%c ", tabularOutput.idxAddB, excStr[tabularOutput.excAddB]);
-				myPrintf( myPrintfString );
-			}
-			else
-			{
-				myPrintf( "       " );
-			}
-
-			if (tabularOutput.idxRemB >= 0)
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d%c ", tabularOutput.idxRemB, excStr[tabularOutput.excRemB]);
-				myPrintf( myPrintfString );
-			}
-			else 
-			{
-				myPrintf( "       " );
-			}
-
-			if (tabularOutput.idxAddC >= 0) 
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d%c ", tabularOutput.idxAddC, excStr[tabularOutput.excAddC]);
-				myPrintf( myPrintfString );
-			}
-			else 
-			{
-				myPrintf( "       " );
-			}
-
-			if (tabularOutput.idxRemC >= 0) 
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d%c ", tabularOutput.idxRemC, excStr[tabularOutput.excRemC]);
-				myPrintf( myPrintfString );
-			}
-			else 
-			{
-				myPrintf( "       " );
-			}
-
-			snprintf( myPrintfString,MAX_STRING_LENGTH, "%9.2e %9.2e\n", homotopyLength, tau);
-			myPrintf( myPrintfString );
-			break;
-
-		case PL_MEDIUM:
-			/* 1) Print header at first iteration. */
- 			if ( iter == 0 )
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH,"\n\n####################   qpOASES  --  QP NO. %3.0d   #####################\n\n", count );
-				myPrintf( myPrintfString );
-
-				myPrintf( "    Iter   |    StepLength    |       Info       |   nFX   |   nAC    \n" );
-				myPrintf( " ----------+------------------+------------------+---------+--------- \n" );
-			}
-
-			/* 2) Print iteration line. */
-			if ( BC_status == ST_UNDEFINED )
-			{
-				if ( hessianType == HST_ZERO )
-					snprintf( info,3,"LP" );
-				else
-					snprintf( info,3,"QP" );
-
-				snprintf( myPrintfString,MAX_STRING_LENGTH,"   %5.1d   |   %1.6e   |    %s SOLVED     |  %4.1d   |  %4.1d   \n", iter,tau,info,getNFX( ),getNAC( ) );
-				myPrintf( myPrintfString );
-			}
-			else
-			{
-				if ( BC_status == ST_INACTIVE )
-					snprintf( info,5,"REM " );
-				else
-					snprintf( info,5,"ADD " );
-
-				if ( BC_isBound == BT_TRUE )
-					snprintf( &(info[4]),4,"BND" );
-				else
-					snprintf( &(info[4]),4,"CON" );
-
-				snprintf( myPrintfString,MAX_STRING_LENGTH,"   %5.1d   |   %1.6e   |   %s %4.1d   |  %4.1d   |  %4.1d   \n", iter,tau,info,BC_idx,getNFX( ),getNAC( ) );
-				myPrintf( myPrintfString );
-			}
-			break;
-
-		default:
-			/* nothing to display */
-			break;
-	}
-
-	#endif /* __XPCTARGET__ */
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- * d r o p I n f e a s i b l e s
- */
-returnValue QProblem::dropInfeasibles( int BC_number, SubjectToStatus BC_status, BooleanType BC_isBound,
-										real_t *xiB, real_t *xiC )
-{
-	int i;
-	
-	int nAC                   = getNAC ();
-	int nFX                   = getNFX ();	
-	int blockingPriority      = (BC_isBound) ? options.dropBoundPriority : options.dropIneqConPriority;
-	int y_min_number          = -1;
-	BooleanType y_min_isBound = BC_isBound;
-	int y_min_priority        = blockingPriority;
-
-	int* AC_idx;
-	constraints.getActive( )->getNumberArray( &AC_idx );
-
-	int* FX_idx;
-	bounds.getFixed( )->getNumberArray( &FX_idx );
-	
-	if (options.dropEqConPriority <= y_min_priority) 
-	{
-		// look for an equality constraint we can drop according to priorities
-		for ( i = 0; i < nAC; ++i )
-			if ( (constraints.getType (i) == ST_EQUALITY)
-				&& (getAbs (xiC[i]) > options.epsDen) ) 
-			{
-				y_min_number = AC_idx[i];
-				y_min_isBound = BT_FALSE;
-				y_min_priority = options.dropEqConPriority;
-				break;
-			}
-	}
-	
-	if (options.dropIneqConPriority <= y_min_priority) 
-	{
-		// look for an inequality constraint we can drop according to priorities
-		for ( i = 0; i < nAC; ++i )
-			if ( (constraints.getType (i) == ST_BOUNDED)
-				&& (getAbs (xiC[i]) > options.epsDen) ) 
-			{
-				y_min_number = AC_idx[i];
-				y_min_isBound = BT_FALSE;
-				y_min_priority = options.dropIneqConPriority;
-				break;
-			}
-	}
-	
-	if (options.dropBoundPriority <= y_min_priority) 
-	{
-		// look for a simple bound we can drop according to priorities
-		for ( i = 0; i < nFX; ++i )
-			if (getAbs (xiB[i]) > options.epsDen) 
-			{
-				y_min_number = FX_idx[i];
-				y_min_isBound = BT_TRUE;
-				y_min_priority = options.dropBoundPriority;
-				break;
-			}
-	}
-
-	if (y_min_number >= 0) {
-
-		// drop active equality or active bound we have found
-		if (y_min_isBound) {
-			SubjectToStatus status_ = bounds.getStatus (y_min_number);
-			removeBound (y_min_number, BT_TRUE, BT_FALSE, BT_FALSE);
-			bounds.setStatus (y_min_number, (status_ == ST_LOWER) ? ST_INFEASIBLE_LOWER : ST_INFEASIBLE_UPPER);
-			// TODO: fix duals y[]
-			/* fprintf (stdFile, "Dropping bounds %d for %s %d\n", y_min_number, BC_isBound?"bound":"constraint", BC_number); */
-		} else {
-			SubjectToStatus status_ = constraints.getStatus (y_min_number);
-			removeConstraint (y_min_number, BT_TRUE, BT_FALSE, BT_FALSE);
-			constraints.setStatus (y_min_number, (status_ == ST_LOWER) ? ST_INFEASIBLE_LOWER : ST_INFEASIBLE_UPPER);
-			// TODO: fix duals y[]
-			/* fprintf (stdFile, "Dropping constraint %d for %s %d\n", y_min_number, BC_isBound?"bound":"constraint", BC_number); */
-		}
-
-		// ... now return, add the blocking constraint, and continue solving QP with dropped bound/constraint
-		return SUCCESSFUL_RETURN;
-
-	} else {
-
-		// nothing found, then drop the blocking (still inactive) constraint
-		if (BC_isBound)
-			bounds.setStatus (BC_number, (BC_status == ST_LOWER) ? ST_INFEASIBLE_LOWER : ST_INFEASIBLE_UPPER);
-		else
-			constraints.setStatus (BC_number, (BC_status == ST_LOWER) ? ST_INFEASIBLE_LOWER : ST_INFEASIBLE_UPPER);
-
-		/* fprintf (stdFile, "Dropping %s %d itself\n", BC_isBound?"bound":"constraint", BC_number); */
-
-		// ... now return, and continue solving QP with dropped bound/constraint
-		return RET_ENSURELI_DROPPED;
-	}	
-}
-
-
-
-/*
- *  w r i t e Q p D a t a I n t o M a t F i l e
- */
-returnValue QProblem::writeQpDataIntoMatFile(	const char* const filename
-												) const
-{
-	#ifndef __XPCTARGET__
-	FILE* matFile;
-	matFile = fopen( filename,"w+" );
-
-	if ( matFile == 0 )
-		return RET_UNABLE_TO_OPEN_FILE;
-
-	int nV = getNV();
-	int nC = getNC();
-
-	real_t* Hfull = H->full();
-	writeIntoMatFile( matFile, Hfull, nV,nV, "H"   );
-	delete[] Hfull;
-
-	writeIntoMatFile( matFile, g,     nV,1,  "g"   );
-	
-	real_t* Afull = A->full();
-	writeIntoMatFile( matFile, Afull, nC,nV, "A"   );
-	delete[] Afull;
-	
-	writeIntoMatFile( matFile, lb,    nV,1,  "lb"  );
-	writeIntoMatFile( matFile, ub,    nV,1,  "ub"  );
-	writeIntoMatFile( matFile, lbA,   nC,1,  "lbA" );
-	writeIntoMatFile( matFile, ubA,   nC,1,  "ubA" );
-
-	fclose( matFile );
-
-	return SUCCESSFUL_RETURN;
-	#else /* __XPCTARGET__ */
-
-	return RET_NOT_YET_IMPLEMENTED;
-
-	#endif /* __XPCTARGET__ */
-}
-
-
-/*
- *  w r i t e Q p W o r k s p a c e I n t o M a t F i l e
- */
-returnValue QProblem::writeQpWorkspaceIntoMatFile(	const char* const filename
-													)
-{
-	#ifndef __XPCTARGET__
-	FILE* matFile;
-	matFile = fopen( filename,"w+" );
-
-	if ( matFile == 0 )
-		return RET_UNABLE_TO_OPEN_FILE;
-
-	int nV = getNV();
-	int nC = getNC();
-	int nFR  = getNFR();
-	int nFX  = getNFX();
-	int nAC  = getNAC();
-	int nIAC = getNIAC();
-	
-
-	writeIntoMatFile( matFile, T, sizeT,sizeT, "T" );
-	writeIntoMatFile( matFile, Q, nV,nV, "Q" );
-
-	writeIntoMatFile( matFile, Ax, nC,1, "Ax" );
-	writeIntoMatFile( matFile, Ax_l, nC,1, "Ax_l" );
-	writeIntoMatFile( matFile, Ax_u, nC,1, "Ax_u" );
-
-	
-	int *FR_idx, *FX_idx, *AC_idx, *IAC_idx;
-	bounds.getFree( )->getNumberArray( &FR_idx );
-	bounds.getFixed( )->getNumberArray( &FX_idx );
-	constraints.getActive( )->getNumberArray( &AC_idx );
-	constraints.getInactive( )->getNumberArray( &IAC_idx );
-
-	writeIntoMatFile( matFile, FR_idx,  nFR, 1, "FR_idx"  );
-	writeIntoMatFile( matFile, FX_idx,  nFX, 1, "FX_idx"  );
-	writeIntoMatFile( matFile, AC_idx,  nAC, 1, "AC_idx"  );
-	writeIntoMatFile( matFile, IAC_idx, nIAC,1, "IAC_idx" );
-
-	fclose( matFile );
-
-	return SUCCESSFUL_RETURN;
-	#else /* __XPCTARGET__ */
-
-	return RET_NOT_YET_IMPLEMENTED;
-
-	#endif /* __XPCTARGET__ */
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/QProblemB.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/QProblemB.cpp
deleted file mode 100644
index 6366a304189..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/QProblemB.cpp
+++ /dev/null
@@ -1,3862 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file src/QProblemB.cpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of the QProblemB class which is able to use the newly
- *	developed online active set strategy for parametric quadratic programming.
- */
-
-
-#include <qpOASES/QProblemB.hpp>
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	Q P r o b l e m B
- */
-QProblemB::QProblemB( )
-{
-	/* print copyright notice */
-	if (options.printLevel != PL_NONE)
-		printCopyrightNotice( );
-
-	/* reset global message handler */
-	getGlobalMessageHandler( )->reset( );
-
-	freeHessian = BT_FALSE;
-	H = 0;
-
-	g = 0;
-	lb = 0;
-	ub = 0;
-
-	R = 0;
-	haveCholesky = BT_FALSE;
-
-	x = 0;
-	y = 0;
-
-	tau = 0.0;
-
-	hessianType = HST_UNKNOWN;
-	regVal = 0.0;
-
-	infeasible  = BT_FALSE;
-	unbounded   = BT_FALSE;
-
-	status = QPS_NOTINITIALISED;
-
-	count = 0;
-
-	ramp0 = options.initialRamping;
-	ramp1 = options.finalRamping;
-	rampOffset = 0;
-
-	delta_xFR_TMP = 0;
-
-	setPrintLevel( options.printLevel );
-}
-
-
-/*
- *	Q P r o b l e m B
- */
-QProblemB::QProblemB( int _nV, HessianType _hessianType )
-{
-	int i;
-
-	/* print copyright notice */
-	if (options.printLevel != PL_NONE)
-		printCopyrightNotice( );
-
-	/* consistency check */
-	if ( _nV <= 0 )
-	{
-		_nV = 1;
-		THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	/* reset global message handler */
-	getGlobalMessageHandler( )->reset( );
-
-	freeHessian = BT_FALSE;
-	H = 0;
-
-	g = new real_t[_nV];
-	for( i=0; i<_nV; ++i ) g[i] = 0.0;
-
-	lb = new real_t[_nV];
-	for( i=0; i<_nV; ++i ) lb[i] = 0.0;
-
-	ub = new real_t[_nV];
-	for( i=0; i<_nV; ++i ) ub[i] = 0.0;
-
-	bounds.init( _nV );
-
-	R = new real_t[_nV*_nV];
-	haveCholesky = BT_FALSE;
-
-	x = new real_t[_nV];
-	for( i=0; i<_nV; ++i ) x[i] = 0.0;
-
-	y = new real_t[_nV];
-	for( i=0; i<_nV; ++i ) y[i] = 0.0;
-
-	tau = 0.0;
-
-	hessianType = _hessianType;
-	regVal = 0.0;
-
-	infeasible  = BT_FALSE;
-	unbounded   = BT_FALSE;
-
-	status = QPS_NOTINITIALISED;
-
-	count = 0;
-
-	ramp0 = options.initialRamping;
-	ramp1 = options.finalRamping;
-	rampOffset = 0;
-
-	delta_xFR_TMP = new real_t[_nV];
-
-	setPrintLevel( options.printLevel );
-
-	flipper.init( (unsigned int)_nV );
-}
-
-
-/*
- *	Q P r o b l e m B
- */
-QProblemB::QProblemB( const QProblemB& rhs )
-{
-	freeHessian = BT_FALSE;
-	H = 0;
-
-	copy( rhs );
-}
-
-
-/*
- *	~ Q P r o b l e m B
- */
-QProblemB::~QProblemB( )
-{
-	clear( );
-
-	/* reset global message handler */
-	getGlobalMessageHandler( )->reset( );
-}
-
-
-/*
- *	o p e r a t o r =
- */
-QProblemB& QProblemB::operator=( const QProblemB& rhs )
-{
-	if ( this != &rhs )
-	{
-		clear( );
-		copy( rhs );
-	}
-
-	return *this;
-}
-
-
-/*
- *	r e s e t
- */
-returnValue QProblemB::reset( )
-{
-	int i;
-	int nV = getNV( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* 1) Reset bounds. */
-	bounds.init( nV );
-
-	/* 2) Reset Cholesky decomposition. */
-	for( i=0; i<nV*nV; ++i )
-		R[i] = 0.0;
-	
-	haveCholesky = BT_FALSE;
-
-	/* 3) Reset steplength and status flags. */
-	tau = 0.0;
-
-	hessianType = HST_UNKNOWN;
-	regVal = 0.0;
-
-	infeasible  = BT_FALSE;
-	unbounded   = BT_FALSE;
-
-	status = QPS_NOTINITIALISED;
-
-	ramp0 = options.initialRamping;
-	ramp1 = options.finalRamping;
-	rampOffset = 0;
-
-	/* 4) Reset flipper object */
-	flipper.init( (unsigned int)nV );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	i n i t
- */
-returnValue QProblemB::init(	SymmetricMatrix *_H, const real_t* const _g,
-								const real_t* const _lb, const real_t* const _ub,
-								int& nWSR, real_t* const cputime
-								)
-{
-	if ( getNV( ) == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* 1) Consistency check. */
-	if ( isInitialised( ) == BT_TRUE )
-	{
-		THROWWARNING( RET_QP_ALREADY_INITIALISED );
-		reset( );
-	}
-
-	/* 2) Setup QP data. */
-	if ( setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 3) Call to main initialisation routine (without any additional information). */
-	return solveInitialQP( 0,0,0, nWSR,cputime );
-}
-
-
-/*
- *	i n i t
- */
-returnValue QProblemB::init(	const real_t* const _H, const real_t* const _g,
-								const real_t* const _lb, const real_t* const _ub,
-								int& nWSR, real_t* const cputime
-								)
-{
-	if ( getNV( ) == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* 1) Consistency check. */
-	if ( isInitialised( ) == BT_TRUE )
-	{
-		THROWWARNING( RET_QP_ALREADY_INITIALISED );
-		reset( );
-	}
-
-	/* 2) Setup QP data. */
-	if ( setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 3) Call to main initialisation routine (without any additional information). */
-	return solveInitialQP( 0,0,0, nWSR,cputime );
-}
-
-
-/*
- *	i n i t
- */
-returnValue QProblemB::init(	const char* const H_file, const char* const g_file,
-								const char* const lb_file, const char* const ub_file,
-								int& nWSR, real_t* const cputime
-								)
-{
-	if ( getNV( ) == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* 1) Consistency check. */
-	if ( isInitialised( ) == BT_TRUE )
-	{
-		THROWWARNING( RET_QP_ALREADY_INITIALISED );
-		reset( );
-	}
-
-	/* 2) Setup QP data from files. */
-	if ( setupQPdataFromFile( H_file,g_file,lb_file,ub_file ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_UNABLE_TO_READ_FILE );
-
-	/* 3) Call to main initialisation routine (without any additional information). */
-	return solveInitialQP( 0,0,0, nWSR,cputime );
-}
-
-
-/*
- *	i n i t
- */
-returnValue QProblemB::init( 	SymmetricMatrix *_H, const real_t* const _g,
-								const real_t* const _lb, const real_t* const _ub,
-								int& nWSR, real_t* const cputime,
-								const real_t* const xOpt, const real_t* const yOpt,
-								const Bounds* const guessedBounds
-								)
-{
-	int i;
-	int nV = getNV( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* 1) Consistency checks. */
-	if ( isInitialised( ) == BT_TRUE )
-	{
-		THROWWARNING( RET_QP_ALREADY_INITIALISED );
-		reset( );
-	}
-
-	if ( guessedBounds != 0 )
-	{
-		for( i=0; i<nV; ++i )
-		{
-			if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
-				return THROWERROR( RET_INVALID_ARGUMENTS );
-		}
-	}
-
-	/* exclude this possibility in order to avoid inconsistencies */
-	if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( guessedBounds != 0 ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 2) Setup QP data. */
-	if ( setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 3) Call to main initialisation routine. */
-	return solveInitialQP( xOpt,yOpt,guessedBounds, nWSR,cputime );
-}
-
-
-/*
- *	i n i t
- */
-returnValue QProblemB::init( 	const real_t* const _H, const real_t* const _g,
-								const real_t* const _lb, const real_t* const _ub,
-								int& nWSR, real_t* const cputime,
-								const real_t* const xOpt, const real_t* const yOpt,
-								const Bounds* const guessedBounds
-								)
-{
-	int i;
-	int nV = getNV( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* 1) Consistency checks. */
-	if ( isInitialised( ) == BT_TRUE )
-	{
-		THROWWARNING( RET_QP_ALREADY_INITIALISED );
-		reset( );
-	}
-
-	if ( guessedBounds != 0 )
-	{
-		for( i=0; i<nV; ++i )
-		{
-			if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
-				return THROWERROR( RET_INVALID_ARGUMENTS );
-		}
-	}
-
-	/* exclude this possibility in order to avoid inconsistencies */
-	if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( guessedBounds != 0 ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 2) Setup QP data. */
-	if ( setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 3) Call to main initialisation routine. */
-	return solveInitialQP( xOpt,yOpt,guessedBounds, nWSR,cputime );
-}
-
-
-/*
- *	i n i t
- */
-returnValue QProblemB::init( 	const char* const H_file, const char* const g_file,
-								const char* const lb_file, const char* const ub_file,
-								int& nWSR, real_t* const cputime,
-								const real_t* const xOpt, const real_t* const yOpt,
-								const Bounds* const guessedBounds
-								)
-{
-	int i;
-	int nV = getNV( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* 1) Consistency checks. */
-	if ( isInitialised( ) == BT_TRUE )
-	{
-		THROWWARNING( RET_QP_ALREADY_INITIALISED );
-		reset( );
-	}
-
-	for( i=0; i<nV; ++i )
-	{
-		if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
-			return THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	/* exclude this possibility in order to avoid inconsistencies */
-	if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( guessedBounds != 0 ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 2) Setup QP data from files. */
-	if ( setupQPdataFromFile( H_file,g_file,lb_file,ub_file ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_UNABLE_TO_READ_FILE );
-
-	/* 3) Call to main initialisation routine. */
-	return solveInitialQP( xOpt,yOpt,guessedBounds, nWSR,cputime );
-}
-
-
-/*
- *	h o t s t a r t
- */
-returnValue QProblemB::hotstart(	const real_t* const g_new,
-									const real_t* const lb_new, const real_t* const ub_new,
-									int& nWSR, real_t* const cputime
-									)
-{
-	int i, nActiveFar;
-	int nV = getNV ();
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	returnValue returnvalue = SUCCESSFUL_RETURN;
-
-	/* Simple check for consistency of bounds */
-	if ( areBoundsConsistent(lb_new, ub_new) != SUCCESSFUL_RETURN )
-		return setInfeasibilityFlag(returnvalue,BT_TRUE);
-
-	++count;
-
-
-	int nWSR_max = nWSR;
-	int nWSR_performed = 0;
-
-	real_t cputime_remaining = INFTY;
-	real_t cputime_needed = 0.0;
-
-	real_t farbound = options.initialFarBounds;
-
-	if ( haveCholesky == BT_FALSE )
-	{
-		returnvalue = setupInitialCholesky( );
-		if (returnvalue != SUCCESSFUL_RETURN)
-			return THROWERROR(returnvalue);
-	}
-
-	if ( options.enableFarBounds == BT_FALSE )
-	{
-		/* Automatically call standard solveQP if regularisation is not active. */
-		returnvalue = solveRegularisedQP( g_new,lb_new,ub_new, nWSR,cputime,0 );
-	}
-	else
-	{
-		real_t *ub_new_far = new real_t[nV];
-		real_t *lb_new_far = new real_t[nV];
-
-		/* possibly extend initial far bounds to largest bound/constraint data */
-		if (ub_new)
-			for (i = 0; i < nV; i++)
-				if ((ub_new[i] < INFTY) && (ub_new[i] > farbound)) farbound = ub_new[i];
-		if (lb_new)
-			for (i = 0; i < nV; i++)
-				if ((lb_new[i] > -INFTY) && (lb_new[i] < -farbound)) farbound = -lb_new[i];
-
-		updateFarBounds(	farbound,nV,
-							lb_new,lb_new_far, ub_new,ub_new_far
-							);
-
-		for ( ;; )
-		{
-			nWSR = nWSR_max;
-			if ( cputime != 0 )
-				cputime_remaining = *cputime - cputime_needed;
-
-			/* Automatically call standard solveQP if regularisation is not active. */
-			returnvalue = solveRegularisedQP( g_new,lb_new_far,ub_new_far, nWSR,&cputime_remaining,nWSR_performed );
-
-			nWSR_performed  = nWSR;
-			cputime_needed += cputime_remaining;
-
-			/* Check for active far-bounds and move them away */
-			nActiveFar = 0;
-			farbound *= options.growFarBounds;
-
-			if ( infeasible == BT_TRUE )
-			{
-				if ( farbound >= INFTY )
-				{
-					returnvalue = RET_HOTSTART_STOPPED_INFEASIBILITY;
-					goto farewell;
-				}
-
-				updateFarBounds(	farbound,nV,
-									lb_new,lb_new_far, ub_new,ub_new_far
-									);
-			}
-			else if ( status == QPS_SOLVED )
-			{
-				real_t tol = farbound/options.growFarBounds * options.boundTolerance;
-				nActiveFar = 0;
-				for ( i=0; i<nV; ++i )
-				{
-					if ( ( ( lb_new == 0 ) || ( lb_new_far[i] > lb_new[i] ) ) && ( getAbs ( lb_new_far[i] - x[i] ) < tol ) )
-						++nActiveFar;
-					if ( ( ( ub_new == 0 ) || ( ub_new_far[i] < ub_new[i] ) ) && ( getAbs ( ub_new_far[i] - x[i] ) < tol ) )
-						++nActiveFar;
-				}
-
-				if ( nActiveFar == 0 )
-					break;
-
-				status = QPS_HOMOTOPYQPSOLVED;
-
-				if ( farbound >= INFTY )
-				{
-					unbounded = BT_TRUE;
-					returnvalue = RET_HOTSTART_STOPPED_UNBOUNDEDNESS;
-					goto farewell;
-				}
-
-				updateFarBounds(	farbound,nV,
-									lb_new,lb_new_far, ub_new,ub_new_far
-									);
-			}
-			else
-			{
-				/* some other error when solving QP */
-				break;
-			}
-
-			/* advance ramp offset to avoid Ramping cycles */
-			rampOffset++;
-		}
-
-		farewell:
-			if ( cputime != 0 )
-				*cputime = cputime_needed;
-			delete[] lb_new_far; delete[] ub_new_far;
-	}
-
-	return ( returnvalue != SUCCESSFUL_RETURN ) ? THROWERROR( returnvalue ) : returnvalue;
-}
-
-
-/*
- *	h o t s t a r t
- */
-returnValue QProblemB::hotstart(	const char* const g_file,
-									const char* const lb_file, const char* const ub_file,
-									int& nWSR, real_t* const cputime
-									)
-{
-	int nV  = getNV( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* consistency check */
-	if ( g_file == 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-
-	/* 1) Allocate memory (if bounds exist). */
-	real_t* g_new  = new real_t[nV];
-	real_t* lb_new = 0;
-	real_t* ub_new = 0;
-
-	if ( lb_file != 0 )
-		lb_new = new real_t[nV];
-	if ( ub_file != 0 )
-		ub_new = new real_t[nV];
-
-	/* 2) Load new QP vectors from file. */
-	returnValue returnvalue;
-	returnvalue = loadQPvectorsFromFile(	g_file,lb_file,ub_file,
-											g_new,lb_new,ub_new
-											);
-	if ( returnvalue != SUCCESSFUL_RETURN )
-	{
-		if ( ub_file != 0 )
-			delete[] ub_new;
-		if ( lb_file != 0 )
-			delete[] lb_new;
-		delete[] g_new;
-
-		return THROWERROR( RET_UNABLE_TO_READ_FILE );
-	}
-
-	/* 3) Actually perform hotstart. */
-	returnvalue = hotstart( g_new,lb_new,ub_new, nWSR,cputime );
-
-	/* 4) Free memory. */
-	if ( ub_file != 0 )
-		delete[] ub_new;
-	if ( lb_file != 0 )
-		delete[] lb_new;
-	delete[] g_new;
-
-	return returnvalue;
-}
-
-
-/*
- *	h o t s t a r t
- */
-returnValue QProblemB::hotstart(	const real_t* const g_new,
-									const real_t* const lb_new, const real_t* const ub_new,
-									int& nWSR, real_t* const cputime,
-									const Bounds* const guessedBounds
-									)
-{
-	int nV = getNV( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-
-	/* start runtime measurement */
-	real_t starttime = 0.0;
-	if ( cputime != 0 )
-		starttime = getCPUtime( );
-
-
-	/* 1) Update working set according to guess for working set of bounds. */
-	if ( guessedBounds != 0 )
-	{
-		if ( setupAuxiliaryQP( guessedBounds ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-	}
-	else
-	{
-		/* create empty bounds for setting up auxiliary QP */
-		Bounds emptyBounds( nV );
-		if ( emptyBounds.setupAllFree( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-		if ( setupAuxiliaryQP( &emptyBounds ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-	}
-
-	/* 2) Perform usual homotopy. */
-
-	/* Allow only remaining CPU time for usual hotstart. */
-	if ( cputime != 0 )
-		*cputime -= getCPUtime( ) - starttime;
-
-	returnValue returnvalue = hotstart( g_new,lb_new,ub_new, nWSR,cputime );
-
-	/* stop runtime measurement */
-	if ( cputime != 0 )
-		*cputime = getCPUtime( ) - starttime;
-
-	return returnvalue;
-}
-
-
-/*
- *	h o t s t a r t
- */
-returnValue QProblemB::hotstart(	const char* const g_file,
-									const char* const lb_file, const char* const ub_file,
-									int& nWSR, real_t* const cputime,
-									const Bounds* const guessedBounds
-									)
-{
-	int nV = getNV( );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	/* consistency check */
-	if ( g_file == 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-
-	/* 1) Allocate memory (if bounds exist). */
-	real_t* g_new  = new real_t[nV];
-	real_t* lb_new = 0;
-	real_t* ub_new = 0;
-
-	if ( lb_file != 0 )
-		lb_new = new real_t[nV];
-	if ( ub_file != 0 )
-		ub_new = new real_t[nV];
-
-	/* 2) Load new QP vectors from file. */
-	returnValue returnvalue;
-	returnvalue = loadQPvectorsFromFile(	g_file,lb_file,ub_file,
-											g_new,lb_new,ub_new
-											);
-	if ( returnvalue != SUCCESSFUL_RETURN )
-	{
-		if ( ub_file != 0 )
-			delete[] ub_new;
-		if ( lb_file != 0 )
-			delete[] lb_new;
-		delete[] g_new;
-
-		return THROWERROR( RET_UNABLE_TO_READ_FILE );
-	}
-
-	/* 3) Actually perform hotstart using initialised homotopy. */
-	returnvalue = hotstart(	g_new,lb_new,ub_new, nWSR,cputime,
-							guessedBounds
-							);
-
-	/* 4) Free memory. */
-	if ( ub_file != 0 )
-		delete[] ub_new;
-	if ( lb_file != 0 )
-		delete[] lb_new;
-	delete[] g_new;
-
-	return returnvalue;
-}
-
-
-/*
- *	g e t W o r k i n g S e t
- */
-returnValue QProblemB::getWorkingSet( real_t* workingSet )
-{
-	int nV = this->getNV();
-
-	// At which limit is the bound active?
-	for (int i = 0; i < nV; i++) {
-		switch (bounds.getStatus(i)) {
-			case ST_LOWER: workingSet[i] = -1.0; break;
-			case ST_UPPER: workingSet[i] = +1.0; break;
-			default: workingSet[i] = 0.0; break;
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t N Z
- */
-int QProblemB::getNZ( ) const
-{
-	/* if no constraints are present: nZ=nFR */
-	return getNFR( );
-}
-
-
-/*
- *	g e t O b j V a l
- */
-real_t QProblemB::getObjVal( ) const
-{
-	real_t objVal;
-
-	/* calculated optimal objective function value
-	 * only if current QP has been solved */
-	if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
-		 ( getStatus( ) == QPS_SOLVED ) )
-	{
-		objVal = getObjVal( x );
-	}
-	else
-	{
-		objVal = INFTY;
-	}
-
-	return objVal;
-}
-
-
-/*
- *	g e t O b j V a l
- */
-real_t QProblemB::getObjVal( const real_t* const _x ) const
-{
-	int i;
-	int nV = getNV( );
-
-	if ( nV == 0 )
-		return 0.0;
-
-	real_t objVal = 0.0;
-
-	for( i=0; i<nV; ++i )
-		objVal += _x[i]*g[i];
-
-	switch ( hessianType )
-	{
-		case HST_ZERO:
-			break;
-
-		case HST_IDENTITY:
-			for( i=0; i<nV; ++i )
-				objVal += 0.5*_x[i]*_x[i];
-			break;
-
-		default:
-			real_t *Hx = new real_t[nV];
-			H->times(1, 1.0, _x, nV, 0.0, Hx, nV);
-			for( i=0; i<nV; ++i )
-				objVal += 0.5*_x[i]*Hx[i];
-			delete[] Hx;
-			break;
-	}
-
-	/* When using regularisation, the objective function value
-	 * needs to be modified as follows:
-	 * objVal = objVal - 0.5*_x*(Hmod-H)*_x - _x'*(gMod-g)
-	 *        = objVal - 0.5*_x*eps*_x * - _x'*(-eps*_x)
-	 *        = objVal + 0.5*_x*eps*_x */
-	if ( usingRegularisation( ) == BT_TRUE )
-	{
-		for( i=0; i<nV; ++i )
-			objVal += 0.5*_x[i]*regVal*_x[i];
-	}
-
-	return objVal;
-}
-
-
-/*
- *	g e t P r i m a l S o l u t i o n
- */
-returnValue QProblemB::getPrimalSolution( real_t* const xOpt ) const
-{
-	int i;
-
-	/* return optimal primal solution vector
-	 * only if current QP has been solved */
-	if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
-		 ( getStatus( ) == QPS_SOLVED ) )
-	{
-		for( i=0; i<getNV( ); ++i )
-			xOpt[i] = x[i];
-
-		return SUCCESSFUL_RETURN;
-	}
-	else
-	{
-		return RET_QP_NOT_SOLVED;
-	}
-}
-
-
-/*
- *	g e t D u a l S o l u t i o n
- */
-returnValue QProblemB::getDualSolution( real_t* const yOpt ) const
-{
-	int i;
-
-	for( i=0; i<getNV( ); ++i )
-		yOpt[i] = y[i];
-
-	/* return optimal dual solution vector
-	 * only if current QP has been solved */
-	if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
-		 ( getStatus( ) == QPS_SOLVED ) )
-	{
-		return SUCCESSFUL_RETURN;
-	}
-	else
-	{
-		return RET_QP_NOT_SOLVED;
-	}
-}
-
-
-/*
- *	s e t P r i n t L e v e l
- */
-returnValue QProblemB::setPrintLevel( PrintLevel _printLevel )
-{
-	#ifndef __XPCTARGET__
-		#ifndef __MATLAB__
-			if ( ( options.printLevel == PL_HIGH ) && ( options.printLevel != _printLevel ) )
-				THROWINFO( RET_PRINTLEVEL_CHANGED );
-		#endif /* __MATLAB__ */
-		options.printLevel = _printLevel;
-	#else
-	options.printLevel = PL_NONE;
-	#endif /* __XPCTARGET__ */
-
-	/* update message handler preferences */
- 	switch ( options.printLevel )
- 	{
- 		case PL_NONE:
- 			getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_HIDDEN );
-			getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_HIDDEN );
-			getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );
-			break;
-
-		case PL_TABULAR:
-		case PL_LOW:
-			getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );
-			getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_HIDDEN );
-			getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );
-			break;
-
-		case PL_DEBUG_ITER:
-		case PL_MEDIUM:
-			getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );
-			getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_VISIBLE );
-			getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );
-			break;
-
-		default: /* PL_HIGH */
-			getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );
-			getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_VISIBLE );
-			getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_VISIBLE );
-			break;
- 	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t P r o p e r t i e s
- */
-returnValue QProblemB::printProperties( )
-{
-	#ifndef __XPCTARGET__
-	#ifndef __DSPACE__
-	/* Do not print properties if print level is set to none! */
-	if ( options.printLevel == PL_NONE )
-		return SUCCESSFUL_RETURN;
-
-	char myPrintfString[MAX_STRING_LENGTH];
-
-	myPrintf( "\n#################   qpOASES  --  QP PROPERTIES   #################\n" );
-	myPrintf( "\n" );
-
-	/* 1) Variables properties. */
-	snprintf( myPrintfString,MAX_STRING_LENGTH,  "Number of Variables: %4.1d\n",getNV( ) );
-	myPrintf( myPrintfString );
-
-	if ( bounds.hasNoLower( ) == BT_TRUE )
-			myPrintf( "Variables are not bounded from below.\n" );
-		else
-			myPrintf( "Variables are bounded from below.\n" );
-
-	if ( bounds.hasNoUpper( ) == BT_TRUE )
-			myPrintf( "Variables are not bounded from above.\n" );
-		else
-			myPrintf( "Variables are bounded from above.\n" );
-
-	myPrintf( "\n" );
-
-
-	/* 2) Further properties. */
-	switch ( hessianType )
-	{
-		case HST_ZERO:
-			myPrintf( "Hessian is zero matrix (i.e. actually an LP is solved).\n" );
-			break;
-
-		case HST_IDENTITY:
-			myPrintf( "Hessian is identity matrix.\n" );
-			break;
-
-		case HST_POSDEF:
-			myPrintf( "Hessian matrix is (strictly) positive definite.\n" );
-			break;
-
-		case HST_POSDEF_NULLSPACE:
-			myPrintf( "Hessian matrix is positive definite on null space of active constraints.\n" );
-			break;
-
-		case HST_SEMIDEF:
-			myPrintf( "Hessian matrix is positive semi-definite.\n" );
-			break;
-
-		case HST_INDEF:
-			myPrintf( "Hessian matrix is indefinite.\n" );
-			break;
-
-		default:
-			myPrintf( "Hessian matrix has unknown type.\n" );
-			break;
-	}
-
-	if ( infeasible == BT_TRUE )
-		myPrintf( "QP was found to be infeasible.\n" );
-	else
-		myPrintf( "QP seems to be feasible.\n" );
-
-	if ( unbounded == BT_TRUE )
-		myPrintf( "QP was found to be unbounded from below.\n" );
-	else
-		myPrintf( "QP seems to be bounded from below.\n" );
-
-	myPrintf( "\n" );
-
-
-	/* 3) QP object properties. */
-	switch ( status )
-	{
-		case QPS_NOTINITIALISED:
-			myPrintf( "Status of QP object: freshly instantiated or reset.\n" );
-			break;
-
-		case QPS_PREPARINGAUXILIARYQP:
-			myPrintf( "Status of QP object: an auxiliary QP is currently setup.\n" );
-			break;
-
-		case QPS_AUXILIARYQPSOLVED:
-			myPrintf( "Status of QP object: an auxilary QP was solved.\n" );
-			break;
-
-		case QPS_PERFORMINGHOMOTOPY:
-			myPrintf( "Status of QP object: a homotopy step is performed.\n" );
-			break;
-
-		case QPS_HOMOTOPYQPSOLVED:
-			myPrintf( "Status of QP object: an intermediate QP along the homotopy path was solved.\n" );
-			break;
-
-		case QPS_SOLVED:
-			myPrintf( "Status of QP object: solution of the actual QP was found.\n" );
-			break;
-	}
-
-	switch ( options.printLevel )
-	{
-		case PL_DEBUG_ITER:
-			myPrintf( "Print level of QP object is set to display a tabular output for debugging.\n" );
-			break;
-
-		case PL_TABULAR:
-			myPrintf( "Print level of QP object is set to display a tabular output.\n" );
-			break;
-
-		case PL_LOW:
-					myPrintf( "Print level of QP object is low, i.e. only error are printed.\n" );
-			break;
-
-		case PL_MEDIUM:
-			myPrintf( "Print level of QP object is medium, i.e. error and warnings are printed.\n" );
-			break;
-
-		case PL_HIGH:
-			myPrintf( "Print level of QP object is high, i.e. all available output is printed.\n" );
-			break;
-
-		default:
-			break;
-	}
-
-	myPrintf( "\n" );
-	#endif
-	#endif
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-returnValue QProblemB::printOptions( ) const
-{
-	return options.print( );
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-/*
- *	c l e a r
- */
-returnValue QProblemB::clear( )
-{
-	if ( ( freeHessian == BT_TRUE ) && ( H != 0 ) )
-	{
-		delete H;
-		H = 0;
-	}
-
-	if ( g != 0 )
-	{
-		delete[] g;
-		g = 0;
-	}
-
-	if ( lb != 0 )
-	{
-		delete[] lb;
-		lb = 0;
-	}
-
-	if ( ub != 0 )
-	{
-		delete[] ub;
-		ub = 0;
-	}
-
-	if ( R != 0 )
-	{
-		delete[] R;
-		R = 0;
-	}
-
-	if ( x != 0 )
-	{
-		delete[] x;
-		x = 0;
-	}
-
-	if ( y != 0 )
-	{
-		delete[] y;
-		y = 0;
-	}
-
-	if ( delta_xFR_TMP != 0 )
-	{
-		delete[] delta_xFR_TMP;
-		delta_xFR_TMP = 0;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	c o p y
- */
-returnValue QProblemB::copy(	const QProblemB& rhs
-								)
-{
-	unsigned int _nV = (unsigned int)rhs.getNV( );
-
-	bounds = rhs.bounds;
-
-	freeHessian = rhs.freeHessian;
-	
-	if ( freeHessian == BT_TRUE )
-		H = (SymmetricMatrix *)(rhs.H->duplicateSym());
-	else
-		H = rhs.H;
-
-	if ( rhs.g != 0 )
-	{
-		g = new real_t[_nV];
-		setG( rhs.g );
-	}
-	else
-		g = 0;
-
-	if ( rhs.lb != 0 )
-	{
-		lb = new real_t[_nV];
-		setLB( rhs.lb );
-	}
-	else
-		lb = 0;
-
-	if ( rhs.ub != 0 )
-	{
-		ub = new real_t[_nV];
-		setUB( rhs.ub );
-	}
-	else
-		ub = 0;
-
-	if ( rhs.R != 0 )
-	{
-		R = new real_t[_nV*_nV];
-		memcpy( R,rhs.R,_nV*_nV*sizeof(real_t) );
-	}
-	else
-		R = 0;
-	
-	haveCholesky = rhs.haveCholesky;
-
-	if ( rhs.x != 0 )
-	{
-		x = new real_t[_nV];
-		memcpy( x,rhs.x,_nV*sizeof(real_t) );
-	}
-	else
-		x = 0;
-
-	if ( rhs.y != 0 )
-	{
-		y = new real_t[_nV];
-		memcpy( y,rhs.y,_nV*sizeof(real_t) );
-	}
-	else
-		y = 0;
-
-	tau = rhs.tau;
-
-	hessianType = rhs.hessianType;
-	regVal = rhs.regVal;
-
-	infeasible = rhs.infeasible;
-	unbounded = rhs.unbounded;
-
-	status = rhs.status;
-
-	count = rhs.count;
-
-	ramp0 = rhs.ramp0;
-	ramp1 = rhs.ramp1;
-
-	delta_xFR_TMP = new real_t[_nV];	/* nFR */
-
-	options = rhs.options;
-	setPrintLevel( options.printLevel );
-
-	flipper = rhs.flipper;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	d e t e r m i n e H e s s i a n T y p e
- */
-returnValue QProblemB::determineHessianType( )
-{
-	int i;
-	int nV = getNV( );
-	real_t curDiag;
-
-	/* if Hessian type has been set by user, do NOT change it! */
-	if ( hessianType != HST_UNKNOWN )
-		return SUCCESSFUL_RETURN;
-
-	/* if Hessian has not been allocated, assume it to be all zeros! */
-	if ( H == 0 )
-	{
-		hessianType = HST_ZERO;
-
-		if ( options.enableRegularisation == BT_FALSE )
-			options.enableRegularisation = BT_TRUE;
-
-		return SUCCESSFUL_RETURN;
-	}
-
-	/* 1) If Hessian has outer-diagonal elements,
-	 *    Hessian is assumed to be positive definite. */
-	hessianType = HST_POSDEF;
-	if (H->isDiag() == BT_FALSE)
-		return SUCCESSFUL_RETURN;
-
-	/* 2) Otherwise it is diagonal and test for identity or zero matrix is performed. */
-	BooleanType isIdentity = BT_TRUE;
-	BooleanType isZero = BT_TRUE;
-
-	for ( i=0; i<nV; ++i )
-	{
-        curDiag = H->diag(i);
-        if ( curDiag >= INFTY )
-            return RET_DIAGONAL_NOT_INITIALISED;
-        
-		if ( curDiag < -ZERO )
-		{
-			hessianType = HST_INDEF;
-			if ( options.enableFlippingBounds == BT_FALSE )
-				return THROWERROR( RET_HESSIAN_INDEFINITE );
-			else
-				return SUCCESSFUL_RETURN;
-		}
-
-		if ( getAbs( curDiag - 1.0 ) > EPS )
-			isIdentity = BT_FALSE;
-
-		if ( getAbs( curDiag ) > EPS )
-			isZero = BT_FALSE;
-	}
-
-	if ( isIdentity == BT_TRUE )
-		hessianType = HST_IDENTITY;
-
-	if ( isZero == BT_TRUE )
-		hessianType = HST_ZERO;
-
-	if ( ( hessianType == HST_ZERO ) && ( options.enableRegularisation == BT_FALSE ) )
-		options.enableRegularisation = BT_TRUE;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p S u b j e c t T o T y p e
- */
-returnValue QProblemB::setupSubjectToType( )
-{
-	return setupSubjectToType( lb,ub );
-}
-
-
-/*
- *	s e t u p S u b j e c t T o T y p e
- */
-returnValue QProblemB::setupSubjectToType( const real_t* const lb_new, const real_t* const ub_new )
-{
-	int i;
-	int nV = getNV( );
-
-
-	/* 1) Check if lower bounds are present. */
-	bounds.setNoLower( BT_TRUE );
-	if ( lb_new != 0 )
-	{
-		for( i=0; i<nV; ++i )
-		{
-			if ( lb_new[i] > -INFTY )
-			{
-				bounds.setNoLower( BT_FALSE );
-				break;
-			}
-		}
-	}
-
-	/* 2) Check if upper bounds are present. */
-	bounds.setNoUpper( BT_TRUE );
-	if ( ub_new != 0 )
-	{
-		for( i=0; i<nV; ++i )
-		{
-			if ( ub_new[i] < INFTY )
-			{
-				bounds.setNoUpper( BT_FALSE );
-				break;
-			}
-		}
-	}
-
-	/* 3) Determine implicitly fixed and unbounded variables. */
-	if ( ( lb_new != 0 ) && ( ub_new != 0 ) )
-	{
-		for( i=0; i<nV; ++i )
-		{
-			if ( ( lb_new[i] <= -INFTY ) && ( ub_new[i] >= INFTY )
-					&& (options.enableFarBounds == BT_FALSE))
-			{
-				bounds.setType( i,ST_UNBOUNDED );
-			}
-			else
-			{
-				if (options.enableEqualities
-						&& lb[i] > ub[i] - options.boundTolerance
-						&& lb_new[i] > ub_new[i] - options.boundTolerance)
-					bounds.setType( i,ST_EQUALITY );
-				else
-					bounds.setType( i,ST_BOUNDED );
-			}
-		}
-	}
-	else
-	{
-		if ( ( lb_new == 0 ) && ( ub_new == 0 ) )
-		{
-			for( i=0; i<nV; ++i )
-				bounds.setType( i,ST_UNBOUNDED );
-		}
-		else
-		{
-			for( i=0; i<nV; ++i )
-				bounds.setType( i,ST_BOUNDED );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	c o m p u t e C h o l e s k y
- */
-returnValue QProblemB::computeCholesky( )
-{
-	int i, j;
-	int nV  = getNV( );
-	int nFR = getNFR( );
-	
-	/* 1) Initialises R with all zeros. */
-	for( i=0; i<nV*nV; ++i )
-		R[i] = 0.0;
-
-	/* 2) Calculate Cholesky decomposition of H (projected to free variables). */
-	if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_IDENTITY ) )
-	{
-		if ( hessianType == HST_ZERO )
-		{
-			/* if Hessian is zero matrix and it has been regularised,
-			 * its Cholesky factor is the identity matrix scaled by sqrt(eps). */
-			if ( usingRegularisation( ) == BT_TRUE )
-			{
-				for( i=0; i<nV; ++i )
-					RR(i,i) = getSqrt( regVal );
-			}
-			/*else
-				return THROWERROR( RET_CHOLESKY_OF_ZERO_HESSIAN );*/
-		}
-		else
-		{
-			/* if Hessian is identity, so is its Cholesky factor. */
-			for( i=0; i<nV; ++i )
-				RR(i,i) = 1.0;
-		}
-	}
-	else
-	{
-		if ( nFR > 0 )
-		{
-			int* FR_idx;
-			bounds.getFree( )->getNumberArray( &FR_idx );
-
-			/* get H */
-			for ( j=0; j < nFR; ++j )
-				H->getCol (FR_idx[j], bounds.getFree (), 1.0, &(R[j*nV]) );
-
-			/* R'*R = H */
-			long info = 0;
-			unsigned long _nFR = (unsigned long)nFR, _nV = (unsigned long)nV;
-
-			POTRF( "U", &_nFR, R, &_nV, &info );
-
-			/* <0 = invalid call, =0 ok, >0 not spd */
-			if (info > 0) {
-				if ( R[0] < 0.0 )
-				{
-					/* Cholesky decomposition has tunneled a negative
-					 * diagonal element. */ 
-					options.epsRegularisation = getMin( -R[0]+options.epsRegularisation,getSqrt(getAbs(options.epsRegularisation)) );
-				}
-
-				hessianType = HST_SEMIDEF;
-				return RET_HESSIAN_NOT_SPD;
-			}
-
-			/* zero first subdiagonal to make givens updates work */
-			for (i=0;i<nFR-1;++i)
-				RR(i+1,i) = 0.0;
-
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p I n i t i a l C h o l e s k y
- */
-returnValue QProblemB::setupInitialCholesky( )
-{
-	returnValue returnvalueCholesky;
-
-	/* If regularisation shall be used, always regularise at beginning 
-	 * if initial working set is not empty. */
-	if ( ( getNV() != getNFR()-getNFV() ) && ( options.enableRegularisation == BT_TRUE ) )
-		if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
-			return RET_INIT_FAILED_REGULARISATION;
-	
-	returnvalueCholesky = computeCholesky( );
-
-	/* If Hessian is not positive definite, regularise and try again. */
-	if ( returnvalueCholesky == RET_HESSIAN_NOT_SPD )
-	{
-		if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
-			return RET_INIT_FAILED_REGULARISATION;
-
-		returnvalueCholesky = computeCholesky( );
-	}
-
-	if ( returnvalueCholesky != SUCCESSFUL_RETURN )
-		return RET_INIT_FAILED_CHOLESKY;
-
-	haveCholesky = BT_TRUE;
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	o b t a i n A u x i l i a r y W o r k i n g S e t
- */
-returnValue QProblemB::obtainAuxiliaryWorkingSet(	const real_t* const xOpt, const real_t* const yOpt,
-													const Bounds* const guessedBounds, Bounds* auxiliaryBounds
-													) const
-{
-	int i = 0;
-	int nV = getNV( );
-
-
-	/* 1) Ensure that desiredBounds is allocated (and different from guessedBounds). */
-	if ( ( auxiliaryBounds == 0 ) || ( auxiliaryBounds == guessedBounds ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-
-	/* 2) Setup working set for auxiliary initial QP. */
-	if ( guessedBounds != 0 )
-	{
-		/* If an initial working set is specific, use it!
-		 * Moreover, add all implictly fixed variables if specified. */
-		for( i=0; i<nV; ++i )
-		{
-			#ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
-			if ( bounds.getType( i ) == ST_EQUALITY )
-			{
-				if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-			}
-			else
-			#endif
-			{
-				if ( auxiliaryBounds->setupBound( i,guessedBounds->getStatus( i ) ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-			}
-		}
-	}
-	else	/* No initial working set specified. */
-	{
-		if ( ( xOpt != 0 ) && ( yOpt == 0 ) )
-		{
-			/* Obtain initial working set by "clipping". */
-			for( i=0; i<nV; ++i )
-			{
-				if ( xOpt[i] <= lb[i] + options.boundTolerance )
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-					continue;
-				}
-
-				if ( xOpt[i] >= ub[i] - options.boundTolerance )
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_UPPER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-					continue;
-				}
-
-				/* Moreover, add all implictly fixed variables if specified. */
-				#ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
-				if ( bounds.getType( i ) == ST_EQUALITY )
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-				else
-				#endif
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-			}
-		}
-
-		if ( ( xOpt == 0 ) && ( yOpt != 0 ) )
-		{
-			/* Obtain initial working set in accordance to sign of dual solution vector. */
-			for( i=0; i<nV; ++i )
-			{
-				if ( yOpt[i] > EPS )
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-					continue;
-				}
-
-				if ( yOpt[i] < -EPS )
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_UPPER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-					continue;
-				}
-
-				/* Moreover, add all implictly fixed variables if specified. */
-				#ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
-				if ( bounds.getType( i ) == ST_EQUALITY )
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-				else
-				#endif
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-			}
-		}
-
-		/* If xOpt and yOpt are null pointer and no initial working is specified,
-		 * start with empty working set (or implicitly fixed bounds only)
-		 * for auxiliary QP. */
-		if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
-		{
-			for( i=0; i<nV; ++i )
-			{
-				switch( bounds.getType( i ) )
-				{
-					case ST_UNBOUNDED:
-						if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-							return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-						break;
-
-					/* Only add all implictly fixed variables if specified. */
-					#ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
-					case ST_EQUALITY:
-						if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-							return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-						break;
-					#endif
-
-					default:
-						if ( auxiliaryBounds->setupBound( i,options.initialStatusBounds ) != SUCCESSFUL_RETURN )
-							return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-						break;
-				}
-			}
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	b a c k s o l v e R
- */
-returnValue QProblemB::backsolveR(	const real_t* const b, BooleanType transposed,
-									real_t* const a
-									) const
-{
-	/* Call standard backsolve procedure (i.e. removingBound == BT_FALSE). */
-	return backsolveR( b,transposed,BT_FALSE,a );
-}
-
-
-/*
- *	b a c k s o l v e R
- */
-returnValue QProblemB::backsolveR(	const real_t* const b, BooleanType transposed,
-									BooleanType removingBound,
-									real_t* const a
-									) const
-{
-	int i, j;
-	int nV = getNV( );
-	int nR = getNZ( );
-
-	real_t sum;
-
-	/* if backsolve is called while removing a bound, reduce nZ by one. */
-	if ( removingBound == BT_TRUE )
-		--nR;
-
-	/* nothing to do */
-	if ( nR <= 0 )
-		return SUCCESSFUL_RETURN;
-
-
-	/* Solve Ra = b, where R might be transposed. */
-	if ( transposed == BT_FALSE )
-	{
-		/* solve Ra = b */
-		for( i=(nR-1); i>=0; --i )
-		{
-			sum = b[i];
-			for( j=(i+1); j<nR; ++j )
-				sum -= RR(i,j) * a[j];
-
-			if ( getAbs( RR(i,i) ) >= ZERO*getAbs( sum ) )
-				a[i] = sum / RR(i,i);
-			else
-				return THROWERROR( RET_DIV_BY_ZERO );
-		}
-	}
-	else
-	{
-		/* solve R^T*a = b */
-		for( i=0; i<nR; ++i )
-		{
-			sum = b[i];
-			for( j=0; j<i; ++j )
-				sum -= RR(j,i) * a[j];
-
-			if ( getAbs( RR(i,i) ) >= ZERO*getAbs( sum ) )
-				a[i] = sum / RR(i,i);
-			else
-				return THROWERROR( RET_DIV_BY_ZERO );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	d e t e r m i n e D a t a S h i f t
- */
-returnValue QProblemB::determineDataShift(	const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
-											real_t* const delta_g, real_t* const delta_lb, real_t* const delta_ub,
-											BooleanType& Delta_bB_isZero
-											)
-{
-	int i, ii;
-	int nV  = getNV( );
-	int nFX = getNFX( );
-
-	int* FX_idx;
-	bounds.getFixed( )->getNumberArray( &FX_idx );
-
-
-	/* 1) Calculate shift directions. */
-	for( i=0; i<nV; ++i )
-		delta_g[i]  = g_new[i]  - g[i];
-
-	if ( lb_new != 0 )
-	{
-		for( i=0; i<nV; ++i )
-			delta_lb[i] = lb_new[i] - lb[i];
-	}
-	else
-	{
-		/* if no lower bounds exist, assume the new lower bounds to be -infinity */
-		for( i=0; i<nV; ++i )
-			delta_lb[i] = -INFTY - lb[i];
-	}
-
-	if ( ub_new != 0 )
-	{
-		for( i=0; i<nV; ++i )
-			delta_ub[i] = ub_new[i] - ub[i];
-	}
-	else
-	{
-		/* if no upper bounds exist, assume the new upper bounds to be infinity */
-		for( i=0; i<nV; ++i )
-			delta_ub[i] = INFTY - ub[i];
-	}
-
-	/* 2) Determine if active bounds are to be shifted. */
-	Delta_bB_isZero = BT_TRUE;
-
-	for ( i=0; i<nFX; ++i )
-	{
-		ii = FX_idx[i];
-
-		if ( ( getAbs( delta_lb[ii] ) > EPS ) || ( getAbs( delta_ub[ii] ) > EPS ) )
-		{
-			Delta_bB_isZero = BT_FALSE;
-			break;
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	s e t u p Q P d a t a
- */
-returnValue QProblemB::setupQPdata(	SymmetricMatrix *_H, const real_t* const _g,
-									const real_t* const _lb, const real_t* const _ub
-									)
-{
-	/* 1) Setup Hessian matrix. */
-	setH( _H );
-
-	/* 2) Setup gradient vector. */
-	if ( _g == 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-	else
-		setG( _g );
-
-	/* 3) Setup lower/upper bounds vector. */
-	setLB( _lb );
-	setUB( _ub );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p Q P d a t a
- */
-returnValue QProblemB::setupQPdata(	const real_t* const _H, const real_t* const _g,
-									const real_t* const _lb, const real_t* const _ub
-									)
-{
-	/* 1) Setup Hessian matrix. */
-	setH( _H );
-
-	/* 2) Setup gradient vector. */
-	if ( _g == 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-	else
-		setG( _g );
-
-	/* 3) Setup lower/upper bounds vector. */
-	setLB( _lb );
-	setUB( _ub );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p Q P d a t a F r o m F i l e
- */
-returnValue QProblemB::setupQPdataFromFile(	const char* const H_file, const char* const g_file,
-											const char* const lb_file, const char* const ub_file
-											)
-{
-	int i;
-	int nV = getNV( );
-
-	returnValue returnvalue;
-
-
-	/* 1) Load Hessian matrix from file. */
-	if ( H_file != 0 )
-	{
-		real_t* _H = new real_t[nV * nV];
-		returnvalue = readFromFile( _H, nV,nV, H_file );
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			delete[] _H;
-			return THROWERROR( returnvalue );
-		}
-		setH( _H );
-		H->doFreeMemory( );
-	}
-	else
-	{
-		real_t* _H = 0;
-		setH( _H );
-	}
-
-	/* 2) Load gradient vector from file. */
-	if ( g_file == 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	returnvalue = readFromFile( g, nV, g_file );
-	if ( returnvalue != SUCCESSFUL_RETURN )
-		return THROWERROR( returnvalue );
-
-	/* 3) Load lower bounds vector from file. */
-	if ( lb_file != 0 )
-	{
-		returnvalue = readFromFile( lb, nV, lb_file );
-		if ( returnvalue != SUCCESSFUL_RETURN )
-			return THROWERROR( returnvalue );
-	}
-	else
-	{
-		/* if no lower bounds are specified, set them to -infinity */
-		for( i=0; i<nV; ++i )
-			lb[i] = -INFTY;
-	}
-
-	/* 4) Load upper bounds vector from file. */
-	if ( ub_file != 0 )
-	{
-		returnvalue = readFromFile( ub, nV, ub_file );
-		if ( returnvalue != SUCCESSFUL_RETURN )
-			return THROWERROR( returnvalue );
-	}
-	else
-	{
-		/* if no upper bounds are specified, set them to infinity */
-		for( i=0; i<nV; ++i )
-			ub[i] = INFTY;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	l o a d Q P v e c t o r s F r o m F i l e
- */
-returnValue QProblemB::loadQPvectorsFromFile(	const char* const g_file, const char* const lb_file, const char* const ub_file,
-												real_t* const g_new, real_t* const lb_new, real_t* const ub_new
-												) const
-{
-	int nV = getNV( );
-
-	returnValue returnvalue;
-
-
-	/* 1) Load gradient vector from file. */
-	if ( ( g_file != 0 ) && ( g_new != 0 ) )
-	{
-		returnvalue = readFromFile( g_new, nV, g_file );
-		if ( returnvalue != SUCCESSFUL_RETURN )
-			return THROWERROR( returnvalue );
-	}
-	else
-	{
-		/* At least gradient vector needs to be specified! */
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	/* 2) Load lower bounds vector from file. */
-	if ( lb_file != 0 )
-	{
-		if ( lb_new != 0 )
-		{
-			returnvalue = readFromFile( lb_new, nV, lb_file );
-			if ( returnvalue != SUCCESSFUL_RETURN )
-				return THROWERROR( returnvalue );
-		}
-		else
-		{
-			/* If filename is given, storage must be provided! */
-			return THROWERROR( RET_INVALID_ARGUMENTS );
-		}
-	}
-
-	/* 3) Load upper bounds vector from file. */
-	if ( ub_file != 0 )
-	{
-		if ( ub_new != 0 )
-		{
-			returnvalue = readFromFile( ub_new, nV, ub_file );
-			if ( returnvalue != SUCCESSFUL_RETURN )
-				return THROWERROR( returnvalue );
-		}
-		else
-		{
-			/* If filename is given, storage must be provided! */
-			return THROWERROR( RET_INVALID_ARGUMENTS );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t I n f e a s i b i l i t y F l a g
- */
-returnValue QProblemB::setInfeasibilityFlag(	returnValue returnvalue,
-												BooleanType doThrowError
-												)
-{
-	infeasible = BT_TRUE;
-
-	if ( ( doThrowError == BT_TRUE ) || ( options.enableFarBounds == BT_FALSE ) )
-		THROWERROR( returnvalue );
-
-	return returnvalue;
-}
-
-
-/*
- *	a r e B o u n d s C o n s i s t e n t
- */
-returnValue QProblemB::areBoundsConsistent(	const real_t* const lb_new, const real_t* const ub_new ) const
-{
-	if (lb_new && ub_new) {
-		for (int i = 0; i < getNV(); ++i) {
-			if (lb_new[i] > ub_new[i]+EPS) {
-				return RET_QP_INFEASIBLE;
-			}
-		}
-	}
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	i s C P U t i m e L i m i t E x c e e d e d
- */
-BooleanType QProblemB::isCPUtimeLimitExceeded(	const real_t* const cputime,
-												real_t starttime,
-												int nWSR
-												) const
-{
-	/* Always perform next QP iteration if no CPU time limit is given. */
-	if ( cputime == 0 )
-		return BT_FALSE;
-
-	/* Always perform first QP iteration. */
-	if ( nWSR <= 0 )
-		return BT_FALSE;
-
-	real_t elapsedTime = getCPUtime( ) - starttime;
-	real_t timePerIteration = elapsedTime / ((real_t) nWSR);
-
-	/* Determine if next QP iteration exceed CPU time limit
-	 * considering the (current) average CPU time per iteration. */
-	if ( ( elapsedTime + timePerIteration*1.25 ) <= ( *cputime ) )
-		return BT_FALSE;
-	else
-		return BT_TRUE;
-}
-
-
-/*
- *	r e g u l a r i s e H e s s i a n
- */
-returnValue QProblemB::regulariseHessian( )
-{
-	/* Do nothing if Hessian regularisation is disbaled! */
-	if ( options.enableRegularisation == BT_FALSE )
-		return SUCCESSFUL_RETURN;
-
-	/* Regularisation of identity Hessian not possible. */
-	if ( hessianType == HST_IDENTITY )
-		return THROWERROR( RET_CANNOT_REGULARISE_IDENTITY );
-
-	/* Determine regularisation parameter. */
-	if ( usingRegularisation( ) == BT_TRUE )
-		return SUCCESSFUL_RETURN; /*THROWERROR( RET_HESSIAN_ALREADY_REGULARISED );*/
-	else
-	{
-		/* Regularisation of zero Hessian is done implicitly. */
-		if ( hessianType == HST_ZERO )
-		{
-			regVal = getNorm( g,getNV() ) * options.epsRegularisation;
-		}
-		else
-		{
-			regVal = H->getNorm() * options.epsRegularisation;
-
-			if ( H->addToDiag( regVal ) == RET_NO_DIAGONAL_AVAILABLE )
-				return THROWERROR( RET_CANNOT_REGULARISE_SPARSE );
-		}
-
-		THROWINFO( RET_USING_REGULARISATION );
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *  c r e a t e D i a g S p a r s e M a t
- */
-SymSparseMat* QProblemB::createDiagSparseMat( int n, real_t diagVal )
-{
-	real_t* M_val = new real_t[n];
-	sparse_int_t* M_jc = new sparse_int_t[n+1];
-	sparse_int_t* M_ir = new sparse_int_t[n+1];
-
-	for( int ii=0; ii<n; ++ii )
-	{
-		M_val[ii] = diagVal;
-		M_jc[ii] = (sparse_int_t)ii;
-		M_ir[ii] = (sparse_int_t)ii;
-	}
-	M_jc[n] = (sparse_int_t)n;
-	M_ir[n] = (sparse_int_t)n;
-
-	SymSparseMat* M = new SymSparseMat( n,n, M_ir,M_jc,M_val );
-	M->createDiagInfo( );
-	M->doFreeMemory( );
-
-	return M;
-}
-
-
-
-/*
- *	p e r f o r m R a t i o T e s t
- */
-returnValue QProblemB::performRatioTest(	int nIdx,
-											const int* const idxList,
-											const SubjectTo* const subjectTo,
-											const real_t* const num,
-											const real_t* const den,
-											real_t epsNum,
-											real_t epsDen,
-											real_t& t,
-											int& BC_idx
-											) const
-{
-	int i, ii;
-
-	BC_idx = -1;
-
-	for( i=0; i<nIdx; ++i )
-	{
-		ii = idxList[i];
-
-		if ( subjectTo->getType( ii ) != ST_EQUALITY )
-		{
-			if ( ( subjectTo->getStatus( ii ) == ST_LOWER ) || ( subjectTo->getStatus( ii ) == ST_INACTIVE ) )
-			{
-				if ( isBlocking( num[i],den[i],epsNum,epsDen,t ) == BT_TRUE )
-				{
-					t = num[i] / den[i];
-					BC_idx = ii;
-				}
-			}
-			else
-			if ( subjectTo->getStatus( ii ) == ST_UPPER )
-			{
-				if ( isBlocking( -num[i],-den[i],epsNum,epsDen,t ) == BT_TRUE )
-				{
-					t = num[i] / den[i];
-					BC_idx = ii;
-				}
-			}
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- * g e t R e l a t i v e H o m o t o p y L e n g t h
- */
-real_t QProblemB::getRelativeHomotopyLength(	const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new
-												)
-{
-	int i;
-	int nV = getNV( );
-	real_t d, s, len = 0.0;
-
-	/* gradient */
-	for (i = 0; i < nV; i++)
-	{
-		s = getAbs(g_new[i]);
-		if (s < 1.0) s = 1.0;
-		d = getAbs(g_new[i] - g[i]) / s;
-		if (d > len) len = d;
-	}
-
-	/* lower bounds */
-	for (i = 0; i < nV && lb_new; i++)
-	{
-		s = getAbs(lb_new[i]);
-		if (s < 1.0) s = 1.0;
-		d = getAbs(lb_new[i] - lb[i]) / s;
-		if (d > len) len = d;
-	}
-
-	/* upper bounds */
-	for (i = 0; i < nV && ub_new; i++)
-	{
-		s = getAbs(ub_new[i]);
-		if (s < 1.0) s = 1.0;
-		d = getAbs(ub_new[i] - ub[i]) / s;
-		if (d > len) len = d;
-	}
-
-	return len;
-}
-
-
-/*
- * p e r f o r m R a m p i n g
- */
-returnValue QProblemB::performRamping( )
-{
-	int nV = getNV( ), bstat, i;
-	real_t t, rampVal;
-
-	/* ramp inactive bounds and active dual variables */
-	for (i = 0; i < nV; i++)
-	{
-		switch (bounds.getType(i))
-		{
-			case ST_EQUALITY: lb[i] = x[i]; ub[i] = x[i]; continue; /* reestablish exact feasibility */
-			case ST_UNBOUNDED: continue;
-			case ST_DISABLED: continue;
-			default: break;
-		}
-
-		t = static_cast<real_t>((i + rampOffset) % nV) / static_cast<real_t>(nV-1);
-		rampVal = (1.0-t) * ramp0 + t * ramp1;
-		bstat = bounds.getStatus(i);
-		if (bstat != ST_LOWER) { lb[i] = x[i] - rampVal; }
-		if (bstat != ST_UPPER) { ub[i] = x[i] + rampVal; }
-		if (bstat == ST_LOWER) { lb[i] = x[i]; y[i] = +rampVal; }
-		if (bstat == ST_UPPER) { ub[i] = x[i]; y[i] = -rampVal; }
-		if (bstat == ST_INACTIVE) y[i] = 0.0; /* reestablish exact complementarity */
-	}
-
-	/* reestablish exact stationarity */
-	setupAuxiliaryQPgradient( );
-
-	/* advance ramp offset to avoid Ramping cycles */
-	rampOffset++;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- * u p d a t e F a r B o u n d s
- */
-returnValue QProblemB::updateFarBounds(	real_t curFarBound, int nRamp,
-											const real_t* const lb_new, real_t* const lb_new_far,
-											const real_t* const ub_new, real_t* const ub_new_far
-											) const
-{
-	int i;
-	real_t rampVal, t;
-	int nV = getNV( );
-
-	if ( options.enableRamping == BT_TRUE )
-	{
-		for ( i=0; i<nV; ++i )
-		{
-			t = static_cast<real_t>((i + rampOffset) % nRamp) / static_cast<real_t>(nRamp-1);
-			rampVal = curFarBound * (1.0 + (1.0-t)*ramp0 + t*ramp1);
-
-			if ( lb_new == 0 )
-				lb_new_far[i] = -rampVal;
-			else
-				lb_new_far[i] = getMax( -rampVal,lb_new[i] );
-
-			if ( ub_new == 0 )
-				ub_new_far[i] = rampVal;
-			else
-				ub_new_far[i] = getMin( rampVal,ub_new[i] );
-		}
-	}
-	else
-	{
-		for ( i=0; i<nV; ++i )
-		{
-			if ( lb_new == 0 )
-				lb_new_far[i] = -curFarBound;
-			else
-				lb_new_far[i] = getMax( -curFarBound,lb_new[i] );
-
-			if ( ub_new == 0 )
-				ub_new_far[i] = curFarBound;
-			else
-				ub_new_far[i] = getMin( curFarBound,ub_new[i] );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-
-
-/*****************************************************************************
- *  P R I V A T E                                                            *
- *****************************************************************************/
-
-/*
- *	s o l v e I n i t i a l Q P
- */
-returnValue QProblemB::solveInitialQP(	const real_t* const xOpt, const real_t* const yOpt,
-										const Bounds* const guessedBounds,
-										int& nWSR, real_t* const cputime
-										)
-{
-	int i;
-	int nV = getNV( );
-
-
-	/* start runtime measurement */
-	real_t starttime = 0.0;
-	if ( cputime != 0 )
-		starttime = getCPUtime( );
-
-
-	status = QPS_NOTINITIALISED;
-
-	/* I) ANALYSE QP DATA: */
-	/* 1) Check if Hessian happens to be the identity matrix. */
-	if ( determineHessianType( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 2) Setup type of bounds (i.e. unbounded, implicitly fixed etc.). */
-	if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	status = QPS_PREPARINGAUXILIARYQP;
-
-
-	/* II) SETUP AUXILIARY QP WITH GIVEN OPTIMAL SOLUTION: */
-	/* 1) Setup bounds data structure. */
-	if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 2) Setup optimal primal/dual solution for auxiliary QP. */
-	if ( setupAuxiliaryQPsolution( xOpt,yOpt ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 3) Obtain linear independent working set for auxiliary QP. */
-	Bounds auxiliaryBounds( nV );
-	if ( obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds, &auxiliaryBounds ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 4) Setup working set of auxiliary QP and setup cholesky decomposition. */
-	/* a) Working set of auxiliary QP. */
-	if ( setupAuxiliaryWorkingSet( &auxiliaryBounds,BT_TRUE ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* b) Regularise Hessian if necessary. */
-	if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_SEMIDEF ) )
-	{
-		if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_INIT_FAILED_REGULARISATION );
-	}
-
-	haveCholesky = BT_FALSE;
-
-	/* 5) Store original QP formulation... */
-	real_t* g_original = new real_t[nV];
-	real_t* lb_original = new real_t[nV];
-	real_t* ub_original = new real_t[nV];
-
-	for( i=0; i<nV; ++i )
-	{
-		g_original[i]  = g[i];
-		lb_original[i] = lb[i];
-		ub_original[i] = ub[i];
-	}
-
-	/* ... and setup QP data of an auxiliary QP having an optimal solution
-	 * as specified by the user (or xOpt = yOpt = 0, by default). */
-	if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
-	{
-		delete[] ub_original; delete[] lb_original; delete[] g_original;
-		return THROWERROR( RET_INIT_FAILED );
-	}
-
-	if ( setupAuxiliaryQPbounds( BT_TRUE ) != SUCCESSFUL_RETURN )
-	{
- 		delete[] ub_original; delete[] lb_original; delete[] g_original;
-		return THROWERROR( RET_INIT_FAILED );
-	}
-
-	status = QPS_AUXILIARYQPSOLVED;
-
-
-	/* III) SOLVE ACTUAL INITIAL QP: */
-
-	/* Allow only remaining CPU time for usual hotstart. */
-	if ( cputime != 0 )
-		*cputime -= getCPUtime( ) - starttime;
-
-	/* Use hotstart method to find the solution of the original initial QP,... */
-	returnValue returnvalue = hotstart( g_original,lb_original,ub_original, nWSR,cputime );
-
-	/* ... deallocate memory,... */
-	delete[] ub_original; delete[] lb_original; delete[] g_original;
-
-	/* ... check for infeasibility and unboundedness... */
-	if ( isInfeasible( ) == BT_TRUE )
-		return THROWERROR( RET_INIT_FAILED_INFEASIBILITY );
-
-	if ( isUnbounded( ) == BT_TRUE )
-		return THROWERROR( RET_INIT_FAILED_UNBOUNDEDNESS );
-
-	/* ... and internal errors. */
-	if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
-		return THROWERROR( RET_INIT_FAILED_HOTSTART );
-
-
-	/* stop runtime measurement */
-	if ( cputime != 0 )
-		*cputime = getCPUtime( ) - starttime;
-
-	THROWINFO( RET_INIT_SUCCESSFUL );
-
-	return returnvalue;
-}
-
-
-/*
- *	s o l v e Q P
- */
-returnValue QProblemB::solveQP(	const real_t* const g_new,
-								const real_t* const lb_new, const real_t* const ub_new,
-								int& nWSR, real_t* const cputime, int nWSRperformed
-								)
-{
-	int iter;
-	int nV  = getNV( );
-
-	/* consistency check */
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
-		 ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
-		 ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
-	{
-		return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
-	}
-
-	/* start runtime measurement */
-	real_t starttime = 0.0;
-	if ( cputime != 0 )
-		starttime = getCPUtime( );
-
-
-	/* I) PREPARATIONS */
-	/* 1) Allocate delta vectors of gradient and bounds,
-	 *    index arrays and step direction arrays. */
-	real_t* delta_xFR = new real_t[nV];
-	real_t* delta_xFX = new real_t[nV];
-	real_t* delta_yFX = new real_t[nV];
-
-	real_t* delta_g  = new real_t[nV];
-	real_t* delta_lb = new real_t[nV];
-	real_t* delta_ub = new real_t[nV];
-
-	returnValue returnvalue;
-	BooleanType Delta_bB_isZero;
-
-	int BC_idx;
-	SubjectToStatus BC_status;
-
-	real_t homotopyLength;
-
-	#ifndef __XPCTARGET__
-	char messageString[MAX_STRING_LENGTH];
-	#endif
-
-	/* 2) Update type of bounds, e.g. a formerly implicitly fixed
-	 *    variable might have become a normal one etc. */
-	if ( setupSubjectToType( lb_new,ub_new ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_HOTSTART_FAILED );
-
-	/* 3) Reset status flags. */
-	infeasible = BT_FALSE;
-	unbounded  = BT_FALSE;
-
-
-	/* II) MAIN HOMOTOPY LOOP */
-	for( iter=nWSRperformed; iter<nWSR; ++iter )
-	{
-		tabularOutput.idxAddB = tabularOutput.idxRemB = tabularOutput.idxAddC = tabularOutput.idxRemC = -1;
-		tabularOutput.excAddB = tabularOutput.excRemB = tabularOutput.excAddC = tabularOutput.excRemC = 0;
-
-		if ( isCPUtimeLimitExceeded( cputime,starttime,iter-nWSRperformed ) == BT_TRUE )
-		{
-			/* Assign number of working set recalculations and stop runtime measurement. */
-			nWSR = iter;
-			if ( cputime != 0 )
-				*cputime = getCPUtime( ) - starttime;
-
-			break;
-		}
-
-		status = QPS_PERFORMINGHOMOTOPY;
-
-		#ifndef __XPCTARGET__
-		snprintf( messageString,MAX_STRING_LENGTH,"%d ...",iter );
-		getGlobalMessageHandler( )->throwInfo( RET_ITERATION_STARTED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		#endif
-
-		/* 2) Initialise shift direction of the gradient and the bounds. */
-		returnvalue = determineDataShift(	g_new,lb_new,ub_new,
-											delta_g,delta_lb,delta_ub,
-											Delta_bB_isZero
-											);
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
-			delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
-
-			/* Assign number of working set recalculations and stop runtime measurement. */
-			nWSR = iter;
-			if ( cputime != 0 )
-				*cputime = getCPUtime( ) - starttime;
-
-			THROWERROR( RET_SHIFT_DETERMINATION_FAILED );
-			return returnvalue;
-		}
-
-		/* 3) Determination of step direction of X and Y. */
-		returnvalue = determineStepDirection(	delta_g,delta_lb,delta_ub,
-												Delta_bB_isZero,
-												delta_xFX,delta_xFR,delta_yFX
-												);
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
-			delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
-
-			/* Assign number of working set recalculations and stop runtime measurement. */
-			nWSR = iter;
-			if ( cputime != 0 )
-				*cputime = getCPUtime( ) - starttime;
-
-			THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
-			return returnvalue;
-		}
-
-
-		/* 4) Determination of step length TAU.
-		 *    This step along the homotopy path is also taken (without changing working set). */
-		returnvalue = performStep(	delta_g,delta_lb,delta_ub,
-									delta_xFX,delta_xFR,delta_yFX,
-									BC_idx,BC_status
-									);
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
-			delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
-
-			/* Assign number of working set recalculations and stop runtime measurement. */
-			nWSR = iter;
-			if ( cputime != 0 )
-				*cputime = getCPUtime( ) - starttime;
-
-			THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );
-			return returnvalue;
-		}
-
-		/* 5) Termination criterion. */
-		homotopyLength = getRelativeHomotopyLength(g_new, lb_new, ub_new);
-		if ( homotopyLength <= options.terminationTolerance )
-		{
-			status = QPS_SOLVED;
-
-			THROWINFO( RET_OPTIMAL_SOLUTION_FOUND );
-
-			if ( printIteration( iter,BC_idx,BC_status,homotopyLength ) != SUCCESSFUL_RETURN )
-				THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
-
-			nWSR = iter;
-			if ( cputime != 0 )
-				*cputime = getCPUtime( ) - starttime;
-
-			delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
-			delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
-
-			return SUCCESSFUL_RETURN;
-		}
-
-
-		/* 6) Change active set. */
-		returnvalue = changeActiveSet( BC_idx,BC_status );
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
-			delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
-
-			/* Assign number of working set recalculations and stop runtime measurement. */
-			nWSR = iter;
-			if ( cputime != 0 )
-				*cputime = getCPUtime( ) - starttime;
-
-			/* checks for infeasibility... */
-			if ( infeasible == BT_TRUE )
-			{
-				status = QPS_HOMOTOPYQPSOLVED;
-				return setInfeasibilityFlag( RET_HOTSTART_STOPPED_INFEASIBILITY );
-			}
-
-			/* ...unboundedness... */
-			if ( unbounded == BT_TRUE ) /* not necessary since objective function convex! */
-				return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );
-
-			/* ... and throw unspecific error otherwise */
-			THROWERROR( RET_HOMOTOPY_STEP_FAILED );
-			return returnvalue;
-		}
-
-		/* 6a) Possibly refactorise projected Hessian from scratch. */
-		if ( ( options.enableCholeskyRefactorisation > 0 ) && ( (iter % options.enableCholeskyRefactorisation) == 0 ) )
-		{
-			returnvalue = computeCholesky( );
-			if (returnvalue != SUCCESSFUL_RETURN)
-			{
-				delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
-				delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
-				return returnvalue;
-			}
-		}
-
-
-		/* 7) Perform Ramping Strategy on zero homotopy step or drift correction (if desired). */
-		 if ( ( tau <= EPS ) && ( options.enableRamping == BT_TRUE ) )
-			performRamping( );
-		else
-		if ( (options.enableDriftCorrection > 0) && ((iter+1) % options.enableDriftCorrection == 0) )
-			performDriftCorrection( );  /* always returns SUCCESSFUL_RETURN */
-
-		/* 8) Output information of successful QP iteration. */
-		status = QPS_HOMOTOPYQPSOLVED;
-
-		if ( printIteration( iter,BC_idx,BC_status,homotopyLength ) != SUCCESSFUL_RETURN )
-			THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
-	}
-
-	delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
-	delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
-
-	/* stop runtime measurement */
-	if ( cputime != 0 )
-		*cputime = getCPUtime( ) - starttime;
-
-
-	/* if programm gets to here, output information that QP could not be solved
-	 * within the given maximum numbers of working set changes */
-	if ( options.printLevel == PL_HIGH )
-	{
-		#ifndef __XPCTARGET__
-		snprintf( messageString,MAX_STRING_LENGTH,"(nWSR = %d)",iter );
-		return getGlobalMessageHandler( )->throwWarning( RET_MAX_NWSR_REACHED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		#else
-		return RET_MAX_NWSR_REACHED;
-		#endif
-	}
-	else
-	{
-		return RET_MAX_NWSR_REACHED;
-	}
-}
-
-
-/*
- *	s o l v e R e g u l a r i s e d Q P
- */
-returnValue QProblemB::solveRegularisedQP(	const real_t* const g_new,
-											const real_t* const lb_new, const real_t* const ub_new,
-											int& nWSR, real_t* const cputime, int nWSRperformed
-											)
-{
-	int i, step;
-	int nV = getNV( );
-
-
-	/* Perform normal QP solution if QP has not been regularised. */
-	if ( usingRegularisation( ) == BT_FALSE )
-		return solveQP( g_new,lb_new,ub_new, nWSR,cputime,nWSRperformed );
-
-
-	/* I) SOLVE USUAL REGULARISED QP */
-	returnValue returnvalue;
-
-	int nWSR_max   = nWSR;
-	int nWSR_total = nWSRperformed;
-
-	real_t cputime_total = 0.0;
-	real_t cputime_cur   = 0.0;
-
-	if ( cputime == 0 )
-	{
-		returnvalue = solveQP( g_new,lb_new,ub_new, nWSR,0,nWSRperformed );
-	}
-	else
-	{
-		cputime_cur = *cputime;
-		returnvalue = solveQP( g_new,lb_new,ub_new, nWSR,&cputime_cur,nWSRperformed );
-	}
-	nWSR_total     = nWSR;
-	cputime_total += cputime_cur;
-
-
-	/* Only continue if QP solution has been successful. */
-	if ( returnvalue != SUCCESSFUL_RETURN )
-	{
-		if ( cputime != 0 )
-			*cputime = cputime_total;
-
-		if ( returnvalue == RET_MAX_NWSR_REACHED )
-			THROWWARNING( RET_NO_REGSTEP_NWSR );
-
-		return returnvalue;
-	}
-
-
-	/* II) PERFORM SUCCESSIVE REGULARISATION STEPS */
-	real_t* gMod = new real_t[nV];
-
-	for( step=0; step<options.numRegularisationSteps; ++step )
-	{
-		/* 1) Modify gradient: gMod = g - eps*xOpt
-		 *    (assuming regularisation matrix to be regVal*Id). */
-		for( i=0; i<nV; ++i )
-			gMod[i] = g_new[i] - regVal*x[i];
-
-		/* 2) Solve regularised QP with modified gradient allowing
-		 *    only as many working set recalculations and CPU time
-		 *    as have been left from previous QP solutions. */
-		if ( cputime == 0 )
-		{
-			nWSR = nWSR_max;
-			returnvalue = solveQP( gMod,lb_new,ub_new, nWSR,0,nWSR_total );
-		}
-		else
-		{
-			nWSR = nWSR_max;
-			cputime_cur = *cputime - cputime_total;
-			returnvalue = solveQP( gMod,lb_new,ub_new, nWSR,&cputime_cur,nWSR_total );
-		}
-
-		nWSR_total     = nWSR;
-		cputime_total += cputime_cur;
-
-		/* Only continue if QP solution has been successful. */
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			delete[] gMod;
-
-			if ( cputime != 0 )
-				*cputime = cputime_total;
-
-			if ( returnvalue == RET_MAX_NWSR_REACHED )
-				THROWWARNING( RET_FEWER_REGSTEPS_NWSR );
-
-			return returnvalue;
-		}
-	}
-
-	delete[] gMod;
-
-	if ( cputime != 0 )
-		*cputime = cputime_total;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A u x i l i a r y W o r k i n g S e t
- */
-returnValue QProblemB::setupAuxiliaryWorkingSet( 	const Bounds* const auxiliaryBounds,
-													BooleanType setupAfresh
-													)
-{
-	int i;
-	int nV = getNV( );
-
-	/* consistency checks */
-	if ( auxiliaryBounds != 0 )
-	{
-		for( i=0; i<nV; ++i )
-			if ( ( bounds.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryBounds->getStatus( i ) == ST_UNDEFINED ) )
-				return THROWERROR( RET_UNKNOWN_BUG );
-	}
-	else
-	{
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-
-	/* I) SETUP CHOLESKY FLAG:
-	 *    Cholesky decomposition shall only be updated if working set
-	 *    shall be updated (i.e. NOT setup afresh!) */
-	BooleanType updateCholesky;
-	if ( setupAfresh == BT_TRUE )
-		updateCholesky = BT_FALSE;
-	else
-		updateCholesky = BT_TRUE;
-
-
-	/* II) REMOVE FORMERLY ACTIVE BOUNDS (IF NECESSARY): */
-	if ( setupAfresh == BT_FALSE )
-	{
-		/* Remove all active bounds that shall be inactive AND
-		*  all active bounds that are active at the wrong bound. */
-		for( i=0; i<nV; ++i )
-		{
-			if ( ( bounds.getStatus( i ) == ST_LOWER ) && ( auxiliaryBounds->getStatus( i ) != ST_LOWER ) )
-				if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-
-			if ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) )
-				if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-		}
-	}
-
-
-	/* III) ADD NEWLY ACTIVE BOUNDS: */
-	/*      Add all inactive bounds that shall be active AND
-	 *      all formerly active bounds that have been active at the wrong bound. */
-	for( i=0; i<nV; ++i )
-	{
-		if ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) )
-		{
-			if ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A u x i l i a r y Q P s o l u t i o n
- */
-returnValue QProblemB::setupAuxiliaryQPsolution(	const real_t* const xOpt, const real_t* const yOpt
-													)
-{
-	int i;
-	int nV = getNV( );
-
-
-	/* Setup primal/dual solution vectors for auxiliary initial QP:
-	 * if a null pointer is passed, a zero vector is assigned;
-	 * old solution vector is kept if pointer to internal solution vector is passed. */
-	if ( xOpt != 0 )
-	{
-		if ( xOpt != x )
-			for( i=0; i<nV; ++i )
-				x[i] = xOpt[i];
-	}
-	else
-	{
-		for( i=0; i<nV; ++i )
-			x[i] = 0.0;
-	}
-
-	if ( yOpt != 0 )
-	{
-		if ( yOpt != y )
-			for( i=0; i<nV; ++i )
-				y[i] = yOpt[i];
-	}
-	else
-	{
-		for( i=0; i<nV; ++i )
-			y[i] = 0.0;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A u x i l i a r y Q P g r a d i e n t
- */
-returnValue QProblemB::setupAuxiliaryQPgradient( )
-{
-	int i;
-	int nV = getNV( );
-
-	/* Setup gradient vector: g = -H*x + y'*Id. */
-	switch ( hessianType )
-	{
-		case HST_ZERO:
-			if ( usingRegularisation( ) == BT_FALSE )
-				for ( i=0; i<nV; ++i )
-					g[i] = y[i];
-			else
-				for ( i=0; i<nV; ++i )
-					g[i] = y[i] - regVal*x[i];
-			break;
-
-		case HST_IDENTITY:
-			for ( i=0; i<nV; ++i )
-				g[i] = y[i] - x[i];
-			break;
-
-		default:
-			/* y'*Id */
-			for ( i=0; i<nV; ++i )
-				g[i] = y[i];
-
-			/* -H*x */
-			H->times(1, -1.0, x, nV, 1.0, g, nV);
-
-			break;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A u x i l i a r y Q P b o u n d s
- */
-returnValue QProblemB::setupAuxiliaryQPbounds( BooleanType useRelaxation )
-{
-	int i;
-	int nV = getNV( );
-
-
-	/* Setup bound vectors. */
-	for ( i=0; i<nV; ++i )
-	{
-		switch ( bounds.getStatus( i ) )
-		{
-			case ST_INACTIVE:
-				if ( useRelaxation == BT_TRUE )
-				{
-					if ( bounds.getType( i ) == ST_EQUALITY )
-					{
-						lb[i] = x[i];
-						ub[i] = x[i];
-					}
-					else
-					{
-						lb[i] = x[i] - options.boundRelaxation;
-						ub[i] = x[i] + options.boundRelaxation;
-					}
-				}
-				break;
-
-			case ST_LOWER:
-				lb[i] = x[i];
-				if ( bounds.getType( i ) == ST_EQUALITY )
-				{
-					ub[i] = x[i];
-				}
-				else
-				{
-					if ( useRelaxation == BT_TRUE )
-						ub[i] = x[i] + options.boundRelaxation;
-				}
-				break;
-
-			case ST_UPPER:
-				ub[i] = x[i];
-				if ( bounds.getType( i ) == ST_EQUALITY )
-				{
-					lb[i] = x[i];
-				}
-				else
-				{
-					if ( useRelaxation == BT_TRUE )
-						lb[i] = x[i] - options.boundRelaxation;
-				}
-				break;
-
-            case ST_DISABLED:
-                break;
-                
-			default:
-				return THROWERROR( RET_UNKNOWN_BUG );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A u x i l i a r y Q P
- */
-returnValue QProblemB::setupAuxiliaryQP( const Bounds* const guessedBounds )
-{
-	int i;
-	int nV = getNV( );
-
-	/* nothing to do */
-	if ( guessedBounds == &bounds )
-		return SUCCESSFUL_RETURN;
-
-	status = QPS_PREPARINGAUXILIARYQP;
-
-
-	/* I) SETUP WORKING SET ... */
-	if ( shallRefactorise( guessedBounds ) == BT_TRUE )
-	{
-		/* ... WITH REFACTORISATION: */
-		/* 1) Reset bounds ... */
-		bounds.init( nV );
-
-		/*    ... and set them up afresh. */
-		if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-		if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-		/* 2) Setup guessed working set afresh. */
-		if ( setupAuxiliaryWorkingSet( guessedBounds,BT_TRUE ) != SUCCESSFUL_RETURN )
-			THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-		/* 3) Calculate Cholesky decomposition. */
-		if ( computeCholesky( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-	}
-	else
-	{
-		/* ... WITHOUT REFACTORISATION: */
-		if ( setupAuxiliaryWorkingSet( guessedBounds,BT_FALSE ) != SUCCESSFUL_RETURN )
-			THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-	}
-
-
-	/* II) SETUP AUXILIARY QP DATA: */
-	/* 1) Ensure that dual variable is zero for free bounds. */
-	for ( i=0; i<nV; ++i )
-		if ( bounds.getStatus( i ) == ST_INACTIVE )
-			y[i] = 0.0;
-
-	/* 2) Setup gradient and bound vectors. */
-	if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
-		THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-	if ( setupAuxiliaryQPbounds( BT_FALSE ) != SUCCESSFUL_RETURN )
-		THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	d e t e r m i n e S t e p D i r e c t i o n
- */
-returnValue QProblemB::determineStepDirection(	const real_t* const delta_g, const real_t* const delta_lb, const real_t* const delta_ub,
-												BooleanType Delta_bB_isZero,
-												real_t* const delta_xFX, real_t* const delta_xFR,
-												real_t* const delta_yFX
-												)
-{
-	int i, ii;
-	int r;
-	int nFR = getNFR( );
-	int nFX = getNFX( );
-	
-	int* FR_idx;
-	int* FX_idx;
-
-	bounds.getFree( )->getNumberArray( &FR_idx );
-	bounds.getFixed( )->getNumberArray( &FX_idx );
-
-
-	/* This routine computes
-	 * delta_xFX := delta_b
-	 * delta_xFR := R \ R' \ -( delta_g + HMX*delta_xFX )
-	 * delta_yFX := HMX'*delta_xFR + HFX*delta_xFX  { + eps*delta_xFX }
-	 */
-
-	/* I) DETERMINE delta_xFX := delta_{l|u}b */
-	if ( Delta_bB_isZero == BT_FALSE )
-	{
-		for( i=0; i<nFX; ++i )
-		{
-			ii = FX_idx[i];
-
-			if ( bounds.getStatus( ii ) == ST_LOWER )
-				delta_xFX[i] = delta_lb[ii];
-			else
-				delta_xFX[i] = delta_ub[ii];
-		}
-	}
-	else
-	{
-		for( i=0; i<nFX; ++i )
-			delta_xFX[i] = 0.0;
-	}
-
-
-	/* delta_xFR_TMP holds the residual, initialized with right hand side
-	 * delta_xFR holds the step that gets refined incrementally */
-	for ( i=0; i<nFR; ++i )
-	{
-		ii = FR_idx[i];
-		delta_xFR_TMP[i] = - delta_g[ii];
-		delta_xFR[i] = 0.0;
-	}
-
-
-	/* Iterative refinement loop for delta_xFR */
-	for ( r=0; r<=options.numRefinementSteps; ++r )
-	{
-		/* II) DETERMINE delta_xFR */
-		if ( nFR > 0 )
-		{
-			/* Add - HMX*delta_xFX
-			 * This is skipped if delta_b=0 or mixed part HM=0 (H=0 or H=Id) */
-			if ( ( hessianType != HST_ZERO ) && ( hessianType != HST_IDENTITY ) && ( Delta_bB_isZero == BT_FALSE ) && ( r == 0 ) )
-				H->times(bounds.getFree(), bounds.getFixed(), 1, -1.0, delta_xFX, nFX, 1.0, delta_xFR_TMP, nFR);
-
-			/* Determine R' \ ( - HMX*delta_xFX - delta_gFR ) where R'R = HFR */
-			if ( backsolveR( delta_xFR_TMP,BT_TRUE,delta_xFR_TMP ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
-
-			/* Determine HFR \ ( - HMX*delta_xFX - delta_gFR ) */
-			if ( backsolveR( delta_xFR_TMP,BT_FALSE,delta_xFR_TMP ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
-		}
-
-		/* refine solution found for delta_xFR so far */
-		for ( i=0; i<nFR; ++i )
-			delta_xFR[i] += delta_xFR_TMP[i];
-
-		if ( options.numRefinementSteps > 0 )
-		{
-			real_t rnrm = 0.0;
-			/* compute new residual in delta_xFR_TMP:
-			 * residual := - HFR*delta_xFR - HMX*delta_xFX - delta_gFR
-			 * set to -delta_gFR */
-			for ( i=0; i<nFR; ++i )
-			{
-				ii = FR_idx[i];
-				delta_xFR_TMP[i] = -delta_g[ii];
-			}
-			/* add - HFR*delta_xFR */
-			switch ( hessianType )
-			{
-				case HST_ZERO:
-					break;
-
-				case HST_IDENTITY:
-					for ( i=0; i<nFR; ++i )
-					{
-						delta_xFR_TMP[i] -= delta_xFR[i];
-
-						/* compute max norm */
-						if (rnrm < getAbs (delta_xFR_TMP[i]))
-							rnrm = getAbs (delta_xFR_TMP[i]);
-					}
-					break;
-
-				default:
-					H->times(bounds.getFree(), bounds.getFree(),  1, -1.0, delta_xFR, nFR, 1.0, delta_xFR_TMP, nFR);
-					H->times(bounds.getFree(), bounds.getFixed(), 1, -1.0, delta_xFX, nFX, 1.0, delta_xFR_TMP, nFR);
-
-					/* compute max norm */
-					for ( i=0; i<nFR; ++i )
-						if (rnrm < getAbs (delta_xFR_TMP[i]))
-							rnrm = getAbs (delta_xFR_TMP[i]);
-
-					break;
-			}
-			
-			/* early termination of residual norm small enough */
-			if ( rnrm < options.epsIterRef )
-				break;
-		}
-
-	} /* end of refinement loop for delta_xFR */
-
-	/* III) DETERMINE delta_yFX */
-	if ( nFX > 0 )
-	{
-		if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_IDENTITY ) )
-		{
-			for( i=0; i<nFX; ++i )
-			{
-				/* set to delta_g */
-				ii = FX_idx[i];
-				delta_yFX[i] = delta_g[ii];
-
-				/* add HFX*delta_xFX = {0|I}*delta_xFX */
-				if ( hessianType == HST_ZERO )
-				{
-					if ( usingRegularisation( ) == BT_TRUE )
-						delta_yFX[i] += regVal*delta_xFX[i];
-				}
-				else
-					delta_yFX[i] += delta_xFX[i];
-			}
-		}
-		else
-		{
-			for( i=0; i<nFX; ++i )
-			{
-				/* set to delta_g */
-				ii = FX_idx[i];
-				delta_yFX[i] = delta_g[ii];
-			}
-			H->times(bounds.getFixed(), bounds.getFree(), 1, 1.0, delta_xFR, nFR, 1.0, delta_yFX, nFX);
-			if (Delta_bB_isZero == BT_FALSE)
-				H->times(bounds.getFixed(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, delta_yFX, nFX);
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p e r f o r m S t e p
- */
-returnValue QProblemB::performStep(	const real_t* const delta_g,
-									const real_t* const delta_lb, const real_t* const delta_ub,
-									const real_t* const delta_xFX,
-									const real_t* const delta_xFR,
-									const real_t* const delta_yFX,
-									int& BC_idx, SubjectToStatus& BC_status
-									)
-{
-	int i, ii;
-	int nV = getNV( );
-	int nFR = getNFR( );
-	int nFX = getNFX( );
-
-	int* FR_idx;
-	int* FX_idx;
-
-	bounds.getFree( )->getNumberArray( &FR_idx );
-	bounds.getFixed( )->getNumberArray( &FX_idx );
-
-	tau = 1.0;
-	BC_idx = -1;
-	BC_status = ST_UNDEFINED;
-
-	int BC_idx_tmp = -1;
-
-	real_t* num = new real_t[nV];
-	real_t* den = new real_t[nV];
-
-
-	/* I) DETERMINE MAXIMUM DUAL STEPLENGTH, i.e. ensure that
-	 *    active dual bounds remain valid (ignoring implicitly fixed variables): */
-	for( i=0; i<nFX; ++i )
-	{
-		ii = FX_idx[i];
-		num[i] = y[ii];
-		den[i] = -delta_yFX[i];
-	}
-
-	performRatioTest( nFX,FX_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
-
-	if ( BC_idx_tmp >= 0 )
-	{
-		BC_idx = BC_idx_tmp;
-		BC_status = ST_INACTIVE;
-	}
-
-
-	/* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH, i.e. ensure that
-	 *     inactive bounds remain valid (ignoring unbounded variables). */
-	/* 1) Inactive lower bounds. */
-	if ( bounds.hasNoLower( ) == BT_FALSE )
-	{
-		for( i=0; i<nFR; ++i )
-		{
-			ii = FR_idx[i];
-			num[i] = getMax( x[ii] - lb[ii],0.0 );
-			den[i] = delta_lb[ii] - delta_xFR[i];
-		}
-
-		performRatioTest( nFR,FR_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
-
-		if ( BC_idx_tmp >= 0 )
-		{
-			BC_idx = BC_idx_tmp;
-			BC_status = ST_LOWER;
-		}
-	}
-
-	/* 2) Inactive upper bounds. */
-	if ( bounds.hasNoUpper( ) == BT_FALSE )
-	{
-		for( i=0; i<nFR; ++i )
-		{
-			ii = FR_idx[i];
-			num[i] = getMax( ub[ii] - x[ii],0.0 );
-			den[i] = delta_xFR[i] - delta_ub[ii];
-		}
-
-		performRatioTest( nFR,FR_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
-
-		if ( BC_idx_tmp >= 0 )
-		{
-			BC_idx = BC_idx_tmp;
-			BC_status = ST_UPPER;
-		}
-	}
-
-	delete[] den;
-	delete[] num;
-
-
-	#ifndef __XPCTARGET__
-	char messageString[MAX_STRING_LENGTH];
-
-	if ( BC_status == ST_UNDEFINED )
-		snprintf( messageString,MAX_STRING_LENGTH,"Stepsize is %.15e!",tau );
-	else
-		snprintf( messageString,MAX_STRING_LENGTH,"Stepsize is %.15e! (idx = %d, status = %d)",tau,BC_idx,BC_status );
-
-	getGlobalMessageHandler( )->throwInfo( RET_STEPSIZE_NONPOSITIVE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-	#endif
-
-
-	/* III) PERFORM STEP ALONG HOMOTOPY PATH */
-	if ( tau > ZERO )
-	{
-		/* 1) Perform step in primal und dual space. */
-		for( i=0; i<nFR; ++i )
-		{
-			ii = FR_idx[i];
-			x[ii] += tau*delta_xFR[i];
-		}
-
-		for( i=0; i<nFX; ++i )
-		{
-			ii = FX_idx[i];
-			x[ii] += tau*delta_xFX[i];
-			y[ii] += tau*delta_yFX[i];
-		}
-
-		/* 2) Shift QP data. */
-		for( i=0; i<nV; ++i )
-		{
-			g[i]  += tau*delta_g[i];
-			lb[i] += tau*delta_lb[i];
-			ub[i] += tau*delta_ub[i];
-		}
-	}
-	else
-	{
-		/* print a warning if stepsize is zero */
-		#ifndef __XPCTARGET__
-		snprintf( messageString,MAX_STRING_LENGTH,"Stepsize is %.15e",tau );
-		getGlobalMessageHandler( )->throwWarning( RET_STEPSIZE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		#endif
-	}
-
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	c h a n g e A c t i v e S e t
- */
-returnValue QProblemB::changeActiveSet( int BC_idx, SubjectToStatus BC_status )
-{
-	#ifndef __XPCTARGET__
-	char messageString[MAX_STRING_LENGTH];
-	#endif
-
-	/* IV) UPDATE ACTIVE SET */
-	switch ( BC_status )
-	{
-		/* Optimal solution found as no working set change detected. */
-		case ST_UNDEFINED:
-			return RET_OPTIMAL_SOLUTION_FOUND;
-
-
-		/* Remove one variable from active set. */
-		case ST_INACTIVE:
-			#ifndef __XPCTARGET__
-			snprintf( messageString,MAX_STRING_LENGTH,"bound no. %d.", BC_idx );
-			getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-			#endif
-
-			if ( removeBound( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
-
-			y[BC_idx] = 0.0;
-			break;
-
-
-		/* Add one variable to active set. */
-		default:
-			#ifndef __XPCTARGET__
-			if ( BC_status == ST_LOWER )
-				snprintf( messageString,MAX_STRING_LENGTH,"lower bound no. %d.", BC_idx );
-			else
-				snprintf( messageString,MAX_STRING_LENGTH,"upper bound no. %d.", BC_idx );
-				getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-			#endif
-
-			if ( addBound( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
-			break;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- * p e r f o r m D r i f t C o r r e c t i o n
- */
-returnValue QProblemB::performDriftCorrection( )
-{
-	int i;
-	int nV = getNV ();
-
-	for ( i=0; i<nV; ++i )
-	{
-		switch ( bounds.getType ( i ) )
-		{
-			case ST_BOUNDED:
-				switch ( bounds.getStatus ( i ) )
-				{
-					case ST_LOWER:
-						lb[i] = x[i];
-						ub[i] = getMax (ub[i], x[i]);
-						y[i] = getMax (y[i], 0.0);
-						break;
-					case ST_UPPER:
-						lb[i] = getMin (lb[i], x[i]);
-						ub[i] = x[i];
-						y[i] = getMin (y[i], 0.0);
-						break;
-					case ST_INACTIVE:
-						lb[i] = getMin (lb[i], x[i]);
-						ub[i] = getMax (ub[i], x[i]);
-						y[i] = 0.0;
-						break;
-					case ST_UNDEFINED:
-					case ST_INFEASIBLE_LOWER:
-					case ST_INFEASIBLE_UPPER:
-						break;
-				}
-				break;
-			case ST_EQUALITY:
-				lb[i] = x[i];
-				ub[i] = x[i];
-				break;
-			case ST_UNBOUNDED:
-			case ST_UNKNOWN:
-            case ST_DISABLED:
-				break;
-		}
-	}
-
-	return setupAuxiliaryQPgradient( );
-}
-
-
-/*
- *	s h a l l R e f a c t o r i s e
- */
-BooleanType QProblemB::shallRefactorise( const Bounds* const guessedBounds ) const
-{
-	int i;
-	int nV = getNV( );
-
-	/* always refactorise if Hessian is not known to be positive definite */
-	if ( ( hessianType == HST_SEMIDEF ) || ( hessianType == HST_INDEF ) )
-		return BT_TRUE;
-
-	/* 1) Determine number of bounds that have same status
-	 *    in guessed AND current bounds.*/
-	int differenceNumber = 0;
-
-	for( i=0; i<nV; ++i )
-		if ( guessedBounds->getStatus( i ) != bounds.getStatus( i ) )
-			++differenceNumber;
-
-	/* 2) Decide wheter to refactorise or not. */
-	if ( 2*differenceNumber > guessedBounds->getNFX( ) )
-		return BT_TRUE;
-	else
-		return BT_FALSE;
-}
-
-
-/*
- *	a d d B o u n d
- */
-returnValue QProblemB::addBound(	int number, SubjectToStatus B_status,
-									BooleanType updateCholesky
-									)
-{
-	int i, j;
-	int nV  = getNV( );
-	int nFR = getNFR( );
-
-
-	/* consistency check */
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
-		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
-		 ( getStatus( ) == QPS_SOLVED )            )
-	{
-		return THROWERROR( RET_UNKNOWN_BUG );
-	}
-
-	/* Perform cholesky updates only if QProblemB has been initialised! */
-	if ( getStatus( ) == QPS_PREPARINGAUXILIARYQP )
-	{
-		/* UPDATE INDICES */
-		if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_ADDBOUND_FAILED );
-
-		return SUCCESSFUL_RETURN;
-	}
-
-
-	/* I) PERFORM CHOLESKY UPDATE: */
-	if ( ( updateCholesky == BT_TRUE ) &&
-		 ( hessianType != HST_ZERO )   && ( hessianType != HST_IDENTITY ) )
-	{
-		/* 1) Index of variable to be added within the list of free variables. */
-		int number_idx = bounds.getFree( )->getIndex( number );
-
-		real_t c, s, nu;
-
-		/* 2) Use row-wise Givens rotations to restore upper triangular form of R. */
-		for( i=number_idx+1; i<nFR; ++i )
-		{
-			computeGivens( RR(i-1,i),RR(i,i), RR(i-1,i),RR(i,i),c,s );
-			nu = s/(1.0+c);
-
-			for( j=(1+i); j<nFR; ++j ) /* last column of R is thrown away */
-				applyGivens( c,s,nu,RR(i-1,j),RR(i,j), RR(i-1,j),RR(i,j) );
-		}
-
-		/* 3) Delete <number_idx>th column and ... */
-		for( i=0; i<nFR-1; ++i )
-			for( j=number_idx+1; j<nFR; ++j )
-				RR(i,j-1) = RR(i,j);
-		/* ... last column of R. */
-		for( i=0; i<nFR; ++i )
-			RR(i,nFR-1) = 0.0;
-	}
-
-	/* II) UPDATE INDICES */
-	tabularOutput.idxAddB = number;
-	if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_ADDBOUND_FAILED );
-
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	r e m o v e B o u n d
- */
-returnValue QProblemB::removeBound(	int number,
-									BooleanType updateCholesky
-									)
-{
-	int i;
-	int nV  = getNV( );
-	int nFR = getNFR( );
-
-
-	/* consistency check */
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
-		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
-		 ( getStatus( ) == QPS_SOLVED )            )
-	{
-		return THROWERROR( RET_UNKNOWN_BUG );
-	}
-
-	/* save index sets and decompositions for flipping bounds strategy */
-	if ( options.enableFlippingBounds == BT_TRUE )
-		flipper.set( &bounds,R );
-
-	/* I) UPDATE INDICES */
-	tabularOutput.idxRemB = number;
-	if ( bounds.moveFixedToFree( number ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_REMOVEBOUND_FAILED );
-
-	/* Perform cholesky updates only if QProblemB has been initialised! */
-	if ( getStatus( ) == QPS_PREPARINGAUXILIARYQP )
-		return SUCCESSFUL_RETURN;
-
-
-	/* II) PERFORM CHOLESKY UPDATE */
-	if ( ( updateCholesky == BT_TRUE ) &&
-		 ( hessianType != HST_ZERO )   && ( hessianType != HST_IDENTITY ) )
-	{
-		int* FR_idx;
-		bounds.getFree( )->getNumberArray( &FR_idx );
-
-		/* 1) Calculate new column of cholesky decomposition. */
-		real_t* rhs = new real_t[nFR+1];
-		real_t* r   = new real_t[nFR];
-
-		real_t r0;
-		switch ( hessianType )
-		{
-			case HST_ZERO: /* TODO: Code can/should? never get here!! */
-				if ( usingRegularisation( ) == BT_FALSE )
-					r0 = 0.0;
-				else
-					r0 = regVal;
-				for( i=0; i<nFR; ++i )
-					rhs[i] = 0.0;
-				break;
-
-			case HST_IDENTITY:
-				r0 = 1.0;
-				for( i=0; i<nFR; ++i )
-					rhs[i] = 0.0;
-				break;
-
-			default:
-				H->getRow(number, bounds.getFree(), 1.0, rhs);
-				r0 = H->diag(number);
-				break;
-		}
-
-		if ( backsolveR( rhs,BT_TRUE,BT_TRUE,r ) != SUCCESSFUL_RETURN )
-		{
-			delete[] rhs; delete[] r;
-			return THROWERROR( RET_REMOVEBOUND_FAILED );
-		}
-
-		for( i=0; i<nFR; ++i )
-			r0 -= r[i]*r[i];
-
-		/* 2) Store new column into R. */
-		for( i=0; i<nFR; ++i )
-			RR(i,nFR) = r[i];
-
-		if ( options.enableFlippingBounds == BT_TRUE )
-		{
-			if ( r0 > options.epsFlipping )
-				RR(nFR,nFR) = getSqrt( r0 );
-			else
-			{
-				hessianType = HST_SEMIDEF;
-
-				flipper.get( &bounds,R );
-				bounds.flipFixed(number);
-
-				switch (bounds.getStatus(number))
-				{
-					case ST_LOWER: lb[number] = ub[number]; break;
-					case ST_UPPER: ub[number] = lb[number]; break;
-					default: delete[] rhs; delete[] r; return THROWERROR( RET_MOVING_BOUND_FAILED );
-				}
-
-			}
-		}
-		else
-		{
-			if ( r0 > ZERO )
-				RR(nFR,nFR) = getSqrt( r0 );
-			else
-			{
-				delete[] rhs; delete[] r;
-
-				hessianType = HST_SEMIDEF;
-				return THROWERROR( RET_HESSIAN_NOT_SPD );
-			}
-		}
-
-		delete[] rhs; delete[] r;
-	}
-
-	if ( ( hessianType == HST_ZERO ) && ( options.enableFlippingBounds == BT_TRUE ) )
-	{
-		flipper.get( &bounds,R );
-		bounds.flipFixed(number);
-
-		switch (bounds.getStatus(number))
-		{
-			case ST_LOWER: lb[number] = ub[number]; break;
-			case ST_UPPER: ub[number] = lb[number]; break;
-			default: return THROWERROR( RET_MOVING_BOUND_FAILED );
-		}
-
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	p r i n t I t e r a t i o n
- */
-returnValue QProblemB::printIteration( 	int iter,
-											int BC_idx,	SubjectToStatus BC_status, real_t homotopyLength
-											)
-{
-	#ifndef __XPCTARGET__
-
-	/* consistency check */
-	if ( iter < 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	int i;
-	int nV = getNV();
-	real_t stat, bfeas, bcmpl;
-	real_t *grad = 0;
-		
-	char myPrintfString[MAX_STRING_LENGTH];
-	const char excStr[] = " ef";
-
-	switch ( options.printLevel )
-	{
-		case PL_DEBUG_ITER:
-			grad = new real_t[nV];
-			stat = bfeas = bcmpl = 0.0;
-
-			/* stationarity */
-			for (i = 0; i < nV; i++) grad[i] = g[i] - y[i];
-			H->times(1, 1.0, x, nV, 1.0, grad, nV);
-			for (i = 0; i < nV; i++) if (getAbs(grad[i]) > stat) stat = getAbs(grad[i]);
-
-			/* feasibility */
-			for (i = 0; i < nV; i++) if (lb[i] - x[i] > bfeas) bfeas = lb[i] - x[i];
-			for (i = 0; i < nV; i++) if (x[i] - ub[i] > bfeas) bfeas = x[i] - ub[i];
-
-			/* complementarity */
-			for (i = 0; i < nV; i++) if (y[i] > +EPS && getAbs((lb[i] - x[i])*y[i]) > bcmpl) bcmpl = getAbs((lb[i] - x[i])*y[i]);
-			for (i = 0; i < nV; i++) if (y[i] < -EPS && getAbs((ub[i] - x[i])*y[i]) > bcmpl) bcmpl = getAbs((ub[i] - x[i])*y[i]);
-
-			if (iter % 10 == 0)
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH, "\n%5s %4s %4s %9s %9s %9s %9s %9s\n",
-						"iter", "addB", "remB", "hom len", "tau", "stat", "bfeas", "bcmpl");
-			}
-			myPrintf( myPrintfString );
-
-			snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d ", iter);
-			myPrintf( myPrintfString );
-		
-			if (tabularOutput.idxAddB >= 0)
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH, "%4d ", tabularOutput.idxAddB);
-				myPrintf( myPrintfString );
-			}
-			else
-			{
-				myPrintf( "     " );
-			}
-
-			if (tabularOutput.idxRemB >= 0)
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH, "%4d ", tabularOutput.idxRemB);
-				myPrintf( myPrintfString );
-			}
-			else 
-			{
-				myPrintf( "     " );
-			}
-
-			snprintf( myPrintfString,MAX_STRING_LENGTH, "%9.2e %9.2e %9.2e %9.2e %9.2e\n",
-					homotopyLength, tau, stat, bfeas, bcmpl);
-			myPrintf( myPrintfString );
-
-			delete[] grad;
-			break;
-
-		case PL_TABULAR:
-			if (iter % 10 == 0)
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH, "\n%5s %6s %6s %9s %9s\n",
-						"iter", "addB", "remB", "hom len", "tau");
-				myPrintf( myPrintfString );
-			}
-
-			snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d ", iter);
-			myPrintf( myPrintfString );
-
-			if (tabularOutput.idxAddB >= 0)
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d%c ", tabularOutput.idxAddB, excStr[tabularOutput.excAddB]);
-				myPrintf( myPrintfString );
-			}
-			else
-			{
-				myPrintf( "       " );
-			}
-
-			if (tabularOutput.idxRemB >= 0)
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH, "%5d%c ", tabularOutput.idxRemB, excStr[tabularOutput.excRemB]);
-				myPrintf( myPrintfString );
-			}
-			else 
-			{
-				myPrintf( "       " );
-			}
-					
-			snprintf( myPrintfString,MAX_STRING_LENGTH, "%9.2e %9.2e\n", homotopyLength, tau);
-			myPrintf( myPrintfString );
-			break;
-
-		case PL_MEDIUM:
-			/* 1) Print header at first iteration. */
- 			if ( iter == 0 )
-			{
-				snprintf( myPrintfString,MAX_STRING_LENGTH,"\n\n#################   qpOASES  --  QP NO. %3.0d   ##################\n\n", count );
-				myPrintf( myPrintfString );
-
-				myPrintf( "    Iter   |    StepLength    |       Info       |   nFX    \n" );
-				myPrintf( " ----------+------------------+------------------+--------- \n" );
-			}
-
-			/* 2) Print iteration line. */
-			if ( BC_status == ST_UNDEFINED )
-			{
-				if ( hessianType == HST_ZERO )
-					snprintf( myPrintfString,MAX_STRING_LENGTH,"   %5.1d   |   %1.6e   |    LP SOLVED     |  %4.1d   \n", iter,tau,getNFX( ) );
-				else
-					snprintf( myPrintfString,MAX_STRING_LENGTH,"   %5.1d   |   %1.6e   |    QP SOLVED     |  %4.1d   \n", iter,tau,getNFX( ) );
-				myPrintf( myPrintfString );
-			}
-			else
-			{
-				char info[MAX_STRING_LENGTH];
-
-				if ( BC_status == ST_INACTIVE )
-					snprintf( info,8,"REM BND" );
-				else
-					snprintf( info,8,"ADD BND" );
-
-				snprintf( myPrintfString,MAX_STRING_LENGTH,"   %5.1d   |   %1.6e   |   %s %4.1d   |  %4.1d   \n", iter,tau,info,BC_idx,getNFX( ) );
-				myPrintf( myPrintfString );
-			}
-			break;
-
-		default:
-			/* nothing to display */
-			break;
-	}
-
-	#endif /* __XPCTARGET__ */
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/SQProblem.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/SQProblem.cpp
deleted file mode 100644
index 8860f7dda34..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/SQProblem.cpp
+++ /dev/null
@@ -1,553 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file src/SQProblem.cpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of the SQProblem class which is able to use the newly
- *	developed online active set strategy for parametric quadratic programming
- *	with varying matrices.
- */
-
-
-#include <qpOASES/SQProblem.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	S Q P r o b l e m
- */
-SQProblem::SQProblem( ) : QProblem( )
-{
-}
-
-
-/*
- *	S Q P r o b l e m
- */
-SQProblem::SQProblem( int _nV, int _nC, HessianType _hessianType ) : QProblem( _nV,_nC,_hessianType )
-{
-}
-
-
-/*
- *	S Q P r o b l e m
- */
-SQProblem::SQProblem( const SQProblem& rhs ) : QProblem( rhs )
-{
-}
-
-
-/*
- *	~ S Q P r o b l e m
- */
-SQProblem::~SQProblem( )
-{
-}
-
-
-/*
- *	o p e r a t o r =
- */
-SQProblem& SQProblem::operator=( const SQProblem& rhs )
-{
-	if ( this != &rhs )
-	{
-		QProblem::operator=( rhs );
-	}
-
-	return *this;
-}
-
-
-/*
- *	h o t s t a r t
- */
-returnValue SQProblem::hotstart(	const real_t* const H_new, const real_t* const g_new, const real_t* const A_new,
-									const real_t* const lb_new, const real_t* const ub_new,
-									const real_t* const lbA_new, const real_t* const ubA_new,
-									int& nWSR, real_t* const cputime )
-{
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
-		 ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
-		 ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
-	{
-		return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
-	}
-
-	/* start runtime measurement */
-	real_t starttime = 0.0;
-    if ( cputime != 0 )
-        starttime = getCPUtime( );
-
-
-	/* I) UPDATE QP MATRICES AND VECTORS */
-	if ( setupAuxiliaryQP( H_new,A_new,lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-
-	/* II) PERFORM USUAL HOMOTOPY */
-
-	/* Allow only remaining CPU time for usual hotstart. */
-	if ( cputime != 0 )
-		*cputime -= getCPUtime( ) - starttime;
-
-	returnValue returnvalue = QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
-
-
-	/* stop runtime measurement */
-	if ( cputime != 0 )
-		*cputime = getCPUtime( ) - starttime;
-
-	return returnvalue;
-}
-
-
-/*
- *	h o t s t a r t
- */
-returnValue SQProblem::hotstart(	const char* const H_file, const char* const g_file, const char* const A_file,
-									const char* const lb_file, const char* const ub_file,
-									const char* const lbA_file, const char* const ubA_file,
-									int& nWSR, real_t* const cputime
-									)
-{
-	int nV = getNV( );
-	int nC = getNC( );
-
-	returnValue returnvalue;
-
-	/* consistency checks */
-	if ( ( H_file == 0 ) || ( g_file == 0 ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	if ( ( nC > 0 ) && ( A_file == 0 ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-
-	/* 1) Load new QP matrices from files. */
-	real_t* H_new  = new real_t[nV*nV];
-	real_t* A_new  = new real_t[nC*nV];
-
-	if ( readFromFile( H_new, nV,nV, H_file ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_UNABLE_TO_READ_FILE );
-
-	if ( readFromFile( A_new, nC,nV, A_file ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_UNABLE_TO_READ_FILE );
-
-	/* 2) Load new QP vectors from files. */
-	real_t* g_new  = new real_t[nV];
-	real_t* lb_new = 0;
-	real_t* ub_new = 0;
-	real_t* lbA_new = 0;
-	real_t* ubA_new = 0;
-
-	if ( lb_file != 0 )
-		lb_new = new real_t[nV];
-	if ( ub_file != 0 )
-		ub_new = new real_t[nV];
-	if ( lbA_file != 0 )
-		lbA_new = new real_t[nC];
-	if ( ubA_file != 0 )
-		ubA_new = new real_t[nC];
-
-	returnvalue = loadQPvectorsFromFile(	g_file,lb_file,ub_file,lbA_file,ubA_file,
-											g_new,lb_new,ub_new,lbA_new,ubA_new
-											);
-	if ( returnvalue != SUCCESSFUL_RETURN )
-	{
-		if ( ubA_file != 0 )
-			delete[] ubA_new;
-		if ( lbA_file != 0 )
-			delete[] lbA_new;
-		if ( ub_file != 0 )
-			delete[] ub_new;
-		if ( lb_file != 0 )
-			delete[] lb_new;
-		delete[] g_new;
-		delete[] A_new;
-		delete[] H_new;
-
-		return THROWERROR( RET_UNABLE_TO_READ_FILE );
-	}
-
-	/* 3) Actually perform hotstart. */
-	returnvalue = hotstart(	H_new,g_new,A_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
-
-	if ( ubA_file != 0 )
-		delete[] ubA_new;
-	if ( lbA_file != 0 )
-		delete[] lbA_new;
-	if ( ub_file != 0 )
-		delete[] ub_new;
-	if ( lb_file != 0 )
-		delete[] lb_new;
-	delete[] g_new;
-	delete[] A_new;
-	delete[] H_new;
-
-	return returnvalue;
-}
-
-/*
- * h o t s t a r t
- */
-returnValue SQProblem::hotstart(	SymmetricMatrix *H_new,
-									const real_t* const g_new,
-									Matrix *A_new,
-									const real_t* const lb_new,
-									const real_t* const ub_new,
-									const real_t* const lbA_new,
-									const real_t* const ubA_new,
-									int& nWSR,
-									real_t* const cputime
-									)
-{
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
-		 ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
-		 ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
-	{
-		return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
-	}
-
-	/* start runtime measurement */
-	real_t starttime = 0.0;
-    if ( cputime != 0 )
-        starttime = getCPUtime( );
-
-
-	/* I) UPDATE QP MATRICES AND VECTORS */
-	if ( setupAuxiliaryQP( H_new,A_new,lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-
-	/* II) PERFORM USUAL HOMOTOPY */
-
-	/* Allow only remaining CPU time for usual hotstart. */
-	if ( cputime != 0 )
-		*cputime -= getCPUtime( ) - starttime;
-
-	returnValue returnvalue = QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
-
-
-	/* stop runtime measurement */
-	if ( cputime != 0 )
-		*cputime = getCPUtime( ) - starttime;
-
-	return returnvalue;
-}
-
-
-/*
- *	h o t s t a r t
- */
-returnValue SQProblem::hotstart(	const real_t* const g_new,
-									const real_t* const lb_new, const real_t* const ub_new,
-									const real_t* const lbA_new, const real_t* const ubA_new,
-									int& nWSR, real_t* const cputime
-									)
-{
-	/* Call to hotstart function for fixed QP matrices. */
-	return QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
-}
-
-
-/*
- *	h o t s t a r t
- */
-returnValue SQProblem::hotstart(	const char* const g_file,
-									const char* const lb_file, const char* const ub_file,
-									const char* const lbA_file, const char* const ubA_file,
-									int& nWSR, real_t* const cputime
-									)
-{
-	/* Call to hotstart function for fixed QP matrices. */
-	return QProblem::hotstart( g_file,lb_file,ub_file,lbA_file,ubA_file, nWSR,cputime );
-}
-
-
-/*
- *	h o t s t a r t
- */
-returnValue SQProblem::hotstart(	const real_t* const g_new,
-									const real_t* const lb_new, const real_t* const ub_new,
-									const real_t* const lbA_new, const real_t* const ubA_new,
-									int& nWSR, real_t* const cputime,
-									const Bounds* const guessedBounds, const Constraints* const guessedConstraints
-									)
-{
-	/* Call to hotstart function for fixed QP matrices. */
-	return QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime, guessedBounds,guessedConstraints );
-}
-
-
-/*
- *	h o t s t a r t
- */
-returnValue SQProblem::hotstart(	const char* const g_file,
-									const char* const lb_file, const char* const ub_file,
-									const char* const lbA_file, const char* const ubA_file,
-									int& nWSR, real_t* const cputime,
-									const Bounds* const guessedBounds, const Constraints* const guessedConstraints
-									)
-{
-	/* Call to hotstart function for fixed QP matrices. */
-	return QProblem::hotstart( g_file,lb_file,ub_file,lbA_file,ubA_file, nWSR,cputime, guessedBounds,guessedConstraints );
-}
-
-
-
-#ifdef __MATLAB__
-returnValue SQProblem::resetMatrixPointers( )
-{
-	H = 0;
-	A = 0;
-
-	return SUCCESSFUL_RETURN;
-}
-#endif
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-/*
- *	s e t u p A u x i l i a r y Q P
- */
-returnValue SQProblem::setupAuxiliaryQP( const real_t* const H_new, const real_t* const A_new,
-    const real_t *lb_new, const real_t *ub_new, const real_t *lbA_new, const real_t *ubA_new )
-{
-	int nV = getNV( );
-	int nC = getNC( );
-
-	DenseMatrix *dA = new DenseMatrix(nC, nV, nV, (real_t*) A_new);
-	SymDenseMat *sH = new SymDenseMat(nV, nV, nV, (real_t*) H_new);
-
-	returnValue returnvalue = setupAuxiliaryQP ( sH, dA, lb_new, ub_new, lbA_new, ubA_new );
-
-	if ( H_new != 0 )
-		freeHessian = BT_TRUE;
-	freeConstraintMatrix = BT_TRUE;
-
-	return returnvalue;
-}
-
-
-/*
- *	s e t u p A u x i l i a r y Q P
- */
-returnValue SQProblem::setupAuxiliaryQP ( SymmetricMatrix *H_new, Matrix *A_new,
-    const real_t *lb_new, const real_t *ub_new, const real_t *lbA_new, const real_t *ubA_new
-)
-{
-
-	int i;
-	int nV = getNV( );
-	int nC = getNC( );
-	returnValue returnvalue;
-
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
-		 ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
-		 ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
-	{
-		return THROWERROR( RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED );
-	}
-
-	status = QPS_PREPARINGAUXILIARYQP;
-
-
-	/* I) SETUP NEW QP MATRICES AND VECTORS: */
-	/* 1) Shift constraints' bounds vectors by (A_new - A)'*x_opt to ensure
-	 *    that old optimal solution remains feasible for new QP data. */
-	/*    Firstly, shift by -A'*x_opt and ... */
-	if ( nC > 0 )
-	{
-		if ( A_new == 0 )
-			return THROWERROR( RET_INVALID_ARGUMENTS );
-
-		for ( i=0; i<nC; ++i )
-		{
-			lbA[i] = -Ax_l[i];
-			ubA[i] =  Ax_u[i];
-		}
-
-		/* Set constraint matrix as well as ... */
-		setA( A_new );
-
-		/* ... secondly, shift by +A_new'*x_opt. */
-		for ( i=0; i<nC; ++i )
-		{
-			lbA[i] += Ax[i];
-			ubA[i] += Ax[i];
-		}
-
-		/* update constraint products. */
-		for ( i=0; i<nC; ++i )
-		{
-			Ax_u[i] = ubA[i] - Ax[i];
-			Ax_l[i] = Ax[i] - lbA[i];
-		}
-	}
-
-	/* 2) Set new Hessian matrix, determine Hessian type and
-	 *    regularise new Hessian matrix if necessary. */
-	/* a) Setup new Hessian matrix and determine its type. */
-	if ( H_new != 0 )
-	{
-		setH( H_new );
-
-		hessianType = HST_UNKNOWN;
-		if ( determineHessianType( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-		/* b) Regularise new Hessian if necessary. */
-		if ( ( hessianType == HST_ZERO ) ||
-			 ( hessianType == HST_SEMIDEF ) ||
-			 ( usingRegularisation( ) == BT_TRUE ) )
-		{
-			regVal = 0.0; /* reset previous regularisation */
-
-			if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-		}
-	}
-	else
-	{
-		if ( H != 0 )
-			return THROWERROR( RET_NO_HESSIAN_SPECIFIED );
-	}
-
-	/* 3) Setup QP gradient. */
-	if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-
-	/* II) SETUP WORKING SETS AND MATRIX FACTORISATIONS: */
-	/* 1) Make a copy of current bounds/constraints ... */
-	Bounds      oldBounds      = bounds;
-	Constraints oldConstraints = constraints;
-
-    /* we're trying to find an active set with positive definite null
-     * space Hessian twice:
-     * - first for the current active set including all equalities
-     * - second after moving all inactive variables to a bound
-     *   (depending on Options). This creates an empty null space and
-     *   is guaranteed to succeed. Thus this loop will exit after n_try=1.
-     */
-    int n_try;
-    for (n_try = 0; n_try < 2; ++n_try) {
-
-        if (n_try > 0) {
-            // the current active set leaves an indefinite null space Hessian
-            // move all inactive variables to a bound, creating an empty null space
-            for (int ii = 0; ii < nV; ++ii)
-                if (oldBounds.getStatus (ii) == ST_INACTIVE)
-                    oldBounds.setStatus (ii, options.initialStatusBounds);
-        }
-
-        /*    ... reset them ... */
-        bounds.init( nV );
-        constraints.init( nC );
-
-        /*    ... and set them up afresh. */
-        if ( setupSubjectToType(lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
-            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-        if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
-            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-        if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
-            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-        /* 2) Setup TQ factorisation. */
-        if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
-            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-		// check for equalities that have become bounds ...
-		for (int ii = 0; ii < nC; ++ii) {
-			if (oldConstraints.getType (ii) == ST_EQUALITY && constraints.getType (ii) == ST_BOUNDED) {
-				if (oldConstraints.getStatus (ii) == ST_LOWER && y[nV+ii] < 0.0)
-					oldConstraints.setStatus (ii, ST_UPPER);
-				else if (oldConstraints.getStatus (ii) == ST_UPPER && y[nV+ii] > 0.0)
-					oldConstraints.setStatus (ii, ST_LOWER);
-			}
-		}
-
-		// ... and do the same also for the bounds!
-		for (int ii = 0; ii < nV; ++ii) {
-			if (oldBounds.getType(ii) == ST_EQUALITY
-					&& bounds.getType(ii) == ST_BOUNDED) {
-				if (oldBounds.getStatus(ii) == ST_LOWER && y[ii] < 0.0)
-					oldBounds.setStatus(ii, ST_UPPER);
-				else if (oldBounds.getStatus(ii) == ST_UPPER && y[ii] > 0.0)
-					oldBounds.setStatus(ii, ST_LOWER);
-			}
-		}
-
-        /* 3) Setup old working sets afresh (updating TQ factorisation). */
-        if ( setupAuxiliaryWorkingSet( &oldBounds,&oldConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
-            return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
-
-        /* Factorise projected Hessian
-        * this now handles all special cases (no active bounds/constraints, no nullspace) */
-        returnvalue = computeProjectedCholesky( );
-
-        /* leave the loop if decomposition was successful, i.e. we have
-         * found an active set with positive definite null space Hessian */
-        if ( returnvalue == SUCCESSFUL_RETURN )
-            break;
-    }
-
-    /* adjust lb/ub if we changed the old active set in the second try
-     */
-    if (n_try > 0) {
-		// as per setupAuxiliaryQPbounds assumptions ... oh the troubles
-		for (int ii = 0; ii < nC; ++ii)
-			Ax_l[ii] = Ax_u[ii] = Ax[ii];
-        setupAuxiliaryQPbounds (&bounds, &constraints, BT_FALSE);
-	}
-
-	status = QPS_AUXILIARYQPSOLVED;
-
-	return SUCCESSFUL_RETURN;
-}
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/SolutionAnalysis.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/SolutionAnalysis.cpp
deleted file mode 100644
index d6b766c3db6..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/SolutionAnalysis.cpp
+++ /dev/null
@@ -1,567 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file src/SolutionAnalysis.cpp
- *	\author Hans Joachim Ferreau (thanks to Boris Houska)
- *	\version 3.0
- *	\date 2008-2014
- *
- *	Implementation of the SolutionAnalysis class designed to perform
- *	additional analysis after solving a QP with qpOASES.
- *
- */
-
-
-#include <qpOASES/extras/SolutionAnalysis.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	S o l u t i o n A n a l y s i s
- */
-SolutionAnalysis::SolutionAnalysis( )
-{
-
-}
-
-
-/*
- *	S o l u t i o n A n a l y s i s
- */
-SolutionAnalysis::SolutionAnalysis( const SolutionAnalysis& rhs )
-{
-
-}
-
-
-/*
- *	~ S o l u t i o n A n a l y s i s
- */
-SolutionAnalysis::~SolutionAnalysis( )
-{
-
-}
-
-
-/*
- *	o p e r a t o r =
- */
-SolutionAnalysis& SolutionAnalysis::operator=( const SolutionAnalysis& rhs )
-{
-	if ( this != &rhs )
-	{
-
-	}
-
-	return *this;
-}
-
-
-
-/*
- *	g e t M a x K K T v i o l a t i o n
- */
-returnValue SolutionAnalysis::getMaxKKTviolation( QProblemB* qp, real_t& maxKKTviolation ) const
-{
-	int i;
-	int nV = qp->getNV( );
-
-	real_t *tmp = new real_t[nV];
-	maxKKTviolation = 0.0;
-
-
-	/* 1) Check for Hx + g - y*A' = 0  (here: A = Id). */
-	for( i=0; i<nV; ++i )
-		tmp[i] = qp->g[i];
-
-	switch ( qp->getHessianType( ) )
-	{
-		case HST_ZERO:
-			/*tmp += qp->eps * qp->x[i]; */
-			break;
-
-		case HST_IDENTITY:
-			for( i=0; i<nV; ++i )
-				tmp[i] += qp->x[i];
-			break;
-
-		default:
-			qp->H->times(1, 1.0, qp->x, nV, 1.0, tmp, nV);
-			break;
-	}
-
-	for( i=0; i<nV; ++i )
-	{
-		tmp[i] -= qp->y[i];
-
-		if ( getAbs( tmp[i] ) > maxKKTviolation )
-			maxKKTviolation = getAbs( tmp[i] );
-	}
-	delete[] tmp;
-
-	/* 2) Check for lb <= x <= ub. */
-	for( i=0; i<nV; ++i )
-	{
-		if ( qp->lb[i] - qp->x[i] > maxKKTviolation )
-			maxKKTviolation = qp->lb[i] - qp->x[i];
-
-		if ( qp->x[i] - qp->ub[i] > maxKKTviolation )
-			maxKKTviolation = qp->x[i] - qp->ub[i];
-	}
-
-	/* 3) Check for correct sign of y and for complementary slackness. */
-	for( i=0; i<nV; ++i )
-	{
-		switch ( qp->bounds.getStatus( i ) )
-		{
-			case ST_LOWER:
-				if ( -qp->y[i] > maxKKTviolation )
-					maxKKTviolation = -qp->y[i];
-				if ( getAbs( ( qp->x[i] - qp->lb[i] ) * qp->y[i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( ( qp->x[i] - qp->lb[i] ) * qp->y[i] );
-				break;
-
-			case ST_UPPER:
-				if ( qp->y[i] > maxKKTviolation )
-					maxKKTviolation = qp->y[i];
-				if ( getAbs( ( qp->ub[i] - qp->x[i] ) * qp->y[i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( ( qp->ub[i] - qp->x[i] ) * qp->y[i] );
-				break;
-
-			default: /* inactive */
-			if ( getAbs( qp->y[i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( qp->y[i] );
-				break;
-		}
-	}
-
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t M a x K K T v i o l a t i o n
- */
-returnValue SolutionAnalysis::getMaxKKTviolation( QProblem* qp, real_t& maxKKTviolation ) const
-{
-	int i;
-	int nV = qp->getNV( );
-	int nC = qp->getNC( );
-
-	real_t *tmp = new real_t[nV];
-	maxKKTviolation = 0.0;
-
-
-	/* 1) check for Hx + g - [yFX yAC]*[Id A]' = 0. */
-	for( i=0; i<nV; ++i )
-		tmp[i] = qp->g[i];
-
-	switch ( qp->getHessianType( ) )
-	{
-		case HST_ZERO:
-				/*tmp += qp->eps * qp->x[i]; */
-			break;
-
-		case HST_IDENTITY:
-			for( i=0; i<nV; ++i )
-				tmp[i] += qp->x[i];
-			break;
-
-		default:
-			qp->H->times(1, 1.0, qp->x, nV, 1.0, tmp, nV);
-			break;
-	}
-
-	qp->A->transTimes(1, -1.0, qp->y + nV, nC, 1.0, tmp, nV);
-
-	for( i=0; i<nV; ++i )
-	{
-		tmp[i] -= qp->y[i];
-
-		if ( getAbs( tmp[i] ) > maxKKTviolation )
-			maxKKTviolation = getAbs( tmp[i] );
-	}
-
-	/* 2) Check for [lb lbA] <= [Id A]*x <= [ub ubA]. */
-	/* lbA <= Ax <= ubA */
-	real_t* Ax = new real_t[nC];
-	qp->A->times(1, 1.0, qp->x, nV, 0.0, Ax, nC);
-
-	for( i=0; i<nC; ++i )
-	{
-		if ( qp->lbA[i] - Ax[i] > maxKKTviolation )
-			maxKKTviolation = qp->lbA[i] - Ax[i];
-
-		if ( Ax[i] - qp->ubA[i] > maxKKTviolation )
-			maxKKTviolation = Ax[i] - qp->ubA[i];
-	}
-
-	/* lb <= x <= ub */
-	for( i=0; i<nV; ++i )
-	{
-		if ( qp->lb[i] - qp->x[i] > maxKKTviolation )
-			maxKKTviolation = qp->lb[i] - qp->x[i];
-
-		if ( qp->x[i] - qp->ub[i] > maxKKTviolation )
-			maxKKTviolation = qp->x[i] - qp->ub[i];
-	}
-
-	/* 3) Check for correct sign of y and for complementary slackness. */
-	/* bounds */
-	for( i=0; i<nV; ++i )
-	{
-		switch ( qp->bounds.getStatus( i ) )
-		{
-			case ST_LOWER:
-				if ( -qp->y[i] > maxKKTviolation )
-					maxKKTviolation = -qp->y[i];
-				if ( getAbs( ( qp->x[i] - qp->lb[i] ) * qp->y[i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( ( qp->x[i] - qp->lb[i] ) * qp->y[i] );
-				break;
-
-			case ST_UPPER:
-				if ( qp->y[i] > maxKKTviolation )
-					maxKKTviolation = qp->y[i];
-				if ( getAbs( ( qp->ub[i] - qp->x[i] ) * qp->y[i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( ( qp->ub[i] - qp->x[i] ) * qp->y[i] );
-				break;
-
-			default: /* inactive */
-			if ( getAbs( qp->y[i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( qp->y[i] );
-				break;
-		}
-	}
-
-	/* constraints */
-	for( i=0; i<nC; ++i )
-	{
-		switch ( qp->constraints.getStatus( i ) )
-		{
-			case ST_LOWER:
-				if ( -qp->y[nV+i] > maxKKTviolation )
-					maxKKTviolation = -qp->y[nV+i];
-				if ( getAbs( ( Ax[i] - qp->lbA[i] ) * qp->y[nV+i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( ( Ax[i] - qp->lbA[i] ) * qp->y[nV+i] );
-				break;
-
-			case ST_UPPER:
-				if ( qp->y[nV+i] > maxKKTviolation )
-					maxKKTviolation = qp->y[nV+i];
-				if ( getAbs( ( qp->ubA[i] - Ax[i] ) * qp->y[nV+i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( ( qp->ubA[i] - Ax[i] ) * qp->y[nV+i] );
-				break;
-
-			default: /* inactive */
-			if ( getAbs( qp->y[nV+i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( qp->y[nV+i] );
-				break;
-		}
-	}
-
-	delete[] tmp;
-	delete[] Ax;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t M a x K K T v i o l a t i o n
- */
-returnValue SolutionAnalysis::getMaxKKTviolation( SQProblem* qp, real_t& maxKKTviolation ) const
-{
-	return getMaxKKTviolation( (QProblem*)qp,maxKKTviolation );
-}
-
-
-
-/*
- *	g e t V a r i a n c e C o v a r i a n c e
- */
-returnValue SolutionAnalysis::getVarianceCovariance( QProblemB* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR ) const
-{
-	return THROWERROR( RET_NOT_YET_IMPLEMENTED );
-}
-
-
-/*
- *	g e t V a r i a n c e C o v a r i a n c e
- */
-returnValue SolutionAnalysis::getVarianceCovariance( QProblem* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR ) const
-{
-
-  /* DEFINITION OF THE DIMENSIONS nV AND nC:
-   * --------------------------------------- */
-  int nV  = qp->getNV( );                      /* dimension of x / the bounds */
-  int nC  = qp->getNC( );                      /* dimension of the constraints */
-  int dim = 2*nV+nC;                           /* dimension of input and output */
-                                               /* variance-covariance matrix */
-  int run1, run2, run3;                        /* simple run variables (for loops). */
-
-
-  /* ALLOCATION OF MEMORY:
-   * --------------------- */
-  real_t* delta_g_cov    = new real_t[nV];     /* a covariance-vector of g */
-  real_t* delta_lb_cov   = new real_t[nV];     /* a covariance-vector of lb */
-  real_t* delta_ub_cov   = new real_t[nV];     /* a covariance-vector of ub */
-  real_t* delta_lbA_cov  = new real_t[nC];     /* a covariance-vector of lbA */
-  real_t* delta_ubA_cov  = new real_t[nC];     /* a covariance-vector of ubA */
-
-  returnValue returnvalue;                     /* the return value */
-  BooleanType Delta_bC_isZero = BT_FALSE;      /* (just use FALSE here) */
-  BooleanType Delta_bB_isZero = BT_FALSE;      /* (just use FALSE here) */
-
-
-
-  /* ASK FOR THE NUMBER OF FREE AND FIXED VARIABLES:
-   * (ASSUMES THAT ACTIVE SET IS CONSTANT FOR THE
-   *  VARIANCE-COVARIANCE EVALUATION)
-   * ----------------------------------------------- */
-  int nFR, nFX, nAC;
-
-  nFR = qp->getNFR( );
-  nFX = qp->getNFX( );
-  nAC = qp->getNAC( );
-
-
-  /* ASK FOR THE CORRESPONDING INDEX ARRAYS:
-   * --------------------------------------- */
-  int *FR_idx, *FX_idx, *AC_idx;
-
-  if ( qp->bounds.getFree( )->getNumberArray( &FR_idx ) != SUCCESSFUL_RETURN )
-       return THROWERROR( RET_HOTSTART_FAILED );
-
-  if ( qp->bounds.getFixed( )->getNumberArray( &FX_idx ) != SUCCESSFUL_RETURN )
-       return THROWERROR( RET_HOTSTART_FAILED );
-
-  if ( qp->constraints.getActive( )->getNumberArray( &AC_idx ) != SUCCESSFUL_RETURN )
-       return THROWERROR( RET_HOTSTART_FAILED );
-
-
-
-  /* INTRODUCE VARIABLES TO MEASURE THE REACTION OF THE QP-SOLUTION TO
-   * THE VARIANCE-COVARIANCE DISTURBANCE:
-   * ----------------------------------------------------------------- */
-  real_t *delta_xFR = new real_t[nFR];
-  real_t *delta_xFX = new real_t[nFX];
-  real_t *delta_yAC = new real_t[nAC];
-  real_t *delta_yFX = new real_t[nFX];
-
-  real_t* K             = new real_t[dim*dim];  /* matrix to store */
-                                                /* an intermediate */
-                                                /* result. */
-
-  /* SOME INITIALIZATIONS:
-   * --------------------- */
-  for( run1 = 0; run1 < dim*dim; run1++ ){
-    K              [run1] = 0.0;
-    Primal_Dual_VAR[run1] = 0.0;
-  }
-
-
-  /* ================================================================= */
-
-  /* FIRST MATRIX MULTIPLICATION (OBTAINS THE INTERMEDIATE RESULT
-   *  K := [ ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * g_b_bA_VAR ]^T )
-   * THE EVALUATION OF THE INVERSE OF THE KKT-MATRIX OF THE QP
-   * WITH RESPECT TO THE CURRENT ACTIVE SET
-   * USES THE EXISTING CHOLESKY AND TQ-DECOMPOSITIONS. FOR DETAILS
-   * cf. THE (protected) FUNCTION determineStepDirection. */
-
-  for( run3 = 0; run3 < dim; run3++ ){
-
-
-    for( run1 = 0; run1 < nV; run1++ ){
-      delta_g_cov  [run1]   = g_b_bA_VAR[run3*dim+run1];
-      delta_lb_cov [run1]   = g_b_bA_VAR[run3*dim+nV+run1];         /*  LINE-WISE LOADING OF THE INPUT */
-      delta_ub_cov [run1]   = g_b_bA_VAR[run3*dim+nV+run1];         /*  VARIANCE-COVARIANCE            */
-    }
-    for( run1 = 0; run1 < nC; run1++ ){
-      delta_lbA_cov [run1]  = g_b_bA_VAR[run3*dim+2*nV+run1];
-      delta_ubA_cov [run1]  = g_b_bA_VAR[run3*dim+2*nV+run1];
-    }
-
-
-    /* EVALUATION OF THE STEP:
-     * ------------------------------------------------------------------------------ */
-
-    returnvalue = qp->determineStepDirection( delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
-                                              Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
-                                              delta_yAC,delta_yFX );
-
-    /* ------------------------------------------------------------------------------ */
-
-
-    /* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
-     * ------------------------------------------------------ */
-    if ( returnvalue != SUCCESSFUL_RETURN ){
-
-      delete[] delta_g_cov;
-      delete[] delta_lb_cov;
-      delete[] delta_ub_cov;
-      delete[] delta_lbA_cov;
-      delete[] delta_ubA_cov;
-      delete[] delta_xFR;
-      delete[] delta_xFX;
-      delete[] delta_yAC;
-      delete[] delta_yFX;
-      delete[] K;
-
-      THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
-      return returnvalue;
-    }
-
-
-
-    for( run1=0; run1<nFR; run1++ ){
-      run2                  = FR_idx[run1];
-      K[run3*dim+run2]      = delta_xFR[run1];
-    }                                                               /*  LINE WISE                  */
-    for( run1=0; run1<nFX; run1++ ){                                /*  STORAGE OF THE QP-REACTION */
-      run2                  = FX_idx[run1];                         /*  (uses the index list)      */
-      K[run3*dim+run2]      = delta_xFX[run1];
-      K[run3*dim+nV+run2]   = delta_yFX[run1];
-    }
-    for( run1=0; run1<nAC; run1++ ){
-      run2                  = AC_idx[run1];
-      K[run3*dim+2*nV+run2] = delta_yAC[run1];
-    }
-
-  }
-
-
-  /* ================================================================= */
-
-  /* SECOND MATRIX MULTIPLICATION (OBTAINS THE FINAL RESULT
-   * Primal_Dual_VAR := ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * K )
-   * THE APPLICATION OF THE KKT-INVERSE IS AGAIN REALIZED
-   * BY USING THE PROTECTED FUNCTION
-   * determineStepDirection */
-
-  for( run3 = 0; run3 < dim; run3++ ){
-
-    for( run1 = 0; run1 < nV; run1++ ){
-      delta_g_cov  [run1]   = K[run3+     run1*dim];
-      delta_lb_cov [run1]   = K[run3+(nV+run1)*dim];                /*  ROW WISE LOADING OF THE */
-      delta_ub_cov [run1]   = K[run3+(nV+run1)*dim];                /*  INTERMEDIATE RESULT K   */
-    }
-    for( run1 = 0; run1 < nC; run1++ ){
-      delta_lbA_cov [run1]  = K[run3+(2*nV+run1)*dim];
-      delta_ubA_cov [run1]  = K[run3+(2*nV+run1)*dim];
-    }
-
-
-    /* EVALUATION OF THE STEP:
-     * ------------------------------------------------------------------------------ */
-
-    returnvalue = qp->determineStepDirection( delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
-                                              Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
-                                              delta_yAC,delta_yFX);
-
-
-    /* ------------------------------------------------------------------------------ */
-
-
-    /* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
-     * ------------------------------------------------------ */
-    if ( returnvalue != SUCCESSFUL_RETURN ){
-
-      delete[] delta_g_cov;
-      delete[] delta_lb_cov;
-      delete[] delta_ub_cov;
-      delete[] delta_lbA_cov;
-      delete[] delta_ubA_cov;
-      delete[] delta_xFR;
-      delete[] delta_xFX;
-      delete[] delta_yAC;
-      delete[] delta_yFX;
-      delete[] K;
-
-      THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
-      return returnvalue;
-    }
-
-
-
-    for( run1=0; run1<nFR; run1++ ){
-      run2                                = FR_idx[run1];
-      Primal_Dual_VAR[run3+run2*dim]      = delta_xFR[run1];
-    }
-    for( run1=0; run1<nFX; run1++ ){                                 /*  ROW-WISE STORAGE */
-      run2                  = FX_idx[run1];                          /*  OF THE RESULT.   */
-      Primal_Dual_VAR[run3+run2*dim     ]   = delta_xFX[run1];
-      Primal_Dual_VAR[run3+(nV+run2)*dim]   = delta_yFX[run1];
-    }
-    for( run1=0; run1<nAC; run1++ ){
-      run2                                  = AC_idx[run1];
-      Primal_Dual_VAR[run3+(2*nV+run2)*dim] = delta_yAC[run1];
-    }
-
-  }
-
-
-  /* DEALOCATE MEMORY:
-   * ----------------- */
-
-  delete[] delta_g_cov;
-  delete[] delta_lb_cov;
-  delete[] delta_ub_cov;
-  delete[] delta_lbA_cov;
-  delete[] delta_ubA_cov;
-  delete[] delta_xFR;
-  delete[] delta_xFX;
-  delete[] delta_yAC;
-  delete[] delta_yFX;
-  delete[] K;
-
-  return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t V a r i a n c e C o v a r i a n c e
- */
-returnValue SolutionAnalysis::getVarianceCovariance( SQProblem* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR ) const
-{
-	/* Call QProblem variant. */
-	return getVarianceCovariance( (QProblem*)qp,g_b_bA_VAR,Primal_Dual_VAR );
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/SubjectTo.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/SubjectTo.cpp
deleted file mode 100644
index f135769655b..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/SubjectTo.cpp
+++ /dev/null
@@ -1,289 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file src/SubjectTo.cpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of the SubjectTo class designed to manage working sets of
- *	constraints and bounds within a QProblem.
- */
-
-
-#include <qpOASES/SubjectTo.hpp>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	S u b j e c t T o
- */
-SubjectTo::SubjectTo( )
-{
-	type   = 0;
-	status = 0;
-
-	init( );
-}
-
-
-/*
- *	S u b j e c t T o
- */
-SubjectTo::SubjectTo( int _n )
-{
-	type   = 0;
-	status = 0;
-
-	init( _n );
-}
-
-
-/*
- *	S u b j e c t T o
- */
-SubjectTo::SubjectTo( const SubjectTo& rhs )
-{
-	copy( rhs );
-}
-
-
-/*
- *	~ S u b j e c t T o
- */
-SubjectTo::~SubjectTo( )
-{
-	clear( );
-}
-
-
-/*
- *	o p e r a t o r =
- */
-SubjectTo& SubjectTo::operator=( const SubjectTo& rhs )
-{
-	if ( this != &rhs )
-	{
-		clear( );
-		copy( rhs );
-	}
-
-	return *this;
-}
-
-
-/*
- *	i n i t
- */
-returnValue SubjectTo::init(	int _n
-								)
-{
-	int i;
-
-	if ( _n < 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	clear( );
-
-	n = _n;
-	noLower = BT_TRUE;
-	noUpper = BT_TRUE;
-
-	if ( n > 0 )
-	{
-		type   = new SubjectToType[n];
-		status = new SubjectToStatus[n];
-
-		for( i=0; i<n; ++i )
-		{
-			type[i]   = ST_UNKNOWN;
-			status[i] = ST_UNDEFINED;
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-/*
- *	c l e a r
- */
-returnValue SubjectTo::clear( )
-{
-	if ( type != 0 )
-	{
-		delete[] type;
-		type = 0;
-	}
-
-	if ( status != 0 )
-	{
-		delete[] status;
-		status = 0;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	c o p y
- */
-returnValue SubjectTo::copy(	const SubjectTo& rhs
-								)
-{
-	int i;
-
-	n = rhs.n;
-	noLower = rhs.noLower;
-	noUpper = rhs.noUpper;
-
-	if ( rhs.n != 0 )
-	{
-		type   = new SubjectToType[n];
-		status = new SubjectToStatus[n];
-
-		for( i=0; i<n; ++i )
-		{
-			type[i]   = rhs.type[i];
-			status[i] = rhs.status[i];
-		}
-	}
-	else
-	{
-		type   = 0;
-		status = 0;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	a d d I n d e x
- */
-returnValue SubjectTo::addIndex(	Indexlist* const indexlist,
-									int newnumber, SubjectToStatus newstatus
-									)
-{
-	if ( status != 0 )
-	{
-		/* consistency check */
-		if ( status[newnumber] == newstatus )
-			return THROWERROR( RET_INDEX_ALREADY_OF_DESIRED_STATUS );
-
-		status[newnumber] = newstatus;
-	}
-	else
-		return THROWERROR( RET_ADDINDEX_FAILED );
-
-	if ( indexlist != 0 )
-	{
-		if ( indexlist->addNumber( newnumber ) == RET_INDEXLIST_EXCEEDS_MAX_LENGTH )
-			return THROWERROR( RET_ADDINDEX_FAILED );
-	}
-	else
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	r e m o v e I n d e x
- */
-returnValue SubjectTo::removeIndex(	Indexlist* const indexlist,
-									int removenumber
-									)
-{
-	if ( status != 0 )
-		status[removenumber] = ST_UNDEFINED;
-	else
-		return THROWERROR( RET_REMOVEINDEX_FAILED );
-
-	if ( indexlist != 0 )
-	{
-		if ( indexlist->removeNumber( removenumber ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_REMOVEINDEX_FAILED );
-	}
-	else
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s w a p I n d e x
- */
-returnValue SubjectTo::swapIndex(	Indexlist* const indexlist,
-									int number1, int number2
-									)
-{
-	/* consistency checks */
-	if ( status != 0 )
-	{
-		if ( status[number1] != status[number2] )
-			return THROWERROR( RET_SWAPINDEX_FAILED );
-	}
-	else
-		return THROWERROR( RET_SWAPINDEX_FAILED );
-
-	if ( number1 == number2 )
-	{
-		THROWWARNING( RET_NOTHING_TO_DO );
-		return SUCCESSFUL_RETURN;
-	}
-
-	if ( indexlist != 0 )
-	{
-		if ( indexlist->swapNumbers( number1,number2 ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_SWAPINDEX_FAILED );
-	}
-	else
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/Utils.cpp b/3rdparty/qpOASES/qpOASES-3.0/src/Utils.cpp
deleted file mode 100644
index a76bc7eaa34..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/Utils.cpp
+++ /dev/null
@@ -1,984 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file src/Utils.cpp
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches (thanks to Eckhard Arnold)
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Implementation of some utility functions for working with qpOASES.
- */
-
-
-#include <math.h>
-
-#if defined(__WIN32__) || defined(WIN32)
-  #include <windows.h>
-#elif defined(LINUX) || defined(__LINUX__)
-  #include <sys/stat.h>
-  #include <sys/time.h>
-#endif
-
-#ifdef __MATLAB__
-  #include "mex.h"
-#endif
-
-#ifdef __SCILAB__
-  #include <scilab/sciprint.h>
-#endif
-
-
-#include <qpOASES/Utils.hpp>
-
-
-#ifdef __NO_SNPRINTF__
-#if (!defined(_MSC_VER)) || defined(__DSPACE__)
-/* If snprintf is not available, provide an empty implementation. */
-int snprintf( char* s, size_t n, const char* format, ... )
-{
-	if ( n > 0 )
-		s[0] = '\0';
-
-	return 0;
-}
-#endif
-#endif /* __NO_SNPRINTF__ */
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/*
- *	p r i n t
- */
-returnValue print( const real_t* const v, int n, const char* name )
-{
-	#ifndef __SUPPRESSANYOUTPUT__
-	#ifndef __XPCTARGET__
-	int i;
-	char myPrintfString[MAX_STRING_LENGTH];
-
-	/* Print vector name. */
-	if ( name != 0 )
-	{
-		snprintf( myPrintfString,MAX_STRING_LENGTH,"%s = \n", name );
-		myPrintf( myPrintfString );
-	}
-
-	/* Print vector data. */
-	for( i=0; i<n; ++i )
-	{
-		snprintf( myPrintfString,MAX_STRING_LENGTH," %.16e\t", v[i] );
-		myPrintf( myPrintfString );
-	}
-	myPrintf( "\n" );
-	#endif /* __XPCTARGET__ */
-	#endif /* __SUPPRESSANYOUTPUT__ */
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t
- */
-returnValue print(	const real_t* const v, int n, const int* const V_idx, const char* name )
-{
-	#ifndef __SUPPRESSANYOUTPUT__
-	#ifndef __XPCTARGET__
-	int i;
-	char myPrintfString[MAX_STRING_LENGTH];
-
-	/* Print vector name. */
-	if ( name != 0 )
-	{
-		snprintf( myPrintfString,MAX_STRING_LENGTH,"%s = \n", name );
-		myPrintf( myPrintfString );
-	}
-
-	/* Print a permuted vector data. */
-	for( i=0; i<n; ++i )
-	{
-		snprintf( myPrintfString,MAX_STRING_LENGTH," %.16e\t", v[ V_idx[i] ] );
-		myPrintf( myPrintfString );
-	}
-	myPrintf( "\n" );
-	#endif /* __XPCTARGET__ */
-	#endif /* __SUPPRESSANYOUTPUT__ */
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t
- */
-returnValue print( const real_t* const M, int nrow, int ncol, const char* name )
-{
-	#ifndef __SUPPRESSANYOUTPUT__
-	#ifndef __XPCTARGET__
-	int i;
-	char myPrintfString[MAX_STRING_LENGTH];
-
-	/* Print matrix name. */
-	if ( name != 0 )
-	{
-		snprintf( myPrintfString,MAX_STRING_LENGTH,"%s = \n", name );
-		myPrintf( myPrintfString );
-	}
-
-	/* Print a matrix data as a collection of row vectors. */
-	for( i=0; i<nrow; ++i )
-		print( &(M[i*ncol]), ncol );
-	myPrintf( "\n" );
-	#endif /* __XPCTARGET__ */
-	#endif /* __SUPPRESSANYOUTPUT__ */
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t
- */
-returnValue print(	const real_t* const M, int nrow, int ncol, const int* const ROW_idx, const int* const COL_idx, const char* name )
-{
-	#ifndef __SUPPRESSANYOUTPUT__
-	#ifndef __XPCTARGET__
-	int i;
-	char myPrintfString[MAX_STRING_LENGTH];
-
-	/* Print matrix name. */
-	if ( name != 0 )
-	{
-		snprintf( myPrintfString,MAX_STRING_LENGTH,"%s = \n", name );
-		myPrintf( myPrintfString );
-	}
-
-	/* Print a permuted matrix data as a collection of permuted row vectors. */
-	for( i=0; i<nrow; ++i )
-		print( &( M[ ROW_idx[i]*ncol ] ), ncol, COL_idx );
-	myPrintf( "\n" );
-	#endif /* __XPCTARGET__ */
-	#endif /* __SUPPRESSANYOUTPUT__ */
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t
- */
-returnValue print( const int* const index, int n, const char* name )
-{
-	#ifndef __SUPPRESSANYOUTPUT__
-	#ifndef __XPCTARGET__
-	int i;
-	char myPrintfString[MAX_STRING_LENGTH];
-
-	/* Print indexlist name. */
-	if ( name != 0 )
-	{
-		snprintf( myPrintfString,MAX_STRING_LENGTH,"%s = \n", name );
-		myPrintf( myPrintfString );
-	}
-
-	/* Print a indexlist data. */
-	for( i=0; i<n; ++i )
-	{
-		snprintf( myPrintfString,MAX_STRING_LENGTH," %d\t", index[i] );
-		myPrintf( myPrintfString );
-	}
-	myPrintf( "\n" );
-	#endif /* __XPCTARGET__ */
-	#endif /* __SUPPRESSANYOUTPUT__ */
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	m y P r i n t f
- */
-returnValue myPrintf( const char* s )
-{
-	#ifndef __SUPPRESSANYOUTPUT__
-	#ifndef __XPCTARGET__
-	
-		if ( s == 0 )
-			return RET_INVALID_ARGUMENTS;
-		
-		#ifdef __MATLAB__
-			mexPrintf( s );
-		#else
-			#ifdef __SCILAB__
-				sciprint( s );
-			#else
-				FILE* outputfile = getGlobalMessageHandler( )->getOutputFile( );
-				if ( outputfile == 0 )
-					return THROWERROR( RET_NO_GLOBAL_MESSAGE_OUTPUTFILE );
-				fprintf( outputfile, "%s", s );
-			#endif /* __SCILAB__ */
-		#endif /* __MATLAB__ */
-
-	#endif /* __XPCTARGET__ */
-	#endif /* __SUPPRESSANYOUTPUT__ */
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t C o p y r i g h t N o t i c e
- */
-returnValue printCopyrightNotice( )
-{
-	#ifndef __SUPPRESSANYOUTPUT__
-		#ifndef __XPCTARGET__
-		#ifndef __DSPACE__
-		#ifndef __NO_COPYRIGHT__
-		myPrintf( "\nqpOASES -- An Implementation of the Online Active Set Strategy.\nCopyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,\nChristian Kirches et al. All rights reserved.\n\nqpOASES is distributed under the terms of the \nGNU Lesser General Public License 2.1 in the hope that it will be \nuseful, but WITHOUT ANY WARRANTY; without even the implied warranty \nof MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. \nSee the GNU Lesser General Public License for more details.\n\n" );
-		#endif /* __NO_COPYRIGHT__ */
-		#endif /* __DSPACE__ */
-		#endif /* __XPCTARGET__ */
-	#endif /* __SUPPRESSANYOUTPUT__ */
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	r e a d F r o m F i l e
- */
-returnValue readFromFile(	real_t* data, int nrow, int ncol,
-							const char* datafilename
-							)
-{
-	#ifndef __XPCTARGET__
-	int i, j;
-	double float_data;
-	FILE* datafile;
-
-	/* 1) Open file. */
-	if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
-	{
-		char errstr[MAX_STRING_LENGTH];
-		snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
-		return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-	}
-
-	/* 2) Read data from file. */
-	for( i=0; i<nrow; ++i )
-	{
-		for( j=0; j<ncol; ++j )
-		{
-			//#ifdef __USE_SINGLE_PRECISION__
-			//if ( fscanf( datafile, "%f ", &float_data ) == 0 )
-			//#else
-			if ( fscanf( datafile, "%lf ", &float_data ) == 0 )
-			//#endif /* __USE_SINGLE_PRECISION__ */
-			{
-				fclose( datafile );
-				char errstr[MAX_STRING_LENGTH];
-				snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
-				return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-			}
-			data[i*ncol + j] = ( (real_t) float_data );
-		}
-	}
-
-	/* 3) Close file. */
-	fclose( datafile );
-
-	return SUCCESSFUL_RETURN;
-	#else /* __XPCTARGET__ */
-
-	return RET_NOT_YET_IMPLEMENTED;
-
-	#endif /* __XPCTARGET__ */
-}
-
-
-/*
- *	r e a d F r o m F i l e
- */
-returnValue readFromFile(	real_t* data, int n,
-							const char* datafilename
-							)
-{
-	return readFromFile( data, n, 1, datafilename );
-}
-
-
-
-/*
- *	r e a d F r o m F i l e
- */
-returnValue readFromFile(	int* data, int n,
-							const char* datafilename
-							)
-{
-	#ifndef __XPCTARGET__
-	int i;
-	FILE* datafile;
-
-	/* 1) Open file. */
-	if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
-	{
-		char errstr[MAX_STRING_LENGTH];
-		snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
-		return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-	}
-
-	/* 2) Read data from file. */
-	for( i=0; i<n; ++i )
-	{
-		if ( fscanf( datafile, "%d\n", &(data[i]) ) == 0 )
-		{
-			fclose( datafile );
-			char errstr[MAX_STRING_LENGTH];
-			snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
-			return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		}
-	}
-
-	/* 3) Close file. */
-	fclose( datafile );
-
-	return SUCCESSFUL_RETURN;
-	#else /* __XPCTARGET__ */
-
-	return RET_NOT_YET_IMPLEMENTED;
-
-	#endif /* __XPCTARGET__ */
-}
-
-
-/*
- *	w r i t e I n t o F i l e
- */
-returnValue writeIntoFile(	const real_t* const data, int nrow, int ncol,
-							const char* datafilename, BooleanType append
-							)
-{
-	#ifndef __XPCTARGET__
-	int i, j;
-	FILE* datafile;
-
-	/* 1) Open file. */
-	if ( append == BT_TRUE )
-	{
-		/* append data */
-		if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
-		{
-			char errstr[MAX_STRING_LENGTH];
-			snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
-			return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		}
-	}
-	else
-	{
-		/* do not append data */
-		if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
-		{
-			char errstr[MAX_STRING_LENGTH];
-			snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
-			return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		}
-	}
-
-	/* 2) Write data into file. */
-	for( i=0; i<nrow; ++i )
-	{
-		for( j=0; j<ncol; ++j )
-		 	fprintf( datafile, "%.16e ", data[i*ncol+j] );
-
-		fprintf( datafile, "\n" );
-	}
-
-	/* 3) Close file. */
-	fclose( datafile );
-
-	return SUCCESSFUL_RETURN;
-	#else /* __XPCTARGET__ */
-
-	return RET_NOT_YET_IMPLEMENTED;
-
-	#endif /* __XPCTARGET__ */
-}
-
-
-/*
- *	w r i t e I n t o F i l e
- */
-returnValue writeIntoFile(	const real_t* const data, int n,
-							const char* datafilename, BooleanType append
-							)
-{
-	return writeIntoFile( data,1,n,datafilename,append );
-}
-
-
-/*
- *	w r i t e I n t o F i l e
- */
-returnValue writeIntoFile(	const int* const integer, int n,
-							const char* datafilename, BooleanType append
-							)
-{
-	#ifndef __XPCTARGET__
-	int i;
-
-	FILE* datafile;
-
-	/* 1) Open file. */
-	if ( append == BT_TRUE )
-	{
-		/* append data */
-		if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
-		{
-			char errstr[MAX_STRING_LENGTH];
-			snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
-			return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		}
-	}
-	else
-	{
-		/* do not append data */
-		if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
-		{
-			char errstr[MAX_STRING_LENGTH];
-			snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
-			return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		}
-	}
-
-	/* 2) Write data into file. */
-	for( i=0; i<n; ++i )
-		fprintf( datafile, "%d\n", integer[i] );
-
-	/* 3) Close file. */
-	fclose( datafile );
-
-	return SUCCESSFUL_RETURN;
-	#else /* __XPCTARGET__ */
-
-	return RET_NOT_YET_IMPLEMENTED;
-
-	#endif /* __XPCTARGET__ */
-}
-
-
-/*
- *	w r i t e I n t o M a t F i l e
- */
-returnValue writeIntoMatFile(	FILE* const matFile,
-								const real_t* const data, int nRows, int nCols, const char* name
-								)
-{
-	/*  Note, this code snippet has been inspired from the document
-	 *  "Matlab(R) MAT-file Format, R2013b" by MathWorks */
-
-	#ifndef __XPCTARGET__
-	if ( ( matFile == 0 ) || ( data == 0 ) || ( nRows < 0 ) || ( nCols < 0 ) || ( name == 0 ) )
-		return RET_INVALID_ARGUMENTS;
-
-	MatMatrixHeader var;
-
-	// setup variable header
-	var.numericFormat = 0000;  /* IEEE Little Endian - reserved - double precision (64 bits) - numeric full matrix */
-	var.nRows         = nRows; /* number of rows */
-	var.nCols         = nCols; /* number of columns */
-	var.imaginaryPart = 0;     /* no imaginary part */
-	var.nCharName     = (long)(strlen(name))+1; /* matrix name length */
-	
-	/* write variable header to mat file */
-	if ( fwrite( &var, sizeof(MatMatrixHeader),1,  matFile ) < 1 )
-		return RET_UNABLE_TO_WRITE_FILE;
-
-	if ( fwrite( name, sizeof(char),(unsigned long)(var.nCharName), matFile ) < 1 )
-		return RET_UNABLE_TO_WRITE_FILE;
-
-	int ii, jj;
-	double curData;
-
-	for ( ii=0; ii<nCols; ++ii )
-		for ( jj=0; jj<nRows; ++jj )
-		{
-			curData = (real_t)data[jj*nCols+ii];
-			if ( fwrite( &curData, sizeof(double),1, matFile ) < 1 )
-				return RET_UNABLE_TO_WRITE_FILE;
-		}
-
-	return SUCCESSFUL_RETURN;
-	#else /* __XPCTARGET__ */
-
-	return RET_NOT_YET_IMPLEMENTED;
-
-	#endif /* __XPCTARGET__ */
-}
-
-
-/*
- *	w r i t e I n t o M a t F i l e
- */
-returnValue writeIntoMatFile(	FILE* const matFile,
-								const int* const data, int nRows, int nCols, const char* name
-								)
-{
-	real_t* realData = new real_t[nRows*nCols];
-
-	int ii, jj;
-
-	for ( ii=0; ii<nRows; ++ii )
-		for ( jj=0; jj<nCols; ++jj )
-			realData[ ii*nCols+jj ] = (real_t) data[ ii*nCols+jj ];
-
-	returnValue returnvalue = writeIntoMatFile( matFile,realData,nRows,nCols,name );
-	delete[] realData;
-	
-	return returnvalue;
-}
-
-
-/*
- *	g e t C P U t i m e
- */
-real_t getCPUtime( )
-{
-	real_t current_time = -1.0;
-
-	#if defined(__WIN32__) || defined(WIN32)
-	LARGE_INTEGER counter, frequency;
-	QueryPerformanceFrequency(&frequency);
-	QueryPerformanceCounter(&counter);
-	current_time = ((real_t) counter.QuadPart) / ((real_t) frequency.QuadPart);
-	#elif defined(LINUX) || defined(__LINUX__)
-	struct timeval theclock;
-	gettimeofday( &theclock,0 );
-	current_time = 1.0*theclock.tv_sec + 1.0e-6*theclock.tv_usec;
-	#endif
-
-	return current_time;
-}
-
-
-/*
- *	g e t N o r m
- */
-real_t getNorm( const real_t* const v, int n, int type )
-{
-	int i;
-
-	real_t norm = 0.0;
-
-	switch ( type )
-	{
-		case 2:
-			for( i=0; i<n; ++i )
-				norm += v[i]*v[i];
-			return getSqrt( norm );
-
-		case 1:
-			for( i=0; i<n; ++i )
-				norm += getAbs( v[i] );
-			return norm;
-
-		default:
-			THROWERROR( RET_INVALID_ARGUMENTS );
-			return -INFTY;
-	}
-}
-
-
-/*
- *	g e t K K T R e s i d u a l
- */
-void getKKTResidual(	int nV, int nC,
-						const real_t* const H, const real_t* const g,
-						const real_t* const A, const real_t* const lb, const real_t* const ub,
-						const real_t* const lbA, const real_t* const ubA,
-						const real_t* const x, const real_t* const y,
-						real_t& stat, real_t& feas, real_t& cmpl
-						)
-{
-	/* Tolerance for dual variables considered zero. */
-	const real_t dualActiveTolerance = 1.0e3 * EPS;
-
-	int i, j;
-	real_t sum, prod;
-
-	/* Initialize residuals */
-	stat = feas = cmpl = 0.0;
-
-	/* check stationarity */
-	for (i = 0; i < nV; i++)
-	{
-		/* g term and variable bounds dual term */
-		if ( g != 0 )
-			sum = g[i] - y[i];
-		else
-			sum = 0.0 - y[i];
-
-		/* H*x term */
-		if ( H != 0 )
-			for (j = 0; j < nV; j++) sum += H[i*nV+j] * x[j];
-
-		/* A'*y term */
-		if ( A != 0 )
-			for (j = 0; j < nC; j++) sum -= A[j*nV+i] * y[nV+j];
-		
-		/* update stat */
-		if (getAbs(sum) > stat) stat = getAbs(sum);
-	}
-
-	/* check primal feasibility and complementarity */
-	/* variable bounds */
-	for (i = 0; i < nV; i++)
-	{
-		/* feasibility */
-		if ( lb != 0 )
-			if (lb[i] - x[i] > feas) 
-				feas = lb[i] - x[i];
-
-		if ( ub != 0 )
-			if (x[i] - ub[i] > feas) 
-				feas = x[i] - ub[i];
-
-		/* complementarity */
-		prod = 0.0;
-
-		if ( lb != 0 )
-			if (y[i] > dualActiveTolerance) /* lower bound */
-				prod = (x[i] - lb[i]) * y[i];
-
-		if ( ub != 0 )
-			if (y[i] < -dualActiveTolerance) /* upper bound */
-				prod = (x[i] - ub[i]) * y[i];
-
-		if (getAbs(prod) > cmpl) cmpl = getAbs(prod);
-	}
-	/* A*x bounds */
-	for (i = 0; i < nC; i++)
-	{
-		/* compute sum = (A*x)_i */
-		sum = 0.0;
-		if ( A != 0 )
-			for (j = 0; j < nV; j++) 
-				sum += A[i*nV+j] * x[j];
-
-		/* feasibility */
-		if ( lbA != 0 )
-			if (lbA[i] - sum > feas) 
-				feas = lbA[i] - sum;
-
-		if ( ubA != 0 )
-			if (sum - ubA[i] > feas) 
-				feas = sum - ubA[i];
-
-		/* complementarity */
-		prod = 0.0;
-
-		if ( lbA != 0 )
-			if (y[nV+i] > dualActiveTolerance) /* lower bound */
-				prod = (sum - lbA[i]) * y[nV+i];
-		
-		if ( ubA != 0 )
-			if (y[nV+i] < -dualActiveTolerance) /* upper bound */
-				prod = (sum - ubA[i]) * y[nV+i];
-
-		if (getAbs(prod) > cmpl) cmpl = getAbs(prod);
-	}
-}
-
-
-/*
- *	g e t K K T R e s i d u a l
- */
-void getKKTResidual(	int nV,
-						const real_t* const H, const real_t* const g,
-						const real_t* const lb, const real_t* const ub,
-						const real_t* const x, const real_t* const y,
-						real_t& stat, real_t& feas, real_t& cmpl
-						)
-{
-	getKKTResidual(	nV,0,
-					H,g,0,lb,ub,0,0,
-					x,y,
-					stat,feas,cmpl
-					);
-}
-
-
-/*
- *	c o n v e r t B o o l e a n T y p e T o S t r i n g
- */
-returnValue convertBooleanTypeToString( BooleanType value, char* const string )
-{
-	#ifndef __XPCTARGET__
-	if ( value == BT_FALSE )
-		snprintf( string,20,"BT_FALSE" );
-	else
-		snprintf( string,20,"BT_TRUE" );
-	#endif /* __XPCTARGET__ */
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	c o n v e r t S u b j e c t T o S t a t u s T o S t r i n g
- */
-returnValue convertSubjectToStatusToString( SubjectToStatus value, char* const string )
-{
-	#ifndef __XPCTARGET__
-	switch( value )
-	{
-		case ST_INACTIVE:
-			snprintf( string,20,"ST_INACTIVE" );
-			break;
-
-		case ST_LOWER:
-			snprintf( string,20,"ST_LOWER" );
-			break;
-
-		case ST_UPPER:
-			snprintf( string,20,"ST_UPPER" );
-			break;
-
-		case ST_UNDEFINED:
-			snprintf( string,20,"ST_UNDEFINED" );
-			break;
-			
-		case ST_INFEASIBLE_LOWER:
-			snprintf( string,20,"ST_INFEASIBLE_LOWER" );
-			break;
-
-		case ST_INFEASIBLE_UPPER:
-			snprintf( string,20,"ST_INFEASIBLE_UPPER" );
-			break;
-
-		default:
-			snprintf( string,20,"<invalid value>" );
-			break;
-	}
-	#endif /* __XPCTARGET__ */
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	c o n v e r t P r i n t L e v e l T o S t r i n g
- */
-returnValue convertPrintLevelToString( PrintLevel value, char* const string )
-{
-	#ifndef __XPCTARGET__
-	switch( value )
-	{
-		case PL_NONE:
-			snprintf( string,20,"PL_NONE" );
-			break;
-
-		case PL_LOW:
-			snprintf( string,20,"PL_LOW" );
-			break;
-
-		case PL_MEDIUM:
-			snprintf( string,20,"PL_MEDIUM" );
-			break;
-
-		case PL_HIGH:
-			snprintf( string,20,"PL_HIGH" );
-			break;
-			
-		case PL_TABULAR:
-			snprintf( string,20,"PL_TABULAR" );
-			break;
-
-		case PL_DEBUG_ITER:
-			snprintf( string,20,"PL_DEBUG_ITER" );
-			break;
-
-		default:
-			snprintf( string,20,"<invalid value>" );
-			break;
-	}
-	#endif /* __XPCTARGET__ */
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t S i m p l e S t a t u s
- */
-int getSimpleStatus(	returnValue returnvalue,
-						BooleanType doPrintStatus
-						)
-{
-	int simpleStatus = -1;
-
-	/* determine simple status from returnvalue */
-	switch ( returnvalue )
-	{
-		case SUCCESSFUL_RETURN:
-			simpleStatus = 0;
-			break;
-
-		case RET_MAX_NWSR_REACHED:
-			simpleStatus = 1;
-			break;
-
-		case RET_INIT_FAILED_INFEASIBILITY:
-		case RET_HOTSTART_STOPPED_INFEASIBILITY:
-			simpleStatus = -2;
-			break;
-
-		case RET_INIT_FAILED_UNBOUNDEDNESS:
-		case RET_HOTSTART_STOPPED_UNBOUNDEDNESS:
-			simpleStatus = -3;
-			break;
-
-		default:
-			simpleStatus = -1;
-			break;
-	}
-
-	if ( doPrintStatus == BT_TRUE )
-	{
-		VisibilityStatus vsInfo = getGlobalMessageHandler( )->getInfoVisibilityStatus( );
-		getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_VISIBLE );
-		getGlobalMessageHandler( )->setErrorCount( -1 );
-		
-		int retValNumber = (int)RET_SIMPLE_STATUS_P0 - simpleStatus;
-		THROWINFO( (returnValue)retValNumber );
-
-		getGlobalMessageHandler( )->setInfoVisibilityStatus( vsInfo );
-	}
-
-	return simpleStatus;
-}
-
-
-/*
- *	n o r m a l i s e C o n s t r a i n t s
- */
-returnValue normaliseConstraints(	int nV, int nC,
-									real_t* A, real_t* lbA, real_t* ubA,
-									int type
-									)
-{
-	int ii, jj;
-	real_t curNorm;
-
-	if ( ( nV <= 0 ) || ( nC <= 0 ) || ( A == 0 ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	for( ii=0; ii<nC; ++ii )
-	{
-		/* get row norm */
-		curNorm = getNorm( &(A[ii*nV]),nV,type );
-
-		if ( curNorm > EPS )
-		{
-			/* normalise if norm is positive */
-			for( jj=0; jj<nV; ++jj )
-				A[ii*nV + jj] /= curNorm;
-
-			if ( lbA != 0 ) lbA[ii] /= curNorm;
-			if ( ubA != 0 ) ubA[ii] /= curNorm;
-		}
-		else
-		{
-			/* if row norm is (close to) zero, kind of erase constraint */
-			if ( type == 1 )
-			{
-				for( jj=0; jj<nV; ++jj )
-					A[ii*nV + jj] = 1.0 / ((real_t)nV);
-			}
-			else
-			{
-				/* assume type == 2 */
-				for( jj=0; jj<nV; ++jj )
-					A[ii*nV + jj] = 1.0 / getSqrt((real_t)nV);
-			}
-
-			if ( lbA != 0 ) lbA[ii] = -INFTY;
-			if ( ubA != 0 ) ubA[ii] =  INFTY;
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-#ifdef __DEBUG__
-/*
- *	g d b _ p r i n t m at
- */
-extern "C" void gdb_printmat(const char *fname, real_t *M, int n, int m, int ldim)
-{
-	#ifndef __XPCTARGET__
-	int i, j;
-	FILE *fid;
-
-	fid = fopen(fname, "wt");
-	if (!fid) 
-	{
-		perror("Error opening file: ");
-		return;
-	}
-
-	for (i = 0; i < n; i++)
-	{
-		for (j = 0; j < m; j++)
-			fprintf(fid, " %23.16e", M[j*ldim+i]);
-		fprintf(fid, "\n");
-	}
-	fclose(fid);
-	#endif /* __XPCTARGET__ */
-}
-#endif /* __DEBUG__ */
-
-
-
-#if defined(__DSPACE__) || defined(__XPCTARGET__) 
-/*
- *	_ _ c x a _ p u r e _ v i r t u a l
- */
-void __cxa_pure_virtual( void )
-{
-	/* put your customized implementation here! */
-}
-#endif /* __DSPACE__ || __XPCTARGET__*/ 
-
-
-
-END_NAMESPACE_QPOASES
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/src/index.html b/3rdparty/qpOASES/qpOASES-3.0/src/index.html
deleted file mode 100644
index 22da2b9c4c4..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/src/index.html
+++ /dev/null
@@ -1,24 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/src</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/src</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="BLASReplacement.cpp">BLASReplacement.cpp</a></li>
-  <li><a href="Bounds.cpp">Bounds.cpp</a></li>
-  <li><a href="Constraints.cpp">Constraints.cpp</a></li>
-  <li><a href="Flipper.cpp">Flipper.cpp</a></li>
-  <li><a href="Indexlist.cpp">Indexlist.cpp</a></li>
-  <li><a href="LAPACKReplacement.cpp">LAPACKReplacement.cpp</a></li>
-  <li><a href="Makefile">Makefile</a></li>
-  <li><a href="Matrices.cpp">Matrices.cpp</a></li>
-  <li><a href="MessageHandling.cpp">MessageHandling.cpp</a></li>
-  <li><a href="OQPinterface.cpp">OQPinterface.cpp</a></li>
-  <li><a href="Options.cpp">Options.cpp</a></li>
-  <li><a href="QProblem.cpp">QProblem.cpp</a></li>
-  <li><a href="QProblemB.cpp">QProblemB.cpp</a></li>
-  <li><a href="SQProblem.cpp">SQProblem.cpp</a></li>
-  <li><a href="SolutionAnalysis.cpp">SolutionAnalysis.cpp</a></li>
-  <li><a href="SubjectTo.cpp">SubjectTo.cpp</a></li>
-  <li><a href="Utils.cpp">Utils.cpp</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/checkForMemoryLeaks b/3rdparty/qpOASES/qpOASES-3.0/testing/checkForMemoryLeaks
deleted file mode 100644
index 44abb2c6ee7..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/checkForMemoryLeaks
+++ /dev/null
@@ -1,106 +0,0 @@
-#!/usr/bin/env bash
-
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-##
-##	Filename:  testing/checkForMemoryLeaks
-##	Author:    Hans Joachim Ferreau
-##	Version:   3.0
-##	Date:      2014
-
-# defining colors for output
-red='\e[0;31m'
-green='\e[0;32m'
-NC='\e[0m' # No Color
-
-# runs valgrind on a number of examples to detect possible memory leaks
-
-function callValgrind {
-	echo -n "Checking for memory leaks in $2 $3... "
-
-	valgrind --tool=memcheck --leak-check=yes --error-exitcode=42 -q $2 $3 > dummy.txt;
-	retVal=$?;
-	rm -rf dummy.txt;
-
-	if [ $retVal == 42 ]; then
-		echo -e "${red}failed!${NC}"
-		counter=$[counter+1];
-	else
-		if [ $retVal == 99 ]; then
-			echo "problem data missing!"
-		else
-			echo -e "${green}passed!${NC}"
-		fi
-	fi
-}
-
-
-cd ..;
-make examples testing;
-cd testing;
-
-counter=0;
-
-# run examples in /examples
-callValgrind $counter ../bin/example1;
-callValgrind $counter ../bin/example1a;
-callValgrind $counter ../bin/example1b;
-callValgrind $counter ../bin/example2;
-callValgrind $counter ../bin/example3;
-callValgrind $counter ../bin/example3b;
-callValgrind $counter ../bin/example4;
-callValgrind $counter ../bin/example5;
-callValgrind $counter ../bin/exampleLP;
-callValgrind $counter ../bin/qrecipe;
-
-# run examples in /testing ...
-callValgrind $counter ../bin/test_matrices;
-callValgrind $counter ../bin/test_matrices2;
-callValgrind $counter ../bin/test_indexlist;
-callValgrind $counter ../bin/test_example6;
-callValgrind $counter ../bin/test_example7;
-callValgrind $counter ../bin/test_sebastien1;
-callValgrind $counter ../bin/test_vanBarelsUnboundedQP;
-callValgrind $counter ../bin/test_janick1;
-callValgrind $counter ../bin/test_janick2;
-callValgrind $counter ../bin/test_constraintProduct1;
-callValgrind $counter ../bin/test_constraintProduct2;
-callValgrind $counter ../bin/test_guessedWS1;
-callValgrind $counter ../bin/test_runAllOqpExamples;
-
-
-# ... including testbench with different settings
-callValgrind $counter ../bin/test_bench Odd; # default dense
-callValgrind $counter ../bin/test_bench Ods; # default sparse
-callValgrind $counter ../bin/test_bench Omd; # MPC dense
-callValgrind $counter ../bin/test_bench Ors; # reliable dense
-
-
-if [ $counter == 0 ]; then
-	echo -e "${green}All available tests passed successfully!${NC}"
-else
-	echo -e "${red}$counter test(s) failed!${NC}"
-	exit 1
-fi
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/data/fetch_cpp_data b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/data/fetch_cpp_data
deleted file mode 100644
index 4d841d6ac71..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/data/fetch_cpp_data
+++ /dev/null
@@ -1,34 +0,0 @@
-#!/usr/bin/env bash
-
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-##
-##	Filename:  testing/cpp/data/fetch_cpp_data
-##	Author:    Hans Joachim Ferreau
-##	Version:   3.0
-##	Date:      2014
-
-
-svn export https://projects.coin-or.org/svn/qpOASES/misc/testingdata/cpp . --force
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/data/index.html b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/data/index.html
deleted file mode 100644
index b6c3242cfb8..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/data/index.html
+++ /dev/null
@@ -1,8 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/testing/cpp/data</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/testing/cpp/data</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="fetch_cpp_data">fetch_cpp_data</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/index.html b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/index.html
deleted file mode 100644
index bb4321bcdc7..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/index.html
+++ /dev/null
@@ -1,33 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/testing/cpp</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/testing/cpp</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="data/">data/</a></li>
-  <li><a href="test_bench.cpp">test_bench.cpp</a></li>
-  <li><a href="test_constraintProduct1.cpp">test_constraintProduct1.cpp</a></li>
-  <li><a href="test_constraintProduct2.cpp">test_constraintProduct2.cpp</a></li>
-  <li><a href="test_example1.cpp">test_example1.cpp</a></li>
-  <li><a href="test_example1a.cpp">test_example1a.cpp</a></li>
-  <li><a href="test_example1b.cpp">test_example1b.cpp</a></li>
-  <li><a href="test_example2.cpp">test_example2.cpp</a></li>
-  <li><a href="test_example4.cpp">test_example4.cpp</a></li>
-  <li><a href="test_example5.cpp">test_example5.cpp</a></li>
-  <li><a href="test_example6.cpp">test_example6.cpp</a></li>
-  <li><a href="test_example7.cpp">test_example7.cpp</a></li>
-  <li><a href="test_exampleLP.cpp">test_exampleLP.cpp</a></li>
-  <li><a href="test_gradientShift.cpp">test_gradientShift.cpp</a></li>
-  <li><a href="test_guessedWS1.cpp">test_guessedWS1.cpp</a></li>
-  <li><a href="test_indexlist.cpp">test_indexlist.cpp</a></li>
-  <li><a href="test_janick1.cpp">test_janick1.cpp</a></li>
-  <li><a href="test_janick2.cpp">test_janick2.cpp</a></li>
-  <li><a href="test_matrices.cpp">test_matrices.cpp</a></li>
-  <li><a href="test_matrices2.cpp">test_matrices2.cpp</a></li>
-  <li><a href="test_matrices3.cpp">test_matrices3.cpp</a></li>
-  <li><a href="test_qrecipe.cpp">test_qrecipe.cpp</a></li>
-  <li><a href="test_qrecipe_data.hpp">test_qrecipe_data.hpp</a></li>
-  <li><a href="test_runAllOqpExamples.cpp">test_runAllOqpExamples.cpp</a></li>
-  <li><a href="test_sebastien1.cpp">test_sebastien1.cpp</a></li>
-  <li><a href="test_vanBarelsUnboundedQP.cpp">test_vanBarelsUnboundedQP.cpp</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_bench.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_bench.cpp
deleted file mode 100644
index 762d99859c9..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_bench.cpp
+++ /dev/null
@@ -1,309 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_bench.cpp
- *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2010-2014
- *
- *	Unit test running all benchmark examples stored in problems directory.
- */
-
-
-
-#include <dirent.h>
-#include <cstring>
-#include <cstdlib>
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-/** Run benchmark examples. */
-int main( int argc, char *argv[] )
-{
-	USING_NAMESPACE_QPOASES
-	#ifdef __USE_SINGLE_PRECISION__
-	const real_t TOL = 5e-2;
-	#else
-	const real_t TOL = 1e-5;
-	#endif
-
-	/* 1) Define benchmark arguments. */
-	BooleanType isSparse = BT_FALSE;
-	//BooleanType isSparse = BT_TRUE;
-	Options options;
- 	options.setToDefault();
-	//options.setToMPC();
-	//options.setToReliable();
-	options.printLevel = PL_LOW;
-	//options.printLevel = PL_MEDIUM;
-	//options.printLevel = PL_TABULAR;
-	//options.enableFarBounds = BT_FALSE;
-
-// 	options.initialStatusBounds = ST_LOWER;
-	//options.numRegularisationSteps = 1;
-	//options.epsRegularisation = 1.0e3 * EPS;
- 	
-	//options.enableFlippingBounds = BT_FALSE;
-	//options.enableFlippingBounds = BT_FALSE;
-	//options.enableRamping = BT_TRUE;
-	//options.enableFarBounds = BT_FALSE;
-	//options.enableNZCTests = BT_FALSE;
-	//options.epsNZCTests = 1.0e4 * EPS;
-	//options.epsFlipping = 1.0e5 * EPS;
-	//options.enableFullLITests = BT_TRUE;
- 	//options.enableDriftCorrection = 1;
- 	//options.enableEqualities = BT_TRUE;
-	//options.enableEqualities = BT_FALSE;
- 	//options.epsNum = -1.0e3 * EPS;
- 	//options.epsDen =  1.0e3 * EPS;
-
-
-	int nWSR;
-	real_t maxCPUtime; /* seconds */
-	real_t maxStationarity = 0.0, maxFeasibility = 0.0, maxComplementarity = 0.0;
-	real_t avgStationarity = 0.0, avgFeasibility = 0.0, avgComplementarity = 0.0;
-
-	int scannedDir = 0;
-	int nfail = 0, npass = 0;
-	int nproblems, i;
-	struct dirent **namelist;
-	char resstr[MAX_STRING_LENGTH], OQPproblem[MAX_STRING_LENGTH];
-	char *problem;
-	returnValue returnvalue;
-
-	int expectedNumSolvedProblems = 44;
-	real_t expectedAvgStationarity = TOL;
-	real_t expectedAvgavgFeasibility = TOL;
-	real_t expectedAvgFeasibility = TOL;
-
-	
-	if ( argv[argc-1][0] == 'O' )
-	{
-		if ( strlen(argv[argc-1]) != 3 )
-		{
-			fprintf( stdout,"ERROR (testbench): Invalid options passed!\n" );
-			return TEST_DATA_NOT_FOUND;
-		}
-		
-		fprintf( stdout,"Analysing passed options:  " );
-		switch ( argv[argc-1][1] )
-		{
-			case 'd':
-				fprintf( stdout,"default options, " );
-				options.setToDefault();
-				if ( argv[argc-1][2] == 's' )
-				{
-					expectedNumSolvedProblems = 44;
-					expectedAvgStationarity   = 1e-7;
-					expectedAvgavgFeasibility = 1e-10;
-					expectedAvgFeasibility    = 5e-7;
-				}
-				else
-				{
-					expectedNumSolvedProblems = 44;
-					expectedAvgStationarity   = 1e-7;
-					expectedAvgavgFeasibility = 5e-10;
-					expectedAvgFeasibility    = 5e-8;
-				}
-				break;
-				
-			case 'r':
-				fprintf( stdout,"reliable options, " );
-				options.setToReliable();
-				if ( argv[argc-1][2] == 's' )
-				{
-					expectedNumSolvedProblems = 44;
-					expectedAvgStationarity   = 1e-9;
-					expectedAvgavgFeasibility = 2e-11;
-					expectedAvgFeasibility    = 2e-9;
-				}
-				else
-				{
-					expectedNumSolvedProblems = 44;
-					expectedAvgStationarity   = 2e-9;
-					expectedAvgavgFeasibility = 1e-9;
-					expectedAvgFeasibility    = 2e-7;
-				}
-				break;
-				
-			case 'm':
-				fprintf( stdout,"MPC options, " );
-				options.setToMPC();
-				if ( argv[argc-1][2] == 's' )
-				{
-					expectedNumSolvedProblems = 42;
-					expectedAvgStationarity   = 2e-8;
-					expectedAvgavgFeasibility = 1e-8;
-					expectedAvgFeasibility    = 1e-7;
-				}
-				else
-				{
-					expectedNumSolvedProblems = 42;
-					expectedAvgStationarity   = 2e-8;
-					expectedAvgavgFeasibility = 1e-8;
-					expectedAvgFeasibility    = 5e-8;
-				}
-				break;
-				
-			default:
-				fprintf( stdout,"ERROR (testbench): Invalid options passed!\n" );
-				return TEST_DATA_NOT_FOUND;
-		}
-		
-		switch ( argv[argc-1][2] )
-		{
-			case 's':
-				fprintf( stdout,"sparse QP data\n" );
-				isSparse = BT_TRUE;
-				break;
-				
-			case 'd':
-				fprintf( stdout,"dense QP data\n" );
-				isSparse = BT_FALSE;
-				break;
-				
-			default:
-				fprintf( stdout,"ERROR (testbench): Invalid options passed!\n" );
-				return TEST_DATA_NOT_FOUND;
-		}
-		options.printLevel = PL_NONE;
-		
-		nproblems = argc-2;
-	}
-	else
-	{
-		nproblems = argc-1;
-	}
-
-	
-	if (nproblems == 0)
-	{
-		/* 2a) Scan problem directory */
-		nproblems = scandir("../testing/cpp/data/problems", &namelist, NULL, alphasort);
-		if (nproblems <= 0)
-		{
-			myPrintf( "No test problems found!\n" );
-			return TEST_DATA_NOT_FOUND;
-		}
-		scannedDir = 1;
-	}
-	else
-	{
-		/* 2b) Use problem list given by arguments */
-		scannedDir = 0;
-	}
-
-	/* 3) Run benchmark. */
-	printf("%10s %9s %9s %9s %6s  %-12s\n", "problem", "stat",
-			"feas", "compl", "nWSR", "result");
-	for (i = 0; i < nproblems; i++)
-	{
-		if (scannedDir)
-		{
-			/* skip special directories and zip file cuter.*bz2 */
-			if (namelist[i]->d_name[0] == '.' || namelist[i]->d_name[0] == 'c')
-			{
-				free(namelist[i]);
-				continue;
-			}
-			problem = namelist[i]->d_name;
-		}
-		else
-		{
-			problem = argv[i+1];
-		}
-
-		fprintf(stdFile, "%-10s ", problem);
-		fflush(stdFile);
-
-		snprintf(OQPproblem, MAX_STRING_LENGTH, "../testing/cpp/data/problems/%s/", problem);
-		maxCPUtime = 300.0;
-		nWSR = 2500;
-
-		returnvalue = runOQPbenchmark(	OQPproblem, isSparse, options,
-										nWSR, maxCPUtime, maxStationarity, maxFeasibility, maxComplementarity 
-										);
-		if (returnvalue	== SUCCESSFUL_RETURN
-				&& maxStationarity < TOL
-				&& maxFeasibility < TOL
-				&& maxComplementarity < TOL)
-		{
-			npass++;
-			
-			avgStationarity    += maxStationarity;
-			avgFeasibility     += maxFeasibility;
-			avgComplementarity += maxComplementarity;
-
-			strncpy(resstr, "pass", MAX_STRING_LENGTH);
-		}
-		else
-		{
-			if ( returnvalue == RET_BENCHMARK_ABORTED )
-				return TEST_DATA_NOT_FOUND;
-
-			nfail++;
-			snprintf (resstr, MAX_STRING_LENGTH, "fail (%d)", returnvalue);
-		}
-		fprintf(stdFile, "%9.2e %9.2e %9.2e %6d  %-12s\n", maxStationarity,
-				maxFeasibility, maxComplementarity, nWSR, resstr);
-
-		if (scannedDir) free(namelist[i]);
-	}
-	if (scannedDir) free(namelist);
-
-	avgStationarity    /= npass;
-	avgFeasibility     /= npass;
-	avgComplementarity /= npass;
-
-
-	/* 4) Print results. */
-	printf("\n\n" );
-	printf("Testbench results:\n" );
-	printf("======================\n\n" );
-	printf("Pass:  %3d\n", npass);
-	printf("Fail:  %3d\n", nfail);
-	printf("Ratio: %5.1f%%\n", 100.0 * (real_t)npass / (real_t)(npass+nfail));
-	printf("\n" );
-
-	QPOASES_TEST_FOR_TRUE( npass >= expectedNumSolvedProblems );
-
-
-	printf("avg. stat:  %e\n", avgStationarity);
-	printf("avg. feas:  %e\n", avgFeasibility);
-	printf("avg. cmpl:  %e\n", avgFeasibility);
-
-	QPOASES_TEST_FOR_TOL( avgStationarity,    expectedAvgStationarity );
-	QPOASES_TEST_FOR_TOL( avgFeasibility,     expectedAvgavgFeasibility );
-	QPOASES_TEST_FOR_TOL( avgComplementarity, expectedAvgFeasibility );
-
-
-	return 0;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_constraintProduct1.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_constraintProduct1.cpp
deleted file mode 100644
index ad8aedec49e..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_constraintProduct1.cpp
+++ /dev/null
@@ -1,197 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_constraintProduct1.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2014
- *
- *	Another example for testing qpOASES using the possibility to specify 
- *	user-defined constraint product function.
- */
-
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-
-USING_NAMESPACE_QPOASES
-
-
-/** 
- *	\brief Example illustrating the use of the \a ConstraintProduct class.
- *
- *	Example illustrating the use of the \a ConstraintProduct class.
- *
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- */
-class MpcConstraintProduct : public ConstraintProduct
-{
-	public:
-		/** Default constructor. */
-		MpcConstraintProduct( ) {};
-
-		/** Constructor. */
-		MpcConstraintProduct(	int _nV,
-								int _nC,
-								int _diagOffset,
-								real_t* _A
-								)
-		{
-			nV = _nV;
-			nC = _nC;
-			diagOffset = _diagOffset;
-			A  = _A;
-		};
-
-		/** Copy constructor (flat copy). */
-		MpcConstraintProduct(	const MpcConstraintProduct& rhs
-								)
-		{
-			nV = rhs.nV;
-			nC = rhs.nC;
-			diagOffset = rhs.diagOffset;
-			A  = rhs.A;
-		};
-
-		/** Destructor. */
-		virtual ~MpcConstraintProduct( ) {};
-		
-		/** Assignment operator (flat copy). */
-		MpcConstraintProduct& operator=(	const MpcConstraintProduct& rhs
-										)
-		{
-			if ( this != &rhs )
-			{
-				nV = rhs.nV;
-				nC = rhs.nC;
-				diagOffset = rhs.diagOffset;
-				A  = rhs.A;
-			}
-			else
-				return *this;
-		};
-
-		virtual int operator() (	int constrIndex,
-									const real_t* const x,
-									real_t* const constrValue
-									) const
-		{
-			int maxI = (int)(((real_t)constrIndex) * ((real_t)nV) / ((real_t)nC)) + diagOffset;
-			maxI = getMin( maxI,nV );
-
-			constrValue[0] = 0.0;
-
-			for( int i=0; i<maxI; ++i )
-				constrValue[0] += A[constrIndex*nV + i] * x[i];
-
-			return 0;
-		};
-
-	protected:
-		int nV;			/**< Number of variables. */
-		int nC;			/**< Number of constraints. */
-		int diagOffset;	/**< ... */
-		real_t* A;		/**< Pointer to full constraint matrix (typically not needed!). */
-		
-};
-
-
-
-/**	Example for qpOASES main function using the possibility to specify 
- *	user-defined constraint product function. */
-int main( )
-{
-	int nQP, nV, nC, nEC;
-	real_t *H, *g, *A, *lb, *ub, *lbA, *ubA;
-	real_t cputime;
-
-	real_t xOpt[1000];
-	real_t yOpt[1000];
-	real_t xOptCP[1000+1000];
-	real_t yOptCP[1000+1000];
-	
-	const char* path = "./cpp/data/oqp/chain80w/";
-	int k = 10; //th problem
-	
-		
-	if ( readOQPdimensions(	path, nQP,nV,nC,nEC ) != SUCCESSFUL_RETURN )
-		return TEST_DATA_NOT_FOUND;
-	
-	readOQPdata(	path, nQP,nV,nC,nEC,
-					&H,&g,&A,&lb,&ub,&lbA,&ubA,
-					0,0,0
-					);
-	
-	Options myOptions;
-	//myOptions.setToMPC();
-	myOptions.printLevel = PL_LOW;
-
-	int nWSR = 500;
-	cputime = 20.0;
-	QProblem qp( nV,nC );
-	qp.setOptions( myOptions );
-	qp.init( H,&(g[k*nV]),A,&(lb[k*nV]),&(ub[k*nV]),&(lbA[k*nC]),&(ubA[k*nC]),nWSR,&cputime );
-	qp.getPrimalSolution( xOpt );
-	qp.getDualSolution( yOpt );
-	printf( "cputime without constraintProduct: %.3ems\n", cputime*1000.0 );
-	
-
-	nWSR = 500;
-	cputime = 20.0;
-	MpcConstraintProduct myCP( nV,nC,2,A );
-	QProblem qpCP( nV,nC );
-	qpCP.setOptions( myOptions );
-	qpCP.setConstraintProduct( &myCP );
-	qpCP.init( H,&(g[k*nV]),A,&(lb[k*nV]),&(ub[k*nV]),&(lbA[k*nC]),&(ubA[k*nC]),nWSR,&cputime );
-	qpCP.getPrimalSolution( xOptCP );
-	qpCP.getDualSolution( yOptCP );
-	printf( "cputime with    constraintProduct: %.3ems\n", cputime*1000.0 );
-	
-	delete[] ubA;
-	delete[] lbA;
-	delete[] ub;
-	delete[] lb;
-	delete[] A;
-	delete[] g;
-	delete[] H;
-
-	for( int ii=0; ii<nV; ++ii )
-		QPOASES_TEST_FOR_NEAR( xOptCP[ii],xOpt[ii] );
-
-	for( int ii=0; ii<nV+nC; ++ii )
-		QPOASES_TEST_FOR_NEAR( yOptCP[ii],yOpt[ii] );
-
-	return 0;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_constraintProduct2.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_constraintProduct2.cpp
deleted file mode 100644
index 67fcfa48e20..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_constraintProduct2.cpp
+++ /dev/null
@@ -1,196 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_constraintProduct2.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2014
- *
- *	Another example for testing qpOASES using the possibility to specify 
- *	user-defined constraint product function.
- */
-
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-
-USING_NAMESPACE_QPOASES
-
-
-/** 
- *	\brief Example illustrating the use of the \a ConstraintProduct class.
- *
- *	Example illustrating the use of the \a ConstraintProduct class.
- *
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- */
-class MpcConstraintProduct : public ConstraintProduct
-{
-	public:
-		/** Default constructor. */
-		MpcConstraintProduct( ) {};
-
-		/** Constructor. */
-		MpcConstraintProduct(	int _nV,
-								int _nC,
-								int _diagOffset,
-								real_t* _A
-								)
-		{
-			nV = _nV;
-			nC = _nC;
-			diagOffset = _diagOffset;
-			A  = _A;
-		};
-
-		/** Copy constructor (flat copy). */
-		MpcConstraintProduct(	const MpcConstraintProduct& rhs
-								)
-		{
-			nV = rhs.nV;
-			nC = rhs.nC;
-			diagOffset = rhs.diagOffset;
-			A  = rhs.A;
-		};
-
-		/** Destructor. */
-		virtual ~MpcConstraintProduct( ) {};
-		
-		/** Assignment operator (flat copy). */
-		MpcConstraintProduct& operator=(	const MpcConstraintProduct& rhs
-										)
-		{
-			if ( this != &rhs )
-			{
-				nV = rhs.nV;
-				nC = rhs.nC;
-				diagOffset = rhs.diagOffset;
-				A  = rhs.A;
-			}
-			else
-				return *this;
-		};
-
-		virtual int operator() (	int constrIndex,
-									const real_t* const x,
-									real_t* const constrValue
-									) const
-		{
-			int maxI = (int)(((real_t)constrIndex) * ((real_t)nV) / ((real_t)nC)) + diagOffset;
-			maxI = getMin( maxI,nV );
-
-			constrValue[0] = 0.0;
-
-			for( int i=0; i<maxI; ++i )
-				constrValue[0] += A[constrIndex*nV + i] * x[i];
-
-			return 0;
-		};
-
-	protected:
-		int nV;			/**< Number of variables. */
-		int nC;			/**< Number of constraints. */
-		int diagOffset;	/**< ... */
-		real_t* A;		/**< Pointer to full constraint matrix (typically not needed!). */
-		
-};
-
-
-/**	Example for qpOASES main function using the possibility to specify 
- *	user-defined constraint product function. */
-int main( )
-{
-	int nQP, nV, nC, nEC;
-	real_t *H, *g, *A, *lb, *ub, *lbA, *ubA;
-	real_t cputime;
-	
-	real_t xOpt[1000];
-	real_t yOpt[1000];
-	real_t xOptCP[1000+1000];
-	real_t yOptCP[1000+1000];
-		
-	const char* path = "./cpp/data/oqp/diesel/";
-	int k = 200; //th problem
-	
-	
-	if ( readOQPdimensions(	path, nQP,nV,nC,nEC ) != SUCCESSFUL_RETURN )
-		return TEST_DATA_NOT_FOUND;
-
-	readOQPdata(	path, nQP,nV,nC,nEC,
-					&H,&g,&A,&lb,&ub,&lbA,&ubA,
-					0,0,0
-					);
-	
-	Options myOptions;
-	myOptions.setToMPC();
-	myOptions.printLevel = PL_LOW;
-	
-	int nWSR = 500;
-	cputime = 10.0;
-	QProblem qp( nV,nC );
-	qp.setOptions( myOptions );
-	qp.init( H,&(g[k*nV]),A,&(lb[k*nV]),&(ub[k*nV]),&(lbA[k*nC]),&(ubA[k*nC]),nWSR,&cputime );
-	qp.getPrimalSolution( xOpt );
-	qp.getDualSolution( yOpt );
-	printf( "cputime without constraintProduct: %.3ems\n", cputime*1000.0 );
-	
-	
-	nWSR = 500;
-	cputime = 10.0;
-	MpcConstraintProduct myCP( nV,nC,1,A );
-	QProblem qpCP( nV,nC );
-	qpCP.setOptions( myOptions );
-	qpCP.setConstraintProduct( &myCP );
-	qpCP.init( H,&(g[k*nV]),A,&(lb[k*nV]),&(ub[k*nV]),&(lbA[k*nC]),&(ubA[k*nC]),nWSR,&cputime );
-	qpCP.getPrimalSolution( xOptCP );
-	qpCP.getDualSolution( yOptCP );
-	printf( "cputime without constraintProduct: %.3ems\n", cputime*1000.0 );
-	
-	delete[] ubA;
-	delete[] lbA;
-	delete[] ub;
-	delete[] lb;
-	delete[] A;
-	delete[] g;
-	delete[] H;
-	
-	for( int ii=0; ii<nV; ++ii )
-		QPOASES_TEST_FOR_NEAR( xOptCP[ii],xOpt[ii] );
-
-	for( int ii=0; ii<nV+nC; ++ii )
-		QPOASES_TEST_FOR_NEAR( yOptCP[ii],yOpt[ii] );
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1.cpp
deleted file mode 100644
index 2a7ba863e1c..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1.cpp
+++ /dev/null
@@ -1,123 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_example1.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Very simple example for testing qpOASES (using QProblem class).
- */
-
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-/** Example for qpOASES main function using the QProblem class. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	/* Setup data of first QP. */
-	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
-	real_t A[1*2] = { 1.0, 1.0 };
-	real_t g[2] = { 1.5, 1.0 };
-	real_t lb[2] = { 0.5, -2.0 };
-	real_t ub[2] = { 5.0, 2.0 };
-	real_t lbA[1] = { -1.0 };
-	real_t ubA[1] = { 2.0 };
-
-	/* Setup data of second QP. */
-	real_t g_new[2] = { 1.0, 1.5 };
-	real_t lb_new[2] = { 0.0, -1.0 };
-	real_t ub_new[2] = { 5.0, -0.5 };
-	real_t lbA_new[1] = { -2.0 };
-	real_t ubA_new[1] = { 1.0 };
-
-
-	/* Setting up QProblem object. */
-	QProblem example( 2,1 );
-
-	Options options;
-	example.setOptions( options );
-
-	/* Solve first QP. */
-	int nWSR = 10;
-	example.init( H,g,A,lb,ub,lbA,ubA, nWSR );
-
-	/* Get and print solution of second QP. */
-	real_t xOpt[2];
-	real_t yOpt[2+1];
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-	printf( "\nxOpt = [ %e, %e ];  yOpt = [ %e, %e, %e ];  objVal = %e\n\n", 
-			xOpt[0],xOpt[1],yOpt[0],yOpt[1],yOpt[2],example.getObjVal() );
-	
-	/* Compute KKT tolerances */
-	real_t stat, feas, cmpl;
-
-	getKKTResidual(	2,1,
-					H,g,A,lb,ub,lbA,ubA,
-					xOpt,yOpt,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-15 );
-	QPOASES_TEST_FOR_TOL( feas,1e-15 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
-
-
-	/* Solve second QP. */
-	nWSR = 10;
-	example.hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR );
-
-	/* Get and print solution of second QP. */
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-	printf( "\nxOpt = [ %e, %e ];  yOpt = [ %e, %e, %e ];  objVal = %e\n\n", 
-			xOpt[0],xOpt[1],yOpt[0],yOpt[1],yOpt[2],example.getObjVal() );
-	
-	/* Compute KKT tolerances */
-	getKKTResidual(	2,1,
-					H,g_new,A,lb_new,ub_new,lbA_new,ubA_new,
-					xOpt,yOpt,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-15 );
-	QPOASES_TEST_FOR_TOL( feas,1e-15 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1a.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1a.cpp
deleted file mode 100644
index de43caa7957..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1a.cpp
+++ /dev/null
@@ -1,118 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_example1a.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Very simple example for testing qpOASES using the SQProblem class.
- */
-
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-/** Example for qpOASES main function using the SQProblem class. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	/* Setup data of first QP. */
-	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
-	real_t A[1*2] = { 1.0, 1.0 };
-	real_t g[2] = { 1.5, 1.0 };
-	real_t lb[2] = { 0.5, -2.0 };
-	real_t ub[2] = { 5.0, 2.0 };
-	real_t lbA[1] = { -1.0 };
-	real_t ubA[1] = { 2.0 };
-
-	/* Setup data of second QP. */
-	real_t H_new[2*2] = { 1.0, 0.5, 0.5, 0.5 };
-	real_t A_new[1*2] = { 1.0, 5.0 };
-	real_t g_new[2] = { 1.0, 1.5 };
-	real_t lb_new[2] = { 0.0, -1.0 };
-	real_t ub_new[2] = { 5.0, -0.5 };
-	real_t lbA_new[1] = { -2.0 };
-	real_t ubA_new[1] = { 1.0 };
-
-
-	/* Setting up SQProblem object. */
-	SQProblem example( 2,1 );
-
-	/* Solve first QP. */
-	int nWSR = 10;
-	example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
-
-	real_t xOpt[2];
-	real_t yOpt[2+1];
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-
-	/* Compute KKT tolerances */
-	real_t stat, feas, cmpl;
-
-	getKKTResidual(	2,1,
-					H,g,A,lb,ub,lbA,ubA,
-					xOpt,yOpt,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-15 );
-	QPOASES_TEST_FOR_TOL( feas,1e-15 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
-
-
-	/* Solve second QP. */
-	nWSR = 10;
-	example.hotstart( H_new,g_new,A_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 );
-
-	/* Get and print solution of second QP. */
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-	printf( "\nxOpt = [ %e, %e ];  objVal = %e\n\n", xOpt[0],xOpt[1],example.getObjVal() );
-
-	/* Compute KKT tolerances */
-	getKKTResidual(	2,1,
-					H_new,g_new,A_new,lb_new,ub_new,lbA_new,ubA_new,
-					xOpt,yOpt,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-15 );
-	QPOASES_TEST_FOR_TOL( feas,1e-15 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1b.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1b.cpp
deleted file mode 100644
index bb812576e80..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example1b.cpp
+++ /dev/null
@@ -1,119 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_example1b.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Very simple example for testing qpOASES using the QProblemB class.
- */
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-/** Example for qpOASES main function using the QProblemB class. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	/* Setup data of first QP. */
-	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
-	real_t g[2] = { 1.5, 1.0 };
-	real_t lb[2] = { 0.5, -2.0 };
-	real_t ub[2] = { 5.0, 2.0 };
-
-	/* Setup data of second QP. */
-	real_t g_new[2] = { 1.0, 1.5 };
-	real_t lb_new[2] = { 0.0, -1.0 };
-	real_t ub_new[2] = { 5.0, -0.5 };
-
-
-	/* Setting up QProblemB object. */
-	QProblemB example( 2 );
-
-	Options options;
-	//options.enableFlippingBounds = BT_FALSE;
-	options.initialStatusBounds = ST_INACTIVE;
-	options.numRefinementSteps = 1;
-	options.enableCholeskyRefactorisation = 1;
-	example.setOptions( options );
-
-	/* Solve first QP. */
-	int nWSR = 10;
-	example.init( H,g,lb,ub, nWSR,0 );
-// 	printf( "\nnWSR = %d\n\n", nWSR );
-
-	real_t xOpt[2];
-	real_t yOpt[2];
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-
-	/* Compute KKT tolerances */
-	real_t stat, feas, cmpl;
-
-	getKKTResidual(	2,
-					H,g,lb,ub,
-					xOpt,yOpt,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-15 );
-	QPOASES_TEST_FOR_TOL( feas,1e-15 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
-
-	
-	/* Solve second QP. */
-	nWSR = 10;
-	example.hotstart( g_new,lb_new,ub_new, nWSR,0 );
-// 	printf( "\nnWSR = %d\n\n", nWSR );
-
-	/* Get and print solution of second QP. */
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-	printf( "\nxOpt = [ %e, %e ];  objVal = %e\n\n", xOpt[0],xOpt[1],example.getObjVal() );
-
-	/* Compute KKT tolerances */
-	getKKTResidual(	2,
-					H,g_new,lb_new,ub_new,
-					xOpt,yOpt,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-15 );
-	QPOASES_TEST_FOR_TOL( feas,1e-15 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example2.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example2.cpp
deleted file mode 100644
index dd73899dd4e..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example2.cpp
+++ /dev/null
@@ -1,132 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_example2.cpp
- *	\author Hans Joachim Ferreau (thanks to Boris Houska)
- *	\version 3.0
- *	\date 2008-2014
- *
- *	Very simple example for testing qpOASES in combination
- *  with the SolutionAnalysis class.
- */
-
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-/** Example for qpOASES main function using the SolutionAnalysis class. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	/* Setup data of first QP. */
-	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
-	real_t A[1*2] = { 1.0, 1.0 };
-	real_t g[2] = { 1.5, 1.0 };
-	real_t lb[2] = { 0.5, -2.0 };
-	real_t ub[2] = { 5.0, 2.0 };
-	real_t lbA[1] = { -1.0 };
-	real_t ubA[1] = { 2.0 };
-
-	/* Setup data of second QP. */
-	real_t H_new[2*2] = { 1.0, 0.5, 0.5, 0.5 };
-	real_t A_new[1*2] = { 1.0, 5.0 };
-	real_t g_new[2] = { 1.0, 1.5 };
-	real_t lb_new[2] = { 0.0, -1.0 };
-	real_t ub_new[2] = { 5.0, -0.5 };
-	real_t lbA_new[1] = { -2.0 };
-	real_t ubA_new[1] = { 1.0 };
-
-
-	/* Setting up SQProblem object and solution analyser. */
-	SQProblem example( 2,1 );
-	SolutionAnalysis analyser;
-
-	/* Solve first QP ... */
-	int nWSR = 10;
-	example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
-
-	/* ... and analyse it. */
-	real_t maxKKTviolation;
-    analyser.getMaxKKTviolation( &example, maxKKTviolation );
-    printf( "maxKKTviolation: %e\n", maxKKTviolation );
-
-	QPOASES_TEST_FOR_TOL( maxKKTviolation,1e-15 );
-
-
-	/* Solve second QP ... */
-	nWSR = 10;
-	example.hotstart( H_new,g_new,A_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 );
-
-	/* ... and analyse it. */
-	analyser.getMaxKKTviolation( &example, maxKKTviolation );
-    printf( "maxKKTviolation: %e\n", maxKKTviolation );
-
-	QPOASES_TEST_FOR_TOL( maxKKTviolation,1e-15 );
-
-
-//  ------------ VARIANCE-COVARIANCE EVALUATION --------------------
-
-        real_t *Var              = new real_t[5*5];
-        real_t *Primal_Dual_Var  = new real_t[5*5];
-
-        int run1, run2;
-        for( run1 = 0; run1 < 5*5; run1++ )
-            Var[run1] = 0.0;
-
-        Var[0] = 1.0;
-        Var[6] = 1.0;
-
-//                  (  1   0   0   0   0   )
-//                  (  0   1   0   0   0   )
-//     Var     =    (  0   0   0   0   0   )
-//                  (  0   0   0   0   0   )
-//                  (  0   0   0   0   0   )
-
-
-        analyser.getVarianceCovariance( &example, Var,Primal_Dual_Var );
-
-        printf("\nPrimal_Dual_VAR = \n");
-        for( run1 = 0; run1 < 5; run1++ ){
-          for( run2 = 0; run2 < 5; run2++ ){
-            printf(" %10f", Primal_Dual_Var[run1*5+run2]);
-          }
-          printf("\n");
-        }
-
-        delete[] Primal_Dual_Var;
-        delete[] Var;
-
-		QPOASES_TEST_FOR_NEAR( Primal_Dual_Var[3*5+3], 26.0 );
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example4.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example4.cpp
deleted file mode 100644
index a4cf32983a0..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example4.cpp
+++ /dev/null
@@ -1,211 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_example4.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2009-2014
- *
- *	Very simple example for testing qpOASES (using the possibility to specify 
- *	user-defined constraint product function).
- */
-
-
-
-#include <stdlib.h>
-
-#include <qpOASES.hpp>
-#include "../../examples/example4CP.cpp"
-#include <qpOASES/UnitTesting.hpp>
-
-
-/**	Example for qpOASES main function using the possibility to specify 
- *	user-defined constraint product function. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	int i,j;
-
-	/* Setup data of first QP... */
-	real_t H[7*7];
-	real_t A[50*7];
-	real_t g[7];
-	real_t lbA[50];
-
-	/*	    ( 1.0 0.5 |                    )
-	 *	    ( 0.5 2.0 |                    )
-	 *	    ( --------+------------------- )
-	 *	H = (         | 1e-6               )
-	 *	    (         |      1e-6          )
-	 *	    (         |           ...      )
-	 *	    (         |               1e-6 ) */
-	for( i=0; i<7*7; ++i )
-		H[i] = 0.0;
-	for( i=2; i<7; ++i )
-		H[i*7+i] = 1.0e-6;
-	H[0] = 1.0;
-	H[1] = 0.5;
-	H[7] = 0.5;
-	H[8] = 2.0;
-
-	/*	    ( x.x x.x | 1.0             )
-	 *	    ( x.x x.x | ...             )
-	 *	    ( x.x x.x | 1.0             )
-	 *	    ( x.x x.x |     1.0         )
-	 *	A = ( x.x x.x |     ...         )
-	 *	    ( x.x x.x |     1.0         )
-	 *	    ( x.x x.x |         ...     )
-	 *	    ( x.x x.x |             1.0 )
-	 *	    ( x.x x.x |             ... )
-	 *	    ( x.x x.x |             1.0 ) */
-	for( i=0; i<50*7; ++i )
-		A[i] = 0.0;
-	for( i=0; i<50; ++i )
-	{
-		for( j=0; j<2; ++j )
-			A[i*7+j] = (real_t)rand() / (real_t)RAND_MAX;
-
-		A[i*7 + (i/10)+2] = 1.0;
-	}
-
-	/*	    ( -1.0 )
-	 *	    ( -0.5 )
-	 *	    ( ---- )
-	 *	g = (      )
-	 *	    (      )
-	 *	    (      )
-	 *	    (      ) */
-	for( i=0; i<7; ++i )
-		g[i] = 0.0;
-	g[0] = -1.0;
-	g[1] = -0.5;
-
-	for( i=0; i<50; ++i )
-		lbA[i] = 1.0;
-
-	/* ... and setting up user-defined constraint product function. */
-	MyConstraintProduct myCP( 7,50,A );
-
-
-	/* Setting up QProblem object and set construct product function. */
-	QProblem exampleCP( 7,50 );
-	exampleCP.setPrintLevel( PL_NONE );
-	
-	exampleCP.setConstraintProduct( &myCP );
-
-
-	/* Solve first QP. */
-	real_t cputime = 1.0;
-	int nWSR = 100;
-	exampleCP.init( H,g,A,0,0,lbA,0, nWSR,&cputime );
-
-
-	/* Solve second QP using a modified gradient. */
-	g[0] = -2.0;
-	g[1] =  0.5;
-
-	cputime = 1.0;
-	nWSR = 100;
-	exampleCP.hotstart( g,0,0,lbA,0, nWSR,&cputime );
-
-	/* Get and print solution of second QP. */
-	real_t xOptCP[7];
-	real_t yOptCP[7+50];
-	exampleCP.getPrimalSolution( xOptCP );
-	exampleCP.getDualSolution( yOptCP );
-	printf( "\nxOpt = [ %e, %e, %e ... ];  objVal = %e\n", xOptCP[0],xOptCP[1],xOptCP[2],exampleCP.getObjVal() );
-	printf( "CPU time:  %.3f microseconds\n\n", cputime*1.0e6 ); 
-
-	/* Compute KKT tolerances */
-	real_t stat, feas, cmpl;
-
-	getKKTResidual(	7,50,
-					H,g,A,0,0,lbA,0,
-					xOptCP,yOptCP,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-15 );
-	QPOASES_TEST_FOR_TOL( feas,1e-15 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
-	
-	
-	/* Do the same without specifying constraint product. */
-	QProblem example( 7,50 );
-	example.setPrintLevel( PL_NONE );
-	
-	/* Solve first QP. */
-	g[0] = -1.0;
-	g[1] = -0.5;
-	
-	cputime = 1.0;
-	nWSR = 100;
-	example.init( H,g,A,0,0,lbA,0, nWSR,&cputime );
-	
-	/* Solve second QP using a modified gradient. */
-	g[0] = -2.0;
-	g[1] =  0.5;
-
-	cputime = 1.0;
-	nWSR = 100;
-	example.hotstart( g,0,0,lbA,0, nWSR,&cputime );
-
-	/* Get and print solution of second QP. */
-	real_t xOpt[7];
-	real_t yOpt[7+50];
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-	printf( "\nxOpt = [ %e, %e, %e ... ];  objVal = %e\n", xOpt[0],xOpt[1],xOpt[2],example.getObjVal() );
-	printf( "CPU time:  %.3f microseconds\n\n", cputime*1.0e6 ); 
-
-	/* Compute KKT tolerances */
-	getKKTResidual(	7,50,
-					H,g,A,0,0,lbA,0,
-					xOpt,yOpt,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-15 );
-	QPOASES_TEST_FOR_TOL( feas,1e-15 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
-
-	for( int ii=0; ii<7; ++ii )
-		QPOASES_TEST_FOR_NEAR( xOptCP[ii],xOpt[ii] );
-
-	for( int ii=0; ii<7+50; ++ii )
-		QPOASES_TEST_FOR_NEAR( yOptCP[ii],yOpt[ii] );
-
-	QPOASES_TEST_FOR_NEAR( exampleCP.getObjVal(),example.getObjVal() );
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example5.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example5.cpp
deleted file mode 100644
index c7f919b8277..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example5.cpp
+++ /dev/null
@@ -1,204 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_example5.cpp
- *	\author Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2011-2014
- *
- *	Very simple example for testing qpOASES (using the possibility to
- *  compute the local linear feedback law)
- */
-
-
-
-#include <stdlib.h>
-
-#include <qpOASES.hpp>
-#include "../../examples/example4CP.cpp"
-#include <qpOASES/UnitTesting.hpp>
-
-
-/**	Example for qpOASES main function using the possibility to specify
- *	user-defined constraint product function. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	int i,j,jj;
-	real_t d = 0.0;
-
-	/* Setup data of first QP... */
-	real_t H[7*7];
-	real_t A[50*7];
-	real_t g[7];
-	real_t lbA[50];
-
-	/*	    ( 1.0 0.5 |                    )
-	 *	    ( 0.5 2.0 |                    )
-	 *	    ( --------+------------------- )
-	 *	H = (         | 1e-6               )
-	 *	    (         |      1e-6          )
-	 *	    (         |           ...      )
-	 *	    (         |               1e-6 ) */
-	for( i=0; i<7*7; ++i )
-		H[i] = 0.0;
-	for( i=2; i<7; ++i )
-		H[i*7+i] = 1.0e-6;
-	H[0] = 1.0;
-	H[1] = 0.5;
-	H[7] = 0.5;
-	H[8] = 2.0;
-
-	/*	    ( x.x x.x | 1.0             )
-	 *	    ( x.x x.x | ...             )
-	 *	    ( x.x x.x | 1.0             )
-	 *	    ( x.x x.x |     1.0         )
-	 *	A = ( x.x x.x |     ...         )
-	 *	    ( x.x x.x |     1.0         )
-	 *	    ( x.x x.x |         ...     )
-	 *	    ( x.x x.x |             1.0 )
-	 *	    ( x.x x.x |             ... )
-	 *	    ( x.x x.x |             1.0 ) */
-	for( i=0; i<50*7; ++i )
-		A[i] = 0.0;
-	for( i=0; i<50; ++i )
-	{
-		for( j=0; j<2; ++j )
-			A[i*7+j] = (real_t)rand() / (real_t)RAND_MAX;
-
-		A[i*7 + (i/10)+2] = 1.0;
-	}
-
-	/*	    ( -1.0 )
-	 *	    ( -0.5 )
-	 *	    ( ---- )
-	 *	g = (      )
-	 *	    (      )
-	 *	    (      )
-	 *	    (      ) */
-	for( i=0; i<7; ++i )
-		g[i] = 0.0;
-	g[0] = -1.0;
-	g[1] = -0.5;
-
-	for( i=0; i<50; ++i )
-		lbA[i] = 1.0;
-
-	/* ... and setting up user-defined constraint product function. */
-	MyConstraintProduct myCP( 7,50,A );
-
-
-	/* Setting up QProblem object and set construct product function. */
-	QProblem example( 7,50 );
-	example.setConstraintProduct( &myCP );
-
-
-	/* Solve first QP. */
-	real_t cputime = 1.0;
-	int nWSR = 100;
-	example.init( H,g,A,0,0,lbA,0, nWSR,&cputime );
-
-	/* Get and print solution of QP. */
-	real_t xOpt[7], yOpt[7+50];
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-
-
-	/* Compute local linear feedback law */
-	const int n_rhs = 7+7+50;
-	real_t g_in[7*n_rhs];
-	real_t b_in[7*n_rhs];
-	real_t bA_in[50*n_rhs];
-	real_t x_out[7*n_rhs];
-	real_t y_out[(7+50)*n_rhs];
-
-	int ii;
-	memset (g_in, 0, sizeof (g_in));
-	memset (b_in, 0, sizeof (b_in));
-	memset (bA_in, 0, sizeof (bA_in));
-
-	for ( ii = 0; ii < 7; ++ii )
-		g_in[ii*7 + ii] = 1.0;
-	for ( ii = 0; ii < 7; ++ii )
-		b_in[(ii+7)*7 + ii] = 1.0;
-	for ( ii = 0; ii < 50; ++ii )
-		bA_in[(ii+14)*50 + ii] = 1.0;
-
-	example.solveCurrentEQP ( n_rhs, g_in, b_in, b_in, bA_in, bA_in, x_out, y_out );
-
-	/* Verify validity of local feedback law by perturbation and hot starts */
-	real_t perturb = 1.0e-6;
-	real_t nrm = 0.0;
-	for ( ii = 0; ii < n_rhs; ++ii )
-	{
-		for ( jj = 0; jj < 7; ++jj )
-			g_in[ii*7 + jj] = g[jj] + g_in[ii*7+jj]*perturb;
-		for ( jj = 0; jj < 50; ++jj )
-			bA_in[ii*50 + jj] = lbA[jj] + bA_in[ii*50+jj]*perturb;
-
-		nWSR = 100;
-		example.hotstart( &g_in[ii*7],0,0,&bA_in[ii*50],0, nWSR, 0 );
-
-		real_t xPer[7], yPer[7+50];
-		example.getPrimalSolution( xPer );
-		example.getDualSolution( yPer );
-
-		for ( jj = 0; jj < 7; ++jj )
-		{
-			d = getAbs (x_out[ii*7+jj]*perturb - (xPer[jj]-xOpt[jj]) );
-			if (nrm < d) nrm=d;
-		}
-		for ( jj = 0; jj < 7+50; ++jj )
-		{
-			d = getAbs (y_out[ii*(7+50)+jj]*perturb - (yPer[jj]-yOpt[jj]) );
-			if (nrm < d) nrm=d;
-		}
-	}
-	printf ("Maximum perturbation over all directions: %e\n", nrm);
-
-	QPOASES_TEST_FOR_TOL( nrm,1e-15 );
-
-
-	/* // print feedback matrix
-	for (ii = 0; ii < n_rhs; ++ii)
-	{
-		printf ("x: ");
-		for (jj = 0; jj < 7; ++jj )
-			printf ("%8.2e ", x_out[ii*7+jj]);
-		printf (" y: ");
-		for (jj = 0; jj < 7+50; ++jj )
-			printf ("%8.2e ", y_out[ii*(7+50)+jj]);
-		printf("\n");
-	}
-*/
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example6.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example6.cpp
deleted file mode 100644
index 577aec123de..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example6.cpp
+++ /dev/null
@@ -1,112 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_example6.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Example that caused troubles in an earlier release.
- */
-
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-/** Example for qpOASES main function using the QProblem class. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	/* Setup data of first QP. */
-	real_t H[5*5] = {	1.224642131370767e+01, 2.908638763113702e+00, 0.0, 0.0, 0.0,
-						2.908638763113702e+00, 2.497106275003180e+00, 0.0, 0.0, 0.0,
-						0.0, 0.0, 1.0, 0.0, 0.0,
-						0.0, 0.0, 0.0, 5.158460640334052e-02, 4.723556059962540e-02,
-						0.0, 0.0, 0.0, 4.723556059962540e-02, 4.325317843302175e-02 };
-	real_t A[2*5] = { 	-1.404358970692652e+00, -2.556613491156063e+00, 3.202524559238066e+00, -1.0, 0.0,
-						6.587910295430314e-01, -5.349454475937998e-01, 4.391976356955536e-01, 0.0, -1.0 };
-	real_t g[5] = { 	2.474135331302147e+01,
-						5.857286430296258e+00,
-						2.359382646348721e-01,
-						1.721047069188781e-01,
-						1.575947337774199e-01 };
-	real_t lb[5] = { -10.0, -10.0, -10.0, -10.0, -10.0 };
-	real_t ub[5] = {  10.0,  10.0,  10.0,  10.0,  10.0 };
-	real_t lbA[2] = { 1.643135416077167e+00, 1.056813028189597e+00 };
-	real_t ubA[2] = { 1.643135416077167e+00, 1.056813028189597e+00 };
-
-	/* Setting up QProblem object. */
-	QProblem example( 5,2 );
-
-	Options options;
- 	//options.enableFlippingBounds = BT_FALSE;
-	//options.enableEqualities = BT_TRUE;
-	//options.initialStatusBounds = ST_INACTIVE;
-	example.setOptions( options );
-	example.setPrintLevel( PL_NONE );
-	
-	/* Solve first QP. */
-	returnValue retVal;
-	int simpleStatus = -1;
-
-	int nWSR = 10;
-	retVal = example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
-	simpleStatus = getSimpleStatus( retVal,BT_TRUE );
-
-	QPOASES_TEST_FOR_TRUE( simpleStatus == 0 );
-
-
-	/* Get and print solution of second QP. */
-	real_t xOpt[5];
-	real_t yOpt[5+2];
-	
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-	printf( "\nxOpt = [ %e, %e, ... ];  objVal = %e\n\n", xOpt[0],xOpt[1],example.getObjVal() );
-	
-	/* Compute KKT tolerances */
-	real_t stat, feas, cmpl;
-
-	getKKTResidual(	5,2,
-					H,g,A,lb,ub,lbA,ubA,
-					xOpt,yOpt,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-14 );
-	QPOASES_TEST_FOR_TOL( feas,1e-14 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example7.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example7.cpp
deleted file mode 100644
index a9a7bfc3d4f..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_example7.cpp
+++ /dev/null
@@ -1,90 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_example7.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Example that caused troubles in an earlier release.
- */
-
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	real_t H[5*5] = {0.8514828085899353, -0.15739890933036804, -0.081726007163524628, -0.530426025390625, 0.16773293912410736, -0.15739890933036804, 1.1552412509918213, 0.57780224084854126, -0.0072606131434440613, 0.010559185408055782, -0.081726007163524628, 0.57780224084854126, 0.28925251960754395, 5.324830453901086e-006, -3.0256599075073609e-006, -0.530426025390625, -0.0072606131434440613, 5.324830453901086e-006, 0.35609596967697144, -0.15124998986721039, 0.16773293912410736, 0.010559185408055782, -3.0256599075073609e-006, -0.15124998986721039, 0.15129712224006653};
-	real_t g[5] = {0.30908384919166565, 0.99325823783874512, 0.49822014570236206, -0.26309865713119507, 0.024296050891280174};
-	real_t A[5*5] =   {1,0,0,0,0,
-					   0,1,0,0,0,
-					   0,0,1,0,0,              
-					   0,0,0,1,0,
-					   0,0,0,0,1};
-	real_t lb[5] = {-0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359938621520996};
-	real_t ub[5] = { 0.052359879016876221, 0.052359879016876221, 0.052359879016876221, 0, 0};
-	real_t lbA[5] = {-0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359938621520996};
-	real_t ubA[5] = {0.052359879016876221, 0.052359879016876221, 0.052359879016876221, 0, 0};
-
-	/* Setting up QProblem object. */
-	QProblem example( 5,5 );
-
-	/* Solve first QP. */
-	int nWSR = 100;
-	returnValue retVal = example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
-	printf( "nWSR = %d,  retVal = %d (%s)\n", nWSR,retVal,getGlobalMessageHandler( )->getErrorCodeMessage(retVal) );
-	
-	
-	real_t sol[5]    = {0};
-	real_t yOpt[5+5] = {0};
-	example.getPrimalSolution(sol);
-	example.getDualSolution(yOpt);
-	
-	printf("l1 = %f, l2 = %f, l3 = %f, l4 = %f, l5 = %f\n",(float) lb[0], (float) lb[1], (float) lb[2], (float) lb[3], (float) lb[4]);
-	printf("x1 = %f, x2 = %f, x3 = %f, x4 = %f, x5 = %f\n",(float) sol[0], (float) sol[1], (float) sol[2], (float) sol[3], (float) sol[4]);
-	printf("u1 = %f, u2 = %f, u3 = %f, u4 = %f, u5 = %f\n",(float) ub[0], (float) ub[1], (float) ub[2], (float) ub[3], (float) ub[4]);
-
-	/* Compute KKT tolerances */
-	real_t stat, feas, cmpl;
-
-	getKKTResidual(	5,2,
-					H,g,A,lb,ub,lbA,ubA,
-					sol,yOpt,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-15 );
-	QPOASES_TEST_FOR_TOL( feas,1e-15 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
-
-	return TEST_PASSED;
-}
-
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_exampleLP.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_exampleLP.cpp
deleted file mode 100644
index e4a1681e4b4..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_exampleLP.cpp
+++ /dev/null
@@ -1,121 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_exampleLP.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2008-2014
- *
- *	Very simple example for solving a LP sequence using qpOASES.
- */
-
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-/** Example for qpOASES main function solving LPs. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	real_t tol = EPS;
-
-	/* Setup data of first LP. */
-	real_t A[1*2] = { 1.0, 1.0 };
-	real_t g[2] = { 1.5, 1.0 };
-	real_t lb[2] = { 0.5, -2.0 };
-	real_t ub[2] = { 5.0, 2.0 };
-	real_t lbA[1] = { -1.0 };
-	real_t ubA[1] = { 2.0 };
-
-	/* Setup data of second LP. */
-	real_t g_new[2] = { 1.0, 1.5 };
-	real_t lb_new[2] = { 0.0, -1.0 };
-	real_t ub_new[2] = { 5.0, -0.5 };
-	real_t lbA_new[1] = { -2.0 };
-	real_t ubA_new[1] = { 1.0 };
-
-
-	/* Setting up QProblem object with zero Hessian matrix. */
-	QProblem example( 2,1,HST_ZERO );
-
-	Options options;
- 	//options.setToMPC(); tol = 1e-12;
-	example.setOptions( options );
-
-	/* Solve first LP. */
-	int nWSR = 10;
-	example.init( 0,g,A,lb,ub,lbA,ubA, nWSR,0 );
-
-	real_t xOpt[2];
-	real_t yOpt[2+1];
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-
-	/* Compute KKT tolerances */
-	real_t stat, feas, cmpl;
-
-	getKKTResidual(	2,1,
-					0,g,A,lb,ub,lbA,ubA,
-					xOpt,yOpt,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,tol );
-	QPOASES_TEST_FOR_TOL( feas,tol );
-	QPOASES_TEST_FOR_TOL( cmpl,tol );
-
-
-	/* Solve second LP. */
-	nWSR = 10;
-	example.hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 );
-
-
-	/* Get and print solution of second LP. */
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-	printf( "\nxOpt = [ %e, %e ];  objVal = %e\n\n", xOpt[0],xOpt[1],example.getObjVal() );
-
-	getKKTResidual(	2,1,
-					0,g_new,A,lb_new,ub_new,lbA_new,ubA_new,
-					xOpt,yOpt,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,tol );
-	QPOASES_TEST_FOR_TOL( feas,tol );
-	QPOASES_TEST_FOR_TOL( cmpl,tol );
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_gradientShift.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_gradientShift.cpp
deleted file mode 100644
index d4d8cc81897..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_gradientShift.cpp
+++ /dev/null
@@ -1,125 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2009 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_gradientShift.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Simple test case which caused troubles in version 2.0.
- */
-
- 
-
-#include <math.h>
-#include <iostream>
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-/** Running simple test case which caused troubles in version 2.0. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	
-	double H[2*2] 	= { 0.055944055944055944,0, 0, 0 };
-	double A[1*2] 	= { 0.70514808036997589, -1 };
-	//double g[2]	= { -15.830543073928741, 0};
-	double g[2]		= { 0, 0 };
-	double lb[2]	= { 137.00242299940646, 154.0 };
-	double ub[2]	= { 282.19008595111382, 198.98579740786641};
-	double lbA[1]	= {0.0};
-	double ubA[1]	= {0.0};
-	
-	double gStart=-16.4;
- 
-	/* Setting up QProblem object. */
-	QProblem example( 2,1 );
-	Options options;
-	options.setToMPC();
-	options.printLevel = REFER_NAMESPACE_QPOASES PL_NONE;
-	example.setOptions( options );
-	
-	returnValue ret;
-	int nWSR = 10;
-	double Xopt[2]={0.0,0.0};
-	//fprintf(stdFile, "g[0]\t,\tReturn code\n");
-	int errorCount=0;
-	int i=0;
-	double granularity=0.00001;
-	g[0]=gStart;
-	for( i=0; i< 70000; i++)
-	{
-
-		g[0]=g[0]+granularity;
-		/* Solve first QP. */
-		nWSR = 10;
-		ret= example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
-		if (ret != SUCCESSFUL_RETURN)
-		{
-			//fprintf(stdFile, "%f\t,\t%d\n",g[0],ret);
-			errorCount++;
-		}
-		//fprintf(stdFile, "%f\t,\t%d\n",g[0],ret);
-	}
-	example.printProperties();
-	fprintf(stdFile, "#Number of optimizer runs: %d\n",i);
-	fprintf(stdFile, "#g[0] test interval: %f < g[0] < %f\n",gStart,g[0]);
-	fprintf(stdFile, "#Granularity: %f\n",granularity);
-	double errorPercent = double(errorCount)/double(i)*100.0;
-	fprintf(stdFile, "#Number of errors (error): %d (%f)\n",errorCount,errorPercent);
-
-	example.getPrimalSolution(Xopt);
-	fprintf(stdFile,"#Optimization primary result : LD=%f BD=%f\n",Xopt[0], Xopt[1]);
-
-	double Yopt[3]={0.0,0.0,0.0};
-	example.getDualSolution(Yopt);
-	fprintf(stdFile,"#Optimization dual result : %f %f %f8\n",Yopt[0], Yopt[1], Yopt[2]);
-
-	int Nc=0;
-	Nc=example.getNC();
-	fprintf(stdFile,"#Number of constraints : %d\n",Nc);
-
-	int Nec=0;
-	Nec=example.getNEC();
-	fprintf(stdFile,"#Number of equality constraints : %d\n",Nec);
-
-	int Nac=0;
-	Nac=example.getNAC();
-	fprintf(stdFile,"#Number of active constraints : %d\n",Nac);
-
-	int Niac=0;
-	Niac=example.getNIAC();
-	fprintf(stdFile,"#Number of inactive constraints : %d\n",Niac);
-
-	QPOASES_TEST_FOR_TRUE( errorCount == 0 )
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_guessedWS1.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_guessedWS1.cpp
deleted file mode 100644
index 04de2a710eb..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_guessedWS1.cpp
+++ /dev/null
@@ -1,164 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_guessedWS1.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2014
- *
- *	Very simple example for testing qpOASES (using QProblem class).
- */
-
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-/** Example for qpOASES main function using the QProblem class. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	/* Setup data of first QP. */
-	real_t H[4*4] = { 1.0, 0.0, 0.0, 0.5, 
-	                  0.0, 1.0, 0.0, 0.0,
-	                  0.0, 0.0, 1.0, 0.0,
-	                  0.5, 0.0, 0.0, 1.0 };
-	real_t A[3*4] = { 1.0, 1.0, 0.0, 0.0,
-	                  1.0, 1.0, 1.0, 0.0,
-	                  1.0, 1.0, 1.0, 1.0 };
-	real_t g[4] = { 1.5, 1.0, -1.0, -1.0 };
-	real_t lb[4] = { 0.5, -2.0, 0.0, 0.0 };
-	real_t ub[4] = { 1.0, 2.0, 1.0, 0.5 };
-	real_t lbA[3] = { -1.0, -1.0, -1.0 };
-	real_t ubA[3] = { 0.0, 0.25, 1.0 };
-
-	
-
-	/* Setting up QProblem object. */
-	QProblem example( 4,3 );
-
-	Options options;
-	example.setOptions( options );
-
-	/* Solve first QP. */
-	int nWSR = 10;
-	example.init( H,g,A,lb,ub,lbA,ubA, nWSR );
-
-	/* Get and print solution of second QP. */
-	real_t xOpt[4];
-	real_t yOpt[4+3];
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-	print( xOpt,4,"xOpt" );
-	print( yOpt,4+3,"yOpt" );
-	printf( "objVal = %e\n\n", example.getObjVal() );
-	
-	/* Compute KKT tolerances */
-	real_t stat, feas, cmpl;
-
-	getKKTResidual(	4,3,
-					H,g,A,lb,ub,lbA,ubA,
-					xOpt,yOpt,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-15 );
-	QPOASES_TEST_FOR_TOL( feas,1e-15 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
-
-
-	/* Solve first QP again (with optimal guess for working set). */
-	Bounds prevBounds;
-	Constraints prevConstraints;
-	
-	example.getBounds( prevBounds );
-	example.getConstraints( prevConstraints );
-
-	nWSR = 10;
-	example.hotstart( g,lb,ub,lbA,ubA, nWSR,0,&prevBounds,&prevConstraints );
-
-	/* Get and print solution of second QP. */
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-	print( xOpt,4,"xOpt" );
-	print( yOpt,4+3,"yOpt" );
-	printf( "objVal = %e\n\n", example.getObjVal() );
-	
-	/* Compute KKT tolerances */
-	getKKTResidual(	4,3,
-					H,g,A,lb,ub,lbA,ubA,
-					xOpt,yOpt,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-15 );
-	QPOASES_TEST_FOR_TOL( feas,1e-15 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
-
-
-	/* Solve first QP again (with inaccurate guess for working set). */
-	prevBounds.print();
-	prevBounds.rotate(1);
-	//prevBounds.moveFixedToFree(0);
-	prevBounds.print();
-
-	prevConstraints.print();
-	//prevConstraints.moveInactiveToActive(0,ST_LOWER);
-	prevConstraints.moveActiveToInactive(1);
-	prevConstraints.print();
-
-	nWSR = 10;
-	example.hotstart( g,lb,ub,lbA,ubA, nWSR,0,&prevBounds,&prevConstraints );
-
-	/* Get and print solution of second QP. */
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-	print( xOpt,4,"xOpt" );
-	print( yOpt,4+3,"yOpt" );
-	printf( "objVal = %e\n\n", example.getObjVal() );
-	
-	/* Compute KKT tolerances */
-	getKKTResidual(	4,3,
-					H,g,A,lb,ub,lbA,ubA,
-					xOpt,yOpt,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-15 );
-	QPOASES_TEST_FOR_TOL( feas,1e-15 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_indexlist.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_indexlist.cpp
deleted file mode 100644
index 552dfc2a115..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_indexlist.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_indexlist.cpp
- *	\author Andreas Potschka, Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2010-2014
- *
- *	Unit test for Indexlist class.
- */
-
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-/** Test Indexlist sorting */
-int main()
-{
-	USING_NAMESPACE_QPOASES
-
-	Indexlist il(10);
-	int i, *numbers;
-
-	il.addNumber(1);
-	il.addNumber(3);
-	il.addNumber(5);
-	il.addNumber(2);
-	il.addNumber(4);
-	il.addNumber(0);
-	il.addNumber(7);
-	il.addNumber(6);
-	il.addNumber(8);
-	il.addNumber(9);
-
-	il.getNumberArray(&numbers);
-	fprintf(stdFile, "Unsorted numbers: ");
-	for (i = 0; i < 10; i++)
-		fprintf(stdFile, " %2d", numbers[i]);
-	fprintf(stdFile, "\n");
-
-	fprintf(stdFile, "Unsorted index of number 0: %3d\n", il.getIndex(0));
-
-	QPOASES_TEST_FOR_TRUE( il.getIndex(0) == 5 )
-
-	il.removeNumber(5);
-	fprintf(stdFile, "Unsorted index of (removed) number 5: %3d\n", il.getIndex(5));
-
-	QPOASES_TEST_FOR_TRUE( il.getIndex(5) == -1 )
-
-	il.getNumberArray(&numbers);
-	fprintf(stdFile, "Unsorted numbers: ");
-	for (i = 0; i < 9; i++)
-		fprintf(stdFile, " %2d", numbers[i]);
-	fprintf(stdFile, "\n");
-
-	il.swapNumbers(2, 7);
-
-	il.getNumberArray(&numbers);
-	fprintf(stdFile, "Unsorted numbers: ");
-	for (i = 0; i < 9; i++)
-		fprintf(stdFile, " %2d", numbers[i]);
-	fprintf(stdFile, "\n");
-
-	QPOASES_TEST_FOR_TRUE( numbers[2] == 7 )
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_janick1.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_janick1.cpp
deleted file mode 100644
index 39cc744baeb..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_janick1.cpp
+++ /dev/null
@@ -1,197 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file debugging/cpp/debug_janick1.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Example that causes troubles when hotstarting.
- */
-
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-#include <stdio.h>
-
-
-//#define __MAKE_POS_DEF__
-#undef __MAKE_POS_DEF__
-
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	int nWSR = 100;
-	/* Setting up QProblem object. */
-	SQProblem example( 11,3 );
-
-	Options options;
-	options.setToFast();
-	//options.setToDefault();
-	//options.initialStatusBounds = qpOASES::ST_INACTIVE;
-	example.setOptions( options );
-
-
-	/* Setup data of first QP. */
-	real_t H[11*11] = {
-	6.20100988531485e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-3.84861756786704e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-7.43268431723266e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	0.00000000000000e+00,	 1.00000000000000e-01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	-3.84861756786704e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 5.41188294952735e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 4.61304826562310e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.10000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 1.00000000000000e-01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.01000000000000e+01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.10000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	-7.43268431723266e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 4.61304826562310e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.73544778892019e+01,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+00,	 0.00000000000000e+00,
-	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+01
-	};
-	real_t g[11] =	{
-	-1.28196485091041e-07,	 1.13322235104306e-08,	 4.52417455660281e-06,	 1.95881304363781e-07,	 1.61991589634459e-06,	 2.69786724710014e-09,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.18185813270785e+02,	 9.58903285459640e-06,	 3.82276791847511e-06
-	};
-	real_t zLow[11] =	{
-	-1.07876236159966e+01,	-1.00000000002784e+12,	-1.00000000000000e+12,	 0.00000000000000e+00,	-7.00000000000000e+00,	-2.60479553037772e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-4.50000000000000e+01,	-1.00000000000000e+12
-	};
-	real_t zUpp[11] =	{
-	9.99999999989212e+11,	 9.99999999972157e+11,	 1.00000000000000e+12,	 4.68471854329546e+01,	 7.00000000000000e+00,	 9.99999999997395e+11,	 1.00000000000000e+12,	 1.00000000000000e+12,	 0.00000000000000e+00,	 4.50000000000000e+01,	 1.00000000000000e+12
-	};
-	real_t D[11*3] =	{
-	1.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.00000000000000e-02,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	-1.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-1.00000000000000e-02,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,
-	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 1.00000000000000e+00,	 0.00000000000000e+00,	-1.00000000000000e-02,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00
-	};
-	real_t dLow[3] =	{
-	-1.00000000000000e+12,	-1.00000000000000e+12,	-1.00000000000000e+12
-	};
-	real_t dUpp[3] =	{
-	2.12376384003361e-01,	 4.78762361599664e+00,	 8.95204469622285e-01
-	};
-
-	#ifdef __MAKE_POS_DEF__
-	H[9*11+9] += 30;
-	#endif
-	returnValue status = example.init( H,g,D,zLow,zUpp,dLow,dUpp, nWSR );
-	printf("\nqpOASES_status = %d\n", (int)status );
-
-	/* Get and print solution of first QP. */
-	real_t xOpt[11];
-	real_t yOpt[11+3];
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-	printf("first QP:\n");
-	for (int ii =0; ii<11; ++ii )	{
-		printf("x[%d] = %.3e\n", ii, xOpt[ii]);
-	}
-
-	/* Compute KKT tolerances */
-	real_t stat, feas, cmpl;
-
-	getKKTResidual(	11,3,
-					H,g,D,zLow,zUpp,dLow,dUpp,
-					xOpt,yOpt,
-					stat,feas,cmpl
-					);
-	printf( "\nstat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-9 );
-	QPOASES_TEST_FOR_TOL( feas,1e-7 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
-
-
-
-	nWSR = 100;
-
-	/* Setup data of second QP. */
-	real_t H2[11*11] = {
-	6.20100988531485e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-3.84861756786704e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-7.43268433147671e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	0.00000000000000e+00,	 1.00000000000000e-01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	-3.84861756786704e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 5.41188294952735e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 4.61304827446359e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.10000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 1.00000000000000e-01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.01000000000000e+01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.10000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	-7.43268433147671e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 4.61304827446359e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.73544778550554e+01,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+00,	 0.00000000000000e+00,
-	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+01
-	};
-	real_t g2[11] =	{
-	-6.57752219998813e-04,	 3.61759517784935e-04,	-7.51224940044046e-05,	 6.01232615232452e-04,	 1.23562395380546e-04,	 4.59040118744990e-05,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.18185813270785e+02,	 3.38813178901720e-21,	 0.00000000000000e+00
-	};
-	real_t zLow2[11] =	{
-	-2.41857058824735e-08,	-2.77410094895458e-10,	-9.90563843681406e-09,	-3.85694249871449e-08,	 4.50633213817586e-14,	-2.22044604925031e-15,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-4.49999952054836e+01,	-1.00000000000000e+12
-	};
-	real_t zUpp2[11] =	{
-	-2.41857058824735e-08,	-2.77410094895458e-10,	-9.90563843681406e-09,	-3.85694249871449e-08,	 4.50633213817586e-14,	-2.22044604925031e-15,	 1.00000000000000e+12,	 1.00000000000000e+12,	 0.00000000000000e+00,	 4.50000047945164e+01,	 1.00000000000000e+12,
-	};
-	real_t D2[11*3] =	{
-	1.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.00000000000000e-02,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	-1.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-1.00000000000000e-02,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,
-	0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 1.00000000000000e+00,	 0.00000000000000e+00,	-1.00000000000000e-02,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00
-	};
-	real_t dLow2[3] =	{
-	-1.00000000000000e+12,	-1.00000000000000e+12,	-1.00000000000000e+12
-	};
-	real_t dUpp2[3] =	{
-	2.12376363329877e-01,	 4.78762363667012e+00,	 8.95204496600957e-01
-	};
-	#ifdef __MAKE_POS_DEF__
-	H2[9*11+9] += 30;
-	#endif
-
-
-	status = example.hotstart( H2,g2,D2,zLow2,zUpp2,dLow2,dUpp2, nWSR );
-	printf("qpOASES_status = %d\n", (int)status );
-
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-	printf("second QP:\n");
-	for (int ii =0; ii<11; ++ii )	{
-		printf("x[%d] = %.3e\n", ii, xOpt[ii]);
-	}
-
-	/* Compute KKT tolerances */
-	getKKTResidual(	11,3,
-					H2,g2,D2,zLow2,zUpp2,dLow2,dUpp2,
-					xOpt,yOpt,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-9 );
-	QPOASES_TEST_FOR_TOL( feas,1e-7 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-12 );
-
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_janick2.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_janick2.cpp
deleted file mode 100644
index e25a032c4a8..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_janick2.cpp
+++ /dev/null
@@ -1,251 +0,0 @@
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-#include <stdio.h>
-
-#define __MAKE_POS_DEF__
-// #undef __MAKE_POS_DEF__
-
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	int nWSR = 100;
-	/* Setting up QProblem object. */
-	SQProblem example( 11,3 );
-
-	Options options;
-	options.setToFast();
-// 	options.setToDefault();
-	options.initialStatusBounds = REFER_NAMESPACE_QPOASES ST_INACTIVE;
-
-	//options.terminationTolerance = 1.e-12;
-	options.initialStatusBounds = REFER_NAMESPACE_QPOASES ST_INACTIVE;
-	//options.enableFarBounds = REFER_NAMESPACE_QPOASES BT_FALSE;
-	//options.enableRegularisation = REFER_NAMESPACE_QPOASES BT_FALSE;
-
-	example.setOptions( options );
-
-
-	/* Setup data of first QP. */
-	real_t H[11*11] = {
-	 6.20100988531485e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-3.84861756786704e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-7.43268431723266e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	 0.00000000000000e+00,	 1.00000000000000e-01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	-3.84861756786704e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 5.41188294952735e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 4.61304826562310e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.10000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 1.00000000000000e-01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.01000000000000e+01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.10000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	-7.43268431723266e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 4.61304826562310e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.73544778892019e+01,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+00,	 0.00000000000000e+00,
-	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+01,
-	};
-	real_t g[11] =	{
-//	 9.13378607947379e-07,
-//	 0.00000000000000e+00,
-//	 0.00000000000000e+00,
-//	-1.12448469735682e-06,
-//	 0.00000000000000e+00,
-//	 0.00000000000000e+00,
-//	 0.00000000000000e+00,
-//	 0.00000000000000e+00,
-//	-1.18185650936822e+02,
-//	 0.00000000000000e+00,
-//	 0.00000000000000e+00,
-	 -6.93766478421491e-04,
-	  3.84943289898669e-04,
-	 -3.63779116055460e-05,
-	  6.38114176725135e-04,
-	  1.85797765355698e-04,
-	  6.21922122437904e-05,
-	  0.00000000000000e+00,
-	  0.00000000000000e+00,
-	 -1.18185758699839e+02,
-	  1.54357580390960e-05,
-	  5.39852809009711e-06,
-	};
-	real_t zLow[11] =	{
-	 0.00000000000000e+00,
-	 0.00000000000000e+00,
-	 0.00000000000000e+00,
-	 0.00000000000000e+00,
-	 0.00000000000000e+00,
-	 0.00000000000000e+00,
-	 0.00000000000000e+00,
-	 0.00000000000000e+00,
-	 0.00000000000000e+00,
-	-4.50000000000000e+01,
-	-1.00000000000000e+12,
-	};
-	real_t zUpp[11] =	{
-	 0.00000000000000e+00,
-	 0.00000000000000e+00,
-	 0.00000000000000e+00,
-	 0.00000000000000e+00,
-	 0.00000000000000e+00,
-	 0.00000000000000e+00,
-	 1.00000000000000e+12,
-	 1.00000000000000e+12,
-	 0.00000000000000e+00,
-	 4.50000000000000e+01,
-	 1.00000000000000e+12,
-	};
-	real_t D[11*3] =	{
-	 1.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.00000000000000e-02,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	-1.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-1.00000000000000e-02,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,
-	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 1.00000000000000e+00,	 0.00000000000000e+00,	-1.00000000000000e-02,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	};
-	real_t dLow[3] =	{
-	-1.00000000000000e+12,
-	-1.00000000000000e+12,
-	-1.00000000000000e+12,
-	};
-	real_t dUpp[3] =	{
-	 2.12376384003361e-01,
-	 4.78762361599664e+00,
-	 8.95204469622285e-01,
-	};
-
-
-	#ifdef __MAKE_POS_DEF__
-// 	H[9*11+9] += 30;
-	H[8*11+8] += 30;
-	#endif
-	returnValue status = example.init( H,g,D,zLow,zUpp,dLow,dUpp, nWSR );
-	printf("qpOASES_status = %d\n", (int)status );
-
-	/* Get and print solution of second QP. */
-	real_t xOpt[11];
-	real_t yOpt[11+3];
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-	printf("first QP:\n");
-	for (int ii =0; ii<11; ++ii )	{
-		printf("x[%d] = %.3e\n", ii, xOpt[ii]);
-	}
-
-	/* Compute KKT tolerances */
-	real_t stat, feas, cmpl;
-
-	getKKTResidual( 11,3,
-			H,g,D,zLow,zUpp,dLow,dUpp,
-			xOpt,yOpt,
-			stat,feas,cmpl
-	);
-	printf( "\nstat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-9 );
-	QPOASES_TEST_FOR_TOL( feas,1e-7 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
-
-	nWSR = 100;
-
-	/* Setup data of second QP. */
-	real_t H2[11*11] = {
-	 6.20101055067033e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-3.84861780549400e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-7.43268533746787e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	 0.00000000000000e+00,	 1.00000000000000e-01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	-3.84861780549400e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 5.41188396792859e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 4.61304896387257e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.10000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 1.00000000000000e-01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.01000000000000e+01,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.10000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	-7.43268533746787e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 4.61304896387257e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.73544780086860e+01,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+00,	 0.00000000000000e+00,
-	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 2.00000000000000e+01,
-	};
-	real_t g2[11] =	{
-	-8.92227256391600e-08,
-	 6.89531726031141e-08,
-	-1.91970120006650e-07,
-	 1.77206607789402e-07,
-	-3.83145267945144e-09,
-	-1.88284265021358e-08,
-	 0.00000000000000e+00,
-	 0.00000000000000e+00,
-	-1.18185657392775e+02,
-	 1.45337027424899e-17,
-	-6.04156175796480e-20,
-	};
-	real_t zLow2[11] =	{
-	-1.07876236566374e+01,
-	-1.00000000002784e+12,
-	-1.00000000000000e+12,
-	-8.30554585107279e-08,
-	-7.00000003695781e+00,
-	-2.60479531522807e+00,
-	 0.00000000000000e+00,
-	 0.00000000000000e+00,
-	 0.00000000000000e+00,
-	-4.50000000018062e+01,
-	-1.00000000000000e+12,
-	};
-	real_t zUpp2[11] =	{
-	 9.99999999989212e+11,
-	 9.99999999972157e+11,
-	 1.00000000000000e+12,
-	 4.68471853498991e+01,
-	 6.99999996304219e+00,
-	 9.99999999997395e+11,
-	 1.00000000000000e+12,
-	 1.00000000000000e+12,
-	 0.00000000000000e+00,
-	 4.49999999981938e+01,
-	 1.00000000000000e+12,
-	};
-	real_t D2[11*3] =	{
-	 1.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	-1.00000000000000e-02,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	-1.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-1.00000000000000e-02,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,	-0.00000000000000e+00,
-	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,	 1.00000000000000e+00,	 0.00000000000000e+00,	-1.00000000000000e-02,	 0.00000000000000e+00,	 0.00000000000000e+00,	 0.00000000000000e+00,
-	};
-	real_t dLow2[3] =	{
-	-1.00000000000000e+12,
-	-1.00000000000000e+12,
-	-1.00000000000000e+12,
-	};
-	real_t dUpp2[3] =	{
-	 2.12376343362616e-01,
-	 4.78762365663739e+00,
-	 8.95204684771929e-01,
-	};
-	#ifdef __MAKE_POS_DEF__
-	H2[8*11+8] += 30;
-// 	H2[9*11+9] += 30;
-	#endif
-
-
-	status = example.hotstart( H2,g2,D2,zLow2,zUpp2,dLow2,dUpp2, nWSR );
-	printf("qpOASES_status = %d\n", (int)status );
-
-	example.getPrimalSolution( xOpt );
-	example.getDualSolution( yOpt );
-	printf("second QP:\n");
-	for (int ii =0; ii<11; ++ii )	{
-		printf("x[%d] = %.3e\n", ii, xOpt[ii]);
-	}
-	
-	printf( "\nQP objective value: %.3e\n", example.getObjVal() );
-
-	/* Compute KKT tolerances */
-
-	getKKTResidual( 11,3,
-			H2,g2,D2,zLow2,zUpp2,dLow2,dUpp2,
-			xOpt,yOpt,
-			stat,feas,cmpl
-	);
-	printf( "\nstat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl );
-
-	QPOASES_TEST_FOR_TOL( stat,1e-9 );
-	QPOASES_TEST_FOR_TOL( feas,1e-7 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-15 );
-
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices.cpp
deleted file mode 100644
index 1d5e0cb5607..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices.cpp
+++ /dev/null
@@ -1,903 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_matrices.cpp
- *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2010-2014
- *
- *	Unit test for Matrix classes.
- */
-
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-USING_NAMESPACE_QPOASES
-
-
-/** Compare deviations when computing dot product. */
-int sumOfSquares()
-{
-	int i;
-
-	/* sum of first n squares */
-	const int N = 100;
-	real_t *av = new real_t[N];
-	real_t *aTv = new real_t[N];
-	real_t *bv = new real_t[N];
-	real_t c;
-
-	for (i = 0; i < N; i++) av[i] = i+1.0;
-	for (i = 0; i < N; i++) aTv[i] = i+1.0;
-	for (i = 0; i < N; i++) bv[i] = i+1.0;
-
-	DenseMatrix a(1, N, N, av);
-	DenseMatrix aT(N, 1, 1, aTv);
-
-	a.times(1, 1.0, bv, N, 0.0, &c, 1);
-	real_t err = c - (1.0/6.0)*N*(N+1)*(2*N+1);
-	fprintf(stdFile, "Dot product; Error in sum of first %d squares: %9.2e\n", N, err );
-	
-	aT.transTimes(1, 1.0, bv, N, 0.0, &c, 1);
-	real_t errT = c - (1.0/6.0)*N*(N+1)*(2*N+1);
-	fprintf(stdFile, "Transpose; Error in sum of first %d squares: %9.2e\n", N, errT);
-
-	delete[] bv;
-	aT.free ();	// or delete[] aTv;
-	a.free (); // or delete[] av;
-
-	QPOASES_TEST_FOR_TOL( err ,1e-10 )
-	QPOASES_TEST_FOR_TOL( errT,1e-10 )
-	
-	return TEST_PASSED;
-}
-
-
-/** Compare deviations when multiplying Hilbert matrix with its inverse. */
-int hilbert()
-{
-	int i, j;
-	real_t d, err;
-
-	/* permuted 4x4 Hilbert matrix, row major format */
-	real_t _Av[] = {1.0/3.0, 1.0, 0.5, 0.25,
-		0.25, 0.5, 1.0/3.0, 0.2,
-		0.2, 1.0/3.0, 0.25, 1.0/6.0,
-		1.0/6.0, 0.25, 0.2, 1.0/7.0};
-	/* and its inverse, column major format */
-	real_t Bv[] = {240, 16, -120, -140,
-		-2700, -120, 1200, 1680,
-		6480, 240, -2700, -4200,
-		-4200, -140, 1680, 2800};
-
-	/* result */
-	real_t *Av = new real_t[4*4];
-	real_t *Cv = new real_t[4*4];
-
-	DenseMatrix A(4, 4, 4, Av);
-
-	for (i = 0; i < 16; i++) Av[i] = _Av[i];
-
-	A.times(4, 1.0, Bv, 4, 0.0, Cv, 4);
-
-	err = 0.0;
-	for (j = 0; j < 4; j++)
-	{
-		for (i = 0; i < 4; i++)
-		{
-			d = getAbs(Cv[j*4+i] - static_cast<real_t>(i == j));
-			if (d > err) err = d;
-		}
-	}
-	fprintf(stdFile, "Hilbert; Deviation from identity: %9.2e\n", err);
-
-	delete[] Cv;
-	A.free ();	// or delete[] Av;
-
-	QPOASES_TEST_FOR_TOL( err,1e-12 )
-	
-	return TEST_PASSED;
-}
-
-
-/** Compare deviations when multiplying sub-matrices. */
-int submatrix()
-{
-	int i, j;
-	real_t d, err;
-
-	/* 2x3 transposed submatrix */
-	real_t _Asubv[] = {1.0/3.0, 0.25,
-		1.0, 0.5,
-		0.5, 1.0/3.0,
-		0.25, 0.2};
-	real_t Bsubv[] = {240, 16, -120, -140,
-		-2700, -120, 1200, 1680};
-	real_t Csubv[2*2];
-
-	real_t *Asubv = new real_t[2*4];
-
-	for (i = 0; i < 8; i++) Asubv[i] = _Asubv[i];
-
-	DenseMatrix Asub(4, 2, 2, Asubv);
-
-	Asub.transTimes(2, 1.0, Bsubv, 4, 0.0, Csubv, 2);
-
-	err = 0.0;
-	for (j = 0; j < 2; j++)
-	{
-		for (i = 0; i < 2; i++)
-		{
-			d = getAbs(Csubv[j*2+i] - static_cast<real_t>(i == j));
-			if (d > err) err = d;
-		}
-	}
-	fprintf(stdFile, "Submatrix transpose; Deviation from identity: %9.2e\n", err);
-	
-	Asub.free ();	// or delete[] Asubv;
-
-	QPOASES_TEST_FOR_TOL( err,1e-13 )
-
-	return TEST_PASSED;
-}
-
-
-/** Compare deviations when multiplying dense sub-matrices via index lists. */
-int indexDenseSubmatrix()
-{
-	/* dense submatrices via index lists */
-	const int M = 20, N = 15, K = 5;
-	int i, j, k, m, n;
-	Indexlist rows(N), cols(N);
-	int *rNum, *cNum;
-	real_t err=0.0, errT=0.0;
-	real_t *Av, *X, *Y, *x, *y, *xc, *yc;
-
-	// prepare index lists
-	m = (M-3)/4;
-	n = (N-3)/4;
-	for (i = 0; i < m; i++) { rows.addNumber(M-1 - 2*i); rows.addNumber(2*i); }
-	for (i = 0; i < n; i++) { cols.addNumber(N-1 - 2*i); cols.addNumber(2*i); }
-	m *= 2;
-	n *= 2;
-
-	rows.getNumberArray(&rNum);
-	fprintf(stdFile, "Rows: ");
-	for (i = 0; i < m; i++) fprintf(stdFile, " %2d", rNum[i]);
-	fprintf(stdFile, "\n");
-
-	cols.getNumberArray(&cNum);
-	fprintf(stdFile, "Cols: ");
-	for (i = 0; i < n; i++) fprintf(stdFile, " %2d", cNum[i]);
-	fprintf(stdFile, "\n");
-
-	// prepare input matrices
-	Av = new real_t[M*N];
-	X = new real_t[n*K];
-	Y = new real_t[m*K];
-	x = new real_t[n*K];
-	y = new real_t[m*K];
-	xc = new real_t[n*K];
-	yc = new real_t[m*K];
-
-	DenseMatrix A(M, N, N, Av);
-	for (i = 0; i < M*N; i++) Av[i] = -0.5*N*M + (real_t)i;
-	for (i = 0; i < n*K; i++) X[i] = 1.0 / (real_t)(i+1);
-	for (i = 0; i < m*K; i++) Y[i] = 1.0 / (real_t)(i+1);
-
-	// multiply
-	A.times(&rows, &cols, K, 1.0, X, n, 0.0, y, m);
-
-	// check result
-	for (j = 0; j < m; j++)
-	{
-		for (k = 0; k < K; k++)
-		{
-			yc[j+k*m] = -y[j+k*m];
-			for (i = 0; i < n; i++)
-				yc[j+k*m] += Av[cNum[i]+rNum[j]*N] * X[i+k*n];
-			if (getAbs(yc[j+k*m]) > err) err = getAbs(yc[j+k*m]);
-		}
-	}
-	fprintf(stdFile, "Indexlist submatrix; error: %9.2e\n", err);
-
-	// transpose multiply
-	A.transTimes(&rows, &cols, K, 1.0, Y, m, 0.0, x, n);
-
-	// check result
-	errT = 0.0;
-	for (j = 0; j < n; j++)
-	{
-		for (k = 0; k < K; k++)
-		{
-			xc[j+k*n] = -x[j+k*n];
-			for (i = 0; i < m; i++)
-				xc[j+k*n] += Av[cNum[j]+rNum[i]*N] * Y[i+k*m];
-			if (getAbs(xc[j+k*n]) > errT) errT = getAbs(xc[j+k*n]);
-		}
-	}
-	fprintf(stdFile, "Indexlist transpose submatrix; error: %9.2e\n", errT);
-
-	// check result
-
-	// clean up
-	delete[] yc;
-	delete[] xc;
-	delete[] y;
-	delete[] x;
-	delete[] Y;
-	delete[] X;
-	A.free ();	// or delete[] Av;
-
-	QPOASES_TEST_FOR_TOL( err ,1e-13 )
-	QPOASES_TEST_FOR_TOL( errT,1e-14 )
-
-	return TEST_PASSED;
-}
-
-
-/** Obtain column of sparse matrix. */
-int spGetCol()
-{
-	long j, i;
-	sparse_int_t *ir = new sparse_int_t[10];
-	sparse_int_t *jc = new sparse_int_t[4];
-	real_t *val = new real_t[10];
-	real_t *col = new real_t[4];
-
-	/* Test matrix:
-	 *
-	 *  [  0  3  6 ]
-	 *  [  1  0  7 ]
-	 *  [  2  0  8 ]
-	 *  [  0  4  9 ]
-	 *  [  0  5 10 ]
-	 */
-
-	jc[0] = 0; jc[1] = 2; jc[2] = 5; jc[3] = 10;
-	ir[0] = 1; ir[1] = 2;
-	ir[2] = 0; ir[3] = 3; ir[4] = 4;
-	ir[5] = 0; ir[6] = 1; ir[7] = 2; ir[8] = 3; ir[9] = 4;
-	for (i = 0; i < 10; i++) val[i] = 1.0+i;
-
-	SparseMatrix A(5, 3, ir, jc, val);
-
-	Indexlist rows(4);
-
-	rows.addNumber(2);
-	rows.addNumber(4);
-	rows.addNumber(3);
-	rows.addNumber(0);
-
-	/* Indexed matrix:
-	 *
-	 *  [  2  0  8 ]
-	 *  [  0  5 10 ]
-	 *  [  0  4  9 ]
-	 *  [  0  3  6 ]
-	 */
-
-	for (j = 0; j < 3; j++)
-	{
-		fprintf(stdFile, "Column %ld:\n", j);
-		A.getCol(j, &rows, 1.0, col);
-		for (i = 0; i < 4; i++)
-			fprintf(stdFile, " %3.0f\n", col[i]);
-	}
-				
-	delete[] col;
-	A.free ();	// or delete[] val,jc,ir;
-
-	return TEST_PASSED;
-}
-
-
-/** Obtain row of sparse matrix. */
-int spGetRow()
-{
-	long j, i;
-	sparse_int_t *ir = new sparse_int_t[10];
-	sparse_int_t *jc = new sparse_int_t[6];
-	real_t *val = new real_t[10];
-	real_t *row = new real_t[4];
-
-	/* Test matrix:
-	 *
-	 *  [  0  3  4  6  0 ]
-	 *  [  1  0  0  7  9 ]
-	 *  [  2  0  5  8 10 ]
-	 */
-
-	jc[0] = 0; jc[1] = 2; jc[2] = 3; jc[3] = 5; jc[4] = 8; jc[5] = 10;
-	ir[0] = 1; ir[1] = 2;
-	ir[2] = 0; 
-	ir[3] = 0; ir[4] = 2;
-	ir[5] = 0; ir[6] = 1; ir[7] = 2; 
-	ir[8] = 1; ir[9] = 2;
-	for (i = 0; i < 10; i++) val[i] = 1.0+i;
-
-	SparseMatrix A(3, 4, ir, jc, val);
-
-	Indexlist cols(4);
-
-	cols.addNumber(2);
-	cols.addNumber(4);
-	cols.addNumber(3);
-	cols.addNumber(1);
-
-	/* Indexed matrix:
-	 *
-	 * [  4  0  6  3 ]
-	 * [  0  9  7  0 ]
-	 * [  5 10  8  0 ]
-	 */
-
-	for (j = 0; j < 3; j++)
-	{
-		A.getRow(j, &cols, 1.0, row);
-		for (i = 0; i < 4; i++)
-			fprintf(stdFile, " %3.0f", row[i]);
-		fprintf(stdFile, "\n");
-	}
-				
-	delete[] row;
-	A.free ();
-
-	return TEST_PASSED;
-}
-
-
-/** Compare deviations when multiplying sparse matrix. */
-int spTimes()
-{
-	long i;
-	sparse_int_t *ir = new sparse_int_t[10];
-	sparse_int_t *jc = new sparse_int_t[6];
-	real_t *val = new real_t[10];
-
-	real_t *x = new real_t[5*2];
-	real_t *y = new real_t[3*2];
-
-	real_t Ax[] = {-23, -11, -26, 42, 74, 99};
-	real_t ATy[] = {-63, -69, -222, -423, -359, 272, 126, 663, 1562, 1656};
-	real_t err=0.0, errT=0.0;
-
-	for (i = 0; i < 10; i++) x[i] = -4.0+i;
-
-	/* Test matrix:
-	 *
-	 *  [  0  3  4  6  0 ]
-	 *  [  1  0  0  7  9 ]
-	 *  [  2  0  5  8 10 ]
-	 */
-
-	jc[0] = 0; jc[1] = 2; jc[2] = 3; jc[3] = 5; jc[4] = 8; jc[5] = 10;
-	ir[0] = 1; ir[1] = 2;
-	ir[2] = 0; 
-	ir[3] = 0; ir[4] = 2;
-	ir[5] = 0; ir[6] = 1; ir[7] = 2; 
-	ir[8] = 1; ir[9] = 2;
-	for (i = 0; i < 10; i++) val[i] = 1.0+i;
-
-	SparseMatrix A(3, 5, ir, jc, val);	// reference to ir, jc, val
-
-	A.times(2, 1.0, x, 5, 0.0, y, 3);
-	
-	for (i = 0; i < 6; i++)
-		if (getAbs(y[i] - Ax[i]) > err) err = getAbs(y[i] - Ax[i]);
-	fprintf(stdFile, "Error in sparse A*x: %9.2e\n", err);
-
-	A.transTimes(2, 1.0, y, 3, 0.0, x, 5);
-
-	errT = 0.0;
-	for (i = 0; i < 10; i++)
-		if (getAbs(x[i] - ATy[i]) > errT) errT = getAbs(x[i] - ATy[i]);
-	fprintf(stdFile, "Error in sparse A'*x: %9.2e\n", errT);
-
-	A.free ();	// or delete[] val,ir,jc
-	delete[] y;
-	delete[] x;
-
-	QPOASES_TEST_FOR_TOL( err ,1e-15 )
-	QPOASES_TEST_FOR_TOL( errT,1e-15 )
-
-	return TEST_PASSED;
-}
-
-
-/** Compare deviations when multiplying sparse matrix via index lists. */
-int spIndTimes()
-{
-	const long N = 4;
-	long i, j;
-	long nRows = 2 * N + 1;
-	long nCols = N;
-	long nnz = 3 * N;
-	sparse_int_t *ir = new sparse_int_t[nnz];
-	sparse_int_t *jc = new sparse_int_t[nCols+1];
-	real_t *val = new real_t[nnz];
-	real_t *xc = new real_t[3*2];
-	real_t *yc = new real_t[4*2];
-	real_t Ax[] = {0.31, 0.05, 0.06, 0.30, 0.76, 0.20, 0.24, 0.60};
-	real_t ATy[] = {0.278, 0.000, 0.548, 0.776, 0.000, 1.208};
-	real_t err=0.0, errT=0.0;
-
-	Indexlist rows(4), cols(3), allcols(nCols);
-
-	rows.addNumber(2);
-	rows.addNumber(4);
-	rows.addNumber(3);
-	rows.addNumber(0);
-
-	cols.addNumber(1);
-	cols.addNumber(3);
-	cols.addNumber(0);
-
-	for (i = 0; i < nCols; i++) 
-		allcols.addNumber(i);
-
-	// build test matrix
-	for (i = 0; i <= N; i++) jc[i] = 3*i;
-	for (j = 0; j < N; j++) 
-		for (i = 0; i < 3; i++)
-		{
-			ir[j*3+i] = 2*j + i;
-			val[j*3+i] = 1.0 - 0.1 * (j*3+i);
-		}
-	SparseMatrix A(nRows, nCols, ir, jc, val);
-
-	fprintf(stdFile, "Test matrix A =\n");
-	for (j = 0; j < nRows; j++)
-	{
-		A.getRow(j, &allcols, 1.0, xc);
-		for (i = 0; i < nCols; i++)
-			fprintf(stdFile, "%6.2f", xc[i]);
-		fprintf(stdFile, "\n");
-	}
-
-	for (i = 0; i < 6; i++)
-		xc[i] = (1.0 + i) * 0.1;
-
-	A.times(&rows, &cols, 2, 1.0, xc, 3, 0.0, yc, 4, BT_TRUE);
-
-	for (i = 0; i < 8; i++)
-		if (getAbs(yc[i] - Ax[i]) > err)
-			err = getAbs(yc[i] - Ax[i]);
-	fprintf(stdFile, "Error in sparse indexed A*x: %9.2e\n", err);
-
-	A.transTimes(&rows, &cols, 2, 1.0, yc, 4, 0.0, xc, 3);
-	errT = 0.0;
-	for (i = 0; i < 6; i++)
-		if (getAbs(xc[i] - ATy[i]) > errT)
-			errT = getAbs(xc[i] - ATy[i]);
-	fprintf(stdFile, "Error in sparse indexed A'*y: %9.2e\n", errT);
-
-	delete[] xc;
-	delete[] yc;
-	A.free ();
-
-	QPOASES_TEST_FOR_TOL( err ,1e-15 )
-	QPOASES_TEST_FOR_TOL( errT,1e-15 )
-
-	return TEST_PASSED;
-}
-
-
-/** Obtain column of sparse row matrix. */
-int sprGetCol()
-{
-	long j, i;
-	sparse_int_t *ir = new sparse_int_t[10];
-	sparse_int_t *jc = new sparse_int_t[4];
-	real_t *val = new real_t[10];
-	real_t *col = new real_t[4];
-
-	/* Test matrix:
-	 *
-	 *  [  0  3  6 ]
-	 *  [  1  0  7 ]
-	 *  [  2  0  8 ]
-	 *  [  0  4  9 ]
-	 *  [  0  5 10 ]
-	 */
-
-	jc[0] = 0; jc[1] = 2; jc[2] = 5; jc[3] = 10;
-	ir[0] = 1; ir[1] = 2;
-	ir[2] = 0; ir[3] = 3; ir[4] = 4;
-	ir[5] = 0; ir[6] = 1; ir[7] = 2; ir[8] = 3; ir[9] = 4;
-	for (i = 0; i < 10; i++) val[i] = 1.0+i;
-
-	SparseMatrix Ac(5, 3, ir, jc, val);
-	real_t *Acv = Ac.full(); // row major format
-	SparseMatrixRow A(5, 3, 3, Acv);
-	delete[] Acv;
-	Ac.free ();	// or delete[] val,jc,ir;
-
-	Indexlist rows(4);
-
-	rows.addNumber(2);
-	rows.addNumber(4);
-	rows.addNumber(3);
-	rows.addNumber(0);
-
-	/* Indexed matrix:
-	 *
-	 *  [  2  0  8 ]
-	 *  [  0  5 10 ]
-	 *  [  0  4  9 ]
-	 *  [  0  3  6 ]
-	 */
-
-	for (j = 0; j < 3; j++)
-	{
-		fprintf(stdFile, "Column %ld:\n", j);
-		A.getCol(j, &rows, 1.0, col);
-		for (i = 0; i < 4; i++)
-			fprintf(stdFile, " %3.0f\n", col[i]);
-	}
-				
-	delete[] col;
-	A.free ();	// or delete[] val,jc,ir;
-
-	return TEST_PASSED;
-}
-
-
-/** Obtain row of sparse row matrix. */
-int sprGetRow()
-{
-	long j, i;
-	sparse_int_t *ir = new sparse_int_t[10];
-	sparse_int_t *jc = new sparse_int_t[6];
-	real_t *val = new real_t[10];
-	real_t *row = new real_t[4];
-
-	/* Test matrix:
-	 *
-	 *  [  0  3  4  6  0 ]
-	 *  [  1  0  0  7  9 ]
-	 *  [  2  0  5  8 10 ]
-	 */
-
-	jc[0] = 0; jc[1] = 2; jc[2] = 3; jc[3] = 5; jc[4] = 8; jc[5] = 10;
-	ir[0] = 1; ir[1] = 2;
-	ir[2] = 0; 
-	ir[3] = 0; ir[4] = 2;
-	ir[5] = 0; ir[6] = 1; ir[7] = 2; 
-	ir[8] = 1; ir[9] = 2;
-	for (i = 0; i < 10; i++) val[i] = 1.0+i;
-
-	SparseMatrix Ac(3, 5, ir, jc, val);
-	real_t *Acv = Ac.full(); // row major format
-	SparseMatrixRow A(3, 5, 5, Acv);
-	delete[] Acv;
-	Ac.free ();	// or delete[] val,jc,ir;
-
-	Indexlist cols(4);
-
-	cols.addNumber(2);
-	cols.addNumber(4);
-	cols.addNumber(3);
-	cols.addNumber(1);
-
-	/* Indexed matrix:
-	 *
-	 * [  4  0  6  3 ]
-	 * [  0  9  7  0 ]
-	 * [  5 10  8  0 ]
-	 */
-
-	for (j = 0; j < 3; j++)
-	{
-		A.getRow(j, &cols, 1.0, row);
-		for (i = 0; i < 4; i++)
-			fprintf(stdFile, " %3.0f", row[i]);
-		fprintf(stdFile, "\n");
-	}
-				
-	delete[] row;
-	A.free ();
-
-	return TEST_PASSED;
-}
-
-
-/** Compare deviations when multiplying sparse row matrix. */
-int sprTimes()
-{
-	long i;
-	sparse_int_t *ir = new sparse_int_t[10];
-	sparse_int_t *jc = new sparse_int_t[6];
-	real_t *val = new real_t[10];
-
-	real_t *x = new real_t[5*2];
-	real_t *y = new real_t[3*2];
-
-	real_t Ax[] = {-23, -11, -26, 42, 74, 99};
-	real_t ATy[] = {-63, -69, -222, -423, -359, 272, 126, 663, 1562, 1656};
-	real_t err=0.0, errT=0.0;
-
-	for (i = 0; i < 10; i++) x[i] = -4.0+i;
-
-	/* Test matrix:
-	 *
-	 *  [  0  3  4  6  0 ]
-	 *  [  1  0  0  7  9 ]
-	 *  [  2  0  5  8 10 ]
-	 */
-
-	jc[0] = 0; jc[1] = 2; jc[2] = 3; jc[3] = 5; jc[4] = 8; jc[5] = 10;
-	ir[0] = 1; ir[1] = 2;
-	ir[2] = 0; 
-	ir[3] = 0; ir[4] = 2;
-	ir[5] = 0; ir[6] = 1; ir[7] = 2; 
-	ir[8] = 1; ir[9] = 2;
-	for (i = 0; i < 10; i++) val[i] = 1.0+i;
-
-	SparseMatrix Ac(3, 5, ir, jc, val);	// reference to ir, jc, val
-	real_t *Acv = Ac.full(); // row major format
-	SparseMatrixRow A(3, 5, 5, Acv);
-	delete[] Acv;
-	Ac.free ();	// or delete[] val,jc,ir;
-
-	A.times(2, 1.0, x, 5, 0.0, y, 3);
-	
-	for (i = 0; i < 6; i++)
-		if (getAbs(y[i] - Ax[i]) > err) err = getAbs(y[i] - Ax[i]);
-	fprintf(stdFile, "Error in sparse A*x: %9.2e\n", err);
-
-	A.transTimes(2, 1.0, y, 3, 0.0, x, 5);
-
-	errT = 0.0;
-	for (i = 0; i < 10; i++)
-		if (getAbs(x[i] - ATy[i]) > errT) errT = getAbs(x[i] - ATy[i]);
-	fprintf(stdFile, "Error in sparse A'*x: %9.2e\n", errT);
-
-	A.free ();	// or delete[] val,ir,jc
-	delete[] y;
-	delete[] x;
-
-	QPOASES_TEST_FOR_TOL( err ,1e-15 )
-	QPOASES_TEST_FOR_TOL( errT,1e-15 )
-
-	return TEST_PASSED;
-}
-
-
-/** Compare deviations when multiplying sparse row matrix via index lists. */
-int sprIndTimes()
-{
-	const long N = 4;
-	long i, j;
-	long nRows = 2 * N + 1;
-	long nCols = N;
-	long nnz = 3 * N;
-	sparse_int_t *ir = new sparse_int_t[nnz];
-	sparse_int_t *jc = new sparse_int_t[nCols+1];
-	real_t *val = new real_t[nnz];
-	real_t *xc = new real_t[3*2];
-	real_t *yc = new real_t[4*2];
-	real_t Ax[] = {0.31, 0.05, 0.06, 0.30, 0.76, 0.20, 0.24, 0.60};
-	real_t ATy[] = {0.278, 0.000, 0.548, 0.776, 0.000, 1.208};
-	real_t err=0.0, errT=0.0;
-
-	Indexlist rows(4), cols(3), allcols(nCols);
-
-	rows.addNumber(2);
-	rows.addNumber(4);
-	rows.addNumber(3);
-	rows.addNumber(0);
-
-	cols.addNumber(1);
-	cols.addNumber(3);
-	cols.addNumber(0);
-
-	for (i = 0; i < nCols; i++) 
-		allcols.addNumber(i);
-
-	// build test matrix
-	for (i = 0; i <= N; i++) jc[i] = 3*i;
-	for (j = 0; j < N; j++) 
-		for (i = 0; i < 3; i++)
-		{
-			ir[j*3+i] = 2*j + i;
-			val[j*3+i] = 1.0 - 0.1 * (j*3+i);
-		}
-	SparseMatrix Ac(nRows, nCols, ir, jc, val);
-	real_t *Acv = Ac.full(); // row major format
-	SparseMatrixRow A(nRows, nCols, nCols, Acv);
-	delete[] Acv;
-	Ac.free ();	// or delete[] val,jc,ir;
-
-	fprintf(stdFile, "Test matrix A =\n");
-	for (j = 0; j < nRows; j++)
-	{
-		A.getRow(j, &allcols, 1.0, xc);
-		for (i = 0; i < nCols; i++)
-			fprintf(stdFile, "%6.2f", xc[i]);
-		fprintf(stdFile, "\n");
-	}
-
-	for (i = 0; i < 6; i++)
-		xc[i] = (1.0 + i) * 0.1;
-
-	A.times(&rows, &cols, 2, 1.0, xc, 3, 0.0, yc, 4, BT_TRUE);
-
-	for (i = 0; i < 8; i++)
-		if (getAbs(yc[i] - Ax[i]) > err)
-			err = getAbs(yc[i] - Ax[i]);
-	fprintf(stdFile, "Error in sparse indexed A*x: %9.2e\n", err);
-
-	A.transTimes(&rows, &cols, 2, 1.0, yc, 4, 0.0, xc, 3);
-	for (i = 0; i < 3; i++)
-	{
-		for (j = 0; j < 2; j++)
-			fprintf(stdFile, "%6.2f", ATy[i + j*3]);
-		fprintf(stdFile, "\n");
-	}
-	for (i = 0; i < 3; i++)
-	{
-		for (j = 0; j < 2; j++)
-			fprintf(stdFile, "%6.2f", xc[i + j*3]);
-		fprintf(stdFile, "\n");
-	}
-	for (i = 0; i < 6; i++)
-		if (getAbs(xc[i] - ATy[i]) > errT)
-			errT = getAbs(xc[i] - ATy[i]);
-	fprintf(stdFile, "Error in sparse indexed A'*y: %9.2e\n", errT);
-
-	delete[] xc;
-	delete[] yc;
-	A.free ();
-
-	QPOASES_TEST_FOR_TOL( err ,1e-15 )
-	QPOASES_TEST_FOR_TOL( errT,1e-15 )
-
-	return TEST_PASSED;
-}
-
-
-/** Compare deviations when using bilinear multiplication with dense matrix. */
-int symmetry()
-{
-	int i,j;
-	real_t *Hv = new real_t[6*6];
-	real_t *Z = new real_t[6*3];
-	real_t *ZHZd = new real_t[3*3];
-	real_t *ZHZs = new real_t[3*3];
-	real_t ZHZv[] = {0.144, 0.426, 0.708, 0.426, 1.500, 2.574, 0.708, 2.574, 4.440};
-	real_t err=0.0, errS=0.0;
-	SymDenseMat *Hd;
-	SymSparseMat *Hs;
-	Indexlist *cols = new Indexlist(6);
-
-	for (i = 0; i < 36; i++) Hv[i] = 0.0;
-	for (i = 0; i < 6; i++) Hv[i*7] = 1.0 - 0.1 * i;
-	for (i = 0; i < 5; i++) Hv[i*7+1] = Hv[i*7+6] = -0.1 * (i+1);
-
-	Hd = new SymDenseMat(6, 6, 6, Hv);	// deep-copy from Hv
-	Hs = new SymSparseMat(6, 6, 6, Hv);	// deep-copy from Hv
-	Hs->createDiagInfo();
-
-	for (i = 0; i < 6; ++i)
-	{
-		for (j = 0; j < 6; ++j)
-			fprintf (stdFile, "%3.3f ", Hv[i*6+j]);
-		fprintf (stdFile, "\n");
-	}
-	fprintf (stdFile, "\n");
-	
-	cols->addNumber(3);
-	cols->addNumber(0);
-	cols->addNumber(4);
-	cols->addNumber(1);
-	for (i = 0; i < 18; i++) Z[i] = 0.1 * (i+1);
-
-	fprintf (stdFile, "\n");
-	for (i = 0; i < 6; ++i)
-	{
-		for (j = 0; j < 3; ++j)
-			fprintf (stdFile, "%3.3f ", Z[i+j*6]);
-		fprintf (stdFile, "\n");
-	}
-	fprintf (stdFile, "\n");
-	
-	Hd->bilinear(cols, 3, Z, 6, ZHZd, 3);
-
-	for (i = 0; i < 9; i++)
-		if (getAbs(ZHZd[i] - ZHZv[i]) > err)
-			err = getAbs(ZHZd[i] - ZHZv[i]);
-	fprintf(stdFile, "Error in indexed dense bilinear form: %9.2e\n", err);
-
-	Hs->bilinear(cols, 3, Z, 6, ZHZs, 3);
-	
-	for (i = 0; i < 3; ++i)
-	{
-		for (j = 0; j < 3; ++j)
-			fprintf (stdFile, "%3.3f ", ZHZd[i*3+j]);
-		fprintf (stdFile, "\n");
-	}
-	fprintf (stdFile, "\n");
-
-	for (i = 0; i < 3; ++i)
-	{
-		for (j = 0; j < 3; ++j)
-			fprintf (stdFile, "%3.3f ", ZHZv[i*3+j]);
-		fprintf (stdFile, "\n");
-	}
-	fprintf (stdFile, "\n");
-
-	for (i = 0; i < 9; i++)
-		if (getAbs(ZHZs[i] - ZHZv[i]) > errS)
-			errS = getAbs(ZHZs[i] - ZHZv[i]);
-	fprintf(stdFile, "Error in indexed sparse bilinear form: %9.2e\n", errS);
-
-	delete cols;
-	delete Hs;
-	delete Hd;
-	delete[] ZHZs;
-	delete[] ZHZd;
-	delete[] Z;
-	delete[] Hv;
-
-	QPOASES_TEST_FOR_TOL( err ,1e-15 )
-	QPOASES_TEST_FOR_TOL( errS,1e-15 )
-
-	return TEST_PASSED;
-}
-
-
-/** Run various tests on matrix classes. */
-int main()
-{
-	int errorCount = TEST_PASSED;
-	
-	errorCount += sumOfSquares();
-	errorCount += hilbert();
-	errorCount += submatrix();
-	errorCount += indexDenseSubmatrix();
-
-	errorCount += spGetCol();
-	errorCount += spGetRow();
-	errorCount += spTimes();
-	errorCount += spIndTimes();
-
-	errorCount += sprGetCol();
-	errorCount += sprGetRow();
-	errorCount += sprTimes();
-	errorCount += sprIndTimes();
-
-	errorCount += symmetry();
-	
-	return errorCount;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices2.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices2.cpp
deleted file mode 100644
index 7deed6925e3..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices2.cpp
+++ /dev/null
@@ -1,113 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_matrices2.cpp
- *	\author Hans Joachim Ferreau,Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2014
- *
- *	Unit test for Matrix classes.
- */
-
-
-#include <stdlib.h>
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-#include "test_qrecipe_data.hpp"
-
-
-/** Compare deviations when performing matrix operations. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-	
-	int i;
-
-	real_t errH=0.0, errA=0.0;
-	real_t v[180];
-	real_t resHs[180];
-	real_t resHd[180];
-	real_t resAs[91];
-	real_t resAd[91];
-	
-	/* create sparse matrices */
-	SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val);
-	SparseMatrix *A = new SparseMatrix(91, 180, A_ir, A_jc, A_val);
-
-	H->createDiagInfo();
-
-	real_t* H_full = H->full();
-	real_t* A_full = A->full();
-
-	//print( A_full,91,180 );
-
-
-	SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full);
-	DenseMatrix *Ad = new DenseMatrix(91,180,180,A_full);
-
-	for( i=0; i<180; ++i )
-		v[i] = 2.0 * ((real_t)rand()) / ((real_t)RAND_MAX) - 1.0;
-	
-	H ->times(1, 1.0, v, 180, 0.0, resHs, 180);
-	Hd->times(1, 1.0, v, 180, 0.0, resHd, 180);
-	
-	A ->times(1, 1.0, v, 180, 0.0, resAs, 91);
-	Ad->times(1, 1.0, v, 180, 0.0, resAd, 91);
-
-	
-	for ( i=0; i<180; ++i )
-		if ( getAbs(resHs[i] - resHd[i]) > errH)
-			errH = getAbs(resHs[i] - resHd[i]);
-	
-	fprintf(stdFile, "maximum difference in H*v: %9.2e\n", errH);
-	
-	
-	for ( i=0; i<91; ++i )
-		if ( getAbs(resAs[i] - resAd[i]) > errA)
-			errA = getAbs(resAs[i] - resAd[i]);
-	
-	fprintf(stdFile, "maximum difference in A*v: %9.2e\n", errA);
-
-	delete H;
-	delete A;
-	delete[] H_full;
-	delete[] A_full;
-	delete Hd;
-	delete Ad;
-
-	
-	QPOASES_TEST_FOR_TOL( errH,1e-13 )
-	QPOASES_TEST_FOR_TOL( errA,1e-13 )
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices3.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices3.cpp
deleted file mode 100644
index 88822d6e607..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_matrices3.cpp
+++ /dev/null
@@ -1,90 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_matrices3.cpp
- *	\author Hans Joachim Ferreau,Andreas Potschka, Christian Kirches
- *	\version 3.0
- *	\date 2014
- *
- *	Unit test for Matrix classes.
- */
-
-
-#include <stdlib.h>
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-#include "test_qrecipe_data.hpp"
-
-
-/** Compare deviations when performing matrix operations. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-	
-	int i;
-
-	real_t errH=0.0;
-	real_t v[180];
-	real_t resHn[180];
-	real_t resHt[180];
-	
-	/* create sparse matrices */
-	SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val);
-	
-	H->createDiagInfo();
-
-	real_t* H_full = H->full();
-	
-	SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full);
-
-	for( i=0; i<180; ++i )
-		v[i] = 2.0 * ((real_t)rand()) / ((real_t)RAND_MAX) - 1.0;
-	
-	Hd->times(     1, 1.0, v, 180, 0.0, resHn, 180);
-	Hd->transTimes(1, 1.0, v, 180, 0.0, resHt, 180);
-	
-	for ( i=0; i<180; ++i )
-		if ( getAbs(resHn[i] - resHt[i]) > errH)
-			errH = getAbs(resHn[i] - resHt[i]);
-	
-	fprintf(stdFile, "maximum difference in H*v vs. H'*v: %9.2e\n", errH);
-	
-	delete H;
-	delete[] H_full;
-	delete Hd;
-	
-	
-	QPOASES_TEST_FOR_TOL( errH,1e-15 )
-	
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_qrecipe.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_qrecipe.cpp
deleted file mode 100644
index b96406ac4bc..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_qrecipe.cpp
+++ /dev/null
@@ -1,149 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_qrecipe.cpp
- *	\author Andreas Potschka
- *	\version 3.0
- *	\date 2007-2014
- *
- *	QRECIPE example from the CUTEr test set with sparse matrices.
- */
-
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-#include "test_qrecipe_data.hpp"
-
-
-
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	long i;
-	int nWSR;
-	real_t tic, toc;
-	real_t errP=0.0, errD=0.0;
-	real_t *x1 = new real_t[180];
-	real_t *y1 = new real_t[271];
-	real_t *x2 = new real_t[180];
-	real_t *y2 = new real_t[271];
-
-	/* create sparse matrices */
-	SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val);
-	SparseMatrix *A = new SparseMatrix(91, 180, A_ir, A_jc, A_val);
-
-	H->createDiagInfo();
-
-	real_t* H_full = H->full();
-	real_t* A_full = A->full();
-
-	SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full);
-	DenseMatrix *Ad = new DenseMatrix(91,180,180,A_full);
-
-	/* solve with dense matrices */
-	nWSR = 1000;
-	QProblem qrecipeD(180, 91);
-	tic = getCPUtime();
-	qrecipeD.init(Hd, g, Ad, lb, ub, lbA, ubA, nWSR, 0);
-	toc = getCPUtime();
-	qrecipeD.getPrimalSolution(x1);
-	qrecipeD.getDualSolution(y1);
-
-	fprintf(stdFile, "Solved dense problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic);
-
-	/* Compute KKT tolerances */
-	real_t stat, feas, cmpl;
-
-	getKKTResidual(	180,91,
-					H_full,g,A_full,lb,ub,lbA,ubA,
-					x1,y1,
-					stat,feas,cmpl
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n\n", stat,feas,cmpl );
-
-
-	/* solve with sparse matrices */
-	nWSR = 1000;
-	QProblem qrecipeS(180, 91);
-	tic = getCPUtime();
-	qrecipeS.init(H, g, A, lb, ub, lbA, ubA, nWSR, 0);
-	toc = getCPUtime();
-	qrecipeS.getPrimalSolution(x2);
-	qrecipeS.getDualSolution(y2);
-
-	fprintf(stdFile, "Solved sparse problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic);
-	
-	/* Compute KKT tolerances */
-	real_t stat2, feas2, cmpl2;
-	getKKTResidual(	180,91,
-					H_full,g,A_full,lb,ub,lbA,ubA,
-					x2,y2,
-					stat2,feas2,cmpl2
-					);
-	printf( "stat = %e\nfeas = %e\ncmpl = %e\n\n", stat2,feas2,cmpl2 );
-
-	/* check distance of solutions */
-	for (i = 0; i < 180; i++)
-		if (getAbs(x1[i] - x2[i]) > errP)
-			errP = getAbs(x1[i] - x2[i]);
-	fprintf(stdFile, "Primal error: %9.2e\n", errP);
-
-	for (i = 0; i < 271; i++)
-		if (getAbs(y1[i] - y2[i]) > errD)
-			errD = getAbs(y1[i] - y2[i]);
-	fprintf(stdFile, "Dual error: %9.2e (might not be unique)\n", errD);
-
-	delete H;
-	delete A;
-	delete[] H_full;
-	delete[] A_full;
-	delete Hd;
-	delete Ad;
-
-	delete[] y2;
-	delete[] x2;
-	delete[] y1;
-	delete[] x1;
-
-	QPOASES_TEST_FOR_TOL( stat,1e-14 );
-	QPOASES_TEST_FOR_TOL( feas,1e-14 );
-	QPOASES_TEST_FOR_TOL( cmpl,1e-13 );
-	
-	QPOASES_TEST_FOR_TOL( stat2,1e-14 );
-	QPOASES_TEST_FOR_TOL( feas2,1e-14 );
-	QPOASES_TEST_FOR_TOL( cmpl2,1e-13 );
-
-	QPOASES_TEST_FOR_TOL( errP,1e-13 );
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_qrecipe_data.hpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_qrecipe_data.hpp
deleted file mode 100644
index d4c6342dcdf..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_qrecipe_data.hpp
+++ /dev/null
@@ -1,401 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_qrecipe.cpp
- *	\author Andreas Potschka
- *	\version 3.0
- *	\date 2007-2014
- *
- *	QRECIPE example from the CUTEr test set with sparse matrices.
- */
-
-
-USING_NAMESPACE_QPOASES
-
-
-
-const real_t Inf = INFTY;
-
-sparse_int_t H_jc[] = { 0,  4,  8, 12, 16, 20, 20, 20, 20, 20, 20,
-	                   24, 28, 32, 36, 40, 40, 40, 40, 40, 40,
-					   44, 48, 52, 56, 60, 60, 60, 60, 60, 60, 60, 60, 60, 60,
-					   64, 68, 72, 76, 80, 80, 80, 80, 80, 80,
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 
-					   80, 80, 80, 80, 80, 80 };
-
-sparse_int_t H_ir[] = {
-	0, 10, 20, 34, 1, 11, 21, 35, 2, 12, 22, 36, 3, 13, 23, 37, 4, 14, 24, 38, 
-	0, 10, 20, 34, 1, 11, 21, 35, 2, 12, 22, 36, 3, 13, 23, 37, 4, 14, 24, 38, 
-	0, 10, 20, 34, 1, 11, 21, 35, 2, 12, 22, 36, 3, 13, 23, 37, 4, 14, 24, 38, 
-	0, 10, 20, 34, 1, 11, 21, 35, 2, 12, 22, 36, 3, 13, 23, 37, 4, 14, 24, 38};
-
-real_t H_val[] = {10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1,
-	1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 1,
-	10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 1, 10, 1,
-	1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10, 1, 1, 1, 10};
-
-sparse_int_t A_jc[] = {
-	  0,  10,  20,  30,  40,  50,  60,  70,  80,  90, 100, 110, 120,
-	130, 140, 150, 160, 170, 180, 190, 200, 210, 220, 230, 240, 250, 260, 270,
-	280, 290, 300, 301, 302, 303, 304, 305, 306, 307, 308, 309, 310, 311, 312,
-	313, 314, 318, 319, 320, 321, 322, 323, 324, 325, 326, 327, 328, 329, 331,
-	333, 335, 337, 339, 341, 343, 345, 347, 349, 351, 353, 355, 357, 359, 361,
-	363, 365, 367, 369, 371, 373, 383, 393, 403, 405, 408, 410, 413, 415, 418,
-	420, 422, 424, 426, 428, 430, 432, 434, 436, 438, 440, 442, 444, 446, 448,
-	450, 452, 454, 456, 458, 460, 462, 472, 482, 492, 494, 497, 499, 502, 504,
-	507, 509, 511, 513, 515, 517, 519, 521, 523, 525, 527, 529, 531, 533, 535,
-	537, 539, 541, 543, 545, 547, 549, 551, 561, 571, 581, 583, 586, 588, 591,
-	593, 596, 597, 598, 599, 600, 601, 602, 603, 604, 605, 606, 607, 608, 609,
-	610, 611, 612, 613, 614, 615, 616, 617, 618, 628, 638, 648, 650, 653, 655,
-	658, 660, 663};
-
-sparse_int_t A_ir[] = {0, 14, 35, 36, 71, 72, 85, 86, 87, 88, 1, 14, 35, 36, 71, 72, 85,
-	86, 87, 88, 2, 14, 35, 36, 71, 72, 85, 86, 87, 88, 3, 14, 35, 36, 71, 72,
-	85, 86, 87, 88, 4, 14, 35, 36, 71, 72, 85, 86, 87, 88, 5, 14, 35, 36, 71,
-	72, 85, 86, 87, 88, 6, 14, 35, 36, 71, 72, 85, 86, 87, 88, 7, 14, 35, 36,
-	71, 72, 85, 86, 87, 88, 8, 14, 35, 36, 71, 72, 85, 86, 87, 88, 9, 14, 35,
-	36, 71, 72, 85, 86, 87, 88, 0, 15, 37, 38, 69, 70, 79, 80, 81, 82, 1, 15,
-	37, 38, 69, 70, 79, 80, 81, 82, 2, 15, 37, 38, 69, 70, 79, 80, 81, 82, 3,
-	15, 37, 38, 69, 70, 79, 80, 81, 82, 4, 15, 37, 38, 69, 70, 79, 80, 81, 82,
-	5, 15, 37, 38, 69, 70, 79, 80, 81, 82, 6, 15, 37, 38, 69, 70, 79, 80, 81,
-	82, 7, 15, 37, 38, 69, 70, 79, 80, 81, 82, 8, 15, 37, 38, 69, 70, 79, 80,
-	81, 82, 9, 15, 37, 38, 69, 70, 79, 80, 81, 82, 0, 16, 39, 40, 67, 68, 73,
-	74, 75, 76, 1, 16, 39, 40, 67, 68, 73, 74, 75, 76, 2, 16, 39, 40, 67, 68,
-	73, 74, 75, 76, 3, 16, 39, 40, 67, 68, 73, 74, 75, 76, 4, 16, 39, 40, 67,
-	68, 73, 74, 75, 76, 5, 16, 39, 40, 67, 68, 73, 74, 75, 76, 6, 16, 39, 40,
-	67, 68, 73, 74, 75, 76, 7, 16, 39, 40, 67, 68, 73, 74, 75, 76, 8, 16, 39,
-	40, 67, 68, 73, 74, 75, 76, 9, 16, 39, 40, 67, 68, 73, 74, 75, 76, 10, 11,
-	12, 13, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 47, 48, 49, 50, 51,
-	52, 53, 54, 55, 56, 57, 47, 58, 48, 59, 49, 60, 50, 61, 51, 62, 52, 63, 53,
-	64, 54, 65, 55, 66, 46, 56, 45, 57, 47, 58, 48, 59, 49, 60, 50, 61, 51, 62,
-	52, 63, 53, 64, 54, 65, 55, 66, 46, 56, 45, 57, 10, 14, 71, 72, 85, 86, 87,
-	88, 89, 90, 11, 15, 69, 70, 79, 80, 81, 82, 83, 84, 12, 16, 67, 68, 73, 74,
-	75, 76, 77, 78, 35, 90, 36, 89, 90, 37, 84, 38, 83, 84, 39, 78, 40, 77, 78,
-	44, 58, 43, 59, 42, 60, 41, 61, 34, 62, 33, 63, 32, 64, 31, 65, 30, 66, 29,
-	46, 28, 45, 44, 58, 43, 59, 42, 60, 41, 61, 34, 62, 33, 63, 32, 64, 31, 65,
-	30, 66, 29, 46, 28, 45, 10, 14, 71, 72, 85, 86, 87, 88, 89, 90, 11, 15, 69,
-	70, 79, 80, 81, 82, 83, 84, 12, 16, 67, 68, 73, 74, 75, 76, 77, 78, 35, 90,
-	36, 89, 90, 37, 84, 38, 83, 84, 39, 78, 40, 77, 78, 27, 44, 26, 43, 25, 42,
-	24, 41, 23, 34, 22, 33, 21, 32, 20, 31, 19, 30, 18, 29, 17, 28, 27, 44, 26,
-	43, 25, 42, 24, 41, 23, 34, 22, 33, 21, 32, 20, 31, 19, 30, 18, 29, 17, 28,
-	10, 14, 71, 72, 85, 86, 87, 88, 89, 90, 11, 15, 69, 70, 79, 80, 81, 82, 83,
-	84, 12, 16, 67, 68, 73, 74, 75, 76, 77, 78, 35, 90, 36, 89, 90, 37, 84, 38,
-	83, 84, 39, 78, 40, 77, 78, 27, 26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 27,
-	26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 10, 14, 71, 72, 85, 86, 87, 88, 89,
-	90, 11, 15, 69, 70, 79, 80, 81, 82, 83, 84, 12, 16, 67, 68, 73, 74, 75, 76,
-	77, 78, 35, 90, 36, 89, 90, 37, 84, 38, 83, 84, 39, 78, 40, 77, 78};
-
-real_t A_val[] = {
--1.0000000000000000e+00,  1.0000000000000000e+00,  8.8678200000000004e+01,
- 9.3617050000000006e+01,  1.6000000000000000e+01,  8.1999999999999993e+00,
- 9.9000000000000000e+01,  8.0000000000000000e+01,  1.2000000000000000e+01,
- 9.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 8.0062830000000005e+01,  9.9224010000000007e+01,  1.0000000000000000e+02,
- 2.1100000000000001e+01,  1.0000000000000000e+02,  1.0000000000000000e+02,
- 1.1400000000000000e+02,  1.1680000000000000e+02, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  7.4697360000000003e+01,  8.3801220000000001e+01,
--8.1999999999999993e+00,  2.0000000000000000e+00,  9.0000000000000000e+01,
- 2.3999999999999999e+00, -1.2000000000000000e+01, -1.4800000000000001e+01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  7.9194209999999998e+01,
- 9.0175110000000004e+01,  4.3000000000000000e+01,  8.0000000000000000e+00,
- 1.0000000000000000e+02,  9.5000000000000000e+01,  9.0000000000000000e+00,
- 2.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 7.8568219999999997e+01,  8.5996200000000002e+01, -1.2500000000000000e+01,
- 1.0000000000000000e+00,  9.6500000000000000e+01,  4.0000000000000000e+00,
--1.8000000000000000e+01, -2.1899999999999999e+01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  8.2922240000000002e+01,  8.6963380000000001e+01,
- 6.5000000000000000e+01,  1.2500000000000000e+01,  1.0000000000000000e+02,
- 9.8000000000000000e+01,  4.9000000000000000e+01,  3.7000000000000000e+01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  8.2592740000000006e+01,
- 9.3147599999999997e+01, -1.2000000000000000e+01,  1.0000000000000000e+00,
- 9.6500000000000000e+01,  4.0000000000000000e+00, -1.8000000000000000e+01,
--2.1899999999999999e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 7.6506460000000004e+01,  7.8210250000000002e+01,  7.9000000000000000e+01,
- 1.2000000000000000e+01,  1.0000000000000000e+02,  9.5000000000000000e+01,
- 6.8000000000000000e+01,  6.1000000000000000e+01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  8.8357460000000003e+01,  9.4257840000000002e+01,
- 1.2500000000000000e+02,  6.1299999999999997e+01,  1.0000000000000000e+02,
- 1.0000000000000000e+02,  1.4500000000000000e+02,  1.4500000000000000e+02,
--1.0000000000000000e+00,  1.0000000000000000e+00,  9.0590469999999996e+01,
- 1.0582863000000000e+02,  6.2000000000000002e+00,  6.0000000000000000e+00,
- 9.7000000000000000e+01,  2.8500000000000000e+01,  4.0000000000000000e+00,
- 3.6000000000000001e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 8.8678200000000004e+01,  9.3617050000000006e+01,  1.6000000000000000e+01,
- 8.1999999999999993e+00,  9.9000000000000000e+01,  8.0000000000000000e+01,
- 1.2000000000000000e+01,  9.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  8.0062830000000005e+01,  9.9224010000000007e+01,
- 1.0000000000000000e+02,  2.1100000000000001e+01,  1.0000000000000000e+02,
- 1.0000000000000000e+02,  1.1400000000000000e+02,  1.1680000000000000e+02,
--1.0000000000000000e+00,  1.0000000000000000e+00,  7.4697360000000003e+01,
- 8.3801220000000001e+01, -8.1999999999999993e+00,  2.0000000000000000e+00,
- 9.0000000000000000e+01,  2.3999999999999999e+00, -1.2000000000000000e+01,
--1.4800000000000001e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 7.9194209999999998e+01,  9.0175110000000004e+01,  4.3000000000000000e+01,
- 8.0000000000000000e+00,  1.0000000000000000e+02,  9.5000000000000000e+01,
- 9.0000000000000000e+00,  2.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  7.8568219999999997e+01,  8.5996200000000002e+01,
--1.2500000000000000e+01,  1.0000000000000000e+00,  9.6500000000000000e+01,
- 4.0000000000000000e+00, -1.8000000000000000e+01, -2.1899999999999999e+01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  8.2922240000000002e+01,
- 8.6963380000000001e+01,  6.5000000000000000e+01,  1.2500000000000000e+01,
- 1.0000000000000000e+02,  9.8000000000000000e+01,  4.9000000000000000e+01,
- 3.7000000000000000e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 8.2592740000000006e+01,  9.3147599999999997e+01, -1.2000000000000000e+01,
- 1.0000000000000000e+00,  9.6500000000000000e+01,  4.0000000000000000e+00,
--1.8000000000000000e+01, -2.1899999999999999e+01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  7.6506460000000004e+01,  7.8210250000000002e+01,
- 7.9000000000000000e+01,  1.2000000000000000e+01,  1.0000000000000000e+02,
- 9.5000000000000000e+01,  6.8000000000000000e+01,  6.1000000000000000e+01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  8.8357460000000003e+01,
- 9.4257840000000002e+01,  1.2500000000000000e+02,  6.1299999999999997e+01,
- 1.0000000000000000e+02,  1.0000000000000000e+02,  1.4500000000000000e+02,
- 1.4500000000000000e+02, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 9.0590469999999996e+01,  1.0582863000000000e+02,  6.2000000000000002e+00,
- 6.0000000000000000e+00,  9.7000000000000000e+01,  2.8500000000000000e+01,
- 4.0000000000000000e+00,  3.6000000000000001e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  8.8678200000000004e+01,  9.3617050000000006e+01,
- 1.6000000000000000e+01,  8.1999999999999993e+00,  9.9000000000000000e+01,
- 8.0000000000000000e+01,  1.2000000000000000e+01,  9.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00,  8.0062830000000005e+01,
- 9.9224010000000007e+01,  1.0000000000000000e+02,  2.1100000000000001e+01,
- 1.0000000000000000e+02,  1.0000000000000000e+02,  1.1400000000000000e+02,
- 1.1680000000000000e+02, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 7.4697360000000003e+01,  8.3801220000000001e+01, -8.1999999999999993e+00,
- 2.0000000000000000e+00,  9.0000000000000000e+01,  2.3999999999999999e+00,
--1.2000000000000000e+01, -1.4800000000000001e+01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  7.9194209999999998e+01,  9.0175110000000004e+01,
- 4.3000000000000000e+01,  8.0000000000000000e+00,  1.0000000000000000e+02,
- 9.5000000000000000e+01,  9.0000000000000000e+00,  2.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00,  7.8568219999999997e+01,
- 8.5996200000000002e+01, -1.2500000000000000e+01,  1.0000000000000000e+00,
- 9.6500000000000000e+01,  4.0000000000000000e+00, -1.8000000000000000e+01,
--2.1899999999999999e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 8.2922240000000002e+01,  8.6963380000000001e+01,  6.5000000000000000e+01,
- 1.2500000000000000e+01,  1.0000000000000000e+02,  9.8000000000000000e+01,
- 4.9000000000000000e+01,  3.7000000000000000e+01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  8.2592740000000006e+01,  9.3147599999999997e+01,
--1.2000000000000000e+01,  1.0000000000000000e+00,  9.6500000000000000e+01,
- 4.0000000000000000e+00, -1.8000000000000000e+01, -2.1899999999999999e+01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  7.6506460000000004e+01,
- 7.8210250000000002e+01,  7.9000000000000000e+01,  1.2000000000000000e+01,
- 1.0000000000000000e+02,  9.5000000000000000e+01,  6.8000000000000000e+01,
- 6.1000000000000000e+01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 8.8357460000000003e+01,  9.4257840000000002e+01,  1.2500000000000000e+02,
- 6.1299999999999997e+01,  1.0000000000000000e+02,  1.0000000000000000e+02,
- 1.4500000000000000e+02,  1.4500000000000000e+02, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  9.0590469999999996e+01,  1.0582863000000000e+02,
- 6.2000000000000002e+00,  6.0000000000000000e+00,  9.7000000000000000e+01,
- 2.8500000000000000e+01,  4.0000000000000000e+00,  3.6000000000000001e+00,
--1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
- 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
- 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
- 1.0000000000000000e+00,  1.0000000000000000e+00, -1.2000000000000000e-01,
--3.8000000000000000e-01, -5.0000000000000000e-01,  1.0000000000000000e+00,
- 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
- 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
- 1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
- 1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
--4.7000000000000000e+01, -8.6999999999999993e+00, -9.0000000000000000e+01,
--5.0000000000000000e+01, -1.0000000000000000e+01, -1.0000000000000000e+01,
--9.3000000000000000e+01, -8.9000000000000000e+01,  1.0000000000000000e+00,
--1.0000000000000000e+00, -4.7000000000000000e+01, -8.6999999999999993e+00,
--9.0000000000000000e+01, -5.0000000000000000e+01, -1.0000000000000000e+01,
--1.0000000000000000e+01, -8.9000000000000000e+01, -8.5000000000000000e+01,
- 1.0000000000000000e+00, -1.0000000000000000e+00, -4.7000000000000000e+01,
--8.6999999999999993e+00, -9.0000000000000000e+01, -5.0000000000000000e+01,
--1.0000000000000000e+01, -1.0000000000000000e+01, -9.1000000000000000e+01,
--8.8000000000000000e+01, -1.0000000000000000e+00,  5.0000000000000000e-01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  5.0000000000000000e-01,
--1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
- 5.0000000000000000e-01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 5.0000000000000000e-01,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00, -4.7000000000000000e+01,
--8.6999999999999993e+00, -9.0000000000000000e+01, -5.0000000000000000e+01,
--1.0000000000000000e+01, -1.0000000000000000e+01, -9.3000000000000000e+01,
--8.9000000000000000e+01,  1.0000000000000000e+00, -1.0000000000000000e+00,
--4.7000000000000000e+01, -8.6999999999999993e+00, -9.0000000000000000e+01,
--5.0000000000000000e+01, -1.0000000000000000e+01, -1.0000000000000000e+01,
--8.9000000000000000e+01, -8.5000000000000000e+01,  1.0000000000000000e+00,
--1.0000000000000000e+00, -4.7000000000000000e+01, -8.6999999999999993e+00,
--9.0000000000000000e+01, -5.0000000000000000e+01, -1.0000000000000000e+01,
--1.0000000000000000e+01, -9.1000000000000000e+01, -8.8000000000000000e+01,
--1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
- 5.0000000000000000e-01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 5.0000000000000000e-01, -1.0000000000000000e+00,  5.0000000000000000e-01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  5.0000000000000000e-01,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00,  1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00,  1.0000000000000000e+00,
--1.0000000000000000e+00, -4.7000000000000000e+01, -8.6999999999999993e+00,
--9.0000000000000000e+01, -5.0000000000000000e+01, -1.0000000000000000e+01,
--1.0000000000000000e+01, -9.3000000000000000e+01, -8.9000000000000000e+01,
- 1.0000000000000000e+00, -1.0000000000000000e+00, -4.7000000000000000e+01,
--8.6999999999999993e+00, -9.0000000000000000e+01, -5.0000000000000000e+01,
--1.0000000000000000e+01, -1.0000000000000000e+01, -8.9000000000000000e+01,
--8.5000000000000000e+01,  1.0000000000000000e+00, -1.0000000000000000e+00,
--4.7000000000000000e+01, -8.6999999999999993e+00, -9.0000000000000000e+01,
--5.0000000000000000e+01, -1.0000000000000000e+01, -1.0000000000000000e+01,
--9.1000000000000000e+01, -8.8000000000000000e+01, -1.0000000000000000e+00,
- 5.0000000000000000e-01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 5.0000000000000000e-01, -1.0000000000000000e+00,  5.0000000000000000e-01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  5.0000000000000000e-01,
--1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
--1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
--1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
--1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
--1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
--1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
--1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
--1.0000000000000000e+00, -1.0000000000000000e+00, -1.0000000000000000e+00,
- 1.0000000000000000e+00, -1.0000000000000000e+00, -4.7000000000000000e+01,
--8.6999999999999993e+00, -9.0000000000000000e+01, -5.0000000000000000e+01,
--1.0000000000000000e+01, -1.0000000000000000e+01, -9.3000000000000000e+01,
--8.9000000000000000e+01,  1.0000000000000000e+00, -1.0000000000000000e+00,
--4.7000000000000000e+01, -8.6999999999999993e+00, -9.0000000000000000e+01,
--5.0000000000000000e+01, -1.0000000000000000e+01, -1.0000000000000000e+01,
--8.9000000000000000e+01, -8.5000000000000000e+01,  1.0000000000000000e+00,
--1.0000000000000000e+00, -4.7000000000000000e+01, -8.6999999999999993e+00,
--9.0000000000000000e+01, -5.0000000000000000e+01, -1.0000000000000000e+01,
--1.0000000000000000e+01, -9.1000000000000000e+01, -8.8000000000000000e+01,
--1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
- 1.0000000000000000e+00,  5.0000000000000000e-01, -1.0000000000000000e+00,
- 5.0000000000000000e-01, -1.0000000000000000e+00,  1.0000000000000000e+00,
- 5.0000000000000000e-01, -1.0000000000000000e+00,  5.0000000000000000e-01,
--1.0000000000000000e+00,  1.0000000000000000e+00,  5.0000000000000000e-01};
-
-real_t g[] = {+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
-	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
-	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
-	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
-	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
-	+0e+00, +0e+00, -2e+00, -2e+00, -2e+00, -2e+00, -2e+00, -2e+00, -2e+00,
-	-2e+00, +0e+00, -2e+00, +0e+00, +2e-03, +2e-03, +2e-03, +2e-03, +2e-03,
-	+2e-03, +1e-03, +2e-03, +2e-03, +2e-03, +0e+00, -2e-03, -2e-03, -2e-03,
-	-2e-03, -2e-03, -2e-03, -1e-03, -2e-03, -2e-03, -2e-03, +0e+00, +0e+00,
-	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +2e-03,
-	+2e-03, +2e-03, +2e-03, +2e-03, +2e-03, +1e-03, +2e-03, +2e-03, +2e-03,
-	+0e+00, -2e-03, -2e-03, -2e-03, -2e-03, -2e-03, -2e-03, -1e-03, -2e-03,
-	-2e-03, -2e-03, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
-	+0e+00, +0e+00, +0e+00, +2e-03, +2e-03, +2e-03, +2e-03, +2e-03, +2e-03,
-	+1e-03, +2e-03, +2e-03, +2e-03, +0e+00, -2e-03, -2e-03, -2e-03, -2e-03,
-	-2e-03, -2e-03, -1e-03, -2e-03, -2e-03, -2e-03, +0e+00, +0e+00, +0e+00,
-	+0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +1e-01, +1e-01,
-	+1e-01, +1e-01, +1e-01, +1e-01, +1e-01, +1e-01, +1e-01, +1e-01, +0e+00,
-	-1e-01, -1e-01, -1e-01, -1e-01, -1e-01, -1e-01, -1e-01, -1e-01, -1e-01,
-	-1e-01, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00, +0e+00,
-	+0e+00};
-
-real_t lb[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, -Inf, 0, -Inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 5,
-	10, 5, 0, 10, 0, 2, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 10, 5, 10, 5, 0, 10, 0, 5, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 5, 10, 5, 0, 10, 0, 5, 0, 10, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-
-real_t ub[] = {Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
-	Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
-	Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
-	Inf, Inf, 0, 92, 39, 87, 29, 0, 20, 0, 28, 20, 71, Inf, 130, 45, 53, 55, 75,
-	112, 0, 73, 480, 154, 121, 50, 30, 77, 20, 0, 18, 0, 5, 20, 71, Inf, Inf,
-	Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, 130, 55, 93, 60, 75, 115, 0, 67,
-	480, 154, 121, 50, 20, 37, 15, 0, 15, 0, 8, 20, 71, Inf, Inf, Inf, Inf, Inf,
-	Inf, Inf, Inf, Inf, Inf, 130, 55, 93, 60, 75, 105, 0, 67, 4980, 154, 110,
-	50, 20, 37, 15, 0, 25, 0, 8, 20, 71, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
-	Inf, Inf, 20, 20, 20, 20, 0, 20, 0, 20, 0, 20, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf};
-
-real_t lbA[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -Inf, -Inf,
-	-Inf, -Inf, -Inf, -Inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0};
-
-real_t ubA[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf, Inf,
-	Inf, Inf, Inf, Inf};
-
-long H_nnz =  sizeof(H_val) / sizeof(real_t);
-long A_nnz =  sizeof(A_val) / sizeof(real_t);
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_runAllOqpExamples.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_runAllOqpExamples.cpp
deleted file mode 100644
index 44e4a6ca77b..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_runAllOqpExamples.cpp
+++ /dev/null
@@ -1,177 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_runAllOqpExamples.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2013-2014
- *
- *	Use qpOASES for solving all QP sequences of the Online QP Benchmark 
- *	Collection. In order to run it, you have to download all examples
- *	from http://www.qpOASES.org/onlineQP/.
- */
-
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-/** Example for qpOASES main function using the OQP interface. */
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	/* 1) Define benchmark arguments. */
-	BooleanType isSparse = BT_FALSE;
-	BooleanType useHotstarts;
-
-	Options options;
-	options.setToMPC();
-	options.printLevel = PL_LOW;
-
-	int maxAllowedNWSR;
-	real_t maxNWSR, avgNWSR, maxCPUtime, avgCPUtime;
-	real_t maxStationarity, maxFeasibility, maxComplementarity;
-
-	const int numBenchmarks = 4; //5
-	const char *benchmarkPath[numBenchmarks];
-	benchmarkPath[0] = "../testing/cpp/data/oqp/chain80/";
-	benchmarkPath[1] = "../testing/cpp/data/oqp/chain80w/";
-	benchmarkPath[2] = "../testing/cpp/data/oqp/diesel/";
-	benchmarkPath[3] = "../testing/cpp/data/oqp/crane/";
-	//benchmarkPath[4] = "./cpp/data/oqp/CDU/";
-
-
-	/* 2) Run all benchmarks in a loop */
-	for ( int ii=0; ii<2*numBenchmarks; ++ii )
-	{
-		if ( ii%2 == 0 )
-			useHotstarts = BT_FALSE;
-		else
-			useHotstarts = BT_TRUE;
-		
-		maxAllowedNWSR = 1000;
-		maxNWSR = 0.0;
-		avgNWSR = 0.0;
-		maxCPUtime = 1000.0; /* seconds */
-		avgCPUtime =    0.0; /* seconds */
-		maxStationarity    = 0.0;
-		maxFeasibility     = 0.0;
-		maxComplementarity = 0.0;
-
-		if ( runOQPbenchmark(	benchmarkPath[ii/2],
-								isSparse,useHotstarts,
-								options,maxAllowedNWSR,
-								maxNWSR,avgNWSR,maxCPUtime,avgCPUtime,
-								maxStationarity,maxFeasibility,maxComplementarity
-								) != SUCCESSFUL_RETURN )
-		{
-			myPrintf( "In order to run this example, you need to download example no. 02\nfrom the Online QP Benchmark Collection website first!\n" );
-			return TEST_DATA_NOT_FOUND;
-		}
-
-		/* 3) Print results. */
-		printf( "\n\n" );
-		if ( useHotstarts == BT_FALSE )
-			printf( "OQP Benchmark Results for %s (cold-starts):\n", benchmarkPath[ii/2] );
-		else
-			printf( "OQP Benchmark Results for %s (hot-starts):\n", benchmarkPath[ii/2] );
-
-		printf( "=======================================================\n\n" );
-		//printf( "maximum CPU time:             %.2f milliseconds\n",1000.0*maxCPUtime );
-		//printf( "average CPU time:             %.2f milliseconds\n",1000.0*avgCPUtime );
-		//printf( "\n" );
-		printf( "maximum iterations:    %.1f\n",maxNWSR );
-		printf( "average iterations:    %.1f\n",avgNWSR );
-		printf( "\n" );
-		printf( "maximum violation stationarity:     %.3e\n",maxStationarity );
-		printf( "maximum violation feasibility:      %.3e\n",maxFeasibility );
-		printf( "maximum violation complementarity:  %.3e\n",maxComplementarity );
-		printf( "\n" );
-
-		QPOASES_TEST_FOR_TOL( maxStationarity,    1e-9  );
-		QPOASES_TEST_FOR_TOL( maxFeasibility,     1e-11 );
-		QPOASES_TEST_FOR_TOL( maxComplementarity, 1e-10 );
-		
-		switch( ii )
-		{
-			case 0:
-				/* chain80 (cold) */
-				QPOASES_TEST_FOR_TRUE( maxNWSR <= 62.5 );
-				QPOASES_TEST_FOR_TRUE( avgNWSR <=  7.5 );
-				break;
-
-			case 1:
-				/* chain80 (hot) */
-				QPOASES_TEST_FOR_TRUE( maxNWSR <= 19.5 );
-				QPOASES_TEST_FOR_TRUE( avgNWSR <=  2.4 );
-				break;
-
-			case 2:
-				/* chain80w (cold) */
-				QPOASES_TEST_FOR_TRUE( maxNWSR <= 84.5 );
-				QPOASES_TEST_FOR_TRUE( avgNWSR <= 10.1 );
-				break;
-
-			case 3:
-				/* chain80w (hot) */
-				QPOASES_TEST_FOR_TRUE( maxNWSR <= 16.5 );
-				QPOASES_TEST_FOR_TRUE( avgNWSR <=  2.7 );
-				break;
-
-			case 4:
-				/* diesel (cold) */
-				QPOASES_TEST_FOR_TRUE( maxNWSR <= 26.5 );
-				QPOASES_TEST_FOR_TRUE( avgNWSR <=  0.5 );
-				break;
-
-			case 5:
-				/* diesel (hot) */
-				QPOASES_TEST_FOR_TRUE( maxNWSR <= 22.5 );
-				QPOASES_TEST_FOR_TRUE( avgNWSR <=  0.3 );
-				break;
-
-			case 6:
-				/* crane (cold) */
-				QPOASES_TEST_FOR_TRUE( maxNWSR <= 64.5 );
-				QPOASES_TEST_FOR_TRUE( avgNWSR <= 44.0 );
-				break;
-
-			case 7:
-				/* crane (hot) */
-				QPOASES_TEST_FOR_TRUE( maxNWSR <= 42.5 );
-				QPOASES_TEST_FOR_TRUE( avgNWSR <=  0.4 );
-				break;
-		}
-	}
-	
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_sebastien1.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_sebastien1.cpp
deleted file mode 100644
index 56b79cea5f7..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_sebastien1.cpp
+++ /dev/null
@@ -1,74 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/test_sebastien1.cpp
- *	\author Sebastien B.
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Example that caused troubles in an earlier release.
- */
-
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-
-/** Example for qpOASES main function using the OQP interface. */
-int main( )
-{
-	REFER_NAMESPACE_QPOASES real_t solution[2]       = {0.0f, 0.0f};
-	REFER_NAMESPACE_QPOASES real_t expectedFirst[2]  = {0.5f, -1.5f};
-  
-	REFER_NAMESPACE_QPOASES real_t H[2*2] = {1.0f, 0.0f, 0.0f, 0.5f};
-	REFER_NAMESPACE_QPOASES real_t g[2]   = {1.5f, 1.0f};
-
-	REFER_NAMESPACE_QPOASES real_t A[1*2] = {1.0f, 1.0f};
-	REFER_NAMESPACE_QPOASES real_t lbA[1] = {-1.0f};
-	REFER_NAMESPACE_QPOASES real_t ubA[1] = {2.0f};
-
-	REFER_NAMESPACE_QPOASES real_t lb[2]  = {0.5f, -2.0f};
-	REFER_NAMESPACE_QPOASES real_t ub[2]  = {5.0f, 2.0f};
-
-	REFER_NAMESPACE_QPOASES QProblem example(2, 1);
-	REFER_NAMESPACE_QPOASES Options options = example.getOptions();
-	//options.enableFarBounds = REFER_NAMESPACE_QPOASES BT_FALSE;
-	example.setOptions(options);
-	example.setPrintLevel(REFER_NAMESPACE_QPOASES PL_NONE);
-
-	// Solve first QP.
-	int nWSR = 10;
-	QPOASES_TEST_FOR_TRUE( example.init(H, g, A, lb, ub, lbA, ubA, nWSR, NULL) == REFER_NAMESPACE_QPOASES SUCCESSFUL_RETURN );
-	QPOASES_TEST_FOR_TRUE( example.isSolved() == REFER_NAMESPACE_QPOASES BT_TRUE );
-	example.getPrimalSolution(solution);
-
-	printf( "\nxOpt = [ %e, %e ];\n\n", solution[0],solution[1] );
-
-	for (unsigned int i=0; i<2; i++)
-		QPOASES_TEST_FOR_NEAR( solution[i],expectedFirst[i] );
-
-	return TEST_PASSED;
-}
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_vanBarelsUnboundedQP.cpp b/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_vanBarelsUnboundedQP.cpp
deleted file mode 100644
index bc345e388fe..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/cpp/test_vanBarelsUnboundedQP.cpp
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES 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.
- *
- *	qpOASES 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 qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file testing/cpp/debug_vanBarelsUnboundedQP.cpp
- *	\author Hans Joachim Ferreau
- *	\version 3.0
- *	\date 2007-2014
- *
- *	Example that causes troubles when hotstarting.
- */
-
-
-
-#include <qpOASES.hpp>
-#include <qpOASES/UnitTesting.hpp>
-
-#include <stdio.h>
-
-
-
-int main( )
-{
-	USING_NAMESPACE_QPOASES
-
-	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.0 };
-	real_t g[2] = { 1.5, 1.0 };
-		
-	Options options;
-	//options.enableFarBounds = BT_FALSE;
-
-	QProblemB qp(2);
-	qp.setOptions( options );
-
-	int iter = 10;
-	qp.init( H,g,0,0,iter );
-
-	real_t xOpt[2];
-	qp.getPrimalSolution( xOpt );
-	print( xOpt,2 );
-
-	return TEST_PASSED;
-}
-
-
-/*
- *	end of file
- */
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/index.html b/3rdparty/qpOASES/qpOASES-3.0/testing/index.html
deleted file mode 100644
index 03648241cd0..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/index.html
+++ /dev/null
@@ -1,12 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/testing</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/testing</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="Makefile">Makefile</a></li>
-  <li><a href="checkForMemoryLeaks">checkForMemoryLeaks</a></li>
-  <li><a href="cpp/">cpp/</a></li>
-  <li><a href="matlab/">matlab/</a></li>
-  <li><a href="runUnitTests">runUnitTests</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/generateExample.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/generateExample.m
deleted file mode 100644
index 9476d7eca91..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/generateExample.m
+++ /dev/null
@@ -1,25 +0,0 @@
-function [ qpData ] = generateExample( nV,nC, isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC, seed, givenH,givenA )
-
-    if ( nargin < 11 )
-        givenA = [];
-        if ( nargin < 10 )
-            givenH = [];
-        end
-    end
-
-    qpFeatures = setupQpFeaturesStruct( );
-    
-    qpFeatures.nV = nV;
-    qpFeatures.nC = nC;
-    
-    qpFeatures.isSparseH = isSparseH;
-    qpFeatures.isSparseA = isSparseA;
-    
-    qpFeatures.hasLowerB = hasLowerB;
-    qpFeatures.hasUpperB = hasUpperB;
-    qpFeatures.hasLowerC = hasLowerC;
-    qpFeatures.hasUpperC = hasUpperC;
-    
-    qpData = generateRandomQp( qpFeatures,seed, givenH,givenA );
-
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/generateRandomQp.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/generateRandomQp.m
deleted file mode 100644
index 3fd4fd39787..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/generateRandomQp.m
+++ /dev/null
@@ -1,110 +0,0 @@
-function [ qpData ] = generateRandomQp( qpFeatures,randSeed, givenH,givenA )
-
-    if ( nargin < 4 )
-        givenA = [];
-        if ( nargin < 3 )
-            givenH = [];
-        end
-    end
-    
-    if ( isoctave == 0 )
-        if ( nargin < 2 )
-            s = RandStream( 'mt19937ar', 'Seed','shuffle' );
-            RandStream.setGlobalStream(s);
-        else
-            s = RandStream( 'mt19937ar', 'Seed',randSeed );
-            RandStream.setGlobalStream(s);
-        end
-    end
-    
-
-    qpData = setupQpDataStruct( );
-    qpData.nV = qpFeatures.nV;
-    qpData.nC = qpFeatures.nC;
-    
-    
-    % generate random optimal primal solution
-    xFeas = rand( qpFeatures.nV,1 );
-    
-    % generate random optimal dual solution
-    %yOpt = zeros( qpFeatures.nV+qpFeatures.nC,1 );
-    
-    if ( isempty(givenH) > 0 )
-        
-        switch ( qpFeatures.hessianType )
-
-            case 0
-                qpData.H = 100 * rand( qpData.nV,qpData.nV ) - 50;
-                qpData.H = qpData.H' * qpData.H / 2;
-
-            case 1
-                qpData.H = 100 * rand( qpData.nV,round(qpData.nV/2) ) - 50;
-                qpData.H = qpData.H' * qpData.H / 2;
-
-            case 2
-                qpData.H = eye( qpData.nV );
-
-            case 3
-                qpData.H = zeros( qpData.nV,qpData.nV );
-
-        end
-        
-    else
-        qpData.H = givenH;
-    end
-    
-    qpData.g = 1000 * rand( qpFeatures.nV,1 ) - 500;
-    
-    if ( isempty(givenA) > 0 )
-        
-        if ( qpFeatures.nC > 0 )
-            qpData.Ain = 100 * rand( qpData.nC,qpData.nV ) - 50;
-        else
-            qpData.Ain = [];
-        end
-        
-    else
-        qpData.Ain = givenA;
-    end
-    
-    if ( qpFeatures.makeInfeas > 0 )
-        alpha = -0.1;
-        beta  = -0.001;
-    else
-        alpha = 1;
-        beta  = 1;
-    end
-        
-    if ( qpFeatures.hasLowerB > 0 )
-        qpData.lb = xFeas - 3*rand( qpData.nV,1 );
-    else
-        qpData.lb = [];
-    end
-    
-    if ( qpFeatures.hasUpperB > 0 )
-        qpData.ub = xFeas + alpha*3*rand( qpData.nV,1 );
-    else
-        qpData.ub = [];
-    end
-    
-    if ( ( qpFeatures.hasLowerC > 0 ) && ( qpData.nC > 0 ) )
-        qpData.lbA = qpData.Ain*xFeas - 100*rand( qpData.nC,1 );
-    else
-        qpData.lbA = [];
-    end
-    
-    if ( ( qpFeatures.hasUpperC > 0 ) && ( qpData.nC > 0 ) )
-        qpData.ubA = qpData.Ain*xFeas + beta*100*rand( qpData.nC,1 );
-    else
-        qpData.ubA = [];
-    end
-    
-    if ( qpFeatures.isSparseH > 0 )
-        qpData.H = sparse( qpData.H );
-    end
-    
-    if ( qpFeatures.isSparseA > 0 )
-        qpData.Ain = sparse( qpData.Ain );
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/getKktResidual.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/getKktResidual.m
deleted file mode 100644
index 55b17a08da0..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/getKktResidual.m
+++ /dev/null
@@ -1,134 +0,0 @@
-function [kkt,stat,feas,cmpl] = getKktResidual( H,g,A,lb,ub,lbA,ubA, x,y )
-
-    % Tolerance for dual variables considered zero.
-	dualActiveTolerance = 1.0e3 * eps;
-
-	% Initialize residuals
-    stat = 0.0;
-    feas = 0.0;
-    cmpl = 0.0;
-    
-    if ( isempty(H) == 0 )
-        nV = size( H,1 );
-    else
-        nV = size( g,2 );
-    end
-
-    if ( isempty(A) == 0 )
-        nC = size( A,1 );
-    else
-        nC = 0;
-    end
-
-   
-	%% check stationarity
-	for i=1:nV
-        
-		% g term and variable bounds dual term
-        if ( isempty(g) == 0 )
-            sum = g(i) - y(i);
-        else
-            sum = 0 - y(i);
-        end
-
-		% H*x term
-        if ( isempty(H) == 0 )
-            sum = sum + H(i,:) * x;
-        end
-
-		% A'*y term
-        if ( isempty(A) == 0 )
-            sum = sum - A(:,i)' * y(nV+1:nV+nC);
-        end
-		
-		% update stat
-        if (abs(sum) > stat)
-            stat = abs(sum);
-        end
-        
-	end
-
-    %% check primal feasibility and complementarity
-	% variable bounds
-	for i=1:nV
-
-		% feasibility
-        if ( isempty(lb) == 0 )
-            if (lb(i) - x(i) > feas) 
-				feas = lb(i) - x(i);
-            end
-        end
-
-        if ( isempty(ub) == 0 )
-            if (x(i) - ub(i) > feas) 
-				feas = x(i) - ub(i);
-            end
-        end
-
-		% complementarity
-		prod = 0.0;
-
-        if ( isempty(lb) == 0 )
-             if (y(i) > dualActiveTolerance) % lower bound
-				prod = (x(i) - lb(i)) * y(i);
-            end
-        end
-
-        if ( isempty(ub) == 0 )
-            if (y(i) < -dualActiveTolerance) % upper bound
-				prod = (x(i) - ub(i)) * y(i);
-            end
-        end
-
-        if (abs(prod) > cmpl)
-            cmpl = abs(prod);
-        end
-        
-	end
-    
-	% A*x bounds
-    for i=1:nC
-
-        % compute sum = (A*x)_i
-		sum = 0.0;
-        if ( isempty(A) == 0 )
-            sum = sum + A(i,:) * x;
-        end
-
-		% feasibility
-        if ( isempty(lbA) == 0 )
-            if (lbA(i) - sum > feas) 
-				feas = lbA(i) - sum;
-            end
-        end
-
-        if ( isempty(ubA) == 0 )
-            if (sum - ubA(i) > feas) 
-				feas = sum - ubA(i);
-            end
-        end
-
-		% complementarity
-		prod = 0.0;
-
-        if ( isempty(lbA) == 0 )
-            if (y(nV+i) > dualActiveTolerance) % lower bound
-				prod = (sum - lbA(i)) * y(nV+i);
-            end
-        end
-		
-        if ( isempty(ubA) == 0 )
-            if (y(nV+i) < -dualActiveTolerance) % upper bound
-				prod = (sum - ubA(i)) * y(nV+i);
-            end
-        end
-
-        if (abs(prod) > cmpl)
-            cmpl = abs(prod);
-        end
-        
-    end
-    
-    kkt = max( [stat,feas,cmpl] );
-
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/index.html b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/index.html
deleted file mode 100644
index 80bcff84a22..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/index.html
+++ /dev/null
@@ -1,13 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/testing/matlab/auxFiles</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/testing/matlab/auxFiles</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="generateExample.m">generateExample.m</a></li>
-  <li><a href="generateRandomQp.m">generateRandomQp.m</a></li>
-  <li><a href="getKktResidual.m">getKktResidual.m</a></li>
-  <li><a href="isoctave.m">isoctave.m</a></li>
-  <li><a href="setupQpDataStruct.m">setupQpDataStruct.m</a></li>
-  <li><a href="setupQpFeaturesStruct.m">setupQpFeaturesStruct.m</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/isoctave.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/isoctave.m
deleted file mode 100644
index e4726d45db6..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/isoctave.m
+++ /dev/null
@@ -1,21 +0,0 @@
-
-% ISOCTAVE  True if the operating environment is octave.
-%    Usage: t=isoctave();
-% 
-%    Returns 1 if the operating environment is octave, otherwise
-%    0 (Matlab)
-% 
-% ---------------------------------------------------------------
-function t=isoctave()
-%ISOCTAVE  True if the operating environment is octave.
-%   Usage: t=isoctave();
-%
-%   Returns 1 if the operating environment is octave, otherwise
-%   0 (Matlab)
-
-if exist('OCTAVE_VERSION')
-  % Only Octave has this variable.
-  t=1;
-else
-  t=0;
-end;
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/setupQpDataStruct.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/setupQpDataStruct.m
deleted file mode 100644
index 93d9e3c8172..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/setupQpDataStruct.m
+++ /dev/null
@@ -1,18 +0,0 @@
-function [ qpData ] = setupQpDataStruct( )
-
-    qpData = struct(    'H', [], ...        % Hessian matrix
-                        'g', [], ...        % gradient vector
-                        'Aeq', [], ...      % equality constraints matrix
-                        'beq', [], ...      % equality constraints vector
-                        'lb', [], ...       % lower bound vector
-                        'ub', [], ...       % upper bound vector
-                        'Ain', [], ...      % inequality constraints matrix
-                        'lbA', [], ...      % lower constraints vector
-                        'ubA', [], ...      % upper constraints vector
-                        'x0', [], ...       % primal initial guess
-                        'options', [], ...  % QP solver options
-                        'nV', 0, ...        % number of QP variables
-                        'nC', 0 ...         % number of constraints
-                        );
-
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/setupQpFeaturesStruct.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/setupQpFeaturesStruct.m
deleted file mode 100644
index 903a891a715..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/auxFiles/setupQpFeaturesStruct.m
+++ /dev/null
@@ -1,21 +0,0 @@
-function [ qpFeatures ] = setupQpFeaturesStruct( )
-
-    qpFeatures = struct(    'nV',  0, ...
-                            'nIB', 0, ...
-                            'nEB', 0, ...
-                            'nC',  0, ...
-                            'nIC', 0, ...
-                            'nEC', 0, ...
-                            'nActB', 0, ...
-                            'nActC', 0, ...
-                            'hasLowerB', 0, ...
-                            'hasUpperB', 0, ...
-                            'hasLowerC', 0, ...
-                            'hasUpperC', 0, ...
-                            'isSparseH', 0, ...
-                            'isSparseA', 0, ...
-                            'makeInfeas', 0, ...
-                            'hessianType', 0 ... % 0 = pos def; 1 = pos sem def; 2 = id; 3 = zero
-                            );
-
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/data/fetch_matlab_data b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/data/fetch_matlab_data
deleted file mode 100644
index 44cd54eb588..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/data/fetch_matlab_data
+++ /dev/null
@@ -1,34 +0,0 @@
-#!/usr/bin/env bash
-
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-##
-##	Filename:  testing/matlab/data/fetch_matlab_data
-##	Author:    Hans Joachim Ferreau
-##	Version:   3.0
-##	Date:      2014
-
-
-svn export https://projects.coin-or.org/svn/qpOASES/misc/testingdata/matlab . --force
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/data/index.html b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/data/index.html
deleted file mode 100644
index b2561df2fd3..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/data/index.html
+++ /dev/null
@@ -1,8 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/testing/matlab/data</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/testing/matlab/data</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="fetch_matlab_data">fetch_matlab_data</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/index.html b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/index.html
deleted file mode 100644
index 14c3e5dab82..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/index.html
+++ /dev/null
@@ -1,11 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/testing/matlab</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/testing/matlab</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="auxFiles/">auxFiles/</a></li>
-  <li><a href="data/">data/</a></li>
-  <li><a href="runAllTests.m">runAllTests.m</a></li>
-  <li><a href="tests/">tests/</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/runAllTests.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/runAllTests.m
deleted file mode 100644
index c1e671a6f4e..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/runAllTests.m
+++ /dev/null
@@ -1,153 +0,0 @@
-function [ successFlag ] = runAllTests( doPrint )
-    
-    if ( nargin < 1 )
-        doPrint = 0;
-    end
-    
-    
-    %% add sub-folders to Matlab path
-    addpath('../../interfaces/matlab/');
-    addpath(genpath(pwd));
-    addpath(genpath([pwd 'auxFiles']));
-    addpath(genpath([pwd 'data']));
-    addpath(genpath([pwd 'tests']));
-    
-    
-    successFlag = 1;
-    
-    curWarnLevel = warning;
-    warning('off');
-    
-    clc;
-
-    %% run interface tests
-    fprintf( 'Running qpOASES interface tests... ' )
-    successFlag = updateSuccessFlag( successFlag, runInterfaceTest( 10,20, doPrint,42 ) );
-    
-    fprintf( 'Running qpOASES_sequence interface tests... ' )
-    successFlag = updateSuccessFlag( successFlag, runInterfaceSeqTest( 8,5, doPrint,42 ) );
-    
-    
-    %% run functional tests
-    fprintf( 'Running tests with random QPs having identity Hessian... ' )
-    successFlag = updateSuccessFlag( successFlag, runRandomIdHessian( 12,12, doPrint,4242 ) );
-    
-    %fprintf( 'Running tests with random QPs having zero Hessian... ' )
-    %successFlag = updateSuccessFlag( successFlag, runRandomZeroHessian( 11,41, doPrint,4242 ) );
-    
-    fprintf( 'Running alternativeX0 test... ' )
-    successFlag = updateSuccessFlag( successFlag, runAlternativeX0Test( 50,300,doPrint,4242 ) );
-    
-    fprintf( 'Running testAPrioriKnownSeq1... ' )
-    successFlag = updateSuccessFlag( successFlag, runTestAPrioriKnownSeq1( doPrint ) );
-    
-    fprintf( 'Running testSeq... ' )
-    successFlag = updateSuccessFlag( successFlag, runTestSeq( doPrint ) );
-    
-    fprintf( 'Running testSparse... ' )
-    successFlag = updateSuccessFlag( successFlag, runTestSparse( doPrint ) );
-    
-    fprintf( 'Running testSparse2... ' )
-    successFlag = updateSuccessFlag( successFlag, runTestSparse2( doPrint ) );
-    
-    fprintf( 'Running testSparse3... ' )
-    successFlag = updateSuccessFlag( successFlag, runTestSparse3( doPrint ) );
-    
-    fprintf( 'Running testSparse4... ' )
-    successFlag = updateSuccessFlag( successFlag, runTestSparse4( doPrint ) );
-    
-    fprintf( 'Running simpleSpringExample... ' )
-    successFlag = updateSuccessFlag( successFlag, runSimpleSpringExample( doPrint ) );
-    
-    fprintf( 'Running vanBarelsUnboundedQP... ' )
-    successFlag = updateSuccessFlag( successFlag, runVanBarelsUnboundedQP( doPrint ) );
-    
-    fprintf( 'Running alexInfeas1... ' )
-    successFlag = updateSuccessFlag( successFlag, runAlexInfeas1( doPrint ) );
-    
-    %fprintf( 'Running alexInfeas2... ' )
-    %successFlag = updateSuccessFlag( successFlag, runAlexInfeas2( doPrint ) );
-
-    %fprintf( 'Running QAP8... ' )
-    %successFlag = updateSuccessFlag( successFlag, runQAP( doPrint ) );
-    
-    fprintf( 'Running testWorkingSetLI... ' )
-    successFlag = updateSuccessFlag( successFlag, runTestWorkingSetLI( doPrint ) );
-    
-
-    fprintf( 'Running EXAMPEL1... ' );
-    successFlag = updateSuccessFlag( successFlag, runBenchmarkEXAMPLE1( 10 ) );
-	
-    fprintf( 'Running EXAMPLE1A... ' );
-    successFlag = updateSuccessFlag( successFlag, runBenchmarkEXAMPLE1A( 10 ) );
-	
-    fprintf( 'Running EXAMPLE1B... ' );
-    successFlag = updateSuccessFlag( successFlag, runBenchmarkEXAMPLE1B( 10 ) );
-
-	fprintf( 'Running CHAIN1... ' );
-    successFlag = updateSuccessFlag( successFlag, runBenchmarkCHAIN1( 20 ) );
-	
-    fprintf( 'Running CHAIN1A... ' );
-    successFlag = updateSuccessFlag( successFlag, runBenchmarkCHAIN1A( 20 ) );
-
-	fprintf( 'Running CRANE1... ' );
-    successFlag = updateSuccessFlag( successFlag, runBenchmarkCRANE1( 100 ) );
-
-    fprintf( 'Running CRANE2... ' );
-    successFlag = updateSuccessFlag( successFlag, runBenchmarkCRANE2( 100 ) );
-
-    fprintf( 'Running CRANE3... ' );
-    successFlag = updateSuccessFlag( successFlag, runBenchmarkCRANE3( 100 ) );
-	
-	fprintf( 'Running EQUALITY1... ' );
-    successFlag = updateSuccessFlag( successFlag, runBenchmarkEQUALITY1( 100 ) );
-
-    fprintf( 'Running EQUALITY2... ' );
-    successFlag = updateSuccessFlag( successFlag, runBenchmarkEQUALITY2( 3200 ) );
-
-    %fprintf( 'Running IDHESSIAN1... ' );
-	%successFlag = updateSuccessFlag( successFlag, runBenchmarkIDHESSIAN1( 1200 ) );
-
-    fprintf( 'Running DIESEL... ' );
-    successFlag = updateSuccessFlag( successFlag, runBenchmarkDIESEL( 230 ) );
-    
-    fprintf( 'Running QSHARE1B... ' )
-    successFlag = updateSuccessFlag( successFlag, runQSHARE1B( doPrint ) );
-
-    
-    
-    %% display results
-    disp( ' ' );
-    
-    if ( successFlag == 0 )
-        disp( 'At least one test failed!' );
-    else
-        disp( 'All available tests passed successfully!' );
-    end
-
-    warning( curWarnLevel );
-    
-end
-
-
-function [ newSuccessFlag ] = updateSuccessFlag( curSuccessFlag,curResult )
-
-    switch ( curResult )
-        
-        case 0
-            newSuccessFlag = 0;
-            fprintf( 'failed!\n' );
-        
-        case 1          
-            newSuccessFlag = curSuccessFlag;
-            fprintf( 'passed!\n' );
-            
-        case -1
-            newSuccessFlag = curSuccessFlag;
-            fprintf( 'problem data missing!\n' );
-            
-        otherwise
-            error( 'Unknown success flag!' );
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/index.html b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/index.html
deleted file mode 100644
index 366b7aa9077..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/index.html
+++ /dev/null
@@ -1,37 +0,0 @@
-<html><head><title>qpOASES - Revision 198: /stable/3.0/testing/matlab/tests</title></head>
-<body>
- <h2>qpOASES - Revision 198: /stable/3.0/testing/matlab/tests</h2>
- <ul>
-  <li><a href="../">..</a></li>
-  <li><a href="runAlexInfeas1.m">runAlexInfeas1.m</a></li>
-  <li><a href="runAlexInfeas2.m">runAlexInfeas2.m</a></li>
-  <li><a href="runAlternativeX0Test.m">runAlternativeX0Test.m</a></li>
-  <li><a href="runBenchmarkCHAIN1.m">runBenchmarkCHAIN1.m</a></li>
-  <li><a href="runBenchmarkCHAIN1A.m">runBenchmarkCHAIN1A.m</a></li>
-  <li><a href="runBenchmarkCRANE1.m">runBenchmarkCRANE1.m</a></li>
-  <li><a href="runBenchmarkCRANE2.m">runBenchmarkCRANE2.m</a></li>
-  <li><a href="runBenchmarkCRANE3.m">runBenchmarkCRANE3.m</a></li>
-  <li><a href="runBenchmarkDIESEL.m">runBenchmarkDIESEL.m</a></li>
-  <li><a href="runBenchmarkEQUALITY1.m">runBenchmarkEQUALITY1.m</a></li>
-  <li><a href="runBenchmarkEQUALITY2.m">runBenchmarkEQUALITY2.m</a></li>
-  <li><a href="runBenchmarkEXAMPLE1.m">runBenchmarkEXAMPLE1.m</a></li>
-  <li><a href="runBenchmarkEXAMPLE1A.m">runBenchmarkEXAMPLE1A.m</a></li>
-  <li><a href="runBenchmarkEXAMPLE1B.m">runBenchmarkEXAMPLE1B.m</a></li>
-  <li><a href="runBenchmarkIDHESSIAN1.m">runBenchmarkIDHESSIAN1.m</a></li>
-  <li><a href="runInterfaceSeqTest.m">runInterfaceSeqTest.m</a></li>
-  <li><a href="runInterfaceTest.m">runInterfaceTest.m</a></li>
-  <li><a href="runQAP8.m">runQAP8.m</a></li>
-  <li><a href="runQSHARE1B.m">runQSHARE1B.m</a></li>
-  <li><a href="runRandomIdHessian.m">runRandomIdHessian.m</a></li>
-  <li><a href="runRandomZeroHessian.m">runRandomZeroHessian.m</a></li>
-  <li><a href="runSimpleSpringExample.m">runSimpleSpringExample.m</a></li>
-  <li><a href="runTestAPrioriKnownSeq1.m">runTestAPrioriKnownSeq1.m</a></li>
-  <li><a href="runTestSeq.m">runTestSeq.m</a></li>
-  <li><a href="runTestSparse.m">runTestSparse.m</a></li>
-  <li><a href="runTestSparse2.m">runTestSparse2.m</a></li>
-  <li><a href="runTestSparse3.m">runTestSparse3.m</a></li>
-  <li><a href="runTestSparse4.m">runTestSparse4.m</a></li>
-  <li><a href="runTestWorkingSetLI.m">runTestWorkingSetLI.m</a></li>
-  <li><a href="runVanBarelsUnboundedQP.m">runVanBarelsUnboundedQP.m</a></li>
- </ul>
-</body></html>
\ No newline at end of file
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlexInfeas1.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlexInfeas1.m
deleted file mode 100644
index d1977ed3fe4..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlexInfeas1.m
+++ /dev/null
@@ -1,30 +0,0 @@
-function [ successFlag ] = runAlexInfeas1( doPrint )
-    
-    if ( nargin < 1 )
-        doPrint = 0;
-    end
-
-    successFlag = 0;
-    
-    try
-        data = load( 'alexInfeas1.mat' );
-    catch
-        successFlag = -1;
-        return;
-    end
-    
-    options1 = qpOASES_options( 'default', 'printLevel',-2*doPrint );
-    
-    [x1,dummy,exitflag1] = qpOASES( data.H,data.g,data.A, ...
-                data.lb,data.ub,data.lbA,data.ubA, options1 ); %#ok<*NASGU>
-
-    % should return "QP infeasible"
-    if ( exitflag1 == -2 )
-        successFlag = 1;
-    else
-        if ( doPrint > 0 )
-            disp( [data.lbA data.A*x1 data.ubA] );
-        end
-    end
-   
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlexInfeas2.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlexInfeas2.m
deleted file mode 100644
index 730a01c4871..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlexInfeas2.m
+++ /dev/null
@@ -1,35 +0,0 @@
-function [ successFlag ] = runAlexInfeas2( doPrint )
-    
-    if ( nargin < 1 )
-        doPrint = 0;
-    end
-
-    successFlag = 0;
-    
-    try
-        data = load( 'alexInfeas2.mat' );
-    catch
-        successFlag = -1;
-        return;
-    end
-    
-    options1 = qpOASES_options( 'default', 'printLevel',-2*doPrint,... 
-                                           'maxIter',7000, 'enableFlippingBounds',0,...
-                                           'terminationTolerance',1e7*eps);
-                                       
-    options2 = qpOASES_options( 'MPC', 'printLevel',-2*doPrint,... 
-                                       'maxIter',3000, 'terminationTolerance',1e8*eps );
-    
-    [x1,dummy,exitflag1] = qpOASES( data.H,data.g,data.A, ...
-                data.lb,data.ub,data.lbA,data.ubA, options1 ); %#ok<*NASGU>
-
-    % should return "QP infeasible"
-    if ( exitflag1 == -2 )
-        successFlag = 1;
-    else
-        if ( doPrint > 0 )
-            %disp( [data.lbA data.A*x1 data.ubA] );
-        end
-    end
-   
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlternativeX0Test.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlternativeX0Test.m
deleted file mode 100644
index 3178ed221e5..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runAlternativeX0Test.m
+++ /dev/null
@@ -1,84 +0,0 @@
-function [ successFlag ] = runAlternativeX0Test( nV,nC, doPrint,seed )
-    
-    if ( nargin < 4 )
-        seed = 4242;
-        if ( nargin < 3 )
-            doPrint = 0;
-            if ( nargin < 2 )
-                nC = 300;
-                if ( nargin < 1 )
-                    nV = 50;
-                end
-            end
-        end
-    end
-
-    successFlag = 1;
-    TOL = eps;
-    
-    qpData = generateExample( nV,nC, 0,0, 0,1,1,1, seed );
-    
-    H = qpData.H;
-	g = qpData.g;
-    A = [qpData.Aeq;qpData.Ain];
-	lb = qpData.lb;
-    ub = qpData.ub;
-    lbA = [qpData.beq;qpData.lbA];
-    ubA = [qpData.beq;qpData.ubA];
-    
-    x0 = nV*rand( nV,1 );
-    
-    options = qpOASES_options( 'default', 'printLevel',2*doPrint );
-    auxInput = qpOASES_auxInput( 'x0',x0 );
-    
-    [ x1,f1,e1,i1,l1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],x0 );
-    [ x2,f2,e2,i2,l2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-    [ x3,f3,e3,i3,l3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,x0 );
-    [ x4,f4,e4,i4,l4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-    
-    if ( ( norm(x1-x2) > TOL ) || ...
-         ( norm(x1-x3) > TOL ) || ...
-         ( norm(x1-x4) > TOL ) )
-        if ( doPrint > 0 )
-            disp('diff in x')
-        end
-        successFlag = 0;
-    end
-    
-    if ( ( norm(f1-f2) > TOL ) || ...
-         ( norm(f1-f3) > TOL ) || ...
-         ( norm(f1-f4) > TOL ) )
-        if ( doPrint > 0 )
-            disp('diff in fval')
-        end
-        successFlag = 0;
-    end
-    
-    if ( ( norm(e1-e2) > TOL ) || ...
-         ( norm(e1-e3) > TOL ) || ...
-         ( norm(e1-e4) > TOL ) )
-        if ( doPrint > 0 )
-            disp('diff in exitflag')
-        end
-        successFlag = 0;
-    end
-    
-    if ( ( norm(i1-i2) > TOL ) || ...
-         ( norm(i1-i3) > TOL ) || ...
-         ( norm(i1-i4) > TOL ) )
-        if ( doPrint > 0 )
-            disp('diff in iter')
-        end
-        successFlag = 0;
-    end
-    
-    if ( ( norm(l1-l2) > TOL ) || ...
-         ( norm(l1-l3) > TOL ) || ...
-         ( norm(l1-l4) > TOL ) )
-        if ( doPrint > 0 )
-            disp('diff in lambda')
-        end
-        successFlag = 0;
-    end
-		
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCHAIN1.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCHAIN1.m
deleted file mode 100644
index 70d8a33c480..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCHAIN1.m
+++ /dev/null
@@ -1,56 +0,0 @@
-function [ successFlag ] = runBenchmarkCHAIN1( nWSR )
-
-    successFlag = 0;
-	maxViolation = 0;
-
-    clear H g A lb ub lbA ubA;
-    
-    try
-        load 'benchmarkCHAIN1.mat';
-    catch
-        successFlag = -1;
-        return;
-    end
-	
-    if ( exist( 'A','var' ) )
-        [nC,nV] = size(A);
-    else
-        nC = 0;
-    end
-    [nV,nP] = size(g);
-
-    xOpt = zeros(nV,nP);
-    yOpt = zeros(nV+nC,nP);
-    objOpt = zeros(1,nP);
-    iter = zeros(1,nP);
-
-	options = qpOASES_options( 'fast','maxIter',nWSR );
-    %options = qpOASES_options( 'maxIter',nWSR );
-
-	for i=1:nP
-		%disp(i);
-
-		if ( i == 1 )
-			[QP,x,obj,status,nWSRout,lambda,ws] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-            %disp(ws')
-		else
-			[x,obj,status,nWSRout,lambda,ws] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-            %disp(ws')
-        end
-
-		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
-		maxViolation = max( [maxViolation,maxViolationTMP] );
-        
-        xOpt(:,i) = x;
-        yOpt(:,i) = lambda;
-        objOpt(:,i) = obj;
-        iter(:,i) = nWSRout;
-	end
-	
-	qpOASES_sequence( 'c',QP );
-    
-    if ( ( maxViolation < 9e-13 ) && ( status == 0 ) )
-        successFlag = 1;
-    end
-	
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCHAIN1A.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCHAIN1A.m
deleted file mode 100644
index ed5272c60c2..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCHAIN1A.m
+++ /dev/null
@@ -1,57 +0,0 @@
-function [ successFlag ] = runBenchmarkCHAIN1A( nWSR )
-
-    successFlag = 0;
-	maxViolation = 0;
-
-    clear H g A lb ub lbA ubA;
-    
-    try
-        load 'benchmarkCHAIN1A.mat';
-    catch
-        successFlag = -1;
-        return;
-    end
-	
-    if ( exist( 'A','var' ) )
-        [nC,nV] = size(A);
-    else
-        nC = 0;
-    end
-    [nV,nP] = size(g);
-
-    xOpt = zeros(nV,nP);
-    yOpt = zeros(nV+nC,nP);
-    objOpt = zeros(1,nP);
-    
-	options = qpOASES_options( 'fast','maxIter',nWSR );
-    %options = qpOASES_options( 'reliable', 'maxIter',500,'printLevel',1 );
-% ,'enableRamping',0,'enableFlippingBounds',0,'enableFullLITests',0,'enableDriftCorrection',0
-  
-  
-	for jj=1:1
-  
-	for i=1:nP
-		%disp(i);
-		if ( i == 1 )
-            [QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),lb(:,i),ub(:,i),options );
-		else
-            [x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),options );
-        end
-
-		[ maxViolationTMP ] = getKktResidual( H,g(:,i),[],lb(:,i),ub(:,i),[],[], x,lambda );
-		maxViolation = max( [maxViolation,maxViolationTMP] );
-
-        xOpt(:,i) = x;
-        yOpt(:,i) = lambda;
-        objOpt(:,i) = obj;
-	end
-	
-	qpOASES_sequence( 'c',QP );
-
-    end
-    
-    if ( ( maxViolation < 1e-14 ) && ( status == 0 ) )
-        successFlag = 1;
-    end   
-	
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE1.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE1.m
deleted file mode 100644
index 4016aa86255..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE1.m
+++ /dev/null
@@ -1,51 +0,0 @@
-function [ successFlag ] = runBenchmarkCRANE1( nWSR )
-
-    successFlag = 0;
-	maxViolation = 0;
-
-    clear H g A lb ub lbA ubA;
-	
-    try
-        load 'benchmarkCRANE1.mat';
-    catch
-        successFlag = -1;
-        return;
-    end
-
-    if ( exist( 'A','var' ) )
-        [nC,nV] = size(A);
-    else
-        nC = 0;
-    end
-    [nV,nP] = size(g);
-
-    xOpt = zeros(nV,nP);
-    yOpt = zeros(nV+nC,nP);
-    objOpt = zeros(1,nP);
-
-	options = qpOASES_options( 'fast','maxIter',nWSR );
-
-    for i=1:nP
-		%disp(i);
-
-		if ( i == 1 )
-			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		else
-			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		end
-
-		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
-		maxViolation = max( [maxViolation,maxViolationTMP] );
-
-        xOpt(:,i) = x;
-        yOpt(:,i) = lambda;
-        objOpt(:,i) = obj;
-	end
-	
-	qpOASES_sequence( 'c',QP );
-
-    if ( ( maxViolation < 3e-9 ) && ( status == 0 ) )
-        successFlag = 1;
-    end
-	
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE2.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE2.m
deleted file mode 100644
index 384a4195dd2..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE2.m
+++ /dev/null
@@ -1,52 +0,0 @@
-function [ successFlag ] = runBenchmarkCRANE2( nWSR )
-
-    successFlag = 0;
-	maxViolation = 0;
-
-    clear H g A lb ub lbA ubA;
-	
-    try
-        load 'benchmarkCRANE2.mat';
-	catch
-        successFlag = -1;
-        return;
-    end
-    
-    if ( exist( 'A','var' ) )
-        [nC,nV] = size(A);
-    else
-        nC = 0;
-    end
-    [nV,nP] = size(g);
-    
-    xOpt = zeros(nV,nP);
-    yOpt = zeros(nV+nC,nP);
-    objOpt = zeros(1,nP);
-    
-	%options = qpOASES_options( 'maxIter',nWSR );
-    options = qpOASES_options( 'fast','maxIter',nWSR );
-
-	for i=1:nP
-		%disp(i);
-
-		if ( i == 1 )
-			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		else
-			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'm',QP,H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		end
-
-		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
-		maxViolation = max( [maxViolation,maxViolationTMP] );
-
-        xOpt(:,i) = x;
-        yOpt(:,i) = lambda;
-        objOpt(:,i) = obj;
-	end
-
-	qpOASES_sequence( 'c',QP );
-
-    if ( ( maxViolation < 1e-11 ) && ( status == 0 ) )
-        successFlag = 1;
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE3.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE3.m
deleted file mode 100644
index f62ac4ca36a..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkCRANE3.m
+++ /dev/null
@@ -1,51 +0,0 @@
-function [ successFlag ] = runBenchmarkCRANE3( nWSR )
-
-    successFlag = 0;
-	maxViolation = 0;
-
-    clear H g A lb ub lbA ubA;
-    try
-        load 'benchmarkCRANE3.mat';
-    catch
-        successFlag = -1;
-        return;
-    end
-
-    if ( exist( 'A','var' ) )
-        [nC,nV] = size(A);
-    else
-        nC = 0;
-    end
-    [nV,nP] = size(g);
-
-    xOpt = zeros(nV,nP);
-    yOpt = zeros(nV+nC,nP);
-    objOpt = zeros(1,nP);
-
-	%options = qpOASES_options( 'maxIter',nWSR );
-    options = qpOASES_options( 'fast','maxIter',nWSR );
-
-	for i=1:nP
-		%disp(i);
-
-		if ( i == 1 )
-			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		else
-			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		end
-
-		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
-		maxViolation = max( [maxViolation,maxViolationTMP] );
-
-        xOpt(:,i) = x;
-        yOpt(:,i) = lambda;
-        objOpt(:,i) = obj;
-	end
-	
-	qpOASES_sequence( 'c',QP );
-
-    if ( ( maxViolation < 5e-11 ) && ( status == 0 ) )
-        successFlag = 1;
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkDIESEL.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkDIESEL.m
deleted file mode 100644
index 36ea449fcc0..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkDIESEL.m
+++ /dev/null
@@ -1,55 +0,0 @@
-function [ successFlag ] = runBenchmarkDIESEL( nWSR )
-
-    successFlag = 0;
-	maxViolation = 0;
-
-    clear H g A lb ub lbA ubA;
-    try
-        load 'benchmarkDIESEL.mat';
-    catch
-        successFlag = -1;
-        return;
-    end
-	
-    if ( exist( 'A','var' ) )
-        [nC,nV] = size(A);
-    else
-        nC = 0;
-		nEC = 0;
-    end
-    [nV,nP] = size(g);
-
-    xOpt = zeros(nV,nP);
-    yOpt = zeros(nV+nC,nP);
-    objOpt = zeros(1,nP);
-    iter = zeros(1,nP);
-
-    options = qpOASES_options( 'fast','maxIter',nWSR );
-    %options = qpOASES_options( 'maxIter',nWSR );
-    %options = qpOASES_options( 'maxIter',nWSR, 'initialStatusBounds',0 );
-
-	for i=1:nP
-		%disp(i);
-
-		if ( i == 1 )
-			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		else
-			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-        end
-
-		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
-		maxViolation = max( [maxViolation,maxViolationTMP] );
-        
-        xOpt(:,i) = x;
-        yOpt(:,i) = lambda;
-        objOpt(:,i) = obj;
-        iter(:,i) = nWSRout;
-	end
-	
-	qpOASES_sequence( 'c',QP );
-    
-    if ( ( maxViolation < 6e-13 ) && ( status == 0 ) )
-        successFlag = 1;
-    end	
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEQUALITY1.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEQUALITY1.m
deleted file mode 100644
index e21fd27b3c7..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEQUALITY1.m
+++ /dev/null
@@ -1,52 +0,0 @@
-function [ successFlag ] = runBenchmarkEQUALITY1( nWSR )
-
-    successFlag = 0;
-	maxViolation = 0;
-
-    clear H g A lb ub lbA ubA;
-	
-    try
-        load 'benchmarkEQUALITY1.mat';
-    catch
-        successFlag = -1;
-        return;
-    end
-
-   	if ( exist( 'A','var' ) )
-        [nC,nV] = size(A);
-    else
-        nC = 0;
-    end
-    [nV,nP] = size(g);
-
-    xOpt = zeros(nV,nP);
-    yOpt = zeros(nV+nC,nP);
-    objOpt = zeros(1,nP);
-    
-	%options = qpOASES_options( 'maxIter',nWSR );
-    options = qpOASES_options( 'fast','maxIter',nWSR );
-
-	for i=1:nP
-		%disp(i);
-
-		if ( i == 1 )
-			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		else
-			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		end
-
-		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
-		maxViolation = max( [maxViolation,maxViolationTMP] );
-
-        xOpt(:,i) = x;
-        yOpt(:,i) = lambda;
-        objOpt(:,i) = obj;
-	end
-	
-	qpOASES_sequence( 'c',QP );
-
-    if ( ( maxViolation < 4e-15 ) && ( status == 0 ) )
-        successFlag = 1;
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEQUALITY2.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEQUALITY2.m
deleted file mode 100644
index c743047c4ae..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEQUALITY2.m
+++ /dev/null
@@ -1,56 +0,0 @@
-function [ successFlag ] = runBenchmarkEQUALITY2( nWSR )
-
-    successFlag = 0;
-	maxViolation = 0;
-
-    clear H g A lb ub lbA ubA;
-	
-    try
-        load 'benchmarkEQUALITY2.mat';
-    catch
-        successFlag = -1;
-        return;
-    end
-
-   	if ( exist( 'A','var' ) )
-        [nC,nV] = size(A);
-    else
-        nC = 0;
-    end
-    [nV,nP] = size(g);
-
-    xOpt = zeros(nV,nP);
-    yOpt = zeros(nV+nC,nP);
-    objOpt = zeros(1,nP);
-   
-    %H = H;
-    %lbA(65:320) = lbA(65:320) - 0.000000000;
-    %ubA(65:320) = ubA(65:320) + 0.000000000;
-
-	options = qpOASES_options( 'maxIter',nWSR, 'enableEqualities',1, 'printLevel',0 );
-    %options = qpOASES_options( 'fast','maxIter',nWSR, 'printLevel',-2 );
-
-	for i=1:nP
-		%disp(i);
-
-		if ( i == 1 )
-			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		else
-			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		end
-
-		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
-		maxViolation = max( [maxViolation,maxViolationTMP] );
-
-        xOpt(:,i) = x;
-        yOpt(:,i) = lambda;
-        objOpt(:,i) = obj;
-	end
-	
-	qpOASES_sequence( 'c',QP );
-
-    if ( ( maxViolation < 3e-7 ) && ( status == 0 ) )
-        successFlag = 1;
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1.m
deleted file mode 100644
index 338e95232ac..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1.m
+++ /dev/null
@@ -1,52 +0,0 @@
-function [ successFlag ] = runBenchmarkEXAMPLE1( nWSR )
-
-    successFlag = 0;
-	maxViolation = 0;
-
-    clear H g A lb ub lbA ubA;
-	
-    try
-        load 'benchmarkEXAMPLE1.mat';
-    catch
-        successFlag = -1;
-        return;
-    end
-	
-    if ( exist( 'A','var' ) )
-        [nC,nV] = size(A);
-    else
-        nC = 0;
-    end
-    [nV,nP] = size(g);
-
-    xOpt = zeros(nV,nP);
-    yOpt = zeros(nV+nC,nP);
-    objOpt = zeros(1,nP);
-    
-	%options = qpOASES_options( 'maxIter',nWSR );
-    options = qpOASES_options( 'fast','maxIter',nWSR );
-
-	for i=1:nP
-		%disp(i);
-
-		if ( i == 1 )
-			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		else
-			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		end
-
-		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
-		maxViolation = max( [maxViolation,maxViolationTMP] );
-
-        xOpt(:,i) = x;
-        yOpt(:,i) = lambda;
-        objOpt(:,i) = obj;
-	end
-	
-	qpOASES_sequence( 'c',QP );
-	
-    if ( ( maxViolation < 1e-13 ) && ( status == 0 ) )
-        successFlag = 1;
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1A.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1A.m
deleted file mode 100644
index 22811e5dc80..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1A.m
+++ /dev/null
@@ -1,52 +0,0 @@
-function [ successFlag ] = runBenchmarkEXAMPLE1A( nWSR )
-
-    successFlag = 0;
-    maxViolation = 0;
-
-    clear H g A lb ub lbA ubA;
-	
-    try
-        load 'benchmarkEXAMPLE1A.mat';
-    catch
-        successFlag = -1;
-        return;
-    end
-
-    if ( exist( 'A','var' ) )
-        [nC,nV] = size(A);
-    else
-        nC = 0;
-    end
-    [nV,nP] = size(g);
-
-    xOpt = zeros(nV,nP);
-    yOpt = zeros(nV+nC,nP);
-    objOpt = zeros(1,nP);
-    
-	%options = qpOASES_options( 'maxIter',nWSR );
-    options = qpOASES_options( 'fast','maxIter',nWSR );
-
-	for i=1:2
-		%disp(i);
-
-		if ( i == 1 )
-			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H(:,:,i),g(:,i),A(:,:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		else
-			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'm',QP,H(:,:,i),g(:,i),A(:,:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		end
-
-		[ maxViolationTMP ] = getKktResidual( H(:,:,i),g(:,i),A(:,:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
-		maxViolation = max( [maxViolation,maxViolationTMP] );
-
-        xOpt(:,i) = x;
-        yOpt(:,i) = lambda;
-        objOpt(:,i) = obj;
-	end
-	
-	qpOASES_sequence( 'c',QP );
-
-    if ( ( maxViolation < 3e-13 ) && ( status == 0 ) )
-        successFlag = 1;
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1B.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1B.m
deleted file mode 100644
index 74fc36f5317..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkEXAMPLE1B.m
+++ /dev/null
@@ -1,52 +0,0 @@
-function [ successFlag ] = runBenchmarkEXAMPLE1B( nWSR )
-
-    successFlag = 0;
-	maxViolation = 0;
-
-    clear H g A lb ub lbA ubA;
-	
-    try
-        load 'benchmarkEXAMPLE1B.mat';
-    catch
-        successFlag = -1;
-        return;
-    end
-	
-    if ( exist( 'A','var' ) )
-        [nC,nV] = size(A);
-    else
-        nC = 0;
-    end
-    [nV,nP] = size(g);
-
-    xOpt = zeros(nV,nP);
-    yOpt = zeros(nV+nC,nP);
-    objOpt = zeros(1,nP);
-    
-	%options = qpOASES_options( 'maxIter',nWSR );
-    options = qpOASES_options( 'fast','maxIter',nWSR );
-
-	for i=1:nP
-		%disp(i);
-
-		if ( i == 1 )
-			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),lb(:,i),ub(:,i),options );
-		else
-			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),options );
-		end
-
-		[ maxViolationTMP ] = getKktResidual( H,g(:,i),[],lb(:,i),ub(:,i),[],[], x,lambda );
-		maxViolation = max( [maxViolation,maxViolationTMP] );
-
-        xOpt(:,i) = x;
-        yOpt(:,i) = lambda;
-        objOpt(:,i) = obj;
-	end
-	
-	qpOASES_sequence( 'c',QP );
-
-    if ( ( maxViolation < 3e-12 ) && ( status == 0 ) )
-        successFlag = 1;
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkIDHESSIAN1.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkIDHESSIAN1.m
deleted file mode 100644
index 39920038a40..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runBenchmarkIDHESSIAN1.m
+++ /dev/null
@@ -1,55 +0,0 @@
-function [ successFlag ] = runBenchmarkIDHESSIAN1( nWSR )
-
-    successFlag = 0;
-	maxViolation = 0;
-
-    clear H g A lb ub lbA ubA;
-	
-    try
-        load 'benchmarkIDHESSIAN1.mat';
-    catch
-        successFlag = -1;
-        return;
-    end
-	
-    if ( exist( 'A','var' ) )
-        [nC,nV] = size(A);
-    else
-        nC = 0;
-    end
-    [nV,nP] = size(g);
-
-    xOpt = zeros(nV,nP);
-    yOpt = zeros(nV+nC,nP);
-    objOpt = zeros(1,nP);
-       
-	options = qpOASES_options( 'maxIter',nWSR, 'printLevel',-2 );
-    %options = qpOASES_options( 'fast','maxIter',nWSR, 'printLevel',2 );
-    
-
-	for i=1:nP
-		%disp(i);
-
-		if ( i == 1 )
-			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i)-1e-11,ubA(:,i)+1e-11,options );
-			%disp(status);
-		else
-			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i)-1e-11,ubA(:,i)+1e-11,options );
-			%disp(status);
-		end
-
-		[ maxViolationTMP ] = getKktResidual( H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i), x,lambda );
-		maxViolation = max( [maxViolation,maxViolationTMP] );
-
-        xOpt(:,i) = x;
-        yOpt(:,i) = lambda;
-        objOpt(:,i) = obj;
-	end
-	
-	qpOASES_sequence( 'c',QP );
-
-    if ( ( maxViolation < 8e-6 ) && ( status == 0 ) )
-        successFlag = 1;
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runInterfaceSeqTest.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runInterfaceSeqTest.m
deleted file mode 100644
index a7427c9f4c6..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runInterfaceSeqTest.m
+++ /dev/null
@@ -1,428 +0,0 @@
-function [ successFlag ] = runInterfaceSeqTest( nV,nC, doPrint,seed )
-
-    if ( nargin < 4 )
-        seed = 42;
-        if ( nargin < 3 )
-            doPrint = 1;
-            if ( nargin < 2 )
-                nC = 10;
-                if ( nargin < 1 )
-                    nV = 5;
-                end
-            end
-        end
-    end
-    
-    successFlag = 1;
-
-    for isSparseH=0:1
-        for isSparseA=0:1
-            successFlag = runSeveralInterfaceSeqTests( successFlag, nV,nC,isSparseH,isSparseA, doPrint,seed );
-        end
-    end
-    
-end
-
-
-
-function [ successFlag ] = runSeveralInterfaceSeqTests( successFlag, nV,nC, isSparseH,isSparseA, doPrint,seed )
-
-    %% test without or empty A matrix
-    for hasA=0:1
-        for changeMat=0:hasA % cannot change matrices if QProblemB object is instantiated
-            for hasLowerB=0:1
-                for hasUpperB=0:1
-                    for hasOptions=0:2
-                        for hasX0=0:1
-                            for hasWS=0:2
-                                curSuccessFLAG = runSingleInterfaceSeqTest( nV,0,hasA,isSparseH,isSparseA, hasLowerB,hasUpperB,0,0,hasOptions,hasX0,hasWS,changeMat, doPrint,seed );
-                                successFlag = min( successFlag,curSuccessFLAG );
-                            end
-                        end
-                    end
-                end
-            end
-        end
-    end
-    
-    %% test with non-empty A matrix
-    for hasLowerB=0:1
-        for hasUpperB=0:1
-            for hasLowerC=0:1
-                for hasUpperC=0:1
-                    for hasOptions=0:2
-                        for hasX0=0:1
-                            for hasWS=0:2
-                                for changeMat=0:1
-                                    curSuccessFLAG = runSingleInterfaceSeqTest( nV,nC,1,isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC,hasOptions,hasX0,hasWS,changeMat, doPrint,seed );
-                                    successFlag = min( successFlag,curSuccessFLAG );
-                                end
-                            end
-                        end
-                    end
-                end
-            end
-        end
-    end
-    
-end
-
-
-function [ successFlag ] = runSingleInterfaceSeqTest( nV,nC,hasA,isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC,hasOptions,hasX0,hasWS,changeMat, doPrint,seed )
-
-    successFlag = 0;
-
-    qpData1 = generateExample( nV,nC, isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC, seed );
-    if ( changeMat > 0 )
-        qpData2 = generateExample( nV,nC, isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC, seed+1 );
-    else
-        qpData2 = generateExample( nV,nC, isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC, seed+1,qpData1.H,qpData1.Ain );
-    end
-
-    if ( changeMat == 0 )
-        string = 'Testing qpOASES_sequence( ''i/h/c'',H';
-    else
-        string = 'Testing qpOASES_sequence( ''i/m/c'',H';
-    end
-    
-    if ( isSparseH > 0 )
-        string = [string,'s,g'];
-    else
-        string = [string,'d,g'];
-    end
-    
-    if ( nC > 0 )
-        if ( isSparseA > 0 )
-            string = [string,',As'];
-        else
-            string = [string,',Ad'];
-        end
-    else
-        if ( hasA > 0 )
-            string = [string,',[]'];
-        end
-    end
-    
-    if ( hasLowerB > 0 )
-        string = [string,',lb'];
-    else
-        string = [string,',[]'];
-    end
-    
-    if ( hasUpperB > 0 )
-        string = [string,',ub'];
-    else
-        string = [string,',[]'];
-    end
-    
-    if ( hasLowerC > 0 )
-        string = [string,',lbA'];
-    else
-        if ( hasA > 0 )
-            string = [string,',[] '];
-        end
-    end
-    
-    if ( hasUpperC > 0 )
-        string = [string,',ubA'];
-    else
-        if ( hasA > 0 )
-            string = [string,',[] '];
-        end
-    end
-    
-    switch ( hasOptions )
-        case 1
-            string = [string,',opt'];
-        
-        case 2
-            string = [string,',[] '];
-            
-        case 0
-            if ( ( hasX0 > 0 ) || ( hasWS > 0 ) )
-                string = [string,',[] '];
-            end
-    end
-
-    switch ( hasX0 )
-        case 1
-            string = [string,',{x0'];
-        
-        case 2
-            string = [string,',{[]'];
-            
-        case 0
-            if ( hasWS > 0 )
-                string = [string,',{[]'];
-            end
-    end
-    
-    switch ( hasWS )
-        case 1
-            string = [string,',WS}'];
-        
-        case 2
-            string = [string,',[]}'];
-    end
-
-    string = [string,' )... '];
-    if ( doPrint > 0 )
-        %disp( string );
-    end
-    
-    curSuccessFlag = callQpOasesSeq( qpData1,qpData2,hasA,hasOptions,hasX0,hasWS,changeMat );
-    if ( curSuccessFlag > 0 )
-        string = [string,'pass!'];
-        successFlag = 1;
-    else
-        string = [string,'fail!'];
-    end
-    
-    if ( doPrint > 0 )
-        disp( string );
-        if ( curSuccessFlag == 0 )
-            pause(0.1);
-        end
-    end
-    
-end
-
-function [ successFlag ] = callQpOasesSeq( qpData1,qpData2,hasA,hasOptions,hasX0,hasWS,changeMat, doPrint )
-
-    if ( nargin < 8 )
-        doPrint = 1;
-    end
-
-    %TOL = 1e-15;
-    KKTTOL = 1e-6;
-
-    successFlag = 0;
-    
-    H1 = qpData1.H;
-	g1 = qpData1.g;
-    A1 = [qpData1.Aeq;qpData1.Ain];
-	lb1 = qpData1.lb;
-    ub1 = qpData1.ub;
-    lbA1 = [qpData1.beq;qpData1.lbA];
-    ubA1 = [qpData1.beq;qpData1.ubA];
-    
-    if ( changeMat > 0 )
-        H2 = qpData2.H;
-    else
-        H2 = H1;
-    end
-	g2 = qpData2.g;
-    if ( changeMat > 0 )
-        A2 = [qpData2.Aeq;qpData2.Ain];
-    else
-        A2 = A1;
-    end
-	lb2 = qpData2.lb;
-    ub2 = qpData2.ub;
-    lbA2 = [qpData2.beq;qpData2.lbA];
-    ubA2 = [qpData2.beq;qpData2.ubA];
-
-    [nV,dummy] = size(H1);
-    [nC,dummy] = size(A1);
-    
-    if ( hasWS > 0 )
-        if ( hasWS == 1 )
-            WS = 0 * ones( nV+nC,1 );
-        else
-            WS = [];
-        end
-        
-        if ( hasX0 > 0 )
-            if ( hasX0 == 1 )
-                x0 = -1e-3 * ones( nV,1 );
-            else
-                x0 = [];
-            end
-            
-            auxInput = qpOASES_auxInput( 'x0',x0,'guessedWorkingSet',WS );
-
-            if ( hasOptions > 0 )
-                if ( hasOptions == 1 )
-                    options = qpOASES_options();
-                else
-                    options = [];
-                end
-
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
-                    qpOASES_sequence( 'c',QP );
-                end
-            else
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
-                    qpOASES_sequence( 'c',QP );
-                end
-            end
-
-        else % hasX0 == 0
-
-            auxInput = qpOASES_auxInput( 'guessedWorkingSet',WS );
-            
-            if ( hasOptions > 0 )
-                if ( hasOptions == 1 )
-                    options = qpOASES_options();
-                else
-                    options = [];
-                end
-
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
-                    qpOASES_sequence( 'c',QP );
-                end
-            else
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,[] );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,[] );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
-                    qpOASES_sequence( 'c',QP );
-                end
-            end
-
-        end % hasX0
-        
-    else % hasWS == 0
-        
-        if ( hasX0 > 0 )
-            if ( hasX0 == 1 )
-                x0 = -1e-3 * ones( nV,1 );
-            else
-                x0 = [];
-            end
-            
-            auxInput = qpOASES_auxInput( 'x0',x0 );
-
-            if ( hasOptions > 0 )
-                if ( hasOptions == 1 )
-                    options = qpOASES_options();
-                else
-                    options = [];
-                end
-
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
-                    qpOASES_sequence( 'c',QP );
-                end
-                    
-            else
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
-                    qpOASES_sequence( 'c',QP );
-                end
-            end
-
-        else % hasX0 == 0
-
-            if ( hasOptions > 0 )
-                if ( hasOptions == 1 )
-                    options = qpOASES_options();
-                else
-                    options = [];
-                end
-
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
-                    qpOASES_sequence( 'c',QP );
-                end
-            else
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1 );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1 );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
-                    qpOASES_sequence( 'c',QP );
-                end
-            end
-
-        end % hasX0
-
-    end % hasWS
-
-
-    kktTol1 = getKktResidual( H1,g1,A1,lb1,ub1,lbA1,ubA1, x1,l1 );
-    kktTol2 = getKktResidual( H2,g2,A2,lb2,ub2,lbA2,ubA2, x2,l2 );
-    
-    if ( ( kktTol1 <= KKTTOL ) && ( e1 >= 0 ) && ( kktTol2 <= KKTTOL ) && ( e2 >= 0 ) )
-        successFlag = 1;
-    else
-        if ( doPrint > 0 )
-            if ( ( kktTol1 > KKTTOL ) || ( kktTol2 > KKTTOL ) )
-                disp('kkt error')
-            else
-                disp('exitflag<0')
-            end
-        end
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runInterfaceTest.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runInterfaceTest.m
deleted file mode 100644
index 2f97eda6b2f..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runInterfaceTest.m
+++ /dev/null
@@ -1,459 +0,0 @@
-function [ successFlag ] = runInterfaceTest( nV,nC, doPrint,seed )
-
-    if ( nargin < 4 )
-        seed = 42;
-        if ( nargin < 3 )
-            doPrint = 1;
-            if ( nargin < 2 )
-                nC = 10;
-                if ( nargin < 1 )
-                    nV = 5;
-                end
-            end
-        end
-    end
-    
-    successFlag = 1;
-
-    for isSparseH=0:1
-        for isSparseA=0:1
-            successFlag = runSeveralInterfaceTests( successFlag, nV,nC,isSparseH,isSparseA, doPrint,seed );
-        end
-    end
-    
-end
-
-
-
-function [ successFLAG ] = runSeveralInterfaceTests( successFLAG, nV,nC, isSparseH,isSparseA, doPrint,seed )
-
-    %% test without or empty A matrix
-    for hasA=0:1
-        for hasLowerB=0:1
-            for hasUpperB=0:1
-                for hasOptions=0:2
-                    for hasX0=0:1
-                        for hasWS=0:2
-                            if ( ( hasWS ~= 2 ) || ( hasA ~= 0 ) || ( hasOptions == 1 ) || ( hasX0 ~= 0 ) )
-                                curSuccessFLAG = runSingleInterfaceTest( nV,0,hasA,isSparseH,isSparseA, hasLowerB,hasUpperB,0,0,hasOptions,hasX0,hasWS, doPrint,seed );
-                                successFLAG = min( successFLAG,curSuccessFLAG );
-                            end
-                        end
-                    end
-                end
-            end
-        end
-    end
-    
-    %% test with non-empty A matrix
-    for hasLowerB=0:1
-        for hasUpperB=0:1
-            for hasLowerC=0:1
-                for hasUpperC=0:1
-                    for hasOptions=0:2
-                        for hasX0=0:1
-                            for hasWS=0:2
-                                curSuccessFLAG = runSingleInterfaceTest( nV,nC,1,isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC,hasOptions,hasX0,hasWS, doPrint,seed );
-                                successFLAG = min( successFLAG,curSuccessFLAG );
-                            end
-                        end
-                    end
-                end
-            end
-        end
-    end
-    
-end
-
-
-function [ successFLAG ] = runSingleInterfaceTest( nV,nC,hasA,isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC,hasOptions,hasX0,hasWS, doPrint,seed )
-
-    successFLAG = 0;
-
-    qpData = generateExample( nV,nC, isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC, seed );
-
-    string = 'Testing qpOASES( H';
-    
-    if ( isSparseH > 0 )
-        string = [string,'s,g'];
-    else
-        string = [string,'d,g'];
-    end
-    
-    if ( nC > 0 )
-        if ( isSparseA > 0 )
-            string = [string,',As'];
-        else
-            string = [string,',Ad'];
-        end
-    else
-        if ( hasA > 0 )
-            string = [string,',[]'];
-        end
-    end
-    
-    if ( hasLowerB > 0 )
-        string = [string,',lb'];
-    else
-        string = [string,',[]'];
-    end
-    
-    if ( hasUpperB > 0 )
-        string = [string,',ub'];
-    else
-        string = [string,',[]'];
-    end
-    
-    if ( hasLowerC > 0 )
-        string = [string,',lbA'];
-    else
-        if ( hasA > 0 )
-            string = [string,',[] '];
-        end
-    end
-    
-    if ( hasUpperC > 0 )
-        string = [string,',ubA'];
-    else
-        if ( hasA > 0 )
-            string = [string,',[] '];
-        end
-    end
-    
-    switch ( hasOptions )
-        case 1
-            string = [string,',opt'];
-        
-        case 2
-            string = [string,',[] '];
-            
-        case 0
-            if ( ( hasX0 > 0 ) || ( hasWS > 0 ) )
-                string = [string,',[] '];
-            end
-    end
-
-    switch ( hasX0 )
-        case 1
-            string = [string,',{x0'];
-        
-        case 2
-            string = [string,',{[]'];
-            
-        case 0
-            if ( hasWS > 0 )
-                string = [string,',{[]'];
-            end
-    end
-    
-    switch ( hasWS )
-        case 1
-            string = [string,',WS}'];
-        
-        case 2
-            string = [string,',[]}'];
-    end
-
-    string = [string,' )... '];
-    if ( doPrint > 0 )
-        %disp( string );
-    end
-    
-    curSuccessFlag = callQpOases( qpData,hasA,hasOptions,hasX0,hasWS, 1 );
-    if ( curSuccessFlag > 0 )
-        string = [string,'pass!'];
-        successFLAG = 1;
-    else
-        string = [string,'fail!'];
-    end
-    
-    if ( doPrint > 0 )
-        disp( string );
-        if ( curSuccessFlag == 0 )
-            pause;
-        end
-    end
-    
-end
-
-function [ successFLAG ] = callQpOases( qpData,hasA,hasOptions,hasX0,hasWS, doPrint )
-
-    if ( nargin < 6 )
-        doPrint = 1;
-    end
-
-    TOL = 1e-15;
-    KKTTOL = 1e-6;
-
-    successFLAG = 0;
-    
-    H = qpData.H;
-	g = qpData.g;
-    A = [qpData.Aeq;qpData.Ain];
-	lb = qpData.lb;
-    ub = qpData.ub;
-    lbA = [qpData.beq;qpData.lbA];
-    ubA = [qpData.beq;qpData.ubA];
-
-    [nV,dummy] = size(H);
-    [nC,dummy] = size(A);
-    
-    if ( hasWS > 0 )
-        if ( hasWS == 1 )
-            WS = 0 * ones( nV+nC,1 );
-        else
-            WS = [];
-        end
-        
-        if ( hasX0 > 0 )
-            if ( hasX0 == 1 )
-                x0 = -1e-3 * ones( nV,1 );
-            else
-                x0 = [];
-            end
-            
-            auxInput = qpOASES_auxInput( 'x0',x0,'guessedWorkingSet',WS );
-
-            if ( hasOptions > 0 )
-                if ( hasOptions == 1 )
-                    options = qpOASES_options();
-                else
-                    options = [];
-                end
-
-                if ( hasA > 0 )
-                    [ x1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                    [ x2,f2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                    [ x3,f3,e3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                    [ x4,f4,e4,i4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                else                        
-                    [ x1 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                    [ x2,f2 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                    [ x3,f3,e3 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                    [ x4,f4,e4,i4 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                end
-            else
-                if ( hasA > 0 )
-                    [ x1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                    [ x2,f2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                    [ x3,f3,e3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                    [ x4,f4,e4,i4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                else
-                    [ x1 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                    [ x2,f2 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                    [ x3,f3,e3 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                    [ x4,f4,e4,i4 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                end
-            end
-
-        else % hasX0 == 0
-
-            auxInput = qpOASES_auxInput( 'guessedWorkingSet',WS );
-            
-            if ( hasOptions > 0 )
-                if ( hasOptions == 1 )
-                    options = qpOASES_options();
-                else
-                    options = [];
-                end
-                
-                if ( hasA > 0 )
-                    [ x1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                    [ x2,f2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                    [ x3,f3,e3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                    [ x4,f4,e4,i4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                else
-                    [ x1 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                    [ x2,f2 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                    [ x3,f3,e3 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                    [ x4,f4,e4,i4 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                end
-            else
-                if ( hasA > 0 )
-                    [ x1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                    [ x2,f2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                    [ x3,f3,e3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                    [ x4,f4,e4,i4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                else
-                    [ x1 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                    [ x2,f2 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                    [ x3,f3,e3 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                    [ x4,f4,e4,i4 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                end
-            end
-
-        end % hasX0
-        
-    else % hasWS == 0
-        
-        if ( hasX0 > 0 )
-            if ( hasX0 == 1 )
-                x0 = -1e-3 * ones( nV,1 );
-            else
-                x0 = [];
-            end
-            
-            auxInput = qpOASES_auxInput( 'x0',x0 );
-
-            if ( hasOptions > 0 )
-                if ( hasOptions == 1 )
-                    options = qpOASES_options();
-                else
-                    options = [];
-                end
-
-                if ( hasA > 0 )
-                    [ x1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                    [ x2,f2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                    [ x3,f3,e3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                    [ x4,f4,e4,i4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options,auxInput );
-                else
-                    [ x1 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                    [ x2,f2 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                    [ x3,f3,e3 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                    [ x4,f4,e4,i4 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,lb,ub,options,auxInput );
-                end
-                    
-            else
-                if ( hasA > 0 )
-                    [ x1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                    [ x2,f2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                    [ x3,f3,e3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                    [ x4,f4,e4,i4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,[],auxInput );
-                else
-                    [ x1 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                    [ x2,f2 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                    [ x3,f3,e3 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                    [ x4,f4,e4,i4 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,lb,ub,[],auxInput );
-                end
-            end
-
-        else % hasX0 == 0
-            
-            if ( hasOptions > 0 )
-                if ( hasOptions == 1 )
-                    options = qpOASES_options();
-                else
-                    options = [];
-                end
-
-                if ( hasA > 0 )
-                    [ x1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
-                    [ x2,f2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
-                    [ x3,f3,e3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
-                    [ x4,f4,e4,i4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
-                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
-                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
-                else
-                    [ x1 ] = qpOASES( H,g,lb,ub,options );
-                    [ x2,f2 ] = qpOASES( H,g,lb,ub,options );
-                    [ x3,f3,e3 ] = qpOASES( H,g,lb,ub,options );
-                    [ x4,f4,e4,i4 ] = qpOASES( H,g,lb,ub,options );
-                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,lb,ub,options );
-                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,lb,ub,options );
-                end
-            else
-                if ( hasA > 0 )
-                    [ x1 ] = qpOASES( H,g,A,lb,ub,lbA,ubA );
-                    [ x2,f2 ] = qpOASES( H,g,A,lb,ub,lbA,ubA );
-                    [ x3,f3,e3 ] = qpOASES( H,g,A,lb,ub,lbA,ubA );
-                    [ x4,f4,e4,i4 ] = qpOASES( H,g,A,lb,ub,lbA,ubA );
-                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,A,lb,ub,lbA,ubA );
-                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,A,lb,ub,lbA,ubA );
-                else
-                    [ x1 ] = qpOASES( H,g,lb,ub );
-                    [ x2,f2 ] = qpOASES( H,g,lb,ub );
-                    [ x3,f3,e3 ] = qpOASES( H,g,lb,ub );
-                    [ x4,f4,e4,i4 ] = qpOASES( H,g,lb,ub );
-                    [ x5,f5,e5,i5,l5 ] = qpOASES( H,g,lb,ub );
-                    [ x6,f6,e6,i6,l6,w6 ] = qpOASES( H,g,lb,ub );
-                end
-            end
-
-        end % hasX0
-
-    end % hasWS
-
-    % check whether all calls lead to same optimal solution
-    % independent from output arguments
-    if ( ( norm(x1-x2) > TOL ) || ...
-         ( norm(x1-x3) > TOL ) || ...
-         ( norm(x1-x4) > TOL ) || ...
-         ( norm(x1-x5) > TOL ) || ...
-         ( norm(x1-x6) > TOL ) )
-        if ( doPrint > 0 )
-            disp('diff in x')
-        end
-        return;
-    end
-    
-    if ( ( norm(f2-f3) > TOL ) || ...
-         ( norm(f2-f4) > TOL ) || ...
-         ( norm(f2-f5) > TOL ) || ...
-         ( norm(f2-f6) > TOL ) )
-        if ( doPrint > 0 )
-            disp('diff in fval')
-        end
-        return;
-    end
-
-    if ( ( norm(e3-e4) > TOL ) || ...
-         ( norm(e3-e5) > TOL ) || ...
-         ( norm(e3-e6) > TOL ) )
-        if ( doPrint > 0 )
-            disp('diff in exitflag')
-        end
-        return;
-    end
-
-    if ( ( norm(i4-i5) > TOL ) || ...
-         ( norm(i4-i6) > TOL ) )
-        if ( doPrint > 0 )
-            disp('diff in iter')
-        end
-        return;
-    end
-    
-    if ( norm(l5-l6) > TOL )
-        if ( doPrint > 0 )
-            disp('diff in lambda')
-        end
-        return;
-    end
-    
-    
-    kktTol = getKktResidual( H,g,A,lb,ub,lbA,ubA, x6,l6 );
-    
-    if ( ( kktTol <= KKTTOL ) && ( e6 >= 0 ) )
-        successFLAG = 1;
-    else
-        if ( doPrint > 0 )
-            disp('error')
-        end
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runQAP8.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runQAP8.m
deleted file mode 100644
index 0b3646f7b49..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runQAP8.m
+++ /dev/null
@@ -1,36 +0,0 @@
-function [ successFlag ] = runQAP8( doPrint )
-    
-    if ( nargin < 1 )
-        doPrint = 0;
-    end
-
-    successFlag = 0;
-
-    try
-        load 'QAP8.mat';
-    catch
-        successFlag = -1;
-        return;
-    end
-
-    options = qpOASES_options('default', 'maxIter',30000, 'printLevel',-2*doPrint );
-    tic
-    [x, fval, status, nWSRout, y] = qpOASES(sparse(QP.H), QP.f, ...
-        sparse(QP.C), QP.lb, QP.ub, QP.cl, QP.cu, options);
-    t = toc;
-    
-    if ( doPrint > 0 )
-        disp( ['solution time: ',num2str(t),' seconds'] );
-    end
-    
-    if ( ( status == 0 ) && ( nWSRout < 23200 ) )
-        successFlag = 1;
-    end
-    
-    return;    
-    % check error and print
-    [stat, feas, cmpl] = qpresidual(S.B, S.b1, S.C, S.cl1, S.cu1, x, -y);
-    fprintf('%d iters in %.3fs to tolerance %.2e\n', nWSRout, t, stat + feas + cmpl)
-    fprintf('Status: %d\n', status)
-
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runQSHARE1B.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runQSHARE1B.m
deleted file mode 100644
index b915683666d..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runQSHARE1B.m
+++ /dev/null
@@ -1,43 +0,0 @@
-function [ successFlag ] = runQSHARE1B( doPrint )
-    
-    if ( nargin < 1 )
-        doPrint = 0;
-    end
-
-    successFlag = 0;
-
-    try
-        load 'QSHARE1B.mat';
-    catch
-        successFlag = -1;
-        return;
-    end
-    
-    options = qpOASES_options('default', 'maxIter',550, 'maxCpuTime',2.0, 'printLevel',0 );
-    tic
-    [xD,fvalD,exitflagD,iterD,lambdaD] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
-    tD = toc;
-    kktD = getKktResidual( H,g,A,lb,ub,lbA,ubA, xD,lambdaD );
-    
-    if ( doPrint > 0 )
-        disp( ['dense kkt tol:        ',num2str(kktD, '%.3e')] );
-        disp( ['dense solution time:  ',num2str(tD),' seconds'] );
-		disp( ['dense #iterations:    ',num2str(iterD),] );
-    end
-    
-    tic
-    [xS,fvalS,exitflagS,iterS,lambdaS] = qpOASES( sparse(H),g,sparse(A),lb,ub,lbA,ubA,options );
-    tS = toc;
-    kktS = getKktResidual( H,g,A,lb,ub,lbA,ubA, xS,lambdaS );
-    
-    if ( doPrint > 0 )
-        disp( ['sparse kkt tol:       ',num2str(kktS, '%.3e')] );
-        disp( ['sparse solution time: ',num2str(tS),' seconds'] );
-		disp( ['sparse #iterations:   ',num2str(iterS),] );
-    end
-    
-    if ( ( exitflagD == 0 ) && ( kktD < 1e-9 ) && ( exitflagS == 0 ) && ( kktS < 1e-9 ) )
-        successFlag = 1;
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runRandomIdHessian.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runRandomIdHessian.m
deleted file mode 100644
index 48f9c2c0c31..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runRandomIdHessian.m
+++ /dev/null
@@ -1,428 +0,0 @@
-function [ successFlag ] = runRandomIdHessian( nV,nC, doPrint,seed )
-
-    if ( nargin < 4 )
-        seed = 42;
-        if ( nargin < 3 )
-            doPrint = 1;
-            if ( nargin < 2 )
-                nC = 10;
-                if ( nargin < 1 )
-                    nV = 5;
-                end
-            end
-        end
-    end
-    
-    successFlag = 1;
-
-    for isSparseH=1:1
-        for isSparseA=1:1
-            successFlag = runSeveralIdSeqTests( successFlag, nV,nC,isSparseH,isSparseA, doPrint,seed );
-        end
-    end
-    
-end
-
-
-function [ successFlag ] = runSeveralIdSeqTests( successFlag, nV,nC, isSparseH,isSparseA, doPrint,seed )
-
-    %% test without or empty A matrix
-    for hasA=0:1
-        for changeMat=0:hasA % cannot change matrices if QProblemB object is instantiated
-            for hasLowerB=0:1
-                for hasUpperB=0:1
-                    for hasOptions=0:1
-                        for hasX0=0:1
-                            for hasWS=0:2
-                                curSuccessFLAG = runSingleIdSeqTest( nV,0,hasA,isSparseH,isSparseA, hasLowerB,hasUpperB,0,0,hasOptions,hasX0,hasWS,changeMat, doPrint,seed );
-                                successFlag = min( successFlag,curSuccessFLAG );
-                            end
-                        end
-                    end
-                end
-            end
-        end
-    end
-    
-    %% test with non-empty A matrix
-    for hasLowerB=0:1
-        for hasUpperB=0:1
-            for hasLowerC=0:1
-                for hasUpperC=0:1
-                    for hasOptions=0:1
-                        for hasX0=0:1
-                            for hasWS=0:2
-                                for changeMat=0:1
-                                    curSuccessFLAG = runSingleIdSeqTest( nV,nC,1,isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC,hasOptions,hasX0,hasWS,changeMat, doPrint,seed );
-                                    successFlag = min( successFlag,curSuccessFLAG );
-                                end
-                            end
-                        end
-                    end
-                end
-            end
-        end
-    end
-    
-end
-
-
-function [ successFlag ] = runSingleIdSeqTest( nV,nC,hasA,isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC,hasOptions,hasX0,hasWS,changeMat, doPrint,seed )
-
-    successFlag = 0;
-
-    qpFeatures = setupQpFeaturesStruct( );
-    
-    qpFeatures.nV = nV;
-    qpFeatures.nC = nC;
-    
-    qpFeatures.isSparseH = isSparseH;
-    qpFeatures.isSparseA = isSparseA;
-    
-    qpFeatures.hasLowerB = hasLowerB;
-    qpFeatures.hasUpperB = hasUpperB;
-    qpFeatures.hasLowerC = hasLowerC;
-    qpFeatures.hasUpperC = hasUpperC;
-    
-    qpFeatures.hessianType = 2;
-    
-    qpData1 = generateRandomQp( qpFeatures,seed );
-    qpData2 = generateRandomQp( qpFeatures,seed );
-
-    if ( changeMat == 0 )
-        string = 'Testing qpOASES_sequence( ''i/h/c'',ID';
-    else
-        string = 'Testing qpOASES_sequence( ''i/m/c'',ID';
-    end
-    
-    if ( isSparseH > 0 )
-        string = [string,'s,g'];
-    else
-        string = [string,'d,g'];
-    end
-    
-    if ( nC > 0 )
-        if ( isSparseA > 0 )
-            string = [string,',As'];
-        else
-            string = [string,',Ad'];
-        end
-    else
-        if ( hasA > 0 )
-            string = [string,',[]'];
-        end
-    end
-    
-    if ( hasLowerB > 0 )
-        string = [string,',lb'];
-    else
-        string = [string,',[]'];
-    end
-    
-    if ( hasUpperB > 0 )
-        string = [string,',ub'];
-    else
-        string = [string,',[]'];
-    end
-    
-    if ( hasLowerC > 0 )
-        string = [string,',lbA'];
-    else
-        if ( hasA > 0 )
-            string = [string,',[] '];
-        end
-    end
-    
-    if ( hasUpperC > 0 )
-        string = [string,',ubA'];
-    else
-        if ( hasA > 0 )
-            string = [string,',[] '];
-        end
-    end
-    
-    switch ( hasOptions )
-        case 1
-            string = [string,',opt'];
-        
-        case 2
-            string = [string,',[] '];
-            
-        case 0
-            if ( ( hasX0 > 0 ) || ( hasWS > 0 ) )
-                string = [string,',[] '];
-            end
-    end
-
-    switch ( hasX0 )
-        case 1
-            string = [string,',{x0'];
-        
-        case 2
-            string = [string,',{[]'];
-            
-        case 0
-            if ( hasWS > 0 )
-                string = [string,',{[]'];
-            end
-    end
-    
-    switch ( hasWS )
-        case 1
-            string = [string,',WS}'];
-        
-        case 2
-            string = [string,',[]}'];
-    end
-
-    string = [string,' )... '];
-    if ( doPrint > 0 )
-        %disp( string );
-    end
-    
-    curSuccessFlag = callQpOasesSeq( qpData1,qpData2,hasA,hasOptions,hasX0,hasWS,changeMat,doPrint );
-    if ( curSuccessFlag > 0 )
-        string = [string,'pass!'];
-        successFlag = 1;
-    else
-        string = [string,'fail!'];
-    end
-    
-    if ( doPrint > 0 )
-        disp( string );
-        if ( curSuccessFlag == 0 )
-            pause;
-        end
-    end
-    
-end
-
-
-
-function [ successFlag ] = callQpOasesSeq( qpData1,qpData2,hasA,hasOptions,hasX0,hasWS,changeMat, doPrint )
-
-    if ( nargin < 8 )
-        doPrint = 1;
-    end
-
-    TOL = 1e-15;
-    KKTTOL = 1e-6;
-
-    successFlag = 0;
-    
-    H1 = qpData1.H;
-	g1 = qpData1.g;
-    A1 = [qpData1.Aeq;qpData1.Ain];
-	lb1 = qpData1.lb;
-    ub1 = qpData1.ub;
-    lbA1 = [qpData1.beq;qpData1.lbA];
-    ubA1 = [qpData1.beq;qpData1.ubA];
-    
-    H2 = qpData2.H;
-	g2 = qpData2.g;
-    A2 = [qpData2.Aeq;qpData2.Ain];
-	lb2 = qpData2.lb;
-    ub2 = qpData2.ub;
-    lbA2 = [qpData2.beq;qpData2.lbA];
-    ubA2 = [qpData2.beq;qpData2.ubA];
-
-    [nV,dummy] = size(H1);
-    [nC,dummy] = size(A1);
-    
-    if ( hasWS > 0 )
-        if ( hasWS == 1 )
-            WS = 0 * ones( nV+nC,1 );
-        else
-            WS = [];
-        end
-        
-        if ( hasX0 > 0 )
-            if ( hasX0 == 1 )
-                x0 = -1e-3 * ones( nV,1 );
-            else
-                x0 = [];
-            end
-            
-            auxInput = qpOASES_auxInput( 'x0',x0,'guessedWorkingSet',WS );
-
-            if ( hasOptions > 0 )
-                if ( hasOptions == 1 )
-                    options = qpOASES_options();
-                else
-                    options = [];
-                end
-
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else                        
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
-                    qpOASES_sequence( 'c',QP );
-                end
-            else
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
-                    qpOASES_sequence( 'c',QP );
-                end
-            end
-
-        else % hasX0 == 0
-            
-            auxInput = qpOASES_auxInput( 'guessedWorkingSet',WS );
-
-            if ( hasOptions > 0 )
-                if ( hasOptions == 1 )
-                    options = qpOASES_options();
-                else
-                    options = [];
-                end
-
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
-                    qpOASES_sequence( 'c',QP );
-                end
-            else
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,[] );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,[] );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
-                    qpOASES_sequence( 'c',QP );
-                end
-            end
-
-        end % hasX0
-        
-    else % hasWS == 0
-        
-        if ( hasX0 > 0 )
-            if ( hasX0 == 1 )
-                x0 = -1e-3 * ones( nV,1 );
-            else
-                x0 = [];
-            end
-            
-            auxInput = qpOASES_auxInput( 'x0',x0 );
-
-            if ( hasOptions > 0 )
-                if ( hasOptions == 1 )
-                    options = qpOASES_options();
-                else
-                    options = [];
-                end
-
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
-                    qpOASES_sequence( 'c',QP );
-                end
-                    
-            else
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
-                    qpOASES_sequence( 'c',QP );
-                end
-            end
-
-        else % hasX0 == 0
-
-            if ( hasOptions > 0 )
-                if ( hasOptions == 1 )
-                    options = qpOASES_options();
-                else
-                    options = [];
-                end
-
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
-                    qpOASES_sequence( 'c',QP );
-                end
-            else
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1 );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1 );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
-                    qpOASES_sequence( 'c',QP );
-                end
-            end
-
-        end % hasX0
-
-    end % hasWS
-
-
-    kktTol1 = getKktResidual( H1,g1,A1,lb1,ub1,lbA1,ubA1, x1,l1 );
-    kktTol2 = getKktResidual( H1,g1,A1,lb1,ub1,lbA1,ubA1, x2,l2 );
-    
-    if ( ( kktTol1 <= KKTTOL ) && ( e1 >= 0 ) && ( kktTol2 <= KKTTOL ) && ( e2 >= 0 ) )
-        successFlag = 1;
-    else
-        if ( doPrint > 1 )
-            disp('error')
-        end
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runRandomZeroHessian.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runRandomZeroHessian.m
deleted file mode 100644
index ae2a0444d60..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runRandomZeroHessian.m
+++ /dev/null
@@ -1,426 +0,0 @@
-function [ successFlag ] = runRandomZeroHessian( nV,nC, doPrint,seed )
-
-    if ( nargin < 4 )
-        seed = 42;
-        if ( nargin < 3 )
-            doPrint = 1;
-            if ( nargin < 2 )
-                nC = 10;
-                if ( nargin < 1 )
-                    nV = 5;
-                end
-            end
-        end
-    end
-    
-    successFlag = 1;
-
-    for isSparseH=0:1
-        for isSparseA=0:1
-            successFlag = runSeveralIdSeqTests( successFlag, nV,nC,isSparseH,isSparseA, doPrint,seed );
-        end
-    end
-    
-end
-
-
-function [ successFlag ] = runSeveralIdSeqTests( successFlag, nV,nC, isSparseH,isSparseA, doPrint,seed )
-
-    %% test without or empty A matrix
-    % {
-    for hasA=0:1
-        for changeMat=0:hasA % cannot change matrices if QProblemB object is instantiated
-            for hasOptions=1:1
-                for hasX0=[0 2]
-                    for hasWS=0:2
-                        curSuccessFLAG = runSingleIdSeqTest( nV,0,hasA,isSparseH,isSparseA, 1,1,0,0,hasOptions,hasX0,hasWS,changeMat, doPrint,seed );
-                        successFlag = min( successFlag,curSuccessFLAG );
-                    end
-                end
-            end
-        end
-    end
-    %}
-    
-    %% test with non-empty A matrix
-    for hasLowerC=0:1
-        for hasUpperC=0:1
-            for hasOptions=1:1
-                for hasX0=[0 2]
-                    for hasWS=0:2
-                        for changeMat=0:1
-                            curSuccessFLAG = runSingleIdSeqTest( nV,nC,1,isSparseH,isSparseA, 1,1,hasLowerC,hasUpperC,hasOptions,hasX0,hasWS,changeMat, doPrint,seed );
-                            successFlag = min( successFlag,curSuccessFLAG );
-                        end
-                    end
-                end
-            end
-        end
-    end
-    
-end
-
-
-function [ successFlag ] = runSingleIdSeqTest( nV,nC,hasA,isSparseH,isSparseA, hasLowerB,hasUpperB,hasLowerC,hasUpperC,hasOptions,hasX0,hasWS,changeMat, doPrint,seed )
-
-    successFlag = 0;
-
-    qpFeatures = setupQpFeaturesStruct( );
-    
-    qpFeatures.nV = nV;
-    qpFeatures.nC = nC;
-    
-    qpFeatures.isSparseH = isSparseH;
-    qpFeatures.isSparseA = isSparseA;
-    
-    qpFeatures.hasLowerB = hasLowerB;
-    qpFeatures.hasUpperB = hasUpperB;
-    qpFeatures.hasLowerC = hasLowerC;
-    qpFeatures.hasUpperC = hasUpperC;
-    
-    qpFeatures.hessianType = 3;
-    
-    qpData1 = generateRandomQp( qpFeatures,seed );
-    qpData2 = generateRandomQp( qpFeatures,seed );
-
-    if ( changeMat == 0 )
-        string = 'Testing qpOASES_sequence( ''i/h/c'',0';
-    else
-        string = 'Testing qpOASES_sequence( ''i/m/c'',0';
-    end
-    
-    if ( isSparseH > 0 )
-        string = [string,'s,g'];
-    else
-        string = [string,'d,g'];
-    end
-    
-    if ( nC > 0 )
-        if ( isSparseA > 0 )
-            string = [string,',As'];
-        else
-            string = [string,',Ad'];
-        end
-    else
-        if ( hasA > 0 )
-            string = [string,',[]'];
-        end
-    end
-    
-    if ( hasLowerB > 0 )
-        string = [string,',lb'];
-    else
-        string = [string,',[]'];
-    end
-    
-    if ( hasUpperB > 0 )
-        string = [string,',ub'];
-    else
-        string = [string,',[]'];
-    end
-    
-    if ( hasLowerC > 0 )
-        string = [string,',lbA'];
-    else
-        if ( hasA > 0 )
-            string = [string,',[] '];
-        end
-    end
-    
-    if ( hasUpperC > 0 )
-        string = [string,',ubA'];
-    else
-        if ( hasA > 0 )
-            string = [string,',[] '];
-        end
-    end
-    
-    switch ( hasOptions )
-        case 1
-            string = [string,',opt'];
-        
-        case 2
-            string = [string,',[] '];
-            
-        case 0
-            if ( ( hasX0 > 0 ) || ( hasWS > 0 ) )
-                string = [string,',[] '];
-            end
-    end
-
-    switch ( hasX0 )
-        case 1
-            string = [string,',{x0'];
-        
-        case 2
-            string = [string,',{[]'];
-            
-        case 0
-            if ( hasWS > 0 )
-                string = [string,',{[]'];
-            end
-    end
-    
-    switch ( hasWS )
-        case 1
-            string = [string,',WS}'];
-        
-        case 2
-            string = [string,',[]}'];
-    end
-
-    string = [string,' )... '];
-    if ( doPrint > 0 )
-        %disp( string );
-    end
-    
-	%try
-    curSuccessFlag = callQpOasesSeq( qpData1,qpData2,hasA,hasOptions,hasX0,hasWS,changeMat,doPrint );
-	%catch
-	%curSuccessFlag = 0;
-	%end
-    if ( curSuccessFlag > 0 )
-        string = [string,'pass!'];
-        successFlag = 1;
-    else
-        string = [string,'fail!'];
-    end
-    
-    if ( doPrint > 0 )
-        disp( string );
-        if ( curSuccessFlag == 0 )
-            %pause;
-        end
-    end
-    
-end
-
-
-
-function [ successFlag ] = callQpOasesSeq( qpData1,qpData2,hasA,hasOptions,hasX0,hasWS,changeMat, doPrint )
-
-    if ( nargin < 8 )
-        doPrint = 1;
-    end
-
-    TOL = 1e-15;
-    KKTTOL = 1e-6;
-
-    successFlag = 0;
-    
-    H1 = qpData1.H;
-	g1 = qpData1.g;
-    A1 = [qpData1.Aeq;qpData1.Ain];
-	lb1 = qpData1.lb;
-    ub1 = qpData1.ub;
-    lbA1 = [qpData1.beq;qpData1.lbA];
-    ubA1 = [qpData1.beq;qpData1.ubA];
-    
-    H2 = qpData2.H;
-	g2 = qpData2.g;
-    A2 = [qpData2.Aeq;qpData2.Ain];
-	lb2 = qpData2.lb;
-    ub2 = qpData2.ub;
-    lbA2 = [qpData2.beq;qpData2.lbA];
-    ubA2 = [qpData2.beq;qpData2.ubA];
-
-    [nV,dummy] = size(H1);
-    [nC,dummy] = size(A1);
-    
-    if ( hasWS > 0 )
-        if ( hasWS == 1 )
-            WS = 0 * ones( nV+nC,1 );
-        else
-            WS = [];
-        end
-        
-        if ( hasX0 > 0 )
-            if ( hasX0 == 1 )
-                x0 = -1e-3 * ones( nV,1 );
-            else
-                x0 = [];
-            end
-            
-            auxInput = qpOASES_auxInput( 'x0',x0,'guessedWorkingSet',WS );
-
-            if ( hasOptions > 0 )
-                if ( hasOptions == 1 )
-                    options = qpOASES_options( 'fast' );
-                else
-                    options = [];
-                end
-
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else                        
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
-                    qpOASES_sequence( 'c',QP );
-                end
-            else
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
-                    qpOASES_sequence( 'c',QP );
-                end
-            end
-
-        else % hasX0 == 0
-
-            auxInput = qpOASES_auxInput( 'guessedWorkingSet',WS );
-            
-            if ( hasOptions > 0 )
-                if ( hasOptions == 1 )
-                    options = qpOASES_options( 'fast' );
-                else
-                    options = [];
-                end
-
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
-                    qpOASES_sequence( 'c',QP );
-                end
-            else
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,[] );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,[] );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
-                    qpOASES_sequence( 'c',QP );
-                end
-            end
-
-        end % hasX0
-        
-    else % hasWS == 0
-        
-        if ( hasX0 > 0 )
-            if ( hasX0 == 1 )
-                x0 = -1e-3 * ones( nV,1 );
-            else
-                x0 = [];
-            end
-            
-            auxInput = qpOASES_auxInput( 'x0',x0 );
-
-            if ( hasOptions > 0 )
-                if ( hasOptions == 1 )
-                    options = qpOASES_options( 'fast' );
-                else
-                    options = [];
-                end
-
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options,auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options,auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
-                    qpOASES_sequence( 'c',QP );
-                end
-                    
-            else
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,[],auxInput );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,[],auxInput );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
-                    qpOASES_sequence( 'c',QP );
-                end
-            end
-
-        else % hasX0 == 0
-
-            if ( hasOptions > 0 )
-                if ( hasOptions == 1 )
-                    options = qpOASES_options( 'fast' );
-                else
-                    options = [];
-                end
-
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1,options );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2,options );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2,options );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1,options );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,options );
-                    qpOASES_sequence( 'c',QP );
-                end
-            else
-                if ( hasA > 0 )
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,A1,lb1,ub1,lbA1,ubA1 );
-                    if ( changeMat > 0 )
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'm',QP,H2,g2,A2,lb2,ub2,lbA2,ubA2 );
-                    else
-                        [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2,lbA2,ubA2 );
-                    end
-                    qpOASES_sequence( 'c',QP );
-                else
-                    [ QP,x1,f1,e1,i1,l1,w1 ] = qpOASES_sequence( 'i',H1,g1,lb1,ub1 );
-                    [ x2,f2,e2,i2,l2,w2 ] = qpOASES_sequence( 'h',QP,g2,lb2,ub2 );
-                    qpOASES_sequence( 'c',QP );
-                end
-            end
-
-        end % hasX0
-
-    end % hasWS
-
-
-    kktTol1 = getKktResidual( H1,g1,A1,lb1,ub1,lbA1,ubA1, x1,l1 );
-    kktTol2 = getKktResidual( H1,g1,A1,lb1,ub1,lbA1,ubA1, x2,l2 );
-    
-    if ( ( kktTol1 <= KKTTOL ) && ( e1 >= 0 ) && ( kktTol2 <= KKTTOL ) && ( e2 >= 0 ) )
-        successFlag = 1;
-    else
-        if ( doPrint > 1 )
-            disp('error')
-        end
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runSimpleSpringExample.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runSimpleSpringExample.m
deleted file mode 100644
index 2f494a6b136..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runSimpleSpringExample.m
+++ /dev/null
@@ -1,34 +0,0 @@
-function [ successFlag ] = runSimpleSpringExample( doPrint )
-    
-    if ( nargin < 1 )
-        doPrint = 0;
-    end
-
-    successFlag = 0;
-    
-    % parameter
-    k1 = 100;
-    k2 = 100;
-
-    m = 1;
-    g = 9.81;
-
-    % QP data
-    H = [ k1,0; 0,k2 ];
-    g = [ m*g; 0 ];
-
-    lb = [ 0; 0 ];
-    ub = [ 1; 1 ];
-
-    A = [ 1,1 ];
-    lbA = 1;
-    ubA = 1;
-
-    options = qpOASES_options( 'default', 'printLevel',2*doPrint );
-    [x,fval,exitflag,iter] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
-
-    if ( ( exitflag == 0 ) && ( iter < 5 ) )
-        successFlag = 1;
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestAPrioriKnownSeq1.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestAPrioriKnownSeq1.m
deleted file mode 100644
index 057196db750..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestAPrioriKnownSeq1.m
+++ /dev/null
@@ -1,87 +0,0 @@
-function [ successFlag ] = runTestAPrioriKnownSeq1( doPrint )
-
-    if ( nargin < 1 )
-        doPrint = 0;
-    end
-
-    successFlag = 1;
-    TOL = eps;
-
-    clear H g A lb ub lbA ubA;
-	
-    try
-        load 'benchmarkCRANE1.mat';
-    catch
-        successFlag = -1;
-        return;
-    end
-
-    [nC,nV] = size(A);
-    [nV,nP] = size(g);
-
-    xOptSeq = zeros(nV,nP);
-    objOptSeq = zeros(1,nP);
-    statusSeq = zeros(1,nP);
-    iterSeq = zeros(1,nP);
-    yOptSeq = zeros(nV+nC,nP);
-
-	options = qpOASES_options( 'fast','maxIter',100, 'printLevel',2*doPrint );
-
-    for i=1:nP
-		%disp(i);
-
-		if ( i == 1 )
-			[QP,x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'i',H,g(:,i),A,lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		else
-			[x,obj,status,nWSRout,lambda] = qpOASES_sequence( 'h',QP,g(:,i),lb(:,i),ub(:,i),lbA(:,i),ubA(:,i),options );
-		end
-
-        xOptSeq(:,i) = x;
-        objOptSeq(:,i) = obj;
-        statusSeq(:,i) = status;
-        iterSeq(:,i) = nWSRout;
-        yOptSeq(:,i) = lambda;
-        
-    end
-	
-	qpOASES_sequence( 'c',QP );
-    
-    
-    [xOptAPKSeq,objOptAPKSeq,statusAPKSeq,iterAPKSeq,yOptAPKSeq] = qpOASES( H,g,A,lb,ub,lbA,ubA,options );
-    
-    if ( norm( xOptSeq-xOptAPKSeq ) > TOL )
-        successFlag = 0;
-        if ( doPrint > 0 )
-            disp( 'xOpt error' )
-        end
-    end
-    
-    if ( norm( objOptSeq-objOptAPKSeq ) > TOL )
-        successFlag = 0;
-        if ( doPrint > 0 )
-            disp( 'objOpt error' )
-        end
-    end
-    
-    if ( sum( statusSeq~=statusAPKSeq ) > 0 )
-        successFlag = 0;
-        if ( doPrint > 0 )
-            disp( 'status error' )
-        end
-    end
-    
-    if ( sum( iterSeq~=iterAPKSeq ) > 0 )
-        successFlag = 0;
-        if ( doPrint > 0 )
-            disp( 'iter error' )
-        end
-    end
-    
-    if ( norm( yOptSeq-yOptAPKSeq ) > TOL )
-        successFlag = 0;
-        if ( doPrint > 0 )
-            disp( 'yOpt error' )
-        end
-    end
-	
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSeq.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSeq.m
deleted file mode 100644
index 00c4fe8af4a..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSeq.m
+++ /dev/null
@@ -1,104 +0,0 @@
-function [ successFlag ] = runTestSeq( doPrint )
-    
-    if ( nargin < 1 )
-        doPrint = 0;
-    end
-
-    successFlag = 1;
-    
-
-    % test case constants
-    m = 50;
-    n = 100;
-    nMajSeq = 8;
-    nMinSeq = 4;
-    nSeq = nMajSeq * nMinSeq;
-    p = 2; % interpolation monomial power
-    fldim = 5; % feedback law dimension
-
-    % generate start and end problem
-    Ls = sprand(n, n, 0.03);
-    Hs = Ls' * Ls + 1e-8 * eye(n);
-    As = sprand(m, n, 0.05);
-
-    % negative (!) definite Hessian
-    Le = sprand(n, n, 0.03);
-    He = -Le' * Le;
-    Ae = sprand(m, n, 0.05);
-
-    lbAs = -rand(m,1);
-    ubAs = rand(m,1); 
-    ubs = ones(n,1);
-    lbs = -ones(n,1);
-    gs = 10*rand(n,1);
-
-    lbAe = -rand(m,1);
-    ubAe = rand(m,1); 
-    ube = ones(n,1);
-    lbe = -ones(n,1);
-    ge = 10*rand(n,1);
-
-    % monomial interpolation
-    tmaj = 1 - (1:1/(1-nMajSeq):0).^p;
-    for i = 1:nMajSeq
-        tau = tmaj(i);
-        H{i} = (1-tau) * Hs + tau * He;
-        A{i} = (1-tau) * As + tau * Ae;
-    end
-    t = 1 - (1:1/(1-nSeq):0).^p;
-    for i = 1:nSeq
-        tau = t(i);
-        lbA{i} = (1-tau) * lbAs + tau * lbAe;
-        ubA{i} = (1-tau) * ubAs + tau * ubAe;
-        lb{i} = (1-tau) * lbs + tau * lbe;
-        ub{i} = (1-tau) * ubs + tau * ube;
-        g{i} = (1-tau) * gs + tau * ge;
-    end
-
-    x = zeros(n,1);
-
-    if ~exist('cumIters', 'var')
-        cumIters = zeros(nSeq, 1);
-    end
-
-    % solve sequence of QPs
-    if ( doPrint > 0 )
-        fprintf('%3s %5s\n', 'seq', 'iters')
-    end
-    
-    for i = 1:nSeq
-        if i == 1
-            [QP, x, fval, exitflag, iter, lambda] = qpOASES_sequence('i', ...
-                H{i}, g{i}, A{i}, lb{i}, ub{i}, lbA{i}, ubA{i}, x);
-        else
-            if mod(i-1, nMinSeq) == 0
-                j = (i-1) / nMinSeq + 1;
-                [x, fval, exitflag, iter, lambda] = qpOASES_sequence('m', QP, ...
-                    H{j}, g{i}, A{j}, lb{i}, ub{i}, lbA{i}, ubA{i});
-            else
-                [x, fval, exitflag, iter, lambda] = qpOASES_sequence('h', QP, ...
-                    g{i}, lb{i}, ub{i}, lbA{i}, ubA{i});
-            end
-        end
-        
-        if ( doPrint > 0 )
-            fprintf('%3d %5d\n', i, iter)
-        end
-        
-        cumIters(i) = cumIters(i) + iter;
-        
-        if ( exitflag ~= 0 )
-            successFlag = 0;
-        end
-    end
-
-    % solve EQP
-    V0 = zeros(n, fldim);
-    Lambda = eye(m, fldim);
-    [X, Y] = qpOASES_sequence('e', QP, V0, V0, V0, Lambda, Lambda);
-    FL = X(1:fldim,:);
-
-    % clear
-    qpOASES_sequence('c', QP)
-
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse.m
deleted file mode 100644
index e145a68d494..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse.m
+++ /dev/null
@@ -1,33 +0,0 @@
-function [ successFlag ] = runTestSparse( doPrint )
-    
-    if ( nargin < 1 )
-        doPrint = 0;
-    end
-
-    successFlag = 0;
-
-
-    m = 50;
-    n = 100;
-
-    L = sprand(n, n, 0.03);
-    H = L' * L;
-    A = sprand(m, n, 0.05);
-
-    lbA = -rand(m,1);
-    ubA = rand(m,1); 
-    ub = ones(n,1);
-    lb = -ones(n,1);
-    g = 10*rand(n,1);
-    
-    options = qpOASES_options( 'default', 'printLevel',2*doPrint );
-
-    [x1,~,exitflag1,iter1] = qpOASES(full(H), g, full(A), lb, ub, lbA, ubA, options);
-    [x2,~,exitflag2,iter2] = qpOASES(H, g, A, lb, ub, lbA, ubA, options);
-    
-    if ( ( exitflag1 == 0 ) && ( exitflag2 == 0 ) && ( iter1 == iter2 ) ...
-            && ( norm(x1-x2) < 1e-10 ) )
-        successFlag = 1;
-    end
-
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse2.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse2.m
deleted file mode 100644
index 72789059bff..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse2.m
+++ /dev/null
@@ -1,33 +0,0 @@
-function [ successFlag ] = runTestSparse2( doPrint )
-    
-    if ( nargin < 1 )
-        doPrint = 0;
-    end
-
-    successFlag = 0;
-    
-
-    m = 50;
-    n = 100;
-
-    L = sprand(n, n, 0.03);
-    H = L' * L;
-    A = sprand(m, n, 0.05);
-
-    lbA = -rand(m,1);
-    ubA = rand(m,1); 
-    ub = ones(n,1);
-    lb = -ones(n,1);
-    g = 10*rand(n,1);
-
-    options = qpOASES_options( 'default', 'printLevel',2*doPrint );
-    
-    [QP,~,~,exitflag1] = qpOASES_sequence( 'i', H, g, A, lb, ub, lbA, ubA, options );
-    [~,~,exitflag2] = qpOASES_sequence( 'h',QP, g*2, lb, ub, lbA, ubA );
-    qpOASES_sequence( 'c',QP );
-    
-    if ( ( exitflag1 == 0 ) && ( exitflag2 == 0 ) )
-        successFlag = 1;
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse3.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse3.m
deleted file mode 100644
index c62d3aba028..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse3.m
+++ /dev/null
@@ -1,39 +0,0 @@
-function [ successFlag ] = runTestSparse3( doPrint )
-    
-    if ( nargin < 1 )
-        doPrint = 0;
-    end
-
-    successFlag = 0;
-
-    
-    m = 50;
-    n = 100;
-
-    L = sprand(n, n, 0.03);
-    H = L' * L;
-    A = sprand(m, n, 0.05);
-
-    lbA = -rand(m,1);
-    ubA = rand(m,1); 
-    ub = ones(n,1);
-    lb = -ones(n,1);
-    g = 10*rand(n,1);
-
-    options = qpOASES_options( 'default', 'printLevel',2*doPrint );
-    
-    
-    [QP,~,~,exitflag1] = qpOASES_sequence( 'i', H, g, A, lb, ub, lbA, ubA, options );
-
-    L = sprand(n, n, 0.03);
-    H = L' * L;
-    A = sprand(m, n, 0.05);
-
-    [~,~,exitflag2] = qpOASES_sequence( 'm',QP, H, g*2, A, lb, ub, lbA, ubA );
-    qpOASES_sequence( 'c',QP );
-    
-    if ( ( exitflag1 == 0 ) && ( exitflag2 == 0 ) )
-        successFlag = 1;
-    end
-    
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse4.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse4.m
deleted file mode 100644
index 0841ef3c78b..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestSparse4.m
+++ /dev/null
@@ -1,29 +0,0 @@
-function [ successFlag ] = runTestSparse4( doPrint )
-    
-    if ( nargin < 1 )
-        doPrint = 0;
-    end
-
-    successFlag = 0;
-    
-    
-    n = 100;
-
-    L = sprand(n, n, 0.03);
-    H = L' * L;
-
-    ub = ones(n,1);
-    lb = -ones(n,1);
-    g = 10*rand(n,1);
-    
-    options = qpOASES_options( 'default', 'printLevel',2*doPrint );
-
-    [QP,~,~,exitflag1] = qpOASES_sequence( 'i', H, g, lb, ub, options );
-    [~,~,exitflag2] = qpOASES_sequence( 'h',QP, g*2, lb, ub );
-    qpOASES_sequence( 'c',QP );
-
-    if ( ( exitflag1 == 0 ) && ( exitflag2 == 0 ) )
-        successFlag = 1;
-    end
-
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestWorkingSetLI.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestWorkingSetLI.m
deleted file mode 100644
index ac0dfe5f5f1..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runTestWorkingSetLI.m
+++ /dev/null
@@ -1,50 +0,0 @@
-function [ successFlag ] = runTestWorkingSetLI( doPrint )
-    
-    if ( nargin < 1 )
-        doPrint = 0;
-    end
-
-    successFlag = 1;
-    
-    qpFeatures = setupQpFeaturesStruct( );
-    
-    qpFeatures.nV = 20;
-    qpFeatures.nC = 100;
-    
-    qpFeatures.isSparseH = 0;
-    qpFeatures.isSparseA = 0;
-    
-    qpFeatures.hasLowerB = 1;
-    qpFeatures.hasUpperB = 1;
-    qpFeatures.hasLowerC = 1;
-    qpFeatures.hasUpperC = 1;
-    
-    qpFeatures.makeInfeas = 1;
-    
-    options = qpOASES_options( 'default', 'printLevel',0, 'initialStatusBounds',0 );
-    
-    exitflag = 0;
-    counter = 0;
-    
-    while ( ( exitflag ~= -42 ) && ( counter < 100 ) )
-        
-        counter = counter+1;
-        
-        qpData = generateRandomQp( qpFeatures );
-        B = [ eye( qpFeatures.nV ); qpData.Ain ];
-    
-        [x,dummy1,exitflag,dummy2,dummy3,auxOutput] = qpOASES( qpData.H,qpData.g,qpData.Ain, ...
-                qpData.lb,qpData.ub,qpData.lbA,qpData.ubA, options ); %#ok<*NASGU>
-            
-        WS = auxOutput.workingSet;
-        nAct = sum( WS~=0 );
-        Bact = B( WS~=0,: );
-
-        if ( nAct ~= rank(Bact) )
-            successFlag = 0;
-            return;
-        end
-        
-    end
-   
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runVanBarelsUnboundedQP.m b/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runVanBarelsUnboundedQP.m
deleted file mode 100644
index e6293f30143..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/matlab/tests/runVanBarelsUnboundedQP.m
+++ /dev/null
@@ -1,25 +0,0 @@
-function [ successFlag ] = runVanBarelsUnboundedQP( doPrint )
-    
-    if ( nargin < 1 )
-        doPrint = 0;
-    end
-
-    successFlag = 0;
-    
-    try
-        data = load( 'vanBarelsUnboundedQP.mat' );
-    catch
-        successFlag = -1;
-        return;
-    end
-    
-    options1 = qpOASES_options( 'default', 'printLevel',-1*doPrint );
-    
-    [~,~,exitflag1,iter1] = qpOASES( data.H,data.g,data.lb,data.ub,options1 ); %#ok<*NASGU>
-
-    % should return "QP unbounded"
-    if ( exitflag1 == -3 )
-        successFlag = 1;
-    end
-   
-end
diff --git a/3rdparty/qpOASES/qpOASES-3.0/testing/runUnitTests b/3rdparty/qpOASES/qpOASES-3.0/testing/runUnitTests
deleted file mode 100644
index e50cc44b2a3..00000000000
--- a/3rdparty/qpOASES/qpOASES-3.0/testing/runUnitTests
+++ /dev/null
@@ -1,106 +0,0 @@
-#!/usr/bin/env bash
-
-##
-##	This file is part of qpOASES.
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
-##	Christian Kirches et al. All rights reserved.
-##
-##	qpOASES 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.
-##
-##	qpOASES 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 qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-##
-##	Filename:  testing/runUnitTests
-##	Author:    Hans Joachim Ferreau
-##	Version:   3.0
-##	Date:      2014
-
-# defining colors for output
-red='\e[0;31m'
-green='\e[0;32m'
-NC='\e[0m' # No Color
-
-# runs a number of examples to detect malfunctioning of the code
-
-function runTest {
-	echo -n "Running unit test $2 $3... "
-
-	$2 $3 > dummy.txt;
-	retVal=$?;
-	rm -rf dummy.txt;
-
-	if [ $retVal == 0 ]; then
-		echo -e "${green}passed!${NC}"
-	else
-		if [ $retVal == 99 ]; then
-			echo "problem data missing!"
-		else
-			echo -e "${red}failed!${NC}"
-			counter=$[counter+1];
-		fi
-	fi
-}
-
-
-cd ..;
-make testing;
-cd testing;
-
-counter=0;
-
-# run unit tests
-runTest $counter ../bin/test_matrices;
-runTest $counter ../bin/test_matrices2;
-runTest $counter ../bin/test_matrices3;
-runTest $counter ../bin/test_indexlist;
-
-runTest $counter ../bin/test_example1;
-runTest $counter ../bin/test_example1a;
-runTest $counter ../bin/test_example1b;
-runTest $counter ../bin/test_example2;
-runTest $counter ../bin/test_example4;
-runTest $counter ../bin/test_example5;
-runTest $counter ../bin/test_exampleLP;
-runTest $counter ../bin/test_qrecipe;
-
-runTest $counter ../bin/test_example6;
-runTest $counter ../bin/test_example7;
-runTest $counter ../bin/test_sebastien1;
-runTest $counter ../bin/test_vanBarelsUnboundedQP;
-runTest $counter ../bin/test_janick1;
-runTest $counter ../bin/test_janick2;
-runTest $counter ../bin/test_constraintProduct1;
-runTest $counter ../bin/test_constraintProduct2;
-runTest $counter ../bin/test_guessedWS1;
-runTest $counter ../bin/test_gradientShift;
-runTest $counter ../bin/test_runAllOqpExamples;
-
-runTest $counter ../bin/test_bench Odd;
-runTest $counter ../bin/test_bench Ods;
-#runTest $counter ../bin/test_bench Ord;
-#runTest $counter ../bin/test_bench Ors;
-runTest $counter ../bin/test_bench Omd;
-#runTest $counter ../bin/test_bench Oms;
-
-
-if [ $counter == 0 ]; then
-	echo -e "${green}All available tests passed successfully!${NC}"
-else
-	echo -e "${red}$counter test(s) failed!${NC}"
-	exit 1
-fi
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 891ca89e828..cea6963dac9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -196,7 +196,7 @@ add_subdirectory(idl)
 add_subdirectory(lib)
 add_subdirectory(ec)
 # option for 3rdparty
-option(USE_QPOASES "Build qpOASES" ON)
+option(USE_QPOASES "Build qpOASES" OFF)
 add_subdirectory(3rdparty)
 add_subdirectory(rtc)
 option(ENABLE_DOXYGEN "Use Doxygen" ON)
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index bfbd3621cc9..5a5807ba31c 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -584,6 +584,9 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     m_cogOut.write();
     m_sbpCogOffsetOut.write();
 
+
+//printf("m_zmp:%f, abs:%f\n",m_zmp.data.y,m_basePos.data.y+m_zmp.data.y);
+
     // reference acceleration
     hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
     if (sen != NULL) {
@@ -1025,7 +1028,7 @@ void AutoBalancer::solveLimbIK ()
 
       hsp->calibInitHumanCOMFromZMP();
 
-      std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;//ややCOMのIKに手間取った時プリント
+      std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
 
 	  }
   }else{
@@ -1098,6 +1101,11 @@ void AutoBalancer::solveLimbIK ()
     ref_cog = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;
 
 	  if(loop%100==0)if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
+
+
+//	  printf("root:%f, calcCM:%f, refCM:%f\n",m_robot->rootLink()->p(1),m_robot->calcCM()(1),hsp->rp_ref_out.com(1) + hsp->rp_wld_initpos.com(1));
+
+
   }else{
 	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
 	    if (it->second.is_active) solveLimbIKforLimb(it->second);
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 2dcf27c6e17..b565ba8b3f1 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -119,12 +119,13 @@ class HumanSynchronizer{
     double h2r_ratio;
     hrp::Vector3 init_wld_rp_basepos;
     HumanPose rp_ref_out;
+    FILE* log;
 
     HumanSynchronizer(){
-//      h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
+      h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
 //      h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
-      h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
-      //h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
+//      h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
+//      h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
       cur_rfup_level = 0;       cur_lfup_level = 0;
       is_rf_contact = true;     is_lf_contact = true;
       pre_cont_rfpos = hrp::Vector3::Zero();    pre_cont_lfpos = hrp::Vector3::Zero();
@@ -137,14 +138,18 @@ class HumanSynchronizer{
       FUPLEVELNUM = 200;
       FUPHIGHT = 0.05;
       CNT_F_TH = 20.0;
-//      MAXVEL = 0.004;
-      MAXVEL = 0.001;
+      MAXVEL = 0.004;
+//      MAXVEL = 0.001;
       HumanSyncOn = false;
       ht_first_call = true;
       startCountdownForHumanSync = false;
       countdown_num = 5*500;
+      log = fopen("/home/ishiguro/HumanSyncLog.txt","w+");
+    }
+    ~HumanSynchronizer(){
+      fclose(log);
+      cout<<"HumanSynchronizer destructed"<<endl;
     }
-    ~HumanSynchronizer(){cout<<"HumanSynchronizer destructed"<<endl;}
 
     void readInput(const HumanPose& raw_in){ hp_wld_raw = raw_in; }
     void setInitOffsetPose(){ hp_wld_initpos = hp_wld_raw; }
@@ -162,19 +167,22 @@ class HumanSynchronizer{
     void update(){
       gettimeofday(&t_calc_start, NULL);
       removeInitOffsetPose(hp_wld_raw, hp_wld_initpos, hp_rel_raw);
-      applyInitHumanCOMOffset(hp_wld_raw, init_hp_calibcom, hp_wld_raw);
+//      applyInitHumanCOMOffset(hp_rel_raw, init_hp_calibcom, hp_rel_raw);
       applyLPFilter(hp_rel_raw, hp_filtered, hp_filtered);
       lockFootXYOnContact(hp_filtered, is_rf_contact, is_lf_contact, hp_filtered);//根本から改変すべき
       calcWorldZMP((hp_filtered.rf+init_wld_hp_rfpos), (hp_filtered.lf+init_wld_hp_lfpos), hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
       convertRelHumanPoseToRelRobotPose(hp_filtered,rp_ref_out);
-      judgeFootContactStates(rp_ref_out.rfw, rp_ref_out.lfw, is_rf_contact, is_lf_contact);
+//      judgeFootContactStates(rp_ref_out.rfw, rp_ref_out.lfw, is_rf_contact, is_lf_contact);
+      judgeFootContactStates(hp_wld_raw.rfw, hp_wld_raw.lfw, is_rf_contact, is_lf_contact);//frwはすでに1000->100Hzにフィルタリングされている
       overwriteFootZFromContactStates(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);
 //      lockFootXYOnContact(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);//ここで改変するとLPFかかってないからジャンプする
       applyWorkspaceLimit(rp_ref_out, rp_ref_out);
       applyCOMToSupportRegionLimit(rp_ref_out, rp_ref_out);
       applyVelLimit(rp_ref_out, rp_ref_out_old, rp_ref_out);
-//      rp_ref_out.com(2) = 0;//Z方向固定
+      rp_ref_out.com(2) = 0;//Z方向固定
       rp_ref_out_old = rp_ref_out;
+      fprintf(log,"%f %f %f %f\n", rp_ref_out.com(0), rp_ref_out.com(1), rp_ref_out.zmp(0), rp_ref_out.zmp(1));
+
       gettimeofday(&t_calc_end, NULL);
     }
     void calibInitHumanCOMFromZMP(){
diff --git a/rtc/AutoBalancer/bak/AutoBalancer.cpp b/rtc/AutoBalancer/bak/AutoBalancer.cpp
new file mode 100644
index 00000000000..bfbd3621cc9
--- /dev/null
+++ b/rtc/AutoBalancer/bak/AutoBalancer.cpp
@@ -0,0 +1,2177 @@
+// -*- C++ -*-
+/*!
+ * @file  AutoBalancer.cpp
+ * @brief autobalancer component
+ * $Date$
+ *
+ * $Id$
+ */
+
+#include <rtm/CorbaNaming.h>
+#include <hrpModel/Link.h>
+#include <hrpModel/Sensor.h>
+#include <hrpModel/ModelLoaderUtil.h>
+#include "AutoBalancer.h"
+#include <hrpModel/JointPath.h>
+#include <hrpUtil/MatrixSolvers.h>
+#include "hrpsys/util/Hrpsys.h"
+
+
+typedef coil::Guard<coil::Mutex> Guard;
+using namespace rats;
+
+// Module specification
+// <rtc-template block="module_spec">
+static const char* autobalancer_spec[] =
+    {
+        "implementation_id", "AutoBalancer",
+        "type_name",         "AutoBalancer",
+        "description",       "autobalancer component",
+        "version",           HRPSYS_PACKAGE_VERSION,
+        "vendor",            "AIST",
+        "category",          "example",
+        "activity_type",     "DataFlowComponent",
+        "max_instance",      "10",
+        "language",          "C++",
+        "lang_type",         "compile",
+        // Configuration variables
+        "conf.default.debugLevel", "0",
+        ""
+    };
+// </rtc-template>
+
+AutoBalancer::AutoBalancer(RTC::Manager* manager)
+    : RTC::DataFlowComponentBase(manager),
+      // <rtc-template block="initializer">
+      m_qRefIn("qRef", m_qRef),
+      m_basePosIn("basePosIn", m_basePos),
+      m_baseRpyIn("baseRpyIn", m_baseRpy),
+      m_zmpIn("zmpIn", m_zmp),
+      m_optionalDataIn("optionalData", m_optionalData),
+      m_emergencySignalIn("emergencySignal", m_emergencySignal),
+      //ishiguro
+      m_htzmpIn("htzmpIn", m_htzmp),
+      m_htrfwIn("htrfwIn", m_htrfw),
+      m_htlfwIn("htlfwIn", m_htlfw),
+      m_htcomIn("htcomIn", m_htcom),
+      m_htrfIn("htrfIn", m_htrf),
+      m_htlfIn("htlfIn", m_htlf),
+      m_htrhIn("htrhIn", m_htrh),
+      m_htlhIn("htlhIn", m_htlh),
+      m_actzmpIn("actzmpIn", m_actzmp),
+
+      m_qOut("q", m_qRef),
+      m_zmpOut("zmpOut", m_zmp),
+      m_basePosOut("basePosOut", m_basePos),
+      m_baseRpyOut("baseRpyOut", m_baseRpy),
+      m_baseTformOut("baseTformOut", m_baseTform),
+      m_basePoseOut("basePoseOut", m_basePose),
+      m_accRefOut("accRef", m_accRef),
+      m_contactStatesOut("contactStates", m_contactStates),
+      m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime),
+      m_cogOut("cogOut", m_cog),
+      m_AutoBalancerServicePort("AutoBalancerService"),
+      m_walkingStatesOut("walkingStates", m_walkingStates),
+      m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
+      // </rtc-template>
+      gait_type(BIPED),
+      move_base_gain(0.8),
+      m_robot(hrp::BodyPtr()),
+      m_debugLevel(0)
+{
+    m_service0.autobalancer(this);
+}
+
+AutoBalancer::~AutoBalancer()
+{
+}
+
+
+RTC::ReturnCode_t AutoBalancer::onInitialize()
+{
+    std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
+    bindParameter("debugLevel", m_debugLevel, "0");
+
+    // Registration: InPort/OutPort/Service
+    // <rtc-template block="registration">
+    // Set InPort buffers
+    addInPort("qRef", m_qRefIn);
+    addInPort("basePosIn", m_basePosIn);
+    addInPort("baseRpyIn", m_baseRpyIn);
+    addInPort("zmpIn", m_zmpIn);
+    addInPort("optionalData", m_optionalDataIn);
+    addInPort("emergencySignal", m_emergencySignalIn);
+    //ishiguro
+    addInPort("htzmpIn", m_htzmpIn);
+    addInPort("htrfwIn", m_htrfwIn);
+    addInPort("htlfwIn", m_htlfwIn);
+    addInPort("htcomIn", m_htcomIn);
+    addInPort("htrfIn", m_htrfIn);
+    addInPort("htlfIn", m_htlfIn);
+    addInPort("htrhIn", m_htrhIn);
+    addInPort("htlhIn", m_htlhIn);
+    addInPort("actzmpIn", m_actzmpIn);
+
+    // Set OutPort buffer
+    addOutPort("q", m_qOut);
+    addOutPort("zmpOut", m_zmpOut);
+    addOutPort("basePosOut", m_basePosOut);
+    addOutPort("baseRpyOut", m_baseRpyOut);
+    addOutPort("baseTformOut", m_baseTformOut);
+    addOutPort("basePoseOut", m_basePoseOut);
+    addOutPort("accRef", m_accRefOut);
+    addOutPort("contactStates", m_contactStatesOut);
+    addOutPort("controlSwingSupportTime", m_controlSwingSupportTimeOut);
+    addOutPort("cogOut", m_cogOut);
+    addOutPort("walkingStates", m_walkingStatesOut);
+    addOutPort("sbpCogOffset", m_sbpCogOffsetOut);
+  
+    // Set service provider to Ports
+    m_AutoBalancerServicePort.registerProvider("service0", "AutoBalancerService", m_service0);
+  
+    // Set service consumers to Ports
+  
+    // Set CORBA Service Ports
+    addPort(m_AutoBalancerServicePort);
+  
+    // </rtc-template>
+    // <rtc-template block="bind_config">
+    // Bind variables and configuration variable
+  
+    // </rtc-template>
+
+    RTC::Properties& prop =  getProperties();
+    coil::stringTo(m_dt, prop["dt"].c_str());
+
+    m_robot = hrp::BodyPtr(new hrp::Body());
+    RTC::Manager& rtcManager = RTC::Manager::instance();
+    std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
+    int comPos = nameServer.find(",");
+    if (comPos < 0){
+        comPos = nameServer.length();
+    }
+    nameServer = nameServer.substr(0, comPos);
+    RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
+    if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
+                                 CosNaming::NamingContext::_duplicate(naming.getRootContext())
+                                 )){
+      std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
+      return RTC::RTC_ERROR;
+    }
+
+    // allocate memory for outPorts
+    m_qRef.data.length(m_robot->numJoints());
+    qorg.resize(m_robot->numJoints());
+    qrefv.resize(m_robot->numJoints());
+    m_baseTform.data.length(12);
+    //ishiguro
+    m_htrfw.data.length(6);
+    m_htlfw.data.length(6);
+
+    control_mode = MODE_IDLE;
+    loop = 0;
+
+    // setting from conf file
+    // GaitGenerator requires abc_leg_offset and abc_stride_parameter in robot conf file
+    // setting leg_pos from conf file
+    coil::vstring leg_offset_str = coil::split(prop["abc_leg_offset"], ",");
+    leg_names.push_back("rleg");
+    leg_names.push_back("lleg");
+
+    // setting from conf file
+    // rleg,TARGET_LINK,BASE_LINK
+    coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
+    size_t prop_num = 10;
+    if (end_effectors_str.size() > 0) {
+      size_t num = end_effectors_str.size()/prop_num;
+      for (size_t i = 0; i < num; i++) {
+        std::string ee_name, ee_target, ee_base;
+        coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
+        coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
+        coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
+        ABCIKparam tp;
+        for (size_t j = 0; j < 3; j++) {
+            coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
+        }
+        double tmpv[4];
+        for (int j = 0; j < 4; j++ ) {
+          coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
+        }
+        tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
+        tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
+        // Fix for toe joint
+        //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
+        //   Without toe joints, "end-effector link == force-sensor link" is assumed.
+        //   With toe joints, "end-effector link != force-sensor link" is assumed.
+        if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
+            std::vector<double> optw(tp.manip->numJoints(), 1.0);
+            optw.back() = 0.0; // Set weight = 0 for toe joint by default
+            tp.manip->setOptionalWeightVector(optw);
+            tp.has_toe_joint = true;
+        } else {
+            tp.has_toe_joint = false;
+        }
+        tp.avoid_gain = 0.001;
+        tp.reference_gain = 0.01;
+        tp.pos_ik_error_count = tp.rot_ik_error_count = 0;
+        ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
+        ikp[ee_name].target_link = m_robot->link(ee_target);
+        ee_vec.push_back(ee_name);
+        std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   target = " << ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
+        contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
+      }
+      m_contactStates.data.length(num);
+      if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
+        m_contactStates.data[contact_states_index_map["rleg"]] = true;
+        m_contactStates.data[contact_states_index_map["lleg"]] = true;
+      }
+      if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) {
+        m_contactStates.data[contact_states_index_map["rarm"]] = false;
+        m_contactStates.data[contact_states_index_map["larm"]] = false;
+      }
+      m_controlSwingSupportTime.data.length(num);
+      for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 0.0;
+    }
+    std::vector<hrp::Vector3> leg_pos;
+    if (leg_offset_str.size() > 0) {
+      hrp::Vector3 leg_offset;
+      for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
+      std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+      leg_pos.push_back(hrp::Vector3(-1*leg_offset));
+      leg_pos.push_back(hrp::Vector3(leg_offset));
+    }
+    if (leg_pos.size() < ikp.size()) {
+        size_t tmp_leg_pos_size = leg_pos.size();
+        for (size_t i = 0; i < ikp.size() - tmp_leg_pos_size; i++) {
+            leg_pos.push_back(hrp::Vector3::Zero());
+        }
+    }
+
+    std::vector<std::pair<hrp::Link*, hrp::Link*> > interlocking_joints;
+    readInterlockingJointsParamFromProperties(interlocking_joints, m_robot, prop["interlocking_joints"], std::string(m_profile.instance_name));
+    if (interlocking_joints.size() > 0) {
+        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            std::cerr << "[" << m_profile.instance_name << "] Interlocking Joints for [" << it->first << "]" << std::endl;
+            it->second.manip->setInterlockingJointPairIndices(interlocking_joints, std::string(m_profile.instance_name));
+        }
+    }
+
+    zmp_offset_interpolator = new interpolator(ikp.size()*3, m_dt);
+    zmp_offset_interpolator->setName(std::string(m_profile.instance_name)+" zmp_offset_interpolator");
+    zmp_transition_time = 1.0;
+    transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator");
+    transition_interpolator_ratio = 1.0;
+    adjust_footstep_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    adjust_footstep_interpolator->setName(std::string(m_profile.instance_name)+" adjust_footstep_interpolator");
+    transition_time = 2.0;
+    adjust_footstep_transition_time = 2.0;
+    leg_names_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator");
+    leg_names_interpolator_ratio = 1.0;
+
+    // setting stride limitations from conf file
+    double stride_fwd_x_limit = 0.15;
+    double stride_y_limit = 0.05;
+    double stride_th_limit = 10;
+    double stride_bwd_x_limit = 0.05;
+    std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_y_limit << "[m], " << stride_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl;
+    if (default_zmp_offsets.size() == 0) {
+      for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
+    }
+    if (leg_offset_str.size() > 0) {
+      gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_y_limit/*[m]*/, stride_th_limit/*[deg]*/, stride_bwd_x_limit/*[m]*/));
+      gg->set_default_zmp_offsets(default_zmp_offsets);
+    }
+    gg_is_walking = gg_solved = false;
+    m_walkingStates.data = false;
+    fix_leg_coords = coordinates();
+
+    // load virtual force sensors
+    readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
+    // ref force port
+    int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
+    int nvforce = m_vfs.size();
+    int nforce  = npforce + nvforce;
+    m_ref_force.resize(nforce);
+    m_ref_forceIn.resize(nforce);
+    m_force.resize(nforce);
+    m_ref_forceOut.resize(nforce);
+    m_limbCOPOffset.resize(nforce);
+    m_limbCOPOffsetOut.resize(nforce);
+    for (unsigned int i=0; i<npforce; i++){
+        sensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
+    }
+    for (unsigned int i=0; i<nvforce; i++){
+        for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
+            if (it->second.id == i) sensor_names.push_back(it->first);
+        }
+    }
+    // set ref force port
+    std::cerr << "[" << m_profile.instance_name << "] force sensor ports (" << nforce << ")" << std::endl;
+    for (unsigned int i=0; i<nforce; i++){
+        m_ref_forceIn[i] = new InPort<TimedDoubleSeq>(std::string("ref_"+sensor_names[i]).c_str(), m_ref_force[i]);
+        m_ref_force[i].data.length(6);
+        registerInPort(std::string("ref_"+sensor_names[i]).c_str(), *m_ref_forceIn[i]);
+        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string("ref_"+sensor_names[i]) << std::endl;
+        ref_forces.push_back(hrp::Vector3(0,0,0));
+    }
+    // set force port
+    for (unsigned int i=0; i<nforce; i++){
+        m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string(sensor_names[i]).c_str(), m_force[i]);
+        m_force[i].data.length(6);
+        m_force[i].data[0] = m_force[i].data[1] = m_force[i].data[2] = 0.0;
+        m_force[i].data[3] = m_force[i].data[4] = m_force[i].data[5] = 0.0;
+        registerOutPort(std::string(sensor_names[i]).c_str(), *m_ref_forceOut[i]);
+        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string(sensor_names[i]) << std::endl;
+    }
+    // set limb cop offset port
+    std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << nforce << ")" << std::endl;
+    for (unsigned int i=0; i<nforce; i++){
+        std::string nm("limbCOPOffset_"+sensor_names[i]);
+        m_limbCOPOffsetOut[i] = new OutPort<TimedPoint3D>(nm.c_str(), m_limbCOPOffset[i]);
+        registerOutPort(nm.c_str(), *m_limbCOPOffsetOut[i]);
+        m_limbCOPOffset[i].data.x = m_limbCOPOffset[i].data.y = m_limbCOPOffset[i].data.z = 0.0;
+        std::cerr << "[" << m_profile.instance_name << "]   name = " << nm << std::endl;
+    }
+    sbp_offset = hrp::Vector3(0,0,0);
+    sbp_cog_offset = hrp::Vector3(0,0,0);
+    //use_force = MODE_NO_FORCE;
+    use_force = MODE_REF_FORCE;
+
+    if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
+      is_legged_robot = true;
+    } else {
+      is_legged_robot = false;
+    }
+
+    m_accRef.data.ax = m_accRef.data.ay = m_accRef.data.az = 0.0;
+    prev_imu_sensor_vel = hrp::Vector3::Zero();
+
+    graspless_manip_mode = false;
+    graspless_manip_arm = "arms";
+    graspless_manip_p_gain = hrp::Vector3::Zero();
+
+    is_stop_mode = false;
+    has_ik_failed = false;
+    is_hand_fix_mode = false;
+
+    pos_ik_thre = 0.1*1e-3; // [m]
+    rot_ik_thre = (1e-2)*M_PI/180.0; // [rad]
+    ik_error_debug_print_freq = static_cast<int>(0.2/m_dt); // once per 0.2 [s]
+
+    //for HumanSynchronizer
+    hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer());
+
+    return RTC::RTC_OK;
+}
+
+
+RTC::ReturnCode_t AutoBalancer::onFinalize()
+{
+  delete zmp_offset_interpolator;
+  delete transition_interpolator;
+  delete adjust_footstep_interpolator;
+  delete leg_names_interpolator;
+  return RTC::RTC_OK;
+}
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onStartup(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onShutdown(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id)
+{
+    std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
+    
+    return RTC::RTC_OK;
+}
+
+RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
+{
+  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
+  Guard guard(m_mutex);
+  if (control_mode == MODE_ABC) {
+    control_mode = MODE_SYNC_TO_IDLE;
+    double tmp_ratio = 0.0;
+    transition_interpolator->go(&tmp_ratio, m_dt, true); // sync in one controller loop
+  }
+  return RTC::RTC_OK;
+}
+
+
+
+
+#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
+//#define DEBUGP2 ((loop%200==0))
+#define DEBUGP2 (false)
+RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
+{
+  // std::cerr << "AutoBalancer::onExecute(" << ec_id << ")" << std::endl;
+    loop ++;
+    if (m_qRefIn.isNew()) {
+        m_qRefIn.read();
+    }
+    if (m_basePosIn.isNew()) {
+      m_basePosIn.read();
+      input_basePos(0) = m_basePos.data.x;
+      input_basePos(1) = m_basePos.data.y;
+      input_basePos(2) = m_basePos.data.z;
+    }
+    if (m_baseRpyIn.isNew()) {
+      m_baseRpyIn.read();
+      input_baseRot = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
+    }
+    if (m_zmpIn.isNew()) {
+      m_zmpIn.read();
+      input_zmp(0) = m_zmp.data.x;
+      input_zmp(1) = m_zmp.data.y;
+      input_zmp(2) = m_zmp.data.z;
+    }
+    for (unsigned int i=0; i<m_ref_forceIn.size(); i++){
+        if ( m_ref_forceIn[i]->isNew() ) {
+            m_ref_forceIn[i]->read();
+        }
+    }
+    if (m_optionalDataIn.isNew()) {
+        m_optionalDataIn.read();
+        if (is_legged_robot) {
+          if (m_optionalData.data.length() >= contact_states_index_map.size()*2) {
+            // current optionalData is contactstates x limb and controlSwingSupportTime x limb
+            //   If contactStates in optionalData is 1.0, m_contactStates is true. Otherwise, false.
+            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+                m_contactStates.data[contact_states_index_map[it->first]] = isOptionalDataContact(it->first);
+                m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = m_optionalData.data[contact_states_index_map[it->first]+contact_states_index_map.size()];
+            }
+            if ( !m_contactStates.data[contact_states_index_map["rleg"]] && !m_contactStates.data[contact_states_index_map["lleg"]] ) { // If two feet have no contact, force set double support contact
+              m_contactStates.data[contact_states_index_map["rleg"]] = true;
+              m_contactStates.data[contact_states_index_map["lleg"]] = true;
+            }
+          }
+        }
+    }
+    if (m_emergencySignalIn.isNew()){
+        m_emergencySignalIn.read();
+        // if (!is_stop_mode) {
+        //     std::cerr << "[" << m_profile.instance_name << "] emergencySignal is set!" << std::endl;
+        //     is_stop_mode = true;
+        //     gg->emergency_stop();
+        // }
+    }
+    //for HumanSynchronizer
+    if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
+    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); hp_raw_data.rfw = HumanSynchronizer::DoubleSeqToWrench6(m_htrfw); }
+    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); hp_raw_data.lfw = HumanSynchronizer::DoubleSeqToWrench6(m_htlfw); }
+    if (m_htcomIn.isNew()){ m_htcomIn.read(); hp_raw_data.com = HumanSynchronizer::Point3DToVector3(m_htcom); }
+    if (m_htrfIn.isNew()) { m_htrfIn.read();  hp_raw_data.rf  = HumanSynchronizer::Point3DToVector3(m_htrf); }
+    if (m_htlfIn.isNew()) { m_htlfIn.read();  hp_raw_data.lf  = HumanSynchronizer::Point3DToVector3(m_htlf); }
+    if (m_htrhIn.isNew()) { m_htrhIn.read();  hp_raw_data.rh  = HumanSynchronizer::Point3DToVector3(m_htrh); }
+    if (m_htlhIn.isNew()) { m_htlhIn.read();  hp_raw_data.lh  = HumanSynchronizer::Point3DToVector3(m_htlh); }
+    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
+    hsp->readInput(hp_raw_data);
+
+    Guard guard(m_mutex);
+    hrp::Vector3 ref_basePos;
+    hrp::Matrix33 ref_baseRot;
+    hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
+    if ( is_legged_robot ) {
+      gg->proc_zmp_weight_map_interpolation();
+      getCurrentParameters();
+      getTargetParameters();
+      bool is_transition_interpolator_empty = transition_interpolator->isEmpty();
+      if (!is_transition_interpolator_empty) {
+        transition_interpolator->get(&transition_interpolator_ratio, true);
+      } else {
+        transition_interpolator_ratio = 1.0;
+      }
+      if (control_mode != MODE_IDLE ) {
+        solveLimbIK();
+        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
+      } else {
+        rel_ref_zmp = input_zmp;
+      }
+      // transition
+      if (!is_transition_interpolator_empty) {
+        // transition_interpolator_ratio 0=>1 : IDLE => ABC
+        // transition_interpolator_ratio 1=>0 : ABC => IDLE
+        ref_basePos = (1-transition_interpolator_ratio) * input_basePos + transition_interpolator_ratio * m_robot->rootLink()->p;
+        rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp;
+        rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R);
+        for ( int i = 0; i < m_robot->numJoints(); i++ ) {
+          m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q;
+        }
+      } else {
+        ref_basePos = m_robot->rootLink()->p;
+        ref_baseRot = m_robot->rootLink()->R;
+      }
+      // mode change for sync
+      if (control_mode == MODE_SYNC_TO_ABC) {
+        control_mode = MODE_ABC;
+      } else if (control_mode == MODE_SYNC_TO_IDLE && transition_interpolator->isEmpty() ) {
+        std::cerr << "[" << m_profile.instance_name << "] Finished cleanup" << std::endl;
+        control_mode = MODE_IDLE;
+      }
+    }
+    if ( m_qRef.data.length() != 0 ) { // initialized
+      if (is_legged_robot) {
+        for ( int i = 0; i < m_robot->numJoints(); i++ ){
+          m_qRef.data[i] = m_robot->joint(i)->q;
+        }
+      }
+      m_qOut.write();
+    }
+    if (is_legged_robot) {
+      // basePos
+      m_basePos.data.x = ref_basePos(0);
+      m_basePos.data.y = ref_basePos(1);
+      m_basePos.data.z = ref_basePos(2);
+      m_basePos.tm = m_qRef.tm;
+      // baseRpy
+      hrp::Vector3 baseRpy = hrp::rpyFromRot(ref_baseRot);
+      m_baseRpy.data.r = baseRpy(0);
+      m_baseRpy.data.p = baseRpy(1);
+      m_baseRpy.data.y = baseRpy(2);
+      m_baseRpy.tm = m_qRef.tm;
+      // baseTform
+      double *tform_arr = m_baseTform.data.get_buffer();
+      tform_arr[0] = m_basePos.data.x;
+      tform_arr[1] = m_basePos.data.y;
+      tform_arr[2] = m_basePos.data.z;
+      hrp::setMatrix33ToRowMajorArray(ref_baseRot, tform_arr, 3);
+      m_baseTform.tm = m_qRef.tm;
+      // basePose
+      m_basePose.data.position.x = m_basePos.data.x;
+      m_basePose.data.position.y = m_basePos.data.y;
+      m_basePose.data.position.z = m_basePos.data.z;
+      m_basePose.data.orientation.r = m_baseRpy.data.r;
+      m_basePose.data.orientation.p = m_baseRpy.data.p;
+      m_basePose.data.orientation.y = m_baseRpy.data.y;
+      m_basePose.tm = m_qRef.tm;
+      // zmp
+      m_zmp.data.x = rel_ref_zmp(0);
+      m_zmp.data.y = rel_ref_zmp(1);
+      m_zmp.data.z = rel_ref_zmp(2);
+      m_zmp.tm = m_qRef.tm;
+      // cog
+      m_cog.data.x = ref_cog(0);
+      m_cog.data.y = ref_cog(1);
+      m_cog.data.z = ref_cog(2);
+      m_cog.tm = m_qRef.tm;
+      // sbpCogOffset
+      m_sbpCogOffset.data.x = sbp_cog_offset(0);
+      m_sbpCogOffset.data.y = sbp_cog_offset(1);
+      m_sbpCogOffset.data.z = sbp_cog_offset(2);
+      m_sbpCogOffset.tm = m_qRef.tm;
+    }
+    m_basePosOut.write();
+    m_baseRpyOut.write();
+    m_baseTformOut.write();
+    m_basePoseOut.write();
+    m_zmpOut.write();
+    m_cogOut.write();
+    m_sbpCogOffsetOut.write();
+
+    // reference acceleration
+    hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
+    if (sen != NULL) {
+      hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos;
+      hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt;
+      // convert to imu sensor local acceleration
+      hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt;
+      m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2);
+      m_accRefOut.write();
+      prev_imu_sensor_pos = imu_sensor_pos;
+      prev_imu_sensor_vel = imu_sensor_vel;
+    }
+
+    // control parameters
+    m_contactStates.tm = m_qRef.tm;
+    m_contactStatesOut.write();
+    m_controlSwingSupportTime.tm = m_qRef.tm;
+    m_controlSwingSupportTimeOut.write();
+    m_walkingStates.data = gg_is_walking;
+    m_walkingStates.tm = m_qRef.tm;
+    m_walkingStatesOut.write();
+
+    for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
+        m_force[i].tm = m_qRef.tm;
+        m_ref_forceOut[i]->write();
+    }
+
+    for (unsigned int i=0; i<m_limbCOPOffsetOut.size(); i++){
+        m_limbCOPOffset[i].tm = m_qRef.tm;
+        m_limbCOPOffsetOut[i]->write();
+    }
+
+    return RTC::RTC_OK;
+}
+
+void AutoBalancer::getCurrentParameters()
+{
+  current_root_p = m_robot->rootLink()->p;
+  current_root_R = m_robot->rootLink()->R;
+  for ( int i = 0; i < m_robot->numJoints(); i++ ){
+    qorg[i] = m_robot->joint(i)->q;
+  }
+}
+
+void AutoBalancer::getTargetParameters()
+{
+  // joint angles
+  for ( int i = 0; i < m_robot->numJoints(); i++ ){
+    m_robot->joint(i)->q = m_qRef.data[i];
+    qrefv[i] = m_robot->joint(i)->q;
+  }
+  // basepos, rot, zmp
+  m_robot->rootLink()->p = input_basePos;
+  m_robot->rootLink()->R = input_baseRot;
+  m_robot->calcForwardKinematics();
+  //
+  if (control_mode != MODE_IDLE) {
+    coordinates tmp_fix_coords;
+    if (!zmp_offset_interpolator->isEmpty()) {
+      double *default_zmp_offsets_output = new double[ikp.size()*3];
+      zmp_offset_interpolator->get(default_zmp_offsets_output, true);
+      for (size_t i = 0; i < ikp.size(); i++)
+        for (size_t j = 0; j < 3; j++)
+          default_zmp_offsets[i](j) = default_zmp_offsets_output[i*3+j];
+      delete[] default_zmp_offsets_output;
+      if (DEBUGP) {
+        std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl;
+        std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+        for (size_t i = 0; i < leg_names.size(); i++) {
+            std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
+            std::cerr << "[" << m_profile.instance_name << "]   " << leg_names[i] << " = " << default_zmp_offsets[dst->first].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+        }
+      }
+    }
+    if (!leg_names_interpolator->isEmpty()) {
+        leg_names_interpolator->get(&leg_names_interpolator_ratio, true);
+    }else {
+        leg_names_interpolator_ratio = 1.0;
+    }
+    if ( gg_is_walking ) {
+      gg->set_default_zmp_offsets(default_zmp_offsets);
+      gg_solved = gg->proc_one_tick();
+      {
+          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+          coordinates tmpc;
+          // for support leg
+          for (std::vector<step_node>::const_iterator it = gg->get_support_leg_steps().begin(); it != gg->get_support_leg_steps().end(); it++) {
+              coordinates sp_coords = it->worldcoords;
+              coordinates(ikp[leg_type_map[it->l_r]].localPos,
+                          ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc);
+              sp_coords.transform(tmpc);
+              ikp[leg_type_map[it->l_r]].target_p0 = sp_coords.pos;
+              ikp[leg_type_map[it->l_r]].target_r0 = sp_coords.rot;
+          }
+          // for swing leg
+          for (std::vector<step_node>::const_iterator it = gg->get_swing_leg_steps().begin(); it != gg->get_swing_leg_steps().end(); it++) {
+              coordinates sw_coords = it->worldcoords;
+              coordinates(ikp[leg_type_map[it->l_r]].localPos,
+                          ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc);
+              sw_coords.transform(tmpc);
+              ikp[leg_type_map[it->l_r]].target_p0 = sw_coords.pos;
+              ikp[leg_type_map[it->l_r]].target_r0 = sw_coords.rot;
+          }
+      }
+      gg->get_swing_support_mid_coords(tmp_fix_coords);
+      // set contactStates
+      {
+          std::vector<std::string> tmp_current_support_states_names;
+          {
+              std::vector<leg_type> tmp_current_support_states = gg->get_current_support_states();
+              std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+              for (std::vector<leg_type>::const_iterator it = tmp_current_support_states.begin(); it != tmp_current_support_states.end(); it++)
+                  tmp_current_support_states_names.push_back(leg_type_map[*it]);
+          }
+          // Set Contact States for ee not included in leg_names to false
+          for ( std::map<std::string, size_t>::iterator it = contact_states_index_map.begin(); it != contact_states_index_map.end(); it++ ) {
+              m_contactStates.data[it->second] = false;
+          }
+          for (std::vector<std::string>::const_iterator it = leg_names.begin(); it != leg_names.end(); it++) {
+              std::vector<std::string>::const_iterator dst = std::find_if(tmp_current_support_states_names.begin(), tmp_current_support_states_names.end(), boost::lambda::_1 == *it);
+              if (dst != tmp_current_support_states_names.end()) {
+                  m_contactStates.data[contact_states_index_map[*it]] = true;
+              } else {
+                  m_contactStates.data[contact_states_index_map[*it]] = false;
+              }
+          }
+      }
+      // set controlSwingSupportTime
+      {
+          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+          for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
+              std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
+              m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = gg->get_current_swing_time(dst->first);
+          }
+      }
+      // set ref_forces
+      {
+          std::vector<hrp::Vector3> ee_pos;
+          for (size_t i = 0 ; i < leg_names.size(); i++) {
+              ABCIKparam& tmpikp = ikp[leg_names[i]];
+              ee_pos.push_back(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]);
+          }
+          double alpha = (ref_zmp - ee_pos[1]).norm() / (ee_pos[0] - ee_pos[1]).norm();
+          if (alpha>1.0) alpha = 1.0;
+          if (alpha<0.0) alpha = 0.0;
+          if (DEBUGP) {
+          std::cerr << "[" << m_profile.instance_name << "] alpha:" << alpha << std::endl;
+          }
+          double mg = m_robot->totalMass() * gg->get_gravitational_acceleration();
+          m_force[0].data[0] = alpha * mg;
+          m_force[1].data[0] = (1-alpha) * mg;
+      }
+      // set limbCOPOffset
+      {
+          std::vector<std::string> swg_leg_nms = gg->get_swing_leg_names();
+          for (size_t i = 0; i < swg_leg_nms.size(); i++) {
+              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.x = gg->get_swing_foot_zmp_offsets().at(i)(0);
+              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.y = gg->get_swing_foot_zmp_offsets().at(i)(1);
+              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.z = gg->get_swing_foot_zmp_offsets().at(i)(2);
+          }
+      }
+      {
+          std::vector<std::string> sup_leg_nms = gg->get_support_leg_names();
+          for (size_t i = 0; i < sup_leg_nms.size(); i++) {
+              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.x = gg->get_support_foot_zmp_offsets().at(i)(0);
+              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.y = gg->get_support_foot_zmp_offsets().at(i)(1);
+              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.z = gg->get_support_foot_zmp_offsets().at(i)(2);
+          }
+      }
+    } else {
+      tmp_fix_coords = fix_leg_coords;
+      // double support by default
+      {
+          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+          for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
+              std::vector<std::string>::const_iterator dst = std::find_if(leg_names.begin(), leg_names.end(), (boost::lambda::_1 == it->first));
+              if (dst != leg_names.end()) {
+                  m_contactStates.data[contact_states_index_map[it->first]] = true;
+              } else {
+                  m_contactStates.data[contact_states_index_map[it->first]] = false;
+              }
+              // controlSwingSupportTime is not used while double support period, 1.0 is neglected
+              m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = 1.0;
+              std::map<leg_type, std::string>::const_iterator dst2 = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
+              m_limbCOPOffset[contact_states_index_map[it->first]].data.x = default_zmp_offsets.at(dst2->first)(0);
+              m_limbCOPOffset[contact_states_index_map[it->first]].data.y = default_zmp_offsets.at(dst2->first)(1);
+              m_limbCOPOffset[contact_states_index_map[it->first]].data.z = default_zmp_offsets.at(dst2->first)(2);
+          }
+      }
+    }
+    if (!adjust_footstep_interpolator->isEmpty()) {
+        double tmp = 0.0;
+        adjust_footstep_interpolator->get(&tmp, true);
+        //std::cerr << "[" << m_profile.instance_name << "] adjust ratio " << tmp << std::endl;
+        ikp["rleg"].target_p0 = (1-tmp) * ikp["rleg"].adjust_interpolation_org_p0 + tmp*ikp["rleg"].adjust_interpolation_target_p0;
+        ikp["lleg"].target_p0 = (1-tmp) * ikp["lleg"].adjust_interpolation_org_p0 + tmp*ikp["lleg"].adjust_interpolation_target_p0;
+        rats::mid_rot(ikp["rleg"].target_r0, tmp, ikp["rleg"].adjust_interpolation_org_r0, ikp["rleg"].adjust_interpolation_target_r0);
+        rats::mid_rot(ikp["lleg"].target_r0, tmp, ikp["lleg"].adjust_interpolation_org_r0, ikp["lleg"].adjust_interpolation_target_r0);
+        coordinates tmprc, tmplc;
+        tmprc.pos = ikp["rleg"].target_p0 + ikp["rleg"].target_r0 * ikp["rleg"].localPos;
+        tmprc.rot = ikp["rleg"].target_r0 * ikp["rleg"].localR;
+        tmplc.pos = ikp["lleg"].target_p0 + ikp["lleg"].target_r0 * ikp["lleg"].localPos;
+        tmplc.rot = ikp["lleg"].target_r0 * ikp["lleg"].localR;
+        rats::mid_coords(fix_leg_coords, 0.5, tmprc, tmplc);
+        tmp_fix_coords = fix_leg_coords;
+    }
+    // Tempolarily modify tmp_fix_coords
+    // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272
+    {
+      hrp::Vector3 ex = hrp::Vector3::UnitX();
+      hrp::Vector3 ez = hrp::Vector3::UnitZ();
+      hrp::Vector3 xv1 (tmp_fix_coords.rot * ex);
+      xv1(2) = 0.0;
+      xv1.normalize();
+      hrp::Vector3 yv1(ez.cross(xv1));
+      tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2);
+      tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2);
+      tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2);
+    }
+    // Fix pos
+    fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot);
+
+    /* update ref_forces ;; sp's absolute -> rmc's absolute */
+    for (size_t i = 0; i < m_ref_forceIn.size(); i++) {
+      hrp::Matrix33 eeR;
+      hrp::Link* parentlink;
+      hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
+      if (sensor) parentlink = sensor->link;
+      else parentlink = m_vfs[sensor_names[i]].link;
+      for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+          if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR;
+      }
+      // End effector frame
+      //ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
+      // world frame
+      ref_forces[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
+    }
+    sbp_offset = tmp_fix_coords.rot * hrp::Vector3(sbp_offset);
+
+    target_root_p = m_robot->rootLink()->p;
+    target_root_R = m_robot->rootLink()->R;
+    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+      if ( control_mode == MODE_IDLE || std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end() ) {
+        it->second.target_p0 = it->second.target_link->p;
+        it->second.target_r0 = it->second.target_link->R;
+      }
+    }
+    // Just for ik initial value
+    if (control_mode == MODE_SYNC_TO_ABC) {
+        current_root_p = target_root_p;
+        current_root_R = target_root_R;
+        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
+                it->second.target_p0 = it->second.target_link->p;
+                it->second.target_r0 = it->second.target_link->R;
+            }
+        }
+    }
+    // Move hand for hand fix mode
+    //   If arms' ABCIKparam.is_active is true, move hands according to cog velocity.
+    //   If is_hand_fix_mode is false, no hand fix mode and move hands according to cog velocity.
+    //   If is_hand_fix_mode is true, hand fix mode and do not move hands in Y axis in tmp_fix_coords.rot.    
+    if (gg_is_walking) {
+        // hand control while walking = solve hand ik with is_hand_fix_mode and solve hand ik without is_hand_fix_mode
+        bool is_hand_control_while_walking = false;
+        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
+                 && it->first.find("arm") != std::string::npos ) {
+                is_hand_control_while_walking = true;
+            }
+        }
+        if (is_hand_control_while_walking) {
+        //if (false) { // Disabled temporarily
+            // Store hand_fix_initial_offset in the initialization of walking
+            if (is_hand_fix_initial) hand_fix_initial_offset = tmp_fix_coords.rot.transpose() * (hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos);
+            is_hand_fix_initial = false;
+            hrp::Vector3 dif_p = hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos - tmp_fix_coords.rot * hand_fix_initial_offset;
+            if (is_hand_fix_mode) {
+                dif_p = tmp_fix_coords.rot.transpose() * dif_p;
+                dif_p(1) = 0;
+                dif_p = tmp_fix_coords.rot * dif_p;
+            }
+            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+                if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
+                     && it->first.find("arm") != std::string::npos ) {
+                    it->second.target_p0 = it->second.target_p0 + dif_p;
+                }
+            }
+        }
+    }
+
+    hrp::Vector3 tmp_foot_mid_pos(hrp::Vector3::Zero());
+    {
+        std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+        std::map<leg_type, double> zmp_weight_map = gg->get_zmp_weight_map();
+        double sum_of_weight = 0.0;
+        for (size_t i = 0; i < leg_names.size(); i++) {
+            ABCIKparam& tmpikp = ikp[leg_names[i]];
+            // get target_end_coords
+            tmpikp.target_end_coords.pos = tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos;
+            tmpikp.target_end_coords.rot = tmpikp.target_r0 * tmpikp.localR;
+            // for foot_mid_pos
+            std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
+            tmp_foot_mid_pos += (tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]) * zmp_weight_map[dst->first];
+            sum_of_weight += zmp_weight_map[dst->first];
+        }
+        tmp_foot_mid_pos *= (1.0 / sum_of_weight);
+    }
+    //
+    {
+        if ( gg_is_walking && gg->get_lcg_count() == gg->get_overwrite_check_timing()+2 ) {
+            hrp::Vector3 vel_htc(calc_vel_from_hand_error(tmp_fix_coords));
+            gg->set_offset_velocity_param(vel_htc(0), vel_htc(1) ,vel_htc(2));
+        }//  else {
+        //     if ( gg_is_walking && gg->get_lcg_count() == static_cast<size_t>(gg->get_default_step_time()/(2*m_dt))-1) {
+        //         gg->set_offset_velocity_param(0,0,0);
+        //     }
+        // }
+    }
+
+
+    hrp::Vector3 tmp_ref_cog(m_robot->calcCM());
+    if (gg_is_walking) {
+      ref_cog = gg->get_cog();
+    } else {
+      ref_cog = tmp_foot_mid_pos;
+    }
+    ref_cog(2) = tmp_ref_cog(2);
+    if (gg_is_walking) {
+      ref_zmp = gg->get_refzmp();
+    } else {
+      ref_zmp(0) = ref_cog(0);
+      ref_zmp(1) = ref_cog(1);
+      ref_zmp(2) = tmp_foot_mid_pos(2);
+    }
+  }
+};
+
+hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2)
+{
+  hrp::Vector3 vv = axis1.cross(axis2);
+  if (fabs(vv.norm()-0.0) < 1e-5) {
+    return rot;
+  } else {
+    Eigen::AngleAxis<double> tmpr(std::asin(vv.norm()), vv.normalized());
+    return tmpr.toRotationMatrix() * rot;
+  }
+}
+
+void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot)
+{
+  // get current foot mid pos + rot
+  std::vector<coordinates> foot_coords;
+  for (size_t i = 0; i < leg_names.size(); i++) {
+      ABCIKparam& tmpikp = ikp[leg_names[i]];
+      if (leg_names[i].find("leg") != std::string::npos) foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
+                                                                                           (tmpikp.target_link->R * tmpikp.localR)));
+  }
+  coordinates current_foot_mid_coords;
+  multi_mid_coords(current_foot_mid_coords, foot_coords);
+  hrp::Vector3 current_foot_mid_pos = current_foot_mid_coords.pos;
+  hrp::Matrix33 current_foot_mid_rot = current_foot_mid_coords.rot;
+  // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
+  hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose());
+  m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
+  rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
+  m_robot->calcForwardKinematics();
+}
+
+
+bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
+{
+  param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
+  // IK check
+  hrp::Vector3 vel_p, vel_r;
+  vel_p = param.target_p0 - param.target_link->p;
+  rats::difference_rotation(vel_r, param.target_link->R, param.target_r0);
+  if (vel_p.norm() > pos_ik_thre && transition_interpolator->isEmpty()) {
+      if (param.pos_ik_error_count % ik_error_debug_print_freq == 0) {
+          std::cerr << "[" << m_profile.instance_name << "] Too large IK error (vel_p) = [" << vel_p(0) << " " << vel_p(1) << " " << vel_p(2) << "][m], count = " << param.pos_ik_error_count << std::endl;
+      }
+      param.pos_ik_error_count++;
+      has_ik_failed = true;
+  } else {
+      param.pos_ik_error_count = 0;
+  }
+  if (vel_r.norm() > rot_ik_thre && transition_interpolator->isEmpty()) {
+      if (param.rot_ik_error_count % ik_error_debug_print_freq == 0) {
+          std::cerr << "[" << m_profile.instance_name << "] Too large IK error (vel_r) = [" << vel_r(0) << " " << vel_r(1) << " " << vel_r(2) << "][rad], count = " << param.rot_ik_error_count << std::endl;
+      }
+      param.rot_ik_error_count++;
+      has_ik_failed = true;
+  } else {
+      param.rot_ik_error_count = 0;
+  }
+  return true;
+}
+
+
+
+void AutoBalancer::solveLimbIK ()
+{
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+    if (it->second.is_active) {
+      for ( int j = 0; j < it->second.manip->numJoints(); j++ ){
+	int i = it->second.manip->joint(j)->jointId;
+	m_robot->joint(i)->q = qorg[i];
+      }
+    }
+  }
+  m_robot->rootLink()->p = current_root_p;
+  m_robot->rootLink()->R = current_root_R;
+  m_robot->calcForwardKinematics();
+  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
+  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
+  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
+  dif_cog *= leg_names_interpolator_ratio;
+  dif_cog(2) = m_robot->rootLink()->p(2) - target_root_p(2);
+  m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog;
+  m_robot->rootLink()->R = target_root_R;
+
+
+  //for HumanSynchronizer
+//  static int HumanSyncCountdownNum = 5 * (1/m_dt);
+  if(hsp->startCountdownForHumanSync){
+    std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";//ややCOMのIKに手間取った時プリント
+	  hsp->updateCountDown();
+  }
+  if(hsp->isHumanSyncOn()){
+	  if(hsp->ht_first_call){
+		  hsp->init_wld_rp_basepos = m_robot->rootLink()->p;
+      hsp->setInitOffsetPose();
+      hsp->rp_wld_initpos.com = m_robot->calcCM();
+      hsp->rp_wld_initpos.rf = ikp["rleg"].target_link->p;
+      hsp->rp_wld_initpos.lf = ikp["lleg"].target_link->p;
+      if(ikp.count("rarm"))hsp->rp_wld_initpos.rh = ikp["rarm"].target_link->p;
+      if(ikp.count("larm"))hsp->rp_wld_initpos.lh = ikp["larm"].target_link->p;
+      hsp->ht_first_call = false;
+
+      hsp->calibInitHumanCOMFromZMP();
+
+      std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;//ややCOMのIKに手間取った時プリント
+
+	  }
+  }else{
+    hsp->setInitOffsetPose();
+  }
+
+  hsp->update();//////HumanSynchronizerの主要処理
+  if(loop%100==0)hsp->rp_ref_out.print();
+  if(loop%100==0)cerr<<"getUpdateTime:"<<hsp->getUpdateTime()*1000<<"[ms]"<<endl;
+
+  if(hsp->isHumanSyncOn()){
+	  ////////////////////// 重心拘束位置設定 /////////////////////////
+    m_robot->rootLink()->p = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;//まずは重心目標分，体幹を動かす
+    m_robot->rootLink()->p(2) = hsp->init_wld_rp_basepos(2)+ hsp->rp_ref_out.com(2);//高さだけはベース基準
+    m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
+    m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
+  }
+
+  // Fix for toe joint
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+      if (it->second.is_active && it->second.has_toe_joint && gg->get_use_toe_joint()) {
+          int i = it->second.target_link->jointId;
+          if (gg->get_swing_leg_names().front() == it->first) {
+              m_robot->joint(i)->q = qrefv[i] + -1 * gg->get_foot_dif_rot_angle();
+          } else {
+              m_robot->joint(i)->q = qrefv[i];
+          }
+      }
+  }
+  m_robot->calcForwardKinematics();
+
+
+  // additional COM fitting IK for HumanSynchronizer
+  hrp::Vector3 tmp_com_err = hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM();
+  int com_ik_loop=0;
+  const int COM_IK_MAX_LOOP = 10;
+  const double COM_IK_MAX_ERROR = 0.0001;//0.001だとたまにカクッとなる
+  if(hsp->isHumanSyncOn()){
+    ikp["rleg"].target_p0 = hsp->rp_wld_initpos.rf + hsp->rp_ref_out.rf;
+    ikp["lleg"].target_p0 = hsp->rp_wld_initpos.lf + hsp->rp_ref_out.lf;
+    if(ikp.count("rarm"))ikp["rarm"].target_p0 = hsp->rp_wld_initpos.rh + hsp->rp_ref_out.rh;
+    if(ikp.count("larm"))ikp["larm"].target_p0 = hsp->rp_wld_initpos.lh + hsp->rp_ref_out.lh;
+    while(fabs(tmp_com_err(0))>COM_IK_MAX_ERROR || fabs(tmp_com_err(1))>COM_IK_MAX_ERROR || fabs(tmp_com_err(2))>COM_IK_MAX_ERROR){//Z方向のCOM合わせは要注意(たまに怪しい)
+		  m_robot->rootLink()->p(0) += tmp_com_err(0);
+      m_robot->rootLink()->p(1) += tmp_com_err(1);
+      m_robot->rootLink()->p(2) += tmp_com_err(2);
+		  m_robot->calcForwardKinematics();
+		  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+		    if (it->second.is_active) solveLimbIKforLimb(it->second);
+		  }
+		  m_robot->calcForwardKinematics();
+      tmp_com_err = (hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM());
+		  if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "[" << m_profile.instance_name << "] COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
+	  }
+
+//    static hrp::Vector3 pre_ik_basepos;
+//    for(int i=0;i<3;i++){//重心拘束で解いた結果の体幹軌道の滑らかさは保証されていないので体幹位置に速度制限＆再度IK
+//      if(m_robot->rootLink()->p(i) - pre_ik_basepos(i) > 0.0002){ m_robot->rootLink()->p(i) =  pre_ik_basepos(i) + 0.0002; }
+//      else if(m_robot->rootLink()->p(i) - pre_ik_basepos(i) < -0.0002){ m_robot->rootLink()->p(i) =  pre_ik_basepos(i) - 0.0002; }
+//    }
+//    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+//      if (it->second.is_active) solveLimbIKforLimb(it->second);
+//    }
+//    pre_ik_basepos = m_robot->rootLink()->p;
+
+    //outport用のデータ上書き
+    ref_zmp(0) = hsp->rp_ref_out.zmp(0);
+    ref_zmp(1) = hsp->rp_ref_out.zmp(1);
+    ref_zmp(2) = 0;
+    ref_cog = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;
+
+	  if(loop%100==0)if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
+  }else{
+	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
+	    if (it->second.is_active) solveLimbIKforLimb(it->second);
+	  }
+  }
+  if (gg_is_walking && !gg_solved) stopWalking ();
+}
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onAborting(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onError(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onReset(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onStateUpdate(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onRateChanged(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+bool AutoBalancer::startHumanSyncAfter5sec()
+{
+	std::cerr << "[" << m_profile.instance_name << "] start HumanSync after 5 sec" << std::endl;
+	hsp->startCountdownForHumanSync = true;
+	return true;
+}
+
+bool AutoBalancer::stopHumanSync()
+{
+	std::cerr << "[" << m_profile.instance_name << "] stop HumanSync Now" << std::endl;
+	return true;
+}
+
+void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence& limbs)
+{
+  std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
+  Guard guard(m_mutex);
+  double tmp_ratio = 0.0;
+  transition_interpolator->clear();
+  transition_interpolator->set(&tmp_ratio);
+  tmp_ratio = 1.0;
+  transition_interpolator->go(&tmp_ratio, transition_time, true);
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+    it->second.is_active = false;
+  }
+
+  for (size_t i = 0; i < limbs.length(); i++) {
+    ABCIKparam& tmp = ikp[std::string(limbs[i])];
+    tmp.is_active = true;
+    std::cerr << "[" << m_profile.instance_name << "]   limb [" << std::string(limbs[i]) << "]" << std::endl;
+  }
+
+  control_mode = MODE_SYNC_TO_ABC;
+}
+
+void AutoBalancer::stopABCparam()
+{
+  std::cerr << "[" << m_profile.instance_name << "] stop auto balancer mode" << std::endl;
+  //Guard guard(m_mutex);
+  double tmp_ratio = 1.0;
+  transition_interpolator->clear();
+  transition_interpolator->set(&tmp_ratio);
+  tmp_ratio = 0.0;
+  transition_interpolator->go(&tmp_ratio, transition_time, true);
+  control_mode = MODE_SYNC_TO_IDLE;
+}
+
+void AutoBalancer::startWalking ()
+{
+  if ( control_mode != MODE_ABC ) {
+    return_control_mode = control_mode;
+    OpenHRP::AutoBalancerService::StrSequence fix_limbs;
+    fix_limbs.length(2);
+    fix_limbs[0] = "rleg";
+    fix_limbs[1] = "lleg";
+    startABCparam(fix_limbs);
+    waitABCTransition();
+  }
+  {
+    Guard guard(m_mutex);
+    has_ik_failed = false;
+    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+        it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
+    }
+    std::vector<std::string> init_swing_leg_names(gg->get_footstep_front_leg_names());
+    std::vector<std::string> tmp_all_limbs(leg_names);
+    std::vector<std::string> init_support_leg_names;
+    std::sort(tmp_all_limbs.begin(), tmp_all_limbs.end());
+    std::sort(init_swing_leg_names.begin(), init_swing_leg_names.end());
+    std::set_difference(tmp_all_limbs.begin(), tmp_all_limbs.end(),
+                        init_swing_leg_names.begin(), init_swing_leg_names.end(),
+                        std::back_inserter(init_support_leg_names));
+    std::vector<step_node> init_support_leg_steps, init_swing_leg_dst_steps;
+    for (std::vector<std::string>::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++)
+        init_support_leg_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0));
+    for (std::vector<std::string>::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++)
+        init_swing_leg_dst_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0));
+    gg->set_default_zmp_offsets(default_zmp_offsets);
+    gg->initialize_gait_parameter(ref_cog, init_support_leg_steps, init_swing_leg_dst_steps);
+  }
+  is_hand_fix_initial = true;
+  while ( !gg->proc_one_tick() );
+  {
+    Guard guard(m_mutex);
+    gg_is_walking = gg_solved = true;
+  }
+}
+
+void AutoBalancer::stopWalking ()
+{
+  std::vector<coordinates> tmp_end_coords_list;
+  for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
+      if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(ikp[*it].target_end_coords);
+  }
+  multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
+  fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot);
+  gg->clear_footstep_nodes_list();
+  if (return_control_mode == MODE_IDLE) stopABCparam();
+  gg_is_walking = false;
+}
+
+bool AutoBalancer::startAutoBalancer (const OpenHRP::AutoBalancerService::StrSequence& limbs)
+{
+  if (control_mode == MODE_IDLE) {
+    has_ik_failed = false;
+    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+        it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
+    }
+    startABCparam(limbs);
+    waitABCTransition();
+    return_control_mode = MODE_ABC;
+    return true;
+  } else {
+    return false;
+  }
+}
+
+bool AutoBalancer::stopAutoBalancer ()
+{
+  if (control_mode == MODE_ABC) {
+    stopABCparam();
+    waitABCTransition();
+    return true;
+  } else {
+    return false;
+  }
+}
+
+void AutoBalancer::waitABCTransition()
+{
+  while (!transition_interpolator->isEmpty()) usleep(1000);
+  usleep(1000);
+}
+bool AutoBalancer::goPos(const double& x, const double& y, const double& th)
+{
+    //  if ( !gg_is_walking && !is_stop_mode) {
+  if ( !is_stop_mode) {
+    gg->set_all_limbs(leg_names);
+    coordinates start_ref_coords;
+    std::vector<coordinates> initial_support_legs_coords;
+    std::vector<leg_type> initial_support_legs;
+    bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
+    if (is_valid_gait_type == false) return false;
+    bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th,
+                                                      initial_support_legs_coords, // Dummy if gg_is_walking
+                                                      start_ref_coords,            // Dummy if gg_is_walking
+                                                      initial_support_legs,        // Dummy if gg_is_walking
+                                                      (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps.
+    if ( !gg_is_walking ) { // Initializing
+        startWalking();
+    }
+    if (!ret) {
+        std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl;
+    }
+    return ret;
+  } else {
+    std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl;
+    return false;
+  }
+}
+
+bool AutoBalancer::goVelocity(const double& vx, const double& vy, const double& vth)
+{
+  gg->set_all_limbs(leg_names);
+  if (gg_is_walking && gg_solved) {
+    gg->set_velocity_param(vx, vy, vth);
+  } else {
+    coordinates ref_coords;
+    mid_coords(ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
+    std::vector<leg_type> current_legs;
+    switch(gait_type) {
+    case BIPED:
+        current_legs = (vy > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
+        break;
+    case TROT:
+        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
+        break;
+    case PACE:
+        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
+        break;
+    case CRAWL:
+        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    case GALLOP:
+        /* at least one leg shoud be in contact */
+        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    default: break;
+    }
+    gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs);
+    startWalking();
+  }
+  return true;
+}
+
+bool AutoBalancer::goStop ()
+{
+  gg->finalize_velocity_mode();
+  waitFootSteps();
+  return true;
+}
+
+bool AutoBalancer::emergencyStop ()
+{
+  std::cerr << "[" << m_profile.instance_name << "] emergencyStop" << std::endl;
+  // is_stop_mode = true;
+  gg->emergency_stop();
+  waitFootSteps();
+  return true;
+}
+
+bool AutoBalancer::releaseEmergencyStop ()
+{
+  if (is_stop_mode) {
+      std::cerr << "[" << m_profile.instance_name << "] releaseEmergencyStop" << std::endl;
+      is_stop_mode = false;
+  }
+  return true;
+}
+
+bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
+{
+  OpenHRP::AutoBalancerService::StepParamsSequence spss;
+  spss.length(fss.length());
+  // If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
+  // If gg_is_walking is true, do not set to 0.
+  for (size_t i = 0; i < spss.length(); i++) {
+      spss[i].sps.length(fss[i].fs.length());
+      for (size_t j = 0; j < spss[i].sps.length(); j++) {
+          spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
+          spss[i].sps[j].step_time = gg->get_default_step_time();
+          spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle());
+          spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle());
+      }
+  }
+  setFootStepsWithParam(fss, spss, overwrite_fs_idx);
+}
+
+bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
+{
+    if (!is_stop_mode) {
+        std::cerr << "[" << m_profile.instance_name << "] setFootStepsList" << std::endl;
+
+        // Initial footstep Snapping
+        coordinates tmpfs, fstrans;
+        step_node initial_support_step, initial_input_step;
+        {
+            std::vector<step_node> initial_support_steps;
+            if (gg_is_walking) {
+                if (overwrite_fs_idx <= 0) {
+                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
+                    return false;
+                }
+                if (!gg->get_footstep_nodes_by_index(initial_support_steps, overwrite_fs_idx-1)) {
+                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
+                    return false;
+                }
+            } else {
+                // If walking, snap initial leg to current ABC foot coords.
+                for (size_t i = 0; i < fss[0].fs.length(); i++) {
+                    initial_support_steps.push_back(step_node(std::string(fss[0].fs[i].leg), ikp[std::string(fss[0].fs[i].leg)].target_end_coords, 0, 0, 0, 0));
+                }
+            }
+            initial_support_step = initial_support_steps.front(); /* use only one leg for representation */
+        }
+        {
+            std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+            for (size_t i = 0; i < fss[0].fs.length(); i++) {
+                if (std::string(fss[0].fs[i].leg) == leg_type_map[initial_support_step.l_r]) {
+                    coordinates tmp;
+                    memcpy(tmp.pos.data(), fss[0].fs[i].pos, sizeof(double)*3);
+                    tmp.rot = (Eigen::Quaternion<double>(fss[0].fs[i].rot[0], fss[0].fs[i].rot[1], fss[0].fs[i].rot[2], fss[0].fs[i].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+                    initial_input_step = step_node(std::string(fss[0].fs[i].leg), tmp, 0, 0, 0, 0);
+                }
+            }
+        }
+
+        // Get footsteps
+        std::vector< std::vector<coordinates> > fs_vec_list;
+        std::vector< std::vector<std::string> > leg_name_vec_list;
+        for (size_t i = 0; i < fss.length(); i++) {
+            std::vector<coordinates> fs_vec;
+            std::vector<std::string> leg_name_vec;
+            for (size_t j = 0; j < fss[i].fs.length(); j++) {
+                std::string leg(fss[i].fs[j].leg);
+                if (std::find(leg_names.begin(), leg_names.end(), leg) != leg_names.end()) {
+                    memcpy(tmpfs.pos.data(), fss[i].fs[j].pos, sizeof(double)*3);
+                    tmpfs.rot = (Eigen::Quaternion<double>(fss[i].fs[j].rot[0], fss[i].fs[j].rot[1], fss[i].fs[j].rot[2], fss[i].fs[j].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+                    initial_input_step.worldcoords.transformation(fstrans, tmpfs);
+                    tmpfs = initial_support_step.worldcoords;
+                    tmpfs.transform(fstrans);
+                } else {
+                    std::cerr << "[" << m_profile.instance_name << "]   No such target : " << leg << std::endl;
+                    return false;
+                }
+                leg_name_vec.push_back(leg);
+                fs_vec.push_back(tmpfs);
+            }
+            leg_name_vec_list.push_back(leg_name_vec);
+            fs_vec_list.push_back(fs_vec);
+        }
+        if (spss.length() != fs_vec_list.size()) {
+            std::cerr << "[" << m_profile.instance_name << "]   StepParam length " << spss.length () << " != Footstep length " << fs_vec_list.size() << std::endl;
+            return false;
+        }
+        std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl;
+        std::vector< std::vector<step_node> > fnsl;
+        for (size_t i = 0; i < fs_vec_list.size(); i++) {
+            if (!(gg_is_walking && i == 0)) { // If initial footstep, e.g., not walking, pass user-defined footstep list. If walking, pass cdr footsteps in order to neglect initial double support leg.
+                std::vector<step_node> tmp_fns;
+                for (size_t j = 0; j < fs_vec_list.at(i).size(); j++) {
+                    tmp_fns.push_back(step_node(leg_name_vec_list[i][j], fs_vec_list[i][j], spss[i].sps[j].step_height, spss[i].sps[j].step_time, spss[i].sps[j].toe_angle, spss[i].sps[j].heel_angle));
+                }
+                fnsl.push_back(tmp_fns);
+            }
+        }
+        if (gg_is_walking) {
+            std::cerr << "[" << m_profile.instance_name << "]  Set overwrite footsteps" << std::endl;
+            gg->set_overwrite_foot_steps_list(fnsl);
+            gg->set_overwrite_foot_step_index(overwrite_fs_idx);
+        } else {
+            std::cerr << "[" << m_profile.instance_name << "]  Set normal footsteps" << std::endl;
+            gg->set_foot_steps_list(fnsl);
+            startWalking();
+        }
+        return true;
+    } else {
+        std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl;
+        return false;
+    }
+}
+
+void AutoBalancer::waitFootSteps()
+{
+  //while (gg_is_walking) usleep(10);
+  while (gg_is_walking || !transition_interpolator->isEmpty() )
+    usleep(1000);
+  usleep(1000);
+  gg->set_offset_velocity_param(0,0,0);
+}
+
+void AutoBalancer::waitFootStepsEarly(const double tm)
+{
+  if (!gg_is_walking) { return;}
+  while ( !gg->is_finalizing(tm)|| !transition_interpolator->isEmpty() )
+    usleep(1000);
+  usleep(1000);
+  gg->set_offset_velocity_param(0,0,0);
+}
+
+bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
+{
+  std::cerr << "[" << m_profile.instance_name << "] setGaitGeneratorParam" << std::endl;
+  gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]);
+  std::vector<hrp::Vector3> off;
+  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
+      off.push_back(hrp::Vector3(i_param.leg_default_translate_pos[i][0], i_param.leg_default_translate_pos[i][1], i_param.leg_default_translate_pos[i][2]));
+  }
+  gg->set_leg_default_translate_pos(off);
+  gg->set_default_step_time(i_param.default_step_time);
+  gg->set_default_step_height(i_param.default_step_height);
+  gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio/2.0);
+  gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio/2.0);
+  gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio/2.0);
+  gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio/2.0);
+  gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio/2.0);
+  gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio/2.0);
+  // gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio_before);
+  // gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio_after);
+  // gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio_before);
+  // gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio_after);
+  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_before);
+  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_after);
+  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_swing_before);
+  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_swing_after);
+  if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::SHUFFLING) {
+    gg->set_default_orbit_type(SHUFFLING);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOID) {
+    gg->set_default_orbit_type(CYCLOID);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::RECTANGLE) {
+    gg->set_default_orbit_type(RECTANGLE);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::STAIR) {
+    gg->set_default_orbit_type(STAIR);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAY) {
+    gg->set_default_orbit_type(CYCLOIDDELAY);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK) {
+    gg->set_default_orbit_type(CYCLOIDDELAYKICK);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CROSS) {
+    gg->set_default_orbit_type(CROSS);
+  }
+  gg->set_swing_trajectory_delay_time_offset(i_param.swing_trajectory_delay_time_offset);
+  gg->set_swing_trajectory_final_distance_weight(i_param.swing_trajectory_final_distance_weight);
+  gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2]));
+  gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2]));  
+  gg->set_gravitational_acceleration(i_param.gravitational_acceleration);
+  gg->set_toe_angle(i_param.toe_angle);
+  gg->set_heel_angle(i_param.heel_angle);
+  gg->set_toe_pos_offset_x(i_param.toe_pos_offset_x);
+  gg->set_heel_pos_offset_x(i_param.heel_pos_offset_x);
+  gg->set_toe_zmp_offset_x(i_param.toe_zmp_offset_x);
+  gg->set_heel_zmp_offset_x(i_param.heel_zmp_offset_x);
+  std::vector<double> tmp_ratio(i_param.toe_heel_phase_ratio.get_buffer(), i_param.toe_heel_phase_ratio.get_buffer()+i_param.toe_heel_phase_ratio.length());
+  std::cerr << "[" << m_profile.instance_name << "]   "; // for set_toe_heel_phase_ratio
+  gg->set_toe_heel_phase_ratio(tmp_ratio);
+  gg->set_use_toe_joint(i_param.use_toe_joint);
+  gg->set_use_toe_heel_transition(i_param.use_toe_heel_transition);
+  gg->set_zmp_weight_map(boost::assign::map_list_of<leg_type, double>(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3]));
+  gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num);
+  gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset);
+
+  // print
+  gg->print_param(std::string(m_profile.instance_name));
+  return true;
+};
+
+bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
+{
+  gg->get_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]);
+  std::vector<hrp::Vector3> off;
+  gg->get_leg_default_translate_pos(off);
+  i_param.leg_default_translate_pos.length(off.size());
+  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
+      i_param.leg_default_translate_pos[i].length(3);
+      i_param.leg_default_translate_pos[i][0] = off[i](0);
+      i_param.leg_default_translate_pos[i][1] = off[i](1);
+      i_param.leg_default_translate_pos[i][2] = off[i](2);
+  }
+  i_param.default_step_time = gg->get_default_step_time();
+  i_param.default_step_height = gg->get_default_step_height();
+  i_param.default_double_support_ratio_before = gg->get_default_double_support_ratio_before();
+  i_param.default_double_support_ratio_after = gg->get_default_double_support_ratio_after();
+  i_param.default_double_support_static_ratio_before = gg->get_default_double_support_static_ratio_before();
+  i_param.default_double_support_static_ratio_after = gg->get_default_double_support_static_ratio_after();
+  i_param.default_double_support_ratio_swing_before = gg->get_default_double_support_ratio_swing_before();
+  i_param.default_double_support_ratio_swing_after = gg->get_default_double_support_ratio_swing_after();
+  i_param.default_double_support_ratio = i_param.default_double_support_ratio_before + i_param.default_double_support_ratio_after;
+  i_param.default_double_support_static_ratio = i_param.default_double_support_static_ratio_before + i_param.default_double_support_static_ratio_after;
+  if (gg->get_default_orbit_type() == SHUFFLING) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::SHUFFLING;
+  } else if (gg->get_default_orbit_type() == CYCLOID) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOID;
+  } else if (gg->get_default_orbit_type() == RECTANGLE) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::RECTANGLE;
+  } else if (gg->get_default_orbit_type() == STAIR) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::STAIR;
+  } else if (gg->get_default_orbit_type() == CYCLOIDDELAY) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAY;
+  } else if (gg->get_default_orbit_type() == CYCLOIDDELAYKICK) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK;
+  } else if (gg->get_default_orbit_type() == CROSS) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CROSS;
+  }
+
+  hrp::Vector3 tmpv = gg->get_stair_trajectory_way_point_offset();
+  for (size_t i = 0; i < 3; i++) i_param.stair_trajectory_way_point_offset[i] = tmpv(i);
+  tmpv = gg->get_cycloid_delay_kick_point_offset();
+  for (size_t i = 0; i < 3; i++) i_param.cycloid_delay_kick_point_offset[i] = tmpv(i);
+  i_param.swing_trajectory_delay_time_offset = gg->get_swing_trajectory_delay_time_offset();
+  i_param.swing_trajectory_final_distance_weight = gg->get_swing_trajectory_final_distance_weight();
+  i_param.gravitational_acceleration = gg->get_gravitational_acceleration();
+  i_param.toe_angle = gg->get_toe_angle();
+  i_param.heel_angle = gg->get_heel_angle();
+  i_param.toe_pos_offset_x = gg->get_toe_pos_offset_x();
+  i_param.heel_pos_offset_x = gg->get_heel_pos_offset_x();
+  i_param.toe_zmp_offset_x = gg->get_toe_zmp_offset_x();
+  i_param.heel_zmp_offset_x = gg->get_heel_zmp_offset_x();
+  std::vector<double> ratio(gg->get_NUM_TH_PHASES(),0.0);
+  gg->get_toe_heel_phase_ratio(ratio);
+  for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) i_param.toe_heel_phase_ratio[i] = ratio[i];
+  i_param.use_toe_joint = gg->get_use_toe_joint();
+  i_param.use_toe_heel_transition = gg->get_use_toe_heel_transition();
+  std::map<leg_type, double> tmp_zmp_weight_map = gg->get_zmp_weight_map();
+  i_param.zmp_weight_map[0] = tmp_zmp_weight_map[RLEG];
+  i_param.zmp_weight_map[1] = tmp_zmp_weight_map[LLEG];
+  i_param.zmp_weight_map[2] = tmp_zmp_weight_map[RARM];
+  i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM];
+  i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num();
+  i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset();
+  return true;
+};
+
+bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
+{
+  Guard guard(m_mutex);
+  std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl;
+  double *default_zmp_offsets_array = new double[ikp.size()*3];
+  move_base_gain = i_param.move_base_gain;
+  for (size_t i = 0; i < ikp.size(); i++)
+    for (size_t j = 0; j < 3; j++)
+      default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j];
+  zmp_transition_time = i_param.zmp_transition_time;
+  adjust_footstep_transition_time = i_param.adjust_footstep_transition_time;
+  if (zmp_offset_interpolator->isEmpty()) {
+      zmp_offset_interpolator->clear();
+      zmp_offset_interpolator->go(default_zmp_offsets_array, zmp_transition_time, true);
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets cannot be set because interpolating." << std::endl;
+  }
+  if (control_mode == MODE_IDLE) {
+    switch (i_param.use_force_mode) {
+    case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
+        use_force = MODE_NO_FORCE;
+        break;
+    case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
+        use_force = MODE_REF_FORCE;
+        break;
+    default:
+        break;
+    }
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   use_force_mode cannot be changed to [" << i_param.use_force_mode << "] during MODE_ABC, MODE_SYNC_TO_IDLE or MODE_SYNC_TO_ABC." << std::endl;
+  }
+  graspless_manip_mode = i_param.graspless_manip_mode;
+  graspless_manip_arm = std::string(i_param.graspless_manip_arm);
+  for (size_t j = 0; j < 3; j++)
+      graspless_manip_p_gain[j] = i_param.graspless_manip_p_gain[j];
+  for (size_t j = 0; j < 3; j++)
+      graspless_manip_reference_trans_coords.pos[j] = i_param.graspless_manip_reference_trans_pos[j];
+  graspless_manip_reference_trans_coords.rot = (Eigen::Quaternion<double>(i_param.graspless_manip_reference_trans_rot[0],
+                                                                          i_param.graspless_manip_reference_trans_rot[1],
+                                                                          i_param.graspless_manip_reference_trans_rot[2],
+                                                                          i_param.graspless_manip_reference_trans_rot[3]).normalized().toRotationMatrix()); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+  transition_time = i_param.transition_time;
+  std::vector<std::string> cur_leg_names, dst_leg_names;
+  cur_leg_names = leg_names;
+  for (size_t i = 0; i < i_param.leg_names.length(); i++) {
+      dst_leg_names.push_back(std::string(i_param.leg_names[i]));
+  }
+  std::sort(cur_leg_names.begin(), cur_leg_names.end());
+  std::sort(dst_leg_names.begin(), dst_leg_names.end());
+  if (cur_leg_names != dst_leg_names) {
+      if (leg_names_interpolator->isEmpty()) {
+          leg_names.clear();
+          leg_names = dst_leg_names;
+          if (control_mode == MODE_ABC) {
+              double tmp_ratio = 0.0;
+              leg_names_interpolator->set(&tmp_ratio);
+              tmp_ratio = 1.0;
+              leg_names_interpolator->go(&tmp_ratio, 5.0, true);
+              control_mode = MODE_SYNC_TO_ABC;
+          }
+      }
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   leg_names cannot be set because interpolating." << std::endl;
+  }
+  pos_ik_thre = i_param.pos_ik_thre;
+  rot_ik_thre = i_param.rot_ik_thre;
+  if (!gg_is_walking) {
+      is_hand_fix_mode = i_param.is_hand_fix_mode;
+      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode = " << is_hand_fix_mode << std::endl;
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode cannot be set in (gg_is_walking = true). Current is_hand_fix_mode is " << (is_hand_fix_mode?"true":"false") << std::endl;
+  }
+  if (control_mode == MODE_IDLE) {
+      for (size_t i = 0; i < i_param.end_effector_list.length(); i++) {
+          std::map<std::string, ABCIKparam>::iterator it = ikp.find(std::string(i_param.end_effector_list[i].leg));
+          memcpy(it->second.localPos.data(), i_param.end_effector_list[i].pos, sizeof(double)*3);
+          it->second.localR = (Eigen::Quaternion<double>(i_param.end_effector_list[i].rot[0], i_param.end_effector_list[i].rot[1], i_param.end_effector_list[i].rot[2], i_param.end_effector_list[i].rot[3])).normalized().toRotationMatrix();
+      }
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl;
+  }
+  if (i_param.default_gait_type == OpenHRP::AutoBalancerService::BIPED) {
+      gait_type = BIPED;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::TROT) {
+      gait_type = TROT;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::PACE) {
+      gait_type = PACE;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::CRAWL) {
+      gait_type = CRAWL;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) {
+      gait_type = GALLOP;
+  }
+  for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
+      std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   localR = " << it->second.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
+  }
+
+  std::cerr << "[" << m_profile.instance_name << "]   move_base_gain = " << move_base_gain << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets = ";
+  for (size_t i = 0; i < ikp.size() * 3; i++) {
+      std::cerr << default_zmp_offsets_array[i] << " ";
+  }
+  std::cerr << std::endl;
+  delete[] default_zmp_offsets_array;
+  std::cerr << "[" << m_profile.instance_name << "]   use_force_mode = " << use_force << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_mode = " << graspless_manip_mode << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_arm = " << graspless_manip_arm << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_pos = " << graspless_manip_reference_trans_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_rot = " << graspless_manip_reference_trans_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   transition_time = " << transition_time << "[s], zmp_transition_time = " << zmp_transition_time << "[s], adjust_footstep_transition_time = " << adjust_footstep_transition_time << "[s]" << std::endl;
+  for (std::vector<std::string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) std::cerr << "[" << m_profile.instance_name << "]   leg_names [" << *it << "]" << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   pos_ik_thre = " << pos_ik_thre << "[m], rot_ik_thre = " << rot_ik_thre << "[rad]" << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   default_gait_type = " << gait_type << std::endl;
+  // IK limb parameters
+  std::cerr << "[" << m_profile.instance_name << "]  IK limb parameters" << std::endl;
+  bool is_ik_limb_parameter_valid_length = true;
+  if (i_param.ik_limb_parameters.length() != ee_vec.size()) {
+      is_ik_limb_parameter_valid_length = false;
+      std::cerr << "[" << m_profile.instance_name << "]   ik_limb_parameters invalid length! Cannot be set. (input = " << i_param.ik_limb_parameters.length() << ", desired = " << ee_vec.size() << ")" << std::endl;
+  } else {
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          if (ikp[ee_vec[i]].manip->numJoints() != i_param.ik_limb_parameters[i].ik_optional_weight_vector.length())
+              is_ik_limb_parameter_valid_length = false;
+      }
+      if (is_ik_limb_parameter_valid_length) {
+          for (size_t i = 0; i < ee_vec.size(); i++) {
+              ABCIKparam& param = ikp[ee_vec[i]];
+              const OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i];
+              std::vector<double> ov;
+              ov.resize(param.manip->numJoints());
+              for (size_t j = 0; j < param.manip->numJoints(); j++) {
+                  ov[j] = ilp.ik_optional_weight_vector[j];
+              }
+              param.manip->setOptionalWeightVector(ov);
+              param.manip->setSRGain(ilp.sr_gain);
+              param.avoid_gain = ilp.avoid_gain;
+              param.reference_gain = ilp.reference_gain;
+              param.manip->setManipulabilityLimit(ilp.manipulability_limit);
+          }
+      } else {
+          std::cerr << "[" << m_profile.instance_name << "]   ik_optional_weight_vector invalid length! Cannot be set. (input = [";
+          for (size_t i = 0; i < ee_vec.size(); i++) {
+              std::cerr << i_param.ik_limb_parameters[i].ik_optional_weight_vector.length() << ", ";
+          }
+          std::cerr << "], desired = [";
+          for (size_t i = 0; i < ee_vec.size(); i++) {
+              std::cerr << ikp[ee_vec[i]].manip->numJoints() << ", ";
+          }
+          std::cerr << "])" << std::endl;
+      }
+  }
+  if (is_ik_limb_parameter_valid_length) {
+      std::cerr << "[" << m_profile.instance_name << "]   ik_optional_weight_vectors = ";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          ABCIKparam& param = ikp[ee_vec[i]];
+          std::vector<double> ov;
+          ov.resize(param.manip->numJoints());
+          param.manip->getOptionalWeightVector(ov);
+          std::cerr << "[";
+          for (size_t j = 0; j < param.manip->numJoints(); j++) {
+              std::cerr << ov[j] << " ";
+          }
+          std::cerr << "]";
+      }
+      std::cerr << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   sr_gains = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].manip->getSRGain() << ", ";
+      }
+      std::cerr << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   avoid_gains = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].avoid_gain << ", ";
+      }
+      std::cerr << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   reference_gains = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].reference_gain << ", ";
+      }
+      std::cerr << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   manipulability_limits = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].manip->getManipulabilityLimit() << ", ";
+      }
+      std::cerr << "]" << std::endl;
+  }
+  return true;
+};
+
+bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
+{
+  i_param.move_base_gain = move_base_gain;
+  i_param.default_zmp_offsets.length(ikp.size());
+  for (size_t i = 0; i < ikp.size(); i++) {
+      i_param.default_zmp_offsets[i].length(3);
+      for (size_t j = 0; j < 3; j++) {
+          i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j);
+      }
+  }
+  switch(control_mode) {
+  case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break;
+  case MODE_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_ABC; break;
+  case MODE_SYNC_TO_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_IDLE; break;
+  case MODE_SYNC_TO_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_ABC; break;
+  default: break;
+  }
+  switch(use_force) {
+  case MODE_NO_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_NO_FORCE; break;
+  case MODE_REF_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE; break;
+  default: break;
+  }
+  i_param.graspless_manip_mode = graspless_manip_mode;
+  i_param.graspless_manip_arm = graspless_manip_arm.c_str();
+  for (size_t j = 0; j < 3; j++)
+      i_param.graspless_manip_p_gain[j] = graspless_manip_p_gain[j];
+  for (size_t j = 0; j < 3; j++)
+      i_param.graspless_manip_reference_trans_pos[j] = graspless_manip_reference_trans_coords.pos[j];
+  Eigen::Quaternion<double> qt(graspless_manip_reference_trans_coords.rot);
+  i_param.graspless_manip_reference_trans_rot[0] = qt.w();
+  i_param.graspless_manip_reference_trans_rot[1] = qt.x();
+  i_param.graspless_manip_reference_trans_rot[2] = qt.y();
+  i_param.graspless_manip_reference_trans_rot[3] = qt.z();
+  i_param.transition_time = transition_time;
+  i_param.zmp_transition_time = zmp_transition_time;
+  i_param.adjust_footstep_transition_time = adjust_footstep_transition_time;
+  i_param.leg_names.length(leg_names.size());
+  for (size_t i = 0; i < leg_names.size(); i++) i_param.leg_names[i] = leg_names.at(i).c_str();
+  i_param.pos_ik_thre = pos_ik_thre;
+  i_param.rot_ik_thre = rot_ik_thre;
+  i_param.is_hand_fix_mode = is_hand_fix_mode;
+  i_param.end_effector_list.length(ikp.size());
+  {
+      size_t i = 0;
+      for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
+          copyRatscoords2Footstep(i_param.end_effector_list[i],
+                                  coordinates(it->second.localPos, it->second.localR));
+          i_param.end_effector_list[i].leg = it->first.c_str();
+          i++;
+      }
+  }
+  switch(gait_type) {
+  case BIPED:  i_param.default_gait_type = OpenHRP::AutoBalancerService::BIPED;  break;
+  case TROT:   i_param.default_gait_type = OpenHRP::AutoBalancerService::TROT;   break;
+  case PACE:   i_param.default_gait_type = OpenHRP::AutoBalancerService::PACE;   break;
+  case CRAWL:  i_param.default_gait_type = OpenHRP::AutoBalancerService::CRAWL;  break;
+  case GALLOP: i_param.default_gait_type = OpenHRP::AutoBalancerService::GALLOP; break;
+  default: break;
+  }
+  i_param.ik_limb_parameters.length(ee_vec.size());
+  for (size_t i = 0; i < ee_vec.size(); i++) {
+      ABCIKparam& param = ikp[ee_vec[i]];
+      OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i];
+      ilp.ik_optional_weight_vector.length(param.manip->numJoints());
+      std::vector<double> ov;
+      ov.resize(param.manip->numJoints());
+      param.manip->getOptionalWeightVector(ov);
+      for (size_t j = 0; j < param.manip->numJoints(); j++) {
+          ilp.ik_optional_weight_vector[j] = ov[j];
+      }
+      ilp.sr_gain = param.manip->getSRGain();
+      ilp.avoid_gain = param.avoid_gain;
+      ilp.reference_gain = param.reference_gain;
+      ilp.manipulability_limit = param.manip->getManipulabilityLimit();
+  }
+  return true;
+};
+
+void AutoBalancer::copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs)
+{
+  memcpy(out_fs.pos, in_fs.pos.data(), sizeof(double)*3);
+  Eigen::Quaternion<double> qt(in_fs.rot);
+  out_fs.rot[0] = qt.w();
+  out_fs.rot[1] = qt.x();
+  out_fs.rot[2] = qt.y();
+  out_fs.rot[3] = qt.z();
+};
+
+bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param)
+{
+  copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_leg_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.swing_leg_dst_coords, gg->get_swing_leg_dst_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.dst_foot_midcoords, gg->get_dst_foot_midcoords());
+  if (gg->get_support_leg_names().front() == "rleg") {
+    i_param.support_leg = OpenHRP::AutoBalancerService::RLEG;
+  } else {
+    i_param.support_leg = OpenHRP::AutoBalancerService::LLEG;
+  }
+  switch ( gg->get_current_support_states().front() ) {
+  case BOTH: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::BOTH; break;
+  case RLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::RLEG; break;
+  case LLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::LLEG; break;
+  default: break;
+  }
+  return true;
+};
+
+bool AutoBalancer::adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep)
+{
+  std::cerr << "[" << m_profile.instance_name << "] adjustFootSteps" << std::endl;
+  if (control_mode == MODE_ABC && !gg_is_walking && adjust_footstep_interpolator->isEmpty()) {
+      Guard guard(m_mutex);
+      //
+      hrp::Vector3 eepos, org_mid_rpy, target_mid_rpy;
+      hrp::Matrix33 eerot, tmprot;
+      coordinates org_mid_coords, target_mid_coords;
+      // Get org coords
+      ikp["rleg"].adjust_interpolation_org_p0 = ikp["rleg"].target_p0;
+      ikp["lleg"].adjust_interpolation_org_p0 = ikp["lleg"].target_p0;
+      ikp["rleg"].adjust_interpolation_org_r0 = ikp["rleg"].target_r0;
+      ikp["lleg"].adjust_interpolation_org_r0 = ikp["lleg"].target_r0;
+      mid_coords(org_mid_coords, 0.5,
+                 coordinates(ikp["rleg"].adjust_interpolation_org_p0, ikp["rleg"].adjust_interpolation_org_r0),
+                 coordinates(ikp["lleg"].adjust_interpolation_org_p0, ikp["lleg"].adjust_interpolation_org_r0));
+      org_mid_rpy = hrp::rpyFromRot(org_mid_coords.rot);
+      // Get target coords
+      //   Input : ee coords
+      //   Output : link coords
+      memcpy(eepos.data(), rfootstep.pos, sizeof(double)*3);
+      eerot = (Eigen::Quaternion<double>(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
+      ikp["rleg"].adjust_interpolation_target_r0 = eerot * ikp["rleg"].localR.transpose();
+      ikp["rleg"].adjust_interpolation_target_p0 = eepos - ikp["rleg"].adjust_interpolation_target_r0 * ikp["rleg"].localPos;
+      memcpy(eepos.data(), lfootstep.pos, sizeof(double)*3);
+      eerot = (Eigen::Quaternion<double>(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
+      ikp["lleg"].adjust_interpolation_target_r0 = eerot * ikp["lleg"].localR.transpose();
+      ikp["lleg"].adjust_interpolation_target_p0 = eepos - ikp["lleg"].adjust_interpolation_target_r0 * ikp["lleg"].localPos;
+      mid_coords(target_mid_coords, 0.5,
+                 coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0),
+                 coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
+      coordinates rtrans, ltrans;
+      target_mid_coords.transformation(rtrans, coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0));
+      target_mid_coords.transformation(ltrans, coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
+      target_mid_rpy = hrp::rpyFromRot(target_mid_coords.rot);
+      // Fix target pos => org pos, target yaw => org yaw
+      target_mid_rpy(2) = org_mid_rpy(2);
+      target_mid_coords.rot = hrp::rotFromRpy(target_mid_rpy);
+      target_mid_coords.pos = org_mid_coords.pos;
+      // Calculate rleg and lleg coords
+      coordinates tmpc;
+      tmpc = target_mid_coords;
+      tmpc.transform(rtrans);
+      ikp["rleg"].adjust_interpolation_target_p0 = tmpc.pos;
+      ikp["rleg"].adjust_interpolation_target_r0 = tmpc.rot;
+      tmpc = target_mid_coords;
+      tmpc.transform(ltrans);
+      ikp["lleg"].adjust_interpolation_target_p0 = tmpc.pos;
+      ikp["lleg"].adjust_interpolation_target_r0 = tmpc.rot;
+      // Set interpolator
+      adjust_footstep_interpolator->clear();
+      double tmp = 0.0;
+      adjust_footstep_interpolator->set(&tmp);
+      tmp = 1.0;
+      adjust_footstep_interpolator->go(&tmp, adjust_footstep_transition_time, true);
+  }
+  while (!adjust_footstep_interpolator->isEmpty() )
+    usleep(1000);
+  usleep(1000);
+  return true;
+};
+
+bool AutoBalancer::getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx)
+{
+    std::cerr << "[" << m_profile.instance_name << "] getRemainingFootstepSequence" << std::endl;
+    o_footstep = new OpenHRP::AutoBalancerService::FootstepSequence;
+    if (gg_is_walking) {
+        std::vector< std::vector<step_node> > fsnl = gg->get_remaining_footstep_nodes_list();
+        o_current_fs_idx = gg->get_footstep_index();
+        o_footstep->length(fsnl.size());
+        for (size_t i = 0; i < fsnl.size(); i++) {
+            o_footstep[i].leg = (fsnl[i].front().l_r==RLEG?"rleg":"lleg");
+            copyRatscoords2Footstep(o_footstep[i], fsnl[i].front().worldcoords);
+        }
+    }
+};
+
+bool AutoBalancer::getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep)
+{
+    std::cerr << "[" << m_profile.instance_name << "] getGoPosFootstepsSequence" << std::endl;
+    o_footstep = new OpenHRP::AutoBalancerService::FootstepsSequence;
+    if (gg_is_walking) {
+        std::cerr << "[" << m_profile.instance_name << "] Cannot call getGoPosFootstepsSequence in walking" << std::endl;
+        return false;
+    } else {
+        gg->set_all_limbs(leg_names);
+        std::vector< std::vector<step_node> > new_footstep_nodes_list;
+        coordinates start_ref_coords;
+        std::vector<coordinates> initial_support_legs_coords;
+        std::vector<leg_type> initial_support_legs;
+        bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
+        if (is_valid_gait_type == false) return false;
+        /* go_pos_param_2_footstep_nodes_list_core is const member function  */
+        gg->go_pos_param_2_footstep_nodes_list_core (x, y, th,
+                                                     initial_support_legs_coords, start_ref_coords, initial_support_legs,
+                                                     new_footstep_nodes_list, true, 0);
+        o_footstep->length(new_footstep_nodes_list.size());
+        for (size_t i = 0; i < new_footstep_nodes_list.size(); i++) {
+            o_footstep[i].fs.length(new_footstep_nodes_list.at(i).size());
+            for (size_t j = 0; j < new_footstep_nodes_list.at(i).size(); j++) {
+                leg_type tmp_leg_type = new_footstep_nodes_list.at(i).at(j).l_r;
+                o_footstep[i].fs[j].leg = ((tmp_leg_type == RLEG) ? "rleg":
+                                           (tmp_leg_type == LLEG) ? "lleg":
+                                           (tmp_leg_type == RARM) ? "rarm":
+                                           "larm");
+                copyRatscoords2Footstep(o_footstep[i].fs[j], new_footstep_nodes_list.at(i).at(j).worldcoords);
+            }
+        }
+        return true;
+    }
+};
+
+void AutoBalancer::static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height)
+{
+  hrp::Vector3 target_sbp = hrp::Vector3(0, 0, 0);
+  hrp::Vector3 tmpcog = m_robot->calcCM();
+  switch ( use_force ) {
+  case MODE_REF_FORCE:
+    calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height, ref_forces);
+    tmp_input_sbp = target_sbp - sbp_offset;
+    sbp_cog_offset = tmp_input_sbp - tmpcog;
+    break;
+  case MODE_NO_FORCE:
+    tmp_input_sbp = tmpcog + sbp_cog_offset;
+    break;
+  default: break;
+  }
+};
+
+void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces)
+{
+  hrp::Vector3 denom, nume;
+  /* sb_point[m] = nume[kg * m/s^2 * m] / denom[kg * m/s^2] */
+  double mass = m_robot->totalMass();
+  for (size_t j = 0; j < 2; j++) {
+    nume(j) = mass * gg->get_gravitational_acceleration() * tmpcog(j);
+    denom(j) = mass * gg->get_gravitational_acceleration();
+    for (size_t i = 0; i < sensor_names.size(); i++) {
+      if ( sensor_names[i].find("hsensor") != std::string::npos || sensor_names[i].find("asensor") != std::string::npos ) { // tempolary to get arm force coords
+          hrp::Link* parentlink;
+          hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
+          if (sensor) parentlink = sensor->link;
+          else parentlink = m_vfs[sensor_names[i]].link;
+          for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+              if (it->second.target_link->name == parentlink->name) {
+                  hrp::Vector3 fpos = parentlink->p + parentlink->R * it->second.localPos;
+                  nume(j) += ( (fpos(2) - ref_com_height) * tmp_forces[i](j) - fpos(j) * tmp_forces[i](2) );
+                  denom(j) -= tmp_forces[i](2);
+              }
+          }
+      }
+    }
+    sb_point(j) = nume(j) / denom(j);
+  }
+  sb_point(2) = ref_com_height;
+};
+
+#ifndef rad2deg
+#define rad2deg(rad) (rad * 180 / M_PI)
+#endif
+#ifndef deg2rad
+#define deg2rad(deg) (deg * M_PI / 180)
+#endif
+
+hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_coords)
+{
+  if (graspless_manip_mode) {
+    hrp::Vector3 dp,dr;
+    coordinates ref_hand_coords(gg->get_dst_foot_midcoords()), act_hand_coords;
+    ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords
+    hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos);
+    if ( graspless_manip_arm == "arms" ) {
+        hrp::Vector3 rarm_pos = ikp["rarm"].target_p0 + ikp["rarm"].target_r0 * ikp["rarm"].localPos;
+        hrp::Vector3 larm_pos = ikp["larm"].target_p0 + ikp["larm"].target_r0 * ikp["larm"].localPos;
+        act_hand_coords.pos = (rarm_pos+larm_pos)/2.0;
+        hrp::Vector3 act_y = larm_pos-rarm_pos;
+        act_y(2) = 0;
+        act_y.normalize();
+        hrp::Vector3 ref_y(ref_hand_coords.rot * hrp::Vector3::UnitY());
+        ref_y(2) = 0;
+        ref_y.normalize();
+        dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y))); // fix for rotation
+    } else {
+      ABCIKparam& tmpikp = ikp[graspless_manip_arm];
+      act_hand_coords = rats::coordinates(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos,
+                                          tmpikp.target_r0 * tmpikp.localR);
+      rats::difference_rotation(dr, ref_hand_coords.rot, act_hand_coords.rot);
+      dr(0) = 0; dr(1) = 0;
+    }
+    dp = act_hand_coords.pos - ref_hand_coords.pos
+        + dr.cross(hrp::Vector3(foot_pos - act_hand_coords.pos));
+    dp(2) = 0;
+    hrp::Matrix33 foot_mt(gg->get_dst_foot_midcoords().rot.transpose());
+    //alias(dp) = foot_mt * dp;
+    hrp::Vector3 dp2 = foot_mt * dp;
+    //alias(dr) = foot_mt * dr;
+    return hrp::Vector3(graspless_manip_p_gain[0] * dp2(0)/gg->get_default_step_time(),
+                        graspless_manip_p_gain[1] * dp2(1)/gg->get_default_step_time(),
+                        graspless_manip_p_gain[2] * rad2deg(dr(2))/gg->get_default_step_time());
+  } else {
+    return hrp::Vector3::Zero();
+  }
+};
+
+bool AutoBalancer::calc_inital_support_legs(const double& y, std::vector<coordinates>& initial_support_legs_coords, std::vector<leg_type>& initial_support_legs, coordinates& start_ref_coords) {
+    switch(gait_type) {
+    case BIPED:
+        initial_support_legs_coords = (y > 0 ?
+                                       boost::assign::list_of(ikp["rleg"].target_end_coords)
+                                       : boost::assign::list_of(ikp["lleg"].target_end_coords));
+        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
+        break;
+    case TROT:
+        initial_support_legs_coords = (y > 0 ?
+                                       boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["larm"].target_end_coords)
+                                       : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["rarm"].target_end_coords));
+        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
+        break;
+    case PACE:
+        initial_support_legs_coords = (y > 0 ?
+                                       boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["rarm"].target_end_coords)
+                                       : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["larm"].target_end_coords));
+        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
+        break;
+    case CRAWL:
+        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    case GALLOP:
+        /* at least one leg shoud be in contact */
+        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    default: break;
+    }
+    mid_coords(start_ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
+    return true;
+};
+
+//
+extern "C"
+{
+
+    void AutoBalancerInit(RTC::Manager* manager)
+    {
+        RTC::Properties profile(autobalancer_spec);
+        manager->registerFactory(profile,
+                                 RTC::Create<AutoBalancer>,
+                                 RTC::Delete<AutoBalancer>);
+    }
+
+};
+
+
diff --git a/rtc/AutoBalancer/bak/AutoBalancer.h b/rtc/AutoBalancer/bak/AutoBalancer.h
new file mode 100644
index 00000000000..e793c7c9db6
--- /dev/null
+++ b/rtc/AutoBalancer/bak/AutoBalancer.h
@@ -0,0 +1,596 @@
+// -*- C++ -*-
+/*!
+ * @file  AutoBalancer.h
+ * @brief autobalancer component
+ * @date  $Date$
+ *
+ * $Id$
+ */
+
+#ifndef AUTOBALANCER_H
+#define AUTOBALANCER_H
+
+#include <rtm/Manager.h>
+#include <rtm/DataFlowComponentBase.h>
+#include <rtm/CorbaPort.h>
+#include <rtm/DataInPort.h>
+#include <rtm/DataOutPort.h>
+#include <rtm/idl/BasicDataTypeSkel.h>
+#include <rtm/idl/ExtendedDataTypesSkel.h>
+#include <hrpModel/Body.h>
+#include "../ImpedanceController/JointPathEx.h"
+#include "../ImpedanceController/RatsMatrix.h"
+#include "GaitGenerator.h"
+// Service implementation headers
+// <rtc-template block="service_impl_h">
+#include "AutoBalancerService_impl.h"
+#include "interpolator.h"
+
+// </rtc-template>
+
+// Service Consumer stub headers
+// <rtc-template block="consumer_stub_h">
+
+
+class HumanPose{
+  private: hrp::Vector3 zero;
+  public:
+    hrp::Vector3 com,rf,lf,rh,lh;
+    hrp::Vector3 zmp;
+    int idsize;
+    typedef struct{ hrp::Vector3 f; hrp::Vector3 t; }Wrench6;
+    Wrench6 rfw,lfw;
+    HumanPose() : idsize(10){
+      clear();
+    }
+    ~HumanPose(){cerr<<"HumanPose destructed"<<endl;}
+    void clear(){
+      for(int i=0;i<idsize;i++){ Seq(i) = hrp::Vector3::Zero(); }
+    }
+    hrp::Vector3& Seq(const int i){
+      switch(i){
+        case 0: return com;
+        case 1: return rf;
+        case 2: return lf;
+        case 3: return rh;
+        case 4: return lh;
+        case 5: return zmp;
+        case 6: return rfw.f;
+        case 7: return rfw.t;
+        case 8: return lfw.f;
+        case 9: return lfw.t;
+        default: std::cerr <<"[WARN] HumanPose::getSeq(int) invalid index"<<std::endl;return zero;
+      }
+    }
+    const hrp::Vector3& Seq(const int i) const{//const指定オブジェクトからオーバーロードする時のためのオーバーライド
+//      return Seq(i);//呼べない・・・うーん
+      switch(i){
+        case 0: return com;
+        case 1: return rf;
+        case 2: return lf;
+        case 3: return rh;
+        case 4: return lh;
+        case 5: return zmp;
+        case 6: return rfw.f;
+        case 7: return rfw.t;
+        case 8: return lfw.f;
+        case 9: return lfw.t;
+        default: std::cerr <<"[WARN] HumanPose::getSeq(int) invalid index"<<std::endl;return zero;
+      }
+    }
+    static void hp_printf(HumanPose& in){
+      const std::string cap[] = {"com:","rf:","lf:","rh:","lh:","zmp:","rfw:","","lfw:",""};
+      for(int i=0;i<10;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.Seq(i)(0),in.Seq(i)(1),in.Seq(i)(2)); }
+      printf("\n");
+    }
+    void print(){ hp_printf(*this); }
+};
+
+
+class HumanSynchronizer{
+  private:
+
+    double FNUM;
+    int FUPLEVELNUM;
+    int cur_rfup_level,cur_lfup_level;
+    double FUPHIGHT;
+    double CNT_F_TH;
+    hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
+    hrp::Vector3 init_wld_rp_compos;
+    hrp::Vector3 init_wld_hp_rfpos,init_wld_hp_lfpos;
+    hrp::Vector3 init_wld_rp_rfpos,init_wld_rp_lfpos;
+    hrp::Vector3 init_hp_calibcom;
+    double MAXVEL,MAXACC;
+    HumanPose hp_wld_raw;
+    HumanPose hp_wld_initpos;
+    HumanPose hp_rel_raw;
+    HumanPose hp_filtered;
+    HumanPose rp_ref_out_old;
+    int countdown_num;
+    bool HumanSyncOn;
+    struct timeval t_calc_start, t_calc_end;
+
+  public:
+
+    bool startCountdownForHumanSync;
+    bool ht_first_call;
+    bool is_rf_contact,is_lf_contact;
+    HumanPose rp_wld_initpos;
+    double h2r_ratio;
+    hrp::Vector3 init_wld_rp_basepos;
+    HumanPose rp_ref_out;
+
+    HumanSynchronizer(){
+      h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
+//      h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
+//      h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
+      //h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
+      cur_rfup_level = 0;       cur_lfup_level = 0;
+      is_rf_contact = true;     is_lf_contact = true;
+      pre_cont_rfpos = hrp::Vector3::Zero();    pre_cont_lfpos = hrp::Vector3::Zero();
+      init_wld_hp_rfpos = hrp::Vector3(0,-0.1/h2r_ratio,0);
+      init_wld_hp_lfpos = hrp::Vector3(0, 0.1/h2r_ratio,0);
+      init_wld_rp_rfpos = hrp::Vector3(0,-0.1,0);
+      init_wld_rp_lfpos = hrp::Vector3(0, 0.1,0);
+      init_hp_calibcom.Zero();
+      FNUM = 0.01;
+      FUPLEVELNUM = 200;
+      FUPHIGHT = 0.05;
+      CNT_F_TH = 20.0;
+      MAXVEL = 0.004;
+      //      MAXVEL = 0.001;
+      HumanSyncOn = false;
+      ht_first_call = true;
+      startCountdownForHumanSync = false;
+      countdown_num = 5*500;
+    }
+    ~HumanSynchronizer(){cout<<"HumanSynchronizer destructed"<<endl;}
+
+    void readInput(const HumanPose& raw_in){ hp_wld_raw = raw_in; }
+    void setInitOffsetPose(){ hp_wld_initpos = hp_wld_raw; }
+    const int getRemainingCountDown() const { return countdown_num; }
+    const bool isHumanSyncOn() const { return HumanSyncOn; }
+    const double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0; }
+    void updateCountDown(){
+      if(countdown_num>0){
+        countdown_num--;
+      }else if(countdown_num==0){
+        HumanSyncOn = true;
+        startCountdownForHumanSync = false;
+      }
+    }
+    void update(){
+      gettimeofday(&t_calc_start, NULL);
+      removeInitOffsetPose(hp_wld_raw, hp_wld_initpos, hp_rel_raw);
+      applyInitHumanCOMOffset(hp_wld_raw, init_hp_calibcom, hp_wld_raw);
+      applyLPFilter(hp_rel_raw, hp_filtered, hp_filtered);
+      lockFootXYOnContact(hp_filtered, is_rf_contact, is_lf_contact, hp_filtered);//根本から改変すべき
+      calcWorldZMP((hp_filtered.rf+init_wld_hp_rfpos), (hp_filtered.lf+init_wld_hp_lfpos), hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
+      convertRelHumanPoseToRelRobotPose(hp_filtered,rp_ref_out);
+      judgeFootContactStates(rp_ref_out.rfw, rp_ref_out.lfw, is_rf_contact, is_lf_contact);
+      overwriteFootZFromContactStates(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);
+//      lockFootXYOnContact(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);//ここで改変するとLPFかかってないからジャンプする
+      applyWorkspaceLimit(rp_ref_out, rp_ref_out);
+      applyCOMToSupportRegionLimit(rp_ref_out, rp_ref_out);
+      applyVelLimit(rp_ref_out, rp_ref_out_old, rp_ref_out);
+//      rp_ref_out.com(2) = 0;//Z方向固定
+      rp_ref_out_old = rp_ref_out;
+      gettimeofday(&t_calc_end, NULL);
+    }
+    void calibInitHumanCOMFromZMP(){
+      init_hp_calibcom(0) = hp_filtered.zmp(0);
+      init_hp_calibcom(1) = hp_filtered.zmp(1);
+    }
+    void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const HumanPose::Wrench6& rfwin, const HumanPose::Wrench6& lfwin, hrp::Vector3& zmp_ans){
+      hrp::Vector3 rfzmp,lfzmp;
+      const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
+      if( rfwin.f(2) > 1.0e-6 ){
+        rfzmp(0) = ( - rfwin.t(1) - rfwin.f(0) * F_H_OFFSET + rfwin.f(2) * 0 ) / rfwin.f(2) + rfpos(0);
+        rfzmp(1) = (   rfwin.t(0) - rfwin.f(1) * F_H_OFFSET + rfwin.f(2) * 0 ) / rfwin.f(2) + rfpos(1);
+      }
+      if( lfwin.f(2) > 1.0e-6 ){
+        lfzmp(0) = ( - lfwin.t(1) - lfwin.f(0) * F_H_OFFSET + lfwin.f(2) * 0 ) / lfwin.f(2) + lfpos(0);
+        lfzmp(1) = (   lfwin.t(0) - lfwin.f(1) * F_H_OFFSET + lfwin.f(2) * 0 ) / lfwin.f(2) + lfpos(1);
+      }
+      if( rfwin.f(2) > 1.0e-6 || lfwin.f(2) > 1.0e-6 ){
+        zmp_ans(0) = ( rfzmp(0)*rfwin.f(2) + lfzmp(0)*lfwin.f(2) ) / ( rfwin.f(2) + lfwin.f(2));
+        zmp_ans(1) = ( rfzmp(1)*rfwin.f(2) + lfzmp(1)*lfwin.f(2) ) / ( rfwin.f(2) + lfwin.f(2));
+      }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
+      zmp_ans(2) = 0;
+    }
+    static hrp::Vector3 Point3DToVector3(const RTC::TimedPoint3D& in){ hrp::Vector3 out(in.data.x,in.data.y,in.data.z); return out; }
+    static HumanPose::Wrench6 DoubleSeqToWrench6(const RTC::TimedDoubleSeq& in){
+      HumanPose::Wrench6 out;
+      if(in.data.length() == 6){out.f(0)=in.data[0]; out.f(1)=in.data[1]; out.f(2)=in.data[2]; out.t(0)=in.data[3]; out.t(1)=in.data[4]; out.t(2)=in.data[5];
+      }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; out.f.Zero(); out.t.Zero(); }
+      return out;
+    }
+
+  private:
+
+    void removeInitOffsetPose(const HumanPose& abs_in, const HumanPose& init_offset, HumanPose& rel_out){
+      for(int i=0;i<rel_out.idsize-5;i++){
+        rel_out.Seq(i) =  abs_in.Seq(i) - init_offset.Seq(i);
+      }
+      rel_out.rfw = abs_in.rfw;
+      rel_out.lfw = abs_in.lfw;
+    }
+    void applyInitHumanCOMOffset(const HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
+      out = in;
+      out.com = in.com + com_offs;
+    }
+//    void startHumanSync(){
+//      countdown_flag = true;
+//      setInitOffsetPose();
+//      HumanSyncOn = true;
+//    }
+    void applyLPFilter(const HumanPose& raw_in, const HumanPose& out_old, HumanPose& filt_out){
+      for(int i=0;i<filt_out.idsize;i++){
+        filt_out.Seq(i) = (1-FNUM) * out_old.Seq(i) + FNUM * raw_in.Seq(i);
+      }
+    }
+    void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
+      for(int i=0;i<rp_out.idsize-4;i++){
+        rp_out.Seq(i) =  h2r_ratio * hp_in.Seq(i);
+      }
+      rp_out.rfw  = hp_in.rfw;
+      rp_out.lfw  = hp_in.lfw;
+    }
+    void judgeFootContactStates(const HumanPose::Wrench6& rfw_in, const HumanPose::Wrench6& lfw_in, bool& rfcs_ans, bool& lfcs_ans){
+      //他のプログラムは足に荷重時に力センサの値がZ軸マイナスに出るっぽい？(のでTODO FIX)
+      if      (rfw_in.f(2)<CNT_F_TH && lfw_in.f(2)>CNT_F_TH){rfcs_ans = false;  lfcs_ans = true;}//右足浮遊かつ左足接地
+      else if (rfw_in.f(2)>CNT_F_TH && lfw_in.f(2)<CNT_F_TH){rfcs_ans = true;   lfcs_ans = false;}//右足接地かつ左足浮遊
+      else                                              {rfcs_ans = true;   lfcs_ans = true;}//両足接地または両足浮遊
+    }
+    void overwriteFootZFromContactStates(const HumanPose& in, const bool& rfcs_in, const bool& lfcs_in, HumanPose& out){
+      out = in;
+      if(!rfcs_in){//右足浮遊時
+        if(cur_rfup_level < FUPLEVELNUM){
+          cur_rfup_level++;
+          out.rf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
+        }else{
+          out.rf(2) = FUPHIGHT;
+        }
+      }else{
+        if(cur_rfup_level > 0){
+          cur_rfup_level--;
+          out.rf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
+        }else{
+          out.rf(2) = 0;
+        }
+      }
+      if(!lfcs_in){//左足浮遊時
+        if(cur_lfup_level < FUPLEVELNUM){
+          cur_lfup_level++;
+          out.lf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
+        }else{
+          out.lf(2) = FUPHIGHT;
+        }
+      }else{
+        if(cur_lfup_level > 0){
+          cur_lfup_level--;
+          out.lf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
+        }else{
+          out.lf(2) = 0;
+        }
+      }
+    }
+    void lockFootXYOnContact(const HumanPose& in, const bool& rfcs_in, const bool& lfcs_in, HumanPose& out){
+      out = in;
+      if(rfcs_in){
+        out.rf(0) = pre_cont_rfpos(0);
+        out.rf(1) = pre_cont_rfpos(1);
+      }else{
+        pre_cont_rfpos(0) = in.rf(0);
+        pre_cont_rfpos(1) = in.rf(1);
+      }
+      if(lfcs_in){
+        out.lf(0) = pre_cont_lfpos(0);
+        out.lf(1) = pre_cont_lfpos(1);
+      }else{
+        pre_cont_lfpos(0) = in.lf(0);
+        pre_cont_lfpos(1) = in.lf(1);
+      }
+//      out.rf(0) = 0; out.rf(1) = 0; out.lf(0) = 0; out.lf(1) = 0;//足固定テスト
+    }
+    void applyWorkspaceLimit(const HumanPose& in, HumanPose& out){
+      if(!is_rf_contact && is_lf_contact){//右足浮遊時
+        if(in.rf(1) + rp_wld_initpos.rf(1) > in.lf(1) + rp_wld_initpos.lf(1) - 0.15)out.rf(1) = in.lf(1) + rp_wld_initpos.lf(1) - rp_wld_initpos.rf(1) - 0.15;
+      }
+      else if(is_rf_contact && !is_lf_contact){//左足浮遊時
+        if(in.lf(1) + rp_wld_initpos.lf(1) < in.rf(1) + rp_wld_initpos.rf(1) + 0.15)out.lf(1) = in.rf(1) + rp_wld_initpos.rf(1) - rp_wld_initpos.lf(1) + 0.15;
+      }
+    }
+    void applyCOMToSupportRegionLimit(const HumanPose& in, HumanPose& out){
+      out = in;
+      const double XUMARGIN = 0.08;
+      const double XLMARGIN = 0.04;
+      const double YLRMARGIN = 0.03;
+      double x_upper_region,x_lower_region,y_upper_region,y_lower_region;
+      if(rp_wld_initpos.rf(0) + in.rf(0) > rp_wld_initpos.lf(0) + in.lf(0)){
+        x_upper_region = rp_wld_initpos.rf(0) + in.rf(0) + XUMARGIN;
+        x_lower_region = rp_wld_initpos.lf(0) + in.lf(0) - XLMARGIN;
+      }else{
+        x_upper_region = rp_wld_initpos.lf(0) + in.lf(0) + XUMARGIN;
+        x_lower_region = rp_wld_initpos.rf(0) + in.rf(0) - XLMARGIN;
+      }
+      y_upper_region = rp_wld_initpos.lf(1) + in.lf(1) + YLRMARGIN;
+      y_lower_region = rp_wld_initpos.rf(1) + in.rf(1) - YLRMARGIN;
+      if(in.com(0) < x_lower_region){ out.com(0) = x_lower_region; cerr<<"[WARN] COM X Lower Limit!"<<endl;}
+      else if(in.com(0) > x_upper_region){ out.com(0) = x_upper_region; cerr<<"[WARN] COM X Upper Limit!"<<endl;}
+      if(in.com(1) < y_lower_region){ out.com(1) = y_lower_region; cerr<<"[WARN] COM Y Lower Limit!"<<endl;}
+      else if(in.com(1) > y_upper_region){ out.com(1) = y_upper_region; cerr<<"[WARN] COM Y Upper Limit!"<<endl;}
+    }
+    void applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out){
+      for(int i=0;i<3;i++){if(in.com(i) - in_old.com(i) > MAXVEL){out.com(i) = in_old.com(i) + MAXVEL;}else if(in.com(i) - in_old.com(i) < -MAXVEL){out.com(i) = in_old.com(i) - MAXVEL;}}
+      for(int i=0;i<3;i++){if(in.rf(i)  - in_old.rf(i)  > MAXVEL){out.rf(i)  = in_old.rf(i)  + MAXVEL;}else if(in.rf(i)  - in_old.rf(i)  < -MAXVEL){out.rf(i)  = in_old.rf(i)  - MAXVEL;}}
+      for(int i=0;i<3;i++){if(in.lf(i)  - in_old.lf(i)  > MAXVEL){out.lf(i)  = in_old.lf(i)  + MAXVEL;}else if(in.lf(i)  - in_old.lf(i)  < -MAXVEL){out.lf(i)  = in_old.lf(i)  - MAXVEL;}}
+      for(int i=0;i<3;i++){if(in.rh(i)  - in_old.rh(i)  > MAXVEL){out.rh(i)  = in_old.rh(i)  + MAXVEL;}else if(in.rh(i)  - in_old.rh(i)  < -MAXVEL){out.rh(i)  = in_old.rh(i)  - MAXVEL;}}
+      for(int i=0;i<3;i++){if(in.lh(i)  - in_old.lh(i)  > MAXVEL){out.lh(i)  = in_old.lh(i)  + MAXVEL;}else if(in.lh(i)  - in_old.lh(i)  < -MAXVEL){out.lh(i)  = in_old.lh(i)  - MAXVEL;}}
+    }
+};
+
+
+
+// </rtc-template>
+
+using namespace RTC;
+
+class AutoBalancer
+  : public RTC::DataFlowComponentBase
+{
+ public:
+  AutoBalancer(RTC::Manager* manager);
+  virtual ~AutoBalancer();
+
+  // The initialize action (on CREATED->ALIVE transition)
+  // formaer rtc_init_entry()
+ virtual RTC::ReturnCode_t onInitialize();
+
+  // The finalize action (on ALIVE->END transition)
+  // formaer rtc_exiting_entry()
+  virtual RTC::ReturnCode_t onFinalize();
+
+  // The startup action when ExecutionContext startup
+  // former rtc_starting_entry()
+  // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
+
+  // The shutdown action when ExecutionContext stop
+  // former rtc_stopping_entry()
+  // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
+
+  // The activated action (Active state entry action)
+  // former rtc_active_entry()
+  virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
+
+  // The deactivated action (Active state exit action)
+  // former rtc_active_exit()
+  virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
+
+  // The execution action that is invoked periodically
+  // former rtc_active_do()
+  virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
+
+  // The aborting action when main logic error occurred.
+  // former rtc_aborting_entry()
+  // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
+
+  // The error action in ERROR state
+  // former rtc_error_do()
+  // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
+
+  // The reset action that is invoked resetting
+  // This is same but different the former rtc_init_entry()
+  // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
+
+  // The state update action that is invoked after onExecute() action
+  // no corresponding operation exists in OpenRTm-aist-0.2.0
+  // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
+
+  // The action that is invoked when execution context's rate is changed
+  // no corresponding operation exists in OpenRTm-aist-0.2.0
+  // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
+  bool goPos(const double& x, const double& y, const double& th);
+  bool goVelocity(const double& vx, const double& vy, const double& vth);
+  bool goStop();
+  bool emergencyStop ();
+  bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx);
+  bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx);
+  bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx);
+  bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx);
+  void waitFootSteps();
+  void waitFootStepsEarly(const double tm);
+  bool startAutoBalancer(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
+  bool stopAutoBalancer();
+  bool setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param);
+  bool getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param);
+  bool setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param);
+  bool getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param);
+  bool getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param);
+  bool adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep);
+  bool getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx);
+  bool getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep);
+  bool releaseEmergencyStop();
+  bool startHumanSyncAfter5sec();
+  bool stopHumanSync();
+
+ protected:
+  // Configuration variable declaration
+  // <rtc-template block="config_declare">
+  
+  // </rtc-template>
+
+  // DataInPort declaration
+  // <rtc-template block="inport_declare">
+  TimedDoubleSeq m_qRef;
+  InPort<TimedDoubleSeq> m_qRefIn;
+  TimedPoint3D m_basePos;
+  InPort<TimedPoint3D> m_basePosIn;
+  TimedOrientation3D m_baseRpy;
+  InPort<TimedOrientation3D> m_baseRpyIn;
+  TimedPoint3D m_zmp;
+  InPort<TimedPoint3D> m_zmpIn;
+  TimedDoubleSeq m_optionalData;
+  InPort<TimedDoubleSeq> m_optionalDataIn;
+  std::vector<TimedDoubleSeq> m_ref_force;
+  std::vector<InPort<TimedDoubleSeq> *> m_ref_forceIn;
+  TimedLong m_emergencySignal;
+  InPort<TimedLong> m_emergencySignalIn;
+  // for debug
+  TimedPoint3D m_cog;
+  //for human tracker
+  TimedPoint3D m_htzmp;
+  InPort<TimedPoint3D> m_htzmpIn;
+  TimedDoubleSeq m_htrfw;
+  InPort<TimedDoubleSeq> m_htrfwIn;
+  TimedDoubleSeq m_htlfw;
+  InPort<TimedDoubleSeq> m_htlfwIn;
+  TimedPoint3D m_htcom;
+  InPort<TimedPoint3D> m_htcomIn;
+  TimedPoint3D m_htrf;
+  InPort<TimedPoint3D> m_htrfIn;
+  TimedPoint3D m_htlf;
+  InPort<TimedPoint3D> m_htlfIn;
+  TimedPoint3D m_htrh;
+  InPort<TimedPoint3D> m_htrhIn;
+  TimedPoint3D m_htlh;
+  InPort<TimedPoint3D> m_htlhIn;
+  TimedPoint3D m_actzmp;
+  InPort<TimedPoint3D> m_actzmpIn;
+  
+  // </rtc-template>
+
+  // DataOutPort declaration
+  // <rtc-template block="outport_declare">
+  OutPort<TimedDoubleSeq> m_qOut;
+  RTC::OutPort<RTC::TimedPoint3D> m_zmpOut;
+  OutPort<TimedPoint3D> m_basePosOut;
+  OutPort<TimedOrientation3D> m_baseRpyOut;
+  TimedDoubleSeq m_baseTform;
+  OutPort<TimedDoubleSeq> m_baseTformOut;
+  TimedPose3D m_basePose;
+  OutPort<TimedPose3D> m_basePoseOut;
+  TimedAcceleration3D m_accRef;
+  OutPort<TimedAcceleration3D> m_accRefOut;
+  TimedBooleanSeq m_contactStates;
+  OutPort<TimedBooleanSeq> m_contactStatesOut;
+  TimedDoubleSeq m_controlSwingSupportTime;
+  OutPort<TimedDoubleSeq> m_controlSwingSupportTimeOut;
+  TimedBoolean m_walkingStates;
+  OutPort<TimedBoolean> m_walkingStatesOut;
+  TimedPoint3D m_sbpCogOffset;
+  OutPort<TimedPoint3D> m_sbpCogOffsetOut;
+  std::vector<TimedDoubleSeq> m_force;
+  std::vector<OutPort<TimedDoubleSeq> *> m_ref_forceOut;
+  std::vector<TimedPoint3D> m_limbCOPOffset;
+  std::vector<OutPort<TimedPoint3D> *> m_limbCOPOffsetOut;
+  // for debug
+  OutPort<TimedPoint3D> m_cogOut;
+  
+  // </rtc-template>
+
+  // CORBA Port declaration
+  // <rtc-template block="corbaport_declare">
+  RTC::CorbaPort m_AutoBalancerServicePort;
+
+  // </rtc-template>
+
+  // Service declaration
+  // <rtc-template block="service_declare">
+  AutoBalancerService_impl m_service0;
+
+  // </rtc-template>
+
+  // Consumer declaration
+  // <rtc-template block="consumer_declare">
+  
+  // </rtc-template>
+
+ private:
+  struct ABCIKparam {
+    hrp::Vector3 target_p0, localPos, adjust_interpolation_target_p0, adjust_interpolation_org_p0;
+    hrp::Matrix33 target_r0, localR, adjust_interpolation_target_r0, adjust_interpolation_org_r0;
+    rats::coordinates target_end_coords;
+    hrp::Link* target_link;
+    hrp::JointPathExPtr manip;
+    double avoid_gain, reference_gain;
+    size_t pos_ik_error_count, rot_ik_error_count;
+    bool is_active, has_toe_joint;
+  };
+  void getCurrentParameters();
+  void getTargetParameters();
+  bool solveLimbIKforLimb (ABCIKparam& param);
+  void solveLimbIK();
+  void startABCparam(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
+  void stopABCparam();
+  void waitABCTransition();
+  hrp::Matrix33 OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2);
+  void fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot);
+  void startWalking ();
+  void stopWalking ();
+  void copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs);
+  // static balance point offsetting
+  void static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height);
+  void calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces);
+  hrp::Vector3 calc_vel_from_hand_error (const rats::coordinates& tmp_fix_coords);
+  bool isOptionalDataContact (const std::string& ee_name)
+  {
+      return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false;
+  };
+  bool calc_inital_support_legs(const double& y, std::vector<rats::coordinates>& initial_support_legs_coords, std::vector<rats::leg_type>& initial_support_legs, rats::coordinates& start_ref_coords);
+
+  // for gg
+  typedef boost::shared_ptr<rats::gait_generator> ggPtr;
+  ggPtr gg;
+  bool gg_is_walking, gg_solved;
+  // for abc
+  hrp::Vector3 ref_cog, ref_zmp, prev_imu_sensor_pos, prev_imu_sensor_vel, hand_fix_initial_offset;
+  enum {BIPED, TROT, PACE, CRAWL, GALLOP} gait_type;
+  enum {MODE_IDLE, MODE_ABC, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_ABC} control_mode, return_control_mode;
+  std::map<std::string, ABCIKparam> ikp;
+  std::map<std::string, size_t> contact_states_index_map;
+  std::map<std::string, hrp::VirtualForceSensorParam> m_vfs;
+  std::vector<std::string> sensor_names, leg_names, ee_vec;
+  hrp::dvector qorg, qrefv;
+  hrp::Vector3 current_root_p, target_root_p;
+  hrp::Matrix33 current_root_R, target_root_R;
+  rats::coordinates fix_leg_coords;
+  std::vector<hrp::Vector3> default_zmp_offsets;
+  double m_dt, move_base_gain;
+  hrp::BodyPtr m_robot;
+  coil::Mutex m_mutex;
+
+  double transition_interpolator_ratio, transition_time, zmp_transition_time, adjust_footstep_transition_time, leg_names_interpolator_ratio;
+  interpolator *zmp_offset_interpolator;
+  interpolator *transition_interpolator;
+  interpolator *adjust_footstep_interpolator;
+  interpolator *leg_names_interpolator;
+  hrp::Vector3 input_zmp, input_basePos;
+  hrp::Matrix33 input_baseRot;
+
+  // static balance point offsetting
+  hrp::Vector3 sbp_offset, sbp_cog_offset;
+  enum {MODE_NO_FORCE, MODE_REF_FORCE} use_force;
+  std::vector<hrp::Vector3> ref_forces;
+
+  unsigned int m_debugLevel;
+  bool is_legged_robot, is_stop_mode, has_ik_failed, is_hand_fix_mode, is_hand_fix_initial;
+  int loop, ik_error_debug_print_freq;
+  bool graspless_manip_mode;
+  std::string graspless_manip_arm;
+  hrp::Vector3 graspless_manip_p_gain;
+  rats::coordinates graspless_manip_reference_trans_coords;
+  double pos_ik_thre, rot_ik_thre;
+
+  //for HumanSynchronizer
+  boost::shared_ptr<HumanSynchronizer> hsp;
+  HumanPose hp_raw_data;
+
+};
+
+extern "C"
+{
+  void AutoBalancerInit(RTC::Manager* manager);
+};
+
+#endif // IMPEDANCE_H

From 3357cd152ab4eb8fd9ba0e1b8ac43fdb4622cd89 Mon Sep 17 00:00:00 2001
From: ishiguroJSK <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 14 Aug 2016 00:56:30 +0900
Subject: [PATCH 020/217] add setHumanToRobotRatio and setAllowedXYZSync

---
 idl/AutoBalancerService.idl                   |    6 +-
 rtc/AutoBalancer/AutoBalancer.cpp             |   22 +-
 rtc/AutoBalancer/AutoBalancer.h               |   21 +-
 rtc/AutoBalancer/AutoBalancerService_impl.cpp |   10 +
 rtc/AutoBalancer/AutoBalancerService_impl.h   |    2 +
 rtc/AutoBalancer/bak/AutoBalancer.cpp         | 2177 -----------------
 rtc/AutoBalancer/bak/AutoBalancer.h           |  596 -----
 7 files changed, 51 insertions(+), 2783 deletions(-)
 delete mode 100644 rtc/AutoBalancer/bak/AutoBalancer.cpp
 delete mode 100644 rtc/AutoBalancer/bak/AutoBalancer.h

diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl
index 071bc5d3f62..58c070df727 100644
--- a/idl/AutoBalancerService.idl
+++ b/idl/AutoBalancerService.idl
@@ -415,11 +415,11 @@ module OpenHRP
     boolean releaseEmergencyStop();
 
     /**
-     * @brief Stop stepping immediately.
-     * @param
-     * @return true if set successfully, false otherwise
+     * HumanSync
      */
     boolean startHumanSyncAfter5sec();
+    boolean setHumanToRobotRatio(in double h2r);
+    boolean setAllowedXYZSync(in boolean x_on, in boolean y_on, in boolean z_on);
     boolean stopHumanSync();
   };
 };
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 5a5807ba31c..2789d964ef8 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1151,9 +1151,25 @@ void AutoBalancer::solveLimbIK ()
 
 bool AutoBalancer::startHumanSyncAfter5sec()
 {
-	std::cerr << "[" << m_profile.instance_name << "] start HumanSync after 5 sec" << std::endl;
-	hsp->startCountdownForHumanSync = true;
-	return true;
+  std::cerr << "[" << m_profile.instance_name << "] start HumanSync after 5 sec" << std::endl;
+  hsp->startCountdownForHumanSync = true;
+  return true;
+}
+
+bool AutoBalancer::setHumanToRobotRatio(const double h2r)
+{
+  std::cerr << "[" << m_profile.instance_name << "] set h2r_ratio as "<< h2r << std::endl;
+  if(!hsp->isHumanSyncOn())hsp->h2r_ratio = h2r;
+  return true;
+}
+
+bool AutoBalancer::setAllowedXYZSync(const bool x_on,const bool y_on,const bool z_on)
+{
+  std::cerr << "[" << m_profile.instance_name << "] set allowed XYZ move direction as ("<<x_on<<","<<y_on<<","<<z_on<<")" << std::endl;
+  hsp->use_x = x_on;
+  hsp->use_y = y_on;
+  hsp->use_z = z_on;
+  return true;
 }
 
 bool AutoBalancer::stopHumanSync()
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index b565ba8b3f1..8a512648f6d 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -117,6 +117,7 @@ class HumanSynchronizer{
     bool is_rf_contact,is_lf_contact;
     HumanPose rp_wld_initpos;
     double h2r_ratio;
+    bool use_x,use_y,use_z;
     hrp::Vector3 init_wld_rp_basepos;
     HumanPose rp_ref_out;
     FILE* log;
@@ -126,6 +127,7 @@ class HumanSynchronizer{
 //      h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
 //      h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
 //      h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
+      use_x = true; use_y = true; use_z = true;
       cur_rfup_level = 0;       cur_lfup_level = 0;
       is_rf_contact = true;     is_lf_contact = true;
       pre_cont_rfpos = hrp::Vector3::Zero();    pre_cont_lfpos = hrp::Vector3::Zero();
@@ -144,10 +146,10 @@ class HumanSynchronizer{
       ht_first_call = true;
       startCountdownForHumanSync = false;
       countdown_num = 5*500;
-      log = fopen("/home/ishiguro/HumanSyncLog.txt","w+");
+//      log = fopen("/home/ishiguro/HumanSyncLog.txt","w+");
     }
     ~HumanSynchronizer(){
-      fclose(log);
+//      fclose(log);
       cout<<"HumanSynchronizer destructed"<<endl;
     }
 
@@ -167,7 +169,7 @@ class HumanSynchronizer{
     void update(){
       gettimeofday(&t_calc_start, NULL);
       removeInitOffsetPose(hp_wld_raw, hp_wld_initpos, hp_rel_raw);
-//      applyInitHumanCOMOffset(hp_rel_raw, init_hp_calibcom, hp_rel_raw);
+      applyInitHumanCOMOffset(hp_rel_raw, init_hp_calibcom, hp_rel_raw);
       applyLPFilter(hp_rel_raw, hp_filtered, hp_filtered);
       lockFootXYOnContact(hp_filtered, is_rf_contact, is_lf_contact, hp_filtered);//根本から改変すべき
       calcWorldZMP((hp_filtered.rf+init_wld_hp_rfpos), (hp_filtered.lf+init_wld_hp_lfpos), hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
@@ -179,9 +181,10 @@ class HumanSynchronizer{
       applyWorkspaceLimit(rp_ref_out, rp_ref_out);
       applyCOMToSupportRegionLimit(rp_ref_out, rp_ref_out);
       applyVelLimit(rp_ref_out, rp_ref_out_old, rp_ref_out);
+      applyXYZLock(rp_ref_out, rp_ref_out);
       rp_ref_out.com(2) = 0;//Z方向固定
       rp_ref_out_old = rp_ref_out;
-      fprintf(log,"%f %f %f %f\n", rp_ref_out.com(0), rp_ref_out.com(1), rp_ref_out.zmp(0), rp_ref_out.zmp(1));
+//      fprintf(log,"%f %f %f %f\n", rp_ref_out.com(0), rp_ref_out.com(1), rp_ref_out.zmp(0), rp_ref_out.zmp(1));
 
       gettimeofday(&t_calc_end, NULL);
     }
@@ -336,6 +339,14 @@ class HumanSynchronizer{
       for(int i=0;i<3;i++){if(in.rh(i)  - in_old.rh(i)  > MAXVEL){out.rh(i)  = in_old.rh(i)  + MAXVEL;}else if(in.rh(i)  - in_old.rh(i)  < -MAXVEL){out.rh(i)  = in_old.rh(i)  - MAXVEL;}}
       for(int i=0;i<3;i++){if(in.lh(i)  - in_old.lh(i)  > MAXVEL){out.lh(i)  = in_old.lh(i)  + MAXVEL;}else if(in.lh(i)  - in_old.lh(i)  < -MAXVEL){out.lh(i)  = in_old.lh(i)  - MAXVEL;}}
     }
+    void applyXYZLock(const HumanPose& in, HumanPose& out){
+      out = in;
+      for(int i=0;i<out.idsize-4;i++){
+        if(!use_x)out.Seq(i)(0) = 0;
+        if(!use_y)out.Seq(i)(1) = 0;
+        if(!use_z)out.Seq(i)(2) = 0;
+      }
+    }
 };
 
 
@@ -420,6 +431,8 @@ class AutoBalancer
   bool getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep);
   bool releaseEmergencyStop();
   bool startHumanSyncAfter5sec();
+  bool setHumanToRobotRatio(const double h2r);
+  bool setAllowedXYZSync(const bool x_on,const bool y_on,const bool z_on);
   bool stopHumanSync();
 
  protected:
diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.cpp b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
index 0c899349cd8..a813ae1ac43 100644
--- a/rtc/AutoBalancer/AutoBalancerService_impl.cpp
+++ b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
@@ -120,6 +120,16 @@ CORBA::Boolean AutoBalancerService_impl::startHumanSyncAfter5sec()
     return m_autobalancer->startHumanSyncAfter5sec();
 };
 
+CORBA::Boolean AutoBalancerService_impl::setHumanToRobotRatio(const CORBA::Double h2r)
+{
+    return m_autobalancer->setHumanToRobotRatio(h2r);
+};
+
+CORBA::Boolean AutoBalancerService_impl::setAllowedXYZSync(const CORBA::Boolean x_on,const CORBA::Boolean y_on,const CORBA::Boolean z_on)
+{
+    return m_autobalancer->setAllowedXYZSync(x_on,y_on,z_on);
+};
+
 CORBA::Boolean AutoBalancerService_impl::stopHumanSync()
 {
     return m_autobalancer->stopHumanSync();
diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.h b/rtc/AutoBalancer/AutoBalancerService_impl.h
index 01c0f4977b1..a590f8055c4 100644
--- a/rtc/AutoBalancer/AutoBalancerService_impl.h
+++ b/rtc/AutoBalancer/AutoBalancerService_impl.h
@@ -35,6 +35,8 @@ class AutoBalancerService_impl
   CORBA::Boolean getGoPosFootstepsSequence(CORBA::Double x, CORBA::Double y, CORBA::Double th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep);
   CORBA::Boolean releaseEmergencyStop();
   CORBA::Boolean startHumanSyncAfter5sec();
+  CORBA::Boolean setHumanToRobotRatio(const CORBA::Double h2r);
+  CORBA::Boolean setAllowedXYZSync(const CORBA::Boolean x_on,const CORBA::Boolean y_on,const CORBA::Boolean z_on);
   CORBA::Boolean stopHumanSync();
   //
   //
diff --git a/rtc/AutoBalancer/bak/AutoBalancer.cpp b/rtc/AutoBalancer/bak/AutoBalancer.cpp
deleted file mode 100644
index bfbd3621cc9..00000000000
--- a/rtc/AutoBalancer/bak/AutoBalancer.cpp
+++ /dev/null
@@ -1,2177 +0,0 @@
-// -*- C++ -*-
-/*!
- * @file  AutoBalancer.cpp
- * @brief autobalancer component
- * $Date$
- *
- * $Id$
- */
-
-#include <rtm/CorbaNaming.h>
-#include <hrpModel/Link.h>
-#include <hrpModel/Sensor.h>
-#include <hrpModel/ModelLoaderUtil.h>
-#include "AutoBalancer.h"
-#include <hrpModel/JointPath.h>
-#include <hrpUtil/MatrixSolvers.h>
-#include "hrpsys/util/Hrpsys.h"
-
-
-typedef coil::Guard<coil::Mutex> Guard;
-using namespace rats;
-
-// Module specification
-// <rtc-template block="module_spec">
-static const char* autobalancer_spec[] =
-    {
-        "implementation_id", "AutoBalancer",
-        "type_name",         "AutoBalancer",
-        "description",       "autobalancer component",
-        "version",           HRPSYS_PACKAGE_VERSION,
-        "vendor",            "AIST",
-        "category",          "example",
-        "activity_type",     "DataFlowComponent",
-        "max_instance",      "10",
-        "language",          "C++",
-        "lang_type",         "compile",
-        // Configuration variables
-        "conf.default.debugLevel", "0",
-        ""
-    };
-// </rtc-template>
-
-AutoBalancer::AutoBalancer(RTC::Manager* manager)
-    : RTC::DataFlowComponentBase(manager),
-      // <rtc-template block="initializer">
-      m_qRefIn("qRef", m_qRef),
-      m_basePosIn("basePosIn", m_basePos),
-      m_baseRpyIn("baseRpyIn", m_baseRpy),
-      m_zmpIn("zmpIn", m_zmp),
-      m_optionalDataIn("optionalData", m_optionalData),
-      m_emergencySignalIn("emergencySignal", m_emergencySignal),
-      //ishiguro
-      m_htzmpIn("htzmpIn", m_htzmp),
-      m_htrfwIn("htrfwIn", m_htrfw),
-      m_htlfwIn("htlfwIn", m_htlfw),
-      m_htcomIn("htcomIn", m_htcom),
-      m_htrfIn("htrfIn", m_htrf),
-      m_htlfIn("htlfIn", m_htlf),
-      m_htrhIn("htrhIn", m_htrh),
-      m_htlhIn("htlhIn", m_htlh),
-      m_actzmpIn("actzmpIn", m_actzmp),
-
-      m_qOut("q", m_qRef),
-      m_zmpOut("zmpOut", m_zmp),
-      m_basePosOut("basePosOut", m_basePos),
-      m_baseRpyOut("baseRpyOut", m_baseRpy),
-      m_baseTformOut("baseTformOut", m_baseTform),
-      m_basePoseOut("basePoseOut", m_basePose),
-      m_accRefOut("accRef", m_accRef),
-      m_contactStatesOut("contactStates", m_contactStates),
-      m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime),
-      m_cogOut("cogOut", m_cog),
-      m_AutoBalancerServicePort("AutoBalancerService"),
-      m_walkingStatesOut("walkingStates", m_walkingStates),
-      m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
-      // </rtc-template>
-      gait_type(BIPED),
-      move_base_gain(0.8),
-      m_robot(hrp::BodyPtr()),
-      m_debugLevel(0)
-{
-    m_service0.autobalancer(this);
-}
-
-AutoBalancer::~AutoBalancer()
-{
-}
-
-
-RTC::ReturnCode_t AutoBalancer::onInitialize()
-{
-    std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
-    bindParameter("debugLevel", m_debugLevel, "0");
-
-    // Registration: InPort/OutPort/Service
-    // <rtc-template block="registration">
-    // Set InPort buffers
-    addInPort("qRef", m_qRefIn);
-    addInPort("basePosIn", m_basePosIn);
-    addInPort("baseRpyIn", m_baseRpyIn);
-    addInPort("zmpIn", m_zmpIn);
-    addInPort("optionalData", m_optionalDataIn);
-    addInPort("emergencySignal", m_emergencySignalIn);
-    //ishiguro
-    addInPort("htzmpIn", m_htzmpIn);
-    addInPort("htrfwIn", m_htrfwIn);
-    addInPort("htlfwIn", m_htlfwIn);
-    addInPort("htcomIn", m_htcomIn);
-    addInPort("htrfIn", m_htrfIn);
-    addInPort("htlfIn", m_htlfIn);
-    addInPort("htrhIn", m_htrhIn);
-    addInPort("htlhIn", m_htlhIn);
-    addInPort("actzmpIn", m_actzmpIn);
-
-    // Set OutPort buffer
-    addOutPort("q", m_qOut);
-    addOutPort("zmpOut", m_zmpOut);
-    addOutPort("basePosOut", m_basePosOut);
-    addOutPort("baseRpyOut", m_baseRpyOut);
-    addOutPort("baseTformOut", m_baseTformOut);
-    addOutPort("basePoseOut", m_basePoseOut);
-    addOutPort("accRef", m_accRefOut);
-    addOutPort("contactStates", m_contactStatesOut);
-    addOutPort("controlSwingSupportTime", m_controlSwingSupportTimeOut);
-    addOutPort("cogOut", m_cogOut);
-    addOutPort("walkingStates", m_walkingStatesOut);
-    addOutPort("sbpCogOffset", m_sbpCogOffsetOut);
-  
-    // Set service provider to Ports
-    m_AutoBalancerServicePort.registerProvider("service0", "AutoBalancerService", m_service0);
-  
-    // Set service consumers to Ports
-  
-    // Set CORBA Service Ports
-    addPort(m_AutoBalancerServicePort);
-  
-    // </rtc-template>
-    // <rtc-template block="bind_config">
-    // Bind variables and configuration variable
-  
-    // </rtc-template>
-
-    RTC::Properties& prop =  getProperties();
-    coil::stringTo(m_dt, prop["dt"].c_str());
-
-    m_robot = hrp::BodyPtr(new hrp::Body());
-    RTC::Manager& rtcManager = RTC::Manager::instance();
-    std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
-    int comPos = nameServer.find(",");
-    if (comPos < 0){
-        comPos = nameServer.length();
-    }
-    nameServer = nameServer.substr(0, comPos);
-    RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
-    if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
-                                 CosNaming::NamingContext::_duplicate(naming.getRootContext())
-                                 )){
-      std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
-      return RTC::RTC_ERROR;
-    }
-
-    // allocate memory for outPorts
-    m_qRef.data.length(m_robot->numJoints());
-    qorg.resize(m_robot->numJoints());
-    qrefv.resize(m_robot->numJoints());
-    m_baseTform.data.length(12);
-    //ishiguro
-    m_htrfw.data.length(6);
-    m_htlfw.data.length(6);
-
-    control_mode = MODE_IDLE;
-    loop = 0;
-
-    // setting from conf file
-    // GaitGenerator requires abc_leg_offset and abc_stride_parameter in robot conf file
-    // setting leg_pos from conf file
-    coil::vstring leg_offset_str = coil::split(prop["abc_leg_offset"], ",");
-    leg_names.push_back("rleg");
-    leg_names.push_back("lleg");
-
-    // setting from conf file
-    // rleg,TARGET_LINK,BASE_LINK
-    coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
-    size_t prop_num = 10;
-    if (end_effectors_str.size() > 0) {
-      size_t num = end_effectors_str.size()/prop_num;
-      for (size_t i = 0; i < num; i++) {
-        std::string ee_name, ee_target, ee_base;
-        coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
-        coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
-        coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
-        ABCIKparam tp;
-        for (size_t j = 0; j < 3; j++) {
-            coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
-        }
-        double tmpv[4];
-        for (int j = 0; j < 4; j++ ) {
-          coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
-        }
-        tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
-        tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
-        // Fix for toe joint
-        //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
-        //   Without toe joints, "end-effector link == force-sensor link" is assumed.
-        //   With toe joints, "end-effector link != force-sensor link" is assumed.
-        if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
-            std::vector<double> optw(tp.manip->numJoints(), 1.0);
-            optw.back() = 0.0; // Set weight = 0 for toe joint by default
-            tp.manip->setOptionalWeightVector(optw);
-            tp.has_toe_joint = true;
-        } else {
-            tp.has_toe_joint = false;
-        }
-        tp.avoid_gain = 0.001;
-        tp.reference_gain = 0.01;
-        tp.pos_ik_error_count = tp.rot_ik_error_count = 0;
-        ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
-        ikp[ee_name].target_link = m_robot->link(ee_target);
-        ee_vec.push_back(ee_name);
-        std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   target = " << ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
-        contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
-      }
-      m_contactStates.data.length(num);
-      if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
-        m_contactStates.data[contact_states_index_map["rleg"]] = true;
-        m_contactStates.data[contact_states_index_map["lleg"]] = true;
-      }
-      if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) {
-        m_contactStates.data[contact_states_index_map["rarm"]] = false;
-        m_contactStates.data[contact_states_index_map["larm"]] = false;
-      }
-      m_controlSwingSupportTime.data.length(num);
-      for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 0.0;
-    }
-    std::vector<hrp::Vector3> leg_pos;
-    if (leg_offset_str.size() > 0) {
-      hrp::Vector3 leg_offset;
-      for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
-      std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-      leg_pos.push_back(hrp::Vector3(-1*leg_offset));
-      leg_pos.push_back(hrp::Vector3(leg_offset));
-    }
-    if (leg_pos.size() < ikp.size()) {
-        size_t tmp_leg_pos_size = leg_pos.size();
-        for (size_t i = 0; i < ikp.size() - tmp_leg_pos_size; i++) {
-            leg_pos.push_back(hrp::Vector3::Zero());
-        }
-    }
-
-    std::vector<std::pair<hrp::Link*, hrp::Link*> > interlocking_joints;
-    readInterlockingJointsParamFromProperties(interlocking_joints, m_robot, prop["interlocking_joints"], std::string(m_profile.instance_name));
-    if (interlocking_joints.size() > 0) {
-        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            std::cerr << "[" << m_profile.instance_name << "] Interlocking Joints for [" << it->first << "]" << std::endl;
-            it->second.manip->setInterlockingJointPairIndices(interlocking_joints, std::string(m_profile.instance_name));
-        }
-    }
-
-    zmp_offset_interpolator = new interpolator(ikp.size()*3, m_dt);
-    zmp_offset_interpolator->setName(std::string(m_profile.instance_name)+" zmp_offset_interpolator");
-    zmp_transition_time = 1.0;
-    transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-    transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator");
-    transition_interpolator_ratio = 1.0;
-    adjust_footstep_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-    adjust_footstep_interpolator->setName(std::string(m_profile.instance_name)+" adjust_footstep_interpolator");
-    transition_time = 2.0;
-    adjust_footstep_transition_time = 2.0;
-    leg_names_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-    leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator");
-    leg_names_interpolator_ratio = 1.0;
-
-    // setting stride limitations from conf file
-    double stride_fwd_x_limit = 0.15;
-    double stride_y_limit = 0.05;
-    double stride_th_limit = 10;
-    double stride_bwd_x_limit = 0.05;
-    std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_y_limit << "[m], " << stride_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl;
-    if (default_zmp_offsets.size() == 0) {
-      for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
-    }
-    if (leg_offset_str.size() > 0) {
-      gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_y_limit/*[m]*/, stride_th_limit/*[deg]*/, stride_bwd_x_limit/*[m]*/));
-      gg->set_default_zmp_offsets(default_zmp_offsets);
-    }
-    gg_is_walking = gg_solved = false;
-    m_walkingStates.data = false;
-    fix_leg_coords = coordinates();
-
-    // load virtual force sensors
-    readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
-    // ref force port
-    int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
-    int nvforce = m_vfs.size();
-    int nforce  = npforce + nvforce;
-    m_ref_force.resize(nforce);
-    m_ref_forceIn.resize(nforce);
-    m_force.resize(nforce);
-    m_ref_forceOut.resize(nforce);
-    m_limbCOPOffset.resize(nforce);
-    m_limbCOPOffsetOut.resize(nforce);
-    for (unsigned int i=0; i<npforce; i++){
-        sensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
-    }
-    for (unsigned int i=0; i<nvforce; i++){
-        for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
-            if (it->second.id == i) sensor_names.push_back(it->first);
-        }
-    }
-    // set ref force port
-    std::cerr << "[" << m_profile.instance_name << "] force sensor ports (" << nforce << ")" << std::endl;
-    for (unsigned int i=0; i<nforce; i++){
-        m_ref_forceIn[i] = new InPort<TimedDoubleSeq>(std::string("ref_"+sensor_names[i]).c_str(), m_ref_force[i]);
-        m_ref_force[i].data.length(6);
-        registerInPort(std::string("ref_"+sensor_names[i]).c_str(), *m_ref_forceIn[i]);
-        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string("ref_"+sensor_names[i]) << std::endl;
-        ref_forces.push_back(hrp::Vector3(0,0,0));
-    }
-    // set force port
-    for (unsigned int i=0; i<nforce; i++){
-        m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string(sensor_names[i]).c_str(), m_force[i]);
-        m_force[i].data.length(6);
-        m_force[i].data[0] = m_force[i].data[1] = m_force[i].data[2] = 0.0;
-        m_force[i].data[3] = m_force[i].data[4] = m_force[i].data[5] = 0.0;
-        registerOutPort(std::string(sensor_names[i]).c_str(), *m_ref_forceOut[i]);
-        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string(sensor_names[i]) << std::endl;
-    }
-    // set limb cop offset port
-    std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << nforce << ")" << std::endl;
-    for (unsigned int i=0; i<nforce; i++){
-        std::string nm("limbCOPOffset_"+sensor_names[i]);
-        m_limbCOPOffsetOut[i] = new OutPort<TimedPoint3D>(nm.c_str(), m_limbCOPOffset[i]);
-        registerOutPort(nm.c_str(), *m_limbCOPOffsetOut[i]);
-        m_limbCOPOffset[i].data.x = m_limbCOPOffset[i].data.y = m_limbCOPOffset[i].data.z = 0.0;
-        std::cerr << "[" << m_profile.instance_name << "]   name = " << nm << std::endl;
-    }
-    sbp_offset = hrp::Vector3(0,0,0);
-    sbp_cog_offset = hrp::Vector3(0,0,0);
-    //use_force = MODE_NO_FORCE;
-    use_force = MODE_REF_FORCE;
-
-    if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
-      is_legged_robot = true;
-    } else {
-      is_legged_robot = false;
-    }
-
-    m_accRef.data.ax = m_accRef.data.ay = m_accRef.data.az = 0.0;
-    prev_imu_sensor_vel = hrp::Vector3::Zero();
-
-    graspless_manip_mode = false;
-    graspless_manip_arm = "arms";
-    graspless_manip_p_gain = hrp::Vector3::Zero();
-
-    is_stop_mode = false;
-    has_ik_failed = false;
-    is_hand_fix_mode = false;
-
-    pos_ik_thre = 0.1*1e-3; // [m]
-    rot_ik_thre = (1e-2)*M_PI/180.0; // [rad]
-    ik_error_debug_print_freq = static_cast<int>(0.2/m_dt); // once per 0.2 [s]
-
-    //for HumanSynchronizer
-    hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer());
-
-    return RTC::RTC_OK;
-}
-
-
-RTC::ReturnCode_t AutoBalancer::onFinalize()
-{
-  delete zmp_offset_interpolator;
-  delete transition_interpolator;
-  delete adjust_footstep_interpolator;
-  delete leg_names_interpolator;
-  return RTC::RTC_OK;
-}
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onStartup(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onShutdown(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id)
-{
-    std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
-    
-    return RTC::RTC_OK;
-}
-
-RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
-{
-  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
-  Guard guard(m_mutex);
-  if (control_mode == MODE_ABC) {
-    control_mode = MODE_SYNC_TO_IDLE;
-    double tmp_ratio = 0.0;
-    transition_interpolator->go(&tmp_ratio, m_dt, true); // sync in one controller loop
-  }
-  return RTC::RTC_OK;
-}
-
-
-
-
-#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
-//#define DEBUGP2 ((loop%200==0))
-#define DEBUGP2 (false)
-RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
-{
-  // std::cerr << "AutoBalancer::onExecute(" << ec_id << ")" << std::endl;
-    loop ++;
-    if (m_qRefIn.isNew()) {
-        m_qRefIn.read();
-    }
-    if (m_basePosIn.isNew()) {
-      m_basePosIn.read();
-      input_basePos(0) = m_basePos.data.x;
-      input_basePos(1) = m_basePos.data.y;
-      input_basePos(2) = m_basePos.data.z;
-    }
-    if (m_baseRpyIn.isNew()) {
-      m_baseRpyIn.read();
-      input_baseRot = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
-    }
-    if (m_zmpIn.isNew()) {
-      m_zmpIn.read();
-      input_zmp(0) = m_zmp.data.x;
-      input_zmp(1) = m_zmp.data.y;
-      input_zmp(2) = m_zmp.data.z;
-    }
-    for (unsigned int i=0; i<m_ref_forceIn.size(); i++){
-        if ( m_ref_forceIn[i]->isNew() ) {
-            m_ref_forceIn[i]->read();
-        }
-    }
-    if (m_optionalDataIn.isNew()) {
-        m_optionalDataIn.read();
-        if (is_legged_robot) {
-          if (m_optionalData.data.length() >= contact_states_index_map.size()*2) {
-            // current optionalData is contactstates x limb and controlSwingSupportTime x limb
-            //   If contactStates in optionalData is 1.0, m_contactStates is true. Otherwise, false.
-            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-                m_contactStates.data[contact_states_index_map[it->first]] = isOptionalDataContact(it->first);
-                m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = m_optionalData.data[contact_states_index_map[it->first]+contact_states_index_map.size()];
-            }
-            if ( !m_contactStates.data[contact_states_index_map["rleg"]] && !m_contactStates.data[contact_states_index_map["lleg"]] ) { // If two feet have no contact, force set double support contact
-              m_contactStates.data[contact_states_index_map["rleg"]] = true;
-              m_contactStates.data[contact_states_index_map["lleg"]] = true;
-            }
-          }
-        }
-    }
-    if (m_emergencySignalIn.isNew()){
-        m_emergencySignalIn.read();
-        // if (!is_stop_mode) {
-        //     std::cerr << "[" << m_profile.instance_name << "] emergencySignal is set!" << std::endl;
-        //     is_stop_mode = true;
-        //     gg->emergency_stop();
-        // }
-    }
-    //for HumanSynchronizer
-    if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
-    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); hp_raw_data.rfw = HumanSynchronizer::DoubleSeqToWrench6(m_htrfw); }
-    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); hp_raw_data.lfw = HumanSynchronizer::DoubleSeqToWrench6(m_htlfw); }
-    if (m_htcomIn.isNew()){ m_htcomIn.read(); hp_raw_data.com = HumanSynchronizer::Point3DToVector3(m_htcom); }
-    if (m_htrfIn.isNew()) { m_htrfIn.read();  hp_raw_data.rf  = HumanSynchronizer::Point3DToVector3(m_htrf); }
-    if (m_htlfIn.isNew()) { m_htlfIn.read();  hp_raw_data.lf  = HumanSynchronizer::Point3DToVector3(m_htlf); }
-    if (m_htrhIn.isNew()) { m_htrhIn.read();  hp_raw_data.rh  = HumanSynchronizer::Point3DToVector3(m_htrh); }
-    if (m_htlhIn.isNew()) { m_htlhIn.read();  hp_raw_data.lh  = HumanSynchronizer::Point3DToVector3(m_htlh); }
-    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
-    hsp->readInput(hp_raw_data);
-
-    Guard guard(m_mutex);
-    hrp::Vector3 ref_basePos;
-    hrp::Matrix33 ref_baseRot;
-    hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
-    if ( is_legged_robot ) {
-      gg->proc_zmp_weight_map_interpolation();
-      getCurrentParameters();
-      getTargetParameters();
-      bool is_transition_interpolator_empty = transition_interpolator->isEmpty();
-      if (!is_transition_interpolator_empty) {
-        transition_interpolator->get(&transition_interpolator_ratio, true);
-      } else {
-        transition_interpolator_ratio = 1.0;
-      }
-      if (control_mode != MODE_IDLE ) {
-        solveLimbIK();
-        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
-      } else {
-        rel_ref_zmp = input_zmp;
-      }
-      // transition
-      if (!is_transition_interpolator_empty) {
-        // transition_interpolator_ratio 0=>1 : IDLE => ABC
-        // transition_interpolator_ratio 1=>0 : ABC => IDLE
-        ref_basePos = (1-transition_interpolator_ratio) * input_basePos + transition_interpolator_ratio * m_robot->rootLink()->p;
-        rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp;
-        rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R);
-        for ( int i = 0; i < m_robot->numJoints(); i++ ) {
-          m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q;
-        }
-      } else {
-        ref_basePos = m_robot->rootLink()->p;
-        ref_baseRot = m_robot->rootLink()->R;
-      }
-      // mode change for sync
-      if (control_mode == MODE_SYNC_TO_ABC) {
-        control_mode = MODE_ABC;
-      } else if (control_mode == MODE_SYNC_TO_IDLE && transition_interpolator->isEmpty() ) {
-        std::cerr << "[" << m_profile.instance_name << "] Finished cleanup" << std::endl;
-        control_mode = MODE_IDLE;
-      }
-    }
-    if ( m_qRef.data.length() != 0 ) { // initialized
-      if (is_legged_robot) {
-        for ( int i = 0; i < m_robot->numJoints(); i++ ){
-          m_qRef.data[i] = m_robot->joint(i)->q;
-        }
-      }
-      m_qOut.write();
-    }
-    if (is_legged_robot) {
-      // basePos
-      m_basePos.data.x = ref_basePos(0);
-      m_basePos.data.y = ref_basePos(1);
-      m_basePos.data.z = ref_basePos(2);
-      m_basePos.tm = m_qRef.tm;
-      // baseRpy
-      hrp::Vector3 baseRpy = hrp::rpyFromRot(ref_baseRot);
-      m_baseRpy.data.r = baseRpy(0);
-      m_baseRpy.data.p = baseRpy(1);
-      m_baseRpy.data.y = baseRpy(2);
-      m_baseRpy.tm = m_qRef.tm;
-      // baseTform
-      double *tform_arr = m_baseTform.data.get_buffer();
-      tform_arr[0] = m_basePos.data.x;
-      tform_arr[1] = m_basePos.data.y;
-      tform_arr[2] = m_basePos.data.z;
-      hrp::setMatrix33ToRowMajorArray(ref_baseRot, tform_arr, 3);
-      m_baseTform.tm = m_qRef.tm;
-      // basePose
-      m_basePose.data.position.x = m_basePos.data.x;
-      m_basePose.data.position.y = m_basePos.data.y;
-      m_basePose.data.position.z = m_basePos.data.z;
-      m_basePose.data.orientation.r = m_baseRpy.data.r;
-      m_basePose.data.orientation.p = m_baseRpy.data.p;
-      m_basePose.data.orientation.y = m_baseRpy.data.y;
-      m_basePose.tm = m_qRef.tm;
-      // zmp
-      m_zmp.data.x = rel_ref_zmp(0);
-      m_zmp.data.y = rel_ref_zmp(1);
-      m_zmp.data.z = rel_ref_zmp(2);
-      m_zmp.tm = m_qRef.tm;
-      // cog
-      m_cog.data.x = ref_cog(0);
-      m_cog.data.y = ref_cog(1);
-      m_cog.data.z = ref_cog(2);
-      m_cog.tm = m_qRef.tm;
-      // sbpCogOffset
-      m_sbpCogOffset.data.x = sbp_cog_offset(0);
-      m_sbpCogOffset.data.y = sbp_cog_offset(1);
-      m_sbpCogOffset.data.z = sbp_cog_offset(2);
-      m_sbpCogOffset.tm = m_qRef.tm;
-    }
-    m_basePosOut.write();
-    m_baseRpyOut.write();
-    m_baseTformOut.write();
-    m_basePoseOut.write();
-    m_zmpOut.write();
-    m_cogOut.write();
-    m_sbpCogOffsetOut.write();
-
-    // reference acceleration
-    hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
-    if (sen != NULL) {
-      hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos;
-      hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt;
-      // convert to imu sensor local acceleration
-      hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt;
-      m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2);
-      m_accRefOut.write();
-      prev_imu_sensor_pos = imu_sensor_pos;
-      prev_imu_sensor_vel = imu_sensor_vel;
-    }
-
-    // control parameters
-    m_contactStates.tm = m_qRef.tm;
-    m_contactStatesOut.write();
-    m_controlSwingSupportTime.tm = m_qRef.tm;
-    m_controlSwingSupportTimeOut.write();
-    m_walkingStates.data = gg_is_walking;
-    m_walkingStates.tm = m_qRef.tm;
-    m_walkingStatesOut.write();
-
-    for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
-        m_force[i].tm = m_qRef.tm;
-        m_ref_forceOut[i]->write();
-    }
-
-    for (unsigned int i=0; i<m_limbCOPOffsetOut.size(); i++){
-        m_limbCOPOffset[i].tm = m_qRef.tm;
-        m_limbCOPOffsetOut[i]->write();
-    }
-
-    return RTC::RTC_OK;
-}
-
-void AutoBalancer::getCurrentParameters()
-{
-  current_root_p = m_robot->rootLink()->p;
-  current_root_R = m_robot->rootLink()->R;
-  for ( int i = 0; i < m_robot->numJoints(); i++ ){
-    qorg[i] = m_robot->joint(i)->q;
-  }
-}
-
-void AutoBalancer::getTargetParameters()
-{
-  // joint angles
-  for ( int i = 0; i < m_robot->numJoints(); i++ ){
-    m_robot->joint(i)->q = m_qRef.data[i];
-    qrefv[i] = m_robot->joint(i)->q;
-  }
-  // basepos, rot, zmp
-  m_robot->rootLink()->p = input_basePos;
-  m_robot->rootLink()->R = input_baseRot;
-  m_robot->calcForwardKinematics();
-  //
-  if (control_mode != MODE_IDLE) {
-    coordinates tmp_fix_coords;
-    if (!zmp_offset_interpolator->isEmpty()) {
-      double *default_zmp_offsets_output = new double[ikp.size()*3];
-      zmp_offset_interpolator->get(default_zmp_offsets_output, true);
-      for (size_t i = 0; i < ikp.size(); i++)
-        for (size_t j = 0; j < 3; j++)
-          default_zmp_offsets[i](j) = default_zmp_offsets_output[i*3+j];
-      delete[] default_zmp_offsets_output;
-      if (DEBUGP) {
-        std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl;
-        std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-        for (size_t i = 0; i < leg_names.size(); i++) {
-            std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
-            std::cerr << "[" << m_profile.instance_name << "]   " << leg_names[i] << " = " << default_zmp_offsets[dst->first].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-        }
-      }
-    }
-    if (!leg_names_interpolator->isEmpty()) {
-        leg_names_interpolator->get(&leg_names_interpolator_ratio, true);
-    }else {
-        leg_names_interpolator_ratio = 1.0;
-    }
-    if ( gg_is_walking ) {
-      gg->set_default_zmp_offsets(default_zmp_offsets);
-      gg_solved = gg->proc_one_tick();
-      {
-          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-          coordinates tmpc;
-          // for support leg
-          for (std::vector<step_node>::const_iterator it = gg->get_support_leg_steps().begin(); it != gg->get_support_leg_steps().end(); it++) {
-              coordinates sp_coords = it->worldcoords;
-              coordinates(ikp[leg_type_map[it->l_r]].localPos,
-                          ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc);
-              sp_coords.transform(tmpc);
-              ikp[leg_type_map[it->l_r]].target_p0 = sp_coords.pos;
-              ikp[leg_type_map[it->l_r]].target_r0 = sp_coords.rot;
-          }
-          // for swing leg
-          for (std::vector<step_node>::const_iterator it = gg->get_swing_leg_steps().begin(); it != gg->get_swing_leg_steps().end(); it++) {
-              coordinates sw_coords = it->worldcoords;
-              coordinates(ikp[leg_type_map[it->l_r]].localPos,
-                          ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc);
-              sw_coords.transform(tmpc);
-              ikp[leg_type_map[it->l_r]].target_p0 = sw_coords.pos;
-              ikp[leg_type_map[it->l_r]].target_r0 = sw_coords.rot;
-          }
-      }
-      gg->get_swing_support_mid_coords(tmp_fix_coords);
-      // set contactStates
-      {
-          std::vector<std::string> tmp_current_support_states_names;
-          {
-              std::vector<leg_type> tmp_current_support_states = gg->get_current_support_states();
-              std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-              for (std::vector<leg_type>::const_iterator it = tmp_current_support_states.begin(); it != tmp_current_support_states.end(); it++)
-                  tmp_current_support_states_names.push_back(leg_type_map[*it]);
-          }
-          // Set Contact States for ee not included in leg_names to false
-          for ( std::map<std::string, size_t>::iterator it = contact_states_index_map.begin(); it != contact_states_index_map.end(); it++ ) {
-              m_contactStates.data[it->second] = false;
-          }
-          for (std::vector<std::string>::const_iterator it = leg_names.begin(); it != leg_names.end(); it++) {
-              std::vector<std::string>::const_iterator dst = std::find_if(tmp_current_support_states_names.begin(), tmp_current_support_states_names.end(), boost::lambda::_1 == *it);
-              if (dst != tmp_current_support_states_names.end()) {
-                  m_contactStates.data[contact_states_index_map[*it]] = true;
-              } else {
-                  m_contactStates.data[contact_states_index_map[*it]] = false;
-              }
-          }
-      }
-      // set controlSwingSupportTime
-      {
-          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-          for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
-              std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
-              m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = gg->get_current_swing_time(dst->first);
-          }
-      }
-      // set ref_forces
-      {
-          std::vector<hrp::Vector3> ee_pos;
-          for (size_t i = 0 ; i < leg_names.size(); i++) {
-              ABCIKparam& tmpikp = ikp[leg_names[i]];
-              ee_pos.push_back(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]);
-          }
-          double alpha = (ref_zmp - ee_pos[1]).norm() / (ee_pos[0] - ee_pos[1]).norm();
-          if (alpha>1.0) alpha = 1.0;
-          if (alpha<0.0) alpha = 0.0;
-          if (DEBUGP) {
-          std::cerr << "[" << m_profile.instance_name << "] alpha:" << alpha << std::endl;
-          }
-          double mg = m_robot->totalMass() * gg->get_gravitational_acceleration();
-          m_force[0].data[0] = alpha * mg;
-          m_force[1].data[0] = (1-alpha) * mg;
-      }
-      // set limbCOPOffset
-      {
-          std::vector<std::string> swg_leg_nms = gg->get_swing_leg_names();
-          for (size_t i = 0; i < swg_leg_nms.size(); i++) {
-              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.x = gg->get_swing_foot_zmp_offsets().at(i)(0);
-              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.y = gg->get_swing_foot_zmp_offsets().at(i)(1);
-              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.z = gg->get_swing_foot_zmp_offsets().at(i)(2);
-          }
-      }
-      {
-          std::vector<std::string> sup_leg_nms = gg->get_support_leg_names();
-          for (size_t i = 0; i < sup_leg_nms.size(); i++) {
-              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.x = gg->get_support_foot_zmp_offsets().at(i)(0);
-              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.y = gg->get_support_foot_zmp_offsets().at(i)(1);
-              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.z = gg->get_support_foot_zmp_offsets().at(i)(2);
-          }
-      }
-    } else {
-      tmp_fix_coords = fix_leg_coords;
-      // double support by default
-      {
-          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-          for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
-              std::vector<std::string>::const_iterator dst = std::find_if(leg_names.begin(), leg_names.end(), (boost::lambda::_1 == it->first));
-              if (dst != leg_names.end()) {
-                  m_contactStates.data[contact_states_index_map[it->first]] = true;
-              } else {
-                  m_contactStates.data[contact_states_index_map[it->first]] = false;
-              }
-              // controlSwingSupportTime is not used while double support period, 1.0 is neglected
-              m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = 1.0;
-              std::map<leg_type, std::string>::const_iterator dst2 = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
-              m_limbCOPOffset[contact_states_index_map[it->first]].data.x = default_zmp_offsets.at(dst2->first)(0);
-              m_limbCOPOffset[contact_states_index_map[it->first]].data.y = default_zmp_offsets.at(dst2->first)(1);
-              m_limbCOPOffset[contact_states_index_map[it->first]].data.z = default_zmp_offsets.at(dst2->first)(2);
-          }
-      }
-    }
-    if (!adjust_footstep_interpolator->isEmpty()) {
-        double tmp = 0.0;
-        adjust_footstep_interpolator->get(&tmp, true);
-        //std::cerr << "[" << m_profile.instance_name << "] adjust ratio " << tmp << std::endl;
-        ikp["rleg"].target_p0 = (1-tmp) * ikp["rleg"].adjust_interpolation_org_p0 + tmp*ikp["rleg"].adjust_interpolation_target_p0;
-        ikp["lleg"].target_p0 = (1-tmp) * ikp["lleg"].adjust_interpolation_org_p0 + tmp*ikp["lleg"].adjust_interpolation_target_p0;
-        rats::mid_rot(ikp["rleg"].target_r0, tmp, ikp["rleg"].adjust_interpolation_org_r0, ikp["rleg"].adjust_interpolation_target_r0);
-        rats::mid_rot(ikp["lleg"].target_r0, tmp, ikp["lleg"].adjust_interpolation_org_r0, ikp["lleg"].adjust_interpolation_target_r0);
-        coordinates tmprc, tmplc;
-        tmprc.pos = ikp["rleg"].target_p0 + ikp["rleg"].target_r0 * ikp["rleg"].localPos;
-        tmprc.rot = ikp["rleg"].target_r0 * ikp["rleg"].localR;
-        tmplc.pos = ikp["lleg"].target_p0 + ikp["lleg"].target_r0 * ikp["lleg"].localPos;
-        tmplc.rot = ikp["lleg"].target_r0 * ikp["lleg"].localR;
-        rats::mid_coords(fix_leg_coords, 0.5, tmprc, tmplc);
-        tmp_fix_coords = fix_leg_coords;
-    }
-    // Tempolarily modify tmp_fix_coords
-    // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272
-    {
-      hrp::Vector3 ex = hrp::Vector3::UnitX();
-      hrp::Vector3 ez = hrp::Vector3::UnitZ();
-      hrp::Vector3 xv1 (tmp_fix_coords.rot * ex);
-      xv1(2) = 0.0;
-      xv1.normalize();
-      hrp::Vector3 yv1(ez.cross(xv1));
-      tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2);
-      tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2);
-      tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2);
-    }
-    // Fix pos
-    fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot);
-
-    /* update ref_forces ;; sp's absolute -> rmc's absolute */
-    for (size_t i = 0; i < m_ref_forceIn.size(); i++) {
-      hrp::Matrix33 eeR;
-      hrp::Link* parentlink;
-      hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
-      if (sensor) parentlink = sensor->link;
-      else parentlink = m_vfs[sensor_names[i]].link;
-      for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-          if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR;
-      }
-      // End effector frame
-      //ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
-      // world frame
-      ref_forces[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
-    }
-    sbp_offset = tmp_fix_coords.rot * hrp::Vector3(sbp_offset);
-
-    target_root_p = m_robot->rootLink()->p;
-    target_root_R = m_robot->rootLink()->R;
-    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-      if ( control_mode == MODE_IDLE || std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end() ) {
-        it->second.target_p0 = it->second.target_link->p;
-        it->second.target_r0 = it->second.target_link->R;
-      }
-    }
-    // Just for ik initial value
-    if (control_mode == MODE_SYNC_TO_ABC) {
-        current_root_p = target_root_p;
-        current_root_R = target_root_R;
-        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
-                it->second.target_p0 = it->second.target_link->p;
-                it->second.target_r0 = it->second.target_link->R;
-            }
-        }
-    }
-    // Move hand for hand fix mode
-    //   If arms' ABCIKparam.is_active is true, move hands according to cog velocity.
-    //   If is_hand_fix_mode is false, no hand fix mode and move hands according to cog velocity.
-    //   If is_hand_fix_mode is true, hand fix mode and do not move hands in Y axis in tmp_fix_coords.rot.    
-    if (gg_is_walking) {
-        // hand control while walking = solve hand ik with is_hand_fix_mode and solve hand ik without is_hand_fix_mode
-        bool is_hand_control_while_walking = false;
-        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
-                 && it->first.find("arm") != std::string::npos ) {
-                is_hand_control_while_walking = true;
-            }
-        }
-        if (is_hand_control_while_walking) {
-        //if (false) { // Disabled temporarily
-            // Store hand_fix_initial_offset in the initialization of walking
-            if (is_hand_fix_initial) hand_fix_initial_offset = tmp_fix_coords.rot.transpose() * (hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos);
-            is_hand_fix_initial = false;
-            hrp::Vector3 dif_p = hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos - tmp_fix_coords.rot * hand_fix_initial_offset;
-            if (is_hand_fix_mode) {
-                dif_p = tmp_fix_coords.rot.transpose() * dif_p;
-                dif_p(1) = 0;
-                dif_p = tmp_fix_coords.rot * dif_p;
-            }
-            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-                if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
-                     && it->first.find("arm") != std::string::npos ) {
-                    it->second.target_p0 = it->second.target_p0 + dif_p;
-                }
-            }
-        }
-    }
-
-    hrp::Vector3 tmp_foot_mid_pos(hrp::Vector3::Zero());
-    {
-        std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-        std::map<leg_type, double> zmp_weight_map = gg->get_zmp_weight_map();
-        double sum_of_weight = 0.0;
-        for (size_t i = 0; i < leg_names.size(); i++) {
-            ABCIKparam& tmpikp = ikp[leg_names[i]];
-            // get target_end_coords
-            tmpikp.target_end_coords.pos = tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos;
-            tmpikp.target_end_coords.rot = tmpikp.target_r0 * tmpikp.localR;
-            // for foot_mid_pos
-            std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
-            tmp_foot_mid_pos += (tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]) * zmp_weight_map[dst->first];
-            sum_of_weight += zmp_weight_map[dst->first];
-        }
-        tmp_foot_mid_pos *= (1.0 / sum_of_weight);
-    }
-    //
-    {
-        if ( gg_is_walking && gg->get_lcg_count() == gg->get_overwrite_check_timing()+2 ) {
-            hrp::Vector3 vel_htc(calc_vel_from_hand_error(tmp_fix_coords));
-            gg->set_offset_velocity_param(vel_htc(0), vel_htc(1) ,vel_htc(2));
-        }//  else {
-        //     if ( gg_is_walking && gg->get_lcg_count() == static_cast<size_t>(gg->get_default_step_time()/(2*m_dt))-1) {
-        //         gg->set_offset_velocity_param(0,0,0);
-        //     }
-        // }
-    }
-
-
-    hrp::Vector3 tmp_ref_cog(m_robot->calcCM());
-    if (gg_is_walking) {
-      ref_cog = gg->get_cog();
-    } else {
-      ref_cog = tmp_foot_mid_pos;
-    }
-    ref_cog(2) = tmp_ref_cog(2);
-    if (gg_is_walking) {
-      ref_zmp = gg->get_refzmp();
-    } else {
-      ref_zmp(0) = ref_cog(0);
-      ref_zmp(1) = ref_cog(1);
-      ref_zmp(2) = tmp_foot_mid_pos(2);
-    }
-  }
-};
-
-hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2)
-{
-  hrp::Vector3 vv = axis1.cross(axis2);
-  if (fabs(vv.norm()-0.0) < 1e-5) {
-    return rot;
-  } else {
-    Eigen::AngleAxis<double> tmpr(std::asin(vv.norm()), vv.normalized());
-    return tmpr.toRotationMatrix() * rot;
-  }
-}
-
-void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot)
-{
-  // get current foot mid pos + rot
-  std::vector<coordinates> foot_coords;
-  for (size_t i = 0; i < leg_names.size(); i++) {
-      ABCIKparam& tmpikp = ikp[leg_names[i]];
-      if (leg_names[i].find("leg") != std::string::npos) foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
-                                                                                           (tmpikp.target_link->R * tmpikp.localR)));
-  }
-  coordinates current_foot_mid_coords;
-  multi_mid_coords(current_foot_mid_coords, foot_coords);
-  hrp::Vector3 current_foot_mid_pos = current_foot_mid_coords.pos;
-  hrp::Matrix33 current_foot_mid_rot = current_foot_mid_coords.rot;
-  // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
-  hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose());
-  m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
-  rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
-  m_robot->calcForwardKinematics();
-}
-
-
-bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
-{
-  param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
-  // IK check
-  hrp::Vector3 vel_p, vel_r;
-  vel_p = param.target_p0 - param.target_link->p;
-  rats::difference_rotation(vel_r, param.target_link->R, param.target_r0);
-  if (vel_p.norm() > pos_ik_thre && transition_interpolator->isEmpty()) {
-      if (param.pos_ik_error_count % ik_error_debug_print_freq == 0) {
-          std::cerr << "[" << m_profile.instance_name << "] Too large IK error (vel_p) = [" << vel_p(0) << " " << vel_p(1) << " " << vel_p(2) << "][m], count = " << param.pos_ik_error_count << std::endl;
-      }
-      param.pos_ik_error_count++;
-      has_ik_failed = true;
-  } else {
-      param.pos_ik_error_count = 0;
-  }
-  if (vel_r.norm() > rot_ik_thre && transition_interpolator->isEmpty()) {
-      if (param.rot_ik_error_count % ik_error_debug_print_freq == 0) {
-          std::cerr << "[" << m_profile.instance_name << "] Too large IK error (vel_r) = [" << vel_r(0) << " " << vel_r(1) << " " << vel_r(2) << "][rad], count = " << param.rot_ik_error_count << std::endl;
-      }
-      param.rot_ik_error_count++;
-      has_ik_failed = true;
-  } else {
-      param.rot_ik_error_count = 0;
-  }
-  return true;
-}
-
-
-
-void AutoBalancer::solveLimbIK ()
-{
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-    if (it->second.is_active) {
-      for ( int j = 0; j < it->second.manip->numJoints(); j++ ){
-	int i = it->second.manip->joint(j)->jointId;
-	m_robot->joint(i)->q = qorg[i];
-      }
-    }
-  }
-  m_robot->rootLink()->p = current_root_p;
-  m_robot->rootLink()->R = current_root_R;
-  m_robot->calcForwardKinematics();
-  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
-  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
-  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
-  dif_cog *= leg_names_interpolator_ratio;
-  dif_cog(2) = m_robot->rootLink()->p(2) - target_root_p(2);
-  m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog;
-  m_robot->rootLink()->R = target_root_R;
-
-
-  //for HumanSynchronizer
-//  static int HumanSyncCountdownNum = 5 * (1/m_dt);
-  if(hsp->startCountdownForHumanSync){
-    std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";//ややCOMのIKに手間取った時プリント
-	  hsp->updateCountDown();
-  }
-  if(hsp->isHumanSyncOn()){
-	  if(hsp->ht_first_call){
-		  hsp->init_wld_rp_basepos = m_robot->rootLink()->p;
-      hsp->setInitOffsetPose();
-      hsp->rp_wld_initpos.com = m_robot->calcCM();
-      hsp->rp_wld_initpos.rf = ikp["rleg"].target_link->p;
-      hsp->rp_wld_initpos.lf = ikp["lleg"].target_link->p;
-      if(ikp.count("rarm"))hsp->rp_wld_initpos.rh = ikp["rarm"].target_link->p;
-      if(ikp.count("larm"))hsp->rp_wld_initpos.lh = ikp["larm"].target_link->p;
-      hsp->ht_first_call = false;
-
-      hsp->calibInitHumanCOMFromZMP();
-
-      std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;//ややCOMのIKに手間取った時プリント
-
-	  }
-  }else{
-    hsp->setInitOffsetPose();
-  }
-
-  hsp->update();//////HumanSynchronizerの主要処理
-  if(loop%100==0)hsp->rp_ref_out.print();
-  if(loop%100==0)cerr<<"getUpdateTime:"<<hsp->getUpdateTime()*1000<<"[ms]"<<endl;
-
-  if(hsp->isHumanSyncOn()){
-	  ////////////////////// 重心拘束位置設定 /////////////////////////
-    m_robot->rootLink()->p = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;//まずは重心目標分，体幹を動かす
-    m_robot->rootLink()->p(2) = hsp->init_wld_rp_basepos(2)+ hsp->rp_ref_out.com(2);//高さだけはベース基準
-    m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
-    m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
-  }
-
-  // Fix for toe joint
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-      if (it->second.is_active && it->second.has_toe_joint && gg->get_use_toe_joint()) {
-          int i = it->second.target_link->jointId;
-          if (gg->get_swing_leg_names().front() == it->first) {
-              m_robot->joint(i)->q = qrefv[i] + -1 * gg->get_foot_dif_rot_angle();
-          } else {
-              m_robot->joint(i)->q = qrefv[i];
-          }
-      }
-  }
-  m_robot->calcForwardKinematics();
-
-
-  // additional COM fitting IK for HumanSynchronizer
-  hrp::Vector3 tmp_com_err = hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM();
-  int com_ik_loop=0;
-  const int COM_IK_MAX_LOOP = 10;
-  const double COM_IK_MAX_ERROR = 0.0001;//0.001だとたまにカクッとなる
-  if(hsp->isHumanSyncOn()){
-    ikp["rleg"].target_p0 = hsp->rp_wld_initpos.rf + hsp->rp_ref_out.rf;
-    ikp["lleg"].target_p0 = hsp->rp_wld_initpos.lf + hsp->rp_ref_out.lf;
-    if(ikp.count("rarm"))ikp["rarm"].target_p0 = hsp->rp_wld_initpos.rh + hsp->rp_ref_out.rh;
-    if(ikp.count("larm"))ikp["larm"].target_p0 = hsp->rp_wld_initpos.lh + hsp->rp_ref_out.lh;
-    while(fabs(tmp_com_err(0))>COM_IK_MAX_ERROR || fabs(tmp_com_err(1))>COM_IK_MAX_ERROR || fabs(tmp_com_err(2))>COM_IK_MAX_ERROR){//Z方向のCOM合わせは要注意(たまに怪しい)
-		  m_robot->rootLink()->p(0) += tmp_com_err(0);
-      m_robot->rootLink()->p(1) += tmp_com_err(1);
-      m_robot->rootLink()->p(2) += tmp_com_err(2);
-		  m_robot->calcForwardKinematics();
-		  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-		    if (it->second.is_active) solveLimbIKforLimb(it->second);
-		  }
-		  m_robot->calcForwardKinematics();
-      tmp_com_err = (hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM());
-		  if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "[" << m_profile.instance_name << "] COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
-	  }
-
-//    static hrp::Vector3 pre_ik_basepos;
-//    for(int i=0;i<3;i++){//重心拘束で解いた結果の体幹軌道の滑らかさは保証されていないので体幹位置に速度制限＆再度IK
-//      if(m_robot->rootLink()->p(i) - pre_ik_basepos(i) > 0.0002){ m_robot->rootLink()->p(i) =  pre_ik_basepos(i) + 0.0002; }
-//      else if(m_robot->rootLink()->p(i) - pre_ik_basepos(i) < -0.0002){ m_robot->rootLink()->p(i) =  pre_ik_basepos(i) - 0.0002; }
-//    }
-//    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-//      if (it->second.is_active) solveLimbIKforLimb(it->second);
-//    }
-//    pre_ik_basepos = m_robot->rootLink()->p;
-
-    //outport用のデータ上書き
-    ref_zmp(0) = hsp->rp_ref_out.zmp(0);
-    ref_zmp(1) = hsp->rp_ref_out.zmp(1);
-    ref_zmp(2) = 0;
-    ref_cog = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;
-
-	  if(loop%100==0)if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
-  }else{
-	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
-	    if (it->second.is_active) solveLimbIKforLimb(it->second);
-	  }
-  }
-  if (gg_is_walking && !gg_solved) stopWalking ();
-}
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onAborting(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onError(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onReset(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onStateUpdate(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onRateChanged(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-bool AutoBalancer::startHumanSyncAfter5sec()
-{
-	std::cerr << "[" << m_profile.instance_name << "] start HumanSync after 5 sec" << std::endl;
-	hsp->startCountdownForHumanSync = true;
-	return true;
-}
-
-bool AutoBalancer::stopHumanSync()
-{
-	std::cerr << "[" << m_profile.instance_name << "] stop HumanSync Now" << std::endl;
-	return true;
-}
-
-void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence& limbs)
-{
-  std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
-  Guard guard(m_mutex);
-  double tmp_ratio = 0.0;
-  transition_interpolator->clear();
-  transition_interpolator->set(&tmp_ratio);
-  tmp_ratio = 1.0;
-  transition_interpolator->go(&tmp_ratio, transition_time, true);
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-    it->second.is_active = false;
-  }
-
-  for (size_t i = 0; i < limbs.length(); i++) {
-    ABCIKparam& tmp = ikp[std::string(limbs[i])];
-    tmp.is_active = true;
-    std::cerr << "[" << m_profile.instance_name << "]   limb [" << std::string(limbs[i]) << "]" << std::endl;
-  }
-
-  control_mode = MODE_SYNC_TO_ABC;
-}
-
-void AutoBalancer::stopABCparam()
-{
-  std::cerr << "[" << m_profile.instance_name << "] stop auto balancer mode" << std::endl;
-  //Guard guard(m_mutex);
-  double tmp_ratio = 1.0;
-  transition_interpolator->clear();
-  transition_interpolator->set(&tmp_ratio);
-  tmp_ratio = 0.0;
-  transition_interpolator->go(&tmp_ratio, transition_time, true);
-  control_mode = MODE_SYNC_TO_IDLE;
-}
-
-void AutoBalancer::startWalking ()
-{
-  if ( control_mode != MODE_ABC ) {
-    return_control_mode = control_mode;
-    OpenHRP::AutoBalancerService::StrSequence fix_limbs;
-    fix_limbs.length(2);
-    fix_limbs[0] = "rleg";
-    fix_limbs[1] = "lleg";
-    startABCparam(fix_limbs);
-    waitABCTransition();
-  }
-  {
-    Guard guard(m_mutex);
-    has_ik_failed = false;
-    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-        it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
-    }
-    std::vector<std::string> init_swing_leg_names(gg->get_footstep_front_leg_names());
-    std::vector<std::string> tmp_all_limbs(leg_names);
-    std::vector<std::string> init_support_leg_names;
-    std::sort(tmp_all_limbs.begin(), tmp_all_limbs.end());
-    std::sort(init_swing_leg_names.begin(), init_swing_leg_names.end());
-    std::set_difference(tmp_all_limbs.begin(), tmp_all_limbs.end(),
-                        init_swing_leg_names.begin(), init_swing_leg_names.end(),
-                        std::back_inserter(init_support_leg_names));
-    std::vector<step_node> init_support_leg_steps, init_swing_leg_dst_steps;
-    for (std::vector<std::string>::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++)
-        init_support_leg_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0));
-    for (std::vector<std::string>::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++)
-        init_swing_leg_dst_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0));
-    gg->set_default_zmp_offsets(default_zmp_offsets);
-    gg->initialize_gait_parameter(ref_cog, init_support_leg_steps, init_swing_leg_dst_steps);
-  }
-  is_hand_fix_initial = true;
-  while ( !gg->proc_one_tick() );
-  {
-    Guard guard(m_mutex);
-    gg_is_walking = gg_solved = true;
-  }
-}
-
-void AutoBalancer::stopWalking ()
-{
-  std::vector<coordinates> tmp_end_coords_list;
-  for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
-      if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(ikp[*it].target_end_coords);
-  }
-  multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
-  fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot);
-  gg->clear_footstep_nodes_list();
-  if (return_control_mode == MODE_IDLE) stopABCparam();
-  gg_is_walking = false;
-}
-
-bool AutoBalancer::startAutoBalancer (const OpenHRP::AutoBalancerService::StrSequence& limbs)
-{
-  if (control_mode == MODE_IDLE) {
-    has_ik_failed = false;
-    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-        it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
-    }
-    startABCparam(limbs);
-    waitABCTransition();
-    return_control_mode = MODE_ABC;
-    return true;
-  } else {
-    return false;
-  }
-}
-
-bool AutoBalancer::stopAutoBalancer ()
-{
-  if (control_mode == MODE_ABC) {
-    stopABCparam();
-    waitABCTransition();
-    return true;
-  } else {
-    return false;
-  }
-}
-
-void AutoBalancer::waitABCTransition()
-{
-  while (!transition_interpolator->isEmpty()) usleep(1000);
-  usleep(1000);
-}
-bool AutoBalancer::goPos(const double& x, const double& y, const double& th)
-{
-    //  if ( !gg_is_walking && !is_stop_mode) {
-  if ( !is_stop_mode) {
-    gg->set_all_limbs(leg_names);
-    coordinates start_ref_coords;
-    std::vector<coordinates> initial_support_legs_coords;
-    std::vector<leg_type> initial_support_legs;
-    bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
-    if (is_valid_gait_type == false) return false;
-    bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th,
-                                                      initial_support_legs_coords, // Dummy if gg_is_walking
-                                                      start_ref_coords,            // Dummy if gg_is_walking
-                                                      initial_support_legs,        // Dummy if gg_is_walking
-                                                      (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps.
-    if ( !gg_is_walking ) { // Initializing
-        startWalking();
-    }
-    if (!ret) {
-        std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl;
-    }
-    return ret;
-  } else {
-    std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl;
-    return false;
-  }
-}
-
-bool AutoBalancer::goVelocity(const double& vx, const double& vy, const double& vth)
-{
-  gg->set_all_limbs(leg_names);
-  if (gg_is_walking && gg_solved) {
-    gg->set_velocity_param(vx, vy, vth);
-  } else {
-    coordinates ref_coords;
-    mid_coords(ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
-    std::vector<leg_type> current_legs;
-    switch(gait_type) {
-    case BIPED:
-        current_legs = (vy > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
-        break;
-    case TROT:
-        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
-        break;
-    case PACE:
-        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
-        break;
-    case CRAWL:
-        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    case GALLOP:
-        /* at least one leg shoud be in contact */
-        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    default: break;
-    }
-    gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs);
-    startWalking();
-  }
-  return true;
-}
-
-bool AutoBalancer::goStop ()
-{
-  gg->finalize_velocity_mode();
-  waitFootSteps();
-  return true;
-}
-
-bool AutoBalancer::emergencyStop ()
-{
-  std::cerr << "[" << m_profile.instance_name << "] emergencyStop" << std::endl;
-  // is_stop_mode = true;
-  gg->emergency_stop();
-  waitFootSteps();
-  return true;
-}
-
-bool AutoBalancer::releaseEmergencyStop ()
-{
-  if (is_stop_mode) {
-      std::cerr << "[" << m_profile.instance_name << "] releaseEmergencyStop" << std::endl;
-      is_stop_mode = false;
-  }
-  return true;
-}
-
-bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
-{
-  OpenHRP::AutoBalancerService::StepParamsSequence spss;
-  spss.length(fss.length());
-  // If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
-  // If gg_is_walking is true, do not set to 0.
-  for (size_t i = 0; i < spss.length(); i++) {
-      spss[i].sps.length(fss[i].fs.length());
-      for (size_t j = 0; j < spss[i].sps.length(); j++) {
-          spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
-          spss[i].sps[j].step_time = gg->get_default_step_time();
-          spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle());
-          spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle());
-      }
-  }
-  setFootStepsWithParam(fss, spss, overwrite_fs_idx);
-}
-
-bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
-{
-    if (!is_stop_mode) {
-        std::cerr << "[" << m_profile.instance_name << "] setFootStepsList" << std::endl;
-
-        // Initial footstep Snapping
-        coordinates tmpfs, fstrans;
-        step_node initial_support_step, initial_input_step;
-        {
-            std::vector<step_node> initial_support_steps;
-            if (gg_is_walking) {
-                if (overwrite_fs_idx <= 0) {
-                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
-                    return false;
-                }
-                if (!gg->get_footstep_nodes_by_index(initial_support_steps, overwrite_fs_idx-1)) {
-                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
-                    return false;
-                }
-            } else {
-                // If walking, snap initial leg to current ABC foot coords.
-                for (size_t i = 0; i < fss[0].fs.length(); i++) {
-                    initial_support_steps.push_back(step_node(std::string(fss[0].fs[i].leg), ikp[std::string(fss[0].fs[i].leg)].target_end_coords, 0, 0, 0, 0));
-                }
-            }
-            initial_support_step = initial_support_steps.front(); /* use only one leg for representation */
-        }
-        {
-            std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-            for (size_t i = 0; i < fss[0].fs.length(); i++) {
-                if (std::string(fss[0].fs[i].leg) == leg_type_map[initial_support_step.l_r]) {
-                    coordinates tmp;
-                    memcpy(tmp.pos.data(), fss[0].fs[i].pos, sizeof(double)*3);
-                    tmp.rot = (Eigen::Quaternion<double>(fss[0].fs[i].rot[0], fss[0].fs[i].rot[1], fss[0].fs[i].rot[2], fss[0].fs[i].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
-                    initial_input_step = step_node(std::string(fss[0].fs[i].leg), tmp, 0, 0, 0, 0);
-                }
-            }
-        }
-
-        // Get footsteps
-        std::vector< std::vector<coordinates> > fs_vec_list;
-        std::vector< std::vector<std::string> > leg_name_vec_list;
-        for (size_t i = 0; i < fss.length(); i++) {
-            std::vector<coordinates> fs_vec;
-            std::vector<std::string> leg_name_vec;
-            for (size_t j = 0; j < fss[i].fs.length(); j++) {
-                std::string leg(fss[i].fs[j].leg);
-                if (std::find(leg_names.begin(), leg_names.end(), leg) != leg_names.end()) {
-                    memcpy(tmpfs.pos.data(), fss[i].fs[j].pos, sizeof(double)*3);
-                    tmpfs.rot = (Eigen::Quaternion<double>(fss[i].fs[j].rot[0], fss[i].fs[j].rot[1], fss[i].fs[j].rot[2], fss[i].fs[j].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
-                    initial_input_step.worldcoords.transformation(fstrans, tmpfs);
-                    tmpfs = initial_support_step.worldcoords;
-                    tmpfs.transform(fstrans);
-                } else {
-                    std::cerr << "[" << m_profile.instance_name << "]   No such target : " << leg << std::endl;
-                    return false;
-                }
-                leg_name_vec.push_back(leg);
-                fs_vec.push_back(tmpfs);
-            }
-            leg_name_vec_list.push_back(leg_name_vec);
-            fs_vec_list.push_back(fs_vec);
-        }
-        if (spss.length() != fs_vec_list.size()) {
-            std::cerr << "[" << m_profile.instance_name << "]   StepParam length " << spss.length () << " != Footstep length " << fs_vec_list.size() << std::endl;
-            return false;
-        }
-        std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl;
-        std::vector< std::vector<step_node> > fnsl;
-        for (size_t i = 0; i < fs_vec_list.size(); i++) {
-            if (!(gg_is_walking && i == 0)) { // If initial footstep, e.g., not walking, pass user-defined footstep list. If walking, pass cdr footsteps in order to neglect initial double support leg.
-                std::vector<step_node> tmp_fns;
-                for (size_t j = 0; j < fs_vec_list.at(i).size(); j++) {
-                    tmp_fns.push_back(step_node(leg_name_vec_list[i][j], fs_vec_list[i][j], spss[i].sps[j].step_height, spss[i].sps[j].step_time, spss[i].sps[j].toe_angle, spss[i].sps[j].heel_angle));
-                }
-                fnsl.push_back(tmp_fns);
-            }
-        }
-        if (gg_is_walking) {
-            std::cerr << "[" << m_profile.instance_name << "]  Set overwrite footsteps" << std::endl;
-            gg->set_overwrite_foot_steps_list(fnsl);
-            gg->set_overwrite_foot_step_index(overwrite_fs_idx);
-        } else {
-            std::cerr << "[" << m_profile.instance_name << "]  Set normal footsteps" << std::endl;
-            gg->set_foot_steps_list(fnsl);
-            startWalking();
-        }
-        return true;
-    } else {
-        std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl;
-        return false;
-    }
-}
-
-void AutoBalancer::waitFootSteps()
-{
-  //while (gg_is_walking) usleep(10);
-  while (gg_is_walking || !transition_interpolator->isEmpty() )
-    usleep(1000);
-  usleep(1000);
-  gg->set_offset_velocity_param(0,0,0);
-}
-
-void AutoBalancer::waitFootStepsEarly(const double tm)
-{
-  if (!gg_is_walking) { return;}
-  while ( !gg->is_finalizing(tm)|| !transition_interpolator->isEmpty() )
-    usleep(1000);
-  usleep(1000);
-  gg->set_offset_velocity_param(0,0,0);
-}
-
-bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
-{
-  std::cerr << "[" << m_profile.instance_name << "] setGaitGeneratorParam" << std::endl;
-  gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]);
-  std::vector<hrp::Vector3> off;
-  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
-      off.push_back(hrp::Vector3(i_param.leg_default_translate_pos[i][0], i_param.leg_default_translate_pos[i][1], i_param.leg_default_translate_pos[i][2]));
-  }
-  gg->set_leg_default_translate_pos(off);
-  gg->set_default_step_time(i_param.default_step_time);
-  gg->set_default_step_height(i_param.default_step_height);
-  gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio/2.0);
-  gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio/2.0);
-  gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio/2.0);
-  gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio/2.0);
-  gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio/2.0);
-  gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio/2.0);
-  // gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio_before);
-  // gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio_after);
-  // gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio_before);
-  // gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio_after);
-  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_before);
-  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_after);
-  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_swing_before);
-  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_swing_after);
-  if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::SHUFFLING) {
-    gg->set_default_orbit_type(SHUFFLING);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOID) {
-    gg->set_default_orbit_type(CYCLOID);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::RECTANGLE) {
-    gg->set_default_orbit_type(RECTANGLE);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::STAIR) {
-    gg->set_default_orbit_type(STAIR);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAY) {
-    gg->set_default_orbit_type(CYCLOIDDELAY);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK) {
-    gg->set_default_orbit_type(CYCLOIDDELAYKICK);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CROSS) {
-    gg->set_default_orbit_type(CROSS);
-  }
-  gg->set_swing_trajectory_delay_time_offset(i_param.swing_trajectory_delay_time_offset);
-  gg->set_swing_trajectory_final_distance_weight(i_param.swing_trajectory_final_distance_weight);
-  gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2]));
-  gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2]));  
-  gg->set_gravitational_acceleration(i_param.gravitational_acceleration);
-  gg->set_toe_angle(i_param.toe_angle);
-  gg->set_heel_angle(i_param.heel_angle);
-  gg->set_toe_pos_offset_x(i_param.toe_pos_offset_x);
-  gg->set_heel_pos_offset_x(i_param.heel_pos_offset_x);
-  gg->set_toe_zmp_offset_x(i_param.toe_zmp_offset_x);
-  gg->set_heel_zmp_offset_x(i_param.heel_zmp_offset_x);
-  std::vector<double> tmp_ratio(i_param.toe_heel_phase_ratio.get_buffer(), i_param.toe_heel_phase_ratio.get_buffer()+i_param.toe_heel_phase_ratio.length());
-  std::cerr << "[" << m_profile.instance_name << "]   "; // for set_toe_heel_phase_ratio
-  gg->set_toe_heel_phase_ratio(tmp_ratio);
-  gg->set_use_toe_joint(i_param.use_toe_joint);
-  gg->set_use_toe_heel_transition(i_param.use_toe_heel_transition);
-  gg->set_zmp_weight_map(boost::assign::map_list_of<leg_type, double>(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3]));
-  gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num);
-  gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset);
-
-  // print
-  gg->print_param(std::string(m_profile.instance_name));
-  return true;
-};
-
-bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
-{
-  gg->get_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]);
-  std::vector<hrp::Vector3> off;
-  gg->get_leg_default_translate_pos(off);
-  i_param.leg_default_translate_pos.length(off.size());
-  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
-      i_param.leg_default_translate_pos[i].length(3);
-      i_param.leg_default_translate_pos[i][0] = off[i](0);
-      i_param.leg_default_translate_pos[i][1] = off[i](1);
-      i_param.leg_default_translate_pos[i][2] = off[i](2);
-  }
-  i_param.default_step_time = gg->get_default_step_time();
-  i_param.default_step_height = gg->get_default_step_height();
-  i_param.default_double_support_ratio_before = gg->get_default_double_support_ratio_before();
-  i_param.default_double_support_ratio_after = gg->get_default_double_support_ratio_after();
-  i_param.default_double_support_static_ratio_before = gg->get_default_double_support_static_ratio_before();
-  i_param.default_double_support_static_ratio_after = gg->get_default_double_support_static_ratio_after();
-  i_param.default_double_support_ratio_swing_before = gg->get_default_double_support_ratio_swing_before();
-  i_param.default_double_support_ratio_swing_after = gg->get_default_double_support_ratio_swing_after();
-  i_param.default_double_support_ratio = i_param.default_double_support_ratio_before + i_param.default_double_support_ratio_after;
-  i_param.default_double_support_static_ratio = i_param.default_double_support_static_ratio_before + i_param.default_double_support_static_ratio_after;
-  if (gg->get_default_orbit_type() == SHUFFLING) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::SHUFFLING;
-  } else if (gg->get_default_orbit_type() == CYCLOID) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOID;
-  } else if (gg->get_default_orbit_type() == RECTANGLE) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::RECTANGLE;
-  } else if (gg->get_default_orbit_type() == STAIR) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::STAIR;
-  } else if (gg->get_default_orbit_type() == CYCLOIDDELAY) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAY;
-  } else if (gg->get_default_orbit_type() == CYCLOIDDELAYKICK) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK;
-  } else if (gg->get_default_orbit_type() == CROSS) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CROSS;
-  }
-
-  hrp::Vector3 tmpv = gg->get_stair_trajectory_way_point_offset();
-  for (size_t i = 0; i < 3; i++) i_param.stair_trajectory_way_point_offset[i] = tmpv(i);
-  tmpv = gg->get_cycloid_delay_kick_point_offset();
-  for (size_t i = 0; i < 3; i++) i_param.cycloid_delay_kick_point_offset[i] = tmpv(i);
-  i_param.swing_trajectory_delay_time_offset = gg->get_swing_trajectory_delay_time_offset();
-  i_param.swing_trajectory_final_distance_weight = gg->get_swing_trajectory_final_distance_weight();
-  i_param.gravitational_acceleration = gg->get_gravitational_acceleration();
-  i_param.toe_angle = gg->get_toe_angle();
-  i_param.heel_angle = gg->get_heel_angle();
-  i_param.toe_pos_offset_x = gg->get_toe_pos_offset_x();
-  i_param.heel_pos_offset_x = gg->get_heel_pos_offset_x();
-  i_param.toe_zmp_offset_x = gg->get_toe_zmp_offset_x();
-  i_param.heel_zmp_offset_x = gg->get_heel_zmp_offset_x();
-  std::vector<double> ratio(gg->get_NUM_TH_PHASES(),0.0);
-  gg->get_toe_heel_phase_ratio(ratio);
-  for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) i_param.toe_heel_phase_ratio[i] = ratio[i];
-  i_param.use_toe_joint = gg->get_use_toe_joint();
-  i_param.use_toe_heel_transition = gg->get_use_toe_heel_transition();
-  std::map<leg_type, double> tmp_zmp_weight_map = gg->get_zmp_weight_map();
-  i_param.zmp_weight_map[0] = tmp_zmp_weight_map[RLEG];
-  i_param.zmp_weight_map[1] = tmp_zmp_weight_map[LLEG];
-  i_param.zmp_weight_map[2] = tmp_zmp_weight_map[RARM];
-  i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM];
-  i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num();
-  i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset();
-  return true;
-};
-
-bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
-{
-  Guard guard(m_mutex);
-  std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl;
-  double *default_zmp_offsets_array = new double[ikp.size()*3];
-  move_base_gain = i_param.move_base_gain;
-  for (size_t i = 0; i < ikp.size(); i++)
-    for (size_t j = 0; j < 3; j++)
-      default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j];
-  zmp_transition_time = i_param.zmp_transition_time;
-  adjust_footstep_transition_time = i_param.adjust_footstep_transition_time;
-  if (zmp_offset_interpolator->isEmpty()) {
-      zmp_offset_interpolator->clear();
-      zmp_offset_interpolator->go(default_zmp_offsets_array, zmp_transition_time, true);
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets cannot be set because interpolating." << std::endl;
-  }
-  if (control_mode == MODE_IDLE) {
-    switch (i_param.use_force_mode) {
-    case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
-        use_force = MODE_NO_FORCE;
-        break;
-    case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
-        use_force = MODE_REF_FORCE;
-        break;
-    default:
-        break;
-    }
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   use_force_mode cannot be changed to [" << i_param.use_force_mode << "] during MODE_ABC, MODE_SYNC_TO_IDLE or MODE_SYNC_TO_ABC." << std::endl;
-  }
-  graspless_manip_mode = i_param.graspless_manip_mode;
-  graspless_manip_arm = std::string(i_param.graspless_manip_arm);
-  for (size_t j = 0; j < 3; j++)
-      graspless_manip_p_gain[j] = i_param.graspless_manip_p_gain[j];
-  for (size_t j = 0; j < 3; j++)
-      graspless_manip_reference_trans_coords.pos[j] = i_param.graspless_manip_reference_trans_pos[j];
-  graspless_manip_reference_trans_coords.rot = (Eigen::Quaternion<double>(i_param.graspless_manip_reference_trans_rot[0],
-                                                                          i_param.graspless_manip_reference_trans_rot[1],
-                                                                          i_param.graspless_manip_reference_trans_rot[2],
-                                                                          i_param.graspless_manip_reference_trans_rot[3]).normalized().toRotationMatrix()); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
-  transition_time = i_param.transition_time;
-  std::vector<std::string> cur_leg_names, dst_leg_names;
-  cur_leg_names = leg_names;
-  for (size_t i = 0; i < i_param.leg_names.length(); i++) {
-      dst_leg_names.push_back(std::string(i_param.leg_names[i]));
-  }
-  std::sort(cur_leg_names.begin(), cur_leg_names.end());
-  std::sort(dst_leg_names.begin(), dst_leg_names.end());
-  if (cur_leg_names != dst_leg_names) {
-      if (leg_names_interpolator->isEmpty()) {
-          leg_names.clear();
-          leg_names = dst_leg_names;
-          if (control_mode == MODE_ABC) {
-              double tmp_ratio = 0.0;
-              leg_names_interpolator->set(&tmp_ratio);
-              tmp_ratio = 1.0;
-              leg_names_interpolator->go(&tmp_ratio, 5.0, true);
-              control_mode = MODE_SYNC_TO_ABC;
-          }
-      }
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   leg_names cannot be set because interpolating." << std::endl;
-  }
-  pos_ik_thre = i_param.pos_ik_thre;
-  rot_ik_thre = i_param.rot_ik_thre;
-  if (!gg_is_walking) {
-      is_hand_fix_mode = i_param.is_hand_fix_mode;
-      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode = " << is_hand_fix_mode << std::endl;
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode cannot be set in (gg_is_walking = true). Current is_hand_fix_mode is " << (is_hand_fix_mode?"true":"false") << std::endl;
-  }
-  if (control_mode == MODE_IDLE) {
-      for (size_t i = 0; i < i_param.end_effector_list.length(); i++) {
-          std::map<std::string, ABCIKparam>::iterator it = ikp.find(std::string(i_param.end_effector_list[i].leg));
-          memcpy(it->second.localPos.data(), i_param.end_effector_list[i].pos, sizeof(double)*3);
-          it->second.localR = (Eigen::Quaternion<double>(i_param.end_effector_list[i].rot[0], i_param.end_effector_list[i].rot[1], i_param.end_effector_list[i].rot[2], i_param.end_effector_list[i].rot[3])).normalized().toRotationMatrix();
-      }
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl;
-  }
-  if (i_param.default_gait_type == OpenHRP::AutoBalancerService::BIPED) {
-      gait_type = BIPED;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::TROT) {
-      gait_type = TROT;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::PACE) {
-      gait_type = PACE;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::CRAWL) {
-      gait_type = CRAWL;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) {
-      gait_type = GALLOP;
-  }
-  for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
-      std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   localR = " << it->second.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
-  }
-
-  std::cerr << "[" << m_profile.instance_name << "]   move_base_gain = " << move_base_gain << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets = ";
-  for (size_t i = 0; i < ikp.size() * 3; i++) {
-      std::cerr << default_zmp_offsets_array[i] << " ";
-  }
-  std::cerr << std::endl;
-  delete[] default_zmp_offsets_array;
-  std::cerr << "[" << m_profile.instance_name << "]   use_force_mode = " << use_force << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_mode = " << graspless_manip_mode << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_arm = " << graspless_manip_arm << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_pos = " << graspless_manip_reference_trans_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_rot = " << graspless_manip_reference_trans_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   transition_time = " << transition_time << "[s], zmp_transition_time = " << zmp_transition_time << "[s], adjust_footstep_transition_time = " << adjust_footstep_transition_time << "[s]" << std::endl;
-  for (std::vector<std::string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) std::cerr << "[" << m_profile.instance_name << "]   leg_names [" << *it << "]" << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   pos_ik_thre = " << pos_ik_thre << "[m], rot_ik_thre = " << rot_ik_thre << "[rad]" << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   default_gait_type = " << gait_type << std::endl;
-  // IK limb parameters
-  std::cerr << "[" << m_profile.instance_name << "]  IK limb parameters" << std::endl;
-  bool is_ik_limb_parameter_valid_length = true;
-  if (i_param.ik_limb_parameters.length() != ee_vec.size()) {
-      is_ik_limb_parameter_valid_length = false;
-      std::cerr << "[" << m_profile.instance_name << "]   ik_limb_parameters invalid length! Cannot be set. (input = " << i_param.ik_limb_parameters.length() << ", desired = " << ee_vec.size() << ")" << std::endl;
-  } else {
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          if (ikp[ee_vec[i]].manip->numJoints() != i_param.ik_limb_parameters[i].ik_optional_weight_vector.length())
-              is_ik_limb_parameter_valid_length = false;
-      }
-      if (is_ik_limb_parameter_valid_length) {
-          for (size_t i = 0; i < ee_vec.size(); i++) {
-              ABCIKparam& param = ikp[ee_vec[i]];
-              const OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i];
-              std::vector<double> ov;
-              ov.resize(param.manip->numJoints());
-              for (size_t j = 0; j < param.manip->numJoints(); j++) {
-                  ov[j] = ilp.ik_optional_weight_vector[j];
-              }
-              param.manip->setOptionalWeightVector(ov);
-              param.manip->setSRGain(ilp.sr_gain);
-              param.avoid_gain = ilp.avoid_gain;
-              param.reference_gain = ilp.reference_gain;
-              param.manip->setManipulabilityLimit(ilp.manipulability_limit);
-          }
-      } else {
-          std::cerr << "[" << m_profile.instance_name << "]   ik_optional_weight_vector invalid length! Cannot be set. (input = [";
-          for (size_t i = 0; i < ee_vec.size(); i++) {
-              std::cerr << i_param.ik_limb_parameters[i].ik_optional_weight_vector.length() << ", ";
-          }
-          std::cerr << "], desired = [";
-          for (size_t i = 0; i < ee_vec.size(); i++) {
-              std::cerr << ikp[ee_vec[i]].manip->numJoints() << ", ";
-          }
-          std::cerr << "])" << std::endl;
-      }
-  }
-  if (is_ik_limb_parameter_valid_length) {
-      std::cerr << "[" << m_profile.instance_name << "]   ik_optional_weight_vectors = ";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          ABCIKparam& param = ikp[ee_vec[i]];
-          std::vector<double> ov;
-          ov.resize(param.manip->numJoints());
-          param.manip->getOptionalWeightVector(ov);
-          std::cerr << "[";
-          for (size_t j = 0; j < param.manip->numJoints(); j++) {
-              std::cerr << ov[j] << " ";
-          }
-          std::cerr << "]";
-      }
-      std::cerr << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   sr_gains = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].manip->getSRGain() << ", ";
-      }
-      std::cerr << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   avoid_gains = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].avoid_gain << ", ";
-      }
-      std::cerr << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   reference_gains = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].reference_gain << ", ";
-      }
-      std::cerr << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   manipulability_limits = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].manip->getManipulabilityLimit() << ", ";
-      }
-      std::cerr << "]" << std::endl;
-  }
-  return true;
-};
-
-bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
-{
-  i_param.move_base_gain = move_base_gain;
-  i_param.default_zmp_offsets.length(ikp.size());
-  for (size_t i = 0; i < ikp.size(); i++) {
-      i_param.default_zmp_offsets[i].length(3);
-      for (size_t j = 0; j < 3; j++) {
-          i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j);
-      }
-  }
-  switch(control_mode) {
-  case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break;
-  case MODE_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_ABC; break;
-  case MODE_SYNC_TO_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_IDLE; break;
-  case MODE_SYNC_TO_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_ABC; break;
-  default: break;
-  }
-  switch(use_force) {
-  case MODE_NO_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_NO_FORCE; break;
-  case MODE_REF_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE; break;
-  default: break;
-  }
-  i_param.graspless_manip_mode = graspless_manip_mode;
-  i_param.graspless_manip_arm = graspless_manip_arm.c_str();
-  for (size_t j = 0; j < 3; j++)
-      i_param.graspless_manip_p_gain[j] = graspless_manip_p_gain[j];
-  for (size_t j = 0; j < 3; j++)
-      i_param.graspless_manip_reference_trans_pos[j] = graspless_manip_reference_trans_coords.pos[j];
-  Eigen::Quaternion<double> qt(graspless_manip_reference_trans_coords.rot);
-  i_param.graspless_manip_reference_trans_rot[0] = qt.w();
-  i_param.graspless_manip_reference_trans_rot[1] = qt.x();
-  i_param.graspless_manip_reference_trans_rot[2] = qt.y();
-  i_param.graspless_manip_reference_trans_rot[3] = qt.z();
-  i_param.transition_time = transition_time;
-  i_param.zmp_transition_time = zmp_transition_time;
-  i_param.adjust_footstep_transition_time = adjust_footstep_transition_time;
-  i_param.leg_names.length(leg_names.size());
-  for (size_t i = 0; i < leg_names.size(); i++) i_param.leg_names[i] = leg_names.at(i).c_str();
-  i_param.pos_ik_thre = pos_ik_thre;
-  i_param.rot_ik_thre = rot_ik_thre;
-  i_param.is_hand_fix_mode = is_hand_fix_mode;
-  i_param.end_effector_list.length(ikp.size());
-  {
-      size_t i = 0;
-      for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
-          copyRatscoords2Footstep(i_param.end_effector_list[i],
-                                  coordinates(it->second.localPos, it->second.localR));
-          i_param.end_effector_list[i].leg = it->first.c_str();
-          i++;
-      }
-  }
-  switch(gait_type) {
-  case BIPED:  i_param.default_gait_type = OpenHRP::AutoBalancerService::BIPED;  break;
-  case TROT:   i_param.default_gait_type = OpenHRP::AutoBalancerService::TROT;   break;
-  case PACE:   i_param.default_gait_type = OpenHRP::AutoBalancerService::PACE;   break;
-  case CRAWL:  i_param.default_gait_type = OpenHRP::AutoBalancerService::CRAWL;  break;
-  case GALLOP: i_param.default_gait_type = OpenHRP::AutoBalancerService::GALLOP; break;
-  default: break;
-  }
-  i_param.ik_limb_parameters.length(ee_vec.size());
-  for (size_t i = 0; i < ee_vec.size(); i++) {
-      ABCIKparam& param = ikp[ee_vec[i]];
-      OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i];
-      ilp.ik_optional_weight_vector.length(param.manip->numJoints());
-      std::vector<double> ov;
-      ov.resize(param.manip->numJoints());
-      param.manip->getOptionalWeightVector(ov);
-      for (size_t j = 0; j < param.manip->numJoints(); j++) {
-          ilp.ik_optional_weight_vector[j] = ov[j];
-      }
-      ilp.sr_gain = param.manip->getSRGain();
-      ilp.avoid_gain = param.avoid_gain;
-      ilp.reference_gain = param.reference_gain;
-      ilp.manipulability_limit = param.manip->getManipulabilityLimit();
-  }
-  return true;
-};
-
-void AutoBalancer::copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs)
-{
-  memcpy(out_fs.pos, in_fs.pos.data(), sizeof(double)*3);
-  Eigen::Quaternion<double> qt(in_fs.rot);
-  out_fs.rot[0] = qt.w();
-  out_fs.rot[1] = qt.x();
-  out_fs.rot[2] = qt.y();
-  out_fs.rot[3] = qt.z();
-};
-
-bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param)
-{
-  copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_leg_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.swing_leg_dst_coords, gg->get_swing_leg_dst_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.dst_foot_midcoords, gg->get_dst_foot_midcoords());
-  if (gg->get_support_leg_names().front() == "rleg") {
-    i_param.support_leg = OpenHRP::AutoBalancerService::RLEG;
-  } else {
-    i_param.support_leg = OpenHRP::AutoBalancerService::LLEG;
-  }
-  switch ( gg->get_current_support_states().front() ) {
-  case BOTH: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::BOTH; break;
-  case RLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::RLEG; break;
-  case LLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::LLEG; break;
-  default: break;
-  }
-  return true;
-};
-
-bool AutoBalancer::adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep)
-{
-  std::cerr << "[" << m_profile.instance_name << "] adjustFootSteps" << std::endl;
-  if (control_mode == MODE_ABC && !gg_is_walking && adjust_footstep_interpolator->isEmpty()) {
-      Guard guard(m_mutex);
-      //
-      hrp::Vector3 eepos, org_mid_rpy, target_mid_rpy;
-      hrp::Matrix33 eerot, tmprot;
-      coordinates org_mid_coords, target_mid_coords;
-      // Get org coords
-      ikp["rleg"].adjust_interpolation_org_p0 = ikp["rleg"].target_p0;
-      ikp["lleg"].adjust_interpolation_org_p0 = ikp["lleg"].target_p0;
-      ikp["rleg"].adjust_interpolation_org_r0 = ikp["rleg"].target_r0;
-      ikp["lleg"].adjust_interpolation_org_r0 = ikp["lleg"].target_r0;
-      mid_coords(org_mid_coords, 0.5,
-                 coordinates(ikp["rleg"].adjust_interpolation_org_p0, ikp["rleg"].adjust_interpolation_org_r0),
-                 coordinates(ikp["lleg"].adjust_interpolation_org_p0, ikp["lleg"].adjust_interpolation_org_r0));
-      org_mid_rpy = hrp::rpyFromRot(org_mid_coords.rot);
-      // Get target coords
-      //   Input : ee coords
-      //   Output : link coords
-      memcpy(eepos.data(), rfootstep.pos, sizeof(double)*3);
-      eerot = (Eigen::Quaternion<double>(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
-      ikp["rleg"].adjust_interpolation_target_r0 = eerot * ikp["rleg"].localR.transpose();
-      ikp["rleg"].adjust_interpolation_target_p0 = eepos - ikp["rleg"].adjust_interpolation_target_r0 * ikp["rleg"].localPos;
-      memcpy(eepos.data(), lfootstep.pos, sizeof(double)*3);
-      eerot = (Eigen::Quaternion<double>(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
-      ikp["lleg"].adjust_interpolation_target_r0 = eerot * ikp["lleg"].localR.transpose();
-      ikp["lleg"].adjust_interpolation_target_p0 = eepos - ikp["lleg"].adjust_interpolation_target_r0 * ikp["lleg"].localPos;
-      mid_coords(target_mid_coords, 0.5,
-                 coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0),
-                 coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
-      coordinates rtrans, ltrans;
-      target_mid_coords.transformation(rtrans, coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0));
-      target_mid_coords.transformation(ltrans, coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
-      target_mid_rpy = hrp::rpyFromRot(target_mid_coords.rot);
-      // Fix target pos => org pos, target yaw => org yaw
-      target_mid_rpy(2) = org_mid_rpy(2);
-      target_mid_coords.rot = hrp::rotFromRpy(target_mid_rpy);
-      target_mid_coords.pos = org_mid_coords.pos;
-      // Calculate rleg and lleg coords
-      coordinates tmpc;
-      tmpc = target_mid_coords;
-      tmpc.transform(rtrans);
-      ikp["rleg"].adjust_interpolation_target_p0 = tmpc.pos;
-      ikp["rleg"].adjust_interpolation_target_r0 = tmpc.rot;
-      tmpc = target_mid_coords;
-      tmpc.transform(ltrans);
-      ikp["lleg"].adjust_interpolation_target_p0 = tmpc.pos;
-      ikp["lleg"].adjust_interpolation_target_r0 = tmpc.rot;
-      // Set interpolator
-      adjust_footstep_interpolator->clear();
-      double tmp = 0.0;
-      adjust_footstep_interpolator->set(&tmp);
-      tmp = 1.0;
-      adjust_footstep_interpolator->go(&tmp, adjust_footstep_transition_time, true);
-  }
-  while (!adjust_footstep_interpolator->isEmpty() )
-    usleep(1000);
-  usleep(1000);
-  return true;
-};
-
-bool AutoBalancer::getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx)
-{
-    std::cerr << "[" << m_profile.instance_name << "] getRemainingFootstepSequence" << std::endl;
-    o_footstep = new OpenHRP::AutoBalancerService::FootstepSequence;
-    if (gg_is_walking) {
-        std::vector< std::vector<step_node> > fsnl = gg->get_remaining_footstep_nodes_list();
-        o_current_fs_idx = gg->get_footstep_index();
-        o_footstep->length(fsnl.size());
-        for (size_t i = 0; i < fsnl.size(); i++) {
-            o_footstep[i].leg = (fsnl[i].front().l_r==RLEG?"rleg":"lleg");
-            copyRatscoords2Footstep(o_footstep[i], fsnl[i].front().worldcoords);
-        }
-    }
-};
-
-bool AutoBalancer::getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep)
-{
-    std::cerr << "[" << m_profile.instance_name << "] getGoPosFootstepsSequence" << std::endl;
-    o_footstep = new OpenHRP::AutoBalancerService::FootstepsSequence;
-    if (gg_is_walking) {
-        std::cerr << "[" << m_profile.instance_name << "] Cannot call getGoPosFootstepsSequence in walking" << std::endl;
-        return false;
-    } else {
-        gg->set_all_limbs(leg_names);
-        std::vector< std::vector<step_node> > new_footstep_nodes_list;
-        coordinates start_ref_coords;
-        std::vector<coordinates> initial_support_legs_coords;
-        std::vector<leg_type> initial_support_legs;
-        bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
-        if (is_valid_gait_type == false) return false;
-        /* go_pos_param_2_footstep_nodes_list_core is const member function  */
-        gg->go_pos_param_2_footstep_nodes_list_core (x, y, th,
-                                                     initial_support_legs_coords, start_ref_coords, initial_support_legs,
-                                                     new_footstep_nodes_list, true, 0);
-        o_footstep->length(new_footstep_nodes_list.size());
-        for (size_t i = 0; i < new_footstep_nodes_list.size(); i++) {
-            o_footstep[i].fs.length(new_footstep_nodes_list.at(i).size());
-            for (size_t j = 0; j < new_footstep_nodes_list.at(i).size(); j++) {
-                leg_type tmp_leg_type = new_footstep_nodes_list.at(i).at(j).l_r;
-                o_footstep[i].fs[j].leg = ((tmp_leg_type == RLEG) ? "rleg":
-                                           (tmp_leg_type == LLEG) ? "lleg":
-                                           (tmp_leg_type == RARM) ? "rarm":
-                                           "larm");
-                copyRatscoords2Footstep(o_footstep[i].fs[j], new_footstep_nodes_list.at(i).at(j).worldcoords);
-            }
-        }
-        return true;
-    }
-};
-
-void AutoBalancer::static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height)
-{
-  hrp::Vector3 target_sbp = hrp::Vector3(0, 0, 0);
-  hrp::Vector3 tmpcog = m_robot->calcCM();
-  switch ( use_force ) {
-  case MODE_REF_FORCE:
-    calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height, ref_forces);
-    tmp_input_sbp = target_sbp - sbp_offset;
-    sbp_cog_offset = tmp_input_sbp - tmpcog;
-    break;
-  case MODE_NO_FORCE:
-    tmp_input_sbp = tmpcog + sbp_cog_offset;
-    break;
-  default: break;
-  }
-};
-
-void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces)
-{
-  hrp::Vector3 denom, nume;
-  /* sb_point[m] = nume[kg * m/s^2 * m] / denom[kg * m/s^2] */
-  double mass = m_robot->totalMass();
-  for (size_t j = 0; j < 2; j++) {
-    nume(j) = mass * gg->get_gravitational_acceleration() * tmpcog(j);
-    denom(j) = mass * gg->get_gravitational_acceleration();
-    for (size_t i = 0; i < sensor_names.size(); i++) {
-      if ( sensor_names[i].find("hsensor") != std::string::npos || sensor_names[i].find("asensor") != std::string::npos ) { // tempolary to get arm force coords
-          hrp::Link* parentlink;
-          hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
-          if (sensor) parentlink = sensor->link;
-          else parentlink = m_vfs[sensor_names[i]].link;
-          for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-              if (it->second.target_link->name == parentlink->name) {
-                  hrp::Vector3 fpos = parentlink->p + parentlink->R * it->second.localPos;
-                  nume(j) += ( (fpos(2) - ref_com_height) * tmp_forces[i](j) - fpos(j) * tmp_forces[i](2) );
-                  denom(j) -= tmp_forces[i](2);
-              }
-          }
-      }
-    }
-    sb_point(j) = nume(j) / denom(j);
-  }
-  sb_point(2) = ref_com_height;
-};
-
-#ifndef rad2deg
-#define rad2deg(rad) (rad * 180 / M_PI)
-#endif
-#ifndef deg2rad
-#define deg2rad(deg) (deg * M_PI / 180)
-#endif
-
-hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_coords)
-{
-  if (graspless_manip_mode) {
-    hrp::Vector3 dp,dr;
-    coordinates ref_hand_coords(gg->get_dst_foot_midcoords()), act_hand_coords;
-    ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords
-    hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos);
-    if ( graspless_manip_arm == "arms" ) {
-        hrp::Vector3 rarm_pos = ikp["rarm"].target_p0 + ikp["rarm"].target_r0 * ikp["rarm"].localPos;
-        hrp::Vector3 larm_pos = ikp["larm"].target_p0 + ikp["larm"].target_r0 * ikp["larm"].localPos;
-        act_hand_coords.pos = (rarm_pos+larm_pos)/2.0;
-        hrp::Vector3 act_y = larm_pos-rarm_pos;
-        act_y(2) = 0;
-        act_y.normalize();
-        hrp::Vector3 ref_y(ref_hand_coords.rot * hrp::Vector3::UnitY());
-        ref_y(2) = 0;
-        ref_y.normalize();
-        dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y))); // fix for rotation
-    } else {
-      ABCIKparam& tmpikp = ikp[graspless_manip_arm];
-      act_hand_coords = rats::coordinates(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos,
-                                          tmpikp.target_r0 * tmpikp.localR);
-      rats::difference_rotation(dr, ref_hand_coords.rot, act_hand_coords.rot);
-      dr(0) = 0; dr(1) = 0;
-    }
-    dp = act_hand_coords.pos - ref_hand_coords.pos
-        + dr.cross(hrp::Vector3(foot_pos - act_hand_coords.pos));
-    dp(2) = 0;
-    hrp::Matrix33 foot_mt(gg->get_dst_foot_midcoords().rot.transpose());
-    //alias(dp) = foot_mt * dp;
-    hrp::Vector3 dp2 = foot_mt * dp;
-    //alias(dr) = foot_mt * dr;
-    return hrp::Vector3(graspless_manip_p_gain[0] * dp2(0)/gg->get_default_step_time(),
-                        graspless_manip_p_gain[1] * dp2(1)/gg->get_default_step_time(),
-                        graspless_manip_p_gain[2] * rad2deg(dr(2))/gg->get_default_step_time());
-  } else {
-    return hrp::Vector3::Zero();
-  }
-};
-
-bool AutoBalancer::calc_inital_support_legs(const double& y, std::vector<coordinates>& initial_support_legs_coords, std::vector<leg_type>& initial_support_legs, coordinates& start_ref_coords) {
-    switch(gait_type) {
-    case BIPED:
-        initial_support_legs_coords = (y > 0 ?
-                                       boost::assign::list_of(ikp["rleg"].target_end_coords)
-                                       : boost::assign::list_of(ikp["lleg"].target_end_coords));
-        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
-        break;
-    case TROT:
-        initial_support_legs_coords = (y > 0 ?
-                                       boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["larm"].target_end_coords)
-                                       : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["rarm"].target_end_coords));
-        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
-        break;
-    case PACE:
-        initial_support_legs_coords = (y > 0 ?
-                                       boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["rarm"].target_end_coords)
-                                       : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["larm"].target_end_coords));
-        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
-        break;
-    case CRAWL:
-        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    case GALLOP:
-        /* at least one leg shoud be in contact */
-        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    default: break;
-    }
-    mid_coords(start_ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
-    return true;
-};
-
-//
-extern "C"
-{
-
-    void AutoBalancerInit(RTC::Manager* manager)
-    {
-        RTC::Properties profile(autobalancer_spec);
-        manager->registerFactory(profile,
-                                 RTC::Create<AutoBalancer>,
-                                 RTC::Delete<AutoBalancer>);
-    }
-
-};
-
-
diff --git a/rtc/AutoBalancer/bak/AutoBalancer.h b/rtc/AutoBalancer/bak/AutoBalancer.h
deleted file mode 100644
index e793c7c9db6..00000000000
--- a/rtc/AutoBalancer/bak/AutoBalancer.h
+++ /dev/null
@@ -1,596 +0,0 @@
-// -*- C++ -*-
-/*!
- * @file  AutoBalancer.h
- * @brief autobalancer component
- * @date  $Date$
- *
- * $Id$
- */
-
-#ifndef AUTOBALANCER_H
-#define AUTOBALANCER_H
-
-#include <rtm/Manager.h>
-#include <rtm/DataFlowComponentBase.h>
-#include <rtm/CorbaPort.h>
-#include <rtm/DataInPort.h>
-#include <rtm/DataOutPort.h>
-#include <rtm/idl/BasicDataTypeSkel.h>
-#include <rtm/idl/ExtendedDataTypesSkel.h>
-#include <hrpModel/Body.h>
-#include "../ImpedanceController/JointPathEx.h"
-#include "../ImpedanceController/RatsMatrix.h"
-#include "GaitGenerator.h"
-// Service implementation headers
-// <rtc-template block="service_impl_h">
-#include "AutoBalancerService_impl.h"
-#include "interpolator.h"
-
-// </rtc-template>
-
-// Service Consumer stub headers
-// <rtc-template block="consumer_stub_h">
-
-
-class HumanPose{
-  private: hrp::Vector3 zero;
-  public:
-    hrp::Vector3 com,rf,lf,rh,lh;
-    hrp::Vector3 zmp;
-    int idsize;
-    typedef struct{ hrp::Vector3 f; hrp::Vector3 t; }Wrench6;
-    Wrench6 rfw,lfw;
-    HumanPose() : idsize(10){
-      clear();
-    }
-    ~HumanPose(){cerr<<"HumanPose destructed"<<endl;}
-    void clear(){
-      for(int i=0;i<idsize;i++){ Seq(i) = hrp::Vector3::Zero(); }
-    }
-    hrp::Vector3& Seq(const int i){
-      switch(i){
-        case 0: return com;
-        case 1: return rf;
-        case 2: return lf;
-        case 3: return rh;
-        case 4: return lh;
-        case 5: return zmp;
-        case 6: return rfw.f;
-        case 7: return rfw.t;
-        case 8: return lfw.f;
-        case 9: return lfw.t;
-        default: std::cerr <<"[WARN] HumanPose::getSeq(int) invalid index"<<std::endl;return zero;
-      }
-    }
-    const hrp::Vector3& Seq(const int i) const{//const指定オブジェクトからオーバーロードする時のためのオーバーライド
-//      return Seq(i);//呼べない・・・うーん
-      switch(i){
-        case 0: return com;
-        case 1: return rf;
-        case 2: return lf;
-        case 3: return rh;
-        case 4: return lh;
-        case 5: return zmp;
-        case 6: return rfw.f;
-        case 7: return rfw.t;
-        case 8: return lfw.f;
-        case 9: return lfw.t;
-        default: std::cerr <<"[WARN] HumanPose::getSeq(int) invalid index"<<std::endl;return zero;
-      }
-    }
-    static void hp_printf(HumanPose& in){
-      const std::string cap[] = {"com:","rf:","lf:","rh:","lh:","zmp:","rfw:","","lfw:",""};
-      for(int i=0;i<10;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.Seq(i)(0),in.Seq(i)(1),in.Seq(i)(2)); }
-      printf("\n");
-    }
-    void print(){ hp_printf(*this); }
-};
-
-
-class HumanSynchronizer{
-  private:
-
-    double FNUM;
-    int FUPLEVELNUM;
-    int cur_rfup_level,cur_lfup_level;
-    double FUPHIGHT;
-    double CNT_F_TH;
-    hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
-    hrp::Vector3 init_wld_rp_compos;
-    hrp::Vector3 init_wld_hp_rfpos,init_wld_hp_lfpos;
-    hrp::Vector3 init_wld_rp_rfpos,init_wld_rp_lfpos;
-    hrp::Vector3 init_hp_calibcom;
-    double MAXVEL,MAXACC;
-    HumanPose hp_wld_raw;
-    HumanPose hp_wld_initpos;
-    HumanPose hp_rel_raw;
-    HumanPose hp_filtered;
-    HumanPose rp_ref_out_old;
-    int countdown_num;
-    bool HumanSyncOn;
-    struct timeval t_calc_start, t_calc_end;
-
-  public:
-
-    bool startCountdownForHumanSync;
-    bool ht_first_call;
-    bool is_rf_contact,is_lf_contact;
-    HumanPose rp_wld_initpos;
-    double h2r_ratio;
-    hrp::Vector3 init_wld_rp_basepos;
-    HumanPose rp_ref_out;
-
-    HumanSynchronizer(){
-      h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
-//      h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
-//      h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
-      //h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
-      cur_rfup_level = 0;       cur_lfup_level = 0;
-      is_rf_contact = true;     is_lf_contact = true;
-      pre_cont_rfpos = hrp::Vector3::Zero();    pre_cont_lfpos = hrp::Vector3::Zero();
-      init_wld_hp_rfpos = hrp::Vector3(0,-0.1/h2r_ratio,0);
-      init_wld_hp_lfpos = hrp::Vector3(0, 0.1/h2r_ratio,0);
-      init_wld_rp_rfpos = hrp::Vector3(0,-0.1,0);
-      init_wld_rp_lfpos = hrp::Vector3(0, 0.1,0);
-      init_hp_calibcom.Zero();
-      FNUM = 0.01;
-      FUPLEVELNUM = 200;
-      FUPHIGHT = 0.05;
-      CNT_F_TH = 20.0;
-      MAXVEL = 0.004;
-      //      MAXVEL = 0.001;
-      HumanSyncOn = false;
-      ht_first_call = true;
-      startCountdownForHumanSync = false;
-      countdown_num = 5*500;
-    }
-    ~HumanSynchronizer(){cout<<"HumanSynchronizer destructed"<<endl;}
-
-    void readInput(const HumanPose& raw_in){ hp_wld_raw = raw_in; }
-    void setInitOffsetPose(){ hp_wld_initpos = hp_wld_raw; }
-    const int getRemainingCountDown() const { return countdown_num; }
-    const bool isHumanSyncOn() const { return HumanSyncOn; }
-    const double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0; }
-    void updateCountDown(){
-      if(countdown_num>0){
-        countdown_num--;
-      }else if(countdown_num==0){
-        HumanSyncOn = true;
-        startCountdownForHumanSync = false;
-      }
-    }
-    void update(){
-      gettimeofday(&t_calc_start, NULL);
-      removeInitOffsetPose(hp_wld_raw, hp_wld_initpos, hp_rel_raw);
-      applyInitHumanCOMOffset(hp_wld_raw, init_hp_calibcom, hp_wld_raw);
-      applyLPFilter(hp_rel_raw, hp_filtered, hp_filtered);
-      lockFootXYOnContact(hp_filtered, is_rf_contact, is_lf_contact, hp_filtered);//根本から改変すべき
-      calcWorldZMP((hp_filtered.rf+init_wld_hp_rfpos), (hp_filtered.lf+init_wld_hp_lfpos), hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
-      convertRelHumanPoseToRelRobotPose(hp_filtered,rp_ref_out);
-      judgeFootContactStates(rp_ref_out.rfw, rp_ref_out.lfw, is_rf_contact, is_lf_contact);
-      overwriteFootZFromContactStates(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);
-//      lockFootXYOnContact(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);//ここで改変するとLPFかかってないからジャンプする
-      applyWorkspaceLimit(rp_ref_out, rp_ref_out);
-      applyCOMToSupportRegionLimit(rp_ref_out, rp_ref_out);
-      applyVelLimit(rp_ref_out, rp_ref_out_old, rp_ref_out);
-//      rp_ref_out.com(2) = 0;//Z方向固定
-      rp_ref_out_old = rp_ref_out;
-      gettimeofday(&t_calc_end, NULL);
-    }
-    void calibInitHumanCOMFromZMP(){
-      init_hp_calibcom(0) = hp_filtered.zmp(0);
-      init_hp_calibcom(1) = hp_filtered.zmp(1);
-    }
-    void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const HumanPose::Wrench6& rfwin, const HumanPose::Wrench6& lfwin, hrp::Vector3& zmp_ans){
-      hrp::Vector3 rfzmp,lfzmp;
-      const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
-      if( rfwin.f(2) > 1.0e-6 ){
-        rfzmp(0) = ( - rfwin.t(1) - rfwin.f(0) * F_H_OFFSET + rfwin.f(2) * 0 ) / rfwin.f(2) + rfpos(0);
-        rfzmp(1) = (   rfwin.t(0) - rfwin.f(1) * F_H_OFFSET + rfwin.f(2) * 0 ) / rfwin.f(2) + rfpos(1);
-      }
-      if( lfwin.f(2) > 1.0e-6 ){
-        lfzmp(0) = ( - lfwin.t(1) - lfwin.f(0) * F_H_OFFSET + lfwin.f(2) * 0 ) / lfwin.f(2) + lfpos(0);
-        lfzmp(1) = (   lfwin.t(0) - lfwin.f(1) * F_H_OFFSET + lfwin.f(2) * 0 ) / lfwin.f(2) + lfpos(1);
-      }
-      if( rfwin.f(2) > 1.0e-6 || lfwin.f(2) > 1.0e-6 ){
-        zmp_ans(0) = ( rfzmp(0)*rfwin.f(2) + lfzmp(0)*lfwin.f(2) ) / ( rfwin.f(2) + lfwin.f(2));
-        zmp_ans(1) = ( rfzmp(1)*rfwin.f(2) + lfzmp(1)*lfwin.f(2) ) / ( rfwin.f(2) + lfwin.f(2));
-      }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
-      zmp_ans(2) = 0;
-    }
-    static hrp::Vector3 Point3DToVector3(const RTC::TimedPoint3D& in){ hrp::Vector3 out(in.data.x,in.data.y,in.data.z); return out; }
-    static HumanPose::Wrench6 DoubleSeqToWrench6(const RTC::TimedDoubleSeq& in){
-      HumanPose::Wrench6 out;
-      if(in.data.length() == 6){out.f(0)=in.data[0]; out.f(1)=in.data[1]; out.f(2)=in.data[2]; out.t(0)=in.data[3]; out.t(1)=in.data[4]; out.t(2)=in.data[5];
-      }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; out.f.Zero(); out.t.Zero(); }
-      return out;
-    }
-
-  private:
-
-    void removeInitOffsetPose(const HumanPose& abs_in, const HumanPose& init_offset, HumanPose& rel_out){
-      for(int i=0;i<rel_out.idsize-5;i++){
-        rel_out.Seq(i) =  abs_in.Seq(i) - init_offset.Seq(i);
-      }
-      rel_out.rfw = abs_in.rfw;
-      rel_out.lfw = abs_in.lfw;
-    }
-    void applyInitHumanCOMOffset(const HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
-      out = in;
-      out.com = in.com + com_offs;
-    }
-//    void startHumanSync(){
-//      countdown_flag = true;
-//      setInitOffsetPose();
-//      HumanSyncOn = true;
-//    }
-    void applyLPFilter(const HumanPose& raw_in, const HumanPose& out_old, HumanPose& filt_out){
-      for(int i=0;i<filt_out.idsize;i++){
-        filt_out.Seq(i) = (1-FNUM) * out_old.Seq(i) + FNUM * raw_in.Seq(i);
-      }
-    }
-    void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
-      for(int i=0;i<rp_out.idsize-4;i++){
-        rp_out.Seq(i) =  h2r_ratio * hp_in.Seq(i);
-      }
-      rp_out.rfw  = hp_in.rfw;
-      rp_out.lfw  = hp_in.lfw;
-    }
-    void judgeFootContactStates(const HumanPose::Wrench6& rfw_in, const HumanPose::Wrench6& lfw_in, bool& rfcs_ans, bool& lfcs_ans){
-      //他のプログラムは足に荷重時に力センサの値がZ軸マイナスに出るっぽい？(のでTODO FIX)
-      if      (rfw_in.f(2)<CNT_F_TH && lfw_in.f(2)>CNT_F_TH){rfcs_ans = false;  lfcs_ans = true;}//右足浮遊かつ左足接地
-      else if (rfw_in.f(2)>CNT_F_TH && lfw_in.f(2)<CNT_F_TH){rfcs_ans = true;   lfcs_ans = false;}//右足接地かつ左足浮遊
-      else                                              {rfcs_ans = true;   lfcs_ans = true;}//両足接地または両足浮遊
-    }
-    void overwriteFootZFromContactStates(const HumanPose& in, const bool& rfcs_in, const bool& lfcs_in, HumanPose& out){
-      out = in;
-      if(!rfcs_in){//右足浮遊時
-        if(cur_rfup_level < FUPLEVELNUM){
-          cur_rfup_level++;
-          out.rf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
-        }else{
-          out.rf(2) = FUPHIGHT;
-        }
-      }else{
-        if(cur_rfup_level > 0){
-          cur_rfup_level--;
-          out.rf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
-        }else{
-          out.rf(2) = 0;
-        }
-      }
-      if(!lfcs_in){//左足浮遊時
-        if(cur_lfup_level < FUPLEVELNUM){
-          cur_lfup_level++;
-          out.lf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
-        }else{
-          out.lf(2) = FUPHIGHT;
-        }
-      }else{
-        if(cur_lfup_level > 0){
-          cur_lfup_level--;
-          out.lf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
-        }else{
-          out.lf(2) = 0;
-        }
-      }
-    }
-    void lockFootXYOnContact(const HumanPose& in, const bool& rfcs_in, const bool& lfcs_in, HumanPose& out){
-      out = in;
-      if(rfcs_in){
-        out.rf(0) = pre_cont_rfpos(0);
-        out.rf(1) = pre_cont_rfpos(1);
-      }else{
-        pre_cont_rfpos(0) = in.rf(0);
-        pre_cont_rfpos(1) = in.rf(1);
-      }
-      if(lfcs_in){
-        out.lf(0) = pre_cont_lfpos(0);
-        out.lf(1) = pre_cont_lfpos(1);
-      }else{
-        pre_cont_lfpos(0) = in.lf(0);
-        pre_cont_lfpos(1) = in.lf(1);
-      }
-//      out.rf(0) = 0; out.rf(1) = 0; out.lf(0) = 0; out.lf(1) = 0;//足固定テスト
-    }
-    void applyWorkspaceLimit(const HumanPose& in, HumanPose& out){
-      if(!is_rf_contact && is_lf_contact){//右足浮遊時
-        if(in.rf(1) + rp_wld_initpos.rf(1) > in.lf(1) + rp_wld_initpos.lf(1) - 0.15)out.rf(1) = in.lf(1) + rp_wld_initpos.lf(1) - rp_wld_initpos.rf(1) - 0.15;
-      }
-      else if(is_rf_contact && !is_lf_contact){//左足浮遊時
-        if(in.lf(1) + rp_wld_initpos.lf(1) < in.rf(1) + rp_wld_initpos.rf(1) + 0.15)out.lf(1) = in.rf(1) + rp_wld_initpos.rf(1) - rp_wld_initpos.lf(1) + 0.15;
-      }
-    }
-    void applyCOMToSupportRegionLimit(const HumanPose& in, HumanPose& out){
-      out = in;
-      const double XUMARGIN = 0.08;
-      const double XLMARGIN = 0.04;
-      const double YLRMARGIN = 0.03;
-      double x_upper_region,x_lower_region,y_upper_region,y_lower_region;
-      if(rp_wld_initpos.rf(0) + in.rf(0) > rp_wld_initpos.lf(0) + in.lf(0)){
-        x_upper_region = rp_wld_initpos.rf(0) + in.rf(0) + XUMARGIN;
-        x_lower_region = rp_wld_initpos.lf(0) + in.lf(0) - XLMARGIN;
-      }else{
-        x_upper_region = rp_wld_initpos.lf(0) + in.lf(0) + XUMARGIN;
-        x_lower_region = rp_wld_initpos.rf(0) + in.rf(0) - XLMARGIN;
-      }
-      y_upper_region = rp_wld_initpos.lf(1) + in.lf(1) + YLRMARGIN;
-      y_lower_region = rp_wld_initpos.rf(1) + in.rf(1) - YLRMARGIN;
-      if(in.com(0) < x_lower_region){ out.com(0) = x_lower_region; cerr<<"[WARN] COM X Lower Limit!"<<endl;}
-      else if(in.com(0) > x_upper_region){ out.com(0) = x_upper_region; cerr<<"[WARN] COM X Upper Limit!"<<endl;}
-      if(in.com(1) < y_lower_region){ out.com(1) = y_lower_region; cerr<<"[WARN] COM Y Lower Limit!"<<endl;}
-      else if(in.com(1) > y_upper_region){ out.com(1) = y_upper_region; cerr<<"[WARN] COM Y Upper Limit!"<<endl;}
-    }
-    void applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out){
-      for(int i=0;i<3;i++){if(in.com(i) - in_old.com(i) > MAXVEL){out.com(i) = in_old.com(i) + MAXVEL;}else if(in.com(i) - in_old.com(i) < -MAXVEL){out.com(i) = in_old.com(i) - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.rf(i)  - in_old.rf(i)  > MAXVEL){out.rf(i)  = in_old.rf(i)  + MAXVEL;}else if(in.rf(i)  - in_old.rf(i)  < -MAXVEL){out.rf(i)  = in_old.rf(i)  - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.lf(i)  - in_old.lf(i)  > MAXVEL){out.lf(i)  = in_old.lf(i)  + MAXVEL;}else if(in.lf(i)  - in_old.lf(i)  < -MAXVEL){out.lf(i)  = in_old.lf(i)  - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.rh(i)  - in_old.rh(i)  > MAXVEL){out.rh(i)  = in_old.rh(i)  + MAXVEL;}else if(in.rh(i)  - in_old.rh(i)  < -MAXVEL){out.rh(i)  = in_old.rh(i)  - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.lh(i)  - in_old.lh(i)  > MAXVEL){out.lh(i)  = in_old.lh(i)  + MAXVEL;}else if(in.lh(i)  - in_old.lh(i)  < -MAXVEL){out.lh(i)  = in_old.lh(i)  - MAXVEL;}}
-    }
-};
-
-
-
-// </rtc-template>
-
-using namespace RTC;
-
-class AutoBalancer
-  : public RTC::DataFlowComponentBase
-{
- public:
-  AutoBalancer(RTC::Manager* manager);
-  virtual ~AutoBalancer();
-
-  // The initialize action (on CREATED->ALIVE transition)
-  // formaer rtc_init_entry()
- virtual RTC::ReturnCode_t onInitialize();
-
-  // The finalize action (on ALIVE->END transition)
-  // formaer rtc_exiting_entry()
-  virtual RTC::ReturnCode_t onFinalize();
-
-  // The startup action when ExecutionContext startup
-  // former rtc_starting_entry()
-  // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
-
-  // The shutdown action when ExecutionContext stop
-  // former rtc_stopping_entry()
-  // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
-
-  // The activated action (Active state entry action)
-  // former rtc_active_entry()
-  virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
-
-  // The deactivated action (Active state exit action)
-  // former rtc_active_exit()
-  virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
-
-  // The execution action that is invoked periodically
-  // former rtc_active_do()
-  virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
-
-  // The aborting action when main logic error occurred.
-  // former rtc_aborting_entry()
-  // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
-
-  // The error action in ERROR state
-  // former rtc_error_do()
-  // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
-
-  // The reset action that is invoked resetting
-  // This is same but different the former rtc_init_entry()
-  // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
-
-  // The state update action that is invoked after onExecute() action
-  // no corresponding operation exists in OpenRTm-aist-0.2.0
-  // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
-
-  // The action that is invoked when execution context's rate is changed
-  // no corresponding operation exists in OpenRTm-aist-0.2.0
-  // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
-  bool goPos(const double& x, const double& y, const double& th);
-  bool goVelocity(const double& vx, const double& vy, const double& vth);
-  bool goStop();
-  bool emergencyStop ();
-  bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx);
-  bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx);
-  bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx);
-  bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx);
-  void waitFootSteps();
-  void waitFootStepsEarly(const double tm);
-  bool startAutoBalancer(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
-  bool stopAutoBalancer();
-  bool setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param);
-  bool getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param);
-  bool setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param);
-  bool getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param);
-  bool getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param);
-  bool adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep);
-  bool getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx);
-  bool getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep);
-  bool releaseEmergencyStop();
-  bool startHumanSyncAfter5sec();
-  bool stopHumanSync();
-
- protected:
-  // Configuration variable declaration
-  // <rtc-template block="config_declare">
-  
-  // </rtc-template>
-
-  // DataInPort declaration
-  // <rtc-template block="inport_declare">
-  TimedDoubleSeq m_qRef;
-  InPort<TimedDoubleSeq> m_qRefIn;
-  TimedPoint3D m_basePos;
-  InPort<TimedPoint3D> m_basePosIn;
-  TimedOrientation3D m_baseRpy;
-  InPort<TimedOrientation3D> m_baseRpyIn;
-  TimedPoint3D m_zmp;
-  InPort<TimedPoint3D> m_zmpIn;
-  TimedDoubleSeq m_optionalData;
-  InPort<TimedDoubleSeq> m_optionalDataIn;
-  std::vector<TimedDoubleSeq> m_ref_force;
-  std::vector<InPort<TimedDoubleSeq> *> m_ref_forceIn;
-  TimedLong m_emergencySignal;
-  InPort<TimedLong> m_emergencySignalIn;
-  // for debug
-  TimedPoint3D m_cog;
-  //for human tracker
-  TimedPoint3D m_htzmp;
-  InPort<TimedPoint3D> m_htzmpIn;
-  TimedDoubleSeq m_htrfw;
-  InPort<TimedDoubleSeq> m_htrfwIn;
-  TimedDoubleSeq m_htlfw;
-  InPort<TimedDoubleSeq> m_htlfwIn;
-  TimedPoint3D m_htcom;
-  InPort<TimedPoint3D> m_htcomIn;
-  TimedPoint3D m_htrf;
-  InPort<TimedPoint3D> m_htrfIn;
-  TimedPoint3D m_htlf;
-  InPort<TimedPoint3D> m_htlfIn;
-  TimedPoint3D m_htrh;
-  InPort<TimedPoint3D> m_htrhIn;
-  TimedPoint3D m_htlh;
-  InPort<TimedPoint3D> m_htlhIn;
-  TimedPoint3D m_actzmp;
-  InPort<TimedPoint3D> m_actzmpIn;
-  
-  // </rtc-template>
-
-  // DataOutPort declaration
-  // <rtc-template block="outport_declare">
-  OutPort<TimedDoubleSeq> m_qOut;
-  RTC::OutPort<RTC::TimedPoint3D> m_zmpOut;
-  OutPort<TimedPoint3D> m_basePosOut;
-  OutPort<TimedOrientation3D> m_baseRpyOut;
-  TimedDoubleSeq m_baseTform;
-  OutPort<TimedDoubleSeq> m_baseTformOut;
-  TimedPose3D m_basePose;
-  OutPort<TimedPose3D> m_basePoseOut;
-  TimedAcceleration3D m_accRef;
-  OutPort<TimedAcceleration3D> m_accRefOut;
-  TimedBooleanSeq m_contactStates;
-  OutPort<TimedBooleanSeq> m_contactStatesOut;
-  TimedDoubleSeq m_controlSwingSupportTime;
-  OutPort<TimedDoubleSeq> m_controlSwingSupportTimeOut;
-  TimedBoolean m_walkingStates;
-  OutPort<TimedBoolean> m_walkingStatesOut;
-  TimedPoint3D m_sbpCogOffset;
-  OutPort<TimedPoint3D> m_sbpCogOffsetOut;
-  std::vector<TimedDoubleSeq> m_force;
-  std::vector<OutPort<TimedDoubleSeq> *> m_ref_forceOut;
-  std::vector<TimedPoint3D> m_limbCOPOffset;
-  std::vector<OutPort<TimedPoint3D> *> m_limbCOPOffsetOut;
-  // for debug
-  OutPort<TimedPoint3D> m_cogOut;
-  
-  // </rtc-template>
-
-  // CORBA Port declaration
-  // <rtc-template block="corbaport_declare">
-  RTC::CorbaPort m_AutoBalancerServicePort;
-
-  // </rtc-template>
-
-  // Service declaration
-  // <rtc-template block="service_declare">
-  AutoBalancerService_impl m_service0;
-
-  // </rtc-template>
-
-  // Consumer declaration
-  // <rtc-template block="consumer_declare">
-  
-  // </rtc-template>
-
- private:
-  struct ABCIKparam {
-    hrp::Vector3 target_p0, localPos, adjust_interpolation_target_p0, adjust_interpolation_org_p0;
-    hrp::Matrix33 target_r0, localR, adjust_interpolation_target_r0, adjust_interpolation_org_r0;
-    rats::coordinates target_end_coords;
-    hrp::Link* target_link;
-    hrp::JointPathExPtr manip;
-    double avoid_gain, reference_gain;
-    size_t pos_ik_error_count, rot_ik_error_count;
-    bool is_active, has_toe_joint;
-  };
-  void getCurrentParameters();
-  void getTargetParameters();
-  bool solveLimbIKforLimb (ABCIKparam& param);
-  void solveLimbIK();
-  void startABCparam(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
-  void stopABCparam();
-  void waitABCTransition();
-  hrp::Matrix33 OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2);
-  void fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot);
-  void startWalking ();
-  void stopWalking ();
-  void copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs);
-  // static balance point offsetting
-  void static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height);
-  void calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces);
-  hrp::Vector3 calc_vel_from_hand_error (const rats::coordinates& tmp_fix_coords);
-  bool isOptionalDataContact (const std::string& ee_name)
-  {
-      return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false;
-  };
-  bool calc_inital_support_legs(const double& y, std::vector<rats::coordinates>& initial_support_legs_coords, std::vector<rats::leg_type>& initial_support_legs, rats::coordinates& start_ref_coords);
-
-  // for gg
-  typedef boost::shared_ptr<rats::gait_generator> ggPtr;
-  ggPtr gg;
-  bool gg_is_walking, gg_solved;
-  // for abc
-  hrp::Vector3 ref_cog, ref_zmp, prev_imu_sensor_pos, prev_imu_sensor_vel, hand_fix_initial_offset;
-  enum {BIPED, TROT, PACE, CRAWL, GALLOP} gait_type;
-  enum {MODE_IDLE, MODE_ABC, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_ABC} control_mode, return_control_mode;
-  std::map<std::string, ABCIKparam> ikp;
-  std::map<std::string, size_t> contact_states_index_map;
-  std::map<std::string, hrp::VirtualForceSensorParam> m_vfs;
-  std::vector<std::string> sensor_names, leg_names, ee_vec;
-  hrp::dvector qorg, qrefv;
-  hrp::Vector3 current_root_p, target_root_p;
-  hrp::Matrix33 current_root_R, target_root_R;
-  rats::coordinates fix_leg_coords;
-  std::vector<hrp::Vector3> default_zmp_offsets;
-  double m_dt, move_base_gain;
-  hrp::BodyPtr m_robot;
-  coil::Mutex m_mutex;
-
-  double transition_interpolator_ratio, transition_time, zmp_transition_time, adjust_footstep_transition_time, leg_names_interpolator_ratio;
-  interpolator *zmp_offset_interpolator;
-  interpolator *transition_interpolator;
-  interpolator *adjust_footstep_interpolator;
-  interpolator *leg_names_interpolator;
-  hrp::Vector3 input_zmp, input_basePos;
-  hrp::Matrix33 input_baseRot;
-
-  // static balance point offsetting
-  hrp::Vector3 sbp_offset, sbp_cog_offset;
-  enum {MODE_NO_FORCE, MODE_REF_FORCE} use_force;
-  std::vector<hrp::Vector3> ref_forces;
-
-  unsigned int m_debugLevel;
-  bool is_legged_robot, is_stop_mode, has_ik_failed, is_hand_fix_mode, is_hand_fix_initial;
-  int loop, ik_error_debug_print_freq;
-  bool graspless_manip_mode;
-  std::string graspless_manip_arm;
-  hrp::Vector3 graspless_manip_p_gain;
-  rats::coordinates graspless_manip_reference_trans_coords;
-  double pos_ik_thre, rot_ik_thre;
-
-  //for HumanSynchronizer
-  boost::shared_ptr<HumanSynchronizer> hsp;
-  HumanPose hp_raw_data;
-
-};
-
-extern "C"
-{
-  void AutoBalancerInit(RTC::Manager* manager);
-};
-
-#endif // IMPEDANCE_H

From 4c17e696119c3620a70e651e45c7d186910e11b1 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 15 Sep 2016 00:17:28 +0900
Subject: [PATCH 021/217] add COM to Support Region Limit with convex_hull

---
 rtc/AutoBalancer/AutoBalancer.h | 156 +++++++++++++++++++++++++++-----
 1 file changed, 135 insertions(+), 21 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 8a512648f6d..3782edb191b 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -26,6 +26,17 @@
 #include "AutoBalancerService_impl.h"
 #include "interpolator.h"
 
+//ishiguro
+#include <boost/assert.hpp>
+#include <boost/assign/list_of.hpp>
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/point_xy.hpp>
+#include <boost/geometry/geometries/box.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+#include <boost/geometry/algorithms/disjoint.hpp>
+#include <boost/geometry/geometries/geometries.hpp>
+
+
 // </rtc-template>
 
 // Service Consumer stub headers
@@ -87,6 +98,10 @@ class HumanPose{
 };
 
 
+
+
+
+
 class HumanSynchronizer{
   private:
 
@@ -109,6 +124,7 @@ class HumanSynchronizer{
     int countdown_num;
     bool HumanSyncOn;
     struct timeval t_calc_start, t_calc_end;
+    unsigned int loop;
 
   public:
 
@@ -120,7 +136,7 @@ class HumanSynchronizer{
     bool use_x,use_y,use_z;
     hrp::Vector3 init_wld_rp_basepos;
     HumanPose rp_ref_out;
-    FILE* log;
+    FILE *sr_log,*cz_log;
 
     HumanSynchronizer(){
       h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
@@ -136,20 +152,25 @@ class HumanSynchronizer{
       init_wld_rp_rfpos = hrp::Vector3(0,-0.1,0);
       init_wld_rp_lfpos = hrp::Vector3(0, 0.1,0);
       init_hp_calibcom.Zero();
-      FNUM = 0.01;
+      FNUM = 0.01;//0.01でも荒い?
       FUPLEVELNUM = 200;
       FUPHIGHT = 0.05;
       CNT_F_TH = 20.0;
       MAXVEL = 0.004;
 //      MAXVEL = 0.001;
+//      MAXVEL = 0.0001;
       HumanSyncOn = false;
       ht_first_call = true;
       startCountdownForHumanSync = false;
       countdown_num = 5*500;
-//      log = fopen("/home/ishiguro/HumanSyncLog.txt","w+");
+      loop = 0;
+      std::string home_path(std::getenv("HOME"));
+      sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
+      cz_log = fopen((home_path+"/HumanSync_com_zmp.log").c_str(),"w+");
     }
     ~HumanSynchronizer(){
-//      fclose(log);
+      fclose(sr_log);
+      fclose(cz_log);
       cout<<"HumanSynchronizer destructed"<<endl;
     }
 
@@ -179,13 +200,13 @@ class HumanSynchronizer{
       overwriteFootZFromContactStates(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);
 //      lockFootXYOnContact(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);//ここで改変するとLPFかかってないからジャンプする
       applyWorkspaceLimit(rp_ref_out, rp_ref_out);
-      applyCOMToSupportRegionLimit(rp_ref_out, rp_ref_out);
+//      applyCOMToSupportRegionLimit(rp_ref_out, rp_ref_out);
       applyVelLimit(rp_ref_out, rp_ref_out_old, rp_ref_out);
-      applyXYZLock(rp_ref_out, rp_ref_out);
-      rp_ref_out.com(2) = 0;//Z方向固定
+      applyCOMZMPXYZLock(rp_ref_out, rp_ref_out);
       rp_ref_out_old = rp_ref_out;
-//      fprintf(log,"%f %f %f %f\n", rp_ref_out.com(0), rp_ref_out.com(1), rp_ref_out.zmp(0), rp_ref_out.zmp(1));
-
+//      fprintf(log,"%f %f %f %f %f %f\n", rp_ref_out.com(0), rp_ref_out.com(1), rp_ref_out.zmp(0), rp_ref_out.zmp(1), hp_rel_raw.com(0), hp_rel_raw.com(1));
+      isCOMInSupportRegion(rp_ref_out.lf+init_wld_rp_lfpos,rp_ref_out.rf+init_wld_rp_rfpos,rp_ref_out.com);
+      loop++;
       gettimeofday(&t_calc_end, NULL);
     }
     void calibInitHumanCOMFromZMP(){
@@ -237,8 +258,12 @@ class HumanSynchronizer{
 //    }
     void applyLPFilter(const HumanPose& raw_in, const HumanPose& out_old, HumanPose& filt_out){
       for(int i=0;i<filt_out.idsize;i++){
+//      for(int i=0;i<filt_out.idsize-5;i++){//ここではrfw,zmpとかはフィルターしないほうがいい
         filt_out.Seq(i) = (1-FNUM) * out_old.Seq(i) + FNUM * raw_in.Seq(i);
       }
+//      for(int i=filt_out.idsize-5;i<filt_out.idsize;i++){//ここではrfw,zmpとかはフィルターしないほうがいい
+//        filt_out.Seq(i) = raw_in.Seq(i);
+//      }
     }
     void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
       for(int i=0;i<rp_out.idsize-4;i++){
@@ -248,7 +273,6 @@ class HumanSynchronizer{
       rp_out.lfw  = hp_in.lfw;
     }
     void judgeFootContactStates(const HumanPose::Wrench6& rfw_in, const HumanPose::Wrench6& lfw_in, bool& rfcs_ans, bool& lfcs_ans){
-      //他のプログラムは足に荷重時に力センサの値がZ軸マイナスに出るっぽい？(のでTODO FIX)
       if      (rfw_in.f(2)<CNT_F_TH && lfw_in.f(2)>CNT_F_TH){rfcs_ans = false;  lfcs_ans = true;}//右足浮遊かつ左足接地
       else if (rfw_in.f(2)>CNT_F_TH && lfw_in.f(2)<CNT_F_TH){rfcs_ans = true;   lfcs_ans = false;}//右足接地かつ左足浮遊
       else                                              {rfcs_ans = true;   lfcs_ans = true;}//両足接地または両足浮遊
@@ -316,7 +340,8 @@ class HumanSynchronizer{
       out = in;
       const double XUMARGIN = 0.08;
       const double XLMARGIN = 0.04;
-      const double YLRMARGIN = 0.03;
+//      const double YLRMARGIN = 0.03;
+      const double YLRMARGIN = 0.0;
       double x_upper_region,x_lower_region,y_upper_region,y_lower_region;
       if(rp_wld_initpos.rf(0) + in.rf(0) > rp_wld_initpos.lf(0) + in.lf(0)){
         x_upper_region = rp_wld_initpos.rf(0) + in.rf(0) + XUMARGIN;
@@ -327,10 +352,90 @@ class HumanSynchronizer{
       }
       y_upper_region = rp_wld_initpos.lf(1) + in.lf(1) + YLRMARGIN;
       y_lower_region = rp_wld_initpos.rf(1) + in.rf(1) - YLRMARGIN;
-      if(in.com(0) < x_lower_region){ out.com(0) = x_lower_region; cerr<<"[WARN] COM X Lower Limit!"<<endl;}
-      else if(in.com(0) > x_upper_region){ out.com(0) = x_upper_region; cerr<<"[WARN] COM X Upper Limit!"<<endl;}
-      if(in.com(1) < y_lower_region){ out.com(1) = y_lower_region; cerr<<"[WARN] COM Y Lower Limit!"<<endl;}
-      else if(in.com(1) > y_upper_region){ out.com(1) = y_upper_region; cerr<<"[WARN] COM Y Upper Limit!"<<endl;}
+      if(HumanSyncOn){
+        if(in.com(0) < x_lower_region){ out.com(0) = x_lower_region;}//cerr<<"[WARN] COM X Lower Limit!"<<endl;}
+        else if(in.com(0) > x_upper_region){ out.com(0) = x_upper_region; }//cerr<<"[WARN] COM X Upper Limit!"<<endl;}
+        if(in.com(1) < y_lower_region){
+          out.com(1) = y_lower_region;// cerr<<"[WARN] COM Y Lower Limit!"<<endl;
+//          out.zmp(1) = out.com(1);
+
+          if(loop%50==0)cout<<"rp_wld_initpos"<<rp_wld_initpos.lf<<endl;
+          if(loop%50==0)cout<<"y_upper_region"<<y_upper_region<<"in.com(1)"<<in.com(1)<<endl;
+        }
+        else if(in.com(1) > y_upper_region){
+         out.com(1) = y_upper_region; //cerr<<"[WARN] COM Y Upper Limit!"<<endl;
+//         out.zmp(1) = out.com(1);
+      }
+      }
+    }
+    bool isCOMInSupportRegion(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){
+      namespace bg = boost::geometry;
+      typedef bg::model::d2::point_xy<double> point;
+      typedef bg::model::polygon<point> polygon;
+      polygon lr_region,s_region;
+      const double XUMARGIN = 0.04;
+      const double XLMARGIN = -0.02;
+      const double YUMARGIN = 0.01;
+      const double YLMARGIN = -0.01;
+      bg::model::linestring<point> s_line = boost::assign::list_of<point>
+        (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YLMARGIN)//LFの右下
+        (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN)//LFの左下
+        (lfin_abs(0) + XUMARGIN, lfin_abs(1) + YUMARGIN)//LFの左上
+        (lfin_abs(0) + XUMARGIN, lfin_abs(1) + YLMARGIN)//LFの右上
+        (rfin_abs(0) + XLMARGIN, rfin_abs(1) + YLMARGIN)//RFの右下
+        (rfin_abs(0) + XLMARGIN, rfin_abs(1) + YUMARGIN)//RFの左下
+        (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YUMARGIN)//RFの左上
+        (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YLMARGIN)//RFの右上
+             ;
+      bg::convex_hull(s_line, s_region);
+
+//      std::cout<< "lr_region: " << bg::dsv(lr_region) << std::endl << "s_region: " << bg::dsv(s_region) << std::endl;
+      point com2d(comin_abs(0),comin_abs(1));
+      Eigen::Vector2d ans_point(comin_abs(0),comin_abs(1));
+      if(!bg::within(com2d, s_region)){//対象の点が凸包の外部に存在するかチェックする
+        cout<<"COM out of s_region"<<endl;
+        Eigen::Vector2d check_point(com2d.x(),com2d.y());
+        int ans_lid=-1;
+        for(int i=0;i<s_region.outer().size()-1;i++){//対象の点がある線分への垂線を有するかチェックする
+          Eigen::Vector2d cur_vert(s_region.outer()[i].x(),s_region.outer()[i].y());
+          Eigen::Vector2d next_vert(s_region.outer()[i+1].x(),s_region.outer()[i+1].y());
+          Eigen::Vector2d edge_v = next_vert - cur_vert;
+          Eigen::Vector2d tgt_pt_v = check_point - cur_vert;
+          //ある線分への垂線を有し，かつ外側(時計回りエッジに対して左側)に存在するなら，対象の点からそのエッジへの垂線の交点が最近傍点
+          if(edge_v.dot(tgt_pt_v)/edge_v.norm() > 0 && edge_v.dot(tgt_pt_v)/edge_v.norm() < edge_v.norm() && (edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
+            ans_lid = i;
+            ans_point = cur_vert + edge_v.normalized() * (edge_v.dot(tgt_pt_v)/edge_v.norm());
+            break;
+          }
+        }
+        if(ans_lid != -1){
+          cout<<"point will on the line:"<<ans_lid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
+        }else{//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
+          double cur_min_dis = bg::distance(com2d, s_region.outer()[0]);
+          int ans_pid = 0;
+          for(int i=1;i<s_region.outer().size();i++){
+            if(bg::distance(com2d, s_region.outer()[i]) < cur_min_dis){
+              cur_min_dis = bg::distance(com2d, s_region.outer()[i]);
+              ans_pid = i;
+            }
+          }
+          ans_point(0) = s_region.outer()[ans_pid].x();
+          ans_point(1) = s_region.outer()[ans_pid].y();
+          cout<<"point will on the vertex:"<<ans_pid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
+        }
+      }
+      if(loop%50==0){
+        for(int i=0;i<s_region.outer().size();i++){
+          fprintf(sr_log,"%f %f %f\n",s_region.outer().at(i).x(), s_region.outer().at(i).y(),(double)loop/500.0);
+        }
+        fprintf(sr_log,"\n");
+        fprintf(cz_log,"%f %f %f %f %f %f %f %f %f\n",(double)loop/500.0,comin_abs(0),comin_abs(1),comin_abs(2),rp_ref_out.zmp(0),rp_ref_out.zmp(1),rp_ref_out.zmp(2),ans_point(0),ans_point(1));
+        fprintf(cz_log,"\n");
+      }
+      comin_abs(0) = ans_point(0);
+      comin_abs(0) = ans_point(0);
+      rp_ref_out.zmp = rp_ref_out.com;
+      return true;
     }
     void applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out){
       for(int i=0;i<3;i++){if(in.com(i) - in_old.com(i) > MAXVEL){out.com(i) = in_old.com(i) + MAXVEL;}else if(in.com(i) - in_old.com(i) < -MAXVEL){out.com(i) = in_old.com(i) - MAXVEL;}}
@@ -339,13 +444,22 @@ class HumanSynchronizer{
       for(int i=0;i<3;i++){if(in.rh(i)  - in_old.rh(i)  > MAXVEL){out.rh(i)  = in_old.rh(i)  + MAXVEL;}else if(in.rh(i)  - in_old.rh(i)  < -MAXVEL){out.rh(i)  = in_old.rh(i)  - MAXVEL;}}
       for(int i=0;i<3;i++){if(in.lh(i)  - in_old.lh(i)  > MAXVEL){out.lh(i)  = in_old.lh(i)  + MAXVEL;}else if(in.lh(i)  - in_old.lh(i)  < -MAXVEL){out.lh(i)  = in_old.lh(i)  - MAXVEL;}}
     }
-    void applyXYZLock(const HumanPose& in, HumanPose& out){
+    void applyCOMZMPXYZLock(const HumanPose& in, HumanPose& out){
       out = in;
-      for(int i=0;i<out.idsize-4;i++){
-        if(!use_x)out.Seq(i)(0) = 0;
-        if(!use_y)out.Seq(i)(1) = 0;
-        if(!use_z)out.Seq(i)(2) = 0;
-      }
+//      for(int i=0;i<out.idsize-4;i++){
+//        if(!use_x)out.Seq(i)(0) = 0;
+//        if(!use_y)out.Seq(i)(1) = 0;
+//        if(!use_z)out.Seq(i)(2) = 0;
+//      }
+        if(!use_x){out.com(0) = 0;out.zmp(0) = 0;}
+        if(!use_y){out.com(1) = 0;out.zmp(1) = 0;}
+        if(!use_z){out.com(2) = 0;}
+        out.rh(0)=0;
+        out.rh(1)=out.com(1);
+        out.rh(2)=0;
+        out.lh(0)=0;
+        out.lh(1)=out.com(1);
+        out.lh(2)=0;
     }
 };
 

From 54e1eeb9c2208ca39b64b17972b19363db29281d Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 16 Sep 2016 19:55:16 +0900
Subject: [PATCH 022/217] fix COM limit bug

---
 rtc/AutoBalancer/AutoBalancer.h | 54 ++++++++++++++++++++++++---------
 1 file changed, 39 insertions(+), 15 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 3782edb191b..a59d54482b6 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -137,6 +137,7 @@ class HumanSynchronizer{
     hrp::Vector3 init_wld_rp_basepos;
     HumanPose rp_ref_out;
     FILE *sr_log,*cz_log;
+    hrp::Vector3 lpf_zmp;
 
     HumanSynchronizer(){
       h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
@@ -152,6 +153,7 @@ class HumanSynchronizer{
       init_wld_rp_rfpos = hrp::Vector3(0,-0.1,0);
       init_wld_rp_lfpos = hrp::Vector3(0, 0.1,0);
       init_hp_calibcom.Zero();
+      lpf_zmp.Zero();
       FNUM = 0.01;//0.01でも荒い?
       FUPLEVELNUM = 200;
       FUPHIGHT = 0.05;
@@ -203,9 +205,13 @@ class HumanSynchronizer{
 //      applyCOMToSupportRegionLimit(rp_ref_out, rp_ref_out);
       applyVelLimit(rp_ref_out, rp_ref_out_old, rp_ref_out);
       applyCOMZMPXYZLock(rp_ref_out, rp_ref_out);
+
+
+      isCOMInSupportRegion(rp_ref_out.lf+init_wld_rp_lfpos,rp_ref_out.rf+init_wld_rp_rfpos,rp_ref_out.com);
+
+
       rp_ref_out_old = rp_ref_out;
 //      fprintf(log,"%f %f %f %f %f %f\n", rp_ref_out.com(0), rp_ref_out.com(1), rp_ref_out.zmp(0), rp_ref_out.zmp(1), hp_rel_raw.com(0), hp_rel_raw.com(1));
-      isCOMInSupportRegion(rp_ref_out.lf+init_wld_rp_lfpos,rp_ref_out.rf+init_wld_rp_rfpos,rp_ref_out.com);
       loop++;
       gettimeofday(&t_calc_end, NULL);
     }
@@ -340,8 +346,7 @@ class HumanSynchronizer{
       out = in;
       const double XUMARGIN = 0.08;
       const double XLMARGIN = 0.04;
-//      const double YLRMARGIN = 0.03;
-      const double YLRMARGIN = 0.0;
+      const double YLRMARGIN = 0.03;
       double x_upper_region,x_lower_region,y_upper_region,y_lower_region;
       if(rp_wld_initpos.rf(0) + in.rf(0) > rp_wld_initpos.lf(0) + in.lf(0)){
         x_upper_region = rp_wld_initpos.rf(0) + in.rf(0) + XUMARGIN;
@@ -357,15 +362,13 @@ class HumanSynchronizer{
         else if(in.com(0) > x_upper_region){ out.com(0) = x_upper_region; }//cerr<<"[WARN] COM X Upper Limit!"<<endl;}
         if(in.com(1) < y_lower_region){
           out.com(1) = y_lower_region;// cerr<<"[WARN] COM Y Lower Limit!"<<endl;
-//          out.zmp(1) = out.com(1);
 
           if(loop%50==0)cout<<"rp_wld_initpos"<<rp_wld_initpos.lf<<endl;
           if(loop%50==0)cout<<"y_upper_region"<<y_upper_region<<"in.com(1)"<<in.com(1)<<endl;
         }
         else if(in.com(1) > y_upper_region){
          out.com(1) = y_upper_region; //cerr<<"[WARN] COM Y Upper Limit!"<<endl;
-//         out.zmp(1) = out.com(1);
-      }
+        }
       }
     }
     bool isCOMInSupportRegion(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){
@@ -373,10 +376,14 @@ class HumanSynchronizer{
       typedef bg::model::d2::point_xy<double> point;
       typedef bg::model::polygon<point> polygon;
       polygon lr_region,s_region;
-      const double XUMARGIN = 0.04;
+      const double XUMARGIN = 0.04;//CHIDORI
       const double XLMARGIN = -0.02;
       const double YUMARGIN = 0.01;
       const double YLMARGIN = -0.01;
+//      const double XUMARGIN = 0.02;//JAXON
+//      const double XLMARGIN = -0.01;
+//      const double YUMARGIN = 0.0;
+//      const double YLMARGIN = -0.0;
       bg::model::linestring<point> s_line = boost::assign::list_of<point>
         (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YLMARGIN)//LFの右下
         (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN)//LFの左下
@@ -388,12 +395,15 @@ class HumanSynchronizer{
         (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YLMARGIN)//RFの右上
              ;
       bg::convex_hull(s_line, s_region);
-
 //      std::cout<< "lr_region: " << bg::dsv(lr_region) << std::endl << "s_region: " << bg::dsv(s_region) << std::endl;
       point com2d(comin_abs(0),comin_abs(1));
       Eigen::Vector2d ans_point(comin_abs(0),comin_abs(1));
-      if(!bg::within(com2d, s_region)){//対象の点が凸包の外部に存在するかチェックする
-        cout<<"COM out of s_region"<<endl;
+      enum point_state_t {PT_IN_NO_PATTERN = -1, PT_IN_REGION, PT_FIX_TO_EDGE, PT_FIX_TO_VERTEX};
+      point_state_t pt_state = PT_IN_NO_PATTERN;
+      if(bg::within(com2d, s_region)){//対象の点が凸包の内部に存在するかチェックする
+        pt_state = PT_IN_REGION;
+      }else{
+//        cout<<"COM out of s_region"<<endl;
         Eigen::Vector2d check_point(com2d.x(),com2d.y());
         int ans_lid=-1;
         for(int i=0;i<s_region.outer().size()-1;i++){//対象の点がある線分への垂線を有するかチェックする
@@ -405,10 +415,11 @@ class HumanSynchronizer{
           if(edge_v.dot(tgt_pt_v)/edge_v.norm() > 0 && edge_v.dot(tgt_pt_v)/edge_v.norm() < edge_v.norm() && (edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
             ans_lid = i;
             ans_point = cur_vert + edge_v.normalized() * (edge_v.dot(tgt_pt_v)/edge_v.norm());
+            pt_state = PT_FIX_TO_EDGE;
             break;
           }
         }
-        if(ans_lid != -1){
+        if(pt_state == PT_FIX_TO_EDGE){
           cout<<"point will on the line:"<<ans_lid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
         }else{//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
           double cur_min_dis = bg::distance(com2d, s_region.outer()[0]);
@@ -422,19 +433,32 @@ class HumanSynchronizer{
           ans_point(0) = s_region.outer()[ans_pid].x();
           ans_point(1) = s_region.outer()[ans_pid].y();
           cout<<"point will on the vertex:"<<ans_pid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
+          pt_state = PT_FIX_TO_VERTEX;
         }
       }
+//      double FNUM_Z = 0.005;
+      double FNUM_Z = 0.01;
+      lpf_zmp = (1-FNUM_Z) * lpf_zmp + FNUM_Z * rp_ref_out.zmp;
+      hrp::Vector3 hpf_zmp = rp_ref_out.zmp - lpf_zmp;
+//      rp_ref_out.zmp(0) = ans_point(0) + hpf_zmp(0);
+//      rp_ref_out.zmp(1) = ans_point(1) + hpf_zmp(1);
+
       if(loop%50==0){
+        if(pt_state != PT_IN_REGION){
+          std::cout<<"COM out of Support Region pt_state="<<pt_state<<std::endl;
+        }
         for(int i=0;i<s_region.outer().size();i++){
           fprintf(sr_log,"%f %f %f\n",s_region.outer().at(i).x(), s_region.outer().at(i).y(),(double)loop/500.0);
         }
         fprintf(sr_log,"\n");
-        fprintf(cz_log,"%f %f %f %f %f %f %f %f %f\n",(double)loop/500.0,comin_abs(0),comin_abs(1),comin_abs(2),rp_ref_out.zmp(0),rp_ref_out.zmp(1),rp_ref_out.zmp(2),ans_point(0),ans_point(1));
-        fprintf(cz_log,"\n");
+        fprintf(cz_log,"%f %f %f %f %f %f %f %f %f",(double)loop/500.0,comin_abs(0),comin_abs(1),comin_abs(2),rp_ref_out.zmp(0),rp_ref_out.zmp(1),rp_ref_out.zmp(2),ans_point(0),ans_point(1));
+        fprintf(cz_log," %f %f %f %f",rp_ref_out.zmp(1),hpf_zmp(1),ans_point(1),ans_point(1) + hpf_zmp(1));
+        fprintf(cz_log,"\n\n");
       }
       comin_abs(0) = ans_point(0);
-      comin_abs(0) = ans_point(0);
-      rp_ref_out.zmp = rp_ref_out.com;
+      comin_abs(1) = ans_point(1);
+//      rp_ref_out.zmp(0) = comin_abs(0);
+//      rp_ref_out.zmp(1) = comin_abs(1);
       return true;
     }
     void applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out){

From 93b83b1bd22ab3697b38208affd7a29b28ee4503 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 16 Sep 2016 23:17:47 +0900
Subject: [PATCH 023/217] implement COMToSupportRegionLimit without dependency
 on latest boost

---
 rtc/AutoBalancer/AutoBalancer.h | 254 +++++++++++++++++++-------------
 1 file changed, 151 insertions(+), 103 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index a59d54482b6..091d6bcca18 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -26,16 +26,6 @@
 #include "AutoBalancerService_impl.h"
 #include "interpolator.h"
 
-//ishiguro
-#include <boost/assert.hpp>
-#include <boost/assign/list_of.hpp>
-#include <boost/geometry.hpp>
-#include <boost/geometry/geometries/point_xy.hpp>
-#include <boost/geometry/geometries/box.hpp>
-#include <boost/geometry/geometries/polygon.hpp>
-#include <boost/geometry/algorithms/disjoint.hpp>
-#include <boost/geometry/geometries/geometries.hpp>
-
 
 // </rtc-template>
 
@@ -202,12 +192,11 @@ class HumanSynchronizer{
       overwriteFootZFromContactStates(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);
 //      lockFootXYOnContact(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);//ここで改変するとLPFかかってないからジャンプする
       applyWorkspaceLimit(rp_ref_out, rp_ref_out);
-//      applyCOMToSupportRegionLimit(rp_ref_out, rp_ref_out);
       applyVelLimit(rp_ref_out, rp_ref_out_old, rp_ref_out);
       applyCOMZMPXYZLock(rp_ref_out, rp_ref_out);
 
 
-      isCOMInSupportRegion(rp_ref_out.lf+init_wld_rp_lfpos,rp_ref_out.rf+init_wld_rp_rfpos,rp_ref_out.com);
+      applyCOMToSupportRegionLimit(rp_ref_out.lf+init_wld_rp_lfpos,rp_ref_out.rf+init_wld_rp_rfpos,rp_ref_out.com);
 
 
       rp_ref_out_old = rp_ref_out;
@@ -342,97 +331,161 @@ class HumanSynchronizer{
         if(in.lf(1) + rp_wld_initpos.lf(1) < in.rf(1) + rp_wld_initpos.rf(1) + 0.15)out.lf(1) = in.rf(1) + rp_wld_initpos.rf(1) - rp_wld_initpos.lf(1) + 0.15;
       }
     }
-    void applyCOMToSupportRegionLimit(const HumanPose& in, HumanPose& out){
-      out = in;
-      const double XUMARGIN = 0.08;
-      const double XLMARGIN = 0.04;
-      const double YLRMARGIN = 0.03;
-      double x_upper_region,x_lower_region,y_upper_region,y_lower_region;
-      if(rp_wld_initpos.rf(0) + in.rf(0) > rp_wld_initpos.lf(0) + in.lf(0)){
-        x_upper_region = rp_wld_initpos.rf(0) + in.rf(0) + XUMARGIN;
-        x_lower_region = rp_wld_initpos.lf(0) + in.lf(0) - XLMARGIN;
+//    bool applyCOMToSupportRegionLimit_boost_geometry(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){
+////      ishiguro(ロボット体内のboostが古くて対応してない！！！)
+//      #include <boost/assert.hpp>
+//      #include <boost/assign/list_of.hpp>
+//      #include <boost/geometry.hpp>
+//      #include <boost/geometry/geometries/point_xy.hpp>
+//      #include <boost/geometry/geometries/box.hpp>
+//      #include <boost/geometry/geometries/polygon.hpp>
+//      #include <boost/geometry/algorithms/disjoint.hpp>
+//      #include <boost/geometry/geometries/geometries.hpp>
+//      namespace bg = boost::geometry;
+//      typedef bg::model::d2::point_xy<double> point;
+//      typedef bg::model::polygon<point> polygon;
+//      polygon lr_region,s_region;
+//      const double XUMARGIN = 0.04;//CHIDORI
+//      const double XLMARGIN = -0.02;
+//      const double YUMARGIN = 0.01;
+//      const double YLMARGIN = -0.01;
+//      bg::model::linestring<point> s_line = boost::assign::list_of<point>
+//        (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YLMARGIN)//LFの右下
+//        (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN)//LFの左下
+//        (lfin_abs(0) + XUMARGIN, lfin_abs(1) + YUMARGIN)//LFの左上
+//        (lfin_abs(0) + XUMARGIN, lfin_abs(1) + YLMARGIN)//LFの右上
+//        (rfin_abs(0) + XLMARGIN, rfin_abs(1) + YLMARGIN)//RFの右下
+//        (rfin_abs(0) + XLMARGIN, rfin_abs(1) + YUMARGIN)//RFの左下
+//        (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YUMARGIN)//RFの左上
+//        (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YLMARGIN)//RFの右上
+//             ;
+//      bg::convex_hull(s_line, s_region);
+//      point com2d(comin_abs(0),comin_abs(1));
+//      Eigen::Vector2d ans_point(comin_abs(0),comin_abs(1));
+//      enum point_state_t {PT_IN_NO_PATTERN = -1, PT_IN_REGION, PT_FIX_TO_EDGE, PT_FIX_TO_VERTEX};
+//      point_state_t pt_state = PT_IN_NO_PATTERN;
+//      if(bg::within(com2d, s_region)){//対象の点が凸包の内部に存在するかチェックする
+//        pt_state = PT_IN_REGION;
+//      }else{
+//        Eigen::Vector2d check_point(com2d.x(),com2d.y());
+//        int ans_lid=-1;
+//        for(int i=0;i<s_region.outer().size()-1;i++){//対象の点がある線分への垂線を有するかチェックする
+//          Eigen::Vector2d cur_vert(s_region.outer()[i].x(),s_region.outer()[i].y());
+//          Eigen::Vector2d next_vert(s_region.outer()[i+1].x(),s_region.outer()[i+1].y());
+//          Eigen::Vector2d edge_v = next_vert - cur_vert;
+//          Eigen::Vector2d tgt_pt_v = check_point - cur_vert;
+//          //ある線分への垂線を有し，かつ外側(時計回りエッジに対して左側)に存在するなら，対象の点からそのエッジへの垂線の交点が最近傍点
+//          if(edge_v.dot(tgt_pt_v)/edge_v.norm() > 0 && edge_v.dot(tgt_pt_v)/edge_v.norm() < edge_v.norm() && (edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
+//            ans_lid = i;
+//            ans_point = cur_vert + edge_v.normalized() * (edge_v.dot(tgt_pt_v)/edge_v.norm());
+//            pt_state = PT_FIX_TO_EDGE;
+//            break;
+//          }
+//        }
+//        if(pt_state == PT_FIX_TO_EDGE){
+//          cout<<"point will on the line:"<<ans_lid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
+//        }else{//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
+//          double cur_min_dis = bg::distance(com2d, s_region.outer()[0]);
+//          int ans_pid = 0;
+//          for(int i=1;i<s_region.outer().size();i++){
+//            if(bg::distance(com2d, s_region.outer()[i]) < cur_min_dis){
+//              cur_min_dis = bg::distance(com2d, s_region.outer()[i]);
+//              ans_pid = i;
+//            }
+//          }
+//          ans_point(0) = s_region.outer()[ans_pid].x();
+//          ans_point(1) = s_region.outer()[ans_pid].y();
+//          cout<<"point will on the vertex:"<<ans_pid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
+//          pt_state = PT_FIX_TO_VERTEX;
+//        }
+//      }
+//      if(loop%50==0){
+//        if(pt_state != PT_IN_REGION){
+//          std::cout<<"COM out of Support Region pt_state="<<pt_state<<std::endl;
+//        }
+//        for(int i=0;i<s_region.outer().size();i++){
+//          fprintf(sr_log,"%f %f %f\n",s_region.outer().at(i).x(), s_region.outer().at(i).y(),(double)loop/500.0);
+//        }
+//        fprintf(sr_log,"\n");
+//        fprintf(cz_log,"%f %f %f %f %f %f %f %f %f",(double)loop/500.0,comin_abs(0),comin_abs(1),comin_abs(2),rp_ref_out.zmp(0),rp_ref_out.zmp(1),rp_ref_out.zmp(2),ans_point(0),ans_point(1));
+//        fprintf(cz_log,"\n\n");
+//      }
+//      comin_abs(0) = ans_point(0);
+//      comin_abs(1) = ans_point(1);
+//      return true;
+//    }
+    bool applyCOMToSupportRegionLimit(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){
+//      const double XUMARGIN = 0.04;//CHIDORI
+//      const double XLMARGIN = -0.02;
+//      const double YUMARGIN = 0.01;
+//      const double YLMARGIN = -0.01;
+      const double XUMARGIN = 0.02;//JAXON
+      const double XLMARGIN = -0.01;
+      const double YUMARGIN = 0.0;
+      const double YLMARGIN = -0.0;
+      std::vector<hrp::Vector2> convex_hull;
+      if(lfin_abs(0)>rfin_abs(0)){
+        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN));//LFの左下
+        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XUMARGIN, lfin_abs(1) + YUMARGIN));//LFの左上
+        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XUMARGIN, lfin_abs(1) + YLMARGIN));//LFの右上
+        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + XUMARGIN, rfin_abs(1) + YLMARGIN));//RFの右上
+        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + XLMARGIN, rfin_abs(1) + YLMARGIN));//RFの右下
+        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + XLMARGIN, rfin_abs(1) + YUMARGIN));//RFの左下
+        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN));//LFの左下
       }else{
-        x_upper_region = rp_wld_initpos.lf(0) + in.lf(0) + XUMARGIN;
-        x_lower_region = rp_wld_initpos.rf(0) + in.rf(0) - XLMARGIN;
+        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XLMARGIN, lfin_abs(1) + YLMARGIN));//LFの右下
+        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN));//LFの左下
+        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XUMARGIN, lfin_abs(1) + YUMARGIN));//LFの左上
+        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + XUMARGIN, rfin_abs(1) + YUMARGIN));//RFの左上
+        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + XUMARGIN, rfin_abs(1) + YLMARGIN));//RFの右上
+        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + XLMARGIN, rfin_abs(1) + YLMARGIN));//RFの右下
+        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XLMARGIN, lfin_abs(1) + YLMARGIN));//LFの右下
       }
-      y_upper_region = rp_wld_initpos.lf(1) + in.lf(1) + YLRMARGIN;
-      y_lower_region = rp_wld_initpos.rf(1) + in.rf(1) - YLRMARGIN;
-      if(HumanSyncOn){
-        if(in.com(0) < x_lower_region){ out.com(0) = x_lower_region;}//cerr<<"[WARN] COM X Lower Limit!"<<endl;}
-        else if(in.com(0) > x_upper_region){ out.com(0) = x_upper_region; }//cerr<<"[WARN] COM X Upper Limit!"<<endl;}
-        if(in.com(1) < y_lower_region){
-          out.com(1) = y_lower_region;// cerr<<"[WARN] COM Y Lower Limit!"<<endl;
-
-          if(loop%50==0)cout<<"rp_wld_initpos"<<rp_wld_initpos.lf<<endl;
-          if(loop%50==0)cout<<"y_upper_region"<<y_upper_region<<"in.com(1)"<<in.com(1)<<endl;
-        }
-        else if(in.com(1) > y_upper_region){
-         out.com(1) = y_upper_region; //cerr<<"[WARN] COM Y Upper Limit!"<<endl;
+      hrp::Vector2 check_point(comin_abs(0),comin_abs(1));
+      hrp::Vector2 ans_point = check_point;
+      enum point_state_t {PT_IN_NO_PATTERN = -1, PT_IN_REGION, PT_FIX_TO_EDGE, PT_FIX_TO_VERTEX};
+      point_state_t pt_state = PT_IN_REGION;
+
+      for(int i=0;i<convex_hull.size()-1;i++){//対象の点が凸包の内部に存在するかチェックする
+        hrp::Vector2 cur_vert(convex_hull[i](0),convex_hull[i](1));
+        hrp::Vector2 next_vert(convex_hull[i+1](0),convex_hull[i+1](1));
+        hrp::Vector2 edge_v = next_vert - cur_vert;
+        hrp::Vector2 tgt_pt_v = check_point - cur_vert;
+        if((edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
+          pt_state = PT_IN_NO_PATTERN;
+//          cout<<"COM out of s_region"<<endl;
+          break;
         }
       }
-    }
-    bool isCOMInSupportRegion(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){
-      namespace bg = boost::geometry;
-      typedef bg::model::d2::point_xy<double> point;
-      typedef bg::model::polygon<point> polygon;
-      polygon lr_region,s_region;
-      const double XUMARGIN = 0.04;//CHIDORI
-      const double XLMARGIN = -0.02;
-      const double YUMARGIN = 0.01;
-      const double YLMARGIN = -0.01;
-//      const double XUMARGIN = 0.02;//JAXON
-//      const double XLMARGIN = -0.01;
-//      const double YUMARGIN = 0.0;
-//      const double YLMARGIN = -0.0;
-      bg::model::linestring<point> s_line = boost::assign::list_of<point>
-        (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YLMARGIN)//LFの右下
-        (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN)//LFの左下
-        (lfin_abs(0) + XUMARGIN, lfin_abs(1) + YUMARGIN)//LFの左上
-        (lfin_abs(0) + XUMARGIN, lfin_abs(1) + YLMARGIN)//LFの右上
-        (rfin_abs(0) + XLMARGIN, rfin_abs(1) + YLMARGIN)//RFの右下
-        (rfin_abs(0) + XLMARGIN, rfin_abs(1) + YUMARGIN)//RFの左下
-        (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YUMARGIN)//RFの左上
-        (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YLMARGIN)//RFの右上
-             ;
-      bg::convex_hull(s_line, s_region);
-//      std::cout<< "lr_region: " << bg::dsv(lr_region) << std::endl << "s_region: " << bg::dsv(s_region) << std::endl;
-      point com2d(comin_abs(0),comin_abs(1));
-      Eigen::Vector2d ans_point(comin_abs(0),comin_abs(1));
-      enum point_state_t {PT_IN_NO_PATTERN = -1, PT_IN_REGION, PT_FIX_TO_EDGE, PT_FIX_TO_VERTEX};
-      point_state_t pt_state = PT_IN_NO_PATTERN;
-      if(bg::within(com2d, s_region)){//対象の点が凸包の内部に存在するかチェックする
-        pt_state = PT_IN_REGION;
-      }else{
-//        cout<<"COM out of s_region"<<endl;
-        Eigen::Vector2d check_point(com2d.x(),com2d.y());
+
+      if(pt_state != PT_IN_REGION){
         int ans_lid=-1;
-        for(int i=0;i<s_region.outer().size()-1;i++){//対象の点がある線分への垂線を有するかチェックする
-          Eigen::Vector2d cur_vert(s_region.outer()[i].x(),s_region.outer()[i].y());
-          Eigen::Vector2d next_vert(s_region.outer()[i+1].x(),s_region.outer()[i+1].y());
-          Eigen::Vector2d edge_v = next_vert - cur_vert;
-          Eigen::Vector2d tgt_pt_v = check_point - cur_vert;
+        for(int i=0;i<convex_hull.size()-1;i++){//対象の点がある線分への垂線を有するかチェックする
+          hrp::Vector2 cur_vert(convex_hull[i](0),convex_hull[i](1));
+          hrp::Vector2 next_vert(convex_hull[i+1](0),convex_hull[i+1](1));
+          hrp::Vector2 edge_v = next_vert - cur_vert;
+          hrp::Vector2 tgt_pt_v = check_point - cur_vert;
           //ある線分への垂線を有し，かつ外側(時計回りエッジに対して左側)に存在するなら，対象の点からそのエッジへの垂線の交点が最近傍点
           if(edge_v.dot(tgt_pt_v)/edge_v.norm() > 0 && edge_v.dot(tgt_pt_v)/edge_v.norm() < edge_v.norm() && (edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
             ans_lid = i;
             ans_point = cur_vert + edge_v.normalized() * (edge_v.dot(tgt_pt_v)/edge_v.norm());
             pt_state = PT_FIX_TO_EDGE;
+//            cout<<"point will on the line:"<<ans_lid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
             break;
           }
         }
-        if(pt_state == PT_FIX_TO_EDGE){
-          cout<<"point will on the line:"<<ans_lid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
-        }else{//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
-          double cur_min_dis = bg::distance(com2d, s_region.outer()[0]);
+        if(pt_state != PT_FIX_TO_EDGE){//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
+          double cur_min_dis = (check_point - convex_hull[0]).norm();
           int ans_pid = 0;
-          for(int i=1;i<s_region.outer().size();i++){
-            if(bg::distance(com2d, s_region.outer()[i]) < cur_min_dis){
-              cur_min_dis = bg::distance(com2d, s_region.outer()[i]);
+          for(int i=1;i<convex_hull.size();i++){
+            if((check_point - convex_hull[i]).norm() < cur_min_dis){
+              cur_min_dis = (check_point - convex_hull[i]).norm();
               ans_pid = i;
             }
           }
-          ans_point(0) = s_region.outer()[ans_pid].x();
-          ans_point(1) = s_region.outer()[ans_pid].y();
-          cout<<"point will on the vertex:"<<ans_pid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
+          ans_point(0) = convex_hull[ans_pid](0);
+          ans_point(1) = convex_hull[ans_pid](1);
+//          cout<<"point will on the vertex:"<<ans_pid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
           pt_state = PT_FIX_TO_VERTEX;
         }
       }
@@ -443,18 +496,18 @@ class HumanSynchronizer{
 //      rp_ref_out.zmp(0) = ans_point(0) + hpf_zmp(0);
 //      rp_ref_out.zmp(1) = ans_point(1) + hpf_zmp(1);
 
-      if(loop%50==0){
-        if(pt_state != PT_IN_REGION){
-          std::cout<<"COM out of Support Region pt_state="<<pt_state<<std::endl;
-        }
-        for(int i=0;i<s_region.outer().size();i++){
-          fprintf(sr_log,"%f %f %f\n",s_region.outer().at(i).x(), s_region.outer().at(i).y(),(double)loop/500.0);
-        }
-        fprintf(sr_log,"\n");
-        fprintf(cz_log,"%f %f %f %f %f %f %f %f %f",(double)loop/500.0,comin_abs(0),comin_abs(1),comin_abs(2),rp_ref_out.zmp(0),rp_ref_out.zmp(1),rp_ref_out.zmp(2),ans_point(0),ans_point(1));
-        fprintf(cz_log," %f %f %f %f",rp_ref_out.zmp(1),hpf_zmp(1),ans_point(1),ans_point(1) + hpf_zmp(1));
-        fprintf(cz_log,"\n\n");
-      }
+//      if(loop%50==0){//ログ
+//        if(pt_state != PT_IN_REGION){
+//          std::cout<<"COM out of Support Region pt_state="<<pt_state<<std::endl;
+//        }
+//        for(int i=0;i<convex_hull.size();i++){
+//          fprintf(sr_log,"%f %f %f\n",convex_hull[i](0), convex_hull[i](1),(double)loop/500.0);
+//        }
+//        fprintf(sr_log,"\n");
+//        fprintf(cz_log,"%f %f %f %f %f %f %f %f %f",(double)loop/500.0,comin_abs(0),comin_abs(1),comin_abs(2),rp_ref_out.zmp(0),rp_ref_out.zmp(1),rp_ref_out.zmp(2),ans_point(0),ans_point(1));
+//        fprintf(cz_log," %f %f %f %f",rp_ref_out.zmp(1),hpf_zmp(1),ans_point(1),ans_point(1) + hpf_zmp(1));
+//        fprintf(cz_log,"\n\n");
+//      }
       comin_abs(0) = ans_point(0);
       comin_abs(1) = ans_point(1);
 //      rp_ref_out.zmp(0) = comin_abs(0);
@@ -470,11 +523,6 @@ class HumanSynchronizer{
     }
     void applyCOMZMPXYZLock(const HumanPose& in, HumanPose& out){
       out = in;
-//      for(int i=0;i<out.idsize-4;i++){
-//        if(!use_x)out.Seq(i)(0) = 0;
-//        if(!use_y)out.Seq(i)(1) = 0;
-//        if(!use_z)out.Seq(i)(2) = 0;
-//      }
         if(!use_x){out.com(0) = 0;out.zmp(0) = 0;}
         if(!use_y){out.com(1) = 0;out.zmp(1) = 0;}
         if(!use_z){out.com(2) = 0;}

From fdac1e4aaa2e0c1e36b0dd1345569da0727680f9 Mon Sep 17 00:00:00 2001
From: ishiguroJSK <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 18 Sep 2016 08:06:40 +0900
Subject: [PATCH 024/217] fix h2r_ratio update dependency

---
 idl/AutoBalancerService.idl                   |  1 +
 rtc/AutoBalancer/AutoBalancer.cpp             | 24 ++++--
 rtc/AutoBalancer/AutoBalancer.h               | 78 ++++++++++++-------
 rtc/AutoBalancer/AutoBalancerService_impl.cpp |  5 ++
 rtc/AutoBalancer/AutoBalancerService_impl.h   |  1 +
 5 files changed, 74 insertions(+), 35 deletions(-)

diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl
index afd48bec9ef..d19ac6f761f 100644
--- a/idl/AutoBalancerService.idl
+++ b/idl/AutoBalancerService.idl
@@ -423,6 +423,7 @@ module OpenHRP
      */
     boolean startHumanSyncAfter5sec();
     boolean setHumanToRobotRatio(in double h2r);
+    boolean setCOMMoveModRatio(in double cmmr);
     boolean setAllowedXYZSync(in boolean x_on, in boolean y_on, in boolean z_on);
     boolean stopHumanSync();
   };
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 638a4493673..1ccce1e9b8c 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1169,20 +1169,30 @@ bool AutoBalancer::startHumanSyncAfter5sec()
 
 bool AutoBalancer::setHumanToRobotRatio(const double h2r)
 {
-  std::cerr << "[" << m_profile.instance_name << "] set h2r_ratio as "<< h2r << std::endl;
-  if(!hsp->isHumanSyncOn())hsp->h2r_ratio = h2r;
+  std::cerr << "[" << m_profile.instance_name << "] set target h2r_ratio as "<< h2r << std::endl;
+  hsp->setTargetHumanToRobotRatio(h2r);
   return true;
 }
 
-bool AutoBalancer::setAllowedXYZSync(const bool x_on,const bool y_on,const bool z_on)
+bool AutoBalancer::setCOMMoveModRatio(const double cmmr)
 {
-  std::cerr << "[" << m_profile.instance_name << "] set allowed XYZ move direction as ("<<x_on<<","<<y_on<<","<<z_on<<")" << std::endl;
-  hsp->use_x = x_on;
-  hsp->use_y = y_on;
-  hsp->use_z = z_on;
+  std::cerr << "[" << m_profile.instance_name << "] set target com_move_mod_ratio as "<< cmmr << std::endl;
+  hsp->setTargetCOMMoveModRatio(cmmr);
   return true;
 }
 
+bool AutoBalancer::setAllowedXYZSync(const bool x_on,const bool y_on,const bool z_on)
+{
+  if(!hsp->isHumanSyncOn()){
+    std::cerr << "[" << m_profile.instance_name << "] set allowed XYZ move direction as ("<<x_on<<","<<y_on<<","<<z_on<<")" << std::endl;
+    hsp->use_x = x_on;    hsp->use_y = y_on;    hsp->use_z = z_on;
+    return true;
+  }else{
+    std::cerr << "[" << m_profile.instance_name << "] allowed XYZ move direction cannot be changed in runtime "<< std::endl;
+    return false;
+  }
+}
+
 bool AutoBalancer::stopHumanSync()
 {
 	std::cerr << "[" << m_profile.instance_name << "] stop HumanSync Now" << std::endl;
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index f60ecbd5ba3..efd59c1360a 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -80,8 +80,9 @@ class HumanPose{
       }
     }
     static void hp_printf(HumanPose& in){
-      const std::string cap[] = {"com:","rf:","lf:","rh:","lh:","zmp:","rfw:","","lfw:",""};
-      for(int i=0;i<10;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.Seq(i)(0),in.Seq(i)(1),in.Seq(i)(2)); }
+      const std::string cap[] = {"c","rf","lf","rh","lh","z","rw","","lw",""};
+      for(int i=0;i<6;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.Seq(i)(0),in.Seq(i)(1),in.Seq(i)(2)); }
+      for(int i=6;i<10;i++){ printf("\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.Seq(i)(0),in.Seq(i)(1),in.Seq(i)(2)); }
       printf("\n");
     }
     void print(){ hp_printf(*this); }
@@ -115,6 +116,9 @@ class HumanSynchronizer{
     bool HumanSyncOn;
     struct timeval t_calc_start, t_calc_end;
     unsigned int loop;
+    double h2r_ratio, tgt_h2r_ratio;
+    double cmmr, tgt_cmmr;
+    hrp::Vector3 lpf_zmp;
 
   public:
 
@@ -122,23 +126,22 @@ class HumanSynchronizer{
     bool ht_first_call;
     bool is_rf_contact,is_lf_contact;
     HumanPose rp_wld_initpos;
-    double h2r_ratio;
     bool use_x,use_y,use_z;
     hrp::Vector3 init_wld_rp_basepos;
     HumanPose rp_ref_out;
     FILE *sr_log,*cz_log;
-    hrp::Vector3 lpf_zmp;
 
     HumanSynchronizer(){
-      h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
-//      h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
-//      h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
-//      h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
+      tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
+//      tgt_h2r_ratio = h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
+//      tgt_h2r_ratio = h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
+//      tgt_h2r_ratio = h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
+      tgt_cmmr = cmmr = 1.0;
       use_x = true; use_y = true; use_z = true;
       cur_rfup_level = 0;       cur_lfup_level = 0;
       is_rf_contact = true;     is_lf_contact = true;
       pre_cont_rfpos = hrp::Vector3::Zero();    pre_cont_lfpos = hrp::Vector3::Zero();
-      init_wld_hp_rfpos = hrp::Vector3(0,-0.1/h2r_ratio,0);
+      init_wld_hp_rfpos = hrp::Vector3(0,-0.1/h2r_ratio,0);//h2r_ratioは可変なので後の値変更に注意
       init_wld_hp_lfpos = hrp::Vector3(0, 0.1/h2r_ratio,0);
       init_wld_rp_rfpos = hrp::Vector3(0,-0.1,0);
       init_wld_rp_lfpos = hrp::Vector3(0, 0.1,0);
@@ -149,20 +152,19 @@ class HumanSynchronizer{
       FUPHIGHT = 0.05;
       CNT_F_TH = 20.0;
       MAXVEL = 0.004;
-//      MAXVEL = 0.001;
-//      MAXVEL = 0.0001;
       HumanSyncOn = false;
       ht_first_call = true;
       startCountdownForHumanSync = false;
       countdown_num = 5*500;
       loop = 0;
-      std::string home_path(std::getenv("HOME"));
-      sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
-      cz_log = fopen((home_path+"/HumanSync_com_zmp.log").c_str(),"w+");
+//      std::string home_path(std::getenv("HOME"));
+//      sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
+//      cz_log = fopen((home_path+"/HumanSync_com_zmp.log").c_str(),"w+");
+      cout<<"HumanSynchronizer constructed"<<endl;
     }
     ~HumanSynchronizer(){
-      fclose(sr_log);
-      fclose(cz_log);
+//      fclose(sr_log);
+//      fclose(cz_log);
       cout<<"HumanSynchronizer destructed"<<endl;
     }
 
@@ -171,6 +173,8 @@ class HumanSynchronizer{
     const int getRemainingCountDown() const { return countdown_num; }
     const bool isHumanSyncOn() const { return HumanSyncOn; }
     const double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0; }
+    void setTargetHumanToRobotRatio(const double tgt_h2r_r_in){ tgt_h2r_ratio = tgt_h2r_r_in; }
+    void setTargetCOMMoveModRatio(const double tgt_mod_ratio){ tgt_cmmr = tgt_mod_ratio; }
     void updateCountDown(){
       if(countdown_num>0){
         countdown_num--;
@@ -181,13 +185,15 @@ class HumanSynchronizer{
     }
     void update(){
       gettimeofday(&t_calc_start, NULL);
+      updateHumanToRobotRatio(tgt_h2r_ratio);
+      updateCOMModRatio(tgt_cmmr);
       removeInitOffsetPose(hp_wld_raw, hp_wld_initpos, hp_rel_raw);
       applyInitHumanCOMOffset(hp_rel_raw, init_hp_calibcom, hp_rel_raw);
       applyLPFilter(hp_rel_raw, hp_filtered, hp_filtered);
       lockFootXYOnContact(hp_filtered, is_rf_contact, is_lf_contact, hp_filtered);//根本から改変すべき
       calcWorldZMP((hp_filtered.rf+init_wld_hp_rfpos), (hp_filtered.lf+init_wld_hp_lfpos), hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
       convertRelHumanPoseToRelRobotPose(hp_filtered,rp_ref_out);
-//      judgeFootContactStates(rp_ref_out.rfw, rp_ref_out.lfw, is_rf_contact, is_lf_contact);
+      applyCOMMoveModRatio(rp_ref_out,rp_ref_out);
       judgeFootContactStates(hp_wld_raw.rfw, hp_wld_raw.lfw, is_rf_contact, is_lf_contact);//frwはすでに1000->100Hzにフィルタリングされている
       overwriteFootZFromContactStates(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);
 //      lockFootXYOnContact(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);//ここで改変するとLPFかかってないからジャンプする
@@ -195,10 +201,8 @@ class HumanSynchronizer{
       applyVelLimit(rp_ref_out, rp_ref_out_old, rp_ref_out);
       applyCOMZMPXYZLock(rp_ref_out, rp_ref_out);
 
-
       applyCOMToSupportRegionLimit(rp_ref_out.lf+init_wld_rp_lfpos,rp_ref_out.rf+init_wld_rp_rfpos,rp_ref_out.com);
 
-
       rp_ref_out_old = rp_ref_out;
 //      fprintf(log,"%f %f %f %f %f %f\n", rp_ref_out.com(0), rp_ref_out.com(1), rp_ref_out.zmp(0), rp_ref_out.zmp(1), hp_rel_raw.com(0), hp_rel_raw.com(1));
       loop++;
@@ -234,7 +238,20 @@ class HumanSynchronizer{
     }
 
   private:
-
+    void updateHumanToRobotRatio(const double h2r_r_goal){//h2r_ratioに伴って変わる変数の処理もここに書く
+      const double step = 0.001;
+      if(h2r_r_goal - h2r_ratio > step){h2r_ratio += step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
+      else if(h2r_r_goal - h2r_ratio < (-1)*step){h2r_ratio -= step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
+      else{h2r_ratio = h2r_r_goal;}
+      init_wld_hp_rfpos = hrp::Vector3(0,-0.1/h2r_ratio,0);
+      init_wld_hp_lfpos = hrp::Vector3(0, 0.1/h2r_ratio,0);
+    }
+    void updateCOMModRatio(const double cmmr_goal){
+      const double step = 0.001;
+      if(cmmr_goal - cmmr > step){cmmr += step; cout<<"Interpolating COMMoveModRatio as "<<cmmr<<endl;}
+      else if(cmmr_goal - cmmr < (-1)*step){cmmr -= step; cout<<"Interpolating COMMoveModRatio as "<<cmmr<<endl;}
+      else{cmmr = cmmr_goal;}
+    };
     void removeInitOffsetPose(const HumanPose& abs_in, const HumanPose& init_offset, HumanPose& rel_out){
       for(int i=0;i<rel_out.idsize-5;i++){
         rel_out.Seq(i) =  abs_in.Seq(i) - init_offset.Seq(i);
@@ -267,6 +284,10 @@ class HumanSynchronizer{
       rp_out.rfw  = hp_in.rfw;
       rp_out.lfw  = hp_in.lfw;
     }
+    void applyCOMMoveModRatio(const HumanPose& in, HumanPose& out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
+      out = in;
+      out.com = cmmr * in.com;
+    }
     void judgeFootContactStates(const HumanPose::Wrench6& rfw_in, const HumanPose::Wrench6& lfw_in, bool& rfcs_ans, bool& lfcs_ans){
       if      (rfw_in.f(2)<CNT_F_TH && lfw_in.f(2)>CNT_F_TH){rfcs_ans = false;  lfcs_ans = true;}//右足浮遊かつ左足接地
       else if (rfw_in.f(2)>CNT_F_TH && lfw_in.f(2)<CNT_F_TH){rfcs_ans = true;   lfcs_ans = false;}//右足接地かつ左足浮遊
@@ -415,14 +436,14 @@ class HumanSynchronizer{
 //      return true;
 //    }
     bool applyCOMToSupportRegionLimit(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){
-//      const double XUMARGIN = 0.04;//CHIDORI
-//      const double XLMARGIN = -0.02;
-//      const double YUMARGIN = 0.01;
-//      const double YLMARGIN = -0.01;
-      const double XUMARGIN = 0.02;//JAXON
-      const double XLMARGIN = -0.01;
-      const double YUMARGIN = 0.0;
-      const double YLMARGIN = -0.0;
+      const double XUMARGIN = 0.04;//CHIDORI
+      const double XLMARGIN = -0.02;
+      const double YUMARGIN = 0.01;
+      const double YLMARGIN = -0.01;
+//      const double XUMARGIN = 0.02;//JAXON
+//      const double XLMARGIN = -0.01;
+//      const double YUMARGIN = 0.0;
+//      const double YLMARGIN = -0.0;
       std::vector<hrp::Vector2> convex_hull;
       if(lfin_abs(0)>rfin_abs(0)){
         convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN));//LFの左下
@@ -618,6 +639,7 @@ class AutoBalancer
   bool releaseEmergencyStop();
   bool startHumanSyncAfter5sec();
   bool setHumanToRobotRatio(const double h2r);
+  bool setCOMMoveModRatio(const double cmr);
   bool setAllowedXYZSync(const bool x_on,const bool y_on,const bool z_on);
   bool stopHumanSync();
 
diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.cpp b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
index a813ae1ac43..3da2b57390e 100644
--- a/rtc/AutoBalancer/AutoBalancerService_impl.cpp
+++ b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
@@ -125,6 +125,11 @@ CORBA::Boolean AutoBalancerService_impl::setHumanToRobotRatio(const CORBA::Doubl
     return m_autobalancer->setHumanToRobotRatio(h2r);
 };
 
+CORBA::Boolean AutoBalancerService_impl::setCOMMoveModRatio(const CORBA::Double cmmr)
+{
+    return m_autobalancer->setCOMMoveModRatio(cmmr);
+};
+
 CORBA::Boolean AutoBalancerService_impl::setAllowedXYZSync(const CORBA::Boolean x_on,const CORBA::Boolean y_on,const CORBA::Boolean z_on)
 {
     return m_autobalancer->setAllowedXYZSync(x_on,y_on,z_on);
diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.h b/rtc/AutoBalancer/AutoBalancerService_impl.h
index a590f8055c4..89792b47632 100644
--- a/rtc/AutoBalancer/AutoBalancerService_impl.h
+++ b/rtc/AutoBalancer/AutoBalancerService_impl.h
@@ -36,6 +36,7 @@ class AutoBalancerService_impl
   CORBA::Boolean releaseEmergencyStop();
   CORBA::Boolean startHumanSyncAfter5sec();
   CORBA::Boolean setHumanToRobotRatio(const CORBA::Double h2r);
+  CORBA::Boolean setCOMMoveModRatio(const CORBA::Double cmmr);
   CORBA::Boolean setAllowedXYZSync(const CORBA::Boolean x_on,const CORBA::Boolean y_on,const CORBA::Boolean z_on);
   CORBA::Boolean stopHumanSync();
   //

From dcc0323f26f0726b9267779f982c052a452e5dcb Mon Sep 17 00:00:00 2001
From: CHIDORI <jaxon@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 20 Sep 2016 19:20:11 +0900
Subject: [PATCH 025/217] add histeresis mod

---
 rtc/AutoBalancer/AutoBalancer.cpp.mas  | 2064 ++++++++++++++++++++++
 rtc/AutoBalancer/AutoBalancer.cpp.my   | 2218 +++++++++++++++++++++++
 rtc/AutoBalancer/AutoBalancer.cpp.save | 2219 ++++++++++++++++++++++++
 rtc/AutoBalancer/AutoBalancer.h        |   29 +-
 rtc/AutoBalancer/AutoBalancer.h.mas    |  270 +++
 rtc/KFilter.diff                       |  753 ++++++++
 rtc20837.log                           |    9 +
 7 files changed, 7551 insertions(+), 11 deletions(-)
 create mode 100644 rtc/AutoBalancer/AutoBalancer.cpp.mas
 create mode 100644 rtc/AutoBalancer/AutoBalancer.cpp.my
 create mode 100644 rtc/AutoBalancer/AutoBalancer.cpp.save
 create mode 100644 rtc/AutoBalancer/AutoBalancer.h.mas
 create mode 100644 rtc/KFilter.diff
 create mode 100644 rtc20837.log

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp.mas b/rtc/AutoBalancer/AutoBalancer.cpp.mas
new file mode 100644
index 00000000000..91353e3dd38
--- /dev/null
+++ b/rtc/AutoBalancer/AutoBalancer.cpp.mas
@@ -0,0 +1,2064 @@
+// -*- C++ -*-
+/*!
+ * @file  AutoBalancer.cpp
+ * @brief autobalancer component
+ * $Date$
+ *
+ * $Id$
+ */
+
+#include <rtm/CorbaNaming.h>
+#include <hrpModel/Link.h>
+#include <hrpModel/Sensor.h>
+#include <hrpModel/ModelLoaderUtil.h>
+#include "AutoBalancer.h"
+#include <hrpModel/JointPath.h>
+#include <hrpUtil/MatrixSolvers.h>
+#include "hrpsys/util/Hrpsys.h"
+
+
+typedef coil::Guard<coil::Mutex> Guard;
+using namespace rats;
+
+// Module specification
+// <rtc-template block="module_spec">
+static const char* autobalancer_spec[] =
+    {
+        "implementation_id", "AutoBalancer",
+        "type_name",         "AutoBalancer",
+        "description",       "autobalancer component",
+        "version",           HRPSYS_PACKAGE_VERSION,
+        "vendor",            "AIST",
+        "category",          "example",
+        "activity_type",     "DataFlowComponent",
+        "max_instance",      "10",
+        "language",          "C++",
+        "lang_type",         "compile",
+        // Configuration variables
+        "conf.default.debugLevel", "0",
+        ""
+    };
+// </rtc-template>
+
+AutoBalancer::AutoBalancer(RTC::Manager* manager)
+    : RTC::DataFlowComponentBase(manager),
+      // <rtc-template block="initializer">
+      m_qRefIn("qRef", m_qRef),
+      m_basePosIn("basePosIn", m_basePos),
+      m_baseRpyIn("baseRpyIn", m_baseRpy),
+      m_zmpIn("zmpIn", m_zmp),
+      m_optionalDataIn("optionalData", m_optionalData),
+      m_emergencySignalIn("emergencySignal", m_emergencySignal),
+      m_qOut("q", m_qRef),
+      m_zmpOut("zmpOut", m_zmp),
+      m_basePosOut("basePosOut", m_basePos),
+      m_baseRpyOut("baseRpyOut", m_baseRpy),
+      m_baseTformOut("baseTformOut", m_baseTform),
+      m_basePoseOut("basePoseOut", m_basePose),
+      m_accRefOut("accRef", m_accRef),
+      m_contactStatesOut("contactStates", m_contactStates),
+      m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime),
+      m_cogOut("cogOut", m_cog),
+      m_AutoBalancerServicePort("AutoBalancerService"),
+      m_walkingStatesOut("walkingStates", m_walkingStates),
+      m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
+      // </rtc-template>
+      gait_type(BIPED),
+      move_base_gain(0.8),
+      m_robot(hrp::BodyPtr()),
+      m_debugLevel(0)
+{
+    m_service0.autobalancer(this);
+}
+
+AutoBalancer::~AutoBalancer()
+{
+}
+
+
+RTC::ReturnCode_t AutoBalancer::onInitialize()
+{
+    std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
+    bindParameter("debugLevel", m_debugLevel, "0");
+
+    // Registration: InPort/OutPort/Service
+    // <rtc-template block="registration">
+    // Set InPort buffers
+    addInPort("qRef", m_qRefIn);
+    addInPort("basePosIn", m_basePosIn);
+    addInPort("baseRpyIn", m_baseRpyIn);
+    addInPort("zmpIn", m_zmpIn);
+    addInPort("optionalData", m_optionalDataIn);
+    addInPort("emergencySignal", m_emergencySignalIn);
+
+    // Set OutPort buffer
+    addOutPort("q", m_qOut);
+    addOutPort("zmpOut", m_zmpOut);
+    addOutPort("basePosOut", m_basePosOut);
+    addOutPort("baseRpyOut", m_baseRpyOut);
+    addOutPort("baseTformOut", m_baseTformOut);
+    addOutPort("basePoseOut", m_basePoseOut);
+    addOutPort("accRef", m_accRefOut);
+    addOutPort("contactStates", m_contactStatesOut);
+    addOutPort("controlSwingSupportTime", m_controlSwingSupportTimeOut);
+    addOutPort("cogOut", m_cogOut);
+    addOutPort("walkingStates", m_walkingStatesOut);
+    addOutPort("sbpCogOffset", m_sbpCogOffsetOut);
+  
+    // Set service provider to Ports
+    m_AutoBalancerServicePort.registerProvider("service0", "AutoBalancerService", m_service0);
+  
+    // Set service consumers to Ports
+  
+    // Set CORBA Service Ports
+    addPort(m_AutoBalancerServicePort);
+  
+    // </rtc-template>
+    // <rtc-template block="bind_config">
+    // Bind variables and configuration variable
+  
+    // </rtc-template>
+
+    RTC::Properties& prop =  getProperties();
+    coil::stringTo(m_dt, prop["dt"].c_str());
+
+    m_robot = hrp::BodyPtr(new hrp::Body());
+    RTC::Manager& rtcManager = RTC::Manager::instance();
+    std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
+    int comPos = nameServer.find(",");
+    if (comPos < 0){
+        comPos = nameServer.length();
+    }
+    nameServer = nameServer.substr(0, comPos);
+    RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
+    if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
+                                 CosNaming::NamingContext::_duplicate(naming.getRootContext())
+                                 )){
+      std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
+      return RTC::RTC_ERROR;
+    }
+
+    // allocate memory for outPorts
+    m_qRef.data.length(m_robot->numJoints());
+    qorg.resize(m_robot->numJoints());
+    qrefv.resize(m_robot->numJoints());
+    m_baseTform.data.length(12);
+
+    control_mode = MODE_IDLE;
+    loop = 0;
+
+    // setting from conf file
+    // GaitGenerator requires abc_leg_offset and abc_stride_parameter in robot conf file
+    // setting leg_pos from conf file
+    coil::vstring leg_offset_str = coil::split(prop["abc_leg_offset"], ",");
+    leg_names.push_back("rleg");
+    leg_names.push_back("lleg");
+
+    // setting from conf file
+    // rleg,TARGET_LINK,BASE_LINK
+    coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
+    size_t prop_num = 10;
+    if (end_effectors_str.size() > 0) {
+      size_t num = end_effectors_str.size()/prop_num;
+      for (size_t i = 0; i < num; i++) {
+        std::string ee_name, ee_target, ee_base;
+        coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
+        coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
+        coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
+        ABCIKparam tp;
+        for (size_t j = 0; j < 3; j++) {
+          coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
+        }
+        double tmpv[4];
+        for (int j = 0; j < 4; j++ ) {
+          coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
+        }
+        tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
+        tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
+        // Fix for toe joint
+        //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
+        //   Without toe joints, "end-effector link == force-sensor link" is assumed.
+        //   With toe joints, "end-effector link != force-sensor link" is assumed.
+        if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
+            std::vector<double> optw(tp.manip->numJoints(), 1.0);
+            optw.back() = 0.0; // Set weight = 0 for toe joint by default
+            tp.manip->setOptionalWeightVector(optw);
+            tp.has_toe_joint = true;
+        } else {
+            tp.has_toe_joint = false;
+        }
+        tp.avoid_gain = 0.001;
+        tp.reference_gain = 0.01;
+        tp.pos_ik_error_count = tp.rot_ik_error_count = 0;
+        ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
+        ikp[ee_name].target_link = m_robot->link(ee_target);
+        ee_vec.push_back(ee_name);
+        std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   target = " << ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
+        contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
+      }
+      m_contactStates.data.length(num);
+      if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
+        m_contactStates.data[contact_states_index_map["rleg"]] = true;
+        m_contactStates.data[contact_states_index_map["lleg"]] = true;
+      }
+      if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) {
+        m_contactStates.data[contact_states_index_map["rarm"]] = false;
+        m_contactStates.data[contact_states_index_map["larm"]] = false;
+      }
+      m_controlSwingSupportTime.data.length(num);
+      for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 0.0;
+    }
+    std::vector<hrp::Vector3> leg_pos;
+    if (leg_offset_str.size() > 0) {
+      hrp::Vector3 leg_offset;
+      for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
+      std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+      leg_pos.push_back(hrp::Vector3(-1*leg_offset));
+      leg_pos.push_back(hrp::Vector3(leg_offset));
+    }
+    if (leg_pos.size() < ikp.size()) {
+        size_t tmp_leg_pos_size = leg_pos.size();
+        for (size_t i = 0; i < ikp.size() - tmp_leg_pos_size; i++) {
+            leg_pos.push_back(hrp::Vector3::Zero());
+        }
+    }
+
+    std::vector<std::pair<hrp::Link*, hrp::Link*> > interlocking_joints;
+    readInterlockingJointsParamFromProperties(interlocking_joints, m_robot, prop["interlocking_joints"], std::string(m_profile.instance_name));
+    if (interlocking_joints.size() > 0) {
+        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            std::cerr << "[" << m_profile.instance_name << "] Interlocking Joints for [" << it->first << "]" << std::endl;
+            it->second.manip->setInterlockingJointPairIndices(interlocking_joints, std::string(m_profile.instance_name));
+        }
+    }
+
+    zmp_offset_interpolator = new interpolator(ikp.size()*3, m_dt);
+    zmp_offset_interpolator->setName(std::string(m_profile.instance_name)+" zmp_offset_interpolator");
+    zmp_transition_time = 1.0;
+    transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator");
+    transition_interpolator_ratio = 1.0;
+    adjust_footstep_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    adjust_footstep_interpolator->setName(std::string(m_profile.instance_name)+" adjust_footstep_interpolator");
+    transition_time = 2.0;
+    adjust_footstep_transition_time = 2.0;
+    leg_names_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator");
+    leg_names_interpolator_ratio = 1.0;
+
+    // setting stride limitations from conf file
+    double stride_fwd_x_limit = 0.15;
+    double stride_y_limit = 0.05;
+    double stride_th_limit = 10;
+    double stride_bwd_x_limit = 0.05;
+    std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_y_limit << "[m], " << stride_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl;
+    if (default_zmp_offsets.size() == 0) {
+      for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
+    }
+    if (leg_offset_str.size() > 0) {
+      gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_y_limit/*[m]*/, stride_th_limit/*[deg]*/, stride_bwd_x_limit/*[m]*/));
+      gg->set_default_zmp_offsets(default_zmp_offsets);
+    }
+    gg_is_walking = gg_solved = false;
+    m_walkingStates.data = false;
+    fix_leg_coords = coordinates();
+
+    // load virtual force sensors
+    readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
+    // ref force port
+    int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
+    int nvforce = m_vfs.size();
+    int nforce  = npforce + nvforce;
+    m_ref_force.resize(nforce);
+    m_ref_forceIn.resize(nforce);
+    m_force.resize(nforce);
+    m_ref_forceOut.resize(nforce);
+    m_limbCOPOffset.resize(nforce);
+    m_limbCOPOffsetOut.resize(nforce);
+    for (unsigned int i=0; i<npforce; i++){
+        sensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
+    }
+    for (unsigned int i=0; i<nvforce; i++){
+        for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
+            if (it->second.id == i) sensor_names.push_back(it->first);
+        }
+    }
+    // set ref force port
+    std::cerr << "[" << m_profile.instance_name << "] force sensor ports (" << nforce << ")" << std::endl;
+    for (unsigned int i=0; i<nforce; i++){
+        m_ref_forceIn[i] = new InPort<TimedDoubleSeq>(std::string("ref_"+sensor_names[i]).c_str(), m_ref_force[i]);
+        m_ref_force[i].data.length(6);
+        registerInPort(std::string("ref_"+sensor_names[i]).c_str(), *m_ref_forceIn[i]);
+        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string("ref_"+sensor_names[i]) << std::endl;
+        ref_forces.push_back(hrp::Vector3(0,0,0));
+    }
+    // set force port
+    for (unsigned int i=0; i<nforce; i++){
+        m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string(sensor_names[i]).c_str(), m_force[i]);
+        m_force[i].data.length(6);
+        m_force[i].data[0] = m_force[i].data[1] = m_force[i].data[2] = 0.0;
+        m_force[i].data[3] = m_force[i].data[4] = m_force[i].data[5] = 0.0;
+        registerOutPort(std::string(sensor_names[i]).c_str(), *m_ref_forceOut[i]);
+        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string(sensor_names[i]) << std::endl;
+    }
+    // set limb cop offset port
+    std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << nforce << ")" << std::endl;
+    for (unsigned int i=0; i<nforce; i++){
+        std::string nm("limbCOPOffset_"+sensor_names[i]);
+        m_limbCOPOffsetOut[i] = new OutPort<TimedPoint3D>(nm.c_str(), m_limbCOPOffset[i]);
+        registerOutPort(nm.c_str(), *m_limbCOPOffsetOut[i]);
+        m_limbCOPOffset[i].data.x = m_limbCOPOffset[i].data.y = m_limbCOPOffset[i].data.z = 0.0;
+        std::cerr << "[" << m_profile.instance_name << "]   name = " << nm << std::endl;
+    }
+    sbp_offset = hrp::Vector3(0,0,0);
+    sbp_cog_offset = hrp::Vector3(0,0,0);
+    //use_force = MODE_NO_FORCE;
+    use_force = MODE_REF_FORCE;
+
+    if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
+      is_legged_robot = true;
+    } else {
+      is_legged_robot = false;
+    }
+
+    m_accRef.data.ax = m_accRef.data.ay = m_accRef.data.az = 0.0;
+    prev_imu_sensor_vel = hrp::Vector3::Zero();
+
+    graspless_manip_mode = false;
+    graspless_manip_arm = "arms";
+    graspless_manip_p_gain = hrp::Vector3::Zero();
+
+    is_stop_mode = false;
+    has_ik_failed = false;
+    is_hand_fix_mode = false;
+
+    pos_ik_thre = 0.1*1e-3; // [m]
+    rot_ik_thre = (1e-2)*M_PI/180.0; // [rad]
+    ik_error_debug_print_freq = static_cast<int>(0.2/m_dt); // once per 0.2 [s]
+
+    return RTC::RTC_OK;
+}
+
+
+
+RTC::ReturnCode_t AutoBalancer::onFinalize()
+{
+  delete zmp_offset_interpolator;
+  delete transition_interpolator;
+  delete adjust_footstep_interpolator;
+  delete leg_names_interpolator;
+  return RTC::RTC_OK;
+}
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onStartup(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onShutdown(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id)
+{
+    std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
+    
+    return RTC::RTC_OK;
+}
+
+RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
+{
+  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
+  Guard guard(m_mutex);
+  if (control_mode == MODE_ABC) {
+    control_mode = MODE_SYNC_TO_IDLE;
+    double tmp_ratio = 0.0;
+    transition_interpolator->go(&tmp_ratio, m_dt, true); // sync in one controller loop
+  }
+  return RTC::RTC_OK;
+}
+
+#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
+//#define DEBUGP2 ((loop%200==0))
+#define DEBUGP2 (false)
+RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
+{
+  // std::cerr << "AutoBalancer::onExecute(" << ec_id << ")" << std::endl;
+    loop ++;
+    if (m_qRefIn.isNew()) {
+        m_qRefIn.read();
+    }
+    if (m_basePosIn.isNew()) {
+      m_basePosIn.read();
+      input_basePos(0) = m_basePos.data.x;
+      input_basePos(1) = m_basePos.data.y;
+      input_basePos(2) = m_basePos.data.z;
+    }
+    if (m_baseRpyIn.isNew()) {
+      m_baseRpyIn.read();
+      input_baseRot = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
+    }
+    if (m_zmpIn.isNew()) {
+      m_zmpIn.read();
+      input_zmp(0) = m_zmp.data.x;
+      input_zmp(1) = m_zmp.data.y;
+      input_zmp(2) = m_zmp.data.z;
+    }
+    for (unsigned int i=0; i<m_ref_forceIn.size(); i++){
+        if ( m_ref_forceIn[i]->isNew() ) {
+            m_ref_forceIn[i]->read();
+        }
+    }
+    if (m_optionalDataIn.isNew()) {
+        m_optionalDataIn.read();
+        if (is_legged_robot) {
+          if (m_optionalData.data.length() >= contact_states_index_map.size()*2) {
+            // current optionalData is contactstates x limb and controlSwingSupportTime x limb
+            //   If contactStates in optionalData is 1.0, m_contactStates is true. Otherwise, false.
+            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+                m_contactStates.data[contact_states_index_map[it->first]] = isOptionalDataContact(it->first);
+                m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = m_optionalData.data[contact_states_index_map[it->first]+contact_states_index_map.size()];
+            }
+            if ( !m_contactStates.data[contact_states_index_map["rleg"]] && !m_contactStates.data[contact_states_index_map["lleg"]] ) { // If two feet have no contact, force set double support contact
+              m_contactStates.data[contact_states_index_map["rleg"]] = true;
+              m_contactStates.data[contact_states_index_map["lleg"]] = true;
+            }
+          }
+        }
+    }
+    if (m_emergencySignalIn.isNew()){
+        m_emergencySignalIn.read();
+        // if (!is_stop_mode) {
+        //     std::cerr << "[" << m_profile.instance_name << "] emergencySignal is set!" << std::endl;
+        //     is_stop_mode = true;
+        //     gg->emergency_stop();
+        // }
+    }
+
+    Guard guard(m_mutex);
+    hrp::Vector3 ref_basePos;
+    hrp::Matrix33 ref_baseRot;
+    hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
+    if ( is_legged_robot ) {
+      gg->proc_zmp_weight_map_interpolation();
+      getCurrentParameters();
+      getTargetParameters();
+      bool is_transition_interpolator_empty = transition_interpolator->isEmpty();
+      if (!is_transition_interpolator_empty) {
+        transition_interpolator->get(&transition_interpolator_ratio, true);
+      } else {
+        transition_interpolator_ratio = 1.0;
+      }
+      if (control_mode != MODE_IDLE ) {
+        solveLimbIK();
+        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
+      } else {
+        rel_ref_zmp = input_zmp;
+      }
+      // transition
+      if (!is_transition_interpolator_empty) {
+        // transition_interpolator_ratio 0=>1 : IDLE => ABC
+        // transition_interpolator_ratio 1=>0 : ABC => IDLE
+        ref_basePos = (1-transition_interpolator_ratio) * input_basePos + transition_interpolator_ratio * m_robot->rootLink()->p;
+        rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp;
+        rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R);
+        for ( int i = 0; i < m_robot->numJoints(); i++ ) {
+          m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q;
+        }
+      } else {
+        ref_basePos = m_robot->rootLink()->p;
+        ref_baseRot = m_robot->rootLink()->R;
+      }
+      // mode change for sync
+      if (control_mode == MODE_SYNC_TO_ABC) {
+        control_mode = MODE_ABC;
+      } else if (control_mode == MODE_SYNC_TO_IDLE && transition_interpolator->isEmpty() ) {
+        std::cerr << "[" << m_profile.instance_name << "] Finished cleanup" << std::endl;
+        control_mode = MODE_IDLE;
+      }
+    }
+    if ( m_qRef.data.length() != 0 ) { // initialized
+      if (is_legged_robot) {
+        for ( int i = 0; i < m_robot->numJoints(); i++ ){
+          m_qRef.data[i] = m_robot->joint(i)->q;
+        }
+      }
+      m_qOut.write();
+    }
+    if (is_legged_robot) {
+      // basePos
+      m_basePos.data.x = ref_basePos(0);
+      m_basePos.data.y = ref_basePos(1);
+      m_basePos.data.z = ref_basePos(2);
+      m_basePos.tm = m_qRef.tm;
+      // baseRpy
+      hrp::Vector3 baseRpy = hrp::rpyFromRot(ref_baseRot);
+      m_baseRpy.data.r = baseRpy(0);
+      m_baseRpy.data.p = baseRpy(1);
+      m_baseRpy.data.y = baseRpy(2);
+      m_baseRpy.tm = m_qRef.tm;
+      // baseTform
+      double *tform_arr = m_baseTform.data.get_buffer();
+      tform_arr[0] = m_basePos.data.x;
+      tform_arr[1] = m_basePos.data.y;
+      tform_arr[2] = m_basePos.data.z;
+      hrp::setMatrix33ToRowMajorArray(ref_baseRot, tform_arr, 3);
+      m_baseTform.tm = m_qRef.tm;
+      // basePose
+      m_basePose.data.position.x = m_basePos.data.x;
+      m_basePose.data.position.y = m_basePos.data.y;
+      m_basePose.data.position.z = m_basePos.data.z;
+      m_basePose.data.orientation.r = m_baseRpy.data.r;
+      m_basePose.data.orientation.p = m_baseRpy.data.p;
+      m_basePose.data.orientation.y = m_baseRpy.data.y;
+      m_basePose.tm = m_qRef.tm;
+      // zmp
+      m_zmp.data.x = rel_ref_zmp(0);
+      m_zmp.data.y = rel_ref_zmp(1);
+      m_zmp.data.z = rel_ref_zmp(2);
+      m_zmp.tm = m_qRef.tm;
+      // cog
+      m_cog.data.x = ref_cog(0);
+      m_cog.data.y = ref_cog(1);
+      m_cog.data.z = ref_cog(2);
+      m_cog.tm = m_qRef.tm;
+      // sbpCogOffset
+      m_sbpCogOffset.data.x = sbp_cog_offset(0);
+      m_sbpCogOffset.data.y = sbp_cog_offset(1);
+      m_sbpCogOffset.data.z = sbp_cog_offset(2);
+      m_sbpCogOffset.tm = m_qRef.tm;
+    }
+    m_basePosOut.write();
+    m_baseRpyOut.write();
+    m_baseTformOut.write();
+    m_basePoseOut.write();
+    m_zmpOut.write();
+    m_cogOut.write();
+    m_sbpCogOffsetOut.write();
+
+    // reference acceleration
+    hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
+    if (sen != NULL) {
+      hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos;
+      hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt;
+      // convert to imu sensor local acceleration
+      hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt;
+      m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2);
+      m_accRefOut.write();
+      prev_imu_sensor_pos = imu_sensor_pos;
+      prev_imu_sensor_vel = imu_sensor_vel;
+    }
+
+    // control parameters
+    m_contactStates.tm = m_qRef.tm;
+    m_contactStatesOut.write();
+    m_controlSwingSupportTime.tm = m_qRef.tm;
+    m_controlSwingSupportTimeOut.write();
+    m_walkingStates.data = gg_is_walking;
+    m_walkingStates.tm = m_qRef.tm;
+    m_walkingStatesOut.write();
+
+    for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
+        m_force[i].tm = m_qRef.tm;
+        for (unsigned int j=0; j<6; j++){
+            if (control_mode != MODE_IDLE) m_force[i].data[j] = transition_interpolator_ratio * m_force[i].data[j] + (1-transition_interpolator_ratio) * m_ref_force[i].data[j];
+            else m_force[i].data[j] = m_ref_force[i].data[j];
+        }
+        m_ref_forceOut[i]->write();
+    }
+
+    for (unsigned int i=0; i<m_limbCOPOffsetOut.size(); i++){
+        m_limbCOPOffset[i].tm = m_qRef.tm;
+        m_limbCOPOffsetOut[i]->write();
+    }
+
+    return RTC::RTC_OK;
+}
+
+void AutoBalancer::getCurrentParameters()
+{
+  current_root_p = m_robot->rootLink()->p;
+  current_root_R = m_robot->rootLink()->R;
+  for ( int i = 0; i < m_robot->numJoints(); i++ ){
+    qorg[i] = m_robot->joint(i)->q;
+  }
+}
+
+void AutoBalancer::getTargetParameters()
+{
+  // joint angles
+  for ( int i = 0; i < m_robot->numJoints(); i++ ){
+    m_robot->joint(i)->q = m_qRef.data[i];
+    qrefv[i] = m_robot->joint(i)->q;
+  }
+  // basepos, rot, zmp
+  m_robot->rootLink()->p = input_basePos;
+  m_robot->rootLink()->R = input_baseRot;
+  m_robot->calcForwardKinematics();
+  //
+  if (control_mode != MODE_IDLE) {
+    coordinates tmp_fix_coords;
+    if (!zmp_offset_interpolator->isEmpty()) {
+      double *default_zmp_offsets_output = new double[ikp.size()*3];
+      zmp_offset_interpolator->get(default_zmp_offsets_output, true);
+      for (size_t i = 0; i < ikp.size(); i++)
+        for (size_t j = 0; j < 3; j++)
+          default_zmp_offsets[i](j) = default_zmp_offsets_output[i*3+j];
+      delete[] default_zmp_offsets_output;
+      if (DEBUGP) {
+        std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl;
+        std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+        for (size_t i = 0; i < leg_names.size(); i++) {
+            std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
+            std::cerr << "[" << m_profile.instance_name << "]   " << leg_names[i] << " = " << default_zmp_offsets[dst->first].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+        }
+      }
+    }
+    if (!leg_names_interpolator->isEmpty()) {
+        leg_names_interpolator->get(&leg_names_interpolator_ratio, true);
+    }else {
+        leg_names_interpolator_ratio = 1.0;
+    }
+    if ( gg_is_walking ) {
+      gg->set_default_zmp_offsets(default_zmp_offsets);
+      gg_solved = gg->proc_one_tick();
+      {
+          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+          coordinates tmpc;
+          // for support leg
+          for (std::vector<step_node>::const_iterator it = gg->get_support_leg_steps().begin(); it != gg->get_support_leg_steps().end(); it++) {
+              coordinates sp_coords = it->worldcoords;
+              coordinates(ikp[leg_type_map[it->l_r]].localPos,
+                          ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc);
+              sp_coords.transform(tmpc);
+              ikp[leg_type_map[it->l_r]].target_p0 = sp_coords.pos;
+              ikp[leg_type_map[it->l_r]].target_r0 = sp_coords.rot;
+          }
+          // for swing leg
+          for (std::vector<step_node>::const_iterator it = gg->get_swing_leg_steps().begin(); it != gg->get_swing_leg_steps().end(); it++) {
+              coordinates sw_coords = it->worldcoords;
+              coordinates(ikp[leg_type_map[it->l_r]].localPos,
+                          ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc);
+              sw_coords.transform(tmpc);
+              ikp[leg_type_map[it->l_r]].target_p0 = sw_coords.pos;
+              ikp[leg_type_map[it->l_r]].target_r0 = sw_coords.rot;
+          }
+      }
+      gg->get_swing_support_mid_coords(tmp_fix_coords);
+      // set contactStates
+      {
+          std::vector<std::string> tmp_current_support_states_names;
+          {
+              std::vector<leg_type> tmp_current_support_states = gg->get_current_support_states();
+              std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+              for (std::vector<leg_type>::const_iterator it = tmp_current_support_states.begin(); it != tmp_current_support_states.end(); it++)
+                  tmp_current_support_states_names.push_back(leg_type_map[*it]);
+          }
+          // Set Contact States for ee not included in leg_names to false
+          for ( std::map<std::string, size_t>::iterator it = contact_states_index_map.begin(); it != contact_states_index_map.end(); it++ ) {
+              m_contactStates.data[it->second] = false;
+          }
+          for (std::vector<std::string>::const_iterator it = leg_names.begin(); it != leg_names.end(); it++) {
+              std::vector<std::string>::const_iterator dst = std::find_if(tmp_current_support_states_names.begin(), tmp_current_support_states_names.end(), boost::lambda::_1 == *it);
+              if (dst != tmp_current_support_states_names.end()) {
+                  m_contactStates.data[contact_states_index_map[*it]] = true;
+              } else {
+                  m_contactStates.data[contact_states_index_map[*it]] = false;
+              }
+          }
+      }
+      // set controlSwingSupportTime
+      {
+          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+          for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
+              std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
+              m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = gg->get_current_swing_time(dst->first);
+          }
+      }
+      // set limbCOPOffset
+      {
+          std::vector<std::string> swg_leg_nms = gg->get_swing_leg_names();
+          for (size_t i = 0; i < swg_leg_nms.size(); i++) {
+              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.x = gg->get_swing_foot_zmp_offsets().at(i)(0);
+              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.y = gg->get_swing_foot_zmp_offsets().at(i)(1);
+              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.z = gg->get_swing_foot_zmp_offsets().at(i)(2);
+          }
+      }
+      {
+          std::vector<std::string> sup_leg_nms = gg->get_support_leg_names();
+          for (size_t i = 0; i < sup_leg_nms.size(); i++) {
+              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.x = gg->get_support_foot_zmp_offsets().at(i)(0);
+              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.y = gg->get_support_foot_zmp_offsets().at(i)(1);
+              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.z = gg->get_support_foot_zmp_offsets().at(i)(2);
+          }
+      }
+    } else {
+      tmp_fix_coords = fix_leg_coords;
+      // double support by default
+      {
+          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+          for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
+              std::vector<std::string>::const_iterator dst = std::find_if(leg_names.begin(), leg_names.end(), (boost::lambda::_1 == it->first));
+              if (dst != leg_names.end()) {
+                  m_contactStates.data[contact_states_index_map[it->first]] = true;
+              } else {
+                  m_contactStates.data[contact_states_index_map[it->first]] = false;
+              }
+              // controlSwingSupportTime is not used while double support period, 1.0 is neglected
+              m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = 1.0;
+              std::map<leg_type, std::string>::const_iterator dst2 = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
+              m_limbCOPOffset[contact_states_index_map[it->first]].data.x = default_zmp_offsets.at(dst2->first)(0);
+              m_limbCOPOffset[contact_states_index_map[it->first]].data.y = default_zmp_offsets.at(dst2->first)(1);
+              m_limbCOPOffset[contact_states_index_map[it->first]].data.z = default_zmp_offsets.at(dst2->first)(2);
+          }
+      }
+    }
+    if (!adjust_footstep_interpolator->isEmpty()) {
+        double tmp = 0.0;
+        adjust_footstep_interpolator->get(&tmp, true);
+        //std::cerr << "[" << m_profile.instance_name << "] adjust ratio " << tmp << std::endl;
+        ikp["rleg"].target_p0 = (1-tmp) * ikp["rleg"].adjust_interpolation_org_p0 + tmp*ikp["rleg"].adjust_interpolation_target_p0;
+        ikp["lleg"].target_p0 = (1-tmp) * ikp["lleg"].adjust_interpolation_org_p0 + tmp*ikp["lleg"].adjust_interpolation_target_p0;
+        rats::mid_rot(ikp["rleg"].target_r0, tmp, ikp["rleg"].adjust_interpolation_org_r0, ikp["rleg"].adjust_interpolation_target_r0);
+        rats::mid_rot(ikp["lleg"].target_r0, tmp, ikp["lleg"].adjust_interpolation_org_r0, ikp["lleg"].adjust_interpolation_target_r0);
+        coordinates tmprc, tmplc;
+        tmprc.pos = ikp["rleg"].target_p0 + ikp["rleg"].target_r0 * ikp["rleg"].localPos;
+        tmprc.rot = ikp["rleg"].target_r0 * ikp["rleg"].localR;
+        tmplc.pos = ikp["lleg"].target_p0 + ikp["lleg"].target_r0 * ikp["lleg"].localPos;
+        tmplc.rot = ikp["lleg"].target_r0 * ikp["lleg"].localR;
+        rats::mid_coords(fix_leg_coords, 0.5, tmprc, tmplc);
+        tmp_fix_coords = fix_leg_coords;
+    }
+    // Tempolarily modify tmp_fix_coords
+    // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272
+    {
+      hrp::Vector3 ex = hrp::Vector3::UnitX();
+      hrp::Vector3 ez = hrp::Vector3::UnitZ();
+      hrp::Vector3 xv1 (tmp_fix_coords.rot * ex);
+      xv1(2) = 0.0;
+      xv1.normalize();
+      hrp::Vector3 yv1(ez.cross(xv1));
+      tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2);
+      tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2);
+      tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2);
+    }
+    // Fix pos
+    fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot);
+
+    /* update ref_forces ;; sp's absolute -> rmc's absolute */
+    for (size_t i = 0; i < m_ref_forceIn.size(); i++) {
+      hrp::Matrix33 eeR;
+      hrp::Link* parentlink;
+      hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
+      if (sensor) parentlink = sensor->link;
+      else parentlink = m_vfs[sensor_names[i]].link;
+      for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+          if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR;
+      }
+      // End effector frame
+      //ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
+      // world frame
+      ref_forces[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
+    }
+    sbp_offset = tmp_fix_coords.rot * hrp::Vector3(sbp_offset);
+
+    target_root_p = m_robot->rootLink()->p;
+    target_root_R = m_robot->rootLink()->R;
+    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+      if ( control_mode == MODE_IDLE || std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end() ) {
+        it->second.target_p0 = it->second.target_link->p;
+        it->second.target_r0 = it->second.target_link->R;
+      }
+    }
+    // Just for ik initial value
+    if (control_mode == MODE_SYNC_TO_ABC) {
+        current_root_p = target_root_p;
+        current_root_R = target_root_R;
+        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
+                it->second.target_p0 = it->second.target_link->p;
+                it->second.target_r0 = it->second.target_link->R;
+            }
+        }
+    }
+    // Move hand for hand fix mode
+    //   If arms' ABCIKparam.is_active is true, move hands according to cog velocity.
+    //   If is_hand_fix_mode is false, no hand fix mode and move hands according to cog velocity.
+    //   If is_hand_fix_mode is true, hand fix mode and do not move hands in Y axis in tmp_fix_coords.rot.    
+    if (gg_is_walking) {
+        // hand control while walking = solve hand ik with is_hand_fix_mode and solve hand ik without is_hand_fix_mode
+        bool is_hand_control_while_walking = false;
+        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
+                 && it->first.find("arm") != std::string::npos ) {
+                is_hand_control_while_walking = true;
+            }
+        }
+        if (is_hand_control_while_walking) {
+        //if (false) { // Disabled temporarily
+            // Store hand_fix_initial_offset in the initialization of walking
+            if (is_hand_fix_initial) hand_fix_initial_offset = tmp_fix_coords.rot.transpose() * (hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos);
+            is_hand_fix_initial = false;
+            hrp::Vector3 dif_p = hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos - tmp_fix_coords.rot * hand_fix_initial_offset;
+            if (is_hand_fix_mode) {
+                dif_p = tmp_fix_coords.rot.transpose() * dif_p;
+                dif_p(1) = 0;
+                dif_p = tmp_fix_coords.rot * dif_p;
+            }
+            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+                if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
+                     && it->first.find("arm") != std::string::npos ) {
+                    it->second.target_p0 = it->second.target_p0 + dif_p;
+                }
+            }
+        }
+    }
+
+    // set ref_forces
+    {
+          std::vector<hrp::Vector3> ee_pos;
+          for (size_t i = 0 ; i < leg_names.size(); i++) {
+              ABCIKparam& tmpikp = ikp[leg_names[i]];
+              ee_pos.push_back(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]);
+          }
+          double alpha = (ref_zmp - ee_pos[1]).norm() / ((ee_pos[0] - ref_zmp).norm() + (ee_pos[1] - ref_zmp).norm());
+          if (alpha>1.0) alpha = 1.0;
+          if (alpha<0.0) alpha = 0.0;
+          if (DEBUGP) {
+          std::cerr << "[" << m_profile.instance_name << "] alpha:" << alpha << std::endl;
+          }
+          double mg = m_robot->totalMass() * gg->get_gravitational_acceleration();
+          m_force[0].data[2] = alpha * mg;
+          m_force[1].data[2] = (1-alpha) * mg;
+    }
+
+    hrp::Vector3 tmp_foot_mid_pos(hrp::Vector3::Zero());
+    {
+        std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+        std::map<leg_type, double> zmp_weight_map = gg->get_zmp_weight_map();
+        double sum_of_weight = 0.0;
+        for (size_t i = 0; i < leg_names.size(); i++) {
+            ABCIKparam& tmpikp = ikp[leg_names[i]];
+            // get target_end_coords
+            tmpikp.target_end_coords.pos = tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos;
+            tmpikp.target_end_coords.rot = tmpikp.target_r0 * tmpikp.localR;
+            // for foot_mid_pos
+            std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
+            tmp_foot_mid_pos += (tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]) * zmp_weight_map[dst->first];
+            sum_of_weight += zmp_weight_map[dst->first];
+        }
+        tmp_foot_mid_pos *= (1.0 / sum_of_weight);
+    }
+    //
+    {
+        if ( gg_is_walking && gg->get_lcg_count() == gg->get_overwrite_check_timing()+2 ) {
+            hrp::Vector3 vel_htc(calc_vel_from_hand_error(tmp_fix_coords));
+            gg->set_offset_velocity_param(vel_htc(0), vel_htc(1) ,vel_htc(2));
+        }//  else {
+        //     if ( gg_is_walking && gg->get_lcg_count() == static_cast<size_t>(gg->get_default_step_time()/(2*m_dt))-1) {
+        //         gg->set_offset_velocity_param(0,0,0);
+        //     }
+        // }
+    }
+
+    //
+
+    hrp::Vector3 tmp_ref_cog(m_robot->calcCM());
+    if (gg_is_walking) {
+      ref_cog = gg->get_cog();
+    } else {
+      ref_cog = tmp_foot_mid_pos;
+    }
+    ref_cog(2) = tmp_ref_cog(2);
+    if (gg_is_walking) {
+      ref_zmp = gg->get_refzmp();
+    } else {
+      ref_zmp(0) = ref_cog(0);
+      ref_zmp(1) = ref_cog(1);
+      ref_zmp(2) = tmp_foot_mid_pos(2);
+    }
+  }
+};
+
+hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2)
+{
+  hrp::Vector3 vv = axis1.cross(axis2);
+  if (fabs(vv.norm()-0.0) < 1e-5) {
+    return rot;
+  } else {
+    Eigen::AngleAxis<double> tmpr(std::asin(vv.norm()), vv.normalized());
+    return tmpr.toRotationMatrix() * rot;
+  }
+}
+
+void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot)
+{
+  // get current foot mid pos + rot
+  std::vector<coordinates> foot_coords;
+  for (size_t i = 0; i < leg_names.size(); i++) {
+      ABCIKparam& tmpikp = ikp[leg_names[i]];
+      if (leg_names[i].find("leg") != std::string::npos) foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
+                                                                                           (tmpikp.target_link->R * tmpikp.localR)));
+  }
+  coordinates current_foot_mid_coords;
+  multi_mid_coords(current_foot_mid_coords, foot_coords);
+  hrp::Vector3 current_foot_mid_pos = current_foot_mid_coords.pos;
+  hrp::Matrix33 current_foot_mid_rot = current_foot_mid_coords.rot;
+  // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
+  hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose());
+  m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
+  rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
+  m_robot->calcForwardKinematics();
+}
+
+bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param, const std::string& limb_name)
+{
+  param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
+  // IK check
+  hrp::Vector3 vel_p, vel_r;
+  vel_p = param.target_p0 - param.target_link->p;
+  rats::difference_rotation(vel_r, param.target_link->R, param.target_r0);
+  if (vel_p.norm() > pos_ik_thre && transition_interpolator->isEmpty()) {
+      if (param.pos_ik_error_count % ik_error_debug_print_freq == 0) {
+          std::cerr << "[" << m_profile.instance_name << "] Too large IK error in " << limb_name << " (vel_p) = [" << vel_p(0) << " " << vel_p(1) << " " << vel_p(2) << "][m], count = " << param.pos_ik_error_count << std::endl;
+      }
+      param.pos_ik_error_count++;
+      has_ik_failed = true;
+  } else {
+      param.pos_ik_error_count = 0;
+  }
+  if (vel_r.norm() > rot_ik_thre && transition_interpolator->isEmpty()) {
+      if (param.rot_ik_error_count % ik_error_debug_print_freq == 0) {
+          std::cerr << "[" << m_profile.instance_name << "] Too large IK error in " << limb_name << " (vel_r) = [" << vel_r(0) << " " << vel_r(1) << " " << vel_r(2) << "][rad], count = " << param.rot_ik_error_count << std::endl;
+      }
+      param.rot_ik_error_count++;
+      has_ik_failed = true;
+  } else {
+      param.rot_ik_error_count = 0;
+  }
+  return true;
+}
+
+void AutoBalancer::solveLimbIK ()
+{
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+    if (it->second.is_active) {
+      for ( int j = 0; j < it->second.manip->numJoints(); j++ ){
+	int i = it->second.manip->joint(j)->jointId;
+	m_robot->joint(i)->q = qorg[i];
+      }
+    }
+  }
+  m_robot->rootLink()->p = current_root_p;
+  m_robot->rootLink()->R = current_root_R;
+  m_robot->calcForwardKinematics();
+  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
+  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
+  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
+  dif_cog *= leg_names_interpolator_ratio;
+  dif_cog(2) = m_robot->rootLink()->p(2) - target_root_p(2);
+  m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog;
+  m_robot->rootLink()->R = target_root_R;
+  // Fix for toe joint
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+      if (it->second.is_active && it->second.has_toe_joint && gg->get_use_toe_joint()) {
+          int i = it->second.target_link->jointId;
+          if (gg->get_swing_leg_names().front() == it->first) {
+              m_robot->joint(i)->q = qrefv[i] + -1 * gg->get_foot_dif_rot_angle();
+          } else {
+              m_robot->joint(i)->q = qrefv[i];
+          }
+      }
+  }
+  m_robot->calcForwardKinematics();
+
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+    if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
+  }
+  if (gg_is_walking && !gg_solved) stopWalking ();
+}
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onAborting(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onError(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onReset(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onStateUpdate(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onRateChanged(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence& limbs)
+{
+  std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
+  Guard guard(m_mutex);
+  double tmp_ratio = 0.0;
+  transition_interpolator->clear();
+  transition_interpolator->set(&tmp_ratio);
+  tmp_ratio = 1.0;
+  transition_interpolator->go(&tmp_ratio, transition_time, true);
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+    it->second.is_active = false;
+  }
+
+  for (size_t i = 0; i < limbs.length(); i++) {
+    ABCIKparam& tmp = ikp[std::string(limbs[i])];
+    tmp.is_active = true;
+    std::cerr << "[" << m_profile.instance_name << "]   limb [" << std::string(limbs[i]) << "]" << std::endl;
+  }
+
+  control_mode = MODE_SYNC_TO_ABC;
+}
+
+void AutoBalancer::stopABCparam()
+{
+  std::cerr << "[" << m_profile.instance_name << "] stop auto balancer mode" << std::endl;
+  //Guard guard(m_mutex);
+  double tmp_ratio = 1.0;
+  transition_interpolator->clear();
+  transition_interpolator->set(&tmp_ratio);
+  tmp_ratio = 0.0;
+  transition_interpolator->go(&tmp_ratio, transition_time, true);
+  control_mode = MODE_SYNC_TO_IDLE;
+}
+
+void AutoBalancer::startWalking ()
+{
+  if ( control_mode != MODE_ABC ) {
+    return_control_mode = control_mode;
+    OpenHRP::AutoBalancerService::StrSequence fix_limbs;
+    fix_limbs.length(2);
+    fix_limbs[0] = "rleg";
+    fix_limbs[1] = "lleg";
+    startABCparam(fix_limbs);
+    waitABCTransition();
+  }
+  {
+    Guard guard(m_mutex);
+    has_ik_failed = false;
+    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+        it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
+    }
+    std::vector<std::string> init_swing_leg_names(gg->get_footstep_front_leg_names());
+    std::vector<std::string> tmp_all_limbs(leg_names);
+    std::vector<std::string> init_support_leg_names;
+    std::sort(tmp_all_limbs.begin(), tmp_all_limbs.end());
+    std::sort(init_swing_leg_names.begin(), init_swing_leg_names.end());
+    std::set_difference(tmp_all_limbs.begin(), tmp_all_limbs.end(),
+                        init_swing_leg_names.begin(), init_swing_leg_names.end(),
+                        std::back_inserter(init_support_leg_names));
+    std::vector<step_node> init_support_leg_steps, init_swing_leg_dst_steps;
+    for (std::vector<std::string>::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++)
+        init_support_leg_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0));
+    for (std::vector<std::string>::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++)
+        init_swing_leg_dst_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0));
+    gg->set_default_zmp_offsets(default_zmp_offsets);
+    gg->initialize_gait_parameter(ref_cog, init_support_leg_steps, init_swing_leg_dst_steps);
+  }
+  is_hand_fix_initial = true;
+  while ( !gg->proc_one_tick() );
+  {
+    Guard guard(m_mutex);
+    gg_is_walking = gg_solved = true;
+  }
+}
+
+void AutoBalancer::stopWalking ()
+{
+  std::vector<coordinates> tmp_end_coords_list;
+  for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
+      if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(ikp[*it].target_end_coords);
+  }
+  multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
+  fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot);
+  gg->clear_footstep_nodes_list();
+  if (return_control_mode == MODE_IDLE) stopABCparam();
+  gg_is_walking = false;
+}
+
+bool AutoBalancer::startAutoBalancer (const OpenHRP::AutoBalancerService::StrSequence& limbs)
+{
+  if (control_mode == MODE_IDLE) {
+    has_ik_failed = false;
+    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+        it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
+    }
+    startABCparam(limbs);
+    waitABCTransition();
+    return_control_mode = MODE_ABC;
+    return true;
+  } else {
+    return false;
+  }
+}
+
+bool AutoBalancer::stopAutoBalancer ()
+{
+  if (control_mode == MODE_ABC) {
+    stopABCparam();
+    waitABCTransition();
+    return true;
+  } else {
+    return false;
+  }
+}
+
+void AutoBalancer::waitABCTransition()
+{
+  while (!transition_interpolator->isEmpty()) usleep(1000);
+  usleep(1000);
+}
+bool AutoBalancer::goPos(const double& x, const double& y, const double& th)
+{
+    //  if ( !gg_is_walking && !is_stop_mode) {
+  if ( !is_stop_mode) {
+    gg->set_all_limbs(leg_names);
+    coordinates start_ref_coords;
+    std::vector<coordinates> initial_support_legs_coords;
+    std::vector<leg_type> initial_support_legs;
+    bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
+    if (is_valid_gait_type == false) return false;
+    bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th,
+                                                      initial_support_legs_coords, // Dummy if gg_is_walking
+                                                      start_ref_coords,            // Dummy if gg_is_walking
+                                                      initial_support_legs,        // Dummy if gg_is_walking
+                                                      (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps.
+    if ( !gg_is_walking ) { // Initializing
+        startWalking();
+    }
+    if (!ret) {
+        std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl;
+    }
+    return ret;
+  } else {
+    std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl;
+    return false;
+  }
+}
+
+bool AutoBalancer::goVelocity(const double& vx, const double& vy, const double& vth)
+{
+  gg->set_all_limbs(leg_names);
+  if (gg_is_walking && gg_solved) {
+    gg->set_velocity_param(vx, vy, vth);
+  } else {
+    coordinates ref_coords;
+    mid_coords(ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
+    std::vector<leg_type> current_legs;
+    switch(gait_type) {
+    case BIPED:
+        current_legs = (vy > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
+        break;
+    case TROT:
+        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
+        break;
+    case PACE:
+        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
+        break;
+    case CRAWL:
+        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    case GALLOP:
+        /* at least one leg shoud be in contact */
+        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    default: break;
+    }
+    gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs);
+    startWalking();
+  }
+  return true;
+}
+
+bool AutoBalancer::goStop ()
+{
+  gg->finalize_velocity_mode();
+  waitFootSteps();
+  return true;
+}
+
+bool AutoBalancer::emergencyStop ()
+{
+  std::cerr << "[" << m_profile.instance_name << "] emergencyStop" << std::endl;
+  // is_stop_mode = true;
+  gg->emergency_stop();
+  waitFootSteps();
+  return true;
+}
+
+bool AutoBalancer::releaseEmergencyStop ()
+{
+  if (is_stop_mode) {
+      std::cerr << "[" << m_profile.instance_name << "] releaseEmergencyStop" << std::endl;
+      is_stop_mode = false;
+  }
+  return true;
+}
+
+bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
+{
+  OpenHRP::AutoBalancerService::StepParamsSequence spss;
+  spss.length(fss.length());
+  // If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
+  // If gg_is_walking is true, do not set to 0.
+  for (size_t i = 0; i < spss.length(); i++) {
+      spss[i].sps.length(fss[i].fs.length());
+      for (size_t j = 0; j < spss[i].sps.length(); j++) {
+          spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
+          spss[i].sps[j].step_time = gg->get_default_step_time();
+          spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle());
+          spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle());
+      }
+  }
+  setFootStepsWithParam(fss, spss, overwrite_fs_idx);
+}
+
+bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
+{
+    if (!is_stop_mode) {
+        std::cerr << "[" << m_profile.instance_name << "] setFootStepsList" << std::endl;
+
+        // Initial footstep Snapping
+        coordinates tmpfs, fstrans;
+        step_node initial_support_step, initial_input_step;
+        {
+            std::vector<step_node> initial_support_steps;
+            if (gg_is_walking) {
+                if (overwrite_fs_idx <= 0) {
+                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
+                    return false;
+                }
+                if (!gg->get_footstep_nodes_by_index(initial_support_steps, overwrite_fs_idx-1)) {
+                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
+                    return false;
+                }
+            } else {
+                // If walking, snap initial leg to current ABC foot coords.
+                for (size_t i = 0; i < fss[0].fs.length(); i++) {
+                    initial_support_steps.push_back(step_node(std::string(fss[0].fs[i].leg), ikp[std::string(fss[0].fs[i].leg)].target_end_coords, 0, 0, 0, 0));
+                }
+            }
+            initial_support_step = initial_support_steps.front(); /* use only one leg for representation */
+        }
+        {
+            std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+            for (size_t i = 0; i < fss[0].fs.length(); i++) {
+                if (std::string(fss[0].fs[i].leg) == leg_type_map[initial_support_step.l_r]) {
+                    coordinates tmp;
+                    memcpy(tmp.pos.data(), fss[0].fs[i].pos, sizeof(double)*3);
+                    tmp.rot = (Eigen::Quaternion<double>(fss[0].fs[i].rot[0], fss[0].fs[i].rot[1], fss[0].fs[i].rot[2], fss[0].fs[i].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+                    initial_input_step = step_node(std::string(fss[0].fs[i].leg), tmp, 0, 0, 0, 0);
+                }
+            }
+        }
+
+        // Get footsteps
+        std::vector< std::vector<coordinates> > fs_vec_list;
+        std::vector< std::vector<std::string> > leg_name_vec_list;
+        for (size_t i = 0; i < fss.length(); i++) {
+            std::vector<coordinates> fs_vec;
+            std::vector<std::string> leg_name_vec;
+            for (size_t j = 0; j < fss[i].fs.length(); j++) {
+                std::string leg(fss[i].fs[j].leg);
+                if (std::find(leg_names.begin(), leg_names.end(), leg) != leg_names.end()) {
+                    memcpy(tmpfs.pos.data(), fss[i].fs[j].pos, sizeof(double)*3);
+                    tmpfs.rot = (Eigen::Quaternion<double>(fss[i].fs[j].rot[0], fss[i].fs[j].rot[1], fss[i].fs[j].rot[2], fss[i].fs[j].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+                    initial_input_step.worldcoords.transformation(fstrans, tmpfs);
+                    tmpfs = initial_support_step.worldcoords;
+                    tmpfs.transform(fstrans);
+                } else {
+                    std::cerr << "[" << m_profile.instance_name << "]   No such target : " << leg << std::endl;
+                    return false;
+                }
+                leg_name_vec.push_back(leg);
+                fs_vec.push_back(tmpfs);
+            }
+            leg_name_vec_list.push_back(leg_name_vec);
+            fs_vec_list.push_back(fs_vec);
+        }
+        if (spss.length() != fs_vec_list.size()) {
+            std::cerr << "[" << m_profile.instance_name << "]   StepParam length " << spss.length () << " != Footstep length " << fs_vec_list.size() << std::endl;
+            return false;
+        }
+        std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl;
+        std::vector< std::vector<step_node> > fnsl;
+        for (size_t i = 0; i < fs_vec_list.size(); i++) {
+            if (!(gg_is_walking && i == 0)) { // If initial footstep, e.g., not walking, pass user-defined footstep list. If walking, pass cdr footsteps in order to neglect initial double support leg.
+                std::vector<step_node> tmp_fns;
+                for (size_t j = 0; j < fs_vec_list.at(i).size(); j++) {
+                    tmp_fns.push_back(step_node(leg_name_vec_list[i][j], fs_vec_list[i][j], spss[i].sps[j].step_height, spss[i].sps[j].step_time, spss[i].sps[j].toe_angle, spss[i].sps[j].heel_angle));
+                }
+                fnsl.push_back(tmp_fns);
+            }
+        }
+        if (gg_is_walking) {
+            std::cerr << "[" << m_profile.instance_name << "]  Set overwrite footsteps" << std::endl;
+            gg->set_overwrite_foot_steps_list(fnsl);
+            gg->set_overwrite_foot_step_index(overwrite_fs_idx);
+        } else {
+            std::cerr << "[" << m_profile.instance_name << "]  Set normal footsteps" << std::endl;
+            gg->set_foot_steps_list(fnsl);
+            startWalking();
+        }
+        return true;
+    } else {
+        std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl;
+        return false;
+    }
+}
+
+void AutoBalancer::waitFootSteps()
+{
+  //while (gg_is_walking) usleep(10);
+  while (gg_is_walking || !transition_interpolator->isEmpty() )
+    usleep(1000);
+  usleep(1000);
+  gg->set_offset_velocity_param(0,0,0);
+}
+
+void AutoBalancer::waitFootStepsEarly(const double tm)
+{
+  if (!gg_is_walking) { return;}
+  while ( !gg->is_finalizing(tm)|| !transition_interpolator->isEmpty() )
+    usleep(1000);
+  usleep(1000);
+  gg->set_offset_velocity_param(0,0,0);
+}
+
+bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
+{
+  std::cerr << "[" << m_profile.instance_name << "] setGaitGeneratorParam" << std::endl;
+  gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]);
+  std::vector<hrp::Vector3> off;
+  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
+      off.push_back(hrp::Vector3(i_param.leg_default_translate_pos[i][0], i_param.leg_default_translate_pos[i][1], i_param.leg_default_translate_pos[i][2]));
+  }
+  gg->set_leg_default_translate_pos(off);
+  gg->set_default_step_time(i_param.default_step_time);
+  gg->set_default_step_height(i_param.default_step_height);
+  gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio/2.0);
+  gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio/2.0);
+  gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio/2.0);
+  gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio/2.0);
+  gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio/2.0);
+  gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio/2.0);
+  // gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio_before);
+  // gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio_after);
+  // gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio_before);
+  // gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio_after);
+  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_before);
+  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_after);
+  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_swing_before);
+  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_swing_after);
+  if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::SHUFFLING) {
+    gg->set_default_orbit_type(SHUFFLING);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOID) {
+    gg->set_default_orbit_type(CYCLOID);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::RECTANGLE) {
+    gg->set_default_orbit_type(RECTANGLE);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::STAIR) {
+    gg->set_default_orbit_type(STAIR);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAY) {
+    gg->set_default_orbit_type(CYCLOIDDELAY);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK) {
+    gg->set_default_orbit_type(CYCLOIDDELAYKICK);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CROSS) {
+    gg->set_default_orbit_type(CROSS);
+  }
+  gg->set_swing_trajectory_delay_time_offset(i_param.swing_trajectory_delay_time_offset);
+  gg->set_swing_trajectory_final_distance_weight(i_param.swing_trajectory_final_distance_weight);
+  gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2]));
+  gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2]));  
+  gg->set_gravitational_acceleration(i_param.gravitational_acceleration);
+  gg->set_toe_angle(i_param.toe_angle);
+  gg->set_heel_angle(i_param.heel_angle);
+  gg->set_toe_pos_offset_x(i_param.toe_pos_offset_x);
+  gg->set_heel_pos_offset_x(i_param.heel_pos_offset_x);
+  gg->set_toe_zmp_offset_x(i_param.toe_zmp_offset_x);
+  gg->set_heel_zmp_offset_x(i_param.heel_zmp_offset_x);
+  std::vector<double> tmp_ratio(i_param.toe_heel_phase_ratio.get_buffer(), i_param.toe_heel_phase_ratio.get_buffer()+i_param.toe_heel_phase_ratio.length());
+  std::cerr << "[" << m_profile.instance_name << "]   "; // for set_toe_heel_phase_ratio
+  gg->set_toe_heel_phase_ratio(tmp_ratio);
+  gg->set_use_toe_joint(i_param.use_toe_joint);
+  gg->set_use_toe_heel_transition(i_param.use_toe_heel_transition);
+  gg->set_zmp_weight_map(boost::assign::map_list_of<leg_type, double>(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3]));
+  gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num);
+  gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset);
+  gg->set_leg_margin(i_param.leg_margin);
+  gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation);
+  gg->set_use_stride_limitation(i_param.use_stride_limitation);
+  if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::SQUARE) {
+    gg->set_stride_limitation_type(SQUARE);
+  } else if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::CIRCLE) {
+    gg->set_stride_limitation_type(CIRCLE);
+  }
+
+  // print
+  gg->print_param(std::string(m_profile.instance_name));
+  return true;
+};
+
+bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
+{
+  gg->get_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]);
+  std::vector<hrp::Vector3> off;
+  gg->get_leg_default_translate_pos(off);
+  i_param.leg_default_translate_pos.length(off.size());
+  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
+      i_param.leg_default_translate_pos[i].length(3);
+      i_param.leg_default_translate_pos[i][0] = off[i](0);
+      i_param.leg_default_translate_pos[i][1] = off[i](1);
+      i_param.leg_default_translate_pos[i][2] = off[i](2);
+  }
+  i_param.default_step_time = gg->get_default_step_time();
+  i_param.default_step_height = gg->get_default_step_height();
+  i_param.default_double_support_ratio_before = gg->get_default_double_support_ratio_before();
+  i_param.default_double_support_ratio_after = gg->get_default_double_support_ratio_after();
+  i_param.default_double_support_static_ratio_before = gg->get_default_double_support_static_ratio_before();
+  i_param.default_double_support_static_ratio_after = gg->get_default_double_support_static_ratio_after();
+  i_param.default_double_support_ratio_swing_before = gg->get_default_double_support_ratio_swing_before();
+  i_param.default_double_support_ratio_swing_after = gg->get_default_double_support_ratio_swing_after();
+  i_param.default_double_support_ratio = i_param.default_double_support_ratio_before + i_param.default_double_support_ratio_after;
+  i_param.default_double_support_static_ratio = i_param.default_double_support_static_ratio_before + i_param.default_double_support_static_ratio_after;
+  if (gg->get_default_orbit_type() == SHUFFLING) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::SHUFFLING;
+  } else if (gg->get_default_orbit_type() == CYCLOID) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOID;
+  } else if (gg->get_default_orbit_type() == RECTANGLE) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::RECTANGLE;
+  } else if (gg->get_default_orbit_type() == STAIR) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::STAIR;
+  } else if (gg->get_default_orbit_type() == CYCLOIDDELAY) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAY;
+  } else if (gg->get_default_orbit_type() == CYCLOIDDELAYKICK) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK;
+  } else if (gg->get_default_orbit_type() == CROSS) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CROSS;
+  }
+
+  hrp::Vector3 tmpv = gg->get_stair_trajectory_way_point_offset();
+  for (size_t i = 0; i < 3; i++) i_param.stair_trajectory_way_point_offset[i] = tmpv(i);
+  tmpv = gg->get_cycloid_delay_kick_point_offset();
+  for (size_t i = 0; i < 3; i++) i_param.cycloid_delay_kick_point_offset[i] = tmpv(i);
+  i_param.swing_trajectory_delay_time_offset = gg->get_swing_trajectory_delay_time_offset();
+  i_param.swing_trajectory_final_distance_weight = gg->get_swing_trajectory_final_distance_weight();
+  i_param.gravitational_acceleration = gg->get_gravitational_acceleration();
+  i_param.toe_angle = gg->get_toe_angle();
+  i_param.heel_angle = gg->get_heel_angle();
+  i_param.toe_pos_offset_x = gg->get_toe_pos_offset_x();
+  i_param.heel_pos_offset_x = gg->get_heel_pos_offset_x();
+  i_param.toe_zmp_offset_x = gg->get_toe_zmp_offset_x();
+  i_param.heel_zmp_offset_x = gg->get_heel_zmp_offset_x();
+  std::vector<double> ratio(gg->get_NUM_TH_PHASES(),0.0);
+  gg->get_toe_heel_phase_ratio(ratio);
+  for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) i_param.toe_heel_phase_ratio[i] = ratio[i];
+  i_param.use_toe_joint = gg->get_use_toe_joint();
+  i_param.use_toe_heel_transition = gg->get_use_toe_heel_transition();
+  std::map<leg_type, double> tmp_zmp_weight_map = gg->get_zmp_weight_map();
+  i_param.zmp_weight_map[0] = tmp_zmp_weight_map[RLEG];
+  i_param.zmp_weight_map[1] = tmp_zmp_weight_map[LLEG];
+  i_param.zmp_weight_map[2] = tmp_zmp_weight_map[RARM];
+  i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM];
+  i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num();
+  i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset();
+  for (size_t i=0; i<4; i++) {
+    i_param.leg_margin[i] = gg->get_leg_margin(i);
+  }
+  for (size_t i=0; i<4; i++) {
+    i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i);
+  }
+  i_param.use_stride_limitation = gg->get_use_stride_limitation();
+  if (gg->get_stride_limitation_type() == SQUARE) {
+    i_param.stride_limitation_type = OpenHRP::AutoBalancerService::SQUARE;
+  } else if (gg->get_stride_limitation_type() == CIRCLE) {
+    i_param.stride_limitation_type = OpenHRP::AutoBalancerService::CIRCLE;
+  }
+  return true;
+};
+
+bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
+{
+  Guard guard(m_mutex);
+  std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl;
+  double *default_zmp_offsets_array = new double[ikp.size()*3];
+  move_base_gain = i_param.move_base_gain;
+  for (size_t i = 0; i < ikp.size(); i++)
+    for (size_t j = 0; j < 3; j++)
+      default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j];
+  zmp_transition_time = i_param.zmp_transition_time;
+  adjust_footstep_transition_time = i_param.adjust_footstep_transition_time;
+  if (zmp_offset_interpolator->isEmpty()) {
+      zmp_offset_interpolator->clear();
+      zmp_offset_interpolator->go(default_zmp_offsets_array, zmp_transition_time, true);
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets cannot be set because interpolating." << std::endl;
+  }
+  if (control_mode == MODE_IDLE) {
+    switch (i_param.use_force_mode) {
+    case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
+        use_force = MODE_NO_FORCE;
+        break;
+    case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
+        use_force = MODE_REF_FORCE;
+        break;
+    default:
+        break;
+    }
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   use_force_mode cannot be changed to [" << i_param.use_force_mode << "] during MODE_ABC, MODE_SYNC_TO_IDLE or MODE_SYNC_TO_ABC." << std::endl;
+  }
+  graspless_manip_mode = i_param.graspless_manip_mode;
+  graspless_manip_arm = std::string(i_param.graspless_manip_arm);
+  for (size_t j = 0; j < 3; j++)
+      graspless_manip_p_gain[j] = i_param.graspless_manip_p_gain[j];
+  for (size_t j = 0; j < 3; j++)
+      graspless_manip_reference_trans_coords.pos[j] = i_param.graspless_manip_reference_trans_pos[j];
+  graspless_manip_reference_trans_coords.rot = (Eigen::Quaternion<double>(i_param.graspless_manip_reference_trans_rot[0],
+                                                                          i_param.graspless_manip_reference_trans_rot[1],
+                                                                          i_param.graspless_manip_reference_trans_rot[2],
+                                                                          i_param.graspless_manip_reference_trans_rot[3]).normalized().toRotationMatrix()); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+  transition_time = i_param.transition_time;
+  std::vector<std::string> cur_leg_names, dst_leg_names;
+  cur_leg_names = leg_names;
+  for (size_t i = 0; i < i_param.leg_names.length(); i++) {
+      dst_leg_names.push_back(std::string(i_param.leg_names[i]));
+  }
+  std::sort(cur_leg_names.begin(), cur_leg_names.end());
+  std::sort(dst_leg_names.begin(), dst_leg_names.end());
+  if (cur_leg_names != dst_leg_names) {
+      if (leg_names_interpolator->isEmpty()) {
+          leg_names.clear();
+          leg_names = dst_leg_names;
+          if (control_mode == MODE_ABC) {
+              double tmp_ratio = 0.0;
+              leg_names_interpolator->set(&tmp_ratio);
+              tmp_ratio = 1.0;
+              leg_names_interpolator->go(&tmp_ratio, 5.0, true);
+              control_mode = MODE_SYNC_TO_ABC;
+          }
+      }
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   leg_names cannot be set because interpolating." << std::endl;
+  }
+  pos_ik_thre = i_param.pos_ik_thre;
+  rot_ik_thre = i_param.rot_ik_thre;
+  if (!gg_is_walking) {
+      is_hand_fix_mode = i_param.is_hand_fix_mode;
+      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode = " << is_hand_fix_mode << std::endl;
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode cannot be set in (gg_is_walking = true). Current is_hand_fix_mode is " << (is_hand_fix_mode?"true":"false") << std::endl;
+  }
+  if (control_mode == MODE_IDLE) {
+      for (size_t i = 0; i < i_param.end_effector_list.length(); i++) {
+          std::map<std::string, ABCIKparam>::iterator it = ikp.find(std::string(i_param.end_effector_list[i].leg));
+          memcpy(it->second.localPos.data(), i_param.end_effector_list[i].pos, sizeof(double)*3);
+          it->second.localR = (Eigen::Quaternion<double>(i_param.end_effector_list[i].rot[0], i_param.end_effector_list[i].rot[1], i_param.end_effector_list[i].rot[2], i_param.end_effector_list[i].rot[3])).normalized().toRotationMatrix();
+      }
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl;
+  }
+  if (i_param.default_gait_type == OpenHRP::AutoBalancerService::BIPED) {
+      gait_type = BIPED;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::TROT) {
+      gait_type = TROT;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::PACE) {
+      gait_type = PACE;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::CRAWL) {
+      gait_type = CRAWL;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) {
+      gait_type = GALLOP;
+  }
+  for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
+      std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   localR = " << it->second.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
+  }
+
+  std::cerr << "[" << m_profile.instance_name << "]   move_base_gain = " << move_base_gain << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets = ";
+  for (size_t i = 0; i < ikp.size() * 3; i++) {
+      std::cerr << default_zmp_offsets_array[i] << " ";
+  }
+  std::cerr << std::endl;
+  delete[] default_zmp_offsets_array;
+  std::cerr << "[" << m_profile.instance_name << "]   use_force_mode = " << use_force << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_mode = " << graspless_manip_mode << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_arm = " << graspless_manip_arm << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_pos = " << graspless_manip_reference_trans_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_rot = " << graspless_manip_reference_trans_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   transition_time = " << transition_time << "[s], zmp_transition_time = " << zmp_transition_time << "[s], adjust_footstep_transition_time = " << adjust_footstep_transition_time << "[s]" << std::endl;
+  for (std::vector<std::string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) std::cerr << "[" << m_profile.instance_name << "]   leg_names [" << *it << "]" << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   pos_ik_thre = " << pos_ik_thre << "[m], rot_ik_thre = " << rot_ik_thre << "[rad]" << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   default_gait_type = " << gait_type << std::endl;
+  // IK limb parameters
+  std::cerr << "[" << m_profile.instance_name << "]  IK limb parameters" << std::endl;
+  bool is_ik_limb_parameter_valid_length = true;
+  if (i_param.ik_limb_parameters.length() != ee_vec.size()) {
+      is_ik_limb_parameter_valid_length = false;
+      std::cerr << "[" << m_profile.instance_name << "]   ik_limb_parameters invalid length! Cannot be set. (input = " << i_param.ik_limb_parameters.length() << ", desired = " << ee_vec.size() << ")" << std::endl;
+  } else {
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          if (ikp[ee_vec[i]].manip->numJoints() != i_param.ik_limb_parameters[i].ik_optional_weight_vector.length())
+              is_ik_limb_parameter_valid_length = false;
+      }
+      if (is_ik_limb_parameter_valid_length) {
+          for (size_t i = 0; i < ee_vec.size(); i++) {
+              ABCIKparam& param = ikp[ee_vec[i]];
+              const OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i];
+              std::vector<double> ov;
+              ov.resize(param.manip->numJoints());
+              for (size_t j = 0; j < param.manip->numJoints(); j++) {
+                  ov[j] = ilp.ik_optional_weight_vector[j];
+              }
+              param.manip->setOptionalWeightVector(ov);
+              param.manip->setSRGain(ilp.sr_gain);
+              param.avoid_gain = ilp.avoid_gain;
+              param.reference_gain = ilp.reference_gain;
+              param.manip->setManipulabilityLimit(ilp.manipulability_limit);
+          }
+      } else {
+          std::cerr << "[" << m_profile.instance_name << "]   ik_optional_weight_vector invalid length! Cannot be set. (input = [";
+          for (size_t i = 0; i < ee_vec.size(); i++) {
+              std::cerr << i_param.ik_limb_parameters[i].ik_optional_weight_vector.length() << ", ";
+          }
+          std::cerr << "], desired = [";
+          for (size_t i = 0; i < ee_vec.size(); i++) {
+              std::cerr << ikp[ee_vec[i]].manip->numJoints() << ", ";
+          }
+          std::cerr << "])" << std::endl;
+      }
+  }
+  if (is_ik_limb_parameter_valid_length) {
+      std::cerr << "[" << m_profile.instance_name << "]   ik_optional_weight_vectors = ";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          ABCIKparam& param = ikp[ee_vec[i]];
+          std::vector<double> ov;
+          ov.resize(param.manip->numJoints());
+          param.manip->getOptionalWeightVector(ov);
+          std::cerr << "[";
+          for (size_t j = 0; j < param.manip->numJoints(); j++) {
+              std::cerr << ov[j] << " ";
+          }
+          std::cerr << "]";
+      }
+      std::cerr << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   sr_gains = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].manip->getSRGain() << ", ";
+      }
+      std::cerr << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   avoid_gains = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].avoid_gain << ", ";
+      }
+      std::cerr << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   reference_gains = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].reference_gain << ", ";
+      }
+      std::cerr << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   manipulability_limits = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].manip->getManipulabilityLimit() << ", ";
+      }
+      std::cerr << "]" << std::endl;
+  }
+  return true;
+};
+
+bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
+{
+  i_param.move_base_gain = move_base_gain;
+  i_param.default_zmp_offsets.length(ikp.size());
+  for (size_t i = 0; i < ikp.size(); i++) {
+      i_param.default_zmp_offsets[i].length(3);
+      for (size_t j = 0; j < 3; j++) {
+          i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j);
+      }
+  }
+  switch(control_mode) {
+  case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break;
+  case MODE_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_ABC; break;
+  case MODE_SYNC_TO_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_IDLE; break;
+  case MODE_SYNC_TO_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_ABC; break;
+  default: break;
+  }
+  switch(use_force) {
+  case MODE_NO_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_NO_FORCE; break;
+  case MODE_REF_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE; break;
+  default: break;
+  }
+  i_param.graspless_manip_mode = graspless_manip_mode;
+  i_param.graspless_manip_arm = graspless_manip_arm.c_str();
+  for (size_t j = 0; j < 3; j++)
+      i_param.graspless_manip_p_gain[j] = graspless_manip_p_gain[j];
+  for (size_t j = 0; j < 3; j++)
+      i_param.graspless_manip_reference_trans_pos[j] = graspless_manip_reference_trans_coords.pos[j];
+  Eigen::Quaternion<double> qt(graspless_manip_reference_trans_coords.rot);
+  i_param.graspless_manip_reference_trans_rot[0] = qt.w();
+  i_param.graspless_manip_reference_trans_rot[1] = qt.x();
+  i_param.graspless_manip_reference_trans_rot[2] = qt.y();
+  i_param.graspless_manip_reference_trans_rot[3] = qt.z();
+  i_param.transition_time = transition_time;
+  i_param.zmp_transition_time = zmp_transition_time;
+  i_param.adjust_footstep_transition_time = adjust_footstep_transition_time;
+  i_param.leg_names.length(leg_names.size());
+  for (size_t i = 0; i < leg_names.size(); i++) i_param.leg_names[i] = leg_names.at(i).c_str();
+  i_param.pos_ik_thre = pos_ik_thre;
+  i_param.rot_ik_thre = rot_ik_thre;
+  i_param.is_hand_fix_mode = is_hand_fix_mode;
+  i_param.end_effector_list.length(ikp.size());
+  {
+      size_t i = 0;
+      for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
+          copyRatscoords2Footstep(i_param.end_effector_list[i],
+                                  coordinates(it->second.localPos, it->second.localR));
+          i_param.end_effector_list[i].leg = it->first.c_str();
+          i++;
+      }
+  }
+  switch(gait_type) {
+  case BIPED:  i_param.default_gait_type = OpenHRP::AutoBalancerService::BIPED;  break;
+  case TROT:   i_param.default_gait_type = OpenHRP::AutoBalancerService::TROT;   break;
+  case PACE:   i_param.default_gait_type = OpenHRP::AutoBalancerService::PACE;   break;
+  case CRAWL:  i_param.default_gait_type = OpenHRP::AutoBalancerService::CRAWL;  break;
+  case GALLOP: i_param.default_gait_type = OpenHRP::AutoBalancerService::GALLOP; break;
+  default: break;
+  }
+  i_param.ik_limb_parameters.length(ee_vec.size());
+  for (size_t i = 0; i < ee_vec.size(); i++) {
+      ABCIKparam& param = ikp[ee_vec[i]];
+      OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i];
+      ilp.ik_optional_weight_vector.length(param.manip->numJoints());
+      std::vector<double> ov;
+      ov.resize(param.manip->numJoints());
+      param.manip->getOptionalWeightVector(ov);
+      for (size_t j = 0; j < param.manip->numJoints(); j++) {
+          ilp.ik_optional_weight_vector[j] = ov[j];
+      }
+      ilp.sr_gain = param.manip->getSRGain();
+      ilp.avoid_gain = param.avoid_gain;
+      ilp.reference_gain = param.reference_gain;
+      ilp.manipulability_limit = param.manip->getManipulabilityLimit();
+  }
+  return true;
+};
+
+void AutoBalancer::copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs)
+{
+  memcpy(out_fs.pos, in_fs.pos.data(), sizeof(double)*3);
+  Eigen::Quaternion<double> qt(in_fs.rot);
+  out_fs.rot[0] = qt.w();
+  out_fs.rot[1] = qt.x();
+  out_fs.rot[2] = qt.y();
+  out_fs.rot[3] = qt.z();
+};
+
+bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param)
+{
+  copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_leg_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.swing_leg_dst_coords, gg->get_swing_leg_dst_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.dst_foot_midcoords, gg->get_dst_foot_midcoords());
+  if (gg->get_support_leg_names().front() == "rleg") {
+    i_param.support_leg = OpenHRP::AutoBalancerService::RLEG;
+  } else {
+    i_param.support_leg = OpenHRP::AutoBalancerService::LLEG;
+  }
+  switch ( gg->get_current_support_states().front() ) {
+  case BOTH: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::BOTH; break;
+  case RLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::RLEG; break;
+  case LLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::LLEG; break;
+  default: break;
+  }
+  return true;
+};
+
+bool AutoBalancer::adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep)
+{
+  std::cerr << "[" << m_profile.instance_name << "] adjustFootSteps" << std::endl;
+  if (control_mode == MODE_ABC && !gg_is_walking && adjust_footstep_interpolator->isEmpty()) {
+      Guard guard(m_mutex);
+      //
+      hrp::Vector3 eepos, org_mid_rpy, target_mid_rpy;
+      hrp::Matrix33 eerot, tmprot;
+      coordinates org_mid_coords, target_mid_coords;
+      // Get org coords
+      ikp["rleg"].adjust_interpolation_org_p0 = ikp["rleg"].target_p0;
+      ikp["lleg"].adjust_interpolation_org_p0 = ikp["lleg"].target_p0;
+      ikp["rleg"].adjust_interpolation_org_r0 = ikp["rleg"].target_r0;
+      ikp["lleg"].adjust_interpolation_org_r0 = ikp["lleg"].target_r0;
+      mid_coords(org_mid_coords, 0.5,
+                 coordinates(ikp["rleg"].adjust_interpolation_org_p0, ikp["rleg"].adjust_interpolation_org_r0),
+                 coordinates(ikp["lleg"].adjust_interpolation_org_p0, ikp["lleg"].adjust_interpolation_org_r0));
+      org_mid_rpy = hrp::rpyFromRot(org_mid_coords.rot);
+      // Get target coords
+      //   Input : ee coords
+      //   Output : link coords
+      memcpy(eepos.data(), rfootstep.pos, sizeof(double)*3);
+      eerot = (Eigen::Quaternion<double>(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
+      ikp["rleg"].adjust_interpolation_target_r0 = eerot * ikp["rleg"].localR.transpose();
+      ikp["rleg"].adjust_interpolation_target_p0 = eepos - ikp["rleg"].adjust_interpolation_target_r0 * ikp["rleg"].localPos;
+      memcpy(eepos.data(), lfootstep.pos, sizeof(double)*3);
+      eerot = (Eigen::Quaternion<double>(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
+      ikp["lleg"].adjust_interpolation_target_r0 = eerot * ikp["lleg"].localR.transpose();
+      ikp["lleg"].adjust_interpolation_target_p0 = eepos - ikp["lleg"].adjust_interpolation_target_r0 * ikp["lleg"].localPos;
+      mid_coords(target_mid_coords, 0.5,
+                 coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0),
+                 coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
+      coordinates rtrans, ltrans;
+      target_mid_coords.transformation(rtrans, coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0));
+      target_mid_coords.transformation(ltrans, coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
+      target_mid_rpy = hrp::rpyFromRot(target_mid_coords.rot);
+      // Fix target pos => org pos, target yaw => org yaw
+      target_mid_rpy(2) = org_mid_rpy(2);
+      target_mid_coords.rot = hrp::rotFromRpy(target_mid_rpy);
+      target_mid_coords.pos = org_mid_coords.pos;
+      // Calculate rleg and lleg coords
+      coordinates tmpc;
+      tmpc = target_mid_coords;
+      tmpc.transform(rtrans);
+      ikp["rleg"].adjust_interpolation_target_p0 = tmpc.pos;
+      ikp["rleg"].adjust_interpolation_target_r0 = tmpc.rot;
+      tmpc = target_mid_coords;
+      tmpc.transform(ltrans);
+      ikp["lleg"].adjust_interpolation_target_p0 = tmpc.pos;
+      ikp["lleg"].adjust_interpolation_target_r0 = tmpc.rot;
+      // Set interpolator
+      adjust_footstep_interpolator->clear();
+      double tmp = 0.0;
+      adjust_footstep_interpolator->set(&tmp);
+      tmp = 1.0;
+      adjust_footstep_interpolator->go(&tmp, adjust_footstep_transition_time, true);
+  }
+  while (!adjust_footstep_interpolator->isEmpty() )
+    usleep(1000);
+  usleep(1000);
+  return true;
+};
+
+bool AutoBalancer::getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx)
+{
+    std::cerr << "[" << m_profile.instance_name << "] getRemainingFootstepSequence" << std::endl;
+    o_footstep = new OpenHRP::AutoBalancerService::FootstepSequence;
+    if (gg_is_walking) {
+        std::vector< std::vector<step_node> > fsnl = gg->get_remaining_footstep_nodes_list();
+        o_current_fs_idx = gg->get_footstep_index();
+        o_footstep->length(fsnl.size());
+        for (size_t i = 0; i < fsnl.size(); i++) {
+            o_footstep[i].leg = (fsnl[i].front().l_r==RLEG?"rleg":"lleg");
+            copyRatscoords2Footstep(o_footstep[i], fsnl[i].front().worldcoords);
+        }
+    }
+};
+
+bool AutoBalancer::getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep)
+{
+    std::cerr << "[" << m_profile.instance_name << "] getGoPosFootstepsSequence" << std::endl;
+    o_footstep = new OpenHRP::AutoBalancerService::FootstepsSequence;
+    if (gg_is_walking) {
+        std::cerr << "[" << m_profile.instance_name << "] Cannot call getGoPosFootstepsSequence in walking" << std::endl;
+        return false;
+    } else {
+        gg->set_all_limbs(leg_names);
+        std::vector< std::vector<step_node> > new_footstep_nodes_list;
+        coordinates start_ref_coords;
+        std::vector<coordinates> initial_support_legs_coords;
+        std::vector<leg_type> initial_support_legs;
+        bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
+        if (is_valid_gait_type == false) return false;
+        /* go_pos_param_2_footstep_nodes_list_core is const member function  */
+        gg->go_pos_param_2_footstep_nodes_list_core (x, y, th,
+                                                     initial_support_legs_coords, start_ref_coords, initial_support_legs,
+                                                     new_footstep_nodes_list, true, 0);
+        o_footstep->length(new_footstep_nodes_list.size());
+        for (size_t i = 0; i < new_footstep_nodes_list.size(); i++) {
+            o_footstep[i].fs.length(new_footstep_nodes_list.at(i).size());
+            for (size_t j = 0; j < new_footstep_nodes_list.at(i).size(); j++) {
+                leg_type tmp_leg_type = new_footstep_nodes_list.at(i).at(j).l_r;
+                o_footstep[i].fs[j].leg = ((tmp_leg_type == RLEG) ? "rleg":
+                                           (tmp_leg_type == LLEG) ? "lleg":
+                                           (tmp_leg_type == RARM) ? "rarm":
+                                           "larm");
+                copyRatscoords2Footstep(o_footstep[i].fs[j], new_footstep_nodes_list.at(i).at(j).worldcoords);
+            }
+        }
+        return true;
+    }
+};
+
+void AutoBalancer::static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height)
+{
+  hrp::Vector3 target_sbp = hrp::Vector3(0, 0, 0);
+  hrp::Vector3 tmpcog = m_robot->calcCM();
+  switch ( use_force ) {
+  case MODE_REF_FORCE:
+    calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height, ref_forces);
+    tmp_input_sbp = target_sbp - sbp_offset;
+    sbp_cog_offset = tmp_input_sbp - tmpcog;
+    break;
+  case MODE_NO_FORCE:
+    tmp_input_sbp = tmpcog + sbp_cog_offset;
+    break;
+  default: break;
+  }
+};
+
+void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces)
+{
+  hrp::Vector3 denom, nume;
+  /* sb_point[m] = nume[kg * m/s^2 * m] / denom[kg * m/s^2] */
+  double mass = m_robot->totalMass();
+  for (size_t j = 0; j < 2; j++) {
+    nume(j) = mass * gg->get_gravitational_acceleration() * tmpcog(j);
+    denom(j) = mass * gg->get_gravitational_acceleration();
+    for (size_t i = 0; i < sensor_names.size(); i++) {
+      if ( sensor_names[i].find("hsensor") != std::string::npos || sensor_names[i].find("asensor") != std::string::npos ) { // tempolary to get arm force coords
+          hrp::Link* parentlink;
+          hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
+          if (sensor) parentlink = sensor->link;
+          else parentlink = m_vfs[sensor_names[i]].link;
+          for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+              if (it->second.target_link->name == parentlink->name) {
+                  hrp::Vector3 fpos = parentlink->p + parentlink->R * it->second.localPos;
+                  nume(j) += ( (fpos(2) - ref_com_height) * tmp_forces[i](j) - fpos(j) * tmp_forces[i](2) );
+                  denom(j) -= tmp_forces[i](2);
+              }
+          }
+      }
+    }
+    sb_point(j) = nume(j) / denom(j);
+  }
+  sb_point(2) = ref_com_height;
+};
+
+#ifndef rad2deg
+#define rad2deg(rad) (rad * 180 / M_PI)
+#endif
+#ifndef deg2rad
+#define deg2rad(deg) (deg * M_PI / 180)
+#endif
+
+hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_coords)
+{
+  if (graspless_manip_mode) {
+    hrp::Vector3 dp,dr;
+    coordinates ref_hand_coords(gg->get_dst_foot_midcoords()), act_hand_coords;
+    ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords
+    hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos);
+    if ( graspless_manip_arm == "arms" ) {
+        hrp::Vector3 rarm_pos = ikp["rarm"].target_p0 + ikp["rarm"].target_r0 * ikp["rarm"].localPos;
+        hrp::Vector3 larm_pos = ikp["larm"].target_p0 + ikp["larm"].target_r0 * ikp["larm"].localPos;
+        act_hand_coords.pos = (rarm_pos+larm_pos)/2.0;
+        hrp::Vector3 act_y = larm_pos-rarm_pos;
+        act_y(2) = 0;
+        act_y.normalize();
+        hrp::Vector3 ref_y(ref_hand_coords.rot * hrp::Vector3::UnitY());
+        ref_y(2) = 0;
+        ref_y.normalize();
+        dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y))); // fix for rotation
+    } else {
+      ABCIKparam& tmpikp = ikp[graspless_manip_arm];
+      act_hand_coords = rats::coordinates(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos,
+                                          tmpikp.target_r0 * tmpikp.localR);
+      rats::difference_rotation(dr, ref_hand_coords.rot, act_hand_coords.rot);
+      dr(0) = 0; dr(1) = 0;
+    }
+    dp = act_hand_coords.pos - ref_hand_coords.pos
+        + dr.cross(hrp::Vector3(foot_pos - act_hand_coords.pos));
+    dp(2) = 0;
+    hrp::Matrix33 foot_mt(gg->get_dst_foot_midcoords().rot.transpose());
+    //alias(dp) = foot_mt * dp;
+    hrp::Vector3 dp2 = foot_mt * dp;
+    //alias(dr) = foot_mt * dr;
+    return hrp::Vector3(graspless_manip_p_gain[0] * dp2(0)/gg->get_default_step_time(),
+                        graspless_manip_p_gain[1] * dp2(1)/gg->get_default_step_time(),
+                        graspless_manip_p_gain[2] * rad2deg(dr(2))/gg->get_default_step_time());
+  } else {
+    return hrp::Vector3::Zero();
+  }
+};
+
+bool AutoBalancer::calc_inital_support_legs(const double& y, std::vector<coordinates>& initial_support_legs_coords, std::vector<leg_type>& initial_support_legs, coordinates& start_ref_coords) {
+    switch(gait_type) {
+    case BIPED:
+        initial_support_legs_coords = (y > 0 ?
+                                       boost::assign::list_of(ikp["rleg"].target_end_coords)
+                                       : boost::assign::list_of(ikp["lleg"].target_end_coords));
+        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
+        break;
+    case TROT:
+        initial_support_legs_coords = (y > 0 ?
+                                       boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["larm"].target_end_coords)
+                                       : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["rarm"].target_end_coords));
+        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
+        break;
+    case PACE:
+        initial_support_legs_coords = (y > 0 ?
+                                       boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["rarm"].target_end_coords)
+                                       : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["larm"].target_end_coords));
+        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
+        break;
+    case CRAWL:
+        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    case GALLOP:
+        /* at least one leg shoud be in contact */
+        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    default: break;
+    }
+    mid_coords(start_ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
+    return true;
+};
+
+//
+extern "C"
+{
+
+    void AutoBalancerInit(RTC::Manager* manager)
+    {
+        RTC::Properties profile(autobalancer_spec);
+        manager->registerFactory(profile,
+                                 RTC::Create<AutoBalancer>,
+                                 RTC::Delete<AutoBalancer>);
+    }
+
+};
+
+
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp.my b/rtc/AutoBalancer/AutoBalancer.cpp.my
new file mode 100644
index 00000000000..638a4493673
--- /dev/null
+++ b/rtc/AutoBalancer/AutoBalancer.cpp.my
@@ -0,0 +1,2218 @@
+// -*- C++ -*-
+/*!
+ * @file  AutoBalancer.cpp
+ * @brief autobalancer component
+ * $Date$
+ *
+ * $Id$
+ */
+
+#include <rtm/CorbaNaming.h>
+#include <hrpModel/Link.h>
+#include <hrpModel/Sensor.h>
+#include <hrpModel/ModelLoaderUtil.h>
+#include "AutoBalancer.h"
+#include <hrpModel/JointPath.h>
+#include <hrpUtil/MatrixSolvers.h>
+#include "hrpsys/util/Hrpsys.h"
+
+
+typedef coil::Guard<coil::Mutex> Guard;
+using namespace rats;
+
+// Module specification
+// <rtc-template block="module_spec">
+static const char* autobalancer_spec[] =
+    {
+        "implementation_id", "AutoBalancer",
+        "type_name",         "AutoBalancer",
+        "description",       "autobalancer component",
+        "version",           HRPSYS_PACKAGE_VERSION,
+        "vendor",            "AIST",
+        "category",          "example",
+        "activity_type",     "DataFlowComponent",
+        "max_instance",      "10",
+        "language",          "C++",
+        "lang_type",         "compile",
+        // Configuration variables
+        "conf.default.debugLevel", "0",
+        ""
+    };
+// </rtc-template>
+
+AutoBalancer::AutoBalancer(RTC::Manager* manager)
+    : RTC::DataFlowComponentBase(manager),
+      // <rtc-template block="initializer">
+      m_qRefIn("qRef", m_qRef),
+      m_basePosIn("basePosIn", m_basePos),
+      m_baseRpyIn("baseRpyIn", m_baseRpy),
+      m_zmpIn("zmpIn", m_zmp),
+      m_optionalDataIn("optionalData", m_optionalData),
+      m_emergencySignalIn("emergencySignal", m_emergencySignal),
+      //ishiguro
+      m_htzmpIn("htzmpIn", m_htzmp),
+      m_htrfwIn("htrfwIn", m_htrfw),
+      m_htlfwIn("htlfwIn", m_htlfw),
+      m_htcomIn("htcomIn", m_htcom),
+      m_htrfIn("htrfIn", m_htrf),
+      m_htlfIn("htlfIn", m_htlf),
+      m_htrhIn("htrhIn", m_htrh),
+      m_htlhIn("htlhIn", m_htlh),
+      m_actzmpIn("actzmpIn", m_actzmp),
+
+      m_qOut("q", m_qRef),
+      m_zmpOut("zmpOut", m_zmp),
+      m_basePosOut("basePosOut", m_basePos),
+      m_baseRpyOut("baseRpyOut", m_baseRpy),
+      m_baseTformOut("baseTformOut", m_baseTform),
+      m_basePoseOut("basePoseOut", m_basePose),
+      m_accRefOut("accRef", m_accRef),
+      m_contactStatesOut("contactStates", m_contactStates),
+      m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime),
+      m_cogOut("cogOut", m_cog),
+      m_AutoBalancerServicePort("AutoBalancerService"),
+      m_walkingStatesOut("walkingStates", m_walkingStates),
+      m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
+      m_legMarginIn("legMargin", m_legMargin),
+      // </rtc-template>
+      gait_type(BIPED),
+      move_base_gain(0.8),
+      m_robot(hrp::BodyPtr()),
+      m_debugLevel(0)
+{
+    m_service0.autobalancer(this);
+}
+
+AutoBalancer::~AutoBalancer()
+{
+}
+
+
+RTC::ReturnCode_t AutoBalancer::onInitialize()
+{
+    std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
+    bindParameter("debugLevel", m_debugLevel, "0");
+
+    // Registration: InPort/OutPort/Service
+    // <rtc-template block="registration">
+    // Set InPort buffers
+    addInPort("qRef", m_qRefIn);
+    addInPort("basePosIn", m_basePosIn);
+    addInPort("baseRpyIn", m_baseRpyIn);
+    addInPort("zmpIn", m_zmpIn);
+    addInPort("optionalData", m_optionalDataIn);
+    addInPort("emergencySignal", m_emergencySignalIn);
+    //ishiguro
+    addInPort("htzmpIn", m_htzmpIn);
+    addInPort("htrfwIn", m_htrfwIn);
+    addInPort("htlfwIn", m_htlfwIn);
+    addInPort("htcomIn", m_htcomIn);
+    addInPort("htrfIn", m_htrfIn);
+    addInPort("htlfIn", m_htlfIn);
+    addInPort("htrhIn", m_htrhIn);
+    addInPort("htlhIn", m_htlhIn);
+    addInPort("actzmpIn", m_actzmpIn);
+
+
+    addInPort("legMargin", m_legMarginIn);
+
+    // Set OutPort buffer
+    addOutPort("q", m_qOut);
+    addOutPort("zmpOut", m_zmpOut);
+    addOutPort("basePosOut", m_basePosOut);
+    addOutPort("baseRpyOut", m_baseRpyOut);
+    addOutPort("baseTformOut", m_baseTformOut);
+    addOutPort("basePoseOut", m_basePoseOut);
+    addOutPort("accRef", m_accRefOut);
+    addOutPort("contactStates", m_contactStatesOut);
+    addOutPort("controlSwingSupportTime", m_controlSwingSupportTimeOut);
+    addOutPort("cogOut", m_cogOut);
+    addOutPort("walkingStates", m_walkingStatesOut);
+    addOutPort("sbpCogOffset", m_sbpCogOffsetOut);
+  
+    // Set service provider to Ports
+    m_AutoBalancerServicePort.registerProvider("service0", "AutoBalancerService", m_service0);
+  
+    // Set service consumers to Ports
+  
+    // Set CORBA Service Ports
+    addPort(m_AutoBalancerServicePort);
+  
+    // </rtc-template>
+    // <rtc-template block="bind_config">
+    // Bind variables and configuration variable
+  
+    // </rtc-template>
+
+    RTC::Properties& prop =  getProperties();
+    coil::stringTo(m_dt, prop["dt"].c_str());
+
+    m_robot = hrp::BodyPtr(new hrp::Body());
+    RTC::Manager& rtcManager = RTC::Manager::instance();
+    std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
+    int comPos = nameServer.find(",");
+    if (comPos < 0){
+        comPos = nameServer.length();
+    }
+    nameServer = nameServer.substr(0, comPos);
+    RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
+    if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
+                                 CosNaming::NamingContext::_duplicate(naming.getRootContext())
+                                 )){
+      std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
+      return RTC::RTC_ERROR;
+    }
+
+    // allocate memory for outPorts
+    m_qRef.data.length(m_robot->numJoints());
+    qorg.resize(m_robot->numJoints());
+    qrefv.resize(m_robot->numJoints());
+    m_baseTform.data.length(12);
+    //ishiguro
+    m_htrfw.data.length(6);
+    m_htlfw.data.length(6);
+
+    control_mode = MODE_IDLE;
+    loop = 0;
+
+    // setting from conf file
+    // GaitGenerator requires abc_leg_offset and abc_stride_parameter in robot conf file
+    // setting leg_pos from conf file
+    coil::vstring leg_offset_str = coil::split(prop["abc_leg_offset"], ",");
+    leg_names.push_back("rleg");
+    leg_names.push_back("lleg");
+
+    // setting from conf file
+    // rleg,TARGET_LINK,BASE_LINK
+    coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
+    size_t prop_num = 10;
+    if (end_effectors_str.size() > 0) {
+      size_t num = end_effectors_str.size()/prop_num;
+      for (size_t i = 0; i < num; i++) {
+        std::string ee_name, ee_target, ee_base;
+        coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
+        coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
+        coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
+        ABCIKparam tp;
+        for (size_t j = 0; j < 3; j++) {
+            coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
+        }
+        double tmpv[4];
+        for (int j = 0; j < 4; j++ ) {
+          coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
+        }
+        tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
+        tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
+        // Fix for toe joint
+        //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
+        //   Without toe joints, "end-effector link == force-sensor link" is assumed.
+        //   With toe joints, "end-effector link != force-sensor link" is assumed.
+        if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
+            std::vector<double> optw(tp.manip->numJoints(), 1.0);
+            optw.back() = 0.0; // Set weight = 0 for toe joint by default
+            tp.manip->setOptionalWeightVector(optw);
+            tp.has_toe_joint = true;
+        } else {
+            tp.has_toe_joint = false;
+        }
+        tp.avoid_gain = 0.001;
+        tp.reference_gain = 0.01;
+        tp.pos_ik_error_count = tp.rot_ik_error_count = 0;
+        ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
+        ikp[ee_name].target_link = m_robot->link(ee_target);
+        ee_vec.push_back(ee_name);
+        std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   target = " << ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
+        contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
+      }
+      m_contactStates.data.length(num);
+      if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
+        m_contactStates.data[contact_states_index_map["rleg"]] = true;
+        m_contactStates.data[contact_states_index_map["lleg"]] = true;
+      }
+      if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) {
+        m_contactStates.data[contact_states_index_map["rarm"]] = false;
+        m_contactStates.data[contact_states_index_map["larm"]] = false;
+      }
+      m_controlSwingSupportTime.data.length(num);
+      for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 0.0;
+    }
+    std::vector<hrp::Vector3> leg_pos;
+    if (leg_offset_str.size() > 0) {
+      hrp::Vector3 leg_offset;
+      for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
+      std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+      leg_pos.push_back(hrp::Vector3(-1*leg_offset));
+      leg_pos.push_back(hrp::Vector3(leg_offset));
+    }
+    if (leg_pos.size() < ikp.size()) {
+        size_t tmp_leg_pos_size = leg_pos.size();
+        for (size_t i = 0; i < ikp.size() - tmp_leg_pos_size; i++) {
+            leg_pos.push_back(hrp::Vector3::Zero());
+        }
+    }
+
+    std::vector<std::pair<hrp::Link*, hrp::Link*> > interlocking_joints;
+    readInterlockingJointsParamFromProperties(interlocking_joints, m_robot, prop["interlocking_joints"], std::string(m_profile.instance_name));
+    if (interlocking_joints.size() > 0) {
+        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            std::cerr << "[" << m_profile.instance_name << "] Interlocking Joints for [" << it->first << "]" << std::endl;
+            it->second.manip->setInterlockingJointPairIndices(interlocking_joints, std::string(m_profile.instance_name));
+        }
+    }
+
+    zmp_offset_interpolator = new interpolator(ikp.size()*3, m_dt);
+    zmp_offset_interpolator->setName(std::string(m_profile.instance_name)+" zmp_offset_interpolator");
+    zmp_transition_time = 1.0;
+    transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator");
+    transition_interpolator_ratio = 1.0;
+    adjust_footstep_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    adjust_footstep_interpolator->setName(std::string(m_profile.instance_name)+" adjust_footstep_interpolator");
+    transition_time = 2.0;
+    adjust_footstep_transition_time = 2.0;
+    leg_names_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator");
+    leg_names_interpolator_ratio = 1.0;
+
+    // setting stride limitations from conf file
+    double stride_fwd_x_limit = 0.15;
+    double stride_y_limit = 0.05;
+    double stride_th_limit = 10;
+    double stride_bwd_x_limit = 0.05;
+    std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_y_limit << "[m], " << stride_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl;
+    if (default_zmp_offsets.size() == 0) {
+      for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
+    }
+    if (leg_offset_str.size() > 0) {
+      gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_y_limit/*[m]*/, stride_th_limit/*[deg]*/, stride_bwd_x_limit/*[m]*/));
+      gg->set_default_zmp_offsets(default_zmp_offsets);
+    }
+    gg_is_walking = gg_solved = false;
+    m_walkingStates.data = false;
+    fix_leg_coords = coordinates();
+
+    // load virtual force sensors
+    readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
+    // ref force port
+    int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
+    int nvforce = m_vfs.size();
+    int nforce  = npforce + nvforce;
+    m_ref_force.resize(nforce);
+    m_ref_forceIn.resize(nforce);
+    m_force.resize(nforce);
+    m_ref_forceOut.resize(nforce);
+    m_limbCOPOffset.resize(nforce);
+    m_limbCOPOffsetOut.resize(nforce);
+    for (unsigned int i=0; i<npforce; i++){
+        sensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
+    }
+    for (unsigned int i=0; i<nvforce; i++){
+        for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
+            if (it->second.id == i) sensor_names.push_back(it->first);
+        }
+    }
+    // set ref force port
+    std::cerr << "[" << m_profile.instance_name << "] force sensor ports (" << nforce << ")" << std::endl;
+    for (unsigned int i=0; i<nforce; i++){
+        m_ref_forceIn[i] = new InPort<TimedDoubleSeq>(std::string("ref_"+sensor_names[i]).c_str(), m_ref_force[i]);
+        m_ref_force[i].data.length(6);
+        registerInPort(std::string("ref_"+sensor_names[i]).c_str(), *m_ref_forceIn[i]);
+        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string("ref_"+sensor_names[i]) << std::endl;
+        ref_forces.push_back(hrp::Vector3(0,0,0));
+    }
+    // set force port
+    for (unsigned int i=0; i<nforce; i++){
+        m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string(sensor_names[i]).c_str(), m_force[i]);
+        m_force[i].data.length(6);
+        m_force[i].data[0] = m_force[i].data[1] = m_force[i].data[2] = 0.0;
+        m_force[i].data[3] = m_force[i].data[4] = m_force[i].data[5] = 0.0;
+        registerOutPort(std::string(sensor_names[i]).c_str(), *m_ref_forceOut[i]);
+        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string(sensor_names[i]) << std::endl;
+    }
+    // set limb cop offset port
+    std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << nforce << ")" << std::endl;
+    for (unsigned int i=0; i<nforce; i++){
+        std::string nm("limbCOPOffset_"+sensor_names[i]);
+        m_limbCOPOffsetOut[i] = new OutPort<TimedPoint3D>(nm.c_str(), m_limbCOPOffset[i]);
+        registerOutPort(nm.c_str(), *m_limbCOPOffsetOut[i]);
+        m_limbCOPOffset[i].data.x = m_limbCOPOffset[i].data.y = m_limbCOPOffset[i].data.z = 0.0;
+        std::cerr << "[" << m_profile.instance_name << "]   name = " << nm << std::endl;
+    }
+    sbp_offset = hrp::Vector3(0,0,0);
+    sbp_cog_offset = hrp::Vector3(0,0,0);
+    //use_force = MODE_NO_FORCE;
+    use_force = MODE_REF_FORCE;
+
+    if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
+      is_legged_robot = true;
+    } else {
+      is_legged_robot = false;
+    }
+
+    m_accRef.data.ax = m_accRef.data.ay = m_accRef.data.az = 0.0;
+    prev_imu_sensor_vel = hrp::Vector3::Zero();
+
+    graspless_manip_mode = false;
+    graspless_manip_arm = "arms";
+    graspless_manip_p_gain = hrp::Vector3::Zero();
+
+    is_stop_mode = false;
+    has_ik_failed = false;
+    is_hand_fix_mode = false;
+
+    pos_ik_thre = 0.1*1e-3; // [m]
+    rot_ik_thre = (1e-2)*M_PI/180.0; // [rad]
+    ik_error_debug_print_freq = static_cast<int>(0.2/m_dt); // once per 0.2 [s]
+
+    //for HumanSynchronizer
+    hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer());
+
+    return RTC::RTC_OK;
+}
+
+
+RTC::ReturnCode_t AutoBalancer::onFinalize()
+{
+  delete zmp_offset_interpolator;
+  delete transition_interpolator;
+  delete adjust_footstep_interpolator;
+  delete leg_names_interpolator;
+  return RTC::RTC_OK;
+}
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onStartup(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onShutdown(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id)
+{
+    std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
+    
+    return RTC::RTC_OK;
+}
+
+RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
+{
+  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
+  Guard guard(m_mutex);
+  if (control_mode == MODE_ABC) {
+    control_mode = MODE_SYNC_TO_IDLE;
+    double tmp_ratio = 0.0;
+    transition_interpolator->go(&tmp_ratio, m_dt, true); // sync in one controller loop
+  }
+  return RTC::RTC_OK;
+}
+
+
+
+
+#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
+//#define DEBUGP2 ((loop%200==0))
+#define DEBUGP2 (false)
+RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
+{
+  // std::cerr << "AutoBalancer::onExecute(" << ec_id << ")" << std::endl;
+    loop ++;
+    if (m_qRefIn.isNew()) {
+        m_qRefIn.read();
+    }
+    if (m_basePosIn.isNew()) {
+      m_basePosIn.read();
+      input_basePos(0) = m_basePos.data.x;
+      input_basePos(1) = m_basePos.data.y;
+      input_basePos(2) = m_basePos.data.z;
+    }
+    if (m_baseRpyIn.isNew()) {
+      m_baseRpyIn.read();
+      input_baseRot = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
+    }
+    if (m_zmpIn.isNew()) {
+      m_zmpIn.read();
+      input_zmp(0) = m_zmp.data.x;
+      input_zmp(1) = m_zmp.data.y;
+      input_zmp(2) = m_zmp.data.z;
+    }
+    for (unsigned int i=0; i<m_ref_forceIn.size(); i++){
+        if ( m_ref_forceIn[i]->isNew() ) {
+            m_ref_forceIn[i]->read();
+        }
+    }
+    if (m_optionalDataIn.isNew()) {
+        m_optionalDataIn.read();
+        if (is_legged_robot) {
+          if (m_optionalData.data.length() >= contact_states_index_map.size()*2) {
+            // current optionalData is contactstates x limb and controlSwingSupportTime x limb
+            //   If contactStates in optionalData is 1.0, m_contactStates is true. Otherwise, false.
+            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+                m_contactStates.data[contact_states_index_map[it->first]] = isOptionalDataContact(it->first);
+                m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = m_optionalData.data[contact_states_index_map[it->first]+contact_states_index_map.size()];
+            }
+            if ( !m_contactStates.data[contact_states_index_map["rleg"]] && !m_contactStates.data[contact_states_index_map["lleg"]] ) { // If two feet have no contact, force set double support contact
+              m_contactStates.data[contact_states_index_map["rleg"]] = true;
+              m_contactStates.data[contact_states_index_map["lleg"]] = true;
+            }
+          }
+        }
+    }
+    if (m_emergencySignalIn.isNew()){
+        m_emergencySignalIn.read();
+        // if (!is_stop_mode) {
+        //     std::cerr << "[" << m_profile.instance_name << "] emergencySignal is set!" << std::endl;
+        //     is_stop_mode = true;
+        //     gg->emergency_stop();
+        // }
+    }
+    //for HumanSynchronizer
+    if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
+    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); hp_raw_data.rfw = HumanSynchronizer::DoubleSeqToWrench6(m_htrfw); }
+    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); hp_raw_data.lfw = HumanSynchronizer::DoubleSeqToWrench6(m_htlfw); }
+    if (m_htcomIn.isNew()){ m_htcomIn.read(); hp_raw_data.com = HumanSynchronizer::Point3DToVector3(m_htcom); }
+    if (m_htrfIn.isNew()) { m_htrfIn.read();  hp_raw_data.rf  = HumanSynchronizer::Point3DToVector3(m_htrf); }
+    if (m_htlfIn.isNew()) { m_htlfIn.read();  hp_raw_data.lf  = HumanSynchronizer::Point3DToVector3(m_htlf); }
+    if (m_htrhIn.isNew()) { m_htrhIn.read();  hp_raw_data.rh  = HumanSynchronizer::Point3DToVector3(m_htrh); }
+    if (m_htlhIn.isNew()) { m_htlhIn.read();  hp_raw_data.lh  = HumanSynchronizer::Point3DToVector3(m_htlh); }
+    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
+    hsp->readInput(hp_raw_data);
+
+
+    if (m_legMarginIn.isNew()) {
+      m_legMarginIn.read();
+      for (size_t i = 0; i < 4; i++) {
+        gg->set_leg_margin(m_legMargin.data[i], i);
+      }
+    }
+
+    Guard guard(m_mutex);
+    hrp::Vector3 ref_basePos;
+    hrp::Matrix33 ref_baseRot;
+    hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
+    if ( is_legged_robot ) {
+      gg->proc_zmp_weight_map_interpolation();
+      getCurrentParameters();
+      getTargetParameters();
+      bool is_transition_interpolator_empty = transition_interpolator->isEmpty();
+      if (!is_transition_interpolator_empty) {
+        transition_interpolator->get(&transition_interpolator_ratio, true);
+      } else {
+        transition_interpolator_ratio = 1.0;
+      }
+      if (control_mode != MODE_IDLE ) {
+        solveLimbIK();
+        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
+      } else {
+        rel_ref_zmp = input_zmp;
+      }
+      // transition
+      if (!is_transition_interpolator_empty) {
+        // transition_interpolator_ratio 0=>1 : IDLE => ABC
+        // transition_interpolator_ratio 1=>0 : ABC => IDLE
+        ref_basePos = (1-transition_interpolator_ratio) * input_basePos + transition_interpolator_ratio * m_robot->rootLink()->p;
+        rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp;
+        rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R);
+        for ( int i = 0; i < m_robot->numJoints(); i++ ) {
+          m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q;
+        }
+      } else {
+        ref_basePos = m_robot->rootLink()->p;
+        ref_baseRot = m_robot->rootLink()->R;
+      }
+      // mode change for sync
+      if (control_mode == MODE_SYNC_TO_ABC) {
+        control_mode = MODE_ABC;
+      } else if (control_mode == MODE_SYNC_TO_IDLE && transition_interpolator->isEmpty() ) {
+        std::cerr << "[" << m_profile.instance_name << "] Finished cleanup" << std::endl;
+        control_mode = MODE_IDLE;
+      }
+    }
+    if ( m_qRef.data.length() != 0 ) { // initialized
+      if (is_legged_robot) {
+        for ( int i = 0; i < m_robot->numJoints(); i++ ){
+          m_qRef.data[i] = m_robot->joint(i)->q;
+        }
+      }
+      m_qOut.write();
+    }
+    if (is_legged_robot) {
+      // basePos
+      m_basePos.data.x = ref_basePos(0);
+      m_basePos.data.y = ref_basePos(1);
+      m_basePos.data.z = ref_basePos(2);
+      m_basePos.tm = m_qRef.tm;
+      // baseRpy
+      hrp::Vector3 baseRpy = hrp::rpyFromRot(ref_baseRot);
+      m_baseRpy.data.r = baseRpy(0);
+      m_baseRpy.data.p = baseRpy(1);
+      m_baseRpy.data.y = baseRpy(2);
+      m_baseRpy.tm = m_qRef.tm;
+      // baseTform
+      double *tform_arr = m_baseTform.data.get_buffer();
+      tform_arr[0] = m_basePos.data.x;
+      tform_arr[1] = m_basePos.data.y;
+      tform_arr[2] = m_basePos.data.z;
+      hrp::setMatrix33ToRowMajorArray(ref_baseRot, tform_arr, 3);
+      m_baseTform.tm = m_qRef.tm;
+      // basePose
+      m_basePose.data.position.x = m_basePos.data.x;
+      m_basePose.data.position.y = m_basePos.data.y;
+      m_basePose.data.position.z = m_basePos.data.z;
+      m_basePose.data.orientation.r = m_baseRpy.data.r;
+      m_basePose.data.orientation.p = m_baseRpy.data.p;
+      m_basePose.data.orientation.y = m_baseRpy.data.y;
+      m_basePose.tm = m_qRef.tm;
+      // zmp
+      m_zmp.data.x = rel_ref_zmp(0);
+      m_zmp.data.y = rel_ref_zmp(1);
+      m_zmp.data.z = rel_ref_zmp(2);
+      m_zmp.tm = m_qRef.tm;
+      // cog
+      m_cog.data.x = ref_cog(0);
+      m_cog.data.y = ref_cog(1);
+      m_cog.data.z = ref_cog(2);
+      m_cog.tm = m_qRef.tm;
+      // sbpCogOffset
+      m_sbpCogOffset.data.x = sbp_cog_offset(0);
+      m_sbpCogOffset.data.y = sbp_cog_offset(1);
+      m_sbpCogOffset.data.z = sbp_cog_offset(2);
+      m_sbpCogOffset.tm = m_qRef.tm;
+    }
+    m_basePosOut.write();
+    m_baseRpyOut.write();
+    m_baseTformOut.write();
+    m_basePoseOut.write();
+    m_zmpOut.write();
+    m_cogOut.write();
+    m_sbpCogOffsetOut.write();
+
+
+//printf("m_zmp:%f, abs:%f\n",m_zmp.data.y,m_basePos.data.y+m_zmp.data.y);
+
+    // reference acceleration
+    hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
+    if (sen != NULL) {
+      hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos;
+      hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt;
+      // convert to imu sensor local acceleration
+      hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt;
+      m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2);
+      m_accRefOut.write();
+      prev_imu_sensor_pos = imu_sensor_pos;
+      prev_imu_sensor_vel = imu_sensor_vel;
+    }
+
+    // control parameters
+    m_contactStates.tm = m_qRef.tm;
+    m_contactStatesOut.write();
+    m_controlSwingSupportTime.tm = m_qRef.tm;
+    m_controlSwingSupportTimeOut.write();
+    m_walkingStates.data = gg_is_walking;
+    m_walkingStates.tm = m_qRef.tm;
+    m_walkingStatesOut.write();
+
+    for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
+        m_force[i].tm = m_qRef.tm;
+        m_ref_forceOut[i]->write();
+    }
+
+    for (unsigned int i=0; i<m_limbCOPOffsetOut.size(); i++){
+        m_limbCOPOffset[i].tm = m_qRef.tm;
+        m_limbCOPOffsetOut[i]->write();
+    }
+
+    return RTC::RTC_OK;
+}
+
+void AutoBalancer::getCurrentParameters()
+{
+  current_root_p = m_robot->rootLink()->p;
+  current_root_R = m_robot->rootLink()->R;
+  for ( int i = 0; i < m_robot->numJoints(); i++ ){
+    qorg[i] = m_robot->joint(i)->q;
+  }
+}
+
+void AutoBalancer::getTargetParameters()
+{
+  // joint angles
+  for ( int i = 0; i < m_robot->numJoints(); i++ ){
+    m_robot->joint(i)->q = m_qRef.data[i];
+    qrefv[i] = m_robot->joint(i)->q;
+  }
+  // basepos, rot, zmp
+  m_robot->rootLink()->p = input_basePos;
+  m_robot->rootLink()->R = input_baseRot;
+  m_robot->calcForwardKinematics();
+  //
+  if (control_mode != MODE_IDLE) {
+    coordinates tmp_fix_coords;
+    if (!zmp_offset_interpolator->isEmpty()) {
+      double *default_zmp_offsets_output = new double[ikp.size()*3];
+      zmp_offset_interpolator->get(default_zmp_offsets_output, true);
+      for (size_t i = 0; i < ikp.size(); i++)
+        for (size_t j = 0; j < 3; j++)
+          default_zmp_offsets[i](j) = default_zmp_offsets_output[i*3+j];
+      delete[] default_zmp_offsets_output;
+      if (DEBUGP) {
+        std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl;
+        std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+        for (size_t i = 0; i < leg_names.size(); i++) {
+            std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
+            std::cerr << "[" << m_profile.instance_name << "]   " << leg_names[i] << " = " << default_zmp_offsets[dst->first].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+        }
+      }
+    }
+    if (!leg_names_interpolator->isEmpty()) {
+        leg_names_interpolator->get(&leg_names_interpolator_ratio, true);
+    }else {
+        leg_names_interpolator_ratio = 1.0;
+    }
+    if ( gg_is_walking ) {
+      gg->set_default_zmp_offsets(default_zmp_offsets);
+      gg_solved = gg->proc_one_tick();
+      {
+          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+          coordinates tmpc;
+          // for support leg
+          for (std::vector<step_node>::const_iterator it = gg->get_support_leg_steps().begin(); it != gg->get_support_leg_steps().end(); it++) {
+              coordinates sp_coords = it->worldcoords;
+              coordinates(ikp[leg_type_map[it->l_r]].localPos,
+                          ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc);
+              sp_coords.transform(tmpc);
+              ikp[leg_type_map[it->l_r]].target_p0 = sp_coords.pos;
+              ikp[leg_type_map[it->l_r]].target_r0 = sp_coords.rot;
+          }
+          // for swing leg
+          for (std::vector<step_node>::const_iterator it = gg->get_swing_leg_steps().begin(); it != gg->get_swing_leg_steps().end(); it++) {
+              coordinates sw_coords = it->worldcoords;
+              coordinates(ikp[leg_type_map[it->l_r]].localPos,
+                          ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc);
+              sw_coords.transform(tmpc);
+              ikp[leg_type_map[it->l_r]].target_p0 = sw_coords.pos;
+              ikp[leg_type_map[it->l_r]].target_r0 = sw_coords.rot;
+          }
+      }
+      gg->get_swing_support_mid_coords(tmp_fix_coords);
+      // set contactStates
+      {
+          std::vector<std::string> tmp_current_support_states_names;
+          {
+              std::vector<leg_type> tmp_current_support_states = gg->get_current_support_states();
+              std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+              for (std::vector<leg_type>::const_iterator it = tmp_current_support_states.begin(); it != tmp_current_support_states.end(); it++)
+                  tmp_current_support_states_names.push_back(leg_type_map[*it]);
+          }
+          // Set Contact States for ee not included in leg_names to false
+          for ( std::map<std::string, size_t>::iterator it = contact_states_index_map.begin(); it != contact_states_index_map.end(); it++ ) {
+              m_contactStates.data[it->second] = false;
+          }
+          for (std::vector<std::string>::const_iterator it = leg_names.begin(); it != leg_names.end(); it++) {
+              std::vector<std::string>::const_iterator dst = std::find_if(tmp_current_support_states_names.begin(), tmp_current_support_states_names.end(), boost::lambda::_1 == *it);
+              if (dst != tmp_current_support_states_names.end()) {
+                  m_contactStates.data[contact_states_index_map[*it]] = true;
+              } else {
+                  m_contactStates.data[contact_states_index_map[*it]] = false;
+              }
+          }
+      }
+      // set controlSwingSupportTime
+      {
+          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+          for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
+              std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
+              m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = gg->get_current_swing_time(dst->first);
+          }
+      }
+      // set ref_forces
+      {
+          std::vector<hrp::Vector3> ee_pos;
+          for (size_t i = 0 ; i < leg_names.size(); i++) {
+              ABCIKparam& tmpikp = ikp[leg_names[i]];
+              ee_pos.push_back(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]);
+          }
+          double alpha = (ref_zmp - ee_pos[1]).norm() / (ee_pos[0] - ee_pos[1]).norm();
+          if (alpha>1.0) alpha = 1.0;
+          if (alpha<0.0) alpha = 0.0;
+          if (DEBUGP) {
+          std::cerr << "[" << m_profile.instance_name << "] alpha:" << alpha << std::endl;
+          }
+          double mg = m_robot->totalMass() * gg->get_gravitational_acceleration();
+          m_force[0].data[0] = alpha * mg;
+          m_force[1].data[0] = (1-alpha) * mg;
+      }
+      // set limbCOPOffset
+      {
+          std::vector<std::string> swg_leg_nms = gg->get_swing_leg_names();
+          for (size_t i = 0; i < swg_leg_nms.size(); i++) {
+              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.x = gg->get_swing_foot_zmp_offsets().at(i)(0);
+              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.y = gg->get_swing_foot_zmp_offsets().at(i)(1);
+              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.z = gg->get_swing_foot_zmp_offsets().at(i)(2);
+          }
+      }
+      {
+          std::vector<std::string> sup_leg_nms = gg->get_support_leg_names();
+          for (size_t i = 0; i < sup_leg_nms.size(); i++) {
+              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.x = gg->get_support_foot_zmp_offsets().at(i)(0);
+              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.y = gg->get_support_foot_zmp_offsets().at(i)(1);
+              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.z = gg->get_support_foot_zmp_offsets().at(i)(2);
+          }
+      }
+    } else {
+      tmp_fix_coords = fix_leg_coords;
+      // double support by default
+      {
+          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+          for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
+              std::vector<std::string>::const_iterator dst = std::find_if(leg_names.begin(), leg_names.end(), (boost::lambda::_1 == it->first));
+              if (dst != leg_names.end()) {
+                  m_contactStates.data[contact_states_index_map[it->first]] = true;
+              } else {
+                  m_contactStates.data[contact_states_index_map[it->first]] = false;
+              }
+              // controlSwingSupportTime is not used while double support period, 1.0 is neglected
+              m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = 1.0;
+              std::map<leg_type, std::string>::const_iterator dst2 = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
+              m_limbCOPOffset[contact_states_index_map[it->first]].data.x = default_zmp_offsets.at(dst2->first)(0);
+              m_limbCOPOffset[contact_states_index_map[it->first]].data.y = default_zmp_offsets.at(dst2->first)(1);
+              m_limbCOPOffset[contact_states_index_map[it->first]].data.z = default_zmp_offsets.at(dst2->first)(2);
+          }
+      }
+    }
+    if (!adjust_footstep_interpolator->isEmpty()) {
+        double tmp = 0.0;
+        adjust_footstep_interpolator->get(&tmp, true);
+        //std::cerr << "[" << m_profile.instance_name << "] adjust ratio " << tmp << std::endl;
+        ikp["rleg"].target_p0 = (1-tmp) * ikp["rleg"].adjust_interpolation_org_p0 + tmp*ikp["rleg"].adjust_interpolation_target_p0;
+        ikp["lleg"].target_p0 = (1-tmp) * ikp["lleg"].adjust_interpolation_org_p0 + tmp*ikp["lleg"].adjust_interpolation_target_p0;
+        rats::mid_rot(ikp["rleg"].target_r0, tmp, ikp["rleg"].adjust_interpolation_org_r0, ikp["rleg"].adjust_interpolation_target_r0);
+        rats::mid_rot(ikp["lleg"].target_r0, tmp, ikp["lleg"].adjust_interpolation_org_r0, ikp["lleg"].adjust_interpolation_target_r0);
+        coordinates tmprc, tmplc;
+        tmprc.pos = ikp["rleg"].target_p0 + ikp["rleg"].target_r0 * ikp["rleg"].localPos;
+        tmprc.rot = ikp["rleg"].target_r0 * ikp["rleg"].localR;
+        tmplc.pos = ikp["lleg"].target_p0 + ikp["lleg"].target_r0 * ikp["lleg"].localPos;
+        tmplc.rot = ikp["lleg"].target_r0 * ikp["lleg"].localR;
+        rats::mid_coords(fix_leg_coords, 0.5, tmprc, tmplc);
+        tmp_fix_coords = fix_leg_coords;
+    }
+    // Tempolarily modify tmp_fix_coords
+    // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272
+    {
+      hrp::Vector3 ex = hrp::Vector3::UnitX();
+      hrp::Vector3 ez = hrp::Vector3::UnitZ();
+      hrp::Vector3 xv1 (tmp_fix_coords.rot * ex);
+      xv1(2) = 0.0;
+      xv1.normalize();
+      hrp::Vector3 yv1(ez.cross(xv1));
+      tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2);
+      tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2);
+      tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2);
+    }
+    // Fix pos
+    fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot);
+
+    /* update ref_forces ;; sp's absolute -> rmc's absolute */
+    for (size_t i = 0; i < m_ref_forceIn.size(); i++) {
+      hrp::Matrix33 eeR;
+      hrp::Link* parentlink;
+      hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
+      if (sensor) parentlink = sensor->link;
+      else parentlink = m_vfs[sensor_names[i]].link;
+      for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+          if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR;
+      }
+      // End effector frame
+      //ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
+      // world frame
+      ref_forces[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
+    }
+    sbp_offset = tmp_fix_coords.rot * hrp::Vector3(sbp_offset);
+
+    target_root_p = m_robot->rootLink()->p;
+    target_root_R = m_robot->rootLink()->R;
+    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+      if ( control_mode == MODE_IDLE || std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end() ) {
+        it->second.target_p0 = it->second.target_link->p;
+        it->second.target_r0 = it->second.target_link->R;
+      }
+    }
+    // Just for ik initial value
+    if (control_mode == MODE_SYNC_TO_ABC) {
+        current_root_p = target_root_p;
+        current_root_R = target_root_R;
+        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
+                it->second.target_p0 = it->second.target_link->p;
+                it->second.target_r0 = it->second.target_link->R;
+            }
+        }
+    }
+    // Move hand for hand fix mode
+    //   If arms' ABCIKparam.is_active is true, move hands according to cog velocity.
+    //   If is_hand_fix_mode is false, no hand fix mode and move hands according to cog velocity.
+    //   If is_hand_fix_mode is true, hand fix mode and do not move hands in Y axis in tmp_fix_coords.rot.    
+    if (gg_is_walking) {
+        // hand control while walking = solve hand ik with is_hand_fix_mode and solve hand ik without is_hand_fix_mode
+        bool is_hand_control_while_walking = false;
+        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
+                 && it->first.find("arm") != std::string::npos ) {
+                is_hand_control_while_walking = true;
+            }
+        }
+        if (is_hand_control_while_walking) {
+        //if (false) { // Disabled temporarily
+            // Store hand_fix_initial_offset in the initialization of walking
+            if (is_hand_fix_initial) hand_fix_initial_offset = tmp_fix_coords.rot.transpose() * (hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos);
+            is_hand_fix_initial = false;
+            hrp::Vector3 dif_p = hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos - tmp_fix_coords.rot * hand_fix_initial_offset;
+            if (is_hand_fix_mode) {
+                dif_p = tmp_fix_coords.rot.transpose() * dif_p;
+                dif_p(1) = 0;
+                dif_p = tmp_fix_coords.rot * dif_p;
+            }
+            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+                if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
+                     && it->first.find("arm") != std::string::npos ) {
+                    it->second.target_p0 = it->second.target_p0 + dif_p;
+                }
+            }
+        }
+    }
+
+    hrp::Vector3 tmp_foot_mid_pos(hrp::Vector3::Zero());
+    {
+        std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+        std::map<leg_type, double> zmp_weight_map = gg->get_zmp_weight_map();
+        double sum_of_weight = 0.0;
+        for (size_t i = 0; i < leg_names.size(); i++) {
+            ABCIKparam& tmpikp = ikp[leg_names[i]];
+            // get target_end_coords
+            tmpikp.target_end_coords.pos = tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos;
+            tmpikp.target_end_coords.rot = tmpikp.target_r0 * tmpikp.localR;
+            // for foot_mid_pos
+            std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
+            tmp_foot_mid_pos += (tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]) * zmp_weight_map[dst->first];
+            sum_of_weight += zmp_weight_map[dst->first];
+        }
+        tmp_foot_mid_pos *= (1.0 / sum_of_weight);
+    }
+    //
+    {
+        if ( gg_is_walking && gg->get_lcg_count() == gg->get_overwrite_check_timing()+2 ) {
+            hrp::Vector3 vel_htc(calc_vel_from_hand_error(tmp_fix_coords));
+            gg->set_offset_velocity_param(vel_htc(0), vel_htc(1) ,vel_htc(2));
+        }//  else {
+        //     if ( gg_is_walking && gg->get_lcg_count() == static_cast<size_t>(gg->get_default_step_time()/(2*m_dt))-1) {
+        //         gg->set_offset_velocity_param(0,0,0);
+        //     }
+        // }
+    }
+
+
+    hrp::Vector3 tmp_ref_cog(m_robot->calcCM());
+    if (gg_is_walking) {
+      ref_cog = gg->get_cog();
+    } else {
+      ref_cog = tmp_foot_mid_pos;
+    }
+    ref_cog(2) = tmp_ref_cog(2);
+    if (gg_is_walking) {
+      ref_zmp = gg->get_refzmp();
+    } else {
+      ref_zmp(0) = ref_cog(0);
+      ref_zmp(1) = ref_cog(1);
+      ref_zmp(2) = tmp_foot_mid_pos(2);
+    }
+  }
+};
+
+hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2)
+{
+  hrp::Vector3 vv = axis1.cross(axis2);
+  if (fabs(vv.norm()-0.0) < 1e-5) {
+    return rot;
+  } else {
+    Eigen::AngleAxis<double> tmpr(std::asin(vv.norm()), vv.normalized());
+    return tmpr.toRotationMatrix() * rot;
+  }
+}
+
+void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot)
+{
+  // get current foot mid pos + rot
+  std::vector<coordinates> foot_coords;
+  for (size_t i = 0; i < leg_names.size(); i++) {
+      ABCIKparam& tmpikp = ikp[leg_names[i]];
+      if (leg_names[i].find("leg") != std::string::npos) foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
+                                                                                           (tmpikp.target_link->R * tmpikp.localR)));
+  }
+  coordinates current_foot_mid_coords;
+  multi_mid_coords(current_foot_mid_coords, foot_coords);
+  hrp::Vector3 current_foot_mid_pos = current_foot_mid_coords.pos;
+  hrp::Matrix33 current_foot_mid_rot = current_foot_mid_coords.rot;
+  // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
+  hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose());
+  m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
+  rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
+  m_robot->calcForwardKinematics();
+}
+
+bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param, const std::string& limb_name)
+{
+  param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
+  // IK check
+  hrp::Vector3 vel_p, vel_r;
+  vel_p = param.target_p0 - param.target_link->p;
+  rats::difference_rotation(vel_r, param.target_link->R, param.target_r0);
+  if (vel_p.norm() > pos_ik_thre && transition_interpolator->isEmpty()) {
+      if (param.pos_ik_error_count % ik_error_debug_print_freq == 0) {
+          std::cerr << "[" << m_profile.instance_name << "] Too large IK error in " << limb_name << " (vel_p) = [" << vel_p(0) << " " << vel_p(1) << " " << vel_p(2) << "][m], count = " << param.pos_ik_error_count << std::endl;
+      }
+      param.pos_ik_error_count++;
+      has_ik_failed = true;
+  } else {
+      param.pos_ik_error_count = 0;
+  }
+  if (vel_r.norm() > rot_ik_thre && transition_interpolator->isEmpty()) {
+      if (param.rot_ik_error_count % ik_error_debug_print_freq == 0) {
+          std::cerr << "[" << m_profile.instance_name << "] Too large IK error in " << limb_name << " (vel_r) = [" << vel_r(0) << " " << vel_r(1) << " " << vel_r(2) << "][rad], count = " << param.rot_ik_error_count << std::endl;
+      }
+      param.rot_ik_error_count++;
+      has_ik_failed = true;
+  } else {
+      param.rot_ik_error_count = 0;
+  }
+  return true;
+}
+
+
+
+void AutoBalancer::solveLimbIK ()
+{
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+    if (it->second.is_active) {
+      for ( int j = 0; j < it->second.manip->numJoints(); j++ ){
+	int i = it->second.manip->joint(j)->jointId;
+	m_robot->joint(i)->q = qorg[i];
+      }
+    }
+  }
+  m_robot->rootLink()->p = current_root_p;
+  m_robot->rootLink()->R = current_root_R;
+  m_robot->calcForwardKinematics();
+  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
+  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
+  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
+  dif_cog *= leg_names_interpolator_ratio;
+  dif_cog(2) = m_robot->rootLink()->p(2) - target_root_p(2);
+  m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog;
+  m_robot->rootLink()->R = target_root_R;
+
+
+  //for HumanSynchronizer
+//  static int HumanSyncCountdownNum = 5 * (1/m_dt);
+  if(hsp->startCountdownForHumanSync){
+    std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";//ややCOMのIKに手間取った時プリント
+	  hsp->updateCountDown();
+  }
+  if(hsp->isHumanSyncOn()){
+	  if(hsp->ht_first_call){
+		  hsp->init_wld_rp_basepos = m_robot->rootLink()->p;
+      hsp->setInitOffsetPose();
+      hsp->rp_wld_initpos.com = m_robot->calcCM();
+      hsp->rp_wld_initpos.rf = ikp["rleg"].target_link->p;
+      hsp->rp_wld_initpos.lf = ikp["lleg"].target_link->p;
+      if(ikp.count("rarm"))hsp->rp_wld_initpos.rh = ikp["rarm"].target_link->p;
+      if(ikp.count("larm"))hsp->rp_wld_initpos.lh = ikp["larm"].target_link->p;
+      hsp->ht_first_call = false;
+
+      hsp->calibInitHumanCOMFromZMP();
+
+      std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
+
+	  }
+  }else{
+    hsp->setInitOffsetPose();
+  }
+
+  hsp->update();//////HumanSynchronizerの主要処理
+  if(loop%100==0)hsp->rp_ref_out.print();
+  if(loop%100==0)cerr<<"getUpdateTime:"<<hsp->getUpdateTime()*1000<<"[ms]"<<endl;
+
+  if(hsp->isHumanSyncOn()){
+	  ////////////////////// 重心拘束位置設定 /////////////////////////
+    m_robot->rootLink()->p = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;//まずは重心目標分，体幹を動かす
+    m_robot->rootLink()->p(2) = hsp->init_wld_rp_basepos(2)+ hsp->rp_ref_out.com(2);//高さだけはベース基準
+    m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
+    m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
+  }
+
+  // Fix for toe joint
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+      if (it->second.is_active && it->second.has_toe_joint && gg->get_use_toe_joint()) {
+          int i = it->second.target_link->jointId;
+          if (gg->get_swing_leg_names().front() == it->first) {
+              m_robot->joint(i)->q = qrefv[i] + -1 * gg->get_foot_dif_rot_angle();
+          } else {
+              m_robot->joint(i)->q = qrefv[i];
+          }
+      }
+  }
+  m_robot->calcForwardKinematics();
+
+
+  // additional COM fitting IK for HumanSynchronizer
+  hrp::Vector3 tmp_com_err = hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM();
+  int com_ik_loop=0;
+  const int COM_IK_MAX_LOOP = 10;
+  const double COM_IK_MAX_ERROR = 0.0001;//0.001だとたまにカクッとなる
+  if(hsp->isHumanSyncOn()){
+    ikp["rleg"].target_p0 = hsp->rp_wld_initpos.rf + hsp->rp_ref_out.rf;
+    ikp["lleg"].target_p0 = hsp->rp_wld_initpos.lf + hsp->rp_ref_out.lf;
+    if(ikp.count("rarm"))ikp["rarm"].target_p0 = hsp->rp_wld_initpos.rh + hsp->rp_ref_out.rh;
+    if(ikp.count("larm"))ikp["larm"].target_p0 = hsp->rp_wld_initpos.lh + hsp->rp_ref_out.lh;
+    while(fabs(tmp_com_err(0))>COM_IK_MAX_ERROR || fabs(tmp_com_err(1))>COM_IK_MAX_ERROR || fabs(tmp_com_err(2))>COM_IK_MAX_ERROR){//Z方向のCOM合わせは要注意(たまに怪しい)
+		  m_robot->rootLink()->p(0) += tmp_com_err(0);
+      m_robot->rootLink()->p(1) += tmp_com_err(1);
+      m_robot->rootLink()->p(2) += tmp_com_err(2);
+		  m_robot->calcForwardKinematics();
+		  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+		    if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
+		  }
+		  m_robot->calcForwardKinematics();
+      tmp_com_err = (hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM());
+		  if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "[" << m_profile.instance_name << "] COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
+	  }
+
+//    static hrp::Vector3 pre_ik_basepos;
+//    for(int i=0;i<3;i++){//重心拘束で解いた結果の体幹軌道の滑らかさは保証されていないので体幹位置に速度制限＆再度IK
+//      if(m_robot->rootLink()->p(i) - pre_ik_basepos(i) > 0.0002){ m_robot->rootLink()->p(i) =  pre_ik_basepos(i) + 0.0002; }
+//      else if(m_robot->rootLink()->p(i) - pre_ik_basepos(i) < -0.0002){ m_robot->rootLink()->p(i) =  pre_ik_basepos(i) - 0.0002; }
+//    }
+//    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+//      if (it->second.is_active) solveLimbIKforLimb(it->second);
+//    }
+//    pre_ik_basepos = m_robot->rootLink()->p;
+
+    //outport用のデータ上書き
+    ref_zmp(0) = hsp->rp_ref_out.zmp(0);
+    ref_zmp(1) = hsp->rp_ref_out.zmp(1);
+    ref_zmp(2) = 0;
+    ref_cog = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;
+
+	  if(loop%100==0)if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
+
+
+//	  printf("root:%f, calcCM:%f, refCM:%f\n",m_robot->rootLink()->p(1),m_robot->calcCM()(1),hsp->rp_ref_out.com(1) + hsp->rp_wld_initpos.com(1));
+
+
+  }else{
+	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
+	    if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
+	  }
+  }
+  if (gg_is_walking && !gg_solved) stopWalking ();
+}
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onAborting(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onError(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onReset(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onStateUpdate(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onRateChanged(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+bool AutoBalancer::startHumanSyncAfter5sec()
+{
+  std::cerr << "[" << m_profile.instance_name << "] start HumanSync after 5 sec" << std::endl;
+  hsp->startCountdownForHumanSync = true;
+  return true;
+}
+
+bool AutoBalancer::setHumanToRobotRatio(const double h2r)
+{
+  std::cerr << "[" << m_profile.instance_name << "] set h2r_ratio as "<< h2r << std::endl;
+  if(!hsp->isHumanSyncOn())hsp->h2r_ratio = h2r;
+  return true;
+}
+
+bool AutoBalancer::setAllowedXYZSync(const bool x_on,const bool y_on,const bool z_on)
+{
+  std::cerr << "[" << m_profile.instance_name << "] set allowed XYZ move direction as ("<<x_on<<","<<y_on<<","<<z_on<<")" << std::endl;
+  hsp->use_x = x_on;
+  hsp->use_y = y_on;
+  hsp->use_z = z_on;
+  return true;
+}
+
+bool AutoBalancer::stopHumanSync()
+{
+	std::cerr << "[" << m_profile.instance_name << "] stop HumanSync Now" << std::endl;
+	return true;
+}
+
+void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence& limbs)
+{
+  std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
+  Guard guard(m_mutex);
+  double tmp_ratio = 0.0;
+  transition_interpolator->clear();
+  transition_interpolator->set(&tmp_ratio);
+  tmp_ratio = 1.0;
+  transition_interpolator->go(&tmp_ratio, transition_time, true);
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+    it->second.is_active = false;
+  }
+
+  for (size_t i = 0; i < limbs.length(); i++) {
+    ABCIKparam& tmp = ikp[std::string(limbs[i])];
+    tmp.is_active = true;
+    std::cerr << "[" << m_profile.instance_name << "]   limb [" << std::string(limbs[i]) << "]" << std::endl;
+  }
+
+  control_mode = MODE_SYNC_TO_ABC;
+}
+
+void AutoBalancer::stopABCparam()
+{
+  std::cerr << "[" << m_profile.instance_name << "] stop auto balancer mode" << std::endl;
+  //Guard guard(m_mutex);
+  double tmp_ratio = 1.0;
+  transition_interpolator->clear();
+  transition_interpolator->set(&tmp_ratio);
+  tmp_ratio = 0.0;
+  transition_interpolator->go(&tmp_ratio, transition_time, true);
+  control_mode = MODE_SYNC_TO_IDLE;
+}
+
+void AutoBalancer::startWalking ()
+{
+  if ( control_mode != MODE_ABC ) {
+    return_control_mode = control_mode;
+    OpenHRP::AutoBalancerService::StrSequence fix_limbs;
+    fix_limbs.length(2);
+    fix_limbs[0] = "rleg";
+    fix_limbs[1] = "lleg";
+    startABCparam(fix_limbs);
+    waitABCTransition();
+  }
+  {
+    Guard guard(m_mutex);
+    has_ik_failed = false;
+    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+        it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
+    }
+    std::vector<std::string> init_swing_leg_names(gg->get_footstep_front_leg_names());
+    std::vector<std::string> tmp_all_limbs(leg_names);
+    std::vector<std::string> init_support_leg_names;
+    std::sort(tmp_all_limbs.begin(), tmp_all_limbs.end());
+    std::sort(init_swing_leg_names.begin(), init_swing_leg_names.end());
+    std::set_difference(tmp_all_limbs.begin(), tmp_all_limbs.end(),
+                        init_swing_leg_names.begin(), init_swing_leg_names.end(),
+                        std::back_inserter(init_support_leg_names));
+    std::vector<step_node> init_support_leg_steps, init_swing_leg_dst_steps;
+    for (std::vector<std::string>::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++)
+        init_support_leg_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0));
+    for (std::vector<std::string>::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++)
+        init_swing_leg_dst_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0));
+    gg->set_default_zmp_offsets(default_zmp_offsets);
+    gg->initialize_gait_parameter(ref_cog, init_support_leg_steps, init_swing_leg_dst_steps);
+  }
+  is_hand_fix_initial = true;
+  while ( !gg->proc_one_tick() );
+  {
+    Guard guard(m_mutex);
+    gg_is_walking = gg_solved = true;
+  }
+}
+
+void AutoBalancer::stopWalking ()
+{
+  std::vector<coordinates> tmp_end_coords_list;
+  for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
+      if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(ikp[*it].target_end_coords);
+  }
+  multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
+  fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot);
+  gg->clear_footstep_nodes_list();
+  if (return_control_mode == MODE_IDLE) stopABCparam();
+  gg_is_walking = false;
+}
+
+bool AutoBalancer::startAutoBalancer (const OpenHRP::AutoBalancerService::StrSequence& limbs)
+{
+  if (control_mode == MODE_IDLE) {
+    has_ik_failed = false;
+    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+        it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
+    }
+    startABCparam(limbs);
+    waitABCTransition();
+    return_control_mode = MODE_ABC;
+    return true;
+  } else {
+    return false;
+  }
+}
+
+bool AutoBalancer::stopAutoBalancer ()
+{
+  if (control_mode == MODE_ABC) {
+    stopABCparam();
+    waitABCTransition();
+    return true;
+  } else {
+    return false;
+  }
+}
+
+void AutoBalancer::waitABCTransition()
+{
+  while (!transition_interpolator->isEmpty()) usleep(1000);
+  usleep(1000);
+}
+bool AutoBalancer::goPos(const double& x, const double& y, const double& th)
+{
+    //  if ( !gg_is_walking && !is_stop_mode) {
+  if ( !is_stop_mode) {
+    gg->set_all_limbs(leg_names);
+    coordinates start_ref_coords;
+    std::vector<coordinates> initial_support_legs_coords;
+    std::vector<leg_type> initial_support_legs;
+    bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
+    if (is_valid_gait_type == false) return false;
+    bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th,
+                                                      initial_support_legs_coords, // Dummy if gg_is_walking
+                                                      start_ref_coords,            // Dummy if gg_is_walking
+                                                      initial_support_legs,        // Dummy if gg_is_walking
+                                                      (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps.
+    if ( !gg_is_walking ) { // Initializing
+        startWalking();
+    }
+    if (!ret) {
+        std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl;
+    }
+    return ret;
+  } else {
+    std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl;
+    return false;
+  }
+}
+
+bool AutoBalancer::goVelocity(const double& vx, const double& vy, const double& vth)
+{
+  gg->set_all_limbs(leg_names);
+  if (gg_is_walking && gg_solved) {
+    gg->set_velocity_param(vx, vy, vth);
+  } else {
+    coordinates ref_coords;
+    mid_coords(ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
+    std::vector<leg_type> current_legs;
+    switch(gait_type) {
+    case BIPED:
+        current_legs = (vy > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
+        break;
+    case TROT:
+        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
+        break;
+    case PACE:
+        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
+        break;
+    case CRAWL:
+        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    case GALLOP:
+        /* at least one leg shoud be in contact */
+        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    default: break;
+    }
+    gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs);
+    startWalking();
+  }
+  return true;
+}
+
+bool AutoBalancer::goStop ()
+{
+  gg->finalize_velocity_mode();
+  waitFootSteps();
+  return true;
+}
+
+bool AutoBalancer::emergencyStop ()
+{
+  std::cerr << "[" << m_profile.instance_name << "] emergencyStop" << std::endl;
+  // is_stop_mode = true;
+  gg->emergency_stop();
+  waitFootSteps();
+  return true;
+}
+
+bool AutoBalancer::releaseEmergencyStop ()
+{
+  if (is_stop_mode) {
+      std::cerr << "[" << m_profile.instance_name << "] releaseEmergencyStop" << std::endl;
+      is_stop_mode = false;
+  }
+  return true;
+}
+
+bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
+{
+  OpenHRP::AutoBalancerService::StepParamsSequence spss;
+  spss.length(fss.length());
+  // If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
+  // If gg_is_walking is true, do not set to 0.
+  for (size_t i = 0; i < spss.length(); i++) {
+      spss[i].sps.length(fss[i].fs.length());
+      for (size_t j = 0; j < spss[i].sps.length(); j++) {
+          spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
+          spss[i].sps[j].step_time = gg->get_default_step_time();
+          spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle());
+          spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle());
+      }
+  }
+  setFootStepsWithParam(fss, spss, overwrite_fs_idx);
+}
+
+bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
+{
+    if (!is_stop_mode) {
+        std::cerr << "[" << m_profile.instance_name << "] setFootStepsList" << std::endl;
+
+        // Initial footstep Snapping
+        coordinates tmpfs, fstrans;
+        step_node initial_support_step, initial_input_step;
+        {
+            std::vector<step_node> initial_support_steps;
+            if (gg_is_walking) {
+                if (overwrite_fs_idx <= 0) {
+                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
+                    return false;
+                }
+                if (!gg->get_footstep_nodes_by_index(initial_support_steps, overwrite_fs_idx-1)) {
+                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
+                    return false;
+                }
+            } else {
+                // If walking, snap initial leg to current ABC foot coords.
+                for (size_t i = 0; i < fss[0].fs.length(); i++) {
+                    initial_support_steps.push_back(step_node(std::string(fss[0].fs[i].leg), ikp[std::string(fss[0].fs[i].leg)].target_end_coords, 0, 0, 0, 0));
+                }
+            }
+            initial_support_step = initial_support_steps.front(); /* use only one leg for representation */
+        }
+        {
+            std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+            for (size_t i = 0; i < fss[0].fs.length(); i++) {
+                if (std::string(fss[0].fs[i].leg) == leg_type_map[initial_support_step.l_r]) {
+                    coordinates tmp;
+                    memcpy(tmp.pos.data(), fss[0].fs[i].pos, sizeof(double)*3);
+                    tmp.rot = (Eigen::Quaternion<double>(fss[0].fs[i].rot[0], fss[0].fs[i].rot[1], fss[0].fs[i].rot[2], fss[0].fs[i].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+                    initial_input_step = step_node(std::string(fss[0].fs[i].leg), tmp, 0, 0, 0, 0);
+                }
+            }
+        }
+
+        // Get footsteps
+        std::vector< std::vector<coordinates> > fs_vec_list;
+        std::vector< std::vector<std::string> > leg_name_vec_list;
+        for (size_t i = 0; i < fss.length(); i++) {
+            std::vector<coordinates> fs_vec;
+            std::vector<std::string> leg_name_vec;
+            for (size_t j = 0; j < fss[i].fs.length(); j++) {
+                std::string leg(fss[i].fs[j].leg);
+                if (std::find(leg_names.begin(), leg_names.end(), leg) != leg_names.end()) {
+                    memcpy(tmpfs.pos.data(), fss[i].fs[j].pos, sizeof(double)*3);
+                    tmpfs.rot = (Eigen::Quaternion<double>(fss[i].fs[j].rot[0], fss[i].fs[j].rot[1], fss[i].fs[j].rot[2], fss[i].fs[j].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+                    initial_input_step.worldcoords.transformation(fstrans, tmpfs);
+                    tmpfs = initial_support_step.worldcoords;
+                    tmpfs.transform(fstrans);
+                } else {
+                    std::cerr << "[" << m_profile.instance_name << "]   No such target : " << leg << std::endl;
+                    return false;
+                }
+                leg_name_vec.push_back(leg);
+                fs_vec.push_back(tmpfs);
+            }
+            leg_name_vec_list.push_back(leg_name_vec);
+            fs_vec_list.push_back(fs_vec);
+        }
+        if (spss.length() != fs_vec_list.size()) {
+            std::cerr << "[" << m_profile.instance_name << "]   StepParam length " << spss.length () << " != Footstep length " << fs_vec_list.size() << std::endl;
+            return false;
+        }
+        std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl;
+        std::vector< std::vector<step_node> > fnsl;
+        for (size_t i = 0; i < fs_vec_list.size(); i++) {
+            if (!(gg_is_walking && i == 0)) { // If initial footstep, e.g., not walking, pass user-defined footstep list. If walking, pass cdr footsteps in order to neglect initial double support leg.
+                std::vector<step_node> tmp_fns;
+                for (size_t j = 0; j < fs_vec_list.at(i).size(); j++) {
+                    tmp_fns.push_back(step_node(leg_name_vec_list[i][j], fs_vec_list[i][j], spss[i].sps[j].step_height, spss[i].sps[j].step_time, spss[i].sps[j].toe_angle, spss[i].sps[j].heel_angle));
+                }
+                fnsl.push_back(tmp_fns);
+            }
+        }
+        if (gg_is_walking) {
+            std::cerr << "[" << m_profile.instance_name << "]  Set overwrite footsteps" << std::endl;
+            gg->set_overwrite_foot_steps_list(fnsl);
+            gg->set_overwrite_foot_step_index(overwrite_fs_idx);
+        } else {
+            std::cerr << "[" << m_profile.instance_name << "]  Set normal footsteps" << std::endl;
+            gg->set_foot_steps_list(fnsl);
+            startWalking();
+        }
+        return true;
+    } else {
+        std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl;
+        return false;
+    }
+}
+
+void AutoBalancer::waitFootSteps()
+{
+  //while (gg_is_walking) usleep(10);
+  while (gg_is_walking || !transition_interpolator->isEmpty() )
+    usleep(1000);
+  usleep(1000);
+  gg->set_offset_velocity_param(0,0,0);
+}
+
+void AutoBalancer::waitFootStepsEarly(const double tm)
+{
+  if (!gg_is_walking) { return;}
+  while ( !gg->is_finalizing(tm)|| !transition_interpolator->isEmpty() )
+    usleep(1000);
+  usleep(1000);
+  gg->set_offset_velocity_param(0,0,0);
+}
+
+bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
+{
+  std::cerr << "[" << m_profile.instance_name << "] setGaitGeneratorParam" << std::endl;
+  gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]);
+  std::vector<hrp::Vector3> off;
+  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
+      off.push_back(hrp::Vector3(i_param.leg_default_translate_pos[i][0], i_param.leg_default_translate_pos[i][1], i_param.leg_default_translate_pos[i][2]));
+  }
+  gg->set_leg_default_translate_pos(off);
+  gg->set_default_step_time(i_param.default_step_time);
+  gg->set_default_step_height(i_param.default_step_height);
+  gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio/2.0);
+  gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio/2.0);
+  gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio/2.0);
+  gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio/2.0);
+  gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio/2.0);
+  gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio/2.0);
+  // gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio_before);
+  // gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio_after);
+  // gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio_before);
+  // gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio_after);
+  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_before);
+  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_after);
+  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_swing_before);
+  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_swing_after);
+  if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::SHUFFLING) {
+    gg->set_default_orbit_type(SHUFFLING);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOID) {
+    gg->set_default_orbit_type(CYCLOID);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::RECTANGLE) {
+    gg->set_default_orbit_type(RECTANGLE);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::STAIR) {
+    gg->set_default_orbit_type(STAIR);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAY) {
+    gg->set_default_orbit_type(CYCLOIDDELAY);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK) {
+    gg->set_default_orbit_type(CYCLOIDDELAYKICK);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CROSS) {
+    gg->set_default_orbit_type(CROSS);
+  }
+  gg->set_swing_trajectory_delay_time_offset(i_param.swing_trajectory_delay_time_offset);
+  gg->set_swing_trajectory_final_distance_weight(i_param.swing_trajectory_final_distance_weight);
+  gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2]));
+  gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2]));  
+  gg->set_gravitational_acceleration(i_param.gravitational_acceleration);
+  gg->set_toe_angle(i_param.toe_angle);
+  gg->set_heel_angle(i_param.heel_angle);
+  gg->set_toe_pos_offset_x(i_param.toe_pos_offset_x);
+  gg->set_heel_pos_offset_x(i_param.heel_pos_offset_x);
+  gg->set_toe_zmp_offset_x(i_param.toe_zmp_offset_x);
+  gg->set_heel_zmp_offset_x(i_param.heel_zmp_offset_x);
+  std::vector<double> tmp_ratio(i_param.toe_heel_phase_ratio.get_buffer(), i_param.toe_heel_phase_ratio.get_buffer()+i_param.toe_heel_phase_ratio.length());
+  std::cerr << "[" << m_profile.instance_name << "]   "; // for set_toe_heel_phase_ratio
+  gg->set_toe_heel_phase_ratio(tmp_ratio);
+  gg->set_use_toe_joint(i_param.use_toe_joint);
+  gg->set_use_toe_heel_transition(i_param.use_toe_heel_transition);
+  gg->set_zmp_weight_map(boost::assign::map_list_of<leg_type, double>(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3]));
+  gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num);
+  gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset);
+  gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation);
+  gg->set_use_stride_limitation(i_param.use_stride_limitation);
+
+  // print
+  gg->print_param(std::string(m_profile.instance_name));
+  return true;
+};
+
+bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
+{
+  gg->get_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]);
+  std::vector<hrp::Vector3> off;
+  gg->get_leg_default_translate_pos(off);
+  i_param.leg_default_translate_pos.length(off.size());
+  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
+      i_param.leg_default_translate_pos[i].length(3);
+      i_param.leg_default_translate_pos[i][0] = off[i](0);
+      i_param.leg_default_translate_pos[i][1] = off[i](1);
+      i_param.leg_default_translate_pos[i][2] = off[i](2);
+  }
+  i_param.default_step_time = gg->get_default_step_time();
+  i_param.default_step_height = gg->get_default_step_height();
+  i_param.default_double_support_ratio_before = gg->get_default_double_support_ratio_before();
+  i_param.default_double_support_ratio_after = gg->get_default_double_support_ratio_after();
+  i_param.default_double_support_static_ratio_before = gg->get_default_double_support_static_ratio_before();
+  i_param.default_double_support_static_ratio_after = gg->get_default_double_support_static_ratio_after();
+  i_param.default_double_support_ratio_swing_before = gg->get_default_double_support_ratio_swing_before();
+  i_param.default_double_support_ratio_swing_after = gg->get_default_double_support_ratio_swing_after();
+  i_param.default_double_support_ratio = i_param.default_double_support_ratio_before + i_param.default_double_support_ratio_after;
+  i_param.default_double_support_static_ratio = i_param.default_double_support_static_ratio_before + i_param.default_double_support_static_ratio_after;
+  if (gg->get_default_orbit_type() == SHUFFLING) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::SHUFFLING;
+  } else if (gg->get_default_orbit_type() == CYCLOID) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOID;
+  } else if (gg->get_default_orbit_type() == RECTANGLE) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::RECTANGLE;
+  } else if (gg->get_default_orbit_type() == STAIR) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::STAIR;
+  } else if (gg->get_default_orbit_type() == CYCLOIDDELAY) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAY;
+  } else if (gg->get_default_orbit_type() == CYCLOIDDELAYKICK) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK;
+  } else if (gg->get_default_orbit_type() == CROSS) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CROSS;
+  }
+
+  hrp::Vector3 tmpv = gg->get_stair_trajectory_way_point_offset();
+  for (size_t i = 0; i < 3; i++) i_param.stair_trajectory_way_point_offset[i] = tmpv(i);
+  tmpv = gg->get_cycloid_delay_kick_point_offset();
+  for (size_t i = 0; i < 3; i++) i_param.cycloid_delay_kick_point_offset[i] = tmpv(i);
+  i_param.swing_trajectory_delay_time_offset = gg->get_swing_trajectory_delay_time_offset();
+  i_param.swing_trajectory_final_distance_weight = gg->get_swing_trajectory_final_distance_weight();
+  i_param.gravitational_acceleration = gg->get_gravitational_acceleration();
+  i_param.toe_angle = gg->get_toe_angle();
+  i_param.heel_angle = gg->get_heel_angle();
+  i_param.toe_pos_offset_x = gg->get_toe_pos_offset_x();
+  i_param.heel_pos_offset_x = gg->get_heel_pos_offset_x();
+  i_param.toe_zmp_offset_x = gg->get_toe_zmp_offset_x();
+  i_param.heel_zmp_offset_x = gg->get_heel_zmp_offset_x();
+  std::vector<double> ratio(gg->get_NUM_TH_PHASES(),0.0);
+  gg->get_toe_heel_phase_ratio(ratio);
+  for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) i_param.toe_heel_phase_ratio[i] = ratio[i];
+  i_param.use_toe_joint = gg->get_use_toe_joint();
+  i_param.use_toe_heel_transition = gg->get_use_toe_heel_transition();
+  std::map<leg_type, double> tmp_zmp_weight_map = gg->get_zmp_weight_map();
+  i_param.zmp_weight_map[0] = tmp_zmp_weight_map[RLEG];
+  i_param.zmp_weight_map[1] = tmp_zmp_weight_map[LLEG];
+  i_param.zmp_weight_map[2] = tmp_zmp_weight_map[RARM];
+  i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM];
+  i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num();
+  i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset();
+  for (size_t i=0; i<4; i++) {
+    i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i);
+  }
+  i_param.use_stride_limitation = gg->get_use_stride_limitation();
+  return true;
+};
+
+bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
+{
+  Guard guard(m_mutex);
+  std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl;
+  double *default_zmp_offsets_array = new double[ikp.size()*3];
+  move_base_gain = i_param.move_base_gain;
+  for (size_t i = 0; i < ikp.size(); i++)
+    for (size_t j = 0; j < 3; j++)
+      default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j];
+  zmp_transition_time = i_param.zmp_transition_time;
+  adjust_footstep_transition_time = i_param.adjust_footstep_transition_time;
+  if (zmp_offset_interpolator->isEmpty()) {
+      zmp_offset_interpolator->clear();
+      zmp_offset_interpolator->go(default_zmp_offsets_array, zmp_transition_time, true);
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets cannot be set because interpolating." << std::endl;
+  }
+  if (control_mode == MODE_IDLE) {
+    switch (i_param.use_force_mode) {
+    case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
+        use_force = MODE_NO_FORCE;
+        break;
+    case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
+        use_force = MODE_REF_FORCE;
+        break;
+    default:
+        break;
+    }
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   use_force_mode cannot be changed to [" << i_param.use_force_mode << "] during MODE_ABC, MODE_SYNC_TO_IDLE or MODE_SYNC_TO_ABC." << std::endl;
+  }
+  graspless_manip_mode = i_param.graspless_manip_mode;
+  graspless_manip_arm = std::string(i_param.graspless_manip_arm);
+  for (size_t j = 0; j < 3; j++)
+      graspless_manip_p_gain[j] = i_param.graspless_manip_p_gain[j];
+  for (size_t j = 0; j < 3; j++)
+      graspless_manip_reference_trans_coords.pos[j] = i_param.graspless_manip_reference_trans_pos[j];
+  graspless_manip_reference_trans_coords.rot = (Eigen::Quaternion<double>(i_param.graspless_manip_reference_trans_rot[0],
+                                                                          i_param.graspless_manip_reference_trans_rot[1],
+                                                                          i_param.graspless_manip_reference_trans_rot[2],
+                                                                          i_param.graspless_manip_reference_trans_rot[3]).normalized().toRotationMatrix()); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+  transition_time = i_param.transition_time;
+  std::vector<std::string> cur_leg_names, dst_leg_names;
+  cur_leg_names = leg_names;
+  for (size_t i = 0; i < i_param.leg_names.length(); i++) {
+      dst_leg_names.push_back(std::string(i_param.leg_names[i]));
+  }
+  std::sort(cur_leg_names.begin(), cur_leg_names.end());
+  std::sort(dst_leg_names.begin(), dst_leg_names.end());
+  if (cur_leg_names != dst_leg_names) {
+      if (leg_names_interpolator->isEmpty()) {
+          leg_names.clear();
+          leg_names = dst_leg_names;
+          if (control_mode == MODE_ABC) {
+              double tmp_ratio = 0.0;
+              leg_names_interpolator->set(&tmp_ratio);
+              tmp_ratio = 1.0;
+              leg_names_interpolator->go(&tmp_ratio, 5.0, true);
+              control_mode = MODE_SYNC_TO_ABC;
+          }
+      }
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   leg_names cannot be set because interpolating." << std::endl;
+  }
+  pos_ik_thre = i_param.pos_ik_thre;
+  rot_ik_thre = i_param.rot_ik_thre;
+  if (!gg_is_walking) {
+      is_hand_fix_mode = i_param.is_hand_fix_mode;
+      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode = " << is_hand_fix_mode << std::endl;
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode cannot be set in (gg_is_walking = true). Current is_hand_fix_mode is " << (is_hand_fix_mode?"true":"false") << std::endl;
+  }
+  if (control_mode == MODE_IDLE) {
+      for (size_t i = 0; i < i_param.end_effector_list.length(); i++) {
+          std::map<std::string, ABCIKparam>::iterator it = ikp.find(std::string(i_param.end_effector_list[i].leg));
+          memcpy(it->second.localPos.data(), i_param.end_effector_list[i].pos, sizeof(double)*3);
+          it->second.localR = (Eigen::Quaternion<double>(i_param.end_effector_list[i].rot[0], i_param.end_effector_list[i].rot[1], i_param.end_effector_list[i].rot[2], i_param.end_effector_list[i].rot[3])).normalized().toRotationMatrix();
+      }
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl;
+  }
+  if (i_param.default_gait_type == OpenHRP::AutoBalancerService::BIPED) {
+      gait_type = BIPED;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::TROT) {
+      gait_type = TROT;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::PACE) {
+      gait_type = PACE;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::CRAWL) {
+      gait_type = CRAWL;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) {
+      gait_type = GALLOP;
+  }
+  for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
+      std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   localR = " << it->second.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
+  }
+
+  std::cerr << "[" << m_profile.instance_name << "]   move_base_gain = " << move_base_gain << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets = ";
+  for (size_t i = 0; i < ikp.size() * 3; i++) {
+      std::cerr << default_zmp_offsets_array[i] << " ";
+  }
+  std::cerr << std::endl;
+  delete[] default_zmp_offsets_array;
+  std::cerr << "[" << m_profile.instance_name << "]   use_force_mode = " << use_force << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_mode = " << graspless_manip_mode << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_arm = " << graspless_manip_arm << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_pos = " << graspless_manip_reference_trans_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_rot = " << graspless_manip_reference_trans_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   transition_time = " << transition_time << "[s], zmp_transition_time = " << zmp_transition_time << "[s], adjust_footstep_transition_time = " << adjust_footstep_transition_time << "[s]" << std::endl;
+  for (std::vector<std::string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) std::cerr << "[" << m_profile.instance_name << "]   leg_names [" << *it << "]" << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   pos_ik_thre = " << pos_ik_thre << "[m], rot_ik_thre = " << rot_ik_thre << "[rad]" << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   default_gait_type = " << gait_type << std::endl;
+  // IK limb parameters
+  std::cerr << "[" << m_profile.instance_name << "]  IK limb parameters" << std::endl;
+  bool is_ik_limb_parameter_valid_length = true;
+  if (i_param.ik_limb_parameters.length() != ee_vec.size()) {
+      is_ik_limb_parameter_valid_length = false;
+      std::cerr << "[" << m_profile.instance_name << "]   ik_limb_parameters invalid length! Cannot be set. (input = " << i_param.ik_limb_parameters.length() << ", desired = " << ee_vec.size() << ")" << std::endl;
+  } else {
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          if (ikp[ee_vec[i]].manip->numJoints() != i_param.ik_limb_parameters[i].ik_optional_weight_vector.length())
+              is_ik_limb_parameter_valid_length = false;
+      }
+      if (is_ik_limb_parameter_valid_length) {
+          for (size_t i = 0; i < ee_vec.size(); i++) {
+              ABCIKparam& param = ikp[ee_vec[i]];
+              const OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i];
+              std::vector<double> ov;
+              ov.resize(param.manip->numJoints());
+              for (size_t j = 0; j < param.manip->numJoints(); j++) {
+                  ov[j] = ilp.ik_optional_weight_vector[j];
+              }
+              param.manip->setOptionalWeightVector(ov);
+              param.manip->setSRGain(ilp.sr_gain);
+              param.avoid_gain = ilp.avoid_gain;
+              param.reference_gain = ilp.reference_gain;
+              param.manip->setManipulabilityLimit(ilp.manipulability_limit);
+          }
+      } else {
+          std::cerr << "[" << m_profile.instance_name << "]   ik_optional_weight_vector invalid length! Cannot be set. (input = [";
+          for (size_t i = 0; i < ee_vec.size(); i++) {
+              std::cerr << i_param.ik_limb_parameters[i].ik_optional_weight_vector.length() << ", ";
+          }
+          std::cerr << "], desired = [";
+          for (size_t i = 0; i < ee_vec.size(); i++) {
+              std::cerr << ikp[ee_vec[i]].manip->numJoints() << ", ";
+          }
+          std::cerr << "])" << std::endl;
+      }
+  }
+  if (is_ik_limb_parameter_valid_length) {
+      std::cerr << "[" << m_profile.instance_name << "]   ik_optional_weight_vectors = ";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          ABCIKparam& param = ikp[ee_vec[i]];
+          std::vector<double> ov;
+          ov.resize(param.manip->numJoints());
+          param.manip->getOptionalWeightVector(ov);
+          std::cerr << "[";
+          for (size_t j = 0; j < param.manip->numJoints(); j++) {
+              std::cerr << ov[j] << " ";
+          }
+          std::cerr << "]";
+      }
+      std::cerr << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   sr_gains = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].manip->getSRGain() << ", ";
+      }
+      std::cerr << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   avoid_gains = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].avoid_gain << ", ";
+      }
+      std::cerr << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   reference_gains = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].reference_gain << ", ";
+      }
+      std::cerr << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   manipulability_limits = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].manip->getManipulabilityLimit() << ", ";
+      }
+      std::cerr << "]" << std::endl;
+  }
+  return true;
+};
+
+bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
+{
+  i_param.move_base_gain = move_base_gain;
+  i_param.default_zmp_offsets.length(ikp.size());
+  for (size_t i = 0; i < ikp.size(); i++) {
+      i_param.default_zmp_offsets[i].length(3);
+      for (size_t j = 0; j < 3; j++) {
+          i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j);
+      }
+  }
+  switch(control_mode) {
+  case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break;
+  case MODE_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_ABC; break;
+  case MODE_SYNC_TO_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_IDLE; break;
+  case MODE_SYNC_TO_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_ABC; break;
+  default: break;
+  }
+  switch(use_force) {
+  case MODE_NO_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_NO_FORCE; break;
+  case MODE_REF_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE; break;
+  default: break;
+  }
+  i_param.graspless_manip_mode = graspless_manip_mode;
+  i_param.graspless_manip_arm = graspless_manip_arm.c_str();
+  for (size_t j = 0; j < 3; j++)
+      i_param.graspless_manip_p_gain[j] = graspless_manip_p_gain[j];
+  for (size_t j = 0; j < 3; j++)
+      i_param.graspless_manip_reference_trans_pos[j] = graspless_manip_reference_trans_coords.pos[j];
+  Eigen::Quaternion<double> qt(graspless_manip_reference_trans_coords.rot);
+  i_param.graspless_manip_reference_trans_rot[0] = qt.w();
+  i_param.graspless_manip_reference_trans_rot[1] = qt.x();
+  i_param.graspless_manip_reference_trans_rot[2] = qt.y();
+  i_param.graspless_manip_reference_trans_rot[3] = qt.z();
+  i_param.transition_time = transition_time;
+  i_param.zmp_transition_time = zmp_transition_time;
+  i_param.adjust_footstep_transition_time = adjust_footstep_transition_time;
+  i_param.leg_names.length(leg_names.size());
+  for (size_t i = 0; i < leg_names.size(); i++) i_param.leg_names[i] = leg_names.at(i).c_str();
+  i_param.pos_ik_thre = pos_ik_thre;
+  i_param.rot_ik_thre = rot_ik_thre;
+  i_param.is_hand_fix_mode = is_hand_fix_mode;
+  i_param.end_effector_list.length(ikp.size());
+  {
+      size_t i = 0;
+      for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
+          copyRatscoords2Footstep(i_param.end_effector_list[i],
+                                  coordinates(it->second.localPos, it->second.localR));
+          i_param.end_effector_list[i].leg = it->first.c_str();
+          i++;
+      }
+  }
+  switch(gait_type) {
+  case BIPED:  i_param.default_gait_type = OpenHRP::AutoBalancerService::BIPED;  break;
+  case TROT:   i_param.default_gait_type = OpenHRP::AutoBalancerService::TROT;   break;
+  case PACE:   i_param.default_gait_type = OpenHRP::AutoBalancerService::PACE;   break;
+  case CRAWL:  i_param.default_gait_type = OpenHRP::AutoBalancerService::CRAWL;  break;
+  case GALLOP: i_param.default_gait_type = OpenHRP::AutoBalancerService::GALLOP; break;
+  default: break;
+  }
+  i_param.ik_limb_parameters.length(ee_vec.size());
+  for (size_t i = 0; i < ee_vec.size(); i++) {
+      ABCIKparam& param = ikp[ee_vec[i]];
+      OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i];
+      ilp.ik_optional_weight_vector.length(param.manip->numJoints());
+      std::vector<double> ov;
+      ov.resize(param.manip->numJoints());
+      param.manip->getOptionalWeightVector(ov);
+      for (size_t j = 0; j < param.manip->numJoints(); j++) {
+          ilp.ik_optional_weight_vector[j] = ov[j];
+      }
+      ilp.sr_gain = param.manip->getSRGain();
+      ilp.avoid_gain = param.avoid_gain;
+      ilp.reference_gain = param.reference_gain;
+      ilp.manipulability_limit = param.manip->getManipulabilityLimit();
+  }
+  return true;
+};
+
+void AutoBalancer::copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs)
+{
+  memcpy(out_fs.pos, in_fs.pos.data(), sizeof(double)*3);
+  Eigen::Quaternion<double> qt(in_fs.rot);
+  out_fs.rot[0] = qt.w();
+  out_fs.rot[1] = qt.x();
+  out_fs.rot[2] = qt.y();
+  out_fs.rot[3] = qt.z();
+};
+
+bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param)
+{
+  copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_leg_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.swing_leg_dst_coords, gg->get_swing_leg_dst_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.dst_foot_midcoords, gg->get_dst_foot_midcoords());
+  if (gg->get_support_leg_names().front() == "rleg") {
+    i_param.support_leg = OpenHRP::AutoBalancerService::RLEG;
+  } else {
+    i_param.support_leg = OpenHRP::AutoBalancerService::LLEG;
+  }
+  switch ( gg->get_current_support_states().front() ) {
+  case BOTH: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::BOTH; break;
+  case RLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::RLEG; break;
+  case LLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::LLEG; break;
+  default: break;
+  }
+  return true;
+};
+
+bool AutoBalancer::adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep)
+{
+  std::cerr << "[" << m_profile.instance_name << "] adjustFootSteps" << std::endl;
+  if (control_mode == MODE_ABC && !gg_is_walking && adjust_footstep_interpolator->isEmpty()) {
+      Guard guard(m_mutex);
+      //
+      hrp::Vector3 eepos, org_mid_rpy, target_mid_rpy;
+      hrp::Matrix33 eerot, tmprot;
+      coordinates org_mid_coords, target_mid_coords;
+      // Get org coords
+      ikp["rleg"].adjust_interpolation_org_p0 = ikp["rleg"].target_p0;
+      ikp["lleg"].adjust_interpolation_org_p0 = ikp["lleg"].target_p0;
+      ikp["rleg"].adjust_interpolation_org_r0 = ikp["rleg"].target_r0;
+      ikp["lleg"].adjust_interpolation_org_r0 = ikp["lleg"].target_r0;
+      mid_coords(org_mid_coords, 0.5,
+                 coordinates(ikp["rleg"].adjust_interpolation_org_p0, ikp["rleg"].adjust_interpolation_org_r0),
+                 coordinates(ikp["lleg"].adjust_interpolation_org_p0, ikp["lleg"].adjust_interpolation_org_r0));
+      org_mid_rpy = hrp::rpyFromRot(org_mid_coords.rot);
+      // Get target coords
+      //   Input : ee coords
+      //   Output : link coords
+      memcpy(eepos.data(), rfootstep.pos, sizeof(double)*3);
+      eerot = (Eigen::Quaternion<double>(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
+      ikp["rleg"].adjust_interpolation_target_r0 = eerot * ikp["rleg"].localR.transpose();
+      ikp["rleg"].adjust_interpolation_target_p0 = eepos - ikp["rleg"].adjust_interpolation_target_r0 * ikp["rleg"].localPos;
+      memcpy(eepos.data(), lfootstep.pos, sizeof(double)*3);
+      eerot = (Eigen::Quaternion<double>(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
+      ikp["lleg"].adjust_interpolation_target_r0 = eerot * ikp["lleg"].localR.transpose();
+      ikp["lleg"].adjust_interpolation_target_p0 = eepos - ikp["lleg"].adjust_interpolation_target_r0 * ikp["lleg"].localPos;
+      mid_coords(target_mid_coords, 0.5,
+                 coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0),
+                 coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
+      coordinates rtrans, ltrans;
+      target_mid_coords.transformation(rtrans, coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0));
+      target_mid_coords.transformation(ltrans, coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
+      target_mid_rpy = hrp::rpyFromRot(target_mid_coords.rot);
+      // Fix target pos => org pos, target yaw => org yaw
+      target_mid_rpy(2) = org_mid_rpy(2);
+      target_mid_coords.rot = hrp::rotFromRpy(target_mid_rpy);
+      target_mid_coords.pos = org_mid_coords.pos;
+      // Calculate rleg and lleg coords
+      coordinates tmpc;
+      tmpc = target_mid_coords;
+      tmpc.transform(rtrans);
+      ikp["rleg"].adjust_interpolation_target_p0 = tmpc.pos;
+      ikp["rleg"].adjust_interpolation_target_r0 = tmpc.rot;
+      tmpc = target_mid_coords;
+      tmpc.transform(ltrans);
+      ikp["lleg"].adjust_interpolation_target_p0 = tmpc.pos;
+      ikp["lleg"].adjust_interpolation_target_r0 = tmpc.rot;
+      // Set interpolator
+      adjust_footstep_interpolator->clear();
+      double tmp = 0.0;
+      adjust_footstep_interpolator->set(&tmp);
+      tmp = 1.0;
+      adjust_footstep_interpolator->go(&tmp, adjust_footstep_transition_time, true);
+  }
+  while (!adjust_footstep_interpolator->isEmpty() )
+    usleep(1000);
+  usleep(1000);
+  return true;
+};
+
+bool AutoBalancer::getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx)
+{
+    std::cerr << "[" << m_profile.instance_name << "] getRemainingFootstepSequence" << std::endl;
+    o_footstep = new OpenHRP::AutoBalancerService::FootstepSequence;
+    if (gg_is_walking) {
+        std::vector< std::vector<step_node> > fsnl = gg->get_remaining_footstep_nodes_list();
+        o_current_fs_idx = gg->get_footstep_index();
+        o_footstep->length(fsnl.size());
+        for (size_t i = 0; i < fsnl.size(); i++) {
+            o_footstep[i].leg = (fsnl[i].front().l_r==RLEG?"rleg":"lleg");
+            copyRatscoords2Footstep(o_footstep[i], fsnl[i].front().worldcoords);
+        }
+    }
+};
+
+bool AutoBalancer::getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep)
+{
+    std::cerr << "[" << m_profile.instance_name << "] getGoPosFootstepsSequence" << std::endl;
+    o_footstep = new OpenHRP::AutoBalancerService::FootstepsSequence;
+    if (gg_is_walking) {
+        std::cerr << "[" << m_profile.instance_name << "] Cannot call getGoPosFootstepsSequence in walking" << std::endl;
+        return false;
+    } else {
+        gg->set_all_limbs(leg_names);
+        std::vector< std::vector<step_node> > new_footstep_nodes_list;
+        coordinates start_ref_coords;
+        std::vector<coordinates> initial_support_legs_coords;
+        std::vector<leg_type> initial_support_legs;
+        bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
+        if (is_valid_gait_type == false) return false;
+        /* go_pos_param_2_footstep_nodes_list_core is const member function  */
+        gg->go_pos_param_2_footstep_nodes_list_core (x, y, th,
+                                                     initial_support_legs_coords, start_ref_coords, initial_support_legs,
+                                                     new_footstep_nodes_list, true, 0);
+        o_footstep->length(new_footstep_nodes_list.size());
+        for (size_t i = 0; i < new_footstep_nodes_list.size(); i++) {
+            o_footstep[i].fs.length(new_footstep_nodes_list.at(i).size());
+            for (size_t j = 0; j < new_footstep_nodes_list.at(i).size(); j++) {
+                leg_type tmp_leg_type = new_footstep_nodes_list.at(i).at(j).l_r;
+                o_footstep[i].fs[j].leg = ((tmp_leg_type == RLEG) ? "rleg":
+                                           (tmp_leg_type == LLEG) ? "lleg":
+                                           (tmp_leg_type == RARM) ? "rarm":
+                                           "larm");
+                copyRatscoords2Footstep(o_footstep[i].fs[j], new_footstep_nodes_list.at(i).at(j).worldcoords);
+            }
+        }
+        return true;
+    }
+};
+
+void AutoBalancer::static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height)
+{
+  hrp::Vector3 target_sbp = hrp::Vector3(0, 0, 0);
+  hrp::Vector3 tmpcog = m_robot->calcCM();
+  switch ( use_force ) {
+  case MODE_REF_FORCE:
+    calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height, ref_forces);
+    tmp_input_sbp = target_sbp - sbp_offset;
+    sbp_cog_offset = tmp_input_sbp - tmpcog;
+    break;
+  case MODE_NO_FORCE:
+    tmp_input_sbp = tmpcog + sbp_cog_offset;
+    break;
+  default: break;
+  }
+};
+
+void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces)
+{
+  hrp::Vector3 denom, nume;
+  /* sb_point[m] = nume[kg * m/s^2 * m] / denom[kg * m/s^2] */
+  double mass = m_robot->totalMass();
+  for (size_t j = 0; j < 2; j++) {
+    nume(j) = mass * gg->get_gravitational_acceleration() * tmpcog(j);
+    denom(j) = mass * gg->get_gravitational_acceleration();
+    for (size_t i = 0; i < sensor_names.size(); i++) {
+      if ( sensor_names[i].find("hsensor") != std::string::npos || sensor_names[i].find("asensor") != std::string::npos ) { // tempolary to get arm force coords
+          hrp::Link* parentlink;
+          hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
+          if (sensor) parentlink = sensor->link;
+          else parentlink = m_vfs[sensor_names[i]].link;
+          for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+              if (it->second.target_link->name == parentlink->name) {
+                  hrp::Vector3 fpos = parentlink->p + parentlink->R * it->second.localPos;
+                  nume(j) += ( (fpos(2) - ref_com_height) * tmp_forces[i](j) - fpos(j) * tmp_forces[i](2) );
+                  denom(j) -= tmp_forces[i](2);
+              }
+          }
+      }
+    }
+    sb_point(j) = nume(j) / denom(j);
+  }
+  sb_point(2) = ref_com_height;
+};
+
+#ifndef rad2deg
+#define rad2deg(rad) (rad * 180 / M_PI)
+#endif
+#ifndef deg2rad
+#define deg2rad(deg) (deg * M_PI / 180)
+#endif
+
+hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_coords)
+{
+  if (graspless_manip_mode) {
+    hrp::Vector3 dp,dr;
+    coordinates ref_hand_coords(gg->get_dst_foot_midcoords()), act_hand_coords;
+    ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords
+    hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos);
+    if ( graspless_manip_arm == "arms" ) {
+        hrp::Vector3 rarm_pos = ikp["rarm"].target_p0 + ikp["rarm"].target_r0 * ikp["rarm"].localPos;
+        hrp::Vector3 larm_pos = ikp["larm"].target_p0 + ikp["larm"].target_r0 * ikp["larm"].localPos;
+        act_hand_coords.pos = (rarm_pos+larm_pos)/2.0;
+        hrp::Vector3 act_y = larm_pos-rarm_pos;
+        act_y(2) = 0;
+        act_y.normalize();
+        hrp::Vector3 ref_y(ref_hand_coords.rot * hrp::Vector3::UnitY());
+        ref_y(2) = 0;
+        ref_y.normalize();
+        dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y))); // fix for rotation
+    } else {
+      ABCIKparam& tmpikp = ikp[graspless_manip_arm];
+      act_hand_coords = rats::coordinates(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos,
+                                          tmpikp.target_r0 * tmpikp.localR);
+      rats::difference_rotation(dr, ref_hand_coords.rot, act_hand_coords.rot);
+      dr(0) = 0; dr(1) = 0;
+    }
+    dp = act_hand_coords.pos - ref_hand_coords.pos
+        + dr.cross(hrp::Vector3(foot_pos - act_hand_coords.pos));
+    dp(2) = 0;
+    hrp::Matrix33 foot_mt(gg->get_dst_foot_midcoords().rot.transpose());
+    //alias(dp) = foot_mt * dp;
+    hrp::Vector3 dp2 = foot_mt * dp;
+    //alias(dr) = foot_mt * dr;
+    return hrp::Vector3(graspless_manip_p_gain[0] * dp2(0)/gg->get_default_step_time(),
+                        graspless_manip_p_gain[1] * dp2(1)/gg->get_default_step_time(),
+                        graspless_manip_p_gain[2] * rad2deg(dr(2))/gg->get_default_step_time());
+  } else {
+    return hrp::Vector3::Zero();
+  }
+};
+
+bool AutoBalancer::calc_inital_support_legs(const double& y, std::vector<coordinates>& initial_support_legs_coords, std::vector<leg_type>& initial_support_legs, coordinates& start_ref_coords) {
+    switch(gait_type) {
+    case BIPED:
+        initial_support_legs_coords = (y > 0 ?
+                                       boost::assign::list_of(ikp["rleg"].target_end_coords)
+                                       : boost::assign::list_of(ikp["lleg"].target_end_coords));
+        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
+        break;
+    case TROT:
+        initial_support_legs_coords = (y > 0 ?
+                                       boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["larm"].target_end_coords)
+                                       : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["rarm"].target_end_coords));
+        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
+        break;
+    case PACE:
+        initial_support_legs_coords = (y > 0 ?
+                                       boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["rarm"].target_end_coords)
+                                       : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["larm"].target_end_coords));
+        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
+        break;
+    case CRAWL:
+        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    case GALLOP:
+        /* at least one leg shoud be in contact */
+        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    default: break;
+    }
+    mid_coords(start_ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
+    return true;
+};
+
+//
+extern "C"
+{
+
+    void AutoBalancerInit(RTC::Manager* manager)
+    {
+        RTC::Properties profile(autobalancer_spec);
+        manager->registerFactory(profile,
+                                 RTC::Create<AutoBalancer>,
+                                 RTC::Delete<AutoBalancer>);
+    }
+
+};
+
+
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp.save b/rtc/AutoBalancer/AutoBalancer.cpp.save
new file mode 100644
index 00000000000..b69bd089db6
--- /dev/null
+++ b/rtc/AutoBalancer/AutoBalancer.cpp.save
@@ -0,0 +1,2219 @@
+// -*- C++ -*-
+/*!
+ * @file  AutoBalancer.cpp
+ * @brief autobalancer component
+ * $Date$
+ *
+ * $Id$
+ */
+
+#include <rtm/CorbaNaming.h>
+#include <hrpModel/Link.h>
+#include <hrpModel/Sensor.h>
+#include <hrpModel/ModelLoaderUtil.h>
+#include "AutoBalancer.h"
+#include <hrpModel/JointPath.h>
+#include <hrpUtil/MatrixSolvers.h>
+#include "hrpsys/util/Hrpsys.h"
+
+
+typedef coil::Guard<coil::Mutex> Guard;
+using namespace rats;
+
+// Module specification
+// <rtc-template block="module_spec">
+static const char* autobalancer_spec[] =
+    {
+        "implementation_id", "AutoBalancer",
+        "type_name",         "AutoBalancer",
+        "description",       "autobalancer component",
+        "version",           HRPSYS_PACKAGE_VERSION,
+        "vendor",            "AIST",
+        "category",          "example",
+        "activity_type",     "DataFlowComponent",
+        "max_instance",      "10",
+        "language",          "C++",
+        "lang_type",         "compile",
+        // Configuration variables
+        "conf.default.debugLevel", "0",
+        ""
+    };
+// </rtc-template>
+
+AutoBalancer::AutoBalancer(RTC::Manager* manager)
+    : RTC::DataFlowComponentBase(manager),
+      // <rtc-template block="initializer">
+      m_qRefIn("qRef", m_qRef),
+      m_basePosIn("basePosIn", m_basePos),
+      m_baseRpyIn("baseRpyIn", m_baseRpy),
+      m_zmpIn("zmpIn", m_zmp),
+      m_optionalDataIn("optionalData", m_optionalData),
+      m_emergencySignalIn("emergencySignal", m_emergencySignal),
+      //ishiguro
+      m_htzmpIn("htzmpIn", m_htzmp),
+      m_htrfwIn("htrfwIn", m_htrfw),
+      m_htlfwIn("htlfwIn", m_htlfw),
+      m_htcomIn("htcomIn", m_htcom),
+      m_htrfIn("htrfIn", m_htrf),
+      m_htlfIn("htlfIn", m_htlf),
+      m_htrhIn("htrhIn", m_htrh),
+      m_htlhIn("htlhIn", m_htlh),
+      m_actzmpIn("actzmpIn", m_actzmp),
+
+      m_qOut("q", m_qRef),
+      m_zmpOut("zmpOut", m_zmp),
+      m_basePosOut("basePosOut", m_basePos),
+      m_baseRpyOut("baseRpyOut", m_baseRpy),
+      m_baseTformOut("baseTformOut", m_baseTform),
+      m_basePoseOut("basePoseOut", m_basePose),
+      m_accRefOut("accRef", m_accRef),
+      m_contactStatesOut("contactStates", m_contactStates),
+      m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime),
+      m_cogOut("cogOut", m_cog),
+      m_AutoBalancerServicePort("AutoBalancerService"),
+      m_walkingStatesOut("walkingStates", m_walkingStates),
+      m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
+      m_legMarginIn("legMargin", m_legMargin),
+      // </rtc-template>
+      gait_type(BIPED),
+      move_base_gain(0.8),
+      m_robot(hrp::BodyPtr()),
+      m_debugLevel(0)
+{
+    m_service0.autobalancer(this);
+}
+
+AutoBalancer::~AutoBalancer()
+{
+}
+
+
+RTC::ReturnCode_t AutoBalancer::onInitialize()
+{
+    std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
+    bindParameter("debugLevel", m_debugLevel, "0");
+
+    // Registration: InPort/OutPort/Service
+    // <rtc-template block="registration">
+    // Set InPort buffers
+    addInPort("qRef", m_qRefIn);
+    addInPort("basePosIn", m_basePosIn);
+    addInPort("baseRpyIn", m_baseRpyIn);
+    addInPort("zmpIn", m_zmpIn);
+    addInPort("optionalData", m_optionalDataIn);
+    addInPort("emergencySignal", m_emergencySignalIn);
+    //ishiguro
+    addInPort("htzmpIn", m_htzmpIn);
+    addInPort("htrfwIn", m_htrfwIn);
+    addInPort("htlfwIn", m_htlfwIn);
+    addInPort("htcomIn", m_htcomIn);
+    addInPort("htrfIn", m_htrfIn);
+    addInPort("htlfIn", m_htlfIn);
+    addInPort("htrhIn", m_htrhIn);
+    addInPort("htlhIn", m_htlhIn);
+    addInPort("actzmpIn", m_actzmpIn);
+
+
+    addInPort("legMargin", m_legMarginIn);
+
+    // Set OutPort buffer
+    addOutPort("q", m_qOut);
+    addOutPort("zmpOut", m_zmpOut);
+    addOutPort("basePosOut", m_basePosOut);
+    addOutPort("baseRpyOut", m_baseRpyOut);
+    addOutPort("baseTformOut", m_baseTformOut);
+    addOutPort("basePoseOut", m_basePoseOut);
+    addOutPort("accRef", m_accRefOut);
+    addOutPort("contactStates", m_contactStatesOut);
+    addOutPort("controlSwingSupportTime", m_controlSwingSupportTimeOut);
+    addOutPort("cogOut", m_cogOut);
+    addOutPort("walkingStates", m_walkingStatesOut);
+    addOutPort("sbpCogOffset", m_sbpCogOffsetOut);
+  
+    // Set service provider to Ports
+    m_AutoBalancerServicePort.registerProvider("service0", "AutoBalancerService", m_service0);
+  
+    // Set service consumers to Ports
+  
+    // Set CORBA Service Ports
+    addPort(m_AutoBalancerServicePort);
+  
+    // </rtc-template>
+    // <rtc-template block="bind_config">
+    // Bind variables and configuration variable
+  
+    // </rtc-template>
+
+    RTC::Properties& prop =  getProperties();
+    coil::stringTo(m_dt, prop["dt"].c_str());
+
+    m_robot = hrp::BodyPtr(new hrp::Body());
+    RTC::Manager& rtcManager = RTC::Manager::instance();
+    std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
+    int comPos = nameServer.find(",");
+    if (comPos < 0){
+        comPos = nameServer.length();
+    }
+    nameServer = nameServer.substr(0, comPos);
+    RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
+    if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
+                                 CosNaming::NamingContext::_duplicate(naming.getRootContext())
+                                 )){
+      std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
+      return RTC::RTC_ERROR;
+    }
+
+    // allocate memory for outPorts
+    m_qRef.data.length(m_robot->numJoints());
+    qorg.resize(m_robot->numJoints());
+    qrefv.resize(m_robot->numJoints());
+    m_baseTform.data.length(12);
+    //ishiguro
+    m_htrfw.data.length(6);
+    m_htlfw.data.length(6);
+
+    control_mode = MODE_IDLE;
+    loop = 0;
+
+    // setting from conf file
+    // GaitGenerator requires abc_leg_offset and abc_stride_parameter in robot conf file
+    // setting leg_pos from conf file
+    coil::vstring leg_offset_str = coil::split(prop["abc_leg_offset"], ",");
+    leg_names.push_back("rleg");
+    leg_names.push_back("lleg");
+
+    // setting from conf file
+    // rleg,TARGET_LINK,BASE_LINK
+    coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
+    size_t prop_num = 10;
+    if (end_effectors_str.size() > 0) {
+      size_t num = end_effectors_str.size()/prop_num;
+      for (size_t i = 0; i < num; i++) {
+        std::string ee_name, ee_target, ee_base;
+        coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
+        coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
+        coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
+        ABCIKparam tp;
+        for (size_t j = 0; j < 3; j++) {
+            coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
+        }
+        double tmpv[4];
+        for (int j = 0; j < 4; j++ ) {
+          coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
+        }
+        tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
+        tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
+        // Fix for toe joint
+        //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
+        //   Without toe joints, "end-effector link == force-sensor link" is assumed.
+        //   With toe joints, "end-effector link != force-sensor link" is assumed.
+        if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
+            std::vector<double> optw(tp.manip->numJoints(), 1.0);
+            optw.back() = 0.0; // Set weight = 0 for toe joint by default
+            tp.manip->setOptionalWeightVector(optw);
+            tp.has_toe_joint = true;
+        } else {
+            tp.has_toe_joint = false;
+        }
+        tp.avoid_gain = 0.001;
+        tp.reference_gain = 0.01;
+        tp.pos_ik_error_count = tp.rot_ik_error_count = 0;
+        ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
+        ikp[ee_name].target_link = m_robot->link(ee_target);
+        ee_vec.push_back(ee_name);
+        std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   target = " << ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
+        contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
+      }
+      m_contactStates.data.length(num);
+      if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
+        m_contactStates.data[contact_states_index_map["rleg"]] = true;
+        m_contactStates.data[contact_states_index_map["lleg"]] = true;
+      }
+      if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) {
+        m_contactStates.data[contact_states_index_map["rarm"]] = false;
+        m_contactStates.data[contact_states_index_map["larm"]] = false;
+      }
+      m_controlSwingSupportTime.data.length(num);
+      for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 0.0;
+    }
+    std::vector<hrp::Vector3> leg_pos;
+    if (leg_offset_str.size() > 0) {
+      hrp::Vector3 leg_offset;
+      for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
+      std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+      leg_pos.push_back(hrp::Vector3(-1*leg_offset));
+      leg_pos.push_back(hrp::Vector3(leg_offset));
+    }
+    if (leg_pos.size() < ikp.size()) {
+        size_t tmp_leg_pos_size = leg_pos.size();
+        for (size_t i = 0; i < ikp.size() - tmp_leg_pos_size; i++) {
+            leg_pos.push_back(hrp::Vector3::Zero());
+        }
+    }
+
+    std::vector<std::pair<hrp::Link*, hrp::Link*> > interlocking_joints;
+    readInterlockingJointsParamFromProperties(interlocking_joints, m_robot, prop["interlocking_joints"], std::string(m_profile.instance_name));
+    if (interlocking_joints.size() > 0) {
+        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            std::cerr << "[" << m_profile.instance_name << "] Interlocking Joints for [" << it->first << "]" << std::endl;
+            it->second.manip->setInterlockingJointPairIndices(interlocking_joints, std::string(m_profile.instance_name));
+        }
+    }
+
+    zmp_offset_interpolator = new interpolator(ikp.size()*3, m_dt);
+    zmp_offset_interpolator->setName(std::string(m_profile.instance_name)+" zmp_offset_interpolator");
+    zmp_transition_time = 1.0;
+    transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator");
+    transition_interpolator_ratio = 1.0;
+    adjust_footstep_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    adjust_footstep_interpolator->setName(std::string(m_profile.instance_name)+" adjust_footstep_interpolator");
+    transition_time = 2.0;
+    adjust_footstep_transition_time = 2.0;
+    leg_names_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator");
+    leg_names_interpolator_ratio = 1.0;
+
+    // setting stride limitations from conf file
+    double stride_fwd_x_limit = 0.15;
+    double stride_y_limit = 0.05;
+    double stride_th_limit = 10;
+    double stride_bwd_x_limit = 0.05;
+    std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_y_limit << "[m], " << stride_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl;
+    if (default_zmp_offsets.size() == 0) {
+      for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
+    }
+    if (leg_offset_str.size() > 0) {
+      gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_y_limit/*[m]*/, stride_th_limit/*[deg]*/, stride_bwd_x_limit/*[m]*/));
+      gg->set_default_zmp_offsets(default_zmp_offsets);
+    }
+    gg_is_walking = gg_solved = false;
+    m_walkingStates.data = false;
+    fix_leg_coords = coordinates();
+
+    // load virtual force sensors
+    readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
+    // ref force port
+    int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
+    int nvforce = m_vfs.size();
+    int nforce  = npforce + nvforce;
+    m_ref_force.resize(nforce);
+    m_ref_forceIn.resize(nforce);
+    m_force.resize(nforce);
+    m_ref_forceOut.resize(nforce);
+    m_limbCOPOffset.resize(nforce);
+    m_limbCOPOffsetOut.resize(nforce);
+    for (unsigned int i=0; i<npforce; i++){
+        sensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
+    }
+    for (unsigned int i=0; i<nvforce; i++){
+        for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
+            if (it->second.id == i) sensor_names.push_back(it->first);
+        }
+    }
+    // set ref force port
+    std::cerr << "[" << m_profile.instance_name << "] force sensor ports (" << nforce << ")" << std::endl;
+    for (unsigned int i=0; i<nforce; i++){
+        m_ref_forceIn[i] = new InPort<TimedDoubleSeq>(std::string("ref_"+sensor_names[i]).c_str(), m_ref_force[i]);
+        m_ref_force[i].data.length(6);
+        registerInPort(std::string("ref_"+sensor_names[i]).c_str(), *m_ref_forceIn[i]);
+        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string("ref_"+sensor_names[i]) << std::endl;
+        ref_forces.push_back(hrp::Vector3(0,0,0));
+    }
+    // set force port
+    for (unsigned int i=0; i<nforce; i++){
+        m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string(sensor_names[i]).c_str(), m_force[i]);
+        m_force[i].data.length(6);
+        m_force[i].data[0] = m_force[i].data[1] = m_force[i].data[2] = 0.0;
+        m_force[i].data[3] = m_force[i].data[4] = m_force[i].data[5] = 0.0;
+        registerOutPort(std::string(sensor_names[i]).c_str(), *m_ref_forceOut[i]);
+        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string(sensor_names[i]) << std::endl;
+    }
+    // set limb cop offset port
+    std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << nforce << ")" << std::endl;
+    for (unsigned int i=0; i<nforce; i++){
+        std::string nm("limbCOPOffset_"+sensor_names[i]);
+        m_limbCOPOffsetOut[i] = new OutPort<TimedPoint3D>(nm.c_str(), m_limbCOPOffset[i]);
+        registerOutPort(nm.c_str(), *m_limbCOPOffsetOut[i]);
+        m_limbCOPOffset[i].data.x = m_limbCOPOffset[i].data.y = m_limbCOPOffset[i].data.z = 0.0;
+        std::cerr << "[" << m_profile.instance_name << "]   name = " << nm << std::endl;
+    }
+    sbp_offset = hrp::Vector3(0,0,0);
+    sbp_cog_offset = hrp::Vector3(0,0,0);
+    //use_force = MODE_NO_FORCE;
+    use_force = MODE_REF_FORCE;
+
+    if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
+      is_legged_robot = true;
+    } else {
+      is_legged_robot = false;
+    }
+
+    m_accRef.data.ax = m_accRef.data.ay = m_accRef.data.az = 0.0;
+    prev_imu_sensor_vel = hrp::Vector3::Zero();
+
+    graspless_manip_mode = false;
+    graspless_manip_arm = "arms";
+    graspless_manip_p_gain = hrp::Vector3::Zero();
+
+    is_stop_mode = false;
+    has_ik_failed = false;
+    is_hand_fix_mode = false;
+
+    pos_ik_thre = 0.1*1e-3; // [m]
+    rot_ik_thre = (1e-2)*M_PI/180.0; // [rad]
+    ik_error_debug_print_freq = static_cast<int>(0.2/m_dt); // once per 0.2 [s]
+
+    //for HumanSynchronizer
+    
+hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer());
+
+    return RTC::RTC_OK;
+}
+
+
+RTC::ReturnCode_t AutoBalancer::onFinalize()
+{
+  delete zmp_offset_interpolator;
+  delete transition_interpolator;
+  delete adjust_footstep_interpolator;
+  delete leg_names_interpolator;
+  return RTC::RTC_OK;
+}
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onStartup(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onShutdown(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id)
+{
+    std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
+    
+    return RTC::RTC_OK;
+}
+
+RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
+{
+  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
+  Guard guard(m_mutex);
+  if (control_mode == MODE_ABC) {
+    control_mode = MODE_SYNC_TO_IDLE;
+    double tmp_ratio = 0.0;
+    transition_interpolator->go(&tmp_ratio, m_dt, true); // sync in one controller loop
+  }
+  return RTC::RTC_OK;
+}
+
+
+
+
+#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
+//#define DEBUGP2 ((loop%200==0))
+#define DEBUGP2 (false)
+RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
+{
+  // std::cerr << "AutoBalancer::onExecute(" << ec_id << ")" << std::endl;
+    loop ++;
+    if (m_qRefIn.isNew()) {
+        m_qRefIn.read();
+    }
+    if (m_basePosIn.isNew()) {
+      m_basePosIn.read();
+      input_basePos(0) = m_basePos.data.x;
+      input_basePos(1) = m_basePos.data.y;
+      input_basePos(2) = m_basePos.data.z;
+    }
+    if (m_baseRpyIn.isNew()) {
+      m_baseRpyIn.read();
+      input_baseRot = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
+    }
+    if (m_zmpIn.isNew()) {
+      m_zmpIn.read();
+      input_zmp(0) = m_zmp.data.x;
+      input_zmp(1) = m_zmp.data.y;
+      input_zmp(2) = m_zmp.data.z;
+    }
+    for (unsigned int i=0; i<m_ref_forceIn.size(); i++){
+        if ( m_ref_forceIn[i]->isNew() ) {
+            m_ref_forceIn[i]->read();
+        }
+    }
+    if (m_optionalDataIn.isNew()) {
+        m_optionalDataIn.read();
+        if (is_legged_robot) {
+          if (m_optionalData.data.length() >= contact_states_index_map.size()*2) {
+            // current optionalData is contactstates x limb and controlSwingSupportTime x limb
+            //   If contactStates in optionalData is 1.0, m_contactStates is true. Otherwise, false.
+            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+                m_contactStates.data[contact_states_index_map[it->first]] = isOptionalDataContact(it->first);
+                m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = m_optionalData.data[contact_states_index_map[it->first]+contact_states_index_map.size()];
+            }
+            if ( !m_contactStates.data[contact_states_index_map["rleg"]] && !m_contactStates.data[contact_states_index_map["lleg"]] ) { // If two feet have no contact, force set double support contact
+              m_contactStates.data[contact_states_index_map["rleg"]] = true;
+              m_contactStates.data[contact_states_index_map["lleg"]] = true;
+            }
+          }
+        }
+    }
+    if (m_emergencySignalIn.isNew()){
+        m_emergencySignalIn.read();
+        // if (!is_stop_mode) {
+        //     std::cerr << "[" << m_profile.instance_name << "] emergencySignal is set!" << std::endl;
+        //     is_stop_mode = true;
+        //     gg->emergency_stop();
+        // }
+    }
+    //for HumanSynchronizer
+    if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
+    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); hp_raw_data.rfw = HumanSynchronizer::DoubleSeqToWrench6(m_htrfw); }
+    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); hp_raw_data.lfw = HumanSynchronizer::DoubleSeqToWrench6(m_htlfw); }
+    if (m_htcomIn.isNew()){ m_htcomIn.read(); hp_raw_data.com = HumanSynchronizer::Point3DToVector3(m_htcom); }
+    if (m_htrfIn.isNew()) { m_htrfIn.read();  hp_raw_data.rf  = HumanSynchronizer::Point3DToVector3(m_htrf); }
+    if (m_htlfIn.isNew()) { m_htlfIn.read();  hp_raw_data.lf  = HumanSynchronizer::Point3DToVector3(m_htlf); }
+    if (m_htrhIn.isNew()) { m_htrhIn.read();  hp_raw_data.rh  = HumanSynchronizer::Point3DToVector3(m_htrh); }
+    if (m_htlhIn.isNew()) { m_htlhIn.read();  hp_raw_data.lh  = HumanSynchronizer::Point3DToVector3(m_htlh); }
+    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
+    hsp->readInput(hp_raw_data);
+
+
+    if (m_legMarginIn.isNew()) {
+      m_legMarginIn.read();
+      for (size_t i = 0; i < 4; i++) {
+        gg->set_leg_margin(m_legMargin.data[i], i);
+      }
+    }
+
+    Guard guard(m_mutex);
+    hrp::Vector3 ref_basePos;
+    hrp::Matrix33 ref_baseRot;
+    hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
+    if ( is_legged_robot ) {
+      gg->proc_zmp_weight_map_interpolation();
+      getCurrentParameters();
+      getTargetParameters();
+      bool is_transition_interpolator_empty = transition_interpolator->isEmpty();
+      if (!is_transition_interpolator_empty) {
+        transition_interpolator->get(&transition_interpolator_ratio, true);
+      } else {
+        transition_interpolator_ratio = 1.0;
+      }
+      if (control_mode != MODE_IDLE ) {
+        solveLimbIK();
+        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
+      } else {
+        rel_ref_zmp = input_zmp;
+      }
+      // transition
+      if (!is_transition_interpolator_empty) {
+        // transition_interpolator_ratio 0=>1 : IDLE => ABC
+        // transition_interpolator_ratio 1=>0 : ABC => IDLE
+        ref_basePos = (1-transition_interpolator_ratio) * input_basePos + transition_interpolator_ratio * m_robot->rootLink()->p;
+        rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp;
+        rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R);
+        for ( int i = 0; i < m_robot->numJoints(); i++ ) {
+          m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q;
+        }
+      } else {
+        ref_basePos = m_robot->rootLink()->p;
+        ref_baseRot = m_robot->rootLink()->R;
+      }
+      // mode change for sync
+      if (control_mode == MODE_SYNC_TO_ABC) {
+        control_mode = MODE_ABC;
+      } else if (control_mode == MODE_SYNC_TO_IDLE && transition_interpolator->isEmpty() ) {
+        std::cerr << "[" << m_profile.instance_name << "] Finished cleanup" << std::endl;
+        control_mode = MODE_IDLE;
+      }
+    }
+    if ( m_qRef.data.length() != 0 ) { // initialized
+      if (is_legged_robot) {
+        for ( int i = 0; i < m_robot->numJoints(); i++ ){
+          m_qRef.data[i] = m_robot->joint(i)->q;
+        }
+      }
+      m_qOut.write();
+    }
+    if (is_legged_robot) {
+      // basePos
+      m_basePos.data.x = ref_basePos(0);
+      m_basePos.data.y = ref_basePos(1);
+      m_basePos.data.z = ref_basePos(2);
+      m_basePos.tm = m_qRef.tm;
+      // baseRpy
+      hrp::Vector3 baseRpy = hrp::rpyFromRot(ref_baseRot);
+      m_baseRpy.data.r = baseRpy(0);
+      m_baseRpy.data.p = baseRpy(1);
+      m_baseRpy.data.y = baseRpy(2);
+      m_baseRpy.tm = m_qRef.tm;
+      // baseTform
+      double *tform_arr = m_baseTform.data.get_buffer();
+      tform_arr[0] = m_basePos.data.x;
+      tform_arr[1] = m_basePos.data.y;
+      tform_arr[2] = m_basePos.data.z;
+      hrp::setMatrix33ToRowMajorArray(ref_baseRot, tform_arr, 3);
+      m_baseTform.tm = m_qRef.tm;
+      // basePose
+      m_basePose.data.position.x = m_basePos.data.x;
+      m_basePose.data.position.y = m_basePos.data.y;
+      m_basePose.data.position.z = m_basePos.data.z;
+      m_basePose.data.orientation.r = m_baseRpy.data.r;
+      m_basePose.data.orientation.p = m_baseRpy.data.p;
+      m_basePose.data.orientation.y = m_baseRpy.data.y;
+      m_basePose.tm = m_qRef.tm;
+      // zmp
+      m_zmp.data.x = rel_ref_zmp(0);
+      m_zmp.data.y = rel_ref_zmp(1);
+      m_zmp.data.z = rel_ref_zmp(2);
+      m_zmp.tm = m_qRef.tm;
+      // cog
+      m_cog.data.x = ref_cog(0);
+      m_cog.data.y = ref_cog(1);
+      m_cog.data.z = ref_cog(2);
+      m_cog.tm = m_qRef.tm;
+      // sbpCogOffset
+      m_sbpCogOffset.data.x = sbp_cog_offset(0);
+      m_sbpCogOffset.data.y = sbp_cog_offset(1);
+      m_sbpCogOffset.data.z = sbp_cog_offset(2);
+      m_sbpCogOffset.tm = m_qRef.tm;
+    }
+    m_basePosOut.write();
+    m_baseRpyOut.write();
+    m_baseTformOut.write();
+    m_basePoseOut.write();
+    m_zmpOut.write();
+    m_cogOut.write();
+    m_sbpCogOffsetOut.write();
+
+
+//printf("m_zmp:%f, abs:%f\n",m_zmp.data.y,m_basePos.data.y+m_zmp.data.y);
+
+    // reference acceleration
+    hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
+    if (sen != NULL) {
+      hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos;
+      hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt;
+      // convert to imu sensor local acceleration
+      hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt;
+      m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2);
+      m_accRefOut.write();
+      prev_imu_sensor_pos = imu_sensor_pos;
+      prev_imu_sensor_vel = imu_sensor_vel;
+    }
+
+    // control parameters
+    m_contactStates.tm = m_qRef.tm;
+    m_contactStatesOut.write();
+    m_controlSwingSupportTime.tm = m_qRef.tm;
+    m_controlSwingSupportTimeOut.write();
+    m_walkingStates.data = gg_is_walking;
+    m_walkingStates.tm = m_qRef.tm;
+    m_walkingStatesOut.write();
+
+    for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
+        m_force[i].tm = m_qRef.tm;
+        m_ref_forceOut[i]->write();
+    }
+
+    for (unsigned int i=0; i<m_limbCOPOffsetOut.size(); i++){
+        m_limbCOPOffset[i].tm = m_qRef.tm;
+        m_limbCOPOffsetOut[i]->write();
+    }
+
+    return RTC::RTC_OK;
+}
+
+void AutoBalancer::getCurrentParameters()
+{
+  current_root_p = m_robot->rootLink()->p;
+  current_root_R = m_robot->rootLink()->R;
+  for ( int i = 0; i < m_robot->numJoints(); i++ ){
+    qorg[i] = m_robot->joint(i)->q;
+  }
+}
+
+void AutoBalancer::getTargetParameters()
+{
+  // joint angles
+  for ( int i = 0; i < m_robot->numJoints(); i++ ){
+    m_robot->joint(i)->q = m_qRef.data[i];
+    qrefv[i] = m_robot->joint(i)->q;
+  }
+  // basepos, rot, zmp
+  m_robot->rootLink()->p = input_basePos;
+  m_robot->rootLink()->R = input_baseRot;
+  m_robot->calcForwardKinematics();
+  //
+  if (control_mode != MODE_IDLE) {
+    coordinates tmp_fix_coords;
+    if (!zmp_offset_interpolator->isEmpty()) {
+      double *default_zmp_offsets_output = new double[ikp.size()*3];
+      zmp_offset_interpolator->get(default_zmp_offsets_output, true);
+      for (size_t i = 0; i < ikp.size(); i++)
+        for (size_t j = 0; j < 3; j++)
+          default_zmp_offsets[i](j) = default_zmp_offsets_output[i*3+j];
+      delete[] default_zmp_offsets_output;
+      if (DEBUGP) {
+        std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl;
+        std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+        for (size_t i = 0; i < leg_names.size(); i++) {
+            std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
+            std::cerr << "[" << m_profile.instance_name << "]   " << leg_names[i] << " = " << default_zmp_offsets[dst->first].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+        }
+      }
+    }
+    if (!leg_names_interpolator->isEmpty()) {
+        leg_names_interpolator->get(&leg_names_interpolator_ratio, true);
+    }else {
+        leg_names_interpolator_ratio = 1.0;
+    }
+    if ( gg_is_walking ) {
+      gg->set_default_zmp_offsets(default_zmp_offsets);
+      gg_solved = gg->proc_one_tick();
+      {
+          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+          coordinates tmpc;
+          // for support leg
+          for (std::vector<step_node>::const_iterator it = gg->get_support_leg_steps().begin(); it != gg->get_support_leg_steps().end(); it++) {
+              coordinates sp_coords = it->worldcoords;
+              coordinates(ikp[leg_type_map[it->l_r]].localPos,
+                          ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc);
+              sp_coords.transform(tmpc);
+              ikp[leg_type_map[it->l_r]].target_p0 = sp_coords.pos;
+              ikp[leg_type_map[it->l_r]].target_r0 = sp_coords.rot;
+          }
+          // for swing leg
+          for (std::vector<step_node>::const_iterator it = gg->get_swing_leg_steps().begin(); it != gg->get_swing_leg_steps().end(); it++) {
+              coordinates sw_coords = it->worldcoords;
+              coordinates(ikp[leg_type_map[it->l_r]].localPos,
+                          ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc);
+              sw_coords.transform(tmpc);
+              ikp[leg_type_map[it->l_r]].target_p0 = sw_coords.pos;
+              ikp[leg_type_map[it->l_r]].target_r0 = sw_coords.rot;
+          }
+      }
+      gg->get_swing_support_mid_coords(tmp_fix_coords);
+      // set contactStates
+      {
+          std::vector<std::string> tmp_current_support_states_names;
+          {
+              std::vector<leg_type> tmp_current_support_states = gg->get_current_support_states();
+              std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+              for (std::vector<leg_type>::const_iterator it = tmp_current_support_states.begin(); it != tmp_current_support_states.end(); it++)
+                  tmp_current_support_states_names.push_back(leg_type_map[*it]);
+          }
+          // Set Contact States for ee not included in leg_names to false
+          for ( std::map<std::string, size_t>::iterator it = contact_states_index_map.begin(); it != contact_states_index_map.end(); it++ ) {
+              m_contactStates.data[it->second] = false;
+          }
+          for (std::vector<std::string>::const_iterator it = leg_names.begin(); it != leg_names.end(); it++) {
+              std::vector<std::string>::const_iterator dst = std::find_if(tmp_current_support_states_names.begin(), tmp_current_support_states_names.end(), boost::lambda::_1 == *it);
+              if (dst != tmp_current_support_states_names.end()) {
+                  m_contactStates.data[contact_states_index_map[*it]] = true;
+              } else {
+                  m_contactStates.data[contact_states_index_map[*it]] = false;
+              }
+          }
+      }
+      // set controlSwingSupportTime
+      {
+          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+          for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
+              std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
+              m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = gg->get_current_swing_time(dst->first);
+          }
+      }
+      // set ref_forces
+      {
+          std::vector<hrp::Vector3> ee_pos;
+          for (size_t i = 0 ; i < leg_names.size(); i++) {
+              ABCIKparam& tmpikp = ikp[leg_names[i]];
+              ee_pos.push_back(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]);
+          }
+          double alpha = (ref_zmp - ee_pos[1]).norm() / (ee_pos[0] - ee_pos[1]).norm();
+          if (alpha>1.0) alpha = 1.0;
+          if (alpha<0.0) alpha = 0.0;
+          if (DEBUGP) {
+          std::cerr << "[" << m_profile.instance_name << "] alpha:" << alpha << std::endl;
+          }
+          double mg = m_robot->totalMass() * gg->get_gravitational_acceleration();
+          m_force[0].data[0] = alpha * mg;
+          m_force[1].data[0] = (1-alpha) * mg;
+      }
+      // set limbCOPOffset
+      {
+          std::vector<std::string> swg_leg_nms = gg->get_swing_leg_names();
+          for (size_t i = 0; i < swg_leg_nms.size(); i++) {
+              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.x = gg->get_swing_foot_zmp_offsets().at(i)(0);
+              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.y = gg->get_swing_foot_zmp_offsets().at(i)(1);
+              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.z = gg->get_swing_foot_zmp_offsets().at(i)(2);
+          }
+      }
+      {
+          std::vector<std::string> sup_leg_nms = gg->get_support_leg_names();
+          for (size_t i = 0; i < sup_leg_nms.size(); i++) {
+              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.x = gg->get_support_foot_zmp_offsets().at(i)(0);
+              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.y = gg->get_support_foot_zmp_offsets().at(i)(1);
+              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.z = gg->get_support_foot_zmp_offsets().at(i)(2);
+          }
+      }
+    } else {
+      tmp_fix_coords = fix_leg_coords;
+      // double support by default
+      {
+          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+          for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
+              std::vector<std::string>::const_iterator dst = std::find_if(leg_names.begin(), leg_names.end(), (boost::lambda::_1 == it->first));
+              if (dst != leg_names.end()) {
+                  m_contactStates.data[contact_states_index_map[it->first]] = true;
+              } else {
+                  m_contactStates.data[contact_states_index_map[it->first]] = false;
+              }
+              // controlSwingSupportTime is not used while double support period, 1.0 is neglected
+              m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = 1.0;
+              std::map<leg_type, std::string>::const_iterator dst2 = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
+              m_limbCOPOffset[contact_states_index_map[it->first]].data.x = default_zmp_offsets.at(dst2->first)(0);
+              m_limbCOPOffset[contact_states_index_map[it->first]].data.y = default_zmp_offsets.at(dst2->first)(1);
+              m_limbCOPOffset[contact_states_index_map[it->first]].data.z = default_zmp_offsets.at(dst2->first)(2);
+          }
+      }
+    }
+    if (!adjust_footstep_interpolator->isEmpty()) {
+        double tmp = 0.0;
+        adjust_footstep_interpolator->get(&tmp, true);
+        //std::cerr << "[" << m_profile.instance_name << "] adjust ratio " << tmp << std::endl;
+        ikp["rleg"].target_p0 = (1-tmp) * ikp["rleg"].adjust_interpolation_org_p0 + tmp*ikp["rleg"].adjust_interpolation_target_p0;
+        ikp["lleg"].target_p0 = (1-tmp) * ikp["lleg"].adjust_interpolation_org_p0 + tmp*ikp["lleg"].adjust_interpolation_target_p0;
+        rats::mid_rot(ikp["rleg"].target_r0, tmp, ikp["rleg"].adjust_interpolation_org_r0, ikp["rleg"].adjust_interpolation_target_r0);
+        rats::mid_rot(ikp["lleg"].target_r0, tmp, ikp["lleg"].adjust_interpolation_org_r0, ikp["lleg"].adjust_interpolation_target_r0);
+        coordinates tmprc, tmplc;
+        tmprc.pos = ikp["rleg"].target_p0 + ikp["rleg"].target_r0 * ikp["rleg"].localPos;
+        tmprc.rot = ikp["rleg"].target_r0 * ikp["rleg"].localR;
+        tmplc.pos = ikp["lleg"].target_p0 + ikp["lleg"].target_r0 * ikp["lleg"].localPos;
+        tmplc.rot = ikp["lleg"].target_r0 * ikp["lleg"].localR;
+        rats::mid_coords(fix_leg_coords, 0.5, tmprc, tmplc);
+        tmp_fix_coords = fix_leg_coords;
+    }
+    // Tempolarily modify tmp_fix_coords
+    // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272
+    {
+      hrp::Vector3 ex = hrp::Vector3::UnitX();
+      hrp::Vector3 ez = hrp::Vector3::UnitZ();
+      hrp::Vector3 xv1 (tmp_fix_coords.rot * ex);
+      xv1(2) = 0.0;
+      xv1.normalize();
+      hrp::Vector3 yv1(ez.cross(xv1));
+      tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2);
+      tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2);
+      tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2);
+    }
+    // Fix pos
+    fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot);
+
+    /* update ref_forces ;; sp's absolute -> rmc's absolute */
+    for (size_t i = 0; i < m_ref_forceIn.size(); i++) {
+      hrp::Matrix33 eeR;
+      hrp::Link* parentlink;
+      hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
+      if (sensor) parentlink = sensor->link;
+      else parentlink = m_vfs[sensor_names[i]].link;
+      for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+          if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR;
+      }
+      // End effector frame
+      //ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
+      // world frame
+      ref_forces[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
+    }
+    sbp_offset = tmp_fix_coords.rot * hrp::Vector3(sbp_offset);
+
+    target_root_p = m_robot->rootLink()->p;
+    target_root_R = m_robot->rootLink()->R;
+    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+      if ( control_mode == MODE_IDLE || std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end() ) {
+        it->second.target_p0 = it->second.target_link->p;
+        it->second.target_r0 = it->second.target_link->R;
+      }
+    }
+    // Just for ik initial value
+    if (control_mode == MODE_SYNC_TO_ABC) {
+        current_root_p = target_root_p;
+        current_root_R = target_root_R;
+        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
+                it->second.target_p0 = it->second.target_link->p;
+                it->second.target_r0 = it->second.target_link->R;
+            }
+        }
+    }
+    // Move hand for hand fix mode
+    //   If arms' ABCIKparam.is_active is true, move hands according to cog velocity.
+    //   If is_hand_fix_mode is false, no hand fix mode and move hands according to cog velocity.
+    //   If is_hand_fix_mode is true, hand fix mode and do not move hands in Y axis in tmp_fix_coords.rot.    
+    if (gg_is_walking) {
+        // hand control while walking = solve hand ik with is_hand_fix_mode and solve hand ik without is_hand_fix_mode
+        bool is_hand_control_while_walking = false;
+        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
+                 && it->first.find("arm") != std::string::npos ) {
+                is_hand_control_while_walking = true;
+            }
+        }
+        if (is_hand_control_while_walking) {
+        //if (false) { // Disabled temporarily
+            // Store hand_fix_initial_offset in the initialization of walking
+            if (is_hand_fix_initial) hand_fix_initial_offset = tmp_fix_coords.rot.transpose() * (hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos);
+            is_hand_fix_initial = false;
+            hrp::Vector3 dif_p = hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos - tmp_fix_coords.rot * hand_fix_initial_offset;
+            if (is_hand_fix_mode) {
+                dif_p = tmp_fix_coords.rot.transpose() * dif_p;
+                dif_p(1) = 0;
+                dif_p = tmp_fix_coords.rot * dif_p;
+            }
+            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+                if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
+                     && it->first.find("arm") != std::string::npos ) {
+                    it->second.target_p0 = it->second.target_p0 + dif_p;
+                }
+            }
+        }
+    }
+
+    hrp::Vector3 tmp_foot_mid_pos(hrp::Vector3::Zero());
+    {
+        std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+        std::map<leg_type, double> zmp_weight_map = gg->get_zmp_weight_map();
+        double sum_of_weight = 0.0;
+        for (size_t i = 0; i < leg_names.size(); i++) {
+            ABCIKparam& tmpikp = ikp[leg_names[i]];
+            // get target_end_coords
+            tmpikp.target_end_coords.pos = tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos;
+            tmpikp.target_end_coords.rot = tmpikp.target_r0 * tmpikp.localR;
+            // for foot_mid_pos
+            std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
+            tmp_foot_mid_pos += (tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]) * zmp_weight_map[dst->first];
+            sum_of_weight += zmp_weight_map[dst->first];
+        }
+        tmp_foot_mid_pos *= (1.0 / sum_of_weight);
+    }
+    //
+    {
+        if ( gg_is_walking && gg->get_lcg_count() == gg->get_overwrite_check_timing()+2 ) {
+            hrp::Vector3 vel_htc(calc_vel_from_hand_error(tmp_fix_coords));
+            gg->set_offset_velocity_param(vel_htc(0), vel_htc(1) ,vel_htc(2));
+        }//  else {
+        //     if ( gg_is_walking && gg->get_lcg_count() == static_cast<size_t>(gg->get_default_step_time()/(2*m_dt))-1) {
+        //         gg->set_offset_velocity_param(0,0,0);
+        //     }
+        // }
+    }
+
+
+    hrp::Vector3 tmp_ref_cog(m_robot->calcCM());
+    if (gg_is_walking) {
+      ref_cog = gg->get_cog();
+    } else {
+      ref_cog = tmp_foot_mid_pos;
+    }
+    ref_cog(2) = tmp_ref_cog(2);
+    if (gg_is_walking) {
+      ref_zmp = gg->get_refzmp();
+    } else {
+      ref_zmp(0) = ref_cog(0);
+      ref_zmp(1) = ref_cog(1);
+      ref_zmp(2) = tmp_foot_mid_pos(2);
+    }
+  }
+};
+
+hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2)
+{
+  hrp::Vector3 vv = axis1.cross(axis2);
+  if (fabs(vv.norm()-0.0) < 1e-5) {
+    return rot;
+  } else {
+    Eigen::AngleAxis<double> tmpr(std::asin(vv.norm()), vv.normalized());
+    return tmpr.toRotationMatrix() * rot;
+  }
+}
+
+void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot)
+{
+  // get current foot mid pos + rot
+  std::vector<coordinates> foot_coords;
+  for (size_t i = 0; i < leg_names.size(); i++) {
+      ABCIKparam& tmpikp = ikp[leg_names[i]];
+      if (leg_names[i].find("leg") != std::string::npos) foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
+                                                                                           (tmpikp.target_link->R * tmpikp.localR)));
+  }
+  coordinates current_foot_mid_coords;
+  multi_mid_coords(current_foot_mid_coords, foot_coords);
+  hrp::Vector3 current_foot_mid_pos = current_foot_mid_coords.pos;
+  hrp::Matrix33 current_foot_mid_rot = current_foot_mid_coords.rot;
+  // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
+  hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose());
+  m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
+  rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
+  m_robot->calcForwardKinematics();
+}
+
+bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param, const std::string& limb_name)
+{
+  param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
+  // IK check
+  hrp::Vector3 vel_p, vel_r;
+  vel_p = param.target_p0 - param.target_link->p;
+  rats::difference_rotation(vel_r, param.target_link->R, param.target_r0);
+  if (vel_p.norm() > pos_ik_thre && transition_interpolator->isEmpty()) {
+      if (param.pos_ik_error_count % ik_error_debug_print_freq == 0) {
+          std::cerr << "[" << m_profile.instance_name << "] Too large IK error in " << limb_name << " (vel_p) = [" << vel_p(0) << " " << vel_p(1) << " " << vel_p(2) << "][m], count = " << param.pos_ik_error_count << std::endl;
+      }
+      param.pos_ik_error_count++;
+      has_ik_failed = true;
+  } else {
+      param.pos_ik_error_count = 0;
+  }
+  if (vel_r.norm() > rot_ik_thre && transition_interpolator->isEmpty()) {
+      if (param.rot_ik_error_count % ik_error_debug_print_freq == 0) {
+          std::cerr << "[" << m_profile.instance_name << "] Too large IK error in " << limb_name << " (vel_r) = [" << vel_r(0) << " " << vel_r(1) << " " << vel_r(2) << "][rad], count = " << param.rot_ik_error_count << std::endl;
+      }
+      param.rot_ik_error_count++;
+      has_ik_failed = true;
+  } else {
+      param.rot_ik_error_count = 0;
+  }
+  return true;
+}
+
+
+
+void AutoBalancer::solveLimbIK ()
+{
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+    if (it->second.is_active) {
+      for ( int j = 0; j < it->second.manip->numJoints(); j++ ){
+	int i = it->second.manip->joint(j)->jointId;
+	m_robot->joint(i)->q = qorg[i];
+      }
+    }
+  }
+  m_robot->rootLink()->p = current_root_p;
+  m_robot->rootLink()->R = current_root_R;
+  m_robot->calcForwardKinematics();
+  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
+  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
+  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
+  dif_cog *= leg_names_interpolator_ratio;
+  dif_cog(2) = m_robot->rootLink()->p(2) - target_root_p(2);
+  m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog;
+  m_robot->rootLink()->R = target_root_R;
+
+
+  //for HumanSynchronizer
+//  static int HumanSyncCountdownNum = 5 * (1/m_dt);
+  if(hsp->startCountdownForHumanSync){
+    std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";//ややCOMのIKに手間取った時プリント
+	  hsp->updateCountDown();
+  }
+  if(hsp->isHumanSyncOn()){
+	  if(hsp->ht_first_call){
+		  hsp->init_wld_rp_basepos = m_robot->rootLink()->p;
+      hsp->setInitOffsetPose();
+      hsp->rp_wld_initpos.com = m_robot->calcCM();
+      hsp->rp_wld_initpos.rf = ikp["rleg"].target_link->p;
+      hsp->rp_wld_initpos.lf = ikp["lleg"].target_link->p;
+      if(ikp.count("rarm"))hsp->rp_wld_initpos.rh = ikp["rarm"].target_link->p;
+      if(ikp.count("larm"))hsp->rp_wld_initpos.lh = ikp["larm"].target_link->p;
+      hsp->ht_first_call = false;
+
+      hsp->calibInitHumanCOMFromZMP();
+
+      std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
+
+	  }
+  }else{
+    hsp->setInitOffsetPose();
+  }
+
+  hsp->update();//////HumanSynchronizerの主要処理
+  if(loop%100==0)hsp->rp_ref_out.print();
+  if(loop%100==0)cerr<<"getUpdateTime:"<<hsp->getUpdateTime()*1000<<"[ms]"<<endl;
+
+  if(hsp->isHumanSyncOn()){
+	  ////////////////////// 重心拘束位置設定 /////////////////////////
+    m_robot->rootLink()->p = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;//まずは重心目標分，体幹を動かす
+    m_robot->rootLink()->p(2) = hsp->init_wld_rp_basepos(2)+ hsp->rp_ref_out.com(2);//高さだけはベース基準
+    m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
+    m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
+  }
+
+  // Fix for toe joint
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+      if (it->second.is_active && it->second.has_toe_joint && gg->get_use_toe_joint()) {
+          int i = it->second.target_link->jointId;
+          if (gg->get_swing_leg_names().front() == it->first) {
+              m_robot->joint(i)->q = qrefv[i] + -1 * gg->get_foot_dif_rot_angle();
+          } else {
+              m_robot->joint(i)->q = qrefv[i];
+          }
+      }
+  }
+  m_robot->calcForwardKinematics();
+
+
+  // additional COM fitting IK for HumanSynchronizer
+  hrp::Vector3 tmp_com_err = hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM();
+  int com_ik_loop=0;
+  const int COM_IK_MAX_LOOP = 10;
+  const double COM_IK_MAX_ERROR = 0.0001;//0.001だとたまにカクッとなる
+  if(hsp->isHumanSyncOn()){
+    ikp["rleg"].target_p0 = hsp->rp_wld_initpos.rf + hsp->rp_ref_out.rf;
+    ikp["lleg"].target_p0 = hsp->rp_wld_initpos.lf + hsp->rp_ref_out.lf;
+    if(ikp.count("rarm"))ikp["rarm"].target_p0 = hsp->rp_wld_initpos.rh + hsp->rp_ref_out.rh;
+    if(ikp.count("larm"))ikp["larm"].target_p0 = hsp->rp_wld_initpos.lh + hsp->rp_ref_out.lh;
+    while(fabs(tmp_com_err(0))>COM_IK_MAX_ERROR || fabs(tmp_com_err(1))>COM_IK_MAX_ERROR || fabs(tmp_com_err(2))>COM_IK_MAX_ERROR){//Z方向のCOM合わせは要注意(たまに怪しい)
+		  m_robot->rootLink()->p(0) += tmp_com_err(0);
+      m_robot->rootLink()->p(1) += tmp_com_err(1);
+      m_robot->rootLink()->p(2) += tmp_com_err(2);
+		  m_robot->calcForwardKinematics();
+		  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+		    if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
+		  }
+		  m_robot->calcForwardKinematics();
+      tmp_com_err = (hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM());
+		  if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "[" << m_profile.instance_name << "] COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
+	  }
+
+//    static hrp::Vector3 pre_ik_basepos;
+//    for(int i=0;i<3;i++){//重心拘束で解いた結果の体幹軌道の滑らかさは保証されていないので体幹位置に速度制限＆再度IK
+//      if(m_robot->rootLink()->p(i) - pre_ik_basepos(i) > 0.0002){ m_robot->rootLink()->p(i) =  pre_ik_basepos(i) + 0.0002; }
+//      else if(m_robot->rootLink()->p(i) - pre_ik_basepos(i) < -0.0002){ m_robot->rootLink()->p(i) =  pre_ik_basepos(i) - 0.0002; }
+//    }
+//    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+//      if (it->second.is_active) solveLimbIKforLimb(it->second);
+//    }
+//    pre_ik_basepos = m_robot->rootLink()->p;
+
+    //outport用のデータ上書き
+    ref_zmp(0) = hsp->rp_ref_out.zmp(0);
+    ref_zmp(1) = hsp->rp_ref_out.zmp(1);
+    ref_zmp(2) = 0;
+    ref_cog = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;
+
+	  if(loop%100==0)if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
+
+
+//	  printf("root:%f, calcCM:%f, refCM:%f\n",m_robot->rootLink()->p(1),m_robot->calcCM()(1),hsp->rp_ref_out.com(1) + hsp->rp_wld_initpos.com(1));
+
+
+  }else{
+	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
+	    if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
+	  }
+  }
+  if (gg_is_walking && !gg_solved) stopWalking ();
+}
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onAborting(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onError(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onReset(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onStateUpdate(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+/*
+  RTC::ReturnCode_t AutoBalancer::onRateChanged(RTC::UniqueId ec_id)
+  {
+  return RTC::RTC_OK;
+  }
+*/
+
+bool AutoBalancer::startHumanSyncAfter5sec()
+{
+  std::cerr << "[" << m_profile.instance_name << "] start HumanSync after 5 sec" << std::endl;
+  hsp->startCountdownForHumanSync = true;
+  return true;
+}
+
+bool AutoBalancer::setHumanToRobotRatio(const double h2r)
+{
+  std::cerr << "[" << m_profile.instance_name << "] set h2r_ratio as "<< h2r << std::endl;
+  if(!hsp->isHumanSyncOn())hsp->h2r_ratio = h2r;
+  return true;
+}
+
+bool AutoBalancer::setAllowedXYZSync(const bool x_on,const bool y_on,const bool z_on)
+{
+  std::cerr << "[" << m_profile.instance_name << "] set allowed XYZ move direction as ("<<x_on<<","<<y_on<<","<<z_on<<")" << std::endl;
+  hsp->use_x = x_on;
+  hsp->use_y = y_on;
+  hsp->use_z = z_on;
+  return true;
+}
+
+bool AutoBalancer::stopHumanSync()
+{
+	std::cerr << "[" << m_profile.instance_name << "] stop HumanSync Now" << std::endl;
+	return true;
+}
+
+void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence& limbs)
+{
+  std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
+  Guard guard(m_mutex);
+  double tmp_ratio = 0.0;
+  transition_interpolator->clear();
+  transition_interpolator->set(&tmp_ratio);
+  tmp_ratio = 1.0;
+  transition_interpolator->go(&tmp_ratio, transition_time, true);
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+    it->second.is_active = false;
+  }
+
+  for (size_t i = 0; i < limbs.length(); i++) {
+    ABCIKparam& tmp = ikp[std::string(limbs[i])];
+    tmp.is_active = true;
+    std::cerr << "[" << m_profile.instance_name << "]   limb [" << std::string(limbs[i]) << "]" << std::endl;
+  }
+
+  control_mode = MODE_SYNC_TO_ABC;
+}
+
+void AutoBalancer::stopABCparam()
+{
+  std::cerr << "[" << m_profile.instance_name << "] stop auto balancer mode" << std::endl;
+  //Guard guard(m_mutex);
+  double tmp_ratio = 1.0;
+  transition_interpolator->clear();
+  transition_interpolator->set(&tmp_ratio);
+  tmp_ratio = 0.0;
+  transition_interpolator->go(&tmp_ratio, transition_time, true);
+  control_mode = MODE_SYNC_TO_IDLE;
+}
+
+void AutoBalancer::startWalking ()
+{
+  if ( control_mode != MODE_ABC ) {
+    return_control_mode = control_mode;
+    OpenHRP::AutoBalancerService::StrSequence fix_limbs;
+    fix_limbs.length(2);
+    fix_limbs[0] = "rleg";
+    fix_limbs[1] = "lleg";
+    startABCparam(fix_limbs);
+    waitABCTransition();
+  }
+  {
+    Guard guard(m_mutex);
+    has_ik_failed = false;
+    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+        it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
+    }
+    std::vector<std::string> init_swing_leg_names(gg->get_footstep_front_leg_names());
+    std::vector<std::string> tmp_all_limbs(leg_names);
+    std::vector<std::string> init_support_leg_names;
+    std::sort(tmp_all_limbs.begin(), tmp_all_limbs.end());
+    std::sort(init_swing_leg_names.begin(), init_swing_leg_names.end());
+    std::set_difference(tmp_all_limbs.begin(), tmp_all_limbs.end(),
+                        init_swing_leg_names.begin(), init_swing_leg_names.end(),
+                        std::back_inserter(init_support_leg_names));
+    std::vector<step_node> init_support_leg_steps, init_swing_leg_dst_steps;
+    for (std::vector<std::string>::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++)
+        init_support_leg_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0));
+    for (std::vector<std::string>::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++)
+        init_swing_leg_dst_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0));
+    gg->set_default_zmp_offsets(default_zmp_offsets);
+    gg->initialize_gait_parameter(ref_cog, init_support_leg_steps, init_swing_leg_dst_steps);
+  }
+  is_hand_fix_initial = true;
+  while ( !gg->proc_one_tick() );
+  {
+    Guard guard(m_mutex);
+    gg_is_walking = gg_solved = true;
+  }
+}
+
+void AutoBalancer::stopWalking ()
+{
+  std::vector<coordinates> tmp_end_coords_list;
+  for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
+      if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(ikp[*it].target_end_coords);
+  }
+  multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
+  fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot);
+  gg->clear_footstep_nodes_list();
+  if (return_control_mode == MODE_IDLE) stopABCparam();
+  gg_is_walking = false;
+}
+
+bool AutoBalancer::startAutoBalancer (const OpenHRP::AutoBalancerService::StrSequence& limbs)
+{
+  if (control_mode == MODE_IDLE) {
+    has_ik_failed = false;
+    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+        it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
+    }
+    startABCparam(limbs);
+    waitABCTransition();
+    return_control_mode = MODE_ABC;
+    return true;
+  } else {
+    return false;
+  }
+}
+
+bool AutoBalancer::stopAutoBalancer ()
+{
+  if (control_mode == MODE_ABC) {
+    stopABCparam();
+    waitABCTransition();
+    return true;
+  } else {
+    return false;
+  }
+}
+
+void AutoBalancer::waitABCTransition()
+{
+  while (!transition_interpolator->isEmpty()) usleep(1000);
+  usleep(1000);
+}
+bool AutoBalancer::goPos(const double& x, const double& y, const double& th)
+{
+    //  if ( !gg_is_walking && !is_stop_mode) {
+  if ( !is_stop_mode) {
+    gg->set_all_limbs(leg_names);
+    coordinates start_ref_coords;
+    std::vector<coordinates> initial_support_legs_coords;
+    std::vector<leg_type> initial_support_legs;
+    bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
+    if (is_valid_gait_type == false) return false;
+    bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th,
+                                                      initial_support_legs_coords, // Dummy if gg_is_walking
+                                                      start_ref_coords,            // Dummy if gg_is_walking
+                                                      initial_support_legs,        // Dummy if gg_is_walking
+                                                      (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps.
+    if ( !gg_is_walking ) { // Initializing
+        startWalking();
+    }
+    if (!ret) {
+        std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl;
+    }
+    return ret;
+  } else {
+    std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl;
+    return false;
+  }
+}
+
+bool AutoBalancer::goVelocity(const double& vx, const double& vy, const double& vth)
+{
+  gg->set_all_limbs(leg_names);
+  if (gg_is_walking && gg_solved) {
+    gg->set_velocity_param(vx, vy, vth);
+  } else {
+    coordinates ref_coords;
+    mid_coords(ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
+    std::vector<leg_type> current_legs;
+    switch(gait_type) {
+    case BIPED:
+        current_legs = (vy > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
+        break;
+    case TROT:
+        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
+        break;
+    case PACE:
+        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
+        break;
+    case CRAWL:
+        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    case GALLOP:
+        /* at least one leg shoud be in contact */
+        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    default: break;
+    }
+    gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs);
+    startWalking();
+  }
+  return true;
+}
+
+bool AutoBalancer::goStop ()
+{
+  gg->finalize_velocity_mode();
+  waitFootSteps();
+  return true;
+}
+
+bool AutoBalancer::emergencyStop ()
+{
+  std::cerr << "[" << m_profile.instance_name << "] emergencyStop" << std::endl;
+  // is_stop_mode = true;
+  gg->emergency_stop();
+  waitFootSteps();
+  return true;
+}
+
+bool AutoBalancer::releaseEmergencyStop ()
+{
+  if (is_stop_mode) {
+      std::cerr << "[" << m_profile.instance_name << "] releaseEmergencyStop" << std::endl;
+      is_stop_mode = false;
+  }
+  return true;
+}
+
+bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
+{
+  OpenHRP::AutoBalancerService::StepParamsSequence spss;
+  spss.length(fss.length());
+  // If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
+  // If gg_is_walking is true, do not set to 0.
+  for (size_t i = 0; i < spss.length(); i++) {
+      spss[i].sps.length(fss[i].fs.length());
+      for (size_t j = 0; j < spss[i].sps.length(); j++) {
+          spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
+          spss[i].sps[j].step_time = gg->get_default_step_time();
+          spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle());
+          spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle());
+      }
+  }
+  setFootStepsWithParam(fss, spss, overwrite_fs_idx);
+}
+
+bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
+{
+    if (!is_stop_mode) {
+        std::cerr << "[" << m_profile.instance_name << "] setFootStepsList" << std::endl;
+
+        // Initial footstep Snapping
+        coordinates tmpfs, fstrans;
+        step_node initial_support_step, initial_input_step;
+        {
+            std::vector<step_node> initial_support_steps;
+            if (gg_is_walking) {
+                if (overwrite_fs_idx <= 0) {
+                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
+                    return false;
+                }
+                if (!gg->get_footstep_nodes_by_index(initial_support_steps, overwrite_fs_idx-1)) {
+                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
+                    return false;
+                }
+            } else {
+                // If walking, snap initial leg to current ABC foot coords.
+                for (size_t i = 0; i < fss[0].fs.length(); i++) {
+                    initial_support_steps.push_back(step_node(std::string(fss[0].fs[i].leg), ikp[std::string(fss[0].fs[i].leg)].target_end_coords, 0, 0, 0, 0));
+                }
+            }
+            initial_support_step = initial_support_steps.front(); /* use only one leg for representation */
+        }
+        {
+            std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+            for (size_t i = 0; i < fss[0].fs.length(); i++) {
+                if (std::string(fss[0].fs[i].leg) == leg_type_map[initial_support_step.l_r]) {
+                    coordinates tmp;
+                    memcpy(tmp.pos.data(), fss[0].fs[i].pos, sizeof(double)*3);
+                    tmp.rot = (Eigen::Quaternion<double>(fss[0].fs[i].rot[0], fss[0].fs[i].rot[1], fss[0].fs[i].rot[2], fss[0].fs[i].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+                    initial_input_step = step_node(std::string(fss[0].fs[i].leg), tmp, 0, 0, 0, 0);
+                }
+            }
+        }
+
+        // Get footsteps
+        std::vector< std::vector<coordinates> > fs_vec_list;
+        std::vector< std::vector<std::string> > leg_name_vec_list;
+        for (size_t i = 0; i < fss.length(); i++) {
+            std::vector<coordinates> fs_vec;
+            std::vector<std::string> leg_name_vec;
+            for (size_t j = 0; j < fss[i].fs.length(); j++) {
+                std::string leg(fss[i].fs[j].leg);
+                if (std::find(leg_names.begin(), leg_names.end(), leg) != leg_names.end()) {
+                    memcpy(tmpfs.pos.data(), fss[i].fs[j].pos, sizeof(double)*3);
+                    tmpfs.rot = (Eigen::Quaternion<double>(fss[i].fs[j].rot[0], fss[i].fs[j].rot[1], fss[i].fs[j].rot[2], fss[i].fs[j].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+                    initial_input_step.worldcoords.transformation(fstrans, tmpfs);
+                    tmpfs = initial_support_step.worldcoords;
+                    tmpfs.transform(fstrans);
+                } else {
+                    std::cerr << "[" << m_profile.instance_name << "]   No such target : " << leg << std::endl;
+                    return false;
+                }
+                leg_name_vec.push_back(leg);
+                fs_vec.push_back(tmpfs);
+            }
+            leg_name_vec_list.push_back(leg_name_vec);
+            fs_vec_list.push_back(fs_vec);
+        }
+        if (spss.length() != fs_vec_list.size()) {
+            std::cerr << "[" << m_profile.instance_name << "]   StepParam length " << spss.length () << " != Footstep length " << fs_vec_list.size() << std::endl;
+            return false;
+        }
+        std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl;
+        std::vector< std::vector<step_node> > fnsl;
+        for (size_t i = 0; i < fs_vec_list.size(); i++) {
+            if (!(gg_is_walking && i == 0)) { // If initial footstep, e.g., not walking, pass user-defined footstep list. If walking, pass cdr footsteps in order to neglect initial double support leg.
+                std::vector<step_node> tmp_fns;
+                for (size_t j = 0; j < fs_vec_list.at(i).size(); j++) {
+                    tmp_fns.push_back(step_node(leg_name_vec_list[i][j], fs_vec_list[i][j], spss[i].sps[j].step_height, spss[i].sps[j].step_time, spss[i].sps[j].toe_angle, spss[i].sps[j].heel_angle));
+                }
+                fnsl.push_back(tmp_fns);
+            }
+        }
+        if (gg_is_walking) {
+            std::cerr << "[" << m_profile.instance_name << "]  Set overwrite footsteps" << std::endl;
+            gg->set_overwrite_foot_steps_list(fnsl);
+            gg->set_overwrite_foot_step_index(overwrite_fs_idx);
+        } else {
+            std::cerr << "[" << m_profile.instance_name << "]  Set normal footsteps" << std::endl;
+            gg->set_foot_steps_list(fnsl);
+            startWalking();
+        }
+        return true;
+    } else {
+        std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl;
+        return false;
+    }
+}
+
+void AutoBalancer::waitFootSteps()
+{
+  //while (gg_is_walking) usleep(10);
+  while (gg_is_walking || !transition_interpolator->isEmpty() )
+    usleep(1000);
+  usleep(1000);
+  gg->set_offset_velocity_param(0,0,0);
+}
+
+void AutoBalancer::waitFootStepsEarly(const double tm)
+{
+  if (!gg_is_walking) { return;}
+  while ( !gg->is_finalizing(tm)|| !transition_interpolator->isEmpty() )
+    usleep(1000);
+  usleep(1000);
+  gg->set_offset_velocity_param(0,0,0);
+}
+
+bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
+{
+  std::cerr << "[" << m_profile.instance_name << "] setGaitGeneratorParam" << std::endl;
+  gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]);
+  std::vector<hrp::Vector3> off;
+  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
+      off.push_back(hrp::Vector3(i_param.leg_default_translate_pos[i][0], i_param.leg_default_translate_pos[i][1], i_param.leg_default_translate_pos[i][2]));
+  }
+  gg->set_leg_default_translate_pos(off);
+  gg->set_default_step_time(i_param.default_step_time);
+  gg->set_default_step_height(i_param.default_step_height);
+  gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio/2.0);
+  gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio/2.0);
+  gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio/2.0);
+  gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio/2.0);
+  gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio/2.0);
+  gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio/2.0);
+  // gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio_before);
+  // gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio_after);
+  // gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio_before);
+  // gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio_after);
+  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_before);
+  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_after);
+  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_swing_before);
+  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_swing_after);
+  if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::SHUFFLING) {
+    gg->set_default_orbit_type(SHUFFLING);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOID) {
+    gg->set_default_orbit_type(CYCLOID);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::RECTANGLE) {
+    gg->set_default_orbit_type(RECTANGLE);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::STAIR) {
+    gg->set_default_orbit_type(STAIR);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAY) {
+    gg->set_default_orbit_type(CYCLOIDDELAY);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK) {
+    gg->set_default_orbit_type(CYCLOIDDELAYKICK);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CROSS) {
+    gg->set_default_orbit_type(CROSS);
+  }
+  gg->set_swing_trajectory_delay_time_offset(i_param.swing_trajectory_delay_time_offset);
+  gg->set_swing_trajectory_final_distance_weight(i_param.swing_trajectory_final_distance_weight);
+  gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2]));
+  gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2]));  
+  gg->set_gravitational_acceleration(i_param.gravitational_acceleration);
+  gg->set_toe_angle(i_param.toe_angle);
+  gg->set_heel_angle(i_param.heel_angle);
+  gg->set_toe_pos_offset_x(i_param.toe_pos_offset_x);
+  gg->set_heel_pos_offset_x(i_param.heel_pos_offset_x);
+  gg->set_toe_zmp_offset_x(i_param.toe_zmp_offset_x);
+  gg->set_heel_zmp_offset_x(i_param.heel_zmp_offset_x);
+  std::vector<double> tmp_ratio(i_param.toe_heel_phase_ratio.get_buffer(), i_param.toe_heel_phase_ratio.get_buffer()+i_param.toe_heel_phase_ratio.length());
+  std::cerr << "[" << m_profile.instance_name << "]   "; // for set_toe_heel_phase_ratio
+  gg->set_toe_heel_phase_ratio(tmp_ratio);
+  gg->set_use_toe_joint(i_param.use_toe_joint);
+  gg->set_use_toe_heel_transition(i_param.use_toe_heel_transition);
+  gg->set_zmp_weight_map(boost::assign::map_list_of<leg_type, double>(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3]));
+  gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num);
+  gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset);
+  gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation);
+  gg->set_use_stride_limitation(i_param.use_stride_limitation);
+
+  // print
+  gg->print_param(std::string(m_profile.instance_name));
+  return true;
+};
+
+bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
+{
+  gg->get_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]);
+  std::vector<hrp::Vector3> off;
+  gg->get_leg_default_translate_pos(off);
+  i_param.leg_default_translate_pos.length(off.size());
+  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
+      i_param.leg_default_translate_pos[i].length(3);
+      i_param.leg_default_translate_pos[i][0] = off[i](0);
+      i_param.leg_default_translate_pos[i][1] = off[i](1);
+      i_param.leg_default_translate_pos[i][2] = off[i](2);
+  }
+  i_param.default_step_time = gg->get_default_step_time();
+  i_param.default_step_height = gg->get_default_step_height();
+  i_param.default_double_support_ratio_before = gg->get_default_double_support_ratio_before();
+  i_param.default_double_support_ratio_after = gg->get_default_double_support_ratio_after();
+  i_param.default_double_support_static_ratio_before = gg->get_default_double_support_static_ratio_before();
+  i_param.default_double_support_static_ratio_after = gg->get_default_double_support_static_ratio_after();
+  i_param.default_double_support_ratio_swing_before = gg->get_default_double_support_ratio_swing_before();
+  i_param.default_double_support_ratio_swing_after = gg->get_default_double_support_ratio_swing_after();
+  i_param.default_double_support_ratio = i_param.default_double_support_ratio_before + i_param.default_double_support_ratio_after;
+  i_param.default_double_support_static_ratio = i_param.default_double_support_static_ratio_before + i_param.default_double_support_static_ratio_after;
+  if (gg->get_default_orbit_type() == SHUFFLING) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::SHUFFLING;
+  } else if (gg->get_default_orbit_type() == CYCLOID) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOID;
+  } else if (gg->get_default_orbit_type() == RECTANGLE) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::RECTANGLE;
+  } else if (gg->get_default_orbit_type() == STAIR) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::STAIR;
+  } else if (gg->get_default_orbit_type() == CYCLOIDDELAY) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAY;
+  } else if (gg->get_default_orbit_type() == CYCLOIDDELAYKICK) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK;
+  } else if (gg->get_default_orbit_type() == CROSS) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CROSS;
+  }
+
+  hrp::Vector3 tmpv = gg->get_stair_trajectory_way_point_offset();
+  for (size_t i = 0; i < 3; i++) i_param.stair_trajectory_way_point_offset[i] = tmpv(i);
+  tmpv = gg->get_cycloid_delay_kick_point_offset();
+  for (size_t i = 0; i < 3; i++) i_param.cycloid_delay_kick_point_offset[i] = tmpv(i);
+  i_param.swing_trajectory_delay_time_offset = gg->get_swing_trajectory_delay_time_offset();
+  i_param.swing_trajectory_final_distance_weight = gg->get_swing_trajectory_final_distance_weight();
+  i_param.gravitational_acceleration = gg->get_gravitational_acceleration();
+  i_param.toe_angle = gg->get_toe_angle();
+  i_param.heel_angle = gg->get_heel_angle();
+  i_param.toe_pos_offset_x = gg->get_toe_pos_offset_x();
+  i_param.heel_pos_offset_x = gg->get_heel_pos_offset_x();
+  i_param.toe_zmp_offset_x = gg->get_toe_zmp_offset_x();
+  i_param.heel_zmp_offset_x = gg->get_heel_zmp_offset_x();
+  std::vector<double> ratio(gg->get_NUM_TH_PHASES(),0.0);
+  gg->get_toe_heel_phase_ratio(ratio);
+  for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) i_param.toe_heel_phase_ratio[i] = ratio[i];
+  i_param.use_toe_joint = gg->get_use_toe_joint();
+  i_param.use_toe_heel_transition = gg->get_use_toe_heel_transition();
+  std::map<leg_type, double> tmp_zmp_weight_map = gg->get_zmp_weight_map();
+  i_param.zmp_weight_map[0] = tmp_zmp_weight_map[RLEG];
+  i_param.zmp_weight_map[1] = tmp_zmp_weight_map[LLEG];
+  i_param.zmp_weight_map[2] = tmp_zmp_weight_map[RARM];
+  i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM];
+  i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num();
+  i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset();
+  for (size_t i=0; i<4; i++) {
+    i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i);
+  }
+  i_param.use_stride_limitation = gg->get_use_stride_limitation();
+  return true;
+};
+
+bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
+{
+  Guard guard(m_mutex);
+  std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl;
+  double *default_zmp_offsets_array = new double[ikp.size()*3];
+  move_base_gain = i_param.move_base_gain;
+  for (size_t i = 0; i < ikp.size(); i++)
+    for (size_t j = 0; j < 3; j++)
+      default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j];
+  zmp_transition_time = i_param.zmp_transition_time;
+  adjust_footstep_transition_time = i_param.adjust_footstep_transition_time;
+  if (zmp_offset_interpolator->isEmpty()) {
+      zmp_offset_interpolator->clear();
+      zmp_offset_interpolator->go(default_zmp_offsets_array, zmp_transition_time, true);
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets cannot be set because interpolating." << std::endl;
+  }
+  if (control_mode == MODE_IDLE) {
+    switch (i_param.use_force_mode) {
+    case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
+        use_force = MODE_NO_FORCE;
+        break;
+    case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
+        use_force = MODE_REF_FORCE;
+        break;
+    default:
+        break;
+    }
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   use_force_mode cannot be changed to [" << i_param.use_force_mode << "] during MODE_ABC, MODE_SYNC_TO_IDLE or MODE_SYNC_TO_ABC." << std::endl;
+  }
+  graspless_manip_mode = i_param.graspless_manip_mode;
+  graspless_manip_arm = std::string(i_param.graspless_manip_arm);
+  for (size_t j = 0; j < 3; j++)
+      graspless_manip_p_gain[j] = i_param.graspless_manip_p_gain[j];
+  for (size_t j = 0; j < 3; j++)
+      graspless_manip_reference_trans_coords.pos[j] = i_param.graspless_manip_reference_trans_pos[j];
+  graspless_manip_reference_trans_coords.rot = (Eigen::Quaternion<double>(i_param.graspless_manip_reference_trans_rot[0],
+                                                                          i_param.graspless_manip_reference_trans_rot[1],
+                                                                          i_param.graspless_manip_reference_trans_rot[2],
+                                                                          i_param.graspless_manip_reference_trans_rot[3]).normalized().toRotationMatrix()); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+  transition_time = i_param.transition_time;
+  std::vector<std::string> cur_leg_names, dst_leg_names;
+  cur_leg_names = leg_names;
+  for (size_t i = 0; i < i_param.leg_names.length(); i++) {
+      dst_leg_names.push_back(std::string(i_param.leg_names[i]));
+  }
+  std::sort(cur_leg_names.begin(), cur_leg_names.end());
+  std::sort(dst_leg_names.begin(), dst_leg_names.end());
+  if (cur_leg_names != dst_leg_names) {
+      if (leg_names_interpolator->isEmpty()) {
+          leg_names.clear();
+          leg_names = dst_leg_names;
+          if (control_mode == MODE_ABC) {
+              double tmp_ratio = 0.0;
+              leg_names_interpolator->set(&tmp_ratio);
+              tmp_ratio = 1.0;
+              leg_names_interpolator->go(&tmp_ratio, 5.0, true);
+              control_mode = MODE_SYNC_TO_ABC;
+          }
+      }
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   leg_names cannot be set because interpolating." << std::endl;
+  }
+  pos_ik_thre = i_param.pos_ik_thre;
+  rot_ik_thre = i_param.rot_ik_thre;
+  if (!gg_is_walking) {
+      is_hand_fix_mode = i_param.is_hand_fix_mode;
+      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode = " << is_hand_fix_mode << std::endl;
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode cannot be set in (gg_is_walking = true). Current is_hand_fix_mode is " << (is_hand_fix_mode?"true":"false") << std::endl;
+  }
+  if (control_mode == MODE_IDLE) {
+      for (size_t i = 0; i < i_param.end_effector_list.length(); i++) {
+          std::map<std::string, ABCIKparam>::iterator it = ikp.find(std::string(i_param.end_effector_list[i].leg));
+          memcpy(it->second.localPos.data(), i_param.end_effector_list[i].pos, sizeof(double)*3);
+          it->second.localR = (Eigen::Quaternion<double>(i_param.end_effector_list[i].rot[0], i_param.end_effector_list[i].rot[1], i_param.end_effector_list[i].rot[2], i_param.end_effector_list[i].rot[3])).normalized().toRotationMatrix();
+      }
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl;
+  }
+  if (i_param.default_gait_type == OpenHRP::AutoBalancerService::BIPED) {
+      gait_type = BIPED;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::TROT) {
+      gait_type = TROT;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::PACE) {
+      gait_type = PACE;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::CRAWL) {
+      gait_type = CRAWL;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) {
+      gait_type = GALLOP;
+  }
+  for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
+      std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   localR = " << it->second.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
+  }
+
+  std::cerr << "[" << m_profile.instance_name << "]   move_base_gain = " << move_base_gain << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets = ";
+  for (size_t i = 0; i < ikp.size() * 3; i++) {
+      std::cerr << default_zmp_offsets_array[i] << " ";
+  }
+  std::cerr << std::endl;
+  delete[] default_zmp_offsets_array;
+  std::cerr << "[" << m_profile.instance_name << "]   use_force_mode = " << use_force << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_mode = " << graspless_manip_mode << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_arm = " << graspless_manip_arm << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_pos = " << graspless_manip_reference_trans_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_rot = " << graspless_manip_reference_trans_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   transition_time = " << transition_time << "[s], zmp_transition_time = " << zmp_transition_time << "[s], adjust_footstep_transition_time = " << adjust_footstep_transition_time << "[s]" << std::endl;
+  for (std::vector<std::string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) std::cerr << "[" << m_profile.instance_name << "]   leg_names [" << *it << "]" << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   pos_ik_thre = " << pos_ik_thre << "[m], rot_ik_thre = " << rot_ik_thre << "[rad]" << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   default_gait_type = " << gait_type << std::endl;
+  // IK limb parameters
+  std::cerr << "[" << m_profile.instance_name << "]  IK limb parameters" << std::endl;
+  bool is_ik_limb_parameter_valid_length = true;
+  if (i_param.ik_limb_parameters.length() != ee_vec.size()) {
+      is_ik_limb_parameter_valid_length = false;
+      std::cerr << "[" << m_profile.instance_name << "]   ik_limb_parameters invalid length! Cannot be set. (input = " << i_param.ik_limb_parameters.length() << ", desired = " << ee_vec.size() << ")" << std::endl;
+  } else {
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          if (ikp[ee_vec[i]].manip->numJoints() != i_param.ik_limb_parameters[i].ik_optional_weight_vector.length())
+              is_ik_limb_parameter_valid_length = false;
+      }
+      if (is_ik_limb_parameter_valid_length) {
+          for (size_t i = 0; i < ee_vec.size(); i++) {
+              ABCIKparam& param = ikp[ee_vec[i]];
+              const OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i];
+              std::vector<double> ov;
+              ov.resize(param.manip->numJoints());
+              for (size_t j = 0; j < param.manip->numJoints(); j++) {
+                  ov[j] = ilp.ik_optional_weight_vector[j];
+              }
+              param.manip->setOptionalWeightVector(ov);
+              param.manip->setSRGain(ilp.sr_gain);
+              param.avoid_gain = ilp.avoid_gain;
+              param.reference_gain = ilp.reference_gain;
+              param.manip->setManipulabilityLimit(ilp.manipulability_limit);
+          }
+      } else {
+          std::cerr << "[" << m_profile.instance_name << "]   ik_optional_weight_vector invalid length! Cannot be set. (input = [";
+          for (size_t i = 0; i < ee_vec.size(); i++) {
+              std::cerr << i_param.ik_limb_parameters[i].ik_optional_weight_vector.length() << ", ";
+          }
+          std::cerr << "], desired = [";
+          for (size_t i = 0; i < ee_vec.size(); i++) {
+              std::cerr << ikp[ee_vec[i]].manip->numJoints() << ", ";
+          }
+          std::cerr << "])" << std::endl;
+      }
+  }
+  if (is_ik_limb_parameter_valid_length) {
+      std::cerr << "[" << m_profile.instance_name << "]   ik_optional_weight_vectors = ";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          ABCIKparam& param = ikp[ee_vec[i]];
+          std::vector<double> ov;
+          ov.resize(param.manip->numJoints());
+          param.manip->getOptionalWeightVector(ov);
+          std::cerr << "[";
+          for (size_t j = 0; j < param.manip->numJoints(); j++) {
+              std::cerr << ov[j] << " ";
+          }
+          std::cerr << "]";
+      }
+      std::cerr << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   sr_gains = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].manip->getSRGain() << ", ";
+      }
+      std::cerr << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   avoid_gains = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].avoid_gain << ", ";
+      }
+      std::cerr << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   reference_gains = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].reference_gain << ", ";
+      }
+      std::cerr << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   manipulability_limits = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].manip->getManipulabilityLimit() << ", ";
+      }
+      std::cerr << "]" << std::endl;
+  }
+  return true;
+};
+
+bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
+{
+  i_param.move_base_gain = move_base_gain;
+  i_param.default_zmp_offsets.length(ikp.size());
+  for (size_t i = 0; i < ikp.size(); i++) {
+      i_param.default_zmp_offsets[i].length(3);
+      for (size_t j = 0; j < 3; j++) {
+          i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j);
+      }
+  }
+  switch(control_mode) {
+  case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break;
+  case MODE_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_ABC; break;
+  case MODE_SYNC_TO_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_IDLE; break;
+  case MODE_SYNC_TO_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_ABC; break;
+  default: break;
+  }
+  switch(use_force) {
+  case MODE_NO_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_NO_FORCE; break;
+  case MODE_REF_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE; break;
+  default: break;
+  }
+  i_param.graspless_manip_mode = graspless_manip_mode;
+  i_param.graspless_manip_arm = graspless_manip_arm.c_str();
+  for (size_t j = 0; j < 3; j++)
+      i_param.graspless_manip_p_gain[j] = graspless_manip_p_gain[j];
+  for (size_t j = 0; j < 3; j++)
+      i_param.graspless_manip_reference_trans_pos[j] = graspless_manip_reference_trans_coords.pos[j];
+  Eigen::Quaternion<double> qt(graspless_manip_reference_trans_coords.rot);
+  i_param.graspless_manip_reference_trans_rot[0] = qt.w();
+  i_param.graspless_manip_reference_trans_rot[1] = qt.x();
+  i_param.graspless_manip_reference_trans_rot[2] = qt.y();
+  i_param.graspless_manip_reference_trans_rot[3] = qt.z();
+  i_param.transition_time = transition_time;
+  i_param.zmp_transition_time = zmp_transition_time;
+  i_param.adjust_footstep_transition_time = adjust_footstep_transition_time;
+  i_param.leg_names.length(leg_names.size());
+  for (size_t i = 0; i < leg_names.size(); i++) i_param.leg_names[i] = leg_names.at(i).c_str();
+  i_param.pos_ik_thre = pos_ik_thre;
+  i_param.rot_ik_thre = rot_ik_thre;
+  i_param.is_hand_fix_mode = is_hand_fix_mode;
+  i_param.end_effector_list.length(ikp.size());
+  {
+      size_t i = 0;
+      for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
+          copyRatscoords2Footstep(i_param.end_effector_list[i],
+                                  coordinates(it->second.localPos, it->second.localR));
+          i_param.end_effector_list[i].leg = it->first.c_str();
+          i++;
+      }
+  }
+  switch(gait_type) {
+  case BIPED:  i_param.default_gait_type = OpenHRP::AutoBalancerService::BIPED;  break;
+  case TROT:   i_param.default_gait_type = OpenHRP::AutoBalancerService::TROT;   break;
+  case PACE:   i_param.default_gait_type = OpenHRP::AutoBalancerService::PACE;   break;
+  case CRAWL:  i_param.default_gait_type = OpenHRP::AutoBalancerService::CRAWL;  break;
+  case GALLOP: i_param.default_gait_type = OpenHRP::AutoBalancerService::GALLOP; break;
+  default: break;
+  }
+  i_param.ik_limb_parameters.length(ee_vec.size());
+  for (size_t i = 0; i < ee_vec.size(); i++) {
+      ABCIKparam& param = ikp[ee_vec[i]];
+      OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i];
+      ilp.ik_optional_weight_vector.length(param.manip->numJoints());
+      std::vector<double> ov;
+      ov.resize(param.manip->numJoints());
+      param.manip->getOptionalWeightVector(ov);
+      for (size_t j = 0; j < param.manip->numJoints(); j++) {
+          ilp.ik_optional_weight_vector[j] = ov[j];
+      }
+      ilp.sr_gain = param.manip->getSRGain();
+      ilp.avoid_gain = param.avoid_gain;
+      ilp.reference_gain = param.reference_gain;
+      ilp.manipulability_limit = param.manip->getManipulabilityLimit();
+  }
+  return true;
+};
+
+void AutoBalancer::copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs)
+{
+  memcpy(out_fs.pos, in_fs.pos.data(), sizeof(double)*3);
+  Eigen::Quaternion<double> qt(in_fs.rot);
+  out_fs.rot[0] = qt.w();
+  out_fs.rot[1] = qt.x();
+  out_fs.rot[2] = qt.y();
+  out_fs.rot[3] = qt.z();
+};
+
+bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param)
+{
+  copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_leg_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.swing_leg_dst_coords, gg->get_swing_leg_dst_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.dst_foot_midcoords, gg->get_dst_foot_midcoords());
+  if (gg->get_support_leg_names().front() == "rleg") {
+    i_param.support_leg = OpenHRP::AutoBalancerService::RLEG;
+  } else {
+    i_param.support_leg = OpenHRP::AutoBalancerService::LLEG;
+  }
+  switch ( gg->get_current_support_states().front() ) {
+  case BOTH: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::BOTH; break;
+  case RLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::RLEG; break;
+  case LLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::LLEG; break;
+  default: break;
+  }
+  return true;
+};
+
+bool AutoBalancer::adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep)
+{
+  std::cerr << "[" << m_profile.instance_name << "] adjustFootSteps" << std::endl;
+  if (control_mode == MODE_ABC && !gg_is_walking && adjust_footstep_interpolator->isEmpty()) {
+      Guard guard(m_mutex);
+      //
+      hrp::Vector3 eepos, org_mid_rpy, target_mid_rpy;
+      hrp::Matrix33 eerot, tmprot;
+      coordinates org_mid_coords, target_mid_coords;
+      // Get org coords
+      ikp["rleg"].adjust_interpolation_org_p0 = ikp["rleg"].target_p0;
+      ikp["lleg"].adjust_interpolation_org_p0 = ikp["lleg"].target_p0;
+      ikp["rleg"].adjust_interpolation_org_r0 = ikp["rleg"].target_r0;
+      ikp["lleg"].adjust_interpolation_org_r0 = ikp["lleg"].target_r0;
+      mid_coords(org_mid_coords, 0.5,
+                 coordinates(ikp["rleg"].adjust_interpolation_org_p0, ikp["rleg"].adjust_interpolation_org_r0),
+                 coordinates(ikp["lleg"].adjust_interpolation_org_p0, ikp["lleg"].adjust_interpolation_org_r0));
+      org_mid_rpy = hrp::rpyFromRot(org_mid_coords.rot);
+      // Get target coords
+      //   Input : ee coords
+      //   Output : link coords
+      memcpy(eepos.data(), rfootstep.pos, sizeof(double)*3);
+      eerot = (Eigen::Quaternion<double>(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
+      ikp["rleg"].adjust_interpolation_target_r0 = eerot * ikp["rleg"].localR.transpose();
+      ikp["rleg"].adjust_interpolation_target_p0 = eepos - ikp["rleg"].adjust_interpolation_target_r0 * ikp["rleg"].localPos;
+      memcpy(eepos.data(), lfootstep.pos, sizeof(double)*3);
+      eerot = (Eigen::Quaternion<double>(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
+      ikp["lleg"].adjust_interpolation_target_r0 = eerot * ikp["lleg"].localR.transpose();
+      ikp["lleg"].adjust_interpolation_target_p0 = eepos - ikp["lleg"].adjust_interpolation_target_r0 * ikp["lleg"].localPos;
+      mid_coords(target_mid_coords, 0.5,
+                 coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0),
+                 coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
+      coordinates rtrans, ltrans;
+      target_mid_coords.transformation(rtrans, coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0));
+      target_mid_coords.transformation(ltrans, coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
+      target_mid_rpy = hrp::rpyFromRot(target_mid_coords.rot);
+      // Fix target pos => org pos, target yaw => org yaw
+      target_mid_rpy(2) = org_mid_rpy(2);
+      target_mid_coords.rot = hrp::rotFromRpy(target_mid_rpy);
+      target_mid_coords.pos = org_mid_coords.pos;
+      // Calculate rleg and lleg coords
+      coordinates tmpc;
+      tmpc = target_mid_coords;
+      tmpc.transform(rtrans);
+      ikp["rleg"].adjust_interpolation_target_p0 = tmpc.pos;
+      ikp["rleg"].adjust_interpolation_target_r0 = tmpc.rot;
+      tmpc = target_mid_coords;
+      tmpc.transform(ltrans);
+      ikp["lleg"].adjust_interpolation_target_p0 = tmpc.pos;
+      ikp["lleg"].adjust_interpolation_target_r0 = tmpc.rot;
+      // Set interpolator
+      adjust_footstep_interpolator->clear();
+      double tmp = 0.0;
+      adjust_footstep_interpolator->set(&tmp);
+      tmp = 1.0;
+      adjust_footstep_interpolator->go(&tmp, adjust_footstep_transition_time, true);
+  }
+  while (!adjust_footstep_interpolator->isEmpty() )
+    usleep(1000);
+  usleep(1000);
+  return true;
+};
+
+bool AutoBalancer::getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx)
+{
+    std::cerr << "[" << m_profile.instance_name << "] getRemainingFootstepSequence" << std::endl;
+    o_footstep = new OpenHRP::AutoBalancerService::FootstepSequence;
+    if (gg_is_walking) {
+        std::vector< std::vector<step_node> > fsnl = gg->get_remaining_footstep_nodes_list();
+        o_current_fs_idx = gg->get_footstep_index();
+        o_footstep->length(fsnl.size());
+        for (size_t i = 0; i < fsnl.size(); i++) {
+            o_footstep[i].leg = (fsnl[i].front().l_r==RLEG?"rleg":"lleg");
+            copyRatscoords2Footstep(o_footstep[i], fsnl[i].front().worldcoords);
+        }
+    }
+};
+
+bool AutoBalancer::getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep)
+{
+    std::cerr << "[" << m_profile.instance_name << "] getGoPosFootstepsSequence" << std::endl;
+    o_footstep = new OpenHRP::AutoBalancerService::FootstepsSequence;
+    if (gg_is_walking) {
+        std::cerr << "[" << m_profile.instance_name << "] Cannot call getGoPosFootstepsSequence in walking" << std::endl;
+        return false;
+    } else {
+        gg->set_all_limbs(leg_names);
+        std::vector< std::vector<step_node> > new_footstep_nodes_list;
+        coordinates start_ref_coords;
+        std::vector<coordinates> initial_support_legs_coords;
+        std::vector<leg_type> initial_support_legs;
+        bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
+        if (is_valid_gait_type == false) return false;
+        /* go_pos_param_2_footstep_nodes_list_core is const member function  */
+        gg->go_pos_param_2_footstep_nodes_list_core (x, y, th,
+                                                     initial_support_legs_coords, start_ref_coords, initial_support_legs,
+                                                     new_footstep_nodes_list, true, 0);
+        o_footstep->length(new_footstep_nodes_list.size());
+        for (size_t i = 0; i < new_footstep_nodes_list.size(); i++) {
+            o_footstep[i].fs.length(new_footstep_nodes_list.at(i).size());
+            for (size_t j = 0; j < new_footstep_nodes_list.at(i).size(); j++) {
+                leg_type tmp_leg_type = new_footstep_nodes_list.at(i).at(j).l_r;
+                o_footstep[i].fs[j].leg = ((tmp_leg_type == RLEG) ? "rleg":
+                                           (tmp_leg_type == LLEG) ? "lleg":
+                                           (tmp_leg_type == RARM) ? "rarm":
+                                           "larm");
+                copyRatscoords2Footstep(o_footstep[i].fs[j], new_footstep_nodes_list.at(i).at(j).worldcoords);
+            }
+        }
+        return true;
+    }
+};
+
+void AutoBalancer::static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height)
+{
+  hrp::Vector3 target_sbp = hrp::Vector3(0, 0, 0);
+  hrp::Vector3 tmpcog = m_robot->calcCM();
+  switch ( use_force ) {
+  case MODE_REF_FORCE:
+    calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height, ref_forces);
+    tmp_input_sbp = target_sbp - sbp_offset;
+    sbp_cog_offset = tmp_input_sbp - tmpcog;
+    break;
+  case MODE_NO_FORCE:
+    tmp_input_sbp = tmpcog + sbp_cog_offset;
+    break;
+  default: break;
+  }
+};
+
+void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces)
+{
+  hrp::Vector3 denom, nume;
+  /* sb_point[m] = nume[kg * m/s^2 * m] / denom[kg * m/s^2] */
+  double mass = m_robot->totalMass();
+  for (size_t j = 0; j < 2; j++) {
+    nume(j) = mass * gg->get_gravitational_acceleration() * tmpcog(j);
+    denom(j) = mass * gg->get_gravitational_acceleration();
+    for (size_t i = 0; i < sensor_names.size(); i++) {
+      if ( sensor_names[i].find("hsensor") != std::string::npos || sensor_names[i].find("asensor") != std::string::npos ) { // tempolary to get arm force coords
+          hrp::Link* parentlink;
+          hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
+          if (sensor) parentlink = sensor->link;
+          else parentlink = m_vfs[sensor_names[i]].link;
+          for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+              if (it->second.target_link->name == parentlink->name) {
+                  hrp::Vector3 fpos = parentlink->p + parentlink->R * it->second.localPos;
+                  nume(j) += ( (fpos(2) - ref_com_height) * tmp_forces[i](j) - fpos(j) * tmp_forces[i](2) );
+                  denom(j) -= tmp_forces[i](2);
+              }
+          }
+      }
+    }
+    sb_point(j) = nume(j) / denom(j);
+  }
+  sb_point(2) = ref_com_height;
+};
+
+#ifndef rad2deg
+#define rad2deg(rad) (rad * 180 / M_PI)
+#endif
+#ifndef deg2rad
+#define deg2rad(deg) (deg * M_PI / 180)
+#endif
+
+hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_coords)
+{
+  if (graspless_manip_mode) {
+    hrp::Vector3 dp,dr;
+    coordinates ref_hand_coords(gg->get_dst_foot_midcoords()), act_hand_coords;
+    ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords
+    hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos);
+    if ( graspless_manip_arm == "arms" ) {
+        hrp::Vector3 rarm_pos = ikp["rarm"].target_p0 + ikp["rarm"].target_r0 * ikp["rarm"].localPos;
+        hrp::Vector3 larm_pos = ikp["larm"].target_p0 + ikp["larm"].target_r0 * ikp["larm"].localPos;
+        act_hand_coords.pos = (rarm_pos+larm_pos)/2.0;
+        hrp::Vector3 act_y = larm_pos-rarm_pos;
+        act_y(2) = 0;
+        act_y.normalize();
+        hrp::Vector3 ref_y(ref_hand_coords.rot * hrp::Vector3::UnitY());
+        ref_y(2) = 0;
+        ref_y.normalize();
+        dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y))); // fix for rotation
+    } else {
+      ABCIKparam& tmpikp = ikp[graspless_manip_arm];
+      act_hand_coords = rats::coordinates(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos,
+                                          tmpikp.target_r0 * tmpikp.localR);
+      rats::difference_rotation(dr, ref_hand_coords.rot, act_hand_coords.rot);
+      dr(0) = 0; dr(1) = 0;
+    }
+    dp = act_hand_coords.pos - ref_hand_coords.pos
+        + dr.cross(hrp::Vector3(foot_pos - act_hand_coords.pos));
+    dp(2) = 0;
+    hrp::Matrix33 foot_mt(gg->get_dst_foot_midcoords().rot.transpose());
+    //alias(dp) = foot_mt * dp;
+    hrp::Vector3 dp2 = foot_mt * dp;
+    //alias(dr) = foot_mt * dr;
+    return hrp::Vector3(graspless_manip_p_gain[0] * dp2(0)/gg->get_default_step_time(),
+                        graspless_manip_p_gain[1] * dp2(1)/gg->get_default_step_time(),
+                        graspless_manip_p_gain[2] * rad2deg(dr(2))/gg->get_default_step_time());
+  } else {
+    return hrp::Vector3::Zero();
+  }
+};
+
+bool AutoBalancer::calc_inital_support_legs(const double& y, std::vector<coordinates>& initial_support_legs_coords, std::vector<leg_type>& initial_support_legs, coordinates& start_ref_coords) {
+    switch(gait_type) {
+    case BIPED:
+        initial_support_legs_coords = (y > 0 ?
+                                       boost::assign::list_of(ikp["rleg"].target_end_coords)
+                                       : boost::assign::list_of(ikp["lleg"].target_end_coords));
+        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
+        break;
+    case TROT:
+        initial_support_legs_coords = (y > 0 ?
+                                       boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["larm"].target_end_coords)
+                                       : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["rarm"].target_end_coords));
+        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
+        break;
+    case PACE:
+        initial_support_legs_coords = (y > 0 ?
+                                       boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["rarm"].target_end_coords)
+                                       : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["larm"].target_end_coords));
+        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
+        break;
+    case CRAWL:
+        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    case GALLOP:
+        /* at least one leg shoud be in contact */
+        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    default: break;
+    }
+    mid_coords(start_ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
+    return true;
+};
+
+//
+extern "C"
+{
+
+    void AutoBalancerInit(RTC::Manager* manager)
+    {
+        RTC::Properties profile(autobalancer_spec);
+        manager->registerFactory(profile,
+                                 RTC::Create<AutoBalancer>,
+                                 RTC::Delete<AutoBalancer>);
+    }
+
+};
+
+
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index efd59c1360a..bdd2ddb85ca 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -114,6 +114,7 @@ class HumanSynchronizer{
     HumanPose rp_ref_out_old;
     int countdown_num;
     bool HumanSyncOn;
+    bool is_rf_in_air,is_lf_in_air;
     struct timeval t_calc_start, t_calc_end;
     unsigned int loop;
     double h2r_ratio, tgt_h2r_ratio;
@@ -157,6 +158,7 @@ class HumanSynchronizer{
       startCountdownForHumanSync = false;
       countdown_num = 5*500;
       loop = 0;
+      is_rf_in_air = false; is_lf_in_air = false;
 //      std::string home_path(std::getenv("HOME"));
 //      sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
 //      cz_log = fopen((home_path+"/HumanSync_com_zmp.log").c_str(),"w+");
@@ -289,9 +291,14 @@ class HumanSynchronizer{
       out.com = cmmr * in.com;
     }
     void judgeFootContactStates(const HumanPose::Wrench6& rfw_in, const HumanPose::Wrench6& lfw_in, bool& rfcs_ans, bool& lfcs_ans){
-      if      (rfw_in.f(2)<CNT_F_TH && lfw_in.f(2)>CNT_F_TH){rfcs_ans = false;  lfcs_ans = true;}//右足浮遊かつ左足接地
-      else if (rfw_in.f(2)>CNT_F_TH && lfw_in.f(2)<CNT_F_TH){rfcs_ans = true;   lfcs_ans = false;}//右足接地かつ左足浮遊
-      else                                              {rfcs_ans = true;   lfcs_ans = true;}//両足接地または両足浮遊
+      if      (rfcs_ans  && lfcs_ans && rfw_in.f(2)<CNT_F_TH   ){rfcs_ans = false;}//右足ついた状態から上げる
+      else if (!rfcs_ans && lfcs_ans && rfw_in.f(2)>CNT_F_TH+30){rfcs_ans = true;}//右足浮いた状態から下げる
+      if      (lfcs_ans  && rfcs_ans && lfw_in.f(2)<CNT_F_TH   ){lfcs_ans = false;}//左足ついた状態から上げる
+      else if (!lfcs_ans && rfcs_ans && lfw_in.f(2)>CNT_F_TH+30){lfcs_ans = true;}//左足浮いた状態から下げる
+
+
+      //else if (rfw_in.f(2)>CNT_F_TH && lfw_in.f(2)<CNT_F_TH){rfcs_ans = true;   lfcs_ans = false;}//右足接地かつ左足浮遊
+      //else                                              {rfcs_ans = true;   lfcs_ans = true;}//両足接地または両足浮遊
     }
     void overwriteFootZFromContactStates(const HumanPose& in, const bool& rfcs_in, const bool& lfcs_in, HumanPose& out){
       out = in;
@@ -436,14 +443,14 @@ class HumanSynchronizer{
 //      return true;
 //    }
     bool applyCOMToSupportRegionLimit(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){
-      const double XUMARGIN = 0.04;//CHIDORI
-      const double XLMARGIN = -0.02;
-      const double YUMARGIN = 0.01;
-      const double YLMARGIN = -0.01;
-//      const double XUMARGIN = 0.02;//JAXON
-//      const double XLMARGIN = -0.01;
-//      const double YUMARGIN = 0.0;
-//      const double YLMARGIN = -0.0;
+//      const double XUMARGIN = 0.04;//CHIDORI
+//      const double XLMARGIN = -0.02;
+//      const double YUMARGIN = 0.01;
+//      const double YLMARGIN = -0.01;
+      const double XUMARGIN = 0.02;//JAXON
+      const double XLMARGIN = -0.01;
+      const double YUMARGIN = 0.0;
+      const double YLMARGIN = -0.0;
       std::vector<hrp::Vector2> convex_hull;
       if(lfin_abs(0)>rfin_abs(0)){
         convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN));//LFの左下
diff --git a/rtc/AutoBalancer/AutoBalancer.h.mas b/rtc/AutoBalancer/AutoBalancer.h.mas
new file mode 100644
index 00000000000..be21747854d
--- /dev/null
+++ b/rtc/AutoBalancer/AutoBalancer.h.mas
@@ -0,0 +1,270 @@
+// -*- C++ -*-
+/*!
+ * @file  AutoBalancer.h
+ * @brief autobalancer component
+ * @date  $Date$
+ *
+ * $Id$
+ */
+
+#ifndef AUTOBALANCER_H
+#define AUTOBALANCER_H
+
+#include <rtm/Manager.h>
+#include <rtm/DataFlowComponentBase.h>
+#include <rtm/CorbaPort.h>
+#include <rtm/DataInPort.h>
+#include <rtm/DataOutPort.h>
+#include <rtm/idl/BasicDataTypeSkel.h>
+#include <rtm/idl/ExtendedDataTypesSkel.h>
+#include <hrpModel/Body.h>
+#include "../ImpedanceController/JointPathEx.h"
+#include "../ImpedanceController/RatsMatrix.h"
+#include "GaitGenerator.h"
+// Service implementation headers
+// <rtc-template block="service_impl_h">
+#include "AutoBalancerService_impl.h"
+#include "interpolator.h"
+
+// </rtc-template>
+
+// Service Consumer stub headers
+// <rtc-template block="consumer_stub_h">
+
+// </rtc-template>
+
+using namespace RTC;
+
+class AutoBalancer
+  : public RTC::DataFlowComponentBase
+{
+ public:
+  AutoBalancer(RTC::Manager* manager);
+  virtual ~AutoBalancer();
+
+  // The initialize action (on CREATED->ALIVE transition)
+  // formaer rtc_init_entry()
+ virtual RTC::ReturnCode_t onInitialize();
+
+  // The finalize action (on ALIVE->END transition)
+  // formaer rtc_exiting_entry()
+  virtual RTC::ReturnCode_t onFinalize();
+
+  // The startup action when ExecutionContext startup
+  // former rtc_starting_entry()
+  // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
+
+  // The shutdown action when ExecutionContext stop
+  // former rtc_stopping_entry()
+  // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
+
+  // The activated action (Active state entry action)
+  // former rtc_active_entry()
+  virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
+
+  // The deactivated action (Active state exit action)
+  // former rtc_active_exit()
+  virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
+
+  // The execution action that is invoked periodically
+  // former rtc_active_do()
+  virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
+
+  // The aborting action when main logic error occurred.
+  // former rtc_aborting_entry()
+  // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
+
+  // The error action in ERROR state
+  // former rtc_error_do()
+  // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
+
+  // The reset action that is invoked resetting
+  // This is same but different the former rtc_init_entry()
+  // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
+
+  // The state update action that is invoked after onExecute() action
+  // no corresponding operation exists in OpenRTm-aist-0.2.0
+  // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
+
+  // The action that is invoked when execution context's rate is changed
+  // no corresponding operation exists in OpenRTm-aist-0.2.0
+  // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
+  bool goPos(const double& x, const double& y, const double& th);
+  bool goVelocity(const double& vx, const double& vy, const double& vth);
+  bool goStop();
+  bool emergencyStop ();
+  bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx);
+  bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx);
+  bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx);
+  bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx);
+  void waitFootSteps();
+  void waitFootStepsEarly(const double tm);
+  bool startAutoBalancer(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
+  bool stopAutoBalancer();
+  bool setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param);
+  bool getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param);
+  bool setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param);
+  bool getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param);
+  bool getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param);
+  bool adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep);
+  bool getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx);
+  bool getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep);
+  bool releaseEmergencyStop();
+
+ protected:
+  // Configuration variable declaration
+  // <rtc-template block="config_declare">
+  
+  // </rtc-template>
+
+  // DataInPort declaration
+  // <rtc-template block="inport_declare">
+  TimedDoubleSeq m_qRef;
+  InPort<TimedDoubleSeq> m_qRefIn;
+  TimedPoint3D m_basePos;
+  InPort<TimedPoint3D> m_basePosIn;
+  TimedOrientation3D m_baseRpy;
+  InPort<TimedOrientation3D> m_baseRpyIn;
+  TimedPoint3D m_zmp;
+  InPort<TimedPoint3D> m_zmpIn;
+  TimedDoubleSeq m_optionalData;
+  InPort<TimedDoubleSeq> m_optionalDataIn;
+  std::vector<TimedDoubleSeq> m_ref_force;
+  std::vector<InPort<TimedDoubleSeq> *> m_ref_forceIn;
+  TimedLong m_emergencySignal;
+  InPort<TimedLong> m_emergencySignalIn;
+  // for debug
+  TimedPoint3D m_cog;
+  
+  // </rtc-template>
+
+  // DataOutPort declaration
+  // <rtc-template block="outport_declare">
+  OutPort<TimedDoubleSeq> m_qOut;
+  RTC::OutPort<RTC::TimedPoint3D> m_zmpOut;
+  OutPort<TimedPoint3D> m_basePosOut;
+  OutPort<TimedOrientation3D> m_baseRpyOut;
+  TimedDoubleSeq m_baseTform;
+  OutPort<TimedDoubleSeq> m_baseTformOut;
+  TimedPose3D m_basePose;
+  OutPort<TimedPose3D> m_basePoseOut;
+  TimedAcceleration3D m_accRef;
+  OutPort<TimedAcceleration3D> m_accRefOut;
+  TimedBooleanSeq m_contactStates;
+  OutPort<TimedBooleanSeq> m_contactStatesOut;
+  TimedDoubleSeq m_controlSwingSupportTime;
+  OutPort<TimedDoubleSeq> m_controlSwingSupportTimeOut;
+  TimedBoolean m_walkingStates;
+  OutPort<TimedBoolean> m_walkingStatesOut;
+  TimedPoint3D m_sbpCogOffset;
+  OutPort<TimedPoint3D> m_sbpCogOffsetOut;
+  std::vector<TimedDoubleSeq> m_force;
+  std::vector<OutPort<TimedDoubleSeq> *> m_ref_forceOut;
+  std::vector<TimedPoint3D> m_limbCOPOffset;
+  std::vector<OutPort<TimedPoint3D> *> m_limbCOPOffsetOut;
+  // for debug
+  OutPort<TimedPoint3D> m_cogOut;
+  
+  // </rtc-template>
+
+  // CORBA Port declaration
+  // <rtc-template block="corbaport_declare">
+  RTC::CorbaPort m_AutoBalancerServicePort;
+
+  // </rtc-template>
+
+  // Service declaration
+  // <rtc-template block="service_declare">
+  AutoBalancerService_impl m_service0;
+
+  // </rtc-template>
+
+  // Consumer declaration
+  // <rtc-template block="consumer_declare">
+  
+  // </rtc-template>
+
+ private:
+  struct ABCIKparam {
+    hrp::Vector3 target_p0, localPos, adjust_interpolation_target_p0, adjust_interpolation_org_p0;
+    hrp::Matrix33 target_r0, localR, adjust_interpolation_target_r0, adjust_interpolation_org_r0;
+    rats::coordinates target_end_coords;
+    hrp::Link* target_link;
+    hrp::JointPathExPtr manip;
+    double avoid_gain, reference_gain;
+    size_t pos_ik_error_count, rot_ik_error_count;
+    bool is_active, has_toe_joint;
+  };
+  void getCurrentParameters();
+  void getTargetParameters();
+  bool solveLimbIKforLimb (ABCIKparam& param, const std::string& limb_name);
+  void solveLimbIK();
+  void startABCparam(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
+  void stopABCparam();
+  void waitABCTransition();
+  hrp::Matrix33 OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2);
+  void fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot);
+  void startWalking ();
+  void stopWalking ();
+  void copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs);
+  // static balance point offsetting
+  void static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height);
+  void calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces);
+  hrp::Vector3 calc_vel_from_hand_error (const rats::coordinates& tmp_fix_coords);
+  bool isOptionalDataContact (const std::string& ee_name)
+  {
+      return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false;
+  };
+  bool calc_inital_support_legs(const double& y, std::vector<rats::coordinates>& initial_support_legs_coords, std::vector<rats::leg_type>& initial_support_legs, rats::coordinates& start_ref_coords);
+
+  // for gg
+  typedef boost::shared_ptr<rats::gait_generator> ggPtr;
+  ggPtr gg;
+  bool gg_is_walking, gg_solved;
+  // for abc
+  hrp::Vector3 ref_cog, ref_zmp, prev_imu_sensor_pos, prev_imu_sensor_vel, hand_fix_initial_offset;
+  enum {BIPED, TROT, PACE, CRAWL, GALLOP} gait_type;
+  enum {MODE_IDLE, MODE_ABC, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_ABC} control_mode, return_control_mode;
+  std::map<std::string, ABCIKparam> ikp;
+  std::map<std::string, size_t> contact_states_index_map;
+  std::map<std::string, hrp::VirtualForceSensorParam> m_vfs;
+  std::vector<std::string> sensor_names, leg_names, ee_vec;
+  hrp::dvector qorg, qrefv;
+  hrp::Vector3 current_root_p, target_root_p;
+  hrp::Matrix33 current_root_R, target_root_R;
+  rats::coordinates fix_leg_coords;
+  std::vector<hrp::Vector3> default_zmp_offsets;
+  double m_dt, move_base_gain;
+  hrp::BodyPtr m_robot;
+  coil::Mutex m_mutex;
+
+  double transition_interpolator_ratio, transition_time, zmp_transition_time, adjust_footstep_transition_time, leg_names_interpolator_ratio;
+  interpolator *zmp_offset_interpolator;
+  interpolator *transition_interpolator;
+  interpolator *adjust_footstep_interpolator;
+  interpolator *leg_names_interpolator;
+  hrp::Vector3 input_zmp, input_basePos;
+  hrp::Matrix33 input_baseRot;
+
+  // static balance point offsetting
+  hrp::Vector3 sbp_offset, sbp_cog_offset;
+  enum {MODE_NO_FORCE, MODE_REF_FORCE} use_force;
+  std::vector<hrp::Vector3> ref_forces;
+
+  unsigned int m_debugLevel;
+  bool is_legged_robot, is_stop_mode, has_ik_failed, is_hand_fix_mode, is_hand_fix_initial;
+  int loop, ik_error_debug_print_freq;
+  bool graspless_manip_mode;
+  std::string graspless_manip_arm;
+  hrp::Vector3 graspless_manip_p_gain;
+  rats::coordinates graspless_manip_reference_trans_coords;
+  double pos_ik_thre, rot_ik_thre;
+};
+
+
+extern "C"
+{
+  void AutoBalancerInit(RTC::Manager* manager);
+};
+
+#endif // IMPEDANCE_H
diff --git a/rtc/KFilter.diff b/rtc/KFilter.diff
new file mode 100644
index 00000000000..02bbd6488c4
--- /dev/null
+++ b/rtc/KFilter.diff
@@ -0,0 +1,753 @@
+diff --git a/rtc/KalmanFilter/CMakeLists.txt b/rtc/KalmanFilter/CMakeLists.txt
+index 711cf31..92be2d3 100644
+--- a/rtc/KalmanFilter/CMakeLists.txt
++++ b/rtc/KalmanFilter/CMakeLists.txt
+@@ -21,6 +21,6 @@ target_link_libraries(testKalmanFilterEstimation ${libs})
+ set(target KalmanFilter KalmanFilterComp testKalmanFilterEstimation)
+ 
+ install(TARGETS ${target}
+-  RUNTIME DESTINATION bin
+-  LIBRARY DESTINATION lib
++  RUNTIME DESTINATION bin CONFIGURATIONS Release Debug
++  LIBRARY DESTINATION lib CONFIGURATIONS Release Debug
+ )
+diff --git a/rtc/KalmanFilter/EKFilter.h b/rtc/KalmanFilter/EKFilter.h
+index a6b34c8..8927f62 100644
+--- a/rtc/KalmanFilter/EKFilter.h
++++ b/rtc/KalmanFilter/EKFilter.h
+@@ -1,188 +1,204 @@
+-// -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; -*-
+ #ifndef EKFILTER_H
+ #define EKFILTER_H
+ 
+ #include <hrpUtil/EigenTypes.h>
+ #include <iostream>
+-#include "hrpsys/util/Hrpsys.h"
+-
+-namespace hrp{
+-  typedef Eigen::Matrix<double, 7, 7> Matrix77;
+-  typedef Eigen::Matrix<double, 7, 1> Vector7;
+-};
++#include "util/Hrpsys.h"
+ 
+ class EKFilter {
+ public:
+-  EKFilter()
+-    : P(hrp::Matrix77::Identity() * 0.1),
+-      Q(Eigen::Matrix3d::Identity() * 0.001),
+-      R(Eigen::Matrix3d::Identity() * 0.03),
+-      g_vec(Eigen::Vector3d(0.0, 0.0, 9.80665)),
+-      z_k(Eigen::Vector3d(0.0, 0.0, 9.80665)),
+-      min_mag_thre_acc(0.005), max_mag_thre_acc(0.05),
+-      min_mag_thre_gyro(0.0075), max_mag_thre_gyro(0.035)
+-  {
++  EKFilter() {
+     x << 1, 0, 0, 0, 0, 0, 0;
++    P = Eigen::Matrix<double, 7, 7>::Identity() * 5;
++    Q = Eigen::Matrix<double, 7, 7>::Identity();
++    Q.block<4, 4>(0, 0) *= 0.1;
++    Q.block<3, 3>(4, 4) *= 0.001;
++    R = Eigen::Matrix<double, 3, 3>::Zero();
++    R(0, 0) = 4;
++    R(1, 1) = 4;
++    R(2, 2) = 2;
++    g_vec = Eigen::Vector3d(0.0, 0.0, 9.80665);
+   }
+ 
+-  hrp::Vector7 getx() const { return x; }
++  Eigen::Matrix<double, 7, 1> getx() { return x; }
++
++  Eigen::Matrix<double, 3, 1> accelerationToRpy(const double& acc_x, const double& acc_y, const double& acc_z) {
++    /*
++     * acc = \dot{v} + w \times v + R^T g;
++     * -> R^T g = acc - \dot{v} + w \times v;
++     * -> R^T g = acc;
++     * -> acc_x = -sinb g, acc_y = sina cosb g, acc_z = cosa cosb g;
++     */
++    double roll = atan2(acc_y, acc_z);
++    double pitch = atan2(-acc_x, sqrt(acc_y * acc_y + acc_z * acc_z));
++    double yaw = 0;             /* cannot be defined only by acceleration */
++    Eigen::Vector3d rpy = Eigen::Vector3d(roll, pitch, yaw);
++    return rpy;
++  }
+ 
+-  void calcOmega(Eigen::Matrix4d& omega, const Eigen::Vector3d& w) const {
++  Eigen::Matrix<double, 4, 4> calcOmega(const Eigen::Vector3d& w) {
+     /* \dot{q} = \frac{1}{2} omega q */
++    Eigen::Matrix<double, 4, 4> omega;
+     omega <<
+       0, -w[0], -w[1], -w[2],
+       w[0],     0,  w[2], -w[1],
+       w[1], -w[2],     0,  w[0],
+       w[2],  w[1], -w[0],     0;
++    return omega;
+   }
+ 
+-  void calcPredictedState(hrp::Vector7& _x_a_priori,
+-                          const Eigen::Vector4d& q,
+-                          const Eigen::Vector3d& gyro,
+-                          const Eigen::Vector3d& drift) const {
++  Eigen::Matrix<double, 7, 1> calcPredictedState(Eigen::Matrix<double, 4, 1> q,
++                                                 Eigen::Vector3d gyro,
++                                                 Eigen::Vector3d drift,
++                                                 const double& dt) {
+     /* x_a_priori = f(x, u) */
+-    Eigen::Vector4d q_a_priori;
++    Eigen::Matrix<double, 7, 1> ret;
++    Eigen::Matrix<double, 4, 1> q_a_priori;
+     Eigen::Vector3d gyro_compensated = gyro - drift;
+-    Eigen::Matrix4d omega;
+-    calcOmega(omega, gyro_compensated);
+-    q_a_priori = q + dt / 2 * omega * q;
+-    _x_a_priori.head<4>() = q_a_priori.normalized();
+-    _x_a_priori.tail<3>() = drift;
++    q_a_priori = q + dt / 2 * calcOmega(gyro_compensated) * q;
++    ret.block<4, 1>(0, 0) = q_a_priori;
++    ret.block<3, 1>(4, 0) = drift;
++    return ret;
+   }
+ 
+-  void calcF(hrp::Matrix77& F,
+-             const Eigen::Vector4d& q,
+-             const Eigen::Vector3d& gyro,
+-             const Eigen::Vector3d& drift) const {
+-    F = hrp::Matrix77::Identity();
++  Eigen::Matrix<double, 7, 7> calcF(Eigen::Matrix<double, 4, 1> q,
++                                    const Eigen::Vector3d& gyro,
++                                    Eigen::Vector3d drift,
++                                    const double& dt) {
++    Eigen::Matrix<double, 7, 7> F;
+     Eigen::Vector3d gyro_compensated = gyro - drift;
+-    Eigen::Matrix4d omega;
+-    calcOmega(omega, gyro_compensated);
+-    F.block<4, 4>(0, 0) += dt / 2 * omega;
++    F.block<4, 4>(0, 0) =
++      Eigen::Matrix<double, 4, 4>::Identity() + dt / 2 * calcOmega(gyro_compensated);
+     F.block<4, 3>(0, 4) <<
+-      + q[1], + q[2], + q[3],
+-      - q[0], + q[3], - q[2],
+-      - q[3], - q[0], + q[1],
+-      + q[2], - q[1], - q[0];
+-    F.block<4, 3>(0, 4) *= dt / 2;
++      dt / 2 * q[1],   dt / 2 * q[2],   dt / 2 * q[3],
++      - dt / 2 * q[0],   dt / 2 * q[3], - dt / 2 * q[2],
++      - dt / 2 * q[3], - dt / 2 * q[0],   dt / 2 * q[1],
++      dt / 2 * q[2], - dt / 2 * q[1], - dt / 2 * q[0];
++    F.block<3, 4>(0, 4) = Eigen::Matrix<double, 3, 4>::Zero();
++    F.block<3, 3>(4, 4) = Eigen::Matrix<double, 3, 3>::Identity();
++    return F;
+   }
+ 
+-  void calcPredictedCovariance(hrp::Matrix77& _P_a_priori,
+-                               const hrp::Matrix77& F,
+-                               const Eigen::Vector4d& q) const {
+-    /* P_a_priori = F P F^T + V Q V^T */
+-    Eigen::Matrix<double, 4, 3> V_upper;
+-    V_upper <<
+-      - q[1], - q[2], - q[3],
+-      + q[0], - q[3], + q[2],
+-      + q[3], + q[0], - q[1],
+-      - q[2], + q[1], + q[0];
+-    V_upper *= dt / 2;
+-    hrp::Matrix77 VQVt = hrp::Matrix77::Zero();
+-    VQVt.block<4, 4>(0, 0) = V_upper * Q * V_upper.transpose();
+-    _P_a_priori = F * P * F.transpose() + VQVt;
++  Eigen::Matrix<double, 7, 7> calcPredictedCovariance(Eigen::Matrix<double, 7, 7> F) {
++    /* P_a_priori = F P F^T + Q */
++    return F * P * F.transpose() + Q;
+   }
+ 
+-  Eigen::Vector3d calcAcc(const Eigen::Vector4d& q) const {
+-    Eigen::Quaternion<double> q_tmp(q[0], q[1], q[2], q[3]);
+-    Eigen::Vector3d acc = q_tmp.conjugate()._transformVector(g_vec);
++  Eigen::Vector3d calcAcc(Eigen::Matrix<double, 4, 1> q,
++                          const Eigen::Vector3d& vel_ref,
++                          const Eigen::Vector3d& acc_ref,
++                          const Eigen::Vector3d& angular_rate_ref) {
++    /* acc = \dot{v} + w \times v + R^T g; */
++    Eigen::Quaternion<double> q_tmp = Eigen::Quaternion<double>(q[0], q[1], q[2], q[3]);
++    Eigen::Vector3d acc =
++      acc_ref + angular_rate_ref.cross(vel_ref) +
++      q_tmp.toRotationMatrix().transpose() * g_vec;
++    /* 
++     * Eigen::Vector3d hoge;
++     * hoge <<
++     *   -2 * q_tmp2.w() * q_tmp2.y() + 2 * q_tmp2.x() * q_tmp2.z(),
++     *   2 * q_tmp2.w() * q_tmp2.x() + 2 * q_tmp2.y() * q_tmp2.z(),
++     *   q_tmp2.w() * q_tmp2.w()  - q_tmp2.x() * q_tmp2.x() - q_tmp2.y() * q_tmp2.y() + q_tmp2.z() * q_tmp2.z();
++     * hoge *= g_vec[2];
++     * std::cerr << "diff 2" << std::endl << acc - hoge << std::endl;
++     */
+     return acc;
+   }
+ 
+-  void calcH(Eigen::Matrix<double, 3, 7>& H, const Eigen::Vector4d& q) const {
++  Eigen::Matrix<double, 3, 7> calcH(Eigen::Matrix<double, 4, 1> q) {
++    Eigen::Matrix<double, 3, 7> H;
+     double w = q[0], x = q[1], y = q[2], z = q[3];
++    /* 
++     * H <<
++     *   2 * y,  2 * z,  2 * w,  2 * x, 0, 0, 0,
++     *   -2 * x, -2 * w,  2 * z,  2 * y, 0, 0, 0,
++     *   2 * w, -2 * x, -2 * y,  2 * z, 0, 0, 0;
++     * H *= g_vec[2];
++     */
+     H <<
+-      -y, +z, -w, +x, 0, 0, 0,
+-      +x, +w, +z, +y, 0, 0, 0,
+-      +w, -x, -y, +z, 0, 0, 0;
++      -y,  z, -w, x, 0, 0, 0,
++       x,  w,  z, y, 0, 0, 0,
++       w, -x, -y, z, 0, 0, 0;
+     H *= 2 * g_vec[2];
++    return H;
+   }
+ 
+   Eigen::Vector3d calcMeasurementResidual(const Eigen::Vector3d& acc_measured,
+-                                          const Eigen::Vector4d& q) const {
++                                          const Eigen::Vector3d& vel_ref,
++                                          const Eigen::Vector3d& acc_ref,
++                                          const Eigen::Vector3d& angular_rate_ref,
++                                          Eigen::Matrix<double, 4, 1> q) {
+     /* y = z - h(x) */
+-    Eigen::Vector3d y = acc_measured - calcAcc(q);
++    Eigen::Vector3d y = acc_measured - calcAcc(q, vel_ref, acc_ref, angular_rate_ref);
++    /* 
++     * std::cerr << "acc_measured" << std::endl << acc_measured << std::endl;
++     * std::cerr << "calc acc" << std::endl << calcAcc(q, vel_ref, acc_ref, angular_rate_ref) << std::endl;
++     * std::cerr << "diff" << std::endl << y << std::endl;
++     */
+     return y;
+   }
+ 
+ 
+   void prediction(const Eigen::Vector3d& u) {
+-    Eigen::Vector4d q = x.head<4>();
+-    Eigen::Vector3d drift = x.tail<3>();
+-    hrp::Matrix77 F;
+-    calcF(F, q, u, drift);
+-    hrp::Vector7 x_tmp;
+-    calcPredictedState(x_tmp, q, u, drift);
+-    x_a_priori = x_tmp;
+-    hrp::Matrix77 P_tmp;
+-    calcPredictedCovariance(P_tmp, F, q);
+-    P_a_priori = P_tmp;
++    Eigen::Matrix<double, 4, 1> q = x.block<4, 1>(0, 0);
++    Eigen::Vector3d drift = x.block<3, 1>(4, 0);
++    Eigen::Matrix<double, 7, 7> F = calcF(q, u, drift, dt);
++    x_a_priori = calcPredictedState(q, u, drift, dt);
++    P_a_priori = calcPredictedCovariance(F);
+   }
+ 
+-  void correction(const Eigen::Vector3d& z, const Eigen::Matrix3d& fuzzyR) {
+-    Eigen::Vector4d q_a_priori = x_a_priori.head<4>();
++  void correction(const Eigen::Vector3d& z,
++                  const Eigen::Vector3d& vel_ref,
++                  const Eigen::Vector3d& acc_ref,
++                  const Eigen::Vector3d& angular_rate_ref) {
++    Eigen::Matrix<double, 4, 1> q_a_priori = x_a_priori.block<4, 1>(0, 0).normalized();
+     Eigen::Matrix<double, 3, 7> H;
+-    z_k = z;
+-    Eigen::Vector3d y = calcMeasurementResidual(z, q_a_priori);
+-    calcH(H, q_a_priori);
+-    Eigen::Matrix3d S = H * P_a_priori * H.transpose() + fuzzyR;
+-    Eigen::Matrix<double, 7, 3> K = P_a_priori * H.transpose() * S.inverse();
+-    hrp::Vector7 x_tmp = x_a_priori + K * y;
+-    x.head<4>() = x_tmp.head<4>().normalized(); /* quaternion */
+-    x.tail<3>() = x_tmp.tail<3>(); /* bias */
+-    P = (hrp::Matrix77::Identity() - K * H) * P_a_priori;
++    Eigen::Matrix<double, 3, 3> S;
++    Eigen::Matrix<double, 7, 3> K;
++    Eigen::Vector3d y;
++    /* need to normalize q_a_priori ? */
++    y = calcMeasurementResidual(z, vel_ref, acc_ref, angular_rate_ref, q_a_priori);
++    H = calcH(q_a_priori);
++    S = H * P_a_priori * H.transpose() + R;
++    K = P_a_priori * H.transpose() * S.inverse();
++    Eigen::Matrix<double, 7, 1> x_tmp = x_a_priori + K * y;
++    x = x_tmp.normalized();
++    P = (Eigen::Matrix<double, 7, 7>::Identity() - K * H) * P_a_priori;
+   }
+ 
+-  void printAll() const {
++  void printAll() {
+     std::cerr << "x" << std::endl << x << std::endl;
+     std::cerr << "x_a_priori" << std::endl << x_a_priori << std::endl;
+     std::cerr << "P" << std::endl << P << std::endl << std::endl;
+     std::cerr << "P_a_priori" << std::endl << P_a_priori << std::endl << std::endl;
++    /*
++     * std::cerr << "Q" << std::endl << Q << std::endl << std::endl;
++     * std::cerr << "R" << std::endl << R << std::endl << std::endl;
++     */
+   }
+ 
+-
+-  // Basically Equation (23), (24) and (25) in the paper [1]
+-  // [1] Chul Woo Kang and Chan Gook Park. Attitude estimation with accelerometers and gyros using fuzzy tuned Kalman filter.
+-  //     In European Control Conference, 2009.
+-  void calcRWithFuzzyRule(Eigen::Matrix3d& fuzzyR, const hrp::Vector3& acc, const hrp::Vector3& gyro) const {
+-    double alpha = std::min(std::fabs(acc.norm() - g_vec.norm()) / g_vec.norm(), 0.1);
+-    double beta = std::min(gyro.norm(), 0.05);
+-    double large_mu_acc = std::max(std::min((alpha - min_mag_thre_acc) / (max_mag_thre_acc - min_mag_thre_acc), 1.0), 0.0);
+-    double large_mu_gyro = std::max(std::min((beta - min_mag_thre_gyro) / (max_mag_thre_gyro - min_mag_thre_gyro), 1.0), 0.0);
+-    double w1, w2, w3, w4;
+-    w1 = (1.0 - large_mu_acc) * (1.0 - large_mu_gyro);
+-    w2 = (1.0 - large_mu_acc) * large_mu_gyro;
+-    w3 = large_mu_acc * (1.0 - large_mu_gyro);
+-    w4 = large_mu_acc * large_mu_gyro;
+-    double z = (w1 * 0.0 + w2 * (3.5 * alpha + 8.0 * beta + 0.5) + w3 * (3.5 * alpha + 8.0 * beta + 0.5) + w4 * 1.0) / (w1 + w2 + w3 + w4);
+-    double k1 = 400;
+-    fuzzyR = R + k1 * z * z * Eigen::Matrix3d::Identity();
+-  };
+-
+   void main_one (hrp::Vector3& rpy, hrp::Vector3& rpyRaw, const hrp::Vector3& acc, const hrp::Vector3& gyro)
+   {
+-    Eigen::Matrix3d fuzzyR;
+-    calcRWithFuzzyRule(fuzzyR, acc, gyro);
+-    prediction(gyro);
+-    correction(acc, fuzzyR);
+-    /* ekf_filter.printAll(); */
+-    Eigen::Quaternion<double> q(x[0], x[1], x[2], x[3]);
+-    rpy = hrp::rpyFromRot(q.toRotationMatrix());
++      prediction(gyro);
++      correction(acc, Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0));
++      /* ekf_filter.printAll(); */
++      Eigen::Matrix<double, 7, 1> x = getx();
++      Eigen::Quaternion<double> q = Eigen::Quaternion<double>(x[0], x[1], x[2], x[3]);
++      hrp::Vector3 eulerZYX = q.toRotationMatrix().eulerAngles(2,1,0);
++      rpy(2) = eulerZYX(0);
++      rpy(1) = eulerZYX(1);
++      rpy(0) = eulerZYX(2);
+   };
+ 
+   void setdt (const double _dt) { dt = _dt;};
+-  void resetKalmanFilterState() {
+-    Eigen::Quaternion<double> tmp_q;
+-    tmp_q.setFromTwoVectors(z_k, g_vec);
+-    x << tmp_q.w(), tmp_q.x(), tmp_q.y(), tmp_q.z(), 0, 0, 0;
+-  };
+ private:
+-  hrp::Vector7 x, x_a_priori;
+-  hrp::Matrix77 P, P_a_priori;
+-  Eigen::Matrix3d Q, R;
+-  Eigen::Vector3d g_vec, z_k;
++  Eigen::Matrix<double, 7, 1> x, x_a_priori;
++  Eigen::Matrix<double, 7, 7> P, P_a_priori;
++  Eigen::Matrix<double, 7, 7> Q;
++  Eigen::Matrix<double, 3, 3> R;
++  /* static const Eigen::Vector3d g_vec = Eigen::Vector3d(0.0, 0.0, 9.80665); */
++  Eigen::Vector3d g_vec;
+   double dt;
+-  double min_mag_thre_acc, max_mag_thre_acc, min_mag_thre_gyro, max_mag_thre_gyro;
+ };
+ 
+ #endif /* EKFILTER_H */
+diff --git a/rtc/KalmanFilter/KalmanFilter.cpp b/rtc/KalmanFilter/KalmanFilter.cpp
+index 8084418..4280b51 100644
+--- a/rtc/KalmanFilter/KalmanFilter.cpp
++++ b/rtc/KalmanFilter/KalmanFilter.cpp
+@@ -8,7 +8,7 @@
+  */
+ 
+ #include "KalmanFilter.h"
+-#include "hrpsys/util/VectorConvert.h"
++#include "util/VectorConvert.h"
+ #include <rtm/CorbaNaming.h>
+ #include <hrpModel/ModelLoaderUtil.h>
+ #include <math.h>
+@@ -49,6 +49,13 @@ KalmanFilter::KalmanFilter(RTC::Manager* manager)
+     m_rpyRawOut("rpy_raw", m_rpyRaw),
+     m_baseRpyCurrentOut("baseRpyCurrent", m_baseRpyCurrent),
+     m_KalmanFilterServicePort("KalmanFilterService"),
++#ifdef CALC_VEL_N_ANGVEL
++    //added by karasawa for chidori
++    m_velOut("vel", m_vel),
++    m_angvelOut("angvel", m_angvel),
++    m_accOut("acc_output", m_acc_output),
++    m_emergency_step_flagIn("emergency_step_flag", m_emergency_step_flag),
++#endif
+     // </rtc-template>
+     m_robot(hrp::BodyPtr()),
+     m_debugLevel(0),
+@@ -71,6 +78,7 @@ RTC::ReturnCode_t KalmanFilter::onInitialize()
+   // Bind variables and configuration variable
+   bindParameter("debugLevel", m_debugLevel, "0");
+   
++  
+   // </rtc-template>
+ 
+   // Registration: InPort/OutPort/Service
+@@ -86,6 +94,14 @@ RTC::ReturnCode_t KalmanFilter::onInitialize()
+   addOutPort("rpy", m_rpyOut);
+   addOutPort("rpy_raw", m_rpyRawOut);
+   addOutPort("baseRpyCurrent", m_baseRpyCurrentOut);
++#ifdef CALC_VEL_N_ANGVEL
++  //added by karasawa for chidori
++  addOutPort("vel", m_velOut);
++  addOutPort("angvel", m_angvelOut);
++  addOutPort("acc_output", m_accOut);
++  addInPort("emergency_step_flag", m_emergency_step_flagIn);
++  m_emergency_step_flag.data = false;
++#endif
+ 
+   // Set service provider to Ports
+   m_KalmanFilterServicePort.registerProvider("service0", "KalmanFilterService", m_service0);
+@@ -123,6 +139,18 @@ RTC::ReturnCode_t KalmanFilter::onInitialize()
+   m_rpy.data.r = 0;
+   m_rpy.data.p = 0;
+   m_rpy.data.y = 0;
++#ifdef CALC_VEL_N_ANGVEL
++  //added by karasawa for chidori
++  m_vel.data.vx = 0.0;
++  m_vel.data.vy = 0.0;
++  m_vel.data.vz = 0.0;
++  m_angvel.data.avx = 0.0;
++  m_angvel.data.avy = 0.0;
++  m_angvel.data.avz = 0.0;
++  m_acc_output.data.ax = 0.0;
++  m_acc_output.data.ay = 0.0;
++  m_acc_output.data.az = 0.0;
++#endif
+ 
+   if (m_robot->numSensors(hrp::Sensor::ACCELERATION) > 0) {
+     hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::ACCELERATION, 0);
+@@ -201,9 +229,14 @@ RTC::ReturnCode_t KalmanFilter::onExecute(RTC::UniqueId ec_id)
+   }
+   double sx_ref = 0.0, sy_ref = 0.0, sz_ref = 0.0;
+   if (m_accRefIn.isNew()){
+-    m_accRefIn.read();
+-    sx_ref = m_accRef.data.ax, sy_ref = m_accRef.data.ay, sz_ref = m_accRef.data.az;
++      m_accRefIn.read();
++      sx_ref = m_accRef.data.ax, sy_ref = m_accRef.data.ay, sz_ref = m_accRef.data.az;
++  }
++#ifdef CALC_VEL_N_ANGVEL
++  if (m_emergency_step_flagIn.isNew()) {
++      m_emergency_step_flagIn.read();
+   }
++#endif
+   if (m_accIn.isNew()){
+     m_accIn.read();
+ 
+@@ -231,6 +264,59 @@ RTC::ReturnCode_t KalmanFilter::onExecute(RTC::UniqueId ec_id)
+         }
+         rpy_kf.main_one(rpy, rpyRaw, baseRpyCurrent, acc, gyro, sl_y, BtoS);
+     }
++#ifdef CALC_VEL_N_ANGVEL
++    //calc_vel_n_angvel(vel, angvel, acc, gyro, rpy, BtoS);
++    if(m_robot->numSensors(hrp::Sensor::ACCELERATION) >0) {
++        if(m_emergency_step_flag.data == SequencePlayer::EMERGENCY) {
++           // std::cerr << "\x1b[33m" << "emergency step flag!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << "\x1b[0m" << std::endl;
++        }
++        hrp::Vector3 vel;
++        vel(0)= m_vel.data.vx;
++        vel(1)= m_vel.data.vy;
++        vel(2)= m_vel.data.vz;
++        hrp::Vector3 angvel;
++        hrp::Vector3 acc_without_g;
++        //angvel(0)= m_angvel.data.avx;
++        //angvel(1)= m_angvel.data.avy;
++        //angvel(2)= m_angvel.data.avz;
++        hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::ACCELERATION, 0);
++        rpy_kf.calc_vel_n_angvel(vel, angvel, acc_without_g,acc, gyro, rpy, sensor->localPos);
++        m_vel.data.vx = vel(0);
++        m_vel.data.vy = vel(1);
++        m_vel.data.vz = vel(2);
++        m_angvel.data.avx = angvel(0);
++        m_angvel.data.avy = angvel(1);
++        m_angvel.data.avz = angvel(2);
++        m_acc_output.data.ax = acc_without_g(0);
++        m_acc_output.data.ay = acc_without_g(1);
++        m_acc_output.data.az = acc_without_g(2);
++        /* 
++         * hrp::Vector3 testvec;
++         * testvec << 1, 0, 0;
++         * testvec = sensor->localR*testvec;
++         * std::cerr << "1 0 0  | x:" << testvec(0) << "  y:" << testvec(1) << "  z:" << testvec(2) << std::endl;
++         * testvec << 0, 1, 0;
++         * testvec = sensor->localR*testvec;
++         * std::cerr << "0 1 0  | x:" << testvec(0) << "  y:" << testvec(1) << "  z:" << testvec(2) << std::endl;
++         * testvec << 0, 0, 1;
++         * testvec = sensor->localR*testvec;
++         * std::cerr << "0 0 1  | x:" << testvec(0) << "  y:" << testvec(1) << "  z:" << testvec(2) << std::endl;
++         */
++
++    }
++    //std::cerr << "acc x:" << acc(0) << "  y:" << acc(1) << "  z:" << acc(2) << std::endl;
++
++    /* 
++     * hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::ACCELERATION, 0);
++     * hrp::Vector3 Body2Sensor_pos_relative_to_world = sensor->link->R * sensor->localPos;
++     * //std::cerr << "sensor pos  x:" << sensor->localPos(0) << "  y:" << sensor->localPos(1) << "  z:" << sensor->localPos(2) << std::endl;
++     * std::cerr << "posture::" << std::endl;
++     * std::cerr << sensor->link->R(0,0) << "  " << sensor->link->R(0,1) << "  "<< sensor->link->R(0,2) << std::endl;
++     * std::cerr << sensor->link->R(1,0) << "  " << sensor->link->R(1,1) << "  "<< sensor->link->R(1,2) << std::endl;
++     * std::cerr << sensor->link->R(2,0) << "  " << sensor->link->R(2,1) << "  "<< sensor->link->R(2,2) << std::endl;
++     * std::cerr << std::endl;
++     */
++#endif
+     m_rpyRaw.data.r = rpyRaw(0);
+     m_rpyRaw.data.p = rpyRaw(1);
+     m_rpyRaw.data.y = rpyRaw(2);
+@@ -244,10 +330,22 @@ RTC::ReturnCode_t KalmanFilter::onExecute(RTC::UniqueId ec_id)
+     m_rpyRaw.tm = m_acc.tm;
+     m_rpy.tm = m_acc.tm;
+     m_baseRpyCurrent.tm = m_acc.tm;
++#ifdef CALC_VEL_N_ANGVEL
++    //added by karasawa for chidori
++    m_vel.tm = m_acc.tm;
++    m_angvel.tm = m_acc.tm;
++    m_acc_output.tm = m_acc.tm;
++#endif
+ 
+     m_rpyOut.write();
+     m_rpyRawOut.write();
+     m_baseRpyCurrentOut.write();
++#ifdef CALC_VEL_N_ANGVEL
++    //added by karasawa for chidori
++    m_velOut.write();
++    m_angvelOut.write();
++    m_accOut.write();
++#endif
+   }
+   return RTC::RTC_OK;
+ }
+@@ -309,7 +407,6 @@ bool KalmanFilter::setKalmanFilterParam(const OpenHRP::KalmanFilterService::Kalm
+ bool KalmanFilter::resetKalmanFilterState()
+ {
+     rpy_kf.resetKalmanFilterState();
+-    ekf_filter.resetKalmanFilterState();
+ };
+ 
+ bool KalmanFilter::getKalmanFilterParam(OpenHRP::KalmanFilterService::KalmanFilterParam& i_param)
+diff --git a/rtc/KalmanFilter/KalmanFilter.h b/rtc/KalmanFilter/KalmanFilter.h
+index 7b48040..70a2c57 100644
+--- a/rtc/KalmanFilter/KalmanFilter.h
++++ b/rtc/KalmanFilter/KalmanFilter.h
+@@ -10,6 +10,9 @@
+ #ifndef NULL_COMPONENT_H
+ #define NULL_COMPONENT_H
+ 
++//define if you use velocity and angular velocity of root link
++#define CALC_VEL_N_ANGVEL
++
+ #include <rtm/Manager.h>
+ #include <rtm/DataFlowComponentBase.h>
+ #include <rtm/CorbaPort.h>
+@@ -22,6 +25,10 @@
+ #include "RPYKalmanFilter.h"
+ #include "EKFilter.h"
+ 
++#ifdef CALC_VEL_N_ANGVEL
++#include "../SequencePlayer/SequencePlayer.h"
++#endif
++
+ // Service implementation headers
+ // <rtc-template block="service_impl_h">
+ #include "KalmanFilterService_impl.h"
+@@ -134,6 +141,18 @@ protected:
+   RTC::InPort<RTC::TimedDoubleSeq> m_qCurrentIn;
+   RTC::TimedOrientation3D m_baseRpyCurrent;
+   RTC::OutPort<RTC::TimedOrientation3D> m_baseRpyCurrentOut;
++
++#ifdef CALC_VEL_N_ANGVEL
++  //added by karasawa for chidori
++  TimedVelocity3D m_vel;
++  TimedAngularVelocity3D m_angvel;
++  TimedAcceleration3D m_acc_output;
++  RTC::TimedLong m_emergency_step_flag;
++  RTC::OutPort<RTC::TimedVelocity3D> m_velOut;
++  RTC::OutPort<RTC::TimedAngularVelocity3D> m_angvelOut;
++  RTC::OutPort<RTC::TimedAcceleration3D> m_accOut;
++  RTC::InPort<RTC::TimedLong> m_emergency_step_flagIn;
++#endif
+   
+   // </rtc-template>
+ 
+diff --git a/rtc/KalmanFilter/KalmanFilter.xml b/rtc/KalmanFilter/KalmanFilter.xml
+deleted file mode 100644
+index b0c9dc3..0000000
+--- a/rtc/KalmanFilter/KalmanFilter.xml
++++ /dev/null
+@@ -1,35 +0,0 @@
+-<?xml version="1.0" encoding="UTF-8" standalone="yes"?>
+-<rtc:RtcProfile rtc:version="0.2" rtc:id="RTC:AIST:example:KalmanFilter:1.0" xmlns:rtcExt="http://www.openrtp.org/namespaces/rtc_ext" xmlns:rtcDoc="http://www.openrtp.org/namespaces/rtc_doc" xmlns:rtc="http://www.openrtp.org/namespaces/rtc" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
+-    <rtc:BasicInfo xsi:type="rtcExt:basic_info_ext" rtcExt:saveProject="KalmanFilter" rtc:updateDate="2015-11-18T14:10:18+09:00" rtc:creationDate="2015-11-18T05:07:28Z" rtc:version="1.0" rtc:vendor="AIST" rtc:maxInstances="10" rtc:executionType="PeriodicExecutionContext" rtc:executionRate="0.0" rtc:description="Kalman filter component" rtc:category="example" rtc:componentKind="DataFlowComponent" rtc:activityType="PERIODIC" rtc:componentType="STATIC" rtc:name="KalmanFilter"/>
+-    <rtc:Actions>
+-        <rtc:OnInitialize xsi:type="rtcDoc:action_status_doc" rtc:implemented="true"/>
+-        <rtc:OnFinalize xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
+-        <rtc:OnStartup xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
+-        <rtc:OnShutdown xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
+-        <rtc:OnActivated xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
+-        <rtc:OnDeactivated xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
+-        <rtc:OnAborting xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
+-        <rtc:OnError xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
+-        <rtc:OnReset xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
+-        <rtc:OnExecute xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
+-        <rtc:OnStateUpdate xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
+-        <rtc:OnRateChanged xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
+-        <rtc:OnAction xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
+-        <rtc:OnModeChanged xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
+-    </rtc:Actions>
+-    <rtc:ConfigurationSet>
+-        <rtc:Configuration xsi:type="rtcExt:configuration_ext" rtc:defaultValue="1" rtc:type="string" rtc:name="compensateMode"/>
+-        <rtc:Configuration xsi:type="rtcExt:configuration_ext" rtc:defaultValue="0,0,0" rtc:type="string" rtc:name="offset"/>
+-        <rtc:Configuration xsi:type="rtcExt:configuration_ext" rtc:defaultValue="0.01" rtc:type="string" rtc:name="Q"/>
+-        <rtc:Configuration xsi:type="rtcExt:configuration_ext" rtc:defaultValue="0.0001" rtc:type="string" rtc:name="Qbw"/>
+-        <rtc:Configuration xsi:type="rtcExt:configuration_ext" rtc:defaultValue="0.1" rtc:type="string" rtc:name="R"/>
+-        <rtc:Configuration xsi:type="rtcExt:configuration_ext" rtc:defaultValue="0.15" rtc:type="string" rtc:name="Tgsens"/>
+-        <rtc:Configuration xsi:type="rtcExt:configuration_ext" rtc:defaultValue="3" rtc:type="string" rtc:name="filter_order"/>
+-        <rtc:Configuration xsi:type="rtcExt:configuration_ext" rtc:defaultValue="0" rtc:type="string" rtc:name="debugLevel"/>
+-    </rtc:ConfigurationSet>
+-    <rtc:DataPorts xsi:type="rtcExt:dataport_ext" rtcExt:position="LEFT" rtc:subscriptionType="Any" rtc:dataflowType="push,pull" rtc:interfaceType="corba_cdr" rtc:type="RTC::TimedAcceleration3D" rtc:name="acc" rtc:portType="DataInPort"/>
+-    <rtc:DataPorts xsi:type="rtcExt:dataport_ext" rtcExt:position="LEFT" rtc:subscriptionType="Any" rtc:dataflowType="push,pull" rtc:interfaceType="corba_cdr" rtc:type="RTC::TimedAcceleration3D" rtc:name="accRef" rtc:portType="DataInPort"/>
+-    <rtc:DataPorts xsi:type="rtcExt:dataport_ext" rtcExt:position="LEFT" rtc:subscriptionType="Any" rtc:dataflowType="push,pull" rtc:interfaceType="corba_cdr" rtc:type="RTC::TimedAngularVelocity3D" rtc:name="rate" rtc:portType="DataInPort"/>
+-    <rtc:DataPorts xsi:type="rtcExt:dataport_ext" rtcExt:position="RIGHT" rtc:subscriptionType="flush,new,periodic" rtc:dataflowType="push,pull" rtc:interfaceType="corba_cdr" rtc:type="RTC::TimedOrientation3D" rtc:name="rpy" rtc:portType="DataOutPort"/>
+-    <rtc:Language xsi:type="rtcExt:language_ext" rtc:kind="C++"/>
+-</rtc:RtcProfile>
+diff --git a/rtc/KalmanFilter/KalmanFilterService_impl.h b/rtc/KalmanFilter/KalmanFilterService_impl.h
+index 48aef9a..645205d 100644
+--- a/rtc/KalmanFilter/KalmanFilterService_impl.h
++++ b/rtc/KalmanFilter/KalmanFilterService_impl.h
+@@ -2,7 +2,7 @@
+ #ifndef __KALMANFILTER_SERVICE_H__
+ #define __KALMANFILTER_SERVICE_H__
+ 
+-#include "hrpsys/idl/KalmanFilterService.hh"
++#include "KalmanFilterService.hh"
+ 
+ class KalmanFilter;
+ 
+diff --git a/rtc/KalmanFilter/RPYKalmanFilter.h b/rtc/KalmanFilter/RPYKalmanFilter.h
+index 4c3967a..197af98 100644
+--- a/rtc/KalmanFilter/RPYKalmanFilter.h
++++ b/rtc/KalmanFilter/RPYKalmanFilter.h
+@@ -3,7 +3,7 @@
+ 
+ #include <hrpUtil/EigenTypes.h>
+ #include <hrpUtil/Eigen3d.h>
+-#include "hrpsys/util/Hrpsys.h"
++#include "util/Hrpsys.h"
+ namespace hrp{
+   typedef Eigen::Vector2d Vector2;
+   typedef Eigen::Matrix2d Matrix22;
+@@ -70,7 +70,12 @@ private:
+ 
+ class RPYKalmanFilter {
+ public:
+-    RPYKalmanFilter() : m_sensorR(hrp::Matrix33::Identity()) {};
++#ifndef CALC_VEL_N_ANGVEL
++    RPYKalmanFilter() : m_sensorR(hrp::Matrix33::Identity()){
++#else
++    RPYKalmanFilter() : m_sensorR(hrp::Matrix33::Identity()), dt(0.002), G(9.80665), loop_count(0), count(0), rpy_for_calc_vel_n_angvel(0){
++#endif
++    };
+     void main_one (hrp::Vector3& rpy, hrp::Vector3& rpyRaw, hrp::Vector3& baseRpyCurrent, const hrp::Vector3& acc, const hrp::Vector3& gyro, const double& sl_y, const hrp::Matrix33& BtoS)
+     {
+       //
+@@ -158,10 +163,106 @@ public:
+     double getQangle () const { return Q_angle;};
+     double getQrate () const { return Q_rate;};
+     double getRangle () const { return R_angle;};
++#ifdef CALC_VEL_N_ANGVEL
++    void calc_vel_n_angvel (hrp::Vector3& vel, hrp::Vector3& angvel,hrp::Vector3& acc_without_g, const hrp::Vector3& acc, const hrp::Vector3& gyro, const hrp::Vector3& rpy, const hrp::Vector3& sensorPosition) {
++        loop_count++;
++        if(loop_count > 200) {
++            loop_count = 0;
++            count = 0;
++        }
++
++        double g_norm = sqrt(acc(0)*acc(0) + acc(1)*acc(1) + acc(2)*acc(2));
++        if( ((g_norm-G) < 0.005) && ((g_norm-G) > -0.005) ) {
++            count++;
++        }
++        if (loop_count == 0) {
++            //std::cerr << "\x1b[33m" << "g_norm |" << g_norm << "\x1b[0m" << std::endl;
++        }
++        if(count > 5) {
++            //std::cerr << "\x1b[31m" << "rpy angles is updated by acc!!" << "\x1b[0m" <<std::endl;
++            double r, p;
++            p = atan2( acc(0), sqrt( acc(1) * acc(1) + acc(2) * acc(2) ) );
++            r = atan2( acc(1) , acc(2) );
++            rpy_for_calc_vel_n_angvel(0) = r;
++            rpy_for_calc_vel_n_angvel(1) = p;
++            count = 0;
++            loop_count = 0;
++            //std::cerr << " r:" << r << "  p:" << p << std::endl;
++        }
++
++        hrp::Vector3 rpy_dot;
++        rpy_dot(0) = (gyro(1)*sin(rpy_for_calc_vel_n_angvel(2))+gyro(0)*cos(rpy_for_calc_vel_n_angvel(2)))/cos(rpy_for_calc_vel_n_angvel(1));
++        rpy_dot(1) = gyro(1)*cos(rpy_for_calc_vel_n_angvel(2))-gyro(0)*sin(rpy_for_calc_vel_n_angvel(2));
++        rpy_dot(2) = gyro(2)-rpy_dot(0)*sin(rpy_for_calc_vel_n_angvel(1));
++
++        rpy_for_calc_vel_n_angvel(0) += dt*rpy_dot(0);
++        rpy_for_calc_vel_n_angvel(1) += dt*rpy_dot(1);
++        rpy_for_calc_vel_n_angvel(2) += dt*rpy_dot(2);
++        rpy_for_calc_vel_n_angvel(2) *= 0.99;
++        //hrp::Vector3 modified_rpy = rpy;
++        //modified_rpy(0) = rpy(0) - 3.141592/2.0;
++        //std::cerr << "modified_rpy | r:" << modified_rpy(0) << "  p:" << modified_rpy(1) << "  y:" << modified_rpy(2) << std::endl;
++        //std::cerr << "rpy_for_calc_vel_n_angvel  | r:" << rpy_for_calc_vel_n_angvel(0) << "  p:" << rpy_for_calc_vel_n_angvel(1) << "  y:" << rpy_for_calc_vel_n_angvel(2) << std::endl;
++        hrp::Matrix33 realRotationMatrix = hrp::rotFromRpy(rpy_for_calc_vel_n_angvel);
++        //std::cerr << "realRotationMatrix ::" << std::endl;
++        //std::cerr << realRotationMatrix(0,0) << "  " << realRotationMatrix(0,1) << "  " << realRotationMatrix(0,2) << std::endl;
++        //std::cerr << realRotationMatrix(1,0) << "  " << realRotationMatrix(1,1) << "  " << realRotationMatrix(1,2) << std::endl;
++        //std::cerr << realRotationMatrix(2,0) << "  " << realRotationMatrix(2,1) << "  " << realRotationMatrix(2,2) << std::endl;
++        //std::cerr << std::endl;
++        angvel = gyro;
++
++        hrp::Matrix33 angvel_Matrix;
++        angvel_Matrix <<          0, -angvel(2),  angvel(1),
++                          angvel(2),          0, -angvel(0),
++                         -angvel(1),  angvel(0),          0;
++        hrp::Vector3 sensorPosition_relative_to_world = realRotationMatrix * sensorPosition;
++
++        hrp::Vector3 Body_acc = acc;// + gyro.cross(realRotationMatrix*sensorPosition_relative_to_world) + angvel.cross(angvel_Matrix*realRotationMatrix*sensorPosition_relative_to_world);
++        hrp::Vector3 g;
++        g << 0.0, 0.0, 9.80665;
++        g = realRotationMatrix*g;
++        //std::cerr << "g    x:" << g(0) << "  y:" << g(1) << "  z:" << g(2) << std::endl;
++        Body_acc = Body_acc - g;
++        acc_without_g = Body_acc;
++        /* 
++         * if(Body_acc(0) < 0.05 && Body_acc(0) > -0.05) {
++         *     Body_acc(0) = 0.0;
++         * }
++         * if(Body_acc(1) < 0.05 && Body_acc(1) > -0.05) {
++         *     Body_acc(1) = 0.0;
++         * }
++         * if(Body_acc(2) < 0.05 && Body_acc(2) > -0.05) {
++         *     Body_acc(2) = 0.0;
++         * }
++         */
++        //std::cerr << "acc_norm | " << (acc.norm()) << std::endl;
++        /* 
++         * if ( Body_acc(0) < -2.0 || Body_acc(0) > 2.0 ) {
++         *     std::cerr << "\x1b[33m"  << "body_acc | x:" << Body_acc(0) << "  y:" << Body_acc(1) << "  z:" << Body_acc(2) << "    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << "\x1b[0m" << std::endl;
++         * } else if ( Body_acc(1) < -2.0 || Body_acc(1) > 2.0 ) {
++         *     std::cerr << "\x1b[33m" <<  "body_acc | x:" << Body_acc(0) << "  y:" << Body_acc(1) << "  z:" << Body_acc(2) << "    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << "\x1b[0m" << std::endl;
++         * } else if ( !(loop_count % 100) ) {
++         *     std::cerr << "body_acc | x:" << Body_acc(0) << "  y:" << Body_acc(1) << "  z:" << Body_acc(2) << std::endl;
++         * }
++         */
++        if( (Body_acc(0) < 0.25) && (Body_acc(0) > -0.25) && (Body_acc(1) < 0.25) && (Body_acc(1) > -0.25) ) {
++            vel = (vel + Body_acc*dt)*0.99;//*0.9995;
++        } else {
++            vel = vel + Body_acc*dt;
++        }
++    }
++#endif
+ private:
+     KFilter r_filter, p_filter, y_filter;
+     double Q_angle, Q_rate, R_angle;
+     hrp::Matrix33 m_sensorR;
++#ifdef CALC_VEL_N_ANGVEL
++    int loop_count;
++    char count;
++    const double dt;
++    const double G;
++    hrp::Vector3 rpy_for_calc_vel_n_angvel;
++#endif
+ };
+ 
+ #endif /* RPYKALMANFILTER_H */
diff --git a/rtc20837.log b/rtc20837.log
new file mode 100644
index 00000000000..00bb5aedf43
--- /dev/null
+++ b/rtc20837.log
@@ -0,0 +1,9 @@
+Aug 16 10:19:39.481 INFO: manager: 1.1.0
+Aug 16 10:19:39.481 INFO: manager: Copyright (C) 2003-2010
+Aug 16 10:19:39.481 INFO: manager:   Noriaki Ando
+Aug 16 10:19:39.481 INFO: manager:   Intelligent Systems Research Institute, AIST
+Aug 16 10:19:39.481 INFO: manager: Manager starting.
+Aug 16 10:19:39.481 INFO: manager: Starting local logging.
+Aug 16 10:19:39.493 WARNING: NamingOnCorba: No endpoint for the CORBA naming service (localhost) was found.
+Aug 16 10:19:39.493 INFO: NamingManager: NameServer connection succeeded: corba/localhost
+Aug 16 10:19:39.493 INFO: ManagerServant: Master manager not found

From db6a13bd53ba837e592ea1ce2e9843c80514b2ab Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 12 Oct 2016 18:28:21 +0900
Subject: [PATCH 026/217] test iir filter

---
 rtc/AutoBalancer/AutoBalancer.cpp |   2 +-
 rtc/AutoBalancer/AutoBalancer.h   | 241 ++++++++++++++++++------------
 2 files changed, 145 insertions(+), 98 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 1ccce1e9b8c..0767bfb8225 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1023,7 +1023,7 @@ void AutoBalancer::solveLimbIK ()
   //for HumanSynchronizer
 //  static int HumanSyncCountdownNum = 5 * (1/m_dt);
   if(hsp->startCountdownForHumanSync){
-    std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";//ややCOMのIKに手間取った時プリント
+    std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
 	  hsp->updateCountDown();
   }
   if(hsp->isHumanSyncOn()){
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index bdd2ddb85ca..9aa0b9eb449 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -25,6 +25,7 @@
 // <rtc-template block="service_impl_h">
 #include "AutoBalancerService_impl.h"
 #include "interpolator.h"
+#include "../TorqueFilter/IIRFilter.h"
 
 
 // </rtc-template>
@@ -90,9 +91,6 @@ class HumanPose{
 
 
 
-
-
-
 class HumanSynchronizer{
   private:
 
@@ -120,6 +118,8 @@ class HumanSynchronizer{
     double h2r_ratio, tgt_h2r_ratio;
     double cmmr, tgt_cmmr;
     hrp::Vector3 lpf_zmp;
+    double HZ,DT,G;
+    hrp::Vector3 com_old,com_oldold,comacc;
 
   public:
 
@@ -131,12 +131,21 @@ class HumanSynchronizer{
     hrp::Vector3 init_wld_rp_basepos;
     HumanPose rp_ref_out;
     FILE *sr_log,*cz_log;
+    std::vector<double> fb_coeffs;
+    std::vector<double> ff_coeffs;
+
+
+
 
     HumanSynchronizer(){
       tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
 //      tgt_h2r_ratio = h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
 //      tgt_h2r_ratio = h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
 //      tgt_h2r_ratio = h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
+
+      HZ = 500;
+      DT = 1.0/HZ;
+      G = 9.80665;
       tgt_cmmr = cmmr = 1.0;
       use_x = true; use_y = true; use_z = true;
       cur_rfup_level = 0;       cur_lfup_level = 0;
@@ -156,17 +165,34 @@ class HumanSynchronizer{
       HumanSyncOn = false;
       ht_first_call = true;
       startCountdownForHumanSync = false;
-      countdown_num = 5*500;
+      countdown_num = 5*HZ;
       loop = 0;
       is_rf_in_air = false; is_lf_in_air = false;
-//      std::string home_path(std::getenv("HOME"));
-//      sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
-//      cz_log = fopen((home_path+"/HumanSync_com_zmp.log").c_str(),"w+");
+
+      double fc_digital = 0.5;
+      double fc = tan(fc_digital * M_PI / HZ) / (2 * M_PI);
+      double denom = 1 + (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc;
+      ff_coeffs.assign(3,0.0);
+      fb_coeffs.assign(3,0.0);
+      ff_coeffs[0] = (4 * M_PI * M_PI * fc*fc) / denom;
+      ff_coeffs[1] = (8 * M_PI * M_PI * fc*fc) / denom;
+      ff_coeffs[2] = (4 * M_PI * M_PI * fc*fc) / denom;
+      fb_coeffs[0] = 1.0;
+      fb_coeffs[1] = (8 * M_PI * M_PI * fc*fc - 2) / denom;
+      fb_coeffs[2] = (1 - (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc) / denom;
+
+
+//      IIRFilter com_iir = IIRFilter(2, fb_coeffs, ff_coeffs);
+
+
+      std::string home_path(std::getenv("HOME"));
+      sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
+      cz_log = fopen((home_path+"/HumanSync_com_zmp.log").c_str(),"w+");
       cout<<"HumanSynchronizer constructed"<<endl;
     }
     ~HumanSynchronizer(){
-//      fclose(sr_log);
-//      fclose(cz_log);
+      fclose(sr_log);
+      fclose(cz_log);
       cout<<"HumanSynchronizer destructed"<<endl;
     }
 
@@ -204,6 +230,7 @@ class HumanSynchronizer{
       applyCOMZMPXYZLock(rp_ref_out, rp_ref_out);
 
       applyCOMToSupportRegionLimit(rp_ref_out.lf+init_wld_rp_lfpos,rp_ref_out.rf+init_wld_rp_rfpos,rp_ref_out.com);
+      applyZMPCalcFromCOM(rp_ref_out.com,rp_ref_out.zmp);
 
       rp_ref_out_old = rp_ref_out;
 //      fprintf(log,"%f %f %f %f %f %f\n", rp_ref_out.com(0), rp_ref_out.com(1), rp_ref_out.zmp(0), rp_ref_out.zmp(1), hp_rel_raw.com(0), hp_rel_raw.com(1));
@@ -275,6 +302,14 @@ class HumanSynchronizer{
 //      for(int i=0;i<filt_out.idsize-5;i++){//ここではrfw,zmpとかはフィルターしないほうがいい
         filt_out.Seq(i) = (1-FNUM) * out_old.Seq(i) + FNUM * raw_in.Seq(i);
       }
+
+//      filt_out.com(1) = com_iir.executeFilter(raw_in.com[1]);
+      static double y[3],x[3];
+      x[0] = raw_in.com(1);
+      y[0] = ff_coeffs[0]*x[0] + ff_coeffs[1]*x[1] + ff_coeffs[2]*x[2] - fb_coeffs[1]*y[1] - fb_coeffs[2]*y[2];
+      x[2] = x[1];      x[1] = x[0];      y[2] = y[1];      y[1] = y[0];
+
+      filt_out.com(1) = y[0];
 //      for(int i=filt_out.idsize-5;i<filt_out.idsize;i++){//ここではrfw,zmpとかはフィルターしないほうがいい
 //        filt_out.Seq(i) = raw_in.Seq(i);
 //      }
@@ -349,7 +384,6 @@ class HumanSynchronizer{
         pre_cont_lfpos(0) = in.lf(0);
         pre_cont_lfpos(1) = in.lf(1);
       }
-//      out.rf(0) = 0; out.rf(1) = 0; out.lf(0) = 0; out.lf(1) = 0;//足固定テスト
     }
     void applyWorkspaceLimit(const HumanPose& in, HumanPose& out){
       if(!is_rf_contact && is_lf_contact){//右足浮遊時
@@ -359,89 +393,6 @@ class HumanSynchronizer{
         if(in.lf(1) + rp_wld_initpos.lf(1) < in.rf(1) + rp_wld_initpos.rf(1) + 0.15)out.lf(1) = in.rf(1) + rp_wld_initpos.rf(1) - rp_wld_initpos.lf(1) + 0.15;
       }
     }
-//    bool applyCOMToSupportRegionLimit_boost_geometry(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){
-////      ishiguro(ロボット体内のboostが古くて対応してない！！！)
-//      #include <boost/assert.hpp>
-//      #include <boost/assign/list_of.hpp>
-//      #include <boost/geometry.hpp>
-//      #include <boost/geometry/geometries/point_xy.hpp>
-//      #include <boost/geometry/geometries/box.hpp>
-//      #include <boost/geometry/geometries/polygon.hpp>
-//      #include <boost/geometry/algorithms/disjoint.hpp>
-//      #include <boost/geometry/geometries/geometries.hpp>
-//      namespace bg = boost::geometry;
-//      typedef bg::model::d2::point_xy<double> point;
-//      typedef bg::model::polygon<point> polygon;
-//      polygon lr_region,s_region;
-//      const double XUMARGIN = 0.04;//CHIDORI
-//      const double XLMARGIN = -0.02;
-//      const double YUMARGIN = 0.01;
-//      const double YLMARGIN = -0.01;
-//      bg::model::linestring<point> s_line = boost::assign::list_of<point>
-//        (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YLMARGIN)//LFの右下
-//        (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN)//LFの左下
-//        (lfin_abs(0) + XUMARGIN, lfin_abs(1) + YUMARGIN)//LFの左上
-//        (lfin_abs(0) + XUMARGIN, lfin_abs(1) + YLMARGIN)//LFの右上
-//        (rfin_abs(0) + XLMARGIN, rfin_abs(1) + YLMARGIN)//RFの右下
-//        (rfin_abs(0) + XLMARGIN, rfin_abs(1) + YUMARGIN)//RFの左下
-//        (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YUMARGIN)//RFの左上
-//        (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YLMARGIN)//RFの右上
-//             ;
-//      bg::convex_hull(s_line, s_region);
-//      point com2d(comin_abs(0),comin_abs(1));
-//      Eigen::Vector2d ans_point(comin_abs(0),comin_abs(1));
-//      enum point_state_t {PT_IN_NO_PATTERN = -1, PT_IN_REGION, PT_FIX_TO_EDGE, PT_FIX_TO_VERTEX};
-//      point_state_t pt_state = PT_IN_NO_PATTERN;
-//      if(bg::within(com2d, s_region)){//対象の点が凸包の内部に存在するかチェックする
-//        pt_state = PT_IN_REGION;
-//      }else{
-//        Eigen::Vector2d check_point(com2d.x(),com2d.y());
-//        int ans_lid=-1;
-//        for(int i=0;i<s_region.outer().size()-1;i++){//対象の点がある線分への垂線を有するかチェックする
-//          Eigen::Vector2d cur_vert(s_region.outer()[i].x(),s_region.outer()[i].y());
-//          Eigen::Vector2d next_vert(s_region.outer()[i+1].x(),s_region.outer()[i+1].y());
-//          Eigen::Vector2d edge_v = next_vert - cur_vert;
-//          Eigen::Vector2d tgt_pt_v = check_point - cur_vert;
-//          //ある線分への垂線を有し，かつ外側(時計回りエッジに対して左側)に存在するなら，対象の点からそのエッジへの垂線の交点が最近傍点
-//          if(edge_v.dot(tgt_pt_v)/edge_v.norm() > 0 && edge_v.dot(tgt_pt_v)/edge_v.norm() < edge_v.norm() && (edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
-//            ans_lid = i;
-//            ans_point = cur_vert + edge_v.normalized() * (edge_v.dot(tgt_pt_v)/edge_v.norm());
-//            pt_state = PT_FIX_TO_EDGE;
-//            break;
-//          }
-//        }
-//        if(pt_state == PT_FIX_TO_EDGE){
-//          cout<<"point will on the line:"<<ans_lid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
-//        }else{//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
-//          double cur_min_dis = bg::distance(com2d, s_region.outer()[0]);
-//          int ans_pid = 0;
-//          for(int i=1;i<s_region.outer().size();i++){
-//            if(bg::distance(com2d, s_region.outer()[i]) < cur_min_dis){
-//              cur_min_dis = bg::distance(com2d, s_region.outer()[i]);
-//              ans_pid = i;
-//            }
-//          }
-//          ans_point(0) = s_region.outer()[ans_pid].x();
-//          ans_point(1) = s_region.outer()[ans_pid].y();
-//          cout<<"point will on the vertex:"<<ans_pid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
-//          pt_state = PT_FIX_TO_VERTEX;
-//        }
-//      }
-//      if(loop%50==0){
-//        if(pt_state != PT_IN_REGION){
-//          std::cout<<"COM out of Support Region pt_state="<<pt_state<<std::endl;
-//        }
-//        for(int i=0;i<s_region.outer().size();i++){
-//          fprintf(sr_log,"%f %f %f\n",s_region.outer().at(i).x(), s_region.outer().at(i).y(),(double)loop/500.0);
-//        }
-//        fprintf(sr_log,"\n");
-//        fprintf(cz_log,"%f %f %f %f %f %f %f %f %f",(double)loop/500.0,comin_abs(0),comin_abs(1),comin_abs(2),rp_ref_out.zmp(0),rp_ref_out.zmp(1),rp_ref_out.zmp(2),ans_point(0),ans_point(1));
-//        fprintf(cz_log,"\n\n");
-//      }
-//      comin_abs(0) = ans_point(0);
-//      comin_abs(1) = ans_point(1);
-//      return true;
-//    }
     bool applyCOMToSupportRegionLimit(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){
 //      const double XUMARGIN = 0.04;//CHIDORI
 //      const double XLMARGIN = -0.02;
@@ -451,6 +402,10 @@ class HumanSynchronizer{
       const double XLMARGIN = -0.01;
       const double YUMARGIN = 0.0;
       const double YLMARGIN = -0.0;
+//      const double XUMARGIN = 0.10;//test
+//      const double XLMARGIN = -0.05;
+//      const double YUMARGIN = 0.03;
+//      const double YLMARGIN = -0.03;
       std::vector<hrp::Vector2> convex_hull;
       if(lfin_abs(0)>rfin_abs(0)){
         convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN));//LFの左下
@@ -529,17 +484,51 @@ class HumanSynchronizer{
 //          std::cout<<"COM out of Support Region pt_state="<<pt_state<<std::endl;
 //        }
 //        for(int i=0;i<convex_hull.size();i++){
-//          fprintf(sr_log,"%f %f %f\n",convex_hull[i](0), convex_hull[i](1),(double)loop/500.0);
+//          fprintf(sr_log,"%f %f %f\n",convex_hull[i](0), convex_hull[i](1),(double)loop/HZ);
 //        }
 //        fprintf(sr_log,"\n");
-//        fprintf(cz_log,"%f %f %f %f %f %f %f %f %f",(double)loop/500.0,comin_abs(0),comin_abs(1),comin_abs(2),rp_ref_out.zmp(0),rp_ref_out.zmp(1),rp_ref_out.zmp(2),ans_point(0),ans_point(1));
+//        fprintf(cz_log,"%f %f %f %f %f %f %f %f %f",(double)loop/HZ,comin_abs(0),comin_abs(1),comin_abs(2),rp_ref_out.zmp(0),rp_ref_out.zmp(1),rp_ref_out.zmp(2),ans_point(0),ans_point(1));
 //        fprintf(cz_log," %f %f %f %f",rp_ref_out.zmp(1),hpf_zmp(1),ans_point(1),ans_point(1) + hpf_zmp(1));
 //        fprintf(cz_log,"\n\n");
 //      }
+
       comin_abs(0) = ans_point(0);
       comin_abs(1) = ans_point(1);
-//      rp_ref_out.zmp(0) = comin_abs(0);
-//      rp_ref_out.zmp(1) = comin_abs(1);
+
+      return true;
+    }
+    bool applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
+
+      comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
+
+      const double MAXACC = 5;
+      if(comacc(0)>MAXACC)comacc(0)=MAXACC;else if(comacc(0)<-MAXACC)comacc(0)=-MAXACC;
+      if(comacc(1)>MAXACC)comacc(1)=MAXACC;else if(comacc(1)<-MAXACC)comacc(1)=-MAXACC;
+
+      static hrp::Vector3 comacc_old;
+
+
+
+//      static double y[3],x[3];
+//      x[0] = comacc(1);
+//      y[0] = ff_coeffs[0]*x[0] + ff_coeffs[1]*x[1] + ff_coeffs[2]*x[2] - fb_coeffs[1]*y[1] - fb_coeffs[2]*y[2];
+//      x[2] = x[1];      x[1] = x[0];      y[2] = y[1];      y[1] = y[0];
+//      comacc(1) = y[0];
+//
+//      comacc = 0.999*comacc_old + 0.001*comacc;
+      comacc = 0.99*comacc_old + 0.01*comacc;
+
+      comacc_old = comacc;
+
+      fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.zmp(0),rp_ref_out.zmp(1),comin(0)-(1.06/G)*comacc(0),comin(1)-(1.06/G)*comacc(1));
+
+      zmpout(0) = comin(0)-(1.06/G)*comacc(0);
+      zmpout(1) = comin(1)-(1.06/G)*comacc(1);
+
+
+      com_oldold = com_old;
+      com_old = comin;
+
       return true;
     }
     void applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out){
@@ -565,6 +554,64 @@ class HumanSynchronizer{
 
 
 
+
+
+//class IIR2D{
+//  private: hrp::Vector3 zero;
+//  public:
+//    hrp::Vector3 com,rf,lf,rh,lh;
+//    hrp::Vector3 zmp;
+//    int idsize;
+//    typedef struct{ hrp::Vector3 f; hrp::Vector3 t; }Wrench6;
+//    Wrench6 rfw,lfw;
+//    HumanPose() : idsize(10){
+//      clear();
+//    }
+//    ~HumanPose(){cerr<<"HumanPose destructed"<<endl;}
+//    void clear(){
+//      for(int i=0;i<idsize;i++){ Seq(i) = hrp::Vector3::Zero(); }
+//    }
+//    hrp::Vector3& Seq(const int i){
+//      switch(i){
+//        case 0: return com;
+//        case 1: return rf;
+//        case 2: return lf;
+//        case 3: return rh;
+//        case 4: return lh;
+//        case 5: return zmp;
+//        case 6: return rfw.f;
+//        case 7: return rfw.t;
+//        case 8: return lfw.f;
+//        case 9: return lfw.t;
+//        default: std::cerr <<"[WARN] HumanPose::getSeq(int) invalid index"<<std::endl;return zero;
+//      }
+//    }
+//    const hrp::Vector3& Seq(const int i) const{//const指定オブジェクトからオーバーロードする時のためのオーバーライド
+////      return Seq(i);//呼べない・・・うーん
+//      switch(i){
+//        case 0: return com;
+//        case 1: return rf;
+//        case 2: return lf;
+//        case 3: return rh;
+//        case 4: return lh;
+//        case 5: return zmp;
+//        case 6: return rfw.f;
+//        case 7: return rfw.t;
+//        case 8: return lfw.f;
+//        case 9: return lfw.t;
+//        default: std::cerr <<"[WARN] HumanPose::getSeq(int) invalid index"<<std::endl;return zero;
+//      }
+//    }
+//    static void hp_printf(HumanPose& in){
+//      const std::string cap[] = {"c","rf","lf","rh","lh","z","rw","","lw",""};
+//      for(int i=0;i<6;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.Seq(i)(0),in.Seq(i)(1),in.Seq(i)(2)); }
+//      for(int i=6;i<10;i++){ printf("\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.Seq(i)(0),in.Seq(i)(1),in.Seq(i)(2)); }
+//      printf("\n");
+//    }
+//    void print(){ hp_printf(*this); }
+//};
+
+
 // </rtc-template>
 
 using namespace RTC;

From 3d4194b50fa94a7cc1d211cb661811a69c5bbcea Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 14 Oct 2016 23:59:15 +0900
Subject: [PATCH 027/217] tune IIR filter and Swing leg lock function

---
 rtc/AutoBalancer/AutoBalancer.cpp |   4 +-
 rtc/AutoBalancer/AutoBalancer.h   | 338 +++++++++++++-----------------
 rtc/AutoBalancer/CMakeLists.txt   |   2 +-
 3 files changed, 150 insertions(+), 194 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 64227396782..c6b9c6c0c4e 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1069,8 +1069,8 @@ void AutoBalancer::solveLimbIK ()
   // additional COM fitting IK for HumanSynchronizer
 
 
-//  if(ikp.count("rarm"))ikp["rarm"].is_active = false;
-//  if(ikp.count("larm"))ikp["larm"].is_active = false;
+  if(ikp.count("rarm") && !hsp->use_rh)ikp["rarm"].is_active = false;
+  if(ikp.count("larm") && !hsp->use_lh)ikp["larm"].is_active = false;
 
   hrp::Vector3 tmp_com_err = hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM();
   int com_ik_loop=0;
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index c56c94fb7fe..19f1d0877fd 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -36,6 +36,47 @@
 // <rtc-template block="consumer_stub_h">
 
 
+class D2IIRFilterVec
+{
+private:
+    IIRFilter x_filters,y_filters,z_filters;
+    hrp::Vector3 ans;
+public:
+    D2IIRFilterVec(const std::string& error_prefix = ""){
+    };
+    ~D2IIRFilterVec() {};
+    bool setParameter(const double fc_in, const double HZ){
+      std::vector<double> fb_coeffs, ff_coeffs;
+      double fc = tan(fc_in * M_PI / HZ) / (2 * M_PI);
+      double denom = 1 + (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc;
+      ff_coeffs.resize(3);
+      fb_coeffs.resize(3);
+      ff_coeffs[0] = (4 * M_PI * M_PI * fc*fc) / denom;
+      ff_coeffs[1] = (8 * M_PI * M_PI * fc*fc) / denom;
+      ff_coeffs[2] = (4 * M_PI * M_PI * fc*fc) / denom;
+      fb_coeffs[0] = 1.0;
+      fb_coeffs[1] = (8 * M_PI * M_PI * fc*fc - 2) / denom;
+      fb_coeffs[2] = (1 - (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc) / denom;
+      return (
+        x_filters.setParameter(2,fb_coeffs,ff_coeffs) &&
+        y_filters.setParameter(2,fb_coeffs,ff_coeffs) &&
+        z_filters.setParameter(2,fb_coeffs,ff_coeffs)
+      );
+    };
+    hrp::Vector3 passFilter(const hrp::Vector3& input){
+      ans(0) = x_filters.passFilter(input(0));
+      ans(1) = y_filters.passFilter(input(1));
+      ans(2) = z_filters.passFilter(input(2));
+      return ans;
+    };
+
+};
+
+
+
+
+
+
 class HumanPose{
   private: hrp::Vector3 zero;
   public:
@@ -122,6 +163,8 @@ class HumanSynchronizer{
     hrp::Vector3 lpf_zmp;
     double HZ,DT,G;
     hrp::Vector3 com_old,com_oldold,comacc;
+    std::vector<D2IIRFilterVec> iir_v_filters;
+    D2IIRFilterVec comacc_v_filters;
 
   public:
 
@@ -133,8 +176,8 @@ class HumanSynchronizer{
     hrp::Vector3 init_wld_rp_basepos;
     HumanPose rp_ref_out;
     FILE *sr_log,*cz_log;
-    std::vector<double> fb_coeffs;
-    std::vector<double> ff_coeffs;
+    std::vector<double> fb_coeffs, ff_coeffs;
+    bool use_rh,use_lh;
 
 
 
@@ -144,15 +187,14 @@ class HumanSynchronizer{
 //      tgt_h2r_ratio = h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
 //      tgt_h2r_ratio = h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
 //      tgt_h2r_ratio = h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
-
       HZ = 500;
       DT = 1.0/HZ;
       G = 9.80665;
       tgt_cmmr = cmmr = 1.0;
-      use_x = true; use_y = true; use_z = true;
-      cur_rfup_level = 0;       cur_lfup_level = 0;
-      is_rf_contact = true;     is_lf_contact = true;
-      pre_cont_rfpos = hrp::Vector3::Zero();    pre_cont_lfpos = hrp::Vector3::Zero();
+      use_x = use_y = use_z = true;
+      cur_rfup_level = cur_lfup_level = 0;
+      is_rf_contact = is_lf_contact = true;
+      pre_cont_rfpos = pre_cont_lfpos = hrp::Vector3::Zero();
       init_wld_hp_rfpos = hrp::Vector3(0,-0.1/h2r_ratio,0);//h2r_ratioは可変なので後の値変更に注意
       init_wld_hp_lfpos = hrp::Vector3(0, 0.1/h2r_ratio,0);
       init_wld_rp_rfpos = hrp::Vector3(0,-0.1,0);
@@ -169,23 +211,14 @@ class HumanSynchronizer{
       startCountdownForHumanSync = false;
       countdown_num = 5*HZ;
       loop = 0;
-      is_rf_in_air = false; is_lf_in_air = false;
-
-      double fc_digital = 0.5;
-      double fc = tan(fc_digital * M_PI / HZ) / (2 * M_PI);
-      double denom = 1 + (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc;
-      ff_coeffs.assign(3,0.0);
-      fb_coeffs.assign(3,0.0);
-      ff_coeffs[0] = (4 * M_PI * M_PI * fc*fc) / denom;
-      ff_coeffs[1] = (8 * M_PI * M_PI * fc*fc) / denom;
-      ff_coeffs[2] = (4 * M_PI * M_PI * fc*fc) / denom;
-      fb_coeffs[0] = 1.0;
-      fb_coeffs[1] = (8 * M_PI * M_PI * fc*fc - 2) / denom;
-      fb_coeffs[2] = (1 - (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc) / denom;
+      is_rf_in_air = is_lf_in_air = false;
 
+      iir_v_filters.resize(5);
+      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(0.5,HZ);
+      comacc_v_filters.setParameter(1,HZ);
 
-//      IIRFilter com_iir = IIRFilter(2, fb_coeffs, ff_coeffs);
-
+//      use_rh = use_lh = false;
+      use_rh = use_lh = true;
 
       std::string home_path(std::getenv("HOME"));
       sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
@@ -213,26 +246,35 @@ class HumanSynchronizer{
         startCountdownForHumanSync = false;
       }
     }
-    void update(){
+    void update(){//////////  メインループ  ////////////
       gettimeofday(&t_calc_start, NULL);
-      updateHumanToRobotRatio(tgt_h2r_ratio);
-      updateCOMModRatio(tgt_cmmr);
-      removeInitOffsetPose(hp_wld_raw, hp_wld_initpos, hp_rel_raw);
-      applyInitHumanCOMOffset(hp_rel_raw, init_hp_calibcom, hp_rel_raw);
-      applyLPFilter(hp_rel_raw, hp_filtered, hp_filtered);
-      lockFootXYOnContact(hp_filtered, is_rf_contact, is_lf_contact, hp_filtered);//根本から改変すべき
-      calcWorldZMP((hp_filtered.rf+init_wld_hp_rfpos), (hp_filtered.lf+init_wld_hp_lfpos), hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
-      convertRelHumanPoseToRelRobotPose(hp_filtered,rp_ref_out);
-      applyCOMMoveModRatio(rp_ref_out,rp_ref_out);
-      judgeFootContactStates(hp_wld_raw.rfw, hp_wld_raw.lfw, is_rf_contact, is_lf_contact);//frwはすでに1000->100Hzにフィルタリングされている
-      overwriteFootZFromContactStates(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);
-//      lockFootXYOnContact(rp_ref_out, is_rf_contact, is_lf_contact, rp_ref_out);//ここで改変するとLPFかかってないからジャンプする
-      applyWorkspaceLimit(rp_ref_out, rp_ref_out);
-      applyVelLimit(rp_ref_out, rp_ref_out_old, rp_ref_out);
-      applyCOMZMPXYZLock(rp_ref_out, rp_ref_out);
-
-      applyCOMToSupportRegionLimit(rp_ref_out.lf+init_wld_rp_lfpos,rp_ref_out.rf+init_wld_rp_rfpos,rp_ref_out.com);
-      applyZMPCalcFromCOM(rp_ref_out.com,rp_ref_out.zmp);
+      updateHumanToRobotRatio             (tgt_h2r_ratio);
+      updateCOMModRatio                   (tgt_cmmr);
+      removeInitOffsetPose                (hp_wld_raw, hp_wld_initpos, hp_rel_raw);
+      applyInitHumanCOMOffset             (hp_rel_raw, init_hp_calibcom, hp_rel_raw);
+      lockFootXYOnContact                 (is_rf_contact, is_lf_contact, hp_rel_raw);//根本から改変すべき2
+
+      hp_filtered = hp_rel_raw;
+
+//      applyLPFilterToEE(hp_rel_raw, hp_filtered);//次に使う.rf,.lfのためにEEだけは先にフィルターしておかないといけない説もある
+      calcWorldZMP                        ((hp_filtered.rf+init_wld_hp_rfpos), (hp_filtered.lf+init_wld_hp_lfpos), hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
+      convertRelHumanPoseToRelRobotPose   (hp_filtered,rp_ref_out);
+      applyCOMMoveModRatio                (rp_ref_out);
+      judgeFootContactStates              (hp_wld_raw.rfw, hp_wld_raw.lfw, is_rf_contact, is_lf_contact);//frwはすでに1000->100Hzにフィルタリングされている
+
+      applyEEWorkspaceLimit               (rp_ref_out);
+
+      applyCOMToSupportRegionLimit        (rp_ref_out.lf+init_wld_rp_lfpos,rp_ref_out.rf+init_wld_rp_rfpos,rp_ref_out.com);
+
+      applyLPFilter                       (rp_ref_out);
+      applyZMPCalcFromCOM                 (rp_ref_out.com,rp_ref_out.zmp);
+      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, is_rf_contact, is_lf_contact);//
+
+      applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
+      applyCOMZMPXYZLock                  (rp_ref_out);
+
+
+      overwriteFootZFromContactStates     (is_rf_contact, is_lf_contact, rp_ref_out);
 
       rp_ref_out_old = rp_ref_out;
 //      fprintf(log,"%f %f %f %f %f %f\n", rp_ref_out.com(0), rp_ref_out.com(1), rp_ref_out.zmp(0), rp_ref_out.zmp(1), hp_rel_raw.com(0), hp_rel_raw.com(1));
@@ -299,22 +341,8 @@ class HumanSynchronizer{
 //      setInitOffsetPose();
 //      HumanSyncOn = true;
 //    }
-    void applyLPFilter(const HumanPose& raw_in, const HumanPose& out_old, HumanPose& filt_out){
-      for(int i=0;i<filt_out.idsize;i++){
-//      for(int i=0;i<filt_out.idsize-5;i++){//ここではrfw,zmpとかはフィルターしないほうがいい
-        filt_out.Seq(i) = (1-FNUM) * out_old.Seq(i) + FNUM * raw_in.Seq(i);
-      }
-
-//      filt_out.com(1) = com_iir.executeFilter(raw_in.com[1]);
-      static double y[3],x[3];
-      x[0] = raw_in.com(1);
-      y[0] = ff_coeffs[0]*x[0] + ff_coeffs[1]*x[1] + ff_coeffs[2]*x[2] - fb_coeffs[1]*y[1] - fb_coeffs[2]*y[2];
-      x[2] = x[1];      x[1] = x[0];      y[2] = y[1];      y[1] = y[0];
-
-      filt_out.com(1) = y[0];
-//      for(int i=filt_out.idsize-5;i<filt_out.idsize;i++){//ここではrfw,zmpとかはフィルターしないほうがいい
-//        filt_out.Seq(i) = raw_in.Seq(i);
-//      }
+    void applyLPFilter(HumanPose& tgt){
+      for(int i=0;i<iir_v_filters.size();i++){ tgt.Seq(i) = iir_v_filters[i].passFilter(tgt.Seq(i)); }
     }
     void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
       for(int i=0;i<rp_out.idsize-4;i++){
@@ -323,79 +351,101 @@ class HumanSynchronizer{
       rp_out.rfw  = hp_in.rfw;
       rp_out.lfw  = hp_in.lfw;
     }
-    void applyCOMMoveModRatio(const HumanPose& in, HumanPose& out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
-      out = in;
-      out.com = cmmr * in.com;
+    void applyCOMMoveModRatio(HumanPose& tgt){//結局初期指定からの移動量(=Rel)で計算をしてゆく
+      tgt.com = cmmr * tgt.com;
     }
     void judgeFootContactStates(const HumanPose::Wrench6& rfw_in, const HumanPose::Wrench6& lfw_in, bool& rfcs_ans, bool& lfcs_ans){
-      if      (rfcs_ans  && lfcs_ans && rfw_in.f(2)<CNT_F_TH   ){rfcs_ans = false;}//右足ついた状態から上げる
-      else if (!rfcs_ans && lfcs_ans && rfw_in.f(2)>CNT_F_TH+30){rfcs_ans = true;}//右足浮いた状態から下げる
-      if      (lfcs_ans  && rfcs_ans && lfw_in.f(2)<CNT_F_TH   ){lfcs_ans = false;}//左足ついた状態から上げる
-      else if (!lfcs_ans && rfcs_ans && lfw_in.f(2)>CNT_F_TH+30){lfcs_ans = true;}//左足浮いた状態から下げる
+//      if      (rfcs_ans  && lfcs_ans && rfw_in.f(2)<CNT_F_TH   ){rfcs_ans = false;}//右足ついた状態から上げる
+//      else if (!rfcs_ans && lfcs_ans && rfw_in.f(2)>CNT_F_TH+30){rfcs_ans = true;}//右足浮いた状態から下げる
+//      if      (lfcs_ans  && rfcs_ans && lfw_in.f(2)<CNT_F_TH   ){lfcs_ans = false;}//左足ついた状態から上げる
+//      else if (!lfcs_ans && rfcs_ans && lfw_in.f(2)>CNT_F_TH+30){lfcs_ans = true;}//左足浮いた状態から下げる
+
+//      if(rfw_in.f(2)>CNT_F_TH+30 || lfw_in.f(2)>CNT_F_TH+30 ){
+      if(HumanSyncOn){
+        if      (rfcs_ans  && rfw_in.f(2)<CNT_F_TH   ){rfcs_ans = false;}//右足ついた状態から上げる
+        else if (!rfcs_ans && rfw_in.f(2)>CNT_F_TH+30){rfcs_ans = true;}//右足浮いた状態から下げる
+        if      (lfcs_ans  && lfw_in.f(2)<CNT_F_TH   ){lfcs_ans = false;}//左足ついた状態から上げる
+        else if (!lfcs_ans && lfw_in.f(2)>CNT_F_TH+30){lfcs_ans = true;}//左足浮いた状態から下げる
+      }
+      if(!rfcs_ans && !lfcs_ans){rfcs_ans = lfcs_ans = true;}//両足ジャンプは禁止
 
 
-      //else if (rfw_in.f(2)>CNT_F_TH && lfw_in.f(2)<CNT_F_TH){rfcs_ans = true;   lfcs_ans = false;}//右足接地かつ左足浮遊
-      //else                                              {rfcs_ans = true;   lfcs_ans = true;}//両足接地または両足浮遊
     }
-    void overwriteFootZFromContactStates(const HumanPose& in, const bool& rfcs_in, const bool& lfcs_in, HumanPose& out){
-      out = in;
+    void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
+      if(rfcs_ans && !lfcs_ans){
+        if(tgt.zmp(1) > tgt.rf(1) + init_wld_rp_rfpos(1) + 0.03){
+          lfcs_ans = true;
+          cout<<"LF lock"<<endl;
+        }
+      }
+      if(!rfcs_ans && lfcs_ans){
+        if(tgt.zmp(1) < tgt.lf(1) + init_wld_rp_lfpos(1) - 0.03){
+          rfcs_ans = true;
+          cout<<"RF lock"<<endl;
+        }
+      }
+
+
+    }
+
+
+    void overwriteFootZFromContactStates(const bool& rfcs_in, const bool& lfcs_in, HumanPose& tgt){
       if(!rfcs_in){//右足浮遊時
         if(cur_rfup_level < FUPLEVELNUM){
           cur_rfup_level++;
-          out.rf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
+          tgt.rf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
         }else{
-          out.rf(2) = FUPHIGHT;
+          tgt.rf(2) = FUPHIGHT;
         }
       }else{
         if(cur_rfup_level > 0){
           cur_rfup_level--;
-          out.rf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
+          tgt.rf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
         }else{
-          out.rf(2) = 0;
+          tgt.rf(2) = 0;
         }
       }
       if(!lfcs_in){//左足浮遊時
         if(cur_lfup_level < FUPLEVELNUM){
           cur_lfup_level++;
-          out.lf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
+          tgt.lf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
         }else{
-          out.lf(2) = FUPHIGHT;
+          tgt.lf(2) = FUPHIGHT;
         }
       }else{
         if(cur_lfup_level > 0){
           cur_lfup_level--;
-          out.lf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
+          tgt.lf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
         }else{
-          out.lf(2) = 0;
+          tgt.lf(2) = 0;
         }
       }
     }
-    void lockFootXYOnContact(const HumanPose& in, const bool& rfcs_in, const bool& lfcs_in, HumanPose& out){
-      out = in;
+    void lockFootXYOnContact(const bool& rfcs_in, const bool& lfcs_in, HumanPose& tgt){
       if(rfcs_in){
-        out.rf(0) = pre_cont_rfpos(0);
-        out.rf(1) = pre_cont_rfpos(1);
+        tgt.rf(0) = pre_cont_rfpos(0);
+        tgt.rf(1) = pre_cont_rfpos(1);
       }else{
-        pre_cont_rfpos(0) = in.rf(0);
-        pre_cont_rfpos(1) = in.rf(1);
+        pre_cont_rfpos(0) = tgt.rf(0);
+        pre_cont_rfpos(1) = tgt.rf(1);
       }
       if(lfcs_in){
-        out.lf(0) = pre_cont_lfpos(0);
-        out.lf(1) = pre_cont_lfpos(1);
+        tgt.lf(0) = pre_cont_lfpos(0);
+        tgt.lf(1) = pre_cont_lfpos(1);
       }else{
-        pre_cont_lfpos(0) = in.lf(0);
-        pre_cont_lfpos(1) = in.lf(1);
+        pre_cont_lfpos(0) = tgt.lf(0);
+        pre_cont_lfpos(1) = tgt.lf(1);
       }
     }
-    void applyWorkspaceLimit(const HumanPose& in, HumanPose& out){
+    void applyEEWorkspaceLimit(HumanPose& tgt){
       if(!is_rf_contact && is_lf_contact){//右足浮遊時
-        if(in.rf(1) + rp_wld_initpos.rf(1) > in.lf(1) + rp_wld_initpos.lf(1) - 0.15)out.rf(1) = in.lf(1) + rp_wld_initpos.lf(1) - rp_wld_initpos.rf(1) - 0.15;
+        if(tgt.rf(1) + rp_wld_initpos.rf(1) > tgt.lf(1) + rp_wld_initpos.lf(1) - 0.15)tgt.rf(1) = tgt.lf(1) + rp_wld_initpos.lf(1) - rp_wld_initpos.rf(1) - 0.15;
       }
       else if(is_rf_contact && !is_lf_contact){//左足浮遊時
-        if(in.lf(1) + rp_wld_initpos.lf(1) < in.rf(1) + rp_wld_initpos.rf(1) + 0.15)out.lf(1) = in.rf(1) + rp_wld_initpos.rf(1) - rp_wld_initpos.lf(1) + 0.15;
+        if(tgt.lf(1) + rp_wld_initpos.lf(1) < tgt.rf(1) + rp_wld_initpos.rf(1) + 0.15)tgt.lf(1) = tgt.rf(1) + rp_wld_initpos.rf(1) - rp_wld_initpos.lf(1) + 0.15;
       }
     }
-    bool applyCOMToSupportRegionLimit(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){
+    bool applyCOMToSupportRegionLimit(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
 //      const double XUMARGIN = 0.04;//CHIDORI
 //      const double XLMARGIN = -0.02;
 //      const double YUMARGIN = 0.01;
@@ -474,12 +524,6 @@ class HumanSynchronizer{
           pt_state = PT_FIX_TO_VERTEX;
         }
       }
-//      double FNUM_Z = 0.005;
-      double FNUM_Z = 0.01;
-      lpf_zmp = (1-FNUM_Z) * lpf_zmp + FNUM_Z * rp_ref_out.zmp;
-      hrp::Vector3 hpf_zmp = rp_ref_out.zmp - lpf_zmp;
-//      rp_ref_out.zmp(0) = ans_point(0) + hpf_zmp(0);
-//      rp_ref_out.zmp(1) = ans_point(1) + hpf_zmp(1);
 
 //      if(loop%50==0){//ログ
 //        if(pt_state != PT_IN_REGION){
@@ -499,39 +543,17 @@ class HumanSynchronizer{
 
       return true;
     }
-    bool applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
-
+    void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
       comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
-
       const double MAXACC = 5;
       if(comacc(0)>MAXACC)comacc(0)=MAXACC;else if(comacc(0)<-MAXACC)comacc(0)=-MAXACC;
       if(comacc(1)>MAXACC)comacc(1)=MAXACC;else if(comacc(1)<-MAXACC)comacc(1)=-MAXACC;
-
-      static hrp::Vector3 comacc_old;
-
-
-
-//      static double y[3],x[3];
-//      x[0] = comacc(1);
-//      y[0] = ff_coeffs[0]*x[0] + ff_coeffs[1]*x[1] + ff_coeffs[2]*x[2] - fb_coeffs[1]*y[1] - fb_coeffs[2]*y[2];
-//      x[2] = x[1];      x[1] = x[0];      y[2] = y[1];      y[1] = y[0];
-//      comacc(1) = y[0];
-//
-//      comacc = 0.999*comacc_old + 0.001*comacc;
-      comacc = 0.99*comacc_old + 0.01*comacc;
-
-      comacc_old = comacc;
-
-      fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.zmp(0),rp_ref_out.zmp(1),comin(0)-(1.06/G)*comacc(0),comin(1)-(1.06/G)*comacc(1));
-
+      comacc = comacc_v_filters.passFilter(comacc);
       zmpout(0) = comin(0)-(1.06/G)*comacc(0);
       zmpout(1) = comin(1)-(1.06/G)*comacc(1);
-
-
+      fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.zmp(0),rp_ref_out.zmp(1),zmpout(0),zmpout(1));
       com_oldold = com_old;
       com_old = comin;
-
-      return true;
     }
     void applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out){
       for(int i=0;i<3;i++){if(in.com(i) - in_old.com(i) > MAXVEL){out.com(i) = in_old.com(i) + MAXVEL;}else if(in.com(i) - in_old.com(i) < -MAXVEL){out.com(i) = in_old.com(i) - MAXVEL;}}
@@ -540,80 +562,14 @@ class HumanSynchronizer{
       for(int i=0;i<3;i++){if(in.rh(i)  - in_old.rh(i)  > MAXVEL){out.rh(i)  = in_old.rh(i)  + MAXVEL;}else if(in.rh(i)  - in_old.rh(i)  < -MAXVEL){out.rh(i)  = in_old.rh(i)  - MAXVEL;}}
       for(int i=0;i<3;i++){if(in.lh(i)  - in_old.lh(i)  > MAXVEL){out.lh(i)  = in_old.lh(i)  + MAXVEL;}else if(in.lh(i)  - in_old.lh(i)  < -MAXVEL){out.lh(i)  = in_old.lh(i)  - MAXVEL;}}
     }
-    void applyCOMZMPXYZLock(const HumanPose& in, HumanPose& out){
-      out = in;
-        if(!use_x){out.com(0) = 0;out.zmp(0) = 0;}
-        if(!use_y){out.com(1) = 0;out.zmp(1) = 0;}
-        if(!use_z){out.com(2) = 0;}
-        out.rh(0)=0;
-        out.rh(1)=out.com(1);
-        out.rh(2)=0;
-        out.lh(0)=0;
-        out.lh(1)=out.com(1);
-        out.lh(2)=0;
+    void applyCOMZMPXYZLock(HumanPose& tgt){
+        if(!use_x){tgt.com(0) = 0;tgt.zmp(0) = 0;}
+        if(!use_y){tgt.com(1) = 0;tgt.zmp(1) = 0;}
+        if(!use_z){tgt.com(2) = 0;}
     }
 };
 
 
-
-
-
-//class IIR2D{
-//  private: hrp::Vector3 zero;
-//  public:
-//    hrp::Vector3 com,rf,lf,rh,lh;
-//    hrp::Vector3 zmp;
-//    int idsize;
-//    typedef struct{ hrp::Vector3 f; hrp::Vector3 t; }Wrench6;
-//    Wrench6 rfw,lfw;
-//    HumanPose() : idsize(10){
-//      clear();
-//    }
-//    ~HumanPose(){cerr<<"HumanPose destructed"<<endl;}
-//    void clear(){
-//      for(int i=0;i<idsize;i++){ Seq(i) = hrp::Vector3::Zero(); }
-//    }
-//    hrp::Vector3& Seq(const int i){
-//      switch(i){
-//        case 0: return com;
-//        case 1: return rf;
-//        case 2: return lf;
-//        case 3: return rh;
-//        case 4: return lh;
-//        case 5: return zmp;
-//        case 6: return rfw.f;
-//        case 7: return rfw.t;
-//        case 8: return lfw.f;
-//        case 9: return lfw.t;
-//        default: std::cerr <<"[WARN] HumanPose::getSeq(int) invalid index"<<std::endl;return zero;
-//      }
-//    }
-//    const hrp::Vector3& Seq(const int i) const{//const指定オブジェクトからオーバーロードする時のためのオーバーライド
-////      return Seq(i);//呼べない・・・うーん
-//      switch(i){
-//        case 0: return com;
-//        case 1: return rf;
-//        case 2: return lf;
-//        case 3: return rh;
-//        case 4: return lh;
-//        case 5: return zmp;
-//        case 6: return rfw.f;
-//        case 7: return rfw.t;
-//        case 8: return lfw.f;
-//        case 9: return lfw.t;
-//        default: std::cerr <<"[WARN] HumanPose::getSeq(int) invalid index"<<std::endl;return zero;
-//      }
-//    }
-//    static void hp_printf(HumanPose& in){
-//      const std::string cap[] = {"c","rf","lf","rh","lh","z","rw","","lw",""};
-//      for(int i=0;i<6;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.Seq(i)(0),in.Seq(i)(1),in.Seq(i)(2)); }
-//      for(int i=6;i<10;i++){ printf("\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.Seq(i)(0),in.Seq(i)(1),in.Seq(i)(2)); }
-//      printf("\n");
-//    }
-//    void print(){ hp_printf(*this); }
-//};
-
-
 // </rtc-template>
 
 using namespace RTC;
diff --git a/rtc/AutoBalancer/CMakeLists.txt b/rtc/AutoBalancer/CMakeLists.txt
index 642f5926274..d279c593966 100644
--- a/rtc/AutoBalancer/CMakeLists.txt
+++ b/rtc/AutoBalancer/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(comp_sources AutoBalancer.cpp AutoBalancerService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp PreviewController.cpp GaitGenerator.cpp)
+set(comp_sources AutoBalancer.cpp AutoBalancerService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp PreviewController.cpp GaitGenerator.cpp ../TorqueFilter/IIRFilter.cpp)
 set(libs hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub)
 add_library(AutoBalancer SHARED ${comp_sources})
 target_link_libraries(AutoBalancer ${libs})

From 18c7496af662f9e5b3b5391e0febb960641411e7 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 19 Oct 2016 04:38:22 +0900
Subject: [PATCH 028/217] use interpolator to ZMP-COM offset

---
 rtc/AutoBalancer/AutoBalancer.cpp | 92 ++++++++++++++++---------------
 rtc/AutoBalancer/AutoBalancer.h   | 88 +++++++++++++++--------------
 2 files changed, 90 insertions(+), 90 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index c6b9c6c0c4e..3d664cc07bd 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -365,6 +365,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
 
     //for HumanSynchronizer
     hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer());
+    if(leg_pos.size()>=2){ hsp->init_wld_rp_rfpos = leg_pos[0]; hsp->init_wld_rp_lfpos = leg_pos[1]; }
 
     return RTC::RTC_OK;
 }
@@ -482,6 +483,36 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     if (m_htlhIn.isNew()) { m_htlhIn.read();  hp_raw_data.lh  = HumanSynchronizer::Point3DToVector3(m_htlh); }
     if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
     hsp->readInput(hp_raw_data);
+    hsp->current_basepos = m_robot->rootLink()->p;
+
+//    if(hsp->startCountdownForHumanSync){
+//      std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
+//      hsp->updateCountDown();
+//    }
+//    if(hsp->isHumanSyncOn()){
+//      if(hsp->ht_first_call){
+//        std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
+//        hsp->ht_first_call = false;
+//      }
+//    }else{
+//      hsp->calibInitHumanCOMFromZMP();
+//      hsp->setInitOffsetPose();
+//      hsp->rp_wld_initpos.com = m_robot->calcCM();
+//      hsp->rp_wld_initpos.rf = ikp["rleg"].target_link->p;
+//      hsp->rp_wld_initpos.lf = ikp["lleg"].target_link->p;
+//      if(ikp.count("rarm"))hsp->rp_wld_initpos.rh = ikp["rarm"].target_link->p;
+//      if(ikp.count("larm"))hsp->rp_wld_initpos.lh = ikp["larm"].target_link->p;
+//    }
+//    hsp->update();//////HumanSynchronizerの主要処理
+//    if(loop%100==0)hsp->rp_ref_out.print();
+  //  if(loop%100==0)cerr<<"getUpdateTime:"<<hsp->getUpdateTime()*1000<<"[ms]"<<endl;
+
+
+
+    if(ikp.count("rarm") && !hsp->use_rh)ikp["rarm"].is_active = false;
+    if(ikp.count("larm") && !hsp->use_lh)ikp["larm"].is_active = false;
+    m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
+    m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
 
 
     Guard guard(m_mutex);
@@ -585,9 +616,6 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     m_cogOut.write();
     m_sbpCogOffsetOut.write();
 
-
-//printf("m_zmp:%f, abs:%f\n",m_zmp.data.y,m_basePos.data.y+m_zmp.data.y);
-
     // reference acceleration
     hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
     if (sen != NULL) {
@@ -1015,41 +1043,34 @@ void AutoBalancer::solveLimbIK ()
 
 
   //for HumanSynchronizer
-//  static int HumanSyncCountdownNum = 5 * (1/m_dt);
   if(hsp->startCountdownForHumanSync){
     std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
 	  hsp->updateCountDown();
   }
   if(hsp->isHumanSyncOn()){
-	  if(hsp->ht_first_call){
-		  hsp->init_wld_rp_basepos = m_robot->rootLink()->p;
-      hsp->setInitOffsetPose();
-      hsp->rp_wld_initpos.com = m_robot->calcCM();
-      hsp->rp_wld_initpos.rf = ikp["rleg"].target_link->p;
-      hsp->rp_wld_initpos.lf = ikp["lleg"].target_link->p;
-      if(ikp.count("rarm"))hsp->rp_wld_initpos.rh = ikp["rarm"].target_link->p;
-      if(ikp.count("larm"))hsp->rp_wld_initpos.lh = ikp["larm"].target_link->p;
-      hsp->ht_first_call = false;
-
+    if(hsp->ht_first_call){
       hsp->calibInitHumanCOMFromZMP();
-
       std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
-
-	  }
+      hsp->ht_first_call = false;
+    }
   }else{
     hsp->setInitOffsetPose();
+//    hsp->calibInitHumanCOMFromZMP();
+    hsp->rp_wld_initpos.com = m_robot->calcCM();
+    hsp->rp_wld_initpos.rf = ikp["rleg"].target_link->p;
+    hsp->rp_wld_initpos.lf = ikp["lleg"].target_link->p;
+    if(ikp.count("rarm"))hsp->rp_wld_initpos.rh = ikp["rarm"].target_link->p;
+    if(ikp.count("larm"))hsp->rp_wld_initpos.lh = ikp["larm"].target_link->p;
+    hsp->init_basepos = m_robot->rootLink()->p;
   }
-
   hsp->update();//////HumanSynchronizerの主要処理
   if(loop%100==0)hsp->rp_ref_out.print();
-  if(loop%100==0)cerr<<"getUpdateTime:"<<hsp->getUpdateTime()*1000<<"[ms]"<<endl;
+//  if(loop%100==0)cerr<<"getUpdateTime:"<<hsp->getUpdateTime()*1000<<"[ms]"<<endl;
 
-  if(hsp->isHumanSyncOn()){
-	  ////////////////////// 重心拘束位置設定 /////////////////////////
+
+  //for HumanSynchronizer
+  if(hsp->isHumanSyncOn()){	  ////////////////////// 重心拘束位置設定 /////////////////////////
     m_robot->rootLink()->p = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;//まずは重心目標分，体幹を動かす
-    m_robot->rootLink()->p(2) = hsp->init_wld_rp_basepos(2)+ hsp->rp_ref_out.com(2);//高さだけはベース基準
-    m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
-    m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
   }
 
   // Fix for toe joint
@@ -1067,11 +1088,6 @@ void AutoBalancer::solveLimbIK ()
 
 
   // additional COM fitting IK for HumanSynchronizer
-
-
-  if(ikp.count("rarm") && !hsp->use_rh)ikp["rarm"].is_active = false;
-  if(ikp.count("larm") && !hsp->use_lh)ikp["larm"].is_active = false;
-
   hrp::Vector3 tmp_com_err = hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM();
   int com_ik_loop=0;
   const int COM_IK_MAX_LOOP = 10;
@@ -1082,9 +1098,7 @@ void AutoBalancer::solveLimbIK ()
     if(ikp.count("rarm"))ikp["rarm"].target_p0 = hsp->rp_wld_initpos.rh + hsp->rp_ref_out.rh;
     if(ikp.count("larm"))ikp["larm"].target_p0 = hsp->rp_wld_initpos.lh + hsp->rp_ref_out.lh;
     while(fabs(tmp_com_err(0))>COM_IK_MAX_ERROR || fabs(tmp_com_err(1))>COM_IK_MAX_ERROR || fabs(tmp_com_err(2))>COM_IK_MAX_ERROR){//Z方向のCOM合わせは要注意(たまに怪しい)
-		  m_robot->rootLink()->p(0) += tmp_com_err(0);
-      m_robot->rootLink()->p(1) += tmp_com_err(1);
-      m_robot->rootLink()->p(2) += tmp_com_err(2);
+		  m_robot->rootLink()->p += tmp_com_err;
 		  m_robot->calcForwardKinematics();
 		  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
 		    if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
@@ -1094,20 +1108,8 @@ void AutoBalancer::solveLimbIK ()
 		  if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "[" << m_profile.instance_name << "] COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
 	  }
 
-//    static hrp::Vector3 pre_ik_basepos;
-//    for(int i=0;i<3;i++){//重心拘束で解いた結果の体幹軌道の滑らかさは保証されていないので体幹位置に速度制限＆再度IK
-//      if(m_robot->rootLink()->p(i) - pre_ik_basepos(i) > 0.0002){ m_robot->rootLink()->p(i) =  pre_ik_basepos(i) + 0.0002; }
-//      else if(m_robot->rootLink()->p(i) - pre_ik_basepos(i) < -0.0002){ m_robot->rootLink()->p(i) =  pre_ik_basepos(i) - 0.0002; }
-//    }
-//    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-//      if (it->second.is_active) solveLimbIKforLimb(it->second);
-//    }
-//    pre_ik_basepos = m_robot->rootLink()->p;
-
     //outport用のデータ上書き
-    ref_zmp(0) = hsp->rp_ref_out.zmp(0);
-    ref_zmp(1) = hsp->rp_ref_out.zmp(1);
-    ref_zmp(2) = 0;
+    ref_zmp = hsp->rp_ref_out.zmp;
     ref_cog = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;
 
 	  if(loop%100==0)if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 19f1d0877fd..9f8055c31d0 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -35,16 +35,15 @@
 // Service Consumer stub headers
 // <rtc-template block="consumer_stub_h">
 
-
-class D2IIRFilterVec
+class BiquadIIRFilterVec
 {
 private:
     IIRFilter x_filters,y_filters,z_filters;
     hrp::Vector3 ans;
 public:
-    D2IIRFilterVec(const std::string& error_prefix = ""){
+    BiquadIIRFilterVec(const std::string& error_prefix = ""){
     };
-    ~D2IIRFilterVec() {};
+    ~BiquadIIRFilterVec() {};
     bool setParameter(const double fc_in, const double HZ){
       std::vector<double> fb_coeffs, ff_coeffs;
       double fc = tan(fc_in * M_PI / HZ) / (2 * M_PI);
@@ -69,7 +68,6 @@ class D2IIRFilterVec
       ans(2) = z_filters.passFilter(input(2));
       return ans;
     };
-
 };
 
 
@@ -144,8 +142,6 @@ class HumanSynchronizer{
     double CNT_F_TH;
     hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
     hrp::Vector3 init_wld_rp_compos;
-    hrp::Vector3 init_wld_hp_rfpos,init_wld_hp_lfpos;
-    hrp::Vector3 init_wld_rp_rfpos,init_wld_rp_lfpos;
     hrp::Vector3 init_hp_calibcom;
     double MAXVEL,MAXACC;
     HumanPose hp_wld_raw;
@@ -163,24 +159,27 @@ class HumanSynchronizer{
     hrp::Vector3 lpf_zmp;
     double HZ,DT,G;
     hrp::Vector3 com_old,com_oldold,comacc;
-    std::vector<D2IIRFilterVec> iir_v_filters;
-    D2IIRFilterVec comacc_v_filters;
+    std::vector<BiquadIIRFilterVec> iir_v_filters;
+    BiquadIIRFilterVec comacc_v_filters;
 
   public:
 
     bool startCountdownForHumanSync;
     bool ht_first_call;
     bool is_rf_contact,is_lf_contact;
-    HumanPose rp_wld_initpos;
+    HumanPose rp_wld_initpos;//IKでのリンク原点の位置(足首，手首など)
     bool use_x,use_y,use_z;
-    hrp::Vector3 init_wld_rp_basepos;
     HumanPose rp_ref_out;
     FILE *sr_log,*cz_log;
     std::vector<double> fb_coeffs, ff_coeffs;
     bool use_rh,use_lh;
+    hrp::Vector3 init_wld_rp_rfpos,init_wld_rp_lfpos;
+    hrp::Vector3 init_wld_hp_rfpos,init_wld_hp_lfpos;
+    hrp::Vector3 current_basepos,init_basepos;
 
 
-
+    interpolator *init_zmp_com_offset_interpolator;
+    double zmp_com_offset_ip_ratio;
 
     HumanSynchronizer(){
       tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
@@ -195,10 +194,6 @@ class HumanSynchronizer{
       cur_rfup_level = cur_lfup_level = 0;
       is_rf_contact = is_lf_contact = true;
       pre_cont_rfpos = pre_cont_lfpos = hrp::Vector3::Zero();
-      init_wld_hp_rfpos = hrp::Vector3(0,-0.1/h2r_ratio,0);//h2r_ratioは可変なので後の値変更に注意
-      init_wld_hp_lfpos = hrp::Vector3(0, 0.1/h2r_ratio,0);
-      init_wld_rp_rfpos = hrp::Vector3(0,-0.1,0);
-      init_wld_rp_lfpos = hrp::Vector3(0, 0.1,0);
       init_hp_calibcom.Zero();
       lpf_zmp.Zero();
       FNUM = 0.01;//0.01でも荒い?
@@ -220,6 +215,13 @@ class HumanSynchronizer{
 //      use_rh = use_lh = false;
       use_rh = use_lh = true;
 
+      init_zmp_com_offset_interpolator = new interpolator(1, DT, interpolator::HOFFARBIB, 1);
+      init_zmp_com_offset_interpolator->setName("zmp_com_offset_interpolator");
+      double start_ratio = 0.0;
+      init_zmp_com_offset_interpolator->set(&start_ratio);
+      double goal_ratio = 1.0;
+      init_zmp_com_offset_interpolator->go(&goal_ratio, 3.0, true);//3s
+
       std::string home_path(std::getenv("HOME"));
       sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
       cz_log = fopen((home_path+"/HumanSync_com_zmp.log").c_str(),"w+");
@@ -228,6 +230,7 @@ class HumanSynchronizer{
     ~HumanSynchronizer(){
       fclose(sr_log);
       fclose(cz_log);
+      delete init_zmp_com_offset_interpolator;
       cout<<"HumanSynchronizer destructed"<<endl;
     }
 
@@ -251,7 +254,7 @@ class HumanSynchronizer{
       updateHumanToRobotRatio             (tgt_h2r_ratio);
       updateCOMModRatio                   (tgt_cmmr);
       removeInitOffsetPose                (hp_wld_raw, hp_wld_initpos, hp_rel_raw);
-      applyInitHumanCOMOffset             (hp_rel_raw, init_hp_calibcom, hp_rel_raw);
+      applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);
       lockFootXYOnContact                 (is_rf_contact, is_lf_contact, hp_rel_raw);//根本から改変すべき2
 
       hp_filtered = hp_rel_raw;
@@ -273,9 +276,7 @@ class HumanSynchronizer{
       applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       applyCOMZMPXYZLock                  (rp_ref_out);
 
-
       overwriteFootZFromContactStates     (is_rf_contact, is_lf_contact, rp_ref_out);
-
       rp_ref_out_old = rp_ref_out;
 //      fprintf(log,"%f %f %f %f %f %f\n", rp_ref_out.com(0), rp_ref_out.com(1), rp_ref_out.zmp(0), rp_ref_out.zmp(1), hp_rel_raw.com(0), hp_rel_raw.com(1));
       loop++;
@@ -316,8 +317,8 @@ class HumanSynchronizer{
       if(h2r_r_goal - h2r_ratio > step){h2r_ratio += step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
       else if(h2r_r_goal - h2r_ratio < (-1)*step){h2r_ratio -= step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
       else{h2r_ratio = h2r_r_goal;}
-      init_wld_hp_rfpos = hrp::Vector3(0,-0.1/h2r_ratio,0);
-      init_wld_hp_lfpos = hrp::Vector3(0, 0.1/h2r_ratio,0);
+      init_wld_hp_rfpos = init_wld_rp_rfpos/h2r_ratio;
+      init_wld_hp_lfpos = init_wld_rp_lfpos/h2r_ratio;
     }
     void updateCOMModRatio(const double cmmr_goal){
       const double step = 0.001;
@@ -332,15 +333,17 @@ class HumanSynchronizer{
       rel_out.rfw = abs_in.rfw;
       rel_out.lfw = abs_in.lfw;
     }
-    void applyInitHumanCOMOffset(const HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
+    void applyInitHumanZMPToCOMOffset(const HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
+      if(isHumanSyncOn()){
+        if(!init_zmp_com_offset_interpolator->isEmpty()){
+          init_zmp_com_offset_interpolator->get(&zmp_com_offset_ip_ratio, true);
+        }
+      }else{
+        zmp_com_offset_ip_ratio = 0;
+      }
       out = in;
-      out.com = in.com + com_offs;
+      out.com = in.com + zmp_com_offset_ip_ratio * com_offs;
     }
-//    void startHumanSync(){
-//      countdown_flag = true;
-//      setInitOffsetPose();
-//      HumanSyncOn = true;
-//    }
     void applyLPFilter(HumanPose& tgt){
       for(int i=0;i<iir_v_filters.size();i++){ tgt.Seq(i) = iir_v_filters[i].passFilter(tgt.Seq(i)); }
     }
@@ -351,16 +354,10 @@ class HumanSynchronizer{
       rp_out.rfw  = hp_in.rfw;
       rp_out.lfw  = hp_in.lfw;
     }
-    void applyCOMMoveModRatio(HumanPose& tgt){//結局初期指定からの移動量(=Rel)で計算をしてゆく
+    void applyCOMMoveModRatio(HumanPose& tgt){//
       tgt.com = cmmr * tgt.com;
     }
     void judgeFootContactStates(const HumanPose::Wrench6& rfw_in, const HumanPose::Wrench6& lfw_in, bool& rfcs_ans, bool& lfcs_ans){
-//      if      (rfcs_ans  && lfcs_ans && rfw_in.f(2)<CNT_F_TH   ){rfcs_ans = false;}//右足ついた状態から上げる
-//      else if (!rfcs_ans && lfcs_ans && rfw_in.f(2)>CNT_F_TH+30){rfcs_ans = true;}//右足浮いた状態から下げる
-//      if      (lfcs_ans  && rfcs_ans && lfw_in.f(2)<CNT_F_TH   ){lfcs_ans = false;}//左足ついた状態から上げる
-//      else if (!lfcs_ans && rfcs_ans && lfw_in.f(2)>CNT_F_TH+30){lfcs_ans = true;}//左足浮いた状態から下げる
-
-//      if(rfw_in.f(2)>CNT_F_TH+30 || lfw_in.f(2)>CNT_F_TH+30 ){
       if(HumanSyncOn){
         if      (rfcs_ans  && rfw_in.f(2)<CNT_F_TH   ){rfcs_ans = false;}//右足ついた状態から上げる
         else if (!rfcs_ans && rfw_in.f(2)>CNT_F_TH+30){rfcs_ans = true;}//右足浮いた状態から下げる
@@ -368,8 +365,6 @@ class HumanSynchronizer{
         else if (!lfcs_ans && lfw_in.f(2)>CNT_F_TH+30){lfcs_ans = true;}//左足浮いた状態から下げる
       }
       if(!rfcs_ans && !lfcs_ans){rfcs_ans = lfcs_ans = true;}//両足ジャンプは禁止
-
-
     }
     void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
       if(rfcs_ans && !lfcs_ans){
@@ -384,11 +379,7 @@ class HumanSynchronizer{
           cout<<"RF lock"<<endl;
         }
       }
-
-
     }
-
-
     void overwriteFootZFromContactStates(const bool& rfcs_in, const bool& lfcs_in, HumanPose& tgt){
       if(!rfcs_in){//右足浮遊時
         if(cur_rfup_level < FUPLEVELNUM){
@@ -444,6 +435,15 @@ class HumanSynchronizer{
       else if(is_rf_contact && !is_lf_contact){//左足浮遊時
         if(tgt.lf(1) + rp_wld_initpos.lf(1) < tgt.rf(1) + rp_wld_initpos.rf(1) + 0.15)tgt.lf(1) = tgt.rf(1) + rp_wld_initpos.rf(1) - rp_wld_initpos.lf(1) + 0.15;
       }
+      hrp::Vector3 hand_ulimit = hrp::Vector3(0.1,0.1,0.1);
+      hrp::Vector3 hand_llimit = hrp::Vector3(-0.1,-0.1,-0.1);
+      for(int i=0;i<3;i++){
+        if      (tgt.rh(i) > hand_ulimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.rh(i) = hand_ulimit(i) + current_basepos(i) - init_basepos(i); }
+        else if (tgt.rh(i) < hand_llimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.rh(i) = hand_llimit(i) + current_basepos(i) - init_basepos(i); }
+        if      (tgt.lh(i) > hand_ulimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.lh(i) = hand_ulimit(i) + current_basepos(i) - init_basepos(i); }
+        else if (tgt.lh(i) < hand_llimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.lh(i) = hand_llimit(i) + current_basepos(i) - init_basepos(i); }
+
+      }
     }
     bool applyCOMToSupportRegionLimit(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
 //      const double XUMARGIN = 0.04;//CHIDORI
@@ -537,10 +537,8 @@ class HumanSynchronizer{
 //        fprintf(cz_log," %f %f %f %f",rp_ref_out.zmp(1),hpf_zmp(1),ans_point(1),ans_point(1) + hpf_zmp(1));
 //        fprintf(cz_log,"\n\n");
 //      }
-
       comin_abs(0) = ans_point(0);
       comin_abs(1) = ans_point(1);
-
       return true;
     }
     void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
@@ -549,8 +547,8 @@ class HumanSynchronizer{
       if(comacc(0)>MAXACC)comacc(0)=MAXACC;else if(comacc(0)<-MAXACC)comacc(0)=-MAXACC;
       if(comacc(1)>MAXACC)comacc(1)=MAXACC;else if(comacc(1)<-MAXACC)comacc(1)=-MAXACC;
       comacc = comacc_v_filters.passFilter(comacc);
-      zmpout(0) = comin(0)-(1.06/G)*comacc(0);
-      zmpout(1) = comin(1)-(1.06/G)*comacc(1);
+      zmpout(0) = comin(0)-(rp_wld_initpos.com(2)/G)*comacc(0);
+      zmpout(1) = comin(1)-(rp_wld_initpos.com(2)/G)*comacc(1);
       fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.zmp(0),rp_ref_out.zmp(1),zmpout(0),zmpout(1));
       com_oldold = com_old;
       com_old = comin;

From 0ae4abbad58b9d2fb7ca45b493277368f1e393fb Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 21 Oct 2016 16:29:29 +0900
Subject: [PATCH 029/217] tune for real CHIDORI

---
 rtc/AutoBalancer/AutoBalancer.cpp |  3 +-
 rtc/AutoBalancer/AutoBalancer.h   | 73 +++++++++++++++++++------------
 2 files changed, 45 insertions(+), 31 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 3d664cc07bd..38c99d77d03 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1049,13 +1049,12 @@ void AutoBalancer::solveLimbIK ()
   }
   if(hsp->isHumanSyncOn()){
     if(hsp->ht_first_call){
-      hsp->calibInitHumanCOMFromZMP();
       std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
       hsp->ht_first_call = false;
     }
   }else{
     hsp->setInitOffsetPose();
-//    hsp->calibInitHumanCOMFromZMP();
+    hsp->calibInitHumanCOMFromZMP();
     hsp->rp_wld_initpos.com = m_robot->calcCM();
     hsp->rp_wld_initpos.rf = ikp["rleg"].target_link->p;
     hsp->rp_wld_initpos.lf = ikp["lleg"].target_link->p;
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 9f8055c31d0..6ab4276bf7e 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -35,6 +35,10 @@
 // Service Consumer stub headers
 // <rtc-template block="consumer_stub_h">
 
+//実機だとログ出力で落ちる？getenv("HOME")ダメ？
+#define DEBUG 0
+
+
 class BiquadIIRFilterVec
 {
 private:
@@ -73,8 +77,6 @@ class BiquadIIRFilterVec
 
 
 
-
-
 class HumanPose{
   private: hrp::Vector3 zero;
   public:
@@ -141,7 +143,6 @@ class HumanSynchronizer{
     double FUPHIGHT;
     double CNT_F_TH;
     hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
-    hrp::Vector3 init_wld_rp_compos;
     hrp::Vector3 init_hp_calibcom;
     double MAXVEL,MAXACC;
     HumanPose hp_wld_raw;
@@ -156,12 +157,11 @@ class HumanSynchronizer{
     unsigned int loop;
     double h2r_ratio, tgt_h2r_ratio;
     double cmmr, tgt_cmmr;
-    hrp::Vector3 lpf_zmp;
     double HZ,DT,G;
     hrp::Vector3 com_old,com_oldold,comacc;
     std::vector<BiquadIIRFilterVec> iir_v_filters;
     BiquadIIRFilterVec comacc_v_filters;
-
+    hrp::Vector3 r_zmp_raw;
   public:
 
     bool startCountdownForHumanSync;
@@ -171,7 +171,6 @@ class HumanSynchronizer{
     bool use_x,use_y,use_z;
     HumanPose rp_ref_out;
     FILE *sr_log,*cz_log;
-    std::vector<double> fb_coeffs, ff_coeffs;
     bool use_rh,use_lh;
     hrp::Vector3 init_wld_rp_rfpos,init_wld_rp_lfpos;
     hrp::Vector3 init_wld_hp_rfpos,init_wld_hp_lfpos;
@@ -195,7 +194,6 @@ class HumanSynchronizer{
       is_rf_contact = is_lf_contact = true;
       pre_cont_rfpos = pre_cont_lfpos = hrp::Vector3::Zero();
       init_hp_calibcom.Zero();
-      lpf_zmp.Zero();
       FNUM = 0.01;//0.01でも荒い?
       FUPLEVELNUM = 200;
       FUPHIGHT = 0.05;
@@ -208,13 +206,23 @@ class HumanSynchronizer{
       loop = 0;
       is_rf_in_air = is_lf_in_air = false;
 
+      com_old = com_oldold = comacc = hrp::Vector3::Zero();
+      r_zmp_raw = hrp::Vector3::Zero();
+
+      init_wld_rp_rfpos = init_wld_rp_lfpos = hrp::Vector3::Zero();
+      init_wld_hp_rfpos = init_wld_hp_lfpos = hrp::Vector3::Zero();
+      current_basepos = init_basepos = hrp::Vector3::Zero();
+
       iir_v_filters.resize(5);
-      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(0.5,HZ);
+      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(1.0,HZ);
       comacc_v_filters.setParameter(1,HZ);
 
 //      use_rh = use_lh = false;
       use_rh = use_lh = true;
 
+//      zmp_com_offset_ip_ratio = 0;
+
+
       init_zmp_com_offset_interpolator = new interpolator(1, DT, interpolator::HOFFARBIB, 1);
       init_zmp_com_offset_interpolator->setName("zmp_com_offset_interpolator");
       double start_ratio = 0.0;
@@ -223,13 +231,13 @@ class HumanSynchronizer{
       init_zmp_com_offset_interpolator->go(&goal_ratio, 3.0, true);//3s
 
       std::string home_path(std::getenv("HOME"));
-      sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
-      cz_log = fopen((home_path+"/HumanSync_com_zmp.log").c_str(),"w+");
+      if(DEBUG)sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
+      if(DEBUG)cz_log = fopen((home_path+"/HumanSync_com_zmp.log").c_str(),"w+");
       cout<<"HumanSynchronizer constructed"<<endl;
     }
     ~HumanSynchronizer(){
-      fclose(sr_log);
-      fclose(cz_log);
+      if(DEBUG)fclose(sr_log);
+      if(DEBUG)fclose(cz_log);
       delete init_zmp_com_offset_interpolator;
       cout<<"HumanSynchronizer destructed"<<endl;
     }
@@ -256,7 +264,6 @@ class HumanSynchronizer{
       removeInitOffsetPose                (hp_wld_raw, hp_wld_initpos, hp_rel_raw);
       applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);
       lockFootXYOnContact                 (is_rf_contact, is_lf_contact, hp_rel_raw);//根本から改変すべき2
-
       hp_filtered = hp_rel_raw;
 
 //      applyLPFilterToEE(hp_rel_raw, hp_filtered);//次に使う.rf,.lfのためにEEだけは先にフィルターしておかないといけない説もある
@@ -267,15 +274,16 @@ class HumanSynchronizer{
 
       applyEEWorkspaceLimit               (rp_ref_out);
 
-      applyCOMToSupportRegionLimit        (rp_ref_out.lf+init_wld_rp_lfpos,rp_ref_out.rf+init_wld_rp_rfpos,rp_ref_out.com);
+      applyCOMToSupportRegionLimit        (rp_ref_out.rf+init_wld_rp_rfpos, rp_ref_out.lf+init_wld_rp_lfpos, rp_ref_out.com);
 
       applyLPFilter                       (rp_ref_out);
+//      applyLPFilter_o                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
+      r_zmp_raw = rp_ref_out.zmp;
       applyZMPCalcFromCOM                 (rp_ref_out.com,rp_ref_out.zmp);
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, is_rf_contact, is_lf_contact);//
 
       applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       applyCOMZMPXYZLock                  (rp_ref_out);
-
       overwriteFootZFromContactStates     (is_rf_contact, is_lf_contact, rp_ref_out);
       rp_ref_out_old = rp_ref_out;
 //      fprintf(log,"%f %f %f %f %f %f\n", rp_ref_out.com(0), rp_ref_out.com(1), rp_ref_out.zmp(0), rp_ref_out.zmp(1), hp_rel_raw.com(0), hp_rel_raw.com(1));
@@ -283,8 +291,10 @@ class HumanSynchronizer{
       gettimeofday(&t_calc_end, NULL);
     }
     void calibInitHumanCOMFromZMP(){
-      init_hp_calibcom(0) = hp_filtered.zmp(0);
-      init_hp_calibcom(1) = hp_filtered.zmp(1);
+//      init_hp_calibcom(0) = hp_filtered.zmp(0);
+//      init_hp_calibcom(1) = hp_filtered.zmp(1);
+      init_hp_calibcom(0) = r_zmp_raw(0);
+      init_hp_calibcom(1) = r_zmp_raw(1);
     }
     void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const HumanPose::Wrench6& rfwin, const HumanPose::Wrench6& lfwin, hrp::Vector3& zmp_ans){
       hrp::Vector3 rfzmp,lfzmp;
@@ -347,6 +357,11 @@ class HumanSynchronizer{
     void applyLPFilter(HumanPose& tgt){
       for(int i=0;i<iir_v_filters.size();i++){ tgt.Seq(i) = iir_v_filters[i].passFilter(tgt.Seq(i)); }
     }
+    void applyLPFilter_o(const HumanPose& raw_in, const HumanPose& out_old, HumanPose& filt_out){
+      for(int i=0;i<filt_out.idsize;i++){
+        filt_out.Seq(i) = (1-FNUM) * out_old.Seq(i) + FNUM * raw_in.Seq(i);
+      }
+    }
     void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
       for(int i=0;i<rp_out.idsize-4;i++){
         rp_out.Seq(i) =  h2r_ratio * hp_in.Seq(i);
@@ -368,13 +383,13 @@ class HumanSynchronizer{
     }
     void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
       if(rfcs_ans && !lfcs_ans){
-        if(tgt.zmp(1) > tgt.rf(1) + init_wld_rp_rfpos(1) + 0.03){
+        if(tgt.zmp(1) > tgt.rf(1) + init_wld_rp_rfpos(1) + 0.02){
           lfcs_ans = true;
           cout<<"LF lock"<<endl;
         }
       }
       if(!rfcs_ans && lfcs_ans){
-        if(tgt.zmp(1) < tgt.lf(1) + init_wld_rp_lfpos(1) - 0.03){
+        if(tgt.zmp(1) < tgt.lf(1) + init_wld_rp_lfpos(1) - 0.02){
           rfcs_ans = true;
           cout<<"RF lock"<<endl;
         }
@@ -445,15 +460,15 @@ class HumanSynchronizer{
 
       }
     }
-    bool applyCOMToSupportRegionLimit(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
-//      const double XUMARGIN = 0.04;//CHIDORI
-//      const double XLMARGIN = -0.02;
-//      const double YUMARGIN = 0.01;
-//      const double YLMARGIN = -0.01;
-      const double XUMARGIN = 0.02;//JAXON
-      const double XLMARGIN = -0.01;
-      const double YUMARGIN = 0.0;
-      const double YLMARGIN = -0.0;
+    bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
+      const double XUMARGIN = 0.04;//CHIDORI
+      const double XLMARGIN = -0.02;
+      const double YUMARGIN = 0.01;
+      const double YLMARGIN = -0.01;
+//      const double XUMARGIN = 0.02;//JAXON
+//      const double XLMARGIN = -0.01;
+//      const double YUMARGIN = 0.0;
+//      const double YLMARGIN = -0.0;
 //      const double XUMARGIN = 0.10;//test
 //      const double XLMARGIN = -0.05;
 //      const double YUMARGIN = 0.03;
@@ -549,7 +564,7 @@ class HumanSynchronizer{
       comacc = comacc_v_filters.passFilter(comacc);
       zmpout(0) = comin(0)-(rp_wld_initpos.com(2)/G)*comacc(0);
       zmpout(1) = comin(1)-(rp_wld_initpos.com(2)/G)*comacc(1);
-      fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.zmp(0),rp_ref_out.zmp(1),zmpout(0),zmpout(1));
+      if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.zmp(0),rp_ref_out.zmp(1),zmpout(0),zmpout(1));
       com_oldold = com_old;
       com_old = comin;
     }

From 5a3851f0cab72c450f6e20aa2e4c0d8a5e1b3bf4 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 28 Oct 2016 12:26:07 +0900
Subject: [PATCH 030/217] fix contact state change function

---
 rtc/AutoBalancer/AutoBalancer.h | 50 +++++++++++++++++++--------------
 1 file changed, 29 insertions(+), 21 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 6ab4276bf7e..fe54ba0c0f0 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -152,7 +152,6 @@ class HumanSynchronizer{
     HumanPose rp_ref_out_old;
     int countdown_num;
     bool HumanSyncOn;
-    bool is_rf_in_air,is_lf_in_air;
     struct timeval t_calc_start, t_calc_end;
     unsigned int loop;
     double h2r_ratio, tgt_h2r_ratio;
@@ -166,7 +165,8 @@ class HumanSynchronizer{
 
     bool startCountdownForHumanSync;
     bool ht_first_call;
-    bool is_rf_contact,is_lf_contact;
+    bool is_rf_contact,is_lf_contact;//足上げ高さ0に到達しているか否か
+    bool land_on_rf,land_on_lf;//足上げ指令の有無
     HumanPose rp_wld_initpos;//IKでのリンク原点の位置(足首，手首など)
     bool use_x,use_y,use_z;
     HumanPose rp_ref_out;
@@ -192,6 +192,7 @@ class HumanSynchronizer{
       use_x = use_y = use_z = true;
       cur_rfup_level = cur_lfup_level = 0;
       is_rf_contact = is_lf_contact = true;
+      land_on_rf = land_on_lf = false;
       pre_cont_rfpos = pre_cont_lfpos = hrp::Vector3::Zero();
       init_hp_calibcom.Zero();
       FNUM = 0.01;//0.01でも荒い?
@@ -204,7 +205,6 @@ class HumanSynchronizer{
       startCountdownForHumanSync = false;
       countdown_num = 5*HZ;
       loop = 0;
-      is_rf_in_air = is_lf_in_air = false;
 
       com_old = com_oldold = comacc = hrp::Vector3::Zero();
       r_zmp_raw = hrp::Vector3::Zero();
@@ -214,8 +214,8 @@ class HumanSynchronizer{
       current_basepos = init_basepos = hrp::Vector3::Zero();
 
       iir_v_filters.resize(5);
-      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(1.0,HZ);
-      comacc_v_filters.setParameter(1,HZ);
+      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(0.5,HZ);
+      comacc_v_filters.setParameter(0.5,HZ);
 
 //      use_rh = use_lh = false;
       use_rh = use_lh = true;
@@ -230,9 +230,11 @@ class HumanSynchronizer{
       double goal_ratio = 1.0;
       init_zmp_com_offset_interpolator->go(&goal_ratio, 3.0, true);//3s
 
-      std::string home_path(std::getenv("HOME"));
-      if(DEBUG)sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
-      if(DEBUG)cz_log = fopen((home_path+"/HumanSync_com_zmp.log").c_str(),"w+");
+      if(DEBUG){
+        std::string home_path(std::getenv("HOME"));
+        sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
+        cz_log = fopen((home_path+"/HumanSync_com_zmp.log").c_str(),"w+");
+      }
       cout<<"HumanSynchronizer constructed"<<endl;
     }
     ~HumanSynchronizer(){
@@ -263,14 +265,14 @@ class HumanSynchronizer{
       updateCOMModRatio                   (tgt_cmmr);
       removeInitOffsetPose                (hp_wld_raw, hp_wld_initpos, hp_rel_raw);
       applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);
-      lockFootXYOnContact                 (is_rf_contact, is_lf_contact, hp_rel_raw);//根本から改変すべき2
+      lockFootXYOnContact                 (land_on_rf, land_on_lf, hp_rel_raw);//根本から改変すべき2
       hp_filtered = hp_rel_raw;
 
 //      applyLPFilterToEE(hp_rel_raw, hp_filtered);//次に使う.rf,.lfのためにEEだけは先にフィルターしておかないといけない説もある
       calcWorldZMP                        ((hp_filtered.rf+init_wld_hp_rfpos), (hp_filtered.lf+init_wld_hp_lfpos), hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
       convertRelHumanPoseToRelRobotPose   (hp_filtered,rp_ref_out);
       applyCOMMoveModRatio                (rp_ref_out);
-      judgeFootContactStates              (hp_wld_raw.rfw, hp_wld_raw.lfw, is_rf_contact, is_lf_contact);//frwはすでに1000->100Hzにフィルタリングされている
+      judgeFootLandOnCommand              (hp_wld_raw.rfw, hp_wld_raw.lfw, land_on_rf, land_on_lf);//frwはすでに1000->100Hzにフィルタリングされている
 
       applyEEWorkspaceLimit               (rp_ref_out);
 
@@ -280,11 +282,11 @@ class HumanSynchronizer{
 //      applyLPFilter_o                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       r_zmp_raw = rp_ref_out.zmp;
       applyZMPCalcFromCOM                 (rp_ref_out.com,rp_ref_out.zmp);
-      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, is_rf_contact, is_lf_contact);//
+      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, land_on_rf, land_on_lf);//
 
       applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       applyCOMZMPXYZLock                  (rp_ref_out);
-      overwriteFootZFromContactStates     (is_rf_contact, is_lf_contact, rp_ref_out);
+      overwriteFootZFromFootLandOnCommand (land_on_rf, land_on_lf, rp_ref_out);
       rp_ref_out_old = rp_ref_out;
 //      fprintf(log,"%f %f %f %f %f %f\n", rp_ref_out.com(0), rp_ref_out.com(1), rp_ref_out.zmp(0), rp_ref_out.zmp(1), hp_rel_raw.com(0), hp_rel_raw.com(1));
       loop++;
@@ -372,14 +374,14 @@ class HumanSynchronizer{
     void applyCOMMoveModRatio(HumanPose& tgt){//
       tgt.com = cmmr * tgt.com;
     }
-    void judgeFootContactStates(const HumanPose::Wrench6& rfw_in, const HumanPose::Wrench6& lfw_in, bool& rfcs_ans, bool& lfcs_ans){
+    void judgeFootLandOnCommand(const HumanPose::Wrench6& rfw_in, const HumanPose::Wrench6& lfw_in, bool& rfloc_ans, bool& lfloc_ans){
       if(HumanSyncOn){
-        if      (rfcs_ans  && rfw_in.f(2)<CNT_F_TH   ){rfcs_ans = false;}//右足ついた状態から上げる
-        else if (!rfcs_ans && rfw_in.f(2)>CNT_F_TH+30){rfcs_ans = true;}//右足浮いた状態から下げる
-        if      (lfcs_ans  && lfw_in.f(2)<CNT_F_TH   ){lfcs_ans = false;}//左足ついた状態から上げる
-        else if (!lfcs_ans && lfw_in.f(2)>CNT_F_TH+30){lfcs_ans = true;}//左足浮いた状態から下げる
+        if      (rfloc_ans  && rfw_in.f(2)<CNT_F_TH   ){rfloc_ans = false;}//右足ついた状態から上げる
+        else if (!rfloc_ans && rfw_in.f(2)>CNT_F_TH+30){rfloc_ans = true;}//右足浮いた状態から下げる
+        if      (lfloc_ans  && lfw_in.f(2)<CNT_F_TH   ){lfloc_ans = false;}//左足ついた状態から上げる
+        else if (!lfloc_ans && lfw_in.f(2)>CNT_F_TH+30){lfloc_ans = true;}//左足浮いた状態から下げる
       }
-      if(!rfcs_ans && !lfcs_ans){rfcs_ans = lfcs_ans = true;}//両足ジャンプは禁止
+      if(!rfloc_ans && !lfloc_ans){rfloc_ans = lfloc_ans = true;}//両足ジャンプは禁止
     }
     void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
       if(rfcs_ans && !lfcs_ans){
@@ -395,35 +397,41 @@ class HumanSynchronizer{
         }
       }
     }
-    void overwriteFootZFromContactStates(const bool& rfcs_in, const bool& lfcs_in, HumanPose& tgt){
-      if(!rfcs_in){//右足浮遊時
+    void overwriteFootZFromFootLandOnCommand(const bool& rfloc_in, const bool& lfloc_in, HumanPose& tgt){
+      if(!rfloc_in){//右足浮遊時
         if(cur_rfup_level < FUPLEVELNUM){
           cur_rfup_level++;
           tgt.rf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
         }else{
           tgt.rf(2) = FUPHIGHT;
         }
+        is_rf_contact = false;
       }else{
         if(cur_rfup_level > 0){
           cur_rfup_level--;
           tgt.rf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
+          is_rf_contact = false;
         }else{
           tgt.rf(2) = 0;
+          is_rf_contact = true;
         }
       }
-      if(!lfcs_in){//左足浮遊時
+      if(!lfloc_in){//左足浮遊時
         if(cur_lfup_level < FUPLEVELNUM){
           cur_lfup_level++;
           tgt.lf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
         }else{
           tgt.lf(2) = FUPHIGHT;
         }
+        is_lf_contact = false;
       }else{
         if(cur_lfup_level > 0){
           cur_lfup_level--;
           tgt.lf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
+          is_lf_contact = false;
         }else{
           tgt.lf(2) = 0;
+          is_lf_contact = true;
         }
       }
     }

From 7ab9111df69617bcbb0dcabd35552667ef0027d1 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 1 Nov 2016 18:37:01 +0900
Subject: [PATCH 031/217] sum up contact control

---
 rtc/AutoBalancer/AutoBalancer.h | 74 +++++++++++----------------------
 1 file changed, 24 insertions(+), 50 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index fe54ba0c0f0..a8d8cef01f9 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -136,11 +136,9 @@ class HumanPose{
 
 class HumanSynchronizer{
   private:
-
-    double FNUM;
-    int FUPLEVELNUM;
+    double FUP_TIME;
     int cur_rfup_level,cur_lfup_level;
-    double FUPHIGHT;
+    double FUP_HIGHT;
     double CNT_F_TH;
     hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
     hrp::Vector3 init_hp_calibcom;
@@ -161,12 +159,12 @@ class HumanSynchronizer{
     std::vector<BiquadIIRFilterVec> iir_v_filters;
     BiquadIIRFilterVec comacc_v_filters;
     hrp::Vector3 r_zmp_raw;
-  public:
 
+  public:
     bool startCountdownForHumanSync;
     bool ht_first_call;
     bool is_rf_contact,is_lf_contact;//足上げ高さ0に到達しているか否か
-    bool land_on_rf,land_on_lf;//足上げ指令の有無
+    bool go_rf_landing,go_lf_landing;//足上げ指令の有無
     HumanPose rp_wld_initpos;//IKでのリンク原点の位置(足首，手首など)
     bool use_x,use_y,use_z;
     HumanPose rp_ref_out;
@@ -192,12 +190,11 @@ class HumanSynchronizer{
       use_x = use_y = use_z = true;
       cur_rfup_level = cur_lfup_level = 0;
       is_rf_contact = is_lf_contact = true;
-      land_on_rf = land_on_lf = false;
+      go_rf_landing = go_lf_landing = false;
       pre_cont_rfpos = pre_cont_lfpos = hrp::Vector3::Zero();
       init_hp_calibcom.Zero();
-      FNUM = 0.01;//0.01でも荒い?
-      FUPLEVELNUM = 200;
-      FUPHIGHT = 0.05;
+      FUP_TIME = 0.4;
+      FUP_HIGHT = 0.05;
       CNT_F_TH = 20.0;
       MAXVEL = 0.004;
       HumanSyncOn = false;
@@ -265,14 +262,14 @@ class HumanSynchronizer{
       updateCOMModRatio                   (tgt_cmmr);
       removeInitOffsetPose                (hp_wld_raw, hp_wld_initpos, hp_rel_raw);
       applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);
-      lockFootXYOnContact                 (land_on_rf, land_on_lf, hp_rel_raw);//根本から改変すべき2
+      lockFootXYOnContact                 (go_rf_landing, go_lf_landing, hp_rel_raw);//根本から改変すべき2
       hp_filtered = hp_rel_raw;
 
 //      applyLPFilterToEE(hp_rel_raw, hp_filtered);//次に使う.rf,.lfのためにEEだけは先にフィルターしておかないといけない説もある
       calcWorldZMP                        ((hp_filtered.rf+init_wld_hp_rfpos), (hp_filtered.lf+init_wld_hp_lfpos), hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
       convertRelHumanPoseToRelRobotPose   (hp_filtered,rp_ref_out);
       applyCOMMoveModRatio                (rp_ref_out);
-      judgeFootLandOnCommand              (hp_wld_raw.rfw, hp_wld_raw.lfw, land_on_rf, land_on_lf);//frwはすでに1000->100Hzにフィルタリングされている
+      judgeFootLandOnCommand              (hp_wld_raw.rfw, hp_wld_raw.lfw, go_rf_landing, go_lf_landing);//frwはすでに1000->100Hzにフィルタリングされている
 
       applyEEWorkspaceLimit               (rp_ref_out);
 
@@ -282,11 +279,11 @@ class HumanSynchronizer{
 //      applyLPFilter_o                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       r_zmp_raw = rp_ref_out.zmp;
       applyZMPCalcFromCOM                 (rp_ref_out.com,rp_ref_out.zmp);
-      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, land_on_rf, land_on_lf);//
+      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, go_rf_landing, go_lf_landing);//
 
       applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       applyCOMZMPXYZLock                  (rp_ref_out);
-      overwriteFootZFromFootLandOnCommand (land_on_rf, land_on_lf, rp_ref_out);
+      overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
       rp_ref_out_old = rp_ref_out;
 //      fprintf(log,"%f %f %f %f %f %f\n", rp_ref_out.com(0), rp_ref_out.com(1), rp_ref_out.zmp(0), rp_ref_out.zmp(1), hp_rel_raw.com(0), hp_rel_raw.com(1));
       loop++;
@@ -360,6 +357,7 @@ class HumanSynchronizer{
       for(int i=0;i<iir_v_filters.size();i++){ tgt.Seq(i) = iir_v_filters[i].passFilter(tgt.Seq(i)); }
     }
     void applyLPFilter_o(const HumanPose& raw_in, const HumanPose& out_old, HumanPose& filt_out){
+      double FNUM = 0.01;//0.01でも荒い?
       for(int i=0;i<filt_out.idsize;i++){
         filt_out.Seq(i) = (1-FNUM) * out_old.Seq(i) + FNUM * raw_in.Seq(i);
       }
@@ -397,43 +395,19 @@ class HumanSynchronizer{
         }
       }
     }
-    void overwriteFootZFromFootLandOnCommand(const bool& rfloc_in, const bool& lfloc_in, HumanPose& tgt){
-      if(!rfloc_in){//右足浮遊時
-        if(cur_rfup_level < FUPLEVELNUM){
-          cur_rfup_level++;
-          tgt.rf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
-        }else{
-          tgt.rf(2) = FUPHIGHT;
-        }
-        is_rf_contact = false;
-      }else{
-        if(cur_rfup_level > 0){
-          cur_rfup_level--;
-          tgt.rf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_rfup_level/FUPLEVELNUM));
-          is_rf_contact = false;
-        }else{
-          tgt.rf(2) = 0;
-          is_rf_contact = true;
-        }
-      }
-      if(!lfloc_in){//左足浮遊時
-        if(cur_lfup_level < FUPLEVELNUM){
-          cur_lfup_level++;
-          tgt.lf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
-        }else{
-          tgt.lf(2) = FUPHIGHT;
-        }
-        is_lf_contact = false;
-      }else{
-        if(cur_lfup_level > 0){
-          cur_lfup_level--;
-          tgt.lf(2) = FUPHIGHT/2*(1-cos(M_PI*cur_lfup_level/FUPLEVELNUM));
-          is_lf_contact = false;
-        }else{
-          tgt.lf(2) = 0;
-          is_lf_contact = true;
-        }
+    void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, int& cur_fup_count_in, hrp::Vector3& f_height_out, bool& is_f_contact_out){
+      int FUP_COUNT = (int)(HZ*0.4);
+      if(!go_land_in){//足下げ命令入力時
+        if(cur_fup_count_in < FUP_COUNT){ cur_fup_count_in++; }
+      }else{//足上げ命令入力時
+        if(cur_fup_count_in > 0){ cur_fup_count_in--; }
       }
+      f_height_out(2) = FUP_HIGHT/2*(1-cos(M_PI*cur_fup_count_in/FUP_COUNT));
+      is_f_contact_out = (cur_fup_count_in <= 0);
+    }
+    void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
+      calcFootUpCurveAndJudgeFootContact(rf_goland_in, cur_rfup_level, tgt.rf, is_rf_contact);
+      calcFootUpCurveAndJudgeFootContact(lf_goland_in, cur_lfup_level, tgt.lf, is_lf_contact);
     }
     void lockFootXYOnContact(const bool& rfcs_in, const bool& lfcs_in, HumanPose& tgt){
       if(rfcs_in){

From 7930498ed76b5fa0f5b662f0c634aba1fa3d9a42 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 1 Nov 2016 21:44:59 +0900
Subject: [PATCH 032/217] fix tmp_foot_mid_pos

---
 rtc/AutoBalancer/AutoBalancer.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 35031fcfa61..f246fd5e613 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1145,8 +1145,8 @@ void AutoBalancer::solveLimbIK ()
 	  }
 
     //outport用のデータ上書き
-    ref_zmp = hsp->rp_ref_out.zmp;
-    ref_cog = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;
+    ref_zmp = hsp->rp_ref_out.zmp + hsp->rp_wld_initpos.com;//hsp->rp_wld_initpos.comはtmp_foot_mid_posと同じ
+    ref_cog = hsp->rp_ref_out.com + hsp->rp_wld_initpos.com;
 
 	  if(loop%100==0)if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
 

From 0b20426b6cbea572152cbe78897f52a04bf346eb Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 2 Nov 2016 14:32:59 +0900
Subject: [PATCH 033/217] add FootUpTime method

---
 idl/AutoBalancerService.idl                   |    1 +
 rtc/AutoBalancer/AutoBalancer.cpp             |   17 +-
 rtc/AutoBalancer/AutoBalancer.cpp.save        | 2219 -----------------
 rtc/AutoBalancer/AutoBalancer.h               |    8 +-
 rtc/AutoBalancer/AutoBalancerService_impl.cpp |    5 +
 rtc/AutoBalancer/AutoBalancerService_impl.h   |    1 +
 6 files changed, 22 insertions(+), 2229 deletions(-)
 delete mode 100644 rtc/AutoBalancer/AutoBalancer.cpp.save

diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl
index ea076ba89a3..9c613ad7439 100644
--- a/idl/AutoBalancerService.idl
+++ b/idl/AutoBalancerService.idl
@@ -447,5 +447,6 @@ module OpenHRP
     boolean setCOMMoveModRatio(in double cmmr);
     boolean setAllowedXYZSync(in boolean x_on, in boolean y_on, in boolean z_on);
     boolean stopHumanSync();
+    boolean setFootUpTime(in double fupt);
   };
 };
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index f246fd5e613..86561afe35c 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1095,15 +1095,13 @@ void AutoBalancer::solveLimbIK ()
     hsp->rp_wld_initpos.com = m_robot->calcCM();
     hsp->rp_wld_initpos.rf = ikp["rleg"].target_link->p;
     hsp->rp_wld_initpos.lf = ikp["lleg"].target_link->p;
+    hsp->rp_wld_initpos.zmp = ref_zmp;
     if(ikp.count("rarm"))hsp->rp_wld_initpos.rh = ikp["rarm"].target_link->p;
     if(ikp.count("larm"))hsp->rp_wld_initpos.lh = ikp["larm"].target_link->p;
     hsp->init_basepos = m_robot->rootLink()->p;
   }
   hsp->update();//////HumanSynchronizerの主要処理
   if(loop%100==0)hsp->rp_ref_out.print();
-//  if(loop%100==0)cerr<<"getUpdateTime:"<<hsp->getUpdateTime()*1000<<"[ms]"<<endl;
-
-
   //for HumanSynchronizer
   if(hsp->isHumanSyncOn()){	  ////////////////////// 重心拘束位置設定 /////////////////////////
     m_robot->rootLink()->p = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;//まずは重心目標分，体幹を動かす
@@ -1145,15 +1143,11 @@ void AutoBalancer::solveLimbIK ()
 	  }
 
     //outport用のデータ上書き
-    ref_zmp = hsp->rp_ref_out.zmp + hsp->rp_wld_initpos.com;//hsp->rp_wld_initpos.comはtmp_foot_mid_posと同じ
+    ref_zmp = hsp->rp_ref_out.zmp + hsp->rp_wld_initpos.zmp;
     ref_cog = hsp->rp_ref_out.com + hsp->rp_wld_initpos.com;
 
 	  if(loop%100==0)if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
 
-
-//	  printf("root:%f, calcCM:%f, refCM:%f\n",m_robot->rootLink()->p(1),m_robot->calcCM()(1),hsp->rp_ref_out.com(1) + hsp->rp_wld_initpos.com(1));
-
-
   }else{
 	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
 	    if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
@@ -1218,6 +1212,12 @@ bool AutoBalancer::setCOMMoveModRatio(const double cmmr)
   return true;
 }
 
+bool AutoBalancer::setFootUpTime(const double fupt)
+{
+  std::cerr << "[" << m_profile.instance_name << "] set Foot Up Time as"<< fupt <<" [s]"<< std::endl;
+  if(fupt > 0.05 && fupt < 5){ hsp->tgt_FUP_TIME = fupt; return true; }else{ return false; }//おかしな値は弾く
+}
+
 bool AutoBalancer::setAllowedXYZSync(const bool x_on,const bool y_on,const bool z_on)
 {
   if(!hsp->isHumanSyncOn()){
@@ -1230,6 +1230,7 @@ bool AutoBalancer::setAllowedXYZSync(const bool x_on,const bool y_on,const bool
   }
 }
 
+
 bool AutoBalancer::stopHumanSync()
 {
 	std::cerr << "[" << m_profile.instance_name << "] stop HumanSync Now" << std::endl;
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp.save b/rtc/AutoBalancer/AutoBalancer.cpp.save
deleted file mode 100644
index b69bd089db6..00000000000
--- a/rtc/AutoBalancer/AutoBalancer.cpp.save
+++ /dev/null
@@ -1,2219 +0,0 @@
-// -*- C++ -*-
-/*!
- * @file  AutoBalancer.cpp
- * @brief autobalancer component
- * $Date$
- *
- * $Id$
- */
-
-#include <rtm/CorbaNaming.h>
-#include <hrpModel/Link.h>
-#include <hrpModel/Sensor.h>
-#include <hrpModel/ModelLoaderUtil.h>
-#include "AutoBalancer.h"
-#include <hrpModel/JointPath.h>
-#include <hrpUtil/MatrixSolvers.h>
-#include "hrpsys/util/Hrpsys.h"
-
-
-typedef coil::Guard<coil::Mutex> Guard;
-using namespace rats;
-
-// Module specification
-// <rtc-template block="module_spec">
-static const char* autobalancer_spec[] =
-    {
-        "implementation_id", "AutoBalancer",
-        "type_name",         "AutoBalancer",
-        "description",       "autobalancer component",
-        "version",           HRPSYS_PACKAGE_VERSION,
-        "vendor",            "AIST",
-        "category",          "example",
-        "activity_type",     "DataFlowComponent",
-        "max_instance",      "10",
-        "language",          "C++",
-        "lang_type",         "compile",
-        // Configuration variables
-        "conf.default.debugLevel", "0",
-        ""
-    };
-// </rtc-template>
-
-AutoBalancer::AutoBalancer(RTC::Manager* manager)
-    : RTC::DataFlowComponentBase(manager),
-      // <rtc-template block="initializer">
-      m_qRefIn("qRef", m_qRef),
-      m_basePosIn("basePosIn", m_basePos),
-      m_baseRpyIn("baseRpyIn", m_baseRpy),
-      m_zmpIn("zmpIn", m_zmp),
-      m_optionalDataIn("optionalData", m_optionalData),
-      m_emergencySignalIn("emergencySignal", m_emergencySignal),
-      //ishiguro
-      m_htzmpIn("htzmpIn", m_htzmp),
-      m_htrfwIn("htrfwIn", m_htrfw),
-      m_htlfwIn("htlfwIn", m_htlfw),
-      m_htcomIn("htcomIn", m_htcom),
-      m_htrfIn("htrfIn", m_htrf),
-      m_htlfIn("htlfIn", m_htlf),
-      m_htrhIn("htrhIn", m_htrh),
-      m_htlhIn("htlhIn", m_htlh),
-      m_actzmpIn("actzmpIn", m_actzmp),
-
-      m_qOut("q", m_qRef),
-      m_zmpOut("zmpOut", m_zmp),
-      m_basePosOut("basePosOut", m_basePos),
-      m_baseRpyOut("baseRpyOut", m_baseRpy),
-      m_baseTformOut("baseTformOut", m_baseTform),
-      m_basePoseOut("basePoseOut", m_basePose),
-      m_accRefOut("accRef", m_accRef),
-      m_contactStatesOut("contactStates", m_contactStates),
-      m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime),
-      m_cogOut("cogOut", m_cog),
-      m_AutoBalancerServicePort("AutoBalancerService"),
-      m_walkingStatesOut("walkingStates", m_walkingStates),
-      m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
-      m_legMarginIn("legMargin", m_legMargin),
-      // </rtc-template>
-      gait_type(BIPED),
-      move_base_gain(0.8),
-      m_robot(hrp::BodyPtr()),
-      m_debugLevel(0)
-{
-    m_service0.autobalancer(this);
-}
-
-AutoBalancer::~AutoBalancer()
-{
-}
-
-
-RTC::ReturnCode_t AutoBalancer::onInitialize()
-{
-    std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
-    bindParameter("debugLevel", m_debugLevel, "0");
-
-    // Registration: InPort/OutPort/Service
-    // <rtc-template block="registration">
-    // Set InPort buffers
-    addInPort("qRef", m_qRefIn);
-    addInPort("basePosIn", m_basePosIn);
-    addInPort("baseRpyIn", m_baseRpyIn);
-    addInPort("zmpIn", m_zmpIn);
-    addInPort("optionalData", m_optionalDataIn);
-    addInPort("emergencySignal", m_emergencySignalIn);
-    //ishiguro
-    addInPort("htzmpIn", m_htzmpIn);
-    addInPort("htrfwIn", m_htrfwIn);
-    addInPort("htlfwIn", m_htlfwIn);
-    addInPort("htcomIn", m_htcomIn);
-    addInPort("htrfIn", m_htrfIn);
-    addInPort("htlfIn", m_htlfIn);
-    addInPort("htrhIn", m_htrhIn);
-    addInPort("htlhIn", m_htlhIn);
-    addInPort("actzmpIn", m_actzmpIn);
-
-
-    addInPort("legMargin", m_legMarginIn);
-
-    // Set OutPort buffer
-    addOutPort("q", m_qOut);
-    addOutPort("zmpOut", m_zmpOut);
-    addOutPort("basePosOut", m_basePosOut);
-    addOutPort("baseRpyOut", m_baseRpyOut);
-    addOutPort("baseTformOut", m_baseTformOut);
-    addOutPort("basePoseOut", m_basePoseOut);
-    addOutPort("accRef", m_accRefOut);
-    addOutPort("contactStates", m_contactStatesOut);
-    addOutPort("controlSwingSupportTime", m_controlSwingSupportTimeOut);
-    addOutPort("cogOut", m_cogOut);
-    addOutPort("walkingStates", m_walkingStatesOut);
-    addOutPort("sbpCogOffset", m_sbpCogOffsetOut);
-  
-    // Set service provider to Ports
-    m_AutoBalancerServicePort.registerProvider("service0", "AutoBalancerService", m_service0);
-  
-    // Set service consumers to Ports
-  
-    // Set CORBA Service Ports
-    addPort(m_AutoBalancerServicePort);
-  
-    // </rtc-template>
-    // <rtc-template block="bind_config">
-    // Bind variables and configuration variable
-  
-    // </rtc-template>
-
-    RTC::Properties& prop =  getProperties();
-    coil::stringTo(m_dt, prop["dt"].c_str());
-
-    m_robot = hrp::BodyPtr(new hrp::Body());
-    RTC::Manager& rtcManager = RTC::Manager::instance();
-    std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
-    int comPos = nameServer.find(",");
-    if (comPos < 0){
-        comPos = nameServer.length();
-    }
-    nameServer = nameServer.substr(0, comPos);
-    RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
-    if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
-                                 CosNaming::NamingContext::_duplicate(naming.getRootContext())
-                                 )){
-      std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
-      return RTC::RTC_ERROR;
-    }
-
-    // allocate memory for outPorts
-    m_qRef.data.length(m_robot->numJoints());
-    qorg.resize(m_robot->numJoints());
-    qrefv.resize(m_robot->numJoints());
-    m_baseTform.data.length(12);
-    //ishiguro
-    m_htrfw.data.length(6);
-    m_htlfw.data.length(6);
-
-    control_mode = MODE_IDLE;
-    loop = 0;
-
-    // setting from conf file
-    // GaitGenerator requires abc_leg_offset and abc_stride_parameter in robot conf file
-    // setting leg_pos from conf file
-    coil::vstring leg_offset_str = coil::split(prop["abc_leg_offset"], ",");
-    leg_names.push_back("rleg");
-    leg_names.push_back("lleg");
-
-    // setting from conf file
-    // rleg,TARGET_LINK,BASE_LINK
-    coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
-    size_t prop_num = 10;
-    if (end_effectors_str.size() > 0) {
-      size_t num = end_effectors_str.size()/prop_num;
-      for (size_t i = 0; i < num; i++) {
-        std::string ee_name, ee_target, ee_base;
-        coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
-        coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
-        coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
-        ABCIKparam tp;
-        for (size_t j = 0; j < 3; j++) {
-            coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
-        }
-        double tmpv[4];
-        for (int j = 0; j < 4; j++ ) {
-          coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
-        }
-        tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
-        tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
-        // Fix for toe joint
-        //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
-        //   Without toe joints, "end-effector link == force-sensor link" is assumed.
-        //   With toe joints, "end-effector link != force-sensor link" is assumed.
-        if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
-            std::vector<double> optw(tp.manip->numJoints(), 1.0);
-            optw.back() = 0.0; // Set weight = 0 for toe joint by default
-            tp.manip->setOptionalWeightVector(optw);
-            tp.has_toe_joint = true;
-        } else {
-            tp.has_toe_joint = false;
-        }
-        tp.avoid_gain = 0.001;
-        tp.reference_gain = 0.01;
-        tp.pos_ik_error_count = tp.rot_ik_error_count = 0;
-        ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
-        ikp[ee_name].target_link = m_robot->link(ee_target);
-        ee_vec.push_back(ee_name);
-        std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   target = " << ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
-        contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
-      }
-      m_contactStates.data.length(num);
-      if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
-        m_contactStates.data[contact_states_index_map["rleg"]] = true;
-        m_contactStates.data[contact_states_index_map["lleg"]] = true;
-      }
-      if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) {
-        m_contactStates.data[contact_states_index_map["rarm"]] = false;
-        m_contactStates.data[contact_states_index_map["larm"]] = false;
-      }
-      m_controlSwingSupportTime.data.length(num);
-      for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 0.0;
-    }
-    std::vector<hrp::Vector3> leg_pos;
-    if (leg_offset_str.size() > 0) {
-      hrp::Vector3 leg_offset;
-      for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
-      std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-      leg_pos.push_back(hrp::Vector3(-1*leg_offset));
-      leg_pos.push_back(hrp::Vector3(leg_offset));
-    }
-    if (leg_pos.size() < ikp.size()) {
-        size_t tmp_leg_pos_size = leg_pos.size();
-        for (size_t i = 0; i < ikp.size() - tmp_leg_pos_size; i++) {
-            leg_pos.push_back(hrp::Vector3::Zero());
-        }
-    }
-
-    std::vector<std::pair<hrp::Link*, hrp::Link*> > interlocking_joints;
-    readInterlockingJointsParamFromProperties(interlocking_joints, m_robot, prop["interlocking_joints"], std::string(m_profile.instance_name));
-    if (interlocking_joints.size() > 0) {
-        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            std::cerr << "[" << m_profile.instance_name << "] Interlocking Joints for [" << it->first << "]" << std::endl;
-            it->second.manip->setInterlockingJointPairIndices(interlocking_joints, std::string(m_profile.instance_name));
-        }
-    }
-
-    zmp_offset_interpolator = new interpolator(ikp.size()*3, m_dt);
-    zmp_offset_interpolator->setName(std::string(m_profile.instance_name)+" zmp_offset_interpolator");
-    zmp_transition_time = 1.0;
-    transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-    transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator");
-    transition_interpolator_ratio = 1.0;
-    adjust_footstep_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-    adjust_footstep_interpolator->setName(std::string(m_profile.instance_name)+" adjust_footstep_interpolator");
-    transition_time = 2.0;
-    adjust_footstep_transition_time = 2.0;
-    leg_names_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-    leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator");
-    leg_names_interpolator_ratio = 1.0;
-
-    // setting stride limitations from conf file
-    double stride_fwd_x_limit = 0.15;
-    double stride_y_limit = 0.05;
-    double stride_th_limit = 10;
-    double stride_bwd_x_limit = 0.05;
-    std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_y_limit << "[m], " << stride_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl;
-    if (default_zmp_offsets.size() == 0) {
-      for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
-    }
-    if (leg_offset_str.size() > 0) {
-      gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_y_limit/*[m]*/, stride_th_limit/*[deg]*/, stride_bwd_x_limit/*[m]*/));
-      gg->set_default_zmp_offsets(default_zmp_offsets);
-    }
-    gg_is_walking = gg_solved = false;
-    m_walkingStates.data = false;
-    fix_leg_coords = coordinates();
-
-    // load virtual force sensors
-    readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
-    // ref force port
-    int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
-    int nvforce = m_vfs.size();
-    int nforce  = npforce + nvforce;
-    m_ref_force.resize(nforce);
-    m_ref_forceIn.resize(nforce);
-    m_force.resize(nforce);
-    m_ref_forceOut.resize(nforce);
-    m_limbCOPOffset.resize(nforce);
-    m_limbCOPOffsetOut.resize(nforce);
-    for (unsigned int i=0; i<npforce; i++){
-        sensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
-    }
-    for (unsigned int i=0; i<nvforce; i++){
-        for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
-            if (it->second.id == i) sensor_names.push_back(it->first);
-        }
-    }
-    // set ref force port
-    std::cerr << "[" << m_profile.instance_name << "] force sensor ports (" << nforce << ")" << std::endl;
-    for (unsigned int i=0; i<nforce; i++){
-        m_ref_forceIn[i] = new InPort<TimedDoubleSeq>(std::string("ref_"+sensor_names[i]).c_str(), m_ref_force[i]);
-        m_ref_force[i].data.length(6);
-        registerInPort(std::string("ref_"+sensor_names[i]).c_str(), *m_ref_forceIn[i]);
-        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string("ref_"+sensor_names[i]) << std::endl;
-        ref_forces.push_back(hrp::Vector3(0,0,0));
-    }
-    // set force port
-    for (unsigned int i=0; i<nforce; i++){
-        m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string(sensor_names[i]).c_str(), m_force[i]);
-        m_force[i].data.length(6);
-        m_force[i].data[0] = m_force[i].data[1] = m_force[i].data[2] = 0.0;
-        m_force[i].data[3] = m_force[i].data[4] = m_force[i].data[5] = 0.0;
-        registerOutPort(std::string(sensor_names[i]).c_str(), *m_ref_forceOut[i]);
-        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string(sensor_names[i]) << std::endl;
-    }
-    // set limb cop offset port
-    std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << nforce << ")" << std::endl;
-    for (unsigned int i=0; i<nforce; i++){
-        std::string nm("limbCOPOffset_"+sensor_names[i]);
-        m_limbCOPOffsetOut[i] = new OutPort<TimedPoint3D>(nm.c_str(), m_limbCOPOffset[i]);
-        registerOutPort(nm.c_str(), *m_limbCOPOffsetOut[i]);
-        m_limbCOPOffset[i].data.x = m_limbCOPOffset[i].data.y = m_limbCOPOffset[i].data.z = 0.0;
-        std::cerr << "[" << m_profile.instance_name << "]   name = " << nm << std::endl;
-    }
-    sbp_offset = hrp::Vector3(0,0,0);
-    sbp_cog_offset = hrp::Vector3(0,0,0);
-    //use_force = MODE_NO_FORCE;
-    use_force = MODE_REF_FORCE;
-
-    if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
-      is_legged_robot = true;
-    } else {
-      is_legged_robot = false;
-    }
-
-    m_accRef.data.ax = m_accRef.data.ay = m_accRef.data.az = 0.0;
-    prev_imu_sensor_vel = hrp::Vector3::Zero();
-
-    graspless_manip_mode = false;
-    graspless_manip_arm = "arms";
-    graspless_manip_p_gain = hrp::Vector3::Zero();
-
-    is_stop_mode = false;
-    has_ik_failed = false;
-    is_hand_fix_mode = false;
-
-    pos_ik_thre = 0.1*1e-3; // [m]
-    rot_ik_thre = (1e-2)*M_PI/180.0; // [rad]
-    ik_error_debug_print_freq = static_cast<int>(0.2/m_dt); // once per 0.2 [s]
-
-    //for HumanSynchronizer
-    
-hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer());
-
-    return RTC::RTC_OK;
-}
-
-
-RTC::ReturnCode_t AutoBalancer::onFinalize()
-{
-  delete zmp_offset_interpolator;
-  delete transition_interpolator;
-  delete adjust_footstep_interpolator;
-  delete leg_names_interpolator;
-  return RTC::RTC_OK;
-}
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onStartup(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onShutdown(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id)
-{
-    std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
-    
-    return RTC::RTC_OK;
-}
-
-RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
-{
-  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
-  Guard guard(m_mutex);
-  if (control_mode == MODE_ABC) {
-    control_mode = MODE_SYNC_TO_IDLE;
-    double tmp_ratio = 0.0;
-    transition_interpolator->go(&tmp_ratio, m_dt, true); // sync in one controller loop
-  }
-  return RTC::RTC_OK;
-}
-
-
-
-
-#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
-//#define DEBUGP2 ((loop%200==0))
-#define DEBUGP2 (false)
-RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
-{
-  // std::cerr << "AutoBalancer::onExecute(" << ec_id << ")" << std::endl;
-    loop ++;
-    if (m_qRefIn.isNew()) {
-        m_qRefIn.read();
-    }
-    if (m_basePosIn.isNew()) {
-      m_basePosIn.read();
-      input_basePos(0) = m_basePos.data.x;
-      input_basePos(1) = m_basePos.data.y;
-      input_basePos(2) = m_basePos.data.z;
-    }
-    if (m_baseRpyIn.isNew()) {
-      m_baseRpyIn.read();
-      input_baseRot = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
-    }
-    if (m_zmpIn.isNew()) {
-      m_zmpIn.read();
-      input_zmp(0) = m_zmp.data.x;
-      input_zmp(1) = m_zmp.data.y;
-      input_zmp(2) = m_zmp.data.z;
-    }
-    for (unsigned int i=0; i<m_ref_forceIn.size(); i++){
-        if ( m_ref_forceIn[i]->isNew() ) {
-            m_ref_forceIn[i]->read();
-        }
-    }
-    if (m_optionalDataIn.isNew()) {
-        m_optionalDataIn.read();
-        if (is_legged_robot) {
-          if (m_optionalData.data.length() >= contact_states_index_map.size()*2) {
-            // current optionalData is contactstates x limb and controlSwingSupportTime x limb
-            //   If contactStates in optionalData is 1.0, m_contactStates is true. Otherwise, false.
-            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-                m_contactStates.data[contact_states_index_map[it->first]] = isOptionalDataContact(it->first);
-                m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = m_optionalData.data[contact_states_index_map[it->first]+contact_states_index_map.size()];
-            }
-            if ( !m_contactStates.data[contact_states_index_map["rleg"]] && !m_contactStates.data[contact_states_index_map["lleg"]] ) { // If two feet have no contact, force set double support contact
-              m_contactStates.data[contact_states_index_map["rleg"]] = true;
-              m_contactStates.data[contact_states_index_map["lleg"]] = true;
-            }
-          }
-        }
-    }
-    if (m_emergencySignalIn.isNew()){
-        m_emergencySignalIn.read();
-        // if (!is_stop_mode) {
-        //     std::cerr << "[" << m_profile.instance_name << "] emergencySignal is set!" << std::endl;
-        //     is_stop_mode = true;
-        //     gg->emergency_stop();
-        // }
-    }
-    //for HumanSynchronizer
-    if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
-    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); hp_raw_data.rfw = HumanSynchronizer::DoubleSeqToWrench6(m_htrfw); }
-    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); hp_raw_data.lfw = HumanSynchronizer::DoubleSeqToWrench6(m_htlfw); }
-    if (m_htcomIn.isNew()){ m_htcomIn.read(); hp_raw_data.com = HumanSynchronizer::Point3DToVector3(m_htcom); }
-    if (m_htrfIn.isNew()) { m_htrfIn.read();  hp_raw_data.rf  = HumanSynchronizer::Point3DToVector3(m_htrf); }
-    if (m_htlfIn.isNew()) { m_htlfIn.read();  hp_raw_data.lf  = HumanSynchronizer::Point3DToVector3(m_htlf); }
-    if (m_htrhIn.isNew()) { m_htrhIn.read();  hp_raw_data.rh  = HumanSynchronizer::Point3DToVector3(m_htrh); }
-    if (m_htlhIn.isNew()) { m_htlhIn.read();  hp_raw_data.lh  = HumanSynchronizer::Point3DToVector3(m_htlh); }
-    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
-    hsp->readInput(hp_raw_data);
-
-
-    if (m_legMarginIn.isNew()) {
-      m_legMarginIn.read();
-      for (size_t i = 0; i < 4; i++) {
-        gg->set_leg_margin(m_legMargin.data[i], i);
-      }
-    }
-
-    Guard guard(m_mutex);
-    hrp::Vector3 ref_basePos;
-    hrp::Matrix33 ref_baseRot;
-    hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
-    if ( is_legged_robot ) {
-      gg->proc_zmp_weight_map_interpolation();
-      getCurrentParameters();
-      getTargetParameters();
-      bool is_transition_interpolator_empty = transition_interpolator->isEmpty();
-      if (!is_transition_interpolator_empty) {
-        transition_interpolator->get(&transition_interpolator_ratio, true);
-      } else {
-        transition_interpolator_ratio = 1.0;
-      }
-      if (control_mode != MODE_IDLE ) {
-        solveLimbIK();
-        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
-      } else {
-        rel_ref_zmp = input_zmp;
-      }
-      // transition
-      if (!is_transition_interpolator_empty) {
-        // transition_interpolator_ratio 0=>1 : IDLE => ABC
-        // transition_interpolator_ratio 1=>0 : ABC => IDLE
-        ref_basePos = (1-transition_interpolator_ratio) * input_basePos + transition_interpolator_ratio * m_robot->rootLink()->p;
-        rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp;
-        rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R);
-        for ( int i = 0; i < m_robot->numJoints(); i++ ) {
-          m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q;
-        }
-      } else {
-        ref_basePos = m_robot->rootLink()->p;
-        ref_baseRot = m_robot->rootLink()->R;
-      }
-      // mode change for sync
-      if (control_mode == MODE_SYNC_TO_ABC) {
-        control_mode = MODE_ABC;
-      } else if (control_mode == MODE_SYNC_TO_IDLE && transition_interpolator->isEmpty() ) {
-        std::cerr << "[" << m_profile.instance_name << "] Finished cleanup" << std::endl;
-        control_mode = MODE_IDLE;
-      }
-    }
-    if ( m_qRef.data.length() != 0 ) { // initialized
-      if (is_legged_robot) {
-        for ( int i = 0; i < m_robot->numJoints(); i++ ){
-          m_qRef.data[i] = m_robot->joint(i)->q;
-        }
-      }
-      m_qOut.write();
-    }
-    if (is_legged_robot) {
-      // basePos
-      m_basePos.data.x = ref_basePos(0);
-      m_basePos.data.y = ref_basePos(1);
-      m_basePos.data.z = ref_basePos(2);
-      m_basePos.tm = m_qRef.tm;
-      // baseRpy
-      hrp::Vector3 baseRpy = hrp::rpyFromRot(ref_baseRot);
-      m_baseRpy.data.r = baseRpy(0);
-      m_baseRpy.data.p = baseRpy(1);
-      m_baseRpy.data.y = baseRpy(2);
-      m_baseRpy.tm = m_qRef.tm;
-      // baseTform
-      double *tform_arr = m_baseTform.data.get_buffer();
-      tform_arr[0] = m_basePos.data.x;
-      tform_arr[1] = m_basePos.data.y;
-      tform_arr[2] = m_basePos.data.z;
-      hrp::setMatrix33ToRowMajorArray(ref_baseRot, tform_arr, 3);
-      m_baseTform.tm = m_qRef.tm;
-      // basePose
-      m_basePose.data.position.x = m_basePos.data.x;
-      m_basePose.data.position.y = m_basePos.data.y;
-      m_basePose.data.position.z = m_basePos.data.z;
-      m_basePose.data.orientation.r = m_baseRpy.data.r;
-      m_basePose.data.orientation.p = m_baseRpy.data.p;
-      m_basePose.data.orientation.y = m_baseRpy.data.y;
-      m_basePose.tm = m_qRef.tm;
-      // zmp
-      m_zmp.data.x = rel_ref_zmp(0);
-      m_zmp.data.y = rel_ref_zmp(1);
-      m_zmp.data.z = rel_ref_zmp(2);
-      m_zmp.tm = m_qRef.tm;
-      // cog
-      m_cog.data.x = ref_cog(0);
-      m_cog.data.y = ref_cog(1);
-      m_cog.data.z = ref_cog(2);
-      m_cog.tm = m_qRef.tm;
-      // sbpCogOffset
-      m_sbpCogOffset.data.x = sbp_cog_offset(0);
-      m_sbpCogOffset.data.y = sbp_cog_offset(1);
-      m_sbpCogOffset.data.z = sbp_cog_offset(2);
-      m_sbpCogOffset.tm = m_qRef.tm;
-    }
-    m_basePosOut.write();
-    m_baseRpyOut.write();
-    m_baseTformOut.write();
-    m_basePoseOut.write();
-    m_zmpOut.write();
-    m_cogOut.write();
-    m_sbpCogOffsetOut.write();
-
-
-//printf("m_zmp:%f, abs:%f\n",m_zmp.data.y,m_basePos.data.y+m_zmp.data.y);
-
-    // reference acceleration
-    hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
-    if (sen != NULL) {
-      hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos;
-      hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt;
-      // convert to imu sensor local acceleration
-      hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt;
-      m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2);
-      m_accRefOut.write();
-      prev_imu_sensor_pos = imu_sensor_pos;
-      prev_imu_sensor_vel = imu_sensor_vel;
-    }
-
-    // control parameters
-    m_contactStates.tm = m_qRef.tm;
-    m_contactStatesOut.write();
-    m_controlSwingSupportTime.tm = m_qRef.tm;
-    m_controlSwingSupportTimeOut.write();
-    m_walkingStates.data = gg_is_walking;
-    m_walkingStates.tm = m_qRef.tm;
-    m_walkingStatesOut.write();
-
-    for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
-        m_force[i].tm = m_qRef.tm;
-        m_ref_forceOut[i]->write();
-    }
-
-    for (unsigned int i=0; i<m_limbCOPOffsetOut.size(); i++){
-        m_limbCOPOffset[i].tm = m_qRef.tm;
-        m_limbCOPOffsetOut[i]->write();
-    }
-
-    return RTC::RTC_OK;
-}
-
-void AutoBalancer::getCurrentParameters()
-{
-  current_root_p = m_robot->rootLink()->p;
-  current_root_R = m_robot->rootLink()->R;
-  for ( int i = 0; i < m_robot->numJoints(); i++ ){
-    qorg[i] = m_robot->joint(i)->q;
-  }
-}
-
-void AutoBalancer::getTargetParameters()
-{
-  // joint angles
-  for ( int i = 0; i < m_robot->numJoints(); i++ ){
-    m_robot->joint(i)->q = m_qRef.data[i];
-    qrefv[i] = m_robot->joint(i)->q;
-  }
-  // basepos, rot, zmp
-  m_robot->rootLink()->p = input_basePos;
-  m_robot->rootLink()->R = input_baseRot;
-  m_robot->calcForwardKinematics();
-  //
-  if (control_mode != MODE_IDLE) {
-    coordinates tmp_fix_coords;
-    if (!zmp_offset_interpolator->isEmpty()) {
-      double *default_zmp_offsets_output = new double[ikp.size()*3];
-      zmp_offset_interpolator->get(default_zmp_offsets_output, true);
-      for (size_t i = 0; i < ikp.size(); i++)
-        for (size_t j = 0; j < 3; j++)
-          default_zmp_offsets[i](j) = default_zmp_offsets_output[i*3+j];
-      delete[] default_zmp_offsets_output;
-      if (DEBUGP) {
-        std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl;
-        std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-        for (size_t i = 0; i < leg_names.size(); i++) {
-            std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
-            std::cerr << "[" << m_profile.instance_name << "]   " << leg_names[i] << " = " << default_zmp_offsets[dst->first].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-        }
-      }
-    }
-    if (!leg_names_interpolator->isEmpty()) {
-        leg_names_interpolator->get(&leg_names_interpolator_ratio, true);
-    }else {
-        leg_names_interpolator_ratio = 1.0;
-    }
-    if ( gg_is_walking ) {
-      gg->set_default_zmp_offsets(default_zmp_offsets);
-      gg_solved = gg->proc_one_tick();
-      {
-          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-          coordinates tmpc;
-          // for support leg
-          for (std::vector<step_node>::const_iterator it = gg->get_support_leg_steps().begin(); it != gg->get_support_leg_steps().end(); it++) {
-              coordinates sp_coords = it->worldcoords;
-              coordinates(ikp[leg_type_map[it->l_r]].localPos,
-                          ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc);
-              sp_coords.transform(tmpc);
-              ikp[leg_type_map[it->l_r]].target_p0 = sp_coords.pos;
-              ikp[leg_type_map[it->l_r]].target_r0 = sp_coords.rot;
-          }
-          // for swing leg
-          for (std::vector<step_node>::const_iterator it = gg->get_swing_leg_steps().begin(); it != gg->get_swing_leg_steps().end(); it++) {
-              coordinates sw_coords = it->worldcoords;
-              coordinates(ikp[leg_type_map[it->l_r]].localPos,
-                          ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc);
-              sw_coords.transform(tmpc);
-              ikp[leg_type_map[it->l_r]].target_p0 = sw_coords.pos;
-              ikp[leg_type_map[it->l_r]].target_r0 = sw_coords.rot;
-          }
-      }
-      gg->get_swing_support_mid_coords(tmp_fix_coords);
-      // set contactStates
-      {
-          std::vector<std::string> tmp_current_support_states_names;
-          {
-              std::vector<leg_type> tmp_current_support_states = gg->get_current_support_states();
-              std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-              for (std::vector<leg_type>::const_iterator it = tmp_current_support_states.begin(); it != tmp_current_support_states.end(); it++)
-                  tmp_current_support_states_names.push_back(leg_type_map[*it]);
-          }
-          // Set Contact States for ee not included in leg_names to false
-          for ( std::map<std::string, size_t>::iterator it = contact_states_index_map.begin(); it != contact_states_index_map.end(); it++ ) {
-              m_contactStates.data[it->second] = false;
-          }
-          for (std::vector<std::string>::const_iterator it = leg_names.begin(); it != leg_names.end(); it++) {
-              std::vector<std::string>::const_iterator dst = std::find_if(tmp_current_support_states_names.begin(), tmp_current_support_states_names.end(), boost::lambda::_1 == *it);
-              if (dst != tmp_current_support_states_names.end()) {
-                  m_contactStates.data[contact_states_index_map[*it]] = true;
-              } else {
-                  m_contactStates.data[contact_states_index_map[*it]] = false;
-              }
-          }
-      }
-      // set controlSwingSupportTime
-      {
-          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-          for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
-              std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
-              m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = gg->get_current_swing_time(dst->first);
-          }
-      }
-      // set ref_forces
-      {
-          std::vector<hrp::Vector3> ee_pos;
-          for (size_t i = 0 ; i < leg_names.size(); i++) {
-              ABCIKparam& tmpikp = ikp[leg_names[i]];
-              ee_pos.push_back(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]);
-          }
-          double alpha = (ref_zmp - ee_pos[1]).norm() / (ee_pos[0] - ee_pos[1]).norm();
-          if (alpha>1.0) alpha = 1.0;
-          if (alpha<0.0) alpha = 0.0;
-          if (DEBUGP) {
-          std::cerr << "[" << m_profile.instance_name << "] alpha:" << alpha << std::endl;
-          }
-          double mg = m_robot->totalMass() * gg->get_gravitational_acceleration();
-          m_force[0].data[0] = alpha * mg;
-          m_force[1].data[0] = (1-alpha) * mg;
-      }
-      // set limbCOPOffset
-      {
-          std::vector<std::string> swg_leg_nms = gg->get_swing_leg_names();
-          for (size_t i = 0; i < swg_leg_nms.size(); i++) {
-              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.x = gg->get_swing_foot_zmp_offsets().at(i)(0);
-              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.y = gg->get_swing_foot_zmp_offsets().at(i)(1);
-              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.z = gg->get_swing_foot_zmp_offsets().at(i)(2);
-          }
-      }
-      {
-          std::vector<std::string> sup_leg_nms = gg->get_support_leg_names();
-          for (size_t i = 0; i < sup_leg_nms.size(); i++) {
-              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.x = gg->get_support_foot_zmp_offsets().at(i)(0);
-              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.y = gg->get_support_foot_zmp_offsets().at(i)(1);
-              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.z = gg->get_support_foot_zmp_offsets().at(i)(2);
-          }
-      }
-    } else {
-      tmp_fix_coords = fix_leg_coords;
-      // double support by default
-      {
-          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-          for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
-              std::vector<std::string>::const_iterator dst = std::find_if(leg_names.begin(), leg_names.end(), (boost::lambda::_1 == it->first));
-              if (dst != leg_names.end()) {
-                  m_contactStates.data[contact_states_index_map[it->first]] = true;
-              } else {
-                  m_contactStates.data[contact_states_index_map[it->first]] = false;
-              }
-              // controlSwingSupportTime is not used while double support period, 1.0 is neglected
-              m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = 1.0;
-              std::map<leg_type, std::string>::const_iterator dst2 = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
-              m_limbCOPOffset[contact_states_index_map[it->first]].data.x = default_zmp_offsets.at(dst2->first)(0);
-              m_limbCOPOffset[contact_states_index_map[it->first]].data.y = default_zmp_offsets.at(dst2->first)(1);
-              m_limbCOPOffset[contact_states_index_map[it->first]].data.z = default_zmp_offsets.at(dst2->first)(2);
-          }
-      }
-    }
-    if (!adjust_footstep_interpolator->isEmpty()) {
-        double tmp = 0.0;
-        adjust_footstep_interpolator->get(&tmp, true);
-        //std::cerr << "[" << m_profile.instance_name << "] adjust ratio " << tmp << std::endl;
-        ikp["rleg"].target_p0 = (1-tmp) * ikp["rleg"].adjust_interpolation_org_p0 + tmp*ikp["rleg"].adjust_interpolation_target_p0;
-        ikp["lleg"].target_p0 = (1-tmp) * ikp["lleg"].adjust_interpolation_org_p0 + tmp*ikp["lleg"].adjust_interpolation_target_p0;
-        rats::mid_rot(ikp["rleg"].target_r0, tmp, ikp["rleg"].adjust_interpolation_org_r0, ikp["rleg"].adjust_interpolation_target_r0);
-        rats::mid_rot(ikp["lleg"].target_r0, tmp, ikp["lleg"].adjust_interpolation_org_r0, ikp["lleg"].adjust_interpolation_target_r0);
-        coordinates tmprc, tmplc;
-        tmprc.pos = ikp["rleg"].target_p0 + ikp["rleg"].target_r0 * ikp["rleg"].localPos;
-        tmprc.rot = ikp["rleg"].target_r0 * ikp["rleg"].localR;
-        tmplc.pos = ikp["lleg"].target_p0 + ikp["lleg"].target_r0 * ikp["lleg"].localPos;
-        tmplc.rot = ikp["lleg"].target_r0 * ikp["lleg"].localR;
-        rats::mid_coords(fix_leg_coords, 0.5, tmprc, tmplc);
-        tmp_fix_coords = fix_leg_coords;
-    }
-    // Tempolarily modify tmp_fix_coords
-    // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272
-    {
-      hrp::Vector3 ex = hrp::Vector3::UnitX();
-      hrp::Vector3 ez = hrp::Vector3::UnitZ();
-      hrp::Vector3 xv1 (tmp_fix_coords.rot * ex);
-      xv1(2) = 0.0;
-      xv1.normalize();
-      hrp::Vector3 yv1(ez.cross(xv1));
-      tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2);
-      tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2);
-      tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2);
-    }
-    // Fix pos
-    fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot);
-
-    /* update ref_forces ;; sp's absolute -> rmc's absolute */
-    for (size_t i = 0; i < m_ref_forceIn.size(); i++) {
-      hrp::Matrix33 eeR;
-      hrp::Link* parentlink;
-      hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
-      if (sensor) parentlink = sensor->link;
-      else parentlink = m_vfs[sensor_names[i]].link;
-      for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-          if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR;
-      }
-      // End effector frame
-      //ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
-      // world frame
-      ref_forces[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
-    }
-    sbp_offset = tmp_fix_coords.rot * hrp::Vector3(sbp_offset);
-
-    target_root_p = m_robot->rootLink()->p;
-    target_root_R = m_robot->rootLink()->R;
-    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-      if ( control_mode == MODE_IDLE || std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end() ) {
-        it->second.target_p0 = it->second.target_link->p;
-        it->second.target_r0 = it->second.target_link->R;
-      }
-    }
-    // Just for ik initial value
-    if (control_mode == MODE_SYNC_TO_ABC) {
-        current_root_p = target_root_p;
-        current_root_R = target_root_R;
-        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
-                it->second.target_p0 = it->second.target_link->p;
-                it->second.target_r0 = it->second.target_link->R;
-            }
-        }
-    }
-    // Move hand for hand fix mode
-    //   If arms' ABCIKparam.is_active is true, move hands according to cog velocity.
-    //   If is_hand_fix_mode is false, no hand fix mode and move hands according to cog velocity.
-    //   If is_hand_fix_mode is true, hand fix mode and do not move hands in Y axis in tmp_fix_coords.rot.    
-    if (gg_is_walking) {
-        // hand control while walking = solve hand ik with is_hand_fix_mode and solve hand ik without is_hand_fix_mode
-        bool is_hand_control_while_walking = false;
-        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
-                 && it->first.find("arm") != std::string::npos ) {
-                is_hand_control_while_walking = true;
-            }
-        }
-        if (is_hand_control_while_walking) {
-        //if (false) { // Disabled temporarily
-            // Store hand_fix_initial_offset in the initialization of walking
-            if (is_hand_fix_initial) hand_fix_initial_offset = tmp_fix_coords.rot.transpose() * (hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos);
-            is_hand_fix_initial = false;
-            hrp::Vector3 dif_p = hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos - tmp_fix_coords.rot * hand_fix_initial_offset;
-            if (is_hand_fix_mode) {
-                dif_p = tmp_fix_coords.rot.transpose() * dif_p;
-                dif_p(1) = 0;
-                dif_p = tmp_fix_coords.rot * dif_p;
-            }
-            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-                if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
-                     && it->first.find("arm") != std::string::npos ) {
-                    it->second.target_p0 = it->second.target_p0 + dif_p;
-                }
-            }
-        }
-    }
-
-    hrp::Vector3 tmp_foot_mid_pos(hrp::Vector3::Zero());
-    {
-        std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-        std::map<leg_type, double> zmp_weight_map = gg->get_zmp_weight_map();
-        double sum_of_weight = 0.0;
-        for (size_t i = 0; i < leg_names.size(); i++) {
-            ABCIKparam& tmpikp = ikp[leg_names[i]];
-            // get target_end_coords
-            tmpikp.target_end_coords.pos = tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos;
-            tmpikp.target_end_coords.rot = tmpikp.target_r0 * tmpikp.localR;
-            // for foot_mid_pos
-            std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
-            tmp_foot_mid_pos += (tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]) * zmp_weight_map[dst->first];
-            sum_of_weight += zmp_weight_map[dst->first];
-        }
-        tmp_foot_mid_pos *= (1.0 / sum_of_weight);
-    }
-    //
-    {
-        if ( gg_is_walking && gg->get_lcg_count() == gg->get_overwrite_check_timing()+2 ) {
-            hrp::Vector3 vel_htc(calc_vel_from_hand_error(tmp_fix_coords));
-            gg->set_offset_velocity_param(vel_htc(0), vel_htc(1) ,vel_htc(2));
-        }//  else {
-        //     if ( gg_is_walking && gg->get_lcg_count() == static_cast<size_t>(gg->get_default_step_time()/(2*m_dt))-1) {
-        //         gg->set_offset_velocity_param(0,0,0);
-        //     }
-        // }
-    }
-
-
-    hrp::Vector3 tmp_ref_cog(m_robot->calcCM());
-    if (gg_is_walking) {
-      ref_cog = gg->get_cog();
-    } else {
-      ref_cog = tmp_foot_mid_pos;
-    }
-    ref_cog(2) = tmp_ref_cog(2);
-    if (gg_is_walking) {
-      ref_zmp = gg->get_refzmp();
-    } else {
-      ref_zmp(0) = ref_cog(0);
-      ref_zmp(1) = ref_cog(1);
-      ref_zmp(2) = tmp_foot_mid_pos(2);
-    }
-  }
-};
-
-hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2)
-{
-  hrp::Vector3 vv = axis1.cross(axis2);
-  if (fabs(vv.norm()-0.0) < 1e-5) {
-    return rot;
-  } else {
-    Eigen::AngleAxis<double> tmpr(std::asin(vv.norm()), vv.normalized());
-    return tmpr.toRotationMatrix() * rot;
-  }
-}
-
-void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot)
-{
-  // get current foot mid pos + rot
-  std::vector<coordinates> foot_coords;
-  for (size_t i = 0; i < leg_names.size(); i++) {
-      ABCIKparam& tmpikp = ikp[leg_names[i]];
-      if (leg_names[i].find("leg") != std::string::npos) foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
-                                                                                           (tmpikp.target_link->R * tmpikp.localR)));
-  }
-  coordinates current_foot_mid_coords;
-  multi_mid_coords(current_foot_mid_coords, foot_coords);
-  hrp::Vector3 current_foot_mid_pos = current_foot_mid_coords.pos;
-  hrp::Matrix33 current_foot_mid_rot = current_foot_mid_coords.rot;
-  // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
-  hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose());
-  m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
-  rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
-  m_robot->calcForwardKinematics();
-}
-
-bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param, const std::string& limb_name)
-{
-  param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
-  // IK check
-  hrp::Vector3 vel_p, vel_r;
-  vel_p = param.target_p0 - param.target_link->p;
-  rats::difference_rotation(vel_r, param.target_link->R, param.target_r0);
-  if (vel_p.norm() > pos_ik_thre && transition_interpolator->isEmpty()) {
-      if (param.pos_ik_error_count % ik_error_debug_print_freq == 0) {
-          std::cerr << "[" << m_profile.instance_name << "] Too large IK error in " << limb_name << " (vel_p) = [" << vel_p(0) << " " << vel_p(1) << " " << vel_p(2) << "][m], count = " << param.pos_ik_error_count << std::endl;
-      }
-      param.pos_ik_error_count++;
-      has_ik_failed = true;
-  } else {
-      param.pos_ik_error_count = 0;
-  }
-  if (vel_r.norm() > rot_ik_thre && transition_interpolator->isEmpty()) {
-      if (param.rot_ik_error_count % ik_error_debug_print_freq == 0) {
-          std::cerr << "[" << m_profile.instance_name << "] Too large IK error in " << limb_name << " (vel_r) = [" << vel_r(0) << " " << vel_r(1) << " " << vel_r(2) << "][rad], count = " << param.rot_ik_error_count << std::endl;
-      }
-      param.rot_ik_error_count++;
-      has_ik_failed = true;
-  } else {
-      param.rot_ik_error_count = 0;
-  }
-  return true;
-}
-
-
-
-void AutoBalancer::solveLimbIK ()
-{
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-    if (it->second.is_active) {
-      for ( int j = 0; j < it->second.manip->numJoints(); j++ ){
-	int i = it->second.manip->joint(j)->jointId;
-	m_robot->joint(i)->q = qorg[i];
-      }
-    }
-  }
-  m_robot->rootLink()->p = current_root_p;
-  m_robot->rootLink()->R = current_root_R;
-  m_robot->calcForwardKinematics();
-  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
-  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
-  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
-  dif_cog *= leg_names_interpolator_ratio;
-  dif_cog(2) = m_robot->rootLink()->p(2) - target_root_p(2);
-  m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog;
-  m_robot->rootLink()->R = target_root_R;
-
-
-  //for HumanSynchronizer
-//  static int HumanSyncCountdownNum = 5 * (1/m_dt);
-  if(hsp->startCountdownForHumanSync){
-    std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";//ややCOMのIKに手間取った時プリント
-	  hsp->updateCountDown();
-  }
-  if(hsp->isHumanSyncOn()){
-	  if(hsp->ht_first_call){
-		  hsp->init_wld_rp_basepos = m_robot->rootLink()->p;
-      hsp->setInitOffsetPose();
-      hsp->rp_wld_initpos.com = m_robot->calcCM();
-      hsp->rp_wld_initpos.rf = ikp["rleg"].target_link->p;
-      hsp->rp_wld_initpos.lf = ikp["lleg"].target_link->p;
-      if(ikp.count("rarm"))hsp->rp_wld_initpos.rh = ikp["rarm"].target_link->p;
-      if(ikp.count("larm"))hsp->rp_wld_initpos.lh = ikp["larm"].target_link->p;
-      hsp->ht_first_call = false;
-
-      hsp->calibInitHumanCOMFromZMP();
-
-      std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
-
-	  }
-  }else{
-    hsp->setInitOffsetPose();
-  }
-
-  hsp->update();//////HumanSynchronizerの主要処理
-  if(loop%100==0)hsp->rp_ref_out.print();
-  if(loop%100==0)cerr<<"getUpdateTime:"<<hsp->getUpdateTime()*1000<<"[ms]"<<endl;
-
-  if(hsp->isHumanSyncOn()){
-	  ////////////////////// 重心拘束位置設定 /////////////////////////
-    m_robot->rootLink()->p = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;//まずは重心目標分，体幹を動かす
-    m_robot->rootLink()->p(2) = hsp->init_wld_rp_basepos(2)+ hsp->rp_ref_out.com(2);//高さだけはベース基準
-    m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
-    m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
-  }
-
-  // Fix for toe joint
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-      if (it->second.is_active && it->second.has_toe_joint && gg->get_use_toe_joint()) {
-          int i = it->second.target_link->jointId;
-          if (gg->get_swing_leg_names().front() == it->first) {
-              m_robot->joint(i)->q = qrefv[i] + -1 * gg->get_foot_dif_rot_angle();
-          } else {
-              m_robot->joint(i)->q = qrefv[i];
-          }
-      }
-  }
-  m_robot->calcForwardKinematics();
-
-
-  // additional COM fitting IK for HumanSynchronizer
-  hrp::Vector3 tmp_com_err = hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM();
-  int com_ik_loop=0;
-  const int COM_IK_MAX_LOOP = 10;
-  const double COM_IK_MAX_ERROR = 0.0001;//0.001だとたまにカクッとなる
-  if(hsp->isHumanSyncOn()){
-    ikp["rleg"].target_p0 = hsp->rp_wld_initpos.rf + hsp->rp_ref_out.rf;
-    ikp["lleg"].target_p0 = hsp->rp_wld_initpos.lf + hsp->rp_ref_out.lf;
-    if(ikp.count("rarm"))ikp["rarm"].target_p0 = hsp->rp_wld_initpos.rh + hsp->rp_ref_out.rh;
-    if(ikp.count("larm"))ikp["larm"].target_p0 = hsp->rp_wld_initpos.lh + hsp->rp_ref_out.lh;
-    while(fabs(tmp_com_err(0))>COM_IK_MAX_ERROR || fabs(tmp_com_err(1))>COM_IK_MAX_ERROR || fabs(tmp_com_err(2))>COM_IK_MAX_ERROR){//Z方向のCOM合わせは要注意(たまに怪しい)
-		  m_robot->rootLink()->p(0) += tmp_com_err(0);
-      m_robot->rootLink()->p(1) += tmp_com_err(1);
-      m_robot->rootLink()->p(2) += tmp_com_err(2);
-		  m_robot->calcForwardKinematics();
-		  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-		    if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
-		  }
-		  m_robot->calcForwardKinematics();
-      tmp_com_err = (hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM());
-		  if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "[" << m_profile.instance_name << "] COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
-	  }
-
-//    static hrp::Vector3 pre_ik_basepos;
-//    for(int i=0;i<3;i++){//重心拘束で解いた結果の体幹軌道の滑らかさは保証されていないので体幹位置に速度制限＆再度IK
-//      if(m_robot->rootLink()->p(i) - pre_ik_basepos(i) > 0.0002){ m_robot->rootLink()->p(i) =  pre_ik_basepos(i) + 0.0002; }
-//      else if(m_robot->rootLink()->p(i) - pre_ik_basepos(i) < -0.0002){ m_robot->rootLink()->p(i) =  pre_ik_basepos(i) - 0.0002; }
-//    }
-//    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-//      if (it->second.is_active) solveLimbIKforLimb(it->second);
-//    }
-//    pre_ik_basepos = m_robot->rootLink()->p;
-
-    //outport用のデータ上書き
-    ref_zmp(0) = hsp->rp_ref_out.zmp(0);
-    ref_zmp(1) = hsp->rp_ref_out.zmp(1);
-    ref_zmp(2) = 0;
-    ref_cog = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;
-
-	  if(loop%100==0)if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
-
-
-//	  printf("root:%f, calcCM:%f, refCM:%f\n",m_robot->rootLink()->p(1),m_robot->calcCM()(1),hsp->rp_ref_out.com(1) + hsp->rp_wld_initpos.com(1));
-
-
-  }else{
-	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
-	    if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
-	  }
-  }
-  if (gg_is_walking && !gg_solved) stopWalking ();
-}
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onAborting(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onError(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onReset(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onStateUpdate(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onRateChanged(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-bool AutoBalancer::startHumanSyncAfter5sec()
-{
-  std::cerr << "[" << m_profile.instance_name << "] start HumanSync after 5 sec" << std::endl;
-  hsp->startCountdownForHumanSync = true;
-  return true;
-}
-
-bool AutoBalancer::setHumanToRobotRatio(const double h2r)
-{
-  std::cerr << "[" << m_profile.instance_name << "] set h2r_ratio as "<< h2r << std::endl;
-  if(!hsp->isHumanSyncOn())hsp->h2r_ratio = h2r;
-  return true;
-}
-
-bool AutoBalancer::setAllowedXYZSync(const bool x_on,const bool y_on,const bool z_on)
-{
-  std::cerr << "[" << m_profile.instance_name << "] set allowed XYZ move direction as ("<<x_on<<","<<y_on<<","<<z_on<<")" << std::endl;
-  hsp->use_x = x_on;
-  hsp->use_y = y_on;
-  hsp->use_z = z_on;
-  return true;
-}
-
-bool AutoBalancer::stopHumanSync()
-{
-	std::cerr << "[" << m_profile.instance_name << "] stop HumanSync Now" << std::endl;
-	return true;
-}
-
-void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence& limbs)
-{
-  std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
-  Guard guard(m_mutex);
-  double tmp_ratio = 0.0;
-  transition_interpolator->clear();
-  transition_interpolator->set(&tmp_ratio);
-  tmp_ratio = 1.0;
-  transition_interpolator->go(&tmp_ratio, transition_time, true);
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-    it->second.is_active = false;
-  }
-
-  for (size_t i = 0; i < limbs.length(); i++) {
-    ABCIKparam& tmp = ikp[std::string(limbs[i])];
-    tmp.is_active = true;
-    std::cerr << "[" << m_profile.instance_name << "]   limb [" << std::string(limbs[i]) << "]" << std::endl;
-  }
-
-  control_mode = MODE_SYNC_TO_ABC;
-}
-
-void AutoBalancer::stopABCparam()
-{
-  std::cerr << "[" << m_profile.instance_name << "] stop auto balancer mode" << std::endl;
-  //Guard guard(m_mutex);
-  double tmp_ratio = 1.0;
-  transition_interpolator->clear();
-  transition_interpolator->set(&tmp_ratio);
-  tmp_ratio = 0.0;
-  transition_interpolator->go(&tmp_ratio, transition_time, true);
-  control_mode = MODE_SYNC_TO_IDLE;
-}
-
-void AutoBalancer::startWalking ()
-{
-  if ( control_mode != MODE_ABC ) {
-    return_control_mode = control_mode;
-    OpenHRP::AutoBalancerService::StrSequence fix_limbs;
-    fix_limbs.length(2);
-    fix_limbs[0] = "rleg";
-    fix_limbs[1] = "lleg";
-    startABCparam(fix_limbs);
-    waitABCTransition();
-  }
-  {
-    Guard guard(m_mutex);
-    has_ik_failed = false;
-    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-        it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
-    }
-    std::vector<std::string> init_swing_leg_names(gg->get_footstep_front_leg_names());
-    std::vector<std::string> tmp_all_limbs(leg_names);
-    std::vector<std::string> init_support_leg_names;
-    std::sort(tmp_all_limbs.begin(), tmp_all_limbs.end());
-    std::sort(init_swing_leg_names.begin(), init_swing_leg_names.end());
-    std::set_difference(tmp_all_limbs.begin(), tmp_all_limbs.end(),
-                        init_swing_leg_names.begin(), init_swing_leg_names.end(),
-                        std::back_inserter(init_support_leg_names));
-    std::vector<step_node> init_support_leg_steps, init_swing_leg_dst_steps;
-    for (std::vector<std::string>::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++)
-        init_support_leg_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0));
-    for (std::vector<std::string>::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++)
-        init_swing_leg_dst_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0));
-    gg->set_default_zmp_offsets(default_zmp_offsets);
-    gg->initialize_gait_parameter(ref_cog, init_support_leg_steps, init_swing_leg_dst_steps);
-  }
-  is_hand_fix_initial = true;
-  while ( !gg->proc_one_tick() );
-  {
-    Guard guard(m_mutex);
-    gg_is_walking = gg_solved = true;
-  }
-}
-
-void AutoBalancer::stopWalking ()
-{
-  std::vector<coordinates> tmp_end_coords_list;
-  for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
-      if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(ikp[*it].target_end_coords);
-  }
-  multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
-  fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot);
-  gg->clear_footstep_nodes_list();
-  if (return_control_mode == MODE_IDLE) stopABCparam();
-  gg_is_walking = false;
-}
-
-bool AutoBalancer::startAutoBalancer (const OpenHRP::AutoBalancerService::StrSequence& limbs)
-{
-  if (control_mode == MODE_IDLE) {
-    has_ik_failed = false;
-    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-        it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
-    }
-    startABCparam(limbs);
-    waitABCTransition();
-    return_control_mode = MODE_ABC;
-    return true;
-  } else {
-    return false;
-  }
-}
-
-bool AutoBalancer::stopAutoBalancer ()
-{
-  if (control_mode == MODE_ABC) {
-    stopABCparam();
-    waitABCTransition();
-    return true;
-  } else {
-    return false;
-  }
-}
-
-void AutoBalancer::waitABCTransition()
-{
-  while (!transition_interpolator->isEmpty()) usleep(1000);
-  usleep(1000);
-}
-bool AutoBalancer::goPos(const double& x, const double& y, const double& th)
-{
-    //  if ( !gg_is_walking && !is_stop_mode) {
-  if ( !is_stop_mode) {
-    gg->set_all_limbs(leg_names);
-    coordinates start_ref_coords;
-    std::vector<coordinates> initial_support_legs_coords;
-    std::vector<leg_type> initial_support_legs;
-    bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
-    if (is_valid_gait_type == false) return false;
-    bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th,
-                                                      initial_support_legs_coords, // Dummy if gg_is_walking
-                                                      start_ref_coords,            // Dummy if gg_is_walking
-                                                      initial_support_legs,        // Dummy if gg_is_walking
-                                                      (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps.
-    if ( !gg_is_walking ) { // Initializing
-        startWalking();
-    }
-    if (!ret) {
-        std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl;
-    }
-    return ret;
-  } else {
-    std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl;
-    return false;
-  }
-}
-
-bool AutoBalancer::goVelocity(const double& vx, const double& vy, const double& vth)
-{
-  gg->set_all_limbs(leg_names);
-  if (gg_is_walking && gg_solved) {
-    gg->set_velocity_param(vx, vy, vth);
-  } else {
-    coordinates ref_coords;
-    mid_coords(ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
-    std::vector<leg_type> current_legs;
-    switch(gait_type) {
-    case BIPED:
-        current_legs = (vy > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
-        break;
-    case TROT:
-        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
-        break;
-    case PACE:
-        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
-        break;
-    case CRAWL:
-        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    case GALLOP:
-        /* at least one leg shoud be in contact */
-        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    default: break;
-    }
-    gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs);
-    startWalking();
-  }
-  return true;
-}
-
-bool AutoBalancer::goStop ()
-{
-  gg->finalize_velocity_mode();
-  waitFootSteps();
-  return true;
-}
-
-bool AutoBalancer::emergencyStop ()
-{
-  std::cerr << "[" << m_profile.instance_name << "] emergencyStop" << std::endl;
-  // is_stop_mode = true;
-  gg->emergency_stop();
-  waitFootSteps();
-  return true;
-}
-
-bool AutoBalancer::releaseEmergencyStop ()
-{
-  if (is_stop_mode) {
-      std::cerr << "[" << m_profile.instance_name << "] releaseEmergencyStop" << std::endl;
-      is_stop_mode = false;
-  }
-  return true;
-}
-
-bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
-{
-  OpenHRP::AutoBalancerService::StepParamsSequence spss;
-  spss.length(fss.length());
-  // If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
-  // If gg_is_walking is true, do not set to 0.
-  for (size_t i = 0; i < spss.length(); i++) {
-      spss[i].sps.length(fss[i].fs.length());
-      for (size_t j = 0; j < spss[i].sps.length(); j++) {
-          spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
-          spss[i].sps[j].step_time = gg->get_default_step_time();
-          spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle());
-          spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle());
-      }
-  }
-  setFootStepsWithParam(fss, spss, overwrite_fs_idx);
-}
-
-bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
-{
-    if (!is_stop_mode) {
-        std::cerr << "[" << m_profile.instance_name << "] setFootStepsList" << std::endl;
-
-        // Initial footstep Snapping
-        coordinates tmpfs, fstrans;
-        step_node initial_support_step, initial_input_step;
-        {
-            std::vector<step_node> initial_support_steps;
-            if (gg_is_walking) {
-                if (overwrite_fs_idx <= 0) {
-                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
-                    return false;
-                }
-                if (!gg->get_footstep_nodes_by_index(initial_support_steps, overwrite_fs_idx-1)) {
-                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
-                    return false;
-                }
-            } else {
-                // If walking, snap initial leg to current ABC foot coords.
-                for (size_t i = 0; i < fss[0].fs.length(); i++) {
-                    initial_support_steps.push_back(step_node(std::string(fss[0].fs[i].leg), ikp[std::string(fss[0].fs[i].leg)].target_end_coords, 0, 0, 0, 0));
-                }
-            }
-            initial_support_step = initial_support_steps.front(); /* use only one leg for representation */
-        }
-        {
-            std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-            for (size_t i = 0; i < fss[0].fs.length(); i++) {
-                if (std::string(fss[0].fs[i].leg) == leg_type_map[initial_support_step.l_r]) {
-                    coordinates tmp;
-                    memcpy(tmp.pos.data(), fss[0].fs[i].pos, sizeof(double)*3);
-                    tmp.rot = (Eigen::Quaternion<double>(fss[0].fs[i].rot[0], fss[0].fs[i].rot[1], fss[0].fs[i].rot[2], fss[0].fs[i].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
-                    initial_input_step = step_node(std::string(fss[0].fs[i].leg), tmp, 0, 0, 0, 0);
-                }
-            }
-        }
-
-        // Get footsteps
-        std::vector< std::vector<coordinates> > fs_vec_list;
-        std::vector< std::vector<std::string> > leg_name_vec_list;
-        for (size_t i = 0; i < fss.length(); i++) {
-            std::vector<coordinates> fs_vec;
-            std::vector<std::string> leg_name_vec;
-            for (size_t j = 0; j < fss[i].fs.length(); j++) {
-                std::string leg(fss[i].fs[j].leg);
-                if (std::find(leg_names.begin(), leg_names.end(), leg) != leg_names.end()) {
-                    memcpy(tmpfs.pos.data(), fss[i].fs[j].pos, sizeof(double)*3);
-                    tmpfs.rot = (Eigen::Quaternion<double>(fss[i].fs[j].rot[0], fss[i].fs[j].rot[1], fss[i].fs[j].rot[2], fss[i].fs[j].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
-                    initial_input_step.worldcoords.transformation(fstrans, tmpfs);
-                    tmpfs = initial_support_step.worldcoords;
-                    tmpfs.transform(fstrans);
-                } else {
-                    std::cerr << "[" << m_profile.instance_name << "]   No such target : " << leg << std::endl;
-                    return false;
-                }
-                leg_name_vec.push_back(leg);
-                fs_vec.push_back(tmpfs);
-            }
-            leg_name_vec_list.push_back(leg_name_vec);
-            fs_vec_list.push_back(fs_vec);
-        }
-        if (spss.length() != fs_vec_list.size()) {
-            std::cerr << "[" << m_profile.instance_name << "]   StepParam length " << spss.length () << " != Footstep length " << fs_vec_list.size() << std::endl;
-            return false;
-        }
-        std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl;
-        std::vector< std::vector<step_node> > fnsl;
-        for (size_t i = 0; i < fs_vec_list.size(); i++) {
-            if (!(gg_is_walking && i == 0)) { // If initial footstep, e.g., not walking, pass user-defined footstep list. If walking, pass cdr footsteps in order to neglect initial double support leg.
-                std::vector<step_node> tmp_fns;
-                for (size_t j = 0; j < fs_vec_list.at(i).size(); j++) {
-                    tmp_fns.push_back(step_node(leg_name_vec_list[i][j], fs_vec_list[i][j], spss[i].sps[j].step_height, spss[i].sps[j].step_time, spss[i].sps[j].toe_angle, spss[i].sps[j].heel_angle));
-                }
-                fnsl.push_back(tmp_fns);
-            }
-        }
-        if (gg_is_walking) {
-            std::cerr << "[" << m_profile.instance_name << "]  Set overwrite footsteps" << std::endl;
-            gg->set_overwrite_foot_steps_list(fnsl);
-            gg->set_overwrite_foot_step_index(overwrite_fs_idx);
-        } else {
-            std::cerr << "[" << m_profile.instance_name << "]  Set normal footsteps" << std::endl;
-            gg->set_foot_steps_list(fnsl);
-            startWalking();
-        }
-        return true;
-    } else {
-        std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl;
-        return false;
-    }
-}
-
-void AutoBalancer::waitFootSteps()
-{
-  //while (gg_is_walking) usleep(10);
-  while (gg_is_walking || !transition_interpolator->isEmpty() )
-    usleep(1000);
-  usleep(1000);
-  gg->set_offset_velocity_param(0,0,0);
-}
-
-void AutoBalancer::waitFootStepsEarly(const double tm)
-{
-  if (!gg_is_walking) { return;}
-  while ( !gg->is_finalizing(tm)|| !transition_interpolator->isEmpty() )
-    usleep(1000);
-  usleep(1000);
-  gg->set_offset_velocity_param(0,0,0);
-}
-
-bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
-{
-  std::cerr << "[" << m_profile.instance_name << "] setGaitGeneratorParam" << std::endl;
-  gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]);
-  std::vector<hrp::Vector3> off;
-  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
-      off.push_back(hrp::Vector3(i_param.leg_default_translate_pos[i][0], i_param.leg_default_translate_pos[i][1], i_param.leg_default_translate_pos[i][2]));
-  }
-  gg->set_leg_default_translate_pos(off);
-  gg->set_default_step_time(i_param.default_step_time);
-  gg->set_default_step_height(i_param.default_step_height);
-  gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio/2.0);
-  gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio/2.0);
-  gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio/2.0);
-  gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio/2.0);
-  gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio/2.0);
-  gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio/2.0);
-  // gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio_before);
-  // gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio_after);
-  // gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio_before);
-  // gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio_after);
-  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_before);
-  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_after);
-  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_swing_before);
-  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_swing_after);
-  if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::SHUFFLING) {
-    gg->set_default_orbit_type(SHUFFLING);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOID) {
-    gg->set_default_orbit_type(CYCLOID);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::RECTANGLE) {
-    gg->set_default_orbit_type(RECTANGLE);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::STAIR) {
-    gg->set_default_orbit_type(STAIR);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAY) {
-    gg->set_default_orbit_type(CYCLOIDDELAY);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK) {
-    gg->set_default_orbit_type(CYCLOIDDELAYKICK);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CROSS) {
-    gg->set_default_orbit_type(CROSS);
-  }
-  gg->set_swing_trajectory_delay_time_offset(i_param.swing_trajectory_delay_time_offset);
-  gg->set_swing_trajectory_final_distance_weight(i_param.swing_trajectory_final_distance_weight);
-  gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2]));
-  gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2]));  
-  gg->set_gravitational_acceleration(i_param.gravitational_acceleration);
-  gg->set_toe_angle(i_param.toe_angle);
-  gg->set_heel_angle(i_param.heel_angle);
-  gg->set_toe_pos_offset_x(i_param.toe_pos_offset_x);
-  gg->set_heel_pos_offset_x(i_param.heel_pos_offset_x);
-  gg->set_toe_zmp_offset_x(i_param.toe_zmp_offset_x);
-  gg->set_heel_zmp_offset_x(i_param.heel_zmp_offset_x);
-  std::vector<double> tmp_ratio(i_param.toe_heel_phase_ratio.get_buffer(), i_param.toe_heel_phase_ratio.get_buffer()+i_param.toe_heel_phase_ratio.length());
-  std::cerr << "[" << m_profile.instance_name << "]   "; // for set_toe_heel_phase_ratio
-  gg->set_toe_heel_phase_ratio(tmp_ratio);
-  gg->set_use_toe_joint(i_param.use_toe_joint);
-  gg->set_use_toe_heel_transition(i_param.use_toe_heel_transition);
-  gg->set_zmp_weight_map(boost::assign::map_list_of<leg_type, double>(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3]));
-  gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num);
-  gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset);
-  gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation);
-  gg->set_use_stride_limitation(i_param.use_stride_limitation);
-
-  // print
-  gg->print_param(std::string(m_profile.instance_name));
-  return true;
-};
-
-bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
-{
-  gg->get_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]);
-  std::vector<hrp::Vector3> off;
-  gg->get_leg_default_translate_pos(off);
-  i_param.leg_default_translate_pos.length(off.size());
-  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
-      i_param.leg_default_translate_pos[i].length(3);
-      i_param.leg_default_translate_pos[i][0] = off[i](0);
-      i_param.leg_default_translate_pos[i][1] = off[i](1);
-      i_param.leg_default_translate_pos[i][2] = off[i](2);
-  }
-  i_param.default_step_time = gg->get_default_step_time();
-  i_param.default_step_height = gg->get_default_step_height();
-  i_param.default_double_support_ratio_before = gg->get_default_double_support_ratio_before();
-  i_param.default_double_support_ratio_after = gg->get_default_double_support_ratio_after();
-  i_param.default_double_support_static_ratio_before = gg->get_default_double_support_static_ratio_before();
-  i_param.default_double_support_static_ratio_after = gg->get_default_double_support_static_ratio_after();
-  i_param.default_double_support_ratio_swing_before = gg->get_default_double_support_ratio_swing_before();
-  i_param.default_double_support_ratio_swing_after = gg->get_default_double_support_ratio_swing_after();
-  i_param.default_double_support_ratio = i_param.default_double_support_ratio_before + i_param.default_double_support_ratio_after;
-  i_param.default_double_support_static_ratio = i_param.default_double_support_static_ratio_before + i_param.default_double_support_static_ratio_after;
-  if (gg->get_default_orbit_type() == SHUFFLING) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::SHUFFLING;
-  } else if (gg->get_default_orbit_type() == CYCLOID) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOID;
-  } else if (gg->get_default_orbit_type() == RECTANGLE) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::RECTANGLE;
-  } else if (gg->get_default_orbit_type() == STAIR) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::STAIR;
-  } else if (gg->get_default_orbit_type() == CYCLOIDDELAY) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAY;
-  } else if (gg->get_default_orbit_type() == CYCLOIDDELAYKICK) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK;
-  } else if (gg->get_default_orbit_type() == CROSS) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CROSS;
-  }
-
-  hrp::Vector3 tmpv = gg->get_stair_trajectory_way_point_offset();
-  for (size_t i = 0; i < 3; i++) i_param.stair_trajectory_way_point_offset[i] = tmpv(i);
-  tmpv = gg->get_cycloid_delay_kick_point_offset();
-  for (size_t i = 0; i < 3; i++) i_param.cycloid_delay_kick_point_offset[i] = tmpv(i);
-  i_param.swing_trajectory_delay_time_offset = gg->get_swing_trajectory_delay_time_offset();
-  i_param.swing_trajectory_final_distance_weight = gg->get_swing_trajectory_final_distance_weight();
-  i_param.gravitational_acceleration = gg->get_gravitational_acceleration();
-  i_param.toe_angle = gg->get_toe_angle();
-  i_param.heel_angle = gg->get_heel_angle();
-  i_param.toe_pos_offset_x = gg->get_toe_pos_offset_x();
-  i_param.heel_pos_offset_x = gg->get_heel_pos_offset_x();
-  i_param.toe_zmp_offset_x = gg->get_toe_zmp_offset_x();
-  i_param.heel_zmp_offset_x = gg->get_heel_zmp_offset_x();
-  std::vector<double> ratio(gg->get_NUM_TH_PHASES(),0.0);
-  gg->get_toe_heel_phase_ratio(ratio);
-  for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) i_param.toe_heel_phase_ratio[i] = ratio[i];
-  i_param.use_toe_joint = gg->get_use_toe_joint();
-  i_param.use_toe_heel_transition = gg->get_use_toe_heel_transition();
-  std::map<leg_type, double> tmp_zmp_weight_map = gg->get_zmp_weight_map();
-  i_param.zmp_weight_map[0] = tmp_zmp_weight_map[RLEG];
-  i_param.zmp_weight_map[1] = tmp_zmp_weight_map[LLEG];
-  i_param.zmp_weight_map[2] = tmp_zmp_weight_map[RARM];
-  i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM];
-  i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num();
-  i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset();
-  for (size_t i=0; i<4; i++) {
-    i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i);
-  }
-  i_param.use_stride_limitation = gg->get_use_stride_limitation();
-  return true;
-};
-
-bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
-{
-  Guard guard(m_mutex);
-  std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl;
-  double *default_zmp_offsets_array = new double[ikp.size()*3];
-  move_base_gain = i_param.move_base_gain;
-  for (size_t i = 0; i < ikp.size(); i++)
-    for (size_t j = 0; j < 3; j++)
-      default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j];
-  zmp_transition_time = i_param.zmp_transition_time;
-  adjust_footstep_transition_time = i_param.adjust_footstep_transition_time;
-  if (zmp_offset_interpolator->isEmpty()) {
-      zmp_offset_interpolator->clear();
-      zmp_offset_interpolator->go(default_zmp_offsets_array, zmp_transition_time, true);
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets cannot be set because interpolating." << std::endl;
-  }
-  if (control_mode == MODE_IDLE) {
-    switch (i_param.use_force_mode) {
-    case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
-        use_force = MODE_NO_FORCE;
-        break;
-    case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
-        use_force = MODE_REF_FORCE;
-        break;
-    default:
-        break;
-    }
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   use_force_mode cannot be changed to [" << i_param.use_force_mode << "] during MODE_ABC, MODE_SYNC_TO_IDLE or MODE_SYNC_TO_ABC." << std::endl;
-  }
-  graspless_manip_mode = i_param.graspless_manip_mode;
-  graspless_manip_arm = std::string(i_param.graspless_manip_arm);
-  for (size_t j = 0; j < 3; j++)
-      graspless_manip_p_gain[j] = i_param.graspless_manip_p_gain[j];
-  for (size_t j = 0; j < 3; j++)
-      graspless_manip_reference_trans_coords.pos[j] = i_param.graspless_manip_reference_trans_pos[j];
-  graspless_manip_reference_trans_coords.rot = (Eigen::Quaternion<double>(i_param.graspless_manip_reference_trans_rot[0],
-                                                                          i_param.graspless_manip_reference_trans_rot[1],
-                                                                          i_param.graspless_manip_reference_trans_rot[2],
-                                                                          i_param.graspless_manip_reference_trans_rot[3]).normalized().toRotationMatrix()); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
-  transition_time = i_param.transition_time;
-  std::vector<std::string> cur_leg_names, dst_leg_names;
-  cur_leg_names = leg_names;
-  for (size_t i = 0; i < i_param.leg_names.length(); i++) {
-      dst_leg_names.push_back(std::string(i_param.leg_names[i]));
-  }
-  std::sort(cur_leg_names.begin(), cur_leg_names.end());
-  std::sort(dst_leg_names.begin(), dst_leg_names.end());
-  if (cur_leg_names != dst_leg_names) {
-      if (leg_names_interpolator->isEmpty()) {
-          leg_names.clear();
-          leg_names = dst_leg_names;
-          if (control_mode == MODE_ABC) {
-              double tmp_ratio = 0.0;
-              leg_names_interpolator->set(&tmp_ratio);
-              tmp_ratio = 1.0;
-              leg_names_interpolator->go(&tmp_ratio, 5.0, true);
-              control_mode = MODE_SYNC_TO_ABC;
-          }
-      }
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   leg_names cannot be set because interpolating." << std::endl;
-  }
-  pos_ik_thre = i_param.pos_ik_thre;
-  rot_ik_thre = i_param.rot_ik_thre;
-  if (!gg_is_walking) {
-      is_hand_fix_mode = i_param.is_hand_fix_mode;
-      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode = " << is_hand_fix_mode << std::endl;
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode cannot be set in (gg_is_walking = true). Current is_hand_fix_mode is " << (is_hand_fix_mode?"true":"false") << std::endl;
-  }
-  if (control_mode == MODE_IDLE) {
-      for (size_t i = 0; i < i_param.end_effector_list.length(); i++) {
-          std::map<std::string, ABCIKparam>::iterator it = ikp.find(std::string(i_param.end_effector_list[i].leg));
-          memcpy(it->second.localPos.data(), i_param.end_effector_list[i].pos, sizeof(double)*3);
-          it->second.localR = (Eigen::Quaternion<double>(i_param.end_effector_list[i].rot[0], i_param.end_effector_list[i].rot[1], i_param.end_effector_list[i].rot[2], i_param.end_effector_list[i].rot[3])).normalized().toRotationMatrix();
-      }
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl;
-  }
-  if (i_param.default_gait_type == OpenHRP::AutoBalancerService::BIPED) {
-      gait_type = BIPED;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::TROT) {
-      gait_type = TROT;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::PACE) {
-      gait_type = PACE;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::CRAWL) {
-      gait_type = CRAWL;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) {
-      gait_type = GALLOP;
-  }
-  for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
-      std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   localR = " << it->second.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
-  }
-
-  std::cerr << "[" << m_profile.instance_name << "]   move_base_gain = " << move_base_gain << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets = ";
-  for (size_t i = 0; i < ikp.size() * 3; i++) {
-      std::cerr << default_zmp_offsets_array[i] << " ";
-  }
-  std::cerr << std::endl;
-  delete[] default_zmp_offsets_array;
-  std::cerr << "[" << m_profile.instance_name << "]   use_force_mode = " << use_force << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_mode = " << graspless_manip_mode << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_arm = " << graspless_manip_arm << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_pos = " << graspless_manip_reference_trans_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_rot = " << graspless_manip_reference_trans_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   transition_time = " << transition_time << "[s], zmp_transition_time = " << zmp_transition_time << "[s], adjust_footstep_transition_time = " << adjust_footstep_transition_time << "[s]" << std::endl;
-  for (std::vector<std::string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) std::cerr << "[" << m_profile.instance_name << "]   leg_names [" << *it << "]" << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   pos_ik_thre = " << pos_ik_thre << "[m], rot_ik_thre = " << rot_ik_thre << "[rad]" << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   default_gait_type = " << gait_type << std::endl;
-  // IK limb parameters
-  std::cerr << "[" << m_profile.instance_name << "]  IK limb parameters" << std::endl;
-  bool is_ik_limb_parameter_valid_length = true;
-  if (i_param.ik_limb_parameters.length() != ee_vec.size()) {
-      is_ik_limb_parameter_valid_length = false;
-      std::cerr << "[" << m_profile.instance_name << "]   ik_limb_parameters invalid length! Cannot be set. (input = " << i_param.ik_limb_parameters.length() << ", desired = " << ee_vec.size() << ")" << std::endl;
-  } else {
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          if (ikp[ee_vec[i]].manip->numJoints() != i_param.ik_limb_parameters[i].ik_optional_weight_vector.length())
-              is_ik_limb_parameter_valid_length = false;
-      }
-      if (is_ik_limb_parameter_valid_length) {
-          for (size_t i = 0; i < ee_vec.size(); i++) {
-              ABCIKparam& param = ikp[ee_vec[i]];
-              const OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i];
-              std::vector<double> ov;
-              ov.resize(param.manip->numJoints());
-              for (size_t j = 0; j < param.manip->numJoints(); j++) {
-                  ov[j] = ilp.ik_optional_weight_vector[j];
-              }
-              param.manip->setOptionalWeightVector(ov);
-              param.manip->setSRGain(ilp.sr_gain);
-              param.avoid_gain = ilp.avoid_gain;
-              param.reference_gain = ilp.reference_gain;
-              param.manip->setManipulabilityLimit(ilp.manipulability_limit);
-          }
-      } else {
-          std::cerr << "[" << m_profile.instance_name << "]   ik_optional_weight_vector invalid length! Cannot be set. (input = [";
-          for (size_t i = 0; i < ee_vec.size(); i++) {
-              std::cerr << i_param.ik_limb_parameters[i].ik_optional_weight_vector.length() << ", ";
-          }
-          std::cerr << "], desired = [";
-          for (size_t i = 0; i < ee_vec.size(); i++) {
-              std::cerr << ikp[ee_vec[i]].manip->numJoints() << ", ";
-          }
-          std::cerr << "])" << std::endl;
-      }
-  }
-  if (is_ik_limb_parameter_valid_length) {
-      std::cerr << "[" << m_profile.instance_name << "]   ik_optional_weight_vectors = ";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          ABCIKparam& param = ikp[ee_vec[i]];
-          std::vector<double> ov;
-          ov.resize(param.manip->numJoints());
-          param.manip->getOptionalWeightVector(ov);
-          std::cerr << "[";
-          for (size_t j = 0; j < param.manip->numJoints(); j++) {
-              std::cerr << ov[j] << " ";
-          }
-          std::cerr << "]";
-      }
-      std::cerr << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   sr_gains = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].manip->getSRGain() << ", ";
-      }
-      std::cerr << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   avoid_gains = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].avoid_gain << ", ";
-      }
-      std::cerr << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   reference_gains = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].reference_gain << ", ";
-      }
-      std::cerr << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   manipulability_limits = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].manip->getManipulabilityLimit() << ", ";
-      }
-      std::cerr << "]" << std::endl;
-  }
-  return true;
-};
-
-bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
-{
-  i_param.move_base_gain = move_base_gain;
-  i_param.default_zmp_offsets.length(ikp.size());
-  for (size_t i = 0; i < ikp.size(); i++) {
-      i_param.default_zmp_offsets[i].length(3);
-      for (size_t j = 0; j < 3; j++) {
-          i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j);
-      }
-  }
-  switch(control_mode) {
-  case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break;
-  case MODE_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_ABC; break;
-  case MODE_SYNC_TO_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_IDLE; break;
-  case MODE_SYNC_TO_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_ABC; break;
-  default: break;
-  }
-  switch(use_force) {
-  case MODE_NO_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_NO_FORCE; break;
-  case MODE_REF_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE; break;
-  default: break;
-  }
-  i_param.graspless_manip_mode = graspless_manip_mode;
-  i_param.graspless_manip_arm = graspless_manip_arm.c_str();
-  for (size_t j = 0; j < 3; j++)
-      i_param.graspless_manip_p_gain[j] = graspless_manip_p_gain[j];
-  for (size_t j = 0; j < 3; j++)
-      i_param.graspless_manip_reference_trans_pos[j] = graspless_manip_reference_trans_coords.pos[j];
-  Eigen::Quaternion<double> qt(graspless_manip_reference_trans_coords.rot);
-  i_param.graspless_manip_reference_trans_rot[0] = qt.w();
-  i_param.graspless_manip_reference_trans_rot[1] = qt.x();
-  i_param.graspless_manip_reference_trans_rot[2] = qt.y();
-  i_param.graspless_manip_reference_trans_rot[3] = qt.z();
-  i_param.transition_time = transition_time;
-  i_param.zmp_transition_time = zmp_transition_time;
-  i_param.adjust_footstep_transition_time = adjust_footstep_transition_time;
-  i_param.leg_names.length(leg_names.size());
-  for (size_t i = 0; i < leg_names.size(); i++) i_param.leg_names[i] = leg_names.at(i).c_str();
-  i_param.pos_ik_thre = pos_ik_thre;
-  i_param.rot_ik_thre = rot_ik_thre;
-  i_param.is_hand_fix_mode = is_hand_fix_mode;
-  i_param.end_effector_list.length(ikp.size());
-  {
-      size_t i = 0;
-      for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
-          copyRatscoords2Footstep(i_param.end_effector_list[i],
-                                  coordinates(it->second.localPos, it->second.localR));
-          i_param.end_effector_list[i].leg = it->first.c_str();
-          i++;
-      }
-  }
-  switch(gait_type) {
-  case BIPED:  i_param.default_gait_type = OpenHRP::AutoBalancerService::BIPED;  break;
-  case TROT:   i_param.default_gait_type = OpenHRP::AutoBalancerService::TROT;   break;
-  case PACE:   i_param.default_gait_type = OpenHRP::AutoBalancerService::PACE;   break;
-  case CRAWL:  i_param.default_gait_type = OpenHRP::AutoBalancerService::CRAWL;  break;
-  case GALLOP: i_param.default_gait_type = OpenHRP::AutoBalancerService::GALLOP; break;
-  default: break;
-  }
-  i_param.ik_limb_parameters.length(ee_vec.size());
-  for (size_t i = 0; i < ee_vec.size(); i++) {
-      ABCIKparam& param = ikp[ee_vec[i]];
-      OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i];
-      ilp.ik_optional_weight_vector.length(param.manip->numJoints());
-      std::vector<double> ov;
-      ov.resize(param.manip->numJoints());
-      param.manip->getOptionalWeightVector(ov);
-      for (size_t j = 0; j < param.manip->numJoints(); j++) {
-          ilp.ik_optional_weight_vector[j] = ov[j];
-      }
-      ilp.sr_gain = param.manip->getSRGain();
-      ilp.avoid_gain = param.avoid_gain;
-      ilp.reference_gain = param.reference_gain;
-      ilp.manipulability_limit = param.manip->getManipulabilityLimit();
-  }
-  return true;
-};
-
-void AutoBalancer::copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs)
-{
-  memcpy(out_fs.pos, in_fs.pos.data(), sizeof(double)*3);
-  Eigen::Quaternion<double> qt(in_fs.rot);
-  out_fs.rot[0] = qt.w();
-  out_fs.rot[1] = qt.x();
-  out_fs.rot[2] = qt.y();
-  out_fs.rot[3] = qt.z();
-};
-
-bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param)
-{
-  copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_leg_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.swing_leg_dst_coords, gg->get_swing_leg_dst_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.dst_foot_midcoords, gg->get_dst_foot_midcoords());
-  if (gg->get_support_leg_names().front() == "rleg") {
-    i_param.support_leg = OpenHRP::AutoBalancerService::RLEG;
-  } else {
-    i_param.support_leg = OpenHRP::AutoBalancerService::LLEG;
-  }
-  switch ( gg->get_current_support_states().front() ) {
-  case BOTH: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::BOTH; break;
-  case RLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::RLEG; break;
-  case LLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::LLEG; break;
-  default: break;
-  }
-  return true;
-};
-
-bool AutoBalancer::adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep)
-{
-  std::cerr << "[" << m_profile.instance_name << "] adjustFootSteps" << std::endl;
-  if (control_mode == MODE_ABC && !gg_is_walking && adjust_footstep_interpolator->isEmpty()) {
-      Guard guard(m_mutex);
-      //
-      hrp::Vector3 eepos, org_mid_rpy, target_mid_rpy;
-      hrp::Matrix33 eerot, tmprot;
-      coordinates org_mid_coords, target_mid_coords;
-      // Get org coords
-      ikp["rleg"].adjust_interpolation_org_p0 = ikp["rleg"].target_p0;
-      ikp["lleg"].adjust_interpolation_org_p0 = ikp["lleg"].target_p0;
-      ikp["rleg"].adjust_interpolation_org_r0 = ikp["rleg"].target_r0;
-      ikp["lleg"].adjust_interpolation_org_r0 = ikp["lleg"].target_r0;
-      mid_coords(org_mid_coords, 0.5,
-                 coordinates(ikp["rleg"].adjust_interpolation_org_p0, ikp["rleg"].adjust_interpolation_org_r0),
-                 coordinates(ikp["lleg"].adjust_interpolation_org_p0, ikp["lleg"].adjust_interpolation_org_r0));
-      org_mid_rpy = hrp::rpyFromRot(org_mid_coords.rot);
-      // Get target coords
-      //   Input : ee coords
-      //   Output : link coords
-      memcpy(eepos.data(), rfootstep.pos, sizeof(double)*3);
-      eerot = (Eigen::Quaternion<double>(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
-      ikp["rleg"].adjust_interpolation_target_r0 = eerot * ikp["rleg"].localR.transpose();
-      ikp["rleg"].adjust_interpolation_target_p0 = eepos - ikp["rleg"].adjust_interpolation_target_r0 * ikp["rleg"].localPos;
-      memcpy(eepos.data(), lfootstep.pos, sizeof(double)*3);
-      eerot = (Eigen::Quaternion<double>(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
-      ikp["lleg"].adjust_interpolation_target_r0 = eerot * ikp["lleg"].localR.transpose();
-      ikp["lleg"].adjust_interpolation_target_p0 = eepos - ikp["lleg"].adjust_interpolation_target_r0 * ikp["lleg"].localPos;
-      mid_coords(target_mid_coords, 0.5,
-                 coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0),
-                 coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
-      coordinates rtrans, ltrans;
-      target_mid_coords.transformation(rtrans, coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0));
-      target_mid_coords.transformation(ltrans, coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
-      target_mid_rpy = hrp::rpyFromRot(target_mid_coords.rot);
-      // Fix target pos => org pos, target yaw => org yaw
-      target_mid_rpy(2) = org_mid_rpy(2);
-      target_mid_coords.rot = hrp::rotFromRpy(target_mid_rpy);
-      target_mid_coords.pos = org_mid_coords.pos;
-      // Calculate rleg and lleg coords
-      coordinates tmpc;
-      tmpc = target_mid_coords;
-      tmpc.transform(rtrans);
-      ikp["rleg"].adjust_interpolation_target_p0 = tmpc.pos;
-      ikp["rleg"].adjust_interpolation_target_r0 = tmpc.rot;
-      tmpc = target_mid_coords;
-      tmpc.transform(ltrans);
-      ikp["lleg"].adjust_interpolation_target_p0 = tmpc.pos;
-      ikp["lleg"].adjust_interpolation_target_r0 = tmpc.rot;
-      // Set interpolator
-      adjust_footstep_interpolator->clear();
-      double tmp = 0.0;
-      adjust_footstep_interpolator->set(&tmp);
-      tmp = 1.0;
-      adjust_footstep_interpolator->go(&tmp, adjust_footstep_transition_time, true);
-  }
-  while (!adjust_footstep_interpolator->isEmpty() )
-    usleep(1000);
-  usleep(1000);
-  return true;
-};
-
-bool AutoBalancer::getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx)
-{
-    std::cerr << "[" << m_profile.instance_name << "] getRemainingFootstepSequence" << std::endl;
-    o_footstep = new OpenHRP::AutoBalancerService::FootstepSequence;
-    if (gg_is_walking) {
-        std::vector< std::vector<step_node> > fsnl = gg->get_remaining_footstep_nodes_list();
-        o_current_fs_idx = gg->get_footstep_index();
-        o_footstep->length(fsnl.size());
-        for (size_t i = 0; i < fsnl.size(); i++) {
-            o_footstep[i].leg = (fsnl[i].front().l_r==RLEG?"rleg":"lleg");
-            copyRatscoords2Footstep(o_footstep[i], fsnl[i].front().worldcoords);
-        }
-    }
-};
-
-bool AutoBalancer::getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep)
-{
-    std::cerr << "[" << m_profile.instance_name << "] getGoPosFootstepsSequence" << std::endl;
-    o_footstep = new OpenHRP::AutoBalancerService::FootstepsSequence;
-    if (gg_is_walking) {
-        std::cerr << "[" << m_profile.instance_name << "] Cannot call getGoPosFootstepsSequence in walking" << std::endl;
-        return false;
-    } else {
-        gg->set_all_limbs(leg_names);
-        std::vector< std::vector<step_node> > new_footstep_nodes_list;
-        coordinates start_ref_coords;
-        std::vector<coordinates> initial_support_legs_coords;
-        std::vector<leg_type> initial_support_legs;
-        bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
-        if (is_valid_gait_type == false) return false;
-        /* go_pos_param_2_footstep_nodes_list_core is const member function  */
-        gg->go_pos_param_2_footstep_nodes_list_core (x, y, th,
-                                                     initial_support_legs_coords, start_ref_coords, initial_support_legs,
-                                                     new_footstep_nodes_list, true, 0);
-        o_footstep->length(new_footstep_nodes_list.size());
-        for (size_t i = 0; i < new_footstep_nodes_list.size(); i++) {
-            o_footstep[i].fs.length(new_footstep_nodes_list.at(i).size());
-            for (size_t j = 0; j < new_footstep_nodes_list.at(i).size(); j++) {
-                leg_type tmp_leg_type = new_footstep_nodes_list.at(i).at(j).l_r;
-                o_footstep[i].fs[j].leg = ((tmp_leg_type == RLEG) ? "rleg":
-                                           (tmp_leg_type == LLEG) ? "lleg":
-                                           (tmp_leg_type == RARM) ? "rarm":
-                                           "larm");
-                copyRatscoords2Footstep(o_footstep[i].fs[j], new_footstep_nodes_list.at(i).at(j).worldcoords);
-            }
-        }
-        return true;
-    }
-};
-
-void AutoBalancer::static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height)
-{
-  hrp::Vector3 target_sbp = hrp::Vector3(0, 0, 0);
-  hrp::Vector3 tmpcog = m_robot->calcCM();
-  switch ( use_force ) {
-  case MODE_REF_FORCE:
-    calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height, ref_forces);
-    tmp_input_sbp = target_sbp - sbp_offset;
-    sbp_cog_offset = tmp_input_sbp - tmpcog;
-    break;
-  case MODE_NO_FORCE:
-    tmp_input_sbp = tmpcog + sbp_cog_offset;
-    break;
-  default: break;
-  }
-};
-
-void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces)
-{
-  hrp::Vector3 denom, nume;
-  /* sb_point[m] = nume[kg * m/s^2 * m] / denom[kg * m/s^2] */
-  double mass = m_robot->totalMass();
-  for (size_t j = 0; j < 2; j++) {
-    nume(j) = mass * gg->get_gravitational_acceleration() * tmpcog(j);
-    denom(j) = mass * gg->get_gravitational_acceleration();
-    for (size_t i = 0; i < sensor_names.size(); i++) {
-      if ( sensor_names[i].find("hsensor") != std::string::npos || sensor_names[i].find("asensor") != std::string::npos ) { // tempolary to get arm force coords
-          hrp::Link* parentlink;
-          hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
-          if (sensor) parentlink = sensor->link;
-          else parentlink = m_vfs[sensor_names[i]].link;
-          for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-              if (it->second.target_link->name == parentlink->name) {
-                  hrp::Vector3 fpos = parentlink->p + parentlink->R * it->second.localPos;
-                  nume(j) += ( (fpos(2) - ref_com_height) * tmp_forces[i](j) - fpos(j) * tmp_forces[i](2) );
-                  denom(j) -= tmp_forces[i](2);
-              }
-          }
-      }
-    }
-    sb_point(j) = nume(j) / denom(j);
-  }
-  sb_point(2) = ref_com_height;
-};
-
-#ifndef rad2deg
-#define rad2deg(rad) (rad * 180 / M_PI)
-#endif
-#ifndef deg2rad
-#define deg2rad(deg) (deg * M_PI / 180)
-#endif
-
-hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_coords)
-{
-  if (graspless_manip_mode) {
-    hrp::Vector3 dp,dr;
-    coordinates ref_hand_coords(gg->get_dst_foot_midcoords()), act_hand_coords;
-    ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords
-    hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos);
-    if ( graspless_manip_arm == "arms" ) {
-        hrp::Vector3 rarm_pos = ikp["rarm"].target_p0 + ikp["rarm"].target_r0 * ikp["rarm"].localPos;
-        hrp::Vector3 larm_pos = ikp["larm"].target_p0 + ikp["larm"].target_r0 * ikp["larm"].localPos;
-        act_hand_coords.pos = (rarm_pos+larm_pos)/2.0;
-        hrp::Vector3 act_y = larm_pos-rarm_pos;
-        act_y(2) = 0;
-        act_y.normalize();
-        hrp::Vector3 ref_y(ref_hand_coords.rot * hrp::Vector3::UnitY());
-        ref_y(2) = 0;
-        ref_y.normalize();
-        dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y))); // fix for rotation
-    } else {
-      ABCIKparam& tmpikp = ikp[graspless_manip_arm];
-      act_hand_coords = rats::coordinates(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos,
-                                          tmpikp.target_r0 * tmpikp.localR);
-      rats::difference_rotation(dr, ref_hand_coords.rot, act_hand_coords.rot);
-      dr(0) = 0; dr(1) = 0;
-    }
-    dp = act_hand_coords.pos - ref_hand_coords.pos
-        + dr.cross(hrp::Vector3(foot_pos - act_hand_coords.pos));
-    dp(2) = 0;
-    hrp::Matrix33 foot_mt(gg->get_dst_foot_midcoords().rot.transpose());
-    //alias(dp) = foot_mt * dp;
-    hrp::Vector3 dp2 = foot_mt * dp;
-    //alias(dr) = foot_mt * dr;
-    return hrp::Vector3(graspless_manip_p_gain[0] * dp2(0)/gg->get_default_step_time(),
-                        graspless_manip_p_gain[1] * dp2(1)/gg->get_default_step_time(),
-                        graspless_manip_p_gain[2] * rad2deg(dr(2))/gg->get_default_step_time());
-  } else {
-    return hrp::Vector3::Zero();
-  }
-};
-
-bool AutoBalancer::calc_inital_support_legs(const double& y, std::vector<coordinates>& initial_support_legs_coords, std::vector<leg_type>& initial_support_legs, coordinates& start_ref_coords) {
-    switch(gait_type) {
-    case BIPED:
-        initial_support_legs_coords = (y > 0 ?
-                                       boost::assign::list_of(ikp["rleg"].target_end_coords)
-                                       : boost::assign::list_of(ikp["lleg"].target_end_coords));
-        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
-        break;
-    case TROT:
-        initial_support_legs_coords = (y > 0 ?
-                                       boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["larm"].target_end_coords)
-                                       : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["rarm"].target_end_coords));
-        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
-        break;
-    case PACE:
-        initial_support_legs_coords = (y > 0 ?
-                                       boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["rarm"].target_end_coords)
-                                       : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["larm"].target_end_coords));
-        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
-        break;
-    case CRAWL:
-        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    case GALLOP:
-        /* at least one leg shoud be in contact */
-        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    default: break;
-    }
-    mid_coords(start_ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
-    return true;
-};
-
-//
-extern "C"
-{
-
-    void AutoBalancerInit(RTC::Manager* manager)
-    {
-        RTC::Properties profile(autobalancer_spec);
-        manager->registerFactory(profile,
-                                 RTC::Create<AutoBalancer>,
-                                 RTC::Delete<AutoBalancer>);
-    }
-
-};
-
-
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 451eebeb739..30f7c2358d7 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -161,6 +161,7 @@ class HumanSynchronizer{
     hrp::Vector3 r_zmp_raw;
 
   public:
+    double tgt_FUP_TIME;
     bool startCountdownForHumanSync;
     bool ht_first_call;
     bool is_rf_contact,is_lf_contact;//足上げ高さ0に到達しているか否か
@@ -193,7 +194,7 @@ class HumanSynchronizer{
       go_rf_landing = go_lf_landing = false;
       pre_cont_rfpos = pre_cont_lfpos = hrp::Vector3::Zero();
       init_hp_calibcom.Zero();
-      FUP_TIME = 0.4;
+      tgt_FUP_TIME = FUP_TIME = 0.4;
       FUP_HIGHT = 0.05;
       CNT_F_TH = 20.0;
       MAXVEL = 0.004;
@@ -396,7 +397,8 @@ class HumanSynchronizer{
       }
     }
     void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, int& cur_fup_count_in, hrp::Vector3& f_height_out, bool& is_f_contact_out){
-      int FUP_COUNT = (int)(HZ*0.4);
+      int FUP_COUNT = (int)(HZ*FUP_TIME);
+      if(cur_fup_count_in > FUP_COUNT){cur_fup_count_in = FUP_COUNT;} if(cur_fup_count_in < 0){cur_fup_count_in = 0;}
       if(!go_land_in){//足下げ命令入力時
         if(cur_fup_count_in < FUP_COUNT){ cur_fup_count_in++; }
       }else{//足上げ命令入力時
@@ -406,6 +408,7 @@ class HumanSynchronizer{
       is_f_contact_out = (cur_fup_count_in <= 0);
     }
     void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
+      if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足設置しているタイミングでのみ更新
       calcFootUpCurveAndJudgeFootContact(rf_goland_in, cur_rfup_level, tgt.rf, is_rf_contact);
       calcFootUpCurveAndJudgeFootContact(lf_goland_in, cur_lfup_level, tgt.lf, is_lf_contact);
     }
@@ -647,6 +650,7 @@ class AutoBalancer
   bool startHumanSyncAfter5sec();
   bool setHumanToRobotRatio(const double h2r);
   bool setCOMMoveModRatio(const double cmr);
+  bool setFootUpTime(const double fupt);
   bool setAllowedXYZSync(const bool x_on,const bool y_on,const bool z_on);
   bool stopHumanSync();
 
diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.cpp b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
index 3da2b57390e..6e839832eba 100644
--- a/rtc/AutoBalancer/AutoBalancerService_impl.cpp
+++ b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
@@ -130,6 +130,11 @@ CORBA::Boolean AutoBalancerService_impl::setCOMMoveModRatio(const CORBA::Double
     return m_autobalancer->setCOMMoveModRatio(cmmr);
 };
 
+CORBA::Boolean AutoBalancerService_impl::setFootUpTime(const CORBA::Double fupt)
+{
+    return m_autobalancer->setFootUpTime(fupt);
+};
+
 CORBA::Boolean AutoBalancerService_impl::setAllowedXYZSync(const CORBA::Boolean x_on,const CORBA::Boolean y_on,const CORBA::Boolean z_on)
 {
     return m_autobalancer->setAllowedXYZSync(x_on,y_on,z_on);
diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.h b/rtc/AutoBalancer/AutoBalancerService_impl.h
index 89792b47632..3a77d491343 100644
--- a/rtc/AutoBalancer/AutoBalancerService_impl.h
+++ b/rtc/AutoBalancer/AutoBalancerService_impl.h
@@ -37,6 +37,7 @@ class AutoBalancerService_impl
   CORBA::Boolean startHumanSyncAfter5sec();
   CORBA::Boolean setHumanToRobotRatio(const CORBA::Double h2r);
   CORBA::Boolean setCOMMoveModRatio(const CORBA::Double cmmr);
+  CORBA::Boolean setFootUpTime(const CORBA::Double fupt);
   CORBA::Boolean setAllowedXYZSync(const CORBA::Boolean x_on,const CORBA::Boolean y_on,const CORBA::Boolean z_on);
   CORBA::Boolean stopHumanSync();
   //

From bb62fa1634db8a67b4950c387003bec8dac8c8d1 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 3 Dec 2016 03:40:21 +0900
Subject: [PATCH 034/217] enable ZMP/CP acc/vel safety

---
 rtc/AutoBalancer/AutoBalancer.cpp | 100 ++++++++++-
 rtc/AutoBalancer/AutoBalancer.h   | 276 +++++++++++++++++++++++++-----
 2 files changed, 330 insertions(+), 46 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 86561afe35c..765be5954ee 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -375,7 +375,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
 
     //for HumanSynchronizer
     hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer());
-    if(leg_pos.size()>=2){ hsp->init_wld_rp_rfpos = leg_pos[0]; hsp->init_wld_rp_lfpos = leg_pos[1]; }
+    if(leg_pos.size()>=2){ hsp->init_wld_rp_rfeepos = leg_pos[0]; hsp->init_wld_rp_lfeepos = leg_pos[1]; }
 
     hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
     if (sen == NULL) {
@@ -1078,6 +1078,18 @@ void AutoBalancer::solveLimbIK ()
   m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog;
   m_robot->rootLink()->R = target_root_R;
 
+////////////////////  逆動力学補償テスト  /////////////////////
+//  hrp::Matrix33 rot;
+//  Eigen::Matrix3f AxisAngle;
+//  Eigen::Vector3f axis(1,0,0);
+//  static int loop_l = 0;
+//  AxisAngle=Eigen::AngleAxisf(0.2*sin(2*M_PI*loop_l*m_dt),axis);  //Z軸周りに90度反時計回りに回転
+//  m_robot->rootLink()->R << AxisAngle(0,0), AxisAngle(0,1), AxisAngle(0,2), AxisAngle(1,0), AxisAngle(1,1), AxisAngle(1,2), AxisAngle(2,0), AxisAngle(2,1), AxisAngle(2,2);
+//if(loop>4000)loop_l++;
+//  if(loop_l>250)loop_l=250;
+////////////////////  逆動力学補償テスト  /////////////////////
+
+
 
   //for HumanSynchronizer
   if(hsp->startCountdownForHumanSync){
@@ -1142,6 +1154,9 @@ void AutoBalancer::solveLimbIK ()
 		  if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "[" << m_profile.instance_name << "] COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
 	  }
 
+
+
+
     //outport用のデータ上書き
     ref_zmp = hsp->rp_ref_out.zmp + hsp->rp_wld_initpos.zmp;
     ref_cog = hsp->rp_ref_out.com + hsp->rp_wld_initpos.com;
@@ -1154,6 +1169,89 @@ void AutoBalancer::solveLimbIK ()
 	  }
   }
   if (gg_is_walking && !gg_solved) stopWalking ();
+
+
+
+
+
+
+  ////////////////////  逆動力学補償テスト  /////////////////////
+//  //  for(int i=0;i<m_robot->numLinks();i++)cout<<"m_robot->link("<<i<<")->name: "<<m_robot->link(i)->name<<endl;
+//  static std::vector<double> q,q_old,dq,dq_old,ddq,ddq_f;
+//  static hrp::Vector3 base_acc(0,0,0),base_pos(0,0,0),base_pos_old(0,0,0),base_vel(0,0,0),base_vel_old(0,0,0);
+//
+//  static bool first = true;
+//  if(first){
+//    q.resize(m_robot->numJoints());
+//    q_old.resize(m_robot->numJoints());
+//    dq.resize(m_robot->numJoints());
+//    dq_old.resize(m_robot->numJoints());
+//    ddq.resize(m_robot->numJoints());
+//    ddq_f.resize(m_robot->numJoints(),0);
+//    for(int i=0;i<m_robot->numJoints();i++)q[i] = m_robot->joint(i)->q;
+//    q_old=q;
+//    for(int i=0;i<m_robot->numJoints();i++)dq[i] = (q[i] - q_old[i])/m_dt;
+//    dq_old=dq;
+//    first=false;
+//  }
+//
+//  for(int i=0;i<m_robot->numJoints();i++)q[i] = m_robot->joint(i)->q;
+//  for(int i=0;i<m_robot->numJoints();i++)dq[i] = (q[i] - q_old[i])/m_dt;
+//  for(int i=0;i<m_robot->numJoints();i++)ddq[i] = (dq[i] - dq_old[i])/m_dt;
+//  q_old = q;
+//  dq_old = dq;
+//  for(int i=0;i<m_robot->numJoints();i++)m_robot->joint(i)->dq = dq[i];
+//
+//  for(int i=0;i<m_robot->numJoints();i++)ddq_f[i] = 0.99*ddq_f[i] + 0.01*ddq[i];
+//  for(int i=0;i<m_robot->numJoints();i++)m_robot->joint(i)->ddq = ddq_f[i];
+//
+//base_pos = m_robot->rootLink()->p;
+//  base_vel = (base_pos - base_pos_old)/m_dt;
+//  base_acc = 0.99*base_acc + 0.01*(base_vel - base_vel_old)/m_dt;
+//  base_pos_old = base_pos;
+//  base_vel_old = base_vel;
+//
+////    cout<<"m_robot->link(\"RLEG_JOINT1\")->ddq: "<<m_robot->link("RLEG_JOINT1")->ddq<<endl;
+////    cout<<"m_robot->link(\"RLEG_JOINT1\")->dq: "<<m_robot->link("RLEG_JOINT1")->dq<<endl;
+////    cout<<"m_robot->link(\"RLEG_JOINT1\")->q: "<<m_robot->link("RLEG_JOINT1")->q<<endl;
+//
+//  m_robot->calcForwardKinematics(true,true);
+//
+//    static hrp::Vector3 fff,ttt;
+//    m_robot->calcInverseDynamics(m_robot->rootLink(),fff,ttt);
+//    hrp::Vector3 g(0,0,9.80665);
+////    m_robot->rootLink()->dvo = base_acc;
+//    cout<<"m_robot->rootLink()->dvo\n"<<m_robot->rootLink()->dvo<<endl;
+//    cout<<"m_robot->rootLink()->dw\n"<<m_robot->rootLink()->dw<<endl;
+//
+//    cout<<"calcInverseDynamics["<<loop<<"]\nfff\n"<<fff<<"\nttt\n"<<ttt<<endl;
+//    static hrp::Vector3 fff_f = hrp::Vector3::Zero(),ttt_f = hrp::Vector3::Zero();
+////    fff_f = 0.99*fff_f + 0.01*fff;
+////    ttt_f = 0.99*ttt_f + 0.01*ttt;
+//    //    fff_f = 0.95*fff_f + 0.05*fff;
+//    //    ttt_f = 0.95*ttt_f + 0.05*ttt;
+//        fff_f =fff;
+//        ttt_f =ttt;
+//
+//    hrp::Vector3 ppp,lll;
+//
+//    m_robot->calcTotalMomentum(ppp,lll);
+//    cout<<"calcTotalMomentum\nppp\n"<<ppp<<"\nlll\n"<<lll<<endl;
+//
+////    ref_zmp(0) += ( - 0*ppp(0)*m_robot->calcCM()(2) - lll(1))/(m_robot->totalMass()*9.8);
+////    ref_zmp(1) += ( - 0*ppp(1)*m_robot->calcCM()(2) - lll(0))/(m_robot->totalMass()*9.8);
+////      ref_zmp(0) += ( - fff_f(0)*m_robot->calcCM()(2)*0 + ttt_f(1))/(m_robot->totalMass()*9.8)*1;
+////      ref_zmp(1) += ( - fff_f(1)*m_robot->calcCM()(2)*0 - ttt_f(0))/(m_robot->totalMass()*9.8)*1;
+//
+////    if(hsp->sr_log!=NULL)fprintf(hsp->sr_log,"%f %f %f %f %f\n",m_robot->link("WAIST")->p(0),fff(0),ttt(0),ppp(0),lll(0));
+//    if(hsp->sr_log!=NULL)fprintf(hsp->sr_log,"%f %f %f %f %f %f %f\n",(hrp::rpyFromRot(m_robot->link("WAIST")->R)(0))*100,fff_f(1),ttt_f(0),ppp(1),lll(0),ref_zmp(1)*1000,m_dt);
+
+    ////////////////////  逆動力学補償テスト  /////////////////////
+
+
+
+
+
 }
 
 /*
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 30f7c2358d7..cabf6c758e8 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -36,7 +36,7 @@
 // <rtc-template block="consumer_stub_h">
 
 //実機だとログ出力で落ちる？getenv("HOME")ダメ？
-#define DEBUG 0
+#define DEBUG 1
 
 
 class BiquadIIRFilterVec
@@ -74,6 +74,13 @@ class BiquadIIRFilterVec
     };
 };
 
+class margin_t{
+  public:
+    hrp::Vector3 pt;
+    double x_u, x_l, y_u, y_l;
+    margin_t(){pt = hrp::Vector3::Zero(); x_u = x_l = y_u = y_l = 0;}
+    bool isRagulated(){return (x_u == 0 || x_l == 0 || y_u == 0 || y_l == 0);}
+};
 
 
 
@@ -151,13 +158,13 @@ class HumanSynchronizer{
     int countdown_num;
     bool HumanSyncOn;
     struct timeval t_calc_start, t_calc_end;
-    unsigned int loop;
     double h2r_ratio, tgt_h2r_ratio;
     double cmmr, tgt_cmmr;
     double HZ,DT,G;
     hrp::Vector3 com_old,com_oldold,comacc;
+    hrp::Vector3 comacc_ref;
     std::vector<BiquadIIRFilterVec> iir_v_filters;
-    BiquadIIRFilterVec comacc_v_filters;
+    BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters;
     hrp::Vector3 r_zmp_raw;
 
   public:
@@ -171,9 +178,11 @@ class HumanSynchronizer{
     HumanPose rp_ref_out;
     FILE *sr_log,*cz_log;
     bool use_rh,use_lh;
-    hrp::Vector3 init_wld_rp_rfpos,init_wld_rp_lfpos;
+    hrp::Vector3 init_wld_rp_rfeepos,init_wld_rp_lfeepos;
     hrp::Vector3 init_wld_hp_rfpos,init_wld_hp_lfpos;
     hrp::Vector3 current_basepos,init_basepos;
+    hrp::Vector3 com_vel_old;
+    unsigned int loop;
 
 
     interpolator *init_zmp_com_offset_interpolator;
@@ -193,7 +202,7 @@ class HumanSynchronizer{
       is_rf_contact = is_lf_contact = true;
       go_rf_landing = go_lf_landing = false;
       pre_cont_rfpos = pre_cont_lfpos = hrp::Vector3::Zero();
-      init_hp_calibcom.Zero();
+      init_hp_calibcom = hrp::Vector3::Zero();
       tgt_FUP_TIME = FUP_TIME = 0.4;
       FUP_HIGHT = 0.05;
       CNT_F_TH = 20.0;
@@ -204,22 +213,26 @@ class HumanSynchronizer{
       countdown_num = 5*HZ;
       loop = 0;
 
+      //////////  hrp::Vector は初期化必要  ///////////
       com_old = com_oldold = comacc = hrp::Vector3::Zero();
+      comacc_ref = hrp::Vector3::Zero();
       r_zmp_raw = hrp::Vector3::Zero();
 
-      init_wld_rp_rfpos = init_wld_rp_lfpos = hrp::Vector3::Zero();
+      init_wld_rp_rfeepos = init_wld_rp_lfeepos = hrp::Vector3::Zero();
       init_wld_hp_rfpos = init_wld_hp_lfpos = hrp::Vector3::Zero();
       current_basepos = init_basepos = hrp::Vector3::Zero();
 
+      com_vel_old= hrp::Vector3::Zero();
+
       iir_v_filters.resize(5);
-      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(0.5,HZ);
-      comacc_v_filters.setParameter(0.5,HZ);
+      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(0.5,HZ);//重心＋四肢拘束点用
+      calcacc_v_filters.setParameter(5,HZ);//加速度計算用
+      acc4zmp_v_filters.setParameter(0.5,HZ);//ZMP生成用ほぼこの値でいい
 
-//      use_rh = use_lh = false;
       use_rh = use_lh = true;
 
-//      zmp_com_offset_ip_ratio = 0;
-
+      rp_ref_out_old.clear();
+      rp_ref_out.clear();
 
       init_zmp_com_offset_interpolator = new interpolator(1, DT, interpolator::HOFFARBIB, 1);
       init_zmp_com_offset_interpolator->setName("zmp_com_offset_interpolator");
@@ -229,9 +242,11 @@ class HumanSynchronizer{
       init_zmp_com_offset_interpolator->go(&goal_ratio, 3.0, true);//3s
 
       if(DEBUG){
-        std::string home_path(std::getenv("HOME"));
-        sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
-        cz_log = fopen((home_path+"/HumanSync_com_zmp.log").c_str(),"w+");
+//        std::string home_path(std::getenv("HOME"));
+//        sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
+//        cz_log = fopen((home_path+"/HumanSync_com_zmp.log").c_str(),"w+");
+        sr_log = fopen("/home/ishiguro/HumanSync_support_region.log","w+");
+        cz_log = fopen("/home/ishiguro/HumanSync_com_zmp.log","w+");
       }
       cout<<"HumanSynchronizer constructed"<<endl;
     }
@@ -265,34 +280,26 @@ class HumanSynchronizer{
       applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);
       lockFootXYOnContact                 (go_rf_landing, go_lf_landing, hp_rel_raw);//根本から改変すべき2
       hp_filtered = hp_rel_raw;
-
-//      applyLPFilterToEE(hp_rel_raw, hp_filtered);//次に使う.rf,.lfのためにEEだけは先にフィルターしておかないといけない説もある
       calcWorldZMP                        ((hp_filtered.rf+init_wld_hp_rfpos), (hp_filtered.lf+init_wld_hp_lfpos), hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
       convertRelHumanPoseToRelRobotPose   (hp_filtered,rp_ref_out);
       applyCOMMoveModRatio                (rp_ref_out);
       judgeFootLandOnCommand              (hp_wld_raw.rfw, hp_wld_raw.lfw, go_rf_landing, go_lf_landing);//frwはすでに1000->100Hzにフィルタリングされている
-
       applyEEWorkspaceLimit               (rp_ref_out);
-
-      applyCOMToSupportRegionLimit        (rp_ref_out.rf+init_wld_rp_rfpos, rp_ref_out.lf+init_wld_rp_lfpos, rp_ref_out.com);
-
+      applyCOMToSupportRegionLimit        (rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
       applyLPFilter                       (rp_ref_out);
-//      applyLPFilter_o                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
+      applyCOMStateLimitByCapturePoint    (rp_ref_out.com, rp_ref_out_old.com, com_vel_old, rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
       r_zmp_raw = rp_ref_out.zmp;
       applyZMPCalcFromCOM                 (rp_ref_out.com,rp_ref_out.zmp);
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, go_rf_landing, go_lf_landing);//
-
       applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       applyCOMZMPXYZLock                  (rp_ref_out);
       overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
+      com_vel_old = (rp_ref_out.com - rp_ref_out_old.com)/DT;
       rp_ref_out_old = rp_ref_out;
-//      fprintf(log,"%f %f %f %f %f %f\n", rp_ref_out.com(0), rp_ref_out.com(1), rp_ref_out.zmp(0), rp_ref_out.zmp(1), hp_rel_raw.com(0), hp_rel_raw.com(1));
       loop++;
       gettimeofday(&t_calc_end, NULL);
     }
     void calibInitHumanCOMFromZMP(){
-//      init_hp_calibcom(0) = hp_filtered.zmp(0);
-//      init_hp_calibcom(1) = hp_filtered.zmp(1);
       init_hp_calibcom(0) = r_zmp_raw(0);
       init_hp_calibcom(1) = r_zmp_raw(1);
     }
@@ -327,8 +334,8 @@ class HumanSynchronizer{
       if(h2r_r_goal - h2r_ratio > step){h2r_ratio += step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
       else if(h2r_r_goal - h2r_ratio < (-1)*step){h2r_ratio -= step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
       else{h2r_ratio = h2r_r_goal;}
-      init_wld_hp_rfpos = init_wld_rp_rfpos/h2r_ratio;
-      init_wld_hp_lfpos = init_wld_rp_lfpos/h2r_ratio;
+      init_wld_hp_rfpos = init_wld_rp_rfeepos/h2r_ratio;
+      init_wld_hp_lfpos = init_wld_rp_lfeepos/h2r_ratio;
     }
     void updateCOMModRatio(const double cmmr_goal){
       const double step = 0.001;
@@ -384,15 +391,15 @@ class HumanSynchronizer{
     }
     void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
       if(rfcs_ans && !lfcs_ans){
-        if(tgt.zmp(1) > tgt.rf(1) + init_wld_rp_rfpos(1) + 0.02){
+        if(tgt.zmp(1) > tgt.rf(1) + init_wld_rp_rfeepos(1) + 0.02){
           lfcs_ans = true;
-          cout<<"LF lock"<<endl;
+          if(loop%100==0)cout<<"LF Auto lock"<<endl;
         }
       }
       if(!rfcs_ans && lfcs_ans){
-        if(tgt.zmp(1) < tgt.lf(1) + init_wld_rp_lfpos(1) - 0.02){
+        if(tgt.zmp(1) < tgt.lf(1) + init_wld_rp_lfeepos(1) - 0.02){
           rfcs_ans = true;
-          cout<<"RF lock"<<endl;
+          if(loop%100==0)cout<<"RF Auto lock"<<endl;
         }
       }
     }
@@ -492,7 +499,6 @@ class HumanSynchronizer{
           break;
         }
       }
-
       if(pt_state != PT_IN_REGION){
         int ans_lid=-1;
         for(int i=0;i<convex_hull.size()-1;i++){//対象の点がある線分への垂線を有するかチェックする
@@ -524,29 +530,209 @@ class HumanSynchronizer{
           pt_state = PT_FIX_TO_VERTEX;
         }
       }
+      comin_abs(0) = ans_point(0);
+      comin_abs(1) = ans_point(1);
+      return true;
+    }
 
-//      if(loop%50==0){//ログ
-//        if(pt_state != PT_IN_REGION){
-//          std::cout<<"COM out of Support Region pt_state="<<pt_state<<std::endl;
-//        }
-//        for(int i=0;i<convex_hull.size();i++){
-//          fprintf(sr_log,"%f %f %f\n",convex_hull[i](0), convex_hull[i](1),(double)loop/HZ);
+
+    bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& com_old, const hrp::Vector3& com_vel_old, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans){
+      com_ans = com_in;
+      hrp::Vector3 com_vel = (com_in - com_old)/DT;
+      hrp::Vector3 com_acc = (com_vel - com_vel_old)/DT;//命名規則
+      hrp::Vector3 com_vel_zmpOk, com_vel_zmpOk_cpOk;
+
+      comacc_ref = calcacc_v_filters.passFilter(com_acc);
+
+      const double H = rp_wld_initpos.com(2);
+
+      double maxacc[2],minacc[2];
+      maxacc[0] = (com_old(0) - (-0.05)) * (G/H); if(maxacc[0] < 0){maxacc[0]=0;}
+      minacc[0] = (com_old(0) - (0.10)) * (G/H); if(minacc[0] > 0){minacc[0]=0;}
+      maxacc[1] = (com_old(1) - (-0.04-0.1)) * (G/H); if(maxacc[1] < 0){maxacc[1]=0;}
+      minacc[1] = (com_old(1) - (0.04+0.1)) * (G/H); if(minacc[1] > 0){minacc[1]=0;}
+//      if(com_vel(0) > maxacc[0] * DT + com_vel_old(0)){com_vel(0) = maxacc[0] * DT + com_vel_old(0); cout<<"maxacc[0] over"<<endl;}
+//      if(com_vel(0) < minacc[0] * DT + com_vel_old(0)){com_vel(0) = minacc[0] * DT + com_vel_old(0); cout<<"minacc[0] over"<<endl;}
+//      if(com_vel(1) > maxacc[1] * DT + com_vel_old(1)){com_vel(1) = maxacc[1] * DT + com_vel_old(1); cout<<"maxacc[1] over"<<endl;}
+//      if(com_vel(1) < minacc[1] * DT + com_vel_old(1)){com_vel(1) = minacc[1] * DT + com_vel_old(1); cout<<"minacc[1] over"<<endl;}
+      if(comacc_ref(0) > maxacc[0]){com_vel(0) = maxacc[0] * DT + com_vel_old(0); cout<<"maxacc[0] over"<<endl;}
+      if(comacc_ref(0) < minacc[0]){com_vel(0) = minacc[0] * DT + com_vel_old(0); cout<<"minacc[0] over"<<endl;}
+      if(comacc_ref(1) > maxacc[1]){com_vel(1) = maxacc[1] * DT + com_vel_old(1); cout<<"maxacc[1] over"<<endl;}
+      if(comacc_ref(1) < minacc[1]){com_vel(1) = minacc[1] * DT + com_vel_old(1); cout<<"minacc[1] over"<<endl;}
+
+
+
+////      cout<<"maxacc "<<maxacc[1]<<" minacc "<<minacc[1]<< " acc " << (com_vel(1)-com_vel_old(1))/DT <<endl;
+//
+      hrp::Vector3 bb;
+      hrp::Vector3 cp_ragulated;
+
+      margin_t margin_from_4maxcp;
+      checkMarginToFootSupportRegion(com_old, com_old, rfin_abs, lfin_abs, 0.04, 0.02, 0.01, 0.01, margin_from_4maxcp, bb);
+      hrp::Vector3 max_com_vel,min_com_vel;
+      max_com_vel(0) = margin_from_4maxcp.x_u / sqrt( H / G );
+      min_com_vel(0) = margin_from_4maxcp.x_l / sqrt( H / G );
+      max_com_vel(1) = margin_from_4maxcp.y_u / sqrt( H / G );
+      min_com_vel(1) = margin_from_4maxcp.y_l / sqrt( H / G );
+      if(com_vel(0) > max_com_vel(0)){ com_vel(0) = max_com_vel(0); cout<<"F cp over"<<endl;}//ここブレーキ時のZMP条件に変えたほうがいいかもしれない
+      if(com_vel(0) < min_com_vel(0)){ com_vel(0) = min_com_vel(0); cout<<"B cp over"<<endl;}
+      if(com_vel(1) > max_com_vel(1)){ com_vel(1) = max_com_vel(1); cout<<"L cp over"<<endl;}
+      if(com_vel(1) < min_com_vel(1)){ com_vel(1) = min_com_vel(1); cout<<"R cp over"<<endl;}
+
+      com_ans(0) = com_old(0) + com_vel(0) * DT;
+      com_ans(1) = com_old(1) + com_vel(1) * DT;
+//      cout<<"max_com_vel(0)"<<max_com_vel(0)<<endl;
+//      cout<<"min_com_vel(0)"<<min_com_vel(0)<<endl;
+//      cout<<"com_vel(0)"<<com_vel(0)<<endl;
+//      cout<<"com_ans(0)"<<com_ans(0)<<endl;
+//      cout<<"max_com_vel(1)"<<max_com_vel(1)<<endl;
+//      cout<<"min_com_vel(1)"<<min_com_vel(1)<<endl;
+//      cout<<"com_vel(1)"<<com_vel(1)<<endl;
+//      cout<<"com_ans(1)"<<com_ans(1)<<endl;
+
+//      //ここからはcom_inがそんなに領域外に出ていない仮定
+//      checkMarginToFootSupportRegion(com_in, com_old, rfin_abs, lfin_abs, 0.04, 0.02, 0.01, 0.01, margin_from_4maxcp, cp_ragulated);
+//      hrp::Vector2 com_raw_vel2d,cp_ragulated_2d,com_old_2d;
+//      com_raw_vel2d(0) = (com_in(0) - com_old(0))/DT;
+//      com_raw_vel2d(1) = (com_in(1) - com_old(1))/DT;
+//      cp_ragulated_2d(0) = cp_ragulated(0);
+//      cp_ragulated_2d(1) = cp_ragulated(1);
+//      com_old_2d(0) = com_old(0);
+//      com_old_2d(1) = com_old(1);
+//      if( com_raw_vel2d.norm() * sqrt(H/G) > (cp_ragulated_2d - com_old_2d).norm()){
+//        com_raw_vel2d *= (cp_ragulated_2d - com_old_2d).norm()/( com_raw_vel2d.norm() * sqrt(H/G) );
+//        std::cerr << "CP work" << std::endl;
+//        com_ans(0) = com_old(0) + com_raw_vel2d(0) * DT;
+//        com_ans(1) = com_old(1) + com_raw_vel2d(1) * DT;
+//
+//      }
+      return true;
+    }
+
+
+    bool checkMarginToFootSupportRegion(const hrp::Vector3& pt_in, const hrp::Vector3& pt_old, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const double& forward_mgn, const double& back_mgn, const double& outer_mgn, const double& inner_mgn, margin_t& mgn_ans, hrp::Vector3& pt_ans){
+      std::vector<hrp::Vector2> convex_hull;
+      if(forward_mgn< -back_mgn || outer_mgn< -inner_mgn ){std::cerr << "Invalid margin input!" << std::endl;return false;}
+      if(lfin_abs(0)>rfin_abs(0)){
+        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) + outer_mgn));//LFの左下
+        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + forward_mgn, lfin_abs(1) + outer_mgn));//LFの左上
+        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + forward_mgn, lfin_abs(1) - inner_mgn));//LFの右上
+        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + forward_mgn, rfin_abs(1) - outer_mgn));//RFの右上
+        convex_hull.push_back(hrp::Vector2(rfin_abs(0) - back_mgn,    rfin_abs(1) - outer_mgn));//RFの右下
+        convex_hull.push_back(hrp::Vector2(rfin_abs(0) - back_mgn,    rfin_abs(1) + inner_mgn));//RFの左下
+        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) + outer_mgn));//LFの左下
+      }else{
+        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) - inner_mgn));//LFの右下
+        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) + outer_mgn));//LFの左下
+        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + forward_mgn, lfin_abs(1) + outer_mgn));//LFの左上
+        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + forward_mgn, rfin_abs(1) + inner_mgn));//RFの左上
+        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + forward_mgn, rfin_abs(1) - outer_mgn));//RFの右上
+        convex_hull.push_back(hrp::Vector2(rfin_abs(0) - back_mgn,    rfin_abs(1) - outer_mgn));//RFの右下
+        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) - inner_mgn));//LFの右下
+      }
+      hrp::Vector2 check_point(pt_in(0),pt_in(1));
+      hrp::Vector2 ans_point = check_point;
+      enum point_state_t {PT_IN_NO_PATTERN = -1, PT_IN_REGION, PT_FIX_TO_EDGE, PT_FIX_TO_VERTEX};
+      point_state_t pt_state = PT_IN_REGION;
+      for(int i=0;i<convex_hull.size()-1;i++){//対象の点が凸包の内部に存在するかチェックする
+        hrp::Vector2 cur_vert(convex_hull[i](0),convex_hull[i](1));
+        hrp::Vector2 next_vert(convex_hull[i+1](0),convex_hull[i+1](1));
+        hrp::Vector2 edge_v = next_vert - cur_vert;
+        hrp::Vector2 tgt_pt_v = check_point - cur_vert;
+        if((edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){  pt_state = PT_IN_NO_PATTERN; break; }
+      }
+      if(pt_state != PT_IN_REGION){
+        int ans_lid=-1;
+        for(int i=0;i<convex_hull.size()-1;i++){//対象の点がある線分への垂線を有するかチェックする
+          hrp::Vector2 cur_vert(convex_hull[i](0),convex_hull[i](1));
+          hrp::Vector2 next_vert(convex_hull[i+1](0),convex_hull[i+1](1));
+          hrp::Vector2 edge_v = next_vert - cur_vert;
+          hrp::Vector2 tgt_pt_v = check_point - cur_vert;
+          //ある線分への垂線を有し，かつ外側(時計回りエッジに対して左側)に存在するなら，対象の点からそのエッジへの垂線の交点が最近傍点
+          if(edge_v.dot(tgt_pt_v)/edge_v.norm() > 0 && edge_v.dot(tgt_pt_v)/edge_v.norm() < edge_v.norm() && (edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
+            ans_lid = i;
+            ans_point = cur_vert + edge_v.normalized() * (edge_v.dot(tgt_pt_v)/edge_v.norm());
+            pt_state = PT_FIX_TO_EDGE;
+            break;
+          }
+        }
+        if(pt_state != PT_FIX_TO_EDGE){//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
+          double cur_min_dis = (check_point - convex_hull[0]).norm();
+          int ans_pid = 0;
+          for(int i=1;i<convex_hull.size();i++){
+            if((check_point - convex_hull[i]).norm() < cur_min_dis){
+              cur_min_dis = (check_point - convex_hull[i]).norm();
+              ans_pid = i;
+            }
+          }
+          ans_point(0) = convex_hull[ans_pid](0);
+          ans_point(1) = convex_hull[ans_pid](1);
+          pt_state = PT_FIX_TO_VERTEX;
+        }
+      }
+      pt_ans(0) = ans_point(0);//領域はみ出ている場合この答えは点から領域への最近傍点(だからちょっと意味が違う)
+      pt_ans(1) = ans_point(1);//
+
+      hrp::Vector2 anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
+      hrp::Vector2 cross_pt;
+      double margin[4] = {0};
+      for(int direc=0;direc<4;direc++){
+        for(int i=0;i<convex_hull.size()-1;i++){
+          double dBunbo = anchor_vec[direc](0) * ( convex_hull[i+1](1) - convex_hull[i](1) ) - anchor_vec[direc](1) * ( convex_hull[i+1](0) - convex_hull[i](0) );
+          if( dBunbo != 0.0)  {//平行の場合を外す
+            hrp::Vector2 vectorAC = convex_hull[i] - ans_point;
+            double dR = ( ( convex_hull[i+1](1) - convex_hull[i](1) ) * vectorAC(0) - ( convex_hull[i+1](0) - convex_hull[i](0) ) * vectorAC(1) ) / dBunbo;
+            double dS = ( anchor_vec[direc](1) * vectorAC(0) - anchor_vec[direc](0) * vectorAC(1) ) / dBunbo;
+            //dR==0のときは含めないことで，点が境界上でも領域内側向きのマージンをチェックさせる
+            //dRの上限が無いのはアンカーベクトルの無限遠まで交点を許可するから(アンカーベクトルの長さは1で定義しているため)
+            //点が斜線境界上にあるとまれに dR = 8.67362e-18 とかいう0でない値がでたりする．
+            //恐らく点を斜線境界で打ち切った時に数値誤差で完全に斜線に乗り切らず，数値誤差分はみ出ている状況と思われる．
+            if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){
+              cross_pt = ans_point + dR * anchor_vec[direc];
+              margin[direc] = dR * anchor_vec[direc].norm();
+            }
+          }
+        }
+      }
+      mgn_ans.x_u = margin[0];
+      mgn_ans.x_l = -margin[1];
+      mgn_ans.y_u = margin[2];
+      mgn_ans.y_l = -margin[3];
+
+//      //X,Y独立のマージンチェックではなくXY平面内の二次元距離でマージンを計算したい
+//      hrp::Vector2 cmd_vec,pt_in_2d,pt_old_2d;
+//      pt_in_2d(0) = pt_in(0);      pt_in_2d(1) = pt_in(1);
+//      pt_old_2d(0) = pt_old(0);    pt_old_2d(1) = pt_old(1);
+//      cmd_vec = pt_in_2d - pt_old_2d;
+//      hrp::Vector2 cross_pt;
+//      double margin[4] = {0};
+//      for(int i=0;i<convex_hull.size()-1;i++){
+//        double dBunbo = cmd_vec(0) * ( convex_hull[i+1](1) - convex_hull[i](1) ) - cmd_vec(1) * ( convex_hull[i+1](0) - convex_hull[i](0) );
+//        if( dBunbo != 0.0)  {//平行の場合を外す
+//          hrp::Vector2 vectorAC = convex_hull[i] - pt_old_2d;
+//          double dR = ( ( convex_hull[i+1](1) - convex_hull[i](1) ) * vectorAC(0) - ( convex_hull[i+1](0) - convex_hull[i](0) ) * vectorAC(1) ) / dBunbo;
+//          double dS = ( cmd_vec(1) * vectorAC(0) - cmd_vec(0) * vectorAC(1) ) / dBunbo;
+//          //dR==0のときは含めないことで，点が境界上でも領域内側向きのマージンをチェックさせる
+//          //dRの上限が無いのはアンカーベクトルの無限遠まで交点を許可するから(アンカーベクトルの長さは1で定義しているため)
+//          //点が斜線境界上にあるとまれに dR = 8.67362e-18 とかいう0でない値がでたりする．
+//          //恐らく点を斜線境界で打ち切った時に数値誤差で完全に斜線に乗り切らず，数値誤差分はみ出ている状況と思われる．
+//          if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){
+//            cross_pt = pt_old_2d + dR * cmd_vec;
+//          }
 //        }
-//        fprintf(sr_log,"\n");
-//        fprintf(cz_log,"%f %f %f %f %f %f %f %f %f",(double)loop/HZ,comin_abs(0),comin_abs(1),comin_abs(2),rp_ref_out.zmp(0),rp_ref_out.zmp(1),rp_ref_out.zmp(2),ans_point(0),ans_point(1));
-//        fprintf(cz_log," %f %f %f %f",rp_ref_out.zmp(1),hpf_zmp(1),ans_point(1),ans_point(1) + hpf_zmp(1));
-//        fprintf(cz_log,"\n\n");
 //      }
-      comin_abs(0) = ans_point(0);
-      comin_abs(1) = ans_point(1);
+//      pt_ans(0) = cross_pt(0);
+//      pt_ans(1) = cross_pt(1);
+
       return true;
     }
+
     void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
       comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
       const double MAXACC = 5;
       if(comacc(0)>MAXACC)comacc(0)=MAXACC;else if(comacc(0)<-MAXACC)comacc(0)=-MAXACC;
       if(comacc(1)>MAXACC)comacc(1)=MAXACC;else if(comacc(1)<-MAXACC)comacc(1)=-MAXACC;
-      comacc = comacc_v_filters.passFilter(comacc);
+      comacc = acc4zmp_v_filters.passFilter(comacc);
       zmpout(0) = comin(0)-(rp_wld_initpos.com(2)/G)*comacc(0);
       zmpout(1) = comin(1)-(rp_wld_initpos.com(2)/G)*comacc(1);
       if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.zmp(0),rp_ref_out.zmp(1),zmpout(0),zmpout(1));

From 2d5e64467873268c227911b4cef34bc95b13eb79 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 5 Dec 2016 18:28:39 +0900
Subject: [PATCH 035/217] disable ZMP acc safety

---
 rtc/AutoBalancer/AutoBalancer.h | 30 +++++++++++++++++++-----------
 1 file changed, 19 insertions(+), 11 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index cabf6c758e8..ac16d8ccdf9 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -225,9 +225,10 @@ class HumanSynchronizer{
       com_vel_old= hrp::Vector3::Zero();
 
       iir_v_filters.resize(5);
-      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(0.5,HZ);//重心＋四肢拘束点用
+      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(1,HZ);//四肢拘束点用
+      iir_v_filters[0].setParameter(0.5,HZ);//重心用
       calcacc_v_filters.setParameter(5,HZ);//加速度計算用
-      acc4zmp_v_filters.setParameter(0.5,HZ);//ZMP生成用ほぼこの値でいい
+      acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
 
       use_rh = use_lh = true;
 
@@ -287,7 +288,6 @@ class HumanSynchronizer{
       applyEEWorkspaceLimit               (rp_ref_out);
       applyCOMToSupportRegionLimit        (rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
       applyLPFilter                       (rp_ref_out);
-      applyCOMStateLimitByCapturePoint    (rp_ref_out.com, rp_ref_out_old.com, com_vel_old, rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
       r_zmp_raw = rp_ref_out.zmp;
       applyZMPCalcFromCOM                 (rp_ref_out.com,rp_ref_out.zmp);
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, go_rf_landing, go_lf_landing);//
@@ -541,24 +541,27 @@ class HumanSynchronizer{
       hrp::Vector3 com_vel = (com_in - com_old)/DT;
       hrp::Vector3 com_acc = (com_vel - com_vel_old)/DT;//命名規則
       hrp::Vector3 com_vel_zmpOk, com_vel_zmpOk_cpOk;
+      double accel[4] = {0.05, -0.03, 0.02, -0.02};
+      double decel[4] = {0.04, -0.02, 0.01, -0.01};
 
       comacc_ref = calcacc_v_filters.passFilter(com_acc);
 
       const double H = rp_wld_initpos.com(2);
 
       double maxacc[2],minacc[2];
-      maxacc[0] = (com_old(0) - (-0.05)) * (G/H); if(maxacc[0] < 0){maxacc[0]=0;}
-      minacc[0] = (com_old(0) - (0.10)) * (G/H); if(minacc[0] > 0){minacc[0]=0;}
-      maxacc[1] = (com_old(1) - (-0.04-0.1)) * (G/H); if(maxacc[1] < 0){maxacc[1]=0;}
-      minacc[1] = (com_old(1) - (0.04+0.1)) * (G/H); if(minacc[1] > 0){minacc[1]=0;}
+      maxacc[0] = (com_old(0) - accel[1]) * (G/H); if(maxacc[0] < 0){maxacc[0]=0;}
+      minacc[0] = (com_old(0) - accel[0]) * (G/H); if(minacc[0] > 0){minacc[0]=0;}
+      maxacc[1] = (com_old(1) - (accel[3]-0.1)) * (G/H); if(maxacc[1] < 0){maxacc[1]=0;}
+      minacc[1] = (com_old(1) - (accel[2]+0.1)) * (G/H); if(minacc[1] > 0){minacc[1]=0;}
 //      if(com_vel(0) > maxacc[0] * DT + com_vel_old(0)){com_vel(0) = maxacc[0] * DT + com_vel_old(0); cout<<"maxacc[0] over"<<endl;}
 //      if(com_vel(0) < minacc[0] * DT + com_vel_old(0)){com_vel(0) = minacc[0] * DT + com_vel_old(0); cout<<"minacc[0] over"<<endl;}
 //      if(com_vel(1) > maxacc[1] * DT + com_vel_old(1)){com_vel(1) = maxacc[1] * DT + com_vel_old(1); cout<<"maxacc[1] over"<<endl;}
 //      if(com_vel(1) < minacc[1] * DT + com_vel_old(1)){com_vel(1) = minacc[1] * DT + com_vel_old(1); cout<<"minacc[1] over"<<endl;}
-      if(comacc_ref(0) > maxacc[0]){com_vel(0) = maxacc[0] * DT + com_vel_old(0); cout<<"maxacc[0] over"<<endl;}
-      if(comacc_ref(0) < minacc[0]){com_vel(0) = minacc[0] * DT + com_vel_old(0); cout<<"minacc[0] over"<<endl;}
-      if(comacc_ref(1) > maxacc[1]){com_vel(1) = maxacc[1] * DT + com_vel_old(1); cout<<"maxacc[1] over"<<endl;}
-      if(comacc_ref(1) < minacc[1]){com_vel(1) = minacc[1] * DT + com_vel_old(1); cout<<"minacc[1] over"<<endl;}
+//      if(comacc_ref(0) > maxacc[0]){com_vel(0) = maxacc[0] * DT + com_vel_old(0); cout<<"maxacc[0] over"<<endl;}
+//      if(comacc_ref(0) < minacc[0]){com_vel(0) = minacc[0] * DT + com_vel_old(0); cout<<"minacc[0] over"<<endl;}
+//      if(comacc_ref(1) > maxacc[1]){com_vel(1) = maxacc[1] * DT + com_vel_old(1); cout<<"maxacc[1] over"<<endl;}
+//      if(comacc_ref(1) < minacc[1]){com_vel(1) = minacc[1] * DT + com_vel_old(1); cout<<"minacc[1] over"<<endl;}
+//      cout<<"comacc_ref(1) "<<comacc_ref(1)<<" minacc[1] "<<minacc[1]<<endl;
 
 
 
@@ -579,8 +582,13 @@ class HumanSynchronizer{
       if(com_vel(1) > max_com_vel(1)){ com_vel(1) = max_com_vel(1); cout<<"L cp over"<<endl;}
       if(com_vel(1) < min_com_vel(1)){ com_vel(1) = min_com_vel(1); cout<<"R cp over"<<endl;}
 
+
+
       com_ans(0) = com_old(0) + com_vel(0) * DT;
       com_ans(1) = com_old(1) + com_vel(1) * DT;
+
+
+//      if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,com_ans(0),com_ans(1),com_vel(0),com_vel(1),margin_from_4maxcp.y_u,margin_from_4maxcp.y_l);
 //      cout<<"max_com_vel(0)"<<max_com_vel(0)<<endl;
 //      cout<<"min_com_vel(0)"<<min_com_vel(0)<<endl;
 //      cout<<"com_vel(0)"<<com_vel(0)<<endl;

From 16625c8c58ebf9c30d4481a6110237c6ffd8f379 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 5 Dec 2016 18:29:07 +0900
Subject: [PATCH 036/217] fix IIR setparam

---
 rtc/TorqueFilter/IIRFilter.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/rtc/TorqueFilter/IIRFilter.cpp b/rtc/TorqueFilter/IIRFilter.cpp
index 4b06c4faef6..2f08e1f5b11 100644
--- a/rtc/TorqueFilter/IIRFilter.cpp
+++ b/rtc/TorqueFilter/IIRFilter.cpp
@@ -37,6 +37,9 @@ bool IIRFilter::setParameter(int dim, std::vector<double>& A, std::vector<double
         std::cout << "[" <<  m_error_prefix << "]" << "IIRFilter coefficients size error" << std::endl;
         return false;
     }
+    // clear previous coefficients
+    m_fb_coefficients.clear();
+    m_ff_coefficients.clear();
 
     if (A.size() == dim) {
         m_fb_coefficients.push_back(1.0);

From fff3b7f8bc5e3420dd2b403e81dcb1a6e8a6a62e Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 15 Dec 2016 15:23:53 +0900
Subject: [PATCH 037/217] add foot to foot workspace limit

---
 rtc/AutoBalancer/AutoBalancer.cpp | 22 ++++++---
 rtc/AutoBalancer/AutoBalancer.h   | 82 ++++++++++++++++++++-----------
 2 files changed, 68 insertions(+), 36 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 765be5954ee..706971f62c4 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -59,6 +59,7 @@ AutoBalancer::AutoBalancer(RTC::Manager* manager)
       m_htrhIn("htrhIn", m_htrh),
       m_htlhIn("htlhIn", m_htlh),
       m_actzmpIn("actzmpIn", m_actzmp),
+      m_htcamIn("htcamIn", m_htcam),
 
       m_qOut("q", m_qRef),
       m_zmpOut("zmpOut", m_zmp),
@@ -112,6 +113,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     addInPort("htrhIn", m_htrhIn);
     addInPort("htlhIn", m_htlhIn);
     addInPort("actzmpIn", m_actzmpIn);
+    addInPort("htcamIn", m_htcamIn);
 
     // Set OutPort buffer
     addOutPort("q", m_qOut);
@@ -488,13 +490,14 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     }
     //for HumanSynchronizer
     if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
-    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); hp_raw_data.rfw = HumanSynchronizer::DoubleSeqToWrench6(m_htrfw); }
-    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); hp_raw_data.lfw = HumanSynchronizer::DoubleSeqToWrench6(m_htlfw); }
-    if (m_htcomIn.isNew()){ m_htcomIn.read(); hp_raw_data.com = HumanSynchronizer::Point3DToVector3(m_htcom); }
-    if (m_htrfIn.isNew()) { m_htrfIn.read();  hp_raw_data.rf  = HumanSynchronizer::Point3DToVector3(m_htrf); }
-    if (m_htlfIn.isNew()) { m_htlfIn.read();  hp_raw_data.lf  = HumanSynchronizer::Point3DToVector3(m_htlf); }
-    if (m_htrhIn.isNew()) { m_htrhIn.read();  hp_raw_data.rh  = HumanSynchronizer::Point3DToVector3(m_htrh); }
-    if (m_htlhIn.isNew()) { m_htlhIn.read();  hp_raw_data.lh  = HumanSynchronizer::Point3DToVector3(m_htlh); }
+    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hp_raw_data.rfw); }
+    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hp_raw_data.lfw); }
+    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Point3DToVector3(m_htcom.data,hp_raw_data.com); }
+    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Point3DToVector3(m_htrf.data,hp_raw_data.rf); }
+    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Point3DToVector3(m_htlf.data,hp_raw_data.lf); }
+    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Point3DToVector3(m_htrh.data,hp_raw_data.rh); }
+    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Point3DToVector3(m_htlh.data,hp_raw_data.lh); }
+    if (m_htcamIn.isNew()){ m_htcamIn.read(); HumanSynchronizer::Pose3DToVector3(m_htcam.data,hsp->cam_pos,hsp->cam_rpy); }
     if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
     hsp->readInput(hp_raw_data);
     hsp->current_basepos = m_robot->rootLink()->p;
@@ -1132,7 +1135,6 @@ void AutoBalancer::solveLimbIK ()
   }
   m_robot->calcForwardKinematics();
 
-
   // additional COM fitting IK for HumanSynchronizer
   hrp::Vector3 tmp_com_err = hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM();
   int com_ik_loop=0;
@@ -1155,6 +1157,10 @@ void AutoBalancer::solveLimbIK ()
 	  }
 
 
+    m_robot->joint(15)->q = hsp->cam_rpy_filtered(2);
+    m_robot->joint(16)->q = hsp->cam_rpy_filtered(1);
+//    if(loop%50==0)cout<<"cam_rpy_filtered:\n"<<hsp->cam_rpy_filtered<<endl;
+
 
 
     //outport用のデータ上書き
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index ac16d8ccdf9..a5d74216cf3 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -36,7 +36,7 @@
 // <rtc-template block="consumer_stub_h">
 
 //実機だとログ出力で落ちる？getenv("HOME")ダメ？
-#define DEBUG 1
+#define DEBUG 0
 
 
 class BiquadIIRFilterVec
@@ -44,12 +44,12 @@ class BiquadIIRFilterVec
 private:
     IIRFilter x_filters,y_filters,z_filters;
     hrp::Vector3 ans;
+    std::vector<double> fb_coeffs, ff_coeffs;
 public:
     BiquadIIRFilterVec(const std::string& error_prefix = ""){
     };
     ~BiquadIIRFilterVec() {};
     bool setParameter(const double fc_in, const double HZ){
-      std::vector<double> fb_coeffs, ff_coeffs;
       double fc = tan(fc_in * M_PI / HZ) / (2 * M_PI);
       double denom = 1 + (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc;
       ff_coeffs.resize(3);
@@ -165,6 +165,7 @@ class HumanSynchronizer{
     hrp::Vector3 comacc_ref;
     std::vector<BiquadIIRFilterVec> iir_v_filters;
     BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters;
+    BiquadIIRFilterVec cam_rpy_filter;
     hrp::Vector3 r_zmp_raw;
 
   public:
@@ -183,6 +184,7 @@ class HumanSynchronizer{
     hrp::Vector3 current_basepos,init_basepos;
     hrp::Vector3 com_vel_old;
     unsigned int loop;
+    hrp::Vector3 cam_pos,cam_rpy,cam_pos_filtered,cam_rpy_filtered;
 
 
     interpolator *init_zmp_com_offset_interpolator;
@@ -203,7 +205,7 @@ class HumanSynchronizer{
       go_rf_landing = go_lf_landing = false;
       pre_cont_rfpos = pre_cont_lfpos = hrp::Vector3::Zero();
       init_hp_calibcom = hrp::Vector3::Zero();
-      tgt_FUP_TIME = FUP_TIME = 0.4;
+      tgt_FUP_TIME = FUP_TIME = 0.8;
       FUP_HIGHT = 0.05;
       CNT_F_TH = 20.0;
       MAXVEL = 0.004;
@@ -222,15 +224,18 @@ class HumanSynchronizer{
       init_wld_hp_rfpos = init_wld_hp_lfpos = hrp::Vector3::Zero();
       current_basepos = init_basepos = hrp::Vector3::Zero();
 
-      com_vel_old= hrp::Vector3::Zero();
+      com_vel_old = hrp::Vector3::Zero();
+      cam_pos = cam_rpy = cam_pos_filtered = cam_rpy_filtered = hrp::Vector3::Zero();
 
       iir_v_filters.resize(5);
-      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(1,HZ);//四肢拘束点用
+      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(0.5,HZ);//四肢拘束点用
       iir_v_filters[0].setParameter(0.5,HZ);//重心用
       calcacc_v_filters.setParameter(5,HZ);//加速度計算用
       acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
+      cam_rpy_filter.setParameter(1,HZ);//カメラアングル
 
-      use_rh = use_lh = true;
+
+      use_rh = use_lh = false;
 
       rp_ref_out_old.clear();
       rp_ref_out.clear();
@@ -279,15 +284,24 @@ class HumanSynchronizer{
       updateCOMModRatio                   (tgt_cmmr);
       removeInitOffsetPose                (hp_wld_raw, hp_wld_initpos, hp_rel_raw);
       applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);
-      lockFootXYOnContact                 (go_rf_landing, go_lf_landing, hp_rel_raw);//根本から改変すべき2
+//      lockFootXYOnContact                 (go_rf_landing, go_lf_landing, hp_rel_raw);//根本から改変すべき2
       hp_filtered = hp_rel_raw;
       calcWorldZMP                        ((hp_filtered.rf+init_wld_hp_rfpos), (hp_filtered.lf+init_wld_hp_lfpos), hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
       convertRelHumanPoseToRelRobotPose   (hp_filtered,rp_ref_out);
       applyCOMMoveModRatio                (rp_ref_out);
-      judgeFootLandOnCommand              (hp_wld_raw.rfw, hp_wld_raw.lfw, go_rf_landing, go_lf_landing);//frwはすでに1000->100Hzにフィルタリングされている
+      judgeFootLandOnCommand              (hp_wld_raw.rfw, hp_wld_raw.lfw, go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
+
+
       applyEEWorkspaceLimit               (rp_ref_out);
+      lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
+
       applyCOMToSupportRegionLimit        (rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
       applyLPFilter                       (rp_ref_out);
+      applyCOMStateLimitByCapturePoint    (rp_ref_out.com, rp_ref_out_old.com, com_vel_old, rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
+
+      cam_rpy_filtered = cam_rpy_filter.passFilter(cam_rpy);
+
+
       r_zmp_raw = rp_ref_out.zmp;
       applyZMPCalcFromCOM                 (rp_ref_out.com,rp_ref_out.zmp);
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, go_rf_landing, go_lf_landing);//
@@ -320,12 +334,12 @@ class HumanSynchronizer{
       }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
       zmp_ans(2) = 0;
     }
-    static hrp::Vector3 Point3DToVector3(const RTC::TimedPoint3D& in){ hrp::Vector3 out(in.data.x,in.data.y,in.data.z); return out; }
-    static HumanPose::Wrench6 DoubleSeqToWrench6(const RTC::TimedDoubleSeq& in){
-      HumanPose::Wrench6 out;
-      if(in.data.length() == 6){out.f(0)=in.data[0]; out.f(1)=in.data[1]; out.f(2)=in.data[2]; out.t(0)=in.data[3]; out.t(1)=in.data[4]; out.t(2)=in.data[5];
+    static void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out(0) = in.x; out(1) = in.y; out(2) = in.z;}
+    static void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out(0) = in.r; out(1) = in.p; out(2) = in.y;}
+    static void Pose3DToVector3(const RTC::Pose3D& in, hrp::Vector3& out_pos, hrp::Vector3& out_rpy){ Point3DToVector3(in.position,out_pos); Oriantation3DToVector3(in.orientation,out_rpy); }
+    static void DoubleSeqToWrench6(const RTC::TimedDoubleSeq::_data_seq& in, HumanPose::Wrench6& out){
+      if(in.length() == 6){out.f(0)=in[0]; out.f(1)=in[1]; out.f(2)=in[2]; out.t(0)=in[3]; out.t(1)=in[4]; out.t(2)=in[5];
       }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; out.f.Zero(); out.t.Zero(); }
-      return out;
     }
 
   private:
@@ -436,11 +450,21 @@ class HumanSynchronizer{
       }
     }
     void applyEEWorkspaceLimit(HumanPose& tgt){
+//      const hrp::Vector2 rf2lf_vec( (tgt.lf(0)+rp_wld_initpos.lf(0))-(tgt.rf(0)+rp_wld_initpos.rf(0)) , (tgt.lf(1)+rp_wld_initpos.lf(1))-(tgt.rf(1)+rp_wld_initpos.rf(1)) );
+      const hrp::Vector2 rf2lf_vec( tgt.lf(0)-tgt.rf(0), tgt.lf(1)-tgt.rf(1) );
+      const hrp::Vector2 lf2rf_vec = - rf2lf_vec;
+      const double MAX = 0.15;
       if(!is_rf_contact && is_lf_contact){//右足浮遊時
-        if(tgt.rf(1) + rp_wld_initpos.rf(1) > tgt.lf(1) + rp_wld_initpos.lf(1) - 0.15)tgt.rf(1) = tgt.lf(1) + rp_wld_initpos.lf(1) - rp_wld_initpos.rf(1) - 0.15;
+        if(lf2rf_vec.norm() > MAX){
+          tgt.rf(0) = (lf2rf_vec.normalized())(0) * MAX + tgt.lf(0);
+          tgt.rf(1) = (lf2rf_vec.normalized())(1) * MAX + tgt.lf(1);
+        }
       }
       else if(is_rf_contact && !is_lf_contact){//左足浮遊時
-        if(tgt.lf(1) + rp_wld_initpos.lf(1) < tgt.rf(1) + rp_wld_initpos.rf(1) + 0.15)tgt.lf(1) = tgt.rf(1) + rp_wld_initpos.rf(1) - rp_wld_initpos.lf(1) + 0.15;
+        if(rf2lf_vec.norm() > MAX){
+          tgt.lf(0) = (rf2lf_vec.normalized())(0) * MAX + tgt.rf(0);
+          tgt.lf(1) = (rf2lf_vec.normalized())(1) * MAX + tgt.rf(1);
+        }
       }
       hrp::Vector3 hand_ulimit = hrp::Vector3(0.1,0.1,0.1);
       hrp::Vector3 hand_llimit = hrp::Vector3(-0.1,-0.1,-0.1);
@@ -453,14 +477,14 @@ class HumanSynchronizer{
       }
     }
     bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
-      const double XUMARGIN = 0.04;//CHIDORI
-      const double XLMARGIN = -0.02;
-      const double YUMARGIN = 0.01;
-      const double YLMARGIN = -0.01;
-//      const double XUMARGIN = 0.02;//JAXON
-//      const double XLMARGIN = -0.01;
-//      const double YUMARGIN = 0.0;
-//      const double YLMARGIN = -0.0;
+//      const double XUMARGIN = 0.04;//CHIDORI
+//      const double XLMARGIN = -0.02;
+//      const double YUMARGIN = 0.01;
+//      const double YLMARGIN = -0.01;
+      const double XUMARGIN = 0.02;//JAXON
+      const double XLMARGIN = -0.01;
+      const double YUMARGIN = 0.0;
+      const double YLMARGIN = -0.0;
 //      const double XUMARGIN = 0.10;//test
 //      const double XLMARGIN = -0.05;
 //      const double YUMARGIN = 0.03;
@@ -571,16 +595,16 @@ class HumanSynchronizer{
       hrp::Vector3 cp_ragulated;
 
       margin_t margin_from_4maxcp;
-      checkMarginToFootSupportRegion(com_old, com_old, rfin_abs, lfin_abs, 0.04, 0.02, 0.01, 0.01, margin_from_4maxcp, bb);
+      checkMarginToFootSupportRegion(com_old, com_old, rfin_abs, lfin_abs, 0.02, 0.01, 0.00, 0.00, margin_from_4maxcp, bb);
       hrp::Vector3 max_com_vel,min_com_vel;
       max_com_vel(0) = margin_from_4maxcp.x_u / sqrt( H / G );
       min_com_vel(0) = margin_from_4maxcp.x_l / sqrt( H / G );
       max_com_vel(1) = margin_from_4maxcp.y_u / sqrt( H / G );
       min_com_vel(1) = margin_from_4maxcp.y_l / sqrt( H / G );
-      if(com_vel(0) > max_com_vel(0)){ com_vel(0) = max_com_vel(0); cout<<"F cp over"<<endl;}//ここブレーキ時のZMP条件に変えたほうがいいかもしれない
-      if(com_vel(0) < min_com_vel(0)){ com_vel(0) = min_com_vel(0); cout<<"B cp over"<<endl;}
-      if(com_vel(1) > max_com_vel(1)){ com_vel(1) = max_com_vel(1); cout<<"L cp over"<<endl;}
-      if(com_vel(1) < min_com_vel(1)){ com_vel(1) = min_com_vel(1); cout<<"R cp over"<<endl;}
+      if(com_vel(0) > max_com_vel(0)){ com_vel(0) = max_com_vel(0); if(loop%100==0)cout<<"F cp over"<<endl;}//ここブレーキ時のZMP条件に変えたほうがいいかもしれない
+      if(com_vel(0) < min_com_vel(0)){ com_vel(0) = min_com_vel(0); if(loop%100==0)cout<<"B cp over"<<endl;}
+      if(com_vel(1) > max_com_vel(1)){ com_vel(1) = max_com_vel(1); if(loop%100==0)cout<<"L cp over"<<endl;}
+      if(com_vel(1) < min_com_vel(1)){ com_vel(1) = min_com_vel(1); if(loop%100==0)cout<<"R cp over"<<endl;}
 
 
 
@@ -891,6 +915,8 @@ class AutoBalancer
   InPort<TimedPoint3D> m_htlhIn;
   TimedPoint3D m_actzmp;
   InPort<TimedPoint3D> m_actzmpIn;
+  TimedPose3D m_htcam;
+  InPort<TimedPose3D> m_htcamIn;
   
   // </rtc-template>
 

From e15ba01c7e7ae16aad71067aa11688acc2304edb Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 19 Dec 2016 12:01:08 +0900
Subject: [PATCH 038/217] remove unused backup files

---
 rtc/AutoBalancer/AutoBalancer.cpp.mas | 2064 -----------------------
 rtc/AutoBalancer/AutoBalancer.cpp.my  | 2218 -------------------------
 rtc/AutoBalancer/AutoBalancer.h.mas   |  270 ---
 3 files changed, 4552 deletions(-)
 delete mode 100644 rtc/AutoBalancer/AutoBalancer.cpp.mas
 delete mode 100644 rtc/AutoBalancer/AutoBalancer.cpp.my
 delete mode 100644 rtc/AutoBalancer/AutoBalancer.h.mas

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp.mas b/rtc/AutoBalancer/AutoBalancer.cpp.mas
deleted file mode 100644
index 91353e3dd38..00000000000
--- a/rtc/AutoBalancer/AutoBalancer.cpp.mas
+++ /dev/null
@@ -1,2064 +0,0 @@
-// -*- C++ -*-
-/*!
- * @file  AutoBalancer.cpp
- * @brief autobalancer component
- * $Date$
- *
- * $Id$
- */
-
-#include <rtm/CorbaNaming.h>
-#include <hrpModel/Link.h>
-#include <hrpModel/Sensor.h>
-#include <hrpModel/ModelLoaderUtil.h>
-#include "AutoBalancer.h"
-#include <hrpModel/JointPath.h>
-#include <hrpUtil/MatrixSolvers.h>
-#include "hrpsys/util/Hrpsys.h"
-
-
-typedef coil::Guard<coil::Mutex> Guard;
-using namespace rats;
-
-// Module specification
-// <rtc-template block="module_spec">
-static const char* autobalancer_spec[] =
-    {
-        "implementation_id", "AutoBalancer",
-        "type_name",         "AutoBalancer",
-        "description",       "autobalancer component",
-        "version",           HRPSYS_PACKAGE_VERSION,
-        "vendor",            "AIST",
-        "category",          "example",
-        "activity_type",     "DataFlowComponent",
-        "max_instance",      "10",
-        "language",          "C++",
-        "lang_type",         "compile",
-        // Configuration variables
-        "conf.default.debugLevel", "0",
-        ""
-    };
-// </rtc-template>
-
-AutoBalancer::AutoBalancer(RTC::Manager* manager)
-    : RTC::DataFlowComponentBase(manager),
-      // <rtc-template block="initializer">
-      m_qRefIn("qRef", m_qRef),
-      m_basePosIn("basePosIn", m_basePos),
-      m_baseRpyIn("baseRpyIn", m_baseRpy),
-      m_zmpIn("zmpIn", m_zmp),
-      m_optionalDataIn("optionalData", m_optionalData),
-      m_emergencySignalIn("emergencySignal", m_emergencySignal),
-      m_qOut("q", m_qRef),
-      m_zmpOut("zmpOut", m_zmp),
-      m_basePosOut("basePosOut", m_basePos),
-      m_baseRpyOut("baseRpyOut", m_baseRpy),
-      m_baseTformOut("baseTformOut", m_baseTform),
-      m_basePoseOut("basePoseOut", m_basePose),
-      m_accRefOut("accRef", m_accRef),
-      m_contactStatesOut("contactStates", m_contactStates),
-      m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime),
-      m_cogOut("cogOut", m_cog),
-      m_AutoBalancerServicePort("AutoBalancerService"),
-      m_walkingStatesOut("walkingStates", m_walkingStates),
-      m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
-      // </rtc-template>
-      gait_type(BIPED),
-      move_base_gain(0.8),
-      m_robot(hrp::BodyPtr()),
-      m_debugLevel(0)
-{
-    m_service0.autobalancer(this);
-}
-
-AutoBalancer::~AutoBalancer()
-{
-}
-
-
-RTC::ReturnCode_t AutoBalancer::onInitialize()
-{
-    std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
-    bindParameter("debugLevel", m_debugLevel, "0");
-
-    // Registration: InPort/OutPort/Service
-    // <rtc-template block="registration">
-    // Set InPort buffers
-    addInPort("qRef", m_qRefIn);
-    addInPort("basePosIn", m_basePosIn);
-    addInPort("baseRpyIn", m_baseRpyIn);
-    addInPort("zmpIn", m_zmpIn);
-    addInPort("optionalData", m_optionalDataIn);
-    addInPort("emergencySignal", m_emergencySignalIn);
-
-    // Set OutPort buffer
-    addOutPort("q", m_qOut);
-    addOutPort("zmpOut", m_zmpOut);
-    addOutPort("basePosOut", m_basePosOut);
-    addOutPort("baseRpyOut", m_baseRpyOut);
-    addOutPort("baseTformOut", m_baseTformOut);
-    addOutPort("basePoseOut", m_basePoseOut);
-    addOutPort("accRef", m_accRefOut);
-    addOutPort("contactStates", m_contactStatesOut);
-    addOutPort("controlSwingSupportTime", m_controlSwingSupportTimeOut);
-    addOutPort("cogOut", m_cogOut);
-    addOutPort("walkingStates", m_walkingStatesOut);
-    addOutPort("sbpCogOffset", m_sbpCogOffsetOut);
-  
-    // Set service provider to Ports
-    m_AutoBalancerServicePort.registerProvider("service0", "AutoBalancerService", m_service0);
-  
-    // Set service consumers to Ports
-  
-    // Set CORBA Service Ports
-    addPort(m_AutoBalancerServicePort);
-  
-    // </rtc-template>
-    // <rtc-template block="bind_config">
-    // Bind variables and configuration variable
-  
-    // </rtc-template>
-
-    RTC::Properties& prop =  getProperties();
-    coil::stringTo(m_dt, prop["dt"].c_str());
-
-    m_robot = hrp::BodyPtr(new hrp::Body());
-    RTC::Manager& rtcManager = RTC::Manager::instance();
-    std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
-    int comPos = nameServer.find(",");
-    if (comPos < 0){
-        comPos = nameServer.length();
-    }
-    nameServer = nameServer.substr(0, comPos);
-    RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
-    if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
-                                 CosNaming::NamingContext::_duplicate(naming.getRootContext())
-                                 )){
-      std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
-      return RTC::RTC_ERROR;
-    }
-
-    // allocate memory for outPorts
-    m_qRef.data.length(m_robot->numJoints());
-    qorg.resize(m_robot->numJoints());
-    qrefv.resize(m_robot->numJoints());
-    m_baseTform.data.length(12);
-
-    control_mode = MODE_IDLE;
-    loop = 0;
-
-    // setting from conf file
-    // GaitGenerator requires abc_leg_offset and abc_stride_parameter in robot conf file
-    // setting leg_pos from conf file
-    coil::vstring leg_offset_str = coil::split(prop["abc_leg_offset"], ",");
-    leg_names.push_back("rleg");
-    leg_names.push_back("lleg");
-
-    // setting from conf file
-    // rleg,TARGET_LINK,BASE_LINK
-    coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
-    size_t prop_num = 10;
-    if (end_effectors_str.size() > 0) {
-      size_t num = end_effectors_str.size()/prop_num;
-      for (size_t i = 0; i < num; i++) {
-        std::string ee_name, ee_target, ee_base;
-        coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
-        coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
-        coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
-        ABCIKparam tp;
-        for (size_t j = 0; j < 3; j++) {
-          coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
-        }
-        double tmpv[4];
-        for (int j = 0; j < 4; j++ ) {
-          coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
-        }
-        tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
-        tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
-        // Fix for toe joint
-        //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
-        //   Without toe joints, "end-effector link == force-sensor link" is assumed.
-        //   With toe joints, "end-effector link != force-sensor link" is assumed.
-        if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
-            std::vector<double> optw(tp.manip->numJoints(), 1.0);
-            optw.back() = 0.0; // Set weight = 0 for toe joint by default
-            tp.manip->setOptionalWeightVector(optw);
-            tp.has_toe_joint = true;
-        } else {
-            tp.has_toe_joint = false;
-        }
-        tp.avoid_gain = 0.001;
-        tp.reference_gain = 0.01;
-        tp.pos_ik_error_count = tp.rot_ik_error_count = 0;
-        ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
-        ikp[ee_name].target_link = m_robot->link(ee_target);
-        ee_vec.push_back(ee_name);
-        std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   target = " << ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
-        contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
-      }
-      m_contactStates.data.length(num);
-      if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
-        m_contactStates.data[contact_states_index_map["rleg"]] = true;
-        m_contactStates.data[contact_states_index_map["lleg"]] = true;
-      }
-      if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) {
-        m_contactStates.data[contact_states_index_map["rarm"]] = false;
-        m_contactStates.data[contact_states_index_map["larm"]] = false;
-      }
-      m_controlSwingSupportTime.data.length(num);
-      for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 0.0;
-    }
-    std::vector<hrp::Vector3> leg_pos;
-    if (leg_offset_str.size() > 0) {
-      hrp::Vector3 leg_offset;
-      for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
-      std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-      leg_pos.push_back(hrp::Vector3(-1*leg_offset));
-      leg_pos.push_back(hrp::Vector3(leg_offset));
-    }
-    if (leg_pos.size() < ikp.size()) {
-        size_t tmp_leg_pos_size = leg_pos.size();
-        for (size_t i = 0; i < ikp.size() - tmp_leg_pos_size; i++) {
-            leg_pos.push_back(hrp::Vector3::Zero());
-        }
-    }
-
-    std::vector<std::pair<hrp::Link*, hrp::Link*> > interlocking_joints;
-    readInterlockingJointsParamFromProperties(interlocking_joints, m_robot, prop["interlocking_joints"], std::string(m_profile.instance_name));
-    if (interlocking_joints.size() > 0) {
-        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            std::cerr << "[" << m_profile.instance_name << "] Interlocking Joints for [" << it->first << "]" << std::endl;
-            it->second.manip->setInterlockingJointPairIndices(interlocking_joints, std::string(m_profile.instance_name));
-        }
-    }
-
-    zmp_offset_interpolator = new interpolator(ikp.size()*3, m_dt);
-    zmp_offset_interpolator->setName(std::string(m_profile.instance_name)+" zmp_offset_interpolator");
-    zmp_transition_time = 1.0;
-    transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-    transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator");
-    transition_interpolator_ratio = 1.0;
-    adjust_footstep_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-    adjust_footstep_interpolator->setName(std::string(m_profile.instance_name)+" adjust_footstep_interpolator");
-    transition_time = 2.0;
-    adjust_footstep_transition_time = 2.0;
-    leg_names_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-    leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator");
-    leg_names_interpolator_ratio = 1.0;
-
-    // setting stride limitations from conf file
-    double stride_fwd_x_limit = 0.15;
-    double stride_y_limit = 0.05;
-    double stride_th_limit = 10;
-    double stride_bwd_x_limit = 0.05;
-    std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_y_limit << "[m], " << stride_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl;
-    if (default_zmp_offsets.size() == 0) {
-      for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
-    }
-    if (leg_offset_str.size() > 0) {
-      gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_y_limit/*[m]*/, stride_th_limit/*[deg]*/, stride_bwd_x_limit/*[m]*/));
-      gg->set_default_zmp_offsets(default_zmp_offsets);
-    }
-    gg_is_walking = gg_solved = false;
-    m_walkingStates.data = false;
-    fix_leg_coords = coordinates();
-
-    // load virtual force sensors
-    readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
-    // ref force port
-    int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
-    int nvforce = m_vfs.size();
-    int nforce  = npforce + nvforce;
-    m_ref_force.resize(nforce);
-    m_ref_forceIn.resize(nforce);
-    m_force.resize(nforce);
-    m_ref_forceOut.resize(nforce);
-    m_limbCOPOffset.resize(nforce);
-    m_limbCOPOffsetOut.resize(nforce);
-    for (unsigned int i=0; i<npforce; i++){
-        sensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
-    }
-    for (unsigned int i=0; i<nvforce; i++){
-        for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
-            if (it->second.id == i) sensor_names.push_back(it->first);
-        }
-    }
-    // set ref force port
-    std::cerr << "[" << m_profile.instance_name << "] force sensor ports (" << nforce << ")" << std::endl;
-    for (unsigned int i=0; i<nforce; i++){
-        m_ref_forceIn[i] = new InPort<TimedDoubleSeq>(std::string("ref_"+sensor_names[i]).c_str(), m_ref_force[i]);
-        m_ref_force[i].data.length(6);
-        registerInPort(std::string("ref_"+sensor_names[i]).c_str(), *m_ref_forceIn[i]);
-        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string("ref_"+sensor_names[i]) << std::endl;
-        ref_forces.push_back(hrp::Vector3(0,0,0));
-    }
-    // set force port
-    for (unsigned int i=0; i<nforce; i++){
-        m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string(sensor_names[i]).c_str(), m_force[i]);
-        m_force[i].data.length(6);
-        m_force[i].data[0] = m_force[i].data[1] = m_force[i].data[2] = 0.0;
-        m_force[i].data[3] = m_force[i].data[4] = m_force[i].data[5] = 0.0;
-        registerOutPort(std::string(sensor_names[i]).c_str(), *m_ref_forceOut[i]);
-        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string(sensor_names[i]) << std::endl;
-    }
-    // set limb cop offset port
-    std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << nforce << ")" << std::endl;
-    for (unsigned int i=0; i<nforce; i++){
-        std::string nm("limbCOPOffset_"+sensor_names[i]);
-        m_limbCOPOffsetOut[i] = new OutPort<TimedPoint3D>(nm.c_str(), m_limbCOPOffset[i]);
-        registerOutPort(nm.c_str(), *m_limbCOPOffsetOut[i]);
-        m_limbCOPOffset[i].data.x = m_limbCOPOffset[i].data.y = m_limbCOPOffset[i].data.z = 0.0;
-        std::cerr << "[" << m_profile.instance_name << "]   name = " << nm << std::endl;
-    }
-    sbp_offset = hrp::Vector3(0,0,0);
-    sbp_cog_offset = hrp::Vector3(0,0,0);
-    //use_force = MODE_NO_FORCE;
-    use_force = MODE_REF_FORCE;
-
-    if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
-      is_legged_robot = true;
-    } else {
-      is_legged_robot = false;
-    }
-
-    m_accRef.data.ax = m_accRef.data.ay = m_accRef.data.az = 0.0;
-    prev_imu_sensor_vel = hrp::Vector3::Zero();
-
-    graspless_manip_mode = false;
-    graspless_manip_arm = "arms";
-    graspless_manip_p_gain = hrp::Vector3::Zero();
-
-    is_stop_mode = false;
-    has_ik_failed = false;
-    is_hand_fix_mode = false;
-
-    pos_ik_thre = 0.1*1e-3; // [m]
-    rot_ik_thre = (1e-2)*M_PI/180.0; // [rad]
-    ik_error_debug_print_freq = static_cast<int>(0.2/m_dt); // once per 0.2 [s]
-
-    return RTC::RTC_OK;
-}
-
-
-
-RTC::ReturnCode_t AutoBalancer::onFinalize()
-{
-  delete zmp_offset_interpolator;
-  delete transition_interpolator;
-  delete adjust_footstep_interpolator;
-  delete leg_names_interpolator;
-  return RTC::RTC_OK;
-}
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onStartup(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onShutdown(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id)
-{
-    std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
-    
-    return RTC::RTC_OK;
-}
-
-RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
-{
-  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
-  Guard guard(m_mutex);
-  if (control_mode == MODE_ABC) {
-    control_mode = MODE_SYNC_TO_IDLE;
-    double tmp_ratio = 0.0;
-    transition_interpolator->go(&tmp_ratio, m_dt, true); // sync in one controller loop
-  }
-  return RTC::RTC_OK;
-}
-
-#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
-//#define DEBUGP2 ((loop%200==0))
-#define DEBUGP2 (false)
-RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
-{
-  // std::cerr << "AutoBalancer::onExecute(" << ec_id << ")" << std::endl;
-    loop ++;
-    if (m_qRefIn.isNew()) {
-        m_qRefIn.read();
-    }
-    if (m_basePosIn.isNew()) {
-      m_basePosIn.read();
-      input_basePos(0) = m_basePos.data.x;
-      input_basePos(1) = m_basePos.data.y;
-      input_basePos(2) = m_basePos.data.z;
-    }
-    if (m_baseRpyIn.isNew()) {
-      m_baseRpyIn.read();
-      input_baseRot = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
-    }
-    if (m_zmpIn.isNew()) {
-      m_zmpIn.read();
-      input_zmp(0) = m_zmp.data.x;
-      input_zmp(1) = m_zmp.data.y;
-      input_zmp(2) = m_zmp.data.z;
-    }
-    for (unsigned int i=0; i<m_ref_forceIn.size(); i++){
-        if ( m_ref_forceIn[i]->isNew() ) {
-            m_ref_forceIn[i]->read();
-        }
-    }
-    if (m_optionalDataIn.isNew()) {
-        m_optionalDataIn.read();
-        if (is_legged_robot) {
-          if (m_optionalData.data.length() >= contact_states_index_map.size()*2) {
-            // current optionalData is contactstates x limb and controlSwingSupportTime x limb
-            //   If contactStates in optionalData is 1.0, m_contactStates is true. Otherwise, false.
-            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-                m_contactStates.data[contact_states_index_map[it->first]] = isOptionalDataContact(it->first);
-                m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = m_optionalData.data[contact_states_index_map[it->first]+contact_states_index_map.size()];
-            }
-            if ( !m_contactStates.data[contact_states_index_map["rleg"]] && !m_contactStates.data[contact_states_index_map["lleg"]] ) { // If two feet have no contact, force set double support contact
-              m_contactStates.data[contact_states_index_map["rleg"]] = true;
-              m_contactStates.data[contact_states_index_map["lleg"]] = true;
-            }
-          }
-        }
-    }
-    if (m_emergencySignalIn.isNew()){
-        m_emergencySignalIn.read();
-        // if (!is_stop_mode) {
-        //     std::cerr << "[" << m_profile.instance_name << "] emergencySignal is set!" << std::endl;
-        //     is_stop_mode = true;
-        //     gg->emergency_stop();
-        // }
-    }
-
-    Guard guard(m_mutex);
-    hrp::Vector3 ref_basePos;
-    hrp::Matrix33 ref_baseRot;
-    hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
-    if ( is_legged_robot ) {
-      gg->proc_zmp_weight_map_interpolation();
-      getCurrentParameters();
-      getTargetParameters();
-      bool is_transition_interpolator_empty = transition_interpolator->isEmpty();
-      if (!is_transition_interpolator_empty) {
-        transition_interpolator->get(&transition_interpolator_ratio, true);
-      } else {
-        transition_interpolator_ratio = 1.0;
-      }
-      if (control_mode != MODE_IDLE ) {
-        solveLimbIK();
-        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
-      } else {
-        rel_ref_zmp = input_zmp;
-      }
-      // transition
-      if (!is_transition_interpolator_empty) {
-        // transition_interpolator_ratio 0=>1 : IDLE => ABC
-        // transition_interpolator_ratio 1=>0 : ABC => IDLE
-        ref_basePos = (1-transition_interpolator_ratio) * input_basePos + transition_interpolator_ratio * m_robot->rootLink()->p;
-        rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp;
-        rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R);
-        for ( int i = 0; i < m_robot->numJoints(); i++ ) {
-          m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q;
-        }
-      } else {
-        ref_basePos = m_robot->rootLink()->p;
-        ref_baseRot = m_robot->rootLink()->R;
-      }
-      // mode change for sync
-      if (control_mode == MODE_SYNC_TO_ABC) {
-        control_mode = MODE_ABC;
-      } else if (control_mode == MODE_SYNC_TO_IDLE && transition_interpolator->isEmpty() ) {
-        std::cerr << "[" << m_profile.instance_name << "] Finished cleanup" << std::endl;
-        control_mode = MODE_IDLE;
-      }
-    }
-    if ( m_qRef.data.length() != 0 ) { // initialized
-      if (is_legged_robot) {
-        for ( int i = 0; i < m_robot->numJoints(); i++ ){
-          m_qRef.data[i] = m_robot->joint(i)->q;
-        }
-      }
-      m_qOut.write();
-    }
-    if (is_legged_robot) {
-      // basePos
-      m_basePos.data.x = ref_basePos(0);
-      m_basePos.data.y = ref_basePos(1);
-      m_basePos.data.z = ref_basePos(2);
-      m_basePos.tm = m_qRef.tm;
-      // baseRpy
-      hrp::Vector3 baseRpy = hrp::rpyFromRot(ref_baseRot);
-      m_baseRpy.data.r = baseRpy(0);
-      m_baseRpy.data.p = baseRpy(1);
-      m_baseRpy.data.y = baseRpy(2);
-      m_baseRpy.tm = m_qRef.tm;
-      // baseTform
-      double *tform_arr = m_baseTform.data.get_buffer();
-      tform_arr[0] = m_basePos.data.x;
-      tform_arr[1] = m_basePos.data.y;
-      tform_arr[2] = m_basePos.data.z;
-      hrp::setMatrix33ToRowMajorArray(ref_baseRot, tform_arr, 3);
-      m_baseTform.tm = m_qRef.tm;
-      // basePose
-      m_basePose.data.position.x = m_basePos.data.x;
-      m_basePose.data.position.y = m_basePos.data.y;
-      m_basePose.data.position.z = m_basePos.data.z;
-      m_basePose.data.orientation.r = m_baseRpy.data.r;
-      m_basePose.data.orientation.p = m_baseRpy.data.p;
-      m_basePose.data.orientation.y = m_baseRpy.data.y;
-      m_basePose.tm = m_qRef.tm;
-      // zmp
-      m_zmp.data.x = rel_ref_zmp(0);
-      m_zmp.data.y = rel_ref_zmp(1);
-      m_zmp.data.z = rel_ref_zmp(2);
-      m_zmp.tm = m_qRef.tm;
-      // cog
-      m_cog.data.x = ref_cog(0);
-      m_cog.data.y = ref_cog(1);
-      m_cog.data.z = ref_cog(2);
-      m_cog.tm = m_qRef.tm;
-      // sbpCogOffset
-      m_sbpCogOffset.data.x = sbp_cog_offset(0);
-      m_sbpCogOffset.data.y = sbp_cog_offset(1);
-      m_sbpCogOffset.data.z = sbp_cog_offset(2);
-      m_sbpCogOffset.tm = m_qRef.tm;
-    }
-    m_basePosOut.write();
-    m_baseRpyOut.write();
-    m_baseTformOut.write();
-    m_basePoseOut.write();
-    m_zmpOut.write();
-    m_cogOut.write();
-    m_sbpCogOffsetOut.write();
-
-    // reference acceleration
-    hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
-    if (sen != NULL) {
-      hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos;
-      hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt;
-      // convert to imu sensor local acceleration
-      hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt;
-      m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2);
-      m_accRefOut.write();
-      prev_imu_sensor_pos = imu_sensor_pos;
-      prev_imu_sensor_vel = imu_sensor_vel;
-    }
-
-    // control parameters
-    m_contactStates.tm = m_qRef.tm;
-    m_contactStatesOut.write();
-    m_controlSwingSupportTime.tm = m_qRef.tm;
-    m_controlSwingSupportTimeOut.write();
-    m_walkingStates.data = gg_is_walking;
-    m_walkingStates.tm = m_qRef.tm;
-    m_walkingStatesOut.write();
-
-    for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
-        m_force[i].tm = m_qRef.tm;
-        for (unsigned int j=0; j<6; j++){
-            if (control_mode != MODE_IDLE) m_force[i].data[j] = transition_interpolator_ratio * m_force[i].data[j] + (1-transition_interpolator_ratio) * m_ref_force[i].data[j];
-            else m_force[i].data[j] = m_ref_force[i].data[j];
-        }
-        m_ref_forceOut[i]->write();
-    }
-
-    for (unsigned int i=0; i<m_limbCOPOffsetOut.size(); i++){
-        m_limbCOPOffset[i].tm = m_qRef.tm;
-        m_limbCOPOffsetOut[i]->write();
-    }
-
-    return RTC::RTC_OK;
-}
-
-void AutoBalancer::getCurrentParameters()
-{
-  current_root_p = m_robot->rootLink()->p;
-  current_root_R = m_robot->rootLink()->R;
-  for ( int i = 0; i < m_robot->numJoints(); i++ ){
-    qorg[i] = m_robot->joint(i)->q;
-  }
-}
-
-void AutoBalancer::getTargetParameters()
-{
-  // joint angles
-  for ( int i = 0; i < m_robot->numJoints(); i++ ){
-    m_robot->joint(i)->q = m_qRef.data[i];
-    qrefv[i] = m_robot->joint(i)->q;
-  }
-  // basepos, rot, zmp
-  m_robot->rootLink()->p = input_basePos;
-  m_robot->rootLink()->R = input_baseRot;
-  m_robot->calcForwardKinematics();
-  //
-  if (control_mode != MODE_IDLE) {
-    coordinates tmp_fix_coords;
-    if (!zmp_offset_interpolator->isEmpty()) {
-      double *default_zmp_offsets_output = new double[ikp.size()*3];
-      zmp_offset_interpolator->get(default_zmp_offsets_output, true);
-      for (size_t i = 0; i < ikp.size(); i++)
-        for (size_t j = 0; j < 3; j++)
-          default_zmp_offsets[i](j) = default_zmp_offsets_output[i*3+j];
-      delete[] default_zmp_offsets_output;
-      if (DEBUGP) {
-        std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl;
-        std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-        for (size_t i = 0; i < leg_names.size(); i++) {
-            std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
-            std::cerr << "[" << m_profile.instance_name << "]   " << leg_names[i] << " = " << default_zmp_offsets[dst->first].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-        }
-      }
-    }
-    if (!leg_names_interpolator->isEmpty()) {
-        leg_names_interpolator->get(&leg_names_interpolator_ratio, true);
-    }else {
-        leg_names_interpolator_ratio = 1.0;
-    }
-    if ( gg_is_walking ) {
-      gg->set_default_zmp_offsets(default_zmp_offsets);
-      gg_solved = gg->proc_one_tick();
-      {
-          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-          coordinates tmpc;
-          // for support leg
-          for (std::vector<step_node>::const_iterator it = gg->get_support_leg_steps().begin(); it != gg->get_support_leg_steps().end(); it++) {
-              coordinates sp_coords = it->worldcoords;
-              coordinates(ikp[leg_type_map[it->l_r]].localPos,
-                          ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc);
-              sp_coords.transform(tmpc);
-              ikp[leg_type_map[it->l_r]].target_p0 = sp_coords.pos;
-              ikp[leg_type_map[it->l_r]].target_r0 = sp_coords.rot;
-          }
-          // for swing leg
-          for (std::vector<step_node>::const_iterator it = gg->get_swing_leg_steps().begin(); it != gg->get_swing_leg_steps().end(); it++) {
-              coordinates sw_coords = it->worldcoords;
-              coordinates(ikp[leg_type_map[it->l_r]].localPos,
-                          ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc);
-              sw_coords.transform(tmpc);
-              ikp[leg_type_map[it->l_r]].target_p0 = sw_coords.pos;
-              ikp[leg_type_map[it->l_r]].target_r0 = sw_coords.rot;
-          }
-      }
-      gg->get_swing_support_mid_coords(tmp_fix_coords);
-      // set contactStates
-      {
-          std::vector<std::string> tmp_current_support_states_names;
-          {
-              std::vector<leg_type> tmp_current_support_states = gg->get_current_support_states();
-              std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-              for (std::vector<leg_type>::const_iterator it = tmp_current_support_states.begin(); it != tmp_current_support_states.end(); it++)
-                  tmp_current_support_states_names.push_back(leg_type_map[*it]);
-          }
-          // Set Contact States for ee not included in leg_names to false
-          for ( std::map<std::string, size_t>::iterator it = contact_states_index_map.begin(); it != contact_states_index_map.end(); it++ ) {
-              m_contactStates.data[it->second] = false;
-          }
-          for (std::vector<std::string>::const_iterator it = leg_names.begin(); it != leg_names.end(); it++) {
-              std::vector<std::string>::const_iterator dst = std::find_if(tmp_current_support_states_names.begin(), tmp_current_support_states_names.end(), boost::lambda::_1 == *it);
-              if (dst != tmp_current_support_states_names.end()) {
-                  m_contactStates.data[contact_states_index_map[*it]] = true;
-              } else {
-                  m_contactStates.data[contact_states_index_map[*it]] = false;
-              }
-          }
-      }
-      // set controlSwingSupportTime
-      {
-          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-          for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
-              std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
-              m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = gg->get_current_swing_time(dst->first);
-          }
-      }
-      // set limbCOPOffset
-      {
-          std::vector<std::string> swg_leg_nms = gg->get_swing_leg_names();
-          for (size_t i = 0; i < swg_leg_nms.size(); i++) {
-              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.x = gg->get_swing_foot_zmp_offsets().at(i)(0);
-              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.y = gg->get_swing_foot_zmp_offsets().at(i)(1);
-              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.z = gg->get_swing_foot_zmp_offsets().at(i)(2);
-          }
-      }
-      {
-          std::vector<std::string> sup_leg_nms = gg->get_support_leg_names();
-          for (size_t i = 0; i < sup_leg_nms.size(); i++) {
-              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.x = gg->get_support_foot_zmp_offsets().at(i)(0);
-              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.y = gg->get_support_foot_zmp_offsets().at(i)(1);
-              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.z = gg->get_support_foot_zmp_offsets().at(i)(2);
-          }
-      }
-    } else {
-      tmp_fix_coords = fix_leg_coords;
-      // double support by default
-      {
-          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-          for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
-              std::vector<std::string>::const_iterator dst = std::find_if(leg_names.begin(), leg_names.end(), (boost::lambda::_1 == it->first));
-              if (dst != leg_names.end()) {
-                  m_contactStates.data[contact_states_index_map[it->first]] = true;
-              } else {
-                  m_contactStates.data[contact_states_index_map[it->first]] = false;
-              }
-              // controlSwingSupportTime is not used while double support period, 1.0 is neglected
-              m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = 1.0;
-              std::map<leg_type, std::string>::const_iterator dst2 = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
-              m_limbCOPOffset[contact_states_index_map[it->first]].data.x = default_zmp_offsets.at(dst2->first)(0);
-              m_limbCOPOffset[contact_states_index_map[it->first]].data.y = default_zmp_offsets.at(dst2->first)(1);
-              m_limbCOPOffset[contact_states_index_map[it->first]].data.z = default_zmp_offsets.at(dst2->first)(2);
-          }
-      }
-    }
-    if (!adjust_footstep_interpolator->isEmpty()) {
-        double tmp = 0.0;
-        adjust_footstep_interpolator->get(&tmp, true);
-        //std::cerr << "[" << m_profile.instance_name << "] adjust ratio " << tmp << std::endl;
-        ikp["rleg"].target_p0 = (1-tmp) * ikp["rleg"].adjust_interpolation_org_p0 + tmp*ikp["rleg"].adjust_interpolation_target_p0;
-        ikp["lleg"].target_p0 = (1-tmp) * ikp["lleg"].adjust_interpolation_org_p0 + tmp*ikp["lleg"].adjust_interpolation_target_p0;
-        rats::mid_rot(ikp["rleg"].target_r0, tmp, ikp["rleg"].adjust_interpolation_org_r0, ikp["rleg"].adjust_interpolation_target_r0);
-        rats::mid_rot(ikp["lleg"].target_r0, tmp, ikp["lleg"].adjust_interpolation_org_r0, ikp["lleg"].adjust_interpolation_target_r0);
-        coordinates tmprc, tmplc;
-        tmprc.pos = ikp["rleg"].target_p0 + ikp["rleg"].target_r0 * ikp["rleg"].localPos;
-        tmprc.rot = ikp["rleg"].target_r0 * ikp["rleg"].localR;
-        tmplc.pos = ikp["lleg"].target_p0 + ikp["lleg"].target_r0 * ikp["lleg"].localPos;
-        tmplc.rot = ikp["lleg"].target_r0 * ikp["lleg"].localR;
-        rats::mid_coords(fix_leg_coords, 0.5, tmprc, tmplc);
-        tmp_fix_coords = fix_leg_coords;
-    }
-    // Tempolarily modify tmp_fix_coords
-    // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272
-    {
-      hrp::Vector3 ex = hrp::Vector3::UnitX();
-      hrp::Vector3 ez = hrp::Vector3::UnitZ();
-      hrp::Vector3 xv1 (tmp_fix_coords.rot * ex);
-      xv1(2) = 0.0;
-      xv1.normalize();
-      hrp::Vector3 yv1(ez.cross(xv1));
-      tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2);
-      tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2);
-      tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2);
-    }
-    // Fix pos
-    fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot);
-
-    /* update ref_forces ;; sp's absolute -> rmc's absolute */
-    for (size_t i = 0; i < m_ref_forceIn.size(); i++) {
-      hrp::Matrix33 eeR;
-      hrp::Link* parentlink;
-      hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
-      if (sensor) parentlink = sensor->link;
-      else parentlink = m_vfs[sensor_names[i]].link;
-      for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-          if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR;
-      }
-      // End effector frame
-      //ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
-      // world frame
-      ref_forces[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
-    }
-    sbp_offset = tmp_fix_coords.rot * hrp::Vector3(sbp_offset);
-
-    target_root_p = m_robot->rootLink()->p;
-    target_root_R = m_robot->rootLink()->R;
-    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-      if ( control_mode == MODE_IDLE || std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end() ) {
-        it->second.target_p0 = it->second.target_link->p;
-        it->second.target_r0 = it->second.target_link->R;
-      }
-    }
-    // Just for ik initial value
-    if (control_mode == MODE_SYNC_TO_ABC) {
-        current_root_p = target_root_p;
-        current_root_R = target_root_R;
-        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
-                it->second.target_p0 = it->second.target_link->p;
-                it->second.target_r0 = it->second.target_link->R;
-            }
-        }
-    }
-    // Move hand for hand fix mode
-    //   If arms' ABCIKparam.is_active is true, move hands according to cog velocity.
-    //   If is_hand_fix_mode is false, no hand fix mode and move hands according to cog velocity.
-    //   If is_hand_fix_mode is true, hand fix mode and do not move hands in Y axis in tmp_fix_coords.rot.    
-    if (gg_is_walking) {
-        // hand control while walking = solve hand ik with is_hand_fix_mode and solve hand ik without is_hand_fix_mode
-        bool is_hand_control_while_walking = false;
-        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
-                 && it->first.find("arm") != std::string::npos ) {
-                is_hand_control_while_walking = true;
-            }
-        }
-        if (is_hand_control_while_walking) {
-        //if (false) { // Disabled temporarily
-            // Store hand_fix_initial_offset in the initialization of walking
-            if (is_hand_fix_initial) hand_fix_initial_offset = tmp_fix_coords.rot.transpose() * (hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos);
-            is_hand_fix_initial = false;
-            hrp::Vector3 dif_p = hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos - tmp_fix_coords.rot * hand_fix_initial_offset;
-            if (is_hand_fix_mode) {
-                dif_p = tmp_fix_coords.rot.transpose() * dif_p;
-                dif_p(1) = 0;
-                dif_p = tmp_fix_coords.rot * dif_p;
-            }
-            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-                if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
-                     && it->first.find("arm") != std::string::npos ) {
-                    it->second.target_p0 = it->second.target_p0 + dif_p;
-                }
-            }
-        }
-    }
-
-    // set ref_forces
-    {
-          std::vector<hrp::Vector3> ee_pos;
-          for (size_t i = 0 ; i < leg_names.size(); i++) {
-              ABCIKparam& tmpikp = ikp[leg_names[i]];
-              ee_pos.push_back(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]);
-          }
-          double alpha = (ref_zmp - ee_pos[1]).norm() / ((ee_pos[0] - ref_zmp).norm() + (ee_pos[1] - ref_zmp).norm());
-          if (alpha>1.0) alpha = 1.0;
-          if (alpha<0.0) alpha = 0.0;
-          if (DEBUGP) {
-          std::cerr << "[" << m_profile.instance_name << "] alpha:" << alpha << std::endl;
-          }
-          double mg = m_robot->totalMass() * gg->get_gravitational_acceleration();
-          m_force[0].data[2] = alpha * mg;
-          m_force[1].data[2] = (1-alpha) * mg;
-    }
-
-    hrp::Vector3 tmp_foot_mid_pos(hrp::Vector3::Zero());
-    {
-        std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-        std::map<leg_type, double> zmp_weight_map = gg->get_zmp_weight_map();
-        double sum_of_weight = 0.0;
-        for (size_t i = 0; i < leg_names.size(); i++) {
-            ABCIKparam& tmpikp = ikp[leg_names[i]];
-            // get target_end_coords
-            tmpikp.target_end_coords.pos = tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos;
-            tmpikp.target_end_coords.rot = tmpikp.target_r0 * tmpikp.localR;
-            // for foot_mid_pos
-            std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
-            tmp_foot_mid_pos += (tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]) * zmp_weight_map[dst->first];
-            sum_of_weight += zmp_weight_map[dst->first];
-        }
-        tmp_foot_mid_pos *= (1.0 / sum_of_weight);
-    }
-    //
-    {
-        if ( gg_is_walking && gg->get_lcg_count() == gg->get_overwrite_check_timing()+2 ) {
-            hrp::Vector3 vel_htc(calc_vel_from_hand_error(tmp_fix_coords));
-            gg->set_offset_velocity_param(vel_htc(0), vel_htc(1) ,vel_htc(2));
-        }//  else {
-        //     if ( gg_is_walking && gg->get_lcg_count() == static_cast<size_t>(gg->get_default_step_time()/(2*m_dt))-1) {
-        //         gg->set_offset_velocity_param(0,0,0);
-        //     }
-        // }
-    }
-
-    //
-
-    hrp::Vector3 tmp_ref_cog(m_robot->calcCM());
-    if (gg_is_walking) {
-      ref_cog = gg->get_cog();
-    } else {
-      ref_cog = tmp_foot_mid_pos;
-    }
-    ref_cog(2) = tmp_ref_cog(2);
-    if (gg_is_walking) {
-      ref_zmp = gg->get_refzmp();
-    } else {
-      ref_zmp(0) = ref_cog(0);
-      ref_zmp(1) = ref_cog(1);
-      ref_zmp(2) = tmp_foot_mid_pos(2);
-    }
-  }
-};
-
-hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2)
-{
-  hrp::Vector3 vv = axis1.cross(axis2);
-  if (fabs(vv.norm()-0.0) < 1e-5) {
-    return rot;
-  } else {
-    Eigen::AngleAxis<double> tmpr(std::asin(vv.norm()), vv.normalized());
-    return tmpr.toRotationMatrix() * rot;
-  }
-}
-
-void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot)
-{
-  // get current foot mid pos + rot
-  std::vector<coordinates> foot_coords;
-  for (size_t i = 0; i < leg_names.size(); i++) {
-      ABCIKparam& tmpikp = ikp[leg_names[i]];
-      if (leg_names[i].find("leg") != std::string::npos) foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
-                                                                                           (tmpikp.target_link->R * tmpikp.localR)));
-  }
-  coordinates current_foot_mid_coords;
-  multi_mid_coords(current_foot_mid_coords, foot_coords);
-  hrp::Vector3 current_foot_mid_pos = current_foot_mid_coords.pos;
-  hrp::Matrix33 current_foot_mid_rot = current_foot_mid_coords.rot;
-  // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
-  hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose());
-  m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
-  rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
-  m_robot->calcForwardKinematics();
-}
-
-bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param, const std::string& limb_name)
-{
-  param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
-  // IK check
-  hrp::Vector3 vel_p, vel_r;
-  vel_p = param.target_p0 - param.target_link->p;
-  rats::difference_rotation(vel_r, param.target_link->R, param.target_r0);
-  if (vel_p.norm() > pos_ik_thre && transition_interpolator->isEmpty()) {
-      if (param.pos_ik_error_count % ik_error_debug_print_freq == 0) {
-          std::cerr << "[" << m_profile.instance_name << "] Too large IK error in " << limb_name << " (vel_p) = [" << vel_p(0) << " " << vel_p(1) << " " << vel_p(2) << "][m], count = " << param.pos_ik_error_count << std::endl;
-      }
-      param.pos_ik_error_count++;
-      has_ik_failed = true;
-  } else {
-      param.pos_ik_error_count = 0;
-  }
-  if (vel_r.norm() > rot_ik_thre && transition_interpolator->isEmpty()) {
-      if (param.rot_ik_error_count % ik_error_debug_print_freq == 0) {
-          std::cerr << "[" << m_profile.instance_name << "] Too large IK error in " << limb_name << " (vel_r) = [" << vel_r(0) << " " << vel_r(1) << " " << vel_r(2) << "][rad], count = " << param.rot_ik_error_count << std::endl;
-      }
-      param.rot_ik_error_count++;
-      has_ik_failed = true;
-  } else {
-      param.rot_ik_error_count = 0;
-  }
-  return true;
-}
-
-void AutoBalancer::solveLimbIK ()
-{
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-    if (it->second.is_active) {
-      for ( int j = 0; j < it->second.manip->numJoints(); j++ ){
-	int i = it->second.manip->joint(j)->jointId;
-	m_robot->joint(i)->q = qorg[i];
-      }
-    }
-  }
-  m_robot->rootLink()->p = current_root_p;
-  m_robot->rootLink()->R = current_root_R;
-  m_robot->calcForwardKinematics();
-  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
-  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
-  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
-  dif_cog *= leg_names_interpolator_ratio;
-  dif_cog(2) = m_robot->rootLink()->p(2) - target_root_p(2);
-  m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog;
-  m_robot->rootLink()->R = target_root_R;
-  // Fix for toe joint
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-      if (it->second.is_active && it->second.has_toe_joint && gg->get_use_toe_joint()) {
-          int i = it->second.target_link->jointId;
-          if (gg->get_swing_leg_names().front() == it->first) {
-              m_robot->joint(i)->q = qrefv[i] + -1 * gg->get_foot_dif_rot_angle();
-          } else {
-              m_robot->joint(i)->q = qrefv[i];
-          }
-      }
-  }
-  m_robot->calcForwardKinematics();
-
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-    if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
-  }
-  if (gg_is_walking && !gg_solved) stopWalking ();
-}
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onAborting(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onError(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onReset(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onStateUpdate(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onRateChanged(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence& limbs)
-{
-  std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
-  Guard guard(m_mutex);
-  double tmp_ratio = 0.0;
-  transition_interpolator->clear();
-  transition_interpolator->set(&tmp_ratio);
-  tmp_ratio = 1.0;
-  transition_interpolator->go(&tmp_ratio, transition_time, true);
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-    it->second.is_active = false;
-  }
-
-  for (size_t i = 0; i < limbs.length(); i++) {
-    ABCIKparam& tmp = ikp[std::string(limbs[i])];
-    tmp.is_active = true;
-    std::cerr << "[" << m_profile.instance_name << "]   limb [" << std::string(limbs[i]) << "]" << std::endl;
-  }
-
-  control_mode = MODE_SYNC_TO_ABC;
-}
-
-void AutoBalancer::stopABCparam()
-{
-  std::cerr << "[" << m_profile.instance_name << "] stop auto balancer mode" << std::endl;
-  //Guard guard(m_mutex);
-  double tmp_ratio = 1.0;
-  transition_interpolator->clear();
-  transition_interpolator->set(&tmp_ratio);
-  tmp_ratio = 0.0;
-  transition_interpolator->go(&tmp_ratio, transition_time, true);
-  control_mode = MODE_SYNC_TO_IDLE;
-}
-
-void AutoBalancer::startWalking ()
-{
-  if ( control_mode != MODE_ABC ) {
-    return_control_mode = control_mode;
-    OpenHRP::AutoBalancerService::StrSequence fix_limbs;
-    fix_limbs.length(2);
-    fix_limbs[0] = "rleg";
-    fix_limbs[1] = "lleg";
-    startABCparam(fix_limbs);
-    waitABCTransition();
-  }
-  {
-    Guard guard(m_mutex);
-    has_ik_failed = false;
-    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-        it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
-    }
-    std::vector<std::string> init_swing_leg_names(gg->get_footstep_front_leg_names());
-    std::vector<std::string> tmp_all_limbs(leg_names);
-    std::vector<std::string> init_support_leg_names;
-    std::sort(tmp_all_limbs.begin(), tmp_all_limbs.end());
-    std::sort(init_swing_leg_names.begin(), init_swing_leg_names.end());
-    std::set_difference(tmp_all_limbs.begin(), tmp_all_limbs.end(),
-                        init_swing_leg_names.begin(), init_swing_leg_names.end(),
-                        std::back_inserter(init_support_leg_names));
-    std::vector<step_node> init_support_leg_steps, init_swing_leg_dst_steps;
-    for (std::vector<std::string>::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++)
-        init_support_leg_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0));
-    for (std::vector<std::string>::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++)
-        init_swing_leg_dst_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0));
-    gg->set_default_zmp_offsets(default_zmp_offsets);
-    gg->initialize_gait_parameter(ref_cog, init_support_leg_steps, init_swing_leg_dst_steps);
-  }
-  is_hand_fix_initial = true;
-  while ( !gg->proc_one_tick() );
-  {
-    Guard guard(m_mutex);
-    gg_is_walking = gg_solved = true;
-  }
-}
-
-void AutoBalancer::stopWalking ()
-{
-  std::vector<coordinates> tmp_end_coords_list;
-  for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
-      if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(ikp[*it].target_end_coords);
-  }
-  multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
-  fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot);
-  gg->clear_footstep_nodes_list();
-  if (return_control_mode == MODE_IDLE) stopABCparam();
-  gg_is_walking = false;
-}
-
-bool AutoBalancer::startAutoBalancer (const OpenHRP::AutoBalancerService::StrSequence& limbs)
-{
-  if (control_mode == MODE_IDLE) {
-    has_ik_failed = false;
-    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-        it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
-    }
-    startABCparam(limbs);
-    waitABCTransition();
-    return_control_mode = MODE_ABC;
-    return true;
-  } else {
-    return false;
-  }
-}
-
-bool AutoBalancer::stopAutoBalancer ()
-{
-  if (control_mode == MODE_ABC) {
-    stopABCparam();
-    waitABCTransition();
-    return true;
-  } else {
-    return false;
-  }
-}
-
-void AutoBalancer::waitABCTransition()
-{
-  while (!transition_interpolator->isEmpty()) usleep(1000);
-  usleep(1000);
-}
-bool AutoBalancer::goPos(const double& x, const double& y, const double& th)
-{
-    //  if ( !gg_is_walking && !is_stop_mode) {
-  if ( !is_stop_mode) {
-    gg->set_all_limbs(leg_names);
-    coordinates start_ref_coords;
-    std::vector<coordinates> initial_support_legs_coords;
-    std::vector<leg_type> initial_support_legs;
-    bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
-    if (is_valid_gait_type == false) return false;
-    bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th,
-                                                      initial_support_legs_coords, // Dummy if gg_is_walking
-                                                      start_ref_coords,            // Dummy if gg_is_walking
-                                                      initial_support_legs,        // Dummy if gg_is_walking
-                                                      (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps.
-    if ( !gg_is_walking ) { // Initializing
-        startWalking();
-    }
-    if (!ret) {
-        std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl;
-    }
-    return ret;
-  } else {
-    std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl;
-    return false;
-  }
-}
-
-bool AutoBalancer::goVelocity(const double& vx, const double& vy, const double& vth)
-{
-  gg->set_all_limbs(leg_names);
-  if (gg_is_walking && gg_solved) {
-    gg->set_velocity_param(vx, vy, vth);
-  } else {
-    coordinates ref_coords;
-    mid_coords(ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
-    std::vector<leg_type> current_legs;
-    switch(gait_type) {
-    case BIPED:
-        current_legs = (vy > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
-        break;
-    case TROT:
-        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
-        break;
-    case PACE:
-        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
-        break;
-    case CRAWL:
-        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    case GALLOP:
-        /* at least one leg shoud be in contact */
-        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    default: break;
-    }
-    gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs);
-    startWalking();
-  }
-  return true;
-}
-
-bool AutoBalancer::goStop ()
-{
-  gg->finalize_velocity_mode();
-  waitFootSteps();
-  return true;
-}
-
-bool AutoBalancer::emergencyStop ()
-{
-  std::cerr << "[" << m_profile.instance_name << "] emergencyStop" << std::endl;
-  // is_stop_mode = true;
-  gg->emergency_stop();
-  waitFootSteps();
-  return true;
-}
-
-bool AutoBalancer::releaseEmergencyStop ()
-{
-  if (is_stop_mode) {
-      std::cerr << "[" << m_profile.instance_name << "] releaseEmergencyStop" << std::endl;
-      is_stop_mode = false;
-  }
-  return true;
-}
-
-bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
-{
-  OpenHRP::AutoBalancerService::StepParamsSequence spss;
-  spss.length(fss.length());
-  // If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
-  // If gg_is_walking is true, do not set to 0.
-  for (size_t i = 0; i < spss.length(); i++) {
-      spss[i].sps.length(fss[i].fs.length());
-      for (size_t j = 0; j < spss[i].sps.length(); j++) {
-          spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
-          spss[i].sps[j].step_time = gg->get_default_step_time();
-          spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle());
-          spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle());
-      }
-  }
-  setFootStepsWithParam(fss, spss, overwrite_fs_idx);
-}
-
-bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
-{
-    if (!is_stop_mode) {
-        std::cerr << "[" << m_profile.instance_name << "] setFootStepsList" << std::endl;
-
-        // Initial footstep Snapping
-        coordinates tmpfs, fstrans;
-        step_node initial_support_step, initial_input_step;
-        {
-            std::vector<step_node> initial_support_steps;
-            if (gg_is_walking) {
-                if (overwrite_fs_idx <= 0) {
-                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
-                    return false;
-                }
-                if (!gg->get_footstep_nodes_by_index(initial_support_steps, overwrite_fs_idx-1)) {
-                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
-                    return false;
-                }
-            } else {
-                // If walking, snap initial leg to current ABC foot coords.
-                for (size_t i = 0; i < fss[0].fs.length(); i++) {
-                    initial_support_steps.push_back(step_node(std::string(fss[0].fs[i].leg), ikp[std::string(fss[0].fs[i].leg)].target_end_coords, 0, 0, 0, 0));
-                }
-            }
-            initial_support_step = initial_support_steps.front(); /* use only one leg for representation */
-        }
-        {
-            std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-            for (size_t i = 0; i < fss[0].fs.length(); i++) {
-                if (std::string(fss[0].fs[i].leg) == leg_type_map[initial_support_step.l_r]) {
-                    coordinates tmp;
-                    memcpy(tmp.pos.data(), fss[0].fs[i].pos, sizeof(double)*3);
-                    tmp.rot = (Eigen::Quaternion<double>(fss[0].fs[i].rot[0], fss[0].fs[i].rot[1], fss[0].fs[i].rot[2], fss[0].fs[i].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
-                    initial_input_step = step_node(std::string(fss[0].fs[i].leg), tmp, 0, 0, 0, 0);
-                }
-            }
-        }
-
-        // Get footsteps
-        std::vector< std::vector<coordinates> > fs_vec_list;
-        std::vector< std::vector<std::string> > leg_name_vec_list;
-        for (size_t i = 0; i < fss.length(); i++) {
-            std::vector<coordinates> fs_vec;
-            std::vector<std::string> leg_name_vec;
-            for (size_t j = 0; j < fss[i].fs.length(); j++) {
-                std::string leg(fss[i].fs[j].leg);
-                if (std::find(leg_names.begin(), leg_names.end(), leg) != leg_names.end()) {
-                    memcpy(tmpfs.pos.data(), fss[i].fs[j].pos, sizeof(double)*3);
-                    tmpfs.rot = (Eigen::Quaternion<double>(fss[i].fs[j].rot[0], fss[i].fs[j].rot[1], fss[i].fs[j].rot[2], fss[i].fs[j].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
-                    initial_input_step.worldcoords.transformation(fstrans, tmpfs);
-                    tmpfs = initial_support_step.worldcoords;
-                    tmpfs.transform(fstrans);
-                } else {
-                    std::cerr << "[" << m_profile.instance_name << "]   No such target : " << leg << std::endl;
-                    return false;
-                }
-                leg_name_vec.push_back(leg);
-                fs_vec.push_back(tmpfs);
-            }
-            leg_name_vec_list.push_back(leg_name_vec);
-            fs_vec_list.push_back(fs_vec);
-        }
-        if (spss.length() != fs_vec_list.size()) {
-            std::cerr << "[" << m_profile.instance_name << "]   StepParam length " << spss.length () << " != Footstep length " << fs_vec_list.size() << std::endl;
-            return false;
-        }
-        std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl;
-        std::vector< std::vector<step_node> > fnsl;
-        for (size_t i = 0; i < fs_vec_list.size(); i++) {
-            if (!(gg_is_walking && i == 0)) { // If initial footstep, e.g., not walking, pass user-defined footstep list. If walking, pass cdr footsteps in order to neglect initial double support leg.
-                std::vector<step_node> tmp_fns;
-                for (size_t j = 0; j < fs_vec_list.at(i).size(); j++) {
-                    tmp_fns.push_back(step_node(leg_name_vec_list[i][j], fs_vec_list[i][j], spss[i].sps[j].step_height, spss[i].sps[j].step_time, spss[i].sps[j].toe_angle, spss[i].sps[j].heel_angle));
-                }
-                fnsl.push_back(tmp_fns);
-            }
-        }
-        if (gg_is_walking) {
-            std::cerr << "[" << m_profile.instance_name << "]  Set overwrite footsteps" << std::endl;
-            gg->set_overwrite_foot_steps_list(fnsl);
-            gg->set_overwrite_foot_step_index(overwrite_fs_idx);
-        } else {
-            std::cerr << "[" << m_profile.instance_name << "]  Set normal footsteps" << std::endl;
-            gg->set_foot_steps_list(fnsl);
-            startWalking();
-        }
-        return true;
-    } else {
-        std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl;
-        return false;
-    }
-}
-
-void AutoBalancer::waitFootSteps()
-{
-  //while (gg_is_walking) usleep(10);
-  while (gg_is_walking || !transition_interpolator->isEmpty() )
-    usleep(1000);
-  usleep(1000);
-  gg->set_offset_velocity_param(0,0,0);
-}
-
-void AutoBalancer::waitFootStepsEarly(const double tm)
-{
-  if (!gg_is_walking) { return;}
-  while ( !gg->is_finalizing(tm)|| !transition_interpolator->isEmpty() )
-    usleep(1000);
-  usleep(1000);
-  gg->set_offset_velocity_param(0,0,0);
-}
-
-bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
-{
-  std::cerr << "[" << m_profile.instance_name << "] setGaitGeneratorParam" << std::endl;
-  gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]);
-  std::vector<hrp::Vector3> off;
-  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
-      off.push_back(hrp::Vector3(i_param.leg_default_translate_pos[i][0], i_param.leg_default_translate_pos[i][1], i_param.leg_default_translate_pos[i][2]));
-  }
-  gg->set_leg_default_translate_pos(off);
-  gg->set_default_step_time(i_param.default_step_time);
-  gg->set_default_step_height(i_param.default_step_height);
-  gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio/2.0);
-  gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio/2.0);
-  gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio/2.0);
-  gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio/2.0);
-  gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio/2.0);
-  gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio/2.0);
-  // gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio_before);
-  // gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio_after);
-  // gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio_before);
-  // gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio_after);
-  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_before);
-  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_after);
-  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_swing_before);
-  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_swing_after);
-  if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::SHUFFLING) {
-    gg->set_default_orbit_type(SHUFFLING);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOID) {
-    gg->set_default_orbit_type(CYCLOID);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::RECTANGLE) {
-    gg->set_default_orbit_type(RECTANGLE);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::STAIR) {
-    gg->set_default_orbit_type(STAIR);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAY) {
-    gg->set_default_orbit_type(CYCLOIDDELAY);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK) {
-    gg->set_default_orbit_type(CYCLOIDDELAYKICK);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CROSS) {
-    gg->set_default_orbit_type(CROSS);
-  }
-  gg->set_swing_trajectory_delay_time_offset(i_param.swing_trajectory_delay_time_offset);
-  gg->set_swing_trajectory_final_distance_weight(i_param.swing_trajectory_final_distance_weight);
-  gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2]));
-  gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2]));  
-  gg->set_gravitational_acceleration(i_param.gravitational_acceleration);
-  gg->set_toe_angle(i_param.toe_angle);
-  gg->set_heel_angle(i_param.heel_angle);
-  gg->set_toe_pos_offset_x(i_param.toe_pos_offset_x);
-  gg->set_heel_pos_offset_x(i_param.heel_pos_offset_x);
-  gg->set_toe_zmp_offset_x(i_param.toe_zmp_offset_x);
-  gg->set_heel_zmp_offset_x(i_param.heel_zmp_offset_x);
-  std::vector<double> tmp_ratio(i_param.toe_heel_phase_ratio.get_buffer(), i_param.toe_heel_phase_ratio.get_buffer()+i_param.toe_heel_phase_ratio.length());
-  std::cerr << "[" << m_profile.instance_name << "]   "; // for set_toe_heel_phase_ratio
-  gg->set_toe_heel_phase_ratio(tmp_ratio);
-  gg->set_use_toe_joint(i_param.use_toe_joint);
-  gg->set_use_toe_heel_transition(i_param.use_toe_heel_transition);
-  gg->set_zmp_weight_map(boost::assign::map_list_of<leg_type, double>(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3]));
-  gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num);
-  gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset);
-  gg->set_leg_margin(i_param.leg_margin);
-  gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation);
-  gg->set_use_stride_limitation(i_param.use_stride_limitation);
-  if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::SQUARE) {
-    gg->set_stride_limitation_type(SQUARE);
-  } else if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::CIRCLE) {
-    gg->set_stride_limitation_type(CIRCLE);
-  }
-
-  // print
-  gg->print_param(std::string(m_profile.instance_name));
-  return true;
-};
-
-bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
-{
-  gg->get_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]);
-  std::vector<hrp::Vector3> off;
-  gg->get_leg_default_translate_pos(off);
-  i_param.leg_default_translate_pos.length(off.size());
-  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
-      i_param.leg_default_translate_pos[i].length(3);
-      i_param.leg_default_translate_pos[i][0] = off[i](0);
-      i_param.leg_default_translate_pos[i][1] = off[i](1);
-      i_param.leg_default_translate_pos[i][2] = off[i](2);
-  }
-  i_param.default_step_time = gg->get_default_step_time();
-  i_param.default_step_height = gg->get_default_step_height();
-  i_param.default_double_support_ratio_before = gg->get_default_double_support_ratio_before();
-  i_param.default_double_support_ratio_after = gg->get_default_double_support_ratio_after();
-  i_param.default_double_support_static_ratio_before = gg->get_default_double_support_static_ratio_before();
-  i_param.default_double_support_static_ratio_after = gg->get_default_double_support_static_ratio_after();
-  i_param.default_double_support_ratio_swing_before = gg->get_default_double_support_ratio_swing_before();
-  i_param.default_double_support_ratio_swing_after = gg->get_default_double_support_ratio_swing_after();
-  i_param.default_double_support_ratio = i_param.default_double_support_ratio_before + i_param.default_double_support_ratio_after;
-  i_param.default_double_support_static_ratio = i_param.default_double_support_static_ratio_before + i_param.default_double_support_static_ratio_after;
-  if (gg->get_default_orbit_type() == SHUFFLING) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::SHUFFLING;
-  } else if (gg->get_default_orbit_type() == CYCLOID) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOID;
-  } else if (gg->get_default_orbit_type() == RECTANGLE) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::RECTANGLE;
-  } else if (gg->get_default_orbit_type() == STAIR) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::STAIR;
-  } else if (gg->get_default_orbit_type() == CYCLOIDDELAY) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAY;
-  } else if (gg->get_default_orbit_type() == CYCLOIDDELAYKICK) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK;
-  } else if (gg->get_default_orbit_type() == CROSS) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CROSS;
-  }
-
-  hrp::Vector3 tmpv = gg->get_stair_trajectory_way_point_offset();
-  for (size_t i = 0; i < 3; i++) i_param.stair_trajectory_way_point_offset[i] = tmpv(i);
-  tmpv = gg->get_cycloid_delay_kick_point_offset();
-  for (size_t i = 0; i < 3; i++) i_param.cycloid_delay_kick_point_offset[i] = tmpv(i);
-  i_param.swing_trajectory_delay_time_offset = gg->get_swing_trajectory_delay_time_offset();
-  i_param.swing_trajectory_final_distance_weight = gg->get_swing_trajectory_final_distance_weight();
-  i_param.gravitational_acceleration = gg->get_gravitational_acceleration();
-  i_param.toe_angle = gg->get_toe_angle();
-  i_param.heel_angle = gg->get_heel_angle();
-  i_param.toe_pos_offset_x = gg->get_toe_pos_offset_x();
-  i_param.heel_pos_offset_x = gg->get_heel_pos_offset_x();
-  i_param.toe_zmp_offset_x = gg->get_toe_zmp_offset_x();
-  i_param.heel_zmp_offset_x = gg->get_heel_zmp_offset_x();
-  std::vector<double> ratio(gg->get_NUM_TH_PHASES(),0.0);
-  gg->get_toe_heel_phase_ratio(ratio);
-  for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) i_param.toe_heel_phase_ratio[i] = ratio[i];
-  i_param.use_toe_joint = gg->get_use_toe_joint();
-  i_param.use_toe_heel_transition = gg->get_use_toe_heel_transition();
-  std::map<leg_type, double> tmp_zmp_weight_map = gg->get_zmp_weight_map();
-  i_param.zmp_weight_map[0] = tmp_zmp_weight_map[RLEG];
-  i_param.zmp_weight_map[1] = tmp_zmp_weight_map[LLEG];
-  i_param.zmp_weight_map[2] = tmp_zmp_weight_map[RARM];
-  i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM];
-  i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num();
-  i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset();
-  for (size_t i=0; i<4; i++) {
-    i_param.leg_margin[i] = gg->get_leg_margin(i);
-  }
-  for (size_t i=0; i<4; i++) {
-    i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i);
-  }
-  i_param.use_stride_limitation = gg->get_use_stride_limitation();
-  if (gg->get_stride_limitation_type() == SQUARE) {
-    i_param.stride_limitation_type = OpenHRP::AutoBalancerService::SQUARE;
-  } else if (gg->get_stride_limitation_type() == CIRCLE) {
-    i_param.stride_limitation_type = OpenHRP::AutoBalancerService::CIRCLE;
-  }
-  return true;
-};
-
-bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
-{
-  Guard guard(m_mutex);
-  std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl;
-  double *default_zmp_offsets_array = new double[ikp.size()*3];
-  move_base_gain = i_param.move_base_gain;
-  for (size_t i = 0; i < ikp.size(); i++)
-    for (size_t j = 0; j < 3; j++)
-      default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j];
-  zmp_transition_time = i_param.zmp_transition_time;
-  adjust_footstep_transition_time = i_param.adjust_footstep_transition_time;
-  if (zmp_offset_interpolator->isEmpty()) {
-      zmp_offset_interpolator->clear();
-      zmp_offset_interpolator->go(default_zmp_offsets_array, zmp_transition_time, true);
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets cannot be set because interpolating." << std::endl;
-  }
-  if (control_mode == MODE_IDLE) {
-    switch (i_param.use_force_mode) {
-    case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
-        use_force = MODE_NO_FORCE;
-        break;
-    case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
-        use_force = MODE_REF_FORCE;
-        break;
-    default:
-        break;
-    }
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   use_force_mode cannot be changed to [" << i_param.use_force_mode << "] during MODE_ABC, MODE_SYNC_TO_IDLE or MODE_SYNC_TO_ABC." << std::endl;
-  }
-  graspless_manip_mode = i_param.graspless_manip_mode;
-  graspless_manip_arm = std::string(i_param.graspless_manip_arm);
-  for (size_t j = 0; j < 3; j++)
-      graspless_manip_p_gain[j] = i_param.graspless_manip_p_gain[j];
-  for (size_t j = 0; j < 3; j++)
-      graspless_manip_reference_trans_coords.pos[j] = i_param.graspless_manip_reference_trans_pos[j];
-  graspless_manip_reference_trans_coords.rot = (Eigen::Quaternion<double>(i_param.graspless_manip_reference_trans_rot[0],
-                                                                          i_param.graspless_manip_reference_trans_rot[1],
-                                                                          i_param.graspless_manip_reference_trans_rot[2],
-                                                                          i_param.graspless_manip_reference_trans_rot[3]).normalized().toRotationMatrix()); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
-  transition_time = i_param.transition_time;
-  std::vector<std::string> cur_leg_names, dst_leg_names;
-  cur_leg_names = leg_names;
-  for (size_t i = 0; i < i_param.leg_names.length(); i++) {
-      dst_leg_names.push_back(std::string(i_param.leg_names[i]));
-  }
-  std::sort(cur_leg_names.begin(), cur_leg_names.end());
-  std::sort(dst_leg_names.begin(), dst_leg_names.end());
-  if (cur_leg_names != dst_leg_names) {
-      if (leg_names_interpolator->isEmpty()) {
-          leg_names.clear();
-          leg_names = dst_leg_names;
-          if (control_mode == MODE_ABC) {
-              double tmp_ratio = 0.0;
-              leg_names_interpolator->set(&tmp_ratio);
-              tmp_ratio = 1.0;
-              leg_names_interpolator->go(&tmp_ratio, 5.0, true);
-              control_mode = MODE_SYNC_TO_ABC;
-          }
-      }
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   leg_names cannot be set because interpolating." << std::endl;
-  }
-  pos_ik_thre = i_param.pos_ik_thre;
-  rot_ik_thre = i_param.rot_ik_thre;
-  if (!gg_is_walking) {
-      is_hand_fix_mode = i_param.is_hand_fix_mode;
-      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode = " << is_hand_fix_mode << std::endl;
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode cannot be set in (gg_is_walking = true). Current is_hand_fix_mode is " << (is_hand_fix_mode?"true":"false") << std::endl;
-  }
-  if (control_mode == MODE_IDLE) {
-      for (size_t i = 0; i < i_param.end_effector_list.length(); i++) {
-          std::map<std::string, ABCIKparam>::iterator it = ikp.find(std::string(i_param.end_effector_list[i].leg));
-          memcpy(it->second.localPos.data(), i_param.end_effector_list[i].pos, sizeof(double)*3);
-          it->second.localR = (Eigen::Quaternion<double>(i_param.end_effector_list[i].rot[0], i_param.end_effector_list[i].rot[1], i_param.end_effector_list[i].rot[2], i_param.end_effector_list[i].rot[3])).normalized().toRotationMatrix();
-      }
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl;
-  }
-  if (i_param.default_gait_type == OpenHRP::AutoBalancerService::BIPED) {
-      gait_type = BIPED;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::TROT) {
-      gait_type = TROT;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::PACE) {
-      gait_type = PACE;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::CRAWL) {
-      gait_type = CRAWL;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) {
-      gait_type = GALLOP;
-  }
-  for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
-      std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   localR = " << it->second.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
-  }
-
-  std::cerr << "[" << m_profile.instance_name << "]   move_base_gain = " << move_base_gain << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets = ";
-  for (size_t i = 0; i < ikp.size() * 3; i++) {
-      std::cerr << default_zmp_offsets_array[i] << " ";
-  }
-  std::cerr << std::endl;
-  delete[] default_zmp_offsets_array;
-  std::cerr << "[" << m_profile.instance_name << "]   use_force_mode = " << use_force << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_mode = " << graspless_manip_mode << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_arm = " << graspless_manip_arm << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_pos = " << graspless_manip_reference_trans_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_rot = " << graspless_manip_reference_trans_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   transition_time = " << transition_time << "[s], zmp_transition_time = " << zmp_transition_time << "[s], adjust_footstep_transition_time = " << adjust_footstep_transition_time << "[s]" << std::endl;
-  for (std::vector<std::string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) std::cerr << "[" << m_profile.instance_name << "]   leg_names [" << *it << "]" << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   pos_ik_thre = " << pos_ik_thre << "[m], rot_ik_thre = " << rot_ik_thre << "[rad]" << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   default_gait_type = " << gait_type << std::endl;
-  // IK limb parameters
-  std::cerr << "[" << m_profile.instance_name << "]  IK limb parameters" << std::endl;
-  bool is_ik_limb_parameter_valid_length = true;
-  if (i_param.ik_limb_parameters.length() != ee_vec.size()) {
-      is_ik_limb_parameter_valid_length = false;
-      std::cerr << "[" << m_profile.instance_name << "]   ik_limb_parameters invalid length! Cannot be set. (input = " << i_param.ik_limb_parameters.length() << ", desired = " << ee_vec.size() << ")" << std::endl;
-  } else {
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          if (ikp[ee_vec[i]].manip->numJoints() != i_param.ik_limb_parameters[i].ik_optional_weight_vector.length())
-              is_ik_limb_parameter_valid_length = false;
-      }
-      if (is_ik_limb_parameter_valid_length) {
-          for (size_t i = 0; i < ee_vec.size(); i++) {
-              ABCIKparam& param = ikp[ee_vec[i]];
-              const OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i];
-              std::vector<double> ov;
-              ov.resize(param.manip->numJoints());
-              for (size_t j = 0; j < param.manip->numJoints(); j++) {
-                  ov[j] = ilp.ik_optional_weight_vector[j];
-              }
-              param.manip->setOptionalWeightVector(ov);
-              param.manip->setSRGain(ilp.sr_gain);
-              param.avoid_gain = ilp.avoid_gain;
-              param.reference_gain = ilp.reference_gain;
-              param.manip->setManipulabilityLimit(ilp.manipulability_limit);
-          }
-      } else {
-          std::cerr << "[" << m_profile.instance_name << "]   ik_optional_weight_vector invalid length! Cannot be set. (input = [";
-          for (size_t i = 0; i < ee_vec.size(); i++) {
-              std::cerr << i_param.ik_limb_parameters[i].ik_optional_weight_vector.length() << ", ";
-          }
-          std::cerr << "], desired = [";
-          for (size_t i = 0; i < ee_vec.size(); i++) {
-              std::cerr << ikp[ee_vec[i]].manip->numJoints() << ", ";
-          }
-          std::cerr << "])" << std::endl;
-      }
-  }
-  if (is_ik_limb_parameter_valid_length) {
-      std::cerr << "[" << m_profile.instance_name << "]   ik_optional_weight_vectors = ";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          ABCIKparam& param = ikp[ee_vec[i]];
-          std::vector<double> ov;
-          ov.resize(param.manip->numJoints());
-          param.manip->getOptionalWeightVector(ov);
-          std::cerr << "[";
-          for (size_t j = 0; j < param.manip->numJoints(); j++) {
-              std::cerr << ov[j] << " ";
-          }
-          std::cerr << "]";
-      }
-      std::cerr << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   sr_gains = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].manip->getSRGain() << ", ";
-      }
-      std::cerr << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   avoid_gains = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].avoid_gain << ", ";
-      }
-      std::cerr << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   reference_gains = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].reference_gain << ", ";
-      }
-      std::cerr << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   manipulability_limits = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].manip->getManipulabilityLimit() << ", ";
-      }
-      std::cerr << "]" << std::endl;
-  }
-  return true;
-};
-
-bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
-{
-  i_param.move_base_gain = move_base_gain;
-  i_param.default_zmp_offsets.length(ikp.size());
-  for (size_t i = 0; i < ikp.size(); i++) {
-      i_param.default_zmp_offsets[i].length(3);
-      for (size_t j = 0; j < 3; j++) {
-          i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j);
-      }
-  }
-  switch(control_mode) {
-  case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break;
-  case MODE_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_ABC; break;
-  case MODE_SYNC_TO_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_IDLE; break;
-  case MODE_SYNC_TO_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_ABC; break;
-  default: break;
-  }
-  switch(use_force) {
-  case MODE_NO_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_NO_FORCE; break;
-  case MODE_REF_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE; break;
-  default: break;
-  }
-  i_param.graspless_manip_mode = graspless_manip_mode;
-  i_param.graspless_manip_arm = graspless_manip_arm.c_str();
-  for (size_t j = 0; j < 3; j++)
-      i_param.graspless_manip_p_gain[j] = graspless_manip_p_gain[j];
-  for (size_t j = 0; j < 3; j++)
-      i_param.graspless_manip_reference_trans_pos[j] = graspless_manip_reference_trans_coords.pos[j];
-  Eigen::Quaternion<double> qt(graspless_manip_reference_trans_coords.rot);
-  i_param.graspless_manip_reference_trans_rot[0] = qt.w();
-  i_param.graspless_manip_reference_trans_rot[1] = qt.x();
-  i_param.graspless_manip_reference_trans_rot[2] = qt.y();
-  i_param.graspless_manip_reference_trans_rot[3] = qt.z();
-  i_param.transition_time = transition_time;
-  i_param.zmp_transition_time = zmp_transition_time;
-  i_param.adjust_footstep_transition_time = adjust_footstep_transition_time;
-  i_param.leg_names.length(leg_names.size());
-  for (size_t i = 0; i < leg_names.size(); i++) i_param.leg_names[i] = leg_names.at(i).c_str();
-  i_param.pos_ik_thre = pos_ik_thre;
-  i_param.rot_ik_thre = rot_ik_thre;
-  i_param.is_hand_fix_mode = is_hand_fix_mode;
-  i_param.end_effector_list.length(ikp.size());
-  {
-      size_t i = 0;
-      for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
-          copyRatscoords2Footstep(i_param.end_effector_list[i],
-                                  coordinates(it->second.localPos, it->second.localR));
-          i_param.end_effector_list[i].leg = it->first.c_str();
-          i++;
-      }
-  }
-  switch(gait_type) {
-  case BIPED:  i_param.default_gait_type = OpenHRP::AutoBalancerService::BIPED;  break;
-  case TROT:   i_param.default_gait_type = OpenHRP::AutoBalancerService::TROT;   break;
-  case PACE:   i_param.default_gait_type = OpenHRP::AutoBalancerService::PACE;   break;
-  case CRAWL:  i_param.default_gait_type = OpenHRP::AutoBalancerService::CRAWL;  break;
-  case GALLOP: i_param.default_gait_type = OpenHRP::AutoBalancerService::GALLOP; break;
-  default: break;
-  }
-  i_param.ik_limb_parameters.length(ee_vec.size());
-  for (size_t i = 0; i < ee_vec.size(); i++) {
-      ABCIKparam& param = ikp[ee_vec[i]];
-      OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i];
-      ilp.ik_optional_weight_vector.length(param.manip->numJoints());
-      std::vector<double> ov;
-      ov.resize(param.manip->numJoints());
-      param.manip->getOptionalWeightVector(ov);
-      for (size_t j = 0; j < param.manip->numJoints(); j++) {
-          ilp.ik_optional_weight_vector[j] = ov[j];
-      }
-      ilp.sr_gain = param.manip->getSRGain();
-      ilp.avoid_gain = param.avoid_gain;
-      ilp.reference_gain = param.reference_gain;
-      ilp.manipulability_limit = param.manip->getManipulabilityLimit();
-  }
-  return true;
-};
-
-void AutoBalancer::copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs)
-{
-  memcpy(out_fs.pos, in_fs.pos.data(), sizeof(double)*3);
-  Eigen::Quaternion<double> qt(in_fs.rot);
-  out_fs.rot[0] = qt.w();
-  out_fs.rot[1] = qt.x();
-  out_fs.rot[2] = qt.y();
-  out_fs.rot[3] = qt.z();
-};
-
-bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param)
-{
-  copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_leg_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.swing_leg_dst_coords, gg->get_swing_leg_dst_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.dst_foot_midcoords, gg->get_dst_foot_midcoords());
-  if (gg->get_support_leg_names().front() == "rleg") {
-    i_param.support_leg = OpenHRP::AutoBalancerService::RLEG;
-  } else {
-    i_param.support_leg = OpenHRP::AutoBalancerService::LLEG;
-  }
-  switch ( gg->get_current_support_states().front() ) {
-  case BOTH: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::BOTH; break;
-  case RLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::RLEG; break;
-  case LLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::LLEG; break;
-  default: break;
-  }
-  return true;
-};
-
-bool AutoBalancer::adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep)
-{
-  std::cerr << "[" << m_profile.instance_name << "] adjustFootSteps" << std::endl;
-  if (control_mode == MODE_ABC && !gg_is_walking && adjust_footstep_interpolator->isEmpty()) {
-      Guard guard(m_mutex);
-      //
-      hrp::Vector3 eepos, org_mid_rpy, target_mid_rpy;
-      hrp::Matrix33 eerot, tmprot;
-      coordinates org_mid_coords, target_mid_coords;
-      // Get org coords
-      ikp["rleg"].adjust_interpolation_org_p0 = ikp["rleg"].target_p0;
-      ikp["lleg"].adjust_interpolation_org_p0 = ikp["lleg"].target_p0;
-      ikp["rleg"].adjust_interpolation_org_r0 = ikp["rleg"].target_r0;
-      ikp["lleg"].adjust_interpolation_org_r0 = ikp["lleg"].target_r0;
-      mid_coords(org_mid_coords, 0.5,
-                 coordinates(ikp["rleg"].adjust_interpolation_org_p0, ikp["rleg"].adjust_interpolation_org_r0),
-                 coordinates(ikp["lleg"].adjust_interpolation_org_p0, ikp["lleg"].adjust_interpolation_org_r0));
-      org_mid_rpy = hrp::rpyFromRot(org_mid_coords.rot);
-      // Get target coords
-      //   Input : ee coords
-      //   Output : link coords
-      memcpy(eepos.data(), rfootstep.pos, sizeof(double)*3);
-      eerot = (Eigen::Quaternion<double>(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
-      ikp["rleg"].adjust_interpolation_target_r0 = eerot * ikp["rleg"].localR.transpose();
-      ikp["rleg"].adjust_interpolation_target_p0 = eepos - ikp["rleg"].adjust_interpolation_target_r0 * ikp["rleg"].localPos;
-      memcpy(eepos.data(), lfootstep.pos, sizeof(double)*3);
-      eerot = (Eigen::Quaternion<double>(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
-      ikp["lleg"].adjust_interpolation_target_r0 = eerot * ikp["lleg"].localR.transpose();
-      ikp["lleg"].adjust_interpolation_target_p0 = eepos - ikp["lleg"].adjust_interpolation_target_r0 * ikp["lleg"].localPos;
-      mid_coords(target_mid_coords, 0.5,
-                 coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0),
-                 coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
-      coordinates rtrans, ltrans;
-      target_mid_coords.transformation(rtrans, coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0));
-      target_mid_coords.transformation(ltrans, coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
-      target_mid_rpy = hrp::rpyFromRot(target_mid_coords.rot);
-      // Fix target pos => org pos, target yaw => org yaw
-      target_mid_rpy(2) = org_mid_rpy(2);
-      target_mid_coords.rot = hrp::rotFromRpy(target_mid_rpy);
-      target_mid_coords.pos = org_mid_coords.pos;
-      // Calculate rleg and lleg coords
-      coordinates tmpc;
-      tmpc = target_mid_coords;
-      tmpc.transform(rtrans);
-      ikp["rleg"].adjust_interpolation_target_p0 = tmpc.pos;
-      ikp["rleg"].adjust_interpolation_target_r0 = tmpc.rot;
-      tmpc = target_mid_coords;
-      tmpc.transform(ltrans);
-      ikp["lleg"].adjust_interpolation_target_p0 = tmpc.pos;
-      ikp["lleg"].adjust_interpolation_target_r0 = tmpc.rot;
-      // Set interpolator
-      adjust_footstep_interpolator->clear();
-      double tmp = 0.0;
-      adjust_footstep_interpolator->set(&tmp);
-      tmp = 1.0;
-      adjust_footstep_interpolator->go(&tmp, adjust_footstep_transition_time, true);
-  }
-  while (!adjust_footstep_interpolator->isEmpty() )
-    usleep(1000);
-  usleep(1000);
-  return true;
-};
-
-bool AutoBalancer::getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx)
-{
-    std::cerr << "[" << m_profile.instance_name << "] getRemainingFootstepSequence" << std::endl;
-    o_footstep = new OpenHRP::AutoBalancerService::FootstepSequence;
-    if (gg_is_walking) {
-        std::vector< std::vector<step_node> > fsnl = gg->get_remaining_footstep_nodes_list();
-        o_current_fs_idx = gg->get_footstep_index();
-        o_footstep->length(fsnl.size());
-        for (size_t i = 0; i < fsnl.size(); i++) {
-            o_footstep[i].leg = (fsnl[i].front().l_r==RLEG?"rleg":"lleg");
-            copyRatscoords2Footstep(o_footstep[i], fsnl[i].front().worldcoords);
-        }
-    }
-};
-
-bool AutoBalancer::getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep)
-{
-    std::cerr << "[" << m_profile.instance_name << "] getGoPosFootstepsSequence" << std::endl;
-    o_footstep = new OpenHRP::AutoBalancerService::FootstepsSequence;
-    if (gg_is_walking) {
-        std::cerr << "[" << m_profile.instance_name << "] Cannot call getGoPosFootstepsSequence in walking" << std::endl;
-        return false;
-    } else {
-        gg->set_all_limbs(leg_names);
-        std::vector< std::vector<step_node> > new_footstep_nodes_list;
-        coordinates start_ref_coords;
-        std::vector<coordinates> initial_support_legs_coords;
-        std::vector<leg_type> initial_support_legs;
-        bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
-        if (is_valid_gait_type == false) return false;
-        /* go_pos_param_2_footstep_nodes_list_core is const member function  */
-        gg->go_pos_param_2_footstep_nodes_list_core (x, y, th,
-                                                     initial_support_legs_coords, start_ref_coords, initial_support_legs,
-                                                     new_footstep_nodes_list, true, 0);
-        o_footstep->length(new_footstep_nodes_list.size());
-        for (size_t i = 0; i < new_footstep_nodes_list.size(); i++) {
-            o_footstep[i].fs.length(new_footstep_nodes_list.at(i).size());
-            for (size_t j = 0; j < new_footstep_nodes_list.at(i).size(); j++) {
-                leg_type tmp_leg_type = new_footstep_nodes_list.at(i).at(j).l_r;
-                o_footstep[i].fs[j].leg = ((tmp_leg_type == RLEG) ? "rleg":
-                                           (tmp_leg_type == LLEG) ? "lleg":
-                                           (tmp_leg_type == RARM) ? "rarm":
-                                           "larm");
-                copyRatscoords2Footstep(o_footstep[i].fs[j], new_footstep_nodes_list.at(i).at(j).worldcoords);
-            }
-        }
-        return true;
-    }
-};
-
-void AutoBalancer::static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height)
-{
-  hrp::Vector3 target_sbp = hrp::Vector3(0, 0, 0);
-  hrp::Vector3 tmpcog = m_robot->calcCM();
-  switch ( use_force ) {
-  case MODE_REF_FORCE:
-    calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height, ref_forces);
-    tmp_input_sbp = target_sbp - sbp_offset;
-    sbp_cog_offset = tmp_input_sbp - tmpcog;
-    break;
-  case MODE_NO_FORCE:
-    tmp_input_sbp = tmpcog + sbp_cog_offset;
-    break;
-  default: break;
-  }
-};
-
-void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces)
-{
-  hrp::Vector3 denom, nume;
-  /* sb_point[m] = nume[kg * m/s^2 * m] / denom[kg * m/s^2] */
-  double mass = m_robot->totalMass();
-  for (size_t j = 0; j < 2; j++) {
-    nume(j) = mass * gg->get_gravitational_acceleration() * tmpcog(j);
-    denom(j) = mass * gg->get_gravitational_acceleration();
-    for (size_t i = 0; i < sensor_names.size(); i++) {
-      if ( sensor_names[i].find("hsensor") != std::string::npos || sensor_names[i].find("asensor") != std::string::npos ) { // tempolary to get arm force coords
-          hrp::Link* parentlink;
-          hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
-          if (sensor) parentlink = sensor->link;
-          else parentlink = m_vfs[sensor_names[i]].link;
-          for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-              if (it->second.target_link->name == parentlink->name) {
-                  hrp::Vector3 fpos = parentlink->p + parentlink->R * it->second.localPos;
-                  nume(j) += ( (fpos(2) - ref_com_height) * tmp_forces[i](j) - fpos(j) * tmp_forces[i](2) );
-                  denom(j) -= tmp_forces[i](2);
-              }
-          }
-      }
-    }
-    sb_point(j) = nume(j) / denom(j);
-  }
-  sb_point(2) = ref_com_height;
-};
-
-#ifndef rad2deg
-#define rad2deg(rad) (rad * 180 / M_PI)
-#endif
-#ifndef deg2rad
-#define deg2rad(deg) (deg * M_PI / 180)
-#endif
-
-hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_coords)
-{
-  if (graspless_manip_mode) {
-    hrp::Vector3 dp,dr;
-    coordinates ref_hand_coords(gg->get_dst_foot_midcoords()), act_hand_coords;
-    ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords
-    hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos);
-    if ( graspless_manip_arm == "arms" ) {
-        hrp::Vector3 rarm_pos = ikp["rarm"].target_p0 + ikp["rarm"].target_r0 * ikp["rarm"].localPos;
-        hrp::Vector3 larm_pos = ikp["larm"].target_p0 + ikp["larm"].target_r0 * ikp["larm"].localPos;
-        act_hand_coords.pos = (rarm_pos+larm_pos)/2.0;
-        hrp::Vector3 act_y = larm_pos-rarm_pos;
-        act_y(2) = 0;
-        act_y.normalize();
-        hrp::Vector3 ref_y(ref_hand_coords.rot * hrp::Vector3::UnitY());
-        ref_y(2) = 0;
-        ref_y.normalize();
-        dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y))); // fix for rotation
-    } else {
-      ABCIKparam& tmpikp = ikp[graspless_manip_arm];
-      act_hand_coords = rats::coordinates(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos,
-                                          tmpikp.target_r0 * tmpikp.localR);
-      rats::difference_rotation(dr, ref_hand_coords.rot, act_hand_coords.rot);
-      dr(0) = 0; dr(1) = 0;
-    }
-    dp = act_hand_coords.pos - ref_hand_coords.pos
-        + dr.cross(hrp::Vector3(foot_pos - act_hand_coords.pos));
-    dp(2) = 0;
-    hrp::Matrix33 foot_mt(gg->get_dst_foot_midcoords().rot.transpose());
-    //alias(dp) = foot_mt * dp;
-    hrp::Vector3 dp2 = foot_mt * dp;
-    //alias(dr) = foot_mt * dr;
-    return hrp::Vector3(graspless_manip_p_gain[0] * dp2(0)/gg->get_default_step_time(),
-                        graspless_manip_p_gain[1] * dp2(1)/gg->get_default_step_time(),
-                        graspless_manip_p_gain[2] * rad2deg(dr(2))/gg->get_default_step_time());
-  } else {
-    return hrp::Vector3::Zero();
-  }
-};
-
-bool AutoBalancer::calc_inital_support_legs(const double& y, std::vector<coordinates>& initial_support_legs_coords, std::vector<leg_type>& initial_support_legs, coordinates& start_ref_coords) {
-    switch(gait_type) {
-    case BIPED:
-        initial_support_legs_coords = (y > 0 ?
-                                       boost::assign::list_of(ikp["rleg"].target_end_coords)
-                                       : boost::assign::list_of(ikp["lleg"].target_end_coords));
-        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
-        break;
-    case TROT:
-        initial_support_legs_coords = (y > 0 ?
-                                       boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["larm"].target_end_coords)
-                                       : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["rarm"].target_end_coords));
-        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
-        break;
-    case PACE:
-        initial_support_legs_coords = (y > 0 ?
-                                       boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["rarm"].target_end_coords)
-                                       : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["larm"].target_end_coords));
-        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
-        break;
-    case CRAWL:
-        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    case GALLOP:
-        /* at least one leg shoud be in contact */
-        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    default: break;
-    }
-    mid_coords(start_ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
-    return true;
-};
-
-//
-extern "C"
-{
-
-    void AutoBalancerInit(RTC::Manager* manager)
-    {
-        RTC::Properties profile(autobalancer_spec);
-        manager->registerFactory(profile,
-                                 RTC::Create<AutoBalancer>,
-                                 RTC::Delete<AutoBalancer>);
-    }
-
-};
-
-
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp.my b/rtc/AutoBalancer/AutoBalancer.cpp.my
deleted file mode 100644
index 638a4493673..00000000000
--- a/rtc/AutoBalancer/AutoBalancer.cpp.my
+++ /dev/null
@@ -1,2218 +0,0 @@
-// -*- C++ -*-
-/*!
- * @file  AutoBalancer.cpp
- * @brief autobalancer component
- * $Date$
- *
- * $Id$
- */
-
-#include <rtm/CorbaNaming.h>
-#include <hrpModel/Link.h>
-#include <hrpModel/Sensor.h>
-#include <hrpModel/ModelLoaderUtil.h>
-#include "AutoBalancer.h"
-#include <hrpModel/JointPath.h>
-#include <hrpUtil/MatrixSolvers.h>
-#include "hrpsys/util/Hrpsys.h"
-
-
-typedef coil::Guard<coil::Mutex> Guard;
-using namespace rats;
-
-// Module specification
-// <rtc-template block="module_spec">
-static const char* autobalancer_spec[] =
-    {
-        "implementation_id", "AutoBalancer",
-        "type_name",         "AutoBalancer",
-        "description",       "autobalancer component",
-        "version",           HRPSYS_PACKAGE_VERSION,
-        "vendor",            "AIST",
-        "category",          "example",
-        "activity_type",     "DataFlowComponent",
-        "max_instance",      "10",
-        "language",          "C++",
-        "lang_type",         "compile",
-        // Configuration variables
-        "conf.default.debugLevel", "0",
-        ""
-    };
-// </rtc-template>
-
-AutoBalancer::AutoBalancer(RTC::Manager* manager)
-    : RTC::DataFlowComponentBase(manager),
-      // <rtc-template block="initializer">
-      m_qRefIn("qRef", m_qRef),
-      m_basePosIn("basePosIn", m_basePos),
-      m_baseRpyIn("baseRpyIn", m_baseRpy),
-      m_zmpIn("zmpIn", m_zmp),
-      m_optionalDataIn("optionalData", m_optionalData),
-      m_emergencySignalIn("emergencySignal", m_emergencySignal),
-      //ishiguro
-      m_htzmpIn("htzmpIn", m_htzmp),
-      m_htrfwIn("htrfwIn", m_htrfw),
-      m_htlfwIn("htlfwIn", m_htlfw),
-      m_htcomIn("htcomIn", m_htcom),
-      m_htrfIn("htrfIn", m_htrf),
-      m_htlfIn("htlfIn", m_htlf),
-      m_htrhIn("htrhIn", m_htrh),
-      m_htlhIn("htlhIn", m_htlh),
-      m_actzmpIn("actzmpIn", m_actzmp),
-
-      m_qOut("q", m_qRef),
-      m_zmpOut("zmpOut", m_zmp),
-      m_basePosOut("basePosOut", m_basePos),
-      m_baseRpyOut("baseRpyOut", m_baseRpy),
-      m_baseTformOut("baseTformOut", m_baseTform),
-      m_basePoseOut("basePoseOut", m_basePose),
-      m_accRefOut("accRef", m_accRef),
-      m_contactStatesOut("contactStates", m_contactStates),
-      m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime),
-      m_cogOut("cogOut", m_cog),
-      m_AutoBalancerServicePort("AutoBalancerService"),
-      m_walkingStatesOut("walkingStates", m_walkingStates),
-      m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
-      m_legMarginIn("legMargin", m_legMargin),
-      // </rtc-template>
-      gait_type(BIPED),
-      move_base_gain(0.8),
-      m_robot(hrp::BodyPtr()),
-      m_debugLevel(0)
-{
-    m_service0.autobalancer(this);
-}
-
-AutoBalancer::~AutoBalancer()
-{
-}
-
-
-RTC::ReturnCode_t AutoBalancer::onInitialize()
-{
-    std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
-    bindParameter("debugLevel", m_debugLevel, "0");
-
-    // Registration: InPort/OutPort/Service
-    // <rtc-template block="registration">
-    // Set InPort buffers
-    addInPort("qRef", m_qRefIn);
-    addInPort("basePosIn", m_basePosIn);
-    addInPort("baseRpyIn", m_baseRpyIn);
-    addInPort("zmpIn", m_zmpIn);
-    addInPort("optionalData", m_optionalDataIn);
-    addInPort("emergencySignal", m_emergencySignalIn);
-    //ishiguro
-    addInPort("htzmpIn", m_htzmpIn);
-    addInPort("htrfwIn", m_htrfwIn);
-    addInPort("htlfwIn", m_htlfwIn);
-    addInPort("htcomIn", m_htcomIn);
-    addInPort("htrfIn", m_htrfIn);
-    addInPort("htlfIn", m_htlfIn);
-    addInPort("htrhIn", m_htrhIn);
-    addInPort("htlhIn", m_htlhIn);
-    addInPort("actzmpIn", m_actzmpIn);
-
-
-    addInPort("legMargin", m_legMarginIn);
-
-    // Set OutPort buffer
-    addOutPort("q", m_qOut);
-    addOutPort("zmpOut", m_zmpOut);
-    addOutPort("basePosOut", m_basePosOut);
-    addOutPort("baseRpyOut", m_baseRpyOut);
-    addOutPort("baseTformOut", m_baseTformOut);
-    addOutPort("basePoseOut", m_basePoseOut);
-    addOutPort("accRef", m_accRefOut);
-    addOutPort("contactStates", m_contactStatesOut);
-    addOutPort("controlSwingSupportTime", m_controlSwingSupportTimeOut);
-    addOutPort("cogOut", m_cogOut);
-    addOutPort("walkingStates", m_walkingStatesOut);
-    addOutPort("sbpCogOffset", m_sbpCogOffsetOut);
-  
-    // Set service provider to Ports
-    m_AutoBalancerServicePort.registerProvider("service0", "AutoBalancerService", m_service0);
-  
-    // Set service consumers to Ports
-  
-    // Set CORBA Service Ports
-    addPort(m_AutoBalancerServicePort);
-  
-    // </rtc-template>
-    // <rtc-template block="bind_config">
-    // Bind variables and configuration variable
-  
-    // </rtc-template>
-
-    RTC::Properties& prop =  getProperties();
-    coil::stringTo(m_dt, prop["dt"].c_str());
-
-    m_robot = hrp::BodyPtr(new hrp::Body());
-    RTC::Manager& rtcManager = RTC::Manager::instance();
-    std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
-    int comPos = nameServer.find(",");
-    if (comPos < 0){
-        comPos = nameServer.length();
-    }
-    nameServer = nameServer.substr(0, comPos);
-    RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
-    if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
-                                 CosNaming::NamingContext::_duplicate(naming.getRootContext())
-                                 )){
-      std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
-      return RTC::RTC_ERROR;
-    }
-
-    // allocate memory for outPorts
-    m_qRef.data.length(m_robot->numJoints());
-    qorg.resize(m_robot->numJoints());
-    qrefv.resize(m_robot->numJoints());
-    m_baseTform.data.length(12);
-    //ishiguro
-    m_htrfw.data.length(6);
-    m_htlfw.data.length(6);
-
-    control_mode = MODE_IDLE;
-    loop = 0;
-
-    // setting from conf file
-    // GaitGenerator requires abc_leg_offset and abc_stride_parameter in robot conf file
-    // setting leg_pos from conf file
-    coil::vstring leg_offset_str = coil::split(prop["abc_leg_offset"], ",");
-    leg_names.push_back("rleg");
-    leg_names.push_back("lleg");
-
-    // setting from conf file
-    // rleg,TARGET_LINK,BASE_LINK
-    coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
-    size_t prop_num = 10;
-    if (end_effectors_str.size() > 0) {
-      size_t num = end_effectors_str.size()/prop_num;
-      for (size_t i = 0; i < num; i++) {
-        std::string ee_name, ee_target, ee_base;
-        coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
-        coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
-        coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
-        ABCIKparam tp;
-        for (size_t j = 0; j < 3; j++) {
-            coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
-        }
-        double tmpv[4];
-        for (int j = 0; j < 4; j++ ) {
-          coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
-        }
-        tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
-        tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
-        // Fix for toe joint
-        //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
-        //   Without toe joints, "end-effector link == force-sensor link" is assumed.
-        //   With toe joints, "end-effector link != force-sensor link" is assumed.
-        if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
-            std::vector<double> optw(tp.manip->numJoints(), 1.0);
-            optw.back() = 0.0; // Set weight = 0 for toe joint by default
-            tp.manip->setOptionalWeightVector(optw);
-            tp.has_toe_joint = true;
-        } else {
-            tp.has_toe_joint = false;
-        }
-        tp.avoid_gain = 0.001;
-        tp.reference_gain = 0.01;
-        tp.pos_ik_error_count = tp.rot_ik_error_count = 0;
-        ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
-        ikp[ee_name].target_link = m_robot->link(ee_target);
-        ee_vec.push_back(ee_name);
-        std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   target = " << ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
-        contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
-      }
-      m_contactStates.data.length(num);
-      if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
-        m_contactStates.data[contact_states_index_map["rleg"]] = true;
-        m_contactStates.data[contact_states_index_map["lleg"]] = true;
-      }
-      if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) {
-        m_contactStates.data[contact_states_index_map["rarm"]] = false;
-        m_contactStates.data[contact_states_index_map["larm"]] = false;
-      }
-      m_controlSwingSupportTime.data.length(num);
-      for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 0.0;
-    }
-    std::vector<hrp::Vector3> leg_pos;
-    if (leg_offset_str.size() > 0) {
-      hrp::Vector3 leg_offset;
-      for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
-      std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-      leg_pos.push_back(hrp::Vector3(-1*leg_offset));
-      leg_pos.push_back(hrp::Vector3(leg_offset));
-    }
-    if (leg_pos.size() < ikp.size()) {
-        size_t tmp_leg_pos_size = leg_pos.size();
-        for (size_t i = 0; i < ikp.size() - tmp_leg_pos_size; i++) {
-            leg_pos.push_back(hrp::Vector3::Zero());
-        }
-    }
-
-    std::vector<std::pair<hrp::Link*, hrp::Link*> > interlocking_joints;
-    readInterlockingJointsParamFromProperties(interlocking_joints, m_robot, prop["interlocking_joints"], std::string(m_profile.instance_name));
-    if (interlocking_joints.size() > 0) {
-        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            std::cerr << "[" << m_profile.instance_name << "] Interlocking Joints for [" << it->first << "]" << std::endl;
-            it->second.manip->setInterlockingJointPairIndices(interlocking_joints, std::string(m_profile.instance_name));
-        }
-    }
-
-    zmp_offset_interpolator = new interpolator(ikp.size()*3, m_dt);
-    zmp_offset_interpolator->setName(std::string(m_profile.instance_name)+" zmp_offset_interpolator");
-    zmp_transition_time = 1.0;
-    transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-    transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator");
-    transition_interpolator_ratio = 1.0;
-    adjust_footstep_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-    adjust_footstep_interpolator->setName(std::string(m_profile.instance_name)+" adjust_footstep_interpolator");
-    transition_time = 2.0;
-    adjust_footstep_transition_time = 2.0;
-    leg_names_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-    leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator");
-    leg_names_interpolator_ratio = 1.0;
-
-    // setting stride limitations from conf file
-    double stride_fwd_x_limit = 0.15;
-    double stride_y_limit = 0.05;
-    double stride_th_limit = 10;
-    double stride_bwd_x_limit = 0.05;
-    std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_y_limit << "[m], " << stride_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl;
-    if (default_zmp_offsets.size() == 0) {
-      for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
-    }
-    if (leg_offset_str.size() > 0) {
-      gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_y_limit/*[m]*/, stride_th_limit/*[deg]*/, stride_bwd_x_limit/*[m]*/));
-      gg->set_default_zmp_offsets(default_zmp_offsets);
-    }
-    gg_is_walking = gg_solved = false;
-    m_walkingStates.data = false;
-    fix_leg_coords = coordinates();
-
-    // load virtual force sensors
-    readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
-    // ref force port
-    int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
-    int nvforce = m_vfs.size();
-    int nforce  = npforce + nvforce;
-    m_ref_force.resize(nforce);
-    m_ref_forceIn.resize(nforce);
-    m_force.resize(nforce);
-    m_ref_forceOut.resize(nforce);
-    m_limbCOPOffset.resize(nforce);
-    m_limbCOPOffsetOut.resize(nforce);
-    for (unsigned int i=0; i<npforce; i++){
-        sensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
-    }
-    for (unsigned int i=0; i<nvforce; i++){
-        for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
-            if (it->second.id == i) sensor_names.push_back(it->first);
-        }
-    }
-    // set ref force port
-    std::cerr << "[" << m_profile.instance_name << "] force sensor ports (" << nforce << ")" << std::endl;
-    for (unsigned int i=0; i<nforce; i++){
-        m_ref_forceIn[i] = new InPort<TimedDoubleSeq>(std::string("ref_"+sensor_names[i]).c_str(), m_ref_force[i]);
-        m_ref_force[i].data.length(6);
-        registerInPort(std::string("ref_"+sensor_names[i]).c_str(), *m_ref_forceIn[i]);
-        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string("ref_"+sensor_names[i]) << std::endl;
-        ref_forces.push_back(hrp::Vector3(0,0,0));
-    }
-    // set force port
-    for (unsigned int i=0; i<nforce; i++){
-        m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string(sensor_names[i]).c_str(), m_force[i]);
-        m_force[i].data.length(6);
-        m_force[i].data[0] = m_force[i].data[1] = m_force[i].data[2] = 0.0;
-        m_force[i].data[3] = m_force[i].data[4] = m_force[i].data[5] = 0.0;
-        registerOutPort(std::string(sensor_names[i]).c_str(), *m_ref_forceOut[i]);
-        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string(sensor_names[i]) << std::endl;
-    }
-    // set limb cop offset port
-    std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << nforce << ")" << std::endl;
-    for (unsigned int i=0; i<nforce; i++){
-        std::string nm("limbCOPOffset_"+sensor_names[i]);
-        m_limbCOPOffsetOut[i] = new OutPort<TimedPoint3D>(nm.c_str(), m_limbCOPOffset[i]);
-        registerOutPort(nm.c_str(), *m_limbCOPOffsetOut[i]);
-        m_limbCOPOffset[i].data.x = m_limbCOPOffset[i].data.y = m_limbCOPOffset[i].data.z = 0.0;
-        std::cerr << "[" << m_profile.instance_name << "]   name = " << nm << std::endl;
-    }
-    sbp_offset = hrp::Vector3(0,0,0);
-    sbp_cog_offset = hrp::Vector3(0,0,0);
-    //use_force = MODE_NO_FORCE;
-    use_force = MODE_REF_FORCE;
-
-    if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
-      is_legged_robot = true;
-    } else {
-      is_legged_robot = false;
-    }
-
-    m_accRef.data.ax = m_accRef.data.ay = m_accRef.data.az = 0.0;
-    prev_imu_sensor_vel = hrp::Vector3::Zero();
-
-    graspless_manip_mode = false;
-    graspless_manip_arm = "arms";
-    graspless_manip_p_gain = hrp::Vector3::Zero();
-
-    is_stop_mode = false;
-    has_ik_failed = false;
-    is_hand_fix_mode = false;
-
-    pos_ik_thre = 0.1*1e-3; // [m]
-    rot_ik_thre = (1e-2)*M_PI/180.0; // [rad]
-    ik_error_debug_print_freq = static_cast<int>(0.2/m_dt); // once per 0.2 [s]
-
-    //for HumanSynchronizer
-    hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer());
-
-    return RTC::RTC_OK;
-}
-
-
-RTC::ReturnCode_t AutoBalancer::onFinalize()
-{
-  delete zmp_offset_interpolator;
-  delete transition_interpolator;
-  delete adjust_footstep_interpolator;
-  delete leg_names_interpolator;
-  return RTC::RTC_OK;
-}
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onStartup(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onShutdown(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id)
-{
-    std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
-    
-    return RTC::RTC_OK;
-}
-
-RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
-{
-  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
-  Guard guard(m_mutex);
-  if (control_mode == MODE_ABC) {
-    control_mode = MODE_SYNC_TO_IDLE;
-    double tmp_ratio = 0.0;
-    transition_interpolator->go(&tmp_ratio, m_dt, true); // sync in one controller loop
-  }
-  return RTC::RTC_OK;
-}
-
-
-
-
-#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
-//#define DEBUGP2 ((loop%200==0))
-#define DEBUGP2 (false)
-RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
-{
-  // std::cerr << "AutoBalancer::onExecute(" << ec_id << ")" << std::endl;
-    loop ++;
-    if (m_qRefIn.isNew()) {
-        m_qRefIn.read();
-    }
-    if (m_basePosIn.isNew()) {
-      m_basePosIn.read();
-      input_basePos(0) = m_basePos.data.x;
-      input_basePos(1) = m_basePos.data.y;
-      input_basePos(2) = m_basePos.data.z;
-    }
-    if (m_baseRpyIn.isNew()) {
-      m_baseRpyIn.read();
-      input_baseRot = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
-    }
-    if (m_zmpIn.isNew()) {
-      m_zmpIn.read();
-      input_zmp(0) = m_zmp.data.x;
-      input_zmp(1) = m_zmp.data.y;
-      input_zmp(2) = m_zmp.data.z;
-    }
-    for (unsigned int i=0; i<m_ref_forceIn.size(); i++){
-        if ( m_ref_forceIn[i]->isNew() ) {
-            m_ref_forceIn[i]->read();
-        }
-    }
-    if (m_optionalDataIn.isNew()) {
-        m_optionalDataIn.read();
-        if (is_legged_robot) {
-          if (m_optionalData.data.length() >= contact_states_index_map.size()*2) {
-            // current optionalData is contactstates x limb and controlSwingSupportTime x limb
-            //   If contactStates in optionalData is 1.0, m_contactStates is true. Otherwise, false.
-            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-                m_contactStates.data[contact_states_index_map[it->first]] = isOptionalDataContact(it->first);
-                m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = m_optionalData.data[contact_states_index_map[it->first]+contact_states_index_map.size()];
-            }
-            if ( !m_contactStates.data[contact_states_index_map["rleg"]] && !m_contactStates.data[contact_states_index_map["lleg"]] ) { // If two feet have no contact, force set double support contact
-              m_contactStates.data[contact_states_index_map["rleg"]] = true;
-              m_contactStates.data[contact_states_index_map["lleg"]] = true;
-            }
-          }
-        }
-    }
-    if (m_emergencySignalIn.isNew()){
-        m_emergencySignalIn.read();
-        // if (!is_stop_mode) {
-        //     std::cerr << "[" << m_profile.instance_name << "] emergencySignal is set!" << std::endl;
-        //     is_stop_mode = true;
-        //     gg->emergency_stop();
-        // }
-    }
-    //for HumanSynchronizer
-    if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
-    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); hp_raw_data.rfw = HumanSynchronizer::DoubleSeqToWrench6(m_htrfw); }
-    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); hp_raw_data.lfw = HumanSynchronizer::DoubleSeqToWrench6(m_htlfw); }
-    if (m_htcomIn.isNew()){ m_htcomIn.read(); hp_raw_data.com = HumanSynchronizer::Point3DToVector3(m_htcom); }
-    if (m_htrfIn.isNew()) { m_htrfIn.read();  hp_raw_data.rf  = HumanSynchronizer::Point3DToVector3(m_htrf); }
-    if (m_htlfIn.isNew()) { m_htlfIn.read();  hp_raw_data.lf  = HumanSynchronizer::Point3DToVector3(m_htlf); }
-    if (m_htrhIn.isNew()) { m_htrhIn.read();  hp_raw_data.rh  = HumanSynchronizer::Point3DToVector3(m_htrh); }
-    if (m_htlhIn.isNew()) { m_htlhIn.read();  hp_raw_data.lh  = HumanSynchronizer::Point3DToVector3(m_htlh); }
-    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
-    hsp->readInput(hp_raw_data);
-
-
-    if (m_legMarginIn.isNew()) {
-      m_legMarginIn.read();
-      for (size_t i = 0; i < 4; i++) {
-        gg->set_leg_margin(m_legMargin.data[i], i);
-      }
-    }
-
-    Guard guard(m_mutex);
-    hrp::Vector3 ref_basePos;
-    hrp::Matrix33 ref_baseRot;
-    hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
-    if ( is_legged_robot ) {
-      gg->proc_zmp_weight_map_interpolation();
-      getCurrentParameters();
-      getTargetParameters();
-      bool is_transition_interpolator_empty = transition_interpolator->isEmpty();
-      if (!is_transition_interpolator_empty) {
-        transition_interpolator->get(&transition_interpolator_ratio, true);
-      } else {
-        transition_interpolator_ratio = 1.0;
-      }
-      if (control_mode != MODE_IDLE ) {
-        solveLimbIK();
-        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
-      } else {
-        rel_ref_zmp = input_zmp;
-      }
-      // transition
-      if (!is_transition_interpolator_empty) {
-        // transition_interpolator_ratio 0=>1 : IDLE => ABC
-        // transition_interpolator_ratio 1=>0 : ABC => IDLE
-        ref_basePos = (1-transition_interpolator_ratio) * input_basePos + transition_interpolator_ratio * m_robot->rootLink()->p;
-        rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp;
-        rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R);
-        for ( int i = 0; i < m_robot->numJoints(); i++ ) {
-          m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q;
-        }
-      } else {
-        ref_basePos = m_robot->rootLink()->p;
-        ref_baseRot = m_robot->rootLink()->R;
-      }
-      // mode change for sync
-      if (control_mode == MODE_SYNC_TO_ABC) {
-        control_mode = MODE_ABC;
-      } else if (control_mode == MODE_SYNC_TO_IDLE && transition_interpolator->isEmpty() ) {
-        std::cerr << "[" << m_profile.instance_name << "] Finished cleanup" << std::endl;
-        control_mode = MODE_IDLE;
-      }
-    }
-    if ( m_qRef.data.length() != 0 ) { // initialized
-      if (is_legged_robot) {
-        for ( int i = 0; i < m_robot->numJoints(); i++ ){
-          m_qRef.data[i] = m_robot->joint(i)->q;
-        }
-      }
-      m_qOut.write();
-    }
-    if (is_legged_robot) {
-      // basePos
-      m_basePos.data.x = ref_basePos(0);
-      m_basePos.data.y = ref_basePos(1);
-      m_basePos.data.z = ref_basePos(2);
-      m_basePos.tm = m_qRef.tm;
-      // baseRpy
-      hrp::Vector3 baseRpy = hrp::rpyFromRot(ref_baseRot);
-      m_baseRpy.data.r = baseRpy(0);
-      m_baseRpy.data.p = baseRpy(1);
-      m_baseRpy.data.y = baseRpy(2);
-      m_baseRpy.tm = m_qRef.tm;
-      // baseTform
-      double *tform_arr = m_baseTform.data.get_buffer();
-      tform_arr[0] = m_basePos.data.x;
-      tform_arr[1] = m_basePos.data.y;
-      tform_arr[2] = m_basePos.data.z;
-      hrp::setMatrix33ToRowMajorArray(ref_baseRot, tform_arr, 3);
-      m_baseTform.tm = m_qRef.tm;
-      // basePose
-      m_basePose.data.position.x = m_basePos.data.x;
-      m_basePose.data.position.y = m_basePos.data.y;
-      m_basePose.data.position.z = m_basePos.data.z;
-      m_basePose.data.orientation.r = m_baseRpy.data.r;
-      m_basePose.data.orientation.p = m_baseRpy.data.p;
-      m_basePose.data.orientation.y = m_baseRpy.data.y;
-      m_basePose.tm = m_qRef.tm;
-      // zmp
-      m_zmp.data.x = rel_ref_zmp(0);
-      m_zmp.data.y = rel_ref_zmp(1);
-      m_zmp.data.z = rel_ref_zmp(2);
-      m_zmp.tm = m_qRef.tm;
-      // cog
-      m_cog.data.x = ref_cog(0);
-      m_cog.data.y = ref_cog(1);
-      m_cog.data.z = ref_cog(2);
-      m_cog.tm = m_qRef.tm;
-      // sbpCogOffset
-      m_sbpCogOffset.data.x = sbp_cog_offset(0);
-      m_sbpCogOffset.data.y = sbp_cog_offset(1);
-      m_sbpCogOffset.data.z = sbp_cog_offset(2);
-      m_sbpCogOffset.tm = m_qRef.tm;
-    }
-    m_basePosOut.write();
-    m_baseRpyOut.write();
-    m_baseTformOut.write();
-    m_basePoseOut.write();
-    m_zmpOut.write();
-    m_cogOut.write();
-    m_sbpCogOffsetOut.write();
-
-
-//printf("m_zmp:%f, abs:%f\n",m_zmp.data.y,m_basePos.data.y+m_zmp.data.y);
-
-    // reference acceleration
-    hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
-    if (sen != NULL) {
-      hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos;
-      hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt;
-      // convert to imu sensor local acceleration
-      hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt;
-      m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2);
-      m_accRefOut.write();
-      prev_imu_sensor_pos = imu_sensor_pos;
-      prev_imu_sensor_vel = imu_sensor_vel;
-    }
-
-    // control parameters
-    m_contactStates.tm = m_qRef.tm;
-    m_contactStatesOut.write();
-    m_controlSwingSupportTime.tm = m_qRef.tm;
-    m_controlSwingSupportTimeOut.write();
-    m_walkingStates.data = gg_is_walking;
-    m_walkingStates.tm = m_qRef.tm;
-    m_walkingStatesOut.write();
-
-    for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
-        m_force[i].tm = m_qRef.tm;
-        m_ref_forceOut[i]->write();
-    }
-
-    for (unsigned int i=0; i<m_limbCOPOffsetOut.size(); i++){
-        m_limbCOPOffset[i].tm = m_qRef.tm;
-        m_limbCOPOffsetOut[i]->write();
-    }
-
-    return RTC::RTC_OK;
-}
-
-void AutoBalancer::getCurrentParameters()
-{
-  current_root_p = m_robot->rootLink()->p;
-  current_root_R = m_robot->rootLink()->R;
-  for ( int i = 0; i < m_robot->numJoints(); i++ ){
-    qorg[i] = m_robot->joint(i)->q;
-  }
-}
-
-void AutoBalancer::getTargetParameters()
-{
-  // joint angles
-  for ( int i = 0; i < m_robot->numJoints(); i++ ){
-    m_robot->joint(i)->q = m_qRef.data[i];
-    qrefv[i] = m_robot->joint(i)->q;
-  }
-  // basepos, rot, zmp
-  m_robot->rootLink()->p = input_basePos;
-  m_robot->rootLink()->R = input_baseRot;
-  m_robot->calcForwardKinematics();
-  //
-  if (control_mode != MODE_IDLE) {
-    coordinates tmp_fix_coords;
-    if (!zmp_offset_interpolator->isEmpty()) {
-      double *default_zmp_offsets_output = new double[ikp.size()*3];
-      zmp_offset_interpolator->get(default_zmp_offsets_output, true);
-      for (size_t i = 0; i < ikp.size(); i++)
-        for (size_t j = 0; j < 3; j++)
-          default_zmp_offsets[i](j) = default_zmp_offsets_output[i*3+j];
-      delete[] default_zmp_offsets_output;
-      if (DEBUGP) {
-        std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl;
-        std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-        for (size_t i = 0; i < leg_names.size(); i++) {
-            std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
-            std::cerr << "[" << m_profile.instance_name << "]   " << leg_names[i] << " = " << default_zmp_offsets[dst->first].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-        }
-      }
-    }
-    if (!leg_names_interpolator->isEmpty()) {
-        leg_names_interpolator->get(&leg_names_interpolator_ratio, true);
-    }else {
-        leg_names_interpolator_ratio = 1.0;
-    }
-    if ( gg_is_walking ) {
-      gg->set_default_zmp_offsets(default_zmp_offsets);
-      gg_solved = gg->proc_one_tick();
-      {
-          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-          coordinates tmpc;
-          // for support leg
-          for (std::vector<step_node>::const_iterator it = gg->get_support_leg_steps().begin(); it != gg->get_support_leg_steps().end(); it++) {
-              coordinates sp_coords = it->worldcoords;
-              coordinates(ikp[leg_type_map[it->l_r]].localPos,
-                          ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc);
-              sp_coords.transform(tmpc);
-              ikp[leg_type_map[it->l_r]].target_p0 = sp_coords.pos;
-              ikp[leg_type_map[it->l_r]].target_r0 = sp_coords.rot;
-          }
-          // for swing leg
-          for (std::vector<step_node>::const_iterator it = gg->get_swing_leg_steps().begin(); it != gg->get_swing_leg_steps().end(); it++) {
-              coordinates sw_coords = it->worldcoords;
-              coordinates(ikp[leg_type_map[it->l_r]].localPos,
-                          ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc);
-              sw_coords.transform(tmpc);
-              ikp[leg_type_map[it->l_r]].target_p0 = sw_coords.pos;
-              ikp[leg_type_map[it->l_r]].target_r0 = sw_coords.rot;
-          }
-      }
-      gg->get_swing_support_mid_coords(tmp_fix_coords);
-      // set contactStates
-      {
-          std::vector<std::string> tmp_current_support_states_names;
-          {
-              std::vector<leg_type> tmp_current_support_states = gg->get_current_support_states();
-              std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-              for (std::vector<leg_type>::const_iterator it = tmp_current_support_states.begin(); it != tmp_current_support_states.end(); it++)
-                  tmp_current_support_states_names.push_back(leg_type_map[*it]);
-          }
-          // Set Contact States for ee not included in leg_names to false
-          for ( std::map<std::string, size_t>::iterator it = contact_states_index_map.begin(); it != contact_states_index_map.end(); it++ ) {
-              m_contactStates.data[it->second] = false;
-          }
-          for (std::vector<std::string>::const_iterator it = leg_names.begin(); it != leg_names.end(); it++) {
-              std::vector<std::string>::const_iterator dst = std::find_if(tmp_current_support_states_names.begin(), tmp_current_support_states_names.end(), boost::lambda::_1 == *it);
-              if (dst != tmp_current_support_states_names.end()) {
-                  m_contactStates.data[contact_states_index_map[*it]] = true;
-              } else {
-                  m_contactStates.data[contact_states_index_map[*it]] = false;
-              }
-          }
-      }
-      // set controlSwingSupportTime
-      {
-          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-          for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
-              std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
-              m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = gg->get_current_swing_time(dst->first);
-          }
-      }
-      // set ref_forces
-      {
-          std::vector<hrp::Vector3> ee_pos;
-          for (size_t i = 0 ; i < leg_names.size(); i++) {
-              ABCIKparam& tmpikp = ikp[leg_names[i]];
-              ee_pos.push_back(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]);
-          }
-          double alpha = (ref_zmp - ee_pos[1]).norm() / (ee_pos[0] - ee_pos[1]).norm();
-          if (alpha>1.0) alpha = 1.0;
-          if (alpha<0.0) alpha = 0.0;
-          if (DEBUGP) {
-          std::cerr << "[" << m_profile.instance_name << "] alpha:" << alpha << std::endl;
-          }
-          double mg = m_robot->totalMass() * gg->get_gravitational_acceleration();
-          m_force[0].data[0] = alpha * mg;
-          m_force[1].data[0] = (1-alpha) * mg;
-      }
-      // set limbCOPOffset
-      {
-          std::vector<std::string> swg_leg_nms = gg->get_swing_leg_names();
-          for (size_t i = 0; i < swg_leg_nms.size(); i++) {
-              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.x = gg->get_swing_foot_zmp_offsets().at(i)(0);
-              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.y = gg->get_swing_foot_zmp_offsets().at(i)(1);
-              m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.z = gg->get_swing_foot_zmp_offsets().at(i)(2);
-          }
-      }
-      {
-          std::vector<std::string> sup_leg_nms = gg->get_support_leg_names();
-          for (size_t i = 0; i < sup_leg_nms.size(); i++) {
-              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.x = gg->get_support_foot_zmp_offsets().at(i)(0);
-              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.y = gg->get_support_foot_zmp_offsets().at(i)(1);
-              m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.z = gg->get_support_foot_zmp_offsets().at(i)(2);
-          }
-      }
-    } else {
-      tmp_fix_coords = fix_leg_coords;
-      // double support by default
-      {
-          std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-          for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
-              std::vector<std::string>::const_iterator dst = std::find_if(leg_names.begin(), leg_names.end(), (boost::lambda::_1 == it->first));
-              if (dst != leg_names.end()) {
-                  m_contactStates.data[contact_states_index_map[it->first]] = true;
-              } else {
-                  m_contactStates.data[contact_states_index_map[it->first]] = false;
-              }
-              // controlSwingSupportTime is not used while double support period, 1.0 is neglected
-              m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = 1.0;
-              std::map<leg_type, std::string>::const_iterator dst2 = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
-              m_limbCOPOffset[contact_states_index_map[it->first]].data.x = default_zmp_offsets.at(dst2->first)(0);
-              m_limbCOPOffset[contact_states_index_map[it->first]].data.y = default_zmp_offsets.at(dst2->first)(1);
-              m_limbCOPOffset[contact_states_index_map[it->first]].data.z = default_zmp_offsets.at(dst2->first)(2);
-          }
-      }
-    }
-    if (!adjust_footstep_interpolator->isEmpty()) {
-        double tmp = 0.0;
-        adjust_footstep_interpolator->get(&tmp, true);
-        //std::cerr << "[" << m_profile.instance_name << "] adjust ratio " << tmp << std::endl;
-        ikp["rleg"].target_p0 = (1-tmp) * ikp["rleg"].adjust_interpolation_org_p0 + tmp*ikp["rleg"].adjust_interpolation_target_p0;
-        ikp["lleg"].target_p0 = (1-tmp) * ikp["lleg"].adjust_interpolation_org_p0 + tmp*ikp["lleg"].adjust_interpolation_target_p0;
-        rats::mid_rot(ikp["rleg"].target_r0, tmp, ikp["rleg"].adjust_interpolation_org_r0, ikp["rleg"].adjust_interpolation_target_r0);
-        rats::mid_rot(ikp["lleg"].target_r0, tmp, ikp["lleg"].adjust_interpolation_org_r0, ikp["lleg"].adjust_interpolation_target_r0);
-        coordinates tmprc, tmplc;
-        tmprc.pos = ikp["rleg"].target_p0 + ikp["rleg"].target_r0 * ikp["rleg"].localPos;
-        tmprc.rot = ikp["rleg"].target_r0 * ikp["rleg"].localR;
-        tmplc.pos = ikp["lleg"].target_p0 + ikp["lleg"].target_r0 * ikp["lleg"].localPos;
-        tmplc.rot = ikp["lleg"].target_r0 * ikp["lleg"].localR;
-        rats::mid_coords(fix_leg_coords, 0.5, tmprc, tmplc);
-        tmp_fix_coords = fix_leg_coords;
-    }
-    // Tempolarily modify tmp_fix_coords
-    // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272
-    {
-      hrp::Vector3 ex = hrp::Vector3::UnitX();
-      hrp::Vector3 ez = hrp::Vector3::UnitZ();
-      hrp::Vector3 xv1 (tmp_fix_coords.rot * ex);
-      xv1(2) = 0.0;
-      xv1.normalize();
-      hrp::Vector3 yv1(ez.cross(xv1));
-      tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2);
-      tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2);
-      tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2);
-    }
-    // Fix pos
-    fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot);
-
-    /* update ref_forces ;; sp's absolute -> rmc's absolute */
-    for (size_t i = 0; i < m_ref_forceIn.size(); i++) {
-      hrp::Matrix33 eeR;
-      hrp::Link* parentlink;
-      hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
-      if (sensor) parentlink = sensor->link;
-      else parentlink = m_vfs[sensor_names[i]].link;
-      for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-          if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR;
-      }
-      // End effector frame
-      //ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
-      // world frame
-      ref_forces[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
-    }
-    sbp_offset = tmp_fix_coords.rot * hrp::Vector3(sbp_offset);
-
-    target_root_p = m_robot->rootLink()->p;
-    target_root_R = m_robot->rootLink()->R;
-    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-      if ( control_mode == MODE_IDLE || std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end() ) {
-        it->second.target_p0 = it->second.target_link->p;
-        it->second.target_r0 = it->second.target_link->R;
-      }
-    }
-    // Just for ik initial value
-    if (control_mode == MODE_SYNC_TO_ABC) {
-        current_root_p = target_root_p;
-        current_root_R = target_root_R;
-        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
-                it->second.target_p0 = it->second.target_link->p;
-                it->second.target_r0 = it->second.target_link->R;
-            }
-        }
-    }
-    // Move hand for hand fix mode
-    //   If arms' ABCIKparam.is_active is true, move hands according to cog velocity.
-    //   If is_hand_fix_mode is false, no hand fix mode and move hands according to cog velocity.
-    //   If is_hand_fix_mode is true, hand fix mode and do not move hands in Y axis in tmp_fix_coords.rot.    
-    if (gg_is_walking) {
-        // hand control while walking = solve hand ik with is_hand_fix_mode and solve hand ik without is_hand_fix_mode
-        bool is_hand_control_while_walking = false;
-        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
-                 && it->first.find("arm") != std::string::npos ) {
-                is_hand_control_while_walking = true;
-            }
-        }
-        if (is_hand_control_while_walking) {
-        //if (false) { // Disabled temporarily
-            // Store hand_fix_initial_offset in the initialization of walking
-            if (is_hand_fix_initial) hand_fix_initial_offset = tmp_fix_coords.rot.transpose() * (hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos);
-            is_hand_fix_initial = false;
-            hrp::Vector3 dif_p = hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos - tmp_fix_coords.rot * hand_fix_initial_offset;
-            if (is_hand_fix_mode) {
-                dif_p = tmp_fix_coords.rot.transpose() * dif_p;
-                dif_p(1) = 0;
-                dif_p = tmp_fix_coords.rot * dif_p;
-            }
-            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-                if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
-                     && it->first.find("arm") != std::string::npos ) {
-                    it->second.target_p0 = it->second.target_p0 + dif_p;
-                }
-            }
-        }
-    }
-
-    hrp::Vector3 tmp_foot_mid_pos(hrp::Vector3::Zero());
-    {
-        std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-        std::map<leg_type, double> zmp_weight_map = gg->get_zmp_weight_map();
-        double sum_of_weight = 0.0;
-        for (size_t i = 0; i < leg_names.size(); i++) {
-            ABCIKparam& tmpikp = ikp[leg_names[i]];
-            // get target_end_coords
-            tmpikp.target_end_coords.pos = tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos;
-            tmpikp.target_end_coords.rot = tmpikp.target_r0 * tmpikp.localR;
-            // for foot_mid_pos
-            std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
-            tmp_foot_mid_pos += (tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]) * zmp_weight_map[dst->first];
-            sum_of_weight += zmp_weight_map[dst->first];
-        }
-        tmp_foot_mid_pos *= (1.0 / sum_of_weight);
-    }
-    //
-    {
-        if ( gg_is_walking && gg->get_lcg_count() == gg->get_overwrite_check_timing()+2 ) {
-            hrp::Vector3 vel_htc(calc_vel_from_hand_error(tmp_fix_coords));
-            gg->set_offset_velocity_param(vel_htc(0), vel_htc(1) ,vel_htc(2));
-        }//  else {
-        //     if ( gg_is_walking && gg->get_lcg_count() == static_cast<size_t>(gg->get_default_step_time()/(2*m_dt))-1) {
-        //         gg->set_offset_velocity_param(0,0,0);
-        //     }
-        // }
-    }
-
-
-    hrp::Vector3 tmp_ref_cog(m_robot->calcCM());
-    if (gg_is_walking) {
-      ref_cog = gg->get_cog();
-    } else {
-      ref_cog = tmp_foot_mid_pos;
-    }
-    ref_cog(2) = tmp_ref_cog(2);
-    if (gg_is_walking) {
-      ref_zmp = gg->get_refzmp();
-    } else {
-      ref_zmp(0) = ref_cog(0);
-      ref_zmp(1) = ref_cog(1);
-      ref_zmp(2) = tmp_foot_mid_pos(2);
-    }
-  }
-};
-
-hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2)
-{
-  hrp::Vector3 vv = axis1.cross(axis2);
-  if (fabs(vv.norm()-0.0) < 1e-5) {
-    return rot;
-  } else {
-    Eigen::AngleAxis<double> tmpr(std::asin(vv.norm()), vv.normalized());
-    return tmpr.toRotationMatrix() * rot;
-  }
-}
-
-void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot)
-{
-  // get current foot mid pos + rot
-  std::vector<coordinates> foot_coords;
-  for (size_t i = 0; i < leg_names.size(); i++) {
-      ABCIKparam& tmpikp = ikp[leg_names[i]];
-      if (leg_names[i].find("leg") != std::string::npos) foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
-                                                                                           (tmpikp.target_link->R * tmpikp.localR)));
-  }
-  coordinates current_foot_mid_coords;
-  multi_mid_coords(current_foot_mid_coords, foot_coords);
-  hrp::Vector3 current_foot_mid_pos = current_foot_mid_coords.pos;
-  hrp::Matrix33 current_foot_mid_rot = current_foot_mid_coords.rot;
-  // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
-  hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose());
-  m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
-  rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
-  m_robot->calcForwardKinematics();
-}
-
-bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param, const std::string& limb_name)
-{
-  param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
-  // IK check
-  hrp::Vector3 vel_p, vel_r;
-  vel_p = param.target_p0 - param.target_link->p;
-  rats::difference_rotation(vel_r, param.target_link->R, param.target_r0);
-  if (vel_p.norm() > pos_ik_thre && transition_interpolator->isEmpty()) {
-      if (param.pos_ik_error_count % ik_error_debug_print_freq == 0) {
-          std::cerr << "[" << m_profile.instance_name << "] Too large IK error in " << limb_name << " (vel_p) = [" << vel_p(0) << " " << vel_p(1) << " " << vel_p(2) << "][m], count = " << param.pos_ik_error_count << std::endl;
-      }
-      param.pos_ik_error_count++;
-      has_ik_failed = true;
-  } else {
-      param.pos_ik_error_count = 0;
-  }
-  if (vel_r.norm() > rot_ik_thre && transition_interpolator->isEmpty()) {
-      if (param.rot_ik_error_count % ik_error_debug_print_freq == 0) {
-          std::cerr << "[" << m_profile.instance_name << "] Too large IK error in " << limb_name << " (vel_r) = [" << vel_r(0) << " " << vel_r(1) << " " << vel_r(2) << "][rad], count = " << param.rot_ik_error_count << std::endl;
-      }
-      param.rot_ik_error_count++;
-      has_ik_failed = true;
-  } else {
-      param.rot_ik_error_count = 0;
-  }
-  return true;
-}
-
-
-
-void AutoBalancer::solveLimbIK ()
-{
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-    if (it->second.is_active) {
-      for ( int j = 0; j < it->second.manip->numJoints(); j++ ){
-	int i = it->second.manip->joint(j)->jointId;
-	m_robot->joint(i)->q = qorg[i];
-      }
-    }
-  }
-  m_robot->rootLink()->p = current_root_p;
-  m_robot->rootLink()->R = current_root_R;
-  m_robot->calcForwardKinematics();
-  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
-  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
-  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
-  dif_cog *= leg_names_interpolator_ratio;
-  dif_cog(2) = m_robot->rootLink()->p(2) - target_root_p(2);
-  m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog;
-  m_robot->rootLink()->R = target_root_R;
-
-
-  //for HumanSynchronizer
-//  static int HumanSyncCountdownNum = 5 * (1/m_dt);
-  if(hsp->startCountdownForHumanSync){
-    std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";//ややCOMのIKに手間取った時プリント
-	  hsp->updateCountDown();
-  }
-  if(hsp->isHumanSyncOn()){
-	  if(hsp->ht_first_call){
-		  hsp->init_wld_rp_basepos = m_robot->rootLink()->p;
-      hsp->setInitOffsetPose();
-      hsp->rp_wld_initpos.com = m_robot->calcCM();
-      hsp->rp_wld_initpos.rf = ikp["rleg"].target_link->p;
-      hsp->rp_wld_initpos.lf = ikp["lleg"].target_link->p;
-      if(ikp.count("rarm"))hsp->rp_wld_initpos.rh = ikp["rarm"].target_link->p;
-      if(ikp.count("larm"))hsp->rp_wld_initpos.lh = ikp["larm"].target_link->p;
-      hsp->ht_first_call = false;
-
-      hsp->calibInitHumanCOMFromZMP();
-
-      std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
-
-	  }
-  }else{
-    hsp->setInitOffsetPose();
-  }
-
-  hsp->update();//////HumanSynchronizerの主要処理
-  if(loop%100==0)hsp->rp_ref_out.print();
-  if(loop%100==0)cerr<<"getUpdateTime:"<<hsp->getUpdateTime()*1000<<"[ms]"<<endl;
-
-  if(hsp->isHumanSyncOn()){
-	  ////////////////////// 重心拘束位置設定 /////////////////////////
-    m_robot->rootLink()->p = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;//まずは重心目標分，体幹を動かす
-    m_robot->rootLink()->p(2) = hsp->init_wld_rp_basepos(2)+ hsp->rp_ref_out.com(2);//高さだけはベース基準
-    m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
-    m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
-  }
-
-  // Fix for toe joint
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-      if (it->second.is_active && it->second.has_toe_joint && gg->get_use_toe_joint()) {
-          int i = it->second.target_link->jointId;
-          if (gg->get_swing_leg_names().front() == it->first) {
-              m_robot->joint(i)->q = qrefv[i] + -1 * gg->get_foot_dif_rot_angle();
-          } else {
-              m_robot->joint(i)->q = qrefv[i];
-          }
-      }
-  }
-  m_robot->calcForwardKinematics();
-
-
-  // additional COM fitting IK for HumanSynchronizer
-  hrp::Vector3 tmp_com_err = hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM();
-  int com_ik_loop=0;
-  const int COM_IK_MAX_LOOP = 10;
-  const double COM_IK_MAX_ERROR = 0.0001;//0.001だとたまにカクッとなる
-  if(hsp->isHumanSyncOn()){
-    ikp["rleg"].target_p0 = hsp->rp_wld_initpos.rf + hsp->rp_ref_out.rf;
-    ikp["lleg"].target_p0 = hsp->rp_wld_initpos.lf + hsp->rp_ref_out.lf;
-    if(ikp.count("rarm"))ikp["rarm"].target_p0 = hsp->rp_wld_initpos.rh + hsp->rp_ref_out.rh;
-    if(ikp.count("larm"))ikp["larm"].target_p0 = hsp->rp_wld_initpos.lh + hsp->rp_ref_out.lh;
-    while(fabs(tmp_com_err(0))>COM_IK_MAX_ERROR || fabs(tmp_com_err(1))>COM_IK_MAX_ERROR || fabs(tmp_com_err(2))>COM_IK_MAX_ERROR){//Z方向のCOM合わせは要注意(たまに怪しい)
-		  m_robot->rootLink()->p(0) += tmp_com_err(0);
-      m_robot->rootLink()->p(1) += tmp_com_err(1);
-      m_robot->rootLink()->p(2) += tmp_com_err(2);
-		  m_robot->calcForwardKinematics();
-		  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-		    if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
-		  }
-		  m_robot->calcForwardKinematics();
-      tmp_com_err = (hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM());
-		  if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "[" << m_profile.instance_name << "] COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
-	  }
-
-//    static hrp::Vector3 pre_ik_basepos;
-//    for(int i=0;i<3;i++){//重心拘束で解いた結果の体幹軌道の滑らかさは保証されていないので体幹位置に速度制限＆再度IK
-//      if(m_robot->rootLink()->p(i) - pre_ik_basepos(i) > 0.0002){ m_robot->rootLink()->p(i) =  pre_ik_basepos(i) + 0.0002; }
-//      else if(m_robot->rootLink()->p(i) - pre_ik_basepos(i) < -0.0002){ m_robot->rootLink()->p(i) =  pre_ik_basepos(i) - 0.0002; }
-//    }
-//    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-//      if (it->second.is_active) solveLimbIKforLimb(it->second);
-//    }
-//    pre_ik_basepos = m_robot->rootLink()->p;
-
-    //outport用のデータ上書き
-    ref_zmp(0) = hsp->rp_ref_out.zmp(0);
-    ref_zmp(1) = hsp->rp_ref_out.zmp(1);
-    ref_zmp(2) = 0;
-    ref_cog = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;
-
-	  if(loop%100==0)if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
-
-
-//	  printf("root:%f, calcCM:%f, refCM:%f\n",m_robot->rootLink()->p(1),m_robot->calcCM()(1),hsp->rp_ref_out.com(1) + hsp->rp_wld_initpos.com(1));
-
-
-  }else{
-	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
-	    if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
-	  }
-  }
-  if (gg_is_walking && !gg_solved) stopWalking ();
-}
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onAborting(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onError(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onReset(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onStateUpdate(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-/*
-  RTC::ReturnCode_t AutoBalancer::onRateChanged(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
-  }
-*/
-
-bool AutoBalancer::startHumanSyncAfter5sec()
-{
-  std::cerr << "[" << m_profile.instance_name << "] start HumanSync after 5 sec" << std::endl;
-  hsp->startCountdownForHumanSync = true;
-  return true;
-}
-
-bool AutoBalancer::setHumanToRobotRatio(const double h2r)
-{
-  std::cerr << "[" << m_profile.instance_name << "] set h2r_ratio as "<< h2r << std::endl;
-  if(!hsp->isHumanSyncOn())hsp->h2r_ratio = h2r;
-  return true;
-}
-
-bool AutoBalancer::setAllowedXYZSync(const bool x_on,const bool y_on,const bool z_on)
-{
-  std::cerr << "[" << m_profile.instance_name << "] set allowed XYZ move direction as ("<<x_on<<","<<y_on<<","<<z_on<<")" << std::endl;
-  hsp->use_x = x_on;
-  hsp->use_y = y_on;
-  hsp->use_z = z_on;
-  return true;
-}
-
-bool AutoBalancer::stopHumanSync()
-{
-	std::cerr << "[" << m_profile.instance_name << "] stop HumanSync Now" << std::endl;
-	return true;
-}
-
-void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence& limbs)
-{
-  std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
-  Guard guard(m_mutex);
-  double tmp_ratio = 0.0;
-  transition_interpolator->clear();
-  transition_interpolator->set(&tmp_ratio);
-  tmp_ratio = 1.0;
-  transition_interpolator->go(&tmp_ratio, transition_time, true);
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-    it->second.is_active = false;
-  }
-
-  for (size_t i = 0; i < limbs.length(); i++) {
-    ABCIKparam& tmp = ikp[std::string(limbs[i])];
-    tmp.is_active = true;
-    std::cerr << "[" << m_profile.instance_name << "]   limb [" << std::string(limbs[i]) << "]" << std::endl;
-  }
-
-  control_mode = MODE_SYNC_TO_ABC;
-}
-
-void AutoBalancer::stopABCparam()
-{
-  std::cerr << "[" << m_profile.instance_name << "] stop auto balancer mode" << std::endl;
-  //Guard guard(m_mutex);
-  double tmp_ratio = 1.0;
-  transition_interpolator->clear();
-  transition_interpolator->set(&tmp_ratio);
-  tmp_ratio = 0.0;
-  transition_interpolator->go(&tmp_ratio, transition_time, true);
-  control_mode = MODE_SYNC_TO_IDLE;
-}
-
-void AutoBalancer::startWalking ()
-{
-  if ( control_mode != MODE_ABC ) {
-    return_control_mode = control_mode;
-    OpenHRP::AutoBalancerService::StrSequence fix_limbs;
-    fix_limbs.length(2);
-    fix_limbs[0] = "rleg";
-    fix_limbs[1] = "lleg";
-    startABCparam(fix_limbs);
-    waitABCTransition();
-  }
-  {
-    Guard guard(m_mutex);
-    has_ik_failed = false;
-    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-        it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
-    }
-    std::vector<std::string> init_swing_leg_names(gg->get_footstep_front_leg_names());
-    std::vector<std::string> tmp_all_limbs(leg_names);
-    std::vector<std::string> init_support_leg_names;
-    std::sort(tmp_all_limbs.begin(), tmp_all_limbs.end());
-    std::sort(init_swing_leg_names.begin(), init_swing_leg_names.end());
-    std::set_difference(tmp_all_limbs.begin(), tmp_all_limbs.end(),
-                        init_swing_leg_names.begin(), init_swing_leg_names.end(),
-                        std::back_inserter(init_support_leg_names));
-    std::vector<step_node> init_support_leg_steps, init_swing_leg_dst_steps;
-    for (std::vector<std::string>::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++)
-        init_support_leg_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0));
-    for (std::vector<std::string>::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++)
-        init_swing_leg_dst_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0));
-    gg->set_default_zmp_offsets(default_zmp_offsets);
-    gg->initialize_gait_parameter(ref_cog, init_support_leg_steps, init_swing_leg_dst_steps);
-  }
-  is_hand_fix_initial = true;
-  while ( !gg->proc_one_tick() );
-  {
-    Guard guard(m_mutex);
-    gg_is_walking = gg_solved = true;
-  }
-}
-
-void AutoBalancer::stopWalking ()
-{
-  std::vector<coordinates> tmp_end_coords_list;
-  for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
-      if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(ikp[*it].target_end_coords);
-  }
-  multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
-  fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot);
-  gg->clear_footstep_nodes_list();
-  if (return_control_mode == MODE_IDLE) stopABCparam();
-  gg_is_walking = false;
-}
-
-bool AutoBalancer::startAutoBalancer (const OpenHRP::AutoBalancerService::StrSequence& limbs)
-{
-  if (control_mode == MODE_IDLE) {
-    has_ik_failed = false;
-    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-        it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
-    }
-    startABCparam(limbs);
-    waitABCTransition();
-    return_control_mode = MODE_ABC;
-    return true;
-  } else {
-    return false;
-  }
-}
-
-bool AutoBalancer::stopAutoBalancer ()
-{
-  if (control_mode == MODE_ABC) {
-    stopABCparam();
-    waitABCTransition();
-    return true;
-  } else {
-    return false;
-  }
-}
-
-void AutoBalancer::waitABCTransition()
-{
-  while (!transition_interpolator->isEmpty()) usleep(1000);
-  usleep(1000);
-}
-bool AutoBalancer::goPos(const double& x, const double& y, const double& th)
-{
-    //  if ( !gg_is_walking && !is_stop_mode) {
-  if ( !is_stop_mode) {
-    gg->set_all_limbs(leg_names);
-    coordinates start_ref_coords;
-    std::vector<coordinates> initial_support_legs_coords;
-    std::vector<leg_type> initial_support_legs;
-    bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
-    if (is_valid_gait_type == false) return false;
-    bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th,
-                                                      initial_support_legs_coords, // Dummy if gg_is_walking
-                                                      start_ref_coords,            // Dummy if gg_is_walking
-                                                      initial_support_legs,        // Dummy if gg_is_walking
-                                                      (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps.
-    if ( !gg_is_walking ) { // Initializing
-        startWalking();
-    }
-    if (!ret) {
-        std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl;
-    }
-    return ret;
-  } else {
-    std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl;
-    return false;
-  }
-}
-
-bool AutoBalancer::goVelocity(const double& vx, const double& vy, const double& vth)
-{
-  gg->set_all_limbs(leg_names);
-  if (gg_is_walking && gg_solved) {
-    gg->set_velocity_param(vx, vy, vth);
-  } else {
-    coordinates ref_coords;
-    mid_coords(ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
-    std::vector<leg_type> current_legs;
-    switch(gait_type) {
-    case BIPED:
-        current_legs = (vy > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
-        break;
-    case TROT:
-        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
-        break;
-    case PACE:
-        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
-        break;
-    case CRAWL:
-        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    case GALLOP:
-        /* at least one leg shoud be in contact */
-        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    default: break;
-    }
-    gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs);
-    startWalking();
-  }
-  return true;
-}
-
-bool AutoBalancer::goStop ()
-{
-  gg->finalize_velocity_mode();
-  waitFootSteps();
-  return true;
-}
-
-bool AutoBalancer::emergencyStop ()
-{
-  std::cerr << "[" << m_profile.instance_name << "] emergencyStop" << std::endl;
-  // is_stop_mode = true;
-  gg->emergency_stop();
-  waitFootSteps();
-  return true;
-}
-
-bool AutoBalancer::releaseEmergencyStop ()
-{
-  if (is_stop_mode) {
-      std::cerr << "[" << m_profile.instance_name << "] releaseEmergencyStop" << std::endl;
-      is_stop_mode = false;
-  }
-  return true;
-}
-
-bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
-{
-  OpenHRP::AutoBalancerService::StepParamsSequence spss;
-  spss.length(fss.length());
-  // If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
-  // If gg_is_walking is true, do not set to 0.
-  for (size_t i = 0; i < spss.length(); i++) {
-      spss[i].sps.length(fss[i].fs.length());
-      for (size_t j = 0; j < spss[i].sps.length(); j++) {
-          spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
-          spss[i].sps[j].step_time = gg->get_default_step_time();
-          spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle());
-          spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle());
-      }
-  }
-  setFootStepsWithParam(fss, spss, overwrite_fs_idx);
-}
-
-bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
-{
-    if (!is_stop_mode) {
-        std::cerr << "[" << m_profile.instance_name << "] setFootStepsList" << std::endl;
-
-        // Initial footstep Snapping
-        coordinates tmpfs, fstrans;
-        step_node initial_support_step, initial_input_step;
-        {
-            std::vector<step_node> initial_support_steps;
-            if (gg_is_walking) {
-                if (overwrite_fs_idx <= 0) {
-                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
-                    return false;
-                }
-                if (!gg->get_footstep_nodes_by_index(initial_support_steps, overwrite_fs_idx-1)) {
-                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
-                    return false;
-                }
-            } else {
-                // If walking, snap initial leg to current ABC foot coords.
-                for (size_t i = 0; i < fss[0].fs.length(); i++) {
-                    initial_support_steps.push_back(step_node(std::string(fss[0].fs[i].leg), ikp[std::string(fss[0].fs[i].leg)].target_end_coords, 0, 0, 0, 0));
-                }
-            }
-            initial_support_step = initial_support_steps.front(); /* use only one leg for representation */
-        }
-        {
-            std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
-            for (size_t i = 0; i < fss[0].fs.length(); i++) {
-                if (std::string(fss[0].fs[i].leg) == leg_type_map[initial_support_step.l_r]) {
-                    coordinates tmp;
-                    memcpy(tmp.pos.data(), fss[0].fs[i].pos, sizeof(double)*3);
-                    tmp.rot = (Eigen::Quaternion<double>(fss[0].fs[i].rot[0], fss[0].fs[i].rot[1], fss[0].fs[i].rot[2], fss[0].fs[i].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
-                    initial_input_step = step_node(std::string(fss[0].fs[i].leg), tmp, 0, 0, 0, 0);
-                }
-            }
-        }
-
-        // Get footsteps
-        std::vector< std::vector<coordinates> > fs_vec_list;
-        std::vector< std::vector<std::string> > leg_name_vec_list;
-        for (size_t i = 0; i < fss.length(); i++) {
-            std::vector<coordinates> fs_vec;
-            std::vector<std::string> leg_name_vec;
-            for (size_t j = 0; j < fss[i].fs.length(); j++) {
-                std::string leg(fss[i].fs[j].leg);
-                if (std::find(leg_names.begin(), leg_names.end(), leg) != leg_names.end()) {
-                    memcpy(tmpfs.pos.data(), fss[i].fs[j].pos, sizeof(double)*3);
-                    tmpfs.rot = (Eigen::Quaternion<double>(fss[i].fs[j].rot[0], fss[i].fs[j].rot[1], fss[i].fs[j].rot[2], fss[i].fs[j].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
-                    initial_input_step.worldcoords.transformation(fstrans, tmpfs);
-                    tmpfs = initial_support_step.worldcoords;
-                    tmpfs.transform(fstrans);
-                } else {
-                    std::cerr << "[" << m_profile.instance_name << "]   No such target : " << leg << std::endl;
-                    return false;
-                }
-                leg_name_vec.push_back(leg);
-                fs_vec.push_back(tmpfs);
-            }
-            leg_name_vec_list.push_back(leg_name_vec);
-            fs_vec_list.push_back(fs_vec);
-        }
-        if (spss.length() != fs_vec_list.size()) {
-            std::cerr << "[" << m_profile.instance_name << "]   StepParam length " << spss.length () << " != Footstep length " << fs_vec_list.size() << std::endl;
-            return false;
-        }
-        std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl;
-        std::vector< std::vector<step_node> > fnsl;
-        for (size_t i = 0; i < fs_vec_list.size(); i++) {
-            if (!(gg_is_walking && i == 0)) { // If initial footstep, e.g., not walking, pass user-defined footstep list. If walking, pass cdr footsteps in order to neglect initial double support leg.
-                std::vector<step_node> tmp_fns;
-                for (size_t j = 0; j < fs_vec_list.at(i).size(); j++) {
-                    tmp_fns.push_back(step_node(leg_name_vec_list[i][j], fs_vec_list[i][j], spss[i].sps[j].step_height, spss[i].sps[j].step_time, spss[i].sps[j].toe_angle, spss[i].sps[j].heel_angle));
-                }
-                fnsl.push_back(tmp_fns);
-            }
-        }
-        if (gg_is_walking) {
-            std::cerr << "[" << m_profile.instance_name << "]  Set overwrite footsteps" << std::endl;
-            gg->set_overwrite_foot_steps_list(fnsl);
-            gg->set_overwrite_foot_step_index(overwrite_fs_idx);
-        } else {
-            std::cerr << "[" << m_profile.instance_name << "]  Set normal footsteps" << std::endl;
-            gg->set_foot_steps_list(fnsl);
-            startWalking();
-        }
-        return true;
-    } else {
-        std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl;
-        return false;
-    }
-}
-
-void AutoBalancer::waitFootSteps()
-{
-  //while (gg_is_walking) usleep(10);
-  while (gg_is_walking || !transition_interpolator->isEmpty() )
-    usleep(1000);
-  usleep(1000);
-  gg->set_offset_velocity_param(0,0,0);
-}
-
-void AutoBalancer::waitFootStepsEarly(const double tm)
-{
-  if (!gg_is_walking) { return;}
-  while ( !gg->is_finalizing(tm)|| !transition_interpolator->isEmpty() )
-    usleep(1000);
-  usleep(1000);
-  gg->set_offset_velocity_param(0,0,0);
-}
-
-bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
-{
-  std::cerr << "[" << m_profile.instance_name << "] setGaitGeneratorParam" << std::endl;
-  gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]);
-  std::vector<hrp::Vector3> off;
-  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
-      off.push_back(hrp::Vector3(i_param.leg_default_translate_pos[i][0], i_param.leg_default_translate_pos[i][1], i_param.leg_default_translate_pos[i][2]));
-  }
-  gg->set_leg_default_translate_pos(off);
-  gg->set_default_step_time(i_param.default_step_time);
-  gg->set_default_step_height(i_param.default_step_height);
-  gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio/2.0);
-  gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio/2.0);
-  gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio/2.0);
-  gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio/2.0);
-  gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio/2.0);
-  gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio/2.0);
-  // gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio_before);
-  // gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio_after);
-  // gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio_before);
-  // gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio_after);
-  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_before);
-  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_after);
-  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_swing_before);
-  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_swing_after);
-  if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::SHUFFLING) {
-    gg->set_default_orbit_type(SHUFFLING);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOID) {
-    gg->set_default_orbit_type(CYCLOID);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::RECTANGLE) {
-    gg->set_default_orbit_type(RECTANGLE);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::STAIR) {
-    gg->set_default_orbit_type(STAIR);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAY) {
-    gg->set_default_orbit_type(CYCLOIDDELAY);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK) {
-    gg->set_default_orbit_type(CYCLOIDDELAYKICK);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CROSS) {
-    gg->set_default_orbit_type(CROSS);
-  }
-  gg->set_swing_trajectory_delay_time_offset(i_param.swing_trajectory_delay_time_offset);
-  gg->set_swing_trajectory_final_distance_weight(i_param.swing_trajectory_final_distance_weight);
-  gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2]));
-  gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2]));  
-  gg->set_gravitational_acceleration(i_param.gravitational_acceleration);
-  gg->set_toe_angle(i_param.toe_angle);
-  gg->set_heel_angle(i_param.heel_angle);
-  gg->set_toe_pos_offset_x(i_param.toe_pos_offset_x);
-  gg->set_heel_pos_offset_x(i_param.heel_pos_offset_x);
-  gg->set_toe_zmp_offset_x(i_param.toe_zmp_offset_x);
-  gg->set_heel_zmp_offset_x(i_param.heel_zmp_offset_x);
-  std::vector<double> tmp_ratio(i_param.toe_heel_phase_ratio.get_buffer(), i_param.toe_heel_phase_ratio.get_buffer()+i_param.toe_heel_phase_ratio.length());
-  std::cerr << "[" << m_profile.instance_name << "]   "; // for set_toe_heel_phase_ratio
-  gg->set_toe_heel_phase_ratio(tmp_ratio);
-  gg->set_use_toe_joint(i_param.use_toe_joint);
-  gg->set_use_toe_heel_transition(i_param.use_toe_heel_transition);
-  gg->set_zmp_weight_map(boost::assign::map_list_of<leg_type, double>(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3]));
-  gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num);
-  gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset);
-  gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation);
-  gg->set_use_stride_limitation(i_param.use_stride_limitation);
-
-  // print
-  gg->print_param(std::string(m_profile.instance_name));
-  return true;
-};
-
-bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
-{
-  gg->get_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]);
-  std::vector<hrp::Vector3> off;
-  gg->get_leg_default_translate_pos(off);
-  i_param.leg_default_translate_pos.length(off.size());
-  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
-      i_param.leg_default_translate_pos[i].length(3);
-      i_param.leg_default_translate_pos[i][0] = off[i](0);
-      i_param.leg_default_translate_pos[i][1] = off[i](1);
-      i_param.leg_default_translate_pos[i][2] = off[i](2);
-  }
-  i_param.default_step_time = gg->get_default_step_time();
-  i_param.default_step_height = gg->get_default_step_height();
-  i_param.default_double_support_ratio_before = gg->get_default_double_support_ratio_before();
-  i_param.default_double_support_ratio_after = gg->get_default_double_support_ratio_after();
-  i_param.default_double_support_static_ratio_before = gg->get_default_double_support_static_ratio_before();
-  i_param.default_double_support_static_ratio_after = gg->get_default_double_support_static_ratio_after();
-  i_param.default_double_support_ratio_swing_before = gg->get_default_double_support_ratio_swing_before();
-  i_param.default_double_support_ratio_swing_after = gg->get_default_double_support_ratio_swing_after();
-  i_param.default_double_support_ratio = i_param.default_double_support_ratio_before + i_param.default_double_support_ratio_after;
-  i_param.default_double_support_static_ratio = i_param.default_double_support_static_ratio_before + i_param.default_double_support_static_ratio_after;
-  if (gg->get_default_orbit_type() == SHUFFLING) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::SHUFFLING;
-  } else if (gg->get_default_orbit_type() == CYCLOID) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOID;
-  } else if (gg->get_default_orbit_type() == RECTANGLE) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::RECTANGLE;
-  } else if (gg->get_default_orbit_type() == STAIR) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::STAIR;
-  } else if (gg->get_default_orbit_type() == CYCLOIDDELAY) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAY;
-  } else if (gg->get_default_orbit_type() == CYCLOIDDELAYKICK) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK;
-  } else if (gg->get_default_orbit_type() == CROSS) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CROSS;
-  }
-
-  hrp::Vector3 tmpv = gg->get_stair_trajectory_way_point_offset();
-  for (size_t i = 0; i < 3; i++) i_param.stair_trajectory_way_point_offset[i] = tmpv(i);
-  tmpv = gg->get_cycloid_delay_kick_point_offset();
-  for (size_t i = 0; i < 3; i++) i_param.cycloid_delay_kick_point_offset[i] = tmpv(i);
-  i_param.swing_trajectory_delay_time_offset = gg->get_swing_trajectory_delay_time_offset();
-  i_param.swing_trajectory_final_distance_weight = gg->get_swing_trajectory_final_distance_weight();
-  i_param.gravitational_acceleration = gg->get_gravitational_acceleration();
-  i_param.toe_angle = gg->get_toe_angle();
-  i_param.heel_angle = gg->get_heel_angle();
-  i_param.toe_pos_offset_x = gg->get_toe_pos_offset_x();
-  i_param.heel_pos_offset_x = gg->get_heel_pos_offset_x();
-  i_param.toe_zmp_offset_x = gg->get_toe_zmp_offset_x();
-  i_param.heel_zmp_offset_x = gg->get_heel_zmp_offset_x();
-  std::vector<double> ratio(gg->get_NUM_TH_PHASES(),0.0);
-  gg->get_toe_heel_phase_ratio(ratio);
-  for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) i_param.toe_heel_phase_ratio[i] = ratio[i];
-  i_param.use_toe_joint = gg->get_use_toe_joint();
-  i_param.use_toe_heel_transition = gg->get_use_toe_heel_transition();
-  std::map<leg_type, double> tmp_zmp_weight_map = gg->get_zmp_weight_map();
-  i_param.zmp_weight_map[0] = tmp_zmp_weight_map[RLEG];
-  i_param.zmp_weight_map[1] = tmp_zmp_weight_map[LLEG];
-  i_param.zmp_weight_map[2] = tmp_zmp_weight_map[RARM];
-  i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM];
-  i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num();
-  i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset();
-  for (size_t i=0; i<4; i++) {
-    i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i);
-  }
-  i_param.use_stride_limitation = gg->get_use_stride_limitation();
-  return true;
-};
-
-bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
-{
-  Guard guard(m_mutex);
-  std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl;
-  double *default_zmp_offsets_array = new double[ikp.size()*3];
-  move_base_gain = i_param.move_base_gain;
-  for (size_t i = 0; i < ikp.size(); i++)
-    for (size_t j = 0; j < 3; j++)
-      default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j];
-  zmp_transition_time = i_param.zmp_transition_time;
-  adjust_footstep_transition_time = i_param.adjust_footstep_transition_time;
-  if (zmp_offset_interpolator->isEmpty()) {
-      zmp_offset_interpolator->clear();
-      zmp_offset_interpolator->go(default_zmp_offsets_array, zmp_transition_time, true);
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets cannot be set because interpolating." << std::endl;
-  }
-  if (control_mode == MODE_IDLE) {
-    switch (i_param.use_force_mode) {
-    case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
-        use_force = MODE_NO_FORCE;
-        break;
-    case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
-        use_force = MODE_REF_FORCE;
-        break;
-    default:
-        break;
-    }
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   use_force_mode cannot be changed to [" << i_param.use_force_mode << "] during MODE_ABC, MODE_SYNC_TO_IDLE or MODE_SYNC_TO_ABC." << std::endl;
-  }
-  graspless_manip_mode = i_param.graspless_manip_mode;
-  graspless_manip_arm = std::string(i_param.graspless_manip_arm);
-  for (size_t j = 0; j < 3; j++)
-      graspless_manip_p_gain[j] = i_param.graspless_manip_p_gain[j];
-  for (size_t j = 0; j < 3; j++)
-      graspless_manip_reference_trans_coords.pos[j] = i_param.graspless_manip_reference_trans_pos[j];
-  graspless_manip_reference_trans_coords.rot = (Eigen::Quaternion<double>(i_param.graspless_manip_reference_trans_rot[0],
-                                                                          i_param.graspless_manip_reference_trans_rot[1],
-                                                                          i_param.graspless_manip_reference_trans_rot[2],
-                                                                          i_param.graspless_manip_reference_trans_rot[3]).normalized().toRotationMatrix()); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
-  transition_time = i_param.transition_time;
-  std::vector<std::string> cur_leg_names, dst_leg_names;
-  cur_leg_names = leg_names;
-  for (size_t i = 0; i < i_param.leg_names.length(); i++) {
-      dst_leg_names.push_back(std::string(i_param.leg_names[i]));
-  }
-  std::sort(cur_leg_names.begin(), cur_leg_names.end());
-  std::sort(dst_leg_names.begin(), dst_leg_names.end());
-  if (cur_leg_names != dst_leg_names) {
-      if (leg_names_interpolator->isEmpty()) {
-          leg_names.clear();
-          leg_names = dst_leg_names;
-          if (control_mode == MODE_ABC) {
-              double tmp_ratio = 0.0;
-              leg_names_interpolator->set(&tmp_ratio);
-              tmp_ratio = 1.0;
-              leg_names_interpolator->go(&tmp_ratio, 5.0, true);
-              control_mode = MODE_SYNC_TO_ABC;
-          }
-      }
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   leg_names cannot be set because interpolating." << std::endl;
-  }
-  pos_ik_thre = i_param.pos_ik_thre;
-  rot_ik_thre = i_param.rot_ik_thre;
-  if (!gg_is_walking) {
-      is_hand_fix_mode = i_param.is_hand_fix_mode;
-      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode = " << is_hand_fix_mode << std::endl;
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode cannot be set in (gg_is_walking = true). Current is_hand_fix_mode is " << (is_hand_fix_mode?"true":"false") << std::endl;
-  }
-  if (control_mode == MODE_IDLE) {
-      for (size_t i = 0; i < i_param.end_effector_list.length(); i++) {
-          std::map<std::string, ABCIKparam>::iterator it = ikp.find(std::string(i_param.end_effector_list[i].leg));
-          memcpy(it->second.localPos.data(), i_param.end_effector_list[i].pos, sizeof(double)*3);
-          it->second.localR = (Eigen::Quaternion<double>(i_param.end_effector_list[i].rot[0], i_param.end_effector_list[i].rot[1], i_param.end_effector_list[i].rot[2], i_param.end_effector_list[i].rot[3])).normalized().toRotationMatrix();
-      }
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl;
-  }
-  if (i_param.default_gait_type == OpenHRP::AutoBalancerService::BIPED) {
-      gait_type = BIPED;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::TROT) {
-      gait_type = TROT;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::PACE) {
-      gait_type = PACE;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::CRAWL) {
-      gait_type = CRAWL;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) {
-      gait_type = GALLOP;
-  }
-  for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
-      std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   localR = " << it->second.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
-  }
-
-  std::cerr << "[" << m_profile.instance_name << "]   move_base_gain = " << move_base_gain << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets = ";
-  for (size_t i = 0; i < ikp.size() * 3; i++) {
-      std::cerr << default_zmp_offsets_array[i] << " ";
-  }
-  std::cerr << std::endl;
-  delete[] default_zmp_offsets_array;
-  std::cerr << "[" << m_profile.instance_name << "]   use_force_mode = " << use_force << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_mode = " << graspless_manip_mode << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_arm = " << graspless_manip_arm << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_pos = " << graspless_manip_reference_trans_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_rot = " << graspless_manip_reference_trans_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   transition_time = " << transition_time << "[s], zmp_transition_time = " << zmp_transition_time << "[s], adjust_footstep_transition_time = " << adjust_footstep_transition_time << "[s]" << std::endl;
-  for (std::vector<std::string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) std::cerr << "[" << m_profile.instance_name << "]   leg_names [" << *it << "]" << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   pos_ik_thre = " << pos_ik_thre << "[m], rot_ik_thre = " << rot_ik_thre << "[rad]" << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   default_gait_type = " << gait_type << std::endl;
-  // IK limb parameters
-  std::cerr << "[" << m_profile.instance_name << "]  IK limb parameters" << std::endl;
-  bool is_ik_limb_parameter_valid_length = true;
-  if (i_param.ik_limb_parameters.length() != ee_vec.size()) {
-      is_ik_limb_parameter_valid_length = false;
-      std::cerr << "[" << m_profile.instance_name << "]   ik_limb_parameters invalid length! Cannot be set. (input = " << i_param.ik_limb_parameters.length() << ", desired = " << ee_vec.size() << ")" << std::endl;
-  } else {
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          if (ikp[ee_vec[i]].manip->numJoints() != i_param.ik_limb_parameters[i].ik_optional_weight_vector.length())
-              is_ik_limb_parameter_valid_length = false;
-      }
-      if (is_ik_limb_parameter_valid_length) {
-          for (size_t i = 0; i < ee_vec.size(); i++) {
-              ABCIKparam& param = ikp[ee_vec[i]];
-              const OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i];
-              std::vector<double> ov;
-              ov.resize(param.manip->numJoints());
-              for (size_t j = 0; j < param.manip->numJoints(); j++) {
-                  ov[j] = ilp.ik_optional_weight_vector[j];
-              }
-              param.manip->setOptionalWeightVector(ov);
-              param.manip->setSRGain(ilp.sr_gain);
-              param.avoid_gain = ilp.avoid_gain;
-              param.reference_gain = ilp.reference_gain;
-              param.manip->setManipulabilityLimit(ilp.manipulability_limit);
-          }
-      } else {
-          std::cerr << "[" << m_profile.instance_name << "]   ik_optional_weight_vector invalid length! Cannot be set. (input = [";
-          for (size_t i = 0; i < ee_vec.size(); i++) {
-              std::cerr << i_param.ik_limb_parameters[i].ik_optional_weight_vector.length() << ", ";
-          }
-          std::cerr << "], desired = [";
-          for (size_t i = 0; i < ee_vec.size(); i++) {
-              std::cerr << ikp[ee_vec[i]].manip->numJoints() << ", ";
-          }
-          std::cerr << "])" << std::endl;
-      }
-  }
-  if (is_ik_limb_parameter_valid_length) {
-      std::cerr << "[" << m_profile.instance_name << "]   ik_optional_weight_vectors = ";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          ABCIKparam& param = ikp[ee_vec[i]];
-          std::vector<double> ov;
-          ov.resize(param.manip->numJoints());
-          param.manip->getOptionalWeightVector(ov);
-          std::cerr << "[";
-          for (size_t j = 0; j < param.manip->numJoints(); j++) {
-              std::cerr << ov[j] << " ";
-          }
-          std::cerr << "]";
-      }
-      std::cerr << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   sr_gains = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].manip->getSRGain() << ", ";
-      }
-      std::cerr << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   avoid_gains = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].avoid_gain << ", ";
-      }
-      std::cerr << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   reference_gains = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].reference_gain << ", ";
-      }
-      std::cerr << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   manipulability_limits = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].manip->getManipulabilityLimit() << ", ";
-      }
-      std::cerr << "]" << std::endl;
-  }
-  return true;
-};
-
-bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
-{
-  i_param.move_base_gain = move_base_gain;
-  i_param.default_zmp_offsets.length(ikp.size());
-  for (size_t i = 0; i < ikp.size(); i++) {
-      i_param.default_zmp_offsets[i].length(3);
-      for (size_t j = 0; j < 3; j++) {
-          i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j);
-      }
-  }
-  switch(control_mode) {
-  case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break;
-  case MODE_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_ABC; break;
-  case MODE_SYNC_TO_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_IDLE; break;
-  case MODE_SYNC_TO_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_ABC; break;
-  default: break;
-  }
-  switch(use_force) {
-  case MODE_NO_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_NO_FORCE; break;
-  case MODE_REF_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE; break;
-  default: break;
-  }
-  i_param.graspless_manip_mode = graspless_manip_mode;
-  i_param.graspless_manip_arm = graspless_manip_arm.c_str();
-  for (size_t j = 0; j < 3; j++)
-      i_param.graspless_manip_p_gain[j] = graspless_manip_p_gain[j];
-  for (size_t j = 0; j < 3; j++)
-      i_param.graspless_manip_reference_trans_pos[j] = graspless_manip_reference_trans_coords.pos[j];
-  Eigen::Quaternion<double> qt(graspless_manip_reference_trans_coords.rot);
-  i_param.graspless_manip_reference_trans_rot[0] = qt.w();
-  i_param.graspless_manip_reference_trans_rot[1] = qt.x();
-  i_param.graspless_manip_reference_trans_rot[2] = qt.y();
-  i_param.graspless_manip_reference_trans_rot[3] = qt.z();
-  i_param.transition_time = transition_time;
-  i_param.zmp_transition_time = zmp_transition_time;
-  i_param.adjust_footstep_transition_time = adjust_footstep_transition_time;
-  i_param.leg_names.length(leg_names.size());
-  for (size_t i = 0; i < leg_names.size(); i++) i_param.leg_names[i] = leg_names.at(i).c_str();
-  i_param.pos_ik_thre = pos_ik_thre;
-  i_param.rot_ik_thre = rot_ik_thre;
-  i_param.is_hand_fix_mode = is_hand_fix_mode;
-  i_param.end_effector_list.length(ikp.size());
-  {
-      size_t i = 0;
-      for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
-          copyRatscoords2Footstep(i_param.end_effector_list[i],
-                                  coordinates(it->second.localPos, it->second.localR));
-          i_param.end_effector_list[i].leg = it->first.c_str();
-          i++;
-      }
-  }
-  switch(gait_type) {
-  case BIPED:  i_param.default_gait_type = OpenHRP::AutoBalancerService::BIPED;  break;
-  case TROT:   i_param.default_gait_type = OpenHRP::AutoBalancerService::TROT;   break;
-  case PACE:   i_param.default_gait_type = OpenHRP::AutoBalancerService::PACE;   break;
-  case CRAWL:  i_param.default_gait_type = OpenHRP::AutoBalancerService::CRAWL;  break;
-  case GALLOP: i_param.default_gait_type = OpenHRP::AutoBalancerService::GALLOP; break;
-  default: break;
-  }
-  i_param.ik_limb_parameters.length(ee_vec.size());
-  for (size_t i = 0; i < ee_vec.size(); i++) {
-      ABCIKparam& param = ikp[ee_vec[i]];
-      OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i];
-      ilp.ik_optional_weight_vector.length(param.manip->numJoints());
-      std::vector<double> ov;
-      ov.resize(param.manip->numJoints());
-      param.manip->getOptionalWeightVector(ov);
-      for (size_t j = 0; j < param.manip->numJoints(); j++) {
-          ilp.ik_optional_weight_vector[j] = ov[j];
-      }
-      ilp.sr_gain = param.manip->getSRGain();
-      ilp.avoid_gain = param.avoid_gain;
-      ilp.reference_gain = param.reference_gain;
-      ilp.manipulability_limit = param.manip->getManipulabilityLimit();
-  }
-  return true;
-};
-
-void AutoBalancer::copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs)
-{
-  memcpy(out_fs.pos, in_fs.pos.data(), sizeof(double)*3);
-  Eigen::Quaternion<double> qt(in_fs.rot);
-  out_fs.rot[0] = qt.w();
-  out_fs.rot[1] = qt.x();
-  out_fs.rot[2] = qt.y();
-  out_fs.rot[3] = qt.z();
-};
-
-bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param)
-{
-  copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_leg_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.swing_leg_dst_coords, gg->get_swing_leg_dst_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.dst_foot_midcoords, gg->get_dst_foot_midcoords());
-  if (gg->get_support_leg_names().front() == "rleg") {
-    i_param.support_leg = OpenHRP::AutoBalancerService::RLEG;
-  } else {
-    i_param.support_leg = OpenHRP::AutoBalancerService::LLEG;
-  }
-  switch ( gg->get_current_support_states().front() ) {
-  case BOTH: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::BOTH; break;
-  case RLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::RLEG; break;
-  case LLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::LLEG; break;
-  default: break;
-  }
-  return true;
-};
-
-bool AutoBalancer::adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep)
-{
-  std::cerr << "[" << m_profile.instance_name << "] adjustFootSteps" << std::endl;
-  if (control_mode == MODE_ABC && !gg_is_walking && adjust_footstep_interpolator->isEmpty()) {
-      Guard guard(m_mutex);
-      //
-      hrp::Vector3 eepos, org_mid_rpy, target_mid_rpy;
-      hrp::Matrix33 eerot, tmprot;
-      coordinates org_mid_coords, target_mid_coords;
-      // Get org coords
-      ikp["rleg"].adjust_interpolation_org_p0 = ikp["rleg"].target_p0;
-      ikp["lleg"].adjust_interpolation_org_p0 = ikp["lleg"].target_p0;
-      ikp["rleg"].adjust_interpolation_org_r0 = ikp["rleg"].target_r0;
-      ikp["lleg"].adjust_interpolation_org_r0 = ikp["lleg"].target_r0;
-      mid_coords(org_mid_coords, 0.5,
-                 coordinates(ikp["rleg"].adjust_interpolation_org_p0, ikp["rleg"].adjust_interpolation_org_r0),
-                 coordinates(ikp["lleg"].adjust_interpolation_org_p0, ikp["lleg"].adjust_interpolation_org_r0));
-      org_mid_rpy = hrp::rpyFromRot(org_mid_coords.rot);
-      // Get target coords
-      //   Input : ee coords
-      //   Output : link coords
-      memcpy(eepos.data(), rfootstep.pos, sizeof(double)*3);
-      eerot = (Eigen::Quaternion<double>(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
-      ikp["rleg"].adjust_interpolation_target_r0 = eerot * ikp["rleg"].localR.transpose();
-      ikp["rleg"].adjust_interpolation_target_p0 = eepos - ikp["rleg"].adjust_interpolation_target_r0 * ikp["rleg"].localPos;
-      memcpy(eepos.data(), lfootstep.pos, sizeof(double)*3);
-      eerot = (Eigen::Quaternion<double>(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
-      ikp["lleg"].adjust_interpolation_target_r0 = eerot * ikp["lleg"].localR.transpose();
-      ikp["lleg"].adjust_interpolation_target_p0 = eepos - ikp["lleg"].adjust_interpolation_target_r0 * ikp["lleg"].localPos;
-      mid_coords(target_mid_coords, 0.5,
-                 coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0),
-                 coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
-      coordinates rtrans, ltrans;
-      target_mid_coords.transformation(rtrans, coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0));
-      target_mid_coords.transformation(ltrans, coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
-      target_mid_rpy = hrp::rpyFromRot(target_mid_coords.rot);
-      // Fix target pos => org pos, target yaw => org yaw
-      target_mid_rpy(2) = org_mid_rpy(2);
-      target_mid_coords.rot = hrp::rotFromRpy(target_mid_rpy);
-      target_mid_coords.pos = org_mid_coords.pos;
-      // Calculate rleg and lleg coords
-      coordinates tmpc;
-      tmpc = target_mid_coords;
-      tmpc.transform(rtrans);
-      ikp["rleg"].adjust_interpolation_target_p0 = tmpc.pos;
-      ikp["rleg"].adjust_interpolation_target_r0 = tmpc.rot;
-      tmpc = target_mid_coords;
-      tmpc.transform(ltrans);
-      ikp["lleg"].adjust_interpolation_target_p0 = tmpc.pos;
-      ikp["lleg"].adjust_interpolation_target_r0 = tmpc.rot;
-      // Set interpolator
-      adjust_footstep_interpolator->clear();
-      double tmp = 0.0;
-      adjust_footstep_interpolator->set(&tmp);
-      tmp = 1.0;
-      adjust_footstep_interpolator->go(&tmp, adjust_footstep_transition_time, true);
-  }
-  while (!adjust_footstep_interpolator->isEmpty() )
-    usleep(1000);
-  usleep(1000);
-  return true;
-};
-
-bool AutoBalancer::getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx)
-{
-    std::cerr << "[" << m_profile.instance_name << "] getRemainingFootstepSequence" << std::endl;
-    o_footstep = new OpenHRP::AutoBalancerService::FootstepSequence;
-    if (gg_is_walking) {
-        std::vector< std::vector<step_node> > fsnl = gg->get_remaining_footstep_nodes_list();
-        o_current_fs_idx = gg->get_footstep_index();
-        o_footstep->length(fsnl.size());
-        for (size_t i = 0; i < fsnl.size(); i++) {
-            o_footstep[i].leg = (fsnl[i].front().l_r==RLEG?"rleg":"lleg");
-            copyRatscoords2Footstep(o_footstep[i], fsnl[i].front().worldcoords);
-        }
-    }
-};
-
-bool AutoBalancer::getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep)
-{
-    std::cerr << "[" << m_profile.instance_name << "] getGoPosFootstepsSequence" << std::endl;
-    o_footstep = new OpenHRP::AutoBalancerService::FootstepsSequence;
-    if (gg_is_walking) {
-        std::cerr << "[" << m_profile.instance_name << "] Cannot call getGoPosFootstepsSequence in walking" << std::endl;
-        return false;
-    } else {
-        gg->set_all_limbs(leg_names);
-        std::vector< std::vector<step_node> > new_footstep_nodes_list;
-        coordinates start_ref_coords;
-        std::vector<coordinates> initial_support_legs_coords;
-        std::vector<leg_type> initial_support_legs;
-        bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
-        if (is_valid_gait_type == false) return false;
-        /* go_pos_param_2_footstep_nodes_list_core is const member function  */
-        gg->go_pos_param_2_footstep_nodes_list_core (x, y, th,
-                                                     initial_support_legs_coords, start_ref_coords, initial_support_legs,
-                                                     new_footstep_nodes_list, true, 0);
-        o_footstep->length(new_footstep_nodes_list.size());
-        for (size_t i = 0; i < new_footstep_nodes_list.size(); i++) {
-            o_footstep[i].fs.length(new_footstep_nodes_list.at(i).size());
-            for (size_t j = 0; j < new_footstep_nodes_list.at(i).size(); j++) {
-                leg_type tmp_leg_type = new_footstep_nodes_list.at(i).at(j).l_r;
-                o_footstep[i].fs[j].leg = ((tmp_leg_type == RLEG) ? "rleg":
-                                           (tmp_leg_type == LLEG) ? "lleg":
-                                           (tmp_leg_type == RARM) ? "rarm":
-                                           "larm");
-                copyRatscoords2Footstep(o_footstep[i].fs[j], new_footstep_nodes_list.at(i).at(j).worldcoords);
-            }
-        }
-        return true;
-    }
-};
-
-void AutoBalancer::static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height)
-{
-  hrp::Vector3 target_sbp = hrp::Vector3(0, 0, 0);
-  hrp::Vector3 tmpcog = m_robot->calcCM();
-  switch ( use_force ) {
-  case MODE_REF_FORCE:
-    calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height, ref_forces);
-    tmp_input_sbp = target_sbp - sbp_offset;
-    sbp_cog_offset = tmp_input_sbp - tmpcog;
-    break;
-  case MODE_NO_FORCE:
-    tmp_input_sbp = tmpcog + sbp_cog_offset;
-    break;
-  default: break;
-  }
-};
-
-void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces)
-{
-  hrp::Vector3 denom, nume;
-  /* sb_point[m] = nume[kg * m/s^2 * m] / denom[kg * m/s^2] */
-  double mass = m_robot->totalMass();
-  for (size_t j = 0; j < 2; j++) {
-    nume(j) = mass * gg->get_gravitational_acceleration() * tmpcog(j);
-    denom(j) = mass * gg->get_gravitational_acceleration();
-    for (size_t i = 0; i < sensor_names.size(); i++) {
-      if ( sensor_names[i].find("hsensor") != std::string::npos || sensor_names[i].find("asensor") != std::string::npos ) { // tempolary to get arm force coords
-          hrp::Link* parentlink;
-          hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
-          if (sensor) parentlink = sensor->link;
-          else parentlink = m_vfs[sensor_names[i]].link;
-          for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-              if (it->second.target_link->name == parentlink->name) {
-                  hrp::Vector3 fpos = parentlink->p + parentlink->R * it->second.localPos;
-                  nume(j) += ( (fpos(2) - ref_com_height) * tmp_forces[i](j) - fpos(j) * tmp_forces[i](2) );
-                  denom(j) -= tmp_forces[i](2);
-              }
-          }
-      }
-    }
-    sb_point(j) = nume(j) / denom(j);
-  }
-  sb_point(2) = ref_com_height;
-};
-
-#ifndef rad2deg
-#define rad2deg(rad) (rad * 180 / M_PI)
-#endif
-#ifndef deg2rad
-#define deg2rad(deg) (deg * M_PI / 180)
-#endif
-
-hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_coords)
-{
-  if (graspless_manip_mode) {
-    hrp::Vector3 dp,dr;
-    coordinates ref_hand_coords(gg->get_dst_foot_midcoords()), act_hand_coords;
-    ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords
-    hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos);
-    if ( graspless_manip_arm == "arms" ) {
-        hrp::Vector3 rarm_pos = ikp["rarm"].target_p0 + ikp["rarm"].target_r0 * ikp["rarm"].localPos;
-        hrp::Vector3 larm_pos = ikp["larm"].target_p0 + ikp["larm"].target_r0 * ikp["larm"].localPos;
-        act_hand_coords.pos = (rarm_pos+larm_pos)/2.0;
-        hrp::Vector3 act_y = larm_pos-rarm_pos;
-        act_y(2) = 0;
-        act_y.normalize();
-        hrp::Vector3 ref_y(ref_hand_coords.rot * hrp::Vector3::UnitY());
-        ref_y(2) = 0;
-        ref_y.normalize();
-        dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y))); // fix for rotation
-    } else {
-      ABCIKparam& tmpikp = ikp[graspless_manip_arm];
-      act_hand_coords = rats::coordinates(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos,
-                                          tmpikp.target_r0 * tmpikp.localR);
-      rats::difference_rotation(dr, ref_hand_coords.rot, act_hand_coords.rot);
-      dr(0) = 0; dr(1) = 0;
-    }
-    dp = act_hand_coords.pos - ref_hand_coords.pos
-        + dr.cross(hrp::Vector3(foot_pos - act_hand_coords.pos));
-    dp(2) = 0;
-    hrp::Matrix33 foot_mt(gg->get_dst_foot_midcoords().rot.transpose());
-    //alias(dp) = foot_mt * dp;
-    hrp::Vector3 dp2 = foot_mt * dp;
-    //alias(dr) = foot_mt * dr;
-    return hrp::Vector3(graspless_manip_p_gain[0] * dp2(0)/gg->get_default_step_time(),
-                        graspless_manip_p_gain[1] * dp2(1)/gg->get_default_step_time(),
-                        graspless_manip_p_gain[2] * rad2deg(dr(2))/gg->get_default_step_time());
-  } else {
-    return hrp::Vector3::Zero();
-  }
-};
-
-bool AutoBalancer::calc_inital_support_legs(const double& y, std::vector<coordinates>& initial_support_legs_coords, std::vector<leg_type>& initial_support_legs, coordinates& start_ref_coords) {
-    switch(gait_type) {
-    case BIPED:
-        initial_support_legs_coords = (y > 0 ?
-                                       boost::assign::list_of(ikp["rleg"].target_end_coords)
-                                       : boost::assign::list_of(ikp["lleg"].target_end_coords));
-        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
-        break;
-    case TROT:
-        initial_support_legs_coords = (y > 0 ?
-                                       boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["larm"].target_end_coords)
-                                       : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["rarm"].target_end_coords));
-        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
-        break;
-    case PACE:
-        initial_support_legs_coords = (y > 0 ?
-                                       boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["rarm"].target_end_coords)
-                                       : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["larm"].target_end_coords));
-        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
-        break;
-    case CRAWL:
-        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    case GALLOP:
-        /* at least one leg shoud be in contact */
-        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    default: break;
-    }
-    mid_coords(start_ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
-    return true;
-};
-
-//
-extern "C"
-{
-
-    void AutoBalancerInit(RTC::Manager* manager)
-    {
-        RTC::Properties profile(autobalancer_spec);
-        manager->registerFactory(profile,
-                                 RTC::Create<AutoBalancer>,
-                                 RTC::Delete<AutoBalancer>);
-    }
-
-};
-
-
diff --git a/rtc/AutoBalancer/AutoBalancer.h.mas b/rtc/AutoBalancer/AutoBalancer.h.mas
deleted file mode 100644
index be21747854d..00000000000
--- a/rtc/AutoBalancer/AutoBalancer.h.mas
+++ /dev/null
@@ -1,270 +0,0 @@
-// -*- C++ -*-
-/*!
- * @file  AutoBalancer.h
- * @brief autobalancer component
- * @date  $Date$
- *
- * $Id$
- */
-
-#ifndef AUTOBALANCER_H
-#define AUTOBALANCER_H
-
-#include <rtm/Manager.h>
-#include <rtm/DataFlowComponentBase.h>
-#include <rtm/CorbaPort.h>
-#include <rtm/DataInPort.h>
-#include <rtm/DataOutPort.h>
-#include <rtm/idl/BasicDataTypeSkel.h>
-#include <rtm/idl/ExtendedDataTypesSkel.h>
-#include <hrpModel/Body.h>
-#include "../ImpedanceController/JointPathEx.h"
-#include "../ImpedanceController/RatsMatrix.h"
-#include "GaitGenerator.h"
-// Service implementation headers
-// <rtc-template block="service_impl_h">
-#include "AutoBalancerService_impl.h"
-#include "interpolator.h"
-
-// </rtc-template>
-
-// Service Consumer stub headers
-// <rtc-template block="consumer_stub_h">
-
-// </rtc-template>
-
-using namespace RTC;
-
-class AutoBalancer
-  : public RTC::DataFlowComponentBase
-{
- public:
-  AutoBalancer(RTC::Manager* manager);
-  virtual ~AutoBalancer();
-
-  // The initialize action (on CREATED->ALIVE transition)
-  // formaer rtc_init_entry()
- virtual RTC::ReturnCode_t onInitialize();
-
-  // The finalize action (on ALIVE->END transition)
-  // formaer rtc_exiting_entry()
-  virtual RTC::ReturnCode_t onFinalize();
-
-  // The startup action when ExecutionContext startup
-  // former rtc_starting_entry()
-  // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
-
-  // The shutdown action when ExecutionContext stop
-  // former rtc_stopping_entry()
-  // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
-
-  // The activated action (Active state entry action)
-  // former rtc_active_entry()
-  virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
-
-  // The deactivated action (Active state exit action)
-  // former rtc_active_exit()
-  virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
-
-  // The execution action that is invoked periodically
-  // former rtc_active_do()
-  virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
-
-  // The aborting action when main logic error occurred.
-  // former rtc_aborting_entry()
-  // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
-
-  // The error action in ERROR state
-  // former rtc_error_do()
-  // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
-
-  // The reset action that is invoked resetting
-  // This is same but different the former rtc_init_entry()
-  // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
-
-  // The state update action that is invoked after onExecute() action
-  // no corresponding operation exists in OpenRTm-aist-0.2.0
-  // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
-
-  // The action that is invoked when execution context's rate is changed
-  // no corresponding operation exists in OpenRTm-aist-0.2.0
-  // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
-  bool goPos(const double& x, const double& y, const double& th);
-  bool goVelocity(const double& vx, const double& vy, const double& vth);
-  bool goStop();
-  bool emergencyStop ();
-  bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx);
-  bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx);
-  bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx);
-  bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx);
-  void waitFootSteps();
-  void waitFootStepsEarly(const double tm);
-  bool startAutoBalancer(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
-  bool stopAutoBalancer();
-  bool setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param);
-  bool getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param);
-  bool setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param);
-  bool getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param);
-  bool getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param);
-  bool adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep);
-  bool getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx);
-  bool getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep);
-  bool releaseEmergencyStop();
-
- protected:
-  // Configuration variable declaration
-  // <rtc-template block="config_declare">
-  
-  // </rtc-template>
-
-  // DataInPort declaration
-  // <rtc-template block="inport_declare">
-  TimedDoubleSeq m_qRef;
-  InPort<TimedDoubleSeq> m_qRefIn;
-  TimedPoint3D m_basePos;
-  InPort<TimedPoint3D> m_basePosIn;
-  TimedOrientation3D m_baseRpy;
-  InPort<TimedOrientation3D> m_baseRpyIn;
-  TimedPoint3D m_zmp;
-  InPort<TimedPoint3D> m_zmpIn;
-  TimedDoubleSeq m_optionalData;
-  InPort<TimedDoubleSeq> m_optionalDataIn;
-  std::vector<TimedDoubleSeq> m_ref_force;
-  std::vector<InPort<TimedDoubleSeq> *> m_ref_forceIn;
-  TimedLong m_emergencySignal;
-  InPort<TimedLong> m_emergencySignalIn;
-  // for debug
-  TimedPoint3D m_cog;
-  
-  // </rtc-template>
-
-  // DataOutPort declaration
-  // <rtc-template block="outport_declare">
-  OutPort<TimedDoubleSeq> m_qOut;
-  RTC::OutPort<RTC::TimedPoint3D> m_zmpOut;
-  OutPort<TimedPoint3D> m_basePosOut;
-  OutPort<TimedOrientation3D> m_baseRpyOut;
-  TimedDoubleSeq m_baseTform;
-  OutPort<TimedDoubleSeq> m_baseTformOut;
-  TimedPose3D m_basePose;
-  OutPort<TimedPose3D> m_basePoseOut;
-  TimedAcceleration3D m_accRef;
-  OutPort<TimedAcceleration3D> m_accRefOut;
-  TimedBooleanSeq m_contactStates;
-  OutPort<TimedBooleanSeq> m_contactStatesOut;
-  TimedDoubleSeq m_controlSwingSupportTime;
-  OutPort<TimedDoubleSeq> m_controlSwingSupportTimeOut;
-  TimedBoolean m_walkingStates;
-  OutPort<TimedBoolean> m_walkingStatesOut;
-  TimedPoint3D m_sbpCogOffset;
-  OutPort<TimedPoint3D> m_sbpCogOffsetOut;
-  std::vector<TimedDoubleSeq> m_force;
-  std::vector<OutPort<TimedDoubleSeq> *> m_ref_forceOut;
-  std::vector<TimedPoint3D> m_limbCOPOffset;
-  std::vector<OutPort<TimedPoint3D> *> m_limbCOPOffsetOut;
-  // for debug
-  OutPort<TimedPoint3D> m_cogOut;
-  
-  // </rtc-template>
-
-  // CORBA Port declaration
-  // <rtc-template block="corbaport_declare">
-  RTC::CorbaPort m_AutoBalancerServicePort;
-
-  // </rtc-template>
-
-  // Service declaration
-  // <rtc-template block="service_declare">
-  AutoBalancerService_impl m_service0;
-
-  // </rtc-template>
-
-  // Consumer declaration
-  // <rtc-template block="consumer_declare">
-  
-  // </rtc-template>
-
- private:
-  struct ABCIKparam {
-    hrp::Vector3 target_p0, localPos, adjust_interpolation_target_p0, adjust_interpolation_org_p0;
-    hrp::Matrix33 target_r0, localR, adjust_interpolation_target_r0, adjust_interpolation_org_r0;
-    rats::coordinates target_end_coords;
-    hrp::Link* target_link;
-    hrp::JointPathExPtr manip;
-    double avoid_gain, reference_gain;
-    size_t pos_ik_error_count, rot_ik_error_count;
-    bool is_active, has_toe_joint;
-  };
-  void getCurrentParameters();
-  void getTargetParameters();
-  bool solveLimbIKforLimb (ABCIKparam& param, const std::string& limb_name);
-  void solveLimbIK();
-  void startABCparam(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
-  void stopABCparam();
-  void waitABCTransition();
-  hrp::Matrix33 OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2);
-  void fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot);
-  void startWalking ();
-  void stopWalking ();
-  void copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs);
-  // static balance point offsetting
-  void static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height);
-  void calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces);
-  hrp::Vector3 calc_vel_from_hand_error (const rats::coordinates& tmp_fix_coords);
-  bool isOptionalDataContact (const std::string& ee_name)
-  {
-      return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false;
-  };
-  bool calc_inital_support_legs(const double& y, std::vector<rats::coordinates>& initial_support_legs_coords, std::vector<rats::leg_type>& initial_support_legs, rats::coordinates& start_ref_coords);
-
-  // for gg
-  typedef boost::shared_ptr<rats::gait_generator> ggPtr;
-  ggPtr gg;
-  bool gg_is_walking, gg_solved;
-  // for abc
-  hrp::Vector3 ref_cog, ref_zmp, prev_imu_sensor_pos, prev_imu_sensor_vel, hand_fix_initial_offset;
-  enum {BIPED, TROT, PACE, CRAWL, GALLOP} gait_type;
-  enum {MODE_IDLE, MODE_ABC, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_ABC} control_mode, return_control_mode;
-  std::map<std::string, ABCIKparam> ikp;
-  std::map<std::string, size_t> contact_states_index_map;
-  std::map<std::string, hrp::VirtualForceSensorParam> m_vfs;
-  std::vector<std::string> sensor_names, leg_names, ee_vec;
-  hrp::dvector qorg, qrefv;
-  hrp::Vector3 current_root_p, target_root_p;
-  hrp::Matrix33 current_root_R, target_root_R;
-  rats::coordinates fix_leg_coords;
-  std::vector<hrp::Vector3> default_zmp_offsets;
-  double m_dt, move_base_gain;
-  hrp::BodyPtr m_robot;
-  coil::Mutex m_mutex;
-
-  double transition_interpolator_ratio, transition_time, zmp_transition_time, adjust_footstep_transition_time, leg_names_interpolator_ratio;
-  interpolator *zmp_offset_interpolator;
-  interpolator *transition_interpolator;
-  interpolator *adjust_footstep_interpolator;
-  interpolator *leg_names_interpolator;
-  hrp::Vector3 input_zmp, input_basePos;
-  hrp::Matrix33 input_baseRot;
-
-  // static balance point offsetting
-  hrp::Vector3 sbp_offset, sbp_cog_offset;
-  enum {MODE_NO_FORCE, MODE_REF_FORCE} use_force;
-  std::vector<hrp::Vector3> ref_forces;
-
-  unsigned int m_debugLevel;
-  bool is_legged_robot, is_stop_mode, has_ik_failed, is_hand_fix_mode, is_hand_fix_initial;
-  int loop, ik_error_debug_print_freq;
-  bool graspless_manip_mode;
-  std::string graspless_manip_arm;
-  hrp::Vector3 graspless_manip_p_gain;
-  rats::coordinates graspless_manip_reference_trans_coords;
-  double pos_ik_thre, rot_ik_thre;
-};
-
-
-extern "C"
-{
-  void AutoBalancerInit(RTC::Manager* manager);
-};
-
-#endif // IMPEDANCE_H

From 12464715b58c22b78aa84d8b3b6edef073ed5604 Mon Sep 17 00:00:00 2001
From: ishiguroJSK <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 19 Dec 2016 18:56:12 +0900
Subject: [PATCH 039/217] enable cv::convexhull

---
 rtc/AutoBalancer/AutoBalancer.h | 108 ++++++++++++++++++++++++++++++++
 rtc/AutoBalancer/CMakeLists.txt |   2 +-
 2 files changed, 109 insertions(+), 1 deletion(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index a5d74216cf3..d18b1e62f7b 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -29,6 +29,10 @@
 #include "interpolator.h"
 #include "../TorqueFilter/IIRFilter.h"
 
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>
+
 
 // </rtc-template>
 
@@ -595,6 +599,7 @@ class HumanSynchronizer{
       hrp::Vector3 cp_ragulated;
 
       margin_t margin_from_4maxcp;
+      checkMarginToFootSupportRegionCV(com_old, com_old, rfin_abs, lfin_abs, 0.02, 0.01, 0.01, 0.01, margin_from_4maxcp, bb);
       checkMarginToFootSupportRegion(com_old, com_old, rfin_abs, lfin_abs, 0.02, 0.01, 0.00, 0.00, margin_from_4maxcp, bb);
       hrp::Vector3 max_com_vel,min_com_vel;
       max_com_vel(0) = margin_from_4maxcp.x_u / sqrt( H / G );
@@ -758,6 +763,109 @@ class HumanSynchronizer{
 
       return true;
     }
+    bool checkMarginToFootSupportRegionCV(const hrp::Vector3& pt_in, const hrp::Vector3& pt_old, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const double& forward_mgn, const double& back_mgn, const double& outer_mgn, const double& inner_mgn, margin_t& mgn_ans, hrp::Vector3& pt_ans){
+      std::vector<cv::Point2f> points,hull;
+      points.push_back(cv::Point2f(lfin_abs(0) - back_mgn,    lfin_abs(1) + outer_mgn));//LFの左下
+      points.push_back(cv::Point2f(lfin_abs(0) + forward_mgn, lfin_abs(1) + outer_mgn));//LFの左上
+      points.push_back(cv::Point2f(lfin_abs(0) + forward_mgn, lfin_abs(1) - inner_mgn));//LFの右上
+      points.push_back(cv::Point2f(lfin_abs(0) - back_mgn,    lfin_abs(1) - inner_mgn));//LFの右下
+      points.push_back(cv::Point2f(rfin_abs(0) - back_mgn,    rfin_abs(1) + inner_mgn));//RFの左下
+      points.push_back(cv::Point2f(rfin_abs(0) + forward_mgn, rfin_abs(1) + inner_mgn));//RFの左上
+      points.push_back(cv::Point2f(rfin_abs(0) + forward_mgn, rfin_abs(1) - outer_mgn));//RFの右上
+      points.push_back(cv::Point2f(rfin_abs(0) - back_mgn,    rfin_abs(1) - outer_mgn));//RFの右下
+      cv::convexHull(cv::Mat(points),hull,true);
+      cout<<"hull"<<hull<<endl;
+//
+//      if(forward_mgn< -back_mgn || outer_mgn< -inner_mgn ){std::cerr << "Invalid margin input!" << std::endl;return false;}
+//      if(lfin_abs(0)>rfin_abs(0)){
+//        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) + outer_mgn));//LFの左下
+//        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + forward_mgn, lfin_abs(1) + outer_mgn));//LFの左上
+//        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + forward_mgn, lfin_abs(1) - inner_mgn));//LFの右上
+//        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + forward_mgn, rfin_abs(1) - outer_mgn));//RFの右上
+//        convex_hull.push_back(hrp::Vector2(rfin_abs(0) - back_mgn,    rfin_abs(1) - outer_mgn));//RFの右下
+//        convex_hull.push_back(hrp::Vector2(rfin_abs(0) - back_mgn,    rfin_abs(1) + inner_mgn));//RFの左下
+//        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) + outer_mgn));//LFの左下
+//      }else{
+//        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) - inner_mgn));//LFの右下
+//        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) + outer_mgn));//LFの左下
+//        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + forward_mgn, lfin_abs(1) + outer_mgn));//LFの左上
+//        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + forward_mgn, rfin_abs(1) + inner_mgn));//RFの左上
+//        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + forward_mgn, rfin_abs(1) - outer_mgn));//RFの右上
+//        convex_hull.push_back(hrp::Vector2(rfin_abs(0) - back_mgn,    rfin_abs(1) - outer_mgn));//RFの右下
+//        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) - inner_mgn));//LFの右下
+//      }
+//      hrp::Vector2 check_point(pt_in(0),pt_in(1));
+//      hrp::Vector2 ans_point = check_point;
+//      enum point_state_t {PT_IN_NO_PATTERN = -1, PT_IN_REGION, PT_FIX_TO_EDGE, PT_FIX_TO_VERTEX};
+//      point_state_t pt_state = PT_IN_REGION;
+//      for(int i=0;i<convex_hull.size()-1;i++){//対象の点が凸包の内部に存在するかチェックする
+//        hrp::Vector2 cur_vert(convex_hull[i](0),convex_hull[i](1));
+//        hrp::Vector2 next_vert(convex_hull[i+1](0),convex_hull[i+1](1));
+//        hrp::Vector2 edge_v = next_vert - cur_vert;
+//        hrp::Vector2 tgt_pt_v = check_point - cur_vert;
+//        if((edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){  pt_state = PT_IN_NO_PATTERN; break; }
+//      }
+//      if(pt_state != PT_IN_REGION){
+//        int ans_lid=-1;
+//        for(int i=0;i<convex_hull.size()-1;i++){//対象の点がある線分への垂線を有するかチェックする
+//          hrp::Vector2 cur_vert(convex_hull[i](0),convex_hull[i](1));
+//          hrp::Vector2 next_vert(convex_hull[i+1](0),convex_hull[i+1](1));
+//          hrp::Vector2 edge_v = next_vert - cur_vert;
+//          hrp::Vector2 tgt_pt_v = check_point - cur_vert;
+//          //ある線分への垂線を有し，かつ外側(時計回りエッジに対して左側)に存在するなら，対象の点からそのエッジへの垂線の交点が最近傍点
+//          if(edge_v.dot(tgt_pt_v)/edge_v.norm() > 0 && edge_v.dot(tgt_pt_v)/edge_v.norm() < edge_v.norm() && (edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
+//            ans_lid = i;
+//            ans_point = cur_vert + edge_v.normalized() * (edge_v.dot(tgt_pt_v)/edge_v.norm());
+//            pt_state = PT_FIX_TO_EDGE;
+//            break;
+//          }
+//        }
+//        if(pt_state != PT_FIX_TO_EDGE){//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
+//          double cur_min_dis = (check_point - convex_hull[0]).norm();
+//          int ans_pid = 0;
+//          for(int i=1;i<convex_hull.size();i++){
+//            if((check_point - convex_hull[i]).norm() < cur_min_dis){
+//              cur_min_dis = (check_point - convex_hull[i]).norm();
+//              ans_pid = i;
+//            }
+//          }
+//          ans_point(0) = convex_hull[ans_pid](0);
+//          ans_point(1) = convex_hull[ans_pid](1);
+//          pt_state = PT_FIX_TO_VERTEX;
+//        }
+//      }
+//      pt_ans(0) = ans_point(0);//領域はみ出ている場合この答えは点から領域への最近傍点(だからちょっと意味が違う)
+//      pt_ans(1) = ans_point(1);//
+//
+//      hrp::Vector2 anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
+//      hrp::Vector2 cross_pt;
+//      double margin[4] = {0};
+//      for(int direc=0;direc<4;direc++){
+//        for(int i=0;i<convex_hull.size()-1;i++){
+//          double dBunbo = anchor_vec[direc](0) * ( convex_hull[i+1](1) - convex_hull[i](1) ) - anchor_vec[direc](1) * ( convex_hull[i+1](0) - convex_hull[i](0) );
+//          if( dBunbo != 0.0)  {//平行の場合を外す
+//            hrp::Vector2 vectorAC = convex_hull[i] - ans_point;
+//            double dR = ( ( convex_hull[i+1](1) - convex_hull[i](1) ) * vectorAC(0) - ( convex_hull[i+1](0) - convex_hull[i](0) ) * vectorAC(1) ) / dBunbo;
+//            double dS = ( anchor_vec[direc](1) * vectorAC(0) - anchor_vec[direc](0) * vectorAC(1) ) / dBunbo;
+//            //dR==0のときは含めないことで，点が境界上でも領域内側向きのマージンをチェックさせる
+//            //dRの上限が無いのはアンカーベクトルの無限遠まで交点を許可するから(アンカーベクトルの長さは1で定義しているため)
+//            //点が斜線境界上にあるとまれに dR = 8.67362e-18 とかいう0でない値がでたりする．
+//            //恐らく点を斜線境界で打ち切った時に数値誤差で完全に斜線に乗り切らず，数値誤差分はみ出ている状況と思われる．
+//            if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){
+//              cross_pt = ans_point + dR * anchor_vec[direc];
+//              margin[direc] = dR * anchor_vec[direc].norm();
+//            }
+//          }
+//        }
+//      }
+//      mgn_ans.x_u = margin[0];
+//      mgn_ans.x_l = -margin[1];
+//      mgn_ans.y_u = margin[2];
+//      mgn_ans.y_l = -margin[3];
+
+
+      return true;
+    }
 
     void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
       comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
diff --git a/rtc/AutoBalancer/CMakeLists.txt b/rtc/AutoBalancer/CMakeLists.txt
index 556f704e5e6..ea2c0e743f0 100644
--- a/rtc/AutoBalancer/CMakeLists.txt
+++ b/rtc/AutoBalancer/CMakeLists.txt
@@ -1,5 +1,5 @@
 set(comp_sources AutoBalancer.cpp AutoBalancerService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp PreviewController.cpp GaitGenerator.cpp ../TorqueFilter/IIRFilter.cpp)
-set(libs hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub)
+set(libs ${OpenCV_LIBRARIES} hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub)
 add_library(AutoBalancer SHARED ${comp_sources})
 target_link_libraries(AutoBalancer ${libs})
 set_target_properties(AutoBalancer PROPERTIES PREFIX "")

From dc914c6c8bbf6f862d0ee6d074c070c620013c49 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 20 Dec 2016 19:19:22 +0900
Subject: [PATCH 040/217] enable convex_hull limit CVver

---
 rtc/AutoBalancer/AutoBalancer.h | 374 ++++++++++----------------------
 1 file changed, 112 insertions(+), 262 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index d18b1e62f7b..95aa83e4392 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -32,6 +32,7 @@
 #include <opencv2/core/core.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/imgproc/types_c.h>
+#include <hrpUtil/Eigen4d.h>
 
 
 // </rtc-template>
@@ -171,6 +172,7 @@ class HumanSynchronizer{
     BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters;
     BiquadIIRFilterVec cam_rpy_filter;
     hrp::Vector3 r_zmp_raw;
+    hrp::Vector4 rf_safe_region,lf_safe_region;//前後左右の順
 
   public:
     double tgt_FUP_TIME;
@@ -238,6 +240,8 @@ class HumanSynchronizer{
       acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
       cam_rpy_filter.setParameter(1,HZ);//カメラアングル
 
+      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
+      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
 
       use_rh = use_lh = false;
 
@@ -299,9 +303,9 @@ class HumanSynchronizer{
       applyEEWorkspaceLimit               (rp_ref_out);
       lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
 
-      applyCOMToSupportRegionLimit        (rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
+//      applyCOMToSupportRegionLimit        (rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
       applyLPFilter                       (rp_ref_out);
-      applyCOMStateLimitByCapturePoint    (rp_ref_out.com, rp_ref_out_old.com, com_vel_old, rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
+//      applyCOMStateLimitByCapturePoint    (rp_ref_out.com, rp_ref_out_old.com, com_vel_old, rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
 
       cam_rpy_filtered = cam_rpy_filter.passFilter(cam_rpy);
 
@@ -433,7 +437,7 @@ class HumanSynchronizer{
       is_f_contact_out = (cur_fup_count_in <= 0);
     }
     void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
-      if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足設置しているタイミングでのみ更新
+      if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足接地しているタイミングでのみ更新
       calcFootUpCurveAndJudgeFootContact(rf_goland_in, cur_rfup_level, tgt.rf, is_rf_contact);
       calcFootUpCurveAndJudgeFootContact(lf_goland_in, cur_lfup_level, tgt.lf, is_lf_contact);
     }
@@ -454,10 +458,9 @@ class HumanSynchronizer{
       }
     }
     void applyEEWorkspaceLimit(HumanPose& tgt){
-//      const hrp::Vector2 rf2lf_vec( (tgt.lf(0)+rp_wld_initpos.lf(0))-(tgt.rf(0)+rp_wld_initpos.rf(0)) , (tgt.lf(1)+rp_wld_initpos.lf(1))-(tgt.rf(1)+rp_wld_initpos.rf(1)) );
       const hrp::Vector2 rf2lf_vec( tgt.lf(0)-tgt.rf(0), tgt.lf(1)-tgt.rf(1) );
       const hrp::Vector2 lf2rf_vec = - rf2lf_vec;
-      const double MAX = 0.15;
+      const double MAX = 10.15;
       if(!is_rf_contact && is_lf_contact){//右足浮遊時
         if(lf2rf_vec.norm() > MAX){
           tgt.rf(0) = (lf2rf_vec.normalized())(0) * MAX + tgt.lf(0);
@@ -477,93 +480,13 @@ class HumanSynchronizer{
         else if (tgt.rh(i) < hand_llimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.rh(i) = hand_llimit(i) + current_basepos(i) - init_basepos(i); }
         if      (tgt.lh(i) > hand_ulimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.lh(i) = hand_ulimit(i) + current_basepos(i) - init_basepos(i); }
         else if (tgt.lh(i) < hand_llimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.lh(i) = hand_llimit(i) + current_basepos(i) - init_basepos(i); }
-
       }
     }
     bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
-//      const double XUMARGIN = 0.04;//CHIDORI
-//      const double XLMARGIN = -0.02;
-//      const double YUMARGIN = 0.01;
-//      const double YLMARGIN = -0.01;
-      const double XUMARGIN = 0.02;//JAXON
-      const double XLMARGIN = -0.01;
-      const double YUMARGIN = 0.0;
-      const double YLMARGIN = -0.0;
-//      const double XUMARGIN = 0.10;//test
-//      const double XLMARGIN = -0.05;
-//      const double YUMARGIN = 0.03;
-//      const double YLMARGIN = -0.03;
-      std::vector<hrp::Vector2> convex_hull;
-      if(lfin_abs(0)>rfin_abs(0)){
-        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN));//LFの左下
-        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XUMARGIN, lfin_abs(1) + YUMARGIN));//LFの左上
-        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XUMARGIN, lfin_abs(1) + YLMARGIN));//LFの右上
-        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + XUMARGIN, rfin_abs(1) + YLMARGIN));//RFの右上
-        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + XLMARGIN, rfin_abs(1) + YLMARGIN));//RFの右下
-        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + XLMARGIN, rfin_abs(1) + YUMARGIN));//RFの左下
-        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN));//LFの左下
-      }else{
-        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XLMARGIN, lfin_abs(1) + YLMARGIN));//LFの右下
-        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN));//LFの左下
-        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XUMARGIN, lfin_abs(1) + YUMARGIN));//LFの左上
-        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + XUMARGIN, rfin_abs(1) + YUMARGIN));//RFの左上
-        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + XUMARGIN, rfin_abs(1) + YLMARGIN));//RFの右上
-        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + XLMARGIN, rfin_abs(1) + YLMARGIN));//RFの右下
-        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + XLMARGIN, lfin_abs(1) + YLMARGIN));//LFの右下
-      }
-      hrp::Vector2 check_point(comin_abs(0),comin_abs(1));
-      hrp::Vector2 ans_point = check_point;
-      enum point_state_t {PT_IN_NO_PATTERN = -1, PT_IN_REGION, PT_FIX_TO_EDGE, PT_FIX_TO_VERTEX};
-      point_state_t pt_state = PT_IN_REGION;
-
-      for(int i=0;i<convex_hull.size()-1;i++){//対象の点が凸包の内部に存在するかチェックする
-        hrp::Vector2 cur_vert(convex_hull[i](0),convex_hull[i](1));
-        hrp::Vector2 next_vert(convex_hull[i+1](0),convex_hull[i+1](1));
-        hrp::Vector2 edge_v = next_vert - cur_vert;
-        hrp::Vector2 tgt_pt_v = check_point - cur_vert;
-        if((edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
-          pt_state = PT_IN_NO_PATTERN;
-//          cout<<"COM out of s_region"<<endl;
-          break;
-        }
-      }
-      if(pt_state != PT_IN_REGION){
-        int ans_lid=-1;
-        for(int i=0;i<convex_hull.size()-1;i++){//対象の点がある線分への垂線を有するかチェックする
-          hrp::Vector2 cur_vert(convex_hull[i](0),convex_hull[i](1));
-          hrp::Vector2 next_vert(convex_hull[i+1](0),convex_hull[i+1](1));
-          hrp::Vector2 edge_v = next_vert - cur_vert;
-          hrp::Vector2 tgt_pt_v = check_point - cur_vert;
-          //ある線分への垂線を有し，かつ外側(時計回りエッジに対して左側)に存在するなら，対象の点からそのエッジへの垂線の交点が最近傍点
-          if(edge_v.dot(tgt_pt_v)/edge_v.norm() > 0 && edge_v.dot(tgt_pt_v)/edge_v.norm() < edge_v.norm() && (edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
-            ans_lid = i;
-            ans_point = cur_vert + edge_v.normalized() * (edge_v.dot(tgt_pt_v)/edge_v.norm());
-            pt_state = PT_FIX_TO_EDGE;
-//            cout<<"point will on the line:"<<ans_lid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
-            break;
-          }
-        }
-        if(pt_state != PT_FIX_TO_EDGE){//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
-          double cur_min_dis = (check_point - convex_hull[0]).norm();
-          int ans_pid = 0;
-          for(int i=1;i<convex_hull.size();i++){
-            if((check_point - convex_hull[i]).norm() < cur_min_dis){
-              cur_min_dis = (check_point - convex_hull[i]).norm();
-              ans_pid = i;
-            }
-          }
-          ans_point(0) = convex_hull[ans_pid](0);
-          ans_point(1) = convex_hull[ans_pid](1);
-//          cout<<"point will on the vertex:"<<ans_pid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
-          pt_state = PT_FIX_TO_VERTEX;
-        }
-      }
-      comin_abs(0) = ans_point(0);
-      comin_abs(1) = ans_point(1);
+      hrp::Vector4 margin;
+      checkMarginToFootSupportRegionCV(comin_abs, rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, margin, comin_abs);
       return true;
     }
-
-
     bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& com_old, const hrp::Vector3& com_vel_old, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans){
       com_ans = com_in;
       hrp::Vector3 com_vel = (com_in - com_old)/DT;
@@ -571,11 +494,8 @@ class HumanSynchronizer{
       hrp::Vector3 com_vel_zmpOk, com_vel_zmpOk_cpOk;
       double accel[4] = {0.05, -0.03, 0.02, -0.02};
       double decel[4] = {0.04, -0.02, 0.01, -0.01};
-
       comacc_ref = calcacc_v_filters.passFilter(com_acc);
-
       const double H = rp_wld_initpos.com(2);
-
       double maxacc[2],minacc[2];
       maxacc[0] = (com_old(0) - accel[1]) * (G/H); if(maxacc[0] < 0){maxacc[0]=0;}
       minacc[0] = (com_old(0) - accel[0]) * (G/H); if(minacc[0] > 0){minacc[0]=0;}
@@ -590,63 +510,120 @@ class HumanSynchronizer{
 //      if(comacc_ref(1) > maxacc[1]){com_vel(1) = maxacc[1] * DT + com_vel_old(1); cout<<"maxacc[1] over"<<endl;}
 //      if(comacc_ref(1) < minacc[1]){com_vel(1) = minacc[1] * DT + com_vel_old(1); cout<<"minacc[1] over"<<endl;}
 //      cout<<"comacc_ref(1) "<<comacc_ref(1)<<" minacc[1] "<<minacc[1]<<endl;
-
-
-
 ////      cout<<"maxacc "<<maxacc[1]<<" minacc "<<minacc[1]<< " acc " << (com_vel(1)-com_vel_old(1))/DT <<endl;
-//
       hrp::Vector3 bb;
       hrp::Vector3 cp_ragulated;
-
-      margin_t margin_from_4maxcp;
-      checkMarginToFootSupportRegionCV(com_old, com_old, rfin_abs, lfin_abs, 0.02, 0.01, 0.01, 0.01, margin_from_4maxcp, bb);
-      checkMarginToFootSupportRegion(com_old, com_old, rfin_abs, lfin_abs, 0.02, 0.01, 0.00, 0.00, margin_from_4maxcp, bb);
+      hrp::Vector4 margin_from_4maxcp;
+      checkMarginToFootSupportRegionCV(com_old, rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, margin_from_4maxcp, bb);
       hrp::Vector3 max_com_vel,min_com_vel;
-      max_com_vel(0) = margin_from_4maxcp.x_u / sqrt( H / G );
-      min_com_vel(0) = margin_from_4maxcp.x_l / sqrt( H / G );
-      max_com_vel(1) = margin_from_4maxcp.y_u / sqrt( H / G );
-      min_com_vel(1) = margin_from_4maxcp.y_l / sqrt( H / G );
+      max_com_vel(0) = margin_from_4maxcp(0) / sqrt( H / G );
+      min_com_vel(0) = margin_from_4maxcp(1) / sqrt( H / G );
+      max_com_vel(1) = margin_from_4maxcp(2) / sqrt( H / G );
+      min_com_vel(1) = margin_from_4maxcp(3) / sqrt( H / G );
       if(com_vel(0) > max_com_vel(0)){ com_vel(0) = max_com_vel(0); if(loop%100==0)cout<<"F cp over"<<endl;}//ここブレーキ時のZMP条件に変えたほうがいいかもしれない
       if(com_vel(0) < min_com_vel(0)){ com_vel(0) = min_com_vel(0); if(loop%100==0)cout<<"B cp over"<<endl;}
       if(com_vel(1) > max_com_vel(1)){ com_vel(1) = max_com_vel(1); if(loop%100==0)cout<<"L cp over"<<endl;}
       if(com_vel(1) < min_com_vel(1)){ com_vel(1) = min_com_vel(1); if(loop%100==0)cout<<"R cp over"<<endl;}
-
-
-
       com_ans(0) = com_old(0) + com_vel(0) * DT;
       com_ans(1) = com_old(1) + com_vel(1) * DT;
-
-
-//      if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,com_ans(0),com_ans(1),com_vel(0),com_vel(1),margin_from_4maxcp.y_u,margin_from_4maxcp.y_l);
-//      cout<<"max_com_vel(0)"<<max_com_vel(0)<<endl;
-//      cout<<"min_com_vel(0)"<<min_com_vel(0)<<endl;
-//      cout<<"com_vel(0)"<<com_vel(0)<<endl;
-//      cout<<"com_ans(0)"<<com_ans(0)<<endl;
-//      cout<<"max_com_vel(1)"<<max_com_vel(1)<<endl;
-//      cout<<"min_com_vel(1)"<<min_com_vel(1)<<endl;
-//      cout<<"com_vel(1)"<<com_vel(1)<<endl;
-//      cout<<"com_ans(1)"<<com_ans(1)<<endl;
-
-//      //ここからはcom_inがそんなに領域外に出ていない仮定
-//      checkMarginToFootSupportRegion(com_in, com_old, rfin_abs, lfin_abs, 0.04, 0.02, 0.01, 0.01, margin_from_4maxcp, cp_ragulated);
-//      hrp::Vector2 com_raw_vel2d,cp_ragulated_2d,com_old_2d;
-//      com_raw_vel2d(0) = (com_in(0) - com_old(0))/DT;
-//      com_raw_vel2d(1) = (com_in(1) - com_old(1))/DT;
-//      cp_ragulated_2d(0) = cp_ragulated(0);
-//      cp_ragulated_2d(1) = cp_ragulated(1);
-//      com_old_2d(0) = com_old(0);
-//      com_old_2d(1) = com_old(1);
-//      if( com_raw_vel2d.norm() * sqrt(H/G) > (cp_ragulated_2d - com_old_2d).norm()){
-//        com_raw_vel2d *= (cp_ragulated_2d - com_old_2d).norm()/( com_raw_vel2d.norm() * sqrt(H/G) );
-//        std::cerr << "CP work" << std::endl;
-//        com_ans(0) = com_old(0) + com_raw_vel2d(0) * DT;
-//        com_ans(1) = com_old(1) + com_raw_vel2d(1) * DT;
-//
-//      }
+      return true;
+    }
+    bool checkMarginToFootSupportRegionCV(const hrp::Vector3& pt_in, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, hrp::Vector4& mgn_ans, hrp::Vector3& pt_ans){
+      std::vector<cv::Point2f> points,hull;
+      for(int i=0;i<2;i++){
+        for(int j=2;j<4;j++){
+          points.push_back(cv::Point2f(rfin_abs(0) + rf_mgn(i),    rfin_abs(1) + rf_mgn(j)));
+          points.push_back(cv::Point2f(lfin_abs(0) + lf_mgn(i),    lfin_abs(1) + lf_mgn(j)));
+        }
+      }
+      cv::convexHull(cv::Mat(points),hull,true);
+      hrp::Vector2 check_point(pt_in(0),pt_in(1));
+      hrp::Vector2 ans_point = check_point;
+      if(cv::pointPolygonTest(cv::Mat(hull), cv::Point2f(pt_in(0),pt_in(1)), false) < 0){//hullの外にいる場合
+        bool pt_can_be_set_to_edge = false;
+        for(int i=0;i<hull.size();i++){//対象の点がある線分への垂線を有するかチェックする
+          int next_i = (i!=hull.size()-1 ? i+1 : 0);
+          hrp::Vector2 cur_vert(hull[i].x,hull[i].y);
+          hrp::Vector2 next_vert(hull[next_i].x,hull[next_i].y);
+          hrp::Vector2 edge_v = next_vert - cur_vert;
+          hrp::Vector2 tgt_pt_v = check_point - cur_vert;
+          //ある線分への垂線を有し，かつ外側(時計回りエッジに対して左側)に存在するなら，対象の点からそのエッジへの垂線の交点が最近傍点
+          if(edge_v.dot(tgt_pt_v)/edge_v.norm() > 0 && edge_v.dot(tgt_pt_v)/edge_v.norm() < edge_v.norm() && (edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
+            ans_point = cur_vert + edge_v.normalized() * (edge_v.dot(tgt_pt_v)/edge_v.norm());
+            pt_can_be_set_to_edge = true;
+            break;
+          }
+        }
+        if(!pt_can_be_set_to_edge){//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
+          double cur_min_dis = (check_point - hrp::Vector2(hull[0].x,hull[0].y)).norm();
+          int ans_pid = 0;
+          for(int i=1;i<hull.size();i++){
+            if((check_point - hrp::Vector2(hull[i].x,hull[i].y)).norm() < cur_min_dis){
+              cur_min_dis = (check_point - hrp::Vector2(hull[i].x,hull[i].y)).norm();
+              ans_pid = i;
+            }
+          }
+          ans_point(0) = hull[ans_pid].x;
+          ans_point(1) = hull[ans_pid].y;
+        }
+      }
+      hrp::Vector2 anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
+      hrp::Vector2 cross_pt;
+      double margin[4] = {0};
+      for(int direc=0;direc<4;direc++){
+        for(int i=0;i<hull.size();i++){
+          int next_i = (i!=hull.size()-1 ? i+1 : 0);
+          double dBunbo = anchor_vec[direc](0) * ( hull[next_i].y - hull[i].y ) - anchor_vec[direc](1) * ( hull[next_i].x - hull[i].x );
+          if( dBunbo != 0.0)  {//平行の場合を外す
+            hrp::Vector2 vectorAC = hrp::Vector2(hull[i].x,hull[i].y) - ans_point;
+            double dR = ( ( hull[next_i].y - hull[i].y ) * vectorAC(0) - ( hull[next_i].x - hull[i].x ) * vectorAC(1) ) / dBunbo;
+            double dS = ( anchor_vec[direc](1) * vectorAC(0) - anchor_vec[direc](0) * vectorAC(1) ) / dBunbo;
+            //dR==0のときは含めないことで，点が境界上でも領域内側向きのマージンをチェックさせる
+            //dRの上限が無いのはアンカーベクトルの無限遠まで交点を許可するから(アンカーベクトルの長さは1で定義しているため)
+            //点が斜線境界上にあるとまれに dR = 8.67362e-18 とかいう0でない値がでたりする．
+            //恐らく点を斜線境界で打ち切った時に数値誤差で完全に斜線に乗り切らず，数値誤差分はみ出ている状況と思われる．
+            if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){
+              cross_pt = ans_point + dR * anchor_vec[direc];
+              margin[direc] = dR * anchor_vec[direc].norm();
+            }
+          }
+        }
+      }
+      mgn_ans(0) = margin[0];
+      mgn_ans(1) = -margin[1];
+      mgn_ans(2) = margin[2];
+      mgn_ans(3) = -margin[3];
+      pt_ans(0) = ans_point(0);//
+      pt_ans(1) = ans_point(1);//
       return true;
     }
 
-
+    void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
+      comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
+      const double MAXACC = 5;
+      if(comacc(0)>MAXACC)comacc(0)=MAXACC;else if(comacc(0)<-MAXACC)comacc(0)=-MAXACC;
+      if(comacc(1)>MAXACC)comacc(1)=MAXACC;else if(comacc(1)<-MAXACC)comacc(1)=-MAXACC;
+      comacc = acc4zmp_v_filters.passFilter(comacc);
+      zmpout(0) = comin(0)-(rp_wld_initpos.com(2)/G)*comacc(0);
+      zmpout(1) = comin(1)-(rp_wld_initpos.com(2)/G)*comacc(1);
+      if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.zmp(0),rp_ref_out.zmp(1),zmpout(0),zmpout(1));
+      com_oldold = com_old;
+      com_old = comin;
+    }
+    void applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out){
+      for(int i=0;i<3;i++){if(in.com(i) - in_old.com(i) > MAXVEL){out.com(i) = in_old.com(i) + MAXVEL;}else if(in.com(i) - in_old.com(i) < -MAXVEL){out.com(i) = in_old.com(i) - MAXVEL;}}
+      for(int i=0;i<3;i++){if(in.rf(i)  - in_old.rf(i)  > MAXVEL){out.rf(i)  = in_old.rf(i)  + MAXVEL;}else if(in.rf(i)  - in_old.rf(i)  < -MAXVEL){out.rf(i)  = in_old.rf(i)  - MAXVEL;}}
+      for(int i=0;i<3;i++){if(in.lf(i)  - in_old.lf(i)  > MAXVEL){out.lf(i)  = in_old.lf(i)  + MAXVEL;}else if(in.lf(i)  - in_old.lf(i)  < -MAXVEL){out.lf(i)  = in_old.lf(i)  - MAXVEL;}}
+      for(int i=0;i<3;i++){if(in.rh(i)  - in_old.rh(i)  > MAXVEL){out.rh(i)  = in_old.rh(i)  + MAXVEL;}else if(in.rh(i)  - in_old.rh(i)  < -MAXVEL){out.rh(i)  = in_old.rh(i)  - MAXVEL;}}
+      for(int i=0;i<3;i++){if(in.lh(i)  - in_old.lh(i)  > MAXVEL){out.lh(i)  = in_old.lh(i)  + MAXVEL;}else if(in.lh(i)  - in_old.lh(i)  < -MAXVEL){out.lh(i)  = in_old.lh(i)  - MAXVEL;}}
+    }
+    void applyCOMZMPXYZLock(HumanPose& tgt){
+        if(!use_x){tgt.com(0) = 0;tgt.zmp(0) = 0;}
+        if(!use_y){tgt.com(1) = 0;tgt.zmp(1) = 0;}
+        if(!use_z){tgt.com(2) = 0;}
+    }
+    //unuse
+    /*
     bool checkMarginToFootSupportRegion(const hrp::Vector3& pt_in, const hrp::Vector3& pt_old, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const double& forward_mgn, const double& back_mgn, const double& outer_mgn, const double& inner_mgn, margin_t& mgn_ans, hrp::Vector3& pt_ans){
       std::vector<hrp::Vector2> convex_hull;
       if(forward_mgn< -back_mgn || outer_mgn< -inner_mgn ){std::cerr << "Invalid margin input!" << std::endl;return false;}
@@ -763,134 +740,7 @@ class HumanSynchronizer{
 
       return true;
     }
-    bool checkMarginToFootSupportRegionCV(const hrp::Vector3& pt_in, const hrp::Vector3& pt_old, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const double& forward_mgn, const double& back_mgn, const double& outer_mgn, const double& inner_mgn, margin_t& mgn_ans, hrp::Vector3& pt_ans){
-      std::vector<cv::Point2f> points,hull;
-      points.push_back(cv::Point2f(lfin_abs(0) - back_mgn,    lfin_abs(1) + outer_mgn));//LFの左下
-      points.push_back(cv::Point2f(lfin_abs(0) + forward_mgn, lfin_abs(1) + outer_mgn));//LFの左上
-      points.push_back(cv::Point2f(lfin_abs(0) + forward_mgn, lfin_abs(1) - inner_mgn));//LFの右上
-      points.push_back(cv::Point2f(lfin_abs(0) - back_mgn,    lfin_abs(1) - inner_mgn));//LFの右下
-      points.push_back(cv::Point2f(rfin_abs(0) - back_mgn,    rfin_abs(1) + inner_mgn));//RFの左下
-      points.push_back(cv::Point2f(rfin_abs(0) + forward_mgn, rfin_abs(1) + inner_mgn));//RFの左上
-      points.push_back(cv::Point2f(rfin_abs(0) + forward_mgn, rfin_abs(1) - outer_mgn));//RFの右上
-      points.push_back(cv::Point2f(rfin_abs(0) - back_mgn,    rfin_abs(1) - outer_mgn));//RFの右下
-      cv::convexHull(cv::Mat(points),hull,true);
-      cout<<"hull"<<hull<<endl;
-//
-//      if(forward_mgn< -back_mgn || outer_mgn< -inner_mgn ){std::cerr << "Invalid margin input!" << std::endl;return false;}
-//      if(lfin_abs(0)>rfin_abs(0)){
-//        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) + outer_mgn));//LFの左下
-//        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + forward_mgn, lfin_abs(1) + outer_mgn));//LFの左上
-//        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + forward_mgn, lfin_abs(1) - inner_mgn));//LFの右上
-//        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + forward_mgn, rfin_abs(1) - outer_mgn));//RFの右上
-//        convex_hull.push_back(hrp::Vector2(rfin_abs(0) - back_mgn,    rfin_abs(1) - outer_mgn));//RFの右下
-//        convex_hull.push_back(hrp::Vector2(rfin_abs(0) - back_mgn,    rfin_abs(1) + inner_mgn));//RFの左下
-//        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) + outer_mgn));//LFの左下
-//      }else{
-//        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) - inner_mgn));//LFの右下
-//        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) + outer_mgn));//LFの左下
-//        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + forward_mgn, lfin_abs(1) + outer_mgn));//LFの左上
-//        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + forward_mgn, rfin_abs(1) + inner_mgn));//RFの左上
-//        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + forward_mgn, rfin_abs(1) - outer_mgn));//RFの右上
-//        convex_hull.push_back(hrp::Vector2(rfin_abs(0) - back_mgn,    rfin_abs(1) - outer_mgn));//RFの右下
-//        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) - inner_mgn));//LFの右下
-//      }
-//      hrp::Vector2 check_point(pt_in(0),pt_in(1));
-//      hrp::Vector2 ans_point = check_point;
-//      enum point_state_t {PT_IN_NO_PATTERN = -1, PT_IN_REGION, PT_FIX_TO_EDGE, PT_FIX_TO_VERTEX};
-//      point_state_t pt_state = PT_IN_REGION;
-//      for(int i=0;i<convex_hull.size()-1;i++){//対象の点が凸包の内部に存在するかチェックする
-//        hrp::Vector2 cur_vert(convex_hull[i](0),convex_hull[i](1));
-//        hrp::Vector2 next_vert(convex_hull[i+1](0),convex_hull[i+1](1));
-//        hrp::Vector2 edge_v = next_vert - cur_vert;
-//        hrp::Vector2 tgt_pt_v = check_point - cur_vert;
-//        if((edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){  pt_state = PT_IN_NO_PATTERN; break; }
-//      }
-//      if(pt_state != PT_IN_REGION){
-//        int ans_lid=-1;
-//        for(int i=0;i<convex_hull.size()-1;i++){//対象の点がある線分への垂線を有するかチェックする
-//          hrp::Vector2 cur_vert(convex_hull[i](0),convex_hull[i](1));
-//          hrp::Vector2 next_vert(convex_hull[i+1](0),convex_hull[i+1](1));
-//          hrp::Vector2 edge_v = next_vert - cur_vert;
-//          hrp::Vector2 tgt_pt_v = check_point - cur_vert;
-//          //ある線分への垂線を有し，かつ外側(時計回りエッジに対して左側)に存在するなら，対象の点からそのエッジへの垂線の交点が最近傍点
-//          if(edge_v.dot(tgt_pt_v)/edge_v.norm() > 0 && edge_v.dot(tgt_pt_v)/edge_v.norm() < edge_v.norm() && (edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
-//            ans_lid = i;
-//            ans_point = cur_vert + edge_v.normalized() * (edge_v.dot(tgt_pt_v)/edge_v.norm());
-//            pt_state = PT_FIX_TO_EDGE;
-//            break;
-//          }
-//        }
-//        if(pt_state != PT_FIX_TO_EDGE){//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
-//          double cur_min_dis = (check_point - convex_hull[0]).norm();
-//          int ans_pid = 0;
-//          for(int i=1;i<convex_hull.size();i++){
-//            if((check_point - convex_hull[i]).norm() < cur_min_dis){
-//              cur_min_dis = (check_point - convex_hull[i]).norm();
-//              ans_pid = i;
-//            }
-//          }
-//          ans_point(0) = convex_hull[ans_pid](0);
-//          ans_point(1) = convex_hull[ans_pid](1);
-//          pt_state = PT_FIX_TO_VERTEX;
-//        }
-//      }
-//      pt_ans(0) = ans_point(0);//領域はみ出ている場合この答えは点から領域への最近傍点(だからちょっと意味が違う)
-//      pt_ans(1) = ans_point(1);//
-//
-//      hrp::Vector2 anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
-//      hrp::Vector2 cross_pt;
-//      double margin[4] = {0};
-//      for(int direc=0;direc<4;direc++){
-//        for(int i=0;i<convex_hull.size()-1;i++){
-//          double dBunbo = anchor_vec[direc](0) * ( convex_hull[i+1](1) - convex_hull[i](1) ) - anchor_vec[direc](1) * ( convex_hull[i+1](0) - convex_hull[i](0) );
-//          if( dBunbo != 0.0)  {//平行の場合を外す
-//            hrp::Vector2 vectorAC = convex_hull[i] - ans_point;
-//            double dR = ( ( convex_hull[i+1](1) - convex_hull[i](1) ) * vectorAC(0) - ( convex_hull[i+1](0) - convex_hull[i](0) ) * vectorAC(1) ) / dBunbo;
-//            double dS = ( anchor_vec[direc](1) * vectorAC(0) - anchor_vec[direc](0) * vectorAC(1) ) / dBunbo;
-//            //dR==0のときは含めないことで，点が境界上でも領域内側向きのマージンをチェックさせる
-//            //dRの上限が無いのはアンカーベクトルの無限遠まで交点を許可するから(アンカーベクトルの長さは1で定義しているため)
-//            //点が斜線境界上にあるとまれに dR = 8.67362e-18 とかいう0でない値がでたりする．
-//            //恐らく点を斜線境界で打ち切った時に数値誤差で完全に斜線に乗り切らず，数値誤差分はみ出ている状況と思われる．
-//            if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){
-//              cross_pt = ans_point + dR * anchor_vec[direc];
-//              margin[direc] = dR * anchor_vec[direc].norm();
-//            }
-//          }
-//        }
-//      }
-//      mgn_ans.x_u = margin[0];
-//      mgn_ans.x_l = -margin[1];
-//      mgn_ans.y_u = margin[2];
-//      mgn_ans.y_l = -margin[3];
-
-
-      return true;
-    }
-
-    void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
-      comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
-      const double MAXACC = 5;
-      if(comacc(0)>MAXACC)comacc(0)=MAXACC;else if(comacc(0)<-MAXACC)comacc(0)=-MAXACC;
-      if(comacc(1)>MAXACC)comacc(1)=MAXACC;else if(comacc(1)<-MAXACC)comacc(1)=-MAXACC;
-      comacc = acc4zmp_v_filters.passFilter(comacc);
-      zmpout(0) = comin(0)-(rp_wld_initpos.com(2)/G)*comacc(0);
-      zmpout(1) = comin(1)-(rp_wld_initpos.com(2)/G)*comacc(1);
-      if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.zmp(0),rp_ref_out.zmp(1),zmpout(0),zmpout(1));
-      com_oldold = com_old;
-      com_old = comin;
-    }
-    void applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out){
-      for(int i=0;i<3;i++){if(in.com(i) - in_old.com(i) > MAXVEL){out.com(i) = in_old.com(i) + MAXVEL;}else if(in.com(i) - in_old.com(i) < -MAXVEL){out.com(i) = in_old.com(i) - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.rf(i)  - in_old.rf(i)  > MAXVEL){out.rf(i)  = in_old.rf(i)  + MAXVEL;}else if(in.rf(i)  - in_old.rf(i)  < -MAXVEL){out.rf(i)  = in_old.rf(i)  - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.lf(i)  - in_old.lf(i)  > MAXVEL){out.lf(i)  = in_old.lf(i)  + MAXVEL;}else if(in.lf(i)  - in_old.lf(i)  < -MAXVEL){out.lf(i)  = in_old.lf(i)  - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.rh(i)  - in_old.rh(i)  > MAXVEL){out.rh(i)  = in_old.rh(i)  + MAXVEL;}else if(in.rh(i)  - in_old.rh(i)  < -MAXVEL){out.rh(i)  = in_old.rh(i)  - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.lh(i)  - in_old.lh(i)  > MAXVEL){out.lh(i)  = in_old.lh(i)  + MAXVEL;}else if(in.lh(i)  - in_old.lh(i)  < -MAXVEL){out.lh(i)  = in_old.lh(i)  - MAXVEL;}}
-    }
-    void applyCOMZMPXYZLock(HumanPose& tgt){
-        if(!use_x){tgt.com(0) = 0;tgt.zmp(0) = 0;}
-        if(!use_y){tgt.com(1) = 0;tgt.zmp(1) = 0;}
-        if(!use_z){tgt.com(2) = 0;}
-    }
+    */
 };
 
 

From e0c04032d8858af94d82cc5713329e9cd6fec2fb Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 21 Dec 2016 00:01:58 +0900
Subject: [PATCH 041/217] fix lockSwingFootIfZMPOutOfSupportFoot() problem

---
 rtc/AutoBalancer/AutoBalancer.h | 29 ++++++++++++++++-------------
 1 file changed, 16 insertions(+), 13 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 95aa83e4392..5f2324cb09d 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -235,13 +235,15 @@ class HumanSynchronizer{
 
       iir_v_filters.resize(5);
       for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(0.5,HZ);//四肢拘束点用
-      iir_v_filters[0].setParameter(0.5,HZ);//重心用
+      iir_v_filters[0].setParameter(0.4,HZ);//重心用
       calcacc_v_filters.setParameter(5,HZ);//加速度計算用
       acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
       cam_rpy_filter.setParameter(1,HZ);//カメラアングル
 
       rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
       lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
+//      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.0);
+//      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.0, -0.02);
 
       use_rh = use_lh = false;
 
@@ -298,21 +300,22 @@ class HumanSynchronizer{
       convertRelHumanPoseToRelRobotPose   (hp_filtered,rp_ref_out);
       applyCOMMoveModRatio                (rp_ref_out);
       judgeFootLandOnCommand              (hp_wld_raw.rfw, hp_wld_raw.lfw, go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
+      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
 
 
       applyEEWorkspaceLimit               (rp_ref_out);
       lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
 
-//      applyCOMToSupportRegionLimit        (rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
+      applyCOMToSupportRegionLimit        (rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
       applyLPFilter                       (rp_ref_out);
-//      applyCOMStateLimitByCapturePoint    (rp_ref_out.com, rp_ref_out_old.com, com_vel_old, rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
+      applyCOMStateLimitByCapturePoint    (rp_ref_out.com, rp_ref_out_old.com, com_vel_old, rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
 
       cam_rpy_filtered = cam_rpy_filter.passFilter(cam_rpy);
 
 
       r_zmp_raw = rp_ref_out.zmp;
       applyZMPCalcFromCOM                 (rp_ref_out.com,rp_ref_out.zmp);
-      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, go_rf_landing, go_lf_landing);//
+//      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, go_rf_landing, go_lf_landing);//ここじゃ判定遅い
       applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       applyCOMZMPXYZLock                  (rp_ref_out);
       overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
@@ -428,9 +431,9 @@ class HumanSynchronizer{
     void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, int& cur_fup_count_in, hrp::Vector3& f_height_out, bool& is_f_contact_out){
       int FUP_COUNT = (int)(HZ*FUP_TIME);
       if(cur_fup_count_in > FUP_COUNT){cur_fup_count_in = FUP_COUNT;} if(cur_fup_count_in < 0){cur_fup_count_in = 0;}
-      if(!go_land_in){//足下げ命令入力時
+      if(!go_land_in){//足上げ命令入力時
         if(cur_fup_count_in < FUP_COUNT){ cur_fup_count_in++; }
-      }else{//足上げ命令入力時
+      }else{//足下げ命令入力時
         if(cur_fup_count_in > 0){ cur_fup_count_in--; }
       }
       f_height_out(2) = FUP_HIGHT/2*(1-cos(M_PI*cur_fup_count_in/FUP_COUNT));
@@ -458,19 +461,19 @@ class HumanSynchronizer{
       }
     }
     void applyEEWorkspaceLimit(HumanPose& tgt){
-      const hrp::Vector2 rf2lf_vec( tgt.lf(0)-tgt.rf(0), tgt.lf(1)-tgt.rf(1) );
-      const hrp::Vector2 lf2rf_vec = - rf2lf_vec;
-      const double MAX = 10.15;
+      const double MAX = 0.15;
       if(!is_rf_contact && is_lf_contact){//右足浮遊時
+        const hrp::Vector2 lf2rf_vec( tgt.rf(0)-pre_cont_lfpos(0), tgt.rf(1)-pre_cont_lfpos(1) );
         if(lf2rf_vec.norm() > MAX){
-          tgt.rf(0) = (lf2rf_vec.normalized())(0) * MAX + tgt.lf(0);
-          tgt.rf(1) = (lf2rf_vec.normalized())(1) * MAX + tgt.lf(1);
+          tgt.rf(0) = (lf2rf_vec.normalized())(0) * MAX + pre_cont_lfpos(0);
+          tgt.rf(1) = (lf2rf_vec.normalized())(1) * MAX + pre_cont_lfpos(1);
         }
       }
       else if(is_rf_contact && !is_lf_contact){//左足浮遊時
+        const hrp::Vector2 rf2lf_vec( tgt.lf(0)-pre_cont_rfpos(0), tgt.lf(1)-pre_cont_rfpos(1) );
         if(rf2lf_vec.norm() > MAX){
-          tgt.lf(0) = (rf2lf_vec.normalized())(0) * MAX + tgt.rf(0);
-          tgt.lf(1) = (rf2lf_vec.normalized())(1) * MAX + tgt.rf(1);
+          tgt.lf(0) = (rf2lf_vec.normalized())(0) * MAX + pre_cont_rfpos(0);
+          tgt.lf(1) = (rf2lf_vec.normalized())(1) * MAX + pre_cont_rfpos(1);
         }
       }
       hrp::Vector3 hand_ulimit = hrp::Vector3(0.1,0.1,0.1);

From f2694a640c0c0c3d02fd8f90768a7a5a4e585886 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 22 Dec 2016 15:28:22 +0900
Subject: [PATCH 042/217] add neck safe margin

---
 rtc/AutoBalancer/AutoBalancer.h | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 5f2324cb09d..ec2d7d8a77b 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -302,7 +302,6 @@ class HumanSynchronizer{
       judgeFootLandOnCommand              (hp_wld_raw.rfw, hp_wld_raw.lfw, go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
 
-
       applyEEWorkspaceLimit               (rp_ref_out);
       lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
 
@@ -310,6 +309,10 @@ class HumanSynchronizer{
       applyLPFilter                       (rp_ref_out);
       applyCOMStateLimitByCapturePoint    (rp_ref_out.com, rp_ref_out_old.com, com_vel_old, rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
 
+      if(cam_rpy(1) > 20*M_PI/180)cam_rpy(1) = 20*M_PI/180;
+      if(cam_rpy(1) < -20*M_PI/180)cam_rpy(1) = -20*M_PI/180;
+      if(cam_rpy(2) > 20*M_PI/180)cam_rpy(2) = 20*M_PI/180;
+      if(cam_rpy(2) < -20*M_PI/180)cam_rpy(2) = -20*M_PI/180;
       cam_rpy_filtered = cam_rpy_filter.passFilter(cam_rpy);
 
 
@@ -488,6 +491,8 @@ class HumanSynchronizer{
     bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
       hrp::Vector4 margin;
       checkMarginToFootSupportRegionCV(comin_abs, rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, margin, comin_abs);
+      if( comin_abs(2) > 0.03 ) comin_abs(2) = 0.03;
+      if( comin_abs(2) < -0.15 ) comin_abs(2) = -0.15;
       return true;
     }
     bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& com_old, const hrp::Vector3& com_vel_old, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans){

From 25bfc162e2ffb744e09a5014169c5955fdd250f3 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 24 Dec 2016 02:06:53 +0900
Subject: [PATCH 043/217] add Qhull convex func and non XY independent CP
 limitation

---
 rtc/AutoBalancer/AutoBalancer.h | 335 +++++++++++++-------------------
 1 file changed, 138 insertions(+), 197 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index ec2d7d8a77b..b583df13a07 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -34,6 +34,23 @@
 #include <opencv2/imgproc/types_c.h>
 #include <hrpUtil/Eigen4d.h>
 
+#if defined(__cplusplus)
+extern "C"
+{
+#endif
+#include <stdio.h>
+#include <stdlib.h>
+#include <qhull/qhull.h>
+#include <qhull/mem.h>
+#include <qhull/qset.h>
+#include <qhull/geom.h>
+#include <qhull/merge.h>
+#include <qhull/poly.h>
+#include <qhull/io.h>
+#include <qhull/stat.h>
+#if defined(__cplusplus)
+}
+#endif
 
 // </rtc-template>
 
@@ -97,6 +114,7 @@ class HumanPose{
     int idsize;
     typedef struct{ hrp::Vector3 f; hrp::Vector3 t; }Wrench6;
     Wrench6 rfw,lfw;
+    hrp::Vector3 neck;
     HumanPose() : idsize(10){
       clear();
     }
@@ -120,7 +138,6 @@ class HumanPose{
       }
     }
     const hrp::Vector3& Seq(const int i) const{//const指定オブジェクトからオーバーロードする時のためのオーバーライド
-//      return Seq(i);//呼べない・・・うーん
       switch(i){
         case 0: return com;
         case 1: return rf;
@@ -158,7 +175,6 @@ class HumanSynchronizer{
     HumanPose hp_wld_raw;
     HumanPose hp_wld_initpos;
     HumanPose hp_rel_raw;
-    HumanPose hp_filtered;
     HumanPose rp_ref_out_old;
     int countdown_num;
     bool HumanSyncOn;
@@ -295,9 +311,8 @@ class HumanSynchronizer{
       removeInitOffsetPose                (hp_wld_raw, hp_wld_initpos, hp_rel_raw);
       applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);
 //      lockFootXYOnContact                 (go_rf_landing, go_lf_landing, hp_rel_raw);//根本から改変すべき2
-      hp_filtered = hp_rel_raw;
-      calcWorldZMP                        ((hp_filtered.rf+init_wld_hp_rfpos), (hp_filtered.lf+init_wld_hp_lfpos), hp_filtered.rfw, hp_filtered.lfw, hp_filtered.zmp);//足の位置はworldにしないと・・・
-      convertRelHumanPoseToRelRobotPose   (hp_filtered,rp_ref_out);
+      calcWorldZMP                        ((hp_rel_raw.rf+init_wld_hp_rfpos), (hp_rel_raw.lf+init_wld_hp_lfpos), hp_rel_raw.rfw, hp_rel_raw.lfw, hp_rel_raw.zmp);//足の位置はworldにしないと・・・
+      convertRelHumanPoseToRelRobotPose   (hp_rel_raw, rp_ref_out);
       applyCOMMoveModRatio                (rp_ref_out);
       judgeFootLandOnCommand              (hp_wld_raw.rfw, hp_wld_raw.lfw, go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
@@ -315,7 +330,6 @@ class HumanSynchronizer{
       if(cam_rpy(2) < -20*M_PI/180)cam_rpy(2) = -20*M_PI/180;
       cam_rpy_filtered = cam_rpy_filter.passFilter(cam_rpy);
 
-
       r_zmp_raw = rp_ref_out.zmp;
       applyZMPCalcFromCOM                 (rp_ref_out.com,rp_ref_out.zmp);
 //      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, go_rf_landing, go_lf_landing);//ここじゃ判定遅い
@@ -355,6 +369,7 @@ class HumanSynchronizer{
       if(in.length() == 6){out.f(0)=in[0]; out.f(1)=in[1]; out.f(2)=in[2]; out.t(0)=in[3]; out.t(1)=in[4]; out.t(2)=in[5];
       }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; out.f.Zero(); out.t.Zero(); }
     }
+    static double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(0)*b(1)-a(1)*b(0); }
 
   private:
     void updateHumanToRobotRatio(const double h2r_r_goal){//h2r_ratioに伴って変わる変数の処理もここに書く
@@ -489,8 +504,12 @@ class HumanSynchronizer{
       }
     }
     bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
-      hrp::Vector4 margin;
-      checkMarginToFootSupportRegionCV(comin_abs, rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, margin, comin_abs);
+      std::vector<hrp::Vector2> hull;
+      createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull);
+      hrp::Vector2 cog(comin_abs(0),comin_abs(1));
+      if(!isPointInHullOpenCV(cog,hull)){ calcNearestPointOnHull(cog,hull,cog); }//外に出たら最近傍点に頭打ち
+      comin_abs(0) = cog(0);
+      comin_abs(1) = cog(1);
       if( comin_abs(2) > 0.03 ) comin_abs(2) = 0.03;
       if( comin_abs(2) < -0.15 ) comin_abs(2) = -0.15;
       return true;
@@ -519,93 +538,134 @@ class HumanSynchronizer{
 //      if(comacc_ref(1) < minacc[1]){com_vel(1) = minacc[1] * DT + com_vel_old(1); cout<<"minacc[1] over"<<endl;}
 //      cout<<"comacc_ref(1) "<<comacc_ref(1)<<" minacc[1] "<<minacc[1]<<endl;
 ////      cout<<"maxacc "<<maxacc[1]<<" minacc "<<minacc[1]<< " acc " << (com_vel(1)-com_vel_old(1))/DT <<endl;
-      hrp::Vector3 bb;
-      hrp::Vector3 cp_ragulated;
+      std::vector<hrp::Vector2> hull;
+      createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull);
+      hrp::Vector2 com_vel_ans_2d;
+      regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);
+      regulateCOMVelocityByCapturePointXY( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);//XY独立に計算する手法のほうがエッジで引っかからない・・・
+      com_ans(0) = com_old(0) + com_vel_ans_2d(0) * DT;
+      com_ans(1) = com_old(1) + com_vel_ans_2d(1) * DT;
+      return true;
+    }
+    void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
+      com_vel_ans = com_vel;
+      const double H = rp_wld_initpos.com(2);
+      hrp::Vector2 cp = com_pos + com_vel * sqrt( H / G );
+      hrp::Vector2 cp_ragulated;
+      if(!isPointInHullOpenCV(cp,hull)){
+        calcCrossPointOnHull(com_pos, cp, hull, cp_ragulated);
+        //calcNearestPointOnHull(cp, hull, cp_ragulated);
+        com_vel_ans = (cp_ragulated - com_pos) / sqrt( H / G );
+      }
+    }
+    void regulateCOMVelocityByCapturePointXY(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
+      com_vel_ans = com_vel;
+      const double H = rp_wld_initpos.com(2);
       hrp::Vector4 margin_from_4maxcp;
-      checkMarginToFootSupportRegionCV(com_old, rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, margin_from_4maxcp, bb);
+      calcXYMarginToHull(com_pos, hull, margin_from_4maxcp);
       hrp::Vector3 max_com_vel,min_com_vel;
       max_com_vel(0) = margin_from_4maxcp(0) / sqrt( H / G );
       min_com_vel(0) = margin_from_4maxcp(1) / sqrt( H / G );
       max_com_vel(1) = margin_from_4maxcp(2) / sqrt( H / G );
       min_com_vel(1) = margin_from_4maxcp(3) / sqrt( H / G );
-      if(com_vel(0) > max_com_vel(0)){ com_vel(0) = max_com_vel(0); if(loop%100==0)cout<<"F cp over"<<endl;}//ここブレーキ時のZMP条件に変えたほうがいいかもしれない
-      if(com_vel(0) < min_com_vel(0)){ com_vel(0) = min_com_vel(0); if(loop%100==0)cout<<"B cp over"<<endl;}
-      if(com_vel(1) > max_com_vel(1)){ com_vel(1) = max_com_vel(1); if(loop%100==0)cout<<"L cp over"<<endl;}
-      if(com_vel(1) < min_com_vel(1)){ com_vel(1) = min_com_vel(1); if(loop%100==0)cout<<"R cp over"<<endl;}
-      com_ans(0) = com_old(0) + com_vel(0) * DT;
-      com_ans(1) = com_old(1) + com_vel(1) * DT;
-      return true;
+      if(com_vel(0) > max_com_vel(0)){ com_vel_ans(0) = max_com_vel(0); if(loop%100==0)cout<<"F cp over"<<endl;}
+      if(com_vel(0) < min_com_vel(0)){ com_vel_ans(0) = min_com_vel(0); if(loop%100==0)cout<<"B cp over"<<endl;}
+      if(com_vel(1) > max_com_vel(1)){ com_vel_ans(1) = max_com_vel(1); if(loop%100==0)cout<<"L cp over"<<endl;}
+      if(com_vel(1) < min_com_vel(1)){ com_vel_ans(1) = min_com_vel(1); if(loop%100==0)cout<<"R cp over"<<endl;}
     }
-    bool checkMarginToFootSupportRegionCV(const hrp::Vector3& pt_in, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, hrp::Vector4& mgn_ans, hrp::Vector3& pt_ans){
-      std::vector<cv::Point2f> points,hull;
+    void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
+      std::vector<hrp::Vector2> points;
       for(int i=0;i<2;i++){
         for(int j=2;j<4;j++){
-          points.push_back(cv::Point2f(rfin_abs(0) + rf_mgn(i),    rfin_abs(1) + rf_mgn(j)));
-          points.push_back(cv::Point2f(lfin_abs(0) + lf_mgn(i),    lfin_abs(1) + lf_mgn(j)));
+          points.push_back(hrp::Vector2(rfin_abs(0) + rf_mgn(i),    rfin_abs(1) + rf_mgn(j)));
+          points.push_back(hrp::Vector2(lfin_abs(0) + lf_mgn(i),    lfin_abs(1) + lf_mgn(j)));
         }
       }
-      cv::convexHull(cv::Mat(points),hull,true);
-      hrp::Vector2 check_point(pt_in(0),pt_in(1));
-      hrp::Vector2 ans_point = check_point;
-      if(cv::pointPolygonTest(cv::Mat(hull), cv::Point2f(pt_in(0),pt_in(1)), false) < 0){//hullの外にいる場合
-        bool pt_can_be_set_to_edge = false;
-        for(int i=0;i<hull.size();i++){//対象の点がある線分への垂線を有するかチェックする
-          int next_i = (i!=hull.size()-1 ? i+1 : 0);
-          hrp::Vector2 cur_vert(hull[i].x,hull[i].y);
-          hrp::Vector2 next_vert(hull[next_i].x,hull[next_i].y);
-          hrp::Vector2 edge_v = next_vert - cur_vert;
-          hrp::Vector2 tgt_pt_v = check_point - cur_vert;
-          //ある線分への垂線を有し，かつ外側(時計回りエッジに対して左側)に存在するなら，対象の点からそのエッジへの垂線の交点が最近傍点
-          if(edge_v.dot(tgt_pt_v)/edge_v.norm() > 0 && edge_v.dot(tgt_pt_v)/edge_v.norm() < edge_v.norm() && (edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
-            ans_point = cur_vert + edge_v.normalized() * (edge_v.dot(tgt_pt_v)/edge_v.norm());
-            pt_can_be_set_to_edge = true;
-            break;
-          }
-        }
-        if(!pt_can_be_set_to_edge){//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
-          double cur_min_dis = (check_point - hrp::Vector2(hull[0].x,hull[0].y)).norm();
-          int ans_pid = 0;
-          for(int i=1;i<hull.size();i++){
-            if((check_point - hrp::Vector2(hull[i].x,hull[i].y)).norm() < cur_min_dis){
-              cur_min_dis = (check_point - hrp::Vector2(hull[i].x,hull[i].y)).norm();
-              ans_pid = i;
-            }
+      makeConvexHullOpenCV(points, hull_ans);
+    }
+    void calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans){
+      hrp::Vector4 margin_abs;
+      hrp::Vector2 cross_pt, anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
+      for(int direc=0;direc<4;direc++){
+        calcCrossPointOnHull(check_point, check_point+anchor_vec[direc], hull, cross_pt);
+        margin_abs(direc) = (cross_pt - check_point).norm();
+      }
+      margin_ans(0) =  margin_abs(0);
+      margin_ans(1) = -margin_abs(1);
+      margin_ans(2) =  margin_abs(2);
+      margin_ans(3) = -margin_abs(3);
+    }
+    bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_will_cross){
+      hrp::Vector2 anchor_vec = pt_out_goal - pt_in_start;
+      for(int i=0;i<hull.size();i++){
+        int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
+        hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
+        hrp::Vector2 cur_edge = nxt_pt - cur_pt;
+        double dBunbo = hrpVector2Cross(anchor_vec,cur_edge);
+        if( dBunbo != 0.0) {//平行の場合を外す
+          hrp::Vector2 vectorAC = hrp::Vector2(hull[i](0),hull[i](1)) - pt_in_start;
+          double dR = hrpVector2Cross(vectorAC,cur_edge) / dBunbo;
+          double dS = hrpVector2Cross(vectorAC,anchor_vec) / dBunbo;
+          if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){//dRには数値誤差が乗る
+            pt_will_cross = pt_in_start + dR * anchor_vec;
+            //dist = dR * anchor_vec.norm();
+            return true;
           }
-          ans_point(0) = hull[ans_pid].x;
-          ans_point(1) = hull[ans_pid].y;
         }
       }
-      hrp::Vector2 anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
-      hrp::Vector2 cross_pt;
-      double margin[4] = {0};
-      for(int direc=0;direc<4;direc++){
-        for(int i=0;i<hull.size();i++){
-          int next_i = (i!=hull.size()-1 ? i+1 : 0);
-          double dBunbo = anchor_vec[direc](0) * ( hull[next_i].y - hull[i].y ) - anchor_vec[direc](1) * ( hull[next_i].x - hull[i].x );
-          if( dBunbo != 0.0)  {//平行の場合を外す
-            hrp::Vector2 vectorAC = hrp::Vector2(hull[i].x,hull[i].y) - ans_point;
-            double dR = ( ( hull[next_i].y - hull[i].y ) * vectorAC(0) - ( hull[next_i].x - hull[i].x ) * vectorAC(1) ) / dBunbo;
-            double dS = ( anchor_vec[direc](1) * vectorAC(0) - anchor_vec[direc](0) * vectorAC(1) ) / dBunbo;
-            //dR==0のときは含めないことで，点が境界上でも領域内側向きのマージンをチェックさせる
-            //dRの上限が無いのはアンカーベクトルの無限遠まで交点を許可するから(アンカーベクトルの長さは1で定義しているため)
-            //点が斜線境界上にあるとまれに dR = 8.67362e-18 とかいう0でない値がでたりする．
-            //恐らく点を斜線境界で打ち切った時に数値誤差で完全に斜線に乗り切らず，数値誤差分はみ出ている状況と思われる．
-            if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){
-              cross_pt = ans_point + dR * anchor_vec[direc];
-              margin[direc] = dR * anchor_vec[direc].norm();
-            }
-          }
+      return false;
+    }
+    double calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_ans){
+      double cur_nearest_dist, ans_nearest_dist;
+      hrp::Vector2 cur_nearest_pt, ans_nearest_pt;
+      for(int i=0;i<hull.size();i++){
+        int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
+        hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
+        hrp::Vector2 cur_edge = nxt_pt - cur_pt;
+        hrp::Vector2 tgt_pt_v = tgt_pt - cur_pt;
+        double tgt_pt_projected_length = tgt_pt_v.dot(cur_edge.normalized());
+        if(tgt_pt_projected_length > cur_edge.norm() ){//tgt_pt's nearest point is on the　i+1-th vertex
+          cur_nearest_pt = nxt_pt;
+        }else if(tgt_pt_projected_length < 0 ){//tgt_pt's nearest point is on the　i-th vertex
+          cur_nearest_pt = cur_pt;
+        }else{//tgt_pt's nearest point is on the line
+          cur_nearest_pt = cur_pt + tgt_pt_projected_length * cur_edge.normalized();
+        }
+        cur_nearest_dist = (tgt_pt - cur_nearest_pt).norm();
+        if(i==0){//set first candidate as nearest
+          ans_nearest_dist = cur_nearest_dist;
+          ans_nearest_pt = cur_nearest_pt;
+        }else if( cur_nearest_dist < ans_nearest_dist ){//update nearest candidate
+          ans_nearest_dist = cur_nearest_dist;
+          ans_nearest_pt = cur_nearest_pt;
         }
       }
-      mgn_ans(0) = margin[0];
-      mgn_ans(1) = -margin[1];
-      mgn_ans(2) = margin[2];
-      mgn_ans(3) = -margin[3];
-      pt_ans(0) = ans_point(0);//
-      pt_ans(1) = ans_point(1);//
-      return true;
+      pt_ans = ans_nearest_pt;
+      return ans_nearest_dist;
+    }
+    void makeConvexHullOpenCV(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
+      std::vector<cv::Point2f> points,hull;
+      for(int i=0;i<pts.size();i++){ points.push_back( cv::Point2f( pts[i](0), pts[i](1)) ); }
+      cv::convexHull(cv::Mat(points),hull,true);
+      hull_ans.clear();
+      for(int i=0;i<hull.size();i++){ hull_ans.push_back( hrp::Vector2( hull[i].x, hull[i].y ) );  }
+    }
+    void makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
+      const int dim = 2;
+      double points[dim*pts.size()];
+      for(int i=0;i<pts.size();i++){ points[dim*i] = pts[i](0); points[dim*i+1] = pts[i](1); }
+      qh_new_qhull( dim, pts.size(), points, false, "qhull ", NULL, stderr);
+      pointT *point, *pointtemp;
+      hull_ans.clear();
+      FORALLpoints hull_ans.push_back( hrp::Vector2(point[0],point[1]) );
+      qh_freeqhull(!qh_ALL);
+      //#define qh_ORIENTclock 1
+    }
+    bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull){
+      std::vector<cv::Point2f> cvhull;
+      for(int i=0;i<hull.size();i++){ cvhull.push_back( cv::Point2f( hull[i](0), hull[i](1)) ); }
+      return (cv::pointPolygonTest(cv::Mat(cvhull), cv::Point2f(pt(0),pt(1)), false) > 0);
     }
-
     void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
       comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
       const double MAXACC = 5;
@@ -630,125 +690,6 @@ class HumanSynchronizer{
         if(!use_y){tgt.com(1) = 0;tgt.zmp(1) = 0;}
         if(!use_z){tgt.com(2) = 0;}
     }
-    //unuse
-    /*
-    bool checkMarginToFootSupportRegion(const hrp::Vector3& pt_in, const hrp::Vector3& pt_old, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const double& forward_mgn, const double& back_mgn, const double& outer_mgn, const double& inner_mgn, margin_t& mgn_ans, hrp::Vector3& pt_ans){
-      std::vector<hrp::Vector2> convex_hull;
-      if(forward_mgn< -back_mgn || outer_mgn< -inner_mgn ){std::cerr << "Invalid margin input!" << std::endl;return false;}
-      if(lfin_abs(0)>rfin_abs(0)){
-        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) + outer_mgn));//LFの左下
-        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + forward_mgn, lfin_abs(1) + outer_mgn));//LFの左上
-        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + forward_mgn, lfin_abs(1) - inner_mgn));//LFの右上
-        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + forward_mgn, rfin_abs(1) - outer_mgn));//RFの右上
-        convex_hull.push_back(hrp::Vector2(rfin_abs(0) - back_mgn,    rfin_abs(1) - outer_mgn));//RFの右下
-        convex_hull.push_back(hrp::Vector2(rfin_abs(0) - back_mgn,    rfin_abs(1) + inner_mgn));//RFの左下
-        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) + outer_mgn));//LFの左下
-      }else{
-        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) - inner_mgn));//LFの右下
-        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) + outer_mgn));//LFの左下
-        convex_hull.push_back(hrp::Vector2(lfin_abs(0) + forward_mgn, lfin_abs(1) + outer_mgn));//LFの左上
-        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + forward_mgn, rfin_abs(1) + inner_mgn));//RFの左上
-        convex_hull.push_back(hrp::Vector2(rfin_abs(0) + forward_mgn, rfin_abs(1) - outer_mgn));//RFの右上
-        convex_hull.push_back(hrp::Vector2(rfin_abs(0) - back_mgn,    rfin_abs(1) - outer_mgn));//RFの右下
-        convex_hull.push_back(hrp::Vector2(lfin_abs(0) - back_mgn,    lfin_abs(1) - inner_mgn));//LFの右下
-      }
-      hrp::Vector2 check_point(pt_in(0),pt_in(1));
-      hrp::Vector2 ans_point = check_point;
-      enum point_state_t {PT_IN_NO_PATTERN = -1, PT_IN_REGION, PT_FIX_TO_EDGE, PT_FIX_TO_VERTEX};
-      point_state_t pt_state = PT_IN_REGION;
-      for(int i=0;i<convex_hull.size()-1;i++){//対象の点が凸包の内部に存在するかチェックする
-        hrp::Vector2 cur_vert(convex_hull[i](0),convex_hull[i](1));
-        hrp::Vector2 next_vert(convex_hull[i+1](0),convex_hull[i+1](1));
-        hrp::Vector2 edge_v = next_vert - cur_vert;
-        hrp::Vector2 tgt_pt_v = check_point - cur_vert;
-        if((edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){  pt_state = PT_IN_NO_PATTERN; break; }
-      }
-      if(pt_state != PT_IN_REGION){
-        int ans_lid=-1;
-        for(int i=0;i<convex_hull.size()-1;i++){//対象の点がある線分への垂線を有するかチェックする
-          hrp::Vector2 cur_vert(convex_hull[i](0),convex_hull[i](1));
-          hrp::Vector2 next_vert(convex_hull[i+1](0),convex_hull[i+1](1));
-          hrp::Vector2 edge_v = next_vert - cur_vert;
-          hrp::Vector2 tgt_pt_v = check_point - cur_vert;
-          //ある線分への垂線を有し，かつ外側(時計回りエッジに対して左側)に存在するなら，対象の点からそのエッジへの垂線の交点が最近傍点
-          if(edge_v.dot(tgt_pt_v)/edge_v.norm() > 0 && edge_v.dot(tgt_pt_v)/edge_v.norm() < edge_v.norm() && (edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
-            ans_lid = i;
-            ans_point = cur_vert + edge_v.normalized() * (edge_v.dot(tgt_pt_v)/edge_v.norm());
-            pt_state = PT_FIX_TO_EDGE;
-            break;
-          }
-        }
-        if(pt_state != PT_FIX_TO_EDGE){//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
-          double cur_min_dis = (check_point - convex_hull[0]).norm();
-          int ans_pid = 0;
-          for(int i=1;i<convex_hull.size();i++){
-            if((check_point - convex_hull[i]).norm() < cur_min_dis){
-              cur_min_dis = (check_point - convex_hull[i]).norm();
-              ans_pid = i;
-            }
-          }
-          ans_point(0) = convex_hull[ans_pid](0);
-          ans_point(1) = convex_hull[ans_pid](1);
-          pt_state = PT_FIX_TO_VERTEX;
-        }
-      }
-      pt_ans(0) = ans_point(0);//領域はみ出ている場合この答えは点から領域への最近傍点(だからちょっと意味が違う)
-      pt_ans(1) = ans_point(1);//
-
-      hrp::Vector2 anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
-      hrp::Vector2 cross_pt;
-      double margin[4] = {0};
-      for(int direc=0;direc<4;direc++){
-        for(int i=0;i<convex_hull.size()-1;i++){
-          double dBunbo = anchor_vec[direc](0) * ( convex_hull[i+1](1) - convex_hull[i](1) ) - anchor_vec[direc](1) * ( convex_hull[i+1](0) - convex_hull[i](0) );
-          if( dBunbo != 0.0)  {//平行の場合を外す
-            hrp::Vector2 vectorAC = convex_hull[i] - ans_point;
-            double dR = ( ( convex_hull[i+1](1) - convex_hull[i](1) ) * vectorAC(0) - ( convex_hull[i+1](0) - convex_hull[i](0) ) * vectorAC(1) ) / dBunbo;
-            double dS = ( anchor_vec[direc](1) * vectorAC(0) - anchor_vec[direc](0) * vectorAC(1) ) / dBunbo;
-            //dR==0のときは含めないことで，点が境界上でも領域内側向きのマージンをチェックさせる
-            //dRの上限が無いのはアンカーベクトルの無限遠まで交点を許可するから(アンカーベクトルの長さは1で定義しているため)
-            //点が斜線境界上にあるとまれに dR = 8.67362e-18 とかいう0でない値がでたりする．
-            //恐らく点を斜線境界で打ち切った時に数値誤差で完全に斜線に乗り切らず，数値誤差分はみ出ている状況と思われる．
-            if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){
-              cross_pt = ans_point + dR * anchor_vec[direc];
-              margin[direc] = dR * anchor_vec[direc].norm();
-            }
-          }
-        }
-      }
-      mgn_ans.x_u = margin[0];
-      mgn_ans.x_l = -margin[1];
-      mgn_ans.y_u = margin[2];
-      mgn_ans.y_l = -margin[3];
-
-//      //X,Y独立のマージンチェックではなくXY平面内の二次元距離でマージンを計算したい
-//      hrp::Vector2 cmd_vec,pt_in_2d,pt_old_2d;
-//      pt_in_2d(0) = pt_in(0);      pt_in_2d(1) = pt_in(1);
-//      pt_old_2d(0) = pt_old(0);    pt_old_2d(1) = pt_old(1);
-//      cmd_vec = pt_in_2d - pt_old_2d;
-//      hrp::Vector2 cross_pt;
-//      double margin[4] = {0};
-//      for(int i=0;i<convex_hull.size()-1;i++){
-//        double dBunbo = cmd_vec(0) * ( convex_hull[i+1](1) - convex_hull[i](1) ) - cmd_vec(1) * ( convex_hull[i+1](0) - convex_hull[i](0) );
-//        if( dBunbo != 0.0)  {//平行の場合を外す
-//          hrp::Vector2 vectorAC = convex_hull[i] - pt_old_2d;
-//          double dR = ( ( convex_hull[i+1](1) - convex_hull[i](1) ) * vectorAC(0) - ( convex_hull[i+1](0) - convex_hull[i](0) ) * vectorAC(1) ) / dBunbo;
-//          double dS = ( cmd_vec(1) * vectorAC(0) - cmd_vec(0) * vectorAC(1) ) / dBunbo;
-//          //dR==0のときは含めないことで，点が境界上でも領域内側向きのマージンをチェックさせる
-//          //dRの上限が無いのはアンカーベクトルの無限遠まで交点を許可するから(アンカーベクトルの長さは1で定義しているため)
-//          //点が斜線境界上にあるとまれに dR = 8.67362e-18 とかいう0でない値がでたりする．
-//          //恐らく点を斜線境界で打ち切った時に数値誤差で完全に斜線に乗り切らず，数値誤差分はみ出ている状況と思われる．
-//          if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){
-//            cross_pt = pt_old_2d + dR * cmd_vec;
-//          }
-//        }
-//      }
-//      pt_ans(0) = cross_pt(0);
-//      pt_ans(1) = cross_pt(1);
-
-      return true;
-    }
-    */
 };
 
 

From 14c4cf9fe3d13727ddbd4bbc15d3ce3f4b70b144 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 11 Jan 2017 13:22:58 +0900
Subject: [PATCH 044/217] apply variable foot lifting speed

---
 rtc/AutoBalancer/AutoBalancer.h | 41 ++++++++++++++++++++++++---------
 1 file changed, 30 insertions(+), 11 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index b583df13a07..3b5e9816ce6 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -167,6 +167,7 @@ class HumanSynchronizer{
   private:
     double FUP_TIME;
     int cur_rfup_level,cur_lfup_level;
+    double cur_rfup_rad,cur_lfup_rad;
     double FUP_HIGHT;
     double CNT_F_TH;
     hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
@@ -223,11 +224,12 @@ class HumanSynchronizer{
       tgt_cmmr = cmmr = 1.0;
       use_x = use_y = use_z = true;
       cur_rfup_level = cur_lfup_level = 0;
+      cur_rfup_rad = cur_lfup_rad = 0;
       is_rf_contact = is_lf_contact = true;
       go_rf_landing = go_lf_landing = false;
       pre_cont_rfpos = pre_cont_lfpos = hrp::Vector3::Zero();
       init_hp_calibcom = hrp::Vector3::Zero();
-      tgt_FUP_TIME = FUP_TIME = 0.8;
+      tgt_FUP_TIME = FUP_TIME = 0.4;
       FUP_HIGHT = 0.05;
       CNT_F_TH = 20.0;
       MAXVEL = 0.004;
@@ -446,6 +448,13 @@ class HumanSynchronizer{
         }
       }
     }
+    void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
+      if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足接地しているタイミングでのみ更新
+      double RFDOWN_TIME = fabs(tgt.rf(1) - tgt.com(1))*4 + FUP_TIME;//下ろす速度は遊脚の重心からの距離に比例したペナルティ
+      double LFDOWN_TIME = fabs(tgt.lf(1) - tgt.com(1))*4 + FUP_TIME;
+      calcFootUpCurveAndJudgeFootContact2(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.rf, is_rf_contact);
+      calcFootUpCurveAndJudgeFootContact2(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.lf, is_lf_contact);
+    }
     void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, int& cur_fup_count_in, hrp::Vector3& f_height_out, bool& is_f_contact_out){
       int FUP_COUNT = (int)(HZ*FUP_TIME);
       if(cur_fup_count_in > FUP_COUNT){cur_fup_count_in = FUP_COUNT;} if(cur_fup_count_in < 0){cur_fup_count_in = 0;}
@@ -457,10 +466,18 @@ class HumanSynchronizer{
       f_height_out(2) = FUP_HIGHT/2*(1-cos(M_PI*cur_fup_count_in/FUP_COUNT));
       is_f_contact_out = (cur_fup_count_in <= 0);
     }
-    void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
-      if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足接地しているタイミングでのみ更新
-      calcFootUpCurveAndJudgeFootContact(rf_goland_in, cur_rfup_level, tgt.rf, is_rf_contact);
-      calcFootUpCurveAndJudgeFootContact(lf_goland_in, cur_lfup_level, tgt.lf, is_lf_contact);
+    void calcFootUpCurveAndJudgeFootContact2(const bool& go_land_in, const double& fup_time_in, const double& fdown_time_in,  double& cur_fup_phase_in,hrp::Vector3& f_height_out, bool& is_f_contact_out){
+      const double omega_up = M_PI / fup_time_in;
+      const double omega_down = M_PI / fdown_time_in;
+      if(!go_land_in){//足上げ命令入力時
+        cur_fup_phase_in += omega_up * DT;
+      }else{//足下げ命令入力時
+        cur_fup_phase_in -= omega_down * DT;
+      }
+      if(cur_fup_phase_in > M_PI){cur_fup_phase_in = M_PI;}
+      else if(cur_fup_phase_in < 0){cur_fup_phase_in = 0;}
+      f_height_out(2) = FUP_HIGHT/2*(1-cos(cur_fup_phase_in));
+      is_f_contact_out = (cur_fup_phase_in <= 0);
     }
     void lockFootXYOnContact(const bool& rfcs_in, const bool& lfcs_in, HumanPose& tgt){
       if(rfcs_in){
@@ -486,6 +503,7 @@ class HumanSynchronizer{
           tgt.rf(0) = (lf2rf_vec.normalized())(0) * MAX + pre_cont_lfpos(0);
           tgt.rf(1) = (lf2rf_vec.normalized())(1) * MAX + pre_cont_lfpos(1);
         }
+        if(tgt.rf(1) > pre_cont_lfpos(1) + 0.05 ) tgt.rf(1) = pre_cont_lfpos(1) + 0.05;
       }
       else if(is_rf_contact && !is_lf_contact){//左足浮遊時
         const hrp::Vector2 rf2lf_vec( tgt.lf(0)-pre_cont_rfpos(0), tgt.lf(1)-pre_cont_rfpos(1) );
@@ -493,6 +511,7 @@ class HumanSynchronizer{
           tgt.lf(0) = (rf2lf_vec.normalized())(0) * MAX + pre_cont_rfpos(0);
           tgt.lf(1) = (rf2lf_vec.normalized())(1) * MAX + pre_cont_rfpos(1);
         }
+        if(tgt.lf(1) < pre_cont_rfpos(1) - 0.05 ) tgt.lf(1) = pre_cont_rfpos(1) - 0.05;
       }
       hrp::Vector3 hand_ulimit = hrp::Vector3(0.1,0.1,0.1);
       hrp::Vector3 hand_llimit = hrp::Vector3(-0.1,-0.1,-0.1);
@@ -523,11 +542,11 @@ class HumanSynchronizer{
       double decel[4] = {0.04, -0.02, 0.01, -0.01};
       comacc_ref = calcacc_v_filters.passFilter(com_acc);
       const double H = rp_wld_initpos.com(2);
-      double maxacc[2],minacc[2];
-      maxacc[0] = (com_old(0) - accel[1]) * (G/H); if(maxacc[0] < 0){maxacc[0]=0;}
-      minacc[0] = (com_old(0) - accel[0]) * (G/H); if(minacc[0] > 0){minacc[0]=0;}
-      maxacc[1] = (com_old(1) - (accel[3]-0.1)) * (G/H); if(maxacc[1] < 0){maxacc[1]=0;}
-      minacc[1] = (com_old(1) - (accel[2]+0.1)) * (G/H); if(minacc[1] > 0){minacc[1]=0;}
+//      double maxacc[2],minacc[2];
+//      maxacc[0] = (com_old(0) - accel[1]) * (G/H); if(maxacc[0] < 0){maxacc[0]=0;}
+//      minacc[0] = (com_old(0) - accel[0]) * (G/H); if(minacc[0] > 0){minacc[0]=0;}
+//      maxacc[1] = (com_old(1) - (accel[3]-0.1)) * (G/H); if(maxacc[1] < 0){maxacc[1]=0;}
+//      minacc[1] = (com_old(1) - (accel[2]+0.1)) * (G/H); if(minacc[1] > 0){minacc[1]=0;}
 //      if(com_vel(0) > maxacc[0] * DT + com_vel_old(0)){com_vel(0) = maxacc[0] * DT + com_vel_old(0); cout<<"maxacc[0] over"<<endl;}
 //      if(com_vel(0) < minacc[0] * DT + com_vel_old(0)){com_vel(0) = minacc[0] * DT + com_vel_old(0); cout<<"minacc[0] over"<<endl;}
 //      if(com_vel(1) > maxacc[1] * DT + com_vel_old(1)){com_vel(1) = maxacc[1] * DT + com_vel_old(1); cout<<"maxacc[1] over"<<endl;}
@@ -541,7 +560,7 @@ class HumanSynchronizer{
       std::vector<hrp::Vector2> hull;
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull);
       hrp::Vector2 com_vel_ans_2d;
-      regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);
+//      regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);
       regulateCOMVelocityByCapturePointXY( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);//XY独立に計算する手法のほうがエッジで引っかからない・・・
       com_ans(0) = com_old(0) + com_vel_ans_2d(0) * DT;
       com_ans(1) = com_old(1) + com_vel_ans_2d(1) * DT;

From d26ad8c3e8f12b56c3870b29bb9f161ae2d3daa2 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 11 Jan 2017 17:04:53 +0900
Subject: [PATCH 045/217] separate source code of HumanMasterSlave

---
 rtc/AutoBalancer/AutoBalancer.cpp     |  90 ++--
 rtc/AutoBalancer/AutoBalancer.h       | 683 +-------------------------
 rtc/AutoBalancer/CMakeLists.txt       |   2 +-
 rtc/AutoBalancer/HumanMasterSlave.cpp | 137 ++++++
 rtc/AutoBalancer/HumanMasterSlave.h   | 556 +++++++++++++++++++++
 5 files changed, 727 insertions(+), 741 deletions(-)
 create mode 100644 rtc/AutoBalancer/HumanMasterSlave.cpp
 create mode 100644 rtc/AutoBalancer/HumanMasterSlave.h

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 706971f62c4..289452f6cb2 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -501,31 +501,6 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
     hsp->readInput(hp_raw_data);
     hsp->current_basepos = m_robot->rootLink()->p;
-
-//    if(hsp->startCountdownForHumanSync){
-//      std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
-//      hsp->updateCountDown();
-//    }
-//    if(hsp->isHumanSyncOn()){
-//      if(hsp->ht_first_call){
-//        std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
-//        hsp->ht_first_call = false;
-//      }
-//    }else{
-//      hsp->calibInitHumanCOMFromZMP();
-//      hsp->setInitOffsetPose();
-//      hsp->rp_wld_initpos.com = m_robot->calcCM();
-//      hsp->rp_wld_initpos.rf = ikp["rleg"].target_link->p;
-//      hsp->rp_wld_initpos.lf = ikp["lleg"].target_link->p;
-//      if(ikp.count("rarm"))hsp->rp_wld_initpos.rh = ikp["rarm"].target_link->p;
-//      if(ikp.count("larm"))hsp->rp_wld_initpos.lh = ikp["larm"].target_link->p;
-//    }
-//    hsp->update();//////HumanSynchronizerの主要処理
-//    if(loop%100==0)hsp->rp_ref_out.print();
-  //  if(loop%100==0)cerr<<"getUpdateTime:"<<hsp->getUpdateTime()*1000<<"[ms]"<<endl;
-
-
-
     if(ikp.count("rarm") && !hsp->use_rh)ikp["rarm"].is_active = false;
     if(ikp.count("larm") && !hsp->use_lh)ikp["larm"].is_active = false;
     m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
@@ -1057,7 +1032,30 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param, const std::string& lim
   }
   return true;
 }
-
+void AutoBalancer::solveWholeBodyCOMIK(const hrp::Vector3& com_ref, const hrp::Vector3& rf_ref, const hrp::Vector3& lf_ref, const hrp::Vector3& rh_ref, const hrp::Vector3& lh_ref, const hrp::Vector3& head_ref){
+  int com_ik_loop=0;
+  const int COM_IK_MAX_LOOP = 5;
+  const double COM_IK_MAX_ERROR = 1e-4;
+  m_robot->rootLink()->p = com_ref;//move base link at first
+  m_robot->calcForwardKinematics();//apply
+  hrp::Vector3 tmp_com_err = com_ref - m_robot->calcCM();
+  ikp["rleg"].target_p0 = rf_ref;
+  ikp["lleg"].target_p0 = lf_ref;
+  if(ikp.count("rarm"))ikp["rarm"].target_p0 = rh_ref;
+  if(ikp.count("larm"))ikp["larm"].target_p0 = lh_ref;
+  while(tmp_com_err.norm() > COM_IK_MAX_ERROR){
+    m_robot->rootLink()->p += tmp_com_err;
+    m_robot->calcForwardKinematics();
+    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+      if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
+    }
+    m_robot->calcForwardKinematics();
+    tmp_com_err = com_ref - m_robot->calcCM();
+    if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
+  }
+  if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref(2);
+  if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref(1);
+}
 
 
 void AutoBalancer::solveLimbIK ()
@@ -1093,7 +1091,6 @@ void AutoBalancer::solveLimbIK ()
 ////////////////////  逆動力学補償テスト  /////////////////////
 
 
-
   //for HumanSynchronizer
   if(hsp->startCountdownForHumanSync){
     std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
@@ -1117,10 +1114,6 @@ void AutoBalancer::solveLimbIK ()
   }
   hsp->update();//////HumanSynchronizerの主要処理
   if(loop%100==0)hsp->rp_ref_out.print();
-  //for HumanSynchronizer
-  if(hsp->isHumanSyncOn()){	  ////////////////////// 重心拘束位置設定 /////////////////////////
-    m_robot->rootLink()->p = hsp->rp_wld_initpos.com + hsp->rp_ref_out.com;//まずは重心目標分，体幹を動かす
-  }
 
   // Fix for toe joint
   for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
@@ -1136,39 +1129,18 @@ void AutoBalancer::solveLimbIK ()
   m_robot->calcForwardKinematics();
 
   // additional COM fitting IK for HumanSynchronizer
-  hrp::Vector3 tmp_com_err = hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM();
-  int com_ik_loop=0;
-  const int COM_IK_MAX_LOOP = 10;
-  const double COM_IK_MAX_ERROR = 0.0001;//0.001だとたまにカクッとなる
   if(hsp->isHumanSyncOn()){
-    ikp["rleg"].target_p0 = hsp->rp_wld_initpos.rf + hsp->rp_ref_out.rf;
-    ikp["lleg"].target_p0 = hsp->rp_wld_initpos.lf + hsp->rp_ref_out.lf;
-    if(ikp.count("rarm"))ikp["rarm"].target_p0 = hsp->rp_wld_initpos.rh + hsp->rp_ref_out.rh;
-    if(ikp.count("larm"))ikp["larm"].target_p0 = hsp->rp_wld_initpos.lh + hsp->rp_ref_out.lh;
-    while(fabs(tmp_com_err(0))>COM_IK_MAX_ERROR || fabs(tmp_com_err(1))>COM_IK_MAX_ERROR || fabs(tmp_com_err(2))>COM_IK_MAX_ERROR){//Z方向のCOM合わせは要注意(たまに怪しい)
-		  m_robot->rootLink()->p += tmp_com_err;
-		  m_robot->calcForwardKinematics();
-		  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-		    if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
-		  }
-		  m_robot->calcForwardKinematics();
-      tmp_com_err = (hsp->rp_ref_out.com + hsp->rp_wld_initpos.com - m_robot->calcCM());
-		  if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "[" << m_profile.instance_name << "] COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
-	  }
-
-
-    m_robot->joint(15)->q = hsp->cam_rpy_filtered(2);
-    m_robot->joint(16)->q = hsp->cam_rpy_filtered(1);
-//    if(loop%50==0)cout<<"cam_rpy_filtered:\n"<<hsp->cam_rpy_filtered<<endl;
-
-
-
+	  solveWholeBodyCOMIK(
+	      hsp->rp_wld_initpos.com + hsp->rp_ref_out.com,
+	      hsp->rp_wld_initpos.rf + hsp->rp_ref_out.rf,
+	      hsp->rp_wld_initpos.lf + hsp->rp_ref_out.lf,
+	      hsp->rp_wld_initpos.rh + hsp->rp_ref_out.rh,
+	      hsp->rp_wld_initpos.lh + hsp->rp_ref_out.lh,
+	      hsp->cam_rpy_filtered );
     //outport用のデータ上書き
     ref_zmp = hsp->rp_ref_out.zmp + hsp->rp_wld_initpos.zmp;
     ref_cog = hsp->rp_ref_out.com + hsp->rp_wld_initpos.com;
 
-	  if(loop%100==0)if(com_ik_loop>1)std::cerr << "[" << m_profile.instance_name << "] COM_IK_LOOP ="<<com_ik_loop<< std::endl;//ややCOMのIKに手間取った時プリント
-
   }else{
 	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
 	    if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 3b5e9816ce6..402f1980b28 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -29,687 +29,7 @@
 #include "interpolator.h"
 #include "../TorqueFilter/IIRFilter.h"
 
-#include <opencv2/core/core.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <opencv2/imgproc/types_c.h>
-#include <hrpUtil/Eigen4d.h>
-
-#if defined(__cplusplus)
-extern "C"
-{
-#endif
-#include <stdio.h>
-#include <stdlib.h>
-#include <qhull/qhull.h>
-#include <qhull/mem.h>
-#include <qhull/qset.h>
-#include <qhull/geom.h>
-#include <qhull/merge.h>
-#include <qhull/poly.h>
-#include <qhull/io.h>
-#include <qhull/stat.h>
-#if defined(__cplusplus)
-}
-#endif
-
-// </rtc-template>
-
-// Service Consumer stub headers
-// <rtc-template block="consumer_stub_h">
-
-//実機だとログ出力で落ちる？getenv("HOME")ダメ？
-#define DEBUG 0
-
-
-class BiquadIIRFilterVec
-{
-private:
-    IIRFilter x_filters,y_filters,z_filters;
-    hrp::Vector3 ans;
-    std::vector<double> fb_coeffs, ff_coeffs;
-public:
-    BiquadIIRFilterVec(const std::string& error_prefix = ""){
-    };
-    ~BiquadIIRFilterVec() {};
-    bool setParameter(const double fc_in, const double HZ){
-      double fc = tan(fc_in * M_PI / HZ) / (2 * M_PI);
-      double denom = 1 + (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc;
-      ff_coeffs.resize(3);
-      fb_coeffs.resize(3);
-      ff_coeffs[0] = (4 * M_PI * M_PI * fc*fc) / denom;
-      ff_coeffs[1] = (8 * M_PI * M_PI * fc*fc) / denom;
-      ff_coeffs[2] = (4 * M_PI * M_PI * fc*fc) / denom;
-      fb_coeffs[0] = 1.0;
-      fb_coeffs[1] = (8 * M_PI * M_PI * fc*fc - 2) / denom;
-      fb_coeffs[2] = (1 - (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc) / denom;
-      return (
-        x_filters.setParameter(2,fb_coeffs,ff_coeffs) &&
-        y_filters.setParameter(2,fb_coeffs,ff_coeffs) &&
-        z_filters.setParameter(2,fb_coeffs,ff_coeffs)
-      );
-    };
-    hrp::Vector3 passFilter(const hrp::Vector3& input){
-      ans(0) = x_filters.passFilter(input(0));
-      ans(1) = y_filters.passFilter(input(1));
-      ans(2) = z_filters.passFilter(input(2));
-      return ans;
-    };
-};
-
-class margin_t{
-  public:
-    hrp::Vector3 pt;
-    double x_u, x_l, y_u, y_l;
-    margin_t(){pt = hrp::Vector3::Zero(); x_u = x_l = y_u = y_l = 0;}
-    bool isRagulated(){return (x_u == 0 || x_l == 0 || y_u == 0 || y_l == 0);}
-};
-
-
-
-class HumanPose{
-  private: hrp::Vector3 zero;
-  public:
-    hrp::Vector3 com,rf,lf,rh,lh;
-    hrp::Vector3 zmp;
-    int idsize;
-    typedef struct{ hrp::Vector3 f; hrp::Vector3 t; }Wrench6;
-    Wrench6 rfw,lfw;
-    hrp::Vector3 neck;
-    HumanPose() : idsize(10){
-      clear();
-    }
-    ~HumanPose(){cerr<<"HumanPose destructed"<<endl;}
-    void clear(){
-      for(int i=0;i<idsize;i++){ Seq(i) = hrp::Vector3::Zero(); }
-    }
-    hrp::Vector3& Seq(const int i){
-      switch(i){
-        case 0: return com;
-        case 1: return rf;
-        case 2: return lf;
-        case 3: return rh;
-        case 4: return lh;
-        case 5: return zmp;
-        case 6: return rfw.f;
-        case 7: return rfw.t;
-        case 8: return lfw.f;
-        case 9: return lfw.t;
-        default: std::cerr <<"[WARN] HumanPose::getSeq(int) invalid index"<<std::endl;return zero;
-      }
-    }
-    const hrp::Vector3& Seq(const int i) const{//const指定オブジェクトからオーバーロードする時のためのオーバーライド
-      switch(i){
-        case 0: return com;
-        case 1: return rf;
-        case 2: return lf;
-        case 3: return rh;
-        case 4: return lh;
-        case 5: return zmp;
-        case 6: return rfw.f;
-        case 7: return rfw.t;
-        case 8: return lfw.f;
-        case 9: return lfw.t;
-        default: std::cerr <<"[WARN] HumanPose::getSeq(int) invalid index"<<std::endl;return zero;
-      }
-    }
-    static void hp_printf(HumanPose& in){
-      const std::string cap[] = {"c","rf","lf","rh","lh","z","rw","","lw",""};
-      for(int i=0;i<6;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.Seq(i)(0),in.Seq(i)(1),in.Seq(i)(2)); }
-      for(int i=6;i<10;i++){ printf("\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.Seq(i)(0),in.Seq(i)(1),in.Seq(i)(2)); }
-      printf("\n");
-    }
-    void print(){ hp_printf(*this); }
-};
-
-
-
-class HumanSynchronizer{
-  private:
-    double FUP_TIME;
-    int cur_rfup_level,cur_lfup_level;
-    double cur_rfup_rad,cur_lfup_rad;
-    double FUP_HIGHT;
-    double CNT_F_TH;
-    hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
-    hrp::Vector3 init_hp_calibcom;
-    double MAXVEL,MAXACC;
-    HumanPose hp_wld_raw;
-    HumanPose hp_wld_initpos;
-    HumanPose hp_rel_raw;
-    HumanPose rp_ref_out_old;
-    int countdown_num;
-    bool HumanSyncOn;
-    struct timeval t_calc_start, t_calc_end;
-    double h2r_ratio, tgt_h2r_ratio;
-    double cmmr, tgt_cmmr;
-    double HZ,DT,G;
-    hrp::Vector3 com_old,com_oldold,comacc;
-    hrp::Vector3 comacc_ref;
-    std::vector<BiquadIIRFilterVec> iir_v_filters;
-    BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters;
-    BiquadIIRFilterVec cam_rpy_filter;
-    hrp::Vector3 r_zmp_raw;
-    hrp::Vector4 rf_safe_region,lf_safe_region;//前後左右の順
-
-  public:
-    double tgt_FUP_TIME;
-    bool startCountdownForHumanSync;
-    bool ht_first_call;
-    bool is_rf_contact,is_lf_contact;//足上げ高さ0に到達しているか否か
-    bool go_rf_landing,go_lf_landing;//足上げ指令の有無
-    HumanPose rp_wld_initpos;//IKでのリンク原点の位置(足首，手首など)
-    bool use_x,use_y,use_z;
-    HumanPose rp_ref_out;
-    FILE *sr_log,*cz_log;
-    bool use_rh,use_lh;
-    hrp::Vector3 init_wld_rp_rfeepos,init_wld_rp_lfeepos;
-    hrp::Vector3 init_wld_hp_rfpos,init_wld_hp_lfpos;
-    hrp::Vector3 current_basepos,init_basepos;
-    hrp::Vector3 com_vel_old;
-    unsigned int loop;
-    hrp::Vector3 cam_pos,cam_rpy,cam_pos_filtered,cam_rpy_filtered;
-
-
-    interpolator *init_zmp_com_offset_interpolator;
-    double zmp_com_offset_ip_ratio;
-
-    HumanSynchronizer(){
-      tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
-//      tgt_h2r_ratio = h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
-//      tgt_h2r_ratio = h2r_ratio = 0.69;//human 1.0(with heavy foot sensor) vs chidori 0.69
-//      tgt_h2r_ratio = h2r_ratio = 1.06;//human 1.0(with heavy foot sensor) vs jaxon 1.06
-      HZ = 500;
-      DT = 1.0/HZ;
-      G = 9.80665;
-      tgt_cmmr = cmmr = 1.0;
-      use_x = use_y = use_z = true;
-      cur_rfup_level = cur_lfup_level = 0;
-      cur_rfup_rad = cur_lfup_rad = 0;
-      is_rf_contact = is_lf_contact = true;
-      go_rf_landing = go_lf_landing = false;
-      pre_cont_rfpos = pre_cont_lfpos = hrp::Vector3::Zero();
-      init_hp_calibcom = hrp::Vector3::Zero();
-      tgt_FUP_TIME = FUP_TIME = 0.4;
-      FUP_HIGHT = 0.05;
-      CNT_F_TH = 20.0;
-      MAXVEL = 0.004;
-      HumanSyncOn = false;
-      ht_first_call = true;
-      startCountdownForHumanSync = false;
-      countdown_num = 5*HZ;
-      loop = 0;
-
-      //////////  hrp::Vector は初期化必要  ///////////
-      com_old = com_oldold = comacc = hrp::Vector3::Zero();
-      comacc_ref = hrp::Vector3::Zero();
-      r_zmp_raw = hrp::Vector3::Zero();
-
-      init_wld_rp_rfeepos = init_wld_rp_lfeepos = hrp::Vector3::Zero();
-      init_wld_hp_rfpos = init_wld_hp_lfpos = hrp::Vector3::Zero();
-      current_basepos = init_basepos = hrp::Vector3::Zero();
-
-      com_vel_old = hrp::Vector3::Zero();
-      cam_pos = cam_rpy = cam_pos_filtered = cam_rpy_filtered = hrp::Vector3::Zero();
-
-      iir_v_filters.resize(5);
-      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(0.5,HZ);//四肢拘束点用
-      iir_v_filters[0].setParameter(0.4,HZ);//重心用
-      calcacc_v_filters.setParameter(5,HZ);//加速度計算用
-      acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
-      cam_rpy_filter.setParameter(1,HZ);//カメラアングル
-
-      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
-      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
-//      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.0);
-//      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.0, -0.02);
-
-      use_rh = use_lh = false;
-
-      rp_ref_out_old.clear();
-      rp_ref_out.clear();
-
-      init_zmp_com_offset_interpolator = new interpolator(1, DT, interpolator::HOFFARBIB, 1);
-      init_zmp_com_offset_interpolator->setName("zmp_com_offset_interpolator");
-      double start_ratio = 0.0;
-      init_zmp_com_offset_interpolator->set(&start_ratio);
-      double goal_ratio = 1.0;
-      init_zmp_com_offset_interpolator->go(&goal_ratio, 3.0, true);//3s
-
-      if(DEBUG){
-//        std::string home_path(std::getenv("HOME"));
-//        sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
-//        cz_log = fopen((home_path+"/HumanSync_com_zmp.log").c_str(),"w+");
-        sr_log = fopen("/home/ishiguro/HumanSync_support_region.log","w+");
-        cz_log = fopen("/home/ishiguro/HumanSync_com_zmp.log","w+");
-      }
-      cout<<"HumanSynchronizer constructed"<<endl;
-    }
-    ~HumanSynchronizer(){
-      if(DEBUG)fclose(sr_log);
-      if(DEBUG)fclose(cz_log);
-      delete init_zmp_com_offset_interpolator;
-      cout<<"HumanSynchronizer destructed"<<endl;
-    }
-
-    void readInput(const HumanPose& raw_in){ hp_wld_raw = raw_in; }
-    void setInitOffsetPose(){ hp_wld_initpos = hp_wld_raw; }
-    const int getRemainingCountDown() const { return countdown_num; }
-    const bool isHumanSyncOn() const { return HumanSyncOn; }
-    const double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0; }
-    void setTargetHumanToRobotRatio(const double tgt_h2r_r_in){ tgt_h2r_ratio = tgt_h2r_r_in; }
-    void setTargetCOMMoveModRatio(const double tgt_mod_ratio){ tgt_cmmr = tgt_mod_ratio; }
-    void updateCountDown(){
-      if(countdown_num>0){
-        countdown_num--;
-      }else if(countdown_num==0){
-        HumanSyncOn = true;
-        startCountdownForHumanSync = false;
-      }
-    }
-    void update(){//////////  メインループ  ////////////
-      gettimeofday(&t_calc_start, NULL);
-      updateHumanToRobotRatio             (tgt_h2r_ratio);
-      updateCOMModRatio                   (tgt_cmmr);
-      removeInitOffsetPose                (hp_wld_raw, hp_wld_initpos, hp_rel_raw);
-      applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);
-//      lockFootXYOnContact                 (go_rf_landing, go_lf_landing, hp_rel_raw);//根本から改変すべき2
-      calcWorldZMP                        ((hp_rel_raw.rf+init_wld_hp_rfpos), (hp_rel_raw.lf+init_wld_hp_lfpos), hp_rel_raw.rfw, hp_rel_raw.lfw, hp_rel_raw.zmp);//足の位置はworldにしないと・・・
-      convertRelHumanPoseToRelRobotPose   (hp_rel_raw, rp_ref_out);
-      applyCOMMoveModRatio                (rp_ref_out);
-      judgeFootLandOnCommand              (hp_wld_raw.rfw, hp_wld_raw.lfw, go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
-      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
-
-      applyEEWorkspaceLimit               (rp_ref_out);
-      lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
-
-      applyCOMToSupportRegionLimit        (rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
-      applyLPFilter                       (rp_ref_out);
-      applyCOMStateLimitByCapturePoint    (rp_ref_out.com, rp_ref_out_old.com, com_vel_old, rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
-
-      if(cam_rpy(1) > 20*M_PI/180)cam_rpy(1) = 20*M_PI/180;
-      if(cam_rpy(1) < -20*M_PI/180)cam_rpy(1) = -20*M_PI/180;
-      if(cam_rpy(2) > 20*M_PI/180)cam_rpy(2) = 20*M_PI/180;
-      if(cam_rpy(2) < -20*M_PI/180)cam_rpy(2) = -20*M_PI/180;
-      cam_rpy_filtered = cam_rpy_filter.passFilter(cam_rpy);
-
-      r_zmp_raw = rp_ref_out.zmp;
-      applyZMPCalcFromCOM                 (rp_ref_out.com,rp_ref_out.zmp);
-//      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, go_rf_landing, go_lf_landing);//ここじゃ判定遅い
-      applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
-      applyCOMZMPXYZLock                  (rp_ref_out);
-      overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
-      com_vel_old = (rp_ref_out.com - rp_ref_out_old.com)/DT;
-      rp_ref_out_old = rp_ref_out;
-      loop++;
-      gettimeofday(&t_calc_end, NULL);
-    }
-    void calibInitHumanCOMFromZMP(){
-      init_hp_calibcom(0) = r_zmp_raw(0);
-      init_hp_calibcom(1) = r_zmp_raw(1);
-    }
-    void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const HumanPose::Wrench6& rfwin, const HumanPose::Wrench6& lfwin, hrp::Vector3& zmp_ans){
-      hrp::Vector3 rfzmp,lfzmp;
-      const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
-      if( rfwin.f(2) > 1.0e-6 ){
-        rfzmp(0) = ( - rfwin.t(1) - rfwin.f(0) * F_H_OFFSET + rfwin.f(2) * 0 ) / rfwin.f(2) + rfpos(0);
-        rfzmp(1) = (   rfwin.t(0) - rfwin.f(1) * F_H_OFFSET + rfwin.f(2) * 0 ) / rfwin.f(2) + rfpos(1);
-      }
-      if( lfwin.f(2) > 1.0e-6 ){
-        lfzmp(0) = ( - lfwin.t(1) - lfwin.f(0) * F_H_OFFSET + lfwin.f(2) * 0 ) / lfwin.f(2) + lfpos(0);
-        lfzmp(1) = (   lfwin.t(0) - lfwin.f(1) * F_H_OFFSET + lfwin.f(2) * 0 ) / lfwin.f(2) + lfpos(1);
-      }
-      if( rfwin.f(2) > 1.0e-6 || lfwin.f(2) > 1.0e-6 ){
-        zmp_ans(0) = ( rfzmp(0)*rfwin.f(2) + lfzmp(0)*lfwin.f(2) ) / ( rfwin.f(2) + lfwin.f(2));
-        zmp_ans(1) = ( rfzmp(1)*rfwin.f(2) + lfzmp(1)*lfwin.f(2) ) / ( rfwin.f(2) + lfwin.f(2));
-      }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
-      zmp_ans(2) = 0;
-    }
-    static void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out(0) = in.x; out(1) = in.y; out(2) = in.z;}
-    static void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out(0) = in.r; out(1) = in.p; out(2) = in.y;}
-    static void Pose3DToVector3(const RTC::Pose3D& in, hrp::Vector3& out_pos, hrp::Vector3& out_rpy){ Point3DToVector3(in.position,out_pos); Oriantation3DToVector3(in.orientation,out_rpy); }
-    static void DoubleSeqToWrench6(const RTC::TimedDoubleSeq::_data_seq& in, HumanPose::Wrench6& out){
-      if(in.length() == 6){out.f(0)=in[0]; out.f(1)=in[1]; out.f(2)=in[2]; out.t(0)=in[3]; out.t(1)=in[4]; out.t(2)=in[5];
-      }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; out.f.Zero(); out.t.Zero(); }
-    }
-    static double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(0)*b(1)-a(1)*b(0); }
-
-  private:
-    void updateHumanToRobotRatio(const double h2r_r_goal){//h2r_ratioに伴って変わる変数の処理もここに書く
-      const double step = 0.001;
-      if(h2r_r_goal - h2r_ratio > step){h2r_ratio += step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
-      else if(h2r_r_goal - h2r_ratio < (-1)*step){h2r_ratio -= step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
-      else{h2r_ratio = h2r_r_goal;}
-      init_wld_hp_rfpos = init_wld_rp_rfeepos/h2r_ratio;
-      init_wld_hp_lfpos = init_wld_rp_lfeepos/h2r_ratio;
-    }
-    void updateCOMModRatio(const double cmmr_goal){
-      const double step = 0.001;
-      if(cmmr_goal - cmmr > step){cmmr += step; cout<<"Interpolating COMMoveModRatio as "<<cmmr<<endl;}
-      else if(cmmr_goal - cmmr < (-1)*step){cmmr -= step; cout<<"Interpolating COMMoveModRatio as "<<cmmr<<endl;}
-      else{cmmr = cmmr_goal;}
-    };
-    void removeInitOffsetPose(const HumanPose& abs_in, const HumanPose& init_offset, HumanPose& rel_out){
-      for(int i=0;i<rel_out.idsize-5;i++){
-        rel_out.Seq(i) =  abs_in.Seq(i) - init_offset.Seq(i);
-      }
-      rel_out.rfw = abs_in.rfw;
-      rel_out.lfw = abs_in.lfw;
-    }
-    void applyInitHumanZMPToCOMOffset(const HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
-      if(isHumanSyncOn()){
-        if(!init_zmp_com_offset_interpolator->isEmpty()){
-          init_zmp_com_offset_interpolator->get(&zmp_com_offset_ip_ratio, true);
-        }
-      }else{
-        zmp_com_offset_ip_ratio = 0;
-      }
-      out = in;
-      out.com = in.com + zmp_com_offset_ip_ratio * com_offs;
-    }
-    void applyLPFilter(HumanPose& tgt){
-      for(int i=0;i<iir_v_filters.size();i++){ tgt.Seq(i) = iir_v_filters[i].passFilter(tgt.Seq(i)); }
-    }
-    void applyLPFilter_o(const HumanPose& raw_in, const HumanPose& out_old, HumanPose& filt_out){
-      double FNUM = 0.01;//0.01でも荒い?
-      for(int i=0;i<filt_out.idsize;i++){
-        filt_out.Seq(i) = (1-FNUM) * out_old.Seq(i) + FNUM * raw_in.Seq(i);
-      }
-    }
-    void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
-      for(int i=0;i<rp_out.idsize-4;i++){
-        rp_out.Seq(i) =  h2r_ratio * hp_in.Seq(i);
-      }
-      rp_out.rfw  = hp_in.rfw;
-      rp_out.lfw  = hp_in.lfw;
-    }
-    void applyCOMMoveModRatio(HumanPose& tgt){//
-      tgt.com = cmmr * tgt.com;
-    }
-    void judgeFootLandOnCommand(const HumanPose::Wrench6& rfw_in, const HumanPose::Wrench6& lfw_in, bool& rfloc_ans, bool& lfloc_ans){
-      if(HumanSyncOn){
-        if      (rfloc_ans  && rfw_in.f(2)<CNT_F_TH   ){rfloc_ans = false;}//右足ついた状態から上げる
-        else if (!rfloc_ans && rfw_in.f(2)>CNT_F_TH+30){rfloc_ans = true;}//右足浮いた状態から下げる
-        if      (lfloc_ans  && lfw_in.f(2)<CNT_F_TH   ){lfloc_ans = false;}//左足ついた状態から上げる
-        else if (!lfloc_ans && lfw_in.f(2)>CNT_F_TH+30){lfloc_ans = true;}//左足浮いた状態から下げる
-      }
-      if(!rfloc_ans && !lfloc_ans){rfloc_ans = lfloc_ans = true;}//両足ジャンプは禁止
-    }
-    void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
-      if(rfcs_ans && !lfcs_ans){
-        if(tgt.zmp(1) > tgt.rf(1) + init_wld_rp_rfeepos(1) + 0.02){
-          lfcs_ans = true;
-          if(loop%100==0)cout<<"LF Auto lock"<<endl;
-        }
-      }
-      if(!rfcs_ans && lfcs_ans){
-        if(tgt.zmp(1) < tgt.lf(1) + init_wld_rp_lfeepos(1) - 0.02){
-          rfcs_ans = true;
-          if(loop%100==0)cout<<"RF Auto lock"<<endl;
-        }
-      }
-    }
-    void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
-      if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足接地しているタイミングでのみ更新
-      double RFDOWN_TIME = fabs(tgt.rf(1) - tgt.com(1))*4 + FUP_TIME;//下ろす速度は遊脚の重心からの距離に比例したペナルティ
-      double LFDOWN_TIME = fabs(tgt.lf(1) - tgt.com(1))*4 + FUP_TIME;
-      calcFootUpCurveAndJudgeFootContact2(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.rf, is_rf_contact);
-      calcFootUpCurveAndJudgeFootContact2(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.lf, is_lf_contact);
-    }
-    void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, int& cur_fup_count_in, hrp::Vector3& f_height_out, bool& is_f_contact_out){
-      int FUP_COUNT = (int)(HZ*FUP_TIME);
-      if(cur_fup_count_in > FUP_COUNT){cur_fup_count_in = FUP_COUNT;} if(cur_fup_count_in < 0){cur_fup_count_in = 0;}
-      if(!go_land_in){//足上げ命令入力時
-        if(cur_fup_count_in < FUP_COUNT){ cur_fup_count_in++; }
-      }else{//足下げ命令入力時
-        if(cur_fup_count_in > 0){ cur_fup_count_in--; }
-      }
-      f_height_out(2) = FUP_HIGHT/2*(1-cos(M_PI*cur_fup_count_in/FUP_COUNT));
-      is_f_contact_out = (cur_fup_count_in <= 0);
-    }
-    void calcFootUpCurveAndJudgeFootContact2(const bool& go_land_in, const double& fup_time_in, const double& fdown_time_in,  double& cur_fup_phase_in,hrp::Vector3& f_height_out, bool& is_f_contact_out){
-      const double omega_up = M_PI / fup_time_in;
-      const double omega_down = M_PI / fdown_time_in;
-      if(!go_land_in){//足上げ命令入力時
-        cur_fup_phase_in += omega_up * DT;
-      }else{//足下げ命令入力時
-        cur_fup_phase_in -= omega_down * DT;
-      }
-      if(cur_fup_phase_in > M_PI){cur_fup_phase_in = M_PI;}
-      else if(cur_fup_phase_in < 0){cur_fup_phase_in = 0;}
-      f_height_out(2) = FUP_HIGHT/2*(1-cos(cur_fup_phase_in));
-      is_f_contact_out = (cur_fup_phase_in <= 0);
-    }
-    void lockFootXYOnContact(const bool& rfcs_in, const bool& lfcs_in, HumanPose& tgt){
-      if(rfcs_in){
-        tgt.rf(0) = pre_cont_rfpos(0);
-        tgt.rf(1) = pre_cont_rfpos(1);
-      }else{
-        pre_cont_rfpos(0) = tgt.rf(0);
-        pre_cont_rfpos(1) = tgt.rf(1);
-      }
-      if(lfcs_in){
-        tgt.lf(0) = pre_cont_lfpos(0);
-        tgt.lf(1) = pre_cont_lfpos(1);
-      }else{
-        pre_cont_lfpos(0) = tgt.lf(0);
-        pre_cont_lfpos(1) = tgt.lf(1);
-      }
-    }
-    void applyEEWorkspaceLimit(HumanPose& tgt){
-      const double MAX = 0.15;
-      if(!is_rf_contact && is_lf_contact){//右足浮遊時
-        const hrp::Vector2 lf2rf_vec( tgt.rf(0)-pre_cont_lfpos(0), tgt.rf(1)-pre_cont_lfpos(1) );
-        if(lf2rf_vec.norm() > MAX){
-          tgt.rf(0) = (lf2rf_vec.normalized())(0) * MAX + pre_cont_lfpos(0);
-          tgt.rf(1) = (lf2rf_vec.normalized())(1) * MAX + pre_cont_lfpos(1);
-        }
-        if(tgt.rf(1) > pre_cont_lfpos(1) + 0.05 ) tgt.rf(1) = pre_cont_lfpos(1) + 0.05;
-      }
-      else if(is_rf_contact && !is_lf_contact){//左足浮遊時
-        const hrp::Vector2 rf2lf_vec( tgt.lf(0)-pre_cont_rfpos(0), tgt.lf(1)-pre_cont_rfpos(1) );
-        if(rf2lf_vec.norm() > MAX){
-          tgt.lf(0) = (rf2lf_vec.normalized())(0) * MAX + pre_cont_rfpos(0);
-          tgt.lf(1) = (rf2lf_vec.normalized())(1) * MAX + pre_cont_rfpos(1);
-        }
-        if(tgt.lf(1) < pre_cont_rfpos(1) - 0.05 ) tgt.lf(1) = pre_cont_rfpos(1) - 0.05;
-      }
-      hrp::Vector3 hand_ulimit = hrp::Vector3(0.1,0.1,0.1);
-      hrp::Vector3 hand_llimit = hrp::Vector3(-0.1,-0.1,-0.1);
-      for(int i=0;i<3;i++){
-        if      (tgt.rh(i) > hand_ulimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.rh(i) = hand_ulimit(i) + current_basepos(i) - init_basepos(i); }
-        else if (tgt.rh(i) < hand_llimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.rh(i) = hand_llimit(i) + current_basepos(i) - init_basepos(i); }
-        if      (tgt.lh(i) > hand_ulimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.lh(i) = hand_ulimit(i) + current_basepos(i) - init_basepos(i); }
-        else if (tgt.lh(i) < hand_llimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.lh(i) = hand_llimit(i) + current_basepos(i) - init_basepos(i); }
-      }
-    }
-    bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
-      std::vector<hrp::Vector2> hull;
-      createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull);
-      hrp::Vector2 cog(comin_abs(0),comin_abs(1));
-      if(!isPointInHullOpenCV(cog,hull)){ calcNearestPointOnHull(cog,hull,cog); }//外に出たら最近傍点に頭打ち
-      comin_abs(0) = cog(0);
-      comin_abs(1) = cog(1);
-      if( comin_abs(2) > 0.03 ) comin_abs(2) = 0.03;
-      if( comin_abs(2) < -0.15 ) comin_abs(2) = -0.15;
-      return true;
-    }
-    bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& com_old, const hrp::Vector3& com_vel_old, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans){
-      com_ans = com_in;
-      hrp::Vector3 com_vel = (com_in - com_old)/DT;
-      hrp::Vector3 com_acc = (com_vel - com_vel_old)/DT;//命名規則
-      hrp::Vector3 com_vel_zmpOk, com_vel_zmpOk_cpOk;
-      double accel[4] = {0.05, -0.03, 0.02, -0.02};
-      double decel[4] = {0.04, -0.02, 0.01, -0.01};
-      comacc_ref = calcacc_v_filters.passFilter(com_acc);
-      const double H = rp_wld_initpos.com(2);
-//      double maxacc[2],minacc[2];
-//      maxacc[0] = (com_old(0) - accel[1]) * (G/H); if(maxacc[0] < 0){maxacc[0]=0;}
-//      minacc[0] = (com_old(0) - accel[0]) * (G/H); if(minacc[0] > 0){minacc[0]=0;}
-//      maxacc[1] = (com_old(1) - (accel[3]-0.1)) * (G/H); if(maxacc[1] < 0){maxacc[1]=0;}
-//      minacc[1] = (com_old(1) - (accel[2]+0.1)) * (G/H); if(minacc[1] > 0){minacc[1]=0;}
-//      if(com_vel(0) > maxacc[0] * DT + com_vel_old(0)){com_vel(0) = maxacc[0] * DT + com_vel_old(0); cout<<"maxacc[0] over"<<endl;}
-//      if(com_vel(0) < minacc[0] * DT + com_vel_old(0)){com_vel(0) = minacc[0] * DT + com_vel_old(0); cout<<"minacc[0] over"<<endl;}
-//      if(com_vel(1) > maxacc[1] * DT + com_vel_old(1)){com_vel(1) = maxacc[1] * DT + com_vel_old(1); cout<<"maxacc[1] over"<<endl;}
-//      if(com_vel(1) < minacc[1] * DT + com_vel_old(1)){com_vel(1) = minacc[1] * DT + com_vel_old(1); cout<<"minacc[1] over"<<endl;}
-//      if(comacc_ref(0) > maxacc[0]){com_vel(0) = maxacc[0] * DT + com_vel_old(0); cout<<"maxacc[0] over"<<endl;}
-//      if(comacc_ref(0) < minacc[0]){com_vel(0) = minacc[0] * DT + com_vel_old(0); cout<<"minacc[0] over"<<endl;}
-//      if(comacc_ref(1) > maxacc[1]){com_vel(1) = maxacc[1] * DT + com_vel_old(1); cout<<"maxacc[1] over"<<endl;}
-//      if(comacc_ref(1) < minacc[1]){com_vel(1) = minacc[1] * DT + com_vel_old(1); cout<<"minacc[1] over"<<endl;}
-//      cout<<"comacc_ref(1) "<<comacc_ref(1)<<" minacc[1] "<<minacc[1]<<endl;
-////      cout<<"maxacc "<<maxacc[1]<<" minacc "<<minacc[1]<< " acc " << (com_vel(1)-com_vel_old(1))/DT <<endl;
-      std::vector<hrp::Vector2> hull;
-      createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull);
-      hrp::Vector2 com_vel_ans_2d;
-//      regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);
-      regulateCOMVelocityByCapturePointXY( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);//XY独立に計算する手法のほうがエッジで引っかからない・・・
-      com_ans(0) = com_old(0) + com_vel_ans_2d(0) * DT;
-      com_ans(1) = com_old(1) + com_vel_ans_2d(1) * DT;
-      return true;
-    }
-    void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
-      com_vel_ans = com_vel;
-      const double H = rp_wld_initpos.com(2);
-      hrp::Vector2 cp = com_pos + com_vel * sqrt( H / G );
-      hrp::Vector2 cp_ragulated;
-      if(!isPointInHullOpenCV(cp,hull)){
-        calcCrossPointOnHull(com_pos, cp, hull, cp_ragulated);
-        //calcNearestPointOnHull(cp, hull, cp_ragulated);
-        com_vel_ans = (cp_ragulated - com_pos) / sqrt( H / G );
-      }
-    }
-    void regulateCOMVelocityByCapturePointXY(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
-      com_vel_ans = com_vel;
-      const double H = rp_wld_initpos.com(2);
-      hrp::Vector4 margin_from_4maxcp;
-      calcXYMarginToHull(com_pos, hull, margin_from_4maxcp);
-      hrp::Vector3 max_com_vel,min_com_vel;
-      max_com_vel(0) = margin_from_4maxcp(0) / sqrt( H / G );
-      min_com_vel(0) = margin_from_4maxcp(1) / sqrt( H / G );
-      max_com_vel(1) = margin_from_4maxcp(2) / sqrt( H / G );
-      min_com_vel(1) = margin_from_4maxcp(3) / sqrt( H / G );
-      if(com_vel(0) > max_com_vel(0)){ com_vel_ans(0) = max_com_vel(0); if(loop%100==0)cout<<"F cp over"<<endl;}
-      if(com_vel(0) < min_com_vel(0)){ com_vel_ans(0) = min_com_vel(0); if(loop%100==0)cout<<"B cp over"<<endl;}
-      if(com_vel(1) > max_com_vel(1)){ com_vel_ans(1) = max_com_vel(1); if(loop%100==0)cout<<"L cp over"<<endl;}
-      if(com_vel(1) < min_com_vel(1)){ com_vel_ans(1) = min_com_vel(1); if(loop%100==0)cout<<"R cp over"<<endl;}
-    }
-    void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
-      std::vector<hrp::Vector2> points;
-      for(int i=0;i<2;i++){
-        for(int j=2;j<4;j++){
-          points.push_back(hrp::Vector2(rfin_abs(0) + rf_mgn(i),    rfin_abs(1) + rf_mgn(j)));
-          points.push_back(hrp::Vector2(lfin_abs(0) + lf_mgn(i),    lfin_abs(1) + lf_mgn(j)));
-        }
-      }
-      makeConvexHullOpenCV(points, hull_ans);
-    }
-    void calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans){
-      hrp::Vector4 margin_abs;
-      hrp::Vector2 cross_pt, anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
-      for(int direc=0;direc<4;direc++){
-        calcCrossPointOnHull(check_point, check_point+anchor_vec[direc], hull, cross_pt);
-        margin_abs(direc) = (cross_pt - check_point).norm();
-      }
-      margin_ans(0) =  margin_abs(0);
-      margin_ans(1) = -margin_abs(1);
-      margin_ans(2) =  margin_abs(2);
-      margin_ans(3) = -margin_abs(3);
-    }
-    bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_will_cross){
-      hrp::Vector2 anchor_vec = pt_out_goal - pt_in_start;
-      for(int i=0;i<hull.size();i++){
-        int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
-        hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
-        hrp::Vector2 cur_edge = nxt_pt - cur_pt;
-        double dBunbo = hrpVector2Cross(anchor_vec,cur_edge);
-        if( dBunbo != 0.0) {//平行の場合を外す
-          hrp::Vector2 vectorAC = hrp::Vector2(hull[i](0),hull[i](1)) - pt_in_start;
-          double dR = hrpVector2Cross(vectorAC,cur_edge) / dBunbo;
-          double dS = hrpVector2Cross(vectorAC,anchor_vec) / dBunbo;
-          if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){//dRには数値誤差が乗る
-            pt_will_cross = pt_in_start + dR * anchor_vec;
-            //dist = dR * anchor_vec.norm();
-            return true;
-          }
-        }
-      }
-      return false;
-    }
-    double calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_ans){
-      double cur_nearest_dist, ans_nearest_dist;
-      hrp::Vector2 cur_nearest_pt, ans_nearest_pt;
-      for(int i=0;i<hull.size();i++){
-        int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
-        hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
-        hrp::Vector2 cur_edge = nxt_pt - cur_pt;
-        hrp::Vector2 tgt_pt_v = tgt_pt - cur_pt;
-        double tgt_pt_projected_length = tgt_pt_v.dot(cur_edge.normalized());
-        if(tgt_pt_projected_length > cur_edge.norm() ){//tgt_pt's nearest point is on the　i+1-th vertex
-          cur_nearest_pt = nxt_pt;
-        }else if(tgt_pt_projected_length < 0 ){//tgt_pt's nearest point is on the　i-th vertex
-          cur_nearest_pt = cur_pt;
-        }else{//tgt_pt's nearest point is on the line
-          cur_nearest_pt = cur_pt + tgt_pt_projected_length * cur_edge.normalized();
-        }
-        cur_nearest_dist = (tgt_pt - cur_nearest_pt).norm();
-        if(i==0){//set first candidate as nearest
-          ans_nearest_dist = cur_nearest_dist;
-          ans_nearest_pt = cur_nearest_pt;
-        }else if( cur_nearest_dist < ans_nearest_dist ){//update nearest candidate
-          ans_nearest_dist = cur_nearest_dist;
-          ans_nearest_pt = cur_nearest_pt;
-        }
-      }
-      pt_ans = ans_nearest_pt;
-      return ans_nearest_dist;
-    }
-    void makeConvexHullOpenCV(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
-      std::vector<cv::Point2f> points,hull;
-      for(int i=0;i<pts.size();i++){ points.push_back( cv::Point2f( pts[i](0), pts[i](1)) ); }
-      cv::convexHull(cv::Mat(points),hull,true);
-      hull_ans.clear();
-      for(int i=0;i<hull.size();i++){ hull_ans.push_back( hrp::Vector2( hull[i].x, hull[i].y ) );  }
-    }
-    void makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
-      const int dim = 2;
-      double points[dim*pts.size()];
-      for(int i=0;i<pts.size();i++){ points[dim*i] = pts[i](0); points[dim*i+1] = pts[i](1); }
-      qh_new_qhull( dim, pts.size(), points, false, "qhull ", NULL, stderr);
-      pointT *point, *pointtemp;
-      hull_ans.clear();
-      FORALLpoints hull_ans.push_back( hrp::Vector2(point[0],point[1]) );
-      qh_freeqhull(!qh_ALL);
-      //#define qh_ORIENTclock 1
-    }
-    bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull){
-      std::vector<cv::Point2f> cvhull;
-      for(int i=0;i<hull.size();i++){ cvhull.push_back( cv::Point2f( hull[i](0), hull[i](1)) ); }
-      return (cv::pointPolygonTest(cv::Mat(cvhull), cv::Point2f(pt(0),pt(1)), false) > 0);
-    }
-    void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
-      comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
-      const double MAXACC = 5;
-      if(comacc(0)>MAXACC)comacc(0)=MAXACC;else if(comacc(0)<-MAXACC)comacc(0)=-MAXACC;
-      if(comacc(1)>MAXACC)comacc(1)=MAXACC;else if(comacc(1)<-MAXACC)comacc(1)=-MAXACC;
-      comacc = acc4zmp_v_filters.passFilter(comacc);
-      zmpout(0) = comin(0)-(rp_wld_initpos.com(2)/G)*comacc(0);
-      zmpout(1) = comin(1)-(rp_wld_initpos.com(2)/G)*comacc(1);
-      if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.zmp(0),rp_ref_out.zmp(1),zmpout(0),zmpout(1));
-      com_oldold = com_old;
-      com_old = comin;
-    }
-    void applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out){
-      for(int i=0;i<3;i++){if(in.com(i) - in_old.com(i) > MAXVEL){out.com(i) = in_old.com(i) + MAXVEL;}else if(in.com(i) - in_old.com(i) < -MAXVEL){out.com(i) = in_old.com(i) - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.rf(i)  - in_old.rf(i)  > MAXVEL){out.rf(i)  = in_old.rf(i)  + MAXVEL;}else if(in.rf(i)  - in_old.rf(i)  < -MAXVEL){out.rf(i)  = in_old.rf(i)  - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.lf(i)  - in_old.lf(i)  > MAXVEL){out.lf(i)  = in_old.lf(i)  + MAXVEL;}else if(in.lf(i)  - in_old.lf(i)  < -MAXVEL){out.lf(i)  = in_old.lf(i)  - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.rh(i)  - in_old.rh(i)  > MAXVEL){out.rh(i)  = in_old.rh(i)  + MAXVEL;}else if(in.rh(i)  - in_old.rh(i)  < -MAXVEL){out.rh(i)  = in_old.rh(i)  - MAXVEL;}}
-      for(int i=0;i<3;i++){if(in.lh(i)  - in_old.lh(i)  > MAXVEL){out.lh(i)  = in_old.lh(i)  + MAXVEL;}else if(in.lh(i)  - in_old.lh(i)  < -MAXVEL){out.lh(i)  = in_old.lh(i)  - MAXVEL;}}
-    }
-    void applyCOMZMPXYZLock(HumanPose& tgt){
-        if(!use_x){tgt.com(0) = 0;tgt.zmp(0) = 0;}
-        if(!use_y){tgt.com(1) = 0;tgt.zmp(1) = 0;}
-        if(!use_z){tgt.com(2) = 0;}
-    }
-};
+#include "HumanMasterSlave.h"
 
 
 // </rtc-template>
@@ -908,6 +228,7 @@ class AutoBalancer
   void getCurrentParameters();
   void getTargetParameters();
   bool solveLimbIKforLimb (ABCIKparam& param, const std::string& limb_name);
+  void solveWholeBodyCOMIK(const hrp::Vector3& com_ref, const hrp::Vector3& rf_ref, const hrp::Vector3& lf_ref, const hrp::Vector3& rh_ref, const hrp::Vector3& lh_ref, const hrp::Vector3& head_ref);
   void solveLimbIK();
   void startABCparam(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
   void stopABCparam();
diff --git a/rtc/AutoBalancer/CMakeLists.txt b/rtc/AutoBalancer/CMakeLists.txt
index ea2c0e743f0..c9e496a3b13 100644
--- a/rtc/AutoBalancer/CMakeLists.txt
+++ b/rtc/AutoBalancer/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(comp_sources AutoBalancer.cpp AutoBalancerService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp PreviewController.cpp GaitGenerator.cpp ../TorqueFilter/IIRFilter.cpp)
+set(comp_sources AutoBalancer.cpp AutoBalancerService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp PreviewController.cpp GaitGenerator.cpp ../TorqueFilter/IIRFilter.cpp HumanMasterSlave.cpp)
 set(libs ${OpenCV_LIBRARIES} hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub)
 add_library(AutoBalancer SHARED ${comp_sources})
 target_link_libraries(AutoBalancer ${libs})
diff --git a/rtc/AutoBalancer/HumanMasterSlave.cpp b/rtc/AutoBalancer/HumanMasterSlave.cpp
new file mode 100644
index 00000000000..c65a1c50d72
--- /dev/null
+++ b/rtc/AutoBalancer/HumanMasterSlave.cpp
@@ -0,0 +1,137 @@
+#include "HumanMasterSlave.h"
+
+void HumanSynchronizer::calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const HumanPose::Wrench6& rfwin, const HumanPose::Wrench6& lfwin, hrp::Vector3& zmp_ans){
+  hrp::Vector3 rfzmp,lfzmp;
+  const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
+  if( rfwin.f(2) > 1.0e-6 ){
+    rfzmp(0) = ( - rfwin.t(1) - rfwin.f(0) * F_H_OFFSET + rfwin.f(2) * 0 ) / rfwin.f(2) + rfpos(0);
+    rfzmp(1) = (   rfwin.t(0) - rfwin.f(1) * F_H_OFFSET + rfwin.f(2) * 0 ) / rfwin.f(2) + rfpos(1);
+  }
+  if( lfwin.f(2) > 1.0e-6 ){
+    lfzmp(0) = ( - lfwin.t(1) - lfwin.f(0) * F_H_OFFSET + lfwin.f(2) * 0 ) / lfwin.f(2) + lfpos(0);
+    lfzmp(1) = (   lfwin.t(0) - lfwin.f(1) * F_H_OFFSET + lfwin.f(2) * 0 ) / lfwin.f(2) + lfpos(1);
+  }
+  if( rfwin.f(2) > 1.0e-6 || lfwin.f(2) > 1.0e-6 ){
+    zmp_ans(0) = ( rfzmp(0)*rfwin.f(2) + lfzmp(0)*lfwin.f(2) ) / ( rfwin.f(2) + lfwin.f(2));
+    zmp_ans(1) = ( rfzmp(1)*rfwin.f(2) + lfzmp(1)*lfwin.f(2) ) / ( rfwin.f(2) + lfwin.f(2));
+  }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
+  zmp_ans(2) = 0;
+}
+
+void HumanSynchronizer::createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
+  std::vector<hrp::Vector2> points;
+  for(int i=0;i<2;i++){
+    for(int j=2;j<4;j++){
+      points.push_back(hrp::Vector2(rfin_abs(0) + rf_mgn(i),    rfin_abs(1) + rf_mgn(j)));
+      points.push_back(hrp::Vector2(lfin_abs(0) + lf_mgn(i),    lfin_abs(1) + lf_mgn(j)));
+    }
+  }
+  makeConvexHullOpenCV(points, hull_ans);
+}
+void HumanSynchronizer::calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans){
+  hrp::Vector4 margin_abs;
+  hrp::Vector2 cross_pt, anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
+  for(int direc=0;direc<4;direc++){
+    calcCrossPointOnHull(check_point, check_point+anchor_vec[direc], hull, cross_pt);
+    margin_abs(direc) = (cross_pt - check_point).norm();
+  }
+  margin_ans(0) =  margin_abs(0);
+  margin_ans(1) = -margin_abs(1);
+  margin_ans(2) =  margin_abs(2);
+  margin_ans(3) = -margin_abs(3);
+}
+bool HumanSynchronizer::calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_will_cross){
+  hrp::Vector2 anchor_vec = pt_out_goal - pt_in_start;
+  for(int i=0;i<hull.size();i++){
+    int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
+    hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
+    hrp::Vector2 cur_edge = nxt_pt - cur_pt;
+    double dBunbo = hrpVector2Cross(anchor_vec,cur_edge);
+    if( dBunbo != 0.0) {//平行の場合を外す
+      hrp::Vector2 vectorAC = hrp::Vector2(hull[i](0),hull[i](1)) - pt_in_start;
+      double dR = hrpVector2Cross(vectorAC,cur_edge) / dBunbo;
+      double dS = hrpVector2Cross(vectorAC,anchor_vec) / dBunbo;
+      if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){//dRには数値誤差が乗る
+        pt_will_cross = pt_in_start + dR * anchor_vec;
+        //dist = dR * anchor_vec.norm();
+        return true;
+      }
+    }
+  }
+  return false;
+}
+double HumanSynchronizer::calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_ans){
+  double cur_nearest_dist, ans_nearest_dist;
+  hrp::Vector2 cur_nearest_pt, ans_nearest_pt;
+  for(int i=0;i<hull.size();i++){
+    int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
+    hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
+    hrp::Vector2 cur_edge = nxt_pt - cur_pt;
+    hrp::Vector2 tgt_pt_v = tgt_pt - cur_pt;
+    double tgt_pt_projected_length = tgt_pt_v.dot(cur_edge.normalized());
+    if(tgt_pt_projected_length > cur_edge.norm() ){//tgt_pt's nearest point is on the　i+1-th vertex
+      cur_nearest_pt = nxt_pt;
+    }else if(tgt_pt_projected_length < 0 ){//tgt_pt's nearest point is on the　i-th vertex
+      cur_nearest_pt = cur_pt;
+    }else{//tgt_pt's nearest point is on the line
+      cur_nearest_pt = cur_pt + tgt_pt_projected_length * cur_edge.normalized();
+    }
+    cur_nearest_dist = (tgt_pt - cur_nearest_pt).norm();
+    if(i==0){//set first candidate as nearest
+      ans_nearest_dist = cur_nearest_dist;
+      ans_nearest_pt = cur_nearest_pt;
+    }else if( cur_nearest_dist < ans_nearest_dist ){//update nearest candidate
+      ans_nearest_dist = cur_nearest_dist;
+      ans_nearest_pt = cur_nearest_pt;
+    }
+  }
+  pt_ans = ans_nearest_pt;
+  return ans_nearest_dist;
+}
+void HumanSynchronizer::makeConvexHullOpenCV(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
+  std::vector<cv::Point2f> points,hull;
+  for(int i=0;i<pts.size();i++){ points.push_back( cv::Point2f( pts[i](0), pts[i](1)) ); }
+  cv::convexHull(cv::Mat(points),hull,true);
+  hull_ans.clear();
+  for(int i=0;i<hull.size();i++){ hull_ans.push_back( hrp::Vector2( hull[i].x, hull[i].y ) );  }
+}
+//void HumanSynchronizer::makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
+//  const int dim = 2;
+//  double points[dim*pts.size()];
+//  for(int i=0;i<pts.size();i++){ points[dim*i] = pts[i](0); points[dim*i+1] = pts[i](1); }
+//  qh_new_qhull( dim, pts.size(), points, false, "qhull ", NULL, stderr);
+//  pointT *point, *pointtemp;
+//  hull_ans.clear();
+//  FORALLpoints hull_ans.push_back( hrp::Vector2(point[0],point[1]) );
+//  qh_freeqhull(!qh_ALL);
+//  //#define qh_ORIENTclock 1
+//}
+bool HumanSynchronizer::isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull){
+  std::vector<cv::Point2f> cvhull;
+  for(int i=0;i<hull.size();i++){ cvhull.push_back( cv::Point2f( hull[i](0), hull[i](1)) ); }
+  return (cv::pointPolygonTest(cv::Mat(cvhull), cv::Point2f(pt(0),pt(1)), false) > 0);
+}
+void HumanSynchronizer::applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
+  comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
+  const double MAXACC = 5;
+  if(comacc(0)>MAXACC)comacc(0)=MAXACC;else if(comacc(0)<-MAXACC)comacc(0)=-MAXACC;
+  if(comacc(1)>MAXACC)comacc(1)=MAXACC;else if(comacc(1)<-MAXACC)comacc(1)=-MAXACC;
+  comacc = acc4zmp_v_filters.passFilter(comacc);
+  zmpout(0) = comin(0)-(rp_wld_initpos.com(2)/G)*comacc(0);
+  zmpout(1) = comin(1)-(rp_wld_initpos.com(2)/G)*comacc(1);
+  if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.zmp(0),rp_ref_out.zmp(1),zmpout(0),zmpout(1));
+  com_oldold = com_old;
+  com_old = comin;
+}
+void HumanSynchronizer::applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out){
+  for(int i=0;i<3;i++){if(in.com(i) - in_old.com(i) > MAXVEL){out.com(i) = in_old.com(i) + MAXVEL;}else if(in.com(i) - in_old.com(i) < -MAXVEL){out.com(i) = in_old.com(i) - MAXVEL;}}
+  for(int i=0;i<3;i++){if(in.rf(i)  - in_old.rf(i)  > MAXVEL){out.rf(i)  = in_old.rf(i)  + MAXVEL;}else if(in.rf(i)  - in_old.rf(i)  < -MAXVEL){out.rf(i)  = in_old.rf(i)  - MAXVEL;}}
+  for(int i=0;i<3;i++){if(in.lf(i)  - in_old.lf(i)  > MAXVEL){out.lf(i)  = in_old.lf(i)  + MAXVEL;}else if(in.lf(i)  - in_old.lf(i)  < -MAXVEL){out.lf(i)  = in_old.lf(i)  - MAXVEL;}}
+  for(int i=0;i<3;i++){if(in.rh(i)  - in_old.rh(i)  > MAXVEL){out.rh(i)  = in_old.rh(i)  + MAXVEL;}else if(in.rh(i)  - in_old.rh(i)  < -MAXVEL){out.rh(i)  = in_old.rh(i)  - MAXVEL;}}
+  for(int i=0;i<3;i++){if(in.lh(i)  - in_old.lh(i)  > MAXVEL){out.lh(i)  = in_old.lh(i)  + MAXVEL;}else if(in.lh(i)  - in_old.lh(i)  < -MAXVEL){out.lh(i)  = in_old.lh(i)  - MAXVEL;}}
+}
+void HumanSynchronizer::applyCOMZMPXYZLock(HumanPose& tgt){
+    if(!use_x){tgt.com(0) = 0;tgt.zmp(0) = 0;}
+    if(!use_y){tgt.com(1) = 0;tgt.zmp(1) = 0;}
+    if(!use_z){tgt.com(2) = 0;}
+}
diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
new file mode 100644
index 00000000000..b8794f5ce63
--- /dev/null
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -0,0 +1,556 @@
+#ifndef HUMANMASTERSLAVE_H
+#define HUMANMASTERSLAVE_H
+
+#include <rtm/idl/BasicDataType.hh>
+#include <rtm/idl/ExtendedDataTypes.hh>
+#include <rtm/Manager.h>
+#include <rtm/DataFlowComponentBase.h>
+#include <rtm/CorbaPort.h>
+#include <rtm/DataInPort.h>
+#include <rtm/DataOutPort.h>
+#include <rtm/idl/BasicDataTypeSkel.h>
+#include <rtm/idl/ExtendedDataTypesSkel.h>
+#include "interpolator.h"
+#include "../TorqueFilter/IIRFilter.h"
+#include <hrpModel/Body.h>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>
+#include <hrpUtil/Eigen4d.h>
+
+#if defined(__cplusplus)
+extern "C"
+{
+#endif
+#include <stdio.h>
+#include <stdlib.h>
+#include <qhull/qhull.h>
+#include <qhull/mem.h>
+#include <qhull/qset.h>
+#include <qhull/geom.h>
+#include <qhull/merge.h>
+#include <qhull/poly.h>
+#include <qhull/io.h>
+#include <qhull/stat.h>
+#if defined(__cplusplus)
+}
+#endif
+
+//実機だとログ出力で落ちる？getenv("HOME")ダメ？
+#define DEBUG 0
+
+class BiquadIIRFilterVec
+{
+private:
+    IIRFilter x_filters,y_filters,z_filters;
+    hrp::Vector3 ans;
+    std::vector<double> fb_coeffs, ff_coeffs;
+public:
+    BiquadIIRFilterVec(const std::string& error_prefix = ""){
+    };
+    ~BiquadIIRFilterVec() {};
+    bool setParameter(const double fc_in, const double HZ){
+      double fc = tan(fc_in * M_PI / HZ) / (2 * M_PI);
+      double denom = 1 + (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc;
+      ff_coeffs.resize(3);
+      fb_coeffs.resize(3);
+      ff_coeffs[0] = (4 * M_PI * M_PI * fc*fc) / denom;
+      ff_coeffs[1] = (8 * M_PI * M_PI * fc*fc) / denom;
+      ff_coeffs[2] = (4 * M_PI * M_PI * fc*fc) / denom;
+      fb_coeffs[0] = 1.0;
+      fb_coeffs[1] = (8 * M_PI * M_PI * fc*fc - 2) / denom;
+      fb_coeffs[2] = (1 - (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc) / denom;
+      return (
+        x_filters.setParameter(2,fb_coeffs,ff_coeffs) &&
+        y_filters.setParameter(2,fb_coeffs,ff_coeffs) &&
+        z_filters.setParameter(2,fb_coeffs,ff_coeffs)
+      );
+    };
+    hrp::Vector3 passFilter(const hrp::Vector3& input){
+      ans(0) = x_filters.passFilter(input(0));
+      ans(1) = y_filters.passFilter(input(1));
+      ans(2) = z_filters.passFilter(input(2));
+      return ans;
+    };
+};
+
+
+class HumanPose{
+  private: hrp::Vector3 zero;
+  public:
+    hrp::Vector3 com,rf,lf,rh,lh;
+    hrp::Vector3 zmp;
+    int idsize;
+    typedef struct{ hrp::Vector3 f; hrp::Vector3 t; }Wrench6;
+    Wrench6 rfw,lfw;
+    hrp::Vector3 neck;
+    HumanPose() : idsize(10){
+      clear();
+    }
+    ~HumanPose(){cerr<<"HumanPose destructed"<<endl;}
+    void clear(){
+      zero = hrp::Vector3::Zero();
+      for(int i=0;i<idsize;i++){ Seq(i) = hrp::Vector3::Zero(); }
+    }
+    hrp::Vector3& Seq(const int i){
+      switch(i){
+        case 0: return com;
+        case 1: return rf;
+        case 2: return lf;
+        case 3: return rh;
+        case 4: return lh;
+        case 5: return zmp;
+        case 6: return rfw.f;
+        case 7: return rfw.t;
+        case 8: return lfw.f;
+        case 9: return lfw.t;
+        default: std::cerr <<"[WARN] HumanPose::getSeq(int) invalid index"<<std::endl;return zero;
+      }
+    }
+    const hrp::Vector3& Seq(const int i) const{//const指定オブジェクトからオーバーロードする時のためのオーバーライド
+      switch(i){
+        case 0: return com;
+        case 1: return rf;
+        case 2: return lf;
+        case 3: return rh;
+        case 4: return lh;
+        case 5: return zmp;
+        case 6: return rfw.f;
+        case 7: return rfw.t;
+        case 8: return lfw.f;
+        case 9: return lfw.t;
+        default: std::cerr <<"[WARN] HumanPose::getSeq(int) invalid index"<<std::endl;return zero;
+      }
+    }
+    static void hp_printf(HumanPose& in){
+      const std::string cap[] = {"c","rf","lf","rh","lh","z","rw","","lw",""};
+      for(int i=0;i<6;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.Seq(i)(0),in.Seq(i)(1),in.Seq(i)(2)); }
+      for(int i=6;i<10;i++){ printf("\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.Seq(i)(0),in.Seq(i)(1),in.Seq(i)(2)); }
+      printf("\n");
+    }
+    void print(){ hp_printf(*this); }
+};
+
+
+
+class HumanSynchronizer{
+  private:
+    double FUP_TIME;
+    int cur_rfup_level,cur_lfup_level;
+    double cur_rfup_rad,cur_lfup_rad;
+    double FUP_HIGHT;
+    double CNT_F_TH;
+    hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
+    hrp::Vector3 init_hp_calibcom;
+    double MAXVEL,MAXACC;
+    HumanPose hp_wld_raw;
+    HumanPose hp_wld_initpos;
+    HumanPose hp_rel_raw;
+    HumanPose rp_ref_out_old;
+    int countdown_num;
+    bool HumanSyncOn;
+    struct timeval t_calc_start, t_calc_end;
+    double h2r_ratio, tgt_h2r_ratio;
+    double cmmr, tgt_cmmr;
+    double HZ,DT,G;
+    hrp::Vector3 com_old,com_oldold,comacc;
+    hrp::Vector3 comacc_ref;
+    std::vector<BiquadIIRFilterVec> iir_v_filters;
+    BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters;
+    BiquadIIRFilterVec cam_rpy_filter;
+    hrp::Vector3 r_zmp_raw;
+    hrp::Vector4 rf_safe_region,lf_safe_region;//前後左右の順
+
+  public:
+    double tgt_FUP_TIME;
+    bool startCountdownForHumanSync;
+    bool ht_first_call;
+    bool is_rf_contact,is_lf_contact;//足上げ高さ0に到達しているか否か
+    bool go_rf_landing,go_lf_landing;//足上げ指令の有無
+    HumanPose rp_wld_initpos;//IKでのリンク原点の位置(足首，手首など)
+    bool use_x,use_y,use_z;
+    HumanPose rp_ref_out;
+    FILE *sr_log,*cz_log;
+    bool use_rh,use_lh;
+    hrp::Vector3 init_wld_rp_rfeepos,init_wld_rp_lfeepos;
+    hrp::Vector3 init_wld_hp_rfpos,init_wld_hp_lfpos;
+    hrp::Vector3 current_basepos,init_basepos;
+    hrp::Vector3 com_vel_old;
+    unsigned int loop;
+    hrp::Vector3 cam_pos,cam_rpy,cam_pos_filtered,cam_rpy_filtered;
+
+
+    interpolator *init_zmp_com_offset_interpolator;
+    double zmp_com_offset_ip_ratio;
+
+    HumanSynchronizer(){
+      tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
+//      tgt_h2r_ratio = h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
+      HZ = 500;
+      DT = 1.0/HZ;
+      G = 9.80665;
+      tgt_cmmr = cmmr = 1.0;
+      use_x = use_y = use_z = true;
+      cur_rfup_level = cur_lfup_level = 0;
+      cur_rfup_rad = cur_lfup_rad = 0;
+      is_rf_contact = is_lf_contact = true;
+      go_rf_landing = go_lf_landing = false;
+      pre_cont_rfpos = pre_cont_lfpos = hrp::Vector3::Zero();
+      init_hp_calibcom = hrp::Vector3::Zero();
+      tgt_FUP_TIME = FUP_TIME = 0.4;
+      FUP_HIGHT = 0.05;
+      CNT_F_TH = 20.0;
+      MAXVEL = 0.004;
+      HumanSyncOn = false;
+      ht_first_call = true;
+      startCountdownForHumanSync = false;
+      countdown_num = 5*HZ;
+      loop = 0;
+
+      //////////  hrp::Vector は初期化必要  ///////////
+      com_old = com_oldold = comacc = hrp::Vector3::Zero();
+      comacc_ref = hrp::Vector3::Zero();
+      r_zmp_raw = hrp::Vector3::Zero();
+
+      init_wld_rp_rfeepos = init_wld_rp_lfeepos = hrp::Vector3::Zero();
+      init_wld_hp_rfpos = init_wld_hp_lfpos = hrp::Vector3::Zero();
+      current_basepos = init_basepos = hrp::Vector3::Zero();
+
+      com_vel_old = hrp::Vector3::Zero();
+      cam_pos = cam_rpy = cam_pos_filtered = cam_rpy_filtered = hrp::Vector3::Zero();
+
+      iir_v_filters.resize(5);
+      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(0.5,HZ);//四肢拘束点用
+      iir_v_filters[0].setParameter(0.4,HZ);//重心用
+      calcacc_v_filters.setParameter(5,HZ);//加速度計算用
+      acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
+      cam_rpy_filter.setParameter(1,HZ);//カメラアングル
+
+      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
+      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
+//      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.0);
+//      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.0, -0.02);
+
+      use_rh = use_lh = false;
+
+      rp_ref_out_old.clear();
+      rp_ref_out.clear();
+
+      init_zmp_com_offset_interpolator = new interpolator(1, DT, interpolator::HOFFARBIB, 1);
+      init_zmp_com_offset_interpolator->setName("zmp_com_offset_interpolator");
+      double start_ratio = 0.0;
+      init_zmp_com_offset_interpolator->set(&start_ratio);
+      double goal_ratio = 1.0;
+      init_zmp_com_offset_interpolator->go(&goal_ratio, 3.0, true);//3s
+
+      if(DEBUG){
+//        std::string home_path(std::getenv("HOME"));
+//        sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
+//        cz_log = fopen((home_path+"/HumanSync_com_zmp.log").c_str(),"w+");
+        sr_log = fopen("/home/ishiguro/HumanSync_support_region.log","w+");
+        cz_log = fopen("/home/ishiguro/HumanSync_com_zmp.log","w+");
+      }
+      cout<<"HumanSynchronizer constructed"<<endl;
+    }
+    ~HumanSynchronizer(){
+      if(DEBUG)fclose(sr_log);
+      if(DEBUG)fclose(cz_log);
+      delete init_zmp_com_offset_interpolator;
+      cout<<"HumanSynchronizer destructed"<<endl;
+    }
+
+    void readInput(const HumanPose& raw_in){ hp_wld_raw = raw_in; }
+    void setInitOffsetPose(){ hp_wld_initpos = hp_wld_raw; }
+    const int getRemainingCountDown() const { return countdown_num; }
+    const bool isHumanSyncOn() const { return HumanSyncOn; }
+    const double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0; }
+    void setTargetHumanToRobotRatio(const double tgt_h2r_r_in){ tgt_h2r_ratio = tgt_h2r_r_in; }
+    void setTargetCOMMoveModRatio(const double tgt_mod_ratio){ tgt_cmmr = tgt_mod_ratio; }
+    void updateCountDown(){
+      if(countdown_num>0){
+        countdown_num--;
+      }else if(countdown_num==0){
+        HumanSyncOn = true;
+        startCountdownForHumanSync = false;
+      }
+    }
+    void update(){//////////  メインループ  ////////////
+      gettimeofday(&t_calc_start, NULL);
+      updateHumanToRobotRatio             (tgt_h2r_ratio);
+      updateCOMModRatio                   (tgt_cmmr);
+      removeInitOffsetPose                (hp_wld_raw, hp_wld_initpos, hp_rel_raw);
+      applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);
+//      lockFootXYOnContact                 (go_rf_landing, go_lf_landing, hp_rel_raw);//根本から改変すべき2
+      calcWorldZMP                        ((hp_rel_raw.rf+init_wld_hp_rfpos), (hp_rel_raw.lf+init_wld_hp_lfpos), hp_rel_raw.rfw, hp_rel_raw.lfw, hp_rel_raw.zmp);//足の位置はworldにしないと・・・
+      convertRelHumanPoseToRelRobotPose   (hp_rel_raw, rp_ref_out);
+      applyCOMMoveModRatio                (rp_ref_out);
+      judgeFootLandOnCommand              (hp_wld_raw.rfw, hp_wld_raw.lfw, go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
+      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
+
+      applyEEWorkspaceLimit               (rp_ref_out);
+      lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
+
+      applyCOMToSupportRegionLimit        (rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
+      applyLPFilter                       (rp_ref_out);
+      applyCOMStateLimitByCapturePoint    (rp_ref_out.com, rp_ref_out_old.com, com_vel_old, rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
+
+      if(cam_rpy(1) > 20*M_PI/180)cam_rpy(1) = 20*M_PI/180;
+      if(cam_rpy(1) < -20*M_PI/180)cam_rpy(1) = -20*M_PI/180;
+      if(cam_rpy(2) > 20*M_PI/180)cam_rpy(2) = 20*M_PI/180;
+      if(cam_rpy(2) < -20*M_PI/180)cam_rpy(2) = -20*M_PI/180;
+      cam_rpy_filtered = cam_rpy_filter.passFilter(cam_rpy);
+
+      r_zmp_raw = rp_ref_out.zmp;
+      applyZMPCalcFromCOM                 (rp_ref_out.com,rp_ref_out.zmp);
+//      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, go_rf_landing, go_lf_landing);//ここじゃ判定遅い
+      applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
+      applyCOMZMPXYZLock                  (rp_ref_out);
+      overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
+      com_vel_old = (rp_ref_out.com - rp_ref_out_old.com)/DT;
+      rp_ref_out_old = rp_ref_out;
+      loop++;
+      gettimeofday(&t_calc_end, NULL);
+    }
+    void calibInitHumanCOMFromZMP(){
+      init_hp_calibcom(0) = r_zmp_raw(0);
+      init_hp_calibcom(1) = r_zmp_raw(1);
+    }
+    static void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out(0) = in.x; out(1) = in.y; out(2) = in.z;}
+    static void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out(0) = in.r; out(1) = in.p; out(2) = in.y;}
+    static void Pose3DToVector3(const RTC::Pose3D& in, hrp::Vector3& out_pos, hrp::Vector3& out_rpy){ Point3DToVector3(in.position,out_pos); Oriantation3DToVector3(in.orientation,out_rpy); }
+    static void DoubleSeqToWrench6(const RTC::TimedDoubleSeq::_data_seq& in, HumanPose::Wrench6& out){
+      if(in.length() == 6){out.f(0)=in[0]; out.f(1)=in[1]; out.f(2)=in[2]; out.t(0)=in[3]; out.t(1)=in[4]; out.t(2)=in[5];
+      }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; out.f.Zero(); out.t.Zero(); }
+    }
+    static double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(0)*b(1)-a(1)*b(0); }
+
+  private:
+    void updateHumanToRobotRatio(const double h2r_r_goal){//h2r_ratioに伴って変わる変数の処理もここに書く
+      const double step = 0.001;
+      if(h2r_r_goal - h2r_ratio > step){h2r_ratio += step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
+      else if(h2r_r_goal - h2r_ratio < (-1)*step){h2r_ratio -= step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
+      else{h2r_ratio = h2r_r_goal;}
+      init_wld_hp_rfpos = init_wld_rp_rfeepos/h2r_ratio;
+      init_wld_hp_lfpos = init_wld_rp_lfeepos/h2r_ratio;
+    }
+    void updateCOMModRatio(const double cmmr_goal){
+      const double step = 0.001;
+      if(cmmr_goal - cmmr > step){cmmr += step; cout<<"Interpolating COMMoveModRatio as "<<cmmr<<endl;}
+      else if(cmmr_goal - cmmr < (-1)*step){cmmr -= step; cout<<"Interpolating COMMoveModRatio as "<<cmmr<<endl;}
+      else{cmmr = cmmr_goal;}
+    };
+    void removeInitOffsetPose(const HumanPose& abs_in, const HumanPose& init_offset, HumanPose& rel_out){
+      for(int i=0;i<rel_out.idsize-5;i++){
+        rel_out.Seq(i) =  abs_in.Seq(i) - init_offset.Seq(i);
+      }
+      rel_out.rfw = abs_in.rfw;
+      rel_out.lfw = abs_in.lfw;
+    }
+    void applyInitHumanZMPToCOMOffset(const HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
+      if(isHumanSyncOn()){
+        if(!init_zmp_com_offset_interpolator->isEmpty()){
+          init_zmp_com_offset_interpolator->get(&zmp_com_offset_ip_ratio, true);
+        }
+      }else{
+        zmp_com_offset_ip_ratio = 0;
+      }
+      out = in;
+      out.com = in.com + zmp_com_offset_ip_ratio * com_offs;
+    }
+    void applyLPFilter(HumanPose& tgt){
+      for(int i=0;i<iir_v_filters.size();i++){ tgt.Seq(i) = iir_v_filters[i].passFilter(tgt.Seq(i)); }
+    }
+    void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
+      for(int i=0;i<rp_out.idsize-4;i++){
+        rp_out.Seq(i) =  h2r_ratio * hp_in.Seq(i);
+      }
+      rp_out.rfw  = hp_in.rfw;
+      rp_out.lfw  = hp_in.lfw;
+    }
+    void applyCOMMoveModRatio(HumanPose& tgt){//
+      tgt.com = cmmr * tgt.com;
+    }
+    void judgeFootLandOnCommand(const HumanPose::Wrench6& rfw_in, const HumanPose::Wrench6& lfw_in, bool& rfloc_ans, bool& lfloc_ans){
+      if(HumanSyncOn){
+        if      (rfloc_ans  && rfw_in.f(2)<CNT_F_TH   ){rfloc_ans = false;}//右足ついた状態から上げる
+        else if (!rfloc_ans && rfw_in.f(2)>CNT_F_TH+30){rfloc_ans = true;}//右足浮いた状態から下げる
+        if      (lfloc_ans  && lfw_in.f(2)<CNT_F_TH   ){lfloc_ans = false;}//左足ついた状態から上げる
+        else if (!lfloc_ans && lfw_in.f(2)>CNT_F_TH+30){lfloc_ans = true;}//左足浮いた状態から下げる
+      }
+      if(!rfloc_ans && !lfloc_ans){rfloc_ans = lfloc_ans = true;}//両足ジャンプは禁止
+    }
+    void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
+      if(rfcs_ans && !lfcs_ans){
+        if(tgt.zmp(1) > tgt.rf(1) + init_wld_rp_rfeepos(1) + 0.02){
+          lfcs_ans = true;
+          if(loop%100==0)cout<<"LF Auto lock"<<endl;
+        }
+      }
+      if(!rfcs_ans && lfcs_ans){
+        if(tgt.zmp(1) < tgt.lf(1) + init_wld_rp_lfeepos(1) - 0.02){
+          rfcs_ans = true;
+          if(loop%100==0)cout<<"RF Auto lock"<<endl;
+        }
+      }
+    }
+    void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
+      if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足接地しているタイミングでのみ更新
+      double RFDOWN_TIME = fabs(tgt.rf(1) - tgt.com(1))*4 + FUP_TIME;//下ろす速度は遊脚の重心からの距離に比例したペナルティ
+      double LFDOWN_TIME = fabs(tgt.lf(1) - tgt.com(1))*4 + FUP_TIME;
+      calcFootUpCurveAndJudgeFootContact2(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.rf, is_rf_contact);
+      calcFootUpCurveAndJudgeFootContact2(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.lf, is_lf_contact);
+    }
+    void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, int& cur_fup_count_in, hrp::Vector3& f_height_out, bool& is_f_contact_out){
+      int FUP_COUNT = (int)(HZ*FUP_TIME);
+      if(cur_fup_count_in > FUP_COUNT){cur_fup_count_in = FUP_COUNT;} if(cur_fup_count_in < 0){cur_fup_count_in = 0;}
+      if(!go_land_in){//足上げ命令入力時
+        if(cur_fup_count_in < FUP_COUNT){ cur_fup_count_in++; }
+      }else{//足下げ命令入力時
+        if(cur_fup_count_in > 0){ cur_fup_count_in--; }
+      }
+      f_height_out(2) = FUP_HIGHT/2*(1-cos(M_PI*cur_fup_count_in/FUP_COUNT));
+      is_f_contact_out = (cur_fup_count_in <= 0);
+    }
+    void calcFootUpCurveAndJudgeFootContact2(const bool& go_land_in, const double& fup_time_in, const double& fdown_time_in,  double& cur_fup_phase_in,hrp::Vector3& f_height_out, bool& is_f_contact_out){
+      const double omega_up = M_PI / fup_time_in;
+      const double omega_down = M_PI / fdown_time_in;
+      if(!go_land_in){//足上げ命令入力時
+        cur_fup_phase_in += omega_up * DT;
+      }else{//足下げ命令入力時
+        cur_fup_phase_in -= omega_down * DT;
+      }
+      if(cur_fup_phase_in > M_PI){cur_fup_phase_in = M_PI;}
+      else if(cur_fup_phase_in < 0){cur_fup_phase_in = 0;}
+      f_height_out(2) = FUP_HIGHT/2*(1-cos(cur_fup_phase_in));
+      is_f_contact_out = (cur_fup_phase_in <= 0);
+    }
+    void lockFootXYOnContact(const bool& rfcs_in, const bool& lfcs_in, HumanPose& tgt){
+      if(rfcs_in){
+        tgt.rf(0) = pre_cont_rfpos(0);
+        tgt.rf(1) = pre_cont_rfpos(1);
+      }else{
+        pre_cont_rfpos(0) = tgt.rf(0);
+        pre_cont_rfpos(1) = tgt.rf(1);
+      }
+      if(lfcs_in){
+        tgt.lf(0) = pre_cont_lfpos(0);
+        tgt.lf(1) = pre_cont_lfpos(1);
+      }else{
+        pre_cont_lfpos(0) = tgt.lf(0);
+        pre_cont_lfpos(1) = tgt.lf(1);
+      }
+    }
+    void applyEEWorkspaceLimit(HumanPose& tgt){
+      const double MAX = 0.15;
+      if(!is_rf_contact && is_lf_contact){//右足浮遊時
+        const hrp::Vector2 lf2rf_vec( tgt.rf(0)-pre_cont_lfpos(0), tgt.rf(1)-pre_cont_lfpos(1) );
+        if(lf2rf_vec.norm() > MAX){
+          tgt.rf(0) = (lf2rf_vec.normalized())(0) * MAX + pre_cont_lfpos(0);
+          tgt.rf(1) = (lf2rf_vec.normalized())(1) * MAX + pre_cont_lfpos(1);
+        }
+        if(tgt.rf(1) > pre_cont_lfpos(1) + 0.05 ) tgt.rf(1) = pre_cont_lfpos(1) + 0.05;
+      }
+      else if(is_rf_contact && !is_lf_contact){//左足浮遊時
+        const hrp::Vector2 rf2lf_vec( tgt.lf(0)-pre_cont_rfpos(0), tgt.lf(1)-pre_cont_rfpos(1) );
+        if(rf2lf_vec.norm() > MAX){
+          tgt.lf(0) = (rf2lf_vec.normalized())(0) * MAX + pre_cont_rfpos(0);
+          tgt.lf(1) = (rf2lf_vec.normalized())(1) * MAX + pre_cont_rfpos(1);
+        }
+        if(tgt.lf(1) < pre_cont_rfpos(1) - 0.05 ) tgt.lf(1) = pre_cont_rfpos(1) - 0.05;
+      }
+      hrp::Vector3 hand_ulimit = hrp::Vector3(0.1,0.1,0.1);
+      hrp::Vector3 hand_llimit = hrp::Vector3(-0.1,-0.1,-0.1);
+      for(int i=0;i<3;i++){
+        if      (tgt.rh(i) > hand_ulimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.rh(i) = hand_ulimit(i) + current_basepos(i) - init_basepos(i); }
+        else if (tgt.rh(i) < hand_llimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.rh(i) = hand_llimit(i) + current_basepos(i) - init_basepos(i); }
+        if      (tgt.lh(i) > hand_ulimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.lh(i) = hand_ulimit(i) + current_basepos(i) - init_basepos(i); }
+        else if (tgt.lh(i) < hand_llimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.lh(i) = hand_llimit(i) + current_basepos(i) - init_basepos(i); }
+      }
+    }
+    bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
+      std::vector<hrp::Vector2> hull;
+      createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull);
+      hrp::Vector2 cog(comin_abs(0),comin_abs(1));
+      if(!isPointInHullOpenCV(cog,hull)){ calcNearestPointOnHull(cog,hull,cog); }//外に出たら最近傍点に頭打ち
+      comin_abs(0) = cog(0);
+      comin_abs(1) = cog(1);
+      if( comin_abs(2) > 0.03 ) comin_abs(2) = 0.03;
+      if( comin_abs(2) < -0.15 ) comin_abs(2) = -0.15;
+      return true;
+    }
+    bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& com_old, const hrp::Vector3& com_vel_old, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans){
+      com_ans = com_in;
+      hrp::Vector3 com_vel = (com_in - com_old)/DT;
+      hrp::Vector3 com_acc = (com_vel - com_vel_old)/DT;//命名規則
+      hrp::Vector3 com_vel_zmpOk, com_vel_zmpOk_cpOk;
+      double accel[4] = {0.05, -0.03, 0.02, -0.02};
+      double decel[4] = {0.04, -0.02, 0.01, -0.01};
+      comacc_ref = calcacc_v_filters.passFilter(com_acc);
+      const double H = rp_wld_initpos.com(2);
+//      double maxacc[2],minacc[2];
+//      maxacc[0] = (com_old(0) - accel[1]) * (G/H); if(maxacc[0] < 0){maxacc[0]=0;}
+//      minacc[0] = (com_old(0) - accel[0]) * (G/H); if(minacc[0] > 0){minacc[0]=0;}
+//      maxacc[1] = (com_old(1) - (accel[3]-0.1)) * (G/H); if(maxacc[1] < 0){maxacc[1]=0;}
+//      minacc[1] = (com_old(1) - (accel[2]+0.1)) * (G/H); if(minacc[1] > 0){minacc[1]=0;}
+//      if(com_vel(0) > maxacc[0] * DT + com_vel_old(0)){com_vel(0) = maxacc[0] * DT + com_vel_old(0); cout<<"maxacc[0] over"<<endl;}
+//      if(com_vel(0) < minacc[0] * DT + com_vel_old(0)){com_vel(0) = minacc[0] * DT + com_vel_old(0); cout<<"minacc[0] over"<<endl;}
+//      if(com_vel(1) > maxacc[1] * DT + com_vel_old(1)){com_vel(1) = maxacc[1] * DT + com_vel_old(1); cout<<"maxacc[1] over"<<endl;}
+//      if(com_vel(1) < minacc[1] * DT + com_vel_old(1)){com_vel(1) = minacc[1] * DT + com_vel_old(1); cout<<"minacc[1] over"<<endl;}
+//      if(comacc_ref(0) > maxacc[0]){com_vel(0) = maxacc[0] * DT + com_vel_old(0); cout<<"maxacc[0] over"<<endl;}
+//      if(comacc_ref(0) < minacc[0]){com_vel(0) = minacc[0] * DT + com_vel_old(0); cout<<"minacc[0] over"<<endl;}
+//      if(comacc_ref(1) > maxacc[1]){com_vel(1) = maxacc[1] * DT + com_vel_old(1); cout<<"maxacc[1] over"<<endl;}
+//      if(comacc_ref(1) < minacc[1]){com_vel(1) = minacc[1] * DT + com_vel_old(1); cout<<"minacc[1] over"<<endl;}
+//      cout<<"comacc_ref(1) "<<comacc_ref(1)<<" minacc[1] "<<minacc[1]<<endl;
+////      cout<<"maxacc "<<maxacc[1]<<" minacc "<<minacc[1]<< " acc " << (com_vel(1)-com_vel_old(1))/DT <<endl;
+      std::vector<hrp::Vector2> hull;
+      createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull);
+      hrp::Vector2 com_vel_ans_2d;
+//      regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);
+      regulateCOMVelocityByCapturePointXY( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);//XY独立に計算する手法のほうがエッジで引っかからない・・・
+      com_ans(0) = com_old(0) + com_vel_ans_2d(0) * DT;
+      com_ans(1) = com_old(1) + com_vel_ans_2d(1) * DT;
+      return true;
+    }
+    void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
+      com_vel_ans = com_vel;
+      const double H = rp_wld_initpos.com(2);
+      hrp::Vector2 cp = com_pos + com_vel * sqrt( H / G );
+      hrp::Vector2 cp_ragulated;
+      if(!isPointInHullOpenCV(cp,hull)){
+        calcCrossPointOnHull(com_pos, cp, hull, cp_ragulated);
+        //calcNearestPointOnHull(cp, hull, cp_ragulated);
+        com_vel_ans = (cp_ragulated - com_pos) / sqrt( H / G );
+      }
+    }
+    void regulateCOMVelocityByCapturePointXY(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
+      com_vel_ans = com_vel;
+      const double H = rp_wld_initpos.com(2);
+      hrp::Vector4 margin_from_4maxcp;
+      calcXYMarginToHull(com_pos, hull, margin_from_4maxcp);
+      hrp::Vector3 max_com_vel,min_com_vel;
+      max_com_vel(0) = margin_from_4maxcp(0) / sqrt( H / G );
+      min_com_vel(0) = margin_from_4maxcp(1) / sqrt( H / G );
+      max_com_vel(1) = margin_from_4maxcp(2) / sqrt( H / G );
+      min_com_vel(1) = margin_from_4maxcp(3) / sqrt( H / G );
+      if(com_vel(0) > max_com_vel(0)){ com_vel_ans(0) = max_com_vel(0); if(loop%100==0)cout<<"F cp over"<<endl;}
+      if(com_vel(0) < min_com_vel(0)){ com_vel_ans(0) = min_com_vel(0); if(loop%100==0)cout<<"B cp over"<<endl;}
+      if(com_vel(1) > max_com_vel(1)){ com_vel_ans(1) = max_com_vel(1); if(loop%100==0)cout<<"L cp over"<<endl;}
+      if(com_vel(1) < min_com_vel(1)){ com_vel_ans(1) = min_com_vel(1); if(loop%100==0)cout<<"R cp over"<<endl;}
+    }
+    void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const HumanPose::Wrench6& rfwin, const HumanPose::Wrench6& lfwin, hrp::Vector3& zmp_ans);
+    void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans);
+    void calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans);
+    bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_will_cross);
+    double calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_ans);
+    void makeConvexHullOpenCV(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans);
+    void makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans);
+    bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull);
+    void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout);
+    void applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out);
+    void applyCOMZMPXYZLock(HumanPose& tgt);
+};
+
+
+
+#endif // HUMANMASTERSLAVE_H

From dceef5c62044e1d00d769b377f6564ae1956a231 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 11 Jan 2017 17:59:21 +0900
Subject: [PATCH 046/217] rm log

---
 rtc20837.log | 9 ---------
 1 file changed, 9 deletions(-)
 delete mode 100644 rtc20837.log

diff --git a/rtc20837.log b/rtc20837.log
deleted file mode 100644
index 00bb5aedf43..00000000000
--- a/rtc20837.log
+++ /dev/null
@@ -1,9 +0,0 @@
-Aug 16 10:19:39.481 INFO: manager: 1.1.0
-Aug 16 10:19:39.481 INFO: manager: Copyright (C) 2003-2010
-Aug 16 10:19:39.481 INFO: manager:   Noriaki Ando
-Aug 16 10:19:39.481 INFO: manager:   Intelligent Systems Research Institute, AIST
-Aug 16 10:19:39.481 INFO: manager: Manager starting.
-Aug 16 10:19:39.481 INFO: manager: Starting local logging.
-Aug 16 10:19:39.493 WARNING: NamingOnCorba: No endpoint for the CORBA naming service (localhost) was found.
-Aug 16 10:19:39.493 INFO: NamingManager: NameServer connection succeeded: corba/localhost
-Aug 16 10:19:39.493 INFO: ManagerServant: Master manager not found

From c4aa3e8de0de8a6167a6a5579c2dfd970fbc5997 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 12 Jan 2017 19:19:25 +0900
Subject: [PATCH 047/217] change to use std::map for HumanPose member

---
 rtc/AutoBalancer/AutoBalancer.cpp     |  41 ++---
 rtc/AutoBalancer/HumanMasterSlave.cpp |  31 ++--
 rtc/AutoBalancer/HumanMasterSlave.h   | 230 +++++++++++++-------------
 3 files changed, 148 insertions(+), 154 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index a028abf36a0..4e29b75ab11 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -501,13 +501,13 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     }
     //for HumanSynchronizer
     if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
-    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hp_raw_data.rfw); }
-    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hp_raw_data.lfw); }
-    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Point3DToVector3(m_htcom.data,hp_raw_data.com); }
-    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Point3DToVector3(m_htrf.data,hp_raw_data.rf); }
-    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Point3DToVector3(m_htlf.data,hp_raw_data.lf); }
-    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Point3DToVector3(m_htrh.data,hp_raw_data.rh); }
-    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Point3DToVector3(m_htlh.data,hp_raw_data.lh); }
+    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hp_raw_data.w["rfw"]); }
+    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hp_raw_data.w["lfw"]); }
+    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Point3DToVector3(m_htcom.data,hp_raw_data.P["com"].p); }
+    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Point3DToVector3(m_htrf.data,hp_raw_data.P["rf"].p); }
+    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Point3DToVector3(m_htlf.data,hp_raw_data.P["lf"].p); }
+    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Point3DToVector3(m_htrh.data,hp_raw_data.P["rh"].p); }
+    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Point3DToVector3(m_htlh.data,hp_raw_data.P["lh"].p); }
     if (m_htcamIn.isNew()){ m_htcamIn.read(); HumanSynchronizer::Pose3DToVector3(m_htcam.data,hsp->cam_pos,hsp->cam_rpy); }
     if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
     hsp->readInput(hp_raw_data);
@@ -1118,16 +1118,17 @@ void AutoBalancer::solveLimbIK ()
   }else{
     hsp->setInitOffsetPose();
     hsp->calibInitHumanCOMFromZMP();
-    hsp->rp_wld_initpos.com = m_robot->calcCM();
-    hsp->rp_wld_initpos.rf = ikp["rleg"].target_link->p;
-    hsp->rp_wld_initpos.lf = ikp["lleg"].target_link->p;
-    hsp->rp_wld_initpos.zmp = ref_zmp;
-    if(ikp.count("rarm"))hsp->rp_wld_initpos.rh = ikp["rarm"].target_link->p;
-    if(ikp.count("larm"))hsp->rp_wld_initpos.lh = ikp["larm"].target_link->p;
+    hsp->rp_wld_initpos.P["com"].p = m_robot->calcCM();
+    hsp->rp_wld_initpos.P["rf"].p = ikp["rleg"].target_link->p;
+    hsp->rp_wld_initpos.P["lf"].p = ikp["lleg"].target_link->p;
+    hsp->rp_wld_initpos.P["zmp"].p = ref_zmp;
+    if(ikp.count("rarm"))hsp->rp_wld_initpos.P["rh"].p = ikp["rarm"].target_link->p;
+    if(ikp.count("larm"))hsp->rp_wld_initpos.P["lh"].p = ikp["larm"].target_link->p;
     hsp->init_basepos = m_robot->rootLink()->p;
   }
   hsp->update();//////HumanSynchronizerの主要処理
   if(loop%100==0)hsp->rp_ref_out.print();
+//  if(loop%100==0)cout<<"time[ms]:"<<hsp->getUpdateTime()*1000<<endl;
 
   // Fix for toe joint
   for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
@@ -1145,15 +1146,15 @@ void AutoBalancer::solveLimbIK ()
   // additional COM fitting IK for HumanSynchronizer
   if(hsp->isHumanSyncOn()){
 	  solveWholeBodyCOMIK(
-	      hsp->rp_wld_initpos.com + hsp->rp_ref_out.com,
-	      hsp->rp_wld_initpos.rf + hsp->rp_ref_out.rf,
-	      hsp->rp_wld_initpos.lf + hsp->rp_ref_out.lf,
-	      hsp->rp_wld_initpos.rh + hsp->rp_ref_out.rh,
-	      hsp->rp_wld_initpos.lh + hsp->rp_ref_out.lh,
+	      hsp->rp_wld_initpos.P["com"].p + hsp->rp_ref_out.P["com"].p,
+	      hsp->rp_wld_initpos.P["rf"].p + hsp->rp_ref_out.P["rf"].p,
+	      hsp->rp_wld_initpos.P["lf"].p + hsp->rp_ref_out.P["lf"].p,
+	      hsp->rp_wld_initpos.P["rh"].p + hsp->rp_ref_out.P["rh"].p,
+	      hsp->rp_wld_initpos.P["lh"].p + hsp->rp_ref_out.P["lh"].p,
 	      hsp->cam_rpy_filtered );
     //outport用のデータ上書き
-    ref_zmp = hsp->rp_ref_out.zmp + hsp->rp_wld_initpos.zmp;
-    ref_cog = hsp->rp_ref_out.com + hsp->rp_wld_initpos.com;
+    ref_zmp = hsp->rp_ref_out.P["zmp"].p + hsp->rp_wld_initpos.P["zmp"].p;
+    ref_cog = hsp->rp_ref_out.P["com"].p + hsp->rp_wld_initpos.P["com"].p;
 
   }else{
 	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
diff --git a/rtc/AutoBalancer/HumanMasterSlave.cpp b/rtc/AutoBalancer/HumanMasterSlave.cpp
index c65a1c50d72..c2b7332327e 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.cpp
+++ b/rtc/AutoBalancer/HumanMasterSlave.cpp
@@ -1,6 +1,6 @@
 #include "HumanMasterSlave.h"
 
-void HumanSynchronizer::calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const HumanPose::Wrench6& rfwin, const HumanPose::Wrench6& lfwin, hrp::Vector3& zmp_ans){
+void HumanSynchronizer::calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const Wrench6& rfwin, const Wrench6& lfwin, hrp::Vector3& zmp_ans){
   hrp::Vector3 rfzmp,lfzmp;
   const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
   if( rfwin.f(2) > 1.0e-6 ){
@@ -114,24 +114,25 @@ bool HumanSynchronizer::isPointInHullOpenCV(const hrp::Vector2& pt, const std::v
 void HumanSynchronizer::applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
   comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
   const double MAXACC = 5;
-  if(comacc(0)>MAXACC)comacc(0)=MAXACC;else if(comacc(0)<-MAXACC)comacc(0)=-MAXACC;
-  if(comacc(1)>MAXACC)comacc(1)=MAXACC;else if(comacc(1)<-MAXACC)comacc(1)=-MAXACC;
+  LIMIT( comacc(0), -MAXACC, MAXACC);
+  LIMIT( comacc(1), -MAXACC, MAXACC);
   comacc = acc4zmp_v_filters.passFilter(comacc);
-  zmpout(0) = comin(0)-(rp_wld_initpos.com(2)/G)*comacc(0);
-  zmpout(1) = comin(1)-(rp_wld_initpos.com(2)/G)*comacc(1);
-  if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.zmp(0),rp_ref_out.zmp(1),zmpout(0),zmpout(1));
+  zmpout(0) = comin(0)-(rp_wld_initpos.P["com"].p(2)/G)*comacc(0);
+  zmpout(1) = comin(1)-(rp_wld_initpos.P["com"].p(2)/G)*comacc(1);
+  if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.P["zmp"].p(0),rp_ref_out.P["zmp"].p(1),zmpout(0),zmpout(1));
   com_oldold = com_old;
   com_old = comin;
 }
-void HumanSynchronizer::applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out){
-  for(int i=0;i<3;i++){if(in.com(i) - in_old.com(i) > MAXVEL){out.com(i) = in_old.com(i) + MAXVEL;}else if(in.com(i) - in_old.com(i) < -MAXVEL){out.com(i) = in_old.com(i) - MAXVEL;}}
-  for(int i=0;i<3;i++){if(in.rf(i)  - in_old.rf(i)  > MAXVEL){out.rf(i)  = in_old.rf(i)  + MAXVEL;}else if(in.rf(i)  - in_old.rf(i)  < -MAXVEL){out.rf(i)  = in_old.rf(i)  - MAXVEL;}}
-  for(int i=0;i<3;i++){if(in.lf(i)  - in_old.lf(i)  > MAXVEL){out.lf(i)  = in_old.lf(i)  + MAXVEL;}else if(in.lf(i)  - in_old.lf(i)  < -MAXVEL){out.lf(i)  = in_old.lf(i)  - MAXVEL;}}
-  for(int i=0;i<3;i++){if(in.rh(i)  - in_old.rh(i)  > MAXVEL){out.rh(i)  = in_old.rh(i)  + MAXVEL;}else if(in.rh(i)  - in_old.rh(i)  < -MAXVEL){out.rh(i)  = in_old.rh(i)  - MAXVEL;}}
-  for(int i=0;i<3;i++){if(in.lh(i)  - in_old.lh(i)  > MAXVEL){out.lh(i)  = in_old.lh(i)  + MAXVEL;}else if(in.lh(i)  - in_old.lh(i)  < -MAXVEL){out.lh(i)  = in_old.lh(i)  - MAXVEL;}}
+void HumanSynchronizer::applyVelLimit(/*const*/ HumanPose& in, /*const*/ HumanPose& in_old, HumanPose& out){
+  std::string names[5] = {"com","rf","lf","rh","lh"};
+  for(int i=0;i<5;i++){
+    hrp::Vector3 diff = in.P[names[i]].p  - in_old.P[names[i]].p;
+    for(int j=0;j<3;j++)LIMIT( diff(j), -MAXVEL*DT, MAXVEL*DT);
+    out.P[names[i]].p = in_old.P[names[i]].p + diff;
+  }
 }
 void HumanSynchronizer::applyCOMZMPXYZLock(HumanPose& tgt){
-    if(!use_x){tgt.com(0) = 0;tgt.zmp(0) = 0;}
-    if(!use_y){tgt.com(1) = 0;tgt.zmp(1) = 0;}
-    if(!use_z){tgt.com(2) = 0;}
+    if(!use_x){tgt.P["com"].p(0) = 0;tgt.P["zmp"].p(0) = 0;}
+    if(!use_y){tgt.P["com"].p(1) = 0;tgt.P["zmp"].p(1) = 0;}
+    if(!use_z){tgt.P["com"].p(2) = 0;}
 }
diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index b8794f5ce63..eb8fe3b6de6 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -37,6 +37,8 @@ extern "C"
 }
 #endif
 
+#define LIMIT(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
+
 //実機だとログ出力で落ちる？getenv("HOME")ダメ？
 #define DEBUG 0
 
@@ -76,64 +78,57 @@ class BiquadIIRFilterVec
 };
 
 
+class HRPPose3D{
+  public:
+    hrp::Vector3 p;
+    hrp::Matrix33 R;
+    HRPPose3D(){ clear(); }
+    ~HRPPose3D(){}
+    void clear(){
+      p = hrp::Vector3::Zero();
+      R = hrp::Matrix33::Zero();
+    }
+};
+class Wrench6{
+  public:
+    hrp::Vector3 f,t;
+    Wrench6(){ clear(); }
+    ~Wrench6(){}
+    void clear(){ f = t = hrp::Vector3::Zero(); }
+};
+
 class HumanPose{
-  private: hrp::Vector3 zero;
   public:
-    hrp::Vector3 com,rf,lf,rh,lh;
-    hrp::Vector3 zmp;
-    int idsize;
-    typedef struct{ hrp::Vector3 f; hrp::Vector3 t; }Wrench6;
-    Wrench6 rfw,lfw;
-    hrp::Vector3 neck;
-    HumanPose() : idsize(10){
+    std::map <std::string, HRPPose3D> P;
+    std::map <std::string, Wrench6> w;
+    HumanPose(){
+      P["com"] = HRPPose3D();
+      P["rf"] = HRPPose3D();
+      P["lf"] = HRPPose3D();
+      P["rh"] = HRPPose3D();
+      P["lh"] = HRPPose3D();
+      P["zmp"] = HRPPose3D();
+      P["head"] = HRPPose3D();
+      w["rfw"] = Wrench6();
+      w["lfw"] = Wrench6();
       clear();
     }
     ~HumanPose(){cerr<<"HumanPose destructed"<<endl;}
     void clear(){
-      zero = hrp::Vector3::Zero();
-      for(int i=0;i<idsize;i++){ Seq(i) = hrp::Vector3::Zero(); }
-    }
-    hrp::Vector3& Seq(const int i){
-      switch(i){
-        case 0: return com;
-        case 1: return rf;
-        case 2: return lf;
-        case 3: return rh;
-        case 4: return lh;
-        case 5: return zmp;
-        case 6: return rfw.f;
-        case 7: return rfw.t;
-        case 8: return lfw.f;
-        case 9: return lfw.t;
-        default: std::cerr <<"[WARN] HumanPose::getSeq(int) invalid index"<<std::endl;return zero;
-      }
-    }
-    const hrp::Vector3& Seq(const int i) const{//const指定オブジェクトからオーバーロードする時のためのオーバーライド
-      switch(i){
-        case 0: return com;
-        case 1: return rf;
-        case 2: return lf;
-        case 3: return rh;
-        case 4: return lh;
-        case 5: return zmp;
-        case 6: return rfw.f;
-        case 7: return rfw.t;
-        case 8: return lfw.f;
-        case 9: return lfw.t;
-        default: std::cerr <<"[WARN] HumanPose::getSeq(int) invalid index"<<std::endl;return zero;
-      }
+      for(std::map<std::string, HRPPose3D>::iterator it = P.begin(); it != P.end(); it++){ it->second.clear(); }
+      for(std::map<std::string, Wrench6>::iterator it = w.begin(); it != w.end(); it++){ it->second.clear(); }
     }
     static void hp_printf(HumanPose& in){
-      const std::string cap[] = {"c","rf","lf","rh","lh","z","rw","","lw",""};
-      for(int i=0;i<6;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.Seq(i)(0),in.Seq(i)(1),in.Seq(i)(2)); }
-      for(int i=6;i<10;i++){ printf("\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.Seq(i)(0),in.Seq(i)(1),in.Seq(i)(2)); }
+      const std::string ns[] = {"com","rf","lf","rh","lh","zmp","rfw","lfw"};
+      const std::string cap[] = {"c","rf","lf","rh","lh","z","rw","lw"};
+      for(int i=0;i<6;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.P[ns[i]].p(0),in.P[ns[i]].p(1),in.P[ns[i]].p(2)); }
+      for(int i=6;i<8;i++){ printf("\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.w[ns[i]].f(0),in.w[ns[i]].f(1),in.w[ns[i]].f(2),in.w[ns[i]].t(0),in.w[ns[i]].t(1),in.w[ns[i]].t(2)); }
       printf("\n");
     }
     void print(){ hp_printf(*this); }
 };
 
 
-
 class HumanSynchronizer{
   private:
     double FUP_TIME;
@@ -201,7 +196,7 @@ class HumanSynchronizer{
       tgt_FUP_TIME = FUP_TIME = 0.4;
       FUP_HIGHT = 0.05;
       CNT_F_TH = 20.0;
-      MAXVEL = 0.004;
+      MAXVEL = 2.0;
       HumanSyncOn = false;
       ht_first_call = true;
       startCountdownForHumanSync = false;
@@ -282,32 +277,29 @@ class HumanSynchronizer{
       removeInitOffsetPose                (hp_wld_raw, hp_wld_initpos, hp_rel_raw);
       applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);
 //      lockFootXYOnContact                 (go_rf_landing, go_lf_landing, hp_rel_raw);//根本から改変すべき2
-      calcWorldZMP                        ((hp_rel_raw.rf+init_wld_hp_rfpos), (hp_rel_raw.lf+init_wld_hp_lfpos), hp_rel_raw.rfw, hp_rel_raw.lfw, hp_rel_raw.zmp);//足の位置はworldにしないと・・・
+      calcWorldZMP                        ((hp_rel_raw.P["rf"].p+init_wld_hp_rfpos), (hp_rel_raw.P["lf"].p+init_wld_hp_lfpos), hp_rel_raw.w["rfw"], hp_rel_raw.w["lfw"], hp_rel_raw.P["zmp"].p);//足の位置はworldにしないと・・・
       convertRelHumanPoseToRelRobotPose   (hp_rel_raw, rp_ref_out);
+
       applyCOMMoveModRatio                (rp_ref_out);
-      judgeFootLandOnCommand              (hp_wld_raw.rfw, hp_wld_raw.lfw, go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
+      judgeFootLandOnCommand              (hp_wld_raw.w["rfw"], hp_wld_raw.w["lfw"], go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
 
       applyEEWorkspaceLimit               (rp_ref_out);
       lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
 
-      applyCOMToSupportRegionLimit        (rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
+      applyCOMToSupportRegionLimit        (rp_ref_out.P["rf"].p+init_wld_rp_rfeepos, rp_ref_out.P["lf"].p+init_wld_rp_lfeepos, rp_ref_out.P["com"].p);
       applyLPFilter                       (rp_ref_out);
-      applyCOMStateLimitByCapturePoint    (rp_ref_out.com, rp_ref_out_old.com, com_vel_old, rp_ref_out.rf+init_wld_rp_rfeepos, rp_ref_out.lf+init_wld_rp_lfeepos, rp_ref_out.com);
+      applyCOMStateLimitByCapturePoint    (rp_ref_out.P["com"].p, rp_ref_out_old.P["com"].p, com_vel_old, rp_ref_out.P["rf"].p+init_wld_rp_rfeepos, rp_ref_out.P["lf"].p+init_wld_rp_lfeepos, rp_ref_out.P["com"].p);
 
-      if(cam_rpy(1) > 20*M_PI/180)cam_rpy(1) = 20*M_PI/180;
-      if(cam_rpy(1) < -20*M_PI/180)cam_rpy(1) = -20*M_PI/180;
-      if(cam_rpy(2) > 20*M_PI/180)cam_rpy(2) = 20*M_PI/180;
-      if(cam_rpy(2) < -20*M_PI/180)cam_rpy(2) = -20*M_PI/180;
       cam_rpy_filtered = cam_rpy_filter.passFilter(cam_rpy);
 
-      r_zmp_raw = rp_ref_out.zmp;
-      applyZMPCalcFromCOM                 (rp_ref_out.com,rp_ref_out.zmp);
+      r_zmp_raw = rp_ref_out.P["zmp"].p;
+      applyZMPCalcFromCOM                 (rp_ref_out.P["com"].p,rp_ref_out.P["zmp"].p);
 //      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, go_rf_landing, go_lf_landing);//ここじゃ判定遅い
       applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       applyCOMZMPXYZLock                  (rp_ref_out);
       overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
-      com_vel_old = (rp_ref_out.com - rp_ref_out_old.com)/DT;
+      com_vel_old = (rp_ref_out.P["com"].p - rp_ref_out_old.P["com"].p)/DT;
       rp_ref_out_old = rp_ref_out;
       loop++;
       gettimeofday(&t_calc_end, NULL);
@@ -319,9 +311,9 @@ class HumanSynchronizer{
     static void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out(0) = in.x; out(1) = in.y; out(2) = in.z;}
     static void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out(0) = in.r; out(1) = in.p; out(2) = in.y;}
     static void Pose3DToVector3(const RTC::Pose3D& in, hrp::Vector3& out_pos, hrp::Vector3& out_rpy){ Point3DToVector3(in.position,out_pos); Oriantation3DToVector3(in.orientation,out_rpy); }
-    static void DoubleSeqToWrench6(const RTC::TimedDoubleSeq::_data_seq& in, HumanPose::Wrench6& out){
+    static void DoubleSeqToWrench6(const RTC::TimedDoubleSeq::_data_seq& in, Wrench6& out){
       if(in.length() == 6){out.f(0)=in[0]; out.f(1)=in[1]; out.f(2)=in[2]; out.t(0)=in[3]; out.t(1)=in[4]; out.t(2)=in[5];
-      }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; out.f.Zero(); out.t.Zero(); }
+      }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; out.clear(); }
     }
     static double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(0)*b(1)-a(1)*b(0); }
 
@@ -340,14 +332,13 @@ class HumanSynchronizer{
       else if(cmmr_goal - cmmr < (-1)*step){cmmr -= step; cout<<"Interpolating COMMoveModRatio as "<<cmmr<<endl;}
       else{cmmr = cmmr_goal;}
     };
-    void removeInitOffsetPose(const HumanPose& abs_in, const HumanPose& init_offset, HumanPose& rel_out){
-      for(int i=0;i<rel_out.idsize-5;i++){
-        rel_out.Seq(i) =  abs_in.Seq(i) - init_offset.Seq(i);
-      }
-      rel_out.rfw = abs_in.rfw;
-      rel_out.lfw = abs_in.lfw;
+    void removeInitOffsetPose(/*const*/ HumanPose& abs_in, /*const*/ HumanPose& init_offset, HumanPose& rel_out){
+      std::string ns[5] = {"com","rf","lf","rh","lh"};
+      for(int i=0;i<5;i++){ rel_out.P[ns[i]].p = abs_in.P[ns[i]].p - init_offset.P[ns[i]].p; }
+      rel_out.w["rfw"] = abs_in.w["rfw"];
+      rel_out.w["lfw"] = abs_in.w["lfw"];
     }
-    void applyInitHumanZMPToCOMOffset(const HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
+    void applyInitHumanZMPToCOMOffset(/*const*/ HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
       if(isHumanSyncOn()){
         if(!init_zmp_com_offset_interpolator->isEmpty()){
           init_zmp_com_offset_interpolator->get(&zmp_com_offset_ip_ratio, true);
@@ -356,22 +347,22 @@ class HumanSynchronizer{
         zmp_com_offset_ip_ratio = 0;
       }
       out = in;
-      out.com = in.com + zmp_com_offset_ip_ratio * com_offs;
+      out.P["com"].p = in.P["com"].p + zmp_com_offset_ip_ratio * com_offs;
     }
     void applyLPFilter(HumanPose& tgt){
-      for(int i=0;i<iir_v_filters.size();i++){ tgt.Seq(i) = iir_v_filters[i].passFilter(tgt.Seq(i)); }
+      std::string ns[5] = {"com","rf","lf","rh","lh"};
+      for(int i=0;i<iir_v_filters.size();i++){ tgt.P[ns[i]].p = iir_v_filters[i].passFilter(tgt.P[ns[i]].p); }
     }
-    void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
-      for(int i=0;i<rp_out.idsize-4;i++){
-        rp_out.Seq(i) =  h2r_ratio * hp_in.Seq(i);
-      }
-      rp_out.rfw  = hp_in.rfw;
-      rp_out.lfw  = hp_in.lfw;
+    void convertRelHumanPoseToRelRobotPose(/*const*/ HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
+      std::string ns[6] = {"com","rf","lf","rh","lh","zmp"};
+      for(int i=0;i<6;i++){ rp_out.P[ns[i]].p =  h2r_ratio * hp_in.P[ns[i]].p; }
+      rp_out.w["rfw"]  = hp_in.w["rfw"];
+      rp_out.w["lfw"]  = hp_in.w["lfw"];
     }
     void applyCOMMoveModRatio(HumanPose& tgt){//
-      tgt.com = cmmr * tgt.com;
+      tgt.P["com"].p = cmmr * tgt.P["com"].p;
     }
-    void judgeFootLandOnCommand(const HumanPose::Wrench6& rfw_in, const HumanPose::Wrench6& lfw_in, bool& rfloc_ans, bool& lfloc_ans){
+    void judgeFootLandOnCommand(const Wrench6& rfw_in, const Wrench6& lfw_in, bool& rfloc_ans, bool& lfloc_ans){
       if(HumanSyncOn){
         if      (rfloc_ans  && rfw_in.f(2)<CNT_F_TH   ){rfloc_ans = false;}//右足ついた状態から上げる
         else if (!rfloc_ans && rfw_in.f(2)>CNT_F_TH+30){rfloc_ans = true;}//右足浮いた状態から下げる
@@ -380,15 +371,15 @@ class HumanSynchronizer{
       }
       if(!rfloc_ans && !lfloc_ans){rfloc_ans = lfloc_ans = true;}//両足ジャンプは禁止
     }
-    void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
+    void lockSwingFootIfZMPOutOfSupportFoot(/*const*/ HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
       if(rfcs_ans && !lfcs_ans){
-        if(tgt.zmp(1) > tgt.rf(1) + init_wld_rp_rfeepos(1) + 0.02){
+        if(tgt.P["zmp"].p(1) > tgt.P["rf"].p(1) + init_wld_rp_rfeepos(1) + 0.03){
           lfcs_ans = true;
           if(loop%100==0)cout<<"LF Auto lock"<<endl;
         }
       }
       if(!rfcs_ans && lfcs_ans){
-        if(tgt.zmp(1) < tgt.lf(1) + init_wld_rp_lfeepos(1) - 0.02){
+        if(tgt.P["zmp"].p(1) < tgt.P["lf"].p(1) + init_wld_rp_lfeepos(1) - 0.03){
           rfcs_ans = true;
           if(loop%100==0)cout<<"RF Auto lock"<<endl;
         }
@@ -396,23 +387,12 @@ class HumanSynchronizer{
     }
     void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
       if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足接地しているタイミングでのみ更新
-      double RFDOWN_TIME = fabs(tgt.rf(1) - tgt.com(1))*4 + FUP_TIME;//下ろす速度は遊脚の重心からの距離に比例したペナルティ
-      double LFDOWN_TIME = fabs(tgt.lf(1) - tgt.com(1))*4 + FUP_TIME;
-      calcFootUpCurveAndJudgeFootContact2(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.rf, is_rf_contact);
-      calcFootUpCurveAndJudgeFootContact2(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.lf, is_lf_contact);
+      double RFDOWN_TIME = fabs(tgt.P["rf"].p(1) - tgt.P["com"].p(1))*2 + FUP_TIME;//下ろす速度は遊脚の重心からの距離に比例したペナルティ
+      double LFDOWN_TIME = fabs(tgt.P["lf"].p(1) - tgt.P["com"].p(1))*2 + FUP_TIME;
+      calcFootUpCurveAndJudgeFootContact(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.P["rf"].p, is_rf_contact);
+      calcFootUpCurveAndJudgeFootContact(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.P["lf"].p, is_lf_contact);
     }
-    void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, int& cur_fup_count_in, hrp::Vector3& f_height_out, bool& is_f_contact_out){
-      int FUP_COUNT = (int)(HZ*FUP_TIME);
-      if(cur_fup_count_in > FUP_COUNT){cur_fup_count_in = FUP_COUNT;} if(cur_fup_count_in < 0){cur_fup_count_in = 0;}
-      if(!go_land_in){//足上げ命令入力時
-        if(cur_fup_count_in < FUP_COUNT){ cur_fup_count_in++; }
-      }else{//足下げ命令入力時
-        if(cur_fup_count_in > 0){ cur_fup_count_in--; }
-      }
-      f_height_out(2) = FUP_HIGHT/2*(1-cos(M_PI*cur_fup_count_in/FUP_COUNT));
-      is_f_contact_out = (cur_fup_count_in <= 0);
-    }
-    void calcFootUpCurveAndJudgeFootContact2(const bool& go_land_in, const double& fup_time_in, const double& fdown_time_in,  double& cur_fup_phase_in,hrp::Vector3& f_height_out, bool& is_f_contact_out){
+    void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, const double& fup_time_in, const double& fdown_time_in,  double& cur_fup_phase_in,hrp::Vector3& f_height_out, bool& is_f_contact_out){
       const double omega_up = M_PI / fup_time_in;
       const double omega_down = M_PI / fdown_time_in;
       if(!go_land_in){//足上げ命令入力時
@@ -425,48 +405,60 @@ class HumanSynchronizer{
       f_height_out(2) = FUP_HIGHT/2*(1-cos(cur_fup_phase_in));
       is_f_contact_out = (cur_fup_phase_in <= 0);
     }
+//    void calcFootUpCurveAndJudgeFootContact_o(const bool& go_land_in, int& cur_fup_count_in, hrp::Vector3& f_height_out, bool& is_f_contact_out){
+//      int FUP_COUNT = (int)(HZ*FUP_TIME);
+//      if(cur_fup_count_in > FUP_COUNT){cur_fup_count_in = FUP_COUNT;} if(cur_fup_count_in < 0){cur_fup_count_in = 0;}
+//      if(!go_land_in){//足上げ命令入力時
+//        if(cur_fup_count_in < FUP_COUNT){ cur_fup_count_in++; }
+//      }else{//足下げ命令入力時
+//        if(cur_fup_count_in > 0){ cur_fup_count_in--; }
+//      }
+//      f_height_out(2) = FUP_HIGHT/2*(1-cos(M_PI*cur_fup_count_in/FUP_COUNT));
+//      is_f_contact_out = (cur_fup_count_in <= 0);
+//    }
     void lockFootXYOnContact(const bool& rfcs_in, const bool& lfcs_in, HumanPose& tgt){
       if(rfcs_in){
-        tgt.rf(0) = pre_cont_rfpos(0);
-        tgt.rf(1) = pre_cont_rfpos(1);
+        tgt.P["rf"].p(0) = pre_cont_rfpos(0);
+        tgt.P["rf"].p(1) = pre_cont_rfpos(1);
       }else{
-        pre_cont_rfpos(0) = tgt.rf(0);
-        pre_cont_rfpos(1) = tgt.rf(1);
+        pre_cont_rfpos(0) = tgt.P["rf"].p(0);
+        pre_cont_rfpos(1) = tgt.P["rf"].p(1);
       }
       if(lfcs_in){
-        tgt.lf(0) = pre_cont_lfpos(0);
-        tgt.lf(1) = pre_cont_lfpos(1);
+        tgt.P["lf"].p(0) = pre_cont_lfpos(0);
+        tgt.P["lf"].p(1) = pre_cont_lfpos(1);
       }else{
-        pre_cont_lfpos(0) = tgt.lf(0);
-        pre_cont_lfpos(1) = tgt.lf(1);
+        pre_cont_lfpos(0) = tgt.P["lf"].p(0);
+        pre_cont_lfpos(1) = tgt.P["lf"].p(1);
       }
     }
     void applyEEWorkspaceLimit(HumanPose& tgt){
       const double MAX = 0.15;
       if(!is_rf_contact && is_lf_contact){//右足浮遊時
-        const hrp::Vector2 lf2rf_vec( tgt.rf(0)-pre_cont_lfpos(0), tgt.rf(1)-pre_cont_lfpos(1) );
+        const hrp::Vector2 lf2rf_vec( tgt.P["rf"].p(0)-pre_cont_lfpos(0), tgt.P["rf"].p(1)-pre_cont_lfpos(1) );
         if(lf2rf_vec.norm() > MAX){
-          tgt.rf(0) = (lf2rf_vec.normalized())(0) * MAX + pre_cont_lfpos(0);
-          tgt.rf(1) = (lf2rf_vec.normalized())(1) * MAX + pre_cont_lfpos(1);
+          tgt.P["rf"].p(0) = (lf2rf_vec.normalized())(0) * MAX + pre_cont_lfpos(0);
+          tgt.P["rf"].p(1) = (lf2rf_vec.normalized())(1) * MAX + pre_cont_lfpos(1);
         }
-        if(tgt.rf(1) > pre_cont_lfpos(1) + 0.05 ) tgt.rf(1) = pre_cont_lfpos(1) + 0.05;
+        if(tgt.P["rf"].p(1) > pre_cont_lfpos(1) + 0.05 ) tgt.P["rf"].p(1) = pre_cont_lfpos(1) + 0.05;
       }
       else if(is_rf_contact && !is_lf_contact){//左足浮遊時
-        const hrp::Vector2 rf2lf_vec( tgt.lf(0)-pre_cont_rfpos(0), tgt.lf(1)-pre_cont_rfpos(1) );
+        const hrp::Vector2 rf2lf_vec( tgt.P["lf"].p(0)-pre_cont_rfpos(0), tgt.P["lf"].p(1)-pre_cont_rfpos(1) );
         if(rf2lf_vec.norm() > MAX){
-          tgt.lf(0) = (rf2lf_vec.normalized())(0) * MAX + pre_cont_rfpos(0);
-          tgt.lf(1) = (rf2lf_vec.normalized())(1) * MAX + pre_cont_rfpos(1);
+          tgt.P["lf"].p(0) = (rf2lf_vec.normalized())(0) * MAX + pre_cont_rfpos(0);
+          tgt.P["lf"].p(1) = (rf2lf_vec.normalized())(1) * MAX + pre_cont_rfpos(1);
         }
-        if(tgt.lf(1) < pre_cont_rfpos(1) - 0.05 ) tgt.lf(1) = pre_cont_rfpos(1) - 0.05;
+        if(tgt.P["lf"].p(1) < pre_cont_rfpos(1) - 0.05 ) tgt.P["lf"].p(1) = pre_cont_rfpos(1) - 0.05;
       }
       hrp::Vector3 hand_ulimit = hrp::Vector3(0.1,0.1,0.1);
       hrp::Vector3 hand_llimit = hrp::Vector3(-0.1,-0.1,-0.1);
       for(int i=0;i<3;i++){
-        if      (tgt.rh(i) > hand_ulimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.rh(i) = hand_ulimit(i) + current_basepos(i) - init_basepos(i); }
-        else if (tgt.rh(i) < hand_llimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.rh(i) = hand_llimit(i) + current_basepos(i) - init_basepos(i); }
-        if      (tgt.lh(i) > hand_ulimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.lh(i) = hand_ulimit(i) + current_basepos(i) - init_basepos(i); }
-        else if (tgt.lh(i) < hand_llimit(i) + current_basepos(i) - init_basepos(i) ){ tgt.lh(i) = hand_llimit(i) + current_basepos(i) - init_basepos(i); }
+        LIMIT( tgt.P["rh"].p(i), hand_llimit(i) + current_basepos(i) - init_basepos(i), hand_ulimit(i) + current_basepos(i) - init_basepos(i) );
+        LIMIT( tgt.P["lh"].p(i), hand_llimit(i) + current_basepos(i) - init_basepos(i), hand_ulimit(i) + current_basepos(i) - init_basepos(i) );
       }
+      LIMIT( cam_rpy(1), -20*M_PI/180, 20*M_PI/180 );
+      LIMIT( cam_rpy(2), -20*M_PI/180, 20*M_PI/180 );
+
     }
     bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
       std::vector<hrp::Vector2> hull;
@@ -487,7 +479,7 @@ class HumanSynchronizer{
       double accel[4] = {0.05, -0.03, 0.02, -0.02};
       double decel[4] = {0.04, -0.02, 0.01, -0.01};
       comacc_ref = calcacc_v_filters.passFilter(com_acc);
-      const double H = rp_wld_initpos.com(2);
+      const double H = rp_wld_initpos.P["com"].p(2);
 //      double maxacc[2],minacc[2];
 //      maxacc[0] = (com_old(0) - accel[1]) * (G/H); if(maxacc[0] < 0){maxacc[0]=0;}
 //      minacc[0] = (com_old(0) - accel[0]) * (G/H); if(minacc[0] > 0){minacc[0]=0;}
@@ -514,7 +506,7 @@ class HumanSynchronizer{
     }
     void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
       com_vel_ans = com_vel;
-      const double H = rp_wld_initpos.com(2);
+      const double H = rp_wld_initpos.P["com"].p(2);
       hrp::Vector2 cp = com_pos + com_vel * sqrt( H / G );
       hrp::Vector2 cp_ragulated;
       if(!isPointInHullOpenCV(cp,hull)){
@@ -525,7 +517,7 @@ class HumanSynchronizer{
     }
     void regulateCOMVelocityByCapturePointXY(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
       com_vel_ans = com_vel;
-      const double H = rp_wld_initpos.com(2);
+      const double H = rp_wld_initpos.P["com"].p(2);
       hrp::Vector4 margin_from_4maxcp;
       calcXYMarginToHull(com_pos, hull, margin_from_4maxcp);
       hrp::Vector3 max_com_vel,min_com_vel;
@@ -538,7 +530,7 @@ class HumanSynchronizer{
       if(com_vel(1) > max_com_vel(1)){ com_vel_ans(1) = max_com_vel(1); if(loop%100==0)cout<<"L cp over"<<endl;}
       if(com_vel(1) < min_com_vel(1)){ com_vel_ans(1) = min_com_vel(1); if(loop%100==0)cout<<"R cp over"<<endl;}
     }
-    void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const HumanPose::Wrench6& rfwin, const HumanPose::Wrench6& lfwin, hrp::Vector3& zmp_ans);
+    void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const Wrench6& rfwin, const Wrench6& lfwin, hrp::Vector3& zmp_ans);
     void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans);
     void calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans);
     bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_will_cross);
@@ -547,7 +539,7 @@ class HumanSynchronizer{
     void makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans);
     bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull);
     void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout);
-    void applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out);
+    void applyVelLimit(/*const*/ HumanPose& in, /*const*/ HumanPose& in_old, HumanPose& out);
     void applyCOMZMPXYZLock(HumanPose& tgt);
 };
 

From f8e30bbe00f47a12037129b5d7ba9c688576ab88 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 12 Jan 2017 21:29:58 +0900
Subject: [PATCH 048/217] add safe getter

---
 rtc/AutoBalancer/AutoBalancer.cpp     |  40 ++++----
 rtc/AutoBalancer/HumanMasterSlave.cpp |  26 ++---
 rtc/AutoBalancer/HumanMasterSlave.h   | 138 ++++++++++++++------------
 3 files changed, 105 insertions(+), 99 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 4e29b75ab11..4d288d8e455 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -501,13 +501,13 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     }
     //for HumanSynchronizer
     if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
-    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hp_raw_data.w["rfw"]); }
-    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hp_raw_data.w["lfw"]); }
-    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Point3DToVector3(m_htcom.data,hp_raw_data.P["com"].p); }
-    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Point3DToVector3(m_htrf.data,hp_raw_data.P["rf"].p); }
-    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Point3DToVector3(m_htlf.data,hp_raw_data.P["lf"].p); }
-    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Point3DToVector3(m_htrh.data,hp_raw_data.P["rh"].p); }
-    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Point3DToVector3(m_htlh.data,hp_raw_data.P["lh"].p); }
+    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hp_raw_data.getw("rfw")); }
+    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hp_raw_data.getw("lfw")); }
+    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Point3DToVector3(m_htcom.data,hp_raw_data.getP("com").p); }
+    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Point3DToVector3(m_htrf.data,hp_raw_data.getP("rf").p); }
+    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Point3DToVector3(m_htlf.data,hp_raw_data.getP("lf").p); }
+    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Point3DToVector3(m_htrh.data,hp_raw_data.getP("rh").p); }
+    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Point3DToVector3(m_htlh.data,hp_raw_data.getP("lh").p); }
     if (m_htcamIn.isNew()){ m_htcamIn.read(); HumanSynchronizer::Pose3DToVector3(m_htcam.data,hsp->cam_pos,hsp->cam_rpy); }
     if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
     hsp->readInput(hp_raw_data);
@@ -1118,12 +1118,12 @@ void AutoBalancer::solveLimbIK ()
   }else{
     hsp->setInitOffsetPose();
     hsp->calibInitHumanCOMFromZMP();
-    hsp->rp_wld_initpos.P["com"].p = m_robot->calcCM();
-    hsp->rp_wld_initpos.P["rf"].p = ikp["rleg"].target_link->p;
-    hsp->rp_wld_initpos.P["lf"].p = ikp["lleg"].target_link->p;
-    hsp->rp_wld_initpos.P["zmp"].p = ref_zmp;
-    if(ikp.count("rarm"))hsp->rp_wld_initpos.P["rh"].p = ikp["rarm"].target_link->p;
-    if(ikp.count("larm"))hsp->rp_wld_initpos.P["lh"].p = ikp["larm"].target_link->p;
+    hsp->rp_wld_initpos.getP("com").p = m_robot->calcCM();
+    hsp->rp_wld_initpos.getP("rf").p = ikp["rleg"].target_link->p;
+    hsp->rp_wld_initpos.getP("lf").p = ikp["lleg"].target_link->p;
+    hsp->rp_wld_initpos.getP("zmp").p = ref_zmp;
+    if(ikp.count("rarm"))hsp->rp_wld_initpos.getP("rh").p = ikp["rarm"].target_link->p;
+    if(ikp.count("larm"))hsp->rp_wld_initpos.getP("lh").p = ikp["larm"].target_link->p;
     hsp->init_basepos = m_robot->rootLink()->p;
   }
   hsp->update();//////HumanSynchronizerの主要処理
@@ -1146,15 +1146,15 @@ void AutoBalancer::solveLimbIK ()
   // additional COM fitting IK for HumanSynchronizer
   if(hsp->isHumanSyncOn()){
 	  solveWholeBodyCOMIK(
-	      hsp->rp_wld_initpos.P["com"].p + hsp->rp_ref_out.P["com"].p,
-	      hsp->rp_wld_initpos.P["rf"].p + hsp->rp_ref_out.P["rf"].p,
-	      hsp->rp_wld_initpos.P["lf"].p + hsp->rp_ref_out.P["lf"].p,
-	      hsp->rp_wld_initpos.P["rh"].p + hsp->rp_ref_out.P["rh"].p,
-	      hsp->rp_wld_initpos.P["lh"].p + hsp->rp_ref_out.P["lh"].p,
+	      hsp->rp_wld_initpos.getP("com").p + hsp->rp_ref_out.getP("com").p,
+	      hsp->rp_wld_initpos.getP("rf").p + hsp->rp_ref_out.getP("rf").p,
+	      hsp->rp_wld_initpos.getP("lf").p + hsp->rp_ref_out.getP("lf").p,
+	      hsp->rp_wld_initpos.getP("rh").p + hsp->rp_ref_out.getP("rh").p,
+	      hsp->rp_wld_initpos.getP("lh").p + hsp->rp_ref_out.getP("lh").p,
 	      hsp->cam_rpy_filtered );
     //outport用のデータ上書き
-    ref_zmp = hsp->rp_ref_out.P["zmp"].p + hsp->rp_wld_initpos.P["zmp"].p;
-    ref_cog = hsp->rp_ref_out.P["com"].p + hsp->rp_wld_initpos.P["com"].p;
+    ref_zmp = hsp->rp_ref_out.getP("zmp").p + hsp->rp_wld_initpos.getP("zmp").p;
+    ref_cog = hsp->rp_ref_out.getP("com").p + hsp->rp_wld_initpos.getP("com").p;
 
   }else{
 	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
diff --git a/rtc/AutoBalancer/HumanMasterSlave.cpp b/rtc/AutoBalancer/HumanMasterSlave.cpp
index c2b7332327e..9a2d7fd2db2 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.cpp
+++ b/rtc/AutoBalancer/HumanMasterSlave.cpp
@@ -114,25 +114,25 @@ bool HumanSynchronizer::isPointInHullOpenCV(const hrp::Vector2& pt, const std::v
 void HumanSynchronizer::applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
   comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
   const double MAXACC = 5;
-  LIMIT( comacc(0), -MAXACC, MAXACC);
-  LIMIT( comacc(1), -MAXACC, MAXACC);
+  LIMIT_MINMAX( comacc(0), -MAXACC, MAXACC);
+  LIMIT_MINMAX( comacc(1), -MAXACC, MAXACC);
   comacc = acc4zmp_v_filters.passFilter(comacc);
-  zmpout(0) = comin(0)-(rp_wld_initpos.P["com"].p(2)/G)*comacc(0);
-  zmpout(1) = comin(1)-(rp_wld_initpos.P["com"].p(2)/G)*comacc(1);
-  if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.P["zmp"].p(0),rp_ref_out.P["zmp"].p(1),zmpout(0),zmpout(1));
+  zmpout(0) = comin(0)-(rp_wld_initpos.getP("com").p(2)/G)*comacc(0);
+  zmpout(1) = comin(1)-(rp_wld_initpos.getP("com").p(2)/G)*comacc(1);
+  if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.getP("zmp").p(0),rp_ref_out.getP("zmp").p(1),zmpout(0),zmpout(1));
   com_oldold = com_old;
   com_old = comin;
 }
-void HumanSynchronizer::applyVelLimit(/*const*/ HumanPose& in, /*const*/ HumanPose& in_old, HumanPose& out){
-  std::string names[5] = {"com","rf","lf","rh","lh"};
+void HumanSynchronizer::applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out){
+  std::string ns[5] = {"com","rf","lf","rh","lh"};
   for(int i=0;i<5;i++){
-    hrp::Vector3 diff = in.P[names[i]].p  - in_old.P[names[i]].p;
-    for(int j=0;j<3;j++)LIMIT( diff(j), -MAXVEL*DT, MAXVEL*DT);
-    out.P[names[i]].p = in_old.P[names[i]].p + diff;
+    hrp::Vector3 diff = in.P.find(ns[i])->second.p  - in_old.P.find(ns[i])->second.p;
+    for(int j=0;j<3;j++)LIMIT_MINMAX( diff(j), -MAXVEL*DT, MAXVEL*DT);
+    out.P.find(ns[i])->second.p = in_old.P.find(ns[i])->second.p + diff;
   }
 }
 void HumanSynchronizer::applyCOMZMPXYZLock(HumanPose& tgt){
-    if(!use_x){tgt.P["com"].p(0) = 0;tgt.P["zmp"].p(0) = 0;}
-    if(!use_y){tgt.P["com"].p(1) = 0;tgt.P["zmp"].p(1) = 0;}
-    if(!use_z){tgt.P["com"].p(2) = 0;}
+    if(!use_x){tgt.getP("com").p(0) = 0;tgt.getP("zmp").p(0) = 0;}
+    if(!use_y){tgt.getP("com").p(1) = 0;tgt.getP("zmp").p(1) = 0;}
+    if(!use_z){tgt.getP("com").p(2) = 0;}
 }
diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index eb8fe3b6de6..33ff80c0743 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -37,7 +37,7 @@ extern "C"
 }
 #endif
 
-#define LIMIT(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
+#define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
 
 //実機だとログ出力で落ちる？getenv("HOME")ダメ？
 #define DEBUG 0
@@ -118,13 +118,29 @@ class HumanPose{
       for(std::map<std::string, HRPPose3D>::iterator it = P.begin(); it != P.end(); it++){ it->second.clear(); }
       for(std::map<std::string, Wrench6>::iterator it = w.begin(); it != w.end(); it++){ it->second.clear(); }
     }
-    static void hp_printf(HumanPose& in){
+    static void hp_printf(const HumanPose& in){
       const std::string ns[] = {"com","rf","lf","rh","lh","zmp","rfw","lfw"};
       const std::string cap[] = {"c","rf","lf","rh","lh","z","rw","lw"};
-      for(int i=0;i<6;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.P[ns[i]].p(0),in.P[ns[i]].p(1),in.P[ns[i]].p(2)); }
-      for(int i=6;i<8;i++){ printf("\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.w[ns[i]].f(0),in.w[ns[i]].f(1),in.w[ns[i]].f(2),in.w[ns[i]].t(0),in.w[ns[i]].t(1),in.w[ns[i]].t(2)); }
+      for(int i=0;i<6;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.getP(ns[i]).p(0),in.getP(ns[i]).p(1),in.getP(ns[i]).p(2)); }
+      for(int i=6;i<8;i++){ printf("\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.getw(ns[i]).f(0),in.getw(ns[i]).f(1),in.getw(ns[i]).f(2),in.getw(ns[i]).t(0),in.getw(ns[i]).t(1),in.getw(ns[i]).t(2)); }
       printf("\n");
     }
+    HRPPose3D& getP(const std::string& key){///Safe accessor
+      if (P.find(key) != P.end() ) { return P.find(key)->second; }
+      else { cerr<<"\n\x1b[31m[FATAL] HumanPose Invalid Key \""<<key<<"\" !!\x1b[39m\n"<<endl; exit(-1); return P.end()->second; }
+    }
+    const HRPPose3D& getP(const std::string& key) const {
+      if (P.find(key) != P.end() ) { return P.find(key)->second; }
+      else { cerr<<"\n\x1b[31m[FATAL] HumanPose Invalid Key \""<<key<<"\" !!\x1b[39m\n"<<endl; exit(-1); return P.end()->second; }
+    }
+    Wrench6& getw(const std::string& key){
+      if (w.find(key) != w.end() ) { return w.find(key)->second; }
+      else { cerr<<"\n\x1b[31m[FATAL] HumanPose Invalid Key \""<<key<<"\" !!\x1b[39m\n"<<endl; exit(-1); return w.end()->second; }
+    }
+    const Wrench6& getw(const std::string& key) const {
+      if (w.find(key) != w.end() ) { return w.find(key)->second; }
+      else { cerr<<"\n\x1b[31m[FATAL] HumanPose Invalid Key \""<<key<<"\" !!\x1b[39m\n"<<endl; exit(-1); return w.end()->second; }
+    }
     void print(){ hp_printf(*this); }
 };
 
@@ -277,29 +293,29 @@ class HumanSynchronizer{
       removeInitOffsetPose                (hp_wld_raw, hp_wld_initpos, hp_rel_raw);
       applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);
 //      lockFootXYOnContact                 (go_rf_landing, go_lf_landing, hp_rel_raw);//根本から改変すべき2
-      calcWorldZMP                        ((hp_rel_raw.P["rf"].p+init_wld_hp_rfpos), (hp_rel_raw.P["lf"].p+init_wld_hp_lfpos), hp_rel_raw.w["rfw"], hp_rel_raw.w["lfw"], hp_rel_raw.P["zmp"].p);//足の位置はworldにしないと・・・
+      calcWorldZMP                        ((hp_rel_raw.getP("rf").p+init_wld_hp_rfpos), (hp_rel_raw.getP("lf").p+init_wld_hp_lfpos), hp_rel_raw.getw("rfw"), hp_rel_raw.getw("lfw"), hp_rel_raw.getP("zmp").p);//足の位置はworldにしないと・・・
       convertRelHumanPoseToRelRobotPose   (hp_rel_raw, rp_ref_out);
 
       applyCOMMoveModRatio                (rp_ref_out);
-      judgeFootLandOnCommand              (hp_wld_raw.w["rfw"], hp_wld_raw.w["lfw"], go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
+      judgeFootLandOnCommand              (hp_wld_raw.getw("rfw"), hp_wld_raw.getw("lfw"), go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
 
       applyEEWorkspaceLimit               (rp_ref_out);
       lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
 
-      applyCOMToSupportRegionLimit        (rp_ref_out.P["rf"].p+init_wld_rp_rfeepos, rp_ref_out.P["lf"].p+init_wld_rp_lfeepos, rp_ref_out.P["com"].p);
+      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p+init_wld_rp_rfeepos, rp_ref_out.getP("lf").p+init_wld_rp_lfeepos, rp_ref_out.getP("com").p);
       applyLPFilter                       (rp_ref_out);
-      applyCOMStateLimitByCapturePoint    (rp_ref_out.P["com"].p, rp_ref_out_old.P["com"].p, com_vel_old, rp_ref_out.P["rf"].p+init_wld_rp_rfeepos, rp_ref_out.P["lf"].p+init_wld_rp_lfeepos, rp_ref_out.P["com"].p);
+      applyCOMStateLimitByCapturePoint    (rp_ref_out.getP("com").p, rp_ref_out_old.getP("com").p, com_vel_old, rp_ref_out.getP("rf").p+init_wld_rp_rfeepos, rp_ref_out.getP("lf").p+init_wld_rp_lfeepos, rp_ref_out.getP("com").p);
 
       cam_rpy_filtered = cam_rpy_filter.passFilter(cam_rpy);
 
-      r_zmp_raw = rp_ref_out.P["zmp"].p;
-      applyZMPCalcFromCOM                 (rp_ref_out.P["com"].p,rp_ref_out.P["zmp"].p);
+      r_zmp_raw = rp_ref_out.getP("zmp").p;
+      applyZMPCalcFromCOM                 (rp_ref_out.getP("com").p,rp_ref_out.getP("zmp").p);
 //      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, go_rf_landing, go_lf_landing);//ここじゃ判定遅い
       applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       applyCOMZMPXYZLock                  (rp_ref_out);
       overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
-      com_vel_old = (rp_ref_out.P["com"].p - rp_ref_out_old.P["com"].p)/DT;
+      com_vel_old = (rp_ref_out.getP("com").p - rp_ref_out_old.getP("com").p)/DT;
       rp_ref_out_old = rp_ref_out;
       loop++;
       gettimeofday(&t_calc_end, NULL);
@@ -332,13 +348,13 @@ class HumanSynchronizer{
       else if(cmmr_goal - cmmr < (-1)*step){cmmr -= step; cout<<"Interpolating COMMoveModRatio as "<<cmmr<<endl;}
       else{cmmr = cmmr_goal;}
     };
-    void removeInitOffsetPose(/*const*/ HumanPose& abs_in, /*const*/ HumanPose& init_offset, HumanPose& rel_out){
+    void removeInitOffsetPose(const HumanPose& abs_in, const HumanPose& init_offset, HumanPose& rel_out){
       std::string ns[5] = {"com","rf","lf","rh","lh"};
-      for(int i=0;i<5;i++){ rel_out.P[ns[i]].p = abs_in.P[ns[i]].p - init_offset.P[ns[i]].p; }
-      rel_out.w["rfw"] = abs_in.w["rfw"];
-      rel_out.w["lfw"] = abs_in.w["lfw"];
+      for(int i=0;i<5;i++){ rel_out.P.find(ns[i])->second.p = abs_in.P.find(ns[i])->second.p - init_offset.P.find(ns[i])->second.p; }
+      rel_out.getw("rfw") = abs_in.getw("rfw");
+      rel_out.getw("lfw") = abs_in.getw("lfw");
     }
-    void applyInitHumanZMPToCOMOffset(/*const*/ HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
+    void applyInitHumanZMPToCOMOffset(const HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
       if(isHumanSyncOn()){
         if(!init_zmp_com_offset_interpolator->isEmpty()){
           init_zmp_com_offset_interpolator->get(&zmp_com_offset_ip_ratio, true);
@@ -347,20 +363,20 @@ class HumanSynchronizer{
         zmp_com_offset_ip_ratio = 0;
       }
       out = in;
-      out.P["com"].p = in.P["com"].p + zmp_com_offset_ip_ratio * com_offs;
+      out.getP("com").p = in.getP("com").p + zmp_com_offset_ip_ratio * com_offs;
     }
     void applyLPFilter(HumanPose& tgt){
       std::string ns[5] = {"com","rf","lf","rh","lh"};
-      for(int i=0;i<iir_v_filters.size();i++){ tgt.P[ns[i]].p = iir_v_filters[i].passFilter(tgt.P[ns[i]].p); }
+      for(int i=0;i<iir_v_filters.size();i++){ tgt.P.find(ns[i])->second.p = iir_v_filters[i].passFilter(tgt.P.find(ns[i])->second.p); }
     }
-    void convertRelHumanPoseToRelRobotPose(/*const*/ HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
+    void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
       std::string ns[6] = {"com","rf","lf","rh","lh","zmp"};
-      for(int i=0;i<6;i++){ rp_out.P[ns[i]].p =  h2r_ratio * hp_in.P[ns[i]].p; }
-      rp_out.w["rfw"]  = hp_in.w["rfw"];
-      rp_out.w["lfw"]  = hp_in.w["lfw"];
+      for(int i=0;i<6;i++){ rp_out.P.find(ns[i])->second.p =  h2r_ratio * hp_in.P.find(ns[i])->second.p; }
+      rp_out.getw("rfw")  = hp_in.getw("rfw");
+      rp_out.getw("lfw")  = hp_in.getw("lfw");
     }
     void applyCOMMoveModRatio(HumanPose& tgt){//
-      tgt.P["com"].p = cmmr * tgt.P["com"].p;
+      tgt.getP("com").p = cmmr * tgt.getP("com").p;
     }
     void judgeFootLandOnCommand(const Wrench6& rfw_in, const Wrench6& lfw_in, bool& rfloc_ans, bool& lfloc_ans){
       if(HumanSyncOn){
@@ -371,15 +387,15 @@ class HumanSynchronizer{
       }
       if(!rfloc_ans && !lfloc_ans){rfloc_ans = lfloc_ans = true;}//両足ジャンプは禁止
     }
-    void lockSwingFootIfZMPOutOfSupportFoot(/*const*/ HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
+    void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
       if(rfcs_ans && !lfcs_ans){
-        if(tgt.P["zmp"].p(1) > tgt.P["rf"].p(1) + init_wld_rp_rfeepos(1) + 0.03){
+        if(tgt.getP("zmp").p(1) > tgt.getP("rf").p(1) + init_wld_rp_rfeepos(1) + 0.03){
           lfcs_ans = true;
           if(loop%100==0)cout<<"LF Auto lock"<<endl;
         }
       }
       if(!rfcs_ans && lfcs_ans){
-        if(tgt.P["zmp"].p(1) < tgt.P["lf"].p(1) + init_wld_rp_lfeepos(1) - 0.03){
+        if(tgt.getP("zmp").p(1) < tgt.getP("lf").p(1) + init_wld_rp_lfeepos(1) - 0.03){
           rfcs_ans = true;
           if(loop%100==0)cout<<"RF Auto lock"<<endl;
         }
@@ -387,12 +403,13 @@ class HumanSynchronizer{
     }
     void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
       if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足接地しているタイミングでのみ更新
-      double RFDOWN_TIME = fabs(tgt.P["rf"].p(1) - tgt.P["com"].p(1))*2 + FUP_TIME;//下ろす速度は遊脚の重心からの距離に比例したペナルティ
-      double LFDOWN_TIME = fabs(tgt.P["lf"].p(1) - tgt.P["com"].p(1))*2 + FUP_TIME;
-      calcFootUpCurveAndJudgeFootContact(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.P["rf"].p, is_rf_contact);
-      calcFootUpCurveAndJudgeFootContact(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.P["lf"].p, is_lf_contact);
+      double RFDOWN_TIME = fabs(tgt.getP("rf").p(1) - tgt.getP("com").p(1))*2 + FUP_TIME;//下ろす速度は遊脚の重心からの距離に比例したペナルティ
+      double LFDOWN_TIME = fabs(tgt.getP("lf").p(1) - tgt.getP("com").p(1))*2 + FUP_TIME;
+      hrp::Vector3 ss;
+      calcFootUpCurveAndJudgeFootContact(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.getP("rf").p, is_rf_contact);
+      calcFootUpCurveAndJudgeFootContact(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.getP("lf").p, is_lf_contact);
     }
-    void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, const double& fup_time_in, const double& fdown_time_in,  double& cur_fup_phase_in,hrp::Vector3& f_height_out, bool& is_f_contact_out){
+    void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, const double& fup_time_in, const double& fdown_time_in,  double& cur_fup_phase_in, hrp::Vector3& f_height_out, bool& is_f_contact_out){
       const double omega_up = M_PI / fup_time_in;
       const double omega_down = M_PI / fdown_time_in;
       if(!go_land_in){//足上げ命令入力時
@@ -405,59 +422,48 @@ class HumanSynchronizer{
       f_height_out(2) = FUP_HIGHT/2*(1-cos(cur_fup_phase_in));
       is_f_contact_out = (cur_fup_phase_in <= 0);
     }
-//    void calcFootUpCurveAndJudgeFootContact_o(const bool& go_land_in, int& cur_fup_count_in, hrp::Vector3& f_height_out, bool& is_f_contact_out){
-//      int FUP_COUNT = (int)(HZ*FUP_TIME);
-//      if(cur_fup_count_in > FUP_COUNT){cur_fup_count_in = FUP_COUNT;} if(cur_fup_count_in < 0){cur_fup_count_in = 0;}
-//      if(!go_land_in){//足上げ命令入力時
-//        if(cur_fup_count_in < FUP_COUNT){ cur_fup_count_in++; }
-//      }else{//足下げ命令入力時
-//        if(cur_fup_count_in > 0){ cur_fup_count_in--; }
-//      }
-//      f_height_out(2) = FUP_HIGHT/2*(1-cos(M_PI*cur_fup_count_in/FUP_COUNT));
-//      is_f_contact_out = (cur_fup_count_in <= 0);
-//    }
     void lockFootXYOnContact(const bool& rfcs_in, const bool& lfcs_in, HumanPose& tgt){
       if(rfcs_in){
-        tgt.P["rf"].p(0) = pre_cont_rfpos(0);
-        tgt.P["rf"].p(1) = pre_cont_rfpos(1);
+        tgt.getP("rf").p(0) = pre_cont_rfpos(0);
+        tgt.getP("rf").p(1) = pre_cont_rfpos(1);
       }else{
-        pre_cont_rfpos(0) = tgt.P["rf"].p(0);
-        pre_cont_rfpos(1) = tgt.P["rf"].p(1);
+        pre_cont_rfpos(0) = tgt.getP("rf").p(0);
+        pre_cont_rfpos(1) = tgt.getP("rf").p(1);
       }
       if(lfcs_in){
-        tgt.P["lf"].p(0) = pre_cont_lfpos(0);
-        tgt.P["lf"].p(1) = pre_cont_lfpos(1);
+        tgt.getP("lf").p(0) = pre_cont_lfpos(0);
+        tgt.getP("lf").p(1) = pre_cont_lfpos(1);
       }else{
-        pre_cont_lfpos(0) = tgt.P["lf"].p(0);
-        pre_cont_lfpos(1) = tgt.P["lf"].p(1);
+        pre_cont_lfpos(0) = tgt.getP("lf").p(0);
+        pre_cont_lfpos(1) = tgt.getP("lf").p(1);
       }
     }
     void applyEEWorkspaceLimit(HumanPose& tgt){
       const double MAX = 0.15;
       if(!is_rf_contact && is_lf_contact){//右足浮遊時
-        const hrp::Vector2 lf2rf_vec( tgt.P["rf"].p(0)-pre_cont_lfpos(0), tgt.P["rf"].p(1)-pre_cont_lfpos(1) );
+        const hrp::Vector2 lf2rf_vec( tgt.getP("rf").p(0)-pre_cont_lfpos(0), tgt.getP("rf").p(1)-pre_cont_lfpos(1) );
         if(lf2rf_vec.norm() > MAX){
-          tgt.P["rf"].p(0) = (lf2rf_vec.normalized())(0) * MAX + pre_cont_lfpos(0);
-          tgt.P["rf"].p(1) = (lf2rf_vec.normalized())(1) * MAX + pre_cont_lfpos(1);
+          tgt.getP("rf").p(0) = (lf2rf_vec.normalized())(0) * MAX + pre_cont_lfpos(0);
+          tgt.getP("rf").p(1) = (lf2rf_vec.normalized())(1) * MAX + pre_cont_lfpos(1);
         }
-        if(tgt.P["rf"].p(1) > pre_cont_lfpos(1) + 0.05 ) tgt.P["rf"].p(1) = pre_cont_lfpos(1) + 0.05;
+        if(tgt.getP("rf").p(1) > pre_cont_lfpos(1) + 0.05 ) tgt.getP("rf").p(1) = pre_cont_lfpos(1) + 0.05;
       }
       else if(is_rf_contact && !is_lf_contact){//左足浮遊時
-        const hrp::Vector2 rf2lf_vec( tgt.P["lf"].p(0)-pre_cont_rfpos(0), tgt.P["lf"].p(1)-pre_cont_rfpos(1) );
+        const hrp::Vector2 rf2lf_vec( tgt.getP("lf").p(0)-pre_cont_rfpos(0), tgt.getP("lf").p(1)-pre_cont_rfpos(1) );
         if(rf2lf_vec.norm() > MAX){
-          tgt.P["lf"].p(0) = (rf2lf_vec.normalized())(0) * MAX + pre_cont_rfpos(0);
-          tgt.P["lf"].p(1) = (rf2lf_vec.normalized())(1) * MAX + pre_cont_rfpos(1);
+          tgt.getP("lf").p(0) = (rf2lf_vec.normalized())(0) * MAX + pre_cont_rfpos(0);
+          tgt.getP("lf").p(1) = (rf2lf_vec.normalized())(1) * MAX + pre_cont_rfpos(1);
         }
-        if(tgt.P["lf"].p(1) < pre_cont_rfpos(1) - 0.05 ) tgt.P["lf"].p(1) = pre_cont_rfpos(1) - 0.05;
+        if(tgt.getP("lf").p(1) < pre_cont_rfpos(1) - 0.05 ) tgt.getP("lf").p(1) = pre_cont_rfpos(1) - 0.05;
       }
       hrp::Vector3 hand_ulimit = hrp::Vector3(0.1,0.1,0.1);
       hrp::Vector3 hand_llimit = hrp::Vector3(-0.1,-0.1,-0.1);
       for(int i=0;i<3;i++){
-        LIMIT( tgt.P["rh"].p(i), hand_llimit(i) + current_basepos(i) - init_basepos(i), hand_ulimit(i) + current_basepos(i) - init_basepos(i) );
-        LIMIT( tgt.P["lh"].p(i), hand_llimit(i) + current_basepos(i) - init_basepos(i), hand_ulimit(i) + current_basepos(i) - init_basepos(i) );
+        LIMIT_MINMAX( tgt.getP("rh").p(i), hand_llimit(i) + current_basepos(i) - init_basepos(i), hand_ulimit(i) + current_basepos(i) - init_basepos(i) );
+        LIMIT_MINMAX( tgt.getP("lh").p(i), hand_llimit(i) + current_basepos(i) - init_basepos(i), hand_ulimit(i) + current_basepos(i) - init_basepos(i) );
       }
-      LIMIT( cam_rpy(1), -20*M_PI/180, 20*M_PI/180 );
-      LIMIT( cam_rpy(2), -20*M_PI/180, 20*M_PI/180 );
+      LIMIT_MINMAX( cam_rpy(1), -20*M_PI/180, 20*M_PI/180 );
+      LIMIT_MINMAX( cam_rpy(2), -20*M_PI/180, 20*M_PI/180 );
 
     }
     bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
@@ -479,7 +485,7 @@ class HumanSynchronizer{
       double accel[4] = {0.05, -0.03, 0.02, -0.02};
       double decel[4] = {0.04, -0.02, 0.01, -0.01};
       comacc_ref = calcacc_v_filters.passFilter(com_acc);
-      const double H = rp_wld_initpos.P["com"].p(2);
+      const double H = rp_wld_initpos.getP("com").p(2);
 //      double maxacc[2],minacc[2];
 //      maxacc[0] = (com_old(0) - accel[1]) * (G/H); if(maxacc[0] < 0){maxacc[0]=0;}
 //      minacc[0] = (com_old(0) - accel[0]) * (G/H); if(minacc[0] > 0){minacc[0]=0;}
@@ -506,7 +512,7 @@ class HumanSynchronizer{
     }
     void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
       com_vel_ans = com_vel;
-      const double H = rp_wld_initpos.P["com"].p(2);
+      const double H = rp_wld_initpos.getP("com").p(2);
       hrp::Vector2 cp = com_pos + com_vel * sqrt( H / G );
       hrp::Vector2 cp_ragulated;
       if(!isPointInHullOpenCV(cp,hull)){
@@ -517,7 +523,7 @@ class HumanSynchronizer{
     }
     void regulateCOMVelocityByCapturePointXY(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
       com_vel_ans = com_vel;
-      const double H = rp_wld_initpos.P["com"].p(2);
+      const double H = rp_wld_initpos.getP("com").p(2);
       hrp::Vector4 margin_from_4maxcp;
       calcXYMarginToHull(com_pos, hull, margin_from_4maxcp);
       hrp::Vector3 max_com_vel,min_com_vel;
@@ -539,7 +545,7 @@ class HumanSynchronizer{
     void makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans);
     bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull);
     void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout);
-    void applyVelLimit(/*const*/ HumanPose& in, /*const*/ HumanPose& in_old, HumanPose& out);
+    void applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out);
     void applyCOMZMPXYZLock(HumanPose& tgt);
 };
 

From 98cadac66d2121a83b8618ecd4fae815013de7f4 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 12 Jan 2017 21:29:58 +0900
Subject: [PATCH 049/217] add safe getter

---
 rtc/AutoBalancer/AutoBalancer.cpp     |  40 +++----
 rtc/AutoBalancer/HumanMasterSlave.cpp |  26 ++---
 rtc/AutoBalancer/HumanMasterSlave.h   | 160 +++++++++++++-------------
 3 files changed, 111 insertions(+), 115 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 4e29b75ab11..4d288d8e455 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -501,13 +501,13 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     }
     //for HumanSynchronizer
     if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
-    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hp_raw_data.w["rfw"]); }
-    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hp_raw_data.w["lfw"]); }
-    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Point3DToVector3(m_htcom.data,hp_raw_data.P["com"].p); }
-    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Point3DToVector3(m_htrf.data,hp_raw_data.P["rf"].p); }
-    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Point3DToVector3(m_htlf.data,hp_raw_data.P["lf"].p); }
-    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Point3DToVector3(m_htrh.data,hp_raw_data.P["rh"].p); }
-    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Point3DToVector3(m_htlh.data,hp_raw_data.P["lh"].p); }
+    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hp_raw_data.getw("rfw")); }
+    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hp_raw_data.getw("lfw")); }
+    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Point3DToVector3(m_htcom.data,hp_raw_data.getP("com").p); }
+    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Point3DToVector3(m_htrf.data,hp_raw_data.getP("rf").p); }
+    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Point3DToVector3(m_htlf.data,hp_raw_data.getP("lf").p); }
+    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Point3DToVector3(m_htrh.data,hp_raw_data.getP("rh").p); }
+    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Point3DToVector3(m_htlh.data,hp_raw_data.getP("lh").p); }
     if (m_htcamIn.isNew()){ m_htcamIn.read(); HumanSynchronizer::Pose3DToVector3(m_htcam.data,hsp->cam_pos,hsp->cam_rpy); }
     if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
     hsp->readInput(hp_raw_data);
@@ -1118,12 +1118,12 @@ void AutoBalancer::solveLimbIK ()
   }else{
     hsp->setInitOffsetPose();
     hsp->calibInitHumanCOMFromZMP();
-    hsp->rp_wld_initpos.P["com"].p = m_robot->calcCM();
-    hsp->rp_wld_initpos.P["rf"].p = ikp["rleg"].target_link->p;
-    hsp->rp_wld_initpos.P["lf"].p = ikp["lleg"].target_link->p;
-    hsp->rp_wld_initpos.P["zmp"].p = ref_zmp;
-    if(ikp.count("rarm"))hsp->rp_wld_initpos.P["rh"].p = ikp["rarm"].target_link->p;
-    if(ikp.count("larm"))hsp->rp_wld_initpos.P["lh"].p = ikp["larm"].target_link->p;
+    hsp->rp_wld_initpos.getP("com").p = m_robot->calcCM();
+    hsp->rp_wld_initpos.getP("rf").p = ikp["rleg"].target_link->p;
+    hsp->rp_wld_initpos.getP("lf").p = ikp["lleg"].target_link->p;
+    hsp->rp_wld_initpos.getP("zmp").p = ref_zmp;
+    if(ikp.count("rarm"))hsp->rp_wld_initpos.getP("rh").p = ikp["rarm"].target_link->p;
+    if(ikp.count("larm"))hsp->rp_wld_initpos.getP("lh").p = ikp["larm"].target_link->p;
     hsp->init_basepos = m_robot->rootLink()->p;
   }
   hsp->update();//////HumanSynchronizerの主要処理
@@ -1146,15 +1146,15 @@ void AutoBalancer::solveLimbIK ()
   // additional COM fitting IK for HumanSynchronizer
   if(hsp->isHumanSyncOn()){
 	  solveWholeBodyCOMIK(
-	      hsp->rp_wld_initpos.P["com"].p + hsp->rp_ref_out.P["com"].p,
-	      hsp->rp_wld_initpos.P["rf"].p + hsp->rp_ref_out.P["rf"].p,
-	      hsp->rp_wld_initpos.P["lf"].p + hsp->rp_ref_out.P["lf"].p,
-	      hsp->rp_wld_initpos.P["rh"].p + hsp->rp_ref_out.P["rh"].p,
-	      hsp->rp_wld_initpos.P["lh"].p + hsp->rp_ref_out.P["lh"].p,
+	      hsp->rp_wld_initpos.getP("com").p + hsp->rp_ref_out.getP("com").p,
+	      hsp->rp_wld_initpos.getP("rf").p + hsp->rp_ref_out.getP("rf").p,
+	      hsp->rp_wld_initpos.getP("lf").p + hsp->rp_ref_out.getP("lf").p,
+	      hsp->rp_wld_initpos.getP("rh").p + hsp->rp_ref_out.getP("rh").p,
+	      hsp->rp_wld_initpos.getP("lh").p + hsp->rp_ref_out.getP("lh").p,
 	      hsp->cam_rpy_filtered );
     //outport用のデータ上書き
-    ref_zmp = hsp->rp_ref_out.P["zmp"].p + hsp->rp_wld_initpos.P["zmp"].p;
-    ref_cog = hsp->rp_ref_out.P["com"].p + hsp->rp_wld_initpos.P["com"].p;
+    ref_zmp = hsp->rp_ref_out.getP("zmp").p + hsp->rp_wld_initpos.getP("zmp").p;
+    ref_cog = hsp->rp_ref_out.getP("com").p + hsp->rp_wld_initpos.getP("com").p;
 
   }else{
 	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
diff --git a/rtc/AutoBalancer/HumanMasterSlave.cpp b/rtc/AutoBalancer/HumanMasterSlave.cpp
index c2b7332327e..ae9a115b975 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.cpp
+++ b/rtc/AutoBalancer/HumanMasterSlave.cpp
@@ -114,25 +114,25 @@ bool HumanSynchronizer::isPointInHullOpenCV(const hrp::Vector2& pt, const std::v
 void HumanSynchronizer::applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
   comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
   const double MAXACC = 5;
-  LIMIT( comacc(0), -MAXACC, MAXACC);
-  LIMIT( comacc(1), -MAXACC, MAXACC);
+  LIMIT_MINMAX( comacc(0), -MAXACC, MAXACC);
+  LIMIT_MINMAX( comacc(1), -MAXACC, MAXACC);
   comacc = acc4zmp_v_filters.passFilter(comacc);
-  zmpout(0) = comin(0)-(rp_wld_initpos.P["com"].p(2)/G)*comacc(0);
-  zmpout(1) = comin(1)-(rp_wld_initpos.P["com"].p(2)/G)*comacc(1);
-  if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.P["zmp"].p(0),rp_ref_out.P["zmp"].p(1),zmpout(0),zmpout(1));
+  zmpout(0) = comin(0)-(rp_wld_initpos.getP("com").p(2)/G)*comacc(0);
+  zmpout(1) = comin(1)-(rp_wld_initpos.getP("com").p(2)/G)*comacc(1);
+  if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.getP("zmp").p(0),rp_ref_out.getP("zmp").p(1),zmpout(0),zmpout(1));
   com_oldold = com_old;
   com_old = comin;
 }
-void HumanSynchronizer::applyVelLimit(/*const*/ HumanPose& in, /*const*/ HumanPose& in_old, HumanPose& out){
-  std::string names[5] = {"com","rf","lf","rh","lh"};
+void HumanSynchronizer::applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out){
+  std::string ns[5] = {"com","rf","lf","rh","lh"};
   for(int i=0;i<5;i++){
-    hrp::Vector3 diff = in.P[names[i]].p  - in_old.P[names[i]].p;
-    for(int j=0;j<3;j++)LIMIT( diff(j), -MAXVEL*DT, MAXVEL*DT);
-    out.P[names[i]].p = in_old.P[names[i]].p + diff;
+    hrp::Vector3 diff = in.getP(ns[i]).p  - in_old.getP(ns[i]).p;
+    for(int j=0;j<3;j++)LIMIT_MINMAX( diff(j), -MAXVEL*DT, MAXVEL*DT);
+    out.getP(ns[i]).p = in_old.getP(ns[i]).p + diff;
   }
 }
 void HumanSynchronizer::applyCOMZMPXYZLock(HumanPose& tgt){
-    if(!use_x){tgt.P["com"].p(0) = 0;tgt.P["zmp"].p(0) = 0;}
-    if(!use_y){tgt.P["com"].p(1) = 0;tgt.P["zmp"].p(1) = 0;}
-    if(!use_z){tgt.P["com"].p(2) = 0;}
+    if(!use_x){tgt.getP("com").p(0) = 0;tgt.getP("zmp").p(0) = 0;}
+    if(!use_y){tgt.getP("com").p(1) = 0;tgt.getP("zmp").p(1) = 0;}
+    if(!use_z){tgt.getP("com").p(2) = 0;}
 }
diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index eb8fe3b6de6..594a99196a0 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -20,8 +20,7 @@
 #include <hrpUtil/Eigen4d.h>
 
 #if defined(__cplusplus)
-extern "C"
-{
+extern "C" {
 #endif
 #include <stdio.h>
 #include <stdlib.h>
@@ -37,18 +36,17 @@ extern "C"
 }
 #endif
 
-#define LIMIT(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
+#define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
 
 //実機だとログ出力で落ちる？getenv("HOME")ダメ？
 #define DEBUG 0
 
-class BiquadIIRFilterVec
-{
-private:
+class BiquadIIRFilterVec{
+  private:
     IIRFilter x_filters,y_filters,z_filters;
     hrp::Vector3 ans;
     std::vector<double> fb_coeffs, ff_coeffs;
-public:
+  public:
     BiquadIIRFilterVec(const std::string& error_prefix = ""){
     };
     ~BiquadIIRFilterVec() {};
@@ -77,7 +75,6 @@ class BiquadIIRFilterVec
     };
 };
 
-
 class HRPPose3D{
   public:
     hrp::Vector3 p;
@@ -99,8 +96,8 @@ class Wrench6{
 
 class HumanPose{
   public:
-    std::map <std::string, HRPPose3D> P;
-    std::map <std::string, Wrench6> w;
+    std::map<std::string, HRPPose3D> P;
+    std::map<std::string, Wrench6> w;
     HumanPose(){
       P["com"] = HRPPose3D();
       P["rf"] = HRPPose3D();
@@ -118,13 +115,29 @@ class HumanPose{
       for(std::map<std::string, HRPPose3D>::iterator it = P.begin(); it != P.end(); it++){ it->second.clear(); }
       for(std::map<std::string, Wrench6>::iterator it = w.begin(); it != w.end(); it++){ it->second.clear(); }
     }
-    static void hp_printf(HumanPose& in){
+    static void hp_printf(const HumanPose& in){
       const std::string ns[] = {"com","rf","lf","rh","lh","zmp","rfw","lfw"};
       const std::string cap[] = {"c","rf","lf","rh","lh","z","rw","lw"};
-      for(int i=0;i<6;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.P[ns[i]].p(0),in.P[ns[i]].p(1),in.P[ns[i]].p(2)); }
-      for(int i=6;i<8;i++){ printf("\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.w[ns[i]].f(0),in.w[ns[i]].f(1),in.w[ns[i]].f(2),in.w[ns[i]].t(0),in.w[ns[i]].t(1),in.w[ns[i]].t(2)); }
+      for(int i=0;i<6;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.getP(ns[i]).p(0),in.getP(ns[i]).p(1),in.getP(ns[i]).p(2)); }
+      for(int i=6;i<8;i++){ printf("\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.getw(ns[i]).f(0),in.getw(ns[i]).f(1),in.getw(ns[i]).f(2),in.getw(ns[i]).t(0),in.getw(ns[i]).t(1),in.getw(ns[i]).t(2)); }
       printf("\n");
     }
+    HRPPose3D& getP(const std::string& key){///Safe accessor
+      if (P.find(key) != P.end() ) { return P.find(key)->second; }
+      else { cerr<<"\n\x1b[31m[FATAL] HumanPose Invalid Key \""<<key<<"\" !!\x1b[39m\n"<<endl; exit(-1); return P.end()->second; }
+    }
+    const HRPPose3D& getP(const std::string& key) const {
+      if (P.find(key) != P.end() ) { return P.find(key)->second; }
+      else { cerr<<"\n\x1b[31m[FATAL] HumanPose Invalid Key \""<<key<<"\" !!\x1b[39m\n"<<endl; exit(-1); return P.end()->second; }
+    }
+    Wrench6& getw(const std::string& key){
+      if (w.find(key) != w.end() ) { return w.find(key)->second; }
+      else { cerr<<"\n\x1b[31m[FATAL] HumanPose Invalid Key \""<<key<<"\" !!\x1b[39m\n"<<endl; exit(-1); return w.end()->second; }
+    }
+    const Wrench6& getw(const std::string& key) const {
+      if (w.find(key) != w.end() ) { return w.find(key)->second; }
+      else { cerr<<"\n\x1b[31m[FATAL] HumanPose Invalid Key \""<<key<<"\" !!\x1b[39m\n"<<endl; exit(-1); return w.end()->second; }
+    }
     void print(){ hp_printf(*this); }
 };
 
@@ -276,30 +289,23 @@ class HumanSynchronizer{
       updateCOMModRatio                   (tgt_cmmr);
       removeInitOffsetPose                (hp_wld_raw, hp_wld_initpos, hp_rel_raw);
       applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);
-//      lockFootXYOnContact                 (go_rf_landing, go_lf_landing, hp_rel_raw);//根本から改変すべき2
-      calcWorldZMP                        ((hp_rel_raw.P["rf"].p+init_wld_hp_rfpos), (hp_rel_raw.P["lf"].p+init_wld_hp_lfpos), hp_rel_raw.w["rfw"], hp_rel_raw.w["lfw"], hp_rel_raw.P["zmp"].p);//足の位置はworldにしないと・・・
+      calcWorldZMP                        ((hp_rel_raw.getP("rf").p+init_wld_hp_rfpos), (hp_rel_raw.getP("lf").p+init_wld_hp_lfpos), hp_rel_raw.getw("rfw"), hp_rel_raw.getw("lfw"), hp_rel_raw.getP("zmp").p);//足の位置はworldにしないと・・・
       convertRelHumanPoseToRelRobotPose   (hp_rel_raw, rp_ref_out);
-
       applyCOMMoveModRatio                (rp_ref_out);
-      judgeFootLandOnCommand              (hp_wld_raw.w["rfw"], hp_wld_raw.w["lfw"], go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
+      judgeFootLandOnCommand              (hp_wld_raw.getw("rfw"), hp_wld_raw.getw("lfw"), go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
-
       applyEEWorkspaceLimit               (rp_ref_out);
       lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
-
-      applyCOMToSupportRegionLimit        (rp_ref_out.P["rf"].p+init_wld_rp_rfeepos, rp_ref_out.P["lf"].p+init_wld_rp_lfeepos, rp_ref_out.P["com"].p);
+      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p+init_wld_rp_rfeepos, rp_ref_out.getP("lf").p+init_wld_rp_lfeepos, rp_ref_out.getP("com").p);
       applyLPFilter                       (rp_ref_out);
-      applyCOMStateLimitByCapturePoint    (rp_ref_out.P["com"].p, rp_ref_out_old.P["com"].p, com_vel_old, rp_ref_out.P["rf"].p+init_wld_rp_rfeepos, rp_ref_out.P["lf"].p+init_wld_rp_lfeepos, rp_ref_out.P["com"].p);
-
+      applyCOMStateLimitByCapturePoint    (rp_ref_out.getP("com").p, rp_ref_out_old.getP("com").p, com_vel_old, rp_ref_out.getP("rf").p+init_wld_rp_rfeepos, rp_ref_out.getP("lf").p+init_wld_rp_lfeepos, rp_ref_out.getP("com").p);
       cam_rpy_filtered = cam_rpy_filter.passFilter(cam_rpy);
-
-      r_zmp_raw = rp_ref_out.P["zmp"].p;
-      applyZMPCalcFromCOM                 (rp_ref_out.P["com"].p,rp_ref_out.P["zmp"].p);
-//      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out, go_rf_landing, go_lf_landing);//ここじゃ判定遅い
+      r_zmp_raw = rp_ref_out.getP("zmp").p;
+      applyZMPCalcFromCOM                 (rp_ref_out.getP("com").p,rp_ref_out.getP("zmp").p);
       applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       applyCOMZMPXYZLock                  (rp_ref_out);
       overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
-      com_vel_old = (rp_ref_out.P["com"].p - rp_ref_out_old.P["com"].p)/DT;
+      com_vel_old = (rp_ref_out.getP("com").p - rp_ref_out_old.getP("com").p)/DT;
       rp_ref_out_old = rp_ref_out;
       loop++;
       gettimeofday(&t_calc_end, NULL);
@@ -332,13 +338,13 @@ class HumanSynchronizer{
       else if(cmmr_goal - cmmr < (-1)*step){cmmr -= step; cout<<"Interpolating COMMoveModRatio as "<<cmmr<<endl;}
       else{cmmr = cmmr_goal;}
     };
-    void removeInitOffsetPose(/*const*/ HumanPose& abs_in, /*const*/ HumanPose& init_offset, HumanPose& rel_out){
+    void removeInitOffsetPose(const HumanPose& abs_in, const HumanPose& init_offset, HumanPose& rel_out){
       std::string ns[5] = {"com","rf","lf","rh","lh"};
-      for(int i=0;i<5;i++){ rel_out.P[ns[i]].p = abs_in.P[ns[i]].p - init_offset.P[ns[i]].p; }
-      rel_out.w["rfw"] = abs_in.w["rfw"];
-      rel_out.w["lfw"] = abs_in.w["lfw"];
+      for(int i=0;i<5;i++){ rel_out.getP(ns[i]).p = abs_in.getP(ns[i]).p - init_offset.getP(ns[i]).p; }
+      rel_out.getw("rfw") = abs_in.getw("rfw");
+      rel_out.getw("lfw") = abs_in.getw("lfw");
     }
-    void applyInitHumanZMPToCOMOffset(/*const*/ HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
+    void applyInitHumanZMPToCOMOffset(const HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
       if(isHumanSyncOn()){
         if(!init_zmp_com_offset_interpolator->isEmpty()){
           init_zmp_com_offset_interpolator->get(&zmp_com_offset_ip_ratio, true);
@@ -347,20 +353,20 @@ class HumanSynchronizer{
         zmp_com_offset_ip_ratio = 0;
       }
       out = in;
-      out.P["com"].p = in.P["com"].p + zmp_com_offset_ip_ratio * com_offs;
+      out.getP("com").p = in.getP("com").p + zmp_com_offset_ip_ratio * com_offs;
     }
     void applyLPFilter(HumanPose& tgt){
       std::string ns[5] = {"com","rf","lf","rh","lh"};
-      for(int i=0;i<iir_v_filters.size();i++){ tgt.P[ns[i]].p = iir_v_filters[i].passFilter(tgt.P[ns[i]].p); }
+      for(int i=0;i<iir_v_filters.size();i++){ tgt.getP(ns[i]).p = iir_v_filters[i].passFilter(tgt.getP(ns[i]).p); }
     }
-    void convertRelHumanPoseToRelRobotPose(/*const*/ HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
+    void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
       std::string ns[6] = {"com","rf","lf","rh","lh","zmp"};
-      for(int i=0;i<6;i++){ rp_out.P[ns[i]].p =  h2r_ratio * hp_in.P[ns[i]].p; }
-      rp_out.w["rfw"]  = hp_in.w["rfw"];
-      rp_out.w["lfw"]  = hp_in.w["lfw"];
+      for(int i=0;i<6;i++){ rp_out.getP(ns[i]).p =  h2r_ratio * hp_in.getP(ns[i]).p; }
+      rp_out.getw("rfw")  = hp_in.getw("rfw");
+      rp_out.getw("lfw")  = hp_in.getw("lfw");
     }
     void applyCOMMoveModRatio(HumanPose& tgt){//
-      tgt.P["com"].p = cmmr * tgt.P["com"].p;
+      tgt.getP("com").p = cmmr * tgt.getP("com").p;
     }
     void judgeFootLandOnCommand(const Wrench6& rfw_in, const Wrench6& lfw_in, bool& rfloc_ans, bool& lfloc_ans){
       if(HumanSyncOn){
@@ -371,15 +377,15 @@ class HumanSynchronizer{
       }
       if(!rfloc_ans && !lfloc_ans){rfloc_ans = lfloc_ans = true;}//両足ジャンプは禁止
     }
-    void lockSwingFootIfZMPOutOfSupportFoot(/*const*/ HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
+    void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
       if(rfcs_ans && !lfcs_ans){
-        if(tgt.P["zmp"].p(1) > tgt.P["rf"].p(1) + init_wld_rp_rfeepos(1) + 0.03){
+        if(tgt.getP("zmp").p(1) > tgt.getP("rf").p(1) + init_wld_rp_rfeepos(1) + 0.03){
           lfcs_ans = true;
           if(loop%100==0)cout<<"LF Auto lock"<<endl;
         }
       }
       if(!rfcs_ans && lfcs_ans){
-        if(tgt.P["zmp"].p(1) < tgt.P["lf"].p(1) + init_wld_rp_lfeepos(1) - 0.03){
+        if(tgt.getP("zmp").p(1) < tgt.getP("lf").p(1) + init_wld_rp_lfeepos(1) - 0.03){
           rfcs_ans = true;
           if(loop%100==0)cout<<"RF Auto lock"<<endl;
         }
@@ -387,12 +393,13 @@ class HumanSynchronizer{
     }
     void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
       if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足接地しているタイミングでのみ更新
-      double RFDOWN_TIME = fabs(tgt.P["rf"].p(1) - tgt.P["com"].p(1))*2 + FUP_TIME;//下ろす速度は遊脚の重心からの距離に比例したペナルティ
-      double LFDOWN_TIME = fabs(tgt.P["lf"].p(1) - tgt.P["com"].p(1))*2 + FUP_TIME;
-      calcFootUpCurveAndJudgeFootContact(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.P["rf"].p, is_rf_contact);
-      calcFootUpCurveAndJudgeFootContact(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.P["lf"].p, is_lf_contact);
+      double RFDOWN_TIME = fabs(tgt.getP("rf").p(1) - tgt.getP("com").p(1))*2 + FUP_TIME;//下ろす速度は遊脚の重心からの距離に比例したペナルティ
+      double LFDOWN_TIME = fabs(tgt.getP("lf").p(1) - tgt.getP("com").p(1))*2 + FUP_TIME;
+      hrp::Vector3 ss;
+      calcFootUpCurveAndJudgeFootContact(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.getP("rf").p, is_rf_contact);
+      calcFootUpCurveAndJudgeFootContact(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.getP("lf").p, is_lf_contact);
     }
-    void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, const double& fup_time_in, const double& fdown_time_in,  double& cur_fup_phase_in,hrp::Vector3& f_height_out, bool& is_f_contact_out){
+    void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, const double& fup_time_in, const double& fdown_time_in,  double& cur_fup_phase_in, hrp::Vector3& f_height_out, bool& is_f_contact_out){
       const double omega_up = M_PI / fup_time_in;
       const double omega_down = M_PI / fdown_time_in;
       if(!go_land_in){//足上げ命令入力時
@@ -405,59 +412,48 @@ class HumanSynchronizer{
       f_height_out(2) = FUP_HIGHT/2*(1-cos(cur_fup_phase_in));
       is_f_contact_out = (cur_fup_phase_in <= 0);
     }
-//    void calcFootUpCurveAndJudgeFootContact_o(const bool& go_land_in, int& cur_fup_count_in, hrp::Vector3& f_height_out, bool& is_f_contact_out){
-//      int FUP_COUNT = (int)(HZ*FUP_TIME);
-//      if(cur_fup_count_in > FUP_COUNT){cur_fup_count_in = FUP_COUNT;} if(cur_fup_count_in < 0){cur_fup_count_in = 0;}
-//      if(!go_land_in){//足上げ命令入力時
-//        if(cur_fup_count_in < FUP_COUNT){ cur_fup_count_in++; }
-//      }else{//足下げ命令入力時
-//        if(cur_fup_count_in > 0){ cur_fup_count_in--; }
-//      }
-//      f_height_out(2) = FUP_HIGHT/2*(1-cos(M_PI*cur_fup_count_in/FUP_COUNT));
-//      is_f_contact_out = (cur_fup_count_in <= 0);
-//    }
     void lockFootXYOnContact(const bool& rfcs_in, const bool& lfcs_in, HumanPose& tgt){
       if(rfcs_in){
-        tgt.P["rf"].p(0) = pre_cont_rfpos(0);
-        tgt.P["rf"].p(1) = pre_cont_rfpos(1);
+        tgt.getP("rf").p(0) = pre_cont_rfpos(0);
+        tgt.getP("rf").p(1) = pre_cont_rfpos(1);
       }else{
-        pre_cont_rfpos(0) = tgt.P["rf"].p(0);
-        pre_cont_rfpos(1) = tgt.P["rf"].p(1);
+        pre_cont_rfpos(0) = tgt.getP("rf").p(0);
+        pre_cont_rfpos(1) = tgt.getP("rf").p(1);
       }
       if(lfcs_in){
-        tgt.P["lf"].p(0) = pre_cont_lfpos(0);
-        tgt.P["lf"].p(1) = pre_cont_lfpos(1);
+        tgt.getP("lf").p(0) = pre_cont_lfpos(0);
+        tgt.getP("lf").p(1) = pre_cont_lfpos(1);
       }else{
-        pre_cont_lfpos(0) = tgt.P["lf"].p(0);
-        pre_cont_lfpos(1) = tgt.P["lf"].p(1);
+        pre_cont_lfpos(0) = tgt.getP("lf").p(0);
+        pre_cont_lfpos(1) = tgt.getP("lf").p(1);
       }
     }
     void applyEEWorkspaceLimit(HumanPose& tgt){
       const double MAX = 0.15;
       if(!is_rf_contact && is_lf_contact){//右足浮遊時
-        const hrp::Vector2 lf2rf_vec( tgt.P["rf"].p(0)-pre_cont_lfpos(0), tgt.P["rf"].p(1)-pre_cont_lfpos(1) );
+        const hrp::Vector2 lf2rf_vec( tgt.getP("rf").p(0)-pre_cont_lfpos(0), tgt.getP("rf").p(1)-pre_cont_lfpos(1) );
         if(lf2rf_vec.norm() > MAX){
-          tgt.P["rf"].p(0) = (lf2rf_vec.normalized())(0) * MAX + pre_cont_lfpos(0);
-          tgt.P["rf"].p(1) = (lf2rf_vec.normalized())(1) * MAX + pre_cont_lfpos(1);
+          tgt.getP("rf").p(0) = (lf2rf_vec.normalized())(0) * MAX + pre_cont_lfpos(0);
+          tgt.getP("rf").p(1) = (lf2rf_vec.normalized())(1) * MAX + pre_cont_lfpos(1);
         }
-        if(tgt.P["rf"].p(1) > pre_cont_lfpos(1) + 0.05 ) tgt.P["rf"].p(1) = pre_cont_lfpos(1) + 0.05;
+        if(tgt.getP("rf").p(1) > pre_cont_lfpos(1) + 0.05 ) tgt.getP("rf").p(1) = pre_cont_lfpos(1) + 0.05;
       }
       else if(is_rf_contact && !is_lf_contact){//左足浮遊時
-        const hrp::Vector2 rf2lf_vec( tgt.P["lf"].p(0)-pre_cont_rfpos(0), tgt.P["lf"].p(1)-pre_cont_rfpos(1) );
+        const hrp::Vector2 rf2lf_vec( tgt.getP("lf").p(0)-pre_cont_rfpos(0), tgt.getP("lf").p(1)-pre_cont_rfpos(1) );
         if(rf2lf_vec.norm() > MAX){
-          tgt.P["lf"].p(0) = (rf2lf_vec.normalized())(0) * MAX + pre_cont_rfpos(0);
-          tgt.P["lf"].p(1) = (rf2lf_vec.normalized())(1) * MAX + pre_cont_rfpos(1);
+          tgt.getP("lf").p(0) = (rf2lf_vec.normalized())(0) * MAX + pre_cont_rfpos(0);
+          tgt.getP("lf").p(1) = (rf2lf_vec.normalized())(1) * MAX + pre_cont_rfpos(1);
         }
-        if(tgt.P["lf"].p(1) < pre_cont_rfpos(1) - 0.05 ) tgt.P["lf"].p(1) = pre_cont_rfpos(1) - 0.05;
+        if(tgt.getP("lf").p(1) < pre_cont_rfpos(1) - 0.05 ) tgt.getP("lf").p(1) = pre_cont_rfpos(1) - 0.05;
       }
       hrp::Vector3 hand_ulimit = hrp::Vector3(0.1,0.1,0.1);
       hrp::Vector3 hand_llimit = hrp::Vector3(-0.1,-0.1,-0.1);
       for(int i=0;i<3;i++){
-        LIMIT( tgt.P["rh"].p(i), hand_llimit(i) + current_basepos(i) - init_basepos(i), hand_ulimit(i) + current_basepos(i) - init_basepos(i) );
-        LIMIT( tgt.P["lh"].p(i), hand_llimit(i) + current_basepos(i) - init_basepos(i), hand_ulimit(i) + current_basepos(i) - init_basepos(i) );
+        LIMIT_MINMAX( tgt.getP("rh").p(i), hand_llimit(i) + current_basepos(i) - init_basepos(i), hand_ulimit(i) + current_basepos(i) - init_basepos(i) );
+        LIMIT_MINMAX( tgt.getP("lh").p(i), hand_llimit(i) + current_basepos(i) - init_basepos(i), hand_ulimit(i) + current_basepos(i) - init_basepos(i) );
       }
-      LIMIT( cam_rpy(1), -20*M_PI/180, 20*M_PI/180 );
-      LIMIT( cam_rpy(2), -20*M_PI/180, 20*M_PI/180 );
+      LIMIT_MINMAX( cam_rpy(1), -20*M_PI/180, 20*M_PI/180 );
+      LIMIT_MINMAX( cam_rpy(2), -20*M_PI/180, 20*M_PI/180 );
 
     }
     bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
@@ -479,7 +475,7 @@ class HumanSynchronizer{
       double accel[4] = {0.05, -0.03, 0.02, -0.02};
       double decel[4] = {0.04, -0.02, 0.01, -0.01};
       comacc_ref = calcacc_v_filters.passFilter(com_acc);
-      const double H = rp_wld_initpos.P["com"].p(2);
+      const double H = rp_wld_initpos.getP("com").p(2);
 //      double maxacc[2],minacc[2];
 //      maxacc[0] = (com_old(0) - accel[1]) * (G/H); if(maxacc[0] < 0){maxacc[0]=0;}
 //      minacc[0] = (com_old(0) - accel[0]) * (G/H); if(minacc[0] > 0){minacc[0]=0;}
@@ -506,7 +502,7 @@ class HumanSynchronizer{
     }
     void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
       com_vel_ans = com_vel;
-      const double H = rp_wld_initpos.P["com"].p(2);
+      const double H = rp_wld_initpos.getP("com").p(2);
       hrp::Vector2 cp = com_pos + com_vel * sqrt( H / G );
       hrp::Vector2 cp_ragulated;
       if(!isPointInHullOpenCV(cp,hull)){
@@ -517,7 +513,7 @@ class HumanSynchronizer{
     }
     void regulateCOMVelocityByCapturePointXY(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
       com_vel_ans = com_vel;
-      const double H = rp_wld_initpos.P["com"].p(2);
+      const double H = rp_wld_initpos.getP("com").p(2);
       hrp::Vector4 margin_from_4maxcp;
       calcXYMarginToHull(com_pos, hull, margin_from_4maxcp);
       hrp::Vector3 max_com_vel,min_com_vel;
@@ -539,7 +535,7 @@ class HumanSynchronizer{
     void makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans);
     bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull);
     void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout);
-    void applyVelLimit(/*const*/ HumanPose& in, /*const*/ HumanPose& in_old, HumanPose& out);
+    void applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out);
     void applyCOMZMPXYZLock(HumanPose& tgt);
 };
 

From 2048133f5375558b86aaccc5e89c6a95787bca18 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 13 Jan 2017 16:07:10 +0900
Subject: [PATCH 050/217] allow variable Z height

---
 rtc/AutoBalancer/AutoBalancer.cpp   | 22 ++++++----
 rtc/AutoBalancer/HumanMasterSlave.h | 66 ++++++++++++++++-------------
 2 files changed, 50 insertions(+), 38 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 4d288d8e455..a1460d0d98b 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1124,6 +1124,16 @@ void AutoBalancer::solveLimbIK ()
     hsp->rp_wld_initpos.getP("zmp").p = ref_zmp;
     if(ikp.count("rarm"))hsp->rp_wld_initpos.getP("rh").p = ikp["rarm"].target_link->p;
     if(ikp.count("larm"))hsp->rp_wld_initpos.getP("lh").p = ikp["larm"].target_link->p;
+
+    hsp->rp_ref_out.getP("com").offs = m_robot->calcCM();
+    hsp->rp_ref_out.getP("rf").offs = ikp["rleg"].target_link->p;
+    hsp->rp_ref_out.getP("lf").offs = ikp["lleg"].target_link->p;
+    hsp->rp_ref_out.getP("zmp").offs = ref_zmp;
+    if(ikp.count("rarm"))hsp->rp_ref_out.getP("rh").offs = ikp["rarm"].target_link->p;
+    if(ikp.count("larm"))hsp->rp_ref_out.getP("lh").offs = ikp["larm"].target_link->p;
+    hsp->pre_cont_rfpos = hsp->rp_ref_out.getP("rf").offs;
+    hsp->pre_cont_lfpos = hsp->rp_ref_out.getP("lf").offs;
+
     hsp->init_basepos = m_robot->rootLink()->p;
   }
   hsp->update();//////HumanSynchronizerの主要処理
@@ -1145,16 +1155,10 @@ void AutoBalancer::solveLimbIK ()
 
   // additional COM fitting IK for HumanSynchronizer
   if(hsp->isHumanSyncOn()){
-	  solveWholeBodyCOMIK(
-	      hsp->rp_wld_initpos.getP("com").p + hsp->rp_ref_out.getP("com").p,
-	      hsp->rp_wld_initpos.getP("rf").p + hsp->rp_ref_out.getP("rf").p,
-	      hsp->rp_wld_initpos.getP("lf").p + hsp->rp_ref_out.getP("lf").p,
-	      hsp->rp_wld_initpos.getP("rh").p + hsp->rp_ref_out.getP("rh").p,
-	      hsp->rp_wld_initpos.getP("lh").p + hsp->rp_ref_out.getP("lh").p,
-	      hsp->cam_rpy_filtered );
+    solveWholeBodyCOMIK( hsp->rp_ref_out.getP("com").p, hsp->rp_ref_out.getP("rf").p, hsp->rp_ref_out.getP("lf").p, hsp->rp_ref_out.getP("rh").p, hsp->rp_ref_out.getP("lh").p, hsp->cam_rpy_filtered );
     //outport用のデータ上書き
-    ref_zmp = hsp->rp_ref_out.getP("zmp").p + hsp->rp_wld_initpos.getP("zmp").p;
-    ref_cog = hsp->rp_ref_out.getP("com").p + hsp->rp_wld_initpos.getP("com").p;
+    ref_zmp = hsp->rp_ref_out.getP("zmp").p;
+    ref_cog = hsp->rp_ref_out.getP("com").p;
 
   }else{
 	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index 9e831c65ff1..408ca6ca188 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -76,17 +76,20 @@ class BiquadIIRFilterVec{
 };
 
 class HRPPose3D{
+  private:
+    hrp::Vector3 wld;
   public:
     hrp::Vector3 p,offs;
     hrp::Matrix33 R;
     HRPPose3D(){ clear(); }
     ~HRPPose3D(){}
     void clear(){
-      p = hrp::Vector3::Zero();
+      p = offs = wld = hrp::Vector3::Zero();
       R = hrp::Matrix33::Zero();
     }
-    const hrp::Vector3& wldp() const {
-      return p+offs;
+    const hrp::Vector3& wldp(){
+      wld = p + offs;
+      return wld;
     }
 };
 class Wrench6{
@@ -152,7 +155,6 @@ class HumanSynchronizer{
     double cur_rfup_rad,cur_lfup_rad;
     double FUP_HIGHT;
     double CNT_F_TH;
-    hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
     hrp::Vector3 init_hp_calibcom;
     double MAXVEL,MAXACC;
     HumanPose hp_wld_raw;
@@ -190,6 +192,7 @@ class HumanSynchronizer{
     hrp::Vector3 com_vel_old;
     unsigned int loop;
     hrp::Vector3 cam_pos,cam_rpy,cam_pos_filtered,cam_rpy_filtered;
+    hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
 
 
     interpolator *init_zmp_com_offset_interpolator;
@@ -238,8 +241,8 @@ class HumanSynchronizer{
       acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
       cam_rpy_filter.setParameter(1,HZ);//カメラアングル
 
-      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
-      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
+      rf_safe_region = hrp::Vector4(0.02, -0.01*3,  0.02,  0.01);
+      lf_safe_region = hrp::Vector4(0.02, -0.01*3, -0.01, -0.02);
 //      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.0);
 //      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.0, -0.02);
 
@@ -291,19 +294,20 @@ class HumanSynchronizer{
       updateHumanToRobotRatio             (tgt_h2r_ratio);
       updateCOMModRatio                   (tgt_cmmr);
       removeInitOffsetPose                (hp_wld_raw, hp_wld_initpos, hp_rel_raw);
-      applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);
+//      applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);//怪しいので要修正
       calcWorldZMP                        ((hp_rel_raw.getP("rf").p+init_wld_hp_rfpos), (hp_rel_raw.getP("lf").p+init_wld_hp_lfpos), hp_rel_raw.getw("rfw"), hp_rel_raw.getw("lfw"), hp_rel_raw.getP("zmp").p);//足の位置はworldにしないと・・・
       convertRelHumanPoseToRelRobotPose   (hp_rel_raw, rp_ref_out);
+
       applyCOMMoveModRatio                (rp_ref_out);
       judgeFootLandOnCommand              (hp_wld_raw.getw("rfw"), hp_wld_raw.getw("lfw"), go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
       applyEEWorkspaceLimit               (rp_ref_out);
       lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
-      rp_ref_out.getP("rf").offs = init_wld_rp_rfeepos;
-      rp_ref_out.getP("lf").offs = init_wld_rp_lfeepos;
-      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").wldp(), rp_ref_out.getP("lf").wldp(), rp_ref_out.getP("com").p);
+
+      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com"));
       applyLPFilter                       (rp_ref_out);
-      applyCOMStateLimitByCapturePoint    (rp_ref_out.getP("com").p, rp_ref_out_old.getP("com").p, com_vel_old, rp_ref_out.getP("rf").wldp(), rp_ref_out.getP("lf").wldp(), rp_ref_out.getP("com").p);
+      applyCOMStateLimitByCapturePoint    (rp_ref_out.getP("com").p, rp_ref_out_old.getP("com").p, com_vel_old, rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com").p);
+
       cam_rpy_filtered = cam_rpy_filter.passFilter(cam_rpy);
       r_zmp_raw = rp_ref_out.getP("zmp").p;
       applyZMPCalcFromCOM                 (rp_ref_out.getP("com").p,rp_ref_out.getP("zmp").p);
@@ -311,6 +315,7 @@ class HumanSynchronizer{
       applyCOMZMPXYZLock                  (rp_ref_out);
       overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
       com_vel_old = (rp_ref_out.getP("com").p - rp_ref_out_old.getP("com").p)/DT;
+
       rp_ref_out_old = rp_ref_out;
       loop++;
       gettimeofday(&t_calc_end, NULL);
@@ -366,7 +371,7 @@ class HumanSynchronizer{
     }
     void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
       std::string ns[6] = {"com","rf","lf","rh","lh","zmp"};
-      for(int i=0;i<6;i++){ rp_out.getP(ns[i]).p =  h2r_ratio * hp_in.getP(ns[i]).p; }
+      for(int i=0;i<6;i++){ rp_out.getP(ns[i]).p =  h2r_ratio * hp_in.getP(ns[i]).p + rp_out.getP(ns[i]).offs; }
       rp_out.getw("rfw")  = hp_in.getw("rfw");
       rp_out.getw("lfw")  = hp_in.getw("lfw");
     }
@@ -384,13 +389,13 @@ class HumanSynchronizer{
     }
     void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
       if(rfcs_ans && !lfcs_ans){
-        if(tgt.getP("zmp").p(1) > tgt.getP("rf").p(1) + init_wld_rp_rfeepos(1) + 0.03){
+        if(tgt.getP("zmp").p(1) > tgt.getP("rf").p(1) + 0.03){
           lfcs_ans = true;
           if(loop%100==0)cout<<"LF Auto lock"<<endl;
         }
       }
       if(!rfcs_ans && lfcs_ans){
-        if(tgt.getP("zmp").p(1) < tgt.getP("lf").p(1) + init_wld_rp_lfeepos(1) - 0.03){
+        if(tgt.getP("zmp").p(1) < tgt.getP("lf").p(1) - 0.03){
           rfcs_ans = true;
           if(loop%100==0)cout<<"RF Auto lock"<<endl;
         }
@@ -400,11 +405,10 @@ class HumanSynchronizer{
       if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足接地しているタイミングでのみ更新
       double RFDOWN_TIME = fabs(tgt.getP("rf").p(1) - tgt.getP("com").p(1))*2 + FUP_TIME;//下ろす速度は遊脚の重心からの距離に比例したペナルティ
       double LFDOWN_TIME = fabs(tgt.getP("lf").p(1) - tgt.getP("com").p(1))*2 + FUP_TIME;
-      hrp::Vector3 ss;
-      calcFootUpCurveAndJudgeFootContact(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.getP("rf").p, is_rf_contact);
-      calcFootUpCurveAndJudgeFootContact(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.getP("lf").p, is_lf_contact);
+      calcFootUpCurveAndJudgeFootContact(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.getP("rf"), is_rf_contact);
+      calcFootUpCurveAndJudgeFootContact(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.getP("lf"), is_lf_contact);
     }
-    void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, const double& fup_time_in, const double& fdown_time_in,  double& cur_fup_phase_in, hrp::Vector3& f_height_out, bool& is_f_contact_out){
+    void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, const double& fup_time_in, const double& fdown_time_in,  double& cur_fup_phase_in, HRPPose3D& f_height_out, bool& is_f_contact_out){
       const double omega_up = M_PI / fup_time_in;
       const double omega_down = M_PI / fdown_time_in;
       if(!go_land_in){//足上げ命令入力時
@@ -414,7 +418,9 @@ class HumanSynchronizer{
       }
       if(cur_fup_phase_in > M_PI){cur_fup_phase_in = M_PI;}
       else if(cur_fup_phase_in < 0){cur_fup_phase_in = 0;}
-      f_height_out(2) = FUP_HIGHT/2*(1-cos(cur_fup_phase_in));
+      double fup_height = f_height_out.p(2)-f_height_out.offs(2);
+      LIMIT_MINMAX( fup_height, 0.03, 0.10 );
+      f_height_out.p(2) = fup_height/2 * (1-cos(cur_fup_phase_in)) + f_height_out.offs(2);
       is_f_contact_out = (cur_fup_phase_in <= 0);
     }
     void lockFootXYOnContact(const bool& rfcs_in, const bool& lfcs_in, HumanPose& tgt){
@@ -434,7 +440,7 @@ class HumanSynchronizer{
       }
     }
     void applyEEWorkspaceLimit(HumanPose& tgt){
-      const double MAX = 0.15;
+      const double MAX = 0.30;
       if(!is_rf_contact && is_lf_contact){//右足浮遊時
         const hrp::Vector2 lf2rf_vec( tgt.getP("rf").p(0)-pre_cont_lfpos(0), tgt.getP("rf").p(1)-pre_cont_lfpos(1) );
         if(lf2rf_vec.norm() > MAX){
@@ -451,25 +457,27 @@ class HumanSynchronizer{
         }
         if(tgt.getP("lf").p(1) < pre_cont_rfpos(1) - 0.05 ) tgt.getP("lf").p(1) = pre_cont_rfpos(1) - 0.05;
       }
+      hrp::Vector3 base2rh = tgt.getP("rh").offs - init_basepos;
+      hrp::Vector3 base2lh = tgt.getP("lh").offs - init_basepos;
       hrp::Vector3 hand_ulimit = hrp::Vector3(0.1,0.1,0.1);
       hrp::Vector3 hand_llimit = hrp::Vector3(-0.1,-0.1,-0.1);
       for(int i=0;i<3;i++){
-        LIMIT_MINMAX( tgt.getP("rh").p(i), hand_llimit(i) + current_basepos(i) - init_basepos(i), hand_ulimit(i) + current_basepos(i) - init_basepos(i) );
-        LIMIT_MINMAX( tgt.getP("lh").p(i), hand_llimit(i) + current_basepos(i) - init_basepos(i), hand_ulimit(i) + current_basepos(i) - init_basepos(i) );
+        LIMIT_MINMAX( tgt.getP("rh").p(i), (current_basepos+base2rh+hand_llimit)(i), (current_basepos+base2rh+hand_ulimit)(i) );
+        LIMIT_MINMAX( tgt.getP("lh").p(i), (current_basepos+base2lh+hand_llimit)(i), (current_basepos+base2lh+hand_ulimit)(i) );
+        LIMIT_MINMAX( tgt.getP("rf").p(2), tgt.getP("rf").offs(2), tgt.getP("rf").offs(2)+0.10);
+        LIMIT_MINMAX( tgt.getP("lf").p(2), tgt.getP("lf").offs(2), tgt.getP("lf").offs(2)+0.10);
       }
       LIMIT_MINMAX( cam_rpy(1), -20*M_PI/180, 20*M_PI/180 );
       LIMIT_MINMAX( cam_rpy(2), -20*M_PI/180, 20*M_PI/180 );
-
     }
-    bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
+    bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, HRPPose3D& comin_abs){//boost::geometryがUbuntu12だとないから・・・
       std::vector<hrp::Vector2> hull;
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull);
-      hrp::Vector2 cog(comin_abs(0),comin_abs(1));
+      hrp::Vector2 cog(comin_abs.p(0),comin_abs.p(1));
       if(!isPointInHullOpenCV(cog,hull)){ calcNearestPointOnHull(cog,hull,cog); }//外に出たら最近傍点に頭打ち
-      comin_abs(0) = cog(0);
-      comin_abs(1) = cog(1);
-      if( comin_abs(2) > 0.03 ) comin_abs(2) = 0.03;
-      if( comin_abs(2) < -0.15 ) comin_abs(2) = -0.15;
+      comin_abs.p(0) = cog(0);
+      comin_abs.p(1) = cog(1);
+      LIMIT_MINMAX( comin_abs.p(2), comin_abs.offs(2) - 0.15, comin_abs.offs(2) + 0.03 );
       return true;
     }
     bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& com_old, const hrp::Vector3& com_vel_old, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans){

From 37b9f5bf76e4a95df430bcced98cf7e253e006a1 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 18 Jan 2017 16:44:26 +0900
Subject: [PATCH 051/217] change data type PointStamped -> PoseStamped

---
 rtc/AutoBalancer/AutoBalancer.cpp   | 33 +++++++++++----------
 rtc/AutoBalancer/AutoBalancer.h     | 30 +++++++++----------
 rtc/AutoBalancer/HumanMasterSlave.h | 46 +++++++++++++++++++----------
 3 files changed, 63 insertions(+), 46 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index a1460d0d98b..af0374d7508 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -503,12 +503,12 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
     if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hp_raw_data.getw("rfw")); }
     if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hp_raw_data.getw("lfw")); }
-    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Point3DToVector3(m_htcom.data,hp_raw_data.getP("com").p); }
-    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Point3DToVector3(m_htrf.data,hp_raw_data.getP("rf").p); }
-    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Point3DToVector3(m_htlf.data,hp_raw_data.getP("lf").p); }
-    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Point3DToVector3(m_htrh.data,hp_raw_data.getP("rh").p); }
-    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Point3DToVector3(m_htlh.data,hp_raw_data.getP("lh").p); }
-    if (m_htcamIn.isNew()){ m_htcamIn.read(); HumanSynchronizer::Pose3DToVector3(m_htcam.data,hsp->cam_pos,hsp->cam_rpy); }
+    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_htcom.data,hp_raw_data.getP("com")); }
+    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrf.data,hp_raw_data.getP("rf")); }
+    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlf.data,hp_raw_data.getP("lf")); }
+    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrh.data,hp_raw_data.getP("rh")); }
+    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlh.data,hp_raw_data.getP("lh")); }
+    if (m_htcamIn.isNew()){ m_htcamIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_htcam.data,hsp->head_cam_pose); }
     if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
     hsp->readInput(hp_raw_data);
     hsp->current_basepos = m_robot->rootLink()->p;
@@ -1046,17 +1046,20 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param, const std::string& lim
   }
   return true;
 }
-void AutoBalancer::solveWholeBodyCOMIK(const hrp::Vector3& com_ref, const hrp::Vector3& rf_ref, const hrp::Vector3& lf_ref, const hrp::Vector3& rh_ref, const hrp::Vector3& lh_ref, const hrp::Vector3& head_ref){
+void AutoBalancer::solveWholeBodyCOMIK(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const hrp::Vector3& head_ref){
   int com_ik_loop=0;
   const int COM_IK_MAX_LOOP = 5;
   const double COM_IK_MAX_ERROR = 1e-4;
-  m_robot->rootLink()->p = com_ref;//move base link at first
+  m_robot->rootLink()->p = com_ref.p;//move base link at first
+  m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
   m_robot->calcForwardKinematics();//apply
-  hrp::Vector3 tmp_com_err = com_ref - m_robot->calcCM();
-  ikp["rleg"].target_p0 = rf_ref;
-  ikp["lleg"].target_p0 = lf_ref;
-  if(ikp.count("rarm"))ikp["rarm"].target_p0 = rh_ref;
-  if(ikp.count("larm"))ikp["larm"].target_p0 = lh_ref;
+  hrp::Vector3 tmp_com_err = com_ref.p - m_robot->calcCM();
+  ikp["rleg"].target_p0 = rf_ref.p;
+  ikp["rleg"].target_r0 = hrp::rotFromRpy(rf_ref.rpy);
+  ikp["lleg"].target_p0 = lf_ref.p;
+  ikp["lleg"].target_r0 = hrp::rotFromRpy(lf_ref.rpy);
+  if(ikp.count("rarm")){ ikp["rarm"].target_p0 = rh_ref.p; ikp["rarm"].target_r0 = hrp::rotFromRpy(rh_ref.rpy); }
+  if(ikp.count("larm")){ ikp["larm"].target_p0 = lh_ref.p; ikp["larm"].target_r0 = hrp::rotFromRpy(lh_ref.rpy); }
   while(tmp_com_err.norm() > COM_IK_MAX_ERROR){
     m_robot->rootLink()->p += tmp_com_err;
     m_robot->calcForwardKinematics();
@@ -1064,7 +1067,7 @@ void AutoBalancer::solveWholeBodyCOMIK(const hrp::Vector3& com_ref, const hrp::V
       if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
     }
     m_robot->calcForwardKinematics();
-    tmp_com_err = com_ref - m_robot->calcCM();
+    tmp_com_err = com_ref.p - m_robot->calcCM();
     if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
   }
   if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref(2);
@@ -1155,7 +1158,7 @@ void AutoBalancer::solveLimbIK ()
 
   // additional COM fitting IK for HumanSynchronizer
   if(hsp->isHumanSyncOn()){
-    solveWholeBodyCOMIK( hsp->rp_ref_out.getP("com").p, hsp->rp_ref_out.getP("rf").p, hsp->rp_ref_out.getP("lf").p, hsp->rp_ref_out.getP("rh").p, hsp->rp_ref_out.getP("lh").p, hsp->cam_rpy_filtered );
+    solveWholeBodyCOMIK( hsp->rp_ref_out.getP("com"), hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
     //outport用のデータ上書き
     ref_zmp = hsp->rp_ref_out.getP("zmp").p;
     ref_cog = hsp->rp_ref_out.getP("com").p;
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 402f1980b28..763b47db88d 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -143,26 +143,26 @@ class AutoBalancer
   // for debug
   TimedPoint3D m_cog;
   //for human tracker
+  TimedPose3D m_htcom;
+  InPort<TimedPose3D> m_htcomIn;
+  TimedPose3D m_htrf;
+  InPort<TimedPose3D> m_htrfIn;
+  TimedPose3D m_htlf;
+  InPort<TimedPose3D> m_htlfIn;
+  TimedPose3D m_htrh;
+  InPort<TimedPose3D> m_htrhIn;
+  TimedPose3D m_htlh;
+  InPort<TimedPose3D> m_htlhIn;
+  TimedPose3D m_htcam;
+  InPort<TimedPose3D> m_htcamIn;
   TimedPoint3D m_htzmp;
   InPort<TimedPoint3D> m_htzmpIn;
+  TimedPoint3D m_actzmp;
+  InPort<TimedPoint3D> m_actzmpIn;
   TimedDoubleSeq m_htrfw;
   InPort<TimedDoubleSeq> m_htrfwIn;
   TimedDoubleSeq m_htlfw;
   InPort<TimedDoubleSeq> m_htlfwIn;
-  TimedPoint3D m_htcom;
-  InPort<TimedPoint3D> m_htcomIn;
-  TimedPoint3D m_htrf;
-  InPort<TimedPoint3D> m_htrfIn;
-  TimedPoint3D m_htlf;
-  InPort<TimedPoint3D> m_htlfIn;
-  TimedPoint3D m_htrh;
-  InPort<TimedPoint3D> m_htrhIn;
-  TimedPoint3D m_htlh;
-  InPort<TimedPoint3D> m_htlhIn;
-  TimedPoint3D m_actzmp;
-  InPort<TimedPoint3D> m_actzmpIn;
-  TimedPose3D m_htcam;
-  InPort<TimedPose3D> m_htcamIn;
   
   // </rtc-template>
 
@@ -228,7 +228,7 @@ class AutoBalancer
   void getCurrentParameters();
   void getTargetParameters();
   bool solveLimbIKforLimb (ABCIKparam& param, const std::string& limb_name);
-  void solveWholeBodyCOMIK(const hrp::Vector3& com_ref, const hrp::Vector3& rf_ref, const hrp::Vector3& lf_ref, const hrp::Vector3& rh_ref, const hrp::Vector3& lh_ref, const hrp::Vector3& head_ref);
+  void solveWholeBodyCOMIK(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const hrp::Vector3& head_ref);
   void solveLimbIK();
   void startABCparam(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
   void stopABCparam();
diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index 408ca6ca188..19279433d8d 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -79,12 +79,12 @@ class HRPPose3D{
   private:
     hrp::Vector3 wld;
   public:
-    hrp::Vector3 p,offs;
+    hrp::Vector3 p,offs,rpy;
     hrp::Matrix33 R;
     HRPPose3D(){ clear(); }
     ~HRPPose3D(){}
     void clear(){
-      p = offs = wld = hrp::Vector3::Zero();
+      p = offs = rpy = wld = hrp::Vector3::Zero();
       R = hrp::Matrix33::Zero();
     }
     const hrp::Vector3& wldp(){
@@ -169,7 +169,7 @@ class HumanSynchronizer{
     double HZ,DT,G;
     hrp::Vector3 com_old,com_oldold,comacc;
     hrp::Vector3 comacc_ref;
-    std::vector<BiquadIIRFilterVec> iir_v_filters;
+    std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rot_filters;
     BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters;
     BiquadIIRFilterVec cam_rpy_filter;
     hrp::Vector3 r_zmp_raw;
@@ -191,7 +191,8 @@ class HumanSynchronizer{
     hrp::Vector3 current_basepos,init_basepos;
     hrp::Vector3 com_vel_old;
     unsigned int loop;
-    hrp::Vector3 cam_pos,cam_rpy,cam_pos_filtered,cam_rpy_filtered;
+    hrp::Vector3 cam_pos_filtered,cam_rpy_filtered;
+    HRPPose3D head_cam_pose;
     hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
 
 
@@ -232,11 +233,15 @@ class HumanSynchronizer{
       current_basepos = init_basepos = hrp::Vector3::Zero();
 
       com_vel_old = hrp::Vector3::Zero();
-      cam_pos = cam_rpy = cam_pos_filtered = cam_rpy_filtered = hrp::Vector3::Zero();
-
-      iir_v_filters.resize(5);
-      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(0.5,HZ);//四肢拘束点用
-      iir_v_filters[0].setParameter(0.4,HZ);//重心用
+      cam_pos_filtered = cam_rpy_filtered = hrp::Vector3::Zero();
+
+      tgt_pos_filters.resize(5);
+      tgt_rot_filters.resize(5);
+//      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(0.5,HZ);//四肢拘束点用
+      for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(0.7,HZ);//四肢拘束点用(position)
+      for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(0.7,HZ);//四肢拘束点用(Rotation)
+      tgt_pos_filters[0].setParameter(0.4,HZ);//重心用
+      tgt_rot_filters[0].setParameter(0.4,HZ);//重心用
       calcacc_v_filters.setParameter(5,HZ);//加速度計算用
       acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
       cam_rpy_filter.setParameter(1,HZ);//カメラアングル
@@ -308,7 +313,7 @@ class HumanSynchronizer{
       applyLPFilter                       (rp_ref_out);
       applyCOMStateLimitByCapturePoint    (rp_ref_out.getP("com").p, rp_ref_out_old.getP("com").p, com_vel_old, rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com").p);
 
-      cam_rpy_filtered = cam_rpy_filter.passFilter(cam_rpy);
+      cam_rpy_filtered = cam_rpy_filter.passFilter(head_cam_pose.rpy);
       r_zmp_raw = rp_ref_out.getP("zmp").p;
       applyZMPCalcFromCOM                 (rp_ref_out.getP("com").p,rp_ref_out.getP("zmp").p);
       applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
@@ -326,7 +331,7 @@ class HumanSynchronizer{
     }
     static void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out(0) = in.x; out(1) = in.y; out(2) = in.z;}
     static void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out(0) = in.r; out(1) = in.p; out(2) = in.y;}
-    static void Pose3DToVector3(const RTC::Pose3D& in, hrp::Vector3& out_pos, hrp::Vector3& out_rpy){ Point3DToVector3(in.position,out_pos); Oriantation3DToVector3(in.orientation,out_rpy); }
+    static void Pose3DToHRPPose3D(const RTC::Pose3D& in, HRPPose3D& out_pose){ Point3DToVector3(in.position,out_pose.p); Oriantation3DToVector3(in.orientation,out_pose.rpy); }
     static void DoubleSeqToWrench6(const RTC::TimedDoubleSeq::_data_seq& in, Wrench6& out){
       if(in.length() == 6){out.f(0)=in[0]; out.f(1)=in[1]; out.f(2)=in[2]; out.t(0)=in[3]; out.t(1)=in[4]; out.t(2)=in[5];
       }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; out.clear(); }
@@ -350,7 +355,10 @@ class HumanSynchronizer{
     };
     void removeInitOffsetPose(const HumanPose& abs_in, const HumanPose& init_offset, HumanPose& rel_out){
       std::string ns[5] = {"com","rf","lf","rh","lh"};
-      for(int i=0;i<5;i++){ rel_out.getP(ns[i]).p = abs_in.getP(ns[i]).p - init_offset.getP(ns[i]).p; }
+      for(int i=0;i<5;i++){
+        rel_out.getP(ns[i]).p = abs_in.getP(ns[i]).p - init_offset.getP(ns[i]).p;
+        rel_out.getP(ns[i]).rpy = abs_in.getP(ns[i]).rpy;
+      }
       rel_out.getw("rfw") = abs_in.getw("rfw");
       rel_out.getw("lfw") = abs_in.getw("lfw");
     }
@@ -367,11 +375,15 @@ class HumanSynchronizer{
     }
     void applyLPFilter(HumanPose& tgt){
       std::string ns[5] = {"com","rf","lf","rh","lh"};
-      for(int i=0;i<iir_v_filters.size();i++){ tgt.getP(ns[i]).p = iir_v_filters[i].passFilter(tgt.getP(ns[i]).p); }
+      for(int i=0;i<tgt_pos_filters.size();i++){ tgt.getP(ns[i]).p = tgt_pos_filters[i].passFilter(tgt.getP(ns[i]).p); }
+      for(int i=0;i<tgt_rot_filters.size();i++){ tgt.getP(ns[i]).rpy = tgt_rot_filters[i].passFilter(tgt.getP(ns[i]).rpy); }
     }
     void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
       std::string ns[6] = {"com","rf","lf","rh","lh","zmp"};
-      for(int i=0;i<6;i++){ rp_out.getP(ns[i]).p =  h2r_ratio * hp_in.getP(ns[i]).p + rp_out.getP(ns[i]).offs; }
+      for(int i=0;i<6;i++){
+        rp_out.getP(ns[i]).p =  h2r_ratio * hp_in.getP(ns[i]).p + rp_out.getP(ns[i]).offs;
+        rp_out.getP(ns[i]).rpy =  hp_in.getP(ns[i]).rpy;
+      }
       rp_out.getw("rfw")  = hp_in.getw("rfw");
       rp_out.getw("lfw")  = hp_in.getw("lfw");
     }
@@ -405,6 +417,8 @@ class HumanSynchronizer{
       if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足接地しているタイミングでのみ更新
       double RFDOWN_TIME = fabs(tgt.getP("rf").p(1) - tgt.getP("com").p(1))*2 + FUP_TIME;//下ろす速度は遊脚の重心からの距離に比例したペナルティ
       double LFDOWN_TIME = fabs(tgt.getP("lf").p(1) - tgt.getP("com").p(1))*2 + FUP_TIME;
+      LIMIT_MINMAX( RFDOWN_TIME, FUP_TIME, 0.8);
+      LIMIT_MINMAX( LFDOWN_TIME, FUP_TIME, 0.8);
       calcFootUpCurveAndJudgeFootContact(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.getP("rf"), is_rf_contact);
       calcFootUpCurveAndJudgeFootContact(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.getP("lf"), is_lf_contact);
     }
@@ -467,8 +481,8 @@ class HumanSynchronizer{
         LIMIT_MINMAX( tgt.getP("rf").p(2), tgt.getP("rf").offs(2), tgt.getP("rf").offs(2)+0.10);
         LIMIT_MINMAX( tgt.getP("lf").p(2), tgt.getP("lf").offs(2), tgt.getP("lf").offs(2)+0.10);
       }
-      LIMIT_MINMAX( cam_rpy(1), -20*M_PI/180, 20*M_PI/180 );
-      LIMIT_MINMAX( cam_rpy(2), -20*M_PI/180, 20*M_PI/180 );
+      LIMIT_MINMAX( head_cam_pose.rpy(1), -20*M_PI/180, 20*M_PI/180 );
+      LIMIT_MINMAX( head_cam_pose.rpy(2), -20*M_PI/180, 20*M_PI/180 );
     }
     bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, HRPPose3D& comin_abs){//boost::geometryがUbuntu12だとないから・・・
       std::vector<hrp::Vector2> hull;

From cf001cd562d23e3473fc54d6430ec6df529e9888 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 20 Jan 2017 15:27:34 +0900
Subject: [PATCH 052/217] implement for joy stick gait

---
 rtc/AutoBalancer/HumanMasterSlave.h | 88 ++++++++++++-----------------
 1 file changed, 36 insertions(+), 52 deletions(-)

diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index 19279433d8d..ddfd42ca831 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -36,6 +36,8 @@ extern "C" {
 }
 #endif
 
+#define LIMIT_MIN(x,min) (x= ( x<min ? min:x ))
+#define LIMIT_MAX(x,max) (x= ( x<max ? x:max ))
 #define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
 
 //実機だとログ出力で落ちる？getenv("HOME")ダメ？
@@ -76,21 +78,15 @@ class BiquadIIRFilterVec{
 };
 
 class HRPPose3D{
-  private:
-    hrp::Vector3 wld;
   public:
     hrp::Vector3 p,offs,rpy;
     hrp::Matrix33 R;
     HRPPose3D(){ clear(); }
     ~HRPPose3D(){}
     void clear(){
-      p = offs = rpy = wld = hrp::Vector3::Zero();
+      p = offs = rpy = hrp::Vector3::Zero();
       R = hrp::Matrix33::Zero();
     }
-    const hrp::Vector3& wldp(){
-      wld = p + offs;
-      return wld;
-    }
 };
 class Wrench6{
   public:
@@ -238,18 +234,16 @@ class HumanSynchronizer{
       tgt_pos_filters.resize(5);
       tgt_rot_filters.resize(5);
 //      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(0.5,HZ);//四肢拘束点用
-      for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(0.7,HZ);//四肢拘束点用(position)
-      for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(0.7,HZ);//四肢拘束点用(Rotation)
+      for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(0.6,HZ);//四肢拘束点用(position)
+      for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(0.6,HZ);//四肢拘束点用(Rotation)
       tgt_pos_filters[0].setParameter(0.4,HZ);//重心用
       tgt_rot_filters[0].setParameter(0.4,HZ);//重心用
       calcacc_v_filters.setParameter(5,HZ);//加速度計算用
       acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
       cam_rpy_filter.setParameter(1,HZ);//カメラアングル
 
-      rf_safe_region = hrp::Vector4(0.02, -0.01*3,  0.02,  0.01);
-      lf_safe_region = hrp::Vector4(0.02, -0.01*3, -0.01, -0.02);
-//      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.0);
-//      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.0, -0.02);
+      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
+      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
 
       use_rh = use_lh = false;
 
@@ -264,9 +258,6 @@ class HumanSynchronizer{
       init_zmp_com_offset_interpolator->go(&goal_ratio, 3.0, true);//3s
 
       if(DEBUG){
-//        std::string home_path(std::getenv("HOME"));
-//        sr_log = fopen((home_path+"/HumanSync_support_region.log").c_str(),"w+");
-//        cz_log = fopen((home_path+"/HumanSync_com_zmp.log").c_str(),"w+");
         sr_log = fopen("/home/ishiguro/HumanSync_support_region.log","w+");
         cz_log = fopen("/home/ishiguro/HumanSync_com_zmp.log","w+");
       }
@@ -302,6 +293,7 @@ class HumanSynchronizer{
 //      applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);//怪しいので要修正
       calcWorldZMP                        ((hp_rel_raw.getP("rf").p+init_wld_hp_rfpos), (hp_rel_raw.getP("lf").p+init_wld_hp_lfpos), hp_rel_raw.getw("rfw"), hp_rel_raw.getw("lfw"), hp_rel_raw.getP("zmp").p);//足の位置はworldにしないと・・・
       convertRelHumanPoseToRelRobotPose   (hp_rel_raw, rp_ref_out);
+      if(loop==0)rp_ref_out_old = rp_ref_out;
 
       applyCOMMoveModRatio                (rp_ref_out);
       judgeFootLandOnCommand              (hp_wld_raw.getw("rfw"), hp_wld_raw.getw("lfw"), go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
@@ -391,7 +383,7 @@ class HumanSynchronizer{
       tgt.getP("com").p = cmmr * tgt.getP("com").p;
     }
     void judgeFootLandOnCommand(const Wrench6& rfw_in, const Wrench6& lfw_in, bool& rfloc_ans, bool& lfloc_ans){
-      if(HumanSyncOn){
+      if(isHumanSyncOn()){
         if      (rfloc_ans  && rfw_in.f(2)<CNT_F_TH   ){rfloc_ans = false;}//右足ついた状態から上げる
         else if (!rfloc_ans && rfw_in.f(2)>CNT_F_TH+30){rfloc_ans = true;}//右足浮いた状態から下げる
         if      (lfloc_ans  && lfw_in.f(2)<CNT_F_TH   ){lfloc_ans = false;}//左足ついた状態から上げる
@@ -430,8 +422,7 @@ class HumanSynchronizer{
       }else{//足下げ命令入力時
         cur_fup_phase_in -= omega_down * DT;
       }
-      if(cur_fup_phase_in > M_PI){cur_fup_phase_in = M_PI;}
-      else if(cur_fup_phase_in < 0){cur_fup_phase_in = 0;}
+      LIMIT_MINMAX( cur_fup_phase_in, 0, M_PI);
       double fup_height = f_height_out.p(2)-f_height_out.offs(2);
       LIMIT_MINMAX( fup_height, 0.03, 0.10 );
       f_height_out.p(2) = fup_height/2 * (1-cos(cur_fup_phase_in)) + f_height_out.offs(2);
@@ -455,13 +446,14 @@ class HumanSynchronizer{
     }
     void applyEEWorkspaceLimit(HumanPose& tgt){
       const double MAX = 0.30;
+      const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
       if(!is_rf_contact && is_lf_contact){//右足浮遊時
         const hrp::Vector2 lf2rf_vec( tgt.getP("rf").p(0)-pre_cont_lfpos(0), tgt.getP("rf").p(1)-pre_cont_lfpos(1) );
         if(lf2rf_vec.norm() > MAX){
           tgt.getP("rf").p(0) = (lf2rf_vec.normalized())(0) * MAX + pre_cont_lfpos(0);
           tgt.getP("rf").p(1) = (lf2rf_vec.normalized())(1) * MAX + pre_cont_lfpos(1);
         }
-        if(tgt.getP("rf").p(1) > pre_cont_lfpos(1) + 0.05 ) tgt.getP("rf").p(1) = pre_cont_lfpos(1) + 0.05;
+        LIMIT_MAX(tgt.getP("rf").p(1), pre_cont_lfpos(1) - FOOT_2_FOOT_COLLISION_MARGIIN);
       }
       else if(is_rf_contact && !is_lf_contact){//左足浮遊時
         const hrp::Vector2 rf2lf_vec( tgt.getP("lf").p(0)-pre_cont_rfpos(0), tgt.getP("lf").p(1)-pre_cont_rfpos(1) );
@@ -469,7 +461,7 @@ class HumanSynchronizer{
           tgt.getP("lf").p(0) = (rf2lf_vec.normalized())(0) * MAX + pre_cont_rfpos(0);
           tgt.getP("lf").p(1) = (rf2lf_vec.normalized())(1) * MAX + pre_cont_rfpos(1);
         }
-        if(tgt.getP("lf").p(1) < pre_cont_rfpos(1) - 0.05 ) tgt.getP("lf").p(1) = pre_cont_rfpos(1) - 0.05;
+        LIMIT_MIN(tgt.getP("lf").p(1), pre_cont_rfpos(1) + FOOT_2_FOOT_COLLISION_MARGIIN);
       }
       hrp::Vector3 base2rh = tgt.getP("rh").offs - init_basepos;
       hrp::Vector3 base2lh = tgt.getP("lh").offs - init_basepos;
@@ -491,35 +483,21 @@ class HumanSynchronizer{
       if(!isPointInHullOpenCV(cog,hull)){ calcNearestPointOnHull(cog,hull,cog); }//外に出たら最近傍点に頭打ち
       comin_abs.p(0) = cog(0);
       comin_abs.p(1) = cog(1);
-      LIMIT_MINMAX( comin_abs.p(2), comin_abs.offs(2) - 0.15, comin_abs.offs(2) + 0.03 );
+      LIMIT_MINMAX( comin_abs.p(2), comin_abs.offs(2) - 0.15, comin_abs.offs(2) + 0.03 );//COM高さ方向の制限
       return true;
     }
     bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& com_old, const hrp::Vector3& com_vel_old, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans){
       com_ans = com_in;
       hrp::Vector3 com_vel = (com_in - com_old)/DT;
-      hrp::Vector3 com_acc = (com_vel - com_vel_old)/DT;//命名規則
-      hrp::Vector3 com_vel_zmpOk, com_vel_zmpOk_cpOk;
-      double accel[4] = {0.05, -0.03, 0.02, -0.02};
-      double decel[4] = {0.04, -0.02, 0.01, -0.01};
-      comacc_ref = calcacc_v_filters.passFilter(com_acc);
+//      hrp::Vector3 com_acc = (com_vel - com_vel_old)/DT;//命名規則
+//      hrp::Vector3 com_vel_zmpOk, com_vel_zmpOk_cpOk;
+//      double accel[4] = {0.05, -0.03, 0.02, -0.02};
+//      double decel[4] = {0.04, -0.02, 0.01, -0.01};
+//      comacc_ref = calcacc_v_filters.passFilter(com_acc);
       const double H = rp_wld_initpos.getP("com").p(2);
-//      double maxacc[2],minacc[2];
-//      maxacc[0] = (com_old(0) - accel[1]) * (G/H); if(maxacc[0] < 0){maxacc[0]=0;}
-//      minacc[0] = (com_old(0) - accel[0]) * (G/H); if(minacc[0] > 0){minacc[0]=0;}
-//      maxacc[1] = (com_old(1) - (accel[3]-0.1)) * (G/H); if(maxacc[1] < 0){maxacc[1]=0;}
-//      minacc[1] = (com_old(1) - (accel[2]+0.1)) * (G/H); if(minacc[1] > 0){minacc[1]=0;}
-//      if(com_vel(0) > maxacc[0] * DT + com_vel_old(0)){com_vel(0) = maxacc[0] * DT + com_vel_old(0); cout<<"maxacc[0] over"<<endl;}
-//      if(com_vel(0) < minacc[0] * DT + com_vel_old(0)){com_vel(0) = minacc[0] * DT + com_vel_old(0); cout<<"minacc[0] over"<<endl;}
-//      if(com_vel(1) > maxacc[1] * DT + com_vel_old(1)){com_vel(1) = maxacc[1] * DT + com_vel_old(1); cout<<"maxacc[1] over"<<endl;}
-//      if(com_vel(1) < minacc[1] * DT + com_vel_old(1)){com_vel(1) = minacc[1] * DT + com_vel_old(1); cout<<"minacc[1] over"<<endl;}
-//      if(comacc_ref(0) > maxacc[0]){com_vel(0) = maxacc[0] * DT + com_vel_old(0); cout<<"maxacc[0] over"<<endl;}
-//      if(comacc_ref(0) < minacc[0]){com_vel(0) = minacc[0] * DT + com_vel_old(0); cout<<"minacc[0] over"<<endl;}
-//      if(comacc_ref(1) > maxacc[1]){com_vel(1) = maxacc[1] * DT + com_vel_old(1); cout<<"maxacc[1] over"<<endl;}
-//      if(comacc_ref(1) < minacc[1]){com_vel(1) = minacc[1] * DT + com_vel_old(1); cout<<"minacc[1] over"<<endl;}
-//      cout<<"comacc_ref(1) "<<comacc_ref(1)<<" minacc[1] "<<minacc[1]<<endl;
-////      cout<<"maxacc "<<maxacc[1]<<" minacc "<<minacc[1]<< " acc " << (com_vel(1)-com_vel_old(1))/DT <<endl;
       std::vector<hrp::Vector2> hull;
-      createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull);
+      hrp::Vector4 marginDelta(+0.001,-0.001,0.001,-0.001);
+      createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region+marginDelta, lf_safe_region+marginDelta, hull);
       hrp::Vector2 com_vel_ans_2d;
 //      regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);
       regulateCOMVelocityByCapturePointXY( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);//XY独立に計算する手法のほうがエッジで引っかからない・・・
@@ -541,17 +519,23 @@ class HumanSynchronizer{
     void regulateCOMVelocityByCapturePointXY(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
       com_vel_ans = com_vel;
       const double H = rp_wld_initpos.getP("com").p(2);
+      enum Direc{F,B,L,R} direc;
       hrp::Vector4 margin_from_4maxcp;
       calcXYMarginToHull(com_pos, hull, margin_from_4maxcp);
-      hrp::Vector3 max_com_vel,min_com_vel;
-      max_com_vel(0) = margin_from_4maxcp(0) / sqrt( H / G );
-      min_com_vel(0) = margin_from_4maxcp(1) / sqrt( H / G );
-      max_com_vel(1) = margin_from_4maxcp(2) / sqrt( H / G );
-      min_com_vel(1) = margin_from_4maxcp(3) / sqrt( H / G );
-      if(com_vel(0) > max_com_vel(0)){ com_vel_ans(0) = max_com_vel(0); if(loop%100==0)cout<<"F cp over"<<endl;}
-      if(com_vel(0) < min_com_vel(0)){ com_vel_ans(0) = min_com_vel(0); if(loop%100==0)cout<<"B cp over"<<endl;}
-      if(com_vel(1) > max_com_vel(1)){ com_vel_ans(1) = max_com_vel(1); if(loop%100==0)cout<<"L cp over"<<endl;}
-      if(com_vel(1) < min_com_vel(1)){ com_vel_ans(1) = min_com_vel(1); if(loop%100==0)cout<<"R cp over"<<endl;}
+      hrp::Vector4 com_vel_decel_limit,com_vel_accel_limit;
+      const double accel_cp_tolerance = 0.01;//境界上でcom_vel_accel_limitが0になって抜け出せなくなるのを防ぐため
+      com_vel_decel_limit(F) = margin_from_4maxcp(F) / sqrt( H / G );
+      com_vel_decel_limit(B) = margin_from_4maxcp(B) / sqrt( H / G );
+      com_vel_decel_limit(L) = margin_from_4maxcp(L) / sqrt( H / G );
+      com_vel_decel_limit(R) = margin_from_4maxcp(R) / sqrt( H / G );
+      com_vel_accel_limit(F) = (-margin_from_4maxcp(B)+accel_cp_tolerance) / sqrt( H / G );
+      com_vel_accel_limit(B) = (-margin_from_4maxcp(F)-accel_cp_tolerance) / sqrt( H / G );
+      com_vel_accel_limit(L) = (-margin_from_4maxcp(R)+accel_cp_tolerance) / sqrt( H / G );
+      com_vel_accel_limit(R) = (-margin_from_4maxcp(L)-accel_cp_tolerance) / sqrt( H / G );
+      LIMIT_MINMAX( com_vel_ans(0), com_vel_decel_limit(B), com_vel_decel_limit(F) );
+      LIMIT_MINMAX( com_vel_ans(1), com_vel_decel_limit(R), com_vel_decel_limit(L) );
+//      LIMIT_MINMAX( com_vel_ans(0), com_vel_accel_limit(B), com_vel_accel_limit(F) );//加速時のCP
+//      LIMIT_MINMAX( com_vel_ans(1), com_vel_accel_limit(R), com_vel_accel_limit(L) );
     }
     void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const Wrench6& rfwin, const Wrench6& lfwin, hrp::Vector3& zmp_ans);
     void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans);

From dcd25e9ca53132f55ef1d14df8c651cdcf02038e Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 21 Jan 2017 00:35:31 +0900
Subject: [PATCH 053/217] apply auto ee rotation horizon

---
 rtc/AutoBalancer/HumanMasterSlave.h | 28 ++++++++++++++++++++++------
 1 file changed, 22 insertions(+), 6 deletions(-)

diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index ddfd42ca831..f50b8cf57a7 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -191,7 +191,6 @@ class HumanSynchronizer{
     HRPPose3D head_cam_pose;
     hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
 
-
     interpolator *init_zmp_com_offset_interpolator;
     double zmp_com_offset_ip_ratio;
 
@@ -235,9 +234,9 @@ class HumanSynchronizer{
       tgt_rot_filters.resize(5);
 //      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(0.5,HZ);//四肢拘束点用
       for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(0.6,HZ);//四肢拘束点用(position)
-      for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(0.6,HZ);//四肢拘束点用(Rotation)
-      tgt_pos_filters[0].setParameter(0.4,HZ);//重心用
-      tgt_rot_filters[0].setParameter(0.4,HZ);//重心用
+      for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1,HZ);//四肢拘束点用(Rotation)
+      tgt_pos_filters[0].setParameter(0.4,HZ);//重心pos用
+      tgt_rot_filters[0].setParameter(0.4,HZ);//重心rot用
       calcacc_v_filters.setParameter(5,HZ);//加速度計算用
       acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
       cam_rpy_filter.setParameter(1,HZ);//カメラアングル
@@ -302,6 +301,8 @@ class HumanSynchronizer{
       lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
 
       applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com"));
+
+      setFootRotHorizontalIfGoLanding     (rp_ref_out_old, rp_ref_out);
       applyLPFilter                       (rp_ref_out);
       applyCOMStateLimitByCapturePoint    (rp_ref_out.getP("com").p, rp_ref_out_old.getP("com").p, com_vel_old, rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com").p);
 
@@ -405,6 +406,10 @@ class HumanSynchronizer{
         }
       }
     }
+    void setFootRotHorizontalIfGoLanding(const HumanPose& rp_ref_out_old, HumanPose& rp_ref_out){
+      if(rp_ref_out_old.getP("rf").p(2)-rp_ref_out_old.getP("rf").offs(2)<0.05){ rp_ref_out.getP("rf").rpy(0) = rp_ref_out.getP("rf").rpy(1) = 0; }
+      if(rp_ref_out_old.getP("lf").p(2)-rp_ref_out_old.getP("lf").offs(2)<0.05){ rp_ref_out.getP("lf").rpy(0) = rp_ref_out.getP("lf").rpy(1) = 0; }
+    }
     void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
       if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足接地しているタイミングでのみ更新
       double RFDOWN_TIME = fabs(tgt.getP("rf").p(1) - tgt.getP("com").p(1))*2 + FUP_TIME;//下ろす速度は遊脚の重心からの距離に比例したペナルティ
@@ -473,6 +478,17 @@ class HumanSynchronizer{
         LIMIT_MINMAX( tgt.getP("rf").p(2), tgt.getP("rf").offs(2), tgt.getP("rf").offs(2)+0.10);
         LIMIT_MINMAX( tgt.getP("lf").p(2), tgt.getP("lf").offs(2), tgt.getP("lf").offs(2)+0.10);
       }
+      std::string ns[5] = {"com","rf","lf","rh","lh"};
+      for(int i=0;i<5;i++){
+        LIMIT_MINMAX( tgt.getP(ns[i]).rpy(0), -30*M_PI/180, 30*M_PI/180 );
+        LIMIT_MINMAX( tgt.getP(ns[i]).rpy(1), -30*M_PI/180, 30*M_PI/180 );
+      }
+      LIMIT_MINMAX( tgt.getP("rf").rpy(2), -30*M_PI/180+tgt.getP("com").rpy(2), 30*M_PI/180+tgt.getP("com").rpy(2) );
+      LIMIT_MINMAX( tgt.getP("lf").rpy(2), -30*M_PI/180+tgt.getP("com").rpy(2), 30*M_PI/180+tgt.getP("com").rpy(2) );
+      LIMIT_MINMAX( tgt.getP("rh").rpy(2), -30*M_PI/180+tgt.getP("com").rpy(2), 30*M_PI/180+tgt.getP("com").rpy(2) );
+      LIMIT_MINMAX( tgt.getP("lh").rpy(2), -30*M_PI/180+tgt.getP("com").rpy(2), 30*M_PI/180+tgt.getP("com").rpy(2) );
+
+
       LIMIT_MINMAX( head_cam_pose.rpy(1), -20*M_PI/180, 20*M_PI/180 );
       LIMIT_MINMAX( head_cam_pose.rpy(2), -20*M_PI/180, 20*M_PI/180 );
     }
@@ -499,8 +515,8 @@ class HumanSynchronizer{
       hrp::Vector4 marginDelta(+0.001,-0.001,0.001,-0.001);
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region+marginDelta, lf_safe_region+marginDelta, hull);
       hrp::Vector2 com_vel_ans_2d;
-//      regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);
-      regulateCOMVelocityByCapturePointXY( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);//XY独立に計算する手法のほうがエッジで引っかからない・・・
+      regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);
+//      regulateCOMVelocityByCapturePointXY( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);//XY独立に計算する手法のほうがエッジで引っかからない・・・
       com_ans(0) = com_old(0) + com_vel_ans_2d(0) * DT;
       com_ans(1) = com_old(1) + com_vel_ans_2d(1) * DT;
       return true;

From 195802a2290432277239d3e94346b0052d7c1473 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 1 Feb 2017 21:23:45 +0900
Subject: [PATCH 054/217] enable rotaion and rot safety

---
 rtc/AutoBalancer/AutoBalancer.cpp   |  49 +++++----
 rtc/AutoBalancer/HumanMasterSlave.h | 161 +++++++++++++++++++---------
 2 files changed, 140 insertions(+), 70 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index af0374d7508..9c1052681c1 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -506,8 +506,8 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_htcom.data,hp_raw_data.getP("com")); }
     if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrf.data,hp_raw_data.getP("rf")); }
     if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlf.data,hp_raw_data.getP("lf")); }
-    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrh.data,hp_raw_data.getP("rh")); }
-    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlh.data,hp_raw_data.getP("lh")); }
+    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrh.data,hp_raw_data.getP("rh"));}
+    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlh.data,hp_raw_data.getP("lh"));}
     if (m_htcamIn.isNew()){ m_htcamIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_htcam.data,hsp->head_cam_pose); }
     if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
     hsp->readInput(hp_raw_data);
@@ -1020,6 +1020,8 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri
 bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param, const std::string& limb_name)
 {
   param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
+//  param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio,
+//      ikp[limb_name].localPos,ikp[limb_name].localR );
   // IK check
   hrp::Vector3 vel_p, vel_r;
   vel_p = param.target_p0 - param.target_link->p;
@@ -1054,12 +1056,22 @@ void AutoBalancer::solveWholeBodyCOMIK(const HRPPose3D& com_ref, const HRPPose3D
   m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
   m_robot->calcForwardKinematics();//apply
   hrp::Vector3 tmp_com_err = com_ref.p - m_robot->calcCM();
-  ikp["rleg"].target_p0 = rf_ref.p;
-  ikp["rleg"].target_r0 = hrp::rotFromRpy(rf_ref.rpy);
-  ikp["lleg"].target_p0 = lf_ref.p;
-  ikp["lleg"].target_r0 = hrp::rotFromRpy(lf_ref.rpy);
-  if(ikp.count("rarm")){ ikp["rarm"].target_p0 = rh_ref.p; ikp["rarm"].target_r0 = hrp::rotFromRpy(rh_ref.rpy); }
-  if(ikp.count("larm")){ ikp["larm"].target_p0 = lh_ref.p; ikp["larm"].target_r0 = hrp::rotFromRpy(lh_ref.rpy); }
+  if(ikp.count("rleg")){
+    ikp["rleg"].target_r0 = hrp::rotFromRpy(rf_ref.rpy) * ikp["rleg"].localR.transpose();//convert End Effector pos into link origin pos
+    ikp["rleg"].target_p0 = rf_ref.p - ikp["rleg"].target_r0 * ikp["rleg"].localPos;
+  }
+  if(ikp.count("lleg")){
+    ikp["lleg"].target_r0 = hrp::rotFromRpy(lf_ref.rpy) * ikp["lleg"].localR.transpose();
+    ikp["lleg"].target_p0 = lf_ref.p - ikp["lleg"].target_r0 * ikp["lleg"].localPos;
+  }
+  if(ikp.count("rarm")){
+    ikp["rarm"].target_r0 = hrp::rotFromRpy(rh_ref.rpy) * ikp["rarm"].localR.transpose();
+    ikp["rarm"].target_p0 = rh_ref.p - ikp["rarm"].target_r0 * ikp["rarm"].localPos;
+  }
+  if(ikp.count("larm")){
+    ikp["larm"].target_r0 = hrp::rotFromRpy(lh_ref.rpy) * ikp["larm"].localR.transpose();
+    ikp["larm"].target_p0 = lh_ref.p - ikp["larm"].target_r0 * ikp["larm"].localPos;
+  }
   while(tmp_com_err.norm() > COM_IK_MAX_ERROR){
     m_robot->rootLink()->p += tmp_com_err;
     m_robot->calcForwardKinematics();
@@ -1128,15 +1140,17 @@ void AutoBalancer::solveLimbIK ()
     if(ikp.count("rarm"))hsp->rp_wld_initpos.getP("rh").p = ikp["rarm"].target_link->p;
     if(ikp.count("larm"))hsp->rp_wld_initpos.getP("lh").p = ikp["larm"].target_link->p;
 
-    hsp->rp_ref_out.getP("com").offs = m_robot->calcCM();
-    hsp->rp_ref_out.getP("rf").offs = ikp["rleg"].target_link->p;
-    hsp->rp_ref_out.getP("lf").offs = ikp["lleg"].target_link->p;
-    hsp->rp_ref_out.getP("zmp").offs = ref_zmp;
-    if(ikp.count("rarm"))hsp->rp_ref_out.getP("rh").offs = ikp["rarm"].target_link->p;
-    if(ikp.count("larm"))hsp->rp_ref_out.getP("lh").offs = ikp["larm"].target_link->p;
-    hsp->pre_cont_rfpos = hsp->rp_ref_out.getP("rf").offs;
-    hsp->pre_cont_lfpos = hsp->rp_ref_out.getP("lf").offs;
-
+    hsp->rp_ref_out.getP("com").p_offs = m_robot->calcCM();
+    hsp->rp_ref_out.getP("zmp").p_offs = ref_zmp;
+    const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"}, human_l_names[4] = {"rf","lf","rh","lh"};
+    for(int i=0;i<4;i++){
+      if(ikp.count(robot_l_names[i])){
+        hsp->rp_ref_out.getP(human_l_names[i]).p_offs = ikp[robot_l_names[i]].target_link->p + ikp[robot_l_names[i]].target_link->R * ikp[robot_l_names[i]].localPos;
+        hsp->rp_ref_out.getP(human_l_names[i]).rpy_offs = hrp::rpyFromRot(ikp[robot_l_names[i]].target_link->R * ikp[robot_l_names[i]].localR);
+      }
+    }
+    hsp->pre_cont_rfpos = hsp->rp_ref_out.getP("rf").p_offs;
+    hsp->pre_cont_lfpos = hsp->rp_ref_out.getP("lf").p_offs;
     hsp->init_basepos = m_robot->rootLink()->p;
   }
   hsp->update();//////HumanSynchronizerの主要処理
@@ -1162,7 +1176,6 @@ void AutoBalancer::solveLimbIK ()
     //outport用のデータ上書き
     ref_zmp = hsp->rp_ref_out.getP("zmp").p;
     ref_cog = hsp->rp_ref_out.getP("com").p;
-
   }else{
 	  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {//本来のIK部分
 	    if (it->second.is_active) solveLimbIKforLimb(it->second, it->first);
diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index f50b8cf57a7..424a0522f19 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -45,46 +45,45 @@ extern "C" {
 
 class BiquadIIRFilterVec{
   private:
-    IIRFilter x_filters,y_filters,z_filters;
+    IIRFilter filters[3];
     hrp::Vector3 ans;
     std::vector<double> fb_coeffs, ff_coeffs;
   public:
     BiquadIIRFilterVec(const std::string& error_prefix = ""){
     };
     ~BiquadIIRFilterVec() {};
-    bool setParameter(const double fc_in, const double HZ){
-      double fc = tan(fc_in * M_PI / HZ) / (2 * M_PI);
-      double denom = 1 + (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc;
+    void setParameter(const hrp::Vector3 fc_in, const double HZ){
       ff_coeffs.resize(3);
       fb_coeffs.resize(3);
-      ff_coeffs[0] = (4 * M_PI * M_PI * fc*fc) / denom;
-      ff_coeffs[1] = (8 * M_PI * M_PI * fc*fc) / denom;
-      ff_coeffs[2] = (4 * M_PI * M_PI * fc*fc) / denom;
-      fb_coeffs[0] = 1.0;
-      fb_coeffs[1] = (8 * M_PI * M_PI * fc*fc - 2) / denom;
-      fb_coeffs[2] = (1 - (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc) / denom;
-      return (
-        x_filters.setParameter(2,fb_coeffs,ff_coeffs) &&
-        y_filters.setParameter(2,fb_coeffs,ff_coeffs) &&
-        z_filters.setParameter(2,fb_coeffs,ff_coeffs)
-      );
+      for(int i=0;i<3;i++){
+        const double fc = tan(fc_in(i) * M_PI / HZ) / (2 * M_PI);
+        const double denom = 1 + (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc;
+        ff_coeffs[0] = (4 * M_PI * M_PI * fc*fc) / denom;
+        ff_coeffs[1] = (8 * M_PI * M_PI * fc*fc) / denom;
+        ff_coeffs[2] = (4 * M_PI * M_PI * fc*fc) / denom;
+        fb_coeffs[0] = 1.0;
+        fb_coeffs[1] = (8 * M_PI * M_PI * fc*fc - 2) / denom;
+        fb_coeffs[2] = (1 - (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc) / denom;
+        filters[i].setParameter(2,fb_coeffs,ff_coeffs);
+      }
+    };
+    void setParameter(const double fc_in, const double HZ){
+      setParameter(hrp::Vector3(fc_in,fc_in,fc_in), HZ);
     };
     hrp::Vector3 passFilter(const hrp::Vector3& input){
-      ans(0) = x_filters.passFilter(input(0));
-      ans(1) = y_filters.passFilter(input(1));
-      ans(2) = z_filters.passFilter(input(2));
+      for(int i=0;i<3;i++){ ans(i) = filters[i].passFilter(input(i)); }
       return ans;
     };
 };
 
 class HRPPose3D{
   public:
-    hrp::Vector3 p,offs,rpy;
+    hrp::Vector3 p,p_offs,rpy,rpy_offs;
     hrp::Matrix33 R;
     HRPPose3D(){ clear(); }
     ~HRPPose3D(){}
     void clear(){
-      p = offs = rpy = hrp::Vector3::Zero();
+      p = p_offs = rpy = hrp::Vector3::Zero();
       R = hrp::Matrix33::Zero();
     }
 };
@@ -97,9 +96,10 @@ class Wrench6{
 };
 
 class HumanPose{
-  public:
+  private:
     std::map<std::string, HRPPose3D> P;
     std::map<std::string, Wrench6> w;
+  public:
     HumanPose(){
       P["com"] = HRPPose3D();
       P["rf"] = HRPPose3D();
@@ -190,6 +190,7 @@ class HumanSynchronizer{
     hrp::Vector3 cam_pos_filtered,cam_rpy_filtered;
     HRPPose3D head_cam_pose;
     hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
+    std::vector<hrp::Vector3> rf_vert,lf_vert;
 
     interpolator *init_zmp_com_offset_interpolator;
     double zmp_com_offset_ip_ratio;
@@ -234,17 +235,28 @@ class HumanSynchronizer{
       tgt_rot_filters.resize(5);
 //      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(0.5,HZ);//四肢拘束点用
       for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(0.6,HZ);//四肢拘束点用(position)
-      for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1,HZ);//四肢拘束点用(Rotation)
+      for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0,HZ);//四肢拘束点用(Rotation)
       tgt_pos_filters[0].setParameter(0.4,HZ);//重心pos用
       tgt_rot_filters[0].setParameter(0.4,HZ);//重心rot用
+      tgt_pos_filters[1].setParameter(hrp::Vector3(0.6,0.6,1.5),HZ);//右足pos用
+      tgt_pos_filters[2].setParameter(hrp::Vector3(0.6,0.6,1.5),HZ);//左足pos用
       calcacc_v_filters.setParameter(5,HZ);//加速度計算用
       acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
       cam_rpy_filter.setParameter(1,HZ);//カメラアングル
 
       rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
       lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
-
-      use_rh = use_lh = false;
+      rf_vert.push_back(hrp::Vector3(0.13,-0.08,0));
+      rf_vert.push_back(hrp::Vector3(-0.10,-0.08,0));
+      rf_vert.push_back(hrp::Vector3(-0.10,0.06,0));
+      rf_vert.push_back(hrp::Vector3(0.13,0.06,0));
+      lf_vert.push_back(hrp::Vector3(0.13,-0.06,0));
+      lf_vert.push_back(hrp::Vector3(-0.10,-0.06,0));
+      lf_vert.push_back(hrp::Vector3(-0.10,0.08,0));
+      lf_vert.push_back(hrp::Vector3(0.13,0.08,0));
+
+//      use_rh = use_lh = false;
+      use_rh = use_lh = true;
 
       rp_ref_out_old.clear();
       rp_ref_out.clear();
@@ -299,6 +311,8 @@ class HumanSynchronizer{
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
       applyEEWorkspaceLimit               (rp_ref_out);
       lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
+      if(go_rf_landing)rp_ref_out.getP("rf").p(2) = rp_ref_out.getP("rf").p_offs(2);
+      if(go_lf_landing)rp_ref_out.getP("lf").p(2) = rp_ref_out.getP("lf").p_offs(2);
 
       applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com"));
 
@@ -312,6 +326,7 @@ class HumanSynchronizer{
       applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       applyCOMZMPXYZLock                  (rp_ref_out);
       overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
+      modifyFootRotAndXYForContact        (rp_ref_out.getP("rf"), rp_ref_out.getP("lf"));
       com_vel_old = (rp_ref_out.getP("com").p - rp_ref_out_old.getP("com").p)/DT;
 
       rp_ref_out_old = rp_ref_out;
@@ -349,8 +364,8 @@ class HumanSynchronizer{
     void removeInitOffsetPose(const HumanPose& abs_in, const HumanPose& init_offset, HumanPose& rel_out){
       std::string ns[5] = {"com","rf","lf","rh","lh"};
       for(int i=0;i<5;i++){
-        rel_out.getP(ns[i]).p = abs_in.getP(ns[i]).p - init_offset.getP(ns[i]).p;
-        rel_out.getP(ns[i]).rpy = abs_in.getP(ns[i]).rpy;
+        rel_out.getP(ns[i]).p   = abs_in.getP(ns[i]).p   - init_offset.getP(ns[i]).p;
+        rel_out.getP(ns[i]).rpy = abs_in.getP(ns[i]).rpy - init_offset.getP(ns[i]).rpy_offs;
       }
       rel_out.getw("rfw") = abs_in.getw("rfw");
       rel_out.getw("lfw") = abs_in.getw("lfw");
@@ -374,8 +389,9 @@ class HumanSynchronizer{
     void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
       std::string ns[6] = {"com","rf","lf","rh","lh","zmp"};
       for(int i=0;i<6;i++){
-        rp_out.getP(ns[i]).p =  h2r_ratio * hp_in.getP(ns[i]).p + rp_out.getP(ns[i]).offs;
-        rp_out.getP(ns[i]).rpy =  hp_in.getP(ns[i]).rpy;
+        rp_out.getP(ns[i]).p   =  h2r_ratio * hp_in.getP(ns[i]).p   + rp_out.getP(ns[i]).p_offs;
+        rp_out.getP(ns[i]).rpy =  hrp::rpyFromRot( hrp::rotFromRpy(hp_in.getP(ns[i]).rpy) * hrp::rotFromRpy(rp_out.getP(ns[i]).rpy_offs) );
+//        rp_out.getP(ns[i]).rpy =         hrp::rpyFromRot(        hrp::rotFromRpy(rp_out.getP(ns[i]).rpy_offs) * hrp::rotFromRpy(hp_in.getP(ns[i]).rpy) );
       }
       rp_out.getw("rfw")  = hp_in.getw("rfw");
       rp_out.getw("lfw")  = hp_in.getw("lfw");
@@ -407,8 +423,10 @@ class HumanSynchronizer{
       }
     }
     void setFootRotHorizontalIfGoLanding(const HumanPose& rp_ref_out_old, HumanPose& rp_ref_out){
-      if(rp_ref_out_old.getP("rf").p(2)-rp_ref_out_old.getP("rf").offs(2)<0.05){ rp_ref_out.getP("rf").rpy(0) = rp_ref_out.getP("rf").rpy(1) = 0; }
-      if(rp_ref_out_old.getP("lf").p(2)-rp_ref_out_old.getP("lf").offs(2)<0.05){ rp_ref_out.getP("lf").rpy(0) = rp_ref_out.getP("lf").rpy(1) = 0; }
+//      if(rp_ref_out_old.getP("rf").p(2)-rp_ref_out_old.getP("rf").p_offs(2)<0.05){ rp_ref_out.getP("rf").rpy(0) = rp_ref_out.getP("rf").rpy(1) = 0; }
+//      if(rp_ref_out_old.getP("lf").p(2)-rp_ref_out_old.getP("lf").p_offs(2)<0.05){ rp_ref_out.getP("lf").rpy(0) = rp_ref_out.getP("lf").rpy(1) = 0; }
+      if(go_rf_landing){ rp_ref_out.getP("rf").rpy(0) = rp_ref_out.getP("rf").rpy(1) = 0; }
+      if(go_lf_landing){ rp_ref_out.getP("lf").rpy(0) = rp_ref_out.getP("lf").rpy(1) = 0; }
     }
     void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
       if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足接地しているタイミングでのみ更新
@@ -416,22 +434,61 @@ class HumanSynchronizer{
       double LFDOWN_TIME = fabs(tgt.getP("lf").p(1) - tgt.getP("com").p(1))*2 + FUP_TIME;
       LIMIT_MINMAX( RFDOWN_TIME, FUP_TIME, 0.8);
       LIMIT_MINMAX( LFDOWN_TIME, FUP_TIME, 0.8);
-      calcFootUpCurveAndJudgeFootContact(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.getP("rf"), is_rf_contact);
-      calcFootUpCurveAndJudgeFootContact(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.getP("lf"), is_lf_contact);
-    }
-    void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, const double& fup_time_in, const double& fdown_time_in,  double& cur_fup_phase_in, HRPPose3D& f_height_out, bool& is_f_contact_out){
-      const double omega_up = M_PI / fup_time_in;
-      const double omega_down = M_PI / fdown_time_in;
-      if(!go_land_in){//足上げ命令入力時
-        cur_fup_phase_in += omega_up * DT;
-      }else{//足下げ命令入力時
-        cur_fup_phase_in -= omega_down * DT;
+//      calcFootUpCurveAndJudgeFootContact(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.getP("rf"), is_rf_contact);
+//      calcFootUpCurveAndJudgeFootContact(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.getP("lf"), is_lf_contact);
+      const double com2rf_dist = fabs(tgt.getP("rf").p(1) - tgt.getP("com").p(1));
+      const double com2lf_dist = fabs(tgt.getP("lf").p(1) - tgt.getP("com").p(1));
+      limitGroundContactVelocity(rf_goland_in, com2rf_dist, rp_ref_out_old.getP("rf"), tgt.getP("rf"), is_rf_contact);
+      limitGroundContactVelocity(lf_goland_in, com2lf_dist, rp_ref_out_old.getP("lf"), tgt.getP("lf"), is_lf_contact);
+    }
+//    void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, const double& fup_time_in, const double& fdown_time_in,  double& cur_fup_phase_in, HRPPose3D& f_height_out, bool& is_f_contact_out){
+//      const double omega_up = M_PI / fup_time_in;
+//      const double omega_down = M_PI / fdown_time_in;
+//      if(!go_land_in){//足上げ命令入力時
+//        cur_fup_phase_in += omega_up * DT;
+//      }else{//足下げ命令入力時
+//        cur_fup_phase_in -= omega_down * DT;
+//      }
+//      LIMIT_MINMAX( cur_fup_phase_in, 0, M_PI);
+//      double fup_height = f_height_out.p(2)-f_height_out.p_offs(2);
+//      LIMIT_MINMAX( fup_height, 0.03, 0.10 );
+//      f_height_out.p(2) = fup_height/2 * (1-cos(cur_fup_phase_in)) + f_height_out.p_offs(2);
+//      is_f_contact_out = (cur_fup_phase_in <= 0);
+//    }
+    void limitGroundContactVelocity(const bool& go_land_in, const double& com2foot_dist, const HRPPose3D& f_old_in, HRPPose3D& f_in_out, bool& is_f_contact_out){
+      double penalty = 1.0 + (0.2 - com2foot_dist) * 5.0;//1.0 ~ 2.0
+      LIMIT_MINMAX( penalty, 1.0, 2.0);
+      LIMIT_MIN( f_in_out.p(2), f_in_out.p_offs(2));
+      double input_vel = (f_in_out.p(2) - f_old_in.p(2)) / DT;
+      const double vel_limit_k = 8.0 * penalty;//地面から0.02[m]地点で0.02*8=0.16[m/s]出ている計算
+      double limit_vel = - vel_limit_k * (f_old_in.p(2) - f_old_in.p_offs(2));//sinの時は平均0.05~0.10[m/s]
+      const double max_vel_threshold = -0.01;
+      LIMIT_MAX( limit_vel, max_vel_threshold);//速度0に近づくと目標になかなか到達しないから
+      LIMIT_MIN( input_vel, limit_vel);//着地時の下向きの速度を制限
+      f_in_out.p(2) = f_old_in.p(2) + input_vel * DT;
+      LIMIT_MIN( f_in_out.p(2), f_in_out.p_offs(2));
+      const double contact_threshold = 0.005;
+      is_f_contact_out = ((f_in_out.p(2)-f_in_out.p_offs(2)) <= contact_threshold);
+    }
+    void modifyFootRotAndXYForContact(HRPPose3D& rf_in, HRPPose3D& lf_in){
+      calcFootRotAndXYTransitionForContact(rf_in,rf_vert);
+      calcFootRotAndXYTransitionForContact(lf_in,lf_vert);
+    }
+    void calcFootRotAndXYTransitionForContact(HRPPose3D& foot_in, const std::vector<hrp::Vector3>& act_sole_size){
+      std::vector<hrp::Vector3> act_foot_vert;
+      double min_height = foot_in.p(2);
+      int min_height_id = 0;
+      for(int i=0;i<act_sole_size.size();i++){
+        act_foot_vert.push_back(hrp::rotFromRpy(foot_in.rpy) * act_sole_size[i]);
+        act_foot_vert[i](2) += foot_in.p(2);
+        if(act_foot_vert[i](2) < min_height){
+          min_height = act_foot_vert[i](2);
+          min_height_id = i;
+        }
+      }
+      if(act_foot_vert[min_height_id](2) < foot_in.p_offs(2)){
+        foot_in.p(2) += foot_in.p_offs(2) - act_foot_vert[min_height_id](2);
       }
-      LIMIT_MINMAX( cur_fup_phase_in, 0, M_PI);
-      double fup_height = f_height_out.p(2)-f_height_out.offs(2);
-      LIMIT_MINMAX( fup_height, 0.03, 0.10 );
-      f_height_out.p(2) = fup_height/2 * (1-cos(cur_fup_phase_in)) + f_height_out.offs(2);
-      is_f_contact_out = (cur_fup_phase_in <= 0);
     }
     void lockFootXYOnContact(const bool& rfcs_in, const bool& lfcs_in, HumanPose& tgt){
       if(rfcs_in){
@@ -468,16 +525,16 @@ class HumanSynchronizer{
         }
         LIMIT_MIN(tgt.getP("lf").p(1), pre_cont_rfpos(1) + FOOT_2_FOOT_COLLISION_MARGIIN);
       }
-      hrp::Vector3 base2rh = tgt.getP("rh").offs - init_basepos;
-      hrp::Vector3 base2lh = tgt.getP("lh").offs - init_basepos;
+      hrp::Vector3 base2rh = tgt.getP("rh").p_offs - init_basepos;
+      hrp::Vector3 base2lh = tgt.getP("lh").p_offs - init_basepos;
       hrp::Vector3 hand_ulimit = hrp::Vector3(0.1,0.1,0.1);
       hrp::Vector3 hand_llimit = hrp::Vector3(-0.1,-0.1,-0.1);
       for(int i=0;i<3;i++){
         LIMIT_MINMAX( tgt.getP("rh").p(i), (current_basepos+base2rh+hand_llimit)(i), (current_basepos+base2rh+hand_ulimit)(i) );
         LIMIT_MINMAX( tgt.getP("lh").p(i), (current_basepos+base2lh+hand_llimit)(i), (current_basepos+base2lh+hand_ulimit)(i) );
-        LIMIT_MINMAX( tgt.getP("rf").p(2), tgt.getP("rf").offs(2), tgt.getP("rf").offs(2)+0.10);
-        LIMIT_MINMAX( tgt.getP("lf").p(2), tgt.getP("lf").offs(2), tgt.getP("lf").offs(2)+0.10);
       }
+      LIMIT_MINMAX( tgt.getP("rf").p(2), tgt.getP("rf").p_offs(2), tgt.getP("rf").p_offs(2)+0.10);
+      LIMIT_MINMAX( tgt.getP("lf").p(2), tgt.getP("lf").p_offs(2), tgt.getP("lf").p_offs(2)+0.10);
       std::string ns[5] = {"com","rf","lf","rh","lh"};
       for(int i=0;i<5;i++){
         LIMIT_MINMAX( tgt.getP(ns[i]).rpy(0), -30*M_PI/180, 30*M_PI/180 );
@@ -499,7 +556,7 @@ class HumanSynchronizer{
       if(!isPointInHullOpenCV(cog,hull)){ calcNearestPointOnHull(cog,hull,cog); }//外に出たら最近傍点に頭打ち
       comin_abs.p(0) = cog(0);
       comin_abs.p(1) = cog(1);
-      LIMIT_MINMAX( comin_abs.p(2), comin_abs.offs(2) - 0.15, comin_abs.offs(2) + 0.03 );//COM高さ方向の制限
+      LIMIT_MINMAX( comin_abs.p(2), comin_abs.p_offs(2) - 0.15, comin_abs.p_offs(2) + 0.03 );//COM高さ方向の制限
       return true;
     }
     bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& com_old, const hrp::Vector3& com_vel_old, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans){
@@ -550,8 +607,8 @@ class HumanSynchronizer{
       com_vel_accel_limit(R) = (-margin_from_4maxcp(L)-accel_cp_tolerance) / sqrt( H / G );
       LIMIT_MINMAX( com_vel_ans(0), com_vel_decel_limit(B), com_vel_decel_limit(F) );
       LIMIT_MINMAX( com_vel_ans(1), com_vel_decel_limit(R), com_vel_decel_limit(L) );
-//      LIMIT_MINMAX( com_vel_ans(0), com_vel_accel_limit(B), com_vel_accel_limit(F) );//加速時のCP
-//      LIMIT_MINMAX( com_vel_ans(1), com_vel_accel_limit(R), com_vel_accel_limit(L) );
+      LIMIT_MINMAX( com_vel_ans(0), com_vel_accel_limit(B), com_vel_accel_limit(F) );//加速時のCP
+      LIMIT_MINMAX( com_vel_ans(1), com_vel_accel_limit(R), com_vel_accel_limit(L) );
     }
     void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const Wrench6& rfwin, const Wrench6& lfwin, hrp::Vector3& zmp_ans);
     void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans);

From 81ef0c4e619ad7aa1e880bf34b3141931f21969a Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 8 Feb 2017 10:36:00 +0900
Subject: [PATCH 055/217] cam -> head

---
 rtc/AutoBalancer/AutoBalancer.cpp     |  6 +++---
 rtc/AutoBalancer/AutoBalancer.h       |  4 ++--
 rtc/AutoBalancer/HumanMasterSlave.cpp | 11 -----------
 rtc/AutoBalancer/HumanMasterSlave.h   | 26 ++++++++++++++++++++++++--
 4 files changed, 29 insertions(+), 18 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 9c1052681c1..6df0c51c584 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -70,7 +70,7 @@ AutoBalancer::AutoBalancer(RTC::Manager* manager)
       m_htrhIn("htrhIn", m_htrh),
       m_htlhIn("htlhIn", m_htlh),
       m_actzmpIn("actzmpIn", m_actzmp),
-      m_htcamIn("htcamIn", m_htcam),
+      m_htheadIn("htheadIn", m_hthead),
 
       m_qOut("q", m_qRef),
       m_zmpOut("zmpOut", m_zmp),
@@ -124,7 +124,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     addInPort("htrhIn", m_htrhIn);
     addInPort("htlhIn", m_htlhIn);
     addInPort("actzmpIn", m_actzmpIn);
-    addInPort("htcamIn", m_htcamIn);
+    addInPort("htheadIn", m_htheadIn);
 
     // Set OutPort buffer
     addOutPort("q", m_qOut);
@@ -508,7 +508,7 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlf.data,hp_raw_data.getP("lf")); }
     if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrh.data,hp_raw_data.getP("rh"));}
     if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlh.data,hp_raw_data.getP("lh"));}
-    if (m_htcamIn.isNew()){ m_htcamIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_htcam.data,hsp->head_cam_pose); }
+    if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_hthead.data,hsp->head_cam_pose); }
     if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
     hsp->readInput(hp_raw_data);
     hsp->current_basepos = m_robot->rootLink()->p;
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 763b47db88d..050680b2558 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -153,8 +153,8 @@ class AutoBalancer
   InPort<TimedPose3D> m_htrhIn;
   TimedPose3D m_htlh;
   InPort<TimedPose3D> m_htlhIn;
-  TimedPose3D m_htcam;
-  InPort<TimedPose3D> m_htcamIn;
+  TimedPose3D m_hthead;
+  InPort<TimedPose3D> m_htheadIn;
   TimedPoint3D m_htzmp;
   InPort<TimedPoint3D> m_htzmpIn;
   TimedPoint3D m_actzmp;
diff --git a/rtc/AutoBalancer/HumanMasterSlave.cpp b/rtc/AutoBalancer/HumanMasterSlave.cpp
index ae9a115b975..5c039b96b41 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.cpp
+++ b/rtc/AutoBalancer/HumanMasterSlave.cpp
@@ -17,17 +17,6 @@ void HumanSynchronizer::calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vecto
   }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
   zmp_ans(2) = 0;
 }
-
-void HumanSynchronizer::createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
-  std::vector<hrp::Vector2> points;
-  for(int i=0;i<2;i++){
-    for(int j=2;j<4;j++){
-      points.push_back(hrp::Vector2(rfin_abs(0) + rf_mgn(i),    rfin_abs(1) + rf_mgn(j)));
-      points.push_back(hrp::Vector2(lfin_abs(0) + lf_mgn(i),    lfin_abs(1) + lf_mgn(j)));
-    }
-  }
-  makeConvexHullOpenCV(points, hull_ans);
-}
 void HumanSynchronizer::calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans){
   hrp::Vector4 margin_abs;
   hrp::Vector2 cross_pt, anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index 424a0522f19..54730578a64 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -307,7 +307,7 @@ class HumanSynchronizer{
       if(loop==0)rp_ref_out_old = rp_ref_out;
 
       applyCOMMoveModRatio                (rp_ref_out);
-      judgeFootLandOnCommand              (hp_wld_raw.getw("rfw"), hp_wld_raw.getw("lfw"), go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
+//      judgeFootLandOnCommand              (hp_wld_raw.getw("rfw"), hp_wld_raw.getw("lfw"), go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
       applyEEWorkspaceLimit               (rp_ref_out);
       lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
@@ -610,8 +610,30 @@ class HumanSynchronizer{
       LIMIT_MINMAX( com_vel_ans(0), com_vel_accel_limit(B), com_vel_accel_limit(F) );//加速時のCP
       LIMIT_MINMAX( com_vel_ans(1), com_vel_accel_limit(R), com_vel_accel_limit(L) );
     }
+    void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
+      std::vector<hrp::Vector2> points;
+      for(int i=0;i<2;i++){
+        for(int j=2;j<4;j++){
+          points.push_back(hrp::Vector2(rfin_abs(0) + rf_mgn(i),    rfin_abs(1) + rf_mgn(j)));
+          points.push_back(hrp::Vector2(lfin_abs(0) + lf_mgn(i),    lfin_abs(1) + lf_mgn(j)));
+        }
+      }
+//      static unsigned int edge_time = 0;
+//      if(rp_ref_out_old.getP("rf").rpy(1)>0.001){
+//        points.clear();
+//        double mod = rfin_abs(0) + 0.13 * edge_time * 0.002;
+////        LIMIT_MAX( mod, rfin_abs(0) + 0.13 );
+//        for(int i=0;i<2;i++){
+//          for(int j=2;j<4;j++){
+//            points.push_back(hrp::Vector2(rfin_abs(0) + 0.13,    rfin_abs(1) + rf_mgn(j)));
+//            points.push_back(hrp::Vector2(lfin_abs(0) + lf_mgn(i),    lfin_abs(1) + lf_mgn(j)));
+//          }
+//        }
+//        edge_time++;
+//      }
+      makeConvexHullOpenCV(points, hull_ans);
+    }
     void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const Wrench6& rfwin, const Wrench6& lfwin, hrp::Vector3& zmp_ans);
-    void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans);
     void calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans);
     bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_will_cross);
     double calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_ans);

From 79035b8734d98f1a24a0207fd9ab11e48c191ab1 Mon Sep 17 00:00:00 2001
From: ishiguroJSK <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 8 Feb 2017 11:02:27 +0900
Subject: [PATCH 056/217] cam ->head2

---
 rtc/AutoBalancer/AutoBalancer.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 6df0c51c584..a556a3de101 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -508,7 +508,7 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlf.data,hp_raw_data.getP("lf")); }
     if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrh.data,hp_raw_data.getP("rh"));}
     if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlh.data,hp_raw_data.getP("lh"));}
-    if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_hthead.data,hsp->head_cam_pose); }
+    if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_hthead.data,hsp->head_cam_pose);}
     if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
     hsp->readInput(hp_raw_data);
     hsp->current_basepos = m_robot->rootLink()->p;

From 94073fafad585bc3fe6baa2202f4c392f0b54258 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 15 Feb 2017 21:31:53 +0900
Subject: [PATCH 057/217] complete rot control

---
 rtc/AutoBalancer/HumanMasterSlave.h | 290 ++++++++++++++++------------
 1 file changed, 167 insertions(+), 123 deletions(-)

diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index 54730578a64..8d640e75259 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -43,6 +43,16 @@ extern "C" {
 //実機だとログ出力で落ちる？getenv("HOME")ダメ？
 #define DEBUG 0
 
+namespace myconst{
+  const int X = 0, Y = 1, Z = 2, XYZ = 3;
+  const int r = 0, p = 1, y = 2, rpy = 3;
+  const int L = 0, R = 1, LR = 2;
+  const int MIN = 0, MAX = 1, MINMAX = 2;
+  const double INFMIN = std::numeric_limits<double>::min(), INFMAX = std::numeric_limits<double>::max();
+  const double D2R = M_PI/180.0;
+}
+using namespace myconst;
+
 class BiquadIIRFilterVec{
   private:
     IIRFilter filters[3];
@@ -120,8 +130,8 @@ class HumanPose{
     static void hp_printf(const HumanPose& in){
       const std::string ns[] = {"com","rf","lf","rh","lh","zmp","rfw","lfw"};
       const std::string cap[] = {"c","rf","lf","rh","lh","z","rw","lw"};
-      for(int i=0;i<6;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.getP(ns[i]).p(0),in.getP(ns[i]).p(1),in.getP(ns[i]).p(2)); }
-      for(int i=6;i<8;i++){ printf("\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.getw(ns[i]).f(0),in.getw(ns[i]).f(1),in.getw(ns[i]).f(2),in.getw(ns[i]).t(0),in.getw(ns[i]).t(1),in.getw(ns[i]).t(2)); }
+      for(int i=0;i<6;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.getP(ns[i]).p(X),in.getP(ns[i]).p(Y),in.getP(ns[i]).p(Z)); }
+      for(int i=6;i<8;i++){ printf("\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.getw(ns[i]).f(X),in.getw(ns[i]).f(Y),in.getw(ns[i]).f(Z),in.getw(ns[i]).t(X),in.getw(ns[i]).t(Y),in.getw(ns[i]).t(Z)); }
       printf("\n");
     }
     HRPPose3D& getP(const std::string& key){///Safe accessor
@@ -143,6 +153,27 @@ class HumanPose{
     void print(){ hp_printf(*this); }
 };
 
+class RobotConfig{
+  public:
+    std::map<std::string, std::vector<hrp::Vector3> > ee_rot_limit;
+    RobotConfig(){
+      ee_rot_limit["com"].resize(MINMAX);
+      ee_rot_limit["com"][MIN] = hrp::Vector3(-30*D2R, -30*D2R, INFMIN);
+      ee_rot_limit["com"][MAX] = hrp::Vector3( 30*D2R,  30*D2R, INFMAX);
+      ee_rot_limit["rf"].resize(MINMAX);
+      ee_rot_limit["rf"][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -20*D2R);
+      ee_rot_limit["rf"][MAX] = hrp::Vector3( 30*D2R,  30*D2R,   5*D2R);
+      ee_rot_limit["lf"].resize(MINMAX);
+      ee_rot_limit["lf"][MIN] = hrp::Vector3(-30*D2R, -30*D2R,  -5*D2R);
+      ee_rot_limit["lf"][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  20*D2R);
+      ee_rot_limit["rh"].resize(MINMAX);
+      ee_rot_limit["rh"][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -30*D2R);
+      ee_rot_limit["rh"][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
+      ee_rot_limit["lh"].resize(MINMAX);
+      ee_rot_limit["lh"][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -30*D2R);
+      ee_rot_limit["lh"][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
+    }
+};
 
 class HumanSynchronizer{
   private:
@@ -170,6 +201,7 @@ class HumanSynchronizer{
     BiquadIIRFilterVec cam_rpy_filter;
     hrp::Vector3 r_zmp_raw;
     hrp::Vector4 rf_safe_region,lf_safe_region;//前後左右の順
+    RobotConfig rc;
 
   public:
     double tgt_FUP_TIME;
@@ -189,7 +221,7 @@ class HumanSynchronizer{
     unsigned int loop;
     hrp::Vector3 cam_pos_filtered,cam_rpy_filtered;
     HRPPose3D head_cam_pose;
-    hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos;
+    hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos, pre_cont_rfrot,pre_cont_lfrot;
     std::vector<hrp::Vector3> rf_vert,lf_vert;
 
     interpolator *init_zmp_com_offset_interpolator;
@@ -207,7 +239,7 @@ class HumanSynchronizer{
       cur_rfup_rad = cur_lfup_rad = 0;
       is_rf_contact = is_lf_contact = true;
       go_rf_landing = go_lf_landing = false;
-      pre_cont_rfpos = pre_cont_lfpos = hrp::Vector3::Zero();
+      pre_cont_rfpos = pre_cont_lfpos = pre_cont_rfrot = pre_cont_lfrot = hrp::Vector3::Zero();
       init_hp_calibcom = hrp::Vector3::Zero();
       tgt_FUP_TIME = FUP_TIME = 0.4;
       FUP_HIGHT = 0.05;
@@ -238,8 +270,8 @@ class HumanSynchronizer{
       for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0,HZ);//四肢拘束点用(Rotation)
       tgt_pos_filters[0].setParameter(0.4,HZ);//重心pos用
       tgt_rot_filters[0].setParameter(0.4,HZ);//重心rot用
-      tgt_pos_filters[1].setParameter(hrp::Vector3(0.6,0.6,1.5),HZ);//右足pos用
-      tgt_pos_filters[2].setParameter(hrp::Vector3(0.6,0.6,1.5),HZ);//左足pos用
+      tgt_pos_filters[1].setParameter(hrp::Vector3(0.6,0.6,1.0),HZ);//右足pos用
+      tgt_pos_filters[2].setParameter(hrp::Vector3(0.6,0.6,1.0),HZ);//左足pos用
       calcacc_v_filters.setParameter(5,HZ);//加速度計算用
       acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
       cam_rpy_filter.setParameter(1,HZ);//カメラアングル
@@ -255,8 +287,8 @@ class HumanSynchronizer{
       lf_vert.push_back(hrp::Vector3(-0.10,0.08,0));
       lf_vert.push_back(hrp::Vector3(0.13,0.08,0));
 
-//      use_rh = use_lh = false;
-      use_rh = use_lh = true;
+      use_rh = use_lh = false;
+//      use_rh = use_lh = true;
 
       rp_ref_out_old.clear();
       rp_ref_out.clear();
@@ -307,12 +339,12 @@ class HumanSynchronizer{
       if(loop==0)rp_ref_out_old = rp_ref_out;
 
       applyCOMMoveModRatio                (rp_ref_out);
-//      judgeFootLandOnCommand              (hp_wld_raw.getw("rfw"), hp_wld_raw.getw("lfw"), go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
+      judgeFootLandOnCommand              (hp_wld_raw.getw("rfw"), hp_wld_raw.getw("lfw"), go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
       applyEEWorkspaceLimit               (rp_ref_out);
       lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
-      if(go_rf_landing)rp_ref_out.getP("rf").p(2) = rp_ref_out.getP("rf").p_offs(2);
-      if(go_lf_landing)rp_ref_out.getP("lf").p(2) = rp_ref_out.getP("lf").p_offs(2);
+      if(go_rf_landing)rp_ref_out.getP("rf").p(Z) = rp_ref_out.getP("rf").p_offs(Z);
+      if(go_lf_landing)rp_ref_out.getP("lf").p(Z) = rp_ref_out.getP("lf").p_offs(Z);
 
       applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com"));
 
@@ -334,17 +366,17 @@ class HumanSynchronizer{
       gettimeofday(&t_calc_end, NULL);
     }
     void calibInitHumanCOMFromZMP(){
-      init_hp_calibcom(0) = r_zmp_raw(0);
-      init_hp_calibcom(1) = r_zmp_raw(1);
+      init_hp_calibcom(X) = r_zmp_raw(X);
+      init_hp_calibcom(Y) = r_zmp_raw(Y);
     }
-    static void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out(0) = in.x; out(1) = in.y; out(2) = in.z;}
-    static void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out(0) = in.r; out(1) = in.p; out(2) = in.y;}
+    static void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out(X) = in.x; out(Y) = in.y; out(Z) = in.z;}
+    static void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out(X) = in.r; out(Y) = in.p; out(Z) = in.y;}
     static void Pose3DToHRPPose3D(const RTC::Pose3D& in, HRPPose3D& out_pose){ Point3DToVector3(in.position,out_pose.p); Oriantation3DToVector3(in.orientation,out_pose.rpy); }
     static void DoubleSeqToWrench6(const RTC::TimedDoubleSeq::_data_seq& in, Wrench6& out){
-      if(in.length() == 6){out.f(0)=in[0]; out.f(1)=in[1]; out.f(2)=in[2]; out.t(0)=in[3]; out.t(1)=in[4]; out.t(2)=in[5];
+      if(in.length() == 6){out.f(X)=in[0]; out.f(Y)=in[1]; out.f(Z)=in[2]; out.t(X)=in[3]; out.t(Y)=in[4]; out.t(Z)=in[5];
       }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; out.clear(); }
     }
-    static double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(0)*b(1)-a(1)*b(0); }
+    static double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(X)*b(Y)-a(Y)*b(X); }
 
   private:
     void updateHumanToRobotRatio(const double h2r_r_goal){//h2r_ratioに伴って変わる変数の処理もここに書く
@@ -365,7 +397,7 @@ class HumanSynchronizer{
       std::string ns[5] = {"com","rf","lf","rh","lh"};
       for(int i=0;i<5;i++){
         rel_out.getP(ns[i]).p   = abs_in.getP(ns[i]).p   - init_offset.getP(ns[i]).p;
-        rel_out.getP(ns[i]).rpy = abs_in.getP(ns[i]).rpy - init_offset.getP(ns[i]).rpy_offs;
+        rel_out.getP(ns[i]).rpy = abs_in.getP(ns[i]).rpy - init_offset.getP(ns[i]).rpy;
       }
       rel_out.getw("rfw") = abs_in.getw("rfw");
       rel_out.getw("lfw") = abs_in.getw("lfw");
@@ -401,43 +433,43 @@ class HumanSynchronizer{
     }
     void judgeFootLandOnCommand(const Wrench6& rfw_in, const Wrench6& lfw_in, bool& rfloc_ans, bool& lfloc_ans){
       if(isHumanSyncOn()){
-        if      (rfloc_ans  && rfw_in.f(2)<CNT_F_TH   ){rfloc_ans = false;}//右足ついた状態から上げる
-        else if (!rfloc_ans && rfw_in.f(2)>CNT_F_TH+30){rfloc_ans = true;}//右足浮いた状態から下げる
-        if      (lfloc_ans  && lfw_in.f(2)<CNT_F_TH   ){lfloc_ans = false;}//左足ついた状態から上げる
-        else if (!lfloc_ans && lfw_in.f(2)>CNT_F_TH+30){lfloc_ans = true;}//左足浮いた状態から下げる
+        if      (rfloc_ans  && rfw_in.f(Z)<CNT_F_TH   ){rfloc_ans = false;}//右足ついた状態から上げる
+        else if (!rfloc_ans && rfw_in.f(Z)>CNT_F_TH+30){rfloc_ans = true;}//右足浮いた状態から下げる
+        if      (lfloc_ans  && lfw_in.f(Z)<CNT_F_TH   ){lfloc_ans = false;}//左足ついた状態から上げる
+        else if (!lfloc_ans && lfw_in.f(Z)>CNT_F_TH+30){lfloc_ans = true;}//左足浮いた状態から下げる
       }
-      if(!rfloc_ans && !lfloc_ans){rfloc_ans = lfloc_ans = true;}//両足ジャンプは禁止
+//      if(!rfloc_ans && !lfloc_ans){rfloc_ans = lfloc_ans = true;}//両足ジャンプは禁止
     }
     void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
-      if(rfcs_ans && !lfcs_ans){
-        if(tgt.getP("zmp").p(1) > tgt.getP("rf").p(1) + 0.03){
+//      if(rfcs_ans && !lfcs_ans){
+        if(tgt.getP("zmp").p(Y) > tgt.getP("rf").p(Y) + 0.03){
           lfcs_ans = true;
           if(loop%100==0)cout<<"LF Auto lock"<<endl;
         }
-      }
-      if(!rfcs_ans && lfcs_ans){
-        if(tgt.getP("zmp").p(1) < tgt.getP("lf").p(1) - 0.03){
+//      }
+//      if(!rfcs_ans && lfcs_ans){
+        if(tgt.getP("zmp").p(Y) < tgt.getP("lf").p(Y) - 0.03){
           rfcs_ans = true;
           if(loop%100==0)cout<<"RF Auto lock"<<endl;
         }
-      }
+//      }
     }
     void setFootRotHorizontalIfGoLanding(const HumanPose& rp_ref_out_old, HumanPose& rp_ref_out){
-//      if(rp_ref_out_old.getP("rf").p(2)-rp_ref_out_old.getP("rf").p_offs(2)<0.05){ rp_ref_out.getP("rf").rpy(0) = rp_ref_out.getP("rf").rpy(1) = 0; }
-//      if(rp_ref_out_old.getP("lf").p(2)-rp_ref_out_old.getP("lf").p_offs(2)<0.05){ rp_ref_out.getP("lf").rpy(0) = rp_ref_out.getP("lf").rpy(1) = 0; }
-      if(go_rf_landing){ rp_ref_out.getP("rf").rpy(0) = rp_ref_out.getP("rf").rpy(1) = 0; }
-      if(go_lf_landing){ rp_ref_out.getP("lf").rpy(0) = rp_ref_out.getP("lf").rpy(1) = 0; }
+//      if(rp_ref_out_old.getP("rf").p(Z)-rp_ref_out_old.getP("rf").p_offs(Z)<0.05){ rp_ref_out.getP("rf").rpy(X) = rp_ref_out.getP("rf").rpy(Y) = 0; }
+//      if(rp_ref_out_old.getP("lf").p(Z)-rp_ref_out_old.getP("lf").p_offs(Z)<0.05){ rp_ref_out.getP("lf").rpy(X) = rp_ref_out.getP("lf").rpy(Y) = 0; }
+      if(go_rf_landing){ rp_ref_out.getP("rf").rpy(X) = rp_ref_out.getP("rf").rpy(Y) = 0; }
+      if(go_lf_landing){ rp_ref_out.getP("lf").rpy(X) = rp_ref_out.getP("lf").rpy(Y) = 0; }
     }
     void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
-      if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足接地しているタイミングでのみ更新
-      double RFDOWN_TIME = fabs(tgt.getP("rf").p(1) - tgt.getP("com").p(1))*2 + FUP_TIME;//下ろす速度は遊脚の重心からの距離に比例したペナルティ
-      double LFDOWN_TIME = fabs(tgt.getP("lf").p(1) - tgt.getP("com").p(1))*2 + FUP_TIME;
-      LIMIT_MINMAX( RFDOWN_TIME, FUP_TIME, 0.8);
-      LIMIT_MINMAX( LFDOWN_TIME, FUP_TIME, 0.8);
+//      if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足接地しているタイミングでのみ更新
+//      double RFDOWN_TIME = fabs(tgt.getP("rf").p(Y) - tgt.getP("com").p(Y))*2 + FUP_TIME;//下ろす速度は遊脚の重心からの距離に比例したペナルティ
+//      double LFDOWN_TIME = fabs(tgt.getP("lf").p(Y) - tgt.getP("com").p(Y))*2 + FUP_TIME;
+//      LIMIT_MINMAX( RFDOWN_TIME, FUP_TIME, 0.8);
+//      LIMIT_MINMAX( LFDOWN_TIME, FUP_TIME, 0.8);
 //      calcFootUpCurveAndJudgeFootContact(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.getP("rf"), is_rf_contact);
 //      calcFootUpCurveAndJudgeFootContact(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.getP("lf"), is_lf_contact);
-      const double com2rf_dist = fabs(tgt.getP("rf").p(1) - tgt.getP("com").p(1));
-      const double com2lf_dist = fabs(tgt.getP("lf").p(1) - tgt.getP("com").p(1));
+      const double com2rf_dist = fabs(tgt.getP("rf").p(Y) - tgt.getP("com").p(Y));
+      const double com2lf_dist = fabs(tgt.getP("lf").p(Y) - tgt.getP("com").p(Y));
       limitGroundContactVelocity(rf_goland_in, com2rf_dist, rp_ref_out_old.getP("rf"), tgt.getP("rf"), is_rf_contact);
       limitGroundContactVelocity(lf_goland_in, com2lf_dist, rp_ref_out_old.getP("lf"), tgt.getP("lf"), is_lf_contact);
     }
@@ -450,25 +482,25 @@ class HumanSynchronizer{
 //        cur_fup_phase_in -= omega_down * DT;
 //      }
 //      LIMIT_MINMAX( cur_fup_phase_in, 0, M_PI);
-//      double fup_height = f_height_out.p(2)-f_height_out.p_offs(2);
+//      double fup_height = f_height_out.p(Z)-f_height_out.p_offs(Z);
 //      LIMIT_MINMAX( fup_height, 0.03, 0.10 );
-//      f_height_out.p(2) = fup_height/2 * (1-cos(cur_fup_phase_in)) + f_height_out.p_offs(2);
+//      f_height_out.p(Z) = fup_height/2 * (1-cos(cur_fup_phase_in)) + f_height_out.p_offs(Z);
 //      is_f_contact_out = (cur_fup_phase_in <= 0);
 //    }
     void limitGroundContactVelocity(const bool& go_land_in, const double& com2foot_dist, const HRPPose3D& f_old_in, HRPPose3D& f_in_out, bool& is_f_contact_out){
       double penalty = 1.0 + (0.2 - com2foot_dist) * 5.0;//1.0 ~ 2.0
       LIMIT_MINMAX( penalty, 1.0, 2.0);
-      LIMIT_MIN( f_in_out.p(2), f_in_out.p_offs(2));
-      double input_vel = (f_in_out.p(2) - f_old_in.p(2)) / DT;
+      LIMIT_MIN( f_in_out.p(Z), f_in_out.p_offs(Z));
+      double input_vel = (f_in_out.p(Z) - f_old_in.p(Z)) / DT;
       const double vel_limit_k = 8.0 * penalty;//地面から0.02[m]地点で0.02*8=0.16[m/s]出ている計算
-      double limit_vel = - vel_limit_k * (f_old_in.p(2) - f_old_in.p_offs(2));//sinの時は平均0.05~0.10[m/s]
+      double limit_vel = - vel_limit_k * (f_old_in.p(Z) - f_old_in.p_offs(Z));//sinの時は平均0.05~0.10[m/s]
       const double max_vel_threshold = -0.01;
       LIMIT_MAX( limit_vel, max_vel_threshold);//速度0に近づくと目標になかなか到達しないから
       LIMIT_MIN( input_vel, limit_vel);//着地時の下向きの速度を制限
-      f_in_out.p(2) = f_old_in.p(2) + input_vel * DT;
-      LIMIT_MIN( f_in_out.p(2), f_in_out.p_offs(2));
+      f_in_out.p(Z) = f_old_in.p(Z) + input_vel * DT;
+      LIMIT_MIN( f_in_out.p(Z), f_in_out.p_offs(Z));
       const double contact_threshold = 0.005;
-      is_f_contact_out = ((f_in_out.p(2)-f_in_out.p_offs(2)) <= contact_threshold);
+      is_f_contact_out = ((f_in_out.p(Z)-f_in_out.p_offs(Z)) <= contact_threshold);
     }
     void modifyFootRotAndXYForContact(HRPPose3D& rf_in, HRPPose3D& lf_in){
       calcFootRotAndXYTransitionForContact(rf_in,rf_vert);
@@ -476,54 +508,58 @@ class HumanSynchronizer{
     }
     void calcFootRotAndXYTransitionForContact(HRPPose3D& foot_in, const std::vector<hrp::Vector3>& act_sole_size){
       std::vector<hrp::Vector3> act_foot_vert;
-      double min_height = foot_in.p(2);
+      double min_height = foot_in.p(Z);
       int min_height_id = 0;
       for(int i=0;i<act_sole_size.size();i++){
         act_foot_vert.push_back(hrp::rotFromRpy(foot_in.rpy) * act_sole_size[i]);
-        act_foot_vert[i](2) += foot_in.p(2);
-        if(act_foot_vert[i](2) < min_height){
-          min_height = act_foot_vert[i](2);
+        act_foot_vert[i](Z) += foot_in.p(Z);
+        if(act_foot_vert[i](Z) < min_height){
+          min_height = act_foot_vert[i](Z);
           min_height_id = i;
         }
       }
-      if(act_foot_vert[min_height_id](2) < foot_in.p_offs(2)){
-        foot_in.p(2) += foot_in.p_offs(2) - act_foot_vert[min_height_id](2);
+      if(act_foot_vert[min_height_id](Z) < foot_in.p_offs(Z)){
+        foot_in.p(Z) += foot_in.p_offs(Z) - act_foot_vert[min_height_id](Z);
       }
     }
     void lockFootXYOnContact(const bool& rfcs_in, const bool& lfcs_in, HumanPose& tgt){
       if(rfcs_in){
-        tgt.getP("rf").p(0) = pre_cont_rfpos(0);
-        tgt.getP("rf").p(1) = pre_cont_rfpos(1);
+        tgt.getP("rf").p(X) = pre_cont_rfpos(X);
+        tgt.getP("rf").p(Y) = pre_cont_rfpos(Y);
+        tgt.getP("rf").rpy(Z) = pre_cont_rfrot(Z);
       }else{
-        pre_cont_rfpos(0) = tgt.getP("rf").p(0);
-        pre_cont_rfpos(1) = tgt.getP("rf").p(1);
+        pre_cont_rfpos(X) = tgt.getP("rf").p(X);
+        pre_cont_rfpos(Y) = tgt.getP("rf").p(Y);
+        pre_cont_rfrot(Z) = tgt.getP("rf").rpy(Z);
       }
       if(lfcs_in){
-        tgt.getP("lf").p(0) = pre_cont_lfpos(0);
-        tgt.getP("lf").p(1) = pre_cont_lfpos(1);
+        tgt.getP("lf").p(X) = pre_cont_lfpos(X);
+        tgt.getP("lf").p(Y) = pre_cont_lfpos(Y);
+        tgt.getP("lf").rpy(Z) = pre_cont_lfrot(Z);
       }else{
-        pre_cont_lfpos(0) = tgt.getP("lf").p(0);
-        pre_cont_lfpos(1) = tgt.getP("lf").p(1);
+        pre_cont_lfpos(X) = tgt.getP("lf").p(X);
+        pre_cont_lfpos(Y) = tgt.getP("lf").p(Y);
+        pre_cont_lfrot(Z) = tgt.getP("lf").rpy(Z);
       }
     }
     void applyEEWorkspaceLimit(HumanPose& tgt){
-      const double MAX = 0.30;
+      const double MAX_FW = 0.30;
       const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
       if(!is_rf_contact && is_lf_contact){//右足浮遊時
-        const hrp::Vector2 lf2rf_vec( tgt.getP("rf").p(0)-pre_cont_lfpos(0), tgt.getP("rf").p(1)-pre_cont_lfpos(1) );
-        if(lf2rf_vec.norm() > MAX){
-          tgt.getP("rf").p(0) = (lf2rf_vec.normalized())(0) * MAX + pre_cont_lfpos(0);
-          tgt.getP("rf").p(1) = (lf2rf_vec.normalized())(1) * MAX + pre_cont_lfpos(1);
+        const hrp::Vector2 lf2rf_vec( tgt.getP("rf").p(X)-pre_cont_lfpos(X), tgt.getP("rf").p(Y)-pre_cont_lfpos(Y) );
+        if(lf2rf_vec.norm() > MAX_FW){
+          tgt.getP("rf").p(X) = (lf2rf_vec.normalized())(X) * MAX_FW + pre_cont_lfpos(X);
+          tgt.getP("rf").p(Y) = (lf2rf_vec.normalized())(Y) * MAX_FW + pre_cont_lfpos(Y);
         }
-        LIMIT_MAX(tgt.getP("rf").p(1), pre_cont_lfpos(1) - FOOT_2_FOOT_COLLISION_MARGIIN);
+        LIMIT_MAX(tgt.getP("rf").p(Y), pre_cont_lfpos(Y) - FOOT_2_FOOT_COLLISION_MARGIIN);
       }
       else if(is_rf_contact && !is_lf_contact){//左足浮遊時
-        const hrp::Vector2 rf2lf_vec( tgt.getP("lf").p(0)-pre_cont_rfpos(0), tgt.getP("lf").p(1)-pre_cont_rfpos(1) );
-        if(rf2lf_vec.norm() > MAX){
-          tgt.getP("lf").p(0) = (rf2lf_vec.normalized())(0) * MAX + pre_cont_rfpos(0);
-          tgt.getP("lf").p(1) = (rf2lf_vec.normalized())(1) * MAX + pre_cont_rfpos(1);
+        const hrp::Vector2 rf2lf_vec( tgt.getP("lf").p(X)-pre_cont_rfpos(X), tgt.getP("lf").p(Y)-pre_cont_rfpos(Y) );
+        if(rf2lf_vec.norm() > MAX_FW){
+          tgt.getP("lf").p(X) = (rf2lf_vec.normalized())(X) * MAX_FW + pre_cont_rfpos(X);
+          tgt.getP("lf").p(Y) = (rf2lf_vec.normalized())(Y) * MAX_FW + pre_cont_rfpos(Y);
         }
-        LIMIT_MIN(tgt.getP("lf").p(1), pre_cont_rfpos(1) + FOOT_2_FOOT_COLLISION_MARGIIN);
+        LIMIT_MIN(tgt.getP("lf").p(Y), pre_cont_rfpos(Y) + FOOT_2_FOOT_COLLISION_MARGIIN);
       }
       hrp::Vector3 base2rh = tgt.getP("rh").p_offs - init_basepos;
       hrp::Vector3 base2lh = tgt.getP("lh").p_offs - init_basepos;
@@ -533,30 +569,38 @@ class HumanSynchronizer{
         LIMIT_MINMAX( tgt.getP("rh").p(i), (current_basepos+base2rh+hand_llimit)(i), (current_basepos+base2rh+hand_ulimit)(i) );
         LIMIT_MINMAX( tgt.getP("lh").p(i), (current_basepos+base2lh+hand_llimit)(i), (current_basepos+base2lh+hand_ulimit)(i) );
       }
-      LIMIT_MINMAX( tgt.getP("rf").p(2), tgt.getP("rf").p_offs(2), tgt.getP("rf").p_offs(2)+0.10);
-      LIMIT_MINMAX( tgt.getP("lf").p(2), tgt.getP("lf").p_offs(2), tgt.getP("lf").p_offs(2)+0.10);
+      LIMIT_MINMAX( tgt.getP("rf").p(Z), tgt.getP("rf").p_offs(Z), tgt.getP("rf").p_offs(Z)+0.10);
+      LIMIT_MINMAX( tgt.getP("lf").p(Z), tgt.getP("lf").p_offs(Z), tgt.getP("lf").p_offs(Z)+0.10);
+
+
       std::string ns[5] = {"com","rf","lf","rh","lh"};
-      for(int i=0;i<5;i++){
-        LIMIT_MINMAX( tgt.getP(ns[i]).rpy(0), -30*M_PI/180, 30*M_PI/180 );
-        LIMIT_MINMAX( tgt.getP(ns[i]).rpy(1), -30*M_PI/180, 30*M_PI/180 );
-      }
-      LIMIT_MINMAX( tgt.getP("rf").rpy(2), -30*M_PI/180+tgt.getP("com").rpy(2), 30*M_PI/180+tgt.getP("com").rpy(2) );
-      LIMIT_MINMAX( tgt.getP("lf").rpy(2), -30*M_PI/180+tgt.getP("com").rpy(2), 30*M_PI/180+tgt.getP("com").rpy(2) );
-      LIMIT_MINMAX( tgt.getP("rh").rpy(2), -30*M_PI/180+tgt.getP("com").rpy(2), 30*M_PI/180+tgt.getP("com").rpy(2) );
-      LIMIT_MINMAX( tgt.getP("lh").rpy(2), -30*M_PI/180+tgt.getP("com").rpy(2), 30*M_PI/180+tgt.getP("com").rpy(2) );
 
+//
+//      for(int i=0;i<5;i++){
+//        LIMIT_MINMAX( tgt.getP(ns[i]).rpy(X), -30*M_PI/180, 30*M_PI/180 );
+//        LIMIT_MINMAX( tgt.getP(ns[i]).rpy(Y), -30*M_PI/180, 30*M_PI/180 );
+//      }
+//      LIMIT_MINMAX( tgt.getP("rf").rpy(Z), -30*M_PI/180+tgt.getP("com").rpy(Z), 30*M_PI/180+tgt.getP("com").rpy(Z) );
+//      LIMIT_MINMAX( tgt.getP("lf").rpy(Z), -30*M_PI/180+tgt.getP("com").rpy(Z), 30*M_PI/180+tgt.getP("com").rpy(Z) );
+//      LIMIT_MINMAX( tgt.getP("rh").rpy(Z), -30*M_PI/180+tgt.getP("com").rpy(Z), 30*M_PI/180+tgt.getP("com").rpy(Z) );
+//      LIMIT_MINMAX( tgt.getP("lh").rpy(Z), -30*M_PI/180+tgt.getP("com").rpy(Z), 30*M_PI/180+tgt.getP("com").rpy(Z) );
+
+      for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( tgt.getP("com").rpy(i), rc.ee_rot_limit["com"][MIN](i), rc.ee_rot_limit["com"][MAX](i) ); }
+      for(int i=1;i<5;i++){
+        for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( tgt.getP(ns[i]).rpy(j), rc.ee_rot_limit[ns[i]][MIN](j) + tgt.getP("com").rpy(j), rc.ee_rot_limit[ns[i]][MAX](j) + tgt.getP("com").rpy(j) ); }
+      }
 
-      LIMIT_MINMAX( head_cam_pose.rpy(1), -20*M_PI/180, 20*M_PI/180 );
-      LIMIT_MINMAX( head_cam_pose.rpy(2), -20*M_PI/180, 20*M_PI/180 );
+      LIMIT_MINMAX( head_cam_pose.rpy(Y), -20*M_PI/180, 20*M_PI/180 );
+      LIMIT_MINMAX( head_cam_pose.rpy(Z), -20*M_PI/180, 20*M_PI/180 );
     }
     bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, HRPPose3D& comin_abs){//boost::geometryがUbuntu12だとないから・・・
       std::vector<hrp::Vector2> hull;
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull);
-      hrp::Vector2 cog(comin_abs.p(0),comin_abs.p(1));
+      hrp::Vector2 cog(comin_abs.p(X),comin_abs.p(Y));
       if(!isPointInHullOpenCV(cog,hull)){ calcNearestPointOnHull(cog,hull,cog); }//外に出たら最近傍点に頭打ち
-      comin_abs.p(0) = cog(0);
-      comin_abs.p(1) = cog(1);
-      LIMIT_MINMAX( comin_abs.p(2), comin_abs.p_offs(2) - 0.15, comin_abs.p_offs(2) + 0.03 );//COM高さ方向の制限
+      comin_abs.p(X) = cog(X);
+      comin_abs.p(Y) = cog(Y);
+      LIMIT_MINMAX( comin_abs.p(Z), comin_abs.p_offs(Z) - 0.15, comin_abs.p_offs(Z) + 0.03 );//COM高さ方向の制限
       return true;
     }
     bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& com_old, const hrp::Vector3& com_vel_old, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans){
@@ -567,20 +611,20 @@ class HumanSynchronizer{
 //      double accel[4] = {0.05, -0.03, 0.02, -0.02};
 //      double decel[4] = {0.04, -0.02, 0.01, -0.01};
 //      comacc_ref = calcacc_v_filters.passFilter(com_acc);
-      const double H = rp_wld_initpos.getP("com").p(2);
+      const double H = rp_wld_initpos.getP("com").p(Z);
       std::vector<hrp::Vector2> hull;
       hrp::Vector4 marginDelta(+0.001,-0.001,0.001,-0.001);
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region+marginDelta, lf_safe_region+marginDelta, hull);
       hrp::Vector2 com_vel_ans_2d;
-      regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);
-//      regulateCOMVelocityByCapturePointXY( hrp::Vector2(com_old(0),com_old(1)), hrp::Vector2(com_vel(0),com_vel(1)), hull, com_vel_ans_2d);//XY独立に計算する手法のほうがエッジで引っかからない・・・
-      com_ans(0) = com_old(0) + com_vel_ans_2d(0) * DT;
-      com_ans(1) = com_old(1) + com_vel_ans_2d(1) * DT;
+      regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_old(X),com_old(Y)), hrp::Vector2(com_vel(X),com_vel(Y)), hull, com_vel_ans_2d);
+//      regulateCOMVelocityByCapturePointXY( hrp::Vector2(com_old(X),com_old(Y)), hrp::Vector2(com_vel(X),com_vel(Y)), hull, com_vel_ans_2d);//XY独立に計算する手法のほうがエッジで引っかからない・・・
+      com_ans(X) = com_old(X) + com_vel_ans_2d(X) * DT;
+      com_ans(Y) = com_old(Y) + com_vel_ans_2d(Y) * DT;
       return true;
     }
     void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
       com_vel_ans = com_vel;
-      const double H = rp_wld_initpos.getP("com").p(2);
+      const double H = rp_wld_initpos.getP("com").p(Z);
       hrp::Vector2 cp = com_pos + com_vel * sqrt( H / G );
       hrp::Vector2 cp_ragulated;
       if(!isPointInHullOpenCV(cp,hull)){
@@ -589,44 +633,44 @@ class HumanSynchronizer{
         com_vel_ans = (cp_ragulated - com_pos) / sqrt( H / G );
       }
     }
-    void regulateCOMVelocityByCapturePointXY(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
-      com_vel_ans = com_vel;
-      const double H = rp_wld_initpos.getP("com").p(2);
-      enum Direc{F,B,L,R} direc;
-      hrp::Vector4 margin_from_4maxcp;
-      calcXYMarginToHull(com_pos, hull, margin_from_4maxcp);
-      hrp::Vector4 com_vel_decel_limit,com_vel_accel_limit;
-      const double accel_cp_tolerance = 0.01;//境界上でcom_vel_accel_limitが0になって抜け出せなくなるのを防ぐため
-      com_vel_decel_limit(F) = margin_from_4maxcp(F) / sqrt( H / G );
-      com_vel_decel_limit(B) = margin_from_4maxcp(B) / sqrt( H / G );
-      com_vel_decel_limit(L) = margin_from_4maxcp(L) / sqrt( H / G );
-      com_vel_decel_limit(R) = margin_from_4maxcp(R) / sqrt( H / G );
-      com_vel_accel_limit(F) = (-margin_from_4maxcp(B)+accel_cp_tolerance) / sqrt( H / G );
-      com_vel_accel_limit(B) = (-margin_from_4maxcp(F)-accel_cp_tolerance) / sqrt( H / G );
-      com_vel_accel_limit(L) = (-margin_from_4maxcp(R)+accel_cp_tolerance) / sqrt( H / G );
-      com_vel_accel_limit(R) = (-margin_from_4maxcp(L)-accel_cp_tolerance) / sqrt( H / G );
-      LIMIT_MINMAX( com_vel_ans(0), com_vel_decel_limit(B), com_vel_decel_limit(F) );
-      LIMIT_MINMAX( com_vel_ans(1), com_vel_decel_limit(R), com_vel_decel_limit(L) );
-      LIMIT_MINMAX( com_vel_ans(0), com_vel_accel_limit(B), com_vel_accel_limit(F) );//加速時のCP
-      LIMIT_MINMAX( com_vel_ans(1), com_vel_accel_limit(R), com_vel_accel_limit(L) );
-    }
+//    void regulateCOMVelocityByCapturePointXY(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
+//      com_vel_ans = com_vel;
+//      const double H = rp_wld_initpos.getP("com").p(Z);
+//      enum Direc{F,B,L,R} direc;
+//      hrp::Vector4 margin_from_4maxcp;
+//      calcXYMarginToHull(com_pos, hull, margin_from_4maxcp);
+//      hrp::Vector4 com_vel_decel_limit,com_vel_accel_limit;
+//      const double accel_cp_tolerance = 0.01;//境界上でcom_vel_accel_limitが0になって抜け出せなくなるのを防ぐため
+//      com_vel_decel_limit(F) = margin_from_4maxcp(F) / sqrt( H / G );
+//      com_vel_decel_limit(B) = margin_from_4maxcp(B) / sqrt( H / G );
+//      com_vel_decel_limit(L) = margin_from_4maxcp(L) / sqrt( H / G );
+//      com_vel_decel_limit(R) = margin_from_4maxcp(R) / sqrt( H / G );
+//      com_vel_accel_limit(F) = (-margin_from_4maxcp(B)+accel_cp_tolerance) / sqrt( H / G );
+//      com_vel_accel_limit(B) = (-margin_from_4maxcp(F)-accel_cp_tolerance) / sqrt( H / G );
+//      com_vel_accel_limit(L) = (-margin_from_4maxcp(R)+accel_cp_tolerance) / sqrt( H / G );
+//      com_vel_accel_limit(R) = (-margin_from_4maxcp(L)-accel_cp_tolerance) / sqrt( H / G );
+//      LIMIT_MINMAX( com_vel_ans(X), com_vel_decel_limit(B), com_vel_decel_limit(F) );
+//      LIMIT_MINMAX( com_vel_ans(Y), com_vel_decel_limit(R), com_vel_decel_limit(L) );
+//      LIMIT_MINMAX( com_vel_ans(X), com_vel_accel_limit(B), com_vel_accel_limit(F) );//加速時のCP
+//      LIMIT_MINMAX( com_vel_ans(Y), com_vel_accel_limit(R), com_vel_accel_limit(L) );
+//    }
     void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
       std::vector<hrp::Vector2> points;
       for(int i=0;i<2;i++){
         for(int j=2;j<4;j++){
-          points.push_back(hrp::Vector2(rfin_abs(0) + rf_mgn(i),    rfin_abs(1) + rf_mgn(j)));
-          points.push_back(hrp::Vector2(lfin_abs(0) + lf_mgn(i),    lfin_abs(1) + lf_mgn(j)));
+          points.push_back(hrp::Vector2(rfin_abs(X) + rf_mgn(i),    rfin_abs(Y) + rf_mgn(j)));
+          points.push_back(hrp::Vector2(lfin_abs(X) + lf_mgn(i),    lfin_abs(Y) + lf_mgn(j)));
         }
       }
 //      static unsigned int edge_time = 0;
-//      if(rp_ref_out_old.getP("rf").rpy(1)>0.001){
+//      if(rp_ref_out_old.getP("rf").rpy(Y)>0.001){
 //        points.clear();
-//        double mod = rfin_abs(0) + 0.13 * edge_time * 0.002;
-////        LIMIT_MAX( mod, rfin_abs(0) + 0.13 );
+//        double mod = rfin_abs(X) + 0.13 * edge_time * 0.002;
+////        LIMIT_MAX( mod, rfin_abs(X) + 0.13 );
 //        for(int i=0;i<2;i++){
 //          for(int j=2;j<4;j++){
-//            points.push_back(hrp::Vector2(rfin_abs(0) + 0.13,    rfin_abs(1) + rf_mgn(j)));
-//            points.push_back(hrp::Vector2(lfin_abs(0) + lf_mgn(i),    lfin_abs(1) + lf_mgn(j)));
+//            points.push_back(hrp::Vector2(rfin_abs(X) + 0.13,    rfin_abs(Y) + rf_mgn(j)));
+//            points.push_back(hrp::Vector2(lfin_abs(X) + lf_mgn(i),    lfin_abs(Y) + lf_mgn(j)));
 //          }
 //        }
 //        edge_time++;

From f8cbb0e0d76afd4f1f15acb1739849668047abbc Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 16 Feb 2017 00:08:54 +0900
Subject: [PATCH 058/217] remove unused func

---
 idl/AutoBalancerService.idl                   |  1 -
 rtc/AutoBalancer/AutoBalancer.cpp             | 24 ++----
 rtc/AutoBalancer/AutoBalancer.h               |  3 -
 rtc/AutoBalancer/AutoBalancerService_impl.cpp |  5 --
 rtc/AutoBalancer/AutoBalancerService_impl.h   |  1 -
 rtc/AutoBalancer/HumanMasterSlave.h           | 82 ++++---------------
 6 files changed, 22 insertions(+), 94 deletions(-)

diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl
index 9c613ad7439..66b733fd37d 100644
--- a/idl/AutoBalancerService.idl
+++ b/idl/AutoBalancerService.idl
@@ -444,7 +444,6 @@ module OpenHRP
      */
     boolean startHumanSyncAfter5sec();
     boolean setHumanToRobotRatio(in double h2r);
-    boolean setCOMMoveModRatio(in double cmmr);
     boolean setAllowedXYZSync(in boolean x_on, in boolean y_on, in boolean z_on);
     boolean stopHumanSync();
     boolean setFootUpTime(in double fupt);
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index a556a3de101..d10429d7dce 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -501,16 +501,15 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     }
     //for HumanSynchronizer
     if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
-    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hp_raw_data.getw("rfw")); }
-    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hp_raw_data.getw("lfw")); }
-    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_htcom.data,hp_raw_data.getP("com")); }
-    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrf.data,hp_raw_data.getP("rf")); }
-    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlf.data,hp_raw_data.getP("lf")); }
-    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrh.data,hp_raw_data.getP("rh"));}
-    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlh.data,hp_raw_data.getP("lh"));}
+    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hsp->hp_wld_raw.getw("rfw")); }
+    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hsp->hp_wld_raw.getw("lfw")); }
+    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_htcom.data,hsp->hp_wld_raw.getP("com")); }
+    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrf.data,hsp->hp_wld_raw.getP("rf")); }
+    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlf.data,hsp->hp_wld_raw.getP("lf")); }
+    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrh.data,hsp->hp_wld_raw.getP("rh"));}
+    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlh.data,hsp->hp_wld_raw.getP("lh"));}
     if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_hthead.data,hsp->head_cam_pose);}
     if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
-    hsp->readInput(hp_raw_data);
     hsp->current_basepos = m_robot->rootLink()->p;
     if(ikp.count("rarm") && !hsp->use_rh)ikp["rarm"].is_active = false;
     if(ikp.count("larm") && !hsp->use_lh)ikp["larm"].is_active = false;
@@ -1131,7 +1130,7 @@ void AutoBalancer::solveLimbIK ()
       hsp->ht_first_call = false;
     }
   }else{
-    hsp->setInitOffsetPose();
+    hsp->setCurrentInputAsOffset(hsp->hp_wld_raw);
     hsp->calibInitHumanCOMFromZMP();
     hsp->rp_wld_initpos.getP("com").p = m_robot->calcCM();
     hsp->rp_wld_initpos.getP("rf").p = ikp["rleg"].target_link->p;
@@ -1316,13 +1315,6 @@ bool AutoBalancer::setHumanToRobotRatio(const double h2r)
   return true;
 }
 
-bool AutoBalancer::setCOMMoveModRatio(const double cmmr)
-{
-  std::cerr << "[" << m_profile.instance_name << "] set target com_move_mod_ratio as "<< cmmr << std::endl;
-  hsp->setTargetCOMMoveModRatio(cmmr);
-  return true;
-}
-
 bool AutoBalancer::setFootUpTime(const double fupt)
 {
   std::cerr << "[" << m_profile.instance_name << "] set Foot Up Time as"<< fupt <<" [s]"<< std::endl;
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 050680b2558..a5ccc01e236 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -113,7 +113,6 @@ class AutoBalancer
   bool releaseEmergencyStop();
   bool startHumanSyncAfter5sec();
   bool setHumanToRobotRatio(const double h2r);
-  bool setCOMMoveModRatio(const double cmr);
   bool setFootUpTime(const double fupt);
   bool setAllowedXYZSync(const bool x_on,const bool y_on,const bool z_on);
   bool stopHumanSync();
@@ -293,8 +292,6 @@ class AutoBalancer
 
   //for HumanSynchronizer
   boost::shared_ptr<HumanSynchronizer> hsp;
-  HumanPose hp_raw_data;
-
 };
 
 extern "C"
diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.cpp b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
index 6e839832eba..f20d7b1cbdb 100644
--- a/rtc/AutoBalancer/AutoBalancerService_impl.cpp
+++ b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
@@ -125,11 +125,6 @@ CORBA::Boolean AutoBalancerService_impl::setHumanToRobotRatio(const CORBA::Doubl
     return m_autobalancer->setHumanToRobotRatio(h2r);
 };
 
-CORBA::Boolean AutoBalancerService_impl::setCOMMoveModRatio(const CORBA::Double cmmr)
-{
-    return m_autobalancer->setCOMMoveModRatio(cmmr);
-};
-
 CORBA::Boolean AutoBalancerService_impl::setFootUpTime(const CORBA::Double fupt)
 {
     return m_autobalancer->setFootUpTime(fupt);
diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.h b/rtc/AutoBalancer/AutoBalancerService_impl.h
index 3a77d491343..0c76bb8fc86 100644
--- a/rtc/AutoBalancer/AutoBalancerService_impl.h
+++ b/rtc/AutoBalancer/AutoBalancerService_impl.h
@@ -36,7 +36,6 @@ class AutoBalancerService_impl
   CORBA::Boolean releaseEmergencyStop();
   CORBA::Boolean startHumanSyncAfter5sec();
   CORBA::Boolean setHumanToRobotRatio(const CORBA::Double h2r);
-  CORBA::Boolean setCOMMoveModRatio(const CORBA::Double cmmr);
   CORBA::Boolean setFootUpTime(const CORBA::Double fupt);
   CORBA::Boolean setAllowedXYZSync(const CORBA::Boolean x_on,const CORBA::Boolean y_on,const CORBA::Boolean z_on);
   CORBA::Boolean stopHumanSync();
diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index 8d640e75259..251e2f8fe38 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -89,12 +89,10 @@ class BiquadIIRFilterVec{
 class HRPPose3D{
   public:
     hrp::Vector3 p,p_offs,rpy,rpy_offs;
-    hrp::Matrix33 R;
     HRPPose3D(){ clear(); }
     ~HRPPose3D(){}
     void clear(){
       p = p_offs = rpy = hrp::Vector3::Zero();
-      R = hrp::Matrix33::Zero();
     }
 };
 class Wrench6{
@@ -184,15 +182,11 @@ class HumanSynchronizer{
     double CNT_F_TH;
     hrp::Vector3 init_hp_calibcom;
     double MAXVEL,MAXACC;
-    HumanPose hp_wld_raw;
-    HumanPose hp_wld_initpos;
-    HumanPose hp_rel_raw;
     HumanPose rp_ref_out_old;
     int countdown_num;
     bool HumanSyncOn;
     struct timeval t_calc_start, t_calc_end;
     double h2r_ratio, tgt_h2r_ratio;
-    double cmmr, tgt_cmmr;
     double HZ,DT,G;
     hrp::Vector3 com_old,com_oldold,comacc;
     hrp::Vector3 comacc_ref;
@@ -211,6 +205,7 @@ class HumanSynchronizer{
     bool go_rf_landing,go_lf_landing;//足上げ指令の有無
     HumanPose rp_wld_initpos;//IKでのリンク原点の位置(足首，手首など)
     bool use_x,use_y,use_z;
+    HumanPose hp_wld_raw;
     HumanPose rp_ref_out;
     FILE *sr_log,*cz_log;
     bool use_rh,use_lh;
@@ -233,7 +228,6 @@ class HumanSynchronizer{
       HZ = 500;
       DT = 1.0/HZ;
       G = 9.80665;
-      tgt_cmmr = cmmr = 1.0;
       use_x = use_y = use_z = true;
       cur_rfup_level = cur_lfup_level = 0;
       cur_rfup_rad = cur_lfup_rad = 0;
@@ -312,14 +306,17 @@ class HumanSynchronizer{
       delete init_zmp_com_offset_interpolator;
       cout<<"HumanSynchronizer destructed"<<endl;
     }
-
-    void readInput(const HumanPose& raw_in){ hp_wld_raw = raw_in; }
-    void setInitOffsetPose(){ hp_wld_initpos = hp_wld_raw; }
     const int getRemainingCountDown() const { return countdown_num; }
     const bool isHumanSyncOn() const { return HumanSyncOn; }
     const double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0; }
     void setTargetHumanToRobotRatio(const double tgt_h2r_r_in){ tgt_h2r_ratio = tgt_h2r_r_in; }
-    void setTargetCOMMoveModRatio(const double tgt_mod_ratio){ tgt_cmmr = tgt_mod_ratio; }
+    void setCurrentInputAsOffset(HumanPose& tgt){
+      std::string ns[7] = {"com","rf","lf","rh","lh","zmp","head"};
+      for(int i=0;i<7;i++){
+        tgt.getP(ns[i]).p_offs   =  tgt.getP(ns[i]).p;
+        tgt.getP(ns[i]).rpy_offs =  tgt.getP(ns[i]).rpy;
+      }
+    }
     void updateCountDown(){
       if(countdown_num>0){
         countdown_num--;
@@ -331,14 +328,11 @@ class HumanSynchronizer{
     void update(){//////////  メインループ  ////////////
       gettimeofday(&t_calc_start, NULL);
       updateHumanToRobotRatio             (tgt_h2r_ratio);
-      updateCOMModRatio                   (tgt_cmmr);
-      removeInitOffsetPose                (hp_wld_raw, hp_wld_initpos, hp_rel_raw);
 //      applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);//怪しいので要修正
-      calcWorldZMP                        ((hp_rel_raw.getP("rf").p+init_wld_hp_rfpos), (hp_rel_raw.getP("lf").p+init_wld_hp_lfpos), hp_rel_raw.getw("rfw"), hp_rel_raw.getw("lfw"), hp_rel_raw.getP("zmp").p);//足の位置はworldにしないと・・・
-      convertRelHumanPoseToRelRobotPose   (hp_rel_raw, rp_ref_out);
+//      calcWorldZMP                        ((hp_rel_raw.getP("rf").p+init_wld_hp_rfpos), (hp_rel_raw.getP("lf").p+init_wld_hp_lfpos), hp_rel_raw.getw("rfw"), hp_rel_raw.getw("lfw"), hp_rel_raw.getP("zmp").p);//足の位置はworldにしないと・・・
+      convertRelHumanPoseToRelRobotPose   (hp_wld_raw, rp_ref_out);
       if(loop==0)rp_ref_out_old = rp_ref_out;
 
-      applyCOMMoveModRatio                (rp_ref_out);
       judgeFootLandOnCommand              (hp_wld_raw.getw("rfw"), hp_wld_raw.getw("lfw"), go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
       applyEEWorkspaceLimit               (rp_ref_out);
@@ -387,21 +381,6 @@ class HumanSynchronizer{
       init_wld_hp_rfpos = init_wld_rp_rfeepos/h2r_ratio;
       init_wld_hp_lfpos = init_wld_rp_lfeepos/h2r_ratio;
     }
-    void updateCOMModRatio(const double cmmr_goal){
-      const double step = 0.001;
-      if(cmmr_goal - cmmr > step){cmmr += step; cout<<"Interpolating COMMoveModRatio as "<<cmmr<<endl;}
-      else if(cmmr_goal - cmmr < (-1)*step){cmmr -= step; cout<<"Interpolating COMMoveModRatio as "<<cmmr<<endl;}
-      else{cmmr = cmmr_goal;}
-    };
-    void removeInitOffsetPose(const HumanPose& abs_in, const HumanPose& init_offset, HumanPose& rel_out){
-      std::string ns[5] = {"com","rf","lf","rh","lh"};
-      for(int i=0;i<5;i++){
-        rel_out.getP(ns[i]).p   = abs_in.getP(ns[i]).p   - init_offset.getP(ns[i]).p;
-        rel_out.getP(ns[i]).rpy = abs_in.getP(ns[i]).rpy - init_offset.getP(ns[i]).rpy;
-      }
-      rel_out.getw("rfw") = abs_in.getw("rfw");
-      rel_out.getw("lfw") = abs_in.getw("lfw");
-    }
     void applyInitHumanZMPToCOMOffset(const HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
       if(isHumanSyncOn()){
         if(!init_zmp_com_offset_interpolator->isEmpty()){
@@ -421,16 +400,12 @@ class HumanSynchronizer{
     void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
       std::string ns[6] = {"com","rf","lf","rh","lh","zmp"};
       for(int i=0;i<6;i++){
-        rp_out.getP(ns[i]).p   =  h2r_ratio * hp_in.getP(ns[i]).p   + rp_out.getP(ns[i]).p_offs;
-        rp_out.getP(ns[i]).rpy =  hrp::rpyFromRot( hrp::rotFromRpy(hp_in.getP(ns[i]).rpy) * hrp::rotFromRpy(rp_out.getP(ns[i]).rpy_offs) );
-//        rp_out.getP(ns[i]).rpy =         hrp::rpyFromRot(        hrp::rotFromRpy(rp_out.getP(ns[i]).rpy_offs) * hrp::rotFromRpy(hp_in.getP(ns[i]).rpy) );
+        rp_out.getP(ns[i]).p   =  h2r_ratio * (hp_in.getP(ns[i]).p - hp_in.getP(ns[i]).p_offs)   + rp_out.getP(ns[i]).p_offs;
+        rp_out.getP(ns[i]).rpy =  hrp::rpyFromRot( hrp::rotFromRpy(hp_in.getP(ns[i]).rpy) * hrp::rotFromRpy(hp_in.getP(ns[i]).rpy_offs).transpose() * hrp::rotFromRpy(rp_out.getP(ns[i]).rpy_offs) );
       }
       rp_out.getw("rfw")  = hp_in.getw("rfw");
       rp_out.getw("lfw")  = hp_in.getw("lfw");
     }
-    void applyCOMMoveModRatio(HumanPose& tgt){//
-      tgt.getP("com").p = cmmr * tgt.getP("com").p;
-    }
     void judgeFootLandOnCommand(const Wrench6& rfw_in, const Wrench6& lfw_in, bool& rfloc_ans, bool& lfloc_ans){
       if(isHumanSyncOn()){
         if      (rfloc_ans  && rfw_in.f(Z)<CNT_F_TH   ){rfloc_ans = false;}//右足ついた状態から上げる
@@ -444,19 +419,17 @@ class HumanSynchronizer{
 //      if(rfcs_ans && !lfcs_ans){
         if(tgt.getP("zmp").p(Y) > tgt.getP("rf").p(Y) + 0.03){
           lfcs_ans = true;
-          if(loop%100==0)cout<<"LF Auto lock"<<endl;
+//          if(loop%100==0)cout<<"LF Auto lock"<<endl;
         }
 //      }
 //      if(!rfcs_ans && lfcs_ans){
         if(tgt.getP("zmp").p(Y) < tgt.getP("lf").p(Y) - 0.03){
           rfcs_ans = true;
-          if(loop%100==0)cout<<"RF Auto lock"<<endl;
+//          if(loop%100==0)cout<<"RF Auto lock"<<endl;
         }
 //      }
     }
     void setFootRotHorizontalIfGoLanding(const HumanPose& rp_ref_out_old, HumanPose& rp_ref_out){
-//      if(rp_ref_out_old.getP("rf").p(Z)-rp_ref_out_old.getP("rf").p_offs(Z)<0.05){ rp_ref_out.getP("rf").rpy(X) = rp_ref_out.getP("rf").rpy(Y) = 0; }
-//      if(rp_ref_out_old.getP("lf").p(Z)-rp_ref_out_old.getP("lf").p_offs(Z)<0.05){ rp_ref_out.getP("lf").rpy(X) = rp_ref_out.getP("lf").rpy(Y) = 0; }
       if(go_rf_landing){ rp_ref_out.getP("rf").rpy(X) = rp_ref_out.getP("rf").rpy(Y) = 0; }
       if(go_lf_landing){ rp_ref_out.getP("lf").rpy(X) = rp_ref_out.getP("lf").rpy(Y) = 0; }
     }
@@ -473,20 +446,6 @@ class HumanSynchronizer{
       limitGroundContactVelocity(rf_goland_in, com2rf_dist, rp_ref_out_old.getP("rf"), tgt.getP("rf"), is_rf_contact);
       limitGroundContactVelocity(lf_goland_in, com2lf_dist, rp_ref_out_old.getP("lf"), tgt.getP("lf"), is_lf_contact);
     }
-//    void calcFootUpCurveAndJudgeFootContact(const bool& go_land_in, const double& fup_time_in, const double& fdown_time_in,  double& cur_fup_phase_in, HRPPose3D& f_height_out, bool& is_f_contact_out){
-//      const double omega_up = M_PI / fup_time_in;
-//      const double omega_down = M_PI / fdown_time_in;
-//      if(!go_land_in){//足上げ命令入力時
-//        cur_fup_phase_in += omega_up * DT;
-//      }else{//足下げ命令入力時
-//        cur_fup_phase_in -= omega_down * DT;
-//      }
-//      LIMIT_MINMAX( cur_fup_phase_in, 0, M_PI);
-//      double fup_height = f_height_out.p(Z)-f_height_out.p_offs(Z);
-//      LIMIT_MINMAX( fup_height, 0.03, 0.10 );
-//      f_height_out.p(Z) = fup_height/2 * (1-cos(cur_fup_phase_in)) + f_height_out.p_offs(Z);
-//      is_f_contact_out = (cur_fup_phase_in <= 0);
-//    }
     void limitGroundContactVelocity(const bool& go_land_in, const double& com2foot_dist, const HRPPose3D& f_old_in, HRPPose3D& f_in_out, bool& is_f_contact_out){
       double penalty = 1.0 + (0.2 - com2foot_dist) * 5.0;//1.0 ~ 2.0
       LIMIT_MINMAX( penalty, 1.0, 2.0);
@@ -572,24 +531,11 @@ class HumanSynchronizer{
       LIMIT_MINMAX( tgt.getP("rf").p(Z), tgt.getP("rf").p_offs(Z), tgt.getP("rf").p_offs(Z)+0.10);
       LIMIT_MINMAX( tgt.getP("lf").p(Z), tgt.getP("lf").p_offs(Z), tgt.getP("lf").p_offs(Z)+0.10);
 
-
       std::string ns[5] = {"com","rf","lf","rh","lh"};
-
-//
-//      for(int i=0;i<5;i++){
-//        LIMIT_MINMAX( tgt.getP(ns[i]).rpy(X), -30*M_PI/180, 30*M_PI/180 );
-//        LIMIT_MINMAX( tgt.getP(ns[i]).rpy(Y), -30*M_PI/180, 30*M_PI/180 );
-//      }
-//      LIMIT_MINMAX( tgt.getP("rf").rpy(Z), -30*M_PI/180+tgt.getP("com").rpy(Z), 30*M_PI/180+tgt.getP("com").rpy(Z) );
-//      LIMIT_MINMAX( tgt.getP("lf").rpy(Z), -30*M_PI/180+tgt.getP("com").rpy(Z), 30*M_PI/180+tgt.getP("com").rpy(Z) );
-//      LIMIT_MINMAX( tgt.getP("rh").rpy(Z), -30*M_PI/180+tgt.getP("com").rpy(Z), 30*M_PI/180+tgt.getP("com").rpy(Z) );
-//      LIMIT_MINMAX( tgt.getP("lh").rpy(Z), -30*M_PI/180+tgt.getP("com").rpy(Z), 30*M_PI/180+tgt.getP("com").rpy(Z) );
-
       for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( tgt.getP("com").rpy(i), rc.ee_rot_limit["com"][MIN](i), rc.ee_rot_limit["com"][MAX](i) ); }
       for(int i=1;i<5;i++){
         for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( tgt.getP(ns[i]).rpy(j), rc.ee_rot_limit[ns[i]][MIN](j) + tgt.getP("com").rpy(j), rc.ee_rot_limit[ns[i]][MAX](j) + tgt.getP("com").rpy(j) ); }
       }
-
       LIMIT_MINMAX( head_cam_pose.rpy(Y), -20*M_PI/180, 20*M_PI/180 );
       LIMIT_MINMAX( head_cam_pose.rpy(Z), -20*M_PI/180, 20*M_PI/180 );
     }

From 7096b491254bc592b8597f65ed04245489c36766 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 16 Feb 2017 01:34:24 +0900
Subject: [PATCH 059/217] add autoLRswap

---
 rtc/AutoBalancer/HumanMasterSlave.h | 18 +++++++++++++++---
 1 file changed, 15 insertions(+), 3 deletions(-)

diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index 251e2f8fe38..b21e011f17b 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -92,7 +92,7 @@ class HRPPose3D{
     HRPPose3D(){ clear(); }
     ~HRPPose3D(){}
     void clear(){
-      p = p_offs = rpy = hrp::Vector3::Zero();
+      p = p_offs = rpy = rpy_offs = hrp::Vector3::Zero();
     }
 };
 class Wrench6{
@@ -182,7 +182,7 @@ class HumanSynchronizer{
     double CNT_F_TH;
     hrp::Vector3 init_hp_calibcom;
     double MAXVEL,MAXACC;
-    HumanPose rp_ref_out_old;
+    HumanPose rp_ref_out_old,hp_swap_checked;
     int countdown_num;
     bool HumanSyncOn;
     struct timeval t_calc_start, t_calc_end;
@@ -330,7 +330,8 @@ class HumanSynchronizer{
       updateHumanToRobotRatio             (tgt_h2r_ratio);
 //      applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);//怪しいので要修正
 //      calcWorldZMP                        ((hp_rel_raw.getP("rf").p+init_wld_hp_rfpos), (hp_rel_raw.getP("lf").p+init_wld_hp_lfpos), hp_rel_raw.getw("rfw"), hp_rel_raw.getw("lfw"), hp_rel_raw.getP("zmp").p);//足の位置はworldにしないと・・・
-      convertRelHumanPoseToRelRobotPose   (hp_wld_raw, rp_ref_out);
+      autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);
+      convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
       if(loop==0)rp_ref_out_old = rp_ref_out;
 
       judgeFootLandOnCommand              (hp_wld_raw.getw("rfw"), hp_wld_raw.getw("lfw"), go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
@@ -381,6 +382,17 @@ class HumanSynchronizer{
       init_wld_hp_rfpos = init_wld_rp_rfeepos/h2r_ratio;
       init_wld_hp_lfpos = init_wld_rp_lfeepos/h2r_ratio;
     }
+    void autoLRSwapCheck(const HumanPose& in, HumanPose& out){
+      out = in;
+      if(in.getP("rh").p_offs(Y) > in.getP("lh").p_offs(Y) ){
+        out.getP("rh") = in.getP("lh");
+        out.getP("lh") = in.getP("rh");
+      }
+      if(in.getP("rf").p_offs(Y) > in.getP("lf").p_offs(Y) ){
+        out.getP("rf") = in.getP("lf");
+        out.getP("lf") = in.getP("rf");
+      }
+    }
     void applyInitHumanZMPToCOMOffset(const HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
       if(isHumanSyncOn()){
         if(!init_zmp_com_offset_interpolator->isEmpty()){

From 84c987a8c43956695d0797b103826b25c301b385 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 18 Feb 2017 04:22:36 +0900
Subject: [PATCH 060/217] add accelaration CP

---
 rtc/AutoBalancer/AutoBalancer.cpp     |  8 +---
 rtc/AutoBalancer/HumanMasterSlave.cpp | 12 ++---
 rtc/AutoBalancer/HumanMasterSlave.h   | 69 +++++++++++++++++++++------
 rtc/TorqueFilter/IIRFilter.cpp        | 38 ++++++++++-----
 rtc/TorqueFilter/IIRFilter.h          |  1 +
 5 files changed, 91 insertions(+), 37 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index d10429d7dce..2b551a7113d 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1132,12 +1132,6 @@ void AutoBalancer::solveLimbIK ()
   }else{
     hsp->setCurrentInputAsOffset(hsp->hp_wld_raw);
     hsp->calibInitHumanCOMFromZMP();
-    hsp->rp_wld_initpos.getP("com").p = m_robot->calcCM();
-    hsp->rp_wld_initpos.getP("rf").p = ikp["rleg"].target_link->p;
-    hsp->rp_wld_initpos.getP("lf").p = ikp["lleg"].target_link->p;
-    hsp->rp_wld_initpos.getP("zmp").p = ref_zmp;
-    if(ikp.count("rarm"))hsp->rp_wld_initpos.getP("rh").p = ikp["rarm"].target_link->p;
-    if(ikp.count("larm"))hsp->rp_wld_initpos.getP("lh").p = ikp["larm"].target_link->p;
 
     hsp->rp_ref_out.getP("com").p_offs = m_robot->calcCM();
     hsp->rp_ref_out.getP("zmp").p_offs = ref_zmp;
@@ -1146,6 +1140,8 @@ void AutoBalancer::solveLimbIK ()
       if(ikp.count(robot_l_names[i])){
         hsp->rp_ref_out.getP(human_l_names[i]).p_offs = ikp[robot_l_names[i]].target_link->p + ikp[robot_l_names[i]].target_link->R * ikp[robot_l_names[i]].localPos;
         hsp->rp_ref_out.getP(human_l_names[i]).rpy_offs = hrp::rpyFromRot(ikp[robot_l_names[i]].target_link->R * ikp[robot_l_names[i]].localR);
+        hsp->rp_ref_out.getP(human_l_names[i]).p = hsp->rp_ref_out.getP(human_l_names[i]).p_offs;
+        hsp->rp_ref_out.getP(human_l_names[i]).rpy = hsp->rp_ref_out.getP(human_l_names[i]).rpy_offs;
       }
     }
     hsp->pre_cont_rfpos = hsp->rp_ref_out.getP("rf").p_offs;
diff --git a/rtc/AutoBalancer/HumanMasterSlave.cpp b/rtc/AutoBalancer/HumanMasterSlave.cpp
index 5c039b96b41..83d0908f0f7 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.cpp
+++ b/rtc/AutoBalancer/HumanMasterSlave.cpp
@@ -106,9 +106,9 @@ void HumanSynchronizer::applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vect
   LIMIT_MINMAX( comacc(0), -MAXACC, MAXACC);
   LIMIT_MINMAX( comacc(1), -MAXACC, MAXACC);
   comacc = acc4zmp_v_filters.passFilter(comacc);
-  zmpout(0) = comin(0)-(rp_wld_initpos.getP("com").p(2)/G)*comacc(0);
-  zmpout(1) = comin(1)-(rp_wld_initpos.getP("com").p(2)/G)*comacc(1);
-  if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.getP("zmp").p(0),rp_ref_out.getP("zmp").p(1),zmpout(0),zmpout(1));
+  zmpout(0) = comin(0)-(rp_ref_out.getP("com").p(2)/G)*comacc(0);
+  zmpout(1) = comin(1)-(rp_ref_out.getP("com").p(2)/G)*comacc(1);
+//  if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.getP("zmp").p(0),rp_ref_out.getP("zmp").p(1),zmpout(0),zmpout(1));
   com_oldold = com_old;
   com_old = comin;
 }
@@ -121,7 +121,7 @@ void HumanSynchronizer::applyVelLimit(const HumanPose& in, const HumanPose& in_o
   }
 }
 void HumanSynchronizer::applyCOMZMPXYZLock(HumanPose& tgt){
-    if(!use_x){tgt.getP("com").p(0) = 0;tgt.getP("zmp").p(0) = 0;}
-    if(!use_y){tgt.getP("com").p(1) = 0;tgt.getP("zmp").p(1) = 0;}
-    if(!use_z){tgt.getP("com").p(2) = 0;}
+//    if(!use_x){tgt.getP("com").p(0) = 0;tgt.getP("zmp").p(0) = 0;}
+//    if(!use_y){tgt.getP("com").p(1) = 0;tgt.getP("zmp").p(1) = 0;}
+    if(!use_z){tgt.getP("com").p(2) = tgt.getP("com").p_offs(2);}
 }
diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index b21e011f17b..8701c4cf8e6 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -66,7 +66,7 @@ class BiquadIIRFilterVec{
       ff_coeffs.resize(3);
       fb_coeffs.resize(3);
       for(int i=0;i<3;i++){
-        const double fc = tan(fc_in(i) * M_PI / HZ) / (2 * M_PI);
+        const double fc = tan((double)fc_in(i) * M_PI / HZ) / (2 * M_PI);
         const double denom = 1 + (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc;
         ff_coeffs[0] = (4 * M_PI * M_PI * fc*fc) / denom;
         ff_coeffs[1] = (8 * M_PI * M_PI * fc*fc) / denom;
@@ -81,9 +81,12 @@ class BiquadIIRFilterVec{
       setParameter(hrp::Vector3(fc_in,fc_in,fc_in), HZ);
     };
     hrp::Vector3 passFilter(const hrp::Vector3& input){
-      for(int i=0;i<3;i++){ ans(i) = filters[i].passFilter(input(i)); }
+      for(int i=0;i<3;i++){ ans(i) = filters[i].passFilter((double)input(i)); }
       return ans;
     };
+    void init(const hrp::Vector3& initial_input){
+      for(int i=0;i<3;i++){ filters[i].reset((double)initial_input(i));}
+    }
 };
 
 class HRPPose3D{
@@ -203,7 +206,7 @@ class HumanSynchronizer{
     bool ht_first_call;
     bool is_rf_contact,is_lf_contact;//足上げ高さ0に到達しているか否か
     bool go_rf_landing,go_lf_landing;//足上げ指令の有無
-    HumanPose rp_wld_initpos;//IKでのリンク原点の位置(足首，手首など)
+//    HumanPose rp_wld_initpos;//IKでのリンク原点の位置(足首，手首など)
     bool use_x,use_y,use_z;
     HumanPose hp_wld_raw;
     HumanPose rp_ref_out;
@@ -270,8 +273,10 @@ class HumanSynchronizer{
       acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
       cam_rpy_filter.setParameter(1,HZ);//カメラアングル
 
-      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
-      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
+//      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
+//      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
+      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.03,  0.02);
+      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.02, -0.03);
       rf_vert.push_back(hrp::Vector3(0.13,-0.08,0));
       rf_vert.push_back(hrp::Vector3(-0.10,-0.08,0));
       rf_vert.push_back(hrp::Vector3(-0.10,0.06,0));
@@ -341,15 +346,24 @@ class HumanSynchronizer{
       if(go_rf_landing)rp_ref_out.getP("rf").p(Z) = rp_ref_out.getP("rf").p_offs(Z);
       if(go_lf_landing)rp_ref_out.getP("lf").p(Z) = rp_ref_out.getP("lf").p_offs(Z);
 
+
+      if(DEBUG)fprintf(cz_log,"com_ref: %f %f ", rp_ref_out.getP("com").p(X), rp_ref_out.getP("com").p(Y));
+
       applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com"));
 
       setFootRotHorizontalIfGoLanding     (rp_ref_out_old, rp_ref_out);
       applyLPFilter                       (rp_ref_out);
+      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com"));//もう一回？
       applyCOMStateLimitByCapturePoint    (rp_ref_out.getP("com").p, rp_ref_out_old.getP("com").p, com_vel_old, rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com").p);
+      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com"));//もう一回？
 
       cam_rpy_filtered = cam_rpy_filter.passFilter(head_cam_pose.rpy);
       r_zmp_raw = rp_ref_out.getP("zmp").p;
       applyZMPCalcFromCOM                 (rp_ref_out.getP("com").p,rp_ref_out.getP("zmp").p);
+
+      if(DEBUG)fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.getP("zmp").p(X),rp_ref_out.getP("zmp").p(Y));
+      if(DEBUG)fprintf(cz_log,"\n");
+
       applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       applyCOMZMPXYZLock                  (rp_ref_out);
       overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
@@ -406,6 +420,11 @@ class HumanSynchronizer{
     }
     void applyLPFilter(HumanPose& tgt){
       std::string ns[5] = {"com","rf","lf","rh","lh"};
+      if(loop==0){
+        cout<<"init"<<tgt.getP("rf").p.transpose()<<endl;
+        for(int i=0;i<tgt_pos_filters.size();i++){ tgt_pos_filters[i].init(tgt.getP(ns[i]).p); tgt_pos_filters[i].passFilter(tgt.getP(ns[i]).p);}
+        for(int i=0;i<tgt_rot_filters.size();i++){ tgt_rot_filters[i].init(tgt.getP(ns[i]).rpy); tgt_pos_filters[i].passFilter(tgt.getP(ns[i]).rpy);}
+      }
       for(int i=0;i<tgt_pos_filters.size();i++){ tgt.getP(ns[i]).p = tgt_pos_filters[i].passFilter(tgt.getP(ns[i]).p); }
       for(int i=0;i<tgt_rot_filters.size();i++){ tgt.getP(ns[i]).rpy = tgt_rot_filters[i].passFilter(tgt.getP(ns[i]).rpy); }
     }
@@ -569,7 +588,7 @@ class HumanSynchronizer{
 //      double accel[4] = {0.05, -0.03, 0.02, -0.02};
 //      double decel[4] = {0.04, -0.02, 0.01, -0.01};
 //      comacc_ref = calcacc_v_filters.passFilter(com_acc);
-      const double H = rp_wld_initpos.getP("com").p(Z);
+      const double H = rp_ref_out.getP("com").p(Z);
       std::vector<hrp::Vector2> hull;
       hrp::Vector4 marginDelta(+0.001,-0.001,0.001,-0.001);
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region+marginDelta, lf_safe_region+marginDelta, hull);
@@ -578,17 +597,39 @@ class HumanSynchronizer{
 //      regulateCOMVelocityByCapturePointXY( hrp::Vector2(com_old(X),com_old(Y)), hrp::Vector2(com_vel(X),com_vel(Y)), hull, com_vel_ans_2d);//XY独立に計算する手法のほうがエッジで引っかからない・・・
       com_ans(X) = com_old(X) + com_vel_ans_2d(X) * DT;
       com_ans(Y) = com_old(Y) + com_vel_ans_2d(Y) * DT;
+      if(DEBUG){
+        fprintf(cz_log,"com_ans: %f %f ",com_ans(X),com_ans(Y));
+        hrp::Vector2 cp_plot = hrp::Vector2(com_ans(X),com_ans(Y)) + com_vel_ans_2d * sqrt( H / G );
+        fprintf(cz_log,"com_ans_cp: %f %f ",cp_plot(X),cp_plot(Y));
+        for(int i=0;i<hull.size();i++)fprintf(sr_log,"hull_vert%d: %f %f\n",i,hull[i](X),hull[i](Y));
+        fprintf(sr_log,"hull_vert%d: %f %f\n",(int)hull.size(),hull[0](X),hull[0](Y));//閉じる
+        fprintf(sr_log,"\n");
+      }
       return true;
     }
     void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
-      com_vel_ans = com_vel;
-      const double H = rp_wld_initpos.getP("com").p(Z);
-      hrp::Vector2 cp = com_pos + com_vel * sqrt( H / G );
-      hrp::Vector2 cp_ragulated;
-      if(!isPointInHullOpenCV(cp,hull)){
-        calcCrossPointOnHull(com_pos, cp, hull, cp_ragulated);
-        //calcNearestPointOnHull(cp, hull, cp_ragulated);
-        com_vel_ans = (cp_ragulated - com_pos) / sqrt( H / G );
+      hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
+      com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
+      const double H = rp_ref_out.getP("com").p(Z);
+      //減速CP条件
+      hrp::Vector2 cp_dec = com_pos + com_vel * sqrt( H / G );
+      hrp::Vector2 cp_dec_ragulated;
+      if(!isPointInHullOpenCV(cp_dec,hull)){
+        calcCrossPointOnHull(com_pos, cp_dec, hull, cp_dec_ragulated);
+//        calcNearestPointOnHull(cp, hull, cp_ragulated);
+        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H / G );
+      }
+      //加速CP条件
+      hrp::Vector2 cp_acc = com_pos - com_vel * sqrt( H / G );
+      hrp::Vector2 cp_acc_ragulated;
+      if(!isPointInHullOpenCV(cp_acc,hull)){
+        calcCrossPointOnHull(com_pos, cp_acc, hull, cp_acc_ragulated);
+        com_vel_accel_ok = (-cp_acc_ragulated + com_pos) / sqrt( H / G );
+      }
+      if(com_vel_decel_ok.norm() < com_vel_accel_ok.norm()){
+        com_vel_ans = com_vel_decel_ok;
+      }else{
+        com_vel_ans = com_vel_accel_ok;
       }
     }
 //    void regulateCOMVelocityByCapturePointXY(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
diff --git a/rtc/TorqueFilter/IIRFilter.cpp b/rtc/TorqueFilter/IIRFilter.cpp
index a1c76bf58d9..05538f49652 100644
--- a/rtc/TorqueFilter/IIRFilter.cpp
+++ b/rtc/TorqueFilter/IIRFilter.cpp
@@ -20,6 +20,7 @@ IIRFilter::IIRFilter(unsigned int dim, std::vector<double>& fb_coeffs, std::vect
 
     // init previous values
     m_previous_values.assign(dim, 0.0);
+    m_previous_inputs.assign(dim, 0.0);
     m_initialized = true;
     return;
 }
@@ -41,10 +42,6 @@ bool IIRFilter::setParameter(int dim, std::vector<double>& A, std::vector<double
     m_fb_coefficients.clear();
     m_ff_coefficients.clear();
 
-    // clear previous coefficients
-    m_fb_coefficients.clear();
-    m_ff_coefficients.clear();
-
     if (A.size() == dim) {
         m_fb_coefficients.push_back(1.0);
     }
@@ -64,6 +61,7 @@ bool IIRFilter::setParameter(int dim, std::vector<double>& A, std::vector<double
 
     // init previous values
     m_previous_values.assign(dim, 0.0);
+    m_previous_inputs.assign(dim, 0.0);
     m_initialized = true;
     return true;
 }
@@ -87,6 +85,7 @@ void IIRFilter::getParameter(int &dim, std::vector<double>& A, std::vector<doubl
 void IIRFilter::reset(double initial_input)
 {
     m_previous_values.assign(m_dimension, initial_input);
+    m_previous_inputs.assign(m_dimension, initial_input);
 }
 
 double IIRFilter::passFilter(double input)
@@ -94,19 +93,36 @@ double IIRFilter::passFilter(double input)
     if (! m_initialized) {
         return 0.0;
     }
-    double feedback, filtered;
+    //    double feedback, filtered;
+    //    // calcurate retval
+    //    feedback = m_fb_coefficients[0] * input;
+    //    for (int i = 0; i < m_dimension; i++) {
+    //        feedback += m_fb_coefficients[i + 1] * m_previous_values[i];
+    //    }
+    //    filtered = m_ff_coefficients[0] * feedback;
+    //    for(int i = 0; i < m_dimension; i++) {
+    //        filtered += m_ff_coefficients[i + 1] * m_previous_values[i];
+    //    }
+    //    // update previous values
+    //    m_previous_values.push_front(feedback);
+    //    m_previous_values.pop_back();
+    //    return filtered;
+
+    double feedback, feedforward, filtered;
     // calcurate retval
-    feedback = m_fb_coefficients[0] * input;
+    feedback = 0;
     for (int i = 0; i < m_dimension; i++) {
-        feedback += m_fb_coefficients[i + 1] * m_previous_values[i];
+      feedback += m_fb_coefficients[i + 1] * m_previous_values[i];
     }
-    filtered = m_ff_coefficients[0] * feedback;
+    feedforward = m_ff_coefficients[0] * input;
     for(int i = 0; i < m_dimension; i++) {
-        filtered += m_ff_coefficients[i + 1] * m_previous_values[i];
+      feedforward += m_ff_coefficients[i + 1] * m_previous_inputs[i];
     }
+    filtered = feedback + feedforward;
     // update previous values
-    m_previous_values.push_front(feedback);
+    m_previous_values.push_front(filtered);
     m_previous_values.pop_back();
-
+    m_previous_inputs.push_front(input);
+    m_previous_inputs.pop_back();
     return filtered;
 }
diff --git a/rtc/TorqueFilter/IIRFilter.h b/rtc/TorqueFilter/IIRFilter.h
index c196e13bb70..52705e09659 100644
--- a/rtc/TorqueFilter/IIRFilter.h
+++ b/rtc/TorqueFilter/IIRFilter.h
@@ -78,6 +78,7 @@ class IIRFilter
     std::vector<double> m_fb_coefficients; // fb parameters (dim must be m_dimension + 1, m_fb_coefficients[0] would be 1.0)
     std::vector<double> m_ff_coefficients; // ff parameters (dim must be m_dimension + 1)
     std::deque<double> m_previous_values;
+    std::deque<double> m_previous_inputs;
     // double m_prev_output;
     bool m_initialized;
     std::string m_error_prefix;

From 4e60fdaeac62adbb39d7043f550ea1c190bb7956 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 18 Feb 2017 05:50:51 +0900
Subject: [PATCH 061/217] calc com height from com-to-foot

---
 rtc/AutoBalancer/AutoBalancer.cpp     |  2 ++
 rtc/AutoBalancer/HumanMasterSlave.cpp |  4 ++--
 rtc/AutoBalancer/HumanMasterSlave.h   | 18 +++++++++++-------
 3 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 2b551a7113d..f0fc0729aae 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1144,6 +1144,8 @@ void AutoBalancer::solveLimbIK ()
         hsp->rp_ref_out.getP(human_l_names[i]).rpy = hsp->rp_ref_out.getP(human_l_names[i]).rpy_offs;
       }
     }
+    hsp->H_def = hsp->rp_ref_out.getP("com").p_offs(2) - (hsp->rp_ref_out.getP("rf").p_offs(2) + hsp->rp_ref_out.getP("lf").p_offs(2)) / 2;
+
     hsp->pre_cont_rfpos = hsp->rp_ref_out.getP("rf").p_offs;
     hsp->pre_cont_lfpos = hsp->rp_ref_out.getP("lf").p_offs;
     hsp->init_basepos = m_robot->rootLink()->p;
diff --git a/rtc/AutoBalancer/HumanMasterSlave.cpp b/rtc/AutoBalancer/HumanMasterSlave.cpp
index 83d0908f0f7..e52145102b3 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.cpp
+++ b/rtc/AutoBalancer/HumanMasterSlave.cpp
@@ -106,8 +106,8 @@ void HumanSynchronizer::applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vect
   LIMIT_MINMAX( comacc(0), -MAXACC, MAXACC);
   LIMIT_MINMAX( comacc(1), -MAXACC, MAXACC);
   comacc = acc4zmp_v_filters.passFilter(comacc);
-  zmpout(0) = comin(0)-(rp_ref_out.getP("com").p(2)/G)*comacc(0);
-  zmpout(1) = comin(1)-(rp_ref_out.getP("com").p(2)/G)*comacc(1);
+  zmpout(0) = comin(0)-(H_def/G)*comacc(0);
+  zmpout(1) = comin(1)-(H_def/G)*comacc(1);
 //  if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.getP("zmp").p(0),rp_ref_out.getP("zmp").p(1),zmpout(0),zmpout(1));
   com_oldold = com_old;
   com_old = comin;
diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index 8701c4cf8e6..1962c185b08 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -225,6 +225,8 @@ class HumanSynchronizer{
     interpolator *init_zmp_com_offset_interpolator;
     double zmp_com_offset_ip_ratio;
 
+    double H_def;
+
     HumanSynchronizer(){
       tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
 //      tgt_h2r_ratio = h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
@@ -364,6 +366,8 @@ class HumanSynchronizer{
       if(DEBUG)fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.getP("zmp").p(X),rp_ref_out.getP("zmp").p(Y));
       if(DEBUG)fprintf(cz_log,"\n");
 
+      if(loop%100==0)cout<<"H_def: "<<H_def<<endl;
+
       applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       applyCOMZMPXYZLock                  (rp_ref_out);
       overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
@@ -588,7 +592,7 @@ class HumanSynchronizer{
 //      double accel[4] = {0.05, -0.03, 0.02, -0.02};
 //      double decel[4] = {0.04, -0.02, 0.01, -0.01};
 //      comacc_ref = calcacc_v_filters.passFilter(com_acc);
-      const double H = rp_ref_out.getP("com").p(Z);
+//      const double H = rp_ref_out.getP("com").p(Z);
       std::vector<hrp::Vector2> hull;
       hrp::Vector4 marginDelta(+0.001,-0.001,0.001,-0.001);
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region+marginDelta, lf_safe_region+marginDelta, hull);
@@ -599,7 +603,7 @@ class HumanSynchronizer{
       com_ans(Y) = com_old(Y) + com_vel_ans_2d(Y) * DT;
       if(DEBUG){
         fprintf(cz_log,"com_ans: %f %f ",com_ans(X),com_ans(Y));
-        hrp::Vector2 cp_plot = hrp::Vector2(com_ans(X),com_ans(Y)) + com_vel_ans_2d * sqrt( H / G );
+        hrp::Vector2 cp_plot = hrp::Vector2(com_ans(X),com_ans(Y)) + com_vel_ans_2d * sqrt( H_def / G );
         fprintf(cz_log,"com_ans_cp: %f %f ",cp_plot(X),cp_plot(Y));
         for(int i=0;i<hull.size();i++)fprintf(sr_log,"hull_vert%d: %f %f\n",i,hull[i](X),hull[i](Y));
         fprintf(sr_log,"hull_vert%d: %f %f\n",(int)hull.size(),hull[0](X),hull[0](Y));//閉じる
@@ -610,21 +614,21 @@ class HumanSynchronizer{
     void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
       hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
       com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
-      const double H = rp_ref_out.getP("com").p(Z);
+//      const double H = rp_ref_out.getP("com").p(Z);
       //減速CP条件
-      hrp::Vector2 cp_dec = com_pos + com_vel * sqrt( H / G );
+      hrp::Vector2 cp_dec = com_pos + com_vel * sqrt( H_def / G );
       hrp::Vector2 cp_dec_ragulated;
       if(!isPointInHullOpenCV(cp_dec,hull)){
         calcCrossPointOnHull(com_pos, cp_dec, hull, cp_dec_ragulated);
 //        calcNearestPointOnHull(cp, hull, cp_ragulated);
-        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H / G );
+        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_def / G );
       }
       //加速CP条件
-      hrp::Vector2 cp_acc = com_pos - com_vel * sqrt( H / G );
+      hrp::Vector2 cp_acc = com_pos - com_vel * sqrt( H_def / G );
       hrp::Vector2 cp_acc_ragulated;
       if(!isPointInHullOpenCV(cp_acc,hull)){
         calcCrossPointOnHull(com_pos, cp_acc, hull, cp_acc_ragulated);
-        com_vel_accel_ok = (-cp_acc_ragulated + com_pos) / sqrt( H / G );
+        com_vel_accel_ok = (-cp_acc_ragulated + com_pos) / sqrt( H_def / G );
       }
       if(com_vel_decel_ok.norm() < com_vel_accel_ok.norm()){
         com_vel_ans = com_vel_decel_ok;

From de108ff08a94236637d2d46e9e06636621105ed3 Mon Sep 17 00:00:00 2001
From: JAXON <jaxon@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 18 Feb 2017 11:22:07 +0900
Subject: [PATCH 062/217] parameter for real JAXON_RED

---
 rtc/AutoBalancer/HumanMasterSlave.h | 34 ++++++++++++++++-------------
 1 file changed, 19 insertions(+), 15 deletions(-)

diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index 1962c185b08..f1f98fc6c16 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -275,10 +275,10 @@ class HumanSynchronizer{
       acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
       cam_rpy_filter.setParameter(1,HZ);//カメラアングル
 
-//      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
-//      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
-      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.03,  0.02);
-      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.02, -0.03);
+            rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
+      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
+      //      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.03,  0.02);
+      //lf_safe_region = hrp::Vector4(0.02, -0.01, -0.02, -0.03);
       rf_vert.push_back(hrp::Vector3(0.13,-0.08,0));
       rf_vert.push_back(hrp::Vector3(-0.10,-0.08,0));
       rf_vert.push_back(hrp::Vector3(-0.10,0.06,0));
@@ -288,8 +288,8 @@ class HumanSynchronizer{
       lf_vert.push_back(hrp::Vector3(-0.10,0.08,0));
       lf_vert.push_back(hrp::Vector3(0.13,0.08,0));
 
-      use_rh = use_lh = false;
-//      use_rh = use_lh = true;
+            use_rh = use_lh = false;
+      //use_rh = use_lh = true;
 
       rp_ref_out_old.clear();
       rp_ref_out.clear();
@@ -340,7 +340,7 @@ class HumanSynchronizer{
       autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);
       convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
       if(loop==0)rp_ref_out_old = rp_ref_out;
-
+      if(isHumanSyncOn())rp_ref_out.getP("com").p(Z) = rp_ref_out.getP("com").p_offs(Z) + 0.03;
       judgeFootLandOnCommand              (hp_wld_raw.getw("rfw"), hp_wld_raw.getw("lfw"), go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
       applyEEWorkspaceLimit               (rp_ref_out);
@@ -348,6 +348,10 @@ class HumanSynchronizer{
       if(go_rf_landing)rp_ref_out.getP("rf").p(Z) = rp_ref_out.getP("rf").p_offs(Z);
       if(go_lf_landing)rp_ref_out.getP("lf").p(Z) = rp_ref_out.getP("lf").p_offs(Z);
 
+      if(!go_rf_landing)rp_ref_out.getP("rf").p(Z) += 0.03;
+      if(!go_lf_landing)rp_ref_out.getP("lf").p(Z) += 0.03;
+
+
 
       if(DEBUG)fprintf(cz_log,"com_ref: %f %f ", rp_ref_out.getP("com").p(X), rp_ref_out.getP("com").p(Y));
 
@@ -369,7 +373,7 @@ class HumanSynchronizer{
       if(loop%100==0)cout<<"H_def: "<<H_def<<endl;
 
       applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
-      applyCOMZMPXYZLock                  (rp_ref_out);
+      //      applyCOMZMPXYZLock                  (rp_ref_out);
       overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
       modifyFootRotAndXYForContact        (rp_ref_out.getP("rf"), rp_ref_out.getP("lf"));
       com_vel_old = (rp_ref_out.getP("com").p - rp_ref_out_old.getP("com").p)/DT;
@@ -486,7 +490,7 @@ class HumanSynchronizer{
       LIMIT_MINMAX( penalty, 1.0, 2.0);
       LIMIT_MIN( f_in_out.p(Z), f_in_out.p_offs(Z));
       double input_vel = (f_in_out.p(Z) - f_old_in.p(Z)) / DT;
-      const double vel_limit_k = 8.0 * penalty;//地面から0.02[m]地点で0.02*8=0.16[m/s]出ている計算
+      const double vel_limit_k = 2.0 * penalty;//地面から0.02[m]地点で0.02*8=0.16[m/s]出ている計算
       double limit_vel = - vel_limit_k * (f_old_in.p(Z) - f_old_in.p_offs(Z));//sinの時は平均0.05~0.10[m/s]
       const double max_vel_threshold = -0.01;
       LIMIT_MAX( limit_vel, max_vel_threshold);//速度0に近づくと目標になかなか到達しないから
@@ -563,8 +567,8 @@ class HumanSynchronizer{
         LIMIT_MINMAX( tgt.getP("rh").p(i), (current_basepos+base2rh+hand_llimit)(i), (current_basepos+base2rh+hand_ulimit)(i) );
         LIMIT_MINMAX( tgt.getP("lh").p(i), (current_basepos+base2lh+hand_llimit)(i), (current_basepos+base2lh+hand_ulimit)(i) );
       }
-      LIMIT_MINMAX( tgt.getP("rf").p(Z), tgt.getP("rf").p_offs(Z), tgt.getP("rf").p_offs(Z)+0.10);
-      LIMIT_MINMAX( tgt.getP("lf").p(Z), tgt.getP("lf").p_offs(Z), tgt.getP("lf").p_offs(Z)+0.10);
+      LIMIT_MINMAX( tgt.getP("rf").p(Z), tgt.getP("rf").p_offs(Z), tgt.getP("rf").p_offs(Z)+0.05);
+      LIMIT_MINMAX( tgt.getP("lf").p(Z), tgt.getP("lf").p_offs(Z), tgt.getP("lf").p_offs(Z)+0.05);
 
       std::string ns[5] = {"com","rf","lf","rh","lh"};
       for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( tgt.getP("com").rpy(i), rc.ee_rot_limit["com"][MIN](i), rc.ee_rot_limit["com"][MAX](i) ); }
@@ -626,10 +630,10 @@ class HumanSynchronizer{
       //加速CP条件
       hrp::Vector2 cp_acc = com_pos - com_vel * sqrt( H_def / G );
       hrp::Vector2 cp_acc_ragulated;
-      if(!isPointInHullOpenCV(cp_acc,hull)){
-        calcCrossPointOnHull(com_pos, cp_acc, hull, cp_acc_ragulated);
-        com_vel_accel_ok = (-cp_acc_ragulated + com_pos) / sqrt( H_def / G );
-      }
+      //if(!isPointInHullOpenCV(cp_acc,hull)){
+      //       calcCrossPointOnHull(com_pos, cp_acc, hull, cp_acc_ragulated);
+      //        com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_def / G );
+      //      }
       if(com_vel_decel_ok.norm() < com_vel_accel_ok.norm()){
         com_vel_ans = com_vel_decel_ok;
       }else{

From 507e0b9031bbff1ec2d0cae1a26c0ffdbd29c4f5 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 25 Feb 2017 02:13:12 +0900
Subject: [PATCH 063/217] use setParam

---
 idl/AutoBalancerService.idl                   |  16 ++-
 rtc/AutoBalancer/AutoBalancer.cpp             |  67 +++++------
 rtc/AutoBalancer/AutoBalancer.h               |   7 +-
 rtc/AutoBalancer/AutoBalancerService_impl.cpp |  23 ++--
 rtc/AutoBalancer/AutoBalancerService_impl.h   |   7 +-
 rtc/AutoBalancer/HumanMasterSlave.cpp         |   9 +-
 rtc/AutoBalancer/HumanMasterSlave.h           | 105 ++++++++++++------
 7 files changed, 132 insertions(+), 102 deletions(-)

diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl
index 66b733fd37d..728606f8fdb 100644
--- a/idl/AutoBalancerService.idl
+++ b/idl/AutoBalancerService.idl
@@ -442,10 +442,18 @@ module OpenHRP
     /**
      * HumanSync
      */
-    boolean startHumanSyncAfter5sec();
-    boolean setHumanToRobotRatio(in double h2r);
-    boolean setAllowedXYZSync(in boolean x_on, in boolean y_on, in boolean z_on);
+    struct WholeBodyMasterSlaveParam
+    {
+        double human_com_height;
+        double foot_vertical_vel_limit_coeff;
+        boolean set_com_height_fix;
+        double set_com_height_fix_val;
+        boolean use_hands;
+        boolean use_head;
+    };
+    boolean startCountDownForWholeBodyMasterSlave(in double sec);
     boolean stopHumanSync();
-    boolean setFootUpTime(in double fupt);
+    void setWholeBodyMasterSlaveParam(in WholeBodyMasterSlaveParam i_param);
+    void getWholeBodyMasterSlaveParam(out WholeBodyMasterSlaveParam i_param);
   };
 };
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index f0fc0729aae..d2b0d08e3df 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -510,9 +510,8 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlh.data,hsp->hp_wld_raw.getP("lh"));}
     if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_hthead.data,hsp->head_cam_pose);}
     if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
-    hsp->current_basepos = m_robot->rootLink()->p;
-    if(ikp.count("rarm") && !hsp->use_rh)ikp["rarm"].is_active = false;
-    if(ikp.count("larm") && !hsp->use_lh)ikp["larm"].is_active = false;
+    hsp->baselinkpose.p = m_robot->rootLink()->p;
+    hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
     m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
     m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
 
@@ -1132,7 +1131,6 @@ void AutoBalancer::solveLimbIK ()
   }else{
     hsp->setCurrentInputAsOffset(hsp->hp_wld_raw);
     hsp->calibInitHumanCOMFromZMP();
-
     hsp->rp_ref_out.getP("com").p_offs = m_robot->calcCM();
     hsp->rp_ref_out.getP("zmp").p_offs = ref_zmp;
     const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"}, human_l_names[4] = {"rf","lf","rh","lh"};
@@ -1144,12 +1142,14 @@ void AutoBalancer::solveLimbIK ()
         hsp->rp_ref_out.getP(human_l_names[i]).rpy = hsp->rp_ref_out.getP(human_l_names[i]).rpy_offs;
       }
     }
-    hsp->H_def = hsp->rp_ref_out.getP("com").p_offs(2) - (hsp->rp_ref_out.getP("rf").p_offs(2) + hsp->rp_ref_out.getP("lf").p_offs(2)) / 2;
-
     hsp->pre_cont_rfpos = hsp->rp_ref_out.getP("rf").p_offs;
     hsp->pre_cont_lfpos = hsp->rp_ref_out.getP("lf").p_offs;
-    hsp->init_basepos = m_robot->rootLink()->p;
+    hsp->baselinkpose.p_offs = m_robot->rootLink()->p;
+    hsp->baselinkpose.rpy_offs = hrp::rpyFromRot(m_robot->rootLink()->R);
   }
+  hrp::Vector3 rsole_pos = ikp["rleg"].target_link->p+ ikp["rleg"].target_link->R * ikp["rleg"].localPos;
+  hrp::Vector3 lsole_pos = ikp["lleg"].target_link->p+ ikp["lleg"].target_link->R * ikp["lleg"].localPos;
+  hsp->H_cur = m_robot->calcCM()(2) - std::min((double)rsole_pos(2), (double)lsole_pos(2));
   hsp->update();//////HumanSynchronizerの主要処理
   if(loop%100==0)hsp->rp_ref_out.print();
 //  if(loop%100==0)cout<<"time[ms]:"<<hsp->getUpdateTime()*1000<<endl;
@@ -1299,45 +1299,46 @@ void AutoBalancer::solveLimbIK ()
   }
 */
 
-bool AutoBalancer::startHumanSyncAfter5sec()
-{
-  std::cerr << "[" << m_profile.instance_name << "] start HumanSync after 5 sec" << std::endl;
-  hsp->startCountdownForHumanSync = true;
-  return true;
-}
-
-bool AutoBalancer::setHumanToRobotRatio(const double h2r)
-{
-  std::cerr << "[" << m_profile.instance_name << "] set target h2r_ratio as "<< h2r << std::endl;
-  hsp->setTargetHumanToRobotRatio(h2r);
-  return true;
-}
-
-bool AutoBalancer::setFootUpTime(const double fupt)
+bool AutoBalancer::startCountDownForWholeBodyMasterSlave(const double sec)
 {
-  std::cerr << "[" << m_profile.instance_name << "] set Foot Up Time as"<< fupt <<" [s]"<< std::endl;
-  if(fupt > 0.05 && fupt < 5){ hsp->tgt_FUP_TIME = fupt; return true; }else{ return false; }//おかしな値は弾く
-}
-
-bool AutoBalancer::setAllowedXYZSync(const bool x_on,const bool y_on,const bool z_on)
-{
-  if(!hsp->isHumanSyncOn()){
-    std::cerr << "[" << m_profile.instance_name << "] set allowed XYZ move direction as ("<<x_on<<","<<y_on<<","<<z_on<<")" << std::endl;
-    hsp->use_x = x_on;    hsp->use_y = y_on;    hsp->use_z = z_on;
+  if(sec >= 0.0 && sec <= 30.0){
+    std::cerr << "[" << m_profile.instance_name << "] start Synchronization after "<<sec<<" [s]" << std::endl;
+    hsp->countdown_sec = sec;
+    hsp->startCountdownForHumanSync = true;
     return true;
   }else{
-    std::cerr << "[" << m_profile.instance_name << "] allowed XYZ move direction cannot be changed in runtime "<< std::endl;
+    std::cerr << "[" << m_profile.instance_name << "] Count Down Time must be 0 < T < 30 [s]"<< std::endl;
     return false;
   }
 }
 
-
 bool AutoBalancer::stopHumanSync()
 {
 	std::cerr << "[" << m_profile.instance_name << "] stop HumanSync Now" << std::endl;
 	return true;
 }
 
+bool AutoBalancer::setWholeBodyMasterSlaveParam(const OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam& i_param){
+  std::cerr << "[" << m_profile.instance_name << "] setWholeBodyMasterSlaveParam" << std::endl;
+  hsp->WBMSparam.use_rh = hsp->WBMSparam.use_lh = i_param.use_hands;
+  hsp->WBMSparam.use_head = i_param.use_head;
+  hsp->WBMSparam.set_com_height_fix = i_param.set_com_height_fix;
+  hsp->WBMSparam.set_com_height_fix_val = i_param.set_com_height_fix_val;
+  hsp->WBMSparam.foot_vertical_vel_limit_coeff = i_param.foot_vertical_vel_limit_coeff;
+  hsp->WBMSparam.human_com_height = i_param.human_com_height;
+  return true;
+}
+bool AutoBalancer::getWholeBodyMasterSlaveParam(OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam& i_param){
+  std::cerr << "[" << m_profile.instance_name << "] getWholeBodyMasterSlaveParam" << std::endl;
+  i_param.set_com_height_fix = hsp->WBMSparam.set_com_height_fix;
+  i_param.set_com_height_fix_val = hsp->WBMSparam.set_com_height_fix_val;
+  i_param.foot_vertical_vel_limit_coeff = hsp->WBMSparam.foot_vertical_vel_limit_coeff;
+  i_param.human_com_height = hsp->WBMSparam.human_com_height;
+  i_param.use_hands = (hsp->WBMSparam.use_rh || hsp->WBMSparam.use_lh);
+  i_param.use_head = hsp->WBMSparam.use_head;
+  return true;
+}
+
 void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence& limbs)
 {
   std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index a5ccc01e236..b9b47f4ecc2 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -111,11 +111,10 @@ class AutoBalancer
   bool getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx);
   bool getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep);
   bool releaseEmergencyStop();
-  bool startHumanSyncAfter5sec();
-  bool setHumanToRobotRatio(const double h2r);
-  bool setFootUpTime(const double fupt);
-  bool setAllowedXYZSync(const bool x_on,const bool y_on,const bool z_on);
+  bool startCountDownForWholeBodyMasterSlave(const double sec);
   bool stopHumanSync();
+  bool setWholeBodyMasterSlaveParam(const OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam& i_param);
+  bool getWholeBodyMasterSlaveParam(OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam& i_param);
 
  protected:
   // Configuration variable declaration
diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.cpp b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
index f20d7b1cbdb..6d41e542690 100644
--- a/rtc/AutoBalancer/AutoBalancerService_impl.cpp
+++ b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
@@ -115,28 +115,25 @@ void AutoBalancerService_impl::autobalancer(AutoBalancer *i_autobalancer)
   m_autobalancer = i_autobalancer;
 }
 
-CORBA::Boolean AutoBalancerService_impl::startHumanSyncAfter5sec()
+CORBA::Boolean AutoBalancerService_impl::startCountDownForWholeBodyMasterSlave(CORBA::Double sec)
 {
-    return m_autobalancer->startHumanSyncAfter5sec();
+    return m_autobalancer->startCountDownForWholeBodyMasterSlave(sec);
 };
 
-CORBA::Boolean AutoBalancerService_impl::setHumanToRobotRatio(const CORBA::Double h2r)
-{
-    return m_autobalancer->setHumanToRobotRatio(h2r);
-};
-
-CORBA::Boolean AutoBalancerService_impl::setFootUpTime(const CORBA::Double fupt)
+CORBA::Boolean AutoBalancerService_impl::stopHumanSync()
 {
-    return m_autobalancer->setFootUpTime(fupt);
+    return m_autobalancer->stopHumanSync();
 };
 
-CORBA::Boolean AutoBalancerService_impl::setAllowedXYZSync(const CORBA::Boolean x_on,const CORBA::Boolean y_on,const CORBA::Boolean z_on)
+void AutoBalancerService_impl::setWholeBodyMasterSlaveParam(const OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam& i_param)
 {
-    return m_autobalancer->setAllowedXYZSync(x_on,y_on,z_on);
+  m_autobalancer->setWholeBodyMasterSlaveParam(i_param);
 };
 
-CORBA::Boolean AutoBalancerService_impl::stopHumanSync()
+void AutoBalancerService_impl::getWholeBodyMasterSlaveParam(OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam_out i_param)
 {
-    return m_autobalancer->stopHumanSync();
+//  i_param = new OpenHRP::WholeBodyMasterSlaveParam::AutoBalancerParam();
+//  return  m_autobalancer->getWholeBodyMasterSlaveParam(*i_param);
+  m_autobalancer->getWholeBodyMasterSlaveParam(i_param);
 };
 
diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.h b/rtc/AutoBalancer/AutoBalancerService_impl.h
index 0c76bb8fc86..56793b33cac 100644
--- a/rtc/AutoBalancer/AutoBalancerService_impl.h
+++ b/rtc/AutoBalancer/AutoBalancerService_impl.h
@@ -34,11 +34,10 @@ class AutoBalancerService_impl
   CORBA::Boolean getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep , CORBA::Long& o_current_fs_idx);
   CORBA::Boolean getGoPosFootstepsSequence(CORBA::Double x, CORBA::Double y, CORBA::Double th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep);
   CORBA::Boolean releaseEmergencyStop();
-  CORBA::Boolean startHumanSyncAfter5sec();
-  CORBA::Boolean setHumanToRobotRatio(const CORBA::Double h2r);
-  CORBA::Boolean setFootUpTime(const CORBA::Double fupt);
-  CORBA::Boolean setAllowedXYZSync(const CORBA::Boolean x_on,const CORBA::Boolean y_on,const CORBA::Boolean z_on);
+  CORBA::Boolean startCountDownForWholeBodyMasterSlave(const CORBA::Double sec);
   CORBA::Boolean stopHumanSync();
+  void setWholeBodyMasterSlaveParam(const OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam& i_param);
+  void getWholeBodyMasterSlaveParam(OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam_out i_param);
   //
   //
   void autobalancer(AutoBalancer *i_autobalancer);
diff --git a/rtc/AutoBalancer/HumanMasterSlave.cpp b/rtc/AutoBalancer/HumanMasterSlave.cpp
index e52145102b3..b5d2141a560 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.cpp
+++ b/rtc/AutoBalancer/HumanMasterSlave.cpp
@@ -106,8 +106,8 @@ void HumanSynchronizer::applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vect
   LIMIT_MINMAX( comacc(0), -MAXACC, MAXACC);
   LIMIT_MINMAX( comacc(1), -MAXACC, MAXACC);
   comacc = acc4zmp_v_filters.passFilter(comacc);
-  zmpout(0) = comin(0)-(H_def/G)*comacc(0);
-  zmpout(1) = comin(1)-(H_def/G)*comacc(1);
+  zmpout(0) = comin(0)-(H_cur/G)*comacc(0);
+  zmpout(1) = comin(1)-(H_cur/G)*comacc(1);
 //  if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.getP("zmp").p(0),rp_ref_out.getP("zmp").p(1),zmpout(0),zmpout(1));
   com_oldold = com_old;
   com_old = comin;
@@ -120,8 +120,3 @@ void HumanSynchronizer::applyVelLimit(const HumanPose& in, const HumanPose& in_o
     out.getP(ns[i]).p = in_old.getP(ns[i]).p + diff;
   }
 }
-void HumanSynchronizer::applyCOMZMPXYZLock(HumanPose& tgt){
-//    if(!use_x){tgt.getP("com").p(0) = 0;tgt.getP("zmp").p(0) = 0;}
-//    if(!use_y){tgt.getP("com").p(1) = 0;tgt.getP("zmp").p(1) = 0;}
-    if(!use_z){tgt.getP("com").p(2) = tgt.getP("com").p_offs(2);}
-}
diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index f1f98fc6c16..dd2717c6694 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -41,7 +41,7 @@ extern "C" {
 #define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
 
 //実機だとログ出力で落ちる？getenv("HOME")ダメ？
-#define DEBUG 0
+#define DEBUG 1
 
 namespace myconst{
   const int X = 0, Y = 1, Z = 2, XYZ = 3;
@@ -186,7 +186,7 @@ class HumanSynchronizer{
     hrp::Vector3 init_hp_calibcom;
     double MAXVEL,MAXACC;
     HumanPose rp_ref_out_old,hp_swap_checked;
-    int countdown_num;
+//    int countdown_num;
     bool HumanSyncOn;
     struct timeval t_calc_start, t_calc_end;
     double h2r_ratio, tgt_h2r_ratio;
@@ -211,21 +211,30 @@ class HumanSynchronizer{
     HumanPose hp_wld_raw;
     HumanPose rp_ref_out;
     FILE *sr_log,*cz_log;
-    bool use_rh,use_lh;
     hrp::Vector3 init_wld_rp_rfeepos,init_wld_rp_lfeepos;
     hrp::Vector3 init_wld_hp_rfpos,init_wld_hp_lfpos;
-    hrp::Vector3 current_basepos,init_basepos;
+    HRPPose3D baselinkpose;
     hrp::Vector3 com_vel_old;
     unsigned int loop;
     hrp::Vector3 cam_pos_filtered,cam_rpy_filtered;
     HRPPose3D head_cam_pose;
     hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos, pre_cont_rfrot,pre_cont_lfrot;
     std::vector<hrp::Vector3> rf_vert,lf_vert;
+    double countdown_sec;
 
     interpolator *init_zmp_com_offset_interpolator;
     double zmp_com_offset_ip_ratio;
+    struct WBMSparameters {
+      bool set_com_height_fix;
+      double set_com_height_fix_val;
+      double foot_vertical_vel_limit_coeff;
+      double human_com_height;
+      bool use_rh,use_lh;
+      bool use_head;
+    };
+    struct WBMSparameters WBMSparam;
 
-    double H_def;
+    double H_cur;
 
     HumanSynchronizer(){
       tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
@@ -247,7 +256,8 @@ class HumanSynchronizer{
       HumanSyncOn = false;
       ht_first_call = true;
       startCountdownForHumanSync = false;
-      countdown_num = 5*HZ;
+      countdown_sec = 5.0;
+//      countdown_num = (int)(countdown_sec*HZ);
       loop = 0;
 
       //////////  hrp::Vector は初期化必要  ///////////
@@ -257,7 +267,6 @@ class HumanSynchronizer{
 
       init_wld_rp_rfeepos = init_wld_rp_lfeepos = hrp::Vector3::Zero();
       init_wld_hp_rfpos = init_wld_hp_lfpos = hrp::Vector3::Zero();
-      current_basepos = init_basepos = hrp::Vector3::Zero();
 
       com_vel_old = hrp::Vector3::Zero();
       cam_pos_filtered = cam_rpy_filtered = hrp::Vector3::Zero();
@@ -275,7 +284,7 @@ class HumanSynchronizer{
       acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
       cam_rpy_filter.setParameter(1,HZ);//カメラアングル
 
-            rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
+      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
       lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
       //      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.03,  0.02);
       //lf_safe_region = hrp::Vector4(0.02, -0.01, -0.02, -0.03);
@@ -288,8 +297,13 @@ class HumanSynchronizer{
       lf_vert.push_back(hrp::Vector3(-0.10,0.08,0));
       lf_vert.push_back(hrp::Vector3(0.13,0.08,0));
 
-            use_rh = use_lh = false;
-      //use_rh = use_lh = true;
+//      use_rh = use_lh = false;
+      WBMSparam.use_rh = WBMSparam.use_lh = true;
+      WBMSparam.use_head = true;
+      WBMSparam.set_com_height_fix = true;
+      WBMSparam.set_com_height_fix_val = 0.03;
+      WBMSparam.foot_vertical_vel_limit_coeff = 2.0;
+      WBMSparam.human_com_height = 1.00;
 
       rp_ref_out_old.clear();
       rp_ref_out.clear();
@@ -313,7 +327,7 @@ class HumanSynchronizer{
       delete init_zmp_com_offset_interpolator;
       cout<<"HumanSynchronizer destructed"<<endl;
     }
-    const int getRemainingCountDown() const { return countdown_num; }
+    const double getRemainingCountDown() const { return countdown_sec; }
     const bool isHumanSyncOn() const { return HumanSyncOn; }
     const double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0; }
     void setTargetHumanToRobotRatio(const double tgt_h2r_r_in){ tgt_h2r_ratio = tgt_h2r_r_in; }
@@ -325,22 +339,22 @@ class HumanSynchronizer{
       }
     }
     void updateCountDown(){
-      if(countdown_num>0){
-        countdown_num--;
-      }else if(countdown_num==0){
+      if(countdown_sec>0.0){
+        countdown_sec -= DT;
+      }else if(countdown_sec <= 0){
         HumanSyncOn = true;
         startCountdownForHumanSync = false;
       }
     }
     void update(){//////////  メインループ  ////////////
       gettimeofday(&t_calc_start, NULL);
-      updateHumanToRobotRatio             (tgt_h2r_ratio);
+      updateParams                        ();
 //      applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);//怪しいので要修正
 //      calcWorldZMP                        ((hp_rel_raw.getP("rf").p+init_wld_hp_rfpos), (hp_rel_raw.getP("lf").p+init_wld_hp_lfpos), hp_rel_raw.getw("rfw"), hp_rel_raw.getw("lfw"), hp_rel_raw.getP("zmp").p);//足の位置はworldにしないと・・・
       autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);
       convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
       if(loop==0)rp_ref_out_old = rp_ref_out;
-      if(isHumanSyncOn())rp_ref_out.getP("com").p(Z) = rp_ref_out.getP("com").p_offs(Z) + 0.03;
+      if(isHumanSyncOn() && WBMSparam.set_com_height_fix)rp_ref_out.getP("com").p(Z) = rp_ref_out.getP("com").p_offs(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
       judgeFootLandOnCommand              (hp_wld_raw.getw("rfw"), hp_wld_raw.getw("lfw"), go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
       applyEEWorkspaceLimit               (rp_ref_out);
@@ -368,16 +382,16 @@ class HumanSynchronizer{
       applyZMPCalcFromCOM                 (rp_ref_out.getP("com").p,rp_ref_out.getP("zmp").p);
 
       if(DEBUG)fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.getP("zmp").p(X),rp_ref_out.getP("zmp").p(Y));
-      if(DEBUG)fprintf(cz_log,"\n");
-
-      if(loop%100==0)cout<<"H_def: "<<H_def<<endl;
 
       applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
-      //      applyCOMZMPXYZLock                  (rp_ref_out);
       overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
       modifyFootRotAndXYForContact        (rp_ref_out.getP("rf"), rp_ref_out.getP("lf"));
-      com_vel_old = (rp_ref_out.getP("com").p - rp_ref_out_old.getP("com").p)/DT;
 
+      if(DEBUG)fprintf(cz_log,"rf: %f %f %f ",rp_ref_out.getP("rf").p(X),rp_ref_out.getP("rf").p(Y),rp_ref_out.getP("rf").p(Z));
+      if(DEBUG)fprintf(cz_log,"lf: %f %f %f ",rp_ref_out.getP("lf").p(X),rp_ref_out.getP("lf").p(Y),rp_ref_out.getP("lf").p(Z));
+      if(DEBUG)fprintf(cz_log,"\n");
+
+      com_vel_old = (rp_ref_out.getP("com").p - rp_ref_out_old.getP("com").p)/DT;
       rp_ref_out_old = rp_ref_out;
       loop++;
       gettimeofday(&t_calc_end, NULL);
@@ -396,6 +410,10 @@ class HumanSynchronizer{
     static double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(X)*b(Y)-a(Y)*b(X); }
 
   private:
+    void updateParams(){
+//      tgt_h2r_ratio = H_cur/WBMSparam.human_com_height;
+      updateHumanToRobotRatio(tgt_h2r_ratio);
+    }
     void updateHumanToRobotRatio(const double h2r_r_goal){//h2r_ratioに伴って変わる変数の処理もここに書く
       const double step = 0.001;
       if(h2r_r_goal - h2r_ratio > step){h2r_ratio += step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
@@ -480,8 +498,8 @@ class HumanSynchronizer{
 //      LIMIT_MINMAX( LFDOWN_TIME, FUP_TIME, 0.8);
 //      calcFootUpCurveAndJudgeFootContact(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.getP("rf"), is_rf_contact);
 //      calcFootUpCurveAndJudgeFootContact(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.getP("lf"), is_lf_contact);
-      const double com2rf_dist = fabs(tgt.getP("rf").p(Y) - tgt.getP("com").p(Y));
-      const double com2lf_dist = fabs(tgt.getP("lf").p(Y) - tgt.getP("com").p(Y));
+      const double com2rf_dist = fabs((double)tgt.getP("rf").p(Y) - (double)tgt.getP("com").p(Y));
+      const double com2lf_dist = fabs((double)tgt.getP("lf").p(Y) - (double)tgt.getP("com").p(Y));
       limitGroundContactVelocity(rf_goland_in, com2rf_dist, rp_ref_out_old.getP("rf"), tgt.getP("rf"), is_rf_contact);
       limitGroundContactVelocity(lf_goland_in, com2lf_dist, rp_ref_out_old.getP("lf"), tgt.getP("lf"), is_lf_contact);
     }
@@ -490,7 +508,7 @@ class HumanSynchronizer{
       LIMIT_MINMAX( penalty, 1.0, 2.0);
       LIMIT_MIN( f_in_out.p(Z), f_in_out.p_offs(Z));
       double input_vel = (f_in_out.p(Z) - f_old_in.p(Z)) / DT;
-      const double vel_limit_k = 2.0 * penalty;//地面から0.02[m]地点で0.02*8=0.16[m/s]出ている計算
+      const double vel_limit_k = WBMSparam.foot_vertical_vel_limit_coeff * penalty;//地面から0.02[m]地点で0.02*8=0.16[m/s]出ている計算
       double limit_vel = - vel_limit_k * (f_old_in.p(Z) - f_old_in.p_offs(Z));//sinの時は平均0.05~0.10[m/s]
       const double max_vel_threshold = -0.01;
       LIMIT_MAX( limit_vel, max_vel_threshold);//速度0に近づくと目標になかなか到達しないから
@@ -559,13 +577,23 @@ class HumanSynchronizer{
         }
         LIMIT_MIN(tgt.getP("lf").p(Y), pre_cont_rfpos(Y) + FOOT_2_FOOT_COLLISION_MARGIIN);
       }
-      hrp::Vector3 base2rh = tgt.getP("rh").p_offs - init_basepos;
-      hrp::Vector3 base2lh = tgt.getP("lh").p_offs - init_basepos;
+      hrp::Vector3 init_base2rh = tgt.getP("rh").p_offs - baselinkpose.p_offs;
+      hrp::Vector3 init_base2lh = tgt.getP("lh").p_offs - baselinkpose.p_offs;
+      hrp::Matrix33 init_base2rh_R = hrp::rotFromRpy(baselinkpose.rpy_offs).transpose() * hrp::rotFromRpy(tgt.getP("rh").rpy_offs);
+      hrp::Matrix33 init_base2lh_R = hrp::rotFromRpy(baselinkpose.rpy_offs).transpose() * hrp::rotFromRpy(tgt.getP("lh").rpy_offs);
       hrp::Vector3 hand_ulimit = hrp::Vector3(0.1,0.1,0.1);
       hrp::Vector3 hand_llimit = hrp::Vector3(-0.1,-0.1,-0.1);
+      if(!WBMSparam.use_rh){
+        tgt.getP("rh").rpy = hrp::rpyFromRot( hrp::rotFromRpy(baselinkpose.rpy) * init_base2rh_R);
+        tgt.getP("rh").p = baselinkpose.p + hrp::rotFromRpy(tgt.getP("rh").rpy) * init_base2rh;
+      }
+      if(!WBMSparam.use_lh){
+        tgt.getP("lh").rpy = hrp::rpyFromRot( hrp::rotFromRpy(baselinkpose.rpy) * init_base2lh_R);
+        tgt.getP("lh").p = baselinkpose.p + hrp::rotFromRpy(tgt.getP("lh").rpy) * init_base2lh;
+      }
       for(int i=0;i<3;i++){
-        LIMIT_MINMAX( tgt.getP("rh").p(i), (current_basepos+base2rh+hand_llimit)(i), (current_basepos+base2rh+hand_ulimit)(i) );
-        LIMIT_MINMAX( tgt.getP("lh").p(i), (current_basepos+base2lh+hand_llimit)(i), (current_basepos+base2lh+hand_ulimit)(i) );
+        LIMIT_MINMAX( tgt.getP("rh").p(i), (baselinkpose.p+init_base2rh+hand_llimit)(i), (baselinkpose.p+init_base2rh+hand_ulimit)(i) );
+        LIMIT_MINMAX( tgt.getP("lh").p(i), (baselinkpose.p+init_base2lh+hand_llimit)(i), (baselinkpose.p+init_base2lh+hand_ulimit)(i) );
       }
       LIMIT_MINMAX( tgt.getP("rf").p(Z), tgt.getP("rf").p_offs(Z), tgt.getP("rf").p_offs(Z)+0.05);
       LIMIT_MINMAX( tgt.getP("lf").p(Z), tgt.getP("lf").p_offs(Z), tgt.getP("lf").p_offs(Z)+0.05);
@@ -575,6 +603,7 @@ class HumanSynchronizer{
       for(int i=1;i<5;i++){
         for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( tgt.getP(ns[i]).rpy(j), rc.ee_rot_limit[ns[i]][MIN](j) + tgt.getP("com").rpy(j), rc.ee_rot_limit[ns[i]][MAX](j) + tgt.getP("com").rpy(j) ); }
       }
+      if(!WBMSparam.use_head)head_cam_pose.rpy = hrp::Vector3::Zero();
       LIMIT_MINMAX( head_cam_pose.rpy(Y), -20*M_PI/180, 20*M_PI/180 );
       LIMIT_MINMAX( head_cam_pose.rpy(Z), -20*M_PI/180, 20*M_PI/180 );
     }
@@ -607,8 +636,10 @@ class HumanSynchronizer{
       com_ans(Y) = com_old(Y) + com_vel_ans_2d(Y) * DT;
       if(DEBUG){
         fprintf(cz_log,"com_ans: %f %f ",com_ans(X),com_ans(Y));
-        hrp::Vector2 cp_plot = hrp::Vector2(com_ans(X),com_ans(Y)) + com_vel_ans_2d * sqrt( H_def / G );
-        fprintf(cz_log,"com_ans_cp: %f %f ",cp_plot(X),cp_plot(Y));
+        hrp::Vector2 cp_d_plot = hrp::Vector2(com_ans(X),com_ans(Y)) + com_vel_ans_2d * sqrt( H_cur / G );
+        hrp::Vector2 cp_a_plot = hrp::Vector2(com_ans(X),com_ans(Y)) - com_vel_ans_2d * sqrt( H_cur / G );
+        fprintf(cz_log,"com_ans_cp_d: %f %f ",cp_d_plot(X),cp_d_plot(Y));
+        fprintf(cz_log,"com_ans_cp_a: %f %f ",cp_a_plot(X),cp_a_plot(Y));
         for(int i=0;i<hull.size();i++)fprintf(sr_log,"hull_vert%d: %f %f\n",i,hull[i](X),hull[i](Y));
         fprintf(sr_log,"hull_vert%d: %f %f\n",(int)hull.size(),hull[0](X),hull[0](Y));//閉じる
         fprintf(sr_log,"\n");
@@ -620,20 +651,20 @@ class HumanSynchronizer{
       com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
 //      const double H = rp_ref_out.getP("com").p(Z);
       //減速CP条件
-      hrp::Vector2 cp_dec = com_pos + com_vel * sqrt( H_def / G );
+      hrp::Vector2 cp_dec = com_pos + com_vel * sqrt( H_cur / G );
       hrp::Vector2 cp_dec_ragulated;
       if(!isPointInHullOpenCV(cp_dec,hull)){
         calcCrossPointOnHull(com_pos, cp_dec, hull, cp_dec_ragulated);
 //        calcNearestPointOnHull(cp, hull, cp_ragulated);
-        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_def / G );
+        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
       }
       //加速CP条件
-      hrp::Vector2 cp_acc = com_pos - com_vel * sqrt( H_def / G );
+      hrp::Vector2 cp_acc = com_pos - com_vel * sqrt( H_cur / G );
       hrp::Vector2 cp_acc_ragulated;
-      //if(!isPointInHullOpenCV(cp_acc,hull)){
-      //       calcCrossPointOnHull(com_pos, cp_acc, hull, cp_acc_ragulated);
-      //        com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_def / G );
-      //      }
+      if(!isPointInHullOpenCV(cp_acc,hull)){
+         calcCrossPointOnHull(com_pos, cp_acc, hull, cp_acc_ragulated);
+         com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
+      }
       if(com_vel_decel_ok.norm() < com_vel_accel_ok.norm()){
         com_vel_ans = com_vel_decel_ok;
       }else{

From 0c634a14b8527a939d986633eeff1222b73ed2bc Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 4 Mar 2017 09:59:03 +0900
Subject: [PATCH 064/217] change the order of LPF and CPregu, use setParam

---
 idl/AutoBalancerService.idl           |   3 +
 python/hrpsys_config.py               |  20 +++
 rtc/AutoBalancer/AutoBalancer.cpp     | 105 ++++++++++++++
 rtc/AutoBalancer/AutoBalancer.h       |  37 +++++
 rtc/AutoBalancer/HumanMasterSlave.cpp |   7 +-
 rtc/AutoBalancer/HumanMasterSlave.h   | 189 ++++++++++++--------------
 6 files changed, 256 insertions(+), 105 deletions(-)

diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl
index 728606f8fdb..fa53513346f 100644
--- a/idl/AutoBalancerService.idl
+++ b/idl/AutoBalancerService.idl
@@ -450,6 +450,9 @@ module OpenHRP
         double set_com_height_fix_val;
         boolean use_hands;
         boolean use_head;
+        double swing_foot_height_offset;
+        double swing_foot_max_height;
+        double auto_swing_foot_landing_threshold;
     };
     boolean startCountDownForWholeBodyMasterSlave(in double sec);
     boolean stopHumanSync();
diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index 409416e2432..7e1b9eb14f6 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -871,6 +871,26 @@ def setupLogger(self, maxLength=4000):
             self.connectLoggerPort(self.abc, 'contactStates')
             self.connectLoggerPort(self.abc, 'controlSwingSupportTime')
             self.connectLoggerPort(self.abc, 'cogOut')
+            ### ishiguro ###
+            self.connectLoggerPort(self.abc, 'controlSwingSupportTime')
+            self.connectLoggerPort(self.abc, 'htcom_dbgOut')
+            self.connectLoggerPort(self.abc, 'htrf_dbgOut')
+            self.connectLoggerPort(self.abc, 'htlf_dbgOut')
+            self.connectLoggerPort(self.abc, 'htrh_dbgOut')
+            self.connectLoggerPort(self.abc, 'htlh_dbgOut')
+            self.connectLoggerPort(self.abc, 'hthead_dbgOut')
+            self.connectLoggerPort(self.abc, 'htzmp_dbgOut')
+            self.connectLoggerPort(self.abc, 'htrfw_dbgOut')
+            self.connectLoggerPort(self.abc, 'htlfw_dbgOut')
+            self.connectLoggerPort(self.abc, 'rpcom_dbgOut')
+            self.connectLoggerPort(self.abc, 'rprf_dbgOut')
+            self.connectLoggerPort(self.abc, 'rplf_dbgOut')
+            self.connectLoggerPort(self.abc, 'rprh_dbgOut')
+            self.connectLoggerPort(self.abc, 'rplh_dbgOut')
+            self.connectLoggerPort(self.abc, 'rphead_dbgOut')
+            self.connectLoggerPort(self.abc, 'rpzmp_dbgOut')
+            self.connectLoggerPort(self.abc, 'rpdcp_dbgOut')
+            self.connectLoggerPort(self.abc, 'rpacp_dbgOut')
         if self.st != None:
             self.connectLoggerPort(self.st, 'zmp')
             self.connectLoggerPort(self.st, 'originRefZmp')
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index d2b0d08e3df..bdce63951f2 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -72,6 +72,25 @@ AutoBalancer::AutoBalancer(RTC::Manager* manager)
       m_actzmpIn("actzmpIn", m_actzmp),
       m_htheadIn("htheadIn", m_hthead),
 
+      m_htcom_dbgOut("htcom_dbgOut", m_htcom_dbg),
+      m_htrf_dbgOut("htrf_dbgOut", m_htrf_dbg),
+      m_htlf_dbgOut("htlf_dbgOut", m_htlf_dbg),
+      m_htrh_dbgOut("htrh_dbgOut", m_htrh_dbg),
+      m_htlh_dbgOut("htlh_dbgOut", m_htlh_dbg),
+      m_hthead_dbgOut("hthead_dbgOut", m_hthead_dbg),
+      m_htzmp_dbgOut("htzmp_dbgOut", m_htzmp_dbg),
+      m_htrfw_dbgOut("htrfw_dbgOut", m_htrfw_dbg),
+      m_htlfw_dbgOut("htlfw_dbgOut", m_htlfw_dbg),
+      m_rpcom_dbgOut("rpcom_dbgOut", m_rpcom_dbg),
+      m_rprf_dbgOut("rprf_dbgOut", m_rprf_dbg),
+      m_rplf_dbgOut("rplf_dbgOut", m_rplf_dbg),
+      m_rprh_dbgOut("rprh_dbgOut", m_rprh_dbg),
+      m_rplh_dbgOut("rplh_dbgOut", m_rplh_dbg),
+      m_rphead_dbgOut("rphead_dbgOut", m_rphead_dbg),
+      m_rpzmp_dbgOut("rpzmp_dbgOut", m_rpzmp_dbg),
+      m_rpdcp_dbgOut("rpdcp_dbgOut", m_rpdcp_dbg),
+      m_rpacp_dbgOut("rpacp_dbgOut", m_rpacp_dbg),
+
       m_qOut("q", m_qRef),
       m_zmpOut("zmpOut", m_zmp),
       m_basePosOut("basePosOut", m_basePos),
@@ -126,6 +145,25 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     addInPort("actzmpIn", m_actzmpIn);
     addInPort("htheadIn", m_htheadIn);
 
+    addOutPort("htcom_dbgOut", m_htcom_dbgOut);
+    addOutPort("htrf_dbgOut", m_htrf_dbgOut);
+    addOutPort("htlf_dbgOut", m_htlf_dbgOut);
+    addOutPort("htrh_dbgOut", m_htrh_dbgOut);
+    addOutPort("htlh_dbgOut", m_htlh_dbgOut);
+    addOutPort("hthead_dbgOut", m_hthead_dbgOut);
+    addOutPort("htzmp_dbgOut", m_htzmp_dbgOut);
+    addOutPort("htrfw_dbgOut", m_htrfw_dbgOut);
+    addOutPort("htlfw_dbgOut", m_htlfw_dbgOut);
+    addOutPort("rpcom_dbgOut", m_rpcom_dbgOut);
+    addOutPort("rprf_dbgOut", m_rprf_dbgOut);
+    addOutPort("rplf_dbgOut", m_rplf_dbgOut);
+    addOutPort("rprh_dbgOut", m_rprh_dbgOut);
+    addOutPort("rplh_dbgOut", m_rplh_dbgOut);
+    addOutPort("rphead_dbgOut", m_rphead_dbgOut);
+    addOutPort("rpzmp_dbgOut", m_rpzmp_dbgOut);
+    addOutPort("rpdcp_dbgOut", m_rpdcp_dbgOut);
+    addOutPort("rpacp_dbgOut", m_rpacp_dbgOut);
+
     // Set OutPort buffer
     addOutPort("q", m_qOut);
     addOutPort("zmpOut", m_zmpOut);
@@ -512,6 +550,8 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
     hsp->baselinkpose.p = m_robot->rootLink()->p;
     hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
+    if(ikp.count("rarm"))ikp["rarm"].is_active = false;
+    if(ikp.count("larm"))ikp["larm"].is_active = false;
     m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
     m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
 
@@ -656,6 +696,65 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
         m_limbCOPOffsetOut[i]->write();
     }
 
+    //ishiguro dbg plot
+    m_htcom.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("com"),m_htcom_dbg.data);
+    m_htcom_dbgOut.write();
+    m_htrf.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("rf"),m_htrf_dbg.data);
+    m_htrf_dbgOut.write();
+    m_htlf.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("lf"),m_htlf_dbg.data);
+    m_htlf_dbgOut.write();
+    m_htrh.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("rh"),m_htrh_dbg.data);
+    m_htrh_dbgOut.write();
+    m_htlh.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("lh"),m_htlh_dbg.data);
+    m_htlh_dbgOut.write();
+    m_hthead.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("head"),m_hthead_dbg.data);
+    m_hthead_dbgOut.write();
+    m_htzmp.tm = m_qRef.tm;
+    HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.getP("zmp").p,m_rpzmp_dbg.data);
+    m_htzmp_dbgOut.write();
+    m_htrfw.tm = m_qRef.tm;
+    m_htrfw_dbg.data.length(6);
+    HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.getw("rfw"),m_htrfw_dbg.data);
+    m_htrfw_dbgOut.write();
+    m_htlfw.tm = m_qRef.tm;
+    m_htlfw_dbg.data.length(6);
+    HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.getw("lfw"),m_htlfw_dbg.data);
+    m_htlfw_dbgOut.write();
+    m_rpcom_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("com"),m_rpcom_dbg.data);
+    m_rpcom_dbgOut.write();
+    m_rprf_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("rf"),m_rprf_dbg.data);
+    m_rprf_dbgOut.write();
+    m_rplf_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("lf"),m_rplf_dbg.data);
+    m_rplf_dbgOut.write();
+    m_rprh_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("rh"),m_rprh_dbg.data);
+    m_rprh_dbgOut.write();
+    m_rplh_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("lh"),m_rplh_dbg.data);
+    m_rplh_dbgOut.write();
+    m_rphead_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("head"),m_rphead_dbg.data);
+    m_rphead_dbgOut.write();
+    m_rpzmp_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.getP("zmp").p,m_rpzmp_dbg.data);
+    m_rpzmp_dbgOut.write();
+    m_rpdcp_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::Vector3ToPoint3D(hsp->cp_dec,m_rpdcp_dbg.data);
+    m_rpdcp_dbgOut.write();
+    m_rpacp_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::Vector3ToPoint3D(hsp->cp_acc,m_rpacp_dbg.data);
+    m_rpacp_dbgOut.write();
+
+
     return RTC::RTC_OK;
 }
 
@@ -1326,6 +1425,9 @@ bool AutoBalancer::setWholeBodyMasterSlaveParam(const OpenHRP::AutoBalancerServi
   hsp->WBMSparam.set_com_height_fix_val = i_param.set_com_height_fix_val;
   hsp->WBMSparam.foot_vertical_vel_limit_coeff = i_param.foot_vertical_vel_limit_coeff;
   hsp->WBMSparam.human_com_height = i_param.human_com_height;
+  hsp->WBMSparam.swing_foot_height_offset = i_param.swing_foot_height_offset;
+  hsp->WBMSparam.swing_foot_max_height = i_param.swing_foot_max_height;
+  hsp->WBMSparam.auto_swing_foot_landing_threshold = i_param.auto_swing_foot_landing_threshold;
   return true;
 }
 bool AutoBalancer::getWholeBodyMasterSlaveParam(OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam& i_param){
@@ -1336,6 +1438,9 @@ bool AutoBalancer::getWholeBodyMasterSlaveParam(OpenHRP::AutoBalancerService::Wh
   i_param.human_com_height = hsp->WBMSparam.human_com_height;
   i_param.use_hands = (hsp->WBMSparam.use_rh || hsp->WBMSparam.use_lh);
   i_param.use_head = hsp->WBMSparam.use_head;
+  i_param.swing_foot_height_offset = hsp->WBMSparam.swing_foot_height_offset;
+  i_param.swing_foot_max_height = hsp->WBMSparam.swing_foot_max_height;
+  i_param.auto_swing_foot_landing_threshold = hsp->WBMSparam.auto_swing_foot_landing_threshold;
   return true;
 }
 
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index b9b47f4ecc2..c090ad66be6 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -161,6 +161,43 @@ class AutoBalancer
   InPort<TimedDoubleSeq> m_htrfwIn;
   TimedDoubleSeq m_htlfw;
   InPort<TimedDoubleSeq> m_htlfwIn;
+  //ishiguro dbg
+  TimedPose3D m_htcom_dbg;
+  OutPort<TimedPose3D> m_htcom_dbgOut;
+  TimedPose3D m_htrf_dbg;
+  OutPort<TimedPose3D> m_htrf_dbgOut;
+  TimedPose3D m_htlf_dbg;
+  OutPort<TimedPose3D> m_htlf_dbgOut;
+  TimedPose3D m_htrh_dbg;
+  OutPort<TimedPose3D> m_htrh_dbgOut;
+  TimedPose3D m_htlh_dbg;
+  OutPort<TimedPose3D> m_htlh_dbgOut;
+  TimedPose3D m_hthead_dbg;
+  OutPort<TimedPose3D> m_hthead_dbgOut;
+  TimedPoint3D m_htzmp_dbg;
+  OutPort<TimedPoint3D> m_htzmp_dbgOut;
+  TimedDoubleSeq m_htrfw_dbg;
+  OutPort<TimedDoubleSeq> m_htrfw_dbgOut;
+  TimedDoubleSeq m_htlfw_dbg;
+  OutPort<TimedDoubleSeq> m_htlfw_dbgOut;
+  TimedPose3D m_rpcom_dbg;
+  OutPort<TimedPose3D> m_rpcom_dbgOut;
+  TimedPose3D m_rprf_dbg;
+  OutPort<TimedPose3D> m_rprf_dbgOut;
+  TimedPose3D m_rplf_dbg;
+  OutPort<TimedPose3D> m_rplf_dbgOut;
+  TimedPose3D m_rprh_dbg;
+  OutPort<TimedPose3D> m_rprh_dbgOut;
+  TimedPose3D m_rplh_dbg;
+  OutPort<TimedPose3D> m_rplh_dbgOut;
+  TimedPose3D m_rphead_dbg;
+  OutPort<TimedPose3D> m_rphead_dbgOut;
+  TimedPoint3D m_rpzmp_dbg;
+  OutPort<TimedPoint3D> m_rpzmp_dbgOut;
+  TimedPoint3D m_rpdcp_dbg;
+  OutPort<TimedPoint3D> m_rpdcp_dbgOut;
+  TimedPoint3D m_rpacp_dbg;
+  OutPort<TimedPoint3D> m_rpacp_dbgOut;
   
   // </rtc-template>
 
diff --git a/rtc/AutoBalancer/HumanMasterSlave.cpp b/rtc/AutoBalancer/HumanMasterSlave.cpp
index b5d2141a560..fbb05e329ae 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.cpp
+++ b/rtc/AutoBalancer/HumanMasterSlave.cpp
@@ -112,11 +112,12 @@ void HumanSynchronizer::applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vect
   com_oldold = com_old;
   com_old = comin;
 }
-void HumanSynchronizer::applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out){
+void HumanSynchronizer::applyVelLimit(const HumanPose& in, HumanPose& out_old, HumanPose& out){
   std::string ns[5] = {"com","rf","lf","rh","lh"};
   for(int i=0;i<5;i++){
-    hrp::Vector3 diff = in.getP(ns[i]).p  - in_old.getP(ns[i]).p;
+    hrp::Vector3 diff = in.getP(ns[i]).p  - out_old.getP(ns[i]).p;
     for(int j=0;j<3;j++)LIMIT_MINMAX( diff(j), -MAXVEL*DT, MAXVEL*DT);
-    out.getP(ns[i]).p = in_old.getP(ns[i]).p + diff;
+    out.getP(ns[i]).p = out_old.getP(ns[i]).p + diff;
   }
+  out_old = out;
 }
diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index dd2717c6694..0707265c0c8 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -185,14 +185,14 @@ class HumanSynchronizer{
     double CNT_F_TH;
     hrp::Vector3 init_hp_calibcom;
     double MAXVEL,MAXACC;
-    HumanPose rp_ref_out_old,hp_swap_checked;
-//    int countdown_num;
+    HumanPose rp_ref_out_old, hp_swap_checked, rp_old_ref_Vel;
     bool HumanSyncOn;
     struct timeval t_calc_start, t_calc_end;
     double h2r_ratio, tgt_h2r_ratio;
     double HZ,DT,G;
     hrp::Vector3 com_old,com_oldold,comacc;
     hrp::Vector3 comacc_ref;
+    hrp::Vector3 com_CP_ref_old;
     std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rot_filters;
     BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters;
     BiquadIIRFilterVec cam_rpy_filter;
@@ -209,6 +209,7 @@ class HumanSynchronizer{
 //    HumanPose rp_wld_initpos;//IKでのリンク原点の位置(足首，手首など)
     bool use_x,use_y,use_z;
     HumanPose hp_wld_raw;
+    HumanPose hp_plot;
     HumanPose rp_ref_out;
     FILE *sr_log,*cz_log;
     hrp::Vector3 init_wld_rp_rfeepos,init_wld_rp_lfeepos;
@@ -221,6 +222,7 @@ class HumanSynchronizer{
     hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos, pre_cont_rfrot,pre_cont_lfrot;
     std::vector<hrp::Vector3> rf_vert,lf_vert;
     double countdown_sec;
+    hrp::Vector3 cp_dec,cp_acc;
 
     interpolator *init_zmp_com_offset_interpolator;
     double zmp_com_offset_ip_ratio;
@@ -231,6 +233,9 @@ class HumanSynchronizer{
       double human_com_height;
       bool use_rh,use_lh;
       bool use_head;
+      double swing_foot_height_offset;
+      double swing_foot_max_height;
+      double auto_swing_foot_landing_threshold;
     };
     struct WBMSparameters WBMSparam;
 
@@ -252,12 +257,11 @@ class HumanSynchronizer{
       tgt_FUP_TIME = FUP_TIME = 0.4;
       FUP_HIGHT = 0.05;
       CNT_F_TH = 20.0;
-      MAXVEL = 2.0;
+      MAXVEL = 0.4;
       HumanSyncOn = false;
       ht_first_call = true;
       startCountdownForHumanSync = false;
       countdown_sec = 5.0;
-//      countdown_num = (int)(countdown_sec*HZ);
       loop = 0;
 
       //////////  hrp::Vector は初期化必要  ///////////
@@ -270,13 +274,13 @@ class HumanSynchronizer{
 
       com_vel_old = hrp::Vector3::Zero();
       cam_pos_filtered = cam_rpy_filtered = hrp::Vector3::Zero();
+      cp_dec = cp_acc = hrp::Vector3::Zero();
 
       tgt_pos_filters.resize(5);
       tgt_rot_filters.resize(5);
-//      for(int i=0;i<iir_v_filters.size();i++)iir_v_filters[i].setParameter(0.5,HZ);//四肢拘束点用
       for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(0.6,HZ);//四肢拘束点用(position)
       for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0,HZ);//四肢拘束点用(Rotation)
-      tgt_pos_filters[0].setParameter(0.4,HZ);//重心pos用
+      tgt_pos_filters[0].setParameter(0.6,HZ);//重心pos用
       tgt_rot_filters[0].setParameter(0.4,HZ);//重心rot用
       tgt_pos_filters[1].setParameter(hrp::Vector3(0.6,0.6,1.0),HZ);//右足pos用
       tgt_pos_filters[2].setParameter(hrp::Vector3(0.6,0.6,1.0),HZ);//左足pos用
@@ -286,8 +290,6 @@ class HumanSynchronizer{
 
       rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
       lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
-      //      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.03,  0.02);
-      //lf_safe_region = hrp::Vector4(0.02, -0.01, -0.02, -0.03);
       rf_vert.push_back(hrp::Vector3(0.13,-0.08,0));
       rf_vert.push_back(hrp::Vector3(-0.10,-0.08,0));
       rf_vert.push_back(hrp::Vector3(-0.10,0.06,0));
@@ -297,13 +299,15 @@ class HumanSynchronizer{
       lf_vert.push_back(hrp::Vector3(-0.10,0.08,0));
       lf_vert.push_back(hrp::Vector3(0.13,0.08,0));
 
-//      use_rh = use_lh = false;
       WBMSparam.use_rh = WBMSparam.use_lh = true;
       WBMSparam.use_head = true;
       WBMSparam.set_com_height_fix = true;
-      WBMSparam.set_com_height_fix_val = 0.03;
-      WBMSparam.foot_vertical_vel_limit_coeff = 2.0;
+      WBMSparam.set_com_height_fix_val = 0.02;
+      WBMSparam.foot_vertical_vel_limit_coeff = 4.0;
       WBMSparam.human_com_height = 1.00;
+      WBMSparam.swing_foot_height_offset = 0.01;
+      WBMSparam.swing_foot_max_height = 0.10;
+      WBMSparam.auto_swing_foot_landing_threshold = 0.02;
 
       rp_ref_out_old.clear();
       rp_ref_out.clear();
@@ -353,6 +357,7 @@ class HumanSynchronizer{
 //      calcWorldZMP                        ((hp_rel_raw.getP("rf").p+init_wld_hp_rfpos), (hp_rel_raw.getP("lf").p+init_wld_hp_lfpos), hp_rel_raw.getw("rfw"), hp_rel_raw.getw("lfw"), hp_rel_raw.getP("zmp").p);//足の位置はworldにしないと・・・
       autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);
       convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
+      hp_plot = rp_ref_out;
       if(loop==0)rp_ref_out_old = rp_ref_out;
       if(isHumanSyncOn() && WBMSparam.set_com_height_fix)rp_ref_out.getP("com").p(Z) = rp_ref_out.getP("com").p_offs(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
       judgeFootLandOnCommand              (hp_wld_raw.getw("rfw"), hp_wld_raw.getw("lfw"), go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
@@ -361,29 +366,36 @@ class HumanSynchronizer{
       lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
       if(go_rf_landing)rp_ref_out.getP("rf").p(Z) = rp_ref_out.getP("rf").p_offs(Z);
       if(go_lf_landing)rp_ref_out.getP("lf").p(Z) = rp_ref_out.getP("lf").p_offs(Z);
+      if(!go_rf_landing)rp_ref_out.getP("rf").p(Z) += WBMSparam.swing_foot_height_offset;
+      if(!go_lf_landing)rp_ref_out.getP("lf").p(Z) += WBMSparam.swing_foot_height_offset;
 
-      if(!go_rf_landing)rp_ref_out.getP("rf").p(Z) += 0.03;
-      if(!go_lf_landing)rp_ref_out.getP("lf").p(Z) += 0.03;
+      if(DEBUG)fprintf(cz_log,"com_ref: %f %f ", rp_ref_out.getP("com").p(X), rp_ref_out.getP("com").p(Y));
 
+      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com").p);
+      setFootRotHorizontalIfGoLanding     (rp_ref_out_old, rp_ref_out);
 
+//      applyLPFilter                       (rp_ref_out);
+//      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com"));//もう一回？
+//      applyCOMStateLimitByCapturePoint    (rp_ref_out.getP("com").p, rp_ref_out_old.getP("com").p, com_vel_old, rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com").p);
+//      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com"));//もう一回？
 
-      if(DEBUG)fprintf(cz_log,"com_ref: %f %f ", rp_ref_out.getP("com").p(X), rp_ref_out.getP("com").p(Y));
+      applyVelLimit                       (rp_ref_out, rp_old_ref_Vel, rp_ref_out);
 
-      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com"));
 
-      setFootRotHorizontalIfGoLanding     (rp_ref_out_old, rp_ref_out);
+      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, com_CP_ref_old);//これやらないと1ステップ前のCOM位置はもうはみ出てるかもしれないから
+
+      applyCOMStateLimitByCapturePoint    (rp_ref_out.getP("com").p, rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, com_CP_ref_old, rp_ref_out.getP("com").p);
+      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com").p);
       applyLPFilter                       (rp_ref_out);
-      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com"));//もう一回？
-      applyCOMStateLimitByCapturePoint    (rp_ref_out.getP("com").p, rp_ref_out_old.getP("com").p, com_vel_old, rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com").p);
-      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com"));//もう一回？
 
       cam_rpy_filtered = cam_rpy_filter.passFilter(head_cam_pose.rpy);
       r_zmp_raw = rp_ref_out.getP("zmp").p;
       applyZMPCalcFromCOM                 (rp_ref_out.getP("com").p,rp_ref_out.getP("zmp").p);
 
       if(DEBUG)fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.getP("zmp").p(X),rp_ref_out.getP("zmp").p(Y));
+      if(DEBUG)fprintf(cz_log,"com_ans2: %f %f ",rp_ref_out.getP("com").p(X),rp_ref_out.getP("com").p(Y));
 
-      applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
+//      applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
       modifyFootRotAndXYForContact        (rp_ref_out.getP("rf"), rp_ref_out.getP("lf"));
 
@@ -400,13 +412,22 @@ class HumanSynchronizer{
       init_hp_calibcom(X) = r_zmp_raw(X);
       init_hp_calibcom(Y) = r_zmp_raw(Y);
     }
+    static void Vector3ToVector2(const hrp::Vector3& in, hrp::Vector2& out){ out(X) = in(X); out(Y) = in(Y);}
+    static void Vector2ToVector3(const hrp::Vector2& in, hrp::Vector3& out){ out(X) = in(X); out(Y) = in(Y);}
     static void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out(X) = in.x; out(Y) = in.y; out(Z) = in.z;}
     static void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out(X) = in.r; out(Y) = in.p; out(Z) = in.y;}
-    static void Pose3DToHRPPose3D(const RTC::Pose3D& in, HRPPose3D& out_pose){ Point3DToVector3(in.position,out_pose.p); Oriantation3DToVector3(in.orientation,out_pose.rpy); }
+    static void Vector3ToPoint3D(const hrp::Vector3& in, RTC::Point3D& out){ out.x = in(X); out.y = in(Y); out.z = in(Z);}
+    static void Vector3ToOriantation3D(const hrp::Vector3& in, RTC::Orientation3D& out){ out.r = in(X); out.p = in(Y); out.y = in(Z);}
+    static void Pose3DToHRPPose3D(const RTC::Pose3D& in, HRPPose3D& out){ Point3DToVector3(in.position,out.p); Oriantation3DToVector3(in.orientation,out.rpy); }
+    static void HRPPose3DToPose3D(const HRPPose3D& in, RTC::Pose3D& out){ Vector3ToPoint3D(in.p,out.position); Vector3ToOriantation3D(in.rpy,out.orientation); }
     static void DoubleSeqToWrench6(const RTC::TimedDoubleSeq::_data_seq& in, Wrench6& out){
       if(in.length() == 6){out.f(X)=in[0]; out.f(Y)=in[1]; out.f(Z)=in[2]; out.t(X)=in[3]; out.t(Y)=in[4]; out.t(Z)=in[5];
       }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; out.clear(); }
     }
+    static void Wrench6ToDoubleSeq(const Wrench6& in, RTC::TimedDoubleSeq::_data_seq& out){
+      if(out.length() == 6){out[0]=in.f(X); out[1]=in.f(Y); out[2]=in.f(Z); out[3]=in.t(X); out[4]=in.t(Y); out[5]=in.t(Z);
+      }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; }
+    }
     static double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(X)*b(Y)-a(Y)*b(X); }
 
   private:
@@ -447,7 +468,7 @@ class HumanSynchronizer{
     void applyLPFilter(HumanPose& tgt){
       std::string ns[5] = {"com","rf","lf","rh","lh"};
       if(loop==0){
-        cout<<"init"<<tgt.getP("rf").p.transpose()<<endl;
+        cout<<"init LPF"<<endl;
         for(int i=0;i<tgt_pos_filters.size();i++){ tgt_pos_filters[i].init(tgt.getP(ns[i]).p); tgt_pos_filters[i].passFilter(tgt.getP(ns[i]).p);}
         for(int i=0;i<tgt_rot_filters.size();i++){ tgt_rot_filters[i].init(tgt.getP(ns[i]).rpy); tgt_pos_filters[i].passFilter(tgt.getP(ns[i]).rpy);}
       }
@@ -473,38 +494,24 @@ class HumanSynchronizer{
 //      if(!rfloc_ans && !lfloc_ans){rfloc_ans = lfloc_ans = true;}//両足ジャンプは禁止
     }
     void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
-//      if(rfcs_ans && !lfcs_ans){
-        if(tgt.getP("zmp").p(Y) > tgt.getP("rf").p(Y) + 0.03){
-          lfcs_ans = true;
-//          if(loop%100==0)cout<<"LF Auto lock"<<endl;
-        }
-//      }
-//      if(!rfcs_ans && lfcs_ans){
-        if(tgt.getP("zmp").p(Y) < tgt.getP("lf").p(Y) - 0.03){
-          rfcs_ans = true;
-//          if(loop%100==0)cout<<"RF Auto lock"<<endl;
-        }
-//      }
+      hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);
+      hrp::Vector2 rf2zmp = hrp::Vector2(tgt.getP("zmp").p(X),tgt.getP("zmp").p(Y)) - hrp::Vector2(tgt.getP("rf").p(X),tgt.getP("rf").p(Y));
+      hrp::Vector2 lf2zmp = hrp::Vector2(tgt.getP("zmp").p(X),tgt.getP("zmp").p(Y)) - hrp::Vector2(tgt.getP("lf").p(X),tgt.getP("lf").p(Y));
+      if( inside_vec_rf.dot(rf2zmp) / inside_vec_rf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ lfcs_ans = true; }
+      if( inside_vec_lf.dot(lf2zmp) / inside_vec_lf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ rfcs_ans = true; }
     }
     void setFootRotHorizontalIfGoLanding(const HumanPose& rp_ref_out_old, HumanPose& rp_ref_out){
       if(go_rf_landing){ rp_ref_out.getP("rf").rpy(X) = rp_ref_out.getP("rf").rpy(Y) = 0; }
       if(go_lf_landing){ rp_ref_out.getP("lf").rpy(X) = rp_ref_out.getP("lf").rpy(Y) = 0; }
     }
     void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
-//      if(is_rf_contact&&is_lf_contact)FUP_TIME = tgt_FUP_TIME;//両足接地しているタイミングでのみ更新
-//      double RFDOWN_TIME = fabs(tgt.getP("rf").p(Y) - tgt.getP("com").p(Y))*2 + FUP_TIME;//下ろす速度は遊脚の重心からの距離に比例したペナルティ
-//      double LFDOWN_TIME = fabs(tgt.getP("lf").p(Y) - tgt.getP("com").p(Y))*2 + FUP_TIME;
-//      LIMIT_MINMAX( RFDOWN_TIME, FUP_TIME, 0.8);
-//      LIMIT_MINMAX( LFDOWN_TIME, FUP_TIME, 0.8);
-//      calcFootUpCurveAndJudgeFootContact(rf_goland_in, FUP_TIME, RFDOWN_TIME, cur_rfup_rad, tgt.getP("rf"), is_rf_contact);
-//      calcFootUpCurveAndJudgeFootContact(lf_goland_in, FUP_TIME, LFDOWN_TIME, cur_lfup_rad, tgt.getP("lf"), is_lf_contact);
       const double com2rf_dist = fabs((double)tgt.getP("rf").p(Y) - (double)tgt.getP("com").p(Y));
       const double com2lf_dist = fabs((double)tgt.getP("lf").p(Y) - (double)tgt.getP("com").p(Y));
       limitGroundContactVelocity(rf_goland_in, com2rf_dist, rp_ref_out_old.getP("rf"), tgt.getP("rf"), is_rf_contact);
       limitGroundContactVelocity(lf_goland_in, com2lf_dist, rp_ref_out_old.getP("lf"), tgt.getP("lf"), is_lf_contact);
     }
     void limitGroundContactVelocity(const bool& go_land_in, const double& com2foot_dist, const HRPPose3D& f_old_in, HRPPose3D& f_in_out, bool& is_f_contact_out){
-      double penalty = 1.0 + (0.2 - com2foot_dist) * 5.0;//1.0 ~ 2.0
+      double penalty = 1.0 + (0.2 - com2foot_dist) * 5.0;//1.0 ~ 2.0 本当はCPに比例すべき？
       LIMIT_MINMAX( penalty, 1.0, 2.0);
       LIMIT_MIN( f_in_out.p(Z), f_in_out.p_offs(Z));
       double input_vel = (f_in_out.p(Z) - f_old_in.p(Z)) / DT;
@@ -595,74 +602,73 @@ class HumanSynchronizer{
         LIMIT_MINMAX( tgt.getP("rh").p(i), (baselinkpose.p+init_base2rh+hand_llimit)(i), (baselinkpose.p+init_base2rh+hand_ulimit)(i) );
         LIMIT_MINMAX( tgt.getP("lh").p(i), (baselinkpose.p+init_base2lh+hand_llimit)(i), (baselinkpose.p+init_base2lh+hand_ulimit)(i) );
       }
-      LIMIT_MINMAX( tgt.getP("rf").p(Z), tgt.getP("rf").p_offs(Z), tgt.getP("rf").p_offs(Z)+0.05);
-      LIMIT_MINMAX( tgt.getP("lf").p(Z), tgt.getP("lf").p_offs(Z), tgt.getP("lf").p_offs(Z)+0.05);
+      LIMIT_MINMAX( tgt.getP("rf").p(Z), tgt.getP("rf").p_offs(Z), tgt.getP("rf").p_offs(Z)+WBMSparam.swing_foot_max_height);
+      LIMIT_MINMAX( tgt.getP("lf").p(Z), tgt.getP("lf").p_offs(Z), tgt.getP("lf").p_offs(Z)+WBMSparam.swing_foot_max_height);
 
       std::string ns[5] = {"com","rf","lf","rh","lh"};
       for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( tgt.getP("com").rpy(i), rc.ee_rot_limit["com"][MIN](i), rc.ee_rot_limit["com"][MAX](i) ); }
+      LIMIT_MINMAX( tgt.getP("com").p(Z), tgt.getP("com").p_offs(Z) - 0.15, tgt.getP("com").p_offs(Z) + 0.03 );//COM高さ方向の制限
       for(int i=1;i<5;i++){
         for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( tgt.getP(ns[i]).rpy(j), rc.ee_rot_limit[ns[i]][MIN](j) + tgt.getP("com").rpy(j), rc.ee_rot_limit[ns[i]][MAX](j) + tgt.getP("com").rpy(j) ); }
       }
       if(!WBMSparam.use_head)head_cam_pose.rpy = hrp::Vector3::Zero();
-      LIMIT_MINMAX( head_cam_pose.rpy(Y), -20*M_PI/180, 20*M_PI/180 );
-      LIMIT_MINMAX( head_cam_pose.rpy(Z), -20*M_PI/180, 20*M_PI/180 );
+      if(!isHumanSyncOn())head_cam_pose.rpy = hrp::Vector3::Zero();
+      LIMIT_MINMAX( head_cam_pose.rpy(Y), -10*M_PI/180, 20*M_PI/180 );
+      LIMIT_MINMAX( head_cam_pose.rpy(Z), -40*M_PI/180, 40*M_PI/180 );
     }
-    bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, HRPPose3D& comin_abs){//boost::geometryがUbuntu12だとないから・・・
+    bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
       std::vector<hrp::Vector2> hull;
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull);
-      hrp::Vector2 cog(comin_abs.p(X),comin_abs.p(Y));
+      hrp::Vector2 cog(comin_abs(X),comin_abs(Y));
       if(!isPointInHullOpenCV(cog,hull)){ calcNearestPointOnHull(cog,hull,cog); }//外に出たら最近傍点に頭打ち
-      comin_abs.p(X) = cog(X);
-      comin_abs.p(Y) = cog(Y);
-      LIMIT_MINMAX( comin_abs.p(Z), comin_abs.p_offs(Z) - 0.15, comin_abs.p_offs(Z) + 0.03 );//COM高さ方向の制限
+      comin_abs(X) = cog(X);
+      comin_abs(Y) = cog(Y);
       return true;
     }
-    bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& com_old, const hrp::Vector3& com_vel_old, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans){
+    bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans_old, hrp::Vector3& com_ans){
       com_ans = com_in;
-      hrp::Vector3 com_vel = (com_in - com_old)/DT;
-//      hrp::Vector3 com_acc = (com_vel - com_vel_old)/DT;//命名規則
-//      hrp::Vector3 com_vel_zmpOk, com_vel_zmpOk_cpOk;
-//      double accel[4] = {0.05, -0.03, 0.02, -0.02};
-//      double decel[4] = {0.04, -0.02, 0.01, -0.01};
-//      comacc_ref = calcacc_v_filters.passFilter(com_acc);
-//      const double H = rp_ref_out.getP("com").p(Z);
-      std::vector<hrp::Vector2> hull;
-      hrp::Vector4 marginDelta(+0.001,-0.001,0.001,-0.001);
-      createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region+marginDelta, lf_safe_region+marginDelta, hull);
+      if (loop==0)com_ans_old = com_in;
+      hrp::Vector3 com_vel = (com_in - com_ans_old)/DT;
+      std::vector<hrp::Vector2> hull_dcp,hull_acp;
+//      hrp::Vector4 marginDelta_for_dcp(+0.001,-0.001,0.001,-0.001);
+//      hrp::Vector4 marginDelta_for_acp(+0.002,-0.002,0.002,-0.002);
+      hrp::Vector4 marginDelta_for_dcp(+0.001,-0.001,0.001,-0.001);
+      hrp::Vector4 marginDelta_for_acp(+0.005,-0.005,0.005,-0.005);
+      createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region+marginDelta_for_dcp, lf_safe_region+marginDelta_for_dcp, hull_dcp);
+      createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region+marginDelta_for_acp, lf_safe_region+marginDelta_for_acp, hull_acp);
       hrp::Vector2 com_vel_ans_2d;
-      regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_old(X),com_old(Y)), hrp::Vector2(com_vel(X),com_vel(Y)), hull, com_vel_ans_2d);
-//      regulateCOMVelocityByCapturePointXY( hrp::Vector2(com_old(X),com_old(Y)), hrp::Vector2(com_vel(X),com_vel(Y)), hull, com_vel_ans_2d);//XY独立に計算する手法のほうがエッジで引っかからない・・・
-      com_ans(X) = com_old(X) + com_vel_ans_2d(X) * DT;
-      com_ans(Y) = com_old(Y) + com_vel_ans_2d(Y) * DT;
+      regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_ans_old(X),com_ans_old(Y)), hrp::Vector2(com_vel(X),com_vel(Y)), hull_dcp, hull_acp, com_vel_ans_2d);
+      com_ans(X) = com_ans_old(X) + com_vel_ans_2d(X) * DT;
+      com_ans(Y) = com_ans_old(Y) + com_vel_ans_2d(Y) * DT;
+      com_ans_old = com_ans;
+      Vector2ToVector3(hrp::Vector2(com_ans(X),com_ans(Y)) + com_vel_ans_2d * sqrt( H_cur / G ), cp_dec);
+      Vector2ToVector3(hrp::Vector2(com_ans(X),com_ans(Y)) - com_vel_ans_2d * sqrt( H_cur / G ), cp_acc);
       if(DEBUG){
         fprintf(cz_log,"com_ans: %f %f ",com_ans(X),com_ans(Y));
-        hrp::Vector2 cp_d_plot = hrp::Vector2(com_ans(X),com_ans(Y)) + com_vel_ans_2d * sqrt( H_cur / G );
-        hrp::Vector2 cp_a_plot = hrp::Vector2(com_ans(X),com_ans(Y)) - com_vel_ans_2d * sqrt( H_cur / G );
-        fprintf(cz_log,"com_ans_cp_d: %f %f ",cp_d_plot(X),cp_d_plot(Y));
-        fprintf(cz_log,"com_ans_cp_a: %f %f ",cp_a_plot(X),cp_a_plot(Y));
-        for(int i=0;i<hull.size();i++)fprintf(sr_log,"hull_vert%d: %f %f\n",i,hull[i](X),hull[i](Y));
-        fprintf(sr_log,"hull_vert%d: %f %f\n",(int)hull.size(),hull[0](X),hull[0](Y));//閉じる
+        fprintf(cz_log,"com_ans_cp_d: %f %f ",cp_dec(X),cp_dec(Y));
+        fprintf(cz_log,"com_ans_cp_a: %f %f ",cp_acc(X),cp_acc(Y));
+        for(int i=0;i<hull_dcp.size();i++)fprintf(sr_log,"hull_vert%d: %f %f\n",i,hull_dcp[i](X),hull_dcp[i](Y));
+        fprintf(sr_log,"hull_vert%d: %f %f\n",(int)hull_dcp.size(),hull_dcp[0](X),hull_dcp[0](Y));//閉じる
         fprintf(sr_log,"\n");
       }
       return true;
     }
-    void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
+    void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull_d, const std::vector<hrp::Vector2>& hull_a, hrp::Vector2& com_vel_ans){
       hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
       com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
-//      const double H = rp_ref_out.getP("com").p(Z);
       //減速CP条件
-      hrp::Vector2 cp_dec = com_pos + com_vel * sqrt( H_cur / G );
+      hrp::Vector2 cp_dec_tmp = com_pos + com_vel * sqrt( H_cur / G );
       hrp::Vector2 cp_dec_ragulated;
-      if(!isPointInHullOpenCV(cp_dec,hull)){
-        calcCrossPointOnHull(com_pos, cp_dec, hull, cp_dec_ragulated);
+      if(!isPointInHullOpenCV(cp_dec_tmp,hull_d)){
+        calcCrossPointOnHull(com_pos, cp_dec_tmp, hull_d, cp_dec_ragulated);
 //        calcNearestPointOnHull(cp, hull, cp_ragulated);
         com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
       }
       //加速CP条件
-      hrp::Vector2 cp_acc = com_pos - com_vel * sqrt( H_cur / G );
+      hrp::Vector2 cp_acc_tmp = com_pos - com_vel * sqrt( H_cur / G );
       hrp::Vector2 cp_acc_ragulated;
-      if(!isPointInHullOpenCV(cp_acc,hull)){
-         calcCrossPointOnHull(com_pos, cp_acc, hull, cp_acc_ragulated);
+      if(!isPointInHullOpenCV(cp_acc_tmp,hull_a)){
+         calcCrossPointOnHull(com_pos, cp_acc_tmp, hull_a, cp_acc_ragulated);
          com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
       }
       if(com_vel_decel_ok.norm() < com_vel_accel_ok.norm()){
@@ -671,27 +677,6 @@ class HumanSynchronizer{
         com_vel_ans = com_vel_accel_ok;
       }
     }
-//    void regulateCOMVelocityByCapturePointXY(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull, hrp::Vector2& com_vel_ans){
-//      com_vel_ans = com_vel;
-//      const double H = rp_wld_initpos.getP("com").p(Z);
-//      enum Direc{F,B,L,R} direc;
-//      hrp::Vector4 margin_from_4maxcp;
-//      calcXYMarginToHull(com_pos, hull, margin_from_4maxcp);
-//      hrp::Vector4 com_vel_decel_limit,com_vel_accel_limit;
-//      const double accel_cp_tolerance = 0.01;//境界上でcom_vel_accel_limitが0になって抜け出せなくなるのを防ぐため
-//      com_vel_decel_limit(F) = margin_from_4maxcp(F) / sqrt( H / G );
-//      com_vel_decel_limit(B) = margin_from_4maxcp(B) / sqrt( H / G );
-//      com_vel_decel_limit(L) = margin_from_4maxcp(L) / sqrt( H / G );
-//      com_vel_decel_limit(R) = margin_from_4maxcp(R) / sqrt( H / G );
-//      com_vel_accel_limit(F) = (-margin_from_4maxcp(B)+accel_cp_tolerance) / sqrt( H / G );
-//      com_vel_accel_limit(B) = (-margin_from_4maxcp(F)-accel_cp_tolerance) / sqrt( H / G );
-//      com_vel_accel_limit(L) = (-margin_from_4maxcp(R)+accel_cp_tolerance) / sqrt( H / G );
-//      com_vel_accel_limit(R) = (-margin_from_4maxcp(L)-accel_cp_tolerance) / sqrt( H / G );
-//      LIMIT_MINMAX( com_vel_ans(X), com_vel_decel_limit(B), com_vel_decel_limit(F) );
-//      LIMIT_MINMAX( com_vel_ans(Y), com_vel_decel_limit(R), com_vel_decel_limit(L) );
-//      LIMIT_MINMAX( com_vel_ans(X), com_vel_accel_limit(B), com_vel_accel_limit(F) );//加速時のCP
-//      LIMIT_MINMAX( com_vel_ans(Y), com_vel_accel_limit(R), com_vel_accel_limit(L) );
-//    }
     void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
       std::vector<hrp::Vector2> points;
       for(int i=0;i<2;i++){
@@ -723,7 +708,7 @@ class HumanSynchronizer{
     void makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans);
     bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull);
     void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout);
-    void applyVelLimit(const HumanPose& in, const HumanPose& in_old, HumanPose& out);
+    void applyVelLimit(const HumanPose& in, HumanPose& out_old, HumanPose& out);
     void applyCOMZMPXYZLock(HumanPose& tgt);
 };
 

From 7897d6c20a56af7847ad7b4a331eb03d4c9e8a9c Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 10 Apr 2017 21:58:00 +0900
Subject: [PATCH 065/217] add my invdyn implements

---
 rtc/AutoBalancer/AutoBalancer.cpp     | 180 ++++++++++++--------------
 rtc/AutoBalancer/AutoBalancer.h       |  18 +++
 rtc/AutoBalancer/HumanMasterSlave.cpp | 101 +++++++++++++++
 rtc/AutoBalancer/HumanMasterSlave.h   | 166 ++++++++++++++++++++++--
 4 files changed, 355 insertions(+), 110 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index bdce63951f2..2323b555814 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -91,6 +91,8 @@ AutoBalancer::AutoBalancer(RTC::Manager* manager)
       m_rpdcp_dbgOut("rpdcp_dbgOut", m_rpdcp_dbg),
       m_rpacp_dbgOut("rpacp_dbgOut", m_rpacp_dbg),
 
+      m_invdyn_dbgOut("invdyn_dbgOut", m_invdyn_dbg),
+
       m_qOut("q", m_qRef),
       m_zmpOut("zmpOut", m_zmp),
       m_basePosOut("basePosOut", m_basePos),
@@ -164,6 +166,8 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     addOutPort("rpdcp_dbgOut", m_rpdcp_dbgOut);
     addOutPort("rpacp_dbgOut", m_rpacp_dbgOut);
 
+    addOutPort("invdyn_dbgOut", m_invdyn_dbgOut);
+
     // Set OutPort buffer
     addOutPort("q", m_qOut);
     addOutPort("zmpOut", m_zmpOut);
@@ -572,6 +576,8 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
       }
       if (control_mode != MODE_IDLE ) {
         solveLimbIK();
+        hrp::Vector3 f,t;
+        solveWholeBodyID(m_robot,f,t);
         rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
       } else {
         rel_ref_zmp = input_zmp;
@@ -753,7 +759,10 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     m_rpacp_dbg.tm = m_qRef.tm;
     HumanSynchronizer::Vector3ToPoint3D(hsp->cp_acc,m_rpacp_dbg.data);
     m_rpacp_dbgOut.write();
-
+    m_invdyn_dbg.tm = m_qRef.tm;
+    m_invdyn_dbg.data.length(6);
+    HumanSynchronizer::Wrench6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
+    m_invdyn_dbgOut.write();
 
     return RTC::RTC_OK;
 }
@@ -1145,6 +1154,7 @@ bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param, const std::string& lim
   }
   return true;
 }
+
 void AutoBalancer::solveWholeBodyCOMIK(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const hrp::Vector3& head_ref){
   int com_ik_loop=0;
   const int COM_IK_MAX_LOOP = 5;
@@ -1204,19 +1214,6 @@ void AutoBalancer::solveLimbIK ()
   dif_cog(2) = m_robot->rootLink()->p(2) - target_root_p(2);
   m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog;
   m_robot->rootLink()->R = target_root_R;
-
-////////////////////  逆動力学補償テスト  /////////////////////
-//  hrp::Matrix33 rot;
-//  Eigen::Matrix3f AxisAngle;
-//  Eigen::Vector3f axis(1,0,0);
-//  static int loop_l = 0;
-//  AxisAngle=Eigen::AngleAxisf(0.2*sin(2*M_PI*loop_l*m_dt),axis);  //Z軸周りに90度反時計回りに回転
-//  m_robot->rootLink()->R << AxisAngle(0,0), AxisAngle(0,1), AxisAngle(0,2), AxisAngle(1,0), AxisAngle(1,1), AxisAngle(1,2), AxisAngle(2,0), AxisAngle(2,1), AxisAngle(2,2);
-//if(loop>4000)loop_l++;
-//  if(loop_l>250)loop_l=250;
-////////////////////  逆動力学補償テスト  /////////////////////
-
-
   //for HumanSynchronizer
   if(hsp->startCountdownForHumanSync){
     std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
@@ -1252,7 +1249,6 @@ void AutoBalancer::solveLimbIK ()
   hsp->update();//////HumanSynchronizerの主要処理
   if(loop%100==0)hsp->rp_ref_out.print();
 //  if(loop%100==0)cout<<"time[ms]:"<<hsp->getUpdateTime()*1000<<endl;
-
   // Fix for toe joint
   for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
       if (it->second.is_active && it->second.has_toe_joint && gg->get_use_toe_joint()) {
@@ -1265,7 +1261,6 @@ void AutoBalancer::solveLimbIK ()
       }
   }
   m_robot->calcForwardKinematics();
-
   // additional COM fitting IK for HumanSynchronizer
   if(hsp->isHumanSyncOn()){
     solveWholeBodyCOMIK( hsp->rp_ref_out.getP("com"), hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
@@ -1278,91 +1273,82 @@ void AutoBalancer::solveLimbIK ()
 	  }
   }
   if (gg_is_walking && !gg_solved) stopWalking ();
+}
 
 
-
-
-
-
-  ////////////////////  逆動力学補償テスト  /////////////////////
-//  //  for(int i=0;i<m_robot->numLinks();i++)cout<<"m_robot->link("<<i<<")->name: "<<m_robot->link(i)->name<<endl;
-//  static std::vector<double> q,q_old,dq,dq_old,ddq,ddq_f;
-//  static hrp::Vector3 base_acc(0,0,0),base_pos(0,0,0),base_pos_old(0,0,0),base_vel(0,0,0),base_vel_old(0,0,0);
-//
-//  static bool first = true;
-//  if(first){
-//    q.resize(m_robot->numJoints());
-//    q_old.resize(m_robot->numJoints());
-//    dq.resize(m_robot->numJoints());
-//    dq_old.resize(m_robot->numJoints());
-//    ddq.resize(m_robot->numJoints());
-//    ddq_f.resize(m_robot->numJoints(),0);
-//    for(int i=0;i<m_robot->numJoints();i++)q[i] = m_robot->joint(i)->q;
-//    q_old=q;
-//    for(int i=0;i<m_robot->numJoints();i++)dq[i] = (q[i] - q_old[i])/m_dt;
-//    dq_old=dq;
-//    first=false;
-//  }
-//
-//  for(int i=0;i<m_robot->numJoints();i++)q[i] = m_robot->joint(i)->q;
-//  for(int i=0;i<m_robot->numJoints();i++)dq[i] = (q[i] - q_old[i])/m_dt;
-//  for(int i=0;i<m_robot->numJoints();i++)ddq[i] = (dq[i] - dq_old[i])/m_dt;
-//  q_old = q;
-//  dq_old = dq;
-//  for(int i=0;i<m_robot->numJoints();i++)m_robot->joint(i)->dq = dq[i];
-//
-//  for(int i=0;i<m_robot->numJoints();i++)ddq_f[i] = 0.99*ddq_f[i] + 0.01*ddq[i];
-//  for(int i=0;i<m_robot->numJoints();i++)m_robot->joint(i)->ddq = ddq_f[i];
-//
-//base_pos = m_robot->rootLink()->p;
-//  base_vel = (base_pos - base_pos_old)/m_dt;
-//  base_acc = 0.99*base_acc + 0.01*(base_vel - base_vel_old)/m_dt;
-//  base_pos_old = base_pos;
-//  base_vel_old = base_vel;
-//
-////    cout<<"m_robot->link(\"RLEG_JOINT1\")->ddq: "<<m_robot->link("RLEG_JOINT1")->ddq<<endl;
-////    cout<<"m_robot->link(\"RLEG_JOINT1\")->dq: "<<m_robot->link("RLEG_JOINT1")->dq<<endl;
-////    cout<<"m_robot->link(\"RLEG_JOINT1\")->q: "<<m_robot->link("RLEG_JOINT1")->q<<endl;
-//
-//  m_robot->calcForwardKinematics(true,true);
-//
-//    static hrp::Vector3 fff,ttt;
-//    m_robot->calcInverseDynamics(m_robot->rootLink(),fff,ttt);
-//    hrp::Vector3 g(0,0,9.80665);
-////    m_robot->rootLink()->dvo = base_acc;
-//    cout<<"m_robot->rootLink()->dvo\n"<<m_robot->rootLink()->dvo<<endl;
-//    cout<<"m_robot->rootLink()->dw\n"<<m_robot->rootLink()->dw<<endl;
-//
-//    cout<<"calcInverseDynamics["<<loop<<"]\nfff\n"<<fff<<"\nttt\n"<<ttt<<endl;
-//    static hrp::Vector3 fff_f = hrp::Vector3::Zero(),ttt_f = hrp::Vector3::Zero();
-////    fff_f = 0.99*fff_f + 0.01*fff;
-////    ttt_f = 0.99*ttt_f + 0.01*ttt;
-//    //    fff_f = 0.95*fff_f + 0.05*fff;
-//    //    ttt_f = 0.95*ttt_f + 0.05*ttt;
-//        fff_f =fff;
-//        ttt_f =ttt;
-//
-//    hrp::Vector3 ppp,lll;
-//
-//    m_robot->calcTotalMomentum(ppp,lll);
-//    cout<<"calcTotalMomentum\nppp\n"<<ppp<<"\nlll\n"<<lll<<endl;
-//
-////    ref_zmp(0) += ( - 0*ppp(0)*m_robot->calcCM()(2) - lll(1))/(m_robot->totalMass()*9.8);
-////    ref_zmp(1) += ( - 0*ppp(1)*m_robot->calcCM()(2) - lll(0))/(m_robot->totalMass()*9.8);
-////      ref_zmp(0) += ( - fff_f(0)*m_robot->calcCM()(2)*0 + ttt_f(1))/(m_robot->totalMass()*9.8)*1;
-////      ref_zmp(1) += ( - fff_f(1)*m_robot->calcCM()(2)*0 - ttt_f(0))/(m_robot->totalMass()*9.8)*1;
-//
-////    if(hsp->sr_log!=NULL)fprintf(hsp->sr_log,"%f %f %f %f %f\n",m_robot->link("WAIST")->p(0),fff(0),ttt(0),ppp(0),lll(0));
-//    if(hsp->sr_log!=NULL)fprintf(hsp->sr_log,"%f %f %f %f %f %f %f\n",(hrp::rpyFromRot(m_robot->link("WAIST")->R)(0))*100,fff_f(1),ttt_f(0),ppp(1),lll(0),ref_zmp(1)*1000,m_dt);
-
-    ////////////////////  逆動力学補償テスト  /////////////////////
-
-
-
-
-
+void AutoBalancer::solveWholeBodyID(const hrp::BodyPtr robot, hrp::Vector3& f_ans, hrp::Vector3& t_ans)
+{
+  if(!idp.is_initialized){   // Initialization at once
+    idp.q.resize(m_robot->numJoints());
+    idp.q_old = idp.q_oldold = idp.dq = idp.ddq = idp.ddq_filtered = idp.q;
+    for(int i=0;i<m_robot->numJoints();i++)idp.q[i] = m_robot->joint(i)->q;
+    idp.q_oldold = idp.q_old = idp.q;
+    idp.ddq_filter.resize(m_robot->numJoints());
+    idp.base_dv_filter.resize(3);
+    idp.base_dw_filter.resize(3);
+    idp.filter_fc = 1/m_dt/10;// Theoretical sampling noise frequency is HZ/2
+    //set up filters
+    std::vector<double> fb_coeffs(3), ff_coeffs(3);
+    const double fc = tan(idp.filter_fc * M_PI * m_dt) / (2 * M_PI);
+    const double denom = 1 + (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc;
+    ff_coeffs[0] = (4 * M_PI * M_PI * fc*fc) / denom;
+    ff_coeffs[1] = (8 * M_PI * M_PI * fc*fc) / denom;
+    ff_coeffs[2] = (4 * M_PI * M_PI * fc*fc) / denom;
+    fb_coeffs[0] = 1.0;
+    fb_coeffs[1] = (8 * M_PI * M_PI * fc*fc - 2) / denom;
+    fb_coeffs[2] = (1 - (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc) / denom;
+    for(int i=0;i<m_robot->numJoints();i++)idp.ddq_filter[i].setParameter(2,fb_coeffs,ff_coeffs);
+    for(int i=0;i<3;i++)idp.base_dv_filter[i].setParameter(2,fb_coeffs,ff_coeffs);
+    for(int i=0;i<3;i++)idp.base_dw_filter[i].setParameter(2,fb_coeffs,ff_coeffs);
+    idp.is_initialized = true;
+  }
+  // set joint velocity and acceleration
+  for(int i=0;i<m_robot->numJoints();i++)idp.q(i) = m_robot->joint(i)->q;
+  idp.dq = (idp.q - idp.q_old) / m_dt;
+  idp.ddq = (idp.q - 2 * idp.q_old + idp.q_oldold) / (m_dt * m_dt);
+  for(int i=0;i<m_robot->numJoints();i++)idp.ddq_filtered[i] = idp.ddq_filter[i].passFilter(idp.ddq[i]);
+  idp.q_oldold = idp.q_old;
+  idp.q_old = idp.q;
+  for(int i=0;i<m_robot->numJoints();i++)m_robot->joint(i)->dq = idp.dq(i);
+  for(int i=0;i<m_robot->numJoints();i++)m_robot->joint(i)->ddq = idp.ddq_filtered(i);
+  // set base link velocity and acceleration
+  const hrp::Vector3 g(0,0,9.80665);
+  idp.base_p = m_robot->rootLink()->p;
+  idp.base_v = (idp.base_p - idp.base_p_old) / m_dt;
+  idp.base_dv = g + (idp.base_p - 2 * idp.base_p_old + idp.base_p_oldold) / (m_dt * m_dt);
+  for(int i=0;i<3;i++)idp.base_dv_filtered[i] = idp.base_dv_filter[i].passFilter(idp.base_dv[i]);
+  idp.base_p_oldold = idp.base_p_old;
+  idp.base_p_old = idp.base_p;
+  // set base link angle velocity and angle acceleration
+  idp.base_R =  m_robot->rootLink()->R;
+  idp.base_dR = (idp.base_R - idp.base_R_old) / m_dt;
+  idp.base_w_hat = idp.base_dR * idp.base_R.transpose();
+  idp.base_w = hrp::Vector3(idp.base_w_hat(2,1), - idp.base_w_hat(0,2), idp.base_w_hat(1,0));
+  idp.base_dw = (idp.base_w - idp.base_w_old) / m_dt;
+  for(int i=0;i<3;i++)idp.base_dw_filtered[i] = idp.base_dw_filter[i].passFilter(idp.base_w[i]);
+  idp.base_R_old = idp.base_R;
+  idp.base_w_old = idp.base_w;
+  // set as spacial velocity and acceleration
+  m_robot->rootLink()->vo = idp.base_v - idp.base_w.cross(idp.base_p);
+//  static hrp::Vector3 vo_old; m_robot->rootLink()->dvo = g + (m_robot->rootLink()->vo - vo_old)/m_dt; vo_old = m_robot->rootLink()->vo; // calc from increments
+  m_robot->rootLink()->dvo = idp.base_dv_filtered - idp.base_dw_filtered.cross(idp.base_p) - idp.base_w.cross(idp.base_v); // calc from differential
+  m_robot->rootLink()->w = idp.base_w;
+  m_robot->rootLink()->dw = idp.base_dw_filtered;
+
+  m_robot->calcForwardKinematics(true,true);// calc every link's acc and vel
+  m_robot->calcInverseDynamics(m_robot->rootLink(),f_ans,t_ans);// this returns f,t at the coordinate origin !! (not at base link pos)
+
+  hrp::Vector3 zmp4;
+  zmp4(0) = - t_ans(1)/f_ans(2);
+  zmp4(1) = t_ans(0)/f_ans(2);
+
+  fprintf(hsp->id_log,"com: %f %f ",m_robot->calcCM()(0),m_robot->calcCM()(1));
+  fprintf(hsp->id_log,"ref_zmp: %f %f ",ref_zmp(0),ref_zmp(1));
+  fprintf(hsp->id_log,"id_zmp4: %f %f ",zmp4(0),zmp4(1));
+  fprintf(hsp->id_log,"\n");
 }
 
+
 /*
   RTC::ReturnCode_t AutoBalancer::onAborting(RTC::UniqueId ec_id)
   {
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index c090ad66be6..5c52a3ff1ac 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -198,7 +198,12 @@ class AutoBalancer
   OutPort<TimedPoint3D> m_rpdcp_dbgOut;
   TimedPoint3D m_rpacp_dbg;
   OutPort<TimedPoint3D> m_rpacp_dbgOut;
+
+  TimedDoubleSeq m_invdyn_dbg;
+  OutPort<TimedDoubleSeq> m_invdyn_dbgOut;
   
+
+
   // </rtc-template>
 
   // DataOutPort declaration
@@ -265,6 +270,7 @@ class AutoBalancer
   bool solveLimbIKforLimb (ABCIKparam& param, const std::string& limb_name);
   void solveWholeBodyCOMIK(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const hrp::Vector3& head_ref);
   void solveLimbIK();
+  void solveWholeBodyID(const hrp::BodyPtr robot, hrp::Vector3& f_ans, hrp::Vector3& t_ans);
   void startABCparam(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
   void stopABCparam();
   void waitABCTransition();
@@ -303,6 +309,18 @@ class AutoBalancer
   double m_dt, move_base_gain;
   hrp::BodyPtr m_robot;
   coil::Mutex m_mutex;
+  //for invdyn
+  struct ABCIDparam {
+    hrp::dvector q, q_old, q_oldold, dq, ddq, ddq_filtered;
+    hrp::Vector3 base_p, base_p_old, base_p_oldold, base_v, base_dv, base_dv_filtered;
+    hrp::Matrix33 base_R, base_R_old, base_dR, base_w_hat;
+    hrp::Vector3 base_w, base_w_old, base_dw, base_dw_filtered;
+    std::vector<IIRFilter> ddq_filter, base_dv_filter, base_dw_filter;
+    double filter_fc;
+    bool is_initialized = false;
+  };
+  ABCIDparam idp;
+
 
   double transition_interpolator_ratio, transition_time, zmp_transition_time, adjust_footstep_transition_time, leg_names_interpolator_ratio;
   interpolator *zmp_offset_interpolator;
diff --git a/rtc/AutoBalancer/HumanMasterSlave.cpp b/rtc/AutoBalancer/HumanMasterSlave.cpp
index fbb05e329ae..a275da19b86 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.cpp
+++ b/rtc/AutoBalancer/HumanMasterSlave.cpp
@@ -121,3 +121,104 @@ void HumanSynchronizer::applyVelLimit(const HumanPose& in, HumanPose& out_old, H
   }
   out_old = out;
 }
+
+
+
+template <class T>
+IIRFilter_vec<T>::IIRFilter_vec(const std::string& error_prefix) :
+    m_initialized(false) {
+    m_error_prefix = error_prefix;
+}
+template <class T>
+bool IIRFilter_vec<T>::setParameter(int dim, std::vector<double>& A, std::vector<double>& B) {
+    m_dimension = dim;
+    // init coefficients
+    if((A.size() != dim && A.size() != dim + 1) || B.size() != dim + 1) {
+        std::cout << "[" <<  m_error_prefix << "]" << "IIRFilter coefficients size error" << std::endl;
+        return false;
+    }
+    // clear previous coefficients
+    m_fb_coefficients.clear();
+    m_ff_coefficients.clear();
+
+    if (A.size() == dim) {
+        m_fb_coefficients.push_back(1.0);
+    }
+    for(std::vector<double>::iterator it = A.begin(); it != A.end(); it++){
+        if (it == A.begin()) {
+            if( *it != 1.0 ) {
+                std::cout << "[" <<  m_error_prefix << "]" << "IIRFilter : parameter A[0] is not 1.0 !!!" << std::endl;
+            }
+            m_fb_coefficients.push_back(*it);
+        } else {
+            m_fb_coefficients.push_back(- *it);
+        }
+    }
+    for(std::vector<double>::iterator it = B.begin(); it != B.end(); it++){
+        m_ff_coefficients.push_back(*it);
+    }
+
+    // init previous values
+    m_previous_values.assign(dim, T::Zero());
+    m_initialized = true;
+    return true;
+}
+template <class T>
+bool IIRFilter_vec<T>::setParameterAsBiquad(const double f_cutoff, const double Q, const double hz){
+  std::vector<double> fb_coeffs(3), ff_coeffs(3);
+  const double omega = 2 * 3.14159265 * f_cutoff / hz;
+  const double alpha = std::sin(omega) / (2 * Q);
+  const double denom = 1 + alpha;
+  fb_coeffs[0] = 1;
+  fb_coeffs[1] = -2 * std::cos(omega) / denom;
+  fb_coeffs[2] = (1 - alpha) / denom;
+  ff_coeffs[0] = (1 - std::cos(omega)) / 2 / denom;
+  ff_coeffs[1] = (1 - std::cos(omega)) / denom;
+  ff_coeffs[2] = (1 - std::cos(omega)) / 2 / denom;
+  return this->setParameter(2, fb_coeffs, ff_coeffs);
+}
+template <class T>
+void IIRFilter_vec<T>::getParameter(int &dim, std::vector<double>& A, std::vector<double>& B)
+{
+    dim = m_dimension;
+    B.resize(m_ff_coefficients.size());
+    std::copy(m_ff_coefficients.begin(), m_ff_coefficients.end(), B.begin());
+    A.resize(0);
+    for(std::vector<double>::iterator it = m_fb_coefficients.begin();
+        it != m_fb_coefficients.end(); it++) {
+        if (it == m_fb_coefficients.begin()) {
+            A.push_back(*it);
+        } else {
+            A.push_back(- *it);
+        }
+    }
+}
+template <class T>
+void IIRFilter_vec<T>::reset(T initial_input)
+{
+    double sum_ff_coeffs = std::accumulate(m_ff_coefficients.begin(), m_ff_coefficients.end(), 0.0);
+    T reset_val = initial_input / sum_ff_coeffs;
+    m_previous_values.assign(m_dimension, reset_val);
+}
+template <class T>
+T IIRFilter_vec<T>::passFilter(T input)
+{
+    if (! m_initialized) {
+        return 0.0;
+    }
+    T feedback, filtered;
+    // calcurate retval
+    feedback = m_fb_coefficients[0] * input;
+    for (int i = 0; i < m_dimension; i++) {
+        feedback += m_fb_coefficients[i + 1] * m_previous_values[i];
+    }
+    filtered = m_ff_coefficients[0] * feedback;
+    for(int i = 0; i < m_dimension; i++) {
+        filtered += m_ff_coefficients[i + 1] * m_previous_values[i];
+    }
+    // update previous values
+    m_previous_values.push_front(feedback);
+    m_previous_values.pop_back();
+
+    return filtered;
+}
diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index 0707265c0c8..2c919e50c44 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -19,6 +19,9 @@
 #include <opencv2/imgproc/types_c.h>
 #include <hrpUtil/Eigen4d.h>
 
+
+#include <numeric>
+
 #if defined(__cplusplus)
 extern "C" {
 #endif
@@ -48,7 +51,8 @@ namespace myconst{
   const int r = 0, p = 1, y = 2, rpy = 3;
   const int L = 0, R = 1, LR = 2;
   const int MIN = 0, MAX = 1, MINMAX = 2;
-  const double INFMIN = std::numeric_limits<double>::min(), INFMAX = std::numeric_limits<double>::max();
+//  const double INFMIN = std::numeric_limits<double>::min(), INFMAX = std::numeric_limits<double>::max();
+  const double INFMIN = - std::numeric_limits<double>::max(), INFMAX = std::numeric_limits<double>::max();
   const double D2R = M_PI/180.0;
 }
 using namespace myconst;
@@ -65,16 +69,18 @@ class BiquadIIRFilterVec{
     void setParameter(const hrp::Vector3 fc_in, const double HZ){
       ff_coeffs.resize(3);
       fb_coeffs.resize(3);
-      for(int i=0;i<3;i++){
-        const double fc = tan((double)fc_in(i) * M_PI / HZ) / (2 * M_PI);
-        const double denom = 1 + (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc;
-        ff_coeffs[0] = (4 * M_PI * M_PI * fc*fc) / denom;
-        ff_coeffs[1] = (8 * M_PI * M_PI * fc*fc) / denom;
-        ff_coeffs[2] = (4 * M_PI * M_PI * fc*fc) / denom;
-        fb_coeffs[0] = 1.0;
-        fb_coeffs[1] = (8 * M_PI * M_PI * fc*fc - 2) / denom;
-        fb_coeffs[2] = (1 - (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc) / denom;
-        filters[i].setParameter(2,fb_coeffs,ff_coeffs);
+      const double Q = 1.0/2;
+     for(int i=0;i<3;i++){
+       const double omega = 2 * 3.14159265 * fc_in(i) / HZ;
+       const double alpha = std::sin(omega) / (2 * Q);
+       const double denom = 1 + alpha;
+       fb_coeffs[0] = 1;
+       fb_coeffs[1] = -2 * cos(omega) / denom;
+       fb_coeffs[2] = (1 - alpha) / denom;
+       ff_coeffs[0] = (1 - cos(omega)) / 2 / denom;
+       ff_coeffs[1] = (1 - cos(omega)) / denom;
+       ff_coeffs[2] = (1 - cos(omega)) / 2 / denom;
+       filters[i].setParameter(2, fb_coeffs, ff_coeffs);
       }
     };
     void setParameter(const double fc_in, const double HZ){
@@ -89,6 +95,31 @@ class BiquadIIRFilterVec{
     }
 };
 
+
+
+
+
+template <class T> class IIRFilter_vec
+{
+  public:
+    IIRFilter_vec(const std::string& error_prefix = "");
+    ~IIRFilter_vec() {};
+    bool setParameter(int dim, std::vector<double>& A, std::vector<double>& B);
+    bool setParameterAsBiquad(const double f_cutoff, const double Q, const double hz);
+    void getParameter(int &dim, std::vector<double>&A, std::vector<double>& B);
+    void reset(T initial_input = T::Zero());
+    T passFilter(T input);
+  private:
+    int m_dimension;
+    std::vector<double> m_fb_coefficients; // fb parameters (dim must be m_dimension + 1, m_fb_coefficients[0] would be 1.0)
+    std::vector<double> m_ff_coefficients; // ff parameters (dim must be m_dimension + 1)
+    std::deque<T> m_previous_values;
+    bool m_initialized;
+    std::string m_error_prefix;
+};
+
+
+
 class HRPPose3D{
   public:
     hrp::Vector3 p,p_offs,rpy,rpy_offs;
@@ -212,6 +243,7 @@ class HumanSynchronizer{
     HumanPose hp_plot;
     HumanPose rp_ref_out;
     FILE *sr_log,*cz_log;
+    FILE *id_log;
     hrp::Vector3 init_wld_rp_rfeepos,init_wld_rp_lfeepos;
     hrp::Vector3 init_wld_hp_rfpos,init_wld_hp_lfpos;
     HRPPose3D baselinkpose;
@@ -223,6 +255,7 @@ class HumanSynchronizer{
     std::vector<hrp::Vector3> rf_vert,lf_vert;
     double countdown_sec;
     hrp::Vector3 cp_dec,cp_acc;
+    Wrench6 invdyn_ft;
 
     interpolator *init_zmp_com_offset_interpolator;
     double zmp_com_offset_ip_ratio;
@@ -301,7 +334,8 @@ class HumanSynchronizer{
 
       WBMSparam.use_rh = WBMSparam.use_lh = true;
       WBMSparam.use_head = true;
-      WBMSparam.set_com_height_fix = true;
+//      WBMSparam.set_com_height_fix = true;
+      WBMSparam.set_com_height_fix = false;
       WBMSparam.set_com_height_fix_val = 0.02;
       WBMSparam.foot_vertical_vel_limit_coeff = 4.0;
       WBMSparam.human_com_height = 1.00;
@@ -322,12 +356,14 @@ class HumanSynchronizer{
       if(DEBUG){
         sr_log = fopen("/home/ishiguro/HumanSync_support_region.log","w+");
         cz_log = fopen("/home/ishiguro/HumanSync_com_zmp.log","w+");
+        id_log = fopen("/home/ishiguro/HumanSync_invdyn.log","w+");
       }
       cout<<"HumanSynchronizer constructed"<<endl;
     }
     ~HumanSynchronizer(){
       if(DEBUG)fclose(sr_log);
       if(DEBUG)fclose(cz_log);
+      if(DEBUG)fclose(id_log);
       delete init_zmp_com_offset_interpolator;
       cout<<"HumanSynchronizer destructed"<<endl;
     }
@@ -566,7 +602,7 @@ class HumanSynchronizer{
       }
     }
     void applyEEWorkspaceLimit(HumanPose& tgt){
-      const double MAX_FW = 0.30;
+      const double MAX_FW = 0.25;
       const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
       if(!is_rf_contact && is_lf_contact){//右足浮遊時
         const hrp::Vector2 lf2rf_vec( tgt.getP("rf").p(X)-pre_cont_lfpos(X), tgt.getP("rf").p(Y)-pre_cont_lfpos(Y) );
@@ -712,6 +748,110 @@ class HumanSynchronizer{
     void applyCOMZMPXYZLock(HumanPose& tgt);
 };
 
+//
+//class SimpleFullbodyInverseDynamicsSolver{
+//  private:
+//    hrp::BodyPtr m_robot;
+//    hrp::dvector q, q_old, q_oldold, dq, ddq, ddq_filtered;
+//    hrp::Vector3 base_p, base_p_old, base_p_oldold, base_v, base_dv, base_dv_filtered;
+//    hrp::Matrix33 base_R, base_R_old, base_dR, base_w_hat;
+//    hrp::Vector3 base_w, base_w_old, base_dw, base_dw_filtered;
+//    std::vector<IIRFilter> ddq_filter, base_dv_filter, base_dw_filter;
+//    double DT;
+//    bool is_RobotStateInitialized;
+//
+//  public:
+//    SimpleFullbodyInverseDynamicsSolver(const hrp::BodyPtr& _robot, const double _dt, const double _acceleration_filter_fc = 25.0)
+//        : m_robot(_robot),
+//          DT(_dt),
+//          is_RobotStateInitialized(false)
+//    {
+//      q.resize(m_robot->numJoints());
+//      q_old.resize(m_robot->numJoints());
+//      q_oldold.resize(m_robot->numJoints());
+//      dq.resize(m_robot->numJoints());
+//      ddq.resize(m_robot->numJoints());
+//      ddq_filtered.resize(m_robot->numJoints());
+//      initializeAccelerationFilters(_acceleration_filter_fc, _dt);
+//    };
+//    ~SimpleFullbodyInverseDynamicsSolver(){
+//    };
+//    void initializeAccelerationFilters(const double _fc_in, const double _dt){//set up filters as 2nd order Biquad IIR
+//      ddq_filter.resize(m_robot->numJoints());
+//      base_dv_filter.resize(3);
+//      base_dw_filter.resize(3);
+//      std::vector<double> fb_coeffs(3), ff_coeffs(3);
+//      const double fc = std::tan(_fc_in * M_PI * _dt) / (2 * M_PI);
+//      const double denom = 1 + (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc;
+//      ff_coeffs[0] = (4 * M_PI * M_PI * fc*fc) / denom;
+//      ff_coeffs[1] = (8 * M_PI * M_PI * fc*fc) / denom;
+//      ff_coeffs[2] = (4 * M_PI * M_PI * fc*fc) / denom;
+//      fb_coeffs[0] = 1.0;
+//      fb_coeffs[1] = (8 * M_PI * M_PI * fc*fc - 2) / denom;
+//      fb_coeffs[2] = (1 - (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc) / denom;
+//      for(int i=0;i<m_robot->numJoints();i++)ddq_filter[i].setParameter(2,fb_coeffs,ff_coeffs);
+//      for(int i=0;i<3;i++)base_dv_filter[i].setParameter(2,fb_coeffs,ff_coeffs);
+//      for(int i=0;i<3;i++)base_dw_filter[i].setParameter(2,fb_coeffs,ff_coeffs);
+//    };
+//    void initializeRobotState(){
+//      for(int i=0;i<m_robot->numJoints();i++)q(i) = m_robot->joint(i)->q;
+//      q_oldold = q_old = q;
+//      dq = ddq = ddq_filtered = hrp::dvector::Zero(m_robot->numJoints());
+//      base_p_oldold = base_p_old = base_p = m_robot->rootLink()->p;
+//      base_R_old = base_R = m_robot->rootLink()->R;
+//      base_dR = base_w_hat = hrp::Matrix33::Zero();
+//      base_w_old = base_w = base_dw = base_dw_filtered = hrp::Vector3::Zero();
+//      is_RobotStateInitialized = true;
+//    };
+//    void solveFullbodyID(hrp::Vector3& f_ans, hrp::Vector3& t_ans){
+//      if(!is_RobotStateInitialized) initializeRobotState();
+//      calcAccelerationsForInverseDynamics();
+//      passAccelerationFilters();
+//      setCurrentRobotState();
+//      m_robot->calcForwardKinematics(true,true);// calc every link's acc and vel
+//      m_robot->calcInverseDynamics(m_robot->rootLink(),f_ans,t_ans);// this returns f,t at the coordinate origin (not at base link pos)
+//    };
+//
+//  private:
+//    void calcAccelerationsForInverseDynamics(){
+//      for(int i=0;i<m_robot->numJoints();i++)q(i) = m_robot->joint(i)->q;
+//      dq = (q - q_old) / DT;
+//      ddq = (q - 2 * q_old + q_oldold) / (DT * DT);
+//      q_oldold = q_old;
+//      q_old = q;
+//      const hrp::Vector3 g(0, 0, 9.80665);
+//      base_p = m_robot->rootLink()->p;
+//      base_v = (base_p - base_p_old) / DT;
+//      base_dv = g + (base_p - 2 * base_p_old + base_p_oldold) / (DT * DT);
+//      base_p_oldold = base_p_old;
+//      base_p_old = base_p;
+//      base_R =  m_robot->rootLink()->R;
+//      base_dR = (base_R - base_R_old) / DT;
+//      base_w_hat = base_dR * base_R.transpose();
+//      base_w = hrp::Vector3(base_w_hat(2,1), - base_w_hat(0,2), base_w_hat(1,0));
+//      base_dw = (base_w - base_w_old) / DT;
+//      base_R_old = base_R;
+//      base_w_old = base_w;
+//    };
+//    void passAccelerationFilters(){
+//      for(int i=0;i<m_robot->numJoints();i++)ddq_filtered[i] = ddq_filter[i].passFilter(ddq[i]);
+//      for(int i=0;i<3;i++){
+//        base_dv_filtered[i] = base_dv_filter[i].passFilter(base_dv[i]);
+//        base_dw_filtered[i] = base_dw_filter[i].passFilter(base_dw[i]);
+//      }
+//    };
+//    void setCurrentRobotState(){
+//      for(int i=0;i<m_robot->numJoints();i++){
+//        m_robot->joint(i)->dq = dq(i);
+//        m_robot->joint(i)->ddq = ddq_filtered(i);
+//      }
+//      m_robot->rootLink()->vo = base_v - base_w.cross(base_p);
+//      //static hrp::Vector3 vo_old; m_robot->rootLink()->dvo = g + (m_robot->rootLink()->vo - vo_old)/DT; vo_old = m_robot->rootLink()->vo; // calc in incremental way
+//      m_robot->rootLink()->dvo = base_dv_filtered - base_dw_filtered.cross(base_p) - base_w.cross(base_v); // calc in differential way
+//      m_robot->rootLink()->w = base_w;
+//      m_robot->rootLink()->dw = base_dw_filtered;
+//    };
+//};
 
 
 #endif // HUMANMASTERSLAVE_H

From 4281549903e9a921a8885078b39504fb287a10cc Mon Sep 17 00:00:00 2001
From: ishiguroJSK <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 11 Apr 2017 15:49:13 +0900
Subject: [PATCH 066/217] more independency

---
 rtc/AutoBalancer/AutoBalancer.cpp | 122 ++++++++++++++++--------------
 rtc/AutoBalancer/AutoBalancer.h   |   1 +
 2 files changed, 66 insertions(+), 57 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index cda90da9d82..d07bf41b132 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -549,22 +549,24 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
         transition_interpolator_ratio = (control_mode == MODE_IDLE) ? 0.0 : 1.0;
       }
       if (control_mode != MODE_IDLE ) {
-        solveFullbodyIK();
-//        /////// Inverse Dynamics /////////
-//        if(!idsb.is_initialized){
-//          idsb.setInitState(m_robot, m_dt);
-//          invdyn_zmp_filters.resize(3);
-//          for(int i=0;i<3;i++){
-//            invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
-//            invdyn_zmp_filters[i].reset(ref_zmp(i));
-//          }
-//        }
-//        calcAccelerationsForInverseDynamics(m_robot, idsb);
-//        if(gg_is_walking){
-//          calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp);
-//          for(int i=0;i<3;i++) ref_zmp(i) = invdyn_zmp_filters[i].passFilter(ref_zmp(i));
-//        }
-//        updateInvDynStateBuffer(idsb);
+        if(!hsp->isHumanSyncOn())solveFullbodyIK();
+        processWholeBodyMasterSlave();
+
+        /////// Inverse Dynamics /////////
+        if(!idsb.is_initialized){
+          idsb.setInitState(m_robot, m_dt);
+          invdyn_zmp_filters.resize(3);
+          for(int i=0;i<3;i++){
+            invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
+            invdyn_zmp_filters[i].reset(ref_zmp(i));
+          }
+        }
+        calcAccelerationsForInverseDynamics(m_robot, idsb);
+        if(gg_is_walking){
+          calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp);
+          for(int i=0;i<3;i++) ref_zmp(i) = invdyn_zmp_filters[i].passFilter(ref_zmp(i));
+        }
+        updateInvDynStateBuffer(idsb);
 
         rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
       } else {
@@ -1185,14 +1187,30 @@ void AutoBalancer::solveFullbodyIK ()
   }
   // Revert
   fik->revertRobotStateToCurrent();
+  // TODO : SBP calculation is outside of solve ik?
+  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
+  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
+  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
+  // Solve IK
+  fik->solveFullbodyIK (dif_cog, transition_interpolator->isEmpty());
+}
 
+void AutoBalancer::processWholeBodyMasterSlave(){
+  // Set ik target params
+  fik->target_root_p = target_root_p;
+  fik->target_root_R = target_root_R;
+  for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
+      it->second.target_p0 = ikp[it->first].target_p0;
+      it->second.target_r0 = ikp[it->first].target_r0;
+  }
+  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
+  fik->current_tm = m_qRef.tm;
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+      fik->ikp[it->first].is_ik_enable = it->second.is_active;
+  }
+  // Revert
+  fik->revertRobotStateToCurrent();
 
-
-
-
-
-  ////////////// ishiguro ///////////////
-  //for HumanSynchronizer
   if(hsp->startCountdownForHumanSync){
     std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
     hsp->updateCountDown();
@@ -1233,62 +1251,52 @@ void AutoBalancer::solveFullbodyIK ()
     //outport用のデータ上書き
     ref_zmp = hsp->rp_ref_out.getP("zmp").p;
     ref_cog = hsp->rp_ref_out.getP("com").p;
-    ////////////// ishiguro ///////////////
-
-
-  }else{
-    // TODO : SBP calculation is outside of solve ik?
-    hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
-    static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
-    hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
-    // Solve IK
-    fik->solveFullbodyIK (dif_cog, transition_interpolator->isEmpty());
   }
-
-
 }
 
+
 void AutoBalancer::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const hrp::Vector3& head_ref){
   int com_ik_loop=0;
   const int COM_IK_MAX_LOOP = 5;
   const double COM_IK_MAX_ERROR = 1e-4;
   m_robot->rootLink()->p = com_ref.p;//move base link at first
   m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
-  m_robot->calcForwardKinematics();//apply
-  hrp::Vector3 tmp_com_err = com_ref.p - m_robot->calcCM();
-  if(ikp.count("rleg")){
-    ikp["rleg"].target_r0 = hrp::rotFromRpy(rf_ref.rpy);//convert End Effector pos into link origin pos
-    ikp["rleg"].target_p0 = rf_ref.p;
+  hrp::Vector3 tmp_com_err;
+  if(fik->ikp.count("rleg")){
+    fik->ikp["rleg"].target_r0 = hrp::rotFromRpy(rf_ref.rpy);//convert End Effector pos into link origin pos
+    fik->ikp["rleg"].target_p0 = rf_ref.p;
   }
-  if(ikp.count("lleg")){
-    ikp["lleg"].target_r0 = hrp::rotFromRpy(lf_ref.rpy);
-    ikp["lleg"].target_p0 = lf_ref.p;
+  if(fik->ikp.count("lleg")){
+    fik->ikp["lleg"].target_r0 = hrp::rotFromRpy(lf_ref.rpy);
+    fik->ikp["lleg"].target_p0 = lf_ref.p;
   }
-  if(ikp.count("rarm")){
-    ikp["rarm"].target_r0 = hrp::rotFromRpy(rh_ref.rpy);
-    ikp["rarm"].target_p0 = rh_ref.p;
+  if(fik->ikp.count("rarm")){
+    fik->ikp["rarm"].target_r0 = hrp::rotFromRpy(rh_ref.rpy);
+    fik->ikp["rarm"].target_p0 = rh_ref.p;
   }
-  if(ikp.count("larm")){
-    ikp["larm"].target_r0 = hrp::rotFromRpy(lh_ref.rpy);
-    ikp["larm"].target_p0 = lh_ref.p;
+  if(fik->ikp.count("larm")){
+    fik->ikp["larm"].target_r0 = hrp::rotFromRpy(lh_ref.rpy);
+    fik->ikp["larm"].target_p0 = lh_ref.p;
   }
-  while(tmp_com_err.norm() > COM_IK_MAX_ERROR){
+  if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref(2);
+  if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref(1);
+  //COM 収束ループ
+  while(
+      m_robot->calcForwardKinematics(),
+      tmp_com_err = com_ref.p - m_robot->calcCM(),
+      tmp_com_err.norm() > COM_IK_MAX_ERROR)
+  {
     m_robot->rootLink()->p += tmp_com_err;
     m_robot->calcForwardKinematics();
-    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-      for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-          if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, true);
-      }
+    for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
+        if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, true);
     }
-    m_robot->calcForwardKinematics();
-    tmp_com_err = com_ref.p - m_robot->calcCM();
     if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
   }
-  if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref(2);
-  if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref(1);
 }
 
 
+
 /*
   RTC::ReturnCode_t AutoBalancer::onAborting(RTC::UniqueId ec_id)
   {
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index cec45a79502..a57c4863fb6 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -544,6 +544,7 @@ class AutoBalancer
   };
   void getTargetParameters();
   void solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const hrp::Vector3& head_ref);
+  void processWholeBodyMasterSlave();
   void solveFullbodyIK ();
   void startABCparam(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
   void stopABCparam();

From 7a89eeb15cc27ec79fe65c455578270281155f8f Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 22 Apr 2017 03:00:04 +0900
Subject: [PATCH 067/217] transport master-slave code ABC->WBMS

---
 idl/CMakeLists.txt                            |    1 +
 idl/WholeBodyMasterSlaveService.idl           |   54 +
 python/hrpsys_config.py                       |   33 +-
 rtc/AutoBalancer/AutoBalancer.cpp             |  271 +++-
 rtc/AutoBalancer/AutoBalancer.h               |    7 +
 rtc/AutoBalancer/HumanMasterSlave.h           |   15 +-
 rtc/CMakeLists.txt                            |    1 +
 rtc/WholeBodyMasterSlave/CMakeLists.txt       |   20 +
 .../WholeBodyMasterSlave.cpp                  | 1152 +++++++++++++++++
 .../WholeBodyMasterSlave.h                    |  284 ++++
 .../WholeBodyMasterSlaveComp.cpp              |   91 ++
 .../WholeBodyMasterSlaveService_impl.cpp      |   37 +
 .../WholeBodyMasterSlaveService_impl.h        |   28 +
 13 files changed, 1940 insertions(+), 54 deletions(-)
 create mode 100644 idl/WholeBodyMasterSlaveService.idl
 create mode 100644 rtc/WholeBodyMasterSlave/CMakeLists.txt
 create mode 100644 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
 create mode 100644 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
 create mode 100644 rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveComp.cpp
 create mode 100644 rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp
 create mode 100644 rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h

diff --git a/idl/CMakeLists.txt b/idl/CMakeLists.txt
index 96786042543..290823ef252 100644
--- a/idl/CMakeLists.txt
+++ b/idl/CMakeLists.txt
@@ -29,6 +29,7 @@ set(idl_files
   ReferenceForceUpdaterService.idl
   AccelerationFilterService.idl
   ObjectContactTurnaroundDetectorService.idl
+  WholeBodyMasterSlaveService.idl
   )
 
 set(openhrp_idl_files
diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
new file mode 100644
index 00000000000..3e26e90cb8d
--- /dev/null
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -0,0 +1,54 @@
+/**
+ * @file WholeBodyMasterSlaveService.idl
+ * @brief Services for the WholeBodyMasterSlave interface
+ */
+//#include "OpenHRPCommon.idl"
+module OpenHRP
+{
+  
+  interface WholeBodyMasterSlaveService
+  {
+    typedef sequence<double, 3> DblSequence3;
+    //typedef sequence<double, 4> DblSequence4;
+    typedef double DblArray3[3];
+    typedef double DblArray4[4];
+    typedef sequence<string> StrSequence;
+
+//    /**
+//     * @enum IKLimbParameters
+//     * @brief IKLimbParameters
+//     */
+//    struct IKLimbParameters {
+//        /// Joint weight for Inverse Kinematics calculation.
+//        sequence<double> ik_optional_weight_vector;
+//        /// SR-inverse gain for inverse kinematics.
+//        double sr_gain;
+//        /// Avoid joint limit gain for inverse kinematics.
+//        double avoid_gain;
+//        /// Reference joint angles tracking gain for inverse kinematics.
+//        double reference_gain;
+//        /// Manipulability limit for inverse kinematics.
+//        double manipulability_limit;
+//    };
+
+    /**
+     * HumanSync
+     */
+    struct WholeBodyMasterSlaveParam
+    {
+        double human_com_height;
+        double foot_vertical_vel_limit_coeff;
+        boolean set_com_height_fix;
+        double set_com_height_fix_val;
+        boolean use_hands;
+        boolean use_head;
+        double swing_foot_height_offset;
+        double swing_foot_max_height;
+        double auto_swing_foot_landing_threshold;
+    };
+    boolean startCountDownForWholeBodyMasterSlave(in double sec);
+    boolean stopHumanSync();
+    void setWholeBodyMasterSlaveParam(in WholeBodyMasterSlaveParam i_param);
+    void getWholeBodyMasterSlaveParam(out WholeBodyMasterSlaveParam i_param);
+  };
+};
diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index 6b980c09979..558e0049c91 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -267,6 +267,11 @@ class HrpsysConfigurator(object):
     octd = None
     octd_svc = None
     octd_version = None
+    
+    # WholeBodyMasterSLave
+    wbms = None
+    wbms_svc = None
+    wbms_version = None
 
     # rtm manager
     ms = None
@@ -379,14 +384,30 @@ def connectComps(self):
             connectPorts(self.seq.port(sen + "Ref"),
                          self.sh.port(sen + "In"))
 
+        # connection for wbms
+        if self.wbms:
+            connectPorts(self.sh.port("qOut"), self.wbms.port("qRef"))
+            connectPorts(self.sh.port("zmpOut"), self.wbms.port("zmpIn"))
+            connectPorts(self.sh.port("basePosOut"), self.wbms.port("basePosIn"))
+            connectPorts(self.sh.port("baseRpyOut"), self.wbms.port("baseRpyIn"))
+            connectPorts(self.sh.port("optionalDataOut"), self.wbms.port("optionalData"))
+
         # connection for st
         if rtm.findPort(self.rh.ref, "lfsensor") and rtm.findPort(
                                      self.rh.ref, "rfsensor") and self.st:
             connectPorts(self.kf.port("rpy"), self.st.port("rpy"))
-            connectPorts(self.sh.port("zmpOut"), self.abc.port("zmpIn"))
-            connectPorts(self.sh.port("basePosOut"), self.abc.port("basePosIn"))
-            connectPorts(self.sh.port("baseRpyOut"), self.abc.port("baseRpyIn"))
-            connectPorts(self.sh.port("optionalDataOut"), self.abc.port("optionalData"))
+            #### normal
+#            connectPorts(self.sh.port("zmpOut"), self.abc.port("zmpIn"))
+#            connectPorts(self.sh.port("basePosOut"), self.abc.port("basePosIn"))
+#            connectPorts(self.sh.port("baseRpyOut"), self.abc.port("baseRpyIn"))
+#            connectPorts(self.sh.port("optionalDataOut"), self.abc.port("optionalData"))
+            #### wbms
+            connectPorts(self.wbms.port("q"), self.abc.port("qRef"))
+            connectPorts(self.wbms.port("zmpOut"), self.abc.port("zmpIn"))
+            connectPorts(self.wbms.port("basePosOut"), self.abc.port("basePosIn"))
+            connectPorts(self.wbms.port("baseRpyOut"), self.abc.port("baseRpyIn"))
+            connectPorts(self.wbms.port("optionalDataOut"), self.abc.port("optionalData"))
+            
             connectPorts(self.abc.port("zmpOut"), self.st.port("zmpRef"))
             connectPorts(self.abc.port("baseRpyOut"), self.st.port("baseRpyIn"))
             connectPorts(self.abc.port("basePosOut"), self.st.port("basePosIn"))
@@ -397,8 +418,6 @@ def connectComps(self):
             connectPorts(self.seq.port("qRef"), self.st.port("qRefSeq"))
             connectPorts(self.abc.port("walkingStates"), self.st.port("walkingStates"))
             connectPorts(self.abc.port("sbpCogOffset"), self.st.port("sbpCogOffset"))
-            # for human tracker calib
-            connectPorts(self.st.port("zmp"), self.abc.port("actzmpIn"))
 
             connectPorts(self.abc.port("toeheelRatio"), self.st.port("toeheelRatio"))
             if self.es:
@@ -497,7 +516,6 @@ def connectComps(self):
             if self.rmfo:
                 for sen in filter(lambda x: x.type == "Force", self.sensors):
                     connectPorts(self.rmfo.port("off_" + sen.name), self.octd.port(sen.name))
-
         # connection for gc
         if self.gc:
             connectPorts(self.rh.port("q"), self.gc.port("qCurrent"))  # other connections
@@ -745,6 +763,7 @@ def getRTCListUnstable(self):
             ['es', "EmergencyStopper"],
             ['rfu', "ReferenceForceUpdater"],
             ['ic', "ImpedanceController"],
+            ['wbms', "WholeBodyMasterSlave"],
             ['abc', "AutoBalancer"],
             ['st', "Stabilizer"],
             ['co', "CollisionDetector"],
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index d07bf41b132..be6cf4b6cc0 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -528,8 +528,10 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
     if(ikp.count("rarm"))ikp["rarm"].is_active = false;
     if(ikp.count("larm"))ikp["larm"].is_active = false;
-    m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
-    m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
+//    m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
+//    m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
+
+
 
 
     // Calculation
@@ -549,25 +551,53 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
         transition_interpolator_ratio = (control_mode == MODE_IDLE) ? 0.0 : 1.0;
       }
       if (control_mode != MODE_IDLE ) {
-        if(!hsp->isHumanSyncOn())solveFullbodyIK();
-        processWholeBodyMasterSlave();
+        solveFullbodyIK();
+
+//        processWholeBodyMasterSlave();
 
         /////// Inverse Dynamics /////////
-        if(!idsb.is_initialized){
-          idsb.setInitState(m_robot, m_dt);
-          invdyn_zmp_filters.resize(3);
-          for(int i=0;i<3;i++){
-            invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
-            invdyn_zmp_filters[i].reset(ref_zmp(i));
-          }
-        }
-        calcAccelerationsForInverseDynamics(m_robot, idsb);
-        if(gg_is_walking){
-          calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp);
-          for(int i=0;i<3;i++) ref_zmp(i) = invdyn_zmp_filters[i].passFilter(ref_zmp(i));
+//        if(!idsb.is_initialized){
+//          idsb.setInitState(m_robot, m_dt);
+//          invdyn_zmp_filters.resize(3);
+//          for(int i=0;i<3;i++){
+//            invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
+//            invdyn_zmp_filters[i].reset(ref_zmp(i));
+//          }
+//          idsb2.setInitState(m_robot, m_dt);
+//          invdyn_zmp_filters2.resize(3);
+//          for(int i=0;i<3;i++){
+//            invdyn_zmp_filters2[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
+//            invdyn_zmp_filters2[i].reset(ref_zmp(i));
+//          }
+//        }
+//        calcAccelerationsForInverseDynamics(m_robot, idsb);
+//        hrp::Vector3 ref_zmp_invdyn;
+//        calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);
+//        for(int i=0;i<3;i++) ref_zmp_invdyn(i) = invdyn_zmp_filters[i].passFilter(ref_zmp_invdyn(i));
+//        updateInvDynStateBuffer(idsb);
+
+//        calcDynamicsFilterCompensation(ref_zmp, ref_zmp_invdyn);
+
+//        calcAccelerationsForInverseDynamics(m_robot, idsb2);
+//        calcWorldZMPFromInverseDynamics(m_robot, idsb2, ref_zmp_invdyn2);
+//        for(int i=0;i<3;i++) ref_zmp_invdyn2(i) = invdyn_zmp_filters2[i].passFilter(ref_zmp_invdyn2(i));
+//        updateInvDynStateBuffer(idsb2);
+
+        //        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
+
+        if(m_optionalData.data.length()>=4*2){
+          m_contactStates.data[contact_states_index_map["rleg"]] = isOptionalDataContact("rleg");
+          m_contactStates.data[contact_states_index_map["lleg"]] = isOptionalDataContact("lleg");
         }
-        updateInvDynStateBuffer(idsb);
-
+//        m_robot->rootLink()->p = input_basePos;
+//        m_robot->rootLink()->R = input_baseRot;
+//        for (int i = 0; i < m_robot->numJoints(); i++ ){
+//           m_robot->joint(i)->q = m_qRef.data[i];
+//        }
+//        m_robot->calcForwardKinematics();
+
+//        ref_cog = m_robot->calcCM();
+//        rel_ref_zmp = input_zmp;
         rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
       } else {
         rel_ref_zmp = input_zmp;
@@ -1197,19 +1227,19 @@ void AutoBalancer::solveFullbodyIK ()
 
 void AutoBalancer::processWholeBodyMasterSlave(){
   // Set ik target params
-  fik->target_root_p = target_root_p;
-  fik->target_root_R = target_root_R;
-  for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-      it->second.target_p0 = ikp[it->first].target_p0;
-      it->second.target_r0 = ikp[it->first].target_r0;
-  }
+//  fik->target_root_p = target_root_p;
+//  fik->target_root_R = target_root_R;
+//  for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
+//      it->second.target_p0 = ikp[it->first].target_p0;
+//      it->second.target_r0 = ikp[it->first].target_r0;
+//  }
   fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
   fik->current_tm = m_qRef.tm;
   for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
       fik->ikp[it->first].is_ik_enable = it->second.is_active;
   }
-  // Revert
-  fik->revertRobotStateToCurrent();
+//  // Revert
+//  fik->revertRobotStateToCurrent();
 
   if(hsp->startCountdownForHumanSync){
     std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
@@ -1255,28 +1285,180 @@ void AutoBalancer::processWholeBodyMasterSlave(){
 }
 
 
+hrp::Vector3 torso_pos = hrp::Vector3::Zero();
+hrp::Vector3 torso_rot = hrp::Vector3::Zero();
+//void AutoBalancer::calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody){
+//  hrp::Vector3 torso_pos_acc,torso_rot_acc;
+//  static hrp::Vector3 torso_pos_vel,torso_rot_vel = hrp::Vector3::Zero();
+//  hrp::Matrix33 total_inertia;
+//  for(int i=0; i<m_robot->numLinks(); i++){
+//    total_inertia += m_robot->link(i)->I;
+//  }
+//  hrp::Vector3 zmp_err = zmp_fullbody - zmp_lip;
+//  if(fabs(zmp_err(0))<0.002)zmp_err(0)=0;
+//  if(fabs(zmp_err(1))<0.002)zmp_err(1)=0;
+//  LIMIT_MINMAX(zmp_err(0),-0.05,0.05);
+//  LIMIT_MINMAX(zmp_err(1),-0.05,0.05);
+//  zmp_err(2)=0;
+//  torso_pos_acc(0) = zmp_err(0) / 1.1 * 9.8;
+//  torso_pos_acc(1) = zmp_err(1) / 1.1 * 9.8;
+//  torso_rot_acc(0) = - zmp_err(1) * m_robot->totalMass() * 9.8 / (130*0.2*0.2);
+//  torso_rot_acc(1) = + zmp_err(0) * m_robot->totalMass() * 9.8 / (130*0.2*0.2);
+//
+//  torso_pos_acc(0) *= 0.5;
+//  torso_pos_acc(1) *= 0.5;
+//  torso_rot_acc(0) *= 0.1;
+//  torso_rot_acc(1) *= 0.1;
+//
+//
+//
+//  torso_pos_acc -= 1 * torso_pos + 1 * torso_pos_vel;
+//  torso_rot_acc -= 1 * torso_rot + 1 * torso_rot_vel;
+////  torso_rot_acc(0) -= 10 * torso_rot(0) + 10 * torso_rot_vel(0);
+////  torso_rot_acc(1) -= 10 * torso_rot(1) + 10 * torso_rot_vel(1);
+//
+//  LIMIT_MINMAX(torso_pos_acc(0),-10,10);
+//  LIMIT_MINMAX(torso_pos_acc(1),-10,10);
+//  LIMIT_MINMAX(torso_pos_acc(2),-0,0);
+//  LIMIT_MINMAX(torso_rot_acc(0),-10,10);
+//  LIMIT_MINMAX(torso_rot_acc(1),-10,10);
+//  LIMIT_MINMAX(torso_rot_acc(2),-0,0);
+//
+//
+//  if(hsp->isHumanSyncOn()){
+//    torso_rot_vel += torso_rot_acc * m_dt;
+//    torso_rot += torso_rot_vel * m_dt;
+//    torso_pos_vel += torso_pos_acc * m_dt;
+//    torso_pos += torso_pos_vel * m_dt;
+//  }
+//
+//  LIMIT_MINMAX(torso_pos(0),-0.1,0.1);
+//  LIMIT_MINMAX(torso_pos(1),-0.1,0.1);
+//  LIMIT_MINMAX(torso_pos(2),-0.1,0.1);
+//  LIMIT_MINMAX(torso_rot(0),-10*M_PI/180,10*M_PI/180);
+//  LIMIT_MINMAX(torso_rot(1),-10*M_PI/180,10*M_PI/180);
+//  LIMIT_MINMAX(torso_rot(2),-0.1,0.1);
+//
+//
+////  cout<<"total_inertia: "<<total_inertia<<endl;
+////  cout<<"zmp_lip: "<<zmp_lip.transpose()<<endl;
+////  cout<<"zmp_fullbody: "<<zmp_fullbody.transpose()<<endl;
+////  cout<<"torso_rot_acc: "<<torso_rot_acc.transpose()<<endl;
+////  cout<<"torso_rot_vel: "<<torso_rot_vel.transpose()<<endl;
+////  cout<<"torso_rot: "<<torso_rot.transpose()<<endl;
+//
+//
+//  fprintf(hsp->id_log,"zmp_lip: %f %f ",zmp_lip(0),zmp_lip(1),zmp_lip(2));
+//  fprintf(hsp->id_log,"zmp_fullbody: %f %f ",zmp_fullbody(0),zmp_fullbody(1),zmp_fullbody(2));
+//  fprintf(hsp->id_log,"zmp_fullbody2: %f %f ",ref_zmp_invdyn2(0),ref_zmp_invdyn2(1),ref_zmp_invdyn2(2));
+//  fprintf(hsp->id_log,"torso_pos_acc: %f %f ",torso_pos_acc(0),torso_pos_acc(1),torso_pos_acc(2));
+//  fprintf(hsp->id_log,"torso_pos: %f %f ",torso_pos(0),torso_pos(1),torso_pos(2));
+//  fprintf(hsp->id_log,"torso_rot_acc: %f %f ",torso_rot_acc(0),torso_rot_acc(1),torso_rot_acc(2));
+//  fprintf(hsp->id_log,"torso_rot: %f %f ",torso_rot(0),torso_rot(1),torso_rot(2));
+//  fprintf(hsp->id_log,"\n");
+//
+//
+//
+////  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
+////  fik->current_tm = m_qRef.tm;
+////  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+////      fik->ikp[it->first].is_ik_enable = it->second.is_active;
+////  }
+////  if(hsp->isHumanSyncOn()){
+////    if(hsp->ht_first_call){
+////      std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
+////      hsp->ht_first_call = false;
+////    }
+////  }
+////  m_robot->calcForwardKinematics();
+////  // additional COM fitting IK for HumanSynchronizer
+//  if(hsp->isHumanSyncOn()){
+//    HRPPose3D com_mod = hsp->rp_ref_out.getP("com");
+//    com_mod.p += torso_pos;
+//    com_mod.rpy += torso_rot;
+//    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
+//  }
+//}
+
+
+void AutoBalancer::calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody){
+
+
+
+//  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
+//  fik->current_tm = m_qRef.tm;
+//  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+//      fik->ikp[it->first].is_ik_enable = it->second.is_active;
+//  }
+////  // Revert
+//  fik->revertRobotStateToCurrent();
+
+//  hrp::dmatrix Jp;
+//  hrp::dmatrix Jl;
+//  m_robot->calcCM();
+//  m_robot->calcCMJacobian(NULL, Jp); // Eq.1 upper [M/m E -r]
+//  m_robot->calcAngularMomentumJacobian(NULL, Jl); // Eq.1 lower [H 0 I]
+//  Jp *= m_robot->totalMass();
+//  hrp::dmatrix Jpl(Jp.rows() + Jl.rows(), Jp.cols());
+//  Jpl << Jp, Jl;
+//  hrp::dvector all_vel_vec(3+3+m_robot->numJoints());
+//  all_vel_vec << idsb.base_v, idsb.base_w, idsb.dq;
+  hrp::Vector3 P,L;
+  m_robot->rootLink()->v = idsb.base_v;
+  m_robot->rootLink()->w = idsb.base_w;
+  m_robot->calcTotalMomentum(P,L);
+  L = L - m_robot->calcCM().cross(P);
+
+  torso_rot(0) -= L(0) * 0.1 * m_dt;
+  torso_rot(1) -= L(1) * 0.1 * m_dt;
+  torso_rot(2) -= L(2) * 0.1 * m_dt;
+
+
+
+  fprintf(hsp->id_log,"p: %f %f ",hsp->rp_ref_out.getP("com").p(0),hsp->rp_ref_out.getP("com").p(1),hsp->rp_ref_out.getP("com").p(2));
+  fprintf(hsp->id_log,"rpy: %f %f ",hsp->rp_ref_out.getP("com").rpy(0),hsp->rp_ref_out.getP("com").rpy(1),hsp->rp_ref_out.getP("com").rpy(2));
+  fprintf(hsp->id_log,"P: %f %f ",P(0),P(1),P(2));
+  fprintf(hsp->id_log,"L: %f %f ",L(0),L(1),L(2));
+//  fprintf(hsp->id_log,"p_v: %f %f ",idsb.base_v(0),idsb.base_v(1),idsb.base_v(2));
+//  fprintf(hsp->id_log,"rpy_v: %f %f ",idsb.base_w(0),idsb.base_w(1),idsb.base_w(2));
+    fprintf(hsp->id_log,"torso_pos: %f %f ",torso_pos(0),torso_pos(1),torso_pos(2));
+    fprintf(hsp->id_log,"torso_rot: %f %f ",torso_rot(0),torso_rot(1),torso_rot(2));
+  fprintf(hsp->id_log,"\n");
+
+  if(hsp->isHumanSyncOn()){
+    HRPPose3D com_mod = hsp->rp_ref_out.getP("com");
+//    com_mod.p += torso_pos;
+//    com_mod.rpy += torso_rot;
+    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
+  }
+}
+
+
+
+
 void AutoBalancer::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const hrp::Vector3& head_ref){
   int com_ik_loop=0;
-  const int COM_IK_MAX_LOOP = 5;
+  const int COM_IK_MAX_LOOP = 10;
   const double COM_IK_MAX_ERROR = 1e-4;
+
+  cout<<""<<m_robot->rootLink()->p.transpose()<<" : "<<com_ref.p.transpose()<<endl;
+
   m_robot->rootLink()->p = com_ref.p;//move base link at first
+//  m_robot->rootLink()->p = com_ref.p + hsp->torso_rot;//move base link at first
   m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
+
+  //////////invdyncomp
+//  m_robot->rootLink()->R *= hrp::rotFromRpy(hsp->torso_rot);
+//  m_robot->rootLink()->p += torso_rot;
+
   hrp::Vector3 tmp_com_err;
-  if(fik->ikp.count("rleg")){
-    fik->ikp["rleg"].target_r0 = hrp::rotFromRpy(rf_ref.rpy);//convert End Effector pos into link origin pos
-    fik->ikp["rleg"].target_p0 = rf_ref.p;
-  }
-  if(fik->ikp.count("lleg")){
-    fik->ikp["lleg"].target_r0 = hrp::rotFromRpy(lf_ref.rpy);
-    fik->ikp["lleg"].target_p0 = lf_ref.p;
-  }
-  if(fik->ikp.count("rarm")){
-    fik->ikp["rarm"].target_r0 = hrp::rotFromRpy(rh_ref.rpy);
-    fik->ikp["rarm"].target_p0 = rh_ref.p;
-  }
-  if(fik->ikp.count("larm")){
-    fik->ikp["larm"].target_r0 = hrp::rotFromRpy(lh_ref.rpy);
-    fik->ikp["larm"].target_p0 = lh_ref.p;
+  const std::string names[4] = {"rleg","lleg","rarm","larm"};
+  const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
+  for(int i=0;i<4;i++){
+    if(fik->ikp.count(names[i])){
+      fik->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);//convert End Effector pos into link origin pos
+      fik->ikp[names[i]].target_p0 = refs[i]->p;
+    }
   }
   if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref(2);
   if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref(1);
@@ -1284,15 +1466,18 @@ void AutoBalancer::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPP
   while(
       m_robot->calcForwardKinematics(),
       tmp_com_err = com_ref.p - m_robot->calcCM(),
+//      tmp_com_err = com_ref.p + hsp->torso_rot - m_robot->calcCM(),
       tmp_com_err.norm() > COM_IK_MAX_ERROR)
   {
     m_robot->rootLink()->p += tmp_com_err;
     m_robot->calcForwardKinematics();
     for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-        if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, true);
+        if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
     }
     if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
   }
+
+  cout<<"COM_IK_MAX_LOOP:"<<com_ik_loop<<endl;
 }
 
 
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index a57c4863fb6..76cff6768bc 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -545,6 +545,7 @@ class AutoBalancer
   void getTargetParameters();
   void solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const hrp::Vector3& head_ref);
   void processWholeBodyMasterSlave();
+  void calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody);
   void solveFullbodyIK ();
   void startABCparam(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
   void stopABCparam();
@@ -624,6 +625,12 @@ class AutoBalancer
   hrp::InvDynStateBuffer idsb;
   std::vector<IIRFilter> invdyn_zmp_filters;
 
+  hrp::InvDynStateBuffer idsb2;
+  std::vector<IIRFilter> invdyn_zmp_filters2;
+
+
+  hrp::Vector3 ref_zmp_invdyn2;
+
   //for HumanSynchronizer
   boost::shared_ptr<HumanSynchronizer> hsp;
 };
diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/AutoBalancer/HumanMasterSlave.h
index 2ceae682532..726c68cb554 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/AutoBalancer/HumanMasterSlave.h
@@ -286,7 +286,7 @@ class HumanSynchronizer{
       for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0,HZ);//四肢拘束点用(Rotation)
       tgt_pos_filters[0].setParameter(1.0,HZ);//重心pos用
       tgt_rot_filters[0].setParameter(0.6,HZ);//重心rot用
-      tgt_pos_filters[1].setParameter(hrp::Vector3(1.0,1.0,1.2),HZ);//右足pos用
+      tgt_pos_filters[1].setParameter(hrp::Vector3(10.0,10.0,1.2),HZ);//右足pos用
       tgt_pos_filters[2].setParameter(hrp::Vector3(1.0,1.0,1.2),HZ);//左足pos用
       calcacc_v_filters.setParameter(5,HZ);//加速度計算用
       acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
@@ -311,12 +311,14 @@ class HumanSynchronizer{
       WBMSparam.foot_vertical_vel_limit_coeff = 4.0;
       WBMSparam.human_com_height = 1.00;
       WBMSparam.swing_foot_height_offset = 0.01;
-      WBMSparam.swing_foot_max_height = 0.10;
+//      WBMSparam.swing_foot_max_height = 0.10;
+      WBMSparam.swing_foot_max_height = 0.5;
       WBMSparam.auto_swing_foot_landing_threshold = 0.02;
 
       rp_ref_out_old.clear();
       rp_ref_out.clear();
 
+
       init_zmp_com_offset_interpolator = new interpolator(1, DT, interpolator::HOFFARBIB, 1);
       init_zmp_com_offset_interpolator->setName("zmp_com_offset_interpolator");
       double start_ratio = 0.0;
@@ -366,8 +368,11 @@ class HumanSynchronizer{
       convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
       hp_plot = rp_ref_out;
       if(loop==0)rp_ref_out_old = rp_ref_out;
+
+//      rp_ref_out.getP("com").p += torso_rot;
+
       if(isHumanSyncOn() && WBMSparam.set_com_height_fix)rp_ref_out.getP("com").p(Z) = rp_ref_out.getP("com").p_offs(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
-      judgeFootLandOnCommand              (hp_wld_raw.getw("rfw"), hp_wld_raw.getw("lfw"), go_rf_landing, go_lf_landing);//fwはすでに1000->100Hzにフィルタリングされている
+      judgeFootLandOnCommand              (hp_wld_raw.getw("rfw"), hp_wld_raw.getw("lfw"), go_rf_landing, go_lf_landing);
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
       applyEEWorkspaceLimit               (rp_ref_out);
       lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
@@ -399,6 +404,7 @@ class HumanSynchronizer{
       r_zmp_raw = rp_ref_out.getP("zmp").p;
       applyZMPCalcFromCOM                 (rp_ref_out.getP("com").p,rp_ref_out.getP("zmp").p);
 
+
       if(DEBUG)fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.getP("zmp").p(X),rp_ref_out.getP("zmp").p(Y));
       if(DEBUG)fprintf(cz_log,"com_ans2: %f %f ",rp_ref_out.getP("com").p(X),rp_ref_out.getP("com").p(Y));
 
@@ -573,7 +579,8 @@ class HumanSynchronizer{
       }
     }
     void applyEEWorkspaceLimit(HumanPose& tgt){
-      const double MAX_FW = 0.25;
+//      const double MAX_FW = 0.25;
+      const double MAX_FW = 0.5;
       const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
       if(!is_rf_contact && is_lf_contact){//右足浮遊時
         const hrp::Vector2 lf2rf_vec( tgt.getP("rf").p(X)-pre_cont_lfpos(X), tgt.getP("rf").p(Y)-pre_cont_lfpos(Y) );
diff --git a/rtc/CMakeLists.txt b/rtc/CMakeLists.txt
index a64ceda86e1..3a7df0699c3 100644
--- a/rtc/CMakeLists.txt
+++ b/rtc/CMakeLists.txt
@@ -55,6 +55,7 @@ add_subdirectory(Beeper)
 add_subdirectory(ReferenceForceUpdater)
 add_subdirectory(AccelerationFilter)
 add_subdirectory(ObjectContactTurnaroundDetector)
+add_subdirectory(WholeBodyMasterSlave)
 
 if (NOT APPLE AND USE_HRPSYSUTIL)
   add_subdirectory(VideoCapture)
diff --git a/rtc/WholeBodyMasterSlave/CMakeLists.txt b/rtc/WholeBodyMasterSlave/CMakeLists.txt
new file mode 100644
index 00000000000..55d4cd15773
--- /dev/null
+++ b/rtc/WholeBodyMasterSlave/CMakeLists.txt
@@ -0,0 +1,20 @@
+set(comp_sources WholeBodyMasterSlave.cpp WholeBodyMasterSlaveService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp ../TorqueFilter/IIRFilter.cpp ../AutoBalancer/HumanMasterSlave.cpp ../AutoBalancer/AutoBalancer.h)
+set(libs ${OpenCV_LIBRARIES} hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub)
+add_library(WholeBodyMasterSlave SHARED ${comp_sources})
+target_link_libraries(WholeBodyMasterSlave ${libs})
+set_target_properties(WholeBodyMasterSlave PROPERTIES PREFIX "")
+
+add_executable(WholeBodyMasterSlaveComp WholeBodyMasterSlaveComp.cpp ${comp_sources})
+target_link_libraries(WholeBodyMasterSlaveComp ${libs})
+
+include_directories(${PROJECT_SOURCE_DIR}/rtc/SequencePlayer)
+
+set(target WholeBodyMasterSlave WholeBodyMasterSlaveComp )
+
+#add_test(testPreviewControllerNoGP testPreviewController --use-gnuplot false)
+
+install(TARGETS ${target}
+  RUNTIME DESTINATION bin CONFIGURATIONS Release Debug
+  LIBRARY DESTINATION lib CONFIGURATIONS Release Debug
+)
+
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
new file mode 100644
index 00000000000..06147e1f9f5
--- /dev/null
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -0,0 +1,1152 @@
+// -*- C++ -*-
+/*!
+ * @file  WholeBodyMasterSlave.cpp
+ * @brief WholeBodyMasterSlave component
+ * $Date$
+ *
+ * $Id$
+ */
+
+#include <rtm/CorbaNaming.h>
+#include <hrpModel/Link.h>
+#include <hrpModel/Sensor.h>
+#include <hrpModel/ModelLoaderUtil.h>
+#include "WholeBodyMasterSlave.h"
+#include <hrpModel/JointPath.h>
+#include <hrpUtil/MatrixSolvers.h>
+#include "hrpsys/util/Hrpsys.h"
+
+
+typedef coil::Guard<coil::Mutex> Guard;
+using namespace rats;
+
+// Module specification
+// <rtc-template block="module_spec">
+static const char* WholeBodyMasterSlave_spec[] =
+    {
+        "implementation_id", "WholeBodyMasterSlave",
+        "type_name",         "WholeBodyMasterSlave",
+        "description",       "wholebodymasterslave component",
+        "version",           HRPSYS_PACKAGE_VERSION,
+        "vendor",            "AIST",
+        "category",          "example",
+        "activity_type",     "DataFlowComponent",
+        "max_instance",      "10",
+        "language",          "C++",
+        "lang_type",         "compile",
+        // Configuration variables
+        "conf.default.debugLevel", "0",
+        ""
+    };
+// </rtc-template>
+
+WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager)
+    : RTC::DataFlowComponentBase(manager),
+      // <rtc-template block="initializer">
+      m_qRefIn("qRef", m_qRef),
+      m_zmpIn("zmpIn", m_zmp),
+      m_basePosIn("basePosIn", m_basePos),
+      m_baseRpyIn("baseRpyIn", m_baseRpy),
+      m_optionalDataIn("optionalData", m_optionalData),
+      m_emergencySignalIn("emergencySignal", m_emergencySignal),
+      //ishiguro
+      m_htzmpIn("htzmpIn", m_htzmp),
+      m_htrfwIn("htrfwIn", m_htrfw),
+      m_htlfwIn("htlfwIn", m_htlfw),
+      m_htcomIn("htcomIn", m_htcom),
+      m_htrfIn("htrfIn", m_htrf),
+      m_htlfIn("htlfIn", m_htlf),
+      m_htrhIn("htrhIn", m_htrh),
+      m_htlhIn("htlhIn", m_htlh),
+      m_actzmpIn("actzmpIn", m_actzmp),
+      m_htheadIn("htheadIn", m_hthead),
+
+      m_htcom_dbgOut("htcom_dbgOut", m_htcom_dbg),
+      m_htrf_dbgOut("htrf_dbgOut", m_htrf_dbg),
+      m_htlf_dbgOut("htlf_dbgOut", m_htlf_dbg),
+      m_htrh_dbgOut("htrh_dbgOut", m_htrh_dbg),
+      m_htlh_dbgOut("htlh_dbgOut", m_htlh_dbg),
+      m_hthead_dbgOut("hthead_dbgOut", m_hthead_dbg),
+      m_htzmp_dbgOut("htzmp_dbgOut", m_htzmp_dbg),
+      m_htrfw_dbgOut("htrfw_dbgOut", m_htrfw_dbg),
+      m_htlfw_dbgOut("htlfw_dbgOut", m_htlfw_dbg),
+      m_rpcom_dbgOut("rpcom_dbgOut", m_rpcom_dbg),
+      m_rprf_dbgOut("rprf_dbgOut", m_rprf_dbg),
+      m_rplf_dbgOut("rplf_dbgOut", m_rplf_dbg),
+      m_rprh_dbgOut("rprh_dbgOut", m_rprh_dbg),
+      m_rplh_dbgOut("rplh_dbgOut", m_rplh_dbg),
+      m_rphead_dbgOut("rphead_dbgOut", m_rphead_dbg),
+      m_rpzmp_dbgOut("rpzmp_dbgOut", m_rpzmp_dbg),
+      m_rpdcp_dbgOut("rpdcp_dbgOut", m_rpdcp_dbg),
+      m_rpacp_dbgOut("rpacp_dbgOut", m_rpacp_dbg),
+
+      m_invdyn_dbgOut("invdyn_dbgOut", m_invdyn_dbg),
+
+      m_qOut("q", m_qRef),
+      m_zmpOut("zmpOut", m_zmp),
+      m_basePosOut("basePosOut", m_basePos),
+      m_baseRpyOut("baseRpyOut", m_baseRpy),
+      m_optionalDataOut("optionalDataOut", m_optionalData2),
+      m_baseTformOut("baseTformOut", m_baseTform),
+      m_basePoseOut("basePoseOut", m_basePose),
+      m_accRefOut("accRef", m_accRef),
+      m_contactStatesOut("contactStates", m_contactStates),
+      m_toeheelRatioOut("toeheelRatio", m_toeheelRatio),
+      m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime),
+      m_walkingStatesOut("walkingStates", m_walkingStates),
+      m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
+      m_cogOut("cogOut", m_cog),
+      m_WholeBodyMasterSlaveServicePort("WholeBodyMasterSlaveService"),
+      // </rtc-template>
+//      gait_type(BIPED),
+      m_robot(hrp::BodyPtr()),
+      m_debugLevel(0)
+{
+    m_service0.wholebodymasterslave(this);
+}
+
+WholeBodyMasterSlave::~WholeBodyMasterSlave()
+{
+}
+
+
+RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
+{
+    std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
+    bindParameter("debugLevel", m_debugLevel, "0");
+
+    // Registration: InPort/OutPort/Service
+    // <rtc-template block="registration">
+    // Set InPort buffers
+    addInPort("qRef", m_qRefIn);
+    addInPort("zmpIn", m_zmpIn);
+    addInPort("basePosIn", m_basePosIn);
+    addInPort("baseRpyIn", m_baseRpyIn);
+    addInPort("optionalData", m_optionalDataIn);
+    addInPort("emergencySignal", m_emergencySignalIn);
+    //ishiguro
+    addInPort("htzmpIn", m_htzmpIn);
+    addInPort("htrfwIn", m_htrfwIn);
+    addInPort("htlfwIn", m_htlfwIn);
+    addInPort("htcomIn", m_htcomIn);
+    addInPort("htrfIn", m_htrfIn);
+    addInPort("htlfIn", m_htlfIn);
+    addInPort("htrhIn", m_htrhIn);
+    addInPort("htlhIn", m_htlhIn);
+    addInPort("actzmpIn", m_actzmpIn);
+    addInPort("htheadIn", m_htheadIn);
+
+    addOutPort("htcom_dbgOut", m_htcom_dbgOut);
+    addOutPort("htrf_dbgOut", m_htrf_dbgOut);
+    addOutPort("htlf_dbgOut", m_htlf_dbgOut);
+    addOutPort("htrh_dbgOut", m_htrh_dbgOut);
+    addOutPort("htlh_dbgOut", m_htlh_dbgOut);
+    addOutPort("hthead_dbgOut", m_hthead_dbgOut);
+    addOutPort("htzmp_dbgOut", m_htzmp_dbgOut);
+    addOutPort("htrfw_dbgOut", m_htrfw_dbgOut);
+    addOutPort("htlfw_dbgOut", m_htlfw_dbgOut);
+    addOutPort("rpcom_dbgOut", m_rpcom_dbgOut);
+    addOutPort("rprf_dbgOut", m_rprf_dbgOut);
+    addOutPort("rplf_dbgOut", m_rplf_dbgOut);
+    addOutPort("rprh_dbgOut", m_rprh_dbgOut);
+    addOutPort("rplh_dbgOut", m_rplh_dbgOut);
+    addOutPort("rphead_dbgOut", m_rphead_dbgOut);
+    addOutPort("rpzmp_dbgOut", m_rpzmp_dbgOut);
+    addOutPort("rpdcp_dbgOut", m_rpdcp_dbgOut);
+    addOutPort("rpacp_dbgOut", m_rpacp_dbgOut);
+
+    addOutPort("invdyn_dbgOut", m_invdyn_dbgOut);
+
+    // Set OutPort buffer
+    addOutPort("q", m_qOut);
+    addOutPort("zmpOut", m_zmpOut);
+    addOutPort("basePosOut", m_basePosOut);
+    addOutPort("baseRpyOut", m_baseRpyOut);
+    addOutPort("optionalDataOut", m_optionalDataOut);
+//    addOutPort("baseTformOut", m_baseTformOut);
+//    addOutPort("basePoseOut", m_basePoseOut);
+//    addOutPort("accRef", m_accRefOut);
+//    addOutPort("contactStates", m_contactStatesOut);
+//    addOutPort("toeheelRatio", m_toeheelRatioOut);
+//    addOutPort("controlSwingSupportTime", m_controlSwingSupportTimeOut);
+//    addOutPort("cogOut", m_cogOut);
+//    addOutPort("walkingStates", m_walkingStatesOut);
+//    addOutPort("sbpCogOffset", m_sbpCogOffsetOut);
+  
+    // Set service provider to Ports
+    m_WholeBodyMasterSlaveServicePort.registerProvider("service0", "WholeBodyMasterSlaveService", m_service0);
+  
+    // Set service consumers to Ports
+  
+    // Set CORBA Service Ports
+    addPort(m_WholeBodyMasterSlaveServicePort);
+  
+    // </rtc-template>
+    // <rtc-template block="bind_config">
+    // Bind variables and configuration variable
+  
+    // </rtc-template>
+
+    RTC::Properties& prop =  getProperties();
+    coil::stringTo(m_dt, prop["dt"].c_str());
+
+    m_robot = hrp::BodyPtr(new hrp::Body());
+    RTC::Manager& rtcManager = RTC::Manager::instance();
+    std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
+    int comPos = nameServer.find(",");
+    if (comPos < 0){
+        comPos = nameServer.length();
+    }
+    nameServer = nameServer.substr(0, comPos);
+    RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
+    if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
+                                 CosNaming::NamingContext::_duplicate(naming.getRootContext())
+                                 )){
+      std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
+      return RTC::RTC_ERROR;
+    }
+
+    // allocate memory for outPorts
+    m_qRef.data.length(m_robot->numJoints());
+    m_baseTform.data.length(12);
+    //ishiguro
+    m_htrfw.data.length(6);
+    m_htlfw.data.length(6);
+
+    m_optionalData2.data.length(4*2);//これいいのか？
+    for(int i=0;i<4*2;i++)m_optionalData2.data[i] = 0;
+
+//    control_mode = MODE_IDLE;
+    loop = 0;
+//
+//    // setting from conf file
+//    // GaitGenerator requires abc_leg_offset and abc_stride_parameter in robot conf file
+//    // setting leg_pos from conf file
+//    coil::vstring leg_offset_str = coil::split(prop["abc_leg_offset"], ",");
+//    leg_names.push_back("rleg");
+//    leg_names.push_back("lleg");
+//
+    // Generate FIK
+    fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
+
+    // setting from conf file
+    // rleg,TARGET_LINK,BASE_LINK
+    coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
+    size_t prop_num = 10;
+    if (end_effectors_str.size() > 0) {
+      size_t num = end_effectors_str.size()/prop_num;
+      for (size_t i = 0; i < num; i++) {
+        std::string ee_name, ee_target, ee_base;
+        coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
+        coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
+        coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
+        ABCIKparam tp;
+        for (size_t j = 0; j < 3; j++) {
+            coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
+        }
+        double tmpv[4];
+        for (int j = 0; j < 4; j++ ) {
+          coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
+        }
+        tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
+        // FIK param
+        SimpleFullbodyInverseKinematicsSolver::IKparam tmp_fikp;
+        tmp_fikp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
+        tmp_fikp.target_link = m_robot->link(ee_target);
+        tmp_fikp.localPos = tp.localPos;
+        tmp_fikp.localR = tp.localR;
+        fik->ikp.insert(std::pair<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>(ee_name, tmp_fikp));
+        // Fix for toe joint
+        //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
+        //   Without toe joints, "end-effector link == force-sensor link" is assumed.
+        //   With toe joints, "end-effector link != force-sensor link" is assumed.
+        if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
+            std::vector<double> optw(fik->ikp[ee_name].manip->numJoints(), 1.0);
+            optw.back() = 0.0; // Set weight = 0 for toe joint by default
+            fik->ikp[ee_name].manip->setOptionalWeightVector(optw);
+            tp.has_toe_joint = true;
+        } else {
+            tp.has_toe_joint = false;
+        }
+        tp.target_link = m_robot->link(ee_target);
+//        ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
+//        ee_vec.push_back(ee_name);
+        std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   target = " << fik->ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
+        contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
+      }
+//      m_contactStates.data.length(num);
+//      m_toeheelRatio.data.length(num);
+//      if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
+//        m_contactStates.data[contact_states_index_map["rleg"]] = true;
+//        m_contactStates.data[contact_states_index_map["lleg"]] = true;
+//      }
+//      if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) {
+//        m_contactStates.data[contact_states_index_map["rarm"]] = false;
+//        m_contactStates.data[contact_states_index_map["larm"]] = false;
+//      }
+//      m_controlSwingSupportTime.data.length(num);
+//      for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 0.0;
+//      for (size_t i = 0; i < num; i++) m_toeheelRatio.data[i] = rats::no_using_toe_heel_ratio;
+    }
+//    std::vector<hrp::Vector3> leg_pos;
+//    if (leg_offset_str.size() > 0) {
+//      hrp::Vector3 leg_offset;
+//      for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
+//      std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+//      leg_pos.push_back(hrp::Vector3(-1*leg_offset));
+//      leg_pos.push_back(hrp::Vector3(leg_offset));
+//    }
+//    if (leg_pos.size() < ikp.size()) {
+//        size_t tmp_leg_pos_size = leg_pos.size();
+//        for (size_t i = 0; i < ikp.size() - tmp_leg_pos_size; i++) {
+//            leg_pos.push_back(hrp::Vector3::Zero());
+//        }
+//    }
+//
+//    std::vector<std::pair<hrp::Link*, hrp::Link*> > interlocking_joints;
+//    readInterlockingJointsParamFromProperties(interlocking_joints, m_robot, prop["interlocking_joints"], std::string(m_profile.instance_name));
+//    if (interlocking_joints.size() > 0) {
+//        fik->initializeInterlockingJoints(interlocking_joints);
+//    }
+//
+//    zmp_offset_interpolator = new interpolator(ikp.size()*3, m_dt);
+//    zmp_offset_interpolator->setName(std::string(m_profile.instance_name)+" zmp_offset_interpolator");
+//    zmp_transition_time = 1.0;
+//    transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+//    transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator");
+//    transition_interpolator_ratio = 0.0;
+//    adjust_footstep_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+//    adjust_footstep_interpolator->setName(std::string(m_profile.instance_name)+" adjust_footstep_interpolator");
+//    transition_time = 2.0;
+//    adjust_footstep_transition_time = 2.0;
+//    leg_names_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+//    leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator");
+//    leg_names_interpolator_ratio = 1.0;
+//
+//    // setting stride limitations from conf file
+//    double stride_fwd_x_limit = 0.15;
+//    double stride_y_limit = 0.05;
+//    double stride_th_limit = 10;
+//    double stride_bwd_x_limit = 0.05;
+//    std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_y_limit << "[m], " << stride_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl;
+//    if (default_zmp_offsets.size() == 0) {
+//      for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
+//    }
+//    if (leg_offset_str.size() > 0) {
+//      gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_y_limit/*[m]*/, stride_th_limit/*[deg]*/, stride_bwd_x_limit/*[m]*/));
+//      gg->set_default_zmp_offsets(default_zmp_offsets);
+//    }
+//    gg_is_walking = gg_solved = false;
+//    m_walkingStates.data = false;
+//    fix_leg_coords = coordinates();
+//
+//    // load virtual force sensors
+//    readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
+//    // ref force port
+//    unsigned int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
+//    unsigned int nvforce = m_vfs.size();
+//    unsigned int nforce  = npforce + nvforce;
+//    // check number of force sensors
+//    if (nforce < m_contactStates.data.length()) {
+//        std::cerr << "[" << m_profile.instance_name << "] WARNING! This robot model has less force sensors(" << nforce;
+//        std::cerr << ") than end-effector settings(" << m_contactStates.data.length() << ") !" << std::endl;
+//    }
+//
+//    m_ref_force.resize(nforce);
+//    m_ref_forceIn.resize(nforce);
+//    m_force.resize(nforce);
+//    m_ref_forceOut.resize(nforce);
+//    m_limbCOPOffset.resize(nforce);
+//    m_limbCOPOffsetOut.resize(nforce);
+//    for (unsigned int i=0; i<npforce; i++){
+//        sensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
+//    }
+//    for (unsigned int i=0; i<nvforce; i++){
+//        for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
+//            if (it->second.id == (int)i) sensor_names.push_back(it->first);
+//        }
+//    }
+//    // set ref force port
+//    std::cerr << "[" << m_profile.instance_name << "] force sensor ports (" << nforce << ")" << std::endl;
+//    for (unsigned int i=0; i<nforce; i++){
+//        m_ref_forceIn[i] = new InPort<TimedDoubleSeq>(std::string("ref_"+sensor_names[i]).c_str(), m_ref_force[i]);
+//        m_ref_force[i].data.length(6);
+//        registerInPort(std::string("ref_"+sensor_names[i]).c_str(), *m_ref_forceIn[i]);
+//        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string("ref_"+sensor_names[i]) << std::endl;
+//        ref_forces.push_back(hrp::Vector3(0,0,0));
+//    }
+//    // set force port
+//    for (unsigned int i=0; i<nforce; i++){
+//        m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string(sensor_names[i]).c_str(), m_force[i]);
+//        m_force[i].data.length(6);
+//        m_force[i].data[0] = m_force[i].data[1] = m_force[i].data[2] = 0.0;
+//        m_force[i].data[3] = m_force[i].data[4] = m_force[i].data[5] = 0.0;
+//        registerOutPort(std::string(sensor_names[i]).c_str(), *m_ref_forceOut[i]);
+//        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string(sensor_names[i]) << std::endl;
+//    }
+//    // set limb cop offset port
+//    std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << nforce << ")" << std::endl;
+//    for (unsigned int i=0; i<nforce; i++){
+//        std::string nm("limbCOPOffset_"+sensor_names[i]);
+//        m_limbCOPOffsetOut[i] = new OutPort<TimedPoint3D>(nm.c_str(), m_limbCOPOffset[i]);
+//        registerOutPort(nm.c_str(), *m_limbCOPOffsetOut[i]);
+//        m_limbCOPOffset[i].data.x = m_limbCOPOffset[i].data.y = m_limbCOPOffset[i].data.z = 0.0;
+//        std::cerr << "[" << m_profile.instance_name << "]   name = " << nm << std::endl;
+//    }
+//    sbp_offset = hrp::Vector3(0,0,0);
+//    sbp_cog_offset = hrp::Vector3(0,0,0);
+//    //use_force = MODE_NO_FORCE;
+//    use_force = MODE_REF_FORCE;
+//
+    if (fik->ikp.find("rleg") != fik->ikp.end() && fik->ikp.find("lleg") != fik->ikp.end()) {
+      is_legged_robot = true;
+    } else {
+      is_legged_robot = false;
+    }
+//
+//    m_accRef.data.ax = m_accRef.data.ay = m_accRef.data.az = 0.0;
+//    prev_imu_sensor_vel = hrp::Vector3::Zero();
+//
+//    graspless_manip_mode = false;
+//    graspless_manip_arm = "arms";
+//    graspless_manip_p_gain = hrp::Vector3::Zero();
+//
+//    is_stop_mode = false;
+//    is_hand_fix_mode = false;
+
+    //for HumanSynchronizer
+    hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer());
+//    if(leg_pos.size()>=2){ hsp->init_wld_rp_rfeepos = leg_pos[0]; hsp->init_wld_rp_lfeepos = leg_pos[1]; }
+
+//    hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
+//    if (sen == NULL) {
+//        std::cerr << "[" << m_profile.instance_name << "] WARNING! This robot model has no GyroSensor named 'gyrometer'! " << std::endl;
+//    }
+    return RTC::RTC_OK;
+}
+
+
+RTC::ReturnCode_t WholeBodyMasterSlave::onFinalize()
+{
+//  delete zmp_offset_interpolator;
+//  delete transition_interpolator;
+//  delete adjust_footstep_interpolator;
+//  delete leg_names_interpolator;
+  return RTC::RTC_OK;
+}
+
+RTC::ReturnCode_t WholeBodyMasterSlave::onActivated(RTC::UniqueId ec_id)
+{
+    std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
+    
+    return RTC::RTC_OK;
+}
+
+RTC::ReturnCode_t WholeBodyMasterSlave::onDeactivated(RTC::UniqueId ec_id)
+{
+  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
+//  Guard guard(m_mutex);
+//  if (control_mode == MODE_ABC) {
+//    control_mode = MODE_SYNC_TO_IDLE;
+//    double tmp_ratio = 0.0;
+//    transition_interpolator->setGoal(&tmp_ratio, m_dt, true); // sync in one controller loop
+//  }
+  return RTC::RTC_OK;
+}
+
+
+
+
+#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
+//#define DEBUGP2 ((loop%200==0))
+#define DEBUGP2 (false)
+RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
+{
+  // std::cerr << "WholeBodyMasterSlave::onExecute(" << ec_id << ")" << std::endl;
+    loop ++;
+
+    // Read Inport
+    if (m_qRefIn.isNew()) {
+        m_qRefIn.read();
+    }
+    if (m_basePosIn.isNew()) {
+      m_basePosIn.read();
+      input_basePos(0) = m_basePos.data.x;
+      input_basePos(1) = m_basePos.data.y;
+      input_basePos(2) = m_basePos.data.z;
+    }
+    if (m_baseRpyIn.isNew()) {
+      m_baseRpyIn.read();
+      input_baseRot = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
+    }
+    if (m_zmpIn.isNew()) {
+      m_zmpIn.read();
+//      input_zmp(0) = m_zmp.data.x;
+//      input_zmp(1) = m_zmp.data.y;
+//      input_zmp(2) = m_zmp.data.z;
+    }
+    for (unsigned int i=0; i<m_ref_forceIn.size(); i++){
+        if ( m_ref_forceIn[i]->isNew() ) {
+            m_ref_forceIn[i]->read();
+        }
+    }
+    if (m_optionalDataIn.isNew()) {
+        m_optionalDataIn.read();
+    }
+    if (m_emergencySignalIn.isNew()){
+        m_emergencySignalIn.read();
+    }
+    //for HumanSynchronizer
+    if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
+    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hsp->hp_wld_raw.getw("rfw")); }
+    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hsp->hp_wld_raw.getw("lfw")); }
+    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_htcom.data,hsp->hp_wld_raw.getP("com")); }
+    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrf.data,hsp->hp_wld_raw.getP("rf")); }
+    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlf.data,hsp->hp_wld_raw.getP("lf")); }
+    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrh.data,hsp->hp_wld_raw.getP("rh"));}
+    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlh.data,hsp->hp_wld_raw.getP("lh"));}
+    if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_hthead.data,hsp->head_cam_pose);}
+    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
+
+//    if(ikp.count("rarm"))ikp["rarm"].is_active = false;
+//    if(ikp.count("larm"))ikp["larm"].is_active = false;
+//    m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
+//    m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
+//
+//
+//    // Calculation
+//    Guard guard(m_mutex);
+//    hrp::Vector3 ref_basePos;
+//    hrp::Matrix33 ref_baseRot;
+//    hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
+    if ( is_legged_robot ) {
+//      // For parameters
+//      fik->storeCurrentParameters();
+//      getTargetParameters();
+//      // Get transition ratio
+//      bool is_transition_interpolator_empty = transition_interpolator->isEmpty();
+//      if (!is_transition_interpolator_empty) {
+//        transition_interpolator->get(&transition_interpolator_ratio, true);
+//      } else {
+//        transition_interpolator_ratio = (control_mode == MODE_IDLE) ? 0.0 : 1.0;
+//      }
+        hsp->baselinkpose.p = m_robot->rootLink()->p;
+        hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
+        if(!hsp->isHumanSyncOn()){
+          m_robot->rootLink()->p = input_basePos;
+          m_robot->rootLink()->R = input_baseRot;
+          for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
+            m_robot->joint(i)->q = m_qRef.data[i];
+          }
+
+//          cout<<"m_robot->calcCM()"<<m_robot->calcCM().transpose()<<endl;
+//          cout<<"m_robot->p"<<m_robot->rootLink()->p.transpose()<<endl;
+//          cout<<"m_robot->R"<<m_robot->rootLink()->R<<endl;
+
+          m_robot->calcForwardKinematics();
+//          solveFullbodyIK();
+        }
+
+        processWholeBodyMasterSlave();
+
+        /////// Inverse Dynamics /////////
+//        if(!idsb.is_initialized){
+//          idsb.setInitState(m_robot, m_dt);
+//          invdyn_zmp_filters.resize(3);
+//          for(int i=0;i<3;i++){
+//            invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
+//            invdyn_zmp_filters[i].reset(ref_zmp(i));
+//          }
+//          idsb2.setInitState(m_robot, m_dt);
+//          invdyn_zmp_filters2.resize(3);
+//          for(int i=0;i<3;i++){
+//            invdyn_zmp_filters2[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
+//            invdyn_zmp_filters2[i].reset(ref_zmp(i));
+//          }
+//        }
+//        calcAccelerationsForInverseDynamics(m_robot, idsb);
+//        hrp::Vector3 ref_zmp_invdyn;
+//        calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);
+//        for(int i=0;i<3;i++) ref_zmp_invdyn(i) = invdyn_zmp_filters[i].passFilter(ref_zmp_invdyn(i));
+//        updateInvDynStateBuffer(idsb);
+
+
+//        calcDynamicsFilterCompensation(ref_zmp, ref_zmp_invdyn);
+
+
+//        calcAccelerationsForInverseDynamics(m_robot, idsb2);
+//        calcWorldZMPFromInverseDynamics(m_robot, idsb2, ref_zmp_invdyn2);
+//        for(int i=0;i<3;i++) ref_zmp_invdyn2(i) = invdyn_zmp_filters2[i].passFilter(ref_zmp_invdyn2(i));
+//        updateInvDynStateBuffer(idsb2);
+
+
+//        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
+//      // Transition
+//      if (!is_transition_interpolator_empty) {
+//        // transition_interpolator_ratio 0=>1 : IDLE => ABC
+//        // transition_interpolator_ratio 1=>0 : ABC => IDLE
+//        ref_basePos = (1-transition_interpolator_ratio) * input_basePos + transition_interpolator_ratio * m_robot->rootLink()->p;
+//        rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp;
+//        rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R);
+//        for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ) {
+//          m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q;
+//        }
+//        for (unsigned int i=0; i< m_force.size(); i++) {
+//            for (unsigned int j=0; j<6; j++) {
+//                m_force[i].data[j] = transition_interpolator_ratio * m_force[i].data[j] + (1-transition_interpolator_ratio) * m_ref_force[i].data[j];
+//            }
+//        }
+//        for (unsigned int i=0; i< m_limbCOPOffset.size(); i++) {
+//            // transition (TODO:set stopABCmode value instead of 0)
+//            m_limbCOPOffset[i].data.x = transition_interpolator_ratio * m_limbCOPOffset[i].data.x;// + (1-transition_interpolator_ratio) * 0;
+//            m_limbCOPOffset[i].data.y = transition_interpolator_ratio * m_limbCOPOffset[i].data.y;// + (1-transition_interpolator_ratio) * 0;
+//            m_limbCOPOffset[i].data.z = transition_interpolator_ratio * m_limbCOPOffset[i].data.z;// + (1-transition_interpolator_ratio) * 0;
+//        }
+//      } else {
+//        ref_basePos = m_robot->rootLink()->p;
+//        ref_baseRot = m_robot->rootLink()->R;
+//      }
+//      // mode change for sync
+//      if (control_mode == MODE_SYNC_TO_ABC) {
+//        control_mode = MODE_ABC;
+//      } else if (control_mode == MODE_SYNC_TO_IDLE && transition_interpolator->isEmpty() ) {
+//        std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
+//                  << "] Finished cleanup" << std::endl;
+//        control_mode = MODE_IDLE;
+//      }
+    }
+//
+    // Write Outport
+    if ( m_qRef.data.length() != 0 ) { // initialized
+      if (is_legged_robot) {
+        for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
+          m_qRef.data[i] = m_robot->joint(i)->q;
+        }
+      }
+      m_qOut.write();
+    }
+    if (is_legged_robot) {
+      // basePos
+      m_basePos.data.x = m_robot->rootLink()->p(0);
+      m_basePos.data.y = m_robot->rootLink()->p(1);
+      m_basePos.data.z = m_robot->rootLink()->p(2);
+      m_basePos.tm = m_qRef.tm;
+      // baseRpy
+      hrp::Vector3 baseRpy = hrp::rpyFromRot(m_robot->rootLink()->R);
+      m_baseRpy.data.r = baseRpy(0);
+      m_baseRpy.data.p = baseRpy(1);
+      m_baseRpy.data.y = baseRpy(2);
+      m_baseRpy.tm = m_qRef.tm;
+//      // baseTform
+//      double *tform_arr = m_baseTform.data.get_buffer();
+//      tform_arr[0] = m_basePos.data.x;
+//      tform_arr[1] = m_basePos.data.y;
+//      tform_arr[2] = m_basePos.data.z;
+//      hrp::setMatrix33ToRowMajorArray(ref_baseRot, tform_arr, 3);
+//      m_baseTform.tm = m_qRef.tm;
+//      // basePose
+//      m_basePose.data.position.x = m_basePos.data.x;
+//      m_basePose.data.position.y = m_basePos.data.y;
+//      m_basePose.data.position.z = m_basePos.data.z;
+//      m_basePose.data.orientation.r = m_baseRpy.data.r;
+//      m_basePose.data.orientation.p = m_baseRpy.data.p;
+//      m_basePose.data.orientation.y = m_baseRpy.data.y;
+//      m_basePose.tm = m_qRef.tm;
+      // zmp
+      m_zmp.data.x = rel_ref_zmp(0);
+      m_zmp.data.y = rel_ref_zmp(1);
+      m_zmp.data.z = rel_ref_zmp(2);
+      m_zmp.tm = m_qRef.tm;
+//      // cog
+//      m_cog.data.x = ref_cog(0);
+//      m_cog.data.y = ref_cog(1);
+//      m_cog.data.z = ref_cog(2);
+//      m_cog.tm = m_qRef.tm;
+//      // sbpCogOffset
+//      m_sbpCogOffset.data.x = sbp_cog_offset(0);
+//      m_sbpCogOffset.data.y = sbp_cog_offset(1);
+//      m_sbpCogOffset.data.z = sbp_cog_offset(2);
+//      m_sbpCogOffset.tm = m_qRef.tm;
+//      // write
+      m_basePosOut.write();
+      m_baseRpyOut.write();
+//      m_baseTformOut.write();
+//      m_basePoseOut.write();
+      m_zmpOut.write();
+      m_optionalDataOut.write();
+//      m_cogOut.write();
+//      m_sbpCogOffsetOut.write();
+//
+//      // reference acceleration
+//      hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
+//      if (sen != NULL) {
+//          hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos;
+//          hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt;
+//          // convert to imu sensor local acceleration
+//          hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt;
+//          m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2);
+//          m_accRefOut.write();
+//          prev_imu_sensor_pos = imu_sensor_pos;
+//          prev_imu_sensor_vel = imu_sensor_vel;
+//      }
+//
+//      // control parameters
+//      m_contactStates.tm = m_qRef.tm;
+//      m_contactStatesOut.write();
+//      m_controlSwingSupportTime.tm = m_qRef.tm;
+//      m_controlSwingSupportTimeOut.write();
+//      m_toeheelRatio.tm = m_qRef.tm;
+//      m_toeheelRatioOut.write();
+//      m_walkingStates.data = gg_is_walking;
+//      m_walkingStates.tm = m_qRef.tm;
+//      m_walkingStatesOut.write();
+//
+//      for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
+//          m_force[i].tm = m_qRef.tm;
+//          m_ref_forceOut[i]->write();
+//      }
+//
+//      for (unsigned int i=0; i<m_limbCOPOffsetOut.size(); i++){
+//          m_limbCOPOffset[i].tm = m_qRef.tm;
+//          m_limbCOPOffsetOut[i]->write();
+//      }
+    }
+
+    //ishiguro dbg plot
+    m_htcom.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("com"),m_htcom_dbg.data);
+    m_htcom_dbgOut.write();
+    m_htrf.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("rf"),m_htrf_dbg.data);
+    m_htrf_dbgOut.write();
+    m_htlf.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("lf"),m_htlf_dbg.data);
+    m_htlf_dbgOut.write();
+    m_htrh.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("rh"),m_htrh_dbg.data);
+    m_htrh_dbgOut.write();
+    m_htlh.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("lh"),m_htlh_dbg.data);
+    m_htlh_dbgOut.write();
+    m_hthead.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("head"),m_hthead_dbg.data);
+    m_hthead_dbgOut.write();
+    m_htzmp.tm = m_qRef.tm;
+    HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.getP("zmp").p,m_rpzmp_dbg.data);
+    m_htzmp_dbgOut.write();
+    m_htrfw.tm = m_qRef.tm;
+    m_htrfw_dbg.data.length(6);
+    HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.getw("rfw"),m_htrfw_dbg.data);
+    m_htrfw_dbgOut.write();
+    m_htlfw.tm = m_qRef.tm;
+    m_htlfw_dbg.data.length(6);
+    HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.getw("lfw"),m_htlfw_dbg.data);
+    m_htlfw_dbgOut.write();
+    m_rpcom_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("com"),m_rpcom_dbg.data);
+    m_rpcom_dbgOut.write();
+    m_rprf_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("rf"),m_rprf_dbg.data);
+    m_rprf_dbgOut.write();
+    m_rplf_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("lf"),m_rplf_dbg.data);
+    m_rplf_dbgOut.write();
+    m_rprh_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("rh"),m_rprh_dbg.data);
+    m_rprh_dbgOut.write();
+    m_rplh_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("lh"),m_rplh_dbg.data);
+    m_rplh_dbgOut.write();
+    m_rphead_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("head"),m_rphead_dbg.data);
+    m_rphead_dbgOut.write();
+    m_rpzmp_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.getP("zmp").p,m_rpzmp_dbg.data);
+    m_rpzmp_dbgOut.write();
+    m_rpdcp_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::Vector3ToPoint3D(hsp->cp_dec,m_rpdcp_dbg.data);
+    m_rpdcp_dbgOut.write();
+    m_rpacp_dbg.tm = m_qRef.tm;
+    HumanSynchronizer::Vector3ToPoint3D(hsp->cp_acc,m_rpacp_dbg.data);
+    m_rpacp_dbgOut.write();
+    m_invdyn_dbg.tm = m_qRef.tm;
+    m_invdyn_dbg.data.length(6);
+    HumanSynchronizer::Wrench6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
+    m_invdyn_dbgOut.write();
+
+    return RTC::RTC_OK;
+}
+
+void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
+  // Set ik target params
+//  fik->target_root_p = target_root_p;
+//  fik->target_root_R = target_root_R;
+//  for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
+//      it->second.target_p0 = ikp[it->first].target_p0;
+//      it->second.target_r0 = ikp[it->first].target_r0;
+//  }
+//  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
+  fik->current_tm = m_qRef.tm;
+//  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+//      fik->ikp[it->first].is_ik_enable = it->second.is_active;
+//  }
+
+  fik->ikp["rleg"].is_ik_enable = true;
+  fik->ikp["lleg"].is_ik_enable = true;
+  fik->ikp["rarm"].is_ik_enable = true;
+  fik->ikp["larm"].is_ik_enable = true;
+
+
+//  // Revert
+//  fik->revertRobotStateToCurrent();
+
+  if(hsp->startCountdownForHumanSync){
+    std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
+    hsp->updateCountDown();
+  }
+  if(hsp->isHumanSyncOn()){
+    if(hsp->ht_first_call){
+      std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
+      hsp->ht_first_call = false;
+    }
+  }else{
+//    fik->revertRobotStateToCurrent();
+    fik->storeCurrentParameters();
+    fik->setReferenceJointAngles();
+    hsp->setCurrentInputAsOffset(hsp->hp_wld_raw);
+    hsp->calibInitHumanCOMFromZMP();
+
+    hsp->rp_ref_out.getP("com").p_offs = m_robot->calcCM();
+//    hsp->rp_ref_out.getP("zmp").p_offs = ref_zmp;
+    const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"}, human_l_names[4] = {"rf","lf","rh","lh"};
+    for(int i=0;i<4;i++){
+      if(fik->ikp.count(robot_l_names[i])){
+        hsp->rp_ref_out.getP(human_l_names[i]).p_offs = fik->ikp[robot_l_names[i]].target_link->p + fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localPos;
+        hsp->rp_ref_out.getP(human_l_names[i]).rpy_offs = hrp::rpyFromRot(fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localR);
+        hsp->rp_ref_out.getP(human_l_names[i]).p = hsp->rp_ref_out.getP(human_l_names[i]).p_offs;
+        hsp->rp_ref_out.getP(human_l_names[i]).rpy = hsp->rp_ref_out.getP(human_l_names[i]).rpy_offs;
+      }
+    }
+//
+//    cout<<"r_"<<hsp->rp_ref_out.getP("rh").rpy.transpose()<<endl;
+//    cout<<"r_offs"<<hsp->rp_ref_out.getP("rh").rpy_offs.transpose()<<endl;
+//    cout<<"l_"<<hsp->rp_ref_out.getP("lh").rpy.transpose()<<endl;
+//    cout<<"l_offs"<<hsp->rp_ref_out.getP("lh").rpy_offs.transpose()<<endl;
+
+
+    hsp->pre_cont_rfpos = hsp->rp_ref_out.getP("rf").p_offs;
+    hsp->pre_cont_lfpos = hsp->rp_ref_out.getP("lf").p_offs;
+    hsp->baselinkpose.p_offs = m_robot->rootLink()->p;
+    hsp->baselinkpose.rpy_offs = hrp::rpyFromRot(m_robot->rootLink()->R);
+  }
+  hrp::Vector3 rsole_pos = fik->ikp["rleg"].target_link->p+ fik->ikp["rleg"].target_link->R * fik->ikp["rleg"].localPos;
+  hrp::Vector3 lsole_pos = fik->ikp["lleg"].target_link->p+ fik->ikp["lleg"].target_link->R * fik->ikp["lleg"].localPos;
+  hsp->H_cur = m_robot->calcCM()(2) - std::min((double)rsole_pos(2), (double)lsole_pos(2));
+  hsp->update();//////HumanSynchronizerの主要処理
+  if(loop%100==0)hsp->rp_ref_out.print();
+  m_robot->calcForwardKinematics();
+  // additional COM fitting IK for HumanSynchronizer
+  if(hsp->isHumanSyncOn()){
+
+//    for(int i=0;i<m_robot->numJoints();i++){ cout <<m_robot->joint(i)->name<<" "<<m_robot->joint(i)->q<<endl; }
+    solveFullbodyIKStrictCOM( hsp->rp_ref_out.getP("com"), hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
+
+//    for(int i=0;i<m_robot->numJoints();i++){ cout <<m_robot->joint(i)->name<<" "<<m_robot->joint(i)->q<<endl; }
+//    exit();
+    //outport用のデータ上書き
+//    ref_zmp = hsp->rp_ref_out.getP("zmp").p;
+//    ref_cog = hsp->rp_ref_out.getP("com").p;
+
+    rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.getP("zmp").p - m_robot->rootLink()->p);
+
+//    m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
+//    m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
+//
+//
+//    for ( std::map<std::string, ABCIKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
+//        m_contactStates.data[contact_states_index_map[it->first]] = isOptionalDataContact(it->first);
+//        m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = m_optionalData.data[contact_states_index_map[it->first]+contact_states_index_map.size()];
+//    }
+//
+    m_optionalData2.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
+    m_optionalData2.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
+
+
+//    for(int i=0;i<m_optionalData2.data.length();i++){
+//      cout<<"m_optionalData2.data["<<i<<"]:"<<m_optionalData2.data[i]<<endl;
+//    }
+
+//        cout<<"rleg"<<m_optionalData.data[contact_states_index_map["rleg"]]<<endl;
+//        cout<<"lleg"<<m_optionalData.data[contact_states_index_map["lleg"]]<<endl;
+
+  }else{
+    rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.getP("zmp").p - m_robot->rootLink()->p);
+  }
+
+//  cout<<"len2:"<<m_optionalData2.data.length()<<endl;
+
+
+
+
+
+//  for(int i=0;i<m_robot->numJoints();i++){ cout <<m_robot->joint(i)->name<<" "<<m_robot->joint(i)->q<<endl; }
+}
+//
+//
+//hrp::Vector3 torso_pos = hrp::Vector3::Zero();
+//hrp::Vector3 torso_rot = hrp::Vector3::Zero();
+////void WholeBodyMasterSlave::calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody){
+////  hrp::Vector3 torso_pos_acc,torso_rot_acc;
+////  static hrp::Vector3 torso_pos_vel,torso_rot_vel = hrp::Vector3::Zero();
+////  hrp::Matrix33 total_inertia;
+////  for(int i=0; i<m_robot->numLinks(); i++){
+////    total_inertia += m_robot->link(i)->I;
+////  }
+////  hrp::Vector3 zmp_err = zmp_fullbody - zmp_lip;
+////  if(fabs(zmp_err(0))<0.002)zmp_err(0)=0;
+////  if(fabs(zmp_err(1))<0.002)zmp_err(1)=0;
+////  LIMIT_MINMAX(zmp_err(0),-0.05,0.05);
+////  LIMIT_MINMAX(zmp_err(1),-0.05,0.05);
+////  zmp_err(2)=0;
+////  torso_pos_acc(0) = zmp_err(0) / 1.1 * 9.8;
+////  torso_pos_acc(1) = zmp_err(1) / 1.1 * 9.8;
+////  torso_rot_acc(0) = - zmp_err(1) * m_robot->totalMass() * 9.8 / (130*0.2*0.2);
+////  torso_rot_acc(1) = + zmp_err(0) * m_robot->totalMass() * 9.8 / (130*0.2*0.2);
+////
+////  torso_pos_acc(0) *= 0.5;
+////  torso_pos_acc(1) *= 0.5;
+////  torso_rot_acc(0) *= 0.1;
+////  torso_rot_acc(1) *= 0.1;
+////
+////
+////
+////  torso_pos_acc -= 1 * torso_pos + 1 * torso_pos_vel;
+////  torso_rot_acc -= 1 * torso_rot + 1 * torso_rot_vel;
+//////  torso_rot_acc(0) -= 10 * torso_rot(0) + 10 * torso_rot_vel(0);
+//////  torso_rot_acc(1) -= 10 * torso_rot(1) + 10 * torso_rot_vel(1);
+////
+////  LIMIT_MINMAX(torso_pos_acc(0),-10,10);
+////  LIMIT_MINMAX(torso_pos_acc(1),-10,10);
+////  LIMIT_MINMAX(torso_pos_acc(2),-0,0);
+////  LIMIT_MINMAX(torso_rot_acc(0),-10,10);
+////  LIMIT_MINMAX(torso_rot_acc(1),-10,10);
+////  LIMIT_MINMAX(torso_rot_acc(2),-0,0);
+////
+////
+////  if(hsp->isHumanSyncOn()){
+////    torso_rot_vel += torso_rot_acc * m_dt;
+////    torso_rot += torso_rot_vel * m_dt;
+////    torso_pos_vel += torso_pos_acc * m_dt;
+////    torso_pos += torso_pos_vel * m_dt;
+////  }
+////
+////  LIMIT_MINMAX(torso_pos(0),-0.1,0.1);
+////  LIMIT_MINMAX(torso_pos(1),-0.1,0.1);
+////  LIMIT_MINMAX(torso_pos(2),-0.1,0.1);
+////  LIMIT_MINMAX(torso_rot(0),-10*M_PI/180,10*M_PI/180);
+////  LIMIT_MINMAX(torso_rot(1),-10*M_PI/180,10*M_PI/180);
+////  LIMIT_MINMAX(torso_rot(2),-0.1,0.1);
+////
+////
+//////  cout<<"total_inertia: "<<total_inertia<<endl;
+//////  cout<<"zmp_lip: "<<zmp_lip.transpose()<<endl;
+//////  cout<<"zmp_fullbody: "<<zmp_fullbody.transpose()<<endl;
+//////  cout<<"torso_rot_acc: "<<torso_rot_acc.transpose()<<endl;
+//////  cout<<"torso_rot_vel: "<<torso_rot_vel.transpose()<<endl;
+//////  cout<<"torso_rot: "<<torso_rot.transpose()<<endl;
+////
+////
+////  fprintf(hsp->id_log,"zmp_lip: %f %f ",zmp_lip(0),zmp_lip(1),zmp_lip(2));
+////  fprintf(hsp->id_log,"zmp_fullbody: %f %f ",zmp_fullbody(0),zmp_fullbody(1),zmp_fullbody(2));
+////  fprintf(hsp->id_log,"zmp_fullbody2: %f %f ",ref_zmp_invdyn2(0),ref_zmp_invdyn2(1),ref_zmp_invdyn2(2));
+////  fprintf(hsp->id_log,"torso_pos_acc: %f %f ",torso_pos_acc(0),torso_pos_acc(1),torso_pos_acc(2));
+////  fprintf(hsp->id_log,"torso_pos: %f %f ",torso_pos(0),torso_pos(1),torso_pos(2));
+////  fprintf(hsp->id_log,"torso_rot_acc: %f %f ",torso_rot_acc(0),torso_rot_acc(1),torso_rot_acc(2));
+////  fprintf(hsp->id_log,"torso_rot: %f %f ",torso_rot(0),torso_rot(1),torso_rot(2));
+////  fprintf(hsp->id_log,"\n");
+////
+////
+////
+//////  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
+//////  fik->current_tm = m_qRef.tm;
+//////  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+//////      fik->ikp[it->first].is_ik_enable = it->second.is_active;
+//////  }
+//////  if(hsp->isHumanSyncOn()){
+//////    if(hsp->ht_first_call){
+//////      std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
+//////      hsp->ht_first_call = false;
+//////    }
+//////  }
+//////  m_robot->calcForwardKinematics();
+//////  // additional COM fitting IK for HumanSynchronizer
+////  if(hsp->isHumanSyncOn()){
+////    HRPPose3D com_mod = hsp->rp_ref_out.getP("com");
+////    com_mod.p += torso_pos;
+////    com_mod.rpy += torso_rot;
+////    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
+////  }
+////}
+//
+//
+//void WholeBodyMasterSlave::calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody){
+//
+//
+//
+////  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
+////  fik->current_tm = m_qRef.tm;
+////  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+////      fik->ikp[it->first].is_ik_enable = it->second.is_active;
+////  }
+//////  // Revert
+////  fik->revertRobotStateToCurrent();
+//
+////  hrp::dmatrix Jp;
+////  hrp::dmatrix Jl;
+////  m_robot->calcCM();
+////  m_robot->calcCMJacobian(NULL, Jp); // Eq.1 upper [M/m E -r]
+////  m_robot->calcAngularMomentumJacobian(NULL, Jl); // Eq.1 lower [H 0 I]
+////  Jp *= m_robot->totalMass();
+////  hrp::dmatrix Jpl(Jp.rows() + Jl.rows(), Jp.cols());
+////  Jpl << Jp, Jl;
+////  hrp::dvector all_vel_vec(3+3+m_robot->numJoints());
+////  all_vel_vec << idsb.base_v, idsb.base_w, idsb.dq;
+//  hrp::Vector3 P,L;
+//  m_robot->rootLink()->v = idsb.base_v;
+//  m_robot->rootLink()->w = idsb.base_w;
+//  m_robot->calcTotalMomentum(P,L);
+//  L = L - m_robot->calcCM().cross(P);
+//
+//  torso_rot(0) -= L(0) * 0.1 * m_dt;
+//  torso_rot(1) -= L(1) * 0.1 * m_dt;
+//  torso_rot(2) -= L(2) * 0.1 * m_dt;
+//
+//
+//
+//  fprintf(hsp->id_log,"p: %f %f ",hsp->rp_ref_out.getP("com").p(0),hsp->rp_ref_out.getP("com").p(1),hsp->rp_ref_out.getP("com").p(2));
+//  fprintf(hsp->id_log,"rpy: %f %f ",hsp->rp_ref_out.getP("com").rpy(0),hsp->rp_ref_out.getP("com").rpy(1),hsp->rp_ref_out.getP("com").rpy(2));
+//  fprintf(hsp->id_log,"P: %f %f ",P(0),P(1),P(2));
+//  fprintf(hsp->id_log,"L: %f %f ",L(0),L(1),L(2));
+////  fprintf(hsp->id_log,"p_v: %f %f ",idsb.base_v(0),idsb.base_v(1),idsb.base_v(2));
+////  fprintf(hsp->id_log,"rpy_v: %f %f ",idsb.base_w(0),idsb.base_w(1),idsb.base_w(2));
+//    fprintf(hsp->id_log,"torso_pos: %f %f ",torso_pos(0),torso_pos(1),torso_pos(2));
+//    fprintf(hsp->id_log,"torso_rot: %f %f ",torso_rot(0),torso_rot(1),torso_rot(2));
+//  fprintf(hsp->id_log,"\n");
+//
+//  if(hsp->isHumanSyncOn()){
+//    HRPPose3D com_mod = hsp->rp_ref_out.getP("com");
+////    com_mod.p += torso_pos;
+////    com_mod.rpy += torso_rot;
+//    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
+//  }
+//}
+//
+//
+//
+
+void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const hrp::Vector3& head_ref){
+  int com_ik_loop=0;
+  const int COM_IK_MAX_LOOP = 10;
+  const double COM_IK_MAX_ERROR = 1e-4;
+
+//  cout<<""<<m_robot->rootLink()->p.transpose()<<" : "<<com_ref.p.transpose()<<endl;
+
+  m_robot->rootLink()->p = com_ref.p;//move base link at first
+//  m_robot->rootLink()->p = com_ref.p + hsp->torso_rot;//move base link at first
+  m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
+
+  //////////invdyncomp
+//  m_robot->rootLink()->R *= hrp::rotFromRpy(hsp->torso_rot);
+//  m_robot->rootLink()->p += torso_rot;
+
+  hrp::Vector3 tmp_com_err;
+  const std::string names[4] = {"rleg","lleg","rarm","larm"};
+  const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
+  for(int i=0;i<4;i++){
+    if(fik->ikp.count(names[i])){
+      fik->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);//convert End Effector pos into link origin pos
+      fik->ikp[names[i]].target_p0 = refs[i]->p;
+    }
+  }
+  if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref(2);
+  if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref(1);
+  //COM 収束ループ
+  while(
+      m_robot->calcForwardKinematics(),
+      tmp_com_err = com_ref.p - m_robot->calcCM(),
+      tmp_com_err.norm() > COM_IK_MAX_ERROR)
+  {
+    m_robot->rootLink()->p += tmp_com_err;
+    m_robot->calcForwardKinematics();
+    for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
+        if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
+    }
+    if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
+  }
+//  cout<<"COM_IK_MAX_LOOP:"<<com_ik_loop<<endl;
+}
+
+bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double sec)
+{
+  if(sec >= 0.0 && sec <= 30.0){
+    std::cerr << "[" << m_profile.instance_name << "] start Synchronization after "<<sec<<" [s]" << std::endl;
+    hsp->countdown_sec = sec;
+    hsp->startCountdownForHumanSync = true;
+    return true;
+  }else{
+    std::cerr << "[" << m_profile.instance_name << "] Count Down Time must be 0 < T < 30 [s]"<< std::endl;
+    return false;
+  }
+}
+
+bool WholeBodyMasterSlave::stopHumanSync()
+{
+	std::cerr << "[" << m_profile.instance_name << "] stop HumanSync Now" << std::endl;
+	return true;
+}
+
+bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
+  std::cerr << "[" << m_profile.instance_name << "] setWholeBodyMasterSlaveParam" << std::endl;
+  hsp->WBMSparam.use_rh = hsp->WBMSparam.use_lh = i_param.use_hands;
+  hsp->WBMSparam.use_head = i_param.use_head;
+  hsp->WBMSparam.set_com_height_fix = i_param.set_com_height_fix;
+  hsp->WBMSparam.set_com_height_fix_val = i_param.set_com_height_fix_val;
+  hsp->WBMSparam.foot_vertical_vel_limit_coeff = i_param.foot_vertical_vel_limit_coeff;
+  hsp->WBMSparam.human_com_height = i_param.human_com_height;
+  hsp->WBMSparam.swing_foot_height_offset = i_param.swing_foot_height_offset;
+  hsp->WBMSparam.swing_foot_max_height = i_param.swing_foot_max_height;
+  hsp->WBMSparam.auto_swing_foot_landing_threshold = i_param.auto_swing_foot_landing_threshold;
+  return true;
+}
+bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
+  std::cerr << "[" << m_profile.instance_name << "] getWholeBodyMasterSlaveParam" << std::endl;
+  i_param.set_com_height_fix = hsp->WBMSparam.set_com_height_fix;
+  i_param.set_com_height_fix_val = hsp->WBMSparam.set_com_height_fix_val;
+  i_param.foot_vertical_vel_limit_coeff = hsp->WBMSparam.foot_vertical_vel_limit_coeff;
+  i_param.human_com_height = hsp->WBMSparam.human_com_height;
+  i_param.use_hands = (hsp->WBMSparam.use_rh || hsp->WBMSparam.use_lh);
+  i_param.use_head = hsp->WBMSparam.use_head;
+  i_param.swing_foot_height_offset = hsp->WBMSparam.swing_foot_height_offset;
+  i_param.swing_foot_max_height = hsp->WBMSparam.swing_foot_max_height;
+  i_param.auto_swing_foot_landing_threshold = hsp->WBMSparam.auto_swing_foot_landing_threshold;
+  return true;
+}
+
+//
+extern "C"
+{
+
+    void WholeBodyMasterSlaveInit(RTC::Manager* manager)
+    {
+        RTC::Properties profile(WholeBodyMasterSlave_spec);
+        manager->registerFactory(profile,
+                                 RTC::Create<WholeBodyMasterSlave>,
+                                 RTC::Delete<WholeBodyMasterSlave>);
+    }
+
+};
+
+
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
new file mode 100644
index 00000000000..26260a8daa3
--- /dev/null
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -0,0 +1,284 @@
+// -*- C++ -*-
+/*!
+ * @file  WholeBodyMasterSlave.h
+ * @brief WholeBodyMasterSlave component
+ * @date  $Date$
+ *
+ * $Id$
+ */
+
+#ifndef WholeBodyMasterSlave_H
+#define WholeBodyMasterSlave_H
+
+#include <rtm/idl/BasicDataType.hh>
+#include <rtm/idl/ExtendedDataTypes.hh>
+#include <rtm/Manager.h>
+#include <rtm/DataFlowComponentBase.h>
+#include <rtm/CorbaPort.h>
+#include <rtm/DataInPort.h>
+#include <rtm/DataOutPort.h>
+#include <rtm/idl/BasicDataTypeSkel.h>
+#include <rtm/idl/ExtendedDataTypesSkel.h>
+#include <hrpModel/Body.h>
+#include "../ImpedanceController/JointPathEx.h"
+#include "../ImpedanceController/RatsMatrix.h"
+// Service implementation headers
+// <rtc-template block="service_impl_h">
+#include "WholeBodyMasterSlaveService_impl.h"
+#include "interpolator.h"
+#include "../TorqueFilter/IIRFilter.h"
+#include "../AutoBalancer/HumanMasterSlave.h"
+#include "../AutoBalancer/AutoBalancer.h"
+
+// </rtc-template>
+
+using namespace RTC;
+
+//static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
+//{
+//    int pre = os.precision();
+//    os.setf(std::ios::fixed);
+//    os << std::setprecision(6)
+//       << (tm.sec + tm.nsec/1e9)
+//       << std::setprecision(pre);
+//    os.unsetf(std::ios::fixed);
+//    return os;
+//}
+
+
+class WholeBodyMasterSlave
+  : public RTC::DataFlowComponentBase
+{
+ public:
+  WholeBodyMasterSlave(RTC::Manager* manager);
+  virtual ~WholeBodyMasterSlave();
+  virtual RTC::ReturnCode_t onInitialize();
+  virtual RTC::ReturnCode_t onFinalize();
+  virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
+  virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
+  virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
+  bool startCountDownForWholeBodyMasterSlave(const double sec);
+  bool stopHumanSync();
+  bool setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
+  bool getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
+
+ protected:
+  TimedDoubleSeq m_qRef;
+  InPort<TimedDoubleSeq> m_qRefIn;
+  TimedPoint3D m_basePos;
+  InPort<TimedPoint3D> m_basePosIn;
+  TimedOrientation3D m_baseRpy;
+  InPort<TimedOrientation3D> m_baseRpyIn;
+  TimedPoint3D m_zmp;
+  InPort<TimedPoint3D> m_zmpIn;
+  TimedDoubleSeq m_optionalData;
+  InPort<TimedDoubleSeq> m_optionalDataIn;
+  std::vector<TimedDoubleSeq> m_ref_force;
+  std::vector<InPort<TimedDoubleSeq> *> m_ref_forceIn;
+  TimedLong m_emergencySignal;
+  InPort<TimedLong> m_emergencySignalIn;
+  // for debug
+  TimedPoint3D m_cog;
+  //for human tracker
+  TimedPose3D m_htcom;
+  InPort<TimedPose3D> m_htcomIn;
+  TimedPose3D m_htrf;
+  InPort<TimedPose3D> m_htrfIn;
+  TimedPose3D m_htlf;
+  InPort<TimedPose3D> m_htlfIn;
+  TimedPose3D m_htrh;
+  InPort<TimedPose3D> m_htrhIn;
+  TimedPose3D m_htlh;
+  InPort<TimedPose3D> m_htlhIn;
+  TimedPose3D m_hthead;
+  InPort<TimedPose3D> m_htheadIn;
+  TimedPoint3D m_htzmp;
+  InPort<TimedPoint3D> m_htzmpIn;
+  TimedPoint3D m_actzmp;
+  InPort<TimedPoint3D> m_actzmpIn;
+  TimedDoubleSeq m_htrfw;
+  InPort<TimedDoubleSeq> m_htrfwIn;
+  TimedDoubleSeq m_htlfw;
+  InPort<TimedDoubleSeq> m_htlfwIn;
+  //ishiguro dbg
+  TimedPose3D m_htcom_dbg;
+  OutPort<TimedPose3D> m_htcom_dbgOut;
+  TimedPose3D m_htrf_dbg;
+  OutPort<TimedPose3D> m_htrf_dbgOut;
+  TimedPose3D m_htlf_dbg;
+  OutPort<TimedPose3D> m_htlf_dbgOut;
+  TimedPose3D m_htrh_dbg;
+  OutPort<TimedPose3D> m_htrh_dbgOut;
+  TimedPose3D m_htlh_dbg;
+  OutPort<TimedPose3D> m_htlh_dbgOut;
+  TimedPose3D m_hthead_dbg;
+  OutPort<TimedPose3D> m_hthead_dbgOut;
+  TimedPoint3D m_htzmp_dbg;
+  OutPort<TimedPoint3D> m_htzmp_dbgOut;
+  TimedDoubleSeq m_htrfw_dbg;
+  OutPort<TimedDoubleSeq> m_htrfw_dbgOut;
+  TimedDoubleSeq m_htlfw_dbg;
+  OutPort<TimedDoubleSeq> m_htlfw_dbgOut;
+  TimedPose3D m_rpcom_dbg;
+  OutPort<TimedPose3D> m_rpcom_dbgOut;
+  TimedPose3D m_rprf_dbg;
+  OutPort<TimedPose3D> m_rprf_dbgOut;
+  TimedPose3D m_rplf_dbg;
+  OutPort<TimedPose3D> m_rplf_dbgOut;
+  TimedPose3D m_rprh_dbg;
+  OutPort<TimedPose3D> m_rprh_dbgOut;
+  TimedPose3D m_rplh_dbg;
+  OutPort<TimedPose3D> m_rplh_dbgOut;
+  TimedPose3D m_rphead_dbg;
+  OutPort<TimedPose3D> m_rphead_dbgOut;
+  TimedPoint3D m_rpzmp_dbg;
+  OutPort<TimedPoint3D> m_rpzmp_dbgOut;
+  TimedPoint3D m_rpdcp_dbg;
+  OutPort<TimedPoint3D> m_rpdcp_dbgOut;
+  TimedPoint3D m_rpacp_dbg;
+  OutPort<TimedPoint3D> m_rpacp_dbgOut;
+
+  TimedDoubleSeq m_invdyn_dbg;
+  OutPort<TimedDoubleSeq> m_invdyn_dbgOut;
+
+  OutPort<TimedDoubleSeq> m_qOut;
+  RTC::OutPort<RTC::TimedPoint3D> m_zmpOut;
+  OutPort<TimedPoint3D> m_basePosOut;
+  OutPort<TimedOrientation3D> m_baseRpyOut;
+  TimedDoubleSeq m_baseTform;
+  OutPort<TimedDoubleSeq> m_baseTformOut;
+  TimedDoubleSeq m_optionalData2;
+  OutPort<TimedDoubleSeq> m_optionalDataOut;
+  TimedPose3D m_basePose;
+  OutPort<TimedPose3D> m_basePoseOut;
+  TimedAcceleration3D m_accRef;
+  OutPort<TimedAcceleration3D> m_accRefOut;
+  TimedBooleanSeq m_contactStates;
+  OutPort<TimedBooleanSeq> m_contactStatesOut;
+  TimedDoubleSeq m_toeheelRatio;
+  OutPort<TimedDoubleSeq> m_toeheelRatioOut;
+  TimedDoubleSeq m_controlSwingSupportTime;
+  OutPort<TimedDoubleSeq> m_controlSwingSupportTimeOut;
+  TimedBoolean m_walkingStates;
+  OutPort<TimedBoolean> m_walkingStatesOut;
+  TimedPoint3D m_sbpCogOffset;
+  OutPort<TimedPoint3D> m_sbpCogOffsetOut;
+  std::vector<TimedDoubleSeq> m_force;
+  std::vector<OutPort<TimedDoubleSeq> *> m_ref_forceOut;
+  std::vector<TimedPoint3D> m_limbCOPOffset;
+  std::vector<OutPort<TimedPoint3D> *> m_limbCOPOffsetOut;
+  // for debug
+  OutPort<TimedPoint3D> m_cogOut;
+  
+  RTC::CorbaPort m_WholeBodyMasterSlaveServicePort;
+
+  WholeBodyMasterSlaveService_impl m_service0;
+
+ private:
+  struct ABCIKparam {
+    hrp::Vector3 target_p0, localPos, adjust_interpolation_target_p0, adjust_interpolation_org_p0;
+    hrp::Matrix33 target_r0, localR, adjust_interpolation_target_r0, adjust_interpolation_org_r0;
+    hrp::Link* target_link;
+    bool is_active, has_toe_joint;
+  };
+//  void getTargetParameters();
+  void solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const hrp::Vector3& head_ref);
+  void processWholeBodyMasterSlave();
+//  void calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody);
+//  void solveFullbodyIK ();
+//  void startABCparam(const ::OpenHRP::WholeBodyMasterSlaveService::StrSequence& limbs);
+//  void stopABCparam();
+//  void waitABCTransition();
+//  // Functions to calculate parameters for ABC output.
+//  // Output parameters are EE, limbCOPOffset, contactStates, controlSwingSupportTime, toeheelPhaseRatio
+//  void getOutputParametersForWalking ();
+//  void getOutputParametersForABC ();
+//  void getOutputParametersForIDLE ();
+//  void interpolateLegNamesAndZMPOffsets();
+//  void calcFixCoordsForAdjustFootstep (rats::coordinates& tmp_fix_coords);
+//  void rotateRefForcesForFixCoords (rats::coordinates& tmp_fix_coords);
+//  void updateTargetCoordsForHandFixMode (rats::coordinates& tmp_fix_coords);
+//  void calculateOutputRefForces ();
+//  hrp::Vector3 calcFootMidPosUsingZMPWeightMap ();
+//  void updateWalkingVelocityFromHandError (rats::coordinates& tmp_fix_coords);
+//  void calcReferenceJointAnglesForIK ();
+//  hrp::Matrix33 OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2);
+//  void fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot);
+//  void fixLegToCoords2 (rats::coordinates& tmp_fix_coords);
+//  bool startWalking ();
+//  void stopWalking ();
+//  void copyRatscoords2Footstep(OpenHRP::WholeBodyMasterSlaveService::Footstep& out_fs, const rats::coordinates& in_fs);
+//  // static balance point offsetting
+//  void static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height);
+//  void calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces);
+//  hrp::Vector3 calc_vel_from_hand_error (const rats::coordinates& tmp_fix_coords);
+  bool isOptionalDataContact (const std::string& ee_name)
+  {
+      return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false;
+  };
+//  bool calc_inital_support_legs(const double& y, std::vector<rats::coordinates>& initial_support_legs_coords, std::vector<rats::leg_type>& initial_support_legs, rats::coordinates& start_ref_coords);
+//
+//  // for gg
+//  typedef boost::shared_ptr<rats::gait_generator> ggPtr;
+//  ggPtr gg;
+//  bool gg_is_walking, gg_solved;
+//  // for abc
+  typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
+  fikPtr fik;
+//  hrp::Vector3 ref_cog, ref_zmp, prev_imu_sensor_pos, prev_imu_sensor_vel, hand_fix_initial_offset;
+//  enum {BIPED, TROT, PACE, CRAWL, GALLOP} gait_type;
+//  enum {MODE_IDLE, MODE_ABC, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_ABC} control_mode;
+//  std::map<std::string, ABCIKparam> ikp;
+  std::map<std::string, size_t> contact_states_index_map;
+//  std::map<std::string, hrp::VirtualForceSensorParam> m_vfs;
+//  std::vector<std::string> sensor_names, leg_names, ee_vec;
+//  hrp::Vector3 target_root_p;
+//  hrp::Matrix33 target_root_R;
+//  rats::coordinates fix_leg_coords;
+//  std::vector<hrp::Vector3> default_zmp_offsets;
+  double m_dt;
+  hrp::BodyPtr m_robot;
+//  coil::Mutex m_mutex;
+//
+//  double transition_interpolator_ratio, transition_time, zmp_transition_time, adjust_footstep_transition_time, leg_names_interpolator_ratio;
+//  interpolator *zmp_offset_interpolator;
+//  interpolator *transition_interpolator;
+//  interpolator *adjust_footstep_interpolator;
+//  interpolator *leg_names_interpolator;
+  hrp::Vector3 input_zmp, input_basePos;
+  hrp::Matrix33 input_baseRot;
+
+  hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
+
+//  // static balance point offsetting
+//  hrp::Vector3 sbp_offset, sbp_cog_offset;
+//  enum {MODE_NO_FORCE, MODE_REF_FORCE} use_force;
+//  std::vector<hrp::Vector3> ref_forces;
+//
+  unsigned int m_debugLevel;
+  bool is_legged_robot;// is_stop_mode, is_hand_fix_mode, is_hand_fix_initial;
+  int loop;
+//  bool graspless_manip_mode;
+//  std::string graspless_manip_arm;
+//  hrp::Vector3 graspless_manip_p_gain;
+//  rats::coordinates graspless_manip_reference_trans_coords;
+
+  hrp::InvDynStateBuffer idsb;
+  std::vector<IIRFilter> invdyn_zmp_filters;
+
+  hrp::InvDynStateBuffer idsb2;
+  std::vector<IIRFilter> invdyn_zmp_filters2;
+
+
+  hrp::Vector3 ref_zmp_invdyn2;
+
+  //for HumanSynchronizer
+  boost::shared_ptr<HumanSynchronizer> hsp;
+};
+
+extern "C"
+{
+  void WholeBodyMasterSlaveInit(RTC::Manager* manager);
+};
+
+#endif // WholeBodyMasterSlave_H
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveComp.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveComp.cpp
new file mode 100644
index 00000000000..8d2eb5470e4
--- /dev/null
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveComp.cpp
@@ -0,0 +1,91 @@
+// -*- C++ -*-
+/*!
+ * @file WholeBodyMasterSlaveComp.cpp
+ * @brief Standalone component
+ * @date $Date$
+ *
+ * $Id$
+ */
+
+#include <rtm/Manager.h>
+#include <iostream>
+#include <string>
+#include "WholeBodyMasterSlave.h"
+
+
+void MyModuleInit(RTC::Manager* manager)
+{
+  WholeBodyMasterSlaveInit(manager);
+  RTC::RtcBase* comp;
+
+  // Create a component
+  comp = manager->createComponent("WholeBodyMasterSlave");
+
+
+  // Example
+  // The following procedure is examples how handle RT-Components.
+  // These should not be in this function.
+
+  // Get the component's object reference
+//  RTC::RTObject_var rtobj;
+//  rtobj = RTC::RTObject::_narrow(manager->getPOA()->servant_to_reference(comp));
+
+  // Get the port list of the component
+//  PortList* portlist;
+//  portlist = rtobj->get_ports();
+
+  // getting port profiles
+//  std::cout << "Number of Ports: ";
+//  std::cout << portlist->length() << std::endl << std::endl; 
+//  for (CORBA::ULong i(0), n(portlist->length()); i < n; ++i)
+//  {
+//    Port_ptr port;
+//    port = (*portlist)[i];
+//    std::cout << "Port" << i << " (name): ";
+//    std::cout << port->get_port_profile()->name << std::endl;
+//    
+//    RTC::PortInterfaceProfileList iflist;
+//    iflist = port->get_port_profile()->interfaces;
+//    std::cout << "---interfaces---" << std::endl;
+//    for (CORBA::ULong i(0), n(iflist.length()); i < n; ++i)
+//    {
+//      std::cout << "I/F name: ";
+//      std::cout << iflist[i].instance_name << std::endl;
+//      std::cout << "I/F type: ";
+//      std::cout << iflist[i].type_name << std::endl;
+//      const char* pol;
+//      pol = iflist[i].polarity == 0 ? "PROVIDED" : "REQUIRED";
+//      std::cout << "Polarity: " << pol << std::endl;
+//    }
+//    std::cout << "---properties---" << std::endl;
+//    NVUtil::dump(port->get_port_profile()->properties);
+//    std::cout << "----------------" << std::endl << std::endl;
+//  }
+
+  return;
+}
+
+int main (int argc, char** argv)
+{
+  RTC::Manager* manager;
+  manager = RTC::Manager::init(argc, argv);
+
+  // Initialize manager
+  manager->init(argc, argv);
+
+  // Set module initialization proceduer
+  // This procedure will be invoked in activateManager() function.
+  manager->setModuleInitProc(MyModuleInit);
+
+  // Activate manager and register to naming service
+  manager->activateManager();
+
+  // run the manager in blocking mode
+  // runManager(false) is the default.
+  manager->runManager();
+
+  // If you want to run the manager in non-blocking mode, do like this
+  // manager->runManager(true);
+
+  return 0;
+}
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp
new file mode 100644
index 00000000000..2f29a7fe820
--- /dev/null
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp
@@ -0,0 +1,37 @@
+#include "WholeBodyMasterSlaveService_impl.h"
+#include "WholeBodyMasterSlave.h"
+
+WholeBodyMasterSlaveService_impl::WholeBodyMasterSlaveService_impl() : m_wholebodymasterslave(NULL)
+{
+}
+
+WholeBodyMasterSlaveService_impl::~WholeBodyMasterSlaveService_impl()
+{
+}
+
+void WholeBodyMasterSlaveService_impl::wholebodymasterslave(WholeBodyMasterSlave *i_wholebodymasterslave)
+{
+  m_wholebodymasterslave = i_wholebodymasterslave;
+}
+
+CORBA::Boolean WholeBodyMasterSlaveService_impl::startCountDownForWholeBodyMasterSlave(CORBA::Double sec)
+{
+    return m_wholebodymasterslave->startCountDownForWholeBodyMasterSlave(sec);
+};
+
+CORBA::Boolean WholeBodyMasterSlaveService_impl::stopHumanSync()
+{
+    return m_wholebodymasterslave->stopHumanSync();
+};
+
+void WholeBodyMasterSlaveService_impl::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param)
+{
+  m_wholebodymasterslave->setWholeBodyMasterSlaveParam(i_param);
+};
+
+void WholeBodyMasterSlaveService_impl::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam_out i_param)
+{
+//  i_param = new OpenHRP::WholeBodyMasterSlaveParam::WholeBodyMasterSlaveParam();
+//  return  m_wholebodymasterslave->getWholeBodyMasterSlaveParam(*i_param);
+  m_wholebodymasterslave->getWholeBodyMasterSlaveParam(i_param);
+};
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h
new file mode 100644
index 00000000000..b5e12e74310
--- /dev/null
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h
@@ -0,0 +1,28 @@
+// -*-C++-*-
+#ifndef WholeBodyMasterSlaveSERVICESVC_IMPL_H
+#define WholeBodyMasterSlaveSERVICESVC_IMPL_H
+
+#include "hrpsys/idl/WholeBodyMasterSlaveService.hh"
+
+using namespace OpenHRP;
+
+class WholeBodyMasterSlave;
+
+class WholeBodyMasterSlaveService_impl
+  : public virtual POA_OpenHRP::WholeBodyMasterSlaveService,
+    public virtual PortableServer::RefCountServantBase
+{
+public:
+  WholeBodyMasterSlaveService_impl();
+  virtual ~WholeBodyMasterSlaveService_impl();
+  CORBA::Boolean startCountDownForWholeBodyMasterSlave(const CORBA::Double sec);
+  CORBA::Boolean stopHumanSync();
+  void setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
+  void getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam_out i_param);
+  //
+  void wholebodymasterslave(WholeBodyMasterSlave *i_wholebodymasterslave);
+private:
+  WholeBodyMasterSlave *m_wholebodymasterslave;
+};				 
+
+#endif

From ae3332c2c2874e1617b4728996710a5f38fe6125 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 22 Apr 2017 21:08:34 +0900
Subject: [PATCH 068/217] complete transport ABC->WBMS

---
 idl/AutoBalancerService.idl                   |  20 -
 rtc/AutoBalancer/AutoBalancer.cpp             | 502 +--------------
 rtc/AutoBalancer/AutoBalancer.h               |  84 +--
 rtc/AutoBalancer/AutoBalancerService_impl.cpp |  24 +-
 rtc/AutoBalancer/AutoBalancerService_impl.h   |   4 -
 rtc/AutoBalancer/CMakeLists.txt               |   8 +-
 rtc/AutoBalancer/GaitGenerator.h              |   2 +-
 rtc/WholeBodyMasterSlave/CMakeLists.txt       |   2 +-
 .../WholeBodyMasterSlave.cpp                  | 569 +++---------------
 .../WholeBodyMasterSlave.h                    | 104 +---
 .../wbms_core.cpp}                            |   2 +-
 .../wbms_core.h}                              |   2 +-
 12 files changed, 111 insertions(+), 1212 deletions(-)
 rename rtc/{AutoBalancer/HumanMasterSlave.cpp => WholeBodyMasterSlave/wbms_core.cpp} (99%)
 rename rtc/{AutoBalancer/HumanMasterSlave.h => WholeBodyMasterSlave/wbms_core.h} (99%)

diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl
index fa53513346f..b0c6fb27da5 100644
--- a/idl/AutoBalancerService.idl
+++ b/idl/AutoBalancerService.idl
@@ -438,25 +438,5 @@ module OpenHRP
      * @return true if set successfully, false otherwise
      */
     boolean releaseEmergencyStop();
-
-    /**
-     * HumanSync
-     */
-    struct WholeBodyMasterSlaveParam
-    {
-        double human_com_height;
-        double foot_vertical_vel_limit_coeff;
-        boolean set_com_height_fix;
-        double set_com_height_fix_val;
-        boolean use_hands;
-        boolean use_head;
-        double swing_foot_height_offset;
-        double swing_foot_max_height;
-        double auto_swing_foot_landing_threshold;
-    };
-    boolean startCountDownForWholeBodyMasterSlave(in double sec);
-    boolean stopHumanSync();
-    void setWholeBodyMasterSlaveParam(in WholeBodyMasterSlaveParam i_param);
-    void getWholeBodyMasterSlaveParam(out WholeBodyMasterSlaveParam i_param);
   };
 };
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index be6cf4b6cc0..2a3120913d3 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -49,39 +49,6 @@ AutoBalancer::AutoBalancer(RTC::Manager* manager)
       m_zmpIn("zmpIn", m_zmp),
       m_optionalDataIn("optionalData", m_optionalData),
       m_emergencySignalIn("emergencySignal", m_emergencySignal),
-      //ishiguro
-      m_htzmpIn("htzmpIn", m_htzmp),
-      m_htrfwIn("htrfwIn", m_htrfw),
-      m_htlfwIn("htlfwIn", m_htlfw),
-      m_htcomIn("htcomIn", m_htcom),
-      m_htrfIn("htrfIn", m_htrf),
-      m_htlfIn("htlfIn", m_htlf),
-      m_htrhIn("htrhIn", m_htrh),
-      m_htlhIn("htlhIn", m_htlh),
-      m_actzmpIn("actzmpIn", m_actzmp),
-      m_htheadIn("htheadIn", m_hthead),
-
-      m_htcom_dbgOut("htcom_dbgOut", m_htcom_dbg),
-      m_htrf_dbgOut("htrf_dbgOut", m_htrf_dbg),
-      m_htlf_dbgOut("htlf_dbgOut", m_htlf_dbg),
-      m_htrh_dbgOut("htrh_dbgOut", m_htrh_dbg),
-      m_htlh_dbgOut("htlh_dbgOut", m_htlh_dbg),
-      m_hthead_dbgOut("hthead_dbgOut", m_hthead_dbg),
-      m_htzmp_dbgOut("htzmp_dbgOut", m_htzmp_dbg),
-      m_htrfw_dbgOut("htrfw_dbgOut", m_htrfw_dbg),
-      m_htlfw_dbgOut("htlfw_dbgOut", m_htlfw_dbg),
-      m_rpcom_dbgOut("rpcom_dbgOut", m_rpcom_dbg),
-      m_rprf_dbgOut("rprf_dbgOut", m_rprf_dbg),
-      m_rplf_dbgOut("rplf_dbgOut", m_rplf_dbg),
-      m_rprh_dbgOut("rprh_dbgOut", m_rprh_dbg),
-      m_rplh_dbgOut("rplh_dbgOut", m_rplh_dbg),
-      m_rphead_dbgOut("rphead_dbgOut", m_rphead_dbg),
-      m_rpzmp_dbgOut("rpzmp_dbgOut", m_rpzmp_dbg),
-      m_rpdcp_dbgOut("rpdcp_dbgOut", m_rpdcp_dbg),
-      m_rpacp_dbgOut("rpacp_dbgOut", m_rpacp_dbg),
-
-      m_invdyn_dbgOut("invdyn_dbgOut", m_invdyn_dbg),
-
       m_qOut("q", m_qRef),
       m_zmpOut("zmpOut", m_zmp),
       m_basePosOut("basePosOut", m_basePos),
@@ -123,38 +90,6 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     addInPort("zmpIn", m_zmpIn);
     addInPort("optionalData", m_optionalDataIn);
     addInPort("emergencySignal", m_emergencySignalIn);
-    //ishiguro
-    addInPort("htzmpIn", m_htzmpIn);
-    addInPort("htrfwIn", m_htrfwIn);
-    addInPort("htlfwIn", m_htlfwIn);
-    addInPort("htcomIn", m_htcomIn);
-    addInPort("htrfIn", m_htrfIn);
-    addInPort("htlfIn", m_htlfIn);
-    addInPort("htrhIn", m_htrhIn);
-    addInPort("htlhIn", m_htlhIn);
-    addInPort("actzmpIn", m_actzmpIn);
-    addInPort("htheadIn", m_htheadIn);
-
-    addOutPort("htcom_dbgOut", m_htcom_dbgOut);
-    addOutPort("htrf_dbgOut", m_htrf_dbgOut);
-    addOutPort("htlf_dbgOut", m_htlf_dbgOut);
-    addOutPort("htrh_dbgOut", m_htrh_dbgOut);
-    addOutPort("htlh_dbgOut", m_htlh_dbgOut);
-    addOutPort("hthead_dbgOut", m_hthead_dbgOut);
-    addOutPort("htzmp_dbgOut", m_htzmp_dbgOut);
-    addOutPort("htrfw_dbgOut", m_htrfw_dbgOut);
-    addOutPort("htlfw_dbgOut", m_htlfw_dbgOut);
-    addOutPort("rpcom_dbgOut", m_rpcom_dbgOut);
-    addOutPort("rprf_dbgOut", m_rprf_dbgOut);
-    addOutPort("rplf_dbgOut", m_rplf_dbgOut);
-    addOutPort("rprh_dbgOut", m_rprh_dbgOut);
-    addOutPort("rplh_dbgOut", m_rplh_dbgOut);
-    addOutPort("rphead_dbgOut", m_rphead_dbgOut);
-    addOutPort("rpzmp_dbgOut", m_rpzmp_dbgOut);
-    addOutPort("rpdcp_dbgOut", m_rpdcp_dbgOut);
-    addOutPort("rpacp_dbgOut", m_rpacp_dbgOut);
-
-    addOutPort("invdyn_dbgOut", m_invdyn_dbgOut);
 
     // Set OutPort buffer
     addOutPort("q", m_qOut);
@@ -207,9 +142,6 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     // allocate memory for outPorts
     m_qRef.data.length(m_robot->numJoints());
     m_baseTform.data.length(12);
-    //ishiguro
-    m_htrfw.data.length(6);
-    m_htlfw.data.length(6);
 
     control_mode = MODE_IDLE;
     loop = 0;
@@ -237,7 +169,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
         coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
         ABCIKparam tp;
         for (size_t j = 0; j < 3; j++) {
-            coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
+          coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
         }
         double tmpv[4];
         for (int j = 0; j < 4; j++ ) {
@@ -412,10 +344,6 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     is_stop_mode = false;
     is_hand_fix_mode = false;
 
-    //for HumanSynchronizer
-    hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer());
-    if(leg_pos.size()>=2){ hsp->init_wld_rp_rfeepos = leg_pos[0]; hsp->init_wld_rp_lfeepos = leg_pos[1]; }
-
     hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
     if (sen == NULL) {
         std::cerr << "[" << m_profile.instance_name << "] WARNING! This robot model has no GyroSensor named 'gyrometer'! " << std::endl;
@@ -424,6 +352,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
 }
 
 
+
 RTC::ReturnCode_t AutoBalancer::onFinalize()
 {
   delete zmp_offset_interpolator;
@@ -466,9 +395,6 @@ RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
   return RTC::RTC_OK;
 }
 
-
-
-
 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
 //#define DEBUGP2 ((loop%200==0))
 #define DEBUGP2 (false)
@@ -513,26 +439,6 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
         //     gg->emergency_stop();
         // }
     }
-    //for HumanSynchronizer
-    if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
-    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hsp->hp_wld_raw.getw("rfw")); }
-    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hsp->hp_wld_raw.getw("lfw")); }
-    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_htcom.data,hsp->hp_wld_raw.getP("com")); }
-    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrf.data,hsp->hp_wld_raw.getP("rf")); }
-    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlf.data,hsp->hp_wld_raw.getP("lf")); }
-    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrh.data,hsp->hp_wld_raw.getP("rh"));}
-    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlh.data,hsp->hp_wld_raw.getP("lh"));}
-    if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_hthead.data,hsp->head_cam_pose);}
-    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
-    hsp->baselinkpose.p = m_robot->rootLink()->p;
-    hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
-    if(ikp.count("rarm"))ikp["rarm"].is_active = false;
-    if(ikp.count("larm"))ikp["larm"].is_active = false;
-//    m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
-//    m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
-
-
-
 
     // Calculation
     Guard guard(m_mutex);
@@ -552,10 +458,7 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
       }
       if (control_mode != MODE_IDLE ) {
         solveFullbodyIK();
-
-//        processWholeBodyMasterSlave();
-
-        /////// Inverse Dynamics /////////
+//        /////// Inverse Dynamics /////////
 //        if(!idsb.is_initialized){
 //          idsb.setInitState(m_robot, m_dt);
 //          invdyn_zmp_filters.resize(3);
@@ -563,41 +466,14 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
 //            invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
 //            invdyn_zmp_filters[i].reset(ref_zmp(i));
 //          }
-//          idsb2.setInitState(m_robot, m_dt);
-//          invdyn_zmp_filters2.resize(3);
-//          for(int i=0;i<3;i++){
-//            invdyn_zmp_filters2[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
-//            invdyn_zmp_filters2[i].reset(ref_zmp(i));
-//          }
 //        }
 //        calcAccelerationsForInverseDynamics(m_robot, idsb);
-//        hrp::Vector3 ref_zmp_invdyn;
-//        calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);
-//        for(int i=0;i<3;i++) ref_zmp_invdyn(i) = invdyn_zmp_filters[i].passFilter(ref_zmp_invdyn(i));
-//        updateInvDynStateBuffer(idsb);
-
-//        calcDynamicsFilterCompensation(ref_zmp, ref_zmp_invdyn);
-
-//        calcAccelerationsForInverseDynamics(m_robot, idsb2);
-//        calcWorldZMPFromInverseDynamics(m_robot, idsb2, ref_zmp_invdyn2);
-//        for(int i=0;i<3;i++) ref_zmp_invdyn2(i) = invdyn_zmp_filters2[i].passFilter(ref_zmp_invdyn2(i));
-//        updateInvDynStateBuffer(idsb2);
-
-        //        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
-
-        if(m_optionalData.data.length()>=4*2){
-          m_contactStates.data[contact_states_index_map["rleg"]] = isOptionalDataContact("rleg");
-          m_contactStates.data[contact_states_index_map["lleg"]] = isOptionalDataContact("lleg");
-        }
-//        m_robot->rootLink()->p = input_basePos;
-//        m_robot->rootLink()->R = input_baseRot;
-//        for (int i = 0; i < m_robot->numJoints(); i++ ){
-//           m_robot->joint(i)->q = m_qRef.data[i];
+//        if(gg_is_walking){
+//          calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp);
+//          for(int i=0;i<3;i++) ref_zmp(i) = invdyn_zmp_filters[i].passFilter(ref_zmp(i));
 //        }
-//        m_robot->calcForwardKinematics();
+//        updateInvDynStateBuffer(idsb);
 
-//        ref_cog = m_robot->calcCM();
-//        rel_ref_zmp = input_zmp;
         rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
       } else {
         rel_ref_zmp = input_zmp;
@@ -732,68 +608,6 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
       }
     }
 
-    //ishiguro dbg plot
-    m_htcom.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("com"),m_htcom_dbg.data);
-    m_htcom_dbgOut.write();
-    m_htrf.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("rf"),m_htrf_dbg.data);
-    m_htrf_dbgOut.write();
-    m_htlf.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("lf"),m_htlf_dbg.data);
-    m_htlf_dbgOut.write();
-    m_htrh.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("rh"),m_htrh_dbg.data);
-    m_htrh_dbgOut.write();
-    m_htlh.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("lh"),m_htlh_dbg.data);
-    m_htlh_dbgOut.write();
-    m_hthead.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("head"),m_hthead_dbg.data);
-    m_hthead_dbgOut.write();
-    m_htzmp.tm = m_qRef.tm;
-    HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.getP("zmp").p,m_rpzmp_dbg.data);
-    m_htzmp_dbgOut.write();
-    m_htrfw.tm = m_qRef.tm;
-    m_htrfw_dbg.data.length(6);
-    HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.getw("rfw"),m_htrfw_dbg.data);
-    m_htrfw_dbgOut.write();
-    m_htlfw.tm = m_qRef.tm;
-    m_htlfw_dbg.data.length(6);
-    HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.getw("lfw"),m_htlfw_dbg.data);
-    m_htlfw_dbgOut.write();
-    m_rpcom_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("com"),m_rpcom_dbg.data);
-    m_rpcom_dbgOut.write();
-    m_rprf_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("rf"),m_rprf_dbg.data);
-    m_rprf_dbgOut.write();
-    m_rplf_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("lf"),m_rplf_dbg.data);
-    m_rplf_dbgOut.write();
-    m_rprh_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("rh"),m_rprh_dbg.data);
-    m_rprh_dbgOut.write();
-    m_rplh_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("lh"),m_rplh_dbg.data);
-    m_rplh_dbgOut.write();
-    m_rphead_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("head"),m_rphead_dbg.data);
-    m_rphead_dbgOut.write();
-    m_rpzmp_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.getP("zmp").p,m_rpzmp_dbg.data);
-    m_rpzmp_dbgOut.write();
-    m_rpdcp_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::Vector3ToPoint3D(hsp->cp_dec,m_rpdcp_dbg.data);
-    m_rpdcp_dbgOut.write();
-    m_rpacp_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::Vector3ToPoint3D(hsp->cp_acc,m_rpacp_dbg.data);
-    m_rpacp_dbgOut.write();
-    m_invdyn_dbg.tm = m_qRef.tm;
-    m_invdyn_dbg.data.length(6);
-    HumanSynchronizer::Wrench6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
-    m_invdyn_dbgOut.write();
-
     return RTC::RTC_OK;
 }
 
@@ -1225,262 +1039,6 @@ void AutoBalancer::solveFullbodyIK ()
   fik->solveFullbodyIK (dif_cog, transition_interpolator->isEmpty());
 }
 
-void AutoBalancer::processWholeBodyMasterSlave(){
-  // Set ik target params
-//  fik->target_root_p = target_root_p;
-//  fik->target_root_R = target_root_R;
-//  for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-//      it->second.target_p0 = ikp[it->first].target_p0;
-//      it->second.target_r0 = ikp[it->first].target_r0;
-//  }
-  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
-  fik->current_tm = m_qRef.tm;
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-      fik->ikp[it->first].is_ik_enable = it->second.is_active;
-  }
-//  // Revert
-//  fik->revertRobotStateToCurrent();
-
-  if(hsp->startCountdownForHumanSync){
-    std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
-    hsp->updateCountDown();
-  }
-  if(hsp->isHumanSyncOn()){
-    if(hsp->ht_first_call){
-      std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
-      hsp->ht_first_call = false;
-    }
-  }else{
-    hsp->setCurrentInputAsOffset(hsp->hp_wld_raw);
-    hsp->calibInitHumanCOMFromZMP();
-    hsp->rp_ref_out.getP("com").p_offs = m_robot->calcCM();
-    hsp->rp_ref_out.getP("zmp").p_offs = ref_zmp;
-    const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"}, human_l_names[4] = {"rf","lf","rh","lh"};
-    for(int i=0;i<4;i++){
-      if(ikp.count(robot_l_names[i])){
-        hsp->rp_ref_out.getP(human_l_names[i]).p_offs = ikp[robot_l_names[i]].target_link->p + ikp[robot_l_names[i]].target_link->R * ikp[robot_l_names[i]].localPos;
-        hsp->rp_ref_out.getP(human_l_names[i]).rpy_offs = hrp::rpyFromRot(ikp[robot_l_names[i]].target_link->R * ikp[robot_l_names[i]].localR);
-        hsp->rp_ref_out.getP(human_l_names[i]).p = hsp->rp_ref_out.getP(human_l_names[i]).p_offs;
-        hsp->rp_ref_out.getP(human_l_names[i]).rpy = hsp->rp_ref_out.getP(human_l_names[i]).rpy_offs;
-      }
-    }
-    hsp->pre_cont_rfpos = hsp->rp_ref_out.getP("rf").p_offs;
-    hsp->pre_cont_lfpos = hsp->rp_ref_out.getP("lf").p_offs;
-    hsp->baselinkpose.p_offs = m_robot->rootLink()->p;
-    hsp->baselinkpose.rpy_offs = hrp::rpyFromRot(m_robot->rootLink()->R);
-  }
-  hrp::Vector3 rsole_pos = ikp["rleg"].target_link->p+ ikp["rleg"].target_link->R * ikp["rleg"].localPos;
-  hrp::Vector3 lsole_pos = ikp["lleg"].target_link->p+ ikp["lleg"].target_link->R * ikp["lleg"].localPos;
-  hsp->H_cur = m_robot->calcCM()(2) - std::min((double)rsole_pos(2), (double)lsole_pos(2));
-  hsp->update();//////HumanSynchronizerの主要処理
-  if(loop%100==0)hsp->rp_ref_out.print();
-  m_robot->calcForwardKinematics();
-  // additional COM fitting IK for HumanSynchronizer
-  if(hsp->isHumanSyncOn()){
-    solveFullbodyIKStrictCOM( hsp->rp_ref_out.getP("com"), hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
-    //outport用のデータ上書き
-    ref_zmp = hsp->rp_ref_out.getP("zmp").p;
-    ref_cog = hsp->rp_ref_out.getP("com").p;
-  }
-}
-
-
-hrp::Vector3 torso_pos = hrp::Vector3::Zero();
-hrp::Vector3 torso_rot = hrp::Vector3::Zero();
-//void AutoBalancer::calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody){
-//  hrp::Vector3 torso_pos_acc,torso_rot_acc;
-//  static hrp::Vector3 torso_pos_vel,torso_rot_vel = hrp::Vector3::Zero();
-//  hrp::Matrix33 total_inertia;
-//  for(int i=0; i<m_robot->numLinks(); i++){
-//    total_inertia += m_robot->link(i)->I;
-//  }
-//  hrp::Vector3 zmp_err = zmp_fullbody - zmp_lip;
-//  if(fabs(zmp_err(0))<0.002)zmp_err(0)=0;
-//  if(fabs(zmp_err(1))<0.002)zmp_err(1)=0;
-//  LIMIT_MINMAX(zmp_err(0),-0.05,0.05);
-//  LIMIT_MINMAX(zmp_err(1),-0.05,0.05);
-//  zmp_err(2)=0;
-//  torso_pos_acc(0) = zmp_err(0) / 1.1 * 9.8;
-//  torso_pos_acc(1) = zmp_err(1) / 1.1 * 9.8;
-//  torso_rot_acc(0) = - zmp_err(1) * m_robot->totalMass() * 9.8 / (130*0.2*0.2);
-//  torso_rot_acc(1) = + zmp_err(0) * m_robot->totalMass() * 9.8 / (130*0.2*0.2);
-//
-//  torso_pos_acc(0) *= 0.5;
-//  torso_pos_acc(1) *= 0.5;
-//  torso_rot_acc(0) *= 0.1;
-//  torso_rot_acc(1) *= 0.1;
-//
-//
-//
-//  torso_pos_acc -= 1 * torso_pos + 1 * torso_pos_vel;
-//  torso_rot_acc -= 1 * torso_rot + 1 * torso_rot_vel;
-////  torso_rot_acc(0) -= 10 * torso_rot(0) + 10 * torso_rot_vel(0);
-////  torso_rot_acc(1) -= 10 * torso_rot(1) + 10 * torso_rot_vel(1);
-//
-//  LIMIT_MINMAX(torso_pos_acc(0),-10,10);
-//  LIMIT_MINMAX(torso_pos_acc(1),-10,10);
-//  LIMIT_MINMAX(torso_pos_acc(2),-0,0);
-//  LIMIT_MINMAX(torso_rot_acc(0),-10,10);
-//  LIMIT_MINMAX(torso_rot_acc(1),-10,10);
-//  LIMIT_MINMAX(torso_rot_acc(2),-0,0);
-//
-//
-//  if(hsp->isHumanSyncOn()){
-//    torso_rot_vel += torso_rot_acc * m_dt;
-//    torso_rot += torso_rot_vel * m_dt;
-//    torso_pos_vel += torso_pos_acc * m_dt;
-//    torso_pos += torso_pos_vel * m_dt;
-//  }
-//
-//  LIMIT_MINMAX(torso_pos(0),-0.1,0.1);
-//  LIMIT_MINMAX(torso_pos(1),-0.1,0.1);
-//  LIMIT_MINMAX(torso_pos(2),-0.1,0.1);
-//  LIMIT_MINMAX(torso_rot(0),-10*M_PI/180,10*M_PI/180);
-//  LIMIT_MINMAX(torso_rot(1),-10*M_PI/180,10*M_PI/180);
-//  LIMIT_MINMAX(torso_rot(2),-0.1,0.1);
-//
-//
-////  cout<<"total_inertia: "<<total_inertia<<endl;
-////  cout<<"zmp_lip: "<<zmp_lip.transpose()<<endl;
-////  cout<<"zmp_fullbody: "<<zmp_fullbody.transpose()<<endl;
-////  cout<<"torso_rot_acc: "<<torso_rot_acc.transpose()<<endl;
-////  cout<<"torso_rot_vel: "<<torso_rot_vel.transpose()<<endl;
-////  cout<<"torso_rot: "<<torso_rot.transpose()<<endl;
-//
-//
-//  fprintf(hsp->id_log,"zmp_lip: %f %f ",zmp_lip(0),zmp_lip(1),zmp_lip(2));
-//  fprintf(hsp->id_log,"zmp_fullbody: %f %f ",zmp_fullbody(0),zmp_fullbody(1),zmp_fullbody(2));
-//  fprintf(hsp->id_log,"zmp_fullbody2: %f %f ",ref_zmp_invdyn2(0),ref_zmp_invdyn2(1),ref_zmp_invdyn2(2));
-//  fprintf(hsp->id_log,"torso_pos_acc: %f %f ",torso_pos_acc(0),torso_pos_acc(1),torso_pos_acc(2));
-//  fprintf(hsp->id_log,"torso_pos: %f %f ",torso_pos(0),torso_pos(1),torso_pos(2));
-//  fprintf(hsp->id_log,"torso_rot_acc: %f %f ",torso_rot_acc(0),torso_rot_acc(1),torso_rot_acc(2));
-//  fprintf(hsp->id_log,"torso_rot: %f %f ",torso_rot(0),torso_rot(1),torso_rot(2));
-//  fprintf(hsp->id_log,"\n");
-//
-//
-//
-////  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
-////  fik->current_tm = m_qRef.tm;
-////  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-////      fik->ikp[it->first].is_ik_enable = it->second.is_active;
-////  }
-////  if(hsp->isHumanSyncOn()){
-////    if(hsp->ht_first_call){
-////      std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
-////      hsp->ht_first_call = false;
-////    }
-////  }
-////  m_robot->calcForwardKinematics();
-////  // additional COM fitting IK for HumanSynchronizer
-//  if(hsp->isHumanSyncOn()){
-//    HRPPose3D com_mod = hsp->rp_ref_out.getP("com");
-//    com_mod.p += torso_pos;
-//    com_mod.rpy += torso_rot;
-//    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
-//  }
-//}
-
-
-void AutoBalancer::calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody){
-
-
-
-//  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
-//  fik->current_tm = m_qRef.tm;
-//  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-//      fik->ikp[it->first].is_ik_enable = it->second.is_active;
-//  }
-////  // Revert
-//  fik->revertRobotStateToCurrent();
-
-//  hrp::dmatrix Jp;
-//  hrp::dmatrix Jl;
-//  m_robot->calcCM();
-//  m_robot->calcCMJacobian(NULL, Jp); // Eq.1 upper [M/m E -r]
-//  m_robot->calcAngularMomentumJacobian(NULL, Jl); // Eq.1 lower [H 0 I]
-//  Jp *= m_robot->totalMass();
-//  hrp::dmatrix Jpl(Jp.rows() + Jl.rows(), Jp.cols());
-//  Jpl << Jp, Jl;
-//  hrp::dvector all_vel_vec(3+3+m_robot->numJoints());
-//  all_vel_vec << idsb.base_v, idsb.base_w, idsb.dq;
-  hrp::Vector3 P,L;
-  m_robot->rootLink()->v = idsb.base_v;
-  m_robot->rootLink()->w = idsb.base_w;
-  m_robot->calcTotalMomentum(P,L);
-  L = L - m_robot->calcCM().cross(P);
-
-  torso_rot(0) -= L(0) * 0.1 * m_dt;
-  torso_rot(1) -= L(1) * 0.1 * m_dt;
-  torso_rot(2) -= L(2) * 0.1 * m_dt;
-
-
-
-  fprintf(hsp->id_log,"p: %f %f ",hsp->rp_ref_out.getP("com").p(0),hsp->rp_ref_out.getP("com").p(1),hsp->rp_ref_out.getP("com").p(2));
-  fprintf(hsp->id_log,"rpy: %f %f ",hsp->rp_ref_out.getP("com").rpy(0),hsp->rp_ref_out.getP("com").rpy(1),hsp->rp_ref_out.getP("com").rpy(2));
-  fprintf(hsp->id_log,"P: %f %f ",P(0),P(1),P(2));
-  fprintf(hsp->id_log,"L: %f %f ",L(0),L(1),L(2));
-//  fprintf(hsp->id_log,"p_v: %f %f ",idsb.base_v(0),idsb.base_v(1),idsb.base_v(2));
-//  fprintf(hsp->id_log,"rpy_v: %f %f ",idsb.base_w(0),idsb.base_w(1),idsb.base_w(2));
-    fprintf(hsp->id_log,"torso_pos: %f %f ",torso_pos(0),torso_pos(1),torso_pos(2));
-    fprintf(hsp->id_log,"torso_rot: %f %f ",torso_rot(0),torso_rot(1),torso_rot(2));
-  fprintf(hsp->id_log,"\n");
-
-  if(hsp->isHumanSyncOn()){
-    HRPPose3D com_mod = hsp->rp_ref_out.getP("com");
-//    com_mod.p += torso_pos;
-//    com_mod.rpy += torso_rot;
-    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
-  }
-}
-
-
-
-
-void AutoBalancer::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const hrp::Vector3& head_ref){
-  int com_ik_loop=0;
-  const int COM_IK_MAX_LOOP = 10;
-  const double COM_IK_MAX_ERROR = 1e-4;
-
-  cout<<""<<m_robot->rootLink()->p.transpose()<<" : "<<com_ref.p.transpose()<<endl;
-
-  m_robot->rootLink()->p = com_ref.p;//move base link at first
-//  m_robot->rootLink()->p = com_ref.p + hsp->torso_rot;//move base link at first
-  m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
-
-  //////////invdyncomp
-//  m_robot->rootLink()->R *= hrp::rotFromRpy(hsp->torso_rot);
-//  m_robot->rootLink()->p += torso_rot;
-
-  hrp::Vector3 tmp_com_err;
-  const std::string names[4] = {"rleg","lleg","rarm","larm"};
-  const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
-  for(int i=0;i<4;i++){
-    if(fik->ikp.count(names[i])){
-      fik->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);//convert End Effector pos into link origin pos
-      fik->ikp[names[i]].target_p0 = refs[i]->p;
-    }
-  }
-  if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref(2);
-  if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref(1);
-  //COM 収束ループ
-  while(
-      m_robot->calcForwardKinematics(),
-      tmp_com_err = com_ref.p - m_robot->calcCM(),
-//      tmp_com_err = com_ref.p + hsp->torso_rot - m_robot->calcCM(),
-      tmp_com_err.norm() > COM_IK_MAX_ERROR)
-  {
-    m_robot->rootLink()->p += tmp_com_err;
-    m_robot->calcForwardKinematics();
-    for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-        if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
-    }
-    if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
-  }
-
-  cout<<"COM_IK_MAX_LOOP:"<<com_ik_loop<<endl;
-}
-
-
 
 /*
   RTC::ReturnCode_t AutoBalancer::onAborting(RTC::UniqueId ec_id)
@@ -1517,52 +1075,6 @@ void AutoBalancer::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPP
   }
 */
 
-bool AutoBalancer::startCountDownForWholeBodyMasterSlave(const double sec)
-{
-  if(sec >= 0.0 && sec <= 30.0){
-    std::cerr << "[" << m_profile.instance_name << "] start Synchronization after "<<sec<<" [s]" << std::endl;
-    hsp->countdown_sec = sec;
-    hsp->startCountdownForHumanSync = true;
-    return true;
-  }else{
-    std::cerr << "[" << m_profile.instance_name << "] Count Down Time must be 0 < T < 30 [s]"<< std::endl;
-    return false;
-  }
-}
-
-bool AutoBalancer::stopHumanSync()
-{
-	std::cerr << "[" << m_profile.instance_name << "] stop HumanSync Now" << std::endl;
-	return true;
-}
-
-bool AutoBalancer::setWholeBodyMasterSlaveParam(const OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam& i_param){
-  std::cerr << "[" << m_profile.instance_name << "] setWholeBodyMasterSlaveParam" << std::endl;
-  hsp->WBMSparam.use_rh = hsp->WBMSparam.use_lh = i_param.use_hands;
-  hsp->WBMSparam.use_head = i_param.use_head;
-  hsp->WBMSparam.set_com_height_fix = i_param.set_com_height_fix;
-  hsp->WBMSparam.set_com_height_fix_val = i_param.set_com_height_fix_val;
-  hsp->WBMSparam.foot_vertical_vel_limit_coeff = i_param.foot_vertical_vel_limit_coeff;
-  hsp->WBMSparam.human_com_height = i_param.human_com_height;
-  hsp->WBMSparam.swing_foot_height_offset = i_param.swing_foot_height_offset;
-  hsp->WBMSparam.swing_foot_max_height = i_param.swing_foot_max_height;
-  hsp->WBMSparam.auto_swing_foot_landing_threshold = i_param.auto_swing_foot_landing_threshold;
-  return true;
-}
-bool AutoBalancer::getWholeBodyMasterSlaveParam(OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam& i_param){
-  std::cerr << "[" << m_profile.instance_name << "] getWholeBodyMasterSlaveParam" << std::endl;
-  i_param.set_com_height_fix = hsp->WBMSparam.set_com_height_fix;
-  i_param.set_com_height_fix_val = hsp->WBMSparam.set_com_height_fix_val;
-  i_param.foot_vertical_vel_limit_coeff = hsp->WBMSparam.foot_vertical_vel_limit_coeff;
-  i_param.human_com_height = hsp->WBMSparam.human_com_height;
-  i_param.use_hands = (hsp->WBMSparam.use_rh || hsp->WBMSparam.use_lh);
-  i_param.use_head = hsp->WBMSparam.use_head;
-  i_param.swing_foot_height_offset = hsp->WBMSparam.swing_foot_height_offset;
-  i_param.swing_foot_max_height = hsp->WBMSparam.swing_foot_max_height;
-  i_param.auto_swing_foot_landing_threshold = hsp->WBMSparam.auto_swing_foot_landing_threshold;
-  return true;
-}
-
 void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence& limbs)
 {
   std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 76cff6768bc..d80e305eea5 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -29,8 +29,10 @@
 #include "interpolator.h"
 #include "../TorqueFilter/IIRFilter.h"
 
-#include "HumanMasterSlave.h"
+// </rtc-template>
 
+// Service Consumer stub headers
+// <rtc-template block="consumer_stub_h">
 
 // </rtc-template>
 
@@ -392,10 +394,6 @@ class AutoBalancer
   bool getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx);
   bool getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep);
   bool releaseEmergencyStop();
-  bool startCountDownForWholeBodyMasterSlave(const double sec);
-  bool stopHumanSync();
-  bool setWholeBodyMasterSlaveParam(const OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam& i_param);
-  bool getWholeBodyMasterSlaveParam(OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam& i_param);
 
  protected:
   // Configuration variable declaration
@@ -421,70 +419,7 @@ class AutoBalancer
   InPort<TimedLong> m_emergencySignalIn;
   // for debug
   TimedPoint3D m_cog;
-  //for human tracker
-  TimedPose3D m_htcom;
-  InPort<TimedPose3D> m_htcomIn;
-  TimedPose3D m_htrf;
-  InPort<TimedPose3D> m_htrfIn;
-  TimedPose3D m_htlf;
-  InPort<TimedPose3D> m_htlfIn;
-  TimedPose3D m_htrh;
-  InPort<TimedPose3D> m_htrhIn;
-  TimedPose3D m_htlh;
-  InPort<TimedPose3D> m_htlhIn;
-  TimedPose3D m_hthead;
-  InPort<TimedPose3D> m_htheadIn;
-  TimedPoint3D m_htzmp;
-  InPort<TimedPoint3D> m_htzmpIn;
-  TimedPoint3D m_actzmp;
-  InPort<TimedPoint3D> m_actzmpIn;
-  TimedDoubleSeq m_htrfw;
-  InPort<TimedDoubleSeq> m_htrfwIn;
-  TimedDoubleSeq m_htlfw;
-  InPort<TimedDoubleSeq> m_htlfwIn;
-  //ishiguro dbg
-  TimedPose3D m_htcom_dbg;
-  OutPort<TimedPose3D> m_htcom_dbgOut;
-  TimedPose3D m_htrf_dbg;
-  OutPort<TimedPose3D> m_htrf_dbgOut;
-  TimedPose3D m_htlf_dbg;
-  OutPort<TimedPose3D> m_htlf_dbgOut;
-  TimedPose3D m_htrh_dbg;
-  OutPort<TimedPose3D> m_htrh_dbgOut;
-  TimedPose3D m_htlh_dbg;
-  OutPort<TimedPose3D> m_htlh_dbgOut;
-  TimedPose3D m_hthead_dbg;
-  OutPort<TimedPose3D> m_hthead_dbgOut;
-  TimedPoint3D m_htzmp_dbg;
-  OutPort<TimedPoint3D> m_htzmp_dbgOut;
-  TimedDoubleSeq m_htrfw_dbg;
-  OutPort<TimedDoubleSeq> m_htrfw_dbgOut;
-  TimedDoubleSeq m_htlfw_dbg;
-  OutPort<TimedDoubleSeq> m_htlfw_dbgOut;
-  TimedPose3D m_rpcom_dbg;
-  OutPort<TimedPose3D> m_rpcom_dbgOut;
-  TimedPose3D m_rprf_dbg;
-  OutPort<TimedPose3D> m_rprf_dbgOut;
-  TimedPose3D m_rplf_dbg;
-  OutPort<TimedPose3D> m_rplf_dbgOut;
-  TimedPose3D m_rprh_dbg;
-  OutPort<TimedPose3D> m_rprh_dbgOut;
-  TimedPose3D m_rplh_dbg;
-  OutPort<TimedPose3D> m_rplh_dbgOut;
-  TimedPose3D m_rphead_dbg;
-  OutPort<TimedPose3D> m_rphead_dbgOut;
-  TimedPoint3D m_rpzmp_dbg;
-  OutPort<TimedPoint3D> m_rpzmp_dbgOut;
-  TimedPoint3D m_rpdcp_dbg;
-  OutPort<TimedPoint3D> m_rpdcp_dbgOut;
-  TimedPoint3D m_rpacp_dbg;
-  OutPort<TimedPoint3D> m_rpacp_dbgOut;
-
-  TimedDoubleSeq m_invdyn_dbg;
-  OutPort<TimedDoubleSeq> m_invdyn_dbgOut;
   
-
-
   // </rtc-template>
 
   // DataOutPort declaration
@@ -543,9 +478,6 @@ class AutoBalancer
     bool is_active, has_toe_joint;
   };
   void getTargetParameters();
-  void solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const hrp::Vector3& head_ref);
-  void processWholeBodyMasterSlave();
-  void calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody);
   void solveFullbodyIK ();
   void startABCparam(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
   void stopABCparam();
@@ -624,17 +556,9 @@ class AutoBalancer
 
   hrp::InvDynStateBuffer idsb;
   std::vector<IIRFilter> invdyn_zmp_filters;
-
-  hrp::InvDynStateBuffer idsb2;
-  std::vector<IIRFilter> invdyn_zmp_filters2;
-
-
-  hrp::Vector3 ref_zmp_invdyn2;
-
-  //for HumanSynchronizer
-  boost::shared_ptr<HumanSynchronizer> hsp;
 };
 
+
 extern "C"
 {
   void AutoBalancerInit(RTC::Manager* manager);
diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.cpp b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
index 6d41e542690..e4faf4ab27e 100644
--- a/rtc/AutoBalancer/AutoBalancerService_impl.cpp
+++ b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
@@ -113,27 +113,5 @@ CORBA::Boolean AutoBalancerService_impl::releaseEmergencyStop()
 void AutoBalancerService_impl::autobalancer(AutoBalancer *i_autobalancer)
 {
   m_autobalancer = i_autobalancer;
-}
-
-CORBA::Boolean AutoBalancerService_impl::startCountDownForWholeBodyMasterSlave(CORBA::Double sec)
-{
-    return m_autobalancer->startCountDownForWholeBodyMasterSlave(sec);
-};
-
-CORBA::Boolean AutoBalancerService_impl::stopHumanSync()
-{
-    return m_autobalancer->stopHumanSync();
-};
-
-void AutoBalancerService_impl::setWholeBodyMasterSlaveParam(const OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam& i_param)
-{
-  m_autobalancer->setWholeBodyMasterSlaveParam(i_param);
-};
-
-void AutoBalancerService_impl::getWholeBodyMasterSlaveParam(OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam_out i_param)
-{
-//  i_param = new OpenHRP::WholeBodyMasterSlaveParam::AutoBalancerParam();
-//  return  m_autobalancer->getWholeBodyMasterSlaveParam(*i_param);
-  m_autobalancer->getWholeBodyMasterSlaveParam(i_param);
-};
+} 
 
diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.h b/rtc/AutoBalancer/AutoBalancerService_impl.h
index 56793b33cac..c6cfe9b04bf 100644
--- a/rtc/AutoBalancer/AutoBalancerService_impl.h
+++ b/rtc/AutoBalancer/AutoBalancerService_impl.h
@@ -34,10 +34,6 @@ class AutoBalancerService_impl
   CORBA::Boolean getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep , CORBA::Long& o_current_fs_idx);
   CORBA::Boolean getGoPosFootstepsSequence(CORBA::Double x, CORBA::Double y, CORBA::Double th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep);
   CORBA::Boolean releaseEmergencyStop();
-  CORBA::Boolean startCountDownForWholeBodyMasterSlave(const CORBA::Double sec);
-  CORBA::Boolean stopHumanSync();
-  void setWholeBodyMasterSlaveParam(const OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam& i_param);
-  void getWholeBodyMasterSlaveParam(OpenHRP::AutoBalancerService::WholeBodyMasterSlaveParam_out i_param);
   //
   //
   void autobalancer(AutoBalancer *i_autobalancer);
diff --git a/rtc/AutoBalancer/CMakeLists.txt b/rtc/AutoBalancer/CMakeLists.txt
index c9e496a3b13..82ff0af2bd2 100644
--- a/rtc/AutoBalancer/CMakeLists.txt
+++ b/rtc/AutoBalancer/CMakeLists.txt
@@ -1,5 +1,5 @@
-set(comp_sources AutoBalancer.cpp AutoBalancerService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp PreviewController.cpp GaitGenerator.cpp ../TorqueFilter/IIRFilter.cpp HumanMasterSlave.cpp)
-set(libs ${OpenCV_LIBRARIES} hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub)
+set(comp_sources AutoBalancer.cpp AutoBalancerService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp PreviewController.cpp GaitGenerator.cpp ../TorqueFilter/IIRFilter.cpp)
+set(libs hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub)
 add_library(AutoBalancer SHARED ${comp_sources})
 target_link_libraries(AutoBalancer ${libs})
 set_target_properties(AutoBalancer PROPERTIES PREFIX "")
@@ -37,7 +37,7 @@ add_test(testGaitGeneratorTest15 testGaitGenerator --test15 --use-gnuplot false)
 add_test(testGaitGeneratorTest16 testGaitGenerator --test16 --use-gnuplot false)
 
 install(TARGETS ${target}
-  RUNTIME DESTINATION bin CONFIGURATIONS Release Debug
-  LIBRARY DESTINATION lib CONFIGURATIONS Release Debug
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION lib
 )
 
diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h
index ea0be3e9764..4112560142b 100644
--- a/rtc/AutoBalancer/GaitGenerator.h
+++ b/rtc/AutoBalancer/GaitGenerator.h
@@ -355,7 +355,7 @@ namespace rats
               zmp_weight_interpolator->clear();
               double zmp_weight_initial_value[4] = {zmp_weight_map[RLEG], zmp_weight_map[LLEG], zmp_weight_map[RARM], zmp_weight_map[LARM]};
               zmp_weight_interpolator->set(zmp_weight_initial_value);
-              zmp_weight_interpolator->go(zmp_weight_array, 2.0, true);
+              zmp_weight_interpolator->setGoal(zmp_weight_array, 2.0, true);
           } else {
               std::cerr << "zmp_weight_map cannot be set because interpolating." << std::endl;
           }
diff --git a/rtc/WholeBodyMasterSlave/CMakeLists.txt b/rtc/WholeBodyMasterSlave/CMakeLists.txt
index 55d4cd15773..041aa57ef1a 100644
--- a/rtc/WholeBodyMasterSlave/CMakeLists.txt
+++ b/rtc/WholeBodyMasterSlave/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(comp_sources WholeBodyMasterSlave.cpp WholeBodyMasterSlaveService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp ../TorqueFilter/IIRFilter.cpp ../AutoBalancer/HumanMasterSlave.cpp ../AutoBalancer/AutoBalancer.h)
+set(comp_sources WholeBodyMasterSlave.cpp WholeBodyMasterSlaveService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp ../TorqueFilter/IIRFilter.cpp wbms_core.cpp ../AutoBalancer/AutoBalancer.h)
 set(libs ${OpenCV_LIBRARIES} hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub)
 add_library(WholeBodyMasterSlave SHARED ${comp_sources})
 target_link_libraries(WholeBodyMasterSlave ${libs})
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 06147e1f9f5..f67b257656f 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -20,8 +20,6 @@
 typedef coil::Guard<coil::Mutex> Guard;
 using namespace rats;
 
-// Module specification
-// <rtc-template block="module_spec">
 static const char* WholeBodyMasterSlave_spec[] =
     {
         "implementation_id", "WholeBodyMasterSlave",
@@ -38,18 +36,22 @@ static const char* WholeBodyMasterSlave_spec[] =
         "conf.default.debugLevel", "0",
         ""
     };
-// </rtc-template>
 
 WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager)
     : RTC::DataFlowComponentBase(manager),
-      // <rtc-template block="initializer">
+      // from sh
       m_qRefIn("qRef", m_qRef),
       m_zmpIn("zmpIn", m_zmp),
       m_basePosIn("basePosIn", m_basePos),
       m_baseRpyIn("baseRpyIn", m_baseRpy),
       m_optionalDataIn("optionalData", m_optionalData),
-      m_emergencySignalIn("emergencySignal", m_emergencySignal),
-      //ishiguro
+      // to abc
+      m_qOut("q", m_qRef),
+      m_zmpOut("zmpOut", m_zmp),
+      m_basePosOut("basePosOut", m_basePos),
+      m_baseRpyOut("baseRpyOut", m_baseRpy),
+      m_optionalDataOut("optionalDataOut", m_optionalData),
+      // from ros bridge
       m_htzmpIn("htzmpIn", m_htzmp),
       m_htrfwIn("htrfwIn", m_htrfw),
       m_htlfwIn("htlfwIn", m_htlfw),
@@ -60,7 +62,7 @@ WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager)
       m_htlhIn("htlhIn", m_htlh),
       m_actzmpIn("actzmpIn", m_actzmp),
       m_htheadIn("htheadIn", m_hthead),
-
+      // to ros bridge
       m_htcom_dbgOut("htcom_dbgOut", m_htcom_dbg),
       m_htrf_dbgOut("htrf_dbgOut", m_htrf_dbg),
       m_htlf_dbgOut("htlf_dbgOut", m_htlf_dbg),
@@ -79,26 +81,8 @@ WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager)
       m_rpzmp_dbgOut("rpzmp_dbgOut", m_rpzmp_dbg),
       m_rpdcp_dbgOut("rpdcp_dbgOut", m_rpdcp_dbg),
       m_rpacp_dbgOut("rpacp_dbgOut", m_rpacp_dbg),
-
       m_invdyn_dbgOut("invdyn_dbgOut", m_invdyn_dbg),
-
-      m_qOut("q", m_qRef),
-      m_zmpOut("zmpOut", m_zmp),
-      m_basePosOut("basePosOut", m_basePos),
-      m_baseRpyOut("baseRpyOut", m_baseRpy),
-      m_optionalDataOut("optionalDataOut", m_optionalData2),
-      m_baseTformOut("baseTformOut", m_baseTform),
-      m_basePoseOut("basePoseOut", m_basePose),
-      m_accRefOut("accRef", m_accRef),
-      m_contactStatesOut("contactStates", m_contactStates),
-      m_toeheelRatioOut("toeheelRatio", m_toeheelRatio),
-      m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime),
-      m_walkingStatesOut("walkingStates", m_walkingStates),
-      m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
-      m_cogOut("cogOut", m_cog),
       m_WholeBodyMasterSlaveServicePort("WholeBodyMasterSlaveService"),
-      // </rtc-template>
-//      gait_type(BIPED),
       m_robot(hrp::BodyPtr()),
       m_debugLevel(0)
 {
@@ -109,22 +93,23 @@ WholeBodyMasterSlave::~WholeBodyMasterSlave()
 {
 }
 
-
 RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
 {
     std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
     bindParameter("debugLevel", m_debugLevel, "0");
 
-    // Registration: InPort/OutPort/Service
-    // <rtc-template block="registration">
-    // Set InPort buffers
     addInPort("qRef", m_qRefIn);
     addInPort("zmpIn", m_zmpIn);
     addInPort("basePosIn", m_basePosIn);
     addInPort("baseRpyIn", m_baseRpyIn);
     addInPort("optionalData", m_optionalDataIn);
-    addInPort("emergencySignal", m_emergencySignalIn);
-    //ishiguro
+
+    addOutPort("q", m_qOut);
+    addOutPort("zmpOut", m_zmpOut);
+    addOutPort("basePosOut", m_basePosOut);
+    addOutPort("baseRpyOut", m_baseRpyOut);
+    addOutPort("optionalDataOut", m_optionalDataOut);
+
     addInPort("htzmpIn", m_htzmpIn);
     addInPort("htrfwIn", m_htrfwIn);
     addInPort("htlfwIn", m_htlfwIn);
@@ -154,39 +139,12 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
     addOutPort("rpzmp_dbgOut", m_rpzmp_dbgOut);
     addOutPort("rpdcp_dbgOut", m_rpdcp_dbgOut);
     addOutPort("rpacp_dbgOut", m_rpacp_dbgOut);
-
     addOutPort("invdyn_dbgOut", m_invdyn_dbgOut);
 
-    // Set OutPort buffer
-    addOutPort("q", m_qOut);
-    addOutPort("zmpOut", m_zmpOut);
-    addOutPort("basePosOut", m_basePosOut);
-    addOutPort("baseRpyOut", m_baseRpyOut);
-    addOutPort("optionalDataOut", m_optionalDataOut);
-//    addOutPort("baseTformOut", m_baseTformOut);
-//    addOutPort("basePoseOut", m_basePoseOut);
-//    addOutPort("accRef", m_accRefOut);
-//    addOutPort("contactStates", m_contactStatesOut);
-//    addOutPort("toeheelRatio", m_toeheelRatioOut);
-//    addOutPort("controlSwingSupportTime", m_controlSwingSupportTimeOut);
-//    addOutPort("cogOut", m_cogOut);
-//    addOutPort("walkingStates", m_walkingStatesOut);
-//    addOutPort("sbpCogOffset", m_sbpCogOffsetOut);
-  
-    // Set service provider to Ports
     m_WholeBodyMasterSlaveServicePort.registerProvider("service0", "WholeBodyMasterSlaveService", m_service0);
   
-    // Set service consumers to Ports
-  
-    // Set CORBA Service Ports
     addPort(m_WholeBodyMasterSlaveServicePort);
   
-    // </rtc-template>
-    // <rtc-template block="bind_config">
-    // Bind variables and configuration variable
-  
-    // </rtc-template>
-
     RTC::Properties& prop =  getProperties();
     coil::stringTo(m_dt, prop["dt"].c_str());
 
@@ -208,15 +166,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
 
     // allocate memory for outPorts
     m_qRef.data.length(m_robot->numJoints());
-    m_baseTform.data.length(12);
-    //ishiguro
     m_htrfw.data.length(6);
     m_htlfw.data.length(6);
-
-    m_optionalData2.data.length(4*2);//これいいのか？
-    for(int i=0;i<4*2;i++)m_optionalData2.data[i] = 0;
-
-//    control_mode = MODE_IDLE;
     loop = 0;
 //
 //    // setting from conf file
@@ -277,197 +228,41 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
         std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
         contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
       }
-//      m_contactStates.data.length(num);
-//      m_toeheelRatio.data.length(num);
-//      if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
-//        m_contactStates.data[contact_states_index_map["rleg"]] = true;
-//        m_contactStates.data[contact_states_index_map["lleg"]] = true;
-//      }
-//      if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) {
-//        m_contactStates.data[contact_states_index_map["rarm"]] = false;
-//        m_contactStates.data[contact_states_index_map["larm"]] = false;
-//      }
-//      m_controlSwingSupportTime.data.length(num);
-//      for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 0.0;
-//      for (size_t i = 0; i < num; i++) m_toeheelRatio.data[i] = rats::no_using_toe_heel_ratio;
     }
-//    std::vector<hrp::Vector3> leg_pos;
-//    if (leg_offset_str.size() > 0) {
-//      hrp::Vector3 leg_offset;
-//      for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
-//      std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-//      leg_pos.push_back(hrp::Vector3(-1*leg_offset));
-//      leg_pos.push_back(hrp::Vector3(leg_offset));
-//    }
-//    if (leg_pos.size() < ikp.size()) {
-//        size_t tmp_leg_pos_size = leg_pos.size();
-//        for (size_t i = 0; i < ikp.size() - tmp_leg_pos_size; i++) {
-//            leg_pos.push_back(hrp::Vector3::Zero());
-//        }
-//    }
-//
-//    std::vector<std::pair<hrp::Link*, hrp::Link*> > interlocking_joints;
-//    readInterlockingJointsParamFromProperties(interlocking_joints, m_robot, prop["interlocking_joints"], std::string(m_profile.instance_name));
-//    if (interlocking_joints.size() > 0) {
-//        fik->initializeInterlockingJoints(interlocking_joints);
-//    }
-//
-//    zmp_offset_interpolator = new interpolator(ikp.size()*3, m_dt);
-//    zmp_offset_interpolator->setName(std::string(m_profile.instance_name)+" zmp_offset_interpolator");
-//    zmp_transition_time = 1.0;
-//    transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-//    transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator");
-//    transition_interpolator_ratio = 0.0;
-//    adjust_footstep_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-//    adjust_footstep_interpolator->setName(std::string(m_profile.instance_name)+" adjust_footstep_interpolator");
-//    transition_time = 2.0;
-//    adjust_footstep_transition_time = 2.0;
-//    leg_names_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-//    leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator");
-//    leg_names_interpolator_ratio = 1.0;
-//
-//    // setting stride limitations from conf file
-//    double stride_fwd_x_limit = 0.15;
-//    double stride_y_limit = 0.05;
-//    double stride_th_limit = 10;
-//    double stride_bwd_x_limit = 0.05;
-//    std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_y_limit << "[m], " << stride_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl;
-//    if (default_zmp_offsets.size() == 0) {
-//      for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
-//    }
-//    if (leg_offset_str.size() > 0) {
-//      gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_y_limit/*[m]*/, stride_th_limit/*[deg]*/, stride_bwd_x_limit/*[m]*/));
-//      gg->set_default_zmp_offsets(default_zmp_offsets);
-//    }
-//    gg_is_walking = gg_solved = false;
-//    m_walkingStates.data = false;
-//    fix_leg_coords = coordinates();
-//
-//    // load virtual force sensors
-//    readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
-//    // ref force port
-//    unsigned int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
-//    unsigned int nvforce = m_vfs.size();
-//    unsigned int nforce  = npforce + nvforce;
-//    // check number of force sensors
-//    if (nforce < m_contactStates.data.length()) {
-//        std::cerr << "[" << m_profile.instance_name << "] WARNING! This robot model has less force sensors(" << nforce;
-//        std::cerr << ") than end-effector settings(" << m_contactStates.data.length() << ") !" << std::endl;
-//    }
-//
-//    m_ref_force.resize(nforce);
-//    m_ref_forceIn.resize(nforce);
-//    m_force.resize(nforce);
-//    m_ref_forceOut.resize(nforce);
-//    m_limbCOPOffset.resize(nforce);
-//    m_limbCOPOffsetOut.resize(nforce);
-//    for (unsigned int i=0; i<npforce; i++){
-//        sensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
-//    }
-//    for (unsigned int i=0; i<nvforce; i++){
-//        for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
-//            if (it->second.id == (int)i) sensor_names.push_back(it->first);
-//        }
-//    }
-//    // set ref force port
-//    std::cerr << "[" << m_profile.instance_name << "] force sensor ports (" << nforce << ")" << std::endl;
-//    for (unsigned int i=0; i<nforce; i++){
-//        m_ref_forceIn[i] = new InPort<TimedDoubleSeq>(std::string("ref_"+sensor_names[i]).c_str(), m_ref_force[i]);
-//        m_ref_force[i].data.length(6);
-//        registerInPort(std::string("ref_"+sensor_names[i]).c_str(), *m_ref_forceIn[i]);
-//        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string("ref_"+sensor_names[i]) << std::endl;
-//        ref_forces.push_back(hrp::Vector3(0,0,0));
-//    }
-//    // set force port
-//    for (unsigned int i=0; i<nforce; i++){
-//        m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string(sensor_names[i]).c_str(), m_force[i]);
-//        m_force[i].data.length(6);
-//        m_force[i].data[0] = m_force[i].data[1] = m_force[i].data[2] = 0.0;
-//        m_force[i].data[3] = m_force[i].data[4] = m_force[i].data[5] = 0.0;
-//        registerOutPort(std::string(sensor_names[i]).c_str(), *m_ref_forceOut[i]);
-//        std::cerr << "[" << m_profile.instance_name << "]   name = " << std::string(sensor_names[i]) << std::endl;
-//    }
-//    // set limb cop offset port
-//    std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << nforce << ")" << std::endl;
-//    for (unsigned int i=0; i<nforce; i++){
-//        std::string nm("limbCOPOffset_"+sensor_names[i]);
-//        m_limbCOPOffsetOut[i] = new OutPort<TimedPoint3D>(nm.c_str(), m_limbCOPOffset[i]);
-//        registerOutPort(nm.c_str(), *m_limbCOPOffsetOut[i]);
-//        m_limbCOPOffset[i].data.x = m_limbCOPOffset[i].data.y = m_limbCOPOffset[i].data.z = 0.0;
-//        std::cerr << "[" << m_profile.instance_name << "]   name = " << nm << std::endl;
-//    }
-//    sbp_offset = hrp::Vector3(0,0,0);
-//    sbp_cog_offset = hrp::Vector3(0,0,0);
-//    //use_force = MODE_NO_FORCE;
-//    use_force = MODE_REF_FORCE;
-//
     if (fik->ikp.find("rleg") != fik->ikp.end() && fik->ikp.find("lleg") != fik->ikp.end()) {
       is_legged_robot = true;
     } else {
       is_legged_robot = false;
     }
-//
-//    m_accRef.data.ax = m_accRef.data.ay = m_accRef.data.az = 0.0;
-//    prev_imu_sensor_vel = hrp::Vector3::Zero();
-//
-//    graspless_manip_mode = false;
-//    graspless_manip_arm = "arms";
-//    graspless_manip_p_gain = hrp::Vector3::Zero();
-//
-//    is_stop_mode = false;
-//    is_hand_fix_mode = false;
 
-    //for HumanSynchronizer
     hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer());
 //    if(leg_pos.size()>=2){ hsp->init_wld_rp_rfeepos = leg_pos[0]; hsp->init_wld_rp_lfeepos = leg_pos[1]; }
 
-//    hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
-//    if (sen == NULL) {
-//        std::cerr << "[" << m_profile.instance_name << "] WARNING! This robot model has no GyroSensor named 'gyrometer'! " << std::endl;
-//    }
     return RTC::RTC_OK;
 }
 
-
 RTC::ReturnCode_t WholeBodyMasterSlave::onFinalize()
 {
-//  delete zmp_offset_interpolator;
-//  delete transition_interpolator;
-//  delete adjust_footstep_interpolator;
-//  delete leg_names_interpolator;
   return RTC::RTC_OK;
 }
 
 RTC::ReturnCode_t WholeBodyMasterSlave::onActivated(RTC::UniqueId ec_id)
 {
     std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
-    
     return RTC::RTC_OK;
 }
 
 RTC::ReturnCode_t WholeBodyMasterSlave::onDeactivated(RTC::UniqueId ec_id)
 {
   std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
-//  Guard guard(m_mutex);
-//  if (control_mode == MODE_ABC) {
-//    control_mode = MODE_SYNC_TO_IDLE;
-//    double tmp_ratio = 0.0;
-//    transition_interpolator->setGoal(&tmp_ratio, m_dt, true); // sync in one controller loop
-//  }
   return RTC::RTC_OK;
 }
 
-
-
-
-#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
+//#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
 //#define DEBUGP2 ((loop%200==0))
-#define DEBUGP2 (false)
 RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
 {
   // std::cerr << "WholeBodyMasterSlave::onExecute(" << ec_id << ")" << std::endl;
-    loop ++;
-
     // Read Inport
     if (m_qRefIn.isNew()) {
         m_qRefIn.read();
@@ -488,17 +283,9 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
 //      input_zmp(1) = m_zmp.data.y;
 //      input_zmp(2) = m_zmp.data.z;
     }
-    for (unsigned int i=0; i<m_ref_forceIn.size(); i++){
-        if ( m_ref_forceIn[i]->isNew() ) {
-            m_ref_forceIn[i]->read();
-        }
-    }
     if (m_optionalDataIn.isNew()) {
         m_optionalDataIn.read();
     }
-    if (m_emergencySignalIn.isNew()){
-        m_emergencySignalIn.read();
-    }
     //for HumanSynchronizer
     if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
     if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hsp->hp_wld_raw.getw("rfw")); }
@@ -523,16 +310,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
 //    hrp::Matrix33 ref_baseRot;
 //    hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
     if ( is_legged_robot ) {
-//      // For parameters
-//      fik->storeCurrentParameters();
-//      getTargetParameters();
-//      // Get transition ratio
-//      bool is_transition_interpolator_empty = transition_interpolator->isEmpty();
-//      if (!is_transition_interpolator_empty) {
-//        transition_interpolator->get(&transition_interpolator_ratio, true);
-//      } else {
-//        transition_interpolator_ratio = (control_mode == MODE_IDLE) ? 0.0 : 1.0;
-//      }
         hsp->baselinkpose.p = m_robot->rootLink()->p;
         hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
         if(!hsp->isHumanSyncOn()){
@@ -542,10 +319,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
             m_robot->joint(i)->q = m_qRef.data[i];
           }
 
-//          cout<<"m_robot->calcCM()"<<m_robot->calcCM().transpose()<<endl;
-//          cout<<"m_robot->p"<<m_robot->rootLink()->p.transpose()<<endl;
-//          cout<<"m_robot->R"<<m_robot->rootLink()->R<<endl;
-
           m_robot->calcForwardKinematics();
 //          solveFullbodyIK();
         }
@@ -553,72 +326,36 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
         processWholeBodyMasterSlave();
 
         /////// Inverse Dynamics /////////
-//        if(!idsb.is_initialized){
-//          idsb.setInitState(m_robot, m_dt);
-//          invdyn_zmp_filters.resize(3);
-//          for(int i=0;i<3;i++){
-//            invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
+        if(!idsb.is_initialized){
+          idsb.setInitState(m_robot, m_dt);
+          invdyn_zmp_filters.resize(3);
+          for(int i=0;i<3;i++){
+            invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
 //            invdyn_zmp_filters[i].reset(ref_zmp(i));
-//          }
-//          idsb2.setInitState(m_robot, m_dt);
-//          invdyn_zmp_filters2.resize(3);
-//          for(int i=0;i<3;i++){
-//            invdyn_zmp_filters2[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
+          }
+          idsb2.setInitState(m_robot, m_dt);
+          invdyn_zmp_filters2.resize(3);
+          for(int i=0;i<3;i++){
+            invdyn_zmp_filters2[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
 //            invdyn_zmp_filters2[i].reset(ref_zmp(i));
-//          }
-//        }
-//        calcAccelerationsForInverseDynamics(m_robot, idsb);
-//        hrp::Vector3 ref_zmp_invdyn;
-//        calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);
-//        for(int i=0;i<3;i++) ref_zmp_invdyn(i) = invdyn_zmp_filters[i].passFilter(ref_zmp_invdyn(i));
-//        updateInvDynStateBuffer(idsb);
+          }
+        }
+        calcAccelerationsForInverseDynamics(m_robot, idsb);
+        hrp::Vector3 ref_zmp_invdyn;
+        calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);
+        for(int i=0;i<3;i++) ref_zmp_invdyn(i) = invdyn_zmp_filters[i].passFilter(ref_zmp_invdyn(i));
+        updateInvDynStateBuffer(idsb);
 
 
 //        calcDynamicsFilterCompensation(ref_zmp, ref_zmp_invdyn);
 
-
-//        calcAccelerationsForInverseDynamics(m_robot, idsb2);
-//        calcWorldZMPFromInverseDynamics(m_robot, idsb2, ref_zmp_invdyn2);
+        calcAccelerationsForInverseDynamics(m_robot, idsb2);
+        calcWorldZMPFromInverseDynamics(m_robot, idsb2, ref_zmp_invdyn2);
 //        for(int i=0;i<3;i++) ref_zmp_invdyn2(i) = invdyn_zmp_filters2[i].passFilter(ref_zmp_invdyn2(i));
-//        updateInvDynStateBuffer(idsb2);
-
+        updateInvDynStateBuffer(idsb2);
 
 //        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
-//      // Transition
-//      if (!is_transition_interpolator_empty) {
-//        // transition_interpolator_ratio 0=>1 : IDLE => ABC
-//        // transition_interpolator_ratio 1=>0 : ABC => IDLE
-//        ref_basePos = (1-transition_interpolator_ratio) * input_basePos + transition_interpolator_ratio * m_robot->rootLink()->p;
-//        rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp;
-//        rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R);
-//        for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ) {
-//          m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q;
-//        }
-//        for (unsigned int i=0; i< m_force.size(); i++) {
-//            for (unsigned int j=0; j<6; j++) {
-//                m_force[i].data[j] = transition_interpolator_ratio * m_force[i].data[j] + (1-transition_interpolator_ratio) * m_ref_force[i].data[j];
-//            }
-//        }
-//        for (unsigned int i=0; i< m_limbCOPOffset.size(); i++) {
-//            // transition (TODO:set stopABCmode value instead of 0)
-//            m_limbCOPOffset[i].data.x = transition_interpolator_ratio * m_limbCOPOffset[i].data.x;// + (1-transition_interpolator_ratio) * 0;
-//            m_limbCOPOffset[i].data.y = transition_interpolator_ratio * m_limbCOPOffset[i].data.y;// + (1-transition_interpolator_ratio) * 0;
-//            m_limbCOPOffset[i].data.z = transition_interpolator_ratio * m_limbCOPOffset[i].data.z;// + (1-transition_interpolator_ratio) * 0;
-//        }
-//      } else {
-//        ref_basePos = m_robot->rootLink()->p;
-//        ref_baseRot = m_robot->rootLink()->R;
-//      }
-//      // mode change for sync
-//      if (control_mode == MODE_SYNC_TO_ABC) {
-//        control_mode = MODE_ABC;
-//      } else if (control_mode == MODE_SYNC_TO_IDLE && transition_interpolator->isEmpty() ) {
-//        std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
-//                  << "] Finished cleanup" << std::endl;
-//        control_mode = MODE_IDLE;
-//      }
     }
-//
     // Write Outport
     if ( m_qRef.data.length() != 0 ) { // initialized
       if (is_legged_robot) {
@@ -640,79 +377,16 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
       m_baseRpy.data.p = baseRpy(1);
       m_baseRpy.data.y = baseRpy(2);
       m_baseRpy.tm = m_qRef.tm;
-//      // baseTform
-//      double *tform_arr = m_baseTform.data.get_buffer();
-//      tform_arr[0] = m_basePos.data.x;
-//      tform_arr[1] = m_basePos.data.y;
-//      tform_arr[2] = m_basePos.data.z;
-//      hrp::setMatrix33ToRowMajorArray(ref_baseRot, tform_arr, 3);
-//      m_baseTform.tm = m_qRef.tm;
-//      // basePose
-//      m_basePose.data.position.x = m_basePos.data.x;
-//      m_basePose.data.position.y = m_basePos.data.y;
-//      m_basePose.data.position.z = m_basePos.data.z;
-//      m_basePose.data.orientation.r = m_baseRpy.data.r;
-//      m_basePose.data.orientation.p = m_baseRpy.data.p;
-//      m_basePose.data.orientation.y = m_baseRpy.data.y;
-//      m_basePose.tm = m_qRef.tm;
       // zmp
       m_zmp.data.x = rel_ref_zmp(0);
       m_zmp.data.y = rel_ref_zmp(1);
       m_zmp.data.z = rel_ref_zmp(2);
       m_zmp.tm = m_qRef.tm;
-//      // cog
-//      m_cog.data.x = ref_cog(0);
-//      m_cog.data.y = ref_cog(1);
-//      m_cog.data.z = ref_cog(2);
-//      m_cog.tm = m_qRef.tm;
-//      // sbpCogOffset
-//      m_sbpCogOffset.data.x = sbp_cog_offset(0);
-//      m_sbpCogOffset.data.y = sbp_cog_offset(1);
-//      m_sbpCogOffset.data.z = sbp_cog_offset(2);
-//      m_sbpCogOffset.tm = m_qRef.tm;
-//      // write
+      // write
       m_basePosOut.write();
       m_baseRpyOut.write();
-//      m_baseTformOut.write();
-//      m_basePoseOut.write();
       m_zmpOut.write();
       m_optionalDataOut.write();
-//      m_cogOut.write();
-//      m_sbpCogOffsetOut.write();
-//
-//      // reference acceleration
-//      hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
-//      if (sen != NULL) {
-//          hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos;
-//          hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt;
-//          // convert to imu sensor local acceleration
-//          hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt;
-//          m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2);
-//          m_accRefOut.write();
-//          prev_imu_sensor_pos = imu_sensor_pos;
-//          prev_imu_sensor_vel = imu_sensor_vel;
-//      }
-//
-//      // control parameters
-//      m_contactStates.tm = m_qRef.tm;
-//      m_contactStatesOut.write();
-//      m_controlSwingSupportTime.tm = m_qRef.tm;
-//      m_controlSwingSupportTimeOut.write();
-//      m_toeheelRatio.tm = m_qRef.tm;
-//      m_toeheelRatioOut.write();
-//      m_walkingStates.data = gg_is_walking;
-//      m_walkingStates.tm = m_qRef.tm;
-//      m_walkingStatesOut.write();
-//
-//      for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
-//          m_force[i].tm = m_qRef.tm;
-//          m_ref_forceOut[i]->write();
-//      }
-//
-//      for (unsigned int i=0; i<m_limbCOPOffsetOut.size(); i++){
-//          m_limbCOPOffset[i].tm = m_qRef.tm;
-//          m_limbCOPOffsetOut[i]->write();
-//      }
     }
 
     //ishiguro dbg plot
@@ -777,32 +451,18 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
     HumanSynchronizer::Wrench6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
     m_invdyn_dbgOut.write();
 
+    loop ++;
     return RTC::RTC_OK;
 }
 
 void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
-  // Set ik target params
-//  fik->target_root_p = target_root_p;
-//  fik->target_root_R = target_root_R;
-//  for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-//      it->second.target_p0 = ikp[it->first].target_p0;
-//      it->second.target_r0 = ikp[it->first].target_r0;
-//  }
 //  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
   fik->current_tm = m_qRef.tm;
-//  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-//      fik->ikp[it->first].is_ik_enable = it->second.is_active;
-//  }
-
   fik->ikp["rleg"].is_ik_enable = true;
   fik->ikp["lleg"].is_ik_enable = true;
   fik->ikp["rarm"].is_ik_enable = true;
   fik->ikp["larm"].is_ik_enable = true;
 
-
-//  // Revert
-//  fik->revertRobotStateToCurrent();
-
   if(hsp->startCountdownForHumanSync){
     std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
     hsp->updateCountDown();
@@ -813,7 +473,6 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
       hsp->ht_first_call = false;
     }
   }else{
-//    fik->revertRobotStateToCurrent();
     fik->storeCurrentParameters();
     fik->setReferenceJointAngles();
     hsp->setCurrentInputAsOffset(hsp->hp_wld_raw);
@@ -830,13 +489,6 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
         hsp->rp_ref_out.getP(human_l_names[i]).rpy = hsp->rp_ref_out.getP(human_l_names[i]).rpy_offs;
       }
     }
-//
-//    cout<<"r_"<<hsp->rp_ref_out.getP("rh").rpy.transpose()<<endl;
-//    cout<<"r_offs"<<hsp->rp_ref_out.getP("rh").rpy_offs.transpose()<<endl;
-//    cout<<"l_"<<hsp->rp_ref_out.getP("lh").rpy.transpose()<<endl;
-//    cout<<"l_offs"<<hsp->rp_ref_out.getP("lh").rpy_offs.transpose()<<endl;
-
-
     hsp->pre_cont_rfpos = hsp->rp_ref_out.getP("rf").p_offs;
     hsp->pre_cont_lfpos = hsp->rp_ref_out.getP("lf").p_offs;
     hsp->baselinkpose.p_offs = m_robot->rootLink()->p;
@@ -848,51 +500,21 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
   hsp->update();//////HumanSynchronizerの主要処理
   if(loop%100==0)hsp->rp_ref_out.print();
   m_robot->calcForwardKinematics();
-  // additional COM fitting IK for HumanSynchronizer
   if(hsp->isHumanSyncOn()){
-
-//    for(int i=0;i<m_robot->numJoints();i++){ cout <<m_robot->joint(i)->name<<" "<<m_robot->joint(i)->q<<endl; }
     solveFullbodyIKStrictCOM( hsp->rp_ref_out.getP("com"), hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
-
-//    for(int i=0;i<m_robot->numJoints();i++){ cout <<m_robot->joint(i)->name<<" "<<m_robot->joint(i)->q<<endl; }
-//    exit();
     //outport用のデータ上書き
 //    ref_zmp = hsp->rp_ref_out.getP("zmp").p;
 //    ref_cog = hsp->rp_ref_out.getP("com").p;
-
     rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.getP("zmp").p - m_robot->rootLink()->p);
-
-//    m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
-//    m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
-//
-//
-//    for ( std::map<std::string, ABCIKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-//        m_contactStates.data[contact_states_index_map[it->first]] = isOptionalDataContact(it->first);
-//        m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = m_optionalData.data[contact_states_index_map[it->first]+contact_states_index_map.size()];
-//    }
-//
-    m_optionalData2.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
-    m_optionalData2.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
-
-
-//    for(int i=0;i<m_optionalData2.data.length();i++){
-//      cout<<"m_optionalData2.data["<<i<<"]:"<<m_optionalData2.data[i]<<endl;
-//    }
-
-//        cout<<"rleg"<<m_optionalData.data[contact_states_index_map["rleg"]]<<endl;
-//        cout<<"lleg"<<m_optionalData.data[contact_states_index_map["lleg"]]<<endl;
-
+    if(m_optionalData.data.length() < 4*2){
+      m_optionalData.data.length(4*2);//これいいのか？
+      for(int i=0;i<4*2;i++)m_optionalData.data[i] = 0;
+    }
+    m_optionalData.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
+    m_optionalData.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
   }else{
     rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.getP("zmp").p - m_robot->rootLink()->p);
   }
-
-//  cout<<"len2:"<<m_optionalData2.data.length()<<endl;
-
-
-
-
-
-//  for(int i=0;i<m_robot->numJoints();i++){ cout <<m_robot->joint(i)->name<<" "<<m_robot->joint(i)->q<<endl; }
 }
 //
 //
@@ -992,76 +614,59 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
 ////}
 //
 //
-//void WholeBodyMasterSlave::calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody){
-//
-//
-//
-////  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
-////  fik->current_tm = m_qRef.tm;
-////  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-////      fik->ikp[it->first].is_ik_enable = it->second.is_active;
-////  }
-//////  // Revert
-////  fik->revertRobotStateToCurrent();
-//
-////  hrp::dmatrix Jp;
-////  hrp::dmatrix Jl;
-////  m_robot->calcCM();
-////  m_robot->calcCMJacobian(NULL, Jp); // Eq.1 upper [M/m E -r]
-////  m_robot->calcAngularMomentumJacobian(NULL, Jl); // Eq.1 lower [H 0 I]
-////  Jp *= m_robot->totalMass();
-////  hrp::dmatrix Jpl(Jp.rows() + Jl.rows(), Jp.cols());
-////  Jpl << Jp, Jl;
-////  hrp::dvector all_vel_vec(3+3+m_robot->numJoints());
-////  all_vel_vec << idsb.base_v, idsb.base_w, idsb.dq;
-//  hrp::Vector3 P,L;
-//  m_robot->rootLink()->v = idsb.base_v;
-//  m_robot->rootLink()->w = idsb.base_w;
-//  m_robot->calcTotalMomentum(P,L);
-//  L = L - m_robot->calcCM().cross(P);
-//
+void WholeBodyMasterSlave::calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody){
+
+
+//  hrp::dmatrix Jp;
+//  hrp::dmatrix Jl;
+//  m_robot->calcCM();
+//  m_robot->calcCMJacobian(NULL, Jp); // Eq.1 upper [M/m E -r]
+//  m_robot->calcAngularMomentumJacobian(NULL, Jl); // Eq.1 lower [H 0 I]
+//  Jp *= m_robot->totalMass();
+//  hrp::dmatrix Jpl(Jp.rows() + Jl.rows(), Jp.cols());
+//  Jpl << Jp, Jl;
+//  hrp::dvector all_vel_vec(3+3+m_robot->numJoints());
+//  all_vel_vec << idsb.base_v, idsb.base_w, idsb.dq;
+  hrp::Vector3 P,L;
+  m_robot->rootLink()->v = idsb.base_v;
+  m_robot->rootLink()->w = idsb.base_w;
+  m_robot->calcTotalMomentum(P,L);
+  L = L - m_robot->calcCM().cross(P);
+
 //  torso_rot(0) -= L(0) * 0.1 * m_dt;
 //  torso_rot(1) -= L(1) * 0.1 * m_dt;
 //  torso_rot(2) -= L(2) * 0.1 * m_dt;
-//
-//
-//
-//  fprintf(hsp->id_log,"p: %f %f ",hsp->rp_ref_out.getP("com").p(0),hsp->rp_ref_out.getP("com").p(1),hsp->rp_ref_out.getP("com").p(2));
-//  fprintf(hsp->id_log,"rpy: %f %f ",hsp->rp_ref_out.getP("com").rpy(0),hsp->rp_ref_out.getP("com").rpy(1),hsp->rp_ref_out.getP("com").rpy(2));
-//  fprintf(hsp->id_log,"P: %f %f ",P(0),P(1),P(2));
-//  fprintf(hsp->id_log,"L: %f %f ",L(0),L(1),L(2));
-////  fprintf(hsp->id_log,"p_v: %f %f ",idsb.base_v(0),idsb.base_v(1),idsb.base_v(2));
-////  fprintf(hsp->id_log,"rpy_v: %f %f ",idsb.base_w(0),idsb.base_w(1),idsb.base_w(2));
+
+
+
+  fprintf(hsp->id_log,"p: %f %f ",hsp->rp_ref_out.getP("com").p(0),hsp->rp_ref_out.getP("com").p(1),hsp->rp_ref_out.getP("com").p(2));
+  fprintf(hsp->id_log,"rpy: %f %f ",hsp->rp_ref_out.getP("com").rpy(0),hsp->rp_ref_out.getP("com").rpy(1),hsp->rp_ref_out.getP("com").rpy(2));
+  fprintf(hsp->id_log,"P: %f %f ",P(0),P(1),P(2));
+  fprintf(hsp->id_log,"L: %f %f ",L(0),L(1),L(2));
+//  fprintf(hsp->id_log,"p_v: %f %f ",idsb.base_v(0),idsb.base_v(1),idsb.base_v(2));
+//  fprintf(hsp->id_log,"rpy_v: %f %f ",idsb.base_w(0),idsb.base_w(1),idsb.base_w(2));
 //    fprintf(hsp->id_log,"torso_pos: %f %f ",torso_pos(0),torso_pos(1),torso_pos(2));
 //    fprintf(hsp->id_log,"torso_rot: %f %f ",torso_rot(0),torso_rot(1),torso_rot(2));
-//  fprintf(hsp->id_log,"\n");
-//
-//  if(hsp->isHumanSyncOn()){
-//    HRPPose3D com_mod = hsp->rp_ref_out.getP("com");
-////    com_mod.p += torso_pos;
-////    com_mod.rpy += torso_rot;
-//    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
-//  }
-//}
-//
-//
-//
+  fprintf(hsp->id_log,"\n");
+
+  if(hsp->isHumanSyncOn()){
+    HRPPose3D com_mod = hsp->rp_ref_out.getP("com");
+//    com_mod.p += torso_pos;
+//    com_mod.rpy += torso_rot;
+    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
+  }
+}
+
+
+
 
 void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const hrp::Vector3& head_ref){
   int com_ik_loop=0;
   const int COM_IK_MAX_LOOP = 10;
   const double COM_IK_MAX_ERROR = 1e-4;
-
-//  cout<<""<<m_robot->rootLink()->p.transpose()<<" : "<<com_ref.p.transpose()<<endl;
-
   m_robot->rootLink()->p = com_ref.p;//move base link at first
-//  m_robot->rootLink()->p = com_ref.p + hsp->torso_rot;//move base link at first
   m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
 
-  //////////invdyncomp
-//  m_robot->rootLink()->R *= hrp::rotFromRpy(hsp->torso_rot);
-//  m_robot->rootLink()->p += torso_rot;
-
   hrp::Vector3 tmp_com_err;
   const std::string names[4] = {"rleg","lleg","rarm","larm"};
   const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
@@ -1104,7 +709,7 @@ bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double se
 
 bool WholeBodyMasterSlave::stopHumanSync()
 {
-	std::cerr << "[" << m_profile.instance_name << "] stop HumanSync Now" << std::endl;
+	std::cerr << "[" << m_profile.instance_name << "] not implemented..." << std::endl;
 	return true;
 }
 
@@ -1121,6 +726,7 @@ bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBody
   hsp->WBMSparam.auto_swing_foot_landing_threshold = i_param.auto_swing_foot_landing_threshold;
   return true;
 }
+
 bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
   std::cerr << "[" << m_profile.instance_name << "] getWholeBodyMasterSlaveParam" << std::endl;
   i_param.set_com_height_fix = hsp->WBMSparam.set_com_height_fix;
@@ -1135,7 +741,6 @@ bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMaster
   return true;
 }
 
-//
 extern "C"
 {
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 26260a8daa3..df5814e97bf 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -27,11 +27,9 @@
 #include "WholeBodyMasterSlaveService_impl.h"
 #include "interpolator.h"
 #include "../TorqueFilter/IIRFilter.h"
-#include "../AutoBalancer/HumanMasterSlave.h"
+#include "wbms_core.h"
 #include "../AutoBalancer/AutoBalancer.h"
 
-// </rtc-template>
-
 using namespace RTC;
 
 //static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
@@ -73,13 +71,7 @@ class WholeBodyMasterSlave
   InPort<TimedPoint3D> m_zmpIn;
   TimedDoubleSeq m_optionalData;
   InPort<TimedDoubleSeq> m_optionalDataIn;
-  std::vector<TimedDoubleSeq> m_ref_force;
-  std::vector<InPort<TimedDoubleSeq> *> m_ref_forceIn;
-  TimedLong m_emergencySignal;
-  InPort<TimedLong> m_emergencySignalIn;
-  // for debug
-  TimedPoint3D m_cog;
-  //for human tracker
+
   TimedPose3D m_htcom;
   InPort<TimedPose3D> m_htcomIn;
   TimedPose3D m_htrf;
@@ -100,7 +92,7 @@ class WholeBodyMasterSlave
   InPort<TimedDoubleSeq> m_htrfwIn;
   TimedDoubleSeq m_htlfw;
   InPort<TimedDoubleSeq> m_htlfwIn;
-  //ishiguro dbg
+
   TimedPose3D m_htcom_dbg;
   OutPort<TimedPose3D> m_htcom_dbgOut;
   TimedPose3D m_htrf_dbg;
@@ -137,7 +129,6 @@ class WholeBodyMasterSlave
   OutPort<TimedPoint3D> m_rpdcp_dbgOut;
   TimedPoint3D m_rpacp_dbg;
   OutPort<TimedPoint3D> m_rpacp_dbgOut;
-
   TimedDoubleSeq m_invdyn_dbg;
   OutPort<TimedDoubleSeq> m_invdyn_dbgOut;
 
@@ -145,31 +136,7 @@ class WholeBodyMasterSlave
   RTC::OutPort<RTC::TimedPoint3D> m_zmpOut;
   OutPort<TimedPoint3D> m_basePosOut;
   OutPort<TimedOrientation3D> m_baseRpyOut;
-  TimedDoubleSeq m_baseTform;
-  OutPort<TimedDoubleSeq> m_baseTformOut;
-  TimedDoubleSeq m_optionalData2;
   OutPort<TimedDoubleSeq> m_optionalDataOut;
-  TimedPose3D m_basePose;
-  OutPort<TimedPose3D> m_basePoseOut;
-  TimedAcceleration3D m_accRef;
-  OutPort<TimedAcceleration3D> m_accRefOut;
-  TimedBooleanSeq m_contactStates;
-  OutPort<TimedBooleanSeq> m_contactStatesOut;
-  TimedDoubleSeq m_toeheelRatio;
-  OutPort<TimedDoubleSeq> m_toeheelRatioOut;
-  TimedDoubleSeq m_controlSwingSupportTime;
-  OutPort<TimedDoubleSeq> m_controlSwingSupportTimeOut;
-  TimedBoolean m_walkingStates;
-  OutPort<TimedBoolean> m_walkingStatesOut;
-  TimedPoint3D m_sbpCogOffset;
-  OutPort<TimedPoint3D> m_sbpCogOffsetOut;
-  std::vector<TimedDoubleSeq> m_force;
-  std::vector<OutPort<TimedDoubleSeq> *> m_ref_forceOut;
-  std::vector<TimedPoint3D> m_limbCOPOffset;
-  std::vector<OutPort<TimedPoint3D> *> m_limbCOPOffsetOut;
-  // for debug
-  OutPort<TimedPoint3D> m_cogOut;
-  
   RTC::CorbaPort m_WholeBodyMasterSlaveServicePort;
 
   WholeBodyMasterSlaveService_impl m_service0;
@@ -181,87 +148,26 @@ class WholeBodyMasterSlave
     hrp::Link* target_link;
     bool is_active, has_toe_joint;
   };
-//  void getTargetParameters();
   void solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const hrp::Vector3& head_ref);
   void processWholeBodyMasterSlave();
-//  void calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody);
-//  void solveFullbodyIK ();
-//  void startABCparam(const ::OpenHRP::WholeBodyMasterSlaveService::StrSequence& limbs);
-//  void stopABCparam();
-//  void waitABCTransition();
-//  // Functions to calculate parameters for ABC output.
-//  // Output parameters are EE, limbCOPOffset, contactStates, controlSwingSupportTime, toeheelPhaseRatio
-//  void getOutputParametersForWalking ();
-//  void getOutputParametersForABC ();
-//  void getOutputParametersForIDLE ();
-//  void interpolateLegNamesAndZMPOffsets();
-//  void calcFixCoordsForAdjustFootstep (rats::coordinates& tmp_fix_coords);
-//  void rotateRefForcesForFixCoords (rats::coordinates& tmp_fix_coords);
-//  void updateTargetCoordsForHandFixMode (rats::coordinates& tmp_fix_coords);
-//  void calculateOutputRefForces ();
-//  hrp::Vector3 calcFootMidPosUsingZMPWeightMap ();
-//  void updateWalkingVelocityFromHandError (rats::coordinates& tmp_fix_coords);
-//  void calcReferenceJointAnglesForIK ();
-//  hrp::Matrix33 OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2);
-//  void fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot);
-//  void fixLegToCoords2 (rats::coordinates& tmp_fix_coords);
-//  bool startWalking ();
-//  void stopWalking ();
-//  void copyRatscoords2Footstep(OpenHRP::WholeBodyMasterSlaveService::Footstep& out_fs, const rats::coordinates& in_fs);
-//  // static balance point offsetting
-//  void static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height);
-//  void calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces);
-//  hrp::Vector3 calc_vel_from_hand_error (const rats::coordinates& tmp_fix_coords);
+  void calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody);
   bool isOptionalDataContact (const std::string& ee_name)
   {
       return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false;
   };
-//  bool calc_inital_support_legs(const double& y, std::vector<rats::coordinates>& initial_support_legs_coords, std::vector<rats::leg_type>& initial_support_legs, rats::coordinates& start_ref_coords);
-//
-//  // for gg
-//  typedef boost::shared_ptr<rats::gait_generator> ggPtr;
-//  ggPtr gg;
-//  bool gg_is_walking, gg_solved;
-//  // for abc
   typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
   fikPtr fik;
-//  hrp::Vector3 ref_cog, ref_zmp, prev_imu_sensor_pos, prev_imu_sensor_vel, hand_fix_initial_offset;
-//  enum {BIPED, TROT, PACE, CRAWL, GALLOP} gait_type;
-//  enum {MODE_IDLE, MODE_ABC, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_ABC} control_mode;
-//  std::map<std::string, ABCIKparam> ikp;
   std::map<std::string, size_t> contact_states_index_map;
-//  std::map<std::string, hrp::VirtualForceSensorParam> m_vfs;
-//  std::vector<std::string> sensor_names, leg_names, ee_vec;
-//  hrp::Vector3 target_root_p;
-//  hrp::Matrix33 target_root_R;
-//  rats::coordinates fix_leg_coords;
-//  std::vector<hrp::Vector3> default_zmp_offsets;
   double m_dt;
   hrp::BodyPtr m_robot;
-//  coil::Mutex m_mutex;
-//
-//  double transition_interpolator_ratio, transition_time, zmp_transition_time, adjust_footstep_transition_time, leg_names_interpolator_ratio;
-//  interpolator *zmp_offset_interpolator;
-//  interpolator *transition_interpolator;
-//  interpolator *adjust_footstep_interpolator;
-//  interpolator *leg_names_interpolator;
   hrp::Vector3 input_zmp, input_basePos;
   hrp::Matrix33 input_baseRot;
 
   hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
 
-//  // static balance point offsetting
-//  hrp::Vector3 sbp_offset, sbp_cog_offset;
-//  enum {MODE_NO_FORCE, MODE_REF_FORCE} use_force;
-//  std::vector<hrp::Vector3> ref_forces;
-//
   unsigned int m_debugLevel;
   bool is_legged_robot;// is_stop_mode, is_hand_fix_mode, is_hand_fix_initial;
   int loop;
-//  bool graspless_manip_mode;
-//  std::string graspless_manip_arm;
-//  hrp::Vector3 graspless_manip_p_gain;
-//  rats::coordinates graspless_manip_reference_trans_coords;
 
   hrp::InvDynStateBuffer idsb;
   std::vector<IIRFilter> invdyn_zmp_filters;
@@ -269,10 +175,8 @@ class WholeBodyMasterSlave
   hrp::InvDynStateBuffer idsb2;
   std::vector<IIRFilter> invdyn_zmp_filters2;
 
-
   hrp::Vector3 ref_zmp_invdyn2;
 
-  //for HumanSynchronizer
   boost::shared_ptr<HumanSynchronizer> hsp;
 };
 
diff --git a/rtc/AutoBalancer/HumanMasterSlave.cpp b/rtc/WholeBodyMasterSlave/wbms_core.cpp
similarity index 99%
rename from rtc/AutoBalancer/HumanMasterSlave.cpp
rename to rtc/WholeBodyMasterSlave/wbms_core.cpp
index fbb05e329ae..bbaa1c364fe 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/wbms_core.cpp
@@ -1,4 +1,4 @@
-#include "HumanMasterSlave.h"
+#include "wbms_core.h"
 
 void HumanSynchronizer::calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const Wrench6& rfwin, const Wrench6& lfwin, hrp::Vector3& zmp_ans){
   hrp::Vector3 rfzmp,lfzmp;
diff --git a/rtc/AutoBalancer/HumanMasterSlave.h b/rtc/WholeBodyMasterSlave/wbms_core.h
similarity index 99%
rename from rtc/AutoBalancer/HumanMasterSlave.h
rename to rtc/WholeBodyMasterSlave/wbms_core.h
index 726c68cb554..ae0b33bafdf 100644
--- a/rtc/AutoBalancer/HumanMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -252,7 +252,7 @@ class HumanSynchronizer{
       tgt_FUP_TIME = FUP_TIME = 0.4;
       FUP_HIGHT = 0.05;
       CNT_F_TH = 20.0;
-      MAXVEL = 0.4;
+      MAXVEL = 0.4*10;
       HumanSyncOn = false;
       ht_first_call = true;
       startCountdownForHumanSync = false;

From 320e790a4a34005eb5d9f5e2ccbb4759fa6ac52d Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 22 Apr 2017 22:50:30 +0900
Subject: [PATCH 069/217] change connection of datalogger

---
 python/hrpsys_config.py              | 44 +++++++++++++++-------------
 rtc/WholeBodyMasterSlave/wbms_core.h |  4 +--
 2 files changed, 26 insertions(+), 22 deletions(-)

diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index 558e0049c91..f4c07e9d1bb 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -896,6 +896,30 @@ def setupLogger(self, maxLength=4000):
             self.connectLoggerPort(self.sh, 'zmpOut')
         if self.ic != None:
             self.connectLoggerPort(self.ic, 'q')
+        if self.wbms != None:
+            self.connectLoggerPort(self.wbms, 'q')
+            self.connectLoggerPort(self.wbms, 'basePosOut')
+            self.connectLoggerPort(self.wbms, 'baseRpyOut')
+            self.connectLoggerPort(self.wbms, 'zmpOut')
+            self.connectLoggerPort(self.wbms, 'baseTformOut')
+            self.connectLoggerPort(self.wbms, 'htcom_dbgOut')
+            self.connectLoggerPort(self.wbms, 'htrf_dbgOut')
+            self.connectLoggerPort(self.wbms, 'htlf_dbgOut')
+            self.connectLoggerPort(self.wbms, 'htrh_dbgOut')
+            self.connectLoggerPort(self.wbms, 'htlh_dbgOut')
+            self.connectLoggerPort(self.wbms, 'hthead_dbgOut')
+            self.connectLoggerPort(self.wbms, 'htzmp_dbgOut')
+            self.connectLoggerPort(self.wbms, 'htrfw_dbgOut')
+            self.connectLoggerPort(self.wbms, 'htlfw_dbgOut')
+            self.connectLoggerPort(self.wbms, 'rpcom_dbgOut')
+            self.connectLoggerPort(self.wbms, 'rprf_dbgOut')
+            self.connectLoggerPort(self.wbms, 'rplf_dbgOut')
+            self.connectLoggerPort(self.wbms, 'rprh_dbgOut')
+            self.connectLoggerPort(self.wbms, 'rplh_dbgOut')
+            self.connectLoggerPort(self.wbms, 'rphead_dbgOut')
+            self.connectLoggerPort(self.wbms, 'rpzmp_dbgOut')
+            self.connectLoggerPort(self.wbms, 'rpdcp_dbgOut')
+            self.connectLoggerPort(self.wbms, 'rpacp_dbgOut')
         if self.abc != None:
             self.connectLoggerPort(self.abc, 'zmpOut')
             self.connectLoggerPort(self.abc, 'baseTformOut')
@@ -903,26 +927,6 @@ def setupLogger(self, maxLength=4000):
             self.connectLoggerPort(self.abc, 'contactStates')
             self.connectLoggerPort(self.abc, 'controlSwingSupportTime')
             self.connectLoggerPort(self.abc, 'cogOut')
-            ### ishiguro ###
-            self.connectLoggerPort(self.abc, 'controlSwingSupportTime')
-            self.connectLoggerPort(self.abc, 'htcom_dbgOut')
-            self.connectLoggerPort(self.abc, 'htrf_dbgOut')
-            self.connectLoggerPort(self.abc, 'htlf_dbgOut')
-            self.connectLoggerPort(self.abc, 'htrh_dbgOut')
-            self.connectLoggerPort(self.abc, 'htlh_dbgOut')
-            self.connectLoggerPort(self.abc, 'hthead_dbgOut')
-            self.connectLoggerPort(self.abc, 'htzmp_dbgOut')
-            self.connectLoggerPort(self.abc, 'htrfw_dbgOut')
-            self.connectLoggerPort(self.abc, 'htlfw_dbgOut')
-            self.connectLoggerPort(self.abc, 'rpcom_dbgOut')
-            self.connectLoggerPort(self.abc, 'rprf_dbgOut')
-            self.connectLoggerPort(self.abc, 'rplf_dbgOut')
-            self.connectLoggerPort(self.abc, 'rprh_dbgOut')
-            self.connectLoggerPort(self.abc, 'rplh_dbgOut')
-            self.connectLoggerPort(self.abc, 'rphead_dbgOut')
-            self.connectLoggerPort(self.abc, 'rpzmp_dbgOut')
-            self.connectLoggerPort(self.abc, 'rpdcp_dbgOut')
-            self.connectLoggerPort(self.abc, 'rpacp_dbgOut')
         if self.st != None:
             self.connectLoggerPort(self.st, 'zmp')
             self.connectLoggerPort(self.st, 'originRefZmp')
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index ae0b33bafdf..27121185888 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -44,7 +44,7 @@ extern "C" {
 #define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
 
 //実機だとログ出力で落ちる？getenv("HOME")ダメ？
-#define DEBUG 1
+#define DEBUG 0
 
 namespace myconst{
   const int X = 0, Y = 1, Z = 2, XYZ = 3;
@@ -286,7 +286,7 @@ class HumanSynchronizer{
       for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0,HZ);//四肢拘束点用(Rotation)
       tgt_pos_filters[0].setParameter(1.0,HZ);//重心pos用
       tgt_rot_filters[0].setParameter(0.6,HZ);//重心rot用
-      tgt_pos_filters[1].setParameter(hrp::Vector3(10.0,10.0,1.2),HZ);//右足pos用
+      tgt_pos_filters[1].setParameter(hrp::Vector3(1.0,1.0,1.2),HZ);//右足pos用
       tgt_pos_filters[2].setParameter(hrp::Vector3(1.0,1.0,1.2),HZ);//左足pos用
       calcacc_v_filters.setParameter(5,HZ);//加速度計算用
       acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい

From 3a1be44dea2846d911295af31f3beb2b84b235e4 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 3 May 2017 21:12:36 +0900
Subject: [PATCH 070/217] add interpolator

---
 .../WholeBodyMasterSlave.cpp                  | 56 ++++++++------
 .../WholeBodyMasterSlave.h                    |  6 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 74 +++++++++++++++----
 3 files changed, 99 insertions(+), 37 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index f67b257656f..688f41c760a 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -169,7 +169,18 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
     m_htrfw.data.length(6);
     m_htlfw.data.length(6);
     loop = 0;
-//
+
+
+    q_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    q_interpolator->setName(std::string(m_profile.instance_name)+" q_interpolator");
+    q_interpolator_ratio = 0;
+    double tmp_ratio = 0.0;
+    q_interpolator->clear();
+    q_interpolator->set(&tmp_ratio);
+    tmp_ratio = 1.0;
+    q_interpolator->setGoal(&tmp_ratio, 3.0, true);
+
+
 //    // setting from conf file
 //    // GaitGenerator requires abc_leg_offset and abc_stride_parameter in robot conf file
 //    // setting leg_pos from conf file
@@ -236,7 +247,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
     }
 
     hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer());
-//    if(leg_pos.size()>=2){ hsp->init_wld_rp_rfeepos = leg_pos[0]; hsp->init_wld_rp_lfeepos = leg_pos[1]; }
 
     return RTC::RTC_OK;
 }
@@ -279,9 +289,9 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
     }
     if (m_zmpIn.isNew()) {
       m_zmpIn.read();
-//      input_zmp(0) = m_zmp.data.x;
-//      input_zmp(1) = m_zmp.data.y;
-//      input_zmp(2) = m_zmp.data.z;
+      input_ref_zmp(0) = m_zmp.data.x;
+      input_ref_zmp(1) = m_zmp.data.y;
+      input_ref_zmp(2) = m_zmp.data.z;
     }
     if (m_optionalDataIn.isNew()) {
         m_optionalDataIn.read();
@@ -298,18 +308,19 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
     if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_hthead.data,hsp->head_cam_pose);}
     if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
 
-//    if(ikp.count("rarm"))ikp["rarm"].is_active = false;
-//    if(ikp.count("larm"))ikp["larm"].is_active = false;
-//    m_contactStates.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
-//    m_contactStates.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
-//
-//
-//    // Calculation
-//    Guard guard(m_mutex);
-//    hrp::Vector3 ref_basePos;
-//    hrp::Matrix33 ref_baseRot;
-//    hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
+
     if ( is_legged_robot ) {
+
+
+      bool is_q_interpolator_empty = q_interpolator->isEmpty();
+      if (!is_q_interpolator_empty && hsp->isHumanSyncOn()) {
+        q_interpolator->get(&q_interpolator_ratio, true);
+      }
+//        else {
+//        q_interpolator_ratio = (control_mode == MODE_IDLE) ? 0.0 : 1.0;
+//      }
+
+
         hsp->baselinkpose.p = m_robot->rootLink()->p;
         hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
         if(!hsp->isHumanSyncOn()){
@@ -318,9 +329,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
           for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
             m_robot->joint(i)->q = m_qRef.data[i];
           }
-
           m_robot->calcForwardKinematics();
-//          solveFullbodyIK();
         }
 
         processWholeBodyMasterSlave();
@@ -354,13 +363,17 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
 //        for(int i=0;i<3;i++) ref_zmp_invdyn2(i) = invdyn_zmp_filters2[i].passFilter(ref_zmp_invdyn2(i));
         updateInvDynStateBuffer(idsb2);
 
+//        ref_zmp = ref_zmp_invdyn;
 //        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
     }
     // Write Outport
     if ( m_qRef.data.length() != 0 ) { // initialized
       if (is_legged_robot) {
         for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
-          m_qRef.data[i] = m_robot->joint(i)->q;
+//          m_qRef.data[i] = m_robot->joint(i)->q;
+          m_qRef.data[i] = q_interpolator_ratio * m_robot->joint(i)->q  + (1 - q_interpolator_ratio) * m_qRef.data[i];
+
+
         }
       }
       m_qOut.write();
@@ -456,7 +469,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
 }
 
 void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
-//  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
   fik->current_tm = m_qRef.tm;
   fik->ikp["rleg"].is_ik_enable = true;
   fik->ikp["lleg"].is_ik_enable = true;
@@ -503,8 +515,6 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
   if(hsp->isHumanSyncOn()){
     solveFullbodyIKStrictCOM( hsp->rp_ref_out.getP("com"), hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
     //outport用のデータ上書き
-//    ref_zmp = hsp->rp_ref_out.getP("zmp").p;
-//    ref_cog = hsp->rp_ref_out.getP("com").p;
     rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.getP("zmp").p - m_robot->rootLink()->p);
     if(m_optionalData.data.length() < 4*2){
       m_optionalData.data.length(4*2);//これいいのか？
@@ -513,7 +523,7 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
     m_optionalData.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
     m_optionalData.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
   }else{
-    rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.getP("zmp").p - m_robot->rootLink()->p);
+    rel_ref_zmp = input_ref_zmp;
   }
 }
 //
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index df5814e97bf..f096a9dc2c2 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -160,9 +160,13 @@ class WholeBodyMasterSlave
   std::map<std::string, size_t> contact_states_index_map;
   double m_dt;
   hrp::BodyPtr m_robot;
-  hrp::Vector3 input_zmp, input_basePos;
+  hrp::Vector3 input_ref_zmp, input_basePos;
   hrp::Matrix33 input_baseRot;
 
+  double q_interpolator_ratio;
+  interpolator *q_interpolator;
+
+  hrp::Vector3 ref_zmp;
   hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
 
   unsigned int m_debugLevel;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 27121185888..62ca98eb414 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -189,6 +189,7 @@ class HumanSynchronizer{
     std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rot_filters;
     BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters;
     BiquadIIRFilterVec cam_rpy_filter;
+    BiquadIIRFilterVec com_in_filter;
     hrp::Vector3 r_zmp_raw;
     hrp::Vector4 rf_safe_region,lf_safe_region;//前後左右の順
     RobotConfig rc;
@@ -218,6 +219,7 @@ class HumanSynchronizer{
     double countdown_sec;
     hrp::Vector3 cp_dec,cp_acc;
     Wrench6 invdyn_ft;
+    hrp::Vector2 com_forcp_ref,com_vel_forcp_ref;
 
     interpolator *init_zmp_com_offset_interpolator;
     double zmp_com_offset_ip_ratio;
@@ -289,8 +291,9 @@ class HumanSynchronizer{
       tgt_pos_filters[1].setParameter(hrp::Vector3(1.0,1.0,1.2),HZ);//右足pos用
       tgt_pos_filters[2].setParameter(hrp::Vector3(1.0,1.0,1.2),HZ);//左足pos用
       calcacc_v_filters.setParameter(5,HZ);//加速度計算用
-      acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
+      acc4zmp_v_filters.setParameter(2,HZ);//ZMP生成用ほぼこの値でいい
       cam_rpy_filter.setParameter(1,HZ);//カメラアングル
+      com_in_filter.setParameter(1,HZ);
 
       rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
       lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
@@ -365,6 +368,9 @@ class HumanSynchronizer{
 //      applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);//怪しいので要修正
 //      calcWorldZMP                        ((hp_rel_raw.getP("rf").p+init_wld_hp_rfpos), (hp_rel_raw.getP("lf").p+init_wld_hp_lfpos), hp_rel_raw.getw("rfw"), hp_rel_raw.getw("lfw"), hp_rel_raw.getP("zmp").p);//足の位置はworldにしないと・・・
       autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);
+
+//      hp_swap_checked.getP("com").p = com_in_filter.passFilter(hp_swap_checked.getP("com").p);
+
       convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
       hp_plot = rp_ref_out;
       if(loop==0)rp_ref_out_old = rp_ref_out;
@@ -394,7 +400,12 @@ class HumanSynchronizer{
       applyVelLimit                       (rp_ref_out, rp_old_ref_Vel, rp_ref_out);
 
 
-      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, com_CP_ref_old);//これやらないと1ステップ前のCOM位置はもうはみ出てるかもしれないから
+      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
+
+      Vector3ToVector2(rp_ref_out.getP("com").p,com_forcp_ref);
+      static hrp::Vector2 com_forcp_ref_old;
+      com_vel_forcp_ref = (com_forcp_ref - com_forcp_ref_old)/DT;
+      com_forcp_ref_old = com_forcp_ref;
 
       applyCOMStateLimitByCapturePoint    (rp_ref_out.getP("com").p, rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, com_CP_ref_old, rp_ref_out.getP("com").p);
       applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com").p);
@@ -504,7 +515,7 @@ class HumanSynchronizer{
         if      (lfloc_ans  && lfw_in.f(Z)<CNT_F_TH   ){lfloc_ans = false;}//左足ついた状態から上げる
         else if (!lfloc_ans && lfw_in.f(Z)>CNT_F_TH+30){lfloc_ans = true;}//左足浮いた状態から下げる
       }
-//      if(!rfloc_ans && !lfloc_ans){rfloc_ans = lfloc_ans = true;}//両足ジャンプは禁止
+      //      if(!rfloc_ans && !lfloc_ans){rfloc_ans = lfloc_ans = true;}//両足ジャンプは禁止
     }
     void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
       hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);
@@ -644,10 +655,9 @@ class HumanSynchronizer{
       if (loop==0)com_ans_old = com_in;
       hrp::Vector3 com_vel = (com_in - com_ans_old)/DT;
       std::vector<hrp::Vector2> hull_dcp,hull_acp;
-//      hrp::Vector4 marginDelta_for_dcp(+0.001,-0.001,0.001,-0.001);
-//      hrp::Vector4 marginDelta_for_acp(+0.002,-0.002,0.002,-0.002);
       hrp::Vector4 marginDelta_for_dcp(+0.001,-0.001,0.001,-0.001);
-      hrp::Vector4 marginDelta_for_acp(+0.005,-0.005,0.005,-0.005);
+//      hrp::Vector4 marginDelta_for_acp(+0.005,-0.005,0.005,-0.005);
+      hrp::Vector4 marginDelta_for_acp(+0.01,-0.01,0.01,-0.01);
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region+marginDelta_for_dcp, lf_safe_region+marginDelta_for_dcp, hull_dcp);
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region+marginDelta_for_acp, lf_safe_region+marginDelta_for_acp, hull_acp);
       hrp::Vector2 com_vel_ans_2d;
@@ -670,26 +680,64 @@ class HumanSynchronizer{
     void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull_d, const std::vector<hrp::Vector2>& hull_a, hrp::Vector2& com_vel_ans){
       hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
       com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
+
       //減速CP条件
       hrp::Vector2 cp_dec_tmp = com_pos + com_vel * sqrt( H_cur / G );
       hrp::Vector2 cp_dec_ragulated;
       if(!isPointInHullOpenCV(cp_dec_tmp,hull_d)){
         calcCrossPointOnHull(com_pos, cp_dec_tmp, hull_d, cp_dec_ragulated);
-//        calcNearestPointOnHull(cp, hull, cp_ragulated);
+        //calcNearestPointOnHull(cp, hull, cp_ragulated);
         com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
       }
-      //加速CP条件
-      hrp::Vector2 cp_acc_tmp = com_pos - com_vel * sqrt( H_cur / G );
-      hrp::Vector2 cp_acc_ragulated;
-      if(!isPointInHullOpenCV(cp_acc_tmp,hull_a)){
-         calcCrossPointOnHull(com_pos, cp_acc_tmp, hull_a, cp_acc_ragulated);
-         com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
+
+      //減速CP条件2
+//      hrp::Vector2 cp_dec_ref = com_forcp_ref + com_vel * sqrt( H_cur / G );
+      hrp::Vector2 cp_dec_ref = com_forcp_ref + com_vel_forcp_ref * sqrt( H_cur / G );
+      hrp::Vector2 cp_dec_ref_ragulated = cp_dec_ref;
+      if(!isPointInHullOpenCV(cp_dec_ref,hull_d)){
+        calcCrossPointOnHull(com_pos, cp_dec_ref, hull_d, cp_dec_ref_ragulated);
+//        calcNearestPointOnHull(cp, hull, cp_ragulated);
       }
+      com_vel_decel_ok = (cp_dec_ref_ragulated - com_pos) / sqrt( H_cur / G );
+
+//      //加速CP条件
+//      hrp::Vector2 cp_acc_tmp = com_pos - com_vel * sqrt( H_cur / G );
+//      hrp::Vector2 cp_acc_ragulated;
+//      if(!isPointInHullOpenCV(cp_acc_tmp,hull_a)){
+//         calcCrossPointOnHull(com_pos, cp_acc_tmp, hull_a, cp_acc_ragulated);
+//         com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
+//      }
+
+//      //加速CP条件2
+//      hrp::Vector2 com_acc = (com_vel - hrp::Vector2(com_vel_old(X),com_vel_old(Y))) / DT;
+//      hrp::Vector2 zmp_in = com_pos - com_acc / G * H_cur;
+//      hrp::Vector2 zmp_regulated;
+//      if(!isPointInHullOpenCV(zmp_in,hull_a)){
+//        calcCrossPointOnHull(com_pos, zmp_in, hull_a, zmp_regulated);
+//        hrp::Vector2 com_acc_regulated = (com_pos - zmp_regulated)*G/H_cur;
+//        com_vel_accel_ok = hrp::Vector2(com_vel_old(X),com_vel_old(Y)) + com_acc_regulated*DT;
+//      }
+
+      //加速CP条件2
+      static hrp::Vector2 com_vel_ans_old = com_vel;
+      hrp::Vector2 com_acc = (com_vel - com_vel_ans_old) / DT;
+      hrp::Vector2 zmp_in = com_pos - com_acc / G * H_cur;
+      hrp::Vector2 zmp_regulated;
+      if(!isPointInHullOpenCV(zmp_in,hull_a)){
+        calcCrossPointOnHull(com_pos, zmp_in, hull_a, zmp_regulated);
+        hrp::Vector2 com_acc_regulated = (com_pos - zmp_regulated)*G/H_cur;
+        com_vel_accel_ok = com_vel_ans_old + com_acc_regulated*DT;
+      }
+
+      //加速減速条件マージ
       if(com_vel_decel_ok.norm() < com_vel_accel_ok.norm()){
         com_vel_ans = com_vel_decel_ok;
       }else{
         com_vel_ans = com_vel_accel_ok;
       }
+
+      com_vel_ans_old = com_vel_ans;
+
     }
     void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
       std::vector<hrp::Vector2> points;

From 4f6623231cc4369b2e6b6f7b0447e8c9a70cdabb Mon Sep 17 00:00:00 2001
From: JAXON <jaxon@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 4 May 2017 21:50:33 +0900
Subject: [PATCH 071/217] debugging wbms-st

---
 .../WholeBodyMasterSlave.cpp                  | 250 ++++++++++--------
 .../WholeBodyMasterSlave.h                    |  76 +++---
 rtc/WholeBodyMasterSlave/wbms_core.h          |   4 +-
 3 files changed, 187 insertions(+), 143 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 688f41c760a..f059ea47973 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -63,25 +63,25 @@ WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager)
       m_actzmpIn("actzmpIn", m_actzmp),
       m_htheadIn("htheadIn", m_hthead),
       // to ros bridge
-      m_htcom_dbgOut("htcom_dbgOut", m_htcom_dbg),
-      m_htrf_dbgOut("htrf_dbgOut", m_htrf_dbg),
-      m_htlf_dbgOut("htlf_dbgOut", m_htlf_dbg),
-      m_htrh_dbgOut("htrh_dbgOut", m_htrh_dbg),
-      m_htlh_dbgOut("htlh_dbgOut", m_htlh_dbg),
-      m_hthead_dbgOut("hthead_dbgOut", m_hthead_dbg),
-      m_htzmp_dbgOut("htzmp_dbgOut", m_htzmp_dbg),
-      m_htrfw_dbgOut("htrfw_dbgOut", m_htrfw_dbg),
-      m_htlfw_dbgOut("htlfw_dbgOut", m_htlfw_dbg),
-      m_rpcom_dbgOut("rpcom_dbgOut", m_rpcom_dbg),
-      m_rprf_dbgOut("rprf_dbgOut", m_rprf_dbg),
-      m_rplf_dbgOut("rplf_dbgOut", m_rplf_dbg),
-      m_rprh_dbgOut("rprh_dbgOut", m_rprh_dbg),
-      m_rplh_dbgOut("rplh_dbgOut", m_rplh_dbg),
-      m_rphead_dbgOut("rphead_dbgOut", m_rphead_dbg),
-      m_rpzmp_dbgOut("rpzmp_dbgOut", m_rpzmp_dbg),
-      m_rpdcp_dbgOut("rpdcp_dbgOut", m_rpdcp_dbg),
-      m_rpacp_dbgOut("rpacp_dbgOut", m_rpacp_dbg),
-      m_invdyn_dbgOut("invdyn_dbgOut", m_invdyn_dbg),
+      // m_htcom_dbgOut("htcom_dbgOut", m_htcom_dbg),
+      // m_htrf_dbgOut("htrf_dbgOut", m_htrf_dbg),
+      // m_htlf_dbgOut("htlf_dbgOut", m_htlf_dbg),
+      // m_htrh_dbgOut("htrh_dbgOut", m_htrh_dbg),
+      // m_htlh_dbgOut("htlh_dbgOut", m_htlh_dbg),
+      // m_hthead_dbgOut("hthead_dbgOut", m_hthead_dbg),
+      // m_htzmp_dbgOut("htzmp_dbgOut", m_htzmp_dbg),
+      // m_htrfw_dbgOut("htrfw_dbgOut", m_htrfw_dbg),
+      // m_htlfw_dbgOut("htlfw_dbgOut", m_htlfw_dbg),
+      // m_rpcom_dbgOut("rpcom_dbgOut", m_rpcom_dbg),
+      // m_rprf_dbgOut("rprf_dbgOut", m_rprf_dbg),
+      // m_rplf_dbgOut("rplf_dbgOut", m_rplf_dbg),
+      // m_rprh_dbgOut("rprh_dbgOut", m_rprh_dbg),
+      // m_rplh_dbgOut("rplh_dbgOut", m_rplh_dbg),
+      // m_rphead_dbgOut("rphead_dbgOut", m_rphead_dbg),
+      // m_rpzmp_dbgOut("rpzmp_dbgOut", m_rpzmp_dbg),
+      // m_rpdcp_dbgOut("rpdcp_dbgOut", m_rpdcp_dbg),
+      // m_rpacp_dbgOut("rpacp_dbgOut", m_rpacp_dbg),
+      // m_invdyn_dbgOut("invdyn_dbgOut", m_invdyn_dbg),
       m_WholeBodyMasterSlaveServicePort("WholeBodyMasterSlaveService"),
       m_robot(hrp::BodyPtr()),
       m_debugLevel(0)
@@ -121,25 +121,25 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
     addInPort("actzmpIn", m_actzmpIn);
     addInPort("htheadIn", m_htheadIn);
 
-    addOutPort("htcom_dbgOut", m_htcom_dbgOut);
-    addOutPort("htrf_dbgOut", m_htrf_dbgOut);
-    addOutPort("htlf_dbgOut", m_htlf_dbgOut);
-    addOutPort("htrh_dbgOut", m_htrh_dbgOut);
-    addOutPort("htlh_dbgOut", m_htlh_dbgOut);
-    addOutPort("hthead_dbgOut", m_hthead_dbgOut);
-    addOutPort("htzmp_dbgOut", m_htzmp_dbgOut);
-    addOutPort("htrfw_dbgOut", m_htrfw_dbgOut);
-    addOutPort("htlfw_dbgOut", m_htlfw_dbgOut);
-    addOutPort("rpcom_dbgOut", m_rpcom_dbgOut);
-    addOutPort("rprf_dbgOut", m_rprf_dbgOut);
-    addOutPort("rplf_dbgOut", m_rplf_dbgOut);
-    addOutPort("rprh_dbgOut", m_rprh_dbgOut);
-    addOutPort("rplh_dbgOut", m_rplh_dbgOut);
-    addOutPort("rphead_dbgOut", m_rphead_dbgOut);
-    addOutPort("rpzmp_dbgOut", m_rpzmp_dbgOut);
-    addOutPort("rpdcp_dbgOut", m_rpdcp_dbgOut);
-    addOutPort("rpacp_dbgOut", m_rpacp_dbgOut);
-    addOutPort("invdyn_dbgOut", m_invdyn_dbgOut);
+    // addOutPort("htcom_dbgOut", m_htcom_dbgOut);
+    // addOutPort("htrf_dbgOut", m_htrf_dbgOut);
+    // addOutPort("htlf_dbgOut", m_htlf_dbgOut);
+    // addOutPort("htrh_dbgOut", m_htrh_dbgOut);
+    // addOutPort("htlh_dbgOut", m_htlh_dbgOut);
+    // addOutPort("hthead_dbgOut", m_hthead_dbgOut);
+    // addOutPort("htzmp_dbgOut", m_htzmp_dbgOut);
+    // addOutPort("htrfw_dbgOut", m_htrfw_dbgOut);
+    // addOutPort("htlfw_dbgOut", m_htlfw_dbgOut);
+    // addOutPort("rpcom_dbgOut", m_rpcom_dbgOut);
+    // addOutPort("rprf_dbgOut", m_rprf_dbgOut);
+    // addOutPort("rplf_dbgOut", m_rplf_dbgOut);
+    // addOutPort("rprh_dbgOut", m_rprh_dbgOut);
+    // addOutPort("rplh_dbgOut", m_rplh_dbgOut);
+    // addOutPort("rphead_dbgOut", m_rphead_dbgOut);
+    // addOutPort("rpzmp_dbgOut", m_rpzmp_dbgOut);
+    // addOutPort("rpdcp_dbgOut", m_rpdcp_dbgOut);
+    // addOutPort("rpacp_dbgOut", m_rpacp_dbgOut);
+    // addOutPort("invdyn_dbgOut", m_invdyn_dbgOut);
 
     m_WholeBodyMasterSlaveServicePort.registerProvider("service0", "WholeBodyMasterSlaveService", m_service0);
   
@@ -269,11 +269,14 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onDeactivated(RTC::UniqueId ec_id)
 }
 
 //#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
-//#define DEBUGP2 ((loop%200==0))
+#define DEBUGP ((loop%200==0))
 RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
 {
   // std::cerr << "WholeBodyMasterSlave::onExecute(" << ec_id << ")" << std::endl;
     // Read Inport
+ 
+
+
     if (m_qRefIn.isNew()) {
         m_qRefIn.read();
     }
@@ -332,8 +335,12 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
           m_robot->calcForwardKinematics();
         }
 
+
+
         processWholeBodyMasterSlave();
 
+
+
         /////// Inverse Dynamics /////////
         if(!idsb.is_initialized){
           idsb.setInitState(m_robot, m_dt);
@@ -366,6 +373,13 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
 //        ref_zmp = ref_zmp_invdyn;
 //        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
     }
+
+
+
+
+
+
+
     // Write Outport
     if ( m_qRef.data.length() != 0 ) { // initialized
       if (is_legged_robot) {
@@ -394,7 +408,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
       m_zmp.data.x = rel_ref_zmp(0);
       m_zmp.data.y = rel_ref_zmp(1);
       m_zmp.data.z = rel_ref_zmp(2);
-      m_zmp.tm = m_qRef.tm;
+       m_zmp.tm = m_qRef.tm;
       // write
       m_basePosOut.write();
       m_baseRpyOut.write();
@@ -403,77 +417,87 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
     }
 
     //ishiguro dbg plot
-    m_htcom.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("com"),m_htcom_dbg.data);
-    m_htcom_dbgOut.write();
-    m_htrf.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("rf"),m_htrf_dbg.data);
-    m_htrf_dbgOut.write();
-    m_htlf.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("lf"),m_htlf_dbg.data);
-    m_htlf_dbgOut.write();
-    m_htrh.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("rh"),m_htrh_dbg.data);
-    m_htrh_dbgOut.write();
-    m_htlh.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("lh"),m_htlh_dbg.data);
-    m_htlh_dbgOut.write();
-    m_hthead.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("head"),m_hthead_dbg.data);
-    m_hthead_dbgOut.write();
-    m_htzmp.tm = m_qRef.tm;
-    HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.getP("zmp").p,m_rpzmp_dbg.data);
-    m_htzmp_dbgOut.write();
-    m_htrfw.tm = m_qRef.tm;
-    m_htrfw_dbg.data.length(6);
-    HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.getw("rfw"),m_htrfw_dbg.data);
-    m_htrfw_dbgOut.write();
-    m_htlfw.tm = m_qRef.tm;
-    m_htlfw_dbg.data.length(6);
-    HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.getw("lfw"),m_htlfw_dbg.data);
-    m_htlfw_dbgOut.write();
-    m_rpcom_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("com"),m_rpcom_dbg.data);
-    m_rpcom_dbgOut.write();
-    m_rprf_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("rf"),m_rprf_dbg.data);
-    m_rprf_dbgOut.write();
-    m_rplf_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("lf"),m_rplf_dbg.data);
-    m_rplf_dbgOut.write();
-    m_rprh_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("rh"),m_rprh_dbg.data);
-    m_rprh_dbgOut.write();
-    m_rplh_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("lh"),m_rplh_dbg.data);
-    m_rplh_dbgOut.write();
-    m_rphead_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("head"),m_rphead_dbg.data);
-    m_rphead_dbgOut.write();
-    m_rpzmp_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.getP("zmp").p,m_rpzmp_dbg.data);
-    m_rpzmp_dbgOut.write();
-    m_rpdcp_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::Vector3ToPoint3D(hsp->cp_dec,m_rpdcp_dbg.data);
-    m_rpdcp_dbgOut.write();
-    m_rpacp_dbg.tm = m_qRef.tm;
-    HumanSynchronizer::Vector3ToPoint3D(hsp->cp_acc,m_rpacp_dbg.data);
-    m_rpacp_dbgOut.write();
-    m_invdyn_dbg.tm = m_qRef.tm;
-    m_invdyn_dbg.data.length(6);
-    HumanSynchronizer::Wrench6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
-    m_invdyn_dbgOut.write();
+    // m_htcom.tm = m_qRef.tm;
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("com"),m_htcom_dbg.data);
+    // m_htcom_dbgOut.write();
+    // m_htrf.tm = m_qRef.tm;
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("rf"),m_htrf_dbg.data);
+    // m_htrf_dbgOut.write();
+    // m_htlf.tm = m_qRef.tm;
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("lf"),m_htlf_dbg.data);
+    // m_htlf_dbgOut.write();
+    // m_htrh.tm = m_qRef.tm;
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("rh"),m_htrh_dbg.data);
+    // m_htrh_dbgOut.write();
+    // m_htlh.tm = m_qRef.tm;
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("lh"),m_htlh_dbg.data);
+    // m_htlh_dbgOut.write();
+    // m_hthead.tm = m_qRef.tm;
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("head"),m_hthead_dbg.data);
+    // m_hthead_dbgOut.write();
+    // m_htzmp.tm = m_qRef.tm;
+    // HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.getP("zmp").p,m_rpzmp_dbg.data);
+    // m_htzmp_dbgOut.write();
+    // m_htrfw.tm = m_qRef.tm;
+    // m_htrfw_dbg.data.length(6);
+    // HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.getw("rfw"),m_htrfw_dbg.data);
+    // m_htrfw_dbgOut.write(); 
+    // m_htlfw.tm = m_qRef.tm;
+    // m_htlfw_dbg.data.length(6);
+    // HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.getw("lfw"),m_htlfw_dbg.data);
+    // m_htlfw_dbgOut.write();
+    // m_rpcom_dbg.tm = m_qRef.tm;
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("com"),m_rpcom_dbg.data);
+    // m_rpcom_dbgOut.write();
+    // m_rprf_dbg.tm = m_qRef.tm;
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("rf"),m_rprf_dbg.data);
+    // m_rprf_dbgOut.write();
+    // m_rplf_dbg.tm = m_qRef.tm;
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("lf"),m_rplf_dbg.data);
+    // m_rplf_dbgOut.write();
+    // m_rprh_dbg.tm = m_qRef.tm;
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("rh"),m_rprh_dbg.data);
+    // m_rprh_dbgOut.write();
+    // m_rplh_dbg.tm = m_qRef.tm;
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("lh"),m_rplh_dbg.data);
+    // m_rplh_dbgOut.write();
+    // m_rphead_dbg.tm = m_qRef.tm;
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("head"),m_rphead_dbg.data);
+    // m_rphead_dbgOut.write();
+    // m_rpzmp_dbg.tm = m_qRef.tm;
+    // HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.getP("zmp").p,m_rpzmp_dbg.data);
+    // m_rpzmp_dbgOut.write();
+    // m_rpdcp_dbg.tm = m_qRef.tm;
+    // HumanSynchronizer::Vector3ToPoint3D(hsp->cp_dec,m_rpdcp_dbg.data);
+    // m_rpdcp_dbgOut.write();
+    // m_rpacp_dbg.tm = m_qRef.tm;
+    // HumanSynchronizer::Vector3ToPoint3D(hsp->cp_acc,m_rpacp_dbg.data);
+    // m_rpacp_dbgOut.write();
+    // m_invdyn_dbg.tm = m_qRef.tm;
+    // m_invdyn_dbg.data.length(6);
+    // HumanSynchronizer::Wrench6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
+    // m_invdyn_dbgOut.write();
+
+
+
 
     loop ++;
     return RTC::RTC_OK;
 }
 
 void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
+
+// struct timeval t_calc_start, t_calc_end;
+// gettimeofday(&t_calc_start, NULL);
+
+
   fik->current_tm = m_qRef.tm;
   fik->ikp["rleg"].is_ik_enable = true;
   fik->ikp["lleg"].is_ik_enable = true;
-  fik->ikp["rarm"].is_ik_enable = true;
-  fik->ikp["larm"].is_ik_enable = true;
+  // fik->ikp["rarm"].is_ik_enable = true;
+  // fik->ikp["larm"].is_ik_enable = true;
+  fik->ikp["rarm"].is_ik_enable = false;
+  fik->ikp["larm"].is_ik_enable = false;
 
   if(hsp->startCountdownForHumanSync){
     std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
@@ -506,22 +530,42 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
     hsp->baselinkpose.p_offs = m_robot->rootLink()->p;
     hsp->baselinkpose.rpy_offs = hrp::rpyFromRot(m_robot->rootLink()->R);
   }
+// gettimeofday(&t_calc_end, NULL); 
+//  if(DEBUGP)cout<<"t1:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl;
+//  t_calc_start = t_calc_end;
+
+
   hrp::Vector3 rsole_pos = fik->ikp["rleg"].target_link->p+ fik->ikp["rleg"].target_link->R * fik->ikp["rleg"].localPos;
   hrp::Vector3 lsole_pos = fik->ikp["lleg"].target_link->p+ fik->ikp["lleg"].target_link->R * fik->ikp["lleg"].localPos;
   hsp->H_cur = m_robot->calcCM()(2) - std::min((double)rsole_pos(2), (double)lsole_pos(2));
+
+
+
+
   hsp->update();//////HumanSynchronizerの主要処理
-  if(loop%100==0)hsp->rp_ref_out.print();
+if(DEBUGP)cout<<"update():"<<hsp->getUpdateTime()<<endl;
+ if(DEBUGP)hsp->rp_ref_out.print();
+
   m_robot->calcForwardKinematics();
   if(hsp->isHumanSyncOn()){
     solveFullbodyIKStrictCOM( hsp->rp_ref_out.getP("com"), hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
     //outport用のデータ上書き
     rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.getP("zmp").p - m_robot->rootLink()->p);
+    rel_ref_zmp += hrp::Vector3(0.02, 0, 0);//debug
     if(m_optionalData.data.length() < 4*2){
       m_optionalData.data.length(4*2);//これいいのか？
       for(int i=0;i<4*2;i++)m_optionalData.data[i] = 0;
     }
     m_optionalData.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
     m_optionalData.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
+
+
+    if(DEBUGP){
+        cout<<"com:"<<hsp->rp_ref_out.getP("com").p.transpose()<<endl;
+        cout<<"rootp:"<<m_robot->rootLink()->p.transpose()<<endl;
+    }
+
+
   }else{
     rel_ref_zmp = input_ref_zmp;
   }
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index f096a9dc2c2..41f95276429 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -93,44 +93,44 @@ class WholeBodyMasterSlave
   TimedDoubleSeq m_htlfw;
   InPort<TimedDoubleSeq> m_htlfwIn;
 
-  TimedPose3D m_htcom_dbg;
-  OutPort<TimedPose3D> m_htcom_dbgOut;
-  TimedPose3D m_htrf_dbg;
-  OutPort<TimedPose3D> m_htrf_dbgOut;
-  TimedPose3D m_htlf_dbg;
-  OutPort<TimedPose3D> m_htlf_dbgOut;
-  TimedPose3D m_htrh_dbg;
-  OutPort<TimedPose3D> m_htrh_dbgOut;
-  TimedPose3D m_htlh_dbg;
-  OutPort<TimedPose3D> m_htlh_dbgOut;
-  TimedPose3D m_hthead_dbg;
-  OutPort<TimedPose3D> m_hthead_dbgOut;
-  TimedPoint3D m_htzmp_dbg;
-  OutPort<TimedPoint3D> m_htzmp_dbgOut;
-  TimedDoubleSeq m_htrfw_dbg;
-  OutPort<TimedDoubleSeq> m_htrfw_dbgOut;
-  TimedDoubleSeq m_htlfw_dbg;
-  OutPort<TimedDoubleSeq> m_htlfw_dbgOut;
-  TimedPose3D m_rpcom_dbg;
-  OutPort<TimedPose3D> m_rpcom_dbgOut;
-  TimedPose3D m_rprf_dbg;
-  OutPort<TimedPose3D> m_rprf_dbgOut;
-  TimedPose3D m_rplf_dbg;
-  OutPort<TimedPose3D> m_rplf_dbgOut;
-  TimedPose3D m_rprh_dbg;
-  OutPort<TimedPose3D> m_rprh_dbgOut;
-  TimedPose3D m_rplh_dbg;
-  OutPort<TimedPose3D> m_rplh_dbgOut;
-  TimedPose3D m_rphead_dbg;
-  OutPort<TimedPose3D> m_rphead_dbgOut;
-  TimedPoint3D m_rpzmp_dbg;
-  OutPort<TimedPoint3D> m_rpzmp_dbgOut;
-  TimedPoint3D m_rpdcp_dbg;
-  OutPort<TimedPoint3D> m_rpdcp_dbgOut;
-  TimedPoint3D m_rpacp_dbg;
-  OutPort<TimedPoint3D> m_rpacp_dbgOut;
-  TimedDoubleSeq m_invdyn_dbg;
-  OutPort<TimedDoubleSeq> m_invdyn_dbgOut;
+  // TimedPose3D m_htcom_dbg;
+  // OutPort<TimedPose3D> m_htcom_dbgOut;
+  // TimedPose3D m_htrf_dbg;
+  // OutPort<TimedPose3D> m_htrf_dbgOut;
+  // TimedPose3D m_htlf_dbg;
+  // OutPort<TimedPose3D> m_htlf_dbgOut;
+  // TimedPose3D m_htrh_dbg;
+  // OutPort<TimedPose3D> m_htrh_dbgOut;
+  // TimedPose3D m_htlh_dbg;
+  // OutPort<TimedPose3D> m_htlh_dbgOut;
+  // TimedPose3D m_hthead_dbg;
+  // OutPort<TimedPose3D> m_hthead_dbgOut;
+  // TimedPoint3D m_htzmp_dbg;
+  // OutPort<TimedPoint3D> m_htzmp_dbgOut;
+  // TimedDoubleSeq m_htrfw_dbg;
+  // OutPort<TimedDoubleSeq> m_htrfw_dbgOut;
+  // TimedDoubleSeq m_htlfw_dbg;
+  // OutPort<TimedDoubleSeq> m_htlfw_dbgOut;
+  // TimedPose3D m_rpcom_dbg;
+  // OutPort<TimedPose3D> m_rpcom_dbgOut;
+  // TimedPose3D m_rprf_dbg;
+  // OutPort<TimedPose3D> m_rprf_dbgOut;
+  // TimedPose3D m_rplf_dbg;
+  // OutPort<TimedPose3D> m_rplf_dbgOut;
+  // TimedPose3D m_rprh_dbg;
+  // OutPort<TimedPose3D> m_rprh_dbgOut;
+  // TimedPose3D m_rplh_dbg;
+  // OutPort<TimedPose3D> m_rplh_dbgOut;
+  // TimedPose3D m_rphead_dbg;
+  // OutPort<TimedPose3D> m_rphead_dbgOut;
+  // TimedPoint3D m_rpzmp_dbg;
+  // OutPort<TimedPoint3D> m_rpzmp_dbgOut;
+  // TimedPoint3D m_rpdcp_dbg;
+  // OutPort<TimedPoint3D> m_rpdcp_dbgOut;
+  // TimedPoint3D m_rpacp_dbg;
+  // OutPort<TimedPoint3D> m_rpacp_dbgOut;
+  // TimedDoubleSeq m_invdyn_dbg;
+  // OutPort<TimedDoubleSeq> m_invdyn_dbgOut;
 
   OutPort<TimedDoubleSeq> m_qOut;
   RTC::OutPort<RTC::TimedPoint3D> m_zmpOut;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 62ca98eb414..ab5a5eaebb2 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -67,7 +67,7 @@ class BiquadIIRFilterVec{
     ~BiquadIIRFilterVec() {};
     void setParameter(const hrp::Vector3 fc_in, const double HZ){
      for(int i=0;i<3;i++){
-       filters[i].setParameterAsBiquad((double)fc_in(i), 0.5, HZ);
+         filters[i].setParameterAsBiquad((double)fc_in(i), 0.5, HZ);
       }
     };
     void setParameter(const double fc_in, const double HZ){
@@ -291,7 +291,7 @@ class HumanSynchronizer{
       tgt_pos_filters[1].setParameter(hrp::Vector3(1.0,1.0,1.2),HZ);//右足pos用
       tgt_pos_filters[2].setParameter(hrp::Vector3(1.0,1.0,1.2),HZ);//左足pos用
       calcacc_v_filters.setParameter(5,HZ);//加速度計算用
-      acc4zmp_v_filters.setParameter(2,HZ);//ZMP生成用ほぼこの値でいい
+      acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
       cam_rpy_filter.setParameter(1,HZ);//カメラアングル
       com_in_filter.setParameter(1,HZ);
 

From 1fe4227f58b4a821f3f9843abcb2b75215c11091 Mon Sep 17 00:00:00 2001
From: JAXON <jaxon@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 4 May 2017 22:49:04 +0900
Subject: [PATCH 072/217] use if for wbms

---
 python/hrpsys_config.py | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index f4c07e9d1bb..50f50a5b968 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -402,11 +402,12 @@ def connectComps(self):
 #            connectPorts(self.sh.port("baseRpyOut"), self.abc.port("baseRpyIn"))
 #            connectPorts(self.sh.port("optionalDataOut"), self.abc.port("optionalData"))
             #### wbms
-            connectPorts(self.wbms.port("q"), self.abc.port("qRef"))
-            connectPorts(self.wbms.port("zmpOut"), self.abc.port("zmpIn"))
-            connectPorts(self.wbms.port("basePosOut"), self.abc.port("basePosIn"))
-            connectPorts(self.wbms.port("baseRpyOut"), self.abc.port("baseRpyIn"))
-            connectPorts(self.wbms.port("optionalDataOut"), self.abc.port("optionalData"))
+            if self.wbms:
+                connectPorts(self.wbms.port("q"), self.abc.port("qRef"))
+                connectPorts(self.wbms.port("zmpOut"), self.abc.port("zmpIn"))
+                connectPorts(self.wbms.port("basePosOut"), self.abc.port("basePosIn"))
+                connectPorts(self.wbms.port("baseRpyOut"), self.abc.port("baseRpyIn"))
+                connectPorts(self.wbms.port("optionalDataOut"), self.abc.port("optionalData"))
             
             connectPorts(self.abc.port("zmpOut"), self.st.port("zmpRef"))
             connectPorts(self.abc.port("baseRpyOut"), self.st.port("baseRpyIn"))

From 38792a9e1f49b8298510ecacbc7ba077d132618d Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 5 May 2017 16:39:35 +0900
Subject: [PATCH 073/217] set initial COM pos to the mid of RF LF

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index f059ea47973..8e88c400139 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -514,8 +514,6 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
     hsp->setCurrentInputAsOffset(hsp->hp_wld_raw);
     hsp->calibInitHumanCOMFromZMP();
 
-    hsp->rp_ref_out.getP("com").p_offs = m_robot->calcCM();
-//    hsp->rp_ref_out.getP("zmp").p_offs = ref_zmp;
     const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"}, human_l_names[4] = {"rf","lf","rh","lh"};
     for(int i=0;i<4;i++){
       if(fik->ikp.count(robot_l_names[i])){
@@ -525,11 +523,18 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
         hsp->rp_ref_out.getP(human_l_names[i]).rpy = hsp->rp_ref_out.getP(human_l_names[i]).rpy_offs;
       }
     }
+    //    hsp->rp_ref_out.getP("com").p_offs = m_robot->calcCM();
+    hsp->rp_ref_out.getP("com").p_offs(0) = (hsp->rp_ref_out.getP("rf").p_offs(0) + hsp->rp_ref_out.getP("lf").p_offs(0)) / 2;
+    hsp->rp_ref_out.getP("com").p_offs(1) = (hsp->rp_ref_out.getP("rf").p_offs(1) + hsp->rp_ref_out.getP("lf").p_offs(1)) / 2;
+    hsp->rp_ref_out.getP("com").p_offs(2) = m_robot->calcCM()(2);
+    //    hsp->rp_ref_out.getP("zmp").p_offs = ref_zmp;
     hsp->pre_cont_rfpos = hsp->rp_ref_out.getP("rf").p_offs;
     hsp->pre_cont_lfpos = hsp->rp_ref_out.getP("lf").p_offs;
     hsp->baselinkpose.p_offs = m_robot->rootLink()->p;
     hsp->baselinkpose.rpy_offs = hrp::rpyFromRot(m_robot->rootLink()->R);
   }
+
+
 // gettimeofday(&t_calc_end, NULL); 
 //  if(DEBUGP)cout<<"t1:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl;
 //  t_calc_start = t_calc_end;
@@ -551,7 +556,6 @@ if(DEBUGP)cout<<"update():"<<hsp->getUpdateTime()<<endl;
     solveFullbodyIKStrictCOM( hsp->rp_ref_out.getP("com"), hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
     //outport用のデータ上書き
     rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.getP("zmp").p - m_robot->rootLink()->p);
-    rel_ref_zmp += hrp::Vector3(0.02, 0, 0);//debug
     if(m_optionalData.data.length() < 4*2){
       m_optionalData.data.length(4*2);//これいいのか？
       for(int i=0;i<4*2;i++)m_optionalData.data[i] = 0;

From a6d6e22921c9f5da28e8e9d8d7f513beff62d98c Mon Sep 17 00:00:00 2001
From: JAXON <jaxon@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 5 May 2017 20:16:47 +0900
Subject: [PATCH 074/217] solve wbms-abc-st bug

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 8e88c400139..bbc566479d6 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -406,7 +406,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
       m_baseRpy.tm = m_qRef.tm;
       // zmp
       m_zmp.data.x = rel_ref_zmp(0);
-      m_zmp.data.y = rel_ref_zmp(1);
+      m_zmp.data.y = rel_ref_zmp(1);//debug
       m_zmp.data.z = rel_ref_zmp(2);
        m_zmp.tm = m_qRef.tm;
       // write
@@ -553,19 +553,31 @@ if(DEBUGP)cout<<"update():"<<hsp->getUpdateTime()<<endl;
 
   m_robot->calcForwardKinematics();
   if(hsp->isHumanSyncOn()){
+
+
+      hsp->rp_ref_out.getP("com").p(0) = 0;
+      hsp->rp_ref_out.getP("rf").p(0) = 0;
+      hsp->rp_ref_out.getP("lf").p(0) = 0;
+      hsp->rp_ref_out.getP("zmp").p(0) = 0;
+
+
     solveFullbodyIKStrictCOM( hsp->rp_ref_out.getP("com"), hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
     //outport用のデータ上書き
     rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.getP("zmp").p - m_robot->rootLink()->p);
+    rel_ref_zmp(2) = -0.97;//debug
     if(m_optionalData.data.length() < 4*2){
       m_optionalData.data.length(4*2);//これいいのか？
       for(int i=0;i<4*2;i++)m_optionalData.data[i] = 0;
     }
     m_optionalData.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
     m_optionalData.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
+    m_optionalData.data[contact_states_index_map["rleg"]+4] = 1;//これいる？
+    m_optionalData.data[contact_states_index_map["lleg"]+4] = 1;//
 
 
     if(DEBUGP){
         cout<<"com:"<<hsp->rp_ref_out.getP("com").p.transpose()<<endl;
+        cout<<"zmp:"<<hsp->rp_ref_out.getP("zmp").p.transpose()<<endl;
         cout<<"rootp:"<<m_robot->rootLink()->p.transpose()<<endl;
     }
 

From f732bced710018f87d279c0c1f27ebc45e309cb8 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 5 May 2017 20:29:12 +0900
Subject: [PATCH 075/217] clean up debug code

---
 .../WholeBodyMasterSlave.cpp                  | 31 ++-----------------
 1 file changed, 3 insertions(+), 28 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index bbc566479d6..85a1d3e9b38 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -490,7 +490,6 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
 // struct timeval t_calc_start, t_calc_end;
 // gettimeofday(&t_calc_start, NULL);
 
-
   fik->current_tm = m_qRef.tm;
   fik->ikp["rleg"].is_ik_enable = true;
   fik->ikp["lleg"].is_ik_enable = true;
@@ -534,54 +533,30 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
     hsp->baselinkpose.rpy_offs = hrp::rpyFromRot(m_robot->rootLink()->R);
   }
 
-
 // gettimeofday(&t_calc_end, NULL); 
 //  if(DEBUGP)cout<<"t1:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl;
 //  t_calc_start = t_calc_end;
 
-
   hrp::Vector3 rsole_pos = fik->ikp["rleg"].target_link->p+ fik->ikp["rleg"].target_link->R * fik->ikp["rleg"].localPos;
   hrp::Vector3 lsole_pos = fik->ikp["lleg"].target_link->p+ fik->ikp["lleg"].target_link->R * fik->ikp["lleg"].localPos;
   hsp->H_cur = m_robot->calcCM()(2) - std::min((double)rsole_pos(2), (double)lsole_pos(2));
 
-
-
-
   hsp->update();//////HumanSynchronizerの主要処理
-if(DEBUGP)cout<<"update():"<<hsp->getUpdateTime()<<endl;
- if(DEBUGP)hsp->rp_ref_out.print();
+  if(DEBUGP)cout<<"update():"<<hsp->getUpdateTime()<<endl;
+  if(DEBUGP)hsp->rp_ref_out.print();
 
   m_robot->calcForwardKinematics();
   if(hsp->isHumanSyncOn()){
-
-
-      hsp->rp_ref_out.getP("com").p(0) = 0;
-      hsp->rp_ref_out.getP("rf").p(0) = 0;
-      hsp->rp_ref_out.getP("lf").p(0) = 0;
-      hsp->rp_ref_out.getP("zmp").p(0) = 0;
-
-
     solveFullbodyIKStrictCOM( hsp->rp_ref_out.getP("com"), hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
     //outport用のデータ上書き
+    hsp->rp_ref_out.getP("zmp").p(2) = (hsp->rp_ref_out.getP("rf").p(2) + hsp->rp_ref_out.getP("lf").p(2))/2 - m_robot->rootLink()->p(2);//体幹相対ZMP高さ設定
     rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.getP("zmp").p - m_robot->rootLink()->p);
-    rel_ref_zmp(2) = -0.97;//debug
     if(m_optionalData.data.length() < 4*2){
       m_optionalData.data.length(4*2);//これいいのか？
       for(int i=0;i<4*2;i++)m_optionalData.data[i] = 0;
     }
     m_optionalData.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
     m_optionalData.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
-    m_optionalData.data[contact_states_index_map["rleg"]+4] = 1;//これいる？
-    m_optionalData.data[contact_states_index_map["lleg"]+4] = 1;//
-
-
-    if(DEBUGP){
-        cout<<"com:"<<hsp->rp_ref_out.getP("com").p.transpose()<<endl;
-        cout<<"zmp:"<<hsp->rp_ref_out.getP("zmp").p.transpose()<<endl;
-        cout<<"rootp:"<<m_robot->rootLink()->p.transpose()<<endl;
-    }
-
-
   }else{
     rel_ref_zmp = input_ref_zmp;
   }

From a9edac3135eb13930d04748afa93da2d18209463 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 5 May 2017 22:26:01 +0900
Subject: [PATCH 076/217] change data types

---
 .../WholeBodyMasterSlave.cpp                  |  85 +++---
 .../WholeBodyMasterSlave.h                    |  13 +
 rtc/WholeBodyMasterSlave/wbms_core.cpp        |  24 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 270 +++++++++---------
 4 files changed, 207 insertions(+), 185 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 85a1d3e9b38..f607d8b6c9d 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -301,13 +301,13 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
     }
     //for HumanSynchronizer
     if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
-    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hsp->hp_wld_raw.getw("rfw")); }
-    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hsp->hp_wld_raw.getw("lfw")); }
-    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_htcom.data,hsp->hp_wld_raw.getP("com")); }
-    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrf.data,hsp->hp_wld_raw.getP("rf")); }
-    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlf.data,hsp->hp_wld_raw.getP("lf")); }
-    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrh.data,hsp->hp_wld_raw.getP("rh"));}
-    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlh.data,hsp->hp_wld_raw.getP("lh"));}
+    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hsp->hp_wld_raw.w[rfw]); }
+    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hsp->hp_wld_raw.w[lfw]); }
+    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_htcom.data,hsp->hp_wld_raw.P[com]); }
+    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrf.data,hsp->hp_wld_raw.P[rf]); }
+    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlf.data,hsp->hp_wld_raw.P[lf]); }
+    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrh.data,hsp->hp_wld_raw.P[rh]);}
+    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlh.data,hsp->hp_wld_raw.P[lh]);}
     if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_hthead.data,hsp->head_cam_pose);}
     if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
 
@@ -418,54 +418,54 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
 
     //ishiguro dbg plot
     // m_htcom.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("com"),m_htcom_dbg.data);
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[com],m_htcom_dbg.data);
     // m_htcom_dbgOut.write();
     // m_htrf.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("rf"),m_htrf_dbg.data);
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[rf],m_htrf_dbg.data);
     // m_htrf_dbgOut.write();
     // m_htlf.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("lf"),m_htlf_dbg.data);
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[lf],m_htlf_dbg.data);
     // m_htlf_dbgOut.write();
     // m_htrh.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("rh"),m_htrh_dbg.data);
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[rh],m_htrh_dbg.data);
     // m_htrh_dbgOut.write();
     // m_htlh.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("lh"),m_htlh_dbg.data);
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[lh],m_htlh_dbg.data);
     // m_htlh_dbgOut.write();
     // m_hthead.tm = m_qRef.tm;
     // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("head"),m_hthead_dbg.data);
     // m_hthead_dbgOut.write();
     // m_htzmp.tm = m_qRef.tm;
-    // HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.getP("zmp").p,m_rpzmp_dbg.data);
+    // HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.P[zmp].p,m_rpzmp_dbg.data);
     // m_htzmp_dbgOut.write();
     // m_htrfw.tm = m_qRef.tm;
     // m_htrfw_dbg.data.length(6);
-    // HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.getw("rfw"),m_htrfw_dbg.data);
+    // HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.w[rfw],m_htrfw_dbg.data);
     // m_htrfw_dbgOut.write(); 
     // m_htlfw.tm = m_qRef.tm;
     // m_htlfw_dbg.data.length(6);
-    // HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.getw("lfw"),m_htlfw_dbg.data);
+    // HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.w[lfw],m_htlfw_dbg.data);
     // m_htlfw_dbgOut.write();
     // m_rpcom_dbg.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("com"),m_rpcom_dbg.data);
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[com],m_rpcom_dbg.data);
     // m_rpcom_dbgOut.write();
     // m_rprf_dbg.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("rf"),m_rprf_dbg.data);
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[rf],m_rprf_dbg.data);
     // m_rprf_dbgOut.write();
     // m_rplf_dbg.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("lf"),m_rplf_dbg.data);
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[lf],m_rplf_dbg.data);
     // m_rplf_dbgOut.write();
     // m_rprh_dbg.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("rh"),m_rprh_dbg.data);
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[rh],m_rprh_dbg.data);
     // m_rprh_dbgOut.write();
     // m_rplh_dbg.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("lh"),m_rplh_dbg.data);
+    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[lh],m_rplh_dbg.data);
     // m_rplh_dbgOut.write();
     // m_rphead_dbg.tm = m_qRef.tm;
     // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("head"),m_rphead_dbg.data);
     // m_rphead_dbgOut.write();
     // m_rpzmp_dbg.tm = m_qRef.tm;
-    // HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.getP("zmp").p,m_rpzmp_dbg.data);
+    // HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.P[zmp].p,m_rpzmp_dbg.data);
     // m_rpzmp_dbgOut.write();
     // m_rpdcp_dbg.tm = m_qRef.tm;
     // HumanSynchronizer::Vector3ToPoint3D(hsp->cp_dec,m_rpdcp_dbg.data);
@@ -513,22 +513,23 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
     hsp->setCurrentInputAsOffset(hsp->hp_wld_raw);
     hsp->calibInitHumanCOMFromZMP();
 
-    const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"}, human_l_names[4] = {"rf","lf","rh","lh"};
+    const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
+    const int human_l_names[4] = {rf,lf,rh,lh};
     for(int i=0;i<4;i++){
       if(fik->ikp.count(robot_l_names[i])){
-        hsp->rp_ref_out.getP(human_l_names[i]).p_offs = fik->ikp[robot_l_names[i]].target_link->p + fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localPos;
-        hsp->rp_ref_out.getP(human_l_names[i]).rpy_offs = hrp::rpyFromRot(fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localR);
-        hsp->rp_ref_out.getP(human_l_names[i]).p = hsp->rp_ref_out.getP(human_l_names[i]).p_offs;
-        hsp->rp_ref_out.getP(human_l_names[i]).rpy = hsp->rp_ref_out.getP(human_l_names[i]).rpy_offs;
+        hsp->rp_ref_out.P[human_l_names[i]].p_offs = fik->ikp[robot_l_names[i]].target_link->p + fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localPos;
+        hsp->rp_ref_out.P[human_l_names[i]].rpy_offs = hrp::rpyFromRot(fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localR);
+        hsp->rp_ref_out.P[human_l_names[i]].p = hsp->rp_ref_out.P[human_l_names[i]].p_offs;
+        hsp->rp_ref_out.P[human_l_names[i]].rpy = hsp->rp_ref_out.P[human_l_names[i]].rpy_offs;
       }
     }
-    //    hsp->rp_ref_out.getP("com").p_offs = m_robot->calcCM();
-    hsp->rp_ref_out.getP("com").p_offs(0) = (hsp->rp_ref_out.getP("rf").p_offs(0) + hsp->rp_ref_out.getP("lf").p_offs(0)) / 2;
-    hsp->rp_ref_out.getP("com").p_offs(1) = (hsp->rp_ref_out.getP("rf").p_offs(1) + hsp->rp_ref_out.getP("lf").p_offs(1)) / 2;
-    hsp->rp_ref_out.getP("com").p_offs(2) = m_robot->calcCM()(2);
-    //    hsp->rp_ref_out.getP("zmp").p_offs = ref_zmp;
-    hsp->pre_cont_rfpos = hsp->rp_ref_out.getP("rf").p_offs;
-    hsp->pre_cont_lfpos = hsp->rp_ref_out.getP("lf").p_offs;
+    //    hsp->rp_ref_out.P[com].p_offs = m_robot->calcCM();
+    hsp->rp_ref_out.P[com].p_offs(0) = (hsp->rp_ref_out.P[rf].p_offs(0) + hsp->rp_ref_out.P[lf].p_offs(0)) / 2;
+    hsp->rp_ref_out.P[com].p_offs(1) = (hsp->rp_ref_out.P[rf].p_offs(1) + hsp->rp_ref_out.P[lf].p_offs(1)) / 2;
+    hsp->rp_ref_out.P[com].p_offs(2) = m_robot->calcCM()(2);
+    //    hsp->rp_ref_out.P[zmp].p_offs = ref_zmp;
+    hsp->pre_cont_rfpos = hsp->rp_ref_out.P[rf].p_offs;
+    hsp->pre_cont_lfpos = hsp->rp_ref_out.P[lf].p_offs;
     hsp->baselinkpose.p_offs = m_robot->rootLink()->p;
     hsp->baselinkpose.rpy_offs = hrp::rpyFromRot(m_robot->rootLink()->R);
   }
@@ -547,10 +548,10 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
 
   m_robot->calcForwardKinematics();
   if(hsp->isHumanSyncOn()){
-    solveFullbodyIKStrictCOM( hsp->rp_ref_out.getP("com"), hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
+    solveFullbodyIKStrictCOM( hsp->rp_ref_out.P[com], hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->cam_rpy_filtered );
     //outport用のデータ上書き
-    hsp->rp_ref_out.getP("zmp").p(2) = (hsp->rp_ref_out.getP("rf").p(2) + hsp->rp_ref_out.getP("lf").p(2))/2 - m_robot->rootLink()->p(2);//体幹相対ZMP高さ設定
-    rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.getP("zmp").p - m_robot->rootLink()->p);
+    hsp->rp_ref_out.P[zmp].p(2) = (hsp->rp_ref_out.P[rf].p(2) + hsp->rp_ref_out.P[lf].p(2))/2 - m_robot->rootLink()->p(2);//体幹相対ZMP高さ設定
+    rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.P[zmp].p - m_robot->rootLink()->p);
     if(m_optionalData.data.length() < 4*2){
       m_optionalData.data.length(4*2);//これいいのか？
       for(int i=0;i<4*2;i++)m_optionalData.data[i] = 0;
@@ -651,10 +652,10 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
 //////  m_robot->calcForwardKinematics();
 //////  // additional COM fitting IK for HumanSynchronizer
 ////  if(hsp->isHumanSyncOn()){
-////    HRPPose3D com_mod = hsp->rp_ref_out.getP("com");
+////    HRPPose3D com_mod = hsp->rp_ref_out.P[com];
 ////    com_mod.p += torso_pos;
 ////    com_mod.rpy += torso_rot;
-////    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
+////    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->cam_rpy_filtered );
 ////  }
 ////}
 //
@@ -684,8 +685,8 @@ void WholeBodyMasterSlave::calcDynamicsFilterCompensation(const hrp::Vector3 zmp
 
 
 
-  fprintf(hsp->id_log,"p: %f %f ",hsp->rp_ref_out.getP("com").p(0),hsp->rp_ref_out.getP("com").p(1),hsp->rp_ref_out.getP("com").p(2));
-  fprintf(hsp->id_log,"rpy: %f %f ",hsp->rp_ref_out.getP("com").rpy(0),hsp->rp_ref_out.getP("com").rpy(1),hsp->rp_ref_out.getP("com").rpy(2));
+  fprintf(hsp->id_log,"p: %f %f ",hsp->rp_ref_out.P[com].p(0),hsp->rp_ref_out.P[com].p(1),hsp->rp_ref_out.P[com].p(2));
+  fprintf(hsp->id_log,"rpy: %f %f ",hsp->rp_ref_out.P[com].rpy(0),hsp->rp_ref_out.P[com].rpy(1),hsp->rp_ref_out.P[com].rpy(2));
   fprintf(hsp->id_log,"P: %f %f ",P(0),P(1),P(2));
   fprintf(hsp->id_log,"L: %f %f ",L(0),L(1),L(2));
 //  fprintf(hsp->id_log,"p_v: %f %f ",idsb.base_v(0),idsb.base_v(1),idsb.base_v(2));
@@ -695,10 +696,10 @@ void WholeBodyMasterSlave::calcDynamicsFilterCompensation(const hrp::Vector3 zmp
   fprintf(hsp->id_log,"\n");
 
   if(hsp->isHumanSyncOn()){
-    HRPPose3D com_mod = hsp->rp_ref_out.getP("com");
+    HRPPose3D com_mod = hsp->rp_ref_out.P[com];
 //    com_mod.p += torso_pos;
 //    com_mod.rpy += torso_rot;
-    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.getP("rf"), hsp->rp_ref_out.getP("lf"), hsp->rp_ref_out.getP("rh"), hsp->rp_ref_out.getP("lh"), hsp->cam_rpy_filtered );
+    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->cam_rpy_filtered );
   }
 }
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 41f95276429..506f83dc5aa 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -47,6 +47,19 @@ using namespace RTC;
 class WholeBodyMasterSlave
   : public RTC::DataFlowComponentBase
 {
+
+
+
+
+    enum pose_tgt{ com, rf, lf, rh, lh, zmp, head, num_pose_tgt } p_id;
+    enum wrench_tgt{ rfw, lfw, num_wrench_tgt } w_id;
+    enum lr_direction{ L, R, LR} lr_direc;
+    enum xyz_direction{ X, Y, Z, XYZ} xyz_direc;
+    enum rpy_direction{ r, p, y, rpy} rpy_direc;
+    enum ft_direction{ fx, fy, fz, tx, ty, tz, ft_xyz} w_direc;
+
+
+
  public:
   WholeBodyMasterSlave(RTC::Manager* manager);
   virtual ~WholeBodyMasterSlave();
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.cpp b/rtc/WholeBodyMasterSlave/wbms_core.cpp
index bbaa1c364fe..06b86315967 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.cpp
+++ b/rtc/WholeBodyMasterSlave/wbms_core.cpp
@@ -3,17 +3,17 @@
 void HumanSynchronizer::calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const Wrench6& rfwin, const Wrench6& lfwin, hrp::Vector3& zmp_ans){
   hrp::Vector3 rfzmp,lfzmp;
   const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
-  if( rfwin.f(2) > 1.0e-6 ){
-    rfzmp(0) = ( - rfwin.t(1) - rfwin.f(0) * F_H_OFFSET + rfwin.f(2) * 0 ) / rfwin.f(2) + rfpos(0);
-    rfzmp(1) = (   rfwin.t(0) - rfwin.f(1) * F_H_OFFSET + rfwin.f(2) * 0 ) / rfwin.f(2) + rfpos(1);
+  if( rfwin.w(fz) > 1.0e-6 ){
+    rfzmp(0) = ( - rfwin.w(ty) - rfwin.w(fx) * F_H_OFFSET + rfwin.w(fz) * 0 ) / rfwin.w(fz) + rfpos(0);
+    rfzmp(1) = (   rfwin.w(tx) - rfwin.w(fy) * F_H_OFFSET + rfwin.w(fz) * 0 ) / rfwin.w(fz) + rfpos(1);
   }
-  if( lfwin.f(2) > 1.0e-6 ){
-    lfzmp(0) = ( - lfwin.t(1) - lfwin.f(0) * F_H_OFFSET + lfwin.f(2) * 0 ) / lfwin.f(2) + lfpos(0);
-    lfzmp(1) = (   lfwin.t(0) - lfwin.f(1) * F_H_OFFSET + lfwin.f(2) * 0 ) / lfwin.f(2) + lfpos(1);
+  if( lfwin.w(fz) > 1.0e-6 ){
+    lfzmp(0) = ( - lfwin.w(ty) - lfwin.w(fx) * F_H_OFFSET + lfwin.w(fz) * 0 ) / lfwin.w(fz) + lfpos(0);
+    lfzmp(1) = (   lfwin.w(tx) - lfwin.w(fy) * F_H_OFFSET + lfwin.w(fz) * 0 ) / lfwin.w(fz) + lfpos(1);
   }
-  if( rfwin.f(2) > 1.0e-6 || lfwin.f(2) > 1.0e-6 ){
-    zmp_ans(0) = ( rfzmp(0)*rfwin.f(2) + lfzmp(0)*lfwin.f(2) ) / ( rfwin.f(2) + lfwin.f(2));
-    zmp_ans(1) = ( rfzmp(1)*rfwin.f(2) + lfzmp(1)*lfwin.f(2) ) / ( rfwin.f(2) + lfwin.f(2));
+  if( rfwin.w(fz) > 1.0e-6 || lfwin.w(fz) > 1.0e-6 ){
+    zmp_ans(0) = ( rfzmp(0)*rfwin.w(fz) + lfzmp(0)*lfwin.w(fz) ) / ( rfwin.w(fz) + lfwin.w(fz));
+    zmp_ans(1) = ( rfzmp(1)*rfwin.w(fz) + lfzmp(1)*lfwin.w(fz) ) / ( rfwin.w(fz) + lfwin.w(fz));
   }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
   zmp_ans(2) = 0;
 }
@@ -114,10 +114,10 @@ void HumanSynchronizer::applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vect
 }
 void HumanSynchronizer::applyVelLimit(const HumanPose& in, HumanPose& out_old, HumanPose& out){
   std::string ns[5] = {"com","rf","lf","rh","lh"};
-  for(int i=0;i<5;i++){
-    hrp::Vector3 diff = in.getP(ns[i]).p  - out_old.getP(ns[i]).p;
+  for(int i=com;i<=lh;i++){
+    hrp::Vector3 diff = in.P[i].p  - out_old.P[i].p;
     for(int j=0;j<3;j++)LIMIT_MINMAX( diff(j), -MAXVEL*DT, MAXVEL*DT);
-    out.getP(ns[i]).p = out_old.getP(ns[i]).p + diff;
+    out.P[i].p = out_old.P[i].p + diff;
   }
   out_old = out;
 }
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index ab5a5eaebb2..8d1235fb4bb 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -47,9 +47,9 @@ extern "C" {
 #define DEBUG 0
 
 namespace myconst{
-  const int X = 0, Y = 1, Z = 2, XYZ = 3;
-  const int r = 0, p = 1, y = 2, rpy = 3;
-  const int L = 0, R = 1, LR = 2;
+//  const int X = 0, Y = 1, Z = 2, XYZ = 3;
+//  const int r = 0, p = 1, y = 2, rpy = 3;
+//  const int L = 0, R = 1, LR = 2;
   const int MIN = 0, MAX = 1, MINMAX = 2;
 //  const double INFMIN = std::numeric_limits<double>::min(), INFMAX = std::numeric_limits<double>::max();
   const double INFMIN = - std::numeric_limits<double>::max(), INFMAX = std::numeric_limits<double>::max();
@@ -57,6 +57,17 @@ namespace myconst{
 }
 using namespace myconst;
 
+
+//namespace wbms{
+//  enum pose_tgt{ com, rf, lf, rh, lh, zmp, head, num_pose_tgt } p_id;
+//  enum wrench_tgt{ rfw, lfw, num_wrench_tgt } w_id;
+//  enum lr_direction{ L, R, LR} lr_direc;
+//  enum xyz_direction{ X, Y, Z, XYZ} xyz_direc;
+//  enum rpy_direction{ r, p, y, rpy} rpy_direc;
+//  enum ft_direction{ fx, fy, fz, tx, ty, tz, ft_xyz} w_direc;
+//}
+//using namespace wbms;
+
 class BiquadIIRFilterVec{
   private:
     IIRFilter filters[3];
@@ -93,57 +104,44 @@ class HRPPose3D{
 };
 class Wrench6{
   public:
-    hrp::Vector3 f,t;
+    hrp::dvector6 w;
     Wrench6(){ clear(); }
     ~Wrench6(){}
-    void clear(){ f = t = hrp::Vector3::Zero(); }
+    void clear(){ w = hrp::dvector6::Zero(); }
 };
 
 class HumanPose{
-  private:
-    std::map<std::string, HRPPose3D> P;
-    std::map<std::string, Wrench6> w;
+
+
+
+
+    enum pose_tgt{ com, rf, lf, rh, lh, zmp, head, num_pose_tgt } p_id;
+    enum wrench_tgt{ rfw, lfw, num_wrench_tgt } w_id;
+    enum lr_direction{ L, R, LR} lr_direc;
+    enum xyz_direction{ X, Y, Z, XYZ} xyz_direc;
+    enum rpy_direction{ r, p, y, rpy} rpy_direc;
+    enum ft_direction{ fx, fy, fz, tx, ty, tz, ft_xyz} w_direc;
+
+
   public:
+    std::vector<HRPPose3D> P;
+    std::vector<Wrench6> w;
     HumanPose(){
-      P["com"] = HRPPose3D();
-      P["rf"] = HRPPose3D();
-      P["lf"] = HRPPose3D();
-      P["rh"] = HRPPose3D();
-      P["lh"] = HRPPose3D();
-      P["zmp"] = HRPPose3D();
-      P["head"] = HRPPose3D();
-      w["rfw"] = Wrench6();
-      w["lfw"] = Wrench6();
+      P.resize(num_pose_tgt);
+      w.resize(num_wrench_tgt);
       clear();
     }
     ~HumanPose(){cerr<<"HumanPose destructed"<<endl;}
     void clear(){
-      for(std::map<std::string, HRPPose3D>::iterator it = P.begin(); it != P.end(); it++){ it->second.clear(); }
-      for(std::map<std::string, Wrench6>::iterator it = w.begin(); it != w.end(); it++){ it->second.clear(); }
+      for(std::vector<HRPPose3D>::iterator it = P.begin(); it != P.end(); it++){ it->clear(); }
+      for(std::vector<Wrench6>::iterator it = w.begin(); it != w.end(); it++){ it->clear(); }
     }
     static void hp_printf(const HumanPose& in){
-      const std::string ns[] = {"com","rf","lf","rh","lh","zmp","rfw","lfw"};
       const std::string cap[] = {"c","rf","lf","rh","lh","z","rw","lw"};
-      for(int i=0;i<6;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.getP(ns[i]).p(X),in.getP(ns[i]).p(Y),in.getP(ns[i]).p(Z)); }
-      for(int i=6;i<8;i++){ printf("\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.getw(ns[i]).f(X),in.getw(ns[i]).f(Y),in.getw(ns[i]).f(Z),in.getw(ns[i]).t(X),in.getw(ns[i]).t(Y),in.getw(ns[i]).t(Z)); }
+      for(int i=com;i<=zmp;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.P[i].p(X),in.P[i].p(Y),in.P[i].p(Z)); }
+      for(int i=rfw;i<lfw;i++){ printf("\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.w[i].w(fx),in.w[i].w(fy),in.w[i].w(fz),in.w[i].w(tx),in.w[i].w(ty),in.w[i].w(tz)); }
       printf("\n");
     }
-    HRPPose3D& getP(const std::string& key){///Safe accessor
-      if (P.find(key) != P.end() ) { return P.find(key)->second; }
-      else { cerr<<"\n\x1b[31m[FATAL] HumanPose Invalid Key \""<<key<<"\" !!\x1b[39m\n"<<endl; exit(-1); return P.end()->second; }
-    }
-    const HRPPose3D& getP(const std::string& key) const {
-      if (P.find(key) != P.end() ) { return P.find(key)->second; }
-      else { cerr<<"\n\x1b[31m[FATAL] HumanPose Invalid Key \""<<key<<"\" !!\x1b[39m\n"<<endl; exit(-1); return P.end()->second; }
-    }
-    Wrench6& getw(const std::string& key){
-      if (w.find(key) != w.end() ) { return w.find(key)->second; }
-      else { cerr<<"\n\x1b[31m[FATAL] HumanPose Invalid Key \""<<key<<"\" !!\x1b[39m\n"<<endl; exit(-1); return w.end()->second; }
-    }
-    const Wrench6& getw(const std::string& key) const {
-      if (w.find(key) != w.end() ) { return w.find(key)->second; }
-      else { cerr<<"\n\x1b[31m[FATAL] HumanPose Invalid Key \""<<key<<"\" !!\x1b[39m\n"<<endl; exit(-1); return w.end()->second; }
-    }
     void print(){ hp_printf(*this); }
 };
 
@@ -170,6 +168,16 @@ class RobotConfig{
 };
 
 class HumanSynchronizer{
+
+
+    enum pose_tgt{ com, rf, lf, rh, lh, zmp, head, num_pose_tgt } p_id;
+    enum wrench_tgt{ rfw, lfw, num_wrench_tgt } w_id;
+    enum lr_direction{ L, R, LR} lr_direc;
+    enum xyz_direction{ X, Y, Z, XYZ} xyz_direc;
+    enum rpy_direction{ r, p, y, rpy} rpy_direc;
+    enum ft_direction{ fx, fy, fz, tx, ty, tz, ft_xyz} w_direc;
+
+
   private:
     double FUP_TIME;
     int cur_rfup_level,cur_lfup_level;
@@ -350,8 +358,8 @@ class HumanSynchronizer{
     void setCurrentInputAsOffset(HumanPose& tgt){
       std::string ns[7] = {"com","rf","lf","rh","lh","zmp","head"};
       for(int i=0;i<7;i++){
-        tgt.getP(ns[i]).p_offs   =  tgt.getP(ns[i]).p;
-        tgt.getP(ns[i]).rpy_offs =  tgt.getP(ns[i]).rpy;
+        tgt.P[i].p_offs   =  tgt.P[i].p;
+        tgt.P[i].rpy_offs =  tgt.P[i].rpy;
       }
     }
     void updateCountDown(){
@@ -366,68 +374,68 @@ class HumanSynchronizer{
       gettimeofday(&t_calc_start, NULL);
       updateParams                        ();
 //      applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);//怪しいので要修正
-//      calcWorldZMP                        ((hp_rel_raw.getP("rf").p+init_wld_hp_rfpos), (hp_rel_raw.getP("lf").p+init_wld_hp_lfpos), hp_rel_raw.getw("rfw"), hp_rel_raw.getw("lfw"), hp_rel_raw.getP("zmp").p);//足の位置はworldにしないと・・・
+//      calcWorldZMP                        ((hp_rel_raw.P[rf].p+init_wld_hp_rfpos), (hp_rel_raw.P[lf].p+init_wld_hp_lfpos), hp_rel_raw.w[rfw], hp_rel_raw.w[lfw], hp_rel_raw.P[zmp].p);//足の位置はworldにしないと・・・
       autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);
 
-//      hp_swap_checked.getP("com").p = com_in_filter.passFilter(hp_swap_checked.getP("com").p);
+//      hp_swap_checked.P[com].p = com_in_filter.passFilter(hp_swap_checked.P[com].p);
 
       convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
       hp_plot = rp_ref_out;
       if(loop==0)rp_ref_out_old = rp_ref_out;
 
-//      rp_ref_out.getP("com").p += torso_rot;
+//      rp_ref_out.P[com].p += torso_rot;
 
-      if(isHumanSyncOn() && WBMSparam.set_com_height_fix)rp_ref_out.getP("com").p(Z) = rp_ref_out.getP("com").p_offs(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
-      judgeFootLandOnCommand              (hp_wld_raw.getw("rfw"), hp_wld_raw.getw("lfw"), go_rf_landing, go_lf_landing);
+      if(isHumanSyncOn() && WBMSparam.set_com_height_fix)rp_ref_out.P[com].p(Z) = rp_ref_out.P[com].p_offs(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
+      judgeFootLandOnCommand              (hp_wld_raw.w[rfw], hp_wld_raw.w[lfw], go_rf_landing, go_lf_landing);
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
       applyEEWorkspaceLimit               (rp_ref_out);
       lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
-      if(go_rf_landing)rp_ref_out.getP("rf").p(Z) = rp_ref_out.getP("rf").p_offs(Z);
-      if(go_lf_landing)rp_ref_out.getP("lf").p(Z) = rp_ref_out.getP("lf").p_offs(Z);
-      if(!go_rf_landing)rp_ref_out.getP("rf").p(Z) += WBMSparam.swing_foot_height_offset;
-      if(!go_lf_landing)rp_ref_out.getP("lf").p(Z) += WBMSparam.swing_foot_height_offset;
+      if(go_rf_landing)rp_ref_out.P[rf].p(Z) = rp_ref_out.P[rf].p_offs(Z);
+      if(go_lf_landing)rp_ref_out.P[lf].p(Z) = rp_ref_out.P[lf].p_offs(Z);
+      if(!go_rf_landing)rp_ref_out.P[rf].p(Z) += WBMSparam.swing_foot_height_offset;
+      if(!go_lf_landing)rp_ref_out.P[lf].p(Z) += WBMSparam.swing_foot_height_offset;
 
-      if(DEBUG)fprintf(cz_log,"com_ref: %f %f ", rp_ref_out.getP("com").p(X), rp_ref_out.getP("com").p(Y));
+      if(DEBUG)fprintf(cz_log,"com_ref: %f %f ", rp_ref_out.P[com].p(X), rp_ref_out.P[com].p(Y));
 
-      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com").p);
+      applyCOMToSupportRegionLimit        (rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, rp_ref_out.P[com].p);
       setFootRotHorizontalIfGoLanding     (rp_ref_out_old, rp_ref_out);
 
 //      applyLPFilter                       (rp_ref_out);
-//      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com"));//もう一回？
-//      applyCOMStateLimitByCapturePoint    (rp_ref_out.getP("com").p, rp_ref_out_old.getP("com").p, com_vel_old, rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com").p);
-//      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com"));//もう一回？
+//      applyCOMToSupportRegionLimit        (rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, rp_ref_out.P[com]);//もう一回？
+//      applyCOMStateLimitByCapturePoint    (rp_ref_out.P[com].p, rp_ref_out_old.P[com].p, com_vel_old, rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, rp_ref_out.P[com].p);
+//      applyCOMToSupportRegionLimit        (rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, rp_ref_out.P[com]);//もう一回？
 
       applyVelLimit                       (rp_ref_out, rp_old_ref_Vel, rp_ref_out);
 
 
-      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
+      applyCOMToSupportRegionLimit        (rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
 
-      Vector3ToVector2(rp_ref_out.getP("com").p,com_forcp_ref);
+      Vector3ToVector2(rp_ref_out.P[com].p,com_forcp_ref);
       static hrp::Vector2 com_forcp_ref_old;
       com_vel_forcp_ref = (com_forcp_ref - com_forcp_ref_old)/DT;
       com_forcp_ref_old = com_forcp_ref;
 
-      applyCOMStateLimitByCapturePoint    (rp_ref_out.getP("com").p, rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, com_CP_ref_old, rp_ref_out.getP("com").p);
-      applyCOMToSupportRegionLimit        (rp_ref_out.getP("rf").p, rp_ref_out.getP("lf").p, rp_ref_out.getP("com").p);
+      applyCOMStateLimitByCapturePoint    (rp_ref_out.P[com].p, rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, com_CP_ref_old, rp_ref_out.P[com].p);
+      applyCOMToSupportRegionLimit        (rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, rp_ref_out.P[com].p);
       applyLPFilter                       (rp_ref_out);
 
       cam_rpy_filtered = cam_rpy_filter.passFilter(head_cam_pose.rpy);
-      r_zmp_raw = rp_ref_out.getP("zmp").p;
-      applyZMPCalcFromCOM                 (rp_ref_out.getP("com").p,rp_ref_out.getP("zmp").p);
+      r_zmp_raw = rp_ref_out.P[zmp].p;
+      applyZMPCalcFromCOM                 (rp_ref_out.P[com].p,rp_ref_out.P[zmp].p);
 
 
-      if(DEBUG)fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.getP("zmp").p(X),rp_ref_out.getP("zmp").p(Y));
-      if(DEBUG)fprintf(cz_log,"com_ans2: %f %f ",rp_ref_out.getP("com").p(X),rp_ref_out.getP("com").p(Y));
+      if(DEBUG)fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.P[zmp].p(X),rp_ref_out.P[zmp].p(Y));
+      if(DEBUG)fprintf(cz_log,"com_ans2: %f %f ",rp_ref_out.P[com].p(X),rp_ref_out.P[com].p(Y));
 
 //      applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
-      modifyFootRotAndXYForContact        (rp_ref_out.getP("rf"), rp_ref_out.getP("lf"));
+      modifyFootRotAndXYForContact        (rp_ref_out.P[rf], rp_ref_out.P[lf]);
 
-      if(DEBUG)fprintf(cz_log,"rf: %f %f %f ",rp_ref_out.getP("rf").p(X),rp_ref_out.getP("rf").p(Y),rp_ref_out.getP("rf").p(Z));
-      if(DEBUG)fprintf(cz_log,"lf: %f %f %f ",rp_ref_out.getP("lf").p(X),rp_ref_out.getP("lf").p(Y),rp_ref_out.getP("lf").p(Z));
+      if(DEBUG)fprintf(cz_log,"rf: %f %f %f ",rp_ref_out.P[rf].p(X),rp_ref_out.P[rf].p(Y),rp_ref_out.P[rf].p(Z));
+      if(DEBUG)fprintf(cz_log,"lf: %f %f %f ",rp_ref_out.P[lf].p(X),rp_ref_out.P[lf].p(Y),rp_ref_out.P[lf].p(Z));
       if(DEBUG)fprintf(cz_log,"\n");
 
-      com_vel_old = (rp_ref_out.getP("com").p - rp_ref_out_old.getP("com").p)/DT;
+      com_vel_old = (rp_ref_out.P[com].p - rp_ref_out_old.P[com].p)/DT;
       rp_ref_out_old = rp_ref_out;
       loop++;
       gettimeofday(&t_calc_end, NULL);
@@ -445,11 +453,11 @@ class HumanSynchronizer{
     static void Pose3DToHRPPose3D(const RTC::Pose3D& in, HRPPose3D& out){ Point3DToVector3(in.position,out.p); Oriantation3DToVector3(in.orientation,out.rpy); }
     static void HRPPose3DToPose3D(const HRPPose3D& in, RTC::Pose3D& out){ Vector3ToPoint3D(in.p,out.position); Vector3ToOriantation3D(in.rpy,out.orientation); }
     static void DoubleSeqToWrench6(const RTC::TimedDoubleSeq::_data_seq& in, Wrench6& out){
-      if(in.length() == 6){out.f(X)=in[0]; out.f(Y)=in[1]; out.f(Z)=in[2]; out.t(X)=in[3]; out.t(Y)=in[4]; out.t(Z)=in[5];
+      if(in.length() == 6){out.w(fx)=in[0]; out.w(fy)=in[1]; out.w(fz)=in[2]; out.w(tz)=in[3]; out.w(ty)=in[4]; out.w(tz)=in[5];
       }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; out.clear(); }
     }
     static void Wrench6ToDoubleSeq(const Wrench6& in, RTC::TimedDoubleSeq::_data_seq& out){
-      if(out.length() == 6){out[0]=in.f(X); out[1]=in.f(Y); out[2]=in.f(Z); out[3]=in.t(X); out[4]=in.t(Y); out[5]=in.t(Z);
+      if(out.length() == 6){out[0]=in.w(fx); out[1]=in.w(fy); out[2]=in.w(fz); out[3]=in.w(tx); out[4]=in.w(ty); out[5]=in.w(tz);
       }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; }
     }
     static double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(X)*b(Y)-a(Y)*b(X); }
@@ -469,13 +477,13 @@ class HumanSynchronizer{
     }
     void autoLRSwapCheck(const HumanPose& in, HumanPose& out){
       out = in;
-      if(in.getP("rh").p_offs(Y) > in.getP("lh").p_offs(Y) ){
-        out.getP("rh") = in.getP("lh");
-        out.getP("lh") = in.getP("rh");
+      if(in.P[rh].p_offs(Y) > in.P[lh].p_offs(Y) ){
+        out.P[rh] = in.P[lh];
+        out.P[lh] = in.P[rh];
       }
-      if(in.getP("rf").p_offs(Y) > in.getP("lf").p_offs(Y) ){
-        out.getP("rf") = in.getP("lf");
-        out.getP("lf") = in.getP("rf");
+      if(in.P[rf].p_offs(Y) > in.P[lf].p_offs(Y) ){
+        out.P[rf] = in.P[lf];
+        out.P[lf] = in.P[rf];
       }
     }
     void applyInitHumanZMPToCOMOffset(const HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
@@ -487,52 +495,52 @@ class HumanSynchronizer{
         zmp_com_offset_ip_ratio = 0;
       }
       out = in;
-      out.getP("com").p = in.getP("com").p + zmp_com_offset_ip_ratio * com_offs;
+      out.P[com].p = in.P[com].p + zmp_com_offset_ip_ratio * com_offs;
     }
     void applyLPFilter(HumanPose& tgt){
       std::string ns[5] = {"com","rf","lf","rh","lh"};
       if(loop==0){
         cout<<"init LPF"<<endl;
-        for(int i=0;i<tgt_pos_filters.size();i++){ tgt_pos_filters[i].init(tgt.getP(ns[i]).p); tgt_pos_filters[i].passFilter(tgt.getP(ns[i]).p);}
-        for(int i=0;i<tgt_rot_filters.size();i++){ tgt_rot_filters[i].init(tgt.getP(ns[i]).rpy); tgt_pos_filters[i].passFilter(tgt.getP(ns[i]).rpy);}
+        for(int i=0;i<tgt_pos_filters.size();i++){ tgt_pos_filters[i].init(tgt.P[i].p); tgt_pos_filters[i].passFilter(tgt.P[i].p);}
+        for(int i=0;i<tgt_rot_filters.size();i++){ tgt_rot_filters[i].init(tgt.P[i].rpy); tgt_pos_filters[i].passFilter(tgt.P[i].rpy);}
       }
-      for(int i=0;i<tgt_pos_filters.size();i++){ tgt.getP(ns[i]).p = tgt_pos_filters[i].passFilter(tgt.getP(ns[i]).p); }
-      for(int i=0;i<tgt_rot_filters.size();i++){ tgt.getP(ns[i]).rpy = tgt_rot_filters[i].passFilter(tgt.getP(ns[i]).rpy); }
+      for(int i=0;i<tgt_pos_filters.size();i++){ tgt.P[i].p = tgt_pos_filters[i].passFilter(tgt.P[i].p); }
+      for(int i=0;i<tgt_rot_filters.size();i++){ tgt.P[i].rpy = tgt_rot_filters[i].passFilter(tgt.P[i].rpy); }
     }
     void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
       std::string ns[6] = {"com","rf","lf","rh","lh","zmp"};
       for(int i=0;i<6;i++){
-        rp_out.getP(ns[i]).p   =  h2r_ratio * (hp_in.getP(ns[i]).p - hp_in.getP(ns[i]).p_offs)   + rp_out.getP(ns[i]).p_offs;
-        rp_out.getP(ns[i]).rpy =  hrp::rpyFromRot( hrp::rotFromRpy(hp_in.getP(ns[i]).rpy) * hrp::rotFromRpy(hp_in.getP(ns[i]).rpy_offs).transpose() * hrp::rotFromRpy(rp_out.getP(ns[i]).rpy_offs) );
+        rp_out.P[i].p   =  h2r_ratio * (hp_in.P[i].p - hp_in.P[i].p_offs)   + rp_out.P[i].p_offs;
+        rp_out.P[i].rpy =  hrp::rpyFromRot( hrp::rotFromRpy(hp_in.P[i].rpy) * hrp::rotFromRpy(hp_in.P[i].rpy_offs).transpose() * hrp::rotFromRpy(rp_out.P[i].rpy_offs) );
       }
-      rp_out.getw("rfw")  = hp_in.getw("rfw");
-      rp_out.getw("lfw")  = hp_in.getw("lfw");
+      rp_out.w[rfw]  = hp_in.w[rfw];
+      rp_out.w[lfw]  = hp_in.w[lfw];
     }
     void judgeFootLandOnCommand(const Wrench6& rfw_in, const Wrench6& lfw_in, bool& rfloc_ans, bool& lfloc_ans){
       if(isHumanSyncOn()){
-        if      (rfloc_ans  && rfw_in.f(Z)<CNT_F_TH   ){rfloc_ans = false;}//右足ついた状態から上げる
-        else if (!rfloc_ans && rfw_in.f(Z)>CNT_F_TH+30){rfloc_ans = true;}//右足浮いた状態から下げる
-        if      (lfloc_ans  && lfw_in.f(Z)<CNT_F_TH   ){lfloc_ans = false;}//左足ついた状態から上げる
-        else if (!lfloc_ans && lfw_in.f(Z)>CNT_F_TH+30){lfloc_ans = true;}//左足浮いた状態から下げる
+        if      (rfloc_ans  && rfw_in.w(fz)<CNT_F_TH   ){rfloc_ans = false;}//右足ついた状態から上げる
+        else if (!rfloc_ans && rfw_in.w(fz)>CNT_F_TH+30){rfloc_ans = true;}//右足浮いた状態から下げる
+        if      (lfloc_ans  && lfw_in.w(fz)<CNT_F_TH   ){lfloc_ans = false;}//左足ついた状態から上げる
+        else if (!lfloc_ans && lfw_in.w(fz)>CNT_F_TH+30){lfloc_ans = true;}//左足浮いた状態から下げる
       }
       //      if(!rfloc_ans && !lfloc_ans){rfloc_ans = lfloc_ans = true;}//両足ジャンプは禁止
     }
     void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
       hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);
-      hrp::Vector2 rf2zmp = hrp::Vector2(tgt.getP("zmp").p(X),tgt.getP("zmp").p(Y)) - hrp::Vector2(tgt.getP("rf").p(X),tgt.getP("rf").p(Y));
-      hrp::Vector2 lf2zmp = hrp::Vector2(tgt.getP("zmp").p(X),tgt.getP("zmp").p(Y)) - hrp::Vector2(tgt.getP("lf").p(X),tgt.getP("lf").p(Y));
+      hrp::Vector2 rf2zmp = hrp::Vector2(tgt.P[zmp].p(X),tgt.P[zmp].p(Y)) - hrp::Vector2(tgt.P[rf].p(X),tgt.P[rf].p(Y));
+      hrp::Vector2 lf2zmp = hrp::Vector2(tgt.P[zmp].p(X),tgt.P[zmp].p(Y)) - hrp::Vector2(tgt.P[lf].p(X),tgt.P[lf].p(Y));
       if( inside_vec_rf.dot(rf2zmp) / inside_vec_rf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ lfcs_ans = true; }
       if( inside_vec_lf.dot(lf2zmp) / inside_vec_lf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ rfcs_ans = true; }
     }
     void setFootRotHorizontalIfGoLanding(const HumanPose& rp_ref_out_old, HumanPose& rp_ref_out){
-      if(go_rf_landing){ rp_ref_out.getP("rf").rpy(X) = rp_ref_out.getP("rf").rpy(Y) = 0; }
-      if(go_lf_landing){ rp_ref_out.getP("lf").rpy(X) = rp_ref_out.getP("lf").rpy(Y) = 0; }
+      if(go_rf_landing){ rp_ref_out.P[rf].rpy(X) = rp_ref_out.P[rf].rpy(Y) = 0; }
+      if(go_lf_landing){ rp_ref_out.P[lf].rpy(X) = rp_ref_out.P[lf].rpy(Y) = 0; }
     }
     void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
-      const double com2rf_dist = fabs((double)tgt.getP("rf").p(Y) - (double)tgt.getP("com").p(Y));
-      const double com2lf_dist = fabs((double)tgt.getP("lf").p(Y) - (double)tgt.getP("com").p(Y));
-      limitGroundContactVelocity(rf_goland_in, com2rf_dist, rp_ref_out_old.getP("rf"), tgt.getP("rf"), is_rf_contact);
-      limitGroundContactVelocity(lf_goland_in, com2lf_dist, rp_ref_out_old.getP("lf"), tgt.getP("lf"), is_lf_contact);
+      const double com2rf_dist = fabs((double)tgt.P[rf].p(Y) - (double)tgt.P[com].p(Y));
+      const double com2lf_dist = fabs((double)tgt.P[lf].p(Y) - (double)tgt.P[com].p(Y));
+      limitGroundContactVelocity(rf_goland_in, com2rf_dist, rp_ref_out_old.P[rf], tgt.P[rf], is_rf_contact);
+      limitGroundContactVelocity(lf_goland_in, com2lf_dist, rp_ref_out_old.P[lf], tgt.P[lf], is_lf_contact);
     }
     void limitGroundContactVelocity(const bool& go_land_in, const double& com2foot_dist, const HRPPose3D& f_old_in, HRPPose3D& f_in_out, bool& is_f_contact_out){
       double penalty = 1.0 + (0.2 - com2foot_dist) * 5.0;//1.0 ~ 2.0 本当はCPに比例すべき？
@@ -571,22 +579,22 @@ class HumanSynchronizer{
     }
     void lockFootXYOnContact(const bool& rfcs_in, const bool& lfcs_in, HumanPose& tgt){
       if(rfcs_in){
-        tgt.getP("rf").p(X) = pre_cont_rfpos(X);
-        tgt.getP("rf").p(Y) = pre_cont_rfpos(Y);
-        tgt.getP("rf").rpy(Z) = pre_cont_rfrot(Z);
+        tgt.P[rf].p(X) = pre_cont_rfpos(X);
+        tgt.P[rf].p(Y) = pre_cont_rfpos(Y);
+        tgt.P[rf].rpy(Z) = pre_cont_rfrot(Z);
       }else{
-        pre_cont_rfpos(X) = tgt.getP("rf").p(X);
-        pre_cont_rfpos(Y) = tgt.getP("rf").p(Y);
-        pre_cont_rfrot(Z) = tgt.getP("rf").rpy(Z);
+        pre_cont_rfpos(X) = tgt.P[rf].p(X);
+        pre_cont_rfpos(Y) = tgt.P[rf].p(Y);
+        pre_cont_rfrot(Z) = tgt.P[rf].rpy(Z);
       }
       if(lfcs_in){
-        tgt.getP("lf").p(X) = pre_cont_lfpos(X);
-        tgt.getP("lf").p(Y) = pre_cont_lfpos(Y);
-        tgt.getP("lf").rpy(Z) = pre_cont_lfrot(Z);
+        tgt.P[lf].p(X) = pre_cont_lfpos(X);
+        tgt.P[lf].p(Y) = pre_cont_lfpos(Y);
+        tgt.P[lf].rpy(Z) = pre_cont_lfrot(Z);
       }else{
-        pre_cont_lfpos(X) = tgt.getP("lf").p(X);
-        pre_cont_lfpos(Y) = tgt.getP("lf").p(Y);
-        pre_cont_lfrot(Z) = tgt.getP("lf").rpy(Z);
+        pre_cont_lfpos(X) = tgt.P[lf].p(X);
+        pre_cont_lfpos(Y) = tgt.P[lf].p(Y);
+        pre_cont_lfrot(Z) = tgt.P[lf].rpy(Z);
       }
     }
     void applyEEWorkspaceLimit(HumanPose& tgt){
@@ -594,47 +602,47 @@ class HumanSynchronizer{
       const double MAX_FW = 0.5;
       const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
       if(!is_rf_contact && is_lf_contact){//右足浮遊時
-        const hrp::Vector2 lf2rf_vec( tgt.getP("rf").p(X)-pre_cont_lfpos(X), tgt.getP("rf").p(Y)-pre_cont_lfpos(Y) );
+        const hrp::Vector2 lf2rf_vec( tgt.P[rf].p(X)-pre_cont_lfpos(X), tgt.P[rf].p(Y)-pre_cont_lfpos(Y) );
         if(lf2rf_vec.norm() > MAX_FW){
-          tgt.getP("rf").p(X) = (lf2rf_vec.normalized())(X) * MAX_FW + pre_cont_lfpos(X);
-          tgt.getP("rf").p(Y) = (lf2rf_vec.normalized())(Y) * MAX_FW + pre_cont_lfpos(Y);
+          tgt.P[rf].p(X) = (lf2rf_vec.normalized())(X) * MAX_FW + pre_cont_lfpos(X);
+          tgt.P[rf].p(Y) = (lf2rf_vec.normalized())(Y) * MAX_FW + pre_cont_lfpos(Y);
         }
-        LIMIT_MAX(tgt.getP("rf").p(Y), pre_cont_lfpos(Y) - FOOT_2_FOOT_COLLISION_MARGIIN);
+        LIMIT_MAX(tgt.P[rf].p(Y), pre_cont_lfpos(Y) - FOOT_2_FOOT_COLLISION_MARGIIN);
       }
       else if(is_rf_contact && !is_lf_contact){//左足浮遊時
-        const hrp::Vector2 rf2lf_vec( tgt.getP("lf").p(X)-pre_cont_rfpos(X), tgt.getP("lf").p(Y)-pre_cont_rfpos(Y) );
+        const hrp::Vector2 rf2lf_vec( tgt.P[lf].p(X)-pre_cont_rfpos(X), tgt.P[lf].p(Y)-pre_cont_rfpos(Y) );
         if(rf2lf_vec.norm() > MAX_FW){
-          tgt.getP("lf").p(X) = (rf2lf_vec.normalized())(X) * MAX_FW + pre_cont_rfpos(X);
-          tgt.getP("lf").p(Y) = (rf2lf_vec.normalized())(Y) * MAX_FW + pre_cont_rfpos(Y);
+          tgt.P[lf].p(X) = (rf2lf_vec.normalized())(X) * MAX_FW + pre_cont_rfpos(X);
+          tgt.P[lf].p(Y) = (rf2lf_vec.normalized())(Y) * MAX_FW + pre_cont_rfpos(Y);
         }
-        LIMIT_MIN(tgt.getP("lf").p(Y), pre_cont_rfpos(Y) + FOOT_2_FOOT_COLLISION_MARGIIN);
+        LIMIT_MIN(tgt.P[lf].p(Y), pre_cont_rfpos(Y) + FOOT_2_FOOT_COLLISION_MARGIIN);
       }
-      hrp::Vector3 init_base2rh = tgt.getP("rh").p_offs - baselinkpose.p_offs;
-      hrp::Vector3 init_base2lh = tgt.getP("lh").p_offs - baselinkpose.p_offs;
-      hrp::Matrix33 init_base2rh_R = hrp::rotFromRpy(baselinkpose.rpy_offs).transpose() * hrp::rotFromRpy(tgt.getP("rh").rpy_offs);
-      hrp::Matrix33 init_base2lh_R = hrp::rotFromRpy(baselinkpose.rpy_offs).transpose() * hrp::rotFromRpy(tgt.getP("lh").rpy_offs);
+      hrp::Vector3 init_base2rh = tgt.P[rh].p_offs - baselinkpose.p_offs;
+      hrp::Vector3 init_base2lh = tgt.P[lh].p_offs - baselinkpose.p_offs;
+      hrp::Matrix33 init_base2rh_R = hrp::rotFromRpy(baselinkpose.rpy_offs).transpose() * hrp::rotFromRpy(tgt.P[rh].rpy_offs);
+      hrp::Matrix33 init_base2lh_R = hrp::rotFromRpy(baselinkpose.rpy_offs).transpose() * hrp::rotFromRpy(tgt.P[lh].rpy_offs);
       hrp::Vector3 hand_ulimit = hrp::Vector3(0.1,0.1,0.1);
       hrp::Vector3 hand_llimit = hrp::Vector3(-0.1,-0.1,-0.1);
       if(!WBMSparam.use_rh){
-        tgt.getP("rh").rpy = hrp::rpyFromRot( hrp::rotFromRpy(baselinkpose.rpy) * init_base2rh_R);
-        tgt.getP("rh").p = baselinkpose.p + hrp::rotFromRpy(tgt.getP("rh").rpy) * init_base2rh;
+        tgt.P[rh].rpy = hrp::rpyFromRot( hrp::rotFromRpy(baselinkpose.rpy) * init_base2rh_R);
+        tgt.P[rh].p = baselinkpose.p + hrp::rotFromRpy(tgt.P[rh].rpy) * init_base2rh;
       }
       if(!WBMSparam.use_lh){
-        tgt.getP("lh").rpy = hrp::rpyFromRot( hrp::rotFromRpy(baselinkpose.rpy) * init_base2lh_R);
-        tgt.getP("lh").p = baselinkpose.p + hrp::rotFromRpy(tgt.getP("lh").rpy) * init_base2lh;
+        tgt.P[lh].rpy = hrp::rpyFromRot( hrp::rotFromRpy(baselinkpose.rpy) * init_base2lh_R);
+        tgt.P[lh].p = baselinkpose.p + hrp::rotFromRpy(tgt.P[lh].rpy) * init_base2lh;
       }
       for(int i=0;i<3;i++){
-        LIMIT_MINMAX( tgt.getP("rh").p(i), (baselinkpose.p+init_base2rh+hand_llimit)(i), (baselinkpose.p+init_base2rh+hand_ulimit)(i) );
-        LIMIT_MINMAX( tgt.getP("lh").p(i), (baselinkpose.p+init_base2lh+hand_llimit)(i), (baselinkpose.p+init_base2lh+hand_ulimit)(i) );
+        LIMIT_MINMAX( tgt.P[rh].p(i), (baselinkpose.p+init_base2rh+hand_llimit)(i), (baselinkpose.p+init_base2rh+hand_ulimit)(i) );
+        LIMIT_MINMAX( tgt.P[lh].p(i), (baselinkpose.p+init_base2lh+hand_llimit)(i), (baselinkpose.p+init_base2lh+hand_ulimit)(i) );
       }
-      LIMIT_MINMAX( tgt.getP("rf").p(Z), tgt.getP("rf").p_offs(Z), tgt.getP("rf").p_offs(Z)+WBMSparam.swing_foot_max_height);
-      LIMIT_MINMAX( tgt.getP("lf").p(Z), tgt.getP("lf").p_offs(Z), tgt.getP("lf").p_offs(Z)+WBMSparam.swing_foot_max_height);
+      LIMIT_MINMAX( tgt.P[rf].p(Z), tgt.P[rf].p_offs(Z), tgt.P[rf].p_offs(Z)+WBMSparam.swing_foot_max_height);
+      LIMIT_MINMAX( tgt.P[lf].p(Z), tgt.P[lf].p_offs(Z), tgt.P[lf].p_offs(Z)+WBMSparam.swing_foot_max_height);
 
       std::string ns[5] = {"com","rf","lf","rh","lh"};
-      for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( tgt.getP("com").rpy(i), rc.ee_rot_limit["com"][MIN](i), rc.ee_rot_limit["com"][MAX](i) ); }
-      LIMIT_MINMAX( tgt.getP("com").p(Z), tgt.getP("com").p_offs(Z) - 0.15, tgt.getP("com").p_offs(Z) + 0.03 );//COM高さ方向の制限
+      for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( tgt.P[com].rpy(i), rc.ee_rot_limit["com"][MIN](i), rc.ee_rot_limit["com"][MAX](i) ); }
+      LIMIT_MINMAX( tgt.P[com].p(Z), tgt.P[com].p_offs(Z) - 0.15, tgt.P[com].p_offs(Z) + 0.03 );//COM高さ方向の制限
       for(int i=1;i<5;i++){
-        for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( tgt.getP(ns[i]).rpy(j), rc.ee_rot_limit[ns[i]][MIN](j) + tgt.getP("com").rpy(j), rc.ee_rot_limit[ns[i]][MAX](j) + tgt.getP("com").rpy(j) ); }
+        for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( tgt.P[i].rpy(j), rc.ee_rot_limit[ns[i]][MIN](j) + tgt.P[com].rpy(j), rc.ee_rot_limit[ns[i]][MAX](j) + tgt.P[com].rpy(j) ); }
       }
       if(!WBMSparam.use_head)head_cam_pose.rpy = hrp::Vector3::Zero();
       if(!isHumanSyncOn())head_cam_pose.rpy = hrp::Vector3::Zero();
@@ -748,7 +756,7 @@ class HumanSynchronizer{
         }
       }
 //      static unsigned int edge_time = 0;
-//      if(rp_ref_out_old.getP("rf").rpy(Y)>0.001){
+//      if(rp_ref_out_old.P[rf].rpy(Y)>0.001){
 //        points.clear();
 //        double mod = rfin_abs(X) + 0.13 * edge_time * 0.002;
 ////        LIMIT_MAX( mod, rfin_abs(X) + 0.13 );

From 08e4057b5762f72392d227a3e13044e9a76df2e1 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 6 May 2017 22:34:52 +0900
Subject: [PATCH 077/217] tune up for speed

---
 .../WholeBodyMasterSlave.cpp                  | 216 +++++++++---------
 .../WholeBodyMasterSlave.h                    |  96 ++++----
 rtc/WholeBodyMasterSlave/wbms_core.cpp        |   1 -
 rtc/WholeBodyMasterSlave/wbms_core.h          | 132 ++++-------
 4 files changed, 199 insertions(+), 246 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index f607d8b6c9d..33115a606cf 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -60,28 +60,31 @@ WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager)
       m_htlfIn("htlfIn", m_htlf),
       m_htrhIn("htrhIn", m_htrh),
       m_htlhIn("htlhIn", m_htlh),
-      m_actzmpIn("actzmpIn", m_actzmp),
+//      m_actzmpIn("actzmpIn", m_actzmp),
       m_htheadIn("htheadIn", m_hthead),
+
+#ifdef USE_DEBUG_PORT
       // to ros bridge
-      // m_htcom_dbgOut("htcom_dbgOut", m_htcom_dbg),
-      // m_htrf_dbgOut("htrf_dbgOut", m_htrf_dbg),
-      // m_htlf_dbgOut("htlf_dbgOut", m_htlf_dbg),
-      // m_htrh_dbgOut("htrh_dbgOut", m_htrh_dbg),
-      // m_htlh_dbgOut("htlh_dbgOut", m_htlh_dbg),
-      // m_hthead_dbgOut("hthead_dbgOut", m_hthead_dbg),
-      // m_htzmp_dbgOut("htzmp_dbgOut", m_htzmp_dbg),
-      // m_htrfw_dbgOut("htrfw_dbgOut", m_htrfw_dbg),
-      // m_htlfw_dbgOut("htlfw_dbgOut", m_htlfw_dbg),
-      // m_rpcom_dbgOut("rpcom_dbgOut", m_rpcom_dbg),
-      // m_rprf_dbgOut("rprf_dbgOut", m_rprf_dbg),
-      // m_rplf_dbgOut("rplf_dbgOut", m_rplf_dbg),
-      // m_rprh_dbgOut("rprh_dbgOut", m_rprh_dbg),
-      // m_rplh_dbgOut("rplh_dbgOut", m_rplh_dbg),
-      // m_rphead_dbgOut("rphead_dbgOut", m_rphead_dbg),
-      // m_rpzmp_dbgOut("rpzmp_dbgOut", m_rpzmp_dbg),
-      // m_rpdcp_dbgOut("rpdcp_dbgOut", m_rpdcp_dbg),
-      // m_rpacp_dbgOut("rpacp_dbgOut", m_rpacp_dbg),
-      // m_invdyn_dbgOut("invdyn_dbgOut", m_invdyn_dbg),
+       m_htcom_dbgOut("htcom_dbgOut", m_htcom_dbg),
+       m_htrf_dbgOut("htrf_dbgOut", m_htrf_dbg),
+       m_htlf_dbgOut("htlf_dbgOut", m_htlf_dbg),
+       m_htrh_dbgOut("htrh_dbgOut", m_htrh_dbg),
+       m_htlh_dbgOut("htlh_dbgOut", m_htlh_dbg),
+       m_hthead_dbgOut("hthead_dbgOut", m_hthead_dbg),
+       m_htzmp_dbgOut("htzmp_dbgOut", m_htzmp_dbg),
+       m_htrfw_dbgOut("htrfw_dbgOut", m_htrfw_dbg),
+       m_htlfw_dbgOut("htlfw_dbgOut", m_htlfw_dbg),
+       m_rpcom_dbgOut("rpcom_dbgOut", m_rpcom_dbg),
+       m_rprf_dbgOut("rprf_dbgOut", m_rprf_dbg),
+       m_rplf_dbgOut("rplf_dbgOut", m_rplf_dbg),
+       m_rprh_dbgOut("rprh_dbgOut", m_rprh_dbg),
+       m_rplh_dbgOut("rplh_dbgOut", m_rplh_dbg),
+       m_rphead_dbgOut("rphead_dbgOut", m_rphead_dbg),
+       m_rpzmp_dbgOut("rpzmp_dbgOut", m_rpzmp_dbg),
+       m_rpdcp_dbgOut("rpdcp_dbgOut", m_rpdcp_dbg),
+       m_rpacp_dbgOut("rpacp_dbgOut", m_rpacp_dbg),
+       m_invdyn_dbgOut("invdyn_dbgOut", m_invdyn_dbg),
+#endif
       m_WholeBodyMasterSlaveServicePort("WholeBodyMasterSlaveService"),
       m_robot(hrp::BodyPtr()),
       m_debugLevel(0)
@@ -118,28 +121,30 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
     addInPort("htlfIn", m_htlfIn);
     addInPort("htrhIn", m_htrhIn);
     addInPort("htlhIn", m_htlhIn);
-    addInPort("actzmpIn", m_actzmpIn);
+//    addInPort("actzmpIn", m_actzmpIn);
     addInPort("htheadIn", m_htheadIn);
 
-    // addOutPort("htcom_dbgOut", m_htcom_dbgOut);
-    // addOutPort("htrf_dbgOut", m_htrf_dbgOut);
-    // addOutPort("htlf_dbgOut", m_htlf_dbgOut);
-    // addOutPort("htrh_dbgOut", m_htrh_dbgOut);
-    // addOutPort("htlh_dbgOut", m_htlh_dbgOut);
-    // addOutPort("hthead_dbgOut", m_hthead_dbgOut);
-    // addOutPort("htzmp_dbgOut", m_htzmp_dbgOut);
-    // addOutPort("htrfw_dbgOut", m_htrfw_dbgOut);
-    // addOutPort("htlfw_dbgOut", m_htlfw_dbgOut);
-    // addOutPort("rpcom_dbgOut", m_rpcom_dbgOut);
-    // addOutPort("rprf_dbgOut", m_rprf_dbgOut);
-    // addOutPort("rplf_dbgOut", m_rplf_dbgOut);
-    // addOutPort("rprh_dbgOut", m_rprh_dbgOut);
-    // addOutPort("rplh_dbgOut", m_rplh_dbgOut);
-    // addOutPort("rphead_dbgOut", m_rphead_dbgOut);
-    // addOutPort("rpzmp_dbgOut", m_rpzmp_dbgOut);
-    // addOutPort("rpdcp_dbgOut", m_rpdcp_dbgOut);
-    // addOutPort("rpacp_dbgOut", m_rpacp_dbgOut);
-    // addOutPort("invdyn_dbgOut", m_invdyn_dbgOut);
+#ifdef USE_DEBUG_PORT
+     addOutPort("htcom_dbgOut", m_htcom_dbgOut);
+     addOutPort("htrf_dbgOut", m_htrf_dbgOut);
+     addOutPort("htlf_dbgOut", m_htlf_dbgOut);
+     addOutPort("htrh_dbgOut", m_htrh_dbgOut);
+     addOutPort("htlh_dbgOut", m_htlh_dbgOut);
+     addOutPort("hthead_dbgOut", m_hthead_dbgOut);
+     addOutPort("htzmp_dbgOut", m_htzmp_dbgOut);
+     addOutPort("htrfw_dbgOut", m_htrfw_dbgOut);
+     addOutPort("htlfw_dbgOut", m_htlfw_dbgOut);
+     addOutPort("rpcom_dbgOut", m_rpcom_dbgOut);
+     addOutPort("rprf_dbgOut", m_rprf_dbgOut);
+     addOutPort("rplf_dbgOut", m_rplf_dbgOut);
+     addOutPort("rprh_dbgOut", m_rprh_dbgOut);
+     addOutPort("rplh_dbgOut", m_rplh_dbgOut);
+     addOutPort("rphead_dbgOut", m_rphead_dbgOut);
+     addOutPort("rpzmp_dbgOut", m_rpzmp_dbgOut);
+     addOutPort("rpdcp_dbgOut", m_rpdcp_dbgOut);
+     addOutPort("rpacp_dbgOut", m_rpacp_dbgOut);
+     addOutPort("invdyn_dbgOut", m_invdyn_dbgOut);
+#endif
 
     m_WholeBodyMasterSlaveServicePort.registerProvider("service0", "WholeBodyMasterSlaveService", m_service0);
   
@@ -309,7 +314,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
     if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrh.data,hsp->hp_wld_raw.P[rh]);}
     if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlh.data,hsp->hp_wld_raw.P[lh]);}
     if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_hthead.data,hsp->head_cam_pose);}
-    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
+//    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
 
 
     if ( is_legged_robot ) {
@@ -416,68 +421,69 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
       m_optionalDataOut.write();
     }
 
-    //ishiguro dbg plot
-    // m_htcom.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[com],m_htcom_dbg.data);
-    // m_htcom_dbgOut.write();
-    // m_htrf.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[rf],m_htrf_dbg.data);
-    // m_htrf_dbgOut.write();
-    // m_htlf.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[lf],m_htlf_dbg.data);
-    // m_htlf_dbgOut.write();
-    // m_htrh.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[rh],m_htrh_dbg.data);
-    // m_htrh_dbgOut.write();
-    // m_htlh.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[lh],m_htlh_dbg.data);
-    // m_htlh_dbgOut.write();
-    // m_hthead.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.getP("head"),m_hthead_dbg.data);
-    // m_hthead_dbgOut.write();
-    // m_htzmp.tm = m_qRef.tm;
-    // HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.P[zmp].p,m_rpzmp_dbg.data);
-    // m_htzmp_dbgOut.write();
-    // m_htrfw.tm = m_qRef.tm;
-    // m_htrfw_dbg.data.length(6);
-    // HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.w[rfw],m_htrfw_dbg.data);
-    // m_htrfw_dbgOut.write(); 
-    // m_htlfw.tm = m_qRef.tm;
-    // m_htlfw_dbg.data.length(6);
-    // HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.w[lfw],m_htlfw_dbg.data);
-    // m_htlfw_dbgOut.write();
-    // m_rpcom_dbg.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[com],m_rpcom_dbg.data);
-    // m_rpcom_dbgOut.write();
-    // m_rprf_dbg.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[rf],m_rprf_dbg.data);
-    // m_rprf_dbgOut.write();
-    // m_rplf_dbg.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[lf],m_rplf_dbg.data);
-    // m_rplf_dbgOut.write();
-    // m_rprh_dbg.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[rh],m_rprh_dbg.data);
-    // m_rprh_dbgOut.write();
-    // m_rplh_dbg.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[lh],m_rplh_dbg.data);
-    // m_rplh_dbgOut.write();
-    // m_rphead_dbg.tm = m_qRef.tm;
-    // HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.getP("head"),m_rphead_dbg.data);
-    // m_rphead_dbgOut.write();
-    // m_rpzmp_dbg.tm = m_qRef.tm;
-    // HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.P[zmp].p,m_rpzmp_dbg.data);
-    // m_rpzmp_dbgOut.write();
-    // m_rpdcp_dbg.tm = m_qRef.tm;
-    // HumanSynchronizer::Vector3ToPoint3D(hsp->cp_dec,m_rpdcp_dbg.data);
-    // m_rpdcp_dbgOut.write();
-    // m_rpacp_dbg.tm = m_qRef.tm;
-    // HumanSynchronizer::Vector3ToPoint3D(hsp->cp_acc,m_rpacp_dbg.data);
-    // m_rpacp_dbgOut.write();
-    // m_invdyn_dbg.tm = m_qRef.tm;
-    // m_invdyn_dbg.data.length(6);
-    // HumanSynchronizer::Wrench6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
-    // m_invdyn_dbgOut.write();
-
+#ifdef USE_DEBUG_PORT
+    // dbg plot
+     m_htcom.tm = m_qRef.tm;
+     HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[com],m_htcom_dbg.data);
+     m_htcom_dbgOut.write();
+     m_htrf.tm = m_qRef.tm;
+     HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[rf],m_htrf_dbg.data);
+     m_htrf_dbgOut.write();
+     m_htlf.tm = m_qRef.tm;
+     HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[lf],m_htlf_dbg.data);
+     m_htlf_dbgOut.write();
+     m_htrh.tm = m_qRef.tm;
+     HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[rh],m_htrh_dbg.data);
+     m_htrh_dbgOut.write();
+     m_htlh.tm = m_qRef.tm;
+     HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[lh],m_htlh_dbg.data);
+     m_htlh_dbgOut.write();
+     m_hthead.tm = m_qRef.tm;
+     HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[head],m_hthead_dbg.data);
+     m_hthead_dbgOut.write();
+     m_htzmp.tm = m_qRef.tm;
+     HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.P[zmp].p,m_rpzmp_dbg.data);
+     m_htzmp_dbgOut.write();
+     m_htrfw.tm = m_qRef.tm;
+     m_htrfw_dbg.data.length(6);
+     HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.w[rfw],m_htrfw_dbg.data);
+     m_htrfw_dbgOut.write();
+     m_htlfw.tm = m_qRef.tm;
+     m_htlfw_dbg.data.length(6);
+     HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.w[lfw],m_htlfw_dbg.data);
+     m_htlfw_dbgOut.write();
+     m_rpcom_dbg.tm = m_qRef.tm;
+     HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[com],m_rpcom_dbg.data);
+     m_rpcom_dbgOut.write();
+     m_rprf_dbg.tm = m_qRef.tm;
+     HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[rf],m_rprf_dbg.data);
+     m_rprf_dbgOut.write();
+     m_rplf_dbg.tm = m_qRef.tm;
+     HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[lf],m_rplf_dbg.data);
+     m_rplf_dbgOut.write();
+     m_rprh_dbg.tm = m_qRef.tm;
+     HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[rh],m_rprh_dbg.data);
+     m_rprh_dbgOut.write();
+     m_rplh_dbg.tm = m_qRef.tm;
+     HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[lh],m_rplh_dbg.data);
+     m_rplh_dbgOut.write();
+     m_rphead_dbg.tm = m_qRef.tm;
+     HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[head],m_rphead_dbg.data);
+     m_rphead_dbgOut.write();
+     m_rpzmp_dbg.tm = m_qRef.tm;
+     HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.P[zmp].p,m_rpzmp_dbg.data);
+     m_rpzmp_dbgOut.write();
+     m_rpdcp_dbg.tm = m_qRef.tm;
+     HumanSynchronizer::Vector3ToPoint3D(hsp->cp_dec,m_rpdcp_dbg.data);
+     m_rpdcp_dbgOut.write();
+     m_rpacp_dbg.tm = m_qRef.tm;
+     HumanSynchronizer::Vector3ToPoint3D(hsp->cp_acc,m_rpacp_dbg.data);
+     m_rpacp_dbgOut.write();
+     m_invdyn_dbg.tm = m_qRef.tm;
+     m_invdyn_dbg.data.length(6);
+     HumanSynchronizer::Wrench6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
+     m_invdyn_dbgOut.write();
+#endif
 
 
 
@@ -550,7 +556,9 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
   if(hsp->isHumanSyncOn()){
     solveFullbodyIKStrictCOM( hsp->rp_ref_out.P[com], hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->cam_rpy_filtered );
     //outport用のデータ上書き
-    hsp->rp_ref_out.P[zmp].p(2) = (hsp->rp_ref_out.P[rf].p(2) + hsp->rp_ref_out.P[lf].p(2))/2 - m_robot->rootLink()->p(2);//体幹相対ZMP高さ設定
+    hsp->rp_ref_out.P[zmp].p(2) = (hsp->rp_ref_out.P[rf].p(2) + hsp->rp_ref_out.P[lf].p(2))/2;//体幹相対ZMP高さ設定
+
+
     rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.P[zmp].p - m_robot->rootLink()->p);
     if(m_optionalData.data.length() < 4*2){
       m_optionalData.data.length(4*2);//これいいのか？
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 506f83dc5aa..9b428c8b4c4 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -43,22 +43,12 @@ using namespace RTC;
 //    return os;
 //}
 
+#define USE_DEBUG_PORT
 
 class WholeBodyMasterSlave
   : public RTC::DataFlowComponentBase
 {
-
-
-
-
-    enum pose_tgt{ com, rf, lf, rh, lh, zmp, head, num_pose_tgt } p_id;
-    enum wrench_tgt{ rfw, lfw, num_wrench_tgt } w_id;
-    enum lr_direction{ L, R, LR} lr_direc;
-    enum xyz_direction{ X, Y, Z, XYZ} xyz_direc;
-    enum rpy_direction{ r, p, y, rpy} rpy_direc;
-    enum ft_direction{ fx, fy, fz, tx, ty, tz, ft_xyz} w_direc;
-
-
+    DEF_UTIL_CONST
 
  public:
   WholeBodyMasterSlave(RTC::Manager* manager);
@@ -99,51 +89,53 @@ class WholeBodyMasterSlave
   InPort<TimedPose3D> m_htheadIn;
   TimedPoint3D m_htzmp;
   InPort<TimedPoint3D> m_htzmpIn;
-  TimedPoint3D m_actzmp;
-  InPort<TimedPoint3D> m_actzmpIn;
+//  TimedPoint3D m_actzmp;
+//  InPort<TimedPoint3D> m_actzmpIn;
   TimedDoubleSeq m_htrfw;
   InPort<TimedDoubleSeq> m_htrfwIn;
   TimedDoubleSeq m_htlfw;
   InPort<TimedDoubleSeq> m_htlfwIn;
 
-  // TimedPose3D m_htcom_dbg;
-  // OutPort<TimedPose3D> m_htcom_dbgOut;
-  // TimedPose3D m_htrf_dbg;
-  // OutPort<TimedPose3D> m_htrf_dbgOut;
-  // TimedPose3D m_htlf_dbg;
-  // OutPort<TimedPose3D> m_htlf_dbgOut;
-  // TimedPose3D m_htrh_dbg;
-  // OutPort<TimedPose3D> m_htrh_dbgOut;
-  // TimedPose3D m_htlh_dbg;
-  // OutPort<TimedPose3D> m_htlh_dbgOut;
-  // TimedPose3D m_hthead_dbg;
-  // OutPort<TimedPose3D> m_hthead_dbgOut;
-  // TimedPoint3D m_htzmp_dbg;
-  // OutPort<TimedPoint3D> m_htzmp_dbgOut;
-  // TimedDoubleSeq m_htrfw_dbg;
-  // OutPort<TimedDoubleSeq> m_htrfw_dbgOut;
-  // TimedDoubleSeq m_htlfw_dbg;
-  // OutPort<TimedDoubleSeq> m_htlfw_dbgOut;
-  // TimedPose3D m_rpcom_dbg;
-  // OutPort<TimedPose3D> m_rpcom_dbgOut;
-  // TimedPose3D m_rprf_dbg;
-  // OutPort<TimedPose3D> m_rprf_dbgOut;
-  // TimedPose3D m_rplf_dbg;
-  // OutPort<TimedPose3D> m_rplf_dbgOut;
-  // TimedPose3D m_rprh_dbg;
-  // OutPort<TimedPose3D> m_rprh_dbgOut;
-  // TimedPose3D m_rplh_dbg;
-  // OutPort<TimedPose3D> m_rplh_dbgOut;
-  // TimedPose3D m_rphead_dbg;
-  // OutPort<TimedPose3D> m_rphead_dbgOut;
-  // TimedPoint3D m_rpzmp_dbg;
-  // OutPort<TimedPoint3D> m_rpzmp_dbgOut;
-  // TimedPoint3D m_rpdcp_dbg;
-  // OutPort<TimedPoint3D> m_rpdcp_dbgOut;
-  // TimedPoint3D m_rpacp_dbg;
-  // OutPort<TimedPoint3D> m_rpacp_dbgOut;
-  // TimedDoubleSeq m_invdyn_dbg;
-  // OutPort<TimedDoubleSeq> m_invdyn_dbgOut;
+#ifdef USE_DEBUG_PORT
+   TimedPose3D m_htcom_dbg;
+   OutPort<TimedPose3D> m_htcom_dbgOut;
+   TimedPose3D m_htrf_dbg;
+   OutPort<TimedPose3D> m_htrf_dbgOut;
+   TimedPose3D m_htlf_dbg;
+   OutPort<TimedPose3D> m_htlf_dbgOut;
+   TimedPose3D m_htrh_dbg;
+   OutPort<TimedPose3D> m_htrh_dbgOut;
+   TimedPose3D m_htlh_dbg;
+   OutPort<TimedPose3D> m_htlh_dbgOut;
+   TimedPose3D m_hthead_dbg;
+   OutPort<TimedPose3D> m_hthead_dbgOut;
+   TimedPoint3D m_htzmp_dbg;
+   OutPort<TimedPoint3D> m_htzmp_dbgOut;
+   TimedDoubleSeq m_htrfw_dbg;
+   OutPort<TimedDoubleSeq> m_htrfw_dbgOut;
+   TimedDoubleSeq m_htlfw_dbg;
+   OutPort<TimedDoubleSeq> m_htlfw_dbgOut;
+   TimedPose3D m_rpcom_dbg;
+   OutPort<TimedPose3D> m_rpcom_dbgOut;
+   TimedPose3D m_rprf_dbg;
+   OutPort<TimedPose3D> m_rprf_dbgOut;
+   TimedPose3D m_rplf_dbg;
+   OutPort<TimedPose3D> m_rplf_dbgOut;
+   TimedPose3D m_rprh_dbg;
+   OutPort<TimedPose3D> m_rprh_dbgOut;
+   TimedPose3D m_rplh_dbg;
+   OutPort<TimedPose3D> m_rplh_dbgOut;
+   TimedPose3D m_rphead_dbg;
+   OutPort<TimedPose3D> m_rphead_dbgOut;
+   TimedPoint3D m_rpzmp_dbg;
+   OutPort<TimedPoint3D> m_rpzmp_dbgOut;
+   TimedPoint3D m_rpdcp_dbg;
+   OutPort<TimedPoint3D> m_rpdcp_dbgOut;
+   TimedPoint3D m_rpacp_dbg;
+   OutPort<TimedPoint3D> m_rpacp_dbgOut;
+   TimedDoubleSeq m_invdyn_dbg;
+   OutPort<TimedDoubleSeq> m_invdyn_dbgOut;
+#endif
 
   OutPort<TimedDoubleSeq> m_qOut;
   RTC::OutPort<RTC::TimedPoint3D> m_zmpOut;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.cpp b/rtc/WholeBodyMasterSlave/wbms_core.cpp
index 06b86315967..8c3d60427f8 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.cpp
+++ b/rtc/WholeBodyMasterSlave/wbms_core.cpp
@@ -113,7 +113,6 @@ void HumanSynchronizer::applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vect
   com_old = comin;
 }
 void HumanSynchronizer::applyVelLimit(const HumanPose& in, HumanPose& out_old, HumanPose& out){
-  std::string ns[5] = {"com","rf","lf","rh","lh"};
   for(int i=com;i<=lh;i++){
     hrp::Vector3 diff = in.P[i].p  - out_old.P[i].p;
     for(int j=0;j<3;j++)LIMIT_MINMAX( diff(j), -MAXVEL*DT, MAXVEL*DT);
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 8d1235fb4bb..4afb2dd458c 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -47,26 +47,20 @@ extern "C" {
 #define DEBUG 0
 
 namespace myconst{
-//  const int X = 0, Y = 1, Z = 2, XYZ = 3;
-//  const int r = 0, p = 1, y = 2, rpy = 3;
-//  const int L = 0, R = 1, LR = 2;
-  const int MIN = 0, MAX = 1, MINMAX = 2;
 //  const double INFMIN = std::numeric_limits<double>::min(), INFMAX = std::numeric_limits<double>::max();
   const double INFMIN = - std::numeric_limits<double>::max(), INFMAX = std::numeric_limits<double>::max();
   const double D2R = M_PI/180.0;
 }
 using namespace myconst;
 
-
-//namespace wbms{
-//  enum pose_tgt{ com, rf, lf, rh, lh, zmp, head, num_pose_tgt } p_id;
-//  enum wrench_tgt{ rfw, lfw, num_wrench_tgt } w_id;
-//  enum lr_direction{ L, R, LR} lr_direc;
-//  enum xyz_direction{ X, Y, Z, XYZ} xyz_direc;
-//  enum rpy_direction{ r, p, y, rpy} rpy_direc;
-//  enum ft_direction{ fx, fy, fz, tx, ty, tz, ft_xyz} w_direc;
-//}
-//using namespace wbms;
+#define DEF_UTIL_CONST \
+    enum pose_tgt{ com, rf, lf, rh, lh, head, zmp, num_pose_tgt } p_id;\
+    enum wrench_tgt{ rfw, lfw, num_wrench_tgt } w_id;\
+    enum lr_direction{ L, R, LR} lr_direc;\
+    enum xyz_direction{ X, Y, Z, XYZ} xyz_direc;\
+    enum rpy_direction{ r, p, y, rpy} rpy_direc;\
+    enum ft_direction{ fx, fy, fz, tx, ty, tz, ft_xyz} w_direc;\
+    enum minmax_id{ MIN, MAX, MINMAX} mm_id;\
 
 class BiquadIIRFilterVec{
   private:
@@ -111,18 +105,7 @@ class Wrench6{
 };
 
 class HumanPose{
-
-
-
-
-    enum pose_tgt{ com, rf, lf, rh, lh, zmp, head, num_pose_tgt } p_id;
-    enum wrench_tgt{ rfw, lfw, num_wrench_tgt } w_id;
-    enum lr_direction{ L, R, LR} lr_direc;
-    enum xyz_direction{ X, Y, Z, XYZ} xyz_direc;
-    enum rpy_direction{ r, p, y, rpy} rpy_direc;
-    enum ft_direction{ fx, fy, fz, tx, ty, tz, ft_xyz} w_direc;
-
-
+    DEF_UTIL_CONST
   public:
     std::vector<HRPPose3D> P;
     std::vector<Wrench6> w;
@@ -146,38 +129,30 @@ class HumanPose{
 };
 
 class RobotConfig{
+    DEF_UTIL_CONST
   public:
-    std::map<std::string, std::vector<hrp::Vector3> > ee_rot_limit;
+    std::vector< std::vector<hrp::Vector3> > ee_rot_limit;
     RobotConfig(){
-      ee_rot_limit["com"].resize(MINMAX);
-      ee_rot_limit["com"][MIN] = hrp::Vector3(-30*D2R, -30*D2R, INFMIN);
-      ee_rot_limit["com"][MAX] = hrp::Vector3( 30*D2R,  30*D2R, INFMAX);
-      ee_rot_limit["rf"].resize(MINMAX);
-      ee_rot_limit["rf"][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -20*D2R);
-      ee_rot_limit["rf"][MAX] = hrp::Vector3( 30*D2R,  30*D2R,   5*D2R);
-      ee_rot_limit["lf"].resize(MINMAX);
-      ee_rot_limit["lf"][MIN] = hrp::Vector3(-30*D2R, -30*D2R,  -5*D2R);
-      ee_rot_limit["lf"][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  20*D2R);
-      ee_rot_limit["rh"].resize(MINMAX);
-      ee_rot_limit["rh"][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -30*D2R);
-      ee_rot_limit["rh"][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
-      ee_rot_limit["lh"].resize(MINMAX);
-      ee_rot_limit["lh"][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -30*D2R);
-      ee_rot_limit["lh"][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
+      std::vector<hrp::Vector3> init;
+      init.resize(MINMAX);
+      ee_rot_limit.resize(num_pose_tgt, init);
+      ee_rot_limit[com][MIN] = hrp::Vector3(-30*D2R, -30*D2R, INFMIN);
+      ee_rot_limit[com][MAX] = hrp::Vector3( 30*D2R,  30*D2R, INFMAX);
+      ee_rot_limit[rf][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -20*D2R);
+      ee_rot_limit[rf][MAX] = hrp::Vector3( 30*D2R,  30*D2R,   5*D2R);
+      ee_rot_limit[lf][MIN] = hrp::Vector3(-30*D2R, -30*D2R,  -5*D2R);
+      ee_rot_limit[lf][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  20*D2R);
+      ee_rot_limit[rh][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -30*D2R);
+      ee_rot_limit[rh][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
+      ee_rot_limit[lh][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -30*D2R);
+      ee_rot_limit[lh][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
+      ee_rot_limit[head][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -30*D2R);
+      ee_rot_limit[head][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
     }
 };
 
 class HumanSynchronizer{
-
-
-    enum pose_tgt{ com, rf, lf, rh, lh, zmp, head, num_pose_tgt } p_id;
-    enum wrench_tgt{ rfw, lfw, num_wrench_tgt } w_id;
-    enum lr_direction{ L, R, LR} lr_direc;
-    enum xyz_direction{ X, Y, Z, XYZ} xyz_direc;
-    enum rpy_direction{ r, p, y, rpy} rpy_direc;
-    enum ft_direction{ fx, fy, fz, tx, ty, tz, ft_xyz} w_direc;
-
-
+    DEF_UTIL_CONST
   private:
     double FUP_TIME;
     int cur_rfup_level,cur_lfup_level;
@@ -299,7 +274,7 @@ class HumanSynchronizer{
       tgt_pos_filters[1].setParameter(hrp::Vector3(1.0,1.0,1.2),HZ);//右足pos用
       tgt_pos_filters[2].setParameter(hrp::Vector3(1.0,1.0,1.2),HZ);//左足pos用
       calcacc_v_filters.setParameter(5,HZ);//加速度計算用
-      acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
+      acc4zmp_v_filters.setParameter(5,HZ);//ZMP生成用ほぼこの値でいい
       cam_rpy_filter.setParameter(1,HZ);//カメラアングル
       com_in_filter.setParameter(1,HZ);
 
@@ -400,14 +375,8 @@ class HumanSynchronizer{
       applyCOMToSupportRegionLimit        (rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, rp_ref_out.P[com].p);
       setFootRotHorizontalIfGoLanding     (rp_ref_out_old, rp_ref_out);
 
-//      applyLPFilter                       (rp_ref_out);
-//      applyCOMToSupportRegionLimit        (rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, rp_ref_out.P[com]);//もう一回？
-//      applyCOMStateLimitByCapturePoint    (rp_ref_out.P[com].p, rp_ref_out_old.P[com].p, com_vel_old, rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, rp_ref_out.P[com].p);
-//      applyCOMToSupportRegionLimit        (rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, rp_ref_out.P[com]);//もう一回？
-
       applyVelLimit                       (rp_ref_out, rp_old_ref_Vel, rp_ref_out);
 
-
       applyCOMToSupportRegionLimit        (rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
 
       Vector3ToVector2(rp_ref_out.P[com].p,com_forcp_ref);
@@ -638,11 +607,11 @@ class HumanSynchronizer{
       LIMIT_MINMAX( tgt.P[rf].p(Z), tgt.P[rf].p_offs(Z), tgt.P[rf].p_offs(Z)+WBMSparam.swing_foot_max_height);
       LIMIT_MINMAX( tgt.P[lf].p(Z), tgt.P[lf].p_offs(Z), tgt.P[lf].p_offs(Z)+WBMSparam.swing_foot_max_height);
 
-      std::string ns[5] = {"com","rf","lf","rh","lh"};
-      for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( tgt.P[com].rpy(i), rc.ee_rot_limit["com"][MIN](i), rc.ee_rot_limit["com"][MAX](i) ); }
+      for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( tgt.P[com].rpy(i), rc.ee_rot_limit[com][MIN](i), rc.ee_rot_limit[com][MAX](i) ); }
       LIMIT_MINMAX( tgt.P[com].p(Z), tgt.P[com].p_offs(Z) - 0.15, tgt.P[com].p_offs(Z) + 0.03 );//COM高さ方向の制限
-      for(int i=1;i<5;i++){
-        for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( tgt.P[i].rpy(j), rc.ee_rot_limit[ns[i]][MIN](j) + tgt.P[com].rpy(j), rc.ee_rot_limit[ns[i]][MAX](j) + tgt.P[com].rpy(j) ); }
+      const int ns[] = {rf,lf,rh,lh,head};
+      for(int i=0;i<sizeof(ns)/sizeof(int);i++){
+        for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( tgt.P[ns[i]].rpy(j), rc.ee_rot_limit[ns[i]][MIN](j) + tgt.P[com].rpy(j), rc.ee_rot_limit[ns[i]][MAX](j) + tgt.P[com].rpy(j) ); }
       }
       if(!WBMSparam.use_head)head_cam_pose.rpy = hrp::Vector3::Zero();
       if(!isHumanSyncOn())head_cam_pose.rpy = hrp::Vector3::Zero();
@@ -689,26 +658,23 @@ class HumanSynchronizer{
       hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
       com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
 
-      //減速CP条件
-      hrp::Vector2 cp_dec_tmp = com_pos + com_vel * sqrt( H_cur / G );
-      hrp::Vector2 cp_dec_ragulated;
-      if(!isPointInHullOpenCV(cp_dec_tmp,hull_d)){
-        calcCrossPointOnHull(com_pos, cp_dec_tmp, hull_d, cp_dec_ragulated);
-        //calcNearestPointOnHull(cp, hull, cp_ragulated);
-        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
-      }
+      //減速CP条件(現在のCPを常に両足裏で頭打ち)
+//      hrp::Vector2 cp_dec_tmp = com_pos + com_vel * sqrt( H_cur / G );
+//      hrp::Vector2 cp_dec_ragulated;
+//      if(!isPointInHullOpenCV(cp_dec_tmp,hull_d)){
+//        calcCrossPointOnHull(com_pos, cp_dec_tmp, hull_d, cp_dec_ragulated);
+//        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
+//      }
 
-      //減速CP条件2
-//      hrp::Vector2 cp_dec_ref = com_forcp_ref + com_vel * sqrt( H_cur / G );
+      //減速CP条件2(指令のCPに対する現在発揮可能なCP)
       hrp::Vector2 cp_dec_ref = com_forcp_ref + com_vel_forcp_ref * sqrt( H_cur / G );
       hrp::Vector2 cp_dec_ref_ragulated = cp_dec_ref;
       if(!isPointInHullOpenCV(cp_dec_ref,hull_d)){
         calcCrossPointOnHull(com_pos, cp_dec_ref, hull_d, cp_dec_ref_ragulated);
-//        calcNearestPointOnHull(cp, hull, cp_ragulated);
       }
       com_vel_decel_ok = (cp_dec_ref_ragulated - com_pos) / sqrt( H_cur / G );
 
-//      //加速CP条件
+//      //加速CP条件(ACP使用)
 //      hrp::Vector2 cp_acc_tmp = com_pos - com_vel * sqrt( H_cur / G );
 //      hrp::Vector2 cp_acc_ragulated;
 //      if(!isPointInHullOpenCV(cp_acc_tmp,hull_a)){
@@ -716,17 +682,7 @@ class HumanSynchronizer{
 //         com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
 //      }
 
-//      //加速CP条件2
-//      hrp::Vector2 com_acc = (com_vel - hrp::Vector2(com_vel_old(X),com_vel_old(Y))) / DT;
-//      hrp::Vector2 zmp_in = com_pos - com_acc / G * H_cur;
-//      hrp::Vector2 zmp_regulated;
-//      if(!isPointInHullOpenCV(zmp_in,hull_a)){
-//        calcCrossPointOnHull(com_pos, zmp_in, hull_a, zmp_regulated);
-//        hrp::Vector2 com_acc_regulated = (com_pos - zmp_regulated)*G/H_cur;
-//        com_vel_accel_ok = hrp::Vector2(com_vel_old(X),com_vel_old(Y)) + com_acc_regulated*DT;
-//      }
-
-      //加速CP条件2
+      //加速CP条件2(ZMP使用)
       static hrp::Vector2 com_vel_ans_old = com_vel;
       hrp::Vector2 com_acc = (com_vel - com_vel_ans_old) / DT;
       hrp::Vector2 zmp_in = com_pos - com_acc / G * H_cur;
@@ -738,14 +694,12 @@ class HumanSynchronizer{
       }
 
       //加速減速条件マージ
-      if(com_vel_decel_ok.norm() < com_vel_accel_ok.norm()){
+      if( com_vel_decel_ok.dot(com_vel) < com_vel_accel_ok.dot(com_vel)){//normじゃダメ？
         com_vel_ans = com_vel_decel_ok;
       }else{
         com_vel_ans = com_vel_accel_ok;
       }
-
       com_vel_ans_old = com_vel_ans;
-
     }
     void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
       std::vector<hrp::Vector2> points;

From f69f90a885abcc3804d62cf89a8468a799690ca0 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 7 May 2017 19:23:48 +0900
Subject: [PATCH 078/217] tune up for speed2

---
 .../WholeBodyMasterSlave.cpp                  |  37 ++---
 .../WholeBodyMasterSlave.h                    |   2 +-
 rtc/WholeBodyMasterSlave/wbms_core.cpp        |  28 ++--
 rtc/WholeBodyMasterSlave/wbms_core.h          | 153 ++++++++----------
 4 files changed, 97 insertions(+), 123 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 33115a606cf..3473464e1a3 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -251,7 +251,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
       is_legged_robot = false;
     }
 
-    hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer());
+    hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer(m_dt));
 
     return RTC::RTC_OK;
 }
@@ -279,8 +279,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
 {
   // std::cerr << "WholeBodyMasterSlave::onExecute(" << ec_id << ")" << std::endl;
     // Read Inport
- 
-
 
     if (m_qRefIn.isNew()) {
         m_qRefIn.read();
@@ -313,7 +311,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
     if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlf.data,hsp->hp_wld_raw.P[lf]); }
     if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrh.data,hsp->hp_wld_raw.P[rh]);}
     if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlh.data,hsp->hp_wld_raw.P[lh]);}
-    if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_hthead.data,hsp->head_cam_pose);}
+    if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_hthead.data,hsp->hp_wld_raw.P[head]);}
 //    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
 
 
@@ -328,8 +326,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
 //        q_interpolator_ratio = (control_mode == MODE_IDLE) ? 0.0 : 1.0;
 //      }
 
-
-        hsp->baselinkpose.p = m_robot->rootLink()->p;
+      hsp->baselinkpose.p = m_robot->rootLink()->p;
         hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
         if(!hsp->isHumanSyncOn()){
           m_robot->rootLink()->p = input_basePos;
@@ -340,12 +337,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
           m_robot->calcForwardKinematics();
         }
 
-
-
         processWholeBodyMasterSlave();
 
-
-
         /////// Inverse Dynamics /////////
         if(!idsb.is_initialized){
           idsb.setInitState(m_robot, m_dt);
@@ -379,12 +372,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
 //        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
     }
 
-
-
-
-
-
-
     // Write Outport
     if ( m_qRef.data.length() != 0 ) { // initialized
       if (is_legged_robot) {
@@ -484,9 +471,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
      HumanSynchronizer::Wrench6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
      m_invdyn_dbgOut.write();
 #endif
-
-
-
     loop ++;
     return RTC::RTC_OK;
 }
@@ -554,7 +538,7 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
 
   m_robot->calcForwardKinematics();
   if(hsp->isHumanSyncOn()){
-    solveFullbodyIKStrictCOM( hsp->rp_ref_out.P[com], hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->cam_rpy_filtered );
+    solveFullbodyIKStrictCOM( hsp->rp_ref_out.P[com], hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head] );
     //outport用のデータ上書き
     hsp->rp_ref_out.P[zmp].p(2) = (hsp->rp_ref_out.P[rf].p(2) + hsp->rp_ref_out.P[lf].p(2))/2;//体幹相対ZMP高さ設定
 
@@ -707,16 +691,16 @@ void WholeBodyMasterSlave::calcDynamicsFilterCompensation(const hrp::Vector3 zmp
     HRPPose3D com_mod = hsp->rp_ref_out.P[com];
 //    com_mod.p += torso_pos;
 //    com_mod.rpy += torso_rot;
-    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->cam_rpy_filtered );
+    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head]);
   }
 }
 
 
 
 
-void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const hrp::Vector3& head_ref){
+void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref){
   int com_ik_loop=0;
-  const int COM_IK_MAX_LOOP = 10;
+  const int COM_IK_MAX_LOOP = 5;
   const double COM_IK_MAX_ERROR = 1e-4;
   m_robot->rootLink()->p = com_ref.p;//move base link at first
   m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
@@ -730,8 +714,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, co
       fik->ikp[names[i]].target_p0 = refs[i]->p;
     }
   }
-  if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref(2);
-  if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref(1);
+  if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref.rpy(y);
+  if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref.rpy(p);
   //COM 収束ループ
   while(
       m_robot->calcForwardKinematics(),
@@ -754,6 +738,9 @@ bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double se
     std::cerr << "[" << m_profile.instance_name << "] start Synchronization after "<<sec<<" [s]" << std::endl;
     hsp->countdown_sec = sec;
     hsp->startCountdownForHumanSync = true;
+    while (!hsp->isHumanSyncOn()){
+        usleep(1000);
+    }
     return true;
   }else{
     std::cerr << "[" << m_profile.instance_name << "] Count Down Time must be 0 < T < 30 [s]"<< std::endl;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 9b428c8b4c4..b7f4eff5738 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -153,7 +153,7 @@ class WholeBodyMasterSlave
     hrp::Link* target_link;
     bool is_active, has_toe_joint;
   };
-  void solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const hrp::Vector3& head_ref);
+  void solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref);
   void processWholeBodyMasterSlave();
   void calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody);
   bool isOptionalDataContact (const std::string& ee_name)
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.cpp b/rtc/WholeBodyMasterSlave/wbms_core.cpp
index 8c3d60427f8..b48a023c92f 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.cpp
+++ b/rtc/WholeBodyMasterSlave/wbms_core.cpp
@@ -78,11 +78,12 @@ double HumanSynchronizer::calcNearestPointOnHull(const hrp::Vector2& tgt_pt, con
   return ans_nearest_dist;
 }
 void HumanSynchronizer::makeConvexHullOpenCV(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
-  std::vector<cv::Point2f> points,hull;
-  for(int i=0;i<pts.size();i++){ points.push_back( cv::Point2f( pts[i](0), pts[i](1)) ); }
-  cv::convexHull(cv::Mat(points),hull,true);
+  points.clear();
+  cvhull.clear();
   hull_ans.clear();
-  for(int i=0;i<hull.size();i++){ hull_ans.push_back( hrp::Vector2( hull[i].x, hull[i].y ) );  }
+  for(int i=0;i<pts.size();i++){ points.push_back( cv::Point2f( pts[i](0), pts[i](1)) ); }
+  cv::convexHull(cv::Mat(points),cvhull,true);
+  for(int i=0;i<cvhull.size();i++){ hull_ans.push_back( hrp::Vector2( cvhull[i].x, cvhull[i].y ) );  }
 }
 //void HumanSynchronizer::makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
 //  const int dim = 2;
@@ -96,7 +97,7 @@ void HumanSynchronizer::makeConvexHullOpenCV(const std::vector<hrp::Vector2>& pt
 //  //#define qh_ORIENTclock 1
 //}
 bool HumanSynchronizer::isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull){
-  std::vector<cv::Point2f> cvhull;
+  cvhull.clear();
   for(int i=0;i<hull.size();i++){ cvhull.push_back( cv::Point2f( hull[i](0), hull[i](1)) ); }
   return (cv::pointPolygonTest(cv::Mat(cvhull), cv::Point2f(pt(0),pt(1)), false) > 0);
 }
@@ -112,11 +113,12 @@ void HumanSynchronizer::applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vect
   com_oldold = com_old;
   com_old = comin;
 }
-void HumanSynchronizer::applyVelLimit(const HumanPose& in, HumanPose& out_old, HumanPose& out){
-  for(int i=com;i<=lh;i++){
-    hrp::Vector3 diff = in.P[i].p  - out_old.P[i].p;
-    for(int j=0;j<3;j++)LIMIT_MINMAX( diff(j), -MAXVEL*DT, MAXVEL*DT);
-    out.P[i].p = out_old.P[i].p + diff;
-  }
-  out_old = out;
-}
+//void HumanSynchronizer::applyVelLimit(const HumanPose& in, HumanPose& out_old, HumanPose& out){
+//  const int ns[] = {com,rf,lf,rh,lh,head}, num_ns = sizeof(ns)/sizeof(int);
+//  for(int i=0;i<num_ns;i++){
+//    hrp::Vector3 diff = in.P[i].p  - out_old.P[i].p;
+//    for(int j=0;j<3;j++)LIMIT_MINMAX( diff(j), -MAXVEL*DT, MAXVEL*DT);
+//    out.P[i].p = out_old.P[i].p + diff;
+//  }
+//  out_old = out;
+//}
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 4afb2dd458c..b924ee677b3 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -54,29 +54,30 @@ namespace myconst{
 using namespace myconst;
 
 #define DEF_UTIL_CONST \
-    enum pose_tgt{ com, rf, lf, rh, lh, head, zmp, num_pose_tgt } p_id;\
-    enum wrench_tgt{ rfw, lfw, num_wrench_tgt } w_id;\
-    enum lr_direction{ L, R, LR} lr_direc;\
-    enum xyz_direction{ X, Y, Z, XYZ} xyz_direc;\
-    enum rpy_direction{ r, p, y, rpy} rpy_direc;\
-    enum ft_direction{ fx, fy, fz, tx, ty, tz, ft_xyz} w_direc;\
-    enum minmax_id{ MIN, MAX, MINMAX} mm_id;\
+    static const enum pose_tgt{ com, rf, lf, rh, lh, head, zmp, num_pose_tgt } p_id;\
+    static const enum wrench_tgt{ rfw, lfw, num_wrench_tgt } w_id;\
+    static const enum lr_direction{ L, R, LR} lr_direc;\
+    static const enum xyz_direction{ X, Y, Z, XYZ} xyz_direc;\
+    static const enum rpy_direction{ r, p, y, rpy} rpy_direc;\
+    static const enum ft_direction{ fx, fy, fz, tx, ty, tz, ft_xyz} w_direc;\
+    static const enum minmax_id{ MIN, MAX, MINMAX} mm_id;\
 
 class BiquadIIRFilterVec{
   private:
     IIRFilter filters[3];
     hrp::Vector3 ans;
   public:
-    BiquadIIRFilterVec(const std::string& error_prefix = ""){
+    static const double Q_BUTTERWORTH = 0.707106781, Q_NOOVERSHOOT = 0.5;
+    BiquadIIRFilterVec(){
     };
     ~BiquadIIRFilterVec() {};
-    void setParameter(const hrp::Vector3 fc_in, const double HZ){
+    void setParameter(const hrp::Vector3& fc_in, const double& HZ, const double& Q = Q_BUTTERWORTH){
      for(int i=0;i<3;i++){
-         filters[i].setParameterAsBiquad((double)fc_in(i), 0.5, HZ);
+         filters[i].setParameterAsBiquad((double)fc_in(i), Q, HZ);
       }
     };
-    void setParameter(const double fc_in, const double HZ){
-      setParameter(hrp::Vector3(fc_in,fc_in,fc_in), HZ);
+    void setParameter(const double& fc_in, const double& HZ, const double& Q = Q_BUTTERWORTH){//overload
+      setParameter(hrp::Vector3(fc_in,fc_in,fc_in), HZ, Q);
     };
     hrp::Vector3 passFilter(const hrp::Vector3& input){
       for(int i=0;i<3;i++){ ans(i) = filters[i].passFilter((double)input(i)); }
@@ -146,8 +147,8 @@ class RobotConfig{
       ee_rot_limit[rh][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
       ee_rot_limit[lh][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -30*D2R);
       ee_rot_limit[lh][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
-      ee_rot_limit[head][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -30*D2R);
-      ee_rot_limit[head][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
+      ee_rot_limit[head][MIN] = hrp::Vector3( 0*D2R, -10*D2R, -40*D2R);
+      ee_rot_limit[head][MAX] = hrp::Vector3( 0*D2R,  20*D2R,  40*D2R);
     }
 };
 
@@ -171,11 +172,13 @@ class HumanSynchronizer{
     hrp::Vector3 com_CP_ref_old;
     std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rot_filters;
     BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters;
-    BiquadIIRFilterVec cam_rpy_filter;
     BiquadIIRFilterVec com_in_filter;
     hrp::Vector3 r_zmp_raw;
     hrp::Vector4 rf_safe_region,lf_safe_region;//前後左右の順
     RobotConfig rc;
+    std::vector<hrp::Vector2> rflf_points;
+    std::vector<hrp::Vector2> hull_dcp,hull_acp;
+    std::vector<cv::Point2f> points,cvhull;
 
   public:
     double tgt_FUP_TIME;
@@ -195,8 +198,6 @@ class HumanSynchronizer{
     HRPPose3D baselinkpose;
     hrp::Vector3 com_vel_old;
     unsigned int loop;
-    hrp::Vector3 cam_pos_filtered,cam_rpy_filtered;
-    HRPPose3D head_cam_pose;
     hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos, pre_cont_rfrot,pre_cont_lfrot;
     std::vector<hrp::Vector3> rf_vert,lf_vert;
     double countdown_sec;
@@ -221,11 +222,11 @@ class HumanSynchronizer{
 
     double H_cur;
 
-    HumanSynchronizer(){
+    HumanSynchronizer(const double& dt){
       tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
 //      tgt_h2r_ratio = h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
-      HZ = 500;
-      DT = 1.0/HZ;
+      DT = dt;
+      HZ = (int)(1.0/DT);
       G = 9.80665;
       use_x = use_y = use_z = true;
       cur_rfup_level = cur_lfup_level = 0;
@@ -253,11 +254,10 @@ class HumanSynchronizer{
       init_wld_hp_rfpos = init_wld_hp_lfpos = hrp::Vector3::Zero();
 
       com_vel_old = hrp::Vector3::Zero();
-      cam_pos_filtered = cam_rpy_filtered = hrp::Vector3::Zero();
       cp_dec = cp_acc = hrp::Vector3::Zero();
 
-      tgt_pos_filters.resize(5);
-      tgt_rot_filters.resize(5);
+      tgt_pos_filters.resize(num_pose_tgt);
+      tgt_rot_filters.resize(num_pose_tgt);
 //      for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(0.6,HZ);//四肢拘束点用(position)
 //      for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0,HZ);//四肢拘束点用(Rotation)
 //      tgt_pos_filters[0].setParameter(0.6,HZ);//重心pos用
@@ -267,16 +267,15 @@ class HumanSynchronizer{
 //      calcacc_v_filters.setParameter(5,HZ);//加速度計算用
 //      acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
 //      cam_rpy_filter.setParameter(1,HZ);//カメラアングル
-      for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(1.0,HZ);//四肢拘束点用(position)
-      for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0,HZ);//四肢拘束点用(Rotation)
-      tgt_pos_filters[0].setParameter(1.0,HZ);//重心pos用
-      tgt_rot_filters[0].setParameter(0.6,HZ);//重心rot用
-      tgt_pos_filters[1].setParameter(hrp::Vector3(1.0,1.0,1.2),HZ);//右足pos用
-      tgt_pos_filters[2].setParameter(hrp::Vector3(1.0,1.0,1.2),HZ);//左足pos用
-      calcacc_v_filters.setParameter(5,HZ);//加速度計算用
-      acc4zmp_v_filters.setParameter(5,HZ);//ZMP生成用ほぼこの値でいい
-      cam_rpy_filter.setParameter(1,HZ);//カメラアングル
-      com_in_filter.setParameter(1,HZ);
+      for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//四肢拘束点用(position)
+      for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//四肢拘束点用(Rotation)
+      tgt_pos_filters[0].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//重心pos用
+      tgt_rot_filters[0].setParameter(0.6, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//重心rot用
+      tgt_pos_filters[1].setParameter(hrp::Vector3(1.0,1.0,1.2), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//右足pos用
+      tgt_pos_filters[2].setParameter(hrp::Vector3(1.0,1.0,1.2), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//左足pos用
+      calcacc_v_filters.setParameter(5, HZ, BiquadIIRFilterVec::Q_BUTTERWORTH);//加速度計算用
+      acc4zmp_v_filters.setParameter(5, HZ, BiquadIIRFilterVec::Q_BUTTERWORTH);//ZMP生成用ほぼこの値でいい
+      com_in_filter.setParameter(1, HZ);
 
       rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
       lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
@@ -304,6 +303,12 @@ class HumanSynchronizer{
       rp_ref_out_old.clear();
       rp_ref_out.clear();
 
+      rflf_points.reserve(8);//あらかじめreserveして高速化
+      hull_dcp.reserve(6);
+      hull_dcp.reserve(6);
+      cvhull.reserve(6);
+      points.reserve(8);
+
 
       init_zmp_com_offset_interpolator = new interpolator(1, DT, interpolator::HOFFARBIB, 1);
       init_zmp_com_offset_interpolator->setName("zmp_com_offset_interpolator");
@@ -328,7 +333,7 @@ class HumanSynchronizer{
     }
     const double getRemainingCountDown() const { return countdown_sec; }
     const bool isHumanSyncOn() const { return HumanSyncOn; }
-    const double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0; }
+    const double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6; }
     void setTargetHumanToRobotRatio(const double tgt_h2r_r_in){ tgt_h2r_ratio = tgt_h2r_r_in; }
     void setCurrentInputAsOffset(HumanPose& tgt){
       std::string ns[7] = {"com","rf","lf","rh","lh","zmp","head"};
@@ -350,16 +355,11 @@ class HumanSynchronizer{
       updateParams                        ();
 //      applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);//怪しいので要修正
 //      calcWorldZMP                        ((hp_rel_raw.P[rf].p+init_wld_hp_rfpos), (hp_rel_raw.P[lf].p+init_wld_hp_lfpos), hp_rel_raw.w[rfw], hp_rel_raw.w[lfw], hp_rel_raw.P[zmp].p);//足の位置はworldにしないと・・・
-      autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);
-
-//      hp_swap_checked.P[com].p = com_in_filter.passFilter(hp_swap_checked.P[com].p);
-
+      autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);//入力の左右反転を常にチェック(＝手足の交差は不可能)
       convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
       hp_plot = rp_ref_out;
       if(loop==0)rp_ref_out_old = rp_ref_out;
 
-//      rp_ref_out.P[com].p += torso_rot;
-
       if(isHumanSyncOn() && WBMSparam.set_com_height_fix)rp_ref_out.P[com].p(Z) = rp_ref_out.P[com].p_offs(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
       judgeFootLandOnCommand              (hp_wld_raw.w[rfw], hp_wld_raw.w[lfw], go_rf_landing, go_lf_landing);
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
@@ -375,7 +375,7 @@ class HumanSynchronizer{
       applyCOMToSupportRegionLimit        (rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, rp_ref_out.P[com].p);
       setFootRotHorizontalIfGoLanding     (rp_ref_out_old, rp_ref_out);
 
-      applyVelLimit                       (rp_ref_out, rp_old_ref_Vel, rp_ref_out);
+//      applyVelLimit                       (rp_ref_out, rp_old_ref_Vel, rp_ref_out);
 
       applyCOMToSupportRegionLimit        (rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
 
@@ -388,7 +388,6 @@ class HumanSynchronizer{
       applyCOMToSupportRegionLimit        (rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, rp_ref_out.P[com].p);
       applyLPFilter                       (rp_ref_out);
 
-      cam_rpy_filtered = cam_rpy_filter.passFilter(head_cam_pose.rpy);
       r_zmp_raw = rp_ref_out.P[zmp].p;
       applyZMPCalcFromCOM                 (rp_ref_out.P[com].p,rp_ref_out.P[zmp].p);
 
@@ -396,7 +395,6 @@ class HumanSynchronizer{
       if(DEBUG)fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.P[zmp].p(X),rp_ref_out.P[zmp].p(Y));
       if(DEBUG)fprintf(cz_log,"com_ans2: %f %f ",rp_ref_out.P[com].p(X),rp_ref_out.P[com].p(Y));
 
-//      applyVelLimit                       (rp_ref_out, rp_ref_out_old, rp_ref_out);
       overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
       modifyFootRotAndXYForContact        (rp_ref_out.P[rf], rp_ref_out.P[lf]);
 
@@ -467,21 +465,22 @@ class HumanSynchronizer{
       out.P[com].p = in.P[com].p + zmp_com_offset_ip_ratio * com_offs;
     }
     void applyLPFilter(HumanPose& tgt){
-      std::string ns[5] = {"com","rf","lf","rh","lh"};
+      const int ns[] = {com,rf,lf,rh,lh,head}, num_ns = sizeof(ns)/sizeof(int);
       if(loop==0){
         cout<<"init LPF"<<endl;
-        for(int i=0;i<tgt_pos_filters.size();i++){ tgt_pos_filters[i].init(tgt.P[i].p); tgt_pos_filters[i].passFilter(tgt.P[i].p);}
-        for(int i=0;i<tgt_rot_filters.size();i++){ tgt_rot_filters[i].init(tgt.P[i].rpy); tgt_pos_filters[i].passFilter(tgt.P[i].rpy);}
+        for(int i=0;i<num_ns;i++){ tgt_pos_filters[ns[i]].init(tgt.P[ns[i]].p);   tgt_pos_filters[ns[i]].passFilter(tgt.P[ns[i]].p);  }
+        for(int i=0;i<num_ns;i++){ tgt_rot_filters[ns[i]].init(tgt.P[ns[i]].rpy); tgt_pos_filters[ns[i]].passFilter(tgt.P[ns[i]].rpy);}
       }
-      for(int i=0;i<tgt_pos_filters.size();i++){ tgt.P[i].p = tgt_pos_filters[i].passFilter(tgt.P[i].p); }
-      for(int i=0;i<tgt_rot_filters.size();i++){ tgt.P[i].rpy = tgt_rot_filters[i].passFilter(tgt.P[i].rpy); }
+      for(int i=0;i<num_ns;i++){ tgt.P[ns[i]].p   = tgt_pos_filters[ns[i]].passFilter(tgt.P[ns[i]].p);  }
+      for(int i=0;i<num_ns;i++){ tgt.P[ns[i]].rpy = tgt_rot_filters[ns[i]].passFilter(tgt.P[ns[i]].rpy);}
     }
     void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
-      std::string ns[6] = {"com","rf","lf","rh","lh","zmp"};
-      for(int i=0;i<6;i++){
-        rp_out.P[i].p   =  h2r_ratio * (hp_in.P[i].p - hp_in.P[i].p_offs)   + rp_out.P[i].p_offs;
-        rp_out.P[i].rpy =  hrp::rpyFromRot( hrp::rotFromRpy(hp_in.P[i].rpy) * hrp::rotFromRpy(hp_in.P[i].rpy_offs).transpose() * hrp::rotFromRpy(rp_out.P[i].rpy_offs) );
+      const int ns[] = {com,rf,lf,rh,lh,zmp}, num_ns = sizeof(ns)/sizeof(int);
+      for(int i=0;i<num_ns;i++){
+        rp_out.P[ns[i]].p   =  h2r_ratio * (hp_in.P[ns[i]].p - hp_in.P[ns[i]].p_offs)   + rp_out.P[ns[i]].p_offs;
+        rp_out.P[ns[i]].rpy =  hrp::rpyFromRot( hrp::rotFromRpy(hp_in.P[ns[i]].rpy) * hrp::rotFromRpy(hp_in.P[ns[i]].rpy_offs).transpose() * hrp::rotFromRpy(rp_out.P[ns[i]].rpy_offs) );
       }
+      rp_out.P[head] = hp_in.P[head];
       rp_out.w[rfw]  = hp_in.w[rfw];
       rp_out.w[lfw]  = hp_in.w[lfw];
     }
@@ -502,8 +501,8 @@ class HumanSynchronizer{
       if( inside_vec_lf.dot(lf2zmp) / inside_vec_lf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ rfcs_ans = true; }
     }
     void setFootRotHorizontalIfGoLanding(const HumanPose& rp_ref_out_old, HumanPose& rp_ref_out){
-      if(go_rf_landing){ rp_ref_out.P[rf].rpy(X) = rp_ref_out.P[rf].rpy(Y) = 0; }
-      if(go_lf_landing){ rp_ref_out.P[lf].rpy(X) = rp_ref_out.P[lf].rpy(Y) = 0; }
+      if(go_rf_landing){ rp_ref_out.P[rf].rpy(r) = rp_ref_out.P[rf].rpy(p) = 0; }
+      if(go_lf_landing){ rp_ref_out.P[lf].rpy(r) = rp_ref_out.P[lf].rpy(p) = 0; }
     }
     void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
       const double com2rf_dist = fabs((double)tgt.P[rf].p(Y) - (double)tgt.P[com].p(Y));
@@ -550,20 +549,20 @@ class HumanSynchronizer{
       if(rfcs_in){
         tgt.P[rf].p(X) = pre_cont_rfpos(X);
         tgt.P[rf].p(Y) = pre_cont_rfpos(Y);
-        tgt.P[rf].rpy(Z) = pre_cont_rfrot(Z);
+        tgt.P[rf].rpy(y) = pre_cont_rfrot(Z);
       }else{
         pre_cont_rfpos(X) = tgt.P[rf].p(X);
         pre_cont_rfpos(Y) = tgt.P[rf].p(Y);
-        pre_cont_rfrot(Z) = tgt.P[rf].rpy(Z);
+        pre_cont_rfrot(Z) = tgt.P[rf].rpy(y);
       }
       if(lfcs_in){
         tgt.P[lf].p(X) = pre_cont_lfpos(X);
         tgt.P[lf].p(Y) = pre_cont_lfpos(Y);
-        tgt.P[lf].rpy(Z) = pre_cont_lfrot(Z);
+        tgt.P[lf].rpy(y) = pre_cont_lfrot(Z);
       }else{
         pre_cont_lfpos(X) = tgt.P[lf].p(X);
         pre_cont_lfpos(Y) = tgt.P[lf].p(Y);
-        pre_cont_lfrot(Z) = tgt.P[lf].rpy(Z);
+        pre_cont_lfrot(Z) = tgt.P[lf].rpy(y);
       }
     }
     void applyEEWorkspaceLimit(HumanPose& tgt){
@@ -613,16 +612,14 @@ class HumanSynchronizer{
       for(int i=0;i<sizeof(ns)/sizeof(int);i++){
         for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( tgt.P[ns[i]].rpy(j), rc.ee_rot_limit[ns[i]][MIN](j) + tgt.P[com].rpy(j), rc.ee_rot_limit[ns[i]][MAX](j) + tgt.P[com].rpy(j) ); }
       }
-      if(!WBMSparam.use_head)head_cam_pose.rpy = hrp::Vector3::Zero();
-      if(!isHumanSyncOn())head_cam_pose.rpy = hrp::Vector3::Zero();
-      LIMIT_MINMAX( head_cam_pose.rpy(Y), -10*M_PI/180, 20*M_PI/180 );
-      LIMIT_MINMAX( head_cam_pose.rpy(Z), -40*M_PI/180, 40*M_PI/180 );
+      if(!WBMSparam.use_head)tgt.P[head].rpy = hrp::Vector3::Zero();
+      if(!isHumanSyncOn())tgt.P[head].rpy = hrp::Vector3::Zero();
     }
     bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
-      std::vector<hrp::Vector2> hull;
-      createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull);
+      std::vector<hrp::Vector2> hull_com;
+      createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull_com);
       hrp::Vector2 cog(comin_abs(X),comin_abs(Y));
-      if(!isPointInHullOpenCV(cog,hull)){ calcNearestPointOnHull(cog,hull,cog); }//外に出たら最近傍点に頭打ち
+      if(!isPointInHullOpenCV(cog,hull_com)){ calcNearestPointOnHull(cog,hull_com,cog); }//外に出たら最近傍点に頭打ち
       comin_abs(X) = cog(X);
       comin_abs(Y) = cog(Y);
       return true;
@@ -631,7 +628,8 @@ class HumanSynchronizer{
       com_ans = com_in;
       if (loop==0)com_ans_old = com_in;
       hrp::Vector3 com_vel = (com_in - com_ans_old)/DT;
-      std::vector<hrp::Vector2> hull_dcp,hull_acp;
+      hull_dcp.clear();
+      hull_acp.clear();
       hrp::Vector4 marginDelta_for_dcp(+0.001,-0.001,0.001,-0.001);
 //      hrp::Vector4 marginDelta_for_acp(+0.005,-0.005,0.005,-0.005);
       hrp::Vector4 marginDelta_for_acp(+0.01,-0.01,0.01,-0.01);
@@ -702,27 +700,14 @@ class HumanSynchronizer{
       com_vel_ans_old = com_vel_ans;
     }
     void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
-      std::vector<hrp::Vector2> points;
+      rflf_points.clear();
       for(int i=0;i<2;i++){
         for(int j=2;j<4;j++){
-          points.push_back(hrp::Vector2(rfin_abs(X) + rf_mgn(i),    rfin_abs(Y) + rf_mgn(j)));
-          points.push_back(hrp::Vector2(lfin_abs(X) + lf_mgn(i),    lfin_abs(Y) + lf_mgn(j)));
+          rflf_points.push_back(hrp::Vector2(rfin_abs(X) + rf_mgn(i),    rfin_abs(Y) + rf_mgn(j)));
+          rflf_points.push_back(hrp::Vector2(lfin_abs(X) + lf_mgn(i),    lfin_abs(Y) + lf_mgn(j)));
         }
       }
-//      static unsigned int edge_time = 0;
-//      if(rp_ref_out_old.P[rf].rpy(Y)>0.001){
-//        points.clear();
-//        double mod = rfin_abs(X) + 0.13 * edge_time * 0.002;
-////        LIMIT_MAX( mod, rfin_abs(X) + 0.13 );
-//        for(int i=0;i<2;i++){
-//          for(int j=2;j<4;j++){
-//            points.push_back(hrp::Vector2(rfin_abs(X) + 0.13,    rfin_abs(Y) + rf_mgn(j)));
-//            points.push_back(hrp::Vector2(lfin_abs(X) + lf_mgn(i),    lfin_abs(Y) + lf_mgn(j)));
-//          }
-//        }
-//        edge_time++;
-//      }
-      makeConvexHullOpenCV(points, hull_ans);
+      makeConvexHullOpenCV(rflf_points, hull_ans);
     }
     void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const Wrench6& rfwin, const Wrench6& lfwin, hrp::Vector3& zmp_ans);
     void calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans);
@@ -732,7 +717,7 @@ class HumanSynchronizer{
     void makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans);
     bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull);
     void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout);
-    void applyVelLimit(const HumanPose& in, HumanPose& out_old, HumanPose& out);
+//    void applyVelLimit(const HumanPose& in, HumanPose& out_old, HumanPose& out);
     void applyCOMZMPXYZLock(HumanPose& tgt);
 };
 

From 9ff46e746c07135433fe6dfe521cd27e65663fff Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 7 May 2017 22:19:18 +0900
Subject: [PATCH 079/217] fi unstrict com IK

---
 .../WholeBodyMasterSlave.cpp                  | 109 ++++++++++++------
 rtc/WholeBodyMasterSlave/wbms_core.cpp        |   4 -
 rtc/WholeBodyMasterSlave/wbms_core.h          |   8 +-
 3 files changed, 76 insertions(+), 45 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 3473464e1a3..d5690093d89 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -483,10 +483,8 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
   fik->current_tm = m_qRef.tm;
   fik->ikp["rleg"].is_ik_enable = true;
   fik->ikp["lleg"].is_ik_enable = true;
-  // fik->ikp["rarm"].is_ik_enable = true;
-  // fik->ikp["larm"].is_ik_enable = true;
-  fik->ikp["rarm"].is_ik_enable = false;
-  fik->ikp["larm"].is_ik_enable = false;
+   fik->ikp["rarm"].is_ik_enable = true;
+   fik->ikp["larm"].is_ik_enable = true;
 
   if(hsp->startCountdownForHumanSync){
     std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
@@ -554,7 +552,76 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
     rel_ref_zmp = input_ref_zmp;
   }
 }
+
+//void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref){
+//  int com_ik_loop=0;
+//  const int COM_IK_MAX_LOOP = 5;
+//  const double COM_IK_MAX_ERROR = 1e-4;
+//  m_robot->rootLink()->p = com_ref.p;//move base link at first
+//  m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
 //
+//  hrp::Vector3 tmp_com_err;
+//  const std::string names[4] = {"rleg","lleg","rarm","larm"};
+//  const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
+//  for(int i=0;i<4;i++){
+//    if(fik->ikp.count(names[i])){
+//      fik->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);//convert End Effector pos into link origin pos
+//      fik->ikp[names[i]].target_p0 = refs[i]->p;
+//    }
+//  }
+//  if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref.rpy(y);
+//  if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref.rpy(p);
+//  //COM 収束ループ
+//  while(
+//      m_robot->calcForwardKinematics(),
+//      tmp_com_err = com_ref.p - m_robot->calcCM(),
+//      tmp_com_err.norm() > COM_IK_MAX_ERROR,
+//      com_ik_loop++)
+//  {
+//    m_robot->rootLink()->p += tmp_com_err;
+//    m_robot->calcForwardKinematics();
+//    for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
+//        if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
+//    }
+//    if(com_ik_loop > COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
+//  }
+//  cout<<"COM_IK_MAX_LOOP:"<<com_ik_loop<<endl;
+//}
+
+void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref){
+  int com_ik_loop=0;
+  const int COM_IK_MAX_LOOP = 5;
+  const double COM_IK_MAX_ERROR = 1e-4;
+  m_robot->rootLink()->p = com_ref.p;//move base link at first
+  m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
+
+  const std::string names[4] = {"rleg","lleg","rarm","larm"};
+  const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
+  for(int i=0;i<4;i++){
+    if(fik->ikp.count(names[i])){
+      fik->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);//convert End Effector pos into link origin pos
+      fik->ikp[names[i]].target_p0 = refs[i]->p;
+    }
+  }
+  if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref.rpy(y);
+  if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref.rpy(p);
+  //COM 収束ループ
+  hrp::Vector3 tmp_com_err;
+  while( 1 ){
+    com_ik_loop++;
+    m_robot->calcForwardKinematics();
+    for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
+        if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
+    }
+    tmp_com_err = com_ref.p - m_robot->calcCM();
+    cout<<"tmp_com_err:"<<tmp_com_err.norm()<<endl;
+    if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
+    m_robot->rootLink()->p += tmp_com_err;
+    if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
+  }
+  cout<<"COM_IK_MAX_LOOP:"<<com_ik_loop<<endl;
+}
+
 //
 //hrp::Vector3 torso_pos = hrp::Vector3::Zero();
 //hrp::Vector3 torso_rot = hrp::Vector3::Zero();
@@ -698,40 +765,6 @@ void WholeBodyMasterSlave::calcDynamicsFilterCompensation(const hrp::Vector3 zmp
 
 
 
-void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref){
-  int com_ik_loop=0;
-  const int COM_IK_MAX_LOOP = 5;
-  const double COM_IK_MAX_ERROR = 1e-4;
-  m_robot->rootLink()->p = com_ref.p;//move base link at first
-  m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
-
-  hrp::Vector3 tmp_com_err;
-  const std::string names[4] = {"rleg","lleg","rarm","larm"};
-  const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
-  for(int i=0;i<4;i++){
-    if(fik->ikp.count(names[i])){
-      fik->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);//convert End Effector pos into link origin pos
-      fik->ikp[names[i]].target_p0 = refs[i]->p;
-    }
-  }
-  if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref.rpy(y);
-  if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref.rpy(p);
-  //COM 収束ループ
-  while(
-      m_robot->calcForwardKinematics(),
-      tmp_com_err = com_ref.p - m_robot->calcCM(),
-      tmp_com_err.norm() > COM_IK_MAX_ERROR)
-  {
-    m_robot->rootLink()->p += tmp_com_err;
-    m_robot->calcForwardKinematics();
-    for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-        if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
-    }
-    if(com_ik_loop++ > COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
-  }
-//  cout<<"COM_IK_MAX_LOOP:"<<com_ik_loop<<endl;
-}
-
 bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double sec)
 {
   if(sec >= 0.0 && sec <= 30.0){
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.cpp b/rtc/WholeBodyMasterSlave/wbms_core.cpp
index b48a023c92f..cb9fd44fc9c 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.cpp
+++ b/rtc/WholeBodyMasterSlave/wbms_core.cpp
@@ -103,13 +103,9 @@ bool HumanSynchronizer::isPointInHullOpenCV(const hrp::Vector2& pt, const std::v
 }
 void HumanSynchronizer::applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
   comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
-  const double MAXACC = 5;
-  LIMIT_MINMAX( comacc(0), -MAXACC, MAXACC);
-  LIMIT_MINMAX( comacc(1), -MAXACC, MAXACC);
   comacc = acc4zmp_v_filters.passFilter(comacc);
   zmpout(0) = comin(0)-(H_cur/G)*comacc(0);
   zmpout(1) = comin(1)-(H_cur/G)*comacc(1);
-//  if(DEBUG)fprintf(cz_log,"%f %f %f %f %f %f %f\n",(double)loop/HZ,comin(0),comin(1),rp_ref_out.getP("zmp").p(0),rp_ref_out.getP("zmp").p(1),zmpout(0),zmpout(1));
   com_oldold = com_old;
   com_old = comin;
 }
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index b924ee677b3..76650b68c5c 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -177,7 +177,7 @@ class HumanSynchronizer{
     hrp::Vector4 rf_safe_region,lf_safe_region;//前後左右の順
     RobotConfig rc;
     std::vector<hrp::Vector2> rflf_points;
-    std::vector<hrp::Vector2> hull_dcp,hull_acp;
+    std::vector<hrp::Vector2> hull_com,hull_dcp,hull_acp;
     std::vector<cv::Point2f> points,cvhull;
 
   public:
@@ -304,10 +304,11 @@ class HumanSynchronizer{
       rp_ref_out.clear();
 
       rflf_points.reserve(8);//あらかじめreserveして高速化
+      points.reserve(8);
+      hull_com.reserve(6);
       hull_dcp.reserve(6);
       hull_dcp.reserve(6);
       cvhull.reserve(6);
-      points.reserve(8);
 
 
       init_zmp_com_offset_interpolator = new interpolator(1, DT, interpolator::HOFFARBIB, 1);
@@ -616,7 +617,7 @@ class HumanSynchronizer{
       if(!isHumanSyncOn())tgt.P[head].rpy = hrp::Vector3::Zero();
     }
     bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
-      std::vector<hrp::Vector2> hull_com;
+      hull_com.clear();
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull_com);
       hrp::Vector2 cog(comin_abs(X),comin_abs(Y));
       if(!isPointInHullOpenCV(cog,hull_com)){ calcNearestPointOnHull(cog,hull_com,cog); }//外に出たら最近傍点に頭打ち
@@ -701,6 +702,7 @@ class HumanSynchronizer{
     }
     void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
       rflf_points.clear();
+      hull_ans.clear();
       for(int i=0;i<2;i++){
         for(int j=2;j<4;j++){
           rflf_points.push_back(hrp::Vector2(rfin_abs(X) + rf_mgn(i),    rfin_abs(Y) + rf_mgn(j)));

From 470d02c426b248f1d1372415cdf0750e40164dde Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 10 May 2017 23:16:28 +0900
Subject: [PATCH 080/217] add raw mode

---
 idl/WholeBodyMasterSlaveService.idl           |   9 +-
 .../WholeBodyMasterSlave.cpp                  | 264 ++++++++++--------
 .../WholeBodyMasterSlave.h                    |   3 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 118 +-------
 4 files changed, 154 insertions(+), 240 deletions(-)

diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
index 3e26e90cb8d..4285a1ec62f 100644
--- a/idl/WholeBodyMasterSlaveService.idl
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -36,15 +36,16 @@ module OpenHRP
      */
     struct WholeBodyMasterSlaveParam
     {
-        double human_com_height;
+        double auto_swing_foot_landing_threshold;
         double foot_vertical_vel_limit_coeff;
+        double human_com_height;
+        boolean is_doctor;
         boolean set_com_height_fix;
         double set_com_height_fix_val;
-        boolean use_hands;
-        boolean use_head;
         double swing_foot_height_offset;
         double swing_foot_max_height;
-        double auto_swing_foot_landing_threshold;
+        boolean use_hands;
+        boolean use_head;
     };
     boolean startCountDownForWholeBodyMasterSlave(in double sec);
     boolean stopHumanSync();
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index d5690093d89..ae160cca25e 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -277,9 +277,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onDeactivated(RTC::UniqueId ec_id)
 #define DEBUGP ((loop%200==0))
 RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
 {
-  // std::cerr << "WholeBodyMasterSlave::onExecute(" << ec_id << ")" << std::endl;
     // Read Inport
-
     if (m_qRefIn.isNew()) {
         m_qRefIn.read();
     }
@@ -303,7 +301,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
         m_optionalDataIn.read();
     }
     //for HumanSynchronizer
-    if (m_htzmpIn.isNew()){	m_htzmpIn.read(); }
     if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hsp->hp_wld_raw.w[rfw]); }
     if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hsp->hp_wld_raw.w[lfw]); }
     if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_htcom.data,hsp->hp_wld_raw.P[com]); }
@@ -312,63 +309,69 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
     if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrh.data,hsp->hp_wld_raw.P[rh]);}
     if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlh.data,hsp->hp_wld_raw.P[lh]);}
     if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_hthead.data,hsp->hp_wld_raw.P[head]);}
+    if (m_htzmpIn.isNew()){ m_htzmpIn.read();  HumanSynchronizer::Point3DToVector3(m_htzmp.data,hsp->hp_wld_raw.P[zmp].p); }
 //    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
 
-
     if ( is_legged_robot ) {
 
-
       bool is_q_interpolator_empty = q_interpolator->isEmpty();
       if (!is_q_interpolator_empty && hsp->isHumanSyncOn()) {
         q_interpolator->get(&q_interpolator_ratio, true);
       }
-//        else {
-//        q_interpolator_ratio = (control_mode == MODE_IDLE) ? 0.0 : 1.0;
-//      }
 
       hsp->baselinkpose.p = m_robot->rootLink()->p;
-        hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
-        if(!hsp->isHumanSyncOn()){
-          m_robot->rootLink()->p = input_basePos;
-          m_robot->rootLink()->R = input_baseRot;
-          for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
-            m_robot->joint(i)->q = m_qRef.data[i];
-          }
-          m_robot->calcForwardKinematics();
+      hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
+
+      if(!hsp->isHumanSyncOn()){
+        m_robot->rootLink()->p = input_basePos;
+        m_robot->rootLink()->R = input_baseRot;
+        for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
+          m_robot->joint(i)->q = m_qRef.data[i];
         }
+        m_robot->calcForwardKinematics();
+      }
 
+      if(hsp->WBMSparam.is_doctor){
         processWholeBodyMasterSlave();
+      }else{
+        processWholeBodyMasterSlave_Raw();
+      }
+
+
 
-        /////// Inverse Dynamics /////////
-        if(!idsb.is_initialized){
-          idsb.setInitState(m_robot, m_dt);
-          invdyn_zmp_filters.resize(3);
-          for(int i=0;i<3;i++){
-            invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
+
+      /////// Inverse Dynamics /////////
+      if(!idsb.is_initialized){
+        idsb.setInitState(m_robot, m_dt);
+        invdyn_zmp_filters.resize(3);
+        for(int i=0;i<3;i++){
+          invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
 //            invdyn_zmp_filters[i].reset(ref_zmp(i));
-          }
-          idsb2.setInitState(m_robot, m_dt);
-          invdyn_zmp_filters2.resize(3);
-          for(int i=0;i<3;i++){
-            invdyn_zmp_filters2[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
+        }
+        idsb2.setInitState(m_robot, m_dt);
+        invdyn_zmp_filters2.resize(3);
+        for(int i=0;i<3;i++){
+          invdyn_zmp_filters2[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
 //            invdyn_zmp_filters2[i].reset(ref_zmp(i));
-          }
         }
-        calcAccelerationsForInverseDynamics(m_robot, idsb);
-        hrp::Vector3 ref_zmp_invdyn;
-        calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);
-        for(int i=0;i<3;i++) ref_zmp_invdyn(i) = invdyn_zmp_filters[i].passFilter(ref_zmp_invdyn(i));
-        updateInvDynStateBuffer(idsb);
-
+      }
+      calcAccelerationsForInverseDynamics(m_robot, idsb);
+      hrp::Vector3 ref_zmp_invdyn;
+      calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);
+      for(int i=0;i<3;i++) ref_zmp_invdyn(i) = invdyn_zmp_filters[i].passFilter(ref_zmp_invdyn(i));
+      updateInvDynStateBuffer(idsb);
 
-//        calcDynamicsFilterCompensation(ref_zmp, ref_zmp_invdyn);
+//        if(hsp->isHumanSyncOn()){
+//          calcDynamicsFilterCompensation(ref_zmp, ref_zmp_invdyn);
+//          rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.P[zmp].p - m_robot->rootLink()->p);//baseのrpy変わるからもう一回
+//        }
 
-        calcAccelerationsForInverseDynamics(m_robot, idsb2);
-        calcWorldZMPFromInverseDynamics(m_robot, idsb2, ref_zmp_invdyn2);
-//        for(int i=0;i<3;i++) ref_zmp_invdyn2(i) = invdyn_zmp_filters2[i].passFilter(ref_zmp_invdyn2(i));
-        updateInvDynStateBuffer(idsb2);
+      calcAccelerationsForInverseDynamics(m_robot, idsb2);
+      calcWorldZMPFromInverseDynamics(m_robot, idsb2, ref_zmp_invdyn2);
+      updateInvDynStateBuffer(idsb2);
 
 //        ref_zmp = ref_zmp_invdyn;
+//        ref_zmp = ref_zmp_invdyn2;
 //        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
     }
 
@@ -378,8 +381,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
         for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
 //          m_qRef.data[i] = m_robot->joint(i)->q;
           m_qRef.data[i] = q_interpolator_ratio * m_robot->joint(i)->q  + (1 - q_interpolator_ratio) * m_qRef.data[i];
-
-
         }
       }
       m_qOut.write();
@@ -475,6 +476,70 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
     return RTC::RTC_OK;
 }
 
+void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(){
+  static HumanPose raw_pose;
+  static BiquadIIRFilterVec pos_filters[num_pose_tgt], rot_filters[num_pose_tgt];
+  static unsigned int callnum;
+  if(callnum++ == 0){
+    for(int i=0;i<num_pose_tgt;i++){
+      pos_filters[i].setParameter(100.0, 1.0/m_dt, BiquadIIRFilterVec::Q_NOOVERSHOOT);
+      rot_filters[i].setParameter(100.0, 1.0/m_dt, BiquadIIRFilterVec::Q_NOOVERSHOOT);
+    }
+  }
+
+  fik->current_tm = m_qRef.tm;
+  fik->ikp["rleg"].is_ik_enable = true;
+  fik->ikp["lleg"].is_ik_enable = true;
+  fik->ikp["rarm"].is_ik_enable = true;
+  fik->ikp["larm"].is_ik_enable = true;
+
+  if(hsp->startCountdownForHumanSync){
+   std::cerr << "[" << m_profile.instance_name << "] Count Down for RawSync ["<<hsp->getRemainingCountDown()<<"]\r";
+   hsp->updateCountDown();
+  }
+  if(hsp->isHumanSyncOn()){
+   if(hsp->ht_first_call){
+     std::cerr << "\n[" << m_profile.instance_name << "] Start RawSync"<< std::endl;
+     hsp->ht_first_call = false;
+   }
+  }else{
+   //初期状態取得
+   fik->storeCurrentParameters();
+   fik->setReferenceJointAngles();
+   const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
+   const int human_l_names[4] = {rf,lf,rh,lh};
+   raw_pose.P[com].p = raw_pose.P[com].p_offs = m_robot->calcCM();
+   raw_pose.P[com].rpy = raw_pose.P[com].rpy_offs = hrp::rpyFromRot(m_robot->rootLink()->R);
+   pos_filters[com].init(raw_pose.P[com].p);
+   rot_filters[com].init(raw_pose.P[com].rpy);
+   pos_filters[zmp].init(hrp::Vector3::Zero());
+   for(int i=0;i<4;i++){
+     if(fik->ikp.count(robot_l_names[i])){
+       raw_pose.P[human_l_names[i]].p_offs = fik->ikp[robot_l_names[i]].target_link->p + fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localPos;
+       raw_pose.P[human_l_names[i]].rpy_offs = hrp::rpyFromRot(fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localR);
+       raw_pose.P[human_l_names[i]].p = hsp->rp_ref_out.P[human_l_names[i]].p_offs;
+       raw_pose.P[human_l_names[i]].rpy = hsp->rp_ref_out.P[human_l_names[i]].rpy_offs;
+       pos_filters[human_l_names[i]].init(raw_pose.P[human_l_names[i]].p);
+       rot_filters[human_l_names[i]].init(raw_pose.P[human_l_names[i]].rpy);
+     }
+   }
+  }
+
+  m_robot->calcForwardKinematics();
+  if(DEBUGP){ printf("\x1b[31mmaster-mode:\x1b[39m"); raw_pose.print(); }
+
+  if(hsp->isHumanSyncOn()){
+    for(int i=0;i<num_pose_tgt;i++){
+      raw_pose.P[i].p = pos_filters[i].passFilter(hsp->hp_wld_raw.P[i].p);
+      raw_pose.P[i].rpy = rot_filters[i].passFilter(hsp->hp_wld_raw.P[i].rpy);
+    }
+    solveFullbodyIKStrictCOM( raw_pose.P[com], raw_pose.P[rf], raw_pose.P[lf], raw_pose.P[rh], raw_pose.P[lh], raw_pose.P[head] );
+    rel_ref_zmp = m_robot->rootLink()->R.transpose() * (raw_pose.P[zmp].p - m_robot->rootLink()->p);
+  }else{
+    rel_ref_zmp = input_ref_zmp;
+  }
+}
+
 void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
 
 // struct timeval t_calc_start, t_calc_end;
@@ -539,8 +604,6 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
     solveFullbodyIKStrictCOM( hsp->rp_ref_out.P[com], hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head] );
     //outport用のデータ上書き
     hsp->rp_ref_out.P[zmp].p(2) = (hsp->rp_ref_out.P[rf].p(2) + hsp->rp_ref_out.P[lf].p(2))/2;//体幹相対ZMP高さ設定
-
-
     rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.P[zmp].p - m_robot->rootLink()->p);
     if(m_optionalData.data.length() < 4*2){
       m_optionalData.data.length(4*2);//これいいのか？
@@ -553,48 +616,14 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
   }
 }
 
-//void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref){
-//  int com_ik_loop=0;
-//  const int COM_IK_MAX_LOOP = 5;
-//  const double COM_IK_MAX_ERROR = 1e-4;
-//  m_robot->rootLink()->p = com_ref.p;//move base link at first
-//  m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
-//
-//  hrp::Vector3 tmp_com_err;
-//  const std::string names[4] = {"rleg","lleg","rarm","larm"};
-//  const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
-//  for(int i=0;i<4;i++){
-//    if(fik->ikp.count(names[i])){
-//      fik->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);//convert End Effector pos into link origin pos
-//      fik->ikp[names[i]].target_p0 = refs[i]->p;
-//    }
-//  }
-//  if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref.rpy(y);
-//  if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref.rpy(p);
-//  //COM 収束ループ
-//  while(
-//      m_robot->calcForwardKinematics(),
-//      tmp_com_err = com_ref.p - m_robot->calcCM(),
-//      tmp_com_err.norm() > COM_IK_MAX_ERROR,
-//      com_ik_loop++)
-//  {
-//    m_robot->rootLink()->p += tmp_com_err;
-//    m_robot->calcForwardKinematics();
-//    for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-//        if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
-//    }
-//    if(com_ik_loop > COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
-//  }
-//  cout<<"COM_IK_MAX_LOOP:"<<com_ik_loop<<endl;
-//}
-
-void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref){
+void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, const bool solve_from_previous_basepos){
   int com_ik_loop=0;
-  const int COM_IK_MAX_LOOP = 5;
+  const int COM_IK_MAX_LOOP = 20;
   const double COM_IK_MAX_ERROR = 1e-4;
+//  static hrp::Vector3 base_pos_pre;
   m_robot->rootLink()->p = com_ref.p;//move base link at first
+//  if(solve_from_previous_basepos) m_robot->rootLink()->p = base_pos_pre;
   m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
-
   const std::string names[4] = {"rleg","lleg","rarm","larm"};
   const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
   for(int i=0;i<4;i++){
@@ -614,14 +643,15 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, co
         if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
     }
     tmp_com_err = com_ref.p - m_robot->calcCM();
-    cout<<"tmp_com_err:"<<tmp_com_err.norm()<<endl;
+//    cout<<"tmp_com_err:"<<tmp_com_err.norm()<<endl;
     if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
     m_robot->rootLink()->p += tmp_com_err;
     if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
   }
-  cout<<"COM_IK_MAX_LOOP:"<<com_ik_loop<<endl;
+  if(solve_from_previous_basepos)cout<<"com_ik_loop"<<com_ik_loop<<endl;
 }
 
+
 //
 //hrp::Vector3 torso_pos = hrp::Vector3::Zero();
 //hrp::Vector3 torso_rot = hrp::Vector3::Zero();
@@ -720,45 +750,29 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, co
 //
 //
 void WholeBodyMasterSlave::calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody){
-
-
-//  hrp::dmatrix Jp;
-//  hrp::dmatrix Jl;
-//  m_robot->calcCM();
-//  m_robot->calcCMJacobian(NULL, Jp); // Eq.1 upper [M/m E -r]
-//  m_robot->calcAngularMomentumJacobian(NULL, Jl); // Eq.1 lower [H 0 I]
-//  Jp *= m_robot->totalMass();
-//  hrp::dmatrix Jpl(Jp.rows() + Jl.rows(), Jp.cols());
-//  Jpl << Jp, Jl;
-//  hrp::dvector all_vel_vec(3+3+m_robot->numJoints());
-//  all_vel_vec << idsb.base_v, idsb.base_w, idsb.dq;
   hrp::Vector3 P,L;
   m_robot->rootLink()->v = idsb.base_v;
   m_robot->rootLink()->w = idsb.base_w;
   m_robot->calcTotalMomentum(P,L);
   L = L - m_robot->calcCM().cross(P);
-
-//  torso_rot(0) -= L(0) * 0.1 * m_dt;
-//  torso_rot(1) -= L(1) * 0.1 * m_dt;
-//  torso_rot(2) -= L(2) * 0.1 * m_dt;
-
-
-
-  fprintf(hsp->id_log,"p: %f %f ",hsp->rp_ref_out.P[com].p(0),hsp->rp_ref_out.P[com].p(1),hsp->rp_ref_out.P[com].p(2));
-  fprintf(hsp->id_log,"rpy: %f %f ",hsp->rp_ref_out.P[com].rpy(0),hsp->rp_ref_out.P[com].rpy(1),hsp->rp_ref_out.P[com].rpy(2));
-  fprintf(hsp->id_log,"P: %f %f ",P(0),P(1),P(2));
-  fprintf(hsp->id_log,"L: %f %f ",L(0),L(1),L(2));
-//  fprintf(hsp->id_log,"p_v: %f %f ",idsb.base_v(0),idsb.base_v(1),idsb.base_v(2));
-//  fprintf(hsp->id_log,"rpy_v: %f %f ",idsb.base_w(0),idsb.base_w(1),idsb.base_w(2));
-//    fprintf(hsp->id_log,"torso_pos: %f %f ",torso_pos(0),torso_pos(1),torso_pos(2));
-//    fprintf(hsp->id_log,"torso_rot: %f %f ",torso_rot(0),torso_rot(1),torso_rot(2));
-  fprintf(hsp->id_log,"\n");
-
+  static hrp::Vector3 torso_rot;
+  hrp::Matrix33 tmp;
+  m_robot->rootLink()->calcSubMassInertia(tmp);
+  cout<<"mass:"<<tmp<<endl;
+  if(tmp != hrp::Matrix33::Zero())torso_rot -= tmp.inverse() * L * m_dt * 0.3;
+//  torso_rot(0) -= L(0) * 0.01 * m_dt;
+//  torso_rot(1) -= L(1) * 0.03 * m_dt;
+//  torso_rot(2) -= L(2) * 0.03 * m_dt;
+//  fprintf(hsp->id_log,"p: %f %f ",hsp->rp_ref_out.P[com].p(0),hsp->rp_ref_out.P[com].p(1),hsp->rp_ref_out.P[com].p(2));
+//  fprintf(hsp->id_log,"rpy: %f %f ",hsp->rp_ref_out.P[com].rpy(0),hsp->rp_ref_out.P[com].rpy(1),hsp->rp_ref_out.P[com].rpy(2));
+//  fprintf(hsp->id_log,"P: %f %f ",P(0),P(1),P(2));
+//  fprintf(hsp->id_log,"L: %f %f ",L(0),L(1),L(2));
+//  fprintf(hsp->id_log,"\n");
   if(hsp->isHumanSyncOn()){
     HRPPose3D com_mod = hsp->rp_ref_out.P[com];
 //    com_mod.p += torso_pos;
-//    com_mod.rpy += torso_rot;
-    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head]);
+    com_mod.rpy += torso_rot;
+    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head], true);
   }
 }
 
@@ -789,29 +803,31 @@ bool WholeBodyMasterSlave::stopHumanSync()
 
 bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
   std::cerr << "[" << m_profile.instance_name << "] setWholeBodyMasterSlaveParam" << std::endl;
-  hsp->WBMSparam.use_rh = hsp->WBMSparam.use_lh = i_param.use_hands;
-  hsp->WBMSparam.use_head = i_param.use_head;
-  hsp->WBMSparam.set_com_height_fix = i_param.set_com_height_fix;
-  hsp->WBMSparam.set_com_height_fix_val = i_param.set_com_height_fix_val;
+  hsp->WBMSparam.auto_swing_foot_landing_threshold = i_param.auto_swing_foot_landing_threshold;
   hsp->WBMSparam.foot_vertical_vel_limit_coeff = i_param.foot_vertical_vel_limit_coeff;
   hsp->WBMSparam.human_com_height = i_param.human_com_height;
+  hsp->WBMSparam.is_doctor = i_param.is_doctor;
+  hsp->WBMSparam.set_com_height_fix = i_param.set_com_height_fix;
+  hsp->WBMSparam.set_com_height_fix_val = i_param.set_com_height_fix_val;
   hsp->WBMSparam.swing_foot_height_offset = i_param.swing_foot_height_offset;
   hsp->WBMSparam.swing_foot_max_height = i_param.swing_foot_max_height;
-  hsp->WBMSparam.auto_swing_foot_landing_threshold = i_param.auto_swing_foot_landing_threshold;
+  hsp->WBMSparam.use_rh = hsp->WBMSparam.use_lh = i_param.use_hands;
+  hsp->WBMSparam.use_head = i_param.use_head;
   return true;
 }
 
 bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
   std::cerr << "[" << m_profile.instance_name << "] getWholeBodyMasterSlaveParam" << std::endl;
-  i_param.set_com_height_fix = hsp->WBMSparam.set_com_height_fix;
-  i_param.set_com_height_fix_val = hsp->WBMSparam.set_com_height_fix_val;
+  i_param.auto_swing_foot_landing_threshold = hsp->WBMSparam.auto_swing_foot_landing_threshold;
   i_param.foot_vertical_vel_limit_coeff = hsp->WBMSparam.foot_vertical_vel_limit_coeff;
   i_param.human_com_height = hsp->WBMSparam.human_com_height;
-  i_param.use_hands = (hsp->WBMSparam.use_rh || hsp->WBMSparam.use_lh);
-  i_param.use_head = hsp->WBMSparam.use_head;
+  i_param.is_doctor = hsp->WBMSparam.is_doctor;
+  i_param.set_com_height_fix = hsp->WBMSparam.set_com_height_fix;
+  i_param.set_com_height_fix_val = hsp->WBMSparam.set_com_height_fix_val;
   i_param.swing_foot_height_offset = hsp->WBMSparam.swing_foot_height_offset;
   i_param.swing_foot_max_height = hsp->WBMSparam.swing_foot_max_height;
-  i_param.auto_swing_foot_landing_threshold = hsp->WBMSparam.auto_swing_foot_landing_threshold;
+  i_param.use_hands = (hsp->WBMSparam.use_rh || hsp->WBMSparam.use_lh);
+  i_param.use_head = hsp->WBMSparam.use_head;
   return true;
 }
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index b7f4eff5738..10d83bcde33 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -153,8 +153,9 @@ class WholeBodyMasterSlave
     hrp::Link* target_link;
     bool is_active, has_toe_joint;
   };
-  void solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref);
+  void solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, const bool solve_from_previous_basepos = false);
   void processWholeBodyMasterSlave();
+  void processWholeBodyMasterSlave_Raw();
   void calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody);
   bool isOptionalDataContact (const std::string& ee_name)
   {
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 76650b68c5c..0611bec6329 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -208,15 +208,16 @@ class HumanSynchronizer{
     interpolator *init_zmp_com_offset_interpolator;
     double zmp_com_offset_ip_ratio;
     struct WBMSparameters {
-      bool set_com_height_fix;
-      double set_com_height_fix_val;
+      double auto_swing_foot_landing_threshold;
       double foot_vertical_vel_limit_coeff;
       double human_com_height;
-      bool use_rh,use_lh;
-      bool use_head;
+      bool is_doctor;
+      bool set_com_height_fix;
+      double set_com_height_fix_val;
       double swing_foot_height_offset;
       double swing_foot_max_height;
-      double auto_swing_foot_landing_threshold;
+      bool use_rh,use_lh;
+      bool use_head;
     };
     struct WBMSparameters WBMSparam;
 
@@ -299,6 +300,7 @@ class HumanSynchronizer{
 //      WBMSparam.swing_foot_max_height = 0.10;
       WBMSparam.swing_foot_max_height = 0.5;
       WBMSparam.auto_swing_foot_landing_threshold = 0.02;
+      WBMSparam.is_doctor = true;
 
       rp_ref_out_old.clear();
       rp_ref_out.clear();
@@ -723,110 +725,4 @@ class HumanSynchronizer{
     void applyCOMZMPXYZLock(HumanPose& tgt);
 };
 
-//
-//class SimpleFullbodyInverseDynamicsSolver{
-//  private:
-//    hrp::BodyPtr m_robot;
-//    hrp::dvector q, q_old, q_oldold, dq, ddq, ddq_filtered;
-//    hrp::Vector3 base_p, base_p_old, base_p_oldold, base_v, base_dv, base_dv_filtered;
-//    hrp::Matrix33 base_R, base_R_old, base_dR, base_w_hat;
-//    hrp::Vector3 base_w, base_w_old, base_dw, base_dw_filtered;
-//    std::vector<IIRFilter> ddq_filter, base_dv_filter, base_dw_filter;
-//    double DT;
-//    bool is_RobotStateInitialized;
-//
-//  public:
-//    SimpleFullbodyInverseDynamicsSolver(const hrp::BodyPtr& _robot, const double _dt, const double _acceleration_filter_fc = 25.0)
-//        : m_robot(_robot),
-//          DT(_dt),
-//          is_RobotStateInitialized(false)
-//    {
-//      q.resize(m_robot->numJoints());
-//      q_old.resize(m_robot->numJoints());
-//      q_oldold.resize(m_robot->numJoints());
-//      dq.resize(m_robot->numJoints());
-//      ddq.resize(m_robot->numJoints());
-//      ddq_filtered.resize(m_robot->numJoints());
-//      initializeAccelerationFilters(_acceleration_filter_fc, _dt);
-//    };
-//    ~SimpleFullbodyInverseDynamicsSolver(){
-//    };
-//    void initializeAccelerationFilters(const double _fc_in, const double _dt){//set up filters as 2nd order Biquad IIR
-//      ddq_filter.resize(m_robot->numJoints());
-//      base_dv_filter.resize(3);
-//      base_dw_filter.resize(3);
-//      std::vector<double> fb_coeffs(3), ff_coeffs(3);
-//      const double fc = std::tan(_fc_in * M_PI * _dt) / (2 * M_PI);
-//      const double denom = 1 + (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc;
-//      ff_coeffs[0] = (4 * M_PI * M_PI * fc*fc) / denom;
-//      ff_coeffs[1] = (8 * M_PI * M_PI * fc*fc) / denom;
-//      ff_coeffs[2] = (4 * M_PI * M_PI * fc*fc) / denom;
-//      fb_coeffs[0] = 1.0;
-//      fb_coeffs[1] = (8 * M_PI * M_PI * fc*fc - 2) / denom;
-//      fb_coeffs[2] = (1 - (2 * sqrt(2) * M_PI * fc) + 4 * M_PI * M_PI * fc*fc) / denom;
-//      for(int i=0;i<m_robot->numJoints();i++)ddq_filter[i].setParameter(2,fb_coeffs,ff_coeffs);
-//      for(int i=0;i<3;i++)base_dv_filter[i].setParameter(2,fb_coeffs,ff_coeffs);
-//      for(int i=0;i<3;i++)base_dw_filter[i].setParameter(2,fb_coeffs,ff_coeffs);
-//    };
-//    void initializeRobotState(){
-//      for(int i=0;i<m_robot->numJoints();i++)q(i) = m_robot->joint(i)->q;
-//      q_oldold = q_old = q;
-//      dq = ddq = ddq_filtered = hrp::dvector::Zero(m_robot->numJoints());
-//      base_p_oldold = base_p_old = base_p = m_robot->rootLink()->p;
-//      base_R_old = base_R = m_robot->rootLink()->R;
-//      base_dR = base_w_hat = hrp::Matrix33::Zero();
-//      base_w_old = base_w = base_dw = base_dw_filtered = hrp::Vector3::Zero();
-//      is_RobotStateInitialized = true;
-//    };
-//    void solveFullbodyID(hrp::Vector3& f_ans, hrp::Vector3& t_ans){
-//      if(!is_RobotStateInitialized) initializeRobotState();
-//      calcAccelerationsForInverseDynamics();
-//      passAccelerationFilters();
-//      setCurrentRobotState();
-//      m_robot->calcForwardKinematics(true,true);// calc every link's acc and vel
-//      m_robot->calcInverseDynamics(m_robot->rootLink(),f_ans,t_ans);// this returns f,t at the coordinate origin (not at base link pos)
-//    };
-//
-//  private:
-//    void calcAccelerationsForInverseDynamics(){
-//      for(int i=0;i<m_robot->numJoints();i++)q(i) = m_robot->joint(i)->q;
-//      dq = (q - q_old) / DT;
-//      ddq = (q - 2 * q_old + q_oldold) / (DT * DT);
-//      q_oldold = q_old;
-//      q_old = q;
-//      const hrp::Vector3 g(0, 0, 9.80665);
-//      base_p = m_robot->rootLink()->p;
-//      base_v = (base_p - base_p_old) / DT;
-//      base_dv = g + (base_p - 2 * base_p_old + base_p_oldold) / (DT * DT);
-//      base_p_oldold = base_p_old;
-//      base_p_old = base_p;
-//      base_R =  m_robot->rootLink()->R;
-//      base_dR = (base_R - base_R_old) / DT;
-//      base_w_hat = base_dR * base_R.transpose();
-//      base_w = hrp::Vector3(base_w_hat(2,1), - base_w_hat(0,2), base_w_hat(1,0));
-//      base_dw = (base_w - base_w_old) / DT;
-//      base_R_old = base_R;
-//      base_w_old = base_w;
-//    };
-//    void passAccelerationFilters(){
-//      for(int i=0;i<m_robot->numJoints();i++)ddq_filtered[i] = ddq_filter[i].passFilter(ddq[i]);
-//      for(int i=0;i<3;i++){
-//        base_dv_filtered[i] = base_dv_filter[i].passFilter(base_dv[i]);
-//        base_dw_filtered[i] = base_dw_filter[i].passFilter(base_dw[i]);
-//      }
-//    };
-//    void setCurrentRobotState(){
-//      for(int i=0;i<m_robot->numJoints();i++){
-//        m_robot->joint(i)->dq = dq(i);
-//        m_robot->joint(i)->ddq = ddq_filtered(i);
-//      }
-//      m_robot->rootLink()->vo = base_v - base_w.cross(base_p);
-//      //static hrp::Vector3 vo_old; m_robot->rootLink()->dvo = g + (m_robot->rootLink()->vo - vo_old)/DT; vo_old = m_robot->rootLink()->vo; // calc in incremental way
-//      m_robot->rootLink()->dvo = base_dv_filtered - base_dw_filtered.cross(base_p) - base_w.cross(base_v); // calc in differential way
-//      m_robot->rootLink()->w = base_w;
-//      m_robot->rootLink()->dw = base_dw_filtered;
-//    };
-//};
-
-
 #endif // HUMANMASTERSLAVE_H

From 429fb9de918e3ea29202d14f033318cdf8268704 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 10 May 2017 23:46:57 +0900
Subject: [PATCH 081/217] stdout -> stderr

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 3 ++-
 rtc/WholeBodyMasterSlave/wbms_core.h              | 4 ++--
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index ae160cca25e..2d6978ecafb 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -277,6 +277,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onDeactivated(RTC::UniqueId ec_id)
 #define DEBUGP ((loop%200==0))
 RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
 {
+//  std::cerr << "[" << m_profile.instance_name<< "] onExecute(" << ec_id << ")" << std::endl;
     // Read Inport
     if (m_qRefIn.isNew()) {
         m_qRefIn.read();
@@ -526,7 +527,7 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(){
   }
 
   m_robot->calcForwardKinematics();
-  if(DEBUGP){ printf("\x1b[31mmaster-mode:\x1b[39m"); raw_pose.print(); }
+  if(DEBUGP){ fprintf(stderr,"\x1b[31mmaster-mode:\x1b[39m"); raw_pose.print(); }
 
   if(hsp->isHumanSyncOn()){
     for(int i=0;i<num_pose_tgt;i++){
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 0611bec6329..abfb7775921 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -122,8 +122,8 @@ class HumanPose{
     }
     static void hp_printf(const HumanPose& in){
       const std::string cap[] = {"c","rf","lf","rh","lh","z","rw","lw"};
-      for(int i=com;i<=zmp;i++){ printf("\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.P[i].p(X),in.P[i].p(Y),in.P[i].p(Z)); }
-      for(int i=rfw;i<lfw;i++){ printf("\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.w[i].w(fx),in.w[i].w(fy),in.w[i].w(fz),in.w[i].w(tx),in.w[i].w(ty),in.w[i].w(tz)); }
+      for(int i=com;i<=zmp;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.P[i].p(X),in.P[i].p(Y),in.P[i].p(Z)); }
+      for(int i=rfw;i<lfw;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.w[i].w(fx),in.w[i].w(fy),in.w[i].w(fz),in.w[i].w(tx),in.w[i].w(ty),in.w[i].w(tz)); }
       printf("\n");
     }
     void print(){ hp_printf(*this); }

From 7ce69d42cead350f058fcf4800e06ca4a442a232 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 11 May 2017 01:21:55 +0900
Subject: [PATCH 082/217] fix contact support time

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 2d6978ecafb..69208a46336 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -610,8 +610,8 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
       m_optionalData.data.length(4*2);//これいいのか？
       for(int i=0;i<4*2;i++)m_optionalData.data[i] = 0;
     }
-    m_optionalData.data[contact_states_index_map["rleg"]] = hsp->is_rf_contact;
-    m_optionalData.data[contact_states_index_map["lleg"]] = hsp->is_lf_contact;
+    m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[contact_states_index_map["rleg"]+4] = hsp->is_rf_contact;
+    m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[contact_states_index_map["lleg"]+4] = hsp->is_lf_contact;
   }else{
     rel_ref_zmp = input_ref_zmp;
   }

From af58c583b16da537b3e01d5270ddd545c5a8485d Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 12 May 2017 19:00:52 +0900
Subject: [PATCH 083/217] tset upperbody RMC

---
 idl/#WholeBodyMasterSlave.idl#                |   2 +
 idl/WholeBodyMasterSlaveService.idl           |   1 +
 rtc/TorqueFilter/IIRFilter.cpp                |  34 +-
 rtc/TorqueFilter/IIRFilter.h                  |   1 -
 .../WholeBodyMasterSlave.cpp                  | 311 ++++++++++++------
 .../WholeBodyMasterSlave.h                    |   4 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          |  29 +-
 7 files changed, 248 insertions(+), 134 deletions(-)
 create mode 100644 idl/#WholeBodyMasterSlave.idl#

diff --git a/idl/#WholeBodyMasterSlave.idl# b/idl/#WholeBodyMasterSlave.idl#
new file mode 100644
index 00000000000..139597f9cb0
--- /dev/null
+++ b/idl/#WholeBodyMasterSlave.idl#
@@ -0,0 +1,2 @@
+
+
diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
index 4285a1ec62f..ff31983cf82 100644
--- a/idl/WholeBodyMasterSlaveService.idl
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -44,6 +44,7 @@ module OpenHRP
         double set_com_height_fix_val;
         double swing_foot_height_offset;
         double swing_foot_max_height;
+        double upper_body_rmc_ratio;
         boolean use_hands;
         boolean use_head;
     };
diff --git a/rtc/TorqueFilter/IIRFilter.cpp b/rtc/TorqueFilter/IIRFilter.cpp
index ee931f9971d..886e29cb68a 100644
--- a/rtc/TorqueFilter/IIRFilter.cpp
+++ b/rtc/TorqueFilter/IIRFilter.cpp
@@ -21,7 +21,6 @@ IIRFilter::IIRFilter(unsigned int dim, std::vector<double>& fb_coeffs, std::vect
 
     // init previous values
     m_previous_values.assign(dim, 0.0);
-    m_previous_inputs.assign(dim, 0.0);
     m_initialized = true;
     return;
 }
@@ -39,6 +38,7 @@ bool IIRFilter::setParameter(int dim, std::vector<double>& A, std::vector<double
         std::cout << "[" <<  m_error_prefix << "]" << "IIRFilter coefficients size error" << std::endl;
         return false;
     }
+
     // clear previous coefficients
     m_fb_coefficients.clear();
     m_ff_coefficients.clear();
@@ -62,7 +62,6 @@ bool IIRFilter::setParameter(int dim, std::vector<double>& A, std::vector<double
 
     // init previous values
     m_previous_values.assign(dim, 0.0);
-    m_previous_inputs.assign(dim, 0.0);
     m_initialized = true;
     return true;
 }
@@ -115,36 +114,19 @@ double IIRFilter::passFilter(double input)
     if (! m_initialized) {
         return 0.0;
     }
-    //    double feedback, filtered;
-    //    // calcurate retval
-    //    feedback = m_fb_coefficients[0] * input;
-    //    for (int i = 0; i < m_dimension; i++) {
-    //        feedback += m_fb_coefficients[i + 1] * m_previous_values[i];
-    //    }
-    //    filtered = m_ff_coefficients[0] * feedback;
-    //    for(int i = 0; i < m_dimension; i++) {
-    //        filtered += m_ff_coefficients[i + 1] * m_previous_values[i];
-    //    }
-    //    // update previous values
-    //    m_previous_values.push_front(feedback);
-    //    m_previous_values.pop_back();
-    //    return filtered;
-
-    double feedback, feedforward, filtered;
+    double feedback, filtered;
     // calcurate retval
-    feedback = 0;
+    feedback = m_fb_coefficients[0] * input;
     for (int i = 0; i < m_dimension; i++) {
-      feedback += m_fb_coefficients[i + 1] * m_previous_values[i];
+        feedback += m_fb_coefficients[i + 1] * m_previous_values[i];
     }
-    feedforward = m_ff_coefficients[0] * input;
+    filtered = m_ff_coefficients[0] * feedback;
     for(int i = 0; i < m_dimension; i++) {
-      feedforward += m_ff_coefficients[i + 1] * m_previous_inputs[i];
+        filtered += m_ff_coefficients[i + 1] * m_previous_values[i];
     }
-    filtered = feedback + feedforward;
     // update previous values
-    m_previous_values.push_front(filtered);
+    m_previous_values.push_front(feedback);
     m_previous_values.pop_back();
-    m_previous_inputs.push_front(input);
-    m_previous_inputs.pop_back();
+
     return filtered;
 }
diff --git a/rtc/TorqueFilter/IIRFilter.h b/rtc/TorqueFilter/IIRFilter.h
index ae23532ad24..213d4c12fc2 100644
--- a/rtc/TorqueFilter/IIRFilter.h
+++ b/rtc/TorqueFilter/IIRFilter.h
@@ -86,7 +86,6 @@ class IIRFilter
     std::vector<double> m_fb_coefficients; // fb parameters (dim must be m_dimension + 1, m_fb_coefficients[0] would be 1.0)
     std::vector<double> m_ff_coefficients; // ff parameters (dim must be m_dimension + 1)
     std::deque<double> m_previous_values;
-    std::deque<double> m_previous_inputs;
     // double m_prev_output;
     bool m_initialized;
     std::string m_error_prefix;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 69208a46336..55760d0cf1f 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -338,9 +338,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
         processWholeBodyMasterSlave_Raw();
       }
 
-
-
-
       /////// Inverse Dynamics /////////
       if(!idsb.is_initialized){
         idsb.setInitState(m_robot, m_dt);
@@ -362,10 +359,10 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
       for(int i=0;i<3;i++) ref_zmp_invdyn(i) = invdyn_zmp_filters[i].passFilter(ref_zmp_invdyn(i));
       updateInvDynStateBuffer(idsb);
 
-//        if(hsp->isHumanSyncOn()){
-//          calcDynamicsFilterCompensation(ref_zmp, ref_zmp_invdyn);
-//          rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.P[zmp].p - m_robot->rootLink()->p);//baseのrpy変わるからもう一回
-//        }
+      if(hsp->isHumanSyncOn() && hsp->WBMSparam.is_doctor){
+        calcDynamicsFilterCompensation(ref_zmp, ref_zmp_invdyn);
+        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.P[zmp].p - m_robot->rootLink()->p);//baseのrpy変わるからもう一回
+      }
 
       calcAccelerationsForInverseDynamics(m_robot, idsb2);
       calcWorldZMPFromInverseDynamics(m_robot, idsb2, ref_zmp_invdyn2);
@@ -477,70 +474,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
     return RTC::RTC_OK;
 }
 
-void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(){
-  static HumanPose raw_pose;
-  static BiquadIIRFilterVec pos_filters[num_pose_tgt], rot_filters[num_pose_tgt];
-  static unsigned int callnum;
-  if(callnum++ == 0){
-    for(int i=0;i<num_pose_tgt;i++){
-      pos_filters[i].setParameter(100.0, 1.0/m_dt, BiquadIIRFilterVec::Q_NOOVERSHOOT);
-      rot_filters[i].setParameter(100.0, 1.0/m_dt, BiquadIIRFilterVec::Q_NOOVERSHOOT);
-    }
-  }
-
-  fik->current_tm = m_qRef.tm;
-  fik->ikp["rleg"].is_ik_enable = true;
-  fik->ikp["lleg"].is_ik_enable = true;
-  fik->ikp["rarm"].is_ik_enable = true;
-  fik->ikp["larm"].is_ik_enable = true;
-
-  if(hsp->startCountdownForHumanSync){
-   std::cerr << "[" << m_profile.instance_name << "] Count Down for RawSync ["<<hsp->getRemainingCountDown()<<"]\r";
-   hsp->updateCountDown();
-  }
-  if(hsp->isHumanSyncOn()){
-   if(hsp->ht_first_call){
-     std::cerr << "\n[" << m_profile.instance_name << "] Start RawSync"<< std::endl;
-     hsp->ht_first_call = false;
-   }
-  }else{
-   //初期状態取得
-   fik->storeCurrentParameters();
-   fik->setReferenceJointAngles();
-   const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
-   const int human_l_names[4] = {rf,lf,rh,lh};
-   raw_pose.P[com].p = raw_pose.P[com].p_offs = m_robot->calcCM();
-   raw_pose.P[com].rpy = raw_pose.P[com].rpy_offs = hrp::rpyFromRot(m_robot->rootLink()->R);
-   pos_filters[com].init(raw_pose.P[com].p);
-   rot_filters[com].init(raw_pose.P[com].rpy);
-   pos_filters[zmp].init(hrp::Vector3::Zero());
-   for(int i=0;i<4;i++){
-     if(fik->ikp.count(robot_l_names[i])){
-       raw_pose.P[human_l_names[i]].p_offs = fik->ikp[robot_l_names[i]].target_link->p + fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localPos;
-       raw_pose.P[human_l_names[i]].rpy_offs = hrp::rpyFromRot(fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localR);
-       raw_pose.P[human_l_names[i]].p = hsp->rp_ref_out.P[human_l_names[i]].p_offs;
-       raw_pose.P[human_l_names[i]].rpy = hsp->rp_ref_out.P[human_l_names[i]].rpy_offs;
-       pos_filters[human_l_names[i]].init(raw_pose.P[human_l_names[i]].p);
-       rot_filters[human_l_names[i]].init(raw_pose.P[human_l_names[i]].rpy);
-     }
-   }
-  }
-
-  m_robot->calcForwardKinematics();
-  if(DEBUGP){ fprintf(stderr,"\x1b[31mmaster-mode:\x1b[39m"); raw_pose.print(); }
-
-  if(hsp->isHumanSyncOn()){
-    for(int i=0;i<num_pose_tgt;i++){
-      raw_pose.P[i].p = pos_filters[i].passFilter(hsp->hp_wld_raw.P[i].p);
-      raw_pose.P[i].rpy = rot_filters[i].passFilter(hsp->hp_wld_raw.P[i].rpy);
-    }
-    solveFullbodyIKStrictCOM( raw_pose.P[com], raw_pose.P[rf], raw_pose.P[lf], raw_pose.P[rh], raw_pose.P[lh], raw_pose.P[head] );
-    rel_ref_zmp = m_robot->rootLink()->R.transpose() * (raw_pose.P[zmp].p - m_robot->rootLink()->p);
-  }else{
-    rel_ref_zmp = input_ref_zmp;
-  }
-}
-
 void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
 
 // struct timeval t_calc_start, t_calc_end;
@@ -549,8 +482,10 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
   fik->current_tm = m_qRef.tm;
   fik->ikp["rleg"].is_ik_enable = true;
   fik->ikp["lleg"].is_ik_enable = true;
-   fik->ikp["rarm"].is_ik_enable = true;
-   fik->ikp["larm"].is_ik_enable = true;
+//  fik->ikp["rarm"].is_ik_enable = true;
+//  fik->ikp["larm"].is_ik_enable = true;
+  fik->ikp["rarm"].is_ik_enable = false;
+  fik->ikp["larm"].is_ik_enable = false;
 
   if(hsp->startCountdownForHumanSync){
     std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
@@ -601,8 +536,15 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
   if(DEBUGP)hsp->rp_ref_out.print();
 
   m_robot->calcForwardKinematics();
+  static hrp::Vector3 basepos_normal_ik = m_robot->rootLink()->p;
+//  static hrp::dvector q_normal_ik;
+  q_normal_ik.resize(m_robot->numJoints());
+  if(!hsp->isHumanSyncOn()){
+    for( int i=0; i<m_robot->numJoints(); i++){ q_normal_ik(i) = m_robot->joint(i)->q; }
+  }
   if(hsp->isHumanSyncOn()){
-    solveFullbodyIKStrictCOM( hsp->rp_ref_out.P[com], hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head] );
+    cout<<"processWholeBodyMasterSlave:"<<endl;
+    solveFullbodyIKStrictCOM( hsp->rp_ref_out.P[com], hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head], basepos_normal_ik, q_normal_ik);
     //outport用のデータ上書き
     hsp->rp_ref_out.P[zmp].p(2) = (hsp->rp_ref_out.P[rf].p(2) + hsp->rp_ref_out.P[lf].p(2))/2;//体幹相対ZMP高さ設定
     rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.P[zmp].p - m_robot->rootLink()->p);
@@ -617,13 +559,81 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
   }
 }
 
-void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, const bool solve_from_previous_basepos){
+void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(){
+  static HumanPose raw_pose;
+  static BiquadIIRFilterVec pos_filters[num_pose_tgt], rot_filters[num_pose_tgt];
+  static unsigned int callnum;
+  if(callnum++ == 0){
+    for(int i=0;i<num_pose_tgt;i++){
+      pos_filters[i].setParameter(100.0, 1.0/m_dt, BiquadIIRFilterVec::Q_NOOVERSHOOT);
+      rot_filters[i].setParameter(100.0, 1.0/m_dt, BiquadIIRFilterVec::Q_NOOVERSHOOT);
+    }
+  }
+
+  fik->current_tm = m_qRef.tm;
+  fik->ikp["rleg"].is_ik_enable = true;
+  fik->ikp["lleg"].is_ik_enable = true;
+  fik->ikp["rarm"].is_ik_enable = true;
+  fik->ikp["larm"].is_ik_enable = true;
+
+  if(hsp->startCountdownForHumanSync){
+   std::cerr << "[" << m_profile.instance_name << "] Count Down for RawSync ["<<hsp->getRemainingCountDown()<<"]\r";
+   hsp->updateCountDown();
+  }
+  if(hsp->isHumanSyncOn()){
+   if(hsp->ht_first_call){
+     std::cerr << "\n[" << m_profile.instance_name << "] Start RawSync"<< std::endl;
+     hsp->ht_first_call = false;
+   }
+  }else{
+   //初期状態取得
+   fik->storeCurrentParameters();
+   fik->setReferenceJointAngles();
+   const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
+   const int human_l_names[4] = {rf,lf,rh,lh};
+   raw_pose.P[com].p = raw_pose.P[com].p_offs = m_robot->calcCM();
+   raw_pose.P[com].rpy = raw_pose.P[com].rpy_offs = hrp::rpyFromRot(m_robot->rootLink()->R);
+   pos_filters[com].init(raw_pose.P[com].p);
+   rot_filters[com].init(raw_pose.P[com].rpy);
+   pos_filters[zmp].init(hrp::Vector3::Zero());
+   for(int i=0;i<4;i++){
+     if(fik->ikp.count(robot_l_names[i])){
+       raw_pose.P[human_l_names[i]].p_offs = fik->ikp[robot_l_names[i]].target_link->p + fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localPos;
+       raw_pose.P[human_l_names[i]].rpy_offs = hrp::rpyFromRot(fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localR);
+       raw_pose.P[human_l_names[i]].p = hsp->rp_ref_out.P[human_l_names[i]].p_offs;
+       raw_pose.P[human_l_names[i]].rpy = hsp->rp_ref_out.P[human_l_names[i]].rpy_offs;
+       pos_filters[human_l_names[i]].init(raw_pose.P[human_l_names[i]].p);
+       rot_filters[human_l_names[i]].init(raw_pose.P[human_l_names[i]].rpy);
+     }
+   }
+  }
+
+  m_robot->calcForwardKinematics();
+  if(DEBUGP){ fprintf(stderr,"\x1b[31mmaster-mode:\x1b[39m"); raw_pose.print(); }
+
+  static hrp::Vector3 basepos_normal_ik = m_robot->rootLink()->p;
+//  static hrp::dvector q_normal_ik;
+  if(!hsp->isHumanSyncOn()){
+    q_normal_ik.resize(m_robot->numJoints());
+    for( int i=0; i<m_robot->numJoints(); i++){ q_normal_ik(i) = m_robot->joint(i)->q; }
+  }
+  if(hsp->isHumanSyncOn()){
+    for(int i=0;i<num_pose_tgt;i++){
+      raw_pose.P[i].p = pos_filters[i].passFilter(hsp->hp_wld_raw.P[i].p);
+      raw_pose.P[i].rpy = rot_filters[i].passFilter(hsp->hp_wld_raw.P[i].rpy);
+    }
+    solveFullbodyIKStrictCOM( raw_pose.P[com], raw_pose.P[rf], raw_pose.P[lf], raw_pose.P[rh], raw_pose.P[lh], raw_pose.P[head], basepos_normal_ik, q_normal_ik );
+    rel_ref_zmp = m_robot->rootLink()->R.transpose() * (raw_pose.P[zmp].p - m_robot->rootLink()->p);
+  }else{
+    rel_ref_zmp = input_ref_zmp;
+  }
+}
+
+void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, hrp::Vector3& basepos_pre, hrp::dvector& q_pre){
   int com_ik_loop=0;
-  const int COM_IK_MAX_LOOP = 20;
-  const double COM_IK_MAX_ERROR = 1e-4;
-//  static hrp::Vector3 base_pos_pre;
-  m_robot->rootLink()->p = com_ref.p;//move base link at first
-//  if(solve_from_previous_basepos) m_robot->rootLink()->p = base_pos_pre;
+  const int COM_IK_MAX_LOOP = 10;
+  const double COM_IK_MAX_ERROR = 1e-5;
+  m_robot->rootLink()->p = 0.01*com_ref.p + 0.99*basepos_pre;//move base link at first
   m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
   const std::string names[4] = {"rleg","lleg","rarm","larm"};
   const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
@@ -633,24 +643,117 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, co
       fik->ikp[names[i]].target_p0 = refs[i]->p;
     }
   }
+  for( int i=0; i<m_robot->numJoints(); i++){ m_robot->joint(i)->q = q_pre(i); }
   if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref.rpy(y);
   if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref.rpy(p);
   //COM 収束ループ
-  hrp::Vector3 tmp_com_err;
+  hrp::Vector3 tmp_com_err = hrp::Vector3::Zero();
   while( 1 ){
     com_ik_loop++;
+    m_robot->rootLink()->p += tmp_com_err;
     m_robot->calcForwardKinematics();
     for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
         if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
     }
     tmp_com_err = com_ref.p - m_robot->calcCM();
-//    cout<<"tmp_com_err:"<<tmp_com_err.norm()<<endl;
     if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
-    m_robot->rootLink()->p += tmp_com_err;
     if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
   }
-  if(solve_from_previous_basepos)cout<<"com_ik_loop"<<com_ik_loop<<endl;
+  basepos_pre = m_robot->rootLink()->p;
+  for ( int i = 0; i < m_robot->numJoints(); i++ ){ q_pre[i] = m_robot->joint(i)->q; }
+  cout<<"com_ik_loop:"<<com_ik_loop<<endl;
 }
+//
+//void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, const bool solve_from_previous_basepos){
+//  int com_ik_loop=0;
+//  const int COM_IK_MAX_LOOP = 20;
+//  const double COM_IK_MAX_ERROR = 1e-4;
+////  static hrp::Vector3 base_pos_pre;
+//  static hrp::Vector3 com2base_offset_old;
+////  hrp::Vector3 com2base_offset = com2base_offset_old;
+//  hrp::Vector3 com2base_offset = 0.99 * com2base_offset_old;
+////  m_robot->rootLink()->p = com_ref.p + com2base_offset_old;//move base link at first
+//  m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
+//  const std::string names[4] = {"rleg","lleg","rarm","larm"};
+//  const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
+//  for(int i=0;i<4;i++){
+//    if(fik->ikp.count(names[i])){
+//      fik->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);//convert End Effector pos into link origin pos
+//      fik->ikp[names[i]].target_p0 = refs[i]->p;
+//    }
+//  }
+//  if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref.rpy(y);
+//  if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref.rpy(p);
+//  //COM 収束ループ
+//  hrp::Vector3 tmp_com_err;
+//  bool offset_vel_over = false;
+//  while( 1 ){
+//    com_ik_loop++;
+//    m_robot->rootLink()->p = com_ref.p + com2base_offset;
+//    m_robot->calcForwardKinematics();
+//    for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
+//        if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
+//    }
+//    m_robot->calcForwardKinematics();
+//    if(offset_vel_over){break;}
+//    tmp_com_err = com_ref.p - m_robot->calcCM();
+//    if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
+//    com2base_offset += tmp_com_err;
+//
+////    if((com2base_offset - com2base_offset_old).norm() > 0.5*m_dt){
+////      com2base_offset = com2base_offset_old + (com2base_offset - com2base_offset_old).normalized() * 0.5*m_dt;
+////      offset_vel_over = true;
+////      cout<<"offset_vel_over!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<endl;
+////    }
+////    cout<<"com2base_offset:"<<com2base_offset.transpose()<<endl;
+//
+//    if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
+//
+////    cout<<"tmp_com_err0:"<<tmp_com_err.transpose()<<endl;
+//  }
+//  com2base_offset_old = com2base_offset;
+////  base_pos_pre = m_robot->rootLink()->p;
+//  cout<<"com_ik_loop:"<<com_ik_loop<<endl;
+//}
+
+//void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, const bool solve_from_previous_basepos){
+//  int com_ik_loop=0;
+//  const int COM_IK_MAX_LOOP = 20;
+//  const double COM_IK_MAX_ERROR = 1e-4;
+//  static hrp::Vector3 base_pos_pre;
+//  m_robot->rootLink()->p = com_ref.p;//move base link at first
+//  if(solve_from_previous_basepos) m_robot->rootLink()->p = base_pos_pre;
+//  m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
+//  const std::string names[4] = {"rleg","lleg","rarm","larm"};
+//  const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
+//  for(int i=0;i<4;i++){
+//    if(fik->ikp.count(names[i])){
+//      fik->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);//convert End Effector pos into link origin pos
+//      fik->ikp[names[i]].target_p0 = refs[i]->p;
+//    }
+//  }
+//  if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref.rpy(y);
+//  if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref.rpy(p);
+//  //COM 収束ループ
+//  hrp::Vector3 tmp_com_err;
+//  while( 1 ){
+//    com_ik_loop++;
+//    m_robot->calcForwardKinematics();
+//    for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
+//        if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
+//    }
+//    tmp_com_err = com_ref.p - m_robot->calcCM();
+////    cout<<"tmp_com_err:"<<tmp_com_err.norm()<<endl;
+//    if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
+//    m_robot->rootLink()->p += tmp_com_err;
+//    if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
+//
+//    cout<<"tmp_com_err0:"<<tmp_com_err.transpose()<<endl;
+//    if(solve_from_previous_basepos)cout<<"tmp_com_err:"<<tmp_com_err.transpose()<<endl;
+//  }
+//  if(solve_from_previous_basepos)base_pos_pre = m_robot->rootLink()->p;
+//  if(solve_from_previous_basepos)cout<<"com_ik_loop:"<<com_ik_loop<<endl;
+//}
 
 
 //
@@ -757,23 +860,45 @@ void WholeBodyMasterSlave::calcDynamicsFilterCompensation(const hrp::Vector3 zmp
   m_robot->calcTotalMomentum(P,L);
   L = L - m_robot->calcCM().cross(P);
   static hrp::Vector3 torso_rot;
-  hrp::Matrix33 tmp;
-  m_robot->rootLink()->calcSubMassInertia(tmp);
-  cout<<"mass:"<<tmp<<endl;
-  if(tmp != hrp::Matrix33::Zero())torso_rot -= tmp.inverse() * L * m_dt * 0.3;
-//  torso_rot(0) -= L(0) * 0.01 * m_dt;
-//  torso_rot(1) -= L(1) * 0.03 * m_dt;
-//  torso_rot(2) -= L(2) * 0.03 * m_dt;
+  hrp::Vector3 torso_rot_vel;
+
+
+  m_robot->calcCM();
+  m_robot->rootLink()->calcSubMassCM();
+  hrp::Matrix33 I_com;
+
+  if(m_robot->link("CHEST_JOINT0")){
+    m_robot->link("CHEST_JOINT0")->calcSubMassInertia(I_com);
+//    cout<<"I_com:"<<endl<<I_com<<endl;
+//    double M = m_robot->link("CHEST_JOINT0")->subm;
+//    hrp::Vector3 a;
+//    hrp::Matrix33 shift_wld2com;
+//    shift_wld2com(0,0) = a(Y)*a(Y)+a(Z)*a(Z);  shift_wld2com(0,1) = -a(Y)*a(X);           shift_wld2com(0,2) = -a(Z)*a(X);
+//    shift_wld2com(1,0) = -a(X)*a(Y);           shift_wld2com(1,1) = a(X)*a(X)+a(Z)*a(Z);  shift_wld2com(1,2) = -a(Z)*a(Y);
+//    shift_wld2com(2,0) = -a(X)*a(Z);           shift_wld2com(2,1) = -a(Y)*a(Z);           shift_wld2com(2,2) = a(X)*a(X)+a(Y)*a(Y);
+    torso_rot_vel = - I_com.inverse() * L * hsp->WBMSparam.upper_body_rmc_ratio;
+    torso_rot += torso_rot_vel * m_dt;
+  }
 //  fprintf(hsp->id_log,"p: %f %f ",hsp->rp_ref_out.P[com].p(0),hsp->rp_ref_out.P[com].p(1),hsp->rp_ref_out.P[com].p(2));
 //  fprintf(hsp->id_log,"rpy: %f %f ",hsp->rp_ref_out.P[com].rpy(0),hsp->rp_ref_out.P[com].rpy(1),hsp->rp_ref_out.P[com].rpy(2));
 //  fprintf(hsp->id_log,"P: %f %f ",P(0),P(1),P(2));
 //  fprintf(hsp->id_log,"L: %f %f ",L(0),L(1),L(2));
 //  fprintf(hsp->id_log,"\n");
+  static hrp::Vector3 basepos_dyn_ik = m_robot->rootLink()->p;
+  static hrp::dvector q_dyn_ik;
+  static int callnum;
+  if(callnum++ == 0){
+    q_dyn_ik.resize(m_robot->numJoints());
+    for( int i=0; i<m_robot->numJoints(); i++){ q_dyn_ik(i) = m_robot->joint(i)->q; }
+  }
+//  for( int i=0; i<m_robot->numJoints(); i++){ q_dyn_ik(i) = q_normal_ik(i); }
   if(hsp->isHumanSyncOn()){
     HRPPose3D com_mod = hsp->rp_ref_out.P[com];
 //    com_mod.p += torso_pos;
     com_mod.rpy += torso_rot;
-    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head], true);
+
+    cout<<"calcDynamicsFilterCompensation:"<<endl;
+    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head], basepos_dyn_ik, q_dyn_ik);
   }
 }
 
@@ -812,6 +937,7 @@ bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBody
   hsp->WBMSparam.set_com_height_fix_val = i_param.set_com_height_fix_val;
   hsp->WBMSparam.swing_foot_height_offset = i_param.swing_foot_height_offset;
   hsp->WBMSparam.swing_foot_max_height = i_param.swing_foot_max_height;
+  hsp->WBMSparam.upper_body_rmc_ratio = i_param.upper_body_rmc_ratio;
   hsp->WBMSparam.use_rh = hsp->WBMSparam.use_lh = i_param.use_hands;
   hsp->WBMSparam.use_head = i_param.use_head;
   return true;
@@ -827,6 +953,7 @@ bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMaster
   i_param.set_com_height_fix_val = hsp->WBMSparam.set_com_height_fix_val;
   i_param.swing_foot_height_offset = hsp->WBMSparam.swing_foot_height_offset;
   i_param.swing_foot_max_height = hsp->WBMSparam.swing_foot_max_height;
+  i_param.upper_body_rmc_ratio = hsp->WBMSparam.upper_body_rmc_ratio;
   i_param.use_hands = (hsp->WBMSparam.use_rh || hsp->WBMSparam.use_lh);
   i_param.use_head = hsp->WBMSparam.use_head;
   return true;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 10d83bcde33..50cbaabaaf7 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -153,7 +153,7 @@ class WholeBodyMasterSlave
     hrp::Link* target_link;
     bool is_active, has_toe_joint;
   };
-  void solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, const bool solve_from_previous_basepos = false);
+  void solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, hrp::Vector3& basepos_pre, hrp::dvector& q_pre);
   void processWholeBodyMasterSlave();
   void processWholeBodyMasterSlave_Raw();
   void calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody);
@@ -187,6 +187,8 @@ class WholeBodyMasterSlave
 
   hrp::Vector3 ref_zmp_invdyn2;
 
+  hrp::dvector q_normal_ik;
+
   boost::shared_ptr<HumanSynchronizer> hsp;
 };
 
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index abfb7775921..32a734bdfb4 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -121,9 +121,9 @@ class HumanPose{
       for(std::vector<Wrench6>::iterator it = w.begin(); it != w.end(); it++){ it->clear(); }
     }
     static void hp_printf(const HumanPose& in){
-      const std::string cap[] = {"c","rf","lf","rh","lh","z","rw","lw"};
-      for(int i=com;i<=zmp;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",cap[i].c_str(),in.P[i].p(X),in.P[i].p(Y),in.P[i].p(Z)); }
-      for(int i=rfw;i<lfw;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",cap[i].c_str(),in.w[i].w(fx),in.w[i].w(fy),in.w[i].w(fz),in.w[i].w(tx),in.w[i].w(ty),in.w[i].w(tz)); }
+      const std::string pcap[] = {"c","rf","lf","rh","lh","hd","z"}, wcap[] = {"rw","lw"};
+      for(int i=0;i<num_pose_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",pcap[i].c_str(),in.P[i].p(X),in.P[i].p(Y),in.P[i].p(Z)); }
+//      for(int i=0;i<num_wrench_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",wcap[i].c_str(),in.w[i].w(fx),in.w[i].w(fy),in.w[i].w(fz),in.w[i].w(tx),in.w[i].w(ty),in.w[i].w(tz)); }
       printf("\n");
     }
     void print(){ hp_printf(*this); }
@@ -216,6 +216,7 @@ class HumanSynchronizer{
       double set_com_height_fix_val;
       double swing_foot_height_offset;
       double swing_foot_max_height;
+      double upper_body_rmc_ratio;
       bool use_rh,use_lh;
       bool use_head;
     };
@@ -272,7 +273,7 @@ class HumanSynchronizer{
       for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//四肢拘束点用(Rotation)
       tgt_pos_filters[0].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//重心pos用
       tgt_rot_filters[0].setParameter(0.6, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//重心rot用
-      tgt_pos_filters[1].setParameter(hrp::Vector3(1.0,1.0,1.2), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//右足pos用
+      tgt_pos_filters[1].setParameter(hrp::Vector3(5.0,1.0,1.2), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//右足pos用
       tgt_pos_filters[2].setParameter(hrp::Vector3(1.0,1.0,1.2), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//左足pos用
       calcacc_v_filters.setParameter(5, HZ, BiquadIIRFilterVec::Q_BUTTERWORTH);//加速度計算用
       acc4zmp_v_filters.setParameter(5, HZ, BiquadIIRFilterVec::Q_BUTTERWORTH);//ZMP生成用ほぼこの値でいい
@@ -289,18 +290,17 @@ class HumanSynchronizer{
       lf_vert.push_back(hrp::Vector3(-0.10,0.08,0));
       lf_vert.push_back(hrp::Vector3(0.13,0.08,0));
 
-      WBMSparam.use_rh = WBMSparam.use_lh = true;
-      WBMSparam.use_head = true;
-//      WBMSparam.set_com_height_fix = true;
-      WBMSparam.set_com_height_fix = false;
-      WBMSparam.set_com_height_fix_val = 0.02;
+      WBMSparam.auto_swing_foot_landing_threshold = 0.02;
       WBMSparam.foot_vertical_vel_limit_coeff = 4.0;
       WBMSparam.human_com_height = 1.00;
+      WBMSparam.is_doctor = true;
+      WBMSparam.set_com_height_fix = false;
+      WBMSparam.set_com_height_fix_val = 0.02;
       WBMSparam.swing_foot_height_offset = 0.01;
-//      WBMSparam.swing_foot_max_height = 0.10;
       WBMSparam.swing_foot_max_height = 0.5;
-      WBMSparam.auto_swing_foot_landing_threshold = 0.02;
-      WBMSparam.is_doctor = true;
+      WBMSparam.upper_body_rmc_ratio = 0.5;
+      WBMSparam.use_rh = WBMSparam.use_lh = true;
+      WBMSparam.use_head = true;
 
       rp_ref_out_old.clear();
       rp_ref_out.clear();
@@ -363,6 +363,7 @@ class HumanSynchronizer{
       hp_plot = rp_ref_out;
       if(loop==0)rp_ref_out_old = rp_ref_out;
 
+
       if(isHumanSyncOn() && WBMSparam.set_com_height_fix)rp_ref_out.P[com].p(Z) = rp_ref_out.P[com].p_offs(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
       judgeFootLandOnCommand              (hp_wld_raw.w[rfw], hp_wld_raw.w[lfw], go_rf_landing, go_lf_landing);
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
@@ -471,8 +472,8 @@ class HumanSynchronizer{
       const int ns[] = {com,rf,lf,rh,lh,head}, num_ns = sizeof(ns)/sizeof(int);
       if(loop==0){
         cout<<"init LPF"<<endl;
-        for(int i=0;i<num_ns;i++){ tgt_pos_filters[ns[i]].init(tgt.P[ns[i]].p);   tgt_pos_filters[ns[i]].passFilter(tgt.P[ns[i]].p);  }
-        for(int i=0;i<num_ns;i++){ tgt_rot_filters[ns[i]].init(tgt.P[ns[i]].rpy); tgt_pos_filters[ns[i]].passFilter(tgt.P[ns[i]].rpy);}
+        for(int i=0;i<num_ns;i++){ tgt_pos_filters[ns[i]].init(tgt.P[ns[i]].p);  }// tgt_pos_filters[ns[i]].passFilter(tgt.P[ns[i]].p);  }
+        for(int i=0;i<num_ns;i++){ tgt_rot_filters[ns[i]].init(tgt.P[ns[i]].rpy); }//tgt_pos_filters[ns[i]].passFilter(tgt.P[ns[i]].rpy);}
       }
       for(int i=0;i<num_ns;i++){ tgt.P[ns[i]].p   = tgt_pos_filters[ns[i]].passFilter(tgt.P[ns[i]].p);  }
       for(int i=0;i<num_ns;i++){ tgt.P[ns[i]].rpy = tgt_rot_filters[ns[i]].passFilter(tgt.P[ns[i]].rpy);}

From 3bb83c01c73c939ce6c299ebade97d3758398eed Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 13 May 2017 01:29:57 +0900
Subject: [PATCH 084/217] sperate m_robot

---
 .../WholeBodyMasterSlave.cpp                  | 640 +++++-------------
 .../WholeBodyMasterSlave.h                    |  23 +-
 2 files changed, 192 insertions(+), 471 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 55760d0cf1f..91f22e9522a 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -1,12 +1,3 @@
-// -*- C++ -*-
-/*!
- * @file  WholeBodyMasterSlave.cpp
- * @brief WholeBodyMasterSlave component
- * $Date$
- *
- * $Id$
- */
-
 #include <rtm/CorbaNaming.h>
 #include <hrpModel/Link.h>
 #include <hrpModel/Sensor.h>
@@ -16,7 +7,6 @@
 #include <hrpUtil/MatrixSolvers.h>
 #include "hrpsys/util/Hrpsys.h"
 
-
 typedef coil::Guard<coil::Mutex> Guard;
 using namespace rats;
 
@@ -32,7 +22,6 @@ static const char* WholeBodyMasterSlave_spec[] =
         "max_instance",      "10",
         "language",          "C++",
         "lang_type",         "compile",
-        // Configuration variables
         "conf.default.debugLevel", "0",
         ""
     };
@@ -154,6 +143,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
     coil::stringTo(m_dt, prop["dt"].c_str());
 
     m_robot = hrp::BodyPtr(new hrp::Body());
+    m_robot_rmc = hrp::BodyPtr(new hrp::Body());
     RTC::Manager& rtcManager = RTC::Manager::instance();
     std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
     int comPos = nameServer.find(",");
@@ -162,12 +152,14 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
     }
     nameServer = nameServer.substr(0, comPos);
     RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
-    if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
-                                 CosNaming::NamingContext::_duplicate(naming.getRootContext())
-                                 )){
+    if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
       std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
       return RTC::RTC_ERROR;
     }
+    if (!loadBodyFromModelLoader(m_robot_rmc, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
+      std::cerr << "[" << m_profile.instance_name << "] failed to load model 2 [" << prop["model"] << "]" << std::endl;
+      return RTC::RTC_ERROR;
+    }
 
     // allocate memory for outPorts
     m_qRef.data.length(m_robot->numJoints());
@@ -175,7 +167,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
     m_htlfw.data.length(6);
     loop = 0;
 
-
     q_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
     q_interpolator->setName(std::string(m_profile.instance_name)+" q_interpolator");
     q_interpolator_ratio = 0;
@@ -185,7 +176,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
     tmp_ratio = 1.0;
     q_interpolator->setGoal(&tmp_ratio, 3.0, true);
 
-
 //    // setting from conf file
 //    // GaitGenerator requires abc_leg_offset and abc_stride_parameter in robot conf file
 //    // setting leg_pos from conf file
@@ -195,56 +185,11 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
 //
     // Generate FIK
     fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
+    fik_rmc = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot_rmc, std::string(m_profile.instance_name), m_dt));
+
+    setupfik(fik, m_robot, prop);
+    setupfik(fik_rmc, m_robot_rmc, prop);
 
-    // setting from conf file
-    // rleg,TARGET_LINK,BASE_LINK
-    coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
-    size_t prop_num = 10;
-    if (end_effectors_str.size() > 0) {
-      size_t num = end_effectors_str.size()/prop_num;
-      for (size_t i = 0; i < num; i++) {
-        std::string ee_name, ee_target, ee_base;
-        coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
-        coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
-        coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
-        ABCIKparam tp;
-        for (size_t j = 0; j < 3; j++) {
-            coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
-        }
-        double tmpv[4];
-        for (int j = 0; j < 4; j++ ) {
-          coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
-        }
-        tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
-        // FIK param
-        SimpleFullbodyInverseKinematicsSolver::IKparam tmp_fikp;
-        tmp_fikp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
-        tmp_fikp.target_link = m_robot->link(ee_target);
-        tmp_fikp.localPos = tp.localPos;
-        tmp_fikp.localR = tp.localR;
-        fik->ikp.insert(std::pair<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>(ee_name, tmp_fikp));
-        // Fix for toe joint
-        //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
-        //   Without toe joints, "end-effector link == force-sensor link" is assumed.
-        //   With toe joints, "end-effector link != force-sensor link" is assumed.
-        if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
-            std::vector<double> optw(fik->ikp[ee_name].manip->numJoints(), 1.0);
-            optw.back() = 0.0; // Set weight = 0 for toe joint by default
-            fik->ikp[ee_name].manip->setOptionalWeightVector(optw);
-            tp.has_toe_joint = true;
-        } else {
-            tp.has_toe_joint = false;
-        }
-        tp.target_link = m_robot->link(ee_target);
-//        ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
-//        ee_vec.push_back(ee_name);
-        std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   target = " << fik->ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
-        contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
-      }
-    }
     if (fik->ikp.find("rleg") != fik->ikp.end() && fik->ikp.find("lleg") != fik->ikp.end()) {
       is_legged_robot = true;
     } else {
@@ -256,6 +201,60 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
     return RTC::RTC_OK;
 }
 
+
+void WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in){
+  // setting from conf file
+  // rleg,TARGET_LINK,BASE_LINK
+  coil::vstring end_effectors_str = coil::split(prop_in["end_effectors"], ",");
+  size_t prop_num = 10;
+  if (end_effectors_str.size() > 0) {
+    size_t num = end_effectors_str.size()/prop_num;
+    for (size_t i = 0; i < num; i++) {
+      std::string ee_name, ee_target, ee_base;
+      coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
+      coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
+      coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
+      ABCIKparam tp;
+      for (size_t j = 0; j < 3; j++) {
+          coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
+      }
+      double tmpv[4];
+      for (int j = 0; j < 4; j++ ) {
+        coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
+      }
+      tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
+      // FIK param
+      SimpleFullbodyInverseKinematicsSolver::IKparam tmp_fikp;
+      tmp_fikp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(robot_in, robot_in->link(ee_base), robot_in->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
+      tmp_fikp.target_link = robot_in->link(ee_target);
+      tmp_fikp.localPos = tp.localPos;
+      tmp_fikp.localR = tp.localR;
+      fik_in->ikp.insert(std::pair<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>(ee_name, tmp_fikp));
+      // Fix for toe joint
+      //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
+      //   Without toe joints, "end-effector link == force-sensor link" is assumed.
+      //   With toe joints, "end-effector link != force-sensor link" is assumed.
+      if (robot_in->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
+          std::vector<double> optw(fik_in->ikp[ee_name].manip->numJoints(), 1.0);
+          optw.back() = 0.0; // Set weight = 0 for toe joint by default
+          fik_in->ikp[ee_name].manip->setOptionalWeightVector(optw);
+          tp.has_toe_joint = true;
+      } else {
+          tp.has_toe_joint = false;
+      }
+      tp.target_link = robot_in->link(ee_target);
+//        ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
+//        ee_vec.push_back(ee_name);
+      std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   target = " << fik_in->ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
+      contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));////TODO:要移動
+    }
+  }
+
+}
+
 RTC::ReturnCode_t WholeBodyMasterSlave::onFinalize()
 {
   return RTC::RTC_OK;
@@ -273,34 +272,17 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onDeactivated(RTC::UniqueId ec_id)
   return RTC::RTC_OK;
 }
 
-//#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
 #define DEBUGP ((loop%200==0))
 RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
 {
 //  std::cerr << "[" << m_profile.instance_name<< "] onExecute(" << ec_id << ")" << std::endl;
-    // Read Inport
-    if (m_qRefIn.isNew()) {
-        m_qRefIn.read();
-    }
-    if (m_basePosIn.isNew()) {
-      m_basePosIn.read();
-      input_basePos(0) = m_basePos.data.x;
-      input_basePos(1) = m_basePos.data.y;
-      input_basePos(2) = m_basePos.data.z;
-    }
-    if (m_baseRpyIn.isNew()) {
-      m_baseRpyIn.read();
-      input_baseRot = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
-    }
-    if (m_zmpIn.isNew()) {
-      m_zmpIn.read();
-      input_ref_zmp(0) = m_zmp.data.x;
-      input_ref_zmp(1) = m_zmp.data.y;
-      input_ref_zmp(2) = m_zmp.data.z;
-    }
-    if (m_optionalDataIn.isNew()) {
-        m_optionalDataIn.read();
-    }
+  // struct timeval t_calc_start, t_calc_end;
+  // gettimeofday(&t_calc_start, NULL);
+    if (m_qRefIn.isNew()) { m_qRefIn.read(); }
+    if (m_basePosIn.isNew()) { m_basePosIn.read(); }
+    if (m_baseRpyIn.isNew()) { m_baseRpyIn.read(); }
+    if (m_zmpIn.isNew()) { m_zmpIn.read(); }
+    if (m_optionalDataIn.isNew()) { m_optionalDataIn.read(); }
     //for HumanSynchronizer
     if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hsp->hp_wld_raw.w[rfw]); }
     if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hsp->hp_wld_raw.w[lfw]); }
@@ -320,22 +302,29 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
         q_interpolator->get(&q_interpolator_ratio, true);
       }
 
-      hsp->baselinkpose.p = m_robot->rootLink()->p;
-      hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
-
-      if(!hsp->isHumanSyncOn()){
-        m_robot->rootLink()->p = input_basePos;
-        m_robot->rootLink()->R = input_baseRot;
-        for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
-          m_robot->joint(i)->q = m_qRef.data[i];
-        }
+      if(!hsp->isHumanSyncOn()){//初期姿勢取得＆FK
+        m_robot->rootLink()->p = m_robot_rmc->rootLink()->p = hrp::Vector3(m_basePos.data.x, m_basePos.data.y, m_basePos.data.z);
+        m_robot->rootLink()->R = m_robot_rmc->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
+        for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ m_robot->joint(i)->q = m_robot_rmc->joint(i)->q = m_qRef.data[i]; }
         m_robot->calcForwardKinematics();
+        m_robot_rmc->calcForwardKinematics();
+
+        fik->setReferenceJointAngles();
+        fik_rmc->setReferenceJointAngles();
       }
 
+      fik->current_tm = fik_rmc->current_tm = m_qRef.tm;
+      fik->ikp["rleg"].is_ik_enable = fik_rmc->ikp["rleg"].is_ik_enable = true;
+      fik->ikp["lleg"].is_ik_enable = fik_rmc->ikp["lleg"].is_ik_enable = true;
+      fik->ikp["rarm"].is_ik_enable = fik_rmc->ikp["rarm"].is_ik_enable = true;
+      fik->ikp["larm"].is_ik_enable = fik_rmc->ikp["larm"].is_ik_enable = true;
+//      fik->ikp["rarm"].is_ik_enable = fik_rmc->ikp["rarm"].is_ik_enable = false;
+//      fik->ikp["larm"].is_ik_enable = fik_rmc->ikp["larm"].is_ik_enable = false;
+
       if(hsp->WBMSparam.is_doctor){
-        processWholeBodyMasterSlave();
+        processWholeBodyMasterSlave(fik, m_robot);
       }else{
-        processWholeBodyMasterSlave_Raw();
+        processWholeBodyMasterSlave_Raw(fik, m_robot);
       }
 
       /////// Inverse Dynamics /////////
@@ -360,52 +349,50 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
       updateInvDynStateBuffer(idsb);
 
       if(hsp->isHumanSyncOn() && hsp->WBMSparam.is_doctor){
-        calcDynamicsFilterCompensation(ref_zmp, ref_zmp_invdyn);
-        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.P[zmp].p - m_robot->rootLink()->p);//baseのrpy変わるからもう一回
+        processMomentumCompensation(fik_rmc, m_robot_rmc);
       }
 
-      calcAccelerationsForInverseDynamics(m_robot, idsb2);
-      calcWorldZMPFromInverseDynamics(m_robot, idsb2, ref_zmp_invdyn2);
-      updateInvDynStateBuffer(idsb2);
+//      calcAccelerationsForInverseDynamics(m_robot, idsb2);
+//      calcWorldZMPFromInverseDynamics(m_robot, idsb2, ref_zmp_invdyn2);
+//      updateInvDynStateBuffer(idsb2);
 
 //        ref_zmp = ref_zmp_invdyn;
 //        ref_zmp = ref_zmp_invdyn2;
-//        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
-    }
+      ref_zmp = hsp->rp_ref_out.P[zmp].p;
+//      hrp::BodyPtr m_robot_for_out = m_robot;
+      hrp::BodyPtr m_robot_for_out = m_robot_rmc;
 
-    // Write Outport
-    if ( m_qRef.data.length() != 0 ) { // initialized
-      if (is_legged_robot) {
-        for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
-//          m_qRef.data[i] = m_robot->joint(i)->q;
-          m_qRef.data[i] = q_interpolator_ratio * m_robot->joint(i)->q  + (1 - q_interpolator_ratio) * m_qRef.data[i];
+      if (hsp->isHumanSyncOn()) {
+        for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ){
+          m_qRef.data[i] = q_interpolator_ratio * m_robot_for_out->joint(i)->q  + (1 - q_interpolator_ratio) * m_qRef.data[i];
         }
+        // basePos
+        m_basePos.data.x = m_robot_for_out->rootLink()->p(0);
+        m_basePos.data.y = m_robot_for_out->rootLink()->p(1);
+        m_basePos.data.z = m_robot_for_out->rootLink()->p(2);
+        m_basePos.tm = m_qRef.tm;
+        // baseRpy
+        hrp::Vector3 baseRpy = hrp::rpyFromRot(m_robot_for_out->rootLink()->R);
+        m_baseRpy.data.r = baseRpy(0);
+        m_baseRpy.data.p = baseRpy(1);
+        m_baseRpy.data.y = baseRpy(2);
+        m_baseRpy.tm = m_qRef.tm;
+        // zmp
+        rel_ref_zmp = m_robot_for_out->rootLink()->R.transpose() * (ref_zmp - m_robot_for_out->rootLink()->p);
+        m_zmp.data.x = rel_ref_zmp(0);
+        m_zmp.data.y = rel_ref_zmp(1);
+        m_zmp.data.z = rel_ref_zmp(2);
+        m_zmp.tm = m_qRef.tm;
       }
-      m_qOut.write();
-    }
-    if (is_legged_robot) {
-      // basePos
-      m_basePos.data.x = m_robot->rootLink()->p(0);
-      m_basePos.data.y = m_robot->rootLink()->p(1);
-      m_basePos.data.z = m_robot->rootLink()->p(2);
-      m_basePos.tm = m_qRef.tm;
-      // baseRpy
-      hrp::Vector3 baseRpy = hrp::rpyFromRot(m_robot->rootLink()->R);
-      m_baseRpy.data.r = baseRpy(0);
-      m_baseRpy.data.p = baseRpy(1);
-      m_baseRpy.data.y = baseRpy(2);
-      m_baseRpy.tm = m_qRef.tm;
-      // zmp
-      m_zmp.data.x = rel_ref_zmp(0);
-      m_zmp.data.y = rel_ref_zmp(1);//debug
-      m_zmp.data.z = rel_ref_zmp(2);
-       m_zmp.tm = m_qRef.tm;
-      // write
-      m_basePosOut.write();
-      m_baseRpyOut.write();
-      m_zmpOut.write();
-      m_optionalDataOut.write();
+      hsp->baselinkpose.p = m_robot->rootLink()->p;
+      hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
     }
+    // write
+     m_qOut.write();
+    m_basePosOut.write();
+    m_baseRpyOut.write();
+    m_zmpOut.write();
+    m_optionalDataOut.write();
 
 #ifdef USE_DEBUG_PORT
     // dbg plot
@@ -470,22 +457,14 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
      HumanSynchronizer::Wrench6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
      m_invdyn_dbgOut.write();
 #endif
+     // gettimeofday(&t_calc_end, NULL);
+     //  if(DEBUGP)cout<<"t1:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl;
+     //  t_calc_start = t_calc_end;
     loop ++;
     return RTC::RTC_OK;
 }
 
-void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
-
-// struct timeval t_calc_start, t_calc_end;
-// gettimeofday(&t_calc_start, NULL);
-
-  fik->current_tm = m_qRef.tm;
-  fik->ikp["rleg"].is_ik_enable = true;
-  fik->ikp["lleg"].is_ik_enable = true;
-//  fik->ikp["rarm"].is_ik_enable = true;
-//  fik->ikp["larm"].is_ik_enable = true;
-  fik->ikp["rarm"].is_ik_enable = false;
-  fik->ikp["larm"].is_ik_enable = false;
+void WholeBodyMasterSlave::processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in){
 
   if(hsp->startCountdownForHumanSync){
     std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
@@ -497,69 +476,54 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(){
       hsp->ht_first_call = false;
     }
   }else{
-    fik->storeCurrentParameters();
-    fik->setReferenceJointAngles();
+    fik_in->storeCurrentParameters();
+    fik_in->setReferenceJointAngles();
     hsp->setCurrentInputAsOffset(hsp->hp_wld_raw);
     hsp->calibInitHumanCOMFromZMP();
 
     const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
     const int human_l_names[4] = {rf,lf,rh,lh};
     for(int i=0;i<4;i++){
-      if(fik->ikp.count(robot_l_names[i])){
-        hsp->rp_ref_out.P[human_l_names[i]].p_offs = fik->ikp[robot_l_names[i]].target_link->p + fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localPos;
-        hsp->rp_ref_out.P[human_l_names[i]].rpy_offs = hrp::rpyFromRot(fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localR);
+      if(fik_in->ikp.count(robot_l_names[i])){
+        hsp->rp_ref_out.P[human_l_names[i]].p_offs = fik_in->ikp[robot_l_names[i]].target_link->p + fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localPos;
+        hsp->rp_ref_out.P[human_l_names[i]].rpy_offs = hrp::rpyFromRot(fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localR);
         hsp->rp_ref_out.P[human_l_names[i]].p = hsp->rp_ref_out.P[human_l_names[i]].p_offs;
         hsp->rp_ref_out.P[human_l_names[i]].rpy = hsp->rp_ref_out.P[human_l_names[i]].rpy_offs;
       }
     }
-    //    hsp->rp_ref_out.P[com].p_offs = m_robot->calcCM();
+    //    hsp->rp_ref_out.P[com].p_offs = robot_in->calcCM();
     hsp->rp_ref_out.P[com].p_offs(0) = (hsp->rp_ref_out.P[rf].p_offs(0) + hsp->rp_ref_out.P[lf].p_offs(0)) / 2;
     hsp->rp_ref_out.P[com].p_offs(1) = (hsp->rp_ref_out.P[rf].p_offs(1) + hsp->rp_ref_out.P[lf].p_offs(1)) / 2;
-    hsp->rp_ref_out.P[com].p_offs(2) = m_robot->calcCM()(2);
+    hsp->rp_ref_out.P[com].p_offs(2) = robot_in->calcCM()(2);
     //    hsp->rp_ref_out.P[zmp].p_offs = ref_zmp;
     hsp->pre_cont_rfpos = hsp->rp_ref_out.P[rf].p_offs;
     hsp->pre_cont_lfpos = hsp->rp_ref_out.P[lf].p_offs;
-    hsp->baselinkpose.p_offs = m_robot->rootLink()->p;
-    hsp->baselinkpose.rpy_offs = hrp::rpyFromRot(m_robot->rootLink()->R);
+    hsp->baselinkpose.p_offs = robot_in->rootLink()->p;
+    hsp->baselinkpose.rpy_offs = hrp::rpyFromRot(robot_in->rootLink()->R);
   }
 
-// gettimeofday(&t_calc_end, NULL); 
-//  if(DEBUGP)cout<<"t1:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl;
-//  t_calc_start = t_calc_end;
-
-  hrp::Vector3 rsole_pos = fik->ikp["rleg"].target_link->p+ fik->ikp["rleg"].target_link->R * fik->ikp["rleg"].localPos;
-  hrp::Vector3 lsole_pos = fik->ikp["lleg"].target_link->p+ fik->ikp["lleg"].target_link->R * fik->ikp["lleg"].localPos;
-  hsp->H_cur = m_robot->calcCM()(2) - std::min((double)rsole_pos(2), (double)lsole_pos(2));
+  hrp::Vector3 rsole_pos = fik_in->ikp["rleg"].target_link->p+ fik_in->ikp["rleg"].target_link->R * fik_in->ikp["rleg"].localPos;
+  hrp::Vector3 lsole_pos = fik_in->ikp["lleg"].target_link->p+ fik_in->ikp["lleg"].target_link->R * fik_in->ikp["lleg"].localPos;
+  hsp->H_cur = robot_in->calcCM()(2) - std::min((double)rsole_pos(2), (double)lsole_pos(2));
 
   hsp->update();//////HumanSynchronizerの主要処理
   if(DEBUGP)cout<<"update():"<<hsp->getUpdateTime()<<endl;
   if(DEBUGP)hsp->rp_ref_out.print();
 
-  m_robot->calcForwardKinematics();
-  static hrp::Vector3 basepos_normal_ik = m_robot->rootLink()->p;
-//  static hrp::dvector q_normal_ik;
-  q_normal_ik.resize(m_robot->numJoints());
-  if(!hsp->isHumanSyncOn()){
-    for( int i=0; i<m_robot->numJoints(); i++){ q_normal_ik(i) = m_robot->joint(i)->q; }
-  }
+  robot_in->calcForwardKinematics();
   if(hsp->isHumanSyncOn()){
-    cout<<"processWholeBodyMasterSlave:"<<endl;
-    solveFullbodyIKStrictCOM( hsp->rp_ref_out.P[com], hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head], basepos_normal_ik, q_normal_ik);
-    //outport用のデータ上書き
+    solveFullbodyIKStrictCOM(fik, m_robot, hsp->rp_ref_out.P[com], hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head],"processWholeBodyMasterSlave");
     hsp->rp_ref_out.P[zmp].p(2) = (hsp->rp_ref_out.P[rf].p(2) + hsp->rp_ref_out.P[lf].p(2))/2;//体幹相対ZMP高さ設定
-    rel_ref_zmp = m_robot->rootLink()->R.transpose() * (hsp->rp_ref_out.P[zmp].p - m_robot->rootLink()->p);
     if(m_optionalData.data.length() < 4*2){
       m_optionalData.data.length(4*2);//これいいのか？
       for(int i=0;i<4*2;i++)m_optionalData.data[i] = 0;
     }
     m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[contact_states_index_map["rleg"]+4] = hsp->is_rf_contact;
     m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[contact_states_index_map["lleg"]+4] = hsp->is_lf_contact;
-  }else{
-    rel_ref_zmp = input_ref_zmp;
   }
 }
 
-void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(){
+void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::BodyPtr& robot_in){
   static HumanPose raw_pose;
   static BiquadIIRFilterVec pos_filters[num_pose_tgt], rot_filters[num_pose_tgt];
   static unsigned int callnum;
@@ -570,12 +534,6 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(){
     }
   }
 
-  fik->current_tm = m_qRef.tm;
-  fik->ikp["rleg"].is_ik_enable = true;
-  fik->ikp["lleg"].is_ik_enable = true;
-  fik->ikp["rarm"].is_ik_enable = true;
-  fik->ikp["larm"].is_ik_enable = true;
-
   if(hsp->startCountdownForHumanSync){
    std::cerr << "[" << m_profile.instance_name << "] Count Down for RawSync ["<<hsp->getRemainingCountDown()<<"]\r";
    hsp->updateCountDown();
@@ -587,19 +545,19 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(){
    }
   }else{
    //初期状態取得
-   fik->storeCurrentParameters();
-   fik->setReferenceJointAngles();
+   fik_in->storeCurrentParameters();
+   fik_in->setReferenceJointAngles();
    const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
    const int human_l_names[4] = {rf,lf,rh,lh};
-   raw_pose.P[com].p = raw_pose.P[com].p_offs = m_robot->calcCM();
-   raw_pose.P[com].rpy = raw_pose.P[com].rpy_offs = hrp::rpyFromRot(m_robot->rootLink()->R);
+   raw_pose.P[com].p = raw_pose.P[com].p_offs = robot_in->calcCM();
+   raw_pose.P[com].rpy = raw_pose.P[com].rpy_offs = hrp::rpyFromRot(robot_in->rootLink()->R);
    pos_filters[com].init(raw_pose.P[com].p);
    rot_filters[com].init(raw_pose.P[com].rpy);
    pos_filters[zmp].init(hrp::Vector3::Zero());
    for(int i=0;i<4;i++){
-     if(fik->ikp.count(robot_l_names[i])){
-       raw_pose.P[human_l_names[i]].p_offs = fik->ikp[robot_l_names[i]].target_link->p + fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localPos;
-       raw_pose.P[human_l_names[i]].rpy_offs = hrp::rpyFromRot(fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localR);
+     if(fik_in->ikp.count(robot_l_names[i])){
+       raw_pose.P[human_l_names[i]].p_offs = fik_in->ikp[robot_l_names[i]].target_link->p + fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localPos;
+       raw_pose.P[human_l_names[i]].rpy_offs = hrp::rpyFromRot(fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localR);
        raw_pose.P[human_l_names[i]].p = hsp->rp_ref_out.P[human_l_names[i]].p_offs;
        raw_pose.P[human_l_names[i]].rpy = hsp->rp_ref_out.P[human_l_names[i]].rpy_offs;
        pos_filters[human_l_names[i]].init(raw_pose.P[human_l_names[i]].p);
@@ -607,253 +565,56 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(){
      }
    }
   }
-
-  m_robot->calcForwardKinematics();
+  robot_in->calcForwardKinematics();
   if(DEBUGP){ fprintf(stderr,"\x1b[31mmaster-mode:\x1b[39m"); raw_pose.print(); }
-
-  static hrp::Vector3 basepos_normal_ik = m_robot->rootLink()->p;
-//  static hrp::dvector q_normal_ik;
   if(!hsp->isHumanSyncOn()){
-    q_normal_ik.resize(m_robot->numJoints());
-    for( int i=0; i<m_robot->numJoints(); i++){ q_normal_ik(i) = m_robot->joint(i)->q; }
+    q_normal_ik.resize(robot_in->numJoints());
+    for( int i=0; i<robot_in->numJoints(); i++){ q_normal_ik(i) = robot_in->joint(i)->q; }
   }
   if(hsp->isHumanSyncOn()){
     for(int i=0;i<num_pose_tgt;i++){
       raw_pose.P[i].p = pos_filters[i].passFilter(hsp->hp_wld_raw.P[i].p);
       raw_pose.P[i].rpy = rot_filters[i].passFilter(hsp->hp_wld_raw.P[i].rpy);
     }
-    solveFullbodyIKStrictCOM( raw_pose.P[com], raw_pose.P[rf], raw_pose.P[lf], raw_pose.P[rh], raw_pose.P[lh], raw_pose.P[head], basepos_normal_ik, q_normal_ik );
-    rel_ref_zmp = m_robot->rootLink()->R.transpose() * (raw_pose.P[zmp].p - m_robot->rootLink()->p);
-  }else{
-    rel_ref_zmp = input_ref_zmp;
+    solveFullbodyIKStrictCOM(fik, m_robot, raw_pose.P[com], raw_pose.P[rf], raw_pose.P[lf], raw_pose.P[rh], raw_pose.P[lh], raw_pose.P[head] );
   }
 }
 
-void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, hrp::Vector3& basepos_pre, hrp::dvector& q_pre){
+void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, const std::string& debug_prefix){
   int com_ik_loop=0;
   const int COM_IK_MAX_LOOP = 10;
-  const double COM_IK_MAX_ERROR = 1e-5;
-  m_robot->rootLink()->p = 0.01*com_ref.p + 0.99*basepos_pre;//move base link at first
-  m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
+  const double COM_IK_MAX_ERROR = 1e-5;//1e-4だと乱れる
+//  robot_in->rootLink()->p = 0.01*com_ref.p + 0.99*robot_in->rootLink()->p;//ベースリンク位置が迷走するのを防ぐ？
+  robot_in->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
   const std::string names[4] = {"rleg","lleg","rarm","larm"};
   const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
   for(int i=0;i<4;i++){
-    if(fik->ikp.count(names[i])){
-      fik->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);//convert End Effector pos into link origin pos
-      fik->ikp[names[i]].target_p0 = refs[i]->p;
+    if(fik_in->ikp.count(names[i])){
+      fik_in->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);
+      fik_in->ikp[names[i]].target_p0 = refs[i]->p;
     }
   }
-  for( int i=0; i<m_robot->numJoints(); i++){ m_robot->joint(i)->q = q_pre(i); }
-  if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref.rpy(y);
-  if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref.rpy(p);
-  //COM 収束ループ
+  fik_in->storeCurrentParameters();
+//  fik_in->setReferenceJointAngles();//これ入れると腕ブワーなる
+  if(robot_in->link("HEAD_JOINT0") != NULL)robot_in->joint(15)->q = head_ref.rpy(y);
+  if(robot_in->link("HEAD_JOINT1") != NULL)robot_in->joint(16)->q = head_ref.rpy(p);
   hrp::Vector3 tmp_com_err = hrp::Vector3::Zero();
-  while( 1 ){
+  while( 1 ){  //COM 収束ループ
     com_ik_loop++;
-    m_robot->rootLink()->p += tmp_com_err;
-    m_robot->calcForwardKinematics();
-    for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-        if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
+    robot_in->rootLink()->p += tmp_com_err;
+    robot_in->calcForwardKinematics();
+    for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_in->ikp.begin(); it != fik_in->ikp.end(); it++ ) {
+        if (it->second.is_ik_enable) fik_in->solveLimbIK (it->second, it->first, fik_in->ratio_for_vel, false);
     }
-    tmp_com_err = com_ref.p - m_robot->calcCM();
+    tmp_com_err = com_ref.p - robot_in->calcCM();
     if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
-    if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
+    if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
   }
-  basepos_pre = m_robot->rootLink()->p;
-  for ( int i = 0; i < m_robot->numJoints(); i++ ){ q_pre[i] = m_robot->joint(i)->q; }
-  cout<<"com_ik_loop:"<<com_ik_loop<<endl;
+  if(com_ik_loop != 1)cout<<"com_ik_loop:"<<com_ik_loop<<" @ "<<debug_prefix<<endl;
 }
-//
-//void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, const bool solve_from_previous_basepos){
-//  int com_ik_loop=0;
-//  const int COM_IK_MAX_LOOP = 20;
-//  const double COM_IK_MAX_ERROR = 1e-4;
-////  static hrp::Vector3 base_pos_pre;
-//  static hrp::Vector3 com2base_offset_old;
-////  hrp::Vector3 com2base_offset = com2base_offset_old;
-//  hrp::Vector3 com2base_offset = 0.99 * com2base_offset_old;
-////  m_robot->rootLink()->p = com_ref.p + com2base_offset_old;//move base link at first
-//  m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
-//  const std::string names[4] = {"rleg","lleg","rarm","larm"};
-//  const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
-//  for(int i=0;i<4;i++){
-//    if(fik->ikp.count(names[i])){
-//      fik->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);//convert End Effector pos into link origin pos
-//      fik->ikp[names[i]].target_p0 = refs[i]->p;
-//    }
-//  }
-//  if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref.rpy(y);
-//  if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref.rpy(p);
-//  //COM 収束ループ
-//  hrp::Vector3 tmp_com_err;
-//  bool offset_vel_over = false;
-//  while( 1 ){
-//    com_ik_loop++;
-//    m_robot->rootLink()->p = com_ref.p + com2base_offset;
-//    m_robot->calcForwardKinematics();
-//    for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-//        if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
-//    }
-//    m_robot->calcForwardKinematics();
-//    if(offset_vel_over){break;}
-//    tmp_com_err = com_ref.p - m_robot->calcCM();
-//    if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
-//    com2base_offset += tmp_com_err;
-//
-////    if((com2base_offset - com2base_offset_old).norm() > 0.5*m_dt){
-////      com2base_offset = com2base_offset_old + (com2base_offset - com2base_offset_old).normalized() * 0.5*m_dt;
-////      offset_vel_over = true;
-////      cout<<"offset_vel_over!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<endl;
-////    }
-////    cout<<"com2base_offset:"<<com2base_offset.transpose()<<endl;
-//
-//    if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
-//
-////    cout<<"tmp_com_err0:"<<tmp_com_err.transpose()<<endl;
-//  }
-//  com2base_offset_old = com2base_offset;
-////  base_pos_pre = m_robot->rootLink()->p;
-//  cout<<"com_ik_loop:"<<com_ik_loop<<endl;
-//}
-
-//void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, const bool solve_from_previous_basepos){
-//  int com_ik_loop=0;
-//  const int COM_IK_MAX_LOOP = 20;
-//  const double COM_IK_MAX_ERROR = 1e-4;
-//  static hrp::Vector3 base_pos_pre;
-//  m_robot->rootLink()->p = com_ref.p;//move base link at first
-//  if(solve_from_previous_basepos) m_robot->rootLink()->p = base_pos_pre;
-//  m_robot->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
-//  const std::string names[4] = {"rleg","lleg","rarm","larm"};
-//  const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
-//  for(int i=0;i<4;i++){
-//    if(fik->ikp.count(names[i])){
-//      fik->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);//convert End Effector pos into link origin pos
-//      fik->ikp[names[i]].target_p0 = refs[i]->p;
-//    }
-//  }
-//  if(m_robot->link("HEAD_JOINT0") != NULL)m_robot->joint(15)->q = head_ref.rpy(y);
-//  if(m_robot->link("HEAD_JOINT1") != NULL)m_robot->joint(16)->q = head_ref.rpy(p);
-//  //COM 収束ループ
-//  hrp::Vector3 tmp_com_err;
-//  while( 1 ){
-//    com_ik_loop++;
-//    m_robot->calcForwardKinematics();
-//    for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-//        if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
-//    }
-//    tmp_com_err = com_ref.p - m_robot->calcCM();
-////    cout<<"tmp_com_err:"<<tmp_com_err.norm()<<endl;
-//    if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
-//    m_robot->rootLink()->p += tmp_com_err;
-//    if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!!" << std::endl; break; };
-//
-//    cout<<"tmp_com_err0:"<<tmp_com_err.transpose()<<endl;
-//    if(solve_from_previous_basepos)cout<<"tmp_com_err:"<<tmp_com_err.transpose()<<endl;
-//  }
-//  if(solve_from_previous_basepos)base_pos_pre = m_robot->rootLink()->p;
-//  if(solve_from_previous_basepos)cout<<"com_ik_loop:"<<com_ik_loop<<endl;
-//}
 
+void WholeBodyMasterSlave::processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in){
 
-//
-//hrp::Vector3 torso_pos = hrp::Vector3::Zero();
-//hrp::Vector3 torso_rot = hrp::Vector3::Zero();
-////void WholeBodyMasterSlave::calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody){
-////  hrp::Vector3 torso_pos_acc,torso_rot_acc;
-////  static hrp::Vector3 torso_pos_vel,torso_rot_vel = hrp::Vector3::Zero();
-////  hrp::Matrix33 total_inertia;
-////  for(int i=0; i<m_robot->numLinks(); i++){
-////    total_inertia += m_robot->link(i)->I;
-////  }
-////  hrp::Vector3 zmp_err = zmp_fullbody - zmp_lip;
-////  if(fabs(zmp_err(0))<0.002)zmp_err(0)=0;
-////  if(fabs(zmp_err(1))<0.002)zmp_err(1)=0;
-////  LIMIT_MINMAX(zmp_err(0),-0.05,0.05);
-////  LIMIT_MINMAX(zmp_err(1),-0.05,0.05);
-////  zmp_err(2)=0;
-////  torso_pos_acc(0) = zmp_err(0) / 1.1 * 9.8;
-////  torso_pos_acc(1) = zmp_err(1) / 1.1 * 9.8;
-////  torso_rot_acc(0) = - zmp_err(1) * m_robot->totalMass() * 9.8 / (130*0.2*0.2);
-////  torso_rot_acc(1) = + zmp_err(0) * m_robot->totalMass() * 9.8 / (130*0.2*0.2);
-////
-////  torso_pos_acc(0) *= 0.5;
-////  torso_pos_acc(1) *= 0.5;
-////  torso_rot_acc(0) *= 0.1;
-////  torso_rot_acc(1) *= 0.1;
-////
-////
-////
-////  torso_pos_acc -= 1 * torso_pos + 1 * torso_pos_vel;
-////  torso_rot_acc -= 1 * torso_rot + 1 * torso_rot_vel;
-//////  torso_rot_acc(0) -= 10 * torso_rot(0) + 10 * torso_rot_vel(0);
-//////  torso_rot_acc(1) -= 10 * torso_rot(1) + 10 * torso_rot_vel(1);
-////
-////  LIMIT_MINMAX(torso_pos_acc(0),-10,10);
-////  LIMIT_MINMAX(torso_pos_acc(1),-10,10);
-////  LIMIT_MINMAX(torso_pos_acc(2),-0,0);
-////  LIMIT_MINMAX(torso_rot_acc(0),-10,10);
-////  LIMIT_MINMAX(torso_rot_acc(1),-10,10);
-////  LIMIT_MINMAX(torso_rot_acc(2),-0,0);
-////
-////
-////  if(hsp->isHumanSyncOn()){
-////    torso_rot_vel += torso_rot_acc * m_dt;
-////    torso_rot += torso_rot_vel * m_dt;
-////    torso_pos_vel += torso_pos_acc * m_dt;
-////    torso_pos += torso_pos_vel * m_dt;
-////  }
-////
-////  LIMIT_MINMAX(torso_pos(0),-0.1,0.1);
-////  LIMIT_MINMAX(torso_pos(1),-0.1,0.1);
-////  LIMIT_MINMAX(torso_pos(2),-0.1,0.1);
-////  LIMIT_MINMAX(torso_rot(0),-10*M_PI/180,10*M_PI/180);
-////  LIMIT_MINMAX(torso_rot(1),-10*M_PI/180,10*M_PI/180);
-////  LIMIT_MINMAX(torso_rot(2),-0.1,0.1);
-////
-////
-//////  cout<<"total_inertia: "<<total_inertia<<endl;
-//////  cout<<"zmp_lip: "<<zmp_lip.transpose()<<endl;
-//////  cout<<"zmp_fullbody: "<<zmp_fullbody.transpose()<<endl;
-//////  cout<<"torso_rot_acc: "<<torso_rot_acc.transpose()<<endl;
-//////  cout<<"torso_rot_vel: "<<torso_rot_vel.transpose()<<endl;
-//////  cout<<"torso_rot: "<<torso_rot.transpose()<<endl;
-////
-////
-////  fprintf(hsp->id_log,"zmp_lip: %f %f ",zmp_lip(0),zmp_lip(1),zmp_lip(2));
-////  fprintf(hsp->id_log,"zmp_fullbody: %f %f ",zmp_fullbody(0),zmp_fullbody(1),zmp_fullbody(2));
-////  fprintf(hsp->id_log,"zmp_fullbody2: %f %f ",ref_zmp_invdyn2(0),ref_zmp_invdyn2(1),ref_zmp_invdyn2(2));
-////  fprintf(hsp->id_log,"torso_pos_acc: %f %f ",torso_pos_acc(0),torso_pos_acc(1),torso_pos_acc(2));
-////  fprintf(hsp->id_log,"torso_pos: %f %f ",torso_pos(0),torso_pos(1),torso_pos(2));
-////  fprintf(hsp->id_log,"torso_rot_acc: %f %f ",torso_rot_acc(0),torso_rot_acc(1),torso_rot_acc(2));
-////  fprintf(hsp->id_log,"torso_rot: %f %f ",torso_rot(0),torso_rot(1),torso_rot(2));
-////  fprintf(hsp->id_log,"\n");
-////
-////
-////
-//////  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
-//////  fik->current_tm = m_qRef.tm;
-//////  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-//////      fik->ikp[it->first].is_ik_enable = it->second.is_active;
-//////  }
-//////  if(hsp->isHumanSyncOn()){
-//////    if(hsp->ht_first_call){
-//////      std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
-//////      hsp->ht_first_call = false;
-//////    }
-//////  }
-//////  m_robot->calcForwardKinematics();
-//////  // additional COM fitting IK for HumanSynchronizer
-////  if(hsp->isHumanSyncOn()){
-////    HRPPose3D com_mod = hsp->rp_ref_out.P[com];
-////    com_mod.p += torso_pos;
-////    com_mod.rpy += torso_rot;
-////    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->cam_rpy_filtered );
-////  }
-////}
-//
-//
-void WholeBodyMasterSlave::calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody){
   hrp::Vector3 P,L;
   m_robot->rootLink()->v = idsb.base_v;
   m_robot->rootLink()->w = idsb.base_w;
@@ -862,58 +623,28 @@ void WholeBodyMasterSlave::calcDynamicsFilterCompensation(const hrp::Vector3 zmp
   static hrp::Vector3 torso_rot;
   hrp::Vector3 torso_rot_vel;
 
-
   m_robot->calcCM();
   m_robot->rootLink()->calcSubMassCM();
   hrp::Matrix33 I_com;
 
   if(m_robot->link("CHEST_JOINT0")){
     m_robot->link("CHEST_JOINT0")->calcSubMassInertia(I_com);
-//    cout<<"I_com:"<<endl<<I_com<<endl;
-//    double M = m_robot->link("CHEST_JOINT0")->subm;
-//    hrp::Vector3 a;
-//    hrp::Matrix33 shift_wld2com;
-//    shift_wld2com(0,0) = a(Y)*a(Y)+a(Z)*a(Z);  shift_wld2com(0,1) = -a(Y)*a(X);           shift_wld2com(0,2) = -a(Z)*a(X);
-//    shift_wld2com(1,0) = -a(X)*a(Y);           shift_wld2com(1,1) = a(X)*a(X)+a(Z)*a(Z);  shift_wld2com(1,2) = -a(Z)*a(Y);
-//    shift_wld2com(2,0) = -a(X)*a(Z);           shift_wld2com(2,1) = -a(Y)*a(Z);           shift_wld2com(2,2) = a(X)*a(X)+a(Y)*a(Y);
     torso_rot_vel = - I_com.inverse() * L * hsp->WBMSparam.upper_body_rmc_ratio;
     torso_rot += torso_rot_vel * m_dt;
   }
-//  fprintf(hsp->id_log,"p: %f %f ",hsp->rp_ref_out.P[com].p(0),hsp->rp_ref_out.P[com].p(1),hsp->rp_ref_out.P[com].p(2));
-//  fprintf(hsp->id_log,"rpy: %f %f ",hsp->rp_ref_out.P[com].rpy(0),hsp->rp_ref_out.P[com].rpy(1),hsp->rp_ref_out.P[com].rpy(2));
-//  fprintf(hsp->id_log,"P: %f %f ",P(0),P(1),P(2));
-//  fprintf(hsp->id_log,"L: %f %f ",L(0),L(1),L(2));
-//  fprintf(hsp->id_log,"\n");
-  static hrp::Vector3 basepos_dyn_ik = m_robot->rootLink()->p;
-  static hrp::dvector q_dyn_ik;
-  static int callnum;
-  if(callnum++ == 0){
-    q_dyn_ik.resize(m_robot->numJoints());
-    for( int i=0; i<m_robot->numJoints(); i++){ q_dyn_ik(i) = m_robot->joint(i)->q; }
-  }
-//  for( int i=0; i<m_robot->numJoints(); i++){ q_dyn_ik(i) = q_normal_ik(i); }
   if(hsp->isHumanSyncOn()){
     HRPPose3D com_mod = hsp->rp_ref_out.P[com];
-//    com_mod.p += torso_pos;
     com_mod.rpy += torso_rot;
-
-    cout<<"calcDynamicsFilterCompensation:"<<endl;
-    solveFullbodyIKStrictCOM( com_mod, hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head], basepos_dyn_ik, q_dyn_ik);
+    solveFullbodyIKStrictCOM(fik_in, robot_in, com_mod, hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head],"calcDynamicsFilterCompensation");
   }
 }
 
-
-
-
-bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double sec)
-{
+bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double sec){
   if(sec >= 0.0 && sec <= 30.0){
     std::cerr << "[" << m_profile.instance_name << "] start Synchronization after "<<sec<<" [s]" << std::endl;
     hsp->countdown_sec = sec;
     hsp->startCountdownForHumanSync = true;
-    while (!hsp->isHumanSyncOn()){
-        usleep(1000);
-    }
+    while (!hsp->isHumanSyncOn()){ usleep(1000); }
     return true;
   }else{
     std::cerr << "[" << m_profile.instance_name << "] Count Down Time must be 0 < T < 30 [s]"<< std::endl;
@@ -921,8 +652,7 @@ bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double se
   }
 }
 
-bool WholeBodyMasterSlave::stopHumanSync()
-{
+bool WholeBodyMasterSlave::stopHumanSync(){
 	std::cerr << "[" << m_profile.instance_name << "] not implemented..." << std::endl;
 	return true;
 }
@@ -959,17 +689,9 @@ bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMaster
   return true;
 }
 
-extern "C"
-{
-
-    void WholeBodyMasterSlaveInit(RTC::Manager* manager)
-    {
+extern "C"{
+    void WholeBodyMasterSlaveInit(RTC::Manager* manager) {
         RTC::Properties profile(WholeBodyMasterSlave_spec);
-        manager->registerFactory(profile,
-                                 RTC::Create<WholeBodyMasterSlave>,
-                                 RTC::Delete<WholeBodyMasterSlave>);
+        manager->registerFactory(profile, RTC::Create<WholeBodyMasterSlave>, RTC::Delete<WholeBodyMasterSlave>);
     }
-
 };
-
-
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 50cbaabaaf7..a5a2023bb87 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -153,21 +153,13 @@ class WholeBodyMasterSlave
     hrp::Link* target_link;
     bool is_active, has_toe_joint;
   };
-  void solveFullbodyIKStrictCOM(const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, hrp::Vector3& basepos_pre, hrp::dvector& q_pre);
-  void processWholeBodyMasterSlave();
-  void processWholeBodyMasterSlave_Raw();
-  void calcDynamicsFilterCompensation(const hrp::Vector3 zmp_lip, const hrp::Vector3 zmp_fullbody);
-  bool isOptionalDataContact (const std::string& ee_name)
-  {
-      return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false;
-  };
   typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
-  fikPtr fik;
+  fikPtr fik, fik_rmc;
   std::map<std::string, size_t> contact_states_index_map;
   double m_dt;
-  hrp::BodyPtr m_robot;
-  hrp::Vector3 input_ref_zmp, input_basePos;
-  hrp::Matrix33 input_baseRot;
+  hrp::BodyPtr m_robot, m_robot_rmc;
+//  hrp::Vector3 input_ref_zmp, input_basePos;
+//  hrp::Matrix33 input_baseRot;
 
   double q_interpolator_ratio;
   interpolator *q_interpolator;
@@ -190,6 +182,13 @@ class WholeBodyMasterSlave
   hrp::dvector q_normal_ik;
 
   boost::shared_ptr<HumanSynchronizer> hsp;
+
+  void setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in);
+  void solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, const std::string& debug_prefix="");
+  void processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in);
+  void processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::BodyPtr& robot_in);
+  void processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in);
+  bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; };
 };
 
 extern "C"

From b24a2b6fe6d20d02a0f52e59f42cab7a2e5aef57 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 13 May 2017 06:05:54 +0900
Subject: [PATCH 085/217] clean up code

---
 .../WholeBodyMasterSlave.cpp                  | 347 +++++++-----------
 .../WholeBodyMasterSlave.h                    |  35 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          |   3 +
 3 files changed, 153 insertions(+), 232 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 91f22e9522a..ce01ba3326c 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -7,8 +7,8 @@
 #include <hrpUtil/MatrixSolvers.h>
 #include "hrpsys/util/Hrpsys.h"
 
-typedef coil::Guard<coil::Mutex> Guard;
-using namespace rats;
+//typedef coil::Guard<coil::Mutex> Guard;
+//using namespace rats;
 
 static const char* WholeBodyMasterSlave_spec[] =
     {
@@ -81,9 +81,7 @@ WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager)
     m_service0.wholebodymasterslave(this);
 }
 
-WholeBodyMasterSlave::~WholeBodyMasterSlave()
-{
-}
+WholeBodyMasterSlave::~WholeBodyMasterSlave(){}
 
 RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
 {
@@ -136,7 +134,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
 #endif
 
     m_WholeBodyMasterSlaveServicePort.registerProvider("service0", "WholeBodyMasterSlaveService", m_service0);
-  
     addPort(m_WholeBodyMasterSlaveServicePort);
   
     RTC::Properties& prop =  getProperties();
@@ -147,9 +144,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
     RTC::Manager& rtcManager = RTC::Manager::instance();
     std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
     int comPos = nameServer.find(",");
-    if (comPos < 0){
-        comPos = nameServer.length();
-    }
+    if (comPos < 0){ comPos = nameServer.length(); }
     nameServer = nameServer.substr(0, comPos);
     RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
     if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
@@ -160,13 +155,11 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
       std::cerr << "[" << m_profile.instance_name << "] failed to load model 2 [" << prop["model"] << "]" << std::endl;
       return RTC::RTC_ERROR;
     }
-
     // allocate memory for outPorts
     m_qRef.data.length(m_robot->numJoints());
     m_htrfw.data.length(6);
     m_htlfw.data.length(6);
     loop = 0;
-
     q_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
     q_interpolator->setName(std::string(m_profile.instance_name)+" q_interpolator");
     q_interpolator_ratio = 0;
@@ -175,36 +168,29 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
     q_interpolator->set(&tmp_ratio);
     tmp_ratio = 1.0;
     q_interpolator->setGoal(&tmp_ratio, 3.0, true);
-
-//    // setting from conf file
-//    // GaitGenerator requires abc_leg_offset and abc_stride_parameter in robot conf file
-//    // setting leg_pos from conf file
-//    coil::vstring leg_offset_str = coil::split(prop["abc_leg_offset"], ",");
-//    leg_names.push_back("rleg");
-//    leg_names.push_back("lleg");
-//
     // Generate FIK
     fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
     fik_rmc = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot_rmc, std::string(m_profile.instance_name), m_dt));
 
-    setupfik(fik, m_robot, prop);
-    setupfik(fik_rmc, m_robot_rmc, prop);
+    ik_robot_list.push_back(std::make_pair( fik,      m_robot     ));
+    ik_robot_list.push_back(std::make_pair( fik_rmc,  m_robot_rmc ));
+    for(int i=0;i<ik_robot_list.size();i++){ setupfik(ik_robot_list[i].first, ik_robot_list[i].second, prop); }
 
     if (fik->ikp.find("rleg") != fik->ikp.end() && fik->ikp.find("lleg") != fik->ikp.end()) {
       is_legged_robot = true;
     } else {
       is_legged_robot = false;
     }
-
     hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer(m_dt));
 
+    invdyn_zmp_filters.setParameter(25, 1/m_dt, BiquadIIRFilterVec::Q_BUTTERWORTH);
+    invdyn_zmp_filters2.setParameter(25, 1/m_dt, BiquadIIRFilterVec::Q_BUTTERWORTH);
+
     return RTC::RTC_OK;
 }
 
 
 void WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in){
-  // setting from conf file
-  // rleg,TARGET_LINK,BASE_LINK
   coil::vstring end_effectors_str = coil::split(prop_in["end_effectors"], ",");
   size_t prop_num = 10;
   if (end_effectors_str.size() > 0) {
@@ -243,39 +229,19 @@ void WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC:
           tp.has_toe_joint = false;
       }
       tp.target_link = robot_in->link(ee_target);
-//        ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
-//        ee_vec.push_back(ee_name);
       std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
       std::cerr << "[" << m_profile.instance_name << "]   target = " << fik_in->ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
       std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
       std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
-      contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));////TODO:要移動
+      contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));////TODO:要移動?
     }
   }
-
-}
-
-RTC::ReturnCode_t WholeBodyMasterSlave::onFinalize()
-{
-  return RTC::RTC_OK;
-}
-
-RTC::ReturnCode_t WholeBodyMasterSlave::onActivated(RTC::UniqueId ec_id)
-{
-    std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
-    return RTC::RTC_OK;
-}
-
-RTC::ReturnCode_t WholeBodyMasterSlave::onDeactivated(RTC::UniqueId ec_id)
-{
-  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
-  return RTC::RTC_OK;
 }
 
-#define DEBUGP ((loop%200==0))
-RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
-{
-//  std::cerr << "[" << m_profile.instance_name<< "] onExecute(" << ec_id << ")" << std::endl;
+#define DEBUGP (loop%200==0)
+#define DEBUGP_ONCE (loop==0)
+RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
+  if(DEBUGP_ONCE)std::cerr << "[" << m_profile.instance_name<< "] onExecute(" << ec_id << ")" << std::endl;
   // struct timeval t_calc_start, t_calc_end;
   // gettimeofday(&t_calc_start, NULL);
     if (m_qRefIn.isNew()) { m_qRefIn.read(); }
@@ -297,75 +263,59 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
 
     if ( is_legged_robot ) {
 
-      bool is_q_interpolator_empty = q_interpolator->isEmpty();
-      if (!is_q_interpolator_empty && hsp->isHumanSyncOn()) {
-        q_interpolator->get(&q_interpolator_ratio, true);
-      }
-
-      if(!hsp->isHumanSyncOn()){//初期姿勢取得＆FK
-        m_robot->rootLink()->p = m_robot_rmc->rootLink()->p = hrp::Vector3(m_basePos.data.x, m_basePos.data.y, m_basePos.data.z);
-        m_robot->rootLink()->R = m_robot_rmc->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
-        for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ m_robot->joint(i)->q = m_robot_rmc->joint(i)->q = m_qRef.data[i]; }
-        m_robot->calcForwardKinematics();
-        m_robot_rmc->calcForwardKinematics();
+      processTransition();
 
-        fik->setReferenceJointAngles();
-        fik_rmc->setReferenceJointAngles();
+      if(hsp->isHumanSyncOn() && hsp->ht_first_call){
+        preProcessForWholeBodyMasterSlave(fik, m_robot);
       }
 
+      if (!q_interpolator->isEmpty() && hsp->isHumanSyncOn()) { q_interpolator->get(&q_interpolator_ratio, true); }
+
       fik->current_tm = fik_rmc->current_tm = m_qRef.tm;
       fik->ikp["rleg"].is_ik_enable = fik_rmc->ikp["rleg"].is_ik_enable = true;
       fik->ikp["lleg"].is_ik_enable = fik_rmc->ikp["lleg"].is_ik_enable = true;
-      fik->ikp["rarm"].is_ik_enable = fik_rmc->ikp["rarm"].is_ik_enable = true;
-      fik->ikp["larm"].is_ik_enable = fik_rmc->ikp["larm"].is_ik_enable = true;
+      fik->ikp["rarm"].is_ik_enable = fik_rmc->ikp["rarm"].is_ik_enable = hsp->WBMSparam.use_rh;
+      fik->ikp["larm"].is_ik_enable = fik_rmc->ikp["larm"].is_ik_enable = hsp->WBMSparam.use_lh;
 //      fik->ikp["rarm"].is_ik_enable = fik_rmc->ikp["rarm"].is_ik_enable = false;
 //      fik->ikp["larm"].is_ik_enable = fik_rmc->ikp["larm"].is_ik_enable = false;
 
-      if(hsp->WBMSparam.is_doctor){
-        processWholeBodyMasterSlave(fik, m_robot);
-      }else{
-        processWholeBodyMasterSlave_Raw(fik, m_robot);
+      if (hsp->isHumanSyncOn()) {
+        if(hsp->WBMSparam.is_doctor){
+          processWholeBodyMasterSlave(fik, m_robot);//安全制限つきマスタ・スレーブ
+        }else{
+          processWholeBodyMasterSlave_Raw(fik, m_robot);//生マスタ・スレーブ
+        }
       }
 
-      /////// Inverse Dynamics /////////
-      if(!idsb.is_initialized){
+      if(hsp->isHumanSyncOn() && hsp->ht_first_call){//逆動力学初期化
         idsb.setInitState(m_robot, m_dt);
-        invdyn_zmp_filters.resize(3);
-        for(int i=0;i<3;i++){
-          invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
-//            invdyn_zmp_filters[i].reset(ref_zmp(i));
-        }
         idsb2.setInitState(m_robot, m_dt);
-        invdyn_zmp_filters2.resize(3);
-        for(int i=0;i<3;i++){
-          invdyn_zmp_filters2[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
-//            invdyn_zmp_filters2[i].reset(ref_zmp(i));
-        }
       }
-      calcAccelerationsForInverseDynamics(m_robot, idsb);
-      hrp::Vector3 ref_zmp_invdyn;
-      calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);
-      for(int i=0;i<3;i++) ref_zmp_invdyn(i) = invdyn_zmp_filters[i].passFilter(ref_zmp_invdyn(i));
-      updateInvDynStateBuffer(idsb);
-
-      if(hsp->isHumanSyncOn() && hsp->WBMSparam.is_doctor){
-        processMomentumCompensation(fik_rmc, m_robot_rmc);
+
+      if (hsp->isHumanSyncOn()) {//逆動力学
+        calcAccelerationsForInverseDynamics(m_robot, idsb);
+        hrp::Vector3 ref_zmp_invdyn;
+        calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);
+        ref_zmp_invdyn = invdyn_zmp_filters.passFilter(ref_zmp_invdyn);
+        updateInvDynStateBuffer(idsb);
       }
 
+      if(hsp->isHumanSyncOn() && hsp->WBMSparam.is_doctor){//角運動量補償
+        processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, hsp->rp_ref_out);
+      }else{
+        processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, raw_pose);
+      }
 //      calcAccelerationsForInverseDynamics(m_robot, idsb2);
 //      calcWorldZMPFromInverseDynamics(m_robot, idsb2, ref_zmp_invdyn2);
 //      updateInvDynStateBuffer(idsb2);
 
-//        ref_zmp = ref_zmp_invdyn;
-//        ref_zmp = ref_zmp_invdyn2;
-      ref_zmp = hsp->rp_ref_out.P[zmp].p;
-//      hrp::BodyPtr m_robot_for_out = m_robot;
-      hrp::BodyPtr m_robot_for_out = m_robot_rmc;
-
-      if (hsp->isHumanSyncOn()) {
-        for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ){
-          m_qRef.data[i] = q_interpolator_ratio * m_robot_for_out->joint(i)->q  + (1 - q_interpolator_ratio) * m_qRef.data[i];
-        }
+      if (hsp->isHumanSyncOn()){//OutPortデータセット
+        //        ref_zmp = ref_zmp_invdyn;
+        hrp::Vector3 ref_zmp = hsp->rp_ref_out.P[zmp].p;
+        //      hrp::BodyPtr m_robot_for_out = m_robot;
+        hrp::BodyPtr m_robot_for_out = m_robot_rmc;
+        // qRef
+        for (int i = 0; i < m_qRef.data.length(); i++ ){ m_qRef.data[i] = q_interpolator_ratio * m_robot_for_out->joint(i)->q  + (1 - q_interpolator_ratio) * m_qRef.data[i]; }
         // basePos
         m_basePos.data.x = m_robot_for_out->rootLink()->p(0);
         m_basePos.data.y = m_robot_for_out->rootLink()->p(1);
@@ -378,17 +328,25 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
         m_baseRpy.data.y = baseRpy(2);
         m_baseRpy.tm = m_qRef.tm;
         // zmp
-        rel_ref_zmp = m_robot_for_out->rootLink()->R.transpose() * (ref_zmp - m_robot_for_out->rootLink()->p);
+        hrp::Vector3 rel_ref_zmp = m_robot_for_out->rootLink()->R.transpose() * (ref_zmp - m_robot_for_out->rootLink()->p);
         m_zmp.data.x = rel_ref_zmp(0);
         m_zmp.data.y = rel_ref_zmp(1);
         m_zmp.data.z = rel_ref_zmp(2);
         m_zmp.tm = m_qRef.tm;
+        // m_optionalData
+        if(m_optionalData.data.length() < 4*2){
+          m_optionalData.data.length(4*2);//TODO:これいいのか？
+          for(int i=0;i<4*2;i++)m_optionalData.data[i] = 0;
+        }
+        m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[contact_states_index_map["rleg"]+4] = hsp->is_rf_contact;
+        m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[contact_states_index_map["lleg"]+4] = hsp->is_lf_contact;
       }
+      if(DEBUGP_ONCE)std::cerr << "[" << m_profile.instance_name<< "] onExecute(" << ec_id << ")" << std::endl;
       hsp->baselinkpose.p = m_robot->rootLink()->p;
       hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
     }
     // write
-     m_qOut.write();
+    m_qOut.write();
     m_basePosOut.write();
     m_baseRpyOut.write();
     m_zmpOut.write();
@@ -464,122 +422,88 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id)
     return RTC::RTC_OK;
 }
 
-void WholeBodyMasterSlave::processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in){
 
+void WholeBodyMasterSlave::processTransition(){
+  if(hsp->isHumanSyncOn()){ hsp->ht_first_call = false;}
   if(hsp->startCountdownForHumanSync){
     std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
     hsp->updateCountDown();
   }
-  if(hsp->isHumanSyncOn()){
-    if(hsp->ht_first_call){
-      std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
-      hsp->ht_first_call = false;
-    }
-  }else{
-    fik_in->storeCurrentParameters();
-    fik_in->setReferenceJointAngles();
-    hsp->setCurrentInputAsOffset(hsp->hp_wld_raw);
-    hsp->calibInitHumanCOMFromZMP();
-
-    const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
-    const int human_l_names[4] = {rf,lf,rh,lh};
-    for(int i=0;i<4;i++){
-      if(fik_in->ikp.count(robot_l_names[i])){
-        hsp->rp_ref_out.P[human_l_names[i]].p_offs = fik_in->ikp[robot_l_names[i]].target_link->p + fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localPos;
-        hsp->rp_ref_out.P[human_l_names[i]].rpy_offs = hrp::rpyFromRot(fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localR);
-        hsp->rp_ref_out.P[human_l_names[i]].p = hsp->rp_ref_out.P[human_l_names[i]].p_offs;
-        hsp->rp_ref_out.P[human_l_names[i]].rpy = hsp->rp_ref_out.P[human_l_names[i]].rpy_offs;
-      }
-    }
-    //    hsp->rp_ref_out.P[com].p_offs = robot_in->calcCM();
-    hsp->rp_ref_out.P[com].p_offs(0) = (hsp->rp_ref_out.P[rf].p_offs(0) + hsp->rp_ref_out.P[lf].p_offs(0)) / 2;
-    hsp->rp_ref_out.P[com].p_offs(1) = (hsp->rp_ref_out.P[rf].p_offs(1) + hsp->rp_ref_out.P[lf].p_offs(1)) / 2;
-    hsp->rp_ref_out.P[com].p_offs(2) = robot_in->calcCM()(2);
-    //    hsp->rp_ref_out.P[zmp].p_offs = ref_zmp;
-    hsp->pre_cont_rfpos = hsp->rp_ref_out.P[rf].p_offs;
-    hsp->pre_cont_lfpos = hsp->rp_ref_out.P[lf].p_offs;
-    hsp->baselinkpose.p_offs = robot_in->rootLink()->p;
-    hsp->baselinkpose.rpy_offs = hrp::rpyFromRot(robot_in->rootLink()->R);
+  if(hsp->isHumanSyncOn() && hsp->ht_first_call){
+    std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
   }
+}
 
+
+void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in){
+  hrp::Vector3 basePos_heightChecked = hrp::Vector3(m_basePos.data.x, m_basePos.data.y, m_basePos.data.z);//ベースリンク高さ調整により足裏高さ0に
+  robot_in->rootLink()->p = basePos_heightChecked;
+  for ( int i = 0; i < robot_in->numJoints(); i++ ){ robot_in->joint(i)->q = m_qRef.data[i]; }
+  robot_in->calcForwardKinematics();
   hrp::Vector3 rsole_pos = fik_in->ikp["rleg"].target_link->p+ fik_in->ikp["rleg"].target_link->R * fik_in->ikp["rleg"].localPos;
   hrp::Vector3 lsole_pos = fik_in->ikp["lleg"].target_link->p+ fik_in->ikp["lleg"].target_link->R * fik_in->ikp["lleg"].localPos;
-  hsp->H_cur = robot_in->calcCM()(2) - std::min((double)rsole_pos(2), (double)lsole_pos(2));
+  hrp::Vector3 init_foot_mid_coord = (rsole_pos + lsole_pos) / 2;
+  if( fabs((double)init_foot_mid_coord(Z)) > 1e-5 ){
+    basePos_heightChecked(Z) -= init_foot_mid_coord(Z);
+    std::cerr<<"["<<m_profile.instance_name<<"] Input basePos height is invalid. Auto modify "<<m_basePos.data.z<<" -> "<<basePos_heightChecked(Z)<<endl;
+  }
+  for(int i=0;i<ik_robot_list.size();i++){//初期姿勢でBodyをFK
+    ik_robot_list[i].second->rootLink()->p = basePos_heightChecked;
+    ik_robot_list[i].second->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
+    for ( int j = 0; j < ik_robot_list[i].second->numJoints(); j++ ){ ik_robot_list[i].second->joint(j)->q = m_qRef.data[j]; }
+    ik_robot_list[i].second->calcForwardKinematics();
+    ik_robot_list[i].first->setReferenceJointAngles();
+  }
+  hsp->setCurrentInputAsOffset(hsp->hp_wld_raw);//現在の入力を人間の初期姿勢としてセット
+  hsp->calibInitHumanCOMFromZMP();//今あんま使ってない
+  const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
+  const int human_l_names[4] = {rf,lf,rh,lh};
+  for(int i=0;i<4;i++){//HumanSynchronizerの初期姿勢オフセットをセット
+    if(fik_in->ikp.count(robot_l_names[i])){
+      hsp->rp_ref_out.P[human_l_names[i]].p_offs = fik_in->ikp[robot_l_names[i]].target_link->p + fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localPos;
+      hsp->rp_ref_out.P[human_l_names[i]].rpy_offs = hrp::rpyFromRot(fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localR);
+      hsp->rp_ref_out.P[human_l_names[i]].p = hsp->rp_ref_out.P[human_l_names[i]].p_offs;
+      hsp->rp_ref_out.P[human_l_names[i]].rpy = hsp->rp_ref_out.P[human_l_names[i]].rpy_offs;
+    }
+  }
+  hsp->rp_ref_out.P[com].p_offs = robot_in->calcCM();
+  hsp->rp_ref_out.P[zmp].p_offs(X) = hsp->rp_ref_out.P[com].p_offs(X);
+  hsp->rp_ref_out.P[zmp].p_offs(Y) = hsp->rp_ref_out.P[com].p_offs(Y);
+  hsp->rp_ref_out.P[zmp].p_offs(Z) = init_foot_mid_coord(Z);
+  hsp->pre_cont_rfpos = hsp->rp_ref_out.P[rf].p_offs;
+  hsp->pre_cont_lfpos = hsp->rp_ref_out.P[lf].p_offs;
+  hsp->baselinkpose.p_offs = robot_in->rootLink()->p;
+  hsp->baselinkpose.rpy_offs = hrp::rpyFromRot(robot_in->rootLink()->R);
+}
 
+
+void WholeBodyMasterSlave::processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in){
   hsp->update();//////HumanSynchronizerの主要処理
   if(DEBUGP)cout<<"update():"<<hsp->getUpdateTime()<<endl;
   if(DEBUGP)hsp->rp_ref_out.print();
-
-  robot_in->calcForwardKinematics();
-  if(hsp->isHumanSyncOn()){
-    solveFullbodyIKStrictCOM(fik, m_robot, hsp->rp_ref_out.P[com], hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head],"processWholeBodyMasterSlave");
-    hsp->rp_ref_out.P[zmp].p(2) = (hsp->rp_ref_out.P[rf].p(2) + hsp->rp_ref_out.P[lf].p(2))/2;//体幹相対ZMP高さ設定
-    if(m_optionalData.data.length() < 4*2){
-      m_optionalData.data.length(4*2);//これいいのか？
-      for(int i=0;i<4*2;i++)m_optionalData.data[i] = 0;
-    }
-    m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[contact_states_index_map["rleg"]+4] = hsp->is_rf_contact;
-    m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[contact_states_index_map["lleg"]+4] = hsp->is_lf_contact;
-  }
+  solveFullbodyIKStrictCOM(fik_in, robot_in, hsp->rp_ref_out.P[com], hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head],"processWholeBodyMasterSlave");
 }
 
+
 void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::BodyPtr& robot_in){
-  static HumanPose raw_pose;
   static BiquadIIRFilterVec pos_filters[num_pose_tgt], rot_filters[num_pose_tgt];
   static unsigned int callnum;
-  if(callnum++ == 0){
-    for(int i=0;i<num_pose_tgt;i++){
+  for(int i=0;i<num_pose_tgt;i++){
+    if(callnum == 0){
       pos_filters[i].setParameter(100.0, 1.0/m_dt, BiquadIIRFilterVec::Q_NOOVERSHOOT);
       rot_filters[i].setParameter(100.0, 1.0/m_dt, BiquadIIRFilterVec::Q_NOOVERSHOOT);
+      pos_filters[i].init(hsp->hp_wld_raw.P[i].p);
+      rot_filters[i].init(hsp->hp_wld_raw.P[i].rpy);
     }
+    raw_pose.P[i].p = pos_filters[i].passFilter(hsp->hp_wld_raw.P[i].p);
+    raw_pose.P[i].rpy = rot_filters[i].passFilter(hsp->hp_wld_raw.P[i].rpy);
   }
-
-  if(hsp->startCountdownForHumanSync){
-   std::cerr << "[" << m_profile.instance_name << "] Count Down for RawSync ["<<hsp->getRemainingCountDown()<<"]\r";
-   hsp->updateCountDown();
-  }
-  if(hsp->isHumanSyncOn()){
-   if(hsp->ht_first_call){
-     std::cerr << "\n[" << m_profile.instance_name << "] Start RawSync"<< std::endl;
-     hsp->ht_first_call = false;
-   }
-  }else{
-   //初期状態取得
-   fik_in->storeCurrentParameters();
-   fik_in->setReferenceJointAngles();
-   const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
-   const int human_l_names[4] = {rf,lf,rh,lh};
-   raw_pose.P[com].p = raw_pose.P[com].p_offs = robot_in->calcCM();
-   raw_pose.P[com].rpy = raw_pose.P[com].rpy_offs = hrp::rpyFromRot(robot_in->rootLink()->R);
-   pos_filters[com].init(raw_pose.P[com].p);
-   rot_filters[com].init(raw_pose.P[com].rpy);
-   pos_filters[zmp].init(hrp::Vector3::Zero());
-   for(int i=0;i<4;i++){
-     if(fik_in->ikp.count(robot_l_names[i])){
-       raw_pose.P[human_l_names[i]].p_offs = fik_in->ikp[robot_l_names[i]].target_link->p + fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localPos;
-       raw_pose.P[human_l_names[i]].rpy_offs = hrp::rpyFromRot(fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localR);
-       raw_pose.P[human_l_names[i]].p = hsp->rp_ref_out.P[human_l_names[i]].p_offs;
-       raw_pose.P[human_l_names[i]].rpy = hsp->rp_ref_out.P[human_l_names[i]].rpy_offs;
-       pos_filters[human_l_names[i]].init(raw_pose.P[human_l_names[i]].p);
-       rot_filters[human_l_names[i]].init(raw_pose.P[human_l_names[i]].rpy);
-     }
-   }
-  }
-  robot_in->calcForwardKinematics();
+  callnum++;
   if(DEBUGP){ fprintf(stderr,"\x1b[31mmaster-mode:\x1b[39m"); raw_pose.print(); }
-  if(!hsp->isHumanSyncOn()){
-    q_normal_ik.resize(robot_in->numJoints());
-    for( int i=0; i<robot_in->numJoints(); i++){ q_normal_ik(i) = robot_in->joint(i)->q; }
-  }
-  if(hsp->isHumanSyncOn()){
-    for(int i=0;i<num_pose_tgt;i++){
-      raw_pose.P[i].p = pos_filters[i].passFilter(hsp->hp_wld_raw.P[i].p);
-      raw_pose.P[i].rpy = rot_filters[i].passFilter(hsp->hp_wld_raw.P[i].rpy);
-    }
-    solveFullbodyIKStrictCOM(fik, m_robot, raw_pose.P[com], raw_pose.P[rf], raw_pose.P[lf], raw_pose.P[rh], raw_pose.P[lh], raw_pose.P[head] );
-  }
+  solveFullbodyIKStrictCOM(fik_in, robot_in, raw_pose.P[com], raw_pose.P[rf], raw_pose.P[lf], raw_pose.P[rh], raw_pose.P[lh], raw_pose.P[head],"processWholeBodyMasterSlave_Raw");
 }
 
+
 void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, const std::string& debug_prefix){
   int com_ik_loop=0;
   const int COM_IK_MAX_LOOP = 10;
@@ -613,32 +537,29 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   if(com_ik_loop != 1)cout<<"com_ik_loop:"<<com_ik_loop<<" @ "<<debug_prefix<<endl;
 }
 
-void WholeBodyMasterSlave::processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in){
 
-  hrp::Vector3 P,L;
-  m_robot->rootLink()->v = idsb.base_v;
-  m_robot->rootLink()->w = idsb.base_w;
-  m_robot->calcTotalMomentum(P,L);
-  L = L - m_robot->calcCM().cross(P);
+void WholeBodyMasterSlave::processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_normal_in, const HumanPose& pose_ref){
+  hrp::Vector3 P,L;//世界座標系
+  robot_normal_in->rootLink()->v = idsb.base_v;
+  robot_normal_in->rootLink()->w = idsb.base_w;
+  robot_normal_in->calcTotalMomentum(P,L);
+  L = L - robot_normal_in->calcCM().cross(P);//重心周りの角運動量に変換
+  robot_normal_in->calcCM();
+  robot_normal_in->rootLink()->calcSubMassCM();
+  hrp::Matrix33 I_com;
   static hrp::Vector3 torso_rot;
   hrp::Vector3 torso_rot_vel;
-
-  m_robot->calcCM();
-  m_robot->rootLink()->calcSubMassCM();
-  hrp::Matrix33 I_com;
-
-  if(m_robot->link("CHEST_JOINT0")){
-    m_robot->link("CHEST_JOINT0")->calcSubMassInertia(I_com);
+  if(robot_normal_in->link("CHEST_JOINT0")){
+    robot_normal_in->link("CHEST_JOINT0")->calcSubMassInertia(I_com);
     torso_rot_vel = - I_com.inverse() * L * hsp->WBMSparam.upper_body_rmc_ratio;
     torso_rot += torso_rot_vel * m_dt;
   }
-  if(hsp->isHumanSyncOn()){
-    HRPPose3D com_mod = hsp->rp_ref_out.P[com];
-    com_mod.rpy += torso_rot;
-    solveFullbodyIKStrictCOM(fik_in, robot_in, com_mod, hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head],"calcDynamicsFilterCompensation");
-  }
+  HRPPose3D com_mod = pose_ref.P[com];
+  com_mod.rpy += torso_rot;
+  solveFullbodyIKStrictCOM(fik_in, robot_in, com_mod, pose_ref.P[rf], pose_ref.P[lf], pose_ref.P[rh], pose_ref.P[lh], pose_ref.P[head],"calcDynamicsFilterCompensation");
 }
 
+
 bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double sec){
   if(sec >= 0.0 && sec <= 30.0){
     std::cerr << "[" << m_profile.instance_name << "] start Synchronization after "<<sec<<" [s]" << std::endl;
@@ -652,11 +573,13 @@ bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double se
   }
 }
 
+
 bool WholeBodyMasterSlave::stopHumanSync(){
 	std::cerr << "[" << m_profile.instance_name << "] not implemented..." << std::endl;
 	return true;
 }
 
+
 bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
   std::cerr << "[" << m_profile.instance_name << "] setWholeBodyMasterSlaveParam" << std::endl;
   hsp->WBMSparam.auto_swing_foot_landing_threshold = i_param.auto_swing_foot_landing_threshold;
@@ -673,6 +596,7 @@ bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBody
   return true;
 }
 
+
 bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
   std::cerr << "[" << m_profile.instance_name << "] getWholeBodyMasterSlaveParam" << std::endl;
   i_param.auto_swing_foot_landing_threshold = hsp->WBMSparam.auto_swing_foot_landing_threshold;
@@ -689,6 +613,11 @@ bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMaster
   return true;
 }
 
+
+RTC::ReturnCode_t WholeBodyMasterSlave::onFinalize(){ return RTC::RTC_OK; }
+RTC::ReturnCode_t WholeBodyMasterSlave::onActivated(RTC::UniqueId ec_id){ std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; }
+RTC::ReturnCode_t WholeBodyMasterSlave::onDeactivated(RTC::UniqueId ec_id){ std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; }
+
 extern "C"{
     void WholeBodyMasterSlaveInit(RTC::Manager* manager) {
         RTC::Properties profile(WholeBodyMasterSlave_spec);
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index a5a2023bb87..6970a73bf68 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -1,12 +1,3 @@
-// -*- C++ -*-
-/*!
- * @file  WholeBodyMasterSlave.h
- * @brief WholeBodyMasterSlave component
- * @date  $Date$
- *
- * $Id$
- */
-
 #ifndef WholeBodyMasterSlave_H
 #define WholeBodyMasterSlave_H
 
@@ -21,7 +12,7 @@
 #include <rtm/idl/ExtendedDataTypesSkel.h>
 #include <hrpModel/Body.h>
 #include "../ImpedanceController/JointPathEx.h"
-#include "../ImpedanceController/RatsMatrix.h"
+//#include "../ImpedanceController/RatsMatrix.h"
 // Service implementation headers
 // <rtc-template block="service_impl_h">
 #include "WholeBodyMasterSlaveService_impl.h"
@@ -158,36 +149,34 @@ class WholeBodyMasterSlave
   std::map<std::string, size_t> contact_states_index_map;
   double m_dt;
   hrp::BodyPtr m_robot, m_robot_rmc;
-//  hrp::Vector3 input_ref_zmp, input_basePos;
-//  hrp::Matrix33 input_baseRot;
+  std::vector< std::pair<fikPtr, hrp::BodyPtr> > ik_robot_list;
+  static const enum ik_robot_usage{ basic, rmc } iru;
 
   double q_interpolator_ratio;
   interpolator *q_interpolator;
 
-  hrp::Vector3 ref_zmp;
-  hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
-
   unsigned int m_debugLevel;
-  bool is_legged_robot;// is_stop_mode, is_hand_fix_mode, is_hand_fix_initial;
-  int loop;
+  bool is_legged_robot;
+  unsigned int loop;
 
   hrp::InvDynStateBuffer idsb;
-  std::vector<IIRFilter> invdyn_zmp_filters;
+  BiquadIIRFilterVec invdyn_zmp_filters;
 
   hrp::InvDynStateBuffer idsb2;
-  std::vector<IIRFilter> invdyn_zmp_filters2;
+  BiquadIIRFilterVec invdyn_zmp_filters2;
 
-  hrp::Vector3 ref_zmp_invdyn2;
-
-  hrp::dvector q_normal_ik;
+  HumanPose raw_pose;
 
   boost::shared_ptr<HumanSynchronizer> hsp;
 
+
   void setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in);
   void solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, const std::string& debug_prefix="");
+  void processTransition();
+  void preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in);
   void processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in);
   void processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::BodyPtr& robot_in);
-  void processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in);
+  void processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_normal_in, const HumanPose& pose_ref);
   bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; };
 };
 
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 32a734bdfb4..826f610708e 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -406,6 +406,9 @@ class HumanSynchronizer{
       if(DEBUG)fprintf(cz_log,"lf: %f %f %f ",rp_ref_out.P[lf].p(X),rp_ref_out.P[lf].p(Y),rp_ref_out.P[lf].p(Z));
       if(DEBUG)fprintf(cz_log,"\n");
 
+
+
+      H_cur = rp_ref_out.P[com].p(Z) - std::min((double)rp_ref_out.P[rf].p(Z), (double)rp_ref_out.P[rf].p(Z));
       com_vel_old = (rp_ref_out.P[com].p - rp_ref_out_old.P[com].p)/DT;
       rp_ref_out_old = rp_ref_out;
       loop++;

From 01e09a57b1e4bc65964c19eb67236f5d461e708a Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 15 May 2017 22:33:19 +0900
Subject: [PATCH 086/217] add manipulability limit

---
 idl/WholeBodyMasterSlaveService.idl           |  1 +
 .../WholeBodyMasterSlave.cpp                  | 89 +++++++++++++++----
 .../WholeBodyMasterSlave.h                    |  1 +
 rtc/WholeBodyMasterSlave/wbms_core.h          |  7 +-
 4 files changed, 78 insertions(+), 20 deletions(-)

diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
index ff31983cf82..2f96332b02e 100644
--- a/idl/WholeBodyMasterSlaveService.idl
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -47,6 +47,7 @@ module OpenHRP
         double upper_body_rmc_ratio;
         boolean use_hands;
         boolean use_head;
+        boolean use_manipulability_limit;
     };
     boolean startCountDownForWholeBodyMasterSlave(in double sec);
     boolean stopHumanSync();
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index ce01ba3326c..04fe963dd99 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -159,6 +159,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
     m_qRef.data.length(m_robot->numJoints());
     m_htrfw.data.length(6);
     m_htlfw.data.length(6);
+    coil::stringTo(optionalDataLength, prop["seq_optional_data_dim"].c_str());
     loop = 0;
     q_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
     q_interpolator->setName(std::string(m_profile.instance_name)+" q_interpolator");
@@ -300,10 +301,12 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         updateInvDynStateBuffer(idsb);
       }
 
-      if(hsp->isHumanSyncOn() && hsp->WBMSparam.is_doctor){//角運動量補償
-        processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, hsp->rp_ref_out);
-      }else{
-        processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, raw_pose);
+      if(hsp->isHumanSyncOn()){
+        if(hsp->WBMSparam.is_doctor){//角運動量補償
+          processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, hsp->rp_ref_out);
+        }else{
+          processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, raw_pose);
+        }
       }
 //      calcAccelerationsForInverseDynamics(m_robot, idsb2);
 //      calcWorldZMPFromInverseDynamics(m_robot, idsb2, ref_zmp_invdyn2);
@@ -312,7 +315,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
       if (hsp->isHumanSyncOn()){//OutPortデータセット
         //        ref_zmp = ref_zmp_invdyn;
         hrp::Vector3 ref_zmp = hsp->rp_ref_out.P[zmp].p;
-        //      hrp::BodyPtr m_robot_for_out = m_robot;
+//        hrp::BodyPtr m_robot_for_out = m_robot;
         hrp::BodyPtr m_robot_for_out = m_robot_rmc;
         // qRef
         for (int i = 0; i < m_qRef.data.length(); i++ ){ m_qRef.data[i] = q_interpolator_ratio * m_robot_for_out->joint(i)->q  + (1 - q_interpolator_ratio) * m_qRef.data[i]; }
@@ -334,9 +337,9 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         m_zmp.data.z = rel_ref_zmp(2);
         m_zmp.tm = m_qRef.tm;
         // m_optionalData
-        if(m_optionalData.data.length() < 4*2){
-          m_optionalData.data.length(4*2);//TODO:これいいのか？
-          for(int i=0;i<4*2;i++)m_optionalData.data[i] = 0;
+        if(m_optionalData.data.length() < optionalDataLength){
+          m_optionalData.data.length(optionalDataLength);//TODO:これいいのか？
+          for(int i=0;i<optionalDataLength;i++)m_optionalData.data[i] = 0;
         }
         m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[contact_states_index_map["rleg"]+4] = hsp->is_rf_contact;
         m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[contact_states_index_map["lleg"]+4] = hsp->is_lf_contact;
@@ -505,6 +508,7 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::
 
 
 void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, const std::string& debug_prefix){
+
   int com_ik_loop=0;
   const int COM_IK_MAX_LOOP = 10;
   const double COM_IK_MAX_ERROR = 1e-5;//1e-4だと乱れる
@@ -523,18 +527,65 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   if(robot_in->link("HEAD_JOINT0") != NULL)robot_in->joint(15)->q = head_ref.rpy(y);
   if(robot_in->link("HEAD_JOINT1") != NULL)robot_in->joint(16)->q = head_ref.rpy(p);
   hrp::Vector3 tmp_com_err = hrp::Vector3::Zero();
-  while( 1 ){  //COM 収束ループ
-    com_ik_loop++;
-    robot_in->rootLink()->p += tmp_com_err;
-    robot_in->calcForwardKinematics();
-    for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_in->ikp.begin(); it != fik_in->ikp.end(); it++ ) {
-        if (it->second.is_ik_enable) fik_in->solveLimbIK (it->second, it->first, fik_in->ratio_for_vel, false);
+
+
+
+  if(hsp->WBMSparam.use_manipulability_limit){
+    double min_manip_val = 1e9;
+    const double min_manip_th = 0.1;
+    while( 1 ){  //COM 収束ループ＋可操作度リミット
+      com_ik_loop++;
+      robot_in->rootLink()->p += tmp_com_err;
+      robot_in->calcForwardKinematics();
+      for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_in->ikp.begin(); it != fik_in->ikp.end(); it++ ) {
+          if (it->second.is_ik_enable) fik_in->solveLimbIK (it->second, it->first, fik_in->ratio_for_vel, false);
+      }
+      tmp_com_err = com_ref.p - robot_in->calcCM();
+
+      const std::string names[4] = {"rleg","lleg","rarm","larm"};
+      const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
+      for(int i=0;i<4;i++){
+        hrp::dmatrix J;
+        fik_in->ikp[names[i]].manip->calcJacobian(J);
+        hrp::dmatrix manipulability_mat = J*J.transpose();
+        double manipulability = sqrt((J*J.transpose()).determinant());
+        //    Eigen::JacobiSVD< Eigen::Matrix<float, 3, 3> > svd(mm3, Eigen::ComputeFullU | Eigen::ComputeFullV);
+        Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+        Eigen::MatrixXd::Index row,col;
+        min_manip_val = svd.singularValues().minCoeff(&row,&col);
+
+        hrp::Vector3 min_manip_direc = svd.matrixU().col(row);
+        //      std::cout << "singular values" << std::endl << svd.singularValues() << std::endl;
+        //      std::cout << "matrix U" << std::endl << svd.matrixU() << std::endl;
+        //      std::cout << "matrix V" << std::endl << svd.matrixV() << std::endl;
+        if(min_manip_direc.dot(fik_in->ikp[names[i]].target_p0 - robot_in->rootLink()->p) > 0){ min_manip_direc *= -1; }//向きみて反転
+
+        if(min_manip_val<min_manip_th){
+          std::cout << names[i]<<" min_manip_direc: " << min_manip_direc.transpose() << "min_manip_val " << min_manip_val << std::endl;
+          hrp::Vector3 cur_p = fik_in->ikp[names[i]].target_link->p+ fik_in->ikp[names[i]].target_link->R * fik_in->ikp[names[i]].localPos;
+          fik_in->ikp[names[i]].target_p0 = cur_p + min_manip_direc * 1.0 * (min_manip_th - min_manip_val);
+        }
+      }
+      if(tmp_com_err.norm() < COM_IK_MAX_ERROR && min_manip_val >= min_manip_th){ break; }
+      if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
+
+    }
+  }else{
+    while( 1 ){  //COM 収束ループ
+      com_ik_loop++;
+      robot_in->rootLink()->p += tmp_com_err;
+      robot_in->calcForwardKinematics();
+      for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_in->ikp.begin(); it != fik_in->ikp.end(); it++ ) {
+          if (it->second.is_ik_enable) fik_in->solveLimbIK (it->second, it->first, fik_in->ratio_for_vel, false);
+      }
+      tmp_com_err = com_ref.p - robot_in->calcCM();
+      if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
+      if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
     }
-    tmp_com_err = com_ref.p - robot_in->calcCM();
-    if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
-    if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
   }
-  if(com_ik_loop != 1)cout<<"com_ik_loop:"<<com_ik_loop<<" @ "<<debug_prefix<<endl;
+
+  if(com_ik_loop != 1 && DEBUGP)cout<<"com_ik_loop:"<<com_ik_loop<<" @ "<<debug_prefix<<endl;
 }
 
 
@@ -593,6 +644,7 @@ bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBody
   hsp->WBMSparam.upper_body_rmc_ratio = i_param.upper_body_rmc_ratio;
   hsp->WBMSparam.use_rh = hsp->WBMSparam.use_lh = i_param.use_hands;
   hsp->WBMSparam.use_head = i_param.use_head;
+  hsp->WBMSparam.use_manipulability_limit = i_param.use_manipulability_limit;
   return true;
 }
 
@@ -610,6 +662,7 @@ bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMaster
   i_param.upper_body_rmc_ratio = hsp->WBMSparam.upper_body_rmc_ratio;
   i_param.use_hands = (hsp->WBMSparam.use_rh || hsp->WBMSparam.use_lh);
   i_param.use_head = hsp->WBMSparam.use_head;
+  i_param.use_manipulability_limit = hsp->WBMSparam.use_manipulability_limit;
   return true;
 }
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 6970a73bf68..217df6c9162 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -155,6 +155,7 @@ class WholeBodyMasterSlave
   double q_interpolator_ratio;
   interpolator *q_interpolator;
 
+  int optionalDataLength;
   unsigned int m_debugLevel;
   bool is_legged_robot;
   unsigned int loop;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 826f610708e..433c0ee2295 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -219,6 +219,7 @@ class HumanSynchronizer{
       double upper_body_rmc_ratio;
       bool use_rh,use_lh;
       bool use_head;
+      bool use_manipulability_limit;
     };
     struct WBMSparameters WBMSparam;
 
@@ -301,6 +302,7 @@ class HumanSynchronizer{
       WBMSparam.upper_body_rmc_ratio = 0.5;
       WBMSparam.use_rh = WBMSparam.use_lh = true;
       WBMSparam.use_head = true;
+      WBMSparam.use_manipulability_limit = false;
 
       rp_ref_out_old.clear();
       rp_ref_out.clear();
@@ -573,8 +575,9 @@ class HumanSynchronizer{
       }
     }
     void applyEEWorkspaceLimit(HumanPose& tgt){
-//      const double MAX_FW = 0.25;
-      const double MAX_FW = 0.5;
+      const double MAX_FW = 0.25;
+//      const double MAX_FW = 0.5;
+//      const double MAX_FW = 1000000;//manipulability test
       const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
       if(!is_rf_contact && is_lf_contact){//右足浮遊時
         const hrp::Vector2 lf2rf_vec( tgt.P[rf].p(X)-pre_cont_lfpos(X), tgt.P[rf].p(Y)-pre_cont_lfpos(Y) );

From 4d3e580e48e90431a8986979d341279481c467f8 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 17 May 2017 14:28:33 +0900
Subject: [PATCH 087/217] tune for demo

---
 rtc/WholeBodyMasterSlave/wbms_core.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 433c0ee2295..7c3e627245b 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -274,7 +274,7 @@ class HumanSynchronizer{
       for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//四肢拘束点用(Rotation)
       tgt_pos_filters[0].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//重心pos用
       tgt_rot_filters[0].setParameter(0.6, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//重心rot用
-      tgt_pos_filters[1].setParameter(hrp::Vector3(5.0,1.0,1.2), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//右足pos用
+      tgt_pos_filters[1].setParameter(hrp::Vector3(1.0,1.0,1.2), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//右足pos用
       tgt_pos_filters[2].setParameter(hrp::Vector3(1.0,1.0,1.2), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//左足pos用
       calcacc_v_filters.setParameter(5, HZ, BiquadIIRFilterVec::Q_BUTTERWORTH);//加速度計算用
       acc4zmp_v_filters.setParameter(5, HZ, BiquadIIRFilterVec::Q_BUTTERWORTH);//ZMP生成用ほぼこの値でいい

From 54ebe9951723262664ec128d73a5426b4fb59ccc Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 17 May 2017 15:26:12 +0900
Subject: [PATCH 088/217] fix zmp height abou init_foot_mid_coord

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 04fe963dd99..9a134bf550f 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -448,6 +448,7 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
   hrp::Vector3 init_foot_mid_coord = (rsole_pos + lsole_pos) / 2;
   if( fabs((double)init_foot_mid_coord(Z)) > 1e-5 ){
     basePos_heightChecked(Z) -= init_foot_mid_coord(Z);
+    init_foot_mid_coord(Z) = 0;
     std::cerr<<"["<<m_profile.instance_name<<"] Input basePos height is invalid. Auto modify "<<m_basePos.data.z<<" -> "<<basePos_heightChecked(Z)<<endl;
   }
   for(int i=0;i<ik_robot_list.size();i++){//初期姿勢でBodyをFK

From d59c542b1f85738153ea0bfedb7131cb48d34548 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 21 May 2017 23:36:24 +0900
Subject: [PATCH 089/217] refactor data structure

---
 .../WholeBodyMasterSlave.cpp                  | 142 ++--
 .../WholeBodyMasterSlave.h                    | 190 +++--
 rtc/WholeBodyMasterSlave/wbms_core.cpp        | 234 +++---
 rtc/WholeBodyMasterSlave/wbms_core.h          | 771 +++++++++---------
 4 files changed, 644 insertions(+), 693 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 9a134bf550f..6f735794648 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -7,8 +7,8 @@
 #include <hrpUtil/MatrixSolvers.h>
 #include "hrpsys/util/Hrpsys.h"
 
-//typedef coil::Guard<coil::Mutex> Guard;
-//using namespace rats;
+#define DEBUGP (loop%200==0)
+#define DEBUGP_ONCE (loop==0)
 
 static const char* WholeBodyMasterSlave_spec[] =
     {
@@ -26,22 +26,18 @@ static const char* WholeBodyMasterSlave_spec[] =
         ""
     };
 
-WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager)
-    : RTC::DataFlowComponentBase(manager),
-      // from sh
-      m_qRefIn("qRef", m_qRef),
+WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager),
+      m_qRefIn("qRef", m_qRef),// from sh
       m_zmpIn("zmpIn", m_zmp),
       m_basePosIn("basePosIn", m_basePos),
       m_baseRpyIn("baseRpyIn", m_baseRpy),
       m_optionalDataIn("optionalData", m_optionalData),
-      // to abc
-      m_qOut("q", m_qRef),
+      m_qOut("q", m_qRef),// to abc
       m_zmpOut("zmpOut", m_zmp),
       m_basePosOut("basePosOut", m_basePos),
       m_baseRpyOut("baseRpyOut", m_baseRpy),
       m_optionalDataOut("optionalDataOut", m_optionalData),
-      // from ros bridge
-      m_htzmpIn("htzmpIn", m_htzmp),
+      m_htzmpIn("htzmpIn", m_htzmp),// from ros bridge
       m_htrfwIn("htrfwIn", m_htrfw),
       m_htlfwIn("htlfwIn", m_htlfw),
       m_htcomIn("htcomIn", m_htcom),
@@ -51,10 +47,8 @@ WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager)
       m_htlhIn("htlhIn", m_htlh),
 //      m_actzmpIn("actzmpIn", m_actzmp),
       m_htheadIn("htheadIn", m_hthead),
-
 #ifdef USE_DEBUG_PORT
-      // to ros bridge
-       m_htcom_dbgOut("htcom_dbgOut", m_htcom_dbg),
+       m_htcom_dbgOut("htcom_dbgOut", m_htcom_dbg),// to ros bridge
        m_htrf_dbgOut("htrf_dbgOut", m_htrf_dbg),
        m_htlf_dbgOut("htlf_dbgOut", m_htlf_dbg),
        m_htrh_dbgOut("htrh_dbgOut", m_htrh_dbg),
@@ -83,24 +77,20 @@ WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager)
 
 WholeBodyMasterSlave::~WholeBodyMasterSlave(){}
 
-RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
-{
+RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
     bindParameter("debugLevel", m_debugLevel, "0");
-
-    addInPort("qRef", m_qRefIn);
+    addInPort("qRef", m_qRefIn);// from sh
     addInPort("zmpIn", m_zmpIn);
     addInPort("basePosIn", m_basePosIn);
     addInPort("baseRpyIn", m_baseRpyIn);
     addInPort("optionalData", m_optionalDataIn);
-
-    addOutPort("q", m_qOut);
+    addOutPort("q", m_qOut);// to abc
     addOutPort("zmpOut", m_zmpOut);
     addOutPort("basePosOut", m_basePosOut);
     addOutPort("baseRpyOut", m_baseRpyOut);
     addOutPort("optionalDataOut", m_optionalDataOut);
-
-    addInPort("htzmpIn", m_htzmpIn);
+    addInPort("htzmpIn", m_htzmpIn);// from ros bridge
     addInPort("htrfwIn", m_htrfwIn);
     addInPort("htlfwIn", m_htlfwIn);
     addInPort("htcomIn", m_htcomIn);
@@ -110,9 +100,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
     addInPort("htlhIn", m_htlhIn);
 //    addInPort("actzmpIn", m_actzmpIn);
     addInPort("htheadIn", m_htheadIn);
-
 #ifdef USE_DEBUG_PORT
-     addOutPort("htcom_dbgOut", m_htcom_dbgOut);
+     addOutPort("htcom_dbgOut", m_htcom_dbgOut);// to ros bridge
      addOutPort("htrf_dbgOut", m_htrf_dbgOut);
      addOutPort("htlf_dbgOut", m_htlf_dbgOut);
      addOutPort("htrh_dbgOut", m_htrh_dbgOut);
@@ -132,7 +121,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize()
      addOutPort("rpacp_dbgOut", m_rpacp_dbgOut);
      addOutPort("invdyn_dbgOut", m_invdyn_dbgOut);
 #endif
-
     m_WholeBodyMasterSlaveServicePort.registerProvider("service0", "WholeBodyMasterSlaveService", m_service0);
     addPort(m_WholeBodyMasterSlaveServicePort);
   
@@ -239,8 +227,6 @@ void WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC:
   }
 }
 
-#define DEBUGP (loop%200==0)
-#define DEBUGP_ONCE (loop==0)
 RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
   if(DEBUGP_ONCE)std::cerr << "[" << m_profile.instance_name<< "] onExecute(" << ec_id << ")" << std::endl;
   // struct timeval t_calc_start, t_calc_end;
@@ -251,15 +237,15 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if (m_zmpIn.isNew()) { m_zmpIn.read(); }
     if (m_optionalDataIn.isNew()) { m_optionalDataIn.read(); }
     //for HumanSynchronizer
-    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htrfw.data,hsp->hp_wld_raw.w[rfw]); }
-    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToWrench6(m_htlfw.data,hsp->hp_wld_raw.w[lfw]); }
-    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_htcom.data,hsp->hp_wld_raw.P[com]); }
-    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrf.data,hsp->hp_wld_raw.P[rf]); }
-    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlf.data,hsp->hp_wld_raw.P[lf]); }
-    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htrh.data,hsp->hp_wld_raw.P[rh]);}
-    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToHRPPose3D(m_htlh.data,hsp->hp_wld_raw.P[lh]);}
-    if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToHRPPose3D(m_hthead.data,hsp->hp_wld_raw.P[head]);}
-    if (m_htzmpIn.isNew()){ m_htzmpIn.read();  HumanSynchronizer::Point3DToVector3(m_htzmp.data,hsp->hp_wld_raw.P[zmp].p); }
+    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToVector6(m_htrfw.data,hsp->hp_wld_raw.tgt[rf].w); }
+    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToVector6(m_htlfw.data,hsp->hp_wld_raw.tgt[lf].w); }
+    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Pose3DToWBMSPose3D(m_htcom.data,hsp->hp_wld_raw.tgt[com].abs); }
+    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Pose3DToWBMSPose3D(m_htrf.data,hsp->hp_wld_raw.tgt[rf].abs); }
+    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Pose3DToWBMSPose3D(m_htlf.data,hsp->hp_wld_raw.tgt[lf].abs); }
+    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToWBMSPose3D(m_htrh.data,hsp->hp_wld_raw.tgt[rh].abs);}
+    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToWBMSPose3D(m_htlh.data,hsp->hp_wld_raw.tgt[lh].abs);}
+    if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToWBMSPose3D(m_hthead.data,hsp->hp_wld_raw.tgt[head].abs);}
+    if (m_htzmpIn.isNew()){ m_htzmpIn.read();  HumanSynchronizer::Point3DToVector3(m_htzmp.data,hsp->hp_wld_raw.tgt[zmp].abs.p); }
 //    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
 
     if ( is_legged_robot ) {
@@ -314,7 +300,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
       if (hsp->isHumanSyncOn()){//OutPortデータセット
         //        ref_zmp = ref_zmp_invdyn;
-        hrp::Vector3 ref_zmp = hsp->rp_ref_out.P[zmp].p;
+        hrp::Vector3 ref_zmp = hsp->rp_ref_out.tgt[zmp].abs.p;
 //        hrp::BodyPtr m_robot_for_out = m_robot;
         hrp::BodyPtr m_robot_for_out = m_robot_rmc;
         // qRef
@@ -341,8 +327,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
           m_optionalData.data.length(optionalDataLength);//TODO:これいいのか？
           for(int i=0;i<optionalDataLength;i++)m_optionalData.data[i] = 0;
         }
-        m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[contact_states_index_map["rleg"]+4] = hsp->is_rf_contact;
-        m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[contact_states_index_map["lleg"]+4] = hsp->is_lf_contact;
+        m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[contact_states_index_map["rleg"]+4] = hsp->rp_ref_out.tgt[rf].is_contact;
+        m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[contact_states_index_map["lleg"]+4] = hsp->rp_ref_out.tgt[lf].is_contact;
       }
       if(DEBUGP_ONCE)std::cerr << "[" << m_profile.instance_name<< "] onExecute(" << ec_id << ")" << std::endl;
       hsp->baselinkpose.p = m_robot->rootLink()->p;
@@ -358,54 +344,54 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 #ifdef USE_DEBUG_PORT
     // dbg plot
      m_htcom.tm = m_qRef.tm;
-     HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[com],m_htcom_dbg.data);
+     HumanSynchronizer::WBMSPose3DToPose3D(hsp->hp_plot.tgt[com].abs, m_htcom_dbg.data);
      m_htcom_dbgOut.write();
      m_htrf.tm = m_qRef.tm;
-     HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[rf],m_htrf_dbg.data);
+     HumanSynchronizer::WBMSPose3DToPose3D(hsp->hp_plot.tgt[rf].abs, m_htrf_dbg.data);
      m_htrf_dbgOut.write();
      m_htlf.tm = m_qRef.tm;
-     HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[lf],m_htlf_dbg.data);
+     HumanSynchronizer::WBMSPose3DToPose3D(hsp->hp_plot.tgt[lf].abs, m_htlf_dbg.data);
      m_htlf_dbgOut.write();
      m_htrh.tm = m_qRef.tm;
-     HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[rh],m_htrh_dbg.data);
+     HumanSynchronizer::WBMSPose3DToPose3D(hsp->hp_plot.tgt[rh].abs, m_htrh_dbg.data);
      m_htrh_dbgOut.write();
      m_htlh.tm = m_qRef.tm;
-     HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[lh],m_htlh_dbg.data);
+     HumanSynchronizer::WBMSPose3DToPose3D(hsp->hp_plot.tgt[lh].abs, m_htlh_dbg.data);
      m_htlh_dbgOut.write();
      m_hthead.tm = m_qRef.tm;
-     HumanSynchronizer::HRPPose3DToPose3D(hsp->hp_plot.P[head],m_hthead_dbg.data);
+     HumanSynchronizer::WBMSPose3DToPose3D(hsp->hp_plot.tgt[head].abs, m_hthead_dbg.data);
      m_hthead_dbgOut.write();
      m_htzmp.tm = m_qRef.tm;
-     HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.P[zmp].p,m_rpzmp_dbg.data);
+     HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.tgt[zmp].abs.p,m_rpzmp_dbg.data);
      m_htzmp_dbgOut.write();
      m_htrfw.tm = m_qRef.tm;
      m_htrfw_dbg.data.length(6);
-     HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.w[rfw],m_htrfw_dbg.data);
+     HumanSynchronizer::Vector6ToDoubleSeq(hsp->hp_plot.tgt[rf].w, m_htrfw_dbg.data);
      m_htrfw_dbgOut.write();
      m_htlfw.tm = m_qRef.tm;
      m_htlfw_dbg.data.length(6);
-     HumanSynchronizer::Wrench6ToDoubleSeq(hsp->hp_plot.w[lfw],m_htlfw_dbg.data);
+     HumanSynchronizer::Vector6ToDoubleSeq(hsp->hp_plot.tgt[lf].w, m_htlfw_dbg.data);
      m_htlfw_dbgOut.write();
      m_rpcom_dbg.tm = m_qRef.tm;
-     HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[com],m_rpcom_dbg.data);
+     HumanSynchronizer::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[com].abs, m_rpcom_dbg.data);
      m_rpcom_dbgOut.write();
      m_rprf_dbg.tm = m_qRef.tm;
-     HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[rf],m_rprf_dbg.data);
+     HumanSynchronizer::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[rf].abs, m_rprf_dbg.data);
      m_rprf_dbgOut.write();
      m_rplf_dbg.tm = m_qRef.tm;
-     HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[lf],m_rplf_dbg.data);
+     HumanSynchronizer::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[lf].abs, m_rplf_dbg.data);
      m_rplf_dbgOut.write();
      m_rprh_dbg.tm = m_qRef.tm;
-     HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[rh],m_rprh_dbg.data);
+     HumanSynchronizer::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[rh].abs, m_rprh_dbg.data);
      m_rprh_dbgOut.write();
      m_rplh_dbg.tm = m_qRef.tm;
-     HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[lh],m_rplh_dbg.data);
+     HumanSynchronizer::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[lh].abs, m_rplh_dbg.data);
      m_rplh_dbgOut.write();
      m_rphead_dbg.tm = m_qRef.tm;
-     HumanSynchronizer::HRPPose3DToPose3D(hsp->rp_ref_out.P[head],m_rphead_dbg.data);
+     HumanSynchronizer::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[head].abs, m_rphead_dbg.data);
      m_rphead_dbgOut.write();
      m_rpzmp_dbg.tm = m_qRef.tm;
-     HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.P[zmp].p,m_rpzmp_dbg.data);
+     HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.tgt[zmp].abs.p, m_rpzmp_dbg.data);
      m_rpzmp_dbgOut.write();
      m_rpdcp_dbg.tm = m_qRef.tm;
      HumanSynchronizer::Vector3ToPoint3D(hsp->cp_dec,m_rpdcp_dbg.data);
@@ -415,7 +401,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
      m_rpacp_dbgOut.write();
      m_invdyn_dbg.tm = m_qRef.tm;
      m_invdyn_dbg.data.length(6);
-     HumanSynchronizer::Wrench6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
+     HumanSynchronizer::Vector6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
      m_invdyn_dbgOut.write();
 #endif
      // gettimeofday(&t_calc_end, NULL);
@@ -459,25 +445,23 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
     ik_robot_list[i].first->setReferenceJointAngles();
   }
   hsp->setCurrentInputAsOffset(hsp->hp_wld_raw);//現在の入力を人間の初期姿勢としてセット
-  hsp->calibInitHumanCOMFromZMP();//今あんま使ってない
   const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
   const int human_l_names[4] = {rf,lf,rh,lh};
   for(int i=0;i<4;i++){//HumanSynchronizerの初期姿勢オフセットをセット
     if(fik_in->ikp.count(robot_l_names[i])){
-      hsp->rp_ref_out.P[human_l_names[i]].p_offs = fik_in->ikp[robot_l_names[i]].target_link->p + fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localPos;
-      hsp->rp_ref_out.P[human_l_names[i]].rpy_offs = hrp::rpyFromRot(fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localR);
-      hsp->rp_ref_out.P[human_l_names[i]].p = hsp->rp_ref_out.P[human_l_names[i]].p_offs;
-      hsp->rp_ref_out.P[human_l_names[i]].rpy = hsp->rp_ref_out.P[human_l_names[i]].rpy_offs;
+      hsp->rp_ref_out.tgt[human_l_names[i]].offs.p = fik_in->ikp[robot_l_names[i]].target_link->p + fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localPos;
+      hsp->rp_ref_out.tgt[human_l_names[i]].offs.rpy = hrp::rpyFromRot(fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localR);
+      hsp->rp_ref_out.tgt[human_l_names[i]].abs = hsp->rp_ref_out.tgt[human_l_names[i]].offs;
     }
   }
-  hsp->rp_ref_out.P[com].p_offs = robot_in->calcCM();
-  hsp->rp_ref_out.P[zmp].p_offs(X) = hsp->rp_ref_out.P[com].p_offs(X);
-  hsp->rp_ref_out.P[zmp].p_offs(Y) = hsp->rp_ref_out.P[com].p_offs(Y);
-  hsp->rp_ref_out.P[zmp].p_offs(Z) = init_foot_mid_coord(Z);
-  hsp->pre_cont_rfpos = hsp->rp_ref_out.P[rf].p_offs;
-  hsp->pre_cont_lfpos = hsp->rp_ref_out.P[lf].p_offs;
-  hsp->baselinkpose.p_offs = robot_in->rootLink()->p;
-  hsp->baselinkpose.rpy_offs = hrp::rpyFromRot(robot_in->rootLink()->R);
+  hsp->rp_ref_out.tgt[com].offs.p = robot_in->calcCM();
+  hsp->rp_ref_out.tgt[zmp].offs.p(X) = hsp->rp_ref_out.tgt[com].offs.p(X);
+  hsp->rp_ref_out.tgt[zmp].offs.p(Y) = hsp->rp_ref_out.tgt[com].offs.p(Y);
+  hsp->rp_ref_out.tgt[zmp].offs.p(Z) = init_foot_mid_coord(Z);
+  hsp->rp_ref_out.tgt[rf].cnt = hsp->rp_ref_out.tgt[rf].offs;
+  hsp->rp_ref_out.tgt[lf].cnt = hsp->rp_ref_out.tgt[lf].offs;
+  hsp->baselinkpose.p = robot_in->rootLink()->p;
+  hsp->baselinkpose.rpy = hrp::rpyFromRot(robot_in->rootLink()->R);
 }
 
 
@@ -485,7 +469,7 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(fikPtr& fik_in, hrp::Body
   hsp->update();//////HumanSynchronizerの主要処理
   if(DEBUGP)cout<<"update():"<<hsp->getUpdateTime()<<endl;
   if(DEBUGP)hsp->rp_ref_out.print();
-  solveFullbodyIKStrictCOM(fik_in, robot_in, hsp->rp_ref_out.P[com], hsp->rp_ref_out.P[rf], hsp->rp_ref_out.P[lf], hsp->rp_ref_out.P[rh], hsp->rp_ref_out.P[lh], hsp->rp_ref_out.P[head],"processWholeBodyMasterSlave");
+  solveFullbodyIKStrictCOM(fik_in, robot_in, hsp->rp_ref_out.tgt[com].abs, hsp->rp_ref_out.tgt[rf].abs, hsp->rp_ref_out.tgt[lf].abs, hsp->rp_ref_out.tgt[rh].abs, hsp->rp_ref_out.tgt[lh].abs, hsp->rp_ref_out.tgt[head].abs,"processWholeBodyMasterSlave");
 }
 
 
@@ -496,19 +480,19 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::
     if(callnum == 0){
       pos_filters[i].setParameter(100.0, 1.0/m_dt, BiquadIIRFilterVec::Q_NOOVERSHOOT);
       rot_filters[i].setParameter(100.0, 1.0/m_dt, BiquadIIRFilterVec::Q_NOOVERSHOOT);
-      pos_filters[i].init(hsp->hp_wld_raw.P[i].p);
-      rot_filters[i].init(hsp->hp_wld_raw.P[i].rpy);
+      pos_filters[i].init(hsp->hp_wld_raw.tgt[i].abs.p);
+      rot_filters[i].init(hsp->hp_wld_raw.tgt[i].abs.rpy);
     }
-    raw_pose.P[i].p = pos_filters[i].passFilter(hsp->hp_wld_raw.P[i].p);
-    raw_pose.P[i].rpy = rot_filters[i].passFilter(hsp->hp_wld_raw.P[i].rpy);
+    raw_pose.tgt[i].abs.p = pos_filters[i].passFilter(hsp->hp_wld_raw.tgt[i].abs.p);
+    raw_pose.tgt[i].abs.rpy = rot_filters[i].passFilter(hsp->hp_wld_raw.tgt[i].abs.rpy);
   }
   callnum++;
   if(DEBUGP){ fprintf(stderr,"\x1b[31mmaster-mode:\x1b[39m"); raw_pose.print(); }
-  solveFullbodyIKStrictCOM(fik_in, robot_in, raw_pose.P[com], raw_pose.P[rf], raw_pose.P[lf], raw_pose.P[rh], raw_pose.P[lh], raw_pose.P[head],"processWholeBodyMasterSlave_Raw");
+  solveFullbodyIKStrictCOM(fik_in, robot_in, raw_pose.tgt[com].abs, raw_pose.tgt[rf].abs, raw_pose.tgt[lf].abs, raw_pose.tgt[rh].abs, raw_pose.tgt[lh].abs, raw_pose.tgt[head].abs,"processWholeBodyMasterSlave_Raw");
 }
 
 
-void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, const std::string& debug_prefix){
+void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix){
 
   int com_ik_loop=0;
   const int COM_IK_MAX_LOOP = 10;
@@ -516,7 +500,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
 //  robot_in->rootLink()->p = 0.01*com_ref.p + 0.99*robot_in->rootLink()->p;//ベースリンク位置が迷走するのを防ぐ？
   robot_in->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
   const std::string names[4] = {"rleg","lleg","rarm","larm"};
-  const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
+  const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
   for(int i=0;i<4;i++){
     if(fik_in->ikp.count(names[i])){
       fik_in->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);
@@ -544,7 +528,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
       tmp_com_err = com_ref.p - robot_in->calcCM();
 
       const std::string names[4] = {"rleg","lleg","rarm","larm"};
-      const HRPPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
+      const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
       for(int i=0;i<4;i++){
         hrp::dmatrix J;
         fik_in->ikp[names[i]].manip->calcJacobian(J);
@@ -606,9 +590,9 @@ void WholeBodyMasterSlave::processMomentumCompensation(fikPtr& fik_in, hrp::Body
     torso_rot_vel = - I_com.inverse() * L * hsp->WBMSparam.upper_body_rmc_ratio;
     torso_rot += torso_rot_vel * m_dt;
   }
-  HRPPose3D com_mod = pose_ref.P[com];
+  WBMSPose3D com_mod = pose_ref.tgt[com].abs;
   com_mod.rpy += torso_rot;
-  solveFullbodyIKStrictCOM(fik_in, robot_in, com_mod, pose_ref.P[rf], pose_ref.P[lf], pose_ref.P[rh], pose_ref.P[lh], pose_ref.P[head],"calcDynamicsFilterCompensation");
+  solveFullbodyIKStrictCOM(fik_in, robot_in, com_mod, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"calcDynamicsFilterCompensation");
 }
 
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 217df6c9162..eb791e9ef23 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -12,7 +12,6 @@
 #include <rtm/idl/ExtendedDataTypesSkel.h>
 #include <hrpModel/Body.h>
 #include "../ImpedanceController/JointPathEx.h"
-//#include "../ImpedanceController/RatsMatrix.h"
 // Service implementation headers
 // <rtc-template block="service_impl_h">
 #include "WholeBodyMasterSlaveService_impl.h"
@@ -34,108 +33,105 @@ using namespace RTC;
 //    return os;
 //}
 
+
 #define USE_DEBUG_PORT
 
-class WholeBodyMasterSlave
-  : public RTC::DataFlowComponentBase
-{
-    DEF_UTIL_CONST
-
- public:
-  WholeBodyMasterSlave(RTC::Manager* manager);
-  virtual ~WholeBodyMasterSlave();
-  virtual RTC::ReturnCode_t onInitialize();
-  virtual RTC::ReturnCode_t onFinalize();
-  virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
-  virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
-  virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
-  bool startCountDownForWholeBodyMasterSlave(const double sec);
-  bool stopHumanSync();
-  bool setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
-  bool getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
-
- protected:
-  TimedDoubleSeq m_qRef;
-  InPort<TimedDoubleSeq> m_qRefIn;
-  TimedPoint3D m_basePos;
-  InPort<TimedPoint3D> m_basePosIn;
-  TimedOrientation3D m_baseRpy;
-  InPort<TimedOrientation3D> m_baseRpyIn;
-  TimedPoint3D m_zmp;
-  InPort<TimedPoint3D> m_zmpIn;
-  TimedDoubleSeq m_optionalData;
-  InPort<TimedDoubleSeq> m_optionalDataIn;
-
-  TimedPose3D m_htcom;
-  InPort<TimedPose3D> m_htcomIn;
-  TimedPose3D m_htrf;
-  InPort<TimedPose3D> m_htrfIn;
-  TimedPose3D m_htlf;
-  InPort<TimedPose3D> m_htlfIn;
-  TimedPose3D m_htrh;
-  InPort<TimedPose3D> m_htrhIn;
-  TimedPose3D m_htlh;
-  InPort<TimedPose3D> m_htlhIn;
-  TimedPose3D m_hthead;
-  InPort<TimedPose3D> m_htheadIn;
-  TimedPoint3D m_htzmp;
-  InPort<TimedPoint3D> m_htzmpIn;
-//  TimedPoint3D m_actzmp;
-//  InPort<TimedPoint3D> m_actzmpIn;
-  TimedDoubleSeq m_htrfw;
-  InPort<TimedDoubleSeq> m_htrfwIn;
-  TimedDoubleSeq m_htlfw;
-  InPort<TimedDoubleSeq> m_htlfwIn;
+class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
+  public:
+    WholeBodyMasterSlave(RTC::Manager* manager);
+    virtual ~WholeBodyMasterSlave();
+    virtual RTC::ReturnCode_t onInitialize();
+    virtual RTC::ReturnCode_t onFinalize();
+    virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
+    virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
+    virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
+    bool startCountDownForWholeBodyMasterSlave(const double sec);
+    bool stopHumanSync();
+    bool setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
+    bool getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
+
+  protected:
+    TimedDoubleSeq m_qRef;
+    InPort<TimedDoubleSeq> m_qRefIn;
+    TimedPoint3D m_basePos;
+    InPort<TimedPoint3D> m_basePosIn;
+    TimedOrientation3D m_baseRpy;
+    InPort<TimedOrientation3D> m_baseRpyIn;
+    TimedPoint3D m_zmp;
+    InPort<TimedPoint3D> m_zmpIn;
+    TimedDoubleSeq m_optionalData;
+    InPort<TimedDoubleSeq> m_optionalDataIn;
+
+    TimedPose3D m_htcom;
+    InPort<TimedPose3D> m_htcomIn;
+    TimedPose3D m_htrf;
+    InPort<TimedPose3D> m_htrfIn;
+    TimedPose3D m_htlf;
+    InPort<TimedPose3D> m_htlfIn;
+    TimedPose3D m_htrh;
+    InPort<TimedPose3D> m_htrhIn;
+    TimedPose3D m_htlh;
+    InPort<TimedPose3D> m_htlhIn;
+    TimedPose3D m_hthead;
+    InPort<TimedPose3D> m_htheadIn;
+    TimedPoint3D m_htzmp;
+    InPort<TimedPoint3D> m_htzmpIn;
+    //  TimedPoint3D m_actzmp;
+    //  InPort<TimedPoint3D> m_actzmpIn;
+    TimedDoubleSeq m_htrfw;
+    InPort<TimedDoubleSeq> m_htrfwIn;
+    TimedDoubleSeq m_htlfw;
+    InPort<TimedDoubleSeq> m_htlfwIn;
 
 #ifdef USE_DEBUG_PORT
-   TimedPose3D m_htcom_dbg;
-   OutPort<TimedPose3D> m_htcom_dbgOut;
-   TimedPose3D m_htrf_dbg;
-   OutPort<TimedPose3D> m_htrf_dbgOut;
-   TimedPose3D m_htlf_dbg;
-   OutPort<TimedPose3D> m_htlf_dbgOut;
-   TimedPose3D m_htrh_dbg;
-   OutPort<TimedPose3D> m_htrh_dbgOut;
-   TimedPose3D m_htlh_dbg;
-   OutPort<TimedPose3D> m_htlh_dbgOut;
-   TimedPose3D m_hthead_dbg;
-   OutPort<TimedPose3D> m_hthead_dbgOut;
-   TimedPoint3D m_htzmp_dbg;
-   OutPort<TimedPoint3D> m_htzmp_dbgOut;
-   TimedDoubleSeq m_htrfw_dbg;
-   OutPort<TimedDoubleSeq> m_htrfw_dbgOut;
-   TimedDoubleSeq m_htlfw_dbg;
-   OutPort<TimedDoubleSeq> m_htlfw_dbgOut;
-   TimedPose3D m_rpcom_dbg;
-   OutPort<TimedPose3D> m_rpcom_dbgOut;
-   TimedPose3D m_rprf_dbg;
-   OutPort<TimedPose3D> m_rprf_dbgOut;
-   TimedPose3D m_rplf_dbg;
-   OutPort<TimedPose3D> m_rplf_dbgOut;
-   TimedPose3D m_rprh_dbg;
-   OutPort<TimedPose3D> m_rprh_dbgOut;
-   TimedPose3D m_rplh_dbg;
-   OutPort<TimedPose3D> m_rplh_dbgOut;
-   TimedPose3D m_rphead_dbg;
-   OutPort<TimedPose3D> m_rphead_dbgOut;
-   TimedPoint3D m_rpzmp_dbg;
-   OutPort<TimedPoint3D> m_rpzmp_dbgOut;
-   TimedPoint3D m_rpdcp_dbg;
-   OutPort<TimedPoint3D> m_rpdcp_dbgOut;
-   TimedPoint3D m_rpacp_dbg;
-   OutPort<TimedPoint3D> m_rpacp_dbgOut;
-   TimedDoubleSeq m_invdyn_dbg;
-   OutPort<TimedDoubleSeq> m_invdyn_dbgOut;
+    TimedPose3D m_htcom_dbg;
+    OutPort<TimedPose3D> m_htcom_dbgOut;
+    TimedPose3D m_htrf_dbg;
+    OutPort<TimedPose3D> m_htrf_dbgOut;
+    TimedPose3D m_htlf_dbg;
+    OutPort<TimedPose3D> m_htlf_dbgOut;
+    TimedPose3D m_htrh_dbg;
+    OutPort<TimedPose3D> m_htrh_dbgOut;
+    TimedPose3D m_htlh_dbg;
+    OutPort<TimedPose3D> m_htlh_dbgOut;
+    TimedPose3D m_hthead_dbg;
+    OutPort<TimedPose3D> m_hthead_dbgOut;
+    TimedPoint3D m_htzmp_dbg;
+    OutPort<TimedPoint3D> m_htzmp_dbgOut;
+    TimedDoubleSeq m_htrfw_dbg;
+    OutPort<TimedDoubleSeq> m_htrfw_dbgOut;
+    TimedDoubleSeq m_htlfw_dbg;
+    OutPort<TimedDoubleSeq> m_htlfw_dbgOut;
+    TimedPose3D m_rpcom_dbg;
+    OutPort<TimedPose3D> m_rpcom_dbgOut;
+    TimedPose3D m_rprf_dbg;
+    OutPort<TimedPose3D> m_rprf_dbgOut;
+    TimedPose3D m_rplf_dbg;
+    OutPort<TimedPose3D> m_rplf_dbgOut;
+    TimedPose3D m_rprh_dbg;
+    OutPort<TimedPose3D> m_rprh_dbgOut;
+    TimedPose3D m_rplh_dbg;
+    OutPort<TimedPose3D> m_rplh_dbgOut;
+    TimedPose3D m_rphead_dbg;
+    OutPort<TimedPose3D> m_rphead_dbgOut;
+    TimedPoint3D m_rpzmp_dbg;
+    OutPort<TimedPoint3D> m_rpzmp_dbgOut;
+    TimedPoint3D m_rpdcp_dbg;
+    OutPort<TimedPoint3D> m_rpdcp_dbgOut;
+    TimedPoint3D m_rpacp_dbg;
+    OutPort<TimedPoint3D> m_rpacp_dbgOut;
+    TimedDoubleSeq m_invdyn_dbg;
+    OutPort<TimedDoubleSeq> m_invdyn_dbgOut;
 #endif
 
-  OutPort<TimedDoubleSeq> m_qOut;
-  RTC::OutPort<RTC::TimedPoint3D> m_zmpOut;
-  OutPort<TimedPoint3D> m_basePosOut;
-  OutPort<TimedOrientation3D> m_baseRpyOut;
-  OutPort<TimedDoubleSeq> m_optionalDataOut;
-  RTC::CorbaPort m_WholeBodyMasterSlaveServicePort;
+    OutPort<TimedDoubleSeq> m_qOut;
+    RTC::OutPort<RTC::TimedPoint3D> m_zmpOut;
+    OutPort<TimedPoint3D> m_basePosOut;
+    OutPort<TimedOrientation3D> m_baseRpyOut;
+    OutPort<TimedDoubleSeq> m_optionalDataOut;
+    RTC::CorbaPort m_WholeBodyMasterSlaveServicePort;
 
-  WholeBodyMasterSlaveService_impl m_service0;
+    WholeBodyMasterSlaveService_impl m_service0;
 
  private:
   struct ABCIKparam {
@@ -172,7 +168,7 @@ class WholeBodyMasterSlave
 
 
   void setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in);
-  void solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HRPPose3D& com_ref, const HRPPose3D& rf_ref, const HRPPose3D& lf_ref, const HRPPose3D& rh_ref, const HRPPose3D& lh_ref, const HRPPose3D& head_ref, const std::string& debug_prefix="");
+  void solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix="");
   void processTransition();
   void preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in);
   void processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in);
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.cpp b/rtc/WholeBodyMasterSlave/wbms_core.cpp
index cb9fd44fc9c..fbf1b1313ee 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.cpp
+++ b/rtc/WholeBodyMasterSlave/wbms_core.cpp
@@ -1,120 +1,120 @@
-#include "wbms_core.h"
-
-void HumanSynchronizer::calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const Wrench6& rfwin, const Wrench6& lfwin, hrp::Vector3& zmp_ans){
-  hrp::Vector3 rfzmp,lfzmp;
-  const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
-  if( rfwin.w(fz) > 1.0e-6 ){
-    rfzmp(0) = ( - rfwin.w(ty) - rfwin.w(fx) * F_H_OFFSET + rfwin.w(fz) * 0 ) / rfwin.w(fz) + rfpos(0);
-    rfzmp(1) = (   rfwin.w(tx) - rfwin.w(fy) * F_H_OFFSET + rfwin.w(fz) * 0 ) / rfwin.w(fz) + rfpos(1);
-  }
-  if( lfwin.w(fz) > 1.0e-6 ){
-    lfzmp(0) = ( - lfwin.w(ty) - lfwin.w(fx) * F_H_OFFSET + lfwin.w(fz) * 0 ) / lfwin.w(fz) + lfpos(0);
-    lfzmp(1) = (   lfwin.w(tx) - lfwin.w(fy) * F_H_OFFSET + lfwin.w(fz) * 0 ) / lfwin.w(fz) + lfpos(1);
-  }
-  if( rfwin.w(fz) > 1.0e-6 || lfwin.w(fz) > 1.0e-6 ){
-    zmp_ans(0) = ( rfzmp(0)*rfwin.w(fz) + lfzmp(0)*lfwin.w(fz) ) / ( rfwin.w(fz) + lfwin.w(fz));
-    zmp_ans(1) = ( rfzmp(1)*rfwin.w(fz) + lfzmp(1)*lfwin.w(fz) ) / ( rfwin.w(fz) + lfwin.w(fz));
-  }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
-  zmp_ans(2) = 0;
-}
-void HumanSynchronizer::calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans){
-  hrp::Vector4 margin_abs;
-  hrp::Vector2 cross_pt, anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
-  for(int direc=0;direc<4;direc++){
-    calcCrossPointOnHull(check_point, check_point+anchor_vec[direc], hull, cross_pt);
-    margin_abs(direc) = (cross_pt - check_point).norm();
-  }
-  margin_ans(0) =  margin_abs(0);
-  margin_ans(1) = -margin_abs(1);
-  margin_ans(2) =  margin_abs(2);
-  margin_ans(3) = -margin_abs(3);
-}
-bool HumanSynchronizer::calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_will_cross){
-  hrp::Vector2 anchor_vec = pt_out_goal - pt_in_start;
-  for(int i=0;i<hull.size();i++){
-    int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
-    hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
-    hrp::Vector2 cur_edge = nxt_pt - cur_pt;
-    double dBunbo = hrpVector2Cross(anchor_vec,cur_edge);
-    if( dBunbo != 0.0) {//平行の場合を外す
-      hrp::Vector2 vectorAC = hrp::Vector2(hull[i](0),hull[i](1)) - pt_in_start;
-      double dR = hrpVector2Cross(vectorAC,cur_edge) / dBunbo;
-      double dS = hrpVector2Cross(vectorAC,anchor_vec) / dBunbo;
-      if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){//dRには数値誤差が乗る
-        pt_will_cross = pt_in_start + dR * anchor_vec;
-        //dist = dR * anchor_vec.norm();
-        return true;
-      }
-    }
-  }
-  return false;
-}
-double HumanSynchronizer::calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_ans){
-  double cur_nearest_dist, ans_nearest_dist;
-  hrp::Vector2 cur_nearest_pt, ans_nearest_pt;
-  for(int i=0;i<hull.size();i++){
-    int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
-    hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
-    hrp::Vector2 cur_edge = nxt_pt - cur_pt;
-    hrp::Vector2 tgt_pt_v = tgt_pt - cur_pt;
-    double tgt_pt_projected_length = tgt_pt_v.dot(cur_edge.normalized());
-    if(tgt_pt_projected_length > cur_edge.norm() ){//tgt_pt's nearest point is on the　i+1-th vertex
-      cur_nearest_pt = nxt_pt;
-    }else if(tgt_pt_projected_length < 0 ){//tgt_pt's nearest point is on the　i-th vertex
-      cur_nearest_pt = cur_pt;
-    }else{//tgt_pt's nearest point is on the line
-      cur_nearest_pt = cur_pt + tgt_pt_projected_length * cur_edge.normalized();
-    }
-    cur_nearest_dist = (tgt_pt - cur_nearest_pt).norm();
-    if(i==0){//set first candidate as nearest
-      ans_nearest_dist = cur_nearest_dist;
-      ans_nearest_pt = cur_nearest_pt;
-    }else if( cur_nearest_dist < ans_nearest_dist ){//update nearest candidate
-      ans_nearest_dist = cur_nearest_dist;
-      ans_nearest_pt = cur_nearest_pt;
-    }
-  }
-  pt_ans = ans_nearest_pt;
-  return ans_nearest_dist;
-}
-void HumanSynchronizer::makeConvexHullOpenCV(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
-  points.clear();
-  cvhull.clear();
-  hull_ans.clear();
-  for(int i=0;i<pts.size();i++){ points.push_back( cv::Point2f( pts[i](0), pts[i](1)) ); }
-  cv::convexHull(cv::Mat(points),cvhull,true);
-  for(int i=0;i<cvhull.size();i++){ hull_ans.push_back( hrp::Vector2( cvhull[i].x, cvhull[i].y ) );  }
-}
-//void HumanSynchronizer::makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
-//  const int dim = 2;
-//  double points[dim*pts.size()];
-//  for(int i=0;i<pts.size();i++){ points[dim*i] = pts[i](0); points[dim*i+1] = pts[i](1); }
-//  qh_new_qhull( dim, pts.size(), points, false, "qhull ", NULL, stderr);
-//  pointT *point, *pointtemp;
-//  hull_ans.clear();
-//  FORALLpoints hull_ans.push_back( hrp::Vector2(point[0],point[1]) );
-//  qh_freeqhull(!qh_ALL);
-//  //#define qh_ORIENTclock 1
+//#include "wbms_core.h"
+//
+//void HumanSynchronizer::calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const hrp::dvector6& rfwin, const hrp::dvector6& lfwin, hrp::Vector3& zmp_ans){
+//  hrp::Vector3 rfzmp,lfzmp;
+//  const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
+//  if( rfwin(fz) > 1.0e-6 ){
+//    rfzmp(0) = ( - rfwin(ty) - rfwin(fx) * F_H_OFFSET + rfwin(fz) * 0 ) / rfwin(fz) + rfpos(0);
+//    rfzmp(1) = (   rfwin(tx) - rfwin(fy) * F_H_OFFSET + rfwin(fz) * 0 ) / rfwin(fz) + rfpos(1);
+//  }
+//  if( lfwin(fz) > 1.0e-6 ){
+//    lfzmp(0) = ( - lfwin(ty) - lfwin(fx) * F_H_OFFSET + lfwin(fz) * 0 ) / lfwin(fz) + lfpos(0);
+//    lfzmp(1) = (   lfwin(tx) - lfwin(fy) * F_H_OFFSET + lfwin(fz) * 0 ) / lfwin(fz) + lfpos(1);
+//  }
+//  if( rfwin(fz) > 1.0e-6 || lfwin(fz) > 1.0e-6 ){
+//    zmp_ans(0) = ( rfzmp(0)*rfwin(fz) + lfzmp(0)*lfwin(fz) ) / ( rfwin(fz) + lfwin(fz));
+//    zmp_ans(1) = ( rfzmp(1)*rfwin(fz) + lfzmp(1)*lfwin(fz) ) / ( rfwin(fz) + lfwin(fz));
+//  }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
+//  zmp_ans(2) = 0;
 //}
-bool HumanSynchronizer::isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull){
-  cvhull.clear();
-  for(int i=0;i<hull.size();i++){ cvhull.push_back( cv::Point2f( hull[i](0), hull[i](1)) ); }
-  return (cv::pointPolygonTest(cv::Mat(cvhull), cv::Point2f(pt(0),pt(1)), false) > 0);
-}
-void HumanSynchronizer::applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
-  comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
-  comacc = acc4zmp_v_filters.passFilter(comacc);
-  zmpout(0) = comin(0)-(H_cur/G)*comacc(0);
-  zmpout(1) = comin(1)-(H_cur/G)*comacc(1);
-  com_oldold = com_old;
-  com_old = comin;
-}
-//void HumanSynchronizer::applyVelLimit(const HumanPose& in, HumanPose& out_old, HumanPose& out){
-//  const int ns[] = {com,rf,lf,rh,lh,head}, num_ns = sizeof(ns)/sizeof(int);
-//  for(int i=0;i<num_ns;i++){
-//    hrp::Vector3 diff = in.P[i].p  - out_old.P[i].p;
-//    for(int j=0;j<3;j++)LIMIT_MINMAX( diff(j), -MAXVEL*DT, MAXVEL*DT);
-//    out.P[i].p = out_old.P[i].p + diff;
+//void HumanSynchronizer::calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans){
+//  hrp::Vector4 margin_abs;
+//  hrp::Vector2 cross_pt, anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
+//  for(int direc=0;direc<4;direc++){
+//    calcCrossPointOnHull(check_point, check_point+anchor_vec[direc], hull, cross_pt);
+//    margin_abs(direc) = (cross_pt - check_point).norm();
 //  }
-//  out_old = out;
+//  margin_ans(0) =  margin_abs(0);
+//  margin_ans(1) = -margin_abs(1);
+//  margin_ans(2) =  margin_abs(2);
+//  margin_ans(3) = -margin_abs(3);
+//}
+//bool HumanSynchronizer::calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_will_cross){
+//  hrp::Vector2 anchor_vec = pt_out_goal - pt_in_start;
+//  for(int i=0;i<hull.size();i++){
+//    int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
+//    hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
+//    hrp::Vector2 cur_edge = nxt_pt - cur_pt;
+//    double dBunbo = hrpVector2Cross(anchor_vec,cur_edge);
+//    if( dBunbo != 0.0) {//平行の場合を外す
+//      hrp::Vector2 vectorAC = hrp::Vector2(hull[i](0),hull[i](1)) - pt_in_start;
+//      double dR = hrpVector2Cross(vectorAC,cur_edge) / dBunbo;
+//      double dS = hrpVector2Cross(vectorAC,anchor_vec) / dBunbo;
+//      if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){//dRには数値誤差が乗る
+//        pt_will_cross = pt_in_start + dR * anchor_vec;
+//        //dist = dR * anchor_vec.norm();
+//        return true;
+//      }
+//    }
+//  }
+//  return false;
+//}
+//double HumanSynchronizer::calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_ans){
+//  double cur_nearest_dist, ans_nearest_dist;
+//  hrp::Vector2 cur_nearest_pt, ans_nearest_pt;
+//  for(int i=0;i<hull.size();i++){
+//    int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
+//    hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
+//    hrp::Vector2 cur_edge = nxt_pt - cur_pt;
+//    hrp::Vector2 tgt_pt_v = tgt_pt - cur_pt;
+//    double tgt_pt_projected_length = tgt_pt_v.dot(cur_edge.normalized());
+//    if(tgt_pt_projected_length > cur_edge.norm() ){//tgt_pt's nearest point is on the　i+1-th vertex
+//      cur_nearest_pt = nxt_pt;
+//    }else if(tgt_pt_projected_length < 0 ){//tgt_pt's nearest point is on the　i-th vertex
+//      cur_nearest_pt = cur_pt;
+//    }else{//tgt_pt's nearest point is on the line
+//      cur_nearest_pt = cur_pt + tgt_pt_projected_length * cur_edge.normalized();
+//    }
+//    cur_nearest_dist = (tgt_pt - cur_nearest_pt).norm();
+//    if(i==0){//set first candidate as nearest
+//      ans_nearest_dist = cur_nearest_dist;
+//      ans_nearest_pt = cur_nearest_pt;
+//    }else if( cur_nearest_dist < ans_nearest_dist ){//update nearest candidate
+//      ans_nearest_dist = cur_nearest_dist;
+//      ans_nearest_pt = cur_nearest_pt;
+//    }
+//  }
+//  pt_ans = ans_nearest_pt;
+//  return ans_nearest_dist;
+//}
+//void HumanSynchronizer::makeConvexHullOpenCV(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
+//  points.clear();
+//  cvhull.clear();
+//  hull_ans.clear();
+//  for(int i=0;i<pts.size();i++){ points.push_back( cv::Point2f( pts[i](0), pts[i](1)) ); }
+//  cv::convexHull(cv::Mat(points),cvhull,true);
+//  for(int i=0;i<cvhull.size();i++){ hull_ans.push_back( hrp::Vector2( cvhull[i].x, cvhull[i].y ) );  }
+//}
+////void HumanSynchronizer::makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
+////  const int dim = 2;
+////  double points[dim*pts.size()];
+////  for(int i=0;i<pts.size();i++){ points[dim*i] = pts[i](0); points[dim*i+1] = pts[i](1); }
+////  qh_new_qhull( dim, pts.size(), points, false, "qhull ", NULL, stderr);
+////  pointT *point, *pointtemp;
+////  hull_ans.clear();
+////  FORALLpoints hull_ans.push_back( hrp::Vector2(point[0],point[1]) );
+////  qh_freeqhull(!qh_ALL);
+////  //#define qh_ORIENTclock 1
+////}
+//bool HumanSynchronizer::isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull){
+//  cvhull.clear();
+//  for(int i=0;i<hull.size();i++){ cvhull.push_back( cv::Point2f( hull[i](0), hull[i](1)) ); }
+//  return (cv::pointPolygonTest(cv::Mat(cvhull), cv::Point2f(pt(0),pt(1)), false) > 0);
+//}
+//void HumanSynchronizer::applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
+//  comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
+//  comacc = acc4zmp_v_filters.passFilter(comacc);
+//  zmpout(0) = comin(0)-(H_cur/G)*comacc(0);
+//  zmpout(1) = comin(1)-(H_cur/G)*comacc(1);
+//  com_oldold = com_old;
+//  com_old = comin;
 //}
+////void HumanSynchronizer::applyVelLimit(const HumanPose& in, HumanPose& out_old, HumanPose& out){
+////  const int ns[] = {com,rf,lf,rh,lh,head}, num_ns = sizeof(ns)/sizeof(int);
+////  for(int i=0;i<num_ns;i++){
+////    hrp::Vector3 diff = in.P[i].p  - out_old.P[i].p;
+////    for(int j=0;j<3;j++)LIMIT_MINMAX( diff(j), -MAXVEL*DT, MAXVEL*DT);
+////    out.P[i].p = out_old.P[i].p + diff;
+////  }
+////  out_old = out;
+////}
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 7c3e627245b..04952aaf044 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -1,6 +1,5 @@
 #ifndef HUMANMASTERSLAVE_H
 #define HUMANMASTERSLAVE_H
-
 #include <rtm/idl/BasicDataType.hh>
 #include <rtm/idl/ExtendedDataTypes.hh>
 #include <rtm/Manager.h>
@@ -13,13 +12,10 @@
 #include "interpolator.h"
 #include "../TorqueFilter/IIRFilter.h"
 #include <hrpModel/Body.h>
-
 #include <opencv2/core/core.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/imgproc/types_c.h>
 #include <hrpUtil/Eigen4d.h>
-
-
 #include <numeric>
 
 #if defined(__cplusplus)
@@ -39,98 +35,79 @@ extern "C" {
 }
 #endif
 
-#define LIMIT_MIN(x,min) (x= ( x<min ? min:x ))
-#define LIMIT_MAX(x,max) (x= ( x<max ? x:max ))
-#define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
-
-//実機だとログ出力で落ちる？getenv("HOME")ダメ？
 #define DEBUG 0
 
-namespace myconst{
-//  const double INFMIN = std::numeric_limits<double>::min(), INFMAX = std::numeric_limits<double>::max();
-  const double INFMIN = - std::numeric_limits<double>::max(), INFMAX = std::numeric_limits<double>::max();
-  const double D2R = M_PI/180.0;
-}
-using namespace myconst;
-
-#define DEF_UTIL_CONST \
-    static const enum pose_tgt{ com, rf, lf, rh, lh, head, zmp, num_pose_tgt } p_id;\
-    static const enum wrench_tgt{ rfw, lfw, num_wrench_tgt } w_id;\
-    static const enum lr_direction{ L, R, LR} lr_direc;\
-    static const enum xyz_direction{ X, Y, Z, XYZ} xyz_direc;\
-    static const enum rpy_direction{ r, p, y, rpy} rpy_direc;\
-    static const enum ft_direction{ fx, fy, fz, tx, ty, tz, ft_xyz} w_direc;\
-    static const enum minmax_id{ MIN, MAX, MINMAX} mm_id;\
+class UTIL_CONST{
+  public:
+    enum { com, rf, lf, rh, lh, head, zmp, num_pose_tgt };
+    enum { num_ee_tgt=4 };
+    enum { R, L, LR };
+    enum { X, Y, Z, XYZ };
+    enum { r, p, y, rpy };
+    enum { fx, fy, fz, tx, ty, tz, ft_xyz };
+    enum { MIN, MAX, MINMAX };
+    double G, D2R, INFMIN,INFMAX;
+    UTIL_CONST() :
+      G(9.80665),
+      D2R( M_PI/180.0),
+      INFMIN( - std::numeric_limits<double>::max()),
+      INFMAX( + std::numeric_limits<double>::max())
+    {};
+    static inline bool LIMIT_MIN(double& x, const double min){ if(x<min){x=min; return true;}else{return false;} }
+    static inline bool LIMIT_MAX(double& x, const double max){ if(x>max){x=max; return true;}else{return false;} }
+    static inline bool LIMIT_MINMAX(double& x, const double min, const double max){ assert( min <=  max); if(x<min){x=min; return true;} else if(x>max){x=max; return true;} else{return false;} }
+};
 
-class BiquadIIRFilterVec{
+class BiquadIIRFilterVec : UTIL_CONST {
   private:
-    IIRFilter filters[3];
+    IIRFilter filters[XYZ];
     hrp::Vector3 ans;
   public:
     static const double Q_BUTTERWORTH = 0.707106781, Q_NOOVERSHOOT = 0.5;
-    BiquadIIRFilterVec(){
-    };
-    ~BiquadIIRFilterVec() {};
-    void setParameter(const hrp::Vector3& fc_in, const double& HZ, const double& Q = Q_BUTTERWORTH){
-     for(int i=0;i<3;i++){
-         filters[i].setParameterAsBiquad((double)fc_in(i), Q, HZ);
-      }
-    };
-    void setParameter(const double& fc_in, const double& HZ, const double& Q = Q_BUTTERWORTH){//overload
-      setParameter(hrp::Vector3(fc_in,fc_in,fc_in), HZ, Q);
-    };
-    hrp::Vector3 passFilter(const hrp::Vector3& input){
-      for(int i=0;i<3;i++){ ans(i) = filters[i].passFilter((double)input(i)); }
-      return ans;
-    };
-    void init(const hrp::Vector3& initial_input){
-      for(int i=0;i<3;i++){ filters[i].reset((double)initial_input(i));}
-    }
+    BiquadIIRFilterVec(){};
+    ~BiquadIIRFilterVec(){};
+    inline void setParameter(const hrp::Vector3& fc_in, const double& HZ, const double& Q = Q_BUTTERWORTH){ for(int i=0;i<XYZ;i++){ filters[i].setParameterAsBiquad((double)fc_in(i), Q, HZ); } };
+    inline void setParameter(const double& fc_in, const double& HZ, const double& Q = Q_BUTTERWORTH){ setParameter(hrp::Vector3(fc_in,fc_in,fc_in), HZ, Q); };//overload
+    inline hrp::Vector3 passFilter(const hrp::Vector3& input){ for(int i=0;i<XYZ;i++){ ans(i) = filters[i].passFilter((double)input(i)); } return ans; };
+    inline void init(const hrp::Vector3& initial_input){ for(int i=0;i<XYZ;i++){ filters[i].reset((double)initial_input(i));} }
 };
 
-class HRPPose3D{
+class WBMSPose3D{
   public:
-    hrp::Vector3 p,p_offs,rpy,rpy_offs;
-    HRPPose3D(){ clear(); }
-    ~HRPPose3D(){}
-    void clear(){
-      p = p_offs = rpy = rpy_offs = hrp::Vector3::Zero();
-    }
+    hrp::Vector3 p,rpy;
+    WBMSPose3D(){ clear(); }
+    ~WBMSPose3D(){}
+    inline void clear(){ p = rpy = hrp::Vector3::Zero(); }
 };
-class Wrench6{
+
+class PoseTGT{
   public:
+    WBMSPose3D abs, offs, cnt;
     hrp::dvector6 w;
-    Wrench6(){ clear(); }
-    ~Wrench6(){}
-    void clear(){ w = hrp::dvector6::Zero(); }
+    bool is_contact, go_contact;
+    PoseTGT(){ clear(); }
+    ~PoseTGT(){}
+    inline void clear(){ abs.clear(); offs.clear(); cnt.clear(); w=hrp::dvector6::Zero(); is_contact = go_contact = false;}
 };
 
-class HumanPose{
-    DEF_UTIL_CONST
+class HumanPose : UTIL_CONST {
   public:
-    std::vector<HRPPose3D> P;
-    std::vector<Wrench6> w;
-    HumanPose(){
-      P.resize(num_pose_tgt);
-      w.resize(num_wrench_tgt);
-      clear();
-    }
+    std::vector<PoseTGT> tgt;
+    HumanPose(){ tgt.resize(num_pose_tgt); }
     ~HumanPose(){cerr<<"HumanPose destructed"<<endl;}
-    void clear(){
-      for(std::vector<HRPPose3D>::iterator it = P.begin(); it != P.end(); it++){ it->clear(); }
-      for(std::vector<Wrench6>::iterator it = w.begin(); it != w.end(); it++){ it->clear(); }
-    }
-    static void hp_printf(const HumanPose& in){
-      const std::string pcap[] = {"c","rf","lf","rh","lh","hd","z"}, wcap[] = {"rw","lw"};
-      for(int i=0;i<num_pose_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",pcap[i].c_str(),in.P[i].p(X),in.P[i].p(Y),in.P[i].p(Z)); }
-//      for(int i=0;i<num_wrench_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",wcap[i].c_str(),in.w[i].w(fx),in.w[i].w(fy),in.w[i].w(fz),in.w[i].w(tx),in.w[i].w(ty),in.w[i].w(tz)); }
+    inline void clear(){
+      for(std::vector<PoseTGT>::iterator it = tgt.begin(); it != tgt.end(); it++){ it->clear(); }
+    }
+    static inline void hp_printf(const HumanPose& in){
+      const std::string pcatgt[] = {"c","rf","lf","rh","lh","hd","z"}, wcatgt[] = {"rw","lw"};
+      for(int i=0;i<num_pose_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",pcatgt[i].c_str(),in.tgt[i].abs.p(X),in.tgt[i].abs.p(Y),in.tgt[i].abs.p(Z)); }
+//      for(int i=0;i<num_wrench_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",wcatgt[i].c_str(),in.w[i](fx),in.w[i](fy),in.w[i](fz),in.w[i](tx),in.w[i](ty),in.w[i](tz)); }
       printf("\n");
     }
-    void print(){ hp_printf(*this); }
+    inline void print(){ hp_printf(*this); }
 };
 
-class RobotConfig{
-    DEF_UTIL_CONST
+class RobotConfig : UTIL_CONST {
   public:
     std::vector< std::vector<hrp::Vector3> > ee_rot_limit;
     RobotConfig(){
@@ -152,61 +129,32 @@ class RobotConfig{
     }
 };
 
-class HumanSynchronizer{
-    DEF_UTIL_CONST
+class HumanSynchronizer : UTIL_CONST {
   private:
-    double FUP_TIME;
-    int cur_rfup_level,cur_lfup_level;
-    double cur_rfup_rad,cur_lfup_rad;
-    double FUP_HIGHT;
-    double CNT_F_TH;
-    hrp::Vector3 init_hp_calibcom;
-    double MAXVEL,MAXACC;
-    HumanPose rp_ref_out_old, hp_swap_checked, rp_old_ref_Vel;
     bool HumanSyncOn;
+    double CNT_F_TH, h2r_ratio, tgt_h2r_ratio, HZ, DT;
     struct timeval t_calc_start, t_calc_end;
-    double h2r_ratio, tgt_h2r_ratio;
-    double HZ,DT,G;
-    hrp::Vector3 com_old,com_oldold,comacc;
-    hrp::Vector3 comacc_ref;
-    hrp::Vector3 com_CP_ref_old;
-    std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rot_filters;
-    BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters;
-    BiquadIIRFilterVec com_in_filter;
-    hrp::Vector3 r_zmp_raw;
+    BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters, com_in_filter;
+    hrp::Vector3 com_old, com_oldold, comacc, com_CP_ref_old, r_zmp_raw;
     hrp::Vector4 rf_safe_region,lf_safe_region;//前後左右の順
-    RobotConfig rc;
-    std::vector<hrp::Vector2> rflf_points;
-    std::vector<hrp::Vector2> hull_com,hull_dcp,hull_acp;
+    std::vector<hrp::Vector2> rflf_points, hull_com, hull_dcp, hull_acp;
+    std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rot_filters;
     std::vector<cv::Point2f> points,cvhull;
-
+    RobotConfig rc;
+    HumanPose rp_ref_out_old, hp_swap_checked;
   public:
-    double tgt_FUP_TIME;
-    bool startCountdownForHumanSync;
-    bool ht_first_call;
-    bool is_rf_contact,is_lf_contact;//足上げ高さ0に到達しているか否か
-    bool go_rf_landing,go_lf_landing;//足上げ指令の有無
-//    HumanPose rp_wld_initpos;//IKでのリンク原点の位置(足首，手首など)
-    bool use_x,use_y,use_z;
-    HumanPose hp_wld_raw;
-    HumanPose hp_plot;
-    HumanPose rp_ref_out;
-    FILE *sr_log,*cz_log;
-    FILE *id_log;
-    hrp::Vector3 init_wld_rp_rfeepos,init_wld_rp_lfeepos;
-    hrp::Vector3 init_wld_hp_rfpos,init_wld_hp_lfpos;
-    HRPPose3D baselinkpose;
-    hrp::Vector3 com_vel_old;
+    bool startCountdownForHumanSync, ht_first_call;//, is_ee_contact[FH][LR], go_ee_contact[FH][LR];
     unsigned int loop;
-    hrp::Vector3 pre_cont_rfpos,pre_cont_lfpos, pre_cont_rfrot,pre_cont_lfrot;
-    std::vector<hrp::Vector3> rf_vert,lf_vert;
     double countdown_sec;
-    hrp::Vector3 cp_dec,cp_acc;
-    Wrench6 invdyn_ft;
+    double H_cur;
+    HumanPose hp_wld_raw, hp_plot, rp_ref_out;
+    FILE *sr_log, *cz_log, *id_log;
+    WBMSPose3D baselinkpose;
+    WBMSPose3D ee_contact_pose[4];
+    hrp::Vector3 com_vel_old, cp_dec, cp_acc;
+    std::vector<hrp::Vector3> rf_vert,lf_vert;
     hrp::Vector2 com_forcp_ref,com_vel_forcp_ref;
-
-    interpolator *init_zmp_com_offset_interpolator;
-    double zmp_com_offset_ip_ratio;
+    hrp::dvector6 invdyn_ft;
     struct WBMSparameters {
       double auto_swing_foot_landing_threshold;
       double foot_vertical_vel_limit_coeff;
@@ -223,63 +171,33 @@ class HumanSynchronizer{
     };
     struct WBMSparameters WBMSparam;
 
-    double H_cur;
-
     HumanSynchronizer(const double& dt){
       tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
-//      tgt_h2r_ratio = h2r_ratio = 0.62;//human 1.1m vs chidori 0.69m
       DT = dt;
       HZ = (int)(1.0/DT);
-      G = 9.80665;
-      use_x = use_y = use_z = true;
-      cur_rfup_level = cur_lfup_level = 0;
-      cur_rfup_rad = cur_lfup_rad = 0;
-      is_rf_contact = is_lf_contact = true;
-      go_rf_landing = go_lf_landing = false;
-      pre_cont_rfpos = pre_cont_lfpos = pre_cont_rfrot = pre_cont_lfrot = hrp::Vector3::Zero();
-      init_hp_calibcom = hrp::Vector3::Zero();
-      tgt_FUP_TIME = FUP_TIME = 0.4;
-      FUP_HIGHT = 0.05;
       CNT_F_TH = 20.0;
-      MAXVEL = 0.4*10;
       HumanSyncOn = false;
       ht_first_call = true;
       startCountdownForHumanSync = false;
       countdown_sec = 5.0;
       loop = 0;
-
       //////////  hrp::Vector は初期化必要  ///////////
       com_old = com_oldold = comacc = hrp::Vector3::Zero();
-      comacc_ref = hrp::Vector3::Zero();
       r_zmp_raw = hrp::Vector3::Zero();
-
-      init_wld_rp_rfeepos = init_wld_rp_lfeepos = hrp::Vector3::Zero();
-      init_wld_hp_rfpos = init_wld_hp_lfpos = hrp::Vector3::Zero();
-
       com_vel_old = hrp::Vector3::Zero();
       cp_dec = cp_acc = hrp::Vector3::Zero();
-
+      invdyn_ft = hrp::dvector6::Zero();
       tgt_pos_filters.resize(num_pose_tgt);
       tgt_rot_filters.resize(num_pose_tgt);
-//      for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(0.6,HZ);//四肢拘束点用(position)
-//      for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0,HZ);//四肢拘束点用(Rotation)
-//      tgt_pos_filters[0].setParameter(0.6,HZ);//重心pos用
-//      tgt_rot_filters[0].setParameter(0.4,HZ);//重心rot用
-//      tgt_pos_filters[1].setParameter(hrp::Vector3(0.6,0.6,1.0),HZ);//右足pos用
-//      tgt_pos_filters[2].setParameter(hrp::Vector3(0.6,0.6,1.0),HZ);//左足pos用
-//      calcacc_v_filters.setParameter(5,HZ);//加速度計算用
-//      acc4zmp_v_filters.setParameter(1,HZ);//ZMP生成用ほぼこの値でいい
-//      cam_rpy_filter.setParameter(1,HZ);//カメラアングル
       for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//四肢拘束点用(position)
       for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//四肢拘束点用(Rotation)
-      tgt_pos_filters[0].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//重心pos用
-      tgt_rot_filters[0].setParameter(0.6, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//重心rot用
-      tgt_pos_filters[1].setParameter(hrp::Vector3(1.0,1.0,1.2), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//右足pos用
-      tgt_pos_filters[2].setParameter(hrp::Vector3(1.0,1.0,1.2), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//左足pos用
+      tgt_pos_filters[com].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//重心pos用
+      tgt_rot_filters[com].setParameter(0.6, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//重心rot用
+      tgt_pos_filters[rf].setParameter(hrp::Vector3(1.0,1.0,1.2), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//右足pos用
+      tgt_pos_filters[lf].setParameter(hrp::Vector3(1.0,1.0,1.2), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//左足pos用
       calcacc_v_filters.setParameter(5, HZ, BiquadIIRFilterVec::Q_BUTTERWORTH);//加速度計算用
       acc4zmp_v_filters.setParameter(5, HZ, BiquadIIRFilterVec::Q_BUTTERWORTH);//ZMP生成用ほぼこの値でいい
       com_in_filter.setParameter(1, HZ);
-
       rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
       lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
       rf_vert.push_back(hrp::Vector3(0.13,-0.08,0));
@@ -303,25 +221,14 @@ class HumanSynchronizer{
       WBMSparam.use_rh = WBMSparam.use_lh = true;
       WBMSparam.use_head = true;
       WBMSparam.use_manipulability_limit = false;
-
       rp_ref_out_old.clear();
       rp_ref_out.clear();
-
       rflf_points.reserve(8);//あらかじめreserveして高速化
       points.reserve(8);
       hull_com.reserve(6);
       hull_dcp.reserve(6);
       hull_dcp.reserve(6);
       cvhull.reserve(6);
-
-
-      init_zmp_com_offset_interpolator = new interpolator(1, DT, interpolator::HOFFARBIB, 1);
-      init_zmp_com_offset_interpolator->setName("zmp_com_offset_interpolator");
-      double start_ratio = 0.0;
-      init_zmp_com_offset_interpolator->set(&start_ratio);
-      double goal_ratio = 1.0;
-      init_zmp_com_offset_interpolator->go(&goal_ratio, 3.0, true);//3s
-
       if(DEBUG){
         sr_log = fopen("/home/ishiguro/HumanSync_support_region.log","w+");
         cz_log = fopen("/home/ishiguro/HumanSync_com_zmp.log","w+");
@@ -333,21 +240,20 @@ class HumanSynchronizer{
       if(DEBUG)fclose(sr_log);
       if(DEBUG)fclose(cz_log);
       if(DEBUG)fclose(id_log);
-      delete init_zmp_com_offset_interpolator;
       cout<<"HumanSynchronizer destructed"<<endl;
     }
-    const double getRemainingCountDown() const { return countdown_sec; }
-    const bool isHumanSyncOn() const { return HumanSyncOn; }
-    const double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6; }
-    void setTargetHumanToRobotRatio(const double tgt_h2r_r_in){ tgt_h2r_ratio = tgt_h2r_r_in; }
-    void setCurrentInputAsOffset(HumanPose& tgt){
+    inline double getRemainingCountDown() const { return countdown_sec; }
+    inline bool isHumanSyncOn() const { return HumanSyncOn; }
+    inline double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6; }
+    inline void setTargetHumanToRobotRatio(const double tgt_h2r_r_in){ tgt_h2r_ratio = tgt_h2r_r_in; }
+    inline void setCurrentInputAsOffset(HumanPose& tgt){
       std::string ns[7] = {"com","rf","lf","rh","lh","zmp","head"};
       for(int i=0;i<7;i++){
-        tgt.P[i].p_offs   =  tgt.P[i].p;
-        tgt.P[i].rpy_offs =  tgt.P[i].rpy;
+        tgt.tgt[i].offs.p   =  tgt.tgt[i].abs.p;
+        tgt.tgt[i].offs.rpy =  tgt.tgt[i].abs.rpy;
       }
     }
-    void updateCountDown(){
+    inline void updateCountDown(){
       if(countdown_sec>0.0){
         countdown_sec -= DT;
       }else if(countdown_sec <= 0){
@@ -355,277 +261,234 @@ class HumanSynchronizer{
         startCountdownForHumanSync = false;
       }
     }
-    void update(){//////////  メインループ  ////////////
+    inline void update(){//////////  メインループ  ////////////
       gettimeofday(&t_calc_start, NULL);
       updateParams                        ();
-//      applyInitHumanZMPToCOMOffset        (hp_rel_raw, init_hp_calibcom, hp_rel_raw);//怪しいので要修正
-//      calcWorldZMP                        ((hp_rel_raw.P[rf].p+init_wld_hp_rfpos), (hp_rel_raw.P[lf].p+init_wld_hp_lfpos), hp_rel_raw.w[rfw], hp_rel_raw.w[lfw], hp_rel_raw.P[zmp].p);//足の位置はworldにしないと・・・
       autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);//入力の左右反転を常にチェック(＝手足の交差は不可能)
       convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
       hp_plot = rp_ref_out;
-      if(loop==0)rp_ref_out_old = rp_ref_out;
-
-
-      if(isHumanSyncOn() && WBMSparam.set_com_height_fix)rp_ref_out.P[com].p(Z) = rp_ref_out.P[com].p_offs(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
-      judgeFootLandOnCommand              (hp_wld_raw.w[rfw], hp_wld_raw.w[lfw], go_rf_landing, go_lf_landing);
-      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, go_rf_landing, go_lf_landing);//ここ
-      applyEEWorkspaceLimit               (rp_ref_out);
-      lockFootXYOnContact                 (go_rf_landing, go_lf_landing, rp_ref_out);//根本から改変すべき3
-      if(go_rf_landing)rp_ref_out.P[rf].p(Z) = rp_ref_out.P[rf].p_offs(Z);
-      if(go_lf_landing)rp_ref_out.P[lf].p(Z) = rp_ref_out.P[lf].p_offs(Z);
-      if(!go_rf_landing)rp_ref_out.P[rf].p(Z) += WBMSparam.swing_foot_height_offset;
-      if(!go_lf_landing)rp_ref_out.P[lf].p(Z) += WBMSparam.swing_foot_height_offset;
-
-      if(DEBUG)fprintf(cz_log,"com_ref: %f %f ", rp_ref_out.P[com].p(X), rp_ref_out.P[com].p(Y));
-
-      applyCOMToSupportRegionLimit        (rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, rp_ref_out.P[com].p);
-      setFootRotHorizontalIfGoLanding     (rp_ref_out_old, rp_ref_out);
+      if(loop==0){
+        for(int i=0, l[LR]={rf,lf}; i<LR; i++){
+          rp_ref_out.tgt[l[i]].is_contact = true;
+//          rp_ref_out.tgt[l[i]].go_contact = true;
+          rp_ref_out.tgt[l[i]].cnt = rp_ref_out.tgt[l[i]].abs;
+        }
+        rp_ref_out_old = rp_ref_out;
+      }
+      if(isHumanSyncOn() && WBMSparam.set_com_height_fix)rp_ref_out.tgt[com].abs.p(Z) = rp_ref_out.tgt[com].offs.p(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
+      judgeFootLandOnCommandByFootForce   (hp_wld_raw);//人体足裏反力から各足の接地指令を生成
+      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
+      limitEEWorkspace                    (rp_ref_out);
+      setFootContactPoseByGoContact       (rp_ref_out);
+      applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
+      setFootRotHorizontalIfGoLanding     (rp_ref_out);
 
 //      applyVelLimit                       (rp_ref_out, rp_old_ref_Vel, rp_ref_out);
 
-      applyCOMToSupportRegionLimit        (rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
+      applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
 
-      Vector3ToVector2(rp_ref_out.P[com].p,com_forcp_ref);
+      Vector3ToVector2(rp_ref_out.tgt[com].abs.p,com_forcp_ref);
       static hrp::Vector2 com_forcp_ref_old;
       com_vel_forcp_ref = (com_forcp_ref - com_forcp_ref_old)/DT;
       com_forcp_ref_old = com_forcp_ref;
 
-      applyCOMStateLimitByCapturePoint    (rp_ref_out.P[com].p, rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, com_CP_ref_old, rp_ref_out.P[com].p);
-      applyCOMToSupportRegionLimit        (rp_ref_out.P[rf].p, rp_ref_out.P[lf].p, rp_ref_out.P[com].p);
+      applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
+      applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
       applyLPFilter                       (rp_ref_out);
 
-      r_zmp_raw = rp_ref_out.P[zmp].p;
-      applyZMPCalcFromCOM                 (rp_ref_out.P[com].p,rp_ref_out.P[zmp].p);
-
-
-      if(DEBUG)fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.P[zmp].p(X),rp_ref_out.P[zmp].p(Y));
-      if(DEBUG)fprintf(cz_log,"com_ans2: %f %f ",rp_ref_out.P[com].p(X),rp_ref_out.P[com].p(Y));
+      r_zmp_raw = rp_ref_out.tgt[zmp].abs.p;
+      applyZMPCalcFromCOM                 (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);
 
-      overwriteFootZFromFootLandOnCommand (go_rf_landing, go_lf_landing, rp_ref_out);
-      modifyFootRotAndXYForContact        (rp_ref_out.P[rf], rp_ref_out.P[lf]);
+      overwriteFootZFromFootLandOnCommand (rp_ref_out);
+      modifyFootRotAndXYForContact        (rp_ref_out);
 
-      if(DEBUG)fprintf(cz_log,"rf: %f %f %f ",rp_ref_out.P[rf].p(X),rp_ref_out.P[rf].p(Y),rp_ref_out.P[rf].p(Z));
-      if(DEBUG)fprintf(cz_log,"lf: %f %f %f ",rp_ref_out.P[lf].p(X),rp_ref_out.P[lf].p(Y),rp_ref_out.P[lf].p(Z));
-      if(DEBUG)fprintf(cz_log,"\n");
-
-
-
-      H_cur = rp_ref_out.P[com].p(Z) - std::min((double)rp_ref_out.P[rf].p(Z), (double)rp_ref_out.P[rf].p(Z));
-      com_vel_old = (rp_ref_out.P[com].p - rp_ref_out_old.P[com].p)/DT;
+      H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[rf].abs.p(Z));
+      com_vel_old = (rp_ref_out.tgt[com].abs.p - rp_ref_out_old.tgt[com].abs.p)/DT;
       rp_ref_out_old = rp_ref_out;
       loop++;
       gettimeofday(&t_calc_end, NULL);
     }
-    void calibInitHumanCOMFromZMP(){
-      init_hp_calibcom(X) = r_zmp_raw(X);
-      init_hp_calibcom(Y) = r_zmp_raw(Y);
-    }
-    static void Vector3ToVector2(const hrp::Vector3& in, hrp::Vector2& out){ out(X) = in(X); out(Y) = in(Y);}
-    static void Vector2ToVector3(const hrp::Vector2& in, hrp::Vector3& out){ out(X) = in(X); out(Y) = in(Y);}
-    static void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out(X) = in.x; out(Y) = in.y; out(Z) = in.z;}
-    static void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out(X) = in.r; out(Y) = in.p; out(Z) = in.y;}
-    static void Vector3ToPoint3D(const hrp::Vector3& in, RTC::Point3D& out){ out.x = in(X); out.y = in(Y); out.z = in(Z);}
-    static void Vector3ToOriantation3D(const hrp::Vector3& in, RTC::Orientation3D& out){ out.r = in(X); out.p = in(Y); out.y = in(Z);}
-    static void Pose3DToHRPPose3D(const RTC::Pose3D& in, HRPPose3D& out){ Point3DToVector3(in.position,out.p); Oriantation3DToVector3(in.orientation,out.rpy); }
-    static void HRPPose3DToPose3D(const HRPPose3D& in, RTC::Pose3D& out){ Vector3ToPoint3D(in.p,out.position); Vector3ToOriantation3D(in.rpy,out.orientation); }
-    static void DoubleSeqToWrench6(const RTC::TimedDoubleSeq::_data_seq& in, Wrench6& out){
-      if(in.length() == 6){out.w(fx)=in[0]; out.w(fy)=in[1]; out.w(fz)=in[2]; out.w(tz)=in[3]; out.w(ty)=in[4]; out.w(tz)=in[5];
-      }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; out.clear(); }
-    }
-    static void Wrench6ToDoubleSeq(const Wrench6& in, RTC::TimedDoubleSeq::_data_seq& out){
-      if(out.length() == 6){out[0]=in.w(fx); out[1]=in.w(fy); out[2]=in.w(fz); out[3]=in.w(tx); out[4]=in.w(ty); out[5]=in.w(tz);
-      }else{ std::cerr<<"[WARN] HumanPose::Wrench6 DoubleSeqToWrench6() invalid data length"<<std::endl; }
-    }
-    static double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(X)*b(Y)-a(Y)*b(X); }
+    static inline void Vector3ToVector2(const hrp::Vector3& in, hrp::Vector2& out){ out(X) = in(X); out(Y) = in(Y);}
+    static inline void Vector2ToVector3(const hrp::Vector2& in, hrp::Vector3& out){ out(X) = in(X); out(Y) = in(Y);}
+    static inline void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out(X) = in.x; out(Y) = in.y; out(Z) = in.z;}
+    static inline void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out(X) = in.r; out(Y) = in.p; out(Z) = in.y;}
+    static inline void Vector3ToPoint3D(const hrp::Vector3& in, RTC::Point3D& out){ out.x = in(X); out.y = in(Y); out.z = in(Z);}
+    static inline void Vector3ToOriantation3D(const hrp::Vector3& in, RTC::Orientation3D& out){ out.r = in(X); out.p = in(Y); out.y = in(Z);}
+    static inline void Pose3DToWBMSPose3D(const RTC::Pose3D& in, WBMSPose3D& out){ Point3DToVector3(in.position,out.p); Oriantation3DToVector3(in.orientation,out.rpy); }
+    static inline void WBMSPose3DToPose3D(const WBMSPose3D& in, RTC::Pose3D& out){ Vector3ToPoint3D(in.p,out.position); Vector3ToOriantation3D(in.rpy,out.orientation); }
+    static inline void DoubleSeqToVector6(const RTC::TimedDoubleSeq::_data_seq& in, hrp::dvector6& out){
+      if(in.length() == 6){out(fx)=in[0]; out(fy)=in[1]; out(fz)=in[2]; out(tz)=in[3]; out(ty)=in[4]; out(tz)=in[5];
+      }else{ std::cerr<<"[WARN] HumanPose::hrp::dvector6 DoubleSeqTohrp::dvector6() invalid data length"<<std::endl; out = hrp::dvector6::Zero(); }
+    }
+    static inline void Vector6ToDoubleSeq(const hrp::dvector6& in, RTC::TimedDoubleSeq::_data_seq& out){
+      if(out.length() == 6){out[0]=in(fx); out[1]=in(fy); out[2]=in(fz); out[3]=in(tx); out[4]=in(ty); out[5]=in(tz);
+      }else{ std::cerr<<"[WARN] HumanPose::hrp::dvector6 DoubleSeqTohrp::dvector6() invalid data length"<<std::endl; }
+    }
+    static inline double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(X)*b(Y)-a(Y)*b(X); }
 
   private:
-    void updateParams(){
-//      tgt_h2r_ratio = H_cur/WBMSparam.human_com_height;
+    inline void updateParams(){
       updateHumanToRobotRatio(tgt_h2r_ratio);
     }
-    void updateHumanToRobotRatio(const double h2r_r_goal){//h2r_ratioに伴って変わる変数の処理もここに書く
+    inline void updateHumanToRobotRatio(const double h2r_r_goal){//h2r_ratioに伴って変わる変数の処理もここに書く
       const double step = 0.001;
       if(h2r_r_goal - h2r_ratio > step){h2r_ratio += step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
       else if(h2r_r_goal - h2r_ratio < (-1)*step){h2r_ratio -= step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
       else{h2r_ratio = h2r_r_goal;}
-      init_wld_hp_rfpos = init_wld_rp_rfeepos/h2r_ratio;
-      init_wld_hp_lfpos = init_wld_rp_lfeepos/h2r_ratio;
     }
-    void autoLRSwapCheck(const HumanPose& in, HumanPose& out){
+    inline void autoLRSwapCheck(const HumanPose& in, HumanPose& out){
       out = in;
-      if(in.P[rh].p_offs(Y) > in.P[lh].p_offs(Y) ){
-        out.P[rh] = in.P[lh];
-        out.P[lh] = in.P[rh];
+      if(in.tgt[rh].offs.p(Y) > in.tgt[lh].offs.p(Y) ){ out.tgt[rh] = in.tgt[lh]; out.tgt[lh] = in.tgt[rh]; }
+      if(in.tgt[rf].offs.p(Y) > in.tgt[lf].offs.p(Y) ){ out.tgt[rf] = in.tgt[lf]; out.tgt[lf] = in.tgt[rf]; }
+    }
+    inline void convertRelHumanPoseToRelRobotPose(const HumanPose& in, HumanPose& out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
+//      out = in;//ダメゼッタイ
+      for(int i=0, l[5]={com,rf,lf,rh,lh}; i<5; i++){
+        out.tgt[l[i]].abs.p   =  h2r_ratio * (in.tgt[l[i]].abs.p - in.tgt[l[i]].offs.p)   + out.tgt[l[i]].offs.p;
+        out.tgt[l[i]].abs.rpy =  hrp::rpyFromRot( hrp::rotFromRpy(in.tgt[l[i]].abs.rpy) * hrp::rotFromRpy(in.tgt[l[i]].offs.rpy).transpose() * hrp::rotFromRpy(out.tgt[l[i]].offs.rpy) );
       }
-      if(in.P[rf].p_offs(Y) > in.P[lf].p_offs(Y) ){
-        out.P[rf] = in.P[lf];
-        out.P[lf] = in.P[rf];
+      for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
+        out.tgt[l[i]].w = in.tgt[l[i]].w;
+        out.tgt[l[i]].go_contact = in.tgt[l[i]].go_contact;
       }
+      out.tgt[head].abs = in.tgt[head].abs;
+      out.tgt[zmp].abs = in.tgt[zmp].abs;//最近使わない
     }
-    void applyInitHumanZMPToCOMOffset(const HumanPose& in, const hrp::Vector3& com_offs, HumanPose& out){
-      if(isHumanSyncOn()){
-        if(!init_zmp_com_offset_interpolator->isEmpty()){
-          init_zmp_com_offset_interpolator->get(&zmp_com_offset_ip_ratio, true);
-        }
-      }else{
-        zmp_com_offset_ip_ratio = 0;
+    inline void judgeFootLandOnCommandByFootForce(HumanPose& in){
+      for(int i=0, l[LR]={rf,lf}; i<LR; i++){
+        if      (in.tgt[l[i]].is_contact  && in.tgt[l[i]].w(fz)<CNT_F_TH   ){in.tgt[l[i]].is_contact = false;}//足ついた状態から上げる
+        else if (!in.tgt[l[i]].is_contact && in.tgt[l[i]].w(fz)>CNT_F_TH+30){in.tgt[l[i]].is_contact = true;}//足浮いた状態から下げる
       }
-      out = in;
-      out.P[com].p = in.P[com].p + zmp_com_offset_ip_ratio * com_offs;
     }
-    void applyLPFilter(HumanPose& tgt){
-      const int ns[] = {com,rf,lf,rh,lh,head}, num_ns = sizeof(ns)/sizeof(int);
+    inline void applyLPFilter(HumanPose& tgt){
       if(loop==0){
-        cout<<"init LPF"<<endl;
-        for(int i=0;i<num_ns;i++){ tgt_pos_filters[ns[i]].init(tgt.P[ns[i]].p);  }// tgt_pos_filters[ns[i]].passFilter(tgt.P[ns[i]].p);  }
-        for(int i=0;i<num_ns;i++){ tgt_rot_filters[ns[i]].init(tgt.P[ns[i]].rpy); }//tgt_pos_filters[ns[i]].passFilter(tgt.P[ns[i]].rpy);}
-      }
-      for(int i=0;i<num_ns;i++){ tgt.P[ns[i]].p   = tgt_pos_filters[ns[i]].passFilter(tgt.P[ns[i]].p);  }
-      for(int i=0;i<num_ns;i++){ tgt.P[ns[i]].rpy = tgt_rot_filters[ns[i]].passFilter(tgt.P[ns[i]].rpy);}
-    }
-    void convertRelHumanPoseToRelRobotPose(const HumanPose& hp_in, HumanPose& rp_out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
-      const int ns[] = {com,rf,lf,rh,lh,zmp}, num_ns = sizeof(ns)/sizeof(int);
-      for(int i=0;i<num_ns;i++){
-        rp_out.P[ns[i]].p   =  h2r_ratio * (hp_in.P[ns[i]].p - hp_in.P[ns[i]].p_offs)   + rp_out.P[ns[i]].p_offs;
-        rp_out.P[ns[i]].rpy =  hrp::rpyFromRot( hrp::rotFromRpy(hp_in.P[ns[i]].rpy) * hrp::rotFromRpy(hp_in.P[ns[i]].rpy_offs).transpose() * hrp::rotFromRpy(rp_out.P[ns[i]].rpy_offs) );
+        for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
+          tgt_pos_filters[l[i]].init(tgt.tgt[l[i]].abs.p);
+          tgt_rot_filters[l[i]].init(tgt.tgt[l[i]].abs.rpy);
+        }
       }
-      rp_out.P[head] = hp_in.P[head];
-      rp_out.w[rfw]  = hp_in.w[rfw];
-      rp_out.w[lfw]  = hp_in.w[lfw];
-    }
-    void judgeFootLandOnCommand(const Wrench6& rfw_in, const Wrench6& lfw_in, bool& rfloc_ans, bool& lfloc_ans){
-      if(isHumanSyncOn()){
-        if      (rfloc_ans  && rfw_in.w(fz)<CNT_F_TH   ){rfloc_ans = false;}//右足ついた状態から上げる
-        else if (!rfloc_ans && rfw_in.w(fz)>CNT_F_TH+30){rfloc_ans = true;}//右足浮いた状態から下げる
-        if      (lfloc_ans  && lfw_in.w(fz)<CNT_F_TH   ){lfloc_ans = false;}//左足ついた状態から上げる
-        else if (!lfloc_ans && lfw_in.w(fz)>CNT_F_TH+30){lfloc_ans = true;}//左足浮いた状態から下げる
+      for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
+        tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
+        tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
       }
-      //      if(!rfloc_ans && !lfloc_ans){rfloc_ans = lfloc_ans = true;}//両足ジャンプは禁止
     }
-    void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& tgt, bool& rfcs_ans, bool& lfcs_ans){
+    inline void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
       hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);
-      hrp::Vector2 rf2zmp = hrp::Vector2(tgt.P[zmp].p(X),tgt.P[zmp].p(Y)) - hrp::Vector2(tgt.P[rf].p(X),tgt.P[rf].p(Y));
-      hrp::Vector2 lf2zmp = hrp::Vector2(tgt.P[zmp].p(X),tgt.P[zmp].p(Y)) - hrp::Vector2(tgt.P[lf].p(X),tgt.P[lf].p(Y));
-      if( inside_vec_rf.dot(rf2zmp) / inside_vec_rf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ lfcs_ans = true; }
-      if( inside_vec_lf.dot(lf2zmp) / inside_vec_lf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ rfcs_ans = true; }
-    }
-    void setFootRotHorizontalIfGoLanding(const HumanPose& rp_ref_out_old, HumanPose& rp_ref_out){
-      if(go_rf_landing){ rp_ref_out.P[rf].rpy(r) = rp_ref_out.P[rf].rpy(p) = 0; }
-      if(go_lf_landing){ rp_ref_out.P[lf].rpy(r) = rp_ref_out.P[lf].rpy(p) = 0; }
-    }
-    void overwriteFootZFromFootLandOnCommand(const bool& rf_goland_in, const bool& lf_goland_in, HumanPose& tgt){
-      const double com2rf_dist = fabs((double)tgt.P[rf].p(Y) - (double)tgt.P[com].p(Y));
-      const double com2lf_dist = fabs((double)tgt.P[lf].p(Y) - (double)tgt.P[com].p(Y));
-      limitGroundContactVelocity(rf_goland_in, com2rf_dist, rp_ref_out_old.P[rf], tgt.P[rf], is_rf_contact);
-      limitGroundContactVelocity(lf_goland_in, com2lf_dist, rp_ref_out_old.P[lf], tgt.P[lf], is_lf_contact);
-    }
-    void limitGroundContactVelocity(const bool& go_land_in, const double& com2foot_dist, const HRPPose3D& f_old_in, HRPPose3D& f_in_out, bool& is_f_contact_out){
+      hrp::Vector2 rf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[rf].abs.p(X),old.tgt[rf].abs.p(Y));
+      hrp::Vector2 lf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[lf].abs.p(X),old.tgt[lf].abs.p(Y));
+      if( inside_vec_rf.dot(rf2zmp) / inside_vec_rf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ out.tgt[lf].go_contact = true; }
+      if( inside_vec_lf.dot(lf2zmp) / inside_vec_lf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ out.tgt[rf].go_contact = true; }
+    }
+    inline void limitEEWorkspace(HumanPose& out){
+      const double MAX_FW = 0.25;
+//      const double MAX_FW = 1000000;//manipulability test
+      const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
+      if(!out.tgt[rf].is_contact && out.tgt[lf].is_contact){//右足浮遊時
+        const hrp::Vector2 lf2rf_vec( out.tgt[rf].abs.p(X) - out.tgt[lf].cnt.p(X), out.tgt[rf].abs.p(Y) - out.tgt[lf].cnt.p(Y) );
+        if(lf2rf_vec.norm() > MAX_FW){
+          out.tgt[rf].abs.p(X) = (lf2rf_vec.normalized())(X) * MAX_FW + out.tgt[lf].cnt.p(X);
+          out.tgt[rf].abs.p(Y) = (lf2rf_vec.normalized())(Y) * MAX_FW + out.tgt[lf].cnt.p(Y);
+        }
+        LIMIT_MAX(out.tgt[rf].abs.p(Y), out.tgt[lf].cnt.p(Y) - FOOT_2_FOOT_COLLISION_MARGIIN);
+      }
+      else if(out.tgt[rf].is_contact && !out.tgt[lf].is_contact){//左足浮遊時
+        const hrp::Vector2 rf2lf_vec( out.tgt[lf].abs.p(X) - out.tgt[rf].cnt.p(X), out.tgt[lf].abs.p(Y) - out.tgt[rf].cnt.p(Y) );
+        if(rf2lf_vec.norm() > MAX_FW){
+          out.tgt[lf].abs.p(X) = (rf2lf_vec.normalized())(X) * MAX_FW + out.tgt[rf].cnt.p(X);
+          out.tgt[lf].abs.p(Y) = (rf2lf_vec.normalized())(Y) * MAX_FW + out.tgt[rf].cnt.p(Y);
+        }
+        LIMIT_MIN(out.tgt[lf].abs.p(Y), out.tgt[rf].cnt.p(Y) + FOOT_2_FOOT_COLLISION_MARGIIN);
+      }
+//      hrp::Vector3 init_base2rh = out.tgt[rh].offs.p - baselinkpose.p_offs;
+//      hrp::Vector3 init_base2lh = out.tgt[lh].offs.p - baselinkpose.p_offs;
+//      hrp::Matrix33 init_base2rh_R = hrp::rotFromRpy(baselinkpose.rpy_offs).transpose() * hrp::rotFromRpy(out.tgt[rh].offs.rpy);
+//      hrp::Matrix33 init_base2lh_R = hrp::rotFromRpy(baselinkpose.rpy_offs).transpose() * hrp::rotFromRpy(out.tgt[lh].offs.rpy);
+//      hrp::Vector3 hand_ulimit = hrp::Vector3(0.1,0.1,0.1);
+//      hrp::Vector3 hand_llimit = hrp::Vector3(-0.1,-0.1,-0.1);
+//      if(!WBMSparam.use_rh){
+//        out.tgt[rh].abs.rpy = hrp::rpyFromRot( hrp::rotFromRpy(baselinkpose.rpy) * init_base2rh_R);
+//        out.tgt[rh].abs.p = baselinkpose.p + hrp::rotFromRpy(out.tgt[rh].abs.rpy) * init_base2rh;
+//      }
+//      if(!WBMSparam.use_lh){
+//        out.tgt[lh].abs.rpy = hrp::rpyFromRot( hrp::rotFromRpy(baselinkpose.rpy) * init_base2lh_R);
+//        out.tgt[lh].abs.p = baselinkpose.p + hrp::rotFromRpy(out.tgt[lh].abs.rpy) * init_base2lh;
+//      }
+//      for(int i=0;i<3;i++){
+//        LIMIT_MINMAX( out.tgt[rh].abs.p(i), (baselinkpose.p+init_base2rh+hand_llimit)(i), (baselinkpose.p+init_base2rh+hand_ulimit)(i) );
+//        LIMIT_MINMAX( out.tgt[lh].abs.p(i), (baselinkpose.p+init_base2lh+hand_llimit)(i), (baselinkpose.p+init_base2lh+hand_ulimit)(i) );
+//      }
+      LIMIT_MINMAX( out.tgt[rf].abs.p(Z), out.tgt[rf].offs.p(Z), out.tgt[rf].offs.p(Z)+WBMSparam.swing_foot_max_height);
+      LIMIT_MINMAX( out.tgt[lf].abs.p(Z), out.tgt[lf].offs.p(Z), out.tgt[lf].offs.p(Z)+WBMSparam.swing_foot_max_height);
+
+      for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( out.tgt[com].abs.rpy(i), rc.ee_rot_limit[com][MIN](i), rc.ee_rot_limit[com][MAX](i) ); }
+      LIMIT_MINMAX( out.tgt[com].abs.p(Z), out.tgt[com].offs.p(Z) - 0.15, out.tgt[com].offs.p(Z) + 0.03 );//COM高さ方向の制限
+      for(int i=0, l[5]={rf,lf,rh,lh,head}; i<5; i++){
+        for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( out.tgt[l[i]].abs.rpy(j), rc.ee_rot_limit[l[i]][MIN](j) + out.tgt[com].abs.rpy(j), rc.ee_rot_limit[l[i]][MAX](j) + out.tgt[com].abs.rpy(j) ); }
+      }
+      if(!WBMSparam.use_head)out.tgt[head].abs.rpy = hrp::Vector3::Zero();
+      if(!isHumanSyncOn())out.tgt[head].abs.rpy = hrp::Vector3::Zero();
+    }
+    inline void setFootContactPoseByGoContact(HumanPose& out){
+      for(int i=0, l[LR]={rf,lf}; i<LR; i++){
+        if(out.tgt[l[i]].go_contact){
+          out.tgt[l[i]].abs.p(X) = out.tgt[l[i]].cnt.p(X);
+          out.tgt[l[i]].abs.p(Y) = out.tgt[l[i]].cnt.p(Y);
+          out.tgt[l[i]].abs.rpy(y) = out.tgt[l[i]].cnt.rpy(y);
+        }else{
+          out.tgt[l[i]].cnt.p(X) = out.tgt[l[i]].abs.p(X);
+          out.tgt[l[i]].cnt.p(Y) = out.tgt[l[i]].abs.p(Y);
+          out.tgt[l[i]].cnt.rpy(y) = out.tgt[l[i]].abs.rpy(y);
+        }
+        if(out.tgt[l[i]].go_contact)rp_ref_out.tgt[l[i]].abs.p(Z) = rp_ref_out.tgt[l[i]].offs.p(Z);
+        if(!out.tgt[l[i]].go_contact)rp_ref_out.tgt[l[i]].abs.p(Z) += WBMSparam.swing_foot_height_offset;
+      }
+    }
+    inline void setFootRotHorizontalIfGoLanding(HumanPose& out){
+      if( out.tgt[rf].go_contact){ out.tgt[rf].abs.rpy(r) = out.tgt[rf].abs.rpy(p) = 0; }
+      if( out.tgt[lf].go_contact){ out.tgt[lf].abs.rpy(r) = out.tgt[lf].abs.rpy(p) = 0; }
+    }
+    inline void overwriteFootZFromFootLandOnCommand(HumanPose& out){
+      const double com2rf_dist = fabs((double)out.tgt[rf].abs.p(Y) - (double)out.tgt[com].abs.p(Y));
+      const double com2lf_dist = fabs((double)out.tgt[lf].abs.p(Y) - (double)out.tgt[com].abs.p(Y));
+      limitGroundContactVelocity(out.tgt[rf].go_contact, com2rf_dist, rp_ref_out_old.tgt[rf], out.tgt[rf], out.tgt[rf].is_contact);
+      limitGroundContactVelocity(out.tgt[lf].go_contact, com2lf_dist, rp_ref_out_old.tgt[lf], out.tgt[lf], out.tgt[lf].is_contact);
+    }
+    inline void limitGroundContactVelocity(const bool& go_land_in, const double& com2foot_dist, const PoseTGT& f_old_in, PoseTGT& f_in_out, bool& is_f_contact_out){
       double penalty = 1.0 + (0.2 - com2foot_dist) * 5.0;//1.0 ~ 2.0 本当はCPに比例すべき？
       LIMIT_MINMAX( penalty, 1.0, 2.0);
-      LIMIT_MIN( f_in_out.p(Z), f_in_out.p_offs(Z));
-      double input_vel = (f_in_out.p(Z) - f_old_in.p(Z)) / DT;
+      LIMIT_MIN( f_in_out.abs.p(Z), f_in_out.offs.p(Z));
+      double input_vel = (f_in_out.abs.p(Z) - f_old_in.abs.p(Z)) / DT;
       const double vel_limit_k = WBMSparam.foot_vertical_vel_limit_coeff * penalty;//地面から0.02[m]地点で0.02*8=0.16[m/s]出ている計算
-      double limit_vel = - vel_limit_k * (f_old_in.p(Z) - f_old_in.p_offs(Z));//sinの時は平均0.05~0.10[m/s]
+      double limit_vel = - vel_limit_k * (f_old_in.abs.p(Z) - f_old_in.offs.p(Z));//sinの時は平均0.05~0.10[m/s]
       const double max_vel_threshold = -0.01;
       LIMIT_MAX( limit_vel, max_vel_threshold);//速度0に近づくと目標になかなか到達しないから
       LIMIT_MIN( input_vel, limit_vel);//着地時の下向きの速度を制限
-      f_in_out.p(Z) = f_old_in.p(Z) + input_vel * DT;
-      LIMIT_MIN( f_in_out.p(Z), f_in_out.p_offs(Z));
+      f_in_out.abs.p(Z) = f_old_in.abs.p(Z) + input_vel * DT;
+      LIMIT_MIN( f_in_out.abs.p(Z), f_in_out.offs.p(Z));
       const double contact_threshold = 0.005;
-      is_f_contact_out = ((f_in_out.p(Z)-f_in_out.p_offs(Z)) <= contact_threshold);
+      is_f_contact_out = ((f_in_out.abs.p(Z)-f_in_out.offs.p(Z)) <= contact_threshold);
     }
-    void modifyFootRotAndXYForContact(HRPPose3D& rf_in, HRPPose3D& lf_in){
-      calcFootRotAndXYTransitionForContact(rf_in,rf_vert);
-      calcFootRotAndXYTransitionForContact(lf_in,lf_vert);
+    inline void modifyFootRotAndXYForContact(HumanPose& out){
+      calcFootRotAndXYTransitionForContact(out.tgt[rf],rf_vert);
+      calcFootRotAndXYTransitionForContact(out.tgt[lf],lf_vert);
     }
-    void calcFootRotAndXYTransitionForContact(HRPPose3D& foot_in, const std::vector<hrp::Vector3>& act_sole_size){
+    inline void calcFootRotAndXYTransitionForContact(PoseTGT& foot_in, const std::vector<hrp::Vector3>& act_sole_size){
       std::vector<hrp::Vector3> act_foot_vert;
-      double min_height = foot_in.p(Z);
+      double min_height = foot_in.abs.p(Z);
       int min_height_id = 0;
       for(int i=0;i<act_sole_size.size();i++){
-        act_foot_vert.push_back(hrp::rotFromRpy(foot_in.rpy) * act_sole_size[i]);
-        act_foot_vert[i](Z) += foot_in.p(Z);
+        act_foot_vert.push_back(hrp::rotFromRpy(foot_in.abs.rpy) * act_sole_size[i]);
+        act_foot_vert[i](Z) += foot_in.abs.p(Z);
         if(act_foot_vert[i](Z) < min_height){
           min_height = act_foot_vert[i](Z);
           min_height_id = i;
         }
       }
-      if(act_foot_vert[min_height_id](Z) < foot_in.p_offs(Z)){
-        foot_in.p(Z) += foot_in.p_offs(Z) - act_foot_vert[min_height_id](Z);
-      }
-    }
-    void lockFootXYOnContact(const bool& rfcs_in, const bool& lfcs_in, HumanPose& tgt){
-      if(rfcs_in){
-        tgt.P[rf].p(X) = pre_cont_rfpos(X);
-        tgt.P[rf].p(Y) = pre_cont_rfpos(Y);
-        tgt.P[rf].rpy(y) = pre_cont_rfrot(Z);
-      }else{
-        pre_cont_rfpos(X) = tgt.P[rf].p(X);
-        pre_cont_rfpos(Y) = tgt.P[rf].p(Y);
-        pre_cont_rfrot(Z) = tgt.P[rf].rpy(y);
-      }
-      if(lfcs_in){
-        tgt.P[lf].p(X) = pre_cont_lfpos(X);
-        tgt.P[lf].p(Y) = pre_cont_lfpos(Y);
-        tgt.P[lf].rpy(y) = pre_cont_lfrot(Z);
-      }else{
-        pre_cont_lfpos(X) = tgt.P[lf].p(X);
-        pre_cont_lfpos(Y) = tgt.P[lf].p(Y);
-        pre_cont_lfrot(Z) = tgt.P[lf].rpy(y);
-      }
-    }
-    void applyEEWorkspaceLimit(HumanPose& tgt){
-      const double MAX_FW = 0.25;
-//      const double MAX_FW = 0.5;
-//      const double MAX_FW = 1000000;//manipulability test
-      const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
-      if(!is_rf_contact && is_lf_contact){//右足浮遊時
-        const hrp::Vector2 lf2rf_vec( tgt.P[rf].p(X)-pre_cont_lfpos(X), tgt.P[rf].p(Y)-pre_cont_lfpos(Y) );
-        if(lf2rf_vec.norm() > MAX_FW){
-          tgt.P[rf].p(X) = (lf2rf_vec.normalized())(X) * MAX_FW + pre_cont_lfpos(X);
-          tgt.P[rf].p(Y) = (lf2rf_vec.normalized())(Y) * MAX_FW + pre_cont_lfpos(Y);
-        }
-        LIMIT_MAX(tgt.P[rf].p(Y), pre_cont_lfpos(Y) - FOOT_2_FOOT_COLLISION_MARGIIN);
-      }
-      else if(is_rf_contact && !is_lf_contact){//左足浮遊時
-        const hrp::Vector2 rf2lf_vec( tgt.P[lf].p(X)-pre_cont_rfpos(X), tgt.P[lf].p(Y)-pre_cont_rfpos(Y) );
-        if(rf2lf_vec.norm() > MAX_FW){
-          tgt.P[lf].p(X) = (rf2lf_vec.normalized())(X) * MAX_FW + pre_cont_rfpos(X);
-          tgt.P[lf].p(Y) = (rf2lf_vec.normalized())(Y) * MAX_FW + pre_cont_rfpos(Y);
-        }
-        LIMIT_MIN(tgt.P[lf].p(Y), pre_cont_rfpos(Y) + FOOT_2_FOOT_COLLISION_MARGIIN);
-      }
-      hrp::Vector3 init_base2rh = tgt.P[rh].p_offs - baselinkpose.p_offs;
-      hrp::Vector3 init_base2lh = tgt.P[lh].p_offs - baselinkpose.p_offs;
-      hrp::Matrix33 init_base2rh_R = hrp::rotFromRpy(baselinkpose.rpy_offs).transpose() * hrp::rotFromRpy(tgt.P[rh].rpy_offs);
-      hrp::Matrix33 init_base2lh_R = hrp::rotFromRpy(baselinkpose.rpy_offs).transpose() * hrp::rotFromRpy(tgt.P[lh].rpy_offs);
-      hrp::Vector3 hand_ulimit = hrp::Vector3(0.1,0.1,0.1);
-      hrp::Vector3 hand_llimit = hrp::Vector3(-0.1,-0.1,-0.1);
-      if(!WBMSparam.use_rh){
-        tgt.P[rh].rpy = hrp::rpyFromRot( hrp::rotFromRpy(baselinkpose.rpy) * init_base2rh_R);
-        tgt.P[rh].p = baselinkpose.p + hrp::rotFromRpy(tgt.P[rh].rpy) * init_base2rh;
-      }
-      if(!WBMSparam.use_lh){
-        tgt.P[lh].rpy = hrp::rpyFromRot( hrp::rotFromRpy(baselinkpose.rpy) * init_base2lh_R);
-        tgt.P[lh].p = baselinkpose.p + hrp::rotFromRpy(tgt.P[lh].rpy) * init_base2lh;
+      if(act_foot_vert[min_height_id](Z) < foot_in.offs.p(Z)){
+        foot_in.abs.p(Z) += foot_in.offs.p(Z) - act_foot_vert[min_height_id](Z);
       }
-      for(int i=0;i<3;i++){
-        LIMIT_MINMAX( tgt.P[rh].p(i), (baselinkpose.p+init_base2rh+hand_llimit)(i), (baselinkpose.p+init_base2rh+hand_ulimit)(i) );
-        LIMIT_MINMAX( tgt.P[lh].p(i), (baselinkpose.p+init_base2lh+hand_llimit)(i), (baselinkpose.p+init_base2lh+hand_ulimit)(i) );
-      }
-      LIMIT_MINMAX( tgt.P[rf].p(Z), tgt.P[rf].p_offs(Z), tgt.P[rf].p_offs(Z)+WBMSparam.swing_foot_max_height);
-      LIMIT_MINMAX( tgt.P[lf].p(Z), tgt.P[lf].p_offs(Z), tgt.P[lf].p_offs(Z)+WBMSparam.swing_foot_max_height);
-
-      for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( tgt.P[com].rpy(i), rc.ee_rot_limit[com][MIN](i), rc.ee_rot_limit[com][MAX](i) ); }
-      LIMIT_MINMAX( tgt.P[com].p(Z), tgt.P[com].p_offs(Z) - 0.15, tgt.P[com].p_offs(Z) + 0.03 );//COM高さ方向の制限
-      const int ns[] = {rf,lf,rh,lh,head};
-      for(int i=0;i<sizeof(ns)/sizeof(int);i++){
-        for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( tgt.P[ns[i]].rpy(j), rc.ee_rot_limit[ns[i]][MIN](j) + tgt.P[com].rpy(j), rc.ee_rot_limit[ns[i]][MAX](j) + tgt.P[com].rpy(j) ); }
-      }
-      if(!WBMSparam.use_head)tgt.P[head].rpy = hrp::Vector3::Zero();
-      if(!isHumanSyncOn())tgt.P[head].rpy = hrp::Vector3::Zero();
     }
-    bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
+    inline bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
       hull_com.clear();
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull_com);
       hrp::Vector2 cog(comin_abs(X),comin_abs(Y));
@@ -634,7 +497,7 @@ class HumanSynchronizer{
       comin_abs(Y) = cog(Y);
       return true;
     }
-    bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans_old, hrp::Vector3& com_ans){
+    inline bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans_old, hrp::Vector3& com_ans){
       com_ans = com_in;
       if (loop==0)com_ans_old = com_in;
       hrp::Vector3 com_vel = (com_in - com_ans_old)/DT;
@@ -662,7 +525,7 @@ class HumanSynchronizer{
       }
       return true;
     }
-    void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull_d, const std::vector<hrp::Vector2>& hull_a, hrp::Vector2& com_vel_ans){
+    inline void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull_d, const std::vector<hrp::Vector2>& hull_a, hrp::Vector2& com_vel_ans){
       hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
       com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
 
@@ -709,7 +572,7 @@ class HumanSynchronizer{
       }
       com_vel_ans_old = com_vel_ans;
     }
-    void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
+    inline void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
       rflf_points.clear();
       hull_ans.clear();
       for(int i=0;i<2;i++){
@@ -720,16 +583,124 @@ class HumanSynchronizer{
       }
       makeConvexHullOpenCV(rflf_points, hull_ans);
     }
-    void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const Wrench6& rfwin, const Wrench6& lfwin, hrp::Vector3& zmp_ans);
-    void calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans);
-    bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_will_cross);
-    double calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_ans);
-    void makeConvexHullOpenCV(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans);
-    void makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans);
-    bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull);
-    void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout);
-//    void applyVelLimit(const HumanPose& in, HumanPose& out_old, HumanPose& out);
-    void applyCOMZMPXYZLock(HumanPose& tgt);
+    inline void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const hrp::dvector6& rfwin, const hrp::dvector6& lfwin, hrp::Vector3& zmp_ans){
+      hrp::Vector3 rfzmp,lfzmp;
+      const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
+      if( rfwin(fz) > 1.0e-6 ){
+        rfzmp(0) = ( - rfwin(ty) - rfwin(fx) * F_H_OFFSET + rfwin(fz) * 0 ) / rfwin(fz) + rfpos(0);
+        rfzmp(1) = (   rfwin(tx) - rfwin(fy) * F_H_OFFSET + rfwin(fz) * 0 ) / rfwin(fz) + rfpos(1);
+      }
+      if( lfwin(fz) > 1.0e-6 ){
+        lfzmp(0) = ( - lfwin(ty) - lfwin(fx) * F_H_OFFSET + lfwin(fz) * 0 ) / lfwin(fz) + lfpos(0);
+        lfzmp(1) = (   lfwin(tx) - lfwin(fy) * F_H_OFFSET + lfwin(fz) * 0 ) / lfwin(fz) + lfpos(1);
+      }
+      if( rfwin(fz) > 1.0e-6 || lfwin(fz) > 1.0e-6 ){
+        zmp_ans(0) = ( rfzmp(0)*rfwin(fz) + lfzmp(0)*lfwin(fz) ) / ( rfwin(fz) + lfwin(fz));
+        zmp_ans(1) = ( rfzmp(1)*rfwin(fz) + lfzmp(1)*lfwin(fz) ) / ( rfwin(fz) + lfwin(fz));
+      }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
+      zmp_ans(2) = 0;
+    }
+    inline void calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans){
+      hrp::Vector4 margin_abs;
+      hrp::Vector2 cross_pt, anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
+      for(int direc=0;direc<4;direc++){
+        calcCrossPointOnHull(check_point, check_point+anchor_vec[direc], hull, cross_pt);
+        margin_abs(direc) = (cross_pt - check_point).norm();
+      }
+      margin_ans(0) =  margin_abs(0);
+      margin_ans(1) = -margin_abs(1);
+      margin_ans(2) =  margin_abs(2);
+      margin_ans(3) = -margin_abs(3);
+    }
+    inline bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_will_cross){
+      hrp::Vector2 anchor_vec = pt_out_goal - pt_in_start;
+      for(int i=0;i<hull.size();i++){
+        int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
+        hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
+        hrp::Vector2 cur_edge = nxt_pt - cur_pt;
+        double dBunbo = hrpVector2Cross(anchor_vec,cur_edge);
+        if( dBunbo != 0.0) {//平行の場合を外す
+          hrp::Vector2 vectorAC = hrp::Vector2(hull[i](0),hull[i](1)) - pt_in_start;
+          double dR = hrpVector2Cross(vectorAC,cur_edge) / dBunbo;
+          double dS = hrpVector2Cross(vectorAC,anchor_vec) / dBunbo;
+          if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){//dRには数値誤差が乗る
+            pt_will_cross = pt_in_start + dR * anchor_vec;
+            //dist = dR * anchor_vec.norm();
+            return true;
+          }
+        }
+      }
+      return false;
+    }
+    inline double calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_ans){
+      double cur_nearest_dist, ans_nearest_dist;
+      hrp::Vector2 cur_nearest_pt, ans_nearest_pt;
+      for(int i=0;i<hull.size();i++){
+        int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
+        hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
+        hrp::Vector2 cur_edge = nxt_pt - cur_pt;
+        hrp::Vector2 tgt_pt_v = tgt_pt - cur_pt;
+        double tgt_pt_projected_length = tgt_pt_v.dot(cur_edge.normalized());
+        if(tgt_pt_projected_length > cur_edge.norm() ){//tgt_pt's nearest point is on the　i+1-th vertex
+          cur_nearest_pt = nxt_pt;
+        }else if(tgt_pt_projected_length < 0 ){//tgt_pt's nearest point is on the　i-th vertex
+          cur_nearest_pt = cur_pt;
+        }else{//tgt_pt's nearest point is on the line
+          cur_nearest_pt = cur_pt + tgt_pt_projected_length * cur_edge.normalized();
+        }
+        cur_nearest_dist = (tgt_pt - cur_nearest_pt).norm();
+        if(i==0){//set first candidate as nearest
+          ans_nearest_dist = cur_nearest_dist;
+          ans_nearest_pt = cur_nearest_pt;
+        }else if( cur_nearest_dist < ans_nearest_dist ){//update nearest candidate
+          ans_nearest_dist = cur_nearest_dist;
+          ans_nearest_pt = cur_nearest_pt;
+        }
+      }
+      pt_ans = ans_nearest_pt;
+      return ans_nearest_dist;
+    }
+    inline void makeConvexHullOpenCV(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
+      points.clear();
+      cvhull.clear();
+      hull_ans.clear();
+      for(int i=0;i<pts.size();i++){ points.push_back( cv::Point2f( pts[i](0), pts[i](1)) ); }
+      cv::convexHull(cv::Mat(points),cvhull,true);
+      for(int i=0;i<cvhull.size();i++){ hull_ans.push_back( hrp::Vector2( cvhull[i].x, cvhull[i].y ) );  }
+    }
+    //void makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
+    //  const int dim = 2;
+    //  double points[dim*pts.size()];
+    //  for(int i=0;i<pts.size();i++){ points[dim*i] = pts[i](0); points[dim*i+1] = pts[i](1); }
+    //  qh_new_qhull( dim, pts.size(), points, false, "qhull ", NULL, stderr);
+    //  pointT *point, *pointtemp;
+    //  hull_ans.clear();
+    //  FORALLpoints hull_ans.push_back( hrp::Vector2(point[0],point[1]) );
+    //  qh_freeqhull(!qh_ALL);
+    //  //#define qh_ORIENTclock 1
+    //}
+    inline bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull){
+      cvhull.clear();
+      for(int i=0;i<hull.size();i++){ cvhull.push_back( cv::Point2f( hull[i](0), hull[i](1)) ); }
+      return (cv::pointPolygonTest(cv::Mat(cvhull), cv::Point2f(pt(0),pt(1)), false) > 0);
+    }
+    inline void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
+      comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
+      comacc = acc4zmp_v_filters.passFilter(comacc);
+      zmpout(X) = comin(X)-(H_cur/G)*comacc(X);
+      zmpout(Y) = comin(Y)-(H_cur/G)*comacc(Y);
+      com_oldold = com_old;
+      com_old = comin;
+    }
+    //void applyVelLimit(const HumanPose& in, HumanPose& out_old, HumanPose& out){
+    //  const int ns[] = {com,rf,lf,rh,lh,head}, num_ns = sizeof(ns)/sizeof(int);
+    //  for(int i=0;i<num_ns;i++){
+    //    hrp::Vector3 diff = in.tgt[i].abs.p  - out_old.tgt[i].abs.p;
+    //    for(int j=0;j<3;j++)LIMIT_MINMAX( diff(j), -MAXVEL*DT, MAXVEL*DT);
+    //    out.tgt[i].abs.p = out_old.tgt[i].abs.p + diff;
+    //  }
+    //  out_old = out;
+    //}
 };
 
 #endif // HUMANMASTERSLAVE_H

From e2b022bee078f51b95ee82c1bac96ca3fd678ea9 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 25 May 2017 20:56:09 +0900
Subject: [PATCH 090/217] add manipulability test

---
 .../WholeBodyMasterSlave.cpp                  | 205 ++++++++++++++++--
 rtc/WholeBodyMasterSlave/wbms_core.h          |   4 +-
 2 files changed, 183 insertions(+), 26 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 6f735794648..e4817d94b3d 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -518,6 +518,185 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   if(hsp->WBMSparam.use_manipulability_limit){
     double min_manip_val = 1e9;
     const double min_manip_th = 0.1;
+//    while( 1 ){  //COM 収束ループ＋可操作度リミット
+//      com_ik_loop++;
+//      robot_in->rootLink()->p += tmp_com_err;
+//      robot_in->calcForwardKinematics();
+//      for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_in->ikp.begin(); it != fik_in->ikp.end(); it++ ) {
+//          if (it->second.is_ik_enable) fik_in->solveLimbIK (it->second, it->first, fik_in->ratio_for_vel, false);
+//      }
+//      tmp_com_err = com_ref.p - robot_in->calcCM();
+//
+//      const std::string names[4] = {"rleg","lleg","rarm","larm"};
+//      const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
+//      for(int i=0;i<4;i++){
+//        hrp::dmatrix J;
+//        fik_in->ikp[names[i]].manip->calcJacobian(J);
+//        hrp::dmatrix manipulability_mat = J*J.transpose();
+//        double manipulability = sqrt((J*J.transpose()).determinant());
+//        //    Eigen::JacobiSVD< Eigen::Matrix<float, 3, 3> > svd(mm3, Eigen::ComputeFullU | Eigen::ComputeFullV);
+//        Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
+//
+//        Eigen::MatrixXd::Index row,col;
+//        min_manip_val = svd.singularValues().minCoeff(&row,&col);
+//
+//        hrp::Vector3 min_manip_direc = svd.matrixU().col(row);
+//        //      std::cout << "singular values" << std::endl << svd.singularValues() << std::endl;
+//        //      std::cout << "matrix U" << std::endl << svd.matrixU() << std::endl;
+//        //      std::cout << "matrix V" << std::endl << svd.matrixV() << std::endl;
+//        if(min_manip_direc.dot(fik_in->ikp[names[i]].target_p0 - robot_in->rootLink()->p) > 0){ min_manip_direc *= -1; }//向きみて反転
+//
+//        if(min_manip_val<min_manip_th){
+//          std::cout << names[i]<<" min_manip_direc: " << min_manip_direc.transpose() << "min_manip_val " << min_manip_val << std::endl;
+//          hrp::Vector3 cur_p = fik_in->ikp[names[i]].target_link->p+ fik_in->ikp[names[i]].target_link->R * fik_in->ikp[names[i]].localPos;
+//          fik_in->ikp[names[i]].target_p0 = cur_p + min_manip_direc * 1.0 * (min_manip_th - min_manip_val);
+//        }
+//      }
+//      if(tmp_com_err.norm() < COM_IK_MAX_ERROR && min_manip_val >= min_manip_th){ break; }
+//      if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
+//
+//    }
+
+
+
+    static hrp::Vector3 target_p0_old[4];
+    static bool firstcall = true;
+//    for(int i=0;i<4;i++){
+    for(int i=0;i<1;i++){
+      hrp::dmatrix J,Jinv,Jnull;
+      fik_in->ikp[names[i]].manip->calcJacobian(J);
+      fik_in->ikp[names[i]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
+
+      hrp::dmatrix manipulability_mat = J*J.transpose();
+      double manipulability = sqrt((J*J.transpose()).determinant());
+      //    Eigen::JacobiSVD< Eigen::Matrix<float, 3, 3> > svd(mm3, Eigen::ComputeFullU | Eigen::ComputeFullV);
+      Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+      Eigen::MatrixXd::Index row,col;
+      min_manip_val = svd.singularValues().minCoeff(&row,&col);
+
+      hrp::Vector3 min_manip_direc = svd.matrixU().col(row);// unit vector
+      //      std::cout << "singular values" << std::endl << svd.singularValues() << std::endl;
+      //      std::cout << "matrix U" << std::endl << svd.matrixU() << std::endl;
+      //      std::cout << "matrix V" << std::endl << svd.matrixV() << std::endl;
+      if(min_manip_direc.dot(fik_in->ikp[names[i]].target_p0 - robot_in->rootLink()->p) < 0){ min_manip_direc *= -1; }//向きみて反転(常に外側正)
+
+
+      if(firstcall){
+        target_p0_old[i] = fik_in->ikp[names[i]].target_p0;
+      }
+
+
+      hrp::Vector3 ref_ee_vel = (fik_in->ikp[names[i]].target_p0 - target_p0_old[i]) / m_dt;
+      hrp::Vector3 ref_ee_vel_mod = ref_ee_vel;
+//      if((ref_vec.normalized()).dot(min_manip_direc)<0.001){
+
+      double penalty = (min_manip_val - min_manip_th) * 10;
+//      LIMIT_MINMAX(penalty, 0.0, 1.0);//
+      LIMIT_MINMAX(penalty, -1.0, 1.0);//
+
+      double lim_penalty = 1.0;
+      static double lim_penalty_old = 1.0;
+      double lim_penalty_tmp = 1.0;
+      double ulim_mergin[6], llim_mergin[6];
+      static double ulim_mergin_old[6], llim_mergin_old[6];
+      const double jlim_start_th = 30*D2R;
+
+
+//      for(int j=0;j<6;j++){
+//        llim_mergin[j] = fabs(robot_in->joint(j)->llimit - robot_in->joint(j)->q);
+//        ulim_mergin[j] = fabs(robot_in->joint(j)->ulimit - robot_in->joint(j)->q);
+//        if( llim_mergin[j] < jlim_start_th && (llim_mergin[j] - llim_mergin_old[j]) < 0){
+//          lim_penalty_tmp =  llim_mergin[j]/jlim_start_th;
+//          if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
+//        }
+//        if( ulim_mergin[j] < jlim_start_th && (ulim_mergin[j] - ulim_mergin_old[j]) < 0){
+//          lim_penalty_tmp =  ulim_mergin[j]/jlim_start_th;
+//          if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
+//        }
+//        llim_mergin_old[j] = llim_mergin[j];
+//        ulim_mergin_old[j] = ulim_mergin[j];
+//      }
+
+      hrp::dvector q_vel(6);
+      q_vel = Jinv.block(0,0,Jinv.cols(),3) * ref_ee_vel;
+
+      for(int j=0;j<6;j++){
+        llim_mergin[j] = fabs(robot_in->joint(j)->llimit - robot_in->joint(j)->q);
+        ulim_mergin[j] = fabs(robot_in->joint(j)->ulimit - robot_in->joint(j)->q);
+        if( llim_mergin[j] < jlim_start_th && q_vel[j] < 0){
+          if(DEBUGP)cout<<"llim"<<j<<": "<<endl;
+          lim_penalty_tmp =  llim_mergin[j]/jlim_start_th;
+          if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
+        }
+        if( ulim_mergin[j] < jlim_start_th && q_vel[j] > 0){
+          if(DEBUGP)cout<<"ulim"<<j<<": "<<endl;
+          lim_penalty_tmp =  ulim_mergin[j]/jlim_start_th;
+          if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
+        }
+      }
+//      LIMIT_MIN(lim_penalty, 0.01);
+
+
+
+//      lim_penalty = lim_penalty * 0.01 + lim_penalty_old * 0.99;
+      lim_penalty_old = lim_penalty;
+      if(DEBUGP){
+        if(lim_penalty<1.0)cout<<"lim_penalty: "<<lim_penalty<<endl;
+      }
+
+      double max_dec_vel = 1.0, max_acc_vel = 1.0;// m/s
+//      if(ref_vec.dot(min_manip_direc)>0){
+        if(1){
+        hrp::Vector3 ref_ee_vel_v = (ref_ee_vel.dot(min_manip_direc)) * min_manip_direc;
+        hrp::Vector3 ref_ee_vel_h = ref_ee_vel - ref_ee_vel_v;
+//        ref_vec_mod -= (ref_vec.dot(min_manip_direc)) * min_manip_direc;
+//        ref_vec_mod += (ref_vec.dot(min_manip_direc)) * min_manip_direc * penalty;
+
+        max_dec_vel *= penalty;
+        max_acc_vel *= penalty;
+        LIMIT_MIN(max_acc_vel, 0.5);//
+
+        if(ref_ee_vel_v.dot(min_manip_direc) > max_dec_vel){
+//        if(ref_ee_vel_v.norm() > max_vel){
+          ref_ee_vel_v = min_manip_direc * max_dec_vel;
+        }
+        if(ref_ee_vel_v.dot(-min_manip_direc) > max_acc_vel){
+          ref_ee_vel_v = ref_ee_vel_v.normalized() * max_acc_vel;
+        }
+
+//        if(min_manip_val<min_manip_th){
+//          ref_ee_vel_v -= min_manip_direc * (min_manip_th - min_manip_val) * 5;
+//        }
+
+
+//        if(min_manip_val<min_manip_th){
+//          ref_vec_mod += (ref_vec.dot(min_manip_direc)) * min_manip_direc * (min_manip_val - min_manip_th);
+//        }
+        ref_ee_vel_mod = ref_ee_vel_h + ref_ee_vel_v;//速度空間で一次フィルターしたほうがいい?
+        ref_ee_vel_mod *= lim_penalty;
+        if(DEBUGP)std::cout << names[i]<<" min_manip_direc: " << min_manip_direc.transpose() << " min_manip_val " << min_manip_val << std::endl;
+      }
+
+//      if(min_manip_val<min_manip_th && ref_vec.dot(min_manip_direc)>0){
+//        ref_vec -= (ref_vec.dot(min_manip_direc))*min_manip_direc;
+//        std::cout << names[i]<<" min_manip_direc: " << min_manip_direc.transpose() << " min_manip_val " << min_manip_val << std::endl;
+//      }
+
+
+      fik_in->ikp[names[i]].target_p0 = target_p0_old[i] + ref_ee_vel_mod * m_dt;
+      target_p0_old[i] = fik_in->ikp[names[i]].target_p0;
+//      fik_in->ikp[names[i]].target_p0 = ee_filter.passFilter(fik_in->ikp[names[i]].target_p0);
+//      cout<<"fik_in->ikp[names[i]].target_p0:"<<fik_in->ikp[names[i]].target_p0.transpose()<<endl;
+
+//      if(min_manip_val<min_manip_th){
+//        std::cout << names[i]<<" min_manip_direc: " << min_manip_direc.transpose() << " min_manip_val " << min_manip_val << std::endl;
+//        hrp::Vector3 cur_p = fik_in->ikp[names[i]].target_link->p+ fik_in->ikp[names[i]].target_link->R * fik_in->ikp[names[i]].localPos;
+//        fik_in->ikp[names[i]].target_p0 = cur_p + min_manip_direc * 1.0 * (min_manip_th - min_manip_val);
+//      }
+      firstcall = false;
+    }
+
     while( 1 ){  //COM 収束ループ＋可操作度リミット
       com_ik_loop++;
       robot_in->rootLink()->p += tmp_com_err;
@@ -529,30 +708,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
 
       const std::string names[4] = {"rleg","lleg","rarm","larm"};
       const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
-      for(int i=0;i<4;i++){
-        hrp::dmatrix J;
-        fik_in->ikp[names[i]].manip->calcJacobian(J);
-        hrp::dmatrix manipulability_mat = J*J.transpose();
-        double manipulability = sqrt((J*J.transpose()).determinant());
-        //    Eigen::JacobiSVD< Eigen::Matrix<float, 3, 3> > svd(mm3, Eigen::ComputeFullU | Eigen::ComputeFullV);
-        Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
-
-        Eigen::MatrixXd::Index row,col;
-        min_manip_val = svd.singularValues().minCoeff(&row,&col);
-
-        hrp::Vector3 min_manip_direc = svd.matrixU().col(row);
-        //      std::cout << "singular values" << std::endl << svd.singularValues() << std::endl;
-        //      std::cout << "matrix U" << std::endl << svd.matrixU() << std::endl;
-        //      std::cout << "matrix V" << std::endl << svd.matrixV() << std::endl;
-        if(min_manip_direc.dot(fik_in->ikp[names[i]].target_p0 - robot_in->rootLink()->p) > 0){ min_manip_direc *= -1; }//向きみて反転
-
-        if(min_manip_val<min_manip_th){
-          std::cout << names[i]<<" min_manip_direc: " << min_manip_direc.transpose() << "min_manip_val " << min_manip_val << std::endl;
-          hrp::Vector3 cur_p = fik_in->ikp[names[i]].target_link->p+ fik_in->ikp[names[i]].target_link->R * fik_in->ikp[names[i]].localPos;
-          fik_in->ikp[names[i]].target_p0 = cur_p + min_manip_direc * 1.0 * (min_manip_th - min_manip_val);
-        }
-      }
-      if(tmp_com_err.norm() < COM_IK_MAX_ERROR && min_manip_val >= min_manip_th){ break; }
+//      if(tmp_com_err.norm() < COM_IK_MAX_ERROR && min_manip_val >= min_manip_th){ break; }
+      if(tmp_com_err.norm() < COM_IK_MAX_ERROR ){ break; }
       if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
 
     }
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 04952aaf044..d5f177e939b 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -380,8 +380,8 @@ class HumanSynchronizer : UTIL_CONST {
       if( inside_vec_lf.dot(lf2zmp) / inside_vec_lf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ out.tgt[rf].go_contact = true; }
     }
     inline void limitEEWorkspace(HumanPose& out){
-      const double MAX_FW = 0.25;
-//      const double MAX_FW = 1000000;//manipulability test
+//      const double MAX_FW = 0.25;
+      const double MAX_FW = 1000000;//manipulability test
       const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
       if(!out.tgt[rf].is_contact && out.tgt[lf].is_contact){//右足浮遊時
         const hrp::Vector2 lf2rf_vec( out.tgt[rf].abs.p(X) - out.tgt[lf].cnt.p(X), out.tgt[rf].abs.p(Y) - out.tgt[lf].cnt.p(Y) );

From 504fd6bbcb8ff3bb3900cf4bc48fc8e9b07fae00 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 28 May 2017 17:57:05 +0900
Subject: [PATCH 091/217] imprement stop method

---
 idl/#WholeBodyMasterSlave.idl#                |   2 -
 idl/WholeBodyMasterSlaveService.idl           |   4 +-
 .../WholeBodyMasterSlave.cpp                  | 414 +++++++++---------
 .../WholeBodyMasterSlave.h                    |  11 +-
 .../WholeBodyMasterSlaveService_impl.cpp      |  14 +-
 .../WholeBodyMasterSlaveService_impl.h        |   4 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          |  77 ++--
 7 files changed, 271 insertions(+), 255 deletions(-)
 delete mode 100644 idl/#WholeBodyMasterSlave.idl#

diff --git a/idl/#WholeBodyMasterSlave.idl# b/idl/#WholeBodyMasterSlave.idl#
deleted file mode 100644
index 139597f9cb0..00000000000
--- a/idl/#WholeBodyMasterSlave.idl#
+++ /dev/null
@@ -1,2 +0,0 @@
-
-
diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
index 2f96332b02e..16e5b7d058f 100644
--- a/idl/WholeBodyMasterSlaveService.idl
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -50,7 +50,9 @@ module OpenHRP
         boolean use_manipulability_limit;
     };
     boolean startCountDownForWholeBodyMasterSlave(in double sec);
-    boolean stopHumanSync();
+    boolean startWholeBodyMasterSlave();
+    boolean pauseWholeBodyMasterSlave();
+    boolean stopWholeBodyMasterSlave();
     void setWholeBodyMasterSlaveParam(in WholeBodyMasterSlaveParam i_param);
     void getWholeBodyMasterSlaveParam(out WholeBodyMasterSlaveParam i_param);
   };
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index e4817d94b3d..2480d04c16d 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -149,14 +149,14 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     m_htlfw.data.length(6);
     coil::stringTo(optionalDataLength, prop["seq_optional_data_dim"].c_str());
     loop = 0;
-    q_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-    q_interpolator->setName(std::string(m_profile.instance_name)+" q_interpolator");
-    q_interpolator_ratio = 0;
+    transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator");
+    transition_interpolator_ratio = 0;
     double tmp_ratio = 0.0;
-    q_interpolator->clear();
-    q_interpolator->set(&tmp_ratio);
+    transition_interpolator->clear();
+    transition_interpolator->set(&tmp_ratio);
     tmp_ratio = 1.0;
-    q_interpolator->setGoal(&tmp_ratio, 3.0, true);
+    transition_interpolator->setGoal(&tmp_ratio, 3.0, true);
     // Generate FIK
     fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
     fik_rmc = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot_rmc, std::string(m_profile.instance_name), m_dt));
@@ -175,6 +175,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     invdyn_zmp_filters.setParameter(25, 1/m_dt, BiquadIIRFilterVec::Q_BUTTERWORTH);
     invdyn_zmp_filters2.setParameter(25, 1/m_dt, BiquadIIRFilterVec::Q_BUTTERWORTH);
 
+    mode = previous_mode = MODE_IDLE;
+
     return RTC::RTC_OK;
 }
 
@@ -252,12 +254,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
       processTransition();
 
-      if(hsp->isHumanSyncOn() && hsp->ht_first_call){
-        preProcessForWholeBodyMasterSlave(fik, m_robot);
-      }
-
-      if (!q_interpolator->isEmpty() && hsp->isHumanSyncOn()) { q_interpolator->get(&q_interpolator_ratio, true); }
-
       fik->current_tm = fik_rmc->current_tm = m_qRef.tm;
       fik->ikp["rleg"].is_ik_enable = fik_rmc->ikp["rleg"].is_ik_enable = true;
       fik->ikp["lleg"].is_ik_enable = fik_rmc->ikp["lleg"].is_ik_enable = true;
@@ -266,61 +262,67 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 //      fik->ikp["rarm"].is_ik_enable = fik_rmc->ikp["rarm"].is_ik_enable = false;
 //      fik->ikp["larm"].is_ik_enable = fik_rmc->ikp["larm"].is_ik_enable = false;
 
-      if (hsp->isHumanSyncOn()) {
+      if (isRunning()) {
+        if(hsp->is_initial_loop){
+          preProcessForWholeBodyMasterSlave(fik, m_robot);
+        }
+
         if(hsp->WBMSparam.is_doctor){
           processWholeBodyMasterSlave(fik, m_robot);//安全制限つきマスタ・スレーブ
         }else{
           processWholeBodyMasterSlave_Raw(fik, m_robot);//生マスタ・スレーブ
         }
-      }
 
-      if(hsp->isHumanSyncOn() && hsp->ht_first_call){//逆動力学初期化
-        idsb.setInitState(m_robot, m_dt);
-        idsb2.setInitState(m_robot, m_dt);
-      }
 
-      if (hsp->isHumanSyncOn()) {//逆動力学
+        //逆動力学初期化
+        if(hsp->is_initial_loop){
+          idsb.setInitState(m_robot, m_dt);
+          idsb2.setInitState(m_robot, m_dt);
+        }
+
+
+        //逆動力学
         calcAccelerationsForInverseDynamics(m_robot, idsb);
         hrp::Vector3 ref_zmp_invdyn;
         calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);
         ref_zmp_invdyn = invdyn_zmp_filters.passFilter(ref_zmp_invdyn);
         updateInvDynStateBuffer(idsb);
-      }
 
-      if(hsp->isHumanSyncOn()){
-        if(hsp->WBMSparam.is_doctor){//角運動量補償
+
+        //角運動量補償
+        if(hsp->WBMSparam.is_doctor){
           processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, hsp->rp_ref_out);
         }else{
           processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, raw_pose);
         }
-      }
 //      calcAccelerationsForInverseDynamics(m_robot, idsb2);
 //      calcWorldZMPFromInverseDynamics(m_robot, idsb2, ref_zmp_invdyn2);
 //      updateInvDynStateBuffer(idsb2);
 
-      if (hsp->isHumanSyncOn()){//OutPortデータセット
+
+        //OutPortデータセット
         //        ref_zmp = ref_zmp_invdyn;
         hrp::Vector3 ref_zmp = hsp->rp_ref_out.tgt[zmp].abs.p;
 //        hrp::BodyPtr m_robot_for_out = m_robot;
         hrp::BodyPtr m_robot_for_out = m_robot_rmc;
         // qRef
-        for (int i = 0; i < m_qRef.data.length(); i++ ){ m_qRef.data[i] = q_interpolator_ratio * m_robot_for_out->joint(i)->q  + (1 - q_interpolator_ratio) * m_qRef.data[i]; }
+        for (int i = 0; i < m_qRef.data.length(); i++ ){ m_qRef.data[i] = transition_interpolator_ratio * m_robot_for_out->joint(i)->q  + (1 - transition_interpolator_ratio) * m_qRef.data[i]; }
         // basePos
-        m_basePos.data.x = m_robot_for_out->rootLink()->p(0);
-        m_basePos.data.y = m_robot_for_out->rootLink()->p(1);
-        m_basePos.data.z = m_robot_for_out->rootLink()->p(2);
+        m_basePos.data.x = transition_interpolator_ratio * m_robot_for_out->rootLink()->p(0) + (1 - transition_interpolator_ratio) * m_basePos.data.x;
+        m_basePos.data.y = transition_interpolator_ratio * m_robot_for_out->rootLink()->p(1) + (1 - transition_interpolator_ratio) * m_basePos.data.y;
+        m_basePos.data.z = transition_interpolator_ratio * m_robot_for_out->rootLink()->p(2) + (1 - transition_interpolator_ratio) * m_basePos.data.z;
         m_basePos.tm = m_qRef.tm;
         // baseRpy
         hrp::Vector3 baseRpy = hrp::rpyFromRot(m_robot_for_out->rootLink()->R);
-        m_baseRpy.data.r = baseRpy(0);
-        m_baseRpy.data.p = baseRpy(1);
-        m_baseRpy.data.y = baseRpy(2);
+        m_baseRpy.data.r = transition_interpolator_ratio * baseRpy(0) + (1 - transition_interpolator_ratio) * m_baseRpy.data.r;
+        m_baseRpy.data.p = transition_interpolator_ratio * baseRpy(1) + (1 - transition_interpolator_ratio) * m_baseRpy.data.p;
+        m_baseRpy.data.y = transition_interpolator_ratio * baseRpy(2) + (1 - transition_interpolator_ratio) * m_baseRpy.data.y;
         m_baseRpy.tm = m_qRef.tm;
         // zmp
         hrp::Vector3 rel_ref_zmp = m_robot_for_out->rootLink()->R.transpose() * (ref_zmp - m_robot_for_out->rootLink()->p);
-        m_zmp.data.x = rel_ref_zmp(0);
-        m_zmp.data.y = rel_ref_zmp(1);
-        m_zmp.data.z = rel_ref_zmp(2);
+        m_zmp.data.x = transition_interpolator_ratio * rel_ref_zmp(0) + (1 - transition_interpolator_ratio) * m_zmp.data.x;
+        m_zmp.data.y = transition_interpolator_ratio * rel_ref_zmp(1) + (1 - transition_interpolator_ratio) * m_zmp.data.y;
+        m_zmp.data.z = transition_interpolator_ratio * rel_ref_zmp(2) + (1 - transition_interpolator_ratio) * m_zmp.data.z;
         m_zmp.tm = m_qRef.tm;
         // m_optionalData
         if(m_optionalData.data.length() < optionalDataLength){
@@ -330,10 +332,10 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[contact_states_index_map["rleg"]+4] = hsp->rp_ref_out.tgt[rf].is_contact;
         m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[contact_states_index_map["lleg"]+4] = hsp->rp_ref_out.tgt[lf].is_contact;
       }
-      if(DEBUGP_ONCE)std::cerr << "[" << m_profile.instance_name<< "] onExecute(" << ec_id << ")" << std::endl;
       hsp->baselinkpose.p = m_robot->rootLink()->p;
       hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
     }
+    previous_mode = mode;
     // write
     m_qOut.write();
     m_basePosOut.write();
@@ -413,13 +415,31 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
 
 void WholeBodyMasterSlave::processTransition(){
-  if(hsp->isHumanSyncOn()){ hsp->ht_first_call = false;}
-  if(hsp->startCountdownForHumanSync){
-    std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
-    hsp->updateCountDown();
-  }
-  if(hsp->isHumanSyncOn() && hsp->ht_first_call){
-    std::cerr << "\n[" << m_profile.instance_name << "] Start HumanSync"<< std::endl;
+//  cout<<"hsp->mode"<<hsp->mode<<endl;
+  switch(mode){
+    case MODE_COUNTDOWN:
+      std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
+//      hsp->updateCountDown();
+      break;
+
+    case MODE_SYNC_TO_WBMS:
+//      if(previous_mode == MODE_COUNTDOWN){ double tmp = 1.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
+      if(previous_mode == MODE_IDLE){ double tmp = 1.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
+      if (!transition_interpolator->isEmpty() ){
+        transition_interpolator->get(&transition_interpolator_ratio, true);
+      }else{
+        mode = MODE_WBMS;
+      }
+      break;
+
+    case MODE_SYNC_TO_IDLE:
+      if(previous_mode == MODE_WBMS){ double tmp = 0.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
+      if (!transition_interpolator->isEmpty()) {
+        transition_interpolator->get(&transition_interpolator_ratio, true);
+      }else{
+        mode = MODE_IDLE;
+      }
+      break;
   }
 }
 
@@ -492,115 +512,53 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::
 }
 
 
-void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix){
-
-  int com_ik_loop=0;
-  const int COM_IK_MAX_LOOP = 10;
-  const double COM_IK_MAX_ERROR = 1e-5;//1e-4だと乱れる
-//  robot_in->rootLink()->p = 0.01*com_ref.p + 0.99*robot_in->rootLink()->p;//ベースリンク位置が迷走するのを防ぐ？
-  robot_in->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
+void WholeBodyMasterSlave::calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in){
   const std::string names[4] = {"rleg","lleg","rarm","larm"};
-  const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
-  for(int i=0;i<4;i++){
-    if(fik_in->ikp.count(names[i])){
-      fik_in->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);
-      fik_in->ikp[names[i]].target_p0 = refs[i]->p;
-    }
-  }
-  fik_in->storeCurrentParameters();
-//  fik_in->setReferenceJointAngles();//これ入れると腕ブワーなる
-  if(robot_in->link("HEAD_JOINT0") != NULL)robot_in->joint(15)->q = head_ref.rpy(y);
-  if(robot_in->link("HEAD_JOINT1") != NULL)robot_in->joint(16)->q = head_ref.rpy(p);
-  hrp::Vector3 tmp_com_err = hrp::Vector3::Zero();
+  double min_manip_val = 1e9;
+  const double min_manip_th = 0.1;
 
-
-
-  if(hsp->WBMSparam.use_manipulability_limit){
-    double min_manip_val = 1e9;
-    const double min_manip_th = 0.1;
-//    while( 1 ){  //COM 収束ループ＋可操作度リミット
-//      com_ik_loop++;
-//      robot_in->rootLink()->p += tmp_com_err;
-//      robot_in->calcForwardKinematics();
-//      for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_in->ikp.begin(); it != fik_in->ikp.end(); it++ ) {
-//          if (it->second.is_ik_enable) fik_in->solveLimbIK (it->second, it->first, fik_in->ratio_for_vel, false);
-//      }
-//      tmp_com_err = com_ref.p - robot_in->calcCM();
-//
-//      const std::string names[4] = {"rleg","lleg","rarm","larm"};
-//      const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
-//      for(int i=0;i<4;i++){
-//        hrp::dmatrix J;
-//        fik_in->ikp[names[i]].manip->calcJacobian(J);
-//        hrp::dmatrix manipulability_mat = J*J.transpose();
-//        double manipulability = sqrt((J*J.transpose()).determinant());
-//        //    Eigen::JacobiSVD< Eigen::Matrix<float, 3, 3> > svd(mm3, Eigen::ComputeFullU | Eigen::ComputeFullV);
-//        Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
-//
-//        Eigen::MatrixXd::Index row,col;
-//        min_manip_val = svd.singularValues().minCoeff(&row,&col);
-//
-//        hrp::Vector3 min_manip_direc = svd.matrixU().col(row);
-//        //      std::cout << "singular values" << std::endl << svd.singularValues() << std::endl;
-//        //      std::cout << "matrix U" << std::endl << svd.matrixU() << std::endl;
-//        //      std::cout << "matrix V" << std::endl << svd.matrixV() << std::endl;
-//        if(min_manip_direc.dot(fik_in->ikp[names[i]].target_p0 - robot_in->rootLink()->p) > 0){ min_manip_direc *= -1; }//向きみて反転
-//
-//        if(min_manip_val<min_manip_th){
-//          std::cout << names[i]<<" min_manip_direc: " << min_manip_direc.transpose() << "min_manip_val " << min_manip_val << std::endl;
-//          hrp::Vector3 cur_p = fik_in->ikp[names[i]].target_link->p+ fik_in->ikp[names[i]].target_link->R * fik_in->ikp[names[i]].localPos;
-//          fik_in->ikp[names[i]].target_p0 = cur_p + min_manip_direc * 1.0 * (min_manip_th - min_manip_val);
-//        }
-//      }
-//      if(tmp_com_err.norm() < COM_IK_MAX_ERROR && min_manip_val >= min_manip_th){ break; }
-//      if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
-//
-//    }
-
-
-
-    static hrp::Vector3 target_p0_old[4];
-    static bool firstcall = true;
+  static hrp::Vector3 target_p0_old[4];
+  static bool firstcall = true;
 //    for(int i=0;i<4;i++){
-    for(int i=0;i<1;i++){
-      hrp::dmatrix J,Jinv,Jnull;
-      fik_in->ikp[names[i]].manip->calcJacobian(J);
-      fik_in->ikp[names[i]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
+  for(int i=0;i<1;i++){
+    hrp::dmatrix J,Jinv,Jnull;
+    fik_in->ikp[names[i]].manip->calcJacobian(J);
+    fik_in->ikp[names[i]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
 
-      hrp::dmatrix manipulability_mat = J*J.transpose();
-      double manipulability = sqrt((J*J.transpose()).determinant());
-      //    Eigen::JacobiSVD< Eigen::Matrix<float, 3, 3> > svd(mm3, Eigen::ComputeFullU | Eigen::ComputeFullV);
-      Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
+    hrp::dmatrix manipulability_mat = J*J.transpose();
+    double manipulability = sqrt((J*J.transpose()).determinant());
+    //    Eigen::JacobiSVD< Eigen::Matrix<float, 3, 3> > svd(mm3, Eigen::ComputeFullU | Eigen::ComputeFullV);
+    Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
 
-      Eigen::MatrixXd::Index row,col;
-      min_manip_val = svd.singularValues().minCoeff(&row,&col);
+    Eigen::MatrixXd::Index row,col;
+    min_manip_val = svd.singularValues().minCoeff(&row,&col);
 
-      hrp::Vector3 min_manip_direc = svd.matrixU().col(row);// unit vector
-      //      std::cout << "singular values" << std::endl << svd.singularValues() << std::endl;
-      //      std::cout << "matrix U" << std::endl << svd.matrixU() << std::endl;
-      //      std::cout << "matrix V" << std::endl << svd.matrixV() << std::endl;
-      if(min_manip_direc.dot(fik_in->ikp[names[i]].target_p0 - robot_in->rootLink()->p) < 0){ min_manip_direc *= -1; }//向きみて反転(常に外側正)
+    hrp::Vector3 min_manip_direc = svd.matrixU().col(row);// unit vector
+    //      std::cout << "singular values" << std::endl << svd.singularValues() << std::endl;
+    //      std::cout << "matrix U" << std::endl << svd.matrixU() << std::endl;
+    //      std::cout << "matrix V" << std::endl << svd.matrixV() << std::endl;
+    if(min_manip_direc.dot(fik_in->ikp[names[i]].target_p0 - robot_in->rootLink()->p) < 0){ min_manip_direc *= -1; }//向きみて反転(常に外側正)
 
 
-      if(firstcall){
-        target_p0_old[i] = fik_in->ikp[names[i]].target_p0;
-      }
+    if(firstcall){
+      target_p0_old[i] = fik_in->ikp[names[i]].target_p0;
+    }
 
 
-      hrp::Vector3 ref_ee_vel = (fik_in->ikp[names[i]].target_p0 - target_p0_old[i]) / m_dt;
-      hrp::Vector3 ref_ee_vel_mod = ref_ee_vel;
+    hrp::Vector3 ref_ee_vel = (fik_in->ikp[names[i]].target_p0 - target_p0_old[i]) / m_dt;
+    hrp::Vector3 ref_ee_vel_mod = ref_ee_vel;
 //      if((ref_vec.normalized()).dot(min_manip_direc)<0.001){
 
-      double penalty = (min_manip_val - min_manip_th) * 10;
+    double penalty = (min_manip_val - min_manip_th) * 10;
 //      LIMIT_MINMAX(penalty, 0.0, 1.0);//
-      LIMIT_MINMAX(penalty, -1.0, 1.0);//
+    LIMIT_MINMAX(penalty, -1.0, 1.0);//
 
-      double lim_penalty = 1.0;
-      static double lim_penalty_old = 1.0;
-      double lim_penalty_tmp = 1.0;
-      double ulim_mergin[6], llim_mergin[6];
-      static double ulim_mergin_old[6], llim_mergin_old[6];
-      const double jlim_start_th = 30*D2R;
+    double lim_penalty = 1.0;
+    static double lim_penalty_old = 1.0;
+    double lim_penalty_tmp = 1.0;
+    double ulim_mergin[6], llim_mergin[6];
+    static double ulim_mergin_old[6], llim_mergin_old[6];
+    const double jlim_start_th = 30*D2R;
 
 
 //      for(int j=0;j<6;j++){
@@ -618,52 +576,53 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
 //        ulim_mergin_old[j] = ulim_mergin[j];
 //      }
 
-      hrp::dvector q_vel(6);
-      q_vel = Jinv.block(0,0,Jinv.cols(),3) * ref_ee_vel;
+    hrp::dvector q_vel(6);
+    q_vel = Jinv.block(0,0,Jinv.cols(),3) * ref_ee_vel;
 
-      for(int j=0;j<6;j++){
-        llim_mergin[j] = fabs(robot_in->joint(j)->llimit - robot_in->joint(j)->q);
-        ulim_mergin[j] = fabs(robot_in->joint(j)->ulimit - robot_in->joint(j)->q);
-        if( llim_mergin[j] < jlim_start_th && q_vel[j] < 0){
-          if(DEBUGP)cout<<"llim"<<j<<": "<<endl;
-          lim_penalty_tmp =  llim_mergin[j]/jlim_start_th;
-          if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
-        }
-        if( ulim_mergin[j] < jlim_start_th && q_vel[j] > 0){
-          if(DEBUGP)cout<<"ulim"<<j<<": "<<endl;
-          lim_penalty_tmp =  ulim_mergin[j]/jlim_start_th;
-          if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
-        }
+    for(int j=0;j<6;j++){
+      llim_mergin[j] = fabs(robot_in->joint(j)->llimit - robot_in->joint(j)->q);
+      ulim_mergin[j] = fabs(robot_in->joint(j)->ulimit - robot_in->joint(j)->q);
+      if( llim_mergin[j] < jlim_start_th && q_vel[j] < 0){
+        if(DEBUGP)cout<<"llim"<<j<<": "<<endl;
+        lim_penalty_tmp =  llim_mergin[j]/jlim_start_th;
+        if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
+      }
+      if( ulim_mergin[j] < jlim_start_th && q_vel[j] > 0){
+        if(DEBUGP)cout<<"ulim"<<j<<": "<<endl;
+        lim_penalty_tmp =  ulim_mergin[j]/jlim_start_th;
+        if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
       }
+    }
 //      LIMIT_MIN(lim_penalty, 0.01);
 
 
 
 //      lim_penalty = lim_penalty * 0.01 + lim_penalty_old * 0.99;
-      lim_penalty_old = lim_penalty;
-      if(DEBUGP){
-        if(lim_penalty<1.0)cout<<"lim_penalty: "<<lim_penalty<<endl;
-      }
+    lim_penalty_old = lim_penalty;
+    if(DEBUGP){
+      if(lim_penalty<1.0)cout<<"lim_penalty: "<<lim_penalty<<endl;
+    }
 
-      double max_dec_vel = 1.0, max_acc_vel = 1.0;// m/s
+    double max_dec_vel = 1.0, max_acc_vel = 1.0;// m/s
 //      if(ref_vec.dot(min_manip_direc)>0){
-        if(1){
-        hrp::Vector3 ref_ee_vel_v = (ref_ee_vel.dot(min_manip_direc)) * min_manip_direc;
-        hrp::Vector3 ref_ee_vel_h = ref_ee_vel - ref_ee_vel_v;
+      if(1){
+      hrp::Vector3 ref_ee_vel_v = (ref_ee_vel.dot(min_manip_direc)) * min_manip_direc;
+      hrp::Vector3 ref_ee_vel_h = ref_ee_vel - ref_ee_vel_v;
 //        ref_vec_mod -= (ref_vec.dot(min_manip_direc)) * min_manip_direc;
 //        ref_vec_mod += (ref_vec.dot(min_manip_direc)) * min_manip_direc * penalty;
 
-        max_dec_vel *= penalty;
-        max_acc_vel *= penalty;
-        LIMIT_MIN(max_acc_vel, 0.5);//
+      max_dec_vel *= penalty;
+      max_acc_vel *= penalty;
+      LIMIT_MIN(max_acc_vel, 0.0);//
+      max_acc_vel += 0.01;
 
-        if(ref_ee_vel_v.dot(min_manip_direc) > max_dec_vel){
+      if(ref_ee_vel_v.dot(min_manip_direc) > max_dec_vel){
 //        if(ref_ee_vel_v.norm() > max_vel){
-          ref_ee_vel_v = min_manip_direc * max_dec_vel;
-        }
-        if(ref_ee_vel_v.dot(-min_manip_direc) > max_acc_vel){
-          ref_ee_vel_v = ref_ee_vel_v.normalized() * max_acc_vel;
-        }
+        ref_ee_vel_v = min_manip_direc * max_dec_vel;
+      }
+      if(ref_ee_vel_v.dot(-min_manip_direc) > max_acc_vel){
+        ref_ee_vel_v = ref_ee_vel_v.normalized() * max_acc_vel;
+      }
 
 //        if(min_manip_val<min_manip_th){
 //          ref_ee_vel_v -= min_manip_direc * (min_manip_th - min_manip_val) * 5;
@@ -673,10 +632,10 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
 //        if(min_manip_val<min_manip_th){
 //          ref_vec_mod += (ref_vec.dot(min_manip_direc)) * min_manip_direc * (min_manip_val - min_manip_th);
 //        }
-        ref_ee_vel_mod = ref_ee_vel_h + ref_ee_vel_v;//速度空間で一次フィルターしたほうがいい?
-        ref_ee_vel_mod *= lim_penalty;
-        if(DEBUGP)std::cout << names[i]<<" min_manip_direc: " << min_manip_direc.transpose() << " min_manip_val " << min_manip_val << std::endl;
-      }
+      ref_ee_vel_mod = ref_ee_vel_h + ref_ee_vel_v;//速度空間で一次フィルターしたほうがいい?
+      ref_ee_vel_mod *= lim_penalty;
+      if(DEBUGP)std::cout << names[i]<<" min_manip_direc: " << min_manip_direc.transpose() << " min_manip_val " << min_manip_val << std::endl;
+    }
 
 //      if(min_manip_val<min_manip_th && ref_vec.dot(min_manip_direc)>0){
 //        ref_vec -= (ref_vec.dot(min_manip_direc))*min_manip_direc;
@@ -684,8 +643,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
 //      }
 
 
-      fik_in->ikp[names[i]].target_p0 = target_p0_old[i] + ref_ee_vel_mod * m_dt;
-      target_p0_old[i] = fik_in->ikp[names[i]].target_p0;
+    fik_in->ikp[names[i]].target_p0 = target_p0_old[i] + ref_ee_vel_mod * m_dt;
+    target_p0_old[i] = fik_in->ikp[names[i]].target_p0;
 //      fik_in->ikp[names[i]].target_p0 = ee_filter.passFilter(fik_in->ikp[names[i]].target_p0);
 //      cout<<"fik_in->ikp[names[i]].target_p0:"<<fik_in->ikp[names[i]].target_p0.transpose()<<endl;
 
@@ -694,37 +653,45 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
 //        hrp::Vector3 cur_p = fik_in->ikp[names[i]].target_link->p+ fik_in->ikp[names[i]].target_link->R * fik_in->ikp[names[i]].localPos;
 //        fik_in->ikp[names[i]].target_p0 = cur_p + min_manip_direc * 1.0 * (min_manip_th - min_manip_val);
 //      }
-      firstcall = false;
-    }
+    firstcall = false;
+  }
+}
 
-    while( 1 ){  //COM 収束ループ＋可操作度リミット
-      com_ik_loop++;
-      robot_in->rootLink()->p += tmp_com_err;
-      robot_in->calcForwardKinematics();
-      for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_in->ikp.begin(); it != fik_in->ikp.end(); it++ ) {
-          if (it->second.is_ik_enable) fik_in->solveLimbIK (it->second, it->first, fik_in->ratio_for_vel, false);
-      }
-      tmp_com_err = com_ref.p - robot_in->calcCM();
 
-      const std::string names[4] = {"rleg","lleg","rarm","larm"};
-      const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
-//      if(tmp_com_err.norm() < COM_IK_MAX_ERROR && min_manip_val >= min_manip_th){ break; }
-      if(tmp_com_err.norm() < COM_IK_MAX_ERROR ){ break; }
-      if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
+void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix){
 
+  int com_ik_loop=0;
+  const int COM_IK_MAX_LOOP = 10;
+  const double COM_IK_MAX_ERROR = 1e-5;//1e-4だと乱れる
+//  robot_in->rootLink()->p = 0.01*com_ref.p + 0.99*robot_in->rootLink()->p;//ベースリンク位置が迷走するのを防ぐ？
+  robot_in->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
+  const std::string names[4] = {"rleg","lleg","rarm","larm"};
+  const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
+  for(int i=0;i<4;i++){
+    if(fik_in->ikp.count(names[i])){
+      fik_in->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);
+      fik_in->ikp[names[i]].target_p0 = refs[i]->p;
     }
-  }else{
-    while( 1 ){  //COM 収束ループ
-      com_ik_loop++;
-      robot_in->rootLink()->p += tmp_com_err;
-      robot_in->calcForwardKinematics();
-      for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_in->ikp.begin(); it != fik_in->ikp.end(); it++ ) {
-          if (it->second.is_ik_enable) fik_in->solveLimbIK (it->second, it->first, fik_in->ratio_for_vel, false);
-      }
-      tmp_com_err = com_ref.p - robot_in->calcCM();
-      if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
-      if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
+  }
+  fik_in->storeCurrentParameters();
+//  fik_in->setReferenceJointAngles();//これ入れると腕ブワーなる
+  if(robot_in->link("HEAD_JOINT0") != NULL)robot_in->joint(15)->q = head_ref.rpy(y);
+  if(robot_in->link("HEAD_JOINT1") != NULL)robot_in->joint(16)->q = head_ref.rpy(p);
+  hrp::Vector3 tmp_com_err = hrp::Vector3::Zero();
+
+  if(hsp->WBMSparam.use_manipulability_limit){
+    calcManipulabilityJointLimit(fik_in, robot_in);
+  }
+  while( 1 ){  //COM 収束ループ
+    com_ik_loop++;
+    robot_in->rootLink()->p += tmp_com_err;
+    robot_in->calcForwardKinematics();
+    for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_in->ikp.begin(); it != fik_in->ikp.end(); it++ ) {
+        if (it->second.is_ik_enable) fik_in->solveLimbIK (it->second, it->first, fik_in->ratio_for_vel, false);
     }
+    tmp_com_err = com_ref.p - robot_in->calcCM();
+    if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
+    if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
   }
 
   if(com_ik_loop != 1 && DEBUGP)cout<<"com_ik_loop:"<<com_ik_loop<<" @ "<<debug_prefix<<endl;
@@ -754,22 +721,41 @@ void WholeBodyMasterSlave::processMomentumCompensation(fikPtr& fik_in, hrp::Body
 
 
 bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double sec){
-  if(sec >= 0.0 && sec <= 30.0){
-    std::cerr << "[" << m_profile.instance_name << "] start Synchronization after "<<sec<<" [s]" << std::endl;
-    hsp->countdown_sec = sec;
-    hsp->startCountdownForHumanSync = true;
-    while (!hsp->isHumanSyncOn()){ usleep(1000); }
-    return true;
-  }else{
-    std::cerr << "[" << m_profile.instance_name << "] Count Down Time must be 0 < T < 30 [s]"<< std::endl;
-    return false;
-  }
+//  if(sec >= 0.0 && sec <= 30.0){
+//    std::cerr << "[" << m_profile.instance_name << "] start Synchronization after "<<sec<<" [s]" << std::endl;
+//    hsp->countdown_sec = sec;
+////    hsp->startCountdownForHumanSync = true;
+//    mode = MODE_COUNTDOWN;
+//    while (mode == MODE_COUNTDOWN){ usleep(1000); }
+//    std::cerr << "[" << m_profile.instance_name << "] start Synchronization" << std::endl;
+//    return true;
+//  }else{
+//    std::cerr << "[" << m_profile.instance_name << "] Count Down Time must be 0 < T < 30 [s]"<< std::endl;
+//    return false;
+//  }
+}
+
+
+bool WholeBodyMasterSlave::startWholeBodyMasterSlave(){
+  std::cerr << "[" << m_profile.instance_name << "] startWholeBodyMasterSlave" << std::endl;
+  mode = MODE_SYNC_TO_WBMS;
+  while(!transition_interpolator->isEmpty()){ usleep(1000); }
+  return true;
+}
+
+
+bool WholeBodyMasterSlave::pauseWholeBodyMasterSlave(){
+  std::cerr << "[" << m_profile.instance_name << "] pauseWholeBodyMasterSlave" << std::endl;
+  mode = MODE_PAUSE;
+  return true;
 }
 
 
-bool WholeBodyMasterSlave::stopHumanSync(){
-	std::cerr << "[" << m_profile.instance_name << "] not implemented..." << std::endl;
-	return true;
+bool WholeBodyMasterSlave::stopWholeBodyMasterSlave(){
+  std::cerr << "[" << m_profile.instance_name << "] stopWholeBodyMasterSlave" << std::endl;
+  mode = MODE_SYNC_TO_IDLE;
+  while(!transition_interpolator->isEmpty()){ usleep(1000); }
+  return true;
 }
 
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index eb791e9ef23..bb2534ff65f 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -46,7 +46,9 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
     virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
     virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
     bool startCountDownForWholeBodyMasterSlave(const double sec);
-    bool stopHumanSync();
+    bool startWholeBodyMasterSlave();
+    bool pauseWholeBodyMasterSlave();
+    bool stopWholeBodyMasterSlave();
     bool setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
     bool getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
 
@@ -148,8 +150,8 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
   std::vector< std::pair<fikPtr, hrp::BodyPtr> > ik_robot_list;
   static const enum ik_robot_usage{ basic, rmc } iru;
 
-  double q_interpolator_ratio;
-  interpolator *q_interpolator;
+  double transition_interpolator_ratio;
+  interpolator *transition_interpolator;
 
   int optionalDataLength;
   unsigned int m_debugLevel;
@@ -166,8 +168,11 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
 
   boost::shared_ptr<HumanSynchronizer> hsp;
 
+  enum { MODE_IDLE, MODE_COUNTDOWN, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE} mode, previous_mode;
+  inline bool isRunning(){ return (mode==MODE_SYNC_TO_WBMS) || (mode==MODE_WBMS) || (mode==MODE_PAUSE) || (mode==MODE_SYNC_TO_IDLE) ;}
 
   void setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in);
+  void calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in);
   void solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix="");
   void processTransition();
   void preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in);
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp
index 2f29a7fe820..35ed2ca6ca3 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp
@@ -19,9 +19,19 @@ CORBA::Boolean WholeBodyMasterSlaveService_impl::startCountDownForWholeBodyMaste
     return m_wholebodymasterslave->startCountDownForWholeBodyMasterSlave(sec);
 };
 
-CORBA::Boolean WholeBodyMasterSlaveService_impl::stopHumanSync()
+CORBA::Boolean WholeBodyMasterSlaveService_impl::startWholeBodyMasterSlave()
 {
-    return m_wholebodymasterslave->stopHumanSync();
+    return m_wholebodymasterslave->startWholeBodyMasterSlave();
+};
+
+CORBA::Boolean WholeBodyMasterSlaveService_impl::pauseWholeBodyMasterSlave()
+{
+    return m_wholebodymasterslave->pauseWholeBodyMasterSlave();
+};
+
+CORBA::Boolean WholeBodyMasterSlaveService_impl::stopWholeBodyMasterSlave()
+{
+    return m_wholebodymasterslave->stopWholeBodyMasterSlave();
 };
 
 void WholeBodyMasterSlaveService_impl::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param)
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h
index b5e12e74310..8f2041c45d9 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h
@@ -16,7 +16,9 @@ class WholeBodyMasterSlaveService_impl
   WholeBodyMasterSlaveService_impl();
   virtual ~WholeBodyMasterSlaveService_impl();
   CORBA::Boolean startCountDownForWholeBodyMasterSlave(const CORBA::Double sec);
-  CORBA::Boolean stopHumanSync();
+  CORBA::Boolean startWholeBodyMasterSlave();
+  CORBA::Boolean pauseWholeBodyMasterSlave();
+  CORBA::Boolean stopWholeBodyMasterSlave();
   void setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
   void getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam_out i_param);
   //
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index d5f177e939b..42c4bff3ecb 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -46,6 +46,7 @@ class UTIL_CONST{
     enum { r, p, y, rpy };
     enum { fx, fy, fz, tx, ty, tz, ft_xyz };
     enum { MIN, MAX, MINMAX };
+//    enum { MODE_IDLE, MODE_COUNTDOWN, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE};
     double G, D2R, INFMIN,INFMAX;
     UTIL_CONST() :
       G(9.80665),
@@ -131,7 +132,6 @@ class RobotConfig : UTIL_CONST {
 
 class HumanSynchronizer : UTIL_CONST {
   private:
-    bool HumanSyncOn;
     double CNT_F_TH, h2r_ratio, tgt_h2r_ratio, HZ, DT;
     struct timeval t_calc_start, t_calc_end;
     BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters, com_in_filter;
@@ -142,12 +142,11 @@ class HumanSynchronizer : UTIL_CONST {
     std::vector<cv::Point2f> points,cvhull;
     RobotConfig rc;
     HumanPose rp_ref_out_old, hp_swap_checked;
-  public:
-    bool startCountdownForHumanSync, ht_first_call;//, is_ee_contact[FH][LR], go_ee_contact[FH][LR];
     unsigned int loop;
+  public:
     double countdown_sec;
     double H_cur;
-    HumanPose hp_wld_raw, hp_plot, rp_ref_out;
+    HumanPose hp_wld_raw, hp_plot, rp_ref_out, rp_ref_vel_old;
     FILE *sr_log, *cz_log, *id_log;
     WBMSPose3D baselinkpose;
     WBMSPose3D ee_contact_pose[4];
@@ -170,15 +169,19 @@ class HumanSynchronizer : UTIL_CONST {
       bool use_manipulability_limit;
     };
     struct WBMSparameters WBMSparam;
+//    enum { MODE_IDLE, MODE_COUNTDOWN, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE} mode, previous_mode;
+//    int mode, previous_mode;
+//    inline bool isRunning(){ return (mode==MODE_SYNC_TO_WBMS) || (mode==MODE_WBMS) || (mode==MODE_PAUSE) || (mode==MODE_SYNC_TO_IDLE) ;}
+    bool is_initial_loop;
 
     HumanSynchronizer(const double& dt){
       tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
       DT = dt;
       HZ = (int)(1.0/DT);
       CNT_F_TH = 20.0;
-      HumanSyncOn = false;
-      ht_first_call = true;
-      startCountdownForHumanSync = false;
+//      HumanSyncOn = false;
+//      ht_first_call = true;
+//      startCountdownForHumanSync = false;
       countdown_sec = 5.0;
       loop = 0;
       //////////  hrp::Vector は初期化必要  ///////////
@@ -193,8 +196,8 @@ class HumanSynchronizer : UTIL_CONST {
       for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//四肢拘束点用(Rotation)
       tgt_pos_filters[com].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//重心pos用
       tgt_rot_filters[com].setParameter(0.6, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//重心rot用
-      tgt_pos_filters[rf].setParameter(hrp::Vector3(1.0,1.0,1.2), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//右足pos用
-      tgt_pos_filters[lf].setParameter(hrp::Vector3(1.0,1.0,1.2), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//左足pos用
+      tgt_pos_filters[rf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//右足pos用
+      tgt_pos_filters[lf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//左足pos用
       calcacc_v_filters.setParameter(5, HZ, BiquadIIRFilterVec::Q_BUTTERWORTH);//加速度計算用
       acc4zmp_v_filters.setParameter(5, HZ, BiquadIIRFilterVec::Q_BUTTERWORTH);//ZMP生成用ほぼこの値でいい
       com_in_filter.setParameter(1, HZ);
@@ -223,6 +226,7 @@ class HumanSynchronizer : UTIL_CONST {
       WBMSparam.use_manipulability_limit = false;
       rp_ref_out_old.clear();
       rp_ref_out.clear();
+      rp_ref_vel_old.clear();
       rflf_points.reserve(8);//あらかじめreserveして高速化
       points.reserve(8);
       hull_com.reserve(6);
@@ -234,6 +238,8 @@ class HumanSynchronizer : UTIL_CONST {
         cz_log = fopen("/home/ishiguro/HumanSync_com_zmp.log","w+");
         id_log = fopen("/home/ishiguro/HumanSync_invdyn.log","w+");
       }
+//      previous_mode = mode = MODE_IDLE;
+      is_initial_loop = true;
       cout<<"HumanSynchronizer constructed"<<endl;
     }
     ~HumanSynchronizer(){
@@ -243,7 +249,7 @@ class HumanSynchronizer : UTIL_CONST {
       cout<<"HumanSynchronizer destructed"<<endl;
     }
     inline double getRemainingCountDown() const { return countdown_sec; }
-    inline bool isHumanSyncOn() const { return HumanSyncOn; }
+//    inline bool isHumanSyncOn() const { return HumanSyncOn; }
     inline double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6; }
     inline void setTargetHumanToRobotRatio(const double tgt_h2r_r_in){ tgt_h2r_ratio = tgt_h2r_r_in; }
     inline void setCurrentInputAsOffset(HumanPose& tgt){
@@ -253,29 +259,36 @@ class HumanSynchronizer : UTIL_CONST {
         tgt.tgt[i].offs.rpy =  tgt.tgt[i].abs.rpy;
       }
     }
-    inline void updateCountDown(){
-      if(countdown_sec>0.0){
-        countdown_sec -= DT;
-      }else if(countdown_sec <= 0){
-        HumanSyncOn = true;
-        startCountdownForHumanSync = false;
-      }
-    }
+//    inline void updateCountDown(){
+//      if(countdown_sec>0.0){
+//        countdown_sec -= DT;
+//      }else if(countdown_sec <= 0){
+////        HumanSyncOn = true;
+////        startCountdownForHumanSync = false;
+////        mode = MODE_SYNC_TO_WBMS;
+//        cout<<"updateCountDown Finish"<<endl;
+//        is_initial_loop = true;
+//      }
+//    }
     inline void update(){//////////  メインループ  ////////////
       gettimeofday(&t_calc_start, NULL);
+      if(loop!=0){is_initial_loop = false;}
       updateParams                        ();
       autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);//入力の左右反転を常にチェック(＝手足の交差は不可能)
       convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
       hp_plot = rp_ref_out;
-      if(loop==0){
+      if(is_initial_loop){
         for(int i=0, l[LR]={rf,lf}; i<LR; i++){
           rp_ref_out.tgt[l[i]].is_contact = true;
 //          rp_ref_out.tgt[l[i]].go_contact = true;
           rp_ref_out.tgt[l[i]].cnt = rp_ref_out.tgt[l[i]].abs;
         }
         rp_ref_out_old = rp_ref_out;
+        rp_ref_vel_old = rp_ref_out;
       }
-      if(isHumanSyncOn() && WBMSparam.set_com_height_fix)rp_ref_out.tgt[com].abs.p(Z) = rp_ref_out.tgt[com].offs.p(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
+      applyVelLimit                       (rp_ref_vel_old, rp_ref_out);
+      rp_ref_vel_old = rp_ref_out;
+      if(WBMSparam.set_com_height_fix)rp_ref_out.tgt[com].abs.p(Z) = rp_ref_out.tgt[com].offs.p(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
       judgeFootLandOnCommandByFootForce   (hp_wld_raw);//人体足裏反力から各足の接地指令を生成
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
       limitEEWorkspace                    (rp_ref_out);
@@ -305,6 +318,7 @@ class HumanSynchronizer : UTIL_CONST {
       H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[rf].abs.p(Z));
       com_vel_old = (rp_ref_out.tgt[com].abs.p - rp_ref_out_old.tgt[com].abs.p)/DT;
       rp_ref_out_old = rp_ref_out;
+//      previous_mode = mode;
       loop++;
       gettimeofday(&t_calc_end, NULL);
     }
@@ -361,7 +375,7 @@ class HumanSynchronizer : UTIL_CONST {
       }
     }
     inline void applyLPFilter(HumanPose& tgt){
-      if(loop==0){
+      if(is_initial_loop){
         for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
           tgt_pos_filters[l[i]].init(tgt.tgt[l[i]].abs.p);
           tgt_rot_filters[l[i]].init(tgt.tgt[l[i]].abs.rpy);
@@ -426,7 +440,7 @@ class HumanSynchronizer : UTIL_CONST {
         for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( out.tgt[l[i]].abs.rpy(j), rc.ee_rot_limit[l[i]][MIN](j) + out.tgt[com].abs.rpy(j), rc.ee_rot_limit[l[i]][MAX](j) + out.tgt[com].abs.rpy(j) ); }
       }
       if(!WBMSparam.use_head)out.tgt[head].abs.rpy = hrp::Vector3::Zero();
-      if(!isHumanSyncOn())out.tgt[head].abs.rpy = hrp::Vector3::Zero();
+//      if(mode!=MODE_WBMS)out.tgt[head].abs.rpy = hrp::Vector3::Zero();//頭は動かしてない時は0
     }
     inline void setFootContactPoseByGoContact(HumanPose& out){
       for(int i=0, l[LR]={rf,lf}; i<LR; i++){
@@ -499,7 +513,7 @@ class HumanSynchronizer : UTIL_CONST {
     }
     inline bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans_old, hrp::Vector3& com_ans){
       com_ans = com_in;
-      if (loop==0)com_ans_old = com_in;
+      if (is_initial_loop)com_ans_old = com_in;
       hrp::Vector3 com_vel = (com_in - com_ans_old)/DT;
       hull_dcp.clear();
       hull_acp.clear();
@@ -692,15 +706,14 @@ class HumanSynchronizer : UTIL_CONST {
       com_oldold = com_old;
       com_old = comin;
     }
-    //void applyVelLimit(const HumanPose& in, HumanPose& out_old, HumanPose& out){
-    //  const int ns[] = {com,rf,lf,rh,lh,head}, num_ns = sizeof(ns)/sizeof(int);
-    //  for(int i=0;i<num_ns;i++){
-    //    hrp::Vector3 diff = in.tgt[i].abs.p  - out_old.tgt[i].abs.p;
-    //    for(int j=0;j<3;j++)LIMIT_MINMAX( diff(j), -MAXVEL*DT, MAXVEL*DT);
-    //    out.tgt[i].abs.p = out_old.tgt[i].abs.p + diff;
-    //  }
-    //  out_old = out;
-    //}
+    inline void applyVelLimit(const HumanPose& out_old, HumanPose& out){
+      const double EE_MAX_VEL = 2.0;// m/s
+      for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
+        hrp::Vector3 diff = out.tgt[i].abs.p  - out_old.tgt[i].abs.p;
+        for(int j=0;j<3;j++)LIMIT_MINMAX( diff(j), -EE_MAX_VEL*DT, EE_MAX_VEL*DT);
+        out.tgt[i].abs.p = out_old.tgt[i].abs.p + diff;
+      }
+    }
 };
 
 #endif // HUMANMASTERSLAVE_H

From 2b445bea009fa00d8f61b3c70c180b73819ba6fa Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 28 May 2017 18:27:07 +0900
Subject: [PATCH 092/217] complete start stop pause resume methods

---
 idl/WholeBodyMasterSlaveService.idl           |  3 +-
 .../WholeBodyMasterSlave.cpp                  | 47 ++++++++++---------
 .../WholeBodyMasterSlave.h                    |  5 +-
 .../WholeBodyMasterSlaveService_impl.cpp      |  9 +++-
 .../WholeBodyMasterSlaveService_impl.h        |  3 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          |  1 -
 6 files changed, 39 insertions(+), 29 deletions(-)

diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
index 16e5b7d058f..ef3489c9b60 100644
--- a/idl/WholeBodyMasterSlaveService.idl
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -51,8 +51,9 @@ module OpenHRP
     };
     boolean startCountDownForWholeBodyMasterSlave(in double sec);
     boolean startWholeBodyMasterSlave();
-    boolean pauseWholeBodyMasterSlave();
     boolean stopWholeBodyMasterSlave();
+    boolean pauseWholeBodyMasterSlave();
+    boolean resumeWholeBodyMasterSlave();
     void setWholeBodyMasterSlaveParam(in WholeBodyMasterSlaveParam i_param);
     void getWholeBodyMasterSlaveParam(out WholeBodyMasterSlaveParam i_param);
   };
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 2480d04c16d..c02190336f5 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -231,23 +231,25 @@ void WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC:
 
 RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
   if(DEBUGP_ONCE)std::cerr << "[" << m_profile.instance_name<< "] onExecute(" << ec_id << ")" << std::endl;
-  // struct timeval t_calc_start, t_calc_end;
-  // gettimeofday(&t_calc_start, NULL);
+   struct timeval t_calc_start, t_calc_end;
+   gettimeofday(&t_calc_start, NULL);
     if (m_qRefIn.isNew()) { m_qRefIn.read(); }
     if (m_basePosIn.isNew()) { m_basePosIn.read(); }
     if (m_baseRpyIn.isNew()) { m_baseRpyIn.read(); }
     if (m_zmpIn.isNew()) { m_zmpIn.read(); }
     if (m_optionalDataIn.isNew()) { m_optionalDataIn.read(); }
     //for HumanSynchronizer
-    if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToVector6(m_htrfw.data,hsp->hp_wld_raw.tgt[rf].w); }
-    if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToVector6(m_htlfw.data,hsp->hp_wld_raw.tgt[lf].w); }
-    if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Pose3DToWBMSPose3D(m_htcom.data,hsp->hp_wld_raw.tgt[com].abs); }
-    if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Pose3DToWBMSPose3D(m_htrf.data,hsp->hp_wld_raw.tgt[rf].abs); }
-    if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Pose3DToWBMSPose3D(m_htlf.data,hsp->hp_wld_raw.tgt[lf].abs); }
-    if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToWBMSPose3D(m_htrh.data,hsp->hp_wld_raw.tgt[rh].abs);}
-    if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToWBMSPose3D(m_htlh.data,hsp->hp_wld_raw.tgt[lh].abs);}
-    if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToWBMSPose3D(m_hthead.data,hsp->hp_wld_raw.tgt[head].abs);}
-    if (m_htzmpIn.isNew()){ m_htzmpIn.read();  HumanSynchronizer::Point3DToVector3(m_htzmp.data,hsp->hp_wld_raw.tgt[zmp].abs.p); }
+    if(mode!=MODE_PAUSE){
+      if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToVector6(m_htrfw.data,hsp->hp_wld_raw.tgt[rf].w); }
+      if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToVector6(m_htlfw.data,hsp->hp_wld_raw.tgt[lf].w); }
+      if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Pose3DToWBMSPose3D(m_htcom.data,hsp->hp_wld_raw.tgt[com].abs); }
+      if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Pose3DToWBMSPose3D(m_htrf.data,hsp->hp_wld_raw.tgt[rf].abs); }
+      if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Pose3DToWBMSPose3D(m_htlf.data,hsp->hp_wld_raw.tgt[lf].abs); }
+      if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToWBMSPose3D(m_htrh.data,hsp->hp_wld_raw.tgt[rh].abs);}
+      if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToWBMSPose3D(m_htlh.data,hsp->hp_wld_raw.tgt[lh].abs);}
+      if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToWBMSPose3D(m_hthead.data,hsp->hp_wld_raw.tgt[head].abs);}
+      if (m_htzmpIn.isNew()){ m_htzmpIn.read();  HumanSynchronizer::Point3DToVector3(m_htzmp.data,hsp->hp_wld_raw.tgt[zmp].abs.p); }
+    }
 //    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
 
     if ( is_legged_robot ) {
@@ -406,24 +408,18 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
      HumanSynchronizer::Vector6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
      m_invdyn_dbgOut.write();
 #endif
-     // gettimeofday(&t_calc_end, NULL);
-     //  if(DEBUGP)cout<<"t1:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl;
-     //  t_calc_start = t_calc_end;
+    gettimeofday(&t_calc_end, NULL);
+    if(DEBUGP)cout<<"t1:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl;
+    t_calc_start = t_calc_end;
     loop ++;
     return RTC::RTC_OK;
 }
 
 
 void WholeBodyMasterSlave::processTransition(){
-//  cout<<"hsp->mode"<<hsp->mode<<endl;
   switch(mode){
-    case MODE_COUNTDOWN:
-      std::cerr << "[" << m_profile.instance_name << "] Count Down for HumanSync ["<<hsp->getRemainingCountDown()<<"]\r";
-//      hsp->updateCountDown();
-      break;
 
-    case MODE_SYNC_TO_WBMS:
-//      if(previous_mode == MODE_COUNTDOWN){ double tmp = 1.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
+    case MODE_SYNC_TO_WBMS://
       if(previous_mode == MODE_IDLE){ double tmp = 1.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
       if (!transition_interpolator->isEmpty() ){
         transition_interpolator->get(&transition_interpolator_ratio, true);
@@ -433,7 +429,7 @@ void WholeBodyMasterSlave::processTransition(){
       break;
 
     case MODE_SYNC_TO_IDLE:
-      if(previous_mode == MODE_WBMS){ double tmp = 0.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
+      if(previous_mode == MODE_WBMS || previous_mode == MODE_PAUSE){ double tmp = 0.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
       if (!transition_interpolator->isEmpty()) {
         transition_interpolator->get(&transition_interpolator_ratio, true);
       }else{
@@ -751,6 +747,13 @@ bool WholeBodyMasterSlave::pauseWholeBodyMasterSlave(){
 }
 
 
+bool WholeBodyMasterSlave::resumeWholeBodyMasterSlave(){
+  std::cerr << "[" << m_profile.instance_name << "] pauseWholeBodyMasterSlave" << std::endl;
+  mode = MODE_WBMS;
+  return true;
+}
+
+
 bool WholeBodyMasterSlave::stopWholeBodyMasterSlave(){
   std::cerr << "[" << m_profile.instance_name << "] stopWholeBodyMasterSlave" << std::endl;
   mode = MODE_SYNC_TO_IDLE;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index bb2534ff65f..e2a24835ffb 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -47,8 +47,9 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
     virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
     bool startCountDownForWholeBodyMasterSlave(const double sec);
     bool startWholeBodyMasterSlave();
-    bool pauseWholeBodyMasterSlave();
     bool stopWholeBodyMasterSlave();
+    bool pauseWholeBodyMasterSlave();
+    bool resumeWholeBodyMasterSlave();
     bool setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
     bool getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
 
@@ -168,7 +169,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
 
   boost::shared_ptr<HumanSynchronizer> hsp;
 
-  enum { MODE_IDLE, MODE_COUNTDOWN, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE} mode, previous_mode;
+  enum { MODE_IDLE,/* MODE_COUNTDOWN,*/ MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE} mode, previous_mode;
   inline bool isRunning(){ return (mode==MODE_SYNC_TO_WBMS) || (mode==MODE_WBMS) || (mode==MODE_PAUSE) || (mode==MODE_SYNC_TO_IDLE) ;}
 
   void setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in);
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp
index 35ed2ca6ca3..e62ea69ca7b 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp
@@ -24,14 +24,19 @@ CORBA::Boolean WholeBodyMasterSlaveService_impl::startWholeBodyMasterSlave()
     return m_wholebodymasterslave->startWholeBodyMasterSlave();
 };
 
+CORBA::Boolean WholeBodyMasterSlaveService_impl::stopWholeBodyMasterSlave()
+{
+    return m_wholebodymasterslave->stopWholeBodyMasterSlave();
+};
+
 CORBA::Boolean WholeBodyMasterSlaveService_impl::pauseWholeBodyMasterSlave()
 {
     return m_wholebodymasterslave->pauseWholeBodyMasterSlave();
 };
 
-CORBA::Boolean WholeBodyMasterSlaveService_impl::stopWholeBodyMasterSlave()
+CORBA::Boolean WholeBodyMasterSlaveService_impl::resumeWholeBodyMasterSlave()
 {
-    return m_wholebodymasterslave->stopWholeBodyMasterSlave();
+    return m_wholebodymasterslave->resumeWholeBodyMasterSlave();
 };
 
 void WholeBodyMasterSlaveService_impl::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param)
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h
index 8f2041c45d9..0f8a12c38f2 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h
@@ -17,8 +17,9 @@ class WholeBodyMasterSlaveService_impl
   virtual ~WholeBodyMasterSlaveService_impl();
   CORBA::Boolean startCountDownForWholeBodyMasterSlave(const CORBA::Double sec);
   CORBA::Boolean startWholeBodyMasterSlave();
-  CORBA::Boolean pauseWholeBodyMasterSlave();
   CORBA::Boolean stopWholeBodyMasterSlave();
+  CORBA::Boolean pauseWholeBodyMasterSlave();
+  CORBA::Boolean resumeWholeBodyMasterSlave();
   void setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
   void getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam_out i_param);
   //
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 42c4bff3ecb..ca5c703550c 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -46,7 +46,6 @@ class UTIL_CONST{
     enum { r, p, y, rpy };
     enum { fx, fy, fz, tx, ty, tz, ft_xyz };
     enum { MIN, MAX, MINMAX };
-//    enum { MODE_IDLE, MODE_COUNTDOWN, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE};
     double G, D2R, INFMIN,INFMAX;
     UTIL_CONST() :
       G(9.80665),

From 747072803d796c520767ce05944b72c3509ffbf5 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 29 May 2017 23:50:40 +0900
Subject: [PATCH 093/217] complete manipulability limit

---
 .../WholeBodyMasterSlave.cpp                  | 310 +++++++++---------
 .../WholeBodyMasterSlave.h                    |   9 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 221 ++++++++-----
 3 files changed, 306 insertions(+), 234 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index c02190336f5..28dcc664629 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -170,7 +170,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     } else {
       is_legged_robot = false;
     }
-    hsp = boost::shared_ptr<HumanSynchronizer>(new HumanSynchronizer(m_dt));
+    hsp = boost::shared_ptr<WBMSCore>(new WBMSCore(m_dt));
 
     invdyn_zmp_filters.setParameter(25, 1/m_dt, BiquadIIRFilterVec::Q_BUTTERWORTH);
     invdyn_zmp_filters2.setParameter(25, 1/m_dt, BiquadIIRFilterVec::Q_BUTTERWORTH);
@@ -229,10 +229,11 @@ void WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC:
   }
 }
 
+#define TIMECALC 1
 RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
   if(DEBUGP_ONCE)std::cerr << "[" << m_profile.instance_name<< "] onExecute(" << ec_id << ")" << std::endl;
-   struct timeval t_calc_start, t_calc_end;
-   gettimeofday(&t_calc_start, NULL);
+  struct timeval t_calc_start, t_calc_end;
+  if(TIMECALC)gettimeofday(&t_calc_start, NULL);
     if (m_qRefIn.isNew()) { m_qRefIn.read(); }
     if (m_basePosIn.isNew()) { m_basePosIn.read(); }
     if (m_baseRpyIn.isNew()) { m_baseRpyIn.read(); }
@@ -240,15 +241,15 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if (m_optionalDataIn.isNew()) { m_optionalDataIn.read(); }
     //for HumanSynchronizer
     if(mode!=MODE_PAUSE){
-      if (m_htrfwIn.isNew()){ m_htrfwIn.read(); HumanSynchronizer::DoubleSeqToVector6(m_htrfw.data,hsp->hp_wld_raw.tgt[rf].w); }
-      if (m_htlfwIn.isNew()){ m_htlfwIn.read(); HumanSynchronizer::DoubleSeqToVector6(m_htlfw.data,hsp->hp_wld_raw.tgt[lf].w); }
-      if (m_htcomIn.isNew()){ m_htcomIn.read(); HumanSynchronizer::Pose3DToWBMSPose3D(m_htcom.data,hsp->hp_wld_raw.tgt[com].abs); }
-      if (m_htrfIn.isNew()) { m_htrfIn.read();  HumanSynchronizer::Pose3DToWBMSPose3D(m_htrf.data,hsp->hp_wld_raw.tgt[rf].abs); }
-      if (m_htlfIn.isNew()) { m_htlfIn.read();  HumanSynchronizer::Pose3DToWBMSPose3D(m_htlf.data,hsp->hp_wld_raw.tgt[lf].abs); }
-      if (m_htrhIn.isNew()) { m_htrhIn.read();  HumanSynchronizer::Pose3DToWBMSPose3D(m_htrh.data,hsp->hp_wld_raw.tgt[rh].abs);}
-      if (m_htlhIn.isNew()) { m_htlhIn.read();  HumanSynchronizer::Pose3DToWBMSPose3D(m_htlh.data,hsp->hp_wld_raw.tgt[lh].abs);}
-      if (m_htheadIn.isNew()){ m_htheadIn.read(); HumanSynchronizer::Pose3DToWBMSPose3D(m_hthead.data,hsp->hp_wld_raw.tgt[head].abs);}
-      if (m_htzmpIn.isNew()){ m_htzmpIn.read();  HumanSynchronizer::Point3DToVector3(m_htzmp.data,hsp->hp_wld_raw.tgt[zmp].abs.p); }
+      if (m_htrfwIn.isNew()){ m_htrfwIn.read(); WBMSCore::DoubleSeqToVector6(m_htrfw.data,hsp->hp_wld_raw.tgt[rf].w); }
+      if (m_htlfwIn.isNew()){ m_htlfwIn.read(); WBMSCore::DoubleSeqToVector6(m_htlfw.data,hsp->hp_wld_raw.tgt[lf].w); }
+      if (m_htcomIn.isNew()){ m_htcomIn.read(); WBMSCore::Pose3DToWBMSPose3D(m_htcom.data,hsp->hp_wld_raw.tgt[com].abs); }
+      if (m_htrfIn.isNew()) { m_htrfIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htrf.data,hsp->hp_wld_raw.tgt[rf].abs); }
+      if (m_htlfIn.isNew()) { m_htlfIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htlf.data,hsp->hp_wld_raw.tgt[lf].abs); }
+      if (m_htrhIn.isNew()) { m_htrhIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htrh.data,hsp->hp_wld_raw.tgt[rh].abs);}
+      if (m_htlhIn.isNew()) { m_htlhIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htlh.data,hsp->hp_wld_raw.tgt[lh].abs);}
+      if (m_htheadIn.isNew()){ m_htheadIn.read(); WBMSCore::Pose3DToWBMSPose3D(m_hthead.data,hsp->hp_wld_raw.tgt[head].abs);}
+      if (m_htzmpIn.isNew()){ m_htzmpIn.read();  WBMSCore::Point3DToVector3(m_htzmp.data,hsp->hp_wld_raw.tgt[zmp].abs.p); }
     }
 //    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
 
@@ -256,6 +257,9 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
       processTransition();
 
+
+      if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"processTransition:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl; t_calc_start = t_calc_end;}
+
       fik->current_tm = fik_rmc->current_tm = m_qRef.tm;
       fik->ikp["rleg"].is_ik_enable = fik_rmc->ikp["rleg"].is_ik_enable = true;
       fik->ikp["lleg"].is_ik_enable = fik_rmc->ikp["lleg"].is_ik_enable = true;
@@ -269,6 +273,9 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
           preProcessForWholeBodyMasterSlave(fik, m_robot);
         }
 
+
+//        calcManipulabilityJointLimitForWBMS(fik, m_robot);
+
         if(hsp->WBMSparam.is_doctor){
           processWholeBodyMasterSlave(fik, m_robot);//安全制限つきマスタ・スレーブ
         }else{
@@ -276,13 +283,13 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         }
 
 
+        if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"processWholeBodyMasterSlave:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl; t_calc_start = t_calc_end;}
         //逆動力学初期化
         if(hsp->is_initial_loop){
           idsb.setInitState(m_robot, m_dt);
           idsb2.setInitState(m_robot, m_dt);
         }
 
-
         //逆動力学
         calcAccelerationsForInverseDynamics(m_robot, idsb);
         hrp::Vector3 ref_zmp_invdyn;
@@ -291,6 +298,9 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         updateInvDynStateBuffer(idsb);
 
 
+
+        if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"calcWorldZMPFromInverseDynamics:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl; t_calc_start = t_calc_end;}
+
         //角運動量補償
         if(hsp->WBMSparam.is_doctor){
           processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, hsp->rp_ref_out);
@@ -302,6 +312,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 //      updateInvDynStateBuffer(idsb2);
 
 
+
+        if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"processMomentumCompensation:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl; t_calc_start = t_calc_end;}
         //OutPortデータセット
         //        ref_zmp = ref_zmp_invdyn;
         hrp::Vector3 ref_zmp = hsp->rp_ref_out.tgt[zmp].abs.p;
@@ -348,69 +360,67 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 #ifdef USE_DEBUG_PORT
     // dbg plot
      m_htcom.tm = m_qRef.tm;
-     HumanSynchronizer::WBMSPose3DToPose3D(hsp->hp_plot.tgt[com].abs, m_htcom_dbg.data);
+     WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[com].abs, m_htcom_dbg.data);
      m_htcom_dbgOut.write();
      m_htrf.tm = m_qRef.tm;
-     HumanSynchronizer::WBMSPose3DToPose3D(hsp->hp_plot.tgt[rf].abs, m_htrf_dbg.data);
+     WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[rf].abs, m_htrf_dbg.data);
      m_htrf_dbgOut.write();
      m_htlf.tm = m_qRef.tm;
-     HumanSynchronizer::WBMSPose3DToPose3D(hsp->hp_plot.tgt[lf].abs, m_htlf_dbg.data);
+     WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[lf].abs, m_htlf_dbg.data);
      m_htlf_dbgOut.write();
      m_htrh.tm = m_qRef.tm;
-     HumanSynchronizer::WBMSPose3DToPose3D(hsp->hp_plot.tgt[rh].abs, m_htrh_dbg.data);
+     WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[rh].abs, m_htrh_dbg.data);
      m_htrh_dbgOut.write();
      m_htlh.tm = m_qRef.tm;
-     HumanSynchronizer::WBMSPose3DToPose3D(hsp->hp_plot.tgt[lh].abs, m_htlh_dbg.data);
+     WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[lh].abs, m_htlh_dbg.data);
      m_htlh_dbgOut.write();
      m_hthead.tm = m_qRef.tm;
-     HumanSynchronizer::WBMSPose3DToPose3D(hsp->hp_plot.tgt[head].abs, m_hthead_dbg.data);
+     WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[head].abs, m_hthead_dbg.data);
      m_hthead_dbgOut.write();
      m_htzmp.tm = m_qRef.tm;
-     HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.tgt[zmp].abs.p,m_rpzmp_dbg.data);
+     WBMSCore::Vector3ToPoint3D(hsp->rp_ref_out.tgt[zmp].abs.p,m_rpzmp_dbg.data);
      m_htzmp_dbgOut.write();
      m_htrfw.tm = m_qRef.tm;
      m_htrfw_dbg.data.length(6);
-     HumanSynchronizer::Vector6ToDoubleSeq(hsp->hp_plot.tgt[rf].w, m_htrfw_dbg.data);
+     WBMSCore::Vector6ToDoubleSeq(hsp->hp_plot.tgt[rf].w, m_htrfw_dbg.data);
      m_htrfw_dbgOut.write();
      m_htlfw.tm = m_qRef.tm;
      m_htlfw_dbg.data.length(6);
-     HumanSynchronizer::Vector6ToDoubleSeq(hsp->hp_plot.tgt[lf].w, m_htlfw_dbg.data);
+     WBMSCore::Vector6ToDoubleSeq(hsp->hp_plot.tgt[lf].w, m_htlfw_dbg.data);
      m_htlfw_dbgOut.write();
      m_rpcom_dbg.tm = m_qRef.tm;
-     HumanSynchronizer::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[com].abs, m_rpcom_dbg.data);
+     WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[com].abs, m_rpcom_dbg.data);
      m_rpcom_dbgOut.write();
      m_rprf_dbg.tm = m_qRef.tm;
-     HumanSynchronizer::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[rf].abs, m_rprf_dbg.data);
+     WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[rf].abs, m_rprf_dbg.data);
      m_rprf_dbgOut.write();
      m_rplf_dbg.tm = m_qRef.tm;
-     HumanSynchronizer::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[lf].abs, m_rplf_dbg.data);
+     WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[lf].abs, m_rplf_dbg.data);
      m_rplf_dbgOut.write();
      m_rprh_dbg.tm = m_qRef.tm;
-     HumanSynchronizer::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[rh].abs, m_rprh_dbg.data);
+     WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[rh].abs, m_rprh_dbg.data);
      m_rprh_dbgOut.write();
      m_rplh_dbg.tm = m_qRef.tm;
-     HumanSynchronizer::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[lh].abs, m_rplh_dbg.data);
+     WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[lh].abs, m_rplh_dbg.data);
      m_rplh_dbgOut.write();
      m_rphead_dbg.tm = m_qRef.tm;
-     HumanSynchronizer::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[head].abs, m_rphead_dbg.data);
+     WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[head].abs, m_rphead_dbg.data);
      m_rphead_dbgOut.write();
      m_rpzmp_dbg.tm = m_qRef.tm;
-     HumanSynchronizer::Vector3ToPoint3D(hsp->rp_ref_out.tgt[zmp].abs.p, m_rpzmp_dbg.data);
+     WBMSCore::Vector3ToPoint3D(hsp->rp_ref_out.tgt[zmp].abs.p, m_rpzmp_dbg.data);
      m_rpzmp_dbgOut.write();
      m_rpdcp_dbg.tm = m_qRef.tm;
-     HumanSynchronizer::Vector3ToPoint3D(hsp->cp_dec,m_rpdcp_dbg.data);
+     WBMSCore::Vector3ToPoint3D(hsp->cp_dec,m_rpdcp_dbg.data);
      m_rpdcp_dbgOut.write();
      m_rpacp_dbg.tm = m_qRef.tm;
-     HumanSynchronizer::Vector3ToPoint3D(hsp->cp_acc,m_rpacp_dbg.data);
+     WBMSCore::Vector3ToPoint3D(hsp->cp_acc,m_rpacp_dbg.data);
      m_rpacp_dbgOut.write();
      m_invdyn_dbg.tm = m_qRef.tm;
      m_invdyn_dbg.data.length(6);
-     HumanSynchronizer::Vector6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
+     WBMSCore::Vector6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
      m_invdyn_dbgOut.write();
 #endif
-    gettimeofday(&t_calc_end, NULL);
-    if(DEBUGP)cout<<"t1:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl;
-    t_calc_start = t_calc_end;
+     if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"t_last:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl; t_calc_start = t_calc_end;}
     loop ++;
     return RTC::RTC_OK;
 }
@@ -419,7 +429,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 void WholeBodyMasterSlave::processTransition(){
   switch(mode){
 
-    case MODE_SYNC_TO_WBMS://
+    case MODE_SYNC_TO_WBMS:
       if(previous_mode == MODE_IDLE){ double tmp = 1.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
       if (!transition_interpolator->isEmpty() ){
         transition_interpolator->get(&transition_interpolator_ratio, true);
@@ -496,8 +506,8 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::
     if(callnum == 0){
       pos_filters[i].setParameter(100.0, 1.0/m_dt, BiquadIIRFilterVec::Q_NOOVERSHOOT);
       rot_filters[i].setParameter(100.0, 1.0/m_dt, BiquadIIRFilterVec::Q_NOOVERSHOOT);
-      pos_filters[i].init(hsp->hp_wld_raw.tgt[i].abs.p);
-      rot_filters[i].init(hsp->hp_wld_raw.tgt[i].abs.rpy);
+      pos_filters[i].reset(hsp->hp_wld_raw.tgt[i].abs.p);
+      rot_filters[i].reset(hsp->hp_wld_raw.tgt[i].abs.rpy);
     }
     raw_pose.tgt[i].abs.p = pos_filters[i].passFilter(hsp->hp_wld_raw.tgt[i].abs.p);
     raw_pose.tgt[i].abs.rpy = rot_filters[i].passFilter(hsp->hp_wld_raw.tgt[i].abs.rpy);
@@ -507,56 +517,43 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::
   solveFullbodyIKStrictCOM(fik_in, robot_in, raw_pose.tgt[com].abs, raw_pose.tgt[rf].abs, raw_pose.tgt[lf].abs, raw_pose.tgt[rh].abs, raw_pose.tgt[lh].abs, raw_pose.tgt[head].abs,"processWholeBodyMasterSlave_Raw");
 }
 
-
-void WholeBodyMasterSlave::calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in){
+//これ超計算重い
+void WholeBodyMasterSlave::calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref){
   const std::string names[4] = {"rleg","lleg","rarm","larm"};
+  const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
   double min_manip_val = 1e9;
-  const double min_manip_th = 0.1;
+  const double min_manip_th = 0.05;
+  hrp::Vector3 target_p0_old[4];
 
-  static hrp::Vector3 target_p0_old[4];
   static bool firstcall = true;
-//    for(int i=0;i<4;i++){
-  for(int i=0;i<1;i++){
-    hrp::dmatrix J,Jinv,Jnull;
+  hrp::dmatrix J,Jinv,Jnull;
+  for(int i=0; i<4; i++){
     fik_in->ikp[names[i]].manip->calcJacobian(J);
     fik_in->ikp[names[i]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
-
-    hrp::dmatrix manipulability_mat = J*J.transpose();
-    double manipulability = sqrt((J*J.transpose()).determinant());
-    //    Eigen::JacobiSVD< Eigen::Matrix<float, 3, 3> > svd(mm3, Eigen::ComputeFullU | Eigen::ComputeFullV);
+//    hrp::dmatrix manipulability_mat = J*J.transpose();
+//    double manipulability = sqrt((J*J.transpose()).determinant());
     Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
 
     Eigen::MatrixXd::Index row,col;
     min_manip_val = svd.singularValues().minCoeff(&row,&col);
-
     hrp::Vector3 min_manip_direc = svd.matrixU().col(row);// unit vector
-    //      std::cout << "singular values" << std::endl << svd.singularValues() << std::endl;
-    //      std::cout << "matrix U" << std::endl << svd.matrixU() << std::endl;
-    //      std::cout << "matrix V" << std::endl << svd.matrixV() << std::endl;
     if(min_manip_direc.dot(fik_in->ikp[names[i]].target_p0 - robot_in->rootLink()->p) < 0){ min_manip_direc *= -1; }//向きみて反転(常に外側正)
 
+//    target_p0_old[i] = fik_in->ikp[names[i]].target_p0;
+    target_p0_old[i] = fik_in->ikp[names[i]].target_link->p + fik_in->ikp[names[i]].target_link->R * fik_in->ikp[names[i]].localPos;
+    fik_in->ikp[names[i]].target_p0 = refs[i]->p;
 
-    if(firstcall){
-      target_p0_old[i] = fik_in->ikp[names[i]].target_p0;
-    }
-
-
-    hrp::Vector3 ref_ee_vel = (fik_in->ikp[names[i]].target_p0 - target_p0_old[i]) / m_dt;
-    hrp::Vector3 ref_ee_vel_mod = ref_ee_vel;
-//      if((ref_vec.normalized()).dot(min_manip_direc)<0.001){
+    hrp::Vector3 ref_ee_vel = (fik_in->ikp[names[i]].target_p0 - target_p0_old[i]) / m_dt * 0.01;
 
     double penalty = (min_manip_val - min_manip_th) * 10;
-//      LIMIT_MINMAX(penalty, 0.0, 1.0);//
     LIMIT_MINMAX(penalty, -1.0, 1.0);//
 
     double lim_penalty = 1.0;
-    static double lim_penalty_old = 1.0;
     double lim_penalty_tmp = 1.0;
     double ulim_mergin[6], llim_mergin[6];
     static double ulim_mergin_old[6], llim_mergin_old[6];
     const double jlim_start_th = 30*D2R;
 
-
 //      for(int j=0;j<6;j++){
 //        llim_mergin[j] = fabs(robot_in->joint(j)->llimit - robot_in->joint(j)->q);
 //        ulim_mergin[j] = fabs(robot_in->joint(j)->ulimit - robot_in->joint(j)->q);
@@ -572,112 +569,122 @@ void WholeBodyMasterSlave::calcManipulabilityJointLimit(fikPtr& fik_in, hrp::Bod
 //        ulim_mergin_old[j] = ulim_mergin[j];
 //      }
 
-    hrp::dvector q_vel(6);
-    q_vel = Jinv.block(0,0,Jinv.cols(),3) * ref_ee_vel;
-
-    for(int j=0;j<6;j++){
-      llim_mergin[j] = fabs(robot_in->joint(j)->llimit - robot_in->joint(j)->q);
-      ulim_mergin[j] = fabs(robot_in->joint(j)->ulimit - robot_in->joint(j)->q);
-      if( llim_mergin[j] < jlim_start_th && q_vel[j] < 0){
-        if(DEBUGP)cout<<"llim"<<j<<": "<<endl;
-        lim_penalty_tmp =  llim_mergin[j]/jlim_start_th;
-        if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
-      }
-      if( ulim_mergin[j] < jlim_start_th && q_vel[j] > 0){
-        if(DEBUGP)cout<<"ulim"<<j<<": "<<endl;
-        lim_penalty_tmp =  ulim_mergin[j]/jlim_start_th;
-        if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
-      }
-    }
-//      LIMIT_MIN(lim_penalty, 0.01);
-
-
+//    hrp::dvector q_vel(6);
+//    q_vel = Jinv.block(0,0,Jinv.cols(),3) * ref_ee_vel;
+//    for(int j=0;j<6;j++){
+//      llim_mergin[j] = fabs(robot_in->joint(j)->llimit - robot_in->joint(j)->q);
+//      ulim_mergin[j] = fabs(robot_in->joint(j)->ulimit - robot_in->joint(j)->q);
+//      if( llim_mergin[j] < jlim_start_th && q_vel[j] < 0){
+//        if(DEBUGP)cout<<"llim"<<j<<": "<<endl;
+//        lim_penalty_tmp =  llim_mergin[j]/jlim_start_th;
+//        if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
+//      }
+//      if( ulim_mergin[j] < jlim_start_th && q_vel[j] > 0){
+//        if(DEBUGP)cout<<"ulim"<<j<<": "<<endl;
+//        lim_penalty_tmp =  ulim_mergin[j]/jlim_start_th;
+//        if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
+//      }
+//    }
 
-//      lim_penalty = lim_penalty * 0.01 + lim_penalty_old * 0.99;
-    lim_penalty_old = lim_penalty;
     if(DEBUGP){
       if(lim_penalty<1.0)cout<<"lim_penalty: "<<lim_penalty<<endl;
     }
 
     double max_dec_vel = 1.0, max_acc_vel = 1.0;// m/s
-//      if(ref_vec.dot(min_manip_direc)>0){
-      if(1){
-      hrp::Vector3 ref_ee_vel_v = (ref_ee_vel.dot(min_manip_direc)) * min_manip_direc;
-      hrp::Vector3 ref_ee_vel_h = ref_ee_vel - ref_ee_vel_v;
+    hrp::Vector3 ref_ee_vel_v = (ref_ee_vel.dot(min_manip_direc)) * min_manip_direc;
+    hrp::Vector3 ref_ee_vel_h = ref_ee_vel - ref_ee_vel_v;
 //        ref_vec_mod -= (ref_vec.dot(min_manip_direc)) * min_manip_direc;
 //        ref_vec_mod += (ref_vec.dot(min_manip_direc)) * min_manip_direc * penalty;
 
-      max_dec_vel *= penalty;
-      max_acc_vel *= penalty;
-      LIMIT_MIN(max_acc_vel, 0.0);//
-      max_acc_vel += 0.01;
-
-      if(ref_ee_vel_v.dot(min_manip_direc) > max_dec_vel){
-//        if(ref_ee_vel_v.norm() > max_vel){
-        ref_ee_vel_v = min_manip_direc * max_dec_vel;
-      }
-      if(ref_ee_vel_v.dot(-min_manip_direc) > max_acc_vel){
-        ref_ee_vel_v = ref_ee_vel_v.normalized() * max_acc_vel;
-      }
-
-//        if(min_manip_val<min_manip_th){
-//          ref_ee_vel_v -= min_manip_direc * (min_manip_th - min_manip_val) * 5;
-//        }
-
-
-//        if(min_manip_val<min_manip_th){
-//          ref_vec_mod += (ref_vec.dot(min_manip_direc)) * min_manip_direc * (min_manip_val - min_manip_th);
-//        }
-      ref_ee_vel_mod = ref_ee_vel_h + ref_ee_vel_v;//速度空間で一次フィルターしたほうがいい?
-      ref_ee_vel_mod *= lim_penalty;
-      if(DEBUGP)std::cout << names[i]<<" min_manip_direc: " << min_manip_direc.transpose() << " min_manip_val " << min_manip_val << std::endl;
+    max_dec_vel *= penalty;
+    max_acc_vel *= penalty;
+//      LIMIT_MIN(max_acc_vel, 0.0);//
+//      max_acc_vel += 0.01;
+    if(ref_ee_vel_v.dot(min_manip_direc) > max_dec_vel){
+      ref_ee_vel_v = min_manip_direc * max_dec_vel;
     }
-
-//      if(min_manip_val<min_manip_th && ref_vec.dot(min_manip_direc)>0){
-//        ref_vec -= (ref_vec.dot(min_manip_direc))*min_manip_direc;
-//        std::cout << names[i]<<" min_manip_direc: " << min_manip_direc.transpose() << " min_manip_val " << min_manip_val << std::endl;
+//      if(ref_ee_vel_v.dot(-min_manip_direc) > max_acc_vel){
+//        ref_ee_vel_v = ref_ee_vel_v.normalized() * max_acc_vel;
 //      }
+//    LIMIT_MIN(max_dec_vel, 0.01);
+//    if(ref_ee_vel_v.norm() > max_dec_vel){
+//      ref_ee_vel_v = ref_ee_vel_v.normalized() * max_dec_vel;
+//    }
 
+    hrp::Vector3 ref_ee_vel_mod = ref_ee_vel_h + ref_ee_vel_v;//速度空間で一次フィルターしたほうがいい?
+    ref_ee_vel_mod *= lim_penalty;
+    if(DEBUGP)std::cout << names[i]<<" min_manip_direc: " << min_manip_direc.transpose() << " min_manip_val " << min_manip_val << std::endl;
 
     fik_in->ikp[names[i]].target_p0 = target_p0_old[i] + ref_ee_vel_mod * m_dt;
-    target_p0_old[i] = fik_in->ikp[names[i]].target_p0;
-//      fik_in->ikp[names[i]].target_p0 = ee_filter.passFilter(fik_in->ikp[names[i]].target_p0);
-//      cout<<"fik_in->ikp[names[i]].target_p0:"<<fik_in->ikp[names[i]].target_p0.transpose()<<endl;
-
-//      if(min_manip_val<min_manip_th){
-//        std::cout << names[i]<<" min_manip_direc: " << min_manip_direc.transpose() << " min_manip_val " << min_manip_val << std::endl;
-//        hrp::Vector3 cur_p = fik_in->ikp[names[i]].target_link->p+ fik_in->ikp[names[i]].target_link->R * fik_in->ikp[names[i]].localPos;
-//        fik_in->ikp[names[i]].target_p0 = cur_p + min_manip_direc * 1.0 * (min_manip_th - min_manip_val);
-//      }
-    firstcall = false;
+    fik_in->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);
+
+//    target_p0_old[i] = fik_in->ikp[names[i]].target_p0;
   }
+  firstcall = false;
 }
 
 
-void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix){
+void WholeBodyMasterSlave::calcManipulabilityJointLimitForWBMS(fikPtr& fik_in, hrp::BodyPtr& robot_in){
+//  const std::string names[4] = {"rleg","lleg","rarm","larm"};
+//  double min_manip_val = 1e9;
+//  const double min_manip_th = 0.1;
+//
+//  static hrp::Vector3 target_p0_old[4];
+//  static bool firstcall = true;
+////    for(int i=0;i<4;i++){
+//  for(int i=0;i<1;i++){
+//    hrp::dmatrix J,Jinv,Jnull;
+//    fik_in->ikp[names[i]].manip->calcJacobian(J);
+//    fik_in->ikp[names[i]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
+//
+//    hrp::dmatrix manipulability_mat = J*J.transpose();
+//    double manipulability = sqrt((J*J.transpose()).determinant());
+//    //    Eigen::JacobiSVD< Eigen::Matrix<float, 3, 3> > svd(mm3, Eigen::ComputeFullU | Eigen::ComputeFullV);
+//    Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
+//
+//    Eigen::MatrixXd::Index row,col;
+//    min_manip_val = svd.singularValues().minCoeff(&row,&col);
+//
+//    hrp::Vector3 min_manip_direc = svd.matrixU().col(row);// unit vector
+//    //      std::cout << "singular values" << std::endl << svd.singularValues() << std::endl;
+//    //      std::cout << "matrix U" << std::endl << svd.matrixU() << std::endl;
+//    //      std::cout << "matrix V" << std::endl << svd.matrixV() << std::endl;
+//    if(min_manip_direc.dot(fik_in->ikp[names[i]].target_p0 - robot_in->rootLink()->p) < 0){ min_manip_direc *= -1; }//向きみて反転(常に外側正)
+//
+//    hsp->fik = fik_in;
+//    hsp->m_robot = robot_in;
+//    hsp->cur_min_manip_direc = min_manip_direc;
+//    hsp->cur_manip_val = min_manip_val;
+//  }
+}
+
 
+void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix){
   int com_ik_loop=0;
   const int COM_IK_MAX_LOOP = 10;
   const double COM_IK_MAX_ERROR = 1e-5;//1e-4だと乱れる
-//  robot_in->rootLink()->p = 0.01*com_ref.p + 0.99*robot_in->rootLink()->p;//ベースリンク位置が迷走するのを防ぐ？
-  robot_in->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
   const std::string names[4] = {"rleg","lleg","rarm","larm"};
   const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
-  for(int i=0;i<4;i++){
-    if(fik_in->ikp.count(names[i])){
-      fik_in->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);
-      fik_in->ikp[names[i]].target_p0 = refs[i]->p;
+
+  if(hsp->WBMSparam.use_manipulability_limit){
+    calcManipulabilityJointLimit(fik_in, robot_in, rf_ref, lf_ref, rh_ref, lh_ref);
+  }else{
+    for(int i=0;i<4;i++){
+      if(fik_in->ikp.count(names[i])){
+        fik_in->ikp[names[i]].target_p0 = refs[i]->p;
+        fik_in->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);
+      }
     }
   }
-  fik_in->storeCurrentParameters();
+//  robot_in->rootLink()->p = 0.01*com_ref.p + 0.99*robot_in->rootLink()->p;//ベースリンク位置が迷走するのを防ぐ？
+  robot_in->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
+
+//  fik_in->storeCurrentParameters();
 //  fik_in->setReferenceJointAngles();//これ入れると腕ブワーなる
   if(robot_in->link("HEAD_JOINT0") != NULL)robot_in->joint(15)->q = head_ref.rpy(y);
   if(robot_in->link("HEAD_JOINT1") != NULL)robot_in->joint(16)->q = head_ref.rpy(p);
   hrp::Vector3 tmp_com_err = hrp::Vector3::Zero();
 
-  if(hsp->WBMSparam.use_manipulability_limit){
-    calcManipulabilityJointLimit(fik_in, robot_in);
-  }
   while( 1 ){  //COM 収束ループ
     com_ik_loop++;
     robot_in->rootLink()->p += tmp_com_err;
@@ -717,18 +724,19 @@ void WholeBodyMasterSlave::processMomentumCompensation(fikPtr& fik_in, hrp::Body
 
 
 bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double sec){
-//  if(sec >= 0.0 && sec <= 30.0){
-//    std::cerr << "[" << m_profile.instance_name << "] start Synchronization after "<<sec<<" [s]" << std::endl;
-//    hsp->countdown_sec = sec;
-////    hsp->startCountdownForHumanSync = true;
-//    mode = MODE_COUNTDOWN;
-//    while (mode == MODE_COUNTDOWN){ usleep(1000); }
-//    std::cerr << "[" << m_profile.instance_name << "] start Synchronization" << std::endl;
-//    return true;
-//  }else{
-//    std::cerr << "[" << m_profile.instance_name << "] Count Down Time must be 0 < T < 30 [s]"<< std::endl;
-//    return false;
-//  }
+  if(sec >= 0.0 && sec <= 30.0){
+    std::cerr << "[" << m_profile.instance_name << "] start Synchronization after "<<sec<<" [s]" << std::endl;
+    double remained_usec = sec * 10e6;
+    while (remained_usec > 0){
+      usleep(2000);
+      remained_usec -= 2000;
+    }
+    startWholeBodyMasterSlave();
+    return true;
+  }else{
+    std::cerr << "[" << m_profile.instance_name << "] Count Down Time must be 0 < T < 30 [s]"<< std::endl;
+    return false;
+  }
 }
 
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index e2a24835ffb..db7ea08a71e 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -165,15 +165,18 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
   hrp::InvDynStateBuffer idsb2;
   BiquadIIRFilterVec invdyn_zmp_filters2;
 
+  IIRFilter ref_ee_vel_v_filter;
+
   HumanPose raw_pose;
 
-  boost::shared_ptr<HumanSynchronizer> hsp;
+  boost::shared_ptr<WBMSCore> hsp;
 
   enum { MODE_IDLE,/* MODE_COUNTDOWN,*/ MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE} mode, previous_mode;
-  inline bool isRunning(){ return (mode==MODE_SYNC_TO_WBMS) || (mode==MODE_WBMS) || (mode==MODE_PAUSE) || (mode==MODE_SYNC_TO_IDLE) ;}
+  bool isRunning(){ return (mode==MODE_SYNC_TO_WBMS) || (mode==MODE_WBMS) || (mode==MODE_PAUSE) || (mode==MODE_SYNC_TO_IDLE) ;}
 
   void setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in);
-  void calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in);
+  void calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref);
+  void calcManipulabilityJointLimitForWBMS(fikPtr& fik_in, hrp::BodyPtr& robot_in);
   void solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix="");
   void processTransition();
   void preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in);
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index ca5c703550c..7c3952d0626 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -37,6 +37,10 @@ extern "C" {
 
 #define DEBUG 0
 
+#define LIMIT_MIN(x,min) (x= ( x<min ? min:x ))
+#define LIMIT_MAX(x,max) (x= ( x<max ? x:max ))
+#define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
+
 class UTIL_CONST{
   public:
     enum { com, rf, lf, rh, lh, head, zmp, num_pose_tgt };
@@ -53,9 +57,9 @@ class UTIL_CONST{
       INFMIN( - std::numeric_limits<double>::max()),
       INFMAX( + std::numeric_limits<double>::max())
     {};
-    static inline bool LIMIT_MIN(double& x, const double min){ if(x<min){x=min; return true;}else{return false;} }
-    static inline bool LIMIT_MAX(double& x, const double max){ if(x>max){x=max; return true;}else{return false;} }
-    static inline bool LIMIT_MINMAX(double& x, const double min, const double max){ assert( min <=  max); if(x<min){x=min; return true;} else if(x>max){x=max; return true;} else{return false;} }
+//    static bool LIMIT_MIN(double& x, const double min){ if(x<min){x=min; return true;}else{return false;} }
+//    static bool LIMIT_MAX(double& x, const double max){ if(x>max){x=max; return true;}else{return false;} }
+//    static bool LIMIT_MINMAX(double& x, const double min, const double max){ assert( min <=  max); if(x<min){x=min; return true;} else if(x>max){x=max; return true;} else{return false;} }
 };
 
 class BiquadIIRFilterVec : UTIL_CONST {
@@ -64,20 +68,34 @@ class BiquadIIRFilterVec : UTIL_CONST {
     hrp::Vector3 ans;
   public:
     static const double Q_BUTTERWORTH = 0.707106781, Q_NOOVERSHOOT = 0.5;
-    BiquadIIRFilterVec(){};
-    ~BiquadIIRFilterVec(){};
-    inline void setParameter(const hrp::Vector3& fc_in, const double& HZ, const double& Q = Q_BUTTERWORTH){ for(int i=0;i<XYZ;i++){ filters[i].setParameterAsBiquad((double)fc_in(i), Q, HZ); } };
-    inline void setParameter(const double& fc_in, const double& HZ, const double& Q = Q_BUTTERWORTH){ setParameter(hrp::Vector3(fc_in,fc_in,fc_in), HZ, Q); };//overload
-    inline hrp::Vector3 passFilter(const hrp::Vector3& input){ for(int i=0;i<XYZ;i++){ ans(i) = filters[i].passFilter((double)input(i)); } return ans; };
-    inline void init(const hrp::Vector3& initial_input){ for(int i=0;i<XYZ;i++){ filters[i].reset((double)initial_input(i));} }
+    BiquadIIRFilterVec(){}
+    ~BiquadIIRFilterVec(){}
+    void setParameter(const hrp::Vector3& fc_in, const double& HZ, const double& Q = Q_BUTTERWORTH){ for(int i=0;i<XYZ;i++){ filters[i].setParameterAsBiquad((double)fc_in(i), Q, HZ); } }
+    void setParameter(const double& fc_in, const double& HZ, const double& Q = Q_BUTTERWORTH){ setParameter(hrp::Vector3(fc_in,fc_in,fc_in), HZ, Q); }//overload
+    hrp::Vector3 passFilter(const hrp::Vector3& input){ for(int i=0;i<XYZ;i++){ ans(i) = filters[i].passFilter((double)input(i)); } return ans; }
+    void reset(const hrp::Vector3& initial_input){ for(int i=0;i<XYZ;i++){ filters[i].reset((double)initial_input(i));} }
 };
 
+//class PDFilter{
+//  public:
+//    PDFilter(){};
+//    ~PDFilter(){};
+//    void setParameter(const double& pgain, const double& dgain, const double& HZ){
+//
+//    }
+//    void passFilter(){
+//
+//    }
+//};
+
+
+
 class WBMSPose3D{
   public:
     hrp::Vector3 p,rpy;
     WBMSPose3D(){ clear(); }
     ~WBMSPose3D(){}
-    inline void clear(){ p = rpy = hrp::Vector3::Zero(); }
+    void clear(){ p = rpy = hrp::Vector3::Zero(); }
 };
 
 class PoseTGT{
@@ -87,7 +105,7 @@ class PoseTGT{
     bool is_contact, go_contact;
     PoseTGT(){ clear(); }
     ~PoseTGT(){}
-    inline void clear(){ abs.clear(); offs.clear(); cnt.clear(); w=hrp::dvector6::Zero(); is_contact = go_contact = false;}
+    void clear(){ abs.clear(); offs.clear(); cnt.clear(); w=hrp::dvector6::Zero(); is_contact = go_contact = false;}
 };
 
 class HumanPose : UTIL_CONST {
@@ -95,16 +113,16 @@ class HumanPose : UTIL_CONST {
     std::vector<PoseTGT> tgt;
     HumanPose(){ tgt.resize(num_pose_tgt); }
     ~HumanPose(){cerr<<"HumanPose destructed"<<endl;}
-    inline void clear(){
+    void clear(){
       for(std::vector<PoseTGT>::iterator it = tgt.begin(); it != tgt.end(); it++){ it->clear(); }
     }
-    static inline void hp_printf(const HumanPose& in){
+    static void hp_printf(const HumanPose& in){
       const std::string pcatgt[] = {"c","rf","lf","rh","lh","hd","z"}, wcatgt[] = {"rw","lw"};
       for(int i=0;i<num_pose_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",pcatgt[i].c_str(),in.tgt[i].abs.p(X),in.tgt[i].abs.p(Y),in.tgt[i].abs.p(Z)); }
 //      for(int i=0;i<num_wrench_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",wcatgt[i].c_str(),in.w[i](fx),in.w[i](fy),in.w[i](fz),in.w[i](tx),in.w[i](ty),in.w[i](tz)); }
       printf("\n");
     }
-    inline void print(){ hp_printf(*this); }
+    void print(){ hp_printf(*this); }
 };
 
 class RobotConfig : UTIL_CONST {
@@ -129,7 +147,8 @@ class RobotConfig : UTIL_CONST {
     }
 };
 
-class HumanSynchronizer : UTIL_CONST {
+#include "../AutoBalancer/AutoBalancer.h"
+class WBMSCore : UTIL_CONST {
   private:
     double CNT_F_TH, h2r_ratio, tgt_h2r_ratio, HZ, DT;
     struct timeval t_calc_start, t_calc_end;
@@ -168,12 +187,16 @@ class HumanSynchronizer : UTIL_CONST {
       bool use_manipulability_limit;
     };
     struct WBMSparameters WBMSparam;
-//    enum { MODE_IDLE, MODE_COUNTDOWN, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE} mode, previous_mode;
-//    int mode, previous_mode;
-//    inline bool isRunning(){ return (mode==MODE_SYNC_TO_WBMS) || (mode==MODE_WBMS) || (mode==MODE_PAUSE) || (mode==MODE_SYNC_TO_IDLE) ;}
     bool is_initial_loop;
+    hrp::Vector3 cur_min_manip_direc;
+    double cur_manip_val;
 
-    HumanSynchronizer(const double& dt){
+    typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
+    fikPtr fik;
+    hrp::BodyPtr m_robot;
+
+
+    WBMSCore(const double& dt){
       tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
       DT = dt;
       HZ = (int)(1.0/DT);
@@ -241,35 +264,24 @@ class HumanSynchronizer : UTIL_CONST {
       is_initial_loop = true;
       cout<<"HumanSynchronizer constructed"<<endl;
     }
-    ~HumanSynchronizer(){
+    ~WBMSCore(){
       if(DEBUG)fclose(sr_log);
       if(DEBUG)fclose(cz_log);
       if(DEBUG)fclose(id_log);
       cout<<"HumanSynchronizer destructed"<<endl;
     }
-    inline double getRemainingCountDown() const { return countdown_sec; }
-//    inline bool isHumanSyncOn() const { return HumanSyncOn; }
-    inline double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6; }
-    inline void setTargetHumanToRobotRatio(const double tgt_h2r_r_in){ tgt_h2r_ratio = tgt_h2r_r_in; }
-    inline void setCurrentInputAsOffset(HumanPose& tgt){
+    double getRemainingCountDown() const { return countdown_sec; }
+//    bool isHumanSyncOn() const { return HumanSyncOn; }
+    double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6; }
+    void setTargetHumanToRobotRatio(const double tgt_h2r_r_in){ tgt_h2r_ratio = tgt_h2r_r_in; }
+    void setCurrentInputAsOffset(HumanPose& tgt){
       std::string ns[7] = {"com","rf","lf","rh","lh","zmp","head"};
       for(int i=0;i<7;i++){
         tgt.tgt[i].offs.p   =  tgt.tgt[i].abs.p;
         tgt.tgt[i].offs.rpy =  tgt.tgt[i].abs.rpy;
       }
     }
-//    inline void updateCountDown(){
-//      if(countdown_sec>0.0){
-//        countdown_sec -= DT;
-//      }else if(countdown_sec <= 0){
-////        HumanSyncOn = true;
-////        startCountdownForHumanSync = false;
-////        mode = MODE_SYNC_TO_WBMS;
-//        cout<<"updateCountDown Finish"<<endl;
-//        is_initial_loop = true;
-//      }
-//    }
-    inline void update(){//////////  メインループ  ////////////
+    void update(){//////////  メインループ  ////////////
       gettimeofday(&t_calc_start, NULL);
       if(loop!=0){is_initial_loop = false;}
       updateParams                        ();
@@ -291,12 +303,11 @@ class HumanSynchronizer : UTIL_CONST {
       judgeFootLandOnCommandByFootForce   (hp_wld_raw);//人体足裏反力から各足の接地指令を生成
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
       limitEEWorkspace                    (rp_ref_out);
+      limitManipulability                  (rp_ref_out);
       setFootContactPoseByGoContact       (rp_ref_out);
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
       setFootRotHorizontalIfGoLanding     (rp_ref_out);
 
-//      applyVelLimit                       (rp_ref_out, rp_old_ref_Vel, rp_ref_out);
-
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
 
       Vector3ToVector2(rp_ref_out.tgt[com].abs.p,com_forcp_ref);
@@ -321,40 +332,40 @@ class HumanSynchronizer : UTIL_CONST {
       loop++;
       gettimeofday(&t_calc_end, NULL);
     }
-    static inline void Vector3ToVector2(const hrp::Vector3& in, hrp::Vector2& out){ out(X) = in(X); out(Y) = in(Y);}
-    static inline void Vector2ToVector3(const hrp::Vector2& in, hrp::Vector3& out){ out(X) = in(X); out(Y) = in(Y);}
-    static inline void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out(X) = in.x; out(Y) = in.y; out(Z) = in.z;}
-    static inline void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out(X) = in.r; out(Y) = in.p; out(Z) = in.y;}
-    static inline void Vector3ToPoint3D(const hrp::Vector3& in, RTC::Point3D& out){ out.x = in(X); out.y = in(Y); out.z = in(Z);}
-    static inline void Vector3ToOriantation3D(const hrp::Vector3& in, RTC::Orientation3D& out){ out.r = in(X); out.p = in(Y); out.y = in(Z);}
-    static inline void Pose3DToWBMSPose3D(const RTC::Pose3D& in, WBMSPose3D& out){ Point3DToVector3(in.position,out.p); Oriantation3DToVector3(in.orientation,out.rpy); }
-    static inline void WBMSPose3DToPose3D(const WBMSPose3D& in, RTC::Pose3D& out){ Vector3ToPoint3D(in.p,out.position); Vector3ToOriantation3D(in.rpy,out.orientation); }
-    static inline void DoubleSeqToVector6(const RTC::TimedDoubleSeq::_data_seq& in, hrp::dvector6& out){
+    static void Vector3ToVector2(const hrp::Vector3& in, hrp::Vector2& out){ out(X) = in(X); out(Y) = in(Y);}
+    static void Vector2ToVector3(const hrp::Vector2& in, hrp::Vector3& out){ out(X) = in(X); out(Y) = in(Y);}
+    static void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out(X) = in.x; out(Y) = in.y; out(Z) = in.z;}
+    static void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out(X) = in.r; out(Y) = in.p; out(Z) = in.y;}
+    static void Vector3ToPoint3D(const hrp::Vector3& in, RTC::Point3D& out){ out.x = in(X); out.y = in(Y); out.z = in(Z);}
+    static void Vector3ToOriantation3D(const hrp::Vector3& in, RTC::Orientation3D& out){ out.r = in(X); out.p = in(Y); out.y = in(Z);}
+    static void Pose3DToWBMSPose3D(const RTC::Pose3D& in, WBMSPose3D& out){ Point3DToVector3(in.position,out.p); Oriantation3DToVector3(in.orientation,out.rpy); }
+    static void WBMSPose3DToPose3D(const WBMSPose3D& in, RTC::Pose3D& out){ Vector3ToPoint3D(in.p,out.position); Vector3ToOriantation3D(in.rpy,out.orientation); }
+    static void DoubleSeqToVector6(const RTC::TimedDoubleSeq::_data_seq& in, hrp::dvector6& out){
       if(in.length() == 6){out(fx)=in[0]; out(fy)=in[1]; out(fz)=in[2]; out(tz)=in[3]; out(ty)=in[4]; out(tz)=in[5];
       }else{ std::cerr<<"[WARN] HumanPose::hrp::dvector6 DoubleSeqTohrp::dvector6() invalid data length"<<std::endl; out = hrp::dvector6::Zero(); }
     }
-    static inline void Vector6ToDoubleSeq(const hrp::dvector6& in, RTC::TimedDoubleSeq::_data_seq& out){
+    static void Vector6ToDoubleSeq(const hrp::dvector6& in, RTC::TimedDoubleSeq::_data_seq& out){
       if(out.length() == 6){out[0]=in(fx); out[1]=in(fy); out[2]=in(fz); out[3]=in(tx); out[4]=in(ty); out[5]=in(tz);
       }else{ std::cerr<<"[WARN] HumanPose::hrp::dvector6 DoubleSeqTohrp::dvector6() invalid data length"<<std::endl; }
     }
-    static inline double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(X)*b(Y)-a(Y)*b(X); }
+    static double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(X)*b(Y)-a(Y)*b(X); }
 
   private:
-    inline void updateParams(){
+    void updateParams(){
       updateHumanToRobotRatio(tgt_h2r_ratio);
     }
-    inline void updateHumanToRobotRatio(const double h2r_r_goal){//h2r_ratioに伴って変わる変数の処理もここに書く
+    void updateHumanToRobotRatio(const double h2r_r_goal){//h2r_ratioに伴って変わる変数の処理もここに書く
       const double step = 0.001;
       if(h2r_r_goal - h2r_ratio > step){h2r_ratio += step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
       else if(h2r_r_goal - h2r_ratio < (-1)*step){h2r_ratio -= step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
       else{h2r_ratio = h2r_r_goal;}
     }
-    inline void autoLRSwapCheck(const HumanPose& in, HumanPose& out){
+    void autoLRSwapCheck(const HumanPose& in, HumanPose& out){
       out = in;
       if(in.tgt[rh].offs.p(Y) > in.tgt[lh].offs.p(Y) ){ out.tgt[rh] = in.tgt[lh]; out.tgt[lh] = in.tgt[rh]; }
       if(in.tgt[rf].offs.p(Y) > in.tgt[lf].offs.p(Y) ){ out.tgt[rf] = in.tgt[lf]; out.tgt[lf] = in.tgt[rf]; }
     }
-    inline void convertRelHumanPoseToRelRobotPose(const HumanPose& in, HumanPose& out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
+    void convertRelHumanPoseToRelRobotPose(const HumanPose& in, HumanPose& out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
 //      out = in;//ダメゼッタイ
       for(int i=0, l[5]={com,rf,lf,rh,lh}; i<5; i++){
         out.tgt[l[i]].abs.p   =  h2r_ratio * (in.tgt[l[i]].abs.p - in.tgt[l[i]].offs.p)   + out.tgt[l[i]].offs.p;
@@ -367,17 +378,17 @@ class HumanSynchronizer : UTIL_CONST {
       out.tgt[head].abs = in.tgt[head].abs;
       out.tgt[zmp].abs = in.tgt[zmp].abs;//最近使わない
     }
-    inline void judgeFootLandOnCommandByFootForce(HumanPose& in){
+    void judgeFootLandOnCommandByFootForce(HumanPose& in){
       for(int i=0, l[LR]={rf,lf}; i<LR; i++){
         if      (in.tgt[l[i]].is_contact  && in.tgt[l[i]].w(fz)<CNT_F_TH   ){in.tgt[l[i]].is_contact = false;}//足ついた状態から上げる
         else if (!in.tgt[l[i]].is_contact && in.tgt[l[i]].w(fz)>CNT_F_TH+30){in.tgt[l[i]].is_contact = true;}//足浮いた状態から下げる
       }
     }
-    inline void applyLPFilter(HumanPose& tgt){
+    void applyLPFilter(HumanPose& tgt){
       if(is_initial_loop){
         for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
-          tgt_pos_filters[l[i]].init(tgt.tgt[l[i]].abs.p);
-          tgt_rot_filters[l[i]].init(tgt.tgt[l[i]].abs.rpy);
+          tgt_pos_filters[l[i]].reset(tgt.tgt[l[i]].abs.p);
+          tgt_rot_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy);
         }
       }
       for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
@@ -385,14 +396,14 @@ class HumanSynchronizer : UTIL_CONST {
         tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
       }
     }
-    inline void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
+    void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
       hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);
       hrp::Vector2 rf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[rf].abs.p(X),old.tgt[rf].abs.p(Y));
       hrp::Vector2 lf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[lf].abs.p(X),old.tgt[lf].abs.p(Y));
       if( inside_vec_rf.dot(rf2zmp) / inside_vec_rf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ out.tgt[lf].go_contact = true; }
       if( inside_vec_lf.dot(lf2zmp) / inside_vec_lf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ out.tgt[rf].go_contact = true; }
     }
-    inline void limitEEWorkspace(HumanPose& out){
+    void limitEEWorkspace(HumanPose& out){
 //      const double MAX_FW = 0.25;
       const double MAX_FW = 1000000;//manipulability test
       const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
@@ -441,7 +452,54 @@ class HumanSynchronizer : UTIL_CONST {
       if(!WBMSparam.use_head)out.tgt[head].abs.rpy = hrp::Vector3::Zero();
 //      if(mode!=MODE_WBMS)out.tgt[head].abs.rpy = hrp::Vector3::Zero();//頭は動かしてない時は0
     }
-    inline void setFootContactPoseByGoContact(HumanPose& out){
+    void limitManipulability(HumanPose& out){
+
+
+//      fik->ikp["rleg"].target_r0 = hrp::rotFromRpy(out.tgt[rf].abs.rpy);
+//      fik->ikp["rleg"].target_p0 = out.tgt[rf].abs.p;
+//
+//      for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
+//          if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
+//      }
+//
+//      hrp::dmatrix J,Jinv,Jnull;
+//      fik->ikp["rleg"].manip->calcJacobian(J);
+//      fik->ikp["rleg"].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
+//
+//      hrp::dmatrix manipulability_mat = J*J.transpose();
+//      double manipulability = sqrt((J*J.transpose()).determinant());
+//      Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
+//      Eigen::MatrixXd::Index row,col;
+//      cur_manip_val = svd.singularValues().minCoeff(&row,&col);
+//      cur_min_manip_direc = svd.matrixU().col(row);// unit vector
+//      if(cur_min_manip_direc.dot(fik->ikp["rleg"].target_p0 - m_robot->rootLink()->p) < 0){ cur_min_manip_direc *= -1; }//向きみて反転(常に外側正)
+
+//      static hrp::Vector3 old_ref;
+//
+//      hrp::Vector3 ref_ee_vel = (out.tgt[rf].abs.p - rp_ref_out_old.tgt[rf].abs.p) / DT;
+//      hrp::Vector3 ref_ee_vel_mod = ref_ee_vel;
+//      hrp::Vector3 ref_ee_vel_v = (ref_ee_vel.dot(cur_min_manip_direc)) * cur_min_manip_direc;
+//      hrp::Vector3 ref_ee_vel_h = ref_ee_vel - ref_ee_vel_v;
+//      double min_manip_th = 0.1;
+//      double penalty = (cur_manip_val - min_manip_th) * 10;
+//      LIMIT_MINMAX(penalty, -1.0, 1.0);//
+//      double max_dec_vel = 1.0;
+//      max_dec_vel *= penalty;
+//      cout<<"cur_manip_val"<<cur_manip_val<<endl;
+//      cout<<"penalty"<<penalty<<endl;
+//      if(ref_ee_vel_v.norm() > max_dec_vel){
+//        ref_ee_vel_v = ref_ee_vel_v.normalized() * max_dec_vel;
+//      }
+////      cout<<"ref_ee_vel_v"<<ref_ee_vel_v.transpose()<<endl;
+////      if(cur_manip_val<min_manip_th){
+////        ref_ee_vel_v = - cur_min_manip_direc * (min_manip_th - cur_manip_val) * 5;
+////      }
+//      ref_ee_vel_mod = ref_ee_vel_h + ref_ee_vel_v;
+//      out.tgt[rf].abs.p = rp_ref_out_old.tgt[rf].abs.p + ref_ee_vel_mod * DT;
+
+
+    }
+    void setFootContactPoseByGoContact(HumanPose& out){
       for(int i=0, l[LR]={rf,lf}; i<LR; i++){
         if(out.tgt[l[i]].go_contact){
           out.tgt[l[i]].abs.p(X) = out.tgt[l[i]].cnt.p(X);
@@ -452,21 +510,24 @@ class HumanSynchronizer : UTIL_CONST {
           out.tgt[l[i]].cnt.p(Y) = out.tgt[l[i]].abs.p(Y);
           out.tgt[l[i]].cnt.rpy(y) = out.tgt[l[i]].abs.rpy(y);
         }
-        if(out.tgt[l[i]].go_contact)rp_ref_out.tgt[l[i]].abs.p(Z) = rp_ref_out.tgt[l[i]].offs.p(Z);
-        if(!out.tgt[l[i]].go_contact)rp_ref_out.tgt[l[i]].abs.p(Z) += WBMSparam.swing_foot_height_offset;
+        if(out.tgt[l[i]].go_contact){
+          out.tgt[l[i]].abs.p(Z) = out.tgt[l[i]].offs.p(Z);
+        }else{
+          LIMIT_MIN( out.tgt[l[i]].abs.p(Z), out.tgt[l[i]].offs.p(Z)+WBMSparam.swing_foot_height_offset);
+        }
       }
     }
-    inline void setFootRotHorizontalIfGoLanding(HumanPose& out){
+    void setFootRotHorizontalIfGoLanding(HumanPose& out){
       if( out.tgt[rf].go_contact){ out.tgt[rf].abs.rpy(r) = out.tgt[rf].abs.rpy(p) = 0; }
       if( out.tgt[lf].go_contact){ out.tgt[lf].abs.rpy(r) = out.tgt[lf].abs.rpy(p) = 0; }
     }
-    inline void overwriteFootZFromFootLandOnCommand(HumanPose& out){
+    void overwriteFootZFromFootLandOnCommand(HumanPose& out){
       const double com2rf_dist = fabs((double)out.tgt[rf].abs.p(Y) - (double)out.tgt[com].abs.p(Y));
       const double com2lf_dist = fabs((double)out.tgt[lf].abs.p(Y) - (double)out.tgt[com].abs.p(Y));
       limitGroundContactVelocity(out.tgt[rf].go_contact, com2rf_dist, rp_ref_out_old.tgt[rf], out.tgt[rf], out.tgt[rf].is_contact);
       limitGroundContactVelocity(out.tgt[lf].go_contact, com2lf_dist, rp_ref_out_old.tgt[lf], out.tgt[lf], out.tgt[lf].is_contact);
     }
-    inline void limitGroundContactVelocity(const bool& go_land_in, const double& com2foot_dist, const PoseTGT& f_old_in, PoseTGT& f_in_out, bool& is_f_contact_out){
+    void limitGroundContactVelocity(const bool& go_land_in, const double& com2foot_dist, const PoseTGT& f_old_in, PoseTGT& f_in_out, bool& is_f_contact_out){
       double penalty = 1.0 + (0.2 - com2foot_dist) * 5.0;//1.0 ~ 2.0 本当はCPに比例すべき？
       LIMIT_MINMAX( penalty, 1.0, 2.0);
       LIMIT_MIN( f_in_out.abs.p(Z), f_in_out.offs.p(Z));
@@ -481,11 +542,11 @@ class HumanSynchronizer : UTIL_CONST {
       const double contact_threshold = 0.005;
       is_f_contact_out = ((f_in_out.abs.p(Z)-f_in_out.offs.p(Z)) <= contact_threshold);
     }
-    inline void modifyFootRotAndXYForContact(HumanPose& out){
+    void modifyFootRotAndXYForContact(HumanPose& out){
       calcFootRotAndXYTransitionForContact(out.tgt[rf],rf_vert);
       calcFootRotAndXYTransitionForContact(out.tgt[lf],lf_vert);
     }
-    inline void calcFootRotAndXYTransitionForContact(PoseTGT& foot_in, const std::vector<hrp::Vector3>& act_sole_size){
+    void calcFootRotAndXYTransitionForContact(PoseTGT& foot_in, const std::vector<hrp::Vector3>& act_sole_size){
       std::vector<hrp::Vector3> act_foot_vert;
       double min_height = foot_in.abs.p(Z);
       int min_height_id = 0;
@@ -501,7 +562,7 @@ class HumanSynchronizer : UTIL_CONST {
         foot_in.abs.p(Z) += foot_in.offs.p(Z) - act_foot_vert[min_height_id](Z);
       }
     }
-    inline bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
+    bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
       hull_com.clear();
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull_com);
       hrp::Vector2 cog(comin_abs(X),comin_abs(Y));
@@ -510,7 +571,7 @@ class HumanSynchronizer : UTIL_CONST {
       comin_abs(Y) = cog(Y);
       return true;
     }
-    inline bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans_old, hrp::Vector3& com_ans){
+    bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans_old, hrp::Vector3& com_ans){
       com_ans = com_in;
       if (is_initial_loop)com_ans_old = com_in;
       hrp::Vector3 com_vel = (com_in - com_ans_old)/DT;
@@ -538,7 +599,7 @@ class HumanSynchronizer : UTIL_CONST {
       }
       return true;
     }
-    inline void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull_d, const std::vector<hrp::Vector2>& hull_a, hrp::Vector2& com_vel_ans){
+    void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull_d, const std::vector<hrp::Vector2>& hull_a, hrp::Vector2& com_vel_ans){
       hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
       com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
 
@@ -585,7 +646,7 @@ class HumanSynchronizer : UTIL_CONST {
       }
       com_vel_ans_old = com_vel_ans;
     }
-    inline void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
+    void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
       rflf_points.clear();
       hull_ans.clear();
       for(int i=0;i<2;i++){
@@ -596,7 +657,7 @@ class HumanSynchronizer : UTIL_CONST {
       }
       makeConvexHullOpenCV(rflf_points, hull_ans);
     }
-    inline void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const hrp::dvector6& rfwin, const hrp::dvector6& lfwin, hrp::Vector3& zmp_ans){
+    void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const hrp::dvector6& rfwin, const hrp::dvector6& lfwin, hrp::Vector3& zmp_ans){
       hrp::Vector3 rfzmp,lfzmp;
       const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
       if( rfwin(fz) > 1.0e-6 ){
@@ -613,7 +674,7 @@ class HumanSynchronizer : UTIL_CONST {
       }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
       zmp_ans(2) = 0;
     }
-    inline void calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans){
+    void calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans){
       hrp::Vector4 margin_abs;
       hrp::Vector2 cross_pt, anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
       for(int direc=0;direc<4;direc++){
@@ -625,7 +686,7 @@ class HumanSynchronizer : UTIL_CONST {
       margin_ans(2) =  margin_abs(2);
       margin_ans(3) = -margin_abs(3);
     }
-    inline bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_will_cross){
+    bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_will_cross){
       hrp::Vector2 anchor_vec = pt_out_goal - pt_in_start;
       for(int i=0;i<hull.size();i++){
         int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
@@ -645,7 +706,7 @@ class HumanSynchronizer : UTIL_CONST {
       }
       return false;
     }
-    inline double calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_ans){
+    double calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_ans){
       double cur_nearest_dist, ans_nearest_dist;
       hrp::Vector2 cur_nearest_pt, ans_nearest_pt;
       for(int i=0;i<hull.size();i++){
@@ -673,7 +734,7 @@ class HumanSynchronizer : UTIL_CONST {
       pt_ans = ans_nearest_pt;
       return ans_nearest_dist;
     }
-    inline void makeConvexHullOpenCV(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
+    void makeConvexHullOpenCV(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
       points.clear();
       cvhull.clear();
       hull_ans.clear();
@@ -692,12 +753,12 @@ class HumanSynchronizer : UTIL_CONST {
     //  qh_freeqhull(!qh_ALL);
     //  //#define qh_ORIENTclock 1
     //}
-    inline bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull){
+    bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull){
       cvhull.clear();
       for(int i=0;i<hull.size();i++){ cvhull.push_back( cv::Point2f( hull[i](0), hull[i](1)) ); }
       return (cv::pointPolygonTest(cv::Mat(cvhull), cv::Point2f(pt(0),pt(1)), false) > 0);
     }
-    inline void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
+    void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
       comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
       comacc = acc4zmp_v_filters.passFilter(comacc);
       zmpout(X) = comin(X)-(H_cur/G)*comacc(X);
@@ -705,7 +766,7 @@ class HumanSynchronizer : UTIL_CONST {
       com_oldold = com_old;
       com_old = comin;
     }
-    inline void applyVelLimit(const HumanPose& out_old, HumanPose& out){
+    void applyVelLimit(const HumanPose& out_old, HumanPose& out){
       const double EE_MAX_VEL = 2.0;// m/s
       for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
         hrp::Vector3 diff = out.tgt[i].abs.p  - out_old.tgt[i].abs.p;

From 417fbcb17978a933c69b7e30b9dc399a5a3e1a12 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 1 Jun 2017 14:55:27 +0900
Subject: [PATCH 094/217] manipulability tune

---
 .../WholeBodyMasterSlave.cpp                  | 206 +++++++++---------
 .../WholeBodyMasterSlave.h                    |   8 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          |  78 +++++--
 3 files changed, 166 insertions(+), 126 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 28dcc664629..e3976621333 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -277,9 +277,9 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 //        calcManipulabilityJointLimitForWBMS(fik, m_robot);
 
         if(hsp->WBMSparam.is_doctor){
-          processWholeBodyMasterSlave(fik, m_robot);//安全制限つきマスタ・スレーブ
+          processWholeBodyMasterSlave(fik, m_robot, hsp->rp_ref_out);//安全制限つきマスタ・スレーブ
         }else{
-          processWholeBodyMasterSlave_Raw(fik, m_robot);//生マスタ・スレーブ
+          processWholeBodyMasterSlave_Raw(fik, m_robot, raw_pose);//生マスタ・スレーブ
         }
 
 
@@ -359,32 +359,32 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
 #ifdef USE_DEBUG_PORT
     // dbg plot
-     m_htcom.tm = m_qRef.tm;
+     m_htcom_dbg.tm = m_qRef.tm;
      WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[com].abs, m_htcom_dbg.data);
      m_htcom_dbgOut.write();
-     m_htrf.tm = m_qRef.tm;
+     m_htrf_dbg.tm = m_qRef.tm;
      WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[rf].abs, m_htrf_dbg.data);
      m_htrf_dbgOut.write();
-     m_htlf.tm = m_qRef.tm;
+     m_htlf_dbg.tm = m_qRef.tm;
      WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[lf].abs, m_htlf_dbg.data);
      m_htlf_dbgOut.write();
-     m_htrh.tm = m_qRef.tm;
+     m_htrh_dbg.tm = m_qRef.tm;
      WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[rh].abs, m_htrh_dbg.data);
      m_htrh_dbgOut.write();
-     m_htlh.tm = m_qRef.tm;
+     m_htlh_dbg.tm = m_qRef.tm;
      WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[lh].abs, m_htlh_dbg.data);
      m_htlh_dbgOut.write();
-     m_hthead.tm = m_qRef.tm;
+     m_hthead_dbg.tm = m_qRef.tm;
      WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[head].abs, m_hthead_dbg.data);
      m_hthead_dbgOut.write();
-     m_htzmp.tm = m_qRef.tm;
+     m_htzmp_dbg.tm = m_qRef.tm;
      WBMSCore::Vector3ToPoint3D(hsp->rp_ref_out.tgt[zmp].abs.p,m_rpzmp_dbg.data);
      m_htzmp_dbgOut.write();
-     m_htrfw.tm = m_qRef.tm;
+     m_htrfw_dbg.tm = m_qRef.tm;
      m_htrfw_dbg.data.length(6);
      WBMSCore::Vector6ToDoubleSeq(hsp->hp_plot.tgt[rf].w, m_htrfw_dbg.data);
      m_htrfw_dbgOut.write();
-     m_htlfw.tm = m_qRef.tm;
+     m_htlfw_dbg.tm = m_qRef.tm;
      m_htlfw_dbg.data.length(6);
      WBMSCore::Vector6ToDoubleSeq(hsp->hp_plot.tgt[lf].w, m_htlfw_dbg.data);
      m_htlfw_dbgOut.write();
@@ -475,9 +475,13 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
   const int human_l_names[4] = {rf,lf,rh,lh};
   for(int i=0;i<4;i++){//HumanSynchronizerの初期姿勢オフセットをセット
     if(fik_in->ikp.count(robot_l_names[i])){
-      hsp->rp_ref_out.tgt[human_l_names[i]].offs.p = fik_in->ikp[robot_l_names[i]].target_link->p + fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localPos;
-      hsp->rp_ref_out.tgt[human_l_names[i]].offs.rpy = hrp::rpyFromRot(fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localR);
+      hrp::Vector3 init_ee_p = fik_in->ikp[robot_l_names[i]].target_link->p + fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localPos;
+      hrp::Matrix33 init_ee_R = fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localR;
+      hsp->rp_ref_out.tgt[human_l_names[i]].offs.p = init_ee_p;
+      hsp->rp_ref_out.tgt[human_l_names[i]].offs.rpy = hrp::rpyFromRot(init_ee_R);
       hsp->rp_ref_out.tgt[human_l_names[i]].abs = hsp->rp_ref_out.tgt[human_l_names[i]].offs;
+      fik->ikp[robot_l_names[i]].target_p0 = fik_rmc->ikp[robot_l_names[i]].target_p0 = init_ee_p;
+      fik->ikp[robot_l_names[i]].target_r0 = fik_rmc->ikp[robot_l_names[i]].target_r0 = init_ee_R;
     }
   }
   hsp->rp_ref_out.tgt[com].offs.p = robot_in->calcCM();
@@ -491,15 +495,25 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
 }
 
 
-void WholeBodyMasterSlave::processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in){
+void WholeBodyMasterSlave::processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HumanPose& pose_ref){
   hsp->update();//////HumanSynchronizerの主要処理
   if(DEBUGP)cout<<"update():"<<hsp->getUpdateTime()<<endl;
-  if(DEBUGP)hsp->rp_ref_out.print();
-  solveFullbodyIKStrictCOM(fik_in, robot_in, hsp->rp_ref_out.tgt[com].abs, hsp->rp_ref_out.tgt[rf].abs, hsp->rp_ref_out.tgt[lf].abs, hsp->rp_ref_out.tgt[rh].abs, hsp->rp_ref_out.tgt[lh].abs, hsp->rp_ref_out.tgt[head].abs,"processWholeBodyMasterSlave");
+  if(DEBUGP)pose_ref.print();
+
+  WBMSPose3D rf_checked = pose_ref.tgt[rf].abs;
+  WBMSPose3D lf_checked = pose_ref.tgt[lf].abs;
+  WBMSPose3D rh_checked = pose_ref.tgt[rh].abs;
+  WBMSPose3D lh_checked = pose_ref.tgt[lh].abs;
+
+  static hrp::Vector3 target_v_old[4];
+  if(hsp->WBMSparam.use_manipulability_limit){
+    calcManipulabilityJointLimit(fik_in, robot_in, target_v_old, rf_checked, lf_checked, rh_checked, lh_checked);
+  }
+  solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, rf_checked, lf_checked, rh_checked, lh_checked, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave");
 }
 
 
-void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::BodyPtr& robot_in){
+void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::BodyPtr& robot_in, HumanPose& pose_ref){
   static BiquadIIRFilterVec pos_filters[num_pose_tgt], rot_filters[num_pose_tgt];
   static unsigned int callnum;
   for(int i=0;i<num_pose_tgt;i++){
@@ -509,41 +523,53 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::
       pos_filters[i].reset(hsp->hp_wld_raw.tgt[i].abs.p);
       rot_filters[i].reset(hsp->hp_wld_raw.tgt[i].abs.rpy);
     }
-    raw_pose.tgt[i].abs.p = pos_filters[i].passFilter(hsp->hp_wld_raw.tgt[i].abs.p);
-    raw_pose.tgt[i].abs.rpy = rot_filters[i].passFilter(hsp->hp_wld_raw.tgt[i].abs.rpy);
+    pose_ref.tgt[i].abs.p = pos_filters[i].passFilter(hsp->hp_wld_raw.tgt[i].abs.p);
+    pose_ref.tgt[i].abs.rpy = rot_filters[i].passFilter(hsp->hp_wld_raw.tgt[i].abs.rpy);
   }
   callnum++;
-  if(DEBUGP){ fprintf(stderr,"\x1b[31mmaster-mode:\x1b[39m"); raw_pose.print(); }
-  solveFullbodyIKStrictCOM(fik_in, robot_in, raw_pose.tgt[com].abs, raw_pose.tgt[rf].abs, raw_pose.tgt[lf].abs, raw_pose.tgt[rh].abs, raw_pose.tgt[lh].abs, raw_pose.tgt[head].abs,"processWholeBodyMasterSlave_Raw");
+  if(DEBUGP){ fprintf(stderr,"\x1b[31mmaster-mode:\x1b[39m"); pose_ref.print(); }
+  solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave_Raw");
+}
+
+
+void WholeBodyMasterSlave::calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
+  hrp::Vector3 direc( SGN(pos_tgt(X)-pos_cur(X)), SGN(pos_tgt(Y)-pos_cur(Y)), SGN(pos_tgt(Z)-pos_cur(Z)));
+  hrp::Vector3 vel_ans = vel_cur + direc * max_acc * m_dt;
+  for(int i=0;i<XYZ;i++){
+    double stop_safe_vel = sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
+    if( direc(i) > 0 ){
+      if(vel_ans(i) > stop_safe_vel){ vel_ans(i) = stop_safe_vel; }
+    }else{
+      if(vel_ans(i) < - stop_safe_vel){ vel_ans(i) = - stop_safe_vel; }
+    }
+  }
+  pos_ans = pos_cur + vel_ans * m_dt;
 }
 
 //これ超計算重い
-void WholeBodyMasterSlave::calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref){
+void WholeBodyMasterSlave::calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::Vector3 target_v_old[], WBMSPose3D& rf_ref, WBMSPose3D& lf_ref, WBMSPose3D& rh_ref, WBMSPose3D& lh_ref){
   const std::string names[4] = {"rleg","lleg","rarm","larm"};
-  const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
+  WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
   double min_manip_val = 1e9;
-  const double min_manip_th = 0.05;
-  hrp::Vector3 target_p0_old[4];
-
-  static bool firstcall = true;
+  const double min_manip_th = 0.1;
+  hrp::Vector3 target_p0_cur[4], target_p0_new[4];
   hrp::dmatrix J,Jinv,Jnull;
+
   for(int i=0; i<4; i++){
     fik_in->ikp[names[i]].manip->calcJacobian(J);
     fik_in->ikp[names[i]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
 //    hrp::dmatrix manipulability_mat = J*J.transpose();
 //    double manipulability = sqrt((J*J.transpose()).determinant());
     Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
-
     Eigen::MatrixXd::Index row,col;
     min_manip_val = svd.singularValues().minCoeff(&row,&col);
     hrp::Vector3 min_manip_direc = svd.matrixU().col(row);// unit vector
     if(min_manip_direc.dot(fik_in->ikp[names[i]].target_p0 - robot_in->rootLink()->p) < 0){ min_manip_direc *= -1; }//向きみて反転(常に外側正)
+//    target_p0_cur[i] = fik_in->ikp[names[i]].target_p0;
+    target_p0_cur[i] = fik_in->ikp[names[i]].target_link->p + fik_in->ikp[names[i]].target_link->R * fik_in->ikp[names[i]].localPos;
+    target_p0_new[i] = refs[i]->p;
 
-//    target_p0_old[i] = fik_in->ikp[names[i]].target_p0;
-    target_p0_old[i] = fik_in->ikp[names[i]].target_link->p + fik_in->ikp[names[i]].target_link->R * fik_in->ikp[names[i]].localPos;
-    fik_in->ikp[names[i]].target_p0 = refs[i]->p;
-
-    hrp::Vector3 ref_ee_vel = (fik_in->ikp[names[i]].target_p0 - target_p0_old[i]) / m_dt * 0.01;
+    hrp::Vector3 ref_ee_vel = (target_p0_new[i] - target_p0_cur[i]) / m_dt;
 
     double penalty = (min_manip_val - min_manip_th) * 10;
     LIMIT_MINMAX(penalty, -1.0, 1.0);//
@@ -554,73 +580,50 @@ void WholeBodyMasterSlave::calcManipulabilityJointLimit(fikPtr& fik_in, hrp::Bod
     static double ulim_mergin_old[6], llim_mergin_old[6];
     const double jlim_start_th = 30*D2R;
 
-//      for(int j=0;j<6;j++){
-//        llim_mergin[j] = fabs(robot_in->joint(j)->llimit - robot_in->joint(j)->q);
-//        ulim_mergin[j] = fabs(robot_in->joint(j)->ulimit - robot_in->joint(j)->q);
-//        if( llim_mergin[j] < jlim_start_th && (llim_mergin[j] - llim_mergin_old[j]) < 0){
-//          lim_penalty_tmp =  llim_mergin[j]/jlim_start_th;
-//          if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
-//        }
-//        if( ulim_mergin[j] < jlim_start_th && (ulim_mergin[j] - ulim_mergin_old[j]) < 0){
-//          lim_penalty_tmp =  ulim_mergin[j]/jlim_start_th;
-//          if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
-//        }
-//        llim_mergin_old[j] = llim_mergin[j];
-//        ulim_mergin_old[j] = ulim_mergin[j];
-//      }
-
-//    hrp::dvector q_vel(6);
-//    q_vel = Jinv.block(0,0,Jinv.cols(),3) * ref_ee_vel;
-//    for(int j=0;j<6;j++){
-//      llim_mergin[j] = fabs(robot_in->joint(j)->llimit - robot_in->joint(j)->q);
-//      ulim_mergin[j] = fabs(robot_in->joint(j)->ulimit - robot_in->joint(j)->q);
-//      if( llim_mergin[j] < jlim_start_th && q_vel[j] < 0){
-//        if(DEBUGP)cout<<"llim"<<j<<": "<<endl;
-//        lim_penalty_tmp =  llim_mergin[j]/jlim_start_th;
-//        if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
-//      }
-//      if( ulim_mergin[j] < jlim_start_th && q_vel[j] > 0){
-//        if(DEBUGP)cout<<"ulim"<<j<<": "<<endl;
-//        lim_penalty_tmp =  ulim_mergin[j]/jlim_start_th;
-//        if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
-//      }
-//    }
-
-    if(DEBUGP){
-      if(lim_penalty<1.0)cout<<"lim_penalty: "<<lim_penalty<<endl;
+    hrp::dvector q_vel(6);
+    q_vel = Jinv.block(0,0,Jinv.cols(),3) * ref_ee_vel;
+    for(int j=0;j<6;j++){
+      llim_mergin[j] = fabs(robot_in->joint(j)->llimit - robot_in->joint(j)->q);
+      ulim_mergin[j] = fabs(robot_in->joint(j)->ulimit - robot_in->joint(j)->q);
+      if( llim_mergin[j] < jlim_start_th && q_vel[j] < 0){
+        lim_penalty_tmp =  llim_mergin[j]/jlim_start_th;
+        if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
+      }
+      if( ulim_mergin[j] < jlim_start_th && q_vel[j] > 0){
+        lim_penalty_tmp =  ulim_mergin[j]/jlim_start_th;
+        if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
+      }
     }
+//    if(DEBUGP){ if(lim_penalty<1.0)cout<<"lim_penalty: "<<lim_penalty<<endl; }
 
-    double max_dec_vel = 1.0, max_acc_vel = 1.0;// m/s
+    double max_dec_vel = 1.0;// m/s
     hrp::Vector3 ref_ee_vel_v = (ref_ee_vel.dot(min_manip_direc)) * min_manip_direc;
     hrp::Vector3 ref_ee_vel_h = ref_ee_vel - ref_ee_vel_v;
 //        ref_vec_mod -= (ref_vec.dot(min_manip_direc)) * min_manip_direc;
 //        ref_vec_mod += (ref_vec.dot(min_manip_direc)) * min_manip_direc * penalty;
 
     max_dec_vel *= penalty;
-    max_acc_vel *= penalty;
-//      LIMIT_MIN(max_acc_vel, 0.0);//
-//      max_acc_vel += 0.01;
-    if(ref_ee_vel_v.dot(min_manip_direc) > max_dec_vel){
-      ref_ee_vel_v = min_manip_direc * max_dec_vel;
-    }
-//      if(ref_ee_vel_v.dot(-min_manip_direc) > max_acc_vel){
-//        ref_ee_vel_v = ref_ee_vel_v.normalized() * max_acc_vel;
-//      }
-//    LIMIT_MIN(max_dec_vel, 0.01);
-//    if(ref_ee_vel_v.norm() > max_dec_vel){
-//      ref_ee_vel_v = ref_ee_vel_v.normalized() * max_dec_vel;
+
+//    if(ref_ee_vel_v.dot(min_manip_direc) > max_dec_vel){
+//      ref_ee_vel_v = min_manip_direc * max_dec_vel;
+//    }
+//    if( (min_manip_val - min_manip_th) <0 ){
+//      ref_ee_vel_v = min_manip_direc * (min_manip_val - min_manip_th) * 10;
 //    }
 
     hrp::Vector3 ref_ee_vel_mod = ref_ee_vel_h + ref_ee_vel_v;//速度空間で一次フィルターしたほうがいい?
-    ref_ee_vel_mod *= lim_penalty;
-    if(DEBUGP)std::cout << names[i]<<" min_manip_direc: " << min_manip_direc.transpose() << " min_manip_val " << min_manip_val << std::endl;
+//    ref_ee_vel_mod *= lim_penalty;
 
-    fik_in->ikp[names[i]].target_p0 = target_p0_old[i] + ref_ee_vel_mod * m_dt;
-    fik_in->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);
+//    penalty *= lim_penalty;
+    double max_acc = 2.0 * penalty;
+    LIMIT_MINMAX(max_acc, 0.1, 1.0);//
+    if(DEBUGP)std::cout << names[i]<<": direc: " << min_manip_direc.transpose() << " min_manip_val: " << min_manip_val << " penalty: "<<penalty<<" max_acc: "<<max_acc<<std::endl;
+    calcVelAccSafeTrajectory(target_p0_cur[i], target_v_old[i], (target_p0_cur[i] + ref_ee_vel_mod * m_dt), max_acc, 1.0, target_p0_new[i]);
 
-//    target_p0_old[i] = fik_in->ikp[names[i]].target_p0;
+    refs[i]->p = target_p0_new[i];
+//    refs[i]->rpy = refs[i]->rpy;
+    target_v_old[i] = (target_p0_new[i] - target_p0_cur[i] )/m_dt;
   }
-  firstcall = false;
 }
 
 
@@ -665,26 +668,19 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   const double COM_IK_MAX_ERROR = 1e-5;//1e-4だと乱れる
   const std::string names[4] = {"rleg","lleg","rarm","larm"};
   const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
-
-  if(hsp->WBMSparam.use_manipulability_limit){
-    calcManipulabilityJointLimit(fik_in, robot_in, rf_ref, lf_ref, rh_ref, lh_ref);
-  }else{
-    for(int i=0;i<4;i++){
-      if(fik_in->ikp.count(names[i])){
-        fik_in->ikp[names[i]].target_p0 = refs[i]->p;
-        fik_in->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);
-      }
-    }
-  }
 //  robot_in->rootLink()->p = 0.01*com_ref.p + 0.99*robot_in->rootLink()->p;//ベースリンク位置が迷走するのを防ぐ？
   robot_in->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
-
+  for(int i=0;i<4;i++){
+    if(fik_in->ikp.count(names[i])){
+      fik_in->ikp[names[i]].target_p0 = refs[i]->p;
+      fik_in->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);
+    }
+  }
 //  fik_in->storeCurrentParameters();
 //  fik_in->setReferenceJointAngles();//これ入れると腕ブワーなる
   if(robot_in->link("HEAD_JOINT0") != NULL)robot_in->joint(15)->q = head_ref.rpy(y);
   if(robot_in->link("HEAD_JOINT1") != NULL)robot_in->joint(16)->q = head_ref.rpy(p);
   hrp::Vector3 tmp_com_err = hrp::Vector3::Zero();
-
   while( 1 ){  //COM 収束ループ
     com_ik_loop++;
     robot_in->rootLink()->p += tmp_com_err;
@@ -719,7 +715,17 @@ void WholeBodyMasterSlave::processMomentumCompensation(fikPtr& fik_in, hrp::Body
   }
   WBMSPose3D com_mod = pose_ref.tgt[com].abs;
   com_mod.rpy += torso_rot;
-  solveFullbodyIKStrictCOM(fik_in, robot_in, com_mod, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"calcDynamicsFilterCompensation");
+
+  WBMSPose3D rf_checked = pose_ref.tgt[rf].abs;
+  WBMSPose3D lf_checked = pose_ref.tgt[lf].abs;
+  WBMSPose3D rh_checked = pose_ref.tgt[rh].abs;
+  WBMSPose3D lh_checked = pose_ref.tgt[lh].abs;
+
+  static hrp::Vector3 target_v_old[4];
+  if(hsp->WBMSparam.use_manipulability_limit){
+    calcManipulabilityJointLimit(fik_in, robot_in, target_v_old, rf_checked, lf_checked, rh_checked, lh_checked);
+  }
+  solveFullbodyIKStrictCOM(fik_in, robot_in, com_mod, rf_checked, lf_checked, rh_checked, lh_checked, pose_ref.tgt[head].abs,"calcDynamicsFilterCompensation");
 }
 
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index db7ea08a71e..0a655921400 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -175,15 +175,17 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
   bool isRunning(){ return (mode==MODE_SYNC_TO_WBMS) || (mode==MODE_WBMS) || (mode==MODE_PAUSE) || (mode==MODE_SYNC_TO_IDLE) ;}
 
   void setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in);
-  void calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref);
+  void calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::Vector3 target_v_old[], WBMSPose3D& rf_ref, WBMSPose3D& lf_ref, WBMSPose3D& rh_ref, WBMSPose3D& lh_ref);
   void calcManipulabilityJointLimitForWBMS(fikPtr& fik_in, hrp::BodyPtr& robot_in);
   void solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix="");
   void processTransition();
   void preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in);
-  void processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in);
-  void processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::BodyPtr& robot_in);
+  void processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HumanPose& pose_ref);
+  void processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::BodyPtr& robot_in, HumanPose& pose_ref);
   void processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_normal_in, const HumanPose& pose_ref);
   bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; };
+
+  void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans);
 };
 
 extern "C"
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 7c3952d0626..e8d4cbaadca 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -40,6 +40,7 @@ extern "C" {
 #define LIMIT_MIN(x,min) (x= ( x<min ? min:x ))
 #define LIMIT_MAX(x,max) (x= ( x<max ? x:max ))
 #define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
+#define SGN(x) ((x)>=0 ? 1 : -1)
 
 class UTIL_CONST{
   public:
@@ -57,9 +58,6 @@ class UTIL_CONST{
       INFMIN( - std::numeric_limits<double>::max()),
       INFMAX( + std::numeric_limits<double>::max())
     {};
-//    static bool LIMIT_MIN(double& x, const double min){ if(x<min){x=min; return true;}else{return false;} }
-//    static bool LIMIT_MAX(double& x, const double max){ if(x>max){x=max; return true;}else{return false;} }
-//    static bool LIMIT_MINMAX(double& x, const double min, const double max){ assert( min <=  max); if(x<min){x=min; return true;} else if(x>max){x=max; return true;} else{return false;} }
 };
 
 class BiquadIIRFilterVec : UTIL_CONST {
@@ -76,20 +74,6 @@ class BiquadIIRFilterVec : UTIL_CONST {
     void reset(const hrp::Vector3& initial_input){ for(int i=0;i<XYZ;i++){ filters[i].reset((double)initial_input(i));} }
 };
 
-//class PDFilter{
-//  public:
-//    PDFilter(){};
-//    ~PDFilter(){};
-//    void setParameter(const double& pgain, const double& dgain, const double& HZ){
-//
-//    }
-//    void passFilter(){
-//
-//    }
-//};
-
-
-
 class WBMSPose3D{
   public:
     hrp::Vector3 p,rpy;
@@ -116,13 +100,13 @@ class HumanPose : UTIL_CONST {
     void clear(){
       for(std::vector<PoseTGT>::iterator it = tgt.begin(); it != tgt.end(); it++){ it->clear(); }
     }
-    static void hp_printf(const HumanPose& in){
+    static void hp_printf(const HumanPose& in) {
       const std::string pcatgt[] = {"c","rf","lf","rh","lh","hd","z"}, wcatgt[] = {"rw","lw"};
       for(int i=0;i<num_pose_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",pcatgt[i].c_str(),in.tgt[i].abs.p(X),in.tgt[i].abs.p(Y),in.tgt[i].abs.p(Z)); }
 //      for(int i=0;i<num_wrench_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",wcatgt[i].c_str(),in.w[i](fx),in.w[i](fy),in.w[i](fz),in.w[i](tx),in.w[i](ty),in.w[i](tz)); }
       printf("\n");
     }
-    void print(){ hp_printf(*this); }
+    void print() const { hp_printf(*this); }
 };
 
 class RobotConfig : UTIL_CONST {
@@ -168,7 +152,7 @@ class WBMSCore : UTIL_CONST {
     FILE *sr_log, *cz_log, *id_log;
     WBMSPose3D baselinkpose;
     WBMSPose3D ee_contact_pose[4];
-    hrp::Vector3 com_vel_old, cp_dec, cp_acc;
+    hrp::Vector3 com_vel_old,rh_vel_old, cp_dec, cp_acc;
     std::vector<hrp::Vector3> rf_vert,lf_vert;
     hrp::Vector2 com_forcp_ref,com_vel_forcp_ref;
     hrp::dvector6 invdyn_ft;
@@ -210,6 +194,7 @@ class WBMSCore : UTIL_CONST {
       com_old = com_oldold = comacc = hrp::Vector3::Zero();
       r_zmp_raw = hrp::Vector3::Zero();
       com_vel_old = hrp::Vector3::Zero();
+      rh_vel_old = hrp::Vector3::Zero();
       cp_dec = cp_acc = hrp::Vector3::Zero();
       invdyn_ft = hrp::dvector6::Zero();
       tgt_pos_filters.resize(num_pose_tgt);
@@ -218,7 +203,7 @@ class WBMSCore : UTIL_CONST {
       for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//四肢拘束点用(Rotation)
       tgt_pos_filters[com].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//重心pos用
       tgt_rot_filters[com].setParameter(0.6, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//重心rot用
-      tgt_pos_filters[rf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//右足pos用
+      tgt_pos_filters[rf].setParameter(hrp::Vector3(10.0,10.0,10.0), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//右足pos用
       tgt_pos_filters[lf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//左足pos用
       calcacc_v_filters.setParameter(5, HZ, BiquadIIRFilterVec::Q_BUTTERWORTH);//加速度計算用
       acc4zmp_v_filters.setParameter(5, HZ, BiquadIIRFilterVec::Q_BUTTERWORTH);//ZMP生成用ほぼこの値でいい
@@ -327,6 +312,7 @@ class WBMSCore : UTIL_CONST {
 
       H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[rf].abs.p(Z));
       com_vel_old = (rp_ref_out.tgt[com].abs.p - rp_ref_out_old.tgt[com].abs.p)/DT;
+      rh_vel_old = (rp_ref_out.tgt[rh].abs.p - rp_ref_out_old.tgt[rh].abs.p)/DT;
       rp_ref_out_old = rp_ref_out;
 //      previous_mode = mode;
       loop++;
@@ -384,17 +370,63 @@ class WBMSCore : UTIL_CONST {
         else if (!in.tgt[l[i]].is_contact && in.tgt[l[i]].w(fz)>CNT_F_TH+30){in.tgt[l[i]].is_contact = true;}//足浮いた状態から下げる
       }
     }
+//    void applyLPFilter(HumanPose& tgt){
+//      if(is_initial_loop){
+//        for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
+//          tgt_pos_filters[l[i]].reset(tgt.tgt[l[i]].abs.p);
+//          tgt_rot_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy);
+//        }
+//      }
+//      for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
+//        tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
+//        tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
+//      }
+//    }
     void applyLPFilter(HumanPose& tgt){
       if(is_initial_loop){
-        for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
+        for(int i=0, l[1]={com}; i<1; i++){
           tgt_pos_filters[l[i]].reset(tgt.tgt[l[i]].abs.p);
           tgt_rot_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy);
         }
       }
-      for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
+      for(int i=0, l[1]={com}; i<1; i++){
         tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
         tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
       }
+//      for(int i=0, l[1]={com}; i<1; i++){
+//        calcVelAccSafeTrajectory_val(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, 1.0, tgt.tgt[l[i]].abs.p);
+//        calcVelAccSafeTrajectory_val(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+//      }
+      for(int i=0, l[5]={rf,lf,rh,lh,head}; i<5; i++){
+        calcVelAccSafeTrajectory(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, 1.0, tgt.tgt[l[i]].abs.p);
+        calcVelAccSafeTrajectory(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+      }
+    }
+    void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
+      hrp::Vector3 direc( SGN(pos_tgt(X)-pos_cur(X)), SGN(pos_tgt(Y)-pos_cur(Y)), SGN(pos_tgt(Z)-pos_cur(Z)));
+      hrp::Vector3 vel_ans = vel_cur + direc * max_acc * DT;
+      for(int i=0;i<XYZ;i++){
+        double stop_safe_vel = sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
+        if( direc(i) > 0 ){
+          if(vel_ans(i) > stop_safe_vel){ vel_ans(i) = stop_safe_vel; }
+        }else{
+          if(vel_ans(i) < - stop_safe_vel){ vel_ans(i) = - stop_safe_vel; }
+        }
+      }
+      pos_ans = pos_cur + vel_ans * DT;
+    }
+    void calcVelAccSafeTrajectory_val(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
+      hrp::Vector3 direc( SGN(pos_tgt(X)-pos_cur(X)), SGN(pos_tgt(Y)-pos_cur(Y)), SGN(pos_tgt(Z)-pos_cur(Z)));
+      hrp::Vector3 vel_ans = vel_cur + (pos_tgt-pos_cur) * 10 * max_acc * DT;
+      for(int i=0;i<XYZ;i++){
+        double stop_safe_vel = sqrt( 2 * (pos_tgt-pos_cur).norm() * 10 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
+        if( direc(i) > 0 ){
+          if(vel_ans(i) > stop_safe_vel){ vel_ans(i) = stop_safe_vel; }
+        }else{
+          if(vel_ans(i) < - stop_safe_vel){ vel_ans(i) = - stop_safe_vel; }
+        }
+      }
+      pos_ans = pos_cur + vel_ans * DT;
     }
     void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
       hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);

From c6339a43960c40ca32bdb23e6b25e35cc999d6d5 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 2 Jun 2017 03:35:57 +0900
Subject: [PATCH 095/217] test pre IK manip limit

---
 .../WholeBodyMasterSlave.cpp                  | 127 ++++++++++--------
 .../WholeBodyMasterSlave.h                    |  10 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          |  31 +++--
 3 files changed, 100 insertions(+), 68 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index e3976621333..de471dcc3d9 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -129,6 +129,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
 
     m_robot = hrp::BodyPtr(new hrp::Body());
     m_robot_rmc = hrp::BodyPtr(new hrp::Body());
+    m_robot_ml = hrp::BodyPtr(new hrp::Body());
     RTC::Manager& rtcManager = RTC::Manager::instance();
     std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
     int comPos = nameServer.find(",");
@@ -143,6 +144,10 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
       std::cerr << "[" << m_profile.instance_name << "] failed to load model 2 [" << prop["model"] << "]" << std::endl;
       return RTC::RTC_ERROR;
     }
+    if (!loadBodyFromModelLoader(m_robot_ml, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
+      std::cerr << "[" << m_profile.instance_name << "] failed to load model 2 [" << prop["model"] << "]" << std::endl;
+      return RTC::RTC_ERROR;
+    }
     // allocate memory for outPorts
     m_qRef.data.length(m_robot->numJoints());
     m_htrfw.data.length(6);
@@ -160,10 +165,14 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     // Generate FIK
     fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
     fik_rmc = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot_rmc, std::string(m_profile.instance_name), m_dt));
-
-    ik_robot_list.push_back(std::make_pair( fik,      m_robot     ));
-    ik_robot_list.push_back(std::make_pair( fik_rmc,  m_robot_rmc ));
-    for(int i=0;i<ik_robot_list.size();i++){ setupfik(ik_robot_list[i].first, ik_robot_list[i].second, prop); }
+    fik_ml = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot_ml, std::string(m_profile.instance_name), m_dt));
+    fik_list.push_back(fik);
+    fik_list.push_back(fik_rmc);
+    fik_list.push_back(fik_ml);
+    body_list.push_back(m_robot);
+    body_list.push_back(m_robot_rmc);
+    body_list.push_back(m_robot_ml);
+    for(int i=0;i<fik_list.size();i++){ setupfik(fik_list[i], body_list[i], prop); }
 
     if (fik->ikp.find("rleg") != fik->ikp.end() && fik->ikp.find("lleg") != fik->ikp.end()) {
       is_legged_robot = true;
@@ -192,7 +201,7 @@ void WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC:
       coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
       coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
       ABCIKparam tp;
-      for (size_t j = 0; j < 3; j++) {
+      for (size_t j = 0; j < XYZ; j++) {
           coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
       }
       double tmpv[4];
@@ -260,21 +269,27 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
       if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"processTransition:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl; t_calc_start = t_calc_end;}
 
-      fik->current_tm = fik_rmc->current_tm = m_qRef.tm;
-      fik->ikp["rleg"].is_ik_enable = fik_rmc->ikp["rleg"].is_ik_enable = true;
-      fik->ikp["lleg"].is_ik_enable = fik_rmc->ikp["lleg"].is_ik_enable = true;
-      fik->ikp["rarm"].is_ik_enable = fik_rmc->ikp["rarm"].is_ik_enable = hsp->WBMSparam.use_rh;
-      fik->ikp["larm"].is_ik_enable = fik_rmc->ikp["larm"].is_ik_enable = hsp->WBMSparam.use_lh;
-//      fik->ikp["rarm"].is_ik_enable = fik_rmc->ikp["rarm"].is_ik_enable = false;
-//      fik->ikp["larm"].is_ik_enable = fik_rmc->ikp["larm"].is_ik_enable = false;
+      for(int i=0; i<fik_list.size(); i++){
+        fik_list[i]->current_tm = m_qRef.tm;
+        fik_list[i]->ikp["rleg"].is_ik_enable = true;
+        fik_list[i]->ikp["lleg"].is_ik_enable = true;
+        fik_list[i]->ikp["rarm"].is_ik_enable = hsp->WBMSparam.use_rh;
+        fik_list[i]->ikp["larm"].is_ik_enable = hsp->WBMSparam.use_lh;
+      }
 
       if (isRunning()) {
         if(hsp->is_initial_loop){
           preProcessForWholeBodyMasterSlave(fik, m_robot);
         }
 
-
-//        calcManipulabilityJointLimitForWBMS(fik, m_robot);
+        m_robot_ml->rootLink()->p = m_robot_rmc->rootLink()->p;
+        m_robot_ml->rootLink()->R = m_robot_rmc->rootLink()->R;
+//        for (int i=0; i<m_robot_ml->numJoints(); i++){ m_robot_ml->joint(i)->q = m_robot_rmc->joint(i)->q;}
+//        m_robot_ml->rootLink()->p = m_robot->rootLink()->p;
+//        m_robot_ml->rootLink()->R = m_robot->rootLink()->R;
+//        for (int i=0; i<m_robot_ml->numJoints(); i++){ m_robot_ml->joint(i)->q = m_robot->joint(i)->q;}
+//        m_robot_ml->calcForwardKinematics();
+        calcManipulabilityJointLimitForWBMS(fik_ml, m_robot_ml);
 
         if(hsp->WBMSparam.is_doctor){
           processWholeBodyMasterSlave(fik, m_robot, hsp->rp_ref_out);//安全制限つきマスタ・スレーブ
@@ -420,7 +435,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
      WBMSCore::Vector6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
      m_invdyn_dbgOut.write();
 #endif
-     if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"t_last:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl; t_calc_start = t_calc_end;}
+     if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"t_last:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6<<endl; t_calc_start = t_calc_end;}
     loop ++;
     return RTC::RTC_OK;
 }
@@ -463,12 +478,12 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
     init_foot_mid_coord(Z) = 0;
     std::cerr<<"["<<m_profile.instance_name<<"] Input basePos height is invalid. Auto modify "<<m_basePos.data.z<<" -> "<<basePos_heightChecked(Z)<<endl;
   }
-  for(int i=0;i<ik_robot_list.size();i++){//初期姿勢でBodyをFK
-    ik_robot_list[i].second->rootLink()->p = basePos_heightChecked;
-    ik_robot_list[i].second->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
-    for ( int j = 0; j < ik_robot_list[i].second->numJoints(); j++ ){ ik_robot_list[i].second->joint(j)->q = m_qRef.data[j]; }
-    ik_robot_list[i].second->calcForwardKinematics();
-    ik_robot_list[i].first->setReferenceJointAngles();
+  for(int i=0;i<fik_list.size();i++){//初期姿勢でBodyをFK
+    body_list[i]->rootLink()->p = basePos_heightChecked;
+    body_list[i]->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
+    for ( int j = 0; j < body_list[i]->numJoints(); j++ ){ body_list[i]->joint(j)->q = m_qRef.data[j]; }
+    body_list[i]->calcForwardKinematics();
+    fik_list[i]->setReferenceJointAngles();
   }
   hsp->setCurrentInputAsOffset(hsp->hp_wld_raw);//現在の入力を人間の初期姿勢としてセット
   const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
@@ -532,11 +547,12 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::
 }
 
 
-void WholeBodyMasterSlave::calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
+void WholeBodyMasterSlave::calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const hrp::Vector3& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
   hrp::Vector3 direc( SGN(pos_tgt(X)-pos_cur(X)), SGN(pos_tgt(Y)-pos_cur(Y)), SGN(pos_tgt(Z)-pos_cur(Z)));
-  hrp::Vector3 vel_ans = vel_cur + direc * max_acc * m_dt;
+  hrp::Vector3 vel_ans;
+  for(int i=0;i<XYZ;i++){vel_ans(i) = vel_cur(i) + direc(i) * max_acc(i) * m_dt;}
   for(int i=0;i<XYZ;i++){
-    double stop_safe_vel = sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
+    double stop_safe_vel = sqrt( 2 * max_acc(i) * fabs(pos_tgt(i) - pos_cur(i)) );
     if( direc(i) > 0 ){
       if(vel_ans(i) > stop_safe_vel){ vel_ans(i) = stop_safe_vel; }
     }else{
@@ -555,24 +571,24 @@ void WholeBodyMasterSlave::calcManipulabilityJointLimit(fikPtr& fik_in, hrp::Bod
   hrp::Vector3 target_p0_cur[4], target_p0_new[4];
   hrp::dmatrix J,Jinv,Jnull;
 
-  for(int i=0; i<4; i++){
-    fik_in->ikp[names[i]].manip->calcJacobian(J);
-    fik_in->ikp[names[i]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
+  for(int l=0; l<4; l++){
+    fik_in->ikp[names[l]].manip->calcJacobian(J);
+    fik_in->ikp[names[l]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
 //    hrp::dmatrix manipulability_mat = J*J.transpose();
 //    double manipulability = sqrt((J*J.transpose()).determinant());
     Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
     Eigen::MatrixXd::Index row,col;
     min_manip_val = svd.singularValues().minCoeff(&row,&col);
     hrp::Vector3 min_manip_direc = svd.matrixU().col(row);// unit vector
-    if(min_manip_direc.dot(fik_in->ikp[names[i]].target_p0 - robot_in->rootLink()->p) < 0){ min_manip_direc *= -1; }//向きみて反転(常に外側正)
+    if(min_manip_direc.dot(fik_in->ikp[names[l]].target_p0 - robot_in->rootLink()->p) < 0){ min_manip_direc *= -1; }//向きみて反転(常に外側正)
 //    target_p0_cur[i] = fik_in->ikp[names[i]].target_p0;
-    target_p0_cur[i] = fik_in->ikp[names[i]].target_link->p + fik_in->ikp[names[i]].target_link->R * fik_in->ikp[names[i]].localPos;
-    target_p0_new[i] = refs[i]->p;
+    target_p0_cur[l] = fik_in->ikp[names[l]].target_link->p + fik_in->ikp[names[l]].target_link->R * fik_in->ikp[names[l]].localPos;
+    target_p0_new[l] = refs[l]->p;
 
-    hrp::Vector3 ref_ee_vel = (target_p0_new[i] - target_p0_cur[i]) / m_dt;
+    hrp::Vector3 ref_ee_vel = (target_p0_new[l] - target_p0_cur[l]) / m_dt;
 
-    double penalty = (min_manip_val - min_manip_th) * 10;
-    LIMIT_MINMAX(penalty, -1.0, 1.0);//
+    double penalty = 1 - (min_manip_val - min_manip_th) * 10;
+    LIMIT_MINMAX(penalty, 0.0, 1.0);//0~1に頭打ち　//thより大で0, thに近づくにつれ0->1, th以下でも1
 
     double lim_penalty = 1.0;
     double lim_penalty_tmp = 1.0;
@@ -595,34 +611,36 @@ void WholeBodyMasterSlave::calcManipulabilityJointLimit(fikPtr& fik_in, hrp::Bod
       }
     }
 //    if(DEBUGP){ if(lim_penalty<1.0)cout<<"lim_penalty: "<<lim_penalty<<endl; }
-
-    double max_dec_vel = 1.0;// m/s
-    hrp::Vector3 ref_ee_vel_v = (ref_ee_vel.dot(min_manip_direc)) * min_manip_direc;
-    hrp::Vector3 ref_ee_vel_h = ref_ee_vel - ref_ee_vel_v;
+//    double max_dec_vel = 1.0;// m/s
+//    hrp::Vector3 ref_ee_vel_v = (ref_ee_vel.dot(min_manip_direc)) * min_manip_direc;
+//    hrp::Vector3 ref_ee_vel_h = ref_ee_vel - ref_ee_vel_v;
 //        ref_vec_mod -= (ref_vec.dot(min_manip_direc)) * min_manip_direc;
 //        ref_vec_mod += (ref_vec.dot(min_manip_direc)) * min_manip_direc * penalty;
-
-    max_dec_vel *= penalty;
-
+//    max_dec_vel *= penalty;
 //    if(ref_ee_vel_v.dot(min_manip_direc) > max_dec_vel){
 //      ref_ee_vel_v = min_manip_direc * max_dec_vel;
 //    }
 //    if( (min_manip_val - min_manip_th) <0 ){
 //      ref_ee_vel_v = min_manip_direc * (min_manip_val - min_manip_th) * 10;
 //    }
-
-    hrp::Vector3 ref_ee_vel_mod = ref_ee_vel_h + ref_ee_vel_v;//速度空間で一次フィルターしたほうがいい?
+//    hrp::Vector3 ref_ee_vel_mod = ref_ee_vel_h + ref_ee_vel_v;//速度空間で一次フィルターしたほうがいい?
 //    ref_ee_vel_mod *= lim_penalty;
-
 //    penalty *= lim_penalty;
-    double max_acc = 2.0 * penalty;
-    LIMIT_MINMAX(max_acc, 0.1, 1.0);//
-    if(DEBUGP)std::cout << names[i]<<": direc: " << min_manip_direc.transpose() << " min_manip_val: " << min_manip_val << " penalty: "<<penalty<<" max_acc: "<<max_acc<<std::endl;
-    calcVelAccSafeTrajectory(target_p0_cur[i], target_v_old[i], (target_p0_cur[i] + ref_ee_vel_mod * m_dt), max_acc, 1.0, target_p0_new[i]);
-
-    refs[i]->p = target_p0_new[i];
-//    refs[i]->rpy = refs[i]->rpy;
-    target_v_old[i] = (target_p0_new[i] - target_p0_cur[i] )/m_dt;
+    hrp::Vector3 max_acc_ratio = hrp::Vector3(1,1,1);
+
+//    LIMIT_MINMAX(penalty, 0.1, 1.0);
+    for(int i=0;i<3;i++){
+      max_acc_ratio(i) -= fabs(min_manip_direc(i)) * penalty;
+    }
+    hrp::Vector3 max_acc = max_acc_ratio * 2.0;
+    for(int i=0;i<XYZ;i++){ LIMIT_MIN(max_acc(i), 0.1); }
+
+    if(DEBUGP)std::cout << names[l]<<": direc: " << min_manip_direc.transpose() << " min_manip_val: " << min_manip_val << " penalty: "<<penalty<<" max_acc: "<<max_acc.transpose()<<std::endl;
+    calcVelAccSafeTrajectory(target_p0_cur[l], target_v_old[l], (target_p0_cur[l] + ref_ee_vel * m_dt), max_acc, 1.0, target_p0_new[l]);
+
+    refs[l]->p = target_p0_new[l];
+//    refs[l]->rpy = refs[l]->rpy;
+    target_v_old[l] = (target_p0_new[l] - target_p0_cur[l] )/m_dt;
   }
 }
 
@@ -654,8 +672,8 @@ void WholeBodyMasterSlave::calcManipulabilityJointLimitForWBMS(fikPtr& fik_in, h
 //    //      std::cout << "matrix V" << std::endl << svd.matrixV() << std::endl;
 //    if(min_manip_direc.dot(fik_in->ikp[names[i]].target_p0 - robot_in->rootLink()->p) < 0){ min_manip_direc *= -1; }//向きみて反転(常に外側正)
 //
-//    hsp->fik = fik_in;
-//    hsp->m_robot = robot_in;
+    hsp->fik = fik_in;
+    hsp->m_robot = robot_in;
 //    hsp->cur_min_manip_direc = min_manip_direc;
 //    hsp->cur_manip_val = min_manip_val;
 //  }
@@ -723,7 +741,10 @@ void WholeBodyMasterSlave::processMomentumCompensation(fikPtr& fik_in, hrp::Body
 
   static hrp::Vector3 target_v_old[4];
   if(hsp->WBMSparam.use_manipulability_limit){
+    struct timeval t_calc_start, t_calc_end;
+    if(TIMECALC)gettimeofday(&t_calc_start, NULL);
     calcManipulabilityJointLimit(fik_in, robot_in, target_v_old, rf_checked, lf_checked, rh_checked, lh_checked);
+    if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"calcManipulabilityJointLimit:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6<<endl; t_calc_start = t_calc_end;}
   }
   solveFullbodyIKStrictCOM(fik_in, robot_in, com_mod, rf_checked, lf_checked, rh_checked, lh_checked, pose_ref.tgt[head].abs,"calcDynamicsFilterCompensation");
 }
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 0a655921400..830cf42317f 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -144,12 +144,12 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
     bool is_active, has_toe_joint;
   };
   typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
-  fikPtr fik, fik_rmc;
+  fikPtr fik, fik_rmc, fik_ml;
+  hrp::BodyPtr m_robot, m_robot_rmc, m_robot_ml;
+  std::vector<fikPtr> fik_list;
+  std::vector<hrp::BodyPtr> body_list;
   std::map<std::string, size_t> contact_states_index_map;
   double m_dt;
-  hrp::BodyPtr m_robot, m_robot_rmc;
-  std::vector< std::pair<fikPtr, hrp::BodyPtr> > ik_robot_list;
-  static const enum ik_robot_usage{ basic, rmc } iru;
 
   double transition_interpolator_ratio;
   interpolator *transition_interpolator;
@@ -185,7 +185,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
   void processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_normal_in, const HumanPose& pose_ref);
   bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; };
 
-  void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans);
+  void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const hrp::Vector3& max_acc, const double& max_vel, hrp::Vector3& pos_ans);
 };
 
 extern "C"
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index e8d4cbaadca..7333d975a8e 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -282,17 +282,19 @@ class WBMSCore : UTIL_CONST {
         rp_ref_out_old = rp_ref_out;
         rp_ref_vel_old = rp_ref_out;
       }
-      applyVelLimit                       (rp_ref_vel_old, rp_ref_out);
+//      applyVelLimit                       (rp_ref_vel_old, rp_ref_out);
       rp_ref_vel_old = rp_ref_out;
       if(WBMSparam.set_com_height_fix)rp_ref_out.tgt[com].abs.p(Z) = rp_ref_out.tgt[com].offs.p(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
       judgeFootLandOnCommandByFootForce   (hp_wld_raw);//人体足裏反力から各足の接地指令を生成
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
       limitEEWorkspace                    (rp_ref_out);
-      limitManipulability                  (rp_ref_out);
+//      limitManipulability                 (rp_ref_out);
       setFootContactPoseByGoContact       (rp_ref_out);
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
       setFootRotHorizontalIfGoLanding     (rp_ref_out);
 
+      limitManipulability                 (rp_ref_out);
+
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
 
       Vector3ToVector2(rp_ref_out.tgt[com].abs.p,com_forcp_ref);
@@ -485,15 +487,23 @@ class WBMSCore : UTIL_CONST {
 //      if(mode!=MODE_WBMS)out.tgt[head].abs.rpy = hrp::Vector3::Zero();//頭は動かしてない時は0
     }
     void limitManipulability(HumanPose& out){
+      const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
+      const int human_l_names[4] = {rf,lf,rh,lh};
+      for(int i=0;i<4;i++){
+        if(fik->ikp.count(robot_l_names[i])){
+          fik->ikp[robot_l_names[i]].target_r0 = hrp::rotFromRpy(out.tgt[human_l_names[i]].abs.rpy);
+          fik->ikp[robot_l_names[i]].target_p0 = out.tgt[human_l_names[i]].abs.p;
+        }
+      }
+      for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
+          if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
+      }
+      for(int i=0;i<4;i++){
+        if(fik->ikp.count(robot_l_names[i])){
+          out.tgt[human_l_names[i]].abs.p = fik->ikp[robot_l_names[i]].target_link->p + fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localPos;
+        }
+      }
 
-
-//      fik->ikp["rleg"].target_r0 = hrp::rotFromRpy(out.tgt[rf].abs.rpy);
-//      fik->ikp["rleg"].target_p0 = out.tgt[rf].abs.p;
-//
-//      for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-//          if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
-//      }
-//
 //      hrp::dmatrix J,Jinv,Jnull;
 //      fik->ikp["rleg"].manip->calcJacobian(J);
 //      fik->ikp["rleg"].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
@@ -529,6 +539,7 @@ class WBMSCore : UTIL_CONST {
 //      ref_ee_vel_mod = ref_ee_vel_h + ref_ee_vel_v;
 //      out.tgt[rf].abs.p = rp_ref_out_old.tgt[rf].abs.p + ref_ee_vel_mod * DT;
 
+//      fik->revertRobotStateToCurrent();
 
     }
     void setFootContactPoseByGoContact(HumanPose& out){

From f8a267be574b1f1d5e27da29c85f9a1d853fd125 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 2 Jun 2017 15:13:13 +0900
Subject: [PATCH 096/217] fix BiquadIIRFilterVec::Q_BUTTERWORTH ->
 invdyn_zmp_filters.Q_BUTTERWORTH for hydro?

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index de471dcc3d9..21309bd5f59 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -181,8 +181,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     }
     hsp = boost::shared_ptr<WBMSCore>(new WBMSCore(m_dt));
 
-    invdyn_zmp_filters.setParameter(25, 1/m_dt, BiquadIIRFilterVec::Q_BUTTERWORTH);
-    invdyn_zmp_filters2.setParameter(25, 1/m_dt, BiquadIIRFilterVec::Q_BUTTERWORTH);
+    invdyn_zmp_filters.setParameter(25, 1/m_dt, invdyn_zmp_filters.Q_BUTTERWORTH);
+    invdyn_zmp_filters2.setParameter(25, 1/m_dt, invdyn_zmp_filters2.Q_BUTTERWORTH);
 
     mode = previous_mode = MODE_IDLE;
 

From b817d6e8a5a271f09d3bc6277ded95b48ed23a6a Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 2 Jun 2017 15:57:36 +0900
Subject: [PATCH 097/217] fix static const member problem

---
 .../WholeBodyMasterSlave.cpp                  |  8 +++---
 rtc/WholeBodyMasterSlave/wbms_core.h          | 28 ++++++++++---------
 2 files changed, 19 insertions(+), 17 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 21309bd5f59..5178e5ce2a6 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -181,8 +181,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     }
     hsp = boost::shared_ptr<WBMSCore>(new WBMSCore(m_dt));
 
-    invdyn_zmp_filters.setParameter(25, 1/m_dt, invdyn_zmp_filters.Q_BUTTERWORTH);
-    invdyn_zmp_filters2.setParameter(25, 1/m_dt, invdyn_zmp_filters2.Q_BUTTERWORTH);
+    invdyn_zmp_filters.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
+    invdyn_zmp_filters2.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
 
     mode = previous_mode = MODE_IDLE;
 
@@ -533,8 +533,8 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::
   static unsigned int callnum;
   for(int i=0;i<num_pose_tgt;i++){
     if(callnum == 0){
-      pos_filters[i].setParameter(100.0, 1.0/m_dt, BiquadIIRFilterVec::Q_NOOVERSHOOT);
-      rot_filters[i].setParameter(100.0, 1.0/m_dt, BiquadIIRFilterVec::Q_NOOVERSHOOT);
+      pos_filters[i].setParameter(100.0, 1.0/m_dt, Q_NOOVERSHOOT);
+      rot_filters[i].setParameter(100.0, 1.0/m_dt, Q_NOOVERSHOOT);
       pos_filters[i].reset(hsp->hp_wld_raw.tgt[i].abs.p);
       rot_filters[i].reset(hsp->hp_wld_raw.tgt[i].abs.rpy);
     }
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 7333d975a8e..5d8a68fd347 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -51,10 +51,12 @@ class UTIL_CONST{
     enum { r, p, y, rpy };
     enum { fx, fy, fz, tx, ty, tz, ft_xyz };
     enum { MIN, MAX, MINMAX };
-    double G, D2R, INFMIN,INFMAX;
+    double G, D2R, INFMIN, INFMAX, Q_BUTTERWORTH, Q_NOOVERSHOOT;
     UTIL_CONST() :
       G(9.80665),
       D2R( M_PI/180.0),
+      Q_BUTTERWORTH(0.707106781),
+      Q_NOOVERSHOOT(0.5),
       INFMIN( - std::numeric_limits<double>::max()),
       INFMAX( + std::numeric_limits<double>::max())
     {};
@@ -65,11 +67,11 @@ class BiquadIIRFilterVec : UTIL_CONST {
     IIRFilter filters[XYZ];
     hrp::Vector3 ans;
   public:
-    static const double Q_BUTTERWORTH = 0.707106781, Q_NOOVERSHOOT = 0.5;
-    BiquadIIRFilterVec(){}
+    BiquadIIRFilterVec()
+    {}
     ~BiquadIIRFilterVec(){}
-    void setParameter(const hrp::Vector3& fc_in, const double& HZ, const double& Q = Q_BUTTERWORTH){ for(int i=0;i<XYZ;i++){ filters[i].setParameterAsBiquad((double)fc_in(i), Q, HZ); } }
-    void setParameter(const double& fc_in, const double& HZ, const double& Q = Q_BUTTERWORTH){ setParameter(hrp::Vector3(fc_in,fc_in,fc_in), HZ, Q); }//overload
+    void setParameter(const hrp::Vector3& fc_in, const double& HZ, const double& Q = 0.5){ for(int i=0;i<XYZ;i++){ filters[i].setParameterAsBiquad((double)fc_in(i), Q, HZ); } }
+    void setParameter(const double& fc_in, const double& HZ, const double& Q = 0.5){ setParameter(hrp::Vector3(fc_in,fc_in,fc_in), HZ, Q); }//overload
     hrp::Vector3 passFilter(const hrp::Vector3& input){ for(int i=0;i<XYZ;i++){ ans(i) = filters[i].passFilter((double)input(i)); } return ans; }
     void reset(const hrp::Vector3& initial_input){ for(int i=0;i<XYZ;i++){ filters[i].reset((double)initial_input(i));} }
 };
@@ -199,14 +201,14 @@ class WBMSCore : UTIL_CONST {
       invdyn_ft = hrp::dvector6::Zero();
       tgt_pos_filters.resize(num_pose_tgt);
       tgt_rot_filters.resize(num_pose_tgt);
-      for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//四肢拘束点用(position)
-      for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//四肢拘束点用(Rotation)
-      tgt_pos_filters[com].setParameter(1.0, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//重心pos用
-      tgt_rot_filters[com].setParameter(0.6, HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//重心rot用
-      tgt_pos_filters[rf].setParameter(hrp::Vector3(10.0,10.0,10.0), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//右足pos用
-      tgt_pos_filters[lf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, BiquadIIRFilterVec::Q_NOOVERSHOOT);//左足pos用
-      calcacc_v_filters.setParameter(5, HZ, BiquadIIRFilterVec::Q_BUTTERWORTH);//加速度計算用
-      acc4zmp_v_filters.setParameter(5, HZ, BiquadIIRFilterVec::Q_BUTTERWORTH);//ZMP生成用ほぼこの値でいい
+      for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(1.0, HZ, Q_NOOVERSHOOT);//四肢拘束点用(position)
+      for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0, HZ, Q_NOOVERSHOOT);//四肢拘束点用(Rotation)
+      tgt_pos_filters[com].setParameter(1.0, HZ, Q_NOOVERSHOOT);//重心pos用
+      tgt_rot_filters[com].setParameter(0.6, HZ, Q_NOOVERSHOOT);//重心rot用
+      tgt_pos_filters[rf].setParameter(hrp::Vector3(10.0,10.0,10.0), HZ, Q_NOOVERSHOOT);//右足pos用
+      tgt_pos_filters[lf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, Q_NOOVERSHOOT);//左足pos用
+      calcacc_v_filters.setParameter(5, HZ, Q_BUTTERWORTH);//加速度計算用
+      acc4zmp_v_filters.setParameter(5, HZ, Q_BUTTERWORTH);//ZMP生成用ほぼこの値でいい
       com_in_filter.setParameter(1, HZ);
       rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
       lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);

From e3b4dac8f42ce8107a40f9327227882c052c44c0 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 6 Jun 2017 15:22:14 +0900
Subject: [PATCH 098/217] update mode -> mode class

---
 rtc/AutoBalancer/AutoBalancer.h               |   6 +
 .../WholeBodyMasterSlave.cpp                  | 173 +++++++-------
 .../WholeBodyMasterSlave.h                    |  47 +++-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 224 ++++++++++--------
 4 files changed, 258 insertions(+), 192 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index e4642f665d4..1c03e8e4433 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -373,6 +373,12 @@ class SimpleFullbodyInverseKinematicsSolver
       }
       std::cerr << "]" << std::endl;
     };
+    hrp::Vector3 getEEPos(const std::string& limb_name){
+      return ikp[limb_name].target_link->p + ikp[limb_name].target_link->R * ikp[limb_name].localPos;
+    }
+    hrp::Matrix33 getEERot(const std::string& limb_name){
+      return ikp[limb_name].target_link->R * ikp[limb_name].localR;
+    }
 };
 
 class AutoBalancer
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 5178e5ce2a6..06a0c1a63f6 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -184,8 +184,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     invdyn_zmp_filters.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
     invdyn_zmp_filters2.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
 
-    mode = previous_mode = MODE_IDLE;
-
     return RTC::RTC_OK;
 }
 
@@ -249,7 +247,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if (m_zmpIn.isNew()) { m_zmpIn.read(); }
     if (m_optionalDataIn.isNew()) { m_optionalDataIn.read(); }
     //for HumanSynchronizer
-    if(mode!=MODE_PAUSE){
+    if(mode.now()!=MODE_PAUSE){
       if (m_htrfwIn.isNew()){ m_htrfwIn.read(); WBMSCore::DoubleSeqToVector6(m_htrfw.data,hsp->hp_wld_raw.tgt[rf].w); }
       if (m_htlfwIn.isNew()){ m_htlfwIn.read(); WBMSCore::DoubleSeqToVector6(m_htlfw.data,hsp->hp_wld_raw.tgt[lf].w); }
       if (m_htcomIn.isNew()){ m_htcomIn.read(); WBMSCore::Pose3DToWBMSPose3D(m_htcom.data,hsp->hp_wld_raw.tgt[com].abs); }
@@ -265,9 +263,13 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if ( is_legged_robot ) {
 
       processTransition();
+      mode.update();
+      if(DEBUGP)dbg(mode.now());
+      struct timespec startT, endT;
+      clock_gettime(CLOCK_REALTIME, &startT);
 
+      if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processTransition" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
-      if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"processTransition:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl; t_calc_start = t_calc_end;}
 
       for(int i=0; i<fik_list.size(); i++){
         fik_list[i]->current_tm = m_qRef.tm;
@@ -277,19 +279,15 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         fik_list[i]->ikp["larm"].is_ik_enable = hsp->WBMSparam.use_lh;
       }
 
-      if (isRunning()) {
-        if(hsp->is_initial_loop){
+      if (mode.isRunning()) {
+        if(mode.isInitialize()){
           preProcessForWholeBodyMasterSlave(fik, m_robot);
         }
 
         m_robot_ml->rootLink()->p = m_robot_rmc->rootLink()->p;
         m_robot_ml->rootLink()->R = m_robot_rmc->rootLink()->R;
-//        for (int i=0; i<m_robot_ml->numJoints(); i++){ m_robot_ml->joint(i)->q = m_robot_rmc->joint(i)->q;}
-//        m_robot_ml->rootLink()->p = m_robot->rootLink()->p;
-//        m_robot_ml->rootLink()->R = m_robot->rootLink()->R;
-//        for (int i=0; i<m_robot_ml->numJoints(); i++){ m_robot_ml->joint(i)->q = m_robot->joint(i)->q;}
-//        m_robot_ml->calcForwardKinematics();
-        calcManipulabilityJointLimitForWBMS(fik_ml, m_robot_ml);
+        hsp->fik = fik_ml;
+        hsp->m_robot = m_robot_ml;
 
         if(hsp->WBMSparam.is_doctor){
           processWholeBodyMasterSlave(fik, m_robot, hsp->rp_ref_out);//安全制限つきマスタ・スレーブ
@@ -298,9 +296,10 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         }
 
 
-        if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"processWholeBodyMasterSlave:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl; t_calc_start = t_calc_end;}
+        if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processWholeBodyMasterSlave" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
+
         //逆動力学初期化
-        if(hsp->is_initial_loop){
+        if(mode.isInitialize()){
           idsb.setInitState(m_robot, m_dt);
           idsb2.setInitState(m_robot, m_dt);
         }
@@ -312,9 +311,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         ref_zmp_invdyn = invdyn_zmp_filters.passFilter(ref_zmp_invdyn);
         updateInvDynStateBuffer(idsb);
 
-
-
-        if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"calcWorldZMPFromInverseDynamics:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl; t_calc_start = t_calc_end;}
+        if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ calcWorldZMPFromInverseDynamics" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
         //角運動量補償
         if(hsp->WBMSparam.is_doctor){
@@ -322,17 +319,11 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         }else{
           processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, raw_pose);
         }
-//      calcAccelerationsForInverseDynamics(m_robot, idsb2);
-//      calcWorldZMPFromInverseDynamics(m_robot, idsb2, ref_zmp_invdyn2);
-//      updateInvDynStateBuffer(idsb2);
-
 
+        if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processMomentumCompensation" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
-        if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"processMomentumCompensation:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1000000.0<<endl; t_calc_start = t_calc_end;}
         //OutPortデータセット
-        //        ref_zmp = ref_zmp_invdyn;
         hrp::Vector3 ref_zmp = hsp->rp_ref_out.tgt[zmp].abs.p;
-//        hrp::BodyPtr m_robot_for_out = m_robot;
         hrp::BodyPtr m_robot_for_out = m_robot_rmc;
         // qRef
         for (int i = 0; i < m_qRef.data.length(); i++ ){ m_qRef.data[i] = transition_interpolator_ratio * m_robot_for_out->joint(i)->q  + (1 - transition_interpolator_ratio) * m_qRef.data[i]; }
@@ -364,7 +355,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
       hsp->baselinkpose.p = m_robot->rootLink()->p;
       hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
     }
-    previous_mode = mode;
     // write
     m_qOut.write();
     m_basePosOut.write();
@@ -442,23 +432,23 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
 
 void WholeBodyMasterSlave::processTransition(){
-  switch(mode){
+  switch(mode.now()){
 
     case MODE_SYNC_TO_WBMS:
-      if(previous_mode == MODE_IDLE){ double tmp = 1.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
+      if(mode.pre() == MODE_IDLE){ double tmp = 1.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
       if (!transition_interpolator->isEmpty() ){
         transition_interpolator->get(&transition_interpolator_ratio, true);
       }else{
-        mode = MODE_WBMS;
+        mode.setModeRequest(MODE_WBMS);
       }
       break;
 
     case MODE_SYNC_TO_IDLE:
-      if(previous_mode == MODE_WBMS || previous_mode == MODE_PAUSE){ double tmp = 0.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
+      if(mode.pre() == MODE_WBMS || mode.pre() == MODE_PAUSE){ double tmp = 0.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
       if (!transition_interpolator->isEmpty()) {
         transition_interpolator->get(&transition_interpolator_ratio, true);
       }else{
-        mode = MODE_IDLE;
+        mode.setModeRequest(MODE_IDLE);
       }
       break;
   }
@@ -470,43 +460,28 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
   robot_in->rootLink()->p = basePos_heightChecked;
   for ( int i = 0; i < robot_in->numJoints(); i++ ){ robot_in->joint(i)->q = m_qRef.data[i]; }
   robot_in->calcForwardKinematics();
-  hrp::Vector3 rsole_pos = fik_in->ikp["rleg"].target_link->p+ fik_in->ikp["rleg"].target_link->R * fik_in->ikp["rleg"].localPos;
-  hrp::Vector3 lsole_pos = fik_in->ikp["lleg"].target_link->p+ fik_in->ikp["lleg"].target_link->R * fik_in->ikp["lleg"].localPos;
-  hrp::Vector3 init_foot_mid_coord = (rsole_pos + lsole_pos) / 2;
+  hrp::Vector3 init_foot_mid_coord = (fik_in->getEEPos("rleg") + fik_in->getEEPos("lleg")) / 2;
   if( fabs((double)init_foot_mid_coord(Z)) > 1e-5 ){
     basePos_heightChecked(Z) -= init_foot_mid_coord(Z);
     init_foot_mid_coord(Z) = 0;
     std::cerr<<"["<<m_profile.instance_name<<"] Input basePos height is invalid. Auto modify "<<m_basePos.data.z<<" -> "<<basePos_heightChecked(Z)<<endl;
   }
+  const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
   for(int i=0;i<fik_list.size();i++){//初期姿勢でBodyをFK
     body_list[i]->rootLink()->p = basePos_heightChecked;
     body_list[i]->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
     for ( int j = 0; j < body_list[i]->numJoints(); j++ ){ body_list[i]->joint(j)->q = m_qRef.data[j]; }
     body_list[i]->calcForwardKinematics();
     fik_list[i]->setReferenceJointAngles();
-  }
-  hsp->setCurrentInputAsOffset(hsp->hp_wld_raw);//現在の入力を人間の初期姿勢としてセット
-  const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
-  const int human_l_names[4] = {rf,lf,rh,lh};
-  for(int i=0;i<4;i++){//HumanSynchronizerの初期姿勢オフセットをセット
-    if(fik_in->ikp.count(robot_l_names[i])){
-      hrp::Vector3 init_ee_p = fik_in->ikp[robot_l_names[i]].target_link->p + fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localPos;
-      hrp::Matrix33 init_ee_R = fik_in->ikp[robot_l_names[i]].target_link->R * fik_in->ikp[robot_l_names[i]].localR;
-      hsp->rp_ref_out.tgt[human_l_names[i]].offs.p = init_ee_p;
-      hsp->rp_ref_out.tgt[human_l_names[i]].offs.rpy = hrp::rpyFromRot(init_ee_R);
-      hsp->rp_ref_out.tgt[human_l_names[i]].abs = hsp->rp_ref_out.tgt[human_l_names[i]].offs;
-      fik->ikp[robot_l_names[i]].target_p0 = fik_rmc->ikp[robot_l_names[i]].target_p0 = init_ee_p;
-      fik->ikp[robot_l_names[i]].target_r0 = fik_rmc->ikp[robot_l_names[i]].target_r0 = init_ee_R;
+    for(int l=0;l<4;l++){//targetを初期化
+      if(fik_list[i]->ikp.count(robot_l_names[l])){
+        fik_list[i]->ikp[robot_l_names[l]].target_p0 = fik_list[i]->getEEPos(robot_l_names[l]);
+        fik_list[i]->ikp[robot_l_names[l]].target_r0 = fik_list[i]->getEERot(robot_l_names[l]);
+      }
     }
   }
-  hsp->rp_ref_out.tgt[com].offs.p = robot_in->calcCM();
-  hsp->rp_ref_out.tgt[zmp].offs.p(X) = hsp->rp_ref_out.tgt[com].offs.p(X);
-  hsp->rp_ref_out.tgt[zmp].offs.p(Y) = hsp->rp_ref_out.tgt[com].offs.p(Y);
-  hsp->rp_ref_out.tgt[zmp].offs.p(Z) = init_foot_mid_coord(Z);
-  hsp->rp_ref_out.tgt[rf].cnt = hsp->rp_ref_out.tgt[rf].offs;
-  hsp->rp_ref_out.tgt[lf].cnt = hsp->rp_ref_out.tgt[lf].offs;
-  hsp->baselinkpose.p = robot_in->rootLink()->p;
-  hsp->baselinkpose.rpy = hrp::rpyFromRot(robot_in->rootLink()->R);
+  torso_rot_rmc = hrp::Vector3::Zero();//RMCの補償量リセット
+  hsp->initializeRequest(fik_in, robot_in);
 }
 
 
@@ -547,20 +522,20 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::
 }
 
 
-void WholeBodyMasterSlave::calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const hrp::Vector3& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
-  hrp::Vector3 direc( SGN(pos_tgt(X)-pos_cur(X)), SGN(pos_tgt(Y)-pos_cur(Y)), SGN(pos_tgt(Z)-pos_cur(Z)));
-  hrp::Vector3 vel_ans;
-  for(int i=0;i<XYZ;i++){vel_ans(i) = vel_cur(i) + direc(i) * max_acc(i) * m_dt;}
-  for(int i=0;i<XYZ;i++){
-    double stop_safe_vel = sqrt( 2 * max_acc(i) * fabs(pos_tgt(i) - pos_cur(i)) );
-    if( direc(i) > 0 ){
-      if(vel_ans(i) > stop_safe_vel){ vel_ans(i) = stop_safe_vel; }
-    }else{
-      if(vel_ans(i) < - stop_safe_vel){ vel_ans(i) = - stop_safe_vel; }
-    }
-  }
-  pos_ans = pos_cur + vel_ans * m_dt;
-}
+//void WholeBodyMasterSlave::calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const hrp::Vector3& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
+//  hrp::Vector3 direc( SGN(pos_tgt(X)-pos_cur(X)), SGN(pos_tgt(Y)-pos_cur(Y)), SGN(pos_tgt(Z)-pos_cur(Z)));
+//  hrp::Vector3 vel_ans;
+//  for(int i=0;i<XYZ;i++){vel_ans(i) = vel_cur(i) + direc(i) * max_acc(i) * m_dt;}
+//  for(int i=0;i<XYZ;i++){
+//    double stop_safe_vel = sqrt( 2 * max_acc(i) * fabs(pos_tgt(i) - pos_cur(i)) );
+//    if( direc(i) > 0 ){
+//      if(vel_ans(i) > stop_safe_vel){ vel_ans(i) = stop_safe_vel; }
+//    }else{
+//      if(vel_ans(i) < - stop_safe_vel){ vel_ans(i) = - stop_safe_vel; }
+//    }
+//  }
+//  pos_ans = pos_cur + vel_ans * m_dt;
+//}
 
 //これ超計算重い
 void WholeBodyMasterSlave::calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::Vector3 target_v_old[], WBMSPose3D& rf_ref, WBMSPose3D& lf_ref, WBMSPose3D& rh_ref, WBMSPose3D& lh_ref){
@@ -636,7 +611,7 @@ void WholeBodyMasterSlave::calcManipulabilityJointLimit(fikPtr& fik_in, hrp::Bod
     for(int i=0;i<XYZ;i++){ LIMIT_MIN(max_acc(i), 0.1); }
 
     if(DEBUGP)std::cout << names[l]<<": direc: " << min_manip_direc.transpose() << " min_manip_val: " << min_manip_val << " penalty: "<<penalty<<" max_acc: "<<max_acc.transpose()<<std::endl;
-    calcVelAccSafeTrajectory(target_p0_cur[l], target_v_old[l], (target_p0_cur[l] + ref_ee_vel * m_dt), max_acc, 1.0, target_p0_new[l]);
+//    calcVelAccSafeTrajectory(target_p0_cur[l], target_v_old[l], (target_p0_cur[l] + ref_ee_vel * m_dt), max_acc, 1.0, target_p0_new[l]);
 
     refs[l]->p = target_p0_new[l];
 //    refs[l]->rpy = refs[l]->rpy;
@@ -672,8 +647,6 @@ void WholeBodyMasterSlave::calcManipulabilityJointLimitForWBMS(fikPtr& fik_in, h
 //    //      std::cout << "matrix V" << std::endl << svd.matrixV() << std::endl;
 //    if(min_manip_direc.dot(fik_in->ikp[names[i]].target_p0 - robot_in->rootLink()->p) < 0){ min_manip_direc *= -1; }//向きみて反転(常に外側正)
 //
-    hsp->fik = fik_in;
-    hsp->m_robot = robot_in;
 //    hsp->cur_min_manip_direc = min_manip_direc;
 //    hsp->cur_manip_val = min_manip_val;
 //  }
@@ -710,7 +683,6 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
     if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
   }
-
   if(com_ik_loop != 1 && DEBUGP)cout<<"com_ik_loop:"<<com_ik_loop<<" @ "<<debug_prefix<<endl;
 }
 
@@ -724,15 +696,14 @@ void WholeBodyMasterSlave::processMomentumCompensation(fikPtr& fik_in, hrp::Body
   robot_normal_in->calcCM();
   robot_normal_in->rootLink()->calcSubMassCM();
   hrp::Matrix33 I_com;
-  static hrp::Vector3 torso_rot;
   hrp::Vector3 torso_rot_vel;
-  if(robot_normal_in->link("CHEST_JOINT0")){
+  if(robot_normal_in->link("CHEST_JOINT0") != NULL){
     robot_normal_in->link("CHEST_JOINT0")->calcSubMassInertia(I_com);
     torso_rot_vel = - I_com.inverse() * L * hsp->WBMSparam.upper_body_rmc_ratio;
-    torso_rot += torso_rot_vel * m_dt;
+    torso_rot_rmc += torso_rot_vel * m_dt;
   }
   WBMSPose3D com_mod = pose_ref.tgt[com].abs;
-  com_mod.rpy += torso_rot;
+  com_mod.rpy += torso_rot_rmc;
 
   WBMSPose3D rf_checked = pose_ref.tgt[rf].abs;
   WBMSPose3D lf_checked = pose_ref.tgt[lf].abs;
@@ -750,7 +721,7 @@ void WholeBodyMasterSlave::processMomentumCompensation(fikPtr& fik_in, hrp::Body
 }
 
 
-bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double sec){
+bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double sec){//遅い
   if(sec >= 0.0 && sec <= 30.0){
     std::cerr << "[" << m_profile.instance_name << "] start Synchronization after "<<sec<<" [s]" << std::endl;
     double remained_usec = sec * 10e6;
@@ -768,32 +739,52 @@ bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double se
 
 
 bool WholeBodyMasterSlave::startWholeBodyMasterSlave(){
-  std::cerr << "[" << m_profile.instance_name << "] startWholeBodyMasterSlave" << std::endl;
-  mode = MODE_SYNC_TO_WBMS;
-  while(!transition_interpolator->isEmpty()){ usleep(1000); }
-  return true;
+  if(mode.now() == MODE_IDLE){
+    std::cerr << "[" << m_profile.instance_name << "] startWholeBodyMasterSlave" << std::endl;
+    mode.setModeRequest(MODE_SYNC_TO_WBMS);
+    while(!transition_interpolator->isEmpty()){ usleep(1000); }
+    return true;
+  }else{
+    std::cerr << "[" << m_profile.instance_name << "] Invalid context to startWholeBodyMasterSlave" << std::endl;
+    return false;
+  }
 }
 
 
 bool WholeBodyMasterSlave::pauseWholeBodyMasterSlave(){
-  std::cerr << "[" << m_profile.instance_name << "] pauseWholeBodyMasterSlave" << std::endl;
-  mode = MODE_PAUSE;
-  return true;
+  if(mode.now() == MODE_WBMS){
+    std::cerr << "[" << m_profile.instance_name << "] pauseWholeBodyMasterSlave" << std::endl;
+    mode.setModeRequest(MODE_PAUSE);
+    return true;
+  }else{
+    std::cerr << "[" << m_profile.instance_name << "] Invalid context to pauseWholeBodyMasterSlave" << std::endl;
+    return false;
+  }
 }
 
 
 bool WholeBodyMasterSlave::resumeWholeBodyMasterSlave(){
-  std::cerr << "[" << m_profile.instance_name << "] pauseWholeBodyMasterSlave" << std::endl;
-  mode = MODE_WBMS;
-  return true;
+  if(mode.now() == MODE_PAUSE){
+    std::cerr << "[" << m_profile.instance_name << "] pauseWholeBodyMasterSlave" << std::endl;
+    mode.setModeRequest(MODE_WBMS);
+    return true;
+  }else{
+    std::cerr << "[" << m_profile.instance_name << "] Invalid context to pauseWholeBodyMasterSlave" << std::endl;
+    return false;
+  }
 }
 
 
 bool WholeBodyMasterSlave::stopWholeBodyMasterSlave(){
-  std::cerr << "[" << m_profile.instance_name << "] stopWholeBodyMasterSlave" << std::endl;
-  mode = MODE_SYNC_TO_IDLE;
-  while(!transition_interpolator->isEmpty()){ usleep(1000); }
-  return true;
+  if(mode.now() == MODE_WBMS || mode.now() == MODE_PAUSE ){
+    std::cerr << "[" << m_profile.instance_name << "] stopWholeBodyMasterSlave" << std::endl;
+    mode.setModeRequest(MODE_SYNC_TO_IDLE);
+    while(!transition_interpolator->isEmpty()){ usleep(1000); }
+    return true;
+  }else{
+    std::cerr << "[" << m_profile.instance_name << "] Invalid context to stopWholeBodyMasterSlave" << std::endl;
+    return false;
+  }
 }
 
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 830cf42317f..55ad0d4cb40 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -36,6 +36,37 @@ using namespace RTC;
 
 #define USE_DEBUG_PORT
 
+enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE};
+
+class ControlMode{
+  private:
+    mode_enum current_mode, previous_mode, requested_mode;
+  public:
+    ControlMode(){ current_mode = previous_mode = requested_mode = MODE_IDLE;}
+    ~ControlMode(){}
+    bool setModeRequest(mode_enum in){
+      switch(in){
+        case MODE_SYNC_TO_WBMS:
+          if(current_mode == MODE_IDLE){ requested_mode = MODE_SYNC_TO_WBMS; return true; }else{ return false; }
+        case MODE_WBMS:
+          if(current_mode == MODE_SYNC_TO_WBMS || current_mode == MODE_PAUSE ){ requested_mode = MODE_WBMS; return true; }else{ return false; }
+        case MODE_PAUSE:
+          if(current_mode == MODE_WBMS){ requested_mode = MODE_PAUSE; return true; }else{ return false; }
+        case MODE_SYNC_TO_IDLE:
+          if(current_mode == MODE_WBMS || current_mode == MODE_PAUSE ){ requested_mode = MODE_SYNC_TO_IDLE; return true; }else{ return false; }
+        case MODE_IDLE:
+          if(current_mode == MODE_SYNC_TO_IDLE ){ requested_mode = MODE_IDLE; return true; }else{ return false; }
+        default:
+          return false;
+      }
+    }
+    void update(){ previous_mode = current_mode; current_mode = requested_mode; }
+    mode_enum now(){ return current_mode; }
+    mode_enum pre(){ return previous_mode; }
+    bool isRunning(){ return (current_mode==MODE_SYNC_TO_WBMS) || (current_mode==MODE_WBMS) || (current_mode==MODE_PAUSE) || (current_mode==MODE_SYNC_TO_IDLE) ;}
+    bool isInitialize(){ return (previous_mode==MODE_IDLE) && (current_mode==MODE_SYNC_TO_WBMS) ;}
+};
+
 class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
   public:
     WholeBodyMasterSlave(RTC::Manager* manager);
@@ -171,8 +202,13 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
 
   boost::shared_ptr<WBMSCore> hsp;
 
-  enum { MODE_IDLE,/* MODE_COUNTDOWN,*/ MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE} mode, previous_mode;
-  bool isRunning(){ return (mode==MODE_SYNC_TO_WBMS) || (mode==MODE_WBMS) || (mode==MODE_PAUSE) || (mode==MODE_SYNC_TO_IDLE) ;}
+  hrp::Vector3 torso_rot_rmc;
+  ControlMode mode;
+
+//  enum { MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE} mode, previous_mode;
+//  bool isRunning(){ return (mode==MODE_SYNC_TO_WBMS) || (mode==MODE_WBMS) || (mode==MODE_PAUSE) || (mode==MODE_SYNC_TO_IDLE) ;}
+//  bool isInitialize(){ return (previous_mode==MODE_IDLE) && (mode==MODE_SYNC_TO_WBMS) ;}
+//  enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE};
 
   void setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in);
   void calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::Vector3 target_v_old[], WBMSPose3D& rf_ref, WBMSPose3D& lf_ref, WBMSPose3D& rh_ref, WBMSPose3D& lh_ref);
@@ -185,9 +221,14 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
   void processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_normal_in, const HumanPose& pose_ref);
   bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; };
 
-  void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const hrp::Vector3& max_acc, const double& max_vel, hrp::Vector3& pos_ans);
+//  void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const hrp::Vector3& max_acc, const double& max_vel, hrp::Vector3& pos_ans);
 };
 
+
+
+
+
+
 extern "C"
 {
   void WholeBodyMasterSlaveInit(RTC::Manager* manager);
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 5d8a68fd347..204acb5e2c9 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -36,7 +36,7 @@ extern "C" {
 #endif
 
 #define DEBUG 0
-
+#define dbg(var) std::cout<<#var"= "<<(var)<<std::endl
 #define LIMIT_MIN(x,min) (x= ( x<min ? min:x ))
 #define LIMIT_MAX(x,max) (x= ( x<max ? x:max ))
 #define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
@@ -67,8 +67,7 @@ class BiquadIIRFilterVec : UTIL_CONST {
     IIRFilter filters[XYZ];
     hrp::Vector3 ans;
   public:
-    BiquadIIRFilterVec()
-    {}
+    BiquadIIRFilterVec(){}
     ~BiquadIIRFilterVec(){}
     void setParameter(const hrp::Vector3& fc_in, const double& HZ, const double& Q = 0.5){ for(int i=0;i<XYZ;i++){ filters[i].setParameterAsBiquad((double)fc_in(i), Q, HZ); } }
     void setParameter(const double& fc_in, const double& HZ, const double& Q = 0.5){ setParameter(hrp::Vector3(fc_in,fc_in,fc_in), HZ, Q); }//overload
@@ -147,8 +146,8 @@ class WBMSCore : UTIL_CONST {
     RobotConfig rc;
     HumanPose rp_ref_out_old, hp_swap_checked;
     unsigned int loop;
+    bool is_initial_loop;
   public:
-    double countdown_sec;
     double H_cur;
     HumanPose hp_wld_raw, hp_plot, rp_ref_out, rp_ref_vel_old;
     FILE *sr_log, *cz_log, *id_log;
@@ -173,7 +172,6 @@ class WBMSCore : UTIL_CONST {
       bool use_manipulability_limit;
     };
     struct WBMSparameters WBMSparam;
-    bool is_initial_loop;
     hrp::Vector3 cur_min_manip_direc;
     double cur_manip_val;
 
@@ -187,10 +185,6 @@ class WBMSCore : UTIL_CONST {
       DT = dt;
       HZ = (int)(1.0/DT);
       CNT_F_TH = 20.0;
-//      HumanSyncOn = false;
-//      ht_first_call = true;
-//      startCountdownForHumanSync = false;
-      countdown_sec = 5.0;
       loop = 0;
       //////////  hrp::Vector は初期化必要  ///////////
       com_old = com_oldold = comacc = hrp::Vector3::Zero();
@@ -247,45 +241,62 @@ class WBMSCore : UTIL_CONST {
         cz_log = fopen("/home/ishiguro/HumanSync_com_zmp.log","w+");
         id_log = fopen("/home/ishiguro/HumanSync_invdyn.log","w+");
       }
-//      previous_mode = mode = MODE_IDLE;
       is_initial_loop = true;
-      cout<<"HumanSynchronizer constructed"<<endl;
+      cout<<"WBMSCore constructed"<<endl;
     }
     ~WBMSCore(){
       if(DEBUG)fclose(sr_log);
       if(DEBUG)fclose(cz_log);
       if(DEBUG)fclose(id_log);
-      cout<<"HumanSynchronizer destructed"<<endl;
+      cout<<"WBMSCore destructed"<<endl;
     }
-    double getRemainingCountDown() const { return countdown_sec; }
-//    bool isHumanSyncOn() const { return HumanSyncOn; }
     double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6; }
-    void setTargetHumanToRobotRatio(const double tgt_h2r_r_in){ tgt_h2r_ratio = tgt_h2r_r_in; }
-    void setCurrentInputAsOffset(HumanPose& tgt){
+    void initializeHumanPoseFromCurrentInput(){
       std::string ns[7] = {"com","rf","lf","rh","lh","zmp","head"};
       for(int i=0;i<7;i++){
-        tgt.tgt[i].offs.p   =  tgt.tgt[i].abs.p;
-        tgt.tgt[i].offs.rpy =  tgt.tgt[i].abs.rpy;
+        hp_wld_raw.tgt[i].offs.p   =  hp_wld_raw.tgt[i].abs.p;
+        hp_wld_raw.tgt[i].offs.rpy =  hp_wld_raw.tgt[i].abs.rpy;
+      }
+    }
+    void initializeRobotPoseFromHRPBody(fikPtr& fik_in, hrp::BodyPtr& robot_in){
+      const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
+      const int human_l_names[4] = {rf,lf,rh,lh};
+      for(int i=0;i<4;i++){//HumanSynchronizerの初期姿勢オフセットをセット
+        if(fik_in->ikp.count(robot_l_names[i])){
+          rp_ref_out.tgt[human_l_names[i]].offs.p = fik_in->getEEPos(robot_l_names[i]);
+          rp_ref_out.tgt[human_l_names[i]].offs.rpy = hrp::rpyFromRot(fik_in->getEERot(robot_l_names[i]));
+          rp_ref_out.tgt[human_l_names[i]].abs = rp_ref_out.tgt[human_l_names[i]].offs;
+        }
+      }
+      rp_ref_out.tgt[com].offs.p = robot_in->calcCM();
+      rp_ref_out.tgt[com].offs.rpy = hrp::rpyFromRot(robot_in->rootLink()->R);
+      rp_ref_out.tgt[zmp].offs.p(X) = rp_ref_out.tgt[com].offs.p(X);
+      rp_ref_out.tgt[zmp].offs.p(Y) = rp_ref_out.tgt[com].offs.p(Y);
+      rp_ref_out.tgt[zmp].offs.p(Z) = (fik_in->getEEPos("rleg")(Z)+fik_in->getEEPos("lleg")(Z))/2;
+      rp_ref_out.tgt[rf].cnt = rp_ref_out.tgt[rf].offs;
+      rp_ref_out.tgt[lf].cnt = rp_ref_out.tgt[lf].offs;
+      baselinkpose.p = robot_in->rootLink()->p;
+      baselinkpose.rpy = hrp::rpyFromRot(robot_in->rootLink()->R);
+      for(int i=0, l[LR]={rf,lf}; i<LR; i++){
+        rp_ref_out.tgt[l[i]].is_contact = true;
+//        rp_ref_out.tgt[l[i]].cnt = rp_ref_out.tgt[l[i]].abs;
       }
     }
+    void initializeRequest(fikPtr& fik_in, hrp::BodyPtr& robot_in){
+      loop = 0;
+      is_initial_loop = true;
+      initializeHumanPoseFromCurrentInput();
+      initializeRobotPoseFromHRPBody(fik_in, robot_in);
+      rp_ref_out_old = rp_ref_vel_old = rp_ref_out;
+    }
     void update(){//////////  メインループ  ////////////
       gettimeofday(&t_calc_start, NULL);
-      if(loop!=0){is_initial_loop = false;}
+//      if(is_initial_loop){ initialize(); }
       updateParams                        ();
       autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);//入力の左右反転を常にチェック(＝手足の交差は不可能)
       convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
       hp_plot = rp_ref_out;
-      if(is_initial_loop){
-        for(int i=0, l[LR]={rf,lf}; i<LR; i++){
-          rp_ref_out.tgt[l[i]].is_contact = true;
-//          rp_ref_out.tgt[l[i]].go_contact = true;
-          rp_ref_out.tgt[l[i]].cnt = rp_ref_out.tgt[l[i]].abs;
-        }
-        rp_ref_out_old = rp_ref_out;
-        rp_ref_vel_old = rp_ref_out;
-      }
-//      applyVelLimit                       (rp_ref_vel_old, rp_ref_out);
-      rp_ref_vel_old = rp_ref_out;
+//      applyVelLimit                       (rp_ref_vel_old, rp_ref_out); rp_ref_vel_old = rp_ref_out;
       if(WBMSparam.set_com_height_fix)rp_ref_out.tgt[com].abs.p(Z) = rp_ref_out.tgt[com].offs.p(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
       judgeFootLandOnCommandByFootForce   (hp_wld_raw);//人体足裏反力から各足の接地指令を生成
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
@@ -318,8 +329,8 @@ class WBMSCore : UTIL_CONST {
       com_vel_old = (rp_ref_out.tgt[com].abs.p - rp_ref_out_old.tgt[com].abs.p)/DT;
       rh_vel_old = (rp_ref_out.tgt[rh].abs.p - rp_ref_out_old.tgt[rh].abs.p)/DT;
       rp_ref_out_old = rp_ref_out;
-//      previous_mode = mode;
       loop++;
+      is_initial_loop = false;
       gettimeofday(&t_calc_end, NULL);
     }
     static void Vector3ToVector2(const hrp::Vector3& in, hrp::Vector2& out){ out(X) = in(X); out(Y) = in(Y);}
@@ -374,18 +385,6 @@ class WBMSCore : UTIL_CONST {
         else if (!in.tgt[l[i]].is_contact && in.tgt[l[i]].w(fz)>CNT_F_TH+30){in.tgt[l[i]].is_contact = true;}//足浮いた状態から下げる
       }
     }
-//    void applyLPFilter(HumanPose& tgt){
-//      if(is_initial_loop){
-//        for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
-//          tgt_pos_filters[l[i]].reset(tgt.tgt[l[i]].abs.p);
-//          tgt_rot_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy);
-//        }
-//      }
-//      for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
-//        tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
-//        tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
-//      }
-//    }
     void applyLPFilter(HumanPose& tgt){
       if(is_initial_loop){
         for(int i=0, l[1]={com}; i<1; i++){
@@ -402,8 +401,24 @@ class WBMSCore : UTIL_CONST {
 //        calcVelAccSafeTrajectory_val(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
 //      }
       for(int i=0, l[5]={rf,lf,rh,lh,head}; i<5; i++){
-        calcVelAccSafeTrajectory(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, 1.0, tgt.tgt[l[i]].abs.p);
-        calcVelAccSafeTrajectory(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+//        calcVelAccSafeTrajectory(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, 1.0, tgt.tgt[l[i]].abs.p);
+//        calcVelAccSafeTrajectory(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, 1.0, tgt.tgt[l[i]].abs.p);
+        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+      }
+    }
+    void calcVelAccSafeTrajectoryVec(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
+      if((pos_tgt - pos_cur).norm() > 1.0e-6){
+        hrp::Vector3 direc = (pos_tgt - pos_cur).normalized();
+        double stop_safe_vel_scalar = sqrt( 2 * max_acc * (pos_tgt - pos_cur).norm() );
+        hrp::Vector3 vel_ans = vel_cur + direc * max_acc * DT;
+        if(vel_ans.dot(direc) > stop_safe_vel_scalar){
+//          vel_ans = vel_ans.normalized() * stop_safe_vel_scalar;
+          vel_ans = vel_ans * stop_safe_vel_scalar / vel_ans.dot(direc);
+        }
+        pos_ans = pos_cur + vel_ans * DT;
+      }else{
+        pos_ans = pos_tgt;
       }
     }
     void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
@@ -440,25 +455,75 @@ class WBMSCore : UTIL_CONST {
       if( inside_vec_lf.dot(lf2zmp) / inside_vec_lf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ out.tgt[rf].go_contact = true; }
     }
     void limitEEWorkspace(HumanPose& out){
-//      const double MAX_FW = 0.25;
-      const double MAX_FW = 1000000;//manipulability test
+      const double MAX_FW = 0.25;
+//      const double MAX_FW = 1000000;//manipulability test
       const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
-      if(!out.tgt[rf].is_contact && out.tgt[lf].is_contact){//右足浮遊時
-        const hrp::Vector2 lf2rf_vec( out.tgt[rf].abs.p(X) - out.tgt[lf].cnt.p(X), out.tgt[rf].abs.p(Y) - out.tgt[lf].cnt.p(Y) );
-        if(lf2rf_vec.norm() > MAX_FW){
-          out.tgt[rf].abs.p(X) = (lf2rf_vec.normalized())(X) * MAX_FW + out.tgt[lf].cnt.p(X);
-          out.tgt[rf].abs.p(Y) = (lf2rf_vec.normalized())(Y) * MAX_FW + out.tgt[lf].cnt.p(Y);
-        }
-        LIMIT_MAX(out.tgt[rf].abs.p(Y), out.tgt[lf].cnt.p(Y) - FOOT_2_FOOT_COLLISION_MARGIIN);
-      }
-      else if(out.tgt[rf].is_contact && !out.tgt[lf].is_contact){//左足浮遊時
-        const hrp::Vector2 rf2lf_vec( out.tgt[lf].abs.p(X) - out.tgt[rf].cnt.p(X), out.tgt[lf].abs.p(Y) - out.tgt[rf].cnt.p(Y) );
-        if(rf2lf_vec.norm() > MAX_FW){
-          out.tgt[lf].abs.p(X) = (rf2lf_vec.normalized())(X) * MAX_FW + out.tgt[rf].cnt.p(X);
-          out.tgt[lf].abs.p(Y) = (rf2lf_vec.normalized())(Y) * MAX_FW + out.tgt[rf].cnt.p(Y);
+//      if(!out.tgt[rf].is_contact && out.tgt[lf].is_contact){//右足浮遊時
+//        const hrp::Vector2 lf2rf_vec( out.tgt[rf].abs.p(X) - out.tgt[lf].cnt.p(X), out.tgt[rf].abs.p(Y) - out.tgt[lf].cnt.p(Y) );
+//        if(lf2rf_vec.norm() > MAX_FW){
+//          out.tgt[rf].abs.p(X) = (lf2rf_vec.normalized())(X) * MAX_FW + out.tgt[lf].cnt.p(X);
+//          out.tgt[rf].abs.p(Y) = (lf2rf_vec.normalized())(Y) * MAX_FW + out.tgt[lf].cnt.p(Y);
+//        }
+//        LIMIT_MAX(out.tgt[rf].abs.p(Y), out.tgt[lf].cnt.p(Y) - FOOT_2_FOOT_COLLISION_MARGIIN);
+//      }
+//      else if(out.tgt[rf].is_contact && !out.tgt[lf].is_contact){//左足浮遊時
+//        const hrp::Vector2 rf2lf_vec( out.tgt[lf].abs.p(X) - out.tgt[rf].cnt.p(X), out.tgt[lf].abs.p(Y) - out.tgt[rf].cnt.p(Y) );
+//        if(rf2lf_vec.norm() > MAX_FW){
+//          out.tgt[lf].abs.p(X) = (rf2lf_vec.normalized())(X) * MAX_FW + out.tgt[rf].cnt.p(X);
+//          out.tgt[lf].abs.p(Y) = (rf2lf_vec.normalized())(Y) * MAX_FW + out.tgt[rf].cnt.p(Y);
+//        }
+//        LIMIT_MIN(out.tgt[lf].abs.p(Y), out.tgt[rf].cnt.p(Y) + FOOT_2_FOOT_COLLISION_MARGIIN);
+//      }
+
+
+      for(int i=0, spl[LR]={rf,lf}, swl[LR]={lf,rf}; i<LR; i++){
+        PoseTGT& support_leg = out.tgt[spl[i]];
+        PoseTGT& swing_leg = out.tgt[swl[i]];
+        hrp::Vector2 inside_vec_baserel = ( spl[i]==rf ? hrp::Vector2(0,+1) : hrp::Vector2(0,-1) );
+
+        hrp::Vector2 sp2sw_vec( swing_leg.abs.p(X) - support_leg.cnt.p(X), swing_leg.abs.p(Y) - support_leg.cnt.p(Y) );
+        if(swing_leg.go_contact && sp2sw_vec.norm() > MAX_FW){ sp2sw_vec = sp2sw_vec.normalized() * MAX_FW; }//着地時に足を広げすぎないよう制限
+        Eigen::Matrix2d base_rot;
+        base_rot = Eigen::Rotation2Dd(baselinkpose.rpy(y));
+        hrp::Vector2 sp2sw_vec_baserel = base_rot.transpose() * sp2sw_vec;
+        if( sp2sw_vec_baserel.dot(inside_vec_baserel) < FOOT_2_FOOT_COLLISION_MARGIIN){
+          sp2sw_vec_baserel += inside_vec_baserel * (FOOT_2_FOOT_COLLISION_MARGIIN - sp2sw_vec_baserel.dot(inside_vec_baserel));
         }
-        LIMIT_MIN(out.tgt[lf].abs.p(Y), out.tgt[rf].cnt.p(Y) + FOOT_2_FOOT_COLLISION_MARGIIN);
+        sp2sw_vec = base_rot * sp2sw_vec_baserel;
+        swing_leg.abs.p(X) = sp2sw_vec(X) + support_leg.cnt.p(X);
+        swing_leg.abs.p(Y) = sp2sw_vec(Y) + support_leg.cnt.p(Y);
       }
+
+
+
+//      if(!out.tgt[rf].is_contact && out.tgt[lf].is_contact){//右足浮遊時
+//        PoseTGT& support_leg = out.tgt[lf];
+//        PoseTGT& swing_leg = out.tgt[rf];
+//        hrp::Vector2 inside_vec_baserel(0, -1);
+//
+//        hrp::Vector2 sp2sw_vec( swing_leg.abs.p(X) - support_leg.cnt.p(X), swing_leg.abs.p(Y) - support_leg.cnt.p(Y) );
+//        if(swing_leg.go_contact && sp2sw_vec.norm() > MAX_FW){ sp2sw_vec = sp2sw_vec.normalized() * MAX_FW; }//着地時に足を広げすぎないよう制限
+//        Eigen::Matrix2d base_rot;
+//        base_rot = Eigen::Rotation2Dd(baselinkpose.rpy(y));
+//        hrp::Vector2 sp2sw_vec_baserel = base_rot.transpose() * sp2sw_vec;
+//
+//        if( sp2sw_vec_baserel.dot(inside_vec_baserel) < FOOT_2_FOOT_COLLISION_MARGIIN){
+//          sp2sw_vec_baserel += inside_vec_baserel * (FOOT_2_FOOT_COLLISION_MARGIIN - sp2sw_vec_baserel.dot(inside_vec_baserel));
+//        }
+//        sp2sw_vec = base_rot * sp2sw_vec_baserel;
+//        swing_leg.abs.p(X) = sp2sw_vec(X) + support_leg.cnt.p(X);
+//        swing_leg.abs.p(Y) = sp2sw_vec(Y) + support_leg.cnt.p(Y);
+//      }
+//      else if(!out.tgt[lf].is_contact && out.tgt[lf].go_contact && out.tgt[rf].is_contact){//左足浮遊時
+//        const hrp::Vector2 rf2lf_vec( out.tgt[lf].abs.p(X) - out.tgt[rf].cnt.p(X), out.tgt[lf].abs.p(Y) - out.tgt[rf].cnt.p(Y) );
+//        if(rf2lf_vec.norm() > MAX_FW){
+//          out.tgt[lf].abs.p(X) = (rf2lf_vec.normalized())(X) * MAX_FW + out.tgt[rf].cnt.p(X);
+//          out.tgt[lf].abs.p(Y) = (rf2lf_vec.normalized())(Y) * MAX_FW + out.tgt[rf].cnt.p(Y);
+//        }
+//        LIMIT_MIN(out.tgt[lf].abs.p(Y), out.tgt[rf].cnt.p(Y) + FOOT_2_FOOT_COLLISION_MARGIIN);
+//      }
+
+
 //      hrp::Vector3 init_base2rh = out.tgt[rh].offs.p - baselinkpose.p_offs;
 //      hrp::Vector3 init_base2lh = out.tgt[lh].offs.p - baselinkpose.p_offs;
 //      hrp::Matrix33 init_base2rh_R = hrp::rotFromRpy(baselinkpose.rpy_offs).transpose() * hrp::rotFromRpy(out.tgt[rh].offs.rpy);
@@ -506,43 +571,6 @@ class WBMSCore : UTIL_CONST {
         }
       }
 
-//      hrp::dmatrix J,Jinv,Jnull;
-//      fik->ikp["rleg"].manip->calcJacobian(J);
-//      fik->ikp["rleg"].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
-//
-//      hrp::dmatrix manipulability_mat = J*J.transpose();
-//      double manipulability = sqrt((J*J.transpose()).determinant());
-//      Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
-//      Eigen::MatrixXd::Index row,col;
-//      cur_manip_val = svd.singularValues().minCoeff(&row,&col);
-//      cur_min_manip_direc = svd.matrixU().col(row);// unit vector
-//      if(cur_min_manip_direc.dot(fik->ikp["rleg"].target_p0 - m_robot->rootLink()->p) < 0){ cur_min_manip_direc *= -1; }//向きみて反転(常に外側正)
-
-//      static hrp::Vector3 old_ref;
-//
-//      hrp::Vector3 ref_ee_vel = (out.tgt[rf].abs.p - rp_ref_out_old.tgt[rf].abs.p) / DT;
-//      hrp::Vector3 ref_ee_vel_mod = ref_ee_vel;
-//      hrp::Vector3 ref_ee_vel_v = (ref_ee_vel.dot(cur_min_manip_direc)) * cur_min_manip_direc;
-//      hrp::Vector3 ref_ee_vel_h = ref_ee_vel - ref_ee_vel_v;
-//      double min_manip_th = 0.1;
-//      double penalty = (cur_manip_val - min_manip_th) * 10;
-//      LIMIT_MINMAX(penalty, -1.0, 1.0);//
-//      double max_dec_vel = 1.0;
-//      max_dec_vel *= penalty;
-//      cout<<"cur_manip_val"<<cur_manip_val<<endl;
-//      cout<<"penalty"<<penalty<<endl;
-//      if(ref_ee_vel_v.norm() > max_dec_vel){
-//        ref_ee_vel_v = ref_ee_vel_v.normalized() * max_dec_vel;
-//      }
-////      cout<<"ref_ee_vel_v"<<ref_ee_vel_v.transpose()<<endl;
-////      if(cur_manip_val<min_manip_th){
-////        ref_ee_vel_v = - cur_min_manip_direc * (min_manip_th - cur_manip_val) * 5;
-////      }
-//      ref_ee_vel_mod = ref_ee_vel_h + ref_ee_vel_v;
-//      out.tgt[rf].abs.p = rp_ref_out_old.tgt[rf].abs.p + ref_ee_vel_mod * DT;
-
-//      fik->revertRobotStateToCurrent();
-
     }
     void setFootContactPoseByGoContact(HumanPose& out){
       for(int i=0, l[LR]={rf,lf}; i<LR; i++){

From 213add032f7a3045702c4c1a1934829a6c101322 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 7 Jun 2017 01:27:32 +0900
Subject: [PATCH 099/217] imprement manip singular acc

---
 .../WholeBodyMasterSlave.cpp                  | 61 ++++++++++++-------
 .../WholeBodyMasterSlave.h                    | 13 +---
 rtc/WholeBodyMasterSlave/wbms_core.h          | 61 +++++++++++--------
 3 files changed, 76 insertions(+), 59 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 06a0c1a63f6..1ce43967e95 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -26,6 +26,17 @@ static const char* WholeBodyMasterSlave_spec[] =
         ""
     };
 
+//static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
+//{
+//    int pre = os.precision();
+//    os.setf(std::ios::fixed);
+//    os << std::setprecision(6)
+//       << (tm.sec + tm.nsec/1e9)
+//       << std::setprecision(pre);
+//    os.unsetf(std::ios::fixed);
+//    return os;
+//}
+
 WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager),
       m_qRefIn("qRef", m_qRef),// from sh
       m_zmpIn("zmpIn", m_zmp),
@@ -288,6 +299,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         m_robot_ml->rootLink()->R = m_robot_rmc->rootLink()->R;
         hsp->fik = fik_ml;
         hsp->m_robot = m_robot_ml;
+        calcManipulability(fik_rmc, m_robot_rmc);
 
         if(hsp->WBMSparam.is_doctor){
           processWholeBodyMasterSlave(fik, m_robot, hsp->rp_ref_out);//安全制限つきマスタ・スレーブ
@@ -495,10 +507,10 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(fikPtr& fik_in, hrp::Body
   WBMSPose3D rh_checked = pose_ref.tgt[rh].abs;
   WBMSPose3D lh_checked = pose_ref.tgt[lh].abs;
 
-  static hrp::Vector3 target_v_old[4];
-  if(hsp->WBMSparam.use_manipulability_limit){
-    calcManipulabilityJointLimit(fik_in, robot_in, target_v_old, rf_checked, lf_checked, rh_checked, lh_checked);
-  }
+//  static hrp::Vector3 target_v_old[4];
+//  if(hsp->WBMSparam.use_manipulability_limit){
+//    calcManipulabilityJointLimit(fik_in, robot_in, target_v_old, rf_checked, lf_checked, rh_checked, lh_checked);
+//  }
   solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, rf_checked, lf_checked, rh_checked, lh_checked, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave");
 }
 
@@ -522,20 +534,27 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::
 }
 
 
-//void WholeBodyMasterSlave::calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const hrp::Vector3& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
-//  hrp::Vector3 direc( SGN(pos_tgt(X)-pos_cur(X)), SGN(pos_tgt(Y)-pos_cur(Y)), SGN(pos_tgt(Z)-pos_cur(Z)));
-//  hrp::Vector3 vel_ans;
-//  for(int i=0;i<XYZ;i++){vel_ans(i) = vel_cur(i) + direc(i) * max_acc(i) * m_dt;}
-//  for(int i=0;i<XYZ;i++){
-//    double stop_safe_vel = sqrt( 2 * max_acc(i) * fabs(pos_tgt(i) - pos_cur(i)) );
-//    if( direc(i) > 0 ){
-//      if(vel_ans(i) > stop_safe_vel){ vel_ans(i) = stop_safe_vel; }
-//    }else{
-//      if(vel_ans(i) < - stop_safe_vel){ vel_ans(i) = - stop_safe_vel; }
-//    }
-//  }
-//  pos_ans = pos_cur + vel_ans * m_dt;
-//}
+void WholeBodyMasterSlave::calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robot_in){
+  const std::string names[4] = {"rleg","lleg","rarm","larm"};
+  hrp::dmatrix J,Jinv,Jnull;
+  for(int l=0; l<4; l++){
+    fik_in->ikp[names[l]].manip->calcJacobian(J);
+    fik_in->ikp[names[l]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
+    Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
+//    Eigen::MatrixXd::Index row,col;
+//    hsp->cur_manip_val[l] = svd.singularValues().minCoeff(&row,&col);
+//    hsp->min_manip_direc[l] = svd.matrixU().col(row);// unit vector
+    hsp->manip_mat[l] = svd.matrixU();
+    for(int i=0;i<3;i++){
+      hsp->manip_sv[l](i) = svd.singularValues()(i);
+//      hsp->manip_mat[l].col(i) *= svd.singularValues()(i);
+//      hsp->cur_manip_val[l][i] = svd.singularValues()(i);
+//      hsp->manip_direc[l][i] = svd.matrixU().col(i);// unit vector
+    }
+//    dbg((hsp->manip_sv[l]).transpose());
+//    if(hsp->manip_direc[l].dot(fik_in->ikp[names[l]].target_p0 - robot_in->rootLink()->p) < 0){ hsp->manip_direc[l] *= -1; }//向きみて反転(常に外側正)
+  }
+}
 
 //これ超計算重い
 void WholeBodyMasterSlave::calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::Vector3 target_v_old[], WBMSPose3D& rf_ref, WBMSPose3D& lf_ref, WBMSPose3D& rh_ref, WBMSPose3D& lh_ref){
@@ -713,9 +732,9 @@ void WholeBodyMasterSlave::processMomentumCompensation(fikPtr& fik_in, hrp::Body
   static hrp::Vector3 target_v_old[4];
   if(hsp->WBMSparam.use_manipulability_limit){
     struct timeval t_calc_start, t_calc_end;
-    if(TIMECALC)gettimeofday(&t_calc_start, NULL);
-    calcManipulabilityJointLimit(fik_in, robot_in, target_v_old, rf_checked, lf_checked, rh_checked, lh_checked);
-    if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"calcManipulabilityJointLimit:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6<<endl; t_calc_start = t_calc_end;}
+//    if(TIMECALC)gettimeofday(&t_calc_start, NULL);
+//    calcManipulabilityJointLimit(fik_in, robot_in, target_v_old, rf_checked, lf_checked, rh_checked, lh_checked);
+//    if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"calcManipulabilityJointLimit:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6<<endl; t_calc_start = t_calc_end;}
   }
   solveFullbodyIKStrictCOM(fik_in, robot_in, com_mod, rf_checked, lf_checked, rh_checked, lh_checked, pose_ref.tgt[head].abs,"calcDynamicsFilterCompensation");
 }
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 55ad0d4cb40..eb570b25892 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -22,18 +22,6 @@
 
 using namespace RTC;
 
-//static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
-//{
-//    int pre = os.precision();
-//    os.setf(std::ios::fixed);
-//    os << std::setprecision(6)
-//       << (tm.sec + tm.nsec/1e9)
-//       << std::setprecision(pre);
-//    os.unsetf(std::ios::fixed);
-//    return os;
-//}
-
-
 #define USE_DEBUG_PORT
 
 enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE};
@@ -211,6 +199,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
 //  enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE};
 
   void setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in);
+  void calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robot_in);
   void calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::Vector3 target_v_old[], WBMSPose3D& rf_ref, WBMSPose3D& lf_ref, WBMSPose3D& rh_ref, WBMSPose3D& lh_ref);
   void calcManipulabilityJointLimitForWBMS(fikPtr& fik_in, hrp::BodyPtr& robot_in);
   void solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix="");
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 204acb5e2c9..f4c0109f9af 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -172,13 +172,14 @@ class WBMSCore : UTIL_CONST {
       bool use_manipulability_limit;
     };
     struct WBMSparameters WBMSparam;
-    hrp::Vector3 cur_min_manip_direc;
-    double cur_manip_val;
 
     typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
     fikPtr fik;
     hrp::BodyPtr m_robot;
-
+    double cur_manip_val[4][3];
+    hrp::Vector3 manip_direc[4][3];
+    hrp::Matrix33 manip_mat[4];
+    hrp::Vector3 manip_sv[4];
 
     WBMSCore(const double& dt){
       tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
@@ -226,7 +227,7 @@ class WBMSCore : UTIL_CONST {
       WBMSparam.upper_body_rmc_ratio = 0.5;
       WBMSparam.use_rh = WBMSparam.use_lh = true;
       WBMSparam.use_head = true;
-      WBMSparam.use_manipulability_limit = false;
+      WBMSparam.use_manipulability_limit = true;
       rp_ref_out_old.clear();
       rp_ref_out.clear();
       rp_ref_vel_old.clear();
@@ -306,7 +307,8 @@ class WBMSCore : UTIL_CONST {
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
       setFootRotHorizontalIfGoLanding     (rp_ref_out);
 
-      limitManipulability                 (rp_ref_out);
+
+      if(WBMSparam.use_manipulability_limit){limitManipulability                 (rp_ref_out);}
 
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
 
@@ -396,13 +398,12 @@ class WBMSCore : UTIL_CONST {
         tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
         tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
       }
-//      for(int i=0, l[1]={com}; i<1; i++){
-//        calcVelAccSafeTrajectory_val(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, 1.0, tgt.tgt[l[i]].abs.p);
-//        calcVelAccSafeTrajectory_val(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
-//      }
-      for(int i=0, l[5]={rf,lf,rh,lh,head}; i<5; i++){
-//        calcVelAccSafeTrajectory(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, 1.0, tgt.tgt[l[i]].abs.p);
-//        calcVelAccSafeTrajectory(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+//      cout<<"calcVelAccSafeTrajectoryVecML"<<endl;
+      for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
+        calcVelAccSafeTrajectoryVecML(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, manip_mat[i], manip_sv[i], tgt.tgt[l[i]].abs.p);
+        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+      }
+      for(int i=0, l[1]={head}; i<1; i++){
         calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, 1.0, tgt.tgt[l[i]].abs.p);
         calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
       }
@@ -421,24 +422,33 @@ class WBMSCore : UTIL_CONST {
         pos_ans = pos_tgt;
       }
     }
-    void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
-      hrp::Vector3 direc( SGN(pos_tgt(X)-pos_cur(X)), SGN(pos_tgt(Y)-pos_cur(Y)), SGN(pos_tgt(Z)-pos_cur(Z)));
-      hrp::Vector3 vel_ans = vel_cur + direc * max_acc * DT;
-      for(int i=0;i<XYZ;i++){
-        double stop_safe_vel = sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
-        if( direc(i) > 0 ){
-          if(vel_ans(i) > stop_safe_vel){ vel_ans(i) = stop_safe_vel; }
-        }else{
-          if(vel_ans(i) < - stop_safe_vel){ vel_ans(i) = - stop_safe_vel; }
+    void calcVelAccSafeTrajectoryVecML(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const hrp::Matrix33& max_acc_mat, const hrp::Vector3& max_acc_sv, hrp::Vector3& pos_ans){
+      if((pos_tgt - pos_cur).norm() > 1.0e-6){
+        hrp::Vector3 direc = (pos_tgt - pos_cur).normalized();
+        double acc_val = 1.0;
+        hrp::Vector3 ref_acc = direc*acc_val;
+        hrp::Vector3 mod_acc = max_acc_mat * max_acc_sv.asDiagonal() * max_acc_mat.transpose() * ref_acc;
+//        cout<<"ref_acc "<<ref_acc.transpose()<<" mod_acc "<<mod_acc.transpose()<<endl;
+//        dbg(max_acc_mat);
+//        dbg(max_acc_sv.transpose());
+//        dbg((max_acc_mat.transpose() * ref_acc).transpose());
+//        dbg((max_acc_sv.asDiagonal() * max_acc_mat.transpose() * ref_acc).transpose());
+        double stop_safe_vel_scalar = sqrt( 2 * fabs(mod_acc.dot(direc)) * (pos_tgt - pos_cur).norm() );
+        hrp::Vector3 vel_ans = vel_cur + mod_acc * DT;
+        if(vel_ans.dot(direc) > stop_safe_vel_scalar){
+//          vel_ans = vel_ans.normalized() * stop_safe_vel_scalar;
+          vel_ans = vel_ans * stop_safe_vel_scalar / vel_ans.dot(direc);
         }
+        pos_ans = pos_cur + vel_ans * DT;
+      }else{
+        pos_ans = pos_tgt;
       }
-      pos_ans = pos_cur + vel_ans * DT;
     }
-    void calcVelAccSafeTrajectory_val(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
+    void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
       hrp::Vector3 direc( SGN(pos_tgt(X)-pos_cur(X)), SGN(pos_tgt(Y)-pos_cur(Y)), SGN(pos_tgt(Z)-pos_cur(Z)));
-      hrp::Vector3 vel_ans = vel_cur + (pos_tgt-pos_cur) * 10 * max_acc * DT;
+      hrp::Vector3 vel_ans = vel_cur + direc * max_acc * DT;
       for(int i=0;i<XYZ;i++){
-        double stop_safe_vel = sqrt( 2 * (pos_tgt-pos_cur).norm() * 10 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
+        double stop_safe_vel = sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
         if( direc(i) > 0 ){
           if(vel_ans(i) > stop_safe_vel){ vel_ans(i) = stop_safe_vel; }
         }else{
@@ -570,7 +580,6 @@ class WBMSCore : UTIL_CONST {
           out.tgt[human_l_names[i]].abs.p = fik->ikp[robot_l_names[i]].target_link->p + fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localPos;
         }
       }
-
     }
     void setFootContactPoseByGoContact(HumanPose& out){
       for(int i=0, l[LR]={rf,lf}; i<LR; i++){

From e314980e95833a9c755cda0047aeb987a1c0cd9b Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 7 Jun 2017 22:57:30 +0900
Subject: [PATCH 100/217] fix for chidori

---
 .../WholeBodyMasterSlave.cpp                  | 43 +++++++++----------
 1 file changed, 21 insertions(+), 22 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 1ce43967e95..330b2a0e8bc 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -195,6 +195,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     invdyn_zmp_filters.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
     invdyn_zmp_filters2.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
 
+    std::cerr << "[" << m_profile.instance_name << "] onInitialize() OK" << std::endl;
     return RTC::RTC_OK;
 }
 
@@ -283,18 +284,16 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
 
       for(int i=0; i<fik_list.size(); i++){
-        fik_list[i]->current_tm = m_qRef.tm;
         fik_list[i]->ikp["rleg"].is_ik_enable = true;
         fik_list[i]->ikp["lleg"].is_ik_enable = true;
-        fik_list[i]->ikp["rarm"].is_ik_enable = hsp->WBMSparam.use_rh;
-        fik_list[i]->ikp["larm"].is_ik_enable = hsp->WBMSparam.use_lh;
+        if(fik_list[i]->ikp.count("rarm"))fik_list[i]->ikp["rarm"].is_ik_enable = hsp->WBMSparam.use_rh;
+        if(fik_list[i]->ikp.count("larm"))fik_list[i]->ikp["larm"].is_ik_enable = hsp->WBMSparam.use_lh;
       }
 
       if (mode.isRunning()) {
         if(mode.isInitialize()){
           preProcessForWholeBodyMasterSlave(fik, m_robot);
         }
-
         m_robot_ml->rootLink()->p = m_robot_rmc->rootLink()->p;
         m_robot_ml->rootLink()->R = m_robot_rmc->rootLink()->R;
         hsp->fik = fik_ml;
@@ -307,7 +306,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
           processWholeBodyMasterSlave_Raw(fik, m_robot, raw_pose);//生マスタ・スレーブ
         }
 
-
         if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processWholeBodyMasterSlave" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
         //逆動力学初期化
@@ -315,7 +313,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
           idsb.setInitState(m_robot, m_dt);
           idsb2.setInitState(m_robot, m_dt);
         }
-
         //逆動力学
         calcAccelerationsForInverseDynamics(m_robot, idsb);
         hrp::Vector3 ref_zmp_invdyn;
@@ -361,8 +358,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
           m_optionalData.data.length(optionalDataLength);//TODO:これいいのか？
           for(int i=0;i<optionalDataLength;i++)m_optionalData.data[i] = 0;
         }
-        m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[contact_states_index_map["rleg"]+4] = hsp->rp_ref_out.tgt[rf].is_contact;
-        m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[contact_states_index_map["lleg"]+4] = hsp->rp_ref_out.tgt[lf].is_contact;
+        m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = hsp->rp_ref_out.tgt[rf].is_contact;
+        m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = hsp->rp_ref_out.tgt[lf].is_contact;
       }
       hsp->baselinkpose.p = m_robot->rootLink()->p;
       hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
@@ -538,21 +535,23 @@ void WholeBodyMasterSlave::calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robo
   const std::string names[4] = {"rleg","lleg","rarm","larm"};
   hrp::dmatrix J,Jinv,Jnull;
   for(int l=0; l<4; l++){
-    fik_in->ikp[names[l]].manip->calcJacobian(J);
-    fik_in->ikp[names[l]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
-    Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
-//    Eigen::MatrixXd::Index row,col;
-//    hsp->cur_manip_val[l] = svd.singularValues().minCoeff(&row,&col);
-//    hsp->min_manip_direc[l] = svd.matrixU().col(row);// unit vector
-    hsp->manip_mat[l] = svd.matrixU();
-    for(int i=0;i<3;i++){
-      hsp->manip_sv[l](i) = svd.singularValues()(i);
-//      hsp->manip_mat[l].col(i) *= svd.singularValues()(i);
-//      hsp->cur_manip_val[l][i] = svd.singularValues()(i);
-//      hsp->manip_direc[l][i] = svd.matrixU().col(i);// unit vector
+    if(fik_in->ikp.count(names[l])){
+      fik_in->ikp[names[l]].manip->calcJacobian(J);
+      fik_in->ikp[names[l]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
+      Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
+  //    Eigen::MatrixXd::Index row,col;
+  //    hsp->cur_manip_val[l] = svd.singularValues().minCoeff(&row,&col);
+  //    hsp->min_manip_direc[l] = svd.matrixU().col(row);// unit vector
+      hsp->manip_mat[l] = svd.matrixU();
+      for(int i=0;i<3;i++){
+        hsp->manip_sv[l](i) = svd.singularValues()(i);
+  //      hsp->manip_mat[l].col(i) *= svd.singularValues()(i);
+  //      hsp->cur_manip_val[l][i] = svd.singularValues()(i);
+  //      hsp->manip_direc[l][i] = svd.matrixU().col(i);// unit vector
+      }
+  //    dbg((hsp->manip_sv[l]).transpose());
+  //    if(hsp->manip_direc[l].dot(fik_in->ikp[names[l]].target_p0 - robot_in->rootLink()->p) < 0){ hsp->manip_direc[l] *= -1; }//向きみて反転(常に外側正)
     }
-//    dbg((hsp->manip_sv[l]).transpose());
-//    if(hsp->manip_direc[l].dot(fik_in->ikp[names[l]].target_p0 - robot_in->rootLink()->p) < 0){ hsp->manip_direc[l] *= -1; }//向きみて反転(常に外側正)
   }
 }
 

From b0afe01fdddac7da7cd7c6e45c124966553ded82 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 30 Jun 2017 17:34:46 +0900
Subject: [PATCH 101/217] tune for demo

---
 rtc/WholeBodyMasterSlave/wbms_core.h | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index f4c0109f9af..24ca6b80aa4 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -117,8 +117,10 @@ class RobotConfig : UTIL_CONST {
       std::vector<hrp::Vector3> init;
       init.resize(MINMAX);
       ee_rot_limit.resize(num_pose_tgt, init);
-      ee_rot_limit[com][MIN] = hrp::Vector3(-30*D2R, -30*D2R, INFMIN);
-      ee_rot_limit[com][MAX] = hrp::Vector3( 30*D2R,  30*D2R, INFMAX);
+//      ee_rot_limit[com][MIN] = hrp::Vector3(-10*D2R, -10*D2R, INFMIN);
+//      ee_rot_limit[com][MAX] = hrp::Vector3( 10*D2R,  10*D2R, INFMAX);
+      ee_rot_limit[com][MIN] = hrp::Vector3(-10*D2R, -10*D2R, -30*D2R);
+      ee_rot_limit[com][MAX] = hrp::Vector3( 10*D2R,  10*D2R, 30*D2R);
       ee_rot_limit[rf][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -20*D2R);
       ee_rot_limit[rf][MAX] = hrp::Vector3( 30*D2R,  30*D2R,   5*D2R);
       ee_rot_limit[lf][MIN] = hrp::Vector3(-30*D2R, -30*D2R,  -5*D2R);
@@ -127,8 +129,8 @@ class RobotConfig : UTIL_CONST {
       ee_rot_limit[rh][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
       ee_rot_limit[lh][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -30*D2R);
       ee_rot_limit[lh][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
-      ee_rot_limit[head][MIN] = hrp::Vector3( 0*D2R, -10*D2R, -40*D2R);
-      ee_rot_limit[head][MAX] = hrp::Vector3( 0*D2R,  20*D2R,  40*D2R);
+      ee_rot_limit[head][MIN] = hrp::Vector3( 0*D2R, -20*D2R, -40*D2R);
+      ee_rot_limit[head][MAX] = hrp::Vector3( 0*D2R,  30*D2R,  40*D2R);
     }
 };
 
@@ -224,7 +226,8 @@ class WBMSCore : UTIL_CONST {
       WBMSparam.set_com_height_fix_val = 0.02;
       WBMSparam.swing_foot_height_offset = 0.01;
       WBMSparam.swing_foot_max_height = 0.5;
-      WBMSparam.upper_body_rmc_ratio = 0.5;
+//      WBMSparam.upper_body_rmc_ratio = 0.5;
+      WBMSparam.upper_body_rmc_ratio = 0.0;
       WBMSparam.use_rh = WBMSparam.use_lh = true;
       WBMSparam.use_head = true;
       WBMSparam.use_manipulability_limit = true;

From 8b5ad2678b5f02b593f893f915d5d77edbbce661 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 4 Jul 2017 19:20:04 +0900
Subject: [PATCH 102/217] tune for demo

---
 rtc/WholeBodyMasterSlave/wbms_core.h | 132 +++++++++++++++++++--------
 1 file changed, 96 insertions(+), 36 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 24ca6b80aa4..dee5bcf472b 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -35,7 +35,7 @@ extern "C" {
 }
 #endif
 
-#define DEBUG 0
+#define DEBUG 1
 #define dbg(var) std::cout<<#var"= "<<(var)<<std::endl
 #define LIMIT_MIN(x,min) (x= ( x<min ? min:x ))
 #define LIMIT_MAX(x,max) (x= ( x<max ? x:max ))
@@ -183,6 +183,8 @@ class WBMSCore : UTIL_CONST {
     hrp::Matrix33 manip_mat[4];
     hrp::Vector3 manip_sv[4];
 
+    hrp::Vector2 cp_acc_old;
+
     WBMSCore(const double& dt){
       tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
       DT = dt;
@@ -307,10 +309,10 @@ class WBMSCore : UTIL_CONST {
       limitEEWorkspace                    (rp_ref_out);
 //      limitManipulability                 (rp_ref_out);
       setFootContactPoseByGoContact       (rp_ref_out);
+      if(DEBUG){ fprintf(cz_log,"com_in: %f %f ",rp_ref_out.tgt[com].abs.p(X),rp_ref_out.tgt[com].abs.p(Y)); }
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
       setFootRotHorizontalIfGoLanding     (rp_ref_out);
 
-
       if(WBMSparam.use_manipulability_limit){limitManipulability                 (rp_ref_out);}
 
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
@@ -320,13 +322,20 @@ class WBMSCore : UTIL_CONST {
       com_vel_forcp_ref = (com_forcp_ref - com_forcp_ref_old)/DT;
       com_forcp_ref_old = com_forcp_ref;
 
+
+      applyLPFilter_pre                       (rp_ref_out);
       applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
+//      dbg(rp_ref_out.tgt[rf].go_contact);
+//      dbg(rp_ref_out.tgt[lf].go_contact);
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
-      applyLPFilter                       (rp_ref_out);
+      applyLPFilter_post                       (rp_ref_out);
 
       r_zmp_raw = rp_ref_out.tgt[zmp].abs.p;
       applyZMPCalcFromCOM                 (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);
-
+      if(DEBUG){
+        fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.tgt[zmp].abs.p(X),rp_ref_out.tgt[zmp].abs.p(Y));
+        fprintf(cz_log,"\n");
+      }
       overwriteFootZFromFootLandOnCommand (rp_ref_out);
       modifyFootRotAndXYForContact        (rp_ref_out);
 
@@ -411,6 +420,38 @@ class WBMSCore : UTIL_CONST {
         calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
       }
     }
+    void applyLPFilter_pre(HumanPose& tgt){
+//      if(is_initial_loop){
+//        for(int i=0, l[1]={com}; i<1; i++){
+//          tgt_pos_filters[l[i]].reset(tgt.tgt[l[i]].abs.p);
+//          tgt_rot_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy);
+//        }
+//      }
+//      for(int i=0, l[1]={com}; i<1; i++){
+//        tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
+//        tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
+//      }
+      for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
+        calcVelAccSafeTrajectoryVecML(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, manip_mat[i], manip_sv[i], tgt.tgt[l[i]].abs.p);
+        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+      }
+      for(int i=0, l[1]={head}; i<1; i++){
+        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, 1.0, tgt.tgt[l[i]].abs.p);
+        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+      }
+    }
+    void applyLPFilter_post(HumanPose& tgt){
+      if(is_initial_loop){
+        for(int i=0, l[1]={com}; i<1; i++){
+          tgt_pos_filters[l[i]].reset(tgt.tgt[l[i]].abs.p);
+          tgt_rot_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy);
+        }
+      }
+      for(int i=0, l[1]={com}; i<1; i++){
+        tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
+        tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
+      }
+    }
     void calcVelAccSafeTrajectoryVec(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
       if((pos_tgt - pos_cur).norm() > 1.0e-6){
         hrp::Vector3 direc = (pos_tgt - pos_cur).normalized();
@@ -591,9 +632,12 @@ class WBMSCore : UTIL_CONST {
           out.tgt[l[i]].abs.p(Y) = out.tgt[l[i]].cnt.p(Y);
           out.tgt[l[i]].abs.rpy(y) = out.tgt[l[i]].cnt.rpy(y);
         }else{
-          out.tgt[l[i]].cnt.p(X) = out.tgt[l[i]].abs.p(X);
-          out.tgt[l[i]].cnt.p(Y) = out.tgt[l[i]].abs.p(Y);
-          out.tgt[l[i]].cnt.rpy(y) = out.tgt[l[i]].abs.rpy(y);
+//          out.tgt[l[i]].cnt.p(X) = out.tgt[l[i]].abs.p(X);
+//          out.tgt[l[i]].cnt.p(Y) = out.tgt[l[i]].abs.p(Y);
+//          out.tgt[l[i]].cnt.rpy(y) = out.tgt[l[i]].abs.rpy(y);
+          out.tgt[l[i]].cnt.p(X) = rp_ref_out_old.tgt[l[i]].abs.p(X);
+          out.tgt[l[i]].cnt.p(Y) = rp_ref_out_old.tgt[l[i]].abs.p(Y);
+          out.tgt[l[i]].cnt.rpy(y) = rp_ref_out_old.tgt[l[i]].abs.rpy(y);
         }
         if(out.tgt[l[i]].go_contact){
           out.tgt[l[i]].abs.p(Z) = out.tgt[l[i]].offs.p(Z);
@@ -663,8 +707,10 @@ class WBMSCore : UTIL_CONST {
       hull_dcp.clear();
       hull_acp.clear();
       hrp::Vector4 marginDelta_for_dcp(+0.001,-0.001,0.001,-0.001);
-//      hrp::Vector4 marginDelta_for_acp(+0.005,-0.005,0.005,-0.005);
-      hrp::Vector4 marginDelta_for_acp(+0.01,-0.01,0.01,-0.01);
+//      hrp::Vector4 marginDelta_for_acp(+0.001,-0.001,0.001,-0.001);
+      hrp::Vector4 marginDelta_for_acp(+0.002,-0.002,0.002,-0.002);
+//      hrp::Vector4 marginDelta_for_dcp(+0.001,-0.001,0.001,-0.001);
+//      hrp::Vector4 marginDelta_for_acp(+0.01,-0.01,0.01,-0.01);
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region+marginDelta_for_dcp, lf_safe_region+marginDelta_for_dcp, hull_dcp);
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region+marginDelta_for_acp, lf_safe_region+marginDelta_for_acp, hull_acp);
       hrp::Vector2 com_vel_ans_2d;
@@ -689,39 +735,53 @@ class WBMSCore : UTIL_CONST {
       com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
 
       //減速CP条件(現在のCPを常に両足裏で頭打ち)
-//      hrp::Vector2 cp_dec_tmp = com_pos + com_vel * sqrt( H_cur / G );
-//      hrp::Vector2 cp_dec_ragulated;
-//      if(!isPointInHullOpenCV(cp_dec_tmp,hull_d)){
-//        calcCrossPointOnHull(com_pos, cp_dec_tmp, hull_d, cp_dec_ragulated);
-//        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
-//      }
+      hrp::Vector2 cp_dec_tmp = com_pos + com_vel * sqrt( H_cur / G );
+      hrp::Vector2 cp_dec_ragulated;
+      if(!isPointInHullOpenCV(cp_dec_tmp,hull_d)){
+        calcCrossPointOnHull(com_pos, cp_dec_tmp, hull_d, cp_dec_ragulated);
+        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
+      }
 
       //減速CP条件2(指令のCPに対する現在発揮可能なCP)
-      hrp::Vector2 cp_dec_ref = com_forcp_ref + com_vel_forcp_ref * sqrt( H_cur / G );
-      hrp::Vector2 cp_dec_ref_ragulated = cp_dec_ref;
-      if(!isPointInHullOpenCV(cp_dec_ref,hull_d)){
-        calcCrossPointOnHull(com_pos, cp_dec_ref, hull_d, cp_dec_ref_ragulated);
-      }
-      com_vel_decel_ok = (cp_dec_ref_ragulated - com_pos) / sqrt( H_cur / G );
-
-//      //加速CP条件(ACP使用)
-//      hrp::Vector2 cp_acc_tmp = com_pos - com_vel * sqrt( H_cur / G );
-//      hrp::Vector2 cp_acc_ragulated;
-//      if(!isPointInHullOpenCV(cp_acc_tmp,hull_a)){
-//         calcCrossPointOnHull(com_pos, cp_acc_tmp, hull_a, cp_acc_ragulated);
-//         com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
+//      hrp::Vector2 cp_dec_ref = com_forcp_ref + com_vel_forcp_ref * sqrt( H_cur / G );
+//      hrp::Vector2 cp_dec_ref_ragulated = cp_dec_ref;
+//      if(!isPointInHullOpenCV(cp_dec_ref,hull_d)){
+//        calcCrossPointOnHull(com_pos, cp_dec_ref, hull_d, cp_dec_ref_ragulated);
+//      }
+//      com_vel_decel_ok = (cp_dec_ref_ragulated - com_pos) / sqrt( H_cur / G );
+
+      //減速CP条件2(指令のCOMに収束する現在発揮可能なCP)
+//      hrp::Vector2 cp_dec_ref = com_forcp_ref;
+//      hrp::Vector2 cp_dec_ref_ragulated = cp_dec_ref;
+//      if(!isPointInHullOpenCV(cp_dec_ref,hull_d)){
+//        calcCrossPointOnHull(com_pos, cp_dec_ref, hull_d, cp_dec_ref_ragulated);
+//      }
+//      hrp::Vector2 vel_max_decel_ok = (cp_dec_ref_ragulated - com_pos) / sqrt( H_cur / G );
+//      if( com_vel.dot(com_vel) > vel_max_decel_ok.dot(com_vel)){
+//        com_vel_decel_ok = vel_max_decel_ok;
+//      }else{
+//        com_vel_decel_ok = com_vel;
 //      }
 
+      //加速CP条件(ACP使用)
+      hrp::Vector2 cp_acc_tmp = com_pos - com_vel * sqrt( H_cur / G );
+      hrp::Vector2 cp_acc_ragulated;
+      if(!isPointInHullOpenCV(cp_acc_tmp,hull_a)){
+         calcCrossPointOnHull(com_pos, cp_acc_tmp, hull_a, cp_acc_ragulated);
+         com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
+      }
+      cp_acc_old = cp_acc_ragulated;
+
       //加速CP条件2(ZMP使用)
       static hrp::Vector2 com_vel_ans_old = com_vel;
-      hrp::Vector2 com_acc = (com_vel - com_vel_ans_old) / DT;
-      hrp::Vector2 zmp_in = com_pos - com_acc / G * H_cur;
-      hrp::Vector2 zmp_regulated;
-      if(!isPointInHullOpenCV(zmp_in,hull_a)){
-        calcCrossPointOnHull(com_pos, zmp_in, hull_a, zmp_regulated);
-        hrp::Vector2 com_acc_regulated = (com_pos - zmp_regulated)*G/H_cur;
-        com_vel_accel_ok = com_vel_ans_old + com_acc_regulated*DT;
-      }
+//      hrp::Vector2 com_acc = (com_vel - com_vel_ans_old) / DT;
+//      hrp::Vector2 zmp_in = com_pos - com_acc / G * H_cur;
+//      hrp::Vector2 zmp_regulated;
+//      if(!isPointInHullOpenCV(zmp_in,hull_a)){
+//        calcCrossPointOnHull(com_pos, zmp_in, hull_a, zmp_regulated);
+//        hrp::Vector2 com_acc_regulated = (com_pos - zmp_regulated)*G/H_cur;
+//        com_vel_accel_ok = com_vel_ans_old + com_acc_regulated*DT;
+//      }
 
       //加速減速条件マージ
       if( com_vel_decel_ok.dot(com_vel) < com_vel_accel_ok.dot(com_vel)){//normじゃダメ？

From f94fda5a1f374f461de11672ad384657f0fcda65 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 4 Jul 2017 22:30:31 +0900
Subject: [PATCH 103/217] tune for demo

---
 rtc/WholeBodyMasterSlave/wbms_core.h | 25 ++++++++++++++++++++-----
 1 file changed, 20 insertions(+), 5 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index dee5bcf472b..ba1ec3841e2 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -125,10 +125,14 @@ class RobotConfig : UTIL_CONST {
       ee_rot_limit[rf][MAX] = hrp::Vector3( 30*D2R,  30*D2R,   5*D2R);
       ee_rot_limit[lf][MIN] = hrp::Vector3(-30*D2R, -30*D2R,  -5*D2R);
       ee_rot_limit[lf][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  20*D2R);
-      ee_rot_limit[rh][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -30*D2R);
-      ee_rot_limit[rh][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
-      ee_rot_limit[lh][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -30*D2R);
-      ee_rot_limit[lh][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
+//      ee_rot_limit[rh][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -30*D2R);
+//      ee_rot_limit[rh][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
+//      ee_rot_limit[lh][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -30*D2R);
+//      ee_rot_limit[lh][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
+      ee_rot_limit[rh][MIN] = hrp::Vector3(INFMIN, INFMIN, INFMIN);
+      ee_rot_limit[rh][MAX] = hrp::Vector3(INFMAX, INFMAX, INFMAX);
+      ee_rot_limit[lh][MIN] = hrp::Vector3(INFMIN, INFMIN, INFMIN);
+      ee_rot_limit[lh][MAX] = hrp::Vector3(INFMAX, INFMAX, INFMAX);
       ee_rot_limit[head][MIN] = hrp::Vector3( 0*D2R, -20*D2R, -40*D2R);
       ee_rot_limit[head][MAX] = hrp::Vector3( 0*D2R,  30*D2R,  40*D2R);
     }
@@ -154,7 +158,7 @@ class WBMSCore : UTIL_CONST {
     HumanPose hp_wld_raw, hp_plot, rp_ref_out, rp_ref_vel_old;
     FILE *sr_log, *cz_log, *id_log;
     WBMSPose3D baselinkpose;
-    WBMSPose3D ee_contact_pose[4];
+//    WBMSPose3D ee_contact_pose[4];
     hrp::Vector3 com_vel_old,rh_vel_old, cp_dec, cp_acc;
     std::vector<hrp::Vector3> rf_vert,lf_vert;
     hrp::Vector2 com_forcp_ref,com_vel_forcp_ref;
@@ -599,6 +603,17 @@ class WBMSCore : UTIL_CONST {
       LIMIT_MINMAX( out.tgt[rf].abs.p(Z), out.tgt[rf].offs.p(Z), out.tgt[rf].offs.p(Z)+WBMSparam.swing_foot_max_height);
       LIMIT_MINMAX( out.tgt[lf].abs.p(Z), out.tgt[lf].offs.p(Z), out.tgt[lf].offs.p(Z)+WBMSparam.swing_foot_max_height);
 
+      for(int i=0, l[2]={rh,lh}; i<2; i++){
+        LIMIT_MIN(out.tgt[l[i]].abs.p(X), baselinkpose.p(X));
+        LIMIT_MAX(out.tgt[l[i]].abs.p(Z), baselinkpose.p(Z) + 0.4);
+        hrp::Vector2 horizontal_dist(out.tgt[l[i]].abs.p(X) - baselinkpose.p(X), out.tgt[l[i]].abs.p(Y) - baselinkpose.p(Y));
+        if(horizontal_dist.norm() < 0.5){
+          horizontal_dist = 0.5 * horizontal_dist.normalized();
+        }
+        out.tgt[l[i]].abs.p(X) = baselinkpose.p(X) + horizontal_dist(X);
+        out.tgt[l[i]].abs.p(Y) = baselinkpose.p(Y) + horizontal_dist(Y);
+      }
+
       for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( out.tgt[com].abs.rpy(i), rc.ee_rot_limit[com][MIN](i), rc.ee_rot_limit[com][MAX](i) ); }
       LIMIT_MINMAX( out.tgt[com].abs.p(Z), out.tgt[com].offs.p(Z) - 0.15, out.tgt[com].offs.p(Z) + 0.03 );//COM高さ方向の制限
       for(int i=0, l[5]={rf,lf,rh,lh,head}; i<5; i++){

From 7f5e16d41513c6cb7c585e1c4445bab8aabadb78 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 29 Jul 2017 14:39:52 +0900
Subject: [PATCH 104/217] acc test

---
 .../WholeBodyMasterSlave.cpp                  | 151 +++++++++++++++++-
 .../WholeBodyMasterSlave.h                    |   1 +
 2 files changed, 147 insertions(+), 5 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 330b2a0e8bc..9dcd2d718cd 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -323,11 +323,13 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ calcWorldZMPFromInverseDynamics" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
         //角運動量補償
-        if(hsp->WBMSparam.is_doctor){
-          processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, hsp->rp_ref_out);
-        }else{
-          processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, raw_pose);
-        }
+//        if(hsp->WBMSparam.is_doctor){
+//          processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, hsp->rp_ref_out);
+//        }else{
+//          processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, raw_pose);
+//        }
+
+        processBBAccelarationFilter(m_robot, m_robot_rmc);
 
         if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processMomentumCompensation" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
@@ -739,6 +741,145 @@ void WholeBodyMasterSlave::processMomentumCompensation(fikPtr& fik_in, hrp::Body
 }
 
 
+void WholeBodyMasterSlave::processBBAccelarationFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out){
+  static bool first = true;
+  static hrp::dvector qdd, qd_ref, qd_ans, qd_old, q_ref, q_old, q_oldold, t_goal;
+  if(first){
+    qd_ref.resize(robot_in->numJoints());
+    qd_ans.resize(robot_in->numJoints());
+    qdd.resize(robot_in->numJoints());
+    qd_old.resize(robot_in->numJoints());
+    for(int i=0; i < robot_in->numJoints(); i++) qd_old(i) = 0;
+    qd_ans = qd_ref = qdd = qd_old;
+    q_ref.resize(robot_in->numJoints());
+    q_old.resize(robot_in->numJoints());
+    q_oldold.resize(robot_in->numJoints());
+    t_goal.resize(robot_in->numJoints());
+  }
+  for(int i=0; i < robot_in->numJoints(); i++){
+    q_ref(i) = robot_in->joint(i)->q;
+  }
+  if(first){
+    q_oldold = q_old = q_ref;
+  }
+  qd_ref = (q_ref - q_old) / m_dt;
+  qd_old = (q_old - q_oldold) / m_dt;
+  qdd = (q_ref - 2 * q_old + q_oldold) / (m_dt * m_dt);
+
+  const double max_acc = 1;
+  double arrowed_ratio = 1;
+//  for(int i=0; i < robot_in->numJoints(); i++){
+//    if(fabs(qd_ref(i)) > 1e-6){
+//      int direc = (q_ref(i) - q_old(i)) > 0 ? 1 : -1;
+//      double stop_safe_vel_scalar = sqrt( 2 * max_acc * fabs(q_ref(i) - q_old(i)) );
+//  //      double stop_safe_vel = direc * stop_safe_vel_scalar;
+//  //      double vel_ans = qd_old(i) + direc * max_acc * m_dt;
+//
+//      if(direc>0){
+//        if(qd_old(i) > stop_safe_vel_scalar){
+//          qd_ans(i) = qd_old(i) - max_acc * m_dt;
+//        }else{
+//          qd_ans(i) = qd_old(i) + max_acc * m_dt;
+//        }
+//      }else{
+//        if(qd_old(i) < -stop_safe_vel_scalar){
+//          qd_ans(i) = qd_old(i) + max_acc * m_dt;
+//        }else{
+//          qd_ans(i) = qd_old(i) - max_acc * m_dt;
+//        }
+//      }
+////      double ratio = qd_ans(i) / qd_ref(i);
+////      if(ratio < arrowed_ratio)arrowed_ratio = ratio;
+//    }
+//  }
+//  double t_worst = 0;
+//  for(int i=0; i < robot_in->numJoints(); i++){
+//    t_goal(i) = 0;
+//    if(fabs(qd_ref(i)) > 1e-6){
+//      double q_stop = q_old(i) + SGN(qd_old(i))*(qd_old(i)*qd_old(i))/max_acc;
+//      if( (q_ref(i) - q_stop) > 0 ){
+//        qd_ans(i) = qd_old(i) + max_acc * m_dt;
+//
+//        if(qd_old(i)>0){
+//          double t0 = fabs(qd_old(i)) / max_acc;
+//          double t1 = sqrt(max_acc * fabs(q_stop - q_ref(i)));
+//          t_goal(i) = t0 + t1;
+//        }else{
+//          double q_0 = q_old(i) - SGN(qd_old(i)) * ( qd_old(i) * qd_old(i) ) / (2 * max_acc);
+//          double t_all = sqrt(max_acc * fabs(q_ref(i) - q_0));
+//          double t_cur = fabs(qd_old(i)) / max_acc;
+//          t_goal(i) = t_all - t_cur;
+//        }
+//
+//      }else{
+//        qd_ans(i) = qd_old(i) - max_acc * m_dt;
+//
+//        if(qd_old(i)>0){
+//          double t0 = fabs(qd_old(i)) / max_acc;
+//          double t1 = sqrt(max_acc * fabs(q_stop - q_ref(i)));
+//          t_goal(i) = t0 + t1;
+//        }else{
+//          double q_0 = q_old(i) - SGN(qd_old(i)) * ( qd_old(i) * qd_old(i) ) / (2 * max_acc);
+//          double t_all = sqrt(max_acc * fabs(q_ref(i) - q_0));
+//          double t_cur = fabs(qd_old(i)) / max_acc;
+//          t_goal(i) = t_all - t_cur;
+//        }
+//
+//      }
+//    }
+//    if(t_goal(i) > t_worst) t_worst = t_goal(i);
+//  }
+//  for(int i=0; i < robot_in->numJoints(); i++){
+//    if(t_worst != 0.0){
+//      qd_ans(i) *= t_goal(i) / t_worst;
+//    }
+//  }
+  double t_worst = 0;
+  for(int i=0; i < robot_in->numJoints(); i++){
+    t_goal(i) = 0;
+    if(fabs(qd_ref(i)) > 1e-6){
+
+      double q_stop = q_old(i) + SGN(qd_old(i))*(qd_old(i)*qd_old(i))/max_acc;
+
+      qd_ans(i) = qd_old(i) + SGN(q_ref(i) - q_stop) * max_acc * m_dt;
+
+      if(SGN(q_ref(i) - q_stop) * SGN(qd_old(i)) < 0){
+        double t0 = fabs(qd_old(i)) / max_acc;
+        double t1 = sqrt( fabs(q_stop - q_ref(i)) / max_acc);
+        t_goal(i) = t0 + t1;
+      }
+      if(SGN(q_ref(i) - q_stop) * SGN(qd_old(i)) > 0){
+        double q_0 = q_old(i) - SGN(qd_old(i)) * ( qd_old(i) * qd_old(i) ) / (2 * max_acc);
+        double t_all = sqrt( fabs(q_ref(i) - q_0) / max_acc);
+        double t_cur = fabs(qd_old(i)) / max_acc;
+        t_goal(i) = t_all - t_cur;
+      }
+    }
+    if(t_goal(i) > t_worst) t_worst = t_goal(i);
+  }
+  for(int i=0; i < robot_in->numJoints(); i++){
+    if(t_worst != 0.0){
+      qd_ans(i) *= t_goal(i) / t_worst;
+    }
+  }
+
+
+//  std::cout << "arrowed_ratio "<<arrowed_ratio<<std::endl;
+
+  for(int i=0; i < robot_in->numJoints(); i++){
+    robot_out->joint(i)->q = q_old(i) + qd_ans(i) * m_dt;
+  }
+
+  q_oldold = q_old;
+  for(int i=0; i < robot_in->numJoints(); i++){
+    q_old(i) = robot_out->joint(i)->q;
+  }
+
+
+
+  first = false;
+}
+
 bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double sec){//遅い
   if(sec >= 0.0 && sec <= 30.0){
     std::cerr << "[" << m_profile.instance_name << "] start Synchronization after "<<sec<<" [s]" << std::endl;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index eb570b25892..537e49697ec 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -208,6 +208,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
   void processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HumanPose& pose_ref);
   void processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::BodyPtr& robot_in, HumanPose& pose_ref);
   void processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_normal_in, const HumanPose& pose_ref);
+  void processBBAccelarationFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out);
   bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; };
 
 //  void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const hrp::Vector3& max_acc, const double& max_vel, hrp::Vector3& pos_ans);

From 781f46bca70214205af768628f50f277648225b0 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 29 Jul 2017 23:28:08 +0900
Subject: [PATCH 105/217] test vel limnit

---
 .../WholeBodyMasterSlave.cpp                  | 262 ++++++++++++------
 rtc/WholeBodyMasterSlave/wbms_core.h          |  72 ++---
 2 files changed, 212 insertions(+), 122 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 9dcd2d718cd..8afb86a9e3c 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -329,13 +329,45 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 //          processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, raw_pose);
 //        }
 
-        processBBAccelarationFilter(m_robot, m_robot_rmc);
+//        processBBAccelarationFilter(m_robot, m_robot_rmc);
 
         if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processMomentumCompensation" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
+        hsp->over_vel_feedback = 1.0;
+        static hrp::dvector q_old,qd;
+        if(mode.isInitialize()){
+          q_old.resize(m_robot->numJoints());
+          qd.resize(m_robot->numJoints());
+          for (int i=0;i<m_robot->numJoints();i++) q_old(i) = m_robot->joint(i)->q;
+        }
+        for (int i=0;i<m_robot->numJoints();i++){
+          double tmp;
+//          qd(i) = (m_robot->joint(i)->q - q_old(i))/m_dt;
+          qd = idsb.dq;
+          if(fabs(qd(i)) > 3.0){
+            tmp = 3.0 / fabs(qd(i));
+            if(tmp < hsp->over_vel_feedback){
+              hsp->over_vel_feedback = tmp;
+            }
+          }
+        }
+        for (int i=0;i<m_robot->numJoints();i++){
+
+//          m_robot->joint(i)->q = hsp->over_vel_feedback * qd(i) * m_dt + q_old(i);
+          m_robot->joint(i)->q = hsp->over_vel_feedback * idsb.dq(i) * m_dt + idsb.q_oldold(i);
+//          LIMIT_MINMAX(m_robot->joint(i)->q, m_robot->joint(i)->llimit+0.01, m_robot->joint(i)->ulimit-0.01);
+          q_old(i) = m_robot->joint(i)->q;
+        }
+        m_robot->rootLink()->p = hsp->over_vel_feedback * idsb.base_v * m_dt + idsb.base_p_old;
+//        dbg(hsp->over_vel_feedback);
+
+
+
+
         //OutPortデータセット
         hrp::Vector3 ref_zmp = hsp->rp_ref_out.tgt[zmp].abs.p;
-        hrp::BodyPtr m_robot_for_out = m_robot_rmc;
+//        hrp::BodyPtr m_robot_for_out = m_robot_rmc;
+        hrp::BodyPtr m_robot_for_out = m_robot;
         // qRef
         for (int i = 0; i < m_qRef.data.length(); i++ ){ m_qRef.data[i] = transition_interpolator_ratio * m_robot_for_out->joint(i)->q  + (1 - transition_interpolator_ratio) * m_qRef.data[i]; }
         // basePos
@@ -533,6 +565,29 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::
 }
 
 
+//void WholeBodyMasterSlave::calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robot_in){
+//  const std::string names[4] = {"rleg","lleg","rarm","larm"};
+//  hrp::dmatrix J,Jinv,Jnull;
+//  for(int l=0; l<4; l++){
+//    if(fik_in->ikp.count(names[l])){
+//      fik_in->ikp[names[l]].manip->calcJacobian(J);
+//      fik_in->ikp[names[l]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
+//      Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
+//  //    Eigen::MatrixXd::Index row,col;
+//  //    hsp->cur_manip_val[l] = svd.singularValues().minCoeff(&row,&col);
+//  //    hsp->min_manip_direc[l] = svd.matrixU().col(row);// unit vector
+//      hsp->manip_mat[l] = svd.matrixU();
+//      for(int i=0;i<3;i++){
+//        hsp->manip_sv[l](i) = svd.singularValues()(i);
+//  //      hsp->manip_mat[l].col(i) *= svd.singularValues()(i);
+//  //      hsp->cur_manip_val[l][i] = svd.singularValues()(i);
+//  //      hsp->manip_direc[l][i] = svd.matrixU().col(i);// unit vector
+//      }
+//  //    dbg((hsp->manip_sv[l]).transpose());
+//  //    if(hsp->manip_direc[l].dot(fik_in->ikp[names[l]].target_p0 - robot_in->rootLink()->p) < 0){ hsp->manip_direc[l] *= -1; }//向きみて反転(常に外側正)
+//    }
+//  }
+//}
 void WholeBodyMasterSlave::calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robot_in){
   const std::string names[4] = {"rleg","lleg","rarm","larm"};
   hrp::dmatrix J,Jinv,Jnull;
@@ -541,18 +596,13 @@ void WholeBodyMasterSlave::calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robo
       fik_in->ikp[names[l]].manip->calcJacobian(J);
       fik_in->ikp[names[l]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
       Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
-  //    Eigen::MatrixXd::Index row,col;
-  //    hsp->cur_manip_val[l] = svd.singularValues().minCoeff(&row,&col);
-  //    hsp->min_manip_direc[l] = svd.matrixU().col(row);// unit vector
+      Eigen::JacobiSVD< Eigen::MatrixXd > svd_rot(J.block(3,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
       hsp->manip_mat[l] = svd.matrixU();
+      hsp->manip_mat_rot[l] = svd_rot.matrixU();
       for(int i=0;i<3;i++){
         hsp->manip_sv[l](i) = svd.singularValues()(i);
-  //      hsp->manip_mat[l].col(i) *= svd.singularValues()(i);
-  //      hsp->cur_manip_val[l][i] = svd.singularValues()(i);
-  //      hsp->manip_direc[l][i] = svd.matrixU().col(i);// unit vector
+        hsp->manip_sv_rot[l](i) = svd_rot.singularValues()(i);
       }
-  //    dbg((hsp->manip_sv[l]).transpose());
-  //    if(hsp->manip_direc[l].dot(fik_in->ikp[names[l]].target_p0 - robot_in->rootLink()->p) < 0){ hsp->manip_direc[l] *= -1; }//向きみて反転(常に外側正)
     }
   }
 }
@@ -766,80 +816,14 @@ void WholeBodyMasterSlave::processBBAccelarationFilter(hrp::BodyPtr& robot_in, h
   qd_old = (q_old - q_oldold) / m_dt;
   qdd = (q_ref - 2 * q_old + q_oldold) / (m_dt * m_dt);
 
-  const double max_acc = 1;
-  double arrowed_ratio = 1;
-//  for(int i=0; i < robot_in->numJoints(); i++){
-//    if(fabs(qd_ref(i)) > 1e-6){
-//      int direc = (q_ref(i) - q_old(i)) > 0 ? 1 : -1;
-//      double stop_safe_vel_scalar = sqrt( 2 * max_acc * fabs(q_ref(i) - q_old(i)) );
-//  //      double stop_safe_vel = direc * stop_safe_vel_scalar;
-//  //      double vel_ans = qd_old(i) + direc * max_acc * m_dt;
-//
-//      if(direc>0){
-//        if(qd_old(i) > stop_safe_vel_scalar){
-//          qd_ans(i) = qd_old(i) - max_acc * m_dt;
-//        }else{
-//          qd_ans(i) = qd_old(i) + max_acc * m_dt;
-//        }
-//      }else{
-//        if(qd_old(i) < -stop_safe_vel_scalar){
-//          qd_ans(i) = qd_old(i) + max_acc * m_dt;
-//        }else{
-//          qd_ans(i) = qd_old(i) - max_acc * m_dt;
-//        }
-//      }
-////      double ratio = qd_ans(i) / qd_ref(i);
-////      if(ratio < arrowed_ratio)arrowed_ratio = ratio;
-//    }
-//  }
-//  double t_worst = 0;
-//  for(int i=0; i < robot_in->numJoints(); i++){
-//    t_goal(i) = 0;
-//    if(fabs(qd_ref(i)) > 1e-6){
-//      double q_stop = q_old(i) + SGN(qd_old(i))*(qd_old(i)*qd_old(i))/max_acc;
-//      if( (q_ref(i) - q_stop) > 0 ){
-//        qd_ans(i) = qd_old(i) + max_acc * m_dt;
-//
-//        if(qd_old(i)>0){
-//          double t0 = fabs(qd_old(i)) / max_acc;
-//          double t1 = sqrt(max_acc * fabs(q_stop - q_ref(i)));
-//          t_goal(i) = t0 + t1;
-//        }else{
-//          double q_0 = q_old(i) - SGN(qd_old(i)) * ( qd_old(i) * qd_old(i) ) / (2 * max_acc);
-//          double t_all = sqrt(max_acc * fabs(q_ref(i) - q_0));
-//          double t_cur = fabs(qd_old(i)) / max_acc;
-//          t_goal(i) = t_all - t_cur;
-//        }
-//
-//      }else{
-//        qd_ans(i) = qd_old(i) - max_acc * m_dt;
-//
-//        if(qd_old(i)>0){
-//          double t0 = fabs(qd_old(i)) / max_acc;
-//          double t1 = sqrt(max_acc * fabs(q_stop - q_ref(i)));
-//          t_goal(i) = t0 + t1;
-//        }else{
-//          double q_0 = q_old(i) - SGN(qd_old(i)) * ( qd_old(i) * qd_old(i) ) / (2 * max_acc);
-//          double t_all = sqrt(max_acc * fabs(q_ref(i) - q_0));
-//          double t_cur = fabs(qd_old(i)) / max_acc;
-//          t_goal(i) = t_all - t_cur;
-//        }
-//
-//      }
-//    }
-//    if(t_goal(i) > t_worst) t_worst = t_goal(i);
-//  }
-//  for(int i=0; i < robot_in->numJoints(); i++){
-//    if(t_worst != 0.0){
-//      qd_ans(i) *= t_goal(i) / t_worst;
-//    }
-//  }
+  const double max_acc = 3;
+
   double t_worst = 0;
   for(int i=0; i < robot_in->numJoints(); i++){
     t_goal(i) = 0;
     if(fabs(qd_ref(i)) > 1e-6){
 
-      double q_stop = q_old(i) + SGN(qd_old(i))*(qd_old(i)*qd_old(i))/max_acc;
+      double q_stop = q_old(i) + SGN(qd_old(i)) * ( qd_old(i)*qd_old(i) ) / ( 2 * max_acc ) * 1.01;
 
       qd_ans(i) = qd_old(i) + SGN(q_ref(i) - q_stop) * max_acc * m_dt;
 
@@ -847,38 +831,136 @@ void WholeBodyMasterSlave::processBBAccelarationFilter(hrp::BodyPtr& robot_in, h
         double t0 = fabs(qd_old(i)) / max_acc;
         double t1 = sqrt( fabs(q_stop - q_ref(i)) / max_acc);
         t_goal(i) = t0 + t1;
+        if(i==15)fprintf(hsp->id_log,"%f %f %f %f %f 0 0 %d\n", q_ref(i), q_old(i), t_goal(i), t0, t1, 1);
       }
       if(SGN(q_ref(i) - q_stop) * SGN(qd_old(i)) > 0){
         double q_0 = q_old(i) - SGN(qd_old(i)) * ( qd_old(i) * qd_old(i) ) / (2 * max_acc);
-        double t_all = sqrt( fabs(q_ref(i) - q_0) / max_acc);
+        double t_all = sqrt( fabs(q_ref(i) - q_0) / max_acc) * 2;
         double t_cur = fabs(qd_old(i)) / max_acc;
         t_goal(i) = t_all - t_cur;
+        if(i==15)fprintf(hsp->id_log,"%f %f %f 0 0 %f %f %d\n", q_ref(i), q_old(i), t_goal(i), t_all, t_cur, 2);
       }
+
+    }else{
+      if(i==15)fprintf(hsp->id_log,"%f %f 0 0 0 0 0 %d\n", q_ref(i), q_old(i), 0);
     }
     if(t_goal(i) > t_worst) t_worst = t_goal(i);
+
   }
   for(int i=0; i < robot_in->numJoints(); i++){
     if(t_worst != 0.0){
-      qd_ans(i) *= t_goal(i) / t_worst;
+//      qd_ans(i) *= t_goal(i) / t_worst;
     }
   }
-
-
-//  std::cout << "arrowed_ratio "<<arrowed_ratio<<std::endl;
-
-  for(int i=0; i < robot_in->numJoints(); i++){
+  for(int i=12; i < robot_in->numJoints(); i++){
     robot_out->joint(i)->q = q_old(i) + qd_ans(i) * m_dt;
   }
-
+  for(int i=0; i < 12; i++){
+    robot_out->joint(i)->q = robot_in->joint(i)->q;
+  }
   q_oldold = q_old;
-  for(int i=0; i < robot_in->numJoints(); i++){
+  for(int i=12; i < robot_in->numJoints(); i++){
     q_old(i) = robot_out->joint(i)->q;
   }
+  robot_out->rootLink()->p = robot_in->rootLink()->p;
+  robot_out->rootLink()->R = robot_in->rootLink()->R;
+  first = false;
+}
 
+//void WholeBodyMasterSlave::processBBAccelarationFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out){
+//  static bool first = true;
+//  static hrp::dvector qdd, qd_ref, qd_ans, qd_old, q_ref, q_old, q_oldold, t_goal;
+//  if(first){
+//    qd_ref.resize(robot_in->numJoints());
+//    qd_ans.resize(robot_in->numJoints());
+//    qdd.resize(robot_in->numJoints());
+//    qd_old.resize(robot_in->numJoints());
+//    for(int i=0; i < robot_in->numJoints(); i++) qd_old(i) = 0;
+//    qd_ans = qd_ref = qdd = qd_old;
+//    q_ref.resize(robot_in->numJoints());
+//    q_old.resize(robot_in->numJoints());
+//    q_oldold.resize(robot_in->numJoints());
+//    t_goal.resize(robot_in->numJoints());
+//  }
+//  for(int i=0; i < robot_in->numJoints(); i++){
+//    q_ref(i) = robot_in->joint(i)->q;
+//  }
+//  if(first){
+//    q_oldold = q_old = q_ref;
+//  }
+//
+//
+//  static interpolator *ip;
+//  if(first){
+//    ip = new interpolator(robot_in->numJoints(), m_dt, interpolator::HOFFARBIB, 1);
+//    ip->setName(std::string(m_profile.instance_name)+" ip");
+//    ip->clear();
+//    ip->set(q_ref.data());
+//  }
+//  ip->setGoal(q_ref.data(), 0.3, true);
+//
+//  qd_ref = (q_ref - q_old) / m_dt;
+//  qd_old = (q_old - q_oldold) / m_dt;
+//  qdd = (q_ref - 2 * q_old + q_oldold) / (m_dt * m_dt);
+//
+////  const double max_acc = 3;
+////
+////  double t_worst = 0;
+////  for(int i=0; i < robot_in->numJoints(); i++){
+////    t_goal(i) = 0;
+////    if(fabs(qd_ref(i)) > 1e-6){
+////
+////      double q_stop = q_old(i) + SGN(qd_old(i)) * ( qd_old(i)*qd_old(i) ) / ( 2 * max_acc ) * 1.01;
+////
+////      qd_ans(i) = qd_old(i) + SGN(q_ref(i) - q_stop) * max_acc * m_dt;
+////
+////      if(SGN(q_ref(i) - q_stop) * SGN(qd_old(i)) < 0){
+////        double t0 = fabs(qd_old(i)) / max_acc;
+////        double t1 = sqrt( fabs(q_stop - q_ref(i)) / max_acc);
+////        t_goal(i) = t0 + t1;
+////        if(i==15)fprintf(hsp->id_log,"%f %f %f %f %f 0 0 %d\n", q_ref(i), q_old(i), t_goal(i), t0, t1, 1);
+////      }
+////      if(SGN(q_ref(i) - q_stop) * SGN(qd_old(i)) > 0){
+////        double q_0 = q_old(i) - SGN(qd_old(i)) * ( qd_old(i) * qd_old(i) ) / (2 * max_acc);
+////        double t_all = sqrt( fabs(q_ref(i) - q_0) / max_acc) * 2;
+////        double t_cur = fabs(qd_old(i)) / max_acc;
+////        t_goal(i) = t_all - t_cur;
+////        if(i==15)fprintf(hsp->id_log,"%f %f %f 0 0 %f %f %d\n", q_ref(i), q_old(i), t_goal(i), t_all, t_cur, 2);
+////      }
+////
+////    }else{
+////      if(i==15)fprintf(hsp->id_log,"%f %f 0 0 0 0 0 %d\n", q_ref(i), q_old(i), 0);
+////    }
+////    if(t_goal(i) > t_worst) t_worst = t_goal(i);
+////
+////  }
+//
+//  double q_ans[robot_in->numJoints()];
+//  if (!ip->isEmpty() ){
+//    ip->get(q_ans, true);
+//  }
+////  for(int i=0; i < robot_in->numJoints(); i++){
+////    if(t_worst != 0.0){
+////      qd_ans(i) *= t_goal(i) / t_worst;
+////    }
+////    fprintf(hsp->id_log,"%f ", t_goal(i));
+////  }
+////  fprintf(hsp->id_log,"\n");
+//
+//
+////  std::cout << "arrowed_ratio "<<arrowed_ratio<<std::endl;
+//
+//  for(int i=0; i < robot_in->numJoints(); i++){
+//    robot_out->joint(i)->q = q_ans[i];
+//  }
+//
+//  robot_out->rootLink()->p = robot_in->rootLink()->p;
+//  robot_out->rootLink()->R = robot_in->rootLink()->R;
+//
+//  first = false;
+//}
 
 
-  first = false;
-}
 
 bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double sec){//遅い
   if(sec >= 0.0 && sec <= 30.0){
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index ba1ec3841e2..ecd832b15c5 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -133,8 +133,8 @@ class RobotConfig : UTIL_CONST {
       ee_rot_limit[rh][MAX] = hrp::Vector3(INFMAX, INFMAX, INFMAX);
       ee_rot_limit[lh][MIN] = hrp::Vector3(INFMIN, INFMIN, INFMIN);
       ee_rot_limit[lh][MAX] = hrp::Vector3(INFMAX, INFMAX, INFMAX);
-      ee_rot_limit[head][MIN] = hrp::Vector3( 0*D2R, -20*D2R, -40*D2R);
-      ee_rot_limit[head][MAX] = hrp::Vector3( 0*D2R,  30*D2R,  40*D2R);
+      ee_rot_limit[head][MIN] = hrp::Vector3( 0*D2R, -20*D2R, -35*D2R);
+      ee_rot_limit[head][MAX] = hrp::Vector3( 0*D2R,  30*D2R,  35*D2R);
     }
 };
 
@@ -186,8 +186,11 @@ class WBMSCore : UTIL_CONST {
     hrp::Vector3 manip_direc[4][3];
     hrp::Matrix33 manip_mat[4];
     hrp::Vector3 manip_sv[4];
+    hrp::Matrix33 manip_mat_rot[4];
+    hrp::Vector3 manip_sv_rot[4];
 
     hrp::Vector2 cp_acc_old;
+    double over_vel_feedback;
 
     WBMSCore(const double& dt){
       tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
@@ -246,6 +249,7 @@ class WBMSCore : UTIL_CONST {
       hull_dcp.reserve(6);
       hull_dcp.reserve(6);
       cvhull.reserve(6);
+      over_vel_feedback = 1.0;
       if(DEBUG){
         sr_log = fopen("/home/ishiguro/HumanSync_support_region.log","w+");
         cz_log = fopen("/home/ishiguro/HumanSync_com_zmp.log","w+");
@@ -329,8 +333,6 @@ class WBMSCore : UTIL_CONST {
 
       applyLPFilter_pre                       (rp_ref_out);
       applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
-//      dbg(rp_ref_out.tgt[rf].go_contact);
-//      dbg(rp_ref_out.tgt[lf].go_contact);
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
       applyLPFilter_post                       (rp_ref_out);
 
@@ -403,27 +405,26 @@ class WBMSCore : UTIL_CONST {
         else if (!in.tgt[l[i]].is_contact && in.tgt[l[i]].w(fz)>CNT_F_TH+30){in.tgt[l[i]].is_contact = true;}//足浮いた状態から下げる
       }
     }
-    void applyLPFilter(HumanPose& tgt){
-      if(is_initial_loop){
-        for(int i=0, l[1]={com}; i<1; i++){
-          tgt_pos_filters[l[i]].reset(tgt.tgt[l[i]].abs.p);
-          tgt_rot_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy);
-        }
-      }
-      for(int i=0, l[1]={com}; i<1; i++){
-        tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
-        tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
-      }
-//      cout<<"calcVelAccSafeTrajectoryVecML"<<endl;
-      for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
-        calcVelAccSafeTrajectoryVecML(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, manip_mat[i], manip_sv[i], tgt.tgt[l[i]].abs.p);
-        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
-      }
-      for(int i=0, l[1]={head}; i<1; i++){
-        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, 1.0, tgt.tgt[l[i]].abs.p);
-        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
-      }
-    }
+//    void applyLPFilter(HumanPose& tgt){
+//      if(is_initial_loop){
+//        for(int i=0, l[1]={com}; i<1; i++){
+//          tgt_pos_filters[l[i]].reset(tgt.tgt[l[i]].abs.p);
+//          tgt_rot_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy);
+//        }
+//      }
+//      for(int i=0, l[1]={com}; i<1; i++){
+//        tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
+//        tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
+//      }
+//      for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
+//        calcVelAccSafeTrajectoryVecML(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, manip_mat[i], manip_sv[i], tgt.tgt[l[i]].abs.p);
+//        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+//      }
+//      for(int i=0, l[1]={head}; i<1; i++){
+//        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, 1.0, tgt.tgt[l[i]].abs.p);
+//        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+//      }
+//    }
     void applyLPFilter_pre(HumanPose& tgt){
 //      if(is_initial_loop){
 //        for(int i=0, l[1]={com}; i<1; i++){
@@ -435,10 +436,16 @@ class WBMSCore : UTIL_CONST {
 //        tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
 //        tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
 //      }
-      for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
-        calcVelAccSafeTrajectoryVecML(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, manip_mat[i], manip_sv[i], tgt.tgt[l[i]].abs.p);
+      for(int i=0, l[2]={rf,lf}; i<2; i++){
+        calcVelAccSafeTrajectoryVecML(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, manip_mat[i], manip_sv[i], tgt.tgt[l[i]].abs.p);
         calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
       }
+      for(int i=0, l[2]={rh,lh}; i<2; i++){
+//        calcVelAccSafeTrajectoryVecML(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 3, manip_mat[i], manip_sv[i], tgt.tgt[l[i]].abs.p);
+//        calcVelAccSafeTrajectoryVecML(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3, manip_mat_rot[i], manip_sv_rot[i], tgt.tgt[l[i]].abs.rpy);
+        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 3.0, 1.0, tgt.tgt[l[i]].abs.p);
+        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+      }
       for(int i=0, l[1]={head}; i<1; i++){
         calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, 1.0, tgt.tgt[l[i]].abs.p);
         calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
@@ -465,16 +472,16 @@ class WBMSCore : UTIL_CONST {
 //          vel_ans = vel_ans.normalized() * stop_safe_vel_scalar;
           vel_ans = vel_ans * stop_safe_vel_scalar / vel_ans.dot(direc);
         }
-        pos_ans = pos_cur + vel_ans * DT;
+//        pos_ans = pos_cur + vel_ans * DT;
+        pos_ans = pos_cur + over_vel_feedback * vel_ans * DT;
       }else{
         pos_ans = pos_tgt;
       }
     }
-    void calcVelAccSafeTrajectoryVecML(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const hrp::Matrix33& max_acc_mat, const hrp::Vector3& max_acc_sv, hrp::Vector3& pos_ans){
+    void calcVelAccSafeTrajectoryVecML(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& acc_base, const hrp::Matrix33& max_acc_mat, const hrp::Vector3& max_acc_sv, hrp::Vector3& pos_ans){
       if((pos_tgt - pos_cur).norm() > 1.0e-6){
         hrp::Vector3 direc = (pos_tgt - pos_cur).normalized();
-        double acc_val = 1.0;
-        hrp::Vector3 ref_acc = direc*acc_val;
+        hrp::Vector3 ref_acc = direc*acc_base;
         hrp::Vector3 mod_acc = max_acc_mat * max_acc_sv.asDiagonal() * max_acc_mat.transpose() * ref_acc;
 //        cout<<"ref_acc "<<ref_acc.transpose()<<" mod_acc "<<mod_acc.transpose()<<endl;
 //        dbg(max_acc_mat);
@@ -487,7 +494,8 @@ class WBMSCore : UTIL_CONST {
 //          vel_ans = vel_ans.normalized() * stop_safe_vel_scalar;
           vel_ans = vel_ans * stop_safe_vel_scalar / vel_ans.dot(direc);
         }
-        pos_ans = pos_cur + vel_ans * DT;
+//        pos_ans = pos_cur + vel_ans * DT;
+        pos_ans = pos_cur + over_vel_feedback * vel_ans * DT;
       }else{
         pos_ans = pos_tgt;
       }

From 322e7ce3ff465316fda698b334b60a74e20c4218 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 30 Jul 2017 02:18:16 +0900
Subject: [PATCH 106/217] vel limit ok?

---
 .../WholeBodyMasterSlave.cpp                  | 375 +++---------------
 .../WholeBodyMasterSlave.h                    |   4 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 200 +++-------
 3 files changed, 115 insertions(+), 464 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 8afb86a9e3c..c878f190cd3 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -141,6 +141,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     m_robot = hrp::BodyPtr(new hrp::Body());
     m_robot_rmc = hrp::BodyPtr(new hrp::Body());
     m_robot_ml = hrp::BodyPtr(new hrp::Body());
+    m_robot_vsafe = hrp::BodyPtr(new hrp::Body());
     RTC::Manager& rtcManager = RTC::Manager::instance();
     std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
     int comPos = nameServer.find(",");
@@ -159,6 +160,10 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
       std::cerr << "[" << m_profile.instance_name << "] failed to load model 2 [" << prop["model"] << "]" << std::endl;
       return RTC::RTC_ERROR;
     }
+    if (!loadBodyFromModelLoader(m_robot_vsafe, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
+      std::cerr << "[" << m_profile.instance_name << "] failed to load model 2 [" << prop["model"] << "]" << std::endl;
+      return RTC::RTC_ERROR;
+    }
     // allocate memory for outPorts
     m_qRef.data.length(m_robot->numJoints());
     m_htrfw.data.length(6);
@@ -177,12 +182,15 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
     fik_rmc = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot_rmc, std::string(m_profile.instance_name), m_dt));
     fik_ml = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot_ml, std::string(m_profile.instance_name), m_dt));
+    fik_vsafe = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot_vsafe, std::string(m_profile.instance_name), m_dt));
     fik_list.push_back(fik);
     fik_list.push_back(fik_rmc);
     fik_list.push_back(fik_ml);
+    fik_list.push_back(fik_vsafe);
     body_list.push_back(m_robot);
     body_list.push_back(m_robot_rmc);
     body_list.push_back(m_robot_ml);
+    body_list.push_back(m_robot_vsafe);
     for(int i=0;i<fik_list.size();i++){ setupfik(fik_list[i], body_list[i], prop); }
 
     if (fik->ikp.find("rleg") != fik->ikp.end() && fik->ikp.find("lleg") != fik->ikp.end()) {
@@ -282,7 +290,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
       if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processTransition" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
-
       for(int i=0; i<fik_list.size(); i++){
         fik_list[i]->ikp["rleg"].is_ik_enable = true;
         fik_list[i]->ikp["lleg"].is_ik_enable = true;
@@ -293,11 +300,11 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
       if (mode.isRunning()) {
         if(mode.isInitialize()){
           preProcessForWholeBodyMasterSlave(fik, m_robot);
+          hsp->fik_ml = fik_ml;
+          hsp->m_robot_ml = m_robot_ml;
+          hsp->fik_act = fik_vsafe;
+          hsp->m_robot_act = m_robot_vsafe;
         }
-        m_robot_ml->rootLink()->p = m_robot_rmc->rootLink()->p;
-        m_robot_ml->rootLink()->R = m_robot_rmc->rootLink()->R;
-        hsp->fik = fik_ml;
-        hsp->m_robot = m_robot_ml;
         calcManipulability(fik_rmc, m_robot_rmc);
 
         if(hsp->WBMSparam.is_doctor){
@@ -328,46 +335,30 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 //        }else{
 //          processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, raw_pose);
 //        }
-
 //        processBBAccelarationFilter(m_robot, m_robot_rmc);
 
         if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processMomentumCompensation" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
-        hsp->over_vel_feedback = 1.0;
-        static hrp::dvector q_old,qd;
-        if(mode.isInitialize()){
-          q_old.resize(m_robot->numJoints());
-          qd.resize(m_robot->numJoints());
-          for (int i=0;i<m_robot->numJoints();i++) q_old(i) = m_robot->joint(i)->q;
-        }
+        double safe_vel_ratio = 1.0;
+        const double qd_max = 3.0;
         for (int i=0;i<m_robot->numJoints();i++){
-          double tmp;
-//          qd(i) = (m_robot->joint(i)->q - q_old(i))/m_dt;
-          qd = idsb.dq;
-          if(fabs(qd(i)) > 3.0){
-            tmp = 3.0 / fabs(qd(i));
-            if(tmp < hsp->over_vel_feedback){
-              hsp->over_vel_feedback = tmp;
-            }
+          double qd = (m_robot->joint(i)->q - m_robot_vsafe->joint(i)->q) / m_dt;
+          if(fabs(qd) > qd_max){
+            double tmp_ratio = qd_max / fabs(qd);
+            if(tmp_ratio < safe_vel_ratio){ safe_vel_ratio = tmp_ratio; }
           }
         }
         for (int i=0;i<m_robot->numJoints();i++){
-
-//          m_robot->joint(i)->q = hsp->over_vel_feedback * qd(i) * m_dt + q_old(i);
-          m_robot->joint(i)->q = hsp->over_vel_feedback * idsb.dq(i) * m_dt + idsb.q_oldold(i);
-//          LIMIT_MINMAX(m_robot->joint(i)->q, m_robot->joint(i)->llimit+0.01, m_robot->joint(i)->ulimit-0.01);
-          q_old(i) = m_robot->joint(i)->q;
+          m_robot_vsafe->joint(i)->q += safe_vel_ratio * (m_robot->joint(i)->q - m_robot_vsafe->joint(i)->q);
         }
-        m_robot->rootLink()->p = hsp->over_vel_feedback * idsb.base_v * m_dt + idsb.base_p_old;
-//        dbg(hsp->over_vel_feedback);
-
-
+        m_robot_vsafe->rootLink()->p += safe_vel_ratio * (m_robot->rootLink()->p - m_robot_vsafe->rootLink()->p);
+        m_robot_vsafe->rootLink()->R += safe_vel_ratio * (m_robot->rootLink()->R - m_robot_vsafe->rootLink()->R);
+        m_robot_vsafe->calcForwardKinematics();
 
+        const hrp::BodyPtr m_robot_for_out = m_robot_vsafe;
 
         //OutPortデータセット
         hrp::Vector3 ref_zmp = hsp->rp_ref_out.tgt[zmp].abs.p;
-//        hrp::BodyPtr m_robot_for_out = m_robot_rmc;
-        hrp::BodyPtr m_robot_for_out = m_robot;
         // qRef
         for (int i = 0; i < m_qRef.data.length(); i++ ){ m_qRef.data[i] = transition_interpolator_ratio * m_robot_for_out->joint(i)->q  + (1 - transition_interpolator_ratio) * m_qRef.data[i]; }
         // basePos
@@ -532,16 +523,10 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(fikPtr& fik_in, hrp::Body
   hsp->update();//////HumanSynchronizerの主要処理
   if(DEBUGP)cout<<"update():"<<hsp->getUpdateTime()<<endl;
   if(DEBUGP)pose_ref.print();
-
   WBMSPose3D rf_checked = pose_ref.tgt[rf].abs;
   WBMSPose3D lf_checked = pose_ref.tgt[lf].abs;
   WBMSPose3D rh_checked = pose_ref.tgt[rh].abs;
   WBMSPose3D lh_checked = pose_ref.tgt[lh].abs;
-
-//  static hrp::Vector3 target_v_old[4];
-//  if(hsp->WBMSparam.use_manipulability_limit){
-//    calcManipulabilityJointLimit(fik_in, robot_in, target_v_old, rf_checked, lf_checked, rh_checked, lh_checked);
-//  }
   solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, rf_checked, lf_checked, rh_checked, lh_checked, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave");
 }
 
@@ -564,30 +549,6 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::
   solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave_Raw");
 }
 
-
-//void WholeBodyMasterSlave::calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robot_in){
-//  const std::string names[4] = {"rleg","lleg","rarm","larm"};
-//  hrp::dmatrix J,Jinv,Jnull;
-//  for(int l=0; l<4; l++){
-//    if(fik_in->ikp.count(names[l])){
-//      fik_in->ikp[names[l]].manip->calcJacobian(J);
-//      fik_in->ikp[names[l]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
-//      Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
-//  //    Eigen::MatrixXd::Index row,col;
-//  //    hsp->cur_manip_val[l] = svd.singularValues().minCoeff(&row,&col);
-//  //    hsp->min_manip_direc[l] = svd.matrixU().col(row);// unit vector
-//      hsp->manip_mat[l] = svd.matrixU();
-//      for(int i=0;i<3;i++){
-//        hsp->manip_sv[l](i) = svd.singularValues()(i);
-//  //      hsp->manip_mat[l].col(i) *= svd.singularValues()(i);
-//  //      hsp->cur_manip_val[l][i] = svd.singularValues()(i);
-//  //      hsp->manip_direc[l][i] = svd.matrixU().col(i);// unit vector
-//      }
-//  //    dbg((hsp->manip_sv[l]).transpose());
-//  //    if(hsp->manip_direc[l].dot(fik_in->ikp[names[l]].target_p0 - robot_in->rootLink()->p) < 0){ hsp->manip_direc[l] *= -1; }//向きみて反転(常に外側正)
-//    }
-//  }
-//}
 void WholeBodyMasterSlave::calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robot_in){
   const std::string names[4] = {"rleg","lleg","rarm","larm"};
   hrp::dmatrix J,Jinv,Jnull;
@@ -607,122 +568,6 @@ void WholeBodyMasterSlave::calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robo
   }
 }
 
-//これ超計算重い
-void WholeBodyMasterSlave::calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::Vector3 target_v_old[], WBMSPose3D& rf_ref, WBMSPose3D& lf_ref, WBMSPose3D& rh_ref, WBMSPose3D& lh_ref){
-  const std::string names[4] = {"rleg","lleg","rarm","larm"};
-  WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
-  double min_manip_val = 1e9;
-  const double min_manip_th = 0.1;
-  hrp::Vector3 target_p0_cur[4], target_p0_new[4];
-  hrp::dmatrix J,Jinv,Jnull;
-
-  for(int l=0; l<4; l++){
-    fik_in->ikp[names[l]].manip->calcJacobian(J);
-    fik_in->ikp[names[l]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
-//    hrp::dmatrix manipulability_mat = J*J.transpose();
-//    double manipulability = sqrt((J*J.transpose()).determinant());
-    Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
-    Eigen::MatrixXd::Index row,col;
-    min_manip_val = svd.singularValues().minCoeff(&row,&col);
-    hrp::Vector3 min_manip_direc = svd.matrixU().col(row);// unit vector
-    if(min_manip_direc.dot(fik_in->ikp[names[l]].target_p0 - robot_in->rootLink()->p) < 0){ min_manip_direc *= -1; }//向きみて反転(常に外側正)
-//    target_p0_cur[i] = fik_in->ikp[names[i]].target_p0;
-    target_p0_cur[l] = fik_in->ikp[names[l]].target_link->p + fik_in->ikp[names[l]].target_link->R * fik_in->ikp[names[l]].localPos;
-    target_p0_new[l] = refs[l]->p;
-
-    hrp::Vector3 ref_ee_vel = (target_p0_new[l] - target_p0_cur[l]) / m_dt;
-
-    double penalty = 1 - (min_manip_val - min_manip_th) * 10;
-    LIMIT_MINMAX(penalty, 0.0, 1.0);//0~1に頭打ち　//thより大で0, thに近づくにつれ0->1, th以下でも1
-
-    double lim_penalty = 1.0;
-    double lim_penalty_tmp = 1.0;
-    double ulim_mergin[6], llim_mergin[6];
-    static double ulim_mergin_old[6], llim_mergin_old[6];
-    const double jlim_start_th = 30*D2R;
-
-    hrp::dvector q_vel(6);
-    q_vel = Jinv.block(0,0,Jinv.cols(),3) * ref_ee_vel;
-    for(int j=0;j<6;j++){
-      llim_mergin[j] = fabs(robot_in->joint(j)->llimit - robot_in->joint(j)->q);
-      ulim_mergin[j] = fabs(robot_in->joint(j)->ulimit - robot_in->joint(j)->q);
-      if( llim_mergin[j] < jlim_start_th && q_vel[j] < 0){
-        lim_penalty_tmp =  llim_mergin[j]/jlim_start_th;
-        if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
-      }
-      if( ulim_mergin[j] < jlim_start_th && q_vel[j] > 0){
-        lim_penalty_tmp =  ulim_mergin[j]/jlim_start_th;
-        if(lim_penalty_tmp < lim_penalty){lim_penalty = lim_penalty_tmp;}
-      }
-    }
-//    if(DEBUGP){ if(lim_penalty<1.0)cout<<"lim_penalty: "<<lim_penalty<<endl; }
-//    double max_dec_vel = 1.0;// m/s
-//    hrp::Vector3 ref_ee_vel_v = (ref_ee_vel.dot(min_manip_direc)) * min_manip_direc;
-//    hrp::Vector3 ref_ee_vel_h = ref_ee_vel - ref_ee_vel_v;
-//        ref_vec_mod -= (ref_vec.dot(min_manip_direc)) * min_manip_direc;
-//        ref_vec_mod += (ref_vec.dot(min_manip_direc)) * min_manip_direc * penalty;
-//    max_dec_vel *= penalty;
-//    if(ref_ee_vel_v.dot(min_manip_direc) > max_dec_vel){
-//      ref_ee_vel_v = min_manip_direc * max_dec_vel;
-//    }
-//    if( (min_manip_val - min_manip_th) <0 ){
-//      ref_ee_vel_v = min_manip_direc * (min_manip_val - min_manip_th) * 10;
-//    }
-//    hrp::Vector3 ref_ee_vel_mod = ref_ee_vel_h + ref_ee_vel_v;//速度空間で一次フィルターしたほうがいい?
-//    ref_ee_vel_mod *= lim_penalty;
-//    penalty *= lim_penalty;
-    hrp::Vector3 max_acc_ratio = hrp::Vector3(1,1,1);
-
-//    LIMIT_MINMAX(penalty, 0.1, 1.0);
-    for(int i=0;i<3;i++){
-      max_acc_ratio(i) -= fabs(min_manip_direc(i)) * penalty;
-    }
-    hrp::Vector3 max_acc = max_acc_ratio * 2.0;
-    for(int i=0;i<XYZ;i++){ LIMIT_MIN(max_acc(i), 0.1); }
-
-    if(DEBUGP)std::cout << names[l]<<": direc: " << min_manip_direc.transpose() << " min_manip_val: " << min_manip_val << " penalty: "<<penalty<<" max_acc: "<<max_acc.transpose()<<std::endl;
-//    calcVelAccSafeTrajectory(target_p0_cur[l], target_v_old[l], (target_p0_cur[l] + ref_ee_vel * m_dt), max_acc, 1.0, target_p0_new[l]);
-
-    refs[l]->p = target_p0_new[l];
-//    refs[l]->rpy = refs[l]->rpy;
-    target_v_old[l] = (target_p0_new[l] - target_p0_cur[l] )/m_dt;
-  }
-}
-
-
-void WholeBodyMasterSlave::calcManipulabilityJointLimitForWBMS(fikPtr& fik_in, hrp::BodyPtr& robot_in){
-//  const std::string names[4] = {"rleg","lleg","rarm","larm"};
-//  double min_manip_val = 1e9;
-//  const double min_manip_th = 0.1;
-//
-//  static hrp::Vector3 target_p0_old[4];
-//  static bool firstcall = true;
-////    for(int i=0;i<4;i++){
-//  for(int i=0;i<1;i++){
-//    hrp::dmatrix J,Jinv,Jnull;
-//    fik_in->ikp[names[i]].manip->calcJacobian(J);
-//    fik_in->ikp[names[i]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
-//
-//    hrp::dmatrix manipulability_mat = J*J.transpose();
-//    double manipulability = sqrt((J*J.transpose()).determinant());
-//    //    Eigen::JacobiSVD< Eigen::Matrix<float, 3, 3> > svd(mm3, Eigen::ComputeFullU | Eigen::ComputeFullV);
-//    Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
-//
-//    Eigen::MatrixXd::Index row,col;
-//    min_manip_val = svd.singularValues().minCoeff(&row,&col);
-//
-//    hrp::Vector3 min_manip_direc = svd.matrixU().col(row);// unit vector
-//    //      std::cout << "singular values" << std::endl << svd.singularValues() << std::endl;
-//    //      std::cout << "matrix U" << std::endl << svd.matrixU() << std::endl;
-//    //      std::cout << "matrix V" << std::endl << svd.matrixV() << std::endl;
-//    if(min_manip_direc.dot(fik_in->ikp[names[i]].target_p0 - robot_in->rootLink()->p) < 0){ min_manip_direc *= -1; }//向きみて反転(常に外側正)
-//
-//    hsp->cur_min_manip_direc = min_manip_direc;
-//    hsp->cur_manip_val = min_manip_val;
-//  }
-}
-
-
 void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix){
   int com_ik_loop=0;
   const int COM_IK_MAX_LOOP = 10;
@@ -791,82 +636,6 @@ void WholeBodyMasterSlave::processMomentumCompensation(fikPtr& fik_in, hrp::Body
 }
 
 
-void WholeBodyMasterSlave::processBBAccelarationFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out){
-  static bool first = true;
-  static hrp::dvector qdd, qd_ref, qd_ans, qd_old, q_ref, q_old, q_oldold, t_goal;
-  if(first){
-    qd_ref.resize(robot_in->numJoints());
-    qd_ans.resize(robot_in->numJoints());
-    qdd.resize(robot_in->numJoints());
-    qd_old.resize(robot_in->numJoints());
-    for(int i=0; i < robot_in->numJoints(); i++) qd_old(i) = 0;
-    qd_ans = qd_ref = qdd = qd_old;
-    q_ref.resize(robot_in->numJoints());
-    q_old.resize(robot_in->numJoints());
-    q_oldold.resize(robot_in->numJoints());
-    t_goal.resize(robot_in->numJoints());
-  }
-  for(int i=0; i < robot_in->numJoints(); i++){
-    q_ref(i) = robot_in->joint(i)->q;
-  }
-  if(first){
-    q_oldold = q_old = q_ref;
-  }
-  qd_ref = (q_ref - q_old) / m_dt;
-  qd_old = (q_old - q_oldold) / m_dt;
-  qdd = (q_ref - 2 * q_old + q_oldold) / (m_dt * m_dt);
-
-  const double max_acc = 3;
-
-  double t_worst = 0;
-  for(int i=0; i < robot_in->numJoints(); i++){
-    t_goal(i) = 0;
-    if(fabs(qd_ref(i)) > 1e-6){
-
-      double q_stop = q_old(i) + SGN(qd_old(i)) * ( qd_old(i)*qd_old(i) ) / ( 2 * max_acc ) * 1.01;
-
-      qd_ans(i) = qd_old(i) + SGN(q_ref(i) - q_stop) * max_acc * m_dt;
-
-      if(SGN(q_ref(i) - q_stop) * SGN(qd_old(i)) < 0){
-        double t0 = fabs(qd_old(i)) / max_acc;
-        double t1 = sqrt( fabs(q_stop - q_ref(i)) / max_acc);
-        t_goal(i) = t0 + t1;
-        if(i==15)fprintf(hsp->id_log,"%f %f %f %f %f 0 0 %d\n", q_ref(i), q_old(i), t_goal(i), t0, t1, 1);
-      }
-      if(SGN(q_ref(i) - q_stop) * SGN(qd_old(i)) > 0){
-        double q_0 = q_old(i) - SGN(qd_old(i)) * ( qd_old(i) * qd_old(i) ) / (2 * max_acc);
-        double t_all = sqrt( fabs(q_ref(i) - q_0) / max_acc) * 2;
-        double t_cur = fabs(qd_old(i)) / max_acc;
-        t_goal(i) = t_all - t_cur;
-        if(i==15)fprintf(hsp->id_log,"%f %f %f 0 0 %f %f %d\n", q_ref(i), q_old(i), t_goal(i), t_all, t_cur, 2);
-      }
-
-    }else{
-      if(i==15)fprintf(hsp->id_log,"%f %f 0 0 0 0 0 %d\n", q_ref(i), q_old(i), 0);
-    }
-    if(t_goal(i) > t_worst) t_worst = t_goal(i);
-
-  }
-  for(int i=0; i < robot_in->numJoints(); i++){
-    if(t_worst != 0.0){
-//      qd_ans(i) *= t_goal(i) / t_worst;
-    }
-  }
-  for(int i=12; i < robot_in->numJoints(); i++){
-    robot_out->joint(i)->q = q_old(i) + qd_ans(i) * m_dt;
-  }
-  for(int i=0; i < 12; i++){
-    robot_out->joint(i)->q = robot_in->joint(i)->q;
-  }
-  q_oldold = q_old;
-  for(int i=12; i < robot_in->numJoints(); i++){
-    q_old(i) = robot_out->joint(i)->q;
-  }
-  robot_out->rootLink()->p = robot_in->rootLink()->p;
-  robot_out->rootLink()->R = robot_in->rootLink()->R;
-  first = false;
-}
-
 //void WholeBodyMasterSlave::processBBAccelarationFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out){
 //  static bool first = true;
 //  static hrp::dvector qdd, qd_ref, qd_ans, qd_old, q_ref, q_old, q_oldold, t_goal;
@@ -888,80 +657,62 @@ void WholeBodyMasterSlave::processBBAccelarationFilter(hrp::BodyPtr& robot_in, h
 //  if(first){
 //    q_oldold = q_old = q_ref;
 //  }
-//
-//
-//  static interpolator *ip;
-//  if(first){
-//    ip = new interpolator(robot_in->numJoints(), m_dt, interpolator::HOFFARBIB, 1);
-//    ip->setName(std::string(m_profile.instance_name)+" ip");
-//    ip->clear();
-//    ip->set(q_ref.data());
-//  }
-//  ip->setGoal(q_ref.data(), 0.3, true);
-//
 //  qd_ref = (q_ref - q_old) / m_dt;
 //  qd_old = (q_old - q_oldold) / m_dt;
 //  qdd = (q_ref - 2 * q_old + q_oldold) / (m_dt * m_dt);
 //
-////  const double max_acc = 3;
-////
-////  double t_worst = 0;
-////  for(int i=0; i < robot_in->numJoints(); i++){
-////    t_goal(i) = 0;
-////    if(fabs(qd_ref(i)) > 1e-6){
-////
-////      double q_stop = q_old(i) + SGN(qd_old(i)) * ( qd_old(i)*qd_old(i) ) / ( 2 * max_acc ) * 1.01;
-////
-////      qd_ans(i) = qd_old(i) + SGN(q_ref(i) - q_stop) * max_acc * m_dt;
-////
-////      if(SGN(q_ref(i) - q_stop) * SGN(qd_old(i)) < 0){
-////        double t0 = fabs(qd_old(i)) / max_acc;
-////        double t1 = sqrt( fabs(q_stop - q_ref(i)) / max_acc);
-////        t_goal(i) = t0 + t1;
-////        if(i==15)fprintf(hsp->id_log,"%f %f %f %f %f 0 0 %d\n", q_ref(i), q_old(i), t_goal(i), t0, t1, 1);
-////      }
-////      if(SGN(q_ref(i) - q_stop) * SGN(qd_old(i)) > 0){
-////        double q_0 = q_old(i) - SGN(qd_old(i)) * ( qd_old(i) * qd_old(i) ) / (2 * max_acc);
-////        double t_all = sqrt( fabs(q_ref(i) - q_0) / max_acc) * 2;
-////        double t_cur = fabs(qd_old(i)) / max_acc;
-////        t_goal(i) = t_all - t_cur;
-////        if(i==15)fprintf(hsp->id_log,"%f %f %f 0 0 %f %f %d\n", q_ref(i), q_old(i), t_goal(i), t_all, t_cur, 2);
-////      }
-////
-////    }else{
-////      if(i==15)fprintf(hsp->id_log,"%f %f 0 0 0 0 0 %d\n", q_ref(i), q_old(i), 0);
-////    }
-////    if(t_goal(i) > t_worst) t_worst = t_goal(i);
-////
-////  }
+//  const double max_acc = 3;
 //
-//  double q_ans[robot_in->numJoints()];
-//  if (!ip->isEmpty() ){
-//    ip->get(q_ans, true);
-//  }
-////  for(int i=0; i < robot_in->numJoints(); i++){
-////    if(t_worst != 0.0){
-////      qd_ans(i) *= t_goal(i) / t_worst;
-////    }
-////    fprintf(hsp->id_log,"%f ", t_goal(i));
-////  }
-////  fprintf(hsp->id_log,"\n");
+//  double t_worst = 0;
+//  for(int i=0; i < robot_in->numJoints(); i++){
+//    t_goal(i) = 0;
+//    if(fabs(qd_ref(i)) > 1e-6){
 //
+//      double q_stop = q_old(i) + SGN(qd_old(i)) * ( qd_old(i)*qd_old(i) ) / ( 2 * max_acc ) * 1.01;
 //
-////  std::cout << "arrowed_ratio "<<arrowed_ratio<<std::endl;
+//      qd_ans(i) = qd_old(i) + SGN(q_ref(i) - q_stop) * max_acc * m_dt;
+//
+//      if(SGN(q_ref(i) - q_stop) * SGN(qd_old(i)) < 0){
+//        double t0 = fabs(qd_old(i)) / max_acc;
+//        double t1 = sqrt( fabs(q_stop - q_ref(i)) / max_acc);
+//        t_goal(i) = t0 + t1;
+//        if(i==15)fprintf(hsp->id_log,"%f %f %f %f %f 0 0 %d\n", q_ref(i), q_old(i), t_goal(i), t0, t1, 1);
+//      }
+//      if(SGN(q_ref(i) - q_stop) * SGN(qd_old(i)) > 0){
+//        double q_0 = q_old(i) - SGN(qd_old(i)) * ( qd_old(i) * qd_old(i) ) / (2 * max_acc);
+//        double t_all = sqrt( fabs(q_ref(i) - q_0) / max_acc) * 2;
+//        double t_cur = fabs(qd_old(i)) / max_acc;
+//        t_goal(i) = t_all - t_cur;
+//        if(i==15)fprintf(hsp->id_log,"%f %f %f 0 0 %f %f %d\n", q_ref(i), q_old(i), t_goal(i), t_all, t_cur, 2);
+//      }
 //
+//    }else{
+//      if(i==15)fprintf(hsp->id_log,"%f %f 0 0 0 0 0 %d\n", q_ref(i), q_old(i), 0);
+//    }
+//    if(t_goal(i) > t_worst) t_worst = t_goal(i);
+//
+//  }
 //  for(int i=0; i < robot_in->numJoints(); i++){
-//    robot_out->joint(i)->q = q_ans[i];
+//    if(t_worst != 0.0){
+////      qd_ans(i) *= t_goal(i) / t_worst;
+//    }
+//  }
+//  for(int i=12; i < robot_in->numJoints(); i++){
+//    robot_out->joint(i)->q = q_old(i) + qd_ans(i) * m_dt;
+//  }
+//  for(int i=0; i < 12; i++){
+//    robot_out->joint(i)->q = robot_in->joint(i)->q;
+//  }
+//  q_oldold = q_old;
+//  for(int i=12; i < robot_in->numJoints(); i++){
+//    q_old(i) = robot_out->joint(i)->q;
 //  }
-//
 //  robot_out->rootLink()->p = robot_in->rootLink()->p;
 //  robot_out->rootLink()->R = robot_in->rootLink()->R;
-//
 //  first = false;
 //}
 
 
-
 bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double sec){//遅い
   if(sec >= 0.0 && sec <= 30.0){
     std::cerr << "[" << m_profile.instance_name << "] start Synchronization after "<<sec<<" [s]" << std::endl;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 537e49697ec..ec2c47ebc17 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -163,8 +163,8 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
     bool is_active, has_toe_joint;
   };
   typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
-  fikPtr fik, fik_rmc, fik_ml;
-  hrp::BodyPtr m_robot, m_robot_rmc, m_robot_ml;
+  fikPtr fik, fik_rmc, fik_ml, fik_vsafe;
+  hrp::BodyPtr m_robot, m_robot_rmc, m_robot_ml, m_robot_vsafe;
   std::vector<fikPtr> fik_list;
   std::vector<hrp::BodyPtr> body_list;
   std::map<std::string, size_t> contact_states_index_map;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index ecd832b15c5..76019e5b73f 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -117,18 +117,12 @@ class RobotConfig : UTIL_CONST {
       std::vector<hrp::Vector3> init;
       init.resize(MINMAX);
       ee_rot_limit.resize(num_pose_tgt, init);
-//      ee_rot_limit[com][MIN] = hrp::Vector3(-10*D2R, -10*D2R, INFMIN);
-//      ee_rot_limit[com][MAX] = hrp::Vector3( 10*D2R,  10*D2R, INFMAX);
       ee_rot_limit[com][MIN] = hrp::Vector3(-10*D2R, -10*D2R, -30*D2R);
       ee_rot_limit[com][MAX] = hrp::Vector3( 10*D2R,  10*D2R, 30*D2R);
       ee_rot_limit[rf][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -20*D2R);
       ee_rot_limit[rf][MAX] = hrp::Vector3( 30*D2R,  30*D2R,   5*D2R);
       ee_rot_limit[lf][MIN] = hrp::Vector3(-30*D2R, -30*D2R,  -5*D2R);
       ee_rot_limit[lf][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  20*D2R);
-//      ee_rot_limit[rh][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -30*D2R);
-//      ee_rot_limit[rh][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
-//      ee_rot_limit[lh][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -30*D2R);
-//      ee_rot_limit[lh][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  30*D2R);
       ee_rot_limit[rh][MIN] = hrp::Vector3(INFMIN, INFMIN, INFMIN);
       ee_rot_limit[rh][MAX] = hrp::Vector3(INFMAX, INFMAX, INFMAX);
       ee_rot_limit[lh][MIN] = hrp::Vector3(INFMIN, INFMIN, INFMIN);
@@ -180,8 +174,10 @@ class WBMSCore : UTIL_CONST {
     struct WBMSparameters WBMSparam;
 
     typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
-    fikPtr fik;
-    hrp::BodyPtr m_robot;
+    fikPtr fik_ml;
+    hrp::BodyPtr m_robot_ml;
+    fikPtr fik_act;
+    hrp::BodyPtr m_robot_act;
     double cur_manip_val[4][3];
     hrp::Vector3 manip_direc[4][3];
     hrp::Matrix33 manip_mat[4];
@@ -190,7 +186,7 @@ class WBMSCore : UTIL_CONST {
     hrp::Vector3 manip_sv_rot[4];
 
     hrp::Vector2 cp_acc_old;
-    double over_vel_feedback;
+    WBMSPose3D ee_pose_old[4];
 
     WBMSCore(const double& dt){
       tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
@@ -209,7 +205,7 @@ class WBMSCore : UTIL_CONST {
       tgt_rot_filters.resize(num_pose_tgt);
       for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(1.0, HZ, Q_NOOVERSHOOT);//四肢拘束点用(position)
       for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0, HZ, Q_NOOVERSHOOT);//四肢拘束点用(Rotation)
-      tgt_pos_filters[com].setParameter(1.0, HZ, Q_NOOVERSHOOT);//重心pos用
+      tgt_pos_filters[com].setParameter(1.5, HZ, Q_NOOVERSHOOT);//重心pos用
       tgt_rot_filters[com].setParameter(0.6, HZ, Q_NOOVERSHOOT);//重心rot用
       tgt_pos_filters[rf].setParameter(hrp::Vector3(10.0,10.0,10.0), HZ, Q_NOOVERSHOOT);//右足pos用
       tgt_pos_filters[lf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, Q_NOOVERSHOOT);//左足pos用
@@ -249,7 +245,6 @@ class WBMSCore : UTIL_CONST {
       hull_dcp.reserve(6);
       hull_dcp.reserve(6);
       cvhull.reserve(6);
-      over_vel_feedback = 1.0;
       if(DEBUG){
         sr_log = fopen("/home/ishiguro/HumanSync_support_region.log","w+");
         cz_log = fopen("/home/ishiguro/HumanSync_com_zmp.log","w+");
@@ -280,6 +275,7 @@ class WBMSCore : UTIL_CONST {
           rp_ref_out.tgt[human_l_names[i]].offs.p = fik_in->getEEPos(robot_l_names[i]);
           rp_ref_out.tgt[human_l_names[i]].offs.rpy = hrp::rpyFromRot(fik_in->getEERot(robot_l_names[i]));
           rp_ref_out.tgt[human_l_names[i]].abs = rp_ref_out.tgt[human_l_names[i]].offs;
+          ee_pose_old[i] = rp_ref_out.tgt[human_l_names[i]].abs;
         }
       }
       rp_ref_out.tgt[com].offs.p = robot_in->calcCM();
@@ -293,7 +289,6 @@ class WBMSCore : UTIL_CONST {
       baselinkpose.rpy = hrp::rpyFromRot(robot_in->rootLink()->R);
       for(int i=0, l[LR]={rf,lf}; i<LR; i++){
         rp_ref_out.tgt[l[i]].is_contact = true;
-//        rp_ref_out.tgt[l[i]].cnt = rp_ref_out.tgt[l[i]].abs;
       }
     }
     void initializeRequest(fikPtr& fik_in, hrp::BodyPtr& robot_in){
@@ -305,7 +300,6 @@ class WBMSCore : UTIL_CONST {
     }
     void update(){//////////  メインループ  ////////////
       gettimeofday(&t_calc_start, NULL);
-//      if(is_initial_loop){ initialize(); }
       updateParams                        ();
       autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);//入力の左右反転を常にチェック(＝手足の交差は不可能)
       convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
@@ -315,13 +309,12 @@ class WBMSCore : UTIL_CONST {
       judgeFootLandOnCommandByFootForce   (hp_wld_raw);//人体足裏反力から各足の接地指令を生成
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
       limitEEWorkspace                    (rp_ref_out);
-//      limitManipulability                 (rp_ref_out);
       setFootContactPoseByGoContact       (rp_ref_out);
       if(DEBUG){ fprintf(cz_log,"com_in: %f %f ",rp_ref_out.tgt[com].abs.p(X),rp_ref_out.tgt[com].abs.p(Y)); }
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
       setFootRotHorizontalIfGoLanding     (rp_ref_out);
 
-      if(WBMSparam.use_manipulability_limit){limitManipulability                 (rp_ref_out);}
+      limitManipulability                 (rp_ref_out);
 
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
 
@@ -405,50 +398,19 @@ class WBMSCore : UTIL_CONST {
         else if (!in.tgt[l[i]].is_contact && in.tgt[l[i]].w(fz)>CNT_F_TH+30){in.tgt[l[i]].is_contact = true;}//足浮いた状態から下げる
       }
     }
-//    void applyLPFilter(HumanPose& tgt){
-//      if(is_initial_loop){
-//        for(int i=0, l[1]={com}; i<1; i++){
-//          tgt_pos_filters[l[i]].reset(tgt.tgt[l[i]].abs.p);
-//          tgt_rot_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy);
-//        }
-//      }
-//      for(int i=0, l[1]={com}; i<1; i++){
-//        tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
-//        tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
-//      }
-//      for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
-//        calcVelAccSafeTrajectoryVecML(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, manip_mat[i], manip_sv[i], tgt.tgt[l[i]].abs.p);
-//        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
-//      }
-//      for(int i=0, l[1]={head}; i<1; i++){
-//        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, 1.0, tgt.tgt[l[i]].abs.p);
-//        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
-//      }
-//    }
     void applyLPFilter_pre(HumanPose& tgt){
-//      if(is_initial_loop){
-//        for(int i=0, l[1]={com}; i<1; i++){
-//          tgt_pos_filters[l[i]].reset(tgt.tgt[l[i]].abs.p);
-//          tgt_rot_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy);
-//        }
-//      }
-//      for(int i=0, l[1]={com}; i<1; i++){
-//        tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
-//        tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
-//      }
-      for(int i=0, l[2]={rf,lf}; i<2; i++){
-        calcVelAccSafeTrajectoryVecML(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, manip_mat[i], manip_sv[i], tgt.tgt[l[i]].abs.p);
-        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
-      }
-      for(int i=0, l[2]={rh,lh}; i<2; i++){
-//        calcVelAccSafeTrajectoryVecML(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 3, manip_mat[i], manip_sv[i], tgt.tgt[l[i]].abs.p);
-//        calcVelAccSafeTrajectoryVecML(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3, manip_mat_rot[i], manip_sv_rot[i], tgt.tgt[l[i]].abs.rpy);
-        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 3.0, 1.0, tgt.tgt[l[i]].abs.p);
-        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+      const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
+      for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
+//        calcVelAccSafeTrajectoryVec(fik_act->getEEPos(robot_l_names[i]), (fik_act->getEEPos(robot_l_names[i]) - ee_pose_old[i].p)/DT, tgt.tgt[l[i]].abs.p, 3.0, 1.0, tgt.tgt[l[i]].abs.p);//何故かダメ
+//        calcVelAccSafeTrajectoryVec(hrp::rpyFromRot(fik_act->getEERot(robot_l_names[i])), (hrp::rpyFromRot(fik_act->getEERot(robot_l_names[i])) - ee_pose_old[i].rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+        calcVelAccSafeTrajectoryVec(fik_act->getEEPos(robot_l_names[i]), (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 3.0, 1.0, tgt.tgt[l[i]].abs.p);
+        calcVelAccSafeTrajectoryVec(hrp::rpyFromRot(fik_act->getEERot(robot_l_names[i])), (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+        ee_pose_old[i].p = fik_act->getEEPos(robot_l_names[i]);
+        ee_pose_old[i].rpy = hrp::rpyFromRot(fik_act->getEERot(robot_l_names[i]));
       }
       for(int i=0, l[1]={head}; i<1; i++){
-        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 1.0, 1.0, tgt.tgt[l[i]].abs.p);
-        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 1.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 2.0, 1.0, tgt.tgt[l[i]].abs.p);
+        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 2.0, 1.0, tgt.tgt[l[i]].abs.rpy);
       }
     }
     void applyLPFilter_post(HumanPose& tgt){
@@ -469,33 +431,39 @@ class WBMSCore : UTIL_CONST {
         double stop_safe_vel_scalar = sqrt( 2 * max_acc * (pos_tgt - pos_cur).norm() );
         hrp::Vector3 vel_ans = vel_cur + direc * max_acc * DT;
         if(vel_ans.dot(direc) > stop_safe_vel_scalar){
-//          vel_ans = vel_ans.normalized() * stop_safe_vel_scalar;
           vel_ans = vel_ans * stop_safe_vel_scalar / vel_ans.dot(direc);
         }
-//        pos_ans = pos_cur + vel_ans * DT;
-        pos_ans = pos_cur + over_vel_feedback * vel_ans * DT;
+        pos_ans = pos_cur + vel_ans * DT;
       }else{
         pos_ans = pos_tgt;
       }
     }
+//    void calcVelAccSafeTrajectoryVec(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
+//      if((pos_tgt - pos_cur).norm() < 1.0e-6 && vel_cur.norm() < 1.0e-6){
+//        pos_ans = pos_tgt;
+//      }else{
+//        hrp::Vector3 direc = (pos_tgt - pos_cur).normalized();
+//        double stop_safe_vel_scalar = sqrt( 2 * max_acc * (pos_tgt - pos_cur).norm() );
+//        hrp::Vector3 vel_ans;
+//        if(vel_cur.dot(direc) > stop_safe_vel_scalar){
+//          vel_ans = vel_cur - direc * max_acc * DT;
+//        }else{
+//          vel_ans = vel_cur + direc * max_acc * DT;
+//        }
+//        pos_ans = pos_cur + vel_ans * DT;
+//      }
+//    }
     void calcVelAccSafeTrajectoryVecML(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& acc_base, const hrp::Matrix33& max_acc_mat, const hrp::Vector3& max_acc_sv, hrp::Vector3& pos_ans){
       if((pos_tgt - pos_cur).norm() > 1.0e-6){
         hrp::Vector3 direc = (pos_tgt - pos_cur).normalized();
         hrp::Vector3 ref_acc = direc*acc_base;
         hrp::Vector3 mod_acc = max_acc_mat * max_acc_sv.asDiagonal() * max_acc_mat.transpose() * ref_acc;
-//        cout<<"ref_acc "<<ref_acc.transpose()<<" mod_acc "<<mod_acc.transpose()<<endl;
-//        dbg(max_acc_mat);
-//        dbg(max_acc_sv.transpose());
-//        dbg((max_acc_mat.transpose() * ref_acc).transpose());
-//        dbg((max_acc_sv.asDiagonal() * max_acc_mat.transpose() * ref_acc).transpose());
         double stop_safe_vel_scalar = sqrt( 2 * fabs(mod_acc.dot(direc)) * (pos_tgt - pos_cur).norm() );
         hrp::Vector3 vel_ans = vel_cur + mod_acc * DT;
         if(vel_ans.dot(direc) > stop_safe_vel_scalar){
-//          vel_ans = vel_ans.normalized() * stop_safe_vel_scalar;
           vel_ans = vel_ans * stop_safe_vel_scalar / vel_ans.dot(direc);
         }
-//        pos_ans = pos_cur + vel_ans * DT;
-        pos_ans = pos_cur + over_vel_feedback * vel_ans * DT;
+        pos_ans = pos_cur + vel_ans * DT;
       }else{
         pos_ans = pos_tgt;
       }
@@ -524,23 +492,6 @@ class WBMSCore : UTIL_CONST {
       const double MAX_FW = 0.25;
 //      const double MAX_FW = 1000000;//manipulability test
       const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
-//      if(!out.tgt[rf].is_contact && out.tgt[lf].is_contact){//右足浮遊時
-//        const hrp::Vector2 lf2rf_vec( out.tgt[rf].abs.p(X) - out.tgt[lf].cnt.p(X), out.tgt[rf].abs.p(Y) - out.tgt[lf].cnt.p(Y) );
-//        if(lf2rf_vec.norm() > MAX_FW){
-//          out.tgt[rf].abs.p(X) = (lf2rf_vec.normalized())(X) * MAX_FW + out.tgt[lf].cnt.p(X);
-//          out.tgt[rf].abs.p(Y) = (lf2rf_vec.normalized())(Y) * MAX_FW + out.tgt[lf].cnt.p(Y);
-//        }
-//        LIMIT_MAX(out.tgt[rf].abs.p(Y), out.tgt[lf].cnt.p(Y) - FOOT_2_FOOT_COLLISION_MARGIIN);
-//      }
-//      else if(out.tgt[rf].is_contact && !out.tgt[lf].is_contact){//左足浮遊時
-//        const hrp::Vector2 rf2lf_vec( out.tgt[lf].abs.p(X) - out.tgt[rf].cnt.p(X), out.tgt[lf].abs.p(Y) - out.tgt[rf].cnt.p(Y) );
-//        if(rf2lf_vec.norm() > MAX_FW){
-//          out.tgt[lf].abs.p(X) = (rf2lf_vec.normalized())(X) * MAX_FW + out.tgt[rf].cnt.p(X);
-//          out.tgt[lf].abs.p(Y) = (rf2lf_vec.normalized())(Y) * MAX_FW + out.tgt[rf].cnt.p(Y);
-//        }
-//        LIMIT_MIN(out.tgt[lf].abs.p(Y), out.tgt[rf].cnt.p(Y) + FOOT_2_FOOT_COLLISION_MARGIIN);
-//      }
-
 
       for(int i=0, spl[LR]={rf,lf}, swl[LR]={lf,rf}; i<LR; i++){
         PoseTGT& support_leg = out.tgt[spl[i]];
@@ -559,55 +510,6 @@ class WBMSCore : UTIL_CONST {
         swing_leg.abs.p(X) = sp2sw_vec(X) + support_leg.cnt.p(X);
         swing_leg.abs.p(Y) = sp2sw_vec(Y) + support_leg.cnt.p(Y);
       }
-
-
-
-//      if(!out.tgt[rf].is_contact && out.tgt[lf].is_contact){//右足浮遊時
-//        PoseTGT& support_leg = out.tgt[lf];
-//        PoseTGT& swing_leg = out.tgt[rf];
-//        hrp::Vector2 inside_vec_baserel(0, -1);
-//
-//        hrp::Vector2 sp2sw_vec( swing_leg.abs.p(X) - support_leg.cnt.p(X), swing_leg.abs.p(Y) - support_leg.cnt.p(Y) );
-//        if(swing_leg.go_contact && sp2sw_vec.norm() > MAX_FW){ sp2sw_vec = sp2sw_vec.normalized() * MAX_FW; }//着地時に足を広げすぎないよう制限
-//        Eigen::Matrix2d base_rot;
-//        base_rot = Eigen::Rotation2Dd(baselinkpose.rpy(y));
-//        hrp::Vector2 sp2sw_vec_baserel = base_rot.transpose() * sp2sw_vec;
-//
-//        if( sp2sw_vec_baserel.dot(inside_vec_baserel) < FOOT_2_FOOT_COLLISION_MARGIIN){
-//          sp2sw_vec_baserel += inside_vec_baserel * (FOOT_2_FOOT_COLLISION_MARGIIN - sp2sw_vec_baserel.dot(inside_vec_baserel));
-//        }
-//        sp2sw_vec = base_rot * sp2sw_vec_baserel;
-//        swing_leg.abs.p(X) = sp2sw_vec(X) + support_leg.cnt.p(X);
-//        swing_leg.abs.p(Y) = sp2sw_vec(Y) + support_leg.cnt.p(Y);
-//      }
-//      else if(!out.tgt[lf].is_contact && out.tgt[lf].go_contact && out.tgt[rf].is_contact){//左足浮遊時
-//        const hrp::Vector2 rf2lf_vec( out.tgt[lf].abs.p(X) - out.tgt[rf].cnt.p(X), out.tgt[lf].abs.p(Y) - out.tgt[rf].cnt.p(Y) );
-//        if(rf2lf_vec.norm() > MAX_FW){
-//          out.tgt[lf].abs.p(X) = (rf2lf_vec.normalized())(X) * MAX_FW + out.tgt[rf].cnt.p(X);
-//          out.tgt[lf].abs.p(Y) = (rf2lf_vec.normalized())(Y) * MAX_FW + out.tgt[rf].cnt.p(Y);
-//        }
-//        LIMIT_MIN(out.tgt[lf].abs.p(Y), out.tgt[rf].cnt.p(Y) + FOOT_2_FOOT_COLLISION_MARGIIN);
-//      }
-
-
-//      hrp::Vector3 init_base2rh = out.tgt[rh].offs.p - baselinkpose.p_offs;
-//      hrp::Vector3 init_base2lh = out.tgt[lh].offs.p - baselinkpose.p_offs;
-//      hrp::Matrix33 init_base2rh_R = hrp::rotFromRpy(baselinkpose.rpy_offs).transpose() * hrp::rotFromRpy(out.tgt[rh].offs.rpy);
-//      hrp::Matrix33 init_base2lh_R = hrp::rotFromRpy(baselinkpose.rpy_offs).transpose() * hrp::rotFromRpy(out.tgt[lh].offs.rpy);
-//      hrp::Vector3 hand_ulimit = hrp::Vector3(0.1,0.1,0.1);
-//      hrp::Vector3 hand_llimit = hrp::Vector3(-0.1,-0.1,-0.1);
-//      if(!WBMSparam.use_rh){
-//        out.tgt[rh].abs.rpy = hrp::rpyFromRot( hrp::rotFromRpy(baselinkpose.rpy) * init_base2rh_R);
-//        out.tgt[rh].abs.p = baselinkpose.p + hrp::rotFromRpy(out.tgt[rh].abs.rpy) * init_base2rh;
-//      }
-//      if(!WBMSparam.use_lh){
-//        out.tgt[lh].abs.rpy = hrp::rpyFromRot( hrp::rotFromRpy(baselinkpose.rpy) * init_base2lh_R);
-//        out.tgt[lh].abs.p = baselinkpose.p + hrp::rotFromRpy(out.tgt[lh].abs.rpy) * init_base2lh;
-//      }
-//      for(int i=0;i<3;i++){
-//        LIMIT_MINMAX( out.tgt[rh].abs.p(i), (baselinkpose.p+init_base2rh+hand_llimit)(i), (baselinkpose.p+init_base2rh+hand_ulimit)(i) );
-//        LIMIT_MINMAX( out.tgt[lh].abs.p(i), (baselinkpose.p+init_base2lh+hand_llimit)(i), (baselinkpose.p+init_base2lh+hand_ulimit)(i) );
-//      }
       LIMIT_MINMAX( out.tgt[rf].abs.p(Z), out.tgt[rf].offs.p(Z), out.tgt[rf].offs.p(Z)+WBMSparam.swing_foot_max_height);
       LIMIT_MINMAX( out.tgt[lf].abs.p(Z), out.tgt[lf].offs.p(Z), out.tgt[lf].offs.p(Z)+WBMSparam.swing_foot_max_height);
 
@@ -628,23 +530,27 @@ class WBMSCore : UTIL_CONST {
         for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( out.tgt[l[i]].abs.rpy(j), rc.ee_rot_limit[l[i]][MIN](j) + out.tgt[com].abs.rpy(j), rc.ee_rot_limit[l[i]][MAX](j) + out.tgt[com].abs.rpy(j) ); }
       }
       if(!WBMSparam.use_head)out.tgt[head].abs.rpy = hrp::Vector3::Zero();
-//      if(mode!=MODE_WBMS)out.tgt[head].abs.rpy = hrp::Vector3::Zero();//頭は動かしてない時は0
     }
     void limitManipulability(HumanPose& out){
       const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
       const int human_l_names[4] = {rf,lf,rh,lh};
+      m_robot_ml->rootLink()->p = m_robot_act->rootLink()->p;
+      m_robot_ml->rootLink()->R = m_robot_act->rootLink()->R;
+//      for (int i=0;i<m_robot_ml->numJoints();i++){ m_robot_ml->joint(i)->q = m_robot_act->joint(i)->q; }
+      m_robot_ml->calcForwardKinematics();
       for(int i=0;i<4;i++){
-        if(fik->ikp.count(robot_l_names[i])){
-          fik->ikp[robot_l_names[i]].target_r0 = hrp::rotFromRpy(out.tgt[human_l_names[i]].abs.rpy);
-          fik->ikp[robot_l_names[i]].target_p0 = out.tgt[human_l_names[i]].abs.p;
+        if(fik_ml->ikp.count(robot_l_names[i])){
+          fik_ml->ikp[robot_l_names[i]].target_r0 = hrp::rotFromRpy(out.tgt[human_l_names[i]].abs.rpy);
+          fik_ml->ikp[robot_l_names[i]].target_p0 = out.tgt[human_l_names[i]].abs.p;
         }
       }
-      for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-          if (it->second.is_ik_enable) fik->solveLimbIK (it->second, it->first, fik->ratio_for_vel, false);
+      for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_ml->ikp.begin(); it != fik_ml->ikp.end(); it++ ) {
+          if (it->second.is_ik_enable) fik_ml->solveLimbIK (it->second, it->first, fik_ml->ratio_for_vel, false);
       }
       for(int i=0;i<4;i++){
-        if(fik->ikp.count(robot_l_names[i])){
-          out.tgt[human_l_names[i]].abs.p = fik->ikp[robot_l_names[i]].target_link->p + fik->ikp[robot_l_names[i]].target_link->R * fik->ikp[robot_l_names[i]].localPos;
+        if(fik_ml->ikp.count(robot_l_names[i])){
+          out.tgt[human_l_names[i]].abs.p = fik_ml->getEEPos(robot_l_names[i]);
+          out.tgt[human_l_names[i]].abs.rpy = hrp::rpyFromRot(fik_ml->getEERot(robot_l_names[i]));
         }
       }
     }
@@ -655,9 +561,6 @@ class WBMSCore : UTIL_CONST {
           out.tgt[l[i]].abs.p(Y) = out.tgt[l[i]].cnt.p(Y);
           out.tgt[l[i]].abs.rpy(y) = out.tgt[l[i]].cnt.rpy(y);
         }else{
-//          out.tgt[l[i]].cnt.p(X) = out.tgt[l[i]].abs.p(X);
-//          out.tgt[l[i]].cnt.p(Y) = out.tgt[l[i]].abs.p(Y);
-//          out.tgt[l[i]].cnt.rpy(y) = out.tgt[l[i]].abs.rpy(y);
           out.tgt[l[i]].cnt.p(X) = rp_ref_out_old.tgt[l[i]].abs.p(X);
           out.tgt[l[i]].cnt.p(Y) = rp_ref_out_old.tgt[l[i]].abs.p(Y);
           out.tgt[l[i]].cnt.rpy(y) = rp_ref_out_old.tgt[l[i]].abs.rpy(y);
@@ -687,8 +590,8 @@ class WBMSCore : UTIL_CONST {
       const double vel_limit_k = WBMSparam.foot_vertical_vel_limit_coeff * penalty;//地面から0.02[m]地点で0.02*8=0.16[m/s]出ている計算
       double limit_vel = - vel_limit_k * (f_old_in.abs.p(Z) - f_old_in.offs.p(Z));//sinの時は平均0.05~0.10[m/s]
       const double max_vel_threshold = -0.01;
-      LIMIT_MAX( limit_vel, max_vel_threshold);//速度0に近づくと目標になかなか到達しないから
-      LIMIT_MIN( input_vel, limit_vel);//着地時の下向きの速度を制限
+//      LIMIT_MAX( limit_vel, max_vel_threshold);//速度0に近づくと目標になかなか到達しないから
+//      LIMIT_MIN( input_vel, limit_vel);//着地時の下向きの速度を制限
       f_in_out.abs.p(Z) = f_old_in.abs.p(Z) + input_vel * DT;
       LIMIT_MIN( f_in_out.abs.p(Z), f_in_out.offs.p(Z));
       const double contact_threshold = 0.005;
@@ -730,10 +633,7 @@ class WBMSCore : UTIL_CONST {
       hull_dcp.clear();
       hull_acp.clear();
       hrp::Vector4 marginDelta_for_dcp(+0.001,-0.001,0.001,-0.001);
-//      hrp::Vector4 marginDelta_for_acp(+0.001,-0.001,0.001,-0.001);
-      hrp::Vector4 marginDelta_for_acp(+0.002,-0.002,0.002,-0.002);
-//      hrp::Vector4 marginDelta_for_dcp(+0.001,-0.001,0.001,-0.001);
-//      hrp::Vector4 marginDelta_for_acp(+0.01,-0.01,0.01,-0.01);
+      hrp::Vector4 marginDelta_for_acp(+0.010,-0.010,0.010,-0.010);
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region+marginDelta_for_dcp, lf_safe_region+marginDelta_for_dcp, hull_dcp);
       createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region+marginDelta_for_acp, lf_safe_region+marginDelta_for_acp, hull_acp);
       hrp::Vector2 com_vel_ans_2d;

From 8277be6936a68b651f384b3ca0ebeadce3876ff1 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 30 Jul 2017 21:38:29 +0900
Subject: [PATCH 107/217] use HOFFARRBIB

---
 .../WholeBodyMasterSlave.cpp                  | 120 +++++-------------
 .../WholeBodyMasterSlave.h                    |   4 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          |  47 ++++---
 3 files changed, 65 insertions(+), 106 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index c878f190cd3..ffa739f30db 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -178,6 +178,10 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     transition_interpolator->set(&tmp_ratio);
     tmp_ratio = 1.0;
     transition_interpolator->setGoal(&tmp_ratio, 3.0, true);
+    q_ip_dim = m_robot->numJoints() + 3 + 3*3;// joints + base_p + base_R
+    q_ip = new interpolator(q_ip_dim, m_dt, interpolator::HOFFARBIB, 1);
+    q_ip->setName(std::string(m_profile.instance_name)+" q_ip");
+    q_ip->clear();
     // Generate FIK
     fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
     fik_rmc = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot_rmc, std::string(m_profile.instance_name), m_dt));
@@ -339,21 +343,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
         if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processMomentumCompensation" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
-        double safe_vel_ratio = 1.0;
-        const double qd_max = 3.0;
-        for (int i=0;i<m_robot->numJoints();i++){
-          double qd = (m_robot->joint(i)->q - m_robot_vsafe->joint(i)->q) / m_dt;
-          if(fabs(qd) > qd_max){
-            double tmp_ratio = qd_max / fabs(qd);
-            if(tmp_ratio < safe_vel_ratio){ safe_vel_ratio = tmp_ratio; }
-          }
-        }
-        for (int i=0;i<m_robot->numJoints();i++){
-          m_robot_vsafe->joint(i)->q += safe_vel_ratio * (m_robot->joint(i)->q - m_robot_vsafe->joint(i)->q);
-        }
-        m_robot_vsafe->rootLink()->p += safe_vel_ratio * (m_robot->rootLink()->p - m_robot_vsafe->rootLink()->p);
-        m_robot_vsafe->rootLink()->R += safe_vel_ratio * (m_robot->rootLink()->R - m_robot_vsafe->rootLink()->R);
-        m_robot_vsafe->calcForwardKinematics();
+        processHOFFARBIBFilter(m_robot, m_robot_vsafe);
 
         const hrp::BodyPtr m_robot_for_out = m_robot_vsafe;
 
@@ -514,6 +504,12 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
       }
     }
   }
+  q_ip->clear();
+  double init_q[q_ip_dim];
+  for(int i=0;i<robot_in->numJoints();i++){ init_q[i] = robot_in->joint(i)->q; }
+  for(int i=0;i<3;i++){ init_q[robot_in->numJoints()+i] = robot_in->rootLink()->p(i); }
+  for(int i=0;i<3;i++){for(int j=0;j<3;j++){ init_q[robot_in->numJoints() + 3 + 3*i + j] = robot_in->rootLink()->R(i,j); }}
+  q_ip->set(init_q);
   torso_rot_rmc = hrp::Vector3::Zero();//RMCの補償量リセット
   hsp->initializeRequest(fik_in, robot_in);
 }
@@ -635,82 +631,26 @@ void WholeBodyMasterSlave::processMomentumCompensation(fikPtr& fik_in, hrp::Body
   solveFullbodyIKStrictCOM(fik_in, robot_in, com_mod, rf_checked, lf_checked, rh_checked, lh_checked, pose_ref.tgt[head].abs,"calcDynamicsFilterCompensation");
 }
 
+void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out){
+  double goal_q[q_ip_dim];
+  for(int i=0;i<robot_in->numJoints();i++){ goal_q[i] = robot_in->joint(i)->q; }
+  for(int i=0;i<3;i++){ goal_q[robot_in->numJoints()+i] = robot_in->rootLink()->p(i); }
+  for(int i=0;i<3;i++){for(int j=0;j<3;j++){ goal_q[robot_in->numJoints() + 3 + 3*i + j] = robot_in->rootLink()->R(i,j); }}
+
+  double goal_time = 0.001;
+  const double avg_q_vel = 1.0;
+  for(int i=0;i<robot_in->numJoints();i++){
+    double tmp_time = (robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
+    if(tmp_time > goal_time){ goal_time = tmp_time; }
+  }
+  q_ip->setGoal(goal_q, goal_time, true);
 
-//void WholeBodyMasterSlave::processBBAccelarationFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out){
-//  static bool first = true;
-//  static hrp::dvector qdd, qd_ref, qd_ans, qd_old, q_ref, q_old, q_oldold, t_goal;
-//  if(first){
-//    qd_ref.resize(robot_in->numJoints());
-//    qd_ans.resize(robot_in->numJoints());
-//    qdd.resize(robot_in->numJoints());
-//    qd_old.resize(robot_in->numJoints());
-//    for(int i=0; i < robot_in->numJoints(); i++) qd_old(i) = 0;
-//    qd_ans = qd_ref = qdd = qd_old;
-//    q_ref.resize(robot_in->numJoints());
-//    q_old.resize(robot_in->numJoints());
-//    q_oldold.resize(robot_in->numJoints());
-//    t_goal.resize(robot_in->numJoints());
-//  }
-//  for(int i=0; i < robot_in->numJoints(); i++){
-//    q_ref(i) = robot_in->joint(i)->q;
-//  }
-//  if(first){
-//    q_oldold = q_old = q_ref;
-//  }
-//  qd_ref = (q_ref - q_old) / m_dt;
-//  qd_old = (q_old - q_oldold) / m_dt;
-//  qdd = (q_ref - 2 * q_old + q_oldold) / (m_dt * m_dt);
-//
-//  const double max_acc = 3;
-//
-//  double t_worst = 0;
-//  for(int i=0; i < robot_in->numJoints(); i++){
-//    t_goal(i) = 0;
-//    if(fabs(qd_ref(i)) > 1e-6){
-//
-//      double q_stop = q_old(i) + SGN(qd_old(i)) * ( qd_old(i)*qd_old(i) ) / ( 2 * max_acc ) * 1.01;
-//
-//      qd_ans(i) = qd_old(i) + SGN(q_ref(i) - q_stop) * max_acc * m_dt;
-//
-//      if(SGN(q_ref(i) - q_stop) * SGN(qd_old(i)) < 0){
-//        double t0 = fabs(qd_old(i)) / max_acc;
-//        double t1 = sqrt( fabs(q_stop - q_ref(i)) / max_acc);
-//        t_goal(i) = t0 + t1;
-//        if(i==15)fprintf(hsp->id_log,"%f %f %f %f %f 0 0 %d\n", q_ref(i), q_old(i), t_goal(i), t0, t1, 1);
-//      }
-//      if(SGN(q_ref(i) - q_stop) * SGN(qd_old(i)) > 0){
-//        double q_0 = q_old(i) - SGN(qd_old(i)) * ( qd_old(i) * qd_old(i) ) / (2 * max_acc);
-//        double t_all = sqrt( fabs(q_ref(i) - q_0) / max_acc) * 2;
-//        double t_cur = fabs(qd_old(i)) / max_acc;
-//        t_goal(i) = t_all - t_cur;
-//        if(i==15)fprintf(hsp->id_log,"%f %f %f 0 0 %f %f %d\n", q_ref(i), q_old(i), t_goal(i), t_all, t_cur, 2);
-//      }
-//
-//    }else{
-//      if(i==15)fprintf(hsp->id_log,"%f %f 0 0 0 0 0 %d\n", q_ref(i), q_old(i), 0);
-//    }
-//    if(t_goal(i) > t_worst) t_worst = t_goal(i);
-//
-//  }
-//  for(int i=0; i < robot_in->numJoints(); i++){
-//    if(t_worst != 0.0){
-////      qd_ans(i) *= t_goal(i) / t_worst;
-//    }
-//  }
-//  for(int i=12; i < robot_in->numJoints(); i++){
-//    robot_out->joint(i)->q = q_old(i) + qd_ans(i) * m_dt;
-//  }
-//  for(int i=0; i < 12; i++){
-//    robot_out->joint(i)->q = robot_in->joint(i)->q;
-//  }
-//  q_oldold = q_old;
-//  for(int i=12; i < robot_in->numJoints(); i++){
-//    q_old(i) = robot_out->joint(i)->q;
-//  }
-//  robot_out->rootLink()->p = robot_in->rootLink()->p;
-//  robot_out->rootLink()->R = robot_in->rootLink()->R;
-//  first = false;
-//}
+  double ans_q[q_ip_dim];
+  if (!q_ip->isEmpty() ){  q_ip->get(ans_q, true); }
+  for(int i=0;i<robot_out->numJoints();i++){ robot_out->joint(i)->q = ans_q[i]; }
+  for(int i=0;i<3;i++){ robot_out->rootLink()->p(i) = ans_q[robot_out->numJoints()+i]; }
+  for(int i=0;i<3;i++){for(int j=0;j<3;j++){ robot_out->rootLink()->R(i,j) = ans_q[robot_out->numJoints() + 3 + 3*i + j]; }}
+}
 
 
 bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double sec){//遅い
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index ec2c47ebc17..1f70a3edced 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -172,6 +172,8 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
 
   double transition_interpolator_ratio;
   interpolator *transition_interpolator;
+  int q_ip_dim;
+  interpolator *q_ip;
 
   int optionalDataLength;
   unsigned int m_debugLevel;
@@ -208,7 +210,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
   void processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HumanPose& pose_ref);
   void processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::BodyPtr& robot_in, HumanPose& pose_ref);
   void processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_normal_in, const HumanPose& pose_ref);
-  void processBBAccelarationFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out);
+  void processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out);
   bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; };
 
 //  void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const hrp::Vector3& max_acc, const double& max_vel, hrp::Vector3& pos_ans);
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 76019e5b73f..d08445873ea 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -35,7 +35,7 @@ extern "C" {
 }
 #endif
 
-#define DEBUG 1
+#define DEBUG 0
 #define dbg(var) std::cout<<#var"= "<<(var)<<std::endl
 #define LIMIT_MIN(x,min) (x= ( x<min ? min:x ))
 #define LIMIT_MAX(x,max) (x= ( x<max ? x:max ))
@@ -186,7 +186,7 @@ class WBMSCore : UTIL_CONST {
     hrp::Vector3 manip_sv_rot[4];
 
     hrp::Vector2 cp_acc_old;
-    WBMSPose3D ee_pose_old[4];
+//    WBMSPose3D ee_pose_old[4];
 
     WBMSCore(const double& dt){
       tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
@@ -275,7 +275,7 @@ class WBMSCore : UTIL_CONST {
           rp_ref_out.tgt[human_l_names[i]].offs.p = fik_in->getEEPos(robot_l_names[i]);
           rp_ref_out.tgt[human_l_names[i]].offs.rpy = hrp::rpyFromRot(fik_in->getEERot(robot_l_names[i]));
           rp_ref_out.tgt[human_l_names[i]].abs = rp_ref_out.tgt[human_l_names[i]].offs;
-          ee_pose_old[i] = rp_ref_out.tgt[human_l_names[i]].abs;
+//          ee_pose_old[i] = rp_ref_out.tgt[human_l_names[i]].abs;
         }
       }
       rp_ref_out.tgt[com].offs.p = robot_in->calcCM();
@@ -403,14 +403,14 @@ class WBMSCore : UTIL_CONST {
       for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
 //        calcVelAccSafeTrajectoryVec(fik_act->getEEPos(robot_l_names[i]), (fik_act->getEEPos(robot_l_names[i]) - ee_pose_old[i].p)/DT, tgt.tgt[l[i]].abs.p, 3.0, 1.0, tgt.tgt[l[i]].abs.p);//何故かダメ
 //        calcVelAccSafeTrajectoryVec(hrp::rpyFromRot(fik_act->getEERot(robot_l_names[i])), (hrp::rpyFromRot(fik_act->getEERot(robot_l_names[i])) - ee_pose_old[i].rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3.0, 1.0, tgt.tgt[l[i]].abs.rpy);
-        calcVelAccSafeTrajectoryVec(fik_act->getEEPos(robot_l_names[i]), (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 3.0, 1.0, tgt.tgt[l[i]].abs.p);
-        calcVelAccSafeTrajectoryVec(hrp::rpyFromRot(fik_act->getEERot(robot_l_names[i])), (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3.0, 1.0, tgt.tgt[l[i]].abs.rpy);
-        ee_pose_old[i].p = fik_act->getEEPos(robot_l_names[i]);
-        ee_pose_old[i].rpy = hrp::rpyFromRot(fik_act->getEERot(robot_l_names[i]));
+//        calcVelAccSafeTrajectoryVec(fik_act->getEEPos(robot_l_names[i]), (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 3.0, 1.0, tgt.tgt[l[i]].abs.p);
+//        calcVelAccSafeTrajectoryVec(hrp::rpyFromRot(fik_act->getEERot(robot_l_names[i])), (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+//        ee_pose_old[i].p = fik_act->getEEPos(robot_l_names[i]);
+//        ee_pose_old[i].rpy = hrp::rpyFromRot(fik_act->getEERot(robot_l_names[i]));
       }
       for(int i=0, l[1]={head}; i<1; i++){
-        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 2.0, 1.0, tgt.tgt[l[i]].abs.p);
-        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 2.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+//        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 2.0, 1.0, tgt.tgt[l[i]].abs.p);
+//        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 2.0, 1.0, tgt.tgt[l[i]].abs.rpy);
       }
     }
     void applyLPFilter_post(HumanPose& tgt){
@@ -427,18 +427,35 @@ class WBMSCore : UTIL_CONST {
     }
     void calcVelAccSafeTrajectoryVec(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
       if((pos_tgt - pos_cur).norm() > 1.0e-6){
-        hrp::Vector3 direc = (pos_tgt - pos_cur).normalized();
-        double stop_safe_vel_scalar = sqrt( 2 * max_acc * (pos_tgt - pos_cur).norm() );
-        hrp::Vector3 vel_ans = vel_cur + direc * max_acc * DT;
-        if(vel_ans.dot(direc) > stop_safe_vel_scalar){
-          vel_ans = vel_ans * stop_safe_vel_scalar / vel_ans.dot(direc);
+
+        for(int i=0;i<3;i++){
+          double stop_safe_vel = SGN(pos_tgt(i) - pos_cur(i)) * sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
+          double vel_ans;
+          if(vel_cur(i) > stop_safe_vel){
+            vel_ans = vel_cur(i) - max_acc * DT;
+          }else{
+            vel_ans = vel_cur(i) + max_acc * DT;
+          }
+          pos_ans(i) = pos_cur(i) + vel_ans * DT;
         }
-        pos_ans = pos_cur + vel_ans * DT;
       }else{
         pos_ans = pos_tgt;
       }
     }
 //    void calcVelAccSafeTrajectoryVec(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
+//      if((pos_tgt - pos_cur).norm() > 1.0e-6){
+//        hrp::Vector3 direc = (pos_tgt - pos_cur).normalized();
+//        double stop_safe_vel_scalar = sqrt( 2 * max_acc * (pos_tgt - pos_cur).norm() );
+//        hrp::Vector3 vel_ans = vel_cur + direc * max_acc * DT;
+//        if(vel_ans.dot(direc) > stop_safe_vel_scalar){
+//          vel_ans = vel_ans * stop_safe_vel_scalar / vel_ans.dot(direc);
+//        }
+//        pos_ans = pos_cur + vel_ans * DT;
+//      }else{
+//        pos_ans = pos_tgt;
+//      }
+//    }
+//    void calcVelAccSafeTrajectoryVec(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
 //      if((pos_tgt - pos_cur).norm() < 1.0e-6 && vel_cur.norm() < 1.0e-6){
 //        pos_ans = pos_tgt;
 //      }else{

From 1b2fddeff55c04c7218f8b30b36da5e00e396ca5 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 1 Aug 2017 19:59:34 +0900
Subject: [PATCH 108/217] enable chest joint

---
 .../WholeBodyMasterSlave.cpp                  | 27 ++++++++++++++++---
 rtc/WholeBodyMasterSlave/wbms_core.h          | 23 ++--------------
 2 files changed, 25 insertions(+), 25 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index ffa739f30db..d33b38cb460 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -495,6 +495,8 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
     body_list[i]->rootLink()->p = basePos_heightChecked;
     body_list[i]->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
     for ( int j = 0; j < body_list[i]->numJoints(); j++ ){ body_list[i]->joint(j)->q = m_qRef.data[j]; }
+    if( body_list[i]->link("RARM_JOINT2") != NULL) body_list[i]->link("RARM_JOINT2")->ulimit = -30 * D2R;//脇の干渉回避のため
+    if( body_list[i]->link("LARM_JOINT2") != NULL) body_list[i]->link("LARM_JOINT2")->llimit = 30 * D2R;
     body_list[i]->calcForwardKinematics();
     fik_list[i]->setReferenceJointAngles();
     for(int l=0;l<4;l++){//targetを初期化
@@ -580,8 +582,24 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   }
 //  fik_in->storeCurrentParameters();
 //  fik_in->setReferenceJointAngles();//これ入れると腕ブワーなる
-  if(robot_in->link("HEAD_JOINT0") != NULL)robot_in->joint(15)->q = head_ref.rpy(y);
-  if(robot_in->link("HEAD_JOINT1") != NULL)robot_in->joint(16)->q = head_ref.rpy(p);
+  if( robot_in->link("HEAD_JOINT0") != NULL) robot_in->link("HEAD_JOINT0")->q = head_ref.rpy(y);
+  if( robot_in->link("HEAD_JOINT1") != NULL) robot_in->link("HEAD_JOINT1")->q = head_ref.rpy(p);
+  if(fik_in->ikp.count("rarm") && fik_in->ikp.count("larm")){
+    hrp::Vector3 base2rh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["rarm"].target_p0 - robot_in->rootLink()->p);
+    hrp::Vector3 base2lh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["larm"].target_p0 - robot_in->rootLink()->p);
+    hrp::Vector3 base2ch = (base2rh + base2lh) / 2;
+    if( robot_in->link("CHEST_JOINT0") != NULL){
+      robot_in->link("CHEST_JOINT0")->q = (base2lh(Z) - base2rh(Z)) * (10 * D2R / 1.0);
+      LIMIT_MINMAX(robot_in->link("CHEST_JOINT0")->q, robot_in->link("CHEST_JOINT0")->llimit, robot_in->link("CHEST_JOINT0")->ulimit);
+    }
+    if(robot_in->link("CHEST_JOINT2") != NULL){
+      robot_in->link("CHEST_JOINT2")->q = (base2rh(X) - base2lh(X)) * (60 * D2R / 1.0);
+//      hrp::Vector2 base2ch2d = hrp::Vector2(base2ch(X),base2ch(Y));
+//      hrp::Vector2 nx = hrp::Vector2(1,0);
+//      robot_in->link("CHEST_JOINT2")->q = SGN(WBMSCore::hrpVector2Cross(base2ch2d,nx)) * base2ch2d.dot(nx) / base2ch2d.norm();
+      LIMIT_MINMAX(robot_in->link("CHEST_JOINT2")->q, robot_in->link("CHEST_JOINT2")->llimit, robot_in->link("CHEST_JOINT2")->ulimit);
+    }
+  }
   hrp::Vector3 tmp_com_err = hrp::Vector3::Zero();
   while( 1 ){  //COM 収束ループ
     com_ik_loop++;
@@ -637,13 +655,14 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
   for(int i=0;i<3;i++){ goal_q[robot_in->numJoints()+i] = robot_in->rootLink()->p(i); }
   for(int i=0;i<3;i++){for(int j=0;j<3;j++){ goal_q[robot_in->numJoints() + 3 + 3*i + j] = robot_in->rootLink()->R(i,j); }}
 
-  double goal_time = 0.001;
+  double goal_time = 0.0;
+  const double min_goal_time_offset = 0.1;
   const double avg_q_vel = 1.0;
   for(int i=0;i<robot_in->numJoints();i++){
     double tmp_time = (robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
     if(tmp_time > goal_time){ goal_time = tmp_time; }
   }
-  q_ip->setGoal(goal_q, goal_time, true);
+  q_ip->setGoal(goal_q, goal_time + min_goal_time_offset, true);
 
   double ans_q[q_ip_dim];
   if (!q_ip->isEmpty() ){  q_ip->get(ans_q, true); }
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index d08445873ea..ec3c39cd526 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -231,7 +231,6 @@ class WBMSCore : UTIL_CONST {
       WBMSparam.set_com_height_fix_val = 0.02;
       WBMSparam.swing_foot_height_offset = 0.01;
       WBMSparam.swing_foot_max_height = 0.5;
-//      WBMSparam.upper_body_rmc_ratio = 0.5;
       WBMSparam.upper_body_rmc_ratio = 0.0;
       WBMSparam.use_rh = WBMSparam.use_lh = true;
       WBMSparam.use_head = true;
@@ -304,7 +303,6 @@ class WBMSCore : UTIL_CONST {
       autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);//入力の左右反転を常にチェック(＝手足の交差は不可能)
       convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
       hp_plot = rp_ref_out;
-//      applyVelLimit                       (rp_ref_vel_old, rp_ref_out); rp_ref_vel_old = rp_ref_out;
       if(WBMSparam.set_com_height_fix)rp_ref_out.tgt[com].abs.p(Z) = rp_ref_out.tgt[com].offs.p(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
       judgeFootLandOnCommandByFootForce   (hp_wld_raw);//人体足裏反力から各足の接地指令を生成
       lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
@@ -405,8 +403,6 @@ class WBMSCore : UTIL_CONST {
 //        calcVelAccSafeTrajectoryVec(hrp::rpyFromRot(fik_act->getEERot(robot_l_names[i])), (hrp::rpyFromRot(fik_act->getEERot(robot_l_names[i])) - ee_pose_old[i].rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3.0, 1.0, tgt.tgt[l[i]].abs.rpy);
 //        calcVelAccSafeTrajectoryVec(fik_act->getEEPos(robot_l_names[i]), (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 3.0, 1.0, tgt.tgt[l[i]].abs.p);
 //        calcVelAccSafeTrajectoryVec(hrp::rpyFromRot(fik_act->getEERot(robot_l_names[i])), (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3.0, 1.0, tgt.tgt[l[i]].abs.rpy);
-//        ee_pose_old[i].p = fik_act->getEEPos(robot_l_names[i]);
-//        ee_pose_old[i].rpy = hrp::rpyFromRot(fik_act->getEERot(robot_l_names[i]));
       }
       for(int i=0, l[1]={head}; i<1; i++){
 //        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 2.0, 1.0, tgt.tgt[l[i]].abs.p);
@@ -427,7 +423,6 @@ class WBMSCore : UTIL_CONST {
     }
     void calcVelAccSafeTrajectoryVec(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
       if((pos_tgt - pos_cur).norm() > 1.0e-6){
-
         for(int i=0;i<3;i++){
           double stop_safe_vel = SGN(pos_tgt(i) - pos_cur(i)) * sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
           double vel_ans;
@@ -443,19 +438,6 @@ class WBMSCore : UTIL_CONST {
       }
     }
 //    void calcVelAccSafeTrajectoryVec(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
-//      if((pos_tgt - pos_cur).norm() > 1.0e-6){
-//        hrp::Vector3 direc = (pos_tgt - pos_cur).normalized();
-//        double stop_safe_vel_scalar = sqrt( 2 * max_acc * (pos_tgt - pos_cur).norm() );
-//        hrp::Vector3 vel_ans = vel_cur + direc * max_acc * DT;
-//        if(vel_ans.dot(direc) > stop_safe_vel_scalar){
-//          vel_ans = vel_ans * stop_safe_vel_scalar / vel_ans.dot(direc);
-//        }
-//        pos_ans = pos_cur + vel_ans * DT;
-//      }else{
-//        pos_ans = pos_tgt;
-//      }
-//    }
-//    void calcVelAccSafeTrajectoryVec(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
 //      if((pos_tgt - pos_cur).norm() < 1.0e-6 && vel_cur.norm() < 1.0e-6){
 //        pos_ans = pos_tgt;
 //      }else{
@@ -531,8 +513,8 @@ class WBMSCore : UTIL_CONST {
       LIMIT_MINMAX( out.tgt[lf].abs.p(Z), out.tgt[lf].offs.p(Z), out.tgt[lf].offs.p(Z)+WBMSparam.swing_foot_max_height);
 
       for(int i=0, l[2]={rh,lh}; i<2; i++){
-        LIMIT_MIN(out.tgt[l[i]].abs.p(X), baselinkpose.p(X));
-        LIMIT_MAX(out.tgt[l[i]].abs.p(Z), baselinkpose.p(Z) + 0.4);
+//        LIMIT_MIN(out.tgt[l[i]].abs.p(X), baselinkpose.p(X));
+//        LIMIT_MAX(out.tgt[l[i]].abs.p(Z), baselinkpose.p(Z) + 0.4);
         hrp::Vector2 horizontal_dist(out.tgt[l[i]].abs.p(X) - baselinkpose.p(X), out.tgt[l[i]].abs.p(Y) - baselinkpose.p(Y));
         if(horizontal_dist.norm() < 0.5){
           horizontal_dist = 0.5 * horizontal_dist.normalized();
@@ -540,7 +522,6 @@ class WBMSCore : UTIL_CONST {
         out.tgt[l[i]].abs.p(X) = baselinkpose.p(X) + horizontal_dist(X);
         out.tgt[l[i]].abs.p(Y) = baselinkpose.p(Y) + horizontal_dist(Y);
       }
-
       for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( out.tgt[com].abs.rpy(i), rc.ee_rot_limit[com][MIN](i), rc.ee_rot_limit[com][MAX](i) ); }
       LIMIT_MINMAX( out.tgt[com].abs.p(Z), out.tgt[com].offs.p(Z) - 0.15, out.tgt[com].offs.p(Z) + 0.03 );//COM高さ方向の制限
       for(int i=0, l[5]={rf,lf,rh,lh,head}; i<5; i++){

From 1b200c0fe132ccde61dff4b38005b11d314a6e8d Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 1 Aug 2017 22:11:44 +0900
Subject: [PATCH 109/217] avoid collision du to the l shoulder roll

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index d33b38cb460..b8eebb6c43a 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -587,17 +587,14 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   if(fik_in->ikp.count("rarm") && fik_in->ikp.count("larm")){
     hrp::Vector3 base2rh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["rarm"].target_p0 - robot_in->rootLink()->p);
     hrp::Vector3 base2lh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["larm"].target_p0 - robot_in->rootLink()->p);
-    hrp::Vector3 base2ch = (base2rh + base2lh) / 2;
     if( robot_in->link("CHEST_JOINT0") != NULL){
       robot_in->link("CHEST_JOINT0")->q = (base2lh(Z) - base2rh(Z)) * (10 * D2R / 1.0);
       LIMIT_MINMAX(robot_in->link("CHEST_JOINT0")->q, robot_in->link("CHEST_JOINT0")->llimit, robot_in->link("CHEST_JOINT0")->ulimit);
     }
     if(robot_in->link("CHEST_JOINT2") != NULL){
       robot_in->link("CHEST_JOINT2")->q = (base2rh(X) - base2lh(X)) * (60 * D2R / 1.0);
-//      hrp::Vector2 base2ch2d = hrp::Vector2(base2ch(X),base2ch(Y));
-//      hrp::Vector2 nx = hrp::Vector2(1,0);
-//      robot_in->link("CHEST_JOINT2")->q = SGN(WBMSCore::hrpVector2Cross(base2ch2d,nx)) * base2ch2d.dot(nx) / base2ch2d.norm();
-      LIMIT_MINMAX(robot_in->link("CHEST_JOINT2")->q, robot_in->link("CHEST_JOINT2")->llimit, robot_in->link("CHEST_JOINT2")->ulimit);
+//      LIMIT_MINMAX(robot_in->link("CHEST_JOINT2")->q, robot_in->link("CHEST_JOINT2")->llimit, robot_in->link("CHEST_JOINT2")->ulimit);
+      LIMIT_MINMAX(robot_in->link("CHEST_JOINT2")->q, robot_in->link("CHEST_JOINT2")->llimit, 10);//左肩ロール故障につき
     }
   }
   hrp::Vector3 tmp_com_err = hrp::Vector3::Zero();

From 5d76ad75b7b4ccc3c9599f139745ba3fec3048ab Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 15 Aug 2017 00:52:58 +0900
Subject: [PATCH 110/217] add fullbody ik

---
 rtc/WholeBodyMasterSlave/FullBodyIK.h | 435 ++++++++++++++++++++++++++
 1 file changed, 435 insertions(+)
 create mode 100644 rtc/WholeBodyMasterSlave/FullBodyIK.h

diff --git a/rtc/WholeBodyMasterSlave/FullBodyIK.h b/rtc/WholeBodyMasterSlave/FullBodyIK.h
new file mode 100644
index 00000000000..50cbca1281f
--- /dev/null
+++ b/rtc/WholeBodyMasterSlave/FullBodyIK.h
@@ -0,0 +1,435 @@
+#ifndef FULLBODYIK_H
+#define FULLBODYIK_H
+
+#include <hrpModel/Body.h>
+#include <hrpUtil/MatrixSolvers.h>
+#include "../ImpedanceController/JointPathEx.h"
+#include "../ImpedanceController/RatsMatrix.h"
+
+#include "../AutoBalancer/SimpleFullbodyInverseKinematicsSolver.h"
+//#include "hrpsys/idl/AutoBalancerService.hh"
+#include "../AutoBalancer/AutoBalancerService_impl.h"
+
+class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSolver{
+//  private:
+//    // Robot model for IK
+//    hrp::BodyPtr m_robot;
+//    // Org (current) joint angles before IK
+//    hrp::dvector qorg;
+//    // IK fail checking
+//    int ik_error_debug_print_freq;
+//    std::string print_str;
+//    bool has_ik_failed;
+  public:
+//    // IK parameter for each limb
+//    struct IKparam {
+//        // IK target EE coords
+//        hrp::Vector3 target_p0;
+//        hrp::Matrix33 target_r0;
+//        // EE offset, EE link
+//        hrp::Vector3 localPos;
+//        hrp::Matrix33 localR;
+//        hrp::Link* target_link;
+//        // IK solver and parameter
+//        hrp::JointPathExPtr manip;
+//        double avoid_gain, reference_gain;
+//        // IK fail checking
+//        size_t pos_ik_error_count, rot_ik_error_count;
+//        // Solve ik or not
+//        bool is_ik_enable;
+//        // Name of parent link in limb
+//        std::string parent_name;
+//        // Limb length
+//        double max_limb_length, limb_length_margin;
+//        IKparam ()
+//            : avoid_gain(0.001), reference_gain(0.01),
+//              pos_ik_error_count(0), rot_ik_error_count(0),
+//              limb_length_margin(0.02)
+//        {
+//        };
+//    };
+//    std::map<std::string, IKparam> ikp;
+//    // Used for ref joint angles overwrite before IK
+//    std::vector<int> overwrite_ref_ja_index_vec;
+//    // IK targets and current?
+//    hrp::dvector qrefv;
+//    hrp::Vector3 target_root_p;
+//    hrp::Matrix33 target_root_R;
+//    hrp::Vector3 current_root_p;
+//    hrp::Matrix33 current_root_R;
+//    // IK params
+//    double move_base_gain, ratio_for_vel;
+//    // For IK fail checking
+//    double pos_ik_thre, rot_ik_thre;
+//    // For limb stretch avoidance
+//    double d_root_height, limb_stretch_avoidance_time_const, limb_stretch_avoidance_vlimit[2], m_dt;
+//    bool use_limb_stretch_avoidance;
+
+    FullbodyInverseKinematicsSolver (hrp::BodyPtr& _robot, const std::string& _print_str, const double _dt) : SimpleFullbodyInverseKinematicsSolver(_robot,_print_str, _dt)
+//        : m_robot(_robot),
+//          ik_error_debug_print_freq(static_cast<int>(0.2/_dt)), // once per 0.2 [s]
+//          has_ik_failed(false),
+//          overwrite_ref_ja_index_vec(), ikp(),
+//          move_base_gain(0.8), ratio_for_vel(1.0),
+//          pos_ik_thre(0.5*1e-3), // [m]
+//          rot_ik_thre((1e-2)*M_PI/180.0), // [rad]
+//          print_str(_print_str), m_dt(_dt),
+//          use_limb_stretch_avoidance(false), limb_stretch_avoidance_time_const(1.5)
+    {
+        qorg.resize(m_robot->numJoints());
+        qrefv.resize(m_robot->numJoints());
+        limb_stretch_avoidance_vlimit[0] = -1000 * 1e-3 * _dt; // lower limit
+        limb_stretch_avoidance_vlimit[1] = 50 * 1e-3 * _dt; // upper limit
+    };
+    ~FullbodyInverseKinematicsSolver () {};
+
+    void initializeInterlockingJoints (std::vector<std::pair<hrp::Link*, hrp::Link*> > & interlocking_joints)
+    {
+        for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            std::cerr << "[" << print_str << "] Interlocking Joints for [" << it->first << "]" << std::endl;
+            it->second.manip->setInterlockingJointPairIndices(interlocking_joints, print_str);
+        }
+    };
+    void storeCurrentParameters()
+    {
+        current_root_p = m_robot->rootLink()->p;
+        current_root_R = m_robot->rootLink()->R;
+        for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
+            qorg[i] = m_robot->joint(i)->q;
+        }
+    };
+    void revertRobotStateToCurrent ()
+    {
+        for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            if (it->second.is_ik_enable) {
+                for ( unsigned int j = 0; j < it->second.manip->numJoints(); j++ ){
+                    int i = it->second.manip->joint(j)->jointId;
+                    m_robot->joint(i)->q = qorg[i];
+                }
+            }
+        }
+        m_robot->rootLink()->p = current_root_p;
+        m_robot->rootLink()->R = current_root_R;
+        m_robot->calcForwardKinematics();
+    };
+    void setReferenceJointAngles ()
+    {
+        for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
+            qrefv[i] = m_robot->joint(i)->q;
+        }
+    };
+    // Solve fullbody IK using limb IK
+    void solveFullbodyIK (const hrp::Vector3& _dif_cog, const bool is_transition)
+    {
+        m_robot->calcForwardKinematics();
+        solveIK();
+        m_robot->calcForwardKinematics();
+    }
+    hrp::Vector3 matrix_logEx(const hrp::Matrix33& m) {// copy
+        hrp::Vector3 mlog;
+        double q0, th;
+        hrp::Vector3 q;
+        double norm;
+
+        Eigen::Quaternion<double> eiq(m);
+        q0 = eiq.w();
+        q = eiq.vec();
+        norm = q.norm();
+        if (norm > 0) {
+            if ((q0 > 1.0e-10) || (q0 < -1.0e-10)) {
+                th = 2 * std::atan(norm / q0);
+            } else if (q0 > 0) {
+                th = M_PI / 2;
+            } else {
+                th = -M_PI / 2;
+            }
+            mlog = (th / norm) * q ;
+        } else {
+            mlog = hrp::Vector3::Zero();
+        }
+        return mlog;
+    }
+
+
+    void solveIK (){
+      const int BASE_DOF = 6;
+      const int WS_DOF = 6;
+      const int eenum = ikp.size();
+      const int ALL_DOF = BASE_DOF + m_robot->numJoints();
+      m_robot->calcForwardKinematics();
+      hrp::dmatrix J_all = hrp::dmatrix::Zero( BASE_DOF+WS_DOF*eenum, ALL_DOF );
+      hrp::dmatrix J_all_inv = hrp::dmatrix::Zero( ALL_DOF,  BASE_DOF+WS_DOF*eenum );
+      hrp::dvector dq_all = hrp::dvector::Zero( ALL_DOF );
+      hrp::dvector eevel_all = hrp::dvector::Zero( BASE_DOF+WS_DOF*eenum );
+
+      int ee_num_count = 0;
+      for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+        hrp::Matrix33 target_link_R(it->second.target_r0 * it->second.localR.transpose());
+        hrp::Vector3 target_link_p(it->second.target_p0 - target_link_R * it->second.localPos);
+        hrp::Vector3 vel_p(target_link_p - it->second.manip->endLink()->p);
+        hrp::Vector3 vel_r(it->second.manip->endLink()->R * matrix_logEx(it->second.manip->endLink()->R.transpose() * target_link_R));
+//        if(it->first == "rleg"){ eevel_all.segment(BASE_DOF, 6) << vel_p , vel_r;}
+//        if(it->first == "lleg"){ eevel_all.segment(BASE_DOF+6, 6) << vel_p , vel_r;}
+//        if(it->first == "rarm"){ eevel_all.segment(BASE_DOF+6+6, 6) << vel_p , vel_r;}
+//        if(it->first == "larm"){ eevel_all.segment(BASE_DOF+6+6+6, 6) << vel_p , vel_r;}
+        eevel_all.segment( BASE_DOF+ee_num_count*WS_DOF, WS_DOF) << vel_p , vel_r;
+        ee_num_count++;
+      }
+
+      J_all.block(0, 0, WS_DOF, WS_DOF ) = hrp::dmatrix::Identity(WS_DOF, WS_DOF);
+
+//      hrp::dmatrix tmp;
+//      ikp["rleg"].manip->calcJacobian(tmp);
+//      J_all.block(BASE_DOF,BASE_DOF, 6,ikp["rleg"].manip->numJoints()) = tmp;
+//      ikp["lleg"].manip->calcJacobian(tmp);
+//      J_all.block(BASE_DOF+6,BASE_DOF+6, 6,ikp["lleg"].manip->numJoints()) = tmp;
+//      ikp["rarm"].manip->calcJacobian(tmp);
+//      J_all.block(BASE_DOF+6+6,BASE_DOF+6+6+2+3, 6,ikp["rarm"].manip->numJoints()) = tmp;
+//      ikp["larm"].manip->calcJacobian(tmp);
+//      J_all.block(BASE_DOF+6+6+6,BASE_DOF+6+6+2+3+8, 6,ikp["larm"].manip->numJoints()) = tmp;
+
+      int ee_num_count2 = 0;
+      for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+        hrp::dmatrix J_ee;
+        it->second.manip->calcJacobian(J_ee);
+        for(int id_in_limb=0; id_in_limb<it->second.manip->numJoints(); id_in_limb++){
+          int id_in_body = it->second.manip->joint(id_in_limb)->jointId; //全身でのjoint番号
+          J_all.block(BASE_DOF+ee_num_count2*WS_DOF, BASE_DOF+id_in_body, 6, 1) = J_ee.col(id_in_limb);
+        }
+        ee_num_count2++;
+      }
+
+      hrp::calcPseudoInverse(J_all,J_all_inv,1.0e-3);
+
+      dq_all = J_all_inv * eevel_all; // dq = pseudoInverse(J) * v
+
+      static int count;
+      if(count++ % 1000 == 0){
+        std::cout<<"eevel_all\n"<<eevel_all<<std::endl;
+        std::cout<<"J\n"<<J_all<<std::endl;
+        std::cout<<"J_all_inv\n"<<J_all_inv<<std::endl;
+        std::cout<<"dq_all\n"<<dq_all<<std::endl;
+
+      }
+
+      for(int i=0;i<m_robot->numJoints();i++){
+        m_robot->joint(i)->q += dq_all(BASE_DOF + i) * m_dt * 0.9;
+      }
+
+      m_robot->calcForwardKinematics();
+//      vel_p *= vel_gain;
+//      vel_r *= vel_gain;
+//      v << dp, omega;
+//      calcJacobian(J);
+//      calcJacobianInverseNullspace(J, Jinv, Jnull);
+//      dq = Jinv * v; // dq = pseudoInverse(J) * v
+    }
+
+    // Solve limb IK
+//    bool solveLimbIK (IKparam& param, const std::string& limb_name, const double ratio_for_vel, const bool is_transition)
+//    {
+//        param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, ratio_for_vel,
+//                                                param.localPos, param.localR);
+//        checkIKTracking(param, limb_name, is_transition);
+//        return true;
+//    }
+    // IK fail check
+    void checkIKTracking (IKparam& param, const std::string& limb_name, const bool is_transition)
+    {
+        hrp::Vector3 vel_p, vel_r;
+        vel_p = param.target_p0 - (param.target_link->p + param.target_link->R * param.localPos);
+        rats::difference_rotation(vel_r, (param.target_link->R * param.localR), param.target_r0);
+        if (vel_p.norm() > pos_ik_thre && is_transition) {
+            if (param.pos_ik_error_count % ik_error_debug_print_freq == 0) {
+                std::cerr << "[" << print_str << "] Too large IK error in " << limb_name << " (vel_p) = [" << vel_p(0) << " " << vel_p(1) << " " << vel_p(2) << "][m], count = " << param.pos_ik_error_count << std::endl;
+            }
+            param.pos_ik_error_count++;
+            has_ik_failed = true;
+        } else {
+            param.pos_ik_error_count = 0;
+        }
+        if (vel_r.norm() > rot_ik_thre && is_transition) {
+            if (param.rot_ik_error_count % ik_error_debug_print_freq == 0) {
+                std::cerr << "[" << print_str << "] Too large IK error in " << limb_name << " (vel_r) = [" << vel_r(0) << " " << vel_r(1) << " " << vel_r(2) << "][rad], count = " << param.rot_ik_error_count << std::endl;
+            }
+            param.rot_ik_error_count++;
+            has_ik_failed = true;
+        } else {
+            param.rot_ik_error_count = 0;
+        }
+    };
+    // Reset IK fail params
+    void resetIKFailParam() {
+        has_ik_failed = false;
+        for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
+        }
+    }
+    // Get IKparam
+//    void getIKParam (std::vector<std::string>& ee_vec /*, _CORBA_Unbounded_Sequence<OpenHRP::AutoBalancerService::IKLimbParameters>& ik_limb_parameters*/)
+//    {
+//        ik_limb_parameters.length(ee_vec.size());
+//        for (size_t i = 0; i < ee_vec.size(); i++) {
+//            IKparam& param = ikp[ee_vec[i]];
+//            OpenHRP::AutoBalancerService::IKLimbParameters& ilp = ik_limb_parameters[i];
+//            ilp.ik_optional_weight_vector.length(param.manip->numJoints());
+//            std::vector<double> ov;
+//            ov.resize(param.manip->numJoints());
+//            param.manip->getOptionalWeightVector(ov);
+//            for (size_t j = 0; j < param.manip->numJoints(); j++) {
+//                ilp.ik_optional_weight_vector[j] = ov[j];
+//            }
+//            ilp.sr_gain = param.manip->getSRGain();
+//            ilp.avoid_gain = param.avoid_gain;
+//            ilp.reference_gain = param.reference_gain;
+//            ilp.manipulability_limit = param.manip->getManipulabilityLimit();
+//        }
+//    };
+//    // Set IKparam
+//    void setIKParam (std::vector<std::string>& ee_vec, const _CORBA_Unbounded_Sequence<OpenHRP::AutoBalancerService::IKLimbParameters>& ik_limb_parameters)
+//    {
+//        std::cerr << "[" << print_str << "]  IK limb parameters" << std::endl;
+//        bool is_ik_limb_parameter_valid_length = true;
+//        if (ik_limb_parameters.length() != ee_vec.size()) {
+//            is_ik_limb_parameter_valid_length = false;
+//            std::cerr << "[" << print_str << "]   ik_limb_parameters invalid length! Cannot be set. (input = " << ik_limb_parameters.length() << ", desired = " << ee_vec.size() << ")" << std::endl;
+//        } else {
+//            for (size_t i = 0; i < ee_vec.size(); i++) {
+//                if (ikp[ee_vec[i]].manip->numJoints() != ik_limb_parameters[i].ik_optional_weight_vector.length())
+//                    is_ik_limb_parameter_valid_length = false;
+//            }
+//            if (is_ik_limb_parameter_valid_length) {
+//                for (size_t i = 0; i < ee_vec.size(); i++) {
+//                    IKparam& param = ikp[ee_vec[i]];
+//                    const OpenHRP::AutoBalancerService::IKLimbParameters& ilp = ik_limb_parameters[i];
+//                    std::vector<double> ov;
+//                    ov.resize(param.manip->numJoints());
+//                    for (size_t j = 0; j < param.manip->numJoints(); j++) {
+//                        ov[j] = ilp.ik_optional_weight_vector[j];
+//                    }
+//                    param.manip->setOptionalWeightVector(ov);
+//                    param.manip->setSRGain(ilp.sr_gain);
+//                    param.avoid_gain = ilp.avoid_gain;
+//                    param.reference_gain = ilp.reference_gain;
+//                    param.manip->setManipulabilityLimit(ilp.manipulability_limit);
+//                }
+//            } else {
+//                std::cerr << "[" << print_str << "]   ik_optional_weight_vector invalid length! Cannot be set. (input = [";
+//                for (size_t i = 0; i < ee_vec.size(); i++) {
+//                    std::cerr << ik_limb_parameters[i].ik_optional_weight_vector.length() << ", ";
+//                }
+//                std::cerr << "], desired = [";
+//                for (size_t i = 0; i < ee_vec.size(); i++) {
+//                    std::cerr << ikp[ee_vec[i]].manip->numJoints() << ", ";
+//                }
+//                std::cerr << "])" << std::endl;
+//            }
+//        }
+//        if (is_ik_limb_parameter_valid_length) {
+//            printIKparam(ee_vec);
+//        }
+//    };
+    // Avoid limb stretch
+    void limbStretchAvoidanceControl (const std::vector<hrp::Vector3>& target_p, const std::vector<std::string>& target_name)
+    {
+      m_robot->calcForwardKinematics();
+      double tmp_d_root_height = 0.0, prev_d_root_height = d_root_height;
+      if (use_limb_stretch_avoidance) {
+        for (size_t i = 0; i < target_p.size(); i++) {
+          // Check whether inside limb length limitation
+          hrp::Link* parent_link = m_robot->link(ikp[target_name[i]].parent_name);
+          hrp::Vector3 rel_target_p = target_p[i] - parent_link->p; // position from parent to target link (world frame)
+          double limb_length_limitation = ikp[target_name[i]].max_limb_length - ikp[target_name[i]].limb_length_margin;
+          double tmp = limb_length_limitation * limb_length_limitation - rel_target_p(0) * rel_target_p(0) - rel_target_p(1) * rel_target_p(1);
+          if (rel_target_p.norm() > limb_length_limitation && tmp >= 0) {
+            tmp_d_root_height = std::min(tmp_d_root_height, rel_target_p(2) + std::sqrt(tmp));
+          }
+        }
+        // Change root link height depending on limb length
+        d_root_height = tmp_d_root_height == 0.0 ? (- 1/limb_stretch_avoidance_time_const * d_root_height * m_dt + d_root_height) : tmp_d_root_height;
+      } else {
+        d_root_height = - 1/limb_stretch_avoidance_time_const * d_root_height * m_dt + d_root_height;
+      }
+      d_root_height = vlimit(d_root_height, prev_d_root_height + limb_stretch_avoidance_vlimit[0], prev_d_root_height + limb_stretch_avoidance_vlimit[1]);
+      m_robot->rootLink()->p(2) += d_root_height;
+    };
+    double vlimit(const double value, const double llimit_value, const double ulimit_value)
+    {
+      if (value > ulimit_value) {
+        return ulimit_value;
+      } else if (value < llimit_value) {
+        return llimit_value;
+      }
+      return value;
+    };
+    // Set parameter
+    void printParam () const
+    {
+        std::cerr << "[" << print_str << "]   move_base_gain = " << move_base_gain << std::endl;
+        std::cerr << "[" << print_str << "]   pos_ik_thre = " << pos_ik_thre << "[m], rot_ik_thre = " << rot_ik_thre << "[rad]" << std::endl;
+    };
+    // Set IKparam
+    void printIKparam (std::vector<std::string>& ee_vec)
+    {
+      std::cerr << "[" << print_str << "]   ik_optional_weight_vectors = ";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          IKparam& param = ikp[ee_vec[i]];
+          std::vector<double> ov;
+          ov.resize(param.manip->numJoints());
+          param.manip->getOptionalWeightVector(ov);
+          std::cerr << "[";
+          for (size_t j = 0; j < param.manip->numJoints(); j++) {
+              std::cerr << ov[j] << " ";
+          }
+          std::cerr << "]";
+      }
+      std::cerr << std::endl;
+      std::cerr << "[" << print_str << "]   sr_gains = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].manip->getSRGain() << ", ";
+      }
+      std::cerr << "]" << std::endl;
+      std::cerr << "[" << print_str << "]   avoid_gains = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].avoid_gain << ", ";
+      }
+      std::cerr << "]" << std::endl;
+      std::cerr << "[" << print_str << "]   reference_gains = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].reference_gain << ", ";
+      }
+      std::cerr << "]" << std::endl;
+      std::cerr << "[" << print_str << "]   manipulability_limits = [";
+      for (size_t i = 0; i < ee_vec.size(); i++) {
+          std::cerr << ikp[ee_vec[i]].manip->getManipulabilityLimit() << ", ";
+      }
+      std::cerr << "]" << std::endl;
+    };
+    hrp::Vector3 getEndEffectorPos(const std::string& limb_name){
+      return ikp[limb_name].target_link->p + ikp[limb_name].target_link->R * ikp[limb_name].localPos;
+    }
+    hrp::Matrix33 getEndEffectorRot(const std::string& limb_name){
+      return ikp[limb_name].target_link->R * ikp[limb_name].localR;
+    }
+};
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+#endif //  FULLBODYIK_H

From ea6a187be32b5cf5464d6dffc379b5aba108e60c Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 15 Aug 2017 08:51:06 +0900
Subject: [PATCH 111/217] full body ik and COM constraint OK

---
 .../SimpleFullbodyInverseKinematicsSolver.h   |   3 +-
 rtc/WholeBodyMasterSlave/FullBodyIK.h         | 446 +++++-------------
 .../WholeBodyMasterSlave.cpp                  |  77 ++-
 .../WholeBodyMasterSlave.h                    |   5 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          |   9 +-
 5 files changed, 197 insertions(+), 343 deletions(-)

diff --git a/rtc/AutoBalancer/SimpleFullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/SimpleFullbodyInverseKinematicsSolver.h
index 1cbabdb7e2d..ac2826233f9 100644
--- a/rtc/AutoBalancer/SimpleFullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/SimpleFullbodyInverseKinematicsSolver.h
@@ -4,6 +4,7 @@
 #include <hrpModel/Body.h>
 #include "../ImpedanceController/JointPathEx.h"
 #include "../ImpedanceController/RatsMatrix.h"
+#include "AutoBalancerService_impl.h"
 
 // Class for Simple Fullbody Inverse Kinematics
 //   Input : target root pos and rot, target COG, target joint angles, target EE coords
@@ -11,7 +12,7 @@
 //   Algorithm : Limb IK + move base
 class SimpleFullbodyInverseKinematicsSolver
 {
-private:
+protected:
     // Robot model for IK
     hrp::BodyPtr m_robot;
     // Org (current) joint angles before IK
diff --git a/rtc/WholeBodyMasterSlave/FullBodyIK.h b/rtc/WholeBodyMasterSlave/FullBodyIK.h
index 50cbca1281f..bcc86f1ea9b 100644
--- a/rtc/WholeBodyMasterSlave/FullBodyIK.h
+++ b/rtc/WholeBodyMasterSlave/FullBodyIK.h
@@ -7,74 +7,19 @@
 #include "../ImpedanceController/RatsMatrix.h"
 
 #include "../AutoBalancer/SimpleFullbodyInverseKinematicsSolver.h"
-//#include "hrpsys/idl/AutoBalancerService.hh"
-#include "../AutoBalancer/AutoBalancerService_impl.h"
+
+#define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
+
 
 class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSolver{
-//  private:
-//    // Robot model for IK
-//    hrp::BodyPtr m_robot;
-//    // Org (current) joint angles before IK
-//    hrp::dvector qorg;
-//    // IK fail checking
-//    int ik_error_debug_print_freq;
-//    std::string print_str;
-//    bool has_ik_failed;
-  public:
-//    // IK parameter for each limb
-//    struct IKparam {
-//        // IK target EE coords
-//        hrp::Vector3 target_p0;
-//        hrp::Matrix33 target_r0;
-//        // EE offset, EE link
-//        hrp::Vector3 localPos;
-//        hrp::Matrix33 localR;
-//        hrp::Link* target_link;
-//        // IK solver and parameter
-//        hrp::JointPathExPtr manip;
-//        double avoid_gain, reference_gain;
-//        // IK fail checking
-//        size_t pos_ik_error_count, rot_ik_error_count;
-//        // Solve ik or not
-//        bool is_ik_enable;
-//        // Name of parent link in limb
-//        std::string parent_name;
-//        // Limb length
-//        double max_limb_length, limb_length_margin;
-//        IKparam ()
-//            : avoid_gain(0.001), reference_gain(0.01),
-//              pos_ik_error_count(0), rot_ik_error_count(0),
-//              limb_length_margin(0.02)
-//        {
-//        };
-//    };
-//    std::map<std::string, IKparam> ikp;
-//    // Used for ref joint angles overwrite before IK
-//    std::vector<int> overwrite_ref_ja_index_vec;
-//    // IK targets and current?
-//    hrp::dvector qrefv;
-//    hrp::Vector3 target_root_p;
-//    hrp::Matrix33 target_root_R;
-//    hrp::Vector3 current_root_p;
-//    hrp::Matrix33 current_root_R;
-//    // IK params
-//    double move_base_gain, ratio_for_vel;
-//    // For IK fail checking
-//    double pos_ik_thre, rot_ik_thre;
-//    // For limb stretch avoidance
-//    double d_root_height, limb_stretch_avoidance_time_const, limb_stretch_avoidance_vlimit[2], m_dt;
-//    bool use_limb_stretch_avoidance;
+  protected:
+    hrp::dmatrix J_all, J_all_inv;
+    hrp::dvector dq_all, eevel_all;
 
+  public:
+    hrp::Vector3 target_com_p0;
+    hrp::Matrix33 target_com_r0;
     FullbodyInverseKinematicsSolver (hrp::BodyPtr& _robot, const std::string& _print_str, const double _dt) : SimpleFullbodyInverseKinematicsSolver(_robot,_print_str, _dt)
-//        : m_robot(_robot),
-//          ik_error_debug_print_freq(static_cast<int>(0.2/_dt)), // once per 0.2 [s]
-//          has_ik_failed(false),
-//          overwrite_ref_ja_index_vec(), ikp(),
-//          move_base_gain(0.8), ratio_for_vel(1.0),
-//          pos_ik_thre(0.5*1e-3), // [m]
-//          rot_ik_thre((1e-2)*M_PI/180.0), // [rad]
-//          print_str(_print_str), m_dt(_dt),
-//          use_limb_stretch_avoidance(false), limb_stretch_avoidance_time_const(1.5)
     {
         qorg.resize(m_robot->numJoints());
         qrefv.resize(m_robot->numJoints());
@@ -83,41 +28,6 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
     };
     ~FullbodyInverseKinematicsSolver () {};
 
-    void initializeInterlockingJoints (std::vector<std::pair<hrp::Link*, hrp::Link*> > & interlocking_joints)
-    {
-        for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            std::cerr << "[" << print_str << "] Interlocking Joints for [" << it->first << "]" << std::endl;
-            it->second.manip->setInterlockingJointPairIndices(interlocking_joints, print_str);
-        }
-    };
-    void storeCurrentParameters()
-    {
-        current_root_p = m_robot->rootLink()->p;
-        current_root_R = m_robot->rootLink()->R;
-        for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
-            qorg[i] = m_robot->joint(i)->q;
-        }
-    };
-    void revertRobotStateToCurrent ()
-    {
-        for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            if (it->second.is_ik_enable) {
-                for ( unsigned int j = 0; j < it->second.manip->numJoints(); j++ ){
-                    int i = it->second.manip->joint(j)->jointId;
-                    m_robot->joint(i)->q = qorg[i];
-                }
-            }
-        }
-        m_robot->rootLink()->p = current_root_p;
-        m_robot->rootLink()->R = current_root_R;
-        m_robot->calcForwardKinematics();
-    };
-    void setReferenceJointAngles ()
-    {
-        for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
-            qrefv[i] = m_robot->joint(i)->q;
-        }
-    };
     // Solve fullbody IK using limb IK
     void solveFullbodyIK (const hrp::Vector3& _dif_cog, const bool is_transition)
     {
@@ -149,18 +59,19 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
         }
         return mlog;
     }
-
-
+#define USE_BASE_CONSTRAINT 0
     void solveIK (){
       const int BASE_DOF = 6;
+      const int COM_DOF = 6;
       const int WS_DOF = 6;
       const int eenum = ikp.size();
       const int ALL_DOF = BASE_DOF + m_robot->numJoints();
+
       m_robot->calcForwardKinematics();
-      hrp::dmatrix J_all = hrp::dmatrix::Zero( BASE_DOF+WS_DOF*eenum, ALL_DOF );
-      hrp::dmatrix J_all_inv = hrp::dmatrix::Zero( ALL_DOF,  BASE_DOF+WS_DOF*eenum );
-      hrp::dvector dq_all = hrp::dvector::Zero( ALL_DOF );
-      hrp::dvector eevel_all = hrp::dvector::Zero( BASE_DOF+WS_DOF*eenum );
+      J_all = hrp::dmatrix::Zero( BASE_DOF+WS_DOF*eenum+COM_DOF, ALL_DOF );
+      J_all_inv = hrp::dmatrix::Zero( ALL_DOF,  BASE_DOF+WS_DOF*eenum+COM_DOF );
+      dq_all = hrp::dvector::Zero( ALL_DOF );
+      eevel_all = hrp::dvector::Zero( BASE_DOF+WS_DOF*eenum+COM_DOF );
 
       int ee_num_count = 0;
       for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
@@ -168,28 +79,25 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
         hrp::Vector3 target_link_p(it->second.target_p0 - target_link_R * it->second.localPos);
         hrp::Vector3 vel_p(target_link_p - it->second.manip->endLink()->p);
         hrp::Vector3 vel_r(it->second.manip->endLink()->R * matrix_logEx(it->second.manip->endLink()->R.transpose() * target_link_R));
-//        if(it->first == "rleg"){ eevel_all.segment(BASE_DOF, 6) << vel_p , vel_r;}
-//        if(it->first == "lleg"){ eevel_all.segment(BASE_DOF+6, 6) << vel_p , vel_r;}
-//        if(it->first == "rarm"){ eevel_all.segment(BASE_DOF+6+6, 6) << vel_p , vel_r;}
-//        if(it->first == "larm"){ eevel_all.segment(BASE_DOF+6+6+6, 6) << vel_p , vel_r;}
         eevel_all.segment( BASE_DOF+ee_num_count*WS_DOF, WS_DOF) << vel_p , vel_r;
         ee_num_count++;
       }
 
-      J_all.block(0, 0, WS_DOF, WS_DOF ) = hrp::dmatrix::Identity(WS_DOF, WS_DOF);
+      const double com_ik_weight = 1;
+      hrp::Vector3 vel_com_p(target_com_p0 - m_robot->calcCM());
+      hrp::Vector3 vel_com_r(m_robot->rootLink()->R * matrix_logEx(m_robot->rootLink()->R.transpose() * target_com_r0));
+      eevel_all.segment( BASE_DOF+eenum*WS_DOF, WS_DOF) << vel_com_p * com_ik_weight , vel_com_r * com_ik_weight;
 
-//      hrp::dmatrix tmp;
-//      ikp["rleg"].manip->calcJacobian(tmp);
-//      J_all.block(BASE_DOF,BASE_DOF, 6,ikp["rleg"].manip->numJoints()) = tmp;
-//      ikp["lleg"].manip->calcJacobian(tmp);
-//      J_all.block(BASE_DOF+6,BASE_DOF+6, 6,ikp["lleg"].manip->numJoints()) = tmp;
-//      ikp["rarm"].manip->calcJacobian(tmp);
-//      J_all.block(BASE_DOF+6+6,BASE_DOF+6+6+2+3, 6,ikp["rarm"].manip->numJoints()) = tmp;
-//      ikp["larm"].manip->calcJacobian(tmp);
-//      J_all.block(BASE_DOF+6+6+6,BASE_DOF+6+6+2+3+8, 6,ikp["larm"].manip->numJoints()) = tmp;
+      J_all.block(0, 0, WS_DOF, WS_DOF ) = hrp::dmatrix::Identity(WS_DOF, WS_DOF);
 
       int ee_num_count2 = 0;
       for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+        hrp::dmatrix J_b2ee = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
+        hrp::Vector3 p_b2ee = it->second.manip->endLink()->p - m_robot->rootLink()->p;
+        J_b2ee.block(0, 3, 3, 1) = hrp::Vector3::UnitX().cross(p_b2ee);
+        J_b2ee.block(0, 4, 3, 1) = hrp::Vector3::UnitY().cross(p_b2ee);
+        J_b2ee.block(0, 5, 3, 1) = hrp::Vector3::UnitZ().cross(p_b2ee);
+        J_all.block(BASE_DOF+WS_DOF*ee_num_count2, 0, WS_DOF, WS_DOF ) = J_b2ee;
         hrp::dmatrix J_ee;
         it->second.manip->calcJacobian(J_ee);
         for(int id_in_limb=0; id_in_limb<it->second.manip->numJoints(); id_in_limb++){
@@ -199,237 +107,115 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
         ee_num_count2++;
       }
 
-      hrp::calcPseudoInverse(J_all,J_all_inv,1.0e-3);
+      hrp::dmatrix J_com;
+      m_robot->calcCMJacobian(NULL, J_com);
+      J_all.block(BASE_DOF+eenum*WS_DOF, BASE_DOF, 3, m_robot->numJoints()) = J_com;
+
+      hrp::dmatrix J_b2com = hrp::dmatrix::Identity( COM_DOF,  BASE_DOF );
+      hrp::Vector3 p_b2com = m_robot->calcCM() - m_robot->rootLink()->p;
+      J_b2com.block(0, 3, 3, 1) = hrp::Vector3::UnitX().cross(p_b2com);
+      J_b2com.block(0, 4, 3, 1) = hrp::Vector3::UnitY().cross(p_b2com);
+      J_b2com.block(0, 5, 3, 1) = hrp::Vector3::UnitZ().cross(p_b2com);
+      J_all.block(BASE_DOF+eenum*WS_DOF, 0, COM_DOF, BASE_DOF) = J_b2com;
+
+//      //base拘束しない場合
+      if(!USE_BASE_CONSTRAINT){
+        eevel_all = eevel_all.bottomRows(WS_DOF*eenum+COM_DOF);//上6要素削除
+        J_all = J_all.bottomRows(WS_DOF*eenum+COM_DOF);//上6行削除
+      }
 
-      dq_all = J_all_inv * eevel_all; // dq = pseudoInverse(J) * v
+      struct timespec startT, endT;
+      clock_gettime(CLOCK_REALTIME, &startT);
+//      hrp::calcPseudoInverse(J_all,J_all_inv,1.0e-3);//超遅い1[ms]とか
+      calcJacobianInverseNullspace_copy(m_robot, J_all, J_all_inv);
 
       static int count;
-      if(count++ % 1000 == 0){
+      if(count % 1000 == 0){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ calcPseudoInverse" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
+
+      dq_all = J_all_inv * eevel_all; // dq = pseudoInverse(J) * v
+      if(count % 1000 == 0){
+        std::cout<<"solveIK"<<std::endl;
         std::cout<<"eevel_all\n"<<eevel_all<<std::endl;
-        std::cout<<"J\n"<<J_all<<std::endl;
-        std::cout<<"J_all_inv\n"<<J_all_inv<<std::endl;
+        std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
+        std::cout<<std::setprecision(2) << "J_all_inv=\n"<<J_all_inv<<std::setprecision(6)<<std::endl;
         std::cout<<"dq_all\n"<<dq_all<<std::endl;
-
       }
 
+      const double LAMBDA = 1.0;
+
+      for(int i=0;i<3;i++){ m_robot->rootLink()->p(i) += dq_all(i) * LAMBDA/* * m_dt*/; }//vel計算する時からm_dt使ってないから使わなくていい
+      m_robot->rootLink()->R *= hrp::rotFromRpy( hrp::Vector3(dq_all(3),dq_all(4),dq_all(5)) * LAMBDA /* * m_dt*/ );
+
       for(int i=0;i<m_robot->numJoints();i++){
-        m_robot->joint(i)->q += dq_all(BASE_DOF + i) * m_dt * 0.9;
+        m_robot->joint(i)->q += dq_all(BASE_DOF + i) * LAMBDA/* * m_dt*/;
+        LIMIT_MINMAX(m_robot->joint(i)->q, m_robot->joint(i)->llimit, m_robot->joint(i)->ulimit);
       }
-
       m_robot->calcForwardKinematics();
-//      vel_p *= vel_gain;
-//      vel_r *= vel_gain;
-//      v << dp, omega;
-//      calcJacobian(J);
-//      calcJacobianInverseNullspace(J, Jinv, Jnull);
-//      dq = Jinv * v; // dq = pseudoInverse(J) * v
+      count++;
     }
 
-    // Solve limb IK
-//    bool solveLimbIK (IKparam& param, const std::string& limb_name, const double ratio_for_vel, const bool is_transition)
-//    {
-//        param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, ratio_for_vel,
-//                                                param.localPos, param.localR);
-//        checkIKTracking(param, limb_name, is_transition);
-//        return true;
-//    }
-    // IK fail check
-    void checkIKTracking (IKparam& param, const std::string& limb_name, const bool is_transition)
-    {
-        hrp::Vector3 vel_p, vel_r;
-        vel_p = param.target_p0 - (param.target_link->p + param.target_link->R * param.localPos);
-        rats::difference_rotation(vel_r, (param.target_link->R * param.localR), param.target_r0);
-        if (vel_p.norm() > pos_ik_thre && is_transition) {
-            if (param.pos_ik_error_count % ik_error_debug_print_freq == 0) {
-                std::cerr << "[" << print_str << "] Too large IK error in " << limb_name << " (vel_p) = [" << vel_p(0) << " " << vel_p(1) << " " << vel_p(2) << "][m], count = " << param.pos_ik_error_count << std::endl;
-            }
-            param.pos_ik_error_count++;
-            has_ik_failed = true;
-        } else {
-            param.pos_ik_error_count = 0;
+
+    bool calcJacobianInverseNullspace_copy(hrp::BodyPtr robot, hrp::dmatrix &J, hrp::dmatrix &Jinv) {
+#define deg2rad(x)((x)*M_PI/180)
+#define rad2deg(rad) (rad * 180 / M_PI)
+#define eps_eq(a, b, c)  (fabs((a)-(b)) <= c)
+      const int BASE_DOF = 6;
+      const int ALL_DOF = J.cols();//BASE_DOF + m_robot->numJoints()
+      hrp::dmatrix w = hrp::dmatrix::Identity(ALL_DOF,ALL_DOF);
+      std::vector<double> avoid_weight_gain, optional_weight_vector;
+      avoid_weight_gain.resize(robot->numJoints());
+      optional_weight_vector.resize(robot->numJoints());
+      for(int j=0; j < m_robot->numJoints(); j++){
+        optional_weight_vector[j] = 1.0;
+        avoid_weight_gain[j] = 100000000000000000000.0;
+      }
+      bool use_inside_joint_weight_retrieval = true;
+
+      for ( int j = 0; j < robot->numJoints() ; j++ ) {
+        double jang = robot->joint(j)->q;
+        double jmax = robot->joint(j)->ulimit;
+        double jmin = robot->joint(j)->llimit;
+        double e = deg2rad(1);
+        if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+        } else if ( eps_eq(jang, jmax,e) ) {
+          jang = jmax - e;
+        } else if ( eps_eq(jang, jmin,e) ) {
+          jang = jmin + e;
         }
-        if (vel_r.norm() > rot_ik_thre && is_transition) {
-            if (param.rot_ik_error_count % ik_error_debug_print_freq == 0) {
-                std::cerr << "[" << print_str << "] Too large IK error in " << limb_name << " (vel_r) = [" << vel_r(0) << " " << vel_r(1) << " " << vel_r(2) << "][rad], count = " << param.rot_ik_error_count << std::endl;
-            }
-            param.rot_ik_error_count++;
-            has_ik_failed = true;
+        double r;
+        if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+          r = DBL_MAX;
         } else {
-            param.rot_ik_error_count = 0;
+          r = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) /
+          (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
+          if (isnan(r)) r = 0;
         }
-    };
-    // Reset IK fail params
-    void resetIKFailParam() {
-        has_ik_failed = false;
-        for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
-        }
-    }
-    // Get IKparam
-//    void getIKParam (std::vector<std::string>& ee_vec /*, _CORBA_Unbounded_Sequence<OpenHRP::AutoBalancerService::IKLimbParameters>& ik_limb_parameters*/)
-//    {
-//        ik_limb_parameters.length(ee_vec.size());
-//        for (size_t i = 0; i < ee_vec.size(); i++) {
-//            IKparam& param = ikp[ee_vec[i]];
-//            OpenHRP::AutoBalancerService::IKLimbParameters& ilp = ik_limb_parameters[i];
-//            ilp.ik_optional_weight_vector.length(param.manip->numJoints());
-//            std::vector<double> ov;
-//            ov.resize(param.manip->numJoints());
-//            param.manip->getOptionalWeightVector(ov);
-//            for (size_t j = 0; j < param.manip->numJoints(); j++) {
-//                ilp.ik_optional_weight_vector[j] = ov[j];
-//            }
-//            ilp.sr_gain = param.manip->getSRGain();
-//            ilp.avoid_gain = param.avoid_gain;
-//            ilp.reference_gain = param.reference_gain;
-//            ilp.manipulability_limit = param.manip->getManipulabilityLimit();
-//        }
-//    };
-//    // Set IKparam
-//    void setIKParam (std::vector<std::string>& ee_vec, const _CORBA_Unbounded_Sequence<OpenHRP::AutoBalancerService::IKLimbParameters>& ik_limb_parameters)
-//    {
-//        std::cerr << "[" << print_str << "]  IK limb parameters" << std::endl;
-//        bool is_ik_limb_parameter_valid_length = true;
-//        if (ik_limb_parameters.length() != ee_vec.size()) {
-//            is_ik_limb_parameter_valid_length = false;
-//            std::cerr << "[" << print_str << "]   ik_limb_parameters invalid length! Cannot be set. (input = " << ik_limb_parameters.length() << ", desired = " << ee_vec.size() << ")" << std::endl;
-//        } else {
-//            for (size_t i = 0; i < ee_vec.size(); i++) {
-//                if (ikp[ee_vec[i]].manip->numJoints() != ik_limb_parameters[i].ik_optional_weight_vector.length())
-//                    is_ik_limb_parameter_valid_length = false;
-//            }
-//            if (is_ik_limb_parameter_valid_length) {
-//                for (size_t i = 0; i < ee_vec.size(); i++) {
-//                    IKparam& param = ikp[ee_vec[i]];
-//                    const OpenHRP::AutoBalancerService::IKLimbParameters& ilp = ik_limb_parameters[i];
-//                    std::vector<double> ov;
-//                    ov.resize(param.manip->numJoints());
-//                    for (size_t j = 0; j < param.manip->numJoints(); j++) {
-//                        ov[j] = ilp.ik_optional_weight_vector[j];
-//                    }
-//                    param.manip->setOptionalWeightVector(ov);
-//                    param.manip->setSRGain(ilp.sr_gain);
-//                    param.avoid_gain = ilp.avoid_gain;
-//                    param.reference_gain = ilp.reference_gain;
-//                    param.manip->setManipulabilityLimit(ilp.manipulability_limit);
-//                }
-//            } else {
-//                std::cerr << "[" << print_str << "]   ik_optional_weight_vector invalid length! Cannot be set. (input = [";
-//                for (size_t i = 0; i < ee_vec.size(); i++) {
-//                    std::cerr << ik_limb_parameters[i].ik_optional_weight_vector.length() << ", ";
-//                }
-//                std::cerr << "], desired = [";
-//                for (size_t i = 0; i < ee_vec.size(); i++) {
-//                    std::cerr << ikp[ee_vec[i]].manip->numJoints() << ", ";
-//                }
-//                std::cerr << "])" << std::endl;
-//            }
-//        }
-//        if (is_ik_limb_parameter_valid_length) {
-//            printIKparam(ee_vec);
-//        }
-//    };
-    // Avoid limb stretch
-    void limbStretchAvoidanceControl (const std::vector<hrp::Vector3>& target_p, const std::vector<std::string>& target_name)
-    {
-      m_robot->calcForwardKinematics();
-      double tmp_d_root_height = 0.0, prev_d_root_height = d_root_height;
-      if (use_limb_stretch_avoidance) {
-        for (size_t i = 0; i < target_p.size(); i++) {
-          // Check whether inside limb length limitation
-          hrp::Link* parent_link = m_robot->link(ikp[target_name[i]].parent_name);
-          hrp::Vector3 rel_target_p = target_p[i] - parent_link->p; // position from parent to target link (world frame)
-          double limb_length_limitation = ikp[target_name[i]].max_limb_length - ikp[target_name[i]].limb_length_margin;
-          double tmp = limb_length_limitation * limb_length_limitation - rel_target_p(0) * rel_target_p(0) - rel_target_p(1) * rel_target_p(1);
-          if (rel_target_p.norm() > limb_length_limitation && tmp >= 0) {
-            tmp_d_root_height = std::min(tmp_d_root_height, rel_target_p(2) + std::sqrt(tmp));
-          }
+        if (( r - avoid_weight_gain[j] ) >= 0 ) {
+          w(BASE_DOF+j, BASE_DOF+j) = optional_weight_vector[j] * ( 1.0 / ( 1.0 + r) );
+        } else {
+          if (use_inside_joint_weight_retrieval)
+          w(BASE_DOF+j, BASE_DOF+j) = optional_weight_vector[j] * 1.0;
+          else
+          w(BASE_DOF+j, BASE_DOF+j) = optional_weight_vector[j] * ( 1.0 / ( 1.0 + r) );
         }
-        // Change root link height depending on limb length
-        d_root_height = tmp_d_root_height == 0.0 ? (- 1/limb_stretch_avoidance_time_const * d_root_height * m_dt + d_root_height) : tmp_d_root_height;
-      } else {
-        d_root_height = - 1/limb_stretch_avoidance_time_const * d_root_height * m_dt + d_root_height;
+        avoid_weight_gain[j] = r;
       }
-      d_root_height = vlimit(d_root_height, prev_d_root_height + limb_stretch_avoidance_vlimit[0], prev_d_root_height + limb_stretch_avoidance_vlimit[1]);
-      m_robot->rootLink()->p(2) += d_root_height;
-    };
-    double vlimit(const double value, const double llimit_value, const double ulimit_value)
-    {
-      if (value > ulimit_value) {
-        return ulimit_value;
-      } else if (value < llimit_value) {
-        return llimit_value;
-      }
-      return value;
-    };
-    // Set parameter
-    void printParam () const
-    {
-        std::cerr << "[" << print_str << "]   move_base_gain = " << move_base_gain << std::endl;
-        std::cerr << "[" << print_str << "]   pos_ik_thre = " << pos_ik_thre << "[m], rot_ik_thre = " << rot_ik_thre << "[rad]" << std::endl;
-    };
-    // Set IKparam
-    void printIKparam (std::vector<std::string>& ee_vec)
-    {
-      std::cerr << "[" << print_str << "]   ik_optional_weight_vectors = ";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          IKparam& param = ikp[ee_vec[i]];
-          std::vector<double> ov;
-          ov.resize(param.manip->numJoints());
-          param.manip->getOptionalWeightVector(ov);
-          std::cerr << "[";
-          for (size_t j = 0; j < param.manip->numJoints(); j++) {
-              std::cerr << ov[j] << " ";
-          }
-          std::cerr << "]";
-      }
-      std::cerr << std::endl;
-      std::cerr << "[" << print_str << "]   sr_gains = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].manip->getSRGain() << ", ";
-      }
-      std::cerr << "]" << std::endl;
-      std::cerr << "[" << print_str << "]   avoid_gains = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].avoid_gain << ", ";
-      }
-      std::cerr << "]" << std::endl;
-      std::cerr << "[" << print_str << "]   reference_gains = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].reference_gain << ", ";
-      }
-      std::cerr << "]" << std::endl;
-      std::cerr << "[" << print_str << "]   manipulability_limits = [";
-      for (size_t i = 0; i < ee_vec.size(); i++) {
-          std::cerr << ikp[ee_vec[i]].manip->getManipulabilityLimit() << ", ";
+      w.topLeftCorner(BASE_DOF, BASE_DOF) = hrp::dmatrix::Identity(BASE_DOF,BASE_DOF) * 0.1;//baseの重みテスト
+
+      const double manipulability_limit = 0.1;
+      const double sr_gain = 1.0;
+      const double manipulability_gain = 0.001;
+
+      double manipulability = sqrt((J*J.transpose()).determinant());
+      double k = 0;
+      if ( manipulability < manipulability_limit ) {
+        k = manipulability_gain * pow((1 - ( manipulability / manipulability_limit )), 2);
       }
-      std::cerr << "]" << std::endl;
-    };
-    hrp::Vector3 getEndEffectorPos(const std::string& limb_name){
-      return ikp[limb_name].target_link->p + ikp[limb_name].target_link->R * ikp[limb_name].localPos;
-    }
-    hrp::Matrix33 getEndEffectorRot(const std::string& limb_name){
-      return ikp[limb_name].target_link->R * ikp[limb_name].localR;
+      hrp::calcSRInverse(J, Jinv, sr_gain * k, w);
+      //        Jnull = ( hrp::dmatrix::Identity(n, n) - Jinv * J);
+      return true;
     }
 };
 
 
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
 #endif //  FULLBODYIK_H
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index a9eca1eb202..f92c0728ba9 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -183,10 +183,10 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     q_ip->setName(std::string(m_profile.instance_name)+" q_ip");
     q_ip->clear();
     // Generate FIK
-    fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
-    fik_rmc = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot_rmc, std::string(m_profile.instance_name), m_dt));
-    fik_ml = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot_ml, std::string(m_profile.instance_name), m_dt));
-    fik_vsafe = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot_vsafe, std::string(m_profile.instance_name), m_dt));
+    fik = fikPtr(new FullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
+    fik_rmc = fikPtr(new FullbodyInverseKinematicsSolver(m_robot_rmc, std::string(m_profile.instance_name), m_dt));
+    fik_ml = fikPtr(new FullbodyInverseKinematicsSolver(m_robot_ml, std::string(m_profile.instance_name), m_dt));
+    fik_vsafe = fikPtr(new FullbodyInverseKinematicsSolver(m_robot_vsafe, std::string(m_profile.instance_name), m_dt));
     fik_list.push_back(fik);
     fik_list.push_back(fik_rmc);
     fik_list.push_back(fik_ml);
@@ -222,6 +222,9 @@ void WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC:
       coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
       coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
       coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
+#ifdef USE_NEW_FIK
+      ee_base = "WAIST";//CHESTも使う実験
+#endif
       ABCIKparam tp;
       for (size_t j = 0; j < XYZ; j++) {
           coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
@@ -544,7 +547,7 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::
   }
   callnum++;
   if(DEBUGP){ fprintf(stderr,"\x1b[31mmaster-mode:\x1b[39m"); pose_ref.print(); }
-  solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave_Raw");
+//  solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave_Raw");
 }
 
 void WholeBodyMasterSlave::calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robot_in){
@@ -566,6 +569,62 @@ void WholeBodyMasterSlave::calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robo
   }
 }
 
+#ifdef USE_NEW_FIK
+void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix){
+  int com_ik_loop=0;
+  const int COM_IK_MAX_LOOP = 3;
+  const double COM_IK_MAX_ERROR = 1e-3;
+  const std::string names[4] = {"rleg","lleg","rarm","larm"};
+  const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
+  fik_in->target_com_p0 = com_ref.p;
+  fik_in->target_com_r0 = hrp::rotFromRpy(com_ref.rpy);//つまりbaseのrpyとして処理される
+  for(int i=0;i<4;i++){
+    if(fik_in->ikp.count(names[i])){
+      fik_in->ikp[names[i]].target_p0 = refs[i]->p;
+      fik_in->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);
+    }
+  }
+
+  //確かに毎回少しreset poseに引き戻すとこじれないが・・・
+  hrp::dvector reset_pose(33);
+  reset_pose <<
+      0.0,0.0,-0.349066,0.698132,-0.349066,0.0,
+      0.0,0.0,-0.349066,0.698132,-0.349066,0.0,
+      0.0,0.0,0.0, 0.0,0.0,
+      0.0,0.698132,-0.349066,-0.087266,-1.39626,0.0,0.0,-0.349066,
+      0.0,0.698132,0.349066,0.087266,-1.39626,0.0,0.0,-0.349066;
+  for(int i=0;i<33;i++){
+    robot_in->joint(i)->q = robot_in->joint(i)->q * 0.999 + reset_pose(i) * 0.001;
+  }
+
+
+  if( robot_in->link("HEAD_JOINT0") != NULL) robot_in->link("HEAD_JOINT0")->q = head_ref.rpy(y);
+  if( robot_in->link("HEAD_JOINT1") != NULL) robot_in->link("HEAD_JOINT1")->q = head_ref.rpy(p);
+  struct timespec startT, endT;
+  clock_gettime(CLOCK_REALTIME, &startT);
+  while( 1 ){  //COM 収束ループ
+    com_ik_loop++;
+    robot_in->calcForwardKinematics();
+    fik_in->solveIK();
+    hrp::Vector3 com_p_err = com_ref.p - robot_in->calcCM();
+    hrp::Vector3 rf_p_err = rf_ref.p - fik_in->getEndEffectorPos("rleg");
+    hrp::Vector3 lf_p_err = lf_ref.p - fik_in->getEndEffectorPos("lleg");
+    hrp::Vector3 rh_p_err = rh_ref.p - fik_in->getEndEffectorPos("rarm");
+    hrp::Vector3 lh_p_err = lh_ref.p - fik_in->getEndEffectorPos("larm");
+    if(
+        com_p_err.norm() < COM_IK_MAX_ERROR &&
+        rf_p_err.norm() < 1e-3 &&
+        lf_p_err.norm() < 1e-3 &&
+        rh_p_err.norm() < 1e-2 &&
+        lh_p_err.norm() < 1e-2){ break; }
+    if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
+  }
+  if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveIK loop" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
+
+  if(com_ik_loop != 1 && DEBUGP)cout<<"com_ik_loop:"<<com_ik_loop<<" @ "<<debug_prefix<<endl;
+}
+#else
+//旧ver
 void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix){
   int com_ik_loop=0;
   const int COM_IK_MAX_LOOP = 10;
@@ -593,24 +652,28 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     }
     if(robot_in->link("CHEST_JOINT2") != NULL){
       robot_in->link("CHEST_JOINT2")->q = (base2rh(X) - base2lh(X)) * (60 * D2R / 1.0);
-//      LIMIT_MINMAX(robot_in->link("CHEST_JOINT2")->q, robot_in->link("CHEST_JOINT2")->llimit, robot_in->link("CHEST_JOINT2")->ulimit);
-      LIMIT_MINMAX(robot_in->link("CHEST_JOINT2")->q, robot_in->link("CHEST_JOINT2")->llimit, 10);//左肩ロール故障につき
+      LIMIT_MINMAX(robot_in->link("CHEST_JOINT2")->q, robot_in->link("CHEST_JOINT2")->llimit, robot_in->link("CHEST_JOINT2")->ulimit);
     }
   }
   hrp::Vector3 tmp_com_err = hrp::Vector3::Zero();
+  struct timespec startT, endT;
   while( 1 ){  //COM 収束ループ
     com_ik_loop++;
     robot_in->rootLink()->p += tmp_com_err;
     robot_in->calcForwardKinematics();
+    clock_gettime(CLOCK_REALTIME, &startT);
     for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_in->ikp.begin(); it != fik_in->ikp.end(); it++ ) {
         if (it->second.is_ik_enable) fik_in->solveLimbIK (it->second, it->first, fik_in->ratio_for_vel, false);
     }
+    if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveLimbIK x 4" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
     tmp_com_err = com_ref.p - robot_in->calcCM();
     if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
     if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
   }
   if(com_ik_loop != 1 && DEBUGP)cout<<"com_ik_loop:"<<com_ik_loop<<" @ "<<debug_prefix<<endl;
 }
+#endif
+
 
 
 void WholeBodyMasterSlave::processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_normal_in, const HumanPose& pose_ref){
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 1f70a3edced..54ff56c3e10 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -18,7 +18,7 @@
 #include "interpolator.h"
 #include "../TorqueFilter/IIRFilter.h"
 #include "wbms_core.h"
-#include "../AutoBalancer/AutoBalancer.h"
+//#include "../AutoBalancer/AutoBalancer.h"
 
 using namespace RTC;
 
@@ -162,7 +162,8 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
     hrp::Link* target_link;
     bool is_active, has_toe_joint;
   };
-  typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
+//  typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
+  typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
   fikPtr fik, fik_rmc, fik_ml, fik_vsafe;
   hrp::BodyPtr m_robot, m_robot_rmc, m_robot_ml, m_robot_vsafe;
   std::vector<fikPtr> fik_list;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 3fa09b3cc1f..7906e134750 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -35,6 +35,10 @@ extern "C" {
 }
 #endif
 
+//#include "../AutoBalancer/AutoBalancer.h"
+#include "FullBodyIK.h"
+#define USE_NEW_FIK
+
 #define DEBUG 0
 #define dbg(var) std::cout<<#var"= "<<(var)<<std::endl
 #define LIMIT_MIN(x,min) (x= ( x<min ? min:x ))
@@ -132,7 +136,6 @@ class RobotConfig : UTIL_CONST {
     }
 };
 
-#include "../AutoBalancer/AutoBalancer.h"
 class WBMSCore : UTIL_CONST {
   private:
     double CNT_F_TH, h2r_ratio, tgt_h2r_ratio, HZ, DT;
@@ -173,7 +176,7 @@ class WBMSCore : UTIL_CONST {
     };
     struct WBMSparameters WBMSparam;
 
-    typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
+    typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
     fikPtr fik_ml;
     hrp::BodyPtr m_robot_ml;
     fikPtr fik_act;
@@ -542,7 +545,7 @@ class WBMSCore : UTIL_CONST {
           fik_ml->ikp[robot_l_names[i]].target_p0 = out.tgt[human_l_names[i]].abs.p;
         }
       }
-      for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_ml->ikp.begin(); it != fik_ml->ikp.end(); it++ ) {
+      for ( std::map<std::string, FullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_ml->ikp.begin(); it != fik_ml->ikp.end(); it++ ) {
           if (it->second.is_ik_enable) fik_ml->solveLimbIK (it->second, it->first, fik_ml->ratio_for_vel, false);
       }
       for(int i=0;i<4;i++){

From 9c84c1420df0cecdfa75a57656052298e2a2aa82 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 17 Aug 2017 05:43:20 +0900
Subject: [PATCH 112/217] almost complete full body ik

---
 .../FullbodyInverseKinematicsSolver.h         | 230 ++++++++++++++++++
 rtc/WholeBodyMasterSlave/CMakeLists.txt       |   2 +-
 rtc/WholeBodyMasterSlave/FullBodyIK.h         | 221 -----------------
 .../WholeBodyMasterSlave.cpp                  | 111 +++++++--
 rtc/WholeBodyMasterSlave/wbms_core.h          |   2 +-
 5 files changed, 325 insertions(+), 241 deletions(-)
 create mode 100644 rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
 delete mode 100644 rtc/WholeBodyMasterSlave/FullBodyIK.h

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
new file mode 100644
index 00000000000..3f821d74bfb
--- /dev/null
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -0,0 +1,230 @@
+#ifndef FULLBODYIK_H
+#define FULLBODYIK_H
+
+#include <hrpModel/Body.h>
+#include <hrpUtil/MatrixSolvers.h>
+#include "../ImpedanceController/JointPathEx.h"
+#include "SimpleFullbodyInverseKinematicsSolver.h"
+
+#define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
+#define deg2rad(x)((x)*M_PI/180)
+#define rad2deg(rad) (rad * 180 / M_PI)
+#define eps_eq(a, b, c)  (fabs((a)-(b)) <= c)
+
+
+class IKConstraintParam {
+  public:
+    std::string target_link_name;
+    // IK target EE coords
+    hrp::Vector3 target_p0;
+    hrp::Matrix33 target_r0;
+    // EE offset, EE link
+    hrp::Vector3 localPos;
+    hrp::Matrix33 localR;
+    hrp::dvector6 weight_vec;
+    hrp::ivector selection_vec;
+
+    IKConstraintParam ()
+      :target_p0(hrp::Vector3::Zero()),
+       target_r0(hrp::Matrix33::Identity()),
+       localPos(hrp::Vector3::Zero()),
+       localR(hrp::Matrix33::Identity()),
+       weight_vec(hrp::dvector6::Ones()),
+       selection_vec(hrp::ivector::Ones(6)){};
+};
+
+
+class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSolver{
+  protected:
+    hrp::dmatrix J_all, J_all_inv;
+    hrp::dvector dq_all, dp_ee_all;
+    hrp::dmatrix J_jpath;
+    hrp::dmatrix J_com, J_am;
+
+  public:
+    std::vector<IKConstraintParam> ik_tgt_list;
+    hrp::Vector3 target_com_p0;
+    hrp::Matrix33 target_com_r0;
+    hrp::dvector optional_weight_vector;
+    FullbodyInverseKinematicsSolver (hrp::BodyPtr& _robot, const std::string& _print_str, const double _dt) : SimpleFullbodyInverseKinematicsSolver(_robot,_print_str, _dt) {
+        qorg.resize(m_robot->numJoints());
+        qrefv.resize(m_robot->numJoints());
+        optional_weight_vector = hrp::dvector::Ones(m_robot->numJoints()+6);
+    };
+    hrp::Vector3 cur_P,cur_L;
+    ~FullbodyInverseKinematicsSolver () {};
+    // Solve fullbody IK using limb IK
+    void solveFullbodyIK ( const hrp::Vector3& _dif_cog, const bool is_transition) {
+
+      solveIK();
+
+    }
+    void solveIK (){
+      const int BASE_DOF = 6;
+      const int WS_DOF = 6;
+      const int ALL_DOF = BASE_DOF + m_robot->numJoints();
+
+      m_robot->calcForwardKinematics();
+      J_all = hrp::dmatrix::Zero(WS_DOF*ik_tgt_list.size(), ALL_DOF);
+      dq_all = hrp::dvector::Zero( ALL_DOF );
+      dp_ee_all = hrp::dvector::Zero(WS_DOF*ik_tgt_list.size());
+      hrp::ivector selection_vec_all = hrp::ivector::Zero(WS_DOF*ik_tgt_list.size());
+      hrp::dvector weight_vec_all = hrp::dvector::Ones(WS_DOF*ik_tgt_list.size());
+
+      for ( int i=0; i<ik_tgt_list.size(); i++ ) {
+        hrp::Link* target_link_ptr = m_robot->link(ik_tgt_list[i].target_link_name);
+        hrp::Matrix33 ref_link_origin_R(ik_tgt_list[i].target_r0 * ik_tgt_list[i].localR.transpose());
+        hrp::Vector3 ref_link_origin_p(ik_tgt_list[i].target_p0 - ref_link_origin_R * ik_tgt_list[i].localPos);
+        hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF);
+
+        if(target_link_ptr){
+          hrp::Vector3 vel_p_ref(ref_link_origin_p - target_link_ptr->p);
+          hrp::Vector3 vel_r_ref(target_link_ptr->R * omegaFromRotEx(target_link_ptr->R.transpose() * ref_link_origin_R));
+          dp_ee_all.segment(WS_DOF*i,WS_DOF) << vel_p_ref , vel_r_ref;
+
+          if(target_link_ptr == m_robot->rootLink()){      //base specific
+            J_part.rightCols(WS_DOF) = hrp::dmatrix::Identity(WS_DOF, WS_DOF);
+          }
+          else{    //normal link
+            hrp::JointPathEx tgt_jpex(m_robot, m_robot->rootLink(), target_link_ptr, m_dt, false, "");
+            tgt_jpex.calcJacobian(J_jpath);
+            for(int id_in_jpath=0; id_in_jpath<tgt_jpex.numJoints(); id_in_jpath++){
+              int id_in_body = tgt_jpex.joint(id_in_jpath)->jointId; //全身でのjoint番号
+              J_part.col(id_in_body) = J_jpath.col(id_in_jpath);
+            }
+            J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
+            J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(target_link_ptr->p - m_robot->rootLink()->p);
+          }
+        }
+        else if(!target_link_ptr && ik_tgt_list[i].target_link_name == "COM"){  // COM
+          hrp::Vector3 vel_com_p_ref(ref_link_origin_p - m_robot->calcCM());
+//          hrp::Vector3 vel_com_r_ref = (omegaFromRotEx(ref_link_origin_R) - cur_L) * m_dt;// COMのrotはAngulerMomentumとして扱う
+          hrp::Vector3 vel_com_r_ref = (omegaFromRotEx(ref_link_origin_R) - (cur_L - m_robot->rootLink()->p.cross(cur_P))) * m_dt;// COMのrotはAngulerMomentumとして扱う
+//          std::cerr<<"cur_L "<<cur_L.transpose()<<std::endl;
+//          std::cerr<<"m_robot->rootLink()->p.cross(cur_P) "<<m_robot->rootLink()->p.cross(cur_P).transpose()<<std::endl;
+//          std::cerr<<"vel_com_r_ref "<<vel_com_r_ref.transpose()<<std::endl;
+          dp_ee_all.segment(WS_DOF*i,WS_DOF) << vel_com_p_ref, vel_com_r_ref;
+          m_robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
+          m_robot->calcAngularMomentumJacobian(NULL, J_am);//すでにrootlink周りの角運動量ヤコビアンが返ってくる？
+          J_part << J_com, J_am;
+        }
+        else{// Error
+          std::cerr<<"Unknown Link Target !!"<<std::endl; continue;
+        }
+        weight_vec_all.segment(WS_DOF*i, WS_DOF) = ik_tgt_list[i].weight_vec;
+        selection_vec_all.segment(WS_DOF*i, WS_DOF) = ik_tgt_list[i].selection_vec;
+        J_all.middleRows(WS_DOF*i, WS_DOF) = J_part;
+      }
+
+      hrp::dmatrix selection_matrix = hrp::dmatrix::Zero(selection_vec_all.sum(), WS_DOF*ik_tgt_list.size());
+      for(int i=0, j=0; i<selection_vec_all.rows(); i++){ if(selection_vec_all(i) == 1){ selection_matrix(j, i) = 1.0; j++; } }
+
+      weight_vec_all /= weight_vec_all.maxCoeff();
+      J_all = selection_matrix * J_all;
+      dp_ee_all = selection_matrix * weight_vec_all.asDiagonal() * dp_ee_all;
+
+//      hrp::calcPseudoInverse(J_all,J_all_inv,1.0e-3);//超遅い1[ms]とか
+      calcJacobianInverseNullspace_copy(m_robot, J_all, J_all_inv);
+
+      static int count;
+
+      dq_all = J_all_inv * dp_ee_all; // dq = pseudoInverse(J) * v
+      if(count % 1000 == 0){
+        std::cout<<"solveIK"<<std::endl;
+        std::cout<<"eevel_all\n"<<dp_ee_all<<std::endl;
+        std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
+        std::cout<<std::setprecision(2) << "J_all_inv=\n"<<J_all_inv<<std::setprecision(6)<<std::endl;
+        std::cout<<std::setprecision(2) << "selection_matrix=\n"<<selection_matrix<<std::setprecision(6)<<std::endl;
+        std::cout<<"dq_all\n"<<dq_all<<std::endl;
+      }
+
+      const double LAMBDA = 1.0;
+
+      m_robot->rootLink()->p += dq_all.segment(m_robot->numJoints(),3) * LAMBDA/* * m_dt*/;//vel計算する時からm_dt使ってないから使わなくていい
+      m_robot->rootLink()->R *= hrp::rotFromRpy( dq_all.segment(m_robot->numJoints()+3,3)) * LAMBDA /* * m_dt*/ ;
+//
+//      m_robot->rootLink()->v = dq_all.segment(m_robot->numJoints(),3) * LAMBDA / m_dt; //for ang moment
+//      m_robot->rootLink()->w = dq_all.segment(m_robot->numJoints()+3,3) * LAMBDA / m_dt; //for ang moment
+
+      for(int i=0;i<m_robot->numJoints();i++){
+        m_robot->joint(i)->q += dq_all(i) * LAMBDA/* * m_dt*/;
+        LIMIT_MINMAX(m_robot->joint(i)->q, m_robot->joint(i)->llimit, m_robot->joint(i)->ulimit);
+//        m_robot->joint(i)->dq = dq_all(i) * LAMBDA / m_dt; //for calc ang moment
+      }
+      count++;
+    }
+
+
+    bool calcJacobianInverseNullspace_copy(hrp::BodyPtr robot, hrp::dmatrix &J, hrp::dmatrix &Jinv) {
+      const int BASE_DOF = 6;
+      const int ALL_DOF = J.cols();//BASE_DOF + m_robot->numJoints()
+      hrp::dmatrix w = hrp::dmatrix::Identity(ALL_DOF,ALL_DOF);
+      std::vector<double> avoid_weight_gain;
+      avoid_weight_gain.resize(robot->numJoints());
+      for(int j=0; j < m_robot->numJoints(); j++){
+        avoid_weight_gain[j] = 100000000000000000000.0;
+      }
+      bool use_inside_joint_weight_retrieval = true;
+
+      for ( int j = 0; j < robot->numJoints() ; j++ ) {
+        double jang = robot->joint(j)->q;
+        double jmax = robot->joint(j)->ulimit;
+        double jmin = robot->joint(j)->llimit;
+        double e = deg2rad(1);
+        if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+        } else if ( eps_eq(jang, jmax,e) ) {
+          jang = jmax - e;
+        } else if ( eps_eq(jang, jmin,e) ) {
+          jang = jmin + e;
+        }
+        double r;
+        if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+          r = DBL_MAX;
+        } else {
+          r = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) /
+          (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
+          if (isnan(r)) r = 0;
+        }
+        if (( r - avoid_weight_gain[j] ) >= 0 ) {
+          w(j, j) = optional_weight_vector[j] * ( 1.0 / ( 1.0 + r) );
+        } else {
+          if (use_inside_joint_weight_retrieval)
+          w(j, j) = optional_weight_vector[j] * 1.0;
+          else
+          w(j, j) = optional_weight_vector[j] * ( 1.0 / ( 1.0 + r) );
+        }
+        avoid_weight_gain[j] = r;
+      }
+
+      const double manipulability_limit = 0.1;
+      const double sr_gain = 1.0;
+      const double manipulability_gain = 0.001;
+
+      double manipulability = sqrt((J*J.transpose()).determinant());
+      double k = 0;
+      if ( manipulability < manipulability_limit ) {
+        k = manipulability_gain * pow((1 - ( manipulability / manipulability_limit )), 2);
+      }
+      hrp::calcSRInverse(J, Jinv, sr_gain * k, w);
+      //        Jnull = ( hrp::dmatrix::Identity(n, n) - Jinv * J);
+      return true;
+    }
+    hrp::Vector3 omegaFromRotEx(const hrp::Matrix33& r) {//copy from JointPathEx.cpp
+        using ::std::numeric_limits;
+        double alpha = (r(0,0) + r(1,1) + r(2,2) - 1.0) / 2.0;
+        if(fabs(alpha - 1.0) < 1.0e-12) {   //th=0,2PI;
+            return hrp::Vector3::Zero();
+        } else {
+            double th = acos(alpha);
+            double s = sin(th);
+            if (s < numeric_limits<double>::epsilon()) {   //th=PI
+                return hrp::Vector3( sqrt((r(0,0)+1)*0.5)*th, sqrt((r(1,1)+1)*0.5)*th, sqrt((r(2,2)+1)*0.5)*th );
+            }
+            double k = -0.5 * th / s;
+            return hrp::Vector3( (r(1,2) - r(2,1)) * k, (r(2,0) - r(0,2)) * k, (r(0,1) - r(1,0)) * k );
+        }
+    }
+};
+
+
+#endif //  FULLBODYIK_H
diff --git a/rtc/WholeBodyMasterSlave/CMakeLists.txt b/rtc/WholeBodyMasterSlave/CMakeLists.txt
index 041aa57ef1a..7914368f528 100644
--- a/rtc/WholeBodyMasterSlave/CMakeLists.txt
+++ b/rtc/WholeBodyMasterSlave/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(comp_sources WholeBodyMasterSlave.cpp WholeBodyMasterSlaveService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp ../TorqueFilter/IIRFilter.cpp wbms_core.cpp ../AutoBalancer/AutoBalancer.h)
+set(comp_sources WholeBodyMasterSlave.cpp WholeBodyMasterSlaveService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp ../TorqueFilter/IIRFilter.cpp wbms_core.cpp)
 set(libs ${OpenCV_LIBRARIES} hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub)
 add_library(WholeBodyMasterSlave SHARED ${comp_sources})
 target_link_libraries(WholeBodyMasterSlave ${libs})
diff --git a/rtc/WholeBodyMasterSlave/FullBodyIK.h b/rtc/WholeBodyMasterSlave/FullBodyIK.h
deleted file mode 100644
index bcc86f1ea9b..00000000000
--- a/rtc/WholeBodyMasterSlave/FullBodyIK.h
+++ /dev/null
@@ -1,221 +0,0 @@
-#ifndef FULLBODYIK_H
-#define FULLBODYIK_H
-
-#include <hrpModel/Body.h>
-#include <hrpUtil/MatrixSolvers.h>
-#include "../ImpedanceController/JointPathEx.h"
-#include "../ImpedanceController/RatsMatrix.h"
-
-#include "../AutoBalancer/SimpleFullbodyInverseKinematicsSolver.h"
-
-#define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
-
-
-class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSolver{
-  protected:
-    hrp::dmatrix J_all, J_all_inv;
-    hrp::dvector dq_all, eevel_all;
-
-  public:
-    hrp::Vector3 target_com_p0;
-    hrp::Matrix33 target_com_r0;
-    FullbodyInverseKinematicsSolver (hrp::BodyPtr& _robot, const std::string& _print_str, const double _dt) : SimpleFullbodyInverseKinematicsSolver(_robot,_print_str, _dt)
-    {
-        qorg.resize(m_robot->numJoints());
-        qrefv.resize(m_robot->numJoints());
-        limb_stretch_avoidance_vlimit[0] = -1000 * 1e-3 * _dt; // lower limit
-        limb_stretch_avoidance_vlimit[1] = 50 * 1e-3 * _dt; // upper limit
-    };
-    ~FullbodyInverseKinematicsSolver () {};
-
-    // Solve fullbody IK using limb IK
-    void solveFullbodyIK (const hrp::Vector3& _dif_cog, const bool is_transition)
-    {
-        m_robot->calcForwardKinematics();
-        solveIK();
-        m_robot->calcForwardKinematics();
-    }
-    hrp::Vector3 matrix_logEx(const hrp::Matrix33& m) {// copy
-        hrp::Vector3 mlog;
-        double q0, th;
-        hrp::Vector3 q;
-        double norm;
-
-        Eigen::Quaternion<double> eiq(m);
-        q0 = eiq.w();
-        q = eiq.vec();
-        norm = q.norm();
-        if (norm > 0) {
-            if ((q0 > 1.0e-10) || (q0 < -1.0e-10)) {
-                th = 2 * std::atan(norm / q0);
-            } else if (q0 > 0) {
-                th = M_PI / 2;
-            } else {
-                th = -M_PI / 2;
-            }
-            mlog = (th / norm) * q ;
-        } else {
-            mlog = hrp::Vector3::Zero();
-        }
-        return mlog;
-    }
-#define USE_BASE_CONSTRAINT 0
-    void solveIK (){
-      const int BASE_DOF = 6;
-      const int COM_DOF = 6;
-      const int WS_DOF = 6;
-      const int eenum = ikp.size();
-      const int ALL_DOF = BASE_DOF + m_robot->numJoints();
-
-      m_robot->calcForwardKinematics();
-      J_all = hrp::dmatrix::Zero( BASE_DOF+WS_DOF*eenum+COM_DOF, ALL_DOF );
-      J_all_inv = hrp::dmatrix::Zero( ALL_DOF,  BASE_DOF+WS_DOF*eenum+COM_DOF );
-      dq_all = hrp::dvector::Zero( ALL_DOF );
-      eevel_all = hrp::dvector::Zero( BASE_DOF+WS_DOF*eenum+COM_DOF );
-
-      int ee_num_count = 0;
-      for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-        hrp::Matrix33 target_link_R(it->second.target_r0 * it->second.localR.transpose());
-        hrp::Vector3 target_link_p(it->second.target_p0 - target_link_R * it->second.localPos);
-        hrp::Vector3 vel_p(target_link_p - it->second.manip->endLink()->p);
-        hrp::Vector3 vel_r(it->second.manip->endLink()->R * matrix_logEx(it->second.manip->endLink()->R.transpose() * target_link_R));
-        eevel_all.segment( BASE_DOF+ee_num_count*WS_DOF, WS_DOF) << vel_p , vel_r;
-        ee_num_count++;
-      }
-
-      const double com_ik_weight = 1;
-      hrp::Vector3 vel_com_p(target_com_p0 - m_robot->calcCM());
-      hrp::Vector3 vel_com_r(m_robot->rootLink()->R * matrix_logEx(m_robot->rootLink()->R.transpose() * target_com_r0));
-      eevel_all.segment( BASE_DOF+eenum*WS_DOF, WS_DOF) << vel_com_p * com_ik_weight , vel_com_r * com_ik_weight;
-
-      J_all.block(0, 0, WS_DOF, WS_DOF ) = hrp::dmatrix::Identity(WS_DOF, WS_DOF);
-
-      int ee_num_count2 = 0;
-      for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-        hrp::dmatrix J_b2ee = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
-        hrp::Vector3 p_b2ee = it->second.manip->endLink()->p - m_robot->rootLink()->p;
-        J_b2ee.block(0, 3, 3, 1) = hrp::Vector3::UnitX().cross(p_b2ee);
-        J_b2ee.block(0, 4, 3, 1) = hrp::Vector3::UnitY().cross(p_b2ee);
-        J_b2ee.block(0, 5, 3, 1) = hrp::Vector3::UnitZ().cross(p_b2ee);
-        J_all.block(BASE_DOF+WS_DOF*ee_num_count2, 0, WS_DOF, WS_DOF ) = J_b2ee;
-        hrp::dmatrix J_ee;
-        it->second.manip->calcJacobian(J_ee);
-        for(int id_in_limb=0; id_in_limb<it->second.manip->numJoints(); id_in_limb++){
-          int id_in_body = it->second.manip->joint(id_in_limb)->jointId; //全身でのjoint番号
-          J_all.block(BASE_DOF+ee_num_count2*WS_DOF, BASE_DOF+id_in_body, 6, 1) = J_ee.col(id_in_limb);
-        }
-        ee_num_count2++;
-      }
-
-      hrp::dmatrix J_com;
-      m_robot->calcCMJacobian(NULL, J_com);
-      J_all.block(BASE_DOF+eenum*WS_DOF, BASE_DOF, 3, m_robot->numJoints()) = J_com;
-
-      hrp::dmatrix J_b2com = hrp::dmatrix::Identity( COM_DOF,  BASE_DOF );
-      hrp::Vector3 p_b2com = m_robot->calcCM() - m_robot->rootLink()->p;
-      J_b2com.block(0, 3, 3, 1) = hrp::Vector3::UnitX().cross(p_b2com);
-      J_b2com.block(0, 4, 3, 1) = hrp::Vector3::UnitY().cross(p_b2com);
-      J_b2com.block(0, 5, 3, 1) = hrp::Vector3::UnitZ().cross(p_b2com);
-      J_all.block(BASE_DOF+eenum*WS_DOF, 0, COM_DOF, BASE_DOF) = J_b2com;
-
-//      //base拘束しない場合
-      if(!USE_BASE_CONSTRAINT){
-        eevel_all = eevel_all.bottomRows(WS_DOF*eenum+COM_DOF);//上6要素削除
-        J_all = J_all.bottomRows(WS_DOF*eenum+COM_DOF);//上6行削除
-      }
-
-      struct timespec startT, endT;
-      clock_gettime(CLOCK_REALTIME, &startT);
-//      hrp::calcPseudoInverse(J_all,J_all_inv,1.0e-3);//超遅い1[ms]とか
-      calcJacobianInverseNullspace_copy(m_robot, J_all, J_all_inv);
-
-      static int count;
-      if(count % 1000 == 0){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ calcPseudoInverse" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
-
-      dq_all = J_all_inv * eevel_all; // dq = pseudoInverse(J) * v
-      if(count % 1000 == 0){
-        std::cout<<"solveIK"<<std::endl;
-        std::cout<<"eevel_all\n"<<eevel_all<<std::endl;
-        std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
-        std::cout<<std::setprecision(2) << "J_all_inv=\n"<<J_all_inv<<std::setprecision(6)<<std::endl;
-        std::cout<<"dq_all\n"<<dq_all<<std::endl;
-      }
-
-      const double LAMBDA = 1.0;
-
-      for(int i=0;i<3;i++){ m_robot->rootLink()->p(i) += dq_all(i) * LAMBDA/* * m_dt*/; }//vel計算する時からm_dt使ってないから使わなくていい
-      m_robot->rootLink()->R *= hrp::rotFromRpy( hrp::Vector3(dq_all(3),dq_all(4),dq_all(5)) * LAMBDA /* * m_dt*/ );
-
-      for(int i=0;i<m_robot->numJoints();i++){
-        m_robot->joint(i)->q += dq_all(BASE_DOF + i) * LAMBDA/* * m_dt*/;
-        LIMIT_MINMAX(m_robot->joint(i)->q, m_robot->joint(i)->llimit, m_robot->joint(i)->ulimit);
-      }
-      m_robot->calcForwardKinematics();
-      count++;
-    }
-
-
-    bool calcJacobianInverseNullspace_copy(hrp::BodyPtr robot, hrp::dmatrix &J, hrp::dmatrix &Jinv) {
-#define deg2rad(x)((x)*M_PI/180)
-#define rad2deg(rad) (rad * 180 / M_PI)
-#define eps_eq(a, b, c)  (fabs((a)-(b)) <= c)
-      const int BASE_DOF = 6;
-      const int ALL_DOF = J.cols();//BASE_DOF + m_robot->numJoints()
-      hrp::dmatrix w = hrp::dmatrix::Identity(ALL_DOF,ALL_DOF);
-      std::vector<double> avoid_weight_gain, optional_weight_vector;
-      avoid_weight_gain.resize(robot->numJoints());
-      optional_weight_vector.resize(robot->numJoints());
-      for(int j=0; j < m_robot->numJoints(); j++){
-        optional_weight_vector[j] = 1.0;
-        avoid_weight_gain[j] = 100000000000000000000.0;
-      }
-      bool use_inside_joint_weight_retrieval = true;
-
-      for ( int j = 0; j < robot->numJoints() ; j++ ) {
-        double jang = robot->joint(j)->q;
-        double jmax = robot->joint(j)->ulimit;
-        double jmin = robot->joint(j)->llimit;
-        double e = deg2rad(1);
-        if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
-        } else if ( eps_eq(jang, jmax,e) ) {
-          jang = jmax - e;
-        } else if ( eps_eq(jang, jmin,e) ) {
-          jang = jmin + e;
-        }
-        double r;
-        if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
-          r = DBL_MAX;
-        } else {
-          r = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) /
-          (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
-          if (isnan(r)) r = 0;
-        }
-        if (( r - avoid_weight_gain[j] ) >= 0 ) {
-          w(BASE_DOF+j, BASE_DOF+j) = optional_weight_vector[j] * ( 1.0 / ( 1.0 + r) );
-        } else {
-          if (use_inside_joint_weight_retrieval)
-          w(BASE_DOF+j, BASE_DOF+j) = optional_weight_vector[j] * 1.0;
-          else
-          w(BASE_DOF+j, BASE_DOF+j) = optional_weight_vector[j] * ( 1.0 / ( 1.0 + r) );
-        }
-        avoid_weight_gain[j] = r;
-      }
-      w.topLeftCorner(BASE_DOF, BASE_DOF) = hrp::dmatrix::Identity(BASE_DOF,BASE_DOF) * 0.1;//baseの重みテスト
-
-      const double manipulability_limit = 0.1;
-      const double sr_gain = 1.0;
-      const double manipulability_gain = 0.001;
-
-      double manipulability = sqrt((J*J.transpose()).determinant());
-      double k = 0;
-      if ( manipulability < manipulability_limit ) {
-        k = manipulability_gain * pow((1 - ( manipulability / manipulability_limit )), 2);
-      }
-      hrp::calcSRInverse(J, Jinv, sr_gain * k, w);
-      //        Jnull = ( hrp::dmatrix::Identity(n, n) - Jinv * J);
-      return true;
-    }
-};
-
-
-#endif //  FULLBODYIK_H
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index f92c0728ba9..b15740faf31 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -222,9 +222,6 @@ void WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC:
       coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
       coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
       coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
-#ifdef USE_NEW_FIK
-      ee_base = "WAIST";//CHESTも使う実験
-#endif
       ABCIKparam tp;
       for (size_t j = 0; j < XYZ; j++) {
           coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
@@ -311,6 +308,9 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
           hsp->m_robot_ml = m_robot_ml;
           hsp->fik_act = fik_vsafe;
           hsp->m_robot_act = m_robot_vsafe;
+          //逆動力学初期化
+          idsb.setInitState(m_robot, m_dt);
+          idsb2.setInitState(m_robot, m_dt);
         }
         calcManipulability(fik_rmc, m_robot_rmc);
 
@@ -322,17 +322,12 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
         if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processWholeBodyMasterSlave" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
-        //逆動力学初期化
-        if(mode.isInitialize()){
-          idsb.setInitState(m_robot, m_dt);
-          idsb2.setInitState(m_robot, m_dt);
-        }
         //逆動力学
-        calcAccelerationsForInverseDynamics(m_robot, idsb);
-        hrp::Vector3 ref_zmp_invdyn;
-        calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);
-        ref_zmp_invdyn = invdyn_zmp_filters.passFilter(ref_zmp_invdyn);
-        updateInvDynStateBuffer(idsb);
+//        calcAccelerationsForInverseDynamics(m_robot, idsb);
+//        hrp::Vector3 ref_zmp_invdyn;
+//        calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);
+//        ref_zmp_invdyn = invdyn_zmp_filters.passFilter(ref_zmp_invdyn);
+//        updateInvDynStateBuffer(idsb);
 
         if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ calcWorldZMPFromInverseDynamics" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
@@ -585,6 +580,74 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     }
   }
 
+  fik_in->ik_tgt_list.clear();
+  IKConstraintParam tmp;
+
+  tmp.target_link_name = "WAIST";
+  tmp.localPos = hrp::Vector3::Zero();
+  tmp.localR = hrp::Matrix33::Identity();
+  tmp.target_p0 = hrp::Vector3::Zero();// will be ignored by selection_vec
+  tmp.target_r0 = hrp::rotFromRpy(com_ref.rpy);
+  tmp.selection_vec << 0,0,0,1,1,1;
+  tmp.weight_vec << 1,1,1,1,1,1;
+  fik_in->ik_tgt_list.push_back(tmp);
+
+  tmp.target_link_name = "RLEG_JOINT5";
+  tmp.localPos = fik_in->ikp["rleg"].localPos;
+  tmp.localR = fik_in->ikp["rleg"].localR;
+  tmp.target_p0 = rf_ref.p;
+  tmp.target_r0 = hrp::rotFromRpy(rf_ref.rpy);
+  tmp.selection_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 1,1,3,1,1,1;
+  fik_in->ik_tgt_list.push_back(tmp);
+
+  tmp.target_link_name = "LLEG_JOINT5";
+  tmp.localPos = fik_in->ikp["lleg"].localPos;
+  tmp.localR = fik_in->ikp["lleg"].localR;
+  tmp.target_p0 = lf_ref.p;
+  tmp.target_r0 = hrp::rotFromRpy(lf_ref.rpy);
+  tmp.selection_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 1,1,3,1,1,1;
+  fik_in->ik_tgt_list.push_back(tmp);
+
+  tmp.target_link_name = "RARM_JOINT7";
+  tmp.localPos = fik_in->ikp["rarm"].localPos;
+  tmp.localR = fik_in->ikp["rarm"].localR;
+  tmp.target_p0 = rh_ref.p;
+  tmp.target_r0 = hrp::rotFromRpy(rh_ref.rpy);
+  tmp.selection_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 1,1,1,1,1,1;
+  fik_in->ik_tgt_list.push_back(tmp);
+
+  tmp.target_link_name = "LARM_JOINT7";
+  tmp.localPos = fik_in->ikp["larm"].localPos;
+  tmp.localR = fik_in->ikp["larm"].localR;
+  tmp.target_p0 = lh_ref.p;
+  tmp.target_r0 = hrp::rotFromRpy(lh_ref.rpy);
+  tmp.selection_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 1,1,1,1,1,1;
+  fik_in->ik_tgt_list.push_back(tmp);
+
+  tmp.target_link_name = "COM";
+  tmp.localPos = hrp::Vector3::Zero();
+  tmp.localR = hrp::Matrix33::Identity();
+  tmp.target_p0 = com_ref.p;// COM height will not be constraint
+  tmp.target_r0 = hrp::Matrix33::Identity();//reference angular momentum
+//  tmp.selection_vec << 1,1,0,1,1,1;
+  tmp.selection_vec << 1,1,0,1,1,0;
+  tmp.weight_vec << 3,3,1,1,1,1;
+  fik_in->ik_tgt_list.push_back(tmp);
+
+  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;
+  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
+  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
+  fik_in->optional_weight_vector.tail(6) = hrp::dvector6::Ones() * 0.01;
+
+  hrp::Vector3 base_p_old = robot_in->rootLink()->p;
+  hrp::Matrix33 base_R_old = robot_in->rootLink()->R;
+  hrp::dvector q_old(robot_in->numJoints());
+  fik_in->cur_P = fik_in->cur_L = hrp::Vector3::Zero();
+
   //確かに毎回少しreset poseに引き戻すとこじれないが・・・
   hrp::dvector reset_pose(33);
   reset_pose <<
@@ -596,16 +659,27 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   for(int i=0;i<33;i++){
     robot_in->joint(i)->q = robot_in->joint(i)->q * 0.999 + reset_pose(i) * 0.001;
   }
-
+  robot_in->rootLink()->p = robot_in->rootLink()->p * 0.999 + hrp::Vector3(0,0,1.1) * 0.001;
+  robot_in->rootLink()->R = robot_in->rootLink()->R * 0.999 + hrp::Matrix33::Identity() * 0.001;
 
   if( robot_in->link("HEAD_JOINT0") != NULL) robot_in->link("HEAD_JOINT0")->q = head_ref.rpy(y);
   if( robot_in->link("HEAD_JOINT1") != NULL) robot_in->link("HEAD_JOINT1")->q = head_ref.rpy(p);
   struct timespec startT, endT;
   clock_gettime(CLOCK_REALTIME, &startT);
+  for(int i=0;i<robot_in->numJoints();i++){ q_old(i) = robot_in->joint(i)->q; }
   while( 1 ){  //COM 収束ループ
     com_ik_loop++;
-    robot_in->calcForwardKinematics();
-    fik_in->solveIK();
+
+//    robot_in->calcForwardKinematics();
+    fik_in->solveFullbodyIK(hrp::Vector3::Zero(), false);
+
+    //check ang moment
+    robot_in->rootLink()->v = (robot_in->rootLink()->p - base_p_old)/ m_dt;
+    robot_in->rootLink()->w = base_R_old * fik_in->omegaFromRotEx(base_R_old.transpose() * robot_in->rootLink()->R) / m_dt;
+    for(int i=0;i<m_robot->numJoints();i++){ robot_in->joint(i)->dq = (robot_in->joint(i)->q - q_old(i)) / m_dt; }
+    robot_in->calcForwardKinematics(true,false);
+    robot_in->calcTotalMomentum(fik_in->cur_P, fik_in->cur_L);
+
     hrp::Vector3 com_p_err = com_ref.p - robot_in->calcCM();
     hrp::Vector3 rf_p_err = rf_ref.p - fik_in->getEndEffectorPos("rleg");
     hrp::Vector3 lf_p_err = lf_ref.p - fik_in->getEndEffectorPos("lleg");
@@ -617,7 +691,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         lf_p_err.norm() < 1e-3 &&
         rh_p_err.norm() < 1e-2 &&
         lh_p_err.norm() < 1e-2){ break; }
-    if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
+    if(com_ik_loop >= COM_IK_MAX_LOOP){if(DEBUGP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl;} break; };
   }
   if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveIK loop" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
@@ -717,7 +791,8 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
 
   double goal_time = 0.0;
   const double min_goal_time_offset = 0.1;
-  const double avg_q_vel = 1.0;
+//  const double avg_q_vel = 1.0;
+  const double avg_q_vel = 2.0;
   for(int i=0;i<robot_in->numJoints();i++){
     double tmp_time = (robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
     if(tmp_time > goal_time){ goal_time = tmp_time; }
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 7906e134750..0b37a25a2f1 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -36,7 +36,7 @@ extern "C" {
 #endif
 
 //#include "../AutoBalancer/AutoBalancer.h"
-#include "FullBodyIK.h"
+#include "../AutoBalancer/FullbodyInverseKinematicsSolver.h"
 #define USE_NEW_FIK
 
 #define DEBUG 0

From 598ecc0964329300907652376ea6a81f4393e029 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 17 Aug 2017 06:43:53 +0900
Subject: [PATCH 113/217] checked in the abc goPos

---
 rtc/AutoBalancer/AutoBalancer.cpp             | 147 +++++++++++++++---
 rtc/AutoBalancer/AutoBalancer.h               |   3 +-
 .../FullbodyInverseKinematicsSolver.h         |   3 +-
 3 files changed, 129 insertions(+), 24 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 75959847573..83c797a3669 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -173,7 +173,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     leg_names.push_back("lleg");
 
     // Generate FIK
-    fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
+    fik = fikPtr(new FullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
 
     // setting from conf file
     // rleg,TARGET_LINK,BASE_LINK
@@ -197,7 +197,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
         }
         tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
         // FIK param
-        SimpleFullbodyInverseKinematicsSolver::IKparam tmp_fikp;
+        FullbodyInverseKinematicsSolver::IKparam tmp_fikp;
         tmp_fikp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
         tmp_fikp.target_link = m_robot->link(ee_target);
         tmp_fikp.localPos = tp.localPos;
@@ -208,7 +208,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
           tmp_fikp.parent_name = root->name;
           root = root->parent;
         }
-        fik->ikp.insert(std::pair<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>(ee_name, tmp_fikp));
+        fik->ikp.insert(std::pair<std::string, FullbodyInverseKinematicsSolver::IKparam>(ee_name, tmp_fikp));
         // Fix for toe joint
         //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
         //   Without toe joints, "end-effector link == force-sensor link" is assumed.
@@ -1073,28 +1073,131 @@ void AutoBalancer::fixLegToCoords2 (coordinates& tmp_fix_coords)
     fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot);
 }
 
+//void AutoBalancer::solveFullbodyIK ()
+//{
+//  // Set ik target params
+//  fik->target_root_p = target_root_p;
+//  fik->target_root_R = target_root_R;
+//  for ( std::map<std::string, FullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
+//      it->second.target_p0 = ikp[it->first].target_p0;
+//      it->second.target_r0 = ikp[it->first].target_r0;
+//  }
+//  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
+////  fik->current_tm = m_qRef.tm;
+//  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+//      fik->ikp[it->first].is_ik_enable = it->second.is_active;
+//  }
+//  // Revert
+//  fik->revertRobotStateToCurrent();
+//  // TODO : SBP calculation is outside of solve ik?
+//  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
+//  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
+//  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
+//  // Solve IK
+//  fik->solveFullbodyIK (dif_cog, transition_interpolator->isEmpty());
+//}
+
 void AutoBalancer::solveFullbodyIK ()
 {
-  // Set ik target params
-  fik->target_root_p = target_root_p;
-  fik->target_root_R = target_root_R;
-  for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-      it->second.target_p0 = ikp[it->first].target_p0;
-      it->second.target_r0 = ikp[it->first].target_r0;
-  }
-  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
-//  fik->current_tm = m_qRef.tm;
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-      fik->ikp[it->first].is_ik_enable = it->second.is_active;
-  }
+  const int IK_MAX_LOOP = 3;
+  fik->ik_tgt_list.clear();
+  IKConstraintParam tmp;
+
+  tmp.target_link_name = "WAIST";
+  tmp.localPos = hrp::Vector3::Zero();
+  tmp.localR = hrp::Matrix33::Identity();
+  tmp.target_p0 = target_root_p;// will be ignored by selection_vec
+  tmp.target_r0 = target_root_R;
+  tmp.selection_vec << 0,0,0,1,1,1;
+  tmp.weight_vec << 1,1,1,1,1,1;
+  fik->ik_tgt_list.push_back(tmp);
+
+  tmp.target_link_name = "RLEG_JOINT5";
+  tmp.localPos = ikp["rleg"].localPos;
+  tmp.localR = ikp["rleg"].localR;
+  tmp.target_p0 = ikp["rleg"].target_p0;
+  tmp.target_r0 = ikp["rleg"].target_r0;
+  tmp.selection_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 1,1,3,1,1,1;
+  fik->ik_tgt_list.push_back(tmp);
+
+  tmp.target_link_name = "LLEG_JOINT5";
+  tmp.localPos = ikp["lleg"].localPos;
+  tmp.localR = ikp["lleg"].localR;
+  tmp.target_p0 = ikp["lleg"].target_p0;
+  tmp.target_r0 = ikp["lleg"].target_r0;
+  tmp.selection_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 1,1,3,1,1,1;
+  fik->ik_tgt_list.push_back(tmp);
+
+  tmp.target_link_name = "RARM_JOINT7";
+  tmp.localPos = ikp["rarm"].localPos;
+  tmp.localR = ikp["rarm"].localR;
+  tmp.target_p0 = ikp["rarm"].target_p0;
+  tmp.target_r0 = ikp["rarm"].target_r0;
+  tmp.selection_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 1,1,1,1,1,1;
+  fik->ik_tgt_list.push_back(tmp);
+
+  tmp.target_link_name = "LARM_JOINT7";
+  tmp.localPos = ikp["larm"].localPos;
+  tmp.localR = ikp["larm"].localR;
+  tmp.target_p0 = ikp["larm"].target_p0;
+  tmp.target_r0 = ikp["larm"].target_r0;
+  tmp.selection_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 1,1,1,1,1,1;
+  fik->ik_tgt_list.push_back(tmp);
+
+  tmp.target_link_name = "COM";
+  tmp.localPos = hrp::Vector3::Zero();
+  tmp.localR = hrp::Matrix33::Identity();
+  tmp.target_p0 = ref_cog;// COM height will not be constraint
+  tmp.target_r0 = hrp::Matrix33::Identity();//reference angular momentum
+  tmp.selection_vec << 1,1,1,1,1,0; // COM pos + Ang Momentum
+//  tmp.selection_vec << 1,1,0,0,0,0;
+  tmp.weight_vec << 3,3,1,1,1,1;
+  fik->ik_tgt_list.push_back(tmp);
+
+  fik->optional_weight_vector(m_robot->link("CHEST_JOINT0")->jointId) = 0.1;
+  fik->optional_weight_vector(m_robot->link("CHEST_JOINT1")->jointId) = 0.1;
+  fik->optional_weight_vector(m_robot->link("CHEST_JOINT2")->jointId) = 0.1;
+  fik->optional_weight_vector.tail(6) = hrp::dvector6::Ones() * 0.01;// 1 rad vs 1 m は不公平だから・・・
+
+  hrp::Vector3 base_p_old = m_robot->rootLink()->p;
+  hrp::Matrix33 base_R_old = m_robot->rootLink()->R;
+  hrp::dvector q_old(m_robot->numJoints());
+  fik->cur_P = fik->cur_L = hrp::Vector3::Zero();
+  for(int i=0;i<m_robot->numJoints();i++){ q_old(i) = m_robot->joint(i)->q; }
+
+  for(int l=0;l<IK_MAX_LOOP;l++){
+    fik->solveFullbodyIK(hrp::Vector3::Zero(), false);
+    //check ang moment
+    m_robot->rootLink()->v = (m_robot->rootLink()->p - base_p_old)/ m_dt;
+    m_robot->rootLink()->w = base_R_old * fik->omegaFromRotEx(base_R_old.transpose() * m_robot->rootLink()->R) / m_dt;
+    for(int i=0;i<m_robot->numJoints();i++){ m_robot->joint(i)->dq = (m_robot->joint(i)->q - q_old(i)) / m_dt; }
+    m_robot->calcForwardKinematics(true,false);
+    m_robot->calcTotalMomentum(fik->cur_P, fik->cur_L);
+
+    hrp::Vector3 com_p_err = ref_cog - m_robot->calcCM();
+    hrp::Vector3 rf_p_err = ikp["rleg"].target_p0 - fik->getEndEffectorPos("rleg");
+    hrp::Vector3 lf_p_err = ikp["lleg"].target_p0 - fik->getEndEffectorPos("lleg");
+    hrp::Vector3 rh_p_err = ikp["rarm"].target_p0 - fik->getEndEffectorPos("rarm");
+    hrp::Vector3 lh_p_err = ikp["larm"].target_p0 - fik->getEndEffectorPos("larm");
+    if(
+        com_p_err.norm() < 1e-3 &&
+        rf_p_err.norm() < 1e-3 &&
+        lf_p_err.norm() < 1e-3 &&
+        rh_p_err.norm() < 1e-2 &&
+        lh_p_err.norm() < 1e-2){ break; }
+    l++;
+    if(l >= IK_MAX_LOOP){if(DEBUGP){std::cerr << "COM constraint IK MAX loop [="<<IK_MAX_LOOP<<"] exceeded!! "<< std::endl;} break; };
+  }
+//  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
+//  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+//      fik->ikp[it->first].is_ik_enable = it->second.is_active;
+//  }
   // Revert
-  fik->revertRobotStateToCurrent();
-  // TODO : SBP calculation is outside of solve ik?
-  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
-  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
-  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
-  // Solve IK
-  fik->solveFullbodyIK (dif_cog, transition_interpolator->isEmpty());
+//  fik->revertRobotStateToCurrent();
 }
 
 
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index b32a8db134f..ba6476bf03c 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -29,6 +29,7 @@
 #include "interpolator.h"
 #include "../TorqueFilter/IIRFilter.h"
 #include "SimpleFullbodyInverseKinematicsSolver.h"
+#include "FullbodyInverseKinematicsSolver.h"
 
 // </rtc-template>
 
@@ -246,7 +247,7 @@ class AutoBalancer
   ggPtr gg;
   bool gg_is_walking, gg_solved;
   // for abc
-  typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
+  typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
   fikPtr fik;
   hrp::Vector3 ref_cog, ref_zmp, prev_ref_zmp, prev_imu_sensor_pos, prev_imu_sensor_vel, hand_fix_initial_offset;
   enum {BIPED, TROT, PACE, CRAWL, GALLOP} gait_type;
diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 3f821d74bfb..089ea18a674 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -1,6 +1,7 @@
 #ifndef FULLBODYIK_H
 #define FULLBODYIK_H
 
+#include <float.h>
 #include <hrpModel/Body.h>
 #include <hrpUtil/MatrixSolvers.h>
 #include "../ImpedanceController/JointPathEx.h"
@@ -129,7 +130,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
       static int count;
 
       dq_all = J_all_inv * dp_ee_all; // dq = pseudoInverse(J) * v
-      if(count % 1000 == 0){
+      if(count % 2000 == 0){
         std::cout<<"solveIK"<<std::endl;
         std::cout<<"eevel_all\n"<<dp_ee_all<<std::endl;
         std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;

From be1cd31dca4cab4b00584d30fce3a4e8370746ea Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 20 Aug 2017 21:44:15 +0900
Subject: [PATCH 114/217] start debug

---
 rtc/AutoBalancer/AutoBalancer.cpp             |  94 ++---
 rtc/AutoBalancer/AutoBalancer.h               |   5 +-
 .../FullbodyInverseKinematicsSolver.h         | 337 +++++++++++++-----
 .../WholeBodyMasterSlave.cpp                  | 259 +++++---------
 .../WholeBodyMasterSlave.h                    |   7 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          |  47 +--
 6 files changed, 400 insertions(+), 349 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 83c797a3669..00067a22af6 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -173,7 +173,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     leg_names.push_back("lleg");
 
     // Generate FIK
-    fik = fikPtr(new FullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
+    fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
 
     // setting from conf file
     // rleg,TARGET_LINK,BASE_LINK
@@ -197,7 +197,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
         }
         tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
         // FIK param
-        FullbodyInverseKinematicsSolver::IKparam tmp_fikp;
+        SimpleFullbodyInverseKinematicsSolver::IKparam tmp_fikp;
         tmp_fikp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
         tmp_fikp.target_link = m_robot->link(ee_target);
         tmp_fikp.localPos = tp.localPos;
@@ -208,7 +208,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
           tmp_fikp.parent_name = root->name;
           root = root->parent;
         }
-        fik->ikp.insert(std::pair<std::string, FullbodyInverseKinematicsSolver::IKparam>(ee_name, tmp_fikp));
+        fik->ikp.insert(std::pair<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>(ee_name, tmp_fikp));
         // Fix for toe joint
         //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
         //   Without toe joints, "end-effector link == force-sensor link" is assumed.
@@ -1073,30 +1073,8 @@ void AutoBalancer::fixLegToCoords2 (coordinates& tmp_fix_coords)
     fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot);
 }
 
-//void AutoBalancer::solveFullbodyIK ()
-//{
-//  // Set ik target params
-//  fik->target_root_p = target_root_p;
-//  fik->target_root_R = target_root_R;
-//  for ( std::map<std::string, FullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-//      it->second.target_p0 = ikp[it->first].target_p0;
-//      it->second.target_r0 = ikp[it->first].target_r0;
-//  }
-//  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
-////  fik->current_tm = m_qRef.tm;
-//  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-//      fik->ikp[it->first].is_ik_enable = it->second.is_active;
-//  }
-//  // Revert
-//  fik->revertRobotStateToCurrent();
-//  // TODO : SBP calculation is outside of solve ik?
-//  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
-//  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
-//  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
-//  // Solve IK
-//  fik->solveFullbodyIK (dif_cog, transition_interpolator->isEmpty());
-//}
-
+//#define USE_NEW_IK
+#ifdef USE_NEW_IK
 void AutoBalancer::solveFullbodyIK ()
 {
   const int IK_MAX_LOOP = 3;
@@ -1106,17 +1084,17 @@ void AutoBalancer::solveFullbodyIK ()
   tmp.target_link_name = "WAIST";
   tmp.localPos = hrp::Vector3::Zero();
   tmp.localR = hrp::Matrix33::Identity();
-  tmp.target_p0 = target_root_p;// will be ignored by selection_vec
-  tmp.target_r0 = target_root_R;
-  tmp.selection_vec << 0,0,0,1,1,1;
+  tmp.target_p = target_root_p;// will be ignored by selection_vec
+  tmp.target_r = target_root_R;
+  tmp.selection_vec << 0,0,0,1,1,0;
   tmp.weight_vec << 1,1,1,1,1,1;
   fik->ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = "RLEG_JOINT5";
   tmp.localPos = ikp["rleg"].localPos;
   tmp.localR = ikp["rleg"].localR;
-  tmp.target_p0 = ikp["rleg"].target_p0;
-  tmp.target_r0 = ikp["rleg"].target_r0;
+  tmp.target_p = ikp["rleg"].target_p0;
+  tmp.target_r = ikp["rleg"].target_r0;
   tmp.selection_vec << 1,1,1,1,1,1;
   tmp.weight_vec << 1,1,3,1,1,1;
   fik->ik_tgt_list.push_back(tmp);
@@ -1124,8 +1102,8 @@ void AutoBalancer::solveFullbodyIK ()
   tmp.target_link_name = "LLEG_JOINT5";
   tmp.localPos = ikp["lleg"].localPos;
   tmp.localR = ikp["lleg"].localR;
-  tmp.target_p0 = ikp["lleg"].target_p0;
-  tmp.target_r0 = ikp["lleg"].target_r0;
+  tmp.target_p = ikp["lleg"].target_p0;
+  tmp.target_r = ikp["lleg"].target_r0;
   tmp.selection_vec << 1,1,1,1,1,1;
   tmp.weight_vec << 1,1,3,1,1,1;
   fik->ik_tgt_list.push_back(tmp);
@@ -1133,8 +1111,8 @@ void AutoBalancer::solveFullbodyIK ()
   tmp.target_link_name = "RARM_JOINT7";
   tmp.localPos = ikp["rarm"].localPos;
   tmp.localR = ikp["rarm"].localR;
-  tmp.target_p0 = ikp["rarm"].target_p0;
-  tmp.target_r0 = ikp["rarm"].target_r0;
+  tmp.target_p = ikp["rarm"].target_p0;
+  tmp.target_r = ikp["rarm"].target_r0;
   tmp.selection_vec << 1,1,1,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
   fik->ik_tgt_list.push_back(tmp);
@@ -1142,8 +1120,8 @@ void AutoBalancer::solveFullbodyIK ()
   tmp.target_link_name = "LARM_JOINT7";
   tmp.localPos = ikp["larm"].localPos;
   tmp.localR = ikp["larm"].localR;
-  tmp.target_p0 = ikp["larm"].target_p0;
-  tmp.target_r0 = ikp["larm"].target_r0;
+  tmp.target_p = ikp["larm"].target_p0;
+  tmp.target_r = ikp["larm"].target_r0;
   tmp.selection_vec << 1,1,1,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
   fik->ik_tgt_list.push_back(tmp);
@@ -1151,17 +1129,16 @@ void AutoBalancer::solveFullbodyIK ()
   tmp.target_link_name = "COM";
   tmp.localPos = hrp::Vector3::Zero();
   tmp.localR = hrp::Matrix33::Identity();
-  tmp.target_p0 = ref_cog;// COM height will not be constraint
-  tmp.target_r0 = hrp::Matrix33::Identity();//reference angular momentum
-  tmp.selection_vec << 1,1,1,1,1,0; // COM pos + Ang Momentum
-//  tmp.selection_vec << 1,1,0,0,0,0;
-  tmp.weight_vec << 3,3,1,1,1,1;
+  tmp.target_p = ref_cog;// COM height will not be constraint
+  tmp.target_r = hrp::Matrix33::Identity();//reference angular momentum
+  tmp.selection_vec << 1,1,1,1,1,1; // COM pos + Ang Momentum
+  tmp.weight_vec << 3,3,1,0.1,0.1,0.1;
   fik->ik_tgt_list.push_back(tmp);
 
   fik->optional_weight_vector(m_robot->link("CHEST_JOINT0")->jointId) = 0.1;
   fik->optional_weight_vector(m_robot->link("CHEST_JOINT1")->jointId) = 0.1;
   fik->optional_weight_vector(m_robot->link("CHEST_JOINT2")->jointId) = 0.1;
-  fik->optional_weight_vector.tail(6) = hrp::dvector6::Ones() * 0.01;// 1 rad vs 1 m は不公平だから・・・
+//  fik->optional_weight_vector.tail(6) = hrp::dvector6::Ones() * 0.1;// 1 rad : 0.1 m くらいに調整
 
   hrp::Vector3 base_p_old = m_robot->rootLink()->p;
   hrp::Matrix33 base_R_old = m_robot->rootLink()->R;
@@ -1189,8 +1166,7 @@ void AutoBalancer::solveFullbodyIK ()
         lf_p_err.norm() < 1e-3 &&
         rh_p_err.norm() < 1e-2 &&
         lh_p_err.norm() < 1e-2){ break; }
-    l++;
-    if(l >= IK_MAX_LOOP){if(DEBUGP){std::cerr << "COM constraint IK MAX loop [="<<IK_MAX_LOOP<<"] exceeded!! "<< std::endl;} break; };
+    if(l == IK_MAX_LOOP-1){if(loop%100 == 0){std::cerr << "COM constraint IK MAX loop [="<<IK_MAX_LOOP<<"] exceeded!! "<< std::endl;} break; };
   }
 //  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
 //  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
@@ -1199,6 +1175,32 @@ void AutoBalancer::solveFullbodyIK ()
   // Revert
 //  fik->revertRobotStateToCurrent();
 }
+#else
+void AutoBalancer::solveFullbodyIK ()
+{
+  // Set ik target params
+  fik->target_root_p = target_root_p;
+  fik->target_root_R = target_root_R;
+  for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
+      it->second.target_p0 = ikp[it->first].target_p0;
+      it->second.target_r0 = ikp[it->first].target_r0;
+  }
+  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
+//  fik->current_tm = m_qRef.tm;
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+      fik->ikp[it->first].is_ik_enable = it->second.is_active;
+  }
+  // Revert
+  fik->revertRobotStateToCurrent();
+  // TODO : SBP calculation is outside of solve ik?
+  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
+  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
+  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
+  // Solve IK
+  fik->solveFullbodyIK (dif_cog, transition_interpolator->isEmpty());
+}
+#endif
+
 
 
 /*
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index ba6476bf03c..2b927780fbc 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -29,7 +29,7 @@
 #include "interpolator.h"
 #include "../TorqueFilter/IIRFilter.h"
 #include "SimpleFullbodyInverseKinematicsSolver.h"
-#include "FullbodyInverseKinematicsSolver.h"
+//#include "FullbodyInverseKinematicsSolver.h"
 
 // </rtc-template>
 
@@ -247,7 +247,8 @@ class AutoBalancer
   ggPtr gg;
   bool gg_is_walking, gg_solved;
   // for abc
-  typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
+//  typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
+  typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
   fikPtr fik;
   hrp::Vector3 ref_cog, ref_zmp, prev_ref_zmp, prev_imu_sensor_pos, prev_imu_sensor_vel, hand_fix_initial_offset;
   enum {BIPED, TROT, PACE, CRAWL, GALLOP} gait_type;
diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 089ea18a674..a68559aaad0 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -6,118 +6,159 @@
 #include <hrpUtil/MatrixSolvers.h>
 #include "../ImpedanceController/JointPathEx.h"
 #include "SimpleFullbodyInverseKinematicsSolver.h"
+#include <unsupported/Eigen/MatrixFunctions>
 
 #define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
 #define deg2rad(x)((x)*M_PI/180)
 #define rad2deg(rad) (rad * 180 / M_PI)
 #define eps_eq(a, b, c)  (fabs((a)-(b)) <= c)
 
-
 class IKConstraintParam {
   public:
     std::string target_link_name;
     // IK target EE coords
-    hrp::Vector3 target_p0;
-    hrp::Matrix33 target_r0;
+    hrp::Vector3 target_p;
+    hrp::Matrix33 target_r;
     // EE offset, EE link
     hrp::Vector3 localPos;
     hrp::Matrix33 localR;
     hrp::dvector6 weight_vec;
     hrp::ivector selection_vec;
+    double pos_precision, rot_precision;
 
     IKConstraintParam ()
-      :target_p0(hrp::Vector3::Zero()),
-       target_r0(hrp::Matrix33::Identity()),
+      :target_p(hrp::Vector3::Zero()),
+       target_r(hrp::Matrix33::Identity()),
        localPos(hrp::Vector3::Zero()),
        localR(hrp::Matrix33::Identity()),
        weight_vec(hrp::dvector6::Ones()),
-       selection_vec(hrp::ivector::Ones(6)){};
+       selection_vec(hrp::ivector::Ones(6)),
+       pos_precision(1e-3), rot_precision(1e-2)
+    {};
 };
 
 
 class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSolver{
   protected:
-    hrp::dmatrix J_all, J_all_inv;
-    hrp::dvector dq_all, dp_ee_all;
-    hrp::dmatrix J_jpath;
-    hrp::dmatrix J_com, J_am;
+    hrp::dmatrix J_all, J_all_inv, J_com, J_am;
+    hrp::dvector dq_all, dp_ee_all, weight_vec_all;
+    hrp::ivector selection_vec_all;
+    int WS_DOF, BASE_DOF, J_DOF, ALL_DOF;
+    hrp::Vector3 cur_P,cur_L;
 
   public:
-    std::vector<IKConstraintParam> ik_tgt_list;
-    hrp::Vector3 target_com_p0;
-    hrp::Matrix33 target_com_r0;
     hrp::dvector optional_weight_vector;
+    hrp::dvector reference_q, reference_gain;
     FullbodyInverseKinematicsSolver (hrp::BodyPtr& _robot, const std::string& _print_str, const double _dt) : SimpleFullbodyInverseKinematicsSolver(_robot,_print_str, _dt) {
-        qorg.resize(m_robot->numJoints());
-        qrefv.resize(m_robot->numJoints());
-        optional_weight_vector = hrp::dvector::Ones(m_robot->numJoints()+6);
+      WS_DOF = 6;
+      BASE_DOF = 6;
+      J_DOF = m_robot->numJoints();
+      ALL_DOF = BASE_DOF + J_DOF;
+      optional_weight_vector = hrp::dvector::Ones(ALL_DOF);
+      reference_q = reference_gain = hrp::dvector::Zero(ALL_DOF);
+      dq_all = hrp::dvector::Zero( ALL_DOF );
     };
-    hrp::Vector3 cur_P,cur_L;
     ~FullbodyInverseKinematicsSolver () {};
     // Solve fullbody IK using limb IK
-    void solveFullbodyIK ( const hrp::Vector3& _dif_cog, const bool is_transition) {
-
-      solveIK();
-
+    int solveFullbodyIKLoop (const std::vector<IKConstraintParam>& _ik_tgt_list, const int _max_iteration) {
+      hrp::Vector3 base_p_old = m_robot->rootLink()->p;
+      hrp::Matrix33 base_R_old = m_robot->rootLink()->R;
+      hrp::dvector q_old(J_DOF);
+      for(int i=0;i<J_DOF;i++){ q_old(i) = m_robot->joint(i)->q; }
+      int loop;
+      for(loop=0; loop < _max_iteration;){
+        solveFullbodyIK(m_robot, _ik_tgt_list);
+        //check ang moment
+        m_robot->rootLink()->v = (m_robot->rootLink()->p - base_p_old)/ m_dt;
+        m_robot->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * m_robot->rootLink()->R) / m_dt;
+        for(int i=0;i<J_DOF;i++){ m_robot->joint(i)->dq = (m_robot->joint(i)->q - q_old(i)) / m_dt; }
+        m_robot->calcForwardKinematics(true,false);
+        m_robot->calcTotalMomentum(cur_P, cur_L);
+        loop++;
+        if(checkIKConvergence(_ik_tgt_list)){ break; }
+      }
+      return loop;
     }
-    void solveIK (){
-      const int BASE_DOF = 6;
-      const int WS_DOF = 6;
-      const int ALL_DOF = BASE_DOF + m_robot->numJoints();
-
-      m_robot->calcForwardKinematics();
-      J_all = hrp::dmatrix::Zero(WS_DOF*ik_tgt_list.size(), ALL_DOF);
-      dq_all = hrp::dvector::Zero( ALL_DOF );
-      dp_ee_all = hrp::dvector::Zero(WS_DOF*ik_tgt_list.size());
-      hrp::ivector selection_vec_all = hrp::ivector::Zero(WS_DOF*ik_tgt_list.size());
-      hrp::dvector weight_vec_all = hrp::dvector::Ones(WS_DOF*ik_tgt_list.size());
-
-      for ( int i=0; i<ik_tgt_list.size(); i++ ) {
-        hrp::Link* target_link_ptr = m_robot->link(ik_tgt_list[i].target_link_name);
-        hrp::Matrix33 ref_link_origin_R(ik_tgt_list[i].target_r0 * ik_tgt_list[i].localR.transpose());
-        hrp::Vector3 ref_link_origin_p(ik_tgt_list[i].target_p0 - ref_link_origin_R * ik_tgt_list[i].localPos);
-        hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF);
-
+    void solveFullbodyIK (const hrp::Vector3& _dif_cog, const bool _is_transition){
+      SimpleFullbodyInverseKinematicsSolver::solveFullbodyIK (_dif_cog, _is_transition);
+    }
+    bool checkIKConvergence(const std::vector<IKConstraintParam>& _ik_tgt_list){
+      for ( int i=0; i<_ik_tgt_list.size(); i++ ) {
+        hrp::Link* target_link_ptr = m_robot->link(_ik_tgt_list[i].target_link_name);
         if(target_link_ptr){
-          hrp::Vector3 vel_p_ref(ref_link_origin_p - target_link_ptr->p);
-          hrp::Vector3 vel_r_ref(target_link_ptr->R * omegaFromRotEx(target_link_ptr->R.transpose() * ref_link_origin_R));
-          dp_ee_all.segment(WS_DOF*i,WS_DOF) << vel_p_ref , vel_r_ref;
+          hrp::Vector3 pos_err = _ik_tgt_list[i].target_p - (target_link_ptr->p + target_link_ptr->R * _ik_tgt_list[i].localPos);
+          hrp::Vector3 rot_err = omegaFromRotEx(_ik_tgt_list[i].target_r.transpose() * target_link_ptr->R * _ik_tgt_list[i].localR);
+          for(int j=0;j<3;j++){
+            if(pos_err(j) > _ik_tgt_list[i].pos_precision && _ik_tgt_list[i].selection_vec(j) != 0){return false;}
+            if(rot_err(j) > _ik_tgt_list[i].rot_precision && _ik_tgt_list[i].selection_vec(j+3) != 0){return false;}
+          }
+        }
+        else if(!target_link_ptr && _ik_tgt_list[i].target_link_name == "COM"){  // COM
+          hrp::Vector3 pos_err = _ik_tgt_list[i].target_p - (m_robot->calcCM() + _ik_tgt_list[i].localR * _ik_tgt_list[i].localPos);
+//            robot_in->calcTotalMomentum(fik_in->cur_P, fik_in->cur_L);
+//            hrp::Vector3 rot_err = omegaFromRotEx(ik_tgt_list[i].target_r.transpose() * ik_tgt_list[i].localR * target_link_ptr->R);
+          for(int j=0;j<3;j++){
+            if(pos_err(j) > _ik_tgt_list[i].pos_precision && _ik_tgt_list[i].selection_vec(j) != 0){return false;}
+//              if(rot_err(j) > ik_tgt_list[i].rot_precision && ik_tgt_list[i].selection_vec(j+3) != 0){return false;}
+          }
+        }
+      }
+      return true;
+    }
+    void solveFullbodyIK(hrp::BodyPtr& _robot, const std::vector<IKConstraintParam>& _ik_tgt_list){
+      J_all = hrp::dmatrix::Zero(WS_DOF*_ik_tgt_list.size(), ALL_DOF);
+      dq_all.fill(0);
+      dp_ee_all = hrp::dvector::Zero(WS_DOF*_ik_tgt_list.size());
+      selection_vec_all = hrp::ivector::Zero(WS_DOF*_ik_tgt_list.size());
+      weight_vec_all = hrp::dvector::Ones(WS_DOF*_ik_tgt_list.size());
+      //リファレンスに微少量戻す
+      for(int i=0;i<J_DOF;i++){ _robot->joint(i)->q = _robot->joint(i)->q * (1-reference_gain(i)) + reference_q(i) * reference_gain(i); }
+      _robot->rootLink()->p = _robot->rootLink()->p.cwiseProduct(hrp::Vector3::Ones()-reference_gain.segment(J_DOF,3)) + reference_q.segment(J_DOF,3).cwiseProduct(reference_gain.segment(J_DOF,3));
+      _robot->rootLink()->R = hrp::rotFromRpy( hrp::rpyFromRot(_robot->rootLink()->R).cwiseProduct(hrp::Vector3::Ones()-reference_gain.segment(J_DOF+3,3)) + reference_q.segment(J_DOF+3,3).cwiseProduct(reference_gain.segment(J_DOF+3,3)));
+      _robot->calcForwardKinematics();
+      //ヤコビアンと各ベクトル生成
+      for ( int i=0; i<_ik_tgt_list.size(); i++ ) {
+        hrp::Link* target_link_ptr = _robot->link(_ik_tgt_list[i].target_link_name);
+        hrp::Matrix33 ref_link_origin_R(_ik_tgt_list[i].target_r * _ik_tgt_list[i].localR.transpose()); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
+        hrp::Vector3 ref_link_origin_p(_ik_tgt_list[i].target_p - ref_link_origin_R * _ik_tgt_list[i].localPos);
+        hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF); //全身to一拘束点へのヤコビアン
+        hrp::Vector3 vel_p_ref, vel_r_ref; //velと言いつつ実際は差分
+        //ベースリンク，通常リンク，重心で場合分け
+        if(target_link_ptr){//ベースリンク，通常リンク共通
+          vel_p_ref = ref_link_origin_p - target_link_ptr->p;
+          vel_r_ref = target_link_ptr->R * omegaFromRotEx(target_link_ptr->R.transpose() * ref_link_origin_R);
 
-          if(target_link_ptr == m_robot->rootLink()){      //base specific
+          if(target_link_ptr == _robot->rootLink()){ //ベース限定
             J_part.rightCols(WS_DOF) = hrp::dmatrix::Identity(WS_DOF, WS_DOF);
           }
-          else{    //normal link
-            hrp::JointPathEx tgt_jpex(m_robot, m_robot->rootLink(), target_link_ptr, m_dt, false, "");
-            tgt_jpex.calcJacobian(J_jpath);
-            for(int id_in_jpath=0; id_in_jpath<tgt_jpex.numJoints(); id_in_jpath++){
-              int id_in_body = tgt_jpex.joint(id_in_jpath)->jointId; //全身でのjoint番号
+          else{ //通常リンク限定
+            hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), target_link_ptr, m_dt, false, "");
+            hrp::dmatrix J_jpath;
+            tgt_jpath.calcJacobian(J_jpath);
+            for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
+              int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
               J_part.col(id_in_body) = J_jpath.col(id_in_jpath);
             }
             J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
-            J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(target_link_ptr->p - m_robot->rootLink()->p);
+            J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(target_link_ptr->p - _robot->rootLink()->p);
           }
         }
-        else if(!target_link_ptr && ik_tgt_list[i].target_link_name == "COM"){  // COM
-          hrp::Vector3 vel_com_p_ref(ref_link_origin_p - m_robot->calcCM());
-//          hrp::Vector3 vel_com_r_ref = (omegaFromRotEx(ref_link_origin_R) - cur_L) * m_dt;// COMのrotはAngulerMomentumとして扱う
-          hrp::Vector3 vel_com_r_ref = (omegaFromRotEx(ref_link_origin_R) - (cur_L - m_robot->rootLink()->p.cross(cur_P))) * m_dt;// COMのrotはAngulerMomentumとして扱う
-//          std::cerr<<"cur_L "<<cur_L.transpose()<<std::endl;
-//          std::cerr<<"m_robot->rootLink()->p.cross(cur_P) "<<m_robot->rootLink()->p.cross(cur_P).transpose()<<std::endl;
-//          std::cerr<<"vel_com_r_ref "<<vel_com_r_ref.transpose()<<std::endl;
-          dp_ee_all.segment(WS_DOF*i,WS_DOF) << vel_com_p_ref, vel_com_r_ref;
-          m_robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
-          m_robot->calcAngularMomentumJacobian(NULL, J_am);//すでにrootlink周りの角運動量ヤコビアンが返ってくる？
+        else if(!target_link_ptr && _ik_tgt_list[i].target_link_name == "COM"){ //重心限定
+          vel_p_ref = ref_link_origin_p - _robot->calcCM();
+          vel_r_ref = (omegaFromRotEx(ref_link_origin_R) - (cur_L - _robot->rootLink()->p.cross(cur_P))) * m_dt;// COMのrotはAngulerMomentumとして扱う
+          _robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
+          _robot->calcAngularMomentumJacobian(NULL, J_am);//すでにrootlink周りの角運動量ヤコビアンが返ってくる？
           J_part << J_com, J_am;
         }
-        else{// Error
-          std::cerr<<"Unknown Link Target !!"<<std::endl; continue;
-        }
-        weight_vec_all.segment(WS_DOF*i, WS_DOF) = ik_tgt_list[i].weight_vec;
-        selection_vec_all.segment(WS_DOF*i, WS_DOF) = ik_tgt_list[i].selection_vec;
+        else{ std::cerr<<"Unknown Link Target !!"<<std::endl; continue; } //不明なリンク指定
+        //全体の中に配置
+        dp_ee_all.segment(WS_DOF*i,WS_DOF) << vel_p_ref, vel_r_ref;
+        weight_vec_all.segment(WS_DOF*i, WS_DOF) = _ik_tgt_list[i].weight_vec;
+        selection_vec_all.segment(WS_DOF*i, WS_DOF) = _ik_tgt_list[i].selection_vec;
         J_all.middleRows(WS_DOF*i, WS_DOF) = J_part;
       }
 
-      hrp::dmatrix selection_matrix = hrp::dmatrix::Zero(selection_vec_all.sum(), WS_DOF*ik_tgt_list.size());
+      hrp::dmatrix selection_matrix = hrp::dmatrix::Zero(selection_vec_all.sum(), WS_DOF*_ik_tgt_list.size());
       for(int i=0, j=0; i<selection_vec_all.rows(); i++){ if(selection_vec_all(i) == 1){ selection_matrix(j, i) = 1.0; j++; } }
 
       weight_vec_all /= weight_vec_all.maxCoeff();
@@ -125,52 +166,58 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
       dp_ee_all = selection_matrix * weight_vec_all.asDiagonal() * dp_ee_all;
 
 //      hrp::calcPseudoInverse(J_all,J_all_inv,1.0e-3);//超遅い1[ms]とか
-      calcJacobianInverseNullspace_copy(m_robot, J_all, J_all_inv);
-
-      static int count;
+      calcJacobianInverseNullspace_copy(_robot, J_all, J_all_inv);
 
       dq_all = J_all_inv * dp_ee_all; // dq = pseudoInverse(J) * v
-      if(count % 2000 == 0){
-        std::cout<<"solveIK"<<std::endl;
-        std::cout<<"eevel_all\n"<<dp_ee_all<<std::endl;
+      static int count;
+      if(count++ % 10000 == 0){
         std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
         std::cout<<std::setprecision(2) << "J_all_inv=\n"<<J_all_inv<<std::setprecision(6)<<std::endl;
         std::cout<<std::setprecision(2) << "selection_matrix=\n"<<selection_matrix<<std::setprecision(6)<<std::endl;
+        std::cout<<"eevel_all\n"<<dp_ee_all<<std::endl;
         std::cout<<"dq_all\n"<<dq_all<<std::endl;
       }
 
       const double LAMBDA = 1.0;
+      // check validity
+      hrp::Matrix33 base_R_ans = hrp::hat(dq_all.segment(J_DOF+3,3) * LAMBDA).exp() * _robot->rootLink()->R;
+      if(!base_R_ans.isUnitary()){
+        std::cerr <<"ERROR base_R_ans is not Unitary" << std::endl;
+        std::cerr <<"base_R_ans\n"<<base_R_ans << std::endl;
+        std::cerr <<"_robot->rootLink()->R\n"<<_robot->rootLink()->R << std::endl;
+        std::cerr <<"dq_all "<<dq_all.transpose() << std::endl;
+        std::cerr <<"dp_ee_all "<<dp_ee_all.transpose() << std::endl;
+        for ( int i=0; i<_ik_tgt_list.size(); i++ ) {
+          std::cerr <<"target_p "<<_ik_tgt_list[i].target_p.transpose()<<std::endl;
+          std::cerr <<"target_link_ptr->p "<<_robot->link(_ik_tgt_list[i].target_link_name)->p.transpose()<<std::endl;
+        }
+        return;
+      }
+      for(int i=0;i<dq_all.rows();i++){ if( isnan(dq_all(i)) || isinf(dq_all(i)) ){ std::cerr <<"ERROR nan/inf is found" << std::endl; return;} }
 
-      m_robot->rootLink()->p += dq_all.segment(m_robot->numJoints(),3) * LAMBDA/* * m_dt*/;//vel計算する時からm_dt使ってないから使わなくていい
-      m_robot->rootLink()->R *= hrp::rotFromRpy( dq_all.segment(m_robot->numJoints()+3,3)) * LAMBDA /* * m_dt*/ ;
-//
-//      m_robot->rootLink()->v = dq_all.segment(m_robot->numJoints(),3) * LAMBDA / m_dt; //for ang moment
-//      m_robot->rootLink()->w = dq_all.segment(m_robot->numJoints()+3,3) * LAMBDA / m_dt; //for ang moment
-
-      for(int i=0;i<m_robot->numJoints();i++){
-        m_robot->joint(i)->q += dq_all(i) * LAMBDA/* * m_dt*/;
-        LIMIT_MINMAX(m_robot->joint(i)->q, m_robot->joint(i)->llimit, m_robot->joint(i)->ulimit);
-//        m_robot->joint(i)->dq = dq_all(i) * LAMBDA / m_dt; //for calc ang moment
+      //関節角+ベース位置姿勢更新
+      for(int i=0;i<J_DOF;i++){
+        _robot->joint(i)->q += dq_all(i) * LAMBDA/* * m_dt*/;
+        LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit);
       }
-      count++;
+      _robot->rootLink()->p += dq_all.segment(J_DOF,3) * LAMBDA/* * m_dt*/;//vel計算する時からm_dt使ってないから使わなくていい
+      _robot->rootLink()->R = base_R_ans;
     }
 
 
-    bool calcJacobianInverseNullspace_copy(hrp::BodyPtr robot, hrp::dmatrix &J, hrp::dmatrix &Jinv) {
-      const int BASE_DOF = 6;
-      const int ALL_DOF = J.cols();//BASE_DOF + m_robot->numJoints()
+    bool calcJacobianInverseNullspace_copy(hrp::BodyPtr _robot, hrp::dmatrix &_J, hrp::dmatrix &_Jinv) {
       hrp::dmatrix w = hrp::dmatrix::Identity(ALL_DOF,ALL_DOF);
       std::vector<double> avoid_weight_gain;
-      avoid_weight_gain.resize(robot->numJoints());
-      for(int j=0; j < m_robot->numJoints(); j++){
+      avoid_weight_gain.resize(_robot->numJoints());
+      for(int j=0; j < J_DOF; j++){
         avoid_weight_gain[j] = 100000000000000000000.0;
       }
       bool use_inside_joint_weight_retrieval = true;
 
-      for ( int j = 0; j < robot->numJoints() ; j++ ) {
-        double jang = robot->joint(j)->q;
-        double jmax = robot->joint(j)->ulimit;
-        double jmin = robot->joint(j)->llimit;
+      for ( int j = 0; j < _robot->numJoints() ; j++ ) {
+        double jang = _robot->joint(j)->q;
+        double jmax = _robot->joint(j)->ulimit;
+        double jmin = _robot->joint(j)->llimit;
         double e = deg2rad(1);
         if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
         } else if ( eps_eq(jang, jmax,e) ) {
@@ -196,18 +243,21 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
         }
         avoid_weight_gain[j] = r;
       }
+      for ( int j = _robot->numJoints(); j < ALL_DOF ; j++ ) {
+        w(j, j) = optional_weight_vector[j];
+      }
 
-      const double manipulability_limit = 0.1;
+      const double manipulability_limit = 0.001;
       const double sr_gain = 1.0;
       const double manipulability_gain = 0.001;
 
-      double manipulability = sqrt((J*J.transpose()).determinant());
+      double manipulability = sqrt((_J*_J.transpose()).determinant());
       double k = 0;
       if ( manipulability < manipulability_limit ) {
         k = manipulability_gain * pow((1 - ( manipulability / manipulability_limit )), 2);
       }
-      hrp::calcSRInverse(J, Jinv, sr_gain * k, w);
-      //        Jnull = ( hrp::dmatrix::Identity(n, n) - Jinv * J);
+      hrp::calcSRInverse(_J, _Jinv, sr_gain * k, w);
+      //        Jnull = ( hrp::dmatrix::Identity(n, n) - _Jinv * _J);
       return true;
     }
     hrp::Vector3 omegaFromRotEx(const hrp::Matrix33& r) {//copy from JointPathEx.cpp
@@ -228,4 +278,101 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
 };
 
 
+void* callMemberFunc(void *obj);
+
+
+class FullbodyInverseKinematicsSolverMT : public FullbodyInverseKinematicsSolver{
+  protected:
+    hrp::BodyPtr m_robot_copy;
+    pthread_t ik_thread;
+    pthread_mutex_t ik_body_mutex;  // Mutex
+    int cur_ik_loop;
+    std::vector<IKConstraintParam> ik_tgt_list;
+    bool is_first;
+    bool ik_thread_kill;
+    bool ik_thread_ik_required;
+    hrp::Vector3 base_p_old;
+    hrp::Matrix33 base_R_old;
+    hrp::dvector q_old;
+  public:
+    FullbodyInverseKinematicsSolverMT (hrp::BodyPtr& _robot, const std::string& _print_str, const double _dt) : FullbodyInverseKinematicsSolver(_robot,_print_str, _dt) {
+      m_robot_copy = hrp::BodyPtr(new hrp::Body(*_robot));
+      cur_ik_loop = 0;
+      is_first = true;
+      ik_thread_kill = false;
+      ik_thread_ik_required = false;
+    };
+    ~FullbodyInverseKinematicsSolverMT () {
+      pthread_cancel(ik_thread);
+      ik_thread_kill = true;
+      pthread_join(ik_thread, NULL );
+      std::cerr<<"[FullbodyInverseKinematicsSolverMT] thread killed successfully"<<std::endl;
+    };
+    int solveFullbodyIKLoopMT (const std::vector<IKConstraintParam>& _ik_tgt_list, const int _max_iteration) {
+      if(is_first){
+        for(int i=0;i<J_DOF;i++){ m_robot_copy->joint(i)->q = m_robot->joint(i)->q; }
+        m_robot_copy->rootLink()->p = m_robot->rootLink()->p;
+        m_robot_copy->rootLink()->R = m_robot->rootLink()->R;
+        pthread_mutex_init( &ik_body_mutex, NULL );
+      }
+
+      for(int i=0;i<J_DOF;i++){ m_robot->joint(i)->q = m_robot_copy->joint(i)->q; }
+      m_robot->rootLink()->p = m_robot_copy->rootLink()->p;
+      m_robot->rootLink()->R = m_robot_copy->rootLink()->R;
+      int result = cur_ik_loop;
+
+//      std::vector<IKConstraintParam> ik_tgt_list_tmp = _ik_tgt_list;
+
+      if(pthread_mutex_lock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_lock err "<<std::endl;}
+
+//        base_p_old = m_robot_copy->rootLink()->p;
+//        base_R_old = m_robot_copy->rootLink()->R;
+//        q_old.resize(J_DOF);
+//        for(int i=0;i<J_DOF;i++){ q_old(i) = m_robot_copy->joint(i)->q; }
+
+        cur_ik_loop = 0;
+        ik_tgt_list = _ik_tgt_list;// "="で代入するとnanが入る！？->そんなことはない
+//        ik_tgt_list = ik_tgt_list_tmp;
+//        ik_tgt_list.resize(_ik_tgt_list.size());
+//        std::copy(_ik_tgt_list.begin(), _ik_tgt_list.end(), ik_tgt_list.begin());
+
+//        for ( int i=0; i<_ik_tgt_list.size(); i++ ) {
+//          std::cerr <<"target_p_pre "<<ik_tgt_list[i].target_p.transpose()<<std::endl;
+//        }
+        ik_thread_ik_required = true;
+      if(pthread_mutex_unlock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_unlock err "<<std::endl;}
+      if(is_first){
+        pthread_create(&ik_thread, NULL, callMemberFunc, this);
+        is_first = false;
+      }
+
+      return result;
+    }
+    void ik_loop(){
+      std::cerr<<"[FullbodyInverseKinematicsSolverMT] start thread"<<std::endl;
+      while(1){
+        usleep(0);//これ無いとmutex解放しない？
+        if(pthread_mutex_lock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_lock2 err "<<std::endl;}
+          if(ik_thread_ik_required){
+//            std::cerr<<"[FullbodyInverseKinematicsSolverMT] solveFullbodyIK in thread"<<std::endl;
+            solveFullbodyIK(m_robot_copy, ik_tgt_list);
+            //check ang moment
+//            m_robot_copy->rootLink()->v = (m_robot_copy->rootLink()->p - base_p_old)/ m_dt;
+//            m_robot_copy->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * m_robot_copy->rootLink()->R) / m_dt;
+//            for(int i=0;i<J_DOF;i++){ m_robot_copy->joint(i)->dq = (m_robot_copy->joint(i)->q - q_old(i)) / m_dt; }
+//            m_robot_copy->calcForwardKinematics(true,false);
+//            m_robot_copy->calcTotalMomentum(cur_P, cur_L);
+            cur_ik_loop++;
+            if(cur_ik_loop > 10000){std::cerr<<"[FullbodyInverseKinematicsSolverMT] thread time out"<<std::endl;break;}//異常終了でスレッドが残ってしまった時にタイムアウト終了
+          }
+        if(pthread_mutex_unlock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_unlock2 err "<<std::endl;}
+        if(ik_thread_kill){break;}
+      }
+      std::cerr<<"[FullbodyInverseKinematicsSolverMT] end of thread"<<std::endl;
+    }
+};
+
+
+
+
 #endif //  FULLBODYIK_H
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index b15740faf31..2f875488680 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -37,6 +37,14 @@ static const char* WholeBodyMasterSlave_spec[] =
 //    return os;
 //}
 
+void* callMemberFunc(void *obj){
+  std::cerr<<"[FullbodyInverseKinematicsSolverMT] callMemberFunc"<<std::endl;
+  FullbodyInverseKinematicsSolverMT* myClass = reinterpret_cast<FullbodyInverseKinematicsSolverMT*>(obj);
+  myClass->ik_loop();
+  std::cerr<<"[FullbodyInverseKinematicsSolverMT] callMemberFunc finished"<<std::endl;
+  return NULL;
+}
+
 WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager),
       m_qRefIn("qRef", m_qRef),// from sh
       m_zmpIn("zmpIn", m_zmp),
@@ -137,33 +145,21 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
   
     RTC::Properties& prop =  getProperties();
     coil::stringTo(m_dt, prop["dt"].c_str());
-
-    m_robot = hrp::BodyPtr(new hrp::Body());
-    m_robot_rmc = hrp::BodyPtr(new hrp::Body());
-    m_robot_ml = hrp::BodyPtr(new hrp::Body());
-    m_robot_vsafe = hrp::BodyPtr(new hrp::Body());
     RTC::Manager& rtcManager = RTC::Manager::instance();
     std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
     int comPos = nameServer.find(",");
     if (comPos < 0){ comPos = nameServer.length(); }
     nameServer = nameServer.substr(0, comPos);
     RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
+    m_robot = hrp::BodyPtr(new hrp::Body());
+    m_robot_ml = hrp::BodyPtr(new hrp::Body());
+    m_robot_vsafe = hrp::BodyPtr(new hrp::Body());
     if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
       std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
       return RTC::RTC_ERROR;
     }
-    if (!loadBodyFromModelLoader(m_robot_rmc, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
-      std::cerr << "[" << m_profile.instance_name << "] failed to load model 2 [" << prop["model"] << "]" << std::endl;
-      return RTC::RTC_ERROR;
-    }
-    if (!loadBodyFromModelLoader(m_robot_ml, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
-      std::cerr << "[" << m_profile.instance_name << "] failed to load model 2 [" << prop["model"] << "]" << std::endl;
-      return RTC::RTC_ERROR;
-    }
-    if (!loadBodyFromModelLoader(m_robot_vsafe, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
-      std::cerr << "[" << m_profile.instance_name << "] failed to load model 2 [" << prop["model"] << "]" << std::endl;
-      return RTC::RTC_ERROR;
-    }
+    m_robot_ml = hrp::BodyPtr(new hrp::Body(*m_robot)); //copy
+    m_robot_vsafe = hrp::BodyPtr(new hrp::Body(*m_robot)); //copy
     // allocate memory for outPorts
     m_qRef.data.length(m_robot->numJoints());
     m_htrfw.data.length(6);
@@ -183,19 +179,16 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     q_ip->setName(std::string(m_profile.instance_name)+" q_ip");
     q_ip->clear();
     // Generate FIK
-    fik = fikPtr(new FullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
-    fik_rmc = fikPtr(new FullbodyInverseKinematicsSolver(m_robot_rmc, std::string(m_profile.instance_name), m_dt));
-    fik_ml = fikPtr(new FullbodyInverseKinematicsSolver(m_robot_ml, std::string(m_profile.instance_name), m_dt));
-    fik_vsafe = fikPtr(new FullbodyInverseKinematicsSolver(m_robot_vsafe, std::string(m_profile.instance_name), m_dt));
+    fik = fikPtr(new FullbodyInverseKinematicsSolverMT(m_robot, std::string(m_profile.instance_name), m_dt));
+    fik_ml = fikPtr(new FullbodyInverseKinematicsSolverMT(m_robot_ml, std::string(m_profile.instance_name), m_dt));
     fik_list.push_back(fik);
-    fik_list.push_back(fik_rmc);
     fik_list.push_back(fik_ml);
-    fik_list.push_back(fik_vsafe);
     body_list.push_back(m_robot);
-    body_list.push_back(m_robot_rmc);
     body_list.push_back(m_robot_ml);
     body_list.push_back(m_robot_vsafe);
-    for(int i=0;i<fik_list.size();i++){ setupfik(fik_list[i], body_list[i], prop); }
+
+    setupfik(fik, m_robot, prop);
+    setupfik(fik_ml, m_robot_ml, prop);
 
     if (fik->ikp.find("rleg") != fik->ikp.end() && fik->ikp.find("lleg") != fik->ikp.end()) {
       is_legged_robot = true;
@@ -211,6 +204,9 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     return RTC::RTC_OK;
 }
 
+RTC::ReturnCode_t WholeBodyMasterSlave::onFinalize(){
+  return RTC::RTC_OK;
+}
 
 void WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in){
   coil::vstring end_effectors_str = coil::split(prop_in["end_effectors"], ",");
@@ -282,7 +278,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
       if (m_htheadIn.isNew()){ m_htheadIn.read(); WBMSCore::Pose3DToWBMSPose3D(m_hthead.data,hsp->hp_wld_raw.tgt[head].abs);}
       if (m_htzmpIn.isNew()){ m_htzmpIn.read();  WBMSCore::Point3DToVector3(m_htzmp.data,hsp->hp_wld_raw.tgt[zmp].abs.p); }
     }
-//    if (m_actzmpIn.isNew()){m_actzmpIn.read(); }
 
     if ( is_legged_robot ) {
 
@@ -306,13 +301,12 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
           preProcessForWholeBodyMasterSlave(fik, m_robot);
           hsp->fik_ml = fik_ml;
           hsp->m_robot_ml = m_robot_ml;
-          hsp->fik_act = fik_vsafe;
+//          hsp->fik_act = fik_vsafe;
           hsp->m_robot_act = m_robot_vsafe;
           //逆動力学初期化
           idsb.setInitState(m_robot, m_dt);
           idsb2.setInitState(m_robot, m_dt);
         }
-        calcManipulability(fik_rmc, m_robot_rmc);
 
         if(hsp->WBMSparam.is_doctor){
           processWholeBodyMasterSlave(fik, m_robot, hsp->rp_ref_out);//安全制限つきマスタ・スレーブ
@@ -331,16 +325,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
         if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ calcWorldZMPFromInverseDynamics" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
-        //角運動量補償
-//        if(hsp->WBMSparam.is_doctor){
-//          processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, hsp->rp_ref_out);
-//        }else{
-//          processMomentumCompensation(fik_rmc, m_robot_rmc, m_robot, raw_pose);
-//        }
-//        processBBAccelarationFilter(m_robot, m_robot_rmc);
-
-        if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processMomentumCompensation" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
-
         processHOFFARBIBFilter(m_robot, m_robot_vsafe);
 
         const hrp::BodyPtr m_robot_for_out = m_robot_vsafe;
@@ -489,13 +473,16 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
     std::cerr<<"["<<m_profile.instance_name<<"] Input basePos height is invalid. Auto modify "<<m_basePos.data.z<<" -> "<<basePos_heightChecked(Z)<<endl;
   }
   const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
-  for(int i=0;i<fik_list.size();i++){//初期姿勢でBodyをFK
+
+  for(int i=0;i<body_list.size();i++){//初期姿勢でBodyをFK
     body_list[i]->rootLink()->p = basePos_heightChecked;
     body_list[i]->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
     for ( int j = 0; j < body_list[i]->numJoints(); j++ ){ body_list[i]->joint(j)->q = m_qRef.data[j]; }
     if( body_list[i]->link("RARM_JOINT2") != NULL) body_list[i]->link("RARM_JOINT2")->ulimit = -30 * D2R;//脇の干渉回避のため
     if( body_list[i]->link("LARM_JOINT2") != NULL) body_list[i]->link("LARM_JOINT2")->llimit = 30 * D2R;
     body_list[i]->calcForwardKinematics();
+  }
+  for(int i=0;i<fik_list.size();i++){//初期姿勢でBodyをFK
     fik_list[i]->setReferenceJointAngles();
     for(int l=0;l<4;l++){//targetを初期化
       if(fik_list[i]->ikp.count(robot_l_names[l])){
@@ -510,7 +497,6 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
   for(int i=0;i<3;i++){ init_q[robot_in->numJoints()+i] = robot_in->rootLink()->p(i); }
   for(int i=0;i<3;i++){for(int j=0;j<3;j++){ init_q[robot_in->numJoints() + 3 + 3*i + j] = robot_in->rootLink()->R(i,j); }}
   q_ip->set(init_q);
-  torso_rot_rmc = hrp::Vector3::Zero();//RMCの補償量リセット
   hsp->initializeRequest(fik_in, robot_in);
 }
 
@@ -519,11 +505,13 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(fikPtr& fik_in, hrp::Body
   hsp->update();//////HumanSynchronizerの主要処理
   if(DEBUGP)cout<<"update():"<<hsp->getUpdateTime()<<endl;
   if(DEBUGP)pose_ref.print();
-  WBMSPose3D rf_checked = pose_ref.tgt[rf].abs;
-  WBMSPose3D lf_checked = pose_ref.tgt[lf].abs;
-  WBMSPose3D rh_checked = pose_ref.tgt[rh].abs;
-  WBMSPose3D lh_checked = pose_ref.tgt[lh].abs;
-  solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, rf_checked, lf_checked, rh_checked, lh_checked, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave");
+//  WBMSPose3D rf_checked = pose_ref.tgt[rf].abs;
+//  WBMSPose3D lf_checked = pose_ref.tgt[lf].abs;
+//  WBMSPose3D rh_checked = pose_ref.tgt[rh].abs;
+//  WBMSPose3D lh_checked = pose_ref.tgt[lh].abs;
+//  solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, rf_checked, lf_checked, rh_checked, lh_checked, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave");
+//  solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave");
+  solveFullbodyIKStrictCOM(fik_in, robot_in, hsp->rp_ref_out.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave");
 }
 
 
@@ -566,136 +554,96 @@ void WholeBodyMasterSlave::calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robo
 
 #ifdef USE_NEW_FIK
 void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix){
-  int com_ik_loop=0;
-  const int COM_IK_MAX_LOOP = 3;
-  const double COM_IK_MAX_ERROR = 1e-3;
-  const std::string names[4] = {"rleg","lleg","rarm","larm"};
-  const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
-  fik_in->target_com_p0 = com_ref.p;
-  fik_in->target_com_r0 = hrp::rotFromRpy(com_ref.rpy);//つまりbaseのrpyとして処理される
-  for(int i=0;i<4;i++){
-    if(fik_in->ikp.count(names[i])){
-      fik_in->ikp[names[i]].target_p0 = refs[i]->p;
-      fik_in->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);
-    }
-  }
-
-  fik_in->ik_tgt_list.clear();
+  std::vector<IKConstraintParam> ik_tgt_list;
   IKConstraintParam tmp;
 
+  std::cerr <<"solveFullbodyIKStrictCOM "<<std::endl;
   tmp.target_link_name = "WAIST";
   tmp.localPos = hrp::Vector3::Zero();
   tmp.localR = hrp::Matrix33::Identity();
-  tmp.target_p0 = hrp::Vector3::Zero();// will be ignored by selection_vec
-  tmp.target_r0 = hrp::rotFromRpy(com_ref.rpy);
+  tmp.target_p = robot_in->rootLink()->p;// will be ignored by selection_vec
+  tmp.target_r = hrp::rotFromRpy(com_ref.rpy);// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
   tmp.selection_vec << 0,0,0,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
-  fik_in->ik_tgt_list.push_back(tmp);
+  ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = "RLEG_JOINT5";
   tmp.localPos = fik_in->ikp["rleg"].localPos;
   tmp.localR = fik_in->ikp["rleg"].localR;
-  tmp.target_p0 = rf_ref.p;
-  tmp.target_r0 = hrp::rotFromRpy(rf_ref.rpy);
+  tmp.target_p = rf_ref.p;
+  tmp.target_r = hrp::rotFromRpy(rf_ref.rpy);
   tmp.selection_vec << 1,1,1,1,1,1;
-  tmp.weight_vec << 1,1,3,1,1,1;
-  fik_in->ik_tgt_list.push_back(tmp);
+  tmp.weight_vec << 1,1,1,1,1,1;
+  ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = "LLEG_JOINT5";
   tmp.localPos = fik_in->ikp["lleg"].localPos;
   tmp.localR = fik_in->ikp["lleg"].localR;
-  tmp.target_p0 = lf_ref.p;
-  tmp.target_r0 = hrp::rotFromRpy(lf_ref.rpy);
+  tmp.target_p = lf_ref.p;
+  tmp.target_r = hrp::rotFromRpy(lf_ref.rpy);
   tmp.selection_vec << 1,1,1,1,1,1;
-  tmp.weight_vec << 1,1,3,1,1,1;
-  fik_in->ik_tgt_list.push_back(tmp);
+  tmp.weight_vec << 1,1,1,1,1,1;
+  ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = "RARM_JOINT7";
   tmp.localPos = fik_in->ikp["rarm"].localPos;
   tmp.localR = fik_in->ikp["rarm"].localR;
-  tmp.target_p0 = rh_ref.p;
-  tmp.target_r0 = hrp::rotFromRpy(rh_ref.rpy);
+  tmp.target_p = rh_ref.p;
+  tmp.target_r = hrp::rotFromRpy(rh_ref.rpy);
   tmp.selection_vec << 1,1,1,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
-  fik_in->ik_tgt_list.push_back(tmp);
+  ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = "LARM_JOINT7";
   tmp.localPos = fik_in->ikp["larm"].localPos;
   tmp.localR = fik_in->ikp["larm"].localR;
-  tmp.target_p0 = lh_ref.p;
-  tmp.target_r0 = hrp::rotFromRpy(lh_ref.rpy);
+  tmp.target_p = lh_ref.p;
+  tmp.target_r = hrp::rotFromRpy(lh_ref.rpy);
   tmp.selection_vec << 1,1,1,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
-  fik_in->ik_tgt_list.push_back(tmp);
+  ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = "COM";
   tmp.localPos = hrp::Vector3::Zero();
   tmp.localR = hrp::Matrix33::Identity();
-  tmp.target_p0 = com_ref.p;// COM height will not be constraint
-  tmp.target_r0 = hrp::Matrix33::Identity();//reference angular momentum
-//  tmp.selection_vec << 1,1,0,1,1,1;
-  tmp.selection_vec << 1,1,0,1,1,0;
-  tmp.weight_vec << 3,3,1,1,1,1;
-  fik_in->ik_tgt_list.push_back(tmp);
-
-  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;
-  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
-  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
-  fik_in->optional_weight_vector.tail(6) = hrp::dvector6::Ones() * 0.01;
-
-  hrp::Vector3 base_p_old = robot_in->rootLink()->p;
-  hrp::Matrix33 base_R_old = robot_in->rootLink()->R;
-  hrp::dvector q_old(robot_in->numJoints());
-  fik_in->cur_P = fik_in->cur_L = hrp::Vector3::Zero();
-
-  //確かに毎回少しreset poseに引き戻すとこじれないが・・・
-  hrp::dvector reset_pose(33);
-  reset_pose <<
+  tmp.target_p = com_ref.p;// COM height will not be constraint
+  tmp.target_r = hrp::Matrix33::Identity();//reference angular momentum
+  tmp.selection_vec << 1,1,1,0,0,0; // COM pos + Ang Momentum
+//  tmp.selection_vec << 1,1,1,1,1,0; // COM pos + Ang Momentum
+  tmp.weight_vec << 3,3,1,0.1,0.1,0.1;
+  ik_tgt_list.push_back(tmp);
+
+  tmp.target_link_name = "HEAD_JOINT1";
+  tmp.target_r = hrp::rotFromRpy(head_ref.rpy);
+  tmp.selection_vec << 0,0,0,0,1,1;
+  tmp.weight_vec << 1,1,1,1,1,1;
+  ik_tgt_list.push_back(tmp);
+
+  std::cerr <<"com_ref.p "<<com_ref.p.transpose()<<std::endl;
+//  for ( int i=0; i<ik_tgt_list.size(); i++ ) {
+//    std::cerr <<"target_p_pre "<<ik_tgt_list[i].target_p.transpose()<<std::endl;
+//  }
+
+  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT0")->jointId) = 0.01;
+  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT1")->jointId) = 0.01;
+  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT2")->jointId) = 0.01;
+//  fik_in->optional_weight_vector.tail(3).fill(0.01);
+  fik_in->reference_q.head(robot_in->numJoints()) <<
       0.0,0.0,-0.349066,0.698132,-0.349066,0.0,
       0.0,0.0,-0.349066,0.698132,-0.349066,0.0,
       0.0,0.0,0.0, 0.0,0.0,
       0.0,0.698132,-0.349066,-0.087266,-1.39626,0.0,0.0,-0.349066,
       0.0,0.698132,0.349066,0.087266,-1.39626,0.0,0.0,-0.349066;
-  for(int i=0;i<33;i++){
-    robot_in->joint(i)->q = robot_in->joint(i)->q * 0.999 + reset_pose(i) * 0.001;
-  }
-  robot_in->rootLink()->p = robot_in->rootLink()->p * 0.999 + hrp::Vector3(0,0,1.1) * 0.001;
-  robot_in->rootLink()->R = robot_in->rootLink()->R * 0.999 + hrp::Matrix33::Identity() * 0.001;
+  fik_in->reference_q.tail(6) << 0.0,0.0,1.1, 0.0,0.0,0.0;
+  fik_in->reference_gain.fill(0.01);
 
-  if( robot_in->link("HEAD_JOINT0") != NULL) robot_in->link("HEAD_JOINT0")->q = head_ref.rpy(y);
-  if( robot_in->link("HEAD_JOINT1") != NULL) robot_in->link("HEAD_JOINT1")->q = head_ref.rpy(p);
   struct timespec startT, endT;
+  int loop_result = 0;
+  const int IK_MAX_LOOP = 3;
   clock_gettime(CLOCK_REALTIME, &startT);
-  for(int i=0;i<robot_in->numJoints();i++){ q_old(i) = robot_in->joint(i)->q; }
-  while( 1 ){  //COM 収束ループ
-    com_ik_loop++;
-
-//    robot_in->calcForwardKinematics();
-    fik_in->solveFullbodyIK(hrp::Vector3::Zero(), false);
-
-    //check ang moment
-    robot_in->rootLink()->v = (robot_in->rootLink()->p - base_p_old)/ m_dt;
-    robot_in->rootLink()->w = base_R_old * fik_in->omegaFromRotEx(base_R_old.transpose() * robot_in->rootLink()->R) / m_dt;
-    for(int i=0;i<m_robot->numJoints();i++){ robot_in->joint(i)->dq = (robot_in->joint(i)->q - q_old(i)) / m_dt; }
-    robot_in->calcForwardKinematics(true,false);
-    robot_in->calcTotalMomentum(fik_in->cur_P, fik_in->cur_L);
-
-    hrp::Vector3 com_p_err = com_ref.p - robot_in->calcCM();
-    hrp::Vector3 rf_p_err = rf_ref.p - fik_in->getEndEffectorPos("rleg");
-    hrp::Vector3 lf_p_err = lf_ref.p - fik_in->getEndEffectorPos("lleg");
-    hrp::Vector3 rh_p_err = rh_ref.p - fik_in->getEndEffectorPos("rarm");
-    hrp::Vector3 lh_p_err = lh_ref.p - fik_in->getEndEffectorPos("larm");
-    if(
-        com_p_err.norm() < COM_IK_MAX_ERROR &&
-        rf_p_err.norm() < 1e-3 &&
-        lf_p_err.norm() < 1e-3 &&
-        rh_p_err.norm() < 1e-2 &&
-        lh_p_err.norm() < 1e-2){ break; }
-    if(com_ik_loop >= COM_IK_MAX_LOOP){if(DEBUGP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl;} break; };
-  }
-  if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveIK loop" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
-
-  if(com_ik_loop != 1 && DEBUGP)cout<<"com_ik_loop:"<<com_ik_loop<<" @ "<<debug_prefix<<endl;
+  loop_result = fik_in->solveFullbodyIKLoopMT(ik_tgt_list, IK_MAX_LOOP);
+  if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveIK"<<loop_result<<"loop" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
+  std::cerr <<"solveFullbodyIKStrictCOM fin"<<std::endl;
 }
 #else
 //旧ver
@@ -709,8 +657,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   robot_in->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
   for(int i=0;i<4;i++){
     if(fik_in->ikp.count(names[i])){
-      fik_in->ikp[names[i]].target_p0 = refs[i]->p;
-      fik_in->ikp[names[i]].target_r0 = hrp::rotFromRpy(refs[i]->rpy);
+      fik_in->ikp[names[i]].target_p = refs[i]->p;
+      fik_in->ikp[names[i]].target_r = hrp::rotFromRpy(refs[i]->rpy);
     }
   }
 //  fik_in->storeCurrentParameters();
@@ -718,8 +666,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   if( robot_in->link("HEAD_JOINT0") != NULL) robot_in->link("HEAD_JOINT0")->q = head_ref.rpy(y);
   if( robot_in->link("HEAD_JOINT1") != NULL) robot_in->link("HEAD_JOINT1")->q = head_ref.rpy(p);
   if(fik_in->ikp.count("rarm") && fik_in->ikp.count("larm")){
-    hrp::Vector3 base2rh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["rarm"].target_p0 - robot_in->rootLink()->p);
-    hrp::Vector3 base2lh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["larm"].target_p0 - robot_in->rootLink()->p);
+    hrp::Vector3 base2rh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["rarm"].target_p - robot_in->rootLink()->p);
+    hrp::Vector3 base2lh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["larm"].target_p - robot_in->rootLink()->p);
     if( robot_in->link("CHEST_JOINT0") != NULL){
       robot_in->link("CHEST_JOINT0")->q = (base2lh(Z) - base2rh(Z)) * (10 * D2R / 1.0);
       LIMIT_MINMAX(robot_in->link("CHEST_JOINT0")->q, robot_in->link("CHEST_JOINT0")->llimit, robot_in->link("CHEST_JOINT0")->ulimit);
@@ -749,40 +697,6 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
 #endif
 
 
-
-void WholeBodyMasterSlave::processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_normal_in, const HumanPose& pose_ref){
-  hrp::Vector3 P,L;//世界座標系
-  robot_normal_in->rootLink()->v = idsb.base_v;
-  robot_normal_in->rootLink()->w = idsb.base_w;
-  robot_normal_in->calcTotalMomentum(P,L);
-  L = L - robot_normal_in->calcCM().cross(P);//重心周りの角運動量に変換
-  robot_normal_in->calcCM();
-  robot_normal_in->rootLink()->calcSubMassCM();
-  hrp::Matrix33 I_com;
-  hrp::Vector3 torso_rot_vel;
-  if(robot_normal_in->link("CHEST_JOINT0") != NULL){
-    robot_normal_in->link("CHEST_JOINT0")->calcSubMassInertia(I_com);
-    torso_rot_vel = - I_com.inverse() * L * hsp->WBMSparam.upper_body_rmc_ratio;
-    torso_rot_rmc += torso_rot_vel * m_dt;
-  }
-  WBMSPose3D com_mod = pose_ref.tgt[com].abs;
-  com_mod.rpy += torso_rot_rmc;
-
-  WBMSPose3D rf_checked = pose_ref.tgt[rf].abs;
-  WBMSPose3D lf_checked = pose_ref.tgt[lf].abs;
-  WBMSPose3D rh_checked = pose_ref.tgt[rh].abs;
-  WBMSPose3D lh_checked = pose_ref.tgt[lh].abs;
-
-  static hrp::Vector3 target_v_old[4];
-  if(hsp->WBMSparam.use_manipulability_limit){
-    struct timeval t_calc_start, t_calc_end;
-//    if(TIMECALC)gettimeofday(&t_calc_start, NULL);
-//    calcManipulabilityJointLimit(fik_in, robot_in, target_v_old, rf_checked, lf_checked, rh_checked, lh_checked);
-//    if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"calcManipulabilityJointLimit:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6<<endl; t_calc_start = t_calc_end;}
-  }
-  solveFullbodyIKStrictCOM(fik_in, robot_in, com_mod, rf_checked, lf_checked, rh_checked, lh_checked, pose_ref.tgt[head].abs,"calcDynamicsFilterCompensation");
-}
-
 void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out){
   double goal_q[q_ip_dim];
   for(int i=0;i<robot_in->numJoints();i++){ goal_q[i] = robot_in->joint(i)->q; }
@@ -910,7 +824,6 @@ bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMaster
 }
 
 
-RTC::ReturnCode_t WholeBodyMasterSlave::onFinalize(){ return RTC::RTC_OK; }
 RTC::ReturnCode_t WholeBodyMasterSlave::onActivated(RTC::UniqueId ec_id){ std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; }
 RTC::ReturnCode_t WholeBodyMasterSlave::onDeactivated(RTC::UniqueId ec_id){ std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; }
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 54ff56c3e10..5e57cdc7a94 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -163,9 +163,10 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
     bool is_active, has_toe_joint;
   };
 //  typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
-  typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
-  fikPtr fik, fik_rmc, fik_ml, fik_vsafe;
-  hrp::BodyPtr m_robot, m_robot_rmc, m_robot_ml, m_robot_vsafe;
+//  typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
+  typedef boost::shared_ptr<FullbodyInverseKinematicsSolverMT> fikPtr;
+  fikPtr fik, fik_ml;
+  hrp::BodyPtr m_robot, m_robot_ml, m_robot_vsafe;
   std::vector<fikPtr> fik_list;
   std::vector<hrp::BodyPtr> body_list;
   std::map<std::string, size_t> contact_states_index_map;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 0b37a25a2f1..ae94d735e2f 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -114,28 +114,6 @@ class HumanPose : UTIL_CONST {
     void print() const { hp_printf(*this); }
 };
 
-class RobotConfig : UTIL_CONST {
-  public:
-    std::vector< std::vector<hrp::Vector3> > ee_rot_limit;
-    RobotConfig(){
-      std::vector<hrp::Vector3> init;
-      init.resize(MINMAX);
-      ee_rot_limit.resize(num_pose_tgt, init);
-      ee_rot_limit[com][MIN] = hrp::Vector3(-10*D2R, -10*D2R, -30*D2R);
-      ee_rot_limit[com][MAX] = hrp::Vector3( 10*D2R,  10*D2R, 30*D2R);
-      ee_rot_limit[rf][MIN] = hrp::Vector3(-30*D2R, -30*D2R, -20*D2R);
-      ee_rot_limit[rf][MAX] = hrp::Vector3( 30*D2R,  30*D2R,   5*D2R);
-      ee_rot_limit[lf][MIN] = hrp::Vector3(-30*D2R, -30*D2R,  -5*D2R);
-      ee_rot_limit[lf][MAX] = hrp::Vector3( 30*D2R,  30*D2R,  20*D2R);
-      ee_rot_limit[rh][MIN] = hrp::Vector3(INFMIN, INFMIN, INFMIN);
-      ee_rot_limit[rh][MAX] = hrp::Vector3(INFMAX, INFMAX, INFMAX);
-      ee_rot_limit[lh][MIN] = hrp::Vector3(INFMIN, INFMIN, INFMIN);
-      ee_rot_limit[lh][MAX] = hrp::Vector3(INFMAX, INFMAX, INFMAX);
-      ee_rot_limit[head][MIN] = hrp::Vector3( 0*D2R, -20*D2R, -35*D2R);
-      ee_rot_limit[head][MAX] = hrp::Vector3( 0*D2R,  30*D2R,  35*D2R);
-    }
-};
-
 class WBMSCore : UTIL_CONST {
   private:
     double CNT_F_TH, h2r_ratio, tgt_h2r_ratio, HZ, DT;
@@ -146,7 +124,7 @@ class WBMSCore : UTIL_CONST {
     std::vector<hrp::Vector2> rflf_points, hull_com, hull_dcp, hull_acp;
     std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rot_filters;
     std::vector<cv::Point2f> points,cvhull;
-    RobotConfig rc;
+//    RobotConfig rc;
     HumanPose rp_ref_out_old, hp_swap_checked;
     unsigned int loop;
     bool is_initial_loop;
@@ -176,10 +154,10 @@ class WBMSCore : UTIL_CONST {
     };
     struct WBMSparameters WBMSparam;
 
-    typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
+    typedef boost::shared_ptr<FullbodyInverseKinematicsSolverMT> fikPtr;
     fikPtr fik_ml;
     hrp::BodyPtr m_robot_ml;
-    fikPtr fik_act;
+//    fikPtr fik_act;
     hrp::BodyPtr m_robot_act;
     double cur_manip_val[4][3];
     hrp::Vector3 manip_direc[4][3];
@@ -193,6 +171,7 @@ class WBMSCore : UTIL_CONST {
 
     WBMSCore(const double& dt){
       tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
+//      tgt_h2r_ratio = h2r_ratio = 1.0;
       DT = dt;
       HZ = (int)(1.0/DT);
       CNT_F_TH = 20.0;
@@ -281,6 +260,7 @@ class WBMSCore : UTIL_CONST {
         }
       }
       rp_ref_out.tgt[com].offs.p = robot_in->calcCM();
+//      std::cerr <<"init com "<<robot_in->calcCM().transpose()<<std::endl;
       rp_ref_out.tgt[com].offs.rpy = hrp::rpyFromRot(robot_in->rootLink()->R);
       rp_ref_out.tgt[zmp].offs.p(X) = rp_ref_out.tgt[com].offs.p(X);
       rp_ref_out.tgt[zmp].offs.p(Y) = rp_ref_out.tgt[com].offs.p(Y);
@@ -292,6 +272,8 @@ class WBMSCore : UTIL_CONST {
       for(int i=0, l[LR]={rf,lf}; i<LR; i++){
         rp_ref_out.tgt[l[i]].is_contact = true;
       }
+//      std::cerr <<"init com2 "<<rp_ref_out.tgt[com].offs.p.transpose()<<std::endl;
+//      std::cerr <<"init com2 "<<rp_ref_out.tgt[com].abs.p.transpose()<<std::endl;
     }
     void initializeRequest(fikPtr& fik_in, hrp::BodyPtr& robot_in){
       loop = 0;
@@ -317,6 +299,7 @@ class WBMSCore : UTIL_CONST {
 
       limitManipulability                 (rp_ref_out);
 
+//      std::cerr <<"out com "<<rp_ref_out.tgt[com].abs.p.transpose()<<std::endl;
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
 
       Vector3ToVector2(rp_ref_out.tgt[com].abs.p,com_forcp_ref);
@@ -324,18 +307,21 @@ class WBMSCore : UTIL_CONST {
       com_vel_forcp_ref = (com_forcp_ref - com_forcp_ref_old)/DT;
       com_forcp_ref_old = com_forcp_ref;
 
+//      std::cerr <<"out com "<<rp_ref_out.tgt[com].abs.p.transpose()<<std::endl;
 
       applyLPFilter_pre                       (rp_ref_out);
       applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
       applyLPFilter_post                       (rp_ref_out);
 
+//      std::cerr <<"out com "<<rp_ref_out.tgt[com].abs.p.transpose()<<std::endl;
       r_zmp_raw = rp_ref_out.tgt[zmp].abs.p;
       applyZMPCalcFromCOM                 (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);
       if(DEBUG){
         fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.tgt[zmp].abs.p(X),rp_ref_out.tgt[zmp].abs.p(Y));
         fprintf(cz_log,"\n");
       }
+//      std::cerr <<"out com "<<rp_ref_out.tgt[com].abs.p.transpose()<<std::endl;
       overwriteFootZFromFootLandOnCommand (rp_ref_out);
       modifyFootRotAndXYForContact        (rp_ref_out);
 
@@ -343,6 +329,7 @@ class WBMSCore : UTIL_CONST {
       com_vel_old = (rp_ref_out.tgt[com].abs.p - rp_ref_out_old.tgt[com].abs.p)/DT;
       rh_vel_old = (rp_ref_out.tgt[rh].abs.p - rp_ref_out_old.tgt[rh].abs.p)/DT;
       rp_ref_out_old = rp_ref_out;
+//      std::cerr <<"out com "<<rp_ref_out.tgt[com].abs.p.transpose()<<std::endl;
       loop++;
       is_initial_loop = false;
       gettimeofday(&t_calc_end, NULL);
@@ -525,11 +512,11 @@ class WBMSCore : UTIL_CONST {
         out.tgt[l[i]].abs.p(X) = baselinkpose.p(X) + horizontal_dist(X);
         out.tgt[l[i]].abs.p(Y) = baselinkpose.p(Y) + horizontal_dist(Y);
       }
-      for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( out.tgt[com].abs.rpy(i), rc.ee_rot_limit[com][MIN](i), rc.ee_rot_limit[com][MAX](i) ); }
       LIMIT_MINMAX( out.tgt[com].abs.p(Z), out.tgt[com].offs.p(Z) - 0.15, out.tgt[com].offs.p(Z) + 0.03 );//COM高さ方向の制限
-      for(int i=0, l[5]={rf,lf,rh,lh,head}; i<5; i++){
-        for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( out.tgt[l[i]].abs.rpy(j), rc.ee_rot_limit[l[i]][MIN](j) + out.tgt[com].abs.rpy(j), rc.ee_rot_limit[l[i]][MAX](j) + out.tgt[com].abs.rpy(j) ); }
-      }
+//      for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( out.tgt[com].abs.rpy(i), rc.ee_rot_limit[com][MIN](i), rc.ee_rot_limit[com][MAX](i) ); }
+//      for(int i=0, l[5]={rf,lf,rh,lh,head}; i<5; i++){
+//        for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( out.tgt[l[i]].abs.rpy(j), rc.ee_rot_limit[l[i]][MIN](j) + out.tgt[com].abs.rpy(j), rc.ee_rot_limit[l[i]][MAX](j) + out.tgt[com].abs.rpy(j) ); }
+//      }
       if(!WBMSparam.use_head)out.tgt[head].abs.rpy = hrp::Vector3::Zero();
     }
     void limitManipulability(HumanPose& out){
@@ -545,7 +532,7 @@ class WBMSCore : UTIL_CONST {
           fik_ml->ikp[robot_l_names[i]].target_p0 = out.tgt[human_l_names[i]].abs.p;
         }
       }
-      for ( std::map<std::string, FullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_ml->ikp.begin(); it != fik_ml->ikp.end(); it++ ) {
+      for ( std::map<std::string, FullbodyInverseKinematicsSolverMT::IKparam>::iterator it = fik_ml->ikp.begin(); it != fik_ml->ikp.end(); it++ ) {
           if (it->second.is_ik_enable) fik_ml->solveLimbIK (it->second, it->first, fik_ml->ratio_for_vel, false);
       }
       for(int i=0;i<4;i++){

From cdd8234e32847662708a5f0f2d91a97b7407a9af Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 21 Aug 2017 01:16:24 +0900
Subject: [PATCH 115/217] finish debugging nan

---
 .../FullbodyInverseKinematicsSolver.h         | 41 +++++---------
 .../WholeBodyMasterSlave.cpp                  | 56 ++++++++-----------
 rtc/WholeBodyMasterSlave/wbms_core.h          | 13 +----
 3 files changed, 38 insertions(+), 72 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index a68559aaad0..3a454245819 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -115,6 +115,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
       for(int i=0;i<J_DOF;i++){ _robot->joint(i)->q = _robot->joint(i)->q * (1-reference_gain(i)) + reference_q(i) * reference_gain(i); }
       _robot->rootLink()->p = _robot->rootLink()->p.cwiseProduct(hrp::Vector3::Ones()-reference_gain.segment(J_DOF,3)) + reference_q.segment(J_DOF,3).cwiseProduct(reference_gain.segment(J_DOF,3));
       _robot->rootLink()->R = hrp::rotFromRpy( hrp::rpyFromRot(_robot->rootLink()->R).cwiseProduct(hrp::Vector3::Ones()-reference_gain.segment(J_DOF+3,3)) + reference_q.segment(J_DOF+3,3).cwiseProduct(reference_gain.segment(J_DOF+3,3)));
+      for(int i=0;i<J_DOF;i++){ LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit); }
       _robot->calcForwardKinematics();
       //ヤコビアンと各ベクトル生成
       for ( int i=0; i<_ik_tgt_list.size(); i++ ) {
@@ -152,6 +153,8 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
         }
         else{ std::cerr<<"Unknown Link Target !!"<<std::endl; continue; } //不明なリンク指定
         //全体の中に配置
+        if(vel_p_ref.norm()>0.1){vel_p_ref = vel_p_ref.normalized() * 0.1;}
+        if(vel_r_ref.norm()>0.1){vel_r_ref = vel_r_ref.normalized() * 0.1;}
         dp_ee_all.segment(WS_DOF*i,WS_DOF) << vel_p_ref, vel_r_ref;
         weight_vec_all.segment(WS_DOF*i, WS_DOF) = _ik_tgt_list[i].weight_vec;
         selection_vec_all.segment(WS_DOF*i, WS_DOF) = _ik_tgt_list[i].selection_vec;
@@ -181,18 +184,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
       const double LAMBDA = 1.0;
       // check validity
       hrp::Matrix33 base_R_ans = hrp::hat(dq_all.segment(J_DOF+3,3) * LAMBDA).exp() * _robot->rootLink()->R;
-      if(!base_R_ans.isUnitary()){
-        std::cerr <<"ERROR base_R_ans is not Unitary" << std::endl;
-        std::cerr <<"base_R_ans\n"<<base_R_ans << std::endl;
-        std::cerr <<"_robot->rootLink()->R\n"<<_robot->rootLink()->R << std::endl;
-        std::cerr <<"dq_all "<<dq_all.transpose() << std::endl;
-        std::cerr <<"dp_ee_all "<<dp_ee_all.transpose() << std::endl;
-        for ( int i=0; i<_ik_tgt_list.size(); i++ ) {
-          std::cerr <<"target_p "<<_ik_tgt_list[i].target_p.transpose()<<std::endl;
-          std::cerr <<"target_link_ptr->p "<<_robot->link(_ik_tgt_list[i].target_link_name)->p.transpose()<<std::endl;
-        }
-        return;
-      }
+      if(!base_R_ans.isUnitary()){ std::cerr <<"ERROR base_R_ans is not Unitary" << std::endl; return; }
       for(int i=0;i<dq_all.rows();i++){ if( isnan(dq_all(i)) || isinf(dq_all(i)) ){ std::cerr <<"ERROR nan/inf is found" << std::endl; return;} }
 
       //関節角+ベース位置姿勢更新
@@ -249,7 +241,8 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
 
       const double manipulability_limit = 0.001;
       const double sr_gain = 1.0;
-      const double manipulability_gain = 0.001;
+//      const double manipulability_gain = 0.001;
+      const double manipulability_gain = 1;
 
       double manipulability = sqrt((_J*_J.transpose()).determinant());
       double k = 0;
@@ -278,9 +271,6 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
 };
 
 
-void* callMemberFunc(void *obj);
-
-
 class FullbodyInverseKinematicsSolverMT : public FullbodyInverseKinematicsSolver{
   protected:
     hrp::BodyPtr m_robot_copy;
@@ -313,16 +303,15 @@ class FullbodyInverseKinematicsSolverMT : public FullbodyInverseKinematicsSolver
         for(int i=0;i<J_DOF;i++){ m_robot_copy->joint(i)->q = m_robot->joint(i)->q; }
         m_robot_copy->rootLink()->p = m_robot->rootLink()->p;
         m_robot_copy->rootLink()->R = m_robot->rootLink()->R;
+        if( m_robot_copy->link("RARM_JOINT2") != NULL) m_robot_copy->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇の干渉回避のため
+        if( m_robot_copy->link("LARM_JOINT2") != NULL) m_robot_copy->link("LARM_JOINT2")->llimit = deg2rad(40);
         pthread_mutex_init( &ik_body_mutex, NULL );
       }
-
       for(int i=0;i<J_DOF;i++){ m_robot->joint(i)->q = m_robot_copy->joint(i)->q; }
       m_robot->rootLink()->p = m_robot_copy->rootLink()->p;
       m_robot->rootLink()->R = m_robot_copy->rootLink()->R;
       int result = cur_ik_loop;
 
-//      std::vector<IKConstraintParam> ik_tgt_list_tmp = _ik_tgt_list;
-
       if(pthread_mutex_lock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_lock err "<<std::endl;}
 
 //        base_p_old = m_robot_copy->rootLink()->p;
@@ -332,17 +321,10 @@ class FullbodyInverseKinematicsSolverMT : public FullbodyInverseKinematicsSolver
 
         cur_ik_loop = 0;
         ik_tgt_list = _ik_tgt_list;// "="で代入するとnanが入る！？->そんなことはない
-//        ik_tgt_list = ik_tgt_list_tmp;
-//        ik_tgt_list.resize(_ik_tgt_list.size());
-//        std::copy(_ik_tgt_list.begin(), _ik_tgt_list.end(), ik_tgt_list.begin());
-
-//        for ( int i=0; i<_ik_tgt_list.size(); i++ ) {
-//          std::cerr <<"target_p_pre "<<ik_tgt_list[i].target_p.transpose()<<std::endl;
-//        }
         ik_thread_ik_required = true;
       if(pthread_mutex_unlock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_unlock err "<<std::endl;}
       if(is_first){
-        pthread_create(&ik_thread, NULL, callMemberFunc, this);
+        pthread_create(&ik_thread, NULL, launchThread, this);
         is_first = false;
       }
 
@@ -354,7 +336,6 @@ class FullbodyInverseKinematicsSolverMT : public FullbodyInverseKinematicsSolver
         usleep(0);//これ無いとmutex解放しない？
         if(pthread_mutex_lock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_lock2 err "<<std::endl;}
           if(ik_thread_ik_required){
-//            std::cerr<<"[FullbodyInverseKinematicsSolverMT] solveFullbodyIK in thread"<<std::endl;
             solveFullbodyIK(m_robot_copy, ik_tgt_list);
             //check ang moment
 //            m_robot_copy->rootLink()->v = (m_robot_copy->rootLink()->p - base_p_old)/ m_dt;
@@ -370,6 +351,10 @@ class FullbodyInverseKinematicsSolverMT : public FullbodyInverseKinematicsSolver
       }
       std::cerr<<"[FullbodyInverseKinematicsSolverMT] end of thread"<<std::endl;
     }
+    static void* launchThread(void *pParam) {
+      reinterpret_cast<FullbodyInverseKinematicsSolverMT*>(pParam)->ik_loop();
+      return NULL;
+    }
 };
 
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 2f875488680..372abc10cc2 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -37,13 +37,13 @@ static const char* WholeBodyMasterSlave_spec[] =
 //    return os;
 //}
 
-void* callMemberFunc(void *obj){
-  std::cerr<<"[FullbodyInverseKinematicsSolverMT] callMemberFunc"<<std::endl;
-  FullbodyInverseKinematicsSolverMT* myClass = reinterpret_cast<FullbodyInverseKinematicsSolverMT*>(obj);
-  myClass->ik_loop();
-  std::cerr<<"[FullbodyInverseKinematicsSolverMT] callMemberFunc finished"<<std::endl;
-  return NULL;
-}
+//void* callMemberFunc(void *obj){
+//  std::cerr<<"[FullbodyInverseKinematicsSolverMT] callMemberFunc"<<std::endl;
+//  FullbodyInverseKinematicsSolverMT* myClass = reinterpret_cast<FullbodyInverseKinematicsSolverMT*>(obj);
+//  myClass->ik_loop();
+//  std::cerr<<"[FullbodyInverseKinematicsSolverMT] callMemberFunc finished"<<std::endl;
+//  return NULL;
+//}
 
 WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager),
       m_qRefIn("qRef", m_qRef),// from sh
@@ -280,7 +280,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     }
 
     if ( is_legged_robot ) {
-
       processTransition();
       mode.update();
       if(DEBUGP)dbg(mode.now());
@@ -361,6 +360,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
       hsp->baselinkpose.p = m_robot->rootLink()->p;
       hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
     }
+
     // write
     m_qOut.write();
     m_basePosOut.write();
@@ -478,8 +478,8 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
     body_list[i]->rootLink()->p = basePos_heightChecked;
     body_list[i]->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
     for ( int j = 0; j < body_list[i]->numJoints(); j++ ){ body_list[i]->joint(j)->q = m_qRef.data[j]; }
-    if( body_list[i]->link("RARM_JOINT2") != NULL) body_list[i]->link("RARM_JOINT2")->ulimit = -30 * D2R;//脇の干渉回避のため
-    if( body_list[i]->link("LARM_JOINT2") != NULL) body_list[i]->link("LARM_JOINT2")->llimit = 30 * D2R;
+    if( body_list[i]->link("RARM_JOINT2") != NULL) body_list[i]->link("RARM_JOINT2")->ulimit = -40 * D2R;//脇の干渉回避のため
+    if( body_list[i]->link("LARM_JOINT2") != NULL) body_list[i]->link("LARM_JOINT2")->llimit = 40 * D2R;
     body_list[i]->calcForwardKinematics();
   }
   for(int i=0;i<fik_list.size();i++){//初期姿勢でBodyをFK
@@ -505,13 +505,7 @@ void WholeBodyMasterSlave::processWholeBodyMasterSlave(fikPtr& fik_in, hrp::Body
   hsp->update();//////HumanSynchronizerの主要処理
   if(DEBUGP)cout<<"update():"<<hsp->getUpdateTime()<<endl;
   if(DEBUGP)pose_ref.print();
-//  WBMSPose3D rf_checked = pose_ref.tgt[rf].abs;
-//  WBMSPose3D lf_checked = pose_ref.tgt[lf].abs;
-//  WBMSPose3D rh_checked = pose_ref.tgt[rh].abs;
-//  WBMSPose3D lh_checked = pose_ref.tgt[lh].abs;
-//  solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, rf_checked, lf_checked, rh_checked, lh_checked, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave");
-//  solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave");
-  solveFullbodyIKStrictCOM(fik_in, robot_in, hsp->rp_ref_out.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave");
+  solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave");
 }
 
 
@@ -557,7 +551,6 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   std::vector<IKConstraintParam> ik_tgt_list;
   IKConstraintParam tmp;
 
-  std::cerr <<"solveFullbodyIKStrictCOM "<<std::endl;
   tmp.target_link_name = "WAIST";
   tmp.localPos = hrp::Vector3::Zero();
   tmp.localR = hrp::Matrix33::Identity();
@@ -610,7 +603,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.target_r = hrp::Matrix33::Identity();//reference angular momentum
   tmp.selection_vec << 1,1,1,0,0,0; // COM pos + Ang Momentum
 //  tmp.selection_vec << 1,1,1,1,1,0; // COM pos + Ang Momentum
-  tmp.weight_vec << 3,3,1,0.1,0.1,0.1;
+  tmp.weight_vec << 3,3,3,0.1,0.1,0.1;
   ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = "HEAD_JOINT1";
@@ -619,23 +612,21 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
 
-  std::cerr <<"com_ref.p "<<com_ref.p.transpose()<<std::endl;
-//  for ( int i=0; i<ik_tgt_list.size(); i++ ) {
-//    std::cerr <<"target_p_pre "<<ik_tgt_list[i].target_p.transpose()<<std::endl;
-//  }
-
-  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT0")->jointId) = 0.01;
-  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT1")->jointId) = 0.01;
-  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT2")->jointId) = 0.01;
-//  fik_in->optional_weight_vector.tail(3).fill(0.01);
+  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;
+  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
+  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
+  fik_in->optional_weight_vector.tail(6).fill(0.01);
   fik_in->reference_q.head(robot_in->numJoints()) <<
       0.0,0.0,-0.349066,0.698132,-0.349066,0.0,
       0.0,0.0,-0.349066,0.698132,-0.349066,0.0,
       0.0,0.0,0.0, 0.0,0.0,
-      0.0,0.698132,-0.349066,-0.087266,-1.39626,0.0,0.0,-0.349066,
-      0.0,0.698132,0.349066,0.087266,-1.39626,0.0,0.0,-0.349066;
+      0.0,0.698132,-0.349066 ,-0.087266,-1.39626,0.0,0.0,-0.349066,
+      0.0,0.698132,0.349066 ,0.087266,-1.39626,0.0,0.0,-0.349066;
   fik_in->reference_q.tail(6) << 0.0,0.0,1.1, 0.0,0.0,0.0;
-  fik_in->reference_gain.fill(0.01);
+  fik_in->reference_gain.fill(0.001);
+//  fik_in->reference_gain(robot_in->link("RARM_JOINT2")->jointId) = 0.1;
+//  fik_in->reference_gain(robot_in->link("LARM_JOINT2")->jointId) = 0.1;
+  fik_in->reference_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
 
   struct timespec startT, endT;
   int loop_result = 0;
@@ -643,7 +634,6 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   clock_gettime(CLOCK_REALTIME, &startT);
   loop_result = fik_in->solveFullbodyIKLoopMT(ik_tgt_list, IK_MAX_LOOP);
   if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveIK"<<loop_result<<"loop" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
-  std::cerr <<"solveFullbodyIKStrictCOM fin"<<std::endl;
 }
 #else
 //旧ver
@@ -708,7 +698,7 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
 //  const double avg_q_vel = 1.0;
   const double avg_q_vel = 2.0;
   for(int i=0;i<robot_in->numJoints();i++){
-    double tmp_time = (robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
+    double tmp_time = fabs(robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
     if(tmp_time > goal_time){ goal_time = tmp_time; }
   }
   q_ip->setGoal(goal_q, goal_time + min_goal_time_offset, true);
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index ae94d735e2f..1bfcb3b1326 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -256,11 +256,10 @@ class WBMSCore : UTIL_CONST {
           rp_ref_out.tgt[human_l_names[i]].offs.p = fik_in->getEndEffectorPos(robot_l_names[i]);
           rp_ref_out.tgt[human_l_names[i]].offs.rpy = hrp::rpyFromRot(fik_in->getEndEffectorRot(robot_l_names[i]));
           rp_ref_out.tgt[human_l_names[i]].abs = rp_ref_out.tgt[human_l_names[i]].offs;
-//          ee_pose_old[i] = rp_ref_out.tgt[human_l_names[i]].abs;
         }
       }
       rp_ref_out.tgt[com].offs.p = robot_in->calcCM();
-//      std::cerr <<"init com "<<robot_in->calcCM().transpose()<<std::endl;
+      com_CP_ref_old = rp_ref_out.tgt[com].offs.p;
       rp_ref_out.tgt[com].offs.rpy = hrp::rpyFromRot(robot_in->rootLink()->R);
       rp_ref_out.tgt[zmp].offs.p(X) = rp_ref_out.tgt[com].offs.p(X);
       rp_ref_out.tgt[zmp].offs.p(Y) = rp_ref_out.tgt[com].offs.p(Y);
@@ -272,8 +271,7 @@ class WBMSCore : UTIL_CONST {
       for(int i=0, l[LR]={rf,lf}; i<LR; i++){
         rp_ref_out.tgt[l[i]].is_contact = true;
       }
-//      std::cerr <<"init com2 "<<rp_ref_out.tgt[com].offs.p.transpose()<<std::endl;
-//      std::cerr <<"init com2 "<<rp_ref_out.tgt[com].abs.p.transpose()<<std::endl;
+      H_cur = rp_ref_out.tgt[com].offs.p(Z) - std::min((double)rp_ref_out.tgt[rf].offs.p(Z), (double)rp_ref_out.tgt[rf].offs.p(Z));
     }
     void initializeRequest(fikPtr& fik_in, hrp::BodyPtr& robot_in){
       loop = 0;
@@ -299,7 +297,6 @@ class WBMSCore : UTIL_CONST {
 
       limitManipulability                 (rp_ref_out);
 
-//      std::cerr <<"out com "<<rp_ref_out.tgt[com].abs.p.transpose()<<std::endl;
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
 
       Vector3ToVector2(rp_ref_out.tgt[com].abs.p,com_forcp_ref);
@@ -307,21 +304,17 @@ class WBMSCore : UTIL_CONST {
       com_vel_forcp_ref = (com_forcp_ref - com_forcp_ref_old)/DT;
       com_forcp_ref_old = com_forcp_ref;
 
-//      std::cerr <<"out com "<<rp_ref_out.tgt[com].abs.p.transpose()<<std::endl;
-
       applyLPFilter_pre                       (rp_ref_out);
       applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
       applyLPFilter_post                       (rp_ref_out);
 
-//      std::cerr <<"out com "<<rp_ref_out.tgt[com].abs.p.transpose()<<std::endl;
       r_zmp_raw = rp_ref_out.tgt[zmp].abs.p;
       applyZMPCalcFromCOM                 (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);
       if(DEBUG){
         fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.tgt[zmp].abs.p(X),rp_ref_out.tgt[zmp].abs.p(Y));
         fprintf(cz_log,"\n");
       }
-//      std::cerr <<"out com "<<rp_ref_out.tgt[com].abs.p.transpose()<<std::endl;
       overwriteFootZFromFootLandOnCommand (rp_ref_out);
       modifyFootRotAndXYForContact        (rp_ref_out);
 
@@ -329,7 +322,6 @@ class WBMSCore : UTIL_CONST {
       com_vel_old = (rp_ref_out.tgt[com].abs.p - rp_ref_out_old.tgt[com].abs.p)/DT;
       rh_vel_old = (rp_ref_out.tgt[rh].abs.p - rp_ref_out_old.tgt[rh].abs.p)/DT;
       rp_ref_out_old = rp_ref_out;
-//      std::cerr <<"out com "<<rp_ref_out.tgt[com].abs.p.transpose()<<std::endl;
       loop++;
       is_initial_loop = false;
       gettimeofday(&t_calc_end, NULL);
@@ -644,7 +636,6 @@ class WBMSCore : UTIL_CONST {
     void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull_d, const std::vector<hrp::Vector2>& hull_a, hrp::Vector2& com_vel_ans){
       hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
       com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
-
       //減速CP条件(現在のCPを常に両足裏で頭打ち)
       hrp::Vector2 cp_dec_tmp = com_pos + com_vel * sqrt( H_cur / G );
       hrp::Vector2 cp_dec_ragulated;

From fa12862ac9a32c86863003bb0f653892ed6d9c3a Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 21 Aug 2017 23:15:40 +0900
Subject: [PATCH 116/217] tune for rhp3

---
 python/hrpsys_config.py                       |   8 +-
 .../WholeBodyMasterSlave.cpp                  | 129 ++++++++++--------
 .../WholeBodyMasterSlave.h                    |   4 +-
 3 files changed, 76 insertions(+), 65 deletions(-)

diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index b887574adbe..a347c3bbd9e 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -403,7 +403,7 @@ def connectComps(self):
 #            connectPorts(self.sh.port("optionalDataOut"), self.abc.port("optionalData"))
             #### wbms
             if self.wbms:
-                connectPorts(self.wbms.port("q"), self.abc.port("qRef"))
+                connectPorts(self.wbms.port("q"), self.ic.port("qRef"))
                 connectPorts(self.wbms.port("zmpOut"), self.abc.port("zmpIn"))
                 connectPorts(self.wbms.port("basePosOut"), self.abc.port("basePosIn"))
                 connectPorts(self.wbms.port("baseRpyOut"), self.abc.port("baseRpyIn"))
@@ -771,8 +771,8 @@ def getRTCListUnstable(self):
             ['octd', "ObjectContactTurnaroundDetector"],
             ['es', "EmergencyStopper"],
             ['rfu', "ReferenceForceUpdater"],
-            ['ic', "ImpedanceController"],
             ['wbms', "WholeBodyMasterSlave"],
+            ['ic', "ImpedanceController"],
             ['abc', "AutoBalancer"],
             ['st', "Stabilizer"],
             ['co', "CollisionDetector"],
@@ -790,8 +790,8 @@ def getJointAngleControllerList(self):
         '''!@brief
         Get list of controller list that need to control joint angles
         '''
-        controller_list = [self.es, self.ic, self.gc, self.abc, self.st, self.co,
-                           self.tc, self.hes, self.el]
+#        controller_list = [self.es, self.ic, self.gc, self.abc, self.st, self.co, self.tc, self.hes, self.el]
+        controller_list = [self.es, self.wbms, self.ic, self.gc, self.abc, self.st, self.co, self.tc, self.hes, self.el]
         return filter(lambda c: c != None, controller_list)  # only return existing controllers
 
     def getRTCInstanceList(self, verbose=True):
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 372abc10cc2..4a0f2eb219e 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -174,8 +174,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     transition_interpolator->set(&tmp_ratio);
     tmp_ratio = 1.0;
     transition_interpolator->setGoal(&tmp_ratio, 3.0, true);
-    q_ip_dim = m_robot->numJoints() + 3 + 3*3;// joints + base_p + base_R
-    q_ip = new interpolator(q_ip_dim, m_dt, interpolator::HOFFARBIB, 1);
+    ROBOT_ALL_DOF = m_robot->numJoints() + 3 + 3;// joints + base_p + base_rpy
+    q_ip = new interpolator(ROBOT_ALL_DOF, m_dt, interpolator::HOFFARBIB, 1);
     q_ip->setName(std::string(m_profile.instance_name)+" q_ip");
     q_ip->clear();
     // Generate FIK
@@ -252,6 +252,12 @@ void WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC:
       std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
       std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
       contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));////TODO:要移動?
+
+      eename_ikcp_map[ee_name].target_link_name = ee_target;
+      for (size_t j = 0; j < XYZ; j++){ coil::stringTo(eename_ikcp_map[ee_name].localPos(j), end_effectors_str[i*prop_num+3+j].c_str()); }
+      double tmp[4];
+      for (int j = 0; j < 4; j++ ){ coil::stringTo(tmp[j], end_effectors_str[i*prop_num+6+j].c_str()); }
+      eename_ikcp_map[ee_name].localR = Eigen::AngleAxis<double>(tmp[3], hrp::Vector3(tmp[0], tmp[1], tmp[2])).toRotationMatrix();
     }
   }
 }
@@ -327,6 +333,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         processHOFFARBIBFilter(m_robot, m_robot_vsafe);
 
         const hrp::BodyPtr m_robot_for_out = m_robot_vsafe;
+//        const hrp::BodyPtr m_robot_for_out = m_robot;
 
         //OutPortデータセット
         hrp::Vector3 ref_zmp = hsp->rp_ref_out.tgt[zmp].abs.p;
@@ -492,11 +499,11 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
     }
   }
   q_ip->clear();
-  double init_q[q_ip_dim];
-  for(int i=0;i<robot_in->numJoints();i++){ init_q[i] = robot_in->joint(i)->q; }
-  for(int i=0;i<3;i++){ init_q[robot_in->numJoints()+i] = robot_in->rootLink()->p(i); }
-  for(int i=0;i<3;i++){for(int j=0;j<3;j++){ init_q[robot_in->numJoints() + 3 + 3*i + j] = robot_in->rootLink()->R(i,j); }}
-  q_ip->set(init_q);
+  init_sync_state.resize(ROBOT_ALL_DOF);
+  for(int i=0;i<robot_in->numJoints();i++){ init_sync_state(i) = robot_in->joint(i)->q; }
+  init_sync_state.bottomRows(6).topRows(3) = robot_in->rootLink()->p;
+  init_sync_state.bottomRows(6).bottomRows(3) = hrp::rpyFromRot(robot_in->rootLink()->R);
+  q_ip->set(init_sync_state.data());
   hsp->initializeRequest(fik_in, robot_in);
 }
 
@@ -560,41 +567,41 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
 
-  tmp.target_link_name = "RLEG_JOINT5";
-  tmp.localPos = fik_in->ikp["rleg"].localPos;
-  tmp.localR = fik_in->ikp["rleg"].localR;
+  tmp.target_link_name = eename_ikcp_map["rleg"].target_link_name;
+  tmp.localPos = eename_ikcp_map["rleg"].localPos;
+  tmp.localR = eename_ikcp_map["rleg"].localR;
   tmp.target_p = rf_ref.p;
   tmp.target_r = hrp::rotFromRpy(rf_ref.rpy);
   tmp.selection_vec << 1,1,1,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
 
-  tmp.target_link_name = "LLEG_JOINT5";
-  tmp.localPos = fik_in->ikp["lleg"].localPos;
-  tmp.localR = fik_in->ikp["lleg"].localR;
+  tmp.target_link_name = eename_ikcp_map["lleg"].target_link_name;
+  tmp.localPos = eename_ikcp_map["lleg"].localPos;
+  tmp.localR = eename_ikcp_map["lleg"].localR;
   tmp.target_p = lf_ref.p;
   tmp.target_r = hrp::rotFromRpy(lf_ref.rpy);
   tmp.selection_vec << 1,1,1,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
 
-  tmp.target_link_name = "RARM_JOINT7";
-  tmp.localPos = fik_in->ikp["rarm"].localPos;
-  tmp.localR = fik_in->ikp["rarm"].localR;
-  tmp.target_p = rh_ref.p;
-  tmp.target_r = hrp::rotFromRpy(rh_ref.rpy);
-  tmp.selection_vec << 1,1,1,1,1,1;
-  tmp.weight_vec << 1,1,1,1,1,1;
-  ik_tgt_list.push_back(tmp);
-
-  tmp.target_link_name = "LARM_JOINT7";
-  tmp.localPos = fik_in->ikp["larm"].localPos;
-  tmp.localR = fik_in->ikp["larm"].localR;
-  tmp.target_p = lh_ref.p;
-  tmp.target_r = hrp::rotFromRpy(lh_ref.rpy);
-  tmp.selection_vec << 1,1,1,1,1,1;
-  tmp.weight_vec << 1,1,1,1,1,1;
-  ik_tgt_list.push_back(tmp);
+//  tmp.target_link_name = eename_ikcp_map["rarm"].target_link_name;
+//  tmp.localPos = eename_ikcp_map["rarm"].localPos;
+//  tmp.localR = eename_ikcp_map["rarm"].localR;
+//  tmp.target_p = rh_ref.p;
+//  tmp.target_r = hrp::rotFromRpy(rh_ref.rpy);
+//  tmp.selection_vec << 1,1,1,1,1,1;
+//  tmp.weight_vec << 1,1,1,1,1,1;
+//  ik_tgt_list.push_back(tmp);
+//
+//  tmp.target_link_name = eename_ikcp_map["larm"].target_link_name;
+//  tmp.localPos = eename_ikcp_map["larm"].localPos;
+//  tmp.localR = eename_ikcp_map["larm"].localR;
+//  tmp.target_p = lh_ref.p;
+//  tmp.target_r = hrp::rotFromRpy(lh_ref.rpy);
+//  tmp.selection_vec << 1,1,1,1,1,1;
+//  tmp.weight_vec << 1,1,1,1,1,1;
+//  ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = "COM";
   tmp.localPos = hrp::Vector3::Zero();
@@ -606,27 +613,28 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.weight_vec << 3,3,3,0.1,0.1,0.1;
   ik_tgt_list.push_back(tmp);
 
-  tmp.target_link_name = "HEAD_JOINT1";
-  tmp.target_r = hrp::rotFromRpy(head_ref.rpy);
-  tmp.selection_vec << 0,0,0,0,1,1;
-  tmp.weight_vec << 1,1,1,1,1,1;
-  ik_tgt_list.push_back(tmp);
+//  tmp.target_link_name = "HEAD_JOINT1";
+//  tmp.target_link_name = "HEAD_P";
+//  tmp.target_r = hrp::rotFromRpy(head_ref.rpy);
+//  tmp.selection_vec << 0,0,0,0,1,1;
+//  tmp.weight_vec << 1,1,1,1,1,1;
+//  ik_tgt_list.push_back(tmp);
 
-  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;
-  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
-  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
+//  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;
+//  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
+//  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
   fik_in->optional_weight_vector.tail(6).fill(0.01);
-  fik_in->reference_q.head(robot_in->numJoints()) <<
-      0.0,0.0,-0.349066,0.698132,-0.349066,0.0,
-      0.0,0.0,-0.349066,0.698132,-0.349066,0.0,
-      0.0,0.0,0.0, 0.0,0.0,
-      0.0,0.698132,-0.349066 ,-0.087266,-1.39626,0.0,0.0,-0.349066,
-      0.0,0.698132,0.349066 ,0.087266,-1.39626,0.0,0.0,-0.349066;
-  fik_in->reference_q.tail(6) << 0.0,0.0,1.1, 0.0,0.0,0.0;
-  fik_in->reference_gain.fill(0.001);
+//  fik_in->reference_q.head(robot_in->numJoints()) <<
+//      0.0,0.0,-0.349066,0.698132,-0.349066,0.0,
+//      0.0,0.0,-0.349066,0.698132,-0.349066,0.0,
+//      0.0,0.0,0.0, 0.0,0.0,
+//      0.0,0.698132,-0.349066 ,-0.087266,-1.39626,0.0,0.0,-0.349066,
+//      0.0,0.698132,0.349066 ,0.087266,-1.39626,0.0,0.0,-0.349066;
+//  fik_in->reference_q.tail(6) << 0.0,0.0,1.1, 0.0,0.0,0.0;
+//  fik_in->reference_gain.fill(0.001);
 //  fik_in->reference_gain(robot_in->link("RARM_JOINT2")->jointId) = 0.1;
 //  fik_in->reference_gain(robot_in->link("LARM_JOINT2")->jointId) = 0.1;
-  fik_in->reference_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
+//  fik_in->reference_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
 
   struct timespec startT, endT;
   int loop_result = 0;
@@ -688,26 +696,27 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
 
 
 void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out){
-  double goal_q[q_ip_dim];
-  for(int i=0;i<robot_in->numJoints();i++){ goal_q[i] = robot_in->joint(i)->q; }
-  for(int i=0;i<3;i++){ goal_q[robot_in->numJoints()+i] = robot_in->rootLink()->p(i); }
-  for(int i=0;i<3;i++){for(int j=0;j<3;j++){ goal_q[robot_in->numJoints() + 3 + 3*i + j] = robot_in->rootLink()->R(i,j); }}
+  hrp::dvector goal_state(ROBOT_ALL_DOF);
+  for(int i=0;i<robot_in->numJoints();i++){ goal_state(i) = robot_in->joint(i)->q; }
+  goal_state.bottomRows(6).topRows(3) = robot_in->rootLink()->p;
+  goal_state.bottomRows(6).bottomRows(3) = hrp::rpyFromRot(robot_in->rootLink()->R);
 
   double goal_time = 0.0;
   const double min_goal_time_offset = 0.1;
-//  const double avg_q_vel = 1.0;
-  const double avg_q_vel = 2.0;
+  const double avg_q_vel = 1.0;
+//  const double avg_q_vel = 2.0;
   for(int i=0;i<robot_in->numJoints();i++){
     double tmp_time = fabs(robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
     if(tmp_time > goal_time){ goal_time = tmp_time; }
   }
-  q_ip->setGoal(goal_q, goal_time + min_goal_time_offset, true);
-
-  double ans_q[q_ip_dim];
-  if (!q_ip->isEmpty() ){  q_ip->get(ans_q, true); }
-  for(int i=0;i<robot_out->numJoints();i++){ robot_out->joint(i)->q = ans_q[i]; }
-  for(int i=0;i<3;i++){ robot_out->rootLink()->p(i) = ans_q[robot_out->numJoints()+i]; }
-  for(int i=0;i<3;i++){for(int j=0;j<3;j++){ robot_out->rootLink()->R(i,j) = ans_q[robot_out->numJoints() + 3 + 3*i + j]; }}
+  q_ip->setGoal(goal_state.data(), goal_time + min_goal_time_offset, true);
+  hrp::dvector ans_state(ROBOT_ALL_DOF);
+  double tmp[ROBOT_ALL_DOF];
+  if (!q_ip->isEmpty() ){  q_ip->get(tmp, true);}
+  ans_state.Map(tmp, ROBOT_ALL_DOF);
+  for(int i=0;i<robot_out->numJoints();i++){ robot_out->joint(i)->q = ans_state(i); }
+  robot_in->rootLink()->p = ans_state.bottomRows(6).topRows(3);
+  robot_in->rootLink()->R = hrp::rotFromRpy(ans_state.bottomRows(6).bottomRows(3));
 }
 
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 5e57cdc7a94..6f91a0152cb 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -169,13 +169,15 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
   hrp::BodyPtr m_robot, m_robot_ml, m_robot_vsafe;
   std::vector<fikPtr> fik_list;
   std::vector<hrp::BodyPtr> body_list;
+  std::map<std::string, IKConstraintParam> eename_ikcp_map;
   std::map<std::string, size_t> contact_states_index_map;
   double m_dt;
 
   double transition_interpolator_ratio;
   interpolator *transition_interpolator;
-  int q_ip_dim;
+  int ROBOT_ALL_DOF;
   interpolator *q_ip;
+  hrp::dvector init_sync_state;
 
   int optionalDataLength;
   unsigned int m_debugLevel;

From 9e37a9d64b81413d07dbe27ad4e1048bcb3e7014 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 21 Aug 2017 23:54:29 +0900
Subject: [PATCH 117/217] refactor

---
 .../FullbodyInverseKinematicsSolver.h         | 27 +++---
 .../WholeBodyMasterSlave.cpp                  | 97 ++++++++-----------
 2 files changed, 56 insertions(+), 68 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 3a454245819..ece78bfe99b 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -16,10 +16,8 @@
 class IKConstraintParam {
   public:
     std::string target_link_name;
-    // IK target EE coords
-    hrp::Vector3 target_p;
-    hrp::Matrix33 target_r;
-    // EE offset, EE link
+    hrp::Vector3 targetPos;
+    hrp::Matrix33 targetR;
     hrp::Vector3 localPos;
     hrp::Matrix33 localR;
     hrp::dvector6 weight_vec;
@@ -27,8 +25,8 @@ class IKConstraintParam {
     double pos_precision, rot_precision;
 
     IKConstraintParam ()
-      :target_p(hrp::Vector3::Zero()),
-       target_r(hrp::Matrix33::Identity()),
+      :targetPos(hrp::Vector3::Zero()),
+       targetR(hrp::Matrix33::Identity()),
        localPos(hrp::Vector3::Zero()),
        localR(hrp::Matrix33::Identity()),
        weight_vec(hrp::dvector6::Ones()),
@@ -86,15 +84,15 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
       for ( int i=0; i<_ik_tgt_list.size(); i++ ) {
         hrp::Link* target_link_ptr = m_robot->link(_ik_tgt_list[i].target_link_name);
         if(target_link_ptr){
-          hrp::Vector3 pos_err = _ik_tgt_list[i].target_p - (target_link_ptr->p + target_link_ptr->R * _ik_tgt_list[i].localPos);
-          hrp::Vector3 rot_err = omegaFromRotEx(_ik_tgt_list[i].target_r.transpose() * target_link_ptr->R * _ik_tgt_list[i].localR);
+          hrp::Vector3 pos_err = _ik_tgt_list[i].targetPos - (target_link_ptr->p + target_link_ptr->R * _ik_tgt_list[i].localPos);
+          hrp::Vector3 rot_err = omegaFromRotEx(_ik_tgt_list[i].targetR.transpose() * target_link_ptr->R * _ik_tgt_list[i].localR);
           for(int j=0;j<3;j++){
             if(pos_err(j) > _ik_tgt_list[i].pos_precision && _ik_tgt_list[i].selection_vec(j) != 0){return false;}
             if(rot_err(j) > _ik_tgt_list[i].rot_precision && _ik_tgt_list[i].selection_vec(j+3) != 0){return false;}
           }
         }
         else if(!target_link_ptr && _ik_tgt_list[i].target_link_name == "COM"){  // COM
-          hrp::Vector3 pos_err = _ik_tgt_list[i].target_p - (m_robot->calcCM() + _ik_tgt_list[i].localR * _ik_tgt_list[i].localPos);
+          hrp::Vector3 pos_err = _ik_tgt_list[i].targetPos - (m_robot->calcCM() + _ik_tgt_list[i].localR * _ik_tgt_list[i].localPos);
 //            robot_in->calcTotalMomentum(fik_in->cur_P, fik_in->cur_L);
 //            hrp::Vector3 rot_err = omegaFromRotEx(ik_tgt_list[i].target_r.transpose() * ik_tgt_list[i].localR * target_link_ptr->R);
           for(int j=0;j<3;j++){
@@ -120,8 +118,8 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
       //ヤコビアンと各ベクトル生成
       for ( int i=0; i<_ik_tgt_list.size(); i++ ) {
         hrp::Link* target_link_ptr = _robot->link(_ik_tgt_list[i].target_link_name);
-        hrp::Matrix33 ref_link_origin_R(_ik_tgt_list[i].target_r * _ik_tgt_list[i].localR.transpose()); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
-        hrp::Vector3 ref_link_origin_p(_ik_tgt_list[i].target_p - ref_link_origin_R * _ik_tgt_list[i].localPos);
+        hrp::Matrix33 ref_link_origin_R(_ik_tgt_list[i].targetR * _ik_tgt_list[i].localR.transpose()); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
+        hrp::Vector3 ref_link_origin_p(_ik_tgt_list[i].targetPos - ref_link_origin_R * _ik_tgt_list[i].localPos);
         hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF); //全身to一拘束点へのヤコビアン
         hrp::Vector3 vel_p_ref, vel_r_ref; //velと言いつつ実際は差分
         //ベースリンク，通常リンク，重心で場合分け
@@ -177,8 +175,11 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
         std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
         std::cout<<std::setprecision(2) << "J_all_inv=\n"<<J_all_inv<<std::setprecision(6)<<std::endl;
         std::cout<<std::setprecision(2) << "selection_matrix=\n"<<selection_matrix<<std::setprecision(6)<<std::endl;
-        std::cout<<"eevel_all\n"<<dp_ee_all<<std::endl;
-        std::cout<<"dq_all\n"<<dq_all<<std::endl;
+        std::cout<<"eevel_all\n"<<dp_ee_all.transpose()<<std::endl;
+        std::cout<<"dq_all\n"<<dq_all.transpose()<<std::endl;
+        std::cout<<"q_ans_all\n";
+        for(int i=0;i<_robot->numJoints();i++)std::cerr<<_robot->joint(i)->q<<" ";
+        std::cout<<std::endl;
       }
 
       const double LAMBDA = 1.0;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 4a0f2eb219e..0446724eea0 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -37,14 +37,6 @@ static const char* WholeBodyMasterSlave_spec[] =
 //    return os;
 //}
 
-//void* callMemberFunc(void *obj){
-//  std::cerr<<"[FullbodyInverseKinematicsSolverMT] callMemberFunc"<<std::endl;
-//  FullbodyInverseKinematicsSolverMT* myClass = reinterpret_cast<FullbodyInverseKinematicsSolverMT*>(obj);
-//  myClass->ik_loop();
-//  std::cerr<<"[FullbodyInverseKinematicsSolverMT] callMemberFunc finished"<<std::endl;
-//  return NULL;
-//}
-
 WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager),
       m_qRefIn("qRef", m_qRef),// from sh
       m_zmpIn("zmpIn", m_zmp),
@@ -561,8 +553,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.target_link_name = "WAIST";
   tmp.localPos = hrp::Vector3::Zero();
   tmp.localR = hrp::Matrix33::Identity();
-  tmp.target_p = robot_in->rootLink()->p;// will be ignored by selection_vec
-  tmp.target_r = hrp::rotFromRpy(com_ref.rpy);// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
+  tmp.targetPos = robot_in->rootLink()->p;// will be ignored by selection_vec
+  tmp.targetR = hrp::rotFromRpy(com_ref.rpy);// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
   tmp.selection_vec << 0,0,0,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
@@ -570,8 +562,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.target_link_name = eename_ikcp_map["rleg"].target_link_name;
   tmp.localPos = eename_ikcp_map["rleg"].localPos;
   tmp.localR = eename_ikcp_map["rleg"].localR;
-  tmp.target_p = rf_ref.p;
-  tmp.target_r = hrp::rotFromRpy(rf_ref.rpy);
+  tmp.targetPos = rf_ref.p;
+  tmp.targetR = hrp::rotFromRpy(rf_ref.rpy);
   tmp.selection_vec << 1,1,1,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
@@ -579,61 +571,56 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.target_link_name = eename_ikcp_map["lleg"].target_link_name;
   tmp.localPos = eename_ikcp_map["lleg"].localPos;
   tmp.localR = eename_ikcp_map["lleg"].localR;
-  tmp.target_p = lf_ref.p;
-  tmp.target_r = hrp::rotFromRpy(lf_ref.rpy);
+  tmp.targetPos = lf_ref.p;
+  tmp.targetR = hrp::rotFromRpy(lf_ref.rpy);
   tmp.selection_vec << 1,1,1,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
 
-//  tmp.target_link_name = eename_ikcp_map["rarm"].target_link_name;
-//  tmp.localPos = eename_ikcp_map["rarm"].localPos;
-//  tmp.localR = eename_ikcp_map["rarm"].localR;
-//  tmp.target_p = rh_ref.p;
-//  tmp.target_r = hrp::rotFromRpy(rh_ref.rpy);
-//  tmp.selection_vec << 1,1,1,1,1,1;
-//  tmp.weight_vec << 1,1,1,1,1,1;
-//  ik_tgt_list.push_back(tmp);
-//
-//  tmp.target_link_name = eename_ikcp_map["larm"].target_link_name;
-//  tmp.localPos = eename_ikcp_map["larm"].localPos;
-//  tmp.localR = eename_ikcp_map["larm"].localR;
-//  tmp.target_p = lh_ref.p;
-//  tmp.target_r = hrp::rotFromRpy(lh_ref.rpy);
-//  tmp.selection_vec << 1,1,1,1,1,1;
-//  tmp.weight_vec << 1,1,1,1,1,1;
-//  ik_tgt_list.push_back(tmp);
+  tmp.target_link_name = eename_ikcp_map["rarm"].target_link_name;
+  tmp.localPos = eename_ikcp_map["rarm"].localPos;
+  tmp.localR = eename_ikcp_map["rarm"].localR;
+  tmp.targetPos = rh_ref.p;
+  tmp.targetR = hrp::rotFromRpy(rh_ref.rpy);
+  tmp.selection_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 1,1,1,1,1,1;
+  ik_tgt_list.push_back(tmp);
+
+  tmp.target_link_name = eename_ikcp_map["larm"].target_link_name;
+  tmp.localPos = eename_ikcp_map["larm"].localPos;
+  tmp.localR = eename_ikcp_map["larm"].localR;
+  tmp.targetPos = lh_ref.p;
+  tmp.targetR = hrp::rotFromRpy(lh_ref.rpy);
+  tmp.selection_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 1,1,1,1,1,1;
+  ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = "COM";
   tmp.localPos = hrp::Vector3::Zero();
   tmp.localR = hrp::Matrix33::Identity();
-  tmp.target_p = com_ref.p;// COM height will not be constraint
-  tmp.target_r = hrp::Matrix33::Identity();//reference angular momentum
+  tmp.targetPos = com_ref.p;// COM height will not be constraint
+  tmp.targetR = hrp::Matrix33::Identity();//reference angular momentum
   tmp.selection_vec << 1,1,1,0,0,0; // COM pos + Ang Momentum
 //  tmp.selection_vec << 1,1,1,1,1,0; // COM pos + Ang Momentum
   tmp.weight_vec << 3,3,3,0.1,0.1,0.1;
   ik_tgt_list.push_back(tmp);
 
-//  tmp.target_link_name = "HEAD_JOINT1";
-//  tmp.target_link_name = "HEAD_P";
-//  tmp.target_r = hrp::rotFromRpy(head_ref.rpy);
-//  tmp.selection_vec << 0,0,0,0,1,1;
-//  tmp.weight_vec << 1,1,1,1,1,1;
-//  ik_tgt_list.push_back(tmp);
+  if(robot_in->link("HEAD_JOINT1") != NULL){
+    tmp.target_link_name = "HEAD_JOINT1";
+    tmp.target_link_name = "HEAD_P";
+    tmp.targetR = hrp::rotFromRpy(head_ref.rpy);
+    tmp.selection_vec << 0,0,0,0,1,1;
+    tmp.weight_vec << 1,1,1,1,1,1;
+    ik_tgt_list.push_back(tmp);
+  }
 
 //  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;
 //  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
 //  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
   fik_in->optional_weight_vector.tail(6).fill(0.01);
-//  fik_in->reference_q.head(robot_in->numJoints()) <<
-//      0.0,0.0,-0.349066,0.698132,-0.349066,0.0,
-//      0.0,0.0,-0.349066,0.698132,-0.349066,0.0,
-//      0.0,0.0,0.0, 0.0,0.0,
-//      0.0,0.698132,-0.349066 ,-0.087266,-1.39626,0.0,0.0,-0.349066,
-//      0.0,0.698132,0.349066 ,0.087266,-1.39626,0.0,0.0,-0.349066;
-//  fik_in->reference_q.tail(6) << 0.0,0.0,1.1, 0.0,0.0,0.0;
+
+  fik_in->reference_q = init_sync_state;
 //  fik_in->reference_gain.fill(0.001);
-//  fik_in->reference_gain(robot_in->link("RARM_JOINT2")->jointId) = 0.1;
-//  fik_in->reference_gain(robot_in->link("LARM_JOINT2")->jointId) = 0.1;
 //  fik_in->reference_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
 
   struct timespec startT, endT;
@@ -655,8 +642,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   robot_in->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
   for(int i=0;i<4;i++){
     if(fik_in->ikp.count(names[i])){
-      fik_in->ikp[names[i]].target_p = refs[i]->p;
-      fik_in->ikp[names[i]].target_r = hrp::rotFromRpy(refs[i]->rpy);
+      fik_in->ikp[names[i]].targetPos = refs[i]->p;
+      fik_in->ikp[names[i]].targetR = hrp::rotFromRpy(refs[i]->rpy);
     }
   }
 //  fik_in->storeCurrentParameters();
@@ -664,8 +651,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   if( robot_in->link("HEAD_JOINT0") != NULL) robot_in->link("HEAD_JOINT0")->q = head_ref.rpy(y);
   if( robot_in->link("HEAD_JOINT1") != NULL) robot_in->link("HEAD_JOINT1")->q = head_ref.rpy(p);
   if(fik_in->ikp.count("rarm") && fik_in->ikp.count("larm")){
-    hrp::Vector3 base2rh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["rarm"].target_p - robot_in->rootLink()->p);
-    hrp::Vector3 base2lh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["larm"].target_p - robot_in->rootLink()->p);
+    hrp::Vector3 base2rh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["rarm"].targetPos - robot_in->rootLink()->p);
+    hrp::Vector3 base2lh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["larm"].targetPos - robot_in->rootLink()->p);
     if( robot_in->link("CHEST_JOINT0") != NULL){
       robot_in->link("CHEST_JOINT0")->q = (base2lh(Z) - base2rh(Z)) * (10 * D2R / 1.0);
       LIMIT_MINMAX(robot_in->link("CHEST_JOINT0")->q, robot_in->link("CHEST_JOINT0")->llimit, robot_in->link("CHEST_JOINT0")->ulimit);
@@ -713,10 +700,10 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
   hrp::dvector ans_state(ROBOT_ALL_DOF);
   double tmp[ROBOT_ALL_DOF];
   if (!q_ip->isEmpty() ){  q_ip->get(tmp, true);}
-  ans_state.Map(tmp, ROBOT_ALL_DOF);
+  ans_state = Eigen::Map<hrp::dvector>(tmp, ROBOT_ALL_DOF);
   for(int i=0;i<robot_out->numJoints();i++){ robot_out->joint(i)->q = ans_state(i); }
-  robot_in->rootLink()->p = ans_state.bottomRows(6).topRows(3);
-  robot_in->rootLink()->R = hrp::rotFromRpy(ans_state.bottomRows(6).bottomRows(3));
+  robot_out->rootLink()->p = ans_state.bottomRows(6).topRows(3);
+  robot_out->rootLink()->R = hrp::rotFromRpy(ans_state.bottomRows(6).bottomRows(3));
 }
 
 

From 26b0415eea12a40792c00b79eb9ccbb0d6c49193 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 22 Aug 2017 23:40:39 +0900
Subject: [PATCH 118/217] change target R -> rpy and tune abc

---
 rtc/AutoBalancer/AutoBalancer.cpp             | 176 ++++++++----------
 rtc/AutoBalancer/AutoBalancer.h               |   9 +-
 .../FullbodyInverseKinematicsSolver.h         |  15 +-
 .../WholeBodyMasterSlave.cpp                  |  16 +-
 4 files changed, 96 insertions(+), 120 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 00067a22af6..f80d11f5d7a 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -173,7 +173,8 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     leg_names.push_back("lleg");
 
     // Generate FIK
-    fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
+//    fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
+    fik = fikPtr(new FullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
 
     // setting from conf file
     // rleg,TARGET_LINK,BASE_LINK
@@ -1073,133 +1074,106 @@ void AutoBalancer::fixLegToCoords2 (coordinates& tmp_fix_coords)
     fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot);
 }
 
-//#define USE_NEW_IK
-#ifdef USE_NEW_IK
 void AutoBalancer::solveFullbodyIK ()
 {
-  const int IK_MAX_LOOP = 3;
-  fik->ik_tgt_list.clear();
+  std::vector<IKConstraintParam> ik_tgt_list;
   IKConstraintParam tmp;
 
   tmp.target_link_name = "WAIST";
   tmp.localPos = hrp::Vector3::Zero();
   tmp.localR = hrp::Matrix33::Identity();
-  tmp.target_p = target_root_p;// will be ignored by selection_vec
-  tmp.target_r = target_root_R;
-  tmp.selection_vec << 0,0,0,1,1,0;
+  tmp.targetPos = target_root_p;// will be ignored by selection_vec
+  tmp.targetRpy = hrp::rpyFromRot(target_root_R);// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
+  tmp.selection_vec << 0,0,0,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
-  fik->ik_tgt_list.push_back(tmp);
+  ik_tgt_list.push_back(tmp);
 
-  tmp.target_link_name = "RLEG_JOINT5";
+  tmp.target_link_name = ikp["rleg"].target_link->name;
   tmp.localPos = ikp["rleg"].localPos;
   tmp.localR = ikp["rleg"].localR;
-  tmp.target_p = ikp["rleg"].target_p0;
-  tmp.target_r = ikp["rleg"].target_r0;
+  tmp.targetPos = ikp["rleg"].target_p0;
+  tmp.targetRpy = hrp::rpyFromRot(ikp["rleg"].target_r0);
   tmp.selection_vec << 1,1,1,1,1,1;
-  tmp.weight_vec << 1,1,3,1,1,1;
-  fik->ik_tgt_list.push_back(tmp);
+  tmp.weight_vec << 1,1,1,1,1,1;
+  ik_tgt_list.push_back(tmp);
 
-  tmp.target_link_name = "LLEG_JOINT5";
+  tmp.target_link_name = ikp["lleg"].target_link->name;
   tmp.localPos = ikp["lleg"].localPos;
   tmp.localR = ikp["lleg"].localR;
-  tmp.target_p = ikp["lleg"].target_p0;
-  tmp.target_r = ikp["lleg"].target_r0;
-  tmp.selection_vec << 1,1,1,1,1,1;
-  tmp.weight_vec << 1,1,3,1,1,1;
-  fik->ik_tgt_list.push_back(tmp);
-
-  tmp.target_link_name = "RARM_JOINT7";
-  tmp.localPos = ikp["rarm"].localPos;
-  tmp.localR = ikp["rarm"].localR;
-  tmp.target_p = ikp["rarm"].target_p0;
-  tmp.target_r = ikp["rarm"].target_r0;
-  tmp.selection_vec << 1,1,1,1,1,1;
-  tmp.weight_vec << 1,1,1,1,1,1;
-  fik->ik_tgt_list.push_back(tmp);
-
-  tmp.target_link_name = "LARM_JOINT7";
-  tmp.localPos = ikp["larm"].localPos;
-  tmp.localR = ikp["larm"].localR;
-  tmp.target_p = ikp["larm"].target_p0;
-  tmp.target_r = ikp["larm"].target_r0;
+  tmp.targetPos = ikp["lleg"].target_p0;
+  tmp.targetRpy = hrp::rpyFromRot(ikp["lleg"].target_r0);
   tmp.selection_vec << 1,1,1,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
-  fik->ik_tgt_list.push_back(tmp);
+  ik_tgt_list.push_back(tmp);
+
+//  tmp.target_link_name = ikp["rarm"].target_link->name;
+//  tmp.localPos = ikp["rarm"].localPos;
+//  tmp.localR = ikp["rarm"].localR;
+//  tmp.targetPos = ikp["rarm"].target_p0;
+//  tmp.targetRpy = hrp::rpyFromRot(ikp["rarm"].target_r0);
+//  tmp.selection_vec << 1,1,1,1,1,1;
+//  tmp.weight_vec << 1,1,1,1,1,1;
+//  ik_tgt_list.push_back(tmp);
+//
+//  tmp.target_link_name = ikp["larm"].target_link->name;
+//  tmp.localPos = ikp["larm"].localPos;
+//  tmp.localR = ikp["larm"].localR;
+//  tmp.targetPos = ikp["larm"].target_p0;
+//  tmp.targetRpy = hrp::rpyFromRot(ikp["larm"].target_r0);
+//  tmp.selection_vec << 1,1,1,1,1,1;
+//  tmp.weight_vec << 1,1,1,1,1,1;
+//  ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = "COM";
   tmp.localPos = hrp::Vector3::Zero();
   tmp.localR = hrp::Matrix33::Identity();
-  tmp.target_p = ref_cog;// COM height will not be constraint
-  tmp.target_r = hrp::Matrix33::Identity();//reference angular momentum
+  tmp.targetPos = ref_cog;// COM height will not be constraint
+  tmp.targetRpy = hrp::Vector3(0, 0, 0);//reference angular momentum
+//  tmp.targetRpy = hrp::Vector3(0, 10, 0);//reference angular momentum
   tmp.selection_vec << 1,1,1,1,1,1; // COM pos + Ang Momentum
-  tmp.weight_vec << 3,3,1,0.1,0.1,0.1;
-  fik->ik_tgt_list.push_back(tmp);
-
-  fik->optional_weight_vector(m_robot->link("CHEST_JOINT0")->jointId) = 0.1;
-  fik->optional_weight_vector(m_robot->link("CHEST_JOINT1")->jointId) = 0.1;
-  fik->optional_weight_vector(m_robot->link("CHEST_JOINT2")->jointId) = 0.1;
-//  fik->optional_weight_vector.tail(6) = hrp::dvector6::Ones() * 0.1;// 1 rad : 0.1 m くらいに調整
-
-  hrp::Vector3 base_p_old = m_robot->rootLink()->p;
-  hrp::Matrix33 base_R_old = m_robot->rootLink()->R;
-  hrp::dvector q_old(m_robot->numJoints());
-  fik->cur_P = fik->cur_L = hrp::Vector3::Zero();
-  for(int i=0;i<m_robot->numJoints();i++){ q_old(i) = m_robot->joint(i)->q; }
-
-  for(int l=0;l<IK_MAX_LOOP;l++){
-    fik->solveFullbodyIK(hrp::Vector3::Zero(), false);
-    //check ang moment
-    m_robot->rootLink()->v = (m_robot->rootLink()->p - base_p_old)/ m_dt;
-    m_robot->rootLink()->w = base_R_old * fik->omegaFromRotEx(base_R_old.transpose() * m_robot->rootLink()->R) / m_dt;
-    for(int i=0;i<m_robot->numJoints();i++){ m_robot->joint(i)->dq = (m_robot->joint(i)->q - q_old(i)) / m_dt; }
-    m_robot->calcForwardKinematics(true,false);
-    m_robot->calcTotalMomentum(fik->cur_P, fik->cur_L);
-
-    hrp::Vector3 com_p_err = ref_cog - m_robot->calcCM();
-    hrp::Vector3 rf_p_err = ikp["rleg"].target_p0 - fik->getEndEffectorPos("rleg");
-    hrp::Vector3 lf_p_err = ikp["lleg"].target_p0 - fik->getEndEffectorPos("lleg");
-    hrp::Vector3 rh_p_err = ikp["rarm"].target_p0 - fik->getEndEffectorPos("rarm");
-    hrp::Vector3 lh_p_err = ikp["larm"].target_p0 - fik->getEndEffectorPos("larm");
-    if(
-        com_p_err.norm() < 1e-3 &&
-        rf_p_err.norm() < 1e-3 &&
-        lf_p_err.norm() < 1e-3 &&
-        rh_p_err.norm() < 1e-2 &&
-        lh_p_err.norm() < 1e-2){ break; }
-    if(l == IK_MAX_LOOP-1){if(loop%100 == 0){std::cerr << "COM constraint IK MAX loop [="<<IK_MAX_LOOP<<"] exceeded!! "<< std::endl;} break; };
-  }
+  tmp.weight_vec << 3,3,3,1,1,1;
+  ik_tgt_list.push_back(tmp);
+
+//  fik->optional_weight_vector(m_robot->link("CHEST_JOINT0")->jointId) = 0.1;
+//  fik->optional_weight_vector(m_robot->link("CHEST_JOINT1")->jointId) = 0.1;
+//  fik->optional_weight_vector(m_robot->link("CHEST_JOINT2")->jointId) = 0.1;
+//  fik->optional_weight_vector.tail(6).fill(0.01);
+
+  if( m_robot->link("RARM_JOINT2") != NULL) m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇の干渉回避のため
+  if( m_robot->link("LARM_JOINT2") != NULL) m_robot->link("LARM_JOINT2")->llimit = deg2rad(40);
+//  fik_in->reference_gain.fill(0.001);
+//  fik_in->reference_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
+
+  struct timespec startT, endT;
+  int loop_result = 0;
+  const int IK_MAX_LOOP = 3;
+  loop_result = fik->solveFullbodyIKLoop(ik_tgt_list, IK_MAX_LOOP);
+}
+
+
+//void AutoBalancer::solveFullbodyIK ()
+//{
+//  // Set ik target params
+//  fik->target_root_p = target_root_p;
+//  fik->target_root_R = target_root_R;
+//  for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
+//      it->second.target_p0 = ikp[it->first].target_p0;
+//      it->second.target_r0 = ikp[it->first].target_r0;
+//  }
 //  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
+////  fik->current_tm = m_qRef.tm;
 //  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
 //      fik->ikp[it->first].is_ik_enable = it->second.is_active;
 //  }
-  // Revert
+//  // Revert
 //  fik->revertRobotStateToCurrent();
-}
-#else
-void AutoBalancer::solveFullbodyIK ()
-{
-  // Set ik target params
-  fik->target_root_p = target_root_p;
-  fik->target_root_R = target_root_R;
-  for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-      it->second.target_p0 = ikp[it->first].target_p0;
-      it->second.target_r0 = ikp[it->first].target_r0;
-  }
-  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
-//  fik->current_tm = m_qRef.tm;
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-      fik->ikp[it->first].is_ik_enable = it->second.is_active;
-  }
-  // Revert
-  fik->revertRobotStateToCurrent();
-  // TODO : SBP calculation is outside of solve ik?
-  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
-  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
-  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
-  // Solve IK
-  fik->solveFullbodyIK (dif_cog, transition_interpolator->isEmpty());
-}
-#endif
+//  // TODO : SBP calculation is outside of solve ik?
+//  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
+//  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
+//  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
+//  // Solve IK
+//  fik->solveFullbodyIK (dif_cog, transition_interpolator->isEmpty());
+//}
 
 
 
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 2b927780fbc..b4de6f35b21 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -28,8 +28,9 @@
 #include "AutoBalancerService_impl.h"
 #include "interpolator.h"
 #include "../TorqueFilter/IIRFilter.h"
-#include "SimpleFullbodyInverseKinematicsSolver.h"
-//#include "FullbodyInverseKinematicsSolver.h"
+
+//#include "SimpleFullbodyInverseKinematicsSolver.h"
+#include "FullbodyInverseKinematicsSolver.h"
 
 // </rtc-template>
 
@@ -247,8 +248,8 @@ class AutoBalancer
   ggPtr gg;
   bool gg_is_walking, gg_solved;
   // for abc
-//  typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
-  typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
+  typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
+//  typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
   fikPtr fik;
   hrp::Vector3 ref_cog, ref_zmp, prev_ref_zmp, prev_imu_sensor_pos, prev_imu_sensor_vel, hand_fix_initial_offset;
   enum {BIPED, TROT, PACE, CRAWL, GALLOP} gait_type;
diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index ece78bfe99b..e1e78209ad1 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -17,7 +17,7 @@ class IKConstraintParam {
   public:
     std::string target_link_name;
     hrp::Vector3 targetPos;
-    hrp::Matrix33 targetR;
+    hrp::Vector3 targetRpy;
     hrp::Vector3 localPos;
     hrp::Matrix33 localR;
     hrp::dvector6 weight_vec;
@@ -26,7 +26,7 @@ class IKConstraintParam {
 
     IKConstraintParam ()
       :targetPos(hrp::Vector3::Zero()),
-       targetR(hrp::Matrix33::Identity()),
+       targetRpy(hrp::Vector3::Zero()),
        localPos(hrp::Vector3::Zero()),
        localR(hrp::Matrix33::Identity()),
        weight_vec(hrp::dvector6::Ones()),
@@ -85,7 +85,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
         hrp::Link* target_link_ptr = m_robot->link(_ik_tgt_list[i].target_link_name);
         if(target_link_ptr){
           hrp::Vector3 pos_err = _ik_tgt_list[i].targetPos - (target_link_ptr->p + target_link_ptr->R * _ik_tgt_list[i].localPos);
-          hrp::Vector3 rot_err = omegaFromRotEx(_ik_tgt_list[i].targetR.transpose() * target_link_ptr->R * _ik_tgt_list[i].localR);
+          hrp::Vector3 rot_err = omegaFromRotEx(hrp::rotFromRpy(_ik_tgt_list[i].targetRpy).transpose() * target_link_ptr->R * _ik_tgt_list[i].localR);
           for(int j=0;j<3;j++){
             if(pos_err(j) > _ik_tgt_list[i].pos_precision && _ik_tgt_list[i].selection_vec(j) != 0){return false;}
             if(rot_err(j) > _ik_tgt_list[i].rot_precision && _ik_tgt_list[i].selection_vec(j+3) != 0){return false;}
@@ -118,7 +118,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
       //ヤコビアンと各ベクトル生成
       for ( int i=0; i<_ik_tgt_list.size(); i++ ) {
         hrp::Link* target_link_ptr = _robot->link(_ik_tgt_list[i].target_link_name);
-        hrp::Matrix33 ref_link_origin_R(_ik_tgt_list[i].targetR * _ik_tgt_list[i].localR.transpose()); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
+        hrp::Matrix33 ref_link_origin_R(hrp::rotFromRpy(_ik_tgt_list[i].targetRpy) * _ik_tgt_list[i].localR.transpose()); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
         hrp::Vector3 ref_link_origin_p(_ik_tgt_list[i].targetPos - ref_link_origin_R * _ik_tgt_list[i].localPos);
         hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF); //全身to一拘束点へのヤコビアン
         hrp::Vector3 vel_p_ref, vel_r_ref; //velと言いつつ実際は差分
@@ -144,15 +144,16 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
         }
         else if(!target_link_ptr && _ik_tgt_list[i].target_link_name == "COM"){ //重心限定
           vel_p_ref = ref_link_origin_p - _robot->calcCM();
-          vel_r_ref = (omegaFromRotEx(ref_link_origin_R) - (cur_L - _robot->rootLink()->p.cross(cur_P))) * m_dt;// COMのrotはAngulerMomentumとして扱う
+//          vel_r_ref = (omegaFromRotEx(ref_link_origin_R) - (cur_L - _robot->rootLink()->p.cross(cur_P))) * m_dt;// COMのrotはAngulerMomentumとして扱う
+          vel_r_ref = _ik_tgt_list[i].targetRpy;// COMのrotはAngulerMomentumとして扱う
           _robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
           _robot->calcAngularMomentumJacobian(NULL, J_am);//すでにrootlink周りの角運動量ヤコビアンが返ってくる？
           J_part << J_com, J_am;
         }
         else{ std::cerr<<"Unknown Link Target !!"<<std::endl; continue; } //不明なリンク指定
         //全体の中に配置
-        if(vel_p_ref.norm()>0.1){vel_p_ref = vel_p_ref.normalized() * 0.1;}
-        if(vel_r_ref.norm()>0.1){vel_r_ref = vel_r_ref.normalized() * 0.1;}
+//        if(vel_p_ref.norm()>0.1){vel_p_ref = vel_p_ref.normalized() * 0.1;}
+//        if(vel_r_ref.norm()>0.1){vel_r_ref = vel_r_ref.normalized() * 0.1;}
         dp_ee_all.segment(WS_DOF*i,WS_DOF) << vel_p_ref, vel_r_ref;
         weight_vec_all.segment(WS_DOF*i, WS_DOF) = _ik_tgt_list[i].weight_vec;
         selection_vec_all.segment(WS_DOF*i, WS_DOF) = _ik_tgt_list[i].selection_vec;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 0446724eea0..02b8966774d 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -554,7 +554,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.localPos = hrp::Vector3::Zero();
   tmp.localR = hrp::Matrix33::Identity();
   tmp.targetPos = robot_in->rootLink()->p;// will be ignored by selection_vec
-  tmp.targetR = hrp::rotFromRpy(com_ref.rpy);// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
+  tmp.targetRpy = com_ref.rpy;// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
   tmp.selection_vec << 0,0,0,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
@@ -563,7 +563,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.localPos = eename_ikcp_map["rleg"].localPos;
   tmp.localR = eename_ikcp_map["rleg"].localR;
   tmp.targetPos = rf_ref.p;
-  tmp.targetR = hrp::rotFromRpy(rf_ref.rpy);
+  tmp.targetRpy = rf_ref.rpy;
   tmp.selection_vec << 1,1,1,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
@@ -572,7 +572,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.localPos = eename_ikcp_map["lleg"].localPos;
   tmp.localR = eename_ikcp_map["lleg"].localR;
   tmp.targetPos = lf_ref.p;
-  tmp.targetR = hrp::rotFromRpy(lf_ref.rpy);
+  tmp.targetRpy = lf_ref.rpy;
   tmp.selection_vec << 1,1,1,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
@@ -581,7 +581,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.localPos = eename_ikcp_map["rarm"].localPos;
   tmp.localR = eename_ikcp_map["rarm"].localR;
   tmp.targetPos = rh_ref.p;
-  tmp.targetR = hrp::rotFromRpy(rh_ref.rpy);
+  tmp.targetRpy = rh_ref.rpy;
   tmp.selection_vec << 1,1,1,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
@@ -590,7 +590,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.localPos = eename_ikcp_map["larm"].localPos;
   tmp.localR = eename_ikcp_map["larm"].localR;
   tmp.targetPos = lh_ref.p;
-  tmp.targetR = hrp::rotFromRpy(lh_ref.rpy);
+  tmp.targetRpy = lh_ref.rpy;
   tmp.selection_vec << 1,1,1,1,1,1;
   tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
@@ -599,7 +599,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.localPos = hrp::Vector3::Zero();
   tmp.localR = hrp::Matrix33::Identity();
   tmp.targetPos = com_ref.p;// COM height will not be constraint
-  tmp.targetR = hrp::Matrix33::Identity();//reference angular momentum
+  tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
   tmp.selection_vec << 1,1,1,0,0,0; // COM pos + Ang Momentum
 //  tmp.selection_vec << 1,1,1,1,1,0; // COM pos + Ang Momentum
   tmp.weight_vec << 3,3,3,0.1,0.1,0.1;
@@ -608,7 +608,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   if(robot_in->link("HEAD_JOINT1") != NULL){
     tmp.target_link_name = "HEAD_JOINT1";
     tmp.target_link_name = "HEAD_P";
-    tmp.targetR = hrp::rotFromRpy(head_ref.rpy);
+    tmp.targetRpy = head_ref.rpy;
     tmp.selection_vec << 0,0,0,0,1,1;
     tmp.weight_vec << 1,1,1,1,1,1;
     ik_tgt_list.push_back(tmp);
@@ -643,7 +643,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   for(int i=0;i<4;i++){
     if(fik_in->ikp.count(names[i])){
       fik_in->ikp[names[i]].targetPos = refs[i]->p;
-      fik_in->ikp[names[i]].targetR = hrp::rotFromRpy(refs[i]->rpy);
+      fik_in->ikp[names[i]].targetRpy = hrp::rotFromRpy(refs[i]->rpy);
     }
   }
 //  fik_in->storeCurrentParameters();

From 135b1a433ecff773a061e8e77a70a445a0d02dc8 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 24 Aug 2017 11:34:52 +0900
Subject: [PATCH 119/217] tune for rhp3

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 2 +-
 rtc/WholeBodyMasterSlave/wbms_core.h              | 5 +++--
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 02b8966774d..fbfa95be336 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -690,7 +690,7 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
 
   double goal_time = 0.0;
   const double min_goal_time_offset = 0.1;
-  const double avg_q_vel = 1.0;
+  const double avg_q_vel = 0.5;
 //  const double avg_q_vel = 2.0;
   for(int i=0;i<robot_in->numJoints();i++){
     double tmp_time = fabs(robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 1bfcb3b1326..a90bde0382c 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -494,12 +494,13 @@ class WBMSCore : UTIL_CONST {
       LIMIT_MINMAX( out.tgt[rf].abs.p(Z), out.tgt[rf].offs.p(Z), out.tgt[rf].offs.p(Z)+WBMSparam.swing_foot_max_height);
       LIMIT_MINMAX( out.tgt[lf].abs.p(Z), out.tgt[lf].offs.p(Z), out.tgt[lf].offs.p(Z)+WBMSparam.swing_foot_max_height);
 
+      const double base2hand_min = 0.4;
       for(int i=0, l[2]={rh,lh}; i<2; i++){
 //        LIMIT_MIN(out.tgt[l[i]].abs.p(X), baselinkpose.p(X));
 //        LIMIT_MAX(out.tgt[l[i]].abs.p(Z), baselinkpose.p(Z) + 0.4);
         hrp::Vector2 horizontal_dist(out.tgt[l[i]].abs.p(X) - baselinkpose.p(X), out.tgt[l[i]].abs.p(Y) - baselinkpose.p(Y));
-        if(horizontal_dist.norm() < 0.5){
-          horizontal_dist = 0.5 * horizontal_dist.normalized();
+        if(horizontal_dist.norm() < base2hand_min){
+          horizontal_dist = base2hand_min * horizontal_dist.normalized();
         }
         out.tgt[l[i]].abs.p(X) = baselinkpose.p(X) + horizontal_dist(X);
         out.tgt[l[i]].abs.p(Y) = baselinkpose.p(Y) + horizontal_dist(Y);

From f0cad60335ef44211ac8b72fa3da2e4e10800389 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 28 Aug 2017 20:27:53 +0900
Subject: [PATCH 120/217] add swing foot height wait

---
 rtc/AutoBalancer/AutoBalancer.cpp             |   4 +-
 .../FullbodyInverseKinematicsSolver.h         |  84 ++++--
 rtc/SequencePlayer/interpolator.cpp           |   3 +-
 .../WholeBodyMasterSlave.cpp                  |  47 +++-
 .../WholeBodyMasterSlave.h                    |   3 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 242 ++++++++++++------
 6 files changed, 259 insertions(+), 124 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index f80d11f5d7a..4f03a7e3914 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1131,16 +1131,16 @@ void AutoBalancer::solveFullbodyIK ()
   tmp.targetRpy = hrp::Vector3(0, 0, 0);//reference angular momentum
 //  tmp.targetRpy = hrp::Vector3(0, 10, 0);//reference angular momentum
   tmp.selection_vec << 1,1,1,1,1,1; // COM pos + Ang Momentum
-  tmp.weight_vec << 3,3,3,1,1,1;
+  tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
 
 //  fik->optional_weight_vector(m_robot->link("CHEST_JOINT0")->jointId) = 0.1;
 //  fik->optional_weight_vector(m_robot->link("CHEST_JOINT1")->jointId) = 0.1;
 //  fik->optional_weight_vector(m_robot->link("CHEST_JOINT2")->jointId) = 0.1;
-//  fik->optional_weight_vector.tail(6).fill(0.01);
 
   if( m_robot->link("RARM_JOINT2") != NULL) m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇の干渉回避のため
   if( m_robot->link("LARM_JOINT2") != NULL) m_robot->link("LARM_JOINT2")->llimit = deg2rad(40);
+
 //  fik_in->reference_gain.fill(0.001);
 //  fik_in->reference_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
 
diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index e1e78209ad1..5fba62b65bf 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -5,13 +5,14 @@
 #include <hrpModel/Body.h>
 #include <hrpUtil/MatrixSolvers.h>
 #include "../ImpedanceController/JointPathEx.h"
-#include "SimpleFullbodyInverseKinematicsSolver.h"
 #include <unsupported/Eigen/MatrixFunctions>
+#include "SimpleFullbodyInverseKinematicsSolver.h"
 
 #define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
-#define deg2rad(x)((x)*M_PI/180)
+#define deg2rad(x) ((x)*M_PI/180)
 #define rad2deg(rad) (rad * 180 / M_PI)
 #define eps_eq(a, b, c)  (fabs((a)-(b)) <= c)
+#define dbg(var) std::cout<<#var"= "<<(var)<<std::endl
 
 class IKConstraintParam {
   public:
@@ -77,9 +78,6 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
       }
       return loop;
     }
-    void solveFullbodyIK (const hrp::Vector3& _dif_cog, const bool _is_transition){
-      SimpleFullbodyInverseKinematicsSolver::solveFullbodyIK (_dif_cog, _is_transition);
-    }
     bool checkIKConvergence(const std::vector<IKConstraintParam>& _ik_tgt_list){
       for ( int i=0; i<_ik_tgt_list.size(); i++ ) {
         hrp::Link* target_link_ptr = m_robot->link(_ik_tgt_list[i].target_link_name);
@@ -163,9 +161,13 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
       hrp::dmatrix selection_matrix = hrp::dmatrix::Zero(selection_vec_all.sum(), WS_DOF*_ik_tgt_list.size());
       for(int i=0, j=0; i<selection_vec_all.rows(); i++){ if(selection_vec_all(i) == 1){ selection_matrix(j, i) = 1.0; j++; } }
 
-      weight_vec_all /= weight_vec_all.maxCoeff();
+//      weight_vec_all /= weight_vec_all.maxCoeff();
       J_all = selection_matrix * J_all;
-      dp_ee_all = selection_matrix * weight_vec_all.asDiagonal() * dp_ee_all;
+      dp_ee_all = selection_matrix * dp_ee_all;
+      weight_vec_all = selection_matrix * weight_vec_all;
+//      J_all *= selection_matrix;
+//      dp_ee_all *= selection_matrix;
+//      weight_vec_all *= selection_matrix;
 
 //      hrp::calcPseudoInverse(J_all,J_all_inv,1.0e-3);//超遅い1[ms]とか
       calcJacobianInverseNullspace_copy(_robot, J_all, J_all_inv);
@@ -178,6 +180,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
         std::cout<<std::setprecision(2) << "selection_matrix=\n"<<selection_matrix<<std::setprecision(6)<<std::endl;
         std::cout<<"eevel_all\n"<<dp_ee_all.transpose()<<std::endl;
         std::cout<<"dq_all\n"<<dq_all.transpose()<<std::endl;
+        std::cout<<"weight_vec_all\n"<<weight_vec_all.transpose()<<std::endl;
         std::cout<<"q_ans_all\n";
         for(int i=0;i<_robot->numJoints();i++)std::cerr<<_robot->joint(i)->q<<" ";
         std::cout<<std::endl;
@@ -196,6 +199,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
       }
       _robot->rootLink()->p += dq_all.segment(J_DOF,3) * LAMBDA/* * m_dt*/;//vel計算する時からm_dt使ってないから使わなくていい
       _robot->rootLink()->R = base_R_ans;
+      _robot->calcForwardKinematics();
     }
 
 
@@ -241,20 +245,26 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
         w(j, j) = optional_weight_vector[j];
       }
 
-      const double manipulability_limit = 0.001;
+      const double manipulability_limit = 0.0001;
       const double sr_gain = 1.0;
 //      const double manipulability_gain = 0.001;
       const double manipulability_gain = 1;
 
       double manipulability = sqrt((_J*_J.transpose()).determinant());
+//      static int count;
+//      if(count++ % 10000 == 0){
+//        std::cout<<"manipulability "<<manipulability<<std::endl;
+//      }
       double k = 0;
       if ( manipulability < manipulability_limit ) {
         k = manipulability_gain * pow((1 - ( manipulability / manipulability_limit )), 2);
       }
-      hrp::calcSRInverse(_J, _Jinv, sr_gain * k, w);
+//      hrp::calcSRInverse(_J, _Jinv, sr_gain * k, w);
+      calcSRInverse_new(_J, _Jinv, sr_gain * k, weight_vec_all, w);
       //        Jnull = ( hrp::dmatrix::Identity(n, n) - _Jinv * _J);
       return true;
     }
+
     hrp::Vector3 omegaFromRotEx(const hrp::Matrix33& r) {//copy from JointPathEx.cpp
         using ::std::numeric_limits;
         double alpha = (r(0,0) + r(1,1) + r(2,2) - 1.0) / 2.0;
@@ -270,6 +280,20 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
             return hrp::Vector3( (r(1,2) - r(2,1)) * k, (r(2,0) - r(0,2)) * k, (r(0,1) - r(1,0)) * k );
         }
     }
+
+    void calcSRInverse_new(const hrp::dmatrix& _a, hrp::dmatrix &_a_sr, double _sr_ratio, hrp::dvector& _ee_w, hrp::dmatrix _w) {
+        const int c = _a.rows(); // 6*x
+        const int n = _a.cols(); // n = all joint dof
+        if ( _w.cols() != n || _w.rows() != n ) {
+            _w = hrp::dmatrix::Identity(n, n);
+        }
+        hrp::dmatrix at = _a.transpose();
+        hrp::dmatrix a1(c, c);
+        hrp::dmatrix eew_mat = _ee_w.cwiseInverse().asDiagonal();
+        a1 = (_a * _w * at +  _sr_ratio * eew_mat).inverse();
+//        a1 = (_a * _w * at +  _sr_ratio *  hrp::dmatrix::Identity(c,c)).inverse();
+        _a_sr  = _w * at * a1;
+    }
 };
 
 
@@ -302,34 +326,40 @@ class FullbodyInverseKinematicsSolverMT : public FullbodyInverseKinematicsSolver
     };
     int solveFullbodyIKLoopMT (const std::vector<IKConstraintParam>& _ik_tgt_list, const int _max_iteration) {
       if(is_first){
-        for(int i=0;i<J_DOF;i++){ m_robot_copy->joint(i)->q = m_robot->joint(i)->q; }
+        for(int i=0;i<J_DOF;i++){
+          m_robot_copy->joint(i)->q = m_robot->joint(i)->q;
+          m_robot_copy->joint(i)->ulimit = m_robot->joint(i)->ulimit;
+          m_robot_copy->joint(i)->llimit = m_robot->joint(i)->llimit;
+        }
         m_robot_copy->rootLink()->p = m_robot->rootLink()->p;
         m_robot_copy->rootLink()->R = m_robot->rootLink()->R;
-        if( m_robot_copy->link("RARM_JOINT2") != NULL) m_robot_copy->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇の干渉回避のため
-        if( m_robot_copy->link("LARM_JOINT2") != NULL) m_robot_copy->link("LARM_JOINT2")->llimit = deg2rad(40);
         pthread_mutex_init( &ik_body_mutex, NULL );
+        pthread_create(&ik_thread, NULL, launchThread, this);
+        is_first = false;
+      }
+      for(int i=0;i<J_DOF;i++){
+        m_robot->joint(i)->q = m_robot_copy->joint(i)->q;
       }
-      for(int i=0;i<J_DOF;i++){ m_robot->joint(i)->q = m_robot_copy->joint(i)->q; }
       m_robot->rootLink()->p = m_robot_copy->rootLink()->p;
       m_robot->rootLink()->R = m_robot_copy->rootLink()->R;
+      m_robot->calcForwardKinematics();
       int result = cur_ik_loop;
 
       if(pthread_mutex_lock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_lock err "<<std::endl;}
 
-//        base_p_old = m_robot_copy->rootLink()->p;
-//        base_R_old = m_robot_copy->rootLink()->R;
-//        q_old.resize(J_DOF);
-//        for(int i=0;i<J_DOF;i++){ q_old(i) = m_robot_copy->joint(i)->q; }
+        base_p_old = m_robot_copy->rootLink()->p;
+        base_R_old = m_robot_copy->rootLink()->R;
+        q_old.resize(J_DOF);
+        for(int i=0;i<J_DOF;i++){ q_old(i) = m_robot_copy->joint(i)->q; }
 
         cur_ik_loop = 0;
         ik_tgt_list = _ik_tgt_list;// "="で代入するとnanが入る！？->そんなことはない
         ik_thread_ik_required = true;
       if(pthread_mutex_unlock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_unlock err "<<std::endl;}
-      if(is_first){
-        pthread_create(&ik_thread, NULL, launchThread, this);
-        is_first = false;
-      }
-
+//      if(is_first){
+//        pthread_create(&ik_thread, NULL, launchThread, this);
+//        is_first = false;
+//      }
       return result;
     }
     void ik_loop(){
@@ -340,11 +370,11 @@ class FullbodyInverseKinematicsSolverMT : public FullbodyInverseKinematicsSolver
           if(ik_thread_ik_required){
             solveFullbodyIK(m_robot_copy, ik_tgt_list);
             //check ang moment
-//            m_robot_copy->rootLink()->v = (m_robot_copy->rootLink()->p - base_p_old)/ m_dt;
-//            m_robot_copy->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * m_robot_copy->rootLink()->R) / m_dt;
-//            for(int i=0;i<J_DOF;i++){ m_robot_copy->joint(i)->dq = (m_robot_copy->joint(i)->q - q_old(i)) / m_dt; }
-//            m_robot_copy->calcForwardKinematics(true,false);
-//            m_robot_copy->calcTotalMomentum(cur_P, cur_L);
+            m_robot_copy->rootLink()->v = (m_robot_copy->rootLink()->p - base_p_old)/ m_dt;
+            m_robot_copy->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * m_robot_copy->rootLink()->R) / m_dt;
+            for(int i=0;i<J_DOF;i++){ m_robot_copy->joint(i)->dq = (m_robot_copy->joint(i)->q - q_old(i)) / m_dt; }
+            m_robot_copy->calcForwardKinematics(true,false);
+            m_robot_copy->calcTotalMomentum(cur_P, cur_L);
             cur_ik_loop++;
             if(cur_ik_loop > 10000){std::cerr<<"[FullbodyInverseKinematicsSolverMT] thread time out"<<std::endl;break;}//異常終了でスレッドが残ってしまった時にタイムアウト終了
           }
diff --git a/rtc/SequencePlayer/interpolator.cpp b/rtc/SequencePlayer/interpolator.cpp
index 5a2637e4046..7a24a695bdb 100644
--- a/rtc/SequencePlayer/interpolator.cpp
+++ b/rtc/SequencePlayer/interpolator.cpp
@@ -167,7 +167,8 @@ void interpolator::setGoal(const double *newg, const double *newv, double time,
         a1[i]=v[i];
         a2[i]=(-3*x[i] + 3*gx[i] - 2*v[i]*target_t - gv[i]*target_t) / (target_t*target_t);
         a3[i]=( 2*x[i] - 2*gx[i] +   v[i]*target_t + gv[i]*target_t) / (target_t*target_t*target_t);
-        a4[i]=a[5]=0;
+//        a4[i]=a[5]=0;
+        a4[i]=a5[i]=0;
         break;
         }
     }
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index fbfa95be336..7b0a7fb286b 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -167,7 +167,9 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     tmp_ratio = 1.0;
     transition_interpolator->setGoal(&tmp_ratio, 3.0, true);
     ROBOT_ALL_DOF = m_robot->numJoints() + 3 + 3;// joints + base_p + base_rpy
-    q_ip = new interpolator(ROBOT_ALL_DOF, m_dt, interpolator::HOFFARBIB, 1);
+//    q_ip = new interpolator(ROBOT_ALL_DOF, m_dt, interpolator::HOFFARBIB, 1);
+//    q_ip = new interpolator(ROBOT_ALL_DOF, m_dt, interpolator::QUINTICSPLINE, 1);
+    q_ip = new interpolator(ROBOT_ALL_DOF, m_dt, interpolator::CUBICSPLINE, 1);
     q_ip->setName(std::string(m_profile.instance_name)+" q_ip");
     q_ip->clear();
     // Generate FIK
@@ -190,7 +192,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     hsp = boost::shared_ptr<WBMSCore>(new WBMSCore(m_dt));
 
     invdyn_zmp_filters.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
-    invdyn_zmp_filters2.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
+    final_ref_zmp_filter.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
 
     std::cerr << "[" << m_profile.instance_name << "] onInitialize() OK" << std::endl;
     return RTC::RTC_OK;
@@ -302,7 +304,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
           hsp->m_robot_act = m_robot_vsafe;
           //逆動力学初期化
           idsb.setInitState(m_robot, m_dt);
-          idsb2.setInitState(m_robot, m_dt);
         }
 
         if(hsp->WBMSparam.is_doctor){
@@ -328,9 +329,23 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 //        const hrp::BodyPtr m_robot_for_out = m_robot;
 
         //OutPortデータセット
-        hrp::Vector3 ref_zmp = hsp->rp_ref_out.tgt[zmp].abs.p;
+//        hrp::Vector3 ref_zmp = hsp->rp_ref_out.tgt[zmp].abs.p;
+
+        hrp::Vector3 com = m_robot_for_out->calcCM();
+        static hrp::Vector3 com_old = com;
+        static hrp::Vector3 com_old_old = com_old;
+        hrp::Vector3 com_acc = (com - 2*com_old + com_old_old)/(m_dt*m_dt);
+        hrp::Vector3 ref_zmp; ref_zmp << com.head(XY)-(com(Z)/G)*com_acc.head(XY), 0;
+        ref_zmp = final_ref_zmp_filter.passFilter(ref_zmp);
+        com_old_old = com_old;
+        com_old = com;
+        hsp->act_rs.com = com;
+        hsp->act_rs.zmp = ref_zmp;
+
         // qRef
-        for (int i = 0; i < m_qRef.data.length(); i++ ){ m_qRef.data[i] = transition_interpolator_ratio * m_robot_for_out->joint(i)->q  + (1 - transition_interpolator_ratio) * m_qRef.data[i]; }
+        for (int i = 0; i < m_qRef.data.length(); i++ ){
+          m_qRef.data[i] = transition_interpolator_ratio * m_robot_for_out->joint(i)->q  + (1 - transition_interpolator_ratio) * m_qRef.data[i];
+        }
         // basePos
         m_basePos.data.x = transition_interpolator_ratio * m_robot_for_out->rootLink()->p(0) + (1 - transition_interpolator_ratio) * m_basePos.data.x;
         m_basePos.data.y = transition_interpolator_ratio * m_robot_for_out->rootLink()->p(1) + (1 - transition_interpolator_ratio) * m_basePos.data.y;
@@ -565,7 +580,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.targetPos = rf_ref.p;
   tmp.targetRpy = rf_ref.rpy;
   tmp.selection_vec << 1,1,1,1,1,1;
-  tmp.weight_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 1,1,3,1,1,1;
+//  tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = eename_ikcp_map["lleg"].target_link_name;
@@ -574,7 +590,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.targetPos = lf_ref.p;
   tmp.targetRpy = lf_ref.rpy;
   tmp.selection_vec << 1,1,1,1,1,1;
-  tmp.weight_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 1,1,3,1,1,1;
+//  tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = eename_ikcp_map["rarm"].target_link_name;
@@ -600,14 +617,14 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.localR = hrp::Matrix33::Identity();
   tmp.targetPos = com_ref.p;// COM height will not be constraint
   tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
+//  tmp.selection_vec << 1,1,1,0,0,0; // COM pos + Ang Momentum
   tmp.selection_vec << 1,1,1,0,0,0; // COM pos + Ang Momentum
-//  tmp.selection_vec << 1,1,1,1,1,0; // COM pos + Ang Momentum
-  tmp.weight_vec << 3,3,3,0.1,0.1,0.1;
+  tmp.weight_vec << 3,3,0.5,1e-4,1e-4,1e-4;
+//  tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
 
   if(robot_in->link("HEAD_JOINT1") != NULL){
     tmp.target_link_name = "HEAD_JOINT1";
-    tmp.target_link_name = "HEAD_P";
     tmp.targetRpy = head_ref.rpy;
     tmp.selection_vec << 0,0,0,0,1,1;
     tmp.weight_vec << 1,1,1,1,1,1;
@@ -617,7 +634,9 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
 //  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;
 //  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
 //  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
-  fik_in->optional_weight_vector.tail(6).fill(0.01);
+//  fik_in->optional_weight_vector.tail(6).fill(0.01);
+  if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->ulimit = deg2rad(-30);//脇の干渉回避のため
+  if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(30);
 
   fik_in->reference_q = init_sync_state;
 //  fik_in->reference_gain.fill(0.001);
@@ -690,8 +709,9 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
 
   double goal_time = 0.0;
   const double min_goal_time_offset = 0.1;
-  const double avg_q_vel = 0.5;
-//  const double avg_q_vel = 2.0;
+//  const double avg_q_vel = 0.5;
+//  const double avg_q_vel = 1.0;
+  const double avg_q_vel = 2.0;
   for(int i=0;i<robot_in->numJoints();i++){
     double tmp_time = fabs(robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
     if(tmp_time > goal_time){ goal_time = tmp_time; }
@@ -704,6 +724,7 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
   for(int i=0;i<robot_out->numJoints();i++){ robot_out->joint(i)->q = ans_state(i); }
   robot_out->rootLink()->p = ans_state.bottomRows(6).topRows(3);
   robot_out->rootLink()->R = hrp::rotFromRpy(ans_state.bottomRows(6).bottomRows(3));
+  robot_out->calcForwardKinematics();
 }
 
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 6f91a0152cb..83f311be9ea 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -187,8 +187,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
   hrp::InvDynStateBuffer idsb;
   BiquadIIRFilterVec invdyn_zmp_filters;
 
-  hrp::InvDynStateBuffer idsb2;
-  BiquadIIRFilterVec invdyn_zmp_filters2;
+  BiquadIIRFilterVec final_ref_zmp_filter;
 
   IIRFilter ref_ee_vel_v_filter;
 
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index a90bde0382c..e1a0b512b54 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -52,6 +52,7 @@ class UTIL_CONST{
     enum { num_ee_tgt=4 };
     enum { R, L, LR };
     enum { X, Y, Z, XYZ };
+    enum { XY = 2 };
     enum { r, p, y, rpy };
     enum { fx, fy, fz, tx, ty, tz, ft_xyz };
     enum { MIN, MAX, MINMAX };
@@ -71,6 +72,7 @@ class BiquadIIRFilterVec : UTIL_CONST {
     IIRFilter filters[XYZ];
     hrp::Vector3 ans;
   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     BiquadIIRFilterVec(){}
     ~BiquadIIRFilterVec(){}
     void setParameter(const hrp::Vector3& fc_in, const double& HZ, const double& Q = 0.5){ for(int i=0;i<XYZ;i++){ filters[i].setParameterAsBiquad((double)fc_in(i), Q, HZ); } }
@@ -81,6 +83,7 @@ class BiquadIIRFilterVec : UTIL_CONST {
 
 class WBMSPose3D{
   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     hrp::Vector3 p,rpy;
     WBMSPose3D(){ clear(); }
     ~WBMSPose3D(){}
@@ -89,6 +92,7 @@ class WBMSPose3D{
 
 class PoseTGT{
   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     WBMSPose3D abs, offs, cnt;
     hrp::dvector6 w;
     bool is_contact, go_contact;
@@ -120,7 +124,6 @@ class WBMSCore : UTIL_CONST {
     struct timeval t_calc_start, t_calc_end;
     BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters, com_in_filter;
     hrp::Vector3 com_old, com_oldold, comacc, com_CP_ref_old, r_zmp_raw;
-    hrp::Vector4 rf_safe_region,lf_safe_region;//前後左右の順
     std::vector<hrp::Vector2> rflf_points, hull_com, hull_dcp, hull_acp;
     std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rot_filters;
     std::vector<cv::Point2f> points,cvhull;
@@ -129,15 +132,17 @@ class WBMSCore : UTIL_CONST {
     unsigned int loop;
     bool is_initial_loop;
   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     double H_cur;
     HumanPose hp_wld_raw, hp_plot, rp_ref_out, rp_ref_vel_old;
     FILE *sr_log, *cz_log, *id_log;
     WBMSPose3D baselinkpose;
 //    WBMSPose3D ee_contact_pose[4];
     hrp::Vector3 com_vel_old,rh_vel_old, cp_dec, cp_acc;
-    std::vector<hrp::Vector3> rf_vert,lf_vert;
+    std::vector<hrp::Vector3> foot_vert_act[LR], foot_vert_safe[LR], foot_vert_check[LR];
     hrp::Vector2 com_forcp_ref,com_vel_forcp_ref;
     hrp::dvector6 invdyn_ft;
+    hrp::Vector4 foot_vert_act_fblr[LR], foot_vert_safe_fblr[LR], foot_vert_check_fblr[LR];
     struct WBMSparameters {
       double auto_swing_foot_landing_threshold;
       double foot_vertical_vel_limit_coeff;
@@ -153,6 +158,9 @@ class WBMSCore : UTIL_CONST {
       bool use_manipulability_limit;
     };
     struct WBMSparameters WBMSparam;
+    struct ActualRobotState {
+        hrp::Vector3 com, zmp;
+    }act_rs;
 
     typedef boost::shared_ptr<FullbodyInverseKinematicsSolverMT> fikPtr;
     fikPtr fik_ml;
@@ -189,21 +197,24 @@ class WBMSCore : UTIL_CONST {
       for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0, HZ, Q_NOOVERSHOOT);//四肢拘束点用(Rotation)
       tgt_pos_filters[com].setParameter(1.5, HZ, Q_NOOVERSHOOT);//重心pos用
       tgt_rot_filters[com].setParameter(0.6, HZ, Q_NOOVERSHOOT);//重心rot用
-      tgt_pos_filters[rf].setParameter(hrp::Vector3(10.0,10.0,10.0), HZ, Q_NOOVERSHOOT);//右足pos用
+      tgt_pos_filters[rf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, Q_NOOVERSHOOT);//右足pos用
       tgt_pos_filters[lf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, Q_NOOVERSHOOT);//左足pos用
       calcacc_v_filters.setParameter(5, HZ, Q_BUTTERWORTH);//加速度計算用
       acc4zmp_v_filters.setParameter(5, HZ, Q_BUTTERWORTH);//ZMP生成用ほぼこの値でいい
       com_in_filter.setParameter(1, HZ);
-      rf_safe_region = hrp::Vector4(0.02, -0.01,  0.02,  0.01);
-      lf_safe_region = hrp::Vector4(0.02, -0.01, -0.01, -0.02);
-      rf_vert.push_back(hrp::Vector3(0.13,-0.08,0));
-      rf_vert.push_back(hrp::Vector3(-0.10,-0.08,0));
-      rf_vert.push_back(hrp::Vector3(-0.10,0.06,0));
-      rf_vert.push_back(hrp::Vector3(0.13,0.06,0));
-      lf_vert.push_back(hrp::Vector3(0.13,-0.06,0));
-      lf_vert.push_back(hrp::Vector3(-0.10,-0.06,0));
-      lf_vert.push_back(hrp::Vector3(-0.10,0.08,0));
-      lf_vert.push_back(hrp::Vector3(0.13,0.08,0));
+      foot_vert_act_fblr[R] << 0.13, -0.10,  0.06, -0.08;
+      foot_vert_act_fblr[L] << 0.13, -0.10,  0.08, -0.06;
+//      foot_vert_check_fblr[R] << 0.03, -0.02,  0.03,  0.0;
+//      foot_vert_check_fblr[L] << 0.03, -0.02, 0.0, -0.03;
+      foot_vert_check_fblr[R] << 0.13, -0.10,  0.06, -0.08;
+      foot_vert_check_fblr[L] << 0.13, -0.10,  0.08, -0.06;
+      foot_vert_safe_fblr[R] << 0.02, -0.01,  0.02,  0.01;
+      foot_vert_safe_fblr[L] << 0.02, -0.01, -0.01, -0.02;
+      for(int i=0;i<LR;i++){
+        make_rect_verts(foot_vert_act_fblr[i], foot_vert_act[i]);
+        make_rect_verts(foot_vert_check_fblr[i], foot_vert_check[i]);
+        make_rect_verts(foot_vert_safe_fblr[i], foot_vert_safe[i]);
+      }
 
       WBMSparam.auto_swing_foot_landing_threshold = 0.02;
       WBMSparam.foot_vertical_vel_limit_coeff = 4.0;
@@ -234,6 +245,13 @@ class WBMSCore : UTIL_CONST {
       is_initial_loop = true;
       cout<<"WBMSCore constructed"<<endl;
     }
+    static void make_rect_verts(const hrp::Vector4& ForwardBackLeftRight, std::vector<hrp::Vector3>& out){
+      out.clear();
+      out.push_back(hrp::Vector3(ForwardBackLeftRight(0), ForwardBackLeftRight(2),  0));//左前
+      out.push_back(hrp::Vector3(ForwardBackLeftRight(1), ForwardBackLeftRight(2),  0));//左後
+      out.push_back(hrp::Vector3(ForwardBackLeftRight(1), ForwardBackLeftRight(3),  0));//右後
+      out.push_back(hrp::Vector3(ForwardBackLeftRight(0), ForwardBackLeftRight(3),  0));//右前
+    }
     ~WBMSCore(){
       if(DEBUG)fclose(sr_log);
       if(DEBUG)fclose(cz_log);
@@ -258,7 +276,7 @@ class WBMSCore : UTIL_CONST {
           rp_ref_out.tgt[human_l_names[i]].abs = rp_ref_out.tgt[human_l_names[i]].offs;
         }
       }
-      rp_ref_out.tgt[com].offs.p = robot_in->calcCM();
+      rp_ref_out.tgt[com].offs.p = act_rs.com = act_rs.zmp = robot_in->calcCM();
       com_CP_ref_old = rp_ref_out.tgt[com].offs.p;
       rp_ref_out.tgt[com].offs.rpy = hrp::rpyFromRot(robot_in->rootLink()->R);
       rp_ref_out.tgt[zmp].offs.p(X) = rp_ref_out.tgt[com].offs.p(X);
@@ -306,6 +324,8 @@ class WBMSCore : UTIL_CONST {
 
       applyLPFilter_pre                       (rp_ref_out);
       applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
+      surpressGoContactChattering         (rp_ref_out);
+//      setFootContactPoseByGoContact       (rp_ref_out);
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
       applyLPFilter_post                       (rp_ref_out);
 
@@ -315,6 +335,7 @@ class WBMSCore : UTIL_CONST {
         fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.tgt[zmp].abs.p(X),rp_ref_out.tgt[zmp].abs.p(Y));
         fprintf(cz_log,"\n");
       }
+
       overwriteFootZFromFootLandOnCommand (rp_ref_out);
       modifyFootRotAndXYForContact        (rp_ref_out);
 
@@ -419,21 +440,6 @@ class WBMSCore : UTIL_CONST {
         pos_ans = pos_tgt;
       }
     }
-//    void calcVelAccSafeTrajectoryVec(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
-//      if((pos_tgt - pos_cur).norm() < 1.0e-6 && vel_cur.norm() < 1.0e-6){
-//        pos_ans = pos_tgt;
-//      }else{
-//        hrp::Vector3 direc = (pos_tgt - pos_cur).normalized();
-//        double stop_safe_vel_scalar = sqrt( 2 * max_acc * (pos_tgt - pos_cur).norm() );
-//        hrp::Vector3 vel_ans;
-//        if(vel_cur.dot(direc) > stop_safe_vel_scalar){
-//          vel_ans = vel_cur - direc * max_acc * DT;
-//        }else{
-//          vel_ans = vel_cur + direc * max_acc * DT;
-//        }
-//        pos_ans = pos_cur + vel_ans * DT;
-//      }
-//    }
     void calcVelAccSafeTrajectoryVecML(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& acc_base, const hrp::Matrix33& max_acc_mat, const hrp::Vector3& max_acc_sv, hrp::Vector3& pos_ans){
       if((pos_tgt - pos_cur).norm() > 1.0e-6){
         hrp::Vector3 direc = (pos_tgt - pos_cur).normalized();
@@ -464,8 +470,10 @@ class WBMSCore : UTIL_CONST {
     }
     void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
       hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);
-      hrp::Vector2 rf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[rf].abs.p(X),old.tgt[rf].abs.p(Y));
-      hrp::Vector2 lf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[lf].abs.p(X),old.tgt[lf].abs.p(Y));
+//      hrp::Vector2 rf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[rf].abs.p(X),old.tgt[rf].abs.p(Y));
+//      hrp::Vector2 lf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[lf].abs.p(X),old.tgt[lf].abs.p(Y));
+      hrp::Vector2 rf2zmp = act_rs.zmp.head(XY) - old.tgt[rf].abs.p.head(XY);
+      hrp::Vector2 lf2zmp = act_rs.zmp.head(XY) - old.tgt[lf].abs.p.head(XY);
       if( inside_vec_rf.dot(rf2zmp) / inside_vec_rf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ out.tgt[lf].go_contact = true; }
       if( inside_vec_lf.dot(lf2zmp) / inside_vec_lf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ out.tgt[rf].go_contact = true; }
     }
@@ -525,8 +533,11 @@ class WBMSCore : UTIL_CONST {
           fik_ml->ikp[robot_l_names[i]].target_p0 = out.tgt[human_l_names[i]].abs.p;
         }
       }
-      for ( std::map<std::string, FullbodyInverseKinematicsSolverMT::IKparam>::iterator it = fik_ml->ikp.begin(); it != fik_ml->ikp.end(); it++ ) {
-          if (it->second.is_ik_enable) fik_ml->solveLimbIK (it->second, it->first, fik_ml->ratio_for_vel, false);
+      const int pre_ik_loop = 2;
+      for(int i=0;i<pre_ik_loop;i++){
+        for ( std::map<std::string, FullbodyInverseKinematicsSolverMT::IKparam>::iterator it = fik_ml->ikp.begin(); it != fik_ml->ikp.end(); it++ ) {
+            if (it->second.is_ik_enable) fik_ml->solveLimbIK (it->second, it->first, fik_ml->ratio_for_vel, false);
+        }
       }
       for(int i=0;i<4;i++){
         if(fik_ml->ikp.count(robot_l_names[i])){
@@ -546,11 +557,6 @@ class WBMSCore : UTIL_CONST {
           out.tgt[l[i]].cnt.p(Y) = rp_ref_out_old.tgt[l[i]].abs.p(Y);
           out.tgt[l[i]].cnt.rpy(y) = rp_ref_out_old.tgt[l[i]].abs.rpy(y);
         }
-        if(out.tgt[l[i]].go_contact){
-          out.tgt[l[i]].abs.p(Z) = out.tgt[l[i]].offs.p(Z);
-        }else{
-          LIMIT_MIN( out.tgt[l[i]].abs.p(Z), out.tgt[l[i]].offs.p(Z)+WBMSparam.swing_foot_height_offset);
-        }
       }
     }
     void setFootRotHorizontalIfGoLanding(HumanPose& out){
@@ -562,6 +568,13 @@ class WBMSCore : UTIL_CONST {
       const double com2lf_dist = fabs((double)out.tgt[lf].abs.p(Y) - (double)out.tgt[com].abs.p(Y));
       limitGroundContactVelocity(out.tgt[rf].go_contact, com2rf_dist, rp_ref_out_old.tgt[rf], out.tgt[rf], out.tgt[rf].is_contact);
       limitGroundContactVelocity(out.tgt[lf].go_contact, com2lf_dist, rp_ref_out_old.tgt[lf], out.tgt[lf], out.tgt[lf].is_contact);
+      for(int i=0, l[LR]={rf,lf}; i<LR; i++){
+        if(out.tgt[l[i]].go_contact){
+          out.tgt[l[i]].abs.p(Z) = out.tgt[l[i]].offs.p(Z);
+        }else{
+          LIMIT_MIN( out.tgt[l[i]].abs.p(Z), out.tgt[l[i]].offs.p(Z)+WBMSparam.swing_foot_height_offset);
+        }
+      }
     }
     void limitGroundContactVelocity(const bool& go_land_in, const double& com2foot_dist, const PoseTGT& f_old_in, PoseTGT& f_in_out, bool& is_f_contact_out){
       double penalty = 1.0 + (0.2 - com2foot_dist) * 5.0;//1.0 ~ 2.0 本当はCPに比例すべき？
@@ -579,8 +592,8 @@ class WBMSCore : UTIL_CONST {
       is_f_contact_out = ((f_in_out.abs.p(Z)-f_in_out.offs.p(Z)) <= contact_threshold);
     }
     void modifyFootRotAndXYForContact(HumanPose& out){
-      calcFootRotAndXYTransitionForContact(out.tgt[rf],rf_vert);
-      calcFootRotAndXYTransitionForContact(out.tgt[lf],lf_vert);
+      calcFootRotAndXYTransitionForContact(out.tgt[rf],foot_vert_act[R]);
+      calcFootRotAndXYTransitionForContact(out.tgt[lf],foot_vert_act[L]);
     }
     void calcFootRotAndXYTransitionForContact(PoseTGT& foot_in, const std::vector<hrp::Vector3>& act_sole_size){
       std::vector<hrp::Vector3> act_foot_vert;
@@ -600,7 +613,7 @@ class WBMSCore : UTIL_CONST {
     }
     bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
       hull_com.clear();
-      createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region, lf_safe_region, hull_com);
+      createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R], foot_vert_safe_fblr[L], hull_com);
       hrp::Vector2 cog(comin_abs(X),comin_abs(Y));
       if(!isPointInHullOpenCV(cog,hull_com)){ calcNearestPointOnHull(cog,hull_com,cog); }//外に出たら最近傍点に頭打ち
       comin_abs(X) = cog(X);
@@ -615,8 +628,8 @@ class WBMSCore : UTIL_CONST {
       hull_acp.clear();
       hrp::Vector4 marginDelta_for_dcp(+0.001,-0.001,0.001,-0.001);
       hrp::Vector4 marginDelta_for_acp(+0.010,-0.010,0.010,-0.010);
-      createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region+marginDelta_for_dcp, lf_safe_region+marginDelta_for_dcp, hull_dcp);
-      createSupportRegionByFootPos(rfin_abs, lfin_abs, rf_safe_region+marginDelta_for_acp, lf_safe_region+marginDelta_for_acp, hull_acp);
+      createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R]+marginDelta_for_dcp, foot_vert_safe_fblr[L]+marginDelta_for_dcp, hull_dcp);
+      createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R]+marginDelta_for_acp, foot_vert_safe_fblr[L]+marginDelta_for_acp, hull_acp);
       hrp::Vector2 com_vel_ans_2d;
       regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_ans_old(X),com_ans_old(Y)), hrp::Vector2(com_vel(X),com_vel(Y)), hull_dcp, hull_acp, com_vel_ans_2d);
       com_ans(X) = com_ans_old(X) + com_vel_ans_2d(X) * DT;
@@ -637,34 +650,36 @@ class WBMSCore : UTIL_CONST {
     void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull_d, const std::vector<hrp::Vector2>& hull_a, hrp::Vector2& com_vel_ans){
       hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
       com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
-      //減速CP条件(現在のCPを常に両足裏で頭打ち)
-      hrp::Vector2 cp_dec_tmp = com_pos + com_vel * sqrt( H_cur / G );
-      hrp::Vector2 cp_dec_ragulated;
-      if(!isPointInHullOpenCV(cp_dec_tmp,hull_d)){
-        calcCrossPointOnHull(com_pos, cp_dec_tmp, hull_d, cp_dec_ragulated);
-        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
-      }
-
-      //減速CP条件2(指令のCPに対する現在発揮可能なCP)
-//      hrp::Vector2 cp_dec_ref = com_forcp_ref + com_vel_forcp_ref * sqrt( H_cur / G );
-//      hrp::Vector2 cp_dec_ref_ragulated = cp_dec_ref;
-//      if(!isPointInHullOpenCV(cp_dec_ref,hull_d)){
-//        calcCrossPointOnHull(com_pos, cp_dec_ref, hull_d, cp_dec_ref_ragulated);
+//      //減速CP条件(現在のCPを常に両足裏で頭打ち)
+//      hrp::Vector2 cp_dec_tmp = com_pos + com_vel * sqrt( H_cur / G );
+//      hrp::Vector2 cp_dec_ragulated;
+//      if(!isPointInHullOpenCV(cp_dec_tmp,hull_d)){
+//        calcCrossPointOnHull(com_pos, cp_dec_tmp, hull_d, cp_dec_ragulated);
+//        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
 //      }
-//      com_vel_decel_ok = (cp_dec_ref_ragulated - com_pos) / sqrt( H_cur / G );
 
-      //減速CP条件2(指令のCOMに収束する現在発揮可能なCP)
-//      hrp::Vector2 cp_dec_ref = com_forcp_ref;
-//      hrp::Vector2 cp_dec_ref_ragulated = cp_dec_ref;
-//      if(!isPointInHullOpenCV(cp_dec_ref,hull_d)){
-//        calcCrossPointOnHull(com_pos, cp_dec_ref, hull_d, cp_dec_ref_ragulated);
-//      }
-//      hrp::Vector2 vel_max_decel_ok = (cp_dec_ref_ragulated - com_pos) / sqrt( H_cur / G );
-//      if( com_vel.dot(com_vel) > vel_max_decel_ok.dot(com_vel)){
-//        com_vel_decel_ok = vel_max_decel_ok;
-//      }else{
-//        com_vel_decel_ok = com_vel;
-//      }
+      const double foot_ave_vel = 1.0;
+      const double lf_landing_delay = (rp_ref_out.tgt[lf].abs.p[Z] - rp_ref_out.tgt[lf].offs.p[Z]) / foot_ave_vel;
+      const double rf_landing_delay = (rp_ref_out.tgt[rf].abs.p[Z] - rp_ref_out.tgt[rf].offs.p[Z]) / foot_ave_vel;
+      const double foot_landing_delay = std::max(rf_landing_delay, lf_landing_delay);
+      //減速CP条件(現在のCPを常に両足裏で頭打ち)
+      hrp::Vector2 cp_dec_ragulated = com_pos + com_vel * sqrt( H_cur / G ) + com_vel * foot_landing_delay;
+      if(!isPointInHullOpenCV(cp_dec_ragulated,hull_d)){
+        calcCrossPointOnHull(com_pos, cp_dec_ragulated, hull_d, cp_dec_ragulated);
+        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / ( sqrt( H_cur / G ) + foot_landing_delay);
+      }
+
+#define F(x) (x == R ? rf : lf)
+
+      std::vector<hrp::Vector2> f_hull_wld[LR], f_hull_wld_ans[LR];
+//      for(int i=0;i<foot_vert_check[R].size();i++){ f_hull_wld[R].push_back(foot_vert_check[R][i].head(XY) + rp_ref_out.tgt[F(R)].abs.p.head(XY)); }
+//      for(int i=0;i<foot_vert_check[L].size();i++){ f_hull_wld[L].push_back(foot_vert_check[L][i].head(XY) + rp_ref_out.tgt[F(L)].abs.p.head(XY)); }
+      for(int i=0;i<foot_vert_safe[R].size();i++){ f_hull_wld[R].push_back(foot_vert_safe[R][i].head(XY) + rp_ref_out.tgt[F(R)].abs.p.head(XY)); }
+      for(int i=0;i<foot_vert_safe[L].size();i++){ f_hull_wld[L].push_back(foot_vert_safe[L][i].head(XY) + rp_ref_out.tgt[F(L)].abs.p.head(XY)); }
+      makeConvexHullOpenCV(f_hull_wld[R], f_hull_wld_ans[R]);
+      makeConvexHullOpenCV(f_hull_wld[L], f_hull_wld_ans[L]);
+      if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[R])){ rp_ref_out.tgt[lf].go_contact = true; }
+      if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[L])){ rp_ref_out.tgt[rf].go_contact = true; }
 
       //加速CP条件(ACP使用)
       hrp::Vector2 cp_acc_tmp = com_pos - com_vel * sqrt( H_cur / G );
@@ -675,17 +690,7 @@ class WBMSCore : UTIL_CONST {
       }
       cp_acc_old = cp_acc_ragulated;
 
-      //加速CP条件2(ZMP使用)
       static hrp::Vector2 com_vel_ans_old = com_vel;
-//      hrp::Vector2 com_acc = (com_vel - com_vel_ans_old) / DT;
-//      hrp::Vector2 zmp_in = com_pos - com_acc / G * H_cur;
-//      hrp::Vector2 zmp_regulated;
-//      if(!isPointInHullOpenCV(zmp_in,hull_a)){
-//        calcCrossPointOnHull(com_pos, zmp_in, hull_a, zmp_regulated);
-//        hrp::Vector2 com_acc_regulated = (com_pos - zmp_regulated)*G/H_cur;
-//        com_vel_accel_ok = com_vel_ans_old + com_acc_regulated*DT;
-//      }
-
       //加速減速条件マージ
       if( com_vel_decel_ok.dot(com_vel) < com_vel_accel_ok.dot(com_vel)){//normじゃダメ？
         com_vel_ans = com_vel_decel_ok;
@@ -694,6 +699,85 @@ class WBMSCore : UTIL_CONST {
       }
       com_vel_ans_old = com_vel_ans;
     }
+//    void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull_d, const std::vector<hrp::Vector2>& hull_a, hrp::Vector2& com_vel_ans){
+//      hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
+//      com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
+//      //減速CP条件(現在のCPを常に両足裏で頭打ち)
+//      hrp::Vector2 cp_dec_tmp = com_pos + com_vel * sqrt( H_cur / G );
+//      hrp::Vector2 cp_dec_ragulated;
+//      if(!isPointInHullOpenCV(cp_dec_tmp,hull_d)){
+//        calcCrossPointOnHull(com_pos, cp_dec_tmp, hull_d, cp_dec_ragulated);
+//        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
+//      }
+//
+//      //減速CP条件2(指令のCPに対する現在発揮可能なCP)
+////      hrp::Vector2 cp_dec_ref = com_forcp_ref + com_vel_forcp_ref * sqrt( H_cur / G );
+////      hrp::Vector2 cp_dec_ref_ragulated = cp_dec_ref;
+////      if(!isPointInHullOpenCV(cp_dec_ref,hull_d)){
+////        calcCrossPointOnHull(com_pos, cp_dec_ref, hull_d, cp_dec_ref_ragulated);
+////      }
+////      com_vel_decel_ok = (cp_dec_ref_ragulated - com_pos) / sqrt( H_cur / G );
+//
+//      //減速CP条件2(指令のCOMに収束する現在発揮可能なCP)
+////      hrp::Vector2 cp_dec_ref = com_forcp_ref;
+////      hrp::Vector2 cp_dec_ref_ragulated = cp_dec_ref;
+////      if(!isPointInHullOpenCV(cp_dec_ref,hull_d)){
+////        calcCrossPointOnHull(com_pos, cp_dec_ref, hull_d, cp_dec_ref_ragulated);
+////      }
+////      hrp::Vector2 vel_max_decel_ok = (cp_dec_ref_ragulated - com_pos) / sqrt( H_cur / G );
+////      if( com_vel.dot(com_vel) > vel_max_decel_ok.dot(com_vel)){
+////        com_vel_decel_ok = vel_max_decel_ok;
+////      }else{
+////        com_vel_decel_ok = com_vel;
+////      }
+//
+//      //加速CP条件(ACP使用)
+//      hrp::Vector2 cp_acc_tmp = com_pos - com_vel * sqrt( H_cur / G );
+//      hrp::Vector2 cp_acc_ragulated;
+//      if(!isPointInHullOpenCV(cp_acc_tmp,hull_a)){
+//         calcCrossPointOnHull(com_pos, cp_acc_tmp, hull_a, cp_acc_ragulated);
+//         com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
+//      }
+//      cp_acc_old = cp_acc_ragulated;
+//
+//      //加速CP条件2(ZMP使用)
+//      static hrp::Vector2 com_vel_ans_old = com_vel;
+////      hrp::Vector2 com_acc = (com_vel - com_vel_ans_old) / DT;
+////      hrp::Vector2 zmp_in = com_pos - com_acc / G * H_cur;
+////      hrp::Vector2 zmp_regulated;
+////      if(!isPointInHullOpenCV(zmp_in,hull_a)){
+////        calcCrossPointOnHull(com_pos, zmp_in, hull_a, zmp_regulated);
+////        hrp::Vector2 com_acc_regulated = (com_pos - zmp_regulated)*G/H_cur;
+////        com_vel_accel_ok = com_vel_ans_old + com_acc_regulated*DT;
+////      }
+//
+//      //加速減速条件マージ
+//      if( com_vel_decel_ok.dot(com_vel) < com_vel_accel_ok.dot(com_vel)){//normじゃダメ？
+//        com_vel_ans = com_vel_decel_ok;
+//      }else{
+//        com_vel_ans = com_vel_accel_ok;
+//      }
+//      com_vel_ans_old = com_vel_ans;
+//    }
+    void surpressGoContactChattering(HumanPose& out){
+//      static std::deque<int> buffer[LR];
+      static int count[LR];
+      for(int i=0;i<LR;i++){
+        if(out.tgt[F(i)].go_contact){ count[i]++; }
+        else{ count[i]--; }
+        LIMIT_MINMAX(count[i],0,100);
+        if(count[i]>0){ out.tgt[F(i)].go_contact = true; }
+      }
+//      buffer[R].push_back((int)out.tgt[rf].go_contact);
+//      buffer[L].push_back((int)out.tgt[lf].go_contact);
+//      if(buffer[R].size()>100){ buffer[R].pop_front(); }
+//      if(buffer[L].size()>100){ buffer[L].pop_front(); }
+//      dbg(std::accumulate(buffer[R].begin(), buffer[R].end(), 0));
+//      if(std::accumulate(buffer[R].begin(), buffer[R].end(), 0) > 0) out.tgt[rf].go_contact = true;
+//      if(std::accumulate(buffer[L].begin(), buffer[L].end(), 0) > 0) out.tgt[lf].go_contact = true;
+
+
+    }
     void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
       rflf_points.clear();
       hull_ans.clear();

From 22f55be035d318ce630ed560e3007e481a574191 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 28 Aug 2017 23:40:56 +0900
Subject: [PATCH 121/217] tune awinf leg height waiting

---
 .../WholeBodyMasterSlave.cpp                  |  8 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 82 ++++++++++---------
 2 files changed, 49 insertions(+), 41 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 7b0a7fb286b..56c8cb5083f 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -192,7 +192,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     hsp = boost::shared_ptr<WBMSCore>(new WBMSCore(m_dt));
 
     invdyn_zmp_filters.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
-    final_ref_zmp_filter.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
+    final_ref_zmp_filter.setParameter(5, 1/m_dt, Q_BUTTERWORTH);
 
     std::cerr << "[" << m_profile.instance_name << "] onInitialize() OK" << std::endl;
     return RTC::RTC_OK;
@@ -336,6 +336,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         static hrp::Vector3 com_old_old = com_old;
         hrp::Vector3 com_acc = (com - 2*com_old + com_old_old)/(m_dt*m_dt);
         hrp::Vector3 ref_zmp; ref_zmp << com.head(XY)-(com(Z)/G)*com_acc.head(XY), 0;
+        if(mode.isInitialize()){ final_ref_zmp_filter.reset(ref_zmp); }
         ref_zmp = final_ref_zmp_filter.passFilter(ref_zmp);
         com_old_old = com_old;
         com_old = com;
@@ -368,6 +369,9 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
           m_optionalData.data.length(optionalDataLength);//TODO:これいいのか？
           for(int i=0;i<optionalDataLength;i++)m_optionalData.data[i] = 0;
         }
+//        m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = hsp->rp_ref_out.tgt[rf].is_contact;
+//        m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = hsp->rp_ref_out.tgt[lf].is_contact;
+        bool rf_is_contact =
         m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = hsp->rp_ref_out.tgt[rf].is_contact;
         m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = hsp->rp_ref_out.tgt[lf].is_contact;
       }
@@ -711,7 +715,7 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
   const double min_goal_time_offset = 0.1;
 //  const double avg_q_vel = 0.5;
 //  const double avg_q_vel = 1.0;
-  const double avg_q_vel = 2.0;
+  const double avg_q_vel = 1.5;
   for(int i=0;i<robot_in->numJoints();i++){
     double tmp_time = fabs(robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
     if(tmp_time > goal_time){ goal_time = tmp_time; }
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index e1a0b512b54..3a32986ced2 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -131,6 +131,7 @@ class WBMSCore : UTIL_CONST {
     HumanPose rp_ref_out_old, hp_swap_checked;
     unsigned int loop;
     bool is_initial_loop;
+    bool cp_force_go_contact[LR];
   public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     double H_cur;
@@ -195,7 +196,7 @@ class WBMSCore : UTIL_CONST {
       tgt_rot_filters.resize(num_pose_tgt);
       for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(1.0, HZ, Q_NOOVERSHOOT);//四肢拘束点用(position)
       for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0, HZ, Q_NOOVERSHOOT);//四肢拘束点用(Rotation)
-      tgt_pos_filters[com].setParameter(1.5, HZ, Q_NOOVERSHOOT);//重心pos用
+      tgt_pos_filters[com].setParameter(1.0, HZ, Q_NOOVERSHOOT);//重心pos用
       tgt_rot_filters[com].setParameter(0.6, HZ, Q_NOOVERSHOOT);//重心rot用
       tgt_pos_filters[rf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, Q_NOOVERSHOOT);//右足pos用
       tgt_pos_filters[lf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, Q_NOOVERSHOOT);//左足pos用
@@ -204,10 +205,10 @@ class WBMSCore : UTIL_CONST {
       com_in_filter.setParameter(1, HZ);
       foot_vert_act_fblr[R] << 0.13, -0.10,  0.06, -0.08;
       foot_vert_act_fblr[L] << 0.13, -0.10,  0.08, -0.06;
-//      foot_vert_check_fblr[R] << 0.03, -0.02,  0.03,  0.0;
-//      foot_vert_check_fblr[L] << 0.03, -0.02, 0.0, -0.03;
-      foot_vert_check_fblr[R] << 0.13, -0.10,  0.06, -0.08;
-      foot_vert_check_fblr[L] << 0.13, -0.10,  0.08, -0.06;
+      foot_vert_check_fblr[R] << 0.02+0.002, -0.01-0.002,  0.02+0.002,  0.01-0.002;
+      foot_vert_check_fblr[L] << 0.02+0.002, -0.01-0.002, -0.01+0.002, -0.02-0.002;
+//      foot_vert_check_fblr[R] << 0.13, -0.10,  0.06, -0.08;
+//      foot_vert_check_fblr[L] << 0.13, -0.10,  0.08, -0.06;
       foot_vert_safe_fblr[R] << 0.02, -0.01,  0.02,  0.01;
       foot_vert_safe_fblr[L] << 0.02, -0.01, -0.01, -0.02;
       for(int i=0;i<LR;i++){
@@ -215,8 +216,11 @@ class WBMSCore : UTIL_CONST {
         make_rect_verts(foot_vert_check_fblr[i], foot_vert_check[i]);
         make_rect_verts(foot_vert_safe_fblr[i], foot_vert_safe[i]);
       }
+      cp_force_go_contact[R] = false;
+      cp_force_go_contact[L] = false;
 
-      WBMSparam.auto_swing_foot_landing_threshold = 0.02;
+//      WBMSparam.auto_swing_foot_landing_threshold = 0.02;
+      WBMSparam.auto_swing_foot_landing_threshold = 0.04;
       WBMSparam.foot_vertical_vel_limit_coeff = 4.0;
       WBMSparam.human_com_height = 1.00;
       WBMSparam.is_doctor = true;
@@ -311,6 +315,8 @@ class WBMSCore : UTIL_CONST {
       setFootContactPoseByGoContact       (rp_ref_out);
       if(DEBUG){ fprintf(cz_log,"com_in: %f %f ",rp_ref_out.tgt[com].abs.p(X),rp_ref_out.tgt[com].abs.p(Y)); }
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
+
+      overwriteFootZFromFootLandOnCommand (rp_ref_out);
       setFootRotHorizontalIfGoLanding     (rp_ref_out);
 
       limitManipulability                 (rp_ref_out);
@@ -324,7 +330,7 @@ class WBMSCore : UTIL_CONST {
 
       applyLPFilter_pre                       (rp_ref_out);
       applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
-      surpressGoContactChattering         (rp_ref_out);
+//      surpressGoContactChattering         (rp_ref_out);
 //      setFootContactPoseByGoContact       (rp_ref_out);
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
       applyLPFilter_post                       (rp_ref_out);
@@ -336,7 +342,7 @@ class WBMSCore : UTIL_CONST {
         fprintf(cz_log,"\n");
       }
 
-      overwriteFootZFromFootLandOnCommand (rp_ref_out);
+//      overwriteFootZFromFootLandOnCommand (rp_ref_out);
       modifyFootRotAndXYForContact        (rp_ref_out);
 
       H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[rf].abs.p(Z));
@@ -469,7 +475,7 @@ class WBMSCore : UTIL_CONST {
       pos_ans = pos_cur + vel_ans * DT;
     }
     void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
-      hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);
+      hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);//TODO implement
 //      hrp::Vector2 rf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[rf].abs.p(X),old.tgt[rf].abs.p(Y));
 //      hrp::Vector2 lf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[lf].abs.p(X),old.tgt[lf].abs.p(Y));
       hrp::Vector2 rf2zmp = act_rs.zmp.head(XY) - old.tgt[rf].abs.p.head(XY);
@@ -564,28 +570,25 @@ class WBMSCore : UTIL_CONST {
       if( out.tgt[lf].go_contact){ out.tgt[lf].abs.rpy(r) = out.tgt[lf].abs.rpy(p) = 0; }
     }
     void overwriteFootZFromFootLandOnCommand(HumanPose& out){
-      const double com2rf_dist = fabs((double)out.tgt[rf].abs.p(Y) - (double)out.tgt[com].abs.p(Y));
-      const double com2lf_dist = fabs((double)out.tgt[lf].abs.p(Y) - (double)out.tgt[com].abs.p(Y));
-      limitGroundContactVelocity(out.tgt[rf].go_contact, com2rf_dist, rp_ref_out_old.tgt[rf], out.tgt[rf], out.tgt[rf].is_contact);
-      limitGroundContactVelocity(out.tgt[lf].go_contact, com2lf_dist, rp_ref_out_old.tgt[lf], out.tgt[lf], out.tgt[lf].is_contact);
+//      limitGroundContactVelocity(out.tgt[rf].go_contact, rp_ref_out_old.tgt[rf], out.tgt[rf], out.tgt[rf].is_contact);
+//      limitGroundContactVelocity(out.tgt[lf].go_contact, rp_ref_out_old.tgt[lf], out.tgt[lf], out.tgt[lf].is_contact);
       for(int i=0, l[LR]={rf,lf}; i<LR; i++){
-        if(out.tgt[l[i]].go_contact){
+        if(out.tgt[l[i]].go_contact || cp_force_go_contact[i]){
           out.tgt[l[i]].abs.p(Z) = out.tgt[l[i]].offs.p(Z);
+          out.tgt[l[i]].is_contact = true;
         }else{
           LIMIT_MIN( out.tgt[l[i]].abs.p(Z), out.tgt[l[i]].offs.p(Z)+WBMSparam.swing_foot_height_offset);
         }
       }
     }
-    void limitGroundContactVelocity(const bool& go_land_in, const double& com2foot_dist, const PoseTGT& f_old_in, PoseTGT& f_in_out, bool& is_f_contact_out){
-      double penalty = 1.0 + (0.2 - com2foot_dist) * 5.0;//1.0 ~ 2.0 本当はCPに比例すべき？
-      LIMIT_MINMAX( penalty, 1.0, 2.0);
+    void limitGroundContactVelocity(const bool& go_land_in, const PoseTGT& f_old_in, PoseTGT& f_in_out, bool& is_f_contact_out){
       LIMIT_MIN( f_in_out.abs.p(Z), f_in_out.offs.p(Z));
       double input_vel = (f_in_out.abs.p(Z) - f_old_in.abs.p(Z)) / DT;
-      const double vel_limit_k = WBMSparam.foot_vertical_vel_limit_coeff * penalty;//地面から0.02[m]地点で0.02*8=0.16[m/s]出ている計算
+      const double vel_limit_k = WBMSparam.foot_vertical_vel_limit_coeff;//地面から0.02[m]地点で0.02*8=0.16[m/s]出ている計算
       double limit_vel = - vel_limit_k * (f_old_in.abs.p(Z) - f_old_in.offs.p(Z));//sinの時は平均0.05~0.10[m/s]
       const double max_vel_threshold = -0.01;
-//      LIMIT_MAX( limit_vel, max_vel_threshold);//速度0に近づくと目標になかなか到達しないから
-//      LIMIT_MIN( input_vel, limit_vel);//着地時の下向きの速度を制限
+      LIMIT_MAX( limit_vel, max_vel_threshold);//速度0に近づくと目標になかなか到達しないから
+      LIMIT_MIN( input_vel, limit_vel);//着地時の下向きの速度を制限
       f_in_out.abs.p(Z) = f_old_in.abs.p(Z) + input_vel * DT;
       LIMIT_MIN( f_in_out.abs.p(Z), f_in_out.offs.p(Z));
       const double contact_threshold = 0.005;
@@ -658,11 +661,19 @@ class WBMSCore : UTIL_CONST {
 //        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
 //      }
 
-      const double foot_ave_vel = 1.0;
-      const double lf_landing_delay = (rp_ref_out.tgt[lf].abs.p[Z] - rp_ref_out.tgt[lf].offs.p[Z]) / foot_ave_vel;
-      const double rf_landing_delay = (rp_ref_out.tgt[rf].abs.p[Z] - rp_ref_out.tgt[rf].offs.p[Z]) / foot_ave_vel;
-      const double foot_landing_delay = std::max(rf_landing_delay, lf_landing_delay);
+      const double foot_ave_vel = 2.0;
+      double lf_landing_delay = (rp_ref_out.tgt[lf].abs.p[Z] - rp_ref_out.tgt[lf].offs.p[Z]) / foot_ave_vel;
+      double rf_landing_delay = (rp_ref_out.tgt[rf].abs.p[Z] - rp_ref_out.tgt[rf].offs.p[Z]) / foot_ave_vel;
+      if(com_vel(Y)>0 ){ rf_landing_delay = 0; }
+      if(com_vel(Y)<0 ){ lf_landing_delay = 0; }
+      double foot_landing_delay = std::max(rf_landing_delay, lf_landing_delay);
+//      const double foot_landing_delay = 0;//debug
       //減速CP条件(現在のCPを常に両足裏で頭打ち)
+
+//      hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);//TODO implement
+//      if(com_vel(Y)>1e-5 || fabs(com_vel(Y))<=1e-5 ){
+//        foot_landing_delay = 0;
+//      }
       hrp::Vector2 cp_dec_ragulated = com_pos + com_vel * sqrt( H_cur / G ) + com_vel * foot_landing_delay;
       if(!isPointInHullOpenCV(cp_dec_ragulated,hull_d)){
         calcCrossPointOnHull(com_pos, cp_dec_ragulated, hull_d, cp_dec_ragulated);
@@ -672,14 +683,16 @@ class WBMSCore : UTIL_CONST {
 #define F(x) (x == R ? rf : lf)
 
       std::vector<hrp::Vector2> f_hull_wld[LR], f_hull_wld_ans[LR];
-//      for(int i=0;i<foot_vert_check[R].size();i++){ f_hull_wld[R].push_back(foot_vert_check[R][i].head(XY) + rp_ref_out.tgt[F(R)].abs.p.head(XY)); }
-//      for(int i=0;i<foot_vert_check[L].size();i++){ f_hull_wld[L].push_back(foot_vert_check[L][i].head(XY) + rp_ref_out.tgt[F(L)].abs.p.head(XY)); }
-      for(int i=0;i<foot_vert_safe[R].size();i++){ f_hull_wld[R].push_back(foot_vert_safe[R][i].head(XY) + rp_ref_out.tgt[F(R)].abs.p.head(XY)); }
-      for(int i=0;i<foot_vert_safe[L].size();i++){ f_hull_wld[L].push_back(foot_vert_safe[L][i].head(XY) + rp_ref_out.tgt[F(L)].abs.p.head(XY)); }
+      for(int i=0;i<foot_vert_check[R].size();i++){ f_hull_wld[R].push_back(foot_vert_check[R][i].head(XY) + rp_ref_out.tgt[F(R)].abs.p.head(XY)); }
+      for(int i=0;i<foot_vert_check[L].size();i++){ f_hull_wld[L].push_back(foot_vert_check[L][i].head(XY) + rp_ref_out.tgt[F(L)].abs.p.head(XY)); }
+//      for(int i=0;i<foot_vert_safe[R].size();i++){ f_hull_wld[R].push_back(foot_vert_safe[R][i].head(XY) + rp_ref_out.tgt[F(R)].abs.p.head(XY)); }
+//      for(int i=0;i<foot_vert_safe[L].size();i++){ f_hull_wld[L].push_back(foot_vert_safe[L][i].head(XY) + rp_ref_out.tgt[F(L)].abs.p.head(XY)); }
       makeConvexHullOpenCV(f_hull_wld[R], f_hull_wld_ans[R]);
       makeConvexHullOpenCV(f_hull_wld[L], f_hull_wld_ans[L]);
-      if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[R])){ rp_ref_out.tgt[lf].go_contact = true; }
-      if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[L])){ rp_ref_out.tgt[rf].go_contact = true; }
+//      if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[R])){ rp_ref_out.tgt[lf].go_contact = true; }
+//      if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[L])){ rp_ref_out.tgt[rf].go_contact = true; }
+      if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[R])){ cp_force_go_contact[L] = true; }else{ cp_force_go_contact[L] = false; }
+      if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[L])){ cp_force_go_contact[R] = true; }else{ cp_force_go_contact[R] = false; }
 
       //加速CP条件(ACP使用)
       hrp::Vector2 cp_acc_tmp = com_pos - com_vel * sqrt( H_cur / G );
@@ -768,15 +781,6 @@ class WBMSCore : UTIL_CONST {
         LIMIT_MINMAX(count[i],0,100);
         if(count[i]>0){ out.tgt[F(i)].go_contact = true; }
       }
-//      buffer[R].push_back((int)out.tgt[rf].go_contact);
-//      buffer[L].push_back((int)out.tgt[lf].go_contact);
-//      if(buffer[R].size()>100){ buffer[R].pop_front(); }
-//      if(buffer[L].size()>100){ buffer[L].pop_front(); }
-//      dbg(std::accumulate(buffer[R].begin(), buffer[R].end(), 0));
-//      if(std::accumulate(buffer[R].begin(), buffer[R].end(), 0) > 0) out.tgt[rf].go_contact = true;
-//      if(std::accumulate(buffer[L].begin(), buffer[L].end(), 0) > 0) out.tgt[lf].go_contact = true;
-
-
     }
     void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
       rflf_points.clear();

From a8c865a3231ab9152cfc6a5e7a6fcac0c560b281 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 1 Sep 2017 01:02:14 +0900
Subject: [PATCH 122/217] implement LM-IK with constant lambda

---
 rtc/AutoBalancer/AutoBalancer.cpp             |  21 +--
 .../FullbodyInverseKinematicsSolver.h         | 140 +++++++++++++++---
 .../WholeBodyMasterSlave.cpp                  |  40 ++---
 .../WholeBodyMasterSlave.h                    |   4 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          |   6 +-
 5 files changed, 143 insertions(+), 68 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 4f03a7e3914..549a7a40a74 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1084,8 +1084,7 @@ void AutoBalancer::solveFullbodyIK ()
   tmp.localR = hrp::Matrix33::Identity();
   tmp.targetPos = target_root_p;// will be ignored by selection_vec
   tmp.targetRpy = hrp::rpyFromRot(target_root_R);// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
-  tmp.selection_vec << 0,0,0,1,1,1;
-  tmp.weight_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 0,0,0,1,1,1;
   ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = ikp["rleg"].target_link->name;
@@ -1093,8 +1092,7 @@ void AutoBalancer::solveFullbodyIK ()
   tmp.localR = ikp["rleg"].localR;
   tmp.targetPos = ikp["rleg"].target_p0;
   tmp.targetRpy = hrp::rpyFromRot(ikp["rleg"].target_r0);
-  tmp.selection_vec << 1,1,1,1,1,1;
-  tmp.weight_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 1,1,3,3,3,1;
   ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = ikp["lleg"].target_link->name;
@@ -1102,8 +1100,7 @@ void AutoBalancer::solveFullbodyIK ()
   tmp.localR = ikp["lleg"].localR;
   tmp.targetPos = ikp["lleg"].target_p0;
   tmp.targetRpy = hrp::rpyFromRot(ikp["lleg"].target_r0);
-  tmp.selection_vec << 1,1,1,1,1,1;
-  tmp.weight_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 1,1,3,3,3,1;
   ik_tgt_list.push_back(tmp);
 
 //  tmp.target_link_name = ikp["rarm"].target_link->name;
@@ -1111,7 +1108,6 @@ void AutoBalancer::solveFullbodyIK ()
 //  tmp.localR = ikp["rarm"].localR;
 //  tmp.targetPos = ikp["rarm"].target_p0;
 //  tmp.targetRpy = hrp::rpyFromRot(ikp["rarm"].target_r0);
-//  tmp.selection_vec << 1,1,1,1,1,1;
 //  tmp.weight_vec << 1,1,1,1,1,1;
 //  ik_tgt_list.push_back(tmp);
 //
@@ -1120,7 +1116,6 @@ void AutoBalancer::solveFullbodyIK ()
 //  tmp.localR = ikp["larm"].localR;
 //  tmp.targetPos = ikp["larm"].target_p0;
 //  tmp.targetRpy = hrp::rpyFromRot(ikp["larm"].target_r0);
-//  tmp.selection_vec << 1,1,1,1,1,1;
 //  tmp.weight_vec << 1,1,1,1,1,1;
 //  ik_tgt_list.push_back(tmp);
 
@@ -1130,23 +1125,17 @@ void AutoBalancer::solveFullbodyIK ()
   tmp.targetPos = ref_cog;// COM height will not be constraint
   tmp.targetRpy = hrp::Vector3(0, 0, 0);//reference angular momentum
 //  tmp.targetRpy = hrp::Vector3(0, 10, 0);//reference angular momentum
-  tmp.selection_vec << 1,1,1,1,1,1; // COM pos + Ang Momentum
-  tmp.weight_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 3,3,1,0.01,0.01,0.01;
   ik_tgt_list.push_back(tmp);
 
-//  fik->optional_weight_vector(m_robot->link("CHEST_JOINT0")->jointId) = 0.1;
-//  fik->optional_weight_vector(m_robot->link("CHEST_JOINT1")->jointId) = 0.1;
-//  fik->optional_weight_vector(m_robot->link("CHEST_JOINT2")->jointId) = 0.1;
-
   if( m_robot->link("RARM_JOINT2") != NULL) m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇の干渉回避のため
   if( m_robot->link("LARM_JOINT2") != NULL) m_robot->link("LARM_JOINT2")->llimit = deg2rad(40);
 
 //  fik_in->reference_gain.fill(0.001);
 //  fik_in->reference_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
 
-  struct timespec startT, endT;
   int loop_result = 0;
-  const int IK_MAX_LOOP = 3;
+  const int IK_MAX_LOOP = 1;
   loop_result = fik->solveFullbodyIKLoop(ik_tgt_list, IK_MAX_LOOP);
 }
 
diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 5fba62b65bf..157b9727007 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -22,7 +22,6 @@ class IKConstraintParam {
     hrp::Vector3 localPos;
     hrp::Matrix33 localR;
     hrp::dvector6 weight_vec;
-    hrp::ivector selection_vec;
     double pos_precision, rot_precision;
 
     IKConstraintParam ()
@@ -31,7 +30,6 @@ class IKConstraintParam {
        localPos(hrp::Vector3::Zero()),
        localR(hrp::Matrix33::Identity()),
        weight_vec(hrp::dvector6::Ones()),
-       selection_vec(hrp::ivector::Ones(6)),
        pos_precision(1e-3), rot_precision(1e-2)
     {};
 };
@@ -41,7 +39,6 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
   protected:
     hrp::dmatrix J_all, J_all_inv, J_com, J_am;
     hrp::dvector dq_all, dp_ee_all, weight_vec_all;
-    hrp::ivector selection_vec_all;
     int WS_DOF, BASE_DOF, J_DOF, ALL_DOF;
     hrp::Vector3 cur_P,cur_L;
 
@@ -85,8 +82,8 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
           hrp::Vector3 pos_err = _ik_tgt_list[i].targetPos - (target_link_ptr->p + target_link_ptr->R * _ik_tgt_list[i].localPos);
           hrp::Vector3 rot_err = omegaFromRotEx(hrp::rotFromRpy(_ik_tgt_list[i].targetRpy).transpose() * target_link_ptr->R * _ik_tgt_list[i].localR);
           for(int j=0;j<3;j++){
-            if(pos_err(j) > _ik_tgt_list[i].pos_precision && _ik_tgt_list[i].selection_vec(j) != 0){return false;}
-            if(rot_err(j) > _ik_tgt_list[i].rot_precision && _ik_tgt_list[i].selection_vec(j+3) != 0){return false;}
+            if(pos_err(j) > _ik_tgt_list[i].pos_precision && _ik_tgt_list[i].weight_vec(j) > 0){return false;}
+            if(rot_err(j) > _ik_tgt_list[i].rot_precision && _ik_tgt_list[i].weight_vec(j+3) > 0){return false;}
           }
         }
         else if(!target_link_ptr && _ik_tgt_list[i].target_link_name == "COM"){  // COM
@@ -94,7 +91,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
 //            robot_in->calcTotalMomentum(fik_in->cur_P, fik_in->cur_L);
 //            hrp::Vector3 rot_err = omegaFromRotEx(ik_tgt_list[i].target_r.transpose() * ik_tgt_list[i].localR * target_link_ptr->R);
           for(int j=0;j<3;j++){
-            if(pos_err(j) > _ik_tgt_list[i].pos_precision && _ik_tgt_list[i].selection_vec(j) != 0){return false;}
+            if(pos_err(j) > _ik_tgt_list[i].pos_precision && _ik_tgt_list[i].weight_vec(j) > 0){return false;}
 //              if(rot_err(j) > ik_tgt_list[i].rot_precision && ik_tgt_list[i].selection_vec(j+3) != 0){return false;}
           }
         }
@@ -105,7 +102,6 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
       J_all = hrp::dmatrix::Zero(WS_DOF*_ik_tgt_list.size(), ALL_DOF);
       dq_all.fill(0);
       dp_ee_all = hrp::dvector::Zero(WS_DOF*_ik_tgt_list.size());
-      selection_vec_all = hrp::ivector::Zero(WS_DOF*_ik_tgt_list.size());
       weight_vec_all = hrp::dvector::Ones(WS_DOF*_ik_tgt_list.size());
       //リファレンスに微少量戻す
       for(int i=0;i<J_DOF;i++){ _robot->joint(i)->q = _robot->joint(i)->q * (1-reference_gain(i)) + reference_q(i) * reference_gain(i); }
@@ -150,37 +146,37 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
         }
         else{ std::cerr<<"Unknown Link Target !!"<<std::endl; continue; } //不明なリンク指定
         //全体の中に配置
-//        if(vel_p_ref.norm()>0.1){vel_p_ref = vel_p_ref.normalized() * 0.1;}
-//        if(vel_r_ref.norm()>0.1){vel_r_ref = vel_r_ref.normalized() * 0.1;}
+        if(vel_p_ref.norm()>0.1){vel_p_ref = vel_p_ref.normalized() * 0.1;}
+        if(vel_r_ref.norm()>0.1){vel_r_ref = vel_r_ref.normalized() * 0.1;}
         dp_ee_all.segment(WS_DOF*i,WS_DOF) << vel_p_ref, vel_r_ref;
         weight_vec_all.segment(WS_DOF*i, WS_DOF) = _ik_tgt_list[i].weight_vec;
-        selection_vec_all.segment(WS_DOF*i, WS_DOF) = _ik_tgt_list[i].selection_vec;
         J_all.middleRows(WS_DOF*i, WS_DOF) = J_part;
       }
 
-      hrp::dmatrix selection_matrix = hrp::dmatrix::Zero(selection_vec_all.sum(), WS_DOF*_ik_tgt_list.size());
-      for(int i=0, j=0; i<selection_vec_all.rows(); i++){ if(selection_vec_all(i) == 1){ selection_matrix(j, i) = 1.0; j++; } }
+      hrp::dmatrix selection_matrix = hrp::dmatrix::Zero((weight_vec_all.array()>0.0).count(), WS_DOF*_ik_tgt_list.size());
+      for(int i=0, j=0; i<weight_vec_all.rows(); i++){ if(weight_vec_all(i) > 0.0){ selection_matrix(j, i) = 1.0; j++; } }
 
 //      weight_vec_all /= weight_vec_all.maxCoeff();
       J_all = selection_matrix * J_all;
       dp_ee_all = selection_matrix * dp_ee_all;
       weight_vec_all = selection_matrix * weight_vec_all;
-//      J_all *= selection_matrix;
-//      dp_ee_all *= selection_matrix;
-//      weight_vec_all *= selection_matrix;
 
-//      hrp::calcPseudoInverse(J_all,J_all_inv,1.0e-3);//超遅い1[ms]とか
-      calcJacobianInverseNullspace_copy(_robot, J_all, J_all_inv);
+      const double lambda = 0.001;
+      hrp::dmatrix H = J_all.transpose() * weight_vec_all.asDiagonal() * J_all + lambda * hrp::dmatrix::Identity(J_all.cols(), J_all.cols());
+      hrp::dvector g = J_all.transpose() * weight_vec_all.asDiagonal() * dp_ee_all;
+
+      dq_all = H.inverse() * g;
 
-      dq_all = J_all_inv * dp_ee_all; // dq = pseudoInverse(J) * v
       static int count;
       if(count++ % 10000 == 0){
         std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
         std::cout<<std::setprecision(2) << "J_all_inv=\n"<<J_all_inv<<std::setprecision(6)<<std::endl;
-        std::cout<<std::setprecision(2) << "selection_matrix=\n"<<selection_matrix<<std::setprecision(6)<<std::endl;
-        std::cout<<"eevel_all\n"<<dp_ee_all.transpose()<<std::endl;
-        std::cout<<"dq_all\n"<<dq_all.transpose()<<std::endl;
-        std::cout<<"weight_vec_all\n"<<weight_vec_all.transpose()<<std::endl;
+        dbg(selection_matrix);
+        dbg(H);
+        dbg(g.transpose());
+        dbg(dp_ee_all.transpose());
+        dbg(dq_all.transpose());
+        dbg(weight_vec_all.transpose());
         std::cout<<"q_ans_all\n";
         for(int i=0;i<_robot->numJoints();i++)std::cerr<<_robot->joint(i)->q<<" ";
         std::cout<<std::endl;
@@ -201,6 +197,106 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
       _robot->rootLink()->R = base_R_ans;
       _robot->calcForwardKinematics();
     }
+//    void solveFullbodyIK(hrp::BodyPtr& _robot, const std::vector<IKConstraintParam>& _ik_tgt_list){
+//      J_all = hrp::dmatrix::Zero(WS_DOF*_ik_tgt_list.size(), ALL_DOF);
+//      dq_all.fill(0);
+//      dp_ee_all = hrp::dvector::Zero(WS_DOF*_ik_tgt_list.size());
+//      selection_vec_all = hrp::ivector::Zero(WS_DOF*_ik_tgt_list.size());
+//      weight_vec_all = hrp::dvector::Ones(WS_DOF*_ik_tgt_list.size());
+//      //リファレンスに微少量戻す
+//      for(int i=0;i<J_DOF;i++){ _robot->joint(i)->q = _robot->joint(i)->q * (1-reference_gain(i)) + reference_q(i) * reference_gain(i); }
+//      _robot->rootLink()->p = _robot->rootLink()->p.cwiseProduct(hrp::Vector3::Ones()-reference_gain.segment(J_DOF,3)) + reference_q.segment(J_DOF,3).cwiseProduct(reference_gain.segment(J_DOF,3));
+//      _robot->rootLink()->R = hrp::rotFromRpy( hrp::rpyFromRot(_robot->rootLink()->R).cwiseProduct(hrp::Vector3::Ones()-reference_gain.segment(J_DOF+3,3)) + reference_q.segment(J_DOF+3,3).cwiseProduct(reference_gain.segment(J_DOF+3,3)));
+//      for(int i=0;i<J_DOF;i++){ LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit); }
+//      _robot->calcForwardKinematics();
+//      //ヤコビアンと各ベクトル生成
+//      for ( int i=0; i<_ik_tgt_list.size(); i++ ) {
+//        hrp::Link* target_link_ptr = _robot->link(_ik_tgt_list[i].target_link_name);
+//        hrp::Matrix33 ref_link_origin_R(hrp::rotFromRpy(_ik_tgt_list[i].targetRpy) * _ik_tgt_list[i].localR.transpose()); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
+//        hrp::Vector3 ref_link_origin_p(_ik_tgt_list[i].targetPos - ref_link_origin_R * _ik_tgt_list[i].localPos);
+//        hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF); //全身to一拘束点へのヤコビアン
+//        hrp::Vector3 vel_p_ref, vel_r_ref; //velと言いつつ実際は差分
+//        //ベースリンク，通常リンク，重心で場合分け
+//        if(target_link_ptr){//ベースリンク，通常リンク共通
+//          vel_p_ref = ref_link_origin_p - target_link_ptr->p;
+//          vel_r_ref = target_link_ptr->R * omegaFromRotEx(target_link_ptr->R.transpose() * ref_link_origin_R);
+//
+//          if(target_link_ptr == _robot->rootLink()){ //ベース限定
+//            J_part.rightCols(WS_DOF) = hrp::dmatrix::Identity(WS_DOF, WS_DOF);
+//          }
+//          else{ //通常リンク限定
+//            hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), target_link_ptr, m_dt, false, "");
+//            hrp::dmatrix J_jpath;
+//            tgt_jpath.calcJacobian(J_jpath);
+//            for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
+//              int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
+//              J_part.col(id_in_body) = J_jpath.col(id_in_jpath);
+//            }
+//            J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
+//            J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(target_link_ptr->p - _robot->rootLink()->p);
+//          }
+//        }
+//        else if(!target_link_ptr && _ik_tgt_list[i].target_link_name == "COM"){ //重心限定
+//          vel_p_ref = ref_link_origin_p - _robot->calcCM();
+////          vel_r_ref = (omegaFromRotEx(ref_link_origin_R) - (cur_L - _robot->rootLink()->p.cross(cur_P))) * m_dt;// COMのrotはAngulerMomentumとして扱う
+//          vel_r_ref = _ik_tgt_list[i].targetRpy;// COMのrotはAngulerMomentumとして扱う
+//          _robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
+//          _robot->calcAngularMomentumJacobian(NULL, J_am);//すでにrootlink周りの角運動量ヤコビアンが返ってくる？
+//          J_part << J_com, J_am;
+//        }
+//        else{ std::cerr<<"Unknown Link Target !!"<<std::endl; continue; } //不明なリンク指定
+//        //全体の中に配置
+////        if(vel_p_ref.norm()>0.1){vel_p_ref = vel_p_ref.normalized() * 0.1;}
+////        if(vel_r_ref.norm()>0.1){vel_r_ref = vel_r_ref.normalized() * 0.1;}
+//        dp_ee_all.segment(WS_DOF*i,WS_DOF) << vel_p_ref, vel_r_ref;
+//        weight_vec_all.segment(WS_DOF*i, WS_DOF) = _ik_tgt_list[i].weight_vec;
+//        selection_vec_all.segment(WS_DOF*i, WS_DOF) = _ik_tgt_list[i].selection_vec;
+//        J_all.middleRows(WS_DOF*i, WS_DOF) = J_part;
+//      }
+//
+//      hrp::dmatrix selection_matrix = hrp::dmatrix::Zero(selection_vec_all.sum(), WS_DOF*_ik_tgt_list.size());
+//      for(int i=0, j=0; i<selection_vec_all.rows(); i++){ if(selection_vec_all(i) == 1){ selection_matrix(j, i) = 1.0; j++; } }
+//
+////      weight_vec_all /= weight_vec_all.maxCoeff();
+//      J_all = selection_matrix * J_all;
+//      dp_ee_all = selection_matrix * dp_ee_all;
+//      weight_vec_all = selection_matrix * weight_vec_all;
+////      J_all *= selection_matrix;
+////      dp_ee_all *= selection_matrix;
+////      weight_vec_all *= selection_matrix;
+//
+////      hrp::calcPseudoInverse(J_all,J_all_inv,1.0e-3);//超遅い1[ms]とか
+//      calcJacobianInverseNullspace_copy(_robot, J_all, J_all_inv);
+//
+//      dq_all = J_all_inv * dp_ee_all; // dq = pseudoInverse(J) * v
+//      static int count;
+//      if(count++ % 10000 == 0){
+//        std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
+//        std::cout<<std::setprecision(2) << "J_all_inv=\n"<<J_all_inv<<std::setprecision(6)<<std::endl;
+//        std::cout<<std::setprecision(2) << "selection_matrix=\n"<<selection_matrix<<std::setprecision(6)<<std::endl;
+//        std::cout<<"eevel_all\n"<<dp_ee_all.transpose()<<std::endl;
+//        std::cout<<"dq_all\n"<<dq_all.transpose()<<std::endl;
+//        std::cout<<"weight_vec_all\n"<<weight_vec_all.transpose()<<std::endl;
+//        std::cout<<"q_ans_all\n";
+//        for(int i=0;i<_robot->numJoints();i++)std::cerr<<_robot->joint(i)->q<<" ";
+//        std::cout<<std::endl;
+//      }
+//
+//      const double LAMBDA = 1.0;
+//      // check validity
+//      hrp::Matrix33 base_R_ans = hrp::hat(dq_all.segment(J_DOF+3,3) * LAMBDA).exp() * _robot->rootLink()->R;
+//      if(!base_R_ans.isUnitary()){ std::cerr <<"ERROR base_R_ans is not Unitary" << std::endl; return; }
+//      for(int i=0;i<dq_all.rows();i++){ if( isnan(dq_all(i)) || isinf(dq_all(i)) ){ std::cerr <<"ERROR nan/inf is found" << std::endl; return;} }
+//
+//      //関節角+ベース位置姿勢更新
+//      for(int i=0;i<J_DOF;i++){
+//        _robot->joint(i)->q += dq_all(i) * LAMBDA/* * m_dt*/;
+//        LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit);
+//      }
+//      _robot->rootLink()->p += dq_all.segment(J_DOF,3) * LAMBDA/* * m_dt*/;//vel計算する時からm_dt使ってないから使わなくていい
+//      _robot->rootLink()->R = base_R_ans;
+//      _robot->calcForwardKinematics();
+//    }
 
 
     bool calcJacobianInverseNullspace_copy(hrp::BodyPtr _robot, hrp::dmatrix &_J, hrp::dmatrix &_Jinv) {
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 56c8cb5083f..3b641a50e6a 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -173,8 +173,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     q_ip->setName(std::string(m_profile.instance_name)+" q_ip");
     q_ip->clear();
     // Generate FIK
-    fik = fikPtr(new FullbodyInverseKinematicsSolverMT(m_robot, std::string(m_profile.instance_name), m_dt));
-    fik_ml = fikPtr(new FullbodyInverseKinematicsSolverMT(m_robot_ml, std::string(m_profile.instance_name), m_dt));
+    fik = fikPtr(new FullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
+    fik_ml = fikPtr(new FullbodyInverseKinematicsSolver(m_robot_ml, std::string(m_profile.instance_name), m_dt));
     fik_list.push_back(fik);
     fik_list.push_back(fik_ml);
     body_list.push_back(m_robot);
@@ -574,8 +574,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.localR = hrp::Matrix33::Identity();
   tmp.targetPos = robot_in->rootLink()->p;// will be ignored by selection_vec
   tmp.targetRpy = com_ref.rpy;// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
-  tmp.selection_vec << 0,0,0,1,1,1;
-  tmp.weight_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 0,0,0,1,1,1;
   ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = eename_ikcp_map["rleg"].target_link_name;
@@ -583,9 +582,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.localR = eename_ikcp_map["rleg"].localR;
   tmp.targetPos = rf_ref.p;
   tmp.targetRpy = rf_ref.rpy;
-  tmp.selection_vec << 1,1,1,1,1,1;
-  tmp.weight_vec << 1,1,3,1,1,1;
-//  tmp.weight_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 1,1,5,1,1,1;
   ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = eename_ikcp_map["lleg"].target_link_name;
@@ -593,9 +590,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.localR = eename_ikcp_map["lleg"].localR;
   tmp.targetPos = lf_ref.p;
   tmp.targetRpy = lf_ref.rpy;
-  tmp.selection_vec << 1,1,1,1,1,1;
-  tmp.weight_vec << 1,1,3,1,1,1;
-//  tmp.weight_vec << 1,1,1,1,1,1;
+  tmp.weight_vec << 1,1,5,1,1,1;
   ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = eename_ikcp_map["rarm"].target_link_name;
@@ -603,8 +598,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.localR = eename_ikcp_map["rarm"].localR;
   tmp.targetPos = rh_ref.p;
   tmp.targetRpy = rh_ref.rpy;
-  tmp.selection_vec << 1,1,1,1,1,1;
-  tmp.weight_vec << 1,1,1,1,1,1;
+  tmp.weight_vec = hrp::dvector6::Constant(0.5);
   ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = eename_ikcp_map["larm"].target_link_name;
@@ -612,8 +606,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.localR = eename_ikcp_map["larm"].localR;
   tmp.targetPos = lh_ref.p;
   tmp.targetRpy = lh_ref.rpy;
-  tmp.selection_vec << 1,1,1,1,1,1;
-  tmp.weight_vec << 1,1,1,1,1,1;
+  tmp.weight_vec = hrp::dvector6::Constant(0.5);
   ik_tgt_list.push_back(tmp);
 
   tmp.target_link_name = "COM";
@@ -621,17 +614,14 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
   tmp.localR = hrp::Matrix33::Identity();
   tmp.targetPos = com_ref.p;// COM height will not be constraint
   tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
-//  tmp.selection_vec << 1,1,1,0,0,0; // COM pos + Ang Momentum
-  tmp.selection_vec << 1,1,1,0,0,0; // COM pos + Ang Momentum
-  tmp.weight_vec << 3,3,0.5,1e-4,1e-4,1e-4;
+  tmp.weight_vec << 100,100,0.5,1e-3,1e-3,1e-3;
 //  tmp.weight_vec << 1,1,1,1,1,1;
   ik_tgt_list.push_back(tmp);
 
   if(robot_in->link("HEAD_JOINT1") != NULL){
     tmp.target_link_name = "HEAD_JOINT1";
     tmp.targetRpy = head_ref.rpy;
-    tmp.selection_vec << 0,0,0,0,1,1;
-    tmp.weight_vec << 1,1,1,1,1,1;
+    tmp.weight_vec << 0,0,0,0,1e-1,1e-1;
     ik_tgt_list.push_back(tmp);
   }
 
@@ -639,18 +629,18 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
 //  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
 //  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
 //  fik_in->optional_weight_vector.tail(6).fill(0.01);
-  if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->ulimit = deg2rad(-30);//脇の干渉回避のため
-  if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(30);
+  if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇の干渉回避のため
+  if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(40);
 
   fik_in->reference_q = init_sync_state;
-//  fik_in->reference_gain.fill(0.001);
-//  fik_in->reference_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
+  fik_in->reference_gain.fill(0.001);
+  fik_in->reference_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
 
   struct timespec startT, endT;
   int loop_result = 0;
-  const int IK_MAX_LOOP = 3;
+  const int IK_MAX_LOOP = 1;
   clock_gettime(CLOCK_REALTIME, &startT);
-  loop_result = fik_in->solveFullbodyIKLoopMT(ik_tgt_list, IK_MAX_LOOP);
+  loop_result = fik_in->solveFullbodyIKLoop(ik_tgt_list, IK_MAX_LOOP);
   if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveIK"<<loop_result<<"loop" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 }
 #else
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 83f311be9ea..e4d4df06496 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -22,7 +22,7 @@
 
 using namespace RTC;
 
-#define USE_DEBUG_PORT
+//#define USE_DEBUG_PORT
 
 enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE};
 
@@ -164,7 +164,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
   };
 //  typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
 //  typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
-  typedef boost::shared_ptr<FullbodyInverseKinematicsSolverMT> fikPtr;
+  typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
   fikPtr fik, fik_ml;
   hrp::BodyPtr m_robot, m_robot_ml, m_robot_vsafe;
   std::vector<fikPtr> fik_list;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 3a32986ced2..281f68c0287 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -163,7 +163,7 @@ class WBMSCore : UTIL_CONST {
         hrp::Vector3 com, zmp;
     }act_rs;
 
-    typedef boost::shared_ptr<FullbodyInverseKinematicsSolverMT> fikPtr;
+    typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
     fikPtr fik_ml;
     hrp::BodyPtr m_robot_ml;
 //    fikPtr fik_act;
@@ -319,7 +319,7 @@ class WBMSCore : UTIL_CONST {
       overwriteFootZFromFootLandOnCommand (rp_ref_out);
       setFootRotHorizontalIfGoLanding     (rp_ref_out);
 
-      limitManipulability                 (rp_ref_out);
+//      limitManipulability                 (rp_ref_out);
 
       applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
 
@@ -541,7 +541,7 @@ class WBMSCore : UTIL_CONST {
       }
       const int pre_ik_loop = 2;
       for(int i=0;i<pre_ik_loop;i++){
-        for ( std::map<std::string, FullbodyInverseKinematicsSolverMT::IKparam>::iterator it = fik_ml->ikp.begin(); it != fik_ml->ikp.end(); it++ ) {
+        for ( std::map<std::string, FullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_ml->ikp.begin(); it != fik_ml->ikp.end(); it++ ) {
             if (it->second.is_ik_enable) fik_ml->solveLimbIK (it->second, it->first, fik_ml->ratio_for_vel, false);
         }
       }

From 0bd43cac241a51a069772d6d9fe32b2dda2287fc Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 2 Sep 2017 09:06:37 +0900
Subject: [PATCH 123/217] tune LM

---
 rtc/AutoBalancer/AutoBalancer.cpp             | 2580 +++++++++--------
 .../FullbodyInverseKinematicsSolver.h         |  808 +++---
 rtc/ImpedanceController/JointPathEx.cpp       |   15 +
 rtc/ImpedanceController/JointPathEx.h         |    1 +
 .../WholeBodyMasterSlave.cpp                  | 1208 ++++----
 .../WholeBodyMasterSlave.h                    |  182 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 1692 +++++------
 7 files changed, 3220 insertions(+), 3266 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 549a7a40a74..6c977c670cd 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -23,7 +23,7 @@ using namespace rats;
 // Module specification
 // <rtc-template block="module_spec">
 static const char* autobalancer_spec[] =
-    {
+{
         "implementation_id", "AutoBalancer",
         "type_name",         "AutoBalancer",
         "description",       "autobalancer component",
@@ -37,7 +37,7 @@ static const char* autobalancer_spec[] =
         // Configuration variables
         "conf.default.debugLevel", "0",
         ""
-    };
+};
 // </rtc-template>
 
 static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
@@ -45,43 +45,43 @@ static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
     int pre = os.precision();
     os.setf(std::ios::fixed);
     os << std::setprecision(6)
-       << (tm.sec + tm.nsec/1e9)
-       << std::setprecision(pre);
+    << (tm.sec + tm.nsec/1e9)
+    << std::setprecision(pre);
     os.unsetf(std::ios::fixed);
     return os;
 }
 
 AutoBalancer::AutoBalancer(RTC::Manager* manager)
-    : RTC::DataFlowComponentBase(manager),
-      // <rtc-template block="initializer">
-      m_qRefIn("qRef", m_qRef),
-      m_basePosIn("basePosIn", m_basePos),
-      m_baseRpyIn("baseRpyIn", m_baseRpy),
-      m_zmpIn("zmpIn", m_zmp),
-      m_optionalDataIn("optionalData", m_optionalData),
-      m_emergencySignalIn("emergencySignal", m_emergencySignal),
-      m_diffCPIn("diffCapturePoint", m_diffCP),
-      m_refFootOriginExtMomentIn("refFootOriginExtMoment", m_refFootOriginExtMoment),
-      m_refFootOriginExtMomentIsHoldValueIn("refFootOriginExtMomentIsHoldValue", m_refFootOriginExtMomentIsHoldValue),
-      m_actContactStatesIn("actContactStates", m_actContactStates),
-      m_qOut("q", m_qRef),
-      m_zmpOut("zmpOut", m_zmp),
-      m_basePosOut("basePosOut", m_basePos),
-      m_baseRpyOut("baseRpyOut", m_baseRpy),
-      m_baseTformOut("baseTformOut", m_baseTform),
-      m_basePoseOut("basePoseOut", m_basePose),
-      m_accRefOut("accRef", m_accRef),
-      m_contactStatesOut("contactStates", m_contactStates),
-      m_toeheelRatioOut("toeheelRatio", m_toeheelRatio),
-      m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime),
-      m_walkingStatesOut("walkingStates", m_walkingStates),
-      m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
-      m_cogOut("cogOut", m_cog),
-      m_AutoBalancerServicePort("AutoBalancerService"),
-      // </rtc-template>
-      gait_type(BIPED),
-      m_robot(hrp::BodyPtr()),
-      m_debugLevel(0)
+: RTC::DataFlowComponentBase(manager),
+  // <rtc-template block="initializer">
+  m_qRefIn("qRef", m_qRef),
+  m_basePosIn("basePosIn", m_basePos),
+  m_baseRpyIn("baseRpyIn", m_baseRpy),
+  m_zmpIn("zmpIn", m_zmp),
+  m_optionalDataIn("optionalData", m_optionalData),
+  m_emergencySignalIn("emergencySignal", m_emergencySignal),
+  m_diffCPIn("diffCapturePoint", m_diffCP),
+  m_refFootOriginExtMomentIn("refFootOriginExtMoment", m_refFootOriginExtMoment),
+  m_refFootOriginExtMomentIsHoldValueIn("refFootOriginExtMomentIsHoldValue", m_refFootOriginExtMomentIsHoldValue),
+  m_actContactStatesIn("actContactStates", m_actContactStates),
+  m_qOut("q", m_qRef),
+  m_zmpOut("zmpOut", m_zmp),
+  m_basePosOut("basePosOut", m_basePos),
+  m_baseRpyOut("baseRpyOut", m_baseRpy),
+  m_baseTformOut("baseTformOut", m_baseTform),
+  m_basePoseOut("basePoseOut", m_basePose),
+  m_accRefOut("accRef", m_accRef),
+  m_contactStatesOut("contactStates", m_contactStates),
+  m_toeheelRatioOut("toeheelRatio", m_toeheelRatio),
+  m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime),
+  m_walkingStatesOut("walkingStates", m_walkingStates),
+  m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
+  m_cogOut("cogOut", m_cog),
+  m_AutoBalancerServicePort("AutoBalancerService"),
+  // </rtc-template>
+  gait_type(BIPED),
+  m_robot(hrp::BodyPtr()),
+  m_debugLevel(0)
 {
     m_service0.autobalancer(this);
 }
@@ -124,19 +124,19 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     addOutPort("cogOut", m_cogOut);
     addOutPort("walkingStates", m_walkingStatesOut);
     addOutPort("sbpCogOffset", m_sbpCogOffsetOut);
-  
+
     // Set service provider to Ports
     m_AutoBalancerServicePort.registerProvider("service0", "AutoBalancerService", m_service0);
-  
+
     // Set service consumers to Ports
-  
+
     // Set CORBA Service Ports
     addPort(m_AutoBalancerServicePort);
-  
+
     // </rtc-template>
     // <rtc-template block="bind_config">
     // Bind variables and configuration variable
-  
+
     // </rtc-template>
 
     RTC::Properties& prop =  getProperties();
@@ -152,10 +152,10 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     nameServer = nameServer.substr(0, comPos);
     RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
     if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
-                                 CosNaming::NamingContext::_duplicate(naming.getRootContext())
-                                 )){
-      std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
-      return RTC::RTC_ERROR;
+            CosNaming::NamingContext::_duplicate(naming.getRootContext())
+    )){
+        std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
+        return RTC::RTC_ERROR;
     }
 
     // allocate memory for outPorts
@@ -173,7 +173,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     leg_names.push_back("lleg");
 
     // Generate FIK
-//    fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
+    //    fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
     fik = fikPtr(new FullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
 
     // setting from conf file
@@ -181,77 +181,77 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
     size_t prop_num = 10;
     if (end_effectors_str.size() > 0) {
-      size_t num = end_effectors_str.size()/prop_num;
-      for (size_t i = 0; i < num; i++) {
-        std::string ee_name, ee_target, ee_base;
-        coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
-        coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
-        coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
-        ABCIKparam tp;
-        hrp::Link* root = m_robot->link(ee_target);
-        for (size_t j = 0; j < 3; j++) {
-          coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
-        }
-        double tmpv[4];
-        for (int j = 0; j < 4; j++ ) {
-          coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
+        size_t num = end_effectors_str.size()/prop_num;
+        for (size_t i = 0; i < num; i++) {
+            std::string ee_name, ee_target, ee_base;
+            coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
+            coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
+            coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
+            ABCIKparam tp;
+            hrp::Link* root = m_robot->link(ee_target);
+            for (size_t j = 0; j < 3; j++) {
+                coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
+            }
+            double tmpv[4];
+            for (int j = 0; j < 4; j++ ) {
+                coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
+            }
+            tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
+            // FIK param
+            SimpleFullbodyInverseKinematicsSolver::IKparam tmp_fikp;
+            tmp_fikp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
+            tmp_fikp.target_link = m_robot->link(ee_target);
+            tmp_fikp.localPos = tp.localPos;
+            tmp_fikp.localR = tp.localR;
+            tmp_fikp.max_limb_length = 0.0;
+            while (!root->isRoot()) {
+                tmp_fikp.max_limb_length += root->b.norm();
+                tmp_fikp.parent_name = root->name;
+                root = root->parent;
+            }
+            fik->ikp.insert(std::pair<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>(ee_name, tmp_fikp));
+            // Fix for toe joint
+            //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
+            //   Without toe joints, "end-effector link == force-sensor link" is assumed.
+            //   With toe joints, "end-effector link != force-sensor link" is assumed.
+            if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
+                std::vector<double> optw(fik->ikp[ee_name].manip->numJoints(), 1.0);
+                optw.back() = 0.0; // Set weight = 0 for toe joint by default
+                fik->ikp[ee_name].manip->setOptionalWeightVector(optw);
+                tp.has_toe_joint = true;
+            } else {
+                tp.has_toe_joint = false;
+            }
+            tp.target_link = m_robot->link(ee_target);
+            ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
+            ee_vec.push_back(ee_name);
+            std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
+            std::cerr << "[" << m_profile.instance_name << "]   target = " << ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
+            std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+            std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
+            contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
         }
-        tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
-        // FIK param
-        SimpleFullbodyInverseKinematicsSolver::IKparam tmp_fikp;
-        tmp_fikp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
-        tmp_fikp.target_link = m_robot->link(ee_target);
-        tmp_fikp.localPos = tp.localPos;
-        tmp_fikp.localR = tp.localR;
-        tmp_fikp.max_limb_length = 0.0;
-        while (!root->isRoot()) {
-          tmp_fikp.max_limb_length += root->b.norm();
-          tmp_fikp.parent_name = root->name;
-          root = root->parent;
+        m_contactStates.data.length(num);
+        m_toeheelRatio.data.length(num);
+        if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
+            m_contactStates.data[contact_states_index_map["rleg"]] = true;
+            m_contactStates.data[contact_states_index_map["lleg"]] = true;
         }
-        fik->ikp.insert(std::pair<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>(ee_name, tmp_fikp));
-        // Fix for toe joint
-        //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
-        //   Without toe joints, "end-effector link == force-sensor link" is assumed.
-        //   With toe joints, "end-effector link != force-sensor link" is assumed.
-        if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
-            std::vector<double> optw(fik->ikp[ee_name].manip->numJoints(), 1.0);
-            optw.back() = 0.0; // Set weight = 0 for toe joint by default
-            fik->ikp[ee_name].manip->setOptionalWeightVector(optw);
-            tp.has_toe_joint = true;
-        } else {
-            tp.has_toe_joint = false;
+        if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) {
+            m_contactStates.data[contact_states_index_map["rarm"]] = false;
+            m_contactStates.data[contact_states_index_map["larm"]] = false;
         }
-        tp.target_link = m_robot->link(ee_target);
-        ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
-        ee_vec.push_back(ee_name);
-        std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   target = " << ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-        std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
-        contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
-      }
-      m_contactStates.data.length(num);
-      m_toeheelRatio.data.length(num);
-      if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
-        m_contactStates.data[contact_states_index_map["rleg"]] = true;
-        m_contactStates.data[contact_states_index_map["lleg"]] = true;
-      }
-      if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) {
-        m_contactStates.data[contact_states_index_map["rarm"]] = false;
-        m_contactStates.data[contact_states_index_map["larm"]] = false;
-      }
-      m_controlSwingSupportTime.data.length(num);
-      for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 0.0;
-      for (size_t i = 0; i < num; i++) m_toeheelRatio.data[i] = rats::no_using_toe_heel_ratio;
+        m_controlSwingSupportTime.data.length(num);
+        for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 0.0;
+        for (size_t i = 0; i < num; i++) m_toeheelRatio.data[i] = rats::no_using_toe_heel_ratio;
     }
     std::vector<hrp::Vector3> leg_pos;
     if (leg_offset_str.size() > 0) {
-      hrp::Vector3 leg_offset;
-      for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
-      std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-      leg_pos.push_back(hrp::Vector3(-1*leg_offset));
-      leg_pos.push_back(hrp::Vector3(leg_offset));
+        hrp::Vector3 leg_offset;
+        for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
+        std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+        leg_pos.push_back(hrp::Vector3(-1*leg_offset));
+        leg_pos.push_back(hrp::Vector3(leg_offset));
     }
     if (leg_pos.size() < ikp.size()) {
         size_t tmp_leg_pos_size = leg_pos.size();
@@ -289,12 +289,12 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     double stride_inside_th_limit = stride_outside_th_limit*0.5;
     std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_outside_y_limit << "[m], " << stride_outside_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl;
     if (default_zmp_offsets.size() == 0) {
-      for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
+        for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
     }
     if (leg_offset_str.size() > 0) {
-      gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_outside_y_limit/*[m]*/, stride_outside_th_limit/*[deg]*/,
-                                          stride_bwd_x_limit/*[m]*/, stride_inside_y_limit/*[m]*/, stride_inside_th_limit/*[m]*/));
-      gg->set_default_zmp_offsets(default_zmp_offsets);
+        gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_outside_y_limit/*[m]*/, stride_outside_th_limit/*[deg]*/,
+                stride_bwd_x_limit/*[m]*/, stride_inside_y_limit/*[m]*/, stride_inside_th_limit/*[m]*/));
+        gg->set_default_zmp_offsets(default_zmp_offsets);
     }
     gg_is_walking = gg_solved = false;
     m_walkingStates.data = false;
@@ -359,9 +359,9 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     use_force = MODE_REF_FORCE;
 
     if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
-      is_legged_robot = true;
+        is_legged_robot = true;
     } else {
-      is_legged_robot = false;
+        is_legged_robot = false;
     }
 
     m_accRef.data.ax = m_accRef.data.ay = m_accRef.data.az = 0.0;
@@ -388,11 +388,11 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
 
 RTC::ReturnCode_t AutoBalancer::onFinalize()
 {
-  delete zmp_offset_interpolator;
-  delete transition_interpolator;
-  delete adjust_footstep_interpolator;
-  delete leg_names_interpolator;
-  return RTC::RTC_OK;
+    delete zmp_offset_interpolator;
+    delete transition_interpolator;
+    delete adjust_footstep_interpolator;
+    delete leg_names_interpolator;
+    return RTC::RTC_OK;
 }
 
 /*
@@ -400,32 +400,32 @@ RTC::ReturnCode_t AutoBalancer::onFinalize()
   {
   return RTC::RTC_OK;
   }
-*/
+ */
 
 /*
   RTC::ReturnCode_t AutoBalancer::onShutdown(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
-*/
+ */
 
 RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id)
 {
     std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
-    
+
     return RTC::RTC_OK;
 }
 
 RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
 {
-  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
-  Guard guard(m_mutex);
-  if (control_mode == MODE_ABC) {
-    control_mode = MODE_SYNC_TO_IDLE;
-    double tmp_ratio = 0.0;
-    transition_interpolator->setGoal(&tmp_ratio, m_dt, true); // sync in one controller loop
-  }
-  return RTC::RTC_OK;
+    std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
+    Guard guard(m_mutex);
+    if (control_mode == MODE_ABC) {
+        control_mode = MODE_SYNC_TO_IDLE;
+        double tmp_ratio = 0.0;
+        transition_interpolator->setGoal(&tmp_ratio, m_dt, true); // sync in one controller loop
+    }
+    return RTC::RTC_OK;
 }
 
 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
@@ -433,7 +433,7 @@ RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
 #define DEBUGP2 (false)
 RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
 {
-  // std::cerr << "AutoBalancer::onExecute(" << ec_id << ")" << std::endl;
+    // std::cerr << "AutoBalancer::onExecute(" << ec_id << ")" << std::endl;
     loop ++;
 
     // Read Inport
@@ -441,20 +441,20 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
         m_qRefIn.read();
     }
     if (m_basePosIn.isNew()) {
-      m_basePosIn.read();
-      input_basePos(0) = m_basePos.data.x;
-      input_basePos(1) = m_basePos.data.y;
-      input_basePos(2) = m_basePos.data.z;
+        m_basePosIn.read();
+        input_basePos(0) = m_basePos.data.x;
+        input_basePos(1) = m_basePos.data.y;
+        input_basePos(2) = m_basePos.data.z;
     }
     if (m_baseRpyIn.isNew()) {
-      m_baseRpyIn.read();
-      input_baseRot = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
+        m_baseRpyIn.read();
+        input_baseRot = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
     }
     if (m_zmpIn.isNew()) {
-      m_zmpIn.read();
-      input_zmp(0) = m_zmp.data.x;
-      input_zmp(1) = m_zmp.data.y;
-      input_zmp(2) = m_zmp.data.z;
+        m_zmpIn.read();
+        input_zmp(0) = m_zmp.data.x;
+        input_zmp(1) = m_zmp.data.y;
+        input_zmp(2) = m_zmp.data.z;
     }
     for (unsigned int i=0; i<m_ref_forceIn.size(); i++){
         if ( m_ref_forceIn[i]->isNew() ) {
@@ -473,22 +473,22 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
         // }
     }
     if (m_diffCPIn.isNew()) {
-      m_diffCPIn.read();
-      gg->set_diff_cp(hrp::Vector3(m_diffCP.data.x, m_diffCP.data.y, m_diffCP.data.z));
+        m_diffCPIn.read();
+        gg->set_diff_cp(hrp::Vector3(m_diffCP.data.x, m_diffCP.data.y, m_diffCP.data.z));
     }
     if (m_refFootOriginExtMomentIn.isNew()) {
-      m_refFootOriginExtMomentIn.read();
+        m_refFootOriginExtMomentIn.read();
     }
     if (m_refFootOriginExtMomentIsHoldValueIn.isNew()) {
-      m_refFootOriginExtMomentIsHoldValueIn.read();
+        m_refFootOriginExtMomentIsHoldValueIn.read();
     }
     if (m_actContactStatesIn.isNew()) {
-      m_actContactStatesIn.read();
-      std::vector<bool> tmp_contacts(m_actContactStates.data.length());
-      for (size_t i = 0; i < m_actContactStates.data.length(); i++) {
-        tmp_contacts[i] = m_actContactStates.data[i];
-      }
-      gg->set_act_contact_states(tmp_contacts);
+        m_actContactStatesIn.read();
+        std::vector<bool> tmp_contacts(m_actContactStates.data.length());
+        for (size_t i = 0; i < m_actContactStates.data.length(); i++) {
+            tmp_contacts[i] = m_actContactStates.data[i];
+        }
+        gg->set_act_contact_states(tmp_contacts);
     }
 
     // Calculation
@@ -497,167 +497,167 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     hrp::Matrix33 ref_baseRot;
     hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
     if ( is_legged_robot ) {
-      // For parameters
-      fik->storeCurrentParameters();
-      getTargetParameters();
-      // Get transition ratio
-      bool is_transition_interpolator_empty = transition_interpolator->isEmpty();
-      if (!is_transition_interpolator_empty) {
-        transition_interpolator->get(&transition_interpolator_ratio, true);
-      } else {
-        transition_interpolator_ratio = (control_mode == MODE_IDLE) ? 0.0 : 1.0;
-      }
-      if (control_mode != MODE_IDLE ) {
-        solveFullbodyIK();
-//        /////// Inverse Dynamics /////////
-//        if(!idsb.is_initialized){
-//          idsb.setInitState(m_robot, m_dt);
-//          invdyn_zmp_filters.resize(3);
-//          for(int i=0;i<3;i++){
-//            invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
-//            invdyn_zmp_filters[i].reset(ref_zmp(i));
-//          }
-//        }
-//        calcAccelerationsForInverseDynamics(m_robot, idsb);
-//        if(gg_is_walking){
-//          calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp);
-//          for(int i=0;i<3;i++) ref_zmp(i) = invdyn_zmp_filters[i].passFilter(ref_zmp(i));
-//        }
-//        updateInvDynStateBuffer(idsb);
-
-        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
-      } else {
-        rel_ref_zmp = input_zmp;
-        fik->d_root_height = 0.0;
-      }
-      // Transition
-      if (!is_transition_interpolator_empty) {
-        // transition_interpolator_ratio 0=>1 : IDLE => ABC
-        // transition_interpolator_ratio 1=>0 : ABC => IDLE
-        ref_basePos = (1-transition_interpolator_ratio) * input_basePos + transition_interpolator_ratio * m_robot->rootLink()->p;
-        rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp;
-        rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R);
-        for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ) {
-          m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q;
+        // For parameters
+        fik->storeCurrentParameters();
+        getTargetParameters();
+        // Get transition ratio
+        bool is_transition_interpolator_empty = transition_interpolator->isEmpty();
+        if (!is_transition_interpolator_empty) {
+            transition_interpolator->get(&transition_interpolator_ratio, true);
+        } else {
+            transition_interpolator_ratio = (control_mode == MODE_IDLE) ? 0.0 : 1.0;
         }
-        for (unsigned int i=0; i< m_force.size(); i++) {
-            for (unsigned int j=0; j<6; j++) {
-                m_force[i].data[j] = transition_interpolator_ratio * m_force[i].data[j] + (1-transition_interpolator_ratio) * m_ref_force[i].data[j];
+        if (control_mode != MODE_IDLE ) {
+            solveFullbodyIK();
+            //        /////// Inverse Dynamics /////////
+            //        if(!idsb.is_initialized){
+            //          idsb.setInitState(m_robot, m_dt);
+            //          invdyn_zmp_filters.resize(3);
+            //          for(int i=0;i<3;i++){
+            //            invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
+            //            invdyn_zmp_filters[i].reset(ref_zmp(i));
+            //          }
+            //        }
+            //        calcAccelerationsForInverseDynamics(m_robot, idsb);
+            //        if(gg_is_walking){
+            //          calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp);
+            //          for(int i=0;i<3;i++) ref_zmp(i) = invdyn_zmp_filters[i].passFilter(ref_zmp(i));
+            //        }
+            //        updateInvDynStateBuffer(idsb);
+
+            rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
+        } else {
+            rel_ref_zmp = input_zmp;
+            fik->d_root_height = 0.0;
+        }
+        // Transition
+        if (!is_transition_interpolator_empty) {
+            // transition_interpolator_ratio 0=>1 : IDLE => ABC
+            // transition_interpolator_ratio 1=>0 : ABC => IDLE
+            ref_basePos = (1-transition_interpolator_ratio) * input_basePos + transition_interpolator_ratio * m_robot->rootLink()->p;
+            rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp;
+            rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R);
+            for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ) {
+                m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q;
+            }
+            for (unsigned int i=0; i< m_force.size(); i++) {
+                for (unsigned int j=0; j<6; j++) {
+                    m_force[i].data[j] = transition_interpolator_ratio * m_force[i].data[j] + (1-transition_interpolator_ratio) * m_ref_force[i].data[j];
+                }
+            }
+            for (unsigned int i=0; i< m_limbCOPOffset.size(); i++) {
+                // transition (TODO:set stopABCmode value instead of 0)
+                m_limbCOPOffset[i].data.x = transition_interpolator_ratio * m_limbCOPOffset[i].data.x;// + (1-transition_interpolator_ratio) * 0;
+                m_limbCOPOffset[i].data.y = transition_interpolator_ratio * m_limbCOPOffset[i].data.y;// + (1-transition_interpolator_ratio) * 0;
+                m_limbCOPOffset[i].data.z = transition_interpolator_ratio * m_limbCOPOffset[i].data.z;// + (1-transition_interpolator_ratio) * 0;
             }
+        } else {
+            ref_basePos = m_robot->rootLink()->p;
+            ref_baseRot = m_robot->rootLink()->R;
         }
-        for (unsigned int i=0; i< m_limbCOPOffset.size(); i++) {
-            // transition (TODO:set stopABCmode value instead of 0)
-            m_limbCOPOffset[i].data.x = transition_interpolator_ratio * m_limbCOPOffset[i].data.x;// + (1-transition_interpolator_ratio) * 0;
-            m_limbCOPOffset[i].data.y = transition_interpolator_ratio * m_limbCOPOffset[i].data.y;// + (1-transition_interpolator_ratio) * 0;
-            m_limbCOPOffset[i].data.z = transition_interpolator_ratio * m_limbCOPOffset[i].data.z;// + (1-transition_interpolator_ratio) * 0;
+        // mode change for sync
+        if (control_mode == MODE_SYNC_TO_ABC) {
+            control_mode = MODE_ABC;
+        } else if (control_mode == MODE_SYNC_TO_IDLE && transition_interpolator->isEmpty() ) {
+            std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
+                    << "] Finished cleanup" << std::endl;
+            control_mode = MODE_IDLE;
         }
-      } else {
-        ref_basePos = m_robot->rootLink()->p;
-        ref_baseRot = m_robot->rootLink()->R;
-      }
-      // mode change for sync
-      if (control_mode == MODE_SYNC_TO_ABC) {
-        control_mode = MODE_ABC;
-      } else if (control_mode == MODE_SYNC_TO_IDLE && transition_interpolator->isEmpty() ) {
-        std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
-                  << "] Finished cleanup" << std::endl;
-        control_mode = MODE_IDLE;
-      }
     }
 
     // Write Outport
     if ( m_qRef.data.length() != 0 ) { // initialized
-      if (is_legged_robot) {
-        for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
-          m_qRef.data[i] = m_robot->joint(i)->q;
+        if (is_legged_robot) {
+            for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
+                m_qRef.data[i] = m_robot->joint(i)->q;
+            }
         }
-      }
-      m_qOut.write();
+        m_qOut.write();
     }
     if (is_legged_robot) {
-      // basePos
-      m_basePos.data.x = ref_basePos(0);
-      m_basePos.data.y = ref_basePos(1);
-      m_basePos.data.z = ref_basePos(2);
-      m_basePos.tm = m_qRef.tm;
-      // baseRpy
-      hrp::Vector3 baseRpy = hrp::rpyFromRot(ref_baseRot);
-      m_baseRpy.data.r = baseRpy(0);
-      m_baseRpy.data.p = baseRpy(1);
-      m_baseRpy.data.y = baseRpy(2);
-      m_baseRpy.tm = m_qRef.tm;
-      // baseTform
-      double *tform_arr = m_baseTform.data.get_buffer();
-      tform_arr[0] = m_basePos.data.x;
-      tform_arr[1] = m_basePos.data.y;
-      tform_arr[2] = m_basePos.data.z;
-      hrp::setMatrix33ToRowMajorArray(ref_baseRot, tform_arr, 3);
-      m_baseTform.tm = m_qRef.tm;
-      // basePose
-      m_basePose.data.position.x = m_basePos.data.x;
-      m_basePose.data.position.y = m_basePos.data.y;
-      m_basePose.data.position.z = m_basePos.data.z;
-      m_basePose.data.orientation.r = m_baseRpy.data.r;
-      m_basePose.data.orientation.p = m_baseRpy.data.p;
-      m_basePose.data.orientation.y = m_baseRpy.data.y;
-      m_basePose.tm = m_qRef.tm;
-      // zmp
-      m_zmp.data.x = rel_ref_zmp(0);
-      m_zmp.data.y = rel_ref_zmp(1);
-      m_zmp.data.z = rel_ref_zmp(2);
-      m_zmp.tm = m_qRef.tm;
-      // cog
-      m_cog.data.x = ref_cog(0);
-      m_cog.data.y = ref_cog(1);
-      m_cog.data.z = ref_cog(2);
-      m_cog.tm = m_qRef.tm;
-      // sbpCogOffset
-      m_sbpCogOffset.data.x = sbp_cog_offset(0);
-      m_sbpCogOffset.data.y = sbp_cog_offset(1);
-      m_sbpCogOffset.data.z = sbp_cog_offset(2);
-      m_sbpCogOffset.tm = m_qRef.tm;
-      // write
-      m_basePosOut.write();
-      m_baseRpyOut.write();
-      m_baseTformOut.write();
-      m_basePoseOut.write();
-      m_zmpOut.write();
-      m_cogOut.write();
-      m_sbpCogOffsetOut.write();
-
-      // reference acceleration
-      hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
-      if (sen != NULL) {
-          hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos;
-          hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt;
-          // convert to imu sensor local acceleration
-          hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt;
-          m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2);
-          m_accRefOut.write();
-          prev_imu_sensor_pos = imu_sensor_pos;
-          prev_imu_sensor_vel = imu_sensor_vel;
-      }
-
-      // control parameters
-      m_contactStates.tm = m_qRef.tm;
-      m_contactStatesOut.write();
-      m_controlSwingSupportTime.tm = m_qRef.tm;
-      m_controlSwingSupportTimeOut.write();
-      m_toeheelRatio.tm = m_qRef.tm;
-      m_toeheelRatioOut.write();
-      m_walkingStates.data = gg_is_walking;
-      m_walkingStates.tm = m_qRef.tm;
-      m_walkingStatesOut.write();
-
-      for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
-          m_force[i].tm = m_qRef.tm;
-          m_ref_forceOut[i]->write();
-      }
-
-      for (unsigned int i=0; i<m_limbCOPOffsetOut.size(); i++){
-          m_limbCOPOffset[i].tm = m_qRef.tm;
-          m_limbCOPOffsetOut[i]->write();
-      }
+        // basePos
+        m_basePos.data.x = ref_basePos(0);
+        m_basePos.data.y = ref_basePos(1);
+        m_basePos.data.z = ref_basePos(2);
+        m_basePos.tm = m_qRef.tm;
+        // baseRpy
+        hrp::Vector3 baseRpy = hrp::rpyFromRot(ref_baseRot);
+        m_baseRpy.data.r = baseRpy(0);
+        m_baseRpy.data.p = baseRpy(1);
+        m_baseRpy.data.y = baseRpy(2);
+        m_baseRpy.tm = m_qRef.tm;
+        // baseTform
+        double *tform_arr = m_baseTform.data.get_buffer();
+        tform_arr[0] = m_basePos.data.x;
+        tform_arr[1] = m_basePos.data.y;
+        tform_arr[2] = m_basePos.data.z;
+        hrp::setMatrix33ToRowMajorArray(ref_baseRot, tform_arr, 3);
+        m_baseTform.tm = m_qRef.tm;
+        // basePose
+        m_basePose.data.position.x = m_basePos.data.x;
+        m_basePose.data.position.y = m_basePos.data.y;
+        m_basePose.data.position.z = m_basePos.data.z;
+        m_basePose.data.orientation.r = m_baseRpy.data.r;
+        m_basePose.data.orientation.p = m_baseRpy.data.p;
+        m_basePose.data.orientation.y = m_baseRpy.data.y;
+        m_basePose.tm = m_qRef.tm;
+        // zmp
+        m_zmp.data.x = rel_ref_zmp(0);
+        m_zmp.data.y = rel_ref_zmp(1);
+        m_zmp.data.z = rel_ref_zmp(2);
+        m_zmp.tm = m_qRef.tm;
+        // cog
+        m_cog.data.x = ref_cog(0);
+        m_cog.data.y = ref_cog(1);
+        m_cog.data.z = ref_cog(2);
+        m_cog.tm = m_qRef.tm;
+        // sbpCogOffset
+        m_sbpCogOffset.data.x = sbp_cog_offset(0);
+        m_sbpCogOffset.data.y = sbp_cog_offset(1);
+        m_sbpCogOffset.data.z = sbp_cog_offset(2);
+        m_sbpCogOffset.tm = m_qRef.tm;
+        // write
+        m_basePosOut.write();
+        m_baseRpyOut.write();
+        m_baseTformOut.write();
+        m_basePoseOut.write();
+        m_zmpOut.write();
+        m_cogOut.write();
+        m_sbpCogOffsetOut.write();
+
+        // reference acceleration
+        hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
+        if (sen != NULL) {
+            hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos;
+            hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt;
+            // convert to imu sensor local acceleration
+            hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt;
+            m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2);
+            m_accRefOut.write();
+            prev_imu_sensor_pos = imu_sensor_pos;
+            prev_imu_sensor_vel = imu_sensor_vel;
+        }
+
+        // control parameters
+        m_contactStates.tm = m_qRef.tm;
+        m_contactStatesOut.write();
+        m_controlSwingSupportTime.tm = m_qRef.tm;
+        m_controlSwingSupportTimeOut.write();
+        m_toeheelRatio.tm = m_qRef.tm;
+        m_toeheelRatioOut.write();
+        m_walkingStates.data = gg_is_walking;
+        m_walkingStates.tm = m_qRef.tm;
+        m_walkingStatesOut.write();
+
+        for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
+            m_force[i].tm = m_qRef.tm;
+            m_ref_forceOut[i]->write();
+        }
+
+        for (unsigned int i=0; i<m_limbCOPOffsetOut.size(); i++){
+            m_limbCOPOffset[i].tm = m_qRef.tm;
+            m_limbCOPOffsetOut[i]->write();
+        }
     }
 
     return RTC::RTC_OK;
@@ -665,99 +665,99 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
 
 void AutoBalancer::getTargetParameters()
 {
-  // joint angles
-  for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
-    m_robot->joint(i)->q = m_qRef.data[i];
-  }
-  fik->setReferenceJointAngles();
-  // basepos, rot, zmp
-  m_robot->rootLink()->p = input_basePos;
-  m_robot->rootLink()->R = input_baseRot;
-  m_robot->calcForwardKinematics();
-  gg->proc_zmp_weight_map_interpolation();
-  if (control_mode != MODE_IDLE) {
-    interpolateLegNamesAndZMPOffsets();
-    // Calculate tmp_fix_coords and something
-    coordinates tmp_fix_coords;
-    if ( gg_is_walking ) {
-      gg->set_default_zmp_offsets(default_zmp_offsets);
-      gg_solved = gg->proc_one_tick();
-      gg->get_swing_support_mid_coords(tmp_fix_coords);
-    } else {
-      tmp_fix_coords = fix_leg_coords;
-    }
-    if (!adjust_footstep_interpolator->isEmpty()) {
-        calcFixCoordsForAdjustFootstep(tmp_fix_coords);
-        fix_leg_coords = tmp_fix_coords;
-    }
-    // TODO : see explanation in this function
-    fixLegToCoords2(tmp_fix_coords);
-    fix_leg_coords2 = tmp_fix_coords;
+    // joint angles
+    for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
+        m_robot->joint(i)->q = m_qRef.data[i];
+    }
+    fik->setReferenceJointAngles();
+    // basepos, rot, zmp
+    m_robot->rootLink()->p = input_basePos;
+    m_robot->rootLink()->R = input_baseRot;
+    m_robot->calcForwardKinematics();
+    gg->proc_zmp_weight_map_interpolation();
+    if (control_mode != MODE_IDLE) {
+        interpolateLegNamesAndZMPOffsets();
+        // Calculate tmp_fix_coords and something
+        coordinates tmp_fix_coords;
+        if ( gg_is_walking ) {
+            gg->set_default_zmp_offsets(default_zmp_offsets);
+            gg_solved = gg->proc_one_tick();
+            gg->get_swing_support_mid_coords(tmp_fix_coords);
+        } else {
+            tmp_fix_coords = fix_leg_coords;
+        }
+        if (!adjust_footstep_interpolator->isEmpty()) {
+            calcFixCoordsForAdjustFootstep(tmp_fix_coords);
+            fix_leg_coords = tmp_fix_coords;
+        }
+        // TODO : see explanation in this function
+        fixLegToCoords2(tmp_fix_coords);
+        fix_leg_coords2 = tmp_fix_coords;
+
+        // Get output parameters and target EE coords
+        target_root_p = m_robot->rootLink()->p;
+        target_root_R = m_robot->rootLink()->R;
+        if ( gg_is_walking ) {
+            getOutputParametersForWalking();
+        } else {
+            getOutputParametersForABC();
+        }
+        //   Just for ik initial value
+        if (control_mode == MODE_SYNC_TO_ABC) {
+            fik->current_root_p = target_root_p;
+            fik->current_root_R = target_root_R;
+            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+                if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
+                    it->second.target_p0 = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
+                    it->second.target_r0 = it->second.target_link->R * it->second.localR;
+                }
+            }
+        }
 
-    // Get output parameters and target EE coords
-    target_root_p = m_robot->rootLink()->p;
-    target_root_R = m_robot->rootLink()->R;
-    if ( gg_is_walking ) {
-      getOutputParametersForWalking();
-    } else {
-      getOutputParametersForABC();
-    }
-    //   Just for ik initial value
-    if (control_mode == MODE_SYNC_TO_ABC) {
-        fik->current_root_p = target_root_p;
-        fik->current_root_R = target_root_R;
+        // Calculate other parameters
+        updateTargetCoordsForHandFixMode (tmp_fix_coords);
+        rotateRefForcesForFixCoords (tmp_fix_coords);
+        // TODO : see explanation in this function
+        calculateOutputRefForces ();
+        // TODO : see explanation in this function
+        updateWalkingVelocityFromHandError(tmp_fix_coords);
+        calcReferenceJointAnglesForIK();
+
+        // Calculate ZMP, COG, and sbp targets
+        hrp::Vector3 tmp_ref_cog(m_robot->calcCM());
+        hrp::Vector3 tmp_foot_mid_pos = calcFootMidPosUsingZMPWeightMap ();
+        if (gg_is_walking) {
+            ref_cog = gg->get_cog();
+        } else {
+            ref_cog = tmp_foot_mid_pos;
+        }
+        ref_cog(2) = tmp_ref_cog(2);
+        if (gg_is_walking) {
+            ref_zmp = gg->get_refzmp();
+        } else {
+            ref_zmp(0) = ref_cog(0);
+            ref_zmp(1) = ref_cog(1);
+            ref_zmp(2) = tmp_foot_mid_pos(2);
+        }
+    } else { // MODE_IDLE
+        // Update fix_leg_coords based on input basePos and rot if MODE_IDLE
+        std::vector<coordinates> tmp_end_coords_list;
         for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
             if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
-                it->second.target_p0 = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
-                it->second.target_r0 = it->second.target_link->R * it->second.localR;
+                tmp_end_coords_list.push_back(coordinates(it->second.target_link->p+it->second.target_link->R*it->second.localPos, it->second.target_link->R*it->second.localR));
+            }
+        }
+        multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
+        getOutputParametersForIDLE();
+        // Set force
+        for (unsigned int i=0; i< m_force.size(); i++) {
+            for (unsigned int j=0; j<6; j++) {
+                m_force[i].data[j] = m_ref_force[i].data[j];
             }
         }
     }
-
-    // Calculate other parameters
-    updateTargetCoordsForHandFixMode (tmp_fix_coords);
-    rotateRefForcesForFixCoords (tmp_fix_coords);
-    // TODO : see explanation in this function
-    calculateOutputRefForces ();
-    // TODO : see explanation in this function
-    updateWalkingVelocityFromHandError(tmp_fix_coords);
-    calcReferenceJointAnglesForIK();
-
-    // Calculate ZMP, COG, and sbp targets
-    hrp::Vector3 tmp_ref_cog(m_robot->calcCM());
-    hrp::Vector3 tmp_foot_mid_pos = calcFootMidPosUsingZMPWeightMap ();
-    if (gg_is_walking) {
-      ref_cog = gg->get_cog();
-    } else {
-      ref_cog = tmp_foot_mid_pos;
-    }
-    ref_cog(2) = tmp_ref_cog(2);
-    if (gg_is_walking) {
-      ref_zmp = gg->get_refzmp();
-    } else {
-      ref_zmp(0) = ref_cog(0);
-      ref_zmp(1) = ref_cog(1);
-      ref_zmp(2) = tmp_foot_mid_pos(2);
-    }
-  } else { // MODE_IDLE
-      // Update fix_leg_coords based on input basePos and rot if MODE_IDLE
-      std::vector<coordinates> tmp_end_coords_list;
-      for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-          if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
-              tmp_end_coords_list.push_back(coordinates(it->second.target_link->p+it->second.target_link->R*it->second.localPos, it->second.target_link->R*it->second.localR));
-          }
-      }
-      multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
-      getOutputParametersForIDLE();
-      // Set force
-      for (unsigned int i=0; i< m_force.size(); i++) {
-          for (unsigned int j=0; j<6; j++) {
-              m_force[i].data[j] = m_ref_force[i].data[j];
-          }
-      }
-  }
-  // Just for stop walking
-  if (gg_is_walking && !gg_solved) stopWalking ();
+    // Just for stop walking
+    if (gg_is_walking && !gg_solved) stopWalking ();
 };
 
 void AutoBalancer::getOutputParametersForWalking ()
@@ -860,18 +860,18 @@ void AutoBalancer::getOutputParametersForIDLE ()
 void AutoBalancer::interpolateLegNamesAndZMPOffsets()
 {
     if (!zmp_offset_interpolator->isEmpty()) {
-      double *default_zmp_offsets_output = new double[ikp.size()*3];
-      zmp_offset_interpolator->get(default_zmp_offsets_output, true);
-      for (size_t i = 0; i < ikp.size(); i++)
-        for (size_t j = 0; j < 3; j++)
-          default_zmp_offsets[i](j) = default_zmp_offsets_output[i*3+j];
-      delete[] default_zmp_offsets_output;
-      if (DEBUGP) {
-        std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl;
-        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            std::cerr << "[" << m_profile.instance_name << "]   " << it->first << " = " << default_zmp_offsets[contact_states_index_map[it->first]].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+        double *default_zmp_offsets_output = new double[ikp.size()*3];
+        zmp_offset_interpolator->get(default_zmp_offsets_output, true);
+        for (size_t i = 0; i < ikp.size(); i++)
+            for (size_t j = 0; j < 3; j++)
+                default_zmp_offsets[i](j) = default_zmp_offsets_output[i*3+j];
+        delete[] default_zmp_offsets_output;
+        if (DEBUGP) {
+            std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl;
+            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+                std::cerr << "[" << m_profile.instance_name << "]   " << it->first << " = " << default_zmp_offsets[contact_states_index_map[it->first]].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+            }
         }
-      }
     }
     if (!leg_names_interpolator->isEmpty()) {
         leg_names_interpolator->get(&leg_names_interpolator_ratio, true);
@@ -902,18 +902,18 @@ void AutoBalancer::rotateRefForcesForFixCoords (coordinates& tmp_fix_coords)
 {
     /* update ref_forces ;; StateHolder's absolute -> AutoBalancer's absolute */
     for (size_t i = 0; i < m_ref_forceIn.size(); i++) {
-      // hrp::Matrix33 eeR;
-      // hrp::Link* parentlink;
-      // hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
-      // if (sensor) parentlink = sensor->link;
-      // else parentlink = m_vfs[sensor_names[i]].link;
-      // for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-      //     if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR;
-      // }
-      // End effector frame
-      //ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
-      // world frame
-      ref_forces[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
+        // hrp::Matrix33 eeR;
+        // hrp::Link* parentlink;
+        // hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
+        // if (sensor) parentlink = sensor->link;
+        // else parentlink = m_vfs[sensor_names[i]].link;
+        // for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+        //     if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR;
+        // }
+        // End effector frame
+        //ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
+        // world frame
+        ref_forces[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
     }
     sbp_offset = tmp_fix_coords.rot * hrp::Vector3(sbp_offset);
 };
@@ -929,12 +929,12 @@ void AutoBalancer::updateTargetCoordsForHandFixMode (coordinates& tmp_fix_coords
         bool is_hand_control_while_walking = false;
         for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
             if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
-                 && it->first.find("arm") != std::string::npos ) {
+                    && it->first.find("arm") != std::string::npos ) {
                 is_hand_control_while_walking = true;
             }
         }
         if (is_hand_control_while_walking) {
-        //if (false) { // Disabled temporarily
+            //if (false) { // Disabled temporarily
             // Store hand_fix_initial_offset in the initialization of walking
             if (is_hand_fix_initial) hand_fix_initial_offset = tmp_fix_coords.rot.transpose() * (hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos);
             is_hand_fix_initial = false;
@@ -946,7 +946,7 @@ void AutoBalancer::updateTargetCoordsForHandFixMode (coordinates& tmp_fix_coords
             }
             for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
                 if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
-                     && it->first.find("arm") != std::string::npos ) {
+                        && it->first.find("arm") != std::string::npos ) {
                     it->second.target_p0 = it->second.target_p0 + dif_p;
                 }
             }
@@ -1026,33 +1026,33 @@ void AutoBalancer::calcReferenceJointAnglesForIK ()
 
 hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2)
 {
-  hrp::Vector3 vv = axis1.cross(axis2);
-  if (fabs(vv.norm()-0.0) < 1e-5) {
-    return rot;
-  } else {
-    Eigen::AngleAxis<double> tmpr(std::asin(vv.norm()), vv.normalized());
-    return tmpr.toRotationMatrix() * rot;
-  }
+    hrp::Vector3 vv = axis1.cross(axis2);
+    if (fabs(vv.norm()-0.0) < 1e-5) {
+        return rot;
+    } else {
+        Eigen::AngleAxis<double> tmpr(std::asin(vv.norm()), vv.normalized());
+        return tmpr.toRotationMatrix() * rot;
+    }
 }
 
 void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot)
 {
-  // get current foot mid pos + rot
-  std::vector<coordinates> foot_coords;
-  for (size_t i = 0; i < leg_names.size(); i++) {
-      ABCIKparam& tmpikp = ikp[leg_names[i]];
-      if (leg_names[i].find("leg") != std::string::npos) foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
-                                                                                           (tmpikp.target_link->R * tmpikp.localR)));
-  }
-  coordinates current_foot_mid_coords;
-  multi_mid_coords(current_foot_mid_coords, foot_coords);
-  hrp::Vector3 current_foot_mid_pos = current_foot_mid_coords.pos;
-  hrp::Matrix33 current_foot_mid_rot = current_foot_mid_coords.rot;
-  // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
-  hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose());
-  m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
-  rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
-  m_robot->calcForwardKinematics();
+    // get current foot mid pos + rot
+    std::vector<coordinates> foot_coords;
+    for (size_t i = 0; i < leg_names.size(); i++) {
+        ABCIKparam& tmpikp = ikp[leg_names[i]];
+        if (leg_names[i].find("leg") != std::string::npos) foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
+                (tmpikp.target_link->R * tmpikp.localR)));
+    }
+    coordinates current_foot_mid_coords;
+    multi_mid_coords(current_foot_mid_coords, foot_coords);
+    hrp::Vector3 current_foot_mid_pos = current_foot_mid_coords.pos;
+    hrp::Matrix33 current_foot_mid_rot = current_foot_mid_coords.rot;
+    // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
+    hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose());
+    m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
+    rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
+    m_robot->calcForwardKinematics();
 }
 
 void AutoBalancer::fixLegToCoords2 (coordinates& tmp_fix_coords)
@@ -1061,82 +1061,92 @@ void AutoBalancer::fixLegToCoords2 (coordinates& tmp_fix_coords)
     // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272
     // Snap input tmp_fix_coords to XY plan projection.
     {
-      hrp::Vector3 ex = hrp::Vector3::UnitX();
-      hrp::Vector3 ez = hrp::Vector3::UnitZ();
-      hrp::Vector3 xv1 (tmp_fix_coords.rot * ex);
-      xv1(2) = 0.0;
-      xv1.normalize();
-      hrp::Vector3 yv1(ez.cross(xv1));
-      tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2);
-      tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2);
-      tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2);
+        hrp::Vector3 ex = hrp::Vector3::UnitX();
+        hrp::Vector3 ez = hrp::Vector3::UnitZ();
+        hrp::Vector3 xv1 (tmp_fix_coords.rot * ex);
+        xv1(2) = 0.0;
+        xv1.normalize();
+        hrp::Vector3 yv1(ez.cross(xv1));
+        tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2);
+        tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2);
+        tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2);
     }
     fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot);
 }
 
 void AutoBalancer::solveFullbodyIK ()
 {
-  std::vector<IKConstraintParam> ik_tgt_list;
-  IKConstraintParam tmp;
-
-  tmp.target_link_name = "WAIST";
-  tmp.localPos = hrp::Vector3::Zero();
-  tmp.localR = hrp::Matrix33::Identity();
-  tmp.targetPos = target_root_p;// will be ignored by selection_vec
-  tmp.targetRpy = hrp::rpyFromRot(target_root_R);// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
-  tmp.weight_vec << 0,0,0,1,1,1;
-  ik_tgt_list.push_back(tmp);
-
-  tmp.target_link_name = ikp["rleg"].target_link->name;
-  tmp.localPos = ikp["rleg"].localPos;
-  tmp.localR = ikp["rleg"].localR;
-  tmp.targetPos = ikp["rleg"].target_p0;
-  tmp.targetRpy = hrp::rpyFromRot(ikp["rleg"].target_r0);
-  tmp.weight_vec << 1,1,3,3,3,1;
-  ik_tgt_list.push_back(tmp);
-
-  tmp.target_link_name = ikp["lleg"].target_link->name;
-  tmp.localPos = ikp["lleg"].localPos;
-  tmp.localR = ikp["lleg"].localR;
-  tmp.targetPos = ikp["lleg"].target_p0;
-  tmp.targetRpy = hrp::rpyFromRot(ikp["lleg"].target_r0);
-  tmp.weight_vec << 1,1,3,3,3,1;
-  ik_tgt_list.push_back(tmp);
-
-//  tmp.target_link_name = ikp["rarm"].target_link->name;
-//  tmp.localPos = ikp["rarm"].localPos;
-//  tmp.localR = ikp["rarm"].localR;
-//  tmp.targetPos = ikp["rarm"].target_p0;
-//  tmp.targetRpy = hrp::rpyFromRot(ikp["rarm"].target_r0);
-//  tmp.weight_vec << 1,1,1,1,1,1;
-//  ik_tgt_list.push_back(tmp);
-//
-//  tmp.target_link_name = ikp["larm"].target_link->name;
-//  tmp.localPos = ikp["larm"].localPos;
-//  tmp.localR = ikp["larm"].localR;
-//  tmp.targetPos = ikp["larm"].target_p0;
-//  tmp.targetRpy = hrp::rpyFromRot(ikp["larm"].target_r0);
-//  tmp.weight_vec << 1,1,1,1,1,1;
-//  ik_tgt_list.push_back(tmp);
-
-  tmp.target_link_name = "COM";
-  tmp.localPos = hrp::Vector3::Zero();
-  tmp.localR = hrp::Matrix33::Identity();
-  tmp.targetPos = ref_cog;// COM height will not be constraint
-  tmp.targetRpy = hrp::Vector3(0, 0, 0);//reference angular momentum
-//  tmp.targetRpy = hrp::Vector3(0, 10, 0);//reference angular momentum
-  tmp.weight_vec << 3,3,1,0.01,0.01,0.01;
-  ik_tgt_list.push_back(tmp);
-
-  if( m_robot->link("RARM_JOINT2") != NULL) m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇の干渉回避のため
-  if( m_robot->link("LARM_JOINT2") != NULL) m_robot->link("LARM_JOINT2")->llimit = deg2rad(40);
-
-//  fik_in->reference_gain.fill(0.001);
-//  fik_in->reference_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
-
-  int loop_result = 0;
-  const int IK_MAX_LOOP = 1;
-  loop_result = fik->solveFullbodyIKLoop(ik_tgt_list, IK_MAX_LOOP);
+    std::vector<IKConstraint> ik_tgt_list;
+    {
+        IKConstraint tmp;
+        tmp.target_link_name = "WAIST";
+        tmp.localPos = hrp::Vector3::Zero();
+        tmp.localR = hrp::Matrix33::Identity();
+        tmp.targetPos = target_root_p;// will be ignored by selection_vec
+        tmp.targetRpy = hrp::rpyFromRot(target_root_R);// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
+        tmp.constraint_weight << 0,0,0,1,1,0;
+        tmp.rot_precision = deg2rad(3);
+        ik_tgt_list.push_back(tmp);
+    }{
+        IKConstraint tmp;
+        tmp.target_link_name = ikp["rleg"].target_link->name;
+        tmp.localPos = ikp["rleg"].localPos;
+        tmp.localR = ikp["rleg"].localR;
+        tmp.targetPos = ikp["rleg"].target_p0;
+        tmp.targetRpy = hrp::rpyFromRot(ikp["rleg"].target_r0);
+        tmp.constraint_weight << 1,1,3,3,3,1;
+        ik_tgt_list.push_back(tmp);
+    }{
+        IKConstraint tmp;
+        tmp.target_link_name = ikp["lleg"].target_link->name;
+        tmp.localPos = ikp["lleg"].localPos;
+        tmp.localR = ikp["lleg"].localR;
+        tmp.targetPos = ikp["lleg"].target_p0;
+        tmp.targetRpy = hrp::rpyFromRot(ikp["lleg"].target_r0);
+        tmp.constraint_weight << 1,1,3,3,3,1;
+        ik_tgt_list.push_back(tmp);
+    }{
+//        IKConstraint tmp;
+//        tmp.target_link_name = ikp["rarm"].target_link->name;
+//        tmp.localPos = ikp["rarm"].localPos;
+//        tmp.localR = ikp["rarm"].localR;
+//        tmp.targetPos = ikp["rarm"].target_p0;
+//        tmp.targetRpy = hrp::rpyFromRot(ikp["rarm"].target_r0);
+//        tmp.weight_vec << 1,1,1,1,1,1;
+//        ik_tgt_list.push_back(tmp);
+    }{
+//        IKConstraint tmp;
+//        tmp.target_link_name = ikp["larm"].target_link->name;
+//        tmp.localPos = ikp["larm"].localPos;
+//        tmp.localR = ikp["larm"].localR;
+//        tmp.targetPos = ikp["larm"].target_p0;
+//        tmp.targetRpy = hrp::rpyFromRot(ikp["larm"].target_r0);
+//        tmp.weight_vec << 1,1,1,1,1,1;
+//        ik_tgt_list.push_back(tmp);
+    }{
+        IKConstraint tmp;
+        tmp.target_link_name = "COM";
+        tmp.localPos = hrp::Vector3::Zero();
+        tmp.localR = hrp::Matrix33::Identity();
+        tmp.targetPos = ref_cog;// COM height will not be constraint
+        tmp.targetRpy = hrp::Vector3(0, 0, 0);//reference angular momentum
+        //  tmp.targetRpy = hrp::Vector3(0, 10, 0);//reference angular momentum
+        tmp.constraint_weight << 3,3,1,0.01,0.01,0.01;
+        tmp.rot_precision = 100;
+        ik_tgt_list.push_back(tmp);
+    }
+    if( m_robot->link("RARM_JOINT2") != NULL) m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇の干渉回避のため
+    if( m_robot->link("LARM_JOINT2") != NULL) m_robot->link("LARM_JOINT2")->llimit = deg2rad(40);
+    const int IK_MAX_LOOP = 2;
+//    const int IK_MAX_LOOP = 1000;
+
+    struct timespec startT, endT;
+    clock_gettime(CLOCK_REALTIME, &startT);
+
+    int loop_result = fik->solveFullbodyIKLoop(m_robot, ik_tgt_list, IK_MAX_LOOP);
+
+    clock_gettime(CLOCK_REALTIME, &endT);
+    if(loop%100==0){ std::cout << (endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-6) << " [ms] @ IK "<<loop_result<<" [loop]" << std::endl; }
 }
 
 
@@ -1171,250 +1181,250 @@ void AutoBalancer::solveFullbodyIK ()
   {
   return RTC::RTC_OK;
   }
-*/
+ */
 
 /*
   RTC::ReturnCode_t AutoBalancer::onError(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
-*/
+ */
 
 /*
   RTC::ReturnCode_t AutoBalancer::onReset(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
-*/
+ */
 
 /*
   RTC::ReturnCode_t AutoBalancer::onStateUpdate(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
-*/
+ */
 
 /*
   RTC::ReturnCode_t AutoBalancer::onRateChanged(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
-*/
+ */
 
 void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence& limbs)
 {
-  std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
-  Guard guard(m_mutex);
-  double tmp_ratio = 0.0;
-  transition_interpolator->clear();
-  transition_interpolator->set(&tmp_ratio);
-  tmp_ratio = 1.0;
-  transition_interpolator->setGoal(&tmp_ratio, transition_time, true);
-  prev_ref_zmp = ref_zmp;
-  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-    it->second.is_active = false;
-  }
+    std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
+    Guard guard(m_mutex);
+    double tmp_ratio = 0.0;
+    transition_interpolator->clear();
+    transition_interpolator->set(&tmp_ratio);
+    tmp_ratio = 1.0;
+    transition_interpolator->setGoal(&tmp_ratio, transition_time, true);
+    prev_ref_zmp = ref_zmp;
+    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+        it->second.is_active = false;
+    }
 
-  for (size_t i = 0; i < limbs.length(); i++) {
-    ABCIKparam& tmp = ikp[std::string(limbs[i])];
-    tmp.is_active = true;
-    std::cerr << "[" << m_profile.instance_name << "]   limb [" << std::string(limbs[i]) << "]" << std::endl;
-  }
+    for (size_t i = 0; i < limbs.length(); i++) {
+        ABCIKparam& tmp = ikp[std::string(limbs[i])];
+        tmp.is_active = true;
+        std::cerr << "[" << m_profile.instance_name << "]   limb [" << std::string(limbs[i]) << "]" << std::endl;
+    }
 
-  control_mode = MODE_SYNC_TO_ABC;
+    control_mode = MODE_SYNC_TO_ABC;
 }
 
 void AutoBalancer::stopABCparam()
 {
-  std::cerr << "[" << m_profile.instance_name << "] stop auto balancer mode" << std::endl;
-  //Guard guard(m_mutex);
-  double tmp_ratio = 1.0;
-  transition_interpolator->clear();
-  transition_interpolator->set(&tmp_ratio);
-  tmp_ratio = 0.0;
-  transition_interpolator->setGoal(&tmp_ratio, transition_time, true);
-  control_mode = MODE_SYNC_TO_IDLE;
+    std::cerr << "[" << m_profile.instance_name << "] stop auto balancer mode" << std::endl;
+    //Guard guard(m_mutex);
+    double tmp_ratio = 1.0;
+    transition_interpolator->clear();
+    transition_interpolator->set(&tmp_ratio);
+    tmp_ratio = 0.0;
+    transition_interpolator->setGoal(&tmp_ratio, transition_time, true);
+    control_mode = MODE_SYNC_TO_IDLE;
 }
 
 bool AutoBalancer::startWalking ()
 {
-  if ( control_mode != MODE_ABC ) {
-    std::cerr << "[" << m_profile.instance_name << "] Cannot start walking without MODE_ABC. Please startAutoBalancer." << std::endl;
-    return false;
-  }
-  {
-    Guard guard(m_mutex);
-    fik->resetIKFailParam();
-    std::vector<std::string> init_swing_leg_names(gg->get_footstep_front_leg_names());
-    std::vector<std::string> tmp_all_limbs(leg_names);
-    std::vector<std::string> init_support_leg_names;
-    std::sort(tmp_all_limbs.begin(), tmp_all_limbs.end());
-    std::sort(init_swing_leg_names.begin(), init_swing_leg_names.end());
-    std::set_difference(tmp_all_limbs.begin(), tmp_all_limbs.end(),
-                        init_swing_leg_names.begin(), init_swing_leg_names.end(),
-                        std::back_inserter(init_support_leg_names));
-    std::vector<step_node> init_support_leg_steps, init_swing_leg_dst_steps;
-    for (std::vector<std::string>::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++)
-        init_support_leg_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0));
-    for (std::vector<std::string>::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++)
-        init_swing_leg_dst_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0));
-    gg->set_default_zmp_offsets(default_zmp_offsets);
-    gg->initialize_gait_parameter(ref_cog, init_support_leg_steps, init_swing_leg_dst_steps);
-  }
-  is_hand_fix_initial = true;
-  while ( !gg->proc_one_tick() );
-  {
-    Guard guard(m_mutex);
-    gg_is_walking = gg_solved = true;
-  }
-  return true;
+    if ( control_mode != MODE_ABC ) {
+        std::cerr << "[" << m_profile.instance_name << "] Cannot start walking without MODE_ABC. Please startAutoBalancer." << std::endl;
+        return false;
+    }
+    {
+        Guard guard(m_mutex);
+        fik->resetIKFailParam();
+        std::vector<std::string> init_swing_leg_names(gg->get_footstep_front_leg_names());
+        std::vector<std::string> tmp_all_limbs(leg_names);
+        std::vector<std::string> init_support_leg_names;
+        std::sort(tmp_all_limbs.begin(), tmp_all_limbs.end());
+        std::sort(init_swing_leg_names.begin(), init_swing_leg_names.end());
+        std::set_difference(tmp_all_limbs.begin(), tmp_all_limbs.end(),
+                init_swing_leg_names.begin(), init_swing_leg_names.end(),
+                std::back_inserter(init_support_leg_names));
+        std::vector<step_node> init_support_leg_steps, init_swing_leg_dst_steps;
+        for (std::vector<std::string>::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++)
+            init_support_leg_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0));
+        for (std::vector<std::string>::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++)
+            init_swing_leg_dst_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0));
+        gg->set_default_zmp_offsets(default_zmp_offsets);
+        gg->initialize_gait_parameter(ref_cog, init_support_leg_steps, init_swing_leg_dst_steps);
+    }
+    is_hand_fix_initial = true;
+    while ( !gg->proc_one_tick() );
+    {
+        Guard guard(m_mutex);
+        gg_is_walking = gg_solved = true;
+    }
+    return true;
 }
 
 void AutoBalancer::stopWalking ()
 {
-  std::vector<coordinates> tmp_end_coords_list;
-  for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
-      if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(coordinates(ikp[*it].target_p0, ikp[*it].target_r0));
-  }
-  multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
-  fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot);
-  gg->clear_footstep_nodes_list();
-  gg_is_walking = false;
+    std::vector<coordinates> tmp_end_coords_list;
+    for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
+        if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(coordinates(ikp[*it].target_p0, ikp[*it].target_r0));
+    }
+    multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
+    fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot);
+    gg->clear_footstep_nodes_list();
+    gg_is_walking = false;
 }
 
 bool AutoBalancer::startAutoBalancer (const OpenHRP::AutoBalancerService::StrSequence& limbs)
 {
-  if (control_mode == MODE_IDLE) {
-    fik->resetIKFailParam();
-    startABCparam(limbs);
-    waitABCTransition();
-    return true;
-  } else {
-    return false;
-  }
+    if (control_mode == MODE_IDLE) {
+        fik->resetIKFailParam();
+        startABCparam(limbs);
+        waitABCTransition();
+        return true;
+    } else {
+        return false;
+    }
 }
 
 bool AutoBalancer::stopAutoBalancer ()
 {
-  if (control_mode == MODE_ABC) {
-    stopABCparam();
-    waitABCTransition();
-    return true;
-  } else {
-    return false;
-  }
+    if (control_mode == MODE_ABC) {
+        stopABCparam();
+        waitABCTransition();
+        return true;
+    } else {
+        return false;
+    }
 }
 
 void AutoBalancer::waitABCTransition()
 {
-  while (!transition_interpolator->isEmpty()) usleep(1000);
-  usleep(1000);
+    while (!transition_interpolator->isEmpty()) usleep(1000);
+    usleep(1000);
 }
 bool AutoBalancer::goPos(const double& x, const double& y, const double& th)
 {
     //  if ( !gg_is_walking && !is_stop_mode) {
-  if ( !is_stop_mode) {
-    gg->set_all_limbs(leg_names);
-    coordinates start_ref_coords;
-    std::vector<coordinates> initial_support_legs_coords;
-    std::vector<leg_type> initial_support_legs;
-    bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
-    if (is_valid_gait_type == false) return false;
-    bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th,
-                                                      initial_support_legs_coords, // Dummy if gg_is_walking
-                                                      start_ref_coords,            // Dummy if gg_is_walking
-                                                      initial_support_legs,        // Dummy if gg_is_walking
-                                                      (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps.
-    if (!ret) {
-        std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl;
-    }
-    if ( !gg_is_walking ) { // Initializing
-        ret = startWalking();
+    if ( !is_stop_mode) {
+        gg->set_all_limbs(leg_names);
+        coordinates start_ref_coords;
+        std::vector<coordinates> initial_support_legs_coords;
+        std::vector<leg_type> initial_support_legs;
+        bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
+        if (is_valid_gait_type == false) return false;
+        bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th,
+                initial_support_legs_coords, // Dummy if gg_is_walking
+                start_ref_coords,            // Dummy if gg_is_walking
+                initial_support_legs,        // Dummy if gg_is_walking
+                (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps.
+        if (!ret) {
+            std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl;
+        }
+        if ( !gg_is_walking ) { // Initializing
+            ret = startWalking();
+        }
+        return ret;
+    } else {
+        std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl;
+        return false;
     }
-    return ret;
-  } else {
-    std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl;
-    return false;
-  }
 }
 
 bool AutoBalancer::goVelocity(const double& vx, const double& vy, const double& vth)
 {
-  gg->set_all_limbs(leg_names);
-  bool ret = true;
-  if (gg_is_walking && gg_solved) {
-    gg->set_velocity_param(vx, vy, vth);
-  } else {
-    coordinates ref_coords;
-    ref_coords.pos = (ikp["rleg"].target_p0+ikp["lleg"].target_p0)*0.5;
-    mid_rot(ref_coords.rot, 0.5, ikp["rleg"].target_r0, ikp["lleg"].target_r0);
-    std::vector<leg_type> current_legs;
-    switch(gait_type) {
-    case BIPED:
-        current_legs = (vy > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
-        break;
-    case TROT:
-        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
-        break;
-    case PACE:
-        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
-        break;
-    case CRAWL:
-        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    case GALLOP:
-        /* at least one leg shoud be in contact */
-        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    default: break;
+    gg->set_all_limbs(leg_names);
+    bool ret = true;
+    if (gg_is_walking && gg_solved) {
+        gg->set_velocity_param(vx, vy, vth);
+    } else {
+        coordinates ref_coords;
+        ref_coords.pos = (ikp["rleg"].target_p0+ikp["lleg"].target_p0)*0.5;
+        mid_rot(ref_coords.rot, 0.5, ikp["rleg"].target_r0, ikp["lleg"].target_r0);
+        std::vector<leg_type> current_legs;
+        switch(gait_type) {
+            case BIPED:
+                current_legs = (vy > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
+                break;
+            case TROT:
+                current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
+                break;
+            case PACE:
+                current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
+                break;
+            case CRAWL:
+                std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
+                return false;
+            case GALLOP:
+                /* at least one leg shoud be in contact */
+                std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
+                return false;
+            default: break;
+        }
+        gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs);
+        ret = startWalking();
     }
-    gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs);
-    ret = startWalking();
-  }
-  return ret;
+    return ret;
 }
 
 bool AutoBalancer::goStop ()
 {
-  gg->finalize_velocity_mode();
-  waitFootSteps();
-  return true;
+    gg->finalize_velocity_mode();
+    waitFootSteps();
+    return true;
 }
 
 bool AutoBalancer::emergencyStop ()
 {
-  std::cerr << "[" << m_profile.instance_name << "] emergencyStop" << std::endl;
-  // is_stop_mode = true;
-  gg->emergency_stop();
-  waitFootSteps();
-  return true;
+    std::cerr << "[" << m_profile.instance_name << "] emergencyStop" << std::endl;
+    // is_stop_mode = true;
+    gg->emergency_stop();
+    waitFootSteps();
+    return true;
 }
 
 bool AutoBalancer::releaseEmergencyStop ()
 {
-  if (is_stop_mode) {
-      std::cerr << "[" << m_profile.instance_name << "] releaseEmergencyStop" << std::endl;
-      is_stop_mode = false;
-  }
-  return true;
+    if (is_stop_mode) {
+        std::cerr << "[" << m_profile.instance_name << "] releaseEmergencyStop" << std::endl;
+        is_stop_mode = false;
+    }
+    return true;
 }
 
 bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
 {
-  OpenHRP::AutoBalancerService::StepParamsSequence spss;
-  spss.length(fss.length());
-  // If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
-  // If gg_is_walking is true, do not set to 0.
-  for (size_t i = 0; i < spss.length(); i++) {
-      spss[i].sps.length(fss[i].fs.length());
-      for (size_t j = 0; j < spss[i].sps.length(); j++) {
-          spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
-          spss[i].sps[j].step_time = gg->get_default_step_time();
-          spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle());
-          spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle());
-      }
-  }
-  setFootStepsWithParam(fss, spss, overwrite_fs_idx);
+    OpenHRP::AutoBalancerService::StepParamsSequence spss;
+    spss.length(fss.length());
+    // If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
+    // If gg_is_walking is true, do not set to 0.
+    for (size_t i = 0; i < spss.length(); i++) {
+        spss[i].sps.length(fss[i].fs.length());
+        for (size_t j = 0; j < spss[i].sps.length(); j++) {
+            spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
+            spss[i].sps[j].step_time = gg->get_default_step_time();
+            spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle());
+            spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle());
+        }
+    }
+    setFootStepsWithParam(fss, spss, overwrite_fs_idx);
 }
 
 bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
@@ -1515,524 +1525,524 @@ bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::Foo
 
 void AutoBalancer::waitFootSteps()
 {
-  //while (gg_is_walking) usleep(10);
-  while (gg_is_walking || !transition_interpolator->isEmpty() )
+    //while (gg_is_walking) usleep(10);
+    while (gg_is_walking || !transition_interpolator->isEmpty() )
+        usleep(1000);
     usleep(1000);
-  usleep(1000);
-  gg->set_offset_velocity_param(0,0,0);
+    gg->set_offset_velocity_param(0,0,0);
 }
 
 void AutoBalancer::waitFootStepsEarly(const double tm)
 {
-  if (!gg_is_walking) { return;}
-  while ( !gg->is_finalizing(tm)|| !transition_interpolator->isEmpty() )
+    if (!gg_is_walking) { return;}
+    while ( !gg->is_finalizing(tm)|| !transition_interpolator->isEmpty() )
+        usleep(1000);
     usleep(1000);
-  usleep(1000);
-  gg->set_offset_velocity_param(0,0,0);
+    gg->set_offset_velocity_param(0,0,0);
 }
 
 bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
 {
-  std::cerr << "[" << m_profile.instance_name << "] setGaitGeneratorParam" << std::endl;
-  if (i_param.stride_parameter.length() == 4) { // Support old stride_parameter definitions
-      gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3],
-                                i_param.stride_parameter[1]*0.5, i_param.stride_parameter[2]*0.5);
-  } else {
-      gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3],
-                                i_param.stride_parameter[4], i_param.stride_parameter[5]);
-  }
-  std::vector<hrp::Vector3> off;
-  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
-      off.push_back(hrp::Vector3(i_param.leg_default_translate_pos[i][0], i_param.leg_default_translate_pos[i][1], i_param.leg_default_translate_pos[i][2]));
-  }
-  gg->set_leg_default_translate_pos(off);
-  gg->set_default_step_time(i_param.default_step_time);
-  gg->set_default_step_height(i_param.default_step_height);
-  gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio/2.0);
-  gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio/2.0);
-  gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio/2.0);
-  gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio/2.0);
-  gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio/2.0);
-  gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio/2.0);
-  // gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio_before);
-  // gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio_after);
-  // gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio_before);
-  // gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio_after);
-  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_before);
-  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_after);
-  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_swing_before);
-  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_swing_after);
-  if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::SHUFFLING) {
-    gg->set_default_orbit_type(SHUFFLING);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOID) {
-    gg->set_default_orbit_type(CYCLOID);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::RECTANGLE) {
-    gg->set_default_orbit_type(RECTANGLE);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::STAIR) {
-    gg->set_default_orbit_type(STAIR);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAY) {
-    gg->set_default_orbit_type(CYCLOIDDELAY);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK) {
-    gg->set_default_orbit_type(CYCLOIDDELAYKICK);
-  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CROSS) {
-    gg->set_default_orbit_type(CROSS);
-  }
-  gg->set_swing_trajectory_delay_time_offset(i_param.swing_trajectory_delay_time_offset);
-  gg->set_swing_trajectory_final_distance_weight(i_param.swing_trajectory_final_distance_weight);
-  gg->set_swing_trajectory_time_offset_xy2z(i_param.swing_trajectory_time_offset_xy2z);
-  gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2]));
-  gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2]));  
-  gg->set_gravitational_acceleration(i_param.gravitational_acceleration);
-  gg->set_toe_angle(i_param.toe_angle);
-  gg->set_heel_angle(i_param.heel_angle);
-  gg->set_toe_pos_offset_x(i_param.toe_pos_offset_x);
-  gg->set_heel_pos_offset_x(i_param.heel_pos_offset_x);
-  gg->set_toe_zmp_offset_x(i_param.toe_zmp_offset_x);
-  gg->set_heel_zmp_offset_x(i_param.heel_zmp_offset_x);
-  gg->set_toe_check_thre(i_param.toe_check_thre);
-  gg->set_heel_check_thre(i_param.heel_check_thre);
-  std::vector<double> tmp_ratio(i_param.toe_heel_phase_ratio.get_buffer(), i_param.toe_heel_phase_ratio.get_buffer()+i_param.toe_heel_phase_ratio.length());
-  std::cerr << "[" << m_profile.instance_name << "]   "; // for set_toe_heel_phase_ratio
-  gg->set_toe_heel_phase_ratio(tmp_ratio);
-  gg->set_use_toe_joint(i_param.use_toe_joint);
-  gg->set_use_toe_heel_transition(i_param.use_toe_heel_transition);
-  gg->set_use_toe_heel_auto_set(i_param.use_toe_heel_auto_set);
-  gg->set_zmp_weight_map(boost::assign::map_list_of<leg_type, double>(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3]));
-  gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num);
-  gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset);
-  gg->set_leg_margin(i_param.leg_margin);
-  gg->set_stride_limitation_for_circle_type(i_param.stride_limitation_for_circle_type);
-  gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation);
-  gg->set_use_stride_limitation(i_param.use_stride_limitation);
-  gg->set_footstep_modification_gain(i_param.footstep_modification_gain);
-  gg->set_modify_footsteps(i_param.modify_footsteps);
-  gg->set_cp_check_margin(i_param.cp_check_margin);
-  gg->set_margin_time_ratio(i_param.margin_time_ratio);
-  if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::SQUARE) {
-    gg->set_stride_limitation_type(SQUARE);
-  } else if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::CIRCLE) {
-    gg->set_stride_limitation_type(CIRCLE);
-  }
-
-  // print
-  gg->print_param(std::string(m_profile.instance_name));
-  return true;
+    std::cerr << "[" << m_profile.instance_name << "] setGaitGeneratorParam" << std::endl;
+    if (i_param.stride_parameter.length() == 4) { // Support old stride_parameter definitions
+        gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3],
+                i_param.stride_parameter[1]*0.5, i_param.stride_parameter[2]*0.5);
+    } else {
+        gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3],
+                i_param.stride_parameter[4], i_param.stride_parameter[5]);
+    }
+    std::vector<hrp::Vector3> off;
+    for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
+        off.push_back(hrp::Vector3(i_param.leg_default_translate_pos[i][0], i_param.leg_default_translate_pos[i][1], i_param.leg_default_translate_pos[i][2]));
+    }
+    gg->set_leg_default_translate_pos(off);
+    gg->set_default_step_time(i_param.default_step_time);
+    gg->set_default_step_height(i_param.default_step_height);
+    gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio/2.0);
+    gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio/2.0);
+    gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio/2.0);
+    gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio/2.0);
+    gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio/2.0);
+    gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio/2.0);
+    // gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio_before);
+    // gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio_after);
+    // gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio_before);
+    // gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio_after);
+    // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_before);
+    // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_after);
+    // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_swing_before);
+    // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_swing_after);
+    if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::SHUFFLING) {
+        gg->set_default_orbit_type(SHUFFLING);
+    } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOID) {
+        gg->set_default_orbit_type(CYCLOID);
+    } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::RECTANGLE) {
+        gg->set_default_orbit_type(RECTANGLE);
+    } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::STAIR) {
+        gg->set_default_orbit_type(STAIR);
+    } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAY) {
+        gg->set_default_orbit_type(CYCLOIDDELAY);
+    } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK) {
+        gg->set_default_orbit_type(CYCLOIDDELAYKICK);
+    } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CROSS) {
+        gg->set_default_orbit_type(CROSS);
+    }
+    gg->set_swing_trajectory_delay_time_offset(i_param.swing_trajectory_delay_time_offset);
+    gg->set_swing_trajectory_final_distance_weight(i_param.swing_trajectory_final_distance_weight);
+    gg->set_swing_trajectory_time_offset_xy2z(i_param.swing_trajectory_time_offset_xy2z);
+    gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2]));
+    gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2]));
+    gg->set_gravitational_acceleration(i_param.gravitational_acceleration);
+    gg->set_toe_angle(i_param.toe_angle);
+    gg->set_heel_angle(i_param.heel_angle);
+    gg->set_toe_pos_offset_x(i_param.toe_pos_offset_x);
+    gg->set_heel_pos_offset_x(i_param.heel_pos_offset_x);
+    gg->set_toe_zmp_offset_x(i_param.toe_zmp_offset_x);
+    gg->set_heel_zmp_offset_x(i_param.heel_zmp_offset_x);
+    gg->set_toe_check_thre(i_param.toe_check_thre);
+    gg->set_heel_check_thre(i_param.heel_check_thre);
+    std::vector<double> tmp_ratio(i_param.toe_heel_phase_ratio.get_buffer(), i_param.toe_heel_phase_ratio.get_buffer()+i_param.toe_heel_phase_ratio.length());
+    std::cerr << "[" << m_profile.instance_name << "]   "; // for set_toe_heel_phase_ratio
+    gg->set_toe_heel_phase_ratio(tmp_ratio);
+    gg->set_use_toe_joint(i_param.use_toe_joint);
+    gg->set_use_toe_heel_transition(i_param.use_toe_heel_transition);
+    gg->set_use_toe_heel_auto_set(i_param.use_toe_heel_auto_set);
+    gg->set_zmp_weight_map(boost::assign::map_list_of<leg_type, double>(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3]));
+    gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num);
+    gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset);
+    gg->set_leg_margin(i_param.leg_margin);
+    gg->set_stride_limitation_for_circle_type(i_param.stride_limitation_for_circle_type);
+    gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation);
+    gg->set_use_stride_limitation(i_param.use_stride_limitation);
+    gg->set_footstep_modification_gain(i_param.footstep_modification_gain);
+    gg->set_modify_footsteps(i_param.modify_footsteps);
+    gg->set_cp_check_margin(i_param.cp_check_margin);
+    gg->set_margin_time_ratio(i_param.margin_time_ratio);
+    if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::SQUARE) {
+        gg->set_stride_limitation_type(SQUARE);
+    } else if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::CIRCLE) {
+        gg->set_stride_limitation_type(CIRCLE);
+    }
+
+    // print
+    gg->print_param(std::string(m_profile.instance_name));
+    return true;
 };
 
 bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
 {
-  gg->get_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3], i_param.stride_parameter[4], i_param.stride_parameter[5]);
-  std::vector<hrp::Vector3> off;
-  gg->get_leg_default_translate_pos(off);
-  i_param.leg_default_translate_pos.length(off.size());
-  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
-      i_param.leg_default_translate_pos[i].length(3);
-      i_param.leg_default_translate_pos[i][0] = off[i](0);
-      i_param.leg_default_translate_pos[i][1] = off[i](1);
-      i_param.leg_default_translate_pos[i][2] = off[i](2);
-  }
-  i_param.default_step_time = gg->get_default_step_time();
-  i_param.default_step_height = gg->get_default_step_height();
-  i_param.default_double_support_ratio_before = gg->get_default_double_support_ratio_before();
-  i_param.default_double_support_ratio_after = gg->get_default_double_support_ratio_after();
-  i_param.default_double_support_static_ratio_before = gg->get_default_double_support_static_ratio_before();
-  i_param.default_double_support_static_ratio_after = gg->get_default_double_support_static_ratio_after();
-  i_param.default_double_support_ratio_swing_before = gg->get_default_double_support_ratio_swing_before();
-  i_param.default_double_support_ratio_swing_after = gg->get_default_double_support_ratio_swing_after();
-  i_param.default_double_support_ratio = i_param.default_double_support_ratio_before + i_param.default_double_support_ratio_after;
-  i_param.default_double_support_static_ratio = i_param.default_double_support_static_ratio_before + i_param.default_double_support_static_ratio_after;
-  if (gg->get_default_orbit_type() == SHUFFLING) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::SHUFFLING;
-  } else if (gg->get_default_orbit_type() == CYCLOID) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOID;
-  } else if (gg->get_default_orbit_type() == RECTANGLE) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::RECTANGLE;
-  } else if (gg->get_default_orbit_type() == STAIR) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::STAIR;
-  } else if (gg->get_default_orbit_type() == CYCLOIDDELAY) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAY;
-  } else if (gg->get_default_orbit_type() == CYCLOIDDELAYKICK) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK;
-  } else if (gg->get_default_orbit_type() == CROSS) {
-    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CROSS;
-  }
-
-  hrp::Vector3 tmpv = gg->get_stair_trajectory_way_point_offset();
-  for (size_t i = 0; i < 3; i++) i_param.stair_trajectory_way_point_offset[i] = tmpv(i);
-  tmpv = gg->get_cycloid_delay_kick_point_offset();
-  for (size_t i = 0; i < 3; i++) i_param.cycloid_delay_kick_point_offset[i] = tmpv(i);
-  i_param.swing_trajectory_delay_time_offset = gg->get_swing_trajectory_delay_time_offset();
-  i_param.swing_trajectory_final_distance_weight = gg->get_swing_trajectory_final_distance_weight();
-  i_param.swing_trajectory_time_offset_xy2z = gg->get_swing_trajectory_time_offset_xy2z();
-  i_param.gravitational_acceleration = gg->get_gravitational_acceleration();
-  i_param.toe_angle = gg->get_toe_angle();
-  i_param.heel_angle = gg->get_heel_angle();
-  i_param.toe_pos_offset_x = gg->get_toe_pos_offset_x();
-  i_param.heel_pos_offset_x = gg->get_heel_pos_offset_x();
-  i_param.toe_zmp_offset_x = gg->get_toe_zmp_offset_x();
-  i_param.heel_zmp_offset_x = gg->get_heel_zmp_offset_x();
-  i_param.toe_check_thre = gg->get_toe_check_thre();
-  i_param.heel_check_thre = gg->get_heel_check_thre();
-  std::vector<double> ratio(gg->get_NUM_TH_PHASES(),0.0);
-  gg->get_toe_heel_phase_ratio(ratio);
-  for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) i_param.toe_heel_phase_ratio[i] = ratio[i];
-  i_param.use_toe_joint = gg->get_use_toe_joint();
-  i_param.use_toe_heel_transition = gg->get_use_toe_heel_transition();
-  i_param.use_toe_heel_auto_set = gg->get_use_toe_heel_auto_set();
-  std::map<leg_type, double> tmp_zmp_weight_map = gg->get_zmp_weight_map();
-  i_param.zmp_weight_map[0] = tmp_zmp_weight_map[RLEG];
-  i_param.zmp_weight_map[1] = tmp_zmp_weight_map[LLEG];
-  i_param.zmp_weight_map[2] = tmp_zmp_weight_map[RARM];
-  i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM];
-  i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num();
-  i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset();
-  for (size_t i=0; i<4; i++) {
-    i_param.leg_margin[i] = gg->get_leg_margin(i);
-  }
-  for (size_t i=0; i<5; i++) {
-    i_param.stride_limitation_for_circle_type[i] = gg->get_stride_limitation_for_circle_type(i);
-  }
-  for (size_t i=0; i<5; i++) {
-    i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i);
-  }
-  i_param.use_stride_limitation = gg->get_use_stride_limitation();
-  i_param.footstep_modification_gain = gg->get_footstep_modification_gain();
-  i_param.modify_footsteps = gg->get_modify_footsteps();
-  for (size_t i=0; i<2; i++) {
-    i_param.cp_check_margin[i] = gg->get_cp_check_margin(i);
-  }
-  i_param.margin_time_ratio = gg->get_margin_time_ratio();
-  if (gg->get_stride_limitation_type() == SQUARE) {
-    i_param.stride_limitation_type = OpenHRP::AutoBalancerService::SQUARE;
-  } else if (gg->get_stride_limitation_type() == CIRCLE) {
-    i_param.stride_limitation_type = OpenHRP::AutoBalancerService::CIRCLE;
-  }
-  return true;
+    gg->get_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3], i_param.stride_parameter[4], i_param.stride_parameter[5]);
+    std::vector<hrp::Vector3> off;
+    gg->get_leg_default_translate_pos(off);
+    i_param.leg_default_translate_pos.length(off.size());
+    for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
+        i_param.leg_default_translate_pos[i].length(3);
+        i_param.leg_default_translate_pos[i][0] = off[i](0);
+        i_param.leg_default_translate_pos[i][1] = off[i](1);
+        i_param.leg_default_translate_pos[i][2] = off[i](2);
+    }
+    i_param.default_step_time = gg->get_default_step_time();
+    i_param.default_step_height = gg->get_default_step_height();
+    i_param.default_double_support_ratio_before = gg->get_default_double_support_ratio_before();
+    i_param.default_double_support_ratio_after = gg->get_default_double_support_ratio_after();
+    i_param.default_double_support_static_ratio_before = gg->get_default_double_support_static_ratio_before();
+    i_param.default_double_support_static_ratio_after = gg->get_default_double_support_static_ratio_after();
+    i_param.default_double_support_ratio_swing_before = gg->get_default_double_support_ratio_swing_before();
+    i_param.default_double_support_ratio_swing_after = gg->get_default_double_support_ratio_swing_after();
+    i_param.default_double_support_ratio = i_param.default_double_support_ratio_before + i_param.default_double_support_ratio_after;
+    i_param.default_double_support_static_ratio = i_param.default_double_support_static_ratio_before + i_param.default_double_support_static_ratio_after;
+    if (gg->get_default_orbit_type() == SHUFFLING) {
+        i_param.default_orbit_type = OpenHRP::AutoBalancerService::SHUFFLING;
+    } else if (gg->get_default_orbit_type() == CYCLOID) {
+        i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOID;
+    } else if (gg->get_default_orbit_type() == RECTANGLE) {
+        i_param.default_orbit_type = OpenHRP::AutoBalancerService::RECTANGLE;
+    } else if (gg->get_default_orbit_type() == STAIR) {
+        i_param.default_orbit_type = OpenHRP::AutoBalancerService::STAIR;
+    } else if (gg->get_default_orbit_type() == CYCLOIDDELAY) {
+        i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAY;
+    } else if (gg->get_default_orbit_type() == CYCLOIDDELAYKICK) {
+        i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK;
+    } else if (gg->get_default_orbit_type() == CROSS) {
+        i_param.default_orbit_type = OpenHRP::AutoBalancerService::CROSS;
+    }
+
+    hrp::Vector3 tmpv = gg->get_stair_trajectory_way_point_offset();
+    for (size_t i = 0; i < 3; i++) i_param.stair_trajectory_way_point_offset[i] = tmpv(i);
+    tmpv = gg->get_cycloid_delay_kick_point_offset();
+    for (size_t i = 0; i < 3; i++) i_param.cycloid_delay_kick_point_offset[i] = tmpv(i);
+    i_param.swing_trajectory_delay_time_offset = gg->get_swing_trajectory_delay_time_offset();
+    i_param.swing_trajectory_final_distance_weight = gg->get_swing_trajectory_final_distance_weight();
+    i_param.swing_trajectory_time_offset_xy2z = gg->get_swing_trajectory_time_offset_xy2z();
+    i_param.gravitational_acceleration = gg->get_gravitational_acceleration();
+    i_param.toe_angle = gg->get_toe_angle();
+    i_param.heel_angle = gg->get_heel_angle();
+    i_param.toe_pos_offset_x = gg->get_toe_pos_offset_x();
+    i_param.heel_pos_offset_x = gg->get_heel_pos_offset_x();
+    i_param.toe_zmp_offset_x = gg->get_toe_zmp_offset_x();
+    i_param.heel_zmp_offset_x = gg->get_heel_zmp_offset_x();
+    i_param.toe_check_thre = gg->get_toe_check_thre();
+    i_param.heel_check_thre = gg->get_heel_check_thre();
+    std::vector<double> ratio(gg->get_NUM_TH_PHASES(),0.0);
+    gg->get_toe_heel_phase_ratio(ratio);
+    for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) i_param.toe_heel_phase_ratio[i] = ratio[i];
+    i_param.use_toe_joint = gg->get_use_toe_joint();
+    i_param.use_toe_heel_transition = gg->get_use_toe_heel_transition();
+    i_param.use_toe_heel_auto_set = gg->get_use_toe_heel_auto_set();
+    std::map<leg_type, double> tmp_zmp_weight_map = gg->get_zmp_weight_map();
+    i_param.zmp_weight_map[0] = tmp_zmp_weight_map[RLEG];
+    i_param.zmp_weight_map[1] = tmp_zmp_weight_map[LLEG];
+    i_param.zmp_weight_map[2] = tmp_zmp_weight_map[RARM];
+    i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM];
+    i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num();
+    i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset();
+    for (size_t i=0; i<4; i++) {
+        i_param.leg_margin[i] = gg->get_leg_margin(i);
+    }
+    for (size_t i=0; i<5; i++) {
+        i_param.stride_limitation_for_circle_type[i] = gg->get_stride_limitation_for_circle_type(i);
+    }
+    for (size_t i=0; i<5; i++) {
+        i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i);
+    }
+    i_param.use_stride_limitation = gg->get_use_stride_limitation();
+    i_param.footstep_modification_gain = gg->get_footstep_modification_gain();
+    i_param.modify_footsteps = gg->get_modify_footsteps();
+    for (size_t i=0; i<2; i++) {
+        i_param.cp_check_margin[i] = gg->get_cp_check_margin(i);
+    }
+    i_param.margin_time_ratio = gg->get_margin_time_ratio();
+    if (gg->get_stride_limitation_type() == SQUARE) {
+        i_param.stride_limitation_type = OpenHRP::AutoBalancerService::SQUARE;
+    } else if (gg->get_stride_limitation_type() == CIRCLE) {
+        i_param.stride_limitation_type = OpenHRP::AutoBalancerService::CIRCLE;
+    }
+    return true;
 };
 
 bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
 {
-  Guard guard(m_mutex);
-  std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl;
-  double *default_zmp_offsets_array = new double[ikp.size()*3];
-  for (size_t i = 0; i < ikp.size(); i++)
+    Guard guard(m_mutex);
+    std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl;
+    double *default_zmp_offsets_array = new double[ikp.size()*3];
+    for (size_t i = 0; i < ikp.size(); i++)
+        for (size_t j = 0; j < 3; j++)
+            default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j];
+    zmp_transition_time = i_param.zmp_transition_time;
+    adjust_footstep_transition_time = i_param.adjust_footstep_transition_time;
+    if (zmp_offset_interpolator->isEmpty()) {
+        zmp_offset_interpolator->clear();
+        zmp_offset_interpolator->setGoal(default_zmp_offsets_array, zmp_transition_time, true);
+    } else {
+        std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets cannot be set because interpolating." << std::endl;
+    }
+    if (control_mode == MODE_IDLE) {
+        switch (i_param.use_force_mode) {
+            case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
+                use_force = MODE_NO_FORCE;
+                break;
+            case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
+                use_force = MODE_REF_FORCE;
+                break;
+            case OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT:
+                use_force = MODE_REF_FORCE_WITH_FOOT;
+                break;
+            case OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT:
+                use_force = MODE_REF_FORCE_RFU_EXT_MOMENT;
+                break;
+            default:
+                break;
+        }
+    } else {
+        std::cerr << "[" << m_profile.instance_name << "]   use_force_mode cannot be changed to [" << i_param.use_force_mode << "] during MODE_ABC, MODE_SYNC_TO_IDLE or MODE_SYNC_TO_ABC." << std::endl;
+    }
+    graspless_manip_mode = i_param.graspless_manip_mode;
+    graspless_manip_arm = std::string(i_param.graspless_manip_arm);
     for (size_t j = 0; j < 3; j++)
-      default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j];
-  zmp_transition_time = i_param.zmp_transition_time;
-  adjust_footstep_transition_time = i_param.adjust_footstep_transition_time;
-  if (zmp_offset_interpolator->isEmpty()) {
-      zmp_offset_interpolator->clear();
-      zmp_offset_interpolator->setGoal(default_zmp_offsets_array, zmp_transition_time, true);
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets cannot be set because interpolating." << std::endl;
-  }
-  if (control_mode == MODE_IDLE) {
-    switch (i_param.use_force_mode) {
-    case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
-        use_force = MODE_NO_FORCE;
-        break;
-    case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
-        use_force = MODE_REF_FORCE;
-        break;
-    case OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT:
-        use_force = MODE_REF_FORCE_WITH_FOOT;
-        break;
-    case OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT:
-        use_force = MODE_REF_FORCE_RFU_EXT_MOMENT;
-        break;
-    default:
-        break;
-    }
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   use_force_mode cannot be changed to [" << i_param.use_force_mode << "] during MODE_ABC, MODE_SYNC_TO_IDLE or MODE_SYNC_TO_ABC." << std::endl;
-  }
-  graspless_manip_mode = i_param.graspless_manip_mode;
-  graspless_manip_arm = std::string(i_param.graspless_manip_arm);
-  for (size_t j = 0; j < 3; j++)
-      graspless_manip_p_gain[j] = i_param.graspless_manip_p_gain[j];
-  for (size_t j = 0; j < 3; j++)
-      graspless_manip_reference_trans_coords.pos[j] = i_param.graspless_manip_reference_trans_pos[j];
-  graspless_manip_reference_trans_coords.rot = (Eigen::Quaternion<double>(i_param.graspless_manip_reference_trans_rot[0],
-                                                                          i_param.graspless_manip_reference_trans_rot[1],
-                                                                          i_param.graspless_manip_reference_trans_rot[2],
-                                                                          i_param.graspless_manip_reference_trans_rot[3]).normalized().toRotationMatrix()); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
-  transition_time = i_param.transition_time;
-  std::vector<std::string> cur_leg_names, dst_leg_names;
-  cur_leg_names = leg_names;
-  for (size_t i = 0; i < i_param.leg_names.length(); i++) {
-      dst_leg_names.push_back(std::string(i_param.leg_names[i]));
-  }
-  std::sort(cur_leg_names.begin(), cur_leg_names.end());
-  std::sort(dst_leg_names.begin(), dst_leg_names.end());
-  if (cur_leg_names != dst_leg_names) {
-      if (leg_names_interpolator->isEmpty()) {
-          leg_names.clear();
-          leg_names = dst_leg_names;
-          if (control_mode == MODE_ABC) {
-              double tmp_ratio = 0.0;
-              leg_names_interpolator->set(&tmp_ratio);
-              tmp_ratio = 1.0;
-              leg_names_interpolator->setGoal(&tmp_ratio, 5.0, true);
-              control_mode = MODE_SYNC_TO_ABC;
-          }
-      }
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   leg_names cannot be set because interpolating." << std::endl;
-  }
-  if (!gg_is_walking) {
-      is_hand_fix_mode = i_param.is_hand_fix_mode;
-      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode = " << is_hand_fix_mode << std::endl;
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode cannot be set in (gg_is_walking = true). Current is_hand_fix_mode is " << (is_hand_fix_mode?"true":"false") << std::endl;
-  }
-  if (control_mode == MODE_IDLE) {
-      for (size_t i = 0; i < i_param.end_effector_list.length(); i++) {
-          std::map<std::string, ABCIKparam>::iterator it = ikp.find(std::string(i_param.end_effector_list[i].leg));
-          memcpy(it->second.localPos.data(), i_param.end_effector_list[i].pos, sizeof(double)*3);
-          it->second.localR = (Eigen::Quaternion<double>(i_param.end_effector_list[i].rot[0], i_param.end_effector_list[i].rot[1], i_param.end_effector_list[i].rot[2], i_param.end_effector_list[i].rot[3])).normalized().toRotationMatrix();
-      }
-  } else {
-      std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl;
-  }
-  if (i_param.default_gait_type == OpenHRP::AutoBalancerService::BIPED) {
-      gait_type = BIPED;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::TROT) {
-      gait_type = TROT;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::PACE) {
-      gait_type = PACE;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::CRAWL) {
-      gait_type = CRAWL;
-  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) {
-      gait_type = GALLOP;
-  }
-  // Ref force balancing
-  std::cerr << "[" << m_profile.instance_name << "] Ref force balancing" << std::endl;
-  if ( use_force == MODE_REF_FORCE_WITH_FOOT && control_mode != MODE_IDLE ) {
-      std::cerr << "[" << m_profile.instance_name << "]   additional_force_applied_point_offset and additional_force_applied_link_name cannot be updated during MODE_REF_FORCE_WITH_FOOT and non-MODE_IDLE"<< std::endl;
-  } else if ( !m_robot->link(std::string(i_param.additional_force_applied_link_name)) ) {
-      std::cerr << "[" << m_profile.instance_name << "]   Invalid link name for additional_force_applied_link_name = " << i_param.additional_force_applied_link_name << std::endl;
-  } else {
-      additional_force_applied_link = m_robot->link(std::string(i_param.additional_force_applied_link_name));
-      for (size_t i = 0; i < 3; i++) {
-          additional_force_applied_point_offset(i) = i_param.additional_force_applied_point_offset[i];
-      }
-      std::cerr << "[" << m_profile.instance_name << "]   Link name for additional_force_applied_link_name = " << additional_force_applied_link->name << ", additional_force_applied_point_offset = " << additional_force_applied_point_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-  }
+        graspless_manip_p_gain[j] = i_param.graspless_manip_p_gain[j];
+    for (size_t j = 0; j < 3; j++)
+        graspless_manip_reference_trans_coords.pos[j] = i_param.graspless_manip_reference_trans_pos[j];
+    graspless_manip_reference_trans_coords.rot = (Eigen::Quaternion<double>(i_param.graspless_manip_reference_trans_rot[0],
+            i_param.graspless_manip_reference_trans_rot[1],
+            i_param.graspless_manip_reference_trans_rot[2],
+            i_param.graspless_manip_reference_trans_rot[3]).normalized().toRotationMatrix()); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+            transition_time = i_param.transition_time;
+            std::vector<std::string> cur_leg_names, dst_leg_names;
+            cur_leg_names = leg_names;
+            for (size_t i = 0; i < i_param.leg_names.length(); i++) {
+                dst_leg_names.push_back(std::string(i_param.leg_names[i]));
+            }
+            std::sort(cur_leg_names.begin(), cur_leg_names.end());
+            std::sort(dst_leg_names.begin(), dst_leg_names.end());
+            if (cur_leg_names != dst_leg_names) {
+                if (leg_names_interpolator->isEmpty()) {
+                    leg_names.clear();
+                    leg_names = dst_leg_names;
+                    if (control_mode == MODE_ABC) {
+                        double tmp_ratio = 0.0;
+                        leg_names_interpolator->set(&tmp_ratio);
+                        tmp_ratio = 1.0;
+                        leg_names_interpolator->setGoal(&tmp_ratio, 5.0, true);
+                        control_mode = MODE_SYNC_TO_ABC;
+                    }
+                }
+            } else {
+                std::cerr << "[" << m_profile.instance_name << "]   leg_names cannot be set because interpolating." << std::endl;
+            }
+            if (!gg_is_walking) {
+                is_hand_fix_mode = i_param.is_hand_fix_mode;
+                std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode = " << is_hand_fix_mode << std::endl;
+            } else {
+                std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode cannot be set in (gg_is_walking = true). Current is_hand_fix_mode is " << (is_hand_fix_mode?"true":"false") << std::endl;
+            }
+            if (control_mode == MODE_IDLE) {
+                for (size_t i = 0; i < i_param.end_effector_list.length(); i++) {
+                    std::map<std::string, ABCIKparam>::iterator it = ikp.find(std::string(i_param.end_effector_list[i].leg));
+                    memcpy(it->second.localPos.data(), i_param.end_effector_list[i].pos, sizeof(double)*3);
+                    it->second.localR = (Eigen::Quaternion<double>(i_param.end_effector_list[i].rot[0], i_param.end_effector_list[i].rot[1], i_param.end_effector_list[i].rot[2], i_param.end_effector_list[i].rot[3])).normalized().toRotationMatrix();
+                }
+            } else {
+                std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl;
+            }
+            if (i_param.default_gait_type == OpenHRP::AutoBalancerService::BIPED) {
+                gait_type = BIPED;
+            } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::TROT) {
+                gait_type = TROT;
+            } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::PACE) {
+                gait_type = PACE;
+            } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::CRAWL) {
+                gait_type = CRAWL;
+            } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) {
+                gait_type = GALLOP;
+            }
+            // Ref force balancing
+            std::cerr << "[" << m_profile.instance_name << "] Ref force balancing" << std::endl;
+            if ( use_force == MODE_REF_FORCE_WITH_FOOT && control_mode != MODE_IDLE ) {
+                std::cerr << "[" << m_profile.instance_name << "]   additional_force_applied_point_offset and additional_force_applied_link_name cannot be updated during MODE_REF_FORCE_WITH_FOOT and non-MODE_IDLE"<< std::endl;
+            } else if ( !m_robot->link(std::string(i_param.additional_force_applied_link_name)) ) {
+                std::cerr << "[" << m_profile.instance_name << "]   Invalid link name for additional_force_applied_link_name = " << i_param.additional_force_applied_link_name << std::endl;
+            } else {
+                additional_force_applied_link = m_robot->link(std::string(i_param.additional_force_applied_link_name));
+                for (size_t i = 0; i < 3; i++) {
+                    additional_force_applied_point_offset(i) = i_param.additional_force_applied_point_offset[i];
+                }
+                std::cerr << "[" << m_profile.instance_name << "]   Link name for additional_force_applied_link_name = " << additional_force_applied_link->name << ", additional_force_applied_point_offset = " << additional_force_applied_point_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+            }
 
-  for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
-      std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   localR = " << it->second.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
-  }
+            for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
+                std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl;
+                std::cerr << "[" << m_profile.instance_name << "]   localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+                std::cerr << "[" << m_profile.instance_name << "]   localR = " << it->second.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
+            }
 
-  std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets = ";
-  for (size_t i = 0; i < ikp.size() * 3; i++) {
-      std::cerr << default_zmp_offsets_array[i] << " ";
-  }
-  std::cerr << std::endl;
-  delete[] default_zmp_offsets_array;
-  std::cerr << "[" << m_profile.instance_name << "]   use_force_mode = " << getUseForceModeString() << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_mode = " << graspless_manip_mode << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_arm = " << graspless_manip_arm << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_pos = " << graspless_manip_reference_trans_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_rot = " << graspless_manip_reference_trans_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   transition_time = " << transition_time << "[s], zmp_transition_time = " << zmp_transition_time << "[s], adjust_footstep_transition_time = " << adjust_footstep_transition_time << "[s]" << std::endl;
-  for (std::vector<std::string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) std::cerr << "[" << m_profile.instance_name << "]   leg_names [" << *it << "]" << std::endl;
-  std::cerr << "[" << m_profile.instance_name << "]   default_gait_type = " << gait_type << std::endl;
-  // FIK
-  fik->move_base_gain = i_param.move_base_gain;
-  fik->pos_ik_thre = i_param.pos_ik_thre;
-  fik->rot_ik_thre = i_param.rot_ik_thre;
-  fik->printParam();
-  // IK limb parameters
-  fik->setIKParam(ee_vec, i_param.ik_limb_parameters);
-  // Limb stretch avoidance
-  fik->use_limb_stretch_avoidance = i_param.use_limb_stretch_avoidance;
-  fik->limb_stretch_avoidance_time_const = i_param.limb_stretch_avoidance_time_const;
-  for (size_t i = 0; i < 2; i++) {
-    fik->limb_stretch_avoidance_vlimit[i] = i_param.limb_stretch_avoidance_vlimit[i];
-  }
-  for (size_t i = 0; i < fik->ikp.size(); i++) {
-    fik->ikp[ee_vec[i]].limb_length_margin = i_param.limb_length_margin[i];
-  }
-  return true;
+            std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets = ";
+            for (size_t i = 0; i < ikp.size() * 3; i++) {
+                std::cerr << default_zmp_offsets_array[i] << " ";
+            }
+            std::cerr << std::endl;
+            delete[] default_zmp_offsets_array;
+            std::cerr << "[" << m_profile.instance_name << "]   use_force_mode = " << getUseForceModeString() << std::endl;
+            std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_mode = " << graspless_manip_mode << std::endl;
+            std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_arm = " << graspless_manip_arm << std::endl;
+            std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
+            std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_pos = " << graspless_manip_reference_trans_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
+            std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_rot = " << graspless_manip_reference_trans_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
+            std::cerr << "[" << m_profile.instance_name << "]   transition_time = " << transition_time << "[s], zmp_transition_time = " << zmp_transition_time << "[s], adjust_footstep_transition_time = " << adjust_footstep_transition_time << "[s]" << std::endl;
+            for (std::vector<std::string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) std::cerr << "[" << m_profile.instance_name << "]   leg_names [" << *it << "]" << std::endl;
+            std::cerr << "[" << m_profile.instance_name << "]   default_gait_type = " << gait_type << std::endl;
+            // FIK
+            fik->move_base_gain = i_param.move_base_gain;
+            fik->pos_ik_thre = i_param.pos_ik_thre;
+            fik->rot_ik_thre = i_param.rot_ik_thre;
+            fik->printParam();
+            // IK limb parameters
+            fik->setIKParam(ee_vec, i_param.ik_limb_parameters);
+            // Limb stretch avoidance
+            fik->use_limb_stretch_avoidance = i_param.use_limb_stretch_avoidance;
+            fik->limb_stretch_avoidance_time_const = i_param.limb_stretch_avoidance_time_const;
+            for (size_t i = 0; i < 2; i++) {
+                fik->limb_stretch_avoidance_vlimit[i] = i_param.limb_stretch_avoidance_vlimit[i];
+            }
+            for (size_t i = 0; i < fik->ikp.size(); i++) {
+                fik->ikp[ee_vec[i]].limb_length_margin = i_param.limb_length_margin[i];
+            }
+            return true;
 };
 
 bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
 {
-  i_param.default_zmp_offsets.length(ikp.size());
-  for (size_t i = 0; i < ikp.size(); i++) {
-      i_param.default_zmp_offsets[i].length(3);
-      for (size_t j = 0; j < 3; j++) {
-          i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j);
-      }
-  }
-  switch(control_mode) {
-  case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break;
-  case MODE_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_ABC; break;
-  case MODE_SYNC_TO_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_IDLE; break;
-  case MODE_SYNC_TO_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_ABC; break;
-  default: break;
-  }
-  switch(use_force) {
-  case MODE_NO_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_NO_FORCE; break;
-  case MODE_REF_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE; break;
-  case MODE_REF_FORCE_WITH_FOOT: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT; break;
-  case MODE_REF_FORCE_RFU_EXT_MOMENT: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT; break;
-  default: break;
-  }
-  i_param.graspless_manip_mode = graspless_manip_mode;
-  i_param.graspless_manip_arm = graspless_manip_arm.c_str();
-  for (size_t j = 0; j < 3; j++)
-      i_param.graspless_manip_p_gain[j] = graspless_manip_p_gain[j];
-  for (size_t j = 0; j < 3; j++)
-      i_param.graspless_manip_reference_trans_pos[j] = graspless_manip_reference_trans_coords.pos[j];
-  Eigen::Quaternion<double> qt(graspless_manip_reference_trans_coords.rot);
-  i_param.graspless_manip_reference_trans_rot[0] = qt.w();
-  i_param.graspless_manip_reference_trans_rot[1] = qt.x();
-  i_param.graspless_manip_reference_trans_rot[2] = qt.y();
-  i_param.graspless_manip_reference_trans_rot[3] = qt.z();
-  i_param.transition_time = transition_time;
-  i_param.zmp_transition_time = zmp_transition_time;
-  i_param.adjust_footstep_transition_time = adjust_footstep_transition_time;
-  i_param.leg_names.length(leg_names.size());
-  for (size_t i = 0; i < leg_names.size(); i++) i_param.leg_names[i] = leg_names.at(i).c_str();
-  i_param.is_hand_fix_mode = is_hand_fix_mode;
-  i_param.end_effector_list.length(ikp.size());
-  {
-      size_t i = 0;
-      for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
-          copyRatscoords2Footstep(i_param.end_effector_list[i],
-                                  coordinates(it->second.localPos, it->second.localR));
-          i_param.end_effector_list[i].leg = it->first.c_str();
-          i++;
-      }
-  }
-  switch(gait_type) {
-  case BIPED:  i_param.default_gait_type = OpenHRP::AutoBalancerService::BIPED;  break;
-  case TROT:   i_param.default_gait_type = OpenHRP::AutoBalancerService::TROT;   break;
-  case PACE:   i_param.default_gait_type = OpenHRP::AutoBalancerService::PACE;   break;
-  case CRAWL:  i_param.default_gait_type = OpenHRP::AutoBalancerService::CRAWL;  break;
-  case GALLOP: i_param.default_gait_type = OpenHRP::AutoBalancerService::GALLOP; break;
-  default: break;
-  }
-  // FIK
-  i_param.move_base_gain = fik->move_base_gain;
-  i_param.pos_ik_thre = fik->pos_ik_thre;
-  i_param.rot_ik_thre = fik->rot_ik_thre;
-  // IK limb parameters
-  fik->getIKParam(ee_vec, i_param.ik_limb_parameters);
-  // Limb stretch avoidance
-  i_param.use_limb_stretch_avoidance = fik->use_limb_stretch_avoidance;
-  i_param.limb_stretch_avoidance_time_const = fik->limb_stretch_avoidance_time_const;
-  i_param.limb_length_margin.length(fik->ikp.size());
-  for (size_t i = 0; i < 2; i++) {
-    i_param.limb_stretch_avoidance_vlimit[i] = fik->limb_stretch_avoidance_vlimit[i];
-  }
-  for (size_t i = 0; i < ikp.size(); i++) {
-    i_param.limb_length_margin[i] = fik->ikp[ee_vec[i]].limb_length_margin;
-  }
-  i_param.additional_force_applied_link_name = additional_force_applied_link->name.c_str();
-  for (size_t i = 0; i < 3; i++) {
-      i_param.additional_force_applied_point_offset[i] = additional_force_applied_point_offset(i);
-  }
-  return true;
+    i_param.default_zmp_offsets.length(ikp.size());
+    for (size_t i = 0; i < ikp.size(); i++) {
+        i_param.default_zmp_offsets[i].length(3);
+        for (size_t j = 0; j < 3; j++) {
+            i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j);
+        }
+    }
+    switch(control_mode) {
+        case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break;
+        case MODE_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_ABC; break;
+        case MODE_SYNC_TO_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_IDLE; break;
+        case MODE_SYNC_TO_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_ABC; break;
+        default: break;
+    }
+    switch(use_force) {
+        case MODE_NO_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_NO_FORCE; break;
+        case MODE_REF_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE; break;
+        case MODE_REF_FORCE_WITH_FOOT: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT; break;
+        case MODE_REF_FORCE_RFU_EXT_MOMENT: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT; break;
+        default: break;
+    }
+    i_param.graspless_manip_mode = graspless_manip_mode;
+    i_param.graspless_manip_arm = graspless_manip_arm.c_str();
+    for (size_t j = 0; j < 3; j++)
+        i_param.graspless_manip_p_gain[j] = graspless_manip_p_gain[j];
+    for (size_t j = 0; j < 3; j++)
+        i_param.graspless_manip_reference_trans_pos[j] = graspless_manip_reference_trans_coords.pos[j];
+    Eigen::Quaternion<double> qt(graspless_manip_reference_trans_coords.rot);
+    i_param.graspless_manip_reference_trans_rot[0] = qt.w();
+    i_param.graspless_manip_reference_trans_rot[1] = qt.x();
+    i_param.graspless_manip_reference_trans_rot[2] = qt.y();
+    i_param.graspless_manip_reference_trans_rot[3] = qt.z();
+    i_param.transition_time = transition_time;
+    i_param.zmp_transition_time = zmp_transition_time;
+    i_param.adjust_footstep_transition_time = adjust_footstep_transition_time;
+    i_param.leg_names.length(leg_names.size());
+    for (size_t i = 0; i < leg_names.size(); i++) i_param.leg_names[i] = leg_names.at(i).c_str();
+    i_param.is_hand_fix_mode = is_hand_fix_mode;
+    i_param.end_effector_list.length(ikp.size());
+    {
+        size_t i = 0;
+        for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
+            copyRatscoords2Footstep(i_param.end_effector_list[i],
+                    coordinates(it->second.localPos, it->second.localR));
+            i_param.end_effector_list[i].leg = it->first.c_str();
+            i++;
+        }
+    }
+    switch(gait_type) {
+        case BIPED:  i_param.default_gait_type = OpenHRP::AutoBalancerService::BIPED;  break;
+        case TROT:   i_param.default_gait_type = OpenHRP::AutoBalancerService::TROT;   break;
+        case PACE:   i_param.default_gait_type = OpenHRP::AutoBalancerService::PACE;   break;
+        case CRAWL:  i_param.default_gait_type = OpenHRP::AutoBalancerService::CRAWL;  break;
+        case GALLOP: i_param.default_gait_type = OpenHRP::AutoBalancerService::GALLOP; break;
+        default: break;
+    }
+    // FIK
+    i_param.move_base_gain = fik->move_base_gain;
+    i_param.pos_ik_thre = fik->pos_ik_thre;
+    i_param.rot_ik_thre = fik->rot_ik_thre;
+    // IK limb parameters
+    fik->getIKParam(ee_vec, i_param.ik_limb_parameters);
+    // Limb stretch avoidance
+    i_param.use_limb_stretch_avoidance = fik->use_limb_stretch_avoidance;
+    i_param.limb_stretch_avoidance_time_const = fik->limb_stretch_avoidance_time_const;
+    i_param.limb_length_margin.length(fik->ikp.size());
+    for (size_t i = 0; i < 2; i++) {
+        i_param.limb_stretch_avoidance_vlimit[i] = fik->limb_stretch_avoidance_vlimit[i];
+    }
+    for (size_t i = 0; i < ikp.size(); i++) {
+        i_param.limb_length_margin[i] = fik->ikp[ee_vec[i]].limb_length_margin;
+    }
+    i_param.additional_force_applied_link_name = additional_force_applied_link->name.c_str();
+    for (size_t i = 0; i < 3; i++) {
+        i_param.additional_force_applied_point_offset[i] = additional_force_applied_point_offset(i);
+    }
+    return true;
 };
 
 void AutoBalancer::copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs)
 {
-  memcpy(out_fs.pos, in_fs.pos.data(), sizeof(double)*3);
-  Eigen::Quaternion<double> qt(in_fs.rot);
-  out_fs.rot[0] = qt.w();
-  out_fs.rot[1] = qt.x();
-  out_fs.rot[2] = qt.y();
-  out_fs.rot[3] = qt.z();
+    memcpy(out_fs.pos, in_fs.pos.data(), sizeof(double)*3);
+    Eigen::Quaternion<double> qt(in_fs.rot);
+    out_fs.rot[0] = qt.w();
+    out_fs.rot[1] = qt.x();
+    out_fs.rot[2] = qt.y();
+    out_fs.rot[3] = qt.z();
 };
 
 bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param)
 {
-  copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_leg_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.swing_leg_dst_coords, gg->get_swing_leg_dst_steps().front().worldcoords);
-  copyRatscoords2Footstep(i_param.dst_foot_midcoords, gg->get_dst_foot_midcoords());
-  if (gg->get_support_leg_names().front() == "rleg") {
-    i_param.support_leg = OpenHRP::AutoBalancerService::RLEG;
-  } else {
-    i_param.support_leg = OpenHRP::AutoBalancerService::LLEG;
-  }
-  switch ( gg->get_current_support_states().front() ) {
-  case BOTH: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::BOTH; break;
-  case RLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::RLEG; break;
-  case LLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::LLEG; break;
-  default: break;
-  }
-  return true;
+    copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_leg_steps().front().worldcoords);
+    copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_steps().front().worldcoords);
+    copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_steps().front().worldcoords);
+    copyRatscoords2Footstep(i_param.swing_leg_dst_coords, gg->get_swing_leg_dst_steps().front().worldcoords);
+    copyRatscoords2Footstep(i_param.dst_foot_midcoords, gg->get_dst_foot_midcoords());
+    if (gg->get_support_leg_names().front() == "rleg") {
+        i_param.support_leg = OpenHRP::AutoBalancerService::RLEG;
+    } else {
+        i_param.support_leg = OpenHRP::AutoBalancerService::LLEG;
+    }
+    switch ( gg->get_current_support_states().front() ) {
+        case BOTH: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::BOTH; break;
+        case RLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::RLEG; break;
+        case LLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::LLEG; break;
+        default: break;
+    }
+    return true;
 };
 
 bool AutoBalancer::adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep)
 {
-  std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
+    std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
             << "] adjustFootSteps" << std::endl;
-  if (control_mode == MODE_ABC && !gg_is_walking && adjust_footstep_interpolator->isEmpty()) {
-      Guard guard(m_mutex);
-      //
-      hrp::Vector3 eepos, org_mid_rpy, target_mid_rpy;
-      hrp::Matrix33 eerot, tmprot;
-      coordinates org_mid_coords, target_mid_coords;
-      // Get org coords
-      ikp["rleg"].adjust_interpolation_org_p0 = ikp["rleg"].target_p0;
-      ikp["lleg"].adjust_interpolation_org_p0 = ikp["lleg"].target_p0;
-      ikp["rleg"].adjust_interpolation_org_r0 = ikp["rleg"].target_r0;
-      ikp["lleg"].adjust_interpolation_org_r0 = ikp["lleg"].target_r0;
-      mid_coords(org_mid_coords, 0.5,
-                 coordinates(ikp["rleg"].adjust_interpolation_org_p0, ikp["rleg"].adjust_interpolation_org_r0),
-                 coordinates(ikp["lleg"].adjust_interpolation_org_p0, ikp["lleg"].adjust_interpolation_org_r0));
-      org_mid_rpy = hrp::rpyFromRot(org_mid_coords.rot);
-      // Get target coords
-      //   Input : ee coords
-      //   Output : link coords
-      memcpy(eepos.data(), rfootstep.pos, sizeof(double)*3);
-      eerot = (Eigen::Quaternion<double>(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
-      ikp["rleg"].adjust_interpolation_target_r0 = eerot;
-      ikp["rleg"].adjust_interpolation_target_p0 = eepos;
-      memcpy(eepos.data(), lfootstep.pos, sizeof(double)*3);
-      eerot = (Eigen::Quaternion<double>(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
-      ikp["lleg"].adjust_interpolation_target_r0 = eerot;
-      ikp["lleg"].adjust_interpolation_target_p0 = eepos;
-      mid_coords(target_mid_coords, 0.5,
-                 coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0),
-                 coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
-      coordinates rtrans, ltrans;
-      target_mid_coords.transformation(rtrans, coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0));
-      target_mid_coords.transformation(ltrans, coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
-      target_mid_rpy = hrp::rpyFromRot(target_mid_coords.rot);
-      // Fix target pos => org pos, target yaw => org yaw
-      target_mid_rpy(2) = org_mid_rpy(2);
-      target_mid_coords.rot = hrp::rotFromRpy(target_mid_rpy);
-      target_mid_coords.pos = org_mid_coords.pos;
-      // Calculate rleg and lleg coords
-      coordinates tmpc;
-      tmpc = target_mid_coords;
-      tmpc.transform(rtrans);
-      ikp["rleg"].adjust_interpolation_target_p0 = tmpc.pos;
-      ikp["rleg"].adjust_interpolation_target_r0 = tmpc.rot;
-      tmpc = target_mid_coords;
-      tmpc.transform(ltrans);
-      ikp["lleg"].adjust_interpolation_target_p0 = tmpc.pos;
-      ikp["lleg"].adjust_interpolation_target_r0 = tmpc.rot;
-      // Set interpolator
-      adjust_footstep_interpolator->clear();
-      double tmp = 0.0;
-      adjust_footstep_interpolator->set(&tmp);
-      tmp = 1.0;
-      adjust_footstep_interpolator->setGoal(&tmp, adjust_footstep_transition_time, true);
-  }
-  while (!adjust_footstep_interpolator->isEmpty() )
+    if (control_mode == MODE_ABC && !gg_is_walking && adjust_footstep_interpolator->isEmpty()) {
+        Guard guard(m_mutex);
+        //
+        hrp::Vector3 eepos, org_mid_rpy, target_mid_rpy;
+        hrp::Matrix33 eerot, tmprot;
+        coordinates org_mid_coords, target_mid_coords;
+        // Get org coords
+        ikp["rleg"].adjust_interpolation_org_p0 = ikp["rleg"].target_p0;
+        ikp["lleg"].adjust_interpolation_org_p0 = ikp["lleg"].target_p0;
+        ikp["rleg"].adjust_interpolation_org_r0 = ikp["rleg"].target_r0;
+        ikp["lleg"].adjust_interpolation_org_r0 = ikp["lleg"].target_r0;
+        mid_coords(org_mid_coords, 0.5,
+                coordinates(ikp["rleg"].adjust_interpolation_org_p0, ikp["rleg"].adjust_interpolation_org_r0),
+                coordinates(ikp["lleg"].adjust_interpolation_org_p0, ikp["lleg"].adjust_interpolation_org_r0));
+        org_mid_rpy = hrp::rpyFromRot(org_mid_coords.rot);
+        // Get target coords
+        //   Input : ee coords
+        //   Output : link coords
+        memcpy(eepos.data(), rfootstep.pos, sizeof(double)*3);
+        eerot = (Eigen::Quaternion<double>(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc:
+        ikp["rleg"].adjust_interpolation_target_r0 = eerot;
+        ikp["rleg"].adjust_interpolation_target_p0 = eepos;
+        memcpy(eepos.data(), lfootstep.pos, sizeof(double)*3);
+        eerot = (Eigen::Quaternion<double>(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc:
+        ikp["lleg"].adjust_interpolation_target_r0 = eerot;
+        ikp["lleg"].adjust_interpolation_target_p0 = eepos;
+        mid_coords(target_mid_coords, 0.5,
+                coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0),
+                coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
+        coordinates rtrans, ltrans;
+        target_mid_coords.transformation(rtrans, coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0));
+        target_mid_coords.transformation(ltrans, coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
+        target_mid_rpy = hrp::rpyFromRot(target_mid_coords.rot);
+        // Fix target pos => org pos, target yaw => org yaw
+        target_mid_rpy(2) = org_mid_rpy(2);
+        target_mid_coords.rot = hrp::rotFromRpy(target_mid_rpy);
+        target_mid_coords.pos = org_mid_coords.pos;
+        // Calculate rleg and lleg coords
+        coordinates tmpc;
+        tmpc = target_mid_coords;
+        tmpc.transform(rtrans);
+        ikp["rleg"].adjust_interpolation_target_p0 = tmpc.pos;
+        ikp["rleg"].adjust_interpolation_target_r0 = tmpc.rot;
+        tmpc = target_mid_coords;
+        tmpc.transform(ltrans);
+        ikp["lleg"].adjust_interpolation_target_p0 = tmpc.pos;
+        ikp["lleg"].adjust_interpolation_target_r0 = tmpc.rot;
+        // Set interpolator
+        adjust_footstep_interpolator->clear();
+        double tmp = 0.0;
+        adjust_footstep_interpolator->set(&tmp);
+        tmp = 1.0;
+        adjust_footstep_interpolator->setGoal(&tmp, adjust_footstep_transition_time, true);
+    }
+    while (!adjust_footstep_interpolator->isEmpty() )
+        usleep(1000);
     usleep(1000);
-  usleep(1000);
-  return true;
+    return true;
 };
 
 bool AutoBalancer::getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx)
@@ -2067,17 +2077,17 @@ bool AutoBalancer::getGoPosFootstepsSequence(const double& x, const double& y, c
         if (is_valid_gait_type == false) return false;
         /* go_pos_param_2_footstep_nodes_list_core is const member function  */
         gg->go_pos_param_2_footstep_nodes_list_core (x, y, th,
-                                                     initial_support_legs_coords, start_ref_coords, initial_support_legs,
-                                                     new_footstep_nodes_list, true, 0);
+                initial_support_legs_coords, start_ref_coords, initial_support_legs,
+                new_footstep_nodes_list, true, 0);
         o_footstep->length(new_footstep_nodes_list.size());
         for (size_t i = 0; i < new_footstep_nodes_list.size(); i++) {
             o_footstep[i].fs.length(new_footstep_nodes_list.at(i).size());
             for (size_t j = 0; j < new_footstep_nodes_list.at(i).size(); j++) {
                 leg_type tmp_leg_type = new_footstep_nodes_list.at(i).at(j).l_r;
                 o_footstep[i].fs[j].leg = ((tmp_leg_type == RLEG) ? "rleg":
-                                           (tmp_leg_type == LLEG) ? "lleg":
-                                           (tmp_leg_type == RARM) ? "rarm":
-                                           "larm");
+                        (tmp_leg_type == LLEG) ? "lleg":
+                                (tmp_leg_type == RARM) ? "rarm":
+                                        "larm");
                 copyRatscoords2Footstep(o_footstep[i].fs[j], new_footstep_nodes_list.at(i).at(j).worldcoords);
             }
         }
@@ -2087,63 +2097,63 @@ bool AutoBalancer::getGoPosFootstepsSequence(const double& x, const double& y, c
 
 void AutoBalancer::static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height)
 {
-  hrp::Vector3 target_sbp = hrp::Vector3(0, 0, 0);
-  hrp::Vector3 tmpcog = m_robot->calcCM();
-  if ( use_force == MODE_NO_FORCE ) {
-    tmp_input_sbp = tmpcog + sbp_cog_offset;
-  } else {
-    calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height, ref_forces);
-    tmp_input_sbp = target_sbp - sbp_offset;
-    sbp_cog_offset = tmp_input_sbp - tmpcog;
-  }
+    hrp::Vector3 target_sbp = hrp::Vector3(0, 0, 0);
+    hrp::Vector3 tmpcog = m_robot->calcCM();
+    if ( use_force == MODE_NO_FORCE ) {
+        tmp_input_sbp = tmpcog + sbp_cog_offset;
+    } else {
+        calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height, ref_forces);
+        tmp_input_sbp = target_sbp - sbp_offset;
+        sbp_cog_offset = tmp_input_sbp - tmpcog;
+    }
 };
 
 void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces)
 {
-  hrp::Vector3 denom, nume;
-  /* sb_point[m] = nume[kg * m/s^2 * m] / denom[kg * m/s^2] */
-  double mass = m_robot->totalMass();
-  double mg = mass * gg->get_gravitational_acceleration();
-  hrp::Vector3 total_sensor_ref_force = hrp::Vector3::Zero();
-  for (size_t i = 0; i < tmp_forces.size(); i++) {
-      total_sensor_ref_force += tmp_forces[i];
-  }
-  hrp::Vector3 total_nosensor_ref_force = mg * hrp::Vector3::UnitZ() - total_sensor_ref_force; // total ref force at the point without sensors, such as torso
-  hrp::Vector3 tmp_ext_moment = fix_leg_coords2.pos.cross(total_nosensor_ref_force) + fix_leg_coords2.rot * hrp::Vector3(m_refFootOriginExtMoment.data.x, m_refFootOriginExtMoment.data.y, m_refFootOriginExtMoment.data.z);
-  // For MODE_REF_FORCE_RFU_EXT_MOMENT, store previous root position to calculate influence from tmp_ext_moment while walking (basically, root link moves while walking).
-  //   Calculate values via fix_leg_coords2 relative/world values.
-  static hrp::Vector3 prev_additional_force_applied_pos = fix_leg_coords2.rot.transpose() * (additional_force_applied_link->p-fix_leg_coords2.pos);
-  //   If not is_hold_value (not hold value), update prev_additional_force_applied_pos
-  if ( !m_refFootOriginExtMomentIsHoldValue.data ) {
-      prev_additional_force_applied_pos = fix_leg_coords2.rot.transpose() * (additional_force_applied_link->p-fix_leg_coords2.pos);
-  }
-  hrp::Vector3 tmp_prev_additional_force_applied_pos = fix_leg_coords2.rot * prev_additional_force_applied_pos + fix_leg_coords2.pos;
-  // Calculate SBP
-  for (size_t j = 0; j < 2; j++) {
-    nume(j) = mg * tmpcog(j);
-    denom(j) = mg;
-    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-      // Check leg_names. leg_names is assumed to be support limb for locomotion, cannot be used for manipulation. If it->first is not included in leg_names, use it for manipulation and static balance point calculation.
-      if (std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()) {
-        size_t idx = contact_states_index_map[it->first];
-        // Force applied point is assumed as end effector
-        hrp::Vector3 fpos = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
-        nume(j) += ( (fpos(2) - ref_com_height) * tmp_forces[idx](j) - fpos(j) * tmp_forces[idx](2) );
-        denom(j) -= tmp_forces[idx](2);
-      }
-    }
-    if ( use_force == MODE_REF_FORCE_WITH_FOOT ) {
-        hrp::Vector3 fpos(additional_force_applied_link->p+additional_force_applied_point_offset);
-        nume(j) += ( (fpos(2) - ref_com_height) * total_nosensor_ref_force(j) - fpos(j) * total_nosensor_ref_force(2) );
-        denom(j) -= total_nosensor_ref_force(2);
-    } else if ( use_force == MODE_REF_FORCE_RFU_EXT_MOMENT ) {
-        //nume(j) += (j==0 ? tmp_ext_moment(1):-tmp_ext_moment(0));
-        nume(j) += (tmp_prev_additional_force_applied_pos(j)-additional_force_applied_link->p(j))*total_nosensor_ref_force(2) + (j==0 ? tmp_ext_moment(1):-tmp_ext_moment(0));
-        denom(j) -= total_nosensor_ref_force(2);
-    }
-    sb_point(j) = nume(j) / denom(j);
-  }
-  sb_point(2) = ref_com_height;
+    hrp::Vector3 denom, nume;
+    /* sb_point[m] = nume[kg * m/s^2 * m] / denom[kg * m/s^2] */
+    double mass = m_robot->totalMass();
+    double mg = mass * gg->get_gravitational_acceleration();
+    hrp::Vector3 total_sensor_ref_force = hrp::Vector3::Zero();
+    for (size_t i = 0; i < tmp_forces.size(); i++) {
+        total_sensor_ref_force += tmp_forces[i];
+    }
+    hrp::Vector3 total_nosensor_ref_force = mg * hrp::Vector3::UnitZ() - total_sensor_ref_force; // total ref force at the point without sensors, such as torso
+    hrp::Vector3 tmp_ext_moment = fix_leg_coords2.pos.cross(total_nosensor_ref_force) + fix_leg_coords2.rot * hrp::Vector3(m_refFootOriginExtMoment.data.x, m_refFootOriginExtMoment.data.y, m_refFootOriginExtMoment.data.z);
+    // For MODE_REF_FORCE_RFU_EXT_MOMENT, store previous root position to calculate influence from tmp_ext_moment while walking (basically, root link moves while walking).
+    //   Calculate values via fix_leg_coords2 relative/world values.
+    static hrp::Vector3 prev_additional_force_applied_pos = fix_leg_coords2.rot.transpose() * (additional_force_applied_link->p-fix_leg_coords2.pos);
+    //   If not is_hold_value (not hold value), update prev_additional_force_applied_pos
+    if ( !m_refFootOriginExtMomentIsHoldValue.data ) {
+        prev_additional_force_applied_pos = fix_leg_coords2.rot.transpose() * (additional_force_applied_link->p-fix_leg_coords2.pos);
+    }
+    hrp::Vector3 tmp_prev_additional_force_applied_pos = fix_leg_coords2.rot * prev_additional_force_applied_pos + fix_leg_coords2.pos;
+    // Calculate SBP
+    for (size_t j = 0; j < 2; j++) {
+        nume(j) = mg * tmpcog(j);
+        denom(j) = mg;
+        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            // Check leg_names. leg_names is assumed to be support limb for locomotion, cannot be used for manipulation. If it->first is not included in leg_names, use it for manipulation and static balance point calculation.
+            if (std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()) {
+                size_t idx = contact_states_index_map[it->first];
+                // Force applied point is assumed as end effector
+                hrp::Vector3 fpos = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
+                nume(j) += ( (fpos(2) - ref_com_height) * tmp_forces[idx](j) - fpos(j) * tmp_forces[idx](2) );
+                denom(j) -= tmp_forces[idx](2);
+            }
+        }
+        if ( use_force == MODE_REF_FORCE_WITH_FOOT ) {
+            hrp::Vector3 fpos(additional_force_applied_link->p+additional_force_applied_point_offset);
+            nume(j) += ( (fpos(2) - ref_com_height) * total_nosensor_ref_force(j) - fpos(j) * total_nosensor_ref_force(2) );
+            denom(j) -= total_nosensor_ref_force(2);
+        } else if ( use_force == MODE_REF_FORCE_RFU_EXT_MOMENT ) {
+            //nume(j) += (j==0 ? tmp_ext_moment(1):-tmp_ext_moment(0));
+            nume(j) += (tmp_prev_additional_force_applied_pos(j)-additional_force_applied_link->p(j))*total_nosensor_ref_force(2) + (j==0 ? tmp_ext_moment(1):-tmp_ext_moment(0));
+            denom(j) -= total_nosensor_ref_force(2);
+        }
+        sb_point(j) = nume(j) / denom(j);
+    }
+    sb_point(2) = ref_com_height;
 };
 
 #ifndef rad2deg
@@ -2155,72 +2165,72 @@ void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point,
 
 hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_coords)
 {
-  if (graspless_manip_mode) {
-    hrp::Vector3 dp,dr;
-    coordinates ref_hand_coords(gg->get_dst_foot_midcoords()), act_hand_coords;
-    ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords
-    hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos);
-    if ( graspless_manip_arm == "arms" ) {
-        hrp::Vector3 rarm_pos = ikp["rarm"].target_p0;
-        hrp::Vector3 larm_pos = ikp["larm"].target_p0;
-        act_hand_coords.pos = (rarm_pos+larm_pos)/2.0;
-        hrp::Vector3 act_y = larm_pos-rarm_pos;
-        act_y(2) = 0;
-        act_y.normalize();
-        hrp::Vector3 ref_y(ref_hand_coords.rot * hrp::Vector3::UnitY());
-        ref_y(2) = 0;
-        ref_y.normalize();
-        dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y))); // fix for rotation
+    if (graspless_manip_mode) {
+        hrp::Vector3 dp,dr;
+        coordinates ref_hand_coords(gg->get_dst_foot_midcoords()), act_hand_coords;
+        ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords
+        hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos);
+        if ( graspless_manip_arm == "arms" ) {
+            hrp::Vector3 rarm_pos = ikp["rarm"].target_p0;
+            hrp::Vector3 larm_pos = ikp["larm"].target_p0;
+            act_hand_coords.pos = (rarm_pos+larm_pos)/2.0;
+            hrp::Vector3 act_y = larm_pos-rarm_pos;
+            act_y(2) = 0;
+            act_y.normalize();
+            hrp::Vector3 ref_y(ref_hand_coords.rot * hrp::Vector3::UnitY());
+            ref_y(2) = 0;
+            ref_y.normalize();
+            dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y))); // fix for rotation
+        } else {
+            ABCIKparam& tmpikp = ikp[graspless_manip_arm];
+            act_hand_coords = rats::coordinates(tmpikp.target_p0,
+                    tmpikp.target_r0);
+            rats::difference_rotation(dr, ref_hand_coords.rot, act_hand_coords.rot);
+            dr(0) = 0; dr(1) = 0;
+        }
+        dp = act_hand_coords.pos - ref_hand_coords.pos
+                + dr.cross(hrp::Vector3(foot_pos - act_hand_coords.pos));
+        dp(2) = 0;
+        hrp::Matrix33 foot_mt(gg->get_dst_foot_midcoords().rot.transpose());
+        //alias(dp) = foot_mt * dp;
+        hrp::Vector3 dp2 = foot_mt * dp;
+        //alias(dr) = foot_mt * dr;
+        return hrp::Vector3(graspless_manip_p_gain[0] * dp2(0)/gg->get_default_step_time(),
+                graspless_manip_p_gain[1] * dp2(1)/gg->get_default_step_time(),
+                graspless_manip_p_gain[2] * rad2deg(dr(2))/gg->get_default_step_time());
     } else {
-      ABCIKparam& tmpikp = ikp[graspless_manip_arm];
-      act_hand_coords = rats::coordinates(tmpikp.target_p0,
-                                          tmpikp.target_r0);
-      rats::difference_rotation(dr, ref_hand_coords.rot, act_hand_coords.rot);
-      dr(0) = 0; dr(1) = 0;
-    }
-    dp = act_hand_coords.pos - ref_hand_coords.pos
-        + dr.cross(hrp::Vector3(foot_pos - act_hand_coords.pos));
-    dp(2) = 0;
-    hrp::Matrix33 foot_mt(gg->get_dst_foot_midcoords().rot.transpose());
-    //alias(dp) = foot_mt * dp;
-    hrp::Vector3 dp2 = foot_mt * dp;
-    //alias(dr) = foot_mt * dr;
-    return hrp::Vector3(graspless_manip_p_gain[0] * dp2(0)/gg->get_default_step_time(),
-                        graspless_manip_p_gain[1] * dp2(1)/gg->get_default_step_time(),
-                        graspless_manip_p_gain[2] * rad2deg(dr(2))/gg->get_default_step_time());
-  } else {
-    return hrp::Vector3::Zero();
-  }
+        return hrp::Vector3::Zero();
+    }
 };
 
 bool AutoBalancer::calc_inital_support_legs(const double& y, std::vector<coordinates>& initial_support_legs_coords, std::vector<leg_type>& initial_support_legs, coordinates& start_ref_coords) {
     switch(gait_type) {
-    case BIPED:
-        initial_support_legs_coords = (y > 0 ?
-                                       boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))
-                                       : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0)));
-        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
-        break;
-    case TROT:
-        initial_support_legs_coords = (y > 0 ?
-                                       boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))(coordinates(ikp["larm"].target_p0, ikp["larm"].target_r0))
-                                       : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0))(coordinates(ikp["rarm"].target_p0, ikp["rarm"].target_r0)));
-        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
-        break;
-    case PACE:
-        initial_support_legs_coords = (y > 0 ?
-                                       boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))(coordinates(ikp["rarm"].target_p0, ikp["rarm"].target_r0))
-                                       : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0))(coordinates(ikp["larm"].target_p0, ikp["larm"].target_r0)));
-        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
-        break;
-    case CRAWL:
-        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    case GALLOP:
-        /* at least one leg shoud be in contact */
-        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
-        return false;
-    default: break;
+        case BIPED:
+            initial_support_legs_coords = (y > 0 ?
+                    boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))
+            : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0)));
+            initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
+            break;
+        case TROT:
+            initial_support_legs_coords = (y > 0 ?
+                    boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))(coordinates(ikp["larm"].target_p0, ikp["larm"].target_r0))
+                    : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0))(coordinates(ikp["rarm"].target_p0, ikp["rarm"].target_r0)));
+            initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
+            break;
+        case PACE:
+            initial_support_legs_coords = (y > 0 ?
+                    boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))(coordinates(ikp["rarm"].target_p0, ikp["rarm"].target_r0))
+                    : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0))(coordinates(ikp["larm"].target_p0, ikp["larm"].target_r0)));
+            initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
+            break;
+        case CRAWL:
+            std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
+            return false;
+        case GALLOP:
+            /* at least one leg shoud be in contact */
+            std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
+            return false;
+        default: break;
     }
     start_ref_coords.pos = (ikp["rleg"].target_p0+ikp["lleg"].target_p0)*0.5;
     mid_rot(start_ref_coords.rot, 0.5, ikp["rleg"].target_r0, ikp["lleg"].target_r0);
@@ -2320,8 +2330,8 @@ void AutoBalancer::distributeReferenceZMPToWrenches (const hrp::Vector3& _ref_zm
         m_force[fidx].data[5] = n_ee(2);
         if (DEBUGP) {
             std::cerr << "[" << m_profile.instance_name << "]   "
-                      << "ref_force  [" << leg_names[i] << "] " << f_ee.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
-                      << "ref_moment [" << leg_names[i] << "] " << n_ee.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
+                    << "ref_force  [" << leg_names[i] << "] " << f_ee.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
+                    << "ref_moment [" << leg_names[i] << "] " << n_ee.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
         }
     }
     if (DEBUGP) {
@@ -2345,16 +2355,16 @@ void AutoBalancer::distributeReferenceZMPToWrenches (const hrp::Vector3& _ref_zm
 std::string AutoBalancer::getUseForceModeString ()
 {
     switch (use_force) {
-    case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
-        return "MODE_NO_FORCE";
-    case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
-        return "MODE_REF_FORCE";
-    case OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT:
-        return "MODE_REF_FORCE_WITH_FOOT";
-    case OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT:
-        return "MODE_REF_FORCE_RFU_EXT_MOMENT";
-    default:
-        return "";
+        case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
+            return "MODE_NO_FORCE";
+        case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
+            return "MODE_REF_FORCE";
+        case OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT:
+            return "MODE_REF_FORCE_WITH_FOOT";
+        case OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT:
+            return "MODE_REF_FORCE_RFU_EXT_MOMENT";
+        default:
+            return "";
     }
 };
 
@@ -2366,8 +2376,8 @@ extern "C"
     {
         RTC::Properties profile(autobalancer_spec);
         manager->registerFactory(profile,
-                                 RTC::Create<AutoBalancer>,
-                                 RTC::Delete<AutoBalancer>);
+                RTC::Create<AutoBalancer>,
+                RTC::Delete<AutoBalancer>);
     }
 
 };
diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 157b9727007..7cb3ee4b450 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -13,476 +13,382 @@
 #define rad2deg(rad) (rad * 180 / M_PI)
 #define eps_eq(a, b, c)  (fabs((a)-(b)) <= c)
 #define dbg(var) std::cout<<#var"= "<<(var)<<std::endl
+#define dbgn(var) std::cout<<#var"= "<<std::endl<<(var)<<std::endl
 
-class IKConstraintParam {
-  public:
-    std::string target_link_name;
-    hrp::Vector3 targetPos;
-    hrp::Vector3 targetRpy;
-    hrp::Vector3 localPos;
-    hrp::Matrix33 localR;
-    hrp::dvector6 weight_vec;
-    double pos_precision, rot_precision;
-
-    IKConstraintParam ()
-      :targetPos(hrp::Vector3::Zero()),
-       targetRpy(hrp::Vector3::Zero()),
-       localPos(hrp::Vector3::Zero()),
-       localR(hrp::Matrix33::Identity()),
-       weight_vec(hrp::dvector6::Ones()),
-       pos_precision(1e-3), rot_precision(1e-2)
-    {};
+class IKConstraint {
+    public:
+        std::string target_link_name;
+        hrp::Vector3 targetPos;
+        hrp::Vector3 targetRpy;
+        hrp::Vector3 localPos;
+        hrp::Matrix33 localR;
+        hrp::dvector6 constraint_weight;
+        double pos_precision, rot_precision;
+
+        IKConstraint ()
+        :targetPos(hrp::Vector3::Zero()),
+         targetRpy(hrp::Vector3::Zero()),
+         localPos(hrp::Vector3::Zero()),
+         localR(hrp::Matrix33::Identity()),
+         constraint_weight(hrp::dvector6::Ones()),
+         pos_precision(1e-4), rot_precision(deg2rad(0.1))//これ以上緩いとガタつく場合あり
+        {}
 };
 
 
 class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSolver{
-  protected:
-    hrp::dmatrix J_all, J_all_inv, J_com, J_am;
-    hrp::dvector dq_all, dp_ee_all, weight_vec_all;
-    int WS_DOF, BASE_DOF, J_DOF, ALL_DOF;
-    hrp::Vector3 cur_P,cur_L;
-
-  public:
-    hrp::dvector optional_weight_vector;
-    hrp::dvector reference_q, reference_gain;
-    FullbodyInverseKinematicsSolver (hrp::BodyPtr& _robot, const std::string& _print_str, const double _dt) : SimpleFullbodyInverseKinematicsSolver(_robot,_print_str, _dt) {
-      WS_DOF = 6;
-      BASE_DOF = 6;
-      J_DOF = m_robot->numJoints();
-      ALL_DOF = BASE_DOF + J_DOF;
-      optional_weight_vector = hrp::dvector::Ones(ALL_DOF);
-      reference_q = reference_gain = hrp::dvector::Zero(ALL_DOF);
-      dq_all = hrp::dvector::Zero( ALL_DOF );
-    };
-    ~FullbodyInverseKinematicsSolver () {};
-    // Solve fullbody IK using limb IK
-    int solveFullbodyIKLoop (const std::vector<IKConstraintParam>& _ik_tgt_list, const int _max_iteration) {
-      hrp::Vector3 base_p_old = m_robot->rootLink()->p;
-      hrp::Matrix33 base_R_old = m_robot->rootLink()->R;
-      hrp::dvector q_old(J_DOF);
-      for(int i=0;i<J_DOF;i++){ q_old(i) = m_robot->joint(i)->q; }
-      int loop;
-      for(loop=0; loop < _max_iteration;){
-        solveFullbodyIK(m_robot, _ik_tgt_list);
-        //check ang moment
-        m_robot->rootLink()->v = (m_robot->rootLink()->p - base_p_old)/ m_dt;
-        m_robot->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * m_robot->rootLink()->R) / m_dt;
-        for(int i=0;i<J_DOF;i++){ m_robot->joint(i)->dq = (m_robot->joint(i)->q - q_old(i)) / m_dt; }
-        m_robot->calcForwardKinematics(true,false);
-        m_robot->calcTotalMomentum(cur_P, cur_L);
-        loop++;
-        if(checkIKConvergence(_ik_tgt_list)){ break; }
-      }
-      return loop;
-    }
-    bool checkIKConvergence(const std::vector<IKConstraintParam>& _ik_tgt_list){
-      for ( int i=0; i<_ik_tgt_list.size(); i++ ) {
-        hrp::Link* target_link_ptr = m_robot->link(_ik_tgt_list[i].target_link_name);
-        if(target_link_ptr){
-          hrp::Vector3 pos_err = _ik_tgt_list[i].targetPos - (target_link_ptr->p + target_link_ptr->R * _ik_tgt_list[i].localPos);
-          hrp::Vector3 rot_err = omegaFromRotEx(hrp::rotFromRpy(_ik_tgt_list[i].targetRpy).transpose() * target_link_ptr->R * _ik_tgt_list[i].localR);
-          for(int j=0;j<3;j++){
-            if(pos_err(j) > _ik_tgt_list[i].pos_precision && _ik_tgt_list[i].weight_vec(j) > 0){return false;}
-            if(rot_err(j) > _ik_tgt_list[i].rot_precision && _ik_tgt_list[i].weight_vec(j+3) > 0){return false;}
-          }
-        }
-        else if(!target_link_ptr && _ik_tgt_list[i].target_link_name == "COM"){  // COM
-          hrp::Vector3 pos_err = _ik_tgt_list[i].targetPos - (m_robot->calcCM() + _ik_tgt_list[i].localR * _ik_tgt_list[i].localPos);
-//            robot_in->calcTotalMomentum(fik_in->cur_P, fik_in->cur_L);
-//            hrp::Vector3 rot_err = omegaFromRotEx(ik_tgt_list[i].target_r.transpose() * ik_tgt_list[i].localR * target_link_ptr->R);
-          for(int j=0;j<3;j++){
-            if(pos_err(j) > _ik_tgt_list[i].pos_precision && _ik_tgt_list[i].weight_vec(j) > 0){return false;}
-//              if(rot_err(j) > ik_tgt_list[i].rot_precision && ik_tgt_list[i].selection_vec(j+3) != 0){return false;}
-          }
-        }
-      }
-      return true;
-    }
-    void solveFullbodyIK(hrp::BodyPtr& _robot, const std::vector<IKConstraintParam>& _ik_tgt_list){
-      J_all = hrp::dmatrix::Zero(WS_DOF*_ik_tgt_list.size(), ALL_DOF);
-      dq_all.fill(0);
-      dp_ee_all = hrp::dvector::Zero(WS_DOF*_ik_tgt_list.size());
-      weight_vec_all = hrp::dvector::Ones(WS_DOF*_ik_tgt_list.size());
-      //リファレンスに微少量戻す
-      for(int i=0;i<J_DOF;i++){ _robot->joint(i)->q = _robot->joint(i)->q * (1-reference_gain(i)) + reference_q(i) * reference_gain(i); }
-      _robot->rootLink()->p = _robot->rootLink()->p.cwiseProduct(hrp::Vector3::Ones()-reference_gain.segment(J_DOF,3)) + reference_q.segment(J_DOF,3).cwiseProduct(reference_gain.segment(J_DOF,3));
-      _robot->rootLink()->R = hrp::rotFromRpy( hrp::rpyFromRot(_robot->rootLink()->R).cwiseProduct(hrp::Vector3::Ones()-reference_gain.segment(J_DOF+3,3)) + reference_q.segment(J_DOF+3,3).cwiseProduct(reference_gain.segment(J_DOF+3,3)));
-      for(int i=0;i<J_DOF;i++){ LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit); }
-      _robot->calcForwardKinematics();
-      //ヤコビアンと各ベクトル生成
-      for ( int i=0; i<_ik_tgt_list.size(); i++ ) {
-        hrp::Link* target_link_ptr = _robot->link(_ik_tgt_list[i].target_link_name);
-        hrp::Matrix33 ref_link_origin_R(hrp::rotFromRpy(_ik_tgt_list[i].targetRpy) * _ik_tgt_list[i].localR.transpose()); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
-        hrp::Vector3 ref_link_origin_p(_ik_tgt_list[i].targetPos - ref_link_origin_R * _ik_tgt_list[i].localPos);
-        hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF); //全身to一拘束点へのヤコビアン
-        hrp::Vector3 vel_p_ref, vel_r_ref; //velと言いつつ実際は差分
-        //ベースリンク，通常リンク，重心で場合分け
-        if(target_link_ptr){//ベースリンク，通常リンク共通
-          vel_p_ref = ref_link_origin_p - target_link_ptr->p;
-          vel_r_ref = target_link_ptr->R * omegaFromRotEx(target_link_ptr->R.transpose() * ref_link_origin_R);
-
-          if(target_link_ptr == _robot->rootLink()){ //ベース限定
-            J_part.rightCols(WS_DOF) = hrp::dmatrix::Identity(WS_DOF, WS_DOF);
-          }
-          else{ //通常リンク限定
-            hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), target_link_ptr, m_dt, false, "");
-            hrp::dmatrix J_jpath;
-            tgt_jpath.calcJacobian(J_jpath);
-            for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
-              int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
-              J_part.col(id_in_body) = J_jpath.col(id_in_jpath);
+    protected:
+        hrp::dmatrix J_all, J_all_inv, J_com, J_am;
+        hrp::dvector dq_all, err_all, weight_all;
+        const int WS_DOF, BASE_DOF, J_DOF, ALL_DOF;
+        hrp::Vector3 cur_momentum_P, cur_momentum_L;
+    public:
+        hrp::dvector joint_weight;
+        hrp::dvector q_ref, q_ref_pullback_gain;
+        FullbodyInverseKinematicsSolver (hrp::BodyPtr _robot, const std::string& _print_str, const double _dt)
+        : SimpleFullbodyInverseKinematicsSolver(_robot,_print_str, _dt),
+          WS_DOF(6),
+          BASE_DOF(6),
+          J_DOF(_robot->numJoints()),
+          ALL_DOF(J_DOF+BASE_DOF) {
+            joint_weight = hrp::dvector::Ones(ALL_DOF);
+            dq_all = q_ref = q_ref_pullback_gain = hrp::dvector::Zero(ALL_DOF);
+            cur_momentum_P = cur_momentum_L = hrp::Vector3::Zero();
+        };
+        ~FullbodyInverseKinematicsSolver () {};
+        int solveFullbodyIKLoop (hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list, const int _max_iteration) {
+            hrp::Vector3 base_p_old = _robot->rootLink()->p;
+            hrp::Matrix33 base_R_old = _robot->rootLink()->R;
+            hrp::dvector q_old(J_DOF);
+            for(int i=0;i<J_DOF;i++){ q_old(i) = _robot->joint(i)->q; }
+            unsigned int loop;
+            for(loop=0; loop < _max_iteration;){
+                solveFullbodyIK(_robot, _ikc_list);
+                //check ang moment
+                _robot->rootLink()->v = (_robot->rootLink()->p - base_p_old)/ m_dt;
+                _robot->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * _robot->rootLink()->R) / m_dt;
+                for(int i=0;i<J_DOF;i++){ _robot->joint(i)->dq = (_robot->joint(i)->q - q_old(i)) / m_dt; }
+                _robot->calcForwardKinematics(true,false);
+                _robot->calcTotalMomentum(cur_momentum_P, cur_momentum_L);
+                loop++;
+                if(checkIKConvergence(_robot, _ikc_list)){ break; }
             }
-            J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
-            J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(target_link_ptr->p - _robot->rootLink()->p);
-          }
+            return loop;
         }
-        else if(!target_link_ptr && _ik_tgt_list[i].target_link_name == "COM"){ //重心限定
-          vel_p_ref = ref_link_origin_p - _robot->calcCM();
-//          vel_r_ref = (omegaFromRotEx(ref_link_origin_R) - (cur_L - _robot->rootLink()->p.cross(cur_P))) * m_dt;// COMのrotはAngulerMomentumとして扱う
-          vel_r_ref = _ik_tgt_list[i].targetRpy;// COMのrotはAngulerMomentumとして扱う
-          _robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
-          _robot->calcAngularMomentumJacobian(NULL, J_am);//すでにrootlink周りの角運動量ヤコビアンが返ってくる？
-          J_part << J_com, J_am;
-        }
-        else{ std::cerr<<"Unknown Link Target !!"<<std::endl; continue; } //不明なリンク指定
-        //全体の中に配置
-        if(vel_p_ref.norm()>0.1){vel_p_ref = vel_p_ref.normalized() * 0.1;}
-        if(vel_r_ref.norm()>0.1){vel_r_ref = vel_r_ref.normalized() * 0.1;}
-        dp_ee_all.segment(WS_DOF*i,WS_DOF) << vel_p_ref, vel_r_ref;
-        weight_vec_all.segment(WS_DOF*i, WS_DOF) = _ik_tgt_list[i].weight_vec;
-        J_all.middleRows(WS_DOF*i, WS_DOF) = J_part;
-      }
-
-      hrp::dmatrix selection_matrix = hrp::dmatrix::Zero((weight_vec_all.array()>0.0).count(), WS_DOF*_ik_tgt_list.size());
-      for(int i=0, j=0; i<weight_vec_all.rows(); i++){ if(weight_vec_all(i) > 0.0){ selection_matrix(j, i) = 1.0; j++; } }
-
-//      weight_vec_all /= weight_vec_all.maxCoeff();
-      J_all = selection_matrix * J_all;
-      dp_ee_all = selection_matrix * dp_ee_all;
-      weight_vec_all = selection_matrix * weight_vec_all;
-
-      const double lambda = 0.001;
-      hrp::dmatrix H = J_all.transpose() * weight_vec_all.asDiagonal() * J_all + lambda * hrp::dmatrix::Identity(J_all.cols(), J_all.cols());
-      hrp::dvector g = J_all.transpose() * weight_vec_all.asDiagonal() * dp_ee_all;
-
-      dq_all = H.inverse() * g;
-
-      static int count;
-      if(count++ % 10000 == 0){
-        std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
-        std::cout<<std::setprecision(2) << "J_all_inv=\n"<<J_all_inv<<std::setprecision(6)<<std::endl;
-        dbg(selection_matrix);
-        dbg(H);
-        dbg(g.transpose());
-        dbg(dp_ee_all.transpose());
-        dbg(dq_all.transpose());
-        dbg(weight_vec_all.transpose());
-        std::cout<<"q_ans_all\n";
-        for(int i=0;i<_robot->numJoints();i++)std::cerr<<_robot->joint(i)->q<<" ";
-        std::cout<<std::endl;
-      }
-
-      const double LAMBDA = 1.0;
-      // check validity
-      hrp::Matrix33 base_R_ans = hrp::hat(dq_all.segment(J_DOF+3,3) * LAMBDA).exp() * _robot->rootLink()->R;
-      if(!base_R_ans.isUnitary()){ std::cerr <<"ERROR base_R_ans is not Unitary" << std::endl; return; }
-      for(int i=0;i<dq_all.rows();i++){ if( isnan(dq_all(i)) || isinf(dq_all(i)) ){ std::cerr <<"ERROR nan/inf is found" << std::endl; return;} }
-
-      //関節角+ベース位置姿勢更新
-      for(int i=0;i<J_DOF;i++){
-        _robot->joint(i)->q += dq_all(i) * LAMBDA/* * m_dt*/;
-        LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit);
-      }
-      _robot->rootLink()->p += dq_all.segment(J_DOF,3) * LAMBDA/* * m_dt*/;//vel計算する時からm_dt使ってないから使わなくていい
-      _robot->rootLink()->R = base_R_ans;
-      _robot->calcForwardKinematics();
-    }
-//    void solveFullbodyIK(hrp::BodyPtr& _robot, const std::vector<IKConstraintParam>& _ik_tgt_list){
-//      J_all = hrp::dmatrix::Zero(WS_DOF*_ik_tgt_list.size(), ALL_DOF);
-//      dq_all.fill(0);
-//      dp_ee_all = hrp::dvector::Zero(WS_DOF*_ik_tgt_list.size());
-//      selection_vec_all = hrp::ivector::Zero(WS_DOF*_ik_tgt_list.size());
-//      weight_vec_all = hrp::dvector::Ones(WS_DOF*_ik_tgt_list.size());
-//      //リファレンスに微少量戻す
-//      for(int i=0;i<J_DOF;i++){ _robot->joint(i)->q = _robot->joint(i)->q * (1-reference_gain(i)) + reference_q(i) * reference_gain(i); }
-//      _robot->rootLink()->p = _robot->rootLink()->p.cwiseProduct(hrp::Vector3::Ones()-reference_gain.segment(J_DOF,3)) + reference_q.segment(J_DOF,3).cwiseProduct(reference_gain.segment(J_DOF,3));
-//      _robot->rootLink()->R = hrp::rotFromRpy( hrp::rpyFromRot(_robot->rootLink()->R).cwiseProduct(hrp::Vector3::Ones()-reference_gain.segment(J_DOF+3,3)) + reference_q.segment(J_DOF+3,3).cwiseProduct(reference_gain.segment(J_DOF+3,3)));
-//      for(int i=0;i<J_DOF;i++){ LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit); }
-//      _robot->calcForwardKinematics();
-//      //ヤコビアンと各ベクトル生成
-//      for ( int i=0; i<_ik_tgt_list.size(); i++ ) {
-//        hrp::Link* target_link_ptr = _robot->link(_ik_tgt_list[i].target_link_name);
-//        hrp::Matrix33 ref_link_origin_R(hrp::rotFromRpy(_ik_tgt_list[i].targetRpy) * _ik_tgt_list[i].localR.transpose()); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
-//        hrp::Vector3 ref_link_origin_p(_ik_tgt_list[i].targetPos - ref_link_origin_R * _ik_tgt_list[i].localPos);
-//        hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF); //全身to一拘束点へのヤコビアン
-//        hrp::Vector3 vel_p_ref, vel_r_ref; //velと言いつつ実際は差分
-//        //ベースリンク，通常リンク，重心で場合分け
-//        if(target_link_ptr){//ベースリンク，通常リンク共通
-//          vel_p_ref = ref_link_origin_p - target_link_ptr->p;
-//          vel_r_ref = target_link_ptr->R * omegaFromRotEx(target_link_ptr->R.transpose() * ref_link_origin_R);
-//
-//          if(target_link_ptr == _robot->rootLink()){ //ベース限定
-//            J_part.rightCols(WS_DOF) = hrp::dmatrix::Identity(WS_DOF, WS_DOF);
-//          }
-//          else{ //通常リンク限定
-//            hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), target_link_ptr, m_dt, false, "");
-//            hrp::dmatrix J_jpath;
-//            tgt_jpath.calcJacobian(J_jpath);
-//            for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
-//              int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
-//              J_part.col(id_in_body) = J_jpath.col(id_in_jpath);
-//            }
-//            J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
-//            J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(target_link_ptr->p - _robot->rootLink()->p);
-//          }
-//        }
-//        else if(!target_link_ptr && _ik_tgt_list[i].target_link_name == "COM"){ //重心限定
-//          vel_p_ref = ref_link_origin_p - _robot->calcCM();
-////          vel_r_ref = (omegaFromRotEx(ref_link_origin_R) - (cur_L - _robot->rootLink()->p.cross(cur_P))) * m_dt;// COMのrotはAngulerMomentumとして扱う
-//          vel_r_ref = _ik_tgt_list[i].targetRpy;// COMのrotはAngulerMomentumとして扱う
-//          _robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
-//          _robot->calcAngularMomentumJacobian(NULL, J_am);//すでにrootlink周りの角運動量ヤコビアンが返ってくる？
-//          J_part << J_com, J_am;
-//        }
-//        else{ std::cerr<<"Unknown Link Target !!"<<std::endl; continue; } //不明なリンク指定
-//        //全体の中に配置
-////        if(vel_p_ref.norm()>0.1){vel_p_ref = vel_p_ref.normalized() * 0.1;}
-////        if(vel_r_ref.norm()>0.1){vel_r_ref = vel_r_ref.normalized() * 0.1;}
-//        dp_ee_all.segment(WS_DOF*i,WS_DOF) << vel_p_ref, vel_r_ref;
-//        weight_vec_all.segment(WS_DOF*i, WS_DOF) = _ik_tgt_list[i].weight_vec;
-//        selection_vec_all.segment(WS_DOF*i, WS_DOF) = _ik_tgt_list[i].selection_vec;
-//        J_all.middleRows(WS_DOF*i, WS_DOF) = J_part;
-//      }
-//
-//      hrp::dmatrix selection_matrix = hrp::dmatrix::Zero(selection_vec_all.sum(), WS_DOF*_ik_tgt_list.size());
-//      for(int i=0, j=0; i<selection_vec_all.rows(); i++){ if(selection_vec_all(i) == 1){ selection_matrix(j, i) = 1.0; j++; } }
-//
-////      weight_vec_all /= weight_vec_all.maxCoeff();
-//      J_all = selection_matrix * J_all;
-//      dp_ee_all = selection_matrix * dp_ee_all;
-//      weight_vec_all = selection_matrix * weight_vec_all;
-////      J_all *= selection_matrix;
-////      dp_ee_all *= selection_matrix;
-////      weight_vec_all *= selection_matrix;
-//
-////      hrp::calcPseudoInverse(J_all,J_all_inv,1.0e-3);//超遅い1[ms]とか
-//      calcJacobianInverseNullspace_copy(_robot, J_all, J_all_inv);
-//
-//      dq_all = J_all_inv * dp_ee_all; // dq = pseudoInverse(J) * v
-//      static int count;
-//      if(count++ % 10000 == 0){
-//        std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
-//        std::cout<<std::setprecision(2) << "J_all_inv=\n"<<J_all_inv<<std::setprecision(6)<<std::endl;
-//        std::cout<<std::setprecision(2) << "selection_matrix=\n"<<selection_matrix<<std::setprecision(6)<<std::endl;
-//        std::cout<<"eevel_all\n"<<dp_ee_all.transpose()<<std::endl;
-//        std::cout<<"dq_all\n"<<dq_all.transpose()<<std::endl;
-//        std::cout<<"weight_vec_all\n"<<weight_vec_all.transpose()<<std::endl;
-//        std::cout<<"q_ans_all\n";
-//        for(int i=0;i<_robot->numJoints();i++)std::cerr<<_robot->joint(i)->q<<" ";
-//        std::cout<<std::endl;
-//      }
-//
-//      const double LAMBDA = 1.0;
-//      // check validity
-//      hrp::Matrix33 base_R_ans = hrp::hat(dq_all.segment(J_DOF+3,3) * LAMBDA).exp() * _robot->rootLink()->R;
-//      if(!base_R_ans.isUnitary()){ std::cerr <<"ERROR base_R_ans is not Unitary" << std::endl; return; }
-//      for(int i=0;i<dq_all.rows();i++){ if( isnan(dq_all(i)) || isinf(dq_all(i)) ){ std::cerr <<"ERROR nan/inf is found" << std::endl; return;} }
-//
-//      //関節角+ベース位置姿勢更新
-//      for(int i=0;i<J_DOF;i++){
-//        _robot->joint(i)->q += dq_all(i) * LAMBDA/* * m_dt*/;
-//        LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit);
-//      }
-//      _robot->rootLink()->p += dq_all.segment(J_DOF,3) * LAMBDA/* * m_dt*/;//vel計算する時からm_dt使ってないから使わなくていい
-//      _robot->rootLink()->R = base_R_ans;
-//      _robot->calcForwardKinematics();
-//    }
-
-
-    bool calcJacobianInverseNullspace_copy(hrp::BodyPtr _robot, hrp::dmatrix &_J, hrp::dmatrix &_Jinv) {
-      hrp::dmatrix w = hrp::dmatrix::Identity(ALL_DOF,ALL_DOF);
-      std::vector<double> avoid_weight_gain;
-      avoid_weight_gain.resize(_robot->numJoints());
-      for(int j=0; j < J_DOF; j++){
-        avoid_weight_gain[j] = 100000000000000000000.0;
-      }
-      bool use_inside_joint_weight_retrieval = true;
-
-      for ( int j = 0; j < _robot->numJoints() ; j++ ) {
-        double jang = _robot->joint(j)->q;
-        double jmax = _robot->joint(j)->ulimit;
-        double jmin = _robot->joint(j)->llimit;
-        double e = deg2rad(1);
-        if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
-        } else if ( eps_eq(jang, jmax,e) ) {
-          jang = jmax - e;
-        } else if ( eps_eq(jang, jmin,e) ) {
-          jang = jmin + e;
+        bool checkIKConvergence(const hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list){
+//            if(dq_all.norm() < deg2rad(0.1)){ return true; }
+            for ( int i=0; i<_ikc_list.size(); i++ ) {
+                hrp::Link* link_tgt_ptr = _robot->link(_ikc_list[i].target_link_name);
+                hrp::Vector3 pos_err, rot_err;
+                if(link_tgt_ptr){
+                    pos_err = _ikc_list[i].targetPos - (link_tgt_ptr->p + link_tgt_ptr->R * _ikc_list[i].localPos);
+//                    rot_err = omegaFromRotEx(hrp::rotFromRpy(_ikc_list[i].targetRpy).transpose() * link_tgt_ptr->R * _ikc_list[i].localR);
+                    rats::difference_rotation(rot_err, (link_tgt_ptr->R * _ikc_list[i].localR), hrp::rotFromRpy(_ikc_list[i].targetRpy));
+//                    rot_err = omegaFromRotEx(hrp::rotFromRpy(_ikc_list[i].targetRpy).transpose() * link_tgt_ptr->R * _ikc_list[i].localR.transpose());
+//                    dr_part = link_tgt_ptr->R * omegaFromRotEx(link_tgt_ptr->R.transpose() * R_origin_ref);
+                }
+                else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){  // COM
+                    pos_err = _ikc_list[i].targetPos - (_robot->calcCM() + _ikc_list[i].localR * _ikc_list[i].localPos);
+                    rot_err = _ikc_list[i].targetRpy - (cur_momentum_L - _robot->rootLink()->p.cross(cur_momentum_P));
+                }
+//                dbg(i); dbg(pos_err.transpose()); dbg(rot_err.transpose());
+                for(int j=0;j<3;j++){
+                    if(fabs(pos_err(j)) > _ikc_list[i].pos_precision && _ikc_list[i].constraint_weight.head(3)(j) > 0){return false;}
+                    if(fabs(rot_err(j)) > _ikc_list[i].rot_precision && _ikc_list[i].constraint_weight.tail(3)(j) > 0){return false;}
+                }
+            }
+            return true;
         }
-        double r;
-        if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
-          r = DBL_MAX;
-        } else {
-          r = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) /
-          (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
-          if (isnan(r)) r = 0;
+        void solveFullbodyIK(hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list){
+            J_all = hrp::dmatrix::Zero(WS_DOF*_ikc_list.size(), ALL_DOF);
+            err_all = hrp::dvector::Zero(WS_DOF*_ikc_list.size());
+            weight_all = hrp::dvector::Ones(WS_DOF*_ikc_list.size());
+            //リファレンスに微少量戻す
+            for(int i=0;i<J_DOF;i++){ _robot->joint(i)->q = _robot->joint(i)->q * ( 1 - q_ref_pullback_gain(i)) + q_ref(i) * q_ref_pullback_gain(i); }
+            _robot->rootLink()->p = _robot->rootLink()->p.cwiseProduct( hrp::Vector3::Ones() - q_ref_pullback_gain.segment(J_DOF, 3) ) + q_ref.segment(J_DOF, 3).cwiseProduct( q_ref_pullback_gain.segment(J_DOF, 3) );
+            _robot->rootLink()->R = hrp::rotFromRpy( hrp::rpyFromRot(_robot->rootLink()->R).cwiseProduct(hrp::Vector3::Ones()-q_ref_pullback_gain.segment(J_DOF+3,3)) + q_ref.segment(J_DOF+3,3).cwiseProduct(q_ref_pullback_gain.segment(J_DOF+3,3)));
+            for(int i=0;i<J_DOF;i++){ LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit); }
+            _robot->calcForwardKinematics();
+            //ヤコビアンと各ベクトル生成
+            for ( int i=0; i<_ikc_list.size(); i++ ) {
+                hrp::Link* link_tgt_ptr = _robot->link(_ikc_list[i].target_link_name);
+                const hrp::Matrix33 R_origin_ref = hrp::rotFromRpy(_ikc_list[i].targetRpy) * _ikc_list[i].localR.transpose(); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
+                const hrp::Vector3 p_origin_ref = _ikc_list[i].targetPos - R_origin_ref * _ikc_list[i].localPos;
+                hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF); //全身to一拘束点へのヤコビアン
+                hrp::Vector3 dp_part, dr_part; //差分
+                //ベースリンク，通常リンク，重心で場合分け
+                if(link_tgt_ptr){//ベースリンク，通常リンク共通
+                    dp_part = p_origin_ref - link_tgt_ptr->p;
+                    dr_part = link_tgt_ptr->R * omegaFromRotEx(link_tgt_ptr->R.transpose() * R_origin_ref);
+//                    dr_part = link_tgt_ptr->R * matrix_logEx(link_tgt_ptr->R.transpose() * R_origin_ref);
+
+                    if(link_tgt_ptr == _robot->rootLink()){ //ベース限定
+                        J_part.rightCols(WS_DOF) = hrp::dmatrix::Identity(WS_DOF, WS_DOF);
+                    }
+                    else{ //通常リンク限定
+                        hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, "");
+                        hrp::dmatrix J_jpath;
+                        tgt_jpath.calcJacobian(J_jpath);
+                        for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
+                            int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
+                            J_part.col(id_in_body) = J_jpath.col(id_in_jpath);
+                        }
+                        J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
+                        J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - _robot->rootLink()->p);
+                    }
+                }
+                else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){ //重心限定
+                    dp_part = p_origin_ref - _robot->calcCM();
+                    dr_part = (_ikc_list[i].targetRpy - (cur_momentum_L - _robot->rootLink()->p.cross(cur_momentum_P))) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける
+                    _robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
+                    _robot->calcAngularMomentumJacobian(NULL, J_am);//すでにrootlink周りの角運動量ヤコビアンが返ってくる？
+                    J_part << J_com, J_am;
+                }
+                else{ std::cerr<<"Unknown Link Target !!"<<std::endl; continue; } //不明なリンク指定
+                err_all.segment(WS_DOF*i,WS_DOF) << dp_part, dr_part;
+                weight_all.segment(WS_DOF*i, WS_DOF) = _ikc_list[i].constraint_weight;
+                J_all.middleRows(WS_DOF*i, WS_DOF) = J_part;
+            }
+
+            hrp::dmatrix selection_mat = hrp::dmatrix::Zero((weight_all.array()>0.0).count(), WS_DOF*_ikc_list.size());
+            for(int i=0, j=0; i<weight_all.rows(); i++){ if(weight_all(i) > 0.0){ selection_mat(j, i) = 1.0; j++; } }
+
+            J_all = selection_mat * J_all;
+            err_all = selection_mat * err_all;
+            weight_all = selection_mat * weight_all;
+
+            const double wn_const = 1e-6;
+            hrp::dmatrix Wn = (err_all.transpose() * weight_all.asDiagonal() * err_all + wn_const) * hrp::dmatrix::Identity(ALL_DOF, ALL_DOF);
+            Wn *= 0.01;
+            hrp::dmatrix H = J_all.transpose() * weight_all.asDiagonal() * J_all + Wn;
+            hrp::dvector g = J_all.transpose() * weight_all.asDiagonal() * err_all;
+
+            dq_all = H.inverse() * g;
+
+            static int count;
+            if(count++ % 10000 == 0){
+                std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
+                std::cout<<std::setprecision(2) << "J_all_inv=\n"<<J_all_inv<<std::setprecision(6)<<std::endl;
+                dbgn(selection_mat);
+                dbgn(H);
+                dbg(Wn(0,0));
+                dbg(g.transpose());
+                dbg(err_all.transpose());
+                dbg(dq_all.transpose());
+                dbg(weight_all.transpose());
+                std::cout<<"q_ans_all\n";
+                for(int i=0;i<_robot->numJoints();i++)std::cerr<<_robot->joint(i)->q<<" ";
+                std::cout<<std::endl;
+            }
+
+            const double LAMBDA = 1.0;
+            // check validity
+            hrp::Matrix33 eWx = hrp::hat(dq_all.segment(J_DOF+3,3) * LAMBDA).exp();
+            hrp::Matrix33 R_base_ans = eWx * _robot->rootLink()->R;
+
+            if(!R_base_ans.isUnitary()){ std::cerr <<"ERROR R_base_ans is not Unitary" << std::endl; return; }
+            for(int i=0;i<dq_all.rows();i++){ if( isnan(dq_all(i)) || isinf(dq_all(i)) ){ std::cerr <<"ERROR nan/inf is found" << std::endl; return;} }
+
+            //関節角+ベース位置姿勢更新
+            for(int i=0;i<J_DOF;i++){
+                _robot->joint(i)->q += dq_all(i) * LAMBDA/* * m_dt*/;
+                LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit);
+            }
+            _robot->rootLink()->p += dq_all.segment(J_DOF,3) * LAMBDA;//vel計算する時からm_dt使ってないから使わなくていい
+
+            Eigen::Quaternion<double> quat(R_base_ans);
+            quat.normalize();
+            R_base_ans = quat.toRotationMatrix();
+
+            _robot->rootLink()->R = R_base_ans;
+            _robot->calcForwardKinematics();
         }
-        if (( r - avoid_weight_gain[j] ) >= 0 ) {
-          w(j, j) = optional_weight_vector[j] * ( 1.0 / ( 1.0 + r) );
-        } else {
-          if (use_inside_joint_weight_retrieval)
-          w(j, j) = optional_weight_vector[j] * 1.0;
-          else
-          w(j, j) = optional_weight_vector[j] * ( 1.0 / ( 1.0 + r) );
+
+    protected:
+        bool calcJacobianInverseNullspace_copy(hrp::BodyPtr _robot, hrp::dmatrix &_J, hrp::dmatrix &_Jinv) {
+            hrp::dmatrix w = hrp::dmatrix::Identity(ALL_DOF,ALL_DOF);
+            std::vector<double> avoid_weight_gain;
+            avoid_weight_gain.resize(_robot->numJoints());
+            for(int j=0; j < J_DOF; j++){
+                avoid_weight_gain[j] = 100000000000000000000.0;
+            }
+            bool use_inside_joint_weight_retrieval = true;
+
+            for ( int j = 0; j < _robot->numJoints() ; j++ ) {
+                double jang = _robot->joint(j)->q;
+                double jmax = _robot->joint(j)->ulimit;
+                double jmin = _robot->joint(j)->llimit;
+                double e = deg2rad(1);
+                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+                } else if ( eps_eq(jang, jmax,e) ) {
+                    jang = jmax - e;
+                } else if ( eps_eq(jang, jmin,e) ) {
+                    jang = jmin + e;
+                }
+                double r;
+                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+                    r = DBL_MAX;
+                } else {
+                    r = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) /
+                            (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
+                    if (isnan(r)) r = 0;
+                }
+                if (( r - avoid_weight_gain[j] ) >= 0 ) {
+                    w(j, j) = joint_weight[j] * ( 1.0 / ( 1.0 + r) );
+                } else {
+                    if (use_inside_joint_weight_retrieval)
+                        w(j, j) = joint_weight[j] * 1.0;
+                    else
+                        w(j, j) = joint_weight[j] * ( 1.0 / ( 1.0 + r) );
+                }
+                avoid_weight_gain[j] = r;
+            }
+            for ( int j = _robot->numJoints(); j < ALL_DOF ; j++ ) {
+                w(j, j) = joint_weight[j];
+            }
+
+            const double manipulability_limit = 0.0001;
+            const double sr_gain = 1.0;
+            //      const double manipulability_gain = 0.001;
+            const double manipulability_gain = 1;
+
+            double manipulability = sqrt((_J*_J.transpose()).determinant());
+            double k = 0;
+            if ( manipulability < manipulability_limit ) {
+                k = manipulability_gain * pow((1 - ( manipulability / manipulability_limit )), 2);
+            }
+            hrp::calcSRInverse(_J, _Jinv, sr_gain * k, w);
+            return true;
         }
-        avoid_weight_gain[j] = r;
-      }
-      for ( int j = _robot->numJoints(); j < ALL_DOF ; j++ ) {
-        w(j, j) = optional_weight_vector[j];
-      }
-
-      const double manipulability_limit = 0.0001;
-      const double sr_gain = 1.0;
-//      const double manipulability_gain = 0.001;
-      const double manipulability_gain = 1;
-
-      double manipulability = sqrt((_J*_J.transpose()).determinant());
-//      static int count;
-//      if(count++ % 10000 == 0){
-//        std::cout<<"manipulability "<<manipulability<<std::endl;
-//      }
-      double k = 0;
-      if ( manipulability < manipulability_limit ) {
-        k = manipulability_gain * pow((1 - ( manipulability / manipulability_limit )), 2);
-      }
-//      hrp::calcSRInverse(_J, _Jinv, sr_gain * k, w);
-      calcSRInverse_new(_J, _Jinv, sr_gain * k, weight_vec_all, w);
-      //        Jnull = ( hrp::dmatrix::Identity(n, n) - _Jinv * _J);
-      return true;
-    }
-
-    hrp::Vector3 omegaFromRotEx(const hrp::Matrix33& r) {//copy from JointPathEx.cpp
-        using ::std::numeric_limits;
-        double alpha = (r(0,0) + r(1,1) + r(2,2) - 1.0) / 2.0;
-        if(fabs(alpha - 1.0) < 1.0e-12) {   //th=0,2PI;
-            return hrp::Vector3::Zero();
-        } else {
-            double th = acos(alpha);
-            double s = sin(th);
-            if (s < numeric_limits<double>::epsilon()) {   //th=PI
-                return hrp::Vector3( sqrt((r(0,0)+1)*0.5)*th, sqrt((r(1,1)+1)*0.5)*th, sqrt((r(2,2)+1)*0.5)*th );
+
+        hrp::Vector3 omegaFromRotEx(const hrp::Matrix33& r) {//copy from JointPathEx.cpp
+            using ::std::numeric_limits;
+            double alpha = (r(0,0) + r(1,1) + r(2,2) - 1.0) / 2.0;
+            if(fabs(alpha - 1.0) < 1.0e-12) {   //th=0,2PI;
+                return hrp::Vector3::Zero();
+            } else {
+                double th = acos(alpha);
+                double s = sin(th);
+                if (s < numeric_limits<double>::epsilon()) {   //th=PI
+                    return hrp::Vector3( sqrt((r(0,0)+1)*0.5)*th, sqrt((r(1,1)+1)*0.5)*th, sqrt((r(2,2)+1)*0.5)*th );
+                }
+                double k = -0.5 * th / s;
+                return hrp::Vector3( (r(1,2) - r(2,1)) * k, (r(2,0) - r(0,2)) * k, (r(0,1) - r(1,0)) * k );
             }
-            double k = -0.5 * th / s;
-            return hrp::Vector3( (r(1,2) - r(2,1)) * k, (r(2,0) - r(0,2)) * k, (r(0,1) - r(1,0)) * k );
         }
-    }
+        hrp::Vector3 matrix_logEx(const hrp::Matrix33& m) {
+            hrp::Vector3 mlog;
+            double q0, th;
+            hrp::Vector3 q;
+            double norm;
 
-    void calcSRInverse_new(const hrp::dmatrix& _a, hrp::dmatrix &_a_sr, double _sr_ratio, hrp::dvector& _ee_w, hrp::dmatrix _w) {
-        const int c = _a.rows(); // 6*x
-        const int n = _a.cols(); // n = all joint dof
-        if ( _w.cols() != n || _w.rows() != n ) {
-            _w = hrp::dmatrix::Identity(n, n);
+            Eigen::Quaternion<double> eiq(m);
+            q0 = eiq.w();
+            q = eiq.vec();
+            norm = q.norm();
+            if (norm > 0) {
+                if ((q0 > 1.0e-10) || (q0 < -1.0e-10)) {
+                    th = 2 * std::atan(norm / q0);
+                } else if (q0 > 0) {
+                    th = M_PI / 2;
+                } else {
+                    th = -M_PI / 2;
+                }
+                mlog = (th / norm) * q ;
+            } else {
+                mlog = hrp::Vector3::Zero();
+            }
+            return mlog;
         }
-        hrp::dmatrix at = _a.transpose();
-        hrp::dmatrix a1(c, c);
-        hrp::dmatrix eew_mat = _ee_w.cwiseInverse().asDiagonal();
-        a1 = (_a * _w * at +  _sr_ratio * eew_mat).inverse();
-//        a1 = (_a * _w * at +  _sr_ratio *  hrp::dmatrix::Identity(c,c)).inverse();
-        _a_sr  = _w * at * a1;
-    }
 };
 
 
 class FullbodyInverseKinematicsSolverMT : public FullbodyInverseKinematicsSolver{
-  protected:
-    hrp::BodyPtr m_robot_copy;
-    pthread_t ik_thread;
-    pthread_mutex_t ik_body_mutex;  // Mutex
-    int cur_ik_loop;
-    std::vector<IKConstraintParam> ik_tgt_list;
-    bool is_first;
-    bool ik_thread_kill;
-    bool ik_thread_ik_required;
-    hrp::Vector3 base_p_old;
-    hrp::Matrix33 base_R_old;
-    hrp::dvector q_old;
-  public:
-    FullbodyInverseKinematicsSolverMT (hrp::BodyPtr& _robot, const std::string& _print_str, const double _dt) : FullbodyInverseKinematicsSolver(_robot,_print_str, _dt) {
-      m_robot_copy = hrp::BodyPtr(new hrp::Body(*_robot));
-      cur_ik_loop = 0;
-      is_first = true;
-      ik_thread_kill = false;
-      ik_thread_ik_required = false;
-    };
-    ~FullbodyInverseKinematicsSolverMT () {
-      pthread_cancel(ik_thread);
-      ik_thread_kill = true;
-      pthread_join(ik_thread, NULL );
-      std::cerr<<"[FullbodyInverseKinematicsSolverMT] thread killed successfully"<<std::endl;
-    };
-    int solveFullbodyIKLoopMT (const std::vector<IKConstraintParam>& _ik_tgt_list, const int _max_iteration) {
-      if(is_first){
-        for(int i=0;i<J_DOF;i++){
-          m_robot_copy->joint(i)->q = m_robot->joint(i)->q;
-          m_robot_copy->joint(i)->ulimit = m_robot->joint(i)->ulimit;
-          m_robot_copy->joint(i)->llimit = m_robot->joint(i)->llimit;
+    protected:
+        hrp::BodyPtr _robot_copy;
+        pthread_t ik_thread;
+        pthread_mutex_t ik_body_mutex;  // Mutex
+        int cur_ik_loop;
+        std::vector<IKConstraint> ikc_list;
+        bool is_first;
+        bool ik_thread_kill;
+        bool ik_thread_ik_required;
+        hrp::Vector3 base_p_old;
+        hrp::Matrix33 base_R_old;
+        hrp::dvector q_old;
+    public:
+        FullbodyInverseKinematicsSolverMT (hrp::BodyPtr& _robot, const std::string& _print_str, const double _dt) : FullbodyInverseKinematicsSolver(_robot,_print_str, _dt) {
+            _robot_copy = hrp::BodyPtr(new hrp::Body(*_robot));
+            cur_ik_loop = 0;
+            is_first = true;
+            ik_thread_kill = false;
+            ik_thread_ik_required = false;
+        };
+        ~FullbodyInverseKinematicsSolverMT () {
+            pthread_cancel(ik_thread);
+            ik_thread_kill = true;
+            pthread_join(ik_thread, NULL );
+            std::cerr<<"[FullbodyInverseKinematicsSolverMT] thread killed successfully"<<std::endl;
+        };
+        int solveFullbodyIKLoopMT (hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list, const int _max_iteration) {
+            if(is_first){
+                for(int i=0;i<J_DOF;i++){
+                    _robot_copy->joint(i)->q = _robot->joint(i)->q;
+                    _robot_copy->joint(i)->ulimit = _robot->joint(i)->ulimit;
+                    _robot_copy->joint(i)->llimit = _robot->joint(i)->llimit;
+                }
+                _robot_copy->rootLink()->p = _robot->rootLink()->p;
+                _robot_copy->rootLink()->R = _robot->rootLink()->R;
+                pthread_mutex_init( &ik_body_mutex, NULL );
+                pthread_create(&ik_thread, NULL, launchThread, this);
+                is_first = false;
+            }
+            for(int i=0;i<J_DOF;i++){
+                _robot->joint(i)->q = _robot_copy->joint(i)->q;
+            }
+            _robot->rootLink()->p = _robot_copy->rootLink()->p;
+            _robot->rootLink()->R = _robot_copy->rootLink()->R;
+            _robot->calcForwardKinematics();
+            int result = cur_ik_loop;
+
+            if(pthread_mutex_lock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_lock err "<<std::endl;}
+
+            base_p_old = _robot_copy->rootLink()->p;
+            base_R_old = _robot_copy->rootLink()->R;
+            q_old.resize(J_DOF);
+            for(int i=0;i<J_DOF;i++){ q_old(i) = _robot_copy->joint(i)->q; }
+
+            cur_ik_loop = 0;
+            ikc_list = _ikc_list;
+            ik_thread_ik_required = true;
+            if(pthread_mutex_unlock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_unlock err "<<std::endl;}
+            return result;
+        }
+        void ik_loop(){
+            std::cerr<<"[FullbodyInverseKinematicsSolverMT] start thread"<<std::endl;
+            while(1){
+                usleep(0);//これ無いとmutex解放しない？
+                if(pthread_mutex_lock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_lock2 err "<<std::endl;}
+                if(ik_thread_ik_required){
+                    solveFullbodyIK(_robot_copy, ikc_list);
+                    //check ang moment
+                    _robot_copy->rootLink()->v = (_robot_copy->rootLink()->p - base_p_old)/ m_dt;
+                    _robot_copy->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * _robot_copy->rootLink()->R) / m_dt;
+                    for(int i=0;i<J_DOF;i++){ _robot_copy->joint(i)->dq = (_robot_copy->joint(i)->q - q_old(i)) / m_dt; }
+                    _robot_copy->calcForwardKinematics(true,false);
+                    _robot_copy->calcTotalMomentum(cur_momentum_P, cur_momentum_L);
+                    cur_ik_loop++;
+                    if(cur_ik_loop > 10000){std::cerr<<"[FullbodyInverseKinematicsSolverMT] thread time out"<<std::endl;break;}//異常終了でスレッドが残ってしまった時にタイムアウト終了
+                }
+                if(pthread_mutex_unlock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_unlock2 err "<<std::endl;}
+                if(ik_thread_kill){break;}
+            }
+            std::cerr<<"[FullbodyInverseKinematicsSolverMT] end of thread"<<std::endl;
+        }
+        static void* launchThread(void *pParam) {
+            reinterpret_cast<FullbodyInverseKinematicsSolverMT*>(pParam)->ik_loop();
+            return NULL;
         }
-        m_robot_copy->rootLink()->p = m_robot->rootLink()->p;
-        m_robot_copy->rootLink()->R = m_robot->rootLink()->R;
-        pthread_mutex_init( &ik_body_mutex, NULL );
-        pthread_create(&ik_thread, NULL, launchThread, this);
-        is_first = false;
-      }
-      for(int i=0;i<J_DOF;i++){
-        m_robot->joint(i)->q = m_robot_copy->joint(i)->q;
-      }
-      m_robot->rootLink()->p = m_robot_copy->rootLink()->p;
-      m_robot->rootLink()->R = m_robot_copy->rootLink()->R;
-      m_robot->calcForwardKinematics();
-      int result = cur_ik_loop;
-
-      if(pthread_mutex_lock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_lock err "<<std::endl;}
-
-        base_p_old = m_robot_copy->rootLink()->p;
-        base_R_old = m_robot_copy->rootLink()->R;
-        q_old.resize(J_DOF);
-        for(int i=0;i<J_DOF;i++){ q_old(i) = m_robot_copy->joint(i)->q; }
-
-        cur_ik_loop = 0;
-        ik_tgt_list = _ik_tgt_list;// "="で代入するとnanが入る！？->そんなことはない
-        ik_thread_ik_required = true;
-      if(pthread_mutex_unlock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_unlock err "<<std::endl;}
-//      if(is_first){
-//        pthread_create(&ik_thread, NULL, launchThread, this);
-//        is_first = false;
-//      }
-      return result;
-    }
-    void ik_loop(){
-      std::cerr<<"[FullbodyInverseKinematicsSolverMT] start thread"<<std::endl;
-      while(1){
-        usleep(0);//これ無いとmutex解放しない？
-        if(pthread_mutex_lock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_lock2 err "<<std::endl;}
-          if(ik_thread_ik_required){
-            solveFullbodyIK(m_robot_copy, ik_tgt_list);
-            //check ang moment
-            m_robot_copy->rootLink()->v = (m_robot_copy->rootLink()->p - base_p_old)/ m_dt;
-            m_robot_copy->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * m_robot_copy->rootLink()->R) / m_dt;
-            for(int i=0;i<J_DOF;i++){ m_robot_copy->joint(i)->dq = (m_robot_copy->joint(i)->q - q_old(i)) / m_dt; }
-            m_robot_copy->calcForwardKinematics(true,false);
-            m_robot_copy->calcTotalMomentum(cur_P, cur_L);
-            cur_ik_loop++;
-            if(cur_ik_loop > 10000){std::cerr<<"[FullbodyInverseKinematicsSolverMT] thread time out"<<std::endl;break;}//異常終了でスレッドが残ってしまった時にタイムアウト終了
-          }
-        if(pthread_mutex_unlock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_unlock2 err "<<std::endl;}
-        if(ik_thread_kill){break;}
-      }
-      std::cerr<<"[FullbodyInverseKinematicsSolverMT] end of thread"<<std::endl;
-    }
-    static void* launchThread(void *pParam) {
-      reinterpret_cast<FullbodyInverseKinematicsSolverMT*>(pParam)->ik_loop();
-      return NULL;
-    }
 };
 
 
diff --git a/rtc/ImpedanceController/JointPathEx.cpp b/rtc/ImpedanceController/JointPathEx.cpp
index aa180fcbf62..f47b588db8d 100644
--- a/rtc/ImpedanceController/JointPathEx.cpp
+++ b/rtc/ImpedanceController/JointPathEx.cpp
@@ -101,6 +101,21 @@ JointPathEx::JointPathEx(BodyPtr& robot, Link* base, Link* end, double control_c
   }
 }
 
+JointPathEx::JointPathEx(BodyPtr& robot, Link* end, double control_cycle, bool _use_inside_joint_weight_retrieval, const std::string& _debug_print_prefix)
+    : JointPath( end), maxIKPosErrorSqr(1.0e-8), maxIKRotErrorSqr(1.0e-6), maxIKIteration(50), interlocking_joint_pair_indices(), sr_gain(1.0), manipulability_limit(0.1), manipulability_gain(0.001), dt(control_cycle),
+      debug_print_prefix(_debug_print_prefix+",JointPathEx"), joint_limit_debug_print_counts(numJoints(), 0),
+      debug_print_freq_count(static_cast<size_t>(0.25/dt)), // once per 0.25[s]
+      use_inside_joint_weight_retrieval(_use_inside_joint_weight_retrieval) {
+  for (unsigned int i = 0 ; i < numJoints(); i++ ) {
+    joints.push_back(joint(i));
+  }
+  avoid_weight_gain.resize(numJoints());
+  optional_weight_vector.resize(numJoints());
+  for (unsigned int i = 0 ; i < numJoints(); i++ ) {
+      optional_weight_vector[i] = 1.0;
+  }
+}
+
 void JointPathEx::setMaxIKError(double epos, double erot) {
   maxIKPosErrorSqr = epos*epos;
   maxIKRotErrorSqr = erot*erot;
diff --git a/rtc/ImpedanceController/JointPathEx.h b/rtc/ImpedanceController/JointPathEx.h
index f25a5c762e4..b360a0d2130 100644
--- a/rtc/ImpedanceController/JointPathEx.h
+++ b/rtc/ImpedanceController/JointPathEx.h
@@ -16,6 +16,7 @@ namespace hrp {
     class JointPathEx : public JointPath {
   public:
     JointPathEx(BodyPtr& robot, Link* base, Link* end, double control_cycle, bool _use_inside_joint_weight_retrieval = true, const std::string& _debug_print_prefix = "");
+    JointPathEx(BodyPtr& robot, Link* end, double control_cycle, bool _use_inside_joint_weight_retrieval = true, const std::string& _debug_print_prefix = "");
     bool calcJacobianInverseNullspace(dmatrix &J, dmatrix &Jinv, dmatrix &Jnull);
     bool calcInverseKinematics2Loop(const Vector3& dp, const Vector3& omega, const double LAMBDA, const double avoid_gain = 0.0, const double reference_gain = 0.0, const dvector* reference_q = NULL);
     bool calcInverseKinematics2Loop(const Vector3& end_effector_p, const Matrix33& end_effector_R,
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 3b641a50e6a..051f68c8c70 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -11,7 +11,7 @@
 #define DEBUGP_ONCE (loop==0)
 
 static const char* WholeBodyMasterSlave_spec[] =
-    {
+{
         "implementation_id", "WholeBodyMasterSlave",
         "type_name",         "WholeBodyMasterSlave",
         "description",       "wholebodymasterslave component",
@@ -24,7 +24,7 @@ static const char* WholeBodyMasterSlave_spec[] =
         "lang_type",         "compile",
         "conf.default.debugLevel", "0",
         ""
-    };
+};
 
 //static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
 //{
@@ -38,50 +38,50 @@ static const char* WholeBodyMasterSlave_spec[] =
 //}
 
 WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager),
-      m_qRefIn("qRef", m_qRef),// from sh
-      m_zmpIn("zmpIn", m_zmp),
-      m_basePosIn("basePosIn", m_basePos),
-      m_baseRpyIn("baseRpyIn", m_baseRpy),
-      m_optionalDataIn("optionalData", m_optionalData),
-      m_qOut("q", m_qRef),// to abc
-      m_zmpOut("zmpOut", m_zmp),
-      m_basePosOut("basePosOut", m_basePos),
-      m_baseRpyOut("baseRpyOut", m_baseRpy),
-      m_optionalDataOut("optionalDataOut", m_optionalData),
-      m_htzmpIn("htzmpIn", m_htzmp),// from ros bridge
-      m_htrfwIn("htrfwIn", m_htrfw),
-      m_htlfwIn("htlfwIn", m_htlfw),
-      m_htcomIn("htcomIn", m_htcom),
-      m_htrfIn("htrfIn", m_htrf),
-      m_htlfIn("htlfIn", m_htlf),
-      m_htrhIn("htrhIn", m_htrh),
-      m_htlhIn("htlhIn", m_htlh),
-//      m_actzmpIn("actzmpIn", m_actzmp),
-      m_htheadIn("htheadIn", m_hthead),
+        m_qRefIn("qRef", m_qRef),// from sh
+        m_zmpIn("zmpIn", m_zmp),
+        m_basePosIn("basePosIn", m_basePos),
+        m_baseRpyIn("baseRpyIn", m_baseRpy),
+        m_optionalDataIn("optionalData", m_optionalData),
+        m_qOut("q", m_qRef),// to abc
+        m_zmpOut("zmpOut", m_zmp),
+        m_basePosOut("basePosOut", m_basePos),
+        m_baseRpyOut("baseRpyOut", m_baseRpy),
+        m_optionalDataOut("optionalDataOut", m_optionalData),
+        m_htzmpIn("htzmpIn", m_htzmp),// from ros bridge
+        m_htrfwIn("htrfwIn", m_htrfw),
+        m_htlfwIn("htlfwIn", m_htlfw),
+        m_htcomIn("htcomIn", m_htcom),
+        m_htrfIn("htrfIn", m_htrf),
+        m_htlfIn("htlfIn", m_htlf),
+        m_htrhIn("htrhIn", m_htrh),
+        m_htlhIn("htlhIn", m_htlh),
+        //      m_actzmpIn("actzmpIn", m_actzmp),
+        m_htheadIn("htheadIn", m_hthead),
 #ifdef USE_DEBUG_PORT
-       m_htcom_dbgOut("htcom_dbgOut", m_htcom_dbg),// to ros bridge
-       m_htrf_dbgOut("htrf_dbgOut", m_htrf_dbg),
-       m_htlf_dbgOut("htlf_dbgOut", m_htlf_dbg),
-       m_htrh_dbgOut("htrh_dbgOut", m_htrh_dbg),
-       m_htlh_dbgOut("htlh_dbgOut", m_htlh_dbg),
-       m_hthead_dbgOut("hthead_dbgOut", m_hthead_dbg),
-       m_htzmp_dbgOut("htzmp_dbgOut", m_htzmp_dbg),
-       m_htrfw_dbgOut("htrfw_dbgOut", m_htrfw_dbg),
-       m_htlfw_dbgOut("htlfw_dbgOut", m_htlfw_dbg),
-       m_rpcom_dbgOut("rpcom_dbgOut", m_rpcom_dbg),
-       m_rprf_dbgOut("rprf_dbgOut", m_rprf_dbg),
-       m_rplf_dbgOut("rplf_dbgOut", m_rplf_dbg),
-       m_rprh_dbgOut("rprh_dbgOut", m_rprh_dbg),
-       m_rplh_dbgOut("rplh_dbgOut", m_rplh_dbg),
-       m_rphead_dbgOut("rphead_dbgOut", m_rphead_dbg),
-       m_rpzmp_dbgOut("rpzmp_dbgOut", m_rpzmp_dbg),
-       m_rpdcp_dbgOut("rpdcp_dbgOut", m_rpdcp_dbg),
-       m_rpacp_dbgOut("rpacp_dbgOut", m_rpacp_dbg),
-       m_invdyn_dbgOut("invdyn_dbgOut", m_invdyn_dbg),
+        m_htcom_dbgOut("htcom_dbgOut", m_htcom_dbg),// to ros bridge
+        m_htrf_dbgOut("htrf_dbgOut", m_htrf_dbg),
+        m_htlf_dbgOut("htlf_dbgOut", m_htlf_dbg),
+        m_htrh_dbgOut("htrh_dbgOut", m_htrh_dbg),
+        m_htlh_dbgOut("htlh_dbgOut", m_htlh_dbg),
+        m_hthead_dbgOut("hthead_dbgOut", m_hthead_dbg),
+        m_htzmp_dbgOut("htzmp_dbgOut", m_htzmp_dbg),
+        m_htrfw_dbgOut("htrfw_dbgOut", m_htrfw_dbg),
+        m_htlfw_dbgOut("htlfw_dbgOut", m_htlfw_dbg),
+        m_rpcom_dbgOut("rpcom_dbgOut", m_rpcom_dbg),
+        m_rprf_dbgOut("rprf_dbgOut", m_rprf_dbg),
+        m_rplf_dbgOut("rplf_dbgOut", m_rplf_dbg),
+        m_rprh_dbgOut("rprh_dbgOut", m_rprh_dbg),
+        m_rplh_dbgOut("rplh_dbgOut", m_rplh_dbg),
+        m_rphead_dbgOut("rphead_dbgOut", m_rphead_dbg),
+        m_rpzmp_dbgOut("rpzmp_dbgOut", m_rpzmp_dbg),
+        m_rpdcp_dbgOut("rpdcp_dbgOut", m_rpdcp_dbg),
+        m_rpacp_dbgOut("rpacp_dbgOut", m_rpacp_dbg),
+        m_invdyn_dbgOut("invdyn_dbgOut", m_invdyn_dbg),
 #endif
-      m_WholeBodyMasterSlaveServicePort("WholeBodyMasterSlaveService"),
-      m_robot(hrp::BodyPtr()),
-      m_debugLevel(0)
+        m_WholeBodyMasterSlaveServicePort("WholeBodyMasterSlaveService"),
+        m_robot(hrp::BodyPtr()),
+        m_debugLevel(0)
 {
     m_service0.wholebodymasterslave(this);
 }
@@ -109,32 +109,32 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     addInPort("htlfIn", m_htlfIn);
     addInPort("htrhIn", m_htrhIn);
     addInPort("htlhIn", m_htlhIn);
-//    addInPort("actzmpIn", m_actzmpIn);
+    //    addInPort("actzmpIn", m_actzmpIn);
     addInPort("htheadIn", m_htheadIn);
 #ifdef USE_DEBUG_PORT
-     addOutPort("htcom_dbgOut", m_htcom_dbgOut);// to ros bridge
-     addOutPort("htrf_dbgOut", m_htrf_dbgOut);
-     addOutPort("htlf_dbgOut", m_htlf_dbgOut);
-     addOutPort("htrh_dbgOut", m_htrh_dbgOut);
-     addOutPort("htlh_dbgOut", m_htlh_dbgOut);
-     addOutPort("hthead_dbgOut", m_hthead_dbgOut);
-     addOutPort("htzmp_dbgOut", m_htzmp_dbgOut);
-     addOutPort("htrfw_dbgOut", m_htrfw_dbgOut);
-     addOutPort("htlfw_dbgOut", m_htlfw_dbgOut);
-     addOutPort("rpcom_dbgOut", m_rpcom_dbgOut);
-     addOutPort("rprf_dbgOut", m_rprf_dbgOut);
-     addOutPort("rplf_dbgOut", m_rplf_dbgOut);
-     addOutPort("rprh_dbgOut", m_rprh_dbgOut);
-     addOutPort("rplh_dbgOut", m_rplh_dbgOut);
-     addOutPort("rphead_dbgOut", m_rphead_dbgOut);
-     addOutPort("rpzmp_dbgOut", m_rpzmp_dbgOut);
-     addOutPort("rpdcp_dbgOut", m_rpdcp_dbgOut);
-     addOutPort("rpacp_dbgOut", m_rpacp_dbgOut);
-     addOutPort("invdyn_dbgOut", m_invdyn_dbgOut);
+    addOutPort("htcom_dbgOut", m_htcom_dbgOut);// to ros bridge
+    addOutPort("htrf_dbgOut", m_htrf_dbgOut);
+    addOutPort("htlf_dbgOut", m_htlf_dbgOut);
+    addOutPort("htrh_dbgOut", m_htrh_dbgOut);
+    addOutPort("htlh_dbgOut", m_htlh_dbgOut);
+    addOutPort("hthead_dbgOut", m_hthead_dbgOut);
+    addOutPort("htzmp_dbgOut", m_htzmp_dbgOut);
+    addOutPort("htrfw_dbgOut", m_htrfw_dbgOut);
+    addOutPort("htlfw_dbgOut", m_htlfw_dbgOut);
+    addOutPort("rpcom_dbgOut", m_rpcom_dbgOut);
+    addOutPort("rprf_dbgOut", m_rprf_dbgOut);
+    addOutPort("rplf_dbgOut", m_rplf_dbgOut);
+    addOutPort("rprh_dbgOut", m_rprh_dbgOut);
+    addOutPort("rplh_dbgOut", m_rplh_dbgOut);
+    addOutPort("rphead_dbgOut", m_rphead_dbgOut);
+    addOutPort("rpzmp_dbgOut", m_rpzmp_dbgOut);
+    addOutPort("rpdcp_dbgOut", m_rpdcp_dbgOut);
+    addOutPort("rpacp_dbgOut", m_rpacp_dbgOut);
+    addOutPort("invdyn_dbgOut", m_invdyn_dbgOut);
 #endif
     m_WholeBodyMasterSlaveServicePort.registerProvider("service0", "WholeBodyMasterSlaveService", m_service0);
     addPort(m_WholeBodyMasterSlaveServicePort);
-  
+
     RTC::Properties& prop =  getProperties();
     coil::stringTo(m_dt, prop["dt"].c_str());
     RTC::Manager& rtcManager = RTC::Manager::instance();
@@ -147,8 +147,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     m_robot_ml = hrp::BodyPtr(new hrp::Body());
     m_robot_vsafe = hrp::BodyPtr(new hrp::Body());
     if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
-      std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
-      return RTC::RTC_ERROR;
+        std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
+        return RTC::RTC_ERROR;
     }
     m_robot_ml = hrp::BodyPtr(new hrp::Body(*m_robot)); //copy
     m_robot_vsafe = hrp::BodyPtr(new hrp::Body(*m_robot)); //copy
@@ -167,8 +167,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     tmp_ratio = 1.0;
     transition_interpolator->setGoal(&tmp_ratio, 3.0, true);
     ROBOT_ALL_DOF = m_robot->numJoints() + 3 + 3;// joints + base_p + base_rpy
-//    q_ip = new interpolator(ROBOT_ALL_DOF, m_dt, interpolator::HOFFARBIB, 1);
-//    q_ip = new interpolator(ROBOT_ALL_DOF, m_dt, interpolator::QUINTICSPLINE, 1);
+    //    q_ip = new interpolator(ROBOT_ALL_DOF, m_dt, interpolator::HOFFARBIB, 1);
+    //    q_ip = new interpolator(ROBOT_ALL_DOF, m_dt, interpolator::QUINTICSPLINE, 1);
     q_ip = new interpolator(ROBOT_ALL_DOF, m_dt, interpolator::CUBICSPLINE, 1);
     q_ip->setName(std::string(m_profile.instance_name)+" q_ip");
     q_ip->clear();
@@ -185,9 +185,9 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     setupfik(fik_ml, m_robot_ml, prop);
 
     if (fik->ikp.find("rleg") != fik->ikp.end() && fik->ikp.find("lleg") != fik->ikp.end()) {
-      is_legged_robot = true;
+        is_legged_robot = true;
     } else {
-      is_legged_robot = false;
+        is_legged_robot = false;
     }
     hsp = boost::shared_ptr<WBMSCore>(new WBMSCore(m_dt));
 
@@ -199,68 +199,68 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
 }
 
 RTC::ReturnCode_t WholeBodyMasterSlave::onFinalize(){
-  return RTC::RTC_OK;
+    return RTC::RTC_OK;
 }
 
 void WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in){
-  coil::vstring end_effectors_str = coil::split(prop_in["end_effectors"], ",");
-  size_t prop_num = 10;
-  if (end_effectors_str.size() > 0) {
-    size_t num = end_effectors_str.size()/prop_num;
-    for (size_t i = 0; i < num; i++) {
-      std::string ee_name, ee_target, ee_base;
-      coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
-      coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
-      coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
-      ABCIKparam tp;
-      for (size_t j = 0; j < XYZ; j++) {
-          coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
-      }
-      double tmpv[4];
-      for (int j = 0; j < 4; j++ ) {
-        coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
-      }
-      tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
-      // FIK param
-      SimpleFullbodyInverseKinematicsSolver::IKparam tmp_fikp;
-      tmp_fikp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(robot_in, robot_in->link(ee_base), robot_in->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
-      tmp_fikp.target_link = robot_in->link(ee_target);
-      tmp_fikp.localPos = tp.localPos;
-      tmp_fikp.localR = tp.localR;
-      fik_in->ikp.insert(std::pair<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>(ee_name, tmp_fikp));
-      // Fix for toe joint
-      //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
-      //   Without toe joints, "end-effector link == force-sensor link" is assumed.
-      //   With toe joints, "end-effector link != force-sensor link" is assumed.
-      if (robot_in->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
-          std::vector<double> optw(fik_in->ikp[ee_name].manip->numJoints(), 1.0);
-          optw.back() = 0.0; // Set weight = 0 for toe joint by default
-          fik_in->ikp[ee_name].manip->setOptionalWeightVector(optw);
-          tp.has_toe_joint = true;
-      } else {
-          tp.has_toe_joint = false;
-      }
-      tp.target_link = robot_in->link(ee_target);
-      std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   target = " << fik_in->ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-      std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
-      contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));////TODO:要移動?
-
-      eename_ikcp_map[ee_name].target_link_name = ee_target;
-      for (size_t j = 0; j < XYZ; j++){ coil::stringTo(eename_ikcp_map[ee_name].localPos(j), end_effectors_str[i*prop_num+3+j].c_str()); }
-      double tmp[4];
-      for (int j = 0; j < 4; j++ ){ coil::stringTo(tmp[j], end_effectors_str[i*prop_num+6+j].c_str()); }
-      eename_ikcp_map[ee_name].localR = Eigen::AngleAxis<double>(tmp[3], hrp::Vector3(tmp[0], tmp[1], tmp[2])).toRotationMatrix();
+    coil::vstring end_effectors_str = coil::split(prop_in["end_effectors"], ",");
+    size_t prop_num = 10;
+    if (end_effectors_str.size() > 0) {
+        size_t num = end_effectors_str.size()/prop_num;
+        for (size_t i = 0; i < num; i++) {
+            std::string ee_name, ee_target, ee_base;
+            coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
+            coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
+            coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
+            ABCIKparam tp;
+            for (size_t j = 0; j < XYZ; j++) {
+                coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
+            }
+            double tmpv[4];
+            for (int j = 0; j < 4; j++ ) {
+                coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
+            }
+            tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
+            // FIK param
+            SimpleFullbodyInverseKinematicsSolver::IKparam tmp_fikp;
+            tmp_fikp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(robot_in, robot_in->link(ee_base), robot_in->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
+            tmp_fikp.target_link = robot_in->link(ee_target);
+            tmp_fikp.localPos = tp.localPos;
+            tmp_fikp.localR = tp.localR;
+            fik_in->ikp.insert(std::pair<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>(ee_name, tmp_fikp));
+            // Fix for toe joint
+            //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
+            //   Without toe joints, "end-effector link == force-sensor link" is assumed.
+            //   With toe joints, "end-effector link != force-sensor link" is assumed.
+            if (robot_in->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
+                std::vector<double> optw(fik_in->ikp[ee_name].manip->numJoints(), 1.0);
+                optw.back() = 0.0; // Set weight = 0 for toe joint by default
+                fik_in->ikp[ee_name].manip->setOptionalWeightVector(optw);
+                tp.has_toe_joint = true;
+            } else {
+                tp.has_toe_joint = false;
+            }
+            tp.target_link = robot_in->link(ee_target);
+            std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
+            std::cerr << "[" << m_profile.instance_name << "]   target = " << fik_in->ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
+            std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+            std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
+            contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));////TODO:要移動?
+
+            eename_ikcp_map[ee_name].target_link_name = ee_target;
+            for (size_t j = 0; j < XYZ; j++){ coil::stringTo(eename_ikcp_map[ee_name].localPos(j), end_effectors_str[i*prop_num+3+j].c_str()); }
+            double tmp[4];
+            for (int j = 0; j < 4; j++ ){ coil::stringTo(tmp[j], end_effectors_str[i*prop_num+6+j].c_str()); }
+            eename_ikcp_map[ee_name].localR = Eigen::AngleAxis<double>(tmp[3], hrp::Vector3(tmp[0], tmp[1], tmp[2])).toRotationMatrix();
+        }
     }
-  }
 }
 
 #define TIMECALC 1
 RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
-  if(DEBUGP_ONCE)std::cerr << "[" << m_profile.instance_name<< "] onExecute(" << ec_id << ")" << std::endl;
-  struct timeval t_calc_start, t_calc_end;
-  if(TIMECALC)gettimeofday(&t_calc_start, NULL);
+    if(DEBUGP_ONCE)std::cerr << "[" << m_profile.instance_name<< "] onExecute(" << ec_id << ")" << std::endl;
+    struct timeval t_calc_start, t_calc_end;
+    if(TIMECALC)gettimeofday(&t_calc_start, NULL);
     if (m_qRefIn.isNew()) { m_qRefIn.read(); }
     if (m_basePosIn.isNew()) { m_basePosIn.read(); }
     if (m_baseRpyIn.isNew()) { m_baseRpyIn.read(); }
@@ -268,115 +268,115 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if (m_optionalDataIn.isNew()) { m_optionalDataIn.read(); }
     //for HumanSynchronizer
     if(mode.now()!=MODE_PAUSE){
-      if (m_htrfwIn.isNew()){ m_htrfwIn.read(); WBMSCore::DoubleSeqToVector6(m_htrfw.data,hsp->hp_wld_raw.tgt[rf].w); }
-      if (m_htlfwIn.isNew()){ m_htlfwIn.read(); WBMSCore::DoubleSeqToVector6(m_htlfw.data,hsp->hp_wld_raw.tgt[lf].w); }
-      if (m_htcomIn.isNew()){ m_htcomIn.read(); WBMSCore::Pose3DToWBMSPose3D(m_htcom.data,hsp->hp_wld_raw.tgt[com].abs); }
-      if (m_htrfIn.isNew()) { m_htrfIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htrf.data,hsp->hp_wld_raw.tgt[rf].abs); }
-      if (m_htlfIn.isNew()) { m_htlfIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htlf.data,hsp->hp_wld_raw.tgt[lf].abs); }
-      if (m_htrhIn.isNew()) { m_htrhIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htrh.data,hsp->hp_wld_raw.tgt[rh].abs);}
-      if (m_htlhIn.isNew()) { m_htlhIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htlh.data,hsp->hp_wld_raw.tgt[lh].abs);}
-      if (m_htheadIn.isNew()){ m_htheadIn.read(); WBMSCore::Pose3DToWBMSPose3D(m_hthead.data,hsp->hp_wld_raw.tgt[head].abs);}
-      if (m_htzmpIn.isNew()){ m_htzmpIn.read();  WBMSCore::Point3DToVector3(m_htzmp.data,hsp->hp_wld_raw.tgt[zmp].abs.p); }
+        if (m_htrfwIn.isNew()){ m_htrfwIn.read(); WBMSCore::DoubleSeqToVector6(m_htrfw.data,hsp->hp_wld_raw.tgt[rf].w); }
+        if (m_htlfwIn.isNew()){ m_htlfwIn.read(); WBMSCore::DoubleSeqToVector6(m_htlfw.data,hsp->hp_wld_raw.tgt[lf].w); }
+        if (m_htcomIn.isNew()){ m_htcomIn.read(); WBMSCore::Pose3DToWBMSPose3D(m_htcom.data,hsp->hp_wld_raw.tgt[com].abs); }
+        if (m_htrfIn.isNew()) { m_htrfIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htrf.data,hsp->hp_wld_raw.tgt[rf].abs); }
+        if (m_htlfIn.isNew()) { m_htlfIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htlf.data,hsp->hp_wld_raw.tgt[lf].abs); }
+        if (m_htrhIn.isNew()) { m_htrhIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htrh.data,hsp->hp_wld_raw.tgt[rh].abs);}
+        if (m_htlhIn.isNew()) { m_htlhIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htlh.data,hsp->hp_wld_raw.tgt[lh].abs);}
+        if (m_htheadIn.isNew()){ m_htheadIn.read(); WBMSCore::Pose3DToWBMSPose3D(m_hthead.data,hsp->hp_wld_raw.tgt[head].abs);}
+        if (m_htzmpIn.isNew()){ m_htzmpIn.read();  WBMSCore::Point3DToVector3(m_htzmp.data,hsp->hp_wld_raw.tgt[zmp].abs.p); }
     }
 
     if ( is_legged_robot ) {
-      processTransition();
-      mode.update();
-      if(DEBUGP)dbg(mode.now());
-      struct timespec startT, endT;
-      clock_gettime(CLOCK_REALTIME, &startT);
-
-      if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processTransition" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
-
-      for(int i=0; i<fik_list.size(); i++){
-        fik_list[i]->ikp["rleg"].is_ik_enable = true;
-        fik_list[i]->ikp["lleg"].is_ik_enable = true;
-        if(fik_list[i]->ikp.count("rarm"))fik_list[i]->ikp["rarm"].is_ik_enable = hsp->WBMSparam.use_rh;
-        if(fik_list[i]->ikp.count("larm"))fik_list[i]->ikp["larm"].is_ik_enable = hsp->WBMSparam.use_lh;
-      }
-
-      if (mode.isRunning()) {
-        if(mode.isInitialize()){
-          preProcessForWholeBodyMasterSlave(fik, m_robot);
-          hsp->fik_ml = fik_ml;
-          hsp->m_robot_ml = m_robot_ml;
-//          hsp->fik_act = fik_vsafe;
-          hsp->m_robot_act = m_robot_vsafe;
-          //逆動力学初期化
-          idsb.setInitState(m_robot, m_dt);
+        processTransition();
+        mode.update();
+        if(DEBUGP)dbg(mode.now());
+        struct timespec startT, endT;
+        clock_gettime(CLOCK_REALTIME, &startT);
+
+        if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processTransition" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
+
+        for(int i=0; i<fik_list.size(); i++){
+            fik_list[i]->ikp["rleg"].is_ik_enable = true;
+            fik_list[i]->ikp["lleg"].is_ik_enable = true;
+            if(fik_list[i]->ikp.count("rarm"))fik_list[i]->ikp["rarm"].is_ik_enable = hsp->WBMSparam.use_rh;
+            if(fik_list[i]->ikp.count("larm"))fik_list[i]->ikp["larm"].is_ik_enable = hsp->WBMSparam.use_lh;
         }
 
-        if(hsp->WBMSparam.is_doctor){
-          processWholeBodyMasterSlave(fik, m_robot, hsp->rp_ref_out);//安全制限つきマスタ・スレーブ
-        }else{
-          processWholeBodyMasterSlave_Raw(fik, m_robot, raw_pose);//生マスタ・スレーブ
+        if (mode.isRunning()) {
+            if(mode.isInitialize()){
+                preProcessForWholeBodyMasterSlave(fik, m_robot);
+                hsp->fik_ml = fik_ml;
+                hsp->m_robot_ml = m_robot_ml;
+                //          hsp->fik_act = fik_vsafe;
+                hsp->m_robot_act = m_robot_vsafe;
+                //逆動力学初期化
+                idsb.setInitState(m_robot, m_dt);
+            }
+
+            if(hsp->WBMSparam.is_doctor){
+                processWholeBodyMasterSlave(fik, m_robot, hsp->rp_ref_out);//安全制限つきマスタ・スレーブ
+            }else{
+                processWholeBodyMasterSlave_Raw(fik, m_robot, raw_pose);//生マスタ・スレーブ
+            }
+
+            if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processWholeBodyMasterSlave" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
+
+            //逆動力学
+            //        calcAccelerationsForInverseDynamics(m_robot, idsb);
+            //        hrp::Vector3 ref_zmp_invdyn;
+            //        calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);
+            //        ref_zmp_invdyn = invdyn_zmp_filters.passFilter(ref_zmp_invdyn);
+            //        updateInvDynStateBuffer(idsb);
+
+            if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ calcWorldZMPFromInverseDynamics" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
+
+            processHOFFARBIBFilter(m_robot, m_robot_vsafe);
+
+            const hrp::BodyPtr m_robot_for_out = m_robot_vsafe;
+            //        const hrp::BodyPtr m_robot_for_out = m_robot;
+
+            //OutPortデータセット
+            //        hrp::Vector3 ref_zmp = hsp->rp_ref_out.tgt[zmp].abs.p;
+
+            hrp::Vector3 com = m_robot_for_out->calcCM();
+            static hrp::Vector3 com_old = com;
+            static hrp::Vector3 com_old_old = com_old;
+            hrp::Vector3 com_acc = (com - 2*com_old + com_old_old)/(m_dt*m_dt);
+            hrp::Vector3 ref_zmp; ref_zmp << com.head(XY)-(com(Z)/G)*com_acc.head(XY), 0;
+            if(mode.isInitialize()){ final_ref_zmp_filter.reset(ref_zmp); }
+            ref_zmp = final_ref_zmp_filter.passFilter(ref_zmp);
+            com_old_old = com_old;
+            com_old = com;
+            hsp->act_rs.com = com;
+            hsp->act_rs.zmp = ref_zmp;
+
+            // qRef
+            for (int i = 0; i < m_qRef.data.length(); i++ ){
+                m_qRef.data[i] = transition_interpolator_ratio * m_robot_for_out->joint(i)->q  + (1 - transition_interpolator_ratio) * m_qRef.data[i];
+            }
+            // basePos
+            m_basePos.data.x = transition_interpolator_ratio * m_robot_for_out->rootLink()->p(0) + (1 - transition_interpolator_ratio) * m_basePos.data.x;
+            m_basePos.data.y = transition_interpolator_ratio * m_robot_for_out->rootLink()->p(1) + (1 - transition_interpolator_ratio) * m_basePos.data.y;
+            m_basePos.data.z = transition_interpolator_ratio * m_robot_for_out->rootLink()->p(2) + (1 - transition_interpolator_ratio) * m_basePos.data.z;
+            m_basePos.tm = m_qRef.tm;
+            // baseRpy
+            hrp::Vector3 baseRpy = hrp::rpyFromRot(m_robot_for_out->rootLink()->R);
+            m_baseRpy.data.r = transition_interpolator_ratio * baseRpy(0) + (1 - transition_interpolator_ratio) * m_baseRpy.data.r;
+            m_baseRpy.data.p = transition_interpolator_ratio * baseRpy(1) + (1 - transition_interpolator_ratio) * m_baseRpy.data.p;
+            m_baseRpy.data.y = transition_interpolator_ratio * baseRpy(2) + (1 - transition_interpolator_ratio) * m_baseRpy.data.y;
+            m_baseRpy.tm = m_qRef.tm;
+            // zmp
+            hrp::Vector3 rel_ref_zmp = m_robot_for_out->rootLink()->R.transpose() * (ref_zmp - m_robot_for_out->rootLink()->p);
+            m_zmp.data.x = transition_interpolator_ratio * rel_ref_zmp(0) + (1 - transition_interpolator_ratio) * m_zmp.data.x;
+            m_zmp.data.y = transition_interpolator_ratio * rel_ref_zmp(1) + (1 - transition_interpolator_ratio) * m_zmp.data.y;
+            m_zmp.data.z = transition_interpolator_ratio * rel_ref_zmp(2) + (1 - transition_interpolator_ratio) * m_zmp.data.z;
+            m_zmp.tm = m_qRef.tm;
+            // m_optionalData
+            if(m_optionalData.data.length() < optionalDataLength){
+                m_optionalData.data.length(optionalDataLength);//TODO:これいいのか？
+                for(int i=0;i<optionalDataLength;i++)m_optionalData.data[i] = 0;
+            }
+            //        m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = hsp->rp_ref_out.tgt[rf].is_contact;
+            //        m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = hsp->rp_ref_out.tgt[lf].is_contact;
+            //        bool rf_is_contact =
+            m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = hsp->rp_ref_out.tgt[rf].is_contact;
+            m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = hsp->rp_ref_out.tgt[lf].is_contact;
         }
-
-        if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processWholeBodyMasterSlave" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
-
-        //逆動力学
-//        calcAccelerationsForInverseDynamics(m_robot, idsb);
-//        hrp::Vector3 ref_zmp_invdyn;
-//        calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);
-//        ref_zmp_invdyn = invdyn_zmp_filters.passFilter(ref_zmp_invdyn);
-//        updateInvDynStateBuffer(idsb);
-
-        if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ calcWorldZMPFromInverseDynamics" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
-
-        processHOFFARBIBFilter(m_robot, m_robot_vsafe);
-
-        const hrp::BodyPtr m_robot_for_out = m_robot_vsafe;
-//        const hrp::BodyPtr m_robot_for_out = m_robot;
-
-        //OutPortデータセット
-//        hrp::Vector3 ref_zmp = hsp->rp_ref_out.tgt[zmp].abs.p;
-
-        hrp::Vector3 com = m_robot_for_out->calcCM();
-        static hrp::Vector3 com_old = com;
-        static hrp::Vector3 com_old_old = com_old;
-        hrp::Vector3 com_acc = (com - 2*com_old + com_old_old)/(m_dt*m_dt);
-        hrp::Vector3 ref_zmp; ref_zmp << com.head(XY)-(com(Z)/G)*com_acc.head(XY), 0;
-        if(mode.isInitialize()){ final_ref_zmp_filter.reset(ref_zmp); }
-        ref_zmp = final_ref_zmp_filter.passFilter(ref_zmp);
-        com_old_old = com_old;
-        com_old = com;
-        hsp->act_rs.com = com;
-        hsp->act_rs.zmp = ref_zmp;
-
-        // qRef
-        for (int i = 0; i < m_qRef.data.length(); i++ ){
-          m_qRef.data[i] = transition_interpolator_ratio * m_robot_for_out->joint(i)->q  + (1 - transition_interpolator_ratio) * m_qRef.data[i];
-        }
-        // basePos
-        m_basePos.data.x = transition_interpolator_ratio * m_robot_for_out->rootLink()->p(0) + (1 - transition_interpolator_ratio) * m_basePos.data.x;
-        m_basePos.data.y = transition_interpolator_ratio * m_robot_for_out->rootLink()->p(1) + (1 - transition_interpolator_ratio) * m_basePos.data.y;
-        m_basePos.data.z = transition_interpolator_ratio * m_robot_for_out->rootLink()->p(2) + (1 - transition_interpolator_ratio) * m_basePos.data.z;
-        m_basePos.tm = m_qRef.tm;
-        // baseRpy
-        hrp::Vector3 baseRpy = hrp::rpyFromRot(m_robot_for_out->rootLink()->R);
-        m_baseRpy.data.r = transition_interpolator_ratio * baseRpy(0) + (1 - transition_interpolator_ratio) * m_baseRpy.data.r;
-        m_baseRpy.data.p = transition_interpolator_ratio * baseRpy(1) + (1 - transition_interpolator_ratio) * m_baseRpy.data.p;
-        m_baseRpy.data.y = transition_interpolator_ratio * baseRpy(2) + (1 - transition_interpolator_ratio) * m_baseRpy.data.y;
-        m_baseRpy.tm = m_qRef.tm;
-        // zmp
-        hrp::Vector3 rel_ref_zmp = m_robot_for_out->rootLink()->R.transpose() * (ref_zmp - m_robot_for_out->rootLink()->p);
-        m_zmp.data.x = transition_interpolator_ratio * rel_ref_zmp(0) + (1 - transition_interpolator_ratio) * m_zmp.data.x;
-        m_zmp.data.y = transition_interpolator_ratio * rel_ref_zmp(1) + (1 - transition_interpolator_ratio) * m_zmp.data.y;
-        m_zmp.data.z = transition_interpolator_ratio * rel_ref_zmp(2) + (1 - transition_interpolator_ratio) * m_zmp.data.z;
-        m_zmp.tm = m_qRef.tm;
-        // m_optionalData
-        if(m_optionalData.data.length() < optionalDataLength){
-          m_optionalData.data.length(optionalDataLength);//TODO:これいいのか？
-          for(int i=0;i<optionalDataLength;i++)m_optionalData.data[i] = 0;
-        }
-//        m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = hsp->rp_ref_out.tgt[rf].is_contact;
-//        m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = hsp->rp_ref_out.tgt[lf].is_contact;
-        bool rf_is_contact =
-        m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = hsp->rp_ref_out.tgt[rf].is_contact;
-        m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = hsp->rp_ref_out.tgt[lf].is_contact;
-      }
-      hsp->baselinkpose.p = m_robot->rootLink()->p;
-      hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
+        hsp->baselinkpose.p = m_robot->rootLink()->p;
+        hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
     }
 
     // write
@@ -388,440 +388,460 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
 #ifdef USE_DEBUG_PORT
     // dbg plot
-     m_htcom_dbg.tm = m_qRef.tm;
-     WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[com].abs, m_htcom_dbg.data);
-     m_htcom_dbgOut.write();
-     m_htrf_dbg.tm = m_qRef.tm;
-     WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[rf].abs, m_htrf_dbg.data);
-     m_htrf_dbgOut.write();
-     m_htlf_dbg.tm = m_qRef.tm;
-     WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[lf].abs, m_htlf_dbg.data);
-     m_htlf_dbgOut.write();
-     m_htrh_dbg.tm = m_qRef.tm;
-     WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[rh].abs, m_htrh_dbg.data);
-     m_htrh_dbgOut.write();
-     m_htlh_dbg.tm = m_qRef.tm;
-     WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[lh].abs, m_htlh_dbg.data);
-     m_htlh_dbgOut.write();
-     m_hthead_dbg.tm = m_qRef.tm;
-     WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[head].abs, m_hthead_dbg.data);
-     m_hthead_dbgOut.write();
-     m_htzmp_dbg.tm = m_qRef.tm;
-     WBMSCore::Vector3ToPoint3D(hsp->rp_ref_out.tgt[zmp].abs.p,m_rpzmp_dbg.data);
-     m_htzmp_dbgOut.write();
-     m_htrfw_dbg.tm = m_qRef.tm;
-     m_htrfw_dbg.data.length(6);
-     WBMSCore::Vector6ToDoubleSeq(hsp->hp_plot.tgt[rf].w, m_htrfw_dbg.data);
-     m_htrfw_dbgOut.write();
-     m_htlfw_dbg.tm = m_qRef.tm;
-     m_htlfw_dbg.data.length(6);
-     WBMSCore::Vector6ToDoubleSeq(hsp->hp_plot.tgt[lf].w, m_htlfw_dbg.data);
-     m_htlfw_dbgOut.write();
-     m_rpcom_dbg.tm = m_qRef.tm;
-     WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[com].abs, m_rpcom_dbg.data);
-     m_rpcom_dbgOut.write();
-     m_rprf_dbg.tm = m_qRef.tm;
-     WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[rf].abs, m_rprf_dbg.data);
-     m_rprf_dbgOut.write();
-     m_rplf_dbg.tm = m_qRef.tm;
-     WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[lf].abs, m_rplf_dbg.data);
-     m_rplf_dbgOut.write();
-     m_rprh_dbg.tm = m_qRef.tm;
-     WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[rh].abs, m_rprh_dbg.data);
-     m_rprh_dbgOut.write();
-     m_rplh_dbg.tm = m_qRef.tm;
-     WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[lh].abs, m_rplh_dbg.data);
-     m_rplh_dbgOut.write();
-     m_rphead_dbg.tm = m_qRef.tm;
-     WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[head].abs, m_rphead_dbg.data);
-     m_rphead_dbgOut.write();
-     m_rpzmp_dbg.tm = m_qRef.tm;
-     WBMSCore::Vector3ToPoint3D(hsp->rp_ref_out.tgt[zmp].abs.p, m_rpzmp_dbg.data);
-     m_rpzmp_dbgOut.write();
-     m_rpdcp_dbg.tm = m_qRef.tm;
-     WBMSCore::Vector3ToPoint3D(hsp->cp_dec,m_rpdcp_dbg.data);
-     m_rpdcp_dbgOut.write();
-     m_rpacp_dbg.tm = m_qRef.tm;
-     WBMSCore::Vector3ToPoint3D(hsp->cp_acc,m_rpacp_dbg.data);
-     m_rpacp_dbgOut.write();
-     m_invdyn_dbg.tm = m_qRef.tm;
-     m_invdyn_dbg.data.length(6);
-     WBMSCore::Vector6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
-     m_invdyn_dbgOut.write();
+    m_htcom_dbg.tm = m_qRef.tm;
+    WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[com].abs, m_htcom_dbg.data);
+    m_htcom_dbgOut.write();
+    m_htrf_dbg.tm = m_qRef.tm;
+    WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[rf].abs, m_htrf_dbg.data);
+    m_htrf_dbgOut.write();
+    m_htlf_dbg.tm = m_qRef.tm;
+    WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[lf].abs, m_htlf_dbg.data);
+    m_htlf_dbgOut.write();
+    m_htrh_dbg.tm = m_qRef.tm;
+    WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[rh].abs, m_htrh_dbg.data);
+    m_htrh_dbgOut.write();
+    m_htlh_dbg.tm = m_qRef.tm;
+    WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[lh].abs, m_htlh_dbg.data);
+    m_htlh_dbgOut.write();
+    m_hthead_dbg.tm = m_qRef.tm;
+    WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[head].abs, m_hthead_dbg.data);
+    m_hthead_dbgOut.write();
+    m_htzmp_dbg.tm = m_qRef.tm;
+    WBMSCore::Vector3ToPoint3D(hsp->rp_ref_out.tgt[zmp].abs.p,m_rpzmp_dbg.data);
+    m_htzmp_dbgOut.write();
+    m_htrfw_dbg.tm = m_qRef.tm;
+    m_htrfw_dbg.data.length(6);
+    WBMSCore::Vector6ToDoubleSeq(hsp->hp_plot.tgt[rf].w, m_htrfw_dbg.data);
+    m_htrfw_dbgOut.write();
+    m_htlfw_dbg.tm = m_qRef.tm;
+    m_htlfw_dbg.data.length(6);
+    WBMSCore::Vector6ToDoubleSeq(hsp->hp_plot.tgt[lf].w, m_htlfw_dbg.data);
+    m_htlfw_dbgOut.write();
+    m_rpcom_dbg.tm = m_qRef.tm;
+    WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[com].abs, m_rpcom_dbg.data);
+    m_rpcom_dbgOut.write();
+    m_rprf_dbg.tm = m_qRef.tm;
+    WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[rf].abs, m_rprf_dbg.data);
+    m_rprf_dbgOut.write();
+    m_rplf_dbg.tm = m_qRef.tm;
+    WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[lf].abs, m_rplf_dbg.data);
+    m_rplf_dbgOut.write();
+    m_rprh_dbg.tm = m_qRef.tm;
+    WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[rh].abs, m_rprh_dbg.data);
+    m_rprh_dbgOut.write();
+    m_rplh_dbg.tm = m_qRef.tm;
+    WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[lh].abs, m_rplh_dbg.data);
+    m_rplh_dbgOut.write();
+    m_rphead_dbg.tm = m_qRef.tm;
+    WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[head].abs, m_rphead_dbg.data);
+    m_rphead_dbgOut.write();
+    m_rpzmp_dbg.tm = m_qRef.tm;
+    WBMSCore::Vector3ToPoint3D(hsp->rp_ref_out.tgt[zmp].abs.p, m_rpzmp_dbg.data);
+    m_rpzmp_dbgOut.write();
+    m_rpdcp_dbg.tm = m_qRef.tm;
+    WBMSCore::Vector3ToPoint3D(hsp->cp_dec,m_rpdcp_dbg.data);
+    m_rpdcp_dbgOut.write();
+    m_rpacp_dbg.tm = m_qRef.tm;
+    WBMSCore::Vector3ToPoint3D(hsp->cp_acc,m_rpacp_dbg.data);
+    m_rpacp_dbgOut.write();
+    m_invdyn_dbg.tm = m_qRef.tm;
+    m_invdyn_dbg.data.length(6);
+    WBMSCore::Vector6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
+    m_invdyn_dbgOut.write();
 #endif
-     if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"t_last:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6<<endl; t_calc_start = t_calc_end;}
-    loop ++;
-    return RTC::RTC_OK;
+if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"t_last:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6<<endl; t_calc_start = t_calc_end;}
+loop ++;
+return RTC::RTC_OK;
 }
 
 
 void WholeBodyMasterSlave::processTransition(){
-  switch(mode.now()){
-
-    case MODE_SYNC_TO_WBMS:
-      if(mode.pre() == MODE_IDLE){ double tmp = 1.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
-      if (!transition_interpolator->isEmpty() ){
-        transition_interpolator->get(&transition_interpolator_ratio, true);
-      }else{
-        mode.setModeRequest(MODE_WBMS);
-      }
-      break;
-
-    case MODE_SYNC_TO_IDLE:
-      if(mode.pre() == MODE_WBMS || mode.pre() == MODE_PAUSE){ double tmp = 0.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
-      if (!transition_interpolator->isEmpty()) {
-        transition_interpolator->get(&transition_interpolator_ratio, true);
-      }else{
-        mode.setModeRequest(MODE_IDLE);
-      }
-      break;
-  }
+    switch(mode.now()){
+
+        case MODE_SYNC_TO_WBMS:
+            if(mode.pre() == MODE_IDLE){ double tmp = 1.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
+            if (!transition_interpolator->isEmpty() ){
+                transition_interpolator->get(&transition_interpolator_ratio, true);
+            }else{
+                mode.setModeRequest(MODE_WBMS);
+            }
+            break;
+
+        case MODE_SYNC_TO_IDLE:
+            if(mode.pre() == MODE_WBMS || mode.pre() == MODE_PAUSE){ double tmp = 0.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
+            if (!transition_interpolator->isEmpty()) {
+                transition_interpolator->get(&transition_interpolator_ratio, true);
+            }else{
+                mode.setModeRequest(MODE_IDLE);
+            }
+            break;
+    }
 }
 
 
 void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in){
-  hrp::Vector3 basePos_heightChecked = hrp::Vector3(m_basePos.data.x, m_basePos.data.y, m_basePos.data.z);//ベースリンク高さ調整により足裏高さ0に
-  robot_in->rootLink()->p = basePos_heightChecked;
-  for ( int i = 0; i < robot_in->numJoints(); i++ ){ robot_in->joint(i)->q = m_qRef.data[i]; }
-  robot_in->calcForwardKinematics();
-  hrp::Vector3 init_foot_mid_coord = (fik_in->getEndEffectorPos("rleg") + fik_in->getEndEffectorPos("lleg")) / 2;
-  if( fabs((double)init_foot_mid_coord(Z)) > 1e-5 ){
-    basePos_heightChecked(Z) -= init_foot_mid_coord(Z);
-    init_foot_mid_coord(Z) = 0;
-    std::cerr<<"["<<m_profile.instance_name<<"] Input basePos height is invalid. Auto modify "<<m_basePos.data.z<<" -> "<<basePos_heightChecked(Z)<<endl;
-  }
-  const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
-
-  for(int i=0;i<body_list.size();i++){//初期姿勢でBodyをFK
-    body_list[i]->rootLink()->p = basePos_heightChecked;
-    body_list[i]->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
-    for ( int j = 0; j < body_list[i]->numJoints(); j++ ){ body_list[i]->joint(j)->q = m_qRef.data[j]; }
-    if( body_list[i]->link("RARM_JOINT2") != NULL) body_list[i]->link("RARM_JOINT2")->ulimit = -40 * D2R;//脇の干渉回避のため
-    if( body_list[i]->link("LARM_JOINT2") != NULL) body_list[i]->link("LARM_JOINT2")->llimit = 40 * D2R;
-    body_list[i]->calcForwardKinematics();
-  }
-  for(int i=0;i<fik_list.size();i++){//初期姿勢でBodyをFK
-    fik_list[i]->setReferenceJointAngles();
-    for(int l=0;l<4;l++){//targetを初期化
-      if(fik_list[i]->ikp.count(robot_l_names[l])){
-        fik_list[i]->ikp[robot_l_names[l]].target_p0 = fik_list[i]->getEndEffectorPos(robot_l_names[l]);
-        fik_list[i]->ikp[robot_l_names[l]].target_r0 = fik_list[i]->getEndEffectorRot(robot_l_names[l]);
-      }
+    hrp::Vector3 basePos_heightChecked = hrp::Vector3(m_basePos.data.x, m_basePos.data.y, m_basePos.data.z);//ベースリンク高さ調整により足裏高さ0に
+    robot_in->rootLink()->p = basePos_heightChecked;
+    for ( int i = 0; i < robot_in->numJoints(); i++ ){ robot_in->joint(i)->q = m_qRef.data[i]; }
+    robot_in->calcForwardKinematics();
+    hrp::Vector3 init_foot_mid_coord = (fik_in->getEndEffectorPos("rleg") + fik_in->getEndEffectorPos("lleg")) / 2;
+    if( fabs((double)init_foot_mid_coord(Z)) > 1e-5 ){
+        basePos_heightChecked(Z) -= init_foot_mid_coord(Z);
+        init_foot_mid_coord(Z) = 0;
+        std::cerr<<"["<<m_profile.instance_name<<"] Input basePos height is invalid. Auto modify "<<m_basePos.data.z<<" -> "<<basePos_heightChecked(Z)<<endl;
     }
-  }
-  q_ip->clear();
-  init_sync_state.resize(ROBOT_ALL_DOF);
-  for(int i=0;i<robot_in->numJoints();i++){ init_sync_state(i) = robot_in->joint(i)->q; }
-  init_sync_state.bottomRows(6).topRows(3) = robot_in->rootLink()->p;
-  init_sync_state.bottomRows(6).bottomRows(3) = hrp::rpyFromRot(robot_in->rootLink()->R);
-  q_ip->set(init_sync_state.data());
-  hsp->initializeRequest(fik_in, robot_in);
+    const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
+
+    for(int i=0;i<body_list.size();i++){//初期姿勢でBodyをFK
+        body_list[i]->rootLink()->p = basePos_heightChecked;
+        body_list[i]->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
+        for ( int j = 0; j < body_list[i]->numJoints(); j++ ){ body_list[i]->joint(j)->q = m_qRef.data[j]; }
+//        if( body_list[i]->link("RARM_JOINT2") != NULL) body_list[i]->link("RARM_JOINT2")->ulimit = -40 * D2R;//脇の干渉回避のため
+//        if( body_list[i]->link("LARM_JOINT2") != NULL) body_list[i]->link("LARM_JOINT2")->llimit = 40 * D2R;
+        body_list[i]->calcForwardKinematics();
+    }
+    for(int i=0;i<fik_list.size();i++){//初期姿勢でBodyをFK
+        fik_list[i]->setReferenceJointAngles();
+        for(int l=0;l<4;l++){//targetを初期化
+            if(fik_list[i]->ikp.count(robot_l_names[l])){
+                fik_list[i]->ikp[robot_l_names[l]].target_p0 = fik_list[i]->getEndEffectorPos(robot_l_names[l]);
+                fik_list[i]->ikp[robot_l_names[l]].target_r0 = fik_list[i]->getEndEffectorRot(robot_l_names[l]);
+            }
+        }
+    }
+    q_ip->clear();
+    init_sync_state.resize(ROBOT_ALL_DOF);
+    for(int i=0;i<robot_in->numJoints();i++){ init_sync_state(i) = robot_in->joint(i)->q; }
+    init_sync_state.bottomRows(6).topRows(3) = robot_in->rootLink()->p;
+    init_sync_state.bottomRows(6).bottomRows(3) = hrp::rpyFromRot(robot_in->rootLink()->R);
+    q_ip->set(init_sync_state.data());
+    hsp->initializeRequest(fik_in, robot_in);
 }
 
 
 void WholeBodyMasterSlave::processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HumanPose& pose_ref){
-  hsp->update();//////HumanSynchronizerの主要処理
-  if(DEBUGP)cout<<"update():"<<hsp->getUpdateTime()<<endl;
-  if(DEBUGP)pose_ref.print();
-  solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave");
+    hsp->update();//////HumanSynchronizerの主要処理
+    if(DEBUGP)cout<<"update():"<<hsp->getUpdateTime()<<endl;
+    if(DEBUGP)pose_ref.print();
+    solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave");
 }
 
 
 void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::BodyPtr& robot_in, HumanPose& pose_ref){
-  static BiquadIIRFilterVec pos_filters[num_pose_tgt], rot_filters[num_pose_tgt];
-  static unsigned int callnum;
-  for(int i=0;i<num_pose_tgt;i++){
-    if(callnum == 0){
-      pos_filters[i].setParameter(100.0, 1.0/m_dt, Q_NOOVERSHOOT);
-      rot_filters[i].setParameter(100.0, 1.0/m_dt, Q_NOOVERSHOOT);
-      pos_filters[i].reset(hsp->hp_wld_raw.tgt[i].abs.p);
-      rot_filters[i].reset(hsp->hp_wld_raw.tgt[i].abs.rpy);
+    static BiquadIIRFilterVec pos_filters[num_pose_tgt], rot_filters[num_pose_tgt];
+    static unsigned int callnum;
+    for(int i=0;i<num_pose_tgt;i++){
+        if(callnum == 0){
+            pos_filters[i].setParameter(100.0, 1.0/m_dt, Q_NOOVERSHOOT);
+            rot_filters[i].setParameter(100.0, 1.0/m_dt, Q_NOOVERSHOOT);
+            pos_filters[i].reset(hsp->hp_wld_raw.tgt[i].abs.p);
+            rot_filters[i].reset(hsp->hp_wld_raw.tgt[i].abs.rpy);
+        }
+        pose_ref.tgt[i].abs.p = pos_filters[i].passFilter(hsp->hp_wld_raw.tgt[i].abs.p);
+        pose_ref.tgt[i].abs.rpy = rot_filters[i].passFilter(hsp->hp_wld_raw.tgt[i].abs.rpy);
     }
-    pose_ref.tgt[i].abs.p = pos_filters[i].passFilter(hsp->hp_wld_raw.tgt[i].abs.p);
-    pose_ref.tgt[i].abs.rpy = rot_filters[i].passFilter(hsp->hp_wld_raw.tgt[i].abs.rpy);
-  }
-  callnum++;
-  if(DEBUGP){ fprintf(stderr,"\x1b[31mmaster-mode:\x1b[39m"); pose_ref.print(); }
-//  solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave_Raw");
+    callnum++;
+    if(DEBUGP){ fprintf(stderr,"\x1b[31mmaster-mode:\x1b[39m"); pose_ref.print(); }
+    //  solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave_Raw");
 }
 
 void WholeBodyMasterSlave::calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robot_in){
-  const std::string names[4] = {"rleg","lleg","rarm","larm"};
-  hrp::dmatrix J,Jinv,Jnull;
-  for(int l=0; l<4; l++){
-    if(fik_in->ikp.count(names[l])){
-      fik_in->ikp[names[l]].manip->calcJacobian(J);
-      fik_in->ikp[names[l]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
-      Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
-      Eigen::JacobiSVD< Eigen::MatrixXd > svd_rot(J.block(3,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
-      hsp->manip_mat[l] = svd.matrixU();
-      hsp->manip_mat_rot[l] = svd_rot.matrixU();
-      for(int i=0;i<3;i++){
-        hsp->manip_sv[l](i) = svd.singularValues()(i);
-        hsp->manip_sv_rot[l](i) = svd_rot.singularValues()(i);
-      }
+    const std::string names[4] = {"rleg","lleg","rarm","larm"};
+    hrp::dmatrix J,Jinv,Jnull;
+    for(int l=0; l<4; l++){
+        if(fik_in->ikp.count(names[l])){
+            fik_in->ikp[names[l]].manip->calcJacobian(J);
+            fik_in->ikp[names[l]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
+            Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
+            Eigen::JacobiSVD< Eigen::MatrixXd > svd_rot(J.block(3,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
+            hsp->manip_mat[l] = svd.matrixU();
+            hsp->manip_mat_rot[l] = svd_rot.matrixU();
+            for(int i=0;i<3;i++){
+                hsp->manip_sv[l](i) = svd.singularValues()(i);
+                hsp->manip_sv_rot[l](i) = svd_rot.singularValues()(i);
+            }
+        }
     }
-  }
 }
 
 #ifdef USE_NEW_FIK
 void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix){
-  std::vector<IKConstraintParam> ik_tgt_list;
-  IKConstraintParam tmp;
-
-  tmp.target_link_name = "WAIST";
-  tmp.localPos = hrp::Vector3::Zero();
-  tmp.localR = hrp::Matrix33::Identity();
-  tmp.targetPos = robot_in->rootLink()->p;// will be ignored by selection_vec
-  tmp.targetRpy = com_ref.rpy;// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
-  tmp.weight_vec << 0,0,0,1,1,1;
-  ik_tgt_list.push_back(tmp);
-
-  tmp.target_link_name = eename_ikcp_map["rleg"].target_link_name;
-  tmp.localPos = eename_ikcp_map["rleg"].localPos;
-  tmp.localR = eename_ikcp_map["rleg"].localR;
-  tmp.targetPos = rf_ref.p;
-  tmp.targetRpy = rf_ref.rpy;
-  tmp.weight_vec << 1,1,5,1,1,1;
-  ik_tgt_list.push_back(tmp);
-
-  tmp.target_link_name = eename_ikcp_map["lleg"].target_link_name;
-  tmp.localPos = eename_ikcp_map["lleg"].localPos;
-  tmp.localR = eename_ikcp_map["lleg"].localR;
-  tmp.targetPos = lf_ref.p;
-  tmp.targetRpy = lf_ref.rpy;
-  tmp.weight_vec << 1,1,5,1,1,1;
-  ik_tgt_list.push_back(tmp);
-
-  tmp.target_link_name = eename_ikcp_map["rarm"].target_link_name;
-  tmp.localPos = eename_ikcp_map["rarm"].localPos;
-  tmp.localR = eename_ikcp_map["rarm"].localR;
-  tmp.targetPos = rh_ref.p;
-  tmp.targetRpy = rh_ref.rpy;
-  tmp.weight_vec = hrp::dvector6::Constant(0.5);
-  ik_tgt_list.push_back(tmp);
-
-  tmp.target_link_name = eename_ikcp_map["larm"].target_link_name;
-  tmp.localPos = eename_ikcp_map["larm"].localPos;
-  tmp.localR = eename_ikcp_map["larm"].localR;
-  tmp.targetPos = lh_ref.p;
-  tmp.targetRpy = lh_ref.rpy;
-  tmp.weight_vec = hrp::dvector6::Constant(0.5);
-  ik_tgt_list.push_back(tmp);
-
-  tmp.target_link_name = "COM";
-  tmp.localPos = hrp::Vector3::Zero();
-  tmp.localR = hrp::Matrix33::Identity();
-  tmp.targetPos = com_ref.p;// COM height will not be constraint
-  tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
-  tmp.weight_vec << 100,100,0.5,1e-3,1e-3,1e-3;
-//  tmp.weight_vec << 1,1,1,1,1,1;
-  ik_tgt_list.push_back(tmp);
-
-  if(robot_in->link("HEAD_JOINT1") != NULL){
-    tmp.target_link_name = "HEAD_JOINT1";
-    tmp.targetRpy = head_ref.rpy;
-    tmp.weight_vec << 0,0,0,0,1e-1,1e-1;
-    ik_tgt_list.push_back(tmp);
-  }
-
-//  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;
-//  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
-//  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
-//  fik_in->optional_weight_vector.tail(6).fill(0.01);
-  if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇の干渉回避のため
-  if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(40);
-
-  fik_in->reference_q = init_sync_state;
-  fik_in->reference_gain.fill(0.001);
-  fik_in->reference_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
-
-  struct timespec startT, endT;
-  int loop_result = 0;
-  const int IK_MAX_LOOP = 1;
-  clock_gettime(CLOCK_REALTIME, &startT);
-  loop_result = fik_in->solveFullbodyIKLoop(ik_tgt_list, IK_MAX_LOOP);
-  if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveIK"<<loop_result<<"loop" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
+    std::vector<IKConstraint> ikc_list;
+    {
+        IKConstraint tmp;
+        tmp.target_link_name = "WAIST";
+        tmp.localPos = hrp::Vector3::Zero();
+        tmp.localR = hrp::Matrix33::Identity();
+        tmp.targetPos = robot_in->rootLink()->p;// will be ignored by selection_vec
+        tmp.targetRpy = com_ref.rpy;// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
+        tmp.constraint_weight << 0,0,0,1,1,1;
+        tmp.rot_precision = deg2rad(3);
+        ikc_list.push_back(tmp);
+    }{
+        IKConstraint tmp;
+        tmp.target_link_name = eename_ikcp_map["rleg"].target_link_name;
+        tmp.localPos = eename_ikcp_map["rleg"].localPos;
+        tmp.localR = eename_ikcp_map["rleg"].localR;
+        tmp.targetPos = rf_ref.p;
+        tmp.targetRpy = rf_ref.rpy;
+        if(hsp->rp_ref_out.tgt[rf].is_contact){
+            tmp.constraint_weight = hrp::dvector6::Constant(10);
+        }else{
+            tmp.constraint_weight = hrp::dvector6::Constant(0.1);
+        }
+        ikc_list.push_back(tmp);
+    }{
+        IKConstraint tmp;
+        tmp.target_link_name = eename_ikcp_map["lleg"].target_link_name;
+        tmp.localPos = eename_ikcp_map["lleg"].localPos;
+        tmp.localR = eename_ikcp_map["lleg"].localR;
+        tmp.targetPos = lf_ref.p;
+        tmp.targetRpy = lf_ref.rpy;
+        if(hsp->rp_ref_out.tgt[lf].is_contact){
+            tmp.constraint_weight = hrp::dvector6::Constant(10);
+        }else{
+            tmp.constraint_weight = hrp::dvector6::Constant(0.1);
+        }
+        ikc_list.push_back(tmp);
+    }{
+        IKConstraint tmp;
+        tmp.target_link_name = eename_ikcp_map["rarm"].target_link_name;
+        tmp.localPos = eename_ikcp_map["rarm"].localPos;
+        tmp.localR = eename_ikcp_map["rarm"].localR;
+        tmp.targetPos = rh_ref.p;
+        tmp.targetRpy = rh_ref.rpy;
+        tmp.constraint_weight = hrp::dvector6::Constant(0.1);
+        tmp.pos_precision = 3e-3;
+        tmp.rot_precision = deg2rad(3);
+        ikc_list.push_back(tmp);
+    }{
+        IKConstraint tmp;
+        tmp.target_link_name = eename_ikcp_map["larm"].target_link_name;
+        tmp.localPos = eename_ikcp_map["larm"].localPos;
+        tmp.localR = eename_ikcp_map["larm"].localR;
+        tmp.targetPos = lh_ref.p;
+        tmp.targetRpy = lh_ref.rpy;
+        tmp.constraint_weight = hrp::dvector6::Constant(0.1);
+        tmp.pos_precision = 3e-3;
+        tmp.rot_precision = deg2rad(3);
+        ikc_list.push_back(tmp);
+    }if(robot_in->link("HEAD_JOINT1") != NULL){
+        IKConstraint tmp;
+        tmp.target_link_name = "HEAD_JOINT1";
+        tmp.targetRpy = head_ref.rpy;
+        tmp.constraint_weight << 0,0,0,0,0.1,0.1;
+        tmp.rot_precision = deg2rad(1);
+        ikc_list.push_back(tmp);
+    }{
+        IKConstraint tmp;
+        tmp.target_link_name = "COM";
+        tmp.localPos = hrp::Vector3::Zero();
+        tmp.localR = hrp::Matrix33::Identity();
+        tmp.targetPos = com_ref.p;// COM height will not be constraint
+        tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
+        tmp.constraint_weight << 10,10,1,0,0,0;
+        tmp.rot_precision = 100;//angular momentum precision
+        ikc_list.push_back(tmp);
+    }
+
+    //  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;
+    //  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
+    //  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
+    //  fik_in->optional_weight_vector.tail(6).fill(0.01);
+    if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇内側の干渉回避
+    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(40);
+    if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->llimit = deg2rad(-140);//肩外側の干渉回避
+    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->ulimit = deg2rad(140);
+
+    fik_in->q_ref = init_sync_state;
+    fik_in->q_ref_pullback_gain.fill(0.001);
+    fik_in->q_ref_pullback_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
+
+    struct timespec startT, endT;
+    const int IK_MAX_LOOP = 3;
+    clock_gettime(CLOCK_REALTIME, &startT);
+    int loop_result = fik_in->solveFullbodyIKLoop(m_robot, ikc_list, IK_MAX_LOOP);
+    if(loop%100==0){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveIK"<<loop_result<<"loop" << std::endl;}
 }
 #else
 //旧ver
 void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix){
-  int com_ik_loop=0;
-  const int COM_IK_MAX_LOOP = 10;
-  const double COM_IK_MAX_ERROR = 1e-5;//1e-4だと乱れる
-  const std::string names[4] = {"rleg","lleg","rarm","larm"};
-  const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
-//  robot_in->rootLink()->p = 0.01*com_ref.p + 0.99*robot_in->rootLink()->p;//ベースリンク位置が迷走するのを防ぐ？
-  robot_in->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
-  for(int i=0;i<4;i++){
-    if(fik_in->ikp.count(names[i])){
-      fik_in->ikp[names[i]].targetPos = refs[i]->p;
-      fik_in->ikp[names[i]].targetRpy = hrp::rotFromRpy(refs[i]->rpy);
-    }
-  }
-//  fik_in->storeCurrentParameters();
-//  fik_in->setReferenceJointAngles();//これ入れると腕ブワーなる
-  if( robot_in->link("HEAD_JOINT0") != NULL) robot_in->link("HEAD_JOINT0")->q = head_ref.rpy(y);
-  if( robot_in->link("HEAD_JOINT1") != NULL) robot_in->link("HEAD_JOINT1")->q = head_ref.rpy(p);
-  if(fik_in->ikp.count("rarm") && fik_in->ikp.count("larm")){
-    hrp::Vector3 base2rh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["rarm"].targetPos - robot_in->rootLink()->p);
-    hrp::Vector3 base2lh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["larm"].targetPos - robot_in->rootLink()->p);
-    if( robot_in->link("CHEST_JOINT0") != NULL){
-      robot_in->link("CHEST_JOINT0")->q = (base2lh(Z) - base2rh(Z)) * (10 * D2R / 1.0);
-      LIMIT_MINMAX(robot_in->link("CHEST_JOINT0")->q, robot_in->link("CHEST_JOINT0")->llimit, robot_in->link("CHEST_JOINT0")->ulimit);
+    int com_ik_loop=0;
+    const int COM_IK_MAX_LOOP = 10;
+    const double COM_IK_MAX_ERROR = 1e-5;//1e-4だと乱れる
+    const std::string names[4] = {"rleg","lleg","rarm","larm"};
+    const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
+    //  robot_in->rootLink()->p = 0.01*com_ref.p + 0.99*robot_in->rootLink()->p;//ベースリンク位置が迷走するのを防ぐ？
+    robot_in->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
+    for(int i=0;i<4;i++){
+        if(fik_in->ikp.count(names[i])){
+            fik_in->ikp[names[i]].targetPos = refs[i]->p;
+            fik_in->ikp[names[i]].targetRpy = hrp::rotFromRpy(refs[i]->rpy);
+        }
     }
-    if(robot_in->link("CHEST_JOINT2") != NULL){
-      robot_in->link("CHEST_JOINT2")->q = (base2rh(X) - base2lh(X)) * (60 * D2R / 1.0);
-      LIMIT_MINMAX(robot_in->link("CHEST_JOINT2")->q, robot_in->link("CHEST_JOINT2")->llimit, robot_in->link("CHEST_JOINT2")->ulimit);
+    //  fik_in->storeCurrentParameters();
+    //  fik_in->setReferenceJointAngles();//これ入れると腕ブワーなる
+    if( robot_in->link("HEAD_JOINT0") != NULL) robot_in->link("HEAD_JOINT0")->q = head_ref.rpy(y);
+    if( robot_in->link("HEAD_JOINT1") != NULL) robot_in->link("HEAD_JOINT1")->q = head_ref.rpy(p);
+    if(fik_in->ikp.count("rarm") && fik_in->ikp.count("larm")){
+        hrp::Vector3 base2rh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["rarm"].targetPos - robot_in->rootLink()->p);
+        hrp::Vector3 base2lh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["larm"].targetPos - robot_in->rootLink()->p);
+        if( robot_in->link("CHEST_JOINT0") != NULL){
+            robot_in->link("CHEST_JOINT0")->q = (base2lh(Z) - base2rh(Z)) * (10 * D2R / 1.0);
+            LIMIT_MINMAX(robot_in->link("CHEST_JOINT0")->q, robot_in->link("CHEST_JOINT0")->llimit, robot_in->link("CHEST_JOINT0")->ulimit);
+        }
+        if(robot_in->link("CHEST_JOINT2") != NULL){
+            robot_in->link("CHEST_JOINT2")->q = (base2rh(X) - base2lh(X)) * (60 * D2R / 1.0);
+            LIMIT_MINMAX(robot_in->link("CHEST_JOINT2")->q, robot_in->link("CHEST_JOINT2")->llimit, robot_in->link("CHEST_JOINT2")->ulimit);
+        }
     }
-  }
-  hrp::Vector3 tmp_com_err = hrp::Vector3::Zero();
-  struct timespec startT, endT;
-  while( 1 ){  //COM 収束ループ
-    com_ik_loop++;
-    robot_in->rootLink()->p += tmp_com_err;
-    robot_in->calcForwardKinematics();
-    clock_gettime(CLOCK_REALTIME, &startT);
-    for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_in->ikp.begin(); it != fik_in->ikp.end(); it++ ) {
-        if (it->second.is_ik_enable) fik_in->solveLimbIK (it->second, it->first, fik_in->ratio_for_vel, false);
+    hrp::Vector3 tmp_com_err = hrp::Vector3::Zero();
+    struct timespec startT, endT;
+    while( 1 ){  //COM 収束ループ
+        com_ik_loop++;
+        robot_in->rootLink()->p += tmp_com_err;
+        robot_in->calcForwardKinematics();
+        clock_gettime(CLOCK_REALTIME, &startT);
+        for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_in->ikp.begin(); it != fik_in->ikp.end(); it++ ) {
+            if (it->second.is_ik_enable) fik_in->solveLimbIK (it->second, it->first, fik_in->ratio_for_vel, false);
+        }
+        if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveLimbIK x 4" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
+        tmp_com_err = com_ref.p - robot_in->calcCM();
+        if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
+        if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
     }
-    if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveLimbIK x 4" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
-    tmp_com_err = com_ref.p - robot_in->calcCM();
-    if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
-    if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
-  }
-  if(com_ik_loop != 1 && DEBUGP)cout<<"com_ik_loop:"<<com_ik_loop<<" @ "<<debug_prefix<<endl;
+    if(com_ik_loop != 1 && DEBUGP)cout<<"com_ik_loop:"<<com_ik_loop<<" @ "<<debug_prefix<<endl;
 }
 #endif
 
 
 void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out){
-  hrp::dvector goal_state(ROBOT_ALL_DOF);
-  for(int i=0;i<robot_in->numJoints();i++){ goal_state(i) = robot_in->joint(i)->q; }
-  goal_state.bottomRows(6).topRows(3) = robot_in->rootLink()->p;
-  goal_state.bottomRows(6).bottomRows(3) = hrp::rpyFromRot(robot_in->rootLink()->R);
-
-  double goal_time = 0.0;
-  const double min_goal_time_offset = 0.1;
-//  const double avg_q_vel = 0.5;
-//  const double avg_q_vel = 1.0;
-  const double avg_q_vel = 1.5;
-  for(int i=0;i<robot_in->numJoints();i++){
-    double tmp_time = fabs(robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
-    if(tmp_time > goal_time){ goal_time = tmp_time; }
-  }
-  q_ip->setGoal(goal_state.data(), goal_time + min_goal_time_offset, true);
-  hrp::dvector ans_state(ROBOT_ALL_DOF);
-  double tmp[ROBOT_ALL_DOF];
-  if (!q_ip->isEmpty() ){  q_ip->get(tmp, true);}
-  ans_state = Eigen::Map<hrp::dvector>(tmp, ROBOT_ALL_DOF);
-  for(int i=0;i<robot_out->numJoints();i++){ robot_out->joint(i)->q = ans_state(i); }
-  robot_out->rootLink()->p = ans_state.bottomRows(6).topRows(3);
-  robot_out->rootLink()->R = hrp::rotFromRpy(ans_state.bottomRows(6).bottomRows(3));
-  robot_out->calcForwardKinematics();
+    hrp::dvector goal_state(ROBOT_ALL_DOF);
+    for(int i=0;i<robot_in->numJoints();i++){ goal_state(i) = robot_in->joint(i)->q; }
+    goal_state.bottomRows(6).topRows(3) = robot_in->rootLink()->p;
+    goal_state.bottomRows(6).bottomRows(3) = hrp::rpyFromRot(robot_in->rootLink()->R);
+
+    double goal_time = 0.0;
+    const double min_goal_time_offset = 0.1;
+    //  const double avg_q_vel = 0.5;
+    //  const double avg_q_vel = 1.0;
+    const double avg_q_vel = 1.5;
+    for(int i=0;i<robot_in->numJoints();i++){
+        double tmp_time = fabs(robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
+        if(tmp_time > goal_time){ goal_time = tmp_time; }
+    }
+    q_ip->setGoal(goal_state.data(), goal_time + min_goal_time_offset, true);
+    hrp::dvector ans_state(ROBOT_ALL_DOF);
+    double tmp[ROBOT_ALL_DOF];
+    if (!q_ip->isEmpty() ){  q_ip->get(tmp, true);}
+    ans_state = Eigen::Map<hrp::dvector>(tmp, ROBOT_ALL_DOF);
+    for(int i=0;i<robot_out->numJoints();i++){ robot_out->joint(i)->q = ans_state(i); }
+    robot_out->rootLink()->p = ans_state.bottomRows(6).topRows(3);
+    robot_out->rootLink()->R = hrp::rotFromRpy(ans_state.bottomRows(6).bottomRows(3));
+    robot_out->calcForwardKinematics();
 }
 
 
 bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double sec){//遅い
-  if(sec >= 0.0 && sec <= 30.0){
-    std::cerr << "[" << m_profile.instance_name << "] start Synchronization after "<<sec<<" [s]" << std::endl;
-    double remained_usec = sec * 10e6;
-    while (remained_usec > 0){
-      usleep(2000);
-      remained_usec -= 2000;
+    if(sec >= 0.0 && sec <= 30.0){
+        std::cerr << "[" << m_profile.instance_name << "] start Synchronization after "<<sec<<" [s]" << std::endl;
+        double remained_usec = sec * 10e6;
+        while (remained_usec > 0){
+            usleep(2000);
+            remained_usec -= 2000;
+        }
+        startWholeBodyMasterSlave();
+        return true;
+    }else{
+        std::cerr << "[" << m_profile.instance_name << "] Count Down Time must be 0 < T < 30 [s]"<< std::endl;
+        return false;
     }
-    startWholeBodyMasterSlave();
-    return true;
-  }else{
-    std::cerr << "[" << m_profile.instance_name << "] Count Down Time must be 0 < T < 30 [s]"<< std::endl;
-    return false;
-  }
 }
 
 
 bool WholeBodyMasterSlave::startWholeBodyMasterSlave(){
-  if(mode.now() == MODE_IDLE){
-    std::cerr << "[" << m_profile.instance_name << "] startWholeBodyMasterSlave" << std::endl;
-    mode.setModeRequest(MODE_SYNC_TO_WBMS);
-    while(!transition_interpolator->isEmpty()){ usleep(1000); }
-    return true;
-  }else{
-    std::cerr << "[" << m_profile.instance_name << "] Invalid context to startWholeBodyMasterSlave" << std::endl;
-    return false;
-  }
+    if(mode.now() == MODE_IDLE){
+        std::cerr << "[" << m_profile.instance_name << "] startWholeBodyMasterSlave" << std::endl;
+        mode.setModeRequest(MODE_SYNC_TO_WBMS);
+        while(!transition_interpolator->isEmpty()){ usleep(1000); }
+        return true;
+    }else{
+        std::cerr << "[" << m_profile.instance_name << "] Invalid context to startWholeBodyMasterSlave" << std::endl;
+        return false;
+    }
 }
 
 
 bool WholeBodyMasterSlave::pauseWholeBodyMasterSlave(){
-  if(mode.now() == MODE_WBMS){
-    std::cerr << "[" << m_profile.instance_name << "] pauseWholeBodyMasterSlave" << std::endl;
-    mode.setModeRequest(MODE_PAUSE);
-    return true;
-  }else{
-    std::cerr << "[" << m_profile.instance_name << "] Invalid context to pauseWholeBodyMasterSlave" << std::endl;
-    return false;
-  }
+    if(mode.now() == MODE_WBMS){
+        std::cerr << "[" << m_profile.instance_name << "] pauseWholeBodyMasterSlave" << std::endl;
+        mode.setModeRequest(MODE_PAUSE);
+        return true;
+    }else{
+        std::cerr << "[" << m_profile.instance_name << "] Invalid context to pauseWholeBodyMasterSlave" << std::endl;
+        return false;
+    }
 }
 
 
 bool WholeBodyMasterSlave::resumeWholeBodyMasterSlave(){
-  if(mode.now() == MODE_PAUSE){
-    std::cerr << "[" << m_profile.instance_name << "] pauseWholeBodyMasterSlave" << std::endl;
-    mode.setModeRequest(MODE_WBMS);
-    return true;
-  }else{
-    std::cerr << "[" << m_profile.instance_name << "] Invalid context to pauseWholeBodyMasterSlave" << std::endl;
-    return false;
-  }
+    if(mode.now() == MODE_PAUSE){
+        std::cerr << "[" << m_profile.instance_name << "] pauseWholeBodyMasterSlave" << std::endl;
+        mode.setModeRequest(MODE_WBMS);
+        return true;
+    }else{
+        std::cerr << "[" << m_profile.instance_name << "] Invalid context to pauseWholeBodyMasterSlave" << std::endl;
+        return false;
+    }
 }
 
 
 bool WholeBodyMasterSlave::stopWholeBodyMasterSlave(){
-  if(mode.now() == MODE_WBMS || mode.now() == MODE_PAUSE ){
-    std::cerr << "[" << m_profile.instance_name << "] stopWholeBodyMasterSlave" << std::endl;
-    mode.setModeRequest(MODE_SYNC_TO_IDLE);
-    while(!transition_interpolator->isEmpty()){ usleep(1000); }
-    return true;
-  }else{
-    std::cerr << "[" << m_profile.instance_name << "] Invalid context to stopWholeBodyMasterSlave" << std::endl;
-    return false;
-  }
+    if(mode.now() == MODE_WBMS || mode.now() == MODE_PAUSE ){
+        std::cerr << "[" << m_profile.instance_name << "] stopWholeBodyMasterSlave" << std::endl;
+        mode.setModeRequest(MODE_SYNC_TO_IDLE);
+        while(!transition_interpolator->isEmpty()){ usleep(1000); }
+        return true;
+    }else{
+        std::cerr << "[" << m_profile.instance_name << "] Invalid context to stopWholeBodyMasterSlave" << std::endl;
+        return false;
+    }
 }
 
 
 bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
-  std::cerr << "[" << m_profile.instance_name << "] setWholeBodyMasterSlaveParam" << std::endl;
-  hsp->WBMSparam.auto_swing_foot_landing_threshold = i_param.auto_swing_foot_landing_threshold;
-  hsp->WBMSparam.foot_vertical_vel_limit_coeff = i_param.foot_vertical_vel_limit_coeff;
-  hsp->WBMSparam.human_com_height = i_param.human_com_height;
-  hsp->WBMSparam.is_doctor = i_param.is_doctor;
-  hsp->WBMSparam.set_com_height_fix = i_param.set_com_height_fix;
-  hsp->WBMSparam.set_com_height_fix_val = i_param.set_com_height_fix_val;
-  hsp->WBMSparam.swing_foot_height_offset = i_param.swing_foot_height_offset;
-  hsp->WBMSparam.swing_foot_max_height = i_param.swing_foot_max_height;
-  hsp->WBMSparam.upper_body_rmc_ratio = i_param.upper_body_rmc_ratio;
-  hsp->WBMSparam.use_rh = hsp->WBMSparam.use_lh = i_param.use_hands;
-  hsp->WBMSparam.use_head = i_param.use_head;
-  hsp->WBMSparam.use_manipulability_limit = i_param.use_manipulability_limit;
-  return true;
+    std::cerr << "[" << m_profile.instance_name << "] setWholeBodyMasterSlaveParam" << std::endl;
+    hsp->WBMSparam.auto_swing_foot_landing_threshold = i_param.auto_swing_foot_landing_threshold;
+    hsp->WBMSparam.foot_vertical_vel_limit_coeff = i_param.foot_vertical_vel_limit_coeff;
+    hsp->WBMSparam.human_com_height = i_param.human_com_height;
+    hsp->WBMSparam.is_doctor = i_param.is_doctor;
+    hsp->WBMSparam.set_com_height_fix = i_param.set_com_height_fix;
+    hsp->WBMSparam.set_com_height_fix_val = i_param.set_com_height_fix_val;
+    hsp->WBMSparam.swing_foot_height_offset = i_param.swing_foot_height_offset;
+    hsp->WBMSparam.swing_foot_max_height = i_param.swing_foot_max_height;
+    hsp->WBMSparam.upper_body_rmc_ratio = i_param.upper_body_rmc_ratio;
+    hsp->WBMSparam.use_rh = hsp->WBMSparam.use_lh = i_param.use_hands;
+    hsp->WBMSparam.use_head = i_param.use_head;
+    hsp->WBMSparam.use_manipulability_limit = i_param.use_manipulability_limit;
+    return true;
 }
 
 
 bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
-  std::cerr << "[" << m_profile.instance_name << "] getWholeBodyMasterSlaveParam" << std::endl;
-  i_param.auto_swing_foot_landing_threshold = hsp->WBMSparam.auto_swing_foot_landing_threshold;
-  i_param.foot_vertical_vel_limit_coeff = hsp->WBMSparam.foot_vertical_vel_limit_coeff;
-  i_param.human_com_height = hsp->WBMSparam.human_com_height;
-  i_param.is_doctor = hsp->WBMSparam.is_doctor;
-  i_param.set_com_height_fix = hsp->WBMSparam.set_com_height_fix;
-  i_param.set_com_height_fix_val = hsp->WBMSparam.set_com_height_fix_val;
-  i_param.swing_foot_height_offset = hsp->WBMSparam.swing_foot_height_offset;
-  i_param.swing_foot_max_height = hsp->WBMSparam.swing_foot_max_height;
-  i_param.upper_body_rmc_ratio = hsp->WBMSparam.upper_body_rmc_ratio;
-  i_param.use_hands = (hsp->WBMSparam.use_rh || hsp->WBMSparam.use_lh);
-  i_param.use_head = hsp->WBMSparam.use_head;
-  i_param.use_manipulability_limit = hsp->WBMSparam.use_manipulability_limit;
-  return true;
+    std::cerr << "[" << m_profile.instance_name << "] getWholeBodyMasterSlaveParam" << std::endl;
+    i_param.auto_swing_foot_landing_threshold = hsp->WBMSparam.auto_swing_foot_landing_threshold;
+    i_param.foot_vertical_vel_limit_coeff = hsp->WBMSparam.foot_vertical_vel_limit_coeff;
+    i_param.human_com_height = hsp->WBMSparam.human_com_height;
+    i_param.is_doctor = hsp->WBMSparam.is_doctor;
+    i_param.set_com_height_fix = hsp->WBMSparam.set_com_height_fix;
+    i_param.set_com_height_fix_val = hsp->WBMSparam.set_com_height_fix_val;
+    i_param.swing_foot_height_offset = hsp->WBMSparam.swing_foot_height_offset;
+    i_param.swing_foot_max_height = hsp->WBMSparam.swing_foot_max_height;
+    i_param.upper_body_rmc_ratio = hsp->WBMSparam.upper_body_rmc_ratio;
+    i_param.use_hands = (hsp->WBMSparam.use_rh || hsp->WBMSparam.use_lh);
+    i_param.use_head = hsp->WBMSparam.use_head;
+    i_param.use_manipulability_limit = hsp->WBMSparam.use_manipulability_limit;
+    return true;
 }
 
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index e4d4df06496..49113a65d02 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -27,36 +27,36 @@ using namespace RTC;
 enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE};
 
 class ControlMode{
-  private:
-    mode_enum current_mode, previous_mode, requested_mode;
-  public:
-    ControlMode(){ current_mode = previous_mode = requested_mode = MODE_IDLE;}
-    ~ControlMode(){}
-    bool setModeRequest(mode_enum in){
-      switch(in){
-        case MODE_SYNC_TO_WBMS:
-          if(current_mode == MODE_IDLE){ requested_mode = MODE_SYNC_TO_WBMS; return true; }else{ return false; }
-        case MODE_WBMS:
-          if(current_mode == MODE_SYNC_TO_WBMS || current_mode == MODE_PAUSE ){ requested_mode = MODE_WBMS; return true; }else{ return false; }
-        case MODE_PAUSE:
-          if(current_mode == MODE_WBMS){ requested_mode = MODE_PAUSE; return true; }else{ return false; }
-        case MODE_SYNC_TO_IDLE:
-          if(current_mode == MODE_WBMS || current_mode == MODE_PAUSE ){ requested_mode = MODE_SYNC_TO_IDLE; return true; }else{ return false; }
-        case MODE_IDLE:
-          if(current_mode == MODE_SYNC_TO_IDLE ){ requested_mode = MODE_IDLE; return true; }else{ return false; }
-        default:
-          return false;
-      }
-    }
-    void update(){ previous_mode = current_mode; current_mode = requested_mode; }
-    mode_enum now(){ return current_mode; }
-    mode_enum pre(){ return previous_mode; }
-    bool isRunning(){ return (current_mode==MODE_SYNC_TO_WBMS) || (current_mode==MODE_WBMS) || (current_mode==MODE_PAUSE) || (current_mode==MODE_SYNC_TO_IDLE) ;}
-    bool isInitialize(){ return (previous_mode==MODE_IDLE) && (current_mode==MODE_SYNC_TO_WBMS) ;}
+    private:
+        mode_enum current_mode, previous_mode, requested_mode;
+    public:
+        ControlMode(){ current_mode = previous_mode = requested_mode = MODE_IDLE;}
+        ~ControlMode(){}
+        bool setModeRequest(mode_enum in){
+            switch(in){
+                case MODE_SYNC_TO_WBMS:
+                    if(current_mode == MODE_IDLE){ requested_mode = MODE_SYNC_TO_WBMS; return true; }else{ return false; }
+                case MODE_WBMS:
+                    if(current_mode == MODE_SYNC_TO_WBMS || current_mode == MODE_PAUSE ){ requested_mode = MODE_WBMS; return true; }else{ return false; }
+                case MODE_PAUSE:
+                    if(current_mode == MODE_WBMS){ requested_mode = MODE_PAUSE; return true; }else{ return false; }
+                case MODE_SYNC_TO_IDLE:
+                    if(current_mode == MODE_WBMS || current_mode == MODE_PAUSE ){ requested_mode = MODE_SYNC_TO_IDLE; return true; }else{ return false; }
+                case MODE_IDLE:
+                    if(current_mode == MODE_SYNC_TO_IDLE ){ requested_mode = MODE_IDLE; return true; }else{ return false; }
+                default:
+                    return false;
+            }
+        }
+        void update(){ previous_mode = current_mode; current_mode = requested_mode; }
+        mode_enum now(){ return current_mode; }
+        mode_enum pre(){ return previous_mode; }
+        bool isRunning(){ return (current_mode==MODE_SYNC_TO_WBMS) || (current_mode==MODE_WBMS) || (current_mode==MODE_PAUSE) || (current_mode==MODE_SYNC_TO_IDLE) ;}
+        bool isInitialize(){ return (previous_mode==MODE_IDLE) && (current_mode==MODE_SYNC_TO_WBMS) ;}
 };
 
 class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
-  public:
+    public:
     WholeBodyMasterSlave(RTC::Manager* manager);
     virtual ~WholeBodyMasterSlave();
     virtual RTC::ReturnCode_t onInitialize();
@@ -72,7 +72,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
     bool setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
     bool getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
 
-  protected:
+    protected:
     TimedDoubleSeq m_qRef;
     InPort<TimedDoubleSeq> m_qRefIn;
     TimedPoint3D m_basePos;
@@ -155,68 +155,68 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
 
     WholeBodyMasterSlaveService_impl m_service0;
 
- private:
-  struct ABCIKparam {
-    hrp::Vector3 target_p0, localPos, adjust_interpolation_target_p0, adjust_interpolation_org_p0;
-    hrp::Matrix33 target_r0, localR, adjust_interpolation_target_r0, adjust_interpolation_org_r0;
-    hrp::Link* target_link;
-    bool is_active, has_toe_joint;
-  };
-//  typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
-//  typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
-  typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
-  fikPtr fik, fik_ml;
-  hrp::BodyPtr m_robot, m_robot_ml, m_robot_vsafe;
-  std::vector<fikPtr> fik_list;
-  std::vector<hrp::BodyPtr> body_list;
-  std::map<std::string, IKConstraintParam> eename_ikcp_map;
-  std::map<std::string, size_t> contact_states_index_map;
-  double m_dt;
-
-  double transition_interpolator_ratio;
-  interpolator *transition_interpolator;
-  int ROBOT_ALL_DOF;
-  interpolator *q_ip;
-  hrp::dvector init_sync_state;
-
-  int optionalDataLength;
-  unsigned int m_debugLevel;
-  bool is_legged_robot;
-  unsigned int loop;
-
-  hrp::InvDynStateBuffer idsb;
-  BiquadIIRFilterVec invdyn_zmp_filters;
-
-  BiquadIIRFilterVec final_ref_zmp_filter;
-
-  IIRFilter ref_ee_vel_v_filter;
-
-  HumanPose raw_pose;
-
-  boost::shared_ptr<WBMSCore> hsp;
-
-  hrp::Vector3 torso_rot_rmc;
-  ControlMode mode;
-
-//  enum { MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE} mode, previous_mode;
-//  bool isRunning(){ return (mode==MODE_SYNC_TO_WBMS) || (mode==MODE_WBMS) || (mode==MODE_PAUSE) || (mode==MODE_SYNC_TO_IDLE) ;}
-//  bool isInitialize(){ return (previous_mode==MODE_IDLE) && (mode==MODE_SYNC_TO_WBMS) ;}
-//  enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE};
-
-  void setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in);
-  void calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robot_in);
-  void calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::Vector3 target_v_old[], WBMSPose3D& rf_ref, WBMSPose3D& lf_ref, WBMSPose3D& rh_ref, WBMSPose3D& lh_ref);
-  void calcManipulabilityJointLimitForWBMS(fikPtr& fik_in, hrp::BodyPtr& robot_in);
-  void solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix="");
-  void processTransition();
-  void preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in);
-  void processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HumanPose& pose_ref);
-  void processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::BodyPtr& robot_in, HumanPose& pose_ref);
-  void processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_normal_in, const HumanPose& pose_ref);
-  void processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out);
-  bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; };
-
-//  void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const hrp::Vector3& max_acc, const double& max_vel, hrp::Vector3& pos_ans);
+    private:
+    struct ABCIKparam {
+        hrp::Vector3 target_p0, localPos, adjust_interpolation_target_p0, adjust_interpolation_org_p0;
+        hrp::Matrix33 target_r0, localR, adjust_interpolation_target_r0, adjust_interpolation_org_r0;
+        hrp::Link* target_link;
+        bool is_active, has_toe_joint;
+    };
+    //  typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
+    //  typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
+    typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
+    fikPtr fik, fik_ml;
+    hrp::BodyPtr m_robot, m_robot_ml, m_robot_vsafe;
+    std::vector<fikPtr> fik_list;
+    std::vector<hrp::BodyPtr> body_list;
+    std::map<std::string, IKConstraint> eename_ikcp_map;
+    std::map<std::string, size_t> contact_states_index_map;
+    double m_dt;
+
+    double transition_interpolator_ratio;
+    interpolator *transition_interpolator;
+    int ROBOT_ALL_DOF;
+    interpolator *q_ip;
+    hrp::dvector init_sync_state;
+
+    int optionalDataLength;
+    unsigned int m_debugLevel;
+    bool is_legged_robot;
+    unsigned int loop;
+
+    hrp::InvDynStateBuffer idsb;
+    BiquadIIRFilterVec invdyn_zmp_filters;
+
+    BiquadIIRFilterVec final_ref_zmp_filter;
+
+    IIRFilter ref_ee_vel_v_filter;
+
+    HumanPose raw_pose;
+
+    boost::shared_ptr<WBMSCore> hsp;
+
+    hrp::Vector3 torso_rot_rmc;
+    ControlMode mode;
+
+    //  enum { MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE} mode, previous_mode;
+    //  bool isRunning(){ return (mode==MODE_SYNC_TO_WBMS) || (mode==MODE_WBMS) || (mode==MODE_PAUSE) || (mode==MODE_SYNC_TO_IDLE) ;}
+    //  bool isInitialize(){ return (previous_mode==MODE_IDLE) && (mode==MODE_SYNC_TO_WBMS) ;}
+    //  enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE};
+
+    void setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in);
+    void calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robot_in);
+    void calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::Vector3 target_v_old[], WBMSPose3D& rf_ref, WBMSPose3D& lf_ref, WBMSPose3D& rh_ref, WBMSPose3D& lh_ref);
+    void calcManipulabilityJointLimitForWBMS(fikPtr& fik_in, hrp::BodyPtr& robot_in);
+    void solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix="");
+    void processTransition();
+    void preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in);
+    void processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HumanPose& pose_ref);
+    void processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::BodyPtr& robot_in, HumanPose& pose_ref);
+    void processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_normal_in, const HumanPose& pose_ref);
+    void processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out);
+    bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; };
+
+    //  void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const hrp::Vector3& max_acc, const double& max_vel, hrp::Vector3& pos_ans);
 };
 
 
@@ -226,7 +226,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
 
 extern "C"
 {
-  void WholeBodyMasterSlaveInit(RTC::Manager* manager);
+    void WholeBodyMasterSlaveInit(RTC::Manager* manager);
 };
 
 #endif // WholeBodyMasterSlave_H
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 281f68c0287..367234f8361 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -41,875 +41,877 @@ extern "C" {
 
 #define DEBUG 0
 #define dbg(var) std::cout<<#var"= "<<(var)<<std::endl
+#define dbgn(var) std::cout<<#var"= "<<std::endl<<(var)<<std::endl
 #define LIMIT_MIN(x,min) (x= ( x<min ? min:x ))
 #define LIMIT_MAX(x,max) (x= ( x<max ? x:max ))
 #define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
 #define SGN(x) ((x)>=0 ? 1 : -1)
 
 class UTIL_CONST{
-  public:
-    enum { com, rf, lf, rh, lh, head, zmp, num_pose_tgt };
-    enum { num_ee_tgt=4 };
-    enum { R, L, LR };
-    enum { X, Y, Z, XYZ };
-    enum { XY = 2 };
-    enum { r, p, y, rpy };
-    enum { fx, fy, fz, tx, ty, tz, ft_xyz };
-    enum { MIN, MAX, MINMAX };
-    double G, D2R, INFMIN, INFMAX, Q_BUTTERWORTH, Q_NOOVERSHOOT;
-    UTIL_CONST() :
-      G(9.80665),
-      D2R( M_PI/180.0),
-      Q_BUTTERWORTH(0.707106781),
-      Q_NOOVERSHOOT(0.5),
-      INFMIN( - std::numeric_limits<double>::max()),
-      INFMAX( + std::numeric_limits<double>::max())
-    {};
+    public:
+        enum { com, rf, lf, rh, lh, head, zmp, num_pose_tgt };
+        enum { num_ee_tgt=4 };
+        enum { R, L, LR };
+        enum { X, Y, Z, XYZ };
+        enum { XY = 2 };
+        enum { r, p, y, rpy };
+        enum { fx, fy, fz, tx, ty, tz, ft_xyz };
+        enum { MIN, MAX, MINMAX };
+        double G, D2R, INFMIN, INFMAX, Q_BUTTERWORTH, Q_NOOVERSHOOT;
+        UTIL_CONST() :
+            G(9.80665),
+            D2R( M_PI/180.0),
+            Q_BUTTERWORTH(0.707106781),
+            Q_NOOVERSHOOT(0.5),
+            INFMIN( - std::numeric_limits<double>::max()),
+            INFMAX( + std::numeric_limits<double>::max())
+        {};
 };
 
 class BiquadIIRFilterVec : UTIL_CONST {
-  private:
-    IIRFilter filters[XYZ];
-    hrp::Vector3 ans;
-  public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    BiquadIIRFilterVec(){}
-    ~BiquadIIRFilterVec(){}
-    void setParameter(const hrp::Vector3& fc_in, const double& HZ, const double& Q = 0.5){ for(int i=0;i<XYZ;i++){ filters[i].setParameterAsBiquad((double)fc_in(i), Q, HZ); } }
-    void setParameter(const double& fc_in, const double& HZ, const double& Q = 0.5){ setParameter(hrp::Vector3(fc_in,fc_in,fc_in), HZ, Q); }//overload
-    hrp::Vector3 passFilter(const hrp::Vector3& input){ for(int i=0;i<XYZ;i++){ ans(i) = filters[i].passFilter((double)input(i)); } return ans; }
-    void reset(const hrp::Vector3& initial_input){ for(int i=0;i<XYZ;i++){ filters[i].reset((double)initial_input(i));} }
+    private:
+        IIRFilter filters[XYZ];
+        hrp::Vector3 ans;
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+        BiquadIIRFilterVec(){}
+        ~BiquadIIRFilterVec(){}
+        void setParameter(const hrp::Vector3& fc_in, const double& HZ, const double& Q = 0.5){ for(int i=0;i<XYZ;i++){ filters[i].setParameterAsBiquad((double)fc_in(i), Q, HZ); } }
+        void setParameter(const double& fc_in, const double& HZ, const double& Q = 0.5){ setParameter(hrp::Vector3(fc_in,fc_in,fc_in), HZ, Q); }//overload
+        hrp::Vector3 passFilter(const hrp::Vector3& input){ for(int i=0;i<XYZ;i++){ ans(i) = filters[i].passFilter((double)input(i)); } return ans; }
+        void reset(const hrp::Vector3& initial_input){ for(int i=0;i<XYZ;i++){ filters[i].reset((double)initial_input(i));} }
 };
 
 class WBMSPose3D{
-  public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    hrp::Vector3 p,rpy;
-    WBMSPose3D(){ clear(); }
-    ~WBMSPose3D(){}
-    void clear(){ p = rpy = hrp::Vector3::Zero(); }
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+        hrp::Vector3 p,rpy;
+        WBMSPose3D(){ clear(); }
+        ~WBMSPose3D(){}
+        void clear(){ p = rpy = hrp::Vector3::Zero(); }
 };
 
 class PoseTGT{
-  public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    WBMSPose3D abs, offs, cnt;
-    hrp::dvector6 w;
-    bool is_contact, go_contact;
-    PoseTGT(){ clear(); }
-    ~PoseTGT(){}
-    void clear(){ abs.clear(); offs.clear(); cnt.clear(); w=hrp::dvector6::Zero(); is_contact = go_contact = false;}
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+        WBMSPose3D abs, offs, cnt;
+        hrp::dvector6 w;
+        bool is_contact, go_contact;
+        PoseTGT(){ clear(); }
+        ~PoseTGT(){}
+        void clear(){ abs.clear(); offs.clear(); cnt.clear(); w=hrp::dvector6::Zero(); is_contact = go_contact = false;}
 };
 
 class HumanPose : UTIL_CONST {
-  public:
-    std::vector<PoseTGT> tgt;
-    HumanPose(){ tgt.resize(num_pose_tgt); }
-    ~HumanPose(){cerr<<"HumanPose destructed"<<endl;}
-    void clear(){
-      for(std::vector<PoseTGT>::iterator it = tgt.begin(); it != tgt.end(); it++){ it->clear(); }
-    }
-    static void hp_printf(const HumanPose& in) {
-      const std::string pcatgt[] = {"c","rf","lf","rh","lh","hd","z"}, wcatgt[] = {"rw","lw"};
-      for(int i=0;i<num_pose_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",pcatgt[i].c_str(),in.tgt[i].abs.p(X),in.tgt[i].abs.p(Y),in.tgt[i].abs.p(Z)); }
-//      for(int i=0;i<num_wrench_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",wcatgt[i].c_str(),in.w[i](fx),in.w[i](fy),in.w[i](fz),in.w[i](tx),in.w[i](ty),in.w[i](tz)); }
-      printf("\n");
-    }
-    void print() const { hp_printf(*this); }
+    public:
+        std::vector<PoseTGT> tgt;
+        HumanPose(){ tgt.resize(num_pose_tgt); }
+        ~HumanPose(){cerr<<"HumanPose destructed"<<endl;}
+        void clear(){
+            for(std::vector<PoseTGT>::iterator it = tgt.begin(); it != tgt.end(); it++){ it->clear(); }
+        }
+        static void hp_printf(const HumanPose& in) {
+            const std::string pcatgt[] = {"c","rf","lf","rh","lh","hd","z"}, wcatgt[] = {"rw","lw"};
+            for(int i=0;i<num_pose_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",pcatgt[i].c_str(),in.tgt[i].abs.p(X),in.tgt[i].abs.p(Y),in.tgt[i].abs.p(Z)); }
+            //      for(int i=0;i<num_wrench_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",wcatgt[i].c_str(),in.w[i](fx),in.w[i](fy),in.w[i](fz),in.w[i](tx),in.w[i](ty),in.w[i](tz)); }
+            printf("\n");
+        }
+        void print() const { hp_printf(*this); }
 };
 
 class WBMSCore : UTIL_CONST {
-  private:
-    double CNT_F_TH, h2r_ratio, tgt_h2r_ratio, HZ, DT;
-    struct timeval t_calc_start, t_calc_end;
-    BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters, com_in_filter;
-    hrp::Vector3 com_old, com_oldold, comacc, com_CP_ref_old, r_zmp_raw;
-    std::vector<hrp::Vector2> rflf_points, hull_com, hull_dcp, hull_acp;
-    std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rot_filters;
-    std::vector<cv::Point2f> points,cvhull;
-//    RobotConfig rc;
-    HumanPose rp_ref_out_old, hp_swap_checked;
-    unsigned int loop;
-    bool is_initial_loop;
-    bool cp_force_go_contact[LR];
-  public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    double H_cur;
-    HumanPose hp_wld_raw, hp_plot, rp_ref_out, rp_ref_vel_old;
-    FILE *sr_log, *cz_log, *id_log;
-    WBMSPose3D baselinkpose;
-//    WBMSPose3D ee_contact_pose[4];
-    hrp::Vector3 com_vel_old,rh_vel_old, cp_dec, cp_acc;
-    std::vector<hrp::Vector3> foot_vert_act[LR], foot_vert_safe[LR], foot_vert_check[LR];
-    hrp::Vector2 com_forcp_ref,com_vel_forcp_ref;
-    hrp::dvector6 invdyn_ft;
-    hrp::Vector4 foot_vert_act_fblr[LR], foot_vert_safe_fblr[LR], foot_vert_check_fblr[LR];
-    struct WBMSparameters {
-      double auto_swing_foot_landing_threshold;
-      double foot_vertical_vel_limit_coeff;
-      double human_com_height;
-      bool is_doctor;
-      bool set_com_height_fix;
-      double set_com_height_fix_val;
-      double swing_foot_height_offset;
-      double swing_foot_max_height;
-      double upper_body_rmc_ratio;
-      bool use_rh,use_lh;
-      bool use_head;
-      bool use_manipulability_limit;
-    };
-    struct WBMSparameters WBMSparam;
-    struct ActualRobotState {
-        hrp::Vector3 com, zmp;
-    }act_rs;
-
-    typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
-    fikPtr fik_ml;
-    hrp::BodyPtr m_robot_ml;
-//    fikPtr fik_act;
-    hrp::BodyPtr m_robot_act;
-    double cur_manip_val[4][3];
-    hrp::Vector3 manip_direc[4][3];
-    hrp::Matrix33 manip_mat[4];
-    hrp::Vector3 manip_sv[4];
-    hrp::Matrix33 manip_mat_rot[4];
-    hrp::Vector3 manip_sv_rot[4];
-
-    hrp::Vector2 cp_acc_old;
-//    WBMSPose3D ee_pose_old[4];
-
-    WBMSCore(const double& dt){
-      tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
-//      tgt_h2r_ratio = h2r_ratio = 1.0;
-      DT = dt;
-      HZ = (int)(1.0/DT);
-      CNT_F_TH = 20.0;
-      loop = 0;
-      //////////  hrp::Vector は初期化必要  ///////////
-      com_old = com_oldold = comacc = hrp::Vector3::Zero();
-      r_zmp_raw = hrp::Vector3::Zero();
-      com_vel_old = hrp::Vector3::Zero();
-      rh_vel_old = hrp::Vector3::Zero();
-      cp_dec = cp_acc = hrp::Vector3::Zero();
-      invdyn_ft = hrp::dvector6::Zero();
-      tgt_pos_filters.resize(num_pose_tgt);
-      tgt_rot_filters.resize(num_pose_tgt);
-      for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(1.0, HZ, Q_NOOVERSHOOT);//四肢拘束点用(position)
-      for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0, HZ, Q_NOOVERSHOOT);//四肢拘束点用(Rotation)
-      tgt_pos_filters[com].setParameter(1.0, HZ, Q_NOOVERSHOOT);//重心pos用
-      tgt_rot_filters[com].setParameter(0.6, HZ, Q_NOOVERSHOOT);//重心rot用
-      tgt_pos_filters[rf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, Q_NOOVERSHOOT);//右足pos用
-      tgt_pos_filters[lf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, Q_NOOVERSHOOT);//左足pos用
-      calcacc_v_filters.setParameter(5, HZ, Q_BUTTERWORTH);//加速度計算用
-      acc4zmp_v_filters.setParameter(5, HZ, Q_BUTTERWORTH);//ZMP生成用ほぼこの値でいい
-      com_in_filter.setParameter(1, HZ);
-      foot_vert_act_fblr[R] << 0.13, -0.10,  0.06, -0.08;
-      foot_vert_act_fblr[L] << 0.13, -0.10,  0.08, -0.06;
-      foot_vert_check_fblr[R] << 0.02+0.002, -0.01-0.002,  0.02+0.002,  0.01-0.002;
-      foot_vert_check_fblr[L] << 0.02+0.002, -0.01-0.002, -0.01+0.002, -0.02-0.002;
-//      foot_vert_check_fblr[R] << 0.13, -0.10,  0.06, -0.08;
-//      foot_vert_check_fblr[L] << 0.13, -0.10,  0.08, -0.06;
-      foot_vert_safe_fblr[R] << 0.02, -0.01,  0.02,  0.01;
-      foot_vert_safe_fblr[L] << 0.02, -0.01, -0.01, -0.02;
-      for(int i=0;i<LR;i++){
-        make_rect_verts(foot_vert_act_fblr[i], foot_vert_act[i]);
-        make_rect_verts(foot_vert_check_fblr[i], foot_vert_check[i]);
-        make_rect_verts(foot_vert_safe_fblr[i], foot_vert_safe[i]);
-      }
-      cp_force_go_contact[R] = false;
-      cp_force_go_contact[L] = false;
-
-//      WBMSparam.auto_swing_foot_landing_threshold = 0.02;
-      WBMSparam.auto_swing_foot_landing_threshold = 0.04;
-      WBMSparam.foot_vertical_vel_limit_coeff = 4.0;
-      WBMSparam.human_com_height = 1.00;
-      WBMSparam.is_doctor = true;
-      WBMSparam.set_com_height_fix = false;
-      WBMSparam.set_com_height_fix_val = 0.02;
-      WBMSparam.swing_foot_height_offset = 0.01;
-      WBMSparam.swing_foot_max_height = 0.5;
-      WBMSparam.upper_body_rmc_ratio = 0.0;
-      WBMSparam.use_rh = WBMSparam.use_lh = true;
-      WBMSparam.use_head = true;
-      WBMSparam.use_manipulability_limit = true;
-      rp_ref_out_old.clear();
-      rp_ref_out.clear();
-      rp_ref_vel_old.clear();
-      rflf_points.reserve(8);//あらかじめreserveして高速化
-      points.reserve(8);
-      hull_com.reserve(6);
-      hull_dcp.reserve(6);
-      hull_dcp.reserve(6);
-      cvhull.reserve(6);
-      if(DEBUG){
-        sr_log = fopen("/home/ishiguro/HumanSync_support_region.log","w+");
-        cz_log = fopen("/home/ishiguro/HumanSync_com_zmp.log","w+");
-        id_log = fopen("/home/ishiguro/HumanSync_invdyn.log","w+");
-      }
-      is_initial_loop = true;
-      cout<<"WBMSCore constructed"<<endl;
-    }
-    static void make_rect_verts(const hrp::Vector4& ForwardBackLeftRight, std::vector<hrp::Vector3>& out){
-      out.clear();
-      out.push_back(hrp::Vector3(ForwardBackLeftRight(0), ForwardBackLeftRight(2),  0));//左前
-      out.push_back(hrp::Vector3(ForwardBackLeftRight(1), ForwardBackLeftRight(2),  0));//左後
-      out.push_back(hrp::Vector3(ForwardBackLeftRight(1), ForwardBackLeftRight(3),  0));//右後
-      out.push_back(hrp::Vector3(ForwardBackLeftRight(0), ForwardBackLeftRight(3),  0));//右前
-    }
-    ~WBMSCore(){
-      if(DEBUG)fclose(sr_log);
-      if(DEBUG)fclose(cz_log);
-      if(DEBUG)fclose(id_log);
-      cout<<"WBMSCore destructed"<<endl;
-    }
-    double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6; }
-    void initializeHumanPoseFromCurrentInput(){
-      std::string ns[7] = {"com","rf","lf","rh","lh","zmp","head"};
-      for(int i=0;i<7;i++){
-        hp_wld_raw.tgt[i].offs.p   =  hp_wld_raw.tgt[i].abs.p;
-        hp_wld_raw.tgt[i].offs.rpy =  hp_wld_raw.tgt[i].abs.rpy;
-      }
-    }
-    void initializeRobotPoseFromHRPBody(fikPtr& fik_in, hrp::BodyPtr& robot_in){
-      const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
-      const int human_l_names[4] = {rf,lf,rh,lh};
-      for(int i=0;i<4;i++){//HumanSynchronizerの初期姿勢オフセットをセット
-        if(fik_in->ikp.count(robot_l_names[i])){
-          rp_ref_out.tgt[human_l_names[i]].offs.p = fik_in->getEndEffectorPos(robot_l_names[i]);
-          rp_ref_out.tgt[human_l_names[i]].offs.rpy = hrp::rpyFromRot(fik_in->getEndEffectorRot(robot_l_names[i]));
-          rp_ref_out.tgt[human_l_names[i]].abs = rp_ref_out.tgt[human_l_names[i]].offs;
-        }
-      }
-      rp_ref_out.tgt[com].offs.p = act_rs.com = act_rs.zmp = robot_in->calcCM();
-      com_CP_ref_old = rp_ref_out.tgt[com].offs.p;
-      rp_ref_out.tgt[com].offs.rpy = hrp::rpyFromRot(robot_in->rootLink()->R);
-      rp_ref_out.tgt[zmp].offs.p(X) = rp_ref_out.tgt[com].offs.p(X);
-      rp_ref_out.tgt[zmp].offs.p(Y) = rp_ref_out.tgt[com].offs.p(Y);
-      rp_ref_out.tgt[zmp].offs.p(Z) = (fik_in->getEndEffectorPos("rleg")(Z)+fik_in->getEndEffectorPos("lleg")(Z))/2;
-      rp_ref_out.tgt[rf].cnt = rp_ref_out.tgt[rf].offs;
-      rp_ref_out.tgt[lf].cnt = rp_ref_out.tgt[lf].offs;
-      baselinkpose.p = robot_in->rootLink()->p;
-      baselinkpose.rpy = hrp::rpyFromRot(robot_in->rootLink()->R);
-      for(int i=0, l[LR]={rf,lf}; i<LR; i++){
-        rp_ref_out.tgt[l[i]].is_contact = true;
-      }
-      H_cur = rp_ref_out.tgt[com].offs.p(Z) - std::min((double)rp_ref_out.tgt[rf].offs.p(Z), (double)rp_ref_out.tgt[rf].offs.p(Z));
-    }
-    void initializeRequest(fikPtr& fik_in, hrp::BodyPtr& robot_in){
-      loop = 0;
-      is_initial_loop = true;
-      initializeHumanPoseFromCurrentInput();
-      initializeRobotPoseFromHRPBody(fik_in, robot_in);
-      rp_ref_out_old = rp_ref_vel_old = rp_ref_out;
-    }
-    void update(){//////////  メインループ  ////////////
-      gettimeofday(&t_calc_start, NULL);
-      updateParams                        ();
-      autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);//入力の左右反転を常にチェック(＝手足の交差は不可能)
-      convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
-      hp_plot = rp_ref_out;
-      if(WBMSparam.set_com_height_fix)rp_ref_out.tgt[com].abs.p(Z) = rp_ref_out.tgt[com].offs.p(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
-      judgeFootLandOnCommandByFootForce   (hp_wld_raw);//人体足裏反力から各足の接地指令を生成
-      lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
-      limitEEWorkspace                    (rp_ref_out);
-      setFootContactPoseByGoContact       (rp_ref_out);
-      if(DEBUG){ fprintf(cz_log,"com_in: %f %f ",rp_ref_out.tgt[com].abs.p(X),rp_ref_out.tgt[com].abs.p(Y)); }
-      applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
-
-      overwriteFootZFromFootLandOnCommand (rp_ref_out);
-      setFootRotHorizontalIfGoLanding     (rp_ref_out);
-
-//      limitManipulability                 (rp_ref_out);
-
-      applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
-
-      Vector3ToVector2(rp_ref_out.tgt[com].abs.p,com_forcp_ref);
-      static hrp::Vector2 com_forcp_ref_old;
-      com_vel_forcp_ref = (com_forcp_ref - com_forcp_ref_old)/DT;
-      com_forcp_ref_old = com_forcp_ref;
-
-      applyLPFilter_pre                       (rp_ref_out);
-      applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
-//      surpressGoContactChattering         (rp_ref_out);
-//      setFootContactPoseByGoContact       (rp_ref_out);
-      applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
-      applyLPFilter_post                       (rp_ref_out);
-
-      r_zmp_raw = rp_ref_out.tgt[zmp].abs.p;
-      applyZMPCalcFromCOM                 (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);
-      if(DEBUG){
-        fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.tgt[zmp].abs.p(X),rp_ref_out.tgt[zmp].abs.p(Y));
-        fprintf(cz_log,"\n");
-      }
-
-//      overwriteFootZFromFootLandOnCommand (rp_ref_out);
-      modifyFootRotAndXYForContact        (rp_ref_out);
-
-      H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[rf].abs.p(Z));
-      com_vel_old = (rp_ref_out.tgt[com].abs.p - rp_ref_out_old.tgt[com].abs.p)/DT;
-      rh_vel_old = (rp_ref_out.tgt[rh].abs.p - rp_ref_out_old.tgt[rh].abs.p)/DT;
-      rp_ref_out_old = rp_ref_out;
-      loop++;
-      is_initial_loop = false;
-      gettimeofday(&t_calc_end, NULL);
-    }
-    static void Vector3ToVector2(const hrp::Vector3& in, hrp::Vector2& out){ out(X) = in(X); out(Y) = in(Y);}
-    static void Vector2ToVector3(const hrp::Vector2& in, hrp::Vector3& out){ out(X) = in(X); out(Y) = in(Y);}
-    static void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out(X) = in.x; out(Y) = in.y; out(Z) = in.z;}
-    static void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out(X) = in.r; out(Y) = in.p; out(Z) = in.y;}
-    static void Vector3ToPoint3D(const hrp::Vector3& in, RTC::Point3D& out){ out.x = in(X); out.y = in(Y); out.z = in(Z);}
-    static void Vector3ToOriantation3D(const hrp::Vector3& in, RTC::Orientation3D& out){ out.r = in(X); out.p = in(Y); out.y = in(Z);}
-    static void Pose3DToWBMSPose3D(const RTC::Pose3D& in, WBMSPose3D& out){ Point3DToVector3(in.position,out.p); Oriantation3DToVector3(in.orientation,out.rpy); }
-    static void WBMSPose3DToPose3D(const WBMSPose3D& in, RTC::Pose3D& out){ Vector3ToPoint3D(in.p,out.position); Vector3ToOriantation3D(in.rpy,out.orientation); }
-    static void DoubleSeqToVector6(const RTC::TimedDoubleSeq::_data_seq& in, hrp::dvector6& out){
-      if(in.length() == 6){out(fx)=in[0]; out(fy)=in[1]; out(fz)=in[2]; out(tz)=in[3]; out(ty)=in[4]; out(tz)=in[5];
-      }else{ std::cerr<<"[WARN] HumanPose::hrp::dvector6 DoubleSeqTohrp::dvector6() invalid data length"<<std::endl; out = hrp::dvector6::Zero(); }
-    }
-    static void Vector6ToDoubleSeq(const hrp::dvector6& in, RTC::TimedDoubleSeq::_data_seq& out){
-      if(out.length() == 6){out[0]=in(fx); out[1]=in(fy); out[2]=in(fz); out[3]=in(tx); out[4]=in(ty); out[5]=in(tz);
-      }else{ std::cerr<<"[WARN] HumanPose::hrp::dvector6 DoubleSeqTohrp::dvector6() invalid data length"<<std::endl; }
-    }
-    static double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(X)*b(Y)-a(Y)*b(X); }
-
-  private:
-    void updateParams(){
-      updateHumanToRobotRatio(tgt_h2r_ratio);
-    }
-    void updateHumanToRobotRatio(const double h2r_r_goal){//h2r_ratioに伴って変わる変数の処理もここに書く
-      const double step = 0.001;
-      if(h2r_r_goal - h2r_ratio > step){h2r_ratio += step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
-      else if(h2r_r_goal - h2r_ratio < (-1)*step){h2r_ratio -= step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
-      else{h2r_ratio = h2r_r_goal;}
-    }
-    void autoLRSwapCheck(const HumanPose& in, HumanPose& out){
-      out = in;
-      if(in.tgt[rh].offs.p(Y) > in.tgt[lh].offs.p(Y) ){ out.tgt[rh] = in.tgt[lh]; out.tgt[lh] = in.tgt[rh]; }
-      if(in.tgt[rf].offs.p(Y) > in.tgt[lf].offs.p(Y) ){ out.tgt[rf] = in.tgt[lf]; out.tgt[lf] = in.tgt[rf]; }
-    }
-    void convertRelHumanPoseToRelRobotPose(const HumanPose& in, HumanPose& out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
-//      out = in;//ダメゼッタイ
-      for(int i=0, l[5]={com,rf,lf,rh,lh}; i<5; i++){
-        out.tgt[l[i]].abs.p   =  h2r_ratio * (in.tgt[l[i]].abs.p - in.tgt[l[i]].offs.p)   + out.tgt[l[i]].offs.p;
-        out.tgt[l[i]].abs.rpy =  hrp::rpyFromRot( hrp::rotFromRpy(in.tgt[l[i]].abs.rpy) * hrp::rotFromRpy(in.tgt[l[i]].offs.rpy).transpose() * hrp::rotFromRpy(out.tgt[l[i]].offs.rpy) );
-      }
-      for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
-        out.tgt[l[i]].w = in.tgt[l[i]].w;
-        out.tgt[l[i]].go_contact = in.tgt[l[i]].go_contact;
-      }
-      out.tgt[head].abs = in.tgt[head].abs;
-      out.tgt[zmp].abs = in.tgt[zmp].abs;//最近使わない
-    }
-    void judgeFootLandOnCommandByFootForce(HumanPose& in){
-      for(int i=0, l[LR]={rf,lf}; i<LR; i++){
-        if      (in.tgt[l[i]].is_contact  && in.tgt[l[i]].w(fz)<CNT_F_TH   ){in.tgt[l[i]].is_contact = false;}//足ついた状態から上げる
-        else if (!in.tgt[l[i]].is_contact && in.tgt[l[i]].w(fz)>CNT_F_TH+30){in.tgt[l[i]].is_contact = true;}//足浮いた状態から下げる
-      }
-    }
-    void applyLPFilter_pre(HumanPose& tgt){
-      const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
-      for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
-//        calcVelAccSafeTrajectoryVec(fik_act->getEndEffectorPos(robot_l_names[i]), (fik_act->getEndEffectorPos(robot_l_names[i]) - ee_pose_old[i].p)/DT, tgt.tgt[l[i]].abs.p, 3.0, 1.0, tgt.tgt[l[i]].abs.p);//何故かダメ
-//        calcVelAccSafeTrajectoryVec(hrp::rpyFromRot(fik_act->getEndEffectorRot(robot_l_names[i])), (hrp::rpyFromRot(fik_act->getEndEffectorRot(robot_l_names[i])) - ee_pose_old[i].rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3.0, 1.0, tgt.tgt[l[i]].abs.rpy);
-//        calcVelAccSafeTrajectoryVec(fik_act->getEndEffectorPos(robot_l_names[i]), (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 3.0, 1.0, tgt.tgt[l[i]].abs.p);
-//        calcVelAccSafeTrajectoryVec(hrp::rpyFromRot(fik_act->getEndEffectorRot(robot_l_names[i])), (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3.0, 1.0, tgt.tgt[l[i]].abs.rpy);
-      }
-      for(int i=0, l[1]={head}; i<1; i++){
-//        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 2.0, 1.0, tgt.tgt[l[i]].abs.p);
-//        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 2.0, 1.0, tgt.tgt[l[i]].abs.rpy);
-      }
-    }
-    void applyLPFilter_post(HumanPose& tgt){
-      if(is_initial_loop){
-        for(int i=0, l[1]={com}; i<1; i++){
-          tgt_pos_filters[l[i]].reset(tgt.tgt[l[i]].abs.p);
-          tgt_rot_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy);
-        }
-      }
-      for(int i=0, l[1]={com}; i<1; i++){
-        tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
-        tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
-      }
-    }
-    void calcVelAccSafeTrajectoryVec(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
-      if((pos_tgt - pos_cur).norm() > 1.0e-6){
-        for(int i=0;i<3;i++){
-          double stop_safe_vel = SGN(pos_tgt(i) - pos_cur(i)) * sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
-          double vel_ans;
-          if(vel_cur(i) > stop_safe_vel){
-            vel_ans = vel_cur(i) - max_acc * DT;
-          }else{
-            vel_ans = vel_cur(i) + max_acc * DT;
-          }
-          pos_ans(i) = pos_cur(i) + vel_ans * DT;
-        }
-      }else{
-        pos_ans = pos_tgt;
-      }
-    }
-    void calcVelAccSafeTrajectoryVecML(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& acc_base, const hrp::Matrix33& max_acc_mat, const hrp::Vector3& max_acc_sv, hrp::Vector3& pos_ans){
-      if((pos_tgt - pos_cur).norm() > 1.0e-6){
-        hrp::Vector3 direc = (pos_tgt - pos_cur).normalized();
-        hrp::Vector3 ref_acc = direc*acc_base;
-        hrp::Vector3 mod_acc = max_acc_mat * max_acc_sv.asDiagonal() * max_acc_mat.transpose() * ref_acc;
-        double stop_safe_vel_scalar = sqrt( 2 * fabs(mod_acc.dot(direc)) * (pos_tgt - pos_cur).norm() );
-        hrp::Vector3 vel_ans = vel_cur + mod_acc * DT;
-        if(vel_ans.dot(direc) > stop_safe_vel_scalar){
-          vel_ans = vel_ans * stop_safe_vel_scalar / vel_ans.dot(direc);
-        }
-        pos_ans = pos_cur + vel_ans * DT;
-      }else{
-        pos_ans = pos_tgt;
-      }
-    }
-    void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
-      hrp::Vector3 direc( SGN(pos_tgt(X)-pos_cur(X)), SGN(pos_tgt(Y)-pos_cur(Y)), SGN(pos_tgt(Z)-pos_cur(Z)));
-      hrp::Vector3 vel_ans = vel_cur + direc * max_acc * DT;
-      for(int i=0;i<XYZ;i++){
-        double stop_safe_vel = sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
-        if( direc(i) > 0 ){
-          if(vel_ans(i) > stop_safe_vel){ vel_ans(i) = stop_safe_vel; }
-        }else{
-          if(vel_ans(i) < - stop_safe_vel){ vel_ans(i) = - stop_safe_vel; }
-        }
-      }
-      pos_ans = pos_cur + vel_ans * DT;
-    }
-    void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
-      hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);//TODO implement
-//      hrp::Vector2 rf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[rf].abs.p(X),old.tgt[rf].abs.p(Y));
-//      hrp::Vector2 lf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[lf].abs.p(X),old.tgt[lf].abs.p(Y));
-      hrp::Vector2 rf2zmp = act_rs.zmp.head(XY) - old.tgt[rf].abs.p.head(XY);
-      hrp::Vector2 lf2zmp = act_rs.zmp.head(XY) - old.tgt[lf].abs.p.head(XY);
-      if( inside_vec_rf.dot(rf2zmp) / inside_vec_rf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ out.tgt[lf].go_contact = true; }
-      if( inside_vec_lf.dot(lf2zmp) / inside_vec_lf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ out.tgt[rf].go_contact = true; }
-    }
-    void limitEEWorkspace(HumanPose& out){
-      const double MAX_FW = 0.25;
-//      const double MAX_FW = 1000000;//manipulability test
-      const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
-
-      for(int i=0, spl[LR]={rf,lf}, swl[LR]={lf,rf}; i<LR; i++){
-        PoseTGT& support_leg = out.tgt[spl[i]];
-        PoseTGT& swing_leg = out.tgt[swl[i]];
-        hrp::Vector2 inside_vec_baserel = ( spl[i]==rf ? hrp::Vector2(0,+1) : hrp::Vector2(0,-1) );
-
-        hrp::Vector2 sp2sw_vec( swing_leg.abs.p(X) - support_leg.cnt.p(X), swing_leg.abs.p(Y) - support_leg.cnt.p(Y) );
-        if(swing_leg.go_contact && sp2sw_vec.norm() > MAX_FW){ sp2sw_vec = sp2sw_vec.normalized() * MAX_FW; }//着地時に足を広げすぎないよう制限
-        Eigen::Matrix2d base_rot;
-        base_rot = Eigen::Rotation2Dd(baselinkpose.rpy(y));
-        hrp::Vector2 sp2sw_vec_baserel = base_rot.transpose() * sp2sw_vec;
-        if( sp2sw_vec_baserel.dot(inside_vec_baserel) < FOOT_2_FOOT_COLLISION_MARGIIN){
-          sp2sw_vec_baserel += inside_vec_baserel * (FOOT_2_FOOT_COLLISION_MARGIIN - sp2sw_vec_baserel.dot(inside_vec_baserel));
-        }
-        sp2sw_vec = base_rot * sp2sw_vec_baserel;
-        swing_leg.abs.p(X) = sp2sw_vec(X) + support_leg.cnt.p(X);
-        swing_leg.abs.p(Y) = sp2sw_vec(Y) + support_leg.cnt.p(Y);
-      }
-      LIMIT_MINMAX( out.tgt[rf].abs.p(Z), out.tgt[rf].offs.p(Z), out.tgt[rf].offs.p(Z)+WBMSparam.swing_foot_max_height);
-      LIMIT_MINMAX( out.tgt[lf].abs.p(Z), out.tgt[lf].offs.p(Z), out.tgt[lf].offs.p(Z)+WBMSparam.swing_foot_max_height);
-
-      const double base2hand_min = 0.4;
-      for(int i=0, l[2]={rh,lh}; i<2; i++){
-//        LIMIT_MIN(out.tgt[l[i]].abs.p(X), baselinkpose.p(X));
-//        LIMIT_MAX(out.tgt[l[i]].abs.p(Z), baselinkpose.p(Z) + 0.4);
-        hrp::Vector2 horizontal_dist(out.tgt[l[i]].abs.p(X) - baselinkpose.p(X), out.tgt[l[i]].abs.p(Y) - baselinkpose.p(Y));
-        if(horizontal_dist.norm() < base2hand_min){
-          horizontal_dist = base2hand_min * horizontal_dist.normalized();
-        }
-        out.tgt[l[i]].abs.p(X) = baselinkpose.p(X) + horizontal_dist(X);
-        out.tgt[l[i]].abs.p(Y) = baselinkpose.p(Y) + horizontal_dist(Y);
-      }
-      LIMIT_MINMAX( out.tgt[com].abs.p(Z), out.tgt[com].offs.p(Z) - 0.15, out.tgt[com].offs.p(Z) + 0.03 );//COM高さ方向の制限
-//      for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( out.tgt[com].abs.rpy(i), rc.ee_rot_limit[com][MIN](i), rc.ee_rot_limit[com][MAX](i) ); }
-//      for(int i=0, l[5]={rf,lf,rh,lh,head}; i<5; i++){
-//        for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( out.tgt[l[i]].abs.rpy(j), rc.ee_rot_limit[l[i]][MIN](j) + out.tgt[com].abs.rpy(j), rc.ee_rot_limit[l[i]][MAX](j) + out.tgt[com].abs.rpy(j) ); }
-//      }
-      if(!WBMSparam.use_head)out.tgt[head].abs.rpy = hrp::Vector3::Zero();
-    }
-    void limitManipulability(HumanPose& out){
-      const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
-      const int human_l_names[4] = {rf,lf,rh,lh};
-      m_robot_ml->rootLink()->p = m_robot_act->rootLink()->p;
-      m_robot_ml->rootLink()->R = m_robot_act->rootLink()->R;
-//      for (int i=0;i<m_robot_ml->numJoints();i++){ m_robot_ml->joint(i)->q = m_robot_act->joint(i)->q; }
-      m_robot_ml->calcForwardKinematics();
-      for(int i=0;i<4;i++){
-        if(fik_ml->ikp.count(robot_l_names[i])){
-          fik_ml->ikp[robot_l_names[i]].target_r0 = hrp::rotFromRpy(out.tgt[human_l_names[i]].abs.rpy);
-          fik_ml->ikp[robot_l_names[i]].target_p0 = out.tgt[human_l_names[i]].abs.p;
-        }
-      }
-      const int pre_ik_loop = 2;
-      for(int i=0;i<pre_ik_loop;i++){
-        for ( std::map<std::string, FullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_ml->ikp.begin(); it != fik_ml->ikp.end(); it++ ) {
-            if (it->second.is_ik_enable) fik_ml->solveLimbIK (it->second, it->first, fik_ml->ratio_for_vel, false);
-        }
-      }
-      for(int i=0;i<4;i++){
-        if(fik_ml->ikp.count(robot_l_names[i])){
-          out.tgt[human_l_names[i]].abs.p = fik_ml->getEndEffectorPos(robot_l_names[i]);
-          out.tgt[human_l_names[i]].abs.rpy = hrp::rpyFromRot(fik_ml->getEndEffectorRot(robot_l_names[i]));
-        }
-      }
-    }
-    void setFootContactPoseByGoContact(HumanPose& out){
-      for(int i=0, l[LR]={rf,lf}; i<LR; i++){
-        if(out.tgt[l[i]].go_contact){
-          out.tgt[l[i]].abs.p(X) = out.tgt[l[i]].cnt.p(X);
-          out.tgt[l[i]].abs.p(Y) = out.tgt[l[i]].cnt.p(Y);
-          out.tgt[l[i]].abs.rpy(y) = out.tgt[l[i]].cnt.rpy(y);
-        }else{
-          out.tgt[l[i]].cnt.p(X) = rp_ref_out_old.tgt[l[i]].abs.p(X);
-          out.tgt[l[i]].cnt.p(Y) = rp_ref_out_old.tgt[l[i]].abs.p(Y);
-          out.tgt[l[i]].cnt.rpy(y) = rp_ref_out_old.tgt[l[i]].abs.rpy(y);
-        }
-      }
-    }
-    void setFootRotHorizontalIfGoLanding(HumanPose& out){
-      if( out.tgt[rf].go_contact){ out.tgt[rf].abs.rpy(r) = out.tgt[rf].abs.rpy(p) = 0; }
-      if( out.tgt[lf].go_contact){ out.tgt[lf].abs.rpy(r) = out.tgt[lf].abs.rpy(p) = 0; }
-    }
-    void overwriteFootZFromFootLandOnCommand(HumanPose& out){
-//      limitGroundContactVelocity(out.tgt[rf].go_contact, rp_ref_out_old.tgt[rf], out.tgt[rf], out.tgt[rf].is_contact);
-//      limitGroundContactVelocity(out.tgt[lf].go_contact, rp_ref_out_old.tgt[lf], out.tgt[lf], out.tgt[lf].is_contact);
-      for(int i=0, l[LR]={rf,lf}; i<LR; i++){
-        if(out.tgt[l[i]].go_contact || cp_force_go_contact[i]){
-          out.tgt[l[i]].abs.p(Z) = out.tgt[l[i]].offs.p(Z);
-          out.tgt[l[i]].is_contact = true;
-        }else{
-          LIMIT_MIN( out.tgt[l[i]].abs.p(Z), out.tgt[l[i]].offs.p(Z)+WBMSparam.swing_foot_height_offset);
-        }
-      }
-    }
-    void limitGroundContactVelocity(const bool& go_land_in, const PoseTGT& f_old_in, PoseTGT& f_in_out, bool& is_f_contact_out){
-      LIMIT_MIN( f_in_out.abs.p(Z), f_in_out.offs.p(Z));
-      double input_vel = (f_in_out.abs.p(Z) - f_old_in.abs.p(Z)) / DT;
-      const double vel_limit_k = WBMSparam.foot_vertical_vel_limit_coeff;//地面から0.02[m]地点で0.02*8=0.16[m/s]出ている計算
-      double limit_vel = - vel_limit_k * (f_old_in.abs.p(Z) - f_old_in.offs.p(Z));//sinの時は平均0.05~0.10[m/s]
-      const double max_vel_threshold = -0.01;
-      LIMIT_MAX( limit_vel, max_vel_threshold);//速度0に近づくと目標になかなか到達しないから
-      LIMIT_MIN( input_vel, limit_vel);//着地時の下向きの速度を制限
-      f_in_out.abs.p(Z) = f_old_in.abs.p(Z) + input_vel * DT;
-      LIMIT_MIN( f_in_out.abs.p(Z), f_in_out.offs.p(Z));
-      const double contact_threshold = 0.005;
-      is_f_contact_out = ((f_in_out.abs.p(Z)-f_in_out.offs.p(Z)) <= contact_threshold);
-    }
-    void modifyFootRotAndXYForContact(HumanPose& out){
-      calcFootRotAndXYTransitionForContact(out.tgt[rf],foot_vert_act[R]);
-      calcFootRotAndXYTransitionForContact(out.tgt[lf],foot_vert_act[L]);
-    }
-    void calcFootRotAndXYTransitionForContact(PoseTGT& foot_in, const std::vector<hrp::Vector3>& act_sole_size){
-      std::vector<hrp::Vector3> act_foot_vert;
-      double min_height = foot_in.abs.p(Z);
-      int min_height_id = 0;
-      for(int i=0;i<act_sole_size.size();i++){
-        act_foot_vert.push_back(hrp::rotFromRpy(foot_in.abs.rpy) * act_sole_size[i]);
-        act_foot_vert[i](Z) += foot_in.abs.p(Z);
-        if(act_foot_vert[i](Z) < min_height){
-          min_height = act_foot_vert[i](Z);
-          min_height_id = i;
-        }
-      }
-      if(act_foot_vert[min_height_id](Z) < foot_in.offs.p(Z)){
-        foot_in.abs.p(Z) += foot_in.offs.p(Z) - act_foot_vert[min_height_id](Z);
-      }
-    }
-    bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
-      hull_com.clear();
-      createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R], foot_vert_safe_fblr[L], hull_com);
-      hrp::Vector2 cog(comin_abs(X),comin_abs(Y));
-      if(!isPointInHullOpenCV(cog,hull_com)){ calcNearestPointOnHull(cog,hull_com,cog); }//外に出たら最近傍点に頭打ち
-      comin_abs(X) = cog(X);
-      comin_abs(Y) = cog(Y);
-      return true;
-    }
-    bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans_old, hrp::Vector3& com_ans){
-      com_ans = com_in;
-      if (is_initial_loop)com_ans_old = com_in;
-      hrp::Vector3 com_vel = (com_in - com_ans_old)/DT;
-      hull_dcp.clear();
-      hull_acp.clear();
-      hrp::Vector4 marginDelta_for_dcp(+0.001,-0.001,0.001,-0.001);
-      hrp::Vector4 marginDelta_for_acp(+0.010,-0.010,0.010,-0.010);
-      createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R]+marginDelta_for_dcp, foot_vert_safe_fblr[L]+marginDelta_for_dcp, hull_dcp);
-      createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R]+marginDelta_for_acp, foot_vert_safe_fblr[L]+marginDelta_for_acp, hull_acp);
-      hrp::Vector2 com_vel_ans_2d;
-      regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_ans_old(X),com_ans_old(Y)), hrp::Vector2(com_vel(X),com_vel(Y)), hull_dcp, hull_acp, com_vel_ans_2d);
-      com_ans(X) = com_ans_old(X) + com_vel_ans_2d(X) * DT;
-      com_ans(Y) = com_ans_old(Y) + com_vel_ans_2d(Y) * DT;
-      com_ans_old = com_ans;
-      Vector2ToVector3(hrp::Vector2(com_ans(X),com_ans(Y)) + com_vel_ans_2d * sqrt( H_cur / G ), cp_dec);
-      Vector2ToVector3(hrp::Vector2(com_ans(X),com_ans(Y)) - com_vel_ans_2d * sqrt( H_cur / G ), cp_acc);
-      if(DEBUG){
-        fprintf(cz_log,"com_ans: %f %f ",com_ans(X),com_ans(Y));
-        fprintf(cz_log,"com_ans_cp_d: %f %f ",cp_dec(X),cp_dec(Y));
-        fprintf(cz_log,"com_ans_cp_a: %f %f ",cp_acc(X),cp_acc(Y));
-        for(int i=0;i<hull_dcp.size();i++)fprintf(sr_log,"hull_vert%d: %f %f\n",i,hull_dcp[i](X),hull_dcp[i](Y));
-        fprintf(sr_log,"hull_vert%d: %f %f\n",(int)hull_dcp.size(),hull_dcp[0](X),hull_dcp[0](Y));//閉じる
-        fprintf(sr_log,"\n");
-      }
-      return true;
-    }
-    void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull_d, const std::vector<hrp::Vector2>& hull_a, hrp::Vector2& com_vel_ans){
-      hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
-      com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
-//      //減速CP条件(現在のCPを常に両足裏で頭打ち)
-//      hrp::Vector2 cp_dec_tmp = com_pos + com_vel * sqrt( H_cur / G );
-//      hrp::Vector2 cp_dec_ragulated;
-//      if(!isPointInHullOpenCV(cp_dec_tmp,hull_d)){
-//        calcCrossPointOnHull(com_pos, cp_dec_tmp, hull_d, cp_dec_ragulated);
-//        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
-//      }
-
-      const double foot_ave_vel = 2.0;
-      double lf_landing_delay = (rp_ref_out.tgt[lf].abs.p[Z] - rp_ref_out.tgt[lf].offs.p[Z]) / foot_ave_vel;
-      double rf_landing_delay = (rp_ref_out.tgt[rf].abs.p[Z] - rp_ref_out.tgt[rf].offs.p[Z]) / foot_ave_vel;
-      if(com_vel(Y)>0 ){ rf_landing_delay = 0; }
-      if(com_vel(Y)<0 ){ lf_landing_delay = 0; }
-      double foot_landing_delay = std::max(rf_landing_delay, lf_landing_delay);
-//      const double foot_landing_delay = 0;//debug
-      //減速CP条件(現在のCPを常に両足裏で頭打ち)
-
-//      hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);//TODO implement
-//      if(com_vel(Y)>1e-5 || fabs(com_vel(Y))<=1e-5 ){
-//        foot_landing_delay = 0;
-//      }
-      hrp::Vector2 cp_dec_ragulated = com_pos + com_vel * sqrt( H_cur / G ) + com_vel * foot_landing_delay;
-      if(!isPointInHullOpenCV(cp_dec_ragulated,hull_d)){
-        calcCrossPointOnHull(com_pos, cp_dec_ragulated, hull_d, cp_dec_ragulated);
-        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / ( sqrt( H_cur / G ) + foot_landing_delay);
-      }
+    private:
+        double CNT_F_TH, h2r_ratio, tgt_h2r_ratio, HZ, DT;
+        struct timeval t_calc_start, t_calc_end;
+        BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters, com_in_filter;
+        hrp::Vector3 com_old, com_oldold, comacc, com_CP_ref_old, r_zmp_raw;
+        std::vector<hrp::Vector2> rflf_points, hull_com, hull_dcp, hull_acp;
+        std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rot_filters;
+        std::vector<cv::Point2f> points,cvhull;
+        //    RobotConfig rc;
+        HumanPose rp_ref_out_old, hp_swap_checked;
+        unsigned int loop;
+        bool is_initial_loop;
+        bool cp_force_go_contact[LR];
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+        double H_cur;
+        HumanPose hp_wld_raw, hp_plot, rp_ref_out, rp_ref_vel_old;
+        FILE *sr_log, *cz_log, *id_log;
+        WBMSPose3D baselinkpose;
+        //    WBMSPose3D ee_contact_pose[4];
+        hrp::Vector3 com_vel_old,rh_vel_old, cp_dec, cp_acc;
+        std::vector<hrp::Vector3> foot_vert_act[LR], foot_vert_safe[LR], foot_vert_check[LR];
+        hrp::Vector2 com_forcp_ref,com_vel_forcp_ref;
+        hrp::dvector6 invdyn_ft;
+        hrp::Vector4 foot_vert_act_fblr[LR], foot_vert_safe_fblr[LR], foot_vert_check_fblr[LR];
+        struct WBMSparameters {
+                double auto_swing_foot_landing_threshold;
+                double foot_vertical_vel_limit_coeff;
+                double human_com_height;
+                bool is_doctor;
+                bool set_com_height_fix;
+                double set_com_height_fix_val;
+                double swing_foot_height_offset;
+                double swing_foot_max_height;
+                double upper_body_rmc_ratio;
+                bool use_rh,use_lh;
+                bool use_head;
+                bool use_manipulability_limit;
+        };
+        struct WBMSparameters WBMSparam;
+        struct ActualRobotState {
+                hrp::Vector3 com, zmp;
+        }act_rs;
+
+        typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
+        fikPtr fik_ml;
+        hrp::BodyPtr m_robot_ml;
+        //    fikPtr fik_act;
+        hrp::BodyPtr m_robot_act;
+        double cur_manip_val[4][3];
+        hrp::Vector3 manip_direc[4][3];
+        hrp::Matrix33 manip_mat[4];
+        hrp::Vector3 manip_sv[4];
+        hrp::Matrix33 manip_mat_rot[4];
+        hrp::Vector3 manip_sv_rot[4];
+
+        hrp::Vector2 cp_acc_old;
+        //    WBMSPose3D ee_pose_old[4];
+
+        WBMSCore(const double& dt){
+            tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
+            //      tgt_h2r_ratio = h2r_ratio = 1.0;
+            DT = dt;
+            HZ = (int)(1.0/DT);
+            CNT_F_TH = 20.0;
+            loop = 0;
+            //////////  hrp::Vector は初期化必要  ///////////
+            com_old = com_oldold = comacc = hrp::Vector3::Zero();
+            r_zmp_raw = hrp::Vector3::Zero();
+            com_vel_old = hrp::Vector3::Zero();
+            rh_vel_old = hrp::Vector3::Zero();
+            cp_dec = cp_acc = hrp::Vector3::Zero();
+            invdyn_ft = hrp::dvector6::Zero();
+            tgt_pos_filters.resize(num_pose_tgt);
+            tgt_rot_filters.resize(num_pose_tgt);
+            for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(1.0, HZ, Q_NOOVERSHOOT);//四肢拘束点用(position)
+            for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0, HZ, Q_NOOVERSHOOT);//四肢拘束点用(Rotation)
+            tgt_pos_filters[com].setParameter(1.0, HZ, Q_NOOVERSHOOT);//重心pos用
+            tgt_rot_filters[com].setParameter(0.6, HZ, Q_NOOVERSHOOT);//重心rot用
+            tgt_pos_filters[rf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, Q_NOOVERSHOOT);//右足pos用
+            tgt_pos_filters[lf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, Q_NOOVERSHOOT);//左足pos用
+            calcacc_v_filters.setParameter(5, HZ, Q_BUTTERWORTH);//加速度計算用
+            acc4zmp_v_filters.setParameter(5, HZ, Q_BUTTERWORTH);//ZMP生成用ほぼこの値でいい
+            com_in_filter.setParameter(1, HZ);
+            foot_vert_act_fblr[R] << 0.13, -0.10,  0.06, -0.08;
+            foot_vert_act_fblr[L] << 0.13, -0.10,  0.08, -0.06;
+            foot_vert_check_fblr[R] << 0.02+0.002, -0.01-0.002,  0.02+0.002,  0.01-0.002;
+            foot_vert_check_fblr[L] << 0.02+0.002, -0.01-0.002, -0.01+0.002, -0.02-0.002;
+            //      foot_vert_check_fblr[R] << 0.13, -0.10,  0.06, -0.08;
+            //      foot_vert_check_fblr[L] << 0.13, -0.10,  0.08, -0.06;
+            foot_vert_safe_fblr[R] << 0.02, -0.01,  0.02,  0.01;
+            foot_vert_safe_fblr[L] << 0.02, -0.01, -0.01, -0.02;
+            for(int i=0;i<LR;i++){
+                make_rect_verts(foot_vert_act_fblr[i], foot_vert_act[i]);
+                make_rect_verts(foot_vert_check_fblr[i], foot_vert_check[i]);
+                make_rect_verts(foot_vert_safe_fblr[i], foot_vert_safe[i]);
+            }
+            cp_force_go_contact[R] = false;
+            cp_force_go_contact[L] = false;
+
+            //      WBMSparam.auto_swing_foot_landing_threshold = 0.02;
+            WBMSparam.auto_swing_foot_landing_threshold = 0.04;
+            WBMSparam.foot_vertical_vel_limit_coeff = 4.0;
+            WBMSparam.human_com_height = 1.00;
+            WBMSparam.is_doctor = true;
+            WBMSparam.set_com_height_fix = false;
+            WBMSparam.set_com_height_fix_val = 0.02;
+            WBMSparam.swing_foot_height_offset = 0.01;
+            WBMSparam.swing_foot_max_height = 0.5;
+            WBMSparam.upper_body_rmc_ratio = 0.0;
+            WBMSparam.use_rh = WBMSparam.use_lh = true;
+            WBMSparam.use_head = true;
+            WBMSparam.use_manipulability_limit = true;
+            rp_ref_out_old.clear();
+            rp_ref_out.clear();
+            rp_ref_vel_old.clear();
+            rflf_points.reserve(8);//あらかじめreserveして高速化
+            points.reserve(8);
+            hull_com.reserve(6);
+            hull_dcp.reserve(6);
+            hull_dcp.reserve(6);
+            cvhull.reserve(6);
+            if(DEBUG){
+                sr_log = fopen("/home/ishiguro/HumanSync_support_region.log","w+");
+                cz_log = fopen("/home/ishiguro/HumanSync_com_zmp.log","w+");
+                id_log = fopen("/home/ishiguro/HumanSync_invdyn.log","w+");
+            }
+            is_initial_loop = true;
+            cout<<"WBMSCore constructed"<<endl;
+        }
+        static void make_rect_verts(const hrp::Vector4& ForwardBackLeftRight, std::vector<hrp::Vector3>& out){
+            out.clear();
+            out.push_back(hrp::Vector3(ForwardBackLeftRight(0), ForwardBackLeftRight(2),  0));//左前
+            out.push_back(hrp::Vector3(ForwardBackLeftRight(1), ForwardBackLeftRight(2),  0));//左後
+            out.push_back(hrp::Vector3(ForwardBackLeftRight(1), ForwardBackLeftRight(3),  0));//右後
+            out.push_back(hrp::Vector3(ForwardBackLeftRight(0), ForwardBackLeftRight(3),  0));//右前
+        }
+        ~WBMSCore(){
+            if(DEBUG)fclose(sr_log);
+            if(DEBUG)fclose(cz_log);
+            if(DEBUG)fclose(id_log);
+            cout<<"WBMSCore destructed"<<endl;
+        }
+        double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6; }
+        void initializeHumanPoseFromCurrentInput(){
+            std::string ns[7] = {"com","rf","lf","rh","lh","zmp","head"};
+            for(int i=0;i<7;i++){
+                hp_wld_raw.tgt[i].offs.p   =  hp_wld_raw.tgt[i].abs.p;
+                hp_wld_raw.tgt[i].offs.rpy =  hp_wld_raw.tgt[i].abs.rpy;
+            }
+        }
+        void initializeRobotPoseFromHRPBody(fikPtr& fik_in, hrp::BodyPtr& robot_in){
+            const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
+            const int human_l_names[4] = {rf,lf,rh,lh};
+            for(int i=0;i<4;i++){//HumanSynchronizerの初期姿勢オフセットをセット
+                if(fik_in->ikp.count(robot_l_names[i])){
+                    rp_ref_out.tgt[human_l_names[i]].offs.p = fik_in->getEndEffectorPos(robot_l_names[i]);
+                    rp_ref_out.tgt[human_l_names[i]].offs.rpy = hrp::rpyFromRot(fik_in->getEndEffectorRot(robot_l_names[i]));
+                    rp_ref_out.tgt[human_l_names[i]].abs = rp_ref_out.tgt[human_l_names[i]].offs;
+                }
+            }
+            rp_ref_out.tgt[com].offs.p = act_rs.com = act_rs.zmp = robot_in->calcCM();
+            com_CP_ref_old = rp_ref_out.tgt[com].offs.p;
+            rp_ref_out.tgt[com].offs.rpy = hrp::rpyFromRot(robot_in->rootLink()->R);
+            rp_ref_out.tgt[zmp].offs.p(X) = rp_ref_out.tgt[com].offs.p(X);
+            rp_ref_out.tgt[zmp].offs.p(Y) = rp_ref_out.tgt[com].offs.p(Y);
+            rp_ref_out.tgt[zmp].offs.p(Z) = (fik_in->getEndEffectorPos("rleg")(Z)+fik_in->getEndEffectorPos("lleg")(Z))/2;
+            rp_ref_out.tgt[rf].cnt = rp_ref_out.tgt[rf].offs;
+            rp_ref_out.tgt[lf].cnt = rp_ref_out.tgt[lf].offs;
+            baselinkpose.p = robot_in->rootLink()->p;
+            baselinkpose.rpy = hrp::rpyFromRot(robot_in->rootLink()->R);
+            for(int i=0, l[LR]={rf,lf}; i<LR; i++){
+                rp_ref_out.tgt[l[i]].is_contact = true;
+            }
+            H_cur = rp_ref_out.tgt[com].offs.p(Z) - std::min((double)rp_ref_out.tgt[rf].offs.p(Z), (double)rp_ref_out.tgt[rf].offs.p(Z));
+        }
+        void initializeRequest(fikPtr& fik_in, hrp::BodyPtr& robot_in){
+            loop = 0;
+            is_initial_loop = true;
+            initializeHumanPoseFromCurrentInput();
+            initializeRobotPoseFromHRPBody(fik_in, robot_in);
+            rp_ref_out_old = rp_ref_vel_old = rp_ref_out;
+        }
+        void update(){//////////  メインループ  ////////////
+            gettimeofday(&t_calc_start, NULL);
+            updateParams                        ();
+            autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);//入力の左右反転を常にチェック(＝手足の交差は不可能)
+            convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
+            hp_plot = rp_ref_out;
+            if(WBMSparam.set_com_height_fix)rp_ref_out.tgt[com].abs.p(Z) = rp_ref_out.tgt[com].offs.p(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
+            judgeFootLandOnCommandByFootForce   (hp_wld_raw);//人体足裏反力から各足の接地指令を生成
+            lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
+            limitEEWorkspace                    (rp_ref_out);
+            setFootContactPoseByGoContact       (rp_ref_out);
+            if(DEBUG){ fprintf(cz_log,"com_in: %f %f ",rp_ref_out.tgt[com].abs.p(X),rp_ref_out.tgt[com].abs.p(Y)); }
+            applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
+
+            overwriteFootZFromFootLandOnCommand (rp_ref_out);
+            setFootRotHorizontalIfGoLanding     (rp_ref_out);
+
+            //      limitManipulability                 (rp_ref_out);
+
+            applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
+
+            Vector3ToVector2(rp_ref_out.tgt[com].abs.p,com_forcp_ref);
+            static hrp::Vector2 com_forcp_ref_old;
+            com_vel_forcp_ref = (com_forcp_ref - com_forcp_ref_old)/DT;
+            com_forcp_ref_old = com_forcp_ref;
+
+            applyLPFilter_pre                       (rp_ref_out);
+            applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
+            //      surpressGoContactChattering         (rp_ref_out);
+            //      setFootContactPoseByGoContact       (rp_ref_out);
+            applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
+            applyLPFilter_post                       (rp_ref_out);
+
+            r_zmp_raw = rp_ref_out.tgt[zmp].abs.p;
+            applyZMPCalcFromCOM                 (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);
+            if(DEBUG){
+                fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.tgt[zmp].abs.p(X),rp_ref_out.tgt[zmp].abs.p(Y));
+                fprintf(cz_log,"\n");
+            }
+
+            //      overwriteFootZFromFootLandOnCommand (rp_ref_out);
+            modifyFootRotAndXYForContact        (rp_ref_out);
+
+            H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[rf].abs.p(Z));
+            com_vel_old = (rp_ref_out.tgt[com].abs.p - rp_ref_out_old.tgt[com].abs.p)/DT;
+            rh_vel_old = (rp_ref_out.tgt[rh].abs.p - rp_ref_out_old.tgt[rh].abs.p)/DT;
+            rp_ref_out_old = rp_ref_out;
+            loop++;
+            is_initial_loop = false;
+            gettimeofday(&t_calc_end, NULL);
+        }
+        static void Vector3ToVector2(const hrp::Vector3& in, hrp::Vector2& out){ out(X) = in(X); out(Y) = in(Y);}
+        static void Vector2ToVector3(const hrp::Vector2& in, hrp::Vector3& out){ out(X) = in(X); out(Y) = in(Y);}
+        static void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out(X) = in.x; out(Y) = in.y; out(Z) = in.z;}
+        static void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out(X) = in.r; out(Y) = in.p; out(Z) = in.y;}
+        static void Vector3ToPoint3D(const hrp::Vector3& in, RTC::Point3D& out){ out.x = in(X); out.y = in(Y); out.z = in(Z);}
+        static void Vector3ToOriantation3D(const hrp::Vector3& in, RTC::Orientation3D& out){ out.r = in(X); out.p = in(Y); out.y = in(Z);}
+        static void Pose3DToWBMSPose3D(const RTC::Pose3D& in, WBMSPose3D& out){ Point3DToVector3(in.position,out.p); Oriantation3DToVector3(in.orientation,out.rpy); }
+        static void WBMSPose3DToPose3D(const WBMSPose3D& in, RTC::Pose3D& out){ Vector3ToPoint3D(in.p,out.position); Vector3ToOriantation3D(in.rpy,out.orientation); }
+        static void DoubleSeqToVector6(const RTC::TimedDoubleSeq::_data_seq& in, hrp::dvector6& out){
+            if(in.length() == 6){out(fx)=in[0]; out(fy)=in[1]; out(fz)=in[2]; out(tz)=in[3]; out(ty)=in[4]; out(tz)=in[5];
+            }else{ std::cerr<<"[WARN] HumanPose::hrp::dvector6 DoubleSeqTohrp::dvector6() invalid data length"<<std::endl; out = hrp::dvector6::Zero(); }
+        }
+        static void Vector6ToDoubleSeq(const hrp::dvector6& in, RTC::TimedDoubleSeq::_data_seq& out){
+            if(out.length() == 6){out[0]=in(fx); out[1]=in(fy); out[2]=in(fz); out[3]=in(tx); out[4]=in(ty); out[5]=in(tz);
+            }else{ std::cerr<<"[WARN] HumanPose::hrp::dvector6 DoubleSeqTohrp::dvector6() invalid data length"<<std::endl; }
+        }
+        static double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(X)*b(Y)-a(Y)*b(X); }
+
+    private:
+        void updateParams(){
+            updateHumanToRobotRatio(tgt_h2r_ratio);
+        }
+        void updateHumanToRobotRatio(const double h2r_r_goal){//h2r_ratioに伴って変わる変数の処理もここに書く
+            const double step = 0.001;
+            if(h2r_r_goal - h2r_ratio > step){h2r_ratio += step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
+            else if(h2r_r_goal - h2r_ratio < (-1)*step){h2r_ratio -= step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
+            else{h2r_ratio = h2r_r_goal;}
+        }
+        void autoLRSwapCheck(const HumanPose& in, HumanPose& out){
+            out = in;
+            if(in.tgt[rh].offs.p(Y) > in.tgt[lh].offs.p(Y) ){ out.tgt[rh] = in.tgt[lh]; out.tgt[lh] = in.tgt[rh]; }
+            if(in.tgt[rf].offs.p(Y) > in.tgt[lf].offs.p(Y) ){ out.tgt[rf] = in.tgt[lf]; out.tgt[lf] = in.tgt[rf]; }
+        }
+        void convertRelHumanPoseToRelRobotPose(const HumanPose& in, HumanPose& out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
+            //      out = in;//ダメゼッタイ
+            for(int i=0, l[5]={com,rf,lf,rh,lh}; i<5; i++){
+                out.tgt[l[i]].abs.p   =  h2r_ratio * (in.tgt[l[i]].abs.p - in.tgt[l[i]].offs.p)   + out.tgt[l[i]].offs.p;
+                out.tgt[l[i]].abs.rpy =  hrp::rpyFromRot( hrp::rotFromRpy(in.tgt[l[i]].abs.rpy) * hrp::rotFromRpy(in.tgt[l[i]].offs.rpy).transpose() * hrp::rotFromRpy(out.tgt[l[i]].offs.rpy) );
+            }
+            for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
+                out.tgt[l[i]].w = in.tgt[l[i]].w;
+                out.tgt[l[i]].go_contact = in.tgt[l[i]].go_contact;
+            }
+            out.tgt[head].abs = in.tgt[head].abs;
+            out.tgt[zmp].abs = in.tgt[zmp].abs;//最近使わない
+        }
+        void judgeFootLandOnCommandByFootForce(HumanPose& in){
+            for(int i=0, l[LR]={rf,lf}; i<LR; i++){
+                if      (in.tgt[l[i]].is_contact  && in.tgt[l[i]].w(fz)<CNT_F_TH   ){in.tgt[l[i]].is_contact = false;}//足ついた状態から上げる
+                else if (!in.tgt[l[i]].is_contact && in.tgt[l[i]].w(fz)>CNT_F_TH+30){in.tgt[l[i]].is_contact = true;}//足浮いた状態から下げる
+            }
+        }
+        void applyLPFilter_pre(HumanPose& tgt){
+            const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
+            for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
+                //        calcVelAccSafeTrajectoryVec(fik_act->getEndEffectorPos(robot_l_names[i]), (fik_act->getEndEffectorPos(robot_l_names[i]) - ee_pose_old[i].p)/DT, tgt.tgt[l[i]].abs.p, 3.0, 1.0, tgt.tgt[l[i]].abs.p);//何故かダメ
+                //        calcVelAccSafeTrajectoryVec(hrp::rpyFromRot(fik_act->getEndEffectorRot(robot_l_names[i])), (hrp::rpyFromRot(fik_act->getEndEffectorRot(robot_l_names[i])) - ee_pose_old[i].rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+                //        calcVelAccSafeTrajectoryVec(fik_act->getEndEffectorPos(robot_l_names[i]), (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 3.0, 1.0, tgt.tgt[l[i]].abs.p);
+                //        calcVelAccSafeTrajectoryVec(hrp::rpyFromRot(fik_act->getEndEffectorRot(robot_l_names[i])), (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+            }
+            for(int i=0, l[1]={head}; i<1; i++){
+                //        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 2.0, 1.0, tgt.tgt[l[i]].abs.p);
+                //        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 2.0, 1.0, tgt.tgt[l[i]].abs.rpy);
+            }
+        }
+        void applyLPFilter_post(HumanPose& tgt){
+            if(is_initial_loop){
+                for(int i=0, l[1]={com}; i<1; i++){
+                    tgt_pos_filters[l[i]].reset(tgt.tgt[l[i]].abs.p);
+                    tgt_rot_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy);
+                }
+            }
+            for(int i=0, l[1]={com}; i<1; i++){
+                tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
+                tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
+            }
+        }
+        void calcVelAccSafeTrajectoryVec(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
+            if((pos_tgt - pos_cur).norm() > 1.0e-6){
+                for(int i=0;i<3;i++){
+                    double stop_safe_vel = SGN(pos_tgt(i) - pos_cur(i)) * sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
+                    double vel_ans;
+                    if(vel_cur(i) > stop_safe_vel){
+                        vel_ans = vel_cur(i) - max_acc * DT;
+                    }else{
+                        vel_ans = vel_cur(i) + max_acc * DT;
+                    }
+                    pos_ans(i) = pos_cur(i) + vel_ans * DT;
+                }
+            }else{
+                pos_ans = pos_tgt;
+            }
+        }
+        void calcVelAccSafeTrajectoryVecML(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& acc_base, const hrp::Matrix33& max_acc_mat, const hrp::Vector3& max_acc_sv, hrp::Vector3& pos_ans){
+            if((pos_tgt - pos_cur).norm() > 1.0e-6){
+                hrp::Vector3 direc = (pos_tgt - pos_cur).normalized();
+                hrp::Vector3 ref_acc = direc*acc_base;
+                hrp::Vector3 mod_acc = max_acc_mat * max_acc_sv.asDiagonal() * max_acc_mat.transpose() * ref_acc;
+                double stop_safe_vel_scalar = sqrt( 2 * fabs(mod_acc.dot(direc)) * (pos_tgt - pos_cur).norm() );
+                hrp::Vector3 vel_ans = vel_cur + mod_acc * DT;
+                if(vel_ans.dot(direc) > stop_safe_vel_scalar){
+                    vel_ans = vel_ans * stop_safe_vel_scalar / vel_ans.dot(direc);
+                }
+                pos_ans = pos_cur + vel_ans * DT;
+            }else{
+                pos_ans = pos_tgt;
+            }
+        }
+        void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
+            hrp::Vector3 direc( SGN(pos_tgt(X)-pos_cur(X)), SGN(pos_tgt(Y)-pos_cur(Y)), SGN(pos_tgt(Z)-pos_cur(Z)));
+            hrp::Vector3 vel_ans = vel_cur + direc * max_acc * DT;
+            for(int i=0;i<XYZ;i++){
+                double stop_safe_vel = sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
+                if( direc(i) > 0 ){
+                    if(vel_ans(i) > stop_safe_vel){ vel_ans(i) = stop_safe_vel; }
+                }else{
+                    if(vel_ans(i) < - stop_safe_vel){ vel_ans(i) = - stop_safe_vel; }
+                }
+            }
+            pos_ans = pos_cur + vel_ans * DT;
+        }
+        void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
+            hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);//TODO implement
+            //      hrp::Vector2 rf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[rf].abs.p(X),old.tgt[rf].abs.p(Y));
+            //      hrp::Vector2 lf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[lf].abs.p(X),old.tgt[lf].abs.p(Y));
+            hrp::Vector2 rf2zmp = act_rs.zmp.head(XY) - old.tgt[rf].abs.p.head(XY);
+            hrp::Vector2 lf2zmp = act_rs.zmp.head(XY) - old.tgt[lf].abs.p.head(XY);
+            if( inside_vec_rf.dot(rf2zmp) / inside_vec_rf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ out.tgt[lf].go_contact = true; }
+            if( inside_vec_lf.dot(lf2zmp) / inside_vec_lf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ out.tgt[rf].go_contact = true; }
+        }
+        void limitEEWorkspace(HumanPose& out){
+            const double MAX_FW = 0.25;
+            //      const double MAX_FW = 1000000;//manipulability test
+            const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
+
+            for(int i=0, spl[LR]={rf,lf}, swl[LR]={lf,rf}; i<LR; i++){
+                PoseTGT& support_leg = out.tgt[spl[i]];
+                PoseTGT& swing_leg = out.tgt[swl[i]];
+                hrp::Vector2 inside_vec_baserel = ( spl[i]==rf ? hrp::Vector2(0,+1) : hrp::Vector2(0,-1) );
+
+                hrp::Vector2 sp2sw_vec( swing_leg.abs.p(X) - support_leg.cnt.p(X), swing_leg.abs.p(Y) - support_leg.cnt.p(Y) );
+                if(swing_leg.go_contact && sp2sw_vec.norm() > MAX_FW){ sp2sw_vec = sp2sw_vec.normalized() * MAX_FW; }//着地時に足を広げすぎないよう制限
+                Eigen::Matrix2d base_rot;
+                base_rot = Eigen::Rotation2Dd(baselinkpose.rpy(y));
+                hrp::Vector2 sp2sw_vec_baserel = base_rot.transpose() * sp2sw_vec;
+                if( sp2sw_vec_baserel.dot(inside_vec_baserel) < FOOT_2_FOOT_COLLISION_MARGIIN){
+                    sp2sw_vec_baserel += inside_vec_baserel * (FOOT_2_FOOT_COLLISION_MARGIIN - sp2sw_vec_baserel.dot(inside_vec_baserel));
+                }
+                sp2sw_vec = base_rot * sp2sw_vec_baserel;
+                swing_leg.abs.p(X) = sp2sw_vec(X) + support_leg.cnt.p(X);
+                swing_leg.abs.p(Y) = sp2sw_vec(Y) + support_leg.cnt.p(Y);
+            }
+            LIMIT_MINMAX( out.tgt[rf].abs.p(Z), out.tgt[rf].offs.p(Z), out.tgt[rf].offs.p(Z)+WBMSparam.swing_foot_max_height);
+            LIMIT_MINMAX( out.tgt[lf].abs.p(Z), out.tgt[lf].offs.p(Z), out.tgt[lf].offs.p(Z)+WBMSparam.swing_foot_max_height);
+
+            const double base2hand_min = 0.4;
+            for(int i=0, l[2]={rh,lh}; i<2; i++){
+                //        LIMIT_MIN(out.tgt[l[i]].abs.p(X), baselinkpose.p(X));
+                //        LIMIT_MAX(out.tgt[l[i]].abs.p(Z), baselinkpose.p(Z) + 0.4);
+                hrp::Vector2 horizontal_dist(out.tgt[l[i]].abs.p(X) - baselinkpose.p(X), out.tgt[l[i]].abs.p(Y) - baselinkpose.p(Y));
+                if(horizontal_dist.norm() < base2hand_min){
+                    horizontal_dist = base2hand_min * horizontal_dist.normalized();
+                }
+                out.tgt[l[i]].abs.p(X) = baselinkpose.p(X) + horizontal_dist(X);
+                out.tgt[l[i]].abs.p(Y) = baselinkpose.p(Y) + horizontal_dist(Y);
+            }
+            LIMIT_MINMAX( out.tgt[com].abs.p(Z), out.tgt[com].offs.p(Z) - 0.15, out.tgt[com].offs.p(Z) + 0.03 );//COM高さ方向の制限
+            //      for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( out.tgt[com].abs.rpy(i), rc.ee_rot_limit[com][MIN](i), rc.ee_rot_limit[com][MAX](i) ); }
+            //      for(int i=0, l[5]={rf,lf,rh,lh,head}; i<5; i++){
+            //        for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( out.tgt[l[i]].abs.rpy(j), rc.ee_rot_limit[l[i]][MIN](j) + out.tgt[com].abs.rpy(j), rc.ee_rot_limit[l[i]][MAX](j) + out.tgt[com].abs.rpy(j) ); }
+            //      }
+            if(!WBMSparam.use_head)out.tgt[head].abs.rpy = hrp::Vector3::Zero();
+        }
+        void limitManipulability(HumanPose& out){
+            const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
+            const int human_l_names[4] = {rf,lf,rh,lh};
+            m_robot_ml->rootLink()->p = m_robot_act->rootLink()->p;
+            m_robot_ml->rootLink()->R = m_robot_act->rootLink()->R;
+            //      for (int i=0;i<m_robot_ml->numJoints();i++){ m_robot_ml->joint(i)->q = m_robot_act->joint(i)->q; }
+            m_robot_ml->calcForwardKinematics();
+            for(int i=0;i<4;i++){
+                if(fik_ml->ikp.count(robot_l_names[i])){
+                    fik_ml->ikp[robot_l_names[i]].target_r0 = hrp::rotFromRpy(out.tgt[human_l_names[i]].abs.rpy);
+                    fik_ml->ikp[robot_l_names[i]].target_p0 = out.tgt[human_l_names[i]].abs.p;
+                }
+            }
+            const int pre_ik_loop = 2;
+            for(int i=0;i<pre_ik_loop;i++){
+                for ( std::map<std::string, FullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_ml->ikp.begin(); it != fik_ml->ikp.end(); it++ ) {
+                    if (it->second.is_ik_enable) fik_ml->solveLimbIK (it->second, it->first, fik_ml->ratio_for_vel, false);
+                }
+            }
+            for(int i=0;i<4;i++){
+                if(fik_ml->ikp.count(robot_l_names[i])){
+                    out.tgt[human_l_names[i]].abs.p = fik_ml->getEndEffectorPos(robot_l_names[i]);
+                    out.tgt[human_l_names[i]].abs.rpy = hrp::rpyFromRot(fik_ml->getEndEffectorRot(robot_l_names[i]));
+                }
+            }
+        }
+        void setFootContactPoseByGoContact(HumanPose& out){
+            for(int i=0, l[LR]={rf,lf}; i<LR; i++){
+                if(out.tgt[l[i]].go_contact){
+                    out.tgt[l[i]].abs.p(X) = out.tgt[l[i]].cnt.p(X);
+                    out.tgt[l[i]].abs.p(Y) = out.tgt[l[i]].cnt.p(Y);
+                    out.tgt[l[i]].abs.rpy(y) = out.tgt[l[i]].cnt.rpy(y);
+                }else{
+                    out.tgt[l[i]].cnt.p(X) = rp_ref_out_old.tgt[l[i]].abs.p(X);
+                    out.tgt[l[i]].cnt.p(Y) = rp_ref_out_old.tgt[l[i]].abs.p(Y);
+                    out.tgt[l[i]].cnt.rpy(y) = rp_ref_out_old.tgt[l[i]].abs.rpy(y);
+                }
+            }
+        }
+        void setFootRotHorizontalIfGoLanding(HumanPose& out){
+            if( out.tgt[rf].go_contact){ out.tgt[rf].abs.rpy(r) = out.tgt[rf].abs.rpy(p) = 0; }
+            if( out.tgt[lf].go_contact){ out.tgt[lf].abs.rpy(r) = out.tgt[lf].abs.rpy(p) = 0; }
+        }
+        void overwriteFootZFromFootLandOnCommand(HumanPose& out){
+            //      limitGroundContactVelocity(out.tgt[rf].go_contact, rp_ref_out_old.tgt[rf], out.tgt[rf], out.tgt[rf].is_contact);
+            //      limitGroundContactVelocity(out.tgt[lf].go_contact, rp_ref_out_old.tgt[lf], out.tgt[lf], out.tgt[lf].is_contact);
+            for(int i=0, l[LR]={rf,lf}; i<LR; i++){
+                if(out.tgt[l[i]].go_contact || cp_force_go_contact[i]){
+                    out.tgt[l[i]].abs.p(Z) = out.tgt[l[i]].offs.p(Z);
+                }else{
+                    LIMIT_MIN( out.tgt[l[i]].abs.p(Z), out.tgt[l[i]].offs.p(Z)+WBMSparam.swing_foot_height_offset);
+                }
+                const double contact_threshold = 0.005;
+                out.tgt[l[i]].is_contact = ((out.tgt[l[i]].abs.p(Z)-out.tgt[l[i]].offs.p(Z)) <= contact_threshold);
+            }
+        }
+        //    void limitGroundContactVelocity(const bool& go_land_in, const PoseTGT& f_old_in, PoseTGT& f_in_out, bool& is_f_contact_out){
+        //      LIMIT_MIN( f_in_out.abs.p(Z), f_in_out.offs.p(Z));
+        //      double input_vel = (f_in_out.abs.p(Z) - f_old_in.abs.p(Z)) / DT;
+        //      const double vel_limit_k = WBMSparam.foot_vertical_vel_limit_coeff;//地面から0.02[m]地点で0.02*8=0.16[m/s]出ている計算
+        //      double limit_vel = - vel_limit_k * (f_old_in.abs.p(Z) - f_old_in.offs.p(Z));//sinの時は平均0.05~0.10[m/s]
+        //      const double max_vel_threshold = -0.01;
+        //      LIMIT_MAX( limit_vel, max_vel_threshold);//速度0に近づくと目標になかなか到達しないから
+        //      LIMIT_MIN( input_vel, limit_vel);//着地時の下向きの速度を制限
+        //      f_in_out.abs.p(Z) = f_old_in.abs.p(Z) + input_vel * DT;
+        //      LIMIT_MIN( f_in_out.abs.p(Z), f_in_out.offs.p(Z));
+        //      const double contact_threshold = 0.005;
+        //      is_f_contact_out = ((f_in_out.abs.p(Z)-f_in_out.offs.p(Z)) <= contact_threshold);
+        //    }
+        void modifyFootRotAndXYForContact(HumanPose& out){
+            calcFootRotAndXYTransitionForContact(out.tgt[rf],foot_vert_act[R]);
+            calcFootRotAndXYTransitionForContact(out.tgt[lf],foot_vert_act[L]);
+        }
+        void calcFootRotAndXYTransitionForContact(PoseTGT& foot_in, const std::vector<hrp::Vector3>& act_sole_size){
+            std::vector<hrp::Vector3> act_foot_vert;
+            double min_height = foot_in.abs.p(Z);
+            int min_height_id = 0;
+            for(int i=0;i<act_sole_size.size();i++){
+                act_foot_vert.push_back(hrp::rotFromRpy(foot_in.abs.rpy) * act_sole_size[i]);
+                act_foot_vert[i](Z) += foot_in.abs.p(Z);
+                if(act_foot_vert[i](Z) < min_height){
+                    min_height = act_foot_vert[i](Z);
+                    min_height_id = i;
+                }
+            }
+            if(act_foot_vert[min_height_id](Z) < foot_in.offs.p(Z)){
+                foot_in.abs.p(Z) += foot_in.offs.p(Z) - act_foot_vert[min_height_id](Z);
+            }
+        }
+        bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
+            hull_com.clear();
+            createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R], foot_vert_safe_fblr[L], hull_com);
+            hrp::Vector2 cog(comin_abs(X),comin_abs(Y));
+            if(!isPointInHullOpenCV(cog,hull_com)){ calcNearestPointOnHull(cog,hull_com,cog); }//外に出たら最近傍点に頭打ち
+            comin_abs(X) = cog(X);
+            comin_abs(Y) = cog(Y);
+            return true;
+        }
+        bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans_old, hrp::Vector3& com_ans){
+            com_ans = com_in;
+            if (is_initial_loop)com_ans_old = com_in;
+            hrp::Vector3 com_vel = (com_in - com_ans_old)/DT;
+            hull_dcp.clear();
+            hull_acp.clear();
+            hrp::Vector4 marginDelta_for_dcp(+0.001,-0.001,0.001,-0.001);
+            hrp::Vector4 marginDelta_for_acp(+0.010,-0.010,0.010,-0.010);
+            createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R]+marginDelta_for_dcp, foot_vert_safe_fblr[L]+marginDelta_for_dcp, hull_dcp);
+            createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R]+marginDelta_for_acp, foot_vert_safe_fblr[L]+marginDelta_for_acp, hull_acp);
+            hrp::Vector2 com_vel_ans_2d;
+            regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_ans_old(X),com_ans_old(Y)), hrp::Vector2(com_vel(X),com_vel(Y)), hull_dcp, hull_acp, com_vel_ans_2d);
+            com_ans(X) = com_ans_old(X) + com_vel_ans_2d(X) * DT;
+            com_ans(Y) = com_ans_old(Y) + com_vel_ans_2d(Y) * DT;
+            com_ans_old = com_ans;
+            Vector2ToVector3(hrp::Vector2(com_ans(X),com_ans(Y)) + com_vel_ans_2d * sqrt( H_cur / G ), cp_dec);
+            Vector2ToVector3(hrp::Vector2(com_ans(X),com_ans(Y)) - com_vel_ans_2d * sqrt( H_cur / G ), cp_acc);
+            if(DEBUG){
+                fprintf(cz_log,"com_ans: %f %f ",com_ans(X),com_ans(Y));
+                fprintf(cz_log,"com_ans_cp_d: %f %f ",cp_dec(X),cp_dec(Y));
+                fprintf(cz_log,"com_ans_cp_a: %f %f ",cp_acc(X),cp_acc(Y));
+                for(int i=0;i<hull_dcp.size();i++)fprintf(sr_log,"hull_vert%d: %f %f\n",i,hull_dcp[i](X),hull_dcp[i](Y));
+                fprintf(sr_log,"hull_vert%d: %f %f\n",(int)hull_dcp.size(),hull_dcp[0](X),hull_dcp[0](Y));//閉じる
+                fprintf(sr_log,"\n");
+            }
+            return true;
+        }
+        void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull_d, const std::vector<hrp::Vector2>& hull_a, hrp::Vector2& com_vel_ans){
+            hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
+            com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
+            //      //減速CP条件(現在のCPを常に両足裏で頭打ち)
+            //      hrp::Vector2 cp_dec_tmp = com_pos + com_vel * sqrt( H_cur / G );
+            //      hrp::Vector2 cp_dec_ragulated;
+            //      if(!isPointInHullOpenCV(cp_dec_tmp,hull_d)){
+            //        calcCrossPointOnHull(com_pos, cp_dec_tmp, hull_d, cp_dec_ragulated);
+            //        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
+            //      }
+
+            const double foot_ave_vel = 2.0;
+            double lf_landing_delay = (rp_ref_out.tgt[lf].abs.p[Z] - rp_ref_out.tgt[lf].offs.p[Z]) / foot_ave_vel;
+            double rf_landing_delay = (rp_ref_out.tgt[rf].abs.p[Z] - rp_ref_out.tgt[rf].offs.p[Z]) / foot_ave_vel;
+            if(com_vel(Y)>0 ){ rf_landing_delay = 0; }
+            if(com_vel(Y)<0 ){ lf_landing_delay = 0; }
+            double foot_landing_delay = std::max(rf_landing_delay, lf_landing_delay);
+            //      const double foot_landing_delay = 0;//debug
+            //減速CP条件(現在のCPを常に両足裏で頭打ち)
+
+            //      hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);//TODO implement
+            //      if(com_vel(Y)>1e-5 || fabs(com_vel(Y))<=1e-5 ){
+            //        foot_landing_delay = 0;
+            //      }
+            hrp::Vector2 cp_dec_ragulated = com_pos + com_vel * sqrt( H_cur / G ) + com_vel * foot_landing_delay;
+            if(!isPointInHullOpenCV(cp_dec_ragulated,hull_d)){
+                calcCrossPointOnHull(com_pos, cp_dec_ragulated, hull_d, cp_dec_ragulated);
+                com_vel_decel_ok = (cp_dec_ragulated - com_pos) / ( sqrt( H_cur / G ) + foot_landing_delay);
+            }
 
 #define F(x) (x == R ? rf : lf)
 
-      std::vector<hrp::Vector2> f_hull_wld[LR], f_hull_wld_ans[LR];
-      for(int i=0;i<foot_vert_check[R].size();i++){ f_hull_wld[R].push_back(foot_vert_check[R][i].head(XY) + rp_ref_out.tgt[F(R)].abs.p.head(XY)); }
-      for(int i=0;i<foot_vert_check[L].size();i++){ f_hull_wld[L].push_back(foot_vert_check[L][i].head(XY) + rp_ref_out.tgt[F(L)].abs.p.head(XY)); }
-//      for(int i=0;i<foot_vert_safe[R].size();i++){ f_hull_wld[R].push_back(foot_vert_safe[R][i].head(XY) + rp_ref_out.tgt[F(R)].abs.p.head(XY)); }
-//      for(int i=0;i<foot_vert_safe[L].size();i++){ f_hull_wld[L].push_back(foot_vert_safe[L][i].head(XY) + rp_ref_out.tgt[F(L)].abs.p.head(XY)); }
-      makeConvexHullOpenCV(f_hull_wld[R], f_hull_wld_ans[R]);
-      makeConvexHullOpenCV(f_hull_wld[L], f_hull_wld_ans[L]);
-//      if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[R])){ rp_ref_out.tgt[lf].go_contact = true; }
-//      if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[L])){ rp_ref_out.tgt[rf].go_contact = true; }
-      if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[R])){ cp_force_go_contact[L] = true; }else{ cp_force_go_contact[L] = false; }
-      if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[L])){ cp_force_go_contact[R] = true; }else{ cp_force_go_contact[R] = false; }
-
-      //加速CP条件(ACP使用)
-      hrp::Vector2 cp_acc_tmp = com_pos - com_vel * sqrt( H_cur / G );
-      hrp::Vector2 cp_acc_ragulated;
-      if(!isPointInHullOpenCV(cp_acc_tmp,hull_a)){
-         calcCrossPointOnHull(com_pos, cp_acc_tmp, hull_a, cp_acc_ragulated);
-         com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
-      }
-      cp_acc_old = cp_acc_ragulated;
-
-      static hrp::Vector2 com_vel_ans_old = com_vel;
-      //加速減速条件マージ
-      if( com_vel_decel_ok.dot(com_vel) < com_vel_accel_ok.dot(com_vel)){//normじゃダメ？
-        com_vel_ans = com_vel_decel_ok;
-      }else{
-        com_vel_ans = com_vel_accel_ok;
-      }
-      com_vel_ans_old = com_vel_ans;
-    }
-//    void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull_d, const std::vector<hrp::Vector2>& hull_a, hrp::Vector2& com_vel_ans){
-//      hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
-//      com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
-//      //減速CP条件(現在のCPを常に両足裏で頭打ち)
-//      hrp::Vector2 cp_dec_tmp = com_pos + com_vel * sqrt( H_cur / G );
-//      hrp::Vector2 cp_dec_ragulated;
-//      if(!isPointInHullOpenCV(cp_dec_tmp,hull_d)){
-//        calcCrossPointOnHull(com_pos, cp_dec_tmp, hull_d, cp_dec_ragulated);
-//        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
-//      }
-//
-//      //減速CP条件2(指令のCPに対する現在発揮可能なCP)
-////      hrp::Vector2 cp_dec_ref = com_forcp_ref + com_vel_forcp_ref * sqrt( H_cur / G );
-////      hrp::Vector2 cp_dec_ref_ragulated = cp_dec_ref;
-////      if(!isPointInHullOpenCV(cp_dec_ref,hull_d)){
-////        calcCrossPointOnHull(com_pos, cp_dec_ref, hull_d, cp_dec_ref_ragulated);
-////      }
-////      com_vel_decel_ok = (cp_dec_ref_ragulated - com_pos) / sqrt( H_cur / G );
-//
-//      //減速CP条件2(指令のCOMに収束する現在発揮可能なCP)
-////      hrp::Vector2 cp_dec_ref = com_forcp_ref;
-////      hrp::Vector2 cp_dec_ref_ragulated = cp_dec_ref;
-////      if(!isPointInHullOpenCV(cp_dec_ref,hull_d)){
-////        calcCrossPointOnHull(com_pos, cp_dec_ref, hull_d, cp_dec_ref_ragulated);
-////      }
-////      hrp::Vector2 vel_max_decel_ok = (cp_dec_ref_ragulated - com_pos) / sqrt( H_cur / G );
-////      if( com_vel.dot(com_vel) > vel_max_decel_ok.dot(com_vel)){
-////        com_vel_decel_ok = vel_max_decel_ok;
-////      }else{
-////        com_vel_decel_ok = com_vel;
-////      }
-//
-//      //加速CP条件(ACP使用)
-//      hrp::Vector2 cp_acc_tmp = com_pos - com_vel * sqrt( H_cur / G );
-//      hrp::Vector2 cp_acc_ragulated;
-//      if(!isPointInHullOpenCV(cp_acc_tmp,hull_a)){
-//         calcCrossPointOnHull(com_pos, cp_acc_tmp, hull_a, cp_acc_ragulated);
-//         com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
-//      }
-//      cp_acc_old = cp_acc_ragulated;
-//
-//      //加速CP条件2(ZMP使用)
-//      static hrp::Vector2 com_vel_ans_old = com_vel;
-////      hrp::Vector2 com_acc = (com_vel - com_vel_ans_old) / DT;
-////      hrp::Vector2 zmp_in = com_pos - com_acc / G * H_cur;
-////      hrp::Vector2 zmp_regulated;
-////      if(!isPointInHullOpenCV(zmp_in,hull_a)){
-////        calcCrossPointOnHull(com_pos, zmp_in, hull_a, zmp_regulated);
-////        hrp::Vector2 com_acc_regulated = (com_pos - zmp_regulated)*G/H_cur;
-////        com_vel_accel_ok = com_vel_ans_old + com_acc_regulated*DT;
-////      }
-//
-//      //加速減速条件マージ
-//      if( com_vel_decel_ok.dot(com_vel) < com_vel_accel_ok.dot(com_vel)){//normじゃダメ？
-//        com_vel_ans = com_vel_decel_ok;
-//      }else{
-//        com_vel_ans = com_vel_accel_ok;
-//      }
-//      com_vel_ans_old = com_vel_ans;
-//    }
-    void surpressGoContactChattering(HumanPose& out){
-//      static std::deque<int> buffer[LR];
-      static int count[LR];
-      for(int i=0;i<LR;i++){
-        if(out.tgt[F(i)].go_contact){ count[i]++; }
-        else{ count[i]--; }
-        LIMIT_MINMAX(count[i],0,100);
-        if(count[i]>0){ out.tgt[F(i)].go_contact = true; }
-      }
-    }
-    void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
-      rflf_points.clear();
-      hull_ans.clear();
-      for(int i=0;i<2;i++){
-        for(int j=2;j<4;j++){
-          rflf_points.push_back(hrp::Vector2(rfin_abs(X) + rf_mgn(i),    rfin_abs(Y) + rf_mgn(j)));
-          rflf_points.push_back(hrp::Vector2(lfin_abs(X) + lf_mgn(i),    lfin_abs(Y) + lf_mgn(j)));
-        }
-      }
-      makeConvexHullOpenCV(rflf_points, hull_ans);
-    }
-    void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const hrp::dvector6& rfwin, const hrp::dvector6& lfwin, hrp::Vector3& zmp_ans){
-      hrp::Vector3 rfzmp,lfzmp;
-      const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
-      if( rfwin(fz) > 1.0e-6 ){
-        rfzmp(0) = ( - rfwin(ty) - rfwin(fx) * F_H_OFFSET + rfwin(fz) * 0 ) / rfwin(fz) + rfpos(0);
-        rfzmp(1) = (   rfwin(tx) - rfwin(fy) * F_H_OFFSET + rfwin(fz) * 0 ) / rfwin(fz) + rfpos(1);
-      }
-      if( lfwin(fz) > 1.0e-6 ){
-        lfzmp(0) = ( - lfwin(ty) - lfwin(fx) * F_H_OFFSET + lfwin(fz) * 0 ) / lfwin(fz) + lfpos(0);
-        lfzmp(1) = (   lfwin(tx) - lfwin(fy) * F_H_OFFSET + lfwin(fz) * 0 ) / lfwin(fz) + lfpos(1);
-      }
-      if( rfwin(fz) > 1.0e-6 || lfwin(fz) > 1.0e-6 ){
-        zmp_ans(0) = ( rfzmp(0)*rfwin(fz) + lfzmp(0)*lfwin(fz) ) / ( rfwin(fz) + lfwin(fz));
-        zmp_ans(1) = ( rfzmp(1)*rfwin(fz) + lfzmp(1)*lfwin(fz) ) / ( rfwin(fz) + lfwin(fz));
-      }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
-      zmp_ans(2) = 0;
-    }
-    void calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans){
-      hrp::Vector4 margin_abs;
-      hrp::Vector2 cross_pt, anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
-      for(int direc=0;direc<4;direc++){
-        calcCrossPointOnHull(check_point, check_point+anchor_vec[direc], hull, cross_pt);
-        margin_abs(direc) = (cross_pt - check_point).norm();
-      }
-      margin_ans(0) =  margin_abs(0);
-      margin_ans(1) = -margin_abs(1);
-      margin_ans(2) =  margin_abs(2);
-      margin_ans(3) = -margin_abs(3);
-    }
-    bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_will_cross){
-      hrp::Vector2 anchor_vec = pt_out_goal - pt_in_start;
-      for(int i=0;i<hull.size();i++){
-        int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
-        hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
-        hrp::Vector2 cur_edge = nxt_pt - cur_pt;
-        double dBunbo = hrpVector2Cross(anchor_vec,cur_edge);
-        if( dBunbo != 0.0) {//平行の場合を外す
-          hrp::Vector2 vectorAC = hrp::Vector2(hull[i](0),hull[i](1)) - pt_in_start;
-          double dR = hrpVector2Cross(vectorAC,cur_edge) / dBunbo;
-          double dS = hrpVector2Cross(vectorAC,anchor_vec) / dBunbo;
-          if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){//dRには数値誤差が乗る
-            pt_will_cross = pt_in_start + dR * anchor_vec;
-            //dist = dR * anchor_vec.norm();
-            return true;
-          }
-        }
-      }
-      return false;
-    }
-    double calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_ans){
-      double cur_nearest_dist, ans_nearest_dist;
-      hrp::Vector2 cur_nearest_pt, ans_nearest_pt;
-      for(int i=0;i<hull.size();i++){
-        int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
-        hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
-        hrp::Vector2 cur_edge = nxt_pt - cur_pt;
-        hrp::Vector2 tgt_pt_v = tgt_pt - cur_pt;
-        double tgt_pt_projected_length = tgt_pt_v.dot(cur_edge.normalized());
-        if(tgt_pt_projected_length > cur_edge.norm() ){//tgt_pt's nearest point is on the　i+1-th vertex
-          cur_nearest_pt = nxt_pt;
-        }else if(tgt_pt_projected_length < 0 ){//tgt_pt's nearest point is on the　i-th vertex
-          cur_nearest_pt = cur_pt;
-        }else{//tgt_pt's nearest point is on the line
-          cur_nearest_pt = cur_pt + tgt_pt_projected_length * cur_edge.normalized();
-        }
-        cur_nearest_dist = (tgt_pt - cur_nearest_pt).norm();
-        if(i==0){//set first candidate as nearest
-          ans_nearest_dist = cur_nearest_dist;
-          ans_nearest_pt = cur_nearest_pt;
-        }else if( cur_nearest_dist < ans_nearest_dist ){//update nearest candidate
-          ans_nearest_dist = cur_nearest_dist;
-          ans_nearest_pt = cur_nearest_pt;
-        }
-      }
-      pt_ans = ans_nearest_pt;
-      return ans_nearest_dist;
-    }
-    void makeConvexHullOpenCV(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
-      points.clear();
-      cvhull.clear();
-      hull_ans.clear();
-      for(int i=0;i<pts.size();i++){ points.push_back( cv::Point2f( pts[i](0), pts[i](1)) ); }
-      cv::convexHull(cv::Mat(points),cvhull,true);
-      for(int i=0;i<cvhull.size();i++){ hull_ans.push_back( hrp::Vector2( cvhull[i].x, cvhull[i].y ) );  }
-    }
-    //void makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
-    //  const int dim = 2;
-    //  double points[dim*pts.size()];
-    //  for(int i=0;i<pts.size();i++){ points[dim*i] = pts[i](0); points[dim*i+1] = pts[i](1); }
-    //  qh_new_qhull( dim, pts.size(), points, false, "qhull ", NULL, stderr);
-    //  pointT *point, *pointtemp;
-    //  hull_ans.clear();
-    //  FORALLpoints hull_ans.push_back( hrp::Vector2(point[0],point[1]) );
-    //  qh_freeqhull(!qh_ALL);
-    //  //#define qh_ORIENTclock 1
-    //}
-    bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull){
-      cvhull.clear();
-      for(int i=0;i<hull.size();i++){ cvhull.push_back( cv::Point2f( hull[i](0), hull[i](1)) ); }
-      return (cv::pointPolygonTest(cv::Mat(cvhull), cv::Point2f(pt(0),pt(1)), false) > 0);
-    }
-    void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
-      comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
-      comacc = acc4zmp_v_filters.passFilter(comacc);
-      zmpout(X) = comin(X)-(H_cur/G)*comacc(X);
-      zmpout(Y) = comin(Y)-(H_cur/G)*comacc(Y);
-      com_oldold = com_old;
-      com_old = comin;
-    }
-    void applyVelLimit(const HumanPose& out_old, HumanPose& out){
-      const double EE_MAX_VEL = 2.0;// m/s
-      for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
-        hrp::Vector3 diff = out.tgt[i].abs.p  - out_old.tgt[i].abs.p;
-        for(int j=0;j<3;j++)LIMIT_MINMAX( diff(j), -EE_MAX_VEL*DT, EE_MAX_VEL*DT);
-        out.tgt[i].abs.p = out_old.tgt[i].abs.p + diff;
-      }
-    }
+            std::vector<hrp::Vector2> f_hull_wld[LR], f_hull_wld_ans[LR];
+            for(int i=0;i<foot_vert_check[R].size();i++){ f_hull_wld[R].push_back(foot_vert_check[R][i].head(XY) + rp_ref_out.tgt[F(R)].abs.p.head(XY)); }
+            for(int i=0;i<foot_vert_check[L].size();i++){ f_hull_wld[L].push_back(foot_vert_check[L][i].head(XY) + rp_ref_out.tgt[F(L)].abs.p.head(XY)); }
+            //      for(int i=0;i<foot_vert_safe[R].size();i++){ f_hull_wld[R].push_back(foot_vert_safe[R][i].head(XY) + rp_ref_out.tgt[F(R)].abs.p.head(XY)); }
+            //      for(int i=0;i<foot_vert_safe[L].size();i++){ f_hull_wld[L].push_back(foot_vert_safe[L][i].head(XY) + rp_ref_out.tgt[F(L)].abs.p.head(XY)); }
+            makeConvexHullOpenCV(f_hull_wld[R], f_hull_wld_ans[R]);
+            makeConvexHullOpenCV(f_hull_wld[L], f_hull_wld_ans[L]);
+            //      if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[R])){ rp_ref_out.tgt[lf].go_contact = true; }
+            //      if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[L])){ rp_ref_out.tgt[rf].go_contact = true; }
+            if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[R])){ cp_force_go_contact[L] = true; }else{ cp_force_go_contact[L] = false; }
+            if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[L])){ cp_force_go_contact[R] = true; }else{ cp_force_go_contact[R] = false; }
+
+            //加速CP条件(ACP使用)
+            hrp::Vector2 cp_acc_tmp = com_pos - com_vel * sqrt( H_cur / G );
+            hrp::Vector2 cp_acc_ragulated;
+            if(!isPointInHullOpenCV(cp_acc_tmp,hull_a)){
+                calcCrossPointOnHull(com_pos, cp_acc_tmp, hull_a, cp_acc_ragulated);
+                com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
+            }
+            cp_acc_old = cp_acc_ragulated;
+
+            static hrp::Vector2 com_vel_ans_old = com_vel;
+            //加速減速条件マージ
+            if( com_vel_decel_ok.dot(com_vel) < com_vel_accel_ok.dot(com_vel)){//normじゃダメ？
+                com_vel_ans = com_vel_decel_ok;
+            }else{
+                com_vel_ans = com_vel_accel_ok;
+            }
+            com_vel_ans_old = com_vel_ans;
+        }
+        //    void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull_d, const std::vector<hrp::Vector2>& hull_a, hrp::Vector2& com_vel_ans){
+        //      hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
+        //      com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
+        //      //減速CP条件(現在のCPを常に両足裏で頭打ち)
+        //      hrp::Vector2 cp_dec_tmp = com_pos + com_vel * sqrt( H_cur / G );
+        //      hrp::Vector2 cp_dec_ragulated;
+        //      if(!isPointInHullOpenCV(cp_dec_tmp,hull_d)){
+        //        calcCrossPointOnHull(com_pos, cp_dec_tmp, hull_d, cp_dec_ragulated);
+        //        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
+        //      }
+        //
+        //      //減速CP条件2(指令のCPに対する現在発揮可能なCP)
+        ////      hrp::Vector2 cp_dec_ref = com_forcp_ref + com_vel_forcp_ref * sqrt( H_cur / G );
+        ////      hrp::Vector2 cp_dec_ref_ragulated = cp_dec_ref;
+        ////      if(!isPointInHullOpenCV(cp_dec_ref,hull_d)){
+        ////        calcCrossPointOnHull(com_pos, cp_dec_ref, hull_d, cp_dec_ref_ragulated);
+        ////      }
+        ////      com_vel_decel_ok = (cp_dec_ref_ragulated - com_pos) / sqrt( H_cur / G );
+        //
+        //      //減速CP条件2(指令のCOMに収束する現在発揮可能なCP)
+        ////      hrp::Vector2 cp_dec_ref = com_forcp_ref;
+        ////      hrp::Vector2 cp_dec_ref_ragulated = cp_dec_ref;
+        ////      if(!isPointInHullOpenCV(cp_dec_ref,hull_d)){
+        ////        calcCrossPointOnHull(com_pos, cp_dec_ref, hull_d, cp_dec_ref_ragulated);
+        ////      }
+        ////      hrp::Vector2 vel_max_decel_ok = (cp_dec_ref_ragulated - com_pos) / sqrt( H_cur / G );
+        ////      if( com_vel.dot(com_vel) > vel_max_decel_ok.dot(com_vel)){
+        ////        com_vel_decel_ok = vel_max_decel_ok;
+        ////      }else{
+        ////        com_vel_decel_ok = com_vel;
+        ////      }
+        //
+        //      //加速CP条件(ACP使用)
+        //      hrp::Vector2 cp_acc_tmp = com_pos - com_vel * sqrt( H_cur / G );
+        //      hrp::Vector2 cp_acc_ragulated;
+        //      if(!isPointInHullOpenCV(cp_acc_tmp,hull_a)){
+        //         calcCrossPointOnHull(com_pos, cp_acc_tmp, hull_a, cp_acc_ragulated);
+        //         com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
+        //      }
+        //      cp_acc_old = cp_acc_ragulated;
+        //
+        //      //加速CP条件2(ZMP使用)
+        //      static hrp::Vector2 com_vel_ans_old = com_vel;
+        ////      hrp::Vector2 com_acc = (com_vel - com_vel_ans_old) / DT;
+        ////      hrp::Vector2 zmp_in = com_pos - com_acc / G * H_cur;
+        ////      hrp::Vector2 zmp_regulated;
+        ////      if(!isPointInHullOpenCV(zmp_in,hull_a)){
+        ////        calcCrossPointOnHull(com_pos, zmp_in, hull_a, zmp_regulated);
+        ////        hrp::Vector2 com_acc_regulated = (com_pos - zmp_regulated)*G/H_cur;
+        ////        com_vel_accel_ok = com_vel_ans_old + com_acc_regulated*DT;
+        ////      }
+        //
+        //      //加速減速条件マージ
+        //      if( com_vel_decel_ok.dot(com_vel) < com_vel_accel_ok.dot(com_vel)){//normじゃダメ？
+        //        com_vel_ans = com_vel_decel_ok;
+        //      }else{
+        //        com_vel_ans = com_vel_accel_ok;
+        //      }
+        //      com_vel_ans_old = com_vel_ans;
+        //    }
+        void surpressGoContactChattering(HumanPose& out){
+            //      static std::deque<int> buffer[LR];
+            static int count[LR];
+            for(int i=0;i<LR;i++){
+                if(out.tgt[F(i)].go_contact){ count[i]++; }
+                else{ count[i]--; }
+                LIMIT_MINMAX(count[i],0,100);
+                if(count[i]>0){ out.tgt[F(i)].go_contact = true; }
+            }
+        }
+        void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
+            rflf_points.clear();
+            hull_ans.clear();
+            for(int i=0;i<2;i++){
+                for(int j=2;j<4;j++){
+                    rflf_points.push_back(hrp::Vector2(rfin_abs(X) + rf_mgn(i),    rfin_abs(Y) + rf_mgn(j)));
+                    rflf_points.push_back(hrp::Vector2(lfin_abs(X) + lf_mgn(i),    lfin_abs(Y) + lf_mgn(j)));
+                }
+            }
+            makeConvexHullOpenCV(rflf_points, hull_ans);
+        }
+        void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const hrp::dvector6& rfwin, const hrp::dvector6& lfwin, hrp::Vector3& zmp_ans){
+            hrp::Vector3 rfzmp,lfzmp;
+            const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
+            if( rfwin(fz) > 1.0e-6 ){
+                rfzmp(0) = ( - rfwin(ty) - rfwin(fx) * F_H_OFFSET + rfwin(fz) * 0 ) / rfwin(fz) + rfpos(0);
+                rfzmp(1) = (   rfwin(tx) - rfwin(fy) * F_H_OFFSET + rfwin(fz) * 0 ) / rfwin(fz) + rfpos(1);
+            }
+            if( lfwin(fz) > 1.0e-6 ){
+                lfzmp(0) = ( - lfwin(ty) - lfwin(fx) * F_H_OFFSET + lfwin(fz) * 0 ) / lfwin(fz) + lfpos(0);
+                lfzmp(1) = (   lfwin(tx) - lfwin(fy) * F_H_OFFSET + lfwin(fz) * 0 ) / lfwin(fz) + lfpos(1);
+            }
+            if( rfwin(fz) > 1.0e-6 || lfwin(fz) > 1.0e-6 ){
+                zmp_ans(0) = ( rfzmp(0)*rfwin(fz) + lfzmp(0)*lfwin(fz) ) / ( rfwin(fz) + lfwin(fz));
+                zmp_ans(1) = ( rfzmp(1)*rfwin(fz) + lfzmp(1)*lfwin(fz) ) / ( rfwin(fz) + lfwin(fz));
+            }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
+            zmp_ans(2) = 0;
+        }
+        void calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans){
+            hrp::Vector4 margin_abs;
+            hrp::Vector2 cross_pt, anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
+            for(int direc=0;direc<4;direc++){
+                calcCrossPointOnHull(check_point, check_point+anchor_vec[direc], hull, cross_pt);
+                margin_abs(direc) = (cross_pt - check_point).norm();
+            }
+            margin_ans(0) =  margin_abs(0);
+            margin_ans(1) = -margin_abs(1);
+            margin_ans(2) =  margin_abs(2);
+            margin_ans(3) = -margin_abs(3);
+        }
+        bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_will_cross){
+            hrp::Vector2 anchor_vec = pt_out_goal - pt_in_start;
+            for(int i=0;i<hull.size();i++){
+                int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
+                hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
+                hrp::Vector2 cur_edge = nxt_pt - cur_pt;
+                double dBunbo = hrpVector2Cross(anchor_vec,cur_edge);
+                if( dBunbo != 0.0) {//平行の場合を外す
+                    hrp::Vector2 vectorAC = hrp::Vector2(hull[i](0),hull[i](1)) - pt_in_start;
+                    double dR = hrpVector2Cross(vectorAC,cur_edge) / dBunbo;
+                    double dS = hrpVector2Cross(vectorAC,anchor_vec) / dBunbo;
+                    if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){//dRには数値誤差が乗る
+                        pt_will_cross = pt_in_start + dR * anchor_vec;
+                        //dist = dR * anchor_vec.norm();
+                        return true;
+                    }
+                }
+            }
+            return false;
+        }
+        double calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_ans){
+            double cur_nearest_dist, ans_nearest_dist;
+            hrp::Vector2 cur_nearest_pt, ans_nearest_pt;
+            for(int i=0;i<hull.size();i++){
+                int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
+                hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
+                hrp::Vector2 cur_edge = nxt_pt - cur_pt;
+                hrp::Vector2 tgt_pt_v = tgt_pt - cur_pt;
+                double tgt_pt_projected_length = tgt_pt_v.dot(cur_edge.normalized());
+                if(tgt_pt_projected_length > cur_edge.norm() ){//tgt_pt's nearest point is on the　i+1-th vertex
+                    cur_nearest_pt = nxt_pt;
+                }else if(tgt_pt_projected_length < 0 ){//tgt_pt's nearest point is on the　i-th vertex
+                    cur_nearest_pt = cur_pt;
+                }else{//tgt_pt's nearest point is on the line
+                    cur_nearest_pt = cur_pt + tgt_pt_projected_length * cur_edge.normalized();
+                }
+                cur_nearest_dist = (tgt_pt - cur_nearest_pt).norm();
+                if(i==0){//set first candidate as nearest
+                    ans_nearest_dist = cur_nearest_dist;
+                    ans_nearest_pt = cur_nearest_pt;
+                }else if( cur_nearest_dist < ans_nearest_dist ){//update nearest candidate
+                    ans_nearest_dist = cur_nearest_dist;
+                    ans_nearest_pt = cur_nearest_pt;
+                }
+            }
+            pt_ans = ans_nearest_pt;
+            return ans_nearest_dist;
+        }
+        void makeConvexHullOpenCV(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
+            points.clear();
+            cvhull.clear();
+            hull_ans.clear();
+            for(int i=0;i<pts.size();i++){ points.push_back( cv::Point2f( pts[i](0), pts[i](1)) ); }
+            cv::convexHull(cv::Mat(points),cvhull,true);
+            for(int i=0;i<cvhull.size();i++){ hull_ans.push_back( hrp::Vector2( cvhull[i].x, cvhull[i].y ) );  }
+        }
+        //void makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
+        //  const int dim = 2;
+        //  double points[dim*pts.size()];
+        //  for(int i=0;i<pts.size();i++){ points[dim*i] = pts[i](0); points[dim*i+1] = pts[i](1); }
+        //  qh_new_qhull( dim, pts.size(), points, false, "qhull ", NULL, stderr);
+        //  pointT *point, *pointtemp;
+        //  hull_ans.clear();
+        //  FORALLpoints hull_ans.push_back( hrp::Vector2(point[0],point[1]) );
+        //  qh_freeqhull(!qh_ALL);
+        //  //#define qh_ORIENTclock 1
+        //}
+        bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull){
+            cvhull.clear();
+            for(int i=0;i<hull.size();i++){ cvhull.push_back( cv::Point2f( hull[i](0), hull[i](1)) ); }
+            return (cv::pointPolygonTest(cv::Mat(cvhull), cv::Point2f(pt(0),pt(1)), false) > 0);
+        }
+        void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
+            comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
+            comacc = acc4zmp_v_filters.passFilter(comacc);
+            zmpout(X) = comin(X)-(H_cur/G)*comacc(X);
+            zmpout(Y) = comin(Y)-(H_cur/G)*comacc(Y);
+            com_oldold = com_old;
+            com_old = comin;
+        }
+        void applyVelLimit(const HumanPose& out_old, HumanPose& out){
+            const double EE_MAX_VEL = 2.0;// m/s
+            for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
+                hrp::Vector3 diff = out.tgt[i].abs.p  - out_old.tgt[i].abs.p;
+                for(int j=0;j<3;j++)LIMIT_MINMAX( diff(j), -EE_MAX_VEL*DT, EE_MAX_VEL*DT);
+                out.tgt[i].abs.p = out_old.tgt[i].abs.p + diff;
+            }
+        }
 };
 
 #endif // HUMANMASTERSLAVE_H

From 589b3fd4920ab2272ee0532fdc6dc4488200ea96 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 4 Sep 2017 20:29:54 +0900
Subject: [PATCH 124/217] speed down at resumeWholeBodyMasterSlave

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 15 +++++++++++----
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h   |  2 ++
 2 files changed, 13 insertions(+), 4 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 051f68c8c70..5b2668183eb 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -194,6 +194,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     invdyn_zmp_filters.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
     final_ref_zmp_filter.setParameter(5, 1/m_dt, Q_BUTTERWORTH);
 
+    avg_q_vel = 1.5;
+
     std::cerr << "[" << m_profile.instance_name << "] onInitialize() OK" << std::endl;
     return RTC::RTC_OK;
 }
@@ -374,6 +376,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
             //        bool rf_is_contact =
             m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = hsp->rp_ref_out.tgt[rf].is_contact;
             m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = hsp->rp_ref_out.tgt[lf].is_contact;
+            dbg(hsp->rp_ref_out.tgt[rf].is_contact);
+            dbg(hsp->rp_ref_out.tgt[lf].is_contact);
         }
         hsp->baselinkpose.p = m_robot->rootLink()->p;
         hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
@@ -658,7 +662,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     fik_in->q_ref_pullback_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
 
     struct timespec startT, endT;
-    const int IK_MAX_LOOP = 3;
+    const int IK_MAX_LOOP = 2;
     clock_gettime(CLOCK_REALTIME, &startT);
     int loop_result = fik_in->solveFullbodyIKLoop(m_robot, ikc_list, IK_MAX_LOOP);
     if(loop%100==0){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveIK"<<loop_result<<"loop" << std::endl;}
@@ -725,7 +729,7 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
     const double min_goal_time_offset = 0.1;
     //  const double avg_q_vel = 0.5;
     //  const double avg_q_vel = 1.0;
-    const double avg_q_vel = 1.5;
+//    const double avg_q_vel = 1.5;
     for(int i=0;i<robot_in->numJoints();i++){
         double tmp_time = fabs(robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
         if(tmp_time > goal_time){ goal_time = tmp_time; }
@@ -786,11 +790,14 @@ bool WholeBodyMasterSlave::pauseWholeBodyMasterSlave(){
 
 bool WholeBodyMasterSlave::resumeWholeBodyMasterSlave(){
     if(mode.now() == MODE_PAUSE){
-        std::cerr << "[" << m_profile.instance_name << "] pauseWholeBodyMasterSlave" << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "] resumeWholeBodyMasterSlave" << std::endl;
         mode.setModeRequest(MODE_WBMS);
+        avg_q_vel /= 5;
+        sleep(5);
+        avg_q_vel *= 5;
         return true;
     }else{
-        std::cerr << "[" << m_profile.instance_name << "] Invalid context to pauseWholeBodyMasterSlave" << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "] Invalid context to resumeWholeBodyMasterSlave" << std::endl;
         return false;
     }
 }
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 49113a65d02..b0df4b02de1 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -184,6 +184,8 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
     bool is_legged_robot;
     unsigned int loop;
 
+    double avg_q_vel;
+
     hrp::InvDynStateBuffer idsb;
     BiquadIIRFilterVec invdyn_zmp_filters;
 

From 6742bcab5d3b0aa90b73f694ed69ab5612b77dad Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 5 Sep 2017 00:47:08 +0900
Subject: [PATCH 125/217] fix abc IK bug and falldown

---
 rtc/AutoBalancer/AutoBalancer.cpp | 2462 ++++++++++++++---------------
 1 file changed, 1229 insertions(+), 1233 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 6c977c670cd..800cc159665 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -23,7 +23,7 @@ using namespace rats;
 // Module specification
 // <rtc-template block="module_spec">
 static const char* autobalancer_spec[] =
-{
+    {
         "implementation_id", "AutoBalancer",
         "type_name",         "AutoBalancer",
         "description",       "autobalancer component",
@@ -37,7 +37,7 @@ static const char* autobalancer_spec[] =
         // Configuration variables
         "conf.default.debugLevel", "0",
         ""
-};
+    };
 // </rtc-template>
 
 static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
@@ -45,43 +45,43 @@ static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
     int pre = os.precision();
     os.setf(std::ios::fixed);
     os << std::setprecision(6)
-    << (tm.sec + tm.nsec/1e9)
-    << std::setprecision(pre);
+       << (tm.sec + tm.nsec/1e9)
+       << std::setprecision(pre);
     os.unsetf(std::ios::fixed);
     return os;
 }
 
 AutoBalancer::AutoBalancer(RTC::Manager* manager)
-: RTC::DataFlowComponentBase(manager),
-  // <rtc-template block="initializer">
-  m_qRefIn("qRef", m_qRef),
-  m_basePosIn("basePosIn", m_basePos),
-  m_baseRpyIn("baseRpyIn", m_baseRpy),
-  m_zmpIn("zmpIn", m_zmp),
-  m_optionalDataIn("optionalData", m_optionalData),
-  m_emergencySignalIn("emergencySignal", m_emergencySignal),
-  m_diffCPIn("diffCapturePoint", m_diffCP),
-  m_refFootOriginExtMomentIn("refFootOriginExtMoment", m_refFootOriginExtMoment),
-  m_refFootOriginExtMomentIsHoldValueIn("refFootOriginExtMomentIsHoldValue", m_refFootOriginExtMomentIsHoldValue),
-  m_actContactStatesIn("actContactStates", m_actContactStates),
-  m_qOut("q", m_qRef),
-  m_zmpOut("zmpOut", m_zmp),
-  m_basePosOut("basePosOut", m_basePos),
-  m_baseRpyOut("baseRpyOut", m_baseRpy),
-  m_baseTformOut("baseTformOut", m_baseTform),
-  m_basePoseOut("basePoseOut", m_basePose),
-  m_accRefOut("accRef", m_accRef),
-  m_contactStatesOut("contactStates", m_contactStates),
-  m_toeheelRatioOut("toeheelRatio", m_toeheelRatio),
-  m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime),
-  m_walkingStatesOut("walkingStates", m_walkingStates),
-  m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
-  m_cogOut("cogOut", m_cog),
-  m_AutoBalancerServicePort("AutoBalancerService"),
-  // </rtc-template>
-  gait_type(BIPED),
-  m_robot(hrp::BodyPtr()),
-  m_debugLevel(0)
+    : RTC::DataFlowComponentBase(manager),
+      // <rtc-template block="initializer">
+      m_qRefIn("qRef", m_qRef),
+      m_basePosIn("basePosIn", m_basePos),
+      m_baseRpyIn("baseRpyIn", m_baseRpy),
+      m_zmpIn("zmpIn", m_zmp),
+      m_optionalDataIn("optionalData", m_optionalData),
+      m_emergencySignalIn("emergencySignal", m_emergencySignal),
+      m_diffCPIn("diffCapturePoint", m_diffCP),
+      m_refFootOriginExtMomentIn("refFootOriginExtMoment", m_refFootOriginExtMoment),
+      m_refFootOriginExtMomentIsHoldValueIn("refFootOriginExtMomentIsHoldValue", m_refFootOriginExtMomentIsHoldValue),
+      m_actContactStatesIn("actContactStates", m_actContactStates),
+      m_qOut("q", m_qRef),
+      m_zmpOut("zmpOut", m_zmp),
+      m_basePosOut("basePosOut", m_basePos),
+      m_baseRpyOut("baseRpyOut", m_baseRpy),
+      m_baseTformOut("baseTformOut", m_baseTform),
+      m_basePoseOut("basePoseOut", m_basePose),
+      m_accRefOut("accRef", m_accRef),
+      m_contactStatesOut("contactStates", m_contactStates),
+      m_toeheelRatioOut("toeheelRatio", m_toeheelRatio),
+      m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime),
+      m_walkingStatesOut("walkingStates", m_walkingStates),
+      m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
+      m_cogOut("cogOut", m_cog),
+      m_AutoBalancerServicePort("AutoBalancerService"),
+      // </rtc-template>
+      gait_type(BIPED),
+      m_robot(hrp::BodyPtr()),
+      m_debugLevel(0)
 {
     m_service0.autobalancer(this);
 }
@@ -152,10 +152,10 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     nameServer = nameServer.substr(0, comPos);
     RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
     if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
-            CosNaming::NamingContext::_duplicate(naming.getRootContext())
-    )){
-        std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
-        return RTC::RTC_ERROR;
+                                 CosNaming::NamingContext::_duplicate(naming.getRootContext())
+                                 )){
+      std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
+      return RTC::RTC_ERROR;
     }
 
     // allocate memory for outPorts
@@ -173,7 +173,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     leg_names.push_back("lleg");
 
     // Generate FIK
-    //    fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
+//    fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
     fik = fikPtr(new FullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
 
     // setting from conf file
@@ -181,77 +181,77 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
     size_t prop_num = 10;
     if (end_effectors_str.size() > 0) {
-        size_t num = end_effectors_str.size()/prop_num;
-        for (size_t i = 0; i < num; i++) {
-            std::string ee_name, ee_target, ee_base;
-            coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
-            coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
-            coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
-            ABCIKparam tp;
-            hrp::Link* root = m_robot->link(ee_target);
-            for (size_t j = 0; j < 3; j++) {
-                coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
-            }
-            double tmpv[4];
-            for (int j = 0; j < 4; j++ ) {
-                coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
-            }
-            tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
-            // FIK param
-            SimpleFullbodyInverseKinematicsSolver::IKparam tmp_fikp;
-            tmp_fikp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
-            tmp_fikp.target_link = m_robot->link(ee_target);
-            tmp_fikp.localPos = tp.localPos;
-            tmp_fikp.localR = tp.localR;
-            tmp_fikp.max_limb_length = 0.0;
-            while (!root->isRoot()) {
-                tmp_fikp.max_limb_length += root->b.norm();
-                tmp_fikp.parent_name = root->name;
-                root = root->parent;
-            }
-            fik->ikp.insert(std::pair<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>(ee_name, tmp_fikp));
-            // Fix for toe joint
-            //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
-            //   Without toe joints, "end-effector link == force-sensor link" is assumed.
-            //   With toe joints, "end-effector link != force-sensor link" is assumed.
-            if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
-                std::vector<double> optw(fik->ikp[ee_name].manip->numJoints(), 1.0);
-                optw.back() = 0.0; // Set weight = 0 for toe joint by default
-                fik->ikp[ee_name].manip->setOptionalWeightVector(optw);
-                tp.has_toe_joint = true;
-            } else {
-                tp.has_toe_joint = false;
-            }
-            tp.target_link = m_robot->link(ee_target);
-            ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
-            ee_vec.push_back(ee_name);
-            std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
-            std::cerr << "[" << m_profile.instance_name << "]   target = " << ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
-            std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-            std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
-            contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
+      size_t num = end_effectors_str.size()/prop_num;
+      for (size_t i = 0; i < num; i++) {
+        std::string ee_name, ee_target, ee_base;
+        coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
+        coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
+        coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
+        ABCIKparam tp;
+        hrp::Link* root = m_robot->link(ee_target);
+        for (size_t j = 0; j < 3; j++) {
+          coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
         }
-        m_contactStates.data.length(num);
-        m_toeheelRatio.data.length(num);
-        if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
-            m_contactStates.data[contact_states_index_map["rleg"]] = true;
-            m_contactStates.data[contact_states_index_map["lleg"]] = true;
+        double tmpv[4];
+        for (int j = 0; j < 4; j++ ) {
+          coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
         }
-        if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) {
-            m_contactStates.data[contact_states_index_map["rarm"]] = false;
-            m_contactStates.data[contact_states_index_map["larm"]] = false;
+        tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
+        // FIK param
+        SimpleFullbodyInverseKinematicsSolver::IKparam tmp_fikp;
+        tmp_fikp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
+        tmp_fikp.target_link = m_robot->link(ee_target);
+        tmp_fikp.localPos = tp.localPos;
+        tmp_fikp.localR = tp.localR;
+        tmp_fikp.max_limb_length = 0.0;
+        while (!root->isRoot()) {
+          tmp_fikp.max_limb_length += root->b.norm();
+          tmp_fikp.parent_name = root->name;
+          root = root->parent;
+        }
+        fik->ikp.insert(std::pair<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>(ee_name, tmp_fikp));
+        // Fix for toe joint
+        //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
+        //   Without toe joints, "end-effector link == force-sensor link" is assumed.
+        //   With toe joints, "end-effector link != force-sensor link" is assumed.
+        if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
+            std::vector<double> optw(fik->ikp[ee_name].manip->numJoints(), 1.0);
+            optw.back() = 0.0; // Set weight = 0 for toe joint by default
+            fik->ikp[ee_name].manip->setOptionalWeightVector(optw);
+            tp.has_toe_joint = true;
+        } else {
+            tp.has_toe_joint = false;
         }
-        m_controlSwingSupportTime.data.length(num);
-        for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 0.0;
-        for (size_t i = 0; i < num; i++) m_toeheelRatio.data[i] = rats::no_using_toe_heel_ratio;
+        tp.target_link = m_robot->link(ee_target);
+        ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
+        ee_vec.push_back(ee_name);
+        std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   target = " << ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+        std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
+        contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
+      }
+      m_contactStates.data.length(num);
+      m_toeheelRatio.data.length(num);
+      if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
+        m_contactStates.data[contact_states_index_map["rleg"]] = true;
+        m_contactStates.data[contact_states_index_map["lleg"]] = true;
+      }
+      if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) {
+        m_contactStates.data[contact_states_index_map["rarm"]] = false;
+        m_contactStates.data[contact_states_index_map["larm"]] = false;
+      }
+      m_controlSwingSupportTime.data.length(num);
+      for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 0.0;
+      for (size_t i = 0; i < num; i++) m_toeheelRatio.data[i] = rats::no_using_toe_heel_ratio;
     }
     std::vector<hrp::Vector3> leg_pos;
     if (leg_offset_str.size() > 0) {
-        hrp::Vector3 leg_offset;
-        for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
-        std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-        leg_pos.push_back(hrp::Vector3(-1*leg_offset));
-        leg_pos.push_back(hrp::Vector3(leg_offset));
+      hrp::Vector3 leg_offset;
+      for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
+      std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+      leg_pos.push_back(hrp::Vector3(-1*leg_offset));
+      leg_pos.push_back(hrp::Vector3(leg_offset));
     }
     if (leg_pos.size() < ikp.size()) {
         size_t tmp_leg_pos_size = leg_pos.size();
@@ -289,12 +289,12 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     double stride_inside_th_limit = stride_outside_th_limit*0.5;
     std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_outside_y_limit << "[m], " << stride_outside_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl;
     if (default_zmp_offsets.size() == 0) {
-        for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
+      for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
     }
     if (leg_offset_str.size() > 0) {
-        gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_outside_y_limit/*[m]*/, stride_outside_th_limit/*[deg]*/,
-                stride_bwd_x_limit/*[m]*/, stride_inside_y_limit/*[m]*/, stride_inside_th_limit/*[m]*/));
-        gg->set_default_zmp_offsets(default_zmp_offsets);
+      gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_outside_y_limit/*[m]*/, stride_outside_th_limit/*[deg]*/,
+                                          stride_bwd_x_limit/*[m]*/, stride_inside_y_limit/*[m]*/, stride_inside_th_limit/*[m]*/));
+      gg->set_default_zmp_offsets(default_zmp_offsets);
     }
     gg_is_walking = gg_solved = false;
     m_walkingStates.data = false;
@@ -359,9 +359,9 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     use_force = MODE_REF_FORCE;
 
     if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
-        is_legged_robot = true;
+      is_legged_robot = true;
     } else {
-        is_legged_robot = false;
+      is_legged_robot = false;
     }
 
     m_accRef.data.ax = m_accRef.data.ay = m_accRef.data.az = 0.0;
@@ -388,11 +388,11 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
 
 RTC::ReturnCode_t AutoBalancer::onFinalize()
 {
-    delete zmp_offset_interpolator;
-    delete transition_interpolator;
-    delete adjust_footstep_interpolator;
-    delete leg_names_interpolator;
-    return RTC::RTC_OK;
+  delete zmp_offset_interpolator;
+  delete transition_interpolator;
+  delete adjust_footstep_interpolator;
+  delete leg_names_interpolator;
+  return RTC::RTC_OK;
 }
 
 /*
@@ -400,14 +400,14 @@ RTC::ReturnCode_t AutoBalancer::onFinalize()
   {
   return RTC::RTC_OK;
   }
- */
+*/
 
 /*
   RTC::ReturnCode_t AutoBalancer::onShutdown(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
- */
+*/
 
 RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id)
 {
@@ -418,14 +418,14 @@ RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id)
 
 RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
 {
-    std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
-    Guard guard(m_mutex);
-    if (control_mode == MODE_ABC) {
-        control_mode = MODE_SYNC_TO_IDLE;
-        double tmp_ratio = 0.0;
-        transition_interpolator->setGoal(&tmp_ratio, m_dt, true); // sync in one controller loop
-    }
-    return RTC::RTC_OK;
+  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
+  Guard guard(m_mutex);
+  if (control_mode == MODE_ABC) {
+    control_mode = MODE_SYNC_TO_IDLE;
+    double tmp_ratio = 0.0;
+    transition_interpolator->setGoal(&tmp_ratio, m_dt, true); // sync in one controller loop
+  }
+  return RTC::RTC_OK;
 }
 
 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
@@ -433,7 +433,7 @@ RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
 #define DEBUGP2 (false)
 RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
 {
-    // std::cerr << "AutoBalancer::onExecute(" << ec_id << ")" << std::endl;
+  // std::cerr << "AutoBalancer::onExecute(" << ec_id << ")" << std::endl;
     loop ++;
 
     // Read Inport
@@ -441,20 +441,20 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
         m_qRefIn.read();
     }
     if (m_basePosIn.isNew()) {
-        m_basePosIn.read();
-        input_basePos(0) = m_basePos.data.x;
-        input_basePos(1) = m_basePos.data.y;
-        input_basePos(2) = m_basePos.data.z;
+      m_basePosIn.read();
+      input_basePos(0) = m_basePos.data.x;
+      input_basePos(1) = m_basePos.data.y;
+      input_basePos(2) = m_basePos.data.z;
     }
     if (m_baseRpyIn.isNew()) {
-        m_baseRpyIn.read();
-        input_baseRot = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
+      m_baseRpyIn.read();
+      input_baseRot = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
     }
     if (m_zmpIn.isNew()) {
-        m_zmpIn.read();
-        input_zmp(0) = m_zmp.data.x;
-        input_zmp(1) = m_zmp.data.y;
-        input_zmp(2) = m_zmp.data.z;
+      m_zmpIn.read();
+      input_zmp(0) = m_zmp.data.x;
+      input_zmp(1) = m_zmp.data.y;
+      input_zmp(2) = m_zmp.data.z;
     }
     for (unsigned int i=0; i<m_ref_forceIn.size(); i++){
         if ( m_ref_forceIn[i]->isNew() ) {
@@ -473,22 +473,22 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
         // }
     }
     if (m_diffCPIn.isNew()) {
-        m_diffCPIn.read();
-        gg->set_diff_cp(hrp::Vector3(m_diffCP.data.x, m_diffCP.data.y, m_diffCP.data.z));
+      m_diffCPIn.read();
+      gg->set_diff_cp(hrp::Vector3(m_diffCP.data.x, m_diffCP.data.y, m_diffCP.data.z));
     }
     if (m_refFootOriginExtMomentIn.isNew()) {
-        m_refFootOriginExtMomentIn.read();
+      m_refFootOriginExtMomentIn.read();
     }
     if (m_refFootOriginExtMomentIsHoldValueIn.isNew()) {
-        m_refFootOriginExtMomentIsHoldValueIn.read();
+      m_refFootOriginExtMomentIsHoldValueIn.read();
     }
     if (m_actContactStatesIn.isNew()) {
-        m_actContactStatesIn.read();
-        std::vector<bool> tmp_contacts(m_actContactStates.data.length());
-        for (size_t i = 0; i < m_actContactStates.data.length(); i++) {
-            tmp_contacts[i] = m_actContactStates.data[i];
-        }
-        gg->set_act_contact_states(tmp_contacts);
+      m_actContactStatesIn.read();
+      std::vector<bool> tmp_contacts(m_actContactStates.data.length());
+      for (size_t i = 0; i < m_actContactStates.data.length(); i++) {
+        tmp_contacts[i] = m_actContactStates.data[i];
+      }
+      gg->set_act_contact_states(tmp_contacts);
     }
 
     // Calculation
@@ -497,167 +497,167 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     hrp::Matrix33 ref_baseRot;
     hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
     if ( is_legged_robot ) {
-        // For parameters
-        fik->storeCurrentParameters();
-        getTargetParameters();
-        // Get transition ratio
-        bool is_transition_interpolator_empty = transition_interpolator->isEmpty();
-        if (!is_transition_interpolator_empty) {
-            transition_interpolator->get(&transition_interpolator_ratio, true);
-        } else {
-            transition_interpolator_ratio = (control_mode == MODE_IDLE) ? 0.0 : 1.0;
-        }
-        if (control_mode != MODE_IDLE ) {
-            solveFullbodyIK();
-            //        /////// Inverse Dynamics /////////
-            //        if(!idsb.is_initialized){
-            //          idsb.setInitState(m_robot, m_dt);
-            //          invdyn_zmp_filters.resize(3);
-            //          for(int i=0;i<3;i++){
-            //            invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
-            //            invdyn_zmp_filters[i].reset(ref_zmp(i));
-            //          }
-            //        }
-            //        calcAccelerationsForInverseDynamics(m_robot, idsb);
-            //        if(gg_is_walking){
-            //          calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp);
-            //          for(int i=0;i<3;i++) ref_zmp(i) = invdyn_zmp_filters[i].passFilter(ref_zmp(i));
-            //        }
-            //        updateInvDynStateBuffer(idsb);
-
-            rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
-        } else {
-            rel_ref_zmp = input_zmp;
-            fik->d_root_height = 0.0;
+      // For parameters
+      fik->storeCurrentParameters();
+      getTargetParameters();
+      // Get transition ratio
+      bool is_transition_interpolator_empty = transition_interpolator->isEmpty();
+      if (!is_transition_interpolator_empty) {
+        transition_interpolator->get(&transition_interpolator_ratio, true);
+      } else {
+        transition_interpolator_ratio = (control_mode == MODE_IDLE) ? 0.0 : 1.0;
+      }
+      if (control_mode != MODE_IDLE ) {
+        solveFullbodyIK();
+//        /////// Inverse Dynamics /////////
+//        if(!idsb.is_initialized){
+//          idsb.setInitState(m_robot, m_dt);
+//          invdyn_zmp_filters.resize(3);
+//          for(int i=0;i<3;i++){
+//            invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
+//            invdyn_zmp_filters[i].reset(ref_zmp(i));
+//          }
+//        }
+//        calcAccelerationsForInverseDynamics(m_robot, idsb);
+//        if(gg_is_walking){
+//          calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp);
+//          for(int i=0;i<3;i++) ref_zmp(i) = invdyn_zmp_filters[i].passFilter(ref_zmp(i));
+//        }
+//        updateInvDynStateBuffer(idsb);
+
+        rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
+      } else {
+        rel_ref_zmp = input_zmp;
+        fik->d_root_height = 0.0;
+      }
+      // Transition
+      if (!is_transition_interpolator_empty) {
+        // transition_interpolator_ratio 0=>1 : IDLE => ABC
+        // transition_interpolator_ratio 1=>0 : ABC => IDLE
+        ref_basePos = (1-transition_interpolator_ratio) * input_basePos + transition_interpolator_ratio * m_robot->rootLink()->p;
+        rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp;
+        rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R);
+        for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ) {
+          m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q;
         }
-        // Transition
-        if (!is_transition_interpolator_empty) {
-            // transition_interpolator_ratio 0=>1 : IDLE => ABC
-            // transition_interpolator_ratio 1=>0 : ABC => IDLE
-            ref_basePos = (1-transition_interpolator_ratio) * input_basePos + transition_interpolator_ratio * m_robot->rootLink()->p;
-            rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp;
-            rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R);
-            for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ) {
-                m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q;
-            }
-            for (unsigned int i=0; i< m_force.size(); i++) {
-                for (unsigned int j=0; j<6; j++) {
-                    m_force[i].data[j] = transition_interpolator_ratio * m_force[i].data[j] + (1-transition_interpolator_ratio) * m_ref_force[i].data[j];
-                }
-            }
-            for (unsigned int i=0; i< m_limbCOPOffset.size(); i++) {
-                // transition (TODO:set stopABCmode value instead of 0)
-                m_limbCOPOffset[i].data.x = transition_interpolator_ratio * m_limbCOPOffset[i].data.x;// + (1-transition_interpolator_ratio) * 0;
-                m_limbCOPOffset[i].data.y = transition_interpolator_ratio * m_limbCOPOffset[i].data.y;// + (1-transition_interpolator_ratio) * 0;
-                m_limbCOPOffset[i].data.z = transition_interpolator_ratio * m_limbCOPOffset[i].data.z;// + (1-transition_interpolator_ratio) * 0;
+        for (unsigned int i=0; i< m_force.size(); i++) {
+            for (unsigned int j=0; j<6; j++) {
+                m_force[i].data[j] = transition_interpolator_ratio * m_force[i].data[j] + (1-transition_interpolator_ratio) * m_ref_force[i].data[j];
             }
-        } else {
-            ref_basePos = m_robot->rootLink()->p;
-            ref_baseRot = m_robot->rootLink()->R;
         }
-        // mode change for sync
-        if (control_mode == MODE_SYNC_TO_ABC) {
-            control_mode = MODE_ABC;
-        } else if (control_mode == MODE_SYNC_TO_IDLE && transition_interpolator->isEmpty() ) {
-            std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
-                    << "] Finished cleanup" << std::endl;
-            control_mode = MODE_IDLE;
+        for (unsigned int i=0; i< m_limbCOPOffset.size(); i++) {
+            // transition (TODO:set stopABCmode value instead of 0)
+            m_limbCOPOffset[i].data.x = transition_interpolator_ratio * m_limbCOPOffset[i].data.x;// + (1-transition_interpolator_ratio) * 0;
+            m_limbCOPOffset[i].data.y = transition_interpolator_ratio * m_limbCOPOffset[i].data.y;// + (1-transition_interpolator_ratio) * 0;
+            m_limbCOPOffset[i].data.z = transition_interpolator_ratio * m_limbCOPOffset[i].data.z;// + (1-transition_interpolator_ratio) * 0;
         }
+      } else {
+        ref_basePos = m_robot->rootLink()->p;
+        ref_baseRot = m_robot->rootLink()->R;
+      }
+      // mode change for sync
+      if (control_mode == MODE_SYNC_TO_ABC) {
+        control_mode = MODE_ABC;
+      } else if (control_mode == MODE_SYNC_TO_IDLE && transition_interpolator->isEmpty() ) {
+        std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
+                  << "] Finished cleanup" << std::endl;
+        control_mode = MODE_IDLE;
+      }
     }
 
     // Write Outport
     if ( m_qRef.data.length() != 0 ) { // initialized
-        if (is_legged_robot) {
-            for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
-                m_qRef.data[i] = m_robot->joint(i)->q;
-            }
+      if (is_legged_robot) {
+        for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
+          m_qRef.data[i] = m_robot->joint(i)->q;
         }
-        m_qOut.write();
+      }
+      m_qOut.write();
     }
     if (is_legged_robot) {
-        // basePos
-        m_basePos.data.x = ref_basePos(0);
-        m_basePos.data.y = ref_basePos(1);
-        m_basePos.data.z = ref_basePos(2);
-        m_basePos.tm = m_qRef.tm;
-        // baseRpy
-        hrp::Vector3 baseRpy = hrp::rpyFromRot(ref_baseRot);
-        m_baseRpy.data.r = baseRpy(0);
-        m_baseRpy.data.p = baseRpy(1);
-        m_baseRpy.data.y = baseRpy(2);
-        m_baseRpy.tm = m_qRef.tm;
-        // baseTform
-        double *tform_arr = m_baseTform.data.get_buffer();
-        tform_arr[0] = m_basePos.data.x;
-        tform_arr[1] = m_basePos.data.y;
-        tform_arr[2] = m_basePos.data.z;
-        hrp::setMatrix33ToRowMajorArray(ref_baseRot, tform_arr, 3);
-        m_baseTform.tm = m_qRef.tm;
-        // basePose
-        m_basePose.data.position.x = m_basePos.data.x;
-        m_basePose.data.position.y = m_basePos.data.y;
-        m_basePose.data.position.z = m_basePos.data.z;
-        m_basePose.data.orientation.r = m_baseRpy.data.r;
-        m_basePose.data.orientation.p = m_baseRpy.data.p;
-        m_basePose.data.orientation.y = m_baseRpy.data.y;
-        m_basePose.tm = m_qRef.tm;
-        // zmp
-        m_zmp.data.x = rel_ref_zmp(0);
-        m_zmp.data.y = rel_ref_zmp(1);
-        m_zmp.data.z = rel_ref_zmp(2);
-        m_zmp.tm = m_qRef.tm;
-        // cog
-        m_cog.data.x = ref_cog(0);
-        m_cog.data.y = ref_cog(1);
-        m_cog.data.z = ref_cog(2);
-        m_cog.tm = m_qRef.tm;
-        // sbpCogOffset
-        m_sbpCogOffset.data.x = sbp_cog_offset(0);
-        m_sbpCogOffset.data.y = sbp_cog_offset(1);
-        m_sbpCogOffset.data.z = sbp_cog_offset(2);
-        m_sbpCogOffset.tm = m_qRef.tm;
-        // write
-        m_basePosOut.write();
-        m_baseRpyOut.write();
-        m_baseTformOut.write();
-        m_basePoseOut.write();
-        m_zmpOut.write();
-        m_cogOut.write();
-        m_sbpCogOffsetOut.write();
-
-        // reference acceleration
-        hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
-        if (sen != NULL) {
-            hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos;
-            hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt;
-            // convert to imu sensor local acceleration
-            hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt;
-            m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2);
-            m_accRefOut.write();
-            prev_imu_sensor_pos = imu_sensor_pos;
-            prev_imu_sensor_vel = imu_sensor_vel;
-        }
-
-        // control parameters
-        m_contactStates.tm = m_qRef.tm;
-        m_contactStatesOut.write();
-        m_controlSwingSupportTime.tm = m_qRef.tm;
-        m_controlSwingSupportTimeOut.write();
-        m_toeheelRatio.tm = m_qRef.tm;
-        m_toeheelRatioOut.write();
-        m_walkingStates.data = gg_is_walking;
-        m_walkingStates.tm = m_qRef.tm;
-        m_walkingStatesOut.write();
-
-        for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
-            m_force[i].tm = m_qRef.tm;
-            m_ref_forceOut[i]->write();
-        }
-
-        for (unsigned int i=0; i<m_limbCOPOffsetOut.size(); i++){
-            m_limbCOPOffset[i].tm = m_qRef.tm;
-            m_limbCOPOffsetOut[i]->write();
-        }
+      // basePos
+      m_basePos.data.x = ref_basePos(0);
+      m_basePos.data.y = ref_basePos(1);
+      m_basePos.data.z = ref_basePos(2);
+      m_basePos.tm = m_qRef.tm;
+      // baseRpy
+      hrp::Vector3 baseRpy = hrp::rpyFromRot(ref_baseRot);
+      m_baseRpy.data.r = baseRpy(0);
+      m_baseRpy.data.p = baseRpy(1);
+      m_baseRpy.data.y = baseRpy(2);
+      m_baseRpy.tm = m_qRef.tm;
+      // baseTform
+      double *tform_arr = m_baseTform.data.get_buffer();
+      tform_arr[0] = m_basePos.data.x;
+      tform_arr[1] = m_basePos.data.y;
+      tform_arr[2] = m_basePos.data.z;
+      hrp::setMatrix33ToRowMajorArray(ref_baseRot, tform_arr, 3);
+      m_baseTform.tm = m_qRef.tm;
+      // basePose
+      m_basePose.data.position.x = m_basePos.data.x;
+      m_basePose.data.position.y = m_basePos.data.y;
+      m_basePose.data.position.z = m_basePos.data.z;
+      m_basePose.data.orientation.r = m_baseRpy.data.r;
+      m_basePose.data.orientation.p = m_baseRpy.data.p;
+      m_basePose.data.orientation.y = m_baseRpy.data.y;
+      m_basePose.tm = m_qRef.tm;
+      // zmp
+      m_zmp.data.x = rel_ref_zmp(0);
+      m_zmp.data.y = rel_ref_zmp(1);
+      m_zmp.data.z = rel_ref_zmp(2);
+      m_zmp.tm = m_qRef.tm;
+      // cog
+      m_cog.data.x = ref_cog(0);
+      m_cog.data.y = ref_cog(1);
+      m_cog.data.z = ref_cog(2);
+      m_cog.tm = m_qRef.tm;
+      // sbpCogOffset
+      m_sbpCogOffset.data.x = sbp_cog_offset(0);
+      m_sbpCogOffset.data.y = sbp_cog_offset(1);
+      m_sbpCogOffset.data.z = sbp_cog_offset(2);
+      m_sbpCogOffset.tm = m_qRef.tm;
+      // write
+      m_basePosOut.write();
+      m_baseRpyOut.write();
+      m_baseTformOut.write();
+      m_basePoseOut.write();
+      m_zmpOut.write();
+      m_cogOut.write();
+      m_sbpCogOffsetOut.write();
+
+      // reference acceleration
+      hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
+      if (sen != NULL) {
+          hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos;
+          hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt;
+          // convert to imu sensor local acceleration
+          hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt;
+          m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2);
+          m_accRefOut.write();
+          prev_imu_sensor_pos = imu_sensor_pos;
+          prev_imu_sensor_vel = imu_sensor_vel;
+      }
+
+      // control parameters
+      m_contactStates.tm = m_qRef.tm;
+      m_contactStatesOut.write();
+      m_controlSwingSupportTime.tm = m_qRef.tm;
+      m_controlSwingSupportTimeOut.write();
+      m_toeheelRatio.tm = m_qRef.tm;
+      m_toeheelRatioOut.write();
+      m_walkingStates.data = gg_is_walking;
+      m_walkingStates.tm = m_qRef.tm;
+      m_walkingStatesOut.write();
+
+      for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
+          m_force[i].tm = m_qRef.tm;
+          m_ref_forceOut[i]->write();
+      }
+
+      for (unsigned int i=0; i<m_limbCOPOffsetOut.size(); i++){
+          m_limbCOPOffset[i].tm = m_qRef.tm;
+          m_limbCOPOffsetOut[i]->write();
+      }
     }
 
     return RTC::RTC_OK;
@@ -665,99 +665,99 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
 
 void AutoBalancer::getTargetParameters()
 {
-    // joint angles
-    for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
-        m_robot->joint(i)->q = m_qRef.data[i];
-    }
-    fik->setReferenceJointAngles();
-    // basepos, rot, zmp
-    m_robot->rootLink()->p = input_basePos;
-    m_robot->rootLink()->R = input_baseRot;
-    m_robot->calcForwardKinematics();
-    gg->proc_zmp_weight_map_interpolation();
-    if (control_mode != MODE_IDLE) {
-        interpolateLegNamesAndZMPOffsets();
-        // Calculate tmp_fix_coords and something
-        coordinates tmp_fix_coords;
-        if ( gg_is_walking ) {
-            gg->set_default_zmp_offsets(default_zmp_offsets);
-            gg_solved = gg->proc_one_tick();
-            gg->get_swing_support_mid_coords(tmp_fix_coords);
-        } else {
-            tmp_fix_coords = fix_leg_coords;
-        }
-        if (!adjust_footstep_interpolator->isEmpty()) {
-            calcFixCoordsForAdjustFootstep(tmp_fix_coords);
-            fix_leg_coords = tmp_fix_coords;
-        }
-        // TODO : see explanation in this function
-        fixLegToCoords2(tmp_fix_coords);
-        fix_leg_coords2 = tmp_fix_coords;
-
-        // Get output parameters and target EE coords
-        target_root_p = m_robot->rootLink()->p;
-        target_root_R = m_robot->rootLink()->R;
-        if ( gg_is_walking ) {
-            getOutputParametersForWalking();
-        } else {
-            getOutputParametersForABC();
-        }
-        //   Just for ik initial value
-        if (control_mode == MODE_SYNC_TO_ABC) {
-            fik->current_root_p = target_root_p;
-            fik->current_root_R = target_root_R;
-            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-                if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
-                    it->second.target_p0 = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
-                    it->second.target_r0 = it->second.target_link->R * it->second.localR;
-                }
-            }
-        }
+  // joint angles
+  for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
+    m_robot->joint(i)->q = m_qRef.data[i];
+  }
+  fik->setReferenceJointAngles();
+  // basepos, rot, zmp
+  m_robot->rootLink()->p = input_basePos;
+  m_robot->rootLink()->R = input_baseRot;
+  m_robot->calcForwardKinematics();
+  gg->proc_zmp_weight_map_interpolation();
+  if (control_mode != MODE_IDLE) {
+    interpolateLegNamesAndZMPOffsets();
+    // Calculate tmp_fix_coords and something
+    coordinates tmp_fix_coords;
+    if ( gg_is_walking ) {
+      gg->set_default_zmp_offsets(default_zmp_offsets);
+      gg_solved = gg->proc_one_tick();
+      gg->get_swing_support_mid_coords(tmp_fix_coords);
+    } else {
+      tmp_fix_coords = fix_leg_coords;
+    }
+    if (!adjust_footstep_interpolator->isEmpty()) {
+        calcFixCoordsForAdjustFootstep(tmp_fix_coords);
+        fix_leg_coords = tmp_fix_coords;
+    }
+    // TODO : see explanation in this function
+    fixLegToCoords2(tmp_fix_coords);
+    fix_leg_coords2 = tmp_fix_coords;
 
-        // Calculate other parameters
-        updateTargetCoordsForHandFixMode (tmp_fix_coords);
-        rotateRefForcesForFixCoords (tmp_fix_coords);
-        // TODO : see explanation in this function
-        calculateOutputRefForces ();
-        // TODO : see explanation in this function
-        updateWalkingVelocityFromHandError(tmp_fix_coords);
-        calcReferenceJointAnglesForIK();
-
-        // Calculate ZMP, COG, and sbp targets
-        hrp::Vector3 tmp_ref_cog(m_robot->calcCM());
-        hrp::Vector3 tmp_foot_mid_pos = calcFootMidPosUsingZMPWeightMap ();
-        if (gg_is_walking) {
-            ref_cog = gg->get_cog();
-        } else {
-            ref_cog = tmp_foot_mid_pos;
-        }
-        ref_cog(2) = tmp_ref_cog(2);
-        if (gg_is_walking) {
-            ref_zmp = gg->get_refzmp();
-        } else {
-            ref_zmp(0) = ref_cog(0);
-            ref_zmp(1) = ref_cog(1);
-            ref_zmp(2) = tmp_foot_mid_pos(2);
-        }
-    } else { // MODE_IDLE
-        // Update fix_leg_coords based on input basePos and rot if MODE_IDLE
-        std::vector<coordinates> tmp_end_coords_list;
+    // Get output parameters and target EE coords
+    target_root_p = m_robot->rootLink()->p;
+    target_root_R = m_robot->rootLink()->R;
+    if ( gg_is_walking ) {
+      getOutputParametersForWalking();
+    } else {
+      getOutputParametersForABC();
+    }
+    //   Just for ik initial value
+    if (control_mode == MODE_SYNC_TO_ABC) {
+        fik->current_root_p = target_root_p;
+        fik->current_root_R = target_root_R;
         for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
             if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
-                tmp_end_coords_list.push_back(coordinates(it->second.target_link->p+it->second.target_link->R*it->second.localPos, it->second.target_link->R*it->second.localR));
-            }
-        }
-        multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
-        getOutputParametersForIDLE();
-        // Set force
-        for (unsigned int i=0; i< m_force.size(); i++) {
-            for (unsigned int j=0; j<6; j++) {
-                m_force[i].data[j] = m_ref_force[i].data[j];
+                it->second.target_p0 = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
+                it->second.target_r0 = it->second.target_link->R * it->second.localR;
             }
         }
     }
-    // Just for stop walking
-    if (gg_is_walking && !gg_solved) stopWalking ();
+
+    // Calculate other parameters
+    updateTargetCoordsForHandFixMode (tmp_fix_coords);
+    rotateRefForcesForFixCoords (tmp_fix_coords);
+    // TODO : see explanation in this function
+    calculateOutputRefForces ();
+    // TODO : see explanation in this function
+    updateWalkingVelocityFromHandError(tmp_fix_coords);
+    calcReferenceJointAnglesForIK();
+
+    // Calculate ZMP, COG, and sbp targets
+    hrp::Vector3 tmp_ref_cog(m_robot->calcCM());
+    hrp::Vector3 tmp_foot_mid_pos = calcFootMidPosUsingZMPWeightMap ();
+    if (gg_is_walking) {
+      ref_cog = gg->get_cog();
+    } else {
+      ref_cog = tmp_foot_mid_pos;
+    }
+    ref_cog(2) = tmp_ref_cog(2);
+    if (gg_is_walking) {
+      ref_zmp = gg->get_refzmp();
+    } else {
+      ref_zmp(0) = ref_cog(0);
+      ref_zmp(1) = ref_cog(1);
+      ref_zmp(2) = tmp_foot_mid_pos(2);
+    }
+  } else { // MODE_IDLE
+      // Update fix_leg_coords based on input basePos and rot if MODE_IDLE
+      std::vector<coordinates> tmp_end_coords_list;
+      for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+          if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
+              tmp_end_coords_list.push_back(coordinates(it->second.target_link->p+it->second.target_link->R*it->second.localPos, it->second.target_link->R*it->second.localR));
+          }
+      }
+      multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
+      getOutputParametersForIDLE();
+      // Set force
+      for (unsigned int i=0; i< m_force.size(); i++) {
+          for (unsigned int j=0; j<6; j++) {
+              m_force[i].data[j] = m_ref_force[i].data[j];
+          }
+      }
+  }
+  // Just for stop walking
+  if (gg_is_walking && !gg_solved) stopWalking ();
 };
 
 void AutoBalancer::getOutputParametersForWalking ()
@@ -860,18 +860,18 @@ void AutoBalancer::getOutputParametersForIDLE ()
 void AutoBalancer::interpolateLegNamesAndZMPOffsets()
 {
     if (!zmp_offset_interpolator->isEmpty()) {
-        double *default_zmp_offsets_output = new double[ikp.size()*3];
-        zmp_offset_interpolator->get(default_zmp_offsets_output, true);
-        for (size_t i = 0; i < ikp.size(); i++)
-            for (size_t j = 0; j < 3; j++)
-                default_zmp_offsets[i](j) = default_zmp_offsets_output[i*3+j];
-        delete[] default_zmp_offsets_output;
-        if (DEBUGP) {
-            std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl;
-            for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-                std::cerr << "[" << m_profile.instance_name << "]   " << it->first << " = " << default_zmp_offsets[contact_states_index_map[it->first]].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-            }
+      double *default_zmp_offsets_output = new double[ikp.size()*3];
+      zmp_offset_interpolator->get(default_zmp_offsets_output, true);
+      for (size_t i = 0; i < ikp.size(); i++)
+        for (size_t j = 0; j < 3; j++)
+          default_zmp_offsets[i](j) = default_zmp_offsets_output[i*3+j];
+      delete[] default_zmp_offsets_output;
+      if (DEBUGP) {
+        std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl;
+        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+            std::cerr << "[" << m_profile.instance_name << "]   " << it->first << " = " << default_zmp_offsets[contact_states_index_map[it->first]].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
         }
+      }
     }
     if (!leg_names_interpolator->isEmpty()) {
         leg_names_interpolator->get(&leg_names_interpolator_ratio, true);
@@ -902,18 +902,18 @@ void AutoBalancer::rotateRefForcesForFixCoords (coordinates& tmp_fix_coords)
 {
     /* update ref_forces ;; StateHolder's absolute -> AutoBalancer's absolute */
     for (size_t i = 0; i < m_ref_forceIn.size(); i++) {
-        // hrp::Matrix33 eeR;
-        // hrp::Link* parentlink;
-        // hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
-        // if (sensor) parentlink = sensor->link;
-        // else parentlink = m_vfs[sensor_names[i]].link;
-        // for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-        //     if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR;
-        // }
-        // End effector frame
-        //ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
-        // world frame
-        ref_forces[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
+      // hrp::Matrix33 eeR;
+      // hrp::Link* parentlink;
+      // hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
+      // if (sensor) parentlink = sensor->link;
+      // else parentlink = m_vfs[sensor_names[i]].link;
+      // for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+      //     if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR;
+      // }
+      // End effector frame
+      //ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
+      // world frame
+      ref_forces[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
     }
     sbp_offset = tmp_fix_coords.rot * hrp::Vector3(sbp_offset);
 };
@@ -929,12 +929,12 @@ void AutoBalancer::updateTargetCoordsForHandFixMode (coordinates& tmp_fix_coords
         bool is_hand_control_while_walking = false;
         for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
             if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
-                    && it->first.find("arm") != std::string::npos ) {
+                 && it->first.find("arm") != std::string::npos ) {
                 is_hand_control_while_walking = true;
             }
         }
         if (is_hand_control_while_walking) {
-            //if (false) { // Disabled temporarily
+        //if (false) { // Disabled temporarily
             // Store hand_fix_initial_offset in the initialization of walking
             if (is_hand_fix_initial) hand_fix_initial_offset = tmp_fix_coords.rot.transpose() * (hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos);
             is_hand_fix_initial = false;
@@ -946,7 +946,7 @@ void AutoBalancer::updateTargetCoordsForHandFixMode (coordinates& tmp_fix_coords
             }
             for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
                 if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
-                        && it->first.find("arm") != std::string::npos ) {
+                     && it->first.find("arm") != std::string::npos ) {
                     it->second.target_p0 = it->second.target_p0 + dif_p;
                 }
             }
@@ -1026,33 +1026,33 @@ void AutoBalancer::calcReferenceJointAnglesForIK ()
 
 hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2)
 {
-    hrp::Vector3 vv = axis1.cross(axis2);
-    if (fabs(vv.norm()-0.0) < 1e-5) {
-        return rot;
-    } else {
-        Eigen::AngleAxis<double> tmpr(std::asin(vv.norm()), vv.normalized());
-        return tmpr.toRotationMatrix() * rot;
-    }
+  hrp::Vector3 vv = axis1.cross(axis2);
+  if (fabs(vv.norm()-0.0) < 1e-5) {
+    return rot;
+  } else {
+    Eigen::AngleAxis<double> tmpr(std::asin(vv.norm()), vv.normalized());
+    return tmpr.toRotationMatrix() * rot;
+  }
 }
 
 void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot)
 {
-    // get current foot mid pos + rot
-    std::vector<coordinates> foot_coords;
-    for (size_t i = 0; i < leg_names.size(); i++) {
-        ABCIKparam& tmpikp = ikp[leg_names[i]];
-        if (leg_names[i].find("leg") != std::string::npos) foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
-                (tmpikp.target_link->R * tmpikp.localR)));
-    }
-    coordinates current_foot_mid_coords;
-    multi_mid_coords(current_foot_mid_coords, foot_coords);
-    hrp::Vector3 current_foot_mid_pos = current_foot_mid_coords.pos;
-    hrp::Matrix33 current_foot_mid_rot = current_foot_mid_coords.rot;
-    // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
-    hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose());
-    m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
-    rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
-    m_robot->calcForwardKinematics();
+  // get current foot mid pos + rot
+  std::vector<coordinates> foot_coords;
+  for (size_t i = 0; i < leg_names.size(); i++) {
+      ABCIKparam& tmpikp = ikp[leg_names[i]];
+      if (leg_names[i].find("leg") != std::string::npos) foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
+                                                                                           (tmpikp.target_link->R * tmpikp.localR)));
+  }
+  coordinates current_foot_mid_coords;
+  multi_mid_coords(current_foot_mid_coords, foot_coords);
+  hrp::Vector3 current_foot_mid_pos = current_foot_mid_coords.pos;
+  hrp::Matrix33 current_foot_mid_rot = current_foot_mid_coords.rot;
+  // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
+  hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose());
+  m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
+  rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
+  m_robot->calcForwardKinematics();
 }
 
 void AutoBalancer::fixLegToCoords2 (coordinates& tmp_fix_coords)
@@ -1061,15 +1061,15 @@ void AutoBalancer::fixLegToCoords2 (coordinates& tmp_fix_coords)
     // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272
     // Snap input tmp_fix_coords to XY plan projection.
     {
-        hrp::Vector3 ex = hrp::Vector3::UnitX();
-        hrp::Vector3 ez = hrp::Vector3::UnitZ();
-        hrp::Vector3 xv1 (tmp_fix_coords.rot * ex);
-        xv1(2) = 0.0;
-        xv1.normalize();
-        hrp::Vector3 yv1(ez.cross(xv1));
-        tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2);
-        tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2);
-        tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2);
+      hrp::Vector3 ex = hrp::Vector3::UnitX();
+      hrp::Vector3 ez = hrp::Vector3::UnitZ();
+      hrp::Vector3 xv1 (tmp_fix_coords.rot * ex);
+      xv1(2) = 0.0;
+      xv1.normalize();
+      hrp::Vector3 yv1(ez.cross(xv1));
+      tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2);
+      tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2);
+      tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2);
     }
     fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot);
 }
@@ -1084,8 +1084,7 @@ void AutoBalancer::solveFullbodyIK ()
         tmp.localR = hrp::Matrix33::Identity();
         tmp.targetPos = target_root_p;// will be ignored by selection_vec
         tmp.targetRpy = hrp::rpyFromRot(target_root_R);// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
-        tmp.constraint_weight << 0,0,0,1,1,0;
-        tmp.rot_precision = deg2rad(3);
+        tmp.constraint_weight << 0,0,0,1,1,1;
         ik_tgt_list.push_back(tmp);
     }{
         IKConstraint tmp;
@@ -1112,7 +1111,7 @@ void AutoBalancer::solveFullbodyIK ()
 //        tmp.localR = ikp["rarm"].localR;
 //        tmp.targetPos = ikp["rarm"].target_p0;
 //        tmp.targetRpy = hrp::rpyFromRot(ikp["rarm"].target_r0);
-//        tmp.weight_vec << 1,1,1,1,1,1;
+//        tmp.constraint_weight << 1,1,1,1,1,1;
 //        ik_tgt_list.push_back(tmp);
     }{
 //        IKConstraint tmp;
@@ -1121,7 +1120,7 @@ void AutoBalancer::solveFullbodyIK ()
 //        tmp.localR = ikp["larm"].localR;
 //        tmp.targetPos = ikp["larm"].target_p0;
 //        tmp.targetRpy = hrp::rpyFromRot(ikp["larm"].target_r0);
-//        tmp.weight_vec << 1,1,1,1,1,1;
+//        tmp.constraint_weight << 1,1,1,1,1,1;
 //        ik_tgt_list.push_back(tmp);
     }{
         IKConstraint tmp;
@@ -1132,21 +1131,18 @@ void AutoBalancer::solveFullbodyIK ()
         tmp.targetRpy = hrp::Vector3(0, 0, 0);//reference angular momentum
         //  tmp.targetRpy = hrp::Vector3(0, 10, 0);//reference angular momentum
         tmp.constraint_weight << 3,3,1,0.01,0.01,0.01;
-        tmp.rot_precision = 100;
         ik_tgt_list.push_back(tmp);
-    }
-    if( m_robot->link("RARM_JOINT2") != NULL) m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇の干渉回避のため
-    if( m_robot->link("LARM_JOINT2") != NULL) m_robot->link("LARM_JOINT2")->llimit = deg2rad(40);
-    const int IK_MAX_LOOP = 2;
-//    const int IK_MAX_LOOP = 1000;
+    }{
 
-    struct timespec startT, endT;
-    clock_gettime(CLOCK_REALTIME, &startT);
+  if( m_robot->link("RARM_JOINT2") != NULL) m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇の干渉回避のため
+  if( m_robot->link("LARM_JOINT2") != NULL) m_robot->link("LARM_JOINT2")->llimit = deg2rad(40);
 
-    int loop_result = fik->solveFullbodyIKLoop(m_robot, ik_tgt_list, IK_MAX_LOOP);
+//  fik_in->reference_gain.fill(0.001);
+//  fik_in->reference_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
 
-    clock_gettime(CLOCK_REALTIME, &endT);
-    if(loop%100==0){ std::cout << (endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-6) << " [ms] @ IK "<<loop_result<<" [loop]" << std::endl; }
+  int loop_result = 0;
+  const int IK_MAX_LOOP = 2;//2回は必須な気がする・・・
+  loop_result = fik->solveFullbodyIKLoop(m_robot, ik_tgt_list, IK_MAX_LOOP);
 }
 
 
@@ -1181,250 +1177,250 @@ void AutoBalancer::solveFullbodyIK ()
   {
   return RTC::RTC_OK;
   }
- */
+*/
 
 /*
   RTC::ReturnCode_t AutoBalancer::onError(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
- */
+*/
 
 /*
   RTC::ReturnCode_t AutoBalancer::onReset(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
- */
+*/
 
 /*
   RTC::ReturnCode_t AutoBalancer::onStateUpdate(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
- */
+*/
 
 /*
   RTC::ReturnCode_t AutoBalancer::onRateChanged(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
- */
+*/
 
 void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence& limbs)
 {
-    std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
-    Guard guard(m_mutex);
-    double tmp_ratio = 0.0;
-    transition_interpolator->clear();
-    transition_interpolator->set(&tmp_ratio);
-    tmp_ratio = 1.0;
-    transition_interpolator->setGoal(&tmp_ratio, transition_time, true);
-    prev_ref_zmp = ref_zmp;
-    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-        it->second.is_active = false;
-    }
+  std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
+  Guard guard(m_mutex);
+  double tmp_ratio = 0.0;
+  transition_interpolator->clear();
+  transition_interpolator->set(&tmp_ratio);
+  tmp_ratio = 1.0;
+  transition_interpolator->setGoal(&tmp_ratio, transition_time, true);
+  prev_ref_zmp = ref_zmp;
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+    it->second.is_active = false;
+  }
 
-    for (size_t i = 0; i < limbs.length(); i++) {
-        ABCIKparam& tmp = ikp[std::string(limbs[i])];
-        tmp.is_active = true;
-        std::cerr << "[" << m_profile.instance_name << "]   limb [" << std::string(limbs[i]) << "]" << std::endl;
-    }
+  for (size_t i = 0; i < limbs.length(); i++) {
+    ABCIKparam& tmp = ikp[std::string(limbs[i])];
+    tmp.is_active = true;
+    std::cerr << "[" << m_profile.instance_name << "]   limb [" << std::string(limbs[i]) << "]" << std::endl;
+  }
 
-    control_mode = MODE_SYNC_TO_ABC;
+  control_mode = MODE_SYNC_TO_ABC;
 }
 
 void AutoBalancer::stopABCparam()
 {
-    std::cerr << "[" << m_profile.instance_name << "] stop auto balancer mode" << std::endl;
-    //Guard guard(m_mutex);
-    double tmp_ratio = 1.0;
-    transition_interpolator->clear();
-    transition_interpolator->set(&tmp_ratio);
-    tmp_ratio = 0.0;
-    transition_interpolator->setGoal(&tmp_ratio, transition_time, true);
-    control_mode = MODE_SYNC_TO_IDLE;
+  std::cerr << "[" << m_profile.instance_name << "] stop auto balancer mode" << std::endl;
+  //Guard guard(m_mutex);
+  double tmp_ratio = 1.0;
+  transition_interpolator->clear();
+  transition_interpolator->set(&tmp_ratio);
+  tmp_ratio = 0.0;
+  transition_interpolator->setGoal(&tmp_ratio, transition_time, true);
+  control_mode = MODE_SYNC_TO_IDLE;
 }
 
 bool AutoBalancer::startWalking ()
 {
-    if ( control_mode != MODE_ABC ) {
-        std::cerr << "[" << m_profile.instance_name << "] Cannot start walking without MODE_ABC. Please startAutoBalancer." << std::endl;
-        return false;
-    }
-    {
-        Guard guard(m_mutex);
-        fik->resetIKFailParam();
-        std::vector<std::string> init_swing_leg_names(gg->get_footstep_front_leg_names());
-        std::vector<std::string> tmp_all_limbs(leg_names);
-        std::vector<std::string> init_support_leg_names;
-        std::sort(tmp_all_limbs.begin(), tmp_all_limbs.end());
-        std::sort(init_swing_leg_names.begin(), init_swing_leg_names.end());
-        std::set_difference(tmp_all_limbs.begin(), tmp_all_limbs.end(),
-                init_swing_leg_names.begin(), init_swing_leg_names.end(),
-                std::back_inserter(init_support_leg_names));
-        std::vector<step_node> init_support_leg_steps, init_swing_leg_dst_steps;
-        for (std::vector<std::string>::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++)
-            init_support_leg_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0));
-        for (std::vector<std::string>::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++)
-            init_swing_leg_dst_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0));
-        gg->set_default_zmp_offsets(default_zmp_offsets);
-        gg->initialize_gait_parameter(ref_cog, init_support_leg_steps, init_swing_leg_dst_steps);
-    }
-    is_hand_fix_initial = true;
-    while ( !gg->proc_one_tick() );
-    {
-        Guard guard(m_mutex);
-        gg_is_walking = gg_solved = true;
-    }
-    return true;
+  if ( control_mode != MODE_ABC ) {
+    std::cerr << "[" << m_profile.instance_name << "] Cannot start walking without MODE_ABC. Please startAutoBalancer." << std::endl;
+    return false;
+  }
+  {
+    Guard guard(m_mutex);
+    fik->resetIKFailParam();
+    std::vector<std::string> init_swing_leg_names(gg->get_footstep_front_leg_names());
+    std::vector<std::string> tmp_all_limbs(leg_names);
+    std::vector<std::string> init_support_leg_names;
+    std::sort(tmp_all_limbs.begin(), tmp_all_limbs.end());
+    std::sort(init_swing_leg_names.begin(), init_swing_leg_names.end());
+    std::set_difference(tmp_all_limbs.begin(), tmp_all_limbs.end(),
+                        init_swing_leg_names.begin(), init_swing_leg_names.end(),
+                        std::back_inserter(init_support_leg_names));
+    std::vector<step_node> init_support_leg_steps, init_swing_leg_dst_steps;
+    for (std::vector<std::string>::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++)
+        init_support_leg_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0));
+    for (std::vector<std::string>::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++)
+        init_swing_leg_dst_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0));
+    gg->set_default_zmp_offsets(default_zmp_offsets);
+    gg->initialize_gait_parameter(ref_cog, init_support_leg_steps, init_swing_leg_dst_steps);
+  }
+  is_hand_fix_initial = true;
+  while ( !gg->proc_one_tick() );
+  {
+    Guard guard(m_mutex);
+    gg_is_walking = gg_solved = true;
+  }
+  return true;
 }
 
 void AutoBalancer::stopWalking ()
 {
-    std::vector<coordinates> tmp_end_coords_list;
-    for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
-        if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(coordinates(ikp[*it].target_p0, ikp[*it].target_r0));
-    }
-    multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
-    fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot);
-    gg->clear_footstep_nodes_list();
-    gg_is_walking = false;
+  std::vector<coordinates> tmp_end_coords_list;
+  for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
+      if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(coordinates(ikp[*it].target_p0, ikp[*it].target_r0));
+  }
+  multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
+  fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot);
+  gg->clear_footstep_nodes_list();
+  gg_is_walking = false;
 }
 
 bool AutoBalancer::startAutoBalancer (const OpenHRP::AutoBalancerService::StrSequence& limbs)
 {
-    if (control_mode == MODE_IDLE) {
-        fik->resetIKFailParam();
-        startABCparam(limbs);
-        waitABCTransition();
-        return true;
-    } else {
-        return false;
-    }
+  if (control_mode == MODE_IDLE) {
+    fik->resetIKFailParam();
+    startABCparam(limbs);
+    waitABCTransition();
+    return true;
+  } else {
+    return false;
+  }
 }
 
 bool AutoBalancer::stopAutoBalancer ()
 {
-    if (control_mode == MODE_ABC) {
-        stopABCparam();
-        waitABCTransition();
-        return true;
-    } else {
-        return false;
-    }
+  if (control_mode == MODE_ABC) {
+    stopABCparam();
+    waitABCTransition();
+    return true;
+  } else {
+    return false;
+  }
 }
 
 void AutoBalancer::waitABCTransition()
 {
-    while (!transition_interpolator->isEmpty()) usleep(1000);
-    usleep(1000);
+  while (!transition_interpolator->isEmpty()) usleep(1000);
+  usleep(1000);
 }
 bool AutoBalancer::goPos(const double& x, const double& y, const double& th)
 {
     //  if ( !gg_is_walking && !is_stop_mode) {
-    if ( !is_stop_mode) {
-        gg->set_all_limbs(leg_names);
-        coordinates start_ref_coords;
-        std::vector<coordinates> initial_support_legs_coords;
-        std::vector<leg_type> initial_support_legs;
-        bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
-        if (is_valid_gait_type == false) return false;
-        bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th,
-                initial_support_legs_coords, // Dummy if gg_is_walking
-                start_ref_coords,            // Dummy if gg_is_walking
-                initial_support_legs,        // Dummy if gg_is_walking
-                (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps.
-        if (!ret) {
-            std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl;
-        }
-        if ( !gg_is_walking ) { // Initializing
-            ret = startWalking();
-        }
-        return ret;
-    } else {
-        std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl;
-        return false;
+  if ( !is_stop_mode) {
+    gg->set_all_limbs(leg_names);
+    coordinates start_ref_coords;
+    std::vector<coordinates> initial_support_legs_coords;
+    std::vector<leg_type> initial_support_legs;
+    bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
+    if (is_valid_gait_type == false) return false;
+    bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th,
+                                                      initial_support_legs_coords, // Dummy if gg_is_walking
+                                                      start_ref_coords,            // Dummy if gg_is_walking
+                                                      initial_support_legs,        // Dummy if gg_is_walking
+                                                      (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps.
+    if (!ret) {
+        std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl;
+    }
+    if ( !gg_is_walking ) { // Initializing
+        ret = startWalking();
     }
+    return ret;
+  } else {
+    std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl;
+    return false;
+  }
 }
 
 bool AutoBalancer::goVelocity(const double& vx, const double& vy, const double& vth)
 {
-    gg->set_all_limbs(leg_names);
-    bool ret = true;
-    if (gg_is_walking && gg_solved) {
-        gg->set_velocity_param(vx, vy, vth);
-    } else {
-        coordinates ref_coords;
-        ref_coords.pos = (ikp["rleg"].target_p0+ikp["lleg"].target_p0)*0.5;
-        mid_rot(ref_coords.rot, 0.5, ikp["rleg"].target_r0, ikp["lleg"].target_r0);
-        std::vector<leg_type> current_legs;
-        switch(gait_type) {
-            case BIPED:
-                current_legs = (vy > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
-                break;
-            case TROT:
-                current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
-                break;
-            case PACE:
-                current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
-                break;
-            case CRAWL:
-                std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
-                return false;
-            case GALLOP:
-                /* at least one leg shoud be in contact */
-                std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
-                return false;
-            default: break;
-        }
-        gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs);
-        ret = startWalking();
+  gg->set_all_limbs(leg_names);
+  bool ret = true;
+  if (gg_is_walking && gg_solved) {
+    gg->set_velocity_param(vx, vy, vth);
+  } else {
+    coordinates ref_coords;
+    ref_coords.pos = (ikp["rleg"].target_p0+ikp["lleg"].target_p0)*0.5;
+    mid_rot(ref_coords.rot, 0.5, ikp["rleg"].target_r0, ikp["lleg"].target_r0);
+    std::vector<leg_type> current_legs;
+    switch(gait_type) {
+    case BIPED:
+        current_legs = (vy > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
+        break;
+    case TROT:
+        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
+        break;
+    case PACE:
+        current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
+        break;
+    case CRAWL:
+        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    case GALLOP:
+        /* at least one leg shoud be in contact */
+        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    default: break;
     }
-    return ret;
+    gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs);
+    ret = startWalking();
+  }
+  return ret;
 }
 
 bool AutoBalancer::goStop ()
 {
-    gg->finalize_velocity_mode();
-    waitFootSteps();
-    return true;
+  gg->finalize_velocity_mode();
+  waitFootSteps();
+  return true;
 }
 
 bool AutoBalancer::emergencyStop ()
 {
-    std::cerr << "[" << m_profile.instance_name << "] emergencyStop" << std::endl;
-    // is_stop_mode = true;
-    gg->emergency_stop();
-    waitFootSteps();
-    return true;
+  std::cerr << "[" << m_profile.instance_name << "] emergencyStop" << std::endl;
+  // is_stop_mode = true;
+  gg->emergency_stop();
+  waitFootSteps();
+  return true;
 }
 
 bool AutoBalancer::releaseEmergencyStop ()
 {
-    if (is_stop_mode) {
-        std::cerr << "[" << m_profile.instance_name << "] releaseEmergencyStop" << std::endl;
-        is_stop_mode = false;
-    }
-    return true;
+  if (is_stop_mode) {
+      std::cerr << "[" << m_profile.instance_name << "] releaseEmergencyStop" << std::endl;
+      is_stop_mode = false;
+  }
+  return true;
 }
 
 bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
 {
-    OpenHRP::AutoBalancerService::StepParamsSequence spss;
-    spss.length(fss.length());
-    // If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
-    // If gg_is_walking is true, do not set to 0.
-    for (size_t i = 0; i < spss.length(); i++) {
-        spss[i].sps.length(fss[i].fs.length());
-        for (size_t j = 0; j < spss[i].sps.length(); j++) {
-            spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
-            spss[i].sps[j].step_time = gg->get_default_step_time();
-            spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle());
-            spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle());
-        }
-    }
-    setFootStepsWithParam(fss, spss, overwrite_fs_idx);
+  OpenHRP::AutoBalancerService::StepParamsSequence spss;
+  spss.length(fss.length());
+  // If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
+  // If gg_is_walking is true, do not set to 0.
+  for (size_t i = 0; i < spss.length(); i++) {
+      spss[i].sps.length(fss[i].fs.length());
+      for (size_t j = 0; j < spss[i].sps.length(); j++) {
+          spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
+          spss[i].sps[j].step_time = gg->get_default_step_time();
+          spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle());
+          spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle());
+      }
+  }
+  setFootStepsWithParam(fss, spss, overwrite_fs_idx);
 }
 
 bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
@@ -1525,524 +1521,524 @@ bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::Foo
 
 void AutoBalancer::waitFootSteps()
 {
-    //while (gg_is_walking) usleep(10);
-    while (gg_is_walking || !transition_interpolator->isEmpty() )
-        usleep(1000);
+  //while (gg_is_walking) usleep(10);
+  while (gg_is_walking || !transition_interpolator->isEmpty() )
     usleep(1000);
-    gg->set_offset_velocity_param(0,0,0);
+  usleep(1000);
+  gg->set_offset_velocity_param(0,0,0);
 }
 
 void AutoBalancer::waitFootStepsEarly(const double tm)
 {
-    if (!gg_is_walking) { return;}
-    while ( !gg->is_finalizing(tm)|| !transition_interpolator->isEmpty() )
-        usleep(1000);
+  if (!gg_is_walking) { return;}
+  while ( !gg->is_finalizing(tm)|| !transition_interpolator->isEmpty() )
     usleep(1000);
-    gg->set_offset_velocity_param(0,0,0);
+  usleep(1000);
+  gg->set_offset_velocity_param(0,0,0);
 }
 
 bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
 {
-    std::cerr << "[" << m_profile.instance_name << "] setGaitGeneratorParam" << std::endl;
-    if (i_param.stride_parameter.length() == 4) { // Support old stride_parameter definitions
-        gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3],
-                i_param.stride_parameter[1]*0.5, i_param.stride_parameter[2]*0.5);
-    } else {
-        gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3],
-                i_param.stride_parameter[4], i_param.stride_parameter[5]);
-    }
-    std::vector<hrp::Vector3> off;
-    for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
-        off.push_back(hrp::Vector3(i_param.leg_default_translate_pos[i][0], i_param.leg_default_translate_pos[i][1], i_param.leg_default_translate_pos[i][2]));
-    }
-    gg->set_leg_default_translate_pos(off);
-    gg->set_default_step_time(i_param.default_step_time);
-    gg->set_default_step_height(i_param.default_step_height);
-    gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio/2.0);
-    gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio/2.0);
-    gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio/2.0);
-    gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio/2.0);
-    gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio/2.0);
-    gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio/2.0);
-    // gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio_before);
-    // gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio_after);
-    // gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio_before);
-    // gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio_after);
-    // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_before);
-    // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_after);
-    // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_swing_before);
-    // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_swing_after);
-    if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::SHUFFLING) {
-        gg->set_default_orbit_type(SHUFFLING);
-    } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOID) {
-        gg->set_default_orbit_type(CYCLOID);
-    } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::RECTANGLE) {
-        gg->set_default_orbit_type(RECTANGLE);
-    } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::STAIR) {
-        gg->set_default_orbit_type(STAIR);
-    } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAY) {
-        gg->set_default_orbit_type(CYCLOIDDELAY);
-    } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK) {
-        gg->set_default_orbit_type(CYCLOIDDELAYKICK);
-    } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CROSS) {
-        gg->set_default_orbit_type(CROSS);
-    }
-    gg->set_swing_trajectory_delay_time_offset(i_param.swing_trajectory_delay_time_offset);
-    gg->set_swing_trajectory_final_distance_weight(i_param.swing_trajectory_final_distance_weight);
-    gg->set_swing_trajectory_time_offset_xy2z(i_param.swing_trajectory_time_offset_xy2z);
-    gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2]));
-    gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2]));
-    gg->set_gravitational_acceleration(i_param.gravitational_acceleration);
-    gg->set_toe_angle(i_param.toe_angle);
-    gg->set_heel_angle(i_param.heel_angle);
-    gg->set_toe_pos_offset_x(i_param.toe_pos_offset_x);
-    gg->set_heel_pos_offset_x(i_param.heel_pos_offset_x);
-    gg->set_toe_zmp_offset_x(i_param.toe_zmp_offset_x);
-    gg->set_heel_zmp_offset_x(i_param.heel_zmp_offset_x);
-    gg->set_toe_check_thre(i_param.toe_check_thre);
-    gg->set_heel_check_thre(i_param.heel_check_thre);
-    std::vector<double> tmp_ratio(i_param.toe_heel_phase_ratio.get_buffer(), i_param.toe_heel_phase_ratio.get_buffer()+i_param.toe_heel_phase_ratio.length());
-    std::cerr << "[" << m_profile.instance_name << "]   "; // for set_toe_heel_phase_ratio
-    gg->set_toe_heel_phase_ratio(tmp_ratio);
-    gg->set_use_toe_joint(i_param.use_toe_joint);
-    gg->set_use_toe_heel_transition(i_param.use_toe_heel_transition);
-    gg->set_use_toe_heel_auto_set(i_param.use_toe_heel_auto_set);
-    gg->set_zmp_weight_map(boost::assign::map_list_of<leg_type, double>(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3]));
-    gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num);
-    gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset);
-    gg->set_leg_margin(i_param.leg_margin);
-    gg->set_stride_limitation_for_circle_type(i_param.stride_limitation_for_circle_type);
-    gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation);
-    gg->set_use_stride_limitation(i_param.use_stride_limitation);
-    gg->set_footstep_modification_gain(i_param.footstep_modification_gain);
-    gg->set_modify_footsteps(i_param.modify_footsteps);
-    gg->set_cp_check_margin(i_param.cp_check_margin);
-    gg->set_margin_time_ratio(i_param.margin_time_ratio);
-    if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::SQUARE) {
-        gg->set_stride_limitation_type(SQUARE);
-    } else if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::CIRCLE) {
-        gg->set_stride_limitation_type(CIRCLE);
-    }
-
-    // print
-    gg->print_param(std::string(m_profile.instance_name));
-    return true;
+  std::cerr << "[" << m_profile.instance_name << "] setGaitGeneratorParam" << std::endl;
+  if (i_param.stride_parameter.length() == 4) { // Support old stride_parameter definitions
+      gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3],
+                                i_param.stride_parameter[1]*0.5, i_param.stride_parameter[2]*0.5);
+  } else {
+      gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3],
+                                i_param.stride_parameter[4], i_param.stride_parameter[5]);
+  }
+  std::vector<hrp::Vector3> off;
+  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
+      off.push_back(hrp::Vector3(i_param.leg_default_translate_pos[i][0], i_param.leg_default_translate_pos[i][1], i_param.leg_default_translate_pos[i][2]));
+  }
+  gg->set_leg_default_translate_pos(off);
+  gg->set_default_step_time(i_param.default_step_time);
+  gg->set_default_step_height(i_param.default_step_height);
+  gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio/2.0);
+  gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio/2.0);
+  gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio/2.0);
+  gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio/2.0);
+  gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio/2.0);
+  gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio/2.0);
+  // gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio_before);
+  // gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio_after);
+  // gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio_before);
+  // gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio_after);
+  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_before);
+  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_after);
+  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_swing_before);
+  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_swing_after);
+  if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::SHUFFLING) {
+    gg->set_default_orbit_type(SHUFFLING);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOID) {
+    gg->set_default_orbit_type(CYCLOID);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::RECTANGLE) {
+    gg->set_default_orbit_type(RECTANGLE);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::STAIR) {
+    gg->set_default_orbit_type(STAIR);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAY) {
+    gg->set_default_orbit_type(CYCLOIDDELAY);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK) {
+    gg->set_default_orbit_type(CYCLOIDDELAYKICK);
+  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CROSS) {
+    gg->set_default_orbit_type(CROSS);
+  }
+  gg->set_swing_trajectory_delay_time_offset(i_param.swing_trajectory_delay_time_offset);
+  gg->set_swing_trajectory_final_distance_weight(i_param.swing_trajectory_final_distance_weight);
+  gg->set_swing_trajectory_time_offset_xy2z(i_param.swing_trajectory_time_offset_xy2z);
+  gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2]));
+  gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2]));
+  gg->set_gravitational_acceleration(i_param.gravitational_acceleration);
+  gg->set_toe_angle(i_param.toe_angle);
+  gg->set_heel_angle(i_param.heel_angle);
+  gg->set_toe_pos_offset_x(i_param.toe_pos_offset_x);
+  gg->set_heel_pos_offset_x(i_param.heel_pos_offset_x);
+  gg->set_toe_zmp_offset_x(i_param.toe_zmp_offset_x);
+  gg->set_heel_zmp_offset_x(i_param.heel_zmp_offset_x);
+  gg->set_toe_check_thre(i_param.toe_check_thre);
+  gg->set_heel_check_thre(i_param.heel_check_thre);
+  std::vector<double> tmp_ratio(i_param.toe_heel_phase_ratio.get_buffer(), i_param.toe_heel_phase_ratio.get_buffer()+i_param.toe_heel_phase_ratio.length());
+  std::cerr << "[" << m_profile.instance_name << "]   "; // for set_toe_heel_phase_ratio
+  gg->set_toe_heel_phase_ratio(tmp_ratio);
+  gg->set_use_toe_joint(i_param.use_toe_joint);
+  gg->set_use_toe_heel_transition(i_param.use_toe_heel_transition);
+  gg->set_use_toe_heel_auto_set(i_param.use_toe_heel_auto_set);
+  gg->set_zmp_weight_map(boost::assign::map_list_of<leg_type, double>(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3]));
+  gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num);
+  gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset);
+  gg->set_leg_margin(i_param.leg_margin);
+  gg->set_stride_limitation_for_circle_type(i_param.stride_limitation_for_circle_type);
+  gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation);
+  gg->set_use_stride_limitation(i_param.use_stride_limitation);
+  gg->set_footstep_modification_gain(i_param.footstep_modification_gain);
+  gg->set_modify_footsteps(i_param.modify_footsteps);
+  gg->set_cp_check_margin(i_param.cp_check_margin);
+  gg->set_margin_time_ratio(i_param.margin_time_ratio);
+  if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::SQUARE) {
+    gg->set_stride_limitation_type(SQUARE);
+  } else if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::CIRCLE) {
+    gg->set_stride_limitation_type(CIRCLE);
+  }
+
+  // print
+  gg->print_param(std::string(m_profile.instance_name));
+  return true;
 };
 
 bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
 {
-    gg->get_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3], i_param.stride_parameter[4], i_param.stride_parameter[5]);
-    std::vector<hrp::Vector3> off;
-    gg->get_leg_default_translate_pos(off);
-    i_param.leg_default_translate_pos.length(off.size());
-    for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
-        i_param.leg_default_translate_pos[i].length(3);
-        i_param.leg_default_translate_pos[i][0] = off[i](0);
-        i_param.leg_default_translate_pos[i][1] = off[i](1);
-        i_param.leg_default_translate_pos[i][2] = off[i](2);
-    }
-    i_param.default_step_time = gg->get_default_step_time();
-    i_param.default_step_height = gg->get_default_step_height();
-    i_param.default_double_support_ratio_before = gg->get_default_double_support_ratio_before();
-    i_param.default_double_support_ratio_after = gg->get_default_double_support_ratio_after();
-    i_param.default_double_support_static_ratio_before = gg->get_default_double_support_static_ratio_before();
-    i_param.default_double_support_static_ratio_after = gg->get_default_double_support_static_ratio_after();
-    i_param.default_double_support_ratio_swing_before = gg->get_default_double_support_ratio_swing_before();
-    i_param.default_double_support_ratio_swing_after = gg->get_default_double_support_ratio_swing_after();
-    i_param.default_double_support_ratio = i_param.default_double_support_ratio_before + i_param.default_double_support_ratio_after;
-    i_param.default_double_support_static_ratio = i_param.default_double_support_static_ratio_before + i_param.default_double_support_static_ratio_after;
-    if (gg->get_default_orbit_type() == SHUFFLING) {
-        i_param.default_orbit_type = OpenHRP::AutoBalancerService::SHUFFLING;
-    } else if (gg->get_default_orbit_type() == CYCLOID) {
-        i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOID;
-    } else if (gg->get_default_orbit_type() == RECTANGLE) {
-        i_param.default_orbit_type = OpenHRP::AutoBalancerService::RECTANGLE;
-    } else if (gg->get_default_orbit_type() == STAIR) {
-        i_param.default_orbit_type = OpenHRP::AutoBalancerService::STAIR;
-    } else if (gg->get_default_orbit_type() == CYCLOIDDELAY) {
-        i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAY;
-    } else if (gg->get_default_orbit_type() == CYCLOIDDELAYKICK) {
-        i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK;
-    } else if (gg->get_default_orbit_type() == CROSS) {
-        i_param.default_orbit_type = OpenHRP::AutoBalancerService::CROSS;
-    }
-
-    hrp::Vector3 tmpv = gg->get_stair_trajectory_way_point_offset();
-    for (size_t i = 0; i < 3; i++) i_param.stair_trajectory_way_point_offset[i] = tmpv(i);
-    tmpv = gg->get_cycloid_delay_kick_point_offset();
-    for (size_t i = 0; i < 3; i++) i_param.cycloid_delay_kick_point_offset[i] = tmpv(i);
-    i_param.swing_trajectory_delay_time_offset = gg->get_swing_trajectory_delay_time_offset();
-    i_param.swing_trajectory_final_distance_weight = gg->get_swing_trajectory_final_distance_weight();
-    i_param.swing_trajectory_time_offset_xy2z = gg->get_swing_trajectory_time_offset_xy2z();
-    i_param.gravitational_acceleration = gg->get_gravitational_acceleration();
-    i_param.toe_angle = gg->get_toe_angle();
-    i_param.heel_angle = gg->get_heel_angle();
-    i_param.toe_pos_offset_x = gg->get_toe_pos_offset_x();
-    i_param.heel_pos_offset_x = gg->get_heel_pos_offset_x();
-    i_param.toe_zmp_offset_x = gg->get_toe_zmp_offset_x();
-    i_param.heel_zmp_offset_x = gg->get_heel_zmp_offset_x();
-    i_param.toe_check_thre = gg->get_toe_check_thre();
-    i_param.heel_check_thre = gg->get_heel_check_thre();
-    std::vector<double> ratio(gg->get_NUM_TH_PHASES(),0.0);
-    gg->get_toe_heel_phase_ratio(ratio);
-    for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) i_param.toe_heel_phase_ratio[i] = ratio[i];
-    i_param.use_toe_joint = gg->get_use_toe_joint();
-    i_param.use_toe_heel_transition = gg->get_use_toe_heel_transition();
-    i_param.use_toe_heel_auto_set = gg->get_use_toe_heel_auto_set();
-    std::map<leg_type, double> tmp_zmp_weight_map = gg->get_zmp_weight_map();
-    i_param.zmp_weight_map[0] = tmp_zmp_weight_map[RLEG];
-    i_param.zmp_weight_map[1] = tmp_zmp_weight_map[LLEG];
-    i_param.zmp_weight_map[2] = tmp_zmp_weight_map[RARM];
-    i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM];
-    i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num();
-    i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset();
-    for (size_t i=0; i<4; i++) {
-        i_param.leg_margin[i] = gg->get_leg_margin(i);
-    }
-    for (size_t i=0; i<5; i++) {
-        i_param.stride_limitation_for_circle_type[i] = gg->get_stride_limitation_for_circle_type(i);
-    }
-    for (size_t i=0; i<5; i++) {
-        i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i);
-    }
-    i_param.use_stride_limitation = gg->get_use_stride_limitation();
-    i_param.footstep_modification_gain = gg->get_footstep_modification_gain();
-    i_param.modify_footsteps = gg->get_modify_footsteps();
-    for (size_t i=0; i<2; i++) {
-        i_param.cp_check_margin[i] = gg->get_cp_check_margin(i);
-    }
-    i_param.margin_time_ratio = gg->get_margin_time_ratio();
-    if (gg->get_stride_limitation_type() == SQUARE) {
-        i_param.stride_limitation_type = OpenHRP::AutoBalancerService::SQUARE;
-    } else if (gg->get_stride_limitation_type() == CIRCLE) {
-        i_param.stride_limitation_type = OpenHRP::AutoBalancerService::CIRCLE;
-    }
-    return true;
+  gg->get_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3], i_param.stride_parameter[4], i_param.stride_parameter[5]);
+  std::vector<hrp::Vector3> off;
+  gg->get_leg_default_translate_pos(off);
+  i_param.leg_default_translate_pos.length(off.size());
+  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
+      i_param.leg_default_translate_pos[i].length(3);
+      i_param.leg_default_translate_pos[i][0] = off[i](0);
+      i_param.leg_default_translate_pos[i][1] = off[i](1);
+      i_param.leg_default_translate_pos[i][2] = off[i](2);
+  }
+  i_param.default_step_time = gg->get_default_step_time();
+  i_param.default_step_height = gg->get_default_step_height();
+  i_param.default_double_support_ratio_before = gg->get_default_double_support_ratio_before();
+  i_param.default_double_support_ratio_after = gg->get_default_double_support_ratio_after();
+  i_param.default_double_support_static_ratio_before = gg->get_default_double_support_static_ratio_before();
+  i_param.default_double_support_static_ratio_after = gg->get_default_double_support_static_ratio_after();
+  i_param.default_double_support_ratio_swing_before = gg->get_default_double_support_ratio_swing_before();
+  i_param.default_double_support_ratio_swing_after = gg->get_default_double_support_ratio_swing_after();
+  i_param.default_double_support_ratio = i_param.default_double_support_ratio_before + i_param.default_double_support_ratio_after;
+  i_param.default_double_support_static_ratio = i_param.default_double_support_static_ratio_before + i_param.default_double_support_static_ratio_after;
+  if (gg->get_default_orbit_type() == SHUFFLING) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::SHUFFLING;
+  } else if (gg->get_default_orbit_type() == CYCLOID) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOID;
+  } else if (gg->get_default_orbit_type() == RECTANGLE) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::RECTANGLE;
+  } else if (gg->get_default_orbit_type() == STAIR) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::STAIR;
+  } else if (gg->get_default_orbit_type() == CYCLOIDDELAY) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAY;
+  } else if (gg->get_default_orbit_type() == CYCLOIDDELAYKICK) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK;
+  } else if (gg->get_default_orbit_type() == CROSS) {
+    i_param.default_orbit_type = OpenHRP::AutoBalancerService::CROSS;
+  }
+
+  hrp::Vector3 tmpv = gg->get_stair_trajectory_way_point_offset();
+  for (size_t i = 0; i < 3; i++) i_param.stair_trajectory_way_point_offset[i] = tmpv(i);
+  tmpv = gg->get_cycloid_delay_kick_point_offset();
+  for (size_t i = 0; i < 3; i++) i_param.cycloid_delay_kick_point_offset[i] = tmpv(i);
+  i_param.swing_trajectory_delay_time_offset = gg->get_swing_trajectory_delay_time_offset();
+  i_param.swing_trajectory_final_distance_weight = gg->get_swing_trajectory_final_distance_weight();
+  i_param.swing_trajectory_time_offset_xy2z = gg->get_swing_trajectory_time_offset_xy2z();
+  i_param.gravitational_acceleration = gg->get_gravitational_acceleration();
+  i_param.toe_angle = gg->get_toe_angle();
+  i_param.heel_angle = gg->get_heel_angle();
+  i_param.toe_pos_offset_x = gg->get_toe_pos_offset_x();
+  i_param.heel_pos_offset_x = gg->get_heel_pos_offset_x();
+  i_param.toe_zmp_offset_x = gg->get_toe_zmp_offset_x();
+  i_param.heel_zmp_offset_x = gg->get_heel_zmp_offset_x();
+  i_param.toe_check_thre = gg->get_toe_check_thre();
+  i_param.heel_check_thre = gg->get_heel_check_thre();
+  std::vector<double> ratio(gg->get_NUM_TH_PHASES(),0.0);
+  gg->get_toe_heel_phase_ratio(ratio);
+  for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) i_param.toe_heel_phase_ratio[i] = ratio[i];
+  i_param.use_toe_joint = gg->get_use_toe_joint();
+  i_param.use_toe_heel_transition = gg->get_use_toe_heel_transition();
+  i_param.use_toe_heel_auto_set = gg->get_use_toe_heel_auto_set();
+  std::map<leg_type, double> tmp_zmp_weight_map = gg->get_zmp_weight_map();
+  i_param.zmp_weight_map[0] = tmp_zmp_weight_map[RLEG];
+  i_param.zmp_weight_map[1] = tmp_zmp_weight_map[LLEG];
+  i_param.zmp_weight_map[2] = tmp_zmp_weight_map[RARM];
+  i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM];
+  i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num();
+  i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset();
+  for (size_t i=0; i<4; i++) {
+    i_param.leg_margin[i] = gg->get_leg_margin(i);
+  }
+  for (size_t i=0; i<5; i++) {
+    i_param.stride_limitation_for_circle_type[i] = gg->get_stride_limitation_for_circle_type(i);
+  }
+  for (size_t i=0; i<5; i++) {
+    i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i);
+  }
+  i_param.use_stride_limitation = gg->get_use_stride_limitation();
+  i_param.footstep_modification_gain = gg->get_footstep_modification_gain();
+  i_param.modify_footsteps = gg->get_modify_footsteps();
+  for (size_t i=0; i<2; i++) {
+    i_param.cp_check_margin[i] = gg->get_cp_check_margin(i);
+  }
+  i_param.margin_time_ratio = gg->get_margin_time_ratio();
+  if (gg->get_stride_limitation_type() == SQUARE) {
+    i_param.stride_limitation_type = OpenHRP::AutoBalancerService::SQUARE;
+  } else if (gg->get_stride_limitation_type() == CIRCLE) {
+    i_param.stride_limitation_type = OpenHRP::AutoBalancerService::CIRCLE;
+  }
+  return true;
 };
 
 bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
 {
-    Guard guard(m_mutex);
-    std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl;
-    double *default_zmp_offsets_array = new double[ikp.size()*3];
-    for (size_t i = 0; i < ikp.size(); i++)
-        for (size_t j = 0; j < 3; j++)
-            default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j];
-    zmp_transition_time = i_param.zmp_transition_time;
-    adjust_footstep_transition_time = i_param.adjust_footstep_transition_time;
-    if (zmp_offset_interpolator->isEmpty()) {
-        zmp_offset_interpolator->clear();
-        zmp_offset_interpolator->setGoal(default_zmp_offsets_array, zmp_transition_time, true);
-    } else {
-        std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets cannot be set because interpolating." << std::endl;
-    }
-    if (control_mode == MODE_IDLE) {
-        switch (i_param.use_force_mode) {
-            case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
-                use_force = MODE_NO_FORCE;
-                break;
-            case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
-                use_force = MODE_REF_FORCE;
-                break;
-            case OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT:
-                use_force = MODE_REF_FORCE_WITH_FOOT;
-                break;
-            case OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT:
-                use_force = MODE_REF_FORCE_RFU_EXT_MOMENT;
-                break;
-            default:
-                break;
-        }
-    } else {
-        std::cerr << "[" << m_profile.instance_name << "]   use_force_mode cannot be changed to [" << i_param.use_force_mode << "] during MODE_ABC, MODE_SYNC_TO_IDLE or MODE_SYNC_TO_ABC." << std::endl;
-    }
-    graspless_manip_mode = i_param.graspless_manip_mode;
-    graspless_manip_arm = std::string(i_param.graspless_manip_arm);
+  Guard guard(m_mutex);
+  std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl;
+  double *default_zmp_offsets_array = new double[ikp.size()*3];
+  for (size_t i = 0; i < ikp.size(); i++)
     for (size_t j = 0; j < 3; j++)
-        graspless_manip_p_gain[j] = i_param.graspless_manip_p_gain[j];
-    for (size_t j = 0; j < 3; j++)
-        graspless_manip_reference_trans_coords.pos[j] = i_param.graspless_manip_reference_trans_pos[j];
-    graspless_manip_reference_trans_coords.rot = (Eigen::Quaternion<double>(i_param.graspless_manip_reference_trans_rot[0],
-            i_param.graspless_manip_reference_trans_rot[1],
-            i_param.graspless_manip_reference_trans_rot[2],
-            i_param.graspless_manip_reference_trans_rot[3]).normalized().toRotationMatrix()); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
-            transition_time = i_param.transition_time;
-            std::vector<std::string> cur_leg_names, dst_leg_names;
-            cur_leg_names = leg_names;
-            for (size_t i = 0; i < i_param.leg_names.length(); i++) {
-                dst_leg_names.push_back(std::string(i_param.leg_names[i]));
-            }
-            std::sort(cur_leg_names.begin(), cur_leg_names.end());
-            std::sort(dst_leg_names.begin(), dst_leg_names.end());
-            if (cur_leg_names != dst_leg_names) {
-                if (leg_names_interpolator->isEmpty()) {
-                    leg_names.clear();
-                    leg_names = dst_leg_names;
-                    if (control_mode == MODE_ABC) {
-                        double tmp_ratio = 0.0;
-                        leg_names_interpolator->set(&tmp_ratio);
-                        tmp_ratio = 1.0;
-                        leg_names_interpolator->setGoal(&tmp_ratio, 5.0, true);
-                        control_mode = MODE_SYNC_TO_ABC;
-                    }
-                }
-            } else {
-                std::cerr << "[" << m_profile.instance_name << "]   leg_names cannot be set because interpolating." << std::endl;
-            }
-            if (!gg_is_walking) {
-                is_hand_fix_mode = i_param.is_hand_fix_mode;
-                std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode = " << is_hand_fix_mode << std::endl;
-            } else {
-                std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode cannot be set in (gg_is_walking = true). Current is_hand_fix_mode is " << (is_hand_fix_mode?"true":"false") << std::endl;
-            }
-            if (control_mode == MODE_IDLE) {
-                for (size_t i = 0; i < i_param.end_effector_list.length(); i++) {
-                    std::map<std::string, ABCIKparam>::iterator it = ikp.find(std::string(i_param.end_effector_list[i].leg));
-                    memcpy(it->second.localPos.data(), i_param.end_effector_list[i].pos, sizeof(double)*3);
-                    it->second.localR = (Eigen::Quaternion<double>(i_param.end_effector_list[i].rot[0], i_param.end_effector_list[i].rot[1], i_param.end_effector_list[i].rot[2], i_param.end_effector_list[i].rot[3])).normalized().toRotationMatrix();
-                }
-            } else {
-                std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl;
-            }
-            if (i_param.default_gait_type == OpenHRP::AutoBalancerService::BIPED) {
-                gait_type = BIPED;
-            } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::TROT) {
-                gait_type = TROT;
-            } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::PACE) {
-                gait_type = PACE;
-            } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::CRAWL) {
-                gait_type = CRAWL;
-            } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) {
-                gait_type = GALLOP;
-            }
-            // Ref force balancing
-            std::cerr << "[" << m_profile.instance_name << "] Ref force balancing" << std::endl;
-            if ( use_force == MODE_REF_FORCE_WITH_FOOT && control_mode != MODE_IDLE ) {
-                std::cerr << "[" << m_profile.instance_name << "]   additional_force_applied_point_offset and additional_force_applied_link_name cannot be updated during MODE_REF_FORCE_WITH_FOOT and non-MODE_IDLE"<< std::endl;
-            } else if ( !m_robot->link(std::string(i_param.additional_force_applied_link_name)) ) {
-                std::cerr << "[" << m_profile.instance_name << "]   Invalid link name for additional_force_applied_link_name = " << i_param.additional_force_applied_link_name << std::endl;
-            } else {
-                additional_force_applied_link = m_robot->link(std::string(i_param.additional_force_applied_link_name));
-                for (size_t i = 0; i < 3; i++) {
-                    additional_force_applied_point_offset(i) = i_param.additional_force_applied_point_offset[i];
-                }
-                std::cerr << "[" << m_profile.instance_name << "]   Link name for additional_force_applied_link_name = " << additional_force_applied_link->name << ", additional_force_applied_point_offset = " << additional_force_applied_point_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-            }
+      default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j];
+  zmp_transition_time = i_param.zmp_transition_time;
+  adjust_footstep_transition_time = i_param.adjust_footstep_transition_time;
+  if (zmp_offset_interpolator->isEmpty()) {
+      zmp_offset_interpolator->clear();
+      zmp_offset_interpolator->setGoal(default_zmp_offsets_array, zmp_transition_time, true);
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets cannot be set because interpolating." << std::endl;
+  }
+  if (control_mode == MODE_IDLE) {
+    switch (i_param.use_force_mode) {
+    case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
+        use_force = MODE_NO_FORCE;
+        break;
+    case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
+        use_force = MODE_REF_FORCE;
+        break;
+    case OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT:
+        use_force = MODE_REF_FORCE_WITH_FOOT;
+        break;
+    case OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT:
+        use_force = MODE_REF_FORCE_RFU_EXT_MOMENT;
+        break;
+    default:
+        break;
+    }
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   use_force_mode cannot be changed to [" << i_param.use_force_mode << "] during MODE_ABC, MODE_SYNC_TO_IDLE or MODE_SYNC_TO_ABC." << std::endl;
+  }
+  graspless_manip_mode = i_param.graspless_manip_mode;
+  graspless_manip_arm = std::string(i_param.graspless_manip_arm);
+  for (size_t j = 0; j < 3; j++)
+      graspless_manip_p_gain[j] = i_param.graspless_manip_p_gain[j];
+  for (size_t j = 0; j < 3; j++)
+      graspless_manip_reference_trans_coords.pos[j] = i_param.graspless_manip_reference_trans_pos[j];
+  graspless_manip_reference_trans_coords.rot = (Eigen::Quaternion<double>(i_param.graspless_manip_reference_trans_rot[0],
+                                                                          i_param.graspless_manip_reference_trans_rot[1],
+                                                                          i_param.graspless_manip_reference_trans_rot[2],
+                                                                          i_param.graspless_manip_reference_trans_rot[3]).normalized().toRotationMatrix()); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+  transition_time = i_param.transition_time;
+  std::vector<std::string> cur_leg_names, dst_leg_names;
+  cur_leg_names = leg_names;
+  for (size_t i = 0; i < i_param.leg_names.length(); i++) {
+      dst_leg_names.push_back(std::string(i_param.leg_names[i]));
+  }
+  std::sort(cur_leg_names.begin(), cur_leg_names.end());
+  std::sort(dst_leg_names.begin(), dst_leg_names.end());
+  if (cur_leg_names != dst_leg_names) {
+      if (leg_names_interpolator->isEmpty()) {
+          leg_names.clear();
+          leg_names = dst_leg_names;
+          if (control_mode == MODE_ABC) {
+              double tmp_ratio = 0.0;
+              leg_names_interpolator->set(&tmp_ratio);
+              tmp_ratio = 1.0;
+              leg_names_interpolator->setGoal(&tmp_ratio, 5.0, true);
+              control_mode = MODE_SYNC_TO_ABC;
+          }
+      }
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   leg_names cannot be set because interpolating." << std::endl;
+  }
+  if (!gg_is_walking) {
+      is_hand_fix_mode = i_param.is_hand_fix_mode;
+      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode = " << is_hand_fix_mode << std::endl;
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   is_hand_fix_mode cannot be set in (gg_is_walking = true). Current is_hand_fix_mode is " << (is_hand_fix_mode?"true":"false") << std::endl;
+  }
+  if (control_mode == MODE_IDLE) {
+      for (size_t i = 0; i < i_param.end_effector_list.length(); i++) {
+          std::map<std::string, ABCIKparam>::iterator it = ikp.find(std::string(i_param.end_effector_list[i].leg));
+          memcpy(it->second.localPos.data(), i_param.end_effector_list[i].pos, sizeof(double)*3);
+          it->second.localR = (Eigen::Quaternion<double>(i_param.end_effector_list[i].rot[0], i_param.end_effector_list[i].rot[1], i_param.end_effector_list[i].rot[2], i_param.end_effector_list[i].rot[3])).normalized().toRotationMatrix();
+      }
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl;
+  }
+  if (i_param.default_gait_type == OpenHRP::AutoBalancerService::BIPED) {
+      gait_type = BIPED;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::TROT) {
+      gait_type = TROT;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::PACE) {
+      gait_type = PACE;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::CRAWL) {
+      gait_type = CRAWL;
+  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) {
+      gait_type = GALLOP;
+  }
+  // Ref force balancing
+  std::cerr << "[" << m_profile.instance_name << "] Ref force balancing" << std::endl;
+  if ( use_force == MODE_REF_FORCE_WITH_FOOT && control_mode != MODE_IDLE ) {
+      std::cerr << "[" << m_profile.instance_name << "]   additional_force_applied_point_offset and additional_force_applied_link_name cannot be updated during MODE_REF_FORCE_WITH_FOOT and non-MODE_IDLE"<< std::endl;
+  } else if ( !m_robot->link(std::string(i_param.additional_force_applied_link_name)) ) {
+      std::cerr << "[" << m_profile.instance_name << "]   Invalid link name for additional_force_applied_link_name = " << i_param.additional_force_applied_link_name << std::endl;
+  } else {
+      additional_force_applied_link = m_robot->link(std::string(i_param.additional_force_applied_link_name));
+      for (size_t i = 0; i < 3; i++) {
+          additional_force_applied_point_offset(i) = i_param.additional_force_applied_point_offset[i];
+      }
+      std::cerr << "[" << m_profile.instance_name << "]   Link name for additional_force_applied_link_name = " << additional_force_applied_link->name << ", additional_force_applied_point_offset = " << additional_force_applied_point_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+  }
 
-            for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
-                std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl;
-                std::cerr << "[" << m_profile.instance_name << "]   localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-                std::cerr << "[" << m_profile.instance_name << "]   localR = " << it->second.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
-            }
+  for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
+      std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
+      std::cerr << "[" << m_profile.instance_name << "]   localR = " << it->second.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
+  }
 
-            std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets = ";
-            for (size_t i = 0; i < ikp.size() * 3; i++) {
-                std::cerr << default_zmp_offsets_array[i] << " ";
-            }
-            std::cerr << std::endl;
-            delete[] default_zmp_offsets_array;
-            std::cerr << "[" << m_profile.instance_name << "]   use_force_mode = " << getUseForceModeString() << std::endl;
-            std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_mode = " << graspless_manip_mode << std::endl;
-            std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_arm = " << graspless_manip_arm << std::endl;
-            std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
-            std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_pos = " << graspless_manip_reference_trans_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
-            std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_rot = " << graspless_manip_reference_trans_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
-            std::cerr << "[" << m_profile.instance_name << "]   transition_time = " << transition_time << "[s], zmp_transition_time = " << zmp_transition_time << "[s], adjust_footstep_transition_time = " << adjust_footstep_transition_time << "[s]" << std::endl;
-            for (std::vector<std::string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) std::cerr << "[" << m_profile.instance_name << "]   leg_names [" << *it << "]" << std::endl;
-            std::cerr << "[" << m_profile.instance_name << "]   default_gait_type = " << gait_type << std::endl;
-            // FIK
-            fik->move_base_gain = i_param.move_base_gain;
-            fik->pos_ik_thre = i_param.pos_ik_thre;
-            fik->rot_ik_thre = i_param.rot_ik_thre;
-            fik->printParam();
-            // IK limb parameters
-            fik->setIKParam(ee_vec, i_param.ik_limb_parameters);
-            // Limb stretch avoidance
-            fik->use_limb_stretch_avoidance = i_param.use_limb_stretch_avoidance;
-            fik->limb_stretch_avoidance_time_const = i_param.limb_stretch_avoidance_time_const;
-            for (size_t i = 0; i < 2; i++) {
-                fik->limb_stretch_avoidance_vlimit[i] = i_param.limb_stretch_avoidance_vlimit[i];
-            }
-            for (size_t i = 0; i < fik->ikp.size(); i++) {
-                fik->ikp[ee_vec[i]].limb_length_margin = i_param.limb_length_margin[i];
-            }
-            return true;
+  std::cerr << "[" << m_profile.instance_name << "]   default_zmp_offsets = ";
+  for (size_t i = 0; i < ikp.size() * 3; i++) {
+      std::cerr << default_zmp_offsets_array[i] << " ";
+  }
+  std::cerr << std::endl;
+  delete[] default_zmp_offsets_array;
+  std::cerr << "[" << m_profile.instance_name << "]   use_force_mode = " << getUseForceModeString() << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_mode = " << graspless_manip_mode << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_arm = " << graspless_manip_arm << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_pos = " << graspless_manip_reference_trans_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   graspless_manip_reference_trans_rot = " << graspless_manip_reference_trans_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   transition_time = " << transition_time << "[s], zmp_transition_time = " << zmp_transition_time << "[s], adjust_footstep_transition_time = " << adjust_footstep_transition_time << "[s]" << std::endl;
+  for (std::vector<std::string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) std::cerr << "[" << m_profile.instance_name << "]   leg_names [" << *it << "]" << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   default_gait_type = " << gait_type << std::endl;
+  // FIK
+  fik->move_base_gain = i_param.move_base_gain;
+  fik->pos_ik_thre = i_param.pos_ik_thre;
+  fik->rot_ik_thre = i_param.rot_ik_thre;
+  fik->printParam();
+  // IK limb parameters
+  fik->setIKParam(ee_vec, i_param.ik_limb_parameters);
+  // Limb stretch avoidance
+  fik->use_limb_stretch_avoidance = i_param.use_limb_stretch_avoidance;
+  fik->limb_stretch_avoidance_time_const = i_param.limb_stretch_avoidance_time_const;
+  for (size_t i = 0; i < 2; i++) {
+    fik->limb_stretch_avoidance_vlimit[i] = i_param.limb_stretch_avoidance_vlimit[i];
+  }
+  for (size_t i = 0; i < fik->ikp.size(); i++) {
+    fik->ikp[ee_vec[i]].limb_length_margin = i_param.limb_length_margin[i];
+  }
+  return true;
 };
 
 bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
 {
-    i_param.default_zmp_offsets.length(ikp.size());
-    for (size_t i = 0; i < ikp.size(); i++) {
-        i_param.default_zmp_offsets[i].length(3);
-        for (size_t j = 0; j < 3; j++) {
-            i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j);
-        }
-    }
-    switch(control_mode) {
-        case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break;
-        case MODE_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_ABC; break;
-        case MODE_SYNC_TO_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_IDLE; break;
-        case MODE_SYNC_TO_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_ABC; break;
-        default: break;
-    }
-    switch(use_force) {
-        case MODE_NO_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_NO_FORCE; break;
-        case MODE_REF_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE; break;
-        case MODE_REF_FORCE_WITH_FOOT: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT; break;
-        case MODE_REF_FORCE_RFU_EXT_MOMENT: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT; break;
-        default: break;
-    }
-    i_param.graspless_manip_mode = graspless_manip_mode;
-    i_param.graspless_manip_arm = graspless_manip_arm.c_str();
-    for (size_t j = 0; j < 3; j++)
-        i_param.graspless_manip_p_gain[j] = graspless_manip_p_gain[j];
-    for (size_t j = 0; j < 3; j++)
-        i_param.graspless_manip_reference_trans_pos[j] = graspless_manip_reference_trans_coords.pos[j];
-    Eigen::Quaternion<double> qt(graspless_manip_reference_trans_coords.rot);
-    i_param.graspless_manip_reference_trans_rot[0] = qt.w();
-    i_param.graspless_manip_reference_trans_rot[1] = qt.x();
-    i_param.graspless_manip_reference_trans_rot[2] = qt.y();
-    i_param.graspless_manip_reference_trans_rot[3] = qt.z();
-    i_param.transition_time = transition_time;
-    i_param.zmp_transition_time = zmp_transition_time;
-    i_param.adjust_footstep_transition_time = adjust_footstep_transition_time;
-    i_param.leg_names.length(leg_names.size());
-    for (size_t i = 0; i < leg_names.size(); i++) i_param.leg_names[i] = leg_names.at(i).c_str();
-    i_param.is_hand_fix_mode = is_hand_fix_mode;
-    i_param.end_effector_list.length(ikp.size());
-    {
-        size_t i = 0;
-        for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
-            copyRatscoords2Footstep(i_param.end_effector_list[i],
-                    coordinates(it->second.localPos, it->second.localR));
-            i_param.end_effector_list[i].leg = it->first.c_str();
-            i++;
-        }
-    }
-    switch(gait_type) {
-        case BIPED:  i_param.default_gait_type = OpenHRP::AutoBalancerService::BIPED;  break;
-        case TROT:   i_param.default_gait_type = OpenHRP::AutoBalancerService::TROT;   break;
-        case PACE:   i_param.default_gait_type = OpenHRP::AutoBalancerService::PACE;   break;
-        case CRAWL:  i_param.default_gait_type = OpenHRP::AutoBalancerService::CRAWL;  break;
-        case GALLOP: i_param.default_gait_type = OpenHRP::AutoBalancerService::GALLOP; break;
-        default: break;
-    }
-    // FIK
-    i_param.move_base_gain = fik->move_base_gain;
-    i_param.pos_ik_thre = fik->pos_ik_thre;
-    i_param.rot_ik_thre = fik->rot_ik_thre;
-    // IK limb parameters
-    fik->getIKParam(ee_vec, i_param.ik_limb_parameters);
-    // Limb stretch avoidance
-    i_param.use_limb_stretch_avoidance = fik->use_limb_stretch_avoidance;
-    i_param.limb_stretch_avoidance_time_const = fik->limb_stretch_avoidance_time_const;
-    i_param.limb_length_margin.length(fik->ikp.size());
-    for (size_t i = 0; i < 2; i++) {
-        i_param.limb_stretch_avoidance_vlimit[i] = fik->limb_stretch_avoidance_vlimit[i];
-    }
-    for (size_t i = 0; i < ikp.size(); i++) {
-        i_param.limb_length_margin[i] = fik->ikp[ee_vec[i]].limb_length_margin;
-    }
-    i_param.additional_force_applied_link_name = additional_force_applied_link->name.c_str();
-    for (size_t i = 0; i < 3; i++) {
-        i_param.additional_force_applied_point_offset[i] = additional_force_applied_point_offset(i);
-    }
-    return true;
+  i_param.default_zmp_offsets.length(ikp.size());
+  for (size_t i = 0; i < ikp.size(); i++) {
+      i_param.default_zmp_offsets[i].length(3);
+      for (size_t j = 0; j < 3; j++) {
+          i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j);
+      }
+  }
+  switch(control_mode) {
+  case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break;
+  case MODE_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_ABC; break;
+  case MODE_SYNC_TO_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_IDLE; break;
+  case MODE_SYNC_TO_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_ABC; break;
+  default: break;
+  }
+  switch(use_force) {
+  case MODE_NO_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_NO_FORCE; break;
+  case MODE_REF_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE; break;
+  case MODE_REF_FORCE_WITH_FOOT: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT; break;
+  case MODE_REF_FORCE_RFU_EXT_MOMENT: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT; break;
+  default: break;
+  }
+  i_param.graspless_manip_mode = graspless_manip_mode;
+  i_param.graspless_manip_arm = graspless_manip_arm.c_str();
+  for (size_t j = 0; j < 3; j++)
+      i_param.graspless_manip_p_gain[j] = graspless_manip_p_gain[j];
+  for (size_t j = 0; j < 3; j++)
+      i_param.graspless_manip_reference_trans_pos[j] = graspless_manip_reference_trans_coords.pos[j];
+  Eigen::Quaternion<double> qt(graspless_manip_reference_trans_coords.rot);
+  i_param.graspless_manip_reference_trans_rot[0] = qt.w();
+  i_param.graspless_manip_reference_trans_rot[1] = qt.x();
+  i_param.graspless_manip_reference_trans_rot[2] = qt.y();
+  i_param.graspless_manip_reference_trans_rot[3] = qt.z();
+  i_param.transition_time = transition_time;
+  i_param.zmp_transition_time = zmp_transition_time;
+  i_param.adjust_footstep_transition_time = adjust_footstep_transition_time;
+  i_param.leg_names.length(leg_names.size());
+  for (size_t i = 0; i < leg_names.size(); i++) i_param.leg_names[i] = leg_names.at(i).c_str();
+  i_param.is_hand_fix_mode = is_hand_fix_mode;
+  i_param.end_effector_list.length(ikp.size());
+  {
+      size_t i = 0;
+      for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
+          copyRatscoords2Footstep(i_param.end_effector_list[i],
+                                  coordinates(it->second.localPos, it->second.localR));
+          i_param.end_effector_list[i].leg = it->first.c_str();
+          i++;
+      }
+  }
+  switch(gait_type) {
+  case BIPED:  i_param.default_gait_type = OpenHRP::AutoBalancerService::BIPED;  break;
+  case TROT:   i_param.default_gait_type = OpenHRP::AutoBalancerService::TROT;   break;
+  case PACE:   i_param.default_gait_type = OpenHRP::AutoBalancerService::PACE;   break;
+  case CRAWL:  i_param.default_gait_type = OpenHRP::AutoBalancerService::CRAWL;  break;
+  case GALLOP: i_param.default_gait_type = OpenHRP::AutoBalancerService::GALLOP; break;
+  default: break;
+  }
+  // FIK
+  i_param.move_base_gain = fik->move_base_gain;
+  i_param.pos_ik_thre = fik->pos_ik_thre;
+  i_param.rot_ik_thre = fik->rot_ik_thre;
+  // IK limb parameters
+  fik->getIKParam(ee_vec, i_param.ik_limb_parameters);
+  // Limb stretch avoidance
+  i_param.use_limb_stretch_avoidance = fik->use_limb_stretch_avoidance;
+  i_param.limb_stretch_avoidance_time_const = fik->limb_stretch_avoidance_time_const;
+  i_param.limb_length_margin.length(fik->ikp.size());
+  for (size_t i = 0; i < 2; i++) {
+    i_param.limb_stretch_avoidance_vlimit[i] = fik->limb_stretch_avoidance_vlimit[i];
+  }
+  for (size_t i = 0; i < ikp.size(); i++) {
+    i_param.limb_length_margin[i] = fik->ikp[ee_vec[i]].limb_length_margin;
+  }
+  i_param.additional_force_applied_link_name = additional_force_applied_link->name.c_str();
+  for (size_t i = 0; i < 3; i++) {
+      i_param.additional_force_applied_point_offset[i] = additional_force_applied_point_offset(i);
+  }
+  return true;
 };
 
 void AutoBalancer::copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs)
 {
-    memcpy(out_fs.pos, in_fs.pos.data(), sizeof(double)*3);
-    Eigen::Quaternion<double> qt(in_fs.rot);
-    out_fs.rot[0] = qt.w();
-    out_fs.rot[1] = qt.x();
-    out_fs.rot[2] = qt.y();
-    out_fs.rot[3] = qt.z();
+  memcpy(out_fs.pos, in_fs.pos.data(), sizeof(double)*3);
+  Eigen::Quaternion<double> qt(in_fs.rot);
+  out_fs.rot[0] = qt.w();
+  out_fs.rot[1] = qt.x();
+  out_fs.rot[2] = qt.y();
+  out_fs.rot[3] = qt.z();
 };
 
 bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param)
 {
-    copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_leg_steps().front().worldcoords);
-    copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_steps().front().worldcoords);
-    copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_steps().front().worldcoords);
-    copyRatscoords2Footstep(i_param.swing_leg_dst_coords, gg->get_swing_leg_dst_steps().front().worldcoords);
-    copyRatscoords2Footstep(i_param.dst_foot_midcoords, gg->get_dst_foot_midcoords());
-    if (gg->get_support_leg_names().front() == "rleg") {
-        i_param.support_leg = OpenHRP::AutoBalancerService::RLEG;
-    } else {
-        i_param.support_leg = OpenHRP::AutoBalancerService::LLEG;
-    }
-    switch ( gg->get_current_support_states().front() ) {
-        case BOTH: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::BOTH; break;
-        case RLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::RLEG; break;
-        case LLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::LLEG; break;
-        default: break;
-    }
-    return true;
+  copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_leg_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.swing_leg_dst_coords, gg->get_swing_leg_dst_steps().front().worldcoords);
+  copyRatscoords2Footstep(i_param.dst_foot_midcoords, gg->get_dst_foot_midcoords());
+  if (gg->get_support_leg_names().front() == "rleg") {
+    i_param.support_leg = OpenHRP::AutoBalancerService::RLEG;
+  } else {
+    i_param.support_leg = OpenHRP::AutoBalancerService::LLEG;
+  }
+  switch ( gg->get_current_support_states().front() ) {
+  case BOTH: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::BOTH; break;
+  case RLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::RLEG; break;
+  case LLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::LLEG; break;
+  default: break;
+  }
+  return true;
 };
 
 bool AutoBalancer::adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep)
 {
-    std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
+  std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
             << "] adjustFootSteps" << std::endl;
-    if (control_mode == MODE_ABC && !gg_is_walking && adjust_footstep_interpolator->isEmpty()) {
-        Guard guard(m_mutex);
-        //
-        hrp::Vector3 eepos, org_mid_rpy, target_mid_rpy;
-        hrp::Matrix33 eerot, tmprot;
-        coordinates org_mid_coords, target_mid_coords;
-        // Get org coords
-        ikp["rleg"].adjust_interpolation_org_p0 = ikp["rleg"].target_p0;
-        ikp["lleg"].adjust_interpolation_org_p0 = ikp["lleg"].target_p0;
-        ikp["rleg"].adjust_interpolation_org_r0 = ikp["rleg"].target_r0;
-        ikp["lleg"].adjust_interpolation_org_r0 = ikp["lleg"].target_r0;
-        mid_coords(org_mid_coords, 0.5,
-                coordinates(ikp["rleg"].adjust_interpolation_org_p0, ikp["rleg"].adjust_interpolation_org_r0),
-                coordinates(ikp["lleg"].adjust_interpolation_org_p0, ikp["lleg"].adjust_interpolation_org_r0));
-        org_mid_rpy = hrp::rpyFromRot(org_mid_coords.rot);
-        // Get target coords
-        //   Input : ee coords
-        //   Output : link coords
-        memcpy(eepos.data(), rfootstep.pos, sizeof(double)*3);
-        eerot = (Eigen::Quaternion<double>(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc:
-        ikp["rleg"].adjust_interpolation_target_r0 = eerot;
-        ikp["rleg"].adjust_interpolation_target_p0 = eepos;
-        memcpy(eepos.data(), lfootstep.pos, sizeof(double)*3);
-        eerot = (Eigen::Quaternion<double>(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc:
-        ikp["lleg"].adjust_interpolation_target_r0 = eerot;
-        ikp["lleg"].adjust_interpolation_target_p0 = eepos;
-        mid_coords(target_mid_coords, 0.5,
-                coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0),
-                coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
-        coordinates rtrans, ltrans;
-        target_mid_coords.transformation(rtrans, coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0));
-        target_mid_coords.transformation(ltrans, coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
-        target_mid_rpy = hrp::rpyFromRot(target_mid_coords.rot);
-        // Fix target pos => org pos, target yaw => org yaw
-        target_mid_rpy(2) = org_mid_rpy(2);
-        target_mid_coords.rot = hrp::rotFromRpy(target_mid_rpy);
-        target_mid_coords.pos = org_mid_coords.pos;
-        // Calculate rleg and lleg coords
-        coordinates tmpc;
-        tmpc = target_mid_coords;
-        tmpc.transform(rtrans);
-        ikp["rleg"].adjust_interpolation_target_p0 = tmpc.pos;
-        ikp["rleg"].adjust_interpolation_target_r0 = tmpc.rot;
-        tmpc = target_mid_coords;
-        tmpc.transform(ltrans);
-        ikp["lleg"].adjust_interpolation_target_p0 = tmpc.pos;
-        ikp["lleg"].adjust_interpolation_target_r0 = tmpc.rot;
-        // Set interpolator
-        adjust_footstep_interpolator->clear();
-        double tmp = 0.0;
-        adjust_footstep_interpolator->set(&tmp);
-        tmp = 1.0;
-        adjust_footstep_interpolator->setGoal(&tmp, adjust_footstep_transition_time, true);
-    }
-    while (!adjust_footstep_interpolator->isEmpty() )
-        usleep(1000);
+  if (control_mode == MODE_ABC && !gg_is_walking && adjust_footstep_interpolator->isEmpty()) {
+      Guard guard(m_mutex);
+      //
+      hrp::Vector3 eepos, org_mid_rpy, target_mid_rpy;
+      hrp::Matrix33 eerot, tmprot;
+      coordinates org_mid_coords, target_mid_coords;
+      // Get org coords
+      ikp["rleg"].adjust_interpolation_org_p0 = ikp["rleg"].target_p0;
+      ikp["lleg"].adjust_interpolation_org_p0 = ikp["lleg"].target_p0;
+      ikp["rleg"].adjust_interpolation_org_r0 = ikp["rleg"].target_r0;
+      ikp["lleg"].adjust_interpolation_org_r0 = ikp["lleg"].target_r0;
+      mid_coords(org_mid_coords, 0.5,
+                 coordinates(ikp["rleg"].adjust_interpolation_org_p0, ikp["rleg"].adjust_interpolation_org_r0),
+                 coordinates(ikp["lleg"].adjust_interpolation_org_p0, ikp["lleg"].adjust_interpolation_org_r0));
+      org_mid_rpy = hrp::rpyFromRot(org_mid_coords.rot);
+      // Get target coords
+      //   Input : ee coords
+      //   Output : link coords
+      memcpy(eepos.data(), rfootstep.pos, sizeof(double)*3);
+      eerot = (Eigen::Quaternion<double>(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc:
+      ikp["rleg"].adjust_interpolation_target_r0 = eerot;
+      ikp["rleg"].adjust_interpolation_target_p0 = eepos;
+      memcpy(eepos.data(), lfootstep.pos, sizeof(double)*3);
+      eerot = (Eigen::Quaternion<double>(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc:
+      ikp["lleg"].adjust_interpolation_target_r0 = eerot;
+      ikp["lleg"].adjust_interpolation_target_p0 = eepos;
+      mid_coords(target_mid_coords, 0.5,
+                 coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0),
+                 coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
+      coordinates rtrans, ltrans;
+      target_mid_coords.transformation(rtrans, coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0));
+      target_mid_coords.transformation(ltrans, coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
+      target_mid_rpy = hrp::rpyFromRot(target_mid_coords.rot);
+      // Fix target pos => org pos, target yaw => org yaw
+      target_mid_rpy(2) = org_mid_rpy(2);
+      target_mid_coords.rot = hrp::rotFromRpy(target_mid_rpy);
+      target_mid_coords.pos = org_mid_coords.pos;
+      // Calculate rleg and lleg coords
+      coordinates tmpc;
+      tmpc = target_mid_coords;
+      tmpc.transform(rtrans);
+      ikp["rleg"].adjust_interpolation_target_p0 = tmpc.pos;
+      ikp["rleg"].adjust_interpolation_target_r0 = tmpc.rot;
+      tmpc = target_mid_coords;
+      tmpc.transform(ltrans);
+      ikp["lleg"].adjust_interpolation_target_p0 = tmpc.pos;
+      ikp["lleg"].adjust_interpolation_target_r0 = tmpc.rot;
+      // Set interpolator
+      adjust_footstep_interpolator->clear();
+      double tmp = 0.0;
+      adjust_footstep_interpolator->set(&tmp);
+      tmp = 1.0;
+      adjust_footstep_interpolator->setGoal(&tmp, adjust_footstep_transition_time, true);
+  }
+  while (!adjust_footstep_interpolator->isEmpty() )
     usleep(1000);
-    return true;
+  usleep(1000);
+  return true;
 };
 
 bool AutoBalancer::getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx)
@@ -2077,17 +2073,17 @@ bool AutoBalancer::getGoPosFootstepsSequence(const double& x, const double& y, c
         if (is_valid_gait_type == false) return false;
         /* go_pos_param_2_footstep_nodes_list_core is const member function  */
         gg->go_pos_param_2_footstep_nodes_list_core (x, y, th,
-                initial_support_legs_coords, start_ref_coords, initial_support_legs,
-                new_footstep_nodes_list, true, 0);
+                                                     initial_support_legs_coords, start_ref_coords, initial_support_legs,
+                                                     new_footstep_nodes_list, true, 0);
         o_footstep->length(new_footstep_nodes_list.size());
         for (size_t i = 0; i < new_footstep_nodes_list.size(); i++) {
             o_footstep[i].fs.length(new_footstep_nodes_list.at(i).size());
             for (size_t j = 0; j < new_footstep_nodes_list.at(i).size(); j++) {
                 leg_type tmp_leg_type = new_footstep_nodes_list.at(i).at(j).l_r;
                 o_footstep[i].fs[j].leg = ((tmp_leg_type == RLEG) ? "rleg":
-                        (tmp_leg_type == LLEG) ? "lleg":
-                                (tmp_leg_type == RARM) ? "rarm":
-                                        "larm");
+                                           (tmp_leg_type == LLEG) ? "lleg":
+                                           (tmp_leg_type == RARM) ? "rarm":
+                                           "larm");
                 copyRatscoords2Footstep(o_footstep[i].fs[j], new_footstep_nodes_list.at(i).at(j).worldcoords);
             }
         }
@@ -2097,63 +2093,63 @@ bool AutoBalancer::getGoPosFootstepsSequence(const double& x, const double& y, c
 
 void AutoBalancer::static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height)
 {
-    hrp::Vector3 target_sbp = hrp::Vector3(0, 0, 0);
-    hrp::Vector3 tmpcog = m_robot->calcCM();
-    if ( use_force == MODE_NO_FORCE ) {
-        tmp_input_sbp = tmpcog + sbp_cog_offset;
-    } else {
-        calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height, ref_forces);
-        tmp_input_sbp = target_sbp - sbp_offset;
-        sbp_cog_offset = tmp_input_sbp - tmpcog;
-    }
+  hrp::Vector3 target_sbp = hrp::Vector3(0, 0, 0);
+  hrp::Vector3 tmpcog = m_robot->calcCM();
+  if ( use_force == MODE_NO_FORCE ) {
+    tmp_input_sbp = tmpcog + sbp_cog_offset;
+  } else {
+    calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height, ref_forces);
+    tmp_input_sbp = target_sbp - sbp_offset;
+    sbp_cog_offset = tmp_input_sbp - tmpcog;
+  }
 };
 
 void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces)
 {
-    hrp::Vector3 denom, nume;
-    /* sb_point[m] = nume[kg * m/s^2 * m] / denom[kg * m/s^2] */
-    double mass = m_robot->totalMass();
-    double mg = mass * gg->get_gravitational_acceleration();
-    hrp::Vector3 total_sensor_ref_force = hrp::Vector3::Zero();
-    for (size_t i = 0; i < tmp_forces.size(); i++) {
-        total_sensor_ref_force += tmp_forces[i];
-    }
-    hrp::Vector3 total_nosensor_ref_force = mg * hrp::Vector3::UnitZ() - total_sensor_ref_force; // total ref force at the point without sensors, such as torso
-    hrp::Vector3 tmp_ext_moment = fix_leg_coords2.pos.cross(total_nosensor_ref_force) + fix_leg_coords2.rot * hrp::Vector3(m_refFootOriginExtMoment.data.x, m_refFootOriginExtMoment.data.y, m_refFootOriginExtMoment.data.z);
-    // For MODE_REF_FORCE_RFU_EXT_MOMENT, store previous root position to calculate influence from tmp_ext_moment while walking (basically, root link moves while walking).
-    //   Calculate values via fix_leg_coords2 relative/world values.
-    static hrp::Vector3 prev_additional_force_applied_pos = fix_leg_coords2.rot.transpose() * (additional_force_applied_link->p-fix_leg_coords2.pos);
-    //   If not is_hold_value (not hold value), update prev_additional_force_applied_pos
-    if ( !m_refFootOriginExtMomentIsHoldValue.data ) {
-        prev_additional_force_applied_pos = fix_leg_coords2.rot.transpose() * (additional_force_applied_link->p-fix_leg_coords2.pos);
-    }
-    hrp::Vector3 tmp_prev_additional_force_applied_pos = fix_leg_coords2.rot * prev_additional_force_applied_pos + fix_leg_coords2.pos;
-    // Calculate SBP
-    for (size_t j = 0; j < 2; j++) {
-        nume(j) = mg * tmpcog(j);
-        denom(j) = mg;
-        for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-            // Check leg_names. leg_names is assumed to be support limb for locomotion, cannot be used for manipulation. If it->first is not included in leg_names, use it for manipulation and static balance point calculation.
-            if (std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()) {
-                size_t idx = contact_states_index_map[it->first];
-                // Force applied point is assumed as end effector
-                hrp::Vector3 fpos = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
-                nume(j) += ( (fpos(2) - ref_com_height) * tmp_forces[idx](j) - fpos(j) * tmp_forces[idx](2) );
-                denom(j) -= tmp_forces[idx](2);
-            }
-        }
-        if ( use_force == MODE_REF_FORCE_WITH_FOOT ) {
-            hrp::Vector3 fpos(additional_force_applied_link->p+additional_force_applied_point_offset);
-            nume(j) += ( (fpos(2) - ref_com_height) * total_nosensor_ref_force(j) - fpos(j) * total_nosensor_ref_force(2) );
-            denom(j) -= total_nosensor_ref_force(2);
-        } else if ( use_force == MODE_REF_FORCE_RFU_EXT_MOMENT ) {
-            //nume(j) += (j==0 ? tmp_ext_moment(1):-tmp_ext_moment(0));
-            nume(j) += (tmp_prev_additional_force_applied_pos(j)-additional_force_applied_link->p(j))*total_nosensor_ref_force(2) + (j==0 ? tmp_ext_moment(1):-tmp_ext_moment(0));
-            denom(j) -= total_nosensor_ref_force(2);
-        }
-        sb_point(j) = nume(j) / denom(j);
-    }
-    sb_point(2) = ref_com_height;
+  hrp::Vector3 denom, nume;
+  /* sb_point[m] = nume[kg * m/s^2 * m] / denom[kg * m/s^2] */
+  double mass = m_robot->totalMass();
+  double mg = mass * gg->get_gravitational_acceleration();
+  hrp::Vector3 total_sensor_ref_force = hrp::Vector3::Zero();
+  for (size_t i = 0; i < tmp_forces.size(); i++) {
+      total_sensor_ref_force += tmp_forces[i];
+  }
+  hrp::Vector3 total_nosensor_ref_force = mg * hrp::Vector3::UnitZ() - total_sensor_ref_force; // total ref force at the point without sensors, such as torso
+  hrp::Vector3 tmp_ext_moment = fix_leg_coords2.pos.cross(total_nosensor_ref_force) + fix_leg_coords2.rot * hrp::Vector3(m_refFootOriginExtMoment.data.x, m_refFootOriginExtMoment.data.y, m_refFootOriginExtMoment.data.z);
+  // For MODE_REF_FORCE_RFU_EXT_MOMENT, store previous root position to calculate influence from tmp_ext_moment while walking (basically, root link moves while walking).
+  //   Calculate values via fix_leg_coords2 relative/world values.
+  static hrp::Vector3 prev_additional_force_applied_pos = fix_leg_coords2.rot.transpose() * (additional_force_applied_link->p-fix_leg_coords2.pos);
+  //   If not is_hold_value (not hold value), update prev_additional_force_applied_pos
+  if ( !m_refFootOriginExtMomentIsHoldValue.data ) {
+      prev_additional_force_applied_pos = fix_leg_coords2.rot.transpose() * (additional_force_applied_link->p-fix_leg_coords2.pos);
+  }
+  hrp::Vector3 tmp_prev_additional_force_applied_pos = fix_leg_coords2.rot * prev_additional_force_applied_pos + fix_leg_coords2.pos;
+  // Calculate SBP
+  for (size_t j = 0; j < 2; j++) {
+    nume(j) = mg * tmpcog(j);
+    denom(j) = mg;
+    for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+      // Check leg_names. leg_names is assumed to be support limb for locomotion, cannot be used for manipulation. If it->first is not included in leg_names, use it for manipulation and static balance point calculation.
+      if (std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()) {
+        size_t idx = contact_states_index_map[it->first];
+        // Force applied point is assumed as end effector
+        hrp::Vector3 fpos = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
+        nume(j) += ( (fpos(2) - ref_com_height) * tmp_forces[idx](j) - fpos(j) * tmp_forces[idx](2) );
+        denom(j) -= tmp_forces[idx](2);
+      }
+    }
+    if ( use_force == MODE_REF_FORCE_WITH_FOOT ) {
+        hrp::Vector3 fpos(additional_force_applied_link->p+additional_force_applied_point_offset);
+        nume(j) += ( (fpos(2) - ref_com_height) * total_nosensor_ref_force(j) - fpos(j) * total_nosensor_ref_force(2) );
+        denom(j) -= total_nosensor_ref_force(2);
+    } else if ( use_force == MODE_REF_FORCE_RFU_EXT_MOMENT ) {
+        //nume(j) += (j==0 ? tmp_ext_moment(1):-tmp_ext_moment(0));
+        nume(j) += (tmp_prev_additional_force_applied_pos(j)-additional_force_applied_link->p(j))*total_nosensor_ref_force(2) + (j==0 ? tmp_ext_moment(1):-tmp_ext_moment(0));
+        denom(j) -= total_nosensor_ref_force(2);
+    }
+    sb_point(j) = nume(j) / denom(j);
+  }
+  sb_point(2) = ref_com_height;
 };
 
 #ifndef rad2deg
@@ -2165,72 +2161,72 @@ void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point,
 
 hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_coords)
 {
-    if (graspless_manip_mode) {
-        hrp::Vector3 dp,dr;
-        coordinates ref_hand_coords(gg->get_dst_foot_midcoords()), act_hand_coords;
-        ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords
-        hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos);
-        if ( graspless_manip_arm == "arms" ) {
-            hrp::Vector3 rarm_pos = ikp["rarm"].target_p0;
-            hrp::Vector3 larm_pos = ikp["larm"].target_p0;
-            act_hand_coords.pos = (rarm_pos+larm_pos)/2.0;
-            hrp::Vector3 act_y = larm_pos-rarm_pos;
-            act_y(2) = 0;
-            act_y.normalize();
-            hrp::Vector3 ref_y(ref_hand_coords.rot * hrp::Vector3::UnitY());
-            ref_y(2) = 0;
-            ref_y.normalize();
-            dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y))); // fix for rotation
-        } else {
-            ABCIKparam& tmpikp = ikp[graspless_manip_arm];
-            act_hand_coords = rats::coordinates(tmpikp.target_p0,
-                    tmpikp.target_r0);
-            rats::difference_rotation(dr, ref_hand_coords.rot, act_hand_coords.rot);
-            dr(0) = 0; dr(1) = 0;
-        }
-        dp = act_hand_coords.pos - ref_hand_coords.pos
-                + dr.cross(hrp::Vector3(foot_pos - act_hand_coords.pos));
-        dp(2) = 0;
-        hrp::Matrix33 foot_mt(gg->get_dst_foot_midcoords().rot.transpose());
-        //alias(dp) = foot_mt * dp;
-        hrp::Vector3 dp2 = foot_mt * dp;
-        //alias(dr) = foot_mt * dr;
-        return hrp::Vector3(graspless_manip_p_gain[0] * dp2(0)/gg->get_default_step_time(),
-                graspless_manip_p_gain[1] * dp2(1)/gg->get_default_step_time(),
-                graspless_manip_p_gain[2] * rad2deg(dr(2))/gg->get_default_step_time());
+  if (graspless_manip_mode) {
+    hrp::Vector3 dp,dr;
+    coordinates ref_hand_coords(gg->get_dst_foot_midcoords()), act_hand_coords;
+    ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords
+    hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos);
+    if ( graspless_manip_arm == "arms" ) {
+        hrp::Vector3 rarm_pos = ikp["rarm"].target_p0;
+        hrp::Vector3 larm_pos = ikp["larm"].target_p0;
+        act_hand_coords.pos = (rarm_pos+larm_pos)/2.0;
+        hrp::Vector3 act_y = larm_pos-rarm_pos;
+        act_y(2) = 0;
+        act_y.normalize();
+        hrp::Vector3 ref_y(ref_hand_coords.rot * hrp::Vector3::UnitY());
+        ref_y(2) = 0;
+        ref_y.normalize();
+        dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y))); // fix for rotation
     } else {
-        return hrp::Vector3::Zero();
-    }
+      ABCIKparam& tmpikp = ikp[graspless_manip_arm];
+      act_hand_coords = rats::coordinates(tmpikp.target_p0,
+                                          tmpikp.target_r0);
+      rats::difference_rotation(dr, ref_hand_coords.rot, act_hand_coords.rot);
+      dr(0) = 0; dr(1) = 0;
+    }
+    dp = act_hand_coords.pos - ref_hand_coords.pos
+        + dr.cross(hrp::Vector3(foot_pos - act_hand_coords.pos));
+    dp(2) = 0;
+    hrp::Matrix33 foot_mt(gg->get_dst_foot_midcoords().rot.transpose());
+    //alias(dp) = foot_mt * dp;
+    hrp::Vector3 dp2 = foot_mt * dp;
+    //alias(dr) = foot_mt * dr;
+    return hrp::Vector3(graspless_manip_p_gain[0] * dp2(0)/gg->get_default_step_time(),
+                        graspless_manip_p_gain[1] * dp2(1)/gg->get_default_step_time(),
+                        graspless_manip_p_gain[2] * rad2deg(dr(2))/gg->get_default_step_time());
+  } else {
+    return hrp::Vector3::Zero();
+  }
 };
 
 bool AutoBalancer::calc_inital_support_legs(const double& y, std::vector<coordinates>& initial_support_legs_coords, std::vector<leg_type>& initial_support_legs, coordinates& start_ref_coords) {
     switch(gait_type) {
-        case BIPED:
-            initial_support_legs_coords = (y > 0 ?
-                    boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))
-            : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0)));
-            initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
-            break;
-        case TROT:
-            initial_support_legs_coords = (y > 0 ?
-                    boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))(coordinates(ikp["larm"].target_p0, ikp["larm"].target_r0))
-                    : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0))(coordinates(ikp["rarm"].target_p0, ikp["rarm"].target_r0)));
-            initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
-            break;
-        case PACE:
-            initial_support_legs_coords = (y > 0 ?
-                    boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))(coordinates(ikp["rarm"].target_p0, ikp["rarm"].target_r0))
-                    : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0))(coordinates(ikp["larm"].target_p0, ikp["larm"].target_r0)));
-            initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
-            break;
-        case CRAWL:
-            std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
-            return false;
-        case GALLOP:
-            /* at least one leg shoud be in contact */
-            std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
-            return false;
-        default: break;
+    case BIPED:
+        initial_support_legs_coords = (y > 0 ?
+                                       boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))
+                                       : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0)));
+        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
+        break;
+    case TROT:
+        initial_support_legs_coords = (y > 0 ?
+                                       boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))(coordinates(ikp["larm"].target_p0, ikp["larm"].target_r0))
+                                       : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0))(coordinates(ikp["rarm"].target_p0, ikp["rarm"].target_r0)));
+        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
+        break;
+    case PACE:
+        initial_support_legs_coords = (y > 0 ?
+                                       boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))(coordinates(ikp["rarm"].target_p0, ikp["rarm"].target_r0))
+                                       : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0))(coordinates(ikp["larm"].target_p0, ikp["larm"].target_r0)));
+        initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
+        break;
+    case CRAWL:
+        std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    case GALLOP:
+        /* at least one leg shoud be in contact */
+        std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
+        return false;
+    default: break;
     }
     start_ref_coords.pos = (ikp["rleg"].target_p0+ikp["lleg"].target_p0)*0.5;
     mid_rot(start_ref_coords.rot, 0.5, ikp["rleg"].target_r0, ikp["lleg"].target_r0);
@@ -2330,8 +2326,8 @@ void AutoBalancer::distributeReferenceZMPToWrenches (const hrp::Vector3& _ref_zm
         m_force[fidx].data[5] = n_ee(2);
         if (DEBUGP) {
             std::cerr << "[" << m_profile.instance_name << "]   "
-                    << "ref_force  [" << leg_names[i] << "] " << f_ee.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
-                    << "ref_moment [" << leg_names[i] << "] " << n_ee.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
+                      << "ref_force  [" << leg_names[i] << "] " << f_ee.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
+                      << "ref_moment [" << leg_names[i] << "] " << n_ee.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
         }
     }
     if (DEBUGP) {
@@ -2355,16 +2351,16 @@ void AutoBalancer::distributeReferenceZMPToWrenches (const hrp::Vector3& _ref_zm
 std::string AutoBalancer::getUseForceModeString ()
 {
     switch (use_force) {
-        case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
-            return "MODE_NO_FORCE";
-        case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
-            return "MODE_REF_FORCE";
-        case OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT:
-            return "MODE_REF_FORCE_WITH_FOOT";
-        case OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT:
-            return "MODE_REF_FORCE_RFU_EXT_MOMENT";
-        default:
-            return "";
+    case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
+        return "MODE_NO_FORCE";
+    case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
+        return "MODE_REF_FORCE";
+    case OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT:
+        return "MODE_REF_FORCE_WITH_FOOT";
+    case OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT:
+        return "MODE_REF_FORCE_RFU_EXT_MOMENT";
+    default:
+        return "";
     }
 };
 
@@ -2376,8 +2372,8 @@ extern "C"
     {
         RTC::Properties profile(autobalancer_spec);
         manager->registerFactory(profile,
-                RTC::Create<AutoBalancer>,
-                RTC::Delete<AutoBalancer>);
+                                 RTC::Create<AutoBalancer>,
+                                 RTC::Delete<AutoBalancer>);
     }
 
 };

From ae4c85642e183a9006dd3fae85efcb307aa80ade Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 5 Sep 2017 00:47:52 +0900
Subject: [PATCH 126/217] clean up

---
 rtc/AutoBalancer/AutoBalancer.cpp                 | 2 +-
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 5 -----
 2 files changed, 1 insertion(+), 6 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 800cc159665..946bf775d0a 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1132,7 +1132,7 @@ void AutoBalancer::solveFullbodyIK ()
         //  tmp.targetRpy = hrp::Vector3(0, 10, 0);//reference angular momentum
         tmp.constraint_weight << 3,3,1,0.01,0.01,0.01;
         ik_tgt_list.push_back(tmp);
-    }{
+    }
 
   if( m_robot->link("RARM_JOINT2") != NULL) m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇の干渉回避のため
   if( m_robot->link("LARM_JOINT2") != NULL) m_robot->link("LARM_JOINT2")->llimit = deg2rad(40);
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 5b2668183eb..636e29026d5 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -371,13 +371,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
                 m_optionalData.data.length(optionalDataLength);//TODO:これいいのか？
                 for(int i=0;i<optionalDataLength;i++)m_optionalData.data[i] = 0;
             }
-            //        m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = hsp->rp_ref_out.tgt[rf].is_contact;
-            //        m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = hsp->rp_ref_out.tgt[lf].is_contact;
-            //        bool rf_is_contact =
             m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = hsp->rp_ref_out.tgt[rf].is_contact;
             m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = hsp->rp_ref_out.tgt[lf].is_contact;
-            dbg(hsp->rp_ref_out.tgt[rf].is_contact);
-            dbg(hsp->rp_ref_out.tgt[lf].is_contact);
         }
         hsp->baselinkpose.p = m_robot->rootLink()->p;
         hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);

From 9a89dd3fd9cca2966e9fc909b08d46ae89c2a963 Mon Sep 17 00:00:00 2001
From: ishiguroJSK <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 5 Sep 2017 14:32:03 +0900
Subject: [PATCH 127/217] clean up

---
 .../FullbodyInverseKinematicsSolver.h         | 71 +++++++++++++------
 1 file changed, 49 insertions(+), 22 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 7cb3ee4b450..2467e6ae99c 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -76,22 +76,22 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
             return loop;
         }
         bool checkIKConvergence(const hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list){
-//            if(dq_all.norm() < deg2rad(0.1)){ return true; }
+            //            if(dq_all.norm() < deg2rad(0.1)){ return true; }
             for ( int i=0; i<_ikc_list.size(); i++ ) {
                 hrp::Link* link_tgt_ptr = _robot->link(_ikc_list[i].target_link_name);
                 hrp::Vector3 pos_err, rot_err;
                 if(link_tgt_ptr){
                     pos_err = _ikc_list[i].targetPos - (link_tgt_ptr->p + link_tgt_ptr->R * _ikc_list[i].localPos);
-//                    rot_err = omegaFromRotEx(hrp::rotFromRpy(_ikc_list[i].targetRpy).transpose() * link_tgt_ptr->R * _ikc_list[i].localR);
+                    //                    rot_err = omegaFromRotEx(hrp::rotFromRpy(_ikc_list[i].targetRpy).transpose() * link_tgt_ptr->R * _ikc_list[i].localR);
                     rats::difference_rotation(rot_err, (link_tgt_ptr->R * _ikc_list[i].localR), hrp::rotFromRpy(_ikc_list[i].targetRpy));
-//                    rot_err = omegaFromRotEx(hrp::rotFromRpy(_ikc_list[i].targetRpy).transpose() * link_tgt_ptr->R * _ikc_list[i].localR.transpose());
-//                    dr_part = link_tgt_ptr->R * omegaFromRotEx(link_tgt_ptr->R.transpose() * R_origin_ref);
+                    //                    rot_err = omegaFromRotEx(hrp::rotFromRpy(_ikc_list[i].targetRpy).transpose() * link_tgt_ptr->R * _ikc_list[i].localR.transpose());
+                    //                    dr_part = link_tgt_ptr->R * omegaFromRotEx(link_tgt_ptr->R.transpose() * R_origin_ref);
                 }
                 else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){  // COM
                     pos_err = _ikc_list[i].targetPos - (_robot->calcCM() + _ikc_list[i].localR * _ikc_list[i].localPos);
                     rot_err = _ikc_list[i].targetRpy - (cur_momentum_L - _robot->rootLink()->p.cross(cur_momentum_P));
                 }
-//                dbg(i); dbg(pos_err.transpose()); dbg(rot_err.transpose());
+                //                dbg(i); dbg(pos_err.transpose()); dbg(rot_err.transpose());
                 for(int j=0;j<3;j++){
                     if(fabs(pos_err(j)) > _ikc_list[i].pos_precision && _ikc_list[i].constraint_weight.head(3)(j) > 0){return false;}
                     if(fabs(rot_err(j)) > _ikc_list[i].rot_precision && _ikc_list[i].constraint_weight.tail(3)(j) > 0){return false;}
@@ -120,22 +120,17 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
                 if(link_tgt_ptr){//ベースリンク，通常リンク共通
                     dp_part = p_origin_ref - link_tgt_ptr->p;
                     dr_part = link_tgt_ptr->R * omegaFromRotEx(link_tgt_ptr->R.transpose() * R_origin_ref);
-//                    dr_part = link_tgt_ptr->R * matrix_logEx(link_tgt_ptr->R.transpose() * R_origin_ref);
-
-                    if(link_tgt_ptr == _robot->rootLink()){ //ベース限定
-                        J_part.rightCols(WS_DOF) = hrp::dmatrix::Identity(WS_DOF, WS_DOF);
-                    }
-                    else{ //通常リンク限定
-                        hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, "");
-                        hrp::dmatrix J_jpath;
-                        tgt_jpath.calcJacobian(J_jpath);
-                        for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
-                            int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
-                            J_part.col(id_in_body) = J_jpath.col(id_in_jpath);
-                        }
-                        J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
-                        J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - _robot->rootLink()->p);
+                    //                    dr_part = link_tgt_ptr->R * matrix_logEx(link_tgt_ptr->R.transpose() * R_origin_ref);
+
+                    hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, "");
+                    hrp::dmatrix J_jpath;
+                    tgt_jpath.calcJacobian(J_jpath);
+                    for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
+                        int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
+                        J_part.col(id_in_body) = J_jpath.col(id_in_jpath);
                     }
+                    J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
+                    J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - _robot->rootLink()->p);
                 }
                 else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){ //重心限定
                     dp_part = p_origin_ref - _robot->calcCM();
@@ -158,8 +153,8 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
             weight_all = selection_mat * weight_all;
 
             const double wn_const = 1e-6;
-            hrp::dmatrix Wn = (err_all.transpose() * weight_all.asDiagonal() * err_all + wn_const) * hrp::dmatrix::Identity(ALL_DOF, ALL_DOF);
-            Wn *= 0.01;
+            const double auto_lambda_gain = 0.01;
+            hrp::dmatrix Wn = (static_cast<double>(err_all.transpose() * weight_all.asDiagonal() * err_all) * auto_lambda_gain + wn_const) * hrp::dmatrix::Identity(ALL_DOF, ALL_DOF);
             hrp::dmatrix H = J_all.transpose() * weight_all.asDiagonal() * J_all + Wn;
             hrp::dvector g = J_all.transpose() * weight_all.asDiagonal() * err_all;
 
@@ -204,6 +199,38 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
             _robot->calcForwardKinematics();
         }
 
+        void calcJacobianAndErrorVector_BaseLink(hrp::BodyPtr _robot, const IKConstraint& _ikc, hrp::dmatrix _J_part, hrp::dvector6 _d_part){
+            hrp::Link* link_tgt_ptr = _robot->link(_ikc.target_link_name);
+            const hrp::Matrix33 R_origin_ref = hrp::rotFromRpy(_ikc.targetRpy) * _ikc.localR.transpose(); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
+            const hrp::Vector3 p_origin_ref = _ikc.targetPos - R_origin_ref * _ikc.localPos;
+            _d_part.head(3) = p_origin_ref - link_tgt_ptr->p;
+            _d_part.tail(3) = link_tgt_ptr->R * omegaFromRotEx(link_tgt_ptr->R.transpose() * R_origin_ref);
+            hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, "");
+            tgt_jpath.calcJacobian(_J_part);
+            for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
+                int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
+                _J_part.col(id_in_body) = _J_part.col(id_in_jpath);
+            }
+            _J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
+            _J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - _robot->rootLink()->p);
+        }
+
+        void calcJacobianAndErrorVector_NormalLink(hrp::BodyPtr _robot, const IKConstraint& _ikc, hrp::dmatrix _J_part, hrp::dvector6 _d_part){
+            hrp::Link* link_tgt_ptr = _robot->link(_ikc.target_link_name);
+            const hrp::Matrix33 R_origin_ref = hrp::rotFromRpy(_ikc.targetRpy) * _ikc.localR.transpose(); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
+            const hrp::Vector3 p_origin_ref = _ikc.targetPos - R_origin_ref * _ikc.localPos;
+            _d_part.head(3) = p_origin_ref - link_tgt_ptr->p;
+            _d_part.tail(3) = link_tgt_ptr->R * omegaFromRotEx(link_tgt_ptr->R.transpose() * R_origin_ref);
+            hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, "");
+            tgt_jpath.calcJacobian(_J_part);
+            for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
+                int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
+                _J_part.col(id_in_body) = _J_part.col(id_in_jpath);
+            }
+            _J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
+            _J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - _robot->rootLink()->p);
+        }
+
     protected:
         bool calcJacobianInverseNullspace_copy(hrp::BodyPtr _robot, hrp::dmatrix &_J, hrp::dmatrix &_Jinv) {
             hrp::dmatrix w = hrp::dmatrix::Identity(ALL_DOF,ALL_DOF);

From 1b303838c0ad91bea94619653746c0ad1414fc67 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 8 Sep 2017 21:34:23 +0900
Subject: [PATCH 128/217] add joint weight

---
 rtc/AutoBalancer/AutoBalancer.cpp             |  20 +-
 .../FullbodyInverseKinematicsSolver.h         | 195 +++++++++++-------
 .../WholeBodyMasterSlave.cpp                  |  44 ++--
 rtc/WholeBodyMasterSlave/wbms_core.h          |   6 +-
 4 files changed, 168 insertions(+), 97 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 946bf775d0a..eb6698f182d 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -496,6 +496,7 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     hrp::Vector3 ref_basePos;
     hrp::Matrix33 ref_baseRot;
     hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
+
     if ( is_legged_robot ) {
       // For parameters
       fik->storeCurrentParameters();
@@ -1076,6 +1077,9 @@ void AutoBalancer::fixLegToCoords2 (coordinates& tmp_fix_coords)
 
 void AutoBalancer::solveFullbodyIK ()
 {
+    static hrp::dvector q_old = hrp::getQAll(m_robot);
+    static hrp::Vector3 base_p_old = m_robot->rootLink()->p;
+    static hrp::Matrix33 base_R_old = m_robot->rootLink()->R;
     std::vector<IKConstraint> ik_tgt_list;
     {
         IKConstraint tmp;
@@ -1137,12 +1141,24 @@ void AutoBalancer::solveFullbodyIK ()
   if( m_robot->link("RARM_JOINT2") != NULL) m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇の干渉回避のため
   if( m_robot->link("LARM_JOINT2") != NULL) m_robot->link("LARM_JOINT2")->llimit = deg2rad(40);
 
-//  fik_in->reference_gain.fill(0.001);
-//  fik_in->reference_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
+  fik->dq_weight_all(m_robot->link("CHEST_JOINT0")->jointId) = 0.1;
+  fik->dq_weight_all(m_robot->link("CHEST_JOINT1")->jointId) = 0.1;
+  fik->dq_weight_all(m_robot->link("CHEST_JOINT2")->jointId) = 0.1;
+
+  for(int i=0;i<m_robot->numJoints();i++) fik->q_ref(i) = m_qRef.data[i];
+  fik->q_ref_pullback_gain.head(m_robot->numJoints()).fill(0.001);
+
+  hrp::setQAll(m_robot, q_old);
+  m_robot->rootLink()->p = base_p_old;
+  m_robot->rootLink()->R = base_R_old;
 
   int loop_result = 0;
   const int IK_MAX_LOOP = 2;//2回は必須な気がする・・・
   loop_result = fik->solveFullbodyIKLoop(m_robot, ik_tgt_list, IK_MAX_LOOP);
+
+  q_old = hrp::getQAll(m_robot);
+  base_p_old = m_robot->rootLink()->p;
+  base_R_old = m_robot->rootLink()->R;
 }
 
 
diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 2467e6ae99c..0e2ca791a83 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -14,6 +14,11 @@
 #define eps_eq(a, b, c)  (fabs((a)-(b)) <= c)
 #define dbg(var) std::cout<<#var"= "<<(var)<<std::endl
 #define dbgn(var) std::cout<<#var"= "<<std::endl<<(var)<<std::endl
+namespace hrp{//ちょっとした関数定義
+    static hrp::dvector getQAll(const hrp::BodyPtr _robot){ hrp::dvector tmp(_robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ tmp(i) = _robot->joint(i)->q; } return tmp; };
+    static void setQAll(hrp::BodyPtr _robot, const hrp::dvector in){ assert(in.size() <= _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = in(i); } };
+}
+
 
 class IKConstraint {
     public:
@@ -39,21 +44,22 @@ class IKConstraint {
 class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSolver{
     protected:
         hrp::dmatrix J_all, J_all_inv, J_com, J_am;
-        hrp::dvector dq_all, err_all, weight_all;
+        hrp::dvector dq_all, err_all, constraint_weight_all, avoid_weight_gain;
         const int WS_DOF, BASE_DOF, J_DOF, ALL_DOF;
-        hrp::Vector3 cur_momentum_P, cur_momentum_L;
     public:
-        hrp::dvector joint_weight;
+        hrp::dvector dq_weight_all;
         hrp::dvector q_ref, q_ref_pullback_gain;
+        hrp::Vector3 cur_momentum_around_COM;
         FullbodyInverseKinematicsSolver (hrp::BodyPtr _robot, const std::string& _print_str, const double _dt)
         : SimpleFullbodyInverseKinematicsSolver(_robot,_print_str, _dt),
           WS_DOF(6),
           BASE_DOF(6),
           J_DOF(_robot->numJoints()),
           ALL_DOF(J_DOF+BASE_DOF) {
-            joint_weight = hrp::dvector::Ones(ALL_DOF);
+            dq_weight_all = hrp::dvector::Ones(ALL_DOF);
             dq_all = q_ref = q_ref_pullback_gain = hrp::dvector::Zero(ALL_DOF);
-            cur_momentum_P = cur_momentum_L = hrp::Vector3::Zero();
+            cur_momentum_around_COM = hrp::Vector3::Zero();
+            avoid_weight_gain = hrp::dvector::Constant(ALL_DOF, 1e12);
         };
         ~FullbodyInverseKinematicsSolver () {};
         int solveFullbodyIKLoop (hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list, const int _max_iteration) {
@@ -68,8 +74,11 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
                 _robot->rootLink()->v = (_robot->rootLink()->p - base_p_old)/ m_dt;
                 _robot->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * _robot->rootLink()->R) / m_dt;
                 for(int i=0;i<J_DOF;i++){ _robot->joint(i)->dq = (_robot->joint(i)->q - q_old(i)) / m_dt; }
+
                 _robot->calcForwardKinematics(true,false);
-                _robot->calcTotalMomentum(cur_momentum_P, cur_momentum_L);
+                hrp::Vector3 tmp_P, tmp_L;
+                _robot->calcTotalMomentum(tmp_P, tmp_L);
+                cur_momentum_around_COM = tmp_L - _robot->calcCM().cross(tmp_P);
                 loop++;
                 if(checkIKConvergence(_robot, _ikc_list)){ break; }
             }
@@ -89,9 +98,8 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
                 }
                 else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){  // COM
                     pos_err = _ikc_list[i].targetPos - (_robot->calcCM() + _ikc_list[i].localR * _ikc_list[i].localPos);
-                    rot_err = _ikc_list[i].targetRpy - (cur_momentum_L - _robot->rootLink()->p.cross(cur_momentum_P));
+                    rot_err = _ikc_list[i].targetRpy - cur_momentum_around_COM;
                 }
-                //                dbg(i); dbg(pos_err.transpose()); dbg(rot_err.transpose());
                 for(int j=0;j<3;j++){
                     if(fabs(pos_err(j)) > _ikc_list[i].pos_precision && _ikc_list[i].constraint_weight.head(3)(j) > 0){return false;}
                     if(fabs(rot_err(j)) > _ikc_list[i].rot_precision && _ikc_list[i].constraint_weight.tail(3)(j) > 0){return false;}
@@ -102,7 +110,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
         void solveFullbodyIK(hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list){
             J_all = hrp::dmatrix::Zero(WS_DOF*_ikc_list.size(), ALL_DOF);
             err_all = hrp::dvector::Zero(WS_DOF*_ikc_list.size());
-            weight_all = hrp::dvector::Ones(WS_DOF*_ikc_list.size());
+            constraint_weight_all = hrp::dvector::Ones(WS_DOF*_ikc_list.size());
             //リファレンスに微少量戻す
             for(int i=0;i<J_DOF;i++){ _robot->joint(i)->q = _robot->joint(i)->q * ( 1 - q_ref_pullback_gain(i)) + q_ref(i) * q_ref_pullback_gain(i); }
             _robot->rootLink()->p = _robot->rootLink()->p.cwiseProduct( hrp::Vector3::Ones() - q_ref_pullback_gain.segment(J_DOF, 3) ) + q_ref.segment(J_DOF, 3).cwiseProduct( q_ref_pullback_gain.segment(J_DOF, 3) );
@@ -134,29 +142,60 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
                 }
                 else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){ //重心限定
                     dp_part = p_origin_ref - _robot->calcCM();
-                    dr_part = (_ikc_list[i].targetRpy - (cur_momentum_L - _robot->rootLink()->p.cross(cur_momentum_P))) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける
+                    dr_part = (_ikc_list[i].targetRpy - cur_momentum_around_COM) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける
                     _robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
                     _robot->calcAngularMomentumJacobian(NULL, J_am);//すでにrootlink周りの角運動量ヤコビアンが返ってくる？
                     J_part << J_com, J_am;
                 }
                 else{ std::cerr<<"Unknown Link Target !!"<<std::endl; continue; } //不明なリンク指定
                 err_all.segment(WS_DOF*i,WS_DOF) << dp_part, dr_part;
-                weight_all.segment(WS_DOF*i, WS_DOF) = _ikc_list[i].constraint_weight;
+                constraint_weight_all.segment(WS_DOF*i, WS_DOF) = _ikc_list[i].constraint_weight;
                 J_all.middleRows(WS_DOF*i, WS_DOF) = J_part;
             }
 
-            hrp::dmatrix selection_mat = hrp::dmatrix::Zero((weight_all.array()>0.0).count(), WS_DOF*_ikc_list.size());
-            for(int i=0, j=0; i<weight_all.rows(); i++){ if(weight_all(i) > 0.0){ selection_mat(j, i) = 1.0; j++; } }
+            hrp::dmatrix selection_mat = hrp::dmatrix::Zero((constraint_weight_all.array()>0.0).count(), WS_DOF*_ikc_list.size());
+            for(int i=0, j=0; i<constraint_weight_all.rows(); i++){ if(constraint_weight_all(i) > 0.0){ selection_mat(j, i) = 1.0; j++; } }
 
             J_all = selection_mat * J_all;
             err_all = selection_mat * err_all;
-            weight_all = selection_mat * weight_all;
+            constraint_weight_all = selection_mat * constraint_weight_all;
+
+
+            hrp::dvector dq_weight_all_jlim = dq_weight_all;
+            for ( int j = 0; j < _robot->numJoints() ; j++ ) {
+                double jang = _robot->joint(j)->q;
+                double jmax = _robot->joint(j)->ulimit;
+                double jmin = _robot->joint(j)->llimit;
+                double e = deg2rad(1);
+                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+                } else if ( eps_eq(jang, jmax,e) ) {
+                    jang = jmax - e;
+                } else if ( eps_eq(jang, jmin,e) ) {
+                    jang = jmin + e;
+                }
+                double r;
+                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+                    r = DBL_MAX;
+                } else {
+                    r = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) /
+                            (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
+                    if (isnan(r)) r = 0;
+                }
+                if (( r - avoid_weight_gain(j) ) >= 0 ) {
+                    dq_weight_all_jlim(j) *= ( 1.0 / ( 1.0 + r) );
+                } else {
+                    dq_weight_all_jlim(j) *= 1.0;
+                }
+                avoid_weight_gain(j) = r;
+            }
 
             const double wn_const = 1e-6;
             const double auto_lambda_gain = 0.01;
-            hrp::dmatrix Wn = (static_cast<double>(err_all.transpose() * weight_all.asDiagonal() * err_all) * auto_lambda_gain + wn_const) * hrp::dmatrix::Identity(ALL_DOF, ALL_DOF);
-            hrp::dmatrix H = J_all.transpose() * weight_all.asDiagonal() * J_all + Wn;
-            hrp::dvector g = J_all.transpose() * weight_all.asDiagonal() * err_all;
+            hrp::dmatrix Wn = (static_cast<double>(err_all.transpose() * constraint_weight_all.asDiagonal() * err_all) * auto_lambda_gain + wn_const) * hrp::dmatrix::Identity(ALL_DOF, ALL_DOF);
+            hrp::dvector dq_weight_all_inv = dq_weight_all_jlim.array().inverse();//重み小でdq小にするため
+            Wn = dq_weight_all_inv.asDiagonal() * Wn;
+            hrp::dmatrix H = J_all.transpose() * constraint_weight_all.asDiagonal() * J_all + Wn;
+            hrp::dvector g = J_all.transpose() * constraint_weight_all.asDiagonal() * err_all;
 
             dq_all = H.inverse() * g;
 
@@ -170,15 +209,15 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
                 dbg(g.transpose());
                 dbg(err_all.transpose());
                 dbg(dq_all.transpose());
-                dbg(weight_all.transpose());
+                dbg(constraint_weight_all.transpose());
+                dbg(dq_weight_all_inv.transpose());
                 std::cout<<"q_ans_all\n";
                 for(int i=0;i<_robot->numJoints();i++)std::cerr<<_robot->joint(i)->q<<" ";
                 std::cout<<std::endl;
             }
 
-            const double LAMBDA = 1.0;
             // check validity
-            hrp::Matrix33 eWx = hrp::hat(dq_all.segment(J_DOF+3,3) * LAMBDA).exp();
+            hrp::Matrix33 eWx = hrp::hat(dq_all.segment(J_DOF+3,3)).exp();
             hrp::Matrix33 R_base_ans = eWx * _robot->rootLink()->R;
 
             if(!R_base_ans.isUnitary()){ std::cerr <<"ERROR R_base_ans is not Unitary" << std::endl; return; }
@@ -186,10 +225,10 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
 
             //関節角+ベース位置姿勢更新
             for(int i=0;i<J_DOF;i++){
-                _robot->joint(i)->q += dq_all(i) * LAMBDA/* * m_dt*/;
+                _robot->joint(i)->q += dq_all(i);
                 LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit);
             }
-            _robot->rootLink()->p += dq_all.segment(J_DOF,3) * LAMBDA;//vel計算する時からm_dt使ってないから使わなくていい
+            _robot->rootLink()->p += dq_all.segment(J_DOF,3);//vel計算する時からm_dt使ってないから使わなくていい
 
             Eigen::Quaternion<double> quat(R_base_ans);
             quat.normalize();
@@ -232,61 +271,61 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
         }
 
     protected:
-        bool calcJacobianInverseNullspace_copy(hrp::BodyPtr _robot, hrp::dmatrix &_J, hrp::dmatrix &_Jinv) {
-            hrp::dmatrix w = hrp::dmatrix::Identity(ALL_DOF,ALL_DOF);
-            std::vector<double> avoid_weight_gain;
-            avoid_weight_gain.resize(_robot->numJoints());
-            for(int j=0; j < J_DOF; j++){
-                avoid_weight_gain[j] = 100000000000000000000.0;
-            }
-            bool use_inside_joint_weight_retrieval = true;
-
-            for ( int j = 0; j < _robot->numJoints() ; j++ ) {
-                double jang = _robot->joint(j)->q;
-                double jmax = _robot->joint(j)->ulimit;
-                double jmin = _robot->joint(j)->llimit;
-                double e = deg2rad(1);
-                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
-                } else if ( eps_eq(jang, jmax,e) ) {
-                    jang = jmax - e;
-                } else if ( eps_eq(jang, jmin,e) ) {
-                    jang = jmin + e;
-                }
-                double r;
-                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
-                    r = DBL_MAX;
-                } else {
-                    r = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) /
-                            (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
-                    if (isnan(r)) r = 0;
-                }
-                if (( r - avoid_weight_gain[j] ) >= 0 ) {
-                    w(j, j) = joint_weight[j] * ( 1.0 / ( 1.0 + r) );
-                } else {
-                    if (use_inside_joint_weight_retrieval)
-                        w(j, j) = joint_weight[j] * 1.0;
-                    else
-                        w(j, j) = joint_weight[j] * ( 1.0 / ( 1.0 + r) );
-                }
-                avoid_weight_gain[j] = r;
-            }
-            for ( int j = _robot->numJoints(); j < ALL_DOF ; j++ ) {
-                w(j, j) = joint_weight[j];
-            }
-
-            const double manipulability_limit = 0.0001;
-            const double sr_gain = 1.0;
-            //      const double manipulability_gain = 0.001;
-            const double manipulability_gain = 1;
-
-            double manipulability = sqrt((_J*_J.transpose()).determinant());
-            double k = 0;
-            if ( manipulability < manipulability_limit ) {
-                k = manipulability_gain * pow((1 - ( manipulability / manipulability_limit )), 2);
-            }
-            hrp::calcSRInverse(_J, _Jinv, sr_gain * k, w);
-            return true;
-        }
+//        bool calcJacobianInverseNullspace_copy(hrp::BodyPtr _robot, hrp::dmatrix &_J, hrp::dmatrix &_Jinv) {
+//            hrp::dmatrix w = hrp::dmatrix::Identity(ALL_DOF,ALL_DOF);
+//            std::vector<double> avoid_weight_gain;
+//            avoid_weight_gain.resize(_robot->numJoints());
+//            for(int j=0; j < J_DOF; j++){
+//                avoid_weight_gain[j] = 100000000000000000000.0;
+//            }
+//            bool use_inside_joint_weight_retrieval = true;
+//
+//            for ( int j = 0; j < _robot->numJoints() ; j++ ) {
+//                double jang = _robot->joint(j)->q;
+//                double jmax = _robot->joint(j)->ulimit;
+//                double jmin = _robot->joint(j)->llimit;
+//                double e = deg2rad(1);
+//                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+//                } else if ( eps_eq(jang, jmax,e) ) {
+//                    jang = jmax - e;
+//                } else if ( eps_eq(jang, jmin,e) ) {
+//                    jang = jmin + e;
+//                }
+//                double r;
+//                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+//                    r = DBL_MAX;
+//                } else {
+//                    r = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) /
+//                            (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
+//                    if (isnan(r)) r = 0;
+//                }
+//                if (( r - avoid_weight_gain[j] ) >= 0 ) {
+//                    w(j, j) = dq_weight_all[j] * ( 1.0 / ( 1.0 + r) );
+//                } else {
+//                    if (use_inside_dq_weight_all_retrieval)
+//                        w(j, j) = dq_weight_all[j] * 1.0;
+//                    else
+//                        w(j, j) = dq_weight_all[j] * ( 1.0 / ( 1.0 + r) );
+//                }
+//                avoid_weight_gain[j] = r;
+//            }
+//            for ( int j = _robot->numJoints(); j < ALL_DOF ; j++ ) {
+//                w(j, j) = dq_weight_all[j];
+//            }
+//
+//            const double manipulability_limit = 0.0001;
+//            const double sr_gain = 1.0;
+//            //      const double manipulability_gain = 0.001;
+//            const double manipulability_gain = 1;
+//
+//            double manipulability = sqrt((_J*_J.transpose()).determinant());
+//            double k = 0;
+//            if ( manipulability < manipulability_limit ) {
+//                k = manipulability_gain * pow((1 - ( manipulability / manipulability_limit )), 2);
+//            }
+//            hrp::calcSRInverse(_J, _Jinv, sr_gain * k, w);
+//            return true;
+//        }
 
         hrp::Vector3 omegaFromRotEx(const hrp::Matrix33& r) {//copy from JointPathEx.cpp
             using ::std::numeric_limits;
@@ -403,7 +442,7 @@ class FullbodyInverseKinematicsSolverMT : public FullbodyInverseKinematicsSolver
                     _robot_copy->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * _robot_copy->rootLink()->R) / m_dt;
                     for(int i=0;i<J_DOF;i++){ _robot_copy->joint(i)->dq = (_robot_copy->joint(i)->q - q_old(i)) / m_dt; }
                     _robot_copy->calcForwardKinematics(true,false);
-                    _robot_copy->calcTotalMomentum(cur_momentum_P, cur_momentum_L);
+//                    _robot_copy->calcTotalMomentum(cur_momentum_P, cur_momentum_L);
                     cur_ik_loop++;
                     if(cur_ik_loop > 10000){std::cerr<<"[FullbodyInverseKinematicsSolverMT] thread time out"<<std::endl;break;}//異常終了でスレッドが残ってしまった時にタイムアウト終了
                 }
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 636e29026d5..8b21bb6da8e 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -194,7 +194,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     invdyn_zmp_filters.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
     final_ref_zmp_filter.setParameter(5, 1/m_dt, Q_BUTTERWORTH);
 
-    avg_q_vel = 1.5;
+    avg_q_vel = 2;
 
     std::cerr << "[" << m_profile.instance_name << "] onInitialize() OK" << std::endl;
     return RTC::RTC_OK;
@@ -508,11 +508,11 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
             }
         }
     }
-    q_ip->clear();
+//    q_ip->clear();//clearは2回目以降しないほうがいい
     init_sync_state.resize(ROBOT_ALL_DOF);
-    for(int i=0;i<robot_in->numJoints();i++){ init_sync_state(i) = robot_in->joint(i)->q; }
-    init_sync_state.bottomRows(6).topRows(3) = robot_in->rootLink()->p;
-    init_sync_state.bottomRows(6).bottomRows(3) = hrp::rpyFromRot(robot_in->rootLink()->R);
+    init_sync_state.head(robot_in->numJoints()) = hrp::getQAll(robot_in);
+    init_sync_state.tail(6).head(3) = robot_in->rootLink()->p;
+    init_sync_state.tail(6).tail(3) = hrp::rpyFromRot(robot_in->rootLink()->R);
     q_ip->set(init_sync_state.data());
     hsp->initializeRequest(fik_in, robot_in);
 }
@@ -631,27 +631,35 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.constraint_weight << 0,0,0,0,0.1,0.1;
         tmp.rot_precision = deg2rad(1);
         ikc_list.push_back(tmp);
-    }{
+    }
+    {
         IKConstraint tmp;
         tmp.target_link_name = "COM";
         tmp.localPos = hrp::Vector3::Zero();
         tmp.localR = hrp::Matrix33::Identity();
         tmp.targetPos = com_ref.p;// COM height will not be constraint
         tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
-        tmp.constraint_weight << 10,10,1,0,0,0;
-        tmp.rot_precision = 100;//angular momentum precision
+        if(fik_in->cur_momentum_around_COM.norm() > 1){
+            tmp.constraint_weight << 10,10,1,1e-5,1e-5,1e-10;
+            tmp.rot_precision = 100;//angular momentum precision
+        }else{
+            tmp.constraint_weight << 10,10,1,0,0,0;
+        }
         ikc_list.push_back(tmp);
     }
 
-    //  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;
-    //  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
-    //  fik_in->optional_weight_vector(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
-    //  fik_in->optional_weight_vector.tail(6).fill(0.01);
+    fik_in->dq_weight_all(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;
+    fik_in->dq_weight_all(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
+    fik_in->dq_weight_all(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
     if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇内側の干渉回避
     if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(40);
     if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->llimit = deg2rad(-140);//肩外側の干渉回避
     if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->ulimit = deg2rad(140);
 
+    for(int i=0;i<robot_in->numJoints();i++){
+        LIMIT_MINMAX(robot_in->joint(i)->q, robot_in->joint(i)->llimit, robot_in->joint(i)->ulimit);
+    }
+
     fik_in->q_ref = init_sync_state;
     fik_in->q_ref_pullback_gain.fill(0.001);
     fik_in->q_ref_pullback_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
@@ -659,7 +667,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     struct timespec startT, endT;
     const int IK_MAX_LOOP = 2;
     clock_gettime(CLOCK_REALTIME, &startT);
-    int loop_result = fik_in->solveFullbodyIKLoop(m_robot, ikc_list, IK_MAX_LOOP);
+    int loop_result = fik_in->solveFullbodyIKLoop(robot_in, ikc_list, IK_MAX_LOOP);
     if(loop%100==0){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveIK"<<loop_result<<"loop" << std::endl;}
 }
 #else
@@ -725,14 +733,22 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
     //  const double avg_q_vel = 0.5;
     //  const double avg_q_vel = 1.0;
 //    const double avg_q_vel = 1.5;
+
+    static hrp::dvector tmp_v = hrp::dvector::Zero(ROBOT_ALL_DOF);
+//    if (!q_ip->isEmpty() ){  q_ip->get(tmp_x, tmp_v, false);}
     for(int i=0;i<robot_in->numJoints();i++){
         double tmp_time = fabs(robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
+        tmp_time += fabs(tmp_v(i))/8; //加速度8
         if(tmp_time > goal_time){ goal_time = tmp_time; }
     }
+    dbg(tmp_v.transpose());
     q_ip->setGoal(goal_state.data(), goal_time + min_goal_time_offset, true);
     hrp::dvector ans_state(ROBOT_ALL_DOF);
     double tmp[ROBOT_ALL_DOF];
-    if (!q_ip->isEmpty() ){  q_ip->get(tmp, true);}
+//    if (!q_ip->isEmpty() ){  q_ip->get(tmp, true);}
+    double tmpv[ROBOT_ALL_DOF];
+    if (!q_ip->isEmpty() ){  q_ip->get(tmp, tmpv, true);}
+    tmp_v = Eigen::Map<hrp::dvector>(tmpv, ROBOT_ALL_DOF);
     ans_state = Eigen::Map<hrp::dvector>(tmp, ROBOT_ALL_DOF);
     for(int i=0;i<robot_out->numJoints();i++){ robot_out->joint(i)->q = ans_state(i); }
     robot_out->rootLink()->p = ans_state.bottomRows(6).topRows(3);
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 367234f8361..72ce63d64f2 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -1,5 +1,5 @@
-#ifndef HUMANMASTERSLAVE_H
-#define HUMANMASTERSLAVE_H
+#ifndef WBMS_CORE_H
+#define WBMS_CORE_H
 #include <rtm/idl/BasicDataType.hh>
 #include <rtm/idl/ExtendedDataTypes.hh>
 #include <rtm/Manager.h>
@@ -914,4 +914,4 @@ class WBMSCore : UTIL_CONST {
         }
 };
 
-#endif // HUMANMASTERSLAVE_H
+#endif // WBMS_CORE_H

From effb4466670b2037a9eb932f3df564eed7df8885 Mon Sep 17 00:00:00 2001
From: jaxonred <jaxonred@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 8 Sep 2017 23:07:15 +0900
Subject: [PATCH 129/217] tune param in real robot

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 8b21bb6da8e..b1016859652 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -194,7 +194,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     invdyn_zmp_filters.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
     final_ref_zmp_filter.setParameter(5, 1/m_dt, Q_BUTTERWORTH);
 
-    avg_q_vel = 2;
+    avg_q_vel = 4;
 
     std::cerr << "[" << m_profile.instance_name << "] onInitialize() OK" << std::endl;
     return RTC::RTC_OK;
@@ -639,7 +639,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.localR = hrp::Matrix33::Identity();
         tmp.targetPos = com_ref.p;// COM height will not be constraint
         tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
-        if(fik_in->cur_momentum_around_COM.norm() > 1){
+        if(fik_in->cur_momentum_around_COM.norm() > 1e9){
             tmp.constraint_weight << 10,10,1,1e-5,1e-5,1e-10;
             tmp.rot_precision = 100;//angular momentum precision
         }else{
@@ -729,7 +729,7 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
     goal_state.bottomRows(6).bottomRows(3) = hrp::rpyFromRot(robot_in->rootLink()->R);
 
     double goal_time = 0.0;
-    const double min_goal_time_offset = 0.1;
+    const double min_goal_time_offset = 0.01;
     //  const double avg_q_vel = 0.5;
     //  const double avg_q_vel = 1.0;
 //    const double avg_q_vel = 1.5;
@@ -738,10 +738,9 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
 //    if (!q_ip->isEmpty() ){  q_ip->get(tmp_x, tmp_v, false);}
     for(int i=0;i<robot_in->numJoints();i++){
         double tmp_time = fabs(robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
-        tmp_time += fabs(tmp_v(i))/8; //加速度8
+        tmp_time += fabs(tmp_v(i))/16; //加速度8
         if(tmp_time > goal_time){ goal_time = tmp_time; }
     }
-    dbg(tmp_v.transpose());
     q_ip->setGoal(goal_state.data(), goal_time + min_goal_time_offset, true);
     hrp::dvector ans_state(ROBOT_ALL_DOF);
     double tmp[ROBOT_ALL_DOF];

From 9213aa995d790f7a1e2ecd0c936d10eb69662dcb Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 11 Sep 2017 21:00:05 +0900
Subject: [PATCH 130/217] tune params

---
 rtc/AutoBalancer/AutoBalancer.cpp             |  6 ++++--
 .../FullbodyInverseKinematicsSolver.h         |  2 +-
 .../WholeBodyMasterSlave.cpp                  | 20 ++++++++++---------
 rtc/WholeBodyMasterSlave/wbms_core.h          |  3 +--
 4 files changed, 17 insertions(+), 14 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index eb6698f182d..e9de730b952 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1134,7 +1134,9 @@ void AutoBalancer::solveFullbodyIK ()
         tmp.targetPos = ref_cog;// COM height will not be constraint
         tmp.targetRpy = hrp::Vector3(0, 0, 0);//reference angular momentum
         //  tmp.targetRpy = hrp::Vector3(0, 10, 0);//reference angular momentum
-        tmp.constraint_weight << 3,3,1,0.01,0.01,0.01;
+        tmp.constraint_weight << 3,3,1,1e-6,1e-6,1e-6;
+//        tmp.constraint_weight << 3,3,1,0,0,0;
+        tmp.rot_precision = 1e-1;//angular momentum precision
         ik_tgt_list.push_back(tmp);
     }
 
@@ -1153,7 +1155,7 @@ void AutoBalancer::solveFullbodyIK ()
   m_robot->rootLink()->R = base_R_old;
 
   int loop_result = 0;
-  const int IK_MAX_LOOP = 2;//2回は必須な気がする・・・
+  const int IK_MAX_LOOP = 1;
   loop_result = fik->solveFullbodyIKLoop(m_robot, ik_tgt_list, IK_MAX_LOOP);
 
   q_old = hrp::getQAll(m_robot);
diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 0e2ca791a83..1430d796ae2 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -190,7 +190,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
             }
 
             const double wn_const = 1e-6;
-            const double auto_lambda_gain = 0.01;
+            const double auto_lambda_gain = 1;
             hrp::dmatrix Wn = (static_cast<double>(err_all.transpose() * constraint_weight_all.asDiagonal() * err_all) * auto_lambda_gain + wn_const) * hrp::dmatrix::Identity(ALL_DOF, ALL_DOF);
             hrp::dvector dq_weight_all_inv = dq_weight_all_jlim.array().inverse();//重み小でdq小にするため
             Wn = dq_weight_all_inv.asDiagonal() * Wn;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index b1016859652..eb312c01e9c 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -639,12 +639,13 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.localR = hrp::Matrix33::Identity();
         tmp.targetPos = com_ref.p;// COM height will not be constraint
         tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
-        if(fik_in->cur_momentum_around_COM.norm() > 1e9){
-            tmp.constraint_weight << 10,10,1,1e-5,1e-5,1e-10;
-            tmp.rot_precision = 100;//angular momentum precision
-        }else{
-            tmp.constraint_weight << 10,10,1,0,0,0;
-        }
+        tmp.constraint_weight << 10,10,1,1e-6,1e-6,1e-6;
+//        if(fik_in->cur_momentum_around_COM.norm() > 1e9){
+//            tmp.constraint_weight << 10,10,1,1e-5,1e-5,1e-10;
+//            tmp.rot_precision = 100;//angular momentum precision
+//        }else{
+//            tmp.constraint_weight << 10,10,1,0,0,0;
+//        }
         ikc_list.push_back(tmp);
     }
 
@@ -661,8 +662,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     }
 
     fik_in->q_ref = init_sync_state;
-    fik_in->q_ref_pullback_gain.fill(0.001);
-    fik_in->q_ref_pullback_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
+//    fik_in->q_ref_pullback_gain.fill(0.001);
+//    fik_in->q_ref_pullback_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
 
     struct timespec startT, endT;
     const int IK_MAX_LOOP = 2;
@@ -729,7 +730,8 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
     goal_state.bottomRows(6).bottomRows(3) = hrp::rpyFromRot(robot_in->rootLink()->R);
 
     double goal_time = 0.0;
-    const double min_goal_time_offset = 0.01;
+//    const double min_goal_time_offset = 0.01;
+    const double min_goal_time_offset = 0.05;
     //  const double avg_q_vel = 0.5;
     //  const double avg_q_vel = 1.0;
 //    const double avg_q_vel = 1.5;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 72ce63d64f2..d979da57a24 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -389,7 +389,7 @@ class WBMSCore : UTIL_CONST {
         }
         void convertRelHumanPoseToRelRobotPose(const HumanPose& in, HumanPose& out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
             //      out = in;//ダメゼッタイ
-            for(int i=0, l[5]={com,rf,lf,rh,lh}; i<5; i++){
+            for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
                 out.tgt[l[i]].abs.p   =  h2r_ratio * (in.tgt[l[i]].abs.p - in.tgt[l[i]].offs.p)   + out.tgt[l[i]].offs.p;
                 out.tgt[l[i]].abs.rpy =  hrp::rpyFromRot( hrp::rotFromRpy(in.tgt[l[i]].abs.rpy) * hrp::rotFromRpy(in.tgt[l[i]].offs.rpy).transpose() * hrp::rotFromRpy(out.tgt[l[i]].offs.rpy) );
             }
@@ -397,7 +397,6 @@ class WBMSCore : UTIL_CONST {
                 out.tgt[l[i]].w = in.tgt[l[i]].w;
                 out.tgt[l[i]].go_contact = in.tgt[l[i]].go_contact;
             }
-            out.tgt[head].abs = in.tgt[head].abs;
             out.tgt[zmp].abs = in.tgt[zmp].abs;//最近使わない
         }
         void judgeFootLandOnCommandByFootForce(HumanPose& in){

From a81390a9b648035ec08d306a9253a5d1d9165e97 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 12 Sep 2017 17:11:22 +0900
Subject: [PATCH 131/217] set horizontal vel limit for floor

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp |  2 +-
 rtc/WholeBodyMasterSlave/wbms_core.h              | 10 +++++++++-
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index eb312c01e9c..78075f4b433 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -194,7 +194,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     invdyn_zmp_filters.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
     final_ref_zmp_filter.setParameter(5, 1/m_dt, Q_BUTTERWORTH);
 
-    avg_q_vel = 4;
+    avg_q_vel = 3;
 
     std::cerr << "[" << m_profile.instance_name << "] onInitialize() OK" << std::endl;
     return RTC::RTC_OK;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index d979da57a24..93191c86d3f 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -227,7 +227,7 @@ class WBMSCore : UTIL_CONST {
             WBMSparam.is_doctor = true;
             WBMSparam.set_com_height_fix = false;
             WBMSparam.set_com_height_fix_val = 0.02;
-            WBMSparam.swing_foot_height_offset = 0.01;
+            WBMSparam.swing_foot_height_offset = 0.02;
             WBMSparam.swing_foot_max_height = 0.5;
             WBMSparam.upper_body_rmc_ratio = 0.0;
             WBMSparam.use_rh = WBMSparam.use_lh = true;
@@ -320,6 +320,14 @@ class WBMSCore : UTIL_CONST {
             overwriteFootZFromFootLandOnCommand (rp_ref_out);
             setFootRotHorizontalIfGoLanding     (rp_ref_out);
 
+            for(int i=0, l[2]={rf,lf}; i<2; i++){
+                double fheight = rp_ref_out_old.tgt[l[i]].abs.p(Z) - rp_ref_out_old.tgt[l[i]].offs.p(Z);
+                double horizontal_max_vel = 0 + fheight * 20;
+                for(int j=0;j<XY;j++)LIMIT_MINMAX( rp_ref_out.tgt[l[i]].abs.p(j), rp_ref_out_old.tgt[l[i]].abs.p(j)-horizontal_max_vel*DT, rp_ref_out_old.tgt[l[i]].abs.p(j)+horizontal_max_vel*DT);
+                double vertical_max_vel = 0 + fheight * 10;
+                LIMIT_MIN( rp_ref_out.tgt[l[i]].abs.p(Z), rp_ref_out_old.tgt[l[i]].abs.p(Z)-vertical_max_vel*DT);
+            }
+
             //      limitManipulability                 (rp_ref_out);
 
             applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから

From dda96ee347416d4f94a82948263ae2fae0c61980 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 12 Sep 2017 18:55:58 +0900
Subject: [PATCH 132/217] improve ref pull back

---
 .../FullbodyInverseKinematicsSolver.h             | 15 ++++++++++-----
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp |  4 +++-
 2 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 1430d796ae2..1069e87e8b8 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -48,7 +48,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
         const int WS_DOF, BASE_DOF, J_DOF, ALL_DOF;
     public:
         hrp::dvector dq_weight_all;
-        hrp::dvector q_ref, q_ref_pullback_gain;
+        hrp::dvector q_ref, q_ref_pullback_gain, dq_ref_pullback;
         hrp::Vector3 cur_momentum_around_COM;
         FullbodyInverseKinematicsSolver (hrp::BodyPtr _robot, const std::string& _print_str, const double _dt)
         : SimpleFullbodyInverseKinematicsSolver(_robot,_print_str, _dt),
@@ -57,7 +57,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
           J_DOF(_robot->numJoints()),
           ALL_DOF(J_DOF+BASE_DOF) {
             dq_weight_all = hrp::dvector::Ones(ALL_DOF);
-            dq_all = q_ref = q_ref_pullback_gain = hrp::dvector::Zero(ALL_DOF);
+            dq_all = q_ref = q_ref_pullback_gain = dq_ref_pullback = hrp::dvector::Zero(ALL_DOF);
             cur_momentum_around_COM = hrp::Vector3::Zero();
             avoid_weight_gain = hrp::dvector::Constant(ALL_DOF, 1e12);
         };
@@ -112,9 +112,14 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
             err_all = hrp::dvector::Zero(WS_DOF*_ikc_list.size());
             constraint_weight_all = hrp::dvector::Ones(WS_DOF*_ikc_list.size());
             //リファレンスに微少量戻す
-            for(int i=0;i<J_DOF;i++){ _robot->joint(i)->q = _robot->joint(i)->q * ( 1 - q_ref_pullback_gain(i)) + q_ref(i) * q_ref_pullback_gain(i); }
-            _robot->rootLink()->p = _robot->rootLink()->p.cwiseProduct( hrp::Vector3::Ones() - q_ref_pullback_gain.segment(J_DOF, 3) ) + q_ref.segment(J_DOF, 3).cwiseProduct( q_ref_pullback_gain.segment(J_DOF, 3) );
-            _robot->rootLink()->R = hrp::rotFromRpy( hrp::rpyFromRot(_robot->rootLink()->R).cwiseProduct(hrp::Vector3::Ones()-q_ref_pullback_gain.segment(J_DOF+3,3)) + q_ref.segment(J_DOF+3,3).cwiseProduct(q_ref_pullback_gain.segment(J_DOF+3,3)));
+//            for(int i=0;i<J_DOF;i++){ _robot->joint(i)->q = _robot->joint(i)->q * ( 1 - q_ref_pullback_gain(i)) + q_ref(i) * q_ref_pullback_gain(i); }
+            for(int i=0;i<J_DOF;i++){
+                double diff = q_ref(i) - _robot->joint(i)->q;
+                LIMIT_MINMAX(diff, -dq_ref_pullback(i), dq_ref_pullback(i));
+                _robot->joint(i)->q += diff;
+            }
+//            _robot->rootLink()->p = _robot->rootLink()->p.cwiseProduct( hrp::Vector3::Ones() - q_ref_pullback_gain.segment(J_DOF, 3) ) + q_ref.segment(J_DOF, 3).cwiseProduct( q_ref_pullback_gain.segment(J_DOF, 3) );
+//            _robot->rootLink()->R = hrp::rotFromRpy( hrp::rpyFromRot(_robot->rootLink()->R).cwiseProduct(hrp::Vector3::Ones()-q_ref_pullback_gain.segment(J_DOF+3,3)) + q_ref.segment(J_DOF+3,3).cwiseProduct(q_ref_pullback_gain.segment(J_DOF+3,3)));
             for(int i=0;i<J_DOF;i++){ LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit); }
             _robot->calcForwardKinematics();
             //ヤコビアンと各ベクトル生成
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 78075f4b433..29ee1997d54 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -640,6 +640,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.targetPos = com_ref.p;// COM height will not be constraint
         tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
         tmp.constraint_weight << 10,10,1,1e-6,1e-6,1e-6;
+//        tmp.constraint_weight << 10,10,1,0,0,0;
 //        if(fik_in->cur_momentum_around_COM.norm() > 1e9){
 //            tmp.constraint_weight << 10,10,1,1e-5,1e-5,1e-10;
 //            tmp.rot_precision = 100;//angular momentum precision
@@ -662,7 +663,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     }
 
     fik_in->q_ref = init_sync_state;
-//    fik_in->q_ref_pullback_gain.fill(0.001);
+//    fik_in->q_ref_pullback_gain.fill(0.005);
+    fik_in->dq_ref_pullback.fill(deg2rad(0.1));
 //    fik_in->q_ref_pullback_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
 
     struct timespec startT, endT;

From 3f67b89feeef8a28b1132686ac140d9ad3de0bb4 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 12 Sep 2017 20:41:00 +0900
Subject: [PATCH 133/217] tune

---
 rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h | 2 +-
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp  | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 1069e87e8b8..57ef3485ee9 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -112,7 +112,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
             err_all = hrp::dvector::Zero(WS_DOF*_ikc_list.size());
             constraint_weight_all = hrp::dvector::Ones(WS_DOF*_ikc_list.size());
             //リファレンスに微少量戻す
-//            for(int i=0;i<J_DOF;i++){ _robot->joint(i)->q = _robot->joint(i)->q * ( 1 - q_ref_pullback_gain(i)) + q_ref(i) * q_ref_pullback_gain(i); }
+            for(int i=0;i<J_DOF;i++){ _robot->joint(i)->q = _robot->joint(i)->q * ( 1 - q_ref_pullback_gain(i)) + q_ref(i) * q_ref_pullback_gain(i); }
             for(int i=0;i<J_DOF;i++){
                 double diff = q_ref(i) - _robot->joint(i)->q;
                 LIMIT_MINMAX(diff, -dq_ref_pullback(i), dq_ref_pullback(i));
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 29ee1997d54..17ad708d98d 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -663,8 +663,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     }
 
     fik_in->q_ref = init_sync_state;
-//    fik_in->q_ref_pullback_gain.fill(0.005);
-    fik_in->dq_ref_pullback.fill(deg2rad(0.1));
+    fik_in->q_ref_pullback_gain.fill(0.005);
+//    fik_in->dq_ref_pullback.fill(deg2rad(0.1));
 //    fik_in->q_ref_pullback_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
 
     struct timespec startT, endT;

From 6d47801fac47f6f18236b585a3c3f3d9c68004db Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 12 Sep 2017 22:39:36 +0900
Subject: [PATCH 134/217] tune

---
 .../FullbodyInverseKinematicsSolver.h           |  7 ++++---
 .../WholeBodyMasterSlave.cpp                    | 17 ++++++++++-------
 2 files changed, 14 insertions(+), 10 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 57ef3485ee9..651d02d7acf 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -112,10 +112,11 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
             err_all = hrp::dvector::Zero(WS_DOF*_ikc_list.size());
             constraint_weight_all = hrp::dvector::Ones(WS_DOF*_ikc_list.size());
             //リファレンスに微少量戻す
-            for(int i=0;i<J_DOF;i++){ _robot->joint(i)->q = _robot->joint(i)->q * ( 1 - q_ref_pullback_gain(i)) + q_ref(i) * q_ref_pullback_gain(i); }
+//            for(int i=0;i<J_DOF;i++){ _robot->joint(i)->q = _robot->joint(i)->q * ( 1 - q_ref_pullback_gain(i)) + q_ref(i) * q_ref_pullback_gain(i); }
             for(int i=0;i<J_DOF;i++){
                 double diff = q_ref(i) - _robot->joint(i)->q;
-                LIMIT_MINMAX(diff, -dq_ref_pullback(i), dq_ref_pullback(i));
+                diff *= q_ref_pullback_gain(i);
+//                LIMIT_MINMAX(diff, -dq_ref_pullback(i), dq_ref_pullback(i));
                 _robot->joint(i)->q += diff;
             }
 //            _robot->rootLink()->p = _robot->rootLink()->p.cwiseProduct( hrp::Vector3::Ones() - q_ref_pullback_gain.segment(J_DOF, 3) ) + q_ref.segment(J_DOF, 3).cwiseProduct( q_ref_pullback_gain.segment(J_DOF, 3) );
@@ -188,7 +189,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
                 }
                 if (( r - avoid_weight_gain(j) ) >= 0 ) {
                     dq_weight_all_jlim(j) *= ( 1.0 / ( 1.0 + r) );
-                } else {
+                } else {//リミットから離れるときは解放
                     dq_weight_all_jlim(j) *= 1.0;
                 }
                 avoid_weight_gain(j) = r;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 17ad708d98d..db7a6c918d1 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -194,7 +194,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     invdyn_zmp_filters.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
     final_ref_zmp_filter.setParameter(5, 1/m_dt, Q_BUTTERWORTH);
 
-    avg_q_vel = 3;
+    avg_q_vel = 4;
 
     std::cerr << "[" << m_profile.instance_name << "] onInitialize() OK" << std::endl;
     return RTC::RTC_OK;
@@ -495,8 +495,6 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
         body_list[i]->rootLink()->p = basePos_heightChecked;
         body_list[i]->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
         for ( int j = 0; j < body_list[i]->numJoints(); j++ ){ body_list[i]->joint(j)->q = m_qRef.data[j]; }
-//        if( body_list[i]->link("RARM_JOINT2") != NULL) body_list[i]->link("RARM_JOINT2")->ulimit = -40 * D2R;//脇の干渉回避のため
-//        if( body_list[i]->link("LARM_JOINT2") != NULL) body_list[i]->link("LARM_JOINT2")->llimit = 40 * D2R;
         body_list[i]->calcForwardKinematics();
     }
     for(int i=0;i<fik_list.size();i++){//初期姿勢でBodyをFK
@@ -663,8 +661,12 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     }
 
     fik_in->q_ref = init_sync_state;
-    fik_in->q_ref_pullback_gain.fill(0.005);
-//    fik_in->dq_ref_pullback.fill(deg2rad(0.1));
+
+    fik_in->q_ref_pullback_gain.fill(0.001);
+//    fik_in->dq_ref_pullback.fill(deg2rad(0.01));
+    fik_in->q_ref_pullback_gain.segment(6+6+3+2, 8*2).fill(0.01);//腕だけ
+//    fik_in->dq_ref_pullback.segment(6+6+3+2, 8*2).fill(deg2rad(0.1));//腕だけ
+
 //    fik_in->q_ref_pullback_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
 
     struct timespec startT, endT;
@@ -733,7 +735,7 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
 
     double goal_time = 0.0;
 //    const double min_goal_time_offset = 0.01;
-    const double min_goal_time_offset = 0.05;
+    const double min_goal_time_offset = 0.1;
     //  const double avg_q_vel = 0.5;
     //  const double avg_q_vel = 1.0;
 //    const double avg_q_vel = 1.5;
@@ -742,7 +744,8 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
 //    if (!q_ip->isEmpty() ){  q_ip->get(tmp_x, tmp_v, false);}
     for(int i=0;i<robot_in->numJoints();i++){
         double tmp_time = fabs(robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
-        tmp_time += fabs(tmp_v(i))/16; //加速度8
+        LIMIT_MIN(tmp_time, fabs(tmp_v(i))/16);
+//        tmp_time += fabs(tmp_v(i))/16; //加速度8
         if(tmp_time > goal_time){ goal_time = tmp_time; }
     }
     q_ip->setGoal(goal_state.data(), goal_time + min_goal_time_offset, true);

From 8761e8d93a6b9e2a0078f98dbacb3158d2ab9cf0 Mon Sep 17 00:00:00 2001
From: ishiguroJSK <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 29 Sep 2017 18:03:36 -0700
Subject: [PATCH 135/217] start capsule collision

---
 .../WholeBodyMasterSlave.cpp                  |  33 ++-
 .../WholeBodyMasterSlave.h                    |   1 +
 rtc/WholeBodyMasterSlave/wbms_core.h          | 223 +++++++++++++++++-
 3 files changed, 250 insertions(+), 7 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index db7a6c918d1..1beaf1d45d3 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -196,6 +196,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
 
     avg_q_vel = 4;
 
+    sccp = boost::shared_ptr<SphereCollisionChecker>(new SphereCollisionChecker(m_robot));
+
     std::cerr << "[" << m_profile.instance_name << "] onInitialize() OK" << std::endl;
     return RTC::RTC_OK;
 }
@@ -325,6 +327,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
             if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ calcWorldZMPFromInverseDynamics" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
+            sccp->checkCollision();
+
             processHOFFARBIBFilter(m_robot, m_robot_vsafe);
 
             const hrp::BodyPtr m_robot_for_out = m_robot_vsafe;
@@ -608,6 +612,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.targetPos = rh_ref.p;
         tmp.targetRpy = rh_ref.rpy;
         tmp.constraint_weight = hrp::dvector6::Constant(0.1);
+        tmp.constraint_weight << 1,1,1,0,0,0;
         tmp.pos_precision = 3e-3;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
@@ -630,6 +635,26 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.rot_precision = deg2rad(1);
         ikc_list.push_back(tmp);
     }
+    for(int i=0;i<sccp->collision_info_list.size();i++){
+        IKConstraint tmp;
+        tmp.localPos = sccp->collision_info_list[i].cp1_local;
+        tmp.target_link_name = m_robot->joint(sccp->collision_info_list[i].id1)->name;
+        tmp.targetPos = sccp->collision_info_list[i].cp0_wld + (sccp->collision_info_list[i].cp1_wld - sccp->collision_info_list[i].cp0_wld).normalized() * (sccp->collision_info_list[i].dist_safe + 1e-3);
+        tmp.constraint_weight << 1,1,1,0,0,0;
+        ikc_list.push_back(tmp);
+    }
+//    for(int i=0;i<sccp->collision_pair.size();i++){
+//        const hrp::Sphere& base = sccp->sphere_list[sccp->collision_pair[i].first];
+//        const hrp::Sphere& move = sccp->sphere_list[sccp->collision_pair[i].second];
+//        IKConstraint tmp;
+//        tmp.localPos = move.local_pos;
+//        tmp.target_link_name = m_robot->joint(sccp->collision_pair[i].second)->name;
+//        tmp.targetPos = base.cur_pos + (move.cur_pos - base.cur_pos).normalized() * (move.r + base.r + 1e-3);
+//        tmp.constraint_weight << 1,1,1,0,0,0;
+//        ikc_list.push_back(tmp);
+//    }
+
+
     {
         IKConstraint tmp;
         tmp.target_link_name = "COM";
@@ -651,10 +676,10 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     fik_in->dq_weight_all(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;
     fik_in->dq_weight_all(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
     fik_in->dq_weight_all(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
-    if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇内側の干渉回避
-    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(40);
-    if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->llimit = deg2rad(-140);//肩外側の干渉回避
-    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->ulimit = deg2rad(140);
+//    if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇内側の干渉回避
+//    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(40);
+//    if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->llimit = deg2rad(-140);//肩外側の干渉回避
+//    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->ulimit = deg2rad(140);
 
     for(int i=0;i<robot_in->numJoints();i++){
         LIMIT_MINMAX(robot_in->joint(i)->q, robot_in->joint(i)->llimit, robot_in->joint(i)->ulimit);
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index b0df4b02de1..522cb33552c 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -196,6 +196,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
     HumanPose raw_pose;
 
     boost::shared_ptr<WBMSCore> hsp;
+    boost::shared_ptr<SphereCollisionChecker> sccp;
 
     hrp::Vector3 torso_rot_rmc;
     ControlMode mode;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 93191c86d3f..376e8952636 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -17,6 +17,7 @@
 #include <opencv2/imgproc/types_c.h>
 #include <hrpUtil/Eigen4d.h>
 #include <numeric>
+#include <hrpCollision/DistFuncs.h>
 
 #if defined(__cplusplus)
 extern "C" {
@@ -119,6 +120,221 @@ class HumanPose : UTIL_CONST {
         void print() const { hp_printf(*this); }
 };
 
+namespace hrp{
+    class Sphere{
+        public:
+            double r;
+            hrp::Vector3 local_pos, cur_pos;
+
+            Sphere(const hrp::Vector3 p_in = hrp::Vector3::Zero(), const double r_in = 0){
+                local_pos = cur_pos = p_in;
+                r = r_in;
+            };
+    };
+}
+
+class Capsule{
+    public:
+        hrp::Vector3 p0, p1;
+        double r;
+        Capsule(const hrp::Vector3 _p0 = hrp::Vector3::Zero(), const hrp::Vector3 _p1 = hrp::Vector3::Zero(), const double _r = 0){
+            p0 = _p0; p1 = _p1; r = _r;
+        }
+};
+
+class CollisionInfo{
+    public:
+        int id0, id1;
+        hrp::Vector3 cp0_local, cp1_local, cp0_wld, cp1_wld;
+        double dist_safe, dist_cur;
+        CollisionInfo(const int _id0 = 0, const int _id1 = 0,
+                const hrp::Vector3 _cp0_local = hrp::Vector3::Zero(), const hrp::Vector3 _cp1_local = hrp::Vector3::Zero(),
+                const hrp::Vector3 _cp0_wld = hrp::Vector3::Zero(), const hrp::Vector3 _cp1_wld = hrp::Vector3::Zero(),
+                const double _dist_safe = 0, const double _dist_cur = 0){
+            id0 = _id0; id1 = _id1; cp0_local = _cp0_local; cp1_local = _cp1_local;  cp0_wld = _cp0_wld; cp1_wld = _cp1_wld; dist_safe = _dist_safe; dist_cur = _dist_cur;
+        }
+};
+
+inline double SegSegDist2(const Point& u0, const Point& u, const Point& v0, const Point& v, Point& cp0, Point& cp1){
+    Point    w = u0 - v0;
+    double    a = u|u;        // always >= 0
+    double    b = u|v;
+    double    c = v|v;        // always >= 0
+    double    d = u|w;
+    double    e = v|w;
+    double    D = a*c - b*b;       // always >= 0
+    double    sc, sN, sD = D;      // sc = sN / sD, default sD = D >= 0
+    double    tc, tN, tD = D;      // tc = tN / tD, default tD = D >= 0
+
+    // compute the line parameters of the two closest points
+#define EPS 1e-8
+    if (D < EPS) { // the lines are almost parallel
+        sN = 0.0;        // force using point P0 on segment S1
+        sD = 1.0;        // to prevent possible division by 0.0 later
+        tN = e;
+        tD = c;
+    }
+    else {                // get the closest points on the infinite lines
+        sN = (b*e - c*d);
+        tN = (a*e - b*d);
+        if (sN < 0.0) {       // sc < 0 => the s=0 edge is visible
+            sN = 0.0;
+            tN = e;
+            tD = c;
+        }
+        else if (sN > sD) {  // sc > 1 => the s=1 edge is visible
+            sN = sD;
+            tN = e + b;
+            tD = c;
+        }
+    }
+
+    if (tN < 0.0) {           // tc < 0 => the t=0 edge is visible
+        tN = 0.0;
+        // recompute sc for this edge
+        if (-d < 0.0)
+            sN = 0.0;
+        else if (-d > a)
+            sN = sD;
+        else {
+            sN = -d;
+            sD = a;
+        }
+    }
+    else if (tN > tD) {      // tc > 1 => the t=1 edge is visible
+        tN = tD;
+        // recompute sc for this edge
+        if ((-d + b) < 0.0)
+            sN = 0;
+        else if ((-d + b) > a)
+            sN = sD;
+        else {
+            sN = (-d + b);
+            sD = a;
+        }
+    }
+    // finally do the division to get sc and tc
+    sc = (fabsf(sN) < EPS ? 0.0f : sN / sD);
+    tc = (fabsf(tN) < EPS ? 0.0f : tN / tD);
+
+    cp0 = u0 + sc * u;
+    cp1 = v0 + tc * v;
+
+    // get the difference of the two closest points
+    Point dP = cp0 - cp1;
+
+    return dP.Magnitude();   // return the closest distance
+}
+
+class SphereCollisionChecker {
+    private:
+        const hrp::BodyPtr m_robot;
+    public:
+        std::vector<Capsule> capsule_list_local, capsule_list_wld;
+        std::vector<CollisionInfo> collision_info_list;
+//        std::vector<hrp::ivector > ignore_list;
+        std::vector<std::vector<int> > ignore_list;
+        SphereCollisionChecker(hrp::BodyPtr robot):
+            m_robot(robot){
+            capsule_list_local.resize(m_robot->numJoints());
+            capsule_list_local[m_robot->link("RLEG_JOINT2")->jointId] = Capsule(hrp::Vector3(0,-0.07,0), hrp::Vector3(0,-0.07,-0.5), 0.13);
+            capsule_list_local[m_robot->link("LLEG_JOINT2")->jointId] = Capsule(hrp::Vector3(0,0.07,0), hrp::Vector3(0,0.07,-0.5), 0.13);
+            capsule_list_local[m_robot->link("CHEST_JOINT2")->jointId] = Capsule(hrp::Vector3(-0.1,0,-0.2), hrp::Vector3(-0.1,0,0.3), 0.3);
+            capsule_list_local[m_robot->link("RARM_JOINT4")->jointId] = Capsule(hrp::Vector3(0,0,0), m_robot->link("RARM_JOINT4")->child->b, 0.1);
+            capsule_list_local[m_robot->link("RARM_JOINT7")->jointId] = Capsule(hrp::Vector3(0,0,0), hrp::Vector3(0,0,-0.2), 0.1);
+            capsule_list_local[m_robot->link("LARM_JOINT4")->jointId] = Capsule(hrp::Vector3(0,0,0), hrp::Vector3(0,0,-0.1), 0.1);
+            capsule_list_local[m_robot->link("LARM_JOINT7")->jointId] = Capsule(hrp::Vector3(0,0,0), hrp::Vector3(0,0,-0.2), 0.1);
+            capsule_list_wld = capsule_list_local;
+//            ignore_list[m_robot->link("RLEG_JOINT2")->jointId] << m_robot->link("CHEST_JOINT2")->jointId;
+//            ignore_list[m_robot->link("LLEG_JOINT2")->jointId] << m_robot->link("CHEST_JOINT2")->jointId;
+            ignore_list.resize(m_robot->numJoints());
+            ignore_list[m_robot->link("RLEG_JOINT2")->jointId].push_back(m_robot->link("CHEST_JOINT2")->jointId);
+            ignore_list[m_robot->link("LLEG_JOINT2")->jointId].push_back(m_robot->link("CHEST_JOINT2")->jointId);
+
+        }
+        ~SphereCollisionChecker(){cerr<<"SphereCollisionChecker destructed"<<endl;}
+        void update(){
+            for(int i=0;i<m_robot->numJoints();i++){
+                capsule_list_wld[i].p0 = m_robot->joint(i)->p + m_robot->joint(i)->R * capsule_list_local[i].p0;
+                capsule_list_wld[i].p1 = m_robot->joint(i)->p + m_robot->joint(i)->R * capsule_list_local[i].p1;
+            };
+        }
+        Point Vec3ToPoint(const hrp::Vector3& in){ return Point(in(0),in(1),in(2)); }
+        bool checkCollision(){
+            update();
+            collision_info_list.clear();
+            for(int me=0;me<m_robot->numJoints();me++){
+                for(int you=me; you<m_robot->numJoints();you++){
+                    if(you == me) continue; //自分自身はスルー
+                    if(m_robot->link(me)->parent == m_robot->link(you) || m_robot->link(you)->parent == m_robot->link(me) ) continue; //接続されたリンク同士は無視
+//                    if((ignore_list[me].array()==you).count()) continue;
+                    if(std::count(ignore_list[me].begin(), ignore_list[me].end(), you)) continue;
+                    if(capsule_list_wld[me].r > 0 && capsule_list_wld[you].r > 0){
+                        Point cp0_ans, cp1_ans;
+                        double dist_ans = SegSegDist2(Vec3ToPoint(capsule_list_wld[me].p0), Vec3ToPoint(capsule_list_wld[me].p1-capsule_list_wld[me].p0),
+                                Vec3ToPoint(capsule_list_wld[you].p0), Vec3ToPoint(capsule_list_wld[you].p1-capsule_list_wld[you].p0), cp0_ans, cp1_ans);
+                        double dist_safe = capsule_list_wld[me].r + capsule_list_wld[you].r;
+                        if (dist_ans < dist_safe){
+                            hrp::Vector3 cp0_wld_tmp = hrp::Vector3(cp0_ans.x,cp0_ans.y,cp0_ans.z);
+                            hrp::Vector3 cp1_wld_tmp = hrp::Vector3(cp1_ans.x,cp1_ans.y,cp1_ans.z);
+                            collision_info_list.push_back(CollisionInfo(me, you,
+                                    m_robot->joint(me)->R.transpose() * (cp0_wld_tmp - m_robot->joint(me)->p), m_robot->joint(you)->R.transpose() * (cp1_wld_tmp - m_robot->joint(you)->p),
+                                    cp0_wld_tmp, cp1_wld_tmp, dist_safe, dist_ans));
+                        }
+                    }
+                }
+            };
+            std::cout<<"pair:"<<std::endl;
+            for(int i=0;i<collision_info_list.size();i++){
+                std::cout<<m_robot->joint(collision_info_list[i].id0)->name<<" "<<m_robot->joint(collision_info_list[i].id1)->name<<endl;
+            }
+        }
+};
+
+//class SphereCollisionChecker {
+//    private:
+//        const hrp::BodyPtr m_robot;
+//    public:
+//        std::vector<hrp::Sphere> sphere_list;
+//        std::vector<std::pair<int, int> > collision_pair;
+//        SphereCollisionChecker(hrp::BodyPtr robot):
+//            m_robot(robot){
+//            sphere_list.resize(m_robot->numJoints());
+//            sphere_list[m_robot->link("RLEG_JOINT0")->jointId] = hrp::Sphere(hrp::Vector3(0,0,0), 0.1);
+//            sphere_list[m_robot->link("RARM_JOINT7")->jointId] = hrp::Sphere(hrp::Vector3(0,0,0), 0.1);
+//            sphere_list[m_robot->link("CHEST_JOINT2")->jointId] = hrp::Sphere(hrp::Vector3(0,0,0), 0.4);
+//            sphere_list[m_robot->link("RARM_JOINT4")->jointId] = hrp::Sphere(hrp::Vector3(0,0,0), 0.1);
+//            sphere_list[m_robot->link("RARM_JOINT7")->jointId] = hrp::Sphere(hrp::Vector3(0,0,0), 0.1);
+//            sphere_list[m_robot->link("LARM_JOINT4")->jointId] = hrp::Sphere(hrp::Vector3(0,0,0), 0.1);
+//            sphere_list[m_robot->link("LARM_JOINT7")->jointId] = hrp::Sphere(hrp::Vector3(0,0,0), 0.1);
+//
+//        }
+//        ~SphereCollisionChecker(){cerr<<"SphereCollisionChecker destructed"<<endl;}
+//        void update(){
+//            for(int i=0;i<m_robot->numJoints();i++){
+//                sphere_list[i].cur_pos = m_robot->joint(i)->p + m_robot->joint(i)->R * sphere_list[i].local_pos;
+//            };
+//        }
+//        bool checkCollision(){
+//            collision_pair.clear();
+//            for(int me=0;me<m_robot->numJoints()-1;me++){
+//                for(int you=me+1; you<m_robot->numJoints();you++){
+//                    if(m_robot->link(me)->parent == m_robot->link(you) || m_robot->link(you)->parent == m_robot->link(me) ) continue; //接続されたリンク同士は無視
+//
+//                    if(sphere_list[me].r > 0 && sphere_list[you].r > 0){
+//                        if((sphere_list[me].cur_pos - sphere_list[you].cur_pos).norm() < sphere_list[me].r + sphere_list[you].r){
+//                            collision_pair.push_back(std::make_pair(me,you));
+//                        }
+//                    }
+//                }
+//            };
+//            std::cout<<"pair:"<<std::endl;
+//            for(int i=0;i<collision_pair.size();i++){
+//                std::cout<<m_robot->joint(collision_pair[i].first)->name<<" "<<m_robot->joint(collision_pair[i].second)->name<<endl;
+//            }
+//        }
+//};
+
 class WBMSCore : UTIL_CONST {
     private:
         double CNT_F_TH, h2r_ratio, tgt_h2r_ratio, HZ, DT;
@@ -439,7 +655,7 @@ class WBMSCore : UTIL_CONST {
             }
         }
         void calcVelAccSafeTrajectoryVec(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
-            if((pos_tgt - pos_cur).norm() > 1.0e-6){
+            if((pos_tgt - pos_cur).norm() > 1e-6){
                 for(int i=0;i<3;i++){
                     double stop_safe_vel = SGN(pos_tgt(i) - pos_cur(i)) * sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
                     double vel_ans;
@@ -455,7 +671,7 @@ class WBMSCore : UTIL_CONST {
             }
         }
         void calcVelAccSafeTrajectoryVecML(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& acc_base, const hrp::Matrix33& max_acc_mat, const hrp::Vector3& max_acc_sv, hrp::Vector3& pos_ans){
-            if((pos_tgt - pos_cur).norm() > 1.0e-6){
+            if((pos_tgt - pos_cur).norm() > 1e-6){
                 hrp::Vector3 direc = (pos_tgt - pos_cur).normalized();
                 hrp::Vector3 ref_acc = direc*acc_base;
                 hrp::Vector3 mod_acc = max_acc_mat * max_acc_sv.asDiagonal() * max_acc_mat.transpose() * ref_acc;
@@ -516,7 +732,8 @@ class WBMSCore : UTIL_CONST {
             LIMIT_MINMAX( out.tgt[rf].abs.p(Z), out.tgt[rf].offs.p(Z), out.tgt[rf].offs.p(Z)+WBMSparam.swing_foot_max_height);
             LIMIT_MINMAX( out.tgt[lf].abs.p(Z), out.tgt[lf].offs.p(Z), out.tgt[lf].offs.p(Z)+WBMSparam.swing_foot_max_height);
 
-            const double base2hand_min = 0.4;
+//            const double base2hand_min = 0.4;
+            const double base2hand_min = 0.3;
             for(int i=0, l[2]={rh,lh}; i<2; i++){
                 //        LIMIT_MIN(out.tgt[l[i]].abs.p(X), baselinkpose.p(X));
                 //        LIMIT_MAX(out.tgt[l[i]].abs.p(Z), baselinkpose.p(Z) + 0.4);

From 3950b652444203f5446a2273fb490c12b93013b3 Mon Sep 17 00:00:00 2001
From: ishiguroJSK <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 29 Sep 2017 20:12:00 -0700
Subject: [PATCH 136/217] update fullnody ik

---
 .../FullbodyInverseKinematicsSolver.h         | 79 +++++++++----------
 .../WholeBodyMasterSlave.cpp                  | 11 ---
 rtc/WholeBodyMasterSlave/wbms_core.h          |  2 +-
 3 files changed, 38 insertions(+), 54 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 651d02d7acf..f12b748b976 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -119,26 +119,23 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
 //                LIMIT_MINMAX(diff, -dq_ref_pullback(i), dq_ref_pullback(i));
                 _robot->joint(i)->q += diff;
             }
-//            _robot->rootLink()->p = _robot->rootLink()->p.cwiseProduct( hrp::Vector3::Ones() - q_ref_pullback_gain.segment(J_DOF, 3) ) + q_ref.segment(J_DOF, 3).cwiseProduct( q_ref_pullback_gain.segment(J_DOF, 3) );
-//            _robot->rootLink()->R = hrp::rotFromRpy( hrp::rpyFromRot(_robot->rootLink()->R).cwiseProduct(hrp::Vector3::Ones()-q_ref_pullback_gain.segment(J_DOF+3,3)) + q_ref.segment(J_DOF+3,3).cwiseProduct(q_ref_pullback_gain.segment(J_DOF+3,3)));
             for(int i=0;i<J_DOF;i++){ LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit); }
             _robot->calcForwardKinematics();
             //ヤコビアンと各ベクトル生成
             for ( int i=0; i<_ikc_list.size(); i++ ) {
                 hrp::Link* link_tgt_ptr = _robot->link(_ikc_list[i].target_link_name);
-                const hrp::Matrix33 R_origin_ref = hrp::rotFromRpy(_ikc_list[i].targetRpy) * _ikc_list[i].localR.transpose(); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
-                const hrp::Vector3 p_origin_ref = _ikc_list[i].targetPos - R_origin_ref * _ikc_list[i].localPos;
                 hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF); //全身to一拘束点へのヤコビアン
                 hrp::Vector3 dp_part, dr_part; //差分
                 //ベースリンク，通常リンク，重心で場合分け
                 if(link_tgt_ptr){//ベースリンク，通常リンク共通
-                    dp_part = p_origin_ref - link_tgt_ptr->p;
-                    dr_part = link_tgt_ptr->R * omegaFromRotEx(link_tgt_ptr->R.transpose() * R_origin_ref);
+                    hrp::Vector3 tgt_cur_pos = link_tgt_ptr->p + link_tgt_ptr->R * _ikc_list[i].localPos;
+                    hrp::Matrix33 tgt_cur_rot = link_tgt_ptr->R * _ikc_list[i].localR;
+                    dp_part =  _ikc_list[i].targetPos - tgt_cur_pos;
+                    dr_part = tgt_cur_rot * omegaFromRotEx(tgt_cur_rot.transpose() * hrp::rotFromRpy(_ikc_list[i].targetRpy));
                     //                    dr_part = link_tgt_ptr->R * matrix_logEx(link_tgt_ptr->R.transpose() * R_origin_ref);
-
                     hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, "");
                     hrp::dmatrix J_jpath;
-                    tgt_jpath.calcJacobian(J_jpath);
+                    tgt_jpath.calcJacobian(J_jpath, _ikc_list[i].localPos);
                     for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
                         int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
                         J_part.col(id_in_body) = J_jpath.col(id_in_jpath);
@@ -147,7 +144,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
                     J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - _robot->rootLink()->p);
                 }
                 else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){ //重心限定
-                    dp_part = p_origin_ref - _robot->calcCM();
+                    dp_part = _ikc_list[i].targetPos - _robot->calcCM();
                     dr_part = (_ikc_list[i].targetRpy - cur_momentum_around_COM) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける
                     _robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
                     _robot->calcAngularMomentumJacobian(NULL, J_am);//すでにrootlink周りの角運動量ヤコビアンが返ってくる？
@@ -158,7 +155,6 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
                 constraint_weight_all.segment(WS_DOF*i, WS_DOF) = _ikc_list[i].constraint_weight;
                 J_all.middleRows(WS_DOF*i, WS_DOF) = J_part;
             }
-
             hrp::dmatrix selection_mat = hrp::dmatrix::Zero((constraint_weight_all.array()>0.0).count(), WS_DOF*_ikc_list.size());
             for(int i=0, j=0; i<constraint_weight_all.rows(); i++){ if(constraint_weight_all(i) > 0.0){ selection_mat(j, i) = 1.0; j++; } }
 
@@ -166,7 +162,6 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
             err_all = selection_mat * err_all;
             constraint_weight_all = selection_mat * constraint_weight_all;
 
-
             hrp::dvector dq_weight_all_jlim = dq_weight_all;
             for ( int j = 0; j < _robot->numJoints() ; j++ ) {
                 double jang = _robot->joint(j)->q;
@@ -244,37 +239,37 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
             _robot->calcForwardKinematics();
         }
 
-        void calcJacobianAndErrorVector_BaseLink(hrp::BodyPtr _robot, const IKConstraint& _ikc, hrp::dmatrix _J_part, hrp::dvector6 _d_part){
-            hrp::Link* link_tgt_ptr = _robot->link(_ikc.target_link_name);
-            const hrp::Matrix33 R_origin_ref = hrp::rotFromRpy(_ikc.targetRpy) * _ikc.localR.transpose(); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
-            const hrp::Vector3 p_origin_ref = _ikc.targetPos - R_origin_ref * _ikc.localPos;
-            _d_part.head(3) = p_origin_ref - link_tgt_ptr->p;
-            _d_part.tail(3) = link_tgt_ptr->R * omegaFromRotEx(link_tgt_ptr->R.transpose() * R_origin_ref);
-            hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, "");
-            tgt_jpath.calcJacobian(_J_part);
-            for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
-                int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
-                _J_part.col(id_in_body) = _J_part.col(id_in_jpath);
-            }
-            _J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
-            _J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - _robot->rootLink()->p);
-        }
-
-        void calcJacobianAndErrorVector_NormalLink(hrp::BodyPtr _robot, const IKConstraint& _ikc, hrp::dmatrix _J_part, hrp::dvector6 _d_part){
-            hrp::Link* link_tgt_ptr = _robot->link(_ikc.target_link_name);
-            const hrp::Matrix33 R_origin_ref = hrp::rotFromRpy(_ikc.targetRpy) * _ikc.localR.transpose(); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
-            const hrp::Vector3 p_origin_ref = _ikc.targetPos - R_origin_ref * _ikc.localPos;
-            _d_part.head(3) = p_origin_ref - link_tgt_ptr->p;
-            _d_part.tail(3) = link_tgt_ptr->R * omegaFromRotEx(link_tgt_ptr->R.transpose() * R_origin_ref);
-            hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, "");
-            tgt_jpath.calcJacobian(_J_part);
-            for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
-                int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
-                _J_part.col(id_in_body) = _J_part.col(id_in_jpath);
-            }
-            _J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
-            _J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - _robot->rootLink()->p);
-        }
+//        void calcJacobianAndErrorVector_BaseLink(hrp::BodyPtr _robot, const IKConstraint& _ikc, hrp::dmatrix _J_part, hrp::dvector6 _d_part){
+//            hrp::Link* link_tgt_ptr = _robot->link(_ikc.target_link_name);
+//            const hrp::Matrix33 R_origin_ref = hrp::rotFromRpy(_ikc.targetRpy) * _ikc.localR.transpose(); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
+//            const hrp::Vector3 p_origin_ref = _ikc.targetPos - R_origin_ref * _ikc.localPos;
+//            _d_part.head(3) = p_origin_ref - link_tgt_ptr->p;
+//            _d_part.tail(3) = link_tgt_ptr->R * omegaFromRotEx(link_tgt_ptr->R.transpose() * R_origin_ref);
+//            hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, "");
+//            tgt_jpath.calcJacobian(_J_part);
+//            for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
+//                int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
+//                _J_part.col(id_in_body) = _J_part.col(id_in_jpath);
+//            }
+//            _J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
+//            _J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - _robot->rootLink()->p);
+//        }
+//
+//        void calcJacobianAndErrorVector_NormalLink(hrp::BodyPtr _robot, const IKConstraint& _ikc, hrp::dmatrix _J_part, hrp::dvector6 _d_part){
+//            hrp::Link* link_tgt_ptr = _robot->link(_ikc.target_link_name);
+//            const hrp::Matrix33 R_origin_ref = hrp::rotFromRpy(_ikc.targetRpy) * _ikc.localR.transpose(); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
+//            const hrp::Vector3 p_origin_ref = _ikc.targetPos - R_origin_ref * _ikc.localPos;
+//            _d_part.head(3) = p_origin_ref - link_tgt_ptr->p;
+//            _d_part.tail(3) = link_tgt_ptr->R * omegaFromRotEx(link_tgt_ptr->R.transpose() * R_origin_ref);
+//            hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, "");
+//            tgt_jpath.calcJacobian(_J_part);
+//            for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
+//                int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
+//                _J_part.col(id_in_body) = _J_part.col(id_in_jpath);
+//            }
+//            _J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
+//            _J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - _robot->rootLink()->p);
+//        }
 
     protected:
 //        bool calcJacobianInverseNullspace_copy(hrp::BodyPtr _robot, hrp::dmatrix &_J, hrp::dmatrix &_Jinv) {
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 1beaf1d45d3..526b45194bc 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -612,7 +612,6 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.targetPos = rh_ref.p;
         tmp.targetRpy = rh_ref.rpy;
         tmp.constraint_weight = hrp::dvector6::Constant(0.1);
-        tmp.constraint_weight << 1,1,1,0,0,0;
         tmp.pos_precision = 3e-3;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
@@ -643,16 +642,6 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.constraint_weight << 1,1,1,0,0,0;
         ikc_list.push_back(tmp);
     }
-//    for(int i=0;i<sccp->collision_pair.size();i++){
-//        const hrp::Sphere& base = sccp->sphere_list[sccp->collision_pair[i].first];
-//        const hrp::Sphere& move = sccp->sphere_list[sccp->collision_pair[i].second];
-//        IKConstraint tmp;
-//        tmp.localPos = move.local_pos;
-//        tmp.target_link_name = m_robot->joint(sccp->collision_pair[i].second)->name;
-//        tmp.targetPos = base.cur_pos + (move.cur_pos - base.cur_pos).normalized() * (move.r + base.r + 1e-3);
-//        tmp.constraint_weight << 1,1,1,0,0,0;
-//        ikc_list.push_back(tmp);
-//    }
 
 
     {
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 376e8952636..f0eec1dc0a9 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -237,7 +237,7 @@ class SphereCollisionChecker {
         SphereCollisionChecker(hrp::BodyPtr robot):
             m_robot(robot){
             capsule_list_local.resize(m_robot->numJoints());
-            capsule_list_local[m_robot->link("RLEG_JOINT2")->jointId] = Capsule(hrp::Vector3(0,-0.07,0), hrp::Vector3(0,-0.07,-0.5), 0.13);
+            capsule_list_local[m_robot->link("RLEG_JOINT2")->jointId] = Capsule(hrp::Vector3(0,-0.07,0), hrp::Vector3(0,-0.07,-0.5), 0.1);
             capsule_list_local[m_robot->link("LLEG_JOINT2")->jointId] = Capsule(hrp::Vector3(0,0.07,0), hrp::Vector3(0,0.07,-0.5), 0.13);
             capsule_list_local[m_robot->link("CHEST_JOINT2")->jointId] = Capsule(hrp::Vector3(-0.1,0,-0.2), hrp::Vector3(-0.1,0,0.3), 0.3);
             capsule_list_local[m_robot->link("RARM_JOINT4")->jointId] = Capsule(hrp::Vector3(0,0,0), m_robot->link("RARM_JOINT4")->child->b, 0.1);

From 75bd6a6230bab1cd9cc5e14824600704f76acba4 Mon Sep 17 00:00:00 2001
From: ishiguroJSK <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 30 Sep 2017 14:29:44 +0900
Subject: [PATCH 137/217] capsule colliison ok

---
 .../WholeBodyMasterSlave.cpp                  |  2 +-
 .../WholeBodyMasterSlave.h                    |  2 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 65 ++++++++++++-------
 3 files changed, 43 insertions(+), 26 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 526b45194bc..6074bc22f92 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -196,7 +196,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
 
     avg_q_vel = 4;
 
-    sccp = boost::shared_ptr<SphereCollisionChecker>(new SphereCollisionChecker(m_robot));
+    sccp = boost::shared_ptr<CapsuleCollisionChecker>(new CapsuleCollisionChecker(m_robot));
 
     std::cerr << "[" << m_profile.instance_name << "] onInitialize() OK" << std::endl;
     return RTC::RTC_OK;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 522cb33552c..110cb172a1d 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -196,7 +196,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
     HumanPose raw_pose;
 
     boost::shared_ptr<WBMSCore> hsp;
-    boost::shared_ptr<SphereCollisionChecker> sccp;
+    boost::shared_ptr<CapsuleCollisionChecker> sccp;
 
     hrp::Vector3 torso_rot_rmc;
     ControlMode mode;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index f0eec1dc0a9..c6185e7bfff 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -226,33 +226,51 @@ inline double SegSegDist2(const Point& u0, const Point& u, const Point& v0, cons
     return dP.Magnitude();   // return the closest distance
 }
 
-class SphereCollisionChecker {
+class CapsuleCollisionChecker {
     private:
         const hrp::BodyPtr m_robot;
     public:
         std::vector<Capsule> capsule_list_local, capsule_list_wld;
         std::vector<CollisionInfo> collision_info_list;
-//        std::vector<hrp::ivector > ignore_list;
-        std::vector<std::vector<int> > ignore_list;
-        SphereCollisionChecker(hrp::BodyPtr robot):
+        Eigen::MatrixXi check_pair_mat;
+        CapsuleCollisionChecker(hrp::BodyPtr robot):
             m_robot(robot){
             capsule_list_local.resize(m_robot->numJoints());
-            capsule_list_local[m_robot->link("RLEG_JOINT2")->jointId] = Capsule(hrp::Vector3(0,-0.07,0), hrp::Vector3(0,-0.07,-0.5), 0.1);
-            capsule_list_local[m_robot->link("LLEG_JOINT2")->jointId] = Capsule(hrp::Vector3(0,0.07,0), hrp::Vector3(0,0.07,-0.5), 0.13);
-            capsule_list_local[m_robot->link("CHEST_JOINT2")->jointId] = Capsule(hrp::Vector3(-0.1,0,-0.2), hrp::Vector3(-0.1,0,0.3), 0.3);
-            capsule_list_local[m_robot->link("RARM_JOINT4")->jointId] = Capsule(hrp::Vector3(0,0,0), m_robot->link("RARM_JOINT4")->child->b, 0.1);
-            capsule_list_local[m_robot->link("RARM_JOINT7")->jointId] = Capsule(hrp::Vector3(0,0,0), hrp::Vector3(0,0,-0.2), 0.1);
-            capsule_list_local[m_robot->link("LARM_JOINT4")->jointId] = Capsule(hrp::Vector3(0,0,0), hrp::Vector3(0,0,-0.1), 0.1);
-            capsule_list_local[m_robot->link("LARM_JOINT7")->jointId] = Capsule(hrp::Vector3(0,0,0), hrp::Vector3(0,0,-0.2), 0.1);
+            for(int i=0; i<m_robot->numJoints(); i++){
+                if(m_robot->joint(i)->child){
+                    double r = 0.0;
+                    if(m_robot->joint(i)->name.find("LEG_JOINT") != std::string::npos ){ r = 0.09; }
+                    else if(m_robot->joint(i)->name.find("ARM_JOINT") != std::string::npos ){ r = 0.075; }
+                    else if(m_robot->joint(i)->name.find("CHEST_JOINT") != std::string::npos ){ r = 0.25; }
+                    if(m_robot->joint(i)->child->b.norm() < 0.1) r = 0;
+                    capsule_list_local[i] = Capsule(hrp::Vector3(0,0,0), m_robot->joint(i)->child->b, r);
+                }
+            };
+
+            for(int i=0; i<m_robot->numJoints(); i++){
+                std::cout<<m_robot->joint(i)->name<<" capsule: "<<capsule_list_local[i].p0.transpose()<<" - "<<capsule_list_local[i].p1.transpose()<<" r: "<<capsule_list_local[i].r<<std::endl;
+            }
             capsule_list_wld = capsule_list_local;
-//            ignore_list[m_robot->link("RLEG_JOINT2")->jointId] << m_robot->link("CHEST_JOINT2")->jointId;
-//            ignore_list[m_robot->link("LLEG_JOINT2")->jointId] << m_robot->link("CHEST_JOINT2")->jointId;
-            ignore_list.resize(m_robot->numJoints());
-            ignore_list[m_robot->link("RLEG_JOINT2")->jointId].push_back(m_robot->link("CHEST_JOINT2")->jointId);
-            ignore_list[m_robot->link("LLEG_JOINT2")->jointId].push_back(m_robot->link("CHEST_JOINT2")->jointId);
+            check_pair_mat = Eigen::MatrixXi::Ones(capsule_list_wld.size(),capsule_list_wld.size());
+            for(int me=0;me<m_robot->numJoints();me++){
+                for(int you=me; you<m_robot->numJoints();you++){
+                    if(me == you) check_pair_mat(me, you) = 0;
+                    if(m_robot->joint(me)->name.find("RLEG_JOINT") != std::string::npos && m_robot->joint(you)->name.find("RLEG_JOINT") != std::string::npos) check_pair_mat(me, you) = 0;
+                    if(m_robot->joint(me)->name.find("LLEG_JOINT") != std::string::npos && m_robot->joint(you)->name.find("LLEG_JOINT") != std::string::npos) check_pair_mat(me, you) = 0;
+                    if(m_robot->joint(me)->name.find("RARM_JOINT") != std::string::npos && m_robot->joint(you)->name.find("RARM_JOINT") != std::string::npos) check_pair_mat(me, you) = 0;
+                    if(m_robot->joint(me)->name.find("LARM_JOINT") != std::string::npos && m_robot->joint(you)->name.find("LARM_JOINT") != std::string::npos) check_pair_mat(me, you) = 0;
+                    if(m_robot->link(me)->parent == m_robot->link(you) || m_robot->link(you)->parent == m_robot->link(me)) check_pair_mat(me, you) = 0;
 
+                    if(m_robot->joint(me)->name.find("LEG_JOINT0") != std::string::npos && m_robot->joint(you)->name.find("CHEST_JOINT") != std::string::npos) check_pair_mat(me, you) = 0;
+                    if(m_robot->joint(me)->name.find("LEG_JOINT1") != std::string::npos && m_robot->joint(you)->name.find("CHEST_JOINT") != std::string::npos) check_pair_mat(me, you) = 0;
+                    if(m_robot->joint(me)->name.find("LEG_JOINT2") != std::string::npos && m_robot->joint(you)->name.find("CHEST_JOINT") != std::string::npos) check_pair_mat(me, you) = 0;
+                    if(m_robot->joint(me)->name.find("CHEST_JOINT") != std::string::npos && m_robot->joint(you)->name.find("ARM_JOINT0") != std::string::npos) check_pair_mat(me, you) = 0;
+                    if(m_robot->joint(me)->name.find("CHEST_JOINT") != std::string::npos && m_robot->joint(you)->name.find("ARM_JOINT1") != std::string::npos) check_pair_mat(me, you) = 0;
+                    if(m_robot->joint(me)->name.find("CHEST_JOINT") != std::string::npos && m_robot->joint(you)->name.find("ARM_JOINT2") != std::string::npos) check_pair_mat(me, you) = 0;
+                }
+            }
         }
-        ~SphereCollisionChecker(){cerr<<"SphereCollisionChecker destructed"<<endl;}
+        ~CapsuleCollisionChecker(){cerr<<"CapsuleCollisionChecker destructed"<<endl;}
         void update(){
             for(int i=0;i<m_robot->numJoints();i++){
                 capsule_list_wld[i].p0 = m_robot->joint(i)->p + m_robot->joint(i)->R * capsule_list_local[i].p0;
@@ -265,11 +283,10 @@ class SphereCollisionChecker {
             collision_info_list.clear();
             for(int me=0;me<m_robot->numJoints();me++){
                 for(int you=me; you<m_robot->numJoints();you++){
-                    if(you == me) continue; //自分自身はスルー
-                    if(m_robot->link(me)->parent == m_robot->link(you) || m_robot->link(you)->parent == m_robot->link(me) ) continue; //接続されたリンク同士は無視
-//                    if((ignore_list[me].array()==you).count()) continue;
-                    if(std::count(ignore_list[me].begin(), ignore_list[me].end(), you)) continue;
-                    if(capsule_list_wld[me].r > 0 && capsule_list_wld[you].r > 0){
+//                    if(you == me) continue; //自分自身はスルー
+//                    if(m_robot->link(me)->parent == m_robot->link(you) || m_robot->link(you)->parent == m_robot->link(me) ) continue; //接続されたリンク同士は無視
+//                    if(std::count(ignore_list[me].begin(), ignore_list[me].end(), you)) continue;
+                    if(capsule_list_wld[me].r > 0 && capsule_list_wld[you].r > 0 && check_pair_mat(me,you)){
                         Point cp0_ans, cp1_ans;
                         double dist_ans = SegSegDist2(Vec3ToPoint(capsule_list_wld[me].p0), Vec3ToPoint(capsule_list_wld[me].p1-capsule_list_wld[me].p0),
                                 Vec3ToPoint(capsule_list_wld[you].p0), Vec3ToPoint(capsule_list_wld[you].p1-capsule_list_wld[you].p0), cp0_ans, cp1_ans);
@@ -291,13 +308,13 @@ class SphereCollisionChecker {
         }
 };
 
-//class SphereCollisionChecker {
+//class CapsuleCollisionChecker {
 //    private:
 //        const hrp::BodyPtr m_robot;
 //    public:
 //        std::vector<hrp::Sphere> sphere_list;
 //        std::vector<std::pair<int, int> > collision_pair;
-//        SphereCollisionChecker(hrp::BodyPtr robot):
+//        CapsuleCollisionChecker(hrp::BodyPtr robot):
 //            m_robot(robot){
 //            sphere_list.resize(m_robot->numJoints());
 //            sphere_list[m_robot->link("RLEG_JOINT0")->jointId] = hrp::Sphere(hrp::Vector3(0,0,0), 0.1);

From 6bd910398c967c08a2ec881af330baa3f5a3adea Mon Sep 17 00:00:00 2001
From: ishiguroJSK <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 30 Sep 2017 15:08:05 +0900
Subject: [PATCH 138/217] add avoid priority

---
 .../WholeBodyMasterSlave.cpp                  | 34 ++++++++++++++++---
 rtc/WholeBodyMasterSlave/wbms_core.h          |  9 +++++
 2 files changed, 39 insertions(+), 4 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 6074bc22f92..4abc540df2b 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -634,13 +634,39 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.rot_precision = deg2rad(1);
         ikc_list.push_back(tmp);
     }
+    if(hsp->rp_ref_out.tgt[rf].is_contact){
+        sccp->avoid_priority.head(12).head(6).fill(4);
+    }else{
+        sccp->avoid_priority.head(12).head(6).fill(3);
+    }
+    if(hsp->rp_ref_out.tgt[lf].is_contact){
+        sccp->avoid_priority.head(12).tail(6).fill(4);
+    }else{
+        sccp->avoid_priority.head(12).tail(6).fill(3);
+    }
     for(int i=0;i<sccp->collision_info_list.size();i++){
         IKConstraint tmp;
-        tmp.localPos = sccp->collision_info_list[i].cp1_local;
-        tmp.target_link_name = m_robot->joint(sccp->collision_info_list[i].id1)->name;
-        tmp.targetPos = sccp->collision_info_list[i].cp0_wld + (sccp->collision_info_list[i].cp1_wld - sccp->collision_info_list[i].cp0_wld).normalized() * (sccp->collision_info_list[i].dist_safe + 1e-3);
         tmp.constraint_weight << 1,1,1,0,0,0;
-        ikc_list.push_back(tmp);
+        if(sccp->avoid_priority(sccp->collision_info_list[i].id0) > sccp->avoid_priority(sccp->collision_info_list[i].id1)){
+            tmp.localPos = sccp->collision_info_list[i].cp1_local;
+            tmp.target_link_name = m_robot->joint(sccp->collision_info_list[i].id1)->name;
+            tmp.targetPos = sccp->collision_info_list[i].cp0_wld + (sccp->collision_info_list[i].cp1_wld - sccp->collision_info_list[i].cp0_wld).normalized() * (sccp->collision_info_list[i].dist_safe + 1e-3);
+            ikc_list.push_back(tmp);
+        }else if(sccp->avoid_priority(sccp->collision_info_list[i].id0) < sccp->avoid_priority(sccp->collision_info_list[i].id1)){
+            tmp.localPos = sccp->collision_info_list[i].cp0_local;
+            tmp.target_link_name = m_robot->joint(sccp->collision_info_list[i].id0)->name;
+            tmp.targetPos = sccp->collision_info_list[i].cp1_wld + (sccp->collision_info_list[i].cp0_wld - sccp->collision_info_list[i].cp1_wld).normalized() * (sccp->collision_info_list[i].dist_safe + 1e-3);
+            ikc_list.push_back(tmp);
+        }else{
+            tmp.localPos = sccp->collision_info_list[i].cp1_local;
+            tmp.target_link_name = m_robot->joint(sccp->collision_info_list[i].id1)->name;
+            tmp.targetPos = sccp->collision_info_list[i].cp0_wld + (sccp->collision_info_list[i].cp1_wld - sccp->collision_info_list[i].cp0_wld).normalized() * (sccp->collision_info_list[i].dist_safe + 1e-3);
+            ikc_list.push_back(tmp);
+            tmp.localPos = sccp->collision_info_list[i].cp0_local;
+            tmp.target_link_name = m_robot->joint(sccp->collision_info_list[i].id0)->name;
+            tmp.targetPos = sccp->collision_info_list[i].cp1_wld + (sccp->collision_info_list[i].cp0_wld - sccp->collision_info_list[i].cp1_wld).normalized() * (sccp->collision_info_list[i].dist_safe + 1e-3);
+            ikc_list.push_back(tmp);
+        }
     }
 
 
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index c6185e7bfff..3a931f9da07 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -233,6 +233,7 @@ class CapsuleCollisionChecker {
         std::vector<Capsule> capsule_list_local, capsule_list_wld;
         std::vector<CollisionInfo> collision_info_list;
         Eigen::MatrixXi check_pair_mat;
+        hrp::ivector avoid_priority;
         CapsuleCollisionChecker(hrp::BodyPtr robot):
             m_robot(robot){
             capsule_list_local.resize(m_robot->numJoints());
@@ -269,6 +270,14 @@ class CapsuleCollisionChecker {
                     if(m_robot->joint(me)->name.find("CHEST_JOINT") != std::string::npos && m_robot->joint(you)->name.find("ARM_JOINT2") != std::string::npos) check_pair_mat(me, you) = 0;
                 }
             }
+            avoid_priority = hrp::ivector::Zero(m_robot->numJoints());
+
+            for(int i=0;i<m_robot->numJoints();i++){
+                if(m_robot->joint(i)->name.find("ARM_JOINT") != std::string::npos) avoid_priority(i) = 1;
+                if(m_robot->joint(i)->name.find("CHEST_JOINT") != std::string::npos) avoid_priority(i) = 2;
+                if(m_robot->joint(i)->name.find("LEG_JOINT") != std::string::npos) avoid_priority(i) = 3;
+            }
+
         }
         ~CapsuleCollisionChecker(){cerr<<"CapsuleCollisionChecker destructed"<<endl;}
         void update(){

From a78f81e8baf646a749323b52e05cbddd0db0753d Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 11 Oct 2017 16:00:20 +0900
Subject: [PATCH 139/217] tune capsule collision

---
 .../FullbodyInverseKinematicsSolver.h         |  57 +++---
 .../WholeBodyMasterSlave.cpp                  |  35 +++-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 192 +++++++++---------
 3 files changed, 150 insertions(+), 134 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index f12b748b976..3ed852898bd 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -136,7 +136,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
                     hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, "");
                     hrp::dmatrix J_jpath;
                     tgt_jpath.calcJacobian(J_jpath, _ikc_list[i].localPos);
-                    for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
+                    for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJacobianを全身用に並び替え
                         int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
                         J_part.col(id_in_body) = J_jpath.col(id_in_jpath);
                     }
@@ -151,6 +151,9 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
                     J_part << J_com, J_am;
                 }
                 else{ std::cerr<<"Unknown Link Target !!"<<std::endl; continue; } //不明なリンク指定
+                if(dp_part.norm()>0.01){
+                    dp_part = dp_part.normalized() * 0.01;
+                }
                 err_all.segment(WS_DOF*i,WS_DOF) << dp_part, dr_part;
                 constraint_weight_all.segment(WS_DOF*i, WS_DOF) = _ikc_list[i].constraint_weight;
                 J_all.middleRows(WS_DOF*i, WS_DOF) = J_part;
@@ -163,32 +166,32 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
             constraint_weight_all = selection_mat * constraint_weight_all;
 
             hrp::dvector dq_weight_all_jlim = dq_weight_all;
-            for ( int j = 0; j < _robot->numJoints() ; j++ ) {
-                double jang = _robot->joint(j)->q;
-                double jmax = _robot->joint(j)->ulimit;
-                double jmin = _robot->joint(j)->llimit;
-                double e = deg2rad(1);
-                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
-                } else if ( eps_eq(jang, jmax,e) ) {
-                    jang = jmax - e;
-                } else if ( eps_eq(jang, jmin,e) ) {
-                    jang = jmin + e;
-                }
-                double r;
-                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
-                    r = DBL_MAX;
-                } else {
-                    r = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) /
-                            (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
-                    if (isnan(r)) r = 0;
-                }
-                if (( r - avoid_weight_gain(j) ) >= 0 ) {
-                    dq_weight_all_jlim(j) *= ( 1.0 / ( 1.0 + r) );
-                } else {//リミットから離れるときは解放
-                    dq_weight_all_jlim(j) *= 1.0;
-                }
-                avoid_weight_gain(j) = r;
-            }
+//            for ( int j = 0; j < _robot->numJoints() ; j++ ) {
+//                double jang = _robot->joint(j)->q;
+//                double jmax = _robot->joint(j)->ulimit;
+//                double jmin = _robot->joint(j)->llimit;
+//                double e = deg2rad(1);
+//                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+//                } else if ( eps_eq(jang, jmax,e) ) {
+//                    jang = jmax - e;
+//                } else if ( eps_eq(jang, jmin,e) ) {
+//                    jang = jmin + e;
+//                }
+//                double r;
+//                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+//                    r = DBL_MAX;
+//                } else {
+//                    r = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) /
+//                            (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
+//                    if (isnan(r)) r = 0;
+//                }
+//                if (( r - avoid_weight_gain(j) ) >= 0 ) {
+//                    dq_weight_all_jlim(j) *= ( 1.0 / ( 1.0 + r) );
+//                } else {//リミットから離れるときは解放
+//                    dq_weight_all_jlim(j) *= 1.0;
+//                }
+//                avoid_weight_gain(j) = r;
+//            }
 
             const double wn_const = 1e-6;
             const double auto_lambda_gain = 1;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 4abc540df2b..91a6b5663ca 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -646,27 +646,44 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     }
     for(int i=0;i<sccp->collision_info_list.size();i++){
         IKConstraint tmp;
-        tmp.constraint_weight << 1,1,1,0,0,0;
+//        tmp.constraint_weight << 1,1,1,0,0,0;
+        tmp.constraint_weight << 3,3,3,0,0,0;
+//        double val_w = (sccp->collision_info_list[i].dist_safe - sccp->collision_info_list[i].dist_cur)*1e3;
+//        LIMIT_MAX(val_w, 3);
+//        tmp.constraint_weight << val_w,val_w,val_w,0,0,0;
+//        double margin = 1e-3;
+        double margin = 1e-3;
         if(sccp->avoid_priority(sccp->collision_info_list[i].id0) > sccp->avoid_priority(sccp->collision_info_list[i].id1)){
             tmp.localPos = sccp->collision_info_list[i].cp1_local;
             tmp.target_link_name = m_robot->joint(sccp->collision_info_list[i].id1)->name;
-            tmp.targetPos = sccp->collision_info_list[i].cp0_wld + (sccp->collision_info_list[i].cp1_wld - sccp->collision_info_list[i].cp0_wld).normalized() * (sccp->collision_info_list[i].dist_safe + 1e-3);
+            tmp.targetPos = sccp->collision_info_list[i].cp0_wld + (sccp->collision_info_list[i].cp1_wld - sccp->collision_info_list[i].cp0_wld).normalized() * (sccp->collision_info_list[i].dist_safe + margin);
             ikc_list.push_back(tmp);
         }else if(sccp->avoid_priority(sccp->collision_info_list[i].id0) < sccp->avoid_priority(sccp->collision_info_list[i].id1)){
             tmp.localPos = sccp->collision_info_list[i].cp0_local;
             tmp.target_link_name = m_robot->joint(sccp->collision_info_list[i].id0)->name;
-            tmp.targetPos = sccp->collision_info_list[i].cp1_wld + (sccp->collision_info_list[i].cp0_wld - sccp->collision_info_list[i].cp1_wld).normalized() * (sccp->collision_info_list[i].dist_safe + 1e-3);
+            tmp.targetPos = sccp->collision_info_list[i].cp1_wld + (sccp->collision_info_list[i].cp0_wld - sccp->collision_info_list[i].cp1_wld).normalized() * (sccp->collision_info_list[i].dist_safe + margin);
             ikc_list.push_back(tmp);
         }else{
             tmp.localPos = sccp->collision_info_list[i].cp1_local;
             tmp.target_link_name = m_robot->joint(sccp->collision_info_list[i].id1)->name;
-            tmp.targetPos = sccp->collision_info_list[i].cp0_wld + (sccp->collision_info_list[i].cp1_wld - sccp->collision_info_list[i].cp0_wld).normalized() * (sccp->collision_info_list[i].dist_safe + 1e-3);
+            tmp.targetPos = sccp->collision_info_list[i].cp0_wld + (sccp->collision_info_list[i].cp1_wld - sccp->collision_info_list[i].cp0_wld).normalized() * (sccp->collision_info_list[i].dist_safe + margin);
             ikc_list.push_back(tmp);
             tmp.localPos = sccp->collision_info_list[i].cp0_local;
             tmp.target_link_name = m_robot->joint(sccp->collision_info_list[i].id0)->name;
-            tmp.targetPos = sccp->collision_info_list[i].cp1_wld + (sccp->collision_info_list[i].cp0_wld - sccp->collision_info_list[i].cp1_wld).normalized() * (sccp->collision_info_list[i].dist_safe + 1e-3);
+            tmp.targetPos = sccp->collision_info_list[i].cp1_wld + (sccp->collision_info_list[i].cp0_wld - sccp->collision_info_list[i].cp1_wld).normalized() * (sccp->collision_info_list[i].dist_safe + margin);
             ikc_list.push_back(tmp);
         }
+        ikc_list[3].constraint_weight =  hrp::dvector6::Constant(1e-4);
+        ikc_list[4].constraint_weight =  hrp::dvector6::Constant(1e-4);
+    }
+
+    if(loop%20==0){
+        if(sccp->collision_info_list.size()>0){
+            std::cout<<"pair:"<<std::endl;
+            for(int i=0;i<sccp->collision_info_list.size();i++){
+                std::cout<<m_robot->joint(sccp->collision_info_list[i].id0)->name<<" "<<m_robot->joint(sccp->collision_info_list[i].id1)->name<<endl;
+            }
+        }
     }
 
 
@@ -677,8 +694,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.localR = hrp::Matrix33::Identity();
         tmp.targetPos = com_ref.p;// COM height will not be constraint
         tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
-        tmp.constraint_weight << 10,10,1,1e-6,1e-6,1e-6;
-//        tmp.constraint_weight << 10,10,1,0,0,0;
+//        tmp.constraint_weight << 10,10,1,1e-6,1e-6,1e-6;
+        tmp.constraint_weight << 10,10,1,0,0,0;
 //        if(fik_in->cur_momentum_around_COM.norm() > 1e9){
 //            tmp.constraint_weight << 10,10,1,1e-5,1e-5,1e-10;
 //            tmp.rot_precision = 100;//angular momentum precision
@@ -691,8 +708,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     fik_in->dq_weight_all(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;
     fik_in->dq_weight_all(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
     fik_in->dq_weight_all(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
-//    if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇内側の干渉回避
-//    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(40);
+    if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->ulimit = deg2rad(-30);//脇内側の干渉回避
+    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(30);
 //    if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->llimit = deg2rad(-140);//肩外側の干渉回避
 //    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->ulimit = deg2rad(140);
 
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 3a931f9da07..ee580603c88 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -133,27 +133,6 @@ namespace hrp{
     };
 }
 
-class Capsule{
-    public:
-        hrp::Vector3 p0, p1;
-        double r;
-        Capsule(const hrp::Vector3 _p0 = hrp::Vector3::Zero(), const hrp::Vector3 _p1 = hrp::Vector3::Zero(), const double _r = 0){
-            p0 = _p0; p1 = _p1; r = _r;
-        }
-};
-
-class CollisionInfo{
-    public:
-        int id0, id1;
-        hrp::Vector3 cp0_local, cp1_local, cp0_wld, cp1_wld;
-        double dist_safe, dist_cur;
-        CollisionInfo(const int _id0 = 0, const int _id1 = 0,
-                const hrp::Vector3 _cp0_local = hrp::Vector3::Zero(), const hrp::Vector3 _cp1_local = hrp::Vector3::Zero(),
-                const hrp::Vector3 _cp0_wld = hrp::Vector3::Zero(), const hrp::Vector3 _cp1_wld = hrp::Vector3::Zero(),
-                const double _dist_safe = 0, const double _dist_cur = 0){
-            id0 = _id0; id1 = _id1; cp0_local = _cp0_local; cp1_local = _cp1_local;  cp0_wld = _cp0_wld; cp1_wld = _cp1_wld; dist_safe = _dist_safe; dist_cur = _dist_cur;
-        }
-};
 
 inline double SegSegDist2(const Point& u0, const Point& u, const Point& v0, const Point& v, Point& cp0, Point& cp1){
     Point    w = u0 - v0;
@@ -214,8 +193,8 @@ inline double SegSegDist2(const Point& u0, const Point& u, const Point& v0, cons
         }
     }
     // finally do the division to get sc and tc
-    sc = (fabsf(sN) < EPS ? 0.0f : sN / sD);
-    tc = (fabsf(tN) < EPS ? 0.0f : tN / tD);
+    sc = (fabs(sN) < EPS ? 0.0 : sN / sD);
+    tc = (fabs(tN) < EPS ? 0.0 : tN / tD);
 
     cp0 = u0 + sc * u;
     cp1 = v0 + tc * v;
@@ -226,33 +205,77 @@ inline double SegSegDist2(const Point& u0, const Point& u, const Point& v0, cons
     return dP.Magnitude();   // return the closest distance
 }
 
+class Capsule{
+    public:
+        hrp::Vector3 p0, p1;
+        double r;
+        Capsule(const hrp::Vector3 _p0 = hrp::Vector3::Zero(), const hrp::Vector3 _p1 = hrp::Vector3::Zero(), const double _r = 0){
+            p0 = _p0; p1 = _p1; r = _r;
+        }
+};
+
+class CollisionInfo{
+    public:
+        int id0, id1;
+        hrp::Vector3 cp0_local, cp1_local, cp0_wld, cp1_wld;
+        double dist_safe, dist_cur;
+        CollisionInfo(const int _id0 = 0, const int _id1 = 0,
+                const hrp::Vector3 _cp0_local = hrp::Vector3::Zero(), const hrp::Vector3 _cp1_local = hrp::Vector3::Zero(),
+                const hrp::Vector3 _cp0_wld = hrp::Vector3::Zero(), const hrp::Vector3 _cp1_wld = hrp::Vector3::Zero(),
+                const double _dist_safe = 0, const double _dist_cur = 0){
+            id0 = _id0; id1 = _id1; cp0_local = _cp0_local; cp1_local = _cp1_local;  cp0_wld = _cp0_wld; cp1_wld = _cp1_wld; dist_safe = _dist_safe; dist_cur = _dist_cur;
+        }
+};
+
+typedef std::vector<Capsule> CapsuleArray;
+
 class CapsuleCollisionChecker {
     private:
         const hrp::BodyPtr m_robot;
+        std::vector<CapsuleArray> capsule_array_list_local, capsule_array_list_wld;
     public:
-        std::vector<Capsule> capsule_list_local, capsule_list_wld;
         std::vector<CollisionInfo> collision_info_list;
         Eigen::MatrixXi check_pair_mat;
         hrp::ivector avoid_priority;
+        bool hasStr(const std::string& str, const std::string key){ return str.find(key) != std::string::npos; }
         CapsuleCollisionChecker(hrp::BodyPtr robot):
             m_robot(robot){
-            capsule_list_local.resize(m_robot->numJoints());
+            capsule_array_list_local.resize(m_robot->numJoints());
             for(int i=0; i<m_robot->numJoints(); i++){
-                if(m_robot->joint(i)->child){
-                    double r = 0.0;
-                    if(m_robot->joint(i)->name.find("LEG_JOINT") != std::string::npos ){ r = 0.09; }
-                    else if(m_robot->joint(i)->name.find("ARM_JOINT") != std::string::npos ){ r = 0.075; }
-                    else if(m_robot->joint(i)->name.find("CHEST_JOINT") != std::string::npos ){ r = 0.25; }
-                    if(m_robot->joint(i)->child->b.norm() < 0.1) r = 0;
-                    capsule_list_local[i] = Capsule(hrp::Vector3(0,0,0), m_robot->joint(i)->child->b, r);
+                if(hasStr(m_robot->joint(i)->name,"LEG_JOINT0")){
+                    capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), m_robot->joint(i)->child->b, 0.099));
+                }
+                if(hasStr(m_robot->joint(i)->name,"LLEG_JOINT2")){
+                    capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), m_robot->joint(i)->child->b, 0.095));
+                    capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0.05,0), m_robot->joint(i)->child->b, 0.095));
+                }
+                if(hasStr(m_robot->joint(i)->name,"RLEG_JOINT2")){
+                    capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), m_robot->joint(i)->child->b, 0.095));
+                    capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,-0.05,0), m_robot->joint(i)->child->b, 0.095));
+                }
+                if(hasStr(m_robot->joint(i)->name,"LEG_JOINT3")){
+                    capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0.05,0,0), m_robot->joint(i)->child->b, 0.095));
+                }
+                if(hasStr(m_robot->joint(i)->name,"ARM_JOINT2") || hasStr(m_robot->joint(i)->name,"ARM_JOINT3") || hasStr(m_robot->joint(i)->name,"ARM_JOINT4") || hasStr(m_robot->joint(i)->name,"ARM_JOINT5") || hasStr(m_robot->joint(i)->name,"ARM_JOINT6")){
+                    capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), m_robot->joint(i)->child->b, 0.09));
+                }
+                if(hasStr(m_robot->joint(i)->name,"ARM_JOINT7")){
+                    capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), hrp::Vector3(0,0,-0.25), 0.1));
+                }
+                if(hasStr(m_robot->joint(i)->name,"CHEST_JOINT1") || hasStr(m_robot->joint(i)->name,"CHEST_JOINT2")){
+                    capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), m_robot->joint(i)->child->b, 0.2));
+                    capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(-0.1,0,0), m_robot->joint(i)->child->b + hrp::Vector3(-0.1,0,0), 0.2));
+                    capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(-0.2,0,0), m_robot->joint(i)->child->b + hrp::Vector3(-0.2,0,0), 0.2));
                 }
             };
 
             for(int i=0; i<m_robot->numJoints(); i++){
-                std::cout<<m_robot->joint(i)->name<<" capsule: "<<capsule_list_local[i].p0.transpose()<<" - "<<capsule_list_local[i].p1.transpose()<<" r: "<<capsule_list_local[i].r<<std::endl;
+                for(int j=0; j<capsule_array_list_local[i].size(); j++){
+                    std::cout<<m_robot->joint(i)->name<<" capsule["<<j<<"] "<<capsule_array_list_local[i][j].p0.transpose()<<" - "<<capsule_array_list_local[i][j].p1.transpose()<<" r: "<<capsule_array_list_local[i][j].r<<std::endl;
+                }
             }
-            capsule_list_wld = capsule_list_local;
-            check_pair_mat = Eigen::MatrixXi::Ones(capsule_list_wld.size(),capsule_list_wld.size());
+            capsule_array_list_wld = capsule_array_list_local;
+            check_pair_mat = Eigen::MatrixXi::Ones(capsule_array_list_wld.size(),capsule_array_list_wld.size());
             for(int me=0;me<m_robot->numJoints();me++){
                 for(int you=me; you<m_robot->numJoints();you++){
                     if(me == you) check_pair_mat(me, you) = 0;
@@ -282,8 +305,10 @@ class CapsuleCollisionChecker {
         ~CapsuleCollisionChecker(){cerr<<"CapsuleCollisionChecker destructed"<<endl;}
         void update(){
             for(int i=0;i<m_robot->numJoints();i++){
-                capsule_list_wld[i].p0 = m_robot->joint(i)->p + m_robot->joint(i)->R * capsule_list_local[i].p0;
-                capsule_list_wld[i].p1 = m_robot->joint(i)->p + m_robot->joint(i)->R * capsule_list_local[i].p1;
+                for(int j=0;j<capsule_array_list_wld[i].size();j++){
+                capsule_array_list_wld[i][j].p0 = m_robot->joint(i)->p + m_robot->joint(i)->R * capsule_array_list_local[i][j].p0;
+                capsule_array_list_wld[i][j].p1 = m_robot->joint(i)->p + m_robot->joint(i)->R * capsule_array_list_local[i][j].p1;
+                }
             };
         }
         Point Vec3ToPoint(const hrp::Vector3& in){ return Point(in(0),in(1),in(2)); }
@@ -292,74 +317,45 @@ class CapsuleCollisionChecker {
             collision_info_list.clear();
             for(int me=0;me<m_robot->numJoints();me++){
                 for(int you=me; you<m_robot->numJoints();you++){
-//                    if(you == me) continue; //自分自身はスルー
-//                    if(m_robot->link(me)->parent == m_robot->link(you) || m_robot->link(you)->parent == m_robot->link(me) ) continue; //接続されたリンク同士は無視
-//                    if(std::count(ignore_list[me].begin(), ignore_list[me].end(), you)) continue;
-                    if(capsule_list_wld[me].r > 0 && capsule_list_wld[you].r > 0 && check_pair_mat(me,you)){
-                        Point cp0_ans, cp1_ans;
-                        double dist_ans = SegSegDist2(Vec3ToPoint(capsule_list_wld[me].p0), Vec3ToPoint(capsule_list_wld[me].p1-capsule_list_wld[me].p0),
-                                Vec3ToPoint(capsule_list_wld[you].p0), Vec3ToPoint(capsule_list_wld[you].p1-capsule_list_wld[you].p0), cp0_ans, cp1_ans);
-                        double dist_safe = capsule_list_wld[me].r + capsule_list_wld[you].r;
-                        if (dist_ans < dist_safe){
-                            hrp::Vector3 cp0_wld_tmp = hrp::Vector3(cp0_ans.x,cp0_ans.y,cp0_ans.z);
-                            hrp::Vector3 cp1_wld_tmp = hrp::Vector3(cp1_ans.x,cp1_ans.y,cp1_ans.z);
-                            collision_info_list.push_back(CollisionInfo(me, you,
-                                    m_robot->joint(me)->R.transpose() * (cp0_wld_tmp - m_robot->joint(me)->p), m_robot->joint(you)->R.transpose() * (cp1_wld_tmp - m_robot->joint(you)->p),
-                                    cp0_wld_tmp, cp1_wld_tmp, dist_safe, dist_ans));
+
+                    for(int mec=0;mec<capsule_array_list_wld[me].size();mec++){
+                        for(int youc=0;youc<capsule_array_list_wld[you].size();youc++){
+                            if(capsule_array_list_wld[me][mec].r > 0 && capsule_array_list_wld[you][youc].r > 0 && check_pair_mat(me,you)){
+                                Point cp0_ans, cp1_ans;
+                                double dist_ans = SegSegDist2(Vec3ToPoint(capsule_array_list_wld[me][mec].p0), Vec3ToPoint(capsule_array_list_wld[me][mec].p1-capsule_array_list_wld[me][mec].p0),
+                                        Vec3ToPoint(capsule_array_list_wld[you][youc].p0), Vec3ToPoint(capsule_array_list_wld[you][youc].p1-capsule_array_list_wld[you][youc].p0), cp0_ans, cp1_ans);
+                                double dist_safe = capsule_array_list_wld[me][mec].r + capsule_array_list_wld[you][youc].r;
+                                if (dist_ans < dist_safe){
+                                    hrp::Vector3 cp0_wld_tmp = hrp::Vector3(cp0_ans.x,cp0_ans.y,cp0_ans.z);
+                                    hrp::Vector3 cp1_wld_tmp = hrp::Vector3(cp1_ans.x,cp1_ans.y,cp1_ans.z);
+                                    collision_info_list.push_back(CollisionInfo(me, you,
+                                            m_robot->joint(me)->R.transpose() * (cp0_wld_tmp - m_robot->joint(me)->p), m_robot->joint(you)->R.transpose() * (cp1_wld_tmp - m_robot->joint(you)->p),
+                                            cp0_wld_tmp, cp1_wld_tmp, dist_safe, dist_ans));
+                                }
+                            }
                         }
                     }
+
                 }
             };
-            std::cout<<"pair:"<<std::endl;
-            for(int i=0;i<collision_info_list.size();i++){
-                std::cout<<m_robot->joint(collision_info_list[i].id0)->name<<" "<<m_robot->joint(collision_info_list[i].id1)->name<<endl;
-            }
+            return (collision_info_list.size() > 0 );
         }
-};
-
-//class CapsuleCollisionChecker {
-//    private:
-//        const hrp::BodyPtr m_robot;
-//    public:
-//        std::vector<hrp::Sphere> sphere_list;
-//        std::vector<std::pair<int, int> > collision_pair;
-//        CapsuleCollisionChecker(hrp::BodyPtr robot):
-//            m_robot(robot){
-//            sphere_list.resize(m_robot->numJoints());
-//            sphere_list[m_robot->link("RLEG_JOINT0")->jointId] = hrp::Sphere(hrp::Vector3(0,0,0), 0.1);
-//            sphere_list[m_robot->link("RARM_JOINT7")->jointId] = hrp::Sphere(hrp::Vector3(0,0,0), 0.1);
-//            sphere_list[m_robot->link("CHEST_JOINT2")->jointId] = hrp::Sphere(hrp::Vector3(0,0,0), 0.4);
-//            sphere_list[m_robot->link("RARM_JOINT4")->jointId] = hrp::Sphere(hrp::Vector3(0,0,0), 0.1);
-//            sphere_list[m_robot->link("RARM_JOINT7")->jointId] = hrp::Sphere(hrp::Vector3(0,0,0), 0.1);
-//            sphere_list[m_robot->link("LARM_JOINT4")->jointId] = hrp::Sphere(hrp::Vector3(0,0,0), 0.1);
-//            sphere_list[m_robot->link("LARM_JOINT7")->jointId] = hrp::Sphere(hrp::Vector3(0,0,0), 0.1);
-//
-//        }
-//        ~SphereCollisionChecker(){cerr<<"SphereCollisionChecker destructed"<<endl;}
-//        void update(){
-//            for(int i=0;i<m_robot->numJoints();i++){
-//                sphere_list[i].cur_pos = m_robot->joint(i)->p + m_robot->joint(i)->R * sphere_list[i].local_pos;
-//            };
-//        }
-//        bool checkCollision(){
-//            collision_pair.clear();
-//            for(int me=0;me<m_robot->numJoints()-1;me++){
-//                for(int you=me+1; you<m_robot->numJoints();you++){
-//                    if(m_robot->link(me)->parent == m_robot->link(you) || m_robot->link(you)->parent == m_robot->link(me) ) continue; //接続されたリンク同士は無視
-//
-//                    if(sphere_list[me].r > 0 && sphere_list[you].r > 0){
-//                        if((sphere_list[me].cur_pos - sphere_list[you].cur_pos).norm() < sphere_list[me].r + sphere_list[you].r){
-//                            collision_pair.push_back(std::make_pair(me,you));
-//                        }
-//                    }
+//        bool checkCapsuleArray2CapsuleArray(const Capsule){
+//            if(capsule_array_list_wld[me].r > 0 && capsule_array_list_wld[you].r > 0 && check_pair_mat(me,you)){
+//                Point cp0_ans, cp1_ans;
+//                double dist_ans = SegSegDist2(Vec3ToPoint(capsule_array_list_wld[me].p0), Vec3ToPoint(capsule_array_list_wld[me].p1-capsule_array_list_wld[me].p0),
+//                        Vec3ToPoint(capsule_array_list_wld[you].p0), Vec3ToPoint(capsule_array_list_wld[you].p1-capsule_array_list_wld[you].p0), cp0_ans, cp1_ans);
+//                double dist_safe = capsule_array_list_wld[me].r + capsule_array_list_wld[you].r;
+//                if (dist_ans < dist_safe){
+//                    hrp::Vector3 cp0_wld_tmp = hrp::Vector3(cp0_ans.x,cp0_ans.y,cp0_ans.z);
+//                    hrp::Vector3 cp1_wld_tmp = hrp::Vector3(cp1_ans.x,cp1_ans.y,cp1_ans.z);
+//                    collision_info_list.push_back(CollisionInfo(me, you,
+//                            m_robot->joint(me)->R.transpose() * (cp0_wld_tmp - m_robot->joint(me)->p), m_robot->joint(you)->R.transpose() * (cp1_wld_tmp - m_robot->joint(you)->p),
+//                            cp0_wld_tmp, cp1_wld_tmp, dist_safe, dist_ans));
 //                }
-//            };
-//            std::cout<<"pair:"<<std::endl;
-//            for(int i=0;i<collision_pair.size();i++){
-//                std::cout<<m_robot->joint(collision_pair[i].first)->name<<" "<<m_robot->joint(collision_pair[i].second)->name<<endl;
 //            }
 //        }
-//};
+};
 
 class WBMSCore : UTIL_CONST {
     private:
@@ -759,7 +755,7 @@ class WBMSCore : UTIL_CONST {
             LIMIT_MINMAX( out.tgt[lf].abs.p(Z), out.tgt[lf].offs.p(Z), out.tgt[lf].offs.p(Z)+WBMSparam.swing_foot_max_height);
 
 //            const double base2hand_min = 0.4;
-            const double base2hand_min = 0.3;
+            const double base2hand_min = 0.0;
             for(int i=0, l[2]={rh,lh}; i<2; i++){
                 //        LIMIT_MIN(out.tgt[l[i]].abs.p(X), baselinkpose.p(X));
                 //        LIMIT_MAX(out.tgt[l[i]].abs.p(Z), baselinkpose.p(Z) + 0.4);

From 7fadb705005de2e06345346ad3c04ca6e0fbd502 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 12 Oct 2017 21:53:45 +0900
Subject: [PATCH 140/217] tune capaolli

---
 .../FullbodyInverseKinematicsSolver.h         | 55 +++++++++----------
 .../WholeBodyMasterSlave.cpp                  | 28 ++++------
 rtc/WholeBodyMasterSlave/wbms_core.h          |  2 +-
 3 files changed, 39 insertions(+), 46 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 3ed852898bd..6fbf5f744a6 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -112,11 +112,10 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
             err_all = hrp::dvector::Zero(WS_DOF*_ikc_list.size());
             constraint_weight_all = hrp::dvector::Ones(WS_DOF*_ikc_list.size());
             //リファレンスに微少量戻す
-//            for(int i=0;i<J_DOF;i++){ _robot->joint(i)->q = _robot->joint(i)->q * ( 1 - q_ref_pullback_gain(i)) + q_ref(i) * q_ref_pullback_gain(i); }
             for(int i=0;i<J_DOF;i++){
                 double diff = q_ref(i) - _robot->joint(i)->q;
                 diff *= q_ref_pullback_gain(i);
-//                LIMIT_MINMAX(diff, -dq_ref_pullback(i), dq_ref_pullback(i));
+                LIMIT_MINMAX(diff, -dq_ref_pullback(i), dq_ref_pullback(i));
                 _robot->joint(i)->q += diff;
             }
             for(int i=0;i<J_DOF;i++){ LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit); }
@@ -166,32 +165,32 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
             constraint_weight_all = selection_mat * constraint_weight_all;
 
             hrp::dvector dq_weight_all_jlim = dq_weight_all;
-//            for ( int j = 0; j < _robot->numJoints() ; j++ ) {
-//                double jang = _robot->joint(j)->q;
-//                double jmax = _robot->joint(j)->ulimit;
-//                double jmin = _robot->joint(j)->llimit;
-//                double e = deg2rad(1);
-//                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
-//                } else if ( eps_eq(jang, jmax,e) ) {
-//                    jang = jmax - e;
-//                } else if ( eps_eq(jang, jmin,e) ) {
-//                    jang = jmin + e;
-//                }
-//                double r;
-//                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
-//                    r = DBL_MAX;
-//                } else {
-//                    r = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) /
-//                            (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
-//                    if (isnan(r)) r = 0;
-//                }
-//                if (( r - avoid_weight_gain(j) ) >= 0 ) {
-//                    dq_weight_all_jlim(j) *= ( 1.0 / ( 1.0 + r) );
-//                } else {//リミットから離れるときは解放
-//                    dq_weight_all_jlim(j) *= 1.0;
-//                }
-//                avoid_weight_gain(j) = r;
-//            }
+            for ( int j = 0; j < _robot->numJoints() ; j++ ) {
+                double jang = _robot->joint(j)->q;
+                double jmax = _robot->joint(j)->ulimit;
+                double jmin = _robot->joint(j)->llimit;
+                double e = deg2rad(1);
+                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+                } else if ( eps_eq(jang, jmax,e) ) {
+                    jang = jmax - e;
+                } else if ( eps_eq(jang, jmin,e) ) {
+                    jang = jmin + e;
+                }
+                double r;
+                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+                    r = DBL_MAX;
+                } else {
+                    r = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) /
+                            (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
+                    if (isnan(r)) r = 0;
+                }
+                if (( r - avoid_weight_gain(j) ) >= 0 ) {
+                    dq_weight_all_jlim(j) *= ( 1.0 / ( 1.0 + r) );
+                } else {//リミットから離れるときは解放
+                    dq_weight_all_jlim(j) *= 1.0;
+                }
+                avoid_weight_gain(j) = r;
+            }
 
             const double wn_const = 1e-6;
             const double auto_lambda_gain = 1;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 91a6b5663ca..04d4c0122a7 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -646,12 +646,10 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     }
     for(int i=0;i<sccp->collision_info_list.size();i++){
         IKConstraint tmp;
-//        tmp.constraint_weight << 1,1,1,0,0,0;
-        tmp.constraint_weight << 3,3,3,0,0,0;
-//        double val_w = (sccp->collision_info_list[i].dist_safe - sccp->collision_info_list[i].dist_cur)*1e3;
-//        LIMIT_MAX(val_w, 3);
-//        tmp.constraint_weight << val_w,val_w,val_w,0,0,0;
-//        double margin = 1e-3;
+        double val_w = (sccp->collision_info_list[i].dist_safe - sccp->collision_info_list[i].dist_cur)*1e2;
+        LIMIT_MAX(val_w, 3);
+        tmp.constraint_weight << val_w,val_w,val_w,0,0,0;
+//        tmp.constraint_weight << 3,3,3,0,0,0;
         double margin = 1e-3;
         if(sccp->avoid_priority(sccp->collision_info_list[i].id0) > sccp->avoid_priority(sccp->collision_info_list[i].id1)){
             tmp.localPos = sccp->collision_info_list[i].cp1_local;
@@ -673,8 +671,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
             tmp.targetPos = sccp->collision_info_list[i].cp1_wld + (sccp->collision_info_list[i].cp0_wld - sccp->collision_info_list[i].cp1_wld).normalized() * (sccp->collision_info_list[i].dist_safe + margin);
             ikc_list.push_back(tmp);
         }
-        ikc_list[3].constraint_weight =  hrp::dvector6::Constant(1e-4);
-        ikc_list[4].constraint_weight =  hrp::dvector6::Constant(1e-4);
+//        ikc_list[3].constraint_weight =  hrp::dvector6::Constant(1e-4);
+//        ikc_list[4].constraint_weight =  hrp::dvector6::Constant(1e-4);
     }
 
     if(loop%20==0){
@@ -708,10 +706,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     fik_in->dq_weight_all(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;
     fik_in->dq_weight_all(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
     fik_in->dq_weight_all(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
-    if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->ulimit = deg2rad(-30);//脇内側の干渉回避
-    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(30);
-//    if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->llimit = deg2rad(-140);//肩外側の干渉回避
-//    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->ulimit = deg2rad(140);
+//    if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->ulimit = deg2rad(-30);//脇内側の干渉回避
+//    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(30);
 
     for(int i=0;i<robot_in->numJoints();i++){
         LIMIT_MINMAX(robot_in->joint(i)->q, robot_in->joint(i)->llimit, robot_in->joint(i)->ulimit);
@@ -719,12 +715,10 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
 
     fik_in->q_ref = init_sync_state;
 
-    fik_in->q_ref_pullback_gain.fill(0.001);
-//    fik_in->dq_ref_pullback.fill(deg2rad(0.01));
-    fik_in->q_ref_pullback_gain.segment(6+6+3+2, 8*2).fill(0.01);//腕だけ
+//    fik_in->q_ref_pullback_gain.segment(6+6+3+2, 8*2).fill(0.01);//腕だけ
 //    fik_in->dq_ref_pullback.segment(6+6+3+2, 8*2).fill(deg2rad(0.1));//腕だけ
-
-//    fik_in->q_ref_pullback_gain.tail(6) << 0.0,0.0,0.0, 0.001,0.001,0.001;
+    fik_in->q_ref_pullback_gain.fill(1);
+    fik_in->dq_ref_pullback.fill(deg2rad(1)*m_dt);
 
     struct timespec startT, endT;
     const int IK_MAX_LOOP = 2;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index ee580603c88..772652e9773 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -260,7 +260,7 @@ class CapsuleCollisionChecker {
                     capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), m_robot->joint(i)->child->b, 0.09));
                 }
                 if(hasStr(m_robot->joint(i)->name,"ARM_JOINT7")){
-                    capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), hrp::Vector3(0,0,-0.25), 0.1));
+                    capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), hrp::Vector3(0,0,-0.35), 0.1));
                 }
                 if(hasStr(m_robot->joint(i)->name,"CHEST_JOINT1") || hasStr(m_robot->joint(i)->name,"CHEST_JOINT2")){
                     capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), m_robot->joint(i)->child->b, 0.2));

From e8b2dfcf8d761e21d20aac6917ce9031cc4be3c5 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 12 Oct 2017 21:58:00 +0900
Subject: [PATCH 141/217] disable max foot height

---
 rtc/WholeBodyMasterSlave/wbms_core.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 772652e9773..f91929e51eb 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -751,8 +751,8 @@ class WBMSCore : UTIL_CONST {
                 swing_leg.abs.p(X) = sp2sw_vec(X) + support_leg.cnt.p(X);
                 swing_leg.abs.p(Y) = sp2sw_vec(Y) + support_leg.cnt.p(Y);
             }
-            LIMIT_MINMAX( out.tgt[rf].abs.p(Z), out.tgt[rf].offs.p(Z), out.tgt[rf].offs.p(Z)+WBMSparam.swing_foot_max_height);
-            LIMIT_MINMAX( out.tgt[lf].abs.p(Z), out.tgt[lf].offs.p(Z), out.tgt[lf].offs.p(Z)+WBMSparam.swing_foot_max_height);
+//            LIMIT_MINMAX( out.tgt[rf].abs.p(Z), out.tgt[rf].offs.p(Z), out.tgt[rf].offs.p(Z)+WBMSparam.swing_foot_max_height);
+//            LIMIT_MINMAX( out.tgt[lf].abs.p(Z), out.tgt[lf].offs.p(Z), out.tgt[lf].offs.p(Z)+WBMSparam.swing_foot_max_height);
 
 //            const double base2hand_min = 0.4;
             const double base2hand_min = 0.0;

From a5e8d0eba4e6195aad178d887be15ca65ea470b3 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 18 Oct 2017 23:27:11 +0900
Subject: [PATCH 142/217] add knee stretch avoidance

---
 idl/WholeBodyMasterSlaveService.idl           | 17 -------------
 .../WholeBodyMasterSlave.cpp                  | 25 +++++++++++--------
 rtc/WholeBodyMasterSlave/wbms_core.h          |  4 ++-
 3 files changed, 18 insertions(+), 28 deletions(-)

diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
index ef3489c9b60..606f253c638 100644
--- a/idl/WholeBodyMasterSlaveService.idl
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -14,23 +14,6 @@ module OpenHRP
     typedef double DblArray4[4];
     typedef sequence<string> StrSequence;
 
-//    /**
-//     * @enum IKLimbParameters
-//     * @brief IKLimbParameters
-//     */
-//    struct IKLimbParameters {
-//        /// Joint weight for Inverse Kinematics calculation.
-//        sequence<double> ik_optional_weight_vector;
-//        /// SR-inverse gain for inverse kinematics.
-//        double sr_gain;
-//        /// Avoid joint limit gain for inverse kinematics.
-//        double avoid_gain;
-//        /// Reference joint angles tracking gain for inverse kinematics.
-//        double reference_gain;
-//        /// Manipulability limit for inverse kinematics.
-//        double manipulability_limit;
-//    };
-
     /**
      * HumanSync
      */
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 04d4c0122a7..63b7f9796ac 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -144,8 +144,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     nameServer = nameServer.substr(0, comPos);
     RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
     m_robot = hrp::BodyPtr(new hrp::Body());
-    m_robot_ml = hrp::BodyPtr(new hrp::Body());
-    m_robot_vsafe = hrp::BodyPtr(new hrp::Body());
+//    m_robot_ml = hrp::BodyPtr(new hrp::Body());
+//    m_robot_vsafe = hrp::BodyPtr(new hrp::Body());
     if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
         std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
         return RTC::RTC_ERROR;
@@ -260,7 +260,7 @@ void WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC:
     }
 }
 
-#define TIMECALC 1
+#define TIMECALC 0
 RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if(DEBUGP_ONCE)std::cerr << "[" << m_profile.instance_name<< "] onExecute(" << ec_id << ")" << std::endl;
     struct timeval t_calc_start, t_calc_end;
@@ -286,7 +286,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if ( is_legged_robot ) {
         processTransition();
         mode.update();
-        if(DEBUGP)dbg(mode.now());
+//        if(DEBUGP)dbg(mode.now());
         struct timespec startT, endT;
         clock_gettime(CLOCK_REALTIME, &startT);
 
@@ -327,8 +327,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
             if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ calcWorldZMPFromInverseDynamics" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
-            sccp->checkCollision();
-
             processHOFFARBIBFilter(m_robot, m_robot_vsafe);
 
             const hrp::BodyPtr m_robot_for_out = m_robot_vsafe;
@@ -644,6 +642,9 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     }else{
         sccp->avoid_priority.head(12).tail(6).fill(3);
     }
+
+//    sccp->checkCollision();
+
     for(int i=0;i<sccp->collision_info_list.size();i++){
         IKConstraint tmp;
         double val_w = (sccp->collision_info_list[i].dist_safe - sccp->collision_info_list[i].dist_cur)*1e2;
@@ -693,7 +694,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.targetPos = com_ref.p;// COM height will not be constraint
         tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
 //        tmp.constraint_weight << 10,10,1,1e-6,1e-6,1e-6;
-        tmp.constraint_weight << 10,10,1,0,0,0;
+        tmp.constraint_weight << 10,10,0.001,0,0,0;
 //        if(fik_in->cur_momentum_around_COM.norm() > 1e9){
 //            tmp.constraint_weight << 10,10,1,1e-5,1e-5,1e-10;
 //            tmp.rot_precision = 100;//angular momentum precision
@@ -703,11 +704,15 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         ikc_list.push_back(tmp);
     }
 
-    fik_in->dq_weight_all(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;
-    fik_in->dq_weight_all(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
-    fik_in->dq_weight_all(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
+    if( robot_in->link("CHEST_JOINT0") != NULL) fik_in->dq_weight_all(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;//JAXON
+    if( robot_in->link("CHEST_JOINT1") != NULL) fik_in->dq_weight_all(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
+    if( robot_in->link("CHEST_JOINT2") != NULL) fik_in->dq_weight_all(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
+    if( robot_in->link("CHEST_Y") != NULL) fik_in->dq_weight_all(robot_in->link("CHEST_Y")->jointId) = 0.1;//K
+    if( robot_in->link("CHEST_P") != NULL) fik_in->dq_weight_all(robot_in->link("CHEST_P")->jointId) = 0.1;
 //    if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->ulimit = deg2rad(-30);//脇内側の干渉回避
 //    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(30);
+    if( m_robot->link("RLEG_JOINT3") != NULL) m_robot->link("RLEG_JOINT3")->llimit = deg2rad(10);//膝伸びきり防止のため
+    if( m_robot->link("LLEG_JOINT3") != NULL) m_robot->link("LLEG_JOINT3")->llimit = deg2rad(10);
 
     for(int i=0;i<robot_in->numJoints();i++){
         LIMIT_MINMAX(robot_in->joint(i)->q, robot_in->joint(i)->llimit, robot_in->joint(i)->ulimit);
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index f91929e51eb..ad3f5ad96ab 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -283,7 +283,8 @@ class CapsuleCollisionChecker {
                     if(m_robot->joint(me)->name.find("LLEG_JOINT") != std::string::npos && m_robot->joint(you)->name.find("LLEG_JOINT") != std::string::npos) check_pair_mat(me, you) = 0;
                     if(m_robot->joint(me)->name.find("RARM_JOINT") != std::string::npos && m_robot->joint(you)->name.find("RARM_JOINT") != std::string::npos) check_pair_mat(me, you) = 0;
                     if(m_robot->joint(me)->name.find("LARM_JOINT") != std::string::npos && m_robot->joint(you)->name.find("LARM_JOINT") != std::string::npos) check_pair_mat(me, you) = 0;
-                    if(m_robot->link(me)->parent == m_robot->link(you) || m_robot->link(you)->parent == m_robot->link(me)) check_pair_mat(me, you) = 0;
+//                    if(m_robot->link(me)->parent == m_robot->link(you) || m_robot->link(you)->parent == m_robot->link(me)) check_pair_mat(me, you) = 0;//実機とシミュで挙動違う？
+                    if(m_robot->joint(me)->parent == m_robot->joint(you) || m_robot->joint(you)->parent == m_robot->joint(me)) check_pair_mat(me, you) = 0;
 
                     if(m_robot->joint(me)->name.find("LEG_JOINT0") != std::string::npos && m_robot->joint(you)->name.find("CHEST_JOINT") != std::string::npos) check_pair_mat(me, you) = 0;
                     if(m_robot->joint(me)->name.find("LEG_JOINT1") != std::string::npos && m_robot->joint(you)->name.find("CHEST_JOINT") != std::string::npos) check_pair_mat(me, you) = 0;
@@ -293,6 +294,7 @@ class CapsuleCollisionChecker {
                     if(m_robot->joint(me)->name.find("CHEST_JOINT") != std::string::npos && m_robot->joint(you)->name.find("ARM_JOINT2") != std::string::npos) check_pair_mat(me, you) = 0;
                 }
             }
+            dbgn(check_pair_mat);
             avoid_priority = hrp::ivector::Zero(m_robot->numJoints());
 
             for(int i=0;i<m_robot->numJoints();i++){

From aca0df7269473051a545ecee7fe1cdc5e9f96514 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 23 Oct 2017 22:59:31 +0900
Subject: [PATCH 143/217] fix abc

---
 rtc/AutoBalancer/AutoBalancer.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index e9de730b952..5e224b10dcd 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1143,9 +1143,11 @@ void AutoBalancer::solveFullbodyIK ()
   if( m_robot->link("RARM_JOINT2") != NULL) m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇の干渉回避のため
   if( m_robot->link("LARM_JOINT2") != NULL) m_robot->link("LARM_JOINT2")->llimit = deg2rad(40);
 
-  fik->dq_weight_all(m_robot->link("CHEST_JOINT0")->jointId) = 0.1;
-  fik->dq_weight_all(m_robot->link("CHEST_JOINT1")->jointId) = 0.1;
-  fik->dq_weight_all(m_robot->link("CHEST_JOINT2")->jointId) = 0.1;
+  if( m_robot->link("CHEST_JOINT0") != NULL) fik->dq_weight_all(m_robot->link("CHEST_JOINT0")->jointId) = 0.1;
+  if( m_robot->link("CHEST_JOINT1") != NULL) fik->dq_weight_all(m_robot->link("CHEST_JOINT1")->jointId) = 0.1;
+  if( m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(m_robot->link("CHEST_JOINT2")->jointId) = 0.1;
+  if( m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(m_robot->link("CHEST_Y")->jointId) = 0.1;
+  if( m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(m_robot->link("CHEST_Y")->jointId) = 0.1;
 
   for(int i=0;i<m_robot->numJoints();i++) fik->q_ref(i) = m_qRef.data[i];
   fik->q_ref_pullback_gain.head(m_robot->numJoints()).fill(0.001);

From 9c077030efc32f62b841c22cea51a5042bce229b Mon Sep 17 00:00:00 2001
From: "RobustHumanoid@JSK" <robust-humanoid@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 9 Nov 2017 17:40:42 +0900
Subject: [PATCH 144/217] tune for khi

---
 .../WholeBodyMasterSlave.cpp                  | 37 ++++++++++++++++---
 1 file changed, 32 insertions(+), 5 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 63b7f9796ac..ed6d394e660 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -194,7 +194,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     invdyn_zmp_filters.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
     final_ref_zmp_filter.setParameter(5, 1/m_dt, Q_BUTTERWORTH);
 
-    avg_q_vel = 4;
+    avg_q_vel = 1;
 
     sccp = boost::shared_ptr<CapsuleCollisionChecker>(new CapsuleCollisionChecker(m_robot));
 
@@ -589,7 +589,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
             tmp.constraint_weight = hrp::dvector6::Constant(0.1);
         }
         ikc_list.push_back(tmp);
-    }{
+    }
+    {
         IKConstraint tmp;
         tmp.target_link_name = eename_ikcp_map["lleg"].target_link_name;
         tmp.localPos = eename_ikcp_map["lleg"].localPos;
@@ -602,12 +603,18 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
             tmp.constraint_weight = hrp::dvector6::Constant(0.1);
         }
         ikc_list.push_back(tmp);
-    }{
+    }
+    const double dist = 0.65;
+    {
         IKConstraint tmp;
         tmp.target_link_name = eename_ikcp_map["rarm"].target_link_name;
         tmp.localPos = eename_ikcp_map["rarm"].localPos;
         tmp.localR = eename_ikcp_map["rarm"].localR;
-        tmp.targetPos = rh_ref.p;
+        if((rh_ref.p-lh_ref.p).norm()<dist){
+            tmp.targetPos = (rh_ref.p+lh_ref.p)/2 + (rh_ref.p-lh_ref.p).normalized()*dist/2;
+        }else{
+            tmp.targetPos = rh_ref.p;
+        }
         tmp.targetRpy = rh_ref.rpy;
         tmp.constraint_weight = hrp::dvector6::Constant(0.1);
         tmp.pos_precision = 3e-3;
@@ -618,7 +625,11 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.target_link_name = eename_ikcp_map["larm"].target_link_name;
         tmp.localPos = eename_ikcp_map["larm"].localPos;
         tmp.localR = eename_ikcp_map["larm"].localR;
-        tmp.targetPos = lh_ref.p;
+        if((rh_ref.p-lh_ref.p).norm()<dist){
+            tmp.targetPos = (rh_ref.p+lh_ref.p)/2 + (lh_ref.p-rh_ref.p).normalized()*dist/2;
+        }else{
+            tmp.targetPos = lh_ref.p;
+        }
         tmp.targetRpy = lh_ref.rpy;
         tmp.constraint_weight = hrp::dvector6::Constant(0.1);
         tmp.pos_precision = 3e-3;
@@ -713,6 +724,22 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
 //    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(30);
     if( m_robot->link("RLEG_JOINT3") != NULL) m_robot->link("RLEG_JOINT3")->llimit = deg2rad(10);//膝伸びきり防止のため
     if( m_robot->link("LLEG_JOINT3") != NULL) m_robot->link("LLEG_JOINT3")->llimit = deg2rad(10);
+    if( m_robot->link("R_KNEE_P") != NULL) m_robot->link("R_KNEE_P")->llimit = deg2rad(15);//K
+    if( m_robot->link("L_KNEE_P") != NULL) m_robot->link("L_KNEE_P")->llimit = deg2rad(15);
+    if( m_robot->link("R_WRIST_R") != NULL) m_robot->link("R_WRIST_P")->llimit = deg2rad(-40);
+    if( m_robot->link("L_WRIST_R") != NULL) m_robot->link("L_WRIST_P")->llimit = deg2rad(-40);
+    if( m_robot->link("R_WRIST_R") != NULL) m_robot->link("R_WRIST_P")->ulimit = deg2rad(40);
+    if( m_robot->link("L_WRIST_R") != NULL) m_robot->link("L_WRIST_P")->ulimit = deg2rad(40);
+    if( m_robot->link("R_WRIST_P") != NULL) m_robot->link("R_WRIST_P")->llimit = deg2rad(-40);
+    if( m_robot->link("L_WRIST_P") != NULL) m_robot->link("L_WRIST_P")->llimit = deg2rad(-40);
+    if( m_robot->link("R_WRIST_P") != NULL) m_robot->link("R_WRIST_P")->ulimit = deg2rad(20);
+    if( m_robot->link("L_WRIST_P") != NULL) m_robot->link("L_WRIST_P")->ulimit = deg2rad(20);
+
+
+    if( m_robot->link("CHEST_Y") != NULL) m_robot->link("CHEST_Y")->llimit = deg2rad(-20);
+    if( m_robot->link("CHEST_Y") != NULL) m_robot->link("CHEST_Y")->ulimit = deg2rad(20);
+    if( m_robot->link("CHEST_P") != NULL) m_robot->link("CHEST_P")->llimit = deg2rad(0);
+    if( m_robot->link("CHEST_P") != NULL) m_robot->link("CHEST_P")->ulimit = deg2rad(30);
 
     for(int i=0;i<robot_in->numJoints();i++){
         LIMIT_MINMAX(robot_in->joint(i)->q, robot_in->joint(i)->llimit, robot_in->joint(i)->ulimit);

From b4d43fdd1ff916400cb2f1b9b40c84e10f4dcfad Mon Sep 17 00:00:00 2001
From: "RobustHumanoid@JSK" <robust-humanoid@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 17 Nov 2017 19:08:54 +0900
Subject: [PATCH 145/217] fix Roll Pitch miss

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index ed6d394e660..66b85b95ecd 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -726,10 +726,10 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     if( m_robot->link("LLEG_JOINT3") != NULL) m_robot->link("LLEG_JOINT3")->llimit = deg2rad(10);
     if( m_robot->link("R_KNEE_P") != NULL) m_robot->link("R_KNEE_P")->llimit = deg2rad(15);//K
     if( m_robot->link("L_KNEE_P") != NULL) m_robot->link("L_KNEE_P")->llimit = deg2rad(15);
-    if( m_robot->link("R_WRIST_R") != NULL) m_robot->link("R_WRIST_P")->llimit = deg2rad(-40);
-    if( m_robot->link("L_WRIST_R") != NULL) m_robot->link("L_WRIST_P")->llimit = deg2rad(-40);
-    if( m_robot->link("R_WRIST_R") != NULL) m_robot->link("R_WRIST_P")->ulimit = deg2rad(40);
-    if( m_robot->link("L_WRIST_R") != NULL) m_robot->link("L_WRIST_P")->ulimit = deg2rad(40);
+    if( m_robot->link("R_WRIST_R") != NULL) m_robot->link("R_WRIST_R")->llimit = deg2rad(-40);
+    if( m_robot->link("L_WRIST_R") != NULL) m_robot->link("L_WRIST_R")->llimit = deg2rad(-40);
+    if( m_robot->link("R_WRIST_R") != NULL) m_robot->link("R_WRIST_R")->ulimit = deg2rad(40);
+    if( m_robot->link("L_WRIST_R") != NULL) m_robot->link("L_WRIST_R")->ulimit = deg2rad(40);
     if( m_robot->link("R_WRIST_P") != NULL) m_robot->link("R_WRIST_P")->llimit = deg2rad(-40);
     if( m_robot->link("L_WRIST_P") != NULL) m_robot->link("L_WRIST_P")->llimit = deg2rad(-40);
     if( m_robot->link("R_WRIST_P") != NULL) m_robot->link("R_WRIST_P")->ulimit = deg2rad(20);

From ed23a94fbb67cc2488dbbb73f0b9f93e5f34fe4a Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 15 Apr 2019 12:26:21 +0900
Subject: [PATCH 146/217] disable lower limbs

---
 .../FullbodyInverseKinematicsSolver.h         |   2 +-
 .../WholeBodyMasterSlave.cpp                  | 115 +++++++++++-------
 2 files changed, 71 insertions(+), 46 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 6fbf5f744a6..f659355c7d3 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -200,7 +200,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
             hrp::dmatrix H = J_all.transpose() * constraint_weight_all.asDiagonal() * J_all + Wn;
             hrp::dvector g = J_all.transpose() * constraint_weight_all.asDiagonal() * err_all;
 
-            dq_all = H.inverse() * g;
+            dq_all = H.ldlt().solve(g);
 
             static int count;
             if(count++ % 10000 == 0){
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 66b85b95ecd..0a578a0d4ee 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -571,40 +571,43 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.target_link_name = "WAIST";
         tmp.localPos = hrp::Vector3::Zero();
         tmp.localR = hrp::Matrix33::Identity();
-        tmp.targetPos = robot_in->rootLink()->p;// will be ignored by selection_vec
-        tmp.targetRpy = com_ref.rpy;// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
-        tmp.constraint_weight << 0,0,0,1,1,1;
+//        tmp.targetPos = robot_in->rootLink()->p;// will be ignored by selection_vec
+//        tmp.targetRpy = com_ref.rpy;// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
+//        tmp.constraint_weight << 0,0,0,1,1,1;
+        tmp.targetPos << m_basePos.data.x,m_basePos.data.y,m_basePos.data.z;// will be ignored by selection_vec
+        tmp.targetRpy << m_baseRpy.data.r,m_baseRpy.data.p,m_baseRpy.data.y;// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
+        tmp.constraint_weight << 1,1,1,1,1,1;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
     }{
-        IKConstraint tmp;
-        tmp.target_link_name = eename_ikcp_map["rleg"].target_link_name;
-        tmp.localPos = eename_ikcp_map["rleg"].localPos;
-        tmp.localR = eename_ikcp_map["rleg"].localR;
-        tmp.targetPos = rf_ref.p;
-        tmp.targetRpy = rf_ref.rpy;
-        if(hsp->rp_ref_out.tgt[rf].is_contact){
-            tmp.constraint_weight = hrp::dvector6::Constant(10);
-        }else{
-            tmp.constraint_weight = hrp::dvector6::Constant(0.1);
-        }
-        ikc_list.push_back(tmp);
+//        IKConstraint tmp;
+//        tmp.target_link_name = eename_ikcp_map["rleg"].target_link_name;
+//        tmp.localPos = eename_ikcp_map["rleg"].localPos;
+//        tmp.localR = eename_ikcp_map["rleg"].localR;
+//        tmp.targetPos = rf_ref.p;
+//        tmp.targetRpy = rf_ref.rpy;
+//        if(hsp->rp_ref_out.tgt[rf].is_contact){
+//            tmp.constraint_weight = hrp::dvector6::Constant(10);
+//        }else{
+//            tmp.constraint_weight = hrp::dvector6::Constant(0.1);
+//        }
+//        ikc_list.push_back(tmp);
     }
     {
-        IKConstraint tmp;
-        tmp.target_link_name = eename_ikcp_map["lleg"].target_link_name;
-        tmp.localPos = eename_ikcp_map["lleg"].localPos;
-        tmp.localR = eename_ikcp_map["lleg"].localR;
-        tmp.targetPos = lf_ref.p;
-        tmp.targetRpy = lf_ref.rpy;
-        if(hsp->rp_ref_out.tgt[lf].is_contact){
-            tmp.constraint_weight = hrp::dvector6::Constant(10);
-        }else{
-            tmp.constraint_weight = hrp::dvector6::Constant(0.1);
-        }
-        ikc_list.push_back(tmp);
+//        IKConstraint tmp;
+//        tmp.target_link_name = eename_ikcp_map["lleg"].target_link_name;
+//        tmp.localPos = eename_ikcp_map["lleg"].localPos;
+//        tmp.localR = eename_ikcp_map["lleg"].localR;
+//        tmp.targetPos = lf_ref.p;
+//        tmp.targetRpy = lf_ref.rpy;
+//        if(hsp->rp_ref_out.tgt[lf].is_contact){
+//            tmp.constraint_weight = hrp::dvector6::Constant(10);
+//        }else{
+//            tmp.constraint_weight = hrp::dvector6::Constant(0.1);
+//        }
+//        ikc_list.push_back(tmp);
     }
-    const double dist = 0.65;
+    const double dist = 0.0;
     {
         IKConstraint tmp;
         tmp.target_link_name = eename_ikcp_map["rarm"].target_link_name;
@@ -642,6 +645,13 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.constraint_weight << 0,0,0,0,0.1,0.1;
         tmp.rot_precision = deg2rad(1);
         ikc_list.push_back(tmp);
+    }if(robot_in->link("HEAD_P") != NULL){
+        IKConstraint tmp;
+        tmp.target_link_name = "HEAD_P";
+        tmp.targetRpy = head_ref.rpy;
+        tmp.constraint_weight << 0,0,0,0,0.1,0.1;
+        tmp.rot_precision = deg2rad(1);
+        ikc_list.push_back(tmp);
     }
     if(hsp->rp_ref_out.tgt[rf].is_contact){
         sccp->avoid_priority.head(12).head(6).fill(4);
@@ -697,23 +707,23 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     }
 
 
-    {
-        IKConstraint tmp;
-        tmp.target_link_name = "COM";
-        tmp.localPos = hrp::Vector3::Zero();
-        tmp.localR = hrp::Matrix33::Identity();
-        tmp.targetPos = com_ref.p;// COM height will not be constraint
-        tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
-//        tmp.constraint_weight << 10,10,1,1e-6,1e-6,1e-6;
-        tmp.constraint_weight << 10,10,0.001,0,0,0;
-//        if(fik_in->cur_momentum_around_COM.norm() > 1e9){
-//            tmp.constraint_weight << 10,10,1,1e-5,1e-5,1e-10;
-//            tmp.rot_precision = 100;//angular momentum precision
-//        }else{
-//            tmp.constraint_weight << 10,10,1,0,0,0;
-//        }
-        ikc_list.push_back(tmp);
-    }
+//    {
+//        IKConstraint tmp;
+//        tmp.target_link_name = "COM";
+//        tmp.localPos = hrp::Vector3::Zero();
+//        tmp.localR = hrp::Matrix33::Identity();
+//        tmp.targetPos = com_ref.p;// COM height will not be constraint
+//        tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
+////        tmp.constraint_weight << 10,10,1,1e-6,1e-6,1e-6;
+//        tmp.constraint_weight << 10,10,0.001,0,0,0;
+////        if(fik_in->cur_momentum_around_COM.norm() > 1e9){
+////            tmp.constraint_weight << 10,10,1,1e-5,1e-5,1e-10;
+////            tmp.rot_precision = 100;//angular momentum precision
+////        }else{
+////            tmp.constraint_weight << 10,10,1,0,0,0;
+////        }
+//        ikc_list.push_back(tmp);
+//    }
 
     if( robot_in->link("CHEST_JOINT0") != NULL) fik_in->dq_weight_all(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;//JAXON
     if( robot_in->link("CHEST_JOINT1") != NULL) fik_in->dq_weight_all(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
@@ -747,6 +757,21 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
 
     fik_in->q_ref = init_sync_state;
 
+
+    std::string tmp[] = {"R_CROTCH_R","R_CROTCH_P","R_CROTCH_Y","R_KNEE_P","R_ANKLE_R","R_ANKLE_P","L_CROTCH_R","L_CROTCH_P","L_CROTCH_Y","L_KNEE_P","L_ANKLE_R","L_ANKLE_P"};
+    const std::vector<std::string> list(tmp, tmp+12);
+    for(int i=0; i<list.size(); i++){
+      if( robot_in->link(list[i]) != NULL){
+        int j=robot_in->link(list[i])->jointId;
+        fik_in->dq_weight_all(j) = 1e-5;
+        robot_in->joint(j)->q = m_qRef.data[j];
+        fik_in->q_ref(j) = m_qRef.data[j];
+      }
+    }
+    robot_in->rootLink()->p << m_basePos.data.x,m_basePos.data.y,m_basePos.data.z;// will be ignored by selection_vec
+    robot_in->rootLink()->R = hrp::rotFromRpy(hrp::Vector3(m_baseRpy.data.r,m_baseRpy.data.p,m_baseRpy.data.y));// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
+
+
 //    fik_in->q_ref_pullback_gain.segment(6+6+3+2, 8*2).fill(0.01);//腕だけ
 //    fik_in->dq_ref_pullback.segment(6+6+3+2, 8*2).fill(deg2rad(0.1));//腕だけ
     fik_in->q_ref_pullback_gain.fill(1);

From 64016e23b72963089475fc8e81d5a75798de98cc Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 16 Apr 2019 19:58:20 +0900
Subject: [PATCH 147/217] revert AutoBalancer files to origin/master

---
 rtc/AutoBalancer/AutoBalancer.cpp | 154 ++++++------------------------
 rtc/AutoBalancer/AutoBalancer.h   |   7 +-
 2 files changed, 32 insertions(+), 129 deletions(-)

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
index 4fab04b6a75..f07206e3f15 100644
--- a/rtc/AutoBalancer/AutoBalancer.cpp
+++ b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -124,19 +124,19 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     addOutPort("cogOut", m_cogOut);
     addOutPort("walkingStates", m_walkingStatesOut);
     addOutPort("sbpCogOffset", m_sbpCogOffsetOut);
-
+  
     // Set service provider to Ports
     m_AutoBalancerServicePort.registerProvider("service0", "AutoBalancerService", m_service0);
-
+  
     // Set service consumers to Ports
-
+  
     // Set CORBA Service Ports
     addPort(m_AutoBalancerServicePort);
-
+  
     // </rtc-template>
     // <rtc-template block="bind_config">
     // Bind variables and configuration variable
-
+  
     // </rtc-template>
 
     RTC::Properties& prop =  getProperties();
@@ -173,8 +173,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     leg_names.push_back("lleg");
 
     // Generate FIK
-//    fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
-    fik = fikPtr(new FullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
+    fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
 
     // setting from conf file
     // rleg,TARGET_LINK,BASE_LINK
@@ -413,7 +412,7 @@ RTC::ReturnCode_t AutoBalancer::onFinalize()
 RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id)
 {
     std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
-
+    
     return RTC::RTC_OK;
 }
 
@@ -497,7 +496,6 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     hrp::Vector3 ref_basePos;
     hrp::Matrix33 ref_baseRot;
     hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
-
     if ( is_legged_robot ) {
       // For parameters
       fik->storeCurrentParameters();
@@ -1079,121 +1077,29 @@ void AutoBalancer::fixLegToCoords2 (coordinates& tmp_fix_coords)
 
 void AutoBalancer::solveFullbodyIK ()
 {
-    static hrp::dvector q_old = hrp::getQAll(m_robot);
-    static hrp::Vector3 base_p_old = m_robot->rootLink()->p;
-    static hrp::Matrix33 base_R_old = m_robot->rootLink()->R;
-    std::vector<IKConstraint> ik_tgt_list;
-    {
-        IKConstraint tmp;
-        tmp.target_link_name = "WAIST";
-        tmp.localPos = hrp::Vector3::Zero();
-        tmp.localR = hrp::Matrix33::Identity();
-        tmp.targetPos = target_root_p;// will be ignored by selection_vec
-        tmp.targetRpy = hrp::rpyFromRot(target_root_R);// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
-        tmp.constraint_weight << 0,0,0,1,1,1;
-        ik_tgt_list.push_back(tmp);
-    }{
-        IKConstraint tmp;
-        tmp.target_link_name = ikp["rleg"].target_link->name;
-        tmp.localPos = ikp["rleg"].localPos;
-        tmp.localR = ikp["rleg"].localR;
-        tmp.targetPos = ikp["rleg"].target_p0;
-        tmp.targetRpy = hrp::rpyFromRot(ikp["rleg"].target_r0);
-        tmp.constraint_weight << 1,1,3,3,3,1;
-        ik_tgt_list.push_back(tmp);
-    }{
-        IKConstraint tmp;
-        tmp.target_link_name = ikp["lleg"].target_link->name;
-        tmp.localPos = ikp["lleg"].localPos;
-        tmp.localR = ikp["lleg"].localR;
-        tmp.targetPos = ikp["lleg"].target_p0;
-        tmp.targetRpy = hrp::rpyFromRot(ikp["lleg"].target_r0);
-        tmp.constraint_weight << 1,1,3,3,3,1;
-        ik_tgt_list.push_back(tmp);
-    }{
-//        IKConstraint tmp;
-//        tmp.target_link_name = ikp["rarm"].target_link->name;
-//        tmp.localPos = ikp["rarm"].localPos;
-//        tmp.localR = ikp["rarm"].localR;
-//        tmp.targetPos = ikp["rarm"].target_p0;
-//        tmp.targetRpy = hrp::rpyFromRot(ikp["rarm"].target_r0);
-//        tmp.constraint_weight << 1,1,1,1,1,1;
-//        ik_tgt_list.push_back(tmp);
-    }{
-//        IKConstraint tmp;
-//        tmp.target_link_name = ikp["larm"].target_link->name;
-//        tmp.localPos = ikp["larm"].localPos;
-//        tmp.localR = ikp["larm"].localR;
-//        tmp.targetPos = ikp["larm"].target_p0;
-//        tmp.targetRpy = hrp::rpyFromRot(ikp["larm"].target_r0);
-//        tmp.constraint_weight << 1,1,1,1,1,1;
-//        ik_tgt_list.push_back(tmp);
-    }{
-        IKConstraint tmp;
-        tmp.target_link_name = "COM";
-        tmp.localPos = hrp::Vector3::Zero();
-        tmp.localR = hrp::Matrix33::Identity();
-        tmp.targetPos = ref_cog;// COM height will not be constraint
-        tmp.targetRpy = hrp::Vector3(0, 0, 0);//reference angular momentum
-        //  tmp.targetRpy = hrp::Vector3(0, 10, 0);//reference angular momentum
-        tmp.constraint_weight << 3,3,1,1e-6,1e-6,1e-6;
-//        tmp.constraint_weight << 3,3,1,0,0,0;
-        tmp.rot_precision = 1e-1;//angular momentum precision
-        ik_tgt_list.push_back(tmp);
-    }
-
-  if( m_robot->link("RARM_JOINT2") != NULL) m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-40);//脇の干渉回避のため
-  if( m_robot->link("LARM_JOINT2") != NULL) m_robot->link("LARM_JOINT2")->llimit = deg2rad(40);
-
-  if( m_robot->link("CHEST_JOINT0") != NULL) fik->dq_weight_all(m_robot->link("CHEST_JOINT0")->jointId) = 0.1;
-  if( m_robot->link("CHEST_JOINT1") != NULL) fik->dq_weight_all(m_robot->link("CHEST_JOINT1")->jointId) = 0.1;
-  if( m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(m_robot->link("CHEST_JOINT2")->jointId) = 0.1;
-  if( m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(m_robot->link("CHEST_Y")->jointId) = 0.1;
-  if( m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(m_robot->link("CHEST_Y")->jointId) = 0.1;
-
-  for(int i=0;i<m_robot->numJoints();i++) fik->q_ref(i) = m_qRef.data[i];
-  fik->q_ref_pullback_gain.head(m_robot->numJoints()).fill(0.001);
-
-  hrp::setQAll(m_robot, q_old);
-  m_robot->rootLink()->p = base_p_old;
-  m_robot->rootLink()->R = base_R_old;
-
-  int loop_result = 0;
-  const int IK_MAX_LOOP = 1;
-  loop_result = fik->solveFullbodyIKLoop(m_robot, ik_tgt_list, IK_MAX_LOOP);
-
-  q_old = hrp::getQAll(m_robot);
-  base_p_old = m_robot->rootLink()->p;
-  base_R_old = m_robot->rootLink()->R;
+  // Set ik target params
+  fik->target_root_p = target_root_p;
+  fik->target_root_R = target_root_R;
+  for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
+      it->second.target_p0 = ikp[it->first].target_p0;
+      it->second.target_r0 = ikp[it->first].target_r0;
+  }
+  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
+//  fik->current_tm = m_qRef.tm;
+  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
+      fik->ikp[it->first].is_ik_enable = it->second.is_active;
+  }
+  // Revert
+  fik->revertRobotStateToCurrent();
+  // TODO : SBP calculation is outside of solve ik?
+  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
+  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
+  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
+  // Solve IK
+  fik->solveFullbodyIK (dif_cog, transition_interpolator->isEmpty());
 }
 
 
-//void AutoBalancer::solveFullbodyIK ()
-//{
-//  // Set ik target params
-//  fik->target_root_p = target_root_p;
-//  fik->target_root_R = target_root_R;
-//  for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
-//      it->second.target_p0 = ikp[it->first].target_p0;
-//      it->second.target_r0 = ikp[it->first].target_r0;
-//  }
-//  fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
-////  fik->current_tm = m_qRef.tm;
-//  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
-//      fik->ikp[it->first].is_ik_enable = it->second.is_active;
-//  }
-//  // Revert
-//  fik->revertRobotStateToCurrent();
-//  // TODO : SBP calculation is outside of solve ik?
-//  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
-//  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
-//  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
-//  // Solve IK
-//  fik->solveFullbodyIK (dif_cog, transition_interpolator->isEmpty());
-//}
-
-
-
 /*
   RTC::ReturnCode_t AutoBalancer::onAborting(RTC::UniqueId ec_id)
   {
@@ -1609,7 +1515,7 @@ bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::Gai
   gg->set_swing_trajectory_final_distance_weight(i_param.swing_trajectory_final_distance_weight);
   gg->set_swing_trajectory_time_offset_xy2z(i_param.swing_trajectory_time_offset_xy2z);
   gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2]));
-  gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2]));
+  gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2]));  
   gg->set_gravitational_acceleration(i_param.gravitational_acceleration);
   gg->set_toe_angle(i_param.toe_angle);
   gg->set_heel_angle(i_param.heel_angle);
@@ -2022,11 +1928,11 @@ bool AutoBalancer::adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep&
       //   Input : ee coords
       //   Output : link coords
       memcpy(eepos.data(), rfootstep.pos, sizeof(double)*3);
-      eerot = (Eigen::Quaternion<double>(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc:
+      eerot = (Eigen::Quaternion<double>(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
       ikp["rleg"].adjust_interpolation_target_r0 = eerot;
       ikp["rleg"].adjust_interpolation_target_p0 = eepos;
       memcpy(eepos.data(), lfootstep.pos, sizeof(double)*3);
-      eerot = (Eigen::Quaternion<double>(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc:
+      eerot = (Eigen::Quaternion<double>(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: 
       ikp["lleg"].adjust_interpolation_target_r0 = eerot;
       ikp["lleg"].adjust_interpolation_target_p0 = eepos;
       mid_coords(target_mid_coords, 0.5,
diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
index 3f33bf9ab74..af192b3559f 100644
--- a/rtc/AutoBalancer/AutoBalancer.h
+++ b/rtc/AutoBalancer/AutoBalancer.h
@@ -28,9 +28,7 @@
 #include "AutoBalancerService_impl.h"
 #include "interpolator.h"
 #include "../TorqueFilter/IIRFilter.h"
-
-//#include "SimpleFullbodyInverseKinematicsSolver.h"
-#include "FullbodyInverseKinematicsSolver.h"
+#include "SimpleFullbodyInverseKinematicsSolver.h"
 
 // </rtc-template>
 
@@ -248,8 +246,7 @@ class AutoBalancer
   ggPtr gg;
   bool gg_is_walking, gg_solved;
   // for abc
-  typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
-//  typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
+  typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
   fikPtr fik;
   hrp::Vector3 ref_cog, ref_zmp, prev_ref_zmp, prev_imu_sensor_pos, prev_imu_sensor_vel, hand_fix_initial_offset;
   enum {BIPED, TROT, PACE, CRAWL, GALLOP} gait_type;

From 5f1224a201edc9991f4ca7fd550cf13d3ac06ac2 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 16 Apr 2019 23:49:57 +0900
Subject: [PATCH 148/217] tune for demo

---
 idl/WholeBodyMasterSlaveService.idl           |  1 +
 .../WholeBodyMasterSlave.cpp                  | 59 ++++++++++++++-----
 rtc/WholeBodyMasterSlave/wbms_core.h          |  2 +
 3 files changed, 48 insertions(+), 14 deletions(-)

diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
index 606f253c638..b780eb357ad 100644
--- a/idl/WholeBodyMasterSlaveService.idl
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -31,6 +31,7 @@ module OpenHRP
         boolean use_hands;
         boolean use_head;
         boolean use_manipulability_limit;
+        boolean disable_lower;
     };
     boolean startCountDownForWholeBodyMasterSlave(in double sec);
     boolean startWholeBodyMasterSlave();
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 66b85b95ecd..1f81a3ff397 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -347,9 +347,15 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
             hsp->act_rs.com = com;
             hsp->act_rs.zmp = ref_zmp;
 
+            const std::string tmp[] = {"R_CROTCH_R","R_CROTCH_P","R_CROTCH_Y","R_KNEE_P","R_ANKLE_R","R_ANKLE_P","L_CROTCH_R","L_CROTCH_P","L_CROTCH_Y","L_KNEE_P","L_ANKLE_R","L_ANKLE_P",};
+            const std::vector<std::string> lower(tmp, tmp+12);
             // qRef
             for (int i = 0; i < m_qRef.data.length(); i++ ){
+              if(m_robot->joint(i)->name == "L_HAND" || m_robot->joint(i)->name == "R_HAND"){}// pass through hand
+              else if(hsp->WBMSparam.disable_lower && std::find(lower.begin(), lower.end(), m_robot->joint(i)->name) != lower.end()){}// pass through lower limbs
+              else{
                 m_qRef.data[i] = transition_interpolator_ratio * m_robot_for_out->joint(i)->q  + (1 - transition_interpolator_ratio) * m_qRef.data[i];
+              }
             }
             // basePos
             m_basePos.data.x = transition_interpolator_ratio * m_robot_for_out->rootLink()->p(0) + (1 - transition_interpolator_ratio) * m_basePos.data.x;
@@ -566,7 +572,7 @@ void WholeBodyMasterSlave::calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robo
 #ifdef USE_NEW_FIK
 void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix){
     std::vector<IKConstraint> ikc_list;
-    {
+    if(!hsp->WBMSparam.disable_lower){
         IKConstraint tmp;
         tmp.target_link_name = "WAIST";
         tmp.localPos = hrp::Vector3::Zero();
@@ -576,7 +582,19 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.constraint_weight << 0,0,0,1,1,1;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
-    }{
+    }
+    if(hsp->WBMSparam.disable_lower){
+      IKConstraint tmp;
+      tmp.target_link_name = "WAIST";
+      tmp.localPos = hrp::Vector3::Zero();
+      tmp.localR = hrp::Matrix33::Identity();
+      tmp.targetPos << m_basePos.data.x, m_basePos.data.y, m_basePos.data.z;// will be ignored by selection_vec
+      tmp.targetRpy << m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y;// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
+      tmp.constraint_weight << 1,1,1,1,1,1;
+      tmp.rot_precision = deg2rad(3);
+      ikc_list.push_back(tmp);
+    }
+    if(!hsp->WBMSparam.disable_lower){
         IKConstraint tmp;
         tmp.target_link_name = eename_ikcp_map["rleg"].target_link_name;
         tmp.localPos = eename_ikcp_map["rleg"].localPos;
@@ -590,7 +608,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         }
         ikc_list.push_back(tmp);
     }
-    {
+    if(!hsp->WBMSparam.disable_lower){
         IKConstraint tmp;
         tmp.target_link_name = eename_ikcp_map["lleg"].target_link_name;
         tmp.localPos = eename_ikcp_map["lleg"].localPos;
@@ -604,7 +622,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         }
         ikc_list.push_back(tmp);
     }
-    const double dist = 0.65;
+    const double dist = 0.0;
     {
         IKConstraint tmp;
         tmp.target_link_name = eename_ikcp_map["rarm"].target_link_name;
@@ -616,7 +634,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
             tmp.targetPos = rh_ref.p;
         }
         tmp.targetRpy = rh_ref.rpy;
-        tmp.constraint_weight = hrp::dvector6::Constant(0.1);
+        tmp.constraint_weight = hrp::dvector6::Constant(0.5);
         tmp.pos_precision = 3e-3;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
@@ -631,17 +649,24 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
             tmp.targetPos = lh_ref.p;
         }
         tmp.targetRpy = lh_ref.rpy;
-        tmp.constraint_weight = hrp::dvector6::Constant(0.1);
+        tmp.constraint_weight = hrp::dvector6::Constant(0.5);
         tmp.pos_precision = 3e-3;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
     }if(robot_in->link("HEAD_JOINT1") != NULL){
-        IKConstraint tmp;
-        tmp.target_link_name = "HEAD_JOINT1";
-        tmp.targetRpy = head_ref.rpy;
-        tmp.constraint_weight << 0,0,0,0,0.1,0.1;
-        tmp.rot_precision = deg2rad(1);
-        ikc_list.push_back(tmp);
+      IKConstraint tmp;
+      tmp.target_link_name = "HEAD_JOINT1";
+      tmp.targetRpy = head_ref.rpy;
+      tmp.constraint_weight << 0,0,0,0,0.1,0.1;
+      tmp.rot_precision = deg2rad(1);
+      ikc_list.push_back(tmp);
+    }if(robot_in->link("HEAD_P") != NULL){
+      IKConstraint tmp;
+      tmp.target_link_name = "HEAD_P";
+      tmp.targetRpy = head_ref.rpy;
+      tmp.constraint_weight << 0,0,0,0,0.1,0.1;
+      tmp.rot_precision = deg2rad(1);
+      ikc_list.push_back(tmp);
     }
     if(hsp->rp_ref_out.tgt[rf].is_contact){
         sccp->avoid_priority.head(12).head(6).fill(4);
@@ -697,7 +722,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     }
 
 
-    {
+    if(!hsp->WBMSparam.disable_lower){
         IKConstraint tmp;
         tmp.target_link_name = "COM";
         tmp.localPos = hrp::Vector3::Zero();
@@ -739,7 +764,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     if( m_robot->link("CHEST_Y") != NULL) m_robot->link("CHEST_Y")->llimit = deg2rad(-20);
     if( m_robot->link("CHEST_Y") != NULL) m_robot->link("CHEST_Y")->ulimit = deg2rad(20);
     if( m_robot->link("CHEST_P") != NULL) m_robot->link("CHEST_P")->llimit = deg2rad(0);
-    if( m_robot->link("CHEST_P") != NULL) m_robot->link("CHEST_P")->ulimit = deg2rad(30);
+    if( m_robot->link("CHEST_P") != NULL) m_robot->link("CHEST_P")->ulimit = deg2rad(60);
 
     for(int i=0;i<robot_in->numJoints();i++){
         LIMIT_MINMAX(robot_in->joint(i)->q, robot_in->joint(i)->llimit, robot_in->joint(i)->ulimit);
@@ -930,6 +955,11 @@ bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBody
     hsp->WBMSparam.use_rh = hsp->WBMSparam.use_lh = i_param.use_hands;
     hsp->WBMSparam.use_head = i_param.use_head;
     hsp->WBMSparam.use_manipulability_limit = i_param.use_manipulability_limit;
+    if(mode.now() == MODE_IDLE){
+      hsp->WBMSparam.disable_lower = i_param.disable_lower;
+    }else{
+      std::cerr << "[" << m_profile.instance_name << "] disable_lower can be changed in MODE_IDLE" << std::endl;
+    }
     return true;
 }
 
@@ -948,6 +978,7 @@ bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMaster
     i_param.use_hands = (hsp->WBMSparam.use_rh || hsp->WBMSparam.use_lh);
     i_param.use_head = hsp->WBMSparam.use_head;
     i_param.use_manipulability_limit = hsp->WBMSparam.use_manipulability_limit;
+    i_param.disable_lower = hsp->WBMSparam.disable_lower;
     return true;
 }
 
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index ad3f5ad96ab..5a049a3390c 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -398,6 +398,7 @@ class WBMSCore : UTIL_CONST {
                 bool use_rh,use_lh;
                 bool use_head;
                 bool use_manipulability_limit;
+                bool disable_lower;
         };
         struct WBMSparameters WBMSparam;
         struct ActualRobotState {
@@ -473,6 +474,7 @@ class WBMSCore : UTIL_CONST {
             WBMSparam.use_rh = WBMSparam.use_lh = true;
             WBMSparam.use_head = true;
             WBMSparam.use_manipulability_limit = true;
+            WBMSparam.disable_lower = true;
             rp_ref_out_old.clear();
             rp_ref_out.clear();
             rp_ref_vel_old.clear();

From 9da36122f8d1eb60081d6ce64d3f1f1592fbc445 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 17 Apr 2019 18:26:17 +0900
Subject: [PATCH 149/217] control hand from rfw lfw port for demo

---
 .../WholeBodyMasterSlave.cpp                  | 35 +++++++++++++++----
 1 file changed, 28 insertions(+), 7 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 474ae312c07..1c25cb58d0a 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -351,8 +351,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
             const std::vector<std::string> lower(tmp, tmp+12);
             // qRef
             for (int i = 0; i < m_qRef.data.length(); i++ ){
-              if(m_robot->joint(i)->name == "L_HAND" || m_robot->joint(i)->name == "R_HAND"){}// pass through hand
-              else if(hsp->WBMSparam.disable_lower && std::find(lower.begin(), lower.end(), m_robot->joint(i)->name) != lower.end()){}// pass through lower limbs
+              if(hsp->WBMSparam.disable_lower && std::find(lower.begin(), lower.end(), m_robot->joint(i)->name) != lower.end()){}// pass through lower limbs
               else{
                 m_qRef.data[i] = transition_interpolator_ratio * m_robot_for_out->joint(i)->q  + (1 - transition_interpolator_ratio) * m_qRef.data[i];
               }
@@ -869,10 +868,12 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
     static hrp::dvector tmp_v = hrp::dvector::Zero(ROBOT_ALL_DOF);
 //    if (!q_ip->isEmpty() ){  q_ip->get(tmp_x, tmp_v, false);}
     for(int i=0;i<robot_in->numJoints();i++){
-        double tmp_time = fabs(robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
-        LIMIT_MIN(tmp_time, fabs(tmp_v(i))/16);
-//        tmp_time += fabs(tmp_v(i))/16; //加速度8
-        if(tmp_time > goal_time){ goal_time = tmp_time; }
+        if(robot_in->joint(i)->name != "L_HAND" && robot_in->joint(i)->name != "R_HAND" ){ //KHI demo
+          double tmp_time = fabs(robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
+          LIMIT_MIN(tmp_time, fabs(tmp_v(i))/16);
+  //        tmp_time += fabs(tmp_v(i))/16; //加速度8
+          if(tmp_time > goal_time){ goal_time = tmp_time; }
+        }
     }
     q_ip->setGoal(goal_state.data(), goal_time + min_goal_time_offset, true);
     hrp::dvector ans_state(ROBOT_ALL_DOF);
@@ -882,7 +883,27 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
     if (!q_ip->isEmpty() ){  q_ip->get(tmp, tmpv, true);}
     tmp_v = Eigen::Map<hrp::dvector>(tmpv, ROBOT_ALL_DOF);
     ans_state = Eigen::Map<hrp::dvector>(tmp, ROBOT_ALL_DOF);
-    for(int i=0;i<robot_out->numJoints();i++){ robot_out->joint(i)->q = ans_state(i); }
+    for(int i=0;i<robot_out->numJoints();i++){
+      if(robot_in->joint(i)->name != "L_HAND" && robot_in->joint(i)->name != "R_HAND" ){ //KHI demo
+        robot_out->joint(i)->q = ans_state(i);
+      }
+    }
+
+
+
+    //// KHI demo
+    const double hand_max_vel = 180.0/0.4 /180.0*M_PI;// 0.4sec for 180deg
+    std::map<std::string, double> joy_inputs;// 0 ~ 1
+    joy_inputs["L_HAND"] = m_htlfw.data[0];// tmp substitute
+    joy_inputs["R_HAND"] = m_htrfw.data[0];// tmp substitute
+    for(std::map<std::string, double>::iterator it = joy_inputs.begin(); it!=joy_inputs.end(); it++){
+      LIMIT_MINMAX(it->second, 0.0, 1.0);
+      double tgt_hand_q = (-29.0 + (124.0-(-29.0))*it->second ) /180.0*M_PI;
+      const double q_old = robot_out->link(it->first)->q;
+      LIMIT_MINMAX(tgt_hand_q, q_old - hand_max_vel*m_dt, q_old + hand_max_vel*m_dt);
+      robot_out->link(it->first)->q = tgt_hand_q;
+    }
+
     robot_out->rootLink()->p = ans_state.bottomRows(6).topRows(3);
     robot_out->rootLink()->R = hrp::rotFromRpy(ans_state.bottomRows(6).bottomRows(3));
     robot_out->calcForwardKinematics();

From 58f4686755bae11759506689b65b196a5262562e Mon Sep 17 00:00:00 2001
From: RHP4B <tmp>
Date: Fri, 19 Apr 2019 18:32:41 +0900
Subject: [PATCH 150/217] tume for demo

---
 .../WholeBodyMasterSlave.cpp                  | 57 ++++++++++++-------
 1 file changed, 36 insertions(+), 21 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 1c25cb58d0a..10d08b03ce4 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -165,7 +165,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     transition_interpolator->clear();
     transition_interpolator->set(&tmp_ratio);
     tmp_ratio = 1.0;
-    transition_interpolator->setGoal(&tmp_ratio, 3.0, true);
+    //    transition_interpolator->setGoal(&tmp_ratio, 3.0, true);
     ROBOT_ALL_DOF = m_robot->numJoints() + 3 + 3;// joints + base_p + base_rpy
     //    q_ip = new interpolator(ROBOT_ALL_DOF, m_dt, interpolator::HOFFARBIB, 1);
     //    q_ip = new interpolator(ROBOT_ALL_DOF, m_dt, interpolator::QUINTICSPLINE, 1);
@@ -283,6 +283,15 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         if (m_htzmpIn.isNew()){ m_htzmpIn.read();  WBMSCore::Point3DToVector3(m_htzmp.data,hsp->hp_wld_raw.tgt[zmp].abs.p); }
     }
 
+    //khi
+    if(m_htlfw.data[1] == 1 && m_htrfw.data[1] == 1 && mode.now() == MODE_IDLE){
+        std::cerr<<"button call"<<std::endl;
+        startWholeBodyMasterSlave();
+        std::cerr<<"button call end"<<std::endl;
+    }else if(m_htlfw.data[1] == 1 && m_htrfw.data[1] == 1 && mode.now() == MODE_WBMS){
+        stopWholeBodyMasterSlave();
+    }
+    
     if ( is_legged_robot ) {
         processTransition();
         mode.update();
@@ -581,7 +590,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
 //        tmp.constraint_weight << 0,0,0,1,1,1;
         tmp.targetPos << m_basePos.data.x,m_basePos.data.y,m_basePos.data.z;// will be ignored by selection_vec
         tmp.targetRpy << m_baseRpy.data.r,m_baseRpy.data.p,m_baseRpy.data.y;// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
-        tmp.constraint_weight << 1,1,1,1,1,1;
+        tmp.constraint_weight << 0,0,0,1,1,1;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
     }
@@ -662,14 +671,15 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.constraint_weight << 0,0,0,0,0.1,0.1;
         tmp.rot_precision = deg2rad(1);
         ikc_list.push_back(tmp);
-    }if(robot_in->link("HEAD_P") != NULL){
-        IKConstraint tmp;
-        tmp.target_link_name = "HEAD_P";
-        tmp.targetRpy = head_ref.rpy;
-        tmp.constraint_weight << 0,0,0,0,0.1,0.1;
-        tmp.rot_precision = deg2rad(1);
-        ikc_list.push_back(tmp);
     }
+    // if(robot_in->link("HEAD_P") != NULL){
+    //     IKConstraint tmp;
+    //     tmp.target_link_name = "HEAD_P";
+    //     tmp.targetRpy = head_ref.rpy;
+    //     tmp.constraint_weight << 0,0,0,0,0.1,0.1;
+    //     tmp.rot_precision = deg2rad(1);
+    //     ikc_list.push_back(tmp);
+    // }
     if(hsp->rp_ref_out.tgt[rf].is_contact){
         sccp->avoid_priority.head(12).head(6).fill(4);
     }else{
@@ -767,6 +777,11 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     if( m_robot->link("CHEST_Y") != NULL) m_robot->link("CHEST_Y")->ulimit = deg2rad(20);
     if( m_robot->link("CHEST_P") != NULL) m_robot->link("CHEST_P")->llimit = deg2rad(0);
     if( m_robot->link("CHEST_P") != NULL) m_robot->link("CHEST_P")->ulimit = deg2rad(60);
+                                              
+    if( m_robot->link("HEAD_Y") != NULL) m_robot->link("HEAD_Y")->llimit = deg2rad(-5);
+    if( m_robot->link("HEAD_Y") != NULL) m_robot->link("HEAD_Y")->ulimit = deg2rad(5);
+    if( m_robot->link("HEAD_P") != NULL) m_robot->link("HEAD_P")->llimit = deg2rad(0);
+    if( m_robot->link("HEAD_P") != NULL) m_robot->link("HEAD_P")->ulimit = deg2rad(60);
 
     for(int i=0;i<robot_in->numJoints();i++){
         LIMIT_MINMAX(robot_in->joint(i)->q, robot_in->joint(i)->llimit, robot_in->joint(i)->ulimit);
@@ -775,18 +790,18 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     fik_in->q_ref = init_sync_state;
 
 
-    std::string tmp[] = {"R_CROTCH_R","R_CROTCH_P","R_CROTCH_Y","R_KNEE_P","R_ANKLE_R","R_ANKLE_P","L_CROTCH_R","L_CROTCH_P","L_CROTCH_Y","L_KNEE_P","L_ANKLE_R","L_ANKLE_P"};
-    const std::vector<std::string> list(tmp, tmp+12);
-    for(int i=0; i<list.size(); i++){
-      if( robot_in->link(list[i]) != NULL){
-        int j=robot_in->link(list[i])->jointId;
-        fik_in->dq_weight_all(j) = 1e-5;
-        robot_in->joint(j)->q = m_qRef.data[j];
-        fik_in->q_ref(j) = m_qRef.data[j];
-      }
-    }
-    robot_in->rootLink()->p << m_basePos.data.x,m_basePos.data.y,m_basePos.data.z;// will be ignored by selection_vec
-    robot_in->rootLink()->R = hrp::rotFromRpy(hrp::Vector3(m_baseRpy.data.r,m_baseRpy.data.p,m_baseRpy.data.y));// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
+    // std::string tmp[] = {"R_CROTCH_R","R_CROTCH_P","R_CROTCH_Y","R_KNEE_P","R_ANKLE_R","R_ANKLE_P","L_CROTCH_R","L_CROTCH_P","L_CROTCH_Y","L_KNEE_P","L_ANKLE_R","L_ANKLE_P"};
+    // const std::vector<std::string> list(tmp, tmp+12);
+    // for(int i=0; i<list.size(); i++){
+    //   if( robot_in->link(list[i]) != NULL){
+    //     int j=robot_in->link(list[i])->jointId;
+    //     fik_in->dq_weight_all(j) = 1e-5;
+    //     robot_in->joint(j)->q = m_qRef.data[j];
+    //     fik_in->q_ref(j) = m_qRef.data[j];
+    //   }
+    // }
+    // robot_in->rootLink()->p << m_basePos.data.x,m_basePos.data.y,m_basePos.data.z;// will be ignored by selection_vec
+    // robot_in->rootLink()->R = hrp::rotFromRpy(hrp::Vector3(m_baseRpy.data.r,m_baseRpy.data.p,m_baseRpy.data.y));// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
 
 
 //    fik_in->q_ref_pullback_gain.segment(6+6+3+2, 8*2).fill(0.01);//腕だけ

From a1f8e04374e242aec19829b2dfb4a5d740ebe538 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 19 Apr 2019 19:38:24 +0900
Subject: [PATCH 151/217] don't use .incerse()

---
 .../FullbodyInverseKinematicsSolver.h         | 635 ++++++++----------
 1 file changed, 274 insertions(+), 361 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index f659355c7d3..64951a2fa7a 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -7,18 +7,21 @@
 #include "../ImpedanceController/JointPathEx.h"
 #include <unsupported/Eigen/MatrixFunctions>
 #include "SimpleFullbodyInverseKinematicsSolver.h"
+#include "../TorqueFilter/IIRFilter.h"
 
+//tips
 #define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
 #define deg2rad(x) ((x)*M_PI/180)
 #define rad2deg(rad) (rad * 180 / M_PI)
 #define eps_eq(a, b, c)  (fabs((a)-(b)) <= c)
 #define dbg(var) std::cout<<#var"= "<<(var)<<std::endl
 #define dbgn(var) std::cout<<#var"= "<<std::endl<<(var)<<std::endl
-namespace hrp{//ちょっとした関数定義
+namespace hrp{
     static hrp::dvector getQAll(const hrp::BodyPtr _robot){ hrp::dvector tmp(_robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ tmp(i) = _robot->joint(i)->q; } return tmp; };
     static void setQAll(hrp::BodyPtr _robot, const hrp::dvector in){ assert(in.size() <= _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = in(i); } };
 }
 
+#define OPENHRP_PACKAGE_VERSION_320
 
 class IKConstraint {
     public:
@@ -29,307 +32,241 @@ class IKConstraint {
         hrp::Matrix33 localR;
         hrp::dvector6 constraint_weight;
         double pos_precision, rot_precision;
-
         IKConstraint ()
         :targetPos(hrp::Vector3::Zero()),
          targetRpy(hrp::Vector3::Zero()),
          localPos(hrp::Vector3::Zero()),
          localR(hrp::Matrix33::Identity()),
          constraint_weight(hrp::dvector6::Ones()),
-         pos_precision(1e-4), rot_precision(deg2rad(0.1))//これ以上緩いとガタつく場合あり
-        {}
+         pos_precision(1e-4), rot_precision(deg2rad(0.1)){}
 };
 
 
 class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSolver{
     protected:
         hrp::dmatrix J_all, J_all_inv, J_com, J_am;
-        hrp::dvector dq_all, err_all, constraint_weight_all, avoid_weight_gain;
+        hrp::dvector dq_all, err_all, constraint_weight_all, jlim_avoid_weight_old;
         const int WS_DOF, BASE_DOF, J_DOF, ALL_DOF;
+        std::vector<IIRFilter> am_filters;
     public:
-        hrp::dvector dq_weight_all;
-        hrp::dvector q_ref, q_ref_pullback_gain, dq_ref_pullback;
-        hrp::Vector3 cur_momentum_around_COM;
+        hrp::dvector dq_weight_all, q_ref, q_ref_max_dq, q_ref_constraint_weight;
+        hrp::Vector3 cur_momentum_around_COM, cur_momentum_around_COM_filtered, rootlink_rpy_llimit, rootlink_rpy_ulimit;
         FullbodyInverseKinematicsSolver (hrp::BodyPtr _robot, const std::string& _print_str, const double _dt)
         : SimpleFullbodyInverseKinematicsSolver(_robot,_print_str, _dt),
           WS_DOF(6),
           BASE_DOF(6),
           J_DOF(_robot->numJoints()),
-          ALL_DOF(J_DOF+BASE_DOF) {
+          ALL_DOF(J_DOF+BASE_DOF){
             dq_weight_all = hrp::dvector::Ones(ALL_DOF);
-            dq_all = q_ref = q_ref_pullback_gain = dq_ref_pullback = hrp::dvector::Zero(ALL_DOF);
-            cur_momentum_around_COM = hrp::Vector3::Zero();
-            avoid_weight_gain = hrp::dvector::Constant(ALL_DOF, 1e12);
+            dq_all = q_ref = q_ref_constraint_weight = hrp::dvector::Zero(ALL_DOF);
+            q_ref_max_dq = hrp::dvector::Constant(ALL_DOF, 1e-2);
+            cur_momentum_around_COM = cur_momentum_around_COM_filtered = hrp::Vector3::Zero();
+//            jlim_avoid_weight_old = hrp::dvector::Constant(ALL_DOF, 1e12);
+            jlim_avoid_weight_old = hrp::dvector::Constant(ALL_DOF, 0);
+            rootlink_rpy_llimit = hrp::Vector3::Constant(-DBL_MAX);
+            rootlink_rpy_ulimit = hrp::Vector3::Constant(DBL_MAX);
+            am_filters.resize(3);
+            for(int i=0;i<3;i++){
+              am_filters[i].setParameterAsBiquad(10, 1/std::sqrt(2), 1.0/m_dt);
+              am_filters[i].reset();
+            }
         };
         ~FullbodyInverseKinematicsSolver () {};
-        int solveFullbodyIKLoop (hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list, const int _max_iteration) {
-            hrp::Vector3 base_p_old = _robot->rootLink()->p;
-            hrp::Matrix33 base_R_old = _robot->rootLink()->R;
-            hrp::dvector q_old(J_DOF);
-            for(int i=0;i<J_DOF;i++){ q_old(i) = _robot->joint(i)->q; }
-            unsigned int loop;
-            for(loop=0; loop < _max_iteration;){
-                solveFullbodyIK(_robot, _ikc_list);
-                //check ang moment
-                _robot->rootLink()->v = (_robot->rootLink()->p - base_p_old)/ m_dt;
-                _robot->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * _robot->rootLink()->R) / m_dt;
-                for(int i=0;i<J_DOF;i++){ _robot->joint(i)->dq = (_robot->joint(i)->q - q_old(i)) / m_dt; }
-
-                _robot->calcForwardKinematics(true,false);
-                hrp::Vector3 tmp_P, tmp_L;
-                _robot->calcTotalMomentum(tmp_P, tmp_L);
-                cur_momentum_around_COM = tmp_L - _robot->calcCM().cross(tmp_P);
-                loop++;
-                if(checkIKConvergence(_robot, _ikc_list)){ break; }
-            }
-            return loop;
-        }
         bool checkIKConvergence(const hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list){
-            //            if(dq_all.norm() < deg2rad(0.1)){ return true; }
             for ( int i=0; i<_ikc_list.size(); i++ ) {
                 hrp::Link* link_tgt_ptr = _robot->link(_ikc_list[i].target_link_name);
                 hrp::Vector3 pos_err, rot_err;
                 if(link_tgt_ptr){
                     pos_err = _ikc_list[i].targetPos - (link_tgt_ptr->p + link_tgt_ptr->R * _ikc_list[i].localPos);
-                    //                    rot_err = omegaFromRotEx(hrp::rotFromRpy(_ikc_list[i].targetRpy).transpose() * link_tgt_ptr->R * _ikc_list[i].localR);
                     rats::difference_rotation(rot_err, (link_tgt_ptr->R * _ikc_list[i].localR), hrp::rotFromRpy(_ikc_list[i].targetRpy));
-                    //                    rot_err = omegaFromRotEx(hrp::rotFromRpy(_ikc_list[i].targetRpy).transpose() * link_tgt_ptr->R * _ikc_list[i].localR.transpose());
-                    //                    dr_part = link_tgt_ptr->R * omegaFromRotEx(link_tgt_ptr->R.transpose() * R_origin_ref);
                 }
                 else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){  // COM
                     pos_err = _ikc_list[i].targetPos - (_robot->calcCM() + _ikc_list[i].localR * _ikc_list[i].localPos);
                     rot_err = _ikc_list[i].targetRpy - cur_momentum_around_COM;
                 }
+                else{ std::cerr<<"Unknown Link Target !!"<<std::endl; continue; }
                 for(int j=0;j<3;j++){
-                    if(fabs(pos_err(j)) > _ikc_list[i].pos_precision && _ikc_list[i].constraint_weight.head(3)(j) > 0){return false;}
-                    if(fabs(rot_err(j)) > _ikc_list[i].rot_precision && _ikc_list[i].constraint_weight.tail(3)(j) > 0){return false;}
+                    if(_ikc_list[i].constraint_weight.head(3)(j) > 0 && fabs(pos_err(j)) > _ikc_list[i].pos_precision){return false;}
+                    if(_ikc_list[i].constraint_weight.tail(3)(j) > 0 && fabs(rot_err(j)) > _ikc_list[i].rot_precision){return false;}
                 }
             }
             return true;
         }
-        void solveFullbodyIK(hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list){
-            J_all = hrp::dmatrix::Zero(WS_DOF*_ikc_list.size(), ALL_DOF);
-            err_all = hrp::dvector::Zero(WS_DOF*_ikc_list.size());
-            constraint_weight_all = hrp::dvector::Ones(WS_DOF*_ikc_list.size());
-            //リファレンスに微少量戻す
-            for(int i=0;i<J_DOF;i++){
-                double diff = q_ref(i) - _robot->joint(i)->q;
-                diff *= q_ref_pullback_gain(i);
-                LIMIT_MINMAX(diff, -dq_ref_pullback(i), dq_ref_pullback(i));
-                _robot->joint(i)->q += diff;
-            }
-            for(int i=0;i<J_DOF;i++){ LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit); }
-            _robot->calcForwardKinematics();
-            //ヤコビアンと各ベクトル生成
-            for ( int i=0; i<_ikc_list.size(); i++ ) {
-                hrp::Link* link_tgt_ptr = _robot->link(_ikc_list[i].target_link_name);
-                hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF); //全身to一拘束点へのヤコビアン
-                hrp::Vector3 dp_part, dr_part; //差分
-                //ベースリンク，通常リンク，重心で場合分け
-                if(link_tgt_ptr){//ベースリンク，通常リンク共通
-                    hrp::Vector3 tgt_cur_pos = link_tgt_ptr->p + link_tgt_ptr->R * _ikc_list[i].localPos;
-                    hrp::Matrix33 tgt_cur_rot = link_tgt_ptr->R * _ikc_list[i].localR;
-                    dp_part =  _ikc_list[i].targetPos - tgt_cur_pos;
-                    dr_part = tgt_cur_rot * omegaFromRotEx(tgt_cur_rot.transpose() * hrp::rotFromRpy(_ikc_list[i].targetRpy));
-                    //                    dr_part = link_tgt_ptr->R * matrix_logEx(link_tgt_ptr->R.transpose() * R_origin_ref);
-                    hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, "");
-                    hrp::dmatrix J_jpath;
-                    tgt_jpath.calcJacobian(J_jpath, _ikc_list[i].localPos);
-                    for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJacobianを全身用に並び替え
-                        int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
-                        J_part.col(id_in_body) = J_jpath.col(id_in_jpath);
+        int solveFullbodyIKLoop (hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list, const int _max_iteration) {
+            #ifdef OPENHRP_PACKAGE_VERSION_320
+                hrp::Vector3 base_p_old = _robot->rootLink()->p;
+                hrp::Matrix33 base_R_old = _robot->rootLink()->R;
+                hrp::dvector q_old(J_DOF);
+                for(int i=0;i<J_DOF;i++){ q_old(i) = _robot->joint(i)->q; }
+                unsigned int loop;
+                for(loop=0; loop < _max_iteration;){
+                    solveFullbodyIKOnce(_robot, _ikc_list);
+                    //check ang moment
+                    _robot->rootLink()->v = (_robot->rootLink()->p - base_p_old)/ m_dt;
+                    _robot->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * _robot->rootLink()->R) / m_dt;
+                    for(int i=0;i<J_DOF;i++){ _robot->joint(i)->dq = (_robot->joint(i)->q - q_old(i)) / m_dt; }
+                    _robot->calcForwardKinematics(true,false);
+                    hrp::Vector3 tmp_P, tmp_L;
+                    _robot->calcTotalMomentum(tmp_P, tmp_L);//calcTotalMomentumは漸化的にWorld周りの並進＋回転運動量を出す
+                    cur_momentum_around_COM = tmp_L - _robot->calcCM().cross(tmp_P);
+//                    _robot->calcTotalMomentumFromJacobian(tmp_P, cur_momentum_around_COM);//calcTotalMomentumFromJacobianは重心ヤコビアンと重心周り角運動量ヤコビアンを用いて重心周りの並進＋回転運動量を出す
+                    for(int i=0; i<3; i++){
+                        cur_momentum_around_COM_filtered(i) = am_filters[i].passFilter(cur_momentum_around_COM(i));
                     }
-                    J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
-                    J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - _robot->rootLink()->p);
+                    loop++;
+                    if(checkIKConvergence(_robot, _ikc_list)){ break; }
                 }
-                else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){ //重心限定
-                    dp_part = _ikc_list[i].targetPos - _robot->calcCM();
-                    dr_part = (_ikc_list[i].targetRpy - cur_momentum_around_COM) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける
-                    _robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
-                    _robot->calcAngularMomentumJacobian(NULL, J_am);//すでにrootlink周りの角運動量ヤコビアンが返ってくる？
-                    J_part << J_com, J_am;
-                }
-                else{ std::cerr<<"Unknown Link Target !!"<<std::endl; continue; } //不明なリンク指定
-                if(dp_part.norm()>0.01){
-                    dp_part = dp_part.normalized() * 0.01;
-                }
-                err_all.segment(WS_DOF*i,WS_DOF) << dp_part, dr_part;
-                constraint_weight_all.segment(WS_DOF*i, WS_DOF) = _ikc_list[i].constraint_weight;
-                J_all.middleRows(WS_DOF*i, WS_DOF) = J_part;
-            }
-            hrp::dmatrix selection_mat = hrp::dmatrix::Zero((constraint_weight_all.array()>0.0).count(), WS_DOF*_ikc_list.size());
-            for(int i=0, j=0; i<constraint_weight_all.rows(); i++){ if(constraint_weight_all(i) > 0.0){ selection_mat(j, i) = 1.0; j++; } }
-
-            J_all = selection_mat * J_all;
-            err_all = selection_mat * err_all;
-            constraint_weight_all = selection_mat * constraint_weight_all;
+                return loop;
+            #else
+                std::cerr<<"solveFullbodyIKLoop() needs OPENHRP_PACKAGE_VERSION_320 !!!"<<std::endl; return -1;
+            #endif
+        }
+        void solveFullbodyIKOnce(hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list){
+            #ifdef OPENHRP_PACKAGE_VERSION_320
+                // count all constraint DOF and allocate Jacobian and vectors
+                int C_DOF = 0; // constraint DOF
+                for (int i=0;i<_ikc_list.size();i++){ C_DOF += (_ikc_list[i].constraint_weight.array()>0.0).count(); }
+                C_DOF += (q_ref_constraint_weight.array()>0.0).count();
+                J_all = hrp::dmatrix::Zero(C_DOF, ALL_DOF);
+                err_all = hrp::dvector::Zero(C_DOF);
+                constraint_weight_all = hrp::dvector::Ones(C_DOF);
 
-            hrp::dvector dq_weight_all_jlim = dq_weight_all;
-            for ( int j = 0; j < _robot->numJoints() ; j++ ) {
-                double jang = _robot->joint(j)->q;
-                double jmax = _robot->joint(j)->ulimit;
-                double jmin = _robot->joint(j)->llimit;
-                double e = deg2rad(1);
-                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
-                } else if ( eps_eq(jang, jmax,e) ) {
-                    jang = jmax - e;
-                } else if ( eps_eq(jang, jmin,e) ) {
-                    jang = jmin + e;
-                }
-                double r;
-                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
-                    r = DBL_MAX;
-                } else {
-                    r = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) /
-                            (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
-                    if (isnan(r)) r = 0;
-                }
-                if (( r - avoid_weight_gain(j) ) >= 0 ) {
-                    dq_weight_all_jlim(j) *= ( 1.0 / ( 1.0 + r) );
-                } else {//リミットから離れるときは解放
-                    dq_weight_all_jlim(j) *= 1.0;
+                // set end effector constraint into Jacobian
+                int cur_cid_all = 0; //current constraint index of all constraints
+                for ( int i=0; i<_ikc_list.size(); i++ ) {
+                    hrp::Link* link_tgt_ptr = _robot->link(_ikc_list[i].target_link_name);
+                    hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF); //全身to拘束点へのヤコビアン
+                    hrp::dvector6 dp_part; //pos + rot 速度ではなく差分
+                    if(link_tgt_ptr){//ベースリンク，通常リンク共通
+                        hrp::Vector3 tgt_cur_pos = link_tgt_ptr->p + link_tgt_ptr->R * _ikc_list[i].localPos;
+                        hrp::Matrix33 tgt_cur_rot = link_tgt_ptr->R * _ikc_list[i].localR;
+                        dp_part.head(3) =  _ikc_list[i].targetPos - tgt_cur_pos;
+                        dp_part.tail(3) = tgt_cur_rot * omegaFromRotEx(tgt_cur_rot.transpose() * hrp::rotFromRpy(_ikc_list[i].targetRpy));
+                        hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, "");
+                        hrp::dmatrix J_jpath;
+                        tgt_jpath.calcJacobian(J_jpath, _ikc_list[i].localPos);
+                        for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ J_part.col(tgt_jpath.joint(id_in_jpath)->jointId) = J_jpath.col(id_in_jpath); } //ジョイントパスのJacobianを全身用に並び替え
+                        J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
+                        J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - _robot->rootLink()->p);
+                    }else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){ //重心限定
+                        dp_part.head(3) = _ikc_list[i].targetPos - _robot->calcCM();
+//                        dp_part.tail(3) = (_ikc_list[i].targetRpy - cur_momentum_around_COM) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける
+                        dp_part.tail(3) = (_ikc_list[i].targetRpy - cur_momentum_around_COM_filtered) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける(+フィルタ)
+                        _robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
+                        _robot->calcAngularMomentumJacobian(NULL, J_am);//world座標系での角運動量を生成するヤコビアン(なので本当はCOM周りには使えない)
+                        J_part << J_com, J_am;
+                    }else{ std::cerr<<"Unknown Link Target !!"<<std::endl; continue; } //不明なリンク指定
+                    for(int cid=0; cid<WS_DOF; cid++){
+                        if(_ikc_list[i].constraint_weight(cid) > 0.0){
+                            J_all.row(cur_cid_all) = J_part.row(cid);
+                            err_all(cur_cid_all) = dp_part(cid);
+                            constraint_weight_all(cur_cid_all) = _ikc_list[i].constraint_weight(cid);
+                            cur_cid_all++;
+                        }
+                    }
                 }
-                avoid_weight_gain(j) = r;
-            }
 
-            const double wn_const = 1e-6;
-            const double auto_lambda_gain = 1;
-            hrp::dmatrix Wn = (static_cast<double>(err_all.transpose() * constraint_weight_all.asDiagonal() * err_all) * auto_lambda_gain + wn_const) * hrp::dmatrix::Identity(ALL_DOF, ALL_DOF);
-            hrp::dvector dq_weight_all_inv = dq_weight_all_jlim.array().inverse();//重み小でdq小にするため
-            Wn = dq_weight_all_inv.asDiagonal() * Wn;
-            hrp::dmatrix H = J_all.transpose() * constraint_weight_all.asDiagonal() * J_all + Wn;
-            hrp::dvector g = J_all.transpose() * constraint_weight_all.asDiagonal() * err_all;
+                // set desired joint pose constraint into Jacobian
+                for(int qid=0; qid<ALL_DOF; qid++){
+                    if(q_ref_constraint_weight(qid) > 0.0){
+                        J_all.row(cur_cid_all)(qid) = 1;
+                        err_all(cur_cid_all) = (qid < J_DOF) ? (q_ref(qid) - _robot->joint(qid)->q) : (q_ref(qid) - hrp::rpyFromRot(_robot->rootLink()->R)(qid-J_DOF));
+                        LIMIT_MINMAX(err_all(cur_cid_all), -q_ref_max_dq(qid), q_ref_max_dq(qid));
+                        constraint_weight_all(cur_cid_all) = q_ref_constraint_weight(qid);
+                        cur_cid_all++;
+                    }
+                }
 
-            dq_all = H.ldlt().solve(g);
+                // joint limit avoidance (copy from JointPathEx)
+                hrp::dvector dq_weight_all_jlim = hrp::dvector::Ones(ALL_DOF);
+                for ( int j = 0; j < ALL_DOF ; j++ ) {
+                    double jang = (j<J_DOF) ? _robot->joint(j)->q       :   hrp::rpyFromRot(_robot->rootLink()->R)(j - J_DOF);
+                    double jmax = (j<J_DOF) ? _robot->joint(j)->ulimit  :   rootlink_rpy_ulimit(j - J_DOF);
+                    double jmin = (j<J_DOF) ? _robot->joint(j)->llimit  :   rootlink_rpy_llimit(j - J_DOF);
+                    double e = deg2rad(1);
+                    if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+                    } else if ( eps_eq(jang, jmax,e) ) {
+                        jang = jmax - e;
+                    } else if ( eps_eq(jang, jmin,e) ) {
+                        jang = jmin + e;
+                    }
+                    double jlim_avoid_weight;
+                    if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+                        jlim_avoid_weight = DBL_MAX;
+                    } else {
+                        jlim_avoid_weight = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) / (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
+                        if (isnan(jlim_avoid_weight)) jlim_avoid_weight = 0;
+                    }
+                    if (( jlim_avoid_weight - jlim_avoid_weight_old(j) ) >= 0 ) { // add weight only if q approaching to the limit
+                        dq_weight_all_jlim(j) += jlim_avoid_weight;
+                    }
+                    jlim_avoid_weight_old(j) = jlim_avoid_weight;
+                }
+                hrp::dvector dq_weight_all_final = dq_weight_all.array() * dq_weight_all_jlim.array();
 
-            static int count;
-            if(count++ % 10000 == 0){
-                std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
-                std::cout<<std::setprecision(2) << "J_all_inv=\n"<<J_all_inv<<std::setprecision(6)<<std::endl;
-                dbgn(selection_mat);
-                dbgn(H);
-                dbg(Wn(0,0));
-                dbg(g.transpose());
-                dbg(err_all.transpose());
-                dbg(dq_all.transpose());
-                dbg(constraint_weight_all.transpose());
-                dbg(dq_weight_all_inv.transpose());
-                std::cout<<"q_ans_all\n";
-                for(int i=0;i<_robot->numJoints();i++)std::cerr<<_robot->joint(i)->q<<" ";
-                std::cout<<std::endl;
-            }
+                // Solvability-unconcerned Inverse Kinematics by Levenberg-Marquardt Method [sugihara:RSJ2009]
+                const double wn_const = 1e-6;
+                hrp::dmatrix Wn = (static_cast<double>(err_all.transpose() * constraint_weight_all.asDiagonal() * err_all) + wn_const) * hrp::dmatrix::Identity(ALL_DOF, ALL_DOF);
+                Wn = dq_weight_all_final.asDiagonal() * Wn;
+                hrp::dmatrix H = J_all.transpose() * constraint_weight_all.asDiagonal() * J_all + Wn;
+                hrp::dvector g = J_all.transpose() * constraint_weight_all.asDiagonal() * err_all;
+//                dq_all = H.inverse() * g; // slow
+                dq_all = H.ldlt().solve(g);
 
-            // check validity
-            hrp::Matrix33 eWx = hrp::hat(dq_all.segment(J_DOF+3,3)).exp();
-            hrp::Matrix33 R_base_ans = eWx * _robot->rootLink()->R;
+    //            // debug print
+    //            static int count;
+    //            if(count++ % 10000 == 0){
+    //                std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
+    //                std::cout<<std::setprecision(2) << "J_all_inv=\n"<<J_all_inv<<std::setprecision(6)<<std::endl;
+    //                dbgn(selection_mat);
+    //                dbgn(H);
+    //                dbg(Wn(0,0));
+    //                dbg(g.transpose());
+    //                dbg(err_all.transpose());
+    //                dbg(dq_all.transpose());
+    //                dbg(constraint_weight_all.transpose());
+    //                dbg(dq_weight_all_inv.transpose());
+    //                std::cout<<"q_ans_all\n";
+    //                for(int i=0;i<_robot->numJoints();i++)std::cerr<<_robot->joint(i)->q<<" ";
+    //                std::cout<<std::endl;
+    //            }
 
-            if(!R_base_ans.isUnitary()){ std::cerr <<"ERROR R_base_ans is not Unitary" << std::endl; return; }
-            for(int i=0;i<dq_all.rows();i++){ if( isnan(dq_all(i)) || isinf(dq_all(i)) ){ std::cerr <<"ERROR nan/inf is found" << std::endl; return;} }
+                // update joint angles
+                for(int i=0;i<dq_all.rows();i++){ if( isnan(dq_all(i)) || isinf(dq_all(i)) ){ std::cerr <<"ERROR nan/inf is found" << std::endl; return;} }
+                for(int i=0;i<J_DOF;i++){
+                    _robot->joint(i)->q += dq_all(i);
+                    LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit);
+                }
 
-            //関節角+ベース位置姿勢更新
-            for(int i=0;i<J_DOF;i++){
-                _robot->joint(i)->q += dq_all(i);
-                LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit);
-            }
-            _robot->rootLink()->p += dq_all.segment(J_DOF,3);//vel計算する時からm_dt使ってないから使わなくていい
+                // update rootlink pos rot
+                _robot->rootLink()->p += dq_all.tail(6).head(3);
+                for(int i=0;i<3;i++){
+                    if(hrp::rpyFromRot(_robot->rootLink()->R)(i) < rootlink_rpy_llimit(i) && dq_all.tail(6).tail(3)(i) < 0) dq_all.tail(6).tail(3)(i) = 0;
+                    if(hrp::rpyFromRot(_robot->rootLink()->R)(i) > rootlink_rpy_ulimit(i) && dq_all.tail(6).tail(3)(i) > 0) dq_all.tail(6).tail(3)(i) = 0;
+                }
 
-            Eigen::Quaternion<double> quat(R_base_ans);
-            quat.normalize();
-            R_base_ans = quat.toRotationMatrix();
 
-            _robot->rootLink()->R = R_base_ans;
-            _robot->calcForwardKinematics();
+                hrp::Matrix33 dR;
+                hrp::Vector3 omega = dq_all.tail(6).tail(3);
+                hrp::calcRodrigues(dR, omega.normalized(), omega.norm());
+                if(!(_robot->rootLink()->R * dR).isUnitary()){
+                    std::cerr <<"ERROR R_base_ans is not Unitary" << std::endl; return;
+                }else{
+                    _robot->rootLink()->R = _robot->rootLink()->R * dR;
+                }
+                _robot->calcForwardKinematics();
+            #else
+                std::cerr<<"solveFullbodyIKOnce() needs OPENHRP_PACKAGE_VERSION_320 !!!"<<std::endl;
+            #endif
         }
-
-//        void calcJacobianAndErrorVector_BaseLink(hrp::BodyPtr _robot, const IKConstraint& _ikc, hrp::dmatrix _J_part, hrp::dvector6 _d_part){
-//            hrp::Link* link_tgt_ptr = _robot->link(_ikc.target_link_name);
-//            const hrp::Matrix33 R_origin_ref = hrp::rotFromRpy(_ikc.targetRpy) * _ikc.localR.transpose(); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
-//            const hrp::Vector3 p_origin_ref = _ikc.targetPos - R_origin_ref * _ikc.localPos;
-//            _d_part.head(3) = p_origin_ref - link_tgt_ptr->p;
-//            _d_part.tail(3) = link_tgt_ptr->R * omegaFromRotEx(link_tgt_ptr->R.transpose() * R_origin_ref);
-//            hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, "");
-//            tgt_jpath.calcJacobian(_J_part);
-//            for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
-//                int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
-//                _J_part.col(id_in_body) = _J_part.col(id_in_jpath);
-//            }
-//            _J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
-//            _J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - _robot->rootLink()->p);
-//        }
-//
-//        void calcJacobianAndErrorVector_NormalLink(hrp::BodyPtr _robot, const IKConstraint& _ikc, hrp::dmatrix _J_part, hrp::dvector6 _d_part){
-//            hrp::Link* link_tgt_ptr = _robot->link(_ikc.target_link_name);
-//            const hrp::Matrix33 R_origin_ref = hrp::rotFromRpy(_ikc.targetRpy) * _ikc.localR.transpose(); //拘束ポイントの位置姿勢->リンク原点の位置姿勢
-//            const hrp::Vector3 p_origin_ref = _ikc.targetPos - R_origin_ref * _ikc.localPos;
-//            _d_part.head(3) = p_origin_ref - link_tgt_ptr->p;
-//            _d_part.tail(3) = link_tgt_ptr->R * omegaFromRotEx(link_tgt_ptr->R.transpose() * R_origin_ref);
-//            hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, "");
-//            tgt_jpath.calcJacobian(_J_part);
-//            for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ //ジョイントパスのJaxobianを全身用に並び替え
-//                int id_in_body = tgt_jpath.joint(id_in_jpath)->jointId; //全身でのjoint番号
-//                _J_part.col(id_in_body) = _J_part.col(id_in_jpath);
-//            }
-//            _J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
-//            _J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - _robot->rootLink()->p);
-//        }
+        void revertRobotStateToCurrentAll (){
+            hrp::setQAll(m_robot, qorg);
+            m_robot->rootLink()->p = current_root_p;
+            m_robot->rootLink()->R = current_root_R;
+            m_robot->calcForwardKinematics();
+        };
 
     protected:
-//        bool calcJacobianInverseNullspace_copy(hrp::BodyPtr _robot, hrp::dmatrix &_J, hrp::dmatrix &_Jinv) {
-//            hrp::dmatrix w = hrp::dmatrix::Identity(ALL_DOF,ALL_DOF);
-//            std::vector<double> avoid_weight_gain;
-//            avoid_weight_gain.resize(_robot->numJoints());
-//            for(int j=0; j < J_DOF; j++){
-//                avoid_weight_gain[j] = 100000000000000000000.0;
-//            }
-//            bool use_inside_joint_weight_retrieval = true;
-//
-//            for ( int j = 0; j < _robot->numJoints() ; j++ ) {
-//                double jang = _robot->joint(j)->q;
-//                double jmax = _robot->joint(j)->ulimit;
-//                double jmin = _robot->joint(j)->llimit;
-//                double e = deg2rad(1);
-//                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
-//                } else if ( eps_eq(jang, jmax,e) ) {
-//                    jang = jmax - e;
-//                } else if ( eps_eq(jang, jmin,e) ) {
-//                    jang = jmin + e;
-//                }
-//                double r;
-//                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
-//                    r = DBL_MAX;
-//                } else {
-//                    r = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) /
-//                            (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
-//                    if (isnan(r)) r = 0;
-//                }
-//                if (( r - avoid_weight_gain[j] ) >= 0 ) {
-//                    w(j, j) = dq_weight_all[j] * ( 1.0 / ( 1.0 + r) );
-//                } else {
-//                    if (use_inside_dq_weight_all_retrieval)
-//                        w(j, j) = dq_weight_all[j] * 1.0;
-//                    else
-//                        w(j, j) = dq_weight_all[j] * ( 1.0 / ( 1.0 + r) );
-//                }
-//                avoid_weight_gain[j] = r;
-//            }
-//            for ( int j = _robot->numJoints(); j < ALL_DOF ; j++ ) {
-//                w(j, j) = dq_weight_all[j];
-//            }
-//
-//            const double manipulability_limit = 0.0001;
-//            const double sr_gain = 1.0;
-//            //      const double manipulability_gain = 0.001;
-//            const double manipulability_gain = 1;
-//
-//            double manipulability = sqrt((_J*_J.transpose()).determinant());
-//            double k = 0;
-//            if ( manipulability < manipulability_limit ) {
-//                k = manipulability_gain * pow((1 - ( manipulability / manipulability_limit )), 2);
-//            }
-//            hrp::calcSRInverse(_J, _Jinv, sr_gain * k, w);
-//            return true;
-//        }
-
         hrp::Vector3 omegaFromRotEx(const hrp::Matrix33& r) {//copy from JointPathEx.cpp
             using ::std::numeric_limits;
             double alpha = (r(0,0) + r(1,1) + r(2,2) - 1.0) / 2.0;
@@ -345,122 +282,98 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
                 return hrp::Vector3( (r(1,2) - r(2,1)) * k, (r(2,0) - r(0,2)) * k, (r(0,1) - r(1,0)) * k );
             }
         }
-        hrp::Vector3 matrix_logEx(const hrp::Matrix33& m) {
-            hrp::Vector3 mlog;
-            double q0, th;
-            hrp::Vector3 q;
-            double norm;
-
-            Eigen::Quaternion<double> eiq(m);
-            q0 = eiq.w();
-            q = eiq.vec();
-            norm = q.norm();
-            if (norm > 0) {
-                if ((q0 > 1.0e-10) || (q0 < -1.0e-10)) {
-                    th = 2 * std::atan(norm / q0);
-                } else if (q0 > 0) {
-                    th = M_PI / 2;
-                } else {
-                    th = -M_PI / 2;
-                }
-                mlog = (th / norm) * q ;
-            } else {
-                mlog = hrp::Vector3::Zero();
-            }
-            return mlog;
-        }
-};
-
-
-class FullbodyInverseKinematicsSolverMT : public FullbodyInverseKinematicsSolver{
-    protected:
-        hrp::BodyPtr _robot_copy;
-        pthread_t ik_thread;
-        pthread_mutex_t ik_body_mutex;  // Mutex
-        int cur_ik_loop;
-        std::vector<IKConstraint> ikc_list;
-        bool is_first;
-        bool ik_thread_kill;
-        bool ik_thread_ik_required;
-        hrp::Vector3 base_p_old;
-        hrp::Matrix33 base_R_old;
-        hrp::dvector q_old;
-    public:
-        FullbodyInverseKinematicsSolverMT (hrp::BodyPtr& _robot, const std::string& _print_str, const double _dt) : FullbodyInverseKinematicsSolver(_robot,_print_str, _dt) {
-            _robot_copy = hrp::BodyPtr(new hrp::Body(*_robot));
-            cur_ik_loop = 0;
-            is_first = true;
-            ik_thread_kill = false;
-            ik_thread_ik_required = false;
-        };
-        ~FullbodyInverseKinematicsSolverMT () {
-            pthread_cancel(ik_thread);
-            ik_thread_kill = true;
-            pthread_join(ik_thread, NULL );
-            std::cerr<<"[FullbodyInverseKinematicsSolverMT] thread killed successfully"<<std::endl;
-        };
-        int solveFullbodyIKLoopMT (hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list, const int _max_iteration) {
-            if(is_first){
-                for(int i=0;i<J_DOF;i++){
-                    _robot_copy->joint(i)->q = _robot->joint(i)->q;
-                    _robot_copy->joint(i)->ulimit = _robot->joint(i)->ulimit;
-                    _robot_copy->joint(i)->llimit = _robot->joint(i)->llimit;
-                }
-                _robot_copy->rootLink()->p = _robot->rootLink()->p;
-                _robot_copy->rootLink()->R = _robot->rootLink()->R;
-                pthread_mutex_init( &ik_body_mutex, NULL );
-                pthread_create(&ik_thread, NULL, launchThread, this);
-                is_first = false;
-            }
-            for(int i=0;i<J_DOF;i++){
-                _robot->joint(i)->q = _robot_copy->joint(i)->q;
-            }
-            _robot->rootLink()->p = _robot_copy->rootLink()->p;
-            _robot->rootLink()->R = _robot_copy->rootLink()->R;
-            _robot->calcForwardKinematics();
-            int result = cur_ik_loop;
-
-            if(pthread_mutex_lock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_lock err "<<std::endl;}
-
-            base_p_old = _robot_copy->rootLink()->p;
-            base_R_old = _robot_copy->rootLink()->R;
-            q_old.resize(J_DOF);
-            for(int i=0;i<J_DOF;i++){ q_old(i) = _robot_copy->joint(i)->q; }
-
-            cur_ik_loop = 0;
-            ikc_list = _ikc_list;
-            ik_thread_ik_required = true;
-            if(pthread_mutex_unlock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_unlock err "<<std::endl;}
-            return result;
-        }
-        void ik_loop(){
-            std::cerr<<"[FullbodyInverseKinematicsSolverMT] start thread"<<std::endl;
-            while(1){
-                usleep(0);//これ無いとmutex解放しない？
-                if(pthread_mutex_lock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_lock2 err "<<std::endl;}
-                if(ik_thread_ik_required){
-                    solveFullbodyIK(_robot_copy, ikc_list);
-                    //check ang moment
-                    _robot_copy->rootLink()->v = (_robot_copy->rootLink()->p - base_p_old)/ m_dt;
-                    _robot_copy->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * _robot_copy->rootLink()->R) / m_dt;
-                    for(int i=0;i<J_DOF;i++){ _robot_copy->joint(i)->dq = (_robot_copy->joint(i)->q - q_old(i)) / m_dt; }
-                    _robot_copy->calcForwardKinematics(true,false);
-//                    _robot_copy->calcTotalMomentum(cur_momentum_P, cur_momentum_L);
-                    cur_ik_loop++;
-                    if(cur_ik_loop > 10000){std::cerr<<"[FullbodyInverseKinematicsSolverMT] thread time out"<<std::endl;break;}//異常終了でスレッドが残ってしまった時にタイムアウト終了
-                }
-                if(pthread_mutex_unlock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_unlock2 err "<<std::endl;}
-                if(ik_thread_kill){break;}
-            }
-            std::cerr<<"[FullbodyInverseKinematicsSolverMT] end of thread"<<std::endl;
-        }
-        static void* launchThread(void *pParam) {
-            reinterpret_cast<FullbodyInverseKinematicsSolverMT*>(pParam)->ik_loop();
-            return NULL;
-        }
 };
 
 
+// // multi-thread IK (answer will delay for 1 control loop)
+//class FullbodyInverseKinematicsSolverMT : public FullbodyInverseKinematicsSolver{
+//    protected:
+//        hrp::BodyPtr _robot_copy;
+//        pthread_t ik_thread;
+//        pthread_mutex_t ik_body_mutex;  // Mutex
+//        int cur_ik_loop;
+//        std::vector<IKConstraint> ikc_list;
+//        bool is_first;
+//        bool ik_thread_kill;
+//        bool ik_thread_ik_required;
+//        hrp::Vector3 base_p_old;
+//        hrp::Matrix33 base_R_old;
+//        hrp::dvector q_old;
+//    public:
+//        FullbodyInverseKinematicsSolverMT (hrp::BodyPtr& _robot, const std::string& _print_str, const double _dt) : FullbodyInverseKinematicsSolver(_robot,_print_str, _dt) {
+//            _robot_copy = hrp::BodyPtr(new hrp::Body(*_robot));
+//            cur_ik_loop = 0;
+//            is_first = true;
+//            ik_thread_kill = false;
+//            ik_thread_ik_required = false;
+//        };
+//        ~FullbodyInverseKinematicsSolverMT () {
+//            pthread_cancel(ik_thread);
+//            ik_thread_kill = true;
+//            pthread_join(ik_thread, NULL );
+//            std::cerr<<"[FullbodyInverseKinematicsSolverMT] thread killed successfully"<<std::endl;
+//        };
+//        int solveFullbodyIKLoopMT (hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list, const int _max_iteration) {
+//            if(is_first){
+//                for(int i=0;i<J_DOF;i++){
+//                    _robot_copy->joint(i)->q = _robot->joint(i)->q;
+//                    _robot_copy->joint(i)->ulimit = _robot->joint(i)->ulimit;
+//                    _robot_copy->joint(i)->llimit = _robot->joint(i)->llimit;
+//                }
+//                _robot_copy->rootLink()->p = _robot->rootLink()->p;
+//                _robot_copy->rootLink()->R = _robot->rootLink()->R;
+//                pthread_mutex_init( &ik_body_mutex, NULL );
+//                pthread_create(&ik_thread, NULL, launchThread, this);
+//                is_first = false;
+//            }
+//            for(int i=0;i<J_DOF;i++){
+//                _robot->joint(i)->q = _robot_copy->joint(i)->q;
+//            }
+//            _robot->rootLink()->p = _robot_copy->rootLink()->p;
+//            _robot->rootLink()->R = _robot_copy->rootLink()->R;
+//            _robot->calcForwardKinematics();
+//            int result = cur_ik_loop;
+//
+//            if(pthread_mutex_lock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_lock err "<<std::endl;}
+//
+//            base_p_old = _robot_copy->rootLink()->p;
+//            base_R_old = _robot_copy->rootLink()->R;
+//            q_old.resize(J_DOF);
+//            for(int i=0;i<J_DOF;i++){ q_old(i) = _robot_copy->joint(i)->q; }
+//
+//            cur_ik_loop = 0;
+//            ikc_list = _ikc_list;
+//            ik_thread_ik_required = true;
+//            if(pthread_mutex_unlock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_unlock err "<<std::endl;}
+//            return result;
+//        }
+//        void ik_loop(){
+//            std::cerr<<"[FullbodyInverseKinematicsSolverMT] start thread"<<std::endl;
+//            while(1){
+//                usleep(0);//これ無いとmutex解放しない？
+//                if(pthread_mutex_lock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_lock2 err "<<std::endl;}
+//                if(ik_thread_ik_required){
+//                    solveFullbodyIK(_robot_copy, ikc_list);
+//                    //check ang moment
+//                    _robot_copy->rootLink()->v = (_robot_copy->rootLink()->p - base_p_old)/ m_dt;
+//                    _robot_copy->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * _robot_copy->rootLink()->R) / m_dt;
+//                    for(int i=0;i<J_DOF;i++){ _robot_copy->joint(i)->dq = (_robot_copy->joint(i)->q - q_old(i)) / m_dt; }
+//                    _robot_copy->calcForwardKinematics(true,false);
+////                    _robot_copy->calcTotalMomentum(cur_momentum_P, cur_momentum_L);
+//                    cur_ik_loop++;
+//                    if(cur_ik_loop > 10000){std::cerr<<"[FullbodyInverseKinematicsSolverMT] thread time out"<<std::endl;break;}//異常終了でスレッドが残ってしまった時にタイムアウト終了
+//                }
+//                if(pthread_mutex_unlock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_unlock2 err "<<std::endl;}
+//                if(ik_thread_kill){break;}
+//            }
+//            std::cerr<<"[FullbodyInverseKinematicsSolverMT] end of thread"<<std::endl;
+//        }
+//        static void* launchThread(void *pParam) {
+//            reinterpret_cast<FullbodyInverseKinematicsSolverMT*>(pParam)->ik_loop();
+//            return NULL;
+//        }
+//};
 
 
 #endif //  FULLBODYIK_H
+

From 9764206b5c9f75c7f2057b38e42e4de67533d14c Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 24 Apr 2019 03:19:16 +0900
Subject: [PATCH 152/217] refactor

---
 idl/WholeBodyMasterSlaveService.idl           |   9 +-
 .../FullbodyInverseKinematicsSolver.h         | 163 ++----
 rtc/WholeBodyMasterSlave/CMakeLists.txt       |   2 +-
 .../WholeBodyMasterSlave.cpp                  | 478 ++++++------------
 .../WholeBodyMasterSlave.h                    | 144 +++---
 .../WholeBodyMasterSlaveService_impl.cpp      |   5 -
 .../WholeBodyMasterSlaveService_impl.h        |   2 +-
 rtc/WholeBodyMasterSlave/wbms_core.cpp        | 120 -----
 rtc/WholeBodyMasterSlave/wbms_core.h          | 145 +++---
 9 files changed, 323 insertions(+), 745 deletions(-)
 delete mode 100644 rtc/WholeBodyMasterSlave/wbms_core.cpp

diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
index b780eb357ad..e2b2e145ac0 100644
--- a/idl/WholeBodyMasterSlaveService.idl
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -22,18 +22,15 @@ module OpenHRP
         double auto_swing_foot_landing_threshold;
         double foot_vertical_vel_limit_coeff;
         double human_com_height;
-        boolean is_doctor;
-        boolean set_com_height_fix;
         double set_com_height_fix_val;
         double swing_foot_height_offset;
         double swing_foot_max_height;
         double upper_body_rmc_ratio;
-        boolean use_hands;
+        boolean set_com_height_fix;
         boolean use_head;
-        boolean use_manipulability_limit;
-        boolean disable_lower;
+        boolean use_upper;
+        boolean use_lower;
     };
-    boolean startCountDownForWholeBodyMasterSlave(in double sec);
     boolean startWholeBodyMasterSlave();
     boolean stopWholeBodyMasterSlave();
     boolean pauseWholeBodyMasterSlave();
diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 64951a2fa7a..7374bf1ffc8 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -5,9 +5,9 @@
 #include <hrpModel/Body.h>
 #include <hrpUtil/MatrixSolvers.h>
 #include "../ImpedanceController/JointPathEx.h"
-#include <unsupported/Eigen/MatrixFunctions>
-#include "SimpleFullbodyInverseKinematicsSolver.h"
+#include "../ImpedanceController/RatsMatrix.h"
 #include "../TorqueFilter/IIRFilter.h"
+#include <unsupported/Eigen/MatrixFunctions>
 
 //tips
 #define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
@@ -39,20 +39,23 @@ class IKConstraint {
          localR(hrp::Matrix33::Identity()),
          constraint_weight(hrp::dvector6::Ones()),
          pos_precision(1e-4), rot_precision(deg2rad(0.1)){}
+        hrp::Vector3 getCurrentTargetPos(hrp::BodyPtr _robot){ return _robot->link(target_link_name)->p + _robot->link(target_link_name)->R * localPos; }
+        hrp::Matrix33 getCurrentTargetRot(hrp::BodyPtr _robot){ return _robot->link(target_link_name)->R * localR; }
 };
 
 
-class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSolver{
+class FullbodyInverseKinematicsSolver{
     protected:
         hrp::dmatrix J_all, J_all_inv, J_com, J_am;
         hrp::dvector dq_all, err_all, constraint_weight_all, jlim_avoid_weight_old;
         const int WS_DOF, BASE_DOF, J_DOF, ALL_DOF;
         std::vector<IIRFilter> am_filters;
+        const double m_dt;
     public:
         hrp::dvector dq_weight_all, q_ref, q_ref_max_dq, q_ref_constraint_weight;
         hrp::Vector3 cur_momentum_around_COM, cur_momentum_around_COM_filtered, rootlink_rpy_llimit, rootlink_rpy_ulimit;
-        FullbodyInverseKinematicsSolver (hrp::BodyPtr _robot, const std::string& _print_str, const double _dt)
-        : SimpleFullbodyInverseKinematicsSolver(_robot,_print_str, _dt),
+        FullbodyInverseKinematicsSolver (hrp::BodyPtr _robot, const std::string& _print_str, const double _dt):
+          m_dt(_dt),
           WS_DOF(6),
           BASE_DOF(6),
           J_DOF(_robot->numJoints()),
@@ -61,7 +64,6 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
             dq_all = q_ref = q_ref_constraint_weight = hrp::dvector::Zero(ALL_DOF);
             q_ref_max_dq = hrp::dvector::Constant(ALL_DOF, 1e-2);
             cur_momentum_around_COM = cur_momentum_around_COM_filtered = hrp::Vector3::Zero();
-//            jlim_avoid_weight_old = hrp::dvector::Constant(ALL_DOF, 1e12);
             jlim_avoid_weight_old = hrp::dvector::Constant(ALL_DOF, 0);
             rootlink_rpy_llimit = hrp::Vector3::Constant(-DBL_MAX);
             rootlink_rpy_ulimit = hrp::Vector3::Constant(DBL_MAX);
@@ -166,7 +168,7 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
                     }
                 }
 
-                // set desired joint pose constraint into Jacobian
+                // set desired joint angle constraint into Jacobian
                 for(int qid=0; qid<ALL_DOF; qid++){
                     if(q_ref_constraint_weight(qid) > 0.0){
                         J_all.row(cur_cid_all)(qid) = 1;
@@ -210,26 +212,32 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
                 Wn = dq_weight_all_final.asDiagonal() * Wn;
                 hrp::dmatrix H = J_all.transpose() * constraint_weight_all.asDiagonal() * J_all + Wn;
                 hrp::dvector g = J_all.transpose() * constraint_weight_all.asDiagonal() * err_all;
-//                dq_all = H.inverse() * g; // slow
-                dq_all = H.ldlt().solve(g);
+                dq_all = H.ldlt().solve(g); // dq_all = H.inverse() * g; is slow
+
+                // rtconf localhost:15005/wbms.rtc set debugLevel 1 とかにしたい
+                // debug print
+                static int count;
+                if(count++ % 10000 == 0){
+                    std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
+                    std::cout<<std::setprecision(2) << "J_all_inv=\n"<<J_all_inv<<std::setprecision(6)<<std::endl;
+//                    dbgn(selection_mat);
+                    dbgn(H);
+//                    dbg(Wn(0,0));
+                    dbg(g.transpose());
+                    dbg(err_all.transpose());
+                    dbg(dq_all.transpose());
+                    dbg(constraint_weight_all.transpose());
+//                    dbg(dq_weight_all_inv.transpose());
+                    std::cout<<"q_ans_all\n";
+                    for(int i=0;i<_robot->numJoints();i++)std::cerr<<_robot->joint(i)->q<<" ";
 
-    //            // debug print
-    //            static int count;
-    //            if(count++ % 10000 == 0){
-    //                std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
-    //                std::cout<<std::setprecision(2) << "J_all_inv=\n"<<J_all_inv<<std::setprecision(6)<<std::endl;
-    //                dbgn(selection_mat);
-    //                dbgn(H);
-    //                dbg(Wn(0,0));
-    //                dbg(g.transpose());
-    //                dbg(err_all.transpose());
-    //                dbg(dq_all.transpose());
-    //                dbg(constraint_weight_all.transpose());
-    //                dbg(dq_weight_all_inv.transpose());
-    //                std::cout<<"q_ans_all\n";
-    //                for(int i=0;i<_robot->numJoints();i++)std::cerr<<_robot->joint(i)->q<<" ";
-    //                std::cout<<std::endl;
-    //            }
+                    for(int i=0;i<_ikc_list.size();i++){
+                        dbg(_ikc_list[i].target_link_name);
+                        dbg(_ikc_list[i].targetPos.transpose());
+                        dbg(_ikc_list[i].targetRpy.transpose());
+                    }
+                    std::cout<<std::endl;
+                }
 
                 // update joint angles
                 for(int i=0;i<dq_all.rows();i++){ if( isnan(dq_all(i)) || isinf(dq_all(i)) ){ std::cerr <<"ERROR nan/inf is found" << std::endl; return;} }
@@ -259,12 +267,12 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
                 std::cerr<<"solveFullbodyIKOnce() needs OPENHRP_PACKAGE_VERSION_320 !!!"<<std::endl;
             #endif
         }
-        void revertRobotStateToCurrentAll (){
-            hrp::setQAll(m_robot, qorg);
-            m_robot->rootLink()->p = current_root_p;
-            m_robot->rootLink()->R = current_root_R;
-            m_robot->calcForwardKinematics();
-        };
+//        void revertRobotStateToCurrentAll (){
+//            hrp::setQAll(m_robot, qorg);
+//            m_robot->rootLink()->p = current_root_p;
+//            m_robot->rootLink()->R = current_root_R;
+//            m_robot->calcForwardKinematics();
+//        };
 
     protected:
         hrp::Vector3 omegaFromRotEx(const hrp::Matrix33& r) {//copy from JointPathEx.cpp
@@ -284,96 +292,5 @@ class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSo
         }
 };
 
-
-// // multi-thread IK (answer will delay for 1 control loop)
-//class FullbodyInverseKinematicsSolverMT : public FullbodyInverseKinematicsSolver{
-//    protected:
-//        hrp::BodyPtr _robot_copy;
-//        pthread_t ik_thread;
-//        pthread_mutex_t ik_body_mutex;  // Mutex
-//        int cur_ik_loop;
-//        std::vector<IKConstraint> ikc_list;
-//        bool is_first;
-//        bool ik_thread_kill;
-//        bool ik_thread_ik_required;
-//        hrp::Vector3 base_p_old;
-//        hrp::Matrix33 base_R_old;
-//        hrp::dvector q_old;
-//    public:
-//        FullbodyInverseKinematicsSolverMT (hrp::BodyPtr& _robot, const std::string& _print_str, const double _dt) : FullbodyInverseKinematicsSolver(_robot,_print_str, _dt) {
-//            _robot_copy = hrp::BodyPtr(new hrp::Body(*_robot));
-//            cur_ik_loop = 0;
-//            is_first = true;
-//            ik_thread_kill = false;
-//            ik_thread_ik_required = false;
-//        };
-//        ~FullbodyInverseKinematicsSolverMT () {
-//            pthread_cancel(ik_thread);
-//            ik_thread_kill = true;
-//            pthread_join(ik_thread, NULL );
-//            std::cerr<<"[FullbodyInverseKinematicsSolverMT] thread killed successfully"<<std::endl;
-//        };
-//        int solveFullbodyIKLoopMT (hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list, const int _max_iteration) {
-//            if(is_first){
-//                for(int i=0;i<J_DOF;i++){
-//                    _robot_copy->joint(i)->q = _robot->joint(i)->q;
-//                    _robot_copy->joint(i)->ulimit = _robot->joint(i)->ulimit;
-//                    _robot_copy->joint(i)->llimit = _robot->joint(i)->llimit;
-//                }
-//                _robot_copy->rootLink()->p = _robot->rootLink()->p;
-//                _robot_copy->rootLink()->R = _robot->rootLink()->R;
-//                pthread_mutex_init( &ik_body_mutex, NULL );
-//                pthread_create(&ik_thread, NULL, launchThread, this);
-//                is_first = false;
-//            }
-//            for(int i=0;i<J_DOF;i++){
-//                _robot->joint(i)->q = _robot_copy->joint(i)->q;
-//            }
-//            _robot->rootLink()->p = _robot_copy->rootLink()->p;
-//            _robot->rootLink()->R = _robot_copy->rootLink()->R;
-//            _robot->calcForwardKinematics();
-//            int result = cur_ik_loop;
-//
-//            if(pthread_mutex_lock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_lock err "<<std::endl;}
-//
-//            base_p_old = _robot_copy->rootLink()->p;
-//            base_R_old = _robot_copy->rootLink()->R;
-//            q_old.resize(J_DOF);
-//            for(int i=0;i<J_DOF;i++){ q_old(i) = _robot_copy->joint(i)->q; }
-//
-//            cur_ik_loop = 0;
-//            ikc_list = _ikc_list;
-//            ik_thread_ik_required = true;
-//            if(pthread_mutex_unlock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_unlock err "<<std::endl;}
-//            return result;
-//        }
-//        void ik_loop(){
-//            std::cerr<<"[FullbodyInverseKinematicsSolverMT] start thread"<<std::endl;
-//            while(1){
-//                usleep(0);//これ無いとmutex解放しない？
-//                if(pthread_mutex_lock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_lock2 err "<<std::endl;}
-//                if(ik_thread_ik_required){
-//                    solveFullbodyIK(_robot_copy, ikc_list);
-//                    //check ang moment
-//                    _robot_copy->rootLink()->v = (_robot_copy->rootLink()->p - base_p_old)/ m_dt;
-//                    _robot_copy->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * _robot_copy->rootLink()->R) / m_dt;
-//                    for(int i=0;i<J_DOF;i++){ _robot_copy->joint(i)->dq = (_robot_copy->joint(i)->q - q_old(i)) / m_dt; }
-//                    _robot_copy->calcForwardKinematics(true,false);
-////                    _robot_copy->calcTotalMomentum(cur_momentum_P, cur_momentum_L);
-//                    cur_ik_loop++;
-//                    if(cur_ik_loop > 10000){std::cerr<<"[FullbodyInverseKinematicsSolverMT] thread time out"<<std::endl;break;}//異常終了でスレッドが残ってしまった時にタイムアウト終了
-//                }
-//                if(pthread_mutex_unlock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_unlock2 err "<<std::endl;}
-//                if(ik_thread_kill){break;}
-//            }
-//            std::cerr<<"[FullbodyInverseKinematicsSolverMT] end of thread"<<std::endl;
-//        }
-//        static void* launchThread(void *pParam) {
-//            reinterpret_cast<FullbodyInverseKinematicsSolverMT*>(pParam)->ik_loop();
-//            return NULL;
-//        }
-//};
-
-
 #endif //  FULLBODYIK_H
 
diff --git a/rtc/WholeBodyMasterSlave/CMakeLists.txt b/rtc/WholeBodyMasterSlave/CMakeLists.txt
index 7914368f528..9d6e063df99 100644
--- a/rtc/WholeBodyMasterSlave/CMakeLists.txt
+++ b/rtc/WholeBodyMasterSlave/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(comp_sources WholeBodyMasterSlave.cpp WholeBodyMasterSlaveService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp ../TorqueFilter/IIRFilter.cpp wbms_core.cpp)
+set(comp_sources WholeBodyMasterSlave.cpp WholeBodyMasterSlaveService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp ../TorqueFilter/IIRFilter.cpp)
 set(libs ${OpenCV_LIBRARIES} hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub)
 add_library(WholeBodyMasterSlave SHARED ${comp_sources})
 target_link_libraries(WholeBodyMasterSlave ${libs})
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 10d08b03ce4..d41463e6e46 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -2,10 +2,10 @@
 #include <hrpModel/Link.h>
 #include <hrpModel/Sensor.h>
 #include <hrpModel/ModelLoaderUtil.h>
-#include "WholeBodyMasterSlave.h"
 #include <hrpModel/JointPath.h>
 #include <hrpUtil/MatrixSolvers.h>
 #include "hrpsys/util/Hrpsys.h"
+#include "WholeBodyMasterSlave.h"
 
 #define DEBUGP (loop%200==0)
 #define DEBUGP_ONCE (loop==0)
@@ -56,7 +56,6 @@ WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlo
         m_htlfIn("htlfIn", m_htlf),
         m_htrhIn("htrhIn", m_htrh),
         m_htlhIn("htlhIn", m_htlh),
-        //      m_actzmpIn("actzmpIn", m_actzmp),
         m_htheadIn("htheadIn", m_hthead),
 #ifdef USE_DEBUG_PORT
         m_htcom_dbgOut("htcom_dbgOut", m_htcom_dbg),// to ros bridge
@@ -109,7 +108,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     addInPort("htlfIn", m_htlfIn);
     addInPort("htrhIn", m_htrhIn);
     addInPort("htlhIn", m_htlhIn);
-    //    addInPort("actzmpIn", m_actzmpIn);
     addInPort("htheadIn", m_htheadIn);
 #ifdef USE_DEBUG_PORT
     addOutPort("htcom_dbgOut", m_htcom_dbgOut);// to ros bridge
@@ -144,18 +142,13 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     nameServer = nameServer.substr(0, comPos);
     RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
     m_robot = hrp::BodyPtr(new hrp::Body());
-//    m_robot_ml = hrp::BodyPtr(new hrp::Body());
-//    m_robot_vsafe = hrp::BodyPtr(new hrp::Body());
     if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
         std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
         return RTC::RTC_ERROR;
     }
-    m_robot_ml = hrp::BodyPtr(new hrp::Body(*m_robot)); //copy
-    m_robot_vsafe = hrp::BodyPtr(new hrp::Body(*m_robot)); //copy
+    m_robot_vsafe = hrp::BodyPtr(new hrp::Body(*m_robot)); //copy from m_robot
     // allocate memory for outPorts
     m_qRef.data.length(m_robot->numJoints());
-    m_htrfw.data.length(6);
-    m_htlfw.data.length(6);
     coil::stringTo(optionalDataLength, prop["seq_optional_data_dim"].c_str());
     loop = 0;
     transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
@@ -174,22 +167,12 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     q_ip->clear();
     // Generate FIK
     fik = fikPtr(new FullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
-    fik_ml = fikPtr(new FullbodyInverseKinematicsSolver(m_robot_ml, std::string(m_profile.instance_name), m_dt));
-    fik_list.push_back(fik);
-    fik_list.push_back(fik_ml);
     body_list.push_back(m_robot);
-    body_list.push_back(m_robot_ml);
     body_list.push_back(m_robot_vsafe);
 
     setupfik(fik, m_robot, prop);
-    setupfik(fik_ml, m_robot_ml, prop);
 
-    if (fik->ikp.find("rleg") != fik->ikp.end() && fik->ikp.find("lleg") != fik->ikp.end()) {
-        is_legged_robot = true;
-    } else {
-        is_legged_robot = false;
-    }
-    hsp = boost::shared_ptr<WBMSCore>(new WBMSCore(m_dt));
+    wbms = boost::shared_ptr<WBMSCore>(new WBMSCore(m_dt));
 
     invdyn_zmp_filters.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
     final_ref_zmp_filter.setParameter(5, 1/m_dt, Q_BUTTERWORTH);
@@ -206,58 +189,35 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onFinalize(){
     return RTC::RTC_OK;
 }
 
-void WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in){
-    coil::vstring end_effectors_str = coil::split(prop_in["end_effectors"], ",");
+RTC::ReturnCode_t WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in){
+    coil::vstring ee_conf_all = coil::split(prop_in["end_effectors"], ",");
     size_t prop_num = 10;
-    if (end_effectors_str.size() > 0) {
-        size_t num = end_effectors_str.size()/prop_num;
-        for (size_t i = 0; i < num; i++) {
-            std::string ee_name, ee_target, ee_base;
-            coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
-            coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
-            coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
-            ABCIKparam tp;
-            for (size_t j = 0; j < XYZ; j++) {
-                coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
-            }
-            double tmpv[4];
-            for (int j = 0; j < 4; j++ ) {
-                coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
-            }
-            tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
-            // FIK param
-            SimpleFullbodyInverseKinematicsSolver::IKparam tmp_fikp;
-            tmp_fikp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(robot_in, robot_in->link(ee_base), robot_in->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
-            tmp_fikp.target_link = robot_in->link(ee_target);
-            tmp_fikp.localPos = tp.localPos;
-            tmp_fikp.localR = tp.localR;
-            fik_in->ikp.insert(std::pair<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>(ee_name, tmp_fikp));
-            // Fix for toe joint
-            //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
-            //   Without toe joints, "end-effector link == force-sensor link" is assumed.
-            //   With toe joints, "end-effector link != force-sensor link" is assumed.
-            if (robot_in->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
-                std::vector<double> optw(fik_in->ikp[ee_name].manip->numJoints(), 1.0);
-                optw.back() = 0.0; // Set weight = 0 for toe joint by default
-                fik_in->ikp[ee_name].manip->setOptionalWeightVector(optw);
-                tp.has_toe_joint = true;
-            } else {
-                tp.has_toe_joint = false;
+    if (ee_conf_all.size() > 0) {
+        size_t ee_num = ee_conf_all.size()/prop_num;
+        for (size_t i = 0; i < ee_num; i++) {
+            std::string ee_name, target_link_name, base_name; // e.g. rleg, RLEG_JOINT5, WAIST
+            coil::stringTo(ee_name, ee_conf_all[i*prop_num].c_str());
+            coil::stringTo(target_link_name, ee_conf_all[i*prop_num+1].c_str());
+            coil::stringTo(base_name, ee_conf_all[i*prop_num+2].c_str());
+            ee_name_ikcp_map[ee_name].target_link_name = target_link_name;
+            for (size_t j = 0; j < XYZ; j++){ coil::stringTo(ee_name_ikcp_map[ee_name].localPos(j), ee_conf_all[i*prop_num+3+j].c_str()); }
+            double tmp_aa[4];
+            for (int j = 0; j < 4; j++ ){ coil::stringTo(tmp_aa[j], ee_conf_all[i*prop_num+6+j].c_str()); }
+            ee_name_ikcp_map[ee_name].localR = Eigen::AngleAxis<double>(tmp_aa[3], hrp::Vector3(tmp_aa[0], tmp_aa[1], tmp_aa[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
+            if(robot_in->link(target_link_name)){
+              RTCOUT << "End Effector [" << ee_name << "]" << std::endl;
+              RTCOUT << "   target_link_name = " << ee_name_ikcp_map[ee_name].target_link_name << ", base = " << base_name << std::endl;
+              RTCOUT << "   offset_pos = " << ee_name_ikcp_map[ee_name].localPos.transpose() << "[m]" << std::endl;
+              RTCOUT << "   has_toe_joint = " << "fix to false now" << std::endl;
+            }else{
+              RTCOUT << "Target link [" << target_link_name << "] not found !" << std::endl;
+              return RTC::RTC_ERROR;
             }
-            tp.target_link = robot_in->link(ee_target);
-            std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
-            std::cerr << "[" << m_profile.instance_name << "]   target = " << fik_in->ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
-            std::cerr << "[" << m_profile.instance_name << "]   offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
-            std::cerr << "[" << m_profile.instance_name << "]   has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
-            contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));////TODO:要移動?
-
-            eename_ikcp_map[ee_name].target_link_name = ee_target;
-            for (size_t j = 0; j < XYZ; j++){ coil::stringTo(eename_ikcp_map[ee_name].localPos(j), end_effectors_str[i*prop_num+3+j].c_str()); }
-            double tmp[4];
-            for (int j = 0; j < 4; j++ ){ coil::stringTo(tmp[j], end_effectors_str[i*prop_num+6+j].c_str()); }
-            eename_ikcp_map[ee_name].localR = Eigen::AngleAxis<double>(tmp[3], hrp::Vector3(tmp[0], tmp[1], tmp[2])).toRotationMatrix();
+            contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));////TODO:要移動? //used for check optional data order
         }
     }
+    is_legged_robot = (ee_name_ikcp_map.find("rleg") != ee_name_ikcp_map.end() && ee_name_ikcp_map.find("lleg") != ee_name_ikcp_map.end());
+    return RTC::RTC_OK;
 }
 
 #define TIMECALC 0
@@ -272,23 +232,23 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if (m_optionalDataIn.isNew()) { m_optionalDataIn.read(); }
     //for HumanSynchronizer
     if(mode.now()!=MODE_PAUSE){
-        if (m_htrfwIn.isNew()){ m_htrfwIn.read(); WBMSCore::DoubleSeqToVector6(m_htrfw.data,hsp->hp_wld_raw.tgt[rf].w); }
-        if (m_htlfwIn.isNew()){ m_htlfwIn.read(); WBMSCore::DoubleSeqToVector6(m_htlfw.data,hsp->hp_wld_raw.tgt[lf].w); }
-        if (m_htcomIn.isNew()){ m_htcomIn.read(); WBMSCore::Pose3DToWBMSPose3D(m_htcom.data,hsp->hp_wld_raw.tgt[com].abs); }
-        if (m_htrfIn.isNew()) { m_htrfIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htrf.data,hsp->hp_wld_raw.tgt[rf].abs); }
-        if (m_htlfIn.isNew()) { m_htlfIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htlf.data,hsp->hp_wld_raw.tgt[lf].abs); }
-        if (m_htrhIn.isNew()) { m_htrhIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htrh.data,hsp->hp_wld_raw.tgt[rh].abs);}
-        if (m_htlhIn.isNew()) { m_htlhIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htlh.data,hsp->hp_wld_raw.tgt[lh].abs);}
-        if (m_htheadIn.isNew()){ m_htheadIn.read(); WBMSCore::Pose3DToWBMSPose3D(m_hthead.data,hsp->hp_wld_raw.tgt[head].abs);}
-        if (m_htzmpIn.isNew()){ m_htzmpIn.read();  WBMSCore::Point3DToVector3(m_htzmp.data,hsp->hp_wld_raw.tgt[zmp].abs.p); }
+        if (m_htrfwIn.isNew()){ m_htrfwIn.read(); WBMSCore::WrenchToVector6(m_htrfw.data,wbms->hp_wld_raw.tgt[rf].w); }
+        if (m_htlfwIn.isNew()){ m_htlfwIn.read(); WBMSCore::WrenchToVector6(m_htlfw.data,wbms->hp_wld_raw.tgt[lf].w); }
+        if (m_htcomIn.isNew()){ m_htcomIn.read(); WBMSCore::Pose3DToWBMSPose3D(m_htcom.data,wbms->hp_wld_raw.tgt[com].abs); }
+        if (m_htrfIn.isNew()) { m_htrfIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htrf.data,wbms->hp_wld_raw.tgt[rf].abs); }
+        if (m_htlfIn.isNew()) { m_htlfIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htlf.data,wbms->hp_wld_raw.tgt[lf].abs); }
+        if (m_htrhIn.isNew()) { m_htrhIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htrh.data,wbms->hp_wld_raw.tgt[rh].abs);}
+        if (m_htlhIn.isNew()) { m_htlhIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htlh.data,wbms->hp_wld_raw.tgt[lh].abs);}
+        if (m_htheadIn.isNew()){ m_htheadIn.read(); WBMSCore::Pose3DToWBMSPose3D(m_hthead.data,wbms->hp_wld_raw.tgt[head].abs);}
+        if (m_htzmpIn.isNew()){ m_htzmpIn.read();  WBMSCore::Point3DToVector3(m_htzmp.data,wbms->hp_wld_raw.tgt[zmp].abs.p); }
     }
 
     //khi
-    if(m_htlfw.data[1] == 1 && m_htrfw.data[1] == 1 && mode.now() == MODE_IDLE){
+    if(m_htlfw.data.force.y == 1 && m_htrfw.data.force.y == 1 && mode.now() == MODE_IDLE){
         std::cerr<<"button call"<<std::endl;
         startWholeBodyMasterSlave();
         std::cerr<<"button call end"<<std::endl;
-    }else if(m_htlfw.data[1] == 1 && m_htrfw.data[1] == 1 && mode.now() == MODE_WBMS){
+    }else if(m_htlfw.data.force.y == 1 && m_htrfw.data.force.y == 1 && mode.now() == MODE_WBMS){
         stopWholeBodyMasterSlave();
     }
     
@@ -301,29 +261,13 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
         if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processTransition" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
-        for(int i=0; i<fik_list.size(); i++){
-            fik_list[i]->ikp["rleg"].is_ik_enable = true;
-            fik_list[i]->ikp["lleg"].is_ik_enable = true;
-            if(fik_list[i]->ikp.count("rarm"))fik_list[i]->ikp["rarm"].is_ik_enable = hsp->WBMSparam.use_rh;
-            if(fik_list[i]->ikp.count("larm"))fik_list[i]->ikp["larm"].is_ik_enable = hsp->WBMSparam.use_lh;
-        }
-
         if (mode.isRunning()) {
             if(mode.isInitialize()){
                 preProcessForWholeBodyMasterSlave(fik, m_robot);
-                hsp->fik_ml = fik_ml;
-                hsp->m_robot_ml = m_robot_ml;
-                //          hsp->fik_act = fik_vsafe;
-                hsp->m_robot_act = m_robot_vsafe;
-                //逆動力学初期化
-                idsb.setInitState(m_robot, m_dt);
+                idsb.setInitState(m_robot, m_dt);//逆動力学初期化
             }
 
-            if(hsp->WBMSparam.is_doctor){
-                processWholeBodyMasterSlave(fik, m_robot, hsp->rp_ref_out);//安全制限つきマスタ・スレーブ
-            }else{
-                processWholeBodyMasterSlave_Raw(fik, m_robot, raw_pose);//生マスタ・スレーブ
-            }
+            processWholeBodyMasterSlave(fik, m_robot, wbms->rp_ref_out);//安全制限つきマスタ・スレーブ
 
             if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processWholeBodyMasterSlave" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
@@ -338,13 +282,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
             processHOFFARBIBFilter(m_robot, m_robot_vsafe);
 
-            const hrp::BodyPtr m_robot_for_out = m_robot_vsafe;
-            //        const hrp::BodyPtr m_robot_for_out = m_robot;
-
-            //OutPortデータセット
-            //        hrp::Vector3 ref_zmp = hsp->rp_ref_out.tgt[zmp].abs.p;
-
-            hrp::Vector3 com = m_robot_for_out->calcCM();
+            hrp::Vector3 com = m_robot_vsafe->calcCM();
             static hrp::Vector3 com_old = com;
             static hrp::Vector3 com_old_old = com_old;
             hrp::Vector3 com_acc = (com - 2*com_old + com_old_old)/(m_dt*m_dt);
@@ -353,31 +291,31 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
             ref_zmp = final_ref_zmp_filter.passFilter(ref_zmp);
             com_old_old = com_old;
             com_old = com;
-            hsp->act_rs.com = com;
-            hsp->act_rs.zmp = ref_zmp;
+            wbms->act_rs.com = com;
+            wbms->act_rs.zmp = ref_zmp;
 
             const std::string tmp[] = {"R_CROTCH_R","R_CROTCH_P","R_CROTCH_Y","R_KNEE_P","R_ANKLE_R","R_ANKLE_P","L_CROTCH_R","L_CROTCH_P","L_CROTCH_Y","L_KNEE_P","L_ANKLE_R","L_ANKLE_P",};
             const std::vector<std::string> lower(tmp, tmp+12);
             // qRef
             for (int i = 0; i < m_qRef.data.length(); i++ ){
-              if(hsp->WBMSparam.disable_lower && std::find(lower.begin(), lower.end(), m_robot->joint(i)->name) != lower.end()){}// pass through lower limbs
+              if(!wbms->wp.use_lower && std::find(lower.begin(), lower.end(), m_robot->joint(i)->name) != lower.end()){}// pass through lower limbs
               else{
-                m_qRef.data[i] = transition_interpolator_ratio * m_robot_for_out->joint(i)->q  + (1 - transition_interpolator_ratio) * m_qRef.data[i];
+                m_qRef.data[i] = transition_interpolator_ratio * m_robot_vsafe->joint(i)->q  + (1 - transition_interpolator_ratio) * m_qRef.data[i];
               }
             }
             // basePos
-            m_basePos.data.x = transition_interpolator_ratio * m_robot_for_out->rootLink()->p(0) + (1 - transition_interpolator_ratio) * m_basePos.data.x;
-            m_basePos.data.y = transition_interpolator_ratio * m_robot_for_out->rootLink()->p(1) + (1 - transition_interpolator_ratio) * m_basePos.data.y;
-            m_basePos.data.z = transition_interpolator_ratio * m_robot_for_out->rootLink()->p(2) + (1 - transition_interpolator_ratio) * m_basePos.data.z;
+            m_basePos.data.x = transition_interpolator_ratio * m_robot_vsafe->rootLink()->p(0) + (1 - transition_interpolator_ratio) * m_basePos.data.x;
+            m_basePos.data.y = transition_interpolator_ratio * m_robot_vsafe->rootLink()->p(1) + (1 - transition_interpolator_ratio) * m_basePos.data.y;
+            m_basePos.data.z = transition_interpolator_ratio * m_robot_vsafe->rootLink()->p(2) + (1 - transition_interpolator_ratio) * m_basePos.data.z;
             m_basePos.tm = m_qRef.tm;
             // baseRpy
-            hrp::Vector3 baseRpy = hrp::rpyFromRot(m_robot_for_out->rootLink()->R);
+            hrp::Vector3 baseRpy = hrp::rpyFromRot(m_robot_vsafe->rootLink()->R);
             m_baseRpy.data.r = transition_interpolator_ratio * baseRpy(0) + (1 - transition_interpolator_ratio) * m_baseRpy.data.r;
             m_baseRpy.data.p = transition_interpolator_ratio * baseRpy(1) + (1 - transition_interpolator_ratio) * m_baseRpy.data.p;
             m_baseRpy.data.y = transition_interpolator_ratio * baseRpy(2) + (1 - transition_interpolator_ratio) * m_baseRpy.data.y;
             m_baseRpy.tm = m_qRef.tm;
             // zmp
-            hrp::Vector3 rel_ref_zmp = m_robot_for_out->rootLink()->R.transpose() * (ref_zmp - m_robot_for_out->rootLink()->p);
+            hrp::Vector3 rel_ref_zmp = m_robot_vsafe->rootLink()->R.transpose() * (ref_zmp - m_robot_vsafe->rootLink()->p);
             m_zmp.data.x = transition_interpolator_ratio * rel_ref_zmp(0) + (1 - transition_interpolator_ratio) * m_zmp.data.x;
             m_zmp.data.y = transition_interpolator_ratio * rel_ref_zmp(1) + (1 - transition_interpolator_ratio) * m_zmp.data.y;
             m_zmp.data.z = transition_interpolator_ratio * rel_ref_zmp(2) + (1 - transition_interpolator_ratio) * m_zmp.data.z;
@@ -387,11 +325,11 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
                 m_optionalData.data.length(optionalDataLength);//TODO:これいいのか？
                 for(int i=0;i<optionalDataLength;i++)m_optionalData.data[i] = 0;
             }
-            m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = hsp->rp_ref_out.tgt[rf].is_contact;
-            m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = hsp->rp_ref_out.tgt[lf].is_contact;
+            m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = wbms->rp_ref_out.tgt[rf].is_contact;
+            m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = wbms->rp_ref_out.tgt[lf].is_contact;
         }
-        hsp->baselinkpose.p = m_robot->rootLink()->p;
-        hsp->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
+        wbms->baselinkpose.p = m_robot->rootLink()->p;
+        wbms->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
     }
 
     // write
@@ -404,64 +342,64 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 #ifdef USE_DEBUG_PORT
     // dbg plot
     m_htcom_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[com].abs, m_htcom_dbg.data);
+    WBMSCore::WBMSPose3DToPose3D(wbms->hp_plot.tgt[com].abs, m_htcom_dbg.data);
     m_htcom_dbgOut.write();
     m_htrf_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[rf].abs, m_htrf_dbg.data);
+    WBMSCore::WBMSPose3DToPose3D(wbms->hp_plot.tgt[rf].abs, m_htrf_dbg.data);
     m_htrf_dbgOut.write();
     m_htlf_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[lf].abs, m_htlf_dbg.data);
+    WBMSCore::WBMSPose3DToPose3D(wbms->hp_plot.tgt[lf].abs, m_htlf_dbg.data);
     m_htlf_dbgOut.write();
     m_htrh_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[rh].abs, m_htrh_dbg.data);
+    WBMSCore::WBMSPose3DToPose3D(wbms->hp_plot.tgt[rh].abs, m_htrh_dbg.data);
     m_htrh_dbgOut.write();
     m_htlh_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[lh].abs, m_htlh_dbg.data);
+    WBMSCore::WBMSPose3DToPose3D(wbms->hp_plot.tgt[lh].abs, m_htlh_dbg.data);
     m_htlh_dbgOut.write();
     m_hthead_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(hsp->hp_plot.tgt[head].abs, m_hthead_dbg.data);
+    WBMSCore::WBMSPose3DToPose3D(wbms->hp_plot.tgt[head].abs, m_hthead_dbg.data);
     m_hthead_dbgOut.write();
     m_htzmp_dbg.tm = m_qRef.tm;
-    WBMSCore::Vector3ToPoint3D(hsp->rp_ref_out.tgt[zmp].abs.p,m_rpzmp_dbg.data);
+    WBMSCore::Vector3ToPoint3D(wbms->rp_ref_out.tgt[zmp].abs.p,m_rpzmp_dbg.data);
     m_htzmp_dbgOut.write();
     m_htrfw_dbg.tm = m_qRef.tm;
     m_htrfw_dbg.data.length(6);
-    WBMSCore::Vector6ToDoubleSeq(hsp->hp_plot.tgt[rf].w, m_htrfw_dbg.data);
+    WBMSCore::Vector6ToDoubleSeq(wbms->hp_plot.tgt[rf].w, m_htrfw_dbg.data);
     m_htrfw_dbgOut.write();
     m_htlfw_dbg.tm = m_qRef.tm;
     m_htlfw_dbg.data.length(6);
-    WBMSCore::Vector6ToDoubleSeq(hsp->hp_plot.tgt[lf].w, m_htlfw_dbg.data);
+    WBMSCore::Vector6ToDoubleSeq(wbms->hp_plot.tgt[lf].w, m_htlfw_dbg.data);
     m_htlfw_dbgOut.write();
     m_rpcom_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[com].abs, m_rpcom_dbg.data);
+    WBMSCore::WBMSPose3DToPose3D(wbms->rp_ref_out.tgt[com].abs, m_rpcom_dbg.data);
     m_rpcom_dbgOut.write();
     m_rprf_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[rf].abs, m_rprf_dbg.data);
+    WBMSCore::WBMSPose3DToPose3D(wbms->rp_ref_out.tgt[rf].abs, m_rprf_dbg.data);
     m_rprf_dbgOut.write();
     m_rplf_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[lf].abs, m_rplf_dbg.data);
+    WBMSCore::WBMSPose3DToPose3D(wbms->rp_ref_out.tgt[lf].abs, m_rplf_dbg.data);
     m_rplf_dbgOut.write();
     m_rprh_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[rh].abs, m_rprh_dbg.data);
+    WBMSCore::WBMSPose3DToPose3D(wbms->rp_ref_out.tgt[rh].abs, m_rprh_dbg.data);
     m_rprh_dbgOut.write();
     m_rplh_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[lh].abs, m_rplh_dbg.data);
+    WBMSCore::WBMSPose3DToPose3D(wbms->rp_ref_out.tgt[lh].abs, m_rplh_dbg.data);
     m_rplh_dbgOut.write();
     m_rphead_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(hsp->rp_ref_out.tgt[head].abs, m_rphead_dbg.data);
+    WBMSCore::WBMSPose3DToPose3D(wbms->rp_ref_out.tgt[head].abs, m_rphead_dbg.data);
     m_rphead_dbgOut.write();
     m_rpzmp_dbg.tm = m_qRef.tm;
-    WBMSCore::Vector3ToPoint3D(hsp->rp_ref_out.tgt[zmp].abs.p, m_rpzmp_dbg.data);
+    WBMSCore::Vector3ToPoint3D(wbms->rp_ref_out.tgt[zmp].abs.p, m_rpzmp_dbg.data);
     m_rpzmp_dbgOut.write();
     m_rpdcp_dbg.tm = m_qRef.tm;
-    WBMSCore::Vector3ToPoint3D(hsp->cp_dec,m_rpdcp_dbg.data);
+    WBMSCore::Vector3ToPoint3D(wbms->cp_dec,m_rpdcp_dbg.data);
     m_rpdcp_dbgOut.write();
     m_rpacp_dbg.tm = m_qRef.tm;
-    WBMSCore::Vector3ToPoint3D(hsp->cp_acc,m_rpacp_dbg.data);
+    WBMSCore::Vector3ToPoint3D(wbms->cp_acc,m_rpacp_dbg.data);
     m_rpacp_dbgOut.write();
     m_invdyn_dbg.tm = m_qRef.tm;
     m_invdyn_dbg.data.length(6);
-    WBMSCore::Vector6ToDoubleSeq(hsp->invdyn_ft,m_invdyn_dbg.data);
+    WBMSCore::Vector6ToDoubleSeq(wbms->invdyn_ft,m_invdyn_dbg.data);
     m_invdyn_dbgOut.write();
 #endif
 if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"t_last:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6<<endl; t_calc_start = t_calc_end;}
@@ -499,12 +437,15 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
     robot_in->rootLink()->p = basePos_heightChecked;
     for ( int i = 0; i < robot_in->numJoints(); i++ ){ robot_in->joint(i)->q = m_qRef.data[i]; }
     robot_in->calcForwardKinematics();
-    hrp::Vector3 init_foot_mid_coord = (fik_in->getEndEffectorPos("rleg") + fik_in->getEndEffectorPos("lleg")) / 2;
-    if( fabs((double)init_foot_mid_coord(Z)) > 1e-5 ){
-        basePos_heightChecked(Z) -= init_foot_mid_coord(Z);
-        init_foot_mid_coord(Z) = 0;
-        std::cerr<<"["<<m_profile.instance_name<<"] Input basePos height is invalid. Auto modify "<<m_basePos.data.z<<" -> "<<basePos_heightChecked(Z)<<endl;
-    }
+
+    //TODO
+//    hrp::Vector3 init_foot_mid_coord = (fik_in->getEndEffectorPos("rleg") + fik_in->getEndEffectorPos("lleg")) / 2;
+//    if( fabs((double)init_foot_mid_coord(Z)) > 1e-5 ){
+//        basePos_heightChecked(Z) -= init_foot_mid_coord(Z);
+//        init_foot_mid_coord(Z) = 0;
+//        std::cerr<<"["<<m_profile.instance_name<<"] Input basePos height is invalid. Auto modify "<<m_basePos.data.z<<" -> "<<basePos_heightChecked(Z)<<endl;
+//    }
+
     const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
 
     for(int i=0;i<body_list.size();i++){//初期姿勢でBodyをFK
@@ -513,74 +454,36 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
         for ( int j = 0; j < body_list[i]->numJoints(); j++ ){ body_list[i]->joint(j)->q = m_qRef.data[j]; }
         body_list[i]->calcForwardKinematics();
     }
-    for(int i=0;i<fik_list.size();i++){//初期姿勢でBodyをFK
-        fik_list[i]->setReferenceJointAngles();
-        for(int l=0;l<4;l++){//targetを初期化
-            if(fik_list[i]->ikp.count(robot_l_names[l])){
-                fik_list[i]->ikp[robot_l_names[l]].target_p0 = fik_list[i]->getEndEffectorPos(robot_l_names[l]);
-                fik_list[i]->ikp[robot_l_names[l]].target_r0 = fik_list[i]->getEndEffectorRot(robot_l_names[l]);
-            }
-        }
-    }
+
+    fik->q_ref << hrp::getQAll(robot_in), robot_in->rootLink()->p, hrp::rpyFromRot(robot_in->rootLink()->R);    //これいらない？
+
+
+//    fik->setReferenceJointAngles();
+//    for(int l=0;l<4;l++){//targetを初期化
+//        if(fik_list[i]->ikp.count(robot_l_names[l])){
+//            fik_list[i]->ikp[robot_l_names[l]].target_p0 = fik_list[i]->getEndEffectorPos(robot_l_names[l]);
+//            fik_list[i]->ikp[robot_l_names[l]].target_r0 = fik_list[i]->getEndEffectorRot(robot_l_names[l]);
+//        }
+//    }
 //    q_ip->clear();//clearは2回目以降しないほうがいい
     init_sync_state.resize(ROBOT_ALL_DOF);
-    init_sync_state.head(robot_in->numJoints()) = hrp::getQAll(robot_in);
-    init_sync_state.tail(6).head(3) = robot_in->rootLink()->p;
-    init_sync_state.tail(6).tail(3) = hrp::rpyFromRot(robot_in->rootLink()->R);
+    init_sync_state << hrp::getQAll(robot_in), robot_in->rootLink()->p, hrp::rpyFromRot(robot_in->rootLink()->R);
     q_ip->set(init_sync_state.data());
-    hsp->initializeRequest(fik_in, robot_in);
+    wbms->initializeRequest(robot_in, ee_name_ikcp_map);
 }
 
 
 void WholeBodyMasterSlave::processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HumanPose& pose_ref){
-    hsp->update();//////HumanSynchronizerの主要処理
-    if(DEBUGP)cout<<"update():"<<hsp->getUpdateTime()<<endl;
+    wbms->update();//////HumanSynchronizerの主要処理
+    if(DEBUGP)cout<<"update():"<<wbms->getUpdateTime()<<endl;
     if(DEBUGP)pose_ref.print();
-    solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave");
-}
-
-
-void WholeBodyMasterSlave::processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::BodyPtr& robot_in, HumanPose& pose_ref){
-    static BiquadIIRFilterVec pos_filters[num_pose_tgt], rot_filters[num_pose_tgt];
-    static unsigned int callnum;
-    for(int i=0;i<num_pose_tgt;i++){
-        if(callnum == 0){
-            pos_filters[i].setParameter(100.0, 1.0/m_dt, Q_NOOVERSHOOT);
-            rot_filters[i].setParameter(100.0, 1.0/m_dt, Q_NOOVERSHOOT);
-            pos_filters[i].reset(hsp->hp_wld_raw.tgt[i].abs.p);
-            rot_filters[i].reset(hsp->hp_wld_raw.tgt[i].abs.rpy);
-        }
-        pose_ref.tgt[i].abs.p = pos_filters[i].passFilter(hsp->hp_wld_raw.tgt[i].abs.p);
-        pose_ref.tgt[i].abs.rpy = rot_filters[i].passFilter(hsp->hp_wld_raw.tgt[i].abs.rpy);
-    }
-    callnum++;
-    if(DEBUGP){ fprintf(stderr,"\x1b[31mmaster-mode:\x1b[39m"); pose_ref.print(); }
-    //  solveFullbodyIKStrictCOM(fik_in, robot_in, pose_ref.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave_Raw");
+    solveFullbodyIK(fik_in, robot_in, pose_ref.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave");
 }
 
-void WholeBodyMasterSlave::calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robot_in){
-    const std::string names[4] = {"rleg","lleg","rarm","larm"};
-    hrp::dmatrix J,Jinv,Jnull;
-    for(int l=0; l<4; l++){
-        if(fik_in->ikp.count(names[l])){
-            fik_in->ikp[names[l]].manip->calcJacobian(J);
-            fik_in->ikp[names[l]].manip->calcJacobianInverseNullspace(J, Jinv, Jnull);
-            Eigen::JacobiSVD< Eigen::MatrixXd > svd(J.block(0,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
-            Eigen::JacobiSVD< Eigen::MatrixXd > svd_rot(J.block(3,0,3,J.cols()), Eigen::ComputeFullU | Eigen::ComputeFullV);
-            hsp->manip_mat[l] = svd.matrixU();
-            hsp->manip_mat_rot[l] = svd_rot.matrixU();
-            for(int i=0;i<3;i++){
-                hsp->manip_sv[l](i) = svd.singularValues()(i);
-                hsp->manip_sv_rot[l](i) = svd_rot.singularValues()(i);
-            }
-        }
-    }
-}
 
-#ifdef USE_NEW_FIK
-void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix){
+void WholeBodyMasterSlave::solveFullbodyIK(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix){
     std::vector<IKConstraint> ikc_list;
-    if(!hsp->WBMSparam.disable_lower){
+    if(wbms->wp.use_lower){
         IKConstraint tmp;
         tmp.target_link_name = "WAIST";
         tmp.localPos = hrp::Vector3::Zero();
@@ -594,7 +497,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
     }
-    if(hsp->WBMSparam.disable_lower){
+    if(!wbms->wp.use_lower){
       IKConstraint tmp;
       tmp.target_link_name = "WAIST";
       tmp.localPos = hrp::Vector3::Zero();
@@ -605,28 +508,28 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
       tmp.rot_precision = deg2rad(3);
       ikc_list.push_back(tmp);
     }
-    if(!hsp->WBMSparam.disable_lower){
+    if(wbms->wp.use_lower){
         IKConstraint tmp;
-        tmp.target_link_name = eename_ikcp_map["rleg"].target_link_name;
-        tmp.localPos = eename_ikcp_map["rleg"].localPos;
-        tmp.localR = eename_ikcp_map["rleg"].localR;
+        tmp.target_link_name = ee_name_ikcp_map["rleg"].target_link_name;
+        tmp.localPos = ee_name_ikcp_map["rleg"].localPos;
+        tmp.localR = ee_name_ikcp_map["rleg"].localR;
         tmp.targetPos = rf_ref.p;
         tmp.targetRpy = rf_ref.rpy;
-        if(hsp->rp_ref_out.tgt[rf].is_contact){
+        if(wbms->rp_ref_out.tgt[rf].is_contact){
             tmp.constraint_weight = hrp::dvector6::Constant(10);
         }else{
             tmp.constraint_weight = hrp::dvector6::Constant(0.1);
         }
         ikc_list.push_back(tmp);
     }
-    if(!hsp->WBMSparam.disable_lower){
+    if(wbms->wp.use_lower){
         IKConstraint tmp;
-        tmp.target_link_name = eename_ikcp_map["lleg"].target_link_name;
-        tmp.localPos = eename_ikcp_map["lleg"].localPos;
-        tmp.localR = eename_ikcp_map["lleg"].localR;
+        tmp.target_link_name = ee_name_ikcp_map["lleg"].target_link_name;
+        tmp.localPos = ee_name_ikcp_map["lleg"].localPos;
+        tmp.localR = ee_name_ikcp_map["lleg"].localR;
         tmp.targetPos = lf_ref.p;
         tmp.targetRpy = lf_ref.rpy;
-        if(hsp->rp_ref_out.tgt[lf].is_contact){
+        if(wbms->rp_ref_out.tgt[lf].is_contact){
             tmp.constraint_weight = hrp::dvector6::Constant(10);
         }else{
             tmp.constraint_weight = hrp::dvector6::Constant(0.1);
@@ -636,9 +539,9 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     const double dist = 0.0;
     {
         IKConstraint tmp;
-        tmp.target_link_name = eename_ikcp_map["rarm"].target_link_name;
-        tmp.localPos = eename_ikcp_map["rarm"].localPos;
-        tmp.localR = eename_ikcp_map["rarm"].localR;
+        tmp.target_link_name = ee_name_ikcp_map["rarm"].target_link_name;
+        tmp.localPos = ee_name_ikcp_map["rarm"].localPos;
+        tmp.localR = ee_name_ikcp_map["rarm"].localR;
         if((rh_ref.p-lh_ref.p).norm()<dist){
             tmp.targetPos = (rh_ref.p+lh_ref.p)/2 + (rh_ref.p-lh_ref.p).normalized()*dist/2;
         }else{
@@ -651,9 +554,9 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
         ikc_list.push_back(tmp);
     }{
         IKConstraint tmp;
-        tmp.target_link_name = eename_ikcp_map["larm"].target_link_name;
-        tmp.localPos = eename_ikcp_map["larm"].localPos;
-        tmp.localR = eename_ikcp_map["larm"].localR;
+        tmp.target_link_name = ee_name_ikcp_map["larm"].target_link_name;
+        tmp.localPos = ee_name_ikcp_map["larm"].localPos;
+        tmp.localR = ee_name_ikcp_map["larm"].localR;
         if((rh_ref.p-lh_ref.p).norm()<dist){
             tmp.targetPos = (rh_ref.p+lh_ref.p)/2 + (lh_ref.p-rh_ref.p).normalized()*dist/2;
         }else{
@@ -680,12 +583,12 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     //     tmp.rot_precision = deg2rad(1);
     //     ikc_list.push_back(tmp);
     // }
-    if(hsp->rp_ref_out.tgt[rf].is_contact){
+    if(wbms->rp_ref_out.tgt[rf].is_contact){
         sccp->avoid_priority.head(12).head(6).fill(4);
     }else{
         sccp->avoid_priority.head(12).head(6).fill(3);
     }
-    if(hsp->rp_ref_out.tgt[lf].is_contact){
+    if(wbms->rp_ref_out.tgt[lf].is_contact){
         sccp->avoid_priority.head(12).tail(6).fill(4);
     }else{
         sccp->avoid_priority.head(12).tail(6).fill(3);
@@ -734,7 +637,7 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     }
 
 
-    if(!hsp->WBMSparam.disable_lower){
+    if(wbms->wp.use_lower){
         IKConstraint tmp;
         tmp.target_link_name = "COM";
         tmp.localPos = hrp::Vector3::Zero();
@@ -805,9 +708,8 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
 
 
 //    fik_in->q_ref_pullback_gain.segment(6+6+3+2, 8*2).fill(0.01);//腕だけ
-//    fik_in->dq_ref_pullback.segment(6+6+3+2, 8*2).fill(deg2rad(0.1));//腕だけ
-    fik_in->q_ref_pullback_gain.fill(1);
-    fik_in->dq_ref_pullback.fill(deg2rad(1)*m_dt);
+//    fik_in->dq_ref_pullback.segment(6+6+3+2, 8*2).fill(deg2rad(WBMSparam0.1));//腕だけ
+//    fik_in->q_ref_constraint_weight.fill(1);
 
     struct timespec startT, endT;
     const int IK_MAX_LOOP = 2;
@@ -815,57 +717,6 @@ void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr
     int loop_result = fik_in->solveFullbodyIKLoop(robot_in, ikc_list, IK_MAX_LOOP);
     if(loop%100==0){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveIK"<<loop_result<<"loop" << std::endl;}
 }
-#else
-//旧ver
-void WholeBodyMasterSlave::solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix){
-    int com_ik_loop=0;
-    const int COM_IK_MAX_LOOP = 10;
-    const double COM_IK_MAX_ERROR = 1e-5;//1e-4だと乱れる
-    const std::string names[4] = {"rleg","lleg","rarm","larm"};
-    const WBMSPose3D* refs[4] = {&rf_ref, &lf_ref, &rh_ref, &lh_ref};
-    //  robot_in->rootLink()->p = 0.01*com_ref.p + 0.99*robot_in->rootLink()->p;//ベースリンク位置が迷走するのを防ぐ？
-    robot_in->rootLink()->R = hrp::rotFromRpy(com_ref.rpy);//move base link at first
-    for(int i=0;i<4;i++){
-        if(fik_in->ikp.count(names[i])){
-            fik_in->ikp[names[i]].targetPos = refs[i]->p;
-            fik_in->ikp[names[i]].targetRpy = hrp::rotFromRpy(refs[i]->rpy);
-        }
-    }
-    //  fik_in->storeCurrentParameters();
-    //  fik_in->setReferenceJointAngles();//これ入れると腕ブワーなる
-    if( robot_in->link("HEAD_JOINT0") != NULL) robot_in->link("HEAD_JOINT0")->q = head_ref.rpy(y);
-    if( robot_in->link("HEAD_JOINT1") != NULL) robot_in->link("HEAD_JOINT1")->q = head_ref.rpy(p);
-    if(fik_in->ikp.count("rarm") && fik_in->ikp.count("larm")){
-        hrp::Vector3 base2rh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["rarm"].targetPos - robot_in->rootLink()->p);
-        hrp::Vector3 base2lh = robot_in->rootLink()->R.transpose() * (fik_in->ikp["larm"].targetPos - robot_in->rootLink()->p);
-        if( robot_in->link("CHEST_JOINT0") != NULL){
-            robot_in->link("CHEST_JOINT0")->q = (base2lh(Z) - base2rh(Z)) * (10 * D2R / 1.0);
-            LIMIT_MINMAX(robot_in->link("CHEST_JOINT0")->q, robot_in->link("CHEST_JOINT0")->llimit, robot_in->link("CHEST_JOINT0")->ulimit);
-        }
-        if(robot_in->link("CHEST_JOINT2") != NULL){
-            robot_in->link("CHEST_JOINT2")->q = (base2rh(X) - base2lh(X)) * (60 * D2R / 1.0);
-            LIMIT_MINMAX(robot_in->link("CHEST_JOINT2")->q, robot_in->link("CHEST_JOINT2")->llimit, robot_in->link("CHEST_JOINT2")->ulimit);
-        }
-    }
-    hrp::Vector3 tmp_com_err = hrp::Vector3::Zero();
-    struct timespec startT, endT;
-    while( 1 ){  //COM 収束ループ
-        com_ik_loop++;
-        robot_in->rootLink()->p += tmp_com_err;
-        robot_in->calcForwardKinematics();
-        clock_gettime(CLOCK_REALTIME, &startT);
-        for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_in->ikp.begin(); it != fik_in->ikp.end(); it++ ) {
-            if (it->second.is_ik_enable) fik_in->solveLimbIK (it->second, it->first, fik_in->ratio_for_vel, false);
-        }
-        if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveLimbIK x 4" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
-        tmp_com_err = com_ref.p - robot_in->calcCM();
-        if(tmp_com_err.norm() < COM_IK_MAX_ERROR){ break; }
-        if(com_ik_loop >= COM_IK_MAX_LOOP){std::cerr << "COM constraint IK MAX loop [="<<COM_IK_MAX_LOOP<<"] exceeded!! @ "<<debug_prefix<< std::endl; break; };
-    }
-    if(com_ik_loop != 1 && DEBUGP)cout<<"com_ik_loop:"<<com_ik_loop<<" @ "<<debug_prefix<<endl;
-}
-#endif
-
 
 void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out){
     hrp::dvector goal_state(ROBOT_ALL_DOF);
@@ -909,14 +760,16 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
     //// KHI demo
     const double hand_max_vel = 180.0/0.4 /180.0*M_PI;// 0.4sec for 180deg
     std::map<std::string, double> joy_inputs;// 0 ~ 1
-    joy_inputs["L_HAND"] = m_htlfw.data[0];// tmp substitute
-    joy_inputs["R_HAND"] = m_htrfw.data[0];// tmp substitute
+    joy_inputs["L_HAND"] = m_htlfw.data.force.x;// tmp substitute
+    joy_inputs["R_HAND"] = m_htrfw.data.force.x;// tmp substitute
     for(std::map<std::string, double>::iterator it = joy_inputs.begin(); it!=joy_inputs.end(); it++){
-      LIMIT_MINMAX(it->second, 0.0, 1.0);
-      double tgt_hand_q = (-29.0 + (124.0-(-29.0))*it->second ) /180.0*M_PI;
-      const double q_old = robot_out->link(it->first)->q;
-      LIMIT_MINMAX(tgt_hand_q, q_old - hand_max_vel*m_dt, q_old + hand_max_vel*m_dt);
-      robot_out->link(it->first)->q = tgt_hand_q;
+      if(robot_out->link(it->first) != NULL){
+          LIMIT_MINMAX(it->second, 0.0, 1.0);
+          double tgt_hand_q = (-29.0 + (124.0-(-29.0))*it->second ) /180.0*M_PI;
+          const double q_old = robot_out->link(it->first)->q;
+          LIMIT_MINMAX(tgt_hand_q, q_old - hand_max_vel*m_dt, q_old + hand_max_vel*m_dt);
+          robot_out->link(it->first)->q = tgt_hand_q;
+      }
     }
 
     robot_out->rootLink()->p = ans_state.bottomRows(6).topRows(3);
@@ -925,23 +778,6 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::B
 }
 
 
-bool WholeBodyMasterSlave::startCountDownForWholeBodyMasterSlave(const double sec){//遅い
-    if(sec >= 0.0 && sec <= 30.0){
-        std::cerr << "[" << m_profile.instance_name << "] start Synchronization after "<<sec<<" [s]" << std::endl;
-        double remained_usec = sec * 10e6;
-        while (remained_usec > 0){
-            usleep(2000);
-            remained_usec -= 2000;
-        }
-        startWholeBodyMasterSlave();
-        return true;
-    }else{
-        std::cerr << "[" << m_profile.instance_name << "] Count Down Time must be 0 < T < 30 [s]"<< std::endl;
-        return false;
-    }
-}
-
-
 bool WholeBodyMasterSlave::startWholeBodyMasterSlave(){
     if(mode.now() == MODE_IDLE){
         std::cerr << "[" << m_profile.instance_name << "] startWholeBodyMasterSlave" << std::endl;
@@ -997,22 +833,20 @@ bool WholeBodyMasterSlave::stopWholeBodyMasterSlave(){
 
 bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
     std::cerr << "[" << m_profile.instance_name << "] setWholeBodyMasterSlaveParam" << std::endl;
-    hsp->WBMSparam.auto_swing_foot_landing_threshold = i_param.auto_swing_foot_landing_threshold;
-    hsp->WBMSparam.foot_vertical_vel_limit_coeff = i_param.foot_vertical_vel_limit_coeff;
-    hsp->WBMSparam.human_com_height = i_param.human_com_height;
-    hsp->WBMSparam.is_doctor = i_param.is_doctor;
-    hsp->WBMSparam.set_com_height_fix = i_param.set_com_height_fix;
-    hsp->WBMSparam.set_com_height_fix_val = i_param.set_com_height_fix_val;
-    hsp->WBMSparam.swing_foot_height_offset = i_param.swing_foot_height_offset;
-    hsp->WBMSparam.swing_foot_max_height = i_param.swing_foot_max_height;
-    hsp->WBMSparam.upper_body_rmc_ratio = i_param.upper_body_rmc_ratio;
-    hsp->WBMSparam.use_rh = hsp->WBMSparam.use_lh = i_param.use_hands;
-    hsp->WBMSparam.use_head = i_param.use_head;
-    hsp->WBMSparam.use_manipulability_limit = i_param.use_manipulability_limit;
+    wbms->wp.auto_swing_foot_landing_threshold = i_param.auto_swing_foot_landing_threshold;
+    wbms->wp.foot_vertical_vel_limit_coeff = i_param.foot_vertical_vel_limit_coeff;
+    wbms->wp.human_com_height = i_param.human_com_height;
+    wbms->wp.set_com_height_fix = i_param.set_com_height_fix;
+    wbms->wp.set_com_height_fix_val = i_param.set_com_height_fix_val;
+    wbms->wp.swing_foot_height_offset = i_param.swing_foot_height_offset;
+    wbms->wp.swing_foot_max_height = i_param.swing_foot_max_height;
+    wbms->wp.upper_body_rmc_ratio = i_param.upper_body_rmc_ratio;
     if(mode.now() == MODE_IDLE){
-      hsp->WBMSparam.disable_lower = i_param.disable_lower;
+        wbms->wp.use_head = i_param.use_head;
+        wbms->wp.use_upper = i_param.use_upper;
+        wbms->wp.use_lower = i_param.use_lower;
     }else{
-      std::cerr << "[" << m_profile.instance_name << "] disable_lower can be changed in MODE_IDLE" << std::endl;
+      RTCOUT << "use_head, use_upper, use_lower can be changed in MODE_IDLE" << std::endl;
     }
     return true;
 }
@@ -1020,19 +854,17 @@ bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBody
 
 bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
     std::cerr << "[" << m_profile.instance_name << "] getWholeBodyMasterSlaveParam" << std::endl;
-    i_param.auto_swing_foot_landing_threshold = hsp->WBMSparam.auto_swing_foot_landing_threshold;
-    i_param.foot_vertical_vel_limit_coeff = hsp->WBMSparam.foot_vertical_vel_limit_coeff;
-    i_param.human_com_height = hsp->WBMSparam.human_com_height;
-    i_param.is_doctor = hsp->WBMSparam.is_doctor;
-    i_param.set_com_height_fix = hsp->WBMSparam.set_com_height_fix;
-    i_param.set_com_height_fix_val = hsp->WBMSparam.set_com_height_fix_val;
-    i_param.swing_foot_height_offset = hsp->WBMSparam.swing_foot_height_offset;
-    i_param.swing_foot_max_height = hsp->WBMSparam.swing_foot_max_height;
-    i_param.upper_body_rmc_ratio = hsp->WBMSparam.upper_body_rmc_ratio;
-    i_param.use_hands = (hsp->WBMSparam.use_rh || hsp->WBMSparam.use_lh);
-    i_param.use_head = hsp->WBMSparam.use_head;
-    i_param.use_manipulability_limit = hsp->WBMSparam.use_manipulability_limit;
-    i_param.disable_lower = hsp->WBMSparam.disable_lower;
+    i_param.auto_swing_foot_landing_threshold = wbms->wp.auto_swing_foot_landing_threshold;
+    i_param.foot_vertical_vel_limit_coeff = wbms->wp.foot_vertical_vel_limit_coeff;
+    i_param.human_com_height = wbms->wp.human_com_height;
+    i_param.set_com_height_fix = wbms->wp.set_com_height_fix;
+    i_param.set_com_height_fix_val = wbms->wp.set_com_height_fix_val;
+    i_param.swing_foot_height_offset = wbms->wp.swing_foot_height_offset;
+    i_param.swing_foot_max_height = wbms->wp.swing_foot_max_height;
+    i_param.upper_body_rmc_ratio = wbms->wp.upper_body_rmc_ratio;
+    i_param.use_head = wbms->wp.use_head;
+    i_param.use_upper = wbms->wp.use_upper;
+    i_param.use_lower = wbms->wp.use_lower;
     return true;
 }
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 110cb172a1d..2c180a3f8dd 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -11,16 +11,14 @@
 #include <rtm/idl/BasicDataTypeSkel.h>
 #include <rtm/idl/ExtendedDataTypesSkel.h>
 #include <hrpModel/Body.h>
-#include "../ImpedanceController/JointPathEx.h"
+//#include "../ImpedanceController/JointPathEx.h"
 // Service implementation headers
-// <rtc-template block="service_impl_h">
 #include "WholeBodyMasterSlaveService_impl.h"
 #include "interpolator.h"
-#include "../TorqueFilter/IIRFilter.h"
+//#include "../TorqueFilter/IIRFilter.h"
 #include "wbms_core.h"
-//#include "../AutoBalancer/AutoBalancer.h"
 
-using namespace RTC;
+//using namespace RTC;
 
 //#define USE_DEBUG_PORT
 
@@ -64,7 +62,6 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
     virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
     virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
     virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
-    bool startCountDownForWholeBodyMasterSlave(const double sec);
     bool startWholeBodyMasterSlave();
     bool stopWholeBodyMasterSlave();
     bool pauseWholeBodyMasterSlave();
@@ -73,103 +70,92 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
     bool getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
 
     protected:
-    TimedDoubleSeq m_qRef;
-    InPort<TimedDoubleSeq> m_qRefIn;
-    TimedPoint3D m_basePos;
-    InPort<TimedPoint3D> m_basePosIn;
-    TimedOrientation3D m_baseRpy;
-    InPort<TimedOrientation3D> m_baseRpyIn;
-    TimedPoint3D m_zmp;
-    InPort<TimedPoint3D> m_zmpIn;
-    TimedDoubleSeq m_optionalData;
-    InPort<TimedDoubleSeq> m_optionalDataIn;
-
-    TimedPose3D m_htcom;
-    InPort<TimedPose3D> m_htcomIn;
-    TimedPose3D m_htrf;
-    InPort<TimedPose3D> m_htrfIn;
-    TimedPose3D m_htlf;
-    InPort<TimedPose3D> m_htlfIn;
-    TimedPose3D m_htrh;
-    InPort<TimedPose3D> m_htrhIn;
-    TimedPose3D m_htlh;
-    InPort<TimedPose3D> m_htlhIn;
-    TimedPose3D m_hthead;
-    InPort<TimedPose3D> m_htheadIn;
-    TimedPoint3D m_htzmp;
-    InPort<TimedPoint3D> m_htzmpIn;
-    //  TimedPoint3D m_actzmp;
-    //  InPort<TimedPoint3D> m_actzmpIn;
-    TimedDoubleSeq m_htrfw;
-    InPort<TimedDoubleSeq> m_htrfwIn;
-    TimedDoubleSeq m_htlfw;
-    InPort<TimedDoubleSeq> m_htlfwIn;
+    RTC::TimedDoubleSeq m_qRef;
+    RTC::InPort<RTC::TimedDoubleSeq> m_qRefIn;
+    RTC::TimedPoint3D m_basePos;
+    RTC::InPort<RTC::TimedPoint3D> m_basePosIn;
+    RTC::TimedOrientation3D m_baseRpy;
+    RTC::InPort<RTC::TimedOrientation3D> m_baseRpyIn;
+    RTC::TimedPoint3D m_zmp;
+    RTC::InPort<RTC::TimedPoint3D> m_zmpIn;
+    RTC::TimedDoubleSeq m_optionalData;
+    RTC::InPort<RTC::TimedDoubleSeq> m_optionalDataIn;
+
+    RTC::TimedPose3D m_htcom;
+    RTC::InPort<RTC::TimedPose3D> m_htcomIn;
+    RTC::TimedPose3D m_htrf;
+    RTC::InPort<RTC::TimedPose3D> m_htrfIn;
+    RTC::TimedPose3D m_htlf;
+    RTC::InPort<RTC::TimedPose3D> m_htlfIn;
+    RTC::TimedPose3D m_htrh;
+    RTC::InPort<RTC::TimedPose3D> m_htrhIn;
+    RTC::TimedPose3D m_htlh;
+    RTC::InPort<RTC::TimedPose3D> m_htlhIn;
+    RTC::TimedPose3D m_hthead;
+    RTC::InPort<RTC::TimedPose3D> m_htheadIn;
+    RTC::TimedPoint3D m_htzmp;
+    RTC::InPort<RTC::TimedPoint3D> m_htzmpIn;
+    OpenHRP::TimedWrench m_htrfw;
+    RTC::InPort<OpenHRP::TimedWrench> m_htrfwIn;
+    OpenHRP::TimedWrench m_htlfw;
+    RTC::InPort<OpenHRP::TimedWrench> m_htlfwIn;
 
 #ifdef USE_DEBUG_PORT
     TimedPose3D m_htcom_dbg;
-    OutPort<TimedPose3D> m_htcom_dbgOut;
+    RTC::OutPort<RTC::TimedPose3D> m_htcom_dbgOut;
     TimedPose3D m_htrf_dbg;
-    OutPort<TimedPose3D> m_htrf_dbgOut;
+    RTC::OutPort<RTC::TimedPose3D> m_htrf_dbgOut;
     TimedPose3D m_htlf_dbg;
-    OutPort<TimedPose3D> m_htlf_dbgOut;
+    RTC::OutPort<RTC::TimedPose3D> m_htlf_dbgOut;
     TimedPose3D m_htrh_dbg;
-    OutPort<TimedPose3D> m_htrh_dbgOut;
+    RTC::OutPort<RTC::TimedPose3D> m_htrh_dbgOut;
     TimedPose3D m_htlh_dbg;
-    OutPort<TimedPose3D> m_htlh_dbgOut;
+    RTC::OutPort<RTC::TimedPose3D> m_htlh_dbgOut;
     TimedPose3D m_hthead_dbg;
-    OutPort<TimedPose3D> m_hthead_dbgOut;
+    RTC::OutPort<RTC::TimedPose3D> m_hthead_dbgOut;
     TimedPoint3D m_htzmp_dbg;
-    OutPort<TimedPoint3D> m_htzmp_dbgOut;
+    RTC::OutPort<RTC::TimedPoint3D> m_htzmp_dbgOut;
     TimedDoubleSeq m_htrfw_dbg;
-    OutPort<TimedDoubleSeq> m_htrfw_dbgOut;
+    RTC::OutPort<RTC::TimedDoubleSeq> m_htrfw_dbgOut;
     TimedDoubleSeq m_htlfw_dbg;
-    OutPort<TimedDoubleSeq> m_htlfw_dbgOut;
+    RTC::OutPort<RTC::TimedDoubleSeq> m_htlfw_dbgOut;
     TimedPose3D m_rpcom_dbg;
-    OutPort<TimedPose3D> m_rpcom_dbgOut;
+    RTC::OutPort<RTC::TimedPose3D> m_rpcom_dbgOut;
     TimedPose3D m_rprf_dbg;
-    OutPort<TimedPose3D> m_rprf_dbgOut;
+    RTC::OutPort<RTC::TimedPose3D> m_rprf_dbgOut;
     TimedPose3D m_rplf_dbg;
-    OutPort<TimedPose3D> m_rplf_dbgOut;
+    RTC::OutPort<RTC::TimedPose3D> m_rplf_dbgOut;
     TimedPose3D m_rprh_dbg;
-    OutPort<TimedPose3D> m_rprh_dbgOut;
+    RTC::OutPort<RTC::TimedPose3D> m_rprh_dbgOut;
     TimedPose3D m_rplh_dbg;
-    OutPort<TimedPose3D> m_rplh_dbgOut;
+    RTC::OutPort<RTC::TimedPose3D> m_rplh_dbgOut;
     TimedPose3D m_rphead_dbg;
-    OutPort<TimedPose3D> m_rphead_dbgOut;
+    RTC::OutPort<RTC::TimedPose3D> m_rphead_dbgOut;
     TimedPoint3D m_rpzmp_dbg;
-    OutPort<TimedPoint3D> m_rpzmp_dbgOut;
+    RTC::OutPort<RTC::TimedPoint3D> m_rpzmp_dbgOut;
     TimedPoint3D m_rpdcp_dbg;
-    OutPort<TimedPoint3D> m_rpdcp_dbgOut;
+    RTC::OutPort<RTC::TimedPoint3D> m_rpdcp_dbgOut;
     TimedPoint3D m_rpacp_dbg;
-    OutPort<TimedPoint3D> m_rpacp_dbgOut;
+    RTC::OutPort<RTC::TimedPoint3D> m_rpacp_dbgOut;
     TimedDoubleSeq m_invdyn_dbg;
-    OutPort<TimedDoubleSeq> m_invdyn_dbgOut;
+    RTC::OutPort<RTC::TimedDoubleSeq> m_invdyn_dbgOut;
 #endif
 
-    OutPort<TimedDoubleSeq> m_qOut;
+    RTC::OutPort<RTC::TimedDoubleSeq> m_qOut;
     RTC::OutPort<RTC::TimedPoint3D> m_zmpOut;
-    OutPort<TimedPoint3D> m_basePosOut;
-    OutPort<TimedOrientation3D> m_baseRpyOut;
-    OutPort<TimedDoubleSeq> m_optionalDataOut;
+    RTC::OutPort<RTC::TimedPoint3D> m_basePosOut;
+    RTC::OutPort<RTC::TimedOrientation3D> m_baseRpyOut;
+    RTC::OutPort<RTC::TimedDoubleSeq> m_optionalDataOut;
     RTC::CorbaPort m_WholeBodyMasterSlaveServicePort;
 
     WholeBodyMasterSlaveService_impl m_service0;
 
     private:
-    struct ABCIKparam {
-        hrp::Vector3 target_p0, localPos, adjust_interpolation_target_p0, adjust_interpolation_org_p0;
-        hrp::Matrix33 target_r0, localR, adjust_interpolation_target_r0, adjust_interpolation_org_r0;
-        hrp::Link* target_link;
-        bool is_active, has_toe_joint;
-    };
-    //  typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
-    //  typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
     typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
-    fikPtr fik, fik_ml;
-    hrp::BodyPtr m_robot, m_robot_ml, m_robot_vsafe;
-    std::vector<fikPtr> fik_list;
+    fikPtr fik;
+    hrp::BodyPtr m_robot, m_robot_vsafe;
     std::vector<hrp::BodyPtr> body_list;
-    std::map<std::string, IKConstraint> eename_ikcp_map;
+    std::map<std::string, IKConstraint> ee_name_ikcp_map;
     std::map<std::string, size_t> contact_states_index_map;
     double m_dt;
 
@@ -195,31 +181,23 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
 
     HumanPose raw_pose;
 
-    boost::shared_ptr<WBMSCore> hsp;
+    boost::shared_ptr<WBMSCore> wbms;
     boost::shared_ptr<CapsuleCollisionChecker> sccp;
 
     hrp::Vector3 torso_rot_rmc;
     ControlMode mode;
 
-    //  enum { MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE} mode, previous_mode;
-    //  bool isRunning(){ return (mode==MODE_SYNC_TO_WBMS) || (mode==MODE_WBMS) || (mode==MODE_PAUSE) || (mode==MODE_SYNC_TO_IDLE) ;}
-    //  bool isInitialize(){ return (previous_mode==MODE_IDLE) && (mode==MODE_SYNC_TO_WBMS) ;}
-    //  enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE};
-
-    void setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in);
+    RTC::ReturnCode_t setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in);
     void calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robot_in);
     void calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::Vector3 target_v_old[], WBMSPose3D& rf_ref, WBMSPose3D& lf_ref, WBMSPose3D& rh_ref, WBMSPose3D& lh_ref);
     void calcManipulabilityJointLimitForWBMS(fikPtr& fik_in, hrp::BodyPtr& robot_in);
-    void solveFullbodyIKStrictCOM(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix="");
+    void solveFullbodyIK(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix="");
     void processTransition();
     void preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in);
     void processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HumanPose& pose_ref);
-    void processWholeBodyMasterSlave_Raw(fikPtr& fik_in, hrp::BodyPtr& robot_in, HumanPose& pose_ref);
     void processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_normal_in, const HumanPose& pose_ref);
     void processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out);
     bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; };
-
-    //  void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const hrp::Vector3& max_acc, const double& max_vel, hrp::Vector3& pos_ans);
 };
 
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp
index e62ea69ca7b..4fa18b6bdbf 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp
@@ -14,11 +14,6 @@ void WholeBodyMasterSlaveService_impl::wholebodymasterslave(WholeBodyMasterSlave
   m_wholebodymasterslave = i_wholebodymasterslave;
 }
 
-CORBA::Boolean WholeBodyMasterSlaveService_impl::startCountDownForWholeBodyMasterSlave(CORBA::Double sec)
-{
-    return m_wholebodymasterslave->startCountDownForWholeBodyMasterSlave(sec);
-};
-
 CORBA::Boolean WholeBodyMasterSlaveService_impl::startWholeBodyMasterSlave()
 {
     return m_wholebodymasterslave->startWholeBodyMasterSlave();
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h
index 0f8a12c38f2..4375dc3102c 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h
@@ -2,6 +2,7 @@
 #ifndef WholeBodyMasterSlaveSERVICESVC_IMPL_H
 #define WholeBodyMasterSlaveSERVICESVC_IMPL_H
 
+#include "hrpsys/idl/HRPDataTypes.hh"
 #include "hrpsys/idl/WholeBodyMasterSlaveService.hh"
 
 using namespace OpenHRP;
@@ -15,7 +16,6 @@ class WholeBodyMasterSlaveService_impl
 public:
   WholeBodyMasterSlaveService_impl();
   virtual ~WholeBodyMasterSlaveService_impl();
-  CORBA::Boolean startCountDownForWholeBodyMasterSlave(const CORBA::Double sec);
   CORBA::Boolean startWholeBodyMasterSlave();
   CORBA::Boolean stopWholeBodyMasterSlave();
   CORBA::Boolean pauseWholeBodyMasterSlave();
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.cpp b/rtc/WholeBodyMasterSlave/wbms_core.cpp
deleted file mode 100644
index fbf1b1313ee..00000000000
--- a/rtc/WholeBodyMasterSlave/wbms_core.cpp
+++ /dev/null
@@ -1,120 +0,0 @@
-//#include "wbms_core.h"
-//
-//void HumanSynchronizer::calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const hrp::dvector6& rfwin, const hrp::dvector6& lfwin, hrp::Vector3& zmp_ans){
-//  hrp::Vector3 rfzmp,lfzmp;
-//  const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
-//  if( rfwin(fz) > 1.0e-6 ){
-//    rfzmp(0) = ( - rfwin(ty) - rfwin(fx) * F_H_OFFSET + rfwin(fz) * 0 ) / rfwin(fz) + rfpos(0);
-//    rfzmp(1) = (   rfwin(tx) - rfwin(fy) * F_H_OFFSET + rfwin(fz) * 0 ) / rfwin(fz) + rfpos(1);
-//  }
-//  if( lfwin(fz) > 1.0e-6 ){
-//    lfzmp(0) = ( - lfwin(ty) - lfwin(fx) * F_H_OFFSET + lfwin(fz) * 0 ) / lfwin(fz) + lfpos(0);
-//    lfzmp(1) = (   lfwin(tx) - lfwin(fy) * F_H_OFFSET + lfwin(fz) * 0 ) / lfwin(fz) + lfpos(1);
-//  }
-//  if( rfwin(fz) > 1.0e-6 || lfwin(fz) > 1.0e-6 ){
-//    zmp_ans(0) = ( rfzmp(0)*rfwin(fz) + lfzmp(0)*lfwin(fz) ) / ( rfwin(fz) + lfwin(fz));
-//    zmp_ans(1) = ( rfzmp(1)*rfwin(fz) + lfzmp(1)*lfwin(fz) ) / ( rfwin(fz) + lfwin(fz));
-//  }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
-//  zmp_ans(2) = 0;
-//}
-//void HumanSynchronizer::calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans){
-//  hrp::Vector4 margin_abs;
-//  hrp::Vector2 cross_pt, anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
-//  for(int direc=0;direc<4;direc++){
-//    calcCrossPointOnHull(check_point, check_point+anchor_vec[direc], hull, cross_pt);
-//    margin_abs(direc) = (cross_pt - check_point).norm();
-//  }
-//  margin_ans(0) =  margin_abs(0);
-//  margin_ans(1) = -margin_abs(1);
-//  margin_ans(2) =  margin_abs(2);
-//  margin_ans(3) = -margin_abs(3);
-//}
-//bool HumanSynchronizer::calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_will_cross){
-//  hrp::Vector2 anchor_vec = pt_out_goal - pt_in_start;
-//  for(int i=0;i<hull.size();i++){
-//    int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
-//    hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
-//    hrp::Vector2 cur_edge = nxt_pt - cur_pt;
-//    double dBunbo = hrpVector2Cross(anchor_vec,cur_edge);
-//    if( dBunbo != 0.0) {//平行の場合を外す
-//      hrp::Vector2 vectorAC = hrp::Vector2(hull[i](0),hull[i](1)) - pt_in_start;
-//      double dR = hrpVector2Cross(vectorAC,cur_edge) / dBunbo;
-//      double dS = hrpVector2Cross(vectorAC,anchor_vec) / dBunbo;
-//      if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){//dRには数値誤差が乗る
-//        pt_will_cross = pt_in_start + dR * anchor_vec;
-//        //dist = dR * anchor_vec.norm();
-//        return true;
-//      }
-//    }
-//  }
-//  return false;
-//}
-//double HumanSynchronizer::calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_ans){
-//  double cur_nearest_dist, ans_nearest_dist;
-//  hrp::Vector2 cur_nearest_pt, ans_nearest_pt;
-//  for(int i=0;i<hull.size();i++){
-//    int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
-//    hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
-//    hrp::Vector2 cur_edge = nxt_pt - cur_pt;
-//    hrp::Vector2 tgt_pt_v = tgt_pt - cur_pt;
-//    double tgt_pt_projected_length = tgt_pt_v.dot(cur_edge.normalized());
-//    if(tgt_pt_projected_length > cur_edge.norm() ){//tgt_pt's nearest point is on the　i+1-th vertex
-//      cur_nearest_pt = nxt_pt;
-//    }else if(tgt_pt_projected_length < 0 ){//tgt_pt's nearest point is on the　i-th vertex
-//      cur_nearest_pt = cur_pt;
-//    }else{//tgt_pt's nearest point is on the line
-//      cur_nearest_pt = cur_pt + tgt_pt_projected_length * cur_edge.normalized();
-//    }
-//    cur_nearest_dist = (tgt_pt - cur_nearest_pt).norm();
-//    if(i==0){//set first candidate as nearest
-//      ans_nearest_dist = cur_nearest_dist;
-//      ans_nearest_pt = cur_nearest_pt;
-//    }else if( cur_nearest_dist < ans_nearest_dist ){//update nearest candidate
-//      ans_nearest_dist = cur_nearest_dist;
-//      ans_nearest_pt = cur_nearest_pt;
-//    }
-//  }
-//  pt_ans = ans_nearest_pt;
-//  return ans_nearest_dist;
-//}
-//void HumanSynchronizer::makeConvexHullOpenCV(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
-//  points.clear();
-//  cvhull.clear();
-//  hull_ans.clear();
-//  for(int i=0;i<pts.size();i++){ points.push_back( cv::Point2f( pts[i](0), pts[i](1)) ); }
-//  cv::convexHull(cv::Mat(points),cvhull,true);
-//  for(int i=0;i<cvhull.size();i++){ hull_ans.push_back( hrp::Vector2( cvhull[i].x, cvhull[i].y ) );  }
-//}
-////void HumanSynchronizer::makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
-////  const int dim = 2;
-////  double points[dim*pts.size()];
-////  for(int i=0;i<pts.size();i++){ points[dim*i] = pts[i](0); points[dim*i+1] = pts[i](1); }
-////  qh_new_qhull( dim, pts.size(), points, false, "qhull ", NULL, stderr);
-////  pointT *point, *pointtemp;
-////  hull_ans.clear();
-////  FORALLpoints hull_ans.push_back( hrp::Vector2(point[0],point[1]) );
-////  qh_freeqhull(!qh_ALL);
-////  //#define qh_ORIENTclock 1
-////}
-//bool HumanSynchronizer::isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull){
-//  cvhull.clear();
-//  for(int i=0;i<hull.size();i++){ cvhull.push_back( cv::Point2f( hull[i](0), hull[i](1)) ); }
-//  return (cv::pointPolygonTest(cv::Mat(cvhull), cv::Point2f(pt(0),pt(1)), false) > 0);
-//}
-//void HumanSynchronizer::applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
-//  comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
-//  comacc = acc4zmp_v_filters.passFilter(comacc);
-//  zmpout(0) = comin(0)-(H_cur/G)*comacc(0);
-//  zmpout(1) = comin(1)-(H_cur/G)*comacc(1);
-//  com_oldold = com_old;
-//  com_old = comin;
-//}
-////void HumanSynchronizer::applyVelLimit(const HumanPose& in, HumanPose& out_old, HumanPose& out){
-////  const int ns[] = {com,rf,lf,rh,lh,head}, num_ns = sizeof(ns)/sizeof(int);
-////  for(int i=0;i<num_ns;i++){
-////    hrp::Vector3 diff = in.P[i].p  - out_old.P[i].p;
-////    for(int j=0;j<3;j++)LIMIT_MINMAX( diff(j), -MAXVEL*DT, MAXVEL*DT);
-////    out.P[i].p = out_old.P[i].p + diff;
-////  }
-////  out_old = out;
-////}
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 5a049a3390c..3f09a8350af 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -36,7 +36,6 @@ extern "C" {
 }
 #endif
 
-//#include "../AutoBalancer/AutoBalancer.h"
 #include "../AutoBalancer/FullbodyInverseKinematicsSolver.h"
 #define USE_NEW_FIK
 
@@ -47,6 +46,7 @@ extern "C" {
 #define LIMIT_MAX(x,max) (x= ( x<max ? x:max ))
 #define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
 #define SGN(x) ((x)>=0 ? 1 : -1)
+#define RTCOUT std::cerr << "[" << m_profile.instance_name << "] "
 
 class UTIL_CONST{
     public:
@@ -74,7 +74,6 @@ class BiquadIIRFilterVec : UTIL_CONST {
         IIRFilter filters[XYZ];
         hrp::Vector3 ans;
     public:
-        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
         BiquadIIRFilterVec(){}
         ~BiquadIIRFilterVec(){}
         void setParameter(const hrp::Vector3& fc_in, const double& HZ, const double& Q = 0.5){ for(int i=0;i<XYZ;i++){ filters[i].setParameterAsBiquad((double)fc_in(i), Q, HZ); } }
@@ -85,7 +84,6 @@ class BiquadIIRFilterVec : UTIL_CONST {
 
 class WBMSPose3D{
     public:
-        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
         hrp::Vector3 p,rpy;
         WBMSPose3D(){ clear(); }
         ~WBMSPose3D(){}
@@ -94,7 +92,6 @@ class WBMSPose3D{
 
 class PoseTGT{
     public:
-        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
         WBMSPose3D abs, offs, cnt;
         hrp::dvector6 w;
         bool is_contact, go_contact;
@@ -131,6 +128,15 @@ namespace hrp{
                 r = r_in;
             };
     };
+//    class Wrench{
+//        public:
+//            hrp::Vector3 f, t;
+//            Wrench(const hrp::Vector3 f_in = hrp::Vector3::Zero(), const hrp::Vector3 t_in = hrp::Vector3::Zero()){
+//              f = f_in;
+//              t = t_in;
+//            }
+//            Wrench operator+(const Wrench& a, const Wrench& b){ return Wrench(a.f+b.f, a.t+b.t); }
+//    };
 }
 
 
@@ -389,27 +395,20 @@ class WBMSCore : UTIL_CONST {
                 double auto_swing_foot_landing_threshold;
                 double foot_vertical_vel_limit_coeff;
                 double human_com_height;
-                bool is_doctor;
                 bool set_com_height_fix;
                 double set_com_height_fix_val;
                 double swing_foot_height_offset;
                 double swing_foot_max_height;
                 double upper_body_rmc_ratio;
-                bool use_rh,use_lh;
                 bool use_head;
-                bool use_manipulability_limit;
-                bool disable_lower;
+                bool use_upper;
+                bool use_lower;
         };
-        struct WBMSparameters WBMSparam;
+        struct WBMSparameters wp;
         struct ActualRobotState {
                 hrp::Vector3 com, zmp;
         }act_rs;
 
-        typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
-        fikPtr fik_ml;
-        hrp::BodyPtr m_robot_ml;
-        //    fikPtr fik_act;
-        hrp::BodyPtr m_robot_act;
         double cur_manip_val[4][3];
         hrp::Vector3 manip_direc[4][3];
         hrp::Matrix33 manip_mat[4];
@@ -462,19 +461,17 @@ class WBMSCore : UTIL_CONST {
             cp_force_go_contact[L] = false;
 
             //      WBMSparam.auto_swing_foot_landing_threshold = 0.02;
-            WBMSparam.auto_swing_foot_landing_threshold = 0.04;
-            WBMSparam.foot_vertical_vel_limit_coeff = 4.0;
-            WBMSparam.human_com_height = 1.00;
-            WBMSparam.is_doctor = true;
-            WBMSparam.set_com_height_fix = false;
-            WBMSparam.set_com_height_fix_val = 0.02;
-            WBMSparam.swing_foot_height_offset = 0.02;
-            WBMSparam.swing_foot_max_height = 0.5;
-            WBMSparam.upper_body_rmc_ratio = 0.0;
-            WBMSparam.use_rh = WBMSparam.use_lh = true;
-            WBMSparam.use_head = true;
-            WBMSparam.use_manipulability_limit = true;
-            WBMSparam.disable_lower = true;
+            wp.auto_swing_foot_landing_threshold = 0.04;
+            wp.foot_vertical_vel_limit_coeff = 4.0;
+            wp.human_com_height = 1.00;
+            wp.set_com_height_fix = false;
+            wp.set_com_height_fix_val = 0.02;
+            wp.swing_foot_height_offset = 0.02;
+            wp.swing_foot_max_height = 0.5;
+            wp.upper_body_rmc_ratio = 0.0;
+            wp.use_head = true;
+            wp.use_upper = true;
+            wp.use_lower = true;
             rp_ref_out_old.clear();
             rp_ref_out.clear();
             rp_ref_vel_old.clear();
@@ -513,13 +510,13 @@ class WBMSCore : UTIL_CONST {
                 hp_wld_raw.tgt[i].offs.rpy =  hp_wld_raw.tgt[i].abs.rpy;
             }
         }
-        void initializeRobotPoseFromHRPBody(fikPtr& fik_in, hrp::BodyPtr& robot_in){
+        void initializeRobotPoseFromHRPBody(const hrp::BodyPtr robot_in, std::map<std::string, IKConstraint>& ee_name_ikcp_map_in){
             const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
             const int human_l_names[4] = {rf,lf,rh,lh};
             for(int i=0;i<4;i++){//HumanSynchronizerの初期姿勢オフセットをセット
-                if(fik_in->ikp.count(robot_l_names[i])){
-                    rp_ref_out.tgt[human_l_names[i]].offs.p = fik_in->getEndEffectorPos(robot_l_names[i]);
-                    rp_ref_out.tgt[human_l_names[i]].offs.rpy = hrp::rpyFromRot(fik_in->getEndEffectorRot(robot_l_names[i]));
+                if(ee_name_ikcp_map_in.count(robot_l_names[i])){
+                    rp_ref_out.tgt[human_l_names[i]].offs.p = ee_name_ikcp_map_in[robot_l_names[i]].getCurrentTargetPos(robot_in);//          fik_in->getEndEffectorPos(robot_l_names[i]);
+                    rp_ref_out.tgt[human_l_names[i]].offs.rpy = hrp::rpyFromRot(ee_name_ikcp_map_in[robot_l_names[i]].getCurrentTargetRot(robot_in));
                     rp_ref_out.tgt[human_l_names[i]].abs = rp_ref_out.tgt[human_l_names[i]].offs;
                 }
             }
@@ -528,7 +525,7 @@ class WBMSCore : UTIL_CONST {
             rp_ref_out.tgt[com].offs.rpy = hrp::rpyFromRot(robot_in->rootLink()->R);
             rp_ref_out.tgt[zmp].offs.p(X) = rp_ref_out.tgt[com].offs.p(X);
             rp_ref_out.tgt[zmp].offs.p(Y) = rp_ref_out.tgt[com].offs.p(Y);
-            rp_ref_out.tgt[zmp].offs.p(Z) = (fik_in->getEndEffectorPos("rleg")(Z)+fik_in->getEndEffectorPos("lleg")(Z))/2;
+            rp_ref_out.tgt[zmp].offs.p(Z) = (rp_ref_out.tgt[rf].offs.p(Z) + rp_ref_out.tgt[lf].offs.p(Z)) / 2;
             rp_ref_out.tgt[rf].cnt = rp_ref_out.tgt[rf].offs;
             rp_ref_out.tgt[lf].cnt = rp_ref_out.tgt[lf].offs;
             baselinkpose.p = robot_in->rootLink()->p;
@@ -538,11 +535,11 @@ class WBMSCore : UTIL_CONST {
             }
             H_cur = rp_ref_out.tgt[com].offs.p(Z) - std::min((double)rp_ref_out.tgt[rf].offs.p(Z), (double)rp_ref_out.tgt[rf].offs.p(Z));
         }
-        void initializeRequest(fikPtr& fik_in, hrp::BodyPtr& robot_in){
+        void initializeRequest(hrp::BodyPtr robot_in, std::map<std::string, IKConstraint>& ee_name_ikcp_map_in){
             loop = 0;
             is_initial_loop = true;
             initializeHumanPoseFromCurrentInput();
-            initializeRobotPoseFromHRPBody(fik_in, robot_in);
+            initializeRobotPoseFromHRPBody(robot_in, ee_name_ikcp_map_in);
             rp_ref_out_old = rp_ref_vel_old = rp_ref_out;
         }
         void update(){//////////  メインループ  ////////////
@@ -551,7 +548,7 @@ class WBMSCore : UTIL_CONST {
             autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);//入力の左右反転を常にチェック(＝手足の交差は不可能)
             convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
             hp_plot = rp_ref_out;
-            if(WBMSparam.set_com_height_fix)rp_ref_out.tgt[com].abs.p(Z) = rp_ref_out.tgt[com].offs.p(Z) + WBMSparam.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
+            if(wp.set_com_height_fix)rp_ref_out.tgt[com].abs.p(Z) = rp_ref_out.tgt[com].offs.p(Z) + wp.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
             judgeFootLandOnCommandByFootForce   (hp_wld_raw);//人体足裏反力から各足の接地指令を生成
             lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
             limitEEWorkspace                    (rp_ref_out);
@@ -570,11 +567,10 @@ class WBMSCore : UTIL_CONST {
                 LIMIT_MIN( rp_ref_out.tgt[l[i]].abs.p(Z), rp_ref_out_old.tgt[l[i]].abs.p(Z)-vertical_max_vel*DT);
             }
 
-            //      limitManipulability                 (rp_ref_out);
-
             applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
 
-            Vector3ToVector2(rp_ref_out.tgt[com].abs.p,com_forcp_ref);
+            com_forcp_ref = rp_ref_out.tgt[com].abs.p.head(XY);
+//            Vector3ToVector2(rp_ref_out.tgt[com].abs.p,com_forcp_ref);
             static hrp::Vector2 com_forcp_ref_old;
             com_vel_forcp_ref = (com_forcp_ref - com_forcp_ref_old)/DT;
             com_forcp_ref_old = com_forcp_ref;
@@ -604,24 +600,31 @@ class WBMSCore : UTIL_CONST {
             is_initial_loop = false;
             gettimeofday(&t_calc_end, NULL);
         }
-        static void Vector3ToVector2(const hrp::Vector3& in, hrp::Vector2& out){ out(X) = in(X); out(Y) = in(Y);}
-        static void Vector2ToVector3(const hrp::Vector2& in, hrp::Vector3& out){ out(X) = in(X); out(Y) = in(Y);}
-        static void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out(X) = in.x; out(Y) = in.y; out(Z) = in.z;}
-        static void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out(X) = in.r; out(Y) = in.p; out(Z) = in.y;}
+        static void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out << in.x, in.y, in.z;}
+        static void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out << in.r, in.p, in.y;}
         static void Vector3ToPoint3D(const hrp::Vector3& in, RTC::Point3D& out){ out.x = in(X); out.y = in(Y); out.z = in(Z);}
         static void Vector3ToOriantation3D(const hrp::Vector3& in, RTC::Orientation3D& out){ out.r = in(X); out.p = in(Y); out.y = in(Z);}
-        static void Pose3DToWBMSPose3D(const RTC::Pose3D& in, WBMSPose3D& out){ Point3DToVector3(in.position,out.p); Oriantation3DToVector3(in.orientation,out.rpy); }
-        static void WBMSPose3DToPose3D(const WBMSPose3D& in, RTC::Pose3D& out){ Vector3ToPoint3D(in.p,out.position); Vector3ToOriantation3D(in.rpy,out.orientation); }
-        static void DoubleSeqToVector6(const RTC::TimedDoubleSeq::_data_seq& in, hrp::dvector6& out){
-            if(in.length() == 6){out(fx)=in[0]; out(fy)=in[1]; out(fz)=in[2]; out(tz)=in[3]; out(ty)=in[4]; out(tz)=in[5];
-            }else{ std::cerr<<"[WARN] HumanPose::hrp::dvector6 DoubleSeqTohrp::dvector6() invalid data length"<<std::endl; out = hrp::dvector6::Zero(); }
+        static void Pose3DToWBMSPose3D(const RTC::Pose3D& in, WBMSPose3D& out){ Point3DToVector3(in.position, out.p); Oriantation3DToVector3(in.orientation, out.rpy); }
+        static void WBMSPose3DToPose3D(const WBMSPose3D& in, RTC::Pose3D& out){ Vector3ToPoint3D(in.p, out.position); Vector3ToOriantation3D(in.rpy, out.orientation); }
+//        static void DoubleSeqToVector6(const RTC::TimedDoubleSeq::_data_seq& in, hrp::dvector6& out){
+//            if(in.length() == 6){out(fx)=in[0]; out(fy)=in[1]; out(fz)=in[2]; out(tz)=in[3]; out(ty)=in[4]; out(tz)=in[5];
+//            }else{ std::cerr<<"[WARN] HumanPose::hrp::dvector6 DoubleSeqTohrp::dvector6() invalid data length"<<std::endl; out = hrp::dvector6::Zero(); }
+//        }
+//        static void Vector6ToDoubleSeq(const hrp::dvector6& in, RTC::TimedDoubleSeq::_data_seq& out){
+//            if(out.length() == 6){out[0]=in(fx); out[1]=in(fy); out[2]=in(fz); out[3]=in(tx); out[4]=in(ty); out[5]=in(tz);
+//            }else{ std::cerr<<"[WARN] HumanPose::hrp::dvector6 DoubleSeqTohrp::dvector6() invalid data length"<<std::endl; }
+//        }
+        static void WrenchToVector6(const OpenHRP::Wrench& in, hrp::dvector6& out){
+          out.head(3) << in.force.x, in.force.y, in.force.z;
+          out.tail(3) << in.torque.x, in.torque.y, in.torque.z;
         }
-        static void Vector6ToDoubleSeq(const hrp::dvector6& in, RTC::TimedDoubleSeq::_data_seq& out){
-            if(out.length() == 6){out[0]=in(fx); out[1]=in(fy); out[2]=in(fz); out[3]=in(tx); out[4]=in(ty); out[5]=in(tz);
-            }else{ std::cerr<<"[WARN] HumanPose::hrp::dvector6 DoubleSeqTohrp::dvector6() invalid data length"<<std::endl; }
+        static void Vector6ToWrench(const hrp::dvector6& in, OpenHRP::Wrench& out){
+            out.force.x = in(0);            out.force.y = in(1);            out.force.z = in(2);
+            out.torque.x = in(3);            out.torque.y = in(4);            out.torque.z = in(5);
         }
         static double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(X)*b(Y)-a(Y)*b(X); }
 
+
     private:
         void updateParams(){
             updateHumanToRobotRatio(tgt_h2r_ratio);
@@ -730,8 +733,8 @@ class WBMSCore : UTIL_CONST {
             //      hrp::Vector2 lf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[lf].abs.p(X),old.tgt[lf].abs.p(Y));
             hrp::Vector2 rf2zmp = act_rs.zmp.head(XY) - old.tgt[rf].abs.p.head(XY);
             hrp::Vector2 lf2zmp = act_rs.zmp.head(XY) - old.tgt[lf].abs.p.head(XY);
-            if( inside_vec_rf.dot(rf2zmp) / inside_vec_rf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ out.tgt[lf].go_contact = true; }
-            if( inside_vec_lf.dot(lf2zmp) / inside_vec_lf.norm() > WBMSparam.auto_swing_foot_landing_threshold ){ out.tgt[rf].go_contact = true; }
+            if( inside_vec_rf.dot(rf2zmp) / inside_vec_rf.norm() > wp.auto_swing_foot_landing_threshold ){ out.tgt[lf].go_contact = true; }
+            if( inside_vec_lf.dot(lf2zmp) / inside_vec_lf.norm() > wp.auto_swing_foot_landing_threshold ){ out.tgt[rf].go_contact = true; }
         }
         void limitEEWorkspace(HumanPose& out){
             const double MAX_FW = 0.25;
@@ -775,33 +778,7 @@ class WBMSCore : UTIL_CONST {
             //      for(int i=0, l[5]={rf,lf,rh,lh,head}; i<5; i++){
             //        for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( out.tgt[l[i]].abs.rpy(j), rc.ee_rot_limit[l[i]][MIN](j) + out.tgt[com].abs.rpy(j), rc.ee_rot_limit[l[i]][MAX](j) + out.tgt[com].abs.rpy(j) ); }
             //      }
-            if(!WBMSparam.use_head)out.tgt[head].abs.rpy = hrp::Vector3::Zero();
-        }
-        void limitManipulability(HumanPose& out){
-            const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
-            const int human_l_names[4] = {rf,lf,rh,lh};
-            m_robot_ml->rootLink()->p = m_robot_act->rootLink()->p;
-            m_robot_ml->rootLink()->R = m_robot_act->rootLink()->R;
-            //      for (int i=0;i<m_robot_ml->numJoints();i++){ m_robot_ml->joint(i)->q = m_robot_act->joint(i)->q; }
-            m_robot_ml->calcForwardKinematics();
-            for(int i=0;i<4;i++){
-                if(fik_ml->ikp.count(robot_l_names[i])){
-                    fik_ml->ikp[robot_l_names[i]].target_r0 = hrp::rotFromRpy(out.tgt[human_l_names[i]].abs.rpy);
-                    fik_ml->ikp[robot_l_names[i]].target_p0 = out.tgt[human_l_names[i]].abs.p;
-                }
-            }
-            const int pre_ik_loop = 2;
-            for(int i=0;i<pre_ik_loop;i++){
-                for ( std::map<std::string, FullbodyInverseKinematicsSolver::IKparam>::iterator it = fik_ml->ikp.begin(); it != fik_ml->ikp.end(); it++ ) {
-                    if (it->second.is_ik_enable) fik_ml->solveLimbIK (it->second, it->first, fik_ml->ratio_for_vel, false);
-                }
-            }
-            for(int i=0;i<4;i++){
-                if(fik_ml->ikp.count(robot_l_names[i])){
-                    out.tgt[human_l_names[i]].abs.p = fik_ml->getEndEffectorPos(robot_l_names[i]);
-                    out.tgt[human_l_names[i]].abs.rpy = hrp::rpyFromRot(fik_ml->getEndEffectorRot(robot_l_names[i]));
-                }
-            }
+            if(!wp.use_head)out.tgt[head].abs.rpy = hrp::Vector3::Zero();
         }
         void setFootContactPoseByGoContact(HumanPose& out){
             for(int i=0, l[LR]={rf,lf}; i<LR; i++){
@@ -827,7 +804,7 @@ class WBMSCore : UTIL_CONST {
                 if(out.tgt[l[i]].go_contact || cp_force_go_contact[i]){
                     out.tgt[l[i]].abs.p(Z) = out.tgt[l[i]].offs.p(Z);
                 }else{
-                    LIMIT_MIN( out.tgt[l[i]].abs.p(Z), out.tgt[l[i]].offs.p(Z)+WBMSparam.swing_foot_height_offset);
+                    LIMIT_MIN( out.tgt[l[i]].abs.p(Z), out.tgt[l[i]].offs.p(Z)+wp.swing_foot_height_offset);
                 }
                 const double contact_threshold = 0.005;
                 out.tgt[l[i]].is_contact = ((out.tgt[l[i]].abs.p(Z)-out.tgt[l[i]].offs.p(Z)) <= contact_threshold);
@@ -890,8 +867,10 @@ class WBMSCore : UTIL_CONST {
             com_ans(X) = com_ans_old(X) + com_vel_ans_2d(X) * DT;
             com_ans(Y) = com_ans_old(Y) + com_vel_ans_2d(Y) * DT;
             com_ans_old = com_ans;
-            Vector2ToVector3(hrp::Vector2(com_ans(X),com_ans(Y)) + com_vel_ans_2d * sqrt( H_cur / G ), cp_dec);
-            Vector2ToVector3(hrp::Vector2(com_ans(X),com_ans(Y)) - com_vel_ans_2d * sqrt( H_cur / G ), cp_acc);
+//            Vector2ToVector3(hrp::Vector2(com_ans(X),com_ans(Y)) + com_vel_ans_2d * sqrt( H_cur / G ), cp_dec);
+//            Vector2ToVector3(hrp::Vector2(com_ans(X),com_ans(Y)) - com_vel_ans_2d * sqrt( H_cur / G ), cp_acc);
+            cp_dec.head(2) = hrp::Vector2(com_ans(X),com_ans(Y)) + com_vel_ans_2d * sqrt( H_cur / G );
+            cp_acc.head(2) = hrp::Vector2(com_ans(X),com_ans(Y)) - com_vel_ans_2d * sqrt( H_cur / G );
             if(DEBUG){
                 fprintf(cz_log,"com_ans: %f %f ",com_ans(X),com_ans(Y));
                 fprintf(cz_log,"com_ans_cp_d: %f %f ",cp_dec(X),cp_dec(Y));
@@ -919,7 +898,7 @@ class WBMSCore : UTIL_CONST {
             if(com_vel(Y)>0 ){ rf_landing_delay = 0; }
             if(com_vel(Y)<0 ){ lf_landing_delay = 0; }
             double foot_landing_delay = std::max(rf_landing_delay, lf_landing_delay);
-            //      const double foot_landing_delay = 0;//debug
+            //      const double foot_landing_delay =0;//debug
             //減速CP条件(現在のCPを常に両足裏で頭打ち)
 
             //      hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);//TODO implement

From f2b36e1e04ac4bad772b8d5cbbebe8f1162ee88b Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 26 Apr 2019 20:58:03 +0900
Subject: [PATCH 153/217] refactor

---
 .../FullbodyInverseKinematicsSolver.h         | 129 ++-
 rtc/AutoBalancer/MyUtils.h                    | 106 +++
 rtc/KFilter.diff                              | 753 ------------------
 .../WholeBodyMasterSlave.cpp                  | 469 +++++------
 .../WholeBodyMasterSlave.h                    | 224 +++---
 rtc/WholeBodyMasterSlave/wbms_core.h          | 137 +---
 6 files changed, 507 insertions(+), 1311 deletions(-)
 create mode 100644 rtc/AutoBalancer/MyUtils.h
 delete mode 100644 rtc/KFilter.diff

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 7374bf1ffc8..e808229adde 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -1,25 +1,14 @@
 #ifndef FULLBODYIK_H
 #define FULLBODYIK_H
 
-#include <float.h>
-#include <hrpModel/Body.h>
-#include <hrpUtil/MatrixSolvers.h>
-#include "../ImpedanceController/JointPathEx.h"
+#include <iomanip>
+#include <hrpModel/JointPath.h>
 #include "../ImpedanceController/RatsMatrix.h"
 #include "../TorqueFilter/IIRFilter.h"
-#include <unsupported/Eigen/MatrixFunctions>
+#include "MyUtils.h"
+//#include <unsupported/Eigen/MatrixFunctions>
+//#include <hrpUtil/MatrixSolvers.h>
 
-//tips
-#define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
-#define deg2rad(x) ((x)*M_PI/180)
-#define rad2deg(rad) (rad * 180 / M_PI)
-#define eps_eq(a, b, c)  (fabs((a)-(b)) <= c)
-#define dbg(var) std::cout<<#var"= "<<(var)<<std::endl
-#define dbgn(var) std::cout<<#var"= "<<std::endl<<(var)<<std::endl
-namespace hrp{
-    static hrp::dvector getQAll(const hrp::BodyPtr _robot){ hrp::dvector tmp(_robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ tmp(i) = _robot->joint(i)->q; } return tmp; };
-    static void setQAll(hrp::BodyPtr _robot, const hrp::dvector in){ assert(in.size() <= _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = in(i); } };
-}
 
 #define OPENHRP_PACKAGE_VERSION_320
 
@@ -52,9 +41,11 @@ class FullbodyInverseKinematicsSolver{
         std::vector<IIRFilter> am_filters;
         const double m_dt;
     public:
+        hrp::BodyPtr m_robot;
         hrp::dvector dq_weight_all, q_ref, q_ref_max_dq, q_ref_constraint_weight;
         hrp::Vector3 cur_momentum_around_COM, cur_momentum_around_COM_filtered, rootlink_rpy_llimit, rootlink_rpy_ulimit;
         FullbodyInverseKinematicsSolver (hrp::BodyPtr _robot, const std::string& _print_str, const double _dt):
+          m_robot(_robot),
           m_dt(_dt),
           WS_DOF(6),
           BASE_DOF(6),
@@ -74,16 +65,17 @@ class FullbodyInverseKinematicsSolver{
             }
         };
         ~FullbodyInverseKinematicsSolver () {};
-        bool checkIKConvergence(const hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list){
+        int numStates(){ return ALL_DOF; };
+        bool checkIKConvergence(const std::vector<IKConstraint>& _ikc_list){
             for ( int i=0; i<_ikc_list.size(); i++ ) {
-                hrp::Link* link_tgt_ptr = _robot->link(_ikc_list[i].target_link_name);
+                hrp::Link* link_tgt_ptr = m_robot->link(_ikc_list[i].target_link_name);
                 hrp::Vector3 pos_err, rot_err;
                 if(link_tgt_ptr){
                     pos_err = _ikc_list[i].targetPos - (link_tgt_ptr->p + link_tgt_ptr->R * _ikc_list[i].localPos);
                     rats::difference_rotation(rot_err, (link_tgt_ptr->R * _ikc_list[i].localR), hrp::rotFromRpy(_ikc_list[i].targetRpy));
                 }
                 else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){  // COM
-                    pos_err = _ikc_list[i].targetPos - (_robot->calcCM() + _ikc_list[i].localR * _ikc_list[i].localPos);
+                    pos_err = _ikc_list[i].targetPos - (m_robot->calcCM() + _ikc_list[i].localR * _ikc_list[i].localPos);
                     rot_err = _ikc_list[i].targetRpy - cur_momentum_around_COM;
                 }
                 else{ std::cerr<<"Unknown Link Target !!"<<std::endl; continue; }
@@ -94,36 +86,36 @@ class FullbodyInverseKinematicsSolver{
             }
             return true;
         }
-        int solveFullbodyIKLoop (hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list, const int _max_iteration) {
+        int solveFullbodyIKLoop (const std::vector<IKConstraint>& _ikc_list, const int _max_iteration) {
             #ifdef OPENHRP_PACKAGE_VERSION_320
-                hrp::Vector3 base_p_old = _robot->rootLink()->p;
-                hrp::Matrix33 base_R_old = _robot->rootLink()->R;
+                hrp::Vector3 base_p_old = m_robot->rootLink()->p;
+                hrp::Matrix33 base_R_old = m_robot->rootLink()->R;
                 hrp::dvector q_old(J_DOF);
-                for(int i=0;i<J_DOF;i++){ q_old(i) = _robot->joint(i)->q; }
+                for(int i=0;i<J_DOF;i++){ q_old(i) = m_robot->joint(i)->q; }
                 unsigned int loop;
                 for(loop=0; loop < _max_iteration;){
-                    solveFullbodyIKOnce(_robot, _ikc_list);
+                    solveFullbodyIKOnce(_ikc_list);
                     //check ang moment
-                    _robot->rootLink()->v = (_robot->rootLink()->p - base_p_old)/ m_dt;
-                    _robot->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * _robot->rootLink()->R) / m_dt;
-                    for(int i=0;i<J_DOF;i++){ _robot->joint(i)->dq = (_robot->joint(i)->q - q_old(i)) / m_dt; }
-                    _robot->calcForwardKinematics(true,false);
+                    m_robot->rootLink()->v = (m_robot->rootLink()->p - base_p_old)/ m_dt;
+                    m_robot->rootLink()->w = base_R_old * hrp::omegaFromRotEx(base_R_old.transpose() * m_robot->rootLink()->R) / m_dt;
+                    for(int i=0;i<J_DOF;i++){ m_robot->joint(i)->dq = (m_robot->joint(i)->q - q_old(i)) / m_dt; }
+                    m_robot->calcForwardKinematics(true,false);
                     hrp::Vector3 tmp_P, tmp_L;
-                    _robot->calcTotalMomentum(tmp_P, tmp_L);//calcTotalMomentumは漸化的にWorld周りの並進＋回転運動量を出す
-                    cur_momentum_around_COM = tmp_L - _robot->calcCM().cross(tmp_P);
-//                    _robot->calcTotalMomentumFromJacobian(tmp_P, cur_momentum_around_COM);//calcTotalMomentumFromJacobianは重心ヤコビアンと重心周り角運動量ヤコビアンを用いて重心周りの並進＋回転運動量を出す
+                    m_robot->calcTotalMomentum(tmp_P, tmp_L);//calcTotalMomentumは漸化的にWorld周りの並進＋回転運動量を出す
+                    cur_momentum_around_COM = tmp_L - m_robot->calcCM().cross(tmp_P);
+//                    m_robot->calcTotalMomentumFromJacobian(tmp_P, cur_momentum_around_COM);//calcTotalMomentumFromJacobianは重心ヤコビアンと重心周り角運動量ヤコビアンを用いて重心周りの並進＋回転運動量を出す
                     for(int i=0; i<3; i++){
                         cur_momentum_around_COM_filtered(i) = am_filters[i].passFilter(cur_momentum_around_COM(i));
                     }
                     loop++;
-                    if(checkIKConvergence(_robot, _ikc_list)){ break; }
+                    if(checkIKConvergence(_ikc_list)){ break; }
                 }
                 return loop;
             #else
                 std::cerr<<"solveFullbodyIKLoop() needs OPENHRP_PACKAGE_VERSION_320 !!!"<<std::endl; return -1;
             #endif
         }
-        void solveFullbodyIKOnce(hrp::BodyPtr _robot, const std::vector<IKConstraint>& _ikc_list){
+        void solveFullbodyIKOnce(const std::vector<IKConstraint>& _ikc_list){
             #ifdef OPENHRP_PACKAGE_VERSION_320
                 // count all constraint DOF and allocate Jacobian and vectors
                 int C_DOF = 0; // constraint DOF
@@ -136,27 +128,29 @@ class FullbodyInverseKinematicsSolver{
                 // set end effector constraint into Jacobian
                 int cur_cid_all = 0; //current constraint index of all constraints
                 for ( int i=0; i<_ikc_list.size(); i++ ) {
-                    hrp::Link* link_tgt_ptr = _robot->link(_ikc_list[i].target_link_name);
+                    hrp::Link* link_tgt_ptr = m_robot->link(_ikc_list[i].target_link_name);
                     hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF); //全身to拘束点へのヤコビアン
                     hrp::dvector6 dp_part; //pos + rot 速度ではなく差分
                     if(link_tgt_ptr){//ベースリンク，通常リンク共通
                         hrp::Vector3 tgt_cur_pos = link_tgt_ptr->p + link_tgt_ptr->R * _ikc_list[i].localPos;
                         hrp::Matrix33 tgt_cur_rot = link_tgt_ptr->R * _ikc_list[i].localR;
                         dp_part.head(3) =  _ikc_list[i].targetPos - tgt_cur_pos;
-                        dp_part.tail(3) = tgt_cur_rot * omegaFromRotEx(tgt_cur_rot.transpose() * hrp::rotFromRpy(_ikc_list[i].targetRpy));
-                        hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, "");
+                        dp_part.tail(3) = tgt_cur_rot * hrp::omegaFromRotEx(tgt_cur_rot.transpose() * hrp::rotFromRpy(_ikc_list[i].targetRpy));
+//                        hrp::JointPathEx tgt_jpath(m_robot, m_robot->rootLink(), link_tgt_ptr, m_dt, false, "");
+                        hrp::JointPath tgt_jpath(m_robot->rootLink(), link_tgt_ptr);
                         hrp::dmatrix J_jpath;
                         tgt_jpath.calcJacobian(J_jpath, _ikc_list[i].localPos);
                         for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ J_part.col(tgt_jpath.joint(id_in_jpath)->jointId) = J_jpath.col(id_in_jpath); } //ジョイントパスのJacobianを全身用に並び替え
                         J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
-                        J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - _robot->rootLink()->p);
+                        J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - m_robot->rootLink()->p);
                     }else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){ //重心限定
-                        dp_part.head(3) = _ikc_list[i].targetPos - _robot->calcCM();
+                        dp_part.head(3) = _ikc_list[i].targetPos - m_robot->calcCM();
 //                        dp_part.tail(3) = (_ikc_list[i].targetRpy - cur_momentum_around_COM) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける
                         dp_part.tail(3) = (_ikc_list[i].targetRpy - cur_momentum_around_COM_filtered) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける(+フィルタ)
-                        _robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
-                        _robot->calcAngularMomentumJacobian(NULL, J_am);//world座標系での角運動量を生成するヤコビアン(なので本当はCOM周りには使えない)
+                        m_robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
+                        m_robot->calcAngularMomentumJacobian(NULL, J_am);//world座標系での角運動量を生成するヤコビアン(なので本当はCOM周りには使えない)
                         J_part << J_com, J_am;
+
                     }else{ std::cerr<<"Unknown Link Target !!"<<std::endl; continue; } //不明なリンク指定
                     for(int cid=0; cid<WS_DOF; cid++){
                         if(_ikc_list[i].constraint_weight(cid) > 0.0){
@@ -172,7 +166,7 @@ class FullbodyInverseKinematicsSolver{
                 for(int qid=0; qid<ALL_DOF; qid++){
                     if(q_ref_constraint_weight(qid) > 0.0){
                         J_all.row(cur_cid_all)(qid) = 1;
-                        err_all(cur_cid_all) = (qid < J_DOF) ? (q_ref(qid) - _robot->joint(qid)->q) : (q_ref(qid) - hrp::rpyFromRot(_robot->rootLink()->R)(qid-J_DOF));
+                        err_all(cur_cid_all) = (qid < J_DOF) ? (q_ref(qid) - m_robot->joint(qid)->q) : (q_ref(qid) - hrp::rpyFromRot(m_robot->rootLink()->R)(qid-J_DOF));
                         LIMIT_MINMAX(err_all(cur_cid_all), -q_ref_max_dq(qid), q_ref_max_dq(qid));
                         constraint_weight_all(cur_cid_all) = q_ref_constraint_weight(qid);
                         cur_cid_all++;
@@ -182,9 +176,9 @@ class FullbodyInverseKinematicsSolver{
                 // joint limit avoidance (copy from JointPathEx)
                 hrp::dvector dq_weight_all_jlim = hrp::dvector::Ones(ALL_DOF);
                 for ( int j = 0; j < ALL_DOF ; j++ ) {
-                    double jang = (j<J_DOF) ? _robot->joint(j)->q       :   hrp::rpyFromRot(_robot->rootLink()->R)(j - J_DOF);
-                    double jmax = (j<J_DOF) ? _robot->joint(j)->ulimit  :   rootlink_rpy_ulimit(j - J_DOF);
-                    double jmin = (j<J_DOF) ? _robot->joint(j)->llimit  :   rootlink_rpy_llimit(j - J_DOF);
+                    double jang = (j<J_DOF) ? m_robot->joint(j)->q       :   hrp::rpyFromRot(m_robot->rootLink()->R)(j - J_DOF);
+                    double jmax = (j<J_DOF) ? m_robot->joint(j)->ulimit  :   rootlink_rpy_ulimit(j - J_DOF);
+                    double jmin = (j<J_DOF) ? m_robot->joint(j)->llimit  :   rootlink_rpy_llimit(j - J_DOF);
                     double e = deg2rad(1);
                     if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
                     } else if ( eps_eq(jang, jmax,e) ) {
@@ -218,18 +212,19 @@ class FullbodyInverseKinematicsSolver{
                 // debug print
                 static int count;
                 if(count++ % 10000 == 0){
+//                if(true){
                     std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
                     std::cout<<std::setprecision(2) << "J_all_inv=\n"<<J_all_inv<<std::setprecision(6)<<std::endl;
 //                    dbgn(selection_mat);
                     dbgn(H);
-//                    dbg(Wn(0,0));
+                    dbgn(Wn);
                     dbg(g.transpose());
                     dbg(err_all.transpose());
                     dbg(dq_all.transpose());
                     dbg(constraint_weight_all.transpose());
 //                    dbg(dq_weight_all_inv.transpose());
                     std::cout<<"q_ans_all\n";
-                    for(int i=0;i<_robot->numJoints();i++)std::cerr<<_robot->joint(i)->q<<" ";
+                    for(int i=0;i<m_robot->numJoints();i++)std::cerr<<m_robot->joint(i)->q<<" ";
 
                     for(int i=0;i<_ikc_list.size();i++){
                         dbg(_ikc_list[i].target_link_name);
@@ -240,57 +235,39 @@ class FullbodyInverseKinematicsSolver{
                 }
 
                 // update joint angles
-                for(int i=0;i<dq_all.rows();i++){ if( isnan(dq_all(i)) || isinf(dq_all(i)) ){ std::cerr <<"ERROR nan/inf is found" << std::endl; return;} }
+                for(int i=0;i<dq_all.rows();i++){ if( isnan(dq_all(i)) || isinf(dq_all(i)) ){ std::cerr <<"[FullbodyIK] ERROR nan/inf is found" << std::endl; return;} }
                 for(int i=0;i<J_DOF;i++){
-                    _robot->joint(i)->q += dq_all(i);
-                    LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit);
+                    m_robot->joint(i)->q += dq_all(i);
+                    LIMIT_MINMAX(m_robot->joint(i)->q, m_robot->joint(i)->llimit, m_robot->joint(i)->ulimit);
                 }
 
                 // update rootlink pos rot
-                _robot->rootLink()->p += dq_all.tail(6).head(3);
+                m_robot->rootLink()->p += dq_all.tail(6).head(3);
                 for(int i=0;i<3;i++){
-                    if(hrp::rpyFromRot(_robot->rootLink()->R)(i) < rootlink_rpy_llimit(i) && dq_all.tail(6).tail(3)(i) < 0) dq_all.tail(6).tail(3)(i) = 0;
-                    if(hrp::rpyFromRot(_robot->rootLink()->R)(i) > rootlink_rpy_ulimit(i) && dq_all.tail(6).tail(3)(i) > 0) dq_all.tail(6).tail(3)(i) = 0;
+                    if(hrp::rpyFromRot(m_robot->rootLink()->R)(i) < rootlink_rpy_llimit(i) && dq_all.tail(6).tail(3)(i) < 0) dq_all.tail(6).tail(3)(i) = 0;
+                    if(hrp::rpyFromRot(m_robot->rootLink()->R)(i) > rootlink_rpy_ulimit(i) && dq_all.tail(6).tail(3)(i) > 0) dq_all.tail(6).tail(3)(i) = 0;
                 }
 
 
                 hrp::Matrix33 dR;
                 hrp::Vector3 omega = dq_all.tail(6).tail(3);
                 hrp::calcRodrigues(dR, omega.normalized(), omega.norm());
-                if(!(_robot->rootLink()->R * dR).isUnitary()){
+                if(!(m_robot->rootLink()->R * dR).isUnitary()){
                     std::cerr <<"ERROR R_base_ans is not Unitary" << std::endl; return;
                 }else{
-                    _robot->rootLink()->R = _robot->rootLink()->R * dR;
+                    m_robot->rootLink()->R = m_robot->rootLink()->R * dR;
                 }
-                _robot->calcForwardKinematics();
+                m_robot->calcForwardKinematics();
             #else
                 std::cerr<<"solveFullbodyIKOnce() needs OPENHRP_PACKAGE_VERSION_320 !!!"<<std::endl;
             #endif
         }
 //        void revertRobotStateToCurrentAll (){
 //            hrp::setQAll(m_robot, qorg);
-//            m_robot->rootLink()->p = current_root_p;
-//            m_robot->rootLink()->R = current_root_R;
-//            m_robot->calcForwardKinematics();
+//            mm_robot->rootLink()->p = current_root_p;
+//            mm_robot->rootLink()->R = current_root_R;
+//            mm_robot->calcForwardKinematics();
 //        };
-
-    protected:
-        hrp::Vector3 omegaFromRotEx(const hrp::Matrix33& r) {//copy from JointPathEx.cpp
-            using ::std::numeric_limits;
-            double alpha = (r(0,0) + r(1,1) + r(2,2) - 1.0) / 2.0;
-            if(fabs(alpha - 1.0) < 1.0e-12) {   //th=0,2PI;
-                return hrp::Vector3::Zero();
-            } else {
-                double th = acos(alpha);
-                double s = sin(th);
-                if (s < numeric_limits<double>::epsilon()) {   //th=PI
-                    return hrp::Vector3( sqrt((r(0,0)+1)*0.5)*th, sqrt((r(1,1)+1)*0.5)*th, sqrt((r(2,2)+1)*0.5)*th );
-                }
-                double k = -0.5 * th / s;
-                return hrp::Vector3( (r(1,2) - r(2,1)) * k, (r(2,0) - r(0,2)) * k, (r(0,1) - r(1,0)) * k );
-            }
-        }
 };
 
 #endif //  FULLBODYIK_H
-
diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
new file mode 100644
index 00000000000..6e68f4fce75
--- /dev/null
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -0,0 +1,106 @@
+#ifndef MYUTILS_H
+#define MYUTILS_H
+
+#include <hrpModel/Body.h>
+#include <hrpModel/Link.h>
+#include <hrpUtil/Eigen3d.h>
+#include <hrpUtil/Eigen4d.h>
+#include "../RobotHardware/defs.h"
+
+enum { R, L, LR };
+//enum {X, Y, Z}; //already exist in RobotHardware/defs.h
+enum { XY = 2 };
+enum { XYZ = 3 };
+enum { r, p, y, rpy };
+enum { fx, fy, fz, tx, ty, tz, ft_xyz };
+enum { com, rf, lf, rh, lh, head, zmp, num_pose_tgt };
+enum { num_ee_tgt=4 };
+
+static const double G = 9.80665;
+static const double Q_NOOVERSHOOT = 0.5;
+static const double Q_BUTTERWORTH = 0.707106781;
+
+#define dbg(var) std::cout<<#var"= "<<(var)<<std::endl
+#define dbgn(var) std::cout<<#var"= "<<std::endl<<(var)<<std::endl
+#define RTCOUT std::cerr << "[" << m_profile.instance_name << "] "
+
+#define LIMIT_MIN(x,min) (x= ( x<min ? min:x ))
+#define LIMIT_MAX(x,max) (x= ( x<max ? x:max ))
+#define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
+#define eps_eq(a, b, c)  (fabs((a)-(b)) <= c)
+
+//inline bool eps_eq(const double& a, const double& b, const double& eps = 0.001){ return fabs((a)-(b)) <= eps;}
+//inline void LIMIT_MIN(int& x, const int& min){ x = ( x<min ? min:x );}
+//inline void LIMIT_MIN(double& x, const double& min){ x = ( x<min ? min:x );}
+//inline void LIMIT_MAX(int& x, const int& max){ x = ( x<max ? x:max );}
+//inline void LIMIT_MAX(double& x, const double& max){ x = ( x<max ? x:max );}
+//inline void LIMIT_MINMAX(int& x, const int& min, const int& max){ x = (x<min  ? min : (x<max ? x : max));}
+//inline void LIMIT_MINMAX(double& x, const double& min, const double& max){ x = (x<min  ? min : (x<max ? x : max));}
+
+
+class WBMSPose3D{
+    public:
+        hrp::Vector3 p,rpy;
+        WBMSPose3D(){ clear(); }
+        ~WBMSPose3D(){}
+        void clear(){ p = rpy = hrp::Vector3::Zero(); }
+};
+
+namespace hrp{
+    class Pose3{
+        public:
+            Pose3(const hrp::dvector6& in){ data = in; }
+            Pose3(const hrp::Vector3& _p, const hrp::Vector3& _rpy){ data << _p, _rpy; }
+            Pose3(const double& X, const double& Y, const double& Z, const double& r, const double& p, const double& y){ data << X,Y,Z,r,p,y; }
+            hrp::dvector6 data;
+            hrp::dvector6::SegmentReturnType p(){ return data.head(3); }
+            hrp::dvector6::SegmentReturnType rpy(){ return data.tail(3); }
+    };
+
+    inline hrp::Vector3         to_Vector3      (const RTC::Point3D& in)        { return hrp::Vector3(in.x, in.y, in.z); }
+    inline hrp::Vector3         to_Vector3      (const RTC::Orientation3D& in)  { return hrp::Vector3(in.r, in.p, in.y); }
+    inline hrp::dvector6        to_dvector6     (const RTC::Pose3D& in)         { return (hrp::dvector6() << in.position.x, in.position.y, in.position.z, in.orientation.r, in.orientation.p, in.orientation.y).finished(); }
+    inline hrp::Pose3           to_Pose3        (const RTC::Pose3D& in)         { return hrp::Pose3(in.position.x, in.position.y, in.position.z, in.orientation.r, in.orientation.p, in.orientation.y); }
+    inline hrp::dvector6        to_dvector6     (const OpenHRP::Wrench& in)     { return (hrp::dvector6() << in.force.x, in.force.y, in.force.z, in.torque.x, in.torque.y, in.torque.z).finished(); }
+    inline hrp::dvector         to_dvector      (const RTC::TimedDoubleSeq::_data_seq& in) { return hrp::dvector::Map(in.get_buffer(), in.length()); }
+
+    inline RTC::Point3D         to_Point3D      (const hrp::Vector3& in)        { return (RTC::Point3D){in(X),in(Y),in(Z)}; }
+    inline RTC::Orientation3D   to_Orientation3D(const hrp::Vector3& in)        { return (RTC::Orientation3D){in(X),in(Y),in(Z)}; }
+    inline RTC::Pose3D          to_Pose3D       (const hrp::dvector6& in)       { return (RTC::Pose3D){in(X),in(Y),in(Z),in(r),in(p),in(y)}; }
+    inline RTC::Pose3D          to_Pose3D       (const hrp::Pose3& in)          { return (RTC::Pose3D){in.data(X),in.data(Y),in.data(Z),in.data(r),in.data(p),in.data(y)}; }
+    inline OpenHRP::Wrench      to_Wrench       (const hrp::dvector6& in)       { return (OpenHRP::Wrench){in(X),in(Y),in(Z),in(r),in(p),in(y)}; }
+    inline RTC::TimedDoubleSeq::_data_seq  to_DoubleSeq(const hrp::dvector& in) { RTC::TimedDoubleSeq::_data_seq out(in.size()); hrp::dvector::Map(out.get_buffer(), in.size()) = in; return out; }
+
+    inline hrp::dvector getQAll         (const hrp::BodyPtr _robot)                     { hrp::dvector tmp(_robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ tmp(i) = _robot->joint(i)->q; } return tmp; }
+    inline void         setQAll         (hrp::BodyPtr _robot, const hrp::dvector& in)   { assert(in.size() <= _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = in(i); } }
+    inline hrp::dvector getRobotStateVec(const hrp::BodyPtr _robot){ return (hrp::dvector(_robot->numJoints()+6) << getQAll(_robot), _robot->rootLink()->p, hrp::rpyFromRot(_robot->rootLink()->R)).finished(); }
+    inline void         setRobotStateVec(hrp::BodyPtr _robot, const hrp::dvector& _q_bpos_brpy){
+                                                                                        assert(_q_bpos_brpy.size() == _robot->numJoints()+6); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q_bpos_brpy(i); }; _robot->rootLink()->p = _q_bpos_brpy.tail(6).head(3); _robot->rootLink()->R = hrp::rotFromRpy(_q_bpos_brpy.tail(6).tail(3)); }
+    inline void         setRobotStateVec(hrp::BodyPtr _robot, const hrp::dvector& _q, const hrp::Vector3& _bpos, const hrp::Vector3& _brpy){
+                                                                                            assert(_q.size() == _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q(i); }; _robot->rootLink()->p = _bpos; _robot->rootLink()->R = hrp::rotFromRpy(_brpy); }
+    inline void         setRobotStateVec(hrp::BodyPtr _robot, const hrp::dvector& _q, const hrp::Vector3& _bpos, const hrp::Matrix33& _bR){
+                                                                                        assert(_q.size() == _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q(i); }; _robot->rootLink()->p = _bpos; _robot->rootLink()->R = _bR; }
+
+    inline hrp::Vector3 omegaFromRotEx(const hrp::Matrix33& r) {//copy from JointPathEx.cpp
+        using ::std::numeric_limits;
+        double alpha = (r(0,0) + r(1,1) + r(2,2) - 1.0) / 2.0;
+        if(fabs(alpha - 1.0) < 1.0e-12) {   //th=0,2PI;
+            return hrp::Vector3::Zero();
+        } else {
+            double th = acos(alpha);
+            double s = sin(th);
+            if (s < numeric_limits<double>::epsilon()) {   //th=PI
+                return hrp::Vector3( sqrt((r(0,0)+1)*0.5)*th, sqrt((r(1,1)+1)*0.5)*th, sqrt((r(2,2)+1)*0.5)*th );
+            }
+            double k = -0.5 * th / s;
+            return hrp::Vector3( (r(1,2) - r(2,1)) * k, (r(2,0) - r(0,2)) * k, (r(0,1) - r(1,0)) * k );
+        }
+    }
+
+    inline void Pose3DToWBMSPose3D(const RTC::Pose3D& in, WBMSPose3D& out){ out.p = hrp::to_Vector3(in.position); out.rpy = hrp::to_Vector3(in.orientation); }
+    inline void WBMSPose3DToPose3D(const WBMSPose3D& in, RTC::Pose3D& out){ hrp::to_Point3D(in.p); hrp::to_Orientation3D(in.rpy); }
+    inline double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(X)*b(Y)-a(Y)*b(X); }
+
+}
+
+#endif //  MYUTILS_H
diff --git a/rtc/KFilter.diff b/rtc/KFilter.diff
deleted file mode 100644
index 02bbd6488c4..00000000000
--- a/rtc/KFilter.diff
+++ /dev/null
@@ -1,753 +0,0 @@
-diff --git a/rtc/KalmanFilter/CMakeLists.txt b/rtc/KalmanFilter/CMakeLists.txt
-index 711cf31..92be2d3 100644
---- a/rtc/KalmanFilter/CMakeLists.txt
-+++ b/rtc/KalmanFilter/CMakeLists.txt
-@@ -21,6 +21,6 @@ target_link_libraries(testKalmanFilterEstimation ${libs})
- set(target KalmanFilter KalmanFilterComp testKalmanFilterEstimation)
- 
- install(TARGETS ${target}
--  RUNTIME DESTINATION bin
--  LIBRARY DESTINATION lib
-+  RUNTIME DESTINATION bin CONFIGURATIONS Release Debug
-+  LIBRARY DESTINATION lib CONFIGURATIONS Release Debug
- )
-diff --git a/rtc/KalmanFilter/EKFilter.h b/rtc/KalmanFilter/EKFilter.h
-index a6b34c8..8927f62 100644
---- a/rtc/KalmanFilter/EKFilter.h
-+++ b/rtc/KalmanFilter/EKFilter.h
-@@ -1,188 +1,204 @@
--// -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; -*-
- #ifndef EKFILTER_H
- #define EKFILTER_H
- 
- #include <hrpUtil/EigenTypes.h>
- #include <iostream>
--#include "hrpsys/util/Hrpsys.h"
--
--namespace hrp{
--  typedef Eigen::Matrix<double, 7, 7> Matrix77;
--  typedef Eigen::Matrix<double, 7, 1> Vector7;
--};
-+#include "util/Hrpsys.h"
- 
- class EKFilter {
- public:
--  EKFilter()
--    : P(hrp::Matrix77::Identity() * 0.1),
--      Q(Eigen::Matrix3d::Identity() * 0.001),
--      R(Eigen::Matrix3d::Identity() * 0.03),
--      g_vec(Eigen::Vector3d(0.0, 0.0, 9.80665)),
--      z_k(Eigen::Vector3d(0.0, 0.0, 9.80665)),
--      min_mag_thre_acc(0.005), max_mag_thre_acc(0.05),
--      min_mag_thre_gyro(0.0075), max_mag_thre_gyro(0.035)
--  {
-+  EKFilter() {
-     x << 1, 0, 0, 0, 0, 0, 0;
-+    P = Eigen::Matrix<double, 7, 7>::Identity() * 5;
-+    Q = Eigen::Matrix<double, 7, 7>::Identity();
-+    Q.block<4, 4>(0, 0) *= 0.1;
-+    Q.block<3, 3>(4, 4) *= 0.001;
-+    R = Eigen::Matrix<double, 3, 3>::Zero();
-+    R(0, 0) = 4;
-+    R(1, 1) = 4;
-+    R(2, 2) = 2;
-+    g_vec = Eigen::Vector3d(0.0, 0.0, 9.80665);
-   }
- 
--  hrp::Vector7 getx() const { return x; }
-+  Eigen::Matrix<double, 7, 1> getx() { return x; }
-+
-+  Eigen::Matrix<double, 3, 1> accelerationToRpy(const double& acc_x, const double& acc_y, const double& acc_z) {
-+    /*
-+     * acc = \dot{v} + w \times v + R^T g;
-+     * -> R^T g = acc - \dot{v} + w \times v;
-+     * -> R^T g = acc;
-+     * -> acc_x = -sinb g, acc_y = sina cosb g, acc_z = cosa cosb g;
-+     */
-+    double roll = atan2(acc_y, acc_z);
-+    double pitch = atan2(-acc_x, sqrt(acc_y * acc_y + acc_z * acc_z));
-+    double yaw = 0;             /* cannot be defined only by acceleration */
-+    Eigen::Vector3d rpy = Eigen::Vector3d(roll, pitch, yaw);
-+    return rpy;
-+  }
- 
--  void calcOmega(Eigen::Matrix4d& omega, const Eigen::Vector3d& w) const {
-+  Eigen::Matrix<double, 4, 4> calcOmega(const Eigen::Vector3d& w) {
-     /* \dot{q} = \frac{1}{2} omega q */
-+    Eigen::Matrix<double, 4, 4> omega;
-     omega <<
-       0, -w[0], -w[1], -w[2],
-       w[0],     0,  w[2], -w[1],
-       w[1], -w[2],     0,  w[0],
-       w[2],  w[1], -w[0],     0;
-+    return omega;
-   }
- 
--  void calcPredictedState(hrp::Vector7& _x_a_priori,
--                          const Eigen::Vector4d& q,
--                          const Eigen::Vector3d& gyro,
--                          const Eigen::Vector3d& drift) const {
-+  Eigen::Matrix<double, 7, 1> calcPredictedState(Eigen::Matrix<double, 4, 1> q,
-+                                                 Eigen::Vector3d gyro,
-+                                                 Eigen::Vector3d drift,
-+                                                 const double& dt) {
-     /* x_a_priori = f(x, u) */
--    Eigen::Vector4d q_a_priori;
-+    Eigen::Matrix<double, 7, 1> ret;
-+    Eigen::Matrix<double, 4, 1> q_a_priori;
-     Eigen::Vector3d gyro_compensated = gyro - drift;
--    Eigen::Matrix4d omega;
--    calcOmega(omega, gyro_compensated);
--    q_a_priori = q + dt / 2 * omega * q;
--    _x_a_priori.head<4>() = q_a_priori.normalized();
--    _x_a_priori.tail<3>() = drift;
-+    q_a_priori = q + dt / 2 * calcOmega(gyro_compensated) * q;
-+    ret.block<4, 1>(0, 0) = q_a_priori;
-+    ret.block<3, 1>(4, 0) = drift;
-+    return ret;
-   }
- 
--  void calcF(hrp::Matrix77& F,
--             const Eigen::Vector4d& q,
--             const Eigen::Vector3d& gyro,
--             const Eigen::Vector3d& drift) const {
--    F = hrp::Matrix77::Identity();
-+  Eigen::Matrix<double, 7, 7> calcF(Eigen::Matrix<double, 4, 1> q,
-+                                    const Eigen::Vector3d& gyro,
-+                                    Eigen::Vector3d drift,
-+                                    const double& dt) {
-+    Eigen::Matrix<double, 7, 7> F;
-     Eigen::Vector3d gyro_compensated = gyro - drift;
--    Eigen::Matrix4d omega;
--    calcOmega(omega, gyro_compensated);
--    F.block<4, 4>(0, 0) += dt / 2 * omega;
-+    F.block<4, 4>(0, 0) =
-+      Eigen::Matrix<double, 4, 4>::Identity() + dt / 2 * calcOmega(gyro_compensated);
-     F.block<4, 3>(0, 4) <<
--      + q[1], + q[2], + q[3],
--      - q[0], + q[3], - q[2],
--      - q[3], - q[0], + q[1],
--      + q[2], - q[1], - q[0];
--    F.block<4, 3>(0, 4) *= dt / 2;
-+      dt / 2 * q[1],   dt / 2 * q[2],   dt / 2 * q[3],
-+      - dt / 2 * q[0],   dt / 2 * q[3], - dt / 2 * q[2],
-+      - dt / 2 * q[3], - dt / 2 * q[0],   dt / 2 * q[1],
-+      dt / 2 * q[2], - dt / 2 * q[1], - dt / 2 * q[0];
-+    F.block<3, 4>(0, 4) = Eigen::Matrix<double, 3, 4>::Zero();
-+    F.block<3, 3>(4, 4) = Eigen::Matrix<double, 3, 3>::Identity();
-+    return F;
-   }
- 
--  void calcPredictedCovariance(hrp::Matrix77& _P_a_priori,
--                               const hrp::Matrix77& F,
--                               const Eigen::Vector4d& q) const {
--    /* P_a_priori = F P F^T + V Q V^T */
--    Eigen::Matrix<double, 4, 3> V_upper;
--    V_upper <<
--      - q[1], - q[2], - q[3],
--      + q[0], - q[3], + q[2],
--      + q[3], + q[0], - q[1],
--      - q[2], + q[1], + q[0];
--    V_upper *= dt / 2;
--    hrp::Matrix77 VQVt = hrp::Matrix77::Zero();
--    VQVt.block<4, 4>(0, 0) = V_upper * Q * V_upper.transpose();
--    _P_a_priori = F * P * F.transpose() + VQVt;
-+  Eigen::Matrix<double, 7, 7> calcPredictedCovariance(Eigen::Matrix<double, 7, 7> F) {
-+    /* P_a_priori = F P F^T + Q */
-+    return F * P * F.transpose() + Q;
-   }
- 
--  Eigen::Vector3d calcAcc(const Eigen::Vector4d& q) const {
--    Eigen::Quaternion<double> q_tmp(q[0], q[1], q[2], q[3]);
--    Eigen::Vector3d acc = q_tmp.conjugate()._transformVector(g_vec);
-+  Eigen::Vector3d calcAcc(Eigen::Matrix<double, 4, 1> q,
-+                          const Eigen::Vector3d& vel_ref,
-+                          const Eigen::Vector3d& acc_ref,
-+                          const Eigen::Vector3d& angular_rate_ref) {
-+    /* acc = \dot{v} + w \times v + R^T g; */
-+    Eigen::Quaternion<double> q_tmp = Eigen::Quaternion<double>(q[0], q[1], q[2], q[3]);
-+    Eigen::Vector3d acc =
-+      acc_ref + angular_rate_ref.cross(vel_ref) +
-+      q_tmp.toRotationMatrix().transpose() * g_vec;
-+    /* 
-+     * Eigen::Vector3d hoge;
-+     * hoge <<
-+     *   -2 * q_tmp2.w() * q_tmp2.y() + 2 * q_tmp2.x() * q_tmp2.z(),
-+     *   2 * q_tmp2.w() * q_tmp2.x() + 2 * q_tmp2.y() * q_tmp2.z(),
-+     *   q_tmp2.w() * q_tmp2.w()  - q_tmp2.x() * q_tmp2.x() - q_tmp2.y() * q_tmp2.y() + q_tmp2.z() * q_tmp2.z();
-+     * hoge *= g_vec[2];
-+     * std::cerr << "diff 2" << std::endl << acc - hoge << std::endl;
-+     */
-     return acc;
-   }
- 
--  void calcH(Eigen::Matrix<double, 3, 7>& H, const Eigen::Vector4d& q) const {
-+  Eigen::Matrix<double, 3, 7> calcH(Eigen::Matrix<double, 4, 1> q) {
-+    Eigen::Matrix<double, 3, 7> H;
-     double w = q[0], x = q[1], y = q[2], z = q[3];
-+    /* 
-+     * H <<
-+     *   2 * y,  2 * z,  2 * w,  2 * x, 0, 0, 0,
-+     *   -2 * x, -2 * w,  2 * z,  2 * y, 0, 0, 0,
-+     *   2 * w, -2 * x, -2 * y,  2 * z, 0, 0, 0;
-+     * H *= g_vec[2];
-+     */
-     H <<
--      -y, +z, -w, +x, 0, 0, 0,
--      +x, +w, +z, +y, 0, 0, 0,
--      +w, -x, -y, +z, 0, 0, 0;
-+      -y,  z, -w, x, 0, 0, 0,
-+       x,  w,  z, y, 0, 0, 0,
-+       w, -x, -y, z, 0, 0, 0;
-     H *= 2 * g_vec[2];
-+    return H;
-   }
- 
-   Eigen::Vector3d calcMeasurementResidual(const Eigen::Vector3d& acc_measured,
--                                          const Eigen::Vector4d& q) const {
-+                                          const Eigen::Vector3d& vel_ref,
-+                                          const Eigen::Vector3d& acc_ref,
-+                                          const Eigen::Vector3d& angular_rate_ref,
-+                                          Eigen::Matrix<double, 4, 1> q) {
-     /* y = z - h(x) */
--    Eigen::Vector3d y = acc_measured - calcAcc(q);
-+    Eigen::Vector3d y = acc_measured - calcAcc(q, vel_ref, acc_ref, angular_rate_ref);
-+    /* 
-+     * std::cerr << "acc_measured" << std::endl << acc_measured << std::endl;
-+     * std::cerr << "calc acc" << std::endl << calcAcc(q, vel_ref, acc_ref, angular_rate_ref) << std::endl;
-+     * std::cerr << "diff" << std::endl << y << std::endl;
-+     */
-     return y;
-   }
- 
- 
-   void prediction(const Eigen::Vector3d& u) {
--    Eigen::Vector4d q = x.head<4>();
--    Eigen::Vector3d drift = x.tail<3>();
--    hrp::Matrix77 F;
--    calcF(F, q, u, drift);
--    hrp::Vector7 x_tmp;
--    calcPredictedState(x_tmp, q, u, drift);
--    x_a_priori = x_tmp;
--    hrp::Matrix77 P_tmp;
--    calcPredictedCovariance(P_tmp, F, q);
--    P_a_priori = P_tmp;
-+    Eigen::Matrix<double, 4, 1> q = x.block<4, 1>(0, 0);
-+    Eigen::Vector3d drift = x.block<3, 1>(4, 0);
-+    Eigen::Matrix<double, 7, 7> F = calcF(q, u, drift, dt);
-+    x_a_priori = calcPredictedState(q, u, drift, dt);
-+    P_a_priori = calcPredictedCovariance(F);
-   }
- 
--  void correction(const Eigen::Vector3d& z, const Eigen::Matrix3d& fuzzyR) {
--    Eigen::Vector4d q_a_priori = x_a_priori.head<4>();
-+  void correction(const Eigen::Vector3d& z,
-+                  const Eigen::Vector3d& vel_ref,
-+                  const Eigen::Vector3d& acc_ref,
-+                  const Eigen::Vector3d& angular_rate_ref) {
-+    Eigen::Matrix<double, 4, 1> q_a_priori = x_a_priori.block<4, 1>(0, 0).normalized();
-     Eigen::Matrix<double, 3, 7> H;
--    z_k = z;
--    Eigen::Vector3d y = calcMeasurementResidual(z, q_a_priori);
--    calcH(H, q_a_priori);
--    Eigen::Matrix3d S = H * P_a_priori * H.transpose() + fuzzyR;
--    Eigen::Matrix<double, 7, 3> K = P_a_priori * H.transpose() * S.inverse();
--    hrp::Vector7 x_tmp = x_a_priori + K * y;
--    x.head<4>() = x_tmp.head<4>().normalized(); /* quaternion */
--    x.tail<3>() = x_tmp.tail<3>(); /* bias */
--    P = (hrp::Matrix77::Identity() - K * H) * P_a_priori;
-+    Eigen::Matrix<double, 3, 3> S;
-+    Eigen::Matrix<double, 7, 3> K;
-+    Eigen::Vector3d y;
-+    /* need to normalize q_a_priori ? */
-+    y = calcMeasurementResidual(z, vel_ref, acc_ref, angular_rate_ref, q_a_priori);
-+    H = calcH(q_a_priori);
-+    S = H * P_a_priori * H.transpose() + R;
-+    K = P_a_priori * H.transpose() * S.inverse();
-+    Eigen::Matrix<double, 7, 1> x_tmp = x_a_priori + K * y;
-+    x = x_tmp.normalized();
-+    P = (Eigen::Matrix<double, 7, 7>::Identity() - K * H) * P_a_priori;
-   }
- 
--  void printAll() const {
-+  void printAll() {
-     std::cerr << "x" << std::endl << x << std::endl;
-     std::cerr << "x_a_priori" << std::endl << x_a_priori << std::endl;
-     std::cerr << "P" << std::endl << P << std::endl << std::endl;
-     std::cerr << "P_a_priori" << std::endl << P_a_priori << std::endl << std::endl;
-+    /*
-+     * std::cerr << "Q" << std::endl << Q << std::endl << std::endl;
-+     * std::cerr << "R" << std::endl << R << std::endl << std::endl;
-+     */
-   }
- 
--
--  // Basically Equation (23), (24) and (25) in the paper [1]
--  // [1] Chul Woo Kang and Chan Gook Park. Attitude estimation with accelerometers and gyros using fuzzy tuned Kalman filter.
--  //     In European Control Conference, 2009.
--  void calcRWithFuzzyRule(Eigen::Matrix3d& fuzzyR, const hrp::Vector3& acc, const hrp::Vector3& gyro) const {
--    double alpha = std::min(std::fabs(acc.norm() - g_vec.norm()) / g_vec.norm(), 0.1);
--    double beta = std::min(gyro.norm(), 0.05);
--    double large_mu_acc = std::max(std::min((alpha - min_mag_thre_acc) / (max_mag_thre_acc - min_mag_thre_acc), 1.0), 0.0);
--    double large_mu_gyro = std::max(std::min((beta - min_mag_thre_gyro) / (max_mag_thre_gyro - min_mag_thre_gyro), 1.0), 0.0);
--    double w1, w2, w3, w4;
--    w1 = (1.0 - large_mu_acc) * (1.0 - large_mu_gyro);
--    w2 = (1.0 - large_mu_acc) * large_mu_gyro;
--    w3 = large_mu_acc * (1.0 - large_mu_gyro);
--    w4 = large_mu_acc * large_mu_gyro;
--    double z = (w1 * 0.0 + w2 * (3.5 * alpha + 8.0 * beta + 0.5) + w3 * (3.5 * alpha + 8.0 * beta + 0.5) + w4 * 1.0) / (w1 + w2 + w3 + w4);
--    double k1 = 400;
--    fuzzyR = R + k1 * z * z * Eigen::Matrix3d::Identity();
--  };
--
-   void main_one (hrp::Vector3& rpy, hrp::Vector3& rpyRaw, const hrp::Vector3& acc, const hrp::Vector3& gyro)
-   {
--    Eigen::Matrix3d fuzzyR;
--    calcRWithFuzzyRule(fuzzyR, acc, gyro);
--    prediction(gyro);
--    correction(acc, fuzzyR);
--    /* ekf_filter.printAll(); */
--    Eigen::Quaternion<double> q(x[0], x[1], x[2], x[3]);
--    rpy = hrp::rpyFromRot(q.toRotationMatrix());
-+      prediction(gyro);
-+      correction(acc, Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0));
-+      /* ekf_filter.printAll(); */
-+      Eigen::Matrix<double, 7, 1> x = getx();
-+      Eigen::Quaternion<double> q = Eigen::Quaternion<double>(x[0], x[1], x[2], x[3]);
-+      hrp::Vector3 eulerZYX = q.toRotationMatrix().eulerAngles(2,1,0);
-+      rpy(2) = eulerZYX(0);
-+      rpy(1) = eulerZYX(1);
-+      rpy(0) = eulerZYX(2);
-   };
- 
-   void setdt (const double _dt) { dt = _dt;};
--  void resetKalmanFilterState() {
--    Eigen::Quaternion<double> tmp_q;
--    tmp_q.setFromTwoVectors(z_k, g_vec);
--    x << tmp_q.w(), tmp_q.x(), tmp_q.y(), tmp_q.z(), 0, 0, 0;
--  };
- private:
--  hrp::Vector7 x, x_a_priori;
--  hrp::Matrix77 P, P_a_priori;
--  Eigen::Matrix3d Q, R;
--  Eigen::Vector3d g_vec, z_k;
-+  Eigen::Matrix<double, 7, 1> x, x_a_priori;
-+  Eigen::Matrix<double, 7, 7> P, P_a_priori;
-+  Eigen::Matrix<double, 7, 7> Q;
-+  Eigen::Matrix<double, 3, 3> R;
-+  /* static const Eigen::Vector3d g_vec = Eigen::Vector3d(0.0, 0.0, 9.80665); */
-+  Eigen::Vector3d g_vec;
-   double dt;
--  double min_mag_thre_acc, max_mag_thre_acc, min_mag_thre_gyro, max_mag_thre_gyro;
- };
- 
- #endif /* EKFILTER_H */
-diff --git a/rtc/KalmanFilter/KalmanFilter.cpp b/rtc/KalmanFilter/KalmanFilter.cpp
-index 8084418..4280b51 100644
---- a/rtc/KalmanFilter/KalmanFilter.cpp
-+++ b/rtc/KalmanFilter/KalmanFilter.cpp
-@@ -8,7 +8,7 @@
-  */
- 
- #include "KalmanFilter.h"
--#include "hrpsys/util/VectorConvert.h"
-+#include "util/VectorConvert.h"
- #include <rtm/CorbaNaming.h>
- #include <hrpModel/ModelLoaderUtil.h>
- #include <math.h>
-@@ -49,6 +49,13 @@ KalmanFilter::KalmanFilter(RTC::Manager* manager)
-     m_rpyRawOut("rpy_raw", m_rpyRaw),
-     m_baseRpyCurrentOut("baseRpyCurrent", m_baseRpyCurrent),
-     m_KalmanFilterServicePort("KalmanFilterService"),
-+#ifdef CALC_VEL_N_ANGVEL
-+    //added by karasawa for chidori
-+    m_velOut("vel", m_vel),
-+    m_angvelOut("angvel", m_angvel),
-+    m_accOut("acc_output", m_acc_output),
-+    m_emergency_step_flagIn("emergency_step_flag", m_emergency_step_flag),
-+#endif
-     // </rtc-template>
-     m_robot(hrp::BodyPtr()),
-     m_debugLevel(0),
-@@ -71,6 +78,7 @@ RTC::ReturnCode_t KalmanFilter::onInitialize()
-   // Bind variables and configuration variable
-   bindParameter("debugLevel", m_debugLevel, "0");
-   
-+  
-   // </rtc-template>
- 
-   // Registration: InPort/OutPort/Service
-@@ -86,6 +94,14 @@ RTC::ReturnCode_t KalmanFilter::onInitialize()
-   addOutPort("rpy", m_rpyOut);
-   addOutPort("rpy_raw", m_rpyRawOut);
-   addOutPort("baseRpyCurrent", m_baseRpyCurrentOut);
-+#ifdef CALC_VEL_N_ANGVEL
-+  //added by karasawa for chidori
-+  addOutPort("vel", m_velOut);
-+  addOutPort("angvel", m_angvelOut);
-+  addOutPort("acc_output", m_accOut);
-+  addInPort("emergency_step_flag", m_emergency_step_flagIn);
-+  m_emergency_step_flag.data = false;
-+#endif
- 
-   // Set service provider to Ports
-   m_KalmanFilterServicePort.registerProvider("service0", "KalmanFilterService", m_service0);
-@@ -123,6 +139,18 @@ RTC::ReturnCode_t KalmanFilter::onInitialize()
-   m_rpy.data.r = 0;
-   m_rpy.data.p = 0;
-   m_rpy.data.y = 0;
-+#ifdef CALC_VEL_N_ANGVEL
-+  //added by karasawa for chidori
-+  m_vel.data.vx = 0.0;
-+  m_vel.data.vy = 0.0;
-+  m_vel.data.vz = 0.0;
-+  m_angvel.data.avx = 0.0;
-+  m_angvel.data.avy = 0.0;
-+  m_angvel.data.avz = 0.0;
-+  m_acc_output.data.ax = 0.0;
-+  m_acc_output.data.ay = 0.0;
-+  m_acc_output.data.az = 0.0;
-+#endif
- 
-   if (m_robot->numSensors(hrp::Sensor::ACCELERATION) > 0) {
-     hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::ACCELERATION, 0);
-@@ -201,9 +229,14 @@ RTC::ReturnCode_t KalmanFilter::onExecute(RTC::UniqueId ec_id)
-   }
-   double sx_ref = 0.0, sy_ref = 0.0, sz_ref = 0.0;
-   if (m_accRefIn.isNew()){
--    m_accRefIn.read();
--    sx_ref = m_accRef.data.ax, sy_ref = m_accRef.data.ay, sz_ref = m_accRef.data.az;
-+      m_accRefIn.read();
-+      sx_ref = m_accRef.data.ax, sy_ref = m_accRef.data.ay, sz_ref = m_accRef.data.az;
-+  }
-+#ifdef CALC_VEL_N_ANGVEL
-+  if (m_emergency_step_flagIn.isNew()) {
-+      m_emergency_step_flagIn.read();
-   }
-+#endif
-   if (m_accIn.isNew()){
-     m_accIn.read();
- 
-@@ -231,6 +264,59 @@ RTC::ReturnCode_t KalmanFilter::onExecute(RTC::UniqueId ec_id)
-         }
-         rpy_kf.main_one(rpy, rpyRaw, baseRpyCurrent, acc, gyro, sl_y, BtoS);
-     }
-+#ifdef CALC_VEL_N_ANGVEL
-+    //calc_vel_n_angvel(vel, angvel, acc, gyro, rpy, BtoS);
-+    if(m_robot->numSensors(hrp::Sensor::ACCELERATION) >0) {
-+        if(m_emergency_step_flag.data == SequencePlayer::EMERGENCY) {
-+           // std::cerr << "\x1b[33m" << "emergency step flag!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << "\x1b[0m" << std::endl;
-+        }
-+        hrp::Vector3 vel;
-+        vel(0)= m_vel.data.vx;
-+        vel(1)= m_vel.data.vy;
-+        vel(2)= m_vel.data.vz;
-+        hrp::Vector3 angvel;
-+        hrp::Vector3 acc_without_g;
-+        //angvel(0)= m_angvel.data.avx;
-+        //angvel(1)= m_angvel.data.avy;
-+        //angvel(2)= m_angvel.data.avz;
-+        hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::ACCELERATION, 0);
-+        rpy_kf.calc_vel_n_angvel(vel, angvel, acc_without_g,acc, gyro, rpy, sensor->localPos);
-+        m_vel.data.vx = vel(0);
-+        m_vel.data.vy = vel(1);
-+        m_vel.data.vz = vel(2);
-+        m_angvel.data.avx = angvel(0);
-+        m_angvel.data.avy = angvel(1);
-+        m_angvel.data.avz = angvel(2);
-+        m_acc_output.data.ax = acc_without_g(0);
-+        m_acc_output.data.ay = acc_without_g(1);
-+        m_acc_output.data.az = acc_without_g(2);
-+        /* 
-+         * hrp::Vector3 testvec;
-+         * testvec << 1, 0, 0;
-+         * testvec = sensor->localR*testvec;
-+         * std::cerr << "1 0 0  | x:" << testvec(0) << "  y:" << testvec(1) << "  z:" << testvec(2) << std::endl;
-+         * testvec << 0, 1, 0;
-+         * testvec = sensor->localR*testvec;
-+         * std::cerr << "0 1 0  | x:" << testvec(0) << "  y:" << testvec(1) << "  z:" << testvec(2) << std::endl;
-+         * testvec << 0, 0, 1;
-+         * testvec = sensor->localR*testvec;
-+         * std::cerr << "0 0 1  | x:" << testvec(0) << "  y:" << testvec(1) << "  z:" << testvec(2) << std::endl;
-+         */
-+
-+    }
-+    //std::cerr << "acc x:" << acc(0) << "  y:" << acc(1) << "  z:" << acc(2) << std::endl;
-+
-+    /* 
-+     * hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::ACCELERATION, 0);
-+     * hrp::Vector3 Body2Sensor_pos_relative_to_world = sensor->link->R * sensor->localPos;
-+     * //std::cerr << "sensor pos  x:" << sensor->localPos(0) << "  y:" << sensor->localPos(1) << "  z:" << sensor->localPos(2) << std::endl;
-+     * std::cerr << "posture::" << std::endl;
-+     * std::cerr << sensor->link->R(0,0) << "  " << sensor->link->R(0,1) << "  "<< sensor->link->R(0,2) << std::endl;
-+     * std::cerr << sensor->link->R(1,0) << "  " << sensor->link->R(1,1) << "  "<< sensor->link->R(1,2) << std::endl;
-+     * std::cerr << sensor->link->R(2,0) << "  " << sensor->link->R(2,1) << "  "<< sensor->link->R(2,2) << std::endl;
-+     * std::cerr << std::endl;
-+     */
-+#endif
-     m_rpyRaw.data.r = rpyRaw(0);
-     m_rpyRaw.data.p = rpyRaw(1);
-     m_rpyRaw.data.y = rpyRaw(2);
-@@ -244,10 +330,22 @@ RTC::ReturnCode_t KalmanFilter::onExecute(RTC::UniqueId ec_id)
-     m_rpyRaw.tm = m_acc.tm;
-     m_rpy.tm = m_acc.tm;
-     m_baseRpyCurrent.tm = m_acc.tm;
-+#ifdef CALC_VEL_N_ANGVEL
-+    //added by karasawa for chidori
-+    m_vel.tm = m_acc.tm;
-+    m_angvel.tm = m_acc.tm;
-+    m_acc_output.tm = m_acc.tm;
-+#endif
- 
-     m_rpyOut.write();
-     m_rpyRawOut.write();
-     m_baseRpyCurrentOut.write();
-+#ifdef CALC_VEL_N_ANGVEL
-+    //added by karasawa for chidori
-+    m_velOut.write();
-+    m_angvelOut.write();
-+    m_accOut.write();
-+#endif
-   }
-   return RTC::RTC_OK;
- }
-@@ -309,7 +407,6 @@ bool KalmanFilter::setKalmanFilterParam(const OpenHRP::KalmanFilterService::Kalm
- bool KalmanFilter::resetKalmanFilterState()
- {
-     rpy_kf.resetKalmanFilterState();
--    ekf_filter.resetKalmanFilterState();
- };
- 
- bool KalmanFilter::getKalmanFilterParam(OpenHRP::KalmanFilterService::KalmanFilterParam& i_param)
-diff --git a/rtc/KalmanFilter/KalmanFilter.h b/rtc/KalmanFilter/KalmanFilter.h
-index 7b48040..70a2c57 100644
---- a/rtc/KalmanFilter/KalmanFilter.h
-+++ b/rtc/KalmanFilter/KalmanFilter.h
-@@ -10,6 +10,9 @@
- #ifndef NULL_COMPONENT_H
- #define NULL_COMPONENT_H
- 
-+//define if you use velocity and angular velocity of root link
-+#define CALC_VEL_N_ANGVEL
-+
- #include <rtm/Manager.h>
- #include <rtm/DataFlowComponentBase.h>
- #include <rtm/CorbaPort.h>
-@@ -22,6 +25,10 @@
- #include "RPYKalmanFilter.h"
- #include "EKFilter.h"
- 
-+#ifdef CALC_VEL_N_ANGVEL
-+#include "../SequencePlayer/SequencePlayer.h"
-+#endif
-+
- // Service implementation headers
- // <rtc-template block="service_impl_h">
- #include "KalmanFilterService_impl.h"
-@@ -134,6 +141,18 @@ protected:
-   RTC::InPort<RTC::TimedDoubleSeq> m_qCurrentIn;
-   RTC::TimedOrientation3D m_baseRpyCurrent;
-   RTC::OutPort<RTC::TimedOrientation3D> m_baseRpyCurrentOut;
-+
-+#ifdef CALC_VEL_N_ANGVEL
-+  //added by karasawa for chidori
-+  TimedVelocity3D m_vel;
-+  TimedAngularVelocity3D m_angvel;
-+  TimedAcceleration3D m_acc_output;
-+  RTC::TimedLong m_emergency_step_flag;
-+  RTC::OutPort<RTC::TimedVelocity3D> m_velOut;
-+  RTC::OutPort<RTC::TimedAngularVelocity3D> m_angvelOut;
-+  RTC::OutPort<RTC::TimedAcceleration3D> m_accOut;
-+  RTC::InPort<RTC::TimedLong> m_emergency_step_flagIn;
-+#endif
-   
-   // </rtc-template>
- 
-diff --git a/rtc/KalmanFilter/KalmanFilter.xml b/rtc/KalmanFilter/KalmanFilter.xml
-deleted file mode 100644
-index b0c9dc3..0000000
---- a/rtc/KalmanFilter/KalmanFilter.xml
-+++ /dev/null
-@@ -1,35 +0,0 @@
--<?xml version="1.0" encoding="UTF-8" standalone="yes"?>
--<rtc:RtcProfile rtc:version="0.2" rtc:id="RTC:AIST:example:KalmanFilter:1.0" xmlns:rtcExt="http://www.openrtp.org/namespaces/rtc_ext" xmlns:rtcDoc="http://www.openrtp.org/namespaces/rtc_doc" xmlns:rtc="http://www.openrtp.org/namespaces/rtc" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
--    <rtc:BasicInfo xsi:type="rtcExt:basic_info_ext" rtcExt:saveProject="KalmanFilter" rtc:updateDate="2015-11-18T14:10:18+09:00" rtc:creationDate="2015-11-18T05:07:28Z" rtc:version="1.0" rtc:vendor="AIST" rtc:maxInstances="10" rtc:executionType="PeriodicExecutionContext" rtc:executionRate="0.0" rtc:description="Kalman filter component" rtc:category="example" rtc:componentKind="DataFlowComponent" rtc:activityType="PERIODIC" rtc:componentType="STATIC" rtc:name="KalmanFilter"/>
--    <rtc:Actions>
--        <rtc:OnInitialize xsi:type="rtcDoc:action_status_doc" rtc:implemented="true"/>
--        <rtc:OnFinalize xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
--        <rtc:OnStartup xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
--        <rtc:OnShutdown xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
--        <rtc:OnActivated xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
--        <rtc:OnDeactivated xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
--        <rtc:OnAborting xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
--        <rtc:OnError xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
--        <rtc:OnReset xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
--        <rtc:OnExecute xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
--        <rtc:OnStateUpdate xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
--        <rtc:OnRateChanged xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
--        <rtc:OnAction xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
--        <rtc:OnModeChanged xsi:type="rtcDoc:action_status_doc" rtc:implemented="false"/>
--    </rtc:Actions>
--    <rtc:ConfigurationSet>
--        <rtc:Configuration xsi:type="rtcExt:configuration_ext" rtc:defaultValue="1" rtc:type="string" rtc:name="compensateMode"/>
--        <rtc:Configuration xsi:type="rtcExt:configuration_ext" rtc:defaultValue="0,0,0" rtc:type="string" rtc:name="offset"/>
--        <rtc:Configuration xsi:type="rtcExt:configuration_ext" rtc:defaultValue="0.01" rtc:type="string" rtc:name="Q"/>
--        <rtc:Configuration xsi:type="rtcExt:configuration_ext" rtc:defaultValue="0.0001" rtc:type="string" rtc:name="Qbw"/>
--        <rtc:Configuration xsi:type="rtcExt:configuration_ext" rtc:defaultValue="0.1" rtc:type="string" rtc:name="R"/>
--        <rtc:Configuration xsi:type="rtcExt:configuration_ext" rtc:defaultValue="0.15" rtc:type="string" rtc:name="Tgsens"/>
--        <rtc:Configuration xsi:type="rtcExt:configuration_ext" rtc:defaultValue="3" rtc:type="string" rtc:name="filter_order"/>
--        <rtc:Configuration xsi:type="rtcExt:configuration_ext" rtc:defaultValue="0" rtc:type="string" rtc:name="debugLevel"/>
--    </rtc:ConfigurationSet>
--    <rtc:DataPorts xsi:type="rtcExt:dataport_ext" rtcExt:position="LEFT" rtc:subscriptionType="Any" rtc:dataflowType="push,pull" rtc:interfaceType="corba_cdr" rtc:type="RTC::TimedAcceleration3D" rtc:name="acc" rtc:portType="DataInPort"/>
--    <rtc:DataPorts xsi:type="rtcExt:dataport_ext" rtcExt:position="LEFT" rtc:subscriptionType="Any" rtc:dataflowType="push,pull" rtc:interfaceType="corba_cdr" rtc:type="RTC::TimedAcceleration3D" rtc:name="accRef" rtc:portType="DataInPort"/>
--    <rtc:DataPorts xsi:type="rtcExt:dataport_ext" rtcExt:position="LEFT" rtc:subscriptionType="Any" rtc:dataflowType="push,pull" rtc:interfaceType="corba_cdr" rtc:type="RTC::TimedAngularVelocity3D" rtc:name="rate" rtc:portType="DataInPort"/>
--    <rtc:DataPorts xsi:type="rtcExt:dataport_ext" rtcExt:position="RIGHT" rtc:subscriptionType="flush,new,periodic" rtc:dataflowType="push,pull" rtc:interfaceType="corba_cdr" rtc:type="RTC::TimedOrientation3D" rtc:name="rpy" rtc:portType="DataOutPort"/>
--    <rtc:Language xsi:type="rtcExt:language_ext" rtc:kind="C++"/>
--</rtc:RtcProfile>
-diff --git a/rtc/KalmanFilter/KalmanFilterService_impl.h b/rtc/KalmanFilter/KalmanFilterService_impl.h
-index 48aef9a..645205d 100644
---- a/rtc/KalmanFilter/KalmanFilterService_impl.h
-+++ b/rtc/KalmanFilter/KalmanFilterService_impl.h
-@@ -2,7 +2,7 @@
- #ifndef __KALMANFILTER_SERVICE_H__
- #define __KALMANFILTER_SERVICE_H__
- 
--#include "hrpsys/idl/KalmanFilterService.hh"
-+#include "KalmanFilterService.hh"
- 
- class KalmanFilter;
- 
-diff --git a/rtc/KalmanFilter/RPYKalmanFilter.h b/rtc/KalmanFilter/RPYKalmanFilter.h
-index 4c3967a..197af98 100644
---- a/rtc/KalmanFilter/RPYKalmanFilter.h
-+++ b/rtc/KalmanFilter/RPYKalmanFilter.h
-@@ -3,7 +3,7 @@
- 
- #include <hrpUtil/EigenTypes.h>
- #include <hrpUtil/Eigen3d.h>
--#include "hrpsys/util/Hrpsys.h"
-+#include "util/Hrpsys.h"
- namespace hrp{
-   typedef Eigen::Vector2d Vector2;
-   typedef Eigen::Matrix2d Matrix22;
-@@ -70,7 +70,12 @@ private:
- 
- class RPYKalmanFilter {
- public:
--    RPYKalmanFilter() : m_sensorR(hrp::Matrix33::Identity()) {};
-+#ifndef CALC_VEL_N_ANGVEL
-+    RPYKalmanFilter() : m_sensorR(hrp::Matrix33::Identity()){
-+#else
-+    RPYKalmanFilter() : m_sensorR(hrp::Matrix33::Identity()), dt(0.002), G(9.80665), loop_count(0), count(0), rpy_for_calc_vel_n_angvel(0){
-+#endif
-+    };
-     void main_one (hrp::Vector3& rpy, hrp::Vector3& rpyRaw, hrp::Vector3& baseRpyCurrent, const hrp::Vector3& acc, const hrp::Vector3& gyro, const double& sl_y, const hrp::Matrix33& BtoS)
-     {
-       //
-@@ -158,10 +163,106 @@ public:
-     double getQangle () const { return Q_angle;};
-     double getQrate () const { return Q_rate;};
-     double getRangle () const { return R_angle;};
-+#ifdef CALC_VEL_N_ANGVEL
-+    void calc_vel_n_angvel (hrp::Vector3& vel, hrp::Vector3& angvel,hrp::Vector3& acc_without_g, const hrp::Vector3& acc, const hrp::Vector3& gyro, const hrp::Vector3& rpy, const hrp::Vector3& sensorPosition) {
-+        loop_count++;
-+        if(loop_count > 200) {
-+            loop_count = 0;
-+            count = 0;
-+        }
-+
-+        double g_norm = sqrt(acc(0)*acc(0) + acc(1)*acc(1) + acc(2)*acc(2));
-+        if( ((g_norm-G) < 0.005) && ((g_norm-G) > -0.005) ) {
-+            count++;
-+        }
-+        if (loop_count == 0) {
-+            //std::cerr << "\x1b[33m" << "g_norm |" << g_norm << "\x1b[0m" << std::endl;
-+        }
-+        if(count > 5) {
-+            //std::cerr << "\x1b[31m" << "rpy angles is updated by acc!!" << "\x1b[0m" <<std::endl;
-+            double r, p;
-+            p = atan2( acc(0), sqrt( acc(1) * acc(1) + acc(2) * acc(2) ) );
-+            r = atan2( acc(1) , acc(2) );
-+            rpy_for_calc_vel_n_angvel(0) = r;
-+            rpy_for_calc_vel_n_angvel(1) = p;
-+            count = 0;
-+            loop_count = 0;
-+            //std::cerr << " r:" << r << "  p:" << p << std::endl;
-+        }
-+
-+        hrp::Vector3 rpy_dot;
-+        rpy_dot(0) = (gyro(1)*sin(rpy_for_calc_vel_n_angvel(2))+gyro(0)*cos(rpy_for_calc_vel_n_angvel(2)))/cos(rpy_for_calc_vel_n_angvel(1));
-+        rpy_dot(1) = gyro(1)*cos(rpy_for_calc_vel_n_angvel(2))-gyro(0)*sin(rpy_for_calc_vel_n_angvel(2));
-+        rpy_dot(2) = gyro(2)-rpy_dot(0)*sin(rpy_for_calc_vel_n_angvel(1));
-+
-+        rpy_for_calc_vel_n_angvel(0) += dt*rpy_dot(0);
-+        rpy_for_calc_vel_n_angvel(1) += dt*rpy_dot(1);
-+        rpy_for_calc_vel_n_angvel(2) += dt*rpy_dot(2);
-+        rpy_for_calc_vel_n_angvel(2) *= 0.99;
-+        //hrp::Vector3 modified_rpy = rpy;
-+        //modified_rpy(0) = rpy(0) - 3.141592/2.0;
-+        //std::cerr << "modified_rpy | r:" << modified_rpy(0) << "  p:" << modified_rpy(1) << "  y:" << modified_rpy(2) << std::endl;
-+        //std::cerr << "rpy_for_calc_vel_n_angvel  | r:" << rpy_for_calc_vel_n_angvel(0) << "  p:" << rpy_for_calc_vel_n_angvel(1) << "  y:" << rpy_for_calc_vel_n_angvel(2) << std::endl;
-+        hrp::Matrix33 realRotationMatrix = hrp::rotFromRpy(rpy_for_calc_vel_n_angvel);
-+        //std::cerr << "realRotationMatrix ::" << std::endl;
-+        //std::cerr << realRotationMatrix(0,0) << "  " << realRotationMatrix(0,1) << "  " << realRotationMatrix(0,2) << std::endl;
-+        //std::cerr << realRotationMatrix(1,0) << "  " << realRotationMatrix(1,1) << "  " << realRotationMatrix(1,2) << std::endl;
-+        //std::cerr << realRotationMatrix(2,0) << "  " << realRotationMatrix(2,1) << "  " << realRotationMatrix(2,2) << std::endl;
-+        //std::cerr << std::endl;
-+        angvel = gyro;
-+
-+        hrp::Matrix33 angvel_Matrix;
-+        angvel_Matrix <<          0, -angvel(2),  angvel(1),
-+                          angvel(2),          0, -angvel(0),
-+                         -angvel(1),  angvel(0),          0;
-+        hrp::Vector3 sensorPosition_relative_to_world = realRotationMatrix * sensorPosition;
-+
-+        hrp::Vector3 Body_acc = acc;// + gyro.cross(realRotationMatrix*sensorPosition_relative_to_world) + angvel.cross(angvel_Matrix*realRotationMatrix*sensorPosition_relative_to_world);
-+        hrp::Vector3 g;
-+        g << 0.0, 0.0, 9.80665;
-+        g = realRotationMatrix*g;
-+        //std::cerr << "g    x:" << g(0) << "  y:" << g(1) << "  z:" << g(2) << std::endl;
-+        Body_acc = Body_acc - g;
-+        acc_without_g = Body_acc;
-+        /* 
-+         * if(Body_acc(0) < 0.05 && Body_acc(0) > -0.05) {
-+         *     Body_acc(0) = 0.0;
-+         * }
-+         * if(Body_acc(1) < 0.05 && Body_acc(1) > -0.05) {
-+         *     Body_acc(1) = 0.0;
-+         * }
-+         * if(Body_acc(2) < 0.05 && Body_acc(2) > -0.05) {
-+         *     Body_acc(2) = 0.0;
-+         * }
-+         */
-+        //std::cerr << "acc_norm | " << (acc.norm()) << std::endl;
-+        /* 
-+         * if ( Body_acc(0) < -2.0 || Body_acc(0) > 2.0 ) {
-+         *     std::cerr << "\x1b[33m"  << "body_acc | x:" << Body_acc(0) << "  y:" << Body_acc(1) << "  z:" << Body_acc(2) << "    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << "\x1b[0m" << std::endl;
-+         * } else if ( Body_acc(1) < -2.0 || Body_acc(1) > 2.0 ) {
-+         *     std::cerr << "\x1b[33m" <<  "body_acc | x:" << Body_acc(0) << "  y:" << Body_acc(1) << "  z:" << Body_acc(2) << "    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << "\x1b[0m" << std::endl;
-+         * } else if ( !(loop_count % 100) ) {
-+         *     std::cerr << "body_acc | x:" << Body_acc(0) << "  y:" << Body_acc(1) << "  z:" << Body_acc(2) << std::endl;
-+         * }
-+         */
-+        if( (Body_acc(0) < 0.25) && (Body_acc(0) > -0.25) && (Body_acc(1) < 0.25) && (Body_acc(1) > -0.25) ) {
-+            vel = (vel + Body_acc*dt)*0.99;//*0.9995;
-+        } else {
-+            vel = vel + Body_acc*dt;
-+        }
-+    }
-+#endif
- private:
-     KFilter r_filter, p_filter, y_filter;
-     double Q_angle, Q_rate, R_angle;
-     hrp::Matrix33 m_sensorR;
-+#ifdef CALC_VEL_N_ANGVEL
-+    int loop_count;
-+    char count;
-+    const double dt;
-+    const double G;
-+    hrp::Vector3 rpy_for_calc_vel_n_angvel;
-+#endif
- };
- 
- #endif /* RPYKALMANFILTER_H */
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index d41463e6e46..07b6504cc88 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -1,10 +1,3 @@
-#include <rtm/CorbaNaming.h>
-#include <hrpModel/Link.h>
-#include <hrpModel/Sensor.h>
-#include <hrpModel/ModelLoaderUtil.h>
-#include <hrpModel/JointPath.h>
-#include <hrpUtil/MatrixSolvers.h>
-#include "hrpsys/util/Hrpsys.h"
 #include "WholeBodyMasterSlave.h"
 
 #define DEBUGP (loop%200==0)
@@ -79,7 +72,6 @@ WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlo
         m_invdyn_dbgOut("invdyn_dbgOut", m_invdyn_dbg),
 #endif
         m_WholeBodyMasterSlaveServicePort("WholeBodyMasterSlaveService"),
-        m_robot(hrp::BodyPtr()),
         m_debugLevel(0)
 {
     m_service0.wholebodymasterslave(this);
@@ -88,7 +80,7 @@ WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlo
 WholeBodyMasterSlave::~WholeBodyMasterSlave(){}
 
 RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
-    std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
+    RTCOUT << "onInitialize()" << std::endl;
     bindParameter("debugLevel", m_debugLevel, "0");
     addInPort("qRef", m_qRefIn);// from sh
     addInPort("zmpIn", m_zmpIn);
@@ -141,47 +133,45 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     if (comPos < 0){ comPos = nameServer.length(); }
     nameServer = nameServer.substr(0, comPos);
     RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
-    m_robot = hrp::BodyPtr(new hrp::Body());
-    if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
-        std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
+
+    hrp::BodyPtr robot_for_ik = hrp::BodyPtr(new hrp::Body());
+    if (!loadBodyFromModelLoader(robot_for_ik, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
+        RTCOUT << "failed to load model[" << prop["model"] << "]" << std::endl;
         return RTC::RTC_ERROR;
     }
-    m_robot_vsafe = hrp::BodyPtr(new hrp::Body(*m_robot)); //copy from m_robot
-    // allocate memory for outPorts
-    m_qRef.data.length(m_robot->numJoints());
-    coil::stringTo(optionalDataLength, prop["seq_optional_data_dim"].c_str());
-    loop = 0;
-    transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
-    transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator");
-    transition_interpolator_ratio = 0;
-    double tmp_ratio = 0.0;
-    transition_interpolator->clear();
-    transition_interpolator->set(&tmp_ratio);
-    tmp_ratio = 1.0;
-    //    transition_interpolator->setGoal(&tmp_ratio, 3.0, true);
-    ROBOT_ALL_DOF = m_robot->numJoints() + 3 + 3;// joints + base_p + base_rpy
-    //    q_ip = new interpolator(ROBOT_ALL_DOF, m_dt, interpolator::HOFFARBIB, 1);
-    //    q_ip = new interpolator(ROBOT_ALL_DOF, m_dt, interpolator::QUINTICSPLINE, 1);
-    q_ip = new interpolator(ROBOT_ALL_DOF, m_dt, interpolator::CUBICSPLINE, 1);
-    q_ip->setName(std::string(m_profile.instance_name)+" q_ip");
-    q_ip->clear();
-    // Generate FIK
-    fik = fikPtr(new FullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
-    body_list.push_back(m_robot);
-    body_list.push_back(m_robot_vsafe);
+    m_robot_vsafe = hrp::BodyPtr(new hrp::Body(*robot_for_ik)); //copy
+    RTCOUT << "setup robot model finished" << std::endl;
 
-    setupfik(fik, m_robot, prop);
+    fik = fikPtr(new FullbodyInverseKinematicsSolver(robot_for_ik, std::string(m_profile.instance_name), m_dt));
+    setupEEIKConstraintFromConf(ee_ikc_map, robot_for_ik, prop);
+    is_legged_robot = (ee_ikc_map.find("rleg") != ee_ikc_map.end() && ee_ikc_map.find("lleg") != ee_ikc_map.end());
+    RTCOUT << "setup fullbody ik finished" << std::endl;
 
     wbms = boost::shared_ptr<WBMSCore>(new WBMSCore(m_dt));
+    sccp = boost::shared_ptr<CapsuleCollisionChecker>(new CapsuleCollisionChecker(fik->m_robot));
+    RTCOUT << "setup main function class finished" << std::endl;
 
-    invdyn_zmp_filters.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
-    final_ref_zmp_filter.setParameter(5, 1/m_dt, Q_BUTTERWORTH);
+    // allocate memory for outPorts
+    m_qRef.data.length(fik->m_robot->numJoints()); // is really needed?
+    coil::stringTo(optionalDataLength, prop["seq_optional_data_dim"].c_str());
 
-    avg_q_vel = 1;
+    output_ratio = 0.0;
+    t_ip = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    t_ip->clear();
+    t_ip->set(&output_ratio);
+    q_ip = new interpolator(fik->numStates(), m_dt, interpolator::CUBICSPLINE, 1); // or HOFFARBIB, QUINTICSPLINE
+    q_ip->clear();
+    avg_q_vel = hrp::dvector::Constant(fik->numStates(), 1.0); // all joint max avarage vel = 1.0 rad/s
+    avg_q_acc = hrp::dvector::Constant(fik->numStates(), 16.0); // all joint max avarage acc = 16.0 rad/s^2
+    avg_q_vel.tail(6).fill(std::numeric_limits<double>::max()); // no limit for base link vel
+    avg_q_acc.tail(6).fill(std::numeric_limits<double>::max()); // no limit for base link acc
+    RTCOUT << "setup interpolator finished" << std::endl;
 
-    sccp = boost::shared_ptr<CapsuleCollisionChecker>(new CapsuleCollisionChecker(m_robot));
+    //    invdyn_zmp_filters.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
+    ref_zmp_filter.setParameter(5, 1/m_dt, Q_BUTTERWORTH);
 
-    std::cerr << "[" << m_profile.instance_name << "] onInitialize() OK" << std::endl;
+    RTCOUT << "onInitialize() OK" << std::endl;
+    loop = 0;
     return RTC::RTC_OK;
 }
 
@@ -189,8 +179,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onFinalize(){
     return RTC::RTC_OK;
 }
 
-RTC::ReturnCode_t WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in){
-    coil::vstring ee_conf_all = coil::split(prop_in["end_effectors"], ",");
+RTC::ReturnCode_t WholeBodyMasterSlave::setupEEIKConstraintFromConf(std::map<std::string, IKConstraint>& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop){
+    coil::vstring ee_conf_all = coil::split(_prop["end_effectors"], ",");
     size_t prop_num = 10;
     if (ee_conf_all.size() > 0) {
         size_t ee_num = ee_conf_all.size()/prop_num;
@@ -199,15 +189,15 @@ RTC::ReturnCode_t WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& r
             coil::stringTo(ee_name, ee_conf_all[i*prop_num].c_str());
             coil::stringTo(target_link_name, ee_conf_all[i*prop_num+1].c_str());
             coil::stringTo(base_name, ee_conf_all[i*prop_num+2].c_str());
-            ee_name_ikcp_map[ee_name].target_link_name = target_link_name;
-            for (size_t j = 0; j < XYZ; j++){ coil::stringTo(ee_name_ikcp_map[ee_name].localPos(j), ee_conf_all[i*prop_num+3+j].c_str()); }
+            _ee_ikc_map[ee_name].target_link_name = target_link_name;
+            for (size_t j = 0; j < XYZ; j++){ coil::stringTo(_ee_ikc_map[ee_name].localPos(j), ee_conf_all[i*prop_num+3+j].c_str()); }
             double tmp_aa[4];
             for (int j = 0; j < 4; j++ ){ coil::stringTo(tmp_aa[j], ee_conf_all[i*prop_num+6+j].c_str()); }
-            ee_name_ikcp_map[ee_name].localR = Eigen::AngleAxis<double>(tmp_aa[3], hrp::Vector3(tmp_aa[0], tmp_aa[1], tmp_aa[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
-            if(robot_in->link(target_link_name)){
+            _ee_ikc_map[ee_name].localR = Eigen::AngleAxis<double>(tmp_aa[3], hrp::Vector3(tmp_aa[0], tmp_aa[1], tmp_aa[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
+            if(_robot->link(target_link_name)){
               RTCOUT << "End Effector [" << ee_name << "]" << std::endl;
-              RTCOUT << "   target_link_name = " << ee_name_ikcp_map[ee_name].target_link_name << ", base = " << base_name << std::endl;
-              RTCOUT << "   offset_pos = " << ee_name_ikcp_map[ee_name].localPos.transpose() << "[m]" << std::endl;
+              RTCOUT << "   target_link_name = " << _ee_ikc_map[ee_name].target_link_name << ", base = " << base_name << std::endl;
+              RTCOUT << "   offset_pos = " << _ee_ikc_map[ee_name].localPos.transpose() << "[m]" << std::endl;
               RTCOUT << "   has_toe_joint = " << "fix to false now" << std::endl;
             }else{
               RTCOUT << "Target link [" << target_link_name << "] not found !" << std::endl;
@@ -216,13 +206,12 @@ RTC::ReturnCode_t WholeBodyMasterSlave::setupfik(fikPtr& fik_in, hrp::BodyPtr& r
             contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));////TODO:要移動? //used for check optional data order
         }
     }
-    is_legged_robot = (ee_name_ikcp_map.find("rleg") != ee_name_ikcp_map.end() && ee_name_ikcp_map.find("lleg") != ee_name_ikcp_map.end());
     return RTC::RTC_OK;
 }
 
-#define TIMECALC 0
+#define TIMECALC 1
 RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
-    if(DEBUGP_ONCE)std::cerr << "[" << m_profile.instance_name<< "] onExecute(" << ec_id << ")" << std::endl;
+    if(DEBUGP_ONCE)RTCOUT << "onExecute(" << ec_id << ")" << std::endl;
     struct timeval t_calc_start, t_calc_end;
     if(TIMECALC)gettimeofday(&t_calc_start, NULL);
     if (m_qRefIn.isNew()) { m_qRefIn.read(); }
@@ -230,17 +219,17 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if (m_baseRpyIn.isNew()) { m_baseRpyIn.read(); }
     if (m_zmpIn.isNew()) { m_zmpIn.read(); }
     if (m_optionalDataIn.isNew()) { m_optionalDataIn.read(); }
-    //for HumanSynchronizer
-    if(mode.now()!=MODE_PAUSE){
-        if (m_htrfwIn.isNew()){ m_htrfwIn.read(); WBMSCore::WrenchToVector6(m_htrfw.data,wbms->hp_wld_raw.tgt[rf].w); }
-        if (m_htlfwIn.isNew()){ m_htlfwIn.read(); WBMSCore::WrenchToVector6(m_htlfw.data,wbms->hp_wld_raw.tgt[lf].w); }
-        if (m_htcomIn.isNew()){ m_htcomIn.read(); WBMSCore::Pose3DToWBMSPose3D(m_htcom.data,wbms->hp_wld_raw.tgt[com].abs); }
-        if (m_htrfIn.isNew()) { m_htrfIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htrf.data,wbms->hp_wld_raw.tgt[rf].abs); }
-        if (m_htlfIn.isNew()) { m_htlfIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htlf.data,wbms->hp_wld_raw.tgt[lf].abs); }
-        if (m_htrhIn.isNew()) { m_htrhIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htrh.data,wbms->hp_wld_raw.tgt[rh].abs);}
-        if (m_htlhIn.isNew()) { m_htlhIn.read();  WBMSCore::Pose3DToWBMSPose3D(m_htlh.data,wbms->hp_wld_raw.tgt[lh].abs);}
-        if (m_htheadIn.isNew()){ m_htheadIn.read(); WBMSCore::Pose3DToWBMSPose3D(m_hthead.data,wbms->hp_wld_raw.tgt[head].abs);}
-        if (m_htzmpIn.isNew()){ m_htzmpIn.read();  WBMSCore::Point3DToVector3(m_htzmp.data,wbms->hp_wld_raw.tgt[zmp].abs.p); }
+
+    if( mode.now() != MODE_PAUSE ){ // stop updating input when MODE_PAUSE
+        if (m_htrfwIn.isNew()){ m_htrfwIn.read(); wbms->hp_wld_raw.tgt[rf].w = hrp::to_dvector6(m_htrfw.data); }
+        if (m_htlfwIn.isNew()){ m_htlfwIn.read(); wbms->hp_wld_raw.tgt[lf].w = hrp::to_dvector6(m_htlfw.data); }
+        if (m_htcomIn.isNew()){ m_htcomIn.read(); hrp::Pose3DToWBMSPose3D(m_htcom.data,wbms->hp_wld_raw.tgt[com].abs); }
+        if (m_htrfIn.isNew()) { m_htrfIn.read();  hrp::Pose3DToWBMSPose3D(m_htrf.data,wbms->hp_wld_raw.tgt[rf].abs); }
+        if (m_htlfIn.isNew()) { m_htlfIn.read();  hrp::Pose3DToWBMSPose3D(m_htlf.data,wbms->hp_wld_raw.tgt[lf].abs); }
+        if (m_htrhIn.isNew()) { m_htrhIn.read();  hrp::Pose3DToWBMSPose3D(m_htrh.data,wbms->hp_wld_raw.tgt[rh].abs);}
+        if (m_htlhIn.isNew()) { m_htlhIn.read();  hrp::Pose3DToWBMSPose3D(m_htlh.data,wbms->hp_wld_raw.tgt[lh].abs);}
+        if (m_htheadIn.isNew()){ m_htheadIn.read(); hrp::Pose3DToWBMSPose3D(m_hthead.data,wbms->hp_wld_raw.tgt[head].abs);}
+        if (m_htzmpIn.isNew()){ m_htzmpIn.read(); wbms->hp_wld_raw.tgt[zmp].abs.p = hrp::to_Vector3(m_htzmp.data); }
     }
 
     //khi
@@ -263,32 +252,36 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 
         if (mode.isRunning()) {
             if(mode.isInitialize()){
-                preProcessForWholeBodyMasterSlave(fik, m_robot);
-                idsb.setInitState(m_robot, m_dt);//逆動力学初期化
+                preProcessForWholeBodyMasterSlave();
+                idsb.setInitState(fik->m_robot, m_dt);//逆動力学初期化
             }
 
-            processWholeBodyMasterSlave(fik, m_robot, wbms->rp_ref_out);//安全制限つきマスタ・スレーブ
+            wbms->update();//////HumanSynchronizerの主要処理
+            if(DEBUGP)RTCOUT << "update():"<<wbms->getUpdateTime()<<std::endl;
+            if(DEBUGP)wbms->rp_ref_out.print();
+
+            solveFullbodyIK(wbms->rp_ref_out.tgt[com].abs, wbms->rp_ref_out.tgt[rf].abs, wbms->rp_ref_out.tgt[lf].abs, wbms->rp_ref_out.tgt[rh].abs, wbms->rp_ref_out.tgt[lh].abs, wbms->rp_ref_out.tgt[head].abs);
 
             if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processWholeBodyMasterSlave" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
             //逆動力学
             //        calcAccelerationsForInverseDynamics(m_robot, idsb);
             //        hrp::Vector3 ref_zmp_invdyn;
-            //        calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);
+            //        calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);processTransition
             //        ref_zmp_invdyn = invdyn_zmp_filters.passFilter(ref_zmp_invdyn);
             //        updateInvDynStateBuffer(idsb);
 
             if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ calcWorldZMPFromInverseDynamics" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
-            processHOFFARBIBFilter(m_robot, m_robot_vsafe);
+            processHOFFARBIBFilter(fik->m_robot, m_robot_vsafe);
 
             hrp::Vector3 com = m_robot_vsafe->calcCM();
             static hrp::Vector3 com_old = com;
             static hrp::Vector3 com_old_old = com_old;
             hrp::Vector3 com_acc = (com - 2*com_old + com_old_old)/(m_dt*m_dt);
             hrp::Vector3 ref_zmp; ref_zmp << com.head(XY)-(com(Z)/G)*com_acc.head(XY), 0;
-            if(mode.isInitialize()){ final_ref_zmp_filter.reset(ref_zmp); }
-            ref_zmp = final_ref_zmp_filter.passFilter(ref_zmp);
+            if(mode.isInitialize()){ ref_zmp_filter.reset(ref_zmp); }
+            ref_zmp = ref_zmp_filter.passFilter(ref_zmp);
             com_old_old = com_old;
             com_old = com;
             wbms->act_rs.com = com;
@@ -298,27 +291,22 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
             const std::vector<std::string> lower(tmp, tmp+12);
             // qRef
             for (int i = 0; i < m_qRef.data.length(); i++ ){
-              if(!wbms->wp.use_lower && std::find(lower.begin(), lower.end(), m_robot->joint(i)->name) != lower.end()){}// pass through lower limbs
+              if(!wbms->wp.use_lower && std::find(lower.begin(), lower.end(), fik->m_robot->joint(i)->name) != lower.end()){}// pass through lower limbs
               else{
-                m_qRef.data[i] = transition_interpolator_ratio * m_robot_vsafe->joint(i)->q  + (1 - transition_interpolator_ratio) * m_qRef.data[i];
+                m_qRef.data[i] = output_ratio * m_robot_vsafe->joint(i)->q  + (1 - output_ratio) * m_qRef.data[i];
               }
             }
+//            m_qRef.data = hrp::to_DoubleSeq(output_ratio * hrp::getQAll(m_robot_vsafe)  + (1 - output_ratio) * hrp::to_dvector(m_qRef.data));
+
             // basePos
-            m_basePos.data.x = transition_interpolator_ratio * m_robot_vsafe->rootLink()->p(0) + (1 - transition_interpolator_ratio) * m_basePos.data.x;
-            m_basePos.data.y = transition_interpolator_ratio * m_robot_vsafe->rootLink()->p(1) + (1 - transition_interpolator_ratio) * m_basePos.data.y;
-            m_basePos.data.z = transition_interpolator_ratio * m_robot_vsafe->rootLink()->p(2) + (1 - transition_interpolator_ratio) * m_basePos.data.z;
+            m_basePos.data = hrp::to_Point3D( output_ratio * m_robot_vsafe->rootLink()->p + (1 - output_ratio) * hrp::to_Vector3(m_basePos.data));
             m_basePos.tm = m_qRef.tm;
             // baseRpy
-            hrp::Vector3 baseRpy = hrp::rpyFromRot(m_robot_vsafe->rootLink()->R);
-            m_baseRpy.data.r = transition_interpolator_ratio * baseRpy(0) + (1 - transition_interpolator_ratio) * m_baseRpy.data.r;
-            m_baseRpy.data.p = transition_interpolator_ratio * baseRpy(1) + (1 - transition_interpolator_ratio) * m_baseRpy.data.p;
-            m_baseRpy.data.y = transition_interpolator_ratio * baseRpy(2) + (1 - transition_interpolator_ratio) * m_baseRpy.data.y;
+            m_baseRpy.data = hrp::to_Orientation3D( output_ratio * hrp::rpyFromRot(m_robot_vsafe->rootLink()->R) + (1 - output_ratio) * hrp::to_Vector3(m_baseRpy.data));
             m_baseRpy.tm = m_qRef.tm;
             // zmp
             hrp::Vector3 rel_ref_zmp = m_robot_vsafe->rootLink()->R.transpose() * (ref_zmp - m_robot_vsafe->rootLink()->p);
-            m_zmp.data.x = transition_interpolator_ratio * rel_ref_zmp(0) + (1 - transition_interpolator_ratio) * m_zmp.data.x;
-            m_zmp.data.y = transition_interpolator_ratio * rel_ref_zmp(1) + (1 - transition_interpolator_ratio) * m_zmp.data.y;
-            m_zmp.data.z = transition_interpolator_ratio * rel_ref_zmp(2) + (1 - transition_interpolator_ratio) * m_zmp.data.z;
+            m_zmp.data = hrp::to_Point3D( output_ratio * rel_ref_zmp + (1 - output_ratio) * hrp::to_Vector3(m_zmp.data));
             m_zmp.tm = m_qRef.tm;
             // m_optionalData
             if(m_optionalData.data.length() < optionalDataLength){
@@ -328,8 +316,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
             m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = wbms->rp_ref_out.tgt[rf].is_contact;
             m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = wbms->rp_ref_out.tgt[lf].is_contact;
         }
-        wbms->baselinkpose.p = m_robot->rootLink()->p;
-        wbms->baselinkpose.rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
+        wbms->baselinkpose.p = fik->m_robot->rootLink()->p;
+        wbms->baselinkpose.rpy = hrp::rpyFromRot(fik->m_robot->rootLink()->R);
     }
 
     // write
@@ -355,7 +343,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     m_htrh_dbgOut.write();
     m_htlh_dbg.tm = m_qRef.tm;
     WBMSCore::WBMSPose3DToPose3D(wbms->hp_plot.tgt[lh].abs, m_htlh_dbg.data);
-    m_htlh_dbgOut.write();
+    m_htlh_dbgOut.write();mode
     m_hthead_dbg.tm = m_qRef.tm;
     WBMSCore::WBMSPose3DToPose3D(wbms->hp_plot.tgt[head].abs, m_hthead_dbg.data);
     m_hthead_dbgOut.write();
@@ -412,31 +400,32 @@ void WholeBodyMasterSlave::processTransition(){
     switch(mode.now()){
 
         case MODE_SYNC_TO_WBMS:
-            if(mode.pre() == MODE_IDLE){ double tmp = 1.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
-            if (!transition_interpolator->isEmpty() ){
-                transition_interpolator->get(&transition_interpolator_ratio, true);
+            if(mode.pre() == MODE_IDLE){ double tmp = 1.0; t_ip->setGoal(&tmp, 3.0, true); }
+            if (!t_ip->isEmpty() ){
+                t_ip->get(&output_ratio, true);
             }else{
-                mode.setModeRequest(MODE_WBMS);
+                mode.setNextMode(MODE_WBMS);
             }
             break;
 
         case MODE_SYNC_TO_IDLE:
-            if(mode.pre() == MODE_WBMS || mode.pre() == MODE_PAUSE){ double tmp = 0.0; transition_interpolator->setGoal(&tmp, 3.0, true); }
-            if (!transition_interpolator->isEmpty()) {
-                transition_interpolator->get(&transition_interpolator_ratio, true);
+            if(mode.pre() == MODE_WBMS || mode.pre() == MODE_PAUSE){ double tmp = 0.0; t_ip->setGoal(&tmp, 3.0, true); }
+            if (!t_ip->isEmpty()) {
+                t_ip->get(&output_ratio, true);
             }else{
-                mode.setModeRequest(MODE_IDLE);
+                mode.setNextMode(MODE_IDLE);
             }
             break;
     }
 }
 
 
-void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in){
-    hrp::Vector3 basePos_heightChecked = hrp::Vector3(m_basePos.data.x, m_basePos.data.y, m_basePos.data.z);//ベースリンク高さ調整により足裏高さ0に
-    robot_in->rootLink()->p = basePos_heightChecked;
-    for ( int i = 0; i < robot_in->numJoints(); i++ ){ robot_in->joint(i)->q = m_qRef.data[i]; }
-    robot_in->calcForwardKinematics();
+void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(){
+    hrp::Vector3 basePos_heightChecked = hrp::to_Vector3(m_basePos.data);//ベースリンク高さ調整により足裏高さ0に
+    fik->m_robot->rootLink()->p = basePos_heightChecked;
+//    for ( int i = 0; i < fik->m_robot->numJoints(); i++ ){ fik->m_robot->joint(i)->q = m_qRef.data[i]; }
+    hrp::setQAll(fik->m_robot, hrp::to_dvector(m_qRef.data));
+    fik->m_robot->calcForwardKinematics();
 
     //TODO
 //    hrp::Vector3 init_foot_mid_coord = (fik_in->getEndEffectorPos("rleg") + fik_in->getEndEffectorPos("lleg")) / 2;
@@ -446,53 +435,29 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp
 //        std::cerr<<"["<<m_profile.instance_name<<"] Input basePos height is invalid. Auto modify "<<m_basePos.data.z<<" -> "<<basePos_heightChecked(Z)<<endl;
 //    }
 
-    const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
-
+    std::vector<hrp::BodyPtr> body_list;
+    body_list.push_back(fik->m_robot);
+    body_list.push_back(m_robot_vsafe);
     for(int i=0;i<body_list.size();i++){//初期姿勢でBodyをFK
-        body_list[i]->rootLink()->p = basePos_heightChecked;
-        body_list[i]->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
-        for ( int j = 0; j < body_list[i]->numJoints(); j++ ){ body_list[i]->joint(j)->q = m_qRef.data[j]; }
+        hrp::setRobotStateVec(body_list[i], hrp::to_dvector(m_qRef.data), basePos_heightChecked, hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y));
         body_list[i]->calcForwardKinematics();
     }
 
-    fik->q_ref << hrp::getQAll(robot_in), robot_in->rootLink()->p, hrp::rpyFromRot(robot_in->rootLink()->R);    //これいらない？
-
-
-//    fik->setReferenceJointAngles();
-//    for(int l=0;l<4;l++){//targetを初期化
-//        if(fik_list[i]->ikp.count(robot_l_names[l])){
-//            fik_list[i]->ikp[robot_l_names[l]].target_p0 = fik_list[i]->getEndEffectorPos(robot_l_names[l]);
-//            fik_list[i]->ikp[robot_l_names[l]].target_r0 = fik_list[i]->getEndEffectorRot(robot_l_names[l]);
-//        }
-//    }
-//    q_ip->clear();//clearは2回目以降しないほうがいい
-    init_sync_state.resize(ROBOT_ALL_DOF);
-    init_sync_state << hrp::getQAll(robot_in), robot_in->rootLink()->p, hrp::rpyFromRot(robot_in->rootLink()->R);
-    q_ip->set(init_sync_state.data());
-    wbms->initializeRequest(robot_in, ee_name_ikcp_map);
-}
-
-
-void WholeBodyMasterSlave::processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HumanPose& pose_ref){
-    wbms->update();//////HumanSynchronizerの主要処理
-    if(DEBUGP)cout<<"update():"<<wbms->getUpdateTime()<<endl;
-    if(DEBUGP)pose_ref.print();
-    solveFullbodyIK(fik_in, robot_in, pose_ref.tgt[com].abs, pose_ref.tgt[rf].abs, pose_ref.tgt[lf].abs, pose_ref.tgt[rh].abs, pose_ref.tgt[lh].abs, pose_ref.tgt[head].abs,"processWholeBodyMasterSlave");
+    fik->q_ref = hrp::getRobotStateVec(fik->m_robot);
+    q_ip->set(fik->q_ref.data());
+    wbms->initializeRequest(fik->m_robot, ee_ikc_map);
 }
 
 
-void WholeBodyMasterSlave::solveFullbodyIK(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix){
+void WholeBodyMasterSlave::solveFullbodyIK(const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref){
     std::vector<IKConstraint> ikc_list;
     if(wbms->wp.use_lower){
         IKConstraint tmp;
         tmp.target_link_name = "WAIST";
         tmp.localPos = hrp::Vector3::Zero();
         tmp.localR = hrp::Matrix33::Identity();
-//        tmp.targetPos = robot_in->rootLink()->p;// will be ignored by selection_vec
-//        tmp.targetRpy = com_ref.rpy;// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
-//        tmp.constraint_weight << 0,0,0,1,1,1;
-        tmp.targetPos << m_basePos.data.x,m_basePos.data.y,m_basePos.data.z;// will be ignored by selection_vec
-        tmp.targetRpy << m_baseRpy.data.r,m_baseRpy.data.p,m_baseRpy.data.y;// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
+        tmp.targetPos = hrp::to_Vector3(m_basePos.data);// will be ignored by selection_vec
+        tmp.targetRpy = hrp::to_Vector3(m_baseRpy.data);// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
         tmp.constraint_weight << 0,0,0,1,1,1;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
@@ -502,17 +467,17 @@ void WholeBodyMasterSlave::solveFullbodyIK(fikPtr& fik_in, hrp::BodyPtr& robot_i
       tmp.target_link_name = "WAIST";
       tmp.localPos = hrp::Vector3::Zero();
       tmp.localR = hrp::Matrix33::Identity();
-      tmp.targetPos << m_basePos.data.x, m_basePos.data.y, m_basePos.data.z;// will be ignored by selection_vec
-      tmp.targetRpy << m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y;// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
+      tmp.targetPos = hrp::to_Vector3(m_basePos.data);// will be ignored by selection_vec
+      tmp.targetRpy = hrp::to_Vector3(m_baseRpy.data);// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
       tmp.constraint_weight << 1,1,1,1,1,1;
       tmp.rot_precision = deg2rad(3);
       ikc_list.push_back(tmp);
     }
     if(wbms->wp.use_lower){
         IKConstraint tmp;
-        tmp.target_link_name = ee_name_ikcp_map["rleg"].target_link_name;
-        tmp.localPos = ee_name_ikcp_map["rleg"].localPos;
-        tmp.localR = ee_name_ikcp_map["rleg"].localR;
+        tmp.target_link_name = ee_ikc_map["rleg"].target_link_name;
+        tmp.localPos = ee_ikc_map["rleg"].localPos;
+        tmp.localR = ee_ikc_map["rleg"].localR;
         tmp.targetPos = rf_ref.p;
         tmp.targetRpy = rf_ref.rpy;
         if(wbms->rp_ref_out.tgt[rf].is_contact){
@@ -524,9 +489,9 @@ void WholeBodyMasterSlave::solveFullbodyIK(fikPtr& fik_in, hrp::BodyPtr& robot_i
     }
     if(wbms->wp.use_lower){
         IKConstraint tmp;
-        tmp.target_link_name = ee_name_ikcp_map["lleg"].target_link_name;
-        tmp.localPos = ee_name_ikcp_map["lleg"].localPos;
-        tmp.localR = ee_name_ikcp_map["lleg"].localR;
+        tmp.target_link_name = ee_ikc_map["lleg"].target_link_name;
+        tmp.localPos = ee_ikc_map["lleg"].localPos;
+        tmp.localR = ee_ikc_map["lleg"].localR;
         tmp.targetPos = lf_ref.p;
         tmp.targetRpy = lf_ref.rpy;
         if(wbms->rp_ref_out.tgt[lf].is_contact){
@@ -539,9 +504,9 @@ void WholeBodyMasterSlave::solveFullbodyIK(fikPtr& fik_in, hrp::BodyPtr& robot_i
     const double dist = 0.0;
     {
         IKConstraint tmp;
-        tmp.target_link_name = ee_name_ikcp_map["rarm"].target_link_name;
-        tmp.localPos = ee_name_ikcp_map["rarm"].localPos;
-        tmp.localR = ee_name_ikcp_map["rarm"].localR;
+        tmp.target_link_name = ee_ikc_map["rarm"].target_link_name;
+        tmp.localPos = ee_ikc_map["rarm"].localPos;
+        tmp.localR = ee_ikc_map["rarm"].localR;
         if((rh_ref.p-lh_ref.p).norm()<dist){
             tmp.targetPos = (rh_ref.p+lh_ref.p)/2 + (rh_ref.p-lh_ref.p).normalized()*dist/2;
         }else{
@@ -554,9 +519,9 @@ void WholeBodyMasterSlave::solveFullbodyIK(fikPtr& fik_in, hrp::BodyPtr& robot_i
         ikc_list.push_back(tmp);
     }{
         IKConstraint tmp;
-        tmp.target_link_name = ee_name_ikcp_map["larm"].target_link_name;
-        tmp.localPos = ee_name_ikcp_map["larm"].localPos;
-        tmp.localR = ee_name_ikcp_map["larm"].localR;
+        tmp.target_link_name = ee_ikc_map["larm"].target_link_name;
+        tmp.localPos = ee_ikc_map["larm"].localPos;
+        tmp.localR = ee_ikc_map["larm"].localR;
         if((rh_ref.p-lh_ref.p).norm()<dist){
             tmp.targetPos = (rh_ref.p+lh_ref.p)/2 + (lh_ref.p-rh_ref.p).normalized()*dist/2;
         }else{
@@ -567,7 +532,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(fikPtr& fik_in, hrp::BodyPtr& robot_i
         tmp.pos_precision = 3e-3;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
-    }if(robot_in->link("HEAD_JOINT1") != NULL){
+    }if(fik->m_robot->link("HEAD_JOINT1") != NULL){
         IKConstraint tmp;
         tmp.target_link_name = "HEAD_JOINT1";
         tmp.targetRpy = head_ref.rpy;
@@ -605,21 +570,21 @@ void WholeBodyMasterSlave::solveFullbodyIK(fikPtr& fik_in, hrp::BodyPtr& robot_i
         double margin = 1e-3;
         if(sccp->avoid_priority(sccp->collision_info_list[i].id0) > sccp->avoid_priority(sccp->collision_info_list[i].id1)){
             tmp.localPos = sccp->collision_info_list[i].cp1_local;
-            tmp.target_link_name = m_robot->joint(sccp->collision_info_list[i].id1)->name;
+            tmp.target_link_name = fik->m_robot->joint(sccp->collision_info_list[i].id1)->name;
             tmp.targetPos = sccp->collision_info_list[i].cp0_wld + (sccp->collision_info_list[i].cp1_wld - sccp->collision_info_list[i].cp0_wld).normalized() * (sccp->collision_info_list[i].dist_safe + margin);
             ikc_list.push_back(tmp);
         }else if(sccp->avoid_priority(sccp->collision_info_list[i].id0) < sccp->avoid_priority(sccp->collision_info_list[i].id1)){
             tmp.localPos = sccp->collision_info_list[i].cp0_local;
-            tmp.target_link_name = m_robot->joint(sccp->collision_info_list[i].id0)->name;
+            tmp.target_link_name = fik->m_robot->joint(sccp->collision_info_list[i].id0)->name;
             tmp.targetPos = sccp->collision_info_list[i].cp1_wld + (sccp->collision_info_list[i].cp0_wld - sccp->collision_info_list[i].cp1_wld).normalized() * (sccp->collision_info_list[i].dist_safe + margin);
             ikc_list.push_back(tmp);
         }else{
             tmp.localPos = sccp->collision_info_list[i].cp1_local;
-            tmp.target_link_name = m_robot->joint(sccp->collision_info_list[i].id1)->name;
+            tmp.target_link_name = fik->m_robot->joint(sccp->collision_info_list[i].id1)->name;
             tmp.targetPos = sccp->collision_info_list[i].cp0_wld + (sccp->collision_info_list[i].cp1_wld - sccp->collision_info_list[i].cp0_wld).normalized() * (sccp->collision_info_list[i].dist_safe + margin);
             ikc_list.push_back(tmp);
             tmp.localPos = sccp->collision_info_list[i].cp0_local;
-            tmp.target_link_name = m_robot->joint(sccp->collision_info_list[i].id0)->name;
+            tmp.target_link_name = fik->m_robot->joint(sccp->collision_info_list[i].id0)->name;
             tmp.targetPos = sccp->collision_info_list[i].cp1_wld + (sccp->collision_info_list[i].cp0_wld - sccp->collision_info_list[i].cp1_wld).normalized() * (sccp->collision_info_list[i].dist_safe + margin);
             ikc_list.push_back(tmp);
         }
@@ -631,7 +596,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(fikPtr& fik_in, hrp::BodyPtr& robot_i
         if(sccp->collision_info_list.size()>0){
             std::cout<<"pair:"<<std::endl;
             for(int i=0;i<sccp->collision_info_list.size();i++){
-                std::cout<<m_robot->joint(sccp->collision_info_list[i].id0)->name<<" "<<m_robot->joint(sccp->collision_info_list[i].id1)->name<<endl;
+                std::cout<<fik->m_robot->joint(sccp->collision_info_list[i].id0)->name<<" "<<fik->m_robot->joint(sccp->collision_info_list[i].id1)->name<<endl;
             }
         }
     }
@@ -655,42 +620,42 @@ void WholeBodyMasterSlave::solveFullbodyIK(fikPtr& fik_in, hrp::BodyPtr& robot_i
         ikc_list.push_back(tmp);
     }
 
-    if( robot_in->link("CHEST_JOINT0") != NULL) fik_in->dq_weight_all(robot_in->link("CHEST_JOINT0")->jointId) = 0.1;//JAXON
-    if( robot_in->link("CHEST_JOINT1") != NULL) fik_in->dq_weight_all(robot_in->link("CHEST_JOINT1")->jointId) = 0.1;
-    if( robot_in->link("CHEST_JOINT2") != NULL) fik_in->dq_weight_all(robot_in->link("CHEST_JOINT2")->jointId) = 0.1;
-    if( robot_in->link("CHEST_Y") != NULL) fik_in->dq_weight_all(robot_in->link("CHEST_Y")->jointId) = 0.1;//K
-    if( robot_in->link("CHEST_P") != NULL) fik_in->dq_weight_all(robot_in->link("CHEST_P")->jointId) = 0.1;
+    if( fik->m_robot->link("CHEST_JOINT0") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT0")->jointId) = 0.1;//JAXON
+    if( fik->m_robot->link("CHEST_JOINT1") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT1")->jointId) = 0.1;
+    if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 0.1;
+    if( fik->m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_Y")->jointId) = 0.1;//K
+    if( fik->m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_P")->jointId) = 0.1;
 //    if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->ulimit = deg2rad(-30);//脇内側の干渉回避
 //    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(30);
-    if( m_robot->link("RLEG_JOINT3") != NULL) m_robot->link("RLEG_JOINT3")->llimit = deg2rad(10);//膝伸びきり防止のため
-    if( m_robot->link("LLEG_JOINT3") != NULL) m_robot->link("LLEG_JOINT3")->llimit = deg2rad(10);
-    if( m_robot->link("R_KNEE_P") != NULL) m_robot->link("R_KNEE_P")->llimit = deg2rad(15);//K
-    if( m_robot->link("L_KNEE_P") != NULL) m_robot->link("L_KNEE_P")->llimit = deg2rad(15);
-    if( m_robot->link("R_WRIST_R") != NULL) m_robot->link("R_WRIST_R")->llimit = deg2rad(-40);
-    if( m_robot->link("L_WRIST_R") != NULL) m_robot->link("L_WRIST_R")->llimit = deg2rad(-40);
-    if( m_robot->link("R_WRIST_R") != NULL) m_robot->link("R_WRIST_R")->ulimit = deg2rad(40);
-    if( m_robot->link("L_WRIST_R") != NULL) m_robot->link("L_WRIST_R")->ulimit = deg2rad(40);
-    if( m_robot->link("R_WRIST_P") != NULL) m_robot->link("R_WRIST_P")->llimit = deg2rad(-40);
-    if( m_robot->link("L_WRIST_P") != NULL) m_robot->link("L_WRIST_P")->llimit = deg2rad(-40);
-    if( m_robot->link("R_WRIST_P") != NULL) m_robot->link("R_WRIST_P")->ulimit = deg2rad(20);
-    if( m_robot->link("L_WRIST_P") != NULL) m_robot->link("L_WRIST_P")->ulimit = deg2rad(20);
-
-
-    if( m_robot->link("CHEST_Y") != NULL) m_robot->link("CHEST_Y")->llimit = deg2rad(-20);
-    if( m_robot->link("CHEST_Y") != NULL) m_robot->link("CHEST_Y")->ulimit = deg2rad(20);
-    if( m_robot->link("CHEST_P") != NULL) m_robot->link("CHEST_P")->llimit = deg2rad(0);
-    if( m_robot->link("CHEST_P") != NULL) m_robot->link("CHEST_P")->ulimit = deg2rad(60);
+    if( fik->m_robot->link("RLEG_JOINT3") != NULL) fik->m_robot->link("RLEG_JOINT3")->llimit = deg2rad(10);//膝伸びきり防止のため
+    if( fik->m_robot->link("LLEG_JOINT3") != NULL) fik->m_robot->link("LLEG_JOINT3")->llimit = deg2rad(10);
+    if( fik->m_robot->link("R_KNEE_P") != NULL) fik->m_robot->link("R_KNEE_P")->llimit = deg2rad(15);//K
+    if( fik->m_robot->link("L_KNEE_P") != NULL) fik->m_robot->link("L_KNEE_P")->llimit = deg2rad(15);
+    if( fik->m_robot->link("R_WRIST_R") != NULL) fik->m_robot->link("R_WRIST_R")->llimit = deg2rad(-40);
+    if( fik->m_robot->link("L_WRIST_R") != NULL) fik->m_robot->link("L_WRIST_R")->llimit = deg2rad(-40);
+    if( fik->m_robot->link("R_WRIST_R") != NULL) fik->m_robot->link("R_WRIST_R")->ulimit = deg2rad(40);
+    if( fik->m_robot->link("L_WRIST_R") != NULL) fik->m_robot->link("L_WRIST_R")->ulimit = deg2rad(40);
+    if( fik->m_robot->link("R_WRIST_P") != NULL) fik->m_robot->link("R_WRIST_P")->llimit = deg2rad(-40);
+    if( fik->m_robot->link("L_WRIST_P") != NULL) fik->m_robot->link("L_WRIST_P")->llimit = deg2rad(-40);
+    if( fik->m_robot->link("R_WRIST_P") != NULL) fik->m_robot->link("R_WRIST_P")->ulimit = deg2rad(20);
+    if( fik->m_robot->link("L_WRIST_P") != NULL) fik->m_robot->link("L_WRIST_P")->ulimit = deg2rad(20);
+
+
+    if( fik->m_robot->link("CHEST_Y") != NULL) fik->m_robot->link("CHEST_Y")->llimit = deg2rad(-20);
+    if( fik->m_robot->link("CHEST_Y") != NULL) fik->m_robot->link("CHEST_Y")->ulimit = deg2rad(20);
+    if( fik->m_robot->link("CHEST_P") != NULL) fik->m_robot->link("CHEST_P")->llimit = deg2rad(0);
+    if( fik->m_robot->link("CHEST_P") != NULL) fik->m_robot->link("CHEST_P")->ulimit = deg2rad(60);
                                               
-    if( m_robot->link("HEAD_Y") != NULL) m_robot->link("HEAD_Y")->llimit = deg2rad(-5);
-    if( m_robot->link("HEAD_Y") != NULL) m_robot->link("HEAD_Y")->ulimit = deg2rad(5);
-    if( m_robot->link("HEAD_P") != NULL) m_robot->link("HEAD_P")->llimit = deg2rad(0);
-    if( m_robot->link("HEAD_P") != NULL) m_robot->link("HEAD_P")->ulimit = deg2rad(60);
+    if( fik->m_robot->link("HEAD_Y") != NULL) fik->m_robot->link("HEAD_Y")->llimit = deg2rad(-5);
+    if( fik->m_robot->link("HEAD_Y") != NULL) fik->m_robot->link("HEAD_Y")->ulimit = deg2rad(5);
+    if( fik->m_robot->link("HEAD_P") != NULL) fik->m_robot->link("HEAD_P")->llimit = deg2rad(0);
+    if( fik->m_robot->link("HEAD_P") != NULL) fik->m_robot->link("HEAD_P")->ulimit = deg2rad(60);
 
-    for(int i=0;i<robot_in->numJoints();i++){
-        LIMIT_MINMAX(robot_in->joint(i)->q, robot_in->joint(i)->llimit, robot_in->joint(i)->ulimit);
+    for(int i=0;i<fik->m_robot->numJoints();i++){
+        LIMIT_MINMAX(fik->m_robot->joint(i)->q, fik->m_robot->joint(i)->llimit, fik->m_robot->joint(i)->ulimit);
     }
 
-    fik_in->q_ref = init_sync_state;
+//    fik_in->q_ref = init_sync_state;
 
 
     // std::string tmp[] = {"R_CROTCH_R","R_CROTCH_P","R_CROTCH_Y","R_KNEE_P","R_ANKLE_R","R_ANKLE_P","L_CROTCH_R","L_CROTCH_P","L_CROTCH_Y","L_KNEE_P","L_ANKLE_R","L_ANKLE_P"};
@@ -698,7 +663,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(fikPtr& fik_in, hrp::BodyPtr& robot_i
     // for(int i=0; i<list.size(); i++){
     //   if( robot_in->link(list[i]) != NULL){
     //     int j=robot_in->link(list[i])->jointId;
-    //     fik_in->dq_weight_all(j) = 1e-5;
+    //     fik_in->dq_weight_all(j) = 1e-5;setNextMode
     //     robot_in->joint(j)->q = m_qRef.data[j];
     //     fik_in->q_ref(j) = m_qRef.data[j];
     //   }
@@ -714,78 +679,76 @@ void WholeBodyMasterSlave::solveFullbodyIK(fikPtr& fik_in, hrp::BodyPtr& robot_i
     struct timespec startT, endT;
     const int IK_MAX_LOOP = 2;
     clock_gettime(CLOCK_REALTIME, &startT);
-    int loop_result = fik_in->solveFullbodyIKLoop(robot_in, ikc_list, IK_MAX_LOOP);
+    int loop_result = fik->solveFullbodyIKLoop(ikc_list, IK_MAX_LOOP);
     if(loop%100==0){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveIK"<<loop_result<<"loop" << std::endl;}
 }
 
-void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out){
-    hrp::dvector goal_state(ROBOT_ALL_DOF);
-    for(int i=0;i<robot_in->numJoints();i++){ goal_state(i) = robot_in->joint(i)->q; }
-    goal_state.bottomRows(6).topRows(3) = robot_in->rootLink()->p;
-    goal_state.bottomRows(6).bottomRows(3) = hrp::rpyFromRot(robot_in->rootLink()->R);
-
+void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr _robot, hrp::BodyPtr _robot_safe){
     double goal_time = 0.0;
-//    const double min_goal_time_offset = 0.01;
     const double min_goal_time_offset = 0.1;
-    //  const double avg_q_vel = 0.5;
-    //  const double avg_q_vel = 1.0;
-//    const double avg_q_vel = 1.5;
-
-    static hrp::dvector tmp_v = hrp::dvector::Zero(ROBOT_ALL_DOF);
-//    if (!q_ip->isEmpty() ){  q_ip->get(tmp_x, tmp_v, false);}
-    for(int i=0;i<robot_in->numJoints();i++){
-        if(robot_in->joint(i)->name != "L_HAND" && robot_in->joint(i)->name != "R_HAND" ){ //KHI demo
-          double tmp_time = fabs(robot_in->joint(i)->q - robot_out->joint(i)->q) / avg_q_vel;
-          LIMIT_MIN(tmp_time, fabs(tmp_v(i))/16);
-  //        tmp_time += fabs(tmp_v(i))/16; //加速度8
-          if(tmp_time > goal_time){ goal_time = tmp_time; }
-        }
-    }
-    q_ip->setGoal(goal_state.data(), goal_time + min_goal_time_offset, true);
-    hrp::dvector ans_state(ROBOT_ALL_DOF);
-    double tmp[ROBOT_ALL_DOF];
-//    if (!q_ip->isEmpty() ){  q_ip->get(tmp, true);}
-    double tmpv[ROBOT_ALL_DOF];
-    if (!q_ip->isEmpty() ){  q_ip->get(tmp, tmpv, true);}
-    tmp_v = Eigen::Map<hrp::dvector>(tmpv, ROBOT_ALL_DOF);
-    ans_state = Eigen::Map<hrp::dvector>(tmp, ROBOT_ALL_DOF);
-    for(int i=0;i<robot_out->numJoints();i++){
-      if(robot_in->joint(i)->name != "L_HAND" && robot_in->joint(i)->name != "R_HAND" ){ //KHI demo
-        robot_out->joint(i)->q = ans_state(i);
-      }
-    }
 
+    static hrp::dvector ans_state_vel = hrp::dvector::Zero(fik->numStates());
+//    if (!q_ip->isEmpty() ){  q_ip->get(tmp_x, ans_state_vel, false);}
 
+//    for(int i=0;i<_robot->numJoints();i++){
+//        if(_robot->joint(i)->name != "L_HAND" && _robot->joint(i)->name != "R_HAND" ){ //KHI demo
+//          double tmp_time = fabs(_robot->joint(i)->q - _robot_safe->joint(i)->q) / avg_q_vel(i);
+//          LIMIT_MIN(tmp_time, fabs(ans_state_vel(i))/16);
+//  //        tmp_time += fabs(ans_state_vel(i))/16; //加速度8
+//          if(tmp_time > goal_time){ goal_time = tmp_time; }
+//        }
+//    }
+//
+//    q_ip->setGoal(hrp::getRobotStateVec(_robot).data(), goal_time + min_goal_time_offset, true);
+//    hrp::dvector ans_state(fik->numStates());
+//    double tmp[fik->numStates()], tmpv[fik->numStates()];
+//    if (!q_ip->isEmpty() ){  q_ip->get(tmp, tmpv, true);}
+//    ans_state_vel = Eigen::Map<hrp::dvector>(tmpv, fik->numStates());
+//    ans_state = Eigen::Map<hrp::dvector>(tmp, fik->numStates());
+
+
+    hrp::dvector estimated_times_from_vel_limit = (hrp::getRobotStateVec(_robot) - hrp::getRobotStateVec(_robot_safe)).array().abs() / avg_q_vel.array();
+    hrp::dvector estimated_times_from_acc_limit = ans_state_vel.array() / avg_q_acc.array();
+    double longest_estimated_time_from_vel_limit = estimated_times_from_vel_limit.maxCoeff();
+    double longest_estimated_time_from_acc_limit = estimated_times_from_acc_limit.maxCoeff();
+    goal_time = hrp::Vector3(longest_estimated_time_from_vel_limit, longest_estimated_time_from_acc_limit, min_goal_time_offset).maxCoeff();
+
+    q_ip->setGoal(hrp::getRobotStateVec(_robot).data(), goal_time, true);
+    double tmp[fik->numStates()], tmpv[fik->numStates()];
+    if (!q_ip->isEmpty() ){  q_ip->get(tmp, tmpv, true);}
+    hrp::dvector ans_state = Eigen::Map<hrp::dvector>(tmp, fik->numStates());
+    ans_state_vel = Eigen::Map<hrp::dvector>(tmpv, fik->numStates());
+
+
+//    //// KHI demo
+//    const double hand_max_vel = 180.0/0.4 /180.0*M_PI;// 0.4sec for 180deg
+//    std::map<std::string, double> joy_inputs;// 0 ~ 1
+//    joy_inputs["L_HAND"] = m_htlfw.data.force.x;// tmp substitute
+//    joy_inputs["R_HAND"] = m_htrfw.data.force.x;// tmp substitute
+//    for(std::map<std::string, double>::iterator it = joy_inputs.begin(); it!=joy_inputs.end(); it++){
+//      if(_robot_safe->link(it->first) != NULL){
+//          LIMIT_MINMAX(it->second, 0.0, 1.0);
+//          double tgt_hand_q = (-29.0 + (124.0-(-29.0))*it->second ) /180.0*M_PI;
+//          const double q_old = _robot_safe->link(it->first)->q;
+//          LIMIT_MINMAX(tgt_hand_q, q_old - hand_max_vel*m_dt, q_old + hand_max_vel*m_dt);
+//          _robot_safe->link(it->first)->q = tgt_hand_q;
+//      }
+//    }
 
-    //// KHI demo
-    const double hand_max_vel = 180.0/0.4 /180.0*M_PI;// 0.4sec for 180deg
-    std::map<std::string, double> joy_inputs;// 0 ~ 1
-    joy_inputs["L_HAND"] = m_htlfw.data.force.x;// tmp substitute
-    joy_inputs["R_HAND"] = m_htrfw.data.force.x;// tmp substitute
-    for(std::map<std::string, double>::iterator it = joy_inputs.begin(); it!=joy_inputs.end(); it++){
-      if(robot_out->link(it->first) != NULL){
-          LIMIT_MINMAX(it->second, 0.0, 1.0);
-          double tgt_hand_q = (-29.0 + (124.0-(-29.0))*it->second ) /180.0*M_PI;
-          const double q_old = robot_out->link(it->first)->q;
-          LIMIT_MINMAX(tgt_hand_q, q_old - hand_max_vel*m_dt, q_old + hand_max_vel*m_dt);
-          robot_out->link(it->first)->q = tgt_hand_q;
-      }
-    }
+    hrp::setRobotStateVec(_robot_safe, ans_state);
 
-    robot_out->rootLink()->p = ans_state.bottomRows(6).topRows(3);
-    robot_out->rootLink()->R = hrp::rotFromRpy(ans_state.bottomRows(6).bottomRows(3));
-    robot_out->calcForwardKinematics();
+    _robot_safe->calcForwardKinematics();
 }
 
 
 bool WholeBodyMasterSlave::startWholeBodyMasterSlave(){
     if(mode.now() == MODE_IDLE){
-        std::cerr << "[" << m_profile.instance_name << "] startWholeBodyMasterSlave" << std::endl;
-        mode.setModeRequest(MODE_SYNC_TO_WBMS);
-        while(!transition_interpolator->isEmpty()){ usleep(1000); }
+        RTCOUT << "startWholeBodyMasterSlave" << std::endl;
+        mode.setNextMode(MODE_SYNC_TO_WBMS);
+        while(!t_ip->isEmpty()){ usleep(1000); }
         return true;
     }else{
-        std::cerr << "[" << m_profile.instance_name << "] Invalid context to startWholeBodyMasterSlave" << std::endl;
+        RTCOUT << "Invalid context to startWholeBodyMasterSlave" << std::endl;
         return false;
     }
 }
@@ -793,11 +756,11 @@ bool WholeBodyMasterSlave::startWholeBodyMasterSlave(){
 
 bool WholeBodyMasterSlave::pauseWholeBodyMasterSlave(){
     if(mode.now() == MODE_WBMS){
-        std::cerr << "[" << m_profile.instance_name << "] pauseWholeBodyMasterSlave" << std::endl;
-        mode.setModeRequest(MODE_PAUSE);
+        RTCOUT << "pauseWholeBodyMasterSlave" << std::endl;
+        mode.setNextMode(MODE_PAUSE);
         return true;
     }else{
-        std::cerr << "[" << m_profile.instance_name << "] Invalid context to pauseWholeBodyMasterSlave" << std::endl;
+        RTCOUT << "Invalid context to pauseWholeBodyMasterSlave" << std::endl;
         return false;
     }
 }
@@ -805,14 +768,14 @@ bool WholeBodyMasterSlave::pauseWholeBodyMasterSlave(){
 
 bool WholeBodyMasterSlave::resumeWholeBodyMasterSlave(){
     if(mode.now() == MODE_PAUSE){
-        std::cerr << "[" << m_profile.instance_name << "] resumeWholeBodyMasterSlave" << std::endl;
-        mode.setModeRequest(MODE_WBMS);
+        RTCOUT << "resumeWholeBodyMasterSlave" << std::endl;
+        mode.setNextMode(MODE_WBMS);
         avg_q_vel /= 5;
         sleep(5);
         avg_q_vel *= 5;
         return true;
     }else{
-        std::cerr << "[" << m_profile.instance_name << "] Invalid context to resumeWholeBodyMasterSlave" << std::endl;
+        RTCOUT << "Invalid context to resumeWholeBodyMasterSlave" << std::endl;
         return false;
     }
 }
@@ -820,19 +783,19 @@ bool WholeBodyMasterSlave::resumeWholeBodyMasterSlave(){
 
 bool WholeBodyMasterSlave::stopWholeBodyMasterSlave(){
     if(mode.now() == MODE_WBMS || mode.now() == MODE_PAUSE ){
-        std::cerr << "[" << m_profile.instance_name << "] stopWholeBodyMasterSlave" << std::endl;
-        mode.setModeRequest(MODE_SYNC_TO_IDLE);
-        while(!transition_interpolator->isEmpty()){ usleep(1000); }
+        RTCOUT << "stopWholeBodyMasterSlave" << std::endl;
+        mode.setNextMode(MODE_SYNC_TO_IDLE);
+        while(!t_ip->isEmpty()){ usleep(1000); }
         return true;
     }else{
-        std::cerr << "[" << m_profile.instance_name << "] Invalid context to stopWholeBodyMasterSlave" << std::endl;
+        RTCOUT << "Invalid context to stopWholeBodyMasterSlave" << std::endl;
         return false;
     }
 }
 
 
 bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
-    std::cerr << "[" << m_profile.instance_name << "] setWholeBodyMasterSlaveParam" << std::endl;
+    RTCOUT << "setWholeBodyMasterSlaveParam" << std::endl;
     wbms->wp.auto_swing_foot_landing_threshold = i_param.auto_swing_foot_landing_threshold;
     wbms->wp.foot_vertical_vel_limit_coeff = i_param.foot_vertical_vel_limit_coeff;
     wbms->wp.human_com_height = i_param.human_com_height;
@@ -853,7 +816,7 @@ bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBody
 
 
 bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
-    std::cerr << "[" << m_profile.instance_name << "] getWholeBodyMasterSlaveParam" << std::endl;
+    RTCOUT << "getWholeBodyMasterSlaveParam" << std::endl;
     i_param.auto_swing_foot_landing_threshold = wbms->wp.auto_swing_foot_landing_threshold;
     i_param.foot_vertical_vel_limit_coeff = wbms->wp.foot_vertical_vel_limit_coeff;
     i_param.human_com_height = wbms->wp.human_com_height;
@@ -869,8 +832,8 @@ bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMaster
 }
 
 
-RTC::ReturnCode_t WholeBodyMasterSlave::onActivated(RTC::UniqueId ec_id){ std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; }
-RTC::ReturnCode_t WholeBodyMasterSlave::onDeactivated(RTC::UniqueId ec_id){ std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; }
+RTC::ReturnCode_t WholeBodyMasterSlave::onActivated(RTC::UniqueId ec_id){ RTCOUT << "onActivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; }
+RTC::ReturnCode_t WholeBodyMasterSlave::onDeactivated(RTC::UniqueId ec_id){ RTCOUT << "onDeactivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; }
 
 extern "C"{
     void WholeBodyMasterSlaveInit(RTC::Manager* manager) {
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 2c180a3f8dd..4c58d794451 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -10,95 +10,91 @@
 #include <rtm/DataOutPort.h>
 #include <rtm/idl/BasicDataTypeSkel.h>
 #include <rtm/idl/ExtendedDataTypesSkel.h>
-#include <hrpModel/Body.h>
-//#include "../ImpedanceController/JointPathEx.h"
-// Service implementation headers
+#include <rtm/CorbaNaming.h>
+#include <hrpModel/ModelLoaderUtil.h>
+
 #include "WholeBodyMasterSlaveService_impl.h"
-#include "interpolator.h"
-//#include "../TorqueFilter/IIRFilter.h"
 #include "wbms_core.h"
 
-//using namespace RTC;
-
 //#define USE_DEBUG_PORT
 
 enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE};
 
 class ControlMode{
     private:
-        mode_enum current_mode, previous_mode, requested_mode;
+        mode_enum current, previous, next;
     public:
-        ControlMode(){ current_mode = previous_mode = requested_mode = MODE_IDLE;}
+        ControlMode(){ current = previous = next = MODE_IDLE;}
         ~ControlMode(){}
-        bool setModeRequest(mode_enum in){
-            switch(in){
+        bool setNextMode(const mode_enum _request){
+            switch(_request){
                 case MODE_SYNC_TO_WBMS:
-                    if(current_mode == MODE_IDLE){ requested_mode = MODE_SYNC_TO_WBMS; return true; }else{ return false; }
+                    if(current == MODE_IDLE){ next = MODE_SYNC_TO_WBMS; return true; }else{ return false; }
                 case MODE_WBMS:
-                    if(current_mode == MODE_SYNC_TO_WBMS || current_mode == MODE_PAUSE ){ requested_mode = MODE_WBMS; return true; }else{ return false; }
+                    if(current == MODE_SYNC_TO_WBMS || current == MODE_PAUSE ){ next = MODE_WBMS; return true; }else{ return false; }
                 case MODE_PAUSE:
-                    if(current_mode == MODE_WBMS){ requested_mode = MODE_PAUSE; return true; }else{ return false; }
+                    if(current == MODE_WBMS){ next = MODE_PAUSE; return true; }else{ return false; }
                 case MODE_SYNC_TO_IDLE:
-                    if(current_mode == MODE_WBMS || current_mode == MODE_PAUSE ){ requested_mode = MODE_SYNC_TO_IDLE; return true; }else{ return false; }
+                    if(current == MODE_WBMS || current == MODE_PAUSE ){ next = MODE_SYNC_TO_IDLE; return true; }else{ return false; }
                 case MODE_IDLE:
-                    if(current_mode == MODE_SYNC_TO_IDLE ){ requested_mode = MODE_IDLE; return true; }else{ return false; }
+                    if(current == MODE_SYNC_TO_IDLE ){ next = MODE_IDLE; return true; }else{ return false; }
                 default:
                     return false;
             }
         }
-        void update(){ previous_mode = current_mode; current_mode = requested_mode; }
-        mode_enum now(){ return current_mode; }
-        mode_enum pre(){ return previous_mode; }
-        bool isRunning(){ return (current_mode==MODE_SYNC_TO_WBMS) || (current_mode==MODE_WBMS) || (current_mode==MODE_PAUSE) || (current_mode==MODE_SYNC_TO_IDLE) ;}
-        bool isInitialize(){ return (previous_mode==MODE_IDLE) && (current_mode==MODE_SYNC_TO_WBMS) ;}
+        void update(){ previous = current; current = next; }
+        mode_enum now(){ return current; }
+        mode_enum pre(){ return previous; }
+        bool isRunning(){ return (current==MODE_SYNC_TO_WBMS) || (current==MODE_WBMS) || (current==MODE_PAUSE) || (current==MODE_SYNC_TO_IDLE) ;}
+        bool isInitialize(){ return (previous==MODE_IDLE) && (current==MODE_SYNC_TO_WBMS) ;}
 };
 
-class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
+class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
     public:
-    WholeBodyMasterSlave(RTC::Manager* manager);
-    virtual ~WholeBodyMasterSlave();
-    virtual RTC::ReturnCode_t onInitialize();
-    virtual RTC::ReturnCode_t onFinalize();
-    virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
-    virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
-    virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
-    bool startWholeBodyMasterSlave();
-    bool stopWholeBodyMasterSlave();
-    bool pauseWholeBodyMasterSlave();
-    bool resumeWholeBodyMasterSlave();
-    bool setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
-    bool getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
+        WholeBodyMasterSlave(RTC::Manager* manager);
+        virtual ~WholeBodyMasterSlave();
+        virtual RTC::ReturnCode_t onInitialize();
+        virtual RTC::ReturnCode_t onFinalize();
+        virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
+        virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
+        virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
+        bool startWholeBodyMasterSlave();
+        bool stopWholeBodyMasterSlave();
+        bool pauseWholeBodyMasterSlave();
+        bool resumeWholeBodyMasterSlave();
+        bool setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
+        bool getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
 
     protected:
-    RTC::TimedDoubleSeq m_qRef;
-    RTC::InPort<RTC::TimedDoubleSeq> m_qRefIn;
-    RTC::TimedPoint3D m_basePos;
-    RTC::InPort<RTC::TimedPoint3D> m_basePosIn;
-    RTC::TimedOrientation3D m_baseRpy;
-    RTC::InPort<RTC::TimedOrientation3D> m_baseRpyIn;
-    RTC::TimedPoint3D m_zmp;
-    RTC::InPort<RTC::TimedPoint3D> m_zmpIn;
-    RTC::TimedDoubleSeq m_optionalData;
-    RTC::InPort<RTC::TimedDoubleSeq> m_optionalDataIn;
-
-    RTC::TimedPose3D m_htcom;
-    RTC::InPort<RTC::TimedPose3D> m_htcomIn;
-    RTC::TimedPose3D m_htrf;
-    RTC::InPort<RTC::TimedPose3D> m_htrfIn;
-    RTC::TimedPose3D m_htlf;
-    RTC::InPort<RTC::TimedPose3D> m_htlfIn;
-    RTC::TimedPose3D m_htrh;
-    RTC::InPort<RTC::TimedPose3D> m_htrhIn;
-    RTC::TimedPose3D m_htlh;
-    RTC::InPort<RTC::TimedPose3D> m_htlhIn;
-    RTC::TimedPose3D m_hthead;
-    RTC::InPort<RTC::TimedPose3D> m_htheadIn;
-    RTC::TimedPoint3D m_htzmp;
-    RTC::InPort<RTC::TimedPoint3D> m_htzmpIn;
-    OpenHRP::TimedWrench m_htrfw;
-    RTC::InPort<OpenHRP::TimedWrench> m_htrfwIn;
-    OpenHRP::TimedWrench m_htlfw;
-    RTC::InPort<OpenHRP::TimedWrench> m_htlfwIn;
+        RTC::TimedDoubleSeq m_qRef;
+        RTC::InPort<RTC::TimedDoubleSeq> m_qRefIn;
+        RTC::TimedPoint3D m_basePos;
+        RTC::InPort<RTC::TimedPoint3D> m_basePosIn;
+        RTC::TimedOrientation3D m_baseRpy;
+        RTC::InPort<RTC::TimedOrientation3D> m_baseRpyIn;
+        RTC::TimedPoint3D m_zmp;
+        RTC::InPort<RTC::TimedPoint3D> m_zmpIn;
+        RTC::TimedDoubleSeq m_optionalData;
+        RTC::InPort<RTC::TimedDoubleSeq> m_optionalDataIn;
+
+        RTC::TimedPose3D m_htcom;
+        RTC::InPort<RTC::TimedPose3D> m_htcomIn;
+        RTC::TimedPose3D m_htrf;
+        RTC::InPort<RTC::TimedPose3D> m_htrfIn;
+        RTC::TimedPose3D m_htlf;
+        RTC::InPort<RTC::TimedPose3D> m_htlfIn;
+        RTC::TimedPose3D m_htrh;
+        RTC::InPort<RTC::TimedPose3D> m_htrhIn;
+        RTC::TimedPose3D m_htlh;
+        RTC::InPort<RTC::TimedPose3D> m_htlhIn;
+        RTC::TimedPose3D m_hthead;
+        RTC::InPort<RTC::TimedPose3D> m_htheadIn;
+        RTC::TimedPoint3D m_htzmp;
+        RTC::InPort<RTC::TimedPoint3D> m_htzmpIn;
+        OpenHRP::TimedWrench m_htrfw;
+        RTC::InPort<OpenHRP::TimedWrench> m_htrfwIn;
+        OpenHRP::TimedWrench m_htlfw;
+        RTC::InPort<OpenHRP::TimedWrench> m_htlfwIn;
 
 #ifdef USE_DEBUG_PORT
     TimedPose3D m_htcom_dbg;
@@ -141,70 +137,54 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase, UTIL_CONST {
     RTC::OutPort<RTC::TimedDoubleSeq> m_invdyn_dbgOut;
 #endif
 
-    RTC::OutPort<RTC::TimedDoubleSeq> m_qOut;
-    RTC::OutPort<RTC::TimedPoint3D> m_zmpOut;
-    RTC::OutPort<RTC::TimedPoint3D> m_basePosOut;
-    RTC::OutPort<RTC::TimedOrientation3D> m_baseRpyOut;
-    RTC::OutPort<RTC::TimedDoubleSeq> m_optionalDataOut;
-    RTC::CorbaPort m_WholeBodyMasterSlaveServicePort;
+        RTC::OutPort<RTC::TimedDoubleSeq> m_qOut;
+        RTC::OutPort<RTC::TimedPoint3D> m_zmpOut;
+        RTC::OutPort<RTC::TimedPoint3D> m_basePosOut;
+        RTC::OutPort<RTC::TimedOrientation3D> m_baseRpyOut;
+        RTC::OutPort<RTC::TimedDoubleSeq> m_optionalDataOut;
+        RTC::CorbaPort m_WholeBodyMasterSlaveServicePort;
 
-    WholeBodyMasterSlaveService_impl m_service0;
+        WholeBodyMasterSlaveService_impl m_service0;
 
     private:
-    typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
-    fikPtr fik;
-    hrp::BodyPtr m_robot, m_robot_vsafe;
-    std::vector<hrp::BodyPtr> body_list;
-    std::map<std::string, IKConstraint> ee_name_ikcp_map;
-    std::map<std::string, size_t> contact_states_index_map;
-    double m_dt;
-
-    double transition_interpolator_ratio;
-    interpolator *transition_interpolator;
-    int ROBOT_ALL_DOF;
-    interpolator *q_ip;
-    hrp::dvector init_sync_state;
-
-    int optionalDataLength;
-    unsigned int m_debugLevel;
-    bool is_legged_robot;
-    unsigned int loop;
-
-    double avg_q_vel;
-
-    hrp::InvDynStateBuffer idsb;
-    BiquadIIRFilterVec invdyn_zmp_filters;
-
-    BiquadIIRFilterVec final_ref_zmp_filter;
-
-    IIRFilter ref_ee_vel_v_filter;
-
-    HumanPose raw_pose;
-
-    boost::shared_ptr<WBMSCore> wbms;
-    boost::shared_ptr<CapsuleCollisionChecker> sccp;
-
-    hrp::Vector3 torso_rot_rmc;
-    ControlMode mode;
-
-    RTC::ReturnCode_t setupfik(fikPtr& fik_in, hrp::BodyPtr& robot_in, RTC::Properties& prop_in);
-    void calcManipulability(fikPtr& fik_in, hrp::BodyPtr& robot_in);
-    void calcManipulabilityJointLimit(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::Vector3 target_v_old[], WBMSPose3D& rf_ref, WBMSPose3D& lf_ref, WBMSPose3D& rh_ref, WBMSPose3D& lh_ref);
-    void calcManipulabilityJointLimitForWBMS(fikPtr& fik_in, hrp::BodyPtr& robot_in);
-    void solveFullbodyIK(fikPtr& fik_in, hrp::BodyPtr& robot_in, const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref, const std::string& debug_prefix="");
-    void processTransition();
-    void preProcessForWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in);
-    void processWholeBodyMasterSlave(fikPtr& fik_in, hrp::BodyPtr& robot_in, const HumanPose& pose_ref);
-    void processMomentumCompensation(fikPtr& fik_in, hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_normal_in, const HumanPose& pose_ref);
-    void processHOFFARBIBFilter(hrp::BodyPtr& robot_in, hrp::BodyPtr& robot_out);
-    bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; };
+        double m_dt;
+        unsigned int loop;
+        unsigned int m_debugLevel;
+        int optionalDataLength;
+        bool is_legged_robot;
+        hrp::BodyPtr m_robot_vsafe; // joint trajectory safe
+        typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
+        fikPtr fik;
+        std::map<std::string, IKConstraint> ee_ikc_map; // e.g. feet hands head com
+        std::map<std::string, size_t> contact_states_index_map;
+
+        double output_ratio;
+        interpolator *t_ip,*q_ip;
+        hrp::dvector avg_q_vel, avg_q_acc;
+
+        hrp::InvDynStateBuffer idsb;
+    //    BiquadIIRFilterVec invdyn_zmp_filters;
+        BiquadIIRFilterVec ref_zmp_filter;
+        IIRFilter ref_ee_vel_v_filter;
+
+        HumanPose raw_pose;
+
+        boost::shared_ptr<WBMSCore> wbms;
+        boost::shared_ptr<CapsuleCollisionChecker> sccp;
+
+        hrp::Vector3 torso_rot_rmc;
+        ControlMode mode;
+
+        RTC::ReturnCode_t setupEEIKConstraintFromConf(std::map<std::string, IKConstraint>& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop);
+        void solveFullbodyIK(const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref);
+        void processTransition();
+        void preProcessForWholeBodyMasterSlave();
+        void processWholeBodyMasterSlave(const HumanPose& pose_ref);
+        void processHOFFARBIBFilter(hrp::BodyPtr _robot, hrp::BodyPtr _robot_safe);
+        bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; };
 };
 
 
-
-
-
-
 extern "C"
 {
     void WholeBodyMasterSlaveInit(RTC::Manager* manager);
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 3f09a8350af..a21b54f7903 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -1,24 +1,16 @@
 #ifndef WBMS_CORE_H
 #define WBMS_CORE_H
-#include <rtm/idl/BasicDataType.hh>
-#include <rtm/idl/ExtendedDataTypes.hh>
-#include <rtm/Manager.h>
-#include <rtm/DataFlowComponentBase.h>
-#include <rtm/CorbaPort.h>
-#include <rtm/DataInPort.h>
-#include <rtm/DataOutPort.h>
-#include <rtm/idl/BasicDataTypeSkel.h>
-#include <rtm/idl/ExtendedDataTypesSkel.h>
+
 #include "interpolator.h"
-#include "../TorqueFilter/IIRFilter.h"
-#include <hrpModel/Body.h>
+#include <hrpUtil/Eigen4d.h>
+#include <hrpCollision/DistFuncs.h>
+#include "../ImpedanceController/JointPathEx.h"
+#include "../AutoBalancer/FullbodyInverseKinematicsSolver.h"
+// geometory
 #include <opencv2/core/core.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/imgproc/types_c.h>
-#include <hrpUtil/Eigen4d.h>
-#include <numeric>
-#include <hrpCollision/DistFuncs.h>
-
+// geometory
 #if defined(__cplusplus)
 extern "C" {
 #endif
@@ -36,40 +28,9 @@ extern "C" {
 }
 #endif
 
-#include "../AutoBalancer/FullbodyInverseKinematicsSolver.h"
-#define USE_NEW_FIK
-
 #define DEBUG 0
-#define dbg(var) std::cout<<#var"= "<<(var)<<std::endl
-#define dbgn(var) std::cout<<#var"= "<<std::endl<<(var)<<std::endl
-#define LIMIT_MIN(x,min) (x= ( x<min ? min:x ))
-#define LIMIT_MAX(x,max) (x= ( x<max ? x:max ))
-#define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
-#define SGN(x) ((x)>=0 ? 1 : -1)
-#define RTCOUT std::cerr << "[" << m_profile.instance_name << "] "
 
-class UTIL_CONST{
-    public:
-        enum { com, rf, lf, rh, lh, head, zmp, num_pose_tgt };
-        enum { num_ee_tgt=4 };
-        enum { R, L, LR };
-        enum { X, Y, Z, XYZ };
-        enum { XY = 2 };
-        enum { r, p, y, rpy };
-        enum { fx, fy, fz, tx, ty, tz, ft_xyz };
-        enum { MIN, MAX, MINMAX };
-        double G, D2R, INFMIN, INFMAX, Q_BUTTERWORTH, Q_NOOVERSHOOT;
-        UTIL_CONST() :
-            G(9.80665),
-            D2R( M_PI/180.0),
-            Q_BUTTERWORTH(0.707106781),
-            Q_NOOVERSHOOT(0.5),
-            INFMIN( - std::numeric_limits<double>::max()),
-            INFMAX( + std::numeric_limits<double>::max())
-        {};
-};
-
-class BiquadIIRFilterVec : UTIL_CONST {
+class BiquadIIRFilterVec{
     private:
         IIRFilter filters[XYZ];
         hrp::Vector3 ans;
@@ -77,18 +38,18 @@ class BiquadIIRFilterVec : UTIL_CONST {
         BiquadIIRFilterVec(){}
         ~BiquadIIRFilterVec(){}
         void setParameter(const hrp::Vector3& fc_in, const double& HZ, const double& Q = 0.5){ for(int i=0;i<XYZ;i++){ filters[i].setParameterAsBiquad((double)fc_in(i), Q, HZ); } }
-        void setParameter(const double& fc_in, const double& HZ, const double& Q = 0.5){ setParameter(hrp::Vector3(fc_in,fc_in,fc_in), HZ, Q); }//overload
-        hrp::Vector3 passFilter(const hrp::Vector3& input){ for(int i=0;i<XYZ;i++){ ans(i) = filters[i].passFilter((double)input(i)); } return ans; }
+        void setParameter(const double& fc_in, const double& HZ, const double& Q = 0.5){
+            setParameter(hrp::Vector3(fc_in,fc_in,fc_in), HZ, Q);
+        }//overload
+        hrp::Vector3 passFilter(const hrp::Vector3& input){
+            for(int i=0;i<XYZ;i++){
+                ans(i) = filters[i].passFilter((double)input(i));
+            }
+            return ans;
+        }
         void reset(const hrp::Vector3& initial_input){ for(int i=0;i<XYZ;i++){ filters[i].reset((double)initial_input(i));} }
 };
 
-class WBMSPose3D{
-    public:
-        hrp::Vector3 p,rpy;
-        WBMSPose3D(){ clear(); }
-        ~WBMSPose3D(){}
-        void clear(){ p = rpy = hrp::Vector3::Zero(); }
-};
 
 class PoseTGT{
     public:
@@ -100,7 +61,7 @@ class PoseTGT{
         void clear(){ abs.clear(); offs.clear(); cnt.clear(); w=hrp::dvector6::Zero(); is_contact = go_contact = false;}
 };
 
-class HumanPose : UTIL_CONST {
+class HumanPose{
     public:
         std::vector<PoseTGT> tgt;
         HumanPose(){ tgt.resize(num_pose_tgt); }
@@ -365,7 +326,7 @@ class CapsuleCollisionChecker {
 //        }
 };
 
-class WBMSCore : UTIL_CONST {
+class WBMSCore{
     private:
         double CNT_F_TH, h2r_ratio, tgt_h2r_ratio, HZ, DT;
         struct timeval t_calc_start, t_calc_end;
@@ -374,7 +335,6 @@ class WBMSCore : UTIL_CONST {
         std::vector<hrp::Vector2> rflf_points, hull_com, hull_dcp, hull_acp;
         std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rot_filters;
         std::vector<cv::Point2f> points,cvhull;
-        //    RobotConfig rc;
         HumanPose rp_ref_out_old, hp_swap_checked;
         unsigned int loop;
         bool is_initial_loop;
@@ -385,7 +345,6 @@ class WBMSCore : UTIL_CONST {
         HumanPose hp_wld_raw, hp_plot, rp_ref_out, rp_ref_vel_old;
         FILE *sr_log, *cz_log, *id_log;
         WBMSPose3D baselinkpose;
-        //    WBMSPose3D ee_contact_pose[4];
         hrp::Vector3 com_vel_old,rh_vel_old, cp_dec, cp_acc;
         std::vector<hrp::Vector3> foot_vert_act[LR], foot_vert_safe[LR], foot_vert_check[LR];
         hrp::Vector2 com_forcp_ref,com_vel_forcp_ref;
@@ -417,7 +376,6 @@ class WBMSCore : UTIL_CONST {
         hrp::Vector3 manip_sv_rot[4];
 
         hrp::Vector2 cp_acc_old;
-        //    WBMSPose3D ee_pose_old[4];
 
         WBMSCore(const double& dt){
             tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
@@ -510,13 +468,13 @@ class WBMSCore : UTIL_CONST {
                 hp_wld_raw.tgt[i].offs.rpy =  hp_wld_raw.tgt[i].abs.rpy;
             }
         }
-        void initializeRobotPoseFromHRPBody(const hrp::BodyPtr robot_in, std::map<std::string, IKConstraint>& ee_name_ikcp_map_in){
+        void initializeRobotPoseFromHRPBody(const hrp::BodyPtr robot_in, std::map<std::string, IKConstraint>& _ee_ikc_map){
             const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
             const int human_l_names[4] = {rf,lf,rh,lh};
             for(int i=0;i<4;i++){//HumanSynchronizerの初期姿勢オフセットをセット
-                if(ee_name_ikcp_map_in.count(robot_l_names[i])){
-                    rp_ref_out.tgt[human_l_names[i]].offs.p = ee_name_ikcp_map_in[robot_l_names[i]].getCurrentTargetPos(robot_in);//          fik_in->getEndEffectorPos(robot_l_names[i]);
-                    rp_ref_out.tgt[human_l_names[i]].offs.rpy = hrp::rpyFromRot(ee_name_ikcp_map_in[robot_l_names[i]].getCurrentTargetRot(robot_in));
+                if(_ee_ikc_map.count(robot_l_names[i])){
+                    rp_ref_out.tgt[human_l_names[i]].offs.p = _ee_ikc_map[robot_l_names[i]].getCurrentTargetPos(robot_in);//          fik_in->getEndEffectorPos(robot_l_names[i]);
+                    rp_ref_out.tgt[human_l_names[i]].offs.rpy = hrp::rpyFromRot(_ee_ikc_map[robot_l_names[i]].getCurrentTargetRot(robot_in));
                     rp_ref_out.tgt[human_l_names[i]].abs = rp_ref_out.tgt[human_l_names[i]].offs;
                 }
             }
@@ -535,11 +493,11 @@ class WBMSCore : UTIL_CONST {
             }
             H_cur = rp_ref_out.tgt[com].offs.p(Z) - std::min((double)rp_ref_out.tgt[rf].offs.p(Z), (double)rp_ref_out.tgt[rf].offs.p(Z));
         }
-        void initializeRequest(hrp::BodyPtr robot_in, std::map<std::string, IKConstraint>& ee_name_ikcp_map_in){
+        void initializeRequest(hrp::BodyPtr robot_in, std::map<std::string, IKConstraint>& _ee_ikc_map){
             loop = 0;
             is_initial_loop = true;
             initializeHumanPoseFromCurrentInput();
-            initializeRobotPoseFromHRPBody(robot_in, ee_name_ikcp_map_in);
+            initializeRobotPoseFromHRPBody(robot_in, _ee_ikc_map);
             rp_ref_out_old = rp_ref_vel_old = rp_ref_out;
         }
         void update(){//////////  メインループ  ////////////
@@ -558,7 +516,6 @@ class WBMSCore : UTIL_CONST {
 
             overwriteFootZFromFootLandOnCommand (rp_ref_out);
             setFootRotHorizontalIfGoLanding     (rp_ref_out);
-
             for(int i=0, l[2]={rf,lf}; i<2; i++){
                 double fheight = rp_ref_out_old.tgt[l[i]].abs.p(Z) - rp_ref_out_old.tgt[l[i]].offs.p(Z);
                 double horizontal_max_vel = 0 + fheight * 20;
@@ -566,29 +523,22 @@ class WBMSCore : UTIL_CONST {
                 double vertical_max_vel = 0 + fheight * 10;
                 LIMIT_MIN( rp_ref_out.tgt[l[i]].abs.p(Z), rp_ref_out_old.tgt[l[i]].abs.p(Z)-vertical_max_vel*DT);
             }
-
             applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
 
             com_forcp_ref = rp_ref_out.tgt[com].abs.p.head(XY);
-//            Vector3ToVector2(rp_ref_out.tgt[com].abs.p,com_forcp_ref);
             static hrp::Vector2 com_forcp_ref_old;
             com_vel_forcp_ref = (com_forcp_ref - com_forcp_ref_old)/DT;
             com_forcp_ref_old = com_forcp_ref;
-
             applyLPFilter_pre                       (rp_ref_out);
             applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
-            //      surpressGoContactChattering         (rp_ref_out);
-            //      setFootContactPoseByGoContact       (rp_ref_out);
             applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
             applyLPFilter_post                       (rp_ref_out);
-
             r_zmp_raw = rp_ref_out.tgt[zmp].abs.p;
             applyZMPCalcFromCOM                 (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);
             if(DEBUG){
                 fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.tgt[zmp].abs.p(X),rp_ref_out.tgt[zmp].abs.p(Y));
                 fprintf(cz_log,"\n");
             }
-
             //      overwriteFootZFromFootLandOnCommand (rp_ref_out);
             modifyFootRotAndXYForContact        (rp_ref_out);
 
@@ -600,30 +550,6 @@ class WBMSCore : UTIL_CONST {
             is_initial_loop = false;
             gettimeofday(&t_calc_end, NULL);
         }
-        static void Point3DToVector3(const RTC::Point3D& in, hrp::Vector3& out){ out << in.x, in.y, in.z;}
-        static void Oriantation3DToVector3(const RTC::Orientation3D& in, hrp::Vector3& out){ out << in.r, in.p, in.y;}
-        static void Vector3ToPoint3D(const hrp::Vector3& in, RTC::Point3D& out){ out.x = in(X); out.y = in(Y); out.z = in(Z);}
-        static void Vector3ToOriantation3D(const hrp::Vector3& in, RTC::Orientation3D& out){ out.r = in(X); out.p = in(Y); out.y = in(Z);}
-        static void Pose3DToWBMSPose3D(const RTC::Pose3D& in, WBMSPose3D& out){ Point3DToVector3(in.position, out.p); Oriantation3DToVector3(in.orientation, out.rpy); }
-        static void WBMSPose3DToPose3D(const WBMSPose3D& in, RTC::Pose3D& out){ Vector3ToPoint3D(in.p, out.position); Vector3ToOriantation3D(in.rpy, out.orientation); }
-//        static void DoubleSeqToVector6(const RTC::TimedDoubleSeq::_data_seq& in, hrp::dvector6& out){
-//            if(in.length() == 6){out(fx)=in[0]; out(fy)=in[1]; out(fz)=in[2]; out(tz)=in[3]; out(ty)=in[4]; out(tz)=in[5];
-//            }else{ std::cerr<<"[WARN] HumanPose::hrp::dvector6 DoubleSeqTohrp::dvector6() invalid data length"<<std::endl; out = hrp::dvector6::Zero(); }
-//        }
-//        static void Vector6ToDoubleSeq(const hrp::dvector6& in, RTC::TimedDoubleSeq::_data_seq& out){
-//            if(out.length() == 6){out[0]=in(fx); out[1]=in(fy); out[2]=in(fz); out[3]=in(tx); out[4]=in(ty); out[5]=in(tz);
-//            }else{ std::cerr<<"[WARN] HumanPose::hrp::dvector6 DoubleSeqTohrp::dvector6() invalid data length"<<std::endl; }
-//        }
-        static void WrenchToVector6(const OpenHRP::Wrench& in, hrp::dvector6& out){
-          out.head(3) << in.force.x, in.force.y, in.force.z;
-          out.tail(3) << in.torque.x, in.torque.y, in.torque.z;
-        }
-        static void Vector6ToWrench(const hrp::dvector6& in, OpenHRP::Wrench& out){
-            out.force.x = in(0);            out.force.y = in(1);            out.force.z = in(2);
-            out.torque.x = in(3);            out.torque.y = in(4);            out.torque.z = in(5);
-        }
-        static double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(X)*b(Y)-a(Y)*b(X); }
-
 
     private:
         void updateParams(){
@@ -686,7 +612,7 @@ class WBMSCore : UTIL_CONST {
         void calcVelAccSafeTrajectoryVec(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
             if((pos_tgt - pos_cur).norm() > 1e-6){
                 for(int i=0;i<3;i++){
-                    double stop_safe_vel = SGN(pos_tgt(i) - pos_cur(i)) * sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
+                    double stop_safe_vel = sgn(pos_tgt(i) - pos_cur(i)) * sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
                     double vel_ans;
                     if(vel_cur(i) > stop_safe_vel){
                         vel_ans = vel_cur(i) - max_acc * DT;
@@ -715,7 +641,7 @@ class WBMSCore : UTIL_CONST {
             }
         }
         void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
-            hrp::Vector3 direc( SGN(pos_tgt(X)-pos_cur(X)), SGN(pos_tgt(Y)-pos_cur(Y)), SGN(pos_tgt(Z)-pos_cur(Z)));
+            hrp::Vector3 direc( sgn(pos_tgt(X)-pos_cur(X)), sgn(pos_tgt(Y)-pos_cur(Y)), sgn(pos_tgt(Z)-pos_cur(Z)));
             hrp::Vector3 vel_ans = vel_cur + direc * max_acc * DT;
             for(int i=0;i<XYZ;i++){
                 double stop_safe_vel = sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
@@ -729,8 +655,6 @@ class WBMSCore : UTIL_CONST {
         }
         void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
             hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);//TODO implement
-            //      hrp::Vector2 rf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[rf].abs.p(X),old.tgt[rf].abs.p(Y));
-            //      hrp::Vector2 lf2zmp = hrp::Vector2(old.tgt[zmp].abs.p(X),old.tgt[zmp].abs.p(Y)) - hrp::Vector2(old.tgt[lf].abs.p(X),old.tgt[lf].abs.p(Y));
             hrp::Vector2 rf2zmp = act_rs.zmp.head(XY) - old.tgt[rf].abs.p.head(XY);
             hrp::Vector2 lf2zmp = act_rs.zmp.head(XY) - old.tgt[lf].abs.p.head(XY);
             if( inside_vec_rf.dot(rf2zmp) / inside_vec_rf.norm() > wp.auto_swing_foot_landing_threshold ){ out.tgt[lf].go_contact = true; }
@@ -738,7 +662,6 @@ class WBMSCore : UTIL_CONST {
         }
         void limitEEWorkspace(HumanPose& out){
             const double MAX_FW = 0.25;
-            //      const double MAX_FW = 1000000;//manipulability test
             const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
 
             for(int i=0, spl[LR]={rf,lf}, swl[LR]={lf,rf}; i<LR; i++){
@@ -1059,11 +982,11 @@ class WBMSCore : UTIL_CONST {
                 int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
                 hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
                 hrp::Vector2 cur_edge = nxt_pt - cur_pt;
-                double dBunbo = hrpVector2Cross(anchor_vec,cur_edge);
+                double dBunbo = hrp::hrpVector2Cross(anchor_vec,cur_edge);
                 if( dBunbo != 0.0) {//平行の場合を外す
                     hrp::Vector2 vectorAC = hrp::Vector2(hull[i](0),hull[i](1)) - pt_in_start;
-                    double dR = hrpVector2Cross(vectorAC,cur_edge) / dBunbo;
-                    double dS = hrpVector2Cross(vectorAC,anchor_vec) / dBunbo;
+                    double dR = hrp::hrpVector2Cross(vectorAC,cur_edge) / dBunbo;
+                    double dS = hrp::hrpVector2Cross(vectorAC,anchor_vec) / dBunbo;
                     if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){//dRには数値誤差が乗る
                         pt_will_cross = pt_in_start + dR * anchor_vec;
                         //dist = dR * anchor_vec.norm();

From f81fbfda4591e5b1ea73857b0172722a09c2ca8b Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 26 Apr 2019 23:16:27 +0900
Subject: [PATCH 154/217] refactor

---
 .../FullbodyInverseKinematicsSolver.h         |  24 +--
 rtc/AutoBalancer/MyUtils.h                    |  40 +++--
 .../WholeBodyMasterSlave.cpp                  |   4 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 163 +-----------------
 4 files changed, 40 insertions(+), 191 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index e808229adde..53cd0febf1b 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -117,7 +117,7 @@ class FullbodyInverseKinematicsSolver{
         }
         void solveFullbodyIKOnce(const std::vector<IKConstraint>& _ikc_list){
             #ifdef OPENHRP_PACKAGE_VERSION_320
-                // count all constraint DOF and allocate Jacobian and vectors
+                // count all valid constraint DOF and allocate Jacobian and vectors
                 int C_DOF = 0; // constraint DOF
                 for (int i=0;i<_ikc_list.size();i++){ C_DOF += (_ikc_list[i].constraint_weight.array()>0.0).count(); }
                 C_DOF += (q_ref_constraint_weight.array()>0.0).count();
@@ -136,7 +136,6 @@ class FullbodyInverseKinematicsSolver{
                         hrp::Matrix33 tgt_cur_rot = link_tgt_ptr->R * _ikc_list[i].localR;
                         dp_part.head(3) =  _ikc_list[i].targetPos - tgt_cur_pos;
                         dp_part.tail(3) = tgt_cur_rot * hrp::omegaFromRotEx(tgt_cur_rot.transpose() * hrp::rotFromRpy(_ikc_list[i].targetRpy));
-//                        hrp::JointPathEx tgt_jpath(m_robot, m_robot->rootLink(), link_tgt_ptr, m_dt, false, "");
                         hrp::JointPath tgt_jpath(m_robot->rootLink(), link_tgt_ptr);
                         hrp::dmatrix J_jpath;
                         tgt_jpath.calcJacobian(J_jpath, _ikc_list[i].localPos);
@@ -155,6 +154,9 @@ class FullbodyInverseKinematicsSolver{
                     for(int cid=0; cid<WS_DOF; cid++){
                         if(_ikc_list[i].constraint_weight(cid) > 0.0){
                             J_all.row(cur_cid_all) = J_part.row(cid);
+
+                            LIMIT_MINMAX(dp_part(cid), -0.1,0.1);
+
                             err_all(cur_cid_all) = dp_part(cid);
                             constraint_weight_all(cur_cid_all) = _ikc_list[i].constraint_weight(cid);
                             cur_cid_all++;
@@ -248,26 +250,24 @@ class FullbodyInverseKinematicsSolver{
                     if(hrp::rpyFromRot(m_robot->rootLink()->R)(i) > rootlink_rpy_ulimit(i) && dq_all.tail(6).tail(3)(i) > 0) dq_all.tail(6).tail(3)(i) = 0;
                 }
 
-
                 hrp::Matrix33 dR;
                 hrp::Vector3 omega = dq_all.tail(6).tail(3);
                 hrp::calcRodrigues(dR, omega.normalized(), omega.norm());
-                if(!(m_robot->rootLink()->R * dR).isUnitary()){
-                    std::cerr <<"ERROR R_base_ans is not Unitary" << std::endl; return;
+                hrp::Matrix33 R_base_ans = m_robot->rootLink()->R * dR;
+                if(!R_base_ans.isUnitary()){
+                    std::cerr <<"[FullbodyIK] WARN R_base_ans is not Unitary, normalize via Quaternion" << std::endl;
+                    Eigen::Quaternion<double> quat(R_base_ans);
+                    quat.normalize();
+                    R_base_ans = quat.toRotationMatrix();
+                    return;
                 }else{
-                    m_robot->rootLink()->R = m_robot->rootLink()->R * dR;
+                    m_robot->rootLink()->R = R_base_ans;
                 }
                 m_robot->calcForwardKinematics();
             #else
                 std::cerr<<"solveFullbodyIKOnce() needs OPENHRP_PACKAGE_VERSION_320 !!!"<<std::endl;
             #endif
         }
-//        void revertRobotStateToCurrentAll (){
-//            hrp::setQAll(m_robot, qorg);
-//            mm_robot->rootLink()->p = current_root_p;
-//            mm_robot->rootLink()->R = current_root_R;
-//            mm_robot->calcForwardKinematics();
-//        };
 };
 
 #endif //  FULLBODYIK_H
diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index 6e68f4fce75..a8aa1a7b455 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -46,15 +46,20 @@ class WBMSPose3D{
         void clear(){ p = rpy = hrp::Vector3::Zero(); }
 };
 
+
 namespace hrp{
     class Pose3{
         public:
-            Pose3(const hrp::dvector6& in){ data = in; }
-            Pose3(const hrp::Vector3& _p, const hrp::Vector3& _rpy){ data << _p, _rpy; }
-            Pose3(const double& X, const double& Y, const double& Z, const double& r, const double& p, const double& y){ data << X,Y,Z,r,p,y; }
-            hrp::dvector6 data;
-            hrp::dvector6::SegmentReturnType p(){ return data.head(3); }
-            hrp::dvector6::SegmentReturnType rpy(){ return data.tail(3); }
+            hrp::Vector3 p;
+            hrp::Matrix33 R;
+            Pose3(const double& _X, const double& _Y, const double& _Z, const double& _r, const double& _p, const double& _y){ p << _X,_Y,_Z; R = hrp::rotFromRpy(_r,_p,_y); }
+            Pose3(const hrp::dvector6& _xyz_rpy){ p = _xyz_rpy.head(3); R = hrp::rotFromRpy(_xyz_rpy.tail(3)); }
+            Pose3(const hrp::Vector3& _xyz, const hrp::Vector3& _rpy){ p = _xyz; R = hrp::rotFromRpy(_rpy); }
+            Pose3(const hrp::Vector3& _xyz, const hrp::Matrix33& _R){ p = _xyz; R = _R; }
+            hrp::Vector3 rpy() const{ return hrp::rpyFromRot(R); }
+            hrp::dvector6 to_dvector6() const{ return (hrp::dvector6() << p, hrp::rpyFromRot(R)).finished(); }
+//            hrp::dvector6::SegmentReturnType p(){ return data.head(3); }
+//            hrp::dvector6::SegmentReturnType rpy(){ return data.tail(3); }
     };
 
     inline hrp::Vector3         to_Vector3      (const RTC::Point3D& in)        { return hrp::Vector3(in.x, in.y, in.z); }
@@ -67,19 +72,16 @@ namespace hrp{
     inline RTC::Point3D         to_Point3D      (const hrp::Vector3& in)        { return (RTC::Point3D){in(X),in(Y),in(Z)}; }
     inline RTC::Orientation3D   to_Orientation3D(const hrp::Vector3& in)        { return (RTC::Orientation3D){in(X),in(Y),in(Z)}; }
     inline RTC::Pose3D          to_Pose3D       (const hrp::dvector6& in)       { return (RTC::Pose3D){in(X),in(Y),in(Z),in(r),in(p),in(y)}; }
-    inline RTC::Pose3D          to_Pose3D       (const hrp::Pose3& in)          { return (RTC::Pose3D){in.data(X),in.data(Y),in.data(Z),in.data(r),in.data(p),in.data(y)}; }
+    inline RTC::Pose3D          to_Pose3D       (const hrp::Pose3& in)          { return (RTC::Pose3D){in.p(X),in.p(Y),in.p(Z),in.rpy()(r),in.rpy()(p),in.rpy()(y)}; }
     inline OpenHRP::Wrench      to_Wrench       (const hrp::dvector6& in)       { return (OpenHRP::Wrench){in(X),in(Y),in(Z),in(r),in(p),in(y)}; }
     inline RTC::TimedDoubleSeq::_data_seq  to_DoubleSeq(const hrp::dvector& in) { RTC::TimedDoubleSeq::_data_seq out(in.size()); hrp::dvector::Map(out.get_buffer(), in.size()) = in; return out; }
 
-    inline hrp::dvector getQAll         (const hrp::BodyPtr _robot)                     { hrp::dvector tmp(_robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ tmp(i) = _robot->joint(i)->q; } return tmp; }
-    inline void         setQAll         (hrp::BodyPtr _robot, const hrp::dvector& in)   { assert(in.size() <= _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = in(i); } }
+    inline hrp::dvector getQAll         (const hrp::BodyPtr _robot){ hrp::dvector tmp(_robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ tmp(i) = _robot->joint(i)->q; } return tmp; }
+    inline void         setQAll         (hrp::BodyPtr       _robot, const hrp::dvector& in){ assert(in.size() <= _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = in(i); } }
     inline hrp::dvector getRobotStateVec(const hrp::BodyPtr _robot){ return (hrp::dvector(_robot->numJoints()+6) << getQAll(_robot), _robot->rootLink()->p, hrp::rpyFromRot(_robot->rootLink()->R)).finished(); }
-    inline void         setRobotStateVec(hrp::BodyPtr _robot, const hrp::dvector& _q_bpos_brpy){
-                                                                                        assert(_q_bpos_brpy.size() == _robot->numJoints()+6); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q_bpos_brpy(i); }; _robot->rootLink()->p = _q_bpos_brpy.tail(6).head(3); _robot->rootLink()->R = hrp::rotFromRpy(_q_bpos_brpy.tail(6).tail(3)); }
-    inline void         setRobotStateVec(hrp::BodyPtr _robot, const hrp::dvector& _q, const hrp::Vector3& _bpos, const hrp::Vector3& _brpy){
-                                                                                            assert(_q.size() == _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q(i); }; _robot->rootLink()->p = _bpos; _robot->rootLink()->R = hrp::rotFromRpy(_brpy); }
-    inline void         setRobotStateVec(hrp::BodyPtr _robot, const hrp::dvector& _q, const hrp::Vector3& _bpos, const hrp::Matrix33& _bR){
-                                                                                        assert(_q.size() == _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q(i); }; _robot->rootLink()->p = _bpos; _robot->rootLink()->R = _bR; }
+    inline void         setRobotStateVec(hrp::BodyPtr       _robot, const hrp::dvector& _q_bpos_brpy){ assert(_q_bpos_brpy.size() == _robot->numJoints()+6); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q_bpos_brpy(i); }; _robot->rootLink()->p = _q_bpos_brpy.tail(6).head(3); _robot->rootLink()->R = hrp::rotFromRpy(_q_bpos_brpy.tail(6).tail(3)); }
+    inline void         setRobotStateVec(hrp::BodyPtr       _robot, const hrp::dvector& _q, const hrp::Vector3& _bpos, const hrp::Vector3& _brpy){ assert(_q.size() == _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q(i); }; _robot->rootLink()->p = _bpos; _robot->rootLink()->R = hrp::rotFromRpy(_brpy); }
+    inline void         setRobotStateVec(hrp::BodyPtr       _robot, const hrp::dvector& _q, const hrp::Vector3& _bpos, const hrp::Matrix33& _bR){ assert(_q.size() == _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q(i); }; _robot->rootLink()->p = _bpos; _robot->rootLink()->R = _bR; }
 
     inline hrp::Vector3 omegaFromRotEx(const hrp::Matrix33& r) {//copy from JointPathEx.cpp
         using ::std::numeric_limits;
@@ -103,4 +105,12 @@ namespace hrp{
 
 }
 
+
+inline std::ostream& operator<<(std::ostream& os, hrp::Pose3& in){
+    os << "p = " << in.p.transpose() << std::endl;
+    os << "R =\n" << in.R << std::endl;
+    os << "(rpy) = " << hrp::rpyFromRot(in.R).transpose() << std::endl;
+    return os;
+}
+
 #endif //  MYUTILS_H
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 07b6504cc88..514121d11d3 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -513,7 +513,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const WBMSPose3D& com_ref, const WBMS
             tmp.targetPos = rh_ref.p;
         }
         tmp.targetRpy = rh_ref.rpy;
-        tmp.constraint_weight = hrp::dvector6::Constant(0.5);
+        tmp.constraint_weight = hrp::dvector6::Constant(0.1);
         tmp.pos_precision = 3e-3;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
@@ -528,7 +528,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const WBMSPose3D& com_ref, const WBMS
             tmp.targetPos = lh_ref.p;
         }
         tmp.targetRpy = lh_ref.rpy;
-        tmp.constraint_weight = hrp::dvector6::Constant(0.5);
+        tmp.constraint_weight = hrp::dvector6::Constant(0.1);
         tmp.pos_precision = 3e-3;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index a21b54f7903..1690a3e1e2b 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -10,23 +10,6 @@
 #include <opencv2/core/core.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/imgproc/types_c.h>
-// geometory
-#if defined(__cplusplus)
-extern "C" {
-#endif
-#include <stdio.h>
-#include <stdlib.h>
-#include <qhull/qhull.h>
-#include <qhull/mem.h>
-#include <qhull/qset.h>
-#include <qhull/geom.h>
-#include <qhull/merge.h>
-#include <qhull/poly.h>
-#include <qhull/io.h>
-#include <qhull/stat.h>
-#if defined(__cplusplus)
-}
-#endif
 
 #define DEBUG 0
 
@@ -89,15 +72,6 @@ namespace hrp{
                 r = r_in;
             };
     };
-//    class Wrench{
-//        public:
-//            hrp::Vector3 f, t;
-//            Wrench(const hrp::Vector3 f_in = hrp::Vector3::Zero(), const hrp::Vector3 t_in = hrp::Vector3::Zero()){
-//              f = f_in;
-//              t = t_in;
-//            }
-//            Wrench operator+(const Wrench& a, const Wrench& b){ return Wrench(a.f+b.f, a.t+b.t); }
-//    };
 }
 
 
@@ -368,13 +342,6 @@ class WBMSCore{
                 hrp::Vector3 com, zmp;
         }act_rs;
 
-        double cur_manip_val[4][3];
-        hrp::Vector3 manip_direc[4][3];
-        hrp::Matrix33 manip_mat[4];
-        hrp::Vector3 manip_sv[4];
-        hrp::Matrix33 manip_mat_rot[4];
-        hrp::Vector3 manip_sv_rot[4];
-
         hrp::Vector2 cp_acc_old;
 
         WBMSCore(const double& dt){
@@ -529,7 +496,6 @@ class WBMSCore{
             static hrp::Vector2 com_forcp_ref_old;
             com_vel_forcp_ref = (com_forcp_ref - com_forcp_ref_old)/DT;
             com_forcp_ref_old = com_forcp_ref;
-            applyLPFilter_pre                       (rp_ref_out);
             applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
             applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
             applyLPFilter_post                       (rp_ref_out);
@@ -584,19 +550,6 @@ class WBMSCore{
                 else if (!in.tgt[l[i]].is_contact && in.tgt[l[i]].w(fz)>CNT_F_TH+30){in.tgt[l[i]].is_contact = true;}//足浮いた状態から下げる
             }
         }
-        void applyLPFilter_pre(HumanPose& tgt){
-            const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"};
-            for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
-                //        calcVelAccSafeTrajectoryVec(fik_act->getEndEffectorPos(robot_l_names[i]), (fik_act->getEndEffectorPos(robot_l_names[i]) - ee_pose_old[i].p)/DT, tgt.tgt[l[i]].abs.p, 3.0, 1.0, tgt.tgt[l[i]].abs.p);//何故かダメ
-                //        calcVelAccSafeTrajectoryVec(hrp::rpyFromRot(fik_act->getEndEffectorRot(robot_l_names[i])), (hrp::rpyFromRot(fik_act->getEndEffectorRot(robot_l_names[i])) - ee_pose_old[i].rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3.0, 1.0, tgt.tgt[l[i]].abs.rpy);
-                //        calcVelAccSafeTrajectoryVec(fik_act->getEndEffectorPos(robot_l_names[i]), (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 3.0, 1.0, tgt.tgt[l[i]].abs.p);
-                //        calcVelAccSafeTrajectoryVec(hrp::rpyFromRot(fik_act->getEndEffectorRot(robot_l_names[i])), (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 3.0, 1.0, tgt.tgt[l[i]].abs.rpy);
-            }
-            for(int i=0, l[1]={head}; i<1; i++){
-                //        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.p, (rp_ref_out.tgt[l[i]].abs.p - rp_ref_out_old.tgt[l[i]].abs.p)/DT, tgt.tgt[l[i]].abs.p, 2.0, 1.0, tgt.tgt[l[i]].abs.p);
-                //        calcVelAccSafeTrajectoryVec(rp_ref_out_old.tgt[l[i]].abs.rpy, (rp_ref_out.tgt[l[i]].abs.rpy - rp_ref_out_old.tgt[l[i]].abs.rpy)/DT, tgt.tgt[l[i]].abs.rpy, 2.0, 1.0, tgt.tgt[l[i]].abs.rpy);
-            }
-        }
         void applyLPFilter_post(HumanPose& tgt){
             if(is_initial_loop){
                 for(int i=0, l[1]={com}; i<1; i++){
@@ -609,50 +562,6 @@ class WBMSCore{
                 tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
             }
         }
-        void calcVelAccSafeTrajectoryVec(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
-            if((pos_tgt - pos_cur).norm() > 1e-6){
-                for(int i=0;i<3;i++){
-                    double stop_safe_vel = sgn(pos_tgt(i) - pos_cur(i)) * sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
-                    double vel_ans;
-                    if(vel_cur(i) > stop_safe_vel){
-                        vel_ans = vel_cur(i) - max_acc * DT;
-                    }else{
-                        vel_ans = vel_cur(i) + max_acc * DT;
-                    }
-                    pos_ans(i) = pos_cur(i) + vel_ans * DT;
-                }
-            }else{
-                pos_ans = pos_tgt;
-            }
-        }
-        void calcVelAccSafeTrajectoryVecML(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& acc_base, const hrp::Matrix33& max_acc_mat, const hrp::Vector3& max_acc_sv, hrp::Vector3& pos_ans){
-            if((pos_tgt - pos_cur).norm() > 1e-6){
-                hrp::Vector3 direc = (pos_tgt - pos_cur).normalized();
-                hrp::Vector3 ref_acc = direc*acc_base;
-                hrp::Vector3 mod_acc = max_acc_mat * max_acc_sv.asDiagonal() * max_acc_mat.transpose() * ref_acc;
-                double stop_safe_vel_scalar = sqrt( 2 * fabs(mod_acc.dot(direc)) * (pos_tgt - pos_cur).norm() );
-                hrp::Vector3 vel_ans = vel_cur + mod_acc * DT;
-                if(vel_ans.dot(direc) > stop_safe_vel_scalar){
-                    vel_ans = vel_ans * stop_safe_vel_scalar / vel_ans.dot(direc);
-                }
-                pos_ans = pos_cur + vel_ans * DT;
-            }else{
-                pos_ans = pos_tgt;
-            }
-        }
-        void calcVelAccSafeTrajectory(const hrp::Vector3& pos_cur, const hrp::Vector3& vel_cur, const hrp::Vector3& pos_tgt, const double& max_acc, const double& max_vel, hrp::Vector3& pos_ans){
-            hrp::Vector3 direc( sgn(pos_tgt(X)-pos_cur(X)), sgn(pos_tgt(Y)-pos_cur(Y)), sgn(pos_tgt(Z)-pos_cur(Z)));
-            hrp::Vector3 vel_ans = vel_cur + direc * max_acc * DT;
-            for(int i=0;i<XYZ;i++){
-                double stop_safe_vel = sqrt( 2 * max_acc * fabs(pos_tgt(i) - pos_cur(i)) );
-                if( direc(i) > 0 ){
-                    if(vel_ans(i) > stop_safe_vel){ vel_ans(i) = stop_safe_vel; }
-                }else{
-                    if(vel_ans(i) < - stop_safe_vel){ vel_ans(i) = - stop_safe_vel; }
-                }
-            }
-            pos_ans = pos_cur + vel_ans * DT;
-        }
         void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
             hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);//TODO implement
             hrp::Vector2 rf2zmp = act_rs.zmp.head(XY) - old.tgt[rf].abs.p.head(XY);
@@ -866,66 +775,7 @@ class WBMSCore{
             }
             com_vel_ans_old = com_vel_ans;
         }
-        //    void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull_d, const std::vector<hrp::Vector2>& hull_a, hrp::Vector2& com_vel_ans){
-        //      hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
-        //      com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
-        //      //減速CP条件(現在のCPを常に両足裏で頭打ち)
-        //      hrp::Vector2 cp_dec_tmp = com_pos + com_vel * sqrt( H_cur / G );
-        //      hrp::Vector2 cp_dec_ragulated;
-        //      if(!isPointInHullOpenCV(cp_dec_tmp,hull_d)){
-        //        calcCrossPointOnHull(com_pos, cp_dec_tmp, hull_d, cp_dec_ragulated);
-        //        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
-        //      }
-        //
-        //      //減速CP条件2(指令のCPに対する現在発揮可能なCP)
-        ////      hrp::Vector2 cp_dec_ref = com_forcp_ref + com_vel_forcp_ref * sqrt( H_cur / G );
-        ////      hrp::Vector2 cp_dec_ref_ragulated = cp_dec_ref;
-        ////      if(!isPointInHullOpenCV(cp_dec_ref,hull_d)){
-        ////        calcCrossPointOnHull(com_pos, cp_dec_ref, hull_d, cp_dec_ref_ragulated);
-        ////      }
-        ////      com_vel_decel_ok = (cp_dec_ref_ragulated - com_pos) / sqrt( H_cur / G );
-        //
-        //      //減速CP条件2(指令のCOMに収束する現在発揮可能なCP)
-        ////      hrp::Vector2 cp_dec_ref = com_forcp_ref;
-        ////      hrp::Vector2 cp_dec_ref_ragulated = cp_dec_ref;
-        ////      if(!isPointInHullOpenCV(cp_dec_ref,hull_d)){
-        ////        calcCrossPointOnHull(com_pos, cp_dec_ref, hull_d, cp_dec_ref_ragulated);
-        ////      }
-        ////      hrp::Vector2 vel_max_decel_ok = (cp_dec_ref_ragulated - com_pos) / sqrt( H_cur / G );
-        ////      if( com_vel.dot(com_vel) > vel_max_decel_ok.dot(com_vel)){
-        ////        com_vel_decel_ok = vel_max_decel_ok;
-        ////      }else{
-        ////        com_vel_decel_ok = com_vel;
-        ////      }
-        //
-        //      //加速CP条件(ACP使用)
-        //      hrp::Vector2 cp_acc_tmp = com_pos - com_vel * sqrt( H_cur / G );
-        //      hrp::Vector2 cp_acc_ragulated;
-        //      if(!isPointInHullOpenCV(cp_acc_tmp,hull_a)){
-        //         calcCrossPointOnHull(com_pos, cp_acc_tmp, hull_a, cp_acc_ragulated);
-        //         com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
-        //      }
-        //      cp_acc_old = cp_acc_ragulated;
-        //
-        //      //加速CP条件2(ZMP使用)
-        //      static hrp::Vector2 com_vel_ans_old = com_vel;
-        ////      hrp::Vector2 com_acc = (com_vel - com_vel_ans_old) / DT;
-        ////      hrp::Vector2 zmp_in = com_pos - com_acc / G * H_cur;
-        ////      hrp::Vector2 zmp_regulated;
-        ////      if(!isPointInHullOpenCV(zmp_in,hull_a)){
-        ////        calcCrossPointOnHull(com_pos, zmp_in, hull_a, zmp_regulated);
-        ////        hrp::Vector2 com_acc_regulated = (com_pos - zmp_regulated)*G/H_cur;
-        ////        com_vel_accel_ok = com_vel_ans_old + com_acc_regulated*DT;
-        ////      }
-        //
-        //      //加速減速条件マージ
-        //      if( com_vel_decel_ok.dot(com_vel) < com_vel_accel_ok.dot(com_vel)){//normじゃダメ？
-        //        com_vel_ans = com_vel_decel_ok;
-        //      }else{
-        //        com_vel_ans = com_vel_accel_ok;
-        //      }
-        //      com_vel_ans_old = com_vel_ans;
-        //    }
+        //    void regulateCOMVelocityB
         void surpressGoContactChattering(HumanPose& out){
             //      static std::deque<int> buffer[LR];
             static int count[LR];
@@ -1032,17 +882,6 @@ class WBMSCore{
             cv::convexHull(cv::Mat(points),cvhull,true);
             for(int i=0;i<cvhull.size();i++){ hull_ans.push_back( hrp::Vector2( cvhull[i].x, cvhull[i].y ) );  }
         }
-        //void makeConvexHullQHull(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
-        //  const int dim = 2;
-        //  double points[dim*pts.size()];
-        //  for(int i=0;i<pts.size();i++){ points[dim*i] = pts[i](0); points[dim*i+1] = pts[i](1); }
-        //  qh_new_qhull( dim, pts.size(), points, false, "qhull ", NULL, stderr);
-        //  pointT *point, *pointtemp;
-        //  hull_ans.clear();
-        //  FORALLpoints hull_ans.push_back( hrp::Vector2(point[0],point[1]) );
-        //  qh_freeqhull(!qh_ALL);
-        //  //#define qh_ORIENTclock 1
-        //}
         bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull){
             cvhull.clear();
             for(int i=0;i<hull.size();i++){ cvhull.push_back( cv::Point2f( hull[i](0), hull[i](1)) ); }

From a80858e77fbbd8eb6179187ffaf30a9ccd7bcf33 Mon Sep 17 00:00:00 2001
From: Yasuhiro Ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 27 Apr 2019 18:53:51 +0900
Subject: [PATCH 155/217] Update WholeBodyMasterSlave.cpp

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 514121d11d3..bbd14cdcba0 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -708,7 +708,7 @@ void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr _robot, hrp::Body
 
 
     hrp::dvector estimated_times_from_vel_limit = (hrp::getRobotStateVec(_robot) - hrp::getRobotStateVec(_robot_safe)).array().abs() / avg_q_vel.array();
-    hrp::dvector estimated_times_from_acc_limit = ans_state_vel.array() / avg_q_acc.array();
+    hrp::dvector estimated_times_from_acc_limit = ans_state_vel.array().abs() / avg_q_acc.array();
     double longest_estimated_time_from_vel_limit = estimated_times_from_vel_limit.maxCoeff();
     double longest_estimated_time_from_acc_limit = estimated_times_from_acc_limit.maxCoeff();
     goal_time = hrp::Vector3(longest_estimated_time_from_vel_limit, longest_estimated_time_from_acc_limit, min_goal_time_offset).maxCoeff();

From 55447574edc3483cd523235238d9493dde127b1b Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 28 Apr 2019 19:11:33 +0900
Subject: [PATCH 156/217] refactor

---
 .../FullbodyInverseKinematicsSolver.h         |  69 ++++++++--
 rtc/AutoBalancer/MyUtils.h                    |  29 ++---
 .../WholeBodyMasterSlave.cpp                  |  43 ++++---
 .../WholeBodyMasterSlave.h                    |   2 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 120 ++++++++----------
 5 files changed, 142 insertions(+), 121 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 53cd0febf1b..0287686f85d 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -35,7 +35,7 @@ class IKConstraint {
 
 class FullbodyInverseKinematicsSolver{
     protected:
-        hrp::dmatrix J_all, J_all_inv, J_com, J_am;
+        hrp::dmatrix J_all, J_com, J_am;
         hrp::dvector dq_all, err_all, constraint_weight_all, jlim_avoid_weight_old;
         const int WS_DOF, BASE_DOF, J_DOF, ALL_DOF;
         std::vector<IIRFilter> am_filters;
@@ -117,7 +117,7 @@ class FullbodyInverseKinematicsSolver{
         }
         void solveFullbodyIKOnce(const std::vector<IKConstraint>& _ikc_list){
             #ifdef OPENHRP_PACKAGE_VERSION_320
-                // count all valid constraint DOF and allocate Jacobian and vectors
+                // count up all valid constraint DOF and allocate Jacobian and vectors
                 int C_DOF = 0; // constraint DOF
                 for (int i=0;i<_ikc_list.size();i++){ C_DOF += (_ikc_list[i].constraint_weight.array()>0.0).count(); }
                 C_DOF += (q_ref_constraint_weight.array()>0.0).count();
@@ -155,7 +155,7 @@ class FullbodyInverseKinematicsSolver{
                         if(_ikc_list[i].constraint_weight(cid) > 0.0){
                             J_all.row(cur_cid_all) = J_part.row(cid);
 
-                            LIMIT_MINMAX(dp_part(cid), -0.1,0.1);
+LIMIT_MINMAX(dp_part(cid), -0.1,0.1);
 
                             err_all(cur_cid_all) = dp_part(cid);
                             constraint_weight_all(cur_cid_all) = _ikc_list[i].constraint_weight(cid);
@@ -200,12 +200,39 @@ class FullbodyInverseKinematicsSolver{
                     }
                     jlim_avoid_weight_old(j) = jlim_avoid_weight;
                 }
+
                 hrp::dvector dq_weight_all_final = dq_weight_all.array() * dq_weight_all_jlim.array();
 
+
+
+
+
+                hrp::ivector selection_vec = hrp::ivector::Zero(ALL_DOF);
+                selection_vec.head(31).fill(1);
+                selection_vec(6) = 0;
+                selection_vec(7) = 0;
+                selection_vec.tail(6).fill(1);
+
+                const int USE_Q_NUM = (selection_vec.array() == 1).count();
+                hrp::dmatrix selection_mat = hrp::to_SelectionMat(selection_vec);
+
+
+
+
+
+
                 // Solvability-unconcerned Inverse Kinematics by Levenberg-Marquardt Method [sugihara:RSJ2009]
                 const double wn_const = 1e-6;
                 hrp::dmatrix Wn = (static_cast<double>(err_all.transpose() * constraint_weight_all.asDiagonal() * err_all) + wn_const) * hrp::dmatrix::Identity(ALL_DOF, ALL_DOF);
                 Wn = dq_weight_all_final.asDiagonal() * Wn;
+
+
+
+//                dq_weight_all_final = selection_mat * dq_weight_all_final;
+                J_all = J_all * selection_mat.transpose();
+                Wn = selection_mat * Wn * selection_mat.transpose();
+
+
                 hrp::dmatrix H = J_all.transpose() * constraint_weight_all.asDiagonal() * J_all + Wn;
                 hrp::dvector g = J_all.transpose() * constraint_weight_all.asDiagonal() * err_all;
                 dq_all = H.ldlt().solve(g); // dq_all = H.inverse() * g; is slow
@@ -216,30 +243,44 @@ class FullbodyInverseKinematicsSolver{
                 if(count++ % 10000 == 0){
 //                if(true){
                     std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
-                    std::cout<<std::setprecision(2) << "J_all_inv=\n"<<J_all_inv<<std::setprecision(6)<<std::endl;
-//                    dbgn(selection_mat);
+                    dbg(J_all.rows());
+                    dbg(J_all.cols());
+                    dbgn(selection_mat);
+                    dbg(selection_mat.rows());
+                    dbg(selection_mat.cols());
                     dbgn(H);
+                    dbg(H.rows());
+                    dbg(H.cols());
                     dbgn(Wn);
+                    dbg(Wn.rows());
+                    dbg(Wn.cols());
                     dbg(g.transpose());
+                    dbg(g.rows());
+                    dbg(g.cols());
                     dbg(err_all.transpose());
+                    dbg(err_all.rows());
+                    dbg(err_all.cols());
                     dbg(dq_all.transpose());
+                    dbg(dq_all.rows());
+                    dbg(dq_all.cols());
                     dbg(constraint_weight_all.transpose());
-//                    dbg(dq_weight_all_inv.transpose());
-                    std::cout<<"q_ans_all\n";
-                    for(int i=0;i<m_robot->numJoints();i++)std::cerr<<m_robot->joint(i)->q<<" ";
-
-                    for(int i=0;i<_ikc_list.size();i++){
-                        dbg(_ikc_list[i].target_link_name);
-                        dbg(_ikc_list[i].targetPos.transpose());
-                        dbg(_ikc_list[i].targetRpy.transpose());
-                    }
+                    dbg(constraint_weight_all.rows());
+                    dbg(constraint_weight_all.cols());
+                    dbg(hrp::getRobotStateVec(m_robot).transpose());
                     std::cout<<std::endl;
                 }
 
                 // update joint angles
                 for(int i=0;i<dq_all.rows();i++){ if( isnan(dq_all(i)) || isinf(dq_all(i)) ){ std::cerr <<"[FullbodyIK] ERROR nan/inf is found" << std::endl; return;} }
+
+                dq_all = selection_mat.transpose() * dq_all;
+//                dbg(dq_all.transpose());
+//                dbg(dq_all.rows());
+//                dbg(dq_all.cols());
+
                 for(int i=0;i<J_DOF;i++){
                     m_robot->joint(i)->q += dq_all(i);
+//                    m_robot->joint(i)->q += (selection_mat.transpose() * dq_all)(i);
                     LIMIT_MINMAX(m_robot->joint(i)->q, m_robot->joint(i)->llimit, m_robot->joint(i)->ulimit);
                 }
 
diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index a8aa1a7b455..6cf13998448 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -38,28 +38,21 @@ static const double Q_BUTTERWORTH = 0.707106781;
 //inline void LIMIT_MINMAX(double& x, const double& min, const double& max){ x = (x<min  ? min : (x<max ? x : max));}
 
 
-class WBMSPose3D{
-    public:
-        hrp::Vector3 p,rpy;
-        WBMSPose3D(){ clear(); }
-        ~WBMSPose3D(){}
-        void clear(){ p = rpy = hrp::Vector3::Zero(); }
-};
-
-
 namespace hrp{
     class Pose3{
         public:
             hrp::Vector3 p;
             hrp::Matrix33 R;
+            Pose3(){}
             Pose3(const double& _X, const double& _Y, const double& _Z, const double& _r, const double& _p, const double& _y){ p << _X,_Y,_Z; R = hrp::rotFromRpy(_r,_p,_y); }
             Pose3(const hrp::dvector6& _xyz_rpy){ p = _xyz_rpy.head(3); R = hrp::rotFromRpy(_xyz_rpy.tail(3)); }
             Pose3(const hrp::Vector3& _xyz, const hrp::Vector3& _rpy){ p = _xyz; R = hrp::rotFromRpy(_rpy); }
             Pose3(const hrp::Vector3& _xyz, const hrp::Matrix33& _R){ p = _xyz; R = _R; }
             hrp::Vector3 rpy() const{ return hrp::rpyFromRot(R); }
+            void setRpy(const double& _r, const double& _p, const double& _y) { R = rotFromRpy(_r,_p,_y); }
+            void setRpy(const hrp::Vector3& _rpy) { R = rotFromRpy(_rpy); }
             hrp::dvector6 to_dvector6() const{ return (hrp::dvector6() << p, hrp::rpyFromRot(R)).finished(); }
-//            hrp::dvector6::SegmentReturnType p(){ return data.head(3); }
-//            hrp::dvector6::SegmentReturnType rpy(){ return data.tail(3); }
+            void reset(){ p.fill(0); R.Identity(); }
     };
 
     inline hrp::Vector3         to_Vector3      (const RTC::Point3D& in)        { return hrp::Vector3(in.x, in.y, in.z); }
@@ -98,11 +91,17 @@ namespace hrp{
             return hrp::Vector3( (r(1,2) - r(2,1)) * k, (r(2,0) - r(0,2)) * k, (r(0,1) - r(1,0)) * k );
         }
     }
-
-    inline void Pose3DToWBMSPose3D(const RTC::Pose3D& in, WBMSPose3D& out){ out.p = hrp::to_Vector3(in.position); out.rpy = hrp::to_Vector3(in.orientation); }
-    inline void WBMSPose3DToPose3D(const WBMSPose3D& in, RTC::Pose3D& out){ hrp::to_Point3D(in.p); hrp::to_Orientation3D(in.rpy); }
     inline double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(X)*b(Y)-a(Y)*b(X); }
-
+    inline hrp::dmatrix to_SelectionMat(const hrp::dvector& in) {
+        hrp::dmatrix ret( (in.array() == 1).count(), in.size() );
+        for (int row=0, col=0; col< in.size(); col++){
+            if(in(col) > 0.0){
+                ret(row, col) = 1;
+                row++;
+            }
+        }
+        return ret;
+    }
 }
 
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index bbd14cdcba0..0d942e44804 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -221,15 +221,15 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if (m_optionalDataIn.isNew()) { m_optionalDataIn.read(); }
 
     if( mode.now() != MODE_PAUSE ){ // stop updating input when MODE_PAUSE
-        if (m_htrfwIn.isNew()){ m_htrfwIn.read(); wbms->hp_wld_raw.tgt[rf].w = hrp::to_dvector6(m_htrfw.data); }
-        if (m_htlfwIn.isNew()){ m_htlfwIn.read(); wbms->hp_wld_raw.tgt[lf].w = hrp::to_dvector6(m_htlfw.data); }
-        if (m_htcomIn.isNew()){ m_htcomIn.read(); hrp::Pose3DToWBMSPose3D(m_htcom.data,wbms->hp_wld_raw.tgt[com].abs); }
-        if (m_htrfIn.isNew()) { m_htrfIn.read();  hrp::Pose3DToWBMSPose3D(m_htrf.data,wbms->hp_wld_raw.tgt[rf].abs); }
-        if (m_htlfIn.isNew()) { m_htlfIn.read();  hrp::Pose3DToWBMSPose3D(m_htlf.data,wbms->hp_wld_raw.tgt[lf].abs); }
-        if (m_htrhIn.isNew()) { m_htrhIn.read();  hrp::Pose3DToWBMSPose3D(m_htrh.data,wbms->hp_wld_raw.tgt[rh].abs);}
-        if (m_htlhIn.isNew()) { m_htlhIn.read();  hrp::Pose3DToWBMSPose3D(m_htlh.data,wbms->hp_wld_raw.tgt[lh].abs);}
-        if (m_htheadIn.isNew()){ m_htheadIn.read(); hrp::Pose3DToWBMSPose3D(m_hthead.data,wbms->hp_wld_raw.tgt[head].abs);}
-        if (m_htzmpIn.isNew()){ m_htzmpIn.read(); wbms->hp_wld_raw.tgt[zmp].abs.p = hrp::to_Vector3(m_htzmp.data); }
+        if (m_htrfwIn.isNew())  { m_htrfwIn.read(); wbms->hp_wld_raw.tgt[rf].w      = hrp::to_dvector6(m_htrfw.data); }
+        if (m_htlfwIn.isNew())  { m_htlfwIn.read(); wbms->hp_wld_raw.tgt[lf].w      = hrp::to_dvector6(m_htlfw.data); }
+        if (m_htcomIn.isNew())  { m_htcomIn.read(); wbms->hp_wld_raw.tgt[com].abs   = hrp::to_Pose3(m_htcom.data); }
+        if (m_htrfIn.isNew())   { m_htrfIn.read();  wbms->hp_wld_raw.tgt[rf].abs    = hrp::to_Pose3(m_htrf.data); }
+        if (m_htlfIn.isNew())   { m_htlfIn.read();  wbms->hp_wld_raw.tgt[lf].abs    = hrp::to_Pose3(m_htlf.data); }
+        if (m_htrhIn.isNew())   { m_htrhIn.read();  wbms->hp_wld_raw.tgt[rh].abs    = hrp::to_Pose3(m_htrh.data);}
+        if (m_htlhIn.isNew())   { m_htlhIn.read();  wbms->hp_wld_raw.tgt[lh].abs    = hrp::to_Pose3(m_htlh.data);}
+        if (m_htheadIn.isNew()) { m_htheadIn.read();wbms->hp_wld_raw.tgt[head].abs  = hrp::to_Pose3(m_hthead.data);}
+        if (m_htzmpIn.isNew())  { m_htzmpIn.read(); wbms->hp_wld_raw.tgt[zmp].abs.p = hrp::to_Vector3(m_htzmp.data); }
     }
 
     //khi
@@ -317,7 +317,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
             m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = wbms->rp_ref_out.tgt[lf].is_contact;
         }
         wbms->baselinkpose.p = fik->m_robot->rootLink()->p;
-        wbms->baselinkpose.rpy = hrp::rpyFromRot(fik->m_robot->rootLink()->R);
+        wbms->baselinkpose.R = fik->m_robot->rootLink()->R;
     }
 
     // write
@@ -449,7 +449,7 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(){
 }
 
 
-void WholeBodyMasterSlave::solveFullbodyIK(const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref){
+void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp::Pose3& rf_ref, const hrp::Pose3& lf_ref, const hrp::Pose3& rh_ref, const hrp::Pose3& lh_ref, const hrp::Pose3& head_ref){
     std::vector<IKConstraint> ikc_list;
     if(wbms->wp.use_lower){
         IKConstraint tmp;
@@ -457,7 +457,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const WBMSPose3D& com_ref, const WBMS
         tmp.localPos = hrp::Vector3::Zero();
         tmp.localR = hrp::Matrix33::Identity();
         tmp.targetPos = hrp::to_Vector3(m_basePos.data);// will be ignored by selection_vec
-        tmp.targetRpy = hrp::to_Vector3(m_baseRpy.data);// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
+        tmp.targetRpy = com_ref.rpy();// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
         tmp.constraint_weight << 0,0,0,1,1,1;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
@@ -468,7 +468,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const WBMSPose3D& com_ref, const WBMS
       tmp.localPos = hrp::Vector3::Zero();
       tmp.localR = hrp::Matrix33::Identity();
       tmp.targetPos = hrp::to_Vector3(m_basePos.data);// will be ignored by selection_vec
-      tmp.targetRpy = hrp::to_Vector3(m_baseRpy.data);// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
+      tmp.targetRpy = com_ref.rpy();// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
       tmp.constraint_weight << 1,1,1,1,1,1;
       tmp.rot_precision = deg2rad(3);
       ikc_list.push_back(tmp);
@@ -479,7 +479,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const WBMSPose3D& com_ref, const WBMS
         tmp.localPos = ee_ikc_map["rleg"].localPos;
         tmp.localR = ee_ikc_map["rleg"].localR;
         tmp.targetPos = rf_ref.p;
-        tmp.targetRpy = rf_ref.rpy;
+        tmp.targetRpy = rf_ref.rpy();
         if(wbms->rp_ref_out.tgt[rf].is_contact){
             tmp.constraint_weight = hrp::dvector6::Constant(10);
         }else{
@@ -493,7 +493,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const WBMSPose3D& com_ref, const WBMS
         tmp.localPos = ee_ikc_map["lleg"].localPos;
         tmp.localR = ee_ikc_map["lleg"].localR;
         tmp.targetPos = lf_ref.p;
-        tmp.targetRpy = lf_ref.rpy;
+        tmp.targetRpy = lf_ref.rpy();
         if(wbms->rp_ref_out.tgt[lf].is_contact){
             tmp.constraint_weight = hrp::dvector6::Constant(10);
         }else{
@@ -512,7 +512,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const WBMSPose3D& com_ref, const WBMS
         }else{
             tmp.targetPos = rh_ref.p;
         }
-        tmp.targetRpy = rh_ref.rpy;
+        tmp.targetRpy = rh_ref.rpy();
         tmp.constraint_weight = hrp::dvector6::Constant(0.1);
         tmp.pos_precision = 3e-3;
         tmp.rot_precision = deg2rad(3);
@@ -527,15 +527,16 @@ void WholeBodyMasterSlave::solveFullbodyIK(const WBMSPose3D& com_ref, const WBMS
         }else{
             tmp.targetPos = lh_ref.p;
         }
-        tmp.targetRpy = lh_ref.rpy;
+        tmp.targetRpy = lh_ref.rpy();
         tmp.constraint_weight = hrp::dvector6::Constant(0.1);
         tmp.pos_precision = 3e-3;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
-    }if(fik->m_robot->link("HEAD_JOINT1") != NULL){
+    }
+    if(fik->m_robot->link("HEAD_JOINT1") != NULL){
         IKConstraint tmp;
         tmp.target_link_name = "HEAD_JOINT1";
-        tmp.targetRpy = head_ref.rpy;
+        tmp.targetRpy = head_ref.rpy();
         tmp.constraint_weight << 0,0,0,0,0.1,0.1;
         tmp.rot_precision = deg2rad(1);
         ikc_list.push_back(tmp);
@@ -543,7 +544,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const WBMSPose3D& com_ref, const WBMS
     // if(robot_in->link("HEAD_P") != NULL){
     //     IKConstraint tmp;
     //     tmp.target_link_name = "HEAD_P";
-    //     tmp.targetRpy = head_ref.rpy;
+    //     tmp.targetRpy = head_ref.rpy();
     //     tmp.constraint_weight << 0,0,0,0,0.1,0.1;
     //     tmp.rot_precision = deg2rad(1);
     //     ikc_list.push_back(tmp);
@@ -612,7 +613,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const WBMSPose3D& com_ref, const WBMS
 //        tmp.constraint_weight << 10,10,1,1e-6,1e-6,1e-6;
         tmp.constraint_weight << 10,10,0.001,0,0,0;
 //        if(fik_in->cur_momentum_around_COM.norm() > 1e9){
-//            tmp.constraint_weight << 10,10,1,1e-5,1e-5,1e-10;
+//            tmp.constraint_weight << 10,10,1,1e-5,1e-5,0;
 //            tmp.rot_precision = 100;//angular momentum precision
 //        }else{
 //            tmp.constraint_weight << 10,10,1,0,0,0;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 4c58d794451..f501a281e2f 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -176,7 +176,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         ControlMode mode;
 
         RTC::ReturnCode_t setupEEIKConstraintFromConf(std::map<std::string, IKConstraint>& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop);
-        void solveFullbodyIK(const WBMSPose3D& com_ref, const WBMSPose3D& rf_ref, const WBMSPose3D& lf_ref, const WBMSPose3D& rh_ref, const WBMSPose3D& lh_ref, const WBMSPose3D& head_ref);
+        void solveFullbodyIK(const hrp::Pose3& com_ref, const hrp::Pose3& rf_ref, const hrp::Pose3& lf_ref, const hrp::Pose3& rh_ref, const hrp::Pose3& lh_ref, const hrp::Pose3& head_ref);
         void processTransition();
         void preProcessForWholeBodyMasterSlave();
         void processWholeBodyMasterSlave(const HumanPose& pose_ref);
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 1690a3e1e2b..2dec64ae9ef 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -36,12 +36,12 @@ class BiquadIIRFilterVec{
 
 class PoseTGT{
     public:
-        WBMSPose3D abs, offs, cnt;
+        hrp::Pose3 abs, offs, cnt;
         hrp::dvector6 w;
         bool is_contact, go_contact;
-        PoseTGT(){ clear(); }
+        PoseTGT(){ reset(); }
         ~PoseTGT(){}
-        void clear(){ abs.clear(); offs.clear(); cnt.clear(); w=hrp::dvector6::Zero(); is_contact = go_contact = false;}
+        void reset(){ abs.reset(); offs.reset(); cnt.reset(); w.fill(0); is_contact = go_contact = false;}
 };
 
 class HumanPose{
@@ -49,13 +49,12 @@ class HumanPose{
         std::vector<PoseTGT> tgt;
         HumanPose(){ tgt.resize(num_pose_tgt); }
         ~HumanPose(){cerr<<"HumanPose destructed"<<endl;}
-        void clear(){
-            for(std::vector<PoseTGT>::iterator it = tgt.begin(); it != tgt.end(); it++){ it->clear(); }
+        void reset(){
+            for(std::vector<PoseTGT>::iterator it = tgt.begin(); it != tgt.end(); it++){ it->reset(); }
         }
         static void hp_printf(const HumanPose& in) {
             const std::string pcatgt[] = {"c","rf","lf","rh","lh","hd","z"}, wcatgt[] = {"rw","lw"};
             for(int i=0;i<num_pose_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",pcatgt[i].c_str(),in.tgt[i].abs.p(X),in.tgt[i].abs.p(Y),in.tgt[i].abs.p(Z)); }
-            //      for(int i=0;i<num_wrench_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.1f %+05.1f %+05.1f %+05.1f %+05.1f %+05.1f ",wcatgt[i].c_str(),in.w[i](fx),in.w[i](fy),in.w[i](fz),in.w[i](tx),in.w[i](ty),in.w[i](tz)); }
             printf("\n");
         }
         void print() const { hp_printf(*this); }
@@ -307,7 +306,7 @@ class WBMSCore{
         BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters, com_in_filter;
         hrp::Vector3 com_old, com_oldold, comacc, com_CP_ref_old, r_zmp_raw;
         std::vector<hrp::Vector2> rflf_points, hull_com, hull_dcp, hull_acp;
-        std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rot_filters;
+        std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rpy_filters;
         std::vector<cv::Point2f> points,cvhull;
         HumanPose rp_ref_out_old, hp_swap_checked;
         unsigned int loop;
@@ -318,7 +317,7 @@ class WBMSCore{
         double H_cur;
         HumanPose hp_wld_raw, hp_plot, rp_ref_out, rp_ref_vel_old;
         FILE *sr_log, *cz_log, *id_log;
-        WBMSPose3D baselinkpose;
+        hrp::Pose3 baselinkpose;
         hrp::Vector3 com_vel_old,rh_vel_old, cp_dec, cp_acc;
         std::vector<hrp::Vector3> foot_vert_act[LR], foot_vert_safe[LR], foot_vert_check[LR];
         hrp::Vector2 com_forcp_ref,com_vel_forcp_ref;
@@ -359,11 +358,11 @@ class WBMSCore{
             cp_dec = cp_acc = hrp::Vector3::Zero();
             invdyn_ft = hrp::dvector6::Zero();
             tgt_pos_filters.resize(num_pose_tgt);
-            tgt_rot_filters.resize(num_pose_tgt);
+            tgt_rpy_filters.resize(num_pose_tgt);
             for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(1.0, HZ, Q_NOOVERSHOOT);//四肢拘束点用(position)
-            for(int i=0;i<tgt_rot_filters.size();i++)tgt_rot_filters[i].setParameter(1.0, HZ, Q_NOOVERSHOOT);//四肢拘束点用(Rotation)
+            for(int i=0;i<tgt_rpy_filters.size();i++)tgt_rpy_filters[i].setParameter(1.0, HZ, Q_NOOVERSHOOT);//四肢拘束点用(Rotation)
             tgt_pos_filters[com].setParameter(1.0, HZ, Q_NOOVERSHOOT);//重心pos用
-            tgt_rot_filters[com].setParameter(0.6, HZ, Q_NOOVERSHOOT);//重心rot用
+            tgt_rpy_filters[com].setParameter(0.6, HZ, Q_NOOVERSHOOT);//重心rot用
             tgt_pos_filters[rf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, Q_NOOVERSHOOT);//右足pos用
             tgt_pos_filters[lf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, Q_NOOVERSHOOT);//左足pos用
             calcacc_v_filters.setParameter(5, HZ, Q_BUTTERWORTH);//加速度計算用
@@ -397,9 +396,9 @@ class WBMSCore{
             wp.use_head = true;
             wp.use_upper = true;
             wp.use_lower = true;
-            rp_ref_out_old.clear();
-            rp_ref_out.clear();
-            rp_ref_vel_old.clear();
+            rp_ref_out_old.reset();
+            rp_ref_out.reset();
+            rp_ref_vel_old.reset();
             rflf_points.reserve(8);//あらかじめreserveして高速化
             points.reserve(8);
             hull_com.reserve(6);
@@ -431,8 +430,7 @@ class WBMSCore{
         void initializeHumanPoseFromCurrentInput(){
             std::string ns[7] = {"com","rf","lf","rh","lh","zmp","head"};
             for(int i=0;i<7;i++){
-                hp_wld_raw.tgt[i].offs.p   =  hp_wld_raw.tgt[i].abs.p;
-                hp_wld_raw.tgt[i].offs.rpy =  hp_wld_raw.tgt[i].abs.rpy;
+                hp_wld_raw.tgt[i].offs   =  hp_wld_raw.tgt[i].abs;
             }
         }
         void initializeRobotPoseFromHRPBody(const hrp::BodyPtr robot_in, std::map<std::string, IKConstraint>& _ee_ikc_map){
@@ -441,20 +439,20 @@ class WBMSCore{
             for(int i=0;i<4;i++){//HumanSynchronizerの初期姿勢オフセットをセット
                 if(_ee_ikc_map.count(robot_l_names[i])){
                     rp_ref_out.tgt[human_l_names[i]].offs.p = _ee_ikc_map[robot_l_names[i]].getCurrentTargetPos(robot_in);//          fik_in->getEndEffectorPos(robot_l_names[i]);
-                    rp_ref_out.tgt[human_l_names[i]].offs.rpy = hrp::rpyFromRot(_ee_ikc_map[robot_l_names[i]].getCurrentTargetRot(robot_in));
+                    rp_ref_out.tgt[human_l_names[i]].offs.R = _ee_ikc_map[robot_l_names[i]].getCurrentTargetRot(robot_in);
                     rp_ref_out.tgt[human_l_names[i]].abs = rp_ref_out.tgt[human_l_names[i]].offs;
                 }
             }
             rp_ref_out.tgt[com].offs.p = act_rs.com = act_rs.zmp = robot_in->calcCM();
             com_CP_ref_old = rp_ref_out.tgt[com].offs.p;
-            rp_ref_out.tgt[com].offs.rpy = hrp::rpyFromRot(robot_in->rootLink()->R);
+            rp_ref_out.tgt[com].offs.R = robot_in->rootLink()->R;
             rp_ref_out.tgt[zmp].offs.p(X) = rp_ref_out.tgt[com].offs.p(X);
             rp_ref_out.tgt[zmp].offs.p(Y) = rp_ref_out.tgt[com].offs.p(Y);
             rp_ref_out.tgt[zmp].offs.p(Z) = (rp_ref_out.tgt[rf].offs.p(Z) + rp_ref_out.tgt[lf].offs.p(Z)) / 2;
             rp_ref_out.tgt[rf].cnt = rp_ref_out.tgt[rf].offs;
             rp_ref_out.tgt[lf].cnt = rp_ref_out.tgt[lf].offs;
             baselinkpose.p = robot_in->rootLink()->p;
-            baselinkpose.rpy = hrp::rpyFromRot(robot_in->rootLink()->R);
+            baselinkpose.R = robot_in->rootLink()->R;
             for(int i=0, l[LR]={rf,lf}; i<LR; i++){
                 rp_ref_out.tgt[l[i]].is_contact = true;
             }
@@ -474,7 +472,6 @@ class WBMSCore{
             convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
             hp_plot = rp_ref_out;
             if(wp.set_com_height_fix)rp_ref_out.tgt[com].abs.p(Z) = rp_ref_out.tgt[com].offs.p(Z) + wp.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
-            judgeFootLandOnCommandByFootForce   (hp_wld_raw);//人体足裏反力から各足の接地指令を生成
             lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
             limitEEWorkspace                    (rp_ref_out);
             setFootContactPoseByGoContact       (rp_ref_out);
@@ -536,7 +533,7 @@ class WBMSCore{
             //      out = in;//ダメゼッタイ
             for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
                 out.tgt[l[i]].abs.p   =  h2r_ratio * (in.tgt[l[i]].abs.p - in.tgt[l[i]].offs.p)   + out.tgt[l[i]].offs.p;
-                out.tgt[l[i]].abs.rpy =  hrp::rpyFromRot( hrp::rotFromRpy(in.tgt[l[i]].abs.rpy) * hrp::rotFromRpy(in.tgt[l[i]].offs.rpy).transpose() * hrp::rotFromRpy(out.tgt[l[i]].offs.rpy) );
+                out.tgt[l[i]].abs.R =  in.tgt[l[i]].abs.R * in.tgt[l[i]].offs.R.transpose() * out.tgt[l[i]].offs.R;
             }
             for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
                 out.tgt[l[i]].w = in.tgt[l[i]].w;
@@ -544,22 +541,16 @@ class WBMSCore{
             }
             out.tgt[zmp].abs = in.tgt[zmp].abs;//最近使わない
         }
-        void judgeFootLandOnCommandByFootForce(HumanPose& in){
-            for(int i=0, l[LR]={rf,lf}; i<LR; i++){
-                if      (in.tgt[l[i]].is_contact  && in.tgt[l[i]].w(fz)<CNT_F_TH   ){in.tgt[l[i]].is_contact = false;}//足ついた状態から上げる
-                else if (!in.tgt[l[i]].is_contact && in.tgt[l[i]].w(fz)>CNT_F_TH+30){in.tgt[l[i]].is_contact = true;}//足浮いた状態から下げる
-            }
-        }
         void applyLPFilter_post(HumanPose& tgt){
             if(is_initial_loop){
                 for(int i=0, l[1]={com}; i<1; i++){
                     tgt_pos_filters[l[i]].reset(tgt.tgt[l[i]].abs.p);
-                    tgt_rot_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy);
+                    tgt_rpy_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy());
                 }
             }
             for(int i=0, l[1]={com}; i<1; i++){
                 tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
-                tgt.tgt[l[i]].abs.rpy = tgt_rot_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy);
+                tgt.tgt[l[i]].abs.setRpy(tgt_rpy_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy()));
             }
         }
         void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
@@ -578,56 +569,55 @@ class WBMSCore{
                 PoseTGT& swing_leg = out.tgt[swl[i]];
                 hrp::Vector2 inside_vec_baserel = ( spl[i]==rf ? hrp::Vector2(0,+1) : hrp::Vector2(0,-1) );
 
-                hrp::Vector2 sp2sw_vec( swing_leg.abs.p(X) - support_leg.cnt.p(X), swing_leg.abs.p(Y) - support_leg.cnt.p(Y) );
+//                hrp::Vector2 sp2sw_vec( swing_leg.abs.p(X) - support_leg.cnt.p(X), swing_leg.abs.p(Y) - support_leg.cnt.p(Y) );
+                hrp::Vector2 sp2sw_vec = swing_leg.abs.p.head(XY) - support_leg.cnt.p.head(XY);
                 if(swing_leg.go_contact && sp2sw_vec.norm() > MAX_FW){ sp2sw_vec = sp2sw_vec.normalized() * MAX_FW; }//着地時に足を広げすぎないよう制限
                 Eigen::Matrix2d base_rot;
-                base_rot = Eigen::Rotation2Dd(baselinkpose.rpy(y));
+                base_rot = Eigen::Rotation2Dd(baselinkpose.rpy()(y));
                 hrp::Vector2 sp2sw_vec_baserel = base_rot.transpose() * sp2sw_vec;
                 if( sp2sw_vec_baserel.dot(inside_vec_baserel) < FOOT_2_FOOT_COLLISION_MARGIIN){
                     sp2sw_vec_baserel += inside_vec_baserel * (FOOT_2_FOOT_COLLISION_MARGIIN - sp2sw_vec_baserel.dot(inside_vec_baserel));
                 }
                 sp2sw_vec = base_rot * sp2sw_vec_baserel;
-                swing_leg.abs.p(X) = sp2sw_vec(X) + support_leg.cnt.p(X);
-                swing_leg.abs.p(Y) = sp2sw_vec(Y) + support_leg.cnt.p(Y);
+                swing_leg.abs.p.head(XY) = sp2sw_vec + support_leg.cnt.p.head(XY);
             }
-//            LIMIT_MINMAX( out.tgt[rf].abs.p(Z), out.tgt[rf].offs.p(Z), out.tgt[rf].offs.p(Z)+WBMSparam.swing_foot_max_height);
-//            LIMIT_MINMAX( out.tgt[lf].abs.p(Z), out.tgt[lf].offs.p(Z), out.tgt[lf].offs.p(Z)+WBMSparam.swing_foot_max_height);
-
-//            const double base2hand_min = 0.4;
             const double base2hand_min = 0.0;
             for(int i=0, l[2]={rh,lh}; i<2; i++){
-                //        LIMIT_MIN(out.tgt[l[i]].abs.p(X), baselinkpose.p(X));
-                //        LIMIT_MAX(out.tgt[l[i]].abs.p(Z), baselinkpose.p(Z) + 0.4);
-                hrp::Vector2 horizontal_dist(out.tgt[l[i]].abs.p(X) - baselinkpose.p(X), out.tgt[l[i]].abs.p(Y) - baselinkpose.p(Y));
+//                hrp::Vector2 horizontal_dist(out.tgt[l[i]].abs.p(X) - baselinkpose.p(X), out.tgt[l[i]].abs.p(Y) - baselinkpose.p(Y));
+                hrp::Vector2 horizontal_dist = out.tgt[l[i]].abs.p.head(XY) - baselinkpose.p.head(XY);
                 if(horizontal_dist.norm() < base2hand_min){
                     horizontal_dist = base2hand_min * horizontal_dist.normalized();
                 }
-                out.tgt[l[i]].abs.p(X) = baselinkpose.p(X) + horizontal_dist(X);
-                out.tgt[l[i]].abs.p(Y) = baselinkpose.p(Y) + horizontal_dist(Y);
+                out.tgt[l[i]].abs.p.head(XY) = baselinkpose.p.head(XY) + horizontal_dist;
             }
             LIMIT_MINMAX( out.tgt[com].abs.p(Z), out.tgt[com].offs.p(Z) - 0.15, out.tgt[com].offs.p(Z) + 0.03 );//COM高さ方向の制限
             //      for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( out.tgt[com].abs.rpy(i), rc.ee_rot_limit[com][MIN](i), rc.ee_rot_limit[com][MAX](i) ); }
             //      for(int i=0, l[5]={rf,lf,rh,lh,head}; i<5; i++){
             //        for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( out.tgt[l[i]].abs.rpy(j), rc.ee_rot_limit[l[i]][MIN](j) + out.tgt[com].abs.rpy(j), rc.ee_rot_limit[l[i]][MAX](j) + out.tgt[com].abs.rpy(j) ); }
             //      }
-            if(!wp.use_head)out.tgt[head].abs.rpy = hrp::Vector3::Zero();
+            if(!wp.use_head)out.tgt[head].abs.R = hrp::Matrix33::Identity();
         }
         void setFootContactPoseByGoContact(HumanPose& out){
             for(int i=0, l[LR]={rf,lf}; i<LR; i++){
                 if(out.tgt[l[i]].go_contact){
-                    out.tgt[l[i]].abs.p(X) = out.tgt[l[i]].cnt.p(X);
-                    out.tgt[l[i]].abs.p(Y) = out.tgt[l[i]].cnt.p(Y);
-                    out.tgt[l[i]].abs.rpy(y) = out.tgt[l[i]].cnt.rpy(y);
+                    out.tgt[l[i]].abs.p.head(XY) = out.tgt[l[i]].cnt.p.head(XY);
+                    out.tgt[l[i]].abs.setRpy(out.tgt[l[i]].abs.rpy()(r), out.tgt[l[i]].abs.rpy()(p), out.tgt[l[i]].cnt.rpy()(y));
+
                 }else{
-                    out.tgt[l[i]].cnt.p(X) = rp_ref_out_old.tgt[l[i]].abs.p(X);
-                    out.tgt[l[i]].cnt.p(Y) = rp_ref_out_old.tgt[l[i]].abs.p(Y);
-                    out.tgt[l[i]].cnt.rpy(y) = rp_ref_out_old.tgt[l[i]].abs.rpy(y);
+                    out.tgt[l[i]].cnt.p.head(XY) = out.tgt[l[i]].abs.p.head(XY);
+                    out.tgt[l[i]].cnt.setRpy(out.tgt[l[i]].cnt.rpy()(r), out.tgt[l[i]].cnt.rpy()(p), out.tgt[l[i]].abs.rpy()(y));
                 }
             }
         }
         void setFootRotHorizontalIfGoLanding(HumanPose& out){
-            if( out.tgt[rf].go_contact){ out.tgt[rf].abs.rpy(r) = out.tgt[rf].abs.rpy(p) = 0; }
-            if( out.tgt[lf].go_contact){ out.tgt[lf].abs.rpy(r) = out.tgt[lf].abs.rpy(p) = 0; }
+            if( out.tgt[rf].go_contact){
+//                out.tgt[rf].abs.rpy(r) = out.tgt[rf].abs.rpy(p) = 0;
+                out.tgt[rf].abs.setRpy(0,0,out.tgt[rf].abs.rpy()(y));
+            }
+            if( out.tgt[lf].go_contact){
+//                out.tgt[lf].abs.rpy(r) = out.tgt[lf].abs.rpy(p) = 0;
+                out.tgt[lf].abs.setRpy(0,0,out.tgt[lf].abs.rpy()(y));
+            }
         }
         void overwriteFootZFromFootLandOnCommand(HumanPose& out){
             //      limitGroundContactVelocity(out.tgt[rf].go_contact, rp_ref_out_old.tgt[rf], out.tgt[rf], out.tgt[rf].is_contact);
@@ -664,7 +654,7 @@ class WBMSCore{
             double min_height = foot_in.abs.p(Z);
             int min_height_id = 0;
             for(int i=0;i<act_sole_size.size();i++){
-                act_foot_vert.push_back(hrp::rotFromRpy(foot_in.abs.rpy) * act_sole_size[i]);
+                act_foot_vert.push_back(foot_in.abs.R * act_sole_size[i]);
                 act_foot_vert[i](Z) += foot_in.abs.p(Z);
                 if(act_foot_vert[i](Z) < min_height){
                     min_height = act_foot_vert[i](Z);
@@ -678,10 +668,9 @@ class WBMSCore{
         bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
             hull_com.clear();
             createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R], foot_vert_safe_fblr[L], hull_com);
-            hrp::Vector2 cog(comin_abs(X),comin_abs(Y));
+            hrp::Vector2 cog = comin_abs.head(XY);
             if(!isPointInHullOpenCV(cog,hull_com)){ calcNearestPointOnHull(cog,hull_com,cog); }//外に出たら最近傍点に頭打ち
-            comin_abs(X) = cog(X);
-            comin_abs(Y) = cog(Y);
+            comin_abs.head(XY) = cog.head(XY);
             return true;
         }
         bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans_old, hrp::Vector3& com_ans){
@@ -695,14 +684,14 @@ class WBMSCore{
             createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R]+marginDelta_for_dcp, foot_vert_safe_fblr[L]+marginDelta_for_dcp, hull_dcp);
             createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R]+marginDelta_for_acp, foot_vert_safe_fblr[L]+marginDelta_for_acp, hull_acp);
             hrp::Vector2 com_vel_ans_2d;
-            regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_ans_old(X),com_ans_old(Y)), hrp::Vector2(com_vel(X),com_vel(Y)), hull_dcp, hull_acp, com_vel_ans_2d);
-            com_ans(X) = com_ans_old(X) + com_vel_ans_2d(X) * DT;
-            com_ans(Y) = com_ans_old(Y) + com_vel_ans_2d(Y) * DT;
+//            regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_ans_old(X),com_ans_old(Y)), hrp::Vector2(com_vel(X),com_vel(Y)), hull_dcp, hull_acp, com_vel_ans_2d);
+            regulateCOMVelocityByCapturePointVec( com_ans_old.head(XY), com_vel.head(XY), hull_dcp, hull_acp, com_vel_ans_2d);
+            com_ans.head(XY) = com_ans_old.head(XY) + com_vel_ans_2d * DT;
             com_ans_old = com_ans;
 //            Vector2ToVector3(hrp::Vector2(com_ans(X),com_ans(Y)) + com_vel_ans_2d * sqrt( H_cur / G ), cp_dec);
 //            Vector2ToVector3(hrp::Vector2(com_ans(X),com_ans(Y)) - com_vel_ans_2d * sqrt( H_cur / G ), cp_acc);
-            cp_dec.head(2) = hrp::Vector2(com_ans(X),com_ans(Y)) + com_vel_ans_2d * sqrt( H_cur / G );
-            cp_acc.head(2) = hrp::Vector2(com_ans(X),com_ans(Y)) - com_vel_ans_2d * sqrt( H_cur / G );
+            cp_dec.head(2) = com_ans.head(XY) + com_vel_ans_2d * sqrt( H_cur / G );
+            cp_acc.head(2) = com_ans.head(XY) - com_vel_ans_2d * sqrt( H_cur / G );
             if(DEBUG){
                 fprintf(cz_log,"com_ans: %f %f ",com_ans(X),com_ans(Y));
                 fprintf(cz_log,"com_ans_cp_d: %f %f ",cp_dec(X),cp_dec(Y));
@@ -890,19 +879,10 @@ class WBMSCore{
         void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
             comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
             comacc = acc4zmp_v_filters.passFilter(comacc);
-            zmpout(X) = comin(X)-(H_cur/G)*comacc(X);
-            zmpout(Y) = comin(Y)-(H_cur/G)*comacc(Y);
+            zmpout.head(XY) = comin.head(XY)-(H_cur/G)*comacc.head(XY);
             com_oldold = com_old;
             com_old = comin;
         }
-        void applyVelLimit(const HumanPose& out_old, HumanPose& out){
-            const double EE_MAX_VEL = 2.0;// m/s
-            for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
-                hrp::Vector3 diff = out.tgt[i].abs.p  - out_old.tgt[i].abs.p;
-                for(int j=0;j<3;j++)LIMIT_MINMAX( diff(j), -EE_MAX_VEL*DT, EE_MAX_VEL*DT);
-                out.tgt[i].abs.p = out_old.tgt[i].abs.p + diff;
-            }
-        }
 };
 
 #endif // WBMS_CORE_H

From dd80dc7a9d042d472943f32f2dddcd1960b3bfad Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 1 May 2019 23:20:16 +0900
Subject: [PATCH 157/217] refactor

---
 python/hrpsys_config.py                       |   2 +
 .../FullbodyInverseKinematicsSolver.h         | 327 ++++++++----------
 rtc/AutoBalancer/MyUtils.h                    |  18 +-
 .../WholeBodyMasterSlave.cpp                  |  98 +++++-
 .../WholeBodyMasterSlave.h                    |  11 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          |  50 +--
 6 files changed, 295 insertions(+), 211 deletions(-)

diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index cff84601d1e..163e0fdf43b 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -408,6 +408,8 @@ def connectComps(self):
                 connectPorts(self.wbms.port("basePosOut"), self.abc.port("basePosIn"))
                 connectPorts(self.wbms.port("baseRpyOut"), self.abc.port("baseRpyIn"))
                 connectPorts(self.wbms.port("optionalDataOut"), self.abc.port("optionalData"))
+                connectPorts(self.st.port("actCapturePoint"), self.wbms.port("actCapturePoint"))
+                connectPorts(self.st.port("zmp"), self.wbms.port("zmp"))
             
             connectPorts(self.abc.port("zmpOut"), self.st.port("zmpRef"))
             connectPorts(self.abc.port("baseRpyOut"), self.st.port("baseRpyIn"))
diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 0287686f85d..d731108c4fd 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -14,6 +14,7 @@
 
 class IKConstraint {
     public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
         std::string target_link_name;
         hrp::Vector3 targetPos;
         hrp::Vector3 targetRpy;
@@ -52,7 +53,7 @@ class FullbodyInverseKinematicsSolver{
           J_DOF(_robot->numJoints()),
           ALL_DOF(J_DOF+BASE_DOF){
             dq_weight_all = hrp::dvector::Ones(ALL_DOF);
-            dq_all = q_ref = q_ref_constraint_weight = hrp::dvector::Zero(ALL_DOF);
+            q_ref = q_ref_constraint_weight = hrp::dvector::Zero(ALL_DOF);
             q_ref_max_dq = hrp::dvector::Constant(ALL_DOF, 1e-2);
             cur_momentum_around_COM = cur_momentum_around_COM_filtered = hrp::Vector3::Zero();
             jlim_avoid_weight_old = hrp::dvector::Constant(ALL_DOF, 0);
@@ -117,194 +118,166 @@ class FullbodyInverseKinematicsSolver{
         }
         void solveFullbodyIKOnce(const std::vector<IKConstraint>& _ikc_list){
             #ifdef OPENHRP_PACKAGE_VERSION_320
-                // count up all valid constraint DOF and allocate Jacobian and vectors
-                int C_DOF = 0; // constraint DOF
-                for (int i=0;i<_ikc_list.size();i++){ C_DOF += (_ikc_list[i].constraint_weight.array()>0.0).count(); }
-                C_DOF += (q_ref_constraint_weight.array()>0.0).count();
-                J_all = hrp::dmatrix::Zero(C_DOF, ALL_DOF);
-                err_all = hrp::dvector::Zero(C_DOF);
-                constraint_weight_all = hrp::dvector::Ones(C_DOF);
 
-                // set end effector constraint into Jacobian
-                int cur_cid_all = 0; //current constraint index of all constraints
-                for ( int i=0; i<_ikc_list.size(); i++ ) {
-                    hrp::Link* link_tgt_ptr = m_robot->link(_ikc_list[i].target_link_name);
-                    hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF); //全身to拘束点へのヤコビアン
-                    hrp::dvector6 dp_part; //pos + rot 速度ではなく差分
-                    if(link_tgt_ptr){//ベースリンク，通常リンク共通
-                        hrp::Vector3 tgt_cur_pos = link_tgt_ptr->p + link_tgt_ptr->R * _ikc_list[i].localPos;
-                        hrp::Matrix33 tgt_cur_rot = link_tgt_ptr->R * _ikc_list[i].localR;
-                        dp_part.head(3) =  _ikc_list[i].targetPos - tgt_cur_pos;
-                        dp_part.tail(3) = tgt_cur_rot * hrp::omegaFromRotEx(tgt_cur_rot.transpose() * hrp::rotFromRpy(_ikc_list[i].targetRpy));
-                        hrp::JointPath tgt_jpath(m_robot->rootLink(), link_tgt_ptr);
-                        hrp::dmatrix J_jpath;
-                        tgt_jpath.calcJacobian(J_jpath, _ikc_list[i].localPos);
-                        for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ J_part.col(tgt_jpath.joint(id_in_jpath)->jointId) = J_jpath.col(id_in_jpath); } //ジョイントパスのJacobianを全身用に並び替え
-                        J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
-                        J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - m_robot->rootLink()->p);
-                    }else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){ //重心限定
-                        dp_part.head(3) = _ikc_list[i].targetPos - m_robot->calcCM();
-//                        dp_part.tail(3) = (_ikc_list[i].targetRpy - cur_momentum_around_COM) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける
-                        dp_part.tail(3) = (_ikc_list[i].targetRpy - cur_momentum_around_COM_filtered) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける(+フィルタ)
-                        m_robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
-                        m_robot->calcAngularMomentumJacobian(NULL, J_am);//world座標系での角運動量を生成するヤコビアン(なので本当はCOM周りには使えない)
-                        J_part << J_com, J_am;
-
-                    }else{ std::cerr<<"Unknown Link Target !!"<<std::endl; continue; } //不明なリンク指定
-                    for(int cid=0; cid<WS_DOF; cid++){
-                        if(_ikc_list[i].constraint_weight(cid) > 0.0){
-                            J_all.row(cur_cid_all) = J_part.row(cid);
-
-LIMIT_MINMAX(dp_part(cid), -0.1,0.1);
-
-                            err_all(cur_cid_all) = dp_part(cid);
-                            constraint_weight_all(cur_cid_all) = _ikc_list[i].constraint_weight(cid);
-                            cur_cid_all++;
-                        }
-                    }
+            // count up all valid constraint DOF and allocate Jacobian and vectors
+            hrp::dmatrix q_select_mat = hrp::to_SelectionMat(dq_weight_all);
+            const int VALID_Q_NUM = q_select_mat.rows();
+            if(VALID_Q_NUM == 0){ return; }
+            int VALID_C_NUM = 0; // count up valid constraint num
+            for (int i=0;i<_ikc_list.size();i++){ VALID_C_NUM += (_ikc_list[i].constraint_weight.array()>0.0).count(); }// count up valid end effector constraint num
+            VALID_C_NUM += (q_ref_constraint_weight.array()>0.0).count();// count up valid reference joint angle constraint num
+            J_all = hrp::dmatrix::Zero(VALID_C_NUM, VALID_Q_NUM);
+            err_all = hrp::dvector::Zero(VALID_C_NUM);
+            constraint_weight_all = hrp::dvector::Ones(VALID_C_NUM);
+
+            // set end effector constraints into Jacobian
+            int CURRENT_C_COUNT = 0;
+            for ( int i=0; i<_ikc_list.size(); i++ ) {
+                hrp::Link* link_tgt_ptr = m_robot->link(_ikc_list[i].target_link_name);
+                hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF); //全身to拘束点へのヤコビアン
+                hrp::dvector6 dp_part; //pos + rot 速度ではなく差分
+                if(link_tgt_ptr){//ベースリンク，通常リンク共通
+                    hrp::Vector3 tgt_cur_pos = link_tgt_ptr->p + link_tgt_ptr->R * _ikc_list[i].localPos;
+                    hrp::Matrix33 tgt_cur_rot = link_tgt_ptr->R * _ikc_list[i].localR;
+                    dp_part.head(3) =  _ikc_list[i].targetPos - tgt_cur_pos;
+                    dp_part.tail(3) = tgt_cur_rot * hrp::omegaFromRotEx(tgt_cur_rot.transpose() * hrp::rotFromRpy(_ikc_list[i].targetRpy));
+                    hrp::JointPath tgt_jpath(m_robot->rootLink(), link_tgt_ptr);
+                    hrp::dmatrix J_jpath;
+                    tgt_jpath.calcJacobian(J_jpath, _ikc_list[i].localPos);
+                    for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ J_part.col(tgt_jpath.joint(id_in_jpath)->jointId) = J_jpath.col(id_in_jpath); } //ジョイントパスのJacobianを全身用に並び替え
+                    J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
+                    J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - m_robot->rootLink()->p);
                 }
-
-                // set desired joint angle constraint into Jacobian
-                for(int qid=0; qid<ALL_DOF; qid++){
-                    if(q_ref_constraint_weight(qid) > 0.0){
-                        J_all.row(cur_cid_all)(qid) = 1;
-                        err_all(cur_cid_all) = (qid < J_DOF) ? (q_ref(qid) - m_robot->joint(qid)->q) : (q_ref(qid) - hrp::rpyFromRot(m_robot->rootLink()->R)(qid-J_DOF));
-                        LIMIT_MINMAX(err_all(cur_cid_all), -q_ref_max_dq(qid), q_ref_max_dq(qid));
-                        constraint_weight_all(cur_cid_all) = q_ref_constraint_weight(qid);
-                        cur_cid_all++;
-                    }
+                else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){ //重心限定
+                    dp_part.head(3) = _ikc_list[i].targetPos - m_robot->calcCM();
+                    dp_part.tail(3) = (_ikc_list[i].targetRpy - cur_momentum_around_COM_filtered) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける(+フィルタ)
+                    m_robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
+                    m_robot->calcAngularMomentumJacobian(NULL, J_am);//world座標系での角運動量を生成するヤコビアン(なので本当はCOM周りには使えない)
+                    J_part << J_com, J_am;
                 }
-
-                // joint limit avoidance (copy from JointPathEx)
-                hrp::dvector dq_weight_all_jlim = hrp::dvector::Ones(ALL_DOF);
-                for ( int j = 0; j < ALL_DOF ; j++ ) {
-                    double jang = (j<J_DOF) ? m_robot->joint(j)->q       :   hrp::rpyFromRot(m_robot->rootLink()->R)(j - J_DOF);
-                    double jmax = (j<J_DOF) ? m_robot->joint(j)->ulimit  :   rootlink_rpy_ulimit(j - J_DOF);
-                    double jmin = (j<J_DOF) ? m_robot->joint(j)->llimit  :   rootlink_rpy_llimit(j - J_DOF);
-                    double e = deg2rad(1);
-                    if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
-                    } else if ( eps_eq(jang, jmax,e) ) {
-                        jang = jmax - e;
-                    } else if ( eps_eq(jang, jmin,e) ) {
-                        jang = jmin + e;
-                    }
-                    double jlim_avoid_weight;
-                    if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
-                        jlim_avoid_weight = DBL_MAX;
-                    } else {
-                        jlim_avoid_weight = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) / (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
-                        if (isnan(jlim_avoid_weight)) jlim_avoid_weight = 0;
-                    }
-                    if (( jlim_avoid_weight - jlim_avoid_weight_old(j) ) >= 0 ) { // add weight only if q approaching to the limit
-                        dq_weight_all_jlim(j) += jlim_avoid_weight;
-                    }
-                    jlim_avoid_weight_old(j) = jlim_avoid_weight;
+                else{
+                    std::cerr<<"Unknown Link Target !!"<<std::endl; continue;  //不明なリンク指定
                 }
-
-                hrp::dvector dq_weight_all_final = dq_weight_all.array() * dq_weight_all_jlim.array();
-
-
-
-
-
-                hrp::ivector selection_vec = hrp::ivector::Zero(ALL_DOF);
-                selection_vec.head(31).fill(1);
-                selection_vec(6) = 0;
-                selection_vec(7) = 0;
-                selection_vec.tail(6).fill(1);
-
-                const int USE_Q_NUM = (selection_vec.array() == 1).count();
-                hrp::dmatrix selection_mat = hrp::to_SelectionMat(selection_vec);
-
-
-
-
+                // set one of the end effector constraints into Jacobian
+                hrp::dmatrix c_part_selection_mat = hrp::to_SelectionMat(_ikc_list[i].constraint_weight);   // select only valid end effector constraint
+                if(c_part_selection_mat.rows() != 0 && c_part_selection_mat.cols() != 0 ){
 
 
-                // Solvability-unconcerned Inverse Kinematics by Levenberg-Marquardt Method [sugihara:RSJ2009]
-                const double wn_const = 1e-6;
-                hrp::dmatrix Wn = (static_cast<double>(err_all.transpose() * constraint_weight_all.asDiagonal() * err_all) + wn_const) * hrp::dmatrix::Identity(ALL_DOF, ALL_DOF);
-                Wn = dq_weight_all_final.asDiagonal() * Wn;
+                    const double dp_max = 9999990.1;
 
-
-
-//                dq_weight_all_final = selection_mat * dq_weight_all_final;
-                J_all = J_all * selection_mat.transpose();
-                Wn = selection_mat * Wn * selection_mat.transpose();
-
-
-                hrp::dmatrix H = J_all.transpose() * constraint_weight_all.asDiagonal() * J_all + Wn;
-                hrp::dvector g = J_all.transpose() * constraint_weight_all.asDiagonal() * err_all;
-                dq_all = H.ldlt().solve(g); // dq_all = H.inverse() * g; is slow
-
-                // rtconf localhost:15005/wbms.rtc set debugLevel 1 とかにしたい
-                // debug print
-                static int count;
-                if(count++ % 10000 == 0){
-//                if(true){
-                    std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
-                    dbg(J_all.rows());
-                    dbg(J_all.cols());
-                    dbgn(selection_mat);
-                    dbg(selection_mat.rows());
-                    dbg(selection_mat.cols());
-                    dbgn(H);
-                    dbg(H.rows());
-                    dbg(H.cols());
-                    dbgn(Wn);
-                    dbg(Wn.rows());
-                    dbg(Wn.cols());
-                    dbg(g.transpose());
-                    dbg(g.rows());
-                    dbg(g.cols());
-                    dbg(err_all.transpose());
-                    dbg(err_all.rows());
-                    dbg(err_all.cols());
-                    dbg(dq_all.transpose());
-                    dbg(dq_all.rows());
-                    dbg(dq_all.cols());
-                    dbg(constraint_weight_all.transpose());
-                    dbg(constraint_weight_all.rows());
-                    dbg(constraint_weight_all.cols());
-                    dbg(hrp::getRobotStateVec(m_robot).transpose());
-                    std::cout<<std::endl;
+                    J_all.middleRows                (CURRENT_C_COUNT, c_part_selection_mat.rows()) = c_part_selection_mat * J_part * q_select_mat.transpose();
+                    err_all.segment                 (CURRENT_C_COUNT, c_part_selection_mat.rows()) = c_part_selection_mat * dp_part.cwiseMin(hrp::dvector6::Constant(dp_max)).cwiseMax(hrp::dvector6::Constant(-dp_max));
+                    constraint_weight_all.segment   (CURRENT_C_COUNT, c_part_selection_mat.rows()) = c_part_selection_mat * _ikc_list[i].constraint_weight;
+                    CURRENT_C_COUNT += c_part_selection_mat.rows();
                 }
+            }
 
-                // update joint angles
-                for(int i=0;i<dq_all.rows();i++){ if( isnan(dq_all(i)) || isinf(dq_all(i)) ){ std::cerr <<"[FullbodyIK] ERROR nan/inf is found" << std::endl; return;} }
-
-                dq_all = selection_mat.transpose() * dq_all;
-//                dbg(dq_all.transpose());
-//                dbg(dq_all.rows());
-//                dbg(dq_all.cols());
-
-                for(int i=0;i<J_DOF;i++){
-                    m_robot->joint(i)->q += dq_all(i);
-//                    m_robot->joint(i)->q += (selection_mat.transpose() * dq_all)(i);
-                    LIMIT_MINMAX(m_robot->joint(i)->q, m_robot->joint(i)->llimit, m_robot->joint(i)->ulimit);
+            // set reference joint angle constraints into Jacobian
+            hrp::dmatrix q_ref_selection_mat = hrp::to_SelectionMat(q_ref_constraint_weight);
+            if(q_ref_selection_mat.rows() != 0 && q_ref_selection_mat.cols() != 0 ){
+                J_all.middleRows                (CURRENT_C_COUNT, q_ref_selection_mat.rows()) = q_ref_selection_mat * hrp::dmatrix::Identity(ALL_DOF,ALL_DOF) * q_select_mat.transpose();
+                err_all.segment                 (CURRENT_C_COUNT, q_ref_selection_mat.rows()) = q_ref_selection_mat * ((q_ref - hrp::getRobotStateVec(m_robot)).cwiseMin(q_ref_max_dq).cwiseMax(-q_ref_max_dq));
+                constraint_weight_all.segment   (CURRENT_C_COUNT, q_ref_selection_mat.rows()) = q_ref_selection_mat * q_ref_constraint_weight;
+                CURRENT_C_COUNT += q_ref_selection_mat.rows();
+            }
+            assert(CURRENT_C_COUNT == VALID_C_NUM);
+
+            // joint limit avoidance (copy from JointPathEx)
+            hrp::dvector dq_weight_all_jlim = hrp::dvector::Ones(ALL_DOF);
+            for ( int j = 0; j < ALL_DOF ; j++ ) {
+                double jang = (j<J_DOF) ? m_robot->joint(j)->q       :   hrp::rpyFromRot(m_robot->rootLink()->R)(j - J_DOF);
+                double jmax = (j<J_DOF) ? m_robot->joint(j)->ulimit  :   rootlink_rpy_ulimit(j - J_DOF);
+                double jmin = (j<J_DOF) ? m_robot->joint(j)->llimit  :   rootlink_rpy_llimit(j - J_DOF);
+                double e = deg2rad(1);
+                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+                } else if ( eps_eq(jang, jmax,e) ) {
+                    jang = jmax - e;
+                } else if ( eps_eq(jang, jmin,e) ) {
+                    jang = jmin + e;
                 }
-
-                // update rootlink pos rot
-                m_robot->rootLink()->p += dq_all.tail(6).head(3);
-                for(int i=0;i<3;i++){
-                    if(hrp::rpyFromRot(m_robot->rootLink()->R)(i) < rootlink_rpy_llimit(i) && dq_all.tail(6).tail(3)(i) < 0) dq_all.tail(6).tail(3)(i) = 0;
-                    if(hrp::rpyFromRot(m_robot->rootLink()->R)(i) > rootlink_rpy_ulimit(i) && dq_all.tail(6).tail(3)(i) > 0) dq_all.tail(6).tail(3)(i) = 0;
+                double jlim_avoid_weight;
+                if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
+                    jlim_avoid_weight = DBL_MAX;
+                } else {
+                    jlim_avoid_weight = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) / (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
+                    if (isnan(jlim_avoid_weight)) jlim_avoid_weight = 0;
                 }
-
-                hrp::Matrix33 dR;
-                hrp::Vector3 omega = dq_all.tail(6).tail(3);
-                hrp::calcRodrigues(dR, omega.normalized(), omega.norm());
-                hrp::Matrix33 R_base_ans = m_robot->rootLink()->R * dR;
-                if(!R_base_ans.isUnitary()){
-                    std::cerr <<"[FullbodyIK] WARN R_base_ans is not Unitary, normalize via Quaternion" << std::endl;
-                    Eigen::Quaternion<double> quat(R_base_ans);
-                    quat.normalize();
-                    R_base_ans = quat.toRotationMatrix();
-                    return;
-                }else{
-                    m_robot->rootLink()->R = R_base_ans;
+                if (( jlim_avoid_weight - jlim_avoid_weight_old(j) ) >= 0 ) { // add weight only if q approaching to the limit
+                    dq_weight_all_jlim(j) += jlim_avoid_weight;
                 }
-                m_robot->calcForwardKinematics();
+                jlim_avoid_weight_old(j) = jlim_avoid_weight;
+            }
+            hrp::dvector dq_weight_all_final = dq_weight_all.array() * dq_weight_all_jlim.array();
+            dq_weight_all_final = q_select_mat * dq_weight_all_final;
+
+            // Solvability-unconcerned Inverse Kinematics by Levenberg-Marquardt Method [sugihara:RSJ2009]
+            const double wn_const = 1e-6;
+            hrp::dmatrix Wn = (static_cast<double>(err_all.transpose() * constraint_weight_all.asDiagonal() * err_all) + wn_const) * hrp::dmatrix::Identity(dq_weight_all_final.size(), dq_weight_all_final.size());
+            Wn = dq_weight_all_final.asDiagonal() * Wn;
+            hrp::dmatrix H = J_all.transpose() * constraint_weight_all.asDiagonal() * J_all + Wn;
+            hrp::dvector g = J_all.transpose() * constraint_weight_all.asDiagonal() * err_all;
+            dq_all = H.ldlt().solve(g); // dq_all = H.inverse() * g; is slow
+
+            // rtconf localhost:15005/wbms.rtc set debugLevel 1 とかにしたい
+            static int count;
+            if(count++ % 1000 == 0){
+//                if(true){
+                std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
+                dbg(J_all.rows());
+                dbg(J_all.cols());
+                dbgn(H);
+                dbg(H.rows());
+                dbg(H.cols());
+                dbgn(Wn);
+                dbg(Wn.rows());
+                dbg(Wn.cols());
+                dbgv(g);
+                dbg(g.rows());
+                dbg(g.cols());
+                dbgv(err_all);
+                dbg(err_all.rows());
+                dbg(err_all.cols());
+                dbgv(dq_all);
+                dbg(dq_all.rows());
+                dbg(dq_all.cols());
+                dbgv(constraint_weight_all);
+                dbg(constraint_weight_all.rows());
+                dbg(constraint_weight_all.cols());
+                dbgn(q_ref_selection_mat);
+                dbgv(q_ref);
+                dbgv(hrp::getRobotStateVec(m_robot));
+                std::cout<<std::endl;
+            }
+
+            // update joint angles
+            for(int i=0;i<dq_all.rows();i++){ if( isnan(dq_all(i)) || isinf(dq_all(i)) ){ std::cerr <<"[FullbodyIK] ERROR nan/inf is found" << std::endl; return;} }
+            dq_all = q_select_mat.transpose() * dq_all;
+            for(int i=0;i<J_DOF;i++){
+                m_robot->joint(i)->q += dq_all(i);
+                LIMIT_MINMAX(m_robot->joint(i)->q, m_robot->joint(i)->llimit, m_robot->joint(i)->ulimit);
+            }
+
+            // update rootlink pos rot
+            m_robot->rootLink()->p += dq_all.tail(6).head(3);
+            for(int i=0;i<3;i++){
+                if(hrp::rpyFromRot(m_robot->rootLink()->R)(i) < rootlink_rpy_llimit(i) && dq_all.tail(6).tail(3)(i) < 0) dq_all.tail(6).tail(3)(i) = 0;
+                if(hrp::rpyFromRot(m_robot->rootLink()->R)(i) > rootlink_rpy_ulimit(i) && dq_all.tail(6).tail(3)(i) > 0) dq_all.tail(6).tail(3)(i) = 0;
+            }
+
+            hrp::Matrix33 dR;
+            hrp::Vector3 omega = dq_all.tail(6).tail(3);
+            hrp::calcRodrigues(dR, omega.normalized(), omega.norm());
+            hrp::Matrix33 R_base_ans = m_robot->rootLink()->R * dR;
+            if(!R_base_ans.isUnitary()){
+                std::cerr <<"[FullbodyIK] WARN R_base_ans is not Unitary, normalize via Quaternion" << std::endl;
+                Eigen::Quaternion<double> quat(R_base_ans);
+                quat.normalize();
+                R_base_ans = quat.toRotationMatrix();
+                return;
+            }else{
+                m_robot->rootLink()->R = R_base_ans;
+            }
+            m_robot->calcForwardKinematics();
             #else
                 std::cerr<<"solveFullbodyIKOnce() needs OPENHRP_PACKAGE_VERSION_320 !!!"<<std::endl;
             #endif
diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index 6cf13998448..81c03d5a459 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -22,6 +22,7 @@ static const double Q_BUTTERWORTH = 0.707106781;
 
 #define dbg(var) std::cout<<#var"= "<<(var)<<std::endl
 #define dbgn(var) std::cout<<#var"= "<<std::endl<<(var)<<std::endl
+#define dbgv(var) std::cout<<#var"= "<<(var.transpose())<<std::endl
 #define RTCOUT std::cerr << "[" << m_profile.instance_name << "] "
 
 #define LIMIT_MIN(x,min) (x= ( x<min ? min:x ))
@@ -41,9 +42,10 @@ static const double Q_BUTTERWORTH = 0.707106781;
 namespace hrp{
     class Pose3{
         public:
+            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
             hrp::Vector3 p;
             hrp::Matrix33 R;
-            Pose3(){}
+            Pose3(){ p.fill(0); R.setIdentity();}
             Pose3(const double& _X, const double& _Y, const double& _Z, const double& _r, const double& _p, const double& _y){ p << _X,_Y,_Z; R = hrp::rotFromRpy(_r,_p,_y); }
             Pose3(const hrp::dvector6& _xyz_rpy){ p = _xyz_rpy.head(3); R = hrp::rotFromRpy(_xyz_rpy.tail(3)); }
             Pose3(const hrp::Vector3& _xyz, const hrp::Vector3& _rpy){ p = _xyz; R = hrp::rotFromRpy(_rpy); }
@@ -52,7 +54,7 @@ namespace hrp{
             void setRpy(const double& _r, const double& _p, const double& _y) { R = rotFromRpy(_r,_p,_y); }
             void setRpy(const hrp::Vector3& _rpy) { R = rotFromRpy(_rpy); }
             hrp::dvector6 to_dvector6() const{ return (hrp::dvector6() << p, hrp::rpyFromRot(R)).finished(); }
-            void reset(){ p.fill(0); R.Identity(); }
+            void reset(){ p.fill(0); R.setIdentity(); }
     };
 
     inline hrp::Vector3         to_Vector3      (const RTC::Point3D& in)        { return hrp::Vector3(in.x, in.y, in.z); }
@@ -93,11 +95,13 @@ namespace hrp{
     }
     inline double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(X)*b(Y)-a(Y)*b(X); }
     inline hrp::dmatrix to_SelectionMat(const hrp::dvector& in) {
-        hrp::dmatrix ret( (in.array() == 1).count(), in.size() );
-        for (int row=0, col=0; col< in.size(); col++){
-            if(in(col) > 0.0){
-                ret(row, col) = 1;
-                row++;
+        hrp::dmatrix ret = hrp::dmatrix::Zero( (in.array() > 0.0).count(), in.size() );
+        if(ret.rows() != 0 && ret.cols() != 0){
+            for (int row=0, col=0; col< in.size(); col++){
+                if(in(col) > 0.0){
+                    ret(row, col) = 1;
+                    row++;
+                }
             }
         }
         return ret;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 0d942e44804..d4f925f1b8d 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -50,6 +50,8 @@ WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlo
         m_htrhIn("htrhIn", m_htrh),
         m_htlhIn("htlhIn", m_htlh),
         m_htheadIn("htheadIn", m_hthead),
+        m_actCPIn("actCapturePoint", m_actCP),
+        m_actZMPIn("zmp", m_actZMP),
 #ifdef USE_DEBUG_PORT
         m_htcom_dbgOut("htcom_dbgOut", m_htcom_dbg),// to ros bridge
         m_htrf_dbgOut("htrf_dbgOut", m_htrf_dbg),
@@ -101,6 +103,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     addInPort("htrhIn", m_htrhIn);
     addInPort("htlhIn", m_htlhIn);
     addInPort("htheadIn", m_htheadIn);
+    addInPort("actCapturePoint", m_actCPIn);
+    addInPort("zmp", m_actZMPIn);
 #ifdef USE_DEBUG_PORT
     addOutPort("htcom_dbgOut", m_htcom_dbgOut);// to ros bridge
     addOutPort("htrf_dbgOut", m_htrf_dbgOut);
@@ -170,6 +174,10 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     //    invdyn_zmp_filters.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
     ref_zmp_filter.setParameter(5, 1/m_dt, Q_BUTTERWORTH);
 
+    cp_flag = -1;
+    lt.fill(0);
+    rt.fill(0);
+
     RTCOUT << "onInitialize() OK" << std::endl;
     loop = 0;
     return RTC::RTC_OK;
@@ -230,6 +238,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         if (m_htlhIn.isNew())   { m_htlhIn.read();  wbms->hp_wld_raw.tgt[lh].abs    = hrp::to_Pose3(m_htlh.data);}
         if (m_htheadIn.isNew()) { m_htheadIn.read();wbms->hp_wld_raw.tgt[head].abs  = hrp::to_Pose3(m_hthead.data);}
         if (m_htzmpIn.isNew())  { m_htzmpIn.read(); wbms->hp_wld_raw.tgt[zmp].abs.p = hrp::to_Vector3(m_htzmp.data); }
+        if (m_actCPIn.isNew())  { m_actCPIn.read(); rel_act_cp = hrp::to_Vector3(m_actCP.data);}
+        if (m_actZMPIn.isNew())  { m_actZMPIn.read(); rel_act_zmp = hrp::to_Vector3(m_actZMP.data);}
     }
 
     //khi
@@ -256,6 +266,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
                 idsb.setInitState(fik->m_robot, m_dt);//逆動力学初期化
             }
 
+            fik->q_ref.head(m_qRef.data.length()) = hrp::to_dvector(m_qRef.data);//あえてseqからのbaselink poseは信用しない
+
             wbms->update();//////HumanSynchronizerの主要処理
             if(DEBUGP)RTCOUT << "update():"<<wbms->getUpdateTime()<<std::endl;
             if(DEBUGP)wbms->rp_ref_out.print();
@@ -263,7 +275,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
             solveFullbodyIK(wbms->rp_ref_out.tgt[com].abs, wbms->rp_ref_out.tgt[rf].abs, wbms->rp_ref_out.tgt[lf].abs, wbms->rp_ref_out.tgt[rh].abs, wbms->rp_ref_out.tgt[lh].abs, wbms->rp_ref_out.tgt[head].abs);
 
             if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processWholeBodyMasterSlave" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
-
             //逆動力学
             //        calcAccelerationsForInverseDynamics(m_robot, idsb);
             //        hrp::Vector3 ref_zmp_invdyn;
@@ -473,6 +484,36 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
       tmp.rot_precision = deg2rad(3);
       ikc_list.push_back(tmp);
     }
+
+    hrp::Vector3 act_cp = fik->m_robot->rootLink()->p + fik->m_robot->rootLink()->R * rel_act_cp;
+    hrp::Vector3 act_zmp = fik->m_robot->rootLink()->p + fik->m_robot->rootLink()->R * rel_act_zmp;
+    static int cp_count = 0;
+    static cp_enum state = CP_IDLE;
+
+//    if(act_zmp.head(XY).norm() > 0.1 && state == CP_IDLE){
+//        if(act_zmp(Y) > 0){
+//            state = CP_LF;
+//        }else{
+//            state = CP_RF;
+//        }
+//    }
+
+    if(state == CP_IDLE){
+        cp_count = 0;
+    }
+    else if(state == CP_LF || state == CP_RF){
+        if(cp_count ++ > 50){
+            state = CP_STATIC;
+        }
+    }
+    else if(state == CP_STATIC){
+        if(cp_count ++ > 100){
+            state = CP_IDLE;
+        }
+    }
+    const double FH = 0.05;
+
+
     if(wbms->wp.use_lower){
         IKConstraint tmp;
         tmp.target_link_name = ee_ikc_map["rleg"].target_link_name;
@@ -481,10 +522,28 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.targetPos = rf_ref.p;
         tmp.targetRpy = rf_ref.rpy();
         if(wbms->rp_ref_out.tgt[rf].is_contact){
-            tmp.constraint_weight = hrp::dvector6::Constant(10);
+            tmp.constraint_weight = hrp::dvector6::Constant(3);
         }else{
             tmp.constraint_weight = hrp::dvector6::Constant(0.1);
         }
+
+        if(state == CP_RF){
+            if(cp_count==1){
+                rt += (hrp::Vector3() << act_zmp.head(XY).normalized() * 0.1, FH).finished();
+                wbms->rp_ref_out.tgt[rf].is_contact = false;
+            }else if(cp_count==50){
+//                rt += hrp::Vector3(0,0,-FH);
+                wbms->rp_ref_out.tgt[rf].is_contact = true;
+            }
+        }
+        if(state == CP_STATIC){
+            if(rt(Z) > 0){
+                rt(Z) -= FH/450;
+            }
+        }
+        tmp.targetPos += rt;
+
+
         ikc_list.push_back(tmp);
     }
     if(wbms->wp.use_lower){
@@ -495,10 +554,28 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.targetPos = lf_ref.p;
         tmp.targetRpy = lf_ref.rpy();
         if(wbms->rp_ref_out.tgt[lf].is_contact){
-            tmp.constraint_weight = hrp::dvector6::Constant(10);
+            tmp.constraint_weight = hrp::dvector6::Constant(3);
         }else{
             tmp.constraint_weight = hrp::dvector6::Constant(0.1);
         }
+
+
+        if(state == CP_LF){
+            if(cp_count==1){
+                lt += (hrp::Vector3() << act_zmp.head(XY).normalized() * 0.1, FH).finished();
+                wbms->rp_ref_out.tgt[lf].is_contact = false;
+            }else if(cp_count==50){
+//                lt += hrp::Vector3(0,0,-FH);
+                wbms->rp_ref_out.tgt[lf].is_contact = true;
+            }
+        }
+        if(state == CP_STATIC){
+            if(lt(Z) > 0){
+                lt(Z) -= FH/50;
+            }
+        }
+        tmp.targetPos += lt;
+
         ikc_list.push_back(tmp);
     }
     const double dist = 0.0;
@@ -610,8 +687,8 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.localR = hrp::Matrix33::Identity();
         tmp.targetPos = com_ref.p;// COM height will not be constraint
         tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
-//        tmp.constraint_weight << 10,10,1,1e-6,1e-6,1e-6;
-        tmp.constraint_weight << 10,10,0.001,0,0,0;
+        tmp.constraint_weight << 3,3,0.01,0.1,0.1,0;
+//        tmp.constraint_weight << 3,3,0.01,0,0,0;
 //        if(fik_in->cur_momentum_around_COM.norm() > 1e9){
 //            tmp.constraint_weight << 10,10,1,1e-5,1e-5,0;
 //            tmp.rot_precision = 100;//angular momentum precision
@@ -626,6 +703,11 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
     if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 0.1;
     if( fik->m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_Y")->jointId) = 0.1;//K
     if( fik->m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_P")->jointId) = 0.1;
+//        if( fik->m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_Y")->jointId) = 0;//K
+//        if( fik->m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_P")->jointId) = 0;
+//    fik->q_ref_constraint_weight(fik->m_robot->link("CHEST_Y")->jointId) = 1;
+//    fik->q_ref_constraint_weight(fik->m_robot->link("CHEST_P")->jointId) = 1;
+
 //    if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->ulimit = deg2rad(-30);//脇内側の干渉回避
 //    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(30);
     if( fik->m_robot->link("RLEG_JOINT3") != NULL) fik->m_robot->link("RLEG_JOINT3")->llimit = deg2rad(10);//膝伸びきり防止のため
@@ -651,12 +733,16 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
     if( fik->m_robot->link("HEAD_Y") != NULL) fik->m_robot->link("HEAD_Y")->ulimit = deg2rad(5);
     if( fik->m_robot->link("HEAD_P") != NULL) fik->m_robot->link("HEAD_P")->llimit = deg2rad(0);
     if( fik->m_robot->link("HEAD_P") != NULL) fik->m_robot->link("HEAD_P")->ulimit = deg2rad(60);
-
     for(int i=0;i<fik->m_robot->numJoints();i++){
         LIMIT_MINMAX(fik->m_robot->joint(i)->q, fik->m_robot->joint(i)->llimit, fik->m_robot->joint(i)->ulimit);
     }
 
 //    fik_in->q_ref = init_sync_state;
+    if(fik->m_robot->name() == "RHP4B"){
+        fik->dq_weight_all.fill(0);
+        fik->dq_weight_all.head(31).fill(1);
+        fik->dq_weight_all.tail(6).fill(1);
+    }
 
 
     // std::string tmp[] = {"R_CROTCH_R","R_CROTCH_P","R_CROTCH_Y","R_KNEE_P","R_ANKLE_R","R_ANKLE_P","L_CROTCH_R","L_CROTCH_P","L_CROTCH_Y","L_KNEE_P","L_ANKLE_R","L_ANKLE_P"};
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index f501a281e2f..1e065360ea7 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -95,7 +95,10 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         RTC::InPort<OpenHRP::TimedWrench> m_htrfwIn;
         OpenHRP::TimedWrench m_htlfw;
         RTC::InPort<OpenHRP::TimedWrench> m_htlfwIn;
-
+        RTC::TimedPoint3D m_actCP;
+        RTC::InPort<RTC::TimedPoint3D> m_actCPIn;
+        RTC::TimedPoint3D m_actZMP;
+        RTC::InPort<RTC::TimedPoint3D> m_actZMPIn;
 #ifdef USE_DEBUG_PORT
     TimedPose3D m_htcom_dbg;
     RTC::OutPort<RTC::TimedPose3D> m_htcom_dbgOut;
@@ -175,6 +178,12 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         hrp::Vector3 torso_rot_rmc;
         ControlMode mode;
 
+        hrp::Vector3 rel_act_cp;
+        hrp::Vector3 rel_act_zmp;
+        int cp_flag;
+        hrp::Vector3 lt,rt;
+        enum cp_enum{ CP_IDLE, CP_LF, CP_RF, CP_STATIC};
+
         RTC::ReturnCode_t setupEEIKConstraintFromConf(std::map<std::string, IKConstraint>& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop);
         void solveFullbodyIK(const hrp::Pose3& com_ref, const hrp::Pose3& rf_ref, const hrp::Pose3& lf_ref, const hrp::Pose3& rh_ref, const hrp::Pose3& lh_ref, const hrp::Pose3& head_ref);
         void processTransition();
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 2dec64ae9ef..2ae130e4e00 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -11,6 +11,11 @@
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/imgproc/types_c.h>
 
+//#include<Eigen/StdVector>
+//EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix2d)
+//EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3d)
+//EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d)
+
 #define DEBUG 0
 
 class BiquadIIRFilterVec{
@@ -36,6 +41,7 @@ class BiquadIIRFilterVec{
 
 class PoseTGT{
     public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
         hrp::Pose3 abs, offs, cnt;
         hrp::dvector6 w;
         bool is_contact, go_contact;
@@ -193,7 +199,7 @@ class CapsuleCollisionChecker {
                     capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), m_robot->joint(i)->child->b, 0.095));
                     capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,-0.05,0), m_robot->joint(i)->child->b, 0.095));
                 }
-                if(hasStr(m_robot->joint(i)->name,"LEG_JOINT3")){
+                if(hasStr(m_robot->joint(i)->name,"LEEIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix2d)G_JOINT3")){
                     capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0.05,0,0), m_robot->joint(i)->child->b, 0.095));
                 }
                 if(hasStr(m_robot->joint(i)->name,"ARM_JOINT2") || hasStr(m_robot->joint(i)->name,"ARM_JOINT3") || hasStr(m_robot->joint(i)->name,"ARM_JOINT4") || hasStr(m_robot->joint(i)->name,"ARM_JOINT5") || hasStr(m_robot->joint(i)->name,"ARM_JOINT6")){
@@ -305,7 +311,7 @@ class WBMSCore{
         struct timeval t_calc_start, t_calc_end;
         BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters, com_in_filter;
         hrp::Vector3 com_old, com_oldold, comacc, com_CP_ref_old, r_zmp_raw;
-        std::vector<hrp::Vector2> rflf_points, hull_com, hull_dcp, hull_acp;
+        std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> > rflf_points, hull_com, hull_dcp, hull_acp;
         std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rpy_filters;
         std::vector<cv::Point2f> points,cvhull;
         HumanPose rp_ref_out_old, hp_swap_checked;
@@ -319,7 +325,7 @@ class WBMSCore{
         FILE *sr_log, *cz_log, *id_log;
         hrp::Pose3 baselinkpose;
         hrp::Vector3 com_vel_old,rh_vel_old, cp_dec, cp_acc;
-        std::vector<hrp::Vector3> foot_vert_act[LR], foot_vert_safe[LR], foot_vert_check[LR];
+        std::vector<hrp::Vector3, Eigen::aligned_allocator<hrp::Vector3> > foot_vert_act[LR], foot_vert_safe[LR], foot_vert_check[LR];
         hrp::Vector2 com_forcp_ref,com_vel_forcp_ref;
         hrp::dvector6 invdyn_ft;
         hrp::Vector4 foot_vert_act_fblr[LR], foot_vert_safe_fblr[LR], foot_vert_check_fblr[LR];
@@ -413,7 +419,7 @@ class WBMSCore{
             is_initial_loop = true;
             cout<<"WBMSCore constructed"<<endl;
         }
-        static void make_rect_verts(const hrp::Vector4& ForwardBackLeftRight, std::vector<hrp::Vector3>& out){
+        static void make_rect_verts(const hrp::Vector4& ForwardBackLeftRight, std::vector<hrp::Vector3, Eigen::aligned_allocator<hrp::Vector3> >& out){
             out.clear();
             out.push_back(hrp::Vector3(ForwardBackLeftRight(0), ForwardBackLeftRight(2),  0));//左前
             out.push_back(hrp::Vector3(ForwardBackLeftRight(1), ForwardBackLeftRight(2),  0));//左後
@@ -443,6 +449,8 @@ class WBMSCore{
                     rp_ref_out.tgt[human_l_names[i]].abs = rp_ref_out.tgt[human_l_names[i]].offs;
                 }
             }
+dbgv(rp_ref_out.tgt[rh].abs.rpy());
+dbgv(rp_ref_out.tgt[rh].offs.rpy());
             rp_ref_out.tgt[com].offs.p = act_rs.com = act_rs.zmp = robot_in->calcCM();
             com_CP_ref_old = rp_ref_out.tgt[com].offs.p;
             rp_ref_out.tgt[com].offs.R = robot_in->rootLink()->R;
@@ -473,7 +481,7 @@ class WBMSCore{
             hp_plot = rp_ref_out;
             if(wp.set_com_height_fix)rp_ref_out.tgt[com].abs.p(Z) = rp_ref_out.tgt[com].offs.p(Z) + wp.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
             lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
-            limitEEWorkspace                    (rp_ref_out);
+//            limitEEWorkspace                    (rp_ref_out);
             setFootContactPoseByGoContact       (rp_ref_out);
             if(DEBUG){ fprintf(cz_log,"com_in: %f %f ",rp_ref_out.tgt[com].abs.p(X),rp_ref_out.tgt[com].abs.p(Y)); }
             applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
@@ -488,7 +496,6 @@ class WBMSCore{
                 LIMIT_MIN( rp_ref_out.tgt[l[i]].abs.p(Z), rp_ref_out_old.tgt[l[i]].abs.p(Z)-vertical_max_vel*DT);
             }
             applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
-
             com_forcp_ref = rp_ref_out.tgt[com].abs.p.head(XY);
             static hrp::Vector2 com_forcp_ref_old;
             com_vel_forcp_ref = (com_forcp_ref - com_forcp_ref_old)/DT;
@@ -502,10 +509,9 @@ class WBMSCore{
                 fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.tgt[zmp].abs.p(X),rp_ref_out.tgt[zmp].abs.p(Y));
                 fprintf(cz_log,"\n");
             }
-            //      overwriteFootZFromFootLandOnCommand (rp_ref_out);
+//                  overwriteFootZFromFootLandOnCommand (rp_ref_out);
             modifyFootRotAndXYForContact        (rp_ref_out);
-
-            H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[rf].abs.p(Z));
+            H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[lf].abs.p(Z));
             com_vel_old = (rp_ref_out.tgt[com].abs.p - rp_ref_out_old.tgt[com].abs.p)/DT;
             rh_vel_old = (rp_ref_out.tgt[rh].abs.p - rp_ref_out_old.tgt[rh].abs.p)/DT;
             rp_ref_out_old = rp_ref_out;
@@ -564,6 +570,7 @@ class WBMSCore{
             const double MAX_FW = 0.25;
             const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
 
+            dbgv(out.tgt[lf].abs.p);
             for(int i=0, spl[LR]={rf,lf}, swl[LR]={lf,rf}; i<LR; i++){
                 PoseTGT& support_leg = out.tgt[spl[i]];
                 PoseTGT& swing_leg = out.tgt[swl[i]];
@@ -582,6 +589,7 @@ class WBMSCore{
                 swing_leg.abs.p.head(XY) = sp2sw_vec + support_leg.cnt.p.head(XY);
             }
             const double base2hand_min = 0.0;
+            dbgv(out.tgt[lf].abs.p);
             for(int i=0, l[2]={rh,lh}; i<2; i++){
 //                hrp::Vector2 horizontal_dist(out.tgt[l[i]].abs.p(X) - baselinkpose.p(X), out.tgt[l[i]].abs.p(Y) - baselinkpose.p(Y));
                 hrp::Vector2 horizontal_dist = out.tgt[l[i]].abs.p.head(XY) - baselinkpose.p.head(XY);
@@ -590,12 +598,14 @@ class WBMSCore{
                 }
                 out.tgt[l[i]].abs.p.head(XY) = baselinkpose.p.head(XY) + horizontal_dist;
             }
+            dbgv(out.tgt[lf].abs.p);
             LIMIT_MINMAX( out.tgt[com].abs.p(Z), out.tgt[com].offs.p(Z) - 0.15, out.tgt[com].offs.p(Z) + 0.03 );//COM高さ方向の制限
             //      for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( out.tgt[com].abs.rpy(i), rc.ee_rot_limit[com][MIN](i), rc.ee_rot_limit[com][MAX](i) ); }
             //      for(int i=0, l[5]={rf,lf,rh,lh,head}; i<5; i++){
             //        for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( out.tgt[l[i]].abs.rpy(j), rc.ee_rot_limit[l[i]][MIN](j) + out.tgt[com].abs.rpy(j), rc.ee_rot_limit[l[i]][MAX](j) + out.tgt[com].abs.rpy(j) ); }
             //      }
             if(!wp.use_head)out.tgt[head].abs.R = hrp::Matrix33::Identity();
+            dbgv(out.tgt[lf].abs.p);
         }
         void setFootContactPoseByGoContact(HumanPose& out){
             for(int i=0, l[LR]={rf,lf}; i<LR; i++){
@@ -649,8 +659,8 @@ class WBMSCore{
             calcFootRotAndXYTransitionForContact(out.tgt[rf],foot_vert_act[R]);
             calcFootRotAndXYTransitionForContact(out.tgt[lf],foot_vert_act[L]);
         }
-        void calcFootRotAndXYTransitionForContact(PoseTGT& foot_in, const std::vector<hrp::Vector3>& act_sole_size){
-            std::vector<hrp::Vector3> act_foot_vert;
+        void calcFootRotAndXYTransitionForContact(PoseTGT& foot_in, const std::vector<hrp::Vector3, Eigen::aligned_allocator<hrp::Vector3> >& act_sole_size){
+            std::vector<hrp::Vector3, Eigen::aligned_allocator<hrp::Vector3> > act_foot_vert;
             double min_height = foot_in.abs.p(Z);
             int min_height_id = 0;
             for(int i=0;i<act_sole_size.size();i++){
@@ -662,7 +672,7 @@ class WBMSCore{
                 }
             }
             if(act_foot_vert[min_height_id](Z) < foot_in.offs.p(Z)){
-                foot_in.abs.p(Z) += foot_in.offs.p(Z) - act_foot_vert[min_height_id](Z);
+//                foot_in.abs.p(Z) += foot_in.offs.p(Z) - act_foot_vert[min_height_id](Z);
             }
         }
         bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
@@ -702,7 +712,7 @@ class WBMSCore{
             }
             return true;
         }
-        void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2>& hull_d, const std::vector<hrp::Vector2>& hull_a, hrp::Vector2& com_vel_ans){
+        void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& hull_d, const std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& hull_a, hrp::Vector2& com_vel_ans){
             hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
             com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
             //      //減速CP条件(現在のCPを常に両足裏で頭打ち)
@@ -734,7 +744,7 @@ class WBMSCore{
 
 #define F(x) (x == R ? rf : lf)
 
-            std::vector<hrp::Vector2> f_hull_wld[LR], f_hull_wld_ans[LR];
+            std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> > f_hull_wld[LR], f_hull_wld_ans[LR];
             for(int i=0;i<foot_vert_check[R].size();i++){ f_hull_wld[R].push_back(foot_vert_check[R][i].head(XY) + rp_ref_out.tgt[F(R)].abs.p.head(XY)); }
             for(int i=0;i<foot_vert_check[L].size();i++){ f_hull_wld[L].push_back(foot_vert_check[L][i].head(XY) + rp_ref_out.tgt[F(L)].abs.p.head(XY)); }
             //      for(int i=0;i<foot_vert_safe[R].size();i++){ f_hull_wld[R].push_back(foot_vert_safe[R][i].head(XY) + rp_ref_out.tgt[F(R)].abs.p.head(XY)); }
@@ -775,7 +785,7 @@ class WBMSCore{
                 if(count[i]>0){ out.tgt[F(i)].go_contact = true; }
             }
         }
-        void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2>& hull_ans){
+        void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& hull_ans){
             rflf_points.clear();
             hull_ans.clear();
             for(int i=0;i<2;i++){
@@ -803,7 +813,7 @@ class WBMSCore{
             }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
             zmp_ans(2) = 0;
         }
-        void calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2>& hull, hrp::Vector4& margin_ans){
+        void calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& hull, hrp::Vector4& margin_ans){
             hrp::Vector4 margin_abs;
             hrp::Vector2 cross_pt, anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
             for(int direc=0;direc<4;direc++){
@@ -815,7 +825,7 @@ class WBMSCore{
             margin_ans(2) =  margin_abs(2);
             margin_ans(3) = -margin_abs(3);
         }
-        bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_will_cross){
+        bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& hull, hrp::Vector2& pt_will_cross){
             hrp::Vector2 anchor_vec = pt_out_goal - pt_in_start;
             for(int i=0;i<hull.size();i++){
                 int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
@@ -835,7 +845,7 @@ class WBMSCore{
             }
             return false;
         }
-        double calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2>& hull, hrp::Vector2& pt_ans){
+        double calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& hull, hrp::Vector2& pt_ans){
             double cur_nearest_dist, ans_nearest_dist;
             hrp::Vector2 cur_nearest_pt, ans_nearest_pt;
             for(int i=0;i<hull.size();i++){
@@ -863,7 +873,7 @@ class WBMSCore{
             pt_ans = ans_nearest_pt;
             return ans_nearest_dist;
         }
-        void makeConvexHullOpenCV(const std::vector<hrp::Vector2>& pts, std::vector<hrp::Vector2>& hull_ans){
+        void makeConvexHullOpenCV(const std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& pts, std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& hull_ans){
             points.clear();
             cvhull.clear();
             hull_ans.clear();
@@ -871,7 +881,7 @@ class WBMSCore{
             cv::convexHull(cv::Mat(points),cvhull,true);
             for(int i=0;i<cvhull.size();i++){ hull_ans.push_back( hrp::Vector2( cvhull[i].x, cvhull[i].y ) );  }
         }
-        bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2>& hull){
+        bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& hull){
             cvhull.clear();
             for(int i=0;i<hull.size();i++){ cvhull.push_back( cv::Point2f( hull[i](0), hull[i](1)) ); }
             return (cv::pointPolygonTest(cv::Mat(cvhull), cv::Point2f(pt(0),pt(1)), false) > 0);

From 4b560762b38a233ed71c400a86fa5907bbb8a1ec Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 2 May 2019 20:42:59 +0900
Subject: [PATCH 158/217] refactor

---
 idl/WholeBodyMasterSlaveService.idl           |   4 +-
 .../FullbodyInverseKinematicsSolver.h         |   2 +-
 rtc/RobotHardware/defs.h                      |   2 +-
 .../WholeBodyMasterSlave.cpp                  | 106 ++--
 rtc/WholeBodyMasterSlave/wbms_core.h          | 488 +++++++++---------
 5 files changed, 290 insertions(+), 312 deletions(-)

diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
index e2b2e145ac0..3792df90f8d 100644
--- a/idl/WholeBodyMasterSlaveService.idl
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -20,11 +20,9 @@ module OpenHRP
     struct WholeBodyMasterSlaveParam
     {
         double auto_swing_foot_landing_threshold;
-        double foot_vertical_vel_limit_coeff;
-        double human_com_height;
+        double human_to_robot_ratio;
         double set_com_height_fix_val;
         double swing_foot_height_offset;
-        double swing_foot_max_height;
         double upper_body_rmc_ratio;
         boolean set_com_height_fix;
         boolean use_head;
diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index d731108c4fd..815ce40179f 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -220,7 +220,7 @@ class FullbodyInverseKinematicsSolver{
 
             // rtconf localhost:15005/wbms.rtc set debugLevel 1 とかにしたい
             static int count;
-            if(count++ % 1000 == 0){
+            if(count++ % 10000 == 0){
 //                if(true){
                 std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
                 dbg(J_all.rows());
diff --git a/rtc/RobotHardware/defs.h b/rtc/RobotHardware/defs.h
index ccfbf4547a5..90a6d1a2fa3 100644
--- a/rtc/RobotHardware/defs.h
+++ b/rtc/RobotHardware/defs.h
@@ -14,7 +14,7 @@ enum {RL, FB};
 #define sgn(x)	(((x)>0)?1:-1)
 #define deg2rad(x)	((x)*M_PI/180)
 #define rad2deg(x)	((x)*180/M_PI)
-#define sqr(x)		((x)*(x))
+//#define sqr(x)		((x)*(x)) //　conflict with boost?
 #define LPF(dT, omega, x, y)	((y) = (((dT)*(omega)/(1+(dT)*(omega)))*(x)+1/(1+(dT)*(omega))*(y)))
 
 template<class T>
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index d4f925f1b8d..3e90e8c70c0 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -165,7 +165,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     t_ip->set(&output_ratio);
     q_ip = new interpolator(fik->numStates(), m_dt, interpolator::CUBICSPLINE, 1); // or HOFFARBIB, QUINTICSPLINE
     q_ip->clear();
-    avg_q_vel = hrp::dvector::Constant(fik->numStates(), 1.0); // all joint max avarage vel = 1.0 rad/s
+    avg_q_vel = hrp::dvector::Constant(fik->numStates(), 2.0); // all joint max avarage vel = 1.0 rad/s
     avg_q_acc = hrp::dvector::Constant(fik->numStates(), 16.0); // all joint max avarage acc = 16.0 rad/s^2
     avg_q_vel.tail(6).fill(std::numeric_limits<double>::max()); // no limit for base link vel
     avg_q_acc.tail(6).fill(std::numeric_limits<double>::max()); // no limit for base link acc
@@ -497,21 +497,21 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
 //            state = CP_RF;
 //        }
 //    }
-
-    if(state == CP_IDLE){
-        cp_count = 0;
-    }
-    else if(state == CP_LF || state == CP_RF){
-        if(cp_count ++ > 50){
-            state = CP_STATIC;
-        }
-    }
-    else if(state == CP_STATIC){
-        if(cp_count ++ > 100){
-            state = CP_IDLE;
-        }
-    }
-    const double FH = 0.05;
+//
+//    if(state == CP_IDLE){
+//        cp_count = 0;
+//    }
+//    else if(state == CP_LF || state == CP_RF){
+//        if(cp_count ++ > 50){
+//            state = CP_STATIC;
+//        }
+//    }
+//    else if(state == CP_STATIC){
+//        if(cp_count ++ > 100){
+//            state = CP_IDLE;
+//        }
+//    }
+//    const double FH = 0.05;
 
 
     if(wbms->wp.use_lower){
@@ -527,21 +527,21 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
             tmp.constraint_weight = hrp::dvector6::Constant(0.1);
         }
 
-        if(state == CP_RF){
-            if(cp_count==1){
-                rt += (hrp::Vector3() << act_zmp.head(XY).normalized() * 0.1, FH).finished();
-                wbms->rp_ref_out.tgt[rf].is_contact = false;
-            }else if(cp_count==50){
-//                rt += hrp::Vector3(0,0,-FH);
-                wbms->rp_ref_out.tgt[rf].is_contact = true;
-            }
-        }
-        if(state == CP_STATIC){
-            if(rt(Z) > 0){
-                rt(Z) -= FH/450;
-            }
-        }
-        tmp.targetPos += rt;
+//        if(state == CP_RF){
+//            if(cp_count==1){
+//                rt += (hrp::Vector3() << act_zmp.head(XY).normalized() * 0.1, FH).finished();
+//                wbms->rp_ref_out.tgt[rf].is_contact = false;
+//            }else if(cp_count==50){
+////                rt += hrp::Vector3(0,0,-FH);
+//                wbms->rp_ref_out.tgt[rf].is_contact = true;
+//            }
+//        }
+//        if(state == CP_STATIC){
+//            if(rt(Z) > 0){
+//                rt(Z) -= FH/450;
+//            }
+//        }
+//        tmp.targetPos += rt;
 
 
         ikc_list.push_back(tmp);
@@ -559,22 +559,22 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
             tmp.constraint_weight = hrp::dvector6::Constant(0.1);
         }
 
-
-        if(state == CP_LF){
-            if(cp_count==1){
-                lt += (hrp::Vector3() << act_zmp.head(XY).normalized() * 0.1, FH).finished();
-                wbms->rp_ref_out.tgt[lf].is_contact = false;
-            }else if(cp_count==50){
-//                lt += hrp::Vector3(0,0,-FH);
-                wbms->rp_ref_out.tgt[lf].is_contact = true;
-            }
-        }
-        if(state == CP_STATIC){
-            if(lt(Z) > 0){
-                lt(Z) -= FH/50;
-            }
-        }
-        tmp.targetPos += lt;
+//
+//        if(state == CP_LF){
+//            if(cp_count==1){
+//                lt += (hrp::Vector3() << act_zmp.head(XY).normalized() * 0.1, FH).finished();
+//                wbms->rp_ref_out.tgt[lf].is_contact = false;
+//            }else if(cp_count==50){
+////                lt += hrp::Vector3(0,0,-FH);
+//                wbms->rp_ref_out.tgt[lf].is_contact = true;
+//            }
+//        }
+//        if(state == CP_STATIC){
+//            if(lt(Z) > 0){
+//                lt(Z) -= FH/50;
+//            }
+//        }
+//        tmp.targetPos += lt;
 
         ikc_list.push_back(tmp);
     }
@@ -687,8 +687,8 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.localR = hrp::Matrix33::Identity();
         tmp.targetPos = com_ref.p;// COM height will not be constraint
         tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
-        tmp.constraint_weight << 3,3,0.01,0.1,0.1,0;
-//        tmp.constraint_weight << 3,3,0.01,0,0,0;
+//        tmp.constraint_weight << 3,3,0.01,0.1,0.1,0;
+        tmp.constraint_weight << 3,3,0.01,0,0,0;
 //        if(fik_in->cur_momentum_around_COM.norm() > 1e9){
 //            tmp.constraint_weight << 10,10,1,1e-5,1e-5,0;
 //            tmp.rot_precision = 100;//angular momentum precision
@@ -884,12 +884,10 @@ bool WholeBodyMasterSlave::stopWholeBodyMasterSlave(){
 bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
     RTCOUT << "setWholeBodyMasterSlaveParam" << std::endl;
     wbms->wp.auto_swing_foot_landing_threshold = i_param.auto_swing_foot_landing_threshold;
-    wbms->wp.foot_vertical_vel_limit_coeff = i_param.foot_vertical_vel_limit_coeff;
-    wbms->wp.human_com_height = i_param.human_com_height;
+    wbms->wp.human_to_robot_ratio = i_param.human_to_robot_ratio;
     wbms->wp.set_com_height_fix = i_param.set_com_height_fix;
     wbms->wp.set_com_height_fix_val = i_param.set_com_height_fix_val;
     wbms->wp.swing_foot_height_offset = i_param.swing_foot_height_offset;
-    wbms->wp.swing_foot_max_height = i_param.swing_foot_max_height;
     wbms->wp.upper_body_rmc_ratio = i_param.upper_body_rmc_ratio;
     if(mode.now() == MODE_IDLE){
         wbms->wp.use_head = i_param.use_head;
@@ -905,12 +903,10 @@ bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBody
 bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
     RTCOUT << "getWholeBodyMasterSlaveParam" << std::endl;
     i_param.auto_swing_foot_landing_threshold = wbms->wp.auto_swing_foot_landing_threshold;
-    i_param.foot_vertical_vel_limit_coeff = wbms->wp.foot_vertical_vel_limit_coeff;
-    i_param.human_com_height = wbms->wp.human_com_height;
+    i_param.human_to_robot_ratio = wbms->wp.human_to_robot_ratio;
     i_param.set_com_height_fix = wbms->wp.set_com_height_fix;
     i_param.set_com_height_fix_val = wbms->wp.set_com_height_fix_val;
     i_param.swing_foot_height_offset = wbms->wp.swing_foot_height_offset;
-    i_param.swing_foot_max_height = wbms->wp.swing_foot_max_height;
     i_param.upper_body_rmc_ratio = wbms->wp.upper_body_rmc_ratio;
     i_param.use_head = wbms->wp.use_head;
     i_param.use_upper = wbms->wp.use_upper;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 2ae130e4e00..5ad8940a6d6 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -6,16 +6,27 @@
 #include <hrpCollision/DistFuncs.h>
 #include "../ImpedanceController/JointPathEx.h"
 #include "../AutoBalancer/FullbodyInverseKinematicsSolver.h"
-// geometory
+// geometry
 #include <opencv2/core/core.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/imgproc/types_c.h>
+#include <opencv2/core/eigen.hpp>
+
+// geometry
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+#include <boost/geometry/geometries/point_xy.hpp>
+#include <boost/assign/list_of.hpp>
 
 //#include<Eigen/StdVector>
 //EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix2d)
 //EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3d)
 //EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d)
 
+
+#define F(x) (x == R ? rf : lf)
+
+
 #define DEBUG 0
 
 class BiquadIIRFilterVec{
@@ -307,13 +318,11 @@ class CapsuleCollisionChecker {
 
 class WBMSCore{
     private:
-        double CNT_F_TH, h2r_ratio, tgt_h2r_ratio, HZ, DT;
+        double HZ, DT;
         struct timeval t_calc_start, t_calc_end;
         BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters, com_in_filter;
         hrp::Vector3 com_old, com_oldold, comacc, com_CP_ref_old, r_zmp_raw;
-        std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> > rflf_points, hull_com, hull_dcp, hull_acp;
         std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rpy_filters;
-        std::vector<cv::Point2f> points,cvhull;
         HumanPose rp_ref_out_old, hp_swap_checked;
         unsigned int loop;
         bool is_initial_loop;
@@ -324,19 +333,17 @@ class WBMSCore{
         HumanPose hp_wld_raw, hp_plot, rp_ref_out, rp_ref_vel_old;
         FILE *sr_log, *cz_log, *id_log;
         hrp::Pose3 baselinkpose;
-        hrp::Vector3 com_vel_old,rh_vel_old, cp_dec, cp_acc;
-        std::vector<hrp::Vector3, Eigen::aligned_allocator<hrp::Vector3> > foot_vert_act[LR], foot_vert_safe[LR], foot_vert_check[LR];
+        hrp::Vector3 cp_dec, cp_acc;
+        hrp::dmatrix foot_vert3d_act[LR], foot_vert3d_check[LR]; // A = [p0 p1 p2 p3]
         hrp::Vector2 com_forcp_ref,com_vel_forcp_ref;
         hrp::dvector6 invdyn_ft;
         hrp::Vector4 foot_vert_act_fblr[LR], foot_vert_safe_fblr[LR], foot_vert_check_fblr[LR];
         struct WBMSparameters {
                 double auto_swing_foot_landing_threshold;
-                double foot_vertical_vel_limit_coeff;
-                double human_com_height;
+                double human_to_robot_ratio;
                 bool set_com_height_fix;
                 double set_com_height_fix_val;
                 double swing_foot_height_offset;
-                double swing_foot_max_height;
                 double upper_body_rmc_ratio;
                 bool use_head;
                 bool use_upper;
@@ -347,20 +354,13 @@ class WBMSCore{
                 hrp::Vector3 com, zmp;
         }act_rs;
 
-        hrp::Vector2 cp_acc_old;
-
         WBMSCore(const double& dt){
-            tgt_h2r_ratio = h2r_ratio = 0.96;//human 1.1m vs jaxon 1.06m
-            //      tgt_h2r_ratio = h2r_ratio = 1.0;
             DT = dt;
             HZ = (int)(1.0/DT);
-            CNT_F_TH = 20.0;
             loop = 0;
             //////////  hrp::Vector は初期化必要  ///////////
             com_old = com_oldold = comacc = hrp::Vector3::Zero();
             r_zmp_raw = hrp::Vector3::Zero();
-            com_vel_old = hrp::Vector3::Zero();
-            rh_vel_old = hrp::Vector3::Zero();
             cp_dec = cp_acc = hrp::Vector3::Zero();
             invdyn_ft = hrp::dvector6::Zero();
             tgt_pos_filters.resize(num_pose_tgt);
@@ -378,26 +378,20 @@ class WBMSCore{
             foot_vert_act_fblr[L] << 0.13, -0.10,  0.08, -0.06;
             foot_vert_check_fblr[R] << 0.02+0.002, -0.01-0.002,  0.02+0.002,  0.01-0.002;
             foot_vert_check_fblr[L] << 0.02+0.002, -0.01-0.002, -0.01+0.002, -0.02-0.002;
-            //      foot_vert_check_fblr[R] << 0.13, -0.10,  0.06, -0.08;
-            //      foot_vert_check_fblr[L] << 0.13, -0.10,  0.08, -0.06;
             foot_vert_safe_fblr[R] << 0.02, -0.01,  0.02,  0.01;
             foot_vert_safe_fblr[L] << 0.02, -0.01, -0.01, -0.02;
             for(int i=0;i<LR;i++){
-                make_rect_verts(foot_vert_act_fblr[i], foot_vert_act[i]);
-                make_rect_verts(foot_vert_check_fblr[i], foot_vert_check[i]);
-                make_rect_verts(foot_vert_safe_fblr[i], foot_vert_safe[i]);
+                foot_vert3d_act[i]      = make_rect_3d(foot_vert_act_fblr[i]);
+                foot_vert3d_check[i]    = make_rect_3d(foot_vert_check_fblr[i]);
             }
             cp_force_go_contact[R] = false;
             cp_force_go_contact[L] = false;
 
-            //      WBMSparam.auto_swing_foot_landing_threshold = 0.02;
             wp.auto_swing_foot_landing_threshold = 0.04;
-            wp.foot_vertical_vel_limit_coeff = 4.0;
-            wp.human_com_height = 1.00;
+            wp.human_to_robot_ratio = 1.0;//human 1.1m vs jaxon 1.06m
             wp.set_com_height_fix = false;
             wp.set_com_height_fix_val = 0.02;
             wp.swing_foot_height_offset = 0.02;
-            wp.swing_foot_max_height = 0.5;
             wp.upper_body_rmc_ratio = 0.0;
             wp.use_head = true;
             wp.use_upper = true;
@@ -405,12 +399,6 @@ class WBMSCore{
             rp_ref_out_old.reset();
             rp_ref_out.reset();
             rp_ref_vel_old.reset();
-            rflf_points.reserve(8);//あらかじめreserveして高速化
-            points.reserve(8);
-            hull_com.reserve(6);
-            hull_dcp.reserve(6);
-            hull_dcp.reserve(6);
-            cvhull.reserve(6);
             if(DEBUG){
                 sr_log = fopen("/home/ishiguro/HumanSync_support_region.log","w+");
                 cz_log = fopen("/home/ishiguro/HumanSync_com_zmp.log","w+");
@@ -419,12 +407,12 @@ class WBMSCore{
             is_initial_loop = true;
             cout<<"WBMSCore constructed"<<endl;
         }
-        static void make_rect_verts(const hrp::Vector4& ForwardBackLeftRight, std::vector<hrp::Vector3, Eigen::aligned_allocator<hrp::Vector3> >& out){
-            out.clear();
-            out.push_back(hrp::Vector3(ForwardBackLeftRight(0), ForwardBackLeftRight(2),  0));//左前
-            out.push_back(hrp::Vector3(ForwardBackLeftRight(1), ForwardBackLeftRight(2),  0));//左後
-            out.push_back(hrp::Vector3(ForwardBackLeftRight(1), ForwardBackLeftRight(3),  0));//右後
-            out.push_back(hrp::Vector3(ForwardBackLeftRight(0), ForwardBackLeftRight(3),  0));//右前
+        hrp::dmatrix make_rect_3d(const hrp::Vector4& ForwardBackLeftRight){
+            return (hrp::dmatrix(3,4) << // (XYZ,4)とかの#defineマクロ効かない
+                hrp::Vector3(ForwardBackLeftRight(0), ForwardBackLeftRight(2), 0),//左前
+                hrp::Vector3(ForwardBackLeftRight(1), ForwardBackLeftRight(2), 0),//左後
+                hrp::Vector3(ForwardBackLeftRight(1), ForwardBackLeftRight(3), 0),//右後
+                hrp::Vector3(ForwardBackLeftRight(0), ForwardBackLeftRight(3), 0)).finished();//右前
         }
         ~WBMSCore(){
             if(DEBUG)fclose(sr_log);
@@ -449,8 +437,6 @@ class WBMSCore{
                     rp_ref_out.tgt[human_l_names[i]].abs = rp_ref_out.tgt[human_l_names[i]].offs;
                 }
             }
-dbgv(rp_ref_out.tgt[rh].abs.rpy());
-dbgv(rp_ref_out.tgt[rh].offs.rpy());
             rp_ref_out.tgt[com].offs.p = act_rs.com = act_rs.zmp = robot_in->calcCM();
             com_CP_ref_old = rp_ref_out.tgt[com].offs.p;
             rp_ref_out.tgt[com].offs.R = robot_in->rootLink()->R;
@@ -475,45 +461,29 @@ dbgv(rp_ref_out.tgt[rh].offs.rpy());
         }
         void update(){//////////  メインループ  ////////////
             gettimeofday(&t_calc_start, NULL);
-            updateParams                        ();
-            autoLRSwapCheck                     (hp_wld_raw,hp_swap_checked);//入力の左右反転を常にチェック(＝手足の交差は不可能)
-            convertRelHumanPoseToRelRobotPose   (hp_swap_checked, rp_ref_out);
+            autoLRSwapCheck                     (hp_wld_raw, hp_swap_checked);//入力の左右反転を常にチェック(＝手足の交差は不可能)(動作中に入れ替わると余計なお世話かも)
+            convertHumanToRobot                 (hp_swap_checked, rp_ref_out);
             hp_plot = rp_ref_out;
-            if(wp.set_com_height_fix)rp_ref_out.tgt[com].abs.p(Z) = rp_ref_out.tgt[com].offs.p(Z) + wp.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
+            if(wp.set_com_height_fix) rp_ref_out.tgt[com].abs.p(Z) = rp_ref_out.tgt[com].offs.p(Z) + wp.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
             lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
-//            limitEEWorkspace                    (rp_ref_out);
+            limitEEWorkspace                    (rp_ref_out);
             setFootContactPoseByGoContact       (rp_ref_out);
-            if(DEBUG){ fprintf(cz_log,"com_in: %f %f ",rp_ref_out.tgt[com].abs.p(X),rp_ref_out.tgt[com].abs.p(Y)); }
-            applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
-
+            applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, rp_ref_out.tgt[com].abs.p);
             overwriteFootZFromFootLandOnCommand (rp_ref_out);
             setFootRotHorizontalIfGoLanding     (rp_ref_out);
-            for(int i=0, l[2]={rf,lf}; i<2; i++){
-                double fheight = rp_ref_out_old.tgt[l[i]].abs.p(Z) - rp_ref_out_old.tgt[l[i]].offs.p(Z);
-                double horizontal_max_vel = 0 + fheight * 20;
-                for(int j=0;j<XY;j++)LIMIT_MINMAX( rp_ref_out.tgt[l[i]].abs.p(j), rp_ref_out_old.tgt[l[i]].abs.p(j)-horizontal_max_vel*DT, rp_ref_out_old.tgt[l[i]].abs.p(j)+horizontal_max_vel*DT);
-                double vertical_max_vel = 0 + fheight * 10;
-                LIMIT_MIN( rp_ref_out.tgt[l[i]].abs.p(Z), rp_ref_out_old.tgt[l[i]].abs.p(Z)-vertical_max_vel*DT);
-            }
-            applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
+            limitFootVelNearGround              (rp_ref_out);
+            applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
             com_forcp_ref = rp_ref_out.tgt[com].abs.p.head(XY);
             static hrp::Vector2 com_forcp_ref_old;
             com_vel_forcp_ref = (com_forcp_ref - com_forcp_ref_old)/DT;
             com_forcp_ref_old = com_forcp_ref;
-            applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
-            applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs.p, rp_ref_out.tgt[lf].abs.p, rp_ref_out.tgt[com].abs.p);
+            applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
+            applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, rp_ref_out.tgt[com].abs.p);
             applyLPFilter_post                       (rp_ref_out);
             r_zmp_raw = rp_ref_out.tgt[zmp].abs.p;
             applyZMPCalcFromCOM                 (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);
-            if(DEBUG){
-                fprintf(cz_log,"com_ans_zmp: %f %f ",rp_ref_out.tgt[zmp].abs.p(X),rp_ref_out.tgt[zmp].abs.p(Y));
-                fprintf(cz_log,"\n");
-            }
-//                  overwriteFootZFromFootLandOnCommand (rp_ref_out);
             modifyFootRotAndXYForContact        (rp_ref_out);
             H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[lf].abs.p(Z));
-            com_vel_old = (rp_ref_out.tgt[com].abs.p - rp_ref_out_old.tgt[com].abs.p)/DT;
-            rh_vel_old = (rp_ref_out.tgt[rh].abs.p - rp_ref_out_old.tgt[rh].abs.p)/DT;
             rp_ref_out_old = rp_ref_out;
             loop++;
             is_initial_loop = false;
@@ -521,25 +491,16 @@ dbgv(rp_ref_out.tgt[rh].offs.rpy());
         }
 
     private:
-        void updateParams(){
-            updateHumanToRobotRatio(tgt_h2r_ratio);
-        }
-        void updateHumanToRobotRatio(const double h2r_r_goal){//h2r_ratioに伴って変わる変数の処理もここに書く
-            const double step = 0.001;
-            if(h2r_r_goal - h2r_ratio > step){h2r_ratio += step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
-            else if(h2r_r_goal - h2r_ratio < (-1)*step){h2r_ratio -= step; cout<<"Interpolating HumanToRobotRatio as "<<h2r_ratio<<endl;}
-            else{h2r_ratio = h2r_r_goal;}
-        }
         void autoLRSwapCheck(const HumanPose& in, HumanPose& out){
             out = in;
             if(in.tgt[rh].offs.p(Y) > in.tgt[lh].offs.p(Y) ){ out.tgt[rh] = in.tgt[lh]; out.tgt[lh] = in.tgt[rh]; }
             if(in.tgt[rf].offs.p(Y) > in.tgt[lf].offs.p(Y) ){ out.tgt[rf] = in.tgt[lf]; out.tgt[lf] = in.tgt[rf]; }
         }
-        void convertRelHumanPoseToRelRobotPose(const HumanPose& in, HumanPose& out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
+        void convertHumanToRobot(const HumanPose& in, HumanPose& out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
             //      out = in;//ダメゼッタイ
             for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
-                out.tgt[l[i]].abs.p   =  h2r_ratio * (in.tgt[l[i]].abs.p - in.tgt[l[i]].offs.p)   + out.tgt[l[i]].offs.p;
-                out.tgt[l[i]].abs.R =  in.tgt[l[i]].abs.R * in.tgt[l[i]].offs.R.transpose() * out.tgt[l[i]].offs.R;
+                out.tgt[l[i]].abs.p = wp.human_to_robot_ratio * (in.tgt[l[i]].abs.p - in.tgt[l[i]].offs.p) + out.tgt[l[i]].offs.p;
+                out.tgt[l[i]].abs.R = in.tgt[l[i]].abs.R * in.tgt[l[i]].offs.R.transpose() * out.tgt[l[i]].offs.R;
             }
             for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
                 out.tgt[l[i]].w = in.tgt[l[i]].w;
@@ -547,36 +508,21 @@ dbgv(rp_ref_out.tgt[rh].offs.rpy());
             }
             out.tgt[zmp].abs = in.tgt[zmp].abs;//最近使わない
         }
-        void applyLPFilter_post(HumanPose& tgt){
-            if(is_initial_loop){
-                for(int i=0, l[1]={com}; i<1; i++){
-                    tgt_pos_filters[l[i]].reset(tgt.tgt[l[i]].abs.p);
-                    tgt_rpy_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy());
-                }
-            }
-            for(int i=0, l[1]={com}; i<1; i++){
-                tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
-                tgt.tgt[l[i]].abs.setRpy(tgt_rpy_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy()));
-            }
-        }
         void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
-            hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);//TODO implement
+            hrp::Vector3 inside_vec_rf = old.tgt[rf].abs.R * hrp::Vector3(0,1,0);
+            hrp::Vector3 inside_vec_lf = old.tgt[lf].abs.R * hrp::Vector3(0,-1,0);
             hrp::Vector2 rf2zmp = act_rs.zmp.head(XY) - old.tgt[rf].abs.p.head(XY);
             hrp::Vector2 lf2zmp = act_rs.zmp.head(XY) - old.tgt[lf].abs.p.head(XY);
-            if( inside_vec_rf.dot(rf2zmp) / inside_vec_rf.norm() > wp.auto_swing_foot_landing_threshold ){ out.tgt[lf].go_contact = true; }
-            if( inside_vec_lf.dot(lf2zmp) / inside_vec_lf.norm() > wp.auto_swing_foot_landing_threshold ){ out.tgt[rf].go_contact = true; }
+            if( inside_vec_rf.head(XY).dot(rf2zmp) / inside_vec_rf.norm() > wp.auto_swing_foot_landing_threshold ){ out.tgt[lf].go_contact = true; }//2D,3D混ざって微妙
+            if( inside_vec_lf.head(XY).dot(lf2zmp) / inside_vec_lf.norm() > wp.auto_swing_foot_landing_threshold ){ out.tgt[rf].go_contact = true; }
         }
         void limitEEWorkspace(HumanPose& out){
             const double MAX_FW = 0.25;
             const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
-
-            dbgv(out.tgt[lf].abs.p);
             for(int i=0, spl[LR]={rf,lf}, swl[LR]={lf,rf}; i<LR; i++){
                 PoseTGT& support_leg = out.tgt[spl[i]];
                 PoseTGT& swing_leg = out.tgt[swl[i]];
                 hrp::Vector2 inside_vec_baserel = ( spl[i]==rf ? hrp::Vector2(0,+1) : hrp::Vector2(0,-1) );
-
-//                hrp::Vector2 sp2sw_vec( swing_leg.abs.p(X) - support_leg.cnt.p(X), swing_leg.abs.p(Y) - support_leg.cnt.p(Y) );
                 hrp::Vector2 sp2sw_vec = swing_leg.abs.p.head(XY) - support_leg.cnt.p.head(XY);
                 if(swing_leg.go_contact && sp2sw_vec.norm() > MAX_FW){ sp2sw_vec = sp2sw_vec.normalized() * MAX_FW; }//着地時に足を広げすぎないよう制限
                 Eigen::Matrix2d base_rot;
@@ -589,23 +535,13 @@ dbgv(rp_ref_out.tgt[rh].offs.rpy());
                 swing_leg.abs.p.head(XY) = sp2sw_vec + support_leg.cnt.p.head(XY);
             }
             const double base2hand_min = 0.0;
-            dbgv(out.tgt[lf].abs.p);
             for(int i=0, l[2]={rh,lh}; i<2; i++){
-//                hrp::Vector2 horizontal_dist(out.tgt[l[i]].abs.p(X) - baselinkpose.p(X), out.tgt[l[i]].abs.p(Y) - baselinkpose.p(Y));
                 hrp::Vector2 horizontal_dist = out.tgt[l[i]].abs.p.head(XY) - baselinkpose.p.head(XY);
                 if(horizontal_dist.norm() < base2hand_min){
                     horizontal_dist = base2hand_min * horizontal_dist.normalized();
                 }
                 out.tgt[l[i]].abs.p.head(XY) = baselinkpose.p.head(XY) + horizontal_dist;
             }
-            dbgv(out.tgt[lf].abs.p);
-            LIMIT_MINMAX( out.tgt[com].abs.p(Z), out.tgt[com].offs.p(Z) - 0.15, out.tgt[com].offs.p(Z) + 0.03 );//COM高さ方向の制限
-            //      for(int i=0;i<XYZ;i++){ LIMIT_MINMAX( out.tgt[com].abs.rpy(i), rc.ee_rot_limit[com][MIN](i), rc.ee_rot_limit[com][MAX](i) ); }
-            //      for(int i=0, l[5]={rf,lf,rh,lh,head}; i<5; i++){
-            //        for(int j=0;j<XYZ;j++){ LIMIT_MINMAX( out.tgt[l[i]].abs.rpy(j), rc.ee_rot_limit[l[i]][MIN](j) + out.tgt[com].abs.rpy(j), rc.ee_rot_limit[l[i]][MAX](j) + out.tgt[com].abs.rpy(j) ); }
-            //      }
-            if(!wp.use_head)out.tgt[head].abs.R = hrp::Matrix33::Identity();
-            dbgv(out.tgt[lf].abs.p);
         }
         void setFootContactPoseByGoContact(HumanPose& out){
             for(int i=0, l[LR]={rf,lf}; i<LR; i++){
@@ -619,19 +555,14 @@ dbgv(rp_ref_out.tgt[rh].offs.rpy());
                 }
             }
         }
-        void setFootRotHorizontalIfGoLanding(HumanPose& out){
-            if( out.tgt[rf].go_contact){
-//                out.tgt[rf].abs.rpy(r) = out.tgt[rf].abs.rpy(p) = 0;
-                out.tgt[rf].abs.setRpy(0,0,out.tgt[rf].abs.rpy()(y));
-            }
-            if( out.tgt[lf].go_contact){
-//                out.tgt[lf].abs.rpy(r) = out.tgt[lf].abs.rpy(p) = 0;
-                out.tgt[lf].abs.setRpy(0,0,out.tgt[lf].abs.rpy()(y));
-            }
+        bool applyCOMToSupportRegionLimit(const hrp::Pose3& rfin_abs, const hrp::Pose3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
+            hrp::dmatrix hull_com = createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R], foot_vert_safe_fblr[L]);
+            hrp::Vector2 cog = comin_abs.head(XY);
+            if(!isPointInHull2DOpenCV(cog,hull_com)){ calcNearestPointOnHull(cog, hull_com, cog); }//外に出たら最近傍点に頭打ち
+            comin_abs.head(XY) = cog.head(XY);
+            return true;
         }
         void overwriteFootZFromFootLandOnCommand(HumanPose& out){
-            //      limitGroundContactVelocity(out.tgt[rf].go_contact, rp_ref_out_old.tgt[rf], out.tgt[rf], out.tgt[rf].is_contact);
-            //      limitGroundContactVelocity(out.tgt[lf].go_contact, rp_ref_out_old.tgt[lf], out.tgt[lf], out.tgt[lf].is_contact);
             for(int i=0, l[LR]={rf,lf}; i<LR; i++){
                 if(out.tgt[l[i]].go_contact || cp_force_go_contact[i]){
                     out.tgt[l[i]].abs.p(Z) = out.tgt[l[i]].offs.p(Z);
@@ -642,86 +573,55 @@ dbgv(rp_ref_out.tgt[rh].offs.rpy());
                 out.tgt[l[i]].is_contact = ((out.tgt[l[i]].abs.p(Z)-out.tgt[l[i]].offs.p(Z)) <= contact_threshold);
             }
         }
-        //    void limitGroundContactVelocity(const bool& go_land_in, const PoseTGT& f_old_in, PoseTGT& f_in_out, bool& is_f_contact_out){
-        //      LIMIT_MIN( f_in_out.abs.p(Z), f_in_out.offs.p(Z));
-        //      double input_vel = (f_in_out.abs.p(Z) - f_old_in.abs.p(Z)) / DT;
-        //      const double vel_limit_k = WBMSparam.foot_vertical_vel_limit_coeff;//地面から0.02[m]地点で0.02*8=0.16[m/s]出ている計算
-        //      double limit_vel = - vel_limit_k * (f_old_in.abs.p(Z) - f_old_in.offs.p(Z));//sinの時は平均0.05~0.10[m/s]
-        //      const double max_vel_threshold = -0.01;
-        //      LIMIT_MAX( limit_vel, max_vel_threshold);//速度0に近づくと目標になかなか到達しないから
-        //      LIMIT_MIN( input_vel, limit_vel);//着地時の下向きの速度を制限
-        //      f_in_out.abs.p(Z) = f_old_in.abs.p(Z) + input_vel * DT;
-        //      LIMIT_MIN( f_in_out.abs.p(Z), f_in_out.offs.p(Z));
-        //      const double contact_threshold = 0.005;
-        //      is_f_contact_out = ((f_in_out.abs.p(Z)-f_in_out.offs.p(Z)) <= contact_threshold);
-        //    }
-        void modifyFootRotAndXYForContact(HumanPose& out){
-            calcFootRotAndXYTransitionForContact(out.tgt[rf],foot_vert_act[R]);
-            calcFootRotAndXYTransitionForContact(out.tgt[lf],foot_vert_act[L]);
-        }
-        void calcFootRotAndXYTransitionForContact(PoseTGT& foot_in, const std::vector<hrp::Vector3, Eigen::aligned_allocator<hrp::Vector3> >& act_sole_size){
-            std::vector<hrp::Vector3, Eigen::aligned_allocator<hrp::Vector3> > act_foot_vert;
-            double min_height = foot_in.abs.p(Z);
-            int min_height_id = 0;
-            for(int i=0;i<act_sole_size.size();i++){
-                act_foot_vert.push_back(foot_in.abs.R * act_sole_size[i]);
-                act_foot_vert[i](Z) += foot_in.abs.p(Z);
-                if(act_foot_vert[i](Z) < min_height){
-                    min_height = act_foot_vert[i](Z);
-                    min_height_id = i;
-                }
+        void setFootRotHorizontalIfGoLanding(HumanPose& out){
+            if( out.tgt[rf].go_contact){
+                out.tgt[rf].abs.setRpy(0,0,out.tgt[rf].abs.rpy()(y));
             }
-            if(act_foot_vert[min_height_id](Z) < foot_in.offs.p(Z)){
-//                foot_in.abs.p(Z) += foot_in.offs.p(Z) - act_foot_vert[min_height_id](Z);
+            if( out.tgt[lf].go_contact){
+                out.tgt[lf].abs.setRpy(0,0,out.tgt[lf].abs.rpy()(y));
             }
         }
-        bool applyCOMToSupportRegionLimit(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
-            hull_com.clear();
-            createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R], foot_vert_safe_fblr[L], hull_com);
-            hrp::Vector2 cog = comin_abs.head(XY);
-            if(!isPointInHullOpenCV(cog,hull_com)){ calcNearestPointOnHull(cog,hull_com,cog); }//外に出たら最近傍点に頭打ち
-            comin_abs.head(XY) = cog.head(XY);
-            return true;
+        void limitFootVelNearGround(HumanPose& out){
+            for(int i=0, l[2]={rf,lf}; i<2; i++){
+                double fheight = rp_ref_out_old.tgt[l[i]].abs.p(Z) - rp_ref_out_old.tgt[l[i]].offs.p(Z);
+                double horizontal_max_vel = 0 + fheight * 20;
+                for(int j=0; j<XY; j++){
+                    LIMIT_MINMAX( out.tgt[l[i]].abs.p(j), rp_ref_out_old.tgt[l[i]].abs.p(j) - horizontal_max_vel * DT, rp_ref_out_old.tgt[l[i]].abs.p(j) + horizontal_max_vel * DT);
+                }
+                double vertical_max_vel = 0 + fheight * 10;
+                LIMIT_MIN( out.tgt[l[i]].abs.p(Z), rp_ref_out_old.tgt[l[i]].abs.p(Z) - vertical_max_vel * DT);
+            }
         }
-        bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, hrp::Vector3& com_ans_old, hrp::Vector3& com_ans){
+        void modifyFootRotAndXYForContact(HumanPose& out){
+            avoidFootSinkIntoFloor(out.tgt[rf],foot_vert3d_act[R]);
+            avoidFootSinkIntoFloor(out.tgt[lf],foot_vert3d_act[L]);
+        }
+        void avoidFootSinkIntoFloor(PoseTGT& foot_in, const hrp::dmatrix& act_sole_vert){
+            hrp::dmatrix act_sole_vert_abs = (foot_in.abs.R * act_sole_vert).colwise() + foot_in.abs.p;// translate and rotate each cols
+            double min_height = act_sole_vert_abs.row(Z).minCoeff(); // pick up min Z height
+            if(min_height < foot_in.offs.p(Z)){
+                foot_in.abs.p(Z) += (foot_in.offs.p(Z) - min_height);
+            }
+        }
+        bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Pose3& rfin_abs, const hrp::Pose3& lfin_abs, hrp::Vector3& com_ans_old, hrp::Vector3& com_ans){
             com_ans = com_in;
             if (is_initial_loop)com_ans_old = com_in;
             hrp::Vector3 com_vel = (com_in - com_ans_old)/DT;
-            hull_dcp.clear();
-            hull_acp.clear();
             hrp::Vector4 marginDelta_for_dcp(+0.001,-0.001,0.001,-0.001);
             hrp::Vector4 marginDelta_for_acp(+0.010,-0.010,0.010,-0.010);
-            createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R]+marginDelta_for_dcp, foot_vert_safe_fblr[L]+marginDelta_for_dcp, hull_dcp);
-            createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R]+marginDelta_for_acp, foot_vert_safe_fblr[L]+marginDelta_for_acp, hull_acp);
+            hrp::dmatrix hull_dcp = createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R]+marginDelta_for_dcp, foot_vert_safe_fblr[L]+marginDelta_for_dcp);
+            hrp::dmatrix hull_acp = createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R]+marginDelta_for_acp, foot_vert_safe_fblr[L]+marginDelta_for_acp);
             hrp::Vector2 com_vel_ans_2d;
-//            regulateCOMVelocityByCapturePointVec( hrp::Vector2(com_ans_old(X),com_ans_old(Y)), hrp::Vector2(com_vel(X),com_vel(Y)), hull_dcp, hull_acp, com_vel_ans_2d);
             regulateCOMVelocityByCapturePointVec( com_ans_old.head(XY), com_vel.head(XY), hull_dcp, hull_acp, com_vel_ans_2d);
             com_ans.head(XY) = com_ans_old.head(XY) + com_vel_ans_2d * DT;
             com_ans_old = com_ans;
-//            Vector2ToVector3(hrp::Vector2(com_ans(X),com_ans(Y)) + com_vel_ans_2d * sqrt( H_cur / G ), cp_dec);
-//            Vector2ToVector3(hrp::Vector2(com_ans(X),com_ans(Y)) - com_vel_ans_2d * sqrt( H_cur / G ), cp_acc);
-            cp_dec.head(2) = com_ans.head(XY) + com_vel_ans_2d * sqrt( H_cur / G );
-            cp_acc.head(2) = com_ans.head(XY) - com_vel_ans_2d * sqrt( H_cur / G );
-            if(DEBUG){
-                fprintf(cz_log,"com_ans: %f %f ",com_ans(X),com_ans(Y));
-                fprintf(cz_log,"com_ans_cp_d: %f %f ",cp_dec(X),cp_dec(Y));
-                fprintf(cz_log,"com_ans_cp_a: %f %f ",cp_acc(X),cp_acc(Y));
-                for(int i=0;i<hull_dcp.size();i++)fprintf(sr_log,"hull_vert%d: %f %f\n",i,hull_dcp[i](X),hull_dcp[i](Y));
-                fprintf(sr_log,"hull_vert%d: %f %f\n",(int)hull_dcp.size(),hull_dcp[0](X),hull_dcp[0](Y));//閉じる
-                fprintf(sr_log,"\n");
-            }
+            cp_dec.head(XY) = com_ans.head(XY) + com_vel_ans_2d * sqrt( H_cur / G );
+            cp_acc.head(XY) = com_ans.head(XY) - com_vel_ans_2d * sqrt( H_cur / G );
             return true;
         }
-        void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& hull_d, const std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& hull_a, hrp::Vector2& com_vel_ans){
+        void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const hrp::dmatrix& hull_d, const hrp::dmatrix& hull_a, hrp::Vector2& com_vel_ans){
             hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
             com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
-            //      //減速CP条件(現在のCPを常に両足裏で頭打ち)
-            //      hrp::Vector2 cp_dec_tmp = com_pos + com_vel * sqrt( H_cur / G );
-            //      hrp::Vector2 cp_dec_ragulated;
-            //      if(!isPointInHullOpenCV(cp_dec_tmp,hull_d)){
-            //        calcCrossPointOnHull(com_pos, cp_dec_tmp, hull_d, cp_dec_ragulated);
-            //        com_vel_decel_ok = (cp_dec_ragulated - com_pos) / sqrt( H_cur / G );
-            //      }
 
             const double foot_ave_vel = 2.0;
             double lf_landing_delay = (rp_ref_out.tgt[lf].abs.p[Z] - rp_ref_out.tgt[lf].offs.p[Z]) / foot_ave_vel;
@@ -729,72 +629,41 @@ dbgv(rp_ref_out.tgt[rh].offs.rpy());
             if(com_vel(Y)>0 ){ rf_landing_delay = 0; }
             if(com_vel(Y)<0 ){ lf_landing_delay = 0; }
             double foot_landing_delay = std::max(rf_landing_delay, lf_landing_delay);
-            //      const double foot_landing_delay =0;//debug
             //減速CP条件(現在のCPを常に両足裏で頭打ち)
-
             //      hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);//TODO implement
             //      if(com_vel(Y)>1e-5 || fabs(com_vel(Y))<=1e-5 ){
             //        foot_landing_delay = 0;
             //      }
-            hrp::Vector2 cp_dec_ragulated = com_pos + com_vel * sqrt( H_cur / G ) + com_vel * foot_landing_delay;
-            if(!isPointInHullOpenCV(cp_dec_ragulated,hull_d)){
+            hrp::Vector2 cp_dec_ragulated = com_pos + com_vel * ( sqrt( H_cur / G ) + foot_landing_delay);
+            if(!isPointInHull2DOpenCV(cp_dec_ragulated, hull_d)){
                 calcCrossPointOnHull(com_pos, cp_dec_ragulated, hull_d, cp_dec_ragulated);
                 com_vel_decel_ok = (cp_dec_ragulated - com_pos) / ( sqrt( H_cur / G ) + foot_landing_delay);
             }
 
-#define F(x) (x == R ? rf : lf)
-
-            std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> > f_hull_wld[LR], f_hull_wld_ans[LR];
-            for(int i=0;i<foot_vert_check[R].size();i++){ f_hull_wld[R].push_back(foot_vert_check[R][i].head(XY) + rp_ref_out.tgt[F(R)].abs.p.head(XY)); }
-            for(int i=0;i<foot_vert_check[L].size();i++){ f_hull_wld[L].push_back(foot_vert_check[L][i].head(XY) + rp_ref_out.tgt[F(L)].abs.p.head(XY)); }
-            //      for(int i=0;i<foot_vert_safe[R].size();i++){ f_hull_wld[R].push_back(foot_vert_safe[R][i].head(XY) + rp_ref_out.tgt[F(R)].abs.p.head(XY)); }
-            //      for(int i=0;i<foot_vert_safe[L].size();i++){ f_hull_wld[L].push_back(foot_vert_safe[L][i].head(XY) + rp_ref_out.tgt[F(L)].abs.p.head(XY)); }
-            makeConvexHullOpenCV(f_hull_wld[R], f_hull_wld_ans[R]);
-            makeConvexHullOpenCV(f_hull_wld[L], f_hull_wld_ans[L]);
-            //      if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[R])){ rp_ref_out.tgt[lf].go_contact = true; }
-            //      if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[L])){ rp_ref_out.tgt[rf].go_contact = true; }
-            if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[R])){ cp_force_go_contact[L] = true; }else{ cp_force_go_contact[L] = false; }
-            if(!isPointInHullOpenCV(cp_dec_ragulated, f_hull_wld_ans[L])){ cp_force_go_contact[R] = true; }else{ cp_force_go_contact[R] = false; }
+            //ついでに減速CPによる強制遊脚着地も判定
+            hrp::dmatrix foot_vert3d_check_wld[LR], one_foot_hull2d[LR];
+            for(int i=0; i<LR; i++){
+                foot_vert3d_check_wld[i] = (rp_ref_out.tgt[F(i)].abs.R * foot_vert3d_check[i]).colwise() + rp_ref_out.tgt[F(i)].abs.p; // rotate and translate in 3D
+                one_foot_hull2d[i] = makeConvexHull2DOpenCV( foot_vert3d_check_wld[i].topRows(XY) ); // project into 2D and make convex hull
+            }
+            cp_force_go_contact[L] = !isPointInHull2DOpenCV(cp_dec_ragulated, one_foot_hull2d[R]); // if CapturePoint go out of R sole region, L foot must be go contact
+            cp_force_go_contact[R] = !isPointInHull2DOpenCV(cp_dec_ragulated, one_foot_hull2d[L]); // if CapturePoint go out of L sole region, R foot must be go contact
 
             //加速CP条件(ACP使用)
-            hrp::Vector2 cp_acc_tmp = com_pos - com_vel * sqrt( H_cur / G );
-            hrp::Vector2 cp_acc_ragulated;
-            if(!isPointInHullOpenCV(cp_acc_tmp,hull_a)){
-                calcCrossPointOnHull(com_pos, cp_acc_tmp, hull_a, cp_acc_ragulated);
+            hrp::Vector2 cp_acc_ragulated = com_pos - com_vel * sqrt( H_cur / G );
+            if(!isPointInHull2DOpenCV(cp_acc_ragulated, hull_a)){
+                calcCrossPointOnHull(com_pos, cp_acc_ragulated, hull_a, cp_acc_ragulated);
                 com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
             }
-            cp_acc_old = cp_acc_ragulated;
 
-            static hrp::Vector2 com_vel_ans_old = com_vel;
-            //加速減速条件マージ
-            if( com_vel_decel_ok.dot(com_vel) < com_vel_accel_ok.dot(com_vel)){//normじゃダメ？
-                com_vel_ans = com_vel_decel_ok;
-            }else{
-                com_vel_ans = com_vel_accel_ok;
-            }
-            com_vel_ans_old = com_vel_ans;
+            //加速減速条件マージ com_vel進行方向をより減速させるものを採用？
+            com_vel_ans = (com_vel_decel_ok.dot(com_vel) < com_vel_accel_ok.dot(com_vel)) ? com_vel_decel_ok : com_vel_accel_ok; //normじゃダメ？
         }
-        //    void regulateCOMVelocityB
-        void surpressGoContactChattering(HumanPose& out){
-            //      static std::deque<int> buffer[LR];
-            static int count[LR];
-            for(int i=0;i<LR;i++){
-                if(out.tgt[F(i)].go_contact){ count[i]++; }
-                else{ count[i]--; }
-                LIMIT_MINMAX(count[i],0,100);
-                if(count[i]>0){ out.tgt[F(i)].go_contact = true; }
-            }
-        }
-        void createSupportRegionByFootPos(const hrp::Vector3& rfin_abs, const hrp::Vector3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn, std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& hull_ans){
-            rflf_points.clear();
-            hull_ans.clear();
-            for(int i=0;i<2;i++){
-                for(int j=2;j<4;j++){
-                    rflf_points.push_back(hrp::Vector2(rfin_abs(X) + rf_mgn(i),    rfin_abs(Y) + rf_mgn(j)));
-                    rflf_points.push_back(hrp::Vector2(lfin_abs(X) + lf_mgn(i),    lfin_abs(Y) + lf_mgn(j)));
-                }
-            }
-            makeConvexHullOpenCV(rflf_points, hull_ans);
+        hrp::dmatrix createSupportRegionByFootPos(const hrp::Pose3& rfin_abs, const hrp::Pose3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn){
+            hrp::dmatrix rf_sole_verts_abs = (rfin_abs.R * make_rect_3d(rf_mgn)).colwise() + rfin_abs.p;
+            hrp::dmatrix lf_sole_verts_abs = (lfin_abs.R * make_rect_3d(lf_mgn)).colwise() + lfin_abs.p;
+            hrp::dmatrix both_sole_verts_abs = (hrp::dmatrix(3, rf_sole_verts_abs.cols()+lf_sole_verts_abs.cols()) << rf_sole_verts_abs, lf_sole_verts_abs).finished();
+            return makeConvexHull2DOpenCV(both_sole_verts_abs.topRows(XY));
         }
         void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const hrp::dvector6& rfwin, const hrp::dvector6& lfwin, hrp::Vector3& zmp_ans){
             hrp::Vector3 rfzmp,lfzmp;
@@ -813,7 +682,8 @@ dbgv(rp_ref_out.tgt[rh].offs.rpy());
             }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
             zmp_ans(2) = 0;
         }
-        void calcXYMarginToHull(const hrp::Vector2& check_point, const std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& hull, hrp::Vector4& margin_ans){
+        void calcXYMarginToHull(const hrp::Vector2& check_point, const hrp::dmatrix& hull, hrp::Vector4& margin_ans){
+            if(hull.rows() != 2){ std::cerr << "Invalid input for calcXYMarginToHull" << std::endl; dbgn(hull); }
             hrp::Vector4 margin_abs;
             hrp::Vector2 cross_pt, anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
             for(int direc=0;direc<4;direc++){
@@ -825,32 +695,33 @@ dbgv(rp_ref_out.tgt[rh].offs.rpy());
             margin_ans(2) =  margin_abs(2);
             margin_ans(3) = -margin_abs(3);
         }
-        bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& hull, hrp::Vector2& pt_will_cross){
+        bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const hrp::dmatrix& hull, hrp::Vector2& pt_will_cross){
+            if(hull.rows() != 2){ std::cerr << "Invalid input for calcCrossPointOnHull" << std::endl; dbgn(hull); }
             hrp::Vector2 anchor_vec = pt_out_goal - pt_in_start;
-            for(int i=0;i<hull.size();i++){
-                int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
-                hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
+            for(int i=0;i<hull.cols();i++){
+                int i_nxt = (i!=hull.cols()-1 ? i+1 : 0);
+                hrp::Vector2 cur_pt = hull.col(i), nxt_pt = hull.col(i_nxt);
                 hrp::Vector2 cur_edge = nxt_pt - cur_pt;
                 double dBunbo = hrp::hrpVector2Cross(anchor_vec,cur_edge);
                 if( dBunbo != 0.0) {//平行の場合を外す
-                    hrp::Vector2 vectorAC = hrp::Vector2(hull[i](0),hull[i](1)) - pt_in_start;
+                    hrp::Vector2 vectorAC = hull.col(i) - pt_in_start;
                     double dR = hrp::hrpVector2Cross(vectorAC,cur_edge) / dBunbo;
                     double dS = hrp::hrpVector2Cross(vectorAC,anchor_vec) / dBunbo;
                     if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){//dRには数値誤差が乗る
                         pt_will_cross = pt_in_start + dR * anchor_vec;
-                        //dist = dR * anchor_vec.norm();
                         return true;
                     }
                 }
             }
             return false;
         }
-        double calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& hull, hrp::Vector2& pt_ans){
+        double calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const hrp::dmatrix& hull, hrp::Vector2& pt_ans){
+            if(hull.rows() != 2){ std::cerr << "Invalid input for calcNearestPointOnHull" << std::endl; dbgn(hull); }
             double cur_nearest_dist, ans_nearest_dist;
             hrp::Vector2 cur_nearest_pt, ans_nearest_pt;
-            for(int i=0;i<hull.size();i++){
-                int i_nxt = (i!=hull.size()-1 ? i+1 : 0);
-                hrp::Vector2 cur_pt = hull[i], nxt_pt = hull[i_nxt];
+            for(int i=0;i<hull.cols();i++){
+                int i_nxt = (i!=hull.cols()-1 ? i+1 : 0);
+                hrp::Vector2 cur_pt = hull.col(i), nxt_pt = hull.col(i_nxt);
                 hrp::Vector2 cur_edge = nxt_pt - cur_pt;
                 hrp::Vector2 tgt_pt_v = tgt_pt - cur_pt;
                 double tgt_pt_projected_length = tgt_pt_v.dot(cur_edge.normalized());
@@ -873,18 +744,39 @@ dbgv(rp_ref_out.tgt[rh].offs.rpy());
             pt_ans = ans_nearest_pt;
             return ans_nearest_dist;
         }
-        void makeConvexHullOpenCV(const std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& pts, std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& hull_ans){
-            points.clear();
-            cvhull.clear();
-            hull_ans.clear();
-            for(int i=0;i<pts.size();i++){ points.push_back( cv::Point2f( pts[i](0), pts[i](1)) ); }
-            cv::convexHull(cv::Mat(points),cvhull,true);
-            for(int i=0;i<cvhull.size();i++){ hull_ans.push_back( hrp::Vector2( cvhull[i].x, cvhull[i].y ) );  }
+        hrp::dmatrix makeConvexHull2DOpenCV(const hrp::dmatrix& pts_2d){
+            if(pts_2d.rows() != 2){ std::cerr << "Invalid input for makeConvexHullOpenCV" << std::endl; dbgn(pts_2d); }
+            cv::Mat pts_2d_cv;
+            std::vector<cv::Point2f> points(pts_2d.cols()), hull_2d_cv;
+            for(int i=0; i<pts_2d.cols(); i++){
+                points[i] = cv::Point2f(pts_2d.col(i)(X), pts_2d.col(i)(Y));
+            }
+            cv::convexHull( cv::Mat(points), hull_2d_cv, true);
+            hrp::dmatrix hull_2d(2, hull_2d_cv.size());
+            for(int i=0; i<hull_2d_cv.size(); i++){
+                hull_2d.col(i) << hull_2d_cv[i].x,  hull_2d_cv[i].y;
+            }
+            return hull_2d;
         }
-        bool isPointInHullOpenCV(const hrp::Vector2& pt, const std::vector<hrp::Vector2, Eigen::aligned_allocator<hrp::Vector2> >& hull){
-            cvhull.clear();
-            for(int i=0;i<hull.size();i++){ cvhull.push_back( cv::Point2f( hull[i](0), hull[i](1)) ); }
-            return (cv::pointPolygonTest(cv::Mat(cvhull), cv::Point2f(pt(0),pt(1)), false) > 0);
+        bool isPointInHull2DOpenCV(const hrp::Vector2& pt, const hrp::dmatrix& hull){
+            if(hull.rows() != 2){ std::cerr << "Invalid input for isPointInHull2dOpenCV" << std::endl; dbgn(hull); }
+            std::vector<cv::Point2f> hull_cv(hull.cols());
+            for(int i=0; i<hull.cols(); i++){
+                hull_cv[i] = cv::Point2f(hull.col(i)(X), hull.col(i)(Y));
+            }
+            return (cv::pointPolygonTest(cv::Mat(hull_cv), cv::Point2f(pt(X),pt(Y)), false) > 0);
+        }
+        void applyLPFilter_post(HumanPose& tgt){
+            if(is_initial_loop){
+                for(int i=0, l[1]={com}; i<1; i++){
+                    tgt_pos_filters[l[i]].reset(tgt.tgt[l[i]].abs.p);
+                    tgt_rpy_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy());
+                }
+            }
+            for(int i=0, l[1]={com}; i<1; i++){
+                tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
+                tgt.tgt[l[i]].abs.setRpy(tgt_rpy_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy()));
+            }
         }
         void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
             comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
@@ -893,6 +785,98 @@ dbgv(rp_ref_out.tgt[rh].offs.rpy());
             com_oldold = com_old;
             com_old = comin;
         }
+
+
+        //        bool isPointInHullBoost(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){
+        //              namespace bg = boost::geometry;
+        //              typedef bg::model::d2::point_xy<double> point;
+        //              typedef bg::model::polygon<point> polygon;
+        //              polygon lr_region,s_region;
+        //              const double XUMARGIN = 0.04;//CHIDORI
+        //              const double XLMARGIN = -0.02;
+        //              const double YUMARGIN = 0.01;
+        //              const double YLMARGIN = -0.01;
+        //        //      const double XUMARGIN = 0.02;//JAXON
+        //        //      const double XLMARGIN = -0.01;
+        //        //      const double YUMARGIN = 0.0;
+        //        //      const double YLMARGIN = -0.0;
+        //              bg::model::linestring<point> s_line = boost::assign::list_of<point>
+        //                (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YLMARGIN)//LFの右下
+        //                (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN)//LFの左下
+        //                (lfin_abs(0) + XUMARGIN, lfin_abs(1) + YUMARGIN)//LFの左上
+        //                (lfin_abs(0) + XUMARGIN, lfin_abs(1) + YLMARGIN)//LFの右上
+        //                (rfin_abs(0) + XLMARGIN, rfin_abs(1) + YLMARGIN)//RFの右下
+        //                (rfin_abs(0) + XLMARGIN, rfin_abs(1) + YUMARGIN)//RFの左下
+        //                (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YUMARGIN)//RFの左上
+        //                (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YLMARGIN)//RFの右上
+        //                     ;
+        //              bg::convex_hull(s_line, s_region);
+        //              point com2d(comin_abs(0),comin_abs(1));
+        //              Eigen::Vector2d ans_point(comin_abs(0),comin_abs(1));
+        //              enum point_state_t {PT_IN_NO_PATTERN = -1, PT_IN_REGION, PT_FIX_TO_EDGE, PT_FIX_TO_VERTEX};
+        //              point_state_t pt_state = PT_IN_NO_PATTERN;
+        //              if(bg::within(com2d, s_region)){//対象の点が凸包の内部に存在するかチェックする
+        //                pt_state = PT_IN_REGION;
+        //              }else{
+        //        //        cout<<"COM out of s_region"<<endl;
+        //                Eigen::Vector2d check_point(com2d.x(),com2d.y());
+        //                int ans_lid=-1;
+        //                for(int i=0;i<s_region.outer().size()-1;i++){//対象の点がある線分への垂線を有するかチェックする
+        //                  Eigen::Vector2d cur_vert(s_region.outer()[i].x(),s_region.outer()[i].y());
+        //                  Eigen::Vector2d next_vert(s_region.outer()[i+1].x(),s_region.outer()[i+1].y());
+        //                  Eigen::Vector2d edge_v = next_vert - cur_vert;
+        //                  Eigen::Vector2d tgt_pt_v = check_point - cur_vert;
+        //                  //ある線分への垂線を有し，かつ外側(時計回りエッジに対して左側)に存在するなら，対象の点からそのエッジへの垂線の交点が最近傍点
+        //                  if(edge_v.dot(tgt_pt_v)/edge_v.norm() > 0 && edge_v.dot(tgt_pt_v)/edge_v.norm() < edge_v.norm() && (edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
+        //                    ans_lid = i;
+        //                    ans_point = cur_vert + edge_v.normalized() * (edge_v.dot(tgt_pt_v)/edge_v.norm());
+        //                    pt_state = PT_FIX_TO_EDGE;
+        //                    break;
+        //                  }
+        //                }
+        //                if(pt_state == PT_FIX_TO_EDGE){
+        //                  cout<<"point will on the line:"<<ans_lid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
+        //                }else{//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
+        //                  double cur_min_dis = bg::distance(com2d, s_region.outer()[0]);
+        //                  int ans_pid = 0;
+        //                  for(int i=1;i<s_region.outer().size();i++){
+        //                    if(bg::distance(com2d, s_region.outer()[i]) < cur_min_dis){
+        //                      cur_min_dis = bg::distance(com2d, s_region.outer()[i]);
+        //                      ans_pid = i;
+        //                    }
+        //                  }
+        //                  ans_point(0) = s_region.outer()[ans_pid].x();
+        //                  ans_point(1) = s_region.outer()[ans_pid].y();
+        //                  cout<<"point will on the vertex:"<<ans_pid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
+        //                  pt_state = PT_FIX_TO_VERTEX;
+        //                }
+        //              }
+        //        //      double FNUM_Z = 0.005;
+        //              double FNUM_Z = 0.01;
+        //              lpf_zmp = (1-FNUM_Z) * lpf_zmp + FNUM_Z * rp_ref_out.zmp;
+        //              hrp::Vector3 hpf_zmp = rp_ref_out.zmp - lpf_zmp;
+        //        //      rp_ref_out.zmp(0) = ans_point(0) + hpf_zmp(0);
+        //        //      rp_ref_out.zmp(1) = ans_point(1) + hpf_zmp(1);
+        //
+        //              if(loop%50==0){
+        //                if(pt_state != PT_IN_REGION){
+        //                  std::cout<<"COM out of Support Region pt_state="<<pt_state<<std::endl;
+        //                }
+        //                for(int i=0;i<s_region.outer().size();i++){
+        //                  fprintf(sr_log,"%f %f %f\n",s_region.outer().at(i).x(), s_region.outer().at(i).y(),(double)loop/500.0);
+        //                }
+        //                fprintf(sr_log,"\n");
+        //                fprintf(cz_log,"%f %f %f %f %f %f %f %f %f",(double)loop/500.0,comin_abs(0),comin_abs(1),comin_abs(2),rp_ref_out.zmp(0),rp_ref_out.zmp(1),rp_ref_out.zmp(2),ans_point(0),ans_point(1));
+        //                fprintf(cz_log," %f %f %f %f",rp_ref_out.zmp(1),hpf_zmp(1),ans_point(1),ans_point(1) + hpf_zmp(1));
+        //                fprintf(cz_log,"\n\n");
+        //              }
+        //              comin_abs(0) = ans_point(0);
+        //              comin_abs(1) = ans_point(1);
+        //        //      rp_ref_out.zmp(0) = comin_abs(0);
+        //        //      rp_ref_out.zmp(1) = comin_abs(1);
+        //              return true;
+        //            }
+
 };
 
 #endif // WBMS_CORE_H

From 889cdb4e0ac87bc6578b9cde7624eac3d744e070 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 3 May 2019 15:14:20 +0900
Subject: [PATCH 159/217] refactor

---
 idl/WholeBodyMasterSlaveService.idl           |   6 +-
 rtc/AutoBalancer/MyUtils.h                    |  43 ++--
 .../WholeBodyMasterSlave.cpp                  | 111 +++++------
 .../WholeBodyMasterSlave.h                    |  18 +-
 .../WholeBodyMasterSlaveService_impl.cpp      |  12 +-
 .../WholeBodyMasterSlaveService_impl.h        |   4 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 185 +++++++++---------
 7 files changed, 184 insertions(+), 195 deletions(-)

diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
index 3792df90f8d..04192f93e85 100644
--- a/idl/WholeBodyMasterSlaveService.idl
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -28,12 +28,14 @@ module OpenHRP
         boolean use_head;
         boolean use_upper;
         boolean use_lower;
+        StrSequence use_joints;
+        StrSequence use_targets;
     };
     boolean startWholeBodyMasterSlave();
     boolean stopWholeBodyMasterSlave();
     boolean pauseWholeBodyMasterSlave();
     boolean resumeWholeBodyMasterSlave();
-    void setWholeBodyMasterSlaveParam(in WholeBodyMasterSlaveParam i_param);
-    void getWholeBodyMasterSlaveParam(out WholeBodyMasterSlaveParam i_param);
+    void setParams(in WholeBodyMasterSlaveParam i_param);
+    void getParams(out WholeBodyMasterSlaveParam i_param);
   };
 };
diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index 81c03d5a459..b02739fdd6e 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -30,31 +30,22 @@ static const double Q_BUTTERWORTH = 0.707106781;
 #define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
 #define eps_eq(a, b, c)  (fabs((a)-(b)) <= c)
 
-//inline bool eps_eq(const double& a, const double& b, const double& eps = 0.001){ return fabs((a)-(b)) <= eps;}
-//inline void LIMIT_MIN(int& x, const int& min){ x = ( x<min ? min:x );}
-//inline void LIMIT_MIN(double& x, const double& min){ x = ( x<min ? min:x );}
-//inline void LIMIT_MAX(int& x, const int& max){ x = ( x<max ? x:max );}
-//inline void LIMIT_MAX(double& x, const double& max){ x = ( x<max ? x:max );}
-//inline void LIMIT_MINMAX(int& x, const int& min, const int& max){ x = (x<min  ? min : (x<max ? x : max));}
-//inline void LIMIT_MINMAX(double& x, const double& min, const double& max){ x = (x<min  ? min : (x<max ? x : max));}
-
-
 namespace hrp{
     class Pose3{
         public:
             EIGEN_MAKE_ALIGNED_OPERATOR_NEW
             hrp::Vector3 p;
             hrp::Matrix33 R;
-            Pose3(){ p.fill(0); R.setIdentity();}
-            Pose3(const double& _X, const double& _Y, const double& _Z, const double& _r, const double& _p, const double& _y){ p << _X,_Y,_Z; R = hrp::rotFromRpy(_r,_p,_y); }
-            Pose3(const hrp::dvector6& _xyz_rpy){ p = _xyz_rpy.head(3); R = hrp::rotFromRpy(_xyz_rpy.tail(3)); }
-            Pose3(const hrp::Vector3& _xyz, const hrp::Vector3& _rpy){ p = _xyz; R = hrp::rotFromRpy(_rpy); }
-            Pose3(const hrp::Vector3& _xyz, const hrp::Matrix33& _R){ p = _xyz; R = _R; }
-            hrp::Vector3 rpy() const{ return hrp::rpyFromRot(R); }
-            void setRpy(const double& _r, const double& _p, const double& _y) { R = rotFromRpy(_r,_p,_y); }
-            void setRpy(const hrp::Vector3& _rpy) { R = rotFromRpy(_rpy); }
-            hrp::dvector6 to_dvector6() const{ return (hrp::dvector6() << p, hrp::rpyFromRot(R)).finished(); }
-            void reset(){ p.fill(0); R.setIdentity(); }
+            Pose3()                                                                                                             { reset();}
+            Pose3(const double& _X, const double& _Y, const double& _Z, const double& _r, const double& _p, const double& _y)   { p << _X,_Y,_Z; R = hrp::rotFromRpy(_r,_p,_y); }
+            Pose3(const hrp::dvector6& _xyz_rpy)                                                                                { p = _xyz_rpy.head(3); R = hrp::rotFromRpy(_xyz_rpy.tail(3)); }
+            Pose3(const hrp::Vector3& _xyz, const hrp::Vector3& _rpy)                                                           { p = _xyz; R = hrp::rotFromRpy(_rpy); }
+            Pose3(const hrp::Vector3& _xyz, const hrp::Matrix33& _R)                                                            { p = _xyz; R = _R; }
+            void reset()                                                                                                        { p.fill(0); R.setIdentity(); }
+            hrp::Vector3 rpy() const                                                                                            { return hrp::rpyFromRot(R); }
+            hrp::dvector6 to_dvector6() const                                                                                   { return (hrp::dvector6() << p, hrp::rpyFromRot(R)).finished(); }
+            void setRpy(const double& _r, const double& _p, const double& _y)                                                   { R = rotFromRpy(_r,_p,_y); }
+            void setRpy(const hrp::Vector3& _rpy)                                                                               { R = rotFromRpy(_rpy); }
     };
 
     inline hrp::Vector3         to_Vector3      (const RTC::Point3D& in)        { return hrp::Vector3(in.x, in.y, in.z); }
@@ -63,13 +54,15 @@ namespace hrp{
     inline hrp::Pose3           to_Pose3        (const RTC::Pose3D& in)         { return hrp::Pose3(in.position.x, in.position.y, in.position.z, in.orientation.r, in.orientation.p, in.orientation.y); }
     inline hrp::dvector6        to_dvector6     (const OpenHRP::Wrench& in)     { return (hrp::dvector6() << in.force.x, in.force.y, in.force.z, in.torque.x, in.torque.y, in.torque.z).finished(); }
     inline hrp::dvector         to_dvector      (const RTC::TimedDoubleSeq::_data_seq& in) { return hrp::dvector::Map(in.get_buffer(), in.length()); }
+    inline std::vector<std::string> to_string_vector (const OpenHRP::WholeBodyMasterSlaveService::StrSequence& in) { std::vector<std::string> ret(in.length()); for(int i=0; i<in.length(); i++){ ret[i] = in[i]; } return ret; }
 
     inline RTC::Point3D         to_Point3D      (const hrp::Vector3& in)        { return (RTC::Point3D){in(X),in(Y),in(Z)}; }
     inline RTC::Orientation3D   to_Orientation3D(const hrp::Vector3& in)        { return (RTC::Orientation3D){in(X),in(Y),in(Z)}; }
     inline RTC::Pose3D          to_Pose3D       (const hrp::dvector6& in)       { return (RTC::Pose3D){in(X),in(Y),in(Z),in(r),in(p),in(y)}; }
     inline RTC::Pose3D          to_Pose3D       (const hrp::Pose3& in)          { return (RTC::Pose3D){in.p(X),in.p(Y),in.p(Z),in.rpy()(r),in.rpy()(p),in.rpy()(y)}; }
     inline OpenHRP::Wrench      to_Wrench       (const hrp::dvector6& in)       { return (OpenHRP::Wrench){in(X),in(Y),in(Z),in(r),in(p),in(y)}; }
-    inline RTC::TimedDoubleSeq::_data_seq  to_DoubleSeq(const hrp::dvector& in) { RTC::TimedDoubleSeq::_data_seq out(in.size()); hrp::dvector::Map(out.get_buffer(), in.size()) = in; return out; }
+    inline RTC::TimedDoubleSeq::_data_seq                       to_DoubleSeq    (const hrp::dvector& in)            { RTC::TimedDoubleSeq::_data_seq out(in.size()); hrp::dvector::Map(out.get_buffer(), in.size()) = in; return out; }
+    inline OpenHRP::WholeBodyMasterSlaveService::StrSequence    to_StrSequence  (const std::vector<std::string>& in){ OpenHRP::WholeBodyMasterSlaveService::StrSequence ret; ret.length(in.size()); for(int i=0; i<in.size(); i++){ ret[i] = in[i].c_str(); } return ret; }
 
     inline hrp::dvector getQAll         (const hrp::BodyPtr _robot){ hrp::dvector tmp(_robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ tmp(i) = _robot->joint(i)->q; } return tmp; }
     inline void         setQAll         (hrp::BodyPtr       _robot, const hrp::dvector& in){ assert(in.size() <= _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = in(i); } }
@@ -77,6 +70,7 @@ namespace hrp{
     inline void         setRobotStateVec(hrp::BodyPtr       _robot, const hrp::dvector& _q_bpos_brpy){ assert(_q_bpos_brpy.size() == _robot->numJoints()+6); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q_bpos_brpy(i); }; _robot->rootLink()->p = _q_bpos_brpy.tail(6).head(3); _robot->rootLink()->R = hrp::rotFromRpy(_q_bpos_brpy.tail(6).tail(3)); }
     inline void         setRobotStateVec(hrp::BodyPtr       _robot, const hrp::dvector& _q, const hrp::Vector3& _bpos, const hrp::Vector3& _brpy){ assert(_q.size() == _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q(i); }; _robot->rootLink()->p = _bpos; _robot->rootLink()->R = hrp::rotFromRpy(_brpy); }
     inline void         setRobotStateVec(hrp::BodyPtr       _robot, const hrp::dvector& _q, const hrp::Vector3& _bpos, const hrp::Matrix33& _bR){ assert(_q.size() == _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q(i); }; _robot->rootLink()->p = _bpos; _robot->rootLink()->R = _bR; }
+    inline std::vector<std::string> getJointNameAll (const hrp::BodyPtr _robot){ std::vector<std::string> ret(_robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ ret[i] = _robot->joint(i)->name; } return ret; }
 
     inline hrp::Vector3 omegaFromRotEx(const hrp::Matrix33& r) {//copy from JointPathEx.cpp
         using ::std::numeric_limits;
@@ -108,7 +102,6 @@ namespace hrp{
     }
 }
 
-
 inline std::ostream& operator<<(std::ostream& os, hrp::Pose3& in){
     os << "p = " << in.p.transpose() << std::endl;
     os << "R =\n" << in.R << std::endl;
@@ -116,4 +109,12 @@ inline std::ostream& operator<<(std::ostream& os, hrp::Pose3& in){
     return os;
 }
 
+template<typename T>
+inline std::ostream& operator<<(std::ostream& os, std::vector<T>& in){
+    for(int i=0; i<in.size(); i++){
+        os << "[" << i << "]: " << in[i] << std::endl;
+    }
+    return os;
+}
+
 #endif //  MYUTILS_H
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 3e90e8c70c0..be1c84f5cea 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -3,8 +3,7 @@
 #define DEBUGP (loop%200==0)
 #define DEBUGP_ONCE (loop==0)
 
-static const char* WholeBodyMasterSlave_spec[] =
-{
+static const char* WholeBodyMasterSlave_spec[] = {
         "implementation_id", "WholeBodyMasterSlave",
         "type_name",         "WholeBodyMasterSlave",
         "description",       "wholebodymasterslave component",
@@ -19,17 +18,6 @@ static const char* WholeBodyMasterSlave_spec[] =
         ""
 };
 
-//static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
-//{
-//    int pre = os.precision();
-//    os.setf(std::ios::fixed);
-//    os << std::setprecision(6)
-//       << (tm.sec + tm.nsec/1e9)
-//       << std::setprecision(pre);
-//    os.unsetf(std::ios::fixed);
-//    return os;
-//}
-
 WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager),
         m_qRefIn("qRef", m_qRef),// from sh
         m_zmpIn("zmpIn", m_zmp),
@@ -152,6 +140,13 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     RTCOUT << "setup fullbody ik finished" << std::endl;
 
     wbms = boost::shared_ptr<WBMSCore>(new WBMSCore(m_dt));
+    wbms->wp.use_joints = getJointNameAll(fik->m_robot);
+    wbms->wp.use_targets.push_back("rleg");
+    wbms->wp.use_targets.push_back("lleg");
+    wbms->wp.use_targets.push_back("rarm");
+    wbms->wp.use_targets.push_back("rarm");
+    wbms->wp.use_targets.push_back("com");
+    wbms->wp.use_targets.push_back("head");
     sccp = boost::shared_ptr<CapsuleCollisionChecker>(new CapsuleCollisionChecker(fik->m_robot));
     RTCOUT << "setup main function class finished" << std::endl;
 
@@ -165,7 +160,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     t_ip->set(&output_ratio);
     q_ip = new interpolator(fik->numStates(), m_dt, interpolator::CUBICSPLINE, 1); // or HOFFARBIB, QUINTICSPLINE
     q_ip->clear();
-    avg_q_vel = hrp::dvector::Constant(fik->numStates(), 2.0); // all joint max avarage vel = 1.0 rad/s
+    avg_q_vel = hrp::dvector::Constant(fik->numStates(), 1.0); // all joint max avarage vel = 1.0 rad/s
     avg_q_acc = hrp::dvector::Constant(fik->numStates(), 16.0); // all joint max avarage acc = 16.0 rad/s^2
     avg_q_vel.tail(6).fill(std::numeric_limits<double>::max()); // no limit for base link vel
     avg_q_acc.tail(6).fill(std::numeric_limits<double>::max()); // no limit for base link acc
@@ -183,10 +178,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     return RTC::RTC_OK;
 }
 
-RTC::ReturnCode_t WholeBodyMasterSlave::onFinalize(){
-    return RTC::RTC_OK;
-}
-
 RTC::ReturnCode_t WholeBodyMasterSlave::setupEEIKConstraintFromConf(std::map<std::string, IKConstraint>& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop){
     coil::vstring ee_conf_all = coil::split(_prop["end_effectors"], ",");
     size_t prop_num = 10;
@@ -217,11 +208,10 @@ RTC::ReturnCode_t WholeBodyMasterSlave::setupEEIKConstraintFromConf(std::map<std
     return RTC::RTC_OK;
 }
 
-#define TIMECALC 1
 RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
-    if(DEBUGP_ONCE)RTCOUT << "onExecute(" << ec_id << ")" << std::endl;
-    struct timeval t_calc_start, t_calc_end;
-    if(TIMECALC)gettimeofday(&t_calc_start, NULL);
+    if(DEBUGP_ONCE) RTCOUT << "onExecute(" << ec_id << ")" << std::endl;
+    time_report_str.clear();
+    clock_gettime(CLOCK_REALTIME, &startT);
     if (m_qRefIn.isNew()) { m_qRefIn.read(); }
     if (m_basePosIn.isNew()) { m_basePosIn.read(); }
     if (m_baseRpyIn.isNew()) { m_baseRpyIn.read(); }
@@ -241,6 +231,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         if (m_actCPIn.isNew())  { m_actCPIn.read(); rel_act_cp = hrp::to_Vector3(m_actCP.data);}
         if (m_actZMPIn.isNew())  { m_actZMPIn.read(); rel_act_zmp = hrp::to_Vector3(m_actZMP.data);}
     }
+    addTimeReport("InPort");
 
     //khi
     if(m_htlfw.data.force.y == 1 && m_htrfw.data.force.y == 1 && mode.now() == MODE_IDLE){
@@ -254,11 +245,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if ( is_legged_robot ) {
         processTransition();
         mode.update();
-//        if(DEBUGP)dbg(mode.now());
-        struct timespec startT, endT;
-        clock_gettime(CLOCK_REALTIME, &startT);
-
-        if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processTransition" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
 
         if (mode.isRunning()) {
             if(mode.isInitialize()){
@@ -269,22 +255,13 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
             fik->q_ref.head(m_qRef.data.length()) = hrp::to_dvector(m_qRef.data);//あえてseqからのbaselink poseは信用しない
 
             wbms->update();//////HumanSynchronizerの主要処理
-            if(DEBUGP)RTCOUT << "update():"<<wbms->getUpdateTime()<<std::endl;
             if(DEBUGP)wbms->rp_ref_out.print();
+            addTimeReport("MainFunc");
 
             solveFullbodyIK(wbms->rp_ref_out.tgt[com].abs, wbms->rp_ref_out.tgt[rf].abs, wbms->rp_ref_out.tgt[lf].abs, wbms->rp_ref_out.tgt[rh].abs, wbms->rp_ref_out.tgt[lh].abs, wbms->rp_ref_out.tgt[head].abs);
+            addTimeReport("IK");
 
-            if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ processWholeBodyMasterSlave" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
-            //逆動力学
-            //        calcAccelerationsForInverseDynamics(m_robot, idsb);
-            //        hrp::Vector3 ref_zmp_invdyn;
-            //        calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp_invdyn);processTransition
-            //        ref_zmp_invdyn = invdyn_zmp_filters.passFilter(ref_zmp_invdyn);
-            //        updateInvDynStateBuffer(idsb);
-
-            if(DEBUGP && TIMECALC){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ calcWorldZMPFromInverseDynamics" << std::endl;  clock_gettime(CLOCK_REALTIME, &startT);}
-
-            processHOFFARBIBFilter(fik->m_robot, m_robot_vsafe);
+            smoothingJointAngles(fik->m_robot, m_robot_vsafe);
 
             hrp::Vector3 com = m_robot_vsafe->calcCM();
             static hrp::Vector3 com_old = com;
@@ -326,6 +303,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
             }
             m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = wbms->rp_ref_out.tgt[rf].is_contact;
             m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = wbms->rp_ref_out.tgt[lf].is_contact;
+            addTimeReport("SetOutPut");
         }
         wbms->baselinkpose.p = fik->m_robot->rootLink()->p;
         wbms->baselinkpose.R = fik->m_robot->rootLink()->R;
@@ -401,9 +379,10 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     WBMSCore::Vector6ToDoubleSeq(wbms->invdyn_ft,m_invdyn_dbg.data);
     m_invdyn_dbgOut.write();
 #endif
-if(TIMECALC){gettimeofday(&t_calc_end, NULL); if(DEBUGP)cout<<"t_last:"<<(double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6<<endl; t_calc_start = t_calc_end;}
-loop ++;
-return RTC::RTC_OK;
+    addTimeReport("OutPort");
+    if(DEBUGP) RTCOUT << time_report_str << std::endl;
+    loop ++;
+    return RTC::RTC_OK;
 }
 
 
@@ -767,10 +746,9 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
     const int IK_MAX_LOOP = 2;
     clock_gettime(CLOCK_REALTIME, &startT);
     int loop_result = fik->solveFullbodyIKLoop(ikc_list, IK_MAX_LOOP);
-    if(loop%100==0){clock_gettime(CLOCK_REALTIME, &endT); std::cout << (endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-9) << " @ solveIK"<<loop_result<<"loop" << std::endl;}
 }
 
-void WholeBodyMasterSlave::processHOFFARBIBFilter(hrp::BodyPtr _robot, hrp::BodyPtr _robot_safe){
+void WholeBodyMasterSlave::smoothingJointAngles(hrp::BodyPtr _robot, hrp::BodyPtr _robot_safe){
     double goal_time = 0.0;
     const double min_goal_time_offset = 0.1;
 
@@ -881,18 +859,20 @@ bool WholeBodyMasterSlave::stopWholeBodyMasterSlave(){
 }
 
 
-bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
+bool WholeBodyMasterSlave::setParams(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
     RTCOUT << "setWholeBodyMasterSlaveParam" << std::endl;
-    wbms->wp.auto_swing_foot_landing_threshold = i_param.auto_swing_foot_landing_threshold;
-    wbms->wp.human_to_robot_ratio = i_param.human_to_robot_ratio;
-    wbms->wp.set_com_height_fix = i_param.set_com_height_fix;
-    wbms->wp.set_com_height_fix_val = i_param.set_com_height_fix_val;
-    wbms->wp.swing_foot_height_offset = i_param.swing_foot_height_offset;
-    wbms->wp.upper_body_rmc_ratio = i_param.upper_body_rmc_ratio;
+    wbms->wp.auto_swing_foot_landing_threshold  = i_param.auto_swing_foot_landing_threshold;
+    wbms->wp.human_to_robot_ratio               = i_param.human_to_robot_ratio;
+    wbms->wp.set_com_height_fix                 = i_param.set_com_height_fix;
+    wbms->wp.set_com_height_fix_val             = i_param.set_com_height_fix_val;
+    wbms->wp.swing_foot_height_offset           = i_param.swing_foot_height_offset;
+    wbms->wp.upper_body_rmc_ratio               = i_param.upper_body_rmc_ratio;
     if(mode.now() == MODE_IDLE){
-        wbms->wp.use_head = i_param.use_head;
-        wbms->wp.use_upper = i_param.use_upper;
-        wbms->wp.use_lower = i_param.use_lower;
+        wbms->wp.use_head                       = i_param.use_head;
+        wbms->wp.use_upper                      = i_param.use_upper;
+        wbms->wp.use_lower                      = i_param.use_lower;
+        wbms->wp.use_joints                     = hrp::to_string_vector(i_param.use_joints);
+        wbms->wp.use_targets                    = hrp::to_string_vector(i_param.use_targets);
     }else{
       RTCOUT << "use_head, use_upper, use_lower can be changed in MODE_IDLE" << std::endl;
     }
@@ -900,23 +880,26 @@ bool WholeBodyMasterSlave::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBody
 }
 
 
-bool WholeBodyMasterSlave::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
+bool WholeBodyMasterSlave::getParams(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
     RTCOUT << "getWholeBodyMasterSlaveParam" << std::endl;
-    i_param.auto_swing_foot_landing_threshold = wbms->wp.auto_swing_foot_landing_threshold;
-    i_param.human_to_robot_ratio = wbms->wp.human_to_robot_ratio;
-    i_param.set_com_height_fix = wbms->wp.set_com_height_fix;
-    i_param.set_com_height_fix_val = wbms->wp.set_com_height_fix_val;
-    i_param.swing_foot_height_offset = wbms->wp.swing_foot_height_offset;
-    i_param.upper_body_rmc_ratio = wbms->wp.upper_body_rmc_ratio;
-    i_param.use_head = wbms->wp.use_head;
-    i_param.use_upper = wbms->wp.use_upper;
-    i_param.use_lower = wbms->wp.use_lower;
+    i_param.auto_swing_foot_landing_threshold   = wbms->wp.auto_swing_foot_landing_threshold;
+    i_param.human_to_robot_ratio                = wbms->wp.human_to_robot_ratio;
+    i_param.set_com_height_fix                  = wbms->wp.set_com_height_fix;
+    i_param.set_com_height_fix_val              = wbms->wp.set_com_height_fix_val;
+    i_param.swing_foot_height_offset            = wbms->wp.swing_foot_height_offset;
+    i_param.upper_body_rmc_ratio                = wbms->wp.upper_body_rmc_ratio;
+    i_param.use_head                            = wbms->wp.use_head;
+    i_param.use_upper                           = wbms->wp.use_upper;
+    i_param.use_lower                           = wbms->wp.use_lower;
+    i_param.use_joints                          = hrp::to_StrSequence(wbms->wp.use_joints);
+    i_param.use_targets                         = hrp::to_StrSequence(wbms->wp.use_targets);
     return true;
 }
 
 
 RTC::ReturnCode_t WholeBodyMasterSlave::onActivated(RTC::UniqueId ec_id){ RTCOUT << "onActivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; }
 RTC::ReturnCode_t WholeBodyMasterSlave::onDeactivated(RTC::UniqueId ec_id){ RTCOUT << "onDeactivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; }
+RTC::ReturnCode_t WholeBodyMasterSlave::onFinalize(){ return RTC::RTC_OK; }
 
 extern "C"{
     void WholeBodyMasterSlaveInit(RTC::Manager* manager) {
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 1e065360ea7..029318af6b6 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -12,6 +12,7 @@
 #include <rtm/idl/ExtendedDataTypesSkel.h>
 #include <rtm/CorbaNaming.h>
 #include <hrpModel/ModelLoaderUtil.h>
+#include <time.h>
 
 #include "WholeBodyMasterSlaveService_impl.h"
 #include "wbms_core.h"
@@ -62,8 +63,8 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         bool stopWholeBodyMasterSlave();
         bool pauseWholeBodyMasterSlave();
         bool resumeWholeBodyMasterSlave();
-        bool setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
-        bool getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
+        bool setParams(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
+        bool getParams(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
 
     protected:
         RTC::TimedDoubleSeq m_qRef;
@@ -183,14 +184,23 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         int cp_flag;
         hrp::Vector3 lt,rt;
         enum cp_enum{ CP_IDLE, CP_LF, CP_RF, CP_STATIC};
+        struct timespec startT, endT;
+        std::string time_report_str;
 
         RTC::ReturnCode_t setupEEIKConstraintFromConf(std::map<std::string, IKConstraint>& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop);
         void solveFullbodyIK(const hrp::Pose3& com_ref, const hrp::Pose3& rf_ref, const hrp::Pose3& lf_ref, const hrp::Pose3& rh_ref, const hrp::Pose3& lh_ref, const hrp::Pose3& head_ref);
         void processTransition();
         void preProcessForWholeBodyMasterSlave();
         void processWholeBodyMasterSlave(const HumanPose& pose_ref);
-        void processHOFFARBIBFilter(hrp::BodyPtr _robot, hrp::BodyPtr _robot_safe);
-        bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; };
+        void smoothingJointAngles(hrp::BodyPtr _robot, hrp::BodyPtr _robot_safe);
+        bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; }
+        void addTimeReport(const std::string& prefix){
+            clock_gettime(CLOCK_REALTIME, &endT);
+            std::stringstream ss;
+            ss << prefix << "= " << std::fixed <<std::setprecision(2) << (double)(endT.tv_sec - startT.tv_sec + (endT.tv_nsec - startT.tv_nsec) * 1e-6) << " [ms] / ";
+            time_report_str += ss.str();
+            clock_gettime(CLOCK_REALTIME, &startT);
+        }
 };
 
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp
index 4fa18b6bdbf..63ed8dc8ff6 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp
@@ -34,14 +34,14 @@ CORBA::Boolean WholeBodyMasterSlaveService_impl::resumeWholeBodyMasterSlave()
     return m_wholebodymasterslave->resumeWholeBodyMasterSlave();
 };
 
-void WholeBodyMasterSlaveService_impl::setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param)
+void WholeBodyMasterSlaveService_impl::setParams(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param)
 {
-  m_wholebodymasterslave->setWholeBodyMasterSlaveParam(i_param);
+  m_wholebodymasterslave->setParams(i_param);
 };
 
-void WholeBodyMasterSlaveService_impl::getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam_out i_param)
+void WholeBodyMasterSlaveService_impl::getParams(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam_out i_param)
 {
-//  i_param = new OpenHRP::WholeBodyMasterSlaveParam::WholeBodyMasterSlaveParam();
-//  return  m_wholebodymasterslave->getWholeBodyMasterSlaveParam(*i_param);
-  m_wholebodymasterslave->getWholeBodyMasterSlaveParam(i_param);
+  i_param = new OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam();
+  m_wholebodymasterslave->getParams(*i_param);
+//  m_wholebodymasterslave->getParams(i_param); // error: no matching function for call to ‘WholeBodyMasterSlave::getParams(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam_out&)’
 };
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h
index 4375dc3102c..92026dabb6b 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h
@@ -20,8 +20,8 @@ class WholeBodyMasterSlaveService_impl
   CORBA::Boolean stopWholeBodyMasterSlave();
   CORBA::Boolean pauseWholeBodyMasterSlave();
   CORBA::Boolean resumeWholeBodyMasterSlave();
-  void setWholeBodyMasterSlaveParam(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
-  void getWholeBodyMasterSlaveParam(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam_out i_param);
+  void setParams(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param);
+  void getParams(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam_out i_param);
   //
   void wholebodymasterslave(WholeBodyMasterSlave *i_wholebodymasterslave);
 private:
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 5ad8940a6d6..f72f0d968a0 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -319,7 +319,6 @@ class CapsuleCollisionChecker {
 class WBMSCore{
     private:
         double HZ, DT;
-        struct timeval t_calc_start, t_calc_end;
         BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters, com_in_filter;
         hrp::Vector3 com_old, com_oldold, comacc, com_CP_ref_old, r_zmp_raw;
         std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rpy_filters;
@@ -348,11 +347,12 @@ class WBMSCore{
                 bool use_head;
                 bool use_upper;
                 bool use_lower;
-        };
-        struct WBMSparameters wp;
+                std::vector<std::string> use_joints;
+                std::vector<std::string> use_targets;
+        } wp;
         struct ActualRobotState {
                 hrp::Vector3 com, zmp;
-        }act_rs;
+        } act_rs;
 
         WBMSCore(const double& dt){
             DT = dt;
@@ -420,7 +420,6 @@ class WBMSCore{
             if(DEBUG)fclose(id_log);
             cout<<"WBMSCore destructed"<<endl;
         }
-        double getUpdateTime() const { return (double)(t_calc_end.tv_sec - t_calc_start.tv_sec) + (t_calc_end.tv_usec - t_calc_start.tv_usec)/1.0e6; }
         void initializeHumanPoseFromCurrentInput(){
             std::string ns[7] = {"com","rf","lf","rh","lh","zmp","head"};
             for(int i=0;i<7;i++){
@@ -460,7 +459,6 @@ class WBMSCore{
             rp_ref_out_old = rp_ref_vel_old = rp_ref_out;
         }
         void update(){//////////  メインループ  ////////////
-            gettimeofday(&t_calc_start, NULL);
             autoLRSwapCheck                     (hp_wld_raw, hp_swap_checked);//入力の左右反転を常にチェック(＝手足の交差は不可能)(動作中に入れ替わると余計なお世話かも)
             convertHumanToRobot                 (hp_swap_checked, rp_ref_out);
             hp_plot = rp_ref_out;
@@ -487,7 +485,6 @@ class WBMSCore{
             rp_ref_out_old = rp_ref_out;
             loop++;
             is_initial_loop = false;
-            gettimeofday(&t_calc_end, NULL);
         }
 
     private:
@@ -787,95 +784,91 @@ class WBMSCore{
         }
 
 
-        //        bool isPointInHullBoost(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){
-        //              namespace bg = boost::geometry;
-        //              typedef bg::model::d2::point_xy<double> point;
-        //              typedef bg::model::polygon<point> polygon;
-        //              polygon lr_region,s_region;
-        //              const double XUMARGIN = 0.04;//CHIDORI
-        //              const double XLMARGIN = -0.02;
-        //              const double YUMARGIN = 0.01;
-        //              const double YLMARGIN = -0.01;
-        //        //      const double XUMARGIN = 0.02;//JAXON
-        //        //      const double XLMARGIN = -0.01;
-        //        //      const double YUMARGIN = 0.0;
-        //        //      const double YLMARGIN = -0.0;
-        //              bg::model::linestring<point> s_line = boost::assign::list_of<point>
-        //                (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YLMARGIN)//LFの右下
-        //                (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN)//LFの左下
-        //                (lfin_abs(0) + XUMARGIN, lfin_abs(1) + YUMARGIN)//LFの左上
-        //                (lfin_abs(0) + XUMARGIN, lfin_abs(1) + YLMARGIN)//LFの右上
-        //                (rfin_abs(0) + XLMARGIN, rfin_abs(1) + YLMARGIN)//RFの右下
-        //                (rfin_abs(0) + XLMARGIN, rfin_abs(1) + YUMARGIN)//RFの左下
-        //                (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YUMARGIN)//RFの左上
-        //                (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YLMARGIN)//RFの右上
-        //                     ;
-        //              bg::convex_hull(s_line, s_region);
-        //              point com2d(comin_abs(0),comin_abs(1));
-        //              Eigen::Vector2d ans_point(comin_abs(0),comin_abs(1));
-        //              enum point_state_t {PT_IN_NO_PATTERN = -1, PT_IN_REGION, PT_FIX_TO_EDGE, PT_FIX_TO_VERTEX};
-        //              point_state_t pt_state = PT_IN_NO_PATTERN;
-        //              if(bg::within(com2d, s_region)){//対象の点が凸包の内部に存在するかチェックする
-        //                pt_state = PT_IN_REGION;
-        //              }else{
-        //        //        cout<<"COM out of s_region"<<endl;
-        //                Eigen::Vector2d check_point(com2d.x(),com2d.y());
-        //                int ans_lid=-1;
-        //                for(int i=0;i<s_region.outer().size()-1;i++){//対象の点がある線分への垂線を有するかチェックする
-        //                  Eigen::Vector2d cur_vert(s_region.outer()[i].x(),s_region.outer()[i].y());
-        //                  Eigen::Vector2d next_vert(s_region.outer()[i+1].x(),s_region.outer()[i+1].y());
-        //                  Eigen::Vector2d edge_v = next_vert - cur_vert;
-        //                  Eigen::Vector2d tgt_pt_v = check_point - cur_vert;
-        //                  //ある線分への垂線を有し，かつ外側(時計回りエッジに対して左側)に存在するなら，対象の点からそのエッジへの垂線の交点が最近傍点
-        //                  if(edge_v.dot(tgt_pt_v)/edge_v.norm() > 0 && edge_v.dot(tgt_pt_v)/edge_v.norm() < edge_v.norm() && (edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
-        //                    ans_lid = i;
-        //                    ans_point = cur_vert + edge_v.normalized() * (edge_v.dot(tgt_pt_v)/edge_v.norm());
-        //                    pt_state = PT_FIX_TO_EDGE;
-        //                    break;
-        //                  }
-        //                }
-        //                if(pt_state == PT_FIX_TO_EDGE){
-        //                  cout<<"point will on the line:"<<ans_lid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
-        //                }else{//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
-        //                  double cur_min_dis = bg::distance(com2d, s_region.outer()[0]);
-        //                  int ans_pid = 0;
-        //                  for(int i=1;i<s_region.outer().size();i++){
-        //                    if(bg::distance(com2d, s_region.outer()[i]) < cur_min_dis){
-        //                      cur_min_dis = bg::distance(com2d, s_region.outer()[i]);
-        //                      ans_pid = i;
-        //                    }
-        //                  }
-        //                  ans_point(0) = s_region.outer()[ans_pid].x();
-        //                  ans_point(1) = s_region.outer()[ans_pid].y();
-        //                  cout<<"point will on the vertex:"<<ans_pid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
-        //                  pt_state = PT_FIX_TO_VERTEX;
-        //                }
-        //              }
-        //        //      double FNUM_Z = 0.005;
-        //              double FNUM_Z = 0.01;
-        //              lpf_zmp = (1-FNUM_Z) * lpf_zmp + FNUM_Z * rp_ref_out.zmp;
-        //              hrp::Vector3 hpf_zmp = rp_ref_out.zmp - lpf_zmp;
-        //        //      rp_ref_out.zmp(0) = ans_point(0) + hpf_zmp(0);
-        //        //      rp_ref_out.zmp(1) = ans_point(1) + hpf_zmp(1);
-        //
-        //              if(loop%50==0){
-        //                if(pt_state != PT_IN_REGION){
-        //                  std::cout<<"COM out of Support Region pt_state="<<pt_state<<std::endl;
-        //                }
-        //                for(int i=0;i<s_region.outer().size();i++){
-        //                  fprintf(sr_log,"%f %f %f\n",s_region.outer().at(i).x(), s_region.outer().at(i).y(),(double)loop/500.0);
-        //                }
-        //                fprintf(sr_log,"\n");
-        //                fprintf(cz_log,"%f %f %f %f %f %f %f %f %f",(double)loop/500.0,comin_abs(0),comin_abs(1),comin_abs(2),rp_ref_out.zmp(0),rp_ref_out.zmp(1),rp_ref_out.zmp(2),ans_point(0),ans_point(1));
-        //                fprintf(cz_log," %f %f %f %f",rp_ref_out.zmp(1),hpf_zmp(1),ans_point(1),ans_point(1) + hpf_zmp(1));
-        //                fprintf(cz_log,"\n\n");
-        //              }
-        //              comin_abs(0) = ans_point(0);
-        //              comin_abs(1) = ans_point(1);
-        //        //      rp_ref_out.zmp(0) = comin_abs(0);
-        //        //      rp_ref_out.zmp(1) = comin_abs(1);
-        //              return true;
-        //            }
+        bool isPointInHullBoost(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){
+//              namespace bg = boost::geometry;
+//              typedef bg::model::d2::point_xy<double> point;
+//              typedef bg::model::polygon<point> polygon;
+//              polygon lr_region,s_region;
+//              const double XUMARGIN = 0.04;//CHIDORI
+//              const double XLMARGIN = -0.02;
+//              const double YUMARGIN = 0.01;
+//              const double YLMARGIN = -0.01;
+//              bg::model::linestring<point> s_line = boost::assign::list_of<point>
+//                (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YLMARGIN)//LFの右下
+//                (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN)//LFの左下
+//                (lfin_abs(0) + XUMARGIN, lfin_abs(1) + YUMARGIN)//LFの左上
+//                (lfin_abs(0) + XUMARGIN, lfin_abs(1) + YLMARGIN)//LFの右上
+//                (rfin_abs(0) + XLMARGIN, rfin_abs(1) + YLMARGIN)//RFの右下
+//                (rfin_abs(0) + XLMARGIN, rfin_abs(1) + YUMARGIN)//RFの左下
+//                (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YUMARGIN)//RFの左上
+//                (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YLMARGIN)//RFの右上
+//                     ;
+//              bg::convex_hull(s_line, s_region);
+//              point com2d(comin_abs(0),comin_abs(1));
+//              Eigen::Vector2d ans_point(comin_abs(0),comin_abs(1));
+//              enum point_state_t {PT_IN_NO_PATTERN = -1, PT_IN_REGION, PT_FIX_TO_EDGE, PT_FIX_TO_VERTEX};
+//              point_state_t pt_state = PT_IN_NO_PATTERN;
+//              if(bg::within(com2d, s_region)){//対象の点が凸包の内部に存在するかチェックする
+//                pt_state = PT_IN_REGION;
+//              }else{
+//        //        cout<<"COM out of s_region"<<endl;
+//                Eigen::Vector2d check_point(com2d.x(),com2d.y());
+//                int ans_lid=-1;
+//                for(int i=0;i<s_region.outer().size()-1;i++){//対象の点がある線分への垂線を有するかチェックする
+//                  Eigen::Vector2d cur_vert(s_region.outer()[i].x(),s_region.outer()[i].y());
+//                  Eigen::Vector2d next_vert(s_region.outer()[i+1].x(),s_region.outer()[i+1].y());
+//                  Eigen::Vector2d edge_v = next_vert - cur_vert;
+//                  Eigen::Vector2d tgt_pt_v = check_point - cur_vert;
+//                  //ある線分への垂線を有し，かつ外側(時計回りエッジに対して左側)に存在するなら，対象の点からそのエッジへの垂線の交点が最近傍点
+//                  if(edge_v.dot(tgt_pt_v)/edge_v.norm() > 0 && edge_v.dot(tgt_pt_v)/edge_v.norm() < edge_v.norm() && (edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
+//                    ans_lid = i;
+//                    ans_point = cur_vert + edge_v.normalized() * (edge_v.dot(tgt_pt_v)/edge_v.norm());
+//                    pt_state = PT_FIX_TO_EDGE;
+//                    break;
+//                  }
+//                }
+//                if(pt_state == PT_FIX_TO_EDGE){
+//                  cout<<"point will on the line:"<<ans_lid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
+//                }else{//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
+//                  double cur_min_dis = bg::distance(com2d, s_region.outer()[0]);
+//                  int ans_pid = 0;
+//                  for(int i=1;i<s_region.outer().size();i++){
+//                    if(bg::distance(com2d, s_region.outer()[i]) < cur_min_dis){
+//                      cur_min_dis = bg::distance(com2d, s_region.outer()[i]);
+//                      ans_pid = i;
+//                    }
+//                  }
+//                  ans_point(0) = s_region.outer()[ans_pid].x();
+//                  ans_point(1) = s_region.outer()[ans_pid].y();
+//                  cout<<"point will on the vertex:"<<ans_pid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
+//                  pt_state = PT_FIX_TO_VERTEX;
+//                }
+//              }
+//        //      double FNUM_Z = 0.005;
+//              double FNUM_Z = 0.01;
+//              lpf_zmp = (1-FNUM_Z) * lpf_zmp + FNUM_Z * rp_ref_out.zmp;
+//              hrp::Vector3 hpf_zmp = rp_ref_out.zmp - lpf_zmp;
+//        //      rp_ref_out.zmp(0) = ans_point(0) + hpf_zmp(0);
+//        //      rp_ref_out.zmp(1) = ans_point(1) + hpf_zmp(1);
+//
+//              if(loop%50==0){
+//                if(pt_state != PT_IN_REGION){
+//                  std::cout<<"COM out of Support Region pt_state="<<pt_state<<std::endl;
+//                }
+//                for(int i=0;i<s_region.outer().size();i++){
+//                  fprintf(sr_log,"%f %f %f\n",s_region.outer().at(i).x(), s_region.outer().at(i).y(),(double)loop/500.0);
+//                }
+//                fprintf(sr_log,"\n");
+//                fprintf(cz_log,"%f %f %f %f %f %f %f %f %f",(double)loop/500.0,comin_abs(0),comin_abs(1),comin_abs(2),rp_ref_out.zmp(0),rp_ref_out.zmp(1),rp_ref_out.zmp(2),ans_point(0),ans_point(1));
+//                fprintf(cz_log," %f %f %f %f",rp_ref_out.zmp(1),hpf_zmp(1),ans_point(1),ans_point(1) + hpf_zmp(1));
+//                fprintf(cz_log,"\n\n");
+//              }
+//              comin_abs(0) = ans_point(0);
+//              comin_abs(1) = ans_point(1);
+//        //      rp_ref_out.zmp(0) = comin_abs(0);
+//        //      rp_ref_out.zmp(1) = comin_abs(1);
+//              return true;
+            }
 
 };
 

From e163ddc4a2f0a1db736265a511b9e3b9f9e8e72c Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 3 May 2019 16:10:49 +0900
Subject: [PATCH 160/217] apply boost getmetries back

---
 rtc/WholeBodyMasterSlave/wbms_core.h | 83 ++++++++++++++++++----------
 1 file changed, 53 insertions(+), 30 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index f72f0d968a0..a412fdff2c3 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -16,7 +16,12 @@
 #include <boost/geometry.hpp>
 #include <boost/geometry/geometries/polygon.hpp>
 #include <boost/geometry/geometries/point_xy.hpp>
+#include <boost/geometry/multi/geometries/multi_point.hpp>
 #include <boost/assign/list_of.hpp>
+namespace bg = boost::geometry;
+typedef bg::model::d2::point_xy<double> bg_point;
+typedef bg::model::multi_point<bg_point> bg_multi_point;
+typedef bg::model::polygon<bg_point> bg_polygon;
 
 //#include<Eigen/StdVector>
 //EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix2d)
@@ -423,7 +428,7 @@ class WBMSCore{
         void initializeHumanPoseFromCurrentInput(){
             std::string ns[7] = {"com","rf","lf","rh","lh","zmp","head"};
             for(int i=0;i<7;i++){
-                hp_wld_raw.tgt[i].offs   =  hp_wld_raw.tgt[i].abs;
+                hp_wld_raw.tgt[i].offs = hp_wld_raw.tgt[i].abs;
             }
         }
         void initializeRobotPoseFromHRPBody(const hrp::BodyPtr robot_in, std::map<std::string, IKConstraint>& _ee_ikc_map){
@@ -555,7 +560,7 @@ class WBMSCore{
         bool applyCOMToSupportRegionLimit(const hrp::Pose3& rfin_abs, const hrp::Pose3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
             hrp::dmatrix hull_com = createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R], foot_vert_safe_fblr[L]);
             hrp::Vector2 cog = comin_abs.head(XY);
-            if(!isPointInHull2DOpenCV(cog,hull_com)){ calcNearestPointOnHull(cog, hull_com, cog); }//外に出たら最近傍点に頭打ち
+            if(!isPointInHull2D(cog,hull_com)){ calcNearestPointOnHull(cog, hull_com, cog); }//外に出たら最近傍点に頭打ち
             comin_abs.head(XY) = cog.head(XY);
             return true;
         }
@@ -571,10 +576,10 @@ class WBMSCore{
             }
         }
         void setFootRotHorizontalIfGoLanding(HumanPose& out){
-            if( out.tgt[rf].go_contact){
+            if( out.tgt[rf].go_contact || cp_force_go_contact[R]){
                 out.tgt[rf].abs.setRpy(0,0,out.tgt[rf].abs.rpy()(y));
             }
-            if( out.tgt[lf].go_contact){
+            if( out.tgt[lf].go_contact || cp_force_go_contact[L]){
                 out.tgt[lf].abs.setRpy(0,0,out.tgt[lf].abs.rpy()(y));
             }
         }
@@ -632,7 +637,7 @@ class WBMSCore{
             //        foot_landing_delay = 0;
             //      }
             hrp::Vector2 cp_dec_ragulated = com_pos + com_vel * ( sqrt( H_cur / G ) + foot_landing_delay);
-            if(!isPointInHull2DOpenCV(cp_dec_ragulated, hull_d)){
+            if(!isPointInHull2D(cp_dec_ragulated, hull_d)){
                 calcCrossPointOnHull(com_pos, cp_dec_ragulated, hull_d, cp_dec_ragulated);
                 com_vel_decel_ok = (cp_dec_ragulated - com_pos) / ( sqrt( H_cur / G ) + foot_landing_delay);
             }
@@ -641,14 +646,14 @@ class WBMSCore{
             hrp::dmatrix foot_vert3d_check_wld[LR], one_foot_hull2d[LR];
             for(int i=0; i<LR; i++){
                 foot_vert3d_check_wld[i] = (rp_ref_out.tgt[F(i)].abs.R * foot_vert3d_check[i]).colwise() + rp_ref_out.tgt[F(i)].abs.p; // rotate and translate in 3D
-                one_foot_hull2d[i] = makeConvexHull2DOpenCV( foot_vert3d_check_wld[i].topRows(XY) ); // project into 2D and make convex hull
+                one_foot_hull2d[i] = makeConvexHull2D( foot_vert3d_check_wld[i].topRows(XY) ); // project into 2D and make convex hull
             }
-            cp_force_go_contact[L] = !isPointInHull2DOpenCV(cp_dec_ragulated, one_foot_hull2d[R]); // if CapturePoint go out of R sole region, L foot must be go contact
-            cp_force_go_contact[R] = !isPointInHull2DOpenCV(cp_dec_ragulated, one_foot_hull2d[L]); // if CapturePoint go out of L sole region, R foot must be go contact
+            cp_force_go_contact[L] = !isPointInHull2D(cp_dec_ragulated, one_foot_hull2d[R]); // if CapturePoint go out of R sole region, L foot must be go contact
+            cp_force_go_contact[R] = !isPointInHull2D(cp_dec_ragulated, one_foot_hull2d[L]); // if CapturePoint go out of L sole region, R foot must be go contact
 
             //加速CP条件(ACP使用)
             hrp::Vector2 cp_acc_ragulated = com_pos - com_vel * sqrt( H_cur / G );
-            if(!isPointInHull2DOpenCV(cp_acc_ragulated, hull_a)){
+            if(!isPointInHull2D(cp_acc_ragulated, hull_a)){
                 calcCrossPointOnHull(com_pos, cp_acc_ragulated, hull_a, cp_acc_ragulated);
                 com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
             }
@@ -660,7 +665,7 @@ class WBMSCore{
             hrp::dmatrix rf_sole_verts_abs = (rfin_abs.R * make_rect_3d(rf_mgn)).colwise() + rfin_abs.p;
             hrp::dmatrix lf_sole_verts_abs = (lfin_abs.R * make_rect_3d(lf_mgn)).colwise() + lfin_abs.p;
             hrp::dmatrix both_sole_verts_abs = (hrp::dmatrix(3, rf_sole_verts_abs.cols()+lf_sole_verts_abs.cols()) << rf_sole_verts_abs, lf_sole_verts_abs).finished();
-            return makeConvexHull2DOpenCV(both_sole_verts_abs.topRows(XY));
+            return makeConvexHull2D(both_sole_verts_abs.topRows(XY));
         }
         void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const hrp::dvector6& rfwin, const hrp::dvector6& lfwin, hrp::Vector3& zmp_ans){
             hrp::Vector3 rfzmp,lfzmp;
@@ -679,19 +684,6 @@ class WBMSCore{
             }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
             zmp_ans(2) = 0;
         }
-        void calcXYMarginToHull(const hrp::Vector2& check_point, const hrp::dmatrix& hull, hrp::Vector4& margin_ans){
-            if(hull.rows() != 2){ std::cerr << "Invalid input for calcXYMarginToHull" << std::endl; dbgn(hull); }
-            hrp::Vector4 margin_abs;
-            hrp::Vector2 cross_pt, anchor_vec[4] = {hrp::Vector2(1,0), hrp::Vector2(-1,0), hrp::Vector2(0,1), hrp::Vector2(0,-1)};//前後左右に伸ばしたアンカーとの交点を見る
-            for(int direc=0;direc<4;direc++){
-                calcCrossPointOnHull(check_point, check_point+anchor_vec[direc], hull, cross_pt);
-                margin_abs(direc) = (cross_pt - check_point).norm();
-            }
-            margin_ans(0) =  margin_abs(0);
-            margin_ans(1) = -margin_abs(1);
-            margin_ans(2) =  margin_abs(2);
-            margin_ans(3) = -margin_abs(3);
-        }
         bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const hrp::dmatrix& hull, hrp::Vector2& pt_will_cross){
             if(hull.rows() != 2){ std::cerr << "Invalid input for calcCrossPointOnHull" << std::endl; dbgn(hull); }
             hrp::Vector2 anchor_vec = pt_out_goal - pt_in_start;
@@ -741,8 +733,12 @@ class WBMSCore{
             pt_ans = ans_nearest_pt;
             return ans_nearest_dist;
         }
-        hrp::dmatrix makeConvexHull2DOpenCV(const hrp::dmatrix& pts_2d){
-            if(pts_2d.rows() != 2){ std::cerr << "Invalid input for makeConvexHullOpenCV" << std::endl; dbgn(pts_2d); }
+        hrp::dmatrix makeConvexHull2D(const hrp::dmatrix& pts_2d){
+//            return makeConvexHull2D_OpenCV(pts_2d);
+            return makeConvexHull2D_Boost(pts_2d);
+        }
+        hrp::dmatrix makeConvexHull2D_OpenCV(const hrp::dmatrix& pts_2d){
+            if(pts_2d.rows() != 2){ std::cerr << "Invalid input for makeConvexHull2D_OpenCV" << std::endl; dbgn(pts_2d); }
             cv::Mat pts_2d_cv;
             std::vector<cv::Point2f> points(pts_2d.cols()), hull_2d_cv;
             for(int i=0; i<pts_2d.cols(); i++){
@@ -755,7 +751,26 @@ class WBMSCore{
             }
             return hull_2d;
         }
-        bool isPointInHull2DOpenCV(const hrp::Vector2& pt, const hrp::dmatrix& hull){
+        hrp::dmatrix makeConvexHull2D_Boost(const hrp::dmatrix& pts_2d){
+            if(pts_2d.rows() != 2){ std::cerr << "Invalid input for makeConvexHull_Boost" << std::endl; dbgn(pts_2d); }
+            bg_multi_point tmp;
+            tmp.resize(pts_2d.cols());
+            for(int i=0; i<pts_2d.cols(); i++){
+                tmp[i] = bg_point(pts_2d.col(i)(X), pts_2d.col(i)(Y));
+            }
+            bg_polygon hull_bg;
+            bg::convex_hull(tmp, hull_bg);
+            hrp::dmatrix hull_2d(2, hull_bg.outer().size());
+            for(int i=0; i<hull_bg.outer().size(); i++){
+                hull_2d.col(i) << hull_bg.outer()[i].x(), hull_bg.outer()[i].y();
+            }
+            return hull_2d;
+        }
+        bool isPointInHull2D(const hrp::Vector2& pt, const hrp::dmatrix& hull){
+//            return isPointInHull2D_OpenCV(pt, hull);
+            return isPointInHull2D_Boost(pt, hull);
+        }
+        bool isPointInHull2D_OpenCV(const hrp::Vector2& pt, const hrp::dmatrix& hull){
             if(hull.rows() != 2){ std::cerr << "Invalid input for isPointInHull2dOpenCV" << std::endl; dbgn(hull); }
             std::vector<cv::Point2f> hull_cv(hull.cols());
             for(int i=0; i<hull.cols(); i++){
@@ -763,6 +778,16 @@ class WBMSCore{
             }
             return (cv::pointPolygonTest(cv::Mat(hull_cv), cv::Point2f(pt(X),pt(Y)), false) > 0);
         }
+        bool isPointInHull2D_Boost(const hrp::Vector2& pt, const hrp::dmatrix& hull){
+            if(hull.rows() != 2){ std::cerr << "Invalid input for isPointInHull2D_Boost" << std::endl; dbgn(hull); }
+            const bg_point pt_bg(pt(X), pt(Y));
+            bg_polygon hull_bg;
+            hull_bg.outer().resize(hull.cols());
+            for(int i=0; i<hull.cols(); i++){
+                hull_bg.outer()[i] = bg_point(hull.col(i)(X), hull.col(i)(Y));
+            }
+            return bg::within(pt_bg, hull_bg);
+        }
         void applyLPFilter_post(HumanPose& tgt){
             if(is_initial_loop){
                 for(int i=0, l[1]={com}; i<1; i++){
@@ -782,9 +807,7 @@ class WBMSCore{
             com_oldold = com_old;
             com_old = comin;
         }
-
-
-        bool isPointInHullBoost(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){
+//        bool isPointInHullBoost(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){
 //              namespace bg = boost::geometry;
 //              typedef bg::model::d2::point_xy<double> point;
 //              typedef bg::model::polygon<point> polygon;
@@ -868,7 +891,7 @@ class WBMSCore{
 //        //      rp_ref_out.zmp(0) = comin_abs(0);
 //        //      rp_ref_out.zmp(1) = comin_abs(1);
 //              return true;
-            }
+//            }
 
 };
 

From cc42b25397b626cffbd5c4507f5d69907644ebf3 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 8 May 2019 19:57:27 +0900
Subject: [PATCH 161/217] call service from rtc

---
 python/hrpsys_config.py                       |   2 +
 rtc/AutoBalancer/MyUtils.h                    |   2 +
 .../WholeBodyMasterSlave.cpp                  | 364 ++++++++++++------
 .../WholeBodyMasterSlave.h                    |   9 +
 rtc/WholeBodyMasterSlave/wbms_core.h          | 247 +++++-------
 5 files changed, 349 insertions(+), 275 deletions(-)

diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index 163e0fdf43b..0427b0faaf7 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -410,6 +410,8 @@ def connectComps(self):
                 connectPorts(self.wbms.port("optionalDataOut"), self.abc.port("optionalData"))
                 connectPorts(self.st.port("actCapturePoint"), self.wbms.port("actCapturePoint"))
                 connectPorts(self.st.port("zmp"), self.wbms.port("zmp"))
+                connectPorts(self.wbms.port("AutoBalancerService"), self.abc.port("AutoBalancerService"))
+                connectPorts(self.wbms.port("StabilizerService"), self.st.port("StabilizerService"))
             
             connectPorts(self.abc.port("zmpOut"), self.st.port("zmpRef"))
             connectPorts(self.abc.port("baseRpyOut"), self.st.port("baseRpyIn"))
diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index b02739fdd6e..897633625c1 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -102,6 +102,8 @@ namespace hrp{
     }
 }
 
+inline bool has(const std::vector<std::string> v, const std::string& s){ return (std::find(v.begin(), v.end(), s) != v.end());}
+
 inline std::ostream& operator<<(std::ostream& os, hrp::Pose3& in){
     os << "p = " << in.p.transpose() << std::endl;
     os << "R =\n" << in.R << std::endl;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index be1c84f5cea..cdb9c8d3067 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -62,6 +62,8 @@ WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlo
         m_invdyn_dbgOut("invdyn_dbgOut", m_invdyn_dbg),
 #endif
         m_WholeBodyMasterSlaveServicePort("WholeBodyMasterSlaveService"),
+        m_AutoBalancerServicePort("AutoBalancerService"),
+        m_StabilizerServicePort("StabilizerService"),
         m_debugLevel(0)
 {
     m_service0.wholebodymasterslave(this);
@@ -117,6 +119,15 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     m_WholeBodyMasterSlaveServicePort.registerProvider("service0", "WholeBodyMasterSlaveService", m_service0);
     addPort(m_WholeBodyMasterSlaveServicePort);
 
+
+
+    m_AutoBalancerServicePort.registerConsumer("service0","AutoBalancerService", m_AutoBalancerServiceConsumer);
+    m_StabilizerServicePort.registerConsumer("service0","StabilizerService", m_StabilizerServiceConsumer);
+    addPort(m_AutoBalancerServicePort);
+    addPort(m_StabilizerServicePort);
+
+
+
     RTC::Properties& prop =  getProperties();
     coil::stringTo(m_dt, prop["dt"].c_str());
     RTC::Manager& rtcManager = RTC::Manager::instance();
@@ -140,11 +151,19 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     RTCOUT << "setup fullbody ik finished" << std::endl;
 
     wbms = boost::shared_ptr<WBMSCore>(new WBMSCore(m_dt));
-    wbms->wp.use_joints = getJointNameAll(fik->m_robot);
+    if(fik->m_robot->name() == "RHP4B"){
+        for(int i=0; i<fik->m_robot->numJoints(); i++){
+            if(fik->m_robot->joint(i)->name.find("-linear-joint") == std::string::npos){
+                wbms->wp.use_joints.push_back(fik->m_robot->joint(i)->name);
+            }
+        }
+    }else{
+        wbms->wp.use_joints = getJointNameAll(fik->m_robot);
+    }
     wbms->wp.use_targets.push_back("rleg");
     wbms->wp.use_targets.push_back("lleg");
     wbms->wp.use_targets.push_back("rarm");
-    wbms->wp.use_targets.push_back("rarm");
+    wbms->wp.use_targets.push_back("larm");
     wbms->wp.use_targets.push_back("com");
     wbms->wp.use_targets.push_back("head");
     sccp = boost::shared_ptr<CapsuleCollisionChecker>(new CapsuleCollisionChecker(fik->m_robot));
@@ -160,7 +179,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     t_ip->set(&output_ratio);
     q_ip = new interpolator(fik->numStates(), m_dt, interpolator::CUBICSPLINE, 1); // or HOFFARBIB, QUINTICSPLINE
     q_ip->clear();
-    avg_q_vel = hrp::dvector::Constant(fik->numStates(), 1.0); // all joint max avarage vel = 1.0 rad/s
+    avg_q_vel = hrp::dvector::Constant(fik->numStates(), 2.0); // all joint max avarage vel = 1.0 rad/s
     avg_q_acc = hrp::dvector::Constant(fik->numStates(), 16.0); // all joint max avarage acc = 16.0 rad/s^2
     avg_q_vel.tail(6).fill(std::numeric_limits<double>::max()); // no limit for base link vel
     avg_q_acc.tail(6).fill(std::numeric_limits<double>::max()); // no limit for base link acc
@@ -251,9 +270,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
                 preProcessForWholeBodyMasterSlave();
                 idsb.setInitState(fik->m_robot, m_dt);//逆動力学初期化
             }
-
-            fik->q_ref.head(m_qRef.data.length()) = hrp::to_dvector(m_qRef.data);//あえてseqからのbaselink poseは信用しない
-
             wbms->update();//////HumanSynchronizerの主要処理
             if(DEBUGP)wbms->rp_ref_out.print();
             addTimeReport("MainFunc");
@@ -275,13 +291,24 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
             wbms->act_rs.com = com;
             wbms->act_rs.zmp = ref_zmp;
 
-            const std::string tmp[] = {"R_CROTCH_R","R_CROTCH_P","R_CROTCH_Y","R_KNEE_P","R_ANKLE_R","R_ANKLE_P","L_CROTCH_R","L_CROTCH_P","L_CROTCH_Y","L_KNEE_P","L_ANKLE_R","L_ANKLE_P",};
-            const std::vector<std::string> lower(tmp, tmp+12);
+
+
+            if(wbms->rp_ref_out.tgt[rf].is_contact && !wbms->rp_ref_out.tgt[lf].is_contact){
+                ref_zmp.head(XY) = m_robot_vsafe->link(ee_ikc_map["rleg"].target_link_name)->p.head(XY);
+//                ref_zmp(X) + 0.05;
+            }
+            if(wbms->rp_ref_out.tgt[lf].is_contact && !wbms->rp_ref_out.tgt[rf].is_contact){
+                ref_zmp.head(XY) = m_robot_vsafe->link(ee_ikc_map["lleg"].target_link_name)->p.head(XY);
+//                ref_zmp(X) + 0.05;
+            }
+
+
+
+
             // qRef
             for (int i = 0; i < m_qRef.data.length(); i++ ){
-              if(!wbms->wp.use_lower && std::find(lower.begin(), lower.end(), fik->m_robot->joint(i)->name) != lower.end()){}// pass through lower limbs
-              else{
-                m_qRef.data[i] = output_ratio * m_robot_vsafe->joint(i)->q  + (1 - output_ratio) * m_qRef.data[i];
+              if(has(wbms->wp.use_joints, fik->m_robot->joint(i)->name)){
+                  m_qRef.data[i] = output_ratio * m_robot_vsafe->joint(i)->q  + (1 - output_ratio) * m_qRef.data[i];
               }
             }
 //            m_qRef.data = hrp::to_DoubleSeq(output_ratio * hrp::getQAll(m_robot_vsafe)  + (1 - output_ratio) * hrp::to_dvector(m_qRef.data));
@@ -326,7 +353,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     m_htrf_dbgOut.write();
     m_htlf_dbg.tm = m_qRef.tm;
     WBMSCore::WBMSPose3DToPose3D(wbms->hp_plot.tgt[lf].abs, m_htlf_dbg.data);
-    m_htlf_dbgOut.write();
+    m_htlf_dbgOut.write();m_AutoBalancerService
     m_htrh_dbg.tm = m_qRef.tm;
     WBMSCore::WBMSPose3DToPose3D(wbms->hp_plot.tgt[rh].abs, m_htrh_dbg.data);
     m_htrh_dbgOut.write();
@@ -463,148 +490,246 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
       tmp.rot_precision = deg2rad(3);
       ikc_list.push_back(tmp);
     }
-
     hrp::Vector3 act_cp = fik->m_robot->rootLink()->p + fik->m_robot->rootLink()->R * rel_act_cp;
-    hrp::Vector3 act_zmp = fik->m_robot->rootLink()->p + fik->m_robot->rootLink()->R * rel_act_zmp;
-    static int cp_count = 0;
-    static cp_enum state = CP_IDLE;
-
-//    if(act_zmp.head(XY).norm() > 0.1 && state == CP_IDLE){
-//        if(act_zmp(Y) > 0){
-//            state = CP_LF;
-//        }else{
-//            state = CP_RF;
+    act_cp(Z) = 0.3;
+//    act_cp = 0.9 * act_cp + 0.1 * (fik->m_robot->rootLink()->p + fik->m_robot->rootLink()->R * rel_act_cp);
+    static hrp::Vector3 track_cp = act_cp;
+
+    if(!std::isnan(act_cp(X)) && !std::isnan(act_cp(Y)) && !std::isnan(act_cp(Z))){
+        hrp::Vector3 direc = act_cp - track_cp;
+        const double speed = 0.3;
+        LIMIT_MINMAX(direc(X), -speed*m_dt, speed*m_dt);
+        LIMIT_MINMAX(direc(Y), -speed*m_dt, speed*m_dt);
+        LIMIT_MINMAX(direc(Z), -0.01*m_dt, 0.01*m_dt);
+//        if(direc.norm()>speed*m_dt){
+//            direc = direc.normalized() * speed*m_dt;
 //        }
-//    }
-//
-//    if(state == CP_IDLE){
-//        cp_count = 0;
-//    }
-//    else if(state == CP_LF || state == CP_RF){
-//        if(cp_count ++ > 50){
-//            state = CP_STATIC;
-//        }
-//    }
-//    else if(state == CP_STATIC){
-//        if(cp_count ++ > 100){
-//            state = CP_IDLE;
-//        }
-//    }
-//    const double FH = 0.05;
+        track_cp += direc;
+    }
 
+//    hrp::Vector3 act_zmp = fik->m_robot->rootLink()->p + fik->m_robot->rootLink()->R * rel_act_zmp;
+//    static int cp_count = 0;
+    const double L = 100;
+    const double sink = 0.0;
+    const double fh = 0.05;
+    const bool start_cp = false;
+//    const bool start_cp = ((act_cp - fik->m_robot->calcCM()).head(XY).norm()>0.025);
+//    dbgv(act_cp);
+//    dbgv(track_cp);
+//    dbgv(fik->m_robot->calcCM());
 
-    if(wbms->wp.use_lower){
+
+
+    if (act_cp.head(XY).norm()>0.02)
+    {
+
+
+
+
+    try {
+        OpenHRP::AutoBalancerService::FootstepsSequence fss;
+        OpenHRP::AutoBalancerService::StepParamsSequence spss;
+        fss.length(2);
+        spss.length(2);
+        fss[0].fs.length(1);
+        fss[1].fs.length(1);
+        spss[0].sps.length(1);
+        spss[1].sps.length(1);
+        fss[0].fs[0].leg = "rleg";
+        fss[0].fs[0].pos[0] = 0;
+        fss[0].fs[0].pos[1] = -0.1;
+        fss[0].fs[0].pos[2] = 0;
+        fss[0].fs[0].rot[0] = 1;
+        fss[0].fs[0].rot[1] = 0;
+        fss[0].fs[0].rot[2] = 0;
+        fss[0].fs[0].rot[3] = 0;
+        fss[1].fs[0].leg = "lleg";
+        fss[1].fs[0].pos[0] = act_cp(Y)>0 ? 0.3*act_cp.head(XY).normalized()(X) : 0;
+        fss[1].fs[0].pos[1] = act_cp(Y)>0 ? 0.3*act_cp.head(XY).normalized()(Y) : 0.1;
+        fss[1].fs[0].pos[2] = 0;
+        fss[1].fs[0].rot[0] = 1;
+        fss[1].fs[0].rot[1] = 0;
+        fss[1].fs[0].rot[2] = 0;
+        fss[1].fs[0].rot[3] = 0;
+        spss[0].sps[0].step_height = 0.1;
+        spss[1].sps[0].step_height = 0.1;
+        spss[0].sps[0].step_time = 0.01;
+        spss[1].sps[0].step_time = 0.5;
+        spss[0].sps[0].toe_angle = 0;
+        spss[1].sps[0].toe_angle = 0;
+        spss[0].sps[0].heel_angle = 0;
+        spss[1].sps[0].heel_angle = 0;
+//        m_AutoBalancerServiceConsumer->setFootSteps(fss,1);
+        m_AutoBalancerServiceConsumer->setFootStepsWithParam(fss,spss,1);
+    } catch(CORBA::COMM_FAILURE& ex) {
+//        dbg("1");
+    } catch(CORBA::MARSHAL& ex) {
+//        dbg("2");
+    } catch(CORBA::BAD_PARAM& ex) {
+//        dbg("3");
+    } catch(CORBA::OBJECT_NOT_EXIST& ex) {
+//        dbg("4");
+    } catch(CORBA::SystemException& ex) {
+//        dbg("5");
+    } catch(CORBA::Exception&) {
+//        dbg("6");
+    } catch(omniORB::fatalException& fe) {
+//        dbg("7");
+    }
+    catch(...) {
+//        dbg("8");
+    }
+//    dbg("ok");
+    // サービスポートによる処理
+}
+
+
+
+
+
+
+
+
+    if(has(wbms->wp.use_targets, "rleg")){
         IKConstraint tmp;
         tmp.target_link_name = ee_ikc_map["rleg"].target_link_name;
         tmp.localPos = ee_ikc_map["rleg"].localPos;
         tmp.localR = ee_ikc_map["rleg"].localR;
         tmp.targetPos = rf_ref.p;
+
+
+//        if(start_cp){
+//            double h = fh * (-sin(2* M_PI * loop / L) - sink);
+//            wbms->rp_ref_out.tgt[rf].is_contact = (h<0);
+//            LIMIT_MIN(h, 0);
+//            tmp.targetPos(Z) += h;
+//            static hrp::Vector2 xy = tmp.targetPos.head(XY);
+//            if(!wbms->rp_ref_out.tgt[rf].is_contact){
+//                xy = track_cp.head(XY);
+//            }
+//            tmp.targetPos.head(XY) += xy;
+//        }
+
+
         tmp.targetRpy = rf_ref.rpy();
         if(wbms->rp_ref_out.tgt[rf].is_contact){
             tmp.constraint_weight = hrp::dvector6::Constant(3);
         }else{
             tmp.constraint_weight = hrp::dvector6::Constant(0.1);
         }
-
-//        if(state == CP_RF){
-//            if(cp_count==1){
-//                rt += (hrp::Vector3() << act_zmp.head(XY).normalized() * 0.1, FH).finished();
-//                wbms->rp_ref_out.tgt[rf].is_contact = false;
-//            }else if(cp_count==50){
-////                rt += hrp::Vector3(0,0,-FH);
-//                wbms->rp_ref_out.tgt[rf].is_contact = true;
-//            }
-//        }
-//        if(state == CP_STATIC){
-//            if(rt(Z) > 0){
-//                rt(Z) -= FH/450;
-//            }
-//        }
-//        tmp.targetPos += rt;
-
-
         ikc_list.push_back(tmp);
     }
-    if(wbms->wp.use_lower){
+    if(has(wbms->wp.use_targets, "lleg")){
         IKConstraint tmp;
         tmp.target_link_name = ee_ikc_map["lleg"].target_link_name;
         tmp.localPos = ee_ikc_map["lleg"].localPos;
         tmp.localR = ee_ikc_map["lleg"].localR;
         tmp.targetPos = lf_ref.p;
+
+
+//        if(start_cp){
+//            double h = fh * (sin(2*M_PI * loop / L) - sink);
+//            wbms->rp_ref_out.tgt[lf].is_contact = (h<0);
+//            LIMIT_MIN(h, 0);
+//            tmp.targetPos(Z) += h;
+//            static hrp::Vector2 xy = tmp.targetPos.head(XY);
+//            if(!wbms->rp_ref_out.tgt[lf].is_contact){
+//                xy = track_cp.head(XY);
+//            }
+//            tmp.targetPos.head(XY) += xy;
+//        }
+
+
+
         tmp.targetRpy = lf_ref.rpy();
         if(wbms->rp_ref_out.tgt[lf].is_contact){
             tmp.constraint_weight = hrp::dvector6::Constant(3);
         }else{
             tmp.constraint_weight = hrp::dvector6::Constant(0.1);
         }
-
-//
-//        if(state == CP_LF){
-//            if(cp_count==1){
-//                lt += (hrp::Vector3() << act_zmp.head(XY).normalized() * 0.1, FH).finished();
-//                wbms->rp_ref_out.tgt[lf].is_contact = false;
-//            }else if(cp_count==50){
-////                lt += hrp::Vector3(0,0,-FH);
-//                wbms->rp_ref_out.tgt[lf].is_contact = true;
-//            }
-//        }
-//        if(state == CP_STATIC){
-//            if(lt(Z) > 0){
-//                lt(Z) -= FH/50;
-//            }
-//        }
-//        tmp.targetPos += lt;
-
         ikc_list.push_back(tmp);
     }
-    const double dist = 0.0;
-    {
+    if(has(wbms->wp.use_targets, "rarm")){
         IKConstraint tmp;
         tmp.target_link_name = ee_ikc_map["rarm"].target_link_name;
         tmp.localPos = ee_ikc_map["rarm"].localPos;
         tmp.localR = ee_ikc_map["rarm"].localR;
-        if((rh_ref.p-lh_ref.p).norm()<dist){
-            tmp.targetPos = (rh_ref.p+lh_ref.p)/2 + (rh_ref.p-lh_ref.p).normalized()*dist/2;
-        }else{
-            tmp.targetPos = rh_ref.p;
-        }
+        tmp.targetPos = rh_ref.p;
         tmp.targetRpy = rh_ref.rpy();
         tmp.constraint_weight = hrp::dvector6::Constant(0.1);
+
+
+
+
+        if(start_cp){
+             tmp.constraint_weight << 0,0,0,0,0,0;
+             if(act_cp(Y)<0 || true){
+                 hrp::Vector2 h_direc = act_cp.head(XY);
+                 h_direc = h_direc.normalized() * 0.5;
+                 tmp.targetPos << h_direc, 0;
+                 tmp.constraint_weight << 1,1,1,0,0,0;
+             }
+             tmp.targetPos << -0.5, -0.3, 0;
+
+
+
+
+         }
+
+
+
         tmp.pos_precision = 3e-3;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
-    }{
+    }
+    if(has(wbms->wp.use_targets, "larm")){
         IKConstraint tmp;
         tmp.target_link_name = ee_ikc_map["larm"].target_link_name;
         tmp.localPos = ee_ikc_map["larm"].localPos;
         tmp.localR = ee_ikc_map["larm"].localR;
-        if((rh_ref.p-lh_ref.p).norm()<dist){
-            tmp.targetPos = (rh_ref.p+lh_ref.p)/2 + (lh_ref.p-rh_ref.p).normalized()*dist/2;
-        }else{
-            tmp.targetPos = lh_ref.p;
-        }
+        tmp.targetPos = lh_ref.p;
         tmp.targetRpy = lh_ref.rpy();
         tmp.constraint_weight = hrp::dvector6::Constant(0.1);
+
+
+
+
+        if(start_cp){
+            tmp.constraint_weight << 0,0,0,0,0,0;
+            if(act_cp(Y)>0 || true){
+                hrp::Vector2 h_direc = act_cp.head(XY);
+                h_direc = h_direc.normalized() * 0.5;
+                tmp.targetPos << h_direc, 0;
+                tmp.constraint_weight << 1,1,1,0,0,0;
+            }
+            tmp.targetPos << -0.5, 0.3, 0;
+        }
+
+
+
+
         tmp.pos_precision = 3e-3;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
     }
-    if(fik->m_robot->link("HEAD_JOINT1") != NULL){
-        IKConstraint tmp;
-        tmp.target_link_name = "HEAD_JOINT1";
-        tmp.targetRpy = head_ref.rpy();
-        tmp.constraint_weight << 0,0,0,0,0.1,0.1;
-        tmp.rot_precision = deg2rad(1);
-        ikc_list.push_back(tmp);
+    if(has(wbms->wp.use_targets, "head")){
+        if(fik->m_robot->link("HEAD_JOINT1") != NULL){
+            IKConstraint tmp;
+            tmp.target_link_name = "HEAD_JOINT1";
+            tmp.targetRpy = head_ref.rpy();
+            tmp.constraint_weight << 0,0,0,0,0.1,0.1;
+            tmp.rot_precision = deg2rad(1);
+            ikc_list.push_back(tmp);
+        }
+        if(fik->m_robot->link("HEAD_P") != NULL){
+            IKConstraint tmp;
+            tmp.target_link_name = "HEAD_P";
+            tmp.targetRpy = head_ref.rpy();
+            tmp.constraint_weight << 0,0,0,0,0.1,0.1;
+            tmp.rot_precision = deg2rad(1);
+            ikc_list.push_back(tmp);
+        }
     }
-    // if(robot_in->link("HEAD_P") != NULL){
-    //     IKConstraint tmp;
-    //     tmp.target_link_name = "HEAD_P";
-    //     tmp.targetRpy = head_ref.rpy();
-    //     tmp.constraint_weight << 0,0,0,0,0.1,0.1;
-    //     tmp.rot_precision = deg2rad(1);
-    //     ikc_list.push_back(tmp);
-    // }
     if(wbms->rp_ref_out.tgt[rf].is_contact){
         sccp->avoid_priority.head(12).head(6).fill(4);
     }else{
@@ -659,12 +784,21 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
     }
 
 
-    if(wbms->wp.use_lower){
+    if(has(wbms->wp.use_targets, "com")){
         IKConstraint tmp;
         tmp.target_link_name = "COM";
         tmp.localPos = hrp::Vector3::Zero();
         tmp.localR = hrp::Matrix33::Identity();
         tmp.targetPos = com_ref.p;// COM height will not be constraint
+
+
+
+//        if(start_cp){
+//            tmp.targetPos.head(XY) += track_cp.head(XY);
+//            tmp.targetPos(Z) = track_cp(Z);
+//        }
+
+
         tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
 //        tmp.constraint_weight << 3,3,0.01,0.1,0.1,0;
         tmp.constraint_weight << 3,3,0.01,0,0,0;
@@ -680,10 +814,8 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
     if( fik->m_robot->link("CHEST_JOINT0") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT0")->jointId) = 0.1;//JAXON
     if( fik->m_robot->link("CHEST_JOINT1") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT1")->jointId) = 0.1;
     if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 0.1;
-    if( fik->m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_Y")->jointId) = 0.1;//K
-    if( fik->m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_P")->jointId) = 0.1;
-//        if( fik->m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_Y")->jointId) = 0;//K
-//        if( fik->m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_P")->jointId) = 0;
+//    if( fik->m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_Y")->jointId) = 0.1;//K
+//    if( fik->m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_P")->jointId) = 0.1;
 //    fik->q_ref_constraint_weight(fik->m_robot->link("CHEST_Y")->jointId) = 1;
 //    fik->q_ref_constraint_weight(fik->m_robot->link("CHEST_P")->jointId) = 1;
 
@@ -716,11 +848,15 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         LIMIT_MINMAX(fik->m_robot->joint(i)->q, fik->m_robot->joint(i)->llimit, fik->m_robot->joint(i)->ulimit);
     }
 
-//    fik_in->q_ref = init_sync_state;
-    if(fik->m_robot->name() == "RHP4B"){
-        fik->dq_weight_all.fill(0);
-        fik->dq_weight_all.head(31).fill(1);
-        fik->dq_weight_all.tail(6).fill(1);
+
+    fik->q_ref.head(m_qRef.data.length()) = hrp::to_dvector(m_qRef.data);//あえてseqからのbaselink poseは信用しない
+
+
+    for(int i=0; i<fik->m_robot->numJoints(); i++){
+        if(!has(wbms->wp.use_joints, fik->m_robot->joint(i)->name)){
+            fik->dq_weight_all(i) = 0;
+            fik->m_robot->joint(i)->q = m_qRef.data[i];
+        }
     }
 
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 029318af6b6..f47af2a5227 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -15,6 +15,8 @@
 #include <time.h>
 
 #include "WholeBodyMasterSlaveService_impl.h"
+#include "../Stabilizer/StabilizerService_impl.h"
+#include "../AutoBalancer/AutoBalancerService_impl.h"
 #include "wbms_core.h"
 
 //#define USE_DEBUG_PORT
@@ -150,6 +152,13 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
 
         WholeBodyMasterSlaveService_impl m_service0;
 
+
+        RTC::CorbaPort m_AutoBalancerServicePort;
+        RTC::CorbaPort m_StabilizerServicePort;
+        RTC::CorbaConsumer<OpenHRP::AutoBalancerService> m_AutoBalancerServiceConsumer;
+        RTC::CorbaConsumer<OpenHRP::StabilizerService> m_StabilizerServiceConsumer;
+
+
     private:
         double m_dt;
         unsigned int loop;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index a412fdff2c3..9d1aa0db579 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -14,13 +14,15 @@
 
 // geometry
 #include <boost/geometry.hpp>
-#include <boost/geometry/geometries/polygon.hpp>
 #include <boost/geometry/geometries/point_xy.hpp>
 #include <boost/geometry/multi/geometries/multi_point.hpp>
-#include <boost/assign/list_of.hpp>
+#include <boost/geometry/geometries/linestring.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+//#include <boost/assign/list_of.hpp>
 namespace bg = boost::geometry;
 typedef bg::model::d2::point_xy<double> bg_point;
 typedef bg::model::multi_point<bg_point> bg_multi_point;
+typedef bg::model::linestring<bg_point> bg_linestring;
 typedef bg::model::polygon<bg_point> bg_polygon;
 
 //#include<Eigen/StdVector>
@@ -34,6 +36,34 @@ typedef bg::model::polygon<bg_point> bg_polygon;
 
 #define DEBUG 0
 
+inline bg_point to_bg_point(const hrp::Vector2& hrp_point2d){ return bg_point(hrp_point2d(X), hrp_point2d(Y)); }
+inline hrp::Vector2 to_Vector2(const bg_point& bg_point2d){ return hrp::Vector2(bg_point2d.x(), bg_point2d.y()); }
+inline bg_polygon to_bg_hull(const hrp::dmatrix& hrp_hull){
+    if(hrp_hull.rows() != 2){ std::cerr << "Invalid input for to_bg_hull" << std::endl; dbgn(hrp_hull); }
+    bg_polygon hull_bg;
+    hull_bg.outer().resize(hrp_hull.cols());
+    for(int i=0; i<hrp_hull.cols(); i++){
+        hull_bg.outer()[i] = bg_point(hrp_hull.col(i)(X), hrp_hull.col(i)(Y));
+    }
+    return hull_bg;
+}
+
+class LIP_model{
+    private:
+        double G, dt;
+    public:
+        LIP_model(){};
+        double com_height;
+        std::deque<hrp::Vector3> com;
+        void update(const hrp::Vector3 com_new){ com.pop_back(); com.push_front(com_new); }
+        hrp::Vector3 vel(){ return (com[1] - com[0]) / dt ; }
+        hrp::Vector3 DCM(){ return com[0] + vel() * sqrt( com_height / G ); }
+        hrp::Vector3 CCM(){ return com[0] - vel() * sqrt( com_height / G ); }
+        hrp::Vector3 acc(){ return (com[2] - 2* com[1] + com[0]) / ( dt * dt ); }
+//        hrp::Vector3 ZMP(){ return (hrp::Vector3() << com[0].head(XY) - acc().head(XY) * ( com_height / G ), com[0](Z) - com_height).finished; }
+};
+
+
 class BiquadIIRFilterVec{
     private:
         IIRFilter filters[XYZ];
@@ -325,7 +355,7 @@ class WBMSCore{
     private:
         double HZ, DT;
         BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters, com_in_filter;
-        hrp::Vector3 com_old, com_oldold, comacc, com_CP_ref_old, r_zmp_raw;
+        hrp::Vector3 com_old, com_oldold, comacc, com_CP_ref_old;
         std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rpy_filters;
         HumanPose rp_ref_out_old, hp_swap_checked;
         unsigned int loop;
@@ -343,20 +373,20 @@ class WBMSCore{
         hrp::dvector6 invdyn_ft;
         hrp::Vector4 foot_vert_act_fblr[LR], foot_vert_safe_fblr[LR], foot_vert_check_fblr[LR];
         struct WBMSparameters {
-                double auto_swing_foot_landing_threshold;
-                double human_to_robot_ratio;
-                bool set_com_height_fix;
-                double set_com_height_fix_val;
-                double swing_foot_height_offset;
-                double upper_body_rmc_ratio;
-                bool use_head;
-                bool use_upper;
-                bool use_lower;
-                std::vector<std::string> use_joints;
-                std::vector<std::string> use_targets;
+            double auto_swing_foot_landing_threshold;
+            double human_to_robot_ratio;
+            bool set_com_height_fix;
+            double set_com_height_fix_val;
+            double swing_foot_height_offset;
+            double upper_body_rmc_ratio;
+            bool use_head;
+            bool use_upper;
+            bool use_lower;
+            std::vector<std::string> use_joints;
+            std::vector<std::string> use_targets;
         } wp;
         struct ActualRobotState {
-                hrp::Vector3 com, zmp;
+            hrp::Vector3 com, zmp;
         } act_rs;
 
         WBMSCore(const double& dt){
@@ -365,7 +395,6 @@ class WBMSCore{
             loop = 0;
             //////////  hrp::Vector は初期化必要  ///////////
             com_old = com_oldold = comacc = hrp::Vector3::Zero();
-            r_zmp_raw = hrp::Vector3::Zero();
             cp_dec = cp_acc = hrp::Vector3::Zero();
             invdyn_ft = hrp::dvector6::Zero();
             tgt_pos_filters.resize(num_pose_tgt);
@@ -482,8 +511,7 @@ class WBMSCore{
             com_forcp_ref_old = com_forcp_ref;
             applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
             applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, rp_ref_out.tgt[com].abs.p);
-            applyLPFilter_post                       (rp_ref_out);
-            r_zmp_raw = rp_ref_out.tgt[zmp].abs.p;
+//            applyLPFilter_post                       (rp_ref_out);
             applyZMPCalcFromCOM                 (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);
             modifyFootRotAndXYForContact        (rp_ref_out);
             H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[lf].abs.p(Z));
@@ -559,9 +587,9 @@ class WBMSCore{
         }
         bool applyCOMToSupportRegionLimit(const hrp::Pose3& rfin_abs, const hrp::Pose3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
             hrp::dmatrix hull_com = createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R], foot_vert_safe_fblr[L]);
-            hrp::Vector2 cog = comin_abs.head(XY);
-            if(!isPointInHull2D(cog,hull_com)){ calcNearestPointOnHull(cog, hull_com, cog); }//外に出たら最近傍点に頭打ち
-            comin_abs.head(XY) = cog.head(XY);
+            if(!isPointInHull2D(comin_abs.head(XY), hull_com)){
+                comin_abs.head(XY) = calcNearestPointOnHull(comin_abs.head(XY), hull_com);
+            }//外に出たら最近傍点に頭打ち
             return true;
         }
         void overwriteFootZFromFootLandOnCommand(HumanPose& out){
@@ -577,10 +605,10 @@ class WBMSCore{
         }
         void setFootRotHorizontalIfGoLanding(HumanPose& out){
             if( out.tgt[rf].go_contact || cp_force_go_contact[R]){
-                out.tgt[rf].abs.setRpy(0,0,out.tgt[rf].abs.rpy()(y));
+                out.tgt[rf].abs.setRpy(0, 0, out.tgt[rf].abs.rpy()(y));
             }
             if( out.tgt[lf].go_contact || cp_force_go_contact[L]){
-                out.tgt[lf].abs.setRpy(0,0,out.tgt[lf].abs.rpy()(y));
+                out.tgt[lf].abs.setRpy(0, 0, out.tgt[lf].abs.rpy()(y));
             }
         }
         void limitFootVelNearGround(HumanPose& out){
@@ -684,59 +712,51 @@ class WBMSCore{
             }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
             zmp_ans(2) = 0;
         }
-        bool calcCrossPointOnHull(const hrp::Vector2& pt_in_start, const hrp::Vector2& pt_out_goal, const hrp::dmatrix& hull, hrp::Vector2& pt_will_cross){
+        bool calcCrossPointOnHull(const hrp::Vector2& inside_start_pt, const hrp::Vector2& outside_goal_pt, const hrp::dmatrix& hull, hrp::Vector2& ans_cross_pt){
             if(hull.rows() != 2){ std::cerr << "Invalid input for calcCrossPointOnHull" << std::endl; dbgn(hull); }
-            hrp::Vector2 anchor_vec = pt_out_goal - pt_in_start;
-            for(int i=0;i<hull.cols();i++){
-                int i_nxt = (i!=hull.cols()-1 ? i+1 : 0);
-                hrp::Vector2 cur_pt = hull.col(i), nxt_pt = hull.col(i_nxt);
-                hrp::Vector2 cur_edge = nxt_pt - cur_pt;
-                double dBunbo = hrp::hrpVector2Cross(anchor_vec,cur_edge);
-                if( dBunbo != 0.0) {//平行の場合を外す
-                    hrp::Vector2 vectorAC = hull.col(i) - pt_in_start;
-                    double dR = hrp::hrpVector2Cross(vectorAC,cur_edge) / dBunbo;
-                    double dS = hrp::hrpVector2Cross(vectorAC,anchor_vec) / dBunbo;
-                    if(dR > 1e-9 && dS >= 0.0 && dS <= 1.0){//dRには数値誤差が乗る
-                        pt_will_cross = pt_in_start + dR * anchor_vec;
-                        return true;
-                    }
-                }
-            }
-            return false;
-        }
-        double calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const hrp::dmatrix& hull, hrp::Vector2& pt_ans){
+            bg_linestring anchor_vec;
+            anchor_vec.push_back(to_bg_point(inside_start_pt));
+            anchor_vec.push_back(to_bg_point(outside_goal_pt));
+            bg_multi_point bg_ans_cross_pt;
+            bg::intersection(anchor_vec, to_bg_hull(hull), bg_ans_cross_pt);
+            switch(bg_ans_cross_pt.size()){
+                case 0:
+                    return false;
+                case 1:
+                    ans_cross_pt = to_Vector2(bg_ans_cross_pt[0]);
+                    return true;
+                default:
+                    std::cerr << "Number of the cross point must be 0 or 1 (current = " << bg_ans_cross_pt.size() << ") something wrong!" << std::endl;
+                    return false;
+            }
+        }
+        hrp::Vector2 calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const hrp::dmatrix& hull){
             if(hull.rows() != 2){ std::cerr << "Invalid input for calcNearestPointOnHull" << std::endl; dbgn(hull); }
             double cur_nearest_dist, ans_nearest_dist;
             hrp::Vector2 cur_nearest_pt, ans_nearest_pt;
-            for(int i=0;i<hull.cols();i++){
-                int i_nxt = (i!=hull.cols()-1 ? i+1 : 0);
-                hrp::Vector2 cur_pt = hull.col(i), nxt_pt = hull.col(i_nxt);
-                hrp::Vector2 cur_edge = nxt_pt - cur_pt;
-                hrp::Vector2 tgt_pt_v = tgt_pt - cur_pt;
-                double tgt_pt_projected_length = tgt_pt_v.dot(cur_edge.normalized());
-                if(tgt_pt_projected_length > cur_edge.norm() ){//tgt_pt's nearest point is on the　i+1-th vertex
-                    cur_nearest_pt = nxt_pt;
-                }else if(tgt_pt_projected_length < 0 ){//tgt_pt's nearest point is on the　i-th vertex
-                    cur_nearest_pt = cur_pt;
-                }else{//tgt_pt's nearest point is on the line
-                    cur_nearest_pt = cur_pt + tgt_pt_projected_length * cur_edge.normalized();
-                }
+            for(int i=0; i<hull.cols()-1; i++){// first and end point in hull are same
+                const hrp::Vector2 cur_pt = hull.col(i), nxt_pt = hull.col(i+1);
+                const hrp::Vector2 cur_edge = nxt_pt - cur_pt;
+                const hrp::Vector2 tgt_pt_v = tgt_pt - cur_pt;
+                double cur_pt_to_projected_tgt_pt = tgt_pt_v.dot(cur_edge.normalized()); // Distance from cur_pt to the projected tgt_pt on to cur_edge
+                LIMIT_MINMAX(cur_pt_to_projected_tgt_pt, 0, cur_edge.norm()); // limit the nearest point onto the line segment "cur_edge"
+                cur_nearest_pt = cur_pt + cur_pt_to_projected_tgt_pt * cur_edge.normalized();
                 cur_nearest_dist = (tgt_pt - cur_nearest_pt).norm();
-                if(i==0){//set first candidate as nearest
-                    ans_nearest_dist = cur_nearest_dist;
-                    ans_nearest_pt = cur_nearest_pt;
-                }else if( cur_nearest_dist < ans_nearest_dist ){//update nearest candidate
+                if( cur_nearest_dist < ans_nearest_dist || i==0 ){//update nearest candidate
                     ans_nearest_dist = cur_nearest_dist;
                     ans_nearest_pt = cur_nearest_pt;
                 }
             }
-            pt_ans = ans_nearest_pt;
-            return ans_nearest_dist;
+            return ans_nearest_pt;
         }
         hrp::dmatrix makeConvexHull2D(const hrp::dmatrix& pts_2d){
 //            return makeConvexHull2D_OpenCV(pts_2d);
             return makeConvexHull2D_Boost(pts_2d);
         }
+        bool isPointInHull2D(const hrp::Vector2& pt, const hrp::dmatrix& hull){
+//            return isPointInHull2D_OpenCV(pt, hull);
+            return isPointInHull2D_Boost(pt, hull);
+        }
         hrp::dmatrix makeConvexHull2D_OpenCV(const hrp::dmatrix& pts_2d){
             if(pts_2d.rows() != 2){ std::cerr << "Invalid input for makeConvexHull2D_OpenCV" << std::endl; dbgn(pts_2d); }
             cv::Mat pts_2d_cv;
@@ -756,20 +776,16 @@ class WBMSCore{
             bg_multi_point tmp;
             tmp.resize(pts_2d.cols());
             for(int i=0; i<pts_2d.cols(); i++){
-                tmp[i] = bg_point(pts_2d.col(i)(X), pts_2d.col(i)(Y));
+                tmp[i] = to_bg_point(pts_2d.col(i));
             }
             bg_polygon hull_bg;
             bg::convex_hull(tmp, hull_bg);
             hrp::dmatrix hull_2d(2, hull_bg.outer().size());
             for(int i=0; i<hull_bg.outer().size(); i++){
-                hull_2d.col(i) << hull_bg.outer()[i].x(), hull_bg.outer()[i].y();
+                hull_2d.col(i) << to_Vector2(hull_bg.outer()[i]);
             }
             return hull_2d;
         }
-        bool isPointInHull2D(const hrp::Vector2& pt, const hrp::dmatrix& hull){
-//            return isPointInHull2D_OpenCV(pt, hull);
-            return isPointInHull2D_Boost(pt, hull);
-        }
         bool isPointInHull2D_OpenCV(const hrp::Vector2& pt, const hrp::dmatrix& hull){
             if(hull.rows() != 2){ std::cerr << "Invalid input for isPointInHull2dOpenCV" << std::endl; dbgn(hull); }
             std::vector<cv::Point2f> hull_cv(hull.cols());
@@ -780,13 +796,7 @@ class WBMSCore{
         }
         bool isPointInHull2D_Boost(const hrp::Vector2& pt, const hrp::dmatrix& hull){
             if(hull.rows() != 2){ std::cerr << "Invalid input for isPointInHull2D_Boost" << std::endl; dbgn(hull); }
-            const bg_point pt_bg(pt(X), pt(Y));
-            bg_polygon hull_bg;
-            hull_bg.outer().resize(hull.cols());
-            for(int i=0; i<hull.cols(); i++){
-                hull_bg.outer()[i] = bg_point(hull.col(i)(X), hull.col(i)(Y));
-            }
-            return bg::within(pt_bg, hull_bg);
+            return bg::within(to_bg_point(pt), to_bg_hull(hull));
         }
         void applyLPFilter_post(HumanPose& tgt){
             if(is_initial_loop){
@@ -807,92 +817,7 @@ class WBMSCore{
             com_oldold = com_old;
             com_old = comin;
         }
-//        bool isPointInHullBoost(const hrp::Vector3& lfin_abs, const hrp::Vector3& rfin_abs, hrp::Vector3& comin_abs){
-//              namespace bg = boost::geometry;
-//              typedef bg::model::d2::point_xy<double> point;
-//              typedef bg::model::polygon<point> polygon;
-//              polygon lr_region,s_region;
-//              const double XUMARGIN = 0.04;//CHIDORI
-//              const double XLMARGIN = -0.02;
-//              const double YUMARGIN = 0.01;
-//              const double YLMARGIN = -0.01;
-//              bg::model::linestring<point> s_line = boost::assign::list_of<point>
-//                (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YLMARGIN)//LFの右下
-//                (lfin_abs(0) + XLMARGIN, lfin_abs(1) + YUMARGIN)//LFの左下
-//                (lfin_abs(0) + XUMARGIN, lfin_abs(1) + YUMARGIN)//LFの左上
-//                (lfin_abs(0) + XUMARGIN, lfin_abs(1) + YLMARGIN)//LFの右上
-//                (rfin_abs(0) + XLMARGIN, rfin_abs(1) + YLMARGIN)//RFの右下
-//                (rfin_abs(0) + XLMARGIN, rfin_abs(1) + YUMARGIN)//RFの左下
-//                (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YUMARGIN)//RFの左上
-//                (rfin_abs(0) + XUMARGIN, rfin_abs(1) + YLMARGIN)//RFの右上
-//                     ;
-//              bg::convex_hull(s_line, s_region);
-//              point com2d(comin_abs(0),comin_abs(1));
-//              Eigen::Vector2d ans_point(comin_abs(0),comin_abs(1));
-//              enum point_state_t {PT_IN_NO_PATTERN = -1, PT_IN_REGION, PT_FIX_TO_EDGE, PT_FIX_TO_VERTEX};
-//              point_state_t pt_state = PT_IN_NO_PATTERN;
-//              if(bg::within(com2d, s_region)){//対象の点が凸包の内部に存在するかチェックする
-//                pt_state = PT_IN_REGION;
-//              }else{
-//        //        cout<<"COM out of s_region"<<endl;
-//                Eigen::Vector2d check_point(com2d.x(),com2d.y());
-//                int ans_lid=-1;
-//                for(int i=0;i<s_region.outer().size()-1;i++){//対象の点がある線分への垂線を有するかチェックする
-//                  Eigen::Vector2d cur_vert(s_region.outer()[i].x(),s_region.outer()[i].y());
-//                  Eigen::Vector2d next_vert(s_region.outer()[i+1].x(),s_region.outer()[i+1].y());
-//                  Eigen::Vector2d edge_v = next_vert - cur_vert;
-//                  Eigen::Vector2d tgt_pt_v = check_point - cur_vert;
-//                  //ある線分への垂線を有し，かつ外側(時計回りエッジに対して左側)に存在するなら，対象の点からそのエッジへの垂線の交点が最近傍点
-//                  if(edge_v.dot(tgt_pt_v)/edge_v.norm() > 0 && edge_v.dot(tgt_pt_v)/edge_v.norm() < edge_v.norm() && (edge_v(0)*tgt_pt_v(1)-edge_v(1)*tgt_pt_v(0)) > 0){
-//                    ans_lid = i;
-//                    ans_point = cur_vert + edge_v.normalized() * (edge_v.dot(tgt_pt_v)/edge_v.norm());
-//                    pt_state = PT_FIX_TO_EDGE;
-//                    break;
-//                  }
-//                }
-//                if(pt_state == PT_FIX_TO_EDGE){
-//                  cout<<"point will on the line:"<<ans_lid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
-//                }else{//対象の点が線分への垂線を持たなければ答えを頂点に絞ってチェックする
-//                  double cur_min_dis = bg::distance(com2d, s_region.outer()[0]);
-//                  int ans_pid = 0;
-//                  for(int i=1;i<s_region.outer().size();i++){
-//                    if(bg::distance(com2d, s_region.outer()[i]) < cur_min_dis){
-//                      cur_min_dis = bg::distance(com2d, s_region.outer()[i]);
-//                      ans_pid = i;
-//                    }
-//                  }
-//                  ans_point(0) = s_region.outer()[ans_pid].x();
-//                  ans_point(1) = s_region.outer()[ans_pid].y();
-//                  cout<<"point will on the vertex:"<<ans_pid<<" point:"<<ans_point(0)<<","<<ans_point(1)<<endl;
-//                  pt_state = PT_FIX_TO_VERTEX;
-//                }
-//              }
-//        //      double FNUM_Z = 0.005;
-//              double FNUM_Z = 0.01;
-//              lpf_zmp = (1-FNUM_Z) * lpf_zmp + FNUM_Z * rp_ref_out.zmp;
-//              hrp::Vector3 hpf_zmp = rp_ref_out.zmp - lpf_zmp;
-//        //      rp_ref_out.zmp(0) = ans_point(0) + hpf_zmp(0);
-//        //      rp_ref_out.zmp(1) = ans_point(1) + hpf_zmp(1);
-//
-//              if(loop%50==0){
-//                if(pt_state != PT_IN_REGION){
-//                  std::cout<<"COM out of Support Region pt_state="<<pt_state<<std::endl;
-//                }
-//                for(int i=0;i<s_region.outer().size();i++){
-//                  fprintf(sr_log,"%f %f %f\n",s_region.outer().at(i).x(), s_region.outer().at(i).y(),(double)loop/500.0);
-//                }
-//                fprintf(sr_log,"\n");
-//                fprintf(cz_log,"%f %f %f %f %f %f %f %f %f",(double)loop/500.0,comin_abs(0),comin_abs(1),comin_abs(2),rp_ref_out.zmp(0),rp_ref_out.zmp(1),rp_ref_out.zmp(2),ans_point(0),ans_point(1));
-//                fprintf(cz_log," %f %f %f %f",rp_ref_out.zmp(1),hpf_zmp(1),ans_point(1),ans_point(1) + hpf_zmp(1));
-//                fprintf(cz_log,"\n\n");
-//              }
-//              comin_abs(0) = ans_point(0);
-//              comin_abs(1) = ans_point(1);
-//        //      rp_ref_out.zmp(0) = comin_abs(0);
-//        //      rp_ref_out.zmp(1) = comin_abs(1);
-//              return true;
-//            }
-
 };
 
+
 #endif // WBMS_CORE_H

From addc013dc573615c3eba5927d8b44bb2d5ee3f57 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 15 May 2019 23:58:46 +0900
Subject: [PATCH 162/217] tune for JAXON

---
 .../FullbodyInverseKinematicsSolver.h         |   2 +-
 .../WholeBodyMasterSlave.cpp                  | 458 +++++++-----------
 rtc/WholeBodyMasterSlave/wbms_core.h          |   5 +-
 3 files changed, 173 insertions(+), 292 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 815ce40179f..3ffc1882fb8 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -163,7 +163,7 @@ class FullbodyInverseKinematicsSolver{
                 if(c_part_selection_mat.rows() != 0 && c_part_selection_mat.cols() != 0 ){
 
 
-                    const double dp_max = 9999990.1;
+                    const double dp_max = 0.1;
 
                     J_all.middleRows                (CURRENT_C_COUNT, c_part_selection_mat.rows()) = c_part_selection_mat * J_part * q_select_mat.transpose();
                     err_all.segment                 (CURRENT_C_COUNT, c_part_selection_mat.rows()) = c_part_selection_mat * dp_part.cwiseMin(hrp::dvector6::Constant(dp_max)).cwiseMax(hrp::dvector6::Constant(-dp_max));
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index cdb9c8d3067..54b49941a5f 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -160,12 +160,22 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     }else{
         wbms->wp.use_joints = getJointNameAll(fik->m_robot);
     }
-    wbms->wp.use_targets.push_back("rleg");
-    wbms->wp.use_targets.push_back("lleg");
-    wbms->wp.use_targets.push_back("rarm");
-    wbms->wp.use_targets.push_back("larm");
-    wbms->wp.use_targets.push_back("com");
-    wbms->wp.use_targets.push_back("head");
+
+    if(fik->m_robot->name().find("JAXON") != std::string::npos){ // for demo
+//        wbms->wp.use_targets.push_back("rleg");
+//        wbms->wp.use_targets.push_back("lleg");
+        wbms->wp.use_targets.push_back("rarm");
+        wbms->wp.use_targets.push_back("larm");
+//        wbms->wp.use_targets.push_back("com");
+        wbms->wp.use_targets.push_back("head");
+    }else{
+        wbms->wp.use_targets.push_back("rleg");
+        wbms->wp.use_targets.push_back("lleg");
+        wbms->wp.use_targets.push_back("rarm");
+        wbms->wp.use_targets.push_back("larm");
+        wbms->wp.use_targets.push_back("com");
+        wbms->wp.use_targets.push_back("head");
+    }
     sccp = boost::shared_ptr<CapsuleCollisionChecker>(new CapsuleCollisionChecker(fik->m_robot));
     RTCOUT << "setup main function class finished" << std::endl;
 
@@ -179,7 +189,11 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     t_ip->set(&output_ratio);
     q_ip = new interpolator(fik->numStates(), m_dt, interpolator::CUBICSPLINE, 1); // or HOFFARBIB, QUINTICSPLINE
     q_ip->clear();
-    avg_q_vel = hrp::dvector::Constant(fik->numStates(), 2.0); // all joint max avarage vel = 1.0 rad/s
+    if(fik->m_robot->name().find("JAXON") != std::string::npos){
+        avg_q_vel = hrp::dvector::Constant(fik->numStates(), 2.0); // all joint max avarage vel = 1.0 rad/s
+    }else{
+        avg_q_vel = hrp::dvector::Constant(fik->numStates(), 1.0); // all joint max avarage vel = 1.0 rad/s
+    }
     avg_q_acc = hrp::dvector::Constant(fik->numStates(), 16.0); // all joint max avarage acc = 16.0 rad/s^2
     avg_q_vel.tail(6).fill(std::numeric_limits<double>::max()); // no limit for base link vel
     avg_q_acc.tail(6).fill(std::numeric_limits<double>::max()); // no limit for base link acc
@@ -236,10 +250,35 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if (m_baseRpyIn.isNew()) { m_baseRpyIn.read(); }
     if (m_zmpIn.isNew()) { m_zmpIn.read(); }
     if (m_optionalDataIn.isNew()) { m_optionalDataIn.read(); }
+    if (m_htrfwIn.isNew())  { m_htrfwIn.read(); wbms->hp_wld_raw.tgt[rf].w      = hrp::to_dvector6(m_htrfw.data); }
+    if (m_htlfwIn.isNew())  { m_htlfwIn.read(); wbms->hp_wld_raw.tgt[lf].w      = hrp::to_dvector6(m_htlfw.data); }
+
+    // button func
+    if(m_htlfw.data.force.y >= 1 && m_htrfw.data.force.y >= 1 && mode.now() == MODE_IDLE){
+        std::cerr<<"startWholeBodyMasterSlave() called by button"<<std::endl;
+        startWholeBodyMasterSlave();
+    }else if(m_htlfw.data.force.y >= 1 && m_htrfw.data.force.y >= 1 && mode.now() == MODE_WBMS){
+        std::cerr<<"stopWholeBodyMasterSlave() called by button"<<std::endl;
+        stopWholeBodyMasterSlave();
+    }
+    static bool blocking_continuous_hits = false;
+    if(m_htlfw.data.force.z >= 1 && m_htrfw.data.force.z >= 1 && mode.now() == MODE_PAUSE){
+        if(!blocking_continuous_hits){
+            std::cerr<<"resumeWholeBodyMasterSlave() called by button"<<std::endl;
+            resumeWholeBodyMasterSlave();
+            blocking_continuous_hits = true;
+        }
+    }else if(m_htlfw.data.force.z >= 1 && m_htrfw.data.force.z >= 1 && mode.now() == MODE_WBMS){
+        if(!blocking_continuous_hits){
+            std::cerr<<"pauseWholeBodyMasterSlave() called by button"<<std::endl;
+            pauseWholeBodyMasterSlave();
+            blocking_continuous_hits = true;
+        }
+    }else{
+        blocking_continuous_hits = false;
+    }
 
     if( mode.now() != MODE_PAUSE ){ // stop updating input when MODE_PAUSE
-        if (m_htrfwIn.isNew())  { m_htrfwIn.read(); wbms->hp_wld_raw.tgt[rf].w      = hrp::to_dvector6(m_htrfw.data); }
-        if (m_htlfwIn.isNew())  { m_htlfwIn.read(); wbms->hp_wld_raw.tgt[lf].w      = hrp::to_dvector6(m_htlfw.data); }
         if (m_htcomIn.isNew())  { m_htcomIn.read(); wbms->hp_wld_raw.tgt[com].abs   = hrp::to_Pose3(m_htcom.data); }
         if (m_htrfIn.isNew())   { m_htrfIn.read();  wbms->hp_wld_raw.tgt[rf].abs    = hrp::to_Pose3(m_htrf.data); }
         if (m_htlfIn.isNew())   { m_htlfIn.read();  wbms->hp_wld_raw.tgt[lf].abs    = hrp::to_Pose3(m_htlf.data); }
@@ -252,15 +291,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     }
     addTimeReport("InPort");
 
-    //khi
-    if(m_htlfw.data.force.y == 1 && m_htrfw.data.force.y == 1 && mode.now() == MODE_IDLE){
-        std::cerr<<"button call"<<std::endl;
-        startWholeBodyMasterSlave();
-        std::cerr<<"button call end"<<std::endl;
-    }else if(m_htlfw.data.force.y == 1 && m_htrfw.data.force.y == 1 && mode.now() == MODE_WBMS){
-        stopWholeBodyMasterSlave();
-    }
-    
     if ( is_legged_robot ) {
         processTransition();
         mode.update();
@@ -291,19 +321,14 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
             wbms->act_rs.com = com;
             wbms->act_rs.zmp = ref_zmp;
 
-
-
-            if(wbms->rp_ref_out.tgt[rf].is_contact && !wbms->rp_ref_out.tgt[lf].is_contact){
-                ref_zmp.head(XY) = m_robot_vsafe->link(ee_ikc_map["rleg"].target_link_name)->p.head(XY);
-//                ref_zmp(X) + 0.05;
-            }
-            if(wbms->rp_ref_out.tgt[lf].is_contact && !wbms->rp_ref_out.tgt[rf].is_contact){
-                ref_zmp.head(XY) = m_robot_vsafe->link(ee_ikc_map["lleg"].target_link_name)->p.head(XY);
-//                ref_zmp(X) + 0.05;
-            }
-
-
-
+//            if(wbms->rp_ref_out.tgt[rf].is_contact && !wbms->rp_ref_out.tgt[lf].is_contact){
+//                ref_zmp.head(XY) = m_robot_vsafe->link(ee_ikc_map["rleg"].target_link_name)->p.head(XY);
+////                ref_zmp(X) + 0.05;
+//            }
+//            if(wbms->rp_ref_out.tgt[lf].is_contact && !wbms->rp_ref_out.tgt[rf].is_contact){
+//                ref_zmp.head(XY) = m_robot_vsafe->link(ee_ikc_map["lleg"].target_link_name)->p.head(XY);
+////                ref_zmp(X) + 0.05;
+//            }
 
             // qRef
             for (int i = 0; i < m_qRef.data.length(); i++ ){
@@ -311,8 +336,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
                   m_qRef.data[i] = output_ratio * m_robot_vsafe->joint(i)->q  + (1 - output_ratio) * m_qRef.data[i];
               }
             }
-//            m_qRef.data = hrp::to_DoubleSeq(output_ratio * hrp::getQAll(m_robot_vsafe)  + (1 - output_ratio) * hrp::to_dvector(m_qRef.data));
-
             // basePos
             m_basePos.data = hrp::to_Point3D( output_ratio * m_robot_vsafe->rootLink()->p + (1 - output_ratio) * hrp::to_Vector3(m_basePos.data));
             m_basePos.tm = m_qRef.tm;
@@ -468,7 +491,7 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(){
 
 void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp::Pose3& rf_ref, const hrp::Pose3& lf_ref, const hrp::Pose3& rh_ref, const hrp::Pose3& lh_ref, const hrp::Pose3& head_ref){
     std::vector<IKConstraint> ikc_list;
-    if(wbms->wp.use_lower){
+    if(has(wbms->wp.use_targets, "com")){
         IKConstraint tmp;
         tmp.target_link_name = "WAIST";
         tmp.localPos = hrp::Vector3::Zero();
@@ -478,8 +501,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.constraint_weight << 0,0,0,1,1,1;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
-    }
-    if(!wbms->wp.use_lower){
+    }else{
       IKConstraint tmp;
       tmp.target_link_name = "WAIST";
       tmp.localPos = hrp::Vector3::Zero();
@@ -490,105 +512,98 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
       tmp.rot_precision = deg2rad(3);
       ikc_list.push_back(tmp);
     }
-    hrp::Vector3 act_cp = fik->m_robot->rootLink()->p + fik->m_robot->rootLink()->R * rel_act_cp;
-    act_cp(Z) = 0.3;
-//    act_cp = 0.9 * act_cp + 0.1 * (fik->m_robot->rootLink()->p + fik->m_robot->rootLink()->R * rel_act_cp);
-    static hrp::Vector3 track_cp = act_cp;
-
-    if(!std::isnan(act_cp(X)) && !std::isnan(act_cp(Y)) && !std::isnan(act_cp(Z))){
-        hrp::Vector3 direc = act_cp - track_cp;
-        const double speed = 0.3;
-        LIMIT_MINMAX(direc(X), -speed*m_dt, speed*m_dt);
-        LIMIT_MINMAX(direc(Y), -speed*m_dt, speed*m_dt);
-        LIMIT_MINMAX(direc(Z), -0.01*m_dt, 0.01*m_dt);
-//        if(direc.norm()>speed*m_dt){
-//            direc = direc.normalized() * speed*m_dt;
-//        }
-        track_cp += direc;
-    }
-
-//    hrp::Vector3 act_zmp = fik->m_robot->rootLink()->p + fik->m_robot->rootLink()->R * rel_act_zmp;
-//    static int cp_count = 0;
-    const double L = 100;
-    const double sink = 0.0;
-    const double fh = 0.05;
-    const bool start_cp = false;
-//    const bool start_cp = ((act_cp - fik->m_robot->calcCM()).head(XY).norm()>0.025);
-//    dbgv(act_cp);
-//    dbgv(track_cp);
-//    dbgv(fik->m_robot->calcCM());
-
-
-
-    if (act_cp.head(XY).norm()>0.02)
-    {
-
-
-
-
-    try {
-        OpenHRP::AutoBalancerService::FootstepsSequence fss;
-        OpenHRP::AutoBalancerService::StepParamsSequence spss;
-        fss.length(2);
-        spss.length(2);
-        fss[0].fs.length(1);
-        fss[1].fs.length(1);
-        spss[0].sps.length(1);
-        spss[1].sps.length(1);
-        fss[0].fs[0].leg = "rleg";
-        fss[0].fs[0].pos[0] = 0;
-        fss[0].fs[0].pos[1] = -0.1;
-        fss[0].fs[0].pos[2] = 0;
-        fss[0].fs[0].rot[0] = 1;
-        fss[0].fs[0].rot[1] = 0;
-        fss[0].fs[0].rot[2] = 0;
-        fss[0].fs[0].rot[3] = 0;
-        fss[1].fs[0].leg = "lleg";
-        fss[1].fs[0].pos[0] = act_cp(Y)>0 ? 0.3*act_cp.head(XY).normalized()(X) : 0;
-        fss[1].fs[0].pos[1] = act_cp(Y)>0 ? 0.3*act_cp.head(XY).normalized()(Y) : 0.1;
-        fss[1].fs[0].pos[2] = 0;
-        fss[1].fs[0].rot[0] = 1;
-        fss[1].fs[0].rot[1] = 0;
-        fss[1].fs[0].rot[2] = 0;
-        fss[1].fs[0].rot[3] = 0;
-        spss[0].sps[0].step_height = 0.1;
-        spss[1].sps[0].step_height = 0.1;
-        spss[0].sps[0].step_time = 0.01;
-        spss[1].sps[0].step_time = 0.5;
-        spss[0].sps[0].toe_angle = 0;
-        spss[1].sps[0].toe_angle = 0;
-        spss[0].sps[0].heel_angle = 0;
-        spss[1].sps[0].heel_angle = 0;
-//        m_AutoBalancerServiceConsumer->setFootSteps(fss,1);
-        m_AutoBalancerServiceConsumer->setFootStepsWithParam(fss,spss,1);
-    } catch(CORBA::COMM_FAILURE& ex) {
-//        dbg("1");
-    } catch(CORBA::MARSHAL& ex) {
-//        dbg("2");
-    } catch(CORBA::BAD_PARAM& ex) {
-//        dbg("3");
-    } catch(CORBA::OBJECT_NOT_EXIST& ex) {
-//        dbg("4");
-    } catch(CORBA::SystemException& ex) {
-//        dbg("5");
-    } catch(CORBA::Exception&) {
-//        dbg("6");
-    } catch(omniORB::fatalException& fe) {
-//        dbg("7");
-    }
-    catch(...) {
-//        dbg("8");
-    }
-//    dbg("ok");
-    // サービスポートによる処理
-}
-
-
-
-
-
-
-
+//    hrp::Vector3 act_cp = fik->m_robot->rootLink()->p + fik->m_robot->rootLink()->R * rel_act_cp;
+//    act_cp(Z) = 0.3;
+////    act_cp = 0.9 * act_cp + 0.1 * (fik->m_robot->rootLink()->p + fik->m_robot->rootLink()->R * rel_act_cp);
+//    static hrp::Vector3 track_cp = act_cp;
+//
+//    if(!std::isnan(act_cp(X)) && !std::isnan(act_cp(Y)) && !std::isnan(act_cp(Z))){
+//        hrp::Vector3 direc = act_cp - track_cp;
+//        const double speed = 0.3;
+//        LIMIT_MINMAX(direc(X), -speed*m_dt, speed*m_dt);
+//        LIMIT_MINMAX(direc(Y), -speed*m_dt, speed*m_dt);
+//        LIMIT_MINMAX(direc(Z), -0.01*m_dt, 0.01*m_dt);
+////        if(direc.norm()>speed*m_dt){
+////            direc = direc.normalized() * speed*m_dt;
+////        }
+//        track_cp += direc;
+//    }
+//
+////    hrp::Vector3 act_zmp = fik->m_robot->rootLink()->p + fik->m_robot->rootLink()->R * rel_act_zmp;
+////    static int cp_count = 0;
+//    const double L = 100;
+//    const double sink = 0.0;
+//    const double fh = 0.05;
+//    const bool start_cp = false;
+////    const bool start_cp = ((act_cp - fik->m_robot->calcCM()).head(XY).norm()>0.025);
+////    dbgv(act_cp);
+////    dbgv(track_cp);
+////    dbgv(fik->m_robot->calcCM());
+//
+//
+//
+//    if (act_cp.head(XY).norm()>0.02)
+//    {
+//
+//
+//
+//
+//    try {
+//        OpenHRP::AutoBalancerService::FootstepsSequence fss;
+//        OpenHRP::AutoBalancerService::StepParamsSequence spss;
+//        fss.length(2);
+//        spss.length(2);
+//        fss[0].fs.length(1);
+//        fss[1].fs.length(1);
+//        spss[0].sps.length(1);
+//        spss[1].sps.length(1);
+//        fss[0].fs[0].leg = "rleg";
+//        fss[0].fs[0].pos[0] = 0;
+//        fss[0].fs[0].pos[1] = -0.1;
+//        fss[0].fs[0].pos[2] = 0;
+//        fss[0].fs[0].rot[0] = 1;
+//        fss[0].fs[0].rot[1] = 0;
+//        fss[0].fs[0].rot[2] = 0;
+//        fss[0].fs[0].rot[3] = 0;
+//        fss[1].fs[0].leg = "lleg";
+//        fss[1].fs[0].pos[0] = act_cp(Y)>0 ? 0.3*act_cp.head(XY).normalized()(X) : 0;
+//        fss[1].fs[0].pos[1] = act_cp(Y)>0 ? 0.3*act_cp.head(XY).normalized()(Y) : 0.1;
+//        fss[1].fs[0].pos[2] = 0;
+//        fss[1].fs[0].rot[0] = 1;
+//        fss[1].fs[0].rot[1] = 0;
+//        fss[1].fs[0].rot[2] = 0;
+//        fss[1].fs[0].rot[3] = 0;
+//        spss[0].sps[0].step_height = 0.1;
+//        spss[1].sps[0].step_height = 0.1;
+//        spss[0].sps[0].step_time = 0.01;
+//        spss[1].sps[0].step_time = 0.5;
+//        spss[0].sps[0].toe_angle = 0;
+//        spss[1].sps[0].toe_angle = 0;
+//        spss[0].sps[0].heel_angle = 0;
+//        spss[1].sps[0].heel_angle = 0;
+////        m_AutoBalancerServiceConsumer->setFootSteps(fss,1);
+//        m_AutoBalancerServiceConsumer->setFootStepsWithParam(fss,spss,1);
+//    } catch(CORBA::COMM_FAILURE& ex) {
+////        dbg("1");
+//    } catch(CORBA::MARSHAL& ex) {
+////        dbg("2");
+//    } catch(CORBA::BAD_PARAM& ex) {
+////        dbg("3");
+//    } catch(CORBA::OBJECT_NOT_EXIST& ex) {
+////        dbg("4");
+//    } catch(CORBA::SystemException& ex) {
+////        dbg("5");
+//    } catch(CORBA::Exception&) {
+////        dbg("6");
+//    } catch(omniORB::fatalException& fe) {
+////        dbg("7");
+//    }
+//    catch(...) {
+////        dbg("8");
+//    }
+////    dbg("ok");
+//    // サービスポートによる処理
+//}
 
     if(has(wbms->wp.use_targets, "rleg")){
         IKConstraint tmp;
@@ -596,21 +611,6 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.localPos = ee_ikc_map["rleg"].localPos;
         tmp.localR = ee_ikc_map["rleg"].localR;
         tmp.targetPos = rf_ref.p;
-
-
-//        if(start_cp){
-//            double h = fh * (-sin(2* M_PI * loop / L) - sink);
-//            wbms->rp_ref_out.tgt[rf].is_contact = (h<0);
-//            LIMIT_MIN(h, 0);
-//            tmp.targetPos(Z) += h;
-//            static hrp::Vector2 xy = tmp.targetPos.head(XY);
-//            if(!wbms->rp_ref_out.tgt[rf].is_contact){
-//                xy = track_cp.head(XY);
-//            }
-//            tmp.targetPos.head(XY) += xy;
-//        }
-
-
         tmp.targetRpy = rf_ref.rpy();
         if(wbms->rp_ref_out.tgt[rf].is_contact){
             tmp.constraint_weight = hrp::dvector6::Constant(3);
@@ -625,22 +625,6 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.localPos = ee_ikc_map["lleg"].localPos;
         tmp.localR = ee_ikc_map["lleg"].localR;
         tmp.targetPos = lf_ref.p;
-
-
-//        if(start_cp){
-//            double h = fh * (sin(2*M_PI * loop / L) - sink);
-//            wbms->rp_ref_out.tgt[lf].is_contact = (h<0);
-//            LIMIT_MIN(h, 0);
-//            tmp.targetPos(Z) += h;
-//            static hrp::Vector2 xy = tmp.targetPos.head(XY);
-//            if(!wbms->rp_ref_out.tgt[lf].is_contact){
-//                xy = track_cp.head(XY);
-//            }
-//            tmp.targetPos.head(XY) += xy;
-//        }
-
-
-
         tmp.targetRpy = lf_ref.rpy();
         if(wbms->rp_ref_out.tgt[lf].is_contact){
             tmp.constraint_weight = hrp::dvector6::Constant(3);
@@ -657,27 +641,6 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.targetPos = rh_ref.p;
         tmp.targetRpy = rh_ref.rpy();
         tmp.constraint_weight = hrp::dvector6::Constant(0.1);
-
-
-
-
-        if(start_cp){
-             tmp.constraint_weight << 0,0,0,0,0,0;
-             if(act_cp(Y)<0 || true){
-                 hrp::Vector2 h_direc = act_cp.head(XY);
-                 h_direc = h_direc.normalized() * 0.5;
-                 tmp.targetPos << h_direc, 0;
-                 tmp.constraint_weight << 1,1,1,0,0,0;
-             }
-             tmp.targetPos << -0.5, -0.3, 0;
-
-
-
-
-         }
-
-
-
         tmp.pos_precision = 3e-3;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
@@ -690,24 +653,6 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.targetPos = lh_ref.p;
         tmp.targetRpy = lh_ref.rpy();
         tmp.constraint_weight = hrp::dvector6::Constant(0.1);
-
-
-
-
-        if(start_cp){
-            tmp.constraint_weight << 0,0,0,0,0,0;
-            if(act_cp(Y)>0 || true){
-                hrp::Vector2 h_direc = act_cp.head(XY);
-                h_direc = h_direc.normalized() * 0.5;
-                tmp.targetPos << h_direc, 0;
-                tmp.constraint_weight << 1,1,1,0,0,0;
-            }
-            tmp.targetPos << -0.5, 0.3, 0;
-        }
-
-
-
-
         tmp.pos_precision = 3e-3;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
@@ -790,15 +735,6 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.localPos = hrp::Vector3::Zero();
         tmp.localR = hrp::Matrix33::Identity();
         tmp.targetPos = com_ref.p;// COM height will not be constraint
-
-
-
-//        if(start_cp){
-//            tmp.targetPos.head(XY) += track_cp.head(XY);
-//            tmp.targetPos(Z) = track_cp(Z);
-//        }
-
-
         tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
 //        tmp.constraint_weight << 3,3,0.01,0.1,0.1,0;
         tmp.constraint_weight << 3,3,0.01,0,0,0;
@@ -811,19 +747,21 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         ikc_list.push_back(tmp);
     }
 
-    if( fik->m_robot->link("CHEST_JOINT0") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT0")->jointId) = 0.1;//JAXON
-    if( fik->m_robot->link("CHEST_JOINT1") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT1")->jointId) = 0.1;
-    if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 0.1;
-//    if( fik->m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_Y")->jointId) = 0.1;//K
-//    if( fik->m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_P")->jointId) = 0.1;
-//    fik->q_ref_constraint_weight(fik->m_robot->link("CHEST_Y")->jointId) = 1;
-//    fik->q_ref_constraint_weight(fik->m_robot->link("CHEST_P")->jointId) = 1;
-
-//    if( robot_in->link("RARM_JOINT2") != NULL) robot_in->link("RARM_JOINT2")->ulimit = deg2rad(-30);//脇内側の干渉回避
-//    if( robot_in->link("LARM_JOINT2") != NULL) robot_in->link("LARM_JOINT2")->llimit = deg2rad(30);
+    if( fik->m_robot->link("CHEST_JOINT0") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT0")->jointId) = 0.01;//JAXON
+    if( fik->m_robot->link("CHEST_JOINT1") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT1")->jointId) = 0.01;
+    if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 0.01;
+    if( fik->m_robot->link("HEAD_JOINT0") != NULL) fik->m_robot->link("HEAD_JOINT0")->llimit = deg2rad(-30);
+    if( fik->m_robot->link("HEAD_JOINT0") != NULL) fik->m_robot->link("HEAD_JOINT0")->ulimit = deg2rad(30);
+    if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->llimit = deg2rad(-45);
+    if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->ulimit = deg2rad(10);
+    if( fik->m_robot->link("RARM_JOINT2") != NULL) fik->m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-45);//脇内側の干渉回避
+    if( fik->m_robot->link("LARM_JOINT2") != NULL) fik->m_robot->link("LARM_JOINT2")->llimit = deg2rad(45);
     if( fik->m_robot->link("RLEG_JOINT3") != NULL) fik->m_robot->link("RLEG_JOINT3")->llimit = deg2rad(10);//膝伸びきり防止のため
     if( fik->m_robot->link("LLEG_JOINT3") != NULL) fik->m_robot->link("LLEG_JOINT3")->llimit = deg2rad(10);
-    if( fik->m_robot->link("R_KNEE_P") != NULL) fik->m_robot->link("R_KNEE_P")->llimit = deg2rad(15);//K
+
+    if( fik->m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_Y")->jointId) = 0.1;//K
+    if( fik->m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_P")->jointId) = 0.1;
+    if( fik->m_robot->link("R_KNEE_P") != NULL) fik->m_robot->link("R_KNEE_P")->llimit = deg2rad(15);
     if( fik->m_robot->link("L_KNEE_P") != NULL) fik->m_robot->link("L_KNEE_P")->llimit = deg2rad(15);
     if( fik->m_robot->link("R_WRIST_R") != NULL) fik->m_robot->link("R_WRIST_R")->llimit = deg2rad(-40);
     if( fik->m_robot->link("L_WRIST_R") != NULL) fik->m_robot->link("L_WRIST_R")->llimit = deg2rad(-40);
@@ -833,13 +771,10 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
     if( fik->m_robot->link("L_WRIST_P") != NULL) fik->m_robot->link("L_WRIST_P")->llimit = deg2rad(-40);
     if( fik->m_robot->link("R_WRIST_P") != NULL) fik->m_robot->link("R_WRIST_P")->ulimit = deg2rad(20);
     if( fik->m_robot->link("L_WRIST_P") != NULL) fik->m_robot->link("L_WRIST_P")->ulimit = deg2rad(20);
-
-
     if( fik->m_robot->link("CHEST_Y") != NULL) fik->m_robot->link("CHEST_Y")->llimit = deg2rad(-20);
     if( fik->m_robot->link("CHEST_Y") != NULL) fik->m_robot->link("CHEST_Y")->ulimit = deg2rad(20);
     if( fik->m_robot->link("CHEST_P") != NULL) fik->m_robot->link("CHEST_P")->llimit = deg2rad(0);
     if( fik->m_robot->link("CHEST_P") != NULL) fik->m_robot->link("CHEST_P")->ulimit = deg2rad(60);
-                                              
     if( fik->m_robot->link("HEAD_Y") != NULL) fik->m_robot->link("HEAD_Y")->llimit = deg2rad(-5);
     if( fik->m_robot->link("HEAD_Y") != NULL) fik->m_robot->link("HEAD_Y")->ulimit = deg2rad(5);
     if( fik->m_robot->link("HEAD_P") != NULL) fik->m_robot->link("HEAD_P")->llimit = deg2rad(0);
@@ -848,10 +783,8 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         LIMIT_MINMAX(fik->m_robot->joint(i)->q, fik->m_robot->joint(i)->llimit, fik->m_robot->joint(i)->ulimit);
     }
 
-
     fik->q_ref.head(m_qRef.data.length()) = hrp::to_dvector(m_qRef.data);//あえてseqからのbaselink poseは信用しない
 
-
     for(int i=0; i<fik->m_robot->numJoints(); i++){
         if(!has(wbms->wp.use_joints, fik->m_robot->joint(i)->name)){
             fik->dq_weight_all(i) = 0;
@@ -860,27 +793,15 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
     }
 
 
-    // std::string tmp[] = {"R_CROTCH_R","R_CROTCH_P","R_CROTCH_Y","R_KNEE_P","R_ANKLE_R","R_ANKLE_P","L_CROTCH_R","L_CROTCH_P","L_CROTCH_Y","L_KNEE_P","L_ANKLE_R","L_ANKLE_P"};
-    // const std::vector<std::string> list(tmp, tmp+12);
-    // for(int i=0; i<list.size(); i++){
-    //   if( robot_in->link(list[i]) != NULL){
-    //     int j=robot_in->link(list[i])->jointId;
-    //     fik_in->dq_weight_all(j) = 1e-5;setNextMode
-    //     robot_in->joint(j)->q = m_qRef.data[j];
-    //     fik_in->q_ref(j) = m_qRef.data[j];
-    //   }
-    // }
-    // robot_in->rootLink()->p << m_basePos.data.x,m_basePos.data.y,m_basePos.data.z;// will be ignored by selection_vec
-    // robot_in->rootLink()->R = hrp::rotFromRpy(hrp::Vector3(m_baseRpy.data.r,m_baseRpy.data.p,m_baseRpy.data.y));// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
-
-
-//    fik_in->q_ref_pullback_gain.segment(6+6+3+2, 8*2).fill(0.01);//腕だけ
-//    fik_in->dq_ref_pullback.segment(6+6+3+2, 8*2).fill(deg2rad(WBMSparam0.1));//腕だけ
-//    fik_in->q_ref_constraint_weight.fill(1);
+    if(fik->m_robot->name().find("JAXON") != std::string::npos){
+        for(int i=0; i<fik->m_robot->numJoints(); i++){
+            if(fik->m_robot->joint(i)->name.find("ARM") != std::string::npos){
+                fik->q_ref_constraint_weight(i) = 1e-3;//腕だけ
+            }
+        }
+    }
 
-    struct timespec startT, endT;
     const int IK_MAX_LOOP = 2;
-    clock_gettime(CLOCK_REALTIME, &startT);
     int loop_result = fik->solveFullbodyIKLoop(ikc_list, IK_MAX_LOOP);
 }
 
@@ -889,24 +810,6 @@ void WholeBodyMasterSlave::smoothingJointAngles(hrp::BodyPtr _robot, hrp::BodyPt
     const double min_goal_time_offset = 0.1;
 
     static hrp::dvector ans_state_vel = hrp::dvector::Zero(fik->numStates());
-//    if (!q_ip->isEmpty() ){  q_ip->get(tmp_x, ans_state_vel, false);}
-
-//    for(int i=0;i<_robot->numJoints();i++){
-//        if(_robot->joint(i)->name != "L_HAND" && _robot->joint(i)->name != "R_HAND" ){ //KHI demo
-//          double tmp_time = fabs(_robot->joint(i)->q - _robot_safe->joint(i)->q) / avg_q_vel(i);
-//          LIMIT_MIN(tmp_time, fabs(ans_state_vel(i))/16);
-//  //        tmp_time += fabs(ans_state_vel(i))/16; //加速度8
-//          if(tmp_time > goal_time){ goal_time = tmp_time; }
-//        }
-//    }
-//
-//    q_ip->setGoal(hrp::getRobotStateVec(_robot).data(), goal_time + min_goal_time_offset, true);
-//    hrp::dvector ans_state(fik->numStates());
-//    double tmp[fik->numStates()], tmpv[fik->numStates()];
-//    if (!q_ip->isEmpty() ){  q_ip->get(tmp, tmpv, true);}
-//    ans_state_vel = Eigen::Map<hrp::dvector>(tmpv, fik->numStates());
-//    ans_state = Eigen::Map<hrp::dvector>(tmp, fik->numStates());
-
 
     hrp::dvector estimated_times_from_vel_limit = (hrp::getRobotStateVec(_robot) - hrp::getRobotStateVec(_robot_safe)).array().abs() / avg_q_vel.array();
     hrp::dvector estimated_times_from_acc_limit = ans_state_vel.array().abs() / avg_q_acc.array();
@@ -920,22 +823,6 @@ void WholeBodyMasterSlave::smoothingJointAngles(hrp::BodyPtr _robot, hrp::BodyPt
     hrp::dvector ans_state = Eigen::Map<hrp::dvector>(tmp, fik->numStates());
     ans_state_vel = Eigen::Map<hrp::dvector>(tmpv, fik->numStates());
 
-
-//    //// KHI demo
-//    const double hand_max_vel = 180.0/0.4 /180.0*M_PI;// 0.4sec for 180deg
-//    std::map<std::string, double> joy_inputs;// 0 ~ 1
-//    joy_inputs["L_HAND"] = m_htlfw.data.force.x;// tmp substitute
-//    joy_inputs["R_HAND"] = m_htrfw.data.force.x;// tmp substitute
-//    for(std::map<std::string, double>::iterator it = joy_inputs.begin(); it!=joy_inputs.end(); it++){
-//      if(_robot_safe->link(it->first) != NULL){
-//          LIMIT_MINMAX(it->second, 0.0, 1.0);
-//          double tgt_hand_q = (-29.0 + (124.0-(-29.0))*it->second ) /180.0*M_PI;
-//          const double q_old = _robot_safe->link(it->first)->q;
-//          LIMIT_MINMAX(tgt_hand_q, q_old - hand_max_vel*m_dt, q_old + hand_max_vel*m_dt);
-//          _robot_safe->link(it->first)->q = tgt_hand_q;
-//      }
-//    }
-
     hrp::setRobotStateVec(_robot_safe, ans_state);
 
     _robot_safe->calcForwardKinematics();
@@ -946,7 +833,6 @@ bool WholeBodyMasterSlave::startWholeBodyMasterSlave(){
     if(mode.now() == MODE_IDLE){
         RTCOUT << "startWholeBodyMasterSlave" << std::endl;
         mode.setNextMode(MODE_SYNC_TO_WBMS);
-        while(!t_ip->isEmpty()){ usleep(1000); }
         return true;
     }else{
         RTCOUT << "Invalid context to startWholeBodyMasterSlave" << std::endl;
@@ -971,9 +857,6 @@ bool WholeBodyMasterSlave::resumeWholeBodyMasterSlave(){
     if(mode.now() == MODE_PAUSE){
         RTCOUT << "resumeWholeBodyMasterSlave" << std::endl;
         mode.setNextMode(MODE_WBMS);
-        avg_q_vel /= 5;
-        sleep(5);
-        avg_q_vel *= 5;
         return true;
     }else{
         RTCOUT << "Invalid context to resumeWholeBodyMasterSlave" << std::endl;
@@ -986,7 +869,6 @@ bool WholeBodyMasterSlave::stopWholeBodyMasterSlave(){
     if(mode.now() == MODE_WBMS || mode.now() == MODE_PAUSE ){
         RTCOUT << "stopWholeBodyMasterSlave" << std::endl;
         mode.setNextMode(MODE_SYNC_TO_IDLE);
-        while(!t_ip->isEmpty()){ usleep(1000); }
         return true;
     }else{
         RTCOUT << "Invalid context to stopWholeBodyMasterSlave" << std::endl;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 9d1aa0db579..87f1d200276 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -564,13 +564,12 @@ class WBMSCore{
                 sp2sw_vec = base_rot * sp2sw_vec_baserel;
                 swing_leg.abs.p.head(XY) = sp2sw_vec + support_leg.cnt.p.head(XY);
             }
-            const double base2hand_min = 0.0;
+            const double base2hand_min = 0.4;// for jaxon demo
             for(int i=0, l[2]={rh,lh}; i<2; i++){
                 hrp::Vector2 horizontal_dist = out.tgt[l[i]].abs.p.head(XY) - baselinkpose.p.head(XY);
                 if(horizontal_dist.norm() < base2hand_min){
-                    horizontal_dist = base2hand_min * horizontal_dist.normalized();
+                    out.tgt[l[i]].abs.p.head(XY) = baselinkpose.p.head(XY) + base2hand_min * horizontal_dist.normalized();
                 }
-                out.tgt[l[i]].abs.p.head(XY) = baselinkpose.p.head(XY) + horizontal_dist;
             }
         }
         void setFootContactPoseByGoContact(HumanPose& out){

From 9a838ce23b6ade1ab363d4d8a7a341b640a45274 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 21 Aug 2019 19:37:20 +0900
Subject: [PATCH 163/217] revert undesired commit

---
 rtc/ImpedanceController/JointPathEx.cpp | 15 ---------------
 rtc/ImpedanceController/JointPathEx.h   |  1 -
 rtc/Stabilizer/Stabilizer.cpp           |  2 +-
 3 files changed, 1 insertion(+), 17 deletions(-)

diff --git a/rtc/ImpedanceController/JointPathEx.cpp b/rtc/ImpedanceController/JointPathEx.cpp
index 2d7fba1ce7f..5b3dc4729e8 100644
--- a/rtc/ImpedanceController/JointPathEx.cpp
+++ b/rtc/ImpedanceController/JointPathEx.cpp
@@ -104,21 +104,6 @@ JointPathEx::JointPathEx(BodyPtr& robot, Link* base, Link* end, double control_c
   }
 }
 
-JointPathEx::JointPathEx(BodyPtr& robot, Link* end, double control_cycle, bool _use_inside_joint_weight_retrieval, const std::string& _debug_print_prefix)
-    : JointPath( end), maxIKPosErrorSqr(1.0e-8), maxIKRotErrorSqr(1.0e-6), maxIKIteration(50), interlocking_joint_pair_indices(), sr_gain(1.0), manipulability_limit(0.1), manipulability_gain(0.001), dt(control_cycle),
-      debug_print_prefix(_debug_print_prefix+",JointPathEx"), joint_limit_debug_print_counts(numJoints(), 0),
-      debug_print_freq_count(static_cast<size_t>(0.25/dt)), // once per 0.25[s]
-      use_inside_joint_weight_retrieval(_use_inside_joint_weight_retrieval) {
-  for (unsigned int i = 0 ; i < numJoints(); i++ ) {
-    joints.push_back(joint(i));
-  }
-  avoid_weight_gain.resize(numJoints());
-  optional_weight_vector.resize(numJoints());
-  for (unsigned int i = 0 ; i < numJoints(); i++ ) {
-      optional_weight_vector[i] = 1.0;
-  }
-}
-
 void JointPathEx::setMaxIKError(double epos, double erot) {
   maxIKPosErrorSqr = epos*epos;
   maxIKRotErrorSqr = erot*erot;
diff --git a/rtc/ImpedanceController/JointPathEx.h b/rtc/ImpedanceController/JointPathEx.h
index 3c6654eb517..32ec31070b0 100644
--- a/rtc/ImpedanceController/JointPathEx.h
+++ b/rtc/ImpedanceController/JointPathEx.h
@@ -16,7 +16,6 @@ namespace hrp {
     class JointPathEx : public JointPath {
   public:
     JointPathEx(BodyPtr& robot, Link* base, Link* end, double control_cycle, bool _use_inside_joint_weight_retrieval = true, const std::string& _debug_print_prefix = "");
-    JointPathEx(BodyPtr& robot, Link* end, double control_cycle, bool _use_inside_joint_weight_retrieval = true, const std::string& _debug_print_prefix = "");
     bool calcJacobianInverseNullspace(dmatrix &J, dmatrix &Jinv, dmatrix &Jnull);
     bool calcInverseKinematics2Loop(const Vector3& dp, const Vector3& omega, const double LAMBDA, const double avoid_gain = 0.0, const double reference_gain = 0.0, const dvector* reference_q = NULL);
     bool calcInverseKinematics2Loop(const Vector3& end_effector_p, const Matrix33& end_effector_R,
diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp
index d7f76aaca89..f6fd157986b 100644
--- a/rtc/Stabilizer/Stabilizer.cpp
+++ b/rtc/Stabilizer/Stabilizer.cpp
@@ -1513,12 +1513,12 @@ void Stabilizer::calcTPCC() {
                                   + k_tpcc_x[i] * transition_smooth_gain * dcog(i);
         newcog(i) = uu * dt + cog(i);
       }
+
       moveBasePosRotForBodyRPYControl ();
 
       // target at ee => target at link-origin
       hrp::Vector3 target_link_p[stikp.size()];
       hrp::Matrix33 target_link_R[stikp.size()];
-        
       for (size_t i = 0; i < stikp.size(); i++) {
         rats::rotm3times(target_link_R[i], target_ee_R[i], stikp[i].localR.transpose());
         target_link_p[i] = target_ee_p[i] - target_ee_R[i] * stikp[i].localCOPPos;

From 22440014176cec503ca9f1e29fef301361555f3e Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 26 Aug 2019 18:02:17 +0900
Subject: [PATCH 164/217] add HapticController

---
 idl/CMakeLists.txt                            |   1 +
 idl/HapticControllerService.idl               |  31 +
 python/hrpsys_config.py                       |  40 +-
 .../FullbodyInverseKinematicsSolver.h         |   5 +-
 rtc/AutoBalancer/MyUtils.h                    |  37 +-
 rtc/CMakeLists.txt                            |   1 +
 rtc/HapticController/CMakeLists.txt           |  20 +
 rtc/HapticController/HapticController.cpp     | 599 ++++++++++++++++++
 rtc/HapticController/HapticController.h       | 153 +++++
 rtc/HapticController/HapticControllerComp.cpp |  50 ++
 .../HapticControllerService_impl.cpp          |  54 ++
 .../HapticControllerService_impl.h            |  31 +
 .../WholeBodyMasterSlave.cpp                  | 124 ++--
 .../WholeBodyMasterSlave.h                    |  80 ++-
 rtc/WholeBodyMasterSlave/wbms_core.h          |   2 +-
 15 files changed, 1129 insertions(+), 99 deletions(-)
 create mode 100644 idl/HapticControllerService.idl
 create mode 100644 rtc/HapticController/CMakeLists.txt
 create mode 100644 rtc/HapticController/HapticController.cpp
 create mode 100644 rtc/HapticController/HapticController.h
 create mode 100644 rtc/HapticController/HapticControllerComp.cpp
 create mode 100644 rtc/HapticController/HapticControllerService_impl.cpp
 create mode 100644 rtc/HapticController/HapticControllerService_impl.h

diff --git a/idl/CMakeLists.txt b/idl/CMakeLists.txt
index 0fe386888ff..7599436c7bc 100644
--- a/idl/CMakeLists.txt
+++ b/idl/CMakeLists.txt
@@ -30,6 +30,7 @@ set(idl_files
   AccelerationFilterService.idl
   ObjectContactTurnaroundDetectorService.idl
   WholeBodyMasterSlaveService.idl
+  HapticControllerService.idl
   PCDLoaderService.idl
   )
 
diff --git a/idl/HapticControllerService.idl b/idl/HapticControllerService.idl
new file mode 100644
index 00000000000..cdaec3709aa
--- /dev/null
+++ b/idl/HapticControllerService.idl
@@ -0,0 +1,31 @@
+/**
+ * @file HapticControllerService.idl
+ * @brief Services for the HapticController interface
+ */
+//#include "OpenHRPCommon.idl"
+module OpenHRP
+{
+  
+  interface HapticControllerService
+  {
+    typedef sequence<double, 3> DblSequence3;
+    //typedef sequence<double, 4> DblSequence4;
+    typedef double DblArray3[3];
+    typedef double DblArray4[4];
+    typedef sequence<string> StrSequence;
+
+    struct HapticControllerParam
+    {
+        double gravity_compensation_ratio;
+        double force_feedback_ratio;
+        double dqAct_filter_cutoff_hz;
+        double wrench_filter_cutoff_hz;
+    };
+    boolean startHapticController();
+    boolean stopHapticController();
+    boolean pauseHapticController();
+    boolean resumeHapticController();
+    void setParams(in HapticControllerParam i_param);
+    void getParams(out HapticControllerParam i_param);
+  };
+};
diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index 0427b0faaf7..1220e812e0a 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -272,6 +272,11 @@ class HrpsysConfigurator(object):
     wbms = None
     wbms_svc = None
     wbms_version = None
+    
+    # HapticController
+    hc = None
+    hc_svc = None
+    hc_version = None
 
     # rtm manager
     ms = None
@@ -396,22 +401,21 @@ def connectComps(self):
         if rtm.findPort(self.rh.ref, "lfsensor") and rtm.findPort(
                                      self.rh.ref, "rfsensor") and self.st:
             connectPorts(self.kf.port("rpy"), self.st.port("rpy"))
-            #### normal
-#            connectPorts(self.sh.port("zmpOut"), self.abc.port("zmpIn"))
-#            connectPorts(self.sh.port("basePosOut"), self.abc.port("basePosIn"))
-#            connectPorts(self.sh.port("baseRpyOut"), self.abc.port("baseRpyIn"))
-#            connectPorts(self.sh.port("optionalDataOut"), self.abc.port("optionalData"))
-            #### wbms
             if self.wbms:
-                connectPorts(self.wbms.port("q"), self.ic.port("qRef"))
-                connectPorts(self.wbms.port("zmpOut"), self.abc.port("zmpIn"))
-                connectPorts(self.wbms.port("basePosOut"), self.abc.port("basePosIn"))
-                connectPorts(self.wbms.port("baseRpyOut"), self.abc.port("baseRpyIn"))
+#                connectPorts(self.wbms.port("q"), self.ic.port("qRef")) // really needed ?
+                connectPorts(self.st.port("actCapturePoint"),   self.wbms.port("actCapturePoint"))
+                connectPorts(self.st.port("zmp"),               self.wbms.port("zmp"))
+                connectPorts(self.wbms.port("zmpOut"),          self.abc.port("zmpIn"))
+                connectPorts(self.wbms.port("basePosOut"),      self.abc.port("basePosIn"))
+                connectPorts(self.wbms.port("baseRpyOut"),      self.abc.port("baseRpyIn"))
                 connectPorts(self.wbms.port("optionalDataOut"), self.abc.port("optionalData"))
-                connectPorts(self.st.port("actCapturePoint"), self.wbms.port("actCapturePoint"))
-                connectPorts(self.st.port("zmp"), self.wbms.port("zmp"))
                 connectPorts(self.wbms.port("AutoBalancerService"), self.abc.port("AutoBalancerService"))
-                connectPorts(self.wbms.port("StabilizerService"), self.st.port("StabilizerService"))
+                connectPorts(self.wbms.port("StabilizerService"),   self.st.port("StabilizerService"))
+            else:
+                connectPorts(self.sh.port("zmpOut"),            self.abc.port("zmpIn"))
+                connectPorts(self.sh.port("basePosOut"),        self.abc.port("basePosIn"))
+                connectPorts(self.sh.port("baseRpyOut"),        self.abc.port("baseRpyIn"))
+                connectPorts(self.sh.port("optionalDataOut"),   self.abc.port("optionalData"))
             
             connectPorts(self.abc.port("zmpOut"), self.st.port("zmpRef"))
             connectPorts(self.abc.port("baseRpyOut"), self.st.port("baseRpyIn"))
@@ -436,6 +440,15 @@ def connectComps(self):
                 connectPorts(self.rfu.port("refFootOriginExtMomentIsHoldValue"), self.abc.port("refFootOriginExtMomentIsHoldValue"))
             if self.octd:
                 connectPorts(self.abc.port("contactStates"), self.octd.port("contactStates"))
+                
+        # connection for hc
+        if self.hc:
+            connectPorts(self.sh.port("q"),     self.hc.port("qRef"))
+            connectPorts(self.rh.port("q"),  self.hc.port("qAct"))
+            connectPorts(self.rh.port("dq"), self.hc.port("dqAct"))
+            connectPorts(self.hc.port("tau"), self.rh.port("tauRef"))
+#            connectPorts(self.hc.port("q"), self.rh.port("??????"))
+#            connectPorts(self.hc.port("teleopOdom"), self.rh.port("??????"))
 
         # ref force moment connection
         for sen in self.getForceSensorNames():
@@ -776,6 +789,7 @@ def getRTCListUnstable(self):
             ['es', "EmergencyStopper"],
             ['rfu', "ReferenceForceUpdater"],
             ['wbms', "WholeBodyMasterSlave"],
+            ['hc', "HapticController"],
             ['ic', "ImpedanceController"],
             ['abc', "AutoBalancer"],
             ['st', "Stabilizer"],
diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 3ffc1882fb8..9dff1948a21 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -29,8 +29,9 @@ class IKConstraint {
          localR(hrp::Matrix33::Identity()),
          constraint_weight(hrp::dvector6::Ones()),
          pos_precision(1e-4), rot_precision(deg2rad(0.1)){}
-        hrp::Vector3 getCurrentTargetPos(hrp::BodyPtr _robot){ return _robot->link(target_link_name)->p + _robot->link(target_link_name)->R * localPos; }
-        hrp::Matrix33 getCurrentTargetRot(hrp::BodyPtr _robot){ return _robot->link(target_link_name)->R * localR; }
+        hrp::Vector3 getCurrentTargetPos(const hrp::BodyPtr _robot){ return _robot->link(target_link_name)->p + _robot->link(target_link_name)->R * localPos; }
+        hrp::Matrix33 getCurrentTargetRot(const hrp::BodyPtr _robot){ return _robot->link(target_link_name)->R * localR; }
+        hrp::Pose3 getCurrentTargetPose(const hrp::BodyPtr _robot){ return hrp::Pose3(getCurrentTargetPos(_robot), getCurrentTargetRot(_robot)); }
 };
 
 
diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index 897633625c1..3784ccf4756 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -7,6 +7,8 @@
 #include <hrpUtil/Eigen4d.h>
 #include "../RobotHardware/defs.h"
 
+#include <float.h>
+
 enum { R, L, LR };
 //enum {X, Y, Z}; //already exist in RobotHardware/defs.h
 enum { XY = 2 };
@@ -29,6 +31,7 @@ static const double Q_BUTTERWORTH = 0.707106781;
 #define LIMIT_MAX(x,max) (x= ( x<max ? x:max ))
 #define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
 #define eps_eq(a, b, c)  (fabs((a)-(b)) <= c)
+#define LIMIT_NORM(v,max) if(v.norm()>max){ v=v.normalized()*max; }
 
 namespace hrp{
     class Pose3{
@@ -39,7 +42,7 @@ namespace hrp{
             Pose3()                                                                                                             { reset();}
             Pose3(const double& _X, const double& _Y, const double& _Z, const double& _r, const double& _p, const double& _y)   { p << _X,_Y,_Z; R = hrp::rotFromRpy(_r,_p,_y); }
             Pose3(const hrp::dvector6& _xyz_rpy)                                                                                { p = _xyz_rpy.head(3); R = hrp::rotFromRpy(_xyz_rpy.tail(3)); }
-            Pose3(const hrp::Vector3& _xyz, const hrp::Vector3& _rpy)                                                           { p = _xyz; R = hrp::rotFromRpy(_rpy); }
+//            Pose3(const hrp::Vector3& _xyz, const hrp::Vector3& _rpy)                                                           { p = _xyz; R = hrp::rotFromRpy(_rpy); }// ambiguous overload...
             Pose3(const hrp::Vector3& _xyz, const hrp::Matrix33& _R)                                                            { p = _xyz; R = _R; }
             void reset()                                                                                                        { p.fill(0); R.setIdentity(); }
             hrp::Vector3 rpy() const                                                                                            { return hrp::rpyFromRot(R); }
@@ -54,15 +57,13 @@ namespace hrp{
     inline hrp::Pose3           to_Pose3        (const RTC::Pose3D& in)         { return hrp::Pose3(in.position.x, in.position.y, in.position.z, in.orientation.r, in.orientation.p, in.orientation.y); }
     inline hrp::dvector6        to_dvector6     (const OpenHRP::Wrench& in)     { return (hrp::dvector6() << in.force.x, in.force.y, in.force.z, in.torque.x, in.torque.y, in.torque.z).finished(); }
     inline hrp::dvector         to_dvector      (const RTC::TimedDoubleSeq::_data_seq& in) { return hrp::dvector::Map(in.get_buffer(), in.length()); }
-    inline std::vector<std::string> to_string_vector (const OpenHRP::WholeBodyMasterSlaveService::StrSequence& in) { std::vector<std::string> ret(in.length()); for(int i=0; i<in.length(); i++){ ret[i] = in[i]; } return ret; }
 
     inline RTC::Point3D         to_Point3D      (const hrp::Vector3& in)        { return (RTC::Point3D){in(X),in(Y),in(Z)}; }
     inline RTC::Orientation3D   to_Orientation3D(const hrp::Vector3& in)        { return (RTC::Orientation3D){in(X),in(Y),in(Z)}; }
     inline RTC::Pose3D          to_Pose3D       (const hrp::dvector6& in)       { return (RTC::Pose3D){in(X),in(Y),in(Z),in(r),in(p),in(y)}; }
     inline RTC::Pose3D          to_Pose3D       (const hrp::Pose3& in)          { return (RTC::Pose3D){in.p(X),in.p(Y),in.p(Z),in.rpy()(r),in.rpy()(p),in.rpy()(y)}; }
     inline OpenHRP::Wrench      to_Wrench       (const hrp::dvector6& in)       { return (OpenHRP::Wrench){in(X),in(Y),in(Z),in(r),in(p),in(y)}; }
-    inline RTC::TimedDoubleSeq::_data_seq                       to_DoubleSeq    (const hrp::dvector& in)            { RTC::TimedDoubleSeq::_data_seq out(in.size()); hrp::dvector::Map(out.get_buffer(), in.size()) = in; return out; }
-    inline OpenHRP::WholeBodyMasterSlaveService::StrSequence    to_StrSequence  (const std::vector<std::string>& in){ OpenHRP::WholeBodyMasterSlaveService::StrSequence ret; ret.length(in.size()); for(int i=0; i<in.size(); i++){ ret[i] = in[i].c_str(); } return ret; }
+    inline RTC::TimedDoubleSeq::_data_seq   to_DoubleSeq    (const hrp::dvector& in)    { RTC::TimedDoubleSeq::_data_seq out; out.length(in.size()); hrp::dvector::Map(out.get_buffer(), in.size()) = in; return out; }
 
     inline hrp::dvector getQAll         (const hrp::BodyPtr _robot){ hrp::dvector tmp(_robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ tmp(i) = _robot->joint(i)->q; } return tmp; }
     inline void         setQAll         (hrp::BodyPtr       _robot, const hrp::dvector& in){ assert(in.size() <= _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = in(i); } }
@@ -71,6 +72,12 @@ namespace hrp{
     inline void         setRobotStateVec(hrp::BodyPtr       _robot, const hrp::dvector& _q, const hrp::Vector3& _bpos, const hrp::Vector3& _brpy){ assert(_q.size() == _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q(i); }; _robot->rootLink()->p = _bpos; _robot->rootLink()->R = hrp::rotFromRpy(_brpy); }
     inline void         setRobotStateVec(hrp::BodyPtr       _robot, const hrp::dvector& _q, const hrp::Vector3& _bpos, const hrp::Matrix33& _bR){ assert(_q.size() == _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q(i); }; _robot->rootLink()->p = _bpos; _robot->rootLink()->R = _bR; }
     inline std::vector<std::string> getJointNameAll (const hrp::BodyPtr _robot){ std::vector<std::string> ret(_robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ ret[i] = _robot->joint(i)->name; } return ret; }
+    inline hrp::dvector getUAll         (const hrp::BodyPtr _robot){ hrp::dvector tmp(_robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ tmp(i) = _robot->joint(i)->u; } return tmp; }
+
+
+    inline std::vector<std::string> to_string_vector (const RTC::TimedStringSeq::_data_seq& in) {
+        std::vector<std::string> ret(in.length()); for(int i=0; i<in.length(); i++){ ret[i] = in[i]; } return ret;
+    }
 
     inline hrp::Vector3 omegaFromRotEx(const hrp::Matrix33& r) {//copy from JointPathEx.cpp
         using ::std::numeric_limits;
@@ -102,6 +109,28 @@ namespace hrp{
     }
 }
 
+
+class BiquadIIRFilterVec2{
+    private:
+        std::vector<IIRFilter> filters;
+        hrp::dvector ans;
+    public:
+        BiquadIIRFilterVec2(){}
+        BiquadIIRFilterVec2(const int len){resize(len);}
+        void resize(const int len){filters.resize(len); ans.resize(len);}
+        ~BiquadIIRFilterVec2(){}
+        void setParameter(const hrp::dvector& fc_in, const double& HZ, const double& Q = 0.5){ for(int i=0;i<filters.size();i++){ filters[i].setParameterAsBiquad((double)fc_in(i), Q, HZ); } }
+        void setParameter(const double& fc_in, const double& HZ, const double& Q = 0.5){ setParameter(hrp::dvector::Constant(filters.size(), fc_in), HZ, Q); }//overload
+        hrp::dvector passFilter(const hrp::dvector& input){
+            for(int i=0;i<filters.size();i++){
+                ans(i) = filters[i].passFilter((double)input(i));
+            }
+            return ans;
+        }
+        void reset(const hrp::dvector& initial_input){ for(int i=0;i<filters.size();i++){ filters[i].reset((double)initial_input(i));} }
+        void reset(const double& initial_input){ for(int i=0;i<filters.size();i++){ filters[i].reset(initial_input);} }
+};
+
 inline bool has(const std::vector<std::string> v, const std::string& s){ return (std::find(v.begin(), v.end(), s) != v.end());}
 
 inline std::ostream& operator<<(std::ostream& os, hrp::Pose3& in){
diff --git a/rtc/CMakeLists.txt b/rtc/CMakeLists.txt
index d62f659d870..c3bca7b9ddc 100644
--- a/rtc/CMakeLists.txt
+++ b/rtc/CMakeLists.txt
@@ -59,6 +59,7 @@ add_subdirectory(ReferenceForceUpdater)
 add_subdirectory(AccelerationFilter)
 add_subdirectory(ObjectContactTurnaroundDetector)
 add_subdirectory(WholeBodyMasterSlave)
+add_subdirectory(HapticController)
 
 if (NOT APPLE AND USE_HRPSYSUTIL)
   add_subdirectory(VideoCapture)
diff --git a/rtc/HapticController/CMakeLists.txt b/rtc/HapticController/CMakeLists.txt
new file mode 100644
index 00000000000..09861c5e331
--- /dev/null
+++ b/rtc/HapticController/CMakeLists.txt
@@ -0,0 +1,20 @@
+set(comp_sources HapticController.cpp HapticControllerService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp ../TorqueFilter/IIRFilter.cpp)
+set(libs ${OpenCV_LIBRARIES} hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub)
+add_library(HapticController SHARED ${comp_sources})
+target_link_libraries(HapticController ${libs})
+set_target_properties(HapticController PROPERTIES PREFIX "")
+
+add_executable(HapticControllerComp HapticControllerComp.cpp ${comp_sources})
+target_link_libraries(HapticControllerComp ${libs})
+
+include_directories(${PROJECT_SOURCE_DIR}/rtc/SequencePlayer)
+
+set(target HapticController HapticControllerComp )
+
+#add_test(testPreviewControllerNoGP testPreviewController --use-gnuplot false)
+
+install(TARGETS ${target}
+  RUNTIME DESTINATION bin CONFIGURATIONS Release Debug
+  LIBRARY DESTINATION lib CONFIGURATIONS Release Debug
+)
+
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
new file mode 100644
index 00000000000..e84279ca9f9
--- /dev/null
+++ b/rtc/HapticController/HapticController.cpp
@@ -0,0 +1,599 @@
+#include "HapticController.h"
+
+#define DEBUGP (loop%200==0)
+#define DEBUGP_ONCE (loop==0)
+
+static const char* HapticController_spec[] = {
+        "implementation_id", "HapticController",
+        "type_name",         "HapticController",
+        "description",       "hapticcontroller component",
+        "version",           HRPSYS_PACKAGE_VERSION,
+        "vendor",            "AIST",
+        "category",          "example",
+        "activity_type",     "DataFlowComponent",
+        "max_instance",      "10",
+        "language",          "C++",
+        "lang_type",         "compile",
+        "conf.default.debugLevel", "0",
+        ""
+};
+
+HapticController::HapticController(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager),
+    m_qRefIn("qRef", m_qRef),
+    m_qActIn("qAct", m_qAct),
+    m_dqActIn("dqAct", m_dqAct),
+    m_qOut("q", m_qRef),
+    m_tauOut("tau", m_tau),
+    m_teleopOdomOut("teleopOdom", m_teleopOdom),
+    m_HapticControllerServicePort("HapticControllerService"),
+    m_debugLevel(0)
+{
+    m_service0.hapticcontroller(this);
+}
+
+HapticController::~HapticController(){}
+
+RTC::ReturnCode_t HapticController::onInitialize(){
+    RTCOUT << "onInitialize()" << std::endl;
+    bindParameter("debugLevel", m_debugLevel, "0");
+    addInPort("qRef", m_qRefIn);
+    addInPort("qAct", m_qActIn);
+    addInPort("dqAct", m_dqActIn);
+    addOutPort("q", m_qOut);
+    addOutPort("tau", m_tauOut);
+    addOutPort("teleopOdom", m_teleopOdomOut);
+    m_HapticControllerServicePort.registerProvider("service0", "HapticControllerService", m_service0);
+    addPort(m_HapticControllerServicePort);
+
+    RTC::Properties& prop = getProperties();
+    coil::stringTo(m_dt, prop["dt"].c_str());
+    RTC::Manager& rtcManager = RTC::Manager::instance();
+    std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
+    int comPos = nameServer.find(",");
+    if (comPos < 0){ comPos = nameServer.length(); }
+    nameServer = nameServer.substr(0, comPos);
+    RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
+
+    m_robot = hrp::BodyPtr(new hrp::Body());
+    if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
+        RTCOUT << "failed to load model[" << prop["model"] << "]" << std::endl;
+        return RTC::RTC_ERROR;
+    }
+
+    setupEEIKConstraintFromConf(ee_ikc_map, m_robot, prop);
+    RTCOUT << "setupEEIKConstraintFromConf finished" << std::endl;
+
+    output_ratio = 0.0;
+    t_ip = new interpolator(1, m_dt, interpolator::LINEAR, 1);
+    t_ip->clear();
+    t_ip->set(&output_ratio);
+    RTCOUT << "setup interpolator finished" << std::endl;
+
+    dqAct_filter = BiquadIIRFilterVec2(m_robot->numJoints());
+    dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+    dqAct_filter.reset(0);
+    for ( int i=0; i<ee_names.size(); i++) {
+        wrench_filter[ee_names[i]] = BiquadIIRFilterVec2(6);
+        wrench_filter[ee_names[i]].setParameter(hcp.wrench_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        wrench_filter[ee_names[i]].reset(0);
+    }
+//    std::vector<std::string> force_sensor_names;
+//    for (int i=0; i<m_robot->numSensors(hrp::Sensor::FORCE); i++) {
+//        force_sensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
+//    }
+
+    for ( int i=0; i<ee_names.size(); i++) {
+        std::string n = "slave_"+ee_names[i]+"_wrench";
+        m_feedbackWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_feedbackWrenches[ee_names[i]]));
+        registerInPort(n.c_str(), *m_feedbackWrenchesIn[ee_names[i]]);
+        m_feedbackWrenches[ee_names[i]].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
+        RTCOUT << " registerInPort " << n << std::endl;
+    }
+
+    tgt_names = ee_names;
+    tgt_names.push_back("com");
+    tgt_names.push_back("head");
+    for ( int i=0; i<tgt_names.size(); i++) {
+        std::string n = "master_"+tgt_names[i]+"_pose";
+        m_eePosesOut[tgt_names[i]] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
+        registerOutPort(n.c_str(), *m_eePosesOut[tgt_names[i]]);
+        RTCOUT << " registerOutPort " << n << std::endl;
+    }
+
+
+    m_tau.data = hrp::to_DoubleSeq(hrp::dvector::Zero(m_robot->numJoints()));
+
+    loop = 0;
+    RTCOUT << "onInitialize() OK" << std::endl;
+    return RTC::RTC_OK;
+}
+
+RTC::ReturnCode_t HapticController::setupEEIKConstraintFromConf(std::map<std::string, IKConstraint>& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop){
+    coil::vstring ee_conf_all = coil::split(_prop["end_effectors"], ",");
+    size_t prop_num = 10; // limbname + linkname + basename + pos(3) + angleaxis(4)
+    if (ee_conf_all.size() > 0) {
+        size_t ee_num = ee_conf_all.size()/prop_num;
+        for (size_t i = 0; i < ee_num; i++) {
+            std::string ee_name, target_link_name, base_name; // e.g. rleg, RLEG_JOINT5, WAIST
+            coil::stringTo(ee_name,             ee_conf_all[i*prop_num].c_str());
+            coil::stringTo(target_link_name,    ee_conf_all[i*prop_num+1].c_str());
+            coil::stringTo(base_name,           ee_conf_all[i*prop_num+2].c_str());
+            ee_names.push_back(ee_name);
+            _ee_ikc_map[ee_name].target_link_name = target_link_name;
+            for (size_t j = 0; j < XYZ; j++){ coil::stringTo(_ee_ikc_map[ee_name].localPos(j), ee_conf_all[i*prop_num+3+j].c_str()); }
+            double tmp_aa[4];
+            for (int j = 0; j < 4; j++ ){ coil::stringTo(tmp_aa[j], ee_conf_all[i*prop_num+6+j].c_str()); }
+            _ee_ikc_map[ee_name].localR = Eigen::AngleAxis<double>(tmp_aa[3], hrp::Vector3(tmp_aa[0], tmp_aa[1], tmp_aa[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
+            if(_robot->link(target_link_name)){
+              RTCOUT << "End Effector [" << ee_name << "]" << std::endl;
+              RTCOUT << "   target_link_name = " << _ee_ikc_map[ee_name].target_link_name << ", base = " << base_name << std::endl;
+              RTCOUT << "   offset_pos = " << _ee_ikc_map[ee_name].localPos.transpose() << "[m]" << std::endl;
+              RTCOUT << "   has_toe_joint = " << "fix to false now" << std::endl;
+            }else{
+              RTCOUT << "Target link [" << target_link_name << "] not found !" << std::endl;
+              return RTC::RTC_ERROR;
+            }
+        }
+    }
+    return RTC::RTC_OK;
+}
+
+RTC::ReturnCode_t HapticController::onExecute(RTC::UniqueId ec_id){
+    if(DEBUGP_ONCE) RTCOUT << "onExecute(" << ec_id << ")" << std::endl;
+    if (m_qRefIn.isNew())   { m_qRefIn.read(); }
+    if (m_qActIn.isNew())   { m_qActIn.read(); }
+    if (m_dqActIn.isNew())  { m_dqActIn.read(); }
+    for (int i=0; i<ee_names.size(); i++) {
+        if ( m_feedbackWrenchesIn[ee_names[i]]->isNew() ) { m_feedbackWrenchesIn[ee_names[i]]->read(); }
+    }
+
+    processTransition();
+    mode.update();
+
+    calcTorque();
+
+    if (mode.isRunning()) {
+        if(mode.isInitialize()){
+//            preProcessForHapticController();
+            idsb.setInitState(m_robot, m_dt);//逆動力学初期化
+        }
+
+        ///////////////////////////
+
+    }
+
+
+    m_tau.data = hrp::to_DoubleSeq(hrp::getUAll(m_robot) * output_ratio);
+
+    // write
+    m_qOut.write();
+    m_tauOut.write();
+    m_teleopOdomOut.write();
+
+    loop ++;
+    return RTC::RTC_OK;
+}
+
+class ContactInfo{
+    private:
+    public:
+    ContactInfo(hrp::Link* l, const hrp::Vector3& lp)
+            :target_link_(l),
+             is_contact(false),
+             local_pos_(lp){}
+     ContactInfo(){}
+        hrp::Link* target_link_;
+        hrp::Vector3 local_pos_;
+        hrp::Vector3 world_pos_;
+        hrp::Vector3 world_pos_old_;
+        hrp::Vector3 world_f_;
+        bool is_contact;
+        hrp::Vector3 CalcWorldPos(){ world_pos_ = target_link_->p + target_link_->R * local_pos_; return world_pos_;}
+        void UpdateState(){world_pos_old_ = world_pos_;}
+        friend std::ostream& operator<<(std::ostream& os, const ContactInfo& ci){
+            os<< "target_link: "<<ci.target_link_->name<<" local_pos: "<<ci.local_pos_.transpose();
+            return os;
+        }
+};
+
+void HapticController::calcTorque (){
+
+    dqAct_filtered = dqAct_filter.passFilter(hrp::to_dvector(m_dqAct.data));
+
+    for(int i=0;i<m_robot->numJoints();i++)m_robot->joint(i)->q = m_qAct.data[i];
+
+    if(!idsb.is_initialized){
+      idsb.setInitState(m_robot, m_dt);
+    }
+//    calcAccelerationsForInverseDynamics(m_robot, idsb);
+    for(int i=0;i<m_robot->numJoints();i++)idsb.q(i) = m_robot->joint(i)->q;
+    idsb.dq.fill(0);
+    idsb.ddq.fill(0);
+    const hrp::Vector3 g(0, 0, 9.80665* hcp.gravity_compensation_ratio);
+    idsb.base_p = m_robot->rootLink()->p;
+    idsb.base_v.fill(0);
+    idsb.base_dv = g;
+    idsb.base_R = hrp::Matrix33::Identity();//m_robot->rootLink()->R;
+    idsb.base_dR.fill(0);
+    idsb.base_w_hat.fill(0);
+    idsb.base_w.fill(0);
+    idsb.base_dw.fill(0);
+
+    hrp::Vector3 f_base_wld, t_base_wld;
+    calcRootLinkWrenchFromInverseDynamics(m_robot, idsb, f_base_wld, t_base_wld);
+
+//    if(loop%500==0)std::cerr<<"ref_torque is"<<std::endl;
+//    if(loop%500==0)dbgv(hrp::getUAll(m_robot));
+
+//    static std::vector<ContactInfo> civ;
+//    if(civ.size() == 0){
+//        const double a = 4;
+//        civ.push_back(ContactInfo(m_robot->link("RLEG_JOINT5"), stikp[0].localp + hrp::Vector3( 0.1,  0.05, 0)*a ));
+//        civ.push_back(ContactInfo(m_robot->link("RLEG_JOINT5"), stikp[0].localp + hrp::Vector3( 0.1, -0.05, 0)*a ));
+//        civ.push_back(ContactInfo(m_robot->link("RLEG_JOINT5"), stikp[0].localp + hrp::Vector3(-0.1,  0.05, 0)*a ));
+//        civ.push_back(ContactInfo(m_robot->link("RLEG_JOINT5"), stikp[0].localp + hrp::Vector3(-0.1, -0.05, 0)*a ));
+//        civ.push_back(ContactInfo(m_robot->link("LLEG_JOINT5"), stikp[1].localp + hrp::Vector3( 0.1,  0.05, 0)*a ));
+//        civ.push_back(ContactInfo(m_robot->link("LLEG_JOINT5"), stikp[1].localp + hrp::Vector3( 0.1, -0.05, 0)*a ));
+//        civ.push_back(ContactInfo(m_robot->link("LLEG_JOINT5"), stikp[1].localp + hrp::Vector3(-0.1,  0.05, 0)*a ));
+//        civ.push_back(ContactInfo(m_robot->link("LLEG_JOINT5"), stikp[1].localp + hrp::Vector3(-0.1, -0.05, 0)*a ));
+//
+////        civ.push_back(ContactInfo(m_robot->link("RARM_JOINT6"), stikp[2].localp + hrp::Vector3( 0.1,  0.05, 0)*a ));
+////        civ.push_back(ContactInfo(m_robot->link("RARM_JOINT6"), stikp[2].localp + hrp::Vector3( 0.1, -0.05, 0)*a ));
+////        civ.push_back(ContactInfo(m_robot->link("RARM_JOINT6"), stikp[2].localp + hrp::Vector3(-0.1,  0.05, 0)*a ));
+////        civ.push_back(ContactInfo(m_robot->link("RARM_JOINT6"), stikp[2].localp + hrp::Vector3(-0.1, -0.05, 0)*a ));
+////        civ.push_back(ContactInfo(m_robot->link("LARM_JOINT6"), stikp[3].localp + hrp::Vector3( 0.1,  0.05, 0)*a ));
+////        civ.push_back(ContactInfo(m_robot->link("LARM_JOINT6"), stikp[3].localp + hrp::Vector3( 0.1, -0.05, 0)*a ));
+////        civ.push_back(ContactInfo(m_robot->link("LARM_JOINT6"), stikp[3].localp + hrp::Vector3(-0.1,  0.05, 0)*a ));
+////        civ.push_back(ContactInfo(m_robot->link("LARM_JOINT6"), stikp[3].localp + hrp::Vector3(-0.1, -0.05, 0)*a ));
+//    }
+
+
+       // calc virtual floor reaction force
+//    {
+//        for (int i=0; i<civ.size(); i++){
+//            civ[i].CalcWorldPos();
+//            double vfloor_h;
+//            if(civ[i].target_link_->name.find("LEG") != std::string::npos){
+//                vfloor_h = m_robot->rootLink()->p(Z) - 1.2;
+//            }else{
+//                vfloor_h = m_robot->rootLink()->p(Z) - 0.1;
+//            }
+//            if(civ[i].world_pos_(2) < vfloor_h){
+//                civ[i].is_contact = true;
+//                const double pos_err = vfloor_h - civ[i].world_pos_(2);
+//                const double vel_err = 0 - (civ[i].world_pos_(2) - civ[i].world_pos_old_(2)) / dt;
+//                civ[i].world_f_ << 0, 0, 5000 * pos_err + 50 * vel_err;// P,D = ?,10 OUT
+//                if(civ[i].world_f_.norm() > 500){
+//                    civ[i].world_f_ = civ[i].world_f_.normalized() * 500;
+//                }
+//                const hrp::Vector3 world_f_rel_base = m_robot->rootLink()->R.transpose() * civ[i].world_f_;
+//                hrp::JointPath jp(m_robot->rootLink(), civ[i].target_link_);
+//                hrp::dmatrix J_contact;
+//                jp.calcJacobian(J_contact, civ[i].local_pos_);
+//                hrp::dvector tq_for_virtual_reaction_force = (J_contact.topRows(3)).transpose() * world_f_rel_base;
+//                for (int j = 0; j < jp.numJoints(); j++) jp.joint(j)->u += tq_for_virtual_reaction_force(j);
+//            }else{
+//                civ[i].is_contact = false;
+//            }
+//            civ[i].UpdateState();
+//            }
+//        if(loop%500==0)dbgv(hrp::getUAll(m_robot));
+//    }
+
+
+    {
+        //fix ee rot horizontal
+        std::vector<std::string> targets;
+        targets.push_back("LLEG_JOINT5");
+        targets.push_back("RLEG_JOINT5");
+        static std::vector<hrp::Vector3> diff_rots_old(targets.size());
+        for (int i=0; i<targets.size();i++){
+            hrp::Vector3 diff_rot;
+            rats::difference_rotation(diff_rot, m_robot->link(targets[i])->R, hrp::Matrix33::Identity());
+            hrp::Vector3 diff_rot_d = (diff_rot - diff_rots_old[i])/m_dt;
+            hrp::dvector6 wrench;
+            wrench << 0,0,0,diff_rot * 1000 + diff_rot_d * 10;
+            hrp::JointPath jp(m_robot->rootLink(), m_robot->link(targets[i]));
+            hrp::dmatrix J_ee;
+            jp.calcJacobian(J_ee);
+            hrp::dvector tq_for_fix = J_ee.transpose() * wrench;
+            for (int j = 0; j < jp.numJoints(); j++) jp.joint(j)->u += tq_for_fix(j);
+            diff_rots_old[i] = diff_rot;
+        }
+    }
+
+    {
+        //keep ee apart each other
+        std::vector<std::string> targets;
+        targets.push_back("LLEG_JOINT5");
+        targets.push_back("RLEG_JOINT5");
+        double dist = m_robot->link("LLEG_JOINT5")->p(Y) - m_robot->link("RLEG_JOINT5")->p(Y);
+        if(dist < 0.3){
+            double err = dist - 0.3;
+            for (int i=0; i<targets.size();i++){
+                hrp::dvector6 wrench;
+                wrench << 0, (i==0 ? -1 : 1) * err*1000,0,0,0,0;
+                hrp::JointPath jp(m_robot->rootLink(), m_robot->link(targets[i]));
+                hrp::dmatrix J_ee;
+                jp.calcJacobian(J_ee);
+                hrp::dvector tq_for_apart = J_ee.transpose() * wrench;
+                for (int j = 0; j < jp.numJoints(); j++) jp.joint(j)->u += tq_for_apart(j);
+            }
+        }
+    }
+
+    {
+        // floor
+        std::vector<std::string> targets;
+        targets.push_back("LLEG_JOINT5");
+        targets.push_back("RLEG_JOINT5");
+        for (int i=0; i<targets.size();i++){
+            double height = m_robot->link(targets[i])->p(Z) - m_robot->rootLink()->p(Z);
+            if(height < -1.0){
+                hrp::dvector6 wrench;
+                wrench << 0,0, -(height-(-1.0))*10000,0,0,0;
+
+                if(m_robot->link(targets[i])->p(X)<0) wrench(X) += - m_robot->link(targets[i])->p(X) * 1000;
+
+                hrp::JointPath jp(m_robot->rootLink(), m_robot->link(targets[i]));
+                hrp::dmatrix J_ee;
+                jp.calcJacobian(J_ee);
+                hrp::dvector tq_for_apart = J_ee.transpose() * wrench;
+                for (int j = 0; j < jp.numJoints(); j++) jp.joint(j)->u += tq_for_apart(j);
+            }
+        }
+    }
+
+
+    // calc real robot feedback force
+    std::vector<hrp::Pose3> ee_pose(ee_names.size());
+    for(int i=0; i<ee_pose.size();i++){
+        ee_pose[i].p = m_robot->link(ee_ikc_map[ee_names[i]].target_link_name)->p;
+        ee_pose[i].R = m_robot->link(ee_ikc_map[ee_names[i]].target_link_name)->R;
+    }
+    static std::vector<hrp::Pose3> ee_pose_old(ee_pose);
+    std::vector<hrp::dvector6> wrench_ee_wld;
+    if(wrench_ee_wld.size() != ee_names.size()){
+        wrench_ee_wld.resize(ee_names.size());
+        for(int i=0;i<wrench_ee_wld.size();i++){ wrench_ee_wld[i] = hrp::dvector6::Zero(); }
+    }
+
+    for (int i=0; i<ee_names.size(); i++){
+        wrench_filtered[ee_names[i]] = wrench_filter[ee_names[i]].passFilter( hrp::to_dvector(m_feedbackWrenches[ee_names[i]].data) );
+        hrp::Vector3 ee_pos_vel = (ee_pose[i].p - ee_pose_old[i].p) / m_dt;
+        hrp::Vector3 ee_rot_vel = ee_pose_old[i].R * hrp::omegaFromRot(ee_pose_old[i].R.transpose() * ee_pose[i].R) / m_dt;
+        hrp::Vector3 f = wrench_filtered[ee_names[i]].head(3) * hcp.force_feedback_ratio;
+        hrp::Vector3 t = wrench_filtered[ee_names[i]].tail(3) * hcp.force_feedback_ratio;
+
+        if(f.norm() > 1e-6 ){ // avoid zero devide
+            f = f * fabs(1 - 1 * f.normalized().dot(ee_pos_vel) );
+        }
+        if(t.norm() > 1e-6){ // avoid zero devide
+            t = t * fabs(1 - 0.1 * t.normalized().dot(ee_rot_vel) );
+        }
+        LIMIT_NORM(f,100);
+        LIMIT_NORM(t,10);
+
+        wrench_ee_wld[i] = (hrp::dvector6() << f,t).finished();
+
+        hrp::JointPath jp(m_robot->rootLink(), m_robot->link(ee_ikc_map[ee_names[i]].target_link_name));
+        hrp::dmatrix J_base_to_ee;
+        jp.calcJacobian(J_base_to_ee, ee_ikc_map[ee_names[i]].localPos);
+        hrp::dvector tq_from_feedbackWrench = J_base_to_ee.transpose() * wrench_ee_wld[i];
+        for (int j = 0; j < jp.numJoints(); j++) jp.joint(j)->u += tq_from_feedbackWrench(j);
+        if(loop%500==0)dbgv(wrench_ee_wld[i]);
+    }
+    ee_pose_old = ee_pose;
+
+
+    {
+        // soft joint limit
+        // joint limit
+        m_robot->link("RARM_JOINT1")->ulimit = deg2rad(-10);
+        m_robot->link("LARM_JOINT1")->llimit = deg2rad(10);
+        for(int i=0;i<m_robot->numJoints();i++){
+            const double soft_ulimit = m_robot->joint(i)->ulimit - deg2rad(15);
+            const double soft_llimit = m_robot->joint(i)->llimit + deg2rad(15);
+            double soft_jlimit_tq = 0;
+            if(m_qAct.data[i] > soft_ulimit){
+                soft_jlimit_tq = 100 * (soft_ulimit - m_qAct.data[i]) + 1 * (0 - dqAct_filtered(i));
+            }
+            if(m_qAct.data[i] < soft_llimit){
+                soft_jlimit_tq = 100 * (soft_llimit - m_qAct.data[i]) + 1 * (0 - dqAct_filtered(i));
+            }
+            m_robot->joint(i)->u += soft_jlimit_tq;
+        }
+        if(loop%500==0)dbgv(hrp::getUAll(m_robot));
+    }
+
+
+
+    {
+        // real robot joint friction damper
+        hrp::dvector friction_tq = -3 * dqAct_filtered;
+        for (int i=0; i<m_robot->numJoints(); i++){
+         LIMIT_MINMAX(friction_tq(i), -10, 10);
+         m_robot->joint(i)->u += friction_tq(i);
+        }
+        //         if(loop%500==0)dbgv(friction_tq);
+    }
+
+
+    for(int i=0;i<m_robot->numJoints();i++){
+        LIMIT_MINMAX(m_robot->joint(i)->u, -80,80);
+        double tlimit = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst;////?????
+    }
+
+    updateInvDynStateBuffer(idsb);
+
+
+
+
+    for (int i=0; i<tgt_names.size(); i++){
+        if(tgt_names[i] == "com"){
+            m_masterTgtPoses[tgt_names[i]].data = hrp::to_Pose3D(hrp::Pose3(m_robot->calcCM(), hrp::Matrix33::Identity()));
+        }else if(tgt_names[i] == "head"){
+            // dummy
+        }else{
+            m_masterTgtPoses[tgt_names[i]].data = hrp::to_Pose3D(ee_ikc_map[ee_names[i]].getCurrentTargetPose(m_robot));//四肢揃ってないと危険
+        }
+        m_masterTgtPoses[tgt_names[i]].tm = m_qRef.tm;
+        m_eePosesOut[tgt_names[i]]->write();
+    }
+
+
+    m_teleopOdom.data = (RTC::Pose3D){1.0,2.0,3.0,0.0,0.0,1.57* loop * 0.001};
+    m_teleopOdom.tm = m_qRef.tm;
+    m_teleopOdomOut.write();
+
+}
+
+void HapticController::processTransition(){
+    switch(mode.now()){
+
+        case MODE_SYNC_TO_HC:
+            if(mode.pre() == MODE_IDLE){ double tmp = 1.0; t_ip->setGoal(&tmp, 10.0, true); }
+            if (!t_ip->isEmpty() ){
+                t_ip->get(&output_ratio, true);
+            }else{
+                mode.setNextMode(MODE_HC);
+            }
+            break;
+
+        case MODE_SYNC_TO_IDLE:
+            if(mode.pre() == MODE_HC || mode.pre() == MODE_PAUSE){ double tmp = 0.0; t_ip->setGoal(&tmp, 10.0, true); }
+            if (!t_ip->isEmpty()) {
+                t_ip->get(&output_ratio, true);
+            }else{
+                mode.setNextMode(MODE_IDLE);
+            }
+            break;
+    }
+}
+
+
+//void HapticController::preProcessForHapticController(){
+//    hrp::Vector3 basePos_heightChecked = hrp::to_Vector3(m_basePos.data);//ベースリンク高さ調整により足裏高さ0に
+//    fik->m_robot->rootLink()->p = basePos_heightChecked;
+////    for ( int i = 0; i < fik->m_robot->numJoints(); i++ ){ fik->m_robot->joint(i)->q = m_qRef.data[i]; }
+//    hrp::setQAll(fik->m_robot, hrp::to_dvector(m_qRef.data));
+//    fik->m_robot->calcForwardKinematics();
+//
+//    //TODO
+////    hrp::Vector3 init_foot_mid_coord = (fik_in->getEndEffectorPos("rleg") + fik_in->getEndEffectorPos("lleg")) / 2;
+////    if( fabs((double)init_foot_mid_coord(Z)) > 1e-5 ){
+////        basePos_heightChecked(Z) -= init_foot_mid_coord(Z);
+////        init_foot_mid_coord(Z) = 0;
+////        std::cerr<<"["<<m_profile.instance_name<<"] Input basePos height is invalid. Auto modify "<<m_basePos.data.z<<" -> "<<basePos_heightChecked(Z)<<endl;
+////    }
+//
+//    std::vector<hrp::BodyPtr> body_list;
+//    body_list.push_back(fik->m_robot);
+//    body_list.push_back(m_robot_vsafe);
+//    for(int i=0;i<body_list.size();i++){//初期姿勢でBodyをFK
+//        hrp::setRobotStateVec(body_list[i], hrp::to_dvector(m_qRef.data), basePos_heightChecked, hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y));
+//        body_list[i]->calcForwardKinematics();
+//    }
+//
+//    fik->q_ref = hrp::getRobotStateVec(fik->m_robot);
+//    q_ip->set(fik->q_ref.data());
+//    wbms->initializeRequest(fik->m_robot, ee_ikc_map);
+//}
+
+
+
+bool HapticController::startHapticController(){
+    if(mode.now() == MODE_IDLE){
+        RTCOUT << "startHapticController" << std::endl;
+        mode.setNextMode(MODE_SYNC_TO_HC);
+        return true;
+    }else{
+        RTCOUT << "Invalid context to startHapticController" << std::endl;
+        return false;
+    }
+}
+
+
+bool HapticController::pauseHapticController(){
+    if(mode.now() == MODE_HC){
+        RTCOUT << "pauseHapticController" << std::endl;
+        mode.setNextMode(MODE_PAUSE);
+        return true;
+    }else{
+        RTCOUT << "Invalid context to pauseHapticController" << std::endl;
+        return false;
+    }
+}
+
+
+bool HapticController::resumeHapticController(){
+    if(mode.now() == MODE_PAUSE){
+        RTCOUT << "resumeHapticController" << std::endl;
+        mode.setNextMode(MODE_HC);
+        return true;
+    }else{
+        RTCOUT << "Invalid context to resumeHapticController" << std::endl;
+        return false;
+    }
+}
+
+
+bool HapticController::stopHapticController(){
+    if(mode.now() == MODE_HC || mode.now() == MODE_PAUSE ){
+        RTCOUT << "stopHapticController" << std::endl;
+        mode.setNextMode(MODE_SYNC_TO_IDLE);
+        return true;
+    }else{
+        RTCOUT << "Invalid context to stopHapticController" << std::endl;
+        return false;
+    }
+}
+
+
+bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param){
+    RTCOUT << "setHapticControllerParam" << std::endl;
+
+    hcp.gravity_compensation_ratio = i_param.gravity_compensation_ratio;
+    hcp.dqAct_filter_cutoff_hz = i_param.dqAct_filter_cutoff_hz;
+    hcp.wrench_filter_cutoff_hz = i_param.wrench_filter_cutoff_hz;
+    hcp.force_feedback_ratio = i_param.force_feedback_ratio;
+//    wbms->wp.auto_swing_foot_landing_threshold  = i_param.auto_swing_foot_landing_threshold;
+//    wbms->wp.human_to_robot_ratio               = i_param.human_to_robot_ratio;
+//    wbms->wp.set_com_height_fix                 = i_param.set_com_height_fix;
+//    wbms->wp.set_com_height_fix_val             = i_param.set_com_height_fix_val;
+//    wbms->wp.swing_foot_height_offset           = i_param.swing_foot_height_offset;
+//    wbms->wp.upper_body_rmc_ratio               = i_param.upper_body_rmc_ratio;
+//    if(mode.now() == MODE_IDLE){
+//        wbms->wp.use_head                       = i_param.use_head;
+//        wbms->wp.use_upper                      = i_param.use_upper;
+//        wbms->wp.use_lower                      = i_param.use_lower;
+//        wbms->wp.use_joints                     = hrp::to_string_vector(i_param.use_joints);
+//        wbms->wp.use_targets                    = hrp::to_string_vector(i_param.use_targets);
+//    }else{
+//      RTCOUT << "use_head, use_upper, use_lower can be changed in MODE_IDLE" << std::endl;
+//    }
+    return true;
+}
+
+
+bool HapticController::getParams(OpenHRP::HapticControllerService::HapticControllerParam& i_param){
+    RTCOUT << "getHapticControllerParam" << std::endl;
+    i_param.gravity_compensation_ratio = hcp.gravity_compensation_ratio;
+    i_param.dqAct_filter_cutoff_hz = hcp.dqAct_filter_cutoff_hz;
+    i_param.wrench_filter_cutoff_hz = hcp.wrench_filter_cutoff_hz;
+    i_param.force_feedback_ratio = hcp.force_feedback_ratio;
+//    i_param.use_targets                         = hrp::to_StrSequence(wbms->wp.use_targets);
+    return true;
+}
+
+
+RTC::ReturnCode_t HapticController::onActivated(RTC::UniqueId ec_id){ RTCOUT << "onActivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; }
+RTC::ReturnCode_t HapticController::onDeactivated(RTC::UniqueId ec_id){ RTCOUT << "onDeactivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; }
+RTC::ReturnCode_t HapticController::onFinalize(){ return RTC::RTC_OK; }
+
+extern "C"{
+    void HapticControllerInit(RTC::Manager* manager) {
+        RTC::Properties profile(HapticController_spec);
+        manager->registerFactory(profile, RTC::Create<HapticController>, RTC::Delete<HapticController>);
+    }
+};
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
new file mode 100644
index 00000000000..210ae30cd82
--- /dev/null
+++ b/rtc/HapticController/HapticController.h
@@ -0,0 +1,153 @@
+#ifndef HAPTICCONTROLLER_H
+#define HAPTICCONTROLLER_H
+
+#include <rtm/idl/BasicDataType.hh>
+#include <rtm/idl/ExtendedDataTypes.hh>
+#include <rtm/Manager.h>
+#include <rtm/DataFlowComponentBase.h>
+#include <rtm/CorbaPort.h>
+#include <rtm/DataInPort.h>
+#include <rtm/DataOutPort.h>
+#include <rtm/idl/BasicDataTypeSkel.h>
+#include <rtm/idl/ExtendedDataTypesSkel.h>
+#include <rtm/CorbaNaming.h>
+#include <hrpModel/ModelLoaderUtil.h>
+#include <hrpModel/Sensor.h>
+#include <time.h>
+
+#include "HapticControllerService_impl.h"
+#include "../Stabilizer/StabilizerService_impl.h"
+#include "../AutoBalancer/AutoBalancerService_impl.h"
+//#include "wbms_core.h"
+#include "../AutoBalancer/FullbodyInverseKinematicsSolver.h"
+
+#include "../SequencePlayer/interpolator.h"
+#include "../ImpedanceController/JointPathEx.h"
+
+enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_HC, MODE_HC, MODE_PAUSE, MODE_SYNC_TO_IDLE};
+
+class ControlMode{
+    private:
+        mode_enum current, previous, next;
+    public:
+        ControlMode(){ current = previous = next = MODE_IDLE;}
+        ~ControlMode(){}
+        bool setNextMode(const mode_enum _request){
+            switch(_request){
+                case MODE_SYNC_TO_HC:
+                    if(current == MODE_IDLE)                                    { next = _request; return true; }else{ return false; }
+                case MODE_HC:
+                    if(current == MODE_SYNC_TO_HC || current == MODE_PAUSE )  { next = _request; return true; }else{ return false; }
+                case MODE_PAUSE:
+                    if(current == MODE_HC)                                    { next = _request; return true; }else{ return false; }
+                case MODE_SYNC_TO_IDLE:
+                    if(current == MODE_HC || current == MODE_PAUSE )          { next = _request; return true; }else{ return false; }
+                case MODE_IDLE:
+                    if(current == MODE_SYNC_TO_IDLE )                           { next = _request; return true; }else{ return false; }
+                default:
+                    return false;
+            }
+        }
+        void update(){ previous = current; current = next; }
+        mode_enum now(){ return current; }
+        mode_enum pre(){ return previous; }
+        bool isRunning(){ return (current==MODE_SYNC_TO_HC) || (current==MODE_HC) || (current==MODE_PAUSE) || (current==MODE_SYNC_TO_IDLE) ;}
+        bool isInitialize(){ return (previous==MODE_IDLE) && (current==MODE_SYNC_TO_HC) ;}
+};
+
+class HapticController : public RTC::DataFlowComponentBase{
+    public:
+        HapticController(RTC::Manager* manager);
+        virtual ~HapticController();
+        virtual RTC::ReturnCode_t onInitialize();
+        virtual RTC::ReturnCode_t onFinalize();
+        virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
+        virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
+        virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
+        bool startHapticController();
+        bool stopHapticController();
+        bool pauseHapticController();
+        bool resumeHapticController();
+        bool setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param);
+        bool getParams(OpenHRP::HapticControllerService::HapticControllerParam& i_param);
+
+    protected:
+
+        RTC::TimedDoubleSeq m_qRef;
+        RTC::InPort<RTC::TimedDoubleSeq> m_qRefIn;
+        RTC::TimedDoubleSeq m_qAct;
+        RTC::InPort<RTC::TimedDoubleSeq> m_qActIn;
+        RTC::TimedDoubleSeq m_dqAct;
+        RTC::InPort<RTC::TimedDoubleSeq> m_dqActIn;
+        std::map<std::string, RTC::TimedDoubleSeq> m_feedbackWrenches;
+        typedef boost::shared_ptr<RTC::InPort<RTC::TimedDoubleSeq> > ITDS_Ptr;
+        std::map<std::string, ITDS_Ptr> m_feedbackWrenchesIn;
+
+        RTC::TimedDoubleSeq m_tau;
+        RTC::OutPort<RTC::TimedDoubleSeq> m_tauOut;
+        RTC::TimedPose3D m_teleopOdom;
+        RTC::OutPort<RTC::TimedPose3D> m_teleopOdomOut;
+        RTC::OutPort<RTC::TimedDoubleSeq> m_qOut;
+        std::map<std::string, RTC::TimedPose3D> m_masterTgtPoses;
+        typedef boost::shared_ptr<RTC::OutPort<RTC::TimedPose3D> > OTP3_Ptr;
+        std::map<std::string, OTP3_Ptr> m_eePosesOut;
+
+//        RTC::TimedDoubleSeq m_dq_filtered_dbg;
+//        RTC::OutPort<RTC::TimedDoubleSeq> m_dq_filtered_dbgOut;
+//        std::map<std::string, RTC::TimedDoubleSeq> m_feedbackWrenches_filtered_dbg;
+//        RTC::OutPort<RTC::TimedDoubleSeq> m_dq_filtered_dbgOut;
+
+
+
+        RTC::CorbaPort m_HapticControllerServicePort;
+
+        hrp::BodyPtr m_robot;
+
+        HapticControllerService_impl m_service0;
+
+    private:
+        double m_dt;
+        unsigned int loop;
+        unsigned int m_debugLevel;
+
+        class HCParams {
+            public:
+                double gravity_compensation_ratio;
+                double force_feedback_ratio;
+                double dqAct_filter_cutoff_hz;
+                double wrench_filter_cutoff_hz;
+            HCParams(){
+                gravity_compensation_ratio = 0.9;
+                force_feedback_ratio = 0.5;
+                dqAct_filter_cutoff_hz = 10;
+                wrench_filter_cutoff_hz = 10;
+            }
+        } hcp;
+
+
+        double output_ratio;
+        interpolator *t_ip,*q_ip;
+        hrp::dvector avg_q_vel, avg_q_acc;
+
+        hrp::InvDynStateBuffer idsb;
+        BiquadIIRFilterVec2 dqAct_filter;
+        hrp::dvector dqAct_filtered;
+        std::map<std::string, BiquadIIRFilterVec2> wrench_filter;
+        std::map<std::string, hrp::dvector6> wrench_filtered;
+
+        std::map<std::string, IKConstraint> ee_ikc_map;
+        ControlMode mode;
+
+        std::vector<std::string> ee_names;
+        std::vector<std::string> tgt_names;
+
+        RTC::ReturnCode_t setupEEIKConstraintFromConf(std::map<std::string, IKConstraint>& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop);
+        void calcTorque();
+        void processTransition();
+//        void processHapticController(const HumanPose& pose_ref);
+};
+
+
+extern "C" {    void HapticControllerInit(RTC::Manager* manager);   };
+
+#endif
diff --git a/rtc/HapticController/HapticControllerComp.cpp b/rtc/HapticController/HapticControllerComp.cpp
new file mode 100644
index 00000000000..41e5b717239
--- /dev/null
+++ b/rtc/HapticController/HapticControllerComp.cpp
@@ -0,0 +1,50 @@
+// -*- C++ -*-
+/*!
+ * @file HapticControllerComp.cpp
+ * @brief Standalone component
+ * @date $Date$
+ *
+ * $Id$
+ */
+
+#include <rtm/Manager.h>
+#include <iostream>
+#include <string>
+#include "HapticController.h"
+
+
+void MyModuleInit(RTC::Manager* manager)
+{
+  HapticControllerInit(manager);
+  RTC::RtcBase* comp;
+
+  // Create a component
+  comp = manager->createComponent("HapticController");
+
+  return;
+}
+
+int main (int argc, char** argv)
+{
+  RTC::Manager* manager;
+  manager = RTC::Manager::init(argc, argv);
+
+  // Initialize manager
+  manager->init(argc, argv);
+
+  // Set module initialization proceduer
+  // This procedure will be invoked in activateManager() function.
+  manager->setModuleInitProc(MyModuleInit);
+
+  // Activate manager and register to naming service
+  manager->activateManager();
+
+  // run the manager in blocking mode
+  // runManager(false) is the default.
+  manager->runManager();
+
+  // If you want to run the manager in non-blocking mode, do like this
+  // manager->runManager(true);
+
+  return 0;
+}
diff --git a/rtc/HapticController/HapticControllerService_impl.cpp b/rtc/HapticController/HapticControllerService_impl.cpp
new file mode 100644
index 00000000000..2c2f1761770
--- /dev/null
+++ b/rtc/HapticController/HapticControllerService_impl.cpp
@@ -0,0 +1,54 @@
+#include "HapticControllerService_impl.h"
+#include "HapticController.h"
+
+HapticControllerService_impl::HapticControllerService_impl() : m_hapticcontroller(NULL)
+{
+}
+
+HapticControllerService_impl::~HapticControllerService_impl()
+{
+}
+
+void HapticControllerService_impl::hapticcontroller(HapticController *i_hapticcontroller)
+{
+  m_hapticcontroller = i_hapticcontroller;
+}
+
+CORBA::Boolean HapticControllerService_impl::startHapticController()
+{
+    return m_hapticcontroller->startHapticController();
+};
+
+CORBA::Boolean HapticControllerService_impl::stopHapticController()
+{
+    return m_hapticcontroller->stopHapticController();
+};
+
+CORBA::Boolean HapticControllerService_impl::pauseHapticController()
+{
+    return m_hapticcontroller->pauseHapticController();
+};
+
+CORBA::Boolean HapticControllerService_impl::resumeHapticController()
+{
+    return m_hapticcontroller->resumeHapticController();
+};
+//
+void HapticControllerService_impl::setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param)
+{
+  m_hapticcontroller->setParams(i_param);
+};
+
+void HapticControllerService_impl::getParams(OpenHRP::HapticControllerService::HapticControllerParam_out i_param)
+{
+    //????????????????????????????????????????????????????????
+    OpenHRP::HapticControllerService::HapticControllerParam tmp;
+    m_hapticcontroller->getParams(tmp);
+    i_param = tmp;
+
+    //今までこれでいけてた？
+//    i_param = new OpenHRP::HapticControllerService::HapticControllerParam();
+//    m_hapticcontroller->getParams(*i_param);
+
+//  m_hapticcontroller->getParams(i_param); // error: no matching function for call to ‘HapticController::getParams(OpenHRP::HapticControllerService::HapticControllerParam_out&)’
+};
diff --git a/rtc/HapticController/HapticControllerService_impl.h b/rtc/HapticController/HapticControllerService_impl.h
new file mode 100644
index 00000000000..0be6cbf64a6
--- /dev/null
+++ b/rtc/HapticController/HapticControllerService_impl.h
@@ -0,0 +1,31 @@
+// -*-C++-*-
+#ifndef HAPTICCONTROLLERSERVICESVC_IMPL_H
+#define HAPTICCONTROLLERSERVICESVC_IMPL_H
+
+#include "hrpsys/idl/HRPDataTypes.hh"
+#include "hrpsys/idl/HapticControllerService.hh"
+
+using namespace OpenHRP;
+
+class HapticController;
+
+class HapticControllerService_impl
+  : public virtual POA_OpenHRP::HapticControllerService,
+    public virtual PortableServer::RefCountServantBase
+{
+public:
+  HapticControllerService_impl();
+  virtual ~HapticControllerService_impl();
+  CORBA::Boolean startHapticController();
+  CORBA::Boolean stopHapticController();
+  CORBA::Boolean pauseHapticController();
+  CORBA::Boolean resumeHapticController();
+  void setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param);
+  void getParams(OpenHRP::HapticControllerService::HapticControllerParam_out i_param);
+  //
+  void hapticcontroller(HapticController *i_hapticcontroller);
+private:
+  HapticController *m_hapticcontroller;
+};				 
+
+#endif
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 54b49941a5f..37774b5b965 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -29,17 +29,10 @@ WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlo
         m_basePosOut("basePosOut", m_basePos),
         m_baseRpyOut("baseRpyOut", m_baseRpy),
         m_optionalDataOut("optionalDataOut", m_optionalData),
-        m_htzmpIn("htzmpIn", m_htzmp),// from ros bridge
-        m_htrfwIn("htrfwIn", m_htrfw),
-        m_htlfwIn("htlfwIn", m_htlfw),
-        m_htcomIn("htcomIn", m_htcom),
-        m_htrfIn("htrfIn", m_htrf),
-        m_htlfIn("htlfIn", m_htlf),
-        m_htrhIn("htrhIn", m_htrh),
-        m_htlhIn("htlhIn", m_htlh),
-        m_htheadIn("htheadIn", m_hthead),
         m_actCPIn("actCapturePoint", m_actCP),
         m_actZMPIn("zmp", m_actZMP),
+        m_exDataIn("exData", m_exData),
+        m_exDataIndexIn("exDataIndex", m_exDataIndex),
 #ifdef USE_DEBUG_PORT
         m_htcom_dbgOut("htcom_dbgOut", m_htcom_dbg),// to ros bridge
         m_htrf_dbgOut("htrf_dbgOut", m_htrf_dbg),
@@ -84,17 +77,10 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     addOutPort("basePosOut", m_basePosOut);
     addOutPort("baseRpyOut", m_baseRpyOut);
     addOutPort("optionalDataOut", m_optionalDataOut);
-    addInPort("htzmpIn", m_htzmpIn);// from ros bridge
-    addInPort("htrfwIn", m_htrfwIn);
-    addInPort("htlfwIn", m_htlfwIn);
-    addInPort("htcomIn", m_htcomIn);
-    addInPort("htrfIn", m_htrfIn);
-    addInPort("htlfIn", m_htlfIn);
-    addInPort("htrhIn", m_htrhIn);
-    addInPort("htlhIn", m_htlhIn);
-    addInPort("htheadIn", m_htheadIn);
     addInPort("actCapturePoint", m_actCPIn);
     addInPort("zmp", m_actZMPIn);
+    addInPort("exData", m_exDataIn);
+    addInPort("exDataIndex", m_exDataIndexIn);
 #ifdef USE_DEBUG_PORT
     addOutPort("htcom_dbgOut", m_htcom_dbgOut);// to ros bridge
     addOutPort("htrf_dbgOut", m_htrf_dbgOut);
@@ -206,6 +192,23 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     lt.fill(0);
     rt.fill(0);
 
+    for ( int i=0; i<ee_names.size(); i++) {
+        std::string n = "slave_"+ee_names[i]+"_wrench";
+        m_slaveEEWrenchesOut[tgt_names[i]] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[tgt_names[i]]));
+        registerOutPort(n.c_str(), *m_slaveEEWrenchesOut[tgt_names[i]]);
+        RTCOUT << " registerOutPort " << n << std::endl;
+    }
+
+    tgt_names = ee_names;
+    tgt_names.push_back("com");
+    tgt_names.push_back("head");
+    for ( int i=0; i<tgt_names.size(); i++) {
+        std::string n = "master_"+tgt_names[i]+"_pose";
+        m_masterTgtPosesIn[tgt_names[i]] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
+        registerInPort(n.c_str(), *m_masterTgtPosesIn[tgt_names[i]]);
+        RTCOUT << " registerInPort " << n << std::endl;
+    }
+
     RTCOUT << "onInitialize() OK" << std::endl;
     loop = 0;
     return RTC::RTC_OK;
@@ -213,14 +216,15 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
 
 RTC::ReturnCode_t WholeBodyMasterSlave::setupEEIKConstraintFromConf(std::map<std::string, IKConstraint>& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop){
     coil::vstring ee_conf_all = coil::split(_prop["end_effectors"], ",");
-    size_t prop_num = 10;
+    size_t prop_num = 10; // limbname + linkname + basename + pos(3) + angleaxis(4)
     if (ee_conf_all.size() > 0) {
         size_t ee_num = ee_conf_all.size()/prop_num;
         for (size_t i = 0; i < ee_num; i++) {
             std::string ee_name, target_link_name, base_name; // e.g. rleg, RLEG_JOINT5, WAIST
-            coil::stringTo(ee_name, ee_conf_all[i*prop_num].c_str());
-            coil::stringTo(target_link_name, ee_conf_all[i*prop_num+1].c_str());
-            coil::stringTo(base_name, ee_conf_all[i*prop_num+2].c_str());
+            coil::stringTo(ee_name,             ee_conf_all[i*prop_num].c_str());
+            coil::stringTo(target_link_name,    ee_conf_all[i*prop_num+1].c_str());
+            coil::stringTo(base_name,           ee_conf_all[i*prop_num+2].c_str());
+            ee_names.push_back(ee_name);
             _ee_ikc_map[ee_name].target_link_name = target_link_name;
             for (size_t j = 0; j < XYZ; j++){ coil::stringTo(_ee_ikc_map[ee_name].localPos(j), ee_conf_all[i*prop_num+3+j].c_str()); }
             double tmp_aa[4];
@@ -250,42 +254,52 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if (m_baseRpyIn.isNew()) { m_baseRpyIn.read(); }
     if (m_zmpIn.isNew()) { m_zmpIn.read(); }
     if (m_optionalDataIn.isNew()) { m_optionalDataIn.read(); }
-    if (m_htrfwIn.isNew())  { m_htrfwIn.read(); wbms->hp_wld_raw.tgt[rf].w      = hrp::to_dvector6(m_htrfw.data); }
-    if (m_htlfwIn.isNew())  { m_htlfwIn.read(); wbms->hp_wld_raw.tgt[lf].w      = hrp::to_dvector6(m_htlfw.data); }
+//    if (m_htrfwIn.isNew())  { m_htrfwIn.read(); wbms->hp_wld_raw.tgt[rf].w      = hrp::to_dvector6(m_htrfw.data); }
+//    if (m_htlfwIn.isNew())  { m_htlfwIn.read(); wbms->hp_wld_raw.tgt[lf].w      = hrp::to_dvector6(m_htlfw.data); }
 
     // button func
-    if(m_htlfw.data.force.y >= 1 && m_htrfw.data.force.y >= 1 && mode.now() == MODE_IDLE){
-        std::cerr<<"startWholeBodyMasterSlave() called by button"<<std::endl;
-        startWholeBodyMasterSlave();
-    }else if(m_htlfw.data.force.y >= 1 && m_htrfw.data.force.y >= 1 && mode.now() == MODE_WBMS){
-        std::cerr<<"stopWholeBodyMasterSlave() called by button"<<std::endl;
-        stopWholeBodyMasterSlave();
-    }
-    static bool blocking_continuous_hits = false;
-    if(m_htlfw.data.force.z >= 1 && m_htrfw.data.force.z >= 1 && mode.now() == MODE_PAUSE){
-        if(!blocking_continuous_hits){
-            std::cerr<<"resumeWholeBodyMasterSlave() called by button"<<std::endl;
-            resumeWholeBodyMasterSlave();
-            blocking_continuous_hits = true;
-        }
-    }else if(m_htlfw.data.force.z >= 1 && m_htrfw.data.force.z >= 1 && mode.now() == MODE_WBMS){
-        if(!blocking_continuous_hits){
-            std::cerr<<"pauseWholeBodyMasterSlave() called by button"<<std::endl;
-            pauseWholeBodyMasterSlave();
-            blocking_continuous_hits = true;
-        }
-    }else{
-        blocking_continuous_hits = false;
+    hrp::dvector ex_data;
+    std::vector<std::string> ex_data_index;
+    if(m_exDataIn.isNew()){
+        m_exDataIn.read();
+        ex_data = hrp::to_dvector(m_exData.data);
     }
+    if(m_exDataIndexIn.isNew()){
+        m_exDataIndexIn.read();
+        ex_data_index = hrp::to_string_vector(m_exDataIndex.data);
+    }
+
+//    if(m_htlfw.data.force.y >= 1 && m_htrfw.data.force.y >= 1 && mode.now() == MODE_IDLE){
+//        std::cerr<<"startWholeBodyMasterSlave() called by button"<<std::endl;
+//        startWholeBodyMasterSlave();
+//    }else if(m_htlfw.data.force.y >= 1 && m_htrfw.data.force.y >= 1 && mode.now() == MODE_WBMS){
+//        std::cerr<<"stopWholeBodyMasterSlave() called by button"<<std::endl;
+//        stopWholeBodyMasterSlave();
+//    }
+//    static bool is_blocking_continuous_hits = false;
+//    if(m_htlfw.data.force.z >= 1 && m_htrfw.data.force.z >= 1 && mode.now() == MODE_PAUSE){
+//        if(!is_blocking_continuous_hits){
+//            std::cerr<<"resumeWholeBodyMasterSlave() called by button"<<std::endl;
+//            resumeWholeBodyMasterSlave();
+//            is_blocking_continuous_hits = true;
+//        }
+//    }else if(m_htlfw.data.force.z >= 1 && m_htrfw.data.force.z >= 1 && mode.now() == MODE_WBMS){
+//        if(!is_blocking_continuous_hits){
+//            std::cerr<<"pauseWholeBodyMasterSlave() called by button"<<std::endl;
+//            pauseWholeBodyMasterSlave();
+//            is_blocking_continuous_hits = true;
+//        }
+//    }else{
+//        is_blocking_continuous_hits = false;
+//    }
 
     if( mode.now() != MODE_PAUSE ){ // stop updating input when MODE_PAUSE
-        if (m_htcomIn.isNew())  { m_htcomIn.read(); wbms->hp_wld_raw.tgt[com].abs   = hrp::to_Pose3(m_htcom.data); }
-        if (m_htrfIn.isNew())   { m_htrfIn.read();  wbms->hp_wld_raw.tgt[rf].abs    = hrp::to_Pose3(m_htrf.data); }
-        if (m_htlfIn.isNew())   { m_htlfIn.read();  wbms->hp_wld_raw.tgt[lf].abs    = hrp::to_Pose3(m_htlf.data); }
-        if (m_htrhIn.isNew())   { m_htrhIn.read();  wbms->hp_wld_raw.tgt[rh].abs    = hrp::to_Pose3(m_htrh.data);}
-        if (m_htlhIn.isNew())   { m_htlhIn.read();  wbms->hp_wld_raw.tgt[lh].abs    = hrp::to_Pose3(m_htlh.data);}
-        if (m_htheadIn.isNew()) { m_htheadIn.read();wbms->hp_wld_raw.tgt[head].abs  = hrp::to_Pose3(m_hthead.data);}
-        if (m_htzmpIn.isNew())  { m_htzmpIn.read(); wbms->hp_wld_raw.tgt[zmp].abs.p = hrp::to_Vector3(m_htzmp.data); }
+        if (m_masterTgtPosesIn["lleg"]->isNew()){ m_masterTgtPosesIn["lleg"]->read(); wbms->hp_wld_raw.tgt[lf].abs   = hrp::to_Pose3(m_masterTgtPoses["lleg"].data); }
+        if (m_masterTgtPosesIn["rleg"]->isNew()){ m_masterTgtPosesIn["rleg"]->read(); wbms->hp_wld_raw.tgt[rf].abs   = hrp::to_Pose3(m_masterTgtPoses["rleg"].data); }
+        if (m_masterTgtPosesIn["larm"]->isNew()){ m_masterTgtPosesIn["larm"]->read(); wbms->hp_wld_raw.tgt[lh].abs   = hrp::to_Pose3(m_masterTgtPoses["larm"].data); }
+        if (m_masterTgtPosesIn["rarm"]->isNew()){ m_masterTgtPosesIn["rarm"]->read(); wbms->hp_wld_raw.tgt[rh].abs   = hrp::to_Pose3(m_masterTgtPoses["rarm"].data);}
+        if (m_masterTgtPosesIn["com" ]->isNew()){ m_masterTgtPosesIn["com" ]->read(); wbms->hp_wld_raw.tgt[com].abs  = hrp::to_Pose3(m_masterTgtPoses["com" ].data);}
+        if (m_masterTgtPosesIn["head"]->isNew()){ m_masterTgtPosesIn["head"]->read(); wbms->hp_wld_raw.tgt[head].abs = hrp::to_Pose3(m_masterTgtPoses["head"].data);}
         if (m_actCPIn.isNew())  { m_actCPIn.read(); rel_act_cp = hrp::to_Vector3(m_actCP.data);}
         if (m_actZMPIn.isNew())  { m_actZMPIn.read(); rel_act_zmp = hrp::to_Vector3(m_actZMP.data);}
     }
@@ -439,7 +453,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 void WholeBodyMasterSlave::processTransition(){
     switch(mode.now()){
 
-        case MODE_SYNC_TO_WBMS:
+        case MODE_SYNC_TO_HC:
             if(mode.pre() == MODE_IDLE){ double tmp = 1.0; t_ip->setGoal(&tmp, 3.0, true); }
             if (!t_ip->isEmpty() ){
                 t_ip->get(&output_ratio, true);
@@ -832,7 +846,7 @@ void WholeBodyMasterSlave::smoothingJointAngles(hrp::BodyPtr _robot, hrp::BodyPt
 bool WholeBodyMasterSlave::startWholeBodyMasterSlave(){
     if(mode.now() == MODE_IDLE){
         RTCOUT << "startWholeBodyMasterSlave" << std::endl;
-        mode.setNextMode(MODE_SYNC_TO_WBMS);
+        mode.setNextMode(MODE_SYNC_TO_HC);
         return true;
     }else{
         RTCOUT << "Invalid context to startWholeBodyMasterSlave" << std::endl;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index f47af2a5227..a3b64e6ee7c 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -21,7 +21,7 @@
 
 //#define USE_DEBUG_PORT
 
-enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE};
+enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_HC, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE};
 
 class ControlMode{
     private:
@@ -31,10 +31,10 @@ class ControlMode{
         ~ControlMode(){}
         bool setNextMode(const mode_enum _request){
             switch(_request){
-                case MODE_SYNC_TO_WBMS:
-                    if(current == MODE_IDLE){ next = MODE_SYNC_TO_WBMS; return true; }else{ return false; }
+                case MODE_SYNC_TO_HC:
+                    if(current == MODE_IDLE){ next = MODE_SYNC_TO_HC; return true; }else{ return false; }
                 case MODE_WBMS:
-                    if(current == MODE_SYNC_TO_WBMS || current == MODE_PAUSE ){ next = MODE_WBMS; return true; }else{ return false; }
+                    if(current == MODE_SYNC_TO_HC || current == MODE_PAUSE ){ next = MODE_WBMS; return true; }else{ return false; }
                 case MODE_PAUSE:
                     if(current == MODE_WBMS){ next = MODE_PAUSE; return true; }else{ return false; }
                 case MODE_SYNC_TO_IDLE:
@@ -48,10 +48,19 @@ class ControlMode{
         void update(){ previous = current; current = next; }
         mode_enum now(){ return current; }
         mode_enum pre(){ return previous; }
-        bool isRunning(){ return (current==MODE_SYNC_TO_WBMS) || (current==MODE_WBMS) || (current==MODE_PAUSE) || (current==MODE_SYNC_TO_IDLE) ;}
-        bool isInitialize(){ return (previous==MODE_IDLE) && (current==MODE_SYNC_TO_WBMS) ;}
+        bool isRunning(){ return (current==MODE_SYNC_TO_HC) || (current==MODE_WBMS) || (current==MODE_PAUSE) || (current==MODE_SYNC_TO_IDLE) ;}
+        bool isInitialize(){ return (previous==MODE_IDLE) && (current==MODE_SYNC_TO_HC) ;}
 };
 
+namespace hrp{
+    inline std::vector<std::string> to_string_vector (const OpenHRP::WholeBodyMasterSlaveService::StrSequence& in) {
+        std::vector<std::string> ret(in.length()); for(int i=0; i<in.length(); i++){ ret[i] = in[i]; } return ret;
+    }
+    inline OpenHRP::WholeBodyMasterSlaveService::StrSequence    to_StrSequence  (const std::vector<std::string>& in){
+        OpenHRP::WholeBodyMasterSlaveService::StrSequence ret; ret.length(in.size()); for(int i=0; i<in.size(); i++){ ret[i] = in[i].c_str(); } return ret;
+    }
+}
+
 class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
     public:
         WholeBodyMasterSlave(RTC::Manager* manager);
@@ -80,24 +89,44 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         RTC::TimedDoubleSeq m_optionalData;
         RTC::InPort<RTC::TimedDoubleSeq> m_optionalDataIn;
 
-        RTC::TimedPose3D m_htcom;
-        RTC::InPort<RTC::TimedPose3D> m_htcomIn;
-        RTC::TimedPose3D m_htrf;
-        RTC::InPort<RTC::TimedPose3D> m_htrfIn;
-        RTC::TimedPose3D m_htlf;
-        RTC::InPort<RTC::TimedPose3D> m_htlfIn;
-        RTC::TimedPose3D m_htrh;
-        RTC::InPort<RTC::TimedPose3D> m_htrhIn;
-        RTC::TimedPose3D m_htlh;
-        RTC::InPort<RTC::TimedPose3D> m_htlhIn;
-        RTC::TimedPose3D m_hthead;
-        RTC::InPort<RTC::TimedPose3D> m_htheadIn;
-        RTC::TimedPoint3D m_htzmp;
-        RTC::InPort<RTC::TimedPoint3D> m_htzmpIn;
-        OpenHRP::TimedWrench m_htrfw;
-        RTC::InPort<OpenHRP::TimedWrench> m_htrfwIn;
-        OpenHRP::TimedWrench m_htlfw;
-        RTC::InPort<OpenHRP::TimedWrench> m_htlfwIn;
+
+
+        std::map<std::string, RTC::TimedPose3D> m_masterTgtPoses;
+        typedef boost::shared_ptr<RTC::InPort<RTC::TimedPose3D> > ITP3_Ptr;
+        std::map<std::string, ITP3_Ptr> m_masterTgtPosesIn;
+
+        RTC::TimedDoubleSeq m_exData;
+        RTC::InPort<RTC::TimedDoubleSeq> m_exDataIn;
+        RTC::TimedStringSeq m_exDataIndex;
+        RTC::InPort<RTC::TimedStringSeq> m_exDataIndexIn;
+
+        std::map<std::string, RTC::TimedDoubleSeq> m_slaveEEWrenches;
+        typedef boost::shared_ptr<RTC::OutPort<RTC::TimedDoubleSeq> > OTDS_Ptr;
+        std::map<std::string, OTDS_Ptr> m_slaveEEWrenchesOut;
+
+
+//        RTC::TimedPose3D m_htcom;
+//        RTC::InPort<RTC::TimedPose3D> m_htcomIn;
+//        RTC::TimedPose3D m_htrf;
+//        RTC::InPort<RTC::TimedPose3D> m_htrfIn;
+//        RTC::TimedPose3D m_htlf;
+//        RTC::InPort<RTC::TimedPose3D> m_htlfIn;
+//        RTC::TimedPose3D m_htrh;
+//        RTC::InPort<RTC::TimedPose3D> m_htrhIn;
+//        RTC::TimedPose3D m_htlh;
+//        RTC::InPort<RTC::TimedPose3D> m_htlhIn;
+//        RTC::TimedPose3D m_hthead;
+//        RTC::InPort<RTC::TimedPose3D> m_htheadIn;
+//        RTC::TimedPoint3D m_htzmp;
+//        RTC::InPort<RTC::TimedPoint3D> m_htzmpIn;
+//        OpenHRP::TimedWrench m_htrfw;
+//        RTC::InPort<OpenHRP::TimedWrench> m_htrfwIn;
+//        OpenHRP::TimedWrench m_htlfw;
+//        RTC::InPort<OpenHRP::TimedWrench> m_htlfwIn;
+
+
+
+
         RTC::TimedPoint3D m_actCP;
         RTC::InPort<RTC::TimedPoint3D> m_actCPIn;
         RTC::TimedPoint3D m_actZMP;
@@ -196,6 +225,9 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         struct timespec startT, endT;
         std::string time_report_str;
 
+        std::vector<std::string> ee_names;
+        std::vector<std::string> tgt_names;
+
         RTC::ReturnCode_t setupEEIKConstraintFromConf(std::map<std::string, IKConstraint>& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop);
         void solveFullbodyIK(const hrp::Pose3& com_ref, const hrp::Pose3& rf_ref, const hrp::Pose3& lf_ref, const hrp::Pose3& rh_ref, const hrp::Pose3& lh_ref, const hrp::Pose3& head_ref);
         void processTransition();
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 87f1d200276..1535dac07ec 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -1,9 +1,9 @@
 #ifndef WBMS_CORE_H
 #define WBMS_CORE_H
 
-#include "interpolator.h"
 #include <hrpUtil/Eigen4d.h>
 #include <hrpCollision/DistFuncs.h>
+#include "../SequencePlayer/interpolator.h"
 #include "../ImpedanceController/JointPathEx.h"
 #include "../AutoBalancer/FullbodyInverseKinematicsSolver.h"
 // geometry

From fe1a4206df5ddb7881299b8d4e6aad406f7e5984 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 29 Aug 2019 10:51:10 +0900
Subject: [PATCH 165/217] update

---
 python/hrpsys_config.py                       |   6 +-
 rtc/HapticController/HapticController.cpp     |   7 +-
 .../WholeBodyMasterSlave.cpp                  | 188 +++++-------------
 .../WholeBodyMasterSlave.h                    |  76 +------
 4 files changed, 67 insertions(+), 210 deletions(-)

diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index 1220e812e0a..3e876f959e8 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -396,6 +396,10 @@ def connectComps(self):
             connectPorts(self.sh.port("basePosOut"), self.wbms.port("basePosIn"))
             connectPorts(self.sh.port("baseRpyOut"), self.wbms.port("baseRpyIn"))
             connectPorts(self.sh.port("optionalDataOut"), self.wbms.port("optionalData"))
+            connectPorts(self.rmfo.port("off_lfsensor"), self.wbms.port("local_lleg_wrench_in"))
+            connectPorts(self.rmfo.port("off_rfsensor"), self.wbms.port("local_rleg_wrench_in"))
+            connectPorts(self.rmfo.port("off_lhsensor"), self.wbms.port("local_larm_wrench_in"))
+            connectPorts(self.rmfo.port("off_rhsensor"), self.wbms.port("local_rarm_wrench_in"))
 
         # connection for st
         if rtm.findPort(self.rh.ref, "lfsensor") and rtm.findPort(
@@ -443,7 +447,7 @@ def connectComps(self):
                 
         # connection for hc
         if self.hc:
-            connectPorts(self.sh.port("q"),     self.hc.port("qRef"))
+            connectPorts(self.abc.port("q"),     self.hc.port("qRef"))
             connectPorts(self.rh.port("q"),  self.hc.port("qAct"))
             connectPorts(self.rh.port("dq"), self.hc.port("dqAct"))
             connectPorts(self.hc.port("tau"), self.rh.port("tauRef"))
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index e84279ca9f9..80fceed7920 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -83,7 +83,7 @@ RTC::ReturnCode_t HapticController::onInitialize(){
 //    }
 
     for ( int i=0; i<ee_names.size(); i++) {
-        std::string n = "slave_"+ee_names[i]+"_wrench";
+        std::string n = "slave_"+ee_names[i]+"_wrench_in";
         m_feedbackWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_feedbackWrenches[ee_names[i]]));
         registerInPort(n.c_str(), *m_feedbackWrenchesIn[ee_names[i]]);
         m_feedbackWrenches[ee_names[i]].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
@@ -94,7 +94,7 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     tgt_names.push_back("com");
     tgt_names.push_back("head");
     for ( int i=0; i<tgt_names.size(); i++) {
-        std::string n = "master_"+tgt_names[i]+"_pose";
+        std::string n = "master_"+tgt_names[i]+"_pose_out";
         m_eePosesOut[tgt_names[i]] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
         registerOutPort(n.c_str(), *m_eePosesOut[tgt_names[i]]);
         RTCOUT << " registerOutPort " << n << std::endl;
@@ -437,7 +437,8 @@ void HapticController::calcTorque (){
         }else{
             m_masterTgtPoses[tgt_names[i]].data = hrp::to_Pose3D(ee_ikc_map[ee_names[i]].getCurrentTargetPose(m_robot));//四肢揃ってないと危険
         }
-        m_masterTgtPoses[tgt_names[i]].tm = m_qRef.tm;
+//        m_masterTgtPoses[tgt_names[i]].tm = m_qRef.tm;
+        m_masterTgtPoses[tgt_names[i]].tm = m_qAct.tm;
         m_eePosesOut[tgt_names[i]]->write();
     }
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 37774b5b965..dfcac28bab4 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -19,45 +19,24 @@ static const char* WholeBodyMasterSlave_spec[] = {
 };
 
 WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager),
-        m_qRefIn("qRef", m_qRef),// from sh
-        m_zmpIn("zmpIn", m_zmp),
-        m_basePosIn("basePosIn", m_basePos),
-        m_baseRpyIn("baseRpyIn", m_baseRpy),
-        m_optionalDataIn("optionalData", m_optionalData),
-        m_qOut("q", m_qRef),// to abc
-        m_zmpOut("zmpOut", m_zmp),
-        m_basePosOut("basePosOut", m_basePos),
-        m_baseRpyOut("baseRpyOut", m_baseRpy),
-        m_optionalDataOut("optionalDataOut", m_optionalData),
-        m_actCPIn("actCapturePoint", m_actCP),
-        m_actZMPIn("zmp", m_actZMP),
-        m_exDataIn("exData", m_exData),
-        m_exDataIndexIn("exDataIndex", m_exDataIndex),
-#ifdef USE_DEBUG_PORT
-        m_htcom_dbgOut("htcom_dbgOut", m_htcom_dbg),// to ros bridge
-        m_htrf_dbgOut("htrf_dbgOut", m_htrf_dbg),
-        m_htlf_dbgOut("htlf_dbgOut", m_htlf_dbg),
-        m_htrh_dbgOut("htrh_dbgOut", m_htrh_dbg),
-        m_htlh_dbgOut("htlh_dbgOut", m_htlh_dbg),
-        m_hthead_dbgOut("hthead_dbgOut", m_hthead_dbg),
-        m_htzmp_dbgOut("htzmp_dbgOut", m_htzmp_dbg),
-        m_htrfw_dbgOut("htrfw_dbgOut", m_htrfw_dbg),
-        m_htlfw_dbgOut("htlfw_dbgOut", m_htlfw_dbg),
-        m_rpcom_dbgOut("rpcom_dbgOut", m_rpcom_dbg),
-        m_rprf_dbgOut("rprf_dbgOut", m_rprf_dbg),
-        m_rplf_dbgOut("rplf_dbgOut", m_rplf_dbg),
-        m_rprh_dbgOut("rprh_dbgOut", m_rprh_dbg),
-        m_rplh_dbgOut("rplh_dbgOut", m_rplh_dbg),
-        m_rphead_dbgOut("rphead_dbgOut", m_rphead_dbg),
-        m_rpzmp_dbgOut("rpzmp_dbgOut", m_rpzmp_dbg),
-        m_rpdcp_dbgOut("rpdcp_dbgOut", m_rpdcp_dbg),
-        m_rpacp_dbgOut("rpacp_dbgOut", m_rpacp_dbg),
-        m_invdyn_dbgOut("invdyn_dbgOut", m_invdyn_dbg),
-#endif
-        m_WholeBodyMasterSlaveServicePort("WholeBodyMasterSlaveService"),
-        m_AutoBalancerServicePort("AutoBalancerService"),
-        m_StabilizerServicePort("StabilizerService"),
-        m_debugLevel(0)
+    m_qRefIn("qRef", m_qRef),// from sh
+    m_zmpIn("zmpIn", m_zmp),
+    m_basePosIn("basePosIn", m_basePos),
+    m_baseRpyIn("baseRpyIn", m_baseRpy),
+    m_optionalDataIn("optionalData", m_optionalData),
+    m_qOut("q", m_qRef),// to abc
+    m_zmpOut("zmpOut", m_zmp),
+    m_basePosOut("basePosOut", m_basePos),
+    m_baseRpyOut("baseRpyOut", m_baseRpy),
+    m_optionalDataOut("optionalDataOut", m_optionalData),
+    m_actCPIn("actCapturePoint", m_actCP),
+    m_actZMPIn("zmp", m_actZMP),
+    m_exDataIn("exData", m_exData),
+    m_exDataIndexIn("exDataIndex", m_exDataIndex),
+    m_WholeBodyMasterSlaveServicePort("WholeBodyMasterSlaveService"),
+    m_AutoBalancerServicePort("AutoBalancerService"),
+    m_StabilizerServicePort("StabilizerService"),
+    m_debugLevel(0)
 {
     m_service0.wholebodymasterslave(this);
 }
@@ -81,32 +60,9 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     addInPort("zmp", m_actZMPIn);
     addInPort("exData", m_exDataIn);
     addInPort("exDataIndex", m_exDataIndexIn);
-#ifdef USE_DEBUG_PORT
-    addOutPort("htcom_dbgOut", m_htcom_dbgOut);// to ros bridge
-    addOutPort("htrf_dbgOut", m_htrf_dbgOut);
-    addOutPort("htlf_dbgOut", m_htlf_dbgOut);
-    addOutPort("htrh_dbgOut", m_htrh_dbgOut);
-    addOutPort("htlh_dbgOut", m_htlh_dbgOut);
-    addOutPort("hthead_dbgOut", m_hthead_dbgOut);
-    addOutPort("htzmp_dbgOut", m_htzmp_dbgOut);
-    addOutPort("htrfw_dbgOut", m_htrfw_dbgOut);
-    addOutPort("htlfw_dbgOut", m_htlfw_dbgOut);
-    addOutPort("rpcom_dbgOut", m_rpcom_dbgOut);
-    addOutPort("rprf_dbgOut", m_rprf_dbgOut);
-    addOutPort("rplf_dbgOut", m_rplf_dbgOut);
-    addOutPort("rprh_dbgOut", m_rprh_dbgOut);
-    addOutPort("rplh_dbgOut", m_rplh_dbgOut);
-    addOutPort("rphead_dbgOut", m_rphead_dbgOut);
-    addOutPort("rpzmp_dbgOut", m_rpzmp_dbgOut);
-    addOutPort("rpdcp_dbgOut", m_rpdcp_dbgOut);
-    addOutPort("rpacp_dbgOut", m_rpacp_dbgOut);
-    addOutPort("invdyn_dbgOut", m_invdyn_dbgOut);
-#endif
     m_WholeBodyMasterSlaveServicePort.registerProvider("service0", "WholeBodyMasterSlaveService", m_service0);
     addPort(m_WholeBodyMasterSlaveServicePort);
 
-
-
     m_AutoBalancerServicePort.registerConsumer("service0","AutoBalancerService", m_AutoBalancerServiceConsumer);
     m_StabilizerServicePort.registerConsumer("service0","StabilizerService", m_StabilizerServiceConsumer);
     addPort(m_AutoBalancerServicePort);
@@ -192,18 +148,31 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     lt.fill(0);
     rt.fill(0);
 
+    to_enum["lleg"] = lf;
+    to_enum["rleg"] = rf;
+    to_enum["larm"] = lh;
+    to_enum["rarm"] = rh;
+    to_enum["com"] =  com;
+    to_enum["head"] = head;
+
     for ( int i=0; i<ee_names.size(); i++) {
-        std::string n = "slave_"+ee_names[i]+"_wrench";
-        m_slaveEEWrenchesOut[tgt_names[i]] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[tgt_names[i]]));
-        registerOutPort(n.c_str(), *m_slaveEEWrenchesOut[tgt_names[i]]);
+        std::string n = "slave_"+ee_names[i]+"_wrench_out";
+        m_slaveEEWrenchesOut[ee_names[i]] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee_names[i]]));
+        registerOutPort(n.c_str(), *m_slaveEEWrenchesOut[ee_names[i]]);
         RTCOUT << " registerOutPort " << n << std::endl;
     }
+    for ( int i=0; i<ee_names.size(); i++) {
+        std::string n = "local_"+ee_names[i]+"_wrench_in";
+        m_localEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_localEEWrenches[ee_names[i]]));
+        registerInPort(n.c_str(), *m_localEEWrenchesIn[ee_names[i]]);
+        RTCOUT << " registerInPort " << n << std::endl;
+    }
 
     tgt_names = ee_names;
     tgt_names.push_back("com");
     tgt_names.push_back("head");
     for ( int i=0; i<tgt_names.size(); i++) {
-        std::string n = "master_"+tgt_names[i]+"_pose";
+        std::string n = "master_"+tgt_names[i]+"_pose_in";
         m_masterTgtPosesIn[tgt_names[i]] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
         registerInPort(n.c_str(), *m_masterTgtPosesIn[tgt_names[i]]);
         RTCOUT << " registerInPort " << n << std::endl;
@@ -254,8 +223,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if (m_baseRpyIn.isNew()) { m_baseRpyIn.read(); }
     if (m_zmpIn.isNew()) { m_zmpIn.read(); }
     if (m_optionalDataIn.isNew()) { m_optionalDataIn.read(); }
-//    if (m_htrfwIn.isNew())  { m_htrfwIn.read(); wbms->hp_wld_raw.tgt[rf].w      = hrp::to_dvector6(m_htrfw.data); }
-//    if (m_htlfwIn.isNew())  { m_htlfwIn.read(); wbms->hp_wld_raw.tgt[lf].w      = hrp::to_dvector6(m_htlfw.data); }
+    for(int i=0; i<ee_names.size(); i++){ if (m_localEEWrenchesIn[ee_names[i]]->isNew()){ m_localEEWrenchesIn[ee_names[i]]->read(); } }
 
     // button func
     hrp::dvector ex_data;
@@ -293,13 +261,14 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 //        is_blocking_continuous_hits = false;
 //    }
 
+
     if( mode.now() != MODE_PAUSE ){ // stop updating input when MODE_PAUSE
-        if (m_masterTgtPosesIn["lleg"]->isNew()){ m_masterTgtPosesIn["lleg"]->read(); wbms->hp_wld_raw.tgt[lf].abs   = hrp::to_Pose3(m_masterTgtPoses["lleg"].data); }
-        if (m_masterTgtPosesIn["rleg"]->isNew()){ m_masterTgtPosesIn["rleg"]->read(); wbms->hp_wld_raw.tgt[rf].abs   = hrp::to_Pose3(m_masterTgtPoses["rleg"].data); }
-        if (m_masterTgtPosesIn["larm"]->isNew()){ m_masterTgtPosesIn["larm"]->read(); wbms->hp_wld_raw.tgt[lh].abs   = hrp::to_Pose3(m_masterTgtPoses["larm"].data); }
-        if (m_masterTgtPosesIn["rarm"]->isNew()){ m_masterTgtPosesIn["rarm"]->read(); wbms->hp_wld_raw.tgt[rh].abs   = hrp::to_Pose3(m_masterTgtPoses["rarm"].data);}
-        if (m_masterTgtPosesIn["com" ]->isNew()){ m_masterTgtPosesIn["com" ]->read(); wbms->hp_wld_raw.tgt[com].abs  = hrp::to_Pose3(m_masterTgtPoses["com" ].data);}
-        if (m_masterTgtPosesIn["head"]->isNew()){ m_masterTgtPosesIn["head"]->read(); wbms->hp_wld_raw.tgt[head].abs = hrp::to_Pose3(m_masterTgtPoses["head"].data);}
+        for(int i=0; i<tgt_names.size(); i++){
+            if (m_masterTgtPosesIn[tgt_names[i]]->isNew()){
+                m_masterTgtPosesIn[tgt_names[i]]->read();
+                wbms->hp_wld_raw.tgt[to_enum[tgt_names[i]]].abs = hrp::to_Pose3(m_masterTgtPoses[tgt_names[i]].data);
+            }
+        }
         if (m_actCPIn.isNew())  { m_actCPIn.read(); rel_act_cp = hrp::to_Vector3(m_actCP.data);}
         if (m_actZMPIn.isNew())  { m_actZMPIn.read(); rel_act_zmp = hrp::to_Vector3(m_actZMP.data);}
     }
@@ -374,75 +343,18 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     }
 
     // write
+    for(int i=0; i<ee_names.size(); i++){
+        const hrp::Vector3 f_wld = ee_ikc_map[ee_names[i]].getCurrentTargetRot(m_robot_vsafe) * hrp::to_dvector(m_localEEWrenches[ee_names[i]].data).head(3);
+        const hrp::Vector3 t_wld = ee_ikc_map[ee_names[i]].getCurrentTargetRot(m_robot_vsafe) * hrp::to_dvector(m_localEEWrenches[ee_names[i]].data).tail(3);
+        m_slaveEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq( (hrp::dvector6()<<f_wld,t_wld).finished());
+        m_slaveEEWrenches[ee_names[i]].tm = m_qRef.tm;
+        m_slaveEEWrenchesOut[ee_names[i]]->write();
+    }
     m_qOut.write();
     m_basePosOut.write();
     m_baseRpyOut.write();
     m_zmpOut.write();
     m_optionalDataOut.write();
-
-#ifdef USE_DEBUG_PORT
-    // dbg plot
-    m_htcom_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(wbms->hp_plot.tgt[com].abs, m_htcom_dbg.data);
-    m_htcom_dbgOut.write();
-    m_htrf_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(wbms->hp_plot.tgt[rf].abs, m_htrf_dbg.data);
-    m_htrf_dbgOut.write();
-    m_htlf_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(wbms->hp_plot.tgt[lf].abs, m_htlf_dbg.data);
-    m_htlf_dbgOut.write();m_AutoBalancerService
-    m_htrh_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(wbms->hp_plot.tgt[rh].abs, m_htrh_dbg.data);
-    m_htrh_dbgOut.write();
-    m_htlh_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(wbms->hp_plot.tgt[lh].abs, m_htlh_dbg.data);
-    m_htlh_dbgOut.write();mode
-    m_hthead_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(wbms->hp_plot.tgt[head].abs, m_hthead_dbg.data);
-    m_hthead_dbgOut.write();
-    m_htzmp_dbg.tm = m_qRef.tm;
-    WBMSCore::Vector3ToPoint3D(wbms->rp_ref_out.tgt[zmp].abs.p,m_rpzmp_dbg.data);
-    m_htzmp_dbgOut.write();
-    m_htrfw_dbg.tm = m_qRef.tm;
-    m_htrfw_dbg.data.length(6);
-    WBMSCore::Vector6ToDoubleSeq(wbms->hp_plot.tgt[rf].w, m_htrfw_dbg.data);
-    m_htrfw_dbgOut.write();
-    m_htlfw_dbg.tm = m_qRef.tm;
-    m_htlfw_dbg.data.length(6);
-    WBMSCore::Vector6ToDoubleSeq(wbms->hp_plot.tgt[lf].w, m_htlfw_dbg.data);
-    m_htlfw_dbgOut.write();
-    m_rpcom_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(wbms->rp_ref_out.tgt[com].abs, m_rpcom_dbg.data);
-    m_rpcom_dbgOut.write();
-    m_rprf_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(wbms->rp_ref_out.tgt[rf].abs, m_rprf_dbg.data);
-    m_rprf_dbgOut.write();
-    m_rplf_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(wbms->rp_ref_out.tgt[lf].abs, m_rplf_dbg.data);
-    m_rplf_dbgOut.write();
-    m_rprh_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(wbms->rp_ref_out.tgt[rh].abs, m_rprh_dbg.data);
-    m_rprh_dbgOut.write();
-    m_rplh_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(wbms->rp_ref_out.tgt[lh].abs, m_rplh_dbg.data);
-    m_rplh_dbgOut.write();
-    m_rphead_dbg.tm = m_qRef.tm;
-    WBMSCore::WBMSPose3DToPose3D(wbms->rp_ref_out.tgt[head].abs, m_rphead_dbg.data);
-    m_rphead_dbgOut.write();
-    m_rpzmp_dbg.tm = m_qRef.tm;
-    WBMSCore::Vector3ToPoint3D(wbms->rp_ref_out.tgt[zmp].abs.p, m_rpzmp_dbg.data);
-    m_rpzmp_dbgOut.write();
-    m_rpdcp_dbg.tm = m_qRef.tm;
-    WBMSCore::Vector3ToPoint3D(wbms->cp_dec,m_rpdcp_dbg.data);
-    m_rpdcp_dbgOut.write();
-    m_rpacp_dbg.tm = m_qRef.tm;
-    WBMSCore::Vector3ToPoint3D(wbms->cp_acc,m_rpacp_dbg.data);
-    m_rpacp_dbgOut.write();
-    m_invdyn_dbg.tm = m_qRef.tm;
-    m_invdyn_dbg.data.length(6);
-    WBMSCore::Vector6ToDoubleSeq(wbms->invdyn_ft,m_invdyn_dbg.data);
-    m_invdyn_dbgOut.write();
-#endif
     addTimeReport("OutPort");
     if(DEBUGP) RTCOUT << time_report_str << std::endl;
     loop ++;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index a3b64e6ee7c..12c5e584ff8 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -89,88 +89,27 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         RTC::TimedDoubleSeq m_optionalData;
         RTC::InPort<RTC::TimedDoubleSeq> m_optionalDataIn;
 
-
-
         std::map<std::string, RTC::TimedPose3D> m_masterTgtPoses;
         typedef boost::shared_ptr<RTC::InPort<RTC::TimedPose3D> > ITP3_Ptr;
         std::map<std::string, ITP3_Ptr> m_masterTgtPosesIn;
 
-        RTC::TimedDoubleSeq m_exData;
-        RTC::InPort<RTC::TimedDoubleSeq> m_exDataIn;
-        RTC::TimedStringSeq m_exDataIndex;
-        RTC::InPort<RTC::TimedStringSeq> m_exDataIndexIn;
-
         std::map<std::string, RTC::TimedDoubleSeq> m_slaveEEWrenches;
         typedef boost::shared_ptr<RTC::OutPort<RTC::TimedDoubleSeq> > OTDS_Ptr;
         std::map<std::string, OTDS_Ptr> m_slaveEEWrenchesOut;
 
+        std::map<std::string, RTC::TimedDoubleSeq> m_localEEWrenches;
+        typedef boost::shared_ptr<RTC::InPort<RTC::TimedDoubleSeq> > ITDS_Ptr;
+        std::map<std::string, ITDS_Ptr> m_localEEWrenchesIn;
 
-//        RTC::TimedPose3D m_htcom;
-//        RTC::InPort<RTC::TimedPose3D> m_htcomIn;
-//        RTC::TimedPose3D m_htrf;
-//        RTC::InPort<RTC::TimedPose3D> m_htrfIn;
-//        RTC::TimedPose3D m_htlf;
-//        RTC::InPort<RTC::TimedPose3D> m_htlfIn;
-//        RTC::TimedPose3D m_htrh;
-//        RTC::InPort<RTC::TimedPose3D> m_htrhIn;
-//        RTC::TimedPose3D m_htlh;
-//        RTC::InPort<RTC::TimedPose3D> m_htlhIn;
-//        RTC::TimedPose3D m_hthead;
-//        RTC::InPort<RTC::TimedPose3D> m_htheadIn;
-//        RTC::TimedPoint3D m_htzmp;
-//        RTC::InPort<RTC::TimedPoint3D> m_htzmpIn;
-//        OpenHRP::TimedWrench m_htrfw;
-//        RTC::InPort<OpenHRP::TimedWrench> m_htrfwIn;
-//        OpenHRP::TimedWrench m_htlfw;
-//        RTC::InPort<OpenHRP::TimedWrench> m_htlfwIn;
-
-
-
+        RTC::TimedDoubleSeq m_exData;
+        RTC::InPort<RTC::TimedDoubleSeq> m_exDataIn;
+        RTC::TimedStringSeq m_exDataIndex;
+        RTC::InPort<RTC::TimedStringSeq> m_exDataIndexIn;
 
         RTC::TimedPoint3D m_actCP;
         RTC::InPort<RTC::TimedPoint3D> m_actCPIn;
         RTC::TimedPoint3D m_actZMP;
         RTC::InPort<RTC::TimedPoint3D> m_actZMPIn;
-#ifdef USE_DEBUG_PORT
-    TimedPose3D m_htcom_dbg;
-    RTC::OutPort<RTC::TimedPose3D> m_htcom_dbgOut;
-    TimedPose3D m_htrf_dbg;
-    RTC::OutPort<RTC::TimedPose3D> m_htrf_dbgOut;
-    TimedPose3D m_htlf_dbg;
-    RTC::OutPort<RTC::TimedPose3D> m_htlf_dbgOut;
-    TimedPose3D m_htrh_dbg;
-    RTC::OutPort<RTC::TimedPose3D> m_htrh_dbgOut;
-    TimedPose3D m_htlh_dbg;
-    RTC::OutPort<RTC::TimedPose3D> m_htlh_dbgOut;
-    TimedPose3D m_hthead_dbg;
-    RTC::OutPort<RTC::TimedPose3D> m_hthead_dbgOut;
-    TimedPoint3D m_htzmp_dbg;
-    RTC::OutPort<RTC::TimedPoint3D> m_htzmp_dbgOut;
-    TimedDoubleSeq m_htrfw_dbg;
-    RTC::OutPort<RTC::TimedDoubleSeq> m_htrfw_dbgOut;
-    TimedDoubleSeq m_htlfw_dbg;
-    RTC::OutPort<RTC::TimedDoubleSeq> m_htlfw_dbgOut;
-    TimedPose3D m_rpcom_dbg;
-    RTC::OutPort<RTC::TimedPose3D> m_rpcom_dbgOut;
-    TimedPose3D m_rprf_dbg;
-    RTC::OutPort<RTC::TimedPose3D> m_rprf_dbgOut;
-    TimedPose3D m_rplf_dbg;
-    RTC::OutPort<RTC::TimedPose3D> m_rplf_dbgOut;
-    TimedPose3D m_rprh_dbg;
-    RTC::OutPort<RTC::TimedPose3D> m_rprh_dbgOut;
-    TimedPose3D m_rplh_dbg;
-    RTC::OutPort<RTC::TimedPose3D> m_rplh_dbgOut;
-    TimedPose3D m_rphead_dbg;
-    RTC::OutPort<RTC::TimedPose3D> m_rphead_dbgOut;
-    TimedPoint3D m_rpzmp_dbg;
-    RTC::OutPort<RTC::TimedPoint3D> m_rpzmp_dbgOut;
-    TimedPoint3D m_rpdcp_dbg;
-    RTC::OutPort<RTC::TimedPoint3D> m_rpdcp_dbgOut;
-    TimedPoint3D m_rpacp_dbg;
-    RTC::OutPort<RTC::TimedPoint3D> m_rpacp_dbgOut;
-    TimedDoubleSeq m_invdyn_dbg;
-    RTC::OutPort<RTC::TimedDoubleSeq> m_invdyn_dbgOut;
-#endif
 
         RTC::OutPort<RTC::TimedDoubleSeq> m_qOut;
         RTC::OutPort<RTC::TimedPoint3D> m_zmpOut;
@@ -181,6 +120,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
 
         WholeBodyMasterSlaveService_impl m_service0;
 
+        std::map<std::string, int> to_enum;
 
         RTC::CorbaPort m_AutoBalancerServicePort;
         RTC::CorbaPort m_StabilizerServicePort;

From 8552219499d1a5d9149d270b178c4ccb0a1da3d4 Mon Sep 17 00:00:00 2001
From: JAXONRED <jaxonred@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 3 Sep 2019 14:25:36 +0900
Subject: [PATCH 166/217] tune for ee contact

---
 python/hrpsys_config.py                       |  1 +
 rtc/AutoBalancer/MyUtils.h                    |  2 +-
 .../WholeBodyMasterSlave.cpp                  | 25 ++++++++++++++-----
 .../WholeBodyMasterSlave.h                    |  4 +++
 4 files changed, 25 insertions(+), 7 deletions(-)

diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index 3e876f959e8..eac4136ffd1 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -392,6 +392,7 @@ def connectComps(self):
         # connection for wbms
         if self.wbms:
             connectPorts(self.sh.port("qOut"), self.wbms.port("qRef"))
+            connectPorts(self.rh.port("q"), self.wbms.port("qAct"))
             connectPorts(self.sh.port("zmpOut"), self.wbms.port("zmpIn"))
             connectPorts(self.sh.port("basePosOut"), self.wbms.port("basePosIn"))
             connectPorts(self.sh.port("baseRpyOut"), self.wbms.port("baseRpyIn"))
diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index 3784ccf4756..00c976cede4 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -66,7 +66,7 @@ namespace hrp{
     inline RTC::TimedDoubleSeq::_data_seq   to_DoubleSeq    (const hrp::dvector& in)    { RTC::TimedDoubleSeq::_data_seq out; out.length(in.size()); hrp::dvector::Map(out.get_buffer(), in.size()) = in; return out; }
 
     inline hrp::dvector getQAll         (const hrp::BodyPtr _robot){ hrp::dvector tmp(_robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ tmp(i) = _robot->joint(i)->q; } return tmp; }
-    inline void         setQAll         (hrp::BodyPtr       _robot, const hrp::dvector& in){ assert(in.size() <= _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = in(i); } }
+    inline void         setQAll         (hrp::BodyPtr       _robot, const hrp::dvector& in){ assert(in.size() <= _robot->numJoints()); for(int i=0;i<in.size();i++){ _robot->joint(i)->q = in(i); } }
     inline hrp::dvector getRobotStateVec(const hrp::BodyPtr _robot){ return (hrp::dvector(_robot->numJoints()+6) << getQAll(_robot), _robot->rootLink()->p, hrp::rpyFromRot(_robot->rootLink()->R)).finished(); }
     inline void         setRobotStateVec(hrp::BodyPtr       _robot, const hrp::dvector& _q_bpos_brpy){ assert(_q_bpos_brpy.size() == _robot->numJoints()+6); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q_bpos_brpy(i); }; _robot->rootLink()->p = _q_bpos_brpy.tail(6).head(3); _robot->rootLink()->R = hrp::rotFromRpy(_q_bpos_brpy.tail(6).tail(3)); }
     inline void         setRobotStateVec(hrp::BodyPtr       _robot, const hrp::dvector& _q, const hrp::Vector3& _bpos, const hrp::Vector3& _brpy){ assert(_q.size() == _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q(i); }; _robot->rootLink()->p = _bpos; _robot->rootLink()->R = hrp::rotFromRpy(_brpy); }
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index dfcac28bab4..354d5b8ba5c 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -20,6 +20,7 @@ static const char* WholeBodyMasterSlave_spec[] = {
 
 WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager),
     m_qRefIn("qRef", m_qRef),// from sh
+    m_qActIn("qAct", m_qAct),
     m_zmpIn("zmpIn", m_zmp),
     m_basePosIn("basePosIn", m_basePos),
     m_baseRpyIn("baseRpyIn", m_baseRpy),
@@ -47,6 +48,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     RTCOUT << "onInitialize()" << std::endl;
     bindParameter("debugLevel", m_debugLevel, "0");
     addInPort("qRef", m_qRefIn);// from sh
+    addInPort("qAct", m_qActIn);
     addInPort("zmpIn", m_zmpIn);
     addInPort("basePosIn", m_basePosIn);
     addInPort("baseRpyIn", m_baseRpyIn);
@@ -68,8 +70,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     addPort(m_AutoBalancerServicePort);
     addPort(m_StabilizerServicePort);
 
-
-
     RTC::Properties& prop =  getProperties();
     coil::stringTo(m_dt, prop["dt"].c_str());
     RTC::Manager& rtcManager = RTC::Manager::instance();
@@ -84,6 +84,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
         RTCOUT << "failed to load model[" << prop["model"] << "]" << std::endl;
         return RTC::RTC_ERROR;
     }
+    m_robot_act = hrp::BodyPtr(new hrp::Body(*robot_for_ik)); //copy
     m_robot_vsafe = hrp::BodyPtr(new hrp::Body(*robot_for_ik)); //copy
     RTCOUT << "setup robot model finished" << std::endl;
 
@@ -219,6 +220,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     time_report_str.clear();
     clock_gettime(CLOCK_REALTIME, &startT);
     if (m_qRefIn.isNew()) { m_qRefIn.read(); }
+    if (m_qActIn.isNew()) { m_qActIn.read(); }
     if (m_basePosIn.isNew()) { m_basePosIn.read(); }
     if (m_baseRpyIn.isNew()) { m_baseRpyIn.read(); }
     if (m_zmpIn.isNew()) { m_zmpIn.read(); }
@@ -342,14 +344,25 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         wbms->baselinkpose.R = fik->m_robot->rootLink()->R;
     }
 
-    // write
-    for(int i=0; i<ee_names.size(); i++){
-        const hrp::Vector3 f_wld = ee_ikc_map[ee_names[i]].getCurrentTargetRot(m_robot_vsafe) * hrp::to_dvector(m_localEEWrenches[ee_names[i]].data).head(3);
-        const hrp::Vector3 t_wld = ee_ikc_map[ee_names[i]].getCurrentTargetRot(m_robot_vsafe) * hrp::to_dvector(m_localEEWrenches[ee_names[i]].data).tail(3);
+
+    std::map<std::string, std::string> to_sname;
+    to_sname["lleg"] = "lfsensor";
+    to_sname["rleg"] = "rfsensor";
+    to_sname["larm"] = "lhsensor";
+    to_sname["rarm"] = "rhsensor";
+    hrp::setQAll(m_robot_act, hrp::to_dvector(m_qAct.data));
+    m_robot_act->calcForwardKinematics();
+    for(int i=0;i<ee_names.size();i++){
+        hrp::ForceSensor* sensor = m_robot_act->sensor<hrp::ForceSensor>(to_sname[ee_names[i]]);
+        hrp::Matrix33 sensorR_wld = sensor->link->R * sensor->localR;
+        hrp::Matrix33 sensorR_from_base = m_robot_act->rootLink()->R.transpose() * sensorR_wld;
+        const hrp::Vector3 f_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee_names[i]].data).head(3);
+        const hrp::Vector3 t_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee_names[i]].data).tail(3);
         m_slaveEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq( (hrp::dvector6()<<f_wld,t_wld).finished());
         m_slaveEEWrenches[ee_names[i]].tm = m_qRef.tm;
         m_slaveEEWrenchesOut[ee_names[i]]->write();
     }
+    // write
     m_qOut.write();
     m_basePosOut.write();
     m_baseRpyOut.write();
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 12c5e584ff8..d05c1d09d0b 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -12,6 +12,7 @@
 #include <rtm/idl/ExtendedDataTypesSkel.h>
 #include <rtm/CorbaNaming.h>
 #include <hrpModel/ModelLoaderUtil.h>
+#include <hrpModel/Sensor.h>
 #include <time.h>
 
 #include "WholeBodyMasterSlaveService_impl.h"
@@ -80,6 +81,8 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
     protected:
         RTC::TimedDoubleSeq m_qRef;
         RTC::InPort<RTC::TimedDoubleSeq> m_qRefIn;
+        RTC::TimedDoubleSeq m_qAct;
+        RTC::InPort<RTC::TimedDoubleSeq> m_qActIn;
         RTC::TimedPoint3D m_basePos;
         RTC::InPort<RTC::TimedPoint3D> m_basePosIn;
         RTC::TimedOrientation3D m_baseRpy;
@@ -134,6 +137,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         unsigned int m_debugLevel;
         int optionalDataLength;
         bool is_legged_robot;
+        hrp::BodyPtr m_robot_act; // actual
         hrp::BodyPtr m_robot_vsafe; // joint trajectory safe
         typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;
         fikPtr fik;

From 8c8f808188ea4aeac35c6f8aaba9bae5b900dcfb Mon Sep 17 00:00:00 2001
From: leus <leus@example.com>
Date: Tue, 3 Sep 2019 14:27:02 +0900
Subject: [PATCH 167/217] tune for ee contact

---
 idl/HapticControllerService.idl           |  11 +-
 python/hrpsys_config.py                   |   5 +-
 rtc/HapticController/HapticController.cpp | 187 ++++++++++++++--------
 rtc/HapticController/HapticController.h   |  31 +++-
 rtc/TorqueFilter/IIRFilter.cpp            |  22 +--
 5 files changed, 174 insertions(+), 82 deletions(-)

diff --git a/idl/HapticControllerService.idl b/idl/HapticControllerService.idl
index cdaec3709aa..62252beaed1 100644
--- a/idl/HapticControllerService.idl
+++ b/idl/HapticControllerService.idl
@@ -8,8 +8,10 @@ module OpenHRP
   
   interface HapticControllerService
   {
-    typedef sequence<double, 3> DblSequence3;
+    //typedef sequence<double, 2> DblSequence2;
+    //typedef sequence<double, 3> DblSequence3;
     //typedef sequence<double, 4> DblSequence4;
+    typedef double DblArray2[2];
     typedef double DblArray3[3];
     typedef double DblArray4[4];
     typedef sequence<string> StrSequence;
@@ -20,6 +22,13 @@ module OpenHRP
         double force_feedback_ratio;
         double dqAct_filter_cutoff_hz;
         double wrench_filter_cutoff_hz;
+        double ee_vel_filter_cutoff_hz;
+        double q_friction_coeff;
+        DblArray2 ee_friction_coeff;
+        double wrench_hpf_cutoff_hz;
+        double wrench_lpf_cutoff_hz;
+        double wrench_hpf_gain;
+        double wrench_lpf_gain;
     };
     boolean startHapticController();
     boolean stopHapticController();
diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index 3e876f959e8..ae648e2a4f1 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -447,7 +447,7 @@ def connectComps(self):
                 
         # connection for hc
         if self.hc:
-            connectPorts(self.abc.port("q"),     self.hc.port("qRef"))
+            # connectPorts(self.abc.port("q"),     self.hc.port("qRef"))
             connectPorts(self.rh.port("q"),  self.hc.port("qAct"))
             connectPorts(self.rh.port("dq"), self.hc.port("dqAct"))
             connectPorts(self.hc.port("tau"), self.rh.port("tauRef"))
@@ -927,6 +927,9 @@ def setupLogger(self, maxLength=4000):
             self.connectLoggerPort(self.sh, 'zmpOut')
         if self.ic != None:
             self.connectLoggerPort(self.ic, 'q')
+        if self.hc != None:
+            self.connectLoggerPort(self.hc, 'tau')
+            self.connectLoggerPort(self.hc, 'debugData')
         if self.wbms != None:
             self.connectLoggerPort(self.wbms, 'q')
             self.connectLoggerPort(self.wbms, 'basePosOut')
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index 80fceed7920..4204acf749c 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -25,6 +25,7 @@ HapticController::HapticController(RTC::Manager* manager) : RTC::DataFlowCompone
     m_qOut("q", m_qRef),
     m_tauOut("tau", m_tau),
     m_teleopOdomOut("teleopOdom", m_teleopOdom),
+    m_debugDataOut("debugData", m_debugData),
     m_HapticControllerServicePort("HapticControllerService"),
     m_debugLevel(0)
 {
@@ -42,6 +43,7 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     addOutPort("q", m_qOut);
     addOutPort("tau", m_tauOut);
     addOutPort("teleopOdom", m_teleopOdomOut);
+    addOutPort("debugData", m_debugDataOut);
     m_HapticControllerServicePort.registerProvider("service0", "HapticControllerService", m_service0);
     addPort(m_HapticControllerServicePort);
 
@@ -73,9 +75,18 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
     dqAct_filter.reset(0);
     for ( int i=0; i<ee_names.size(); i++) {
+        ee_vel_filter[ee_names[i]] = BiquadIIRFilterVec2(6);
+        ee_vel_filter[ee_names[i]].setParameter(hcp.ee_vel_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        ee_vel_filter[ee_names[i]].reset(0);
         wrench_filter[ee_names[i]] = BiquadIIRFilterVec2(6);
         wrench_filter[ee_names[i]].setParameter(hcp.wrench_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
         wrench_filter[ee_names[i]].reset(0);
+        wrench_lpf_for_hpf[ee_names[i]] = BiquadIIRFilterVec2(6);
+        wrench_lpf_for_hpf[ee_names[i]].setParameter(hcp.wrench_hpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        wrench_lpf_for_hpf[ee_names[i]].reset(0);
+        wrench_lpf[ee_names[i]] = BiquadIIRFilterVec2(6);
+        wrench_lpf[ee_names[i]].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        wrench_lpf[ee_names[i]].reset(0);
     }
 //    std::vector<std::string> force_sensor_names;
 //    for (int i=0; i<m_robot->numSensors(hrp::Sensor::FORCE); i++) {
@@ -139,7 +150,7 @@ RTC::ReturnCode_t HapticController::setupEEIKConstraintFromConf(std::map<std::st
 }
 
 RTC::ReturnCode_t HapticController::onExecute(RTC::UniqueId ec_id){
-    if(DEBUGP_ONCE) RTCOUT << "onExecute(" << ec_id << ")" << std::endl;
+    if(DEBUGP_ONCE) RTCOUT << "onExecute(" << ec_id << ") dt = " << m_dt << std::endl;
     if (m_qRefIn.isNew())   { m_qRefIn.read(); }
     if (m_qActIn.isNew())   { m_qActIn.read(); }
     if (m_dqActIn.isNew())  { m_dqActIn.read(); }
@@ -200,6 +211,13 @@ void HapticController::calcTorque (){
 
     dqAct_filtered = dqAct_filter.passFilter(hrp::to_dvector(m_dqAct.data));
 
+//    static hrp::dvector q_old = hrp::to_dvector(m_qAct.data);
+//    hrp::dvector dq_tmp = (hrp::to_dvector(m_qAct.data) - q_old) / m_dt;
+//    dqAct_filtered = dqAct_filter.passFilter(dq_tmp);
+
+
+
+
     for(int i=0;i<m_robot->numJoints();i++)m_robot->joint(i)->q = m_qAct.data[i];
 
     if(!idsb.is_initialized){
@@ -352,43 +370,55 @@ void HapticController::calcTorque (){
         ee_pose[i].R = m_robot->link(ee_ikc_map[ee_names[i]].target_link_name)->R;
     }
     static std::vector<hrp::Pose3> ee_pose_old(ee_pose);
-    std::vector<hrp::dvector6> wrench_ee_wld;
-    if(wrench_ee_wld.size() != ee_names.size()){
-        wrench_ee_wld.resize(ee_names.size());
-        for(int i=0;i<wrench_ee_wld.size();i++){ wrench_ee_wld[i] = hrp::dvector6::Zero(); }
-    }
-
     for (int i=0; i<ee_names.size(); i++){
-        wrench_filtered[ee_names[i]] = wrench_filter[ee_names[i]].passFilter( hrp::to_dvector(m_feedbackWrenches[ee_names[i]].data) );
-        hrp::Vector3 ee_pos_vel = (ee_pose[i].p - ee_pose_old[i].p) / m_dt;
-        hrp::Vector3 ee_rot_vel = ee_pose_old[i].R * hrp::omegaFromRot(ee_pose_old[i].R.transpose() * ee_pose[i].R) / m_dt;
-        hrp::Vector3 f = wrench_filtered[ee_names[i]].head(3) * hcp.force_feedback_ratio;
-        hrp::Vector3 t = wrench_filtered[ee_names[i]].tail(3) * hcp.force_feedback_ratio;
-
-        if(f.norm() > 1e-6 ){ // avoid zero devide
-            f = f * fabs(1 - 1 * f.normalized().dot(ee_pos_vel) );
-        }
-        if(t.norm() > 1e-6){ // avoid zero devide
-            t = t * fabs(1 - 0.1 * t.normalized().dot(ee_rot_vel) );
+        const hrp::dvector6 wrench_raw = hrp::to_dvector(m_feedbackWrenches[ee_names[i]].data);
+        wrench_filtered[ee_names[i]] = wrench_filter[ee_names[i]].passFilter( wrench_raw );
+        const hrp::Vector3 ee_pos_vel = (ee_pose[i].p - ee_pose_old[i].p) / m_dt;
+        const hrp::Vector3 ee_rot_vel = ee_pose_old[i].R * hrp::omegaFromRot(ee_pose_old[i].R.transpose() * ee_pose[i].R) / m_dt;
+        ee_vel_filtered[ee_names[i]] = ee_vel_filter[ee_names[i]].passFilter((hrp::dvector6()<<ee_pos_vel,ee_rot_vel).finished());
+
+        const hrp::dvector6 w_hpf = wrench_raw - wrench_lpf_for_hpf[ee_names[i]].passFilter( wrench_raw );
+        const hrp::dvector6 w_lpf = wrench_lpf[ee_names[i]].passFilter( wrench_raw );
+        wrench_shaped[ee_names[i]] = w_hpf * hcp.wrench_hpf_gain + w_lpf * hcp.wrench_lpf_gain;
+
+        wrench_used[ee_names[i]] = wrench_shaped[ee_names[i]];
+        LIMIT_NORM(wrench_used[ee_names[i]].head(3),50);
+        LIMIT_NORM(wrench_used[ee_names[i]].tail(3),5);
+
+
+//        if(f.norm() > 1e-6 ){ // avoid zero devide
+//            f = f * fabs(1 - 1 * f.normalized().dot(ee_pos_vel) );
+//        }
+//        if(t.norm() > 1e-6){ // avoid zero devide
+//            t = t * fabs(1 - 0.1 * t.normalized().dot(ee_rot_vel) );
+//        }
+
+//        hrp::Vector3 friction_f = - hcp.ee_friction_coeff(0) * ee_vel_filtered[ee_names[i]].head(3);
+//        hrp::Vector3 friction_t = - hcp.ee_friction_coeff(1) * ee_vel_filtered[ee_names[i]].tail(3);
+
+        if(ee_names[i] == "rarm"){
+            m_debugData.data = hrp::to_DoubleSeq((hrp::dvector(6+6+6+6)<<wrench_raw,wrench_shaped[ee_names[i]],w_hpf,w_lpf).finished());
+            m_debugData.tm = m_qRef.tm;
+            m_debugDataOut.write();
+            if(loop%1000==0){
+                dbgv(ee_vel_filtered["rarm"]);
+                dbgv(hcp.ee_friction_coeff);
+                dbg(hcp.ee_vel_filter_cutoff_hz);
+            }
         }
-        LIMIT_NORM(f,100);
-        LIMIT_NORM(t,10);
-
-        wrench_ee_wld[i] = (hrp::dvector6() << f,t).finished();
 
         hrp::JointPath jp(m_robot->rootLink(), m_robot->link(ee_ikc_map[ee_names[i]].target_link_name));
         hrp::dmatrix J_base_to_ee;
         jp.calcJacobian(J_base_to_ee, ee_ikc_map[ee_names[i]].localPos);
-        hrp::dvector tq_from_feedbackWrench = J_base_to_ee.transpose() * wrench_ee_wld[i];
+        hrp::dvector tq_from_feedbackWrench = J_base_to_ee.transpose() * wrench_used[ee_names[i]];
         for (int j = 0; j < jp.numJoints(); j++) jp.joint(j)->u += tq_from_feedbackWrench(j);
-        if(loop%500==0)dbgv(wrench_ee_wld[i]);
+        if(loop%1000==0)dbgv(wrench_used[ee_names[i]]);
     }
     ee_pose_old = ee_pose;
 
 
     {
         // soft joint limit
-        // joint limit
         m_robot->link("RARM_JOINT1")->ulimit = deg2rad(-10);
         m_robot->link("LARM_JOINT1")->llimit = deg2rad(10);
         for(int i=0;i<m_robot->numJoints();i++){
@@ -396,34 +426,45 @@ void HapticController::calcTorque (){
             const double soft_llimit = m_robot->joint(i)->llimit + deg2rad(15);
             double soft_jlimit_tq = 0;
             if(m_qAct.data[i] > soft_ulimit){
-                soft_jlimit_tq = 100 * (soft_ulimit - m_qAct.data[i]) + 1 * (0 - dqAct_filtered(i));
+                soft_jlimit_tq = 100 * (soft_ulimit - m_qAct.data[i]) + 0 * (0 - dqAct_filtered(i));
             }
             if(m_qAct.data[i] < soft_llimit){
-                soft_jlimit_tq = 100 * (soft_llimit - m_qAct.data[i]) + 1 * (0 - dqAct_filtered(i));
+                soft_jlimit_tq = 100 * (soft_llimit - m_qAct.data[i]) + 0 * (0 - dqAct_filtered(i));
             }
             m_robot->joint(i)->u += soft_jlimit_tq;
         }
-        if(loop%500==0)dbgv(hrp::getUAll(m_robot));
     }
 
 
 
-    {
-        // real robot joint friction damper
-        hrp::dvector friction_tq = -3 * dqAct_filtered;
-        for (int i=0; i<m_robot->numJoints(); i++){
-         LIMIT_MINMAX(friction_tq(i), -10, 10);
-         m_robot->joint(i)->u += friction_tq(i);
-        }
-        //         if(loop%500==0)dbgv(friction_tq);
-    }
+//    m_debugData.data = hrp::to_DoubleSeq((hrp::dvector(27*2)<<hrp::to_dvector(m_dqAct.data),dqAct_filtered).finished());
+//    m_debugData.tm = m_qRef.tm;
+//    m_debugDataOut.write();
+
+
+//    {
+//        // real robot joint friction damper
+//        const hrp::dvector q_friction_coeff_const = (hrp::dvector(m_robot->numJoints()) << 0,0,0,0,0,0, 0,0,0,0,0,0, 0, 3,3,2,2,1,0.5,0.5, 3,3,2,2,1,0.5,0.5).finished();
+//        hrp::dvector friction_tq = - q_friction_coeff_const.array() * dqAct_filtered.array();
+////        hrp::dvector friction_tq = - hcp.q_friction_coeff * dqAct_filtered;
+//        for (int i=0; i<m_robot->numJoints(); i++){
+//         LIMIT_MINMAX(friction_tq(i), -5, 5);
+//         m_robot->joint(i)->u += friction_tq(i);
+//        }
+//        if(loop%1000==0)dbgv(friction_tq);
+//    }
+
 
 
+    const hrp::dvector max_torque = (hrp::dvector(m_robot->numJoints()) << 30,80,80,80,30,30, 30,80,80,80,30,30, 80, 30,30,25,25,20,10,10, 30,30,25,25,20,10,10).finished();
+
     for(int i=0;i<m_robot->numJoints();i++){
-        LIMIT_MINMAX(m_robot->joint(i)->u, -80,80);
-        double tlimit = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst;////?????
+        LIMIT_MINMAX(m_robot->joint(i)->u, -max_torque(i), max_torque(i));
+//        double tlimit = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst;////?????
     }
 
+    if(loop%1000==0)dbgv(hrp::getUAll(m_robot));
+
     updateInvDynStateBuffer(idsb);
 
 
@@ -484,7 +525,7 @@ void HapticController::processTransition(){
 ////    hrp::Vector3 init_foot_mid_coord = (fik_in->getEndEffectorPos("rleg") + fik_in->getEndEffectorPos("lleg")) / 2;
 ////    if( fabs((double)init_foot_mid_coord(Z)) > 1e-5 ){
 ////        basePos_heightChecked(Z) -= init_foot_mid_coord(Z);
-////        init_foot_mid_coord(Z) = 0;
+////        init_foot_mid_coord(Z) = 0;dqAct_filter
 ////        std::cerr<<"["<<m_profile.instance_name<<"] Input basePos height is invalid. Auto modify "<<m_basePos.data.z<<" -> "<<basePos_heightChecked(Z)<<endl;
 ////    }
 //
@@ -553,37 +594,53 @@ bool HapticController::stopHapticController(){
 
 bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param){
     RTCOUT << "setHapticControllerParam" << std::endl;
-
-    hcp.gravity_compensation_ratio = i_param.gravity_compensation_ratio;
-    hcp.dqAct_filter_cutoff_hz = i_param.dqAct_filter_cutoff_hz;
-    hcp.wrench_filter_cutoff_hz = i_param.wrench_filter_cutoff_hz;
-    hcp.force_feedback_ratio = i_param.force_feedback_ratio;
-//    wbms->wp.auto_swing_foot_landing_threshold  = i_param.auto_swing_foot_landing_threshold;
-//    wbms->wp.human_to_robot_ratio               = i_param.human_to_robot_ratio;
-//    wbms->wp.set_com_height_fix                 = i_param.set_com_height_fix;
-//    wbms->wp.set_com_height_fix_val             = i_param.set_com_height_fix_val;
-//    wbms->wp.swing_foot_height_offset           = i_param.swing_foot_height_offset;
-//    wbms->wp.upper_body_rmc_ratio               = i_param.upper_body_rmc_ratio;
-//    if(mode.now() == MODE_IDLE){
-//        wbms->wp.use_head                       = i_param.use_head;
-//        wbms->wp.use_upper                      = i_param.use_upper;
-//        wbms->wp.use_lower                      = i_param.use_lower;
-//        wbms->wp.use_joints                     = hrp::to_string_vector(i_param.use_joints);
-//        wbms->wp.use_targets                    = hrp::to_string_vector(i_param.use_targets);
-//    }else{
-//      RTCOUT << "use_head, use_upper, use_lower can be changed in MODE_IDLE" << std::endl;
-//    }
+    hcp.gravity_compensation_ratio  = i_param.gravity_compensation_ratio;
+    hcp.dqAct_filter_cutoff_hz      = i_param.dqAct_filter_cutoff_hz;
+    {
+        LIMIT_MAX(hcp.dqAct_filter_cutoff_hz, 1/m_dt/2);
+        dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        dqAct_filter.reset(0);
+    }
+    hcp.force_feedback_ratio        = i_param.force_feedback_ratio;
+    hcp.q_friction_coeff            = i_param.q_friction_coeff;
+    for(int i=0;i<hcp.ee_friction_coeff.size(); i++){ hcp.ee_friction_coeff(i) = i_param.ee_friction_coeff[i]; }
+    hcp.wrench_filter_cutoff_hz     = i_param.wrench_filter_cutoff_hz;
+    hcp.ee_vel_filter_cutoff_hz     = i_param.ee_vel_filter_cutoff_hz;
+    hcp.wrench_hpf_cutoff_hz        = i_param.wrench_hpf_cutoff_hz;
+    hcp.wrench_lpf_cutoff_hz        = i_param.wrench_lpf_cutoff_hz;
+    for(int i=0;i<ee_names.size();i++){
+        LIMIT_MAX(hcp.wrench_filter_cutoff_hz, 1/m_dt/2);
+        LIMIT_MAX(hcp.ee_vel_filter_cutoff_hz, 1/m_dt/2);
+        LIMIT_MAX(hcp.wrench_hpf_cutoff_hz, 1/m_dt/2);
+        LIMIT_MAX(hcp.wrench_lpf_cutoff_hz, 1/m_dt/2);
+        wrench_filter[ee_names[i]].setParameter(hcp.wrench_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        ee_vel_filter[ee_names[i]].setParameter(hcp.ee_vel_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        wrench_lpf_for_hpf[ee_names[i]].setParameter(hcp.wrench_hpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        wrench_lpf[ee_names[i]].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        wrench_filter[ee_names[i]].reset(0);
+        ee_vel_filter[ee_names[i]].reset(0);
+        wrench_lpf_for_hpf[ee_names[i]].reset(0);
+        wrench_lpf[ee_names[i]].reset(0);
+    }
+    hcp.wrench_hpf_gain             = i_param.wrench_hpf_gain;
+    hcp.wrench_lpf_gain             = i_param.wrench_lpf_gain;
     return true;
 }
 
 
 bool HapticController::getParams(OpenHRP::HapticControllerService::HapticControllerParam& i_param){
     RTCOUT << "getHapticControllerParam" << std::endl;
-    i_param.gravity_compensation_ratio = hcp.gravity_compensation_ratio;
-    i_param.dqAct_filter_cutoff_hz = hcp.dqAct_filter_cutoff_hz;
-    i_param.wrench_filter_cutoff_hz = hcp.wrench_filter_cutoff_hz;
-    i_param.force_feedback_ratio = hcp.force_feedback_ratio;
-//    i_param.use_targets                         = hrp::to_StrSequence(wbms->wp.use_targets);
+    i_param.gravity_compensation_ratio  = hcp.gravity_compensation_ratio;
+    i_param.dqAct_filter_cutoff_hz      = hcp.dqAct_filter_cutoff_hz;
+    i_param.wrench_filter_cutoff_hz     = hcp.wrench_filter_cutoff_hz;
+    i_param.ee_vel_filter_cutoff_hz     = hcp.ee_vel_filter_cutoff_hz;
+    i_param.force_feedback_ratio        = hcp.force_feedback_ratio;
+    i_param.q_friction_coeff            = hcp.q_friction_coeff;
+    for(int i=0;i<hcp.ee_friction_coeff.size(); i++){ i_param.ee_friction_coeff[i] = hcp.ee_friction_coeff(i); }
+    i_param.wrench_hpf_cutoff_hz        = hcp.wrench_hpf_cutoff_hz;
+    i_param.wrench_lpf_cutoff_hz        = hcp.wrench_lpf_cutoff_hz;
+    i_param.wrench_hpf_gain             = hcp.wrench_hpf_gain;
+    i_param.wrench_lpf_gain             = hcp.wrench_lpf_gain;
     return true;
 }
 
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index 210ae30cd82..7dd8e16dd27 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -91,6 +91,8 @@ class HapticController : public RTC::DataFlowComponentBase{
         std::map<std::string, RTC::TimedPose3D> m_masterTgtPoses;
         typedef boost::shared_ptr<RTC::OutPort<RTC::TimedPose3D> > OTP3_Ptr;
         std::map<std::string, OTP3_Ptr> m_eePosesOut;
+        RTC::TimedDoubleSeq m_debugData;
+        RTC::OutPort<RTC::TimedDoubleSeq> m_debugDataOut;
 
 //        RTC::TimedDoubleSeq m_dq_filtered_dbg;
 //        RTC::OutPort<RTC::TimedDoubleSeq> m_dq_filtered_dbgOut;
@@ -115,12 +117,26 @@ class HapticController : public RTC::DataFlowComponentBase{
                 double gravity_compensation_ratio;
                 double force_feedback_ratio;
                 double dqAct_filter_cutoff_hz;
+                double ee_vel_filter_cutoff_hz;
                 double wrench_filter_cutoff_hz;
+                double q_friction_coeff;
+                hrp::Vector2 ee_friction_coeff;
+                double wrench_hpf_cutoff_hz;
+                double wrench_lpf_cutoff_hz;
+                double wrench_hpf_gain;
+                double wrench_lpf_gain;
             HCParams(){
-                gravity_compensation_ratio = 0.9;
-                force_feedback_ratio = 0.5;
-                dqAct_filter_cutoff_hz = 10;
-                wrench_filter_cutoff_hz = 10;
+                gravity_compensation_ratio  = 0.9;
+                force_feedback_ratio        = 0.2;
+                dqAct_filter_cutoff_hz      = 100;// 10以下で確実に位相遅れによる振動
+                ee_vel_filter_cutoff_hz     = 100;// 10以下で確実に位相遅れによる振動
+                wrench_filter_cutoff_hz     = 100;
+                q_friction_coeff            = 1.0;
+                ee_friction_coeff           << 1, 0.1;
+                wrench_hpf_cutoff_hz        = 20;
+                wrench_lpf_cutoff_hz        = 0.3;
+                wrench_hpf_gain             = 1;
+                wrench_lpf_gain             = 0.2;
             }
         } hcp;
 
@@ -132,9 +148,16 @@ class HapticController : public RTC::DataFlowComponentBase{
         hrp::InvDynStateBuffer idsb;
         BiquadIIRFilterVec2 dqAct_filter;
         hrp::dvector dqAct_filtered;
+        std::map<std::string, BiquadIIRFilterVec2> ee_vel_filter;
+        std::map<std::string, hrp::dvector6> ee_vel_filtered;
         std::map<std::string, BiquadIIRFilterVec2> wrench_filter;
         std::map<std::string, hrp::dvector6> wrench_filtered;
 
+        std::map<std::string, BiquadIIRFilterVec2> wrench_lpf_for_hpf;
+        std::map<std::string, BiquadIIRFilterVec2> wrench_lpf;
+        std::map<std::string, hrp::dvector6> wrench_shaped;
+        std::map<std::string, hrp::dvector6> wrench_used;
+
         std::map<std::string, IKConstraint> ee_ikc_map;
         ControlMode mode;
 
diff --git a/rtc/TorqueFilter/IIRFilter.cpp b/rtc/TorqueFilter/IIRFilter.cpp
index 886e29cb68a..7e313040cbc 100644
--- a/rtc/TorqueFilter/IIRFilter.cpp
+++ b/rtc/TorqueFilter/IIRFilter.cpp
@@ -67,17 +67,17 @@ bool IIRFilter::setParameter(int dim, std::vector<double>& A, std::vector<double
 }
 
 bool IIRFilter::setParameterAsBiquad(const double f_cutoff, const double Q, const double hz){
-  std::vector<double> fb_coeffs(3), ff_coeffs(3);
-  const double omega = 2 * 3.14159265 * f_cutoff / hz;
-  const double alpha = std::sin(omega) / (2 * Q);
-  const double denom = 1 + alpha;
-  fb_coeffs[0] = 1;
-  fb_coeffs[1] = -2 * std::cos(omega) / denom;
-  fb_coeffs[2] = (1 - alpha) / denom;
-  ff_coeffs[0] = (1 - std::cos(omega)) / 2 / denom;
-  ff_coeffs[1] = (1 - std::cos(omega)) / denom;
-  ff_coeffs[2] = (1 - std::cos(omega)) / 2 / denom;
-  return this->setParameter(2, fb_coeffs, ff_coeffs);
+    std::vector<double> fb_coeffs(3), ff_coeffs(3);
+    const double omega = 2 * 3.14159265 * f_cutoff / hz;
+    const double alpha = std::sin(omega) / (2 * Q);
+    const double denom = 1 + alpha;
+    fb_coeffs[0] = 1;
+    fb_coeffs[1] = -2 * std::cos(omega) / denom;
+    fb_coeffs[2] = (1 - alpha) / denom;
+    ff_coeffs[0] = (1 - std::cos(omega)) / 2 / denom;
+    ff_coeffs[1] = (1 - std::cos(omega)) / denom;
+    ff_coeffs[2] = (1 - std::cos(omega)) / 2 / denom;
+    return this->setParameter(2, fb_coeffs, ff_coeffs);
 };
 
 

From d3dc8df8a90165f9ab5faa68c44aab539413990b Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 4 Sep 2019 15:34:30 +0900
Subject: [PATCH 168/217] add qref mode

---
 idl/HapticControllerService.idl               |  19 +-
 python/hrpsys_config.py                       |   6 +-
 rtc/AutoBalancer/MyUtils.h                    |   2 +-
 rtc/HapticController/HapticController.cpp     | 325 ++++++++----------
 rtc/HapticController/HapticController.h       |  83 ++---
 .../HapticControllerService_impl.cpp          |  17 +-
 6 files changed, 217 insertions(+), 235 deletions(-)

diff --git a/idl/HapticControllerService.idl b/idl/HapticControllerService.idl
index 62252beaed1..249de476111 100644
--- a/idl/HapticControllerService.idl
+++ b/idl/HapticControllerService.idl
@@ -8,9 +8,9 @@ module OpenHRP
   
   interface HapticControllerService
   {
-    //typedef sequence<double, 2> DblSequence2;
-    //typedef sequence<double, 3> DblSequence3;
-    //typedef sequence<double, 4> DblSequence4;
+    typedef sequence<double, 2> DblSequence2;
+    typedef sequence<double, 3> DblSequence3;
+    typedef sequence<double, 4> DblSequence4;
     typedef double DblArray2[2];
     typedef double DblArray3[3];
     typedef double DblArray4[4];
@@ -18,17 +18,22 @@ module OpenHRP
 
     struct HapticControllerParam
     {
-        double gravity_compensation_ratio;
-        double force_feedback_ratio;
         double dqAct_filter_cutoff_hz;
-        double wrench_filter_cutoff_hz;
         double ee_vel_filter_cutoff_hz;
+        double floor_height_from_base;
+        double foot_min_distance;
+        double force_feedback_ratio;
+        double gravity_compensation_ratio;
         double q_friction_coeff;
-        DblArray2 ee_friction_coeff;
+        double q_ref_output_ratio_goal;
         double wrench_hpf_cutoff_hz;
         double wrench_lpf_cutoff_hz;
         double wrench_hpf_gain;
         double wrench_lpf_gain;
+        DblSequence2 ee_pos_rot_friction_coeff;
+        DblSequence2 floor_pd_gain;
+        DblSequence2 foot_horizontal_pd_gain;
+        DblSequence2 q_ref_pd_gain;
     };
     boolean startHapticController();
     boolean stopHapticController();
diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index c45e19b4c7a..ace7347409b 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -448,9 +448,9 @@ def connectComps(self):
                 
         # connection for hc
         if self.hc:
-            # connectPorts(self.abc.port("q"),     self.hc.port("qRef"))
-            connectPorts(self.rh.port("q"),  self.hc.port("qAct"))
-            connectPorts(self.rh.port("dq"), self.hc.port("dqAct"))
+            connectPorts(self.sh.port("qOut"),   self.hc.port("qRef"))
+            connectPorts(self.rh.port("q"),   self.hc.port("qAct"))
+            connectPorts(self.rh.port("dq"),  self.hc.port("dqAct"))
             connectPorts(self.hc.port("tau"), self.rh.port("tauRef"))
 #            connectPorts(self.hc.port("q"), self.rh.port("??????"))
 #            connectPorts(self.hc.port("teleopOdom"), self.rh.port("??????"))
diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index 00c976cede4..a7fa754bcc8 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -119,7 +119,7 @@ class BiquadIIRFilterVec2{
         BiquadIIRFilterVec2(const int len){resize(len);}
         void resize(const int len){filters.resize(len); ans.resize(len);}
         ~BiquadIIRFilterVec2(){}
-        void setParameter(const hrp::dvector& fc_in, const double& HZ, const double& Q = 0.5){ for(int i=0;i<filters.size();i++){ filters[i].setParameterAsBiquad((double)fc_in(i), Q, HZ); } }
+        void setParameter(const hrp::dvector& fc_in, const double& HZ, const double& Q = 0.5){ for(int i=0;i<filters.size();i++){ filters[i].setParameterAsBiquad(std::min(fc_in(i),HZ/2), Q, HZ); } }
         void setParameter(const double& fc_in, const double& HZ, const double& Q = 0.5){ setParameter(hrp::dvector::Constant(filters.size(), fc_in), HZ, Q); }//overload
         hrp::dvector passFilter(const hrp::dvector& input){
             for(int i=0;i<filters.size();i++){
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index 4204acf749c..d7db74620d4 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -65,10 +65,15 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     setupEEIKConstraintFromConf(ee_ikc_map, m_robot, prop);
     RTCOUT << "setupEEIKConstraintFromConf finished" << std::endl;
 
-    output_ratio = 0.0;
+    const double tmp = 0;
     t_ip = new interpolator(1, m_dt, interpolator::LINEAR, 1);
     t_ip->clear();
-    t_ip->set(&output_ratio);
+    t_ip->set(&tmp);
+    t_ip->get(&output_ratio, false);
+    q_ref_ip = new interpolator(1, m_dt, interpolator::LINEAR, 1);
+    q_ref_ip->clear();
+    q_ref_ip->set(&tmp);
+    q_ref_ip->get(&q_ref_output_ratio, false);
     RTCOUT << "setup interpolator finished" << std::endl;
 
     dqAct_filter = BiquadIIRFilterVec2(m_robot->numJoints());
@@ -78,9 +83,6 @@ RTC::ReturnCode_t HapticController::onInitialize(){
         ee_vel_filter[ee_names[i]] = BiquadIIRFilterVec2(6);
         ee_vel_filter[ee_names[i]].setParameter(hcp.ee_vel_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
         ee_vel_filter[ee_names[i]].reset(0);
-        wrench_filter[ee_names[i]] = BiquadIIRFilterVec2(6);
-        wrench_filter[ee_names[i]].setParameter(hcp.wrench_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        wrench_filter[ee_names[i]].reset(0);
         wrench_lpf_for_hpf[ee_names[i]] = BiquadIIRFilterVec2(6);
         wrench_lpf_for_hpf[ee_names[i]].setParameter(hcp.wrench_hpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
         wrench_lpf_for_hpf[ee_names[i]].reset(0);
@@ -88,10 +90,6 @@ RTC::ReturnCode_t HapticController::onInitialize(){
         wrench_lpf[ee_names[i]].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
         wrench_lpf[ee_names[i]].reset(0);
     }
-//    std::vector<std::string> force_sensor_names;
-//    for (int i=0; i<m_robot->numSensors(hrp::Sensor::FORCE); i++) {
-//        force_sensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
-//    }
 
     for ( int i=0; i<ee_names.size(); i++) {
         std::string n = "slave_"+ee_names[i]+"_wrench_in";
@@ -111,7 +109,6 @@ RTC::ReturnCode_t HapticController::onInitialize(){
         RTCOUT << " registerOutPort " << n << std::endl;
     }
 
-
     m_tau.data = hrp::to_DoubleSeq(hrp::dvector::Zero(m_robot->numJoints()));
 
     loop = 0;
@@ -135,6 +132,7 @@ RTC::ReturnCode_t HapticController::setupEEIKConstraintFromConf(std::map<std::st
             double tmp_aa[4];
             for (int j = 0; j < 4; j++ ){ coil::stringTo(tmp_aa[j], ee_conf_all[i*prop_num+6+j].c_str()); }
             _ee_ikc_map[ee_name].localR = Eigen::AngleAxis<double>(tmp_aa[3], hrp::Vector3(tmp_aa[0], tmp_aa[1], tmp_aa[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
+            jpath_ee[ee_name] = hrp::JointPath(m_robot->rootLink(), m_robot->link(target_link_name));
             if(_robot->link(target_link_name)){
               RTCOUT << "End Effector [" << ee_name << "]" << std::endl;
               RTCOUT << "   target_link_name = " << _ee_ikc_map[ee_name].target_link_name << ", base = " << base_name << std::endl;
@@ -161,6 +159,7 @@ RTC::ReturnCode_t HapticController::onExecute(RTC::UniqueId ec_id){
     processTransition();
     mode.update();
 
+    calcCurrentState();
     calcTorque();
 
     if (mode.isRunning()) {
@@ -207,23 +206,14 @@ class ContactInfo{
         }
 };
 
-void HapticController::calcTorque (){
-
-    dqAct_filtered = dqAct_filter.passFilter(hrp::to_dvector(m_dqAct.data));
-
-//    static hrp::dvector q_old = hrp::to_dvector(m_qAct.data);
-//    hrp::dvector dq_tmp = (hrp::to_dvector(m_qAct.data) - q_old) / m_dt;
-//    dqAct_filtered = dqAct_filter.passFilter(dq_tmp);
-
-
-
 
+void HapticController::calcCurrentState(){
+    ///// set current joint angles
     for(int i=0;i<m_robot->numJoints();i++)m_robot->joint(i)->q = m_qAct.data[i];
+    dqAct_filtered = dqAct_filter.passFilter(hrp::to_dvector(m_dqAct.data));
 
-    if(!idsb.is_initialized){
-      idsb.setInitState(m_robot, m_dt);
-    }
-//    calcAccelerationsForInverseDynamics(m_robot, idsb);
+    ///// calc inverse dynamics (forward kinematics included)
+    if(!idsb.is_initialized){ idsb.setInitState(m_robot, m_dt); }
     for(int i=0;i<m_robot->numJoints();i++)idsb.q(i) = m_robot->joint(i)->q;
     idsb.dq.fill(0);
     idsb.ddq.fill(0);
@@ -236,12 +226,31 @@ void HapticController::calcTorque (){
     idsb.base_w_hat.fill(0);
     idsb.base_w.fill(0);
     idsb.base_dw.fill(0);
-
     hrp::Vector3 f_base_wld, t_base_wld;
-    calcRootLinkWrenchFromInverseDynamics(m_robot, idsb, f_base_wld, t_base_wld);
+    calcRootLinkWrenchFromInverseDynamics(m_robot, idsb, f_base_wld, t_base_wld); // torque (m_robot->joint(i)->u) set
+    updateInvDynStateBuffer(idsb);
 
-//    if(loop%500==0)std::cerr<<"ref_torque is"<<std::endl;
-//    if(loop%500==0)dbgv(hrp::getUAll(m_robot));
+    ///// calc end effector position and velocity
+    for(int i=0; i<ee_names.size();i++){
+        ee_pose[ee_names[i]] = ee_ikc_map[ee_names[i]].getCurrentTargetPose(m_robot);
+    }
+    if(ee_pose_old.size()==0){ ee_pose_old = ee_pose; } //init
+    for(int i=0; i<ee_names.size();i++){
+        ee_vel[ee_names[i]].head(3) = (ee_pose[ee_names[i]].p - ee_pose_old[ee_names[i]].p) / m_dt;
+        ee_vel[ee_names[i]].tail(3) = ee_pose_old[ee_names[i]].R * hrp::omegaFromRot(ee_pose_old[ee_names[i]].R.transpose() * ee_pose[ee_names[i]].R) / m_dt; // world angular velocity
+        ee_vel_filtered[ee_names[i]] = ee_vel_filter[ee_names[i]].passFilter(ee_vel[ee_names[i]]);
+    }
+    ee_pose_old = ee_pose;
+
+    ///// calc base tp end effector Jacobian
+    for(int i=0; i<ee_names.size();i++){
+        hrp::JointPath jp(m_robot->rootLink(), m_robot->link(ee_ikc_map[ee_names[i]].target_link_name));
+        jp.calcJacobian(J_ee[ee_names[i]], ee_ikc_map[ee_names[i]].localPos);
+    }
+}
+
+
+void HapticController::calcTorque(){
 
 //    static std::vector<ContactInfo> civ;
 //    if(civ.size() == 0){
@@ -264,8 +273,6 @@ void HapticController::calcTorque (){
 ////        civ.push_back(ContactInfo(m_robot->link("LARM_JOINT6"), stikp[3].localp + hrp::Vector3(-0.1,  0.05, 0)*a ));
 ////        civ.push_back(ContactInfo(m_robot->link("LARM_JOINT6"), stikp[3].localp + hrp::Vector3(-0.1, -0.05, 0)*a ));
 //    }
-
-
        // calc virtual floor reaction force
 //    {
 //        for (int i=0; i<civ.size(); i++){
@@ -300,83 +307,77 @@ void HapticController::calcTorque (){
 
 
     {
-        //fix ee rot horizontal
-        std::vector<std::string> targets;
-        targets.push_back("LLEG_JOINT5");
-        targets.push_back("RLEG_JOINT5");
-        static std::vector<hrp::Vector3> diff_rots_old(targets.size());
-        for (int i=0; i<targets.size();i++){
+        std::vector<std::string> tgt;
+        tgt.push_back("lleg");
+        tgt.push_back("rleg");
+
+        static double out1=1,out2=1,out3=1;
+//        if(mode.isRunning()){
+//            out1 += m_dt/5;
+//            LIMIT_MAX(out1,1);
+//            if(out1 >= 1){
+//                out2 += m_dt/5;
+//                LIMIT_MAX(out2,1);
+//                if(out2>= 1){
+//                    out3 += m_dt/5;
+//                    LIMIT_MAX(out3,1);
+//                }
+//            }
+//        }
+
+        ///// fix ee rot horizontal
+        for (int i=0; i<tgt.size();i++){
             hrp::Vector3 diff_rot;
-            rats::difference_rotation(diff_rot, m_robot->link(targets[i])->R, hrp::Matrix33::Identity());
-            hrp::Vector3 diff_rot_d = (diff_rot - diff_rots_old[i])/m_dt;
-            hrp::dvector6 wrench;
-            wrench << 0,0,0,diff_rot * 1000 + diff_rot_d * 10;
-            hrp::JointPath jp(m_robot->rootLink(), m_robot->link(targets[i]));
-            hrp::dmatrix J_ee;
-            jp.calcJacobian(J_ee);
-            hrp::dvector tq_for_fix = J_ee.transpose() * wrench;
-            for (int j = 0; j < jp.numJoints(); j++) jp.joint(j)->u += tq_for_fix(j);
-            diff_rots_old[i] = diff_rot;
+            rats::difference_rotation(diff_rot, ee_pose[tgt[i]].R, hrp::Matrix33::Identity());
+            const hrp::Vector3 diff_rot_vel = hrp::Vector3::Zero() - ee_vel[tgt[i]].tail(3);
+            hrp::dvector6 wrench = (hrp::dvector6()<< 0,0,0, diff_rot * hcp.foot_horizontal_pd_gain(0) + diff_rot_vel * hcp.foot_horizontal_pd_gain(1)).finished();
+            LIMIT_NORM(wrench.tail(3), 50);
+            hrp::dvector tq_tmp = J_ee[ee_names[i]].transpose() * wrench;
+            for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j) * out1; }
         }
-    }
 
-    {
-        //keep ee apart each other
-        std::vector<std::string> targets;
-        targets.push_back("LLEG_JOINT5");
-        targets.push_back("RLEG_JOINT5");
-        double dist = m_robot->link("LLEG_JOINT5")->p(Y) - m_robot->link("RLEG_JOINT5")->p(Y);
-        if(dist < 0.3){
-            double err = dist - 0.3;
-            for (int i=0; i<targets.size();i++){
-                hrp::dvector6 wrench;
-                wrench << 0, (i==0 ? -1 : 1) * err*1000,0,0,0,0;
-                hrp::JointPath jp(m_robot->rootLink(), m_robot->link(targets[i]));
-                hrp::dmatrix J_ee;
-                jp.calcJacobian(J_ee);
-                hrp::dvector tq_for_apart = J_ee.transpose() * wrench;
-                for (int j = 0; j < jp.numJoints(); j++) jp.joint(j)->u += tq_for_apart(j);
+        ///// virtual floor
+        for (int i=0; i<tgt.size();i++){
+            const double current_z = ee_pose[tgt[i]].p(Z) - m_robot->rootLink()->p(Z);
+            if(loop%1000==0){
+                dbg(current_z);
+            }
+            if(current_z < hcp.floor_height_from_base){
+                hrp::dvector6 wrench = (hrp::dvector6()<< 0,0, (hcp.floor_height_from_base-current_z)*hcp.floor_pd_gain(0) + (0-ee_vel[tgt[i]](fz))*hcp.floor_pd_gain(1), 0,0,0).finished();
+                LIMIT_NORM(wrench, 1000);
+                hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
+                for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j) * out2; }
             }
         }
-    }
 
-    {
-        // floor
-        std::vector<std::string> targets;
-        targets.push_back("LLEG_JOINT5");
-        targets.push_back("RLEG_JOINT5");
-        for (int i=0; i<targets.size();i++){
-            double height = m_robot->link(targets[i])->p(Z) - m_robot->rootLink()->p(Z);
-            if(height < -1.0){
-                hrp::dvector6 wrench;
-                wrench << 0,0, -(height-(-1.0))*10000,0,0,0;
-
-                if(m_robot->link(targets[i])->p(X)<0) wrench(X) += - m_robot->link(targets[i])->p(X) * 1000;
-
-                hrp::JointPath jp(m_robot->rootLink(), m_robot->link(targets[i]));
-                hrp::dmatrix J_ee;
-                jp.calcJacobian(J_ee);
-                hrp::dvector tq_for_apart = J_ee.transpose() * wrench;
-                for (int j = 0; j < jp.numJoints(); j++) jp.joint(j)->u += tq_for_apart(j);
+        ///// virtual back wall
+        for (int i=0; i<tgt.size();i++){
+            const double wall_x_rel_base = 0.0;
+            const double current_x = ee_pose[tgt[i]].p(X) - m_robot->rootLink()->p(X);
+            if(current_x < wall_x_rel_base){
+                hrp::dvector6 wrench = (hrp::dvector6()<< (wall_x_rel_base - current_x) * 1000,0,0, 0,0,0).finished();
+                LIMIT_NORM(wrench, 100);
+                hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
+                for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j) * out3; }
+            }
+        }
+
+        ///// keep apart each other
+        const double current_dist = ee_pose["lleg"].p(Y) - ee_pose["rleg"].p(Y);
+        if(current_dist < hcp.foot_min_distance){
+            for (int i=0; i<tgt.size();i++){
+                hrp::dvector6 wrench = (hrp::dvector6()<< 0, (tgt[i]=="lleg" ? 1:-1) * (hcp.foot_min_distance - current_dist) * 1000, 0,0,0,0).finished();
+                LIMIT_NORM(wrench, 50);
+                hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
+                for (int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j) * out2; }
             }
         }
     }
 
 
     // calc real robot feedback force
-    std::vector<hrp::Pose3> ee_pose(ee_names.size());
-    for(int i=0; i<ee_pose.size();i++){
-        ee_pose[i].p = m_robot->link(ee_ikc_map[ee_names[i]].target_link_name)->p;
-        ee_pose[i].R = m_robot->link(ee_ikc_map[ee_names[i]].target_link_name)->R;
-    }
-    static std::vector<hrp::Pose3> ee_pose_old(ee_pose);
     for (int i=0; i<ee_names.size(); i++){
         const hrp::dvector6 wrench_raw = hrp::to_dvector(m_feedbackWrenches[ee_names[i]].data);
-        wrench_filtered[ee_names[i]] = wrench_filter[ee_names[i]].passFilter( wrench_raw );
-        const hrp::Vector3 ee_pos_vel = (ee_pose[i].p - ee_pose_old[i].p) / m_dt;
-        const hrp::Vector3 ee_rot_vel = ee_pose_old[i].R * hrp::omegaFromRot(ee_pose_old[i].R.transpose() * ee_pose[i].R) / m_dt;
-        ee_vel_filtered[ee_names[i]] = ee_vel_filter[ee_names[i]].passFilter((hrp::dvector6()<<ee_pos_vel,ee_rot_vel).finished());
-
         const hrp::dvector6 w_hpf = wrench_raw - wrench_lpf_for_hpf[ee_names[i]].passFilter( wrench_raw );
         const hrp::dvector6 w_lpf = wrench_lpf[ee_names[i]].passFilter( wrench_raw );
         wrench_shaped[ee_names[i]] = w_hpf * hcp.wrench_hpf_gain + w_lpf * hcp.wrench_lpf_gain;
@@ -385,16 +386,8 @@ void HapticController::calcTorque (){
         LIMIT_NORM(wrench_used[ee_names[i]].head(3),50);
         LIMIT_NORM(wrench_used[ee_names[i]].tail(3),5);
 
-
-//        if(f.norm() > 1e-6 ){ // avoid zero devide
-//            f = f * fabs(1 - 1 * f.normalized().dot(ee_pos_vel) );
-//        }
-//        if(t.norm() > 1e-6){ // avoid zero devide
-//            t = t * fabs(1 - 0.1 * t.normalized().dot(ee_rot_vel) );
-//        }
-
-//        hrp::Vector3 friction_f = - hcp.ee_friction_coeff(0) * ee_vel_filtered[ee_names[i]].head(3);
-//        hrp::Vector3 friction_t = - hcp.ee_friction_coeff(1) * ee_vel_filtered[ee_names[i]].tail(3);
+        hrp::Vector3 friction_f = - hcp.ee_pos_rot_friction_coeff(0) * ee_vel_filtered[ee_names[i]].head(3);
+        hrp::Vector3 friction_t = - hcp.ee_pos_rot_friction_coeff(1) * ee_vel_filtered[ee_names[i]].tail(3);
 
         if(ee_names[i] == "rarm"){
             m_debugData.data = hrp::to_DoubleSeq((hrp::dvector(6+6+6+6)<<wrench_raw,wrench_shaped[ee_names[i]],w_hpf,w_lpf).finished());
@@ -402,7 +395,7 @@ void HapticController::calcTorque (){
             m_debugDataOut.write();
             if(loop%1000==0){
                 dbgv(ee_vel_filtered["rarm"]);
-                dbgv(hcp.ee_friction_coeff);
+                dbgv(hcp.ee_pos_rot_friction_coeff);
                 dbg(hcp.ee_vel_filter_cutoff_hz);
             }
         }
@@ -414,7 +407,6 @@ void HapticController::calcTorque (){
         for (int j = 0; j < jp.numJoints(); j++) jp.joint(j)->u += tq_from_feedbackWrench(j);
         if(loop%1000==0)dbgv(wrench_used[ee_names[i]]);
     }
-    ee_pose_old = ee_pose;
 
 
     {
@@ -435,25 +427,28 @@ void HapticController::calcTorque (){
         }
     }
 
+    { // real robot joint friction damper
+//        const hrp::dvector q_friction_coeff_const = (hrp::dvector(m_robot->numJoints()) << 0,0,0,0,0,0, 0,0,0,0,0,0, 0, 3,3,2,2,1,0.5,0.5, 3,3,2,2,1,0.5,0.5).finished();
+//        hrp::dvector friction_tq = - q_friction_coeff_const.array() * dqAct_filtered.array();
+        hrp::dvector friction_tq = - hcp.q_friction_coeff * dqAct_filtered;
+        for (int i=0; i<m_robot->numJoints(); i++){
+         LIMIT_MINMAX(friction_tq(i), -5, 5);
+         m_robot->joint(i)->u += friction_tq(i);
+        }
+        if(loop%1000==0)dbgv(friction_tq);
+    }
 
+    {// calc qref pd control torque
+        hrp::dvector q_diff = hrp::to_dvector(m_qRef.data) - hrp::to_dvector(m_qAct.data);
+        hrp::dvector dq_diff = - dqAct_filtered;
+        hrp::dvector torque = q_diff * hcp.q_ref_pd_gain(0) + dq_diff * hcp.q_ref_pd_gain(1);
+        for (int i=0; i<m_robot->numJoints(); i++){
+            m_robot->joint(i)->u += torque(i);
+        }
+    }
 
-//    m_debugData.data = hrp::to_DoubleSeq((hrp::dvector(27*2)<<hrp::to_dvector(m_dqAct.data),dqAct_filtered).finished());
-//    m_debugData.tm = m_qRef.tm;
-//    m_debugDataOut.write();
 
 
-//    {
-//        // real robot joint friction damper
-//        const hrp::dvector q_friction_coeff_const = (hrp::dvector(m_robot->numJoints()) << 0,0,0,0,0,0, 0,0,0,0,0,0, 0, 3,3,2,2,1,0.5,0.5, 3,3,2,2,1,0.5,0.5).finished();
-//        hrp::dvector friction_tq = - q_friction_coeff_const.array() * dqAct_filtered.array();
-////        hrp::dvector friction_tq = - hcp.q_friction_coeff * dqAct_filtered;
-//        for (int i=0; i<m_robot->numJoints(); i++){
-//         LIMIT_MINMAX(friction_tq(i), -5, 5);
-//         m_robot->joint(i)->u += friction_tq(i);
-//        }
-//        if(loop%1000==0)dbgv(friction_tq);
-//    }
-
 
 
     const hrp::dvector max_torque = (hrp::dvector(m_robot->numJoints()) << 30,80,80,80,30,30, 30,80,80,80,30,30, 80, 30,30,25,25,20,10,10, 30,30,25,25,20,10,10).finished();
@@ -465,7 +460,6 @@ void HapticController::calcTorque (){
 
     if(loop%1000==0)dbgv(hrp::getUAll(m_robot));
 
-    updateInvDynStateBuffer(idsb);
 
 
 
@@ -492,7 +486,6 @@ void HapticController::calcTorque (){
 
 void HapticController::processTransition(){
     switch(mode.now()){
-
         case MODE_SYNC_TO_HC:
             if(mode.pre() == MODE_IDLE){ double tmp = 1.0; t_ip->setGoal(&tmp, 10.0, true); }
             if (!t_ip->isEmpty() ){
@@ -503,7 +496,7 @@ void HapticController::processTransition(){
             break;
 
         case MODE_SYNC_TO_IDLE:
-            if(mode.pre() == MODE_HC || mode.pre() == MODE_PAUSE){ double tmp = 0.0; t_ip->setGoal(&tmp, 10.0, true); }
+            if(mode.pre() == MODE_HC || mode.pre() == MODE_PAUSE){ double tmp = 0.0; t_ip->setGoal(&tmp, 5.0, true); }
             if (!t_ip->isEmpty()) {
                 t_ip->get(&output_ratio, true);
             }else{
@@ -511,39 +504,12 @@ void HapticController::processTransition(){
             }
             break;
     }
+    if (!q_ref_ip->isEmpty() ){
+        q_ref_ip->get(&q_ref_output_ratio, true);
+    }
 }
 
 
-//void HapticController::preProcessForHapticController(){
-//    hrp::Vector3 basePos_heightChecked = hrp::to_Vector3(m_basePos.data);//ベースリンク高さ調整により足裏高さ0に
-//    fik->m_robot->rootLink()->p = basePos_heightChecked;
-////    for ( int i = 0; i < fik->m_robot->numJoints(); i++ ){ fik->m_robot->joint(i)->q = m_qRef.data[i]; }
-//    hrp::setQAll(fik->m_robot, hrp::to_dvector(m_qRef.data));
-//    fik->m_robot->calcForwardKinematics();
-//
-//    //TODO
-////    hrp::Vector3 init_foot_mid_coord = (fik_in->getEndEffectorPos("rleg") + fik_in->getEndEffectorPos("lleg")) / 2;
-////    if( fabs((double)init_foot_mid_coord(Z)) > 1e-5 ){
-////        basePos_heightChecked(Z) -= init_foot_mid_coord(Z);
-////        init_foot_mid_coord(Z) = 0;dqAct_filter
-////        std::cerr<<"["<<m_profile.instance_name<<"] Input basePos height is invalid. Auto modify "<<m_basePos.data.z<<" -> "<<basePos_heightChecked(Z)<<endl;
-////    }
-//
-//    std::vector<hrp::BodyPtr> body_list;
-//    body_list.push_back(fik->m_robot);
-//    body_list.push_back(m_robot_vsafe);
-//    for(int i=0;i<body_list.size();i++){//初期姿勢でBodyをFK
-//        hrp::setRobotStateVec(body_list[i], hrp::to_dvector(m_qRef.data), basePos_heightChecked, hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y));
-//        body_list[i]->calcForwardKinematics();
-//    }
-//
-//    fik->q_ref = hrp::getRobotStateVec(fik->m_robot);
-//    q_ip->set(fik->q_ref.data());
-//    wbms->initializeRequest(fik->m_robot, ee_ikc_map);
-//}
-
-
-
 bool HapticController::startHapticController(){
     if(mode.now() == MODE_IDLE){
         RTCOUT << "startHapticController" << std::endl;
@@ -591,56 +557,65 @@ bool HapticController::stopHapticController(){
     }
 }
 
+namespace hrp{
+    hrp::Vector2 to_Vector2(const OpenHRP::HapticControllerService::DblSequence2& in){ return (hrp::Vector2()<< in[0],in[1]).finished(); }
+    OpenHRP::HapticControllerService::DblSequence2 to_DblSequence2(const hrp::Vector2& in){
+        OpenHRP::HapticControllerService::DblSequence2 ret; ret.length(2); ret[0] = in(0); ret[1] = in(1); return ret; }
+}
 
 bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param){
     RTCOUT << "setHapticControllerParam" << std::endl;
-    hcp.gravity_compensation_ratio  = i_param.gravity_compensation_ratio;
     hcp.dqAct_filter_cutoff_hz      = i_param.dqAct_filter_cutoff_hz;
-    {
-        LIMIT_MAX(hcp.dqAct_filter_cutoff_hz, 1/m_dt/2);
-        dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        dqAct_filter.reset(0);
-    }
+    hcp.ee_vel_filter_cutoff_hz     = i_param.ee_vel_filter_cutoff_hz;
+    hcp.floor_height_from_base      = i_param.floor_height_from_base;
+    hcp.foot_min_distance           = i_param.foot_min_distance;
     hcp.force_feedback_ratio        = i_param.force_feedback_ratio;
+    hcp.gravity_compensation_ratio  = i_param.gravity_compensation_ratio;
     hcp.q_friction_coeff            = i_param.q_friction_coeff;
-    for(int i=0;i<hcp.ee_friction_coeff.size(); i++){ hcp.ee_friction_coeff(i) = i_param.ee_friction_coeff[i]; }
-    hcp.wrench_filter_cutoff_hz     = i_param.wrench_filter_cutoff_hz;
-    hcp.ee_vel_filter_cutoff_hz     = i_param.ee_vel_filter_cutoff_hz;
+    hcp.q_ref_output_ratio_goal     = i_param.q_ref_output_ratio_goal;
     hcp.wrench_hpf_cutoff_hz        = i_param.wrench_hpf_cutoff_hz;
     hcp.wrench_lpf_cutoff_hz        = i_param.wrench_lpf_cutoff_hz;
+    hcp.wrench_hpf_gain             = i_param.wrench_hpf_gain;
+    hcp.wrench_lpf_gain             = i_param.wrench_lpf_gain;
+    hcp.ee_pos_rot_friction_coeff   = hrp::to_Vector2(i_param.ee_pos_rot_friction_coeff);
+    hcp.floor_pd_gain               = hrp::to_Vector2(i_param.floor_pd_gain);
+    hcp.foot_horizontal_pd_gain     = hrp::to_Vector2(i_param.foot_horizontal_pd_gain);
+    hcp.q_ref_pd_gain               = hrp::to_Vector2(i_param.q_ref_pd_gain);
+    ///// update process if required
+    q_ref_ip->setGoal(&hcp.q_ref_output_ratio_goal, 5.0, true);
+    dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+    dqAct_filter.reset(0);
     for(int i=0;i<ee_names.size();i++){
-        LIMIT_MAX(hcp.wrench_filter_cutoff_hz, 1/m_dt/2);
-        LIMIT_MAX(hcp.ee_vel_filter_cutoff_hz, 1/m_dt/2);
-        LIMIT_MAX(hcp.wrench_hpf_cutoff_hz, 1/m_dt/2);
-        LIMIT_MAX(hcp.wrench_lpf_cutoff_hz, 1/m_dt/2);
-        wrench_filter[ee_names[i]].setParameter(hcp.wrench_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
         ee_vel_filter[ee_names[i]].setParameter(hcp.ee_vel_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        wrench_lpf_for_hpf[ee_names[i]].setParameter(hcp.wrench_hpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        wrench_lpf[ee_names[i]].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        wrench_filter[ee_names[i]].reset(0);
         ee_vel_filter[ee_names[i]].reset(0);
+        wrench_lpf_for_hpf[ee_names[i]].setParameter(hcp.wrench_hpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
         wrench_lpf_for_hpf[ee_names[i]].reset(0);
+        wrench_lpf[ee_names[i]].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
         wrench_lpf[ee_names[i]].reset(0);
     }
-    hcp.wrench_hpf_gain             = i_param.wrench_hpf_gain;
-    hcp.wrench_lpf_gain             = i_param.wrench_lpf_gain;
+
     return true;
 }
 
 
 bool HapticController::getParams(OpenHRP::HapticControllerService::HapticControllerParam& i_param){
     RTCOUT << "getHapticControllerParam" << std::endl;
-    i_param.gravity_compensation_ratio  = hcp.gravity_compensation_ratio;
     i_param.dqAct_filter_cutoff_hz      = hcp.dqAct_filter_cutoff_hz;
-    i_param.wrench_filter_cutoff_hz     = hcp.wrench_filter_cutoff_hz;
     i_param.ee_vel_filter_cutoff_hz     = hcp.ee_vel_filter_cutoff_hz;
+    i_param.floor_height_from_base      = hcp.floor_height_from_base;
+    i_param.foot_min_distance           = hcp.foot_min_distance;
     i_param.force_feedback_ratio        = hcp.force_feedback_ratio;
+    i_param.gravity_compensation_ratio  = hcp.gravity_compensation_ratio;
     i_param.q_friction_coeff            = hcp.q_friction_coeff;
-    for(int i=0;i<hcp.ee_friction_coeff.size(); i++){ i_param.ee_friction_coeff[i] = hcp.ee_friction_coeff(i); }
+    i_param.q_ref_output_ratio_goal     = hcp.q_ref_output_ratio_goal;
     i_param.wrench_hpf_cutoff_hz        = hcp.wrench_hpf_cutoff_hz;
     i_param.wrench_lpf_cutoff_hz        = hcp.wrench_lpf_cutoff_hz;
     i_param.wrench_hpf_gain             = hcp.wrench_hpf_gain;
     i_param.wrench_lpf_gain             = hcp.wrench_lpf_gain;
+    i_param.ee_pos_rot_friction_coeff   = hrp::to_DblSequence2(hcp.ee_pos_rot_friction_coeff);
+    i_param.floor_pd_gain               = hrp::to_DblSequence2(hcp.floor_pd_gain);
+    i_param.foot_horizontal_pd_gain     = hrp::to_DblSequence2(hcp.foot_horizontal_pd_gain);
+    i_param.q_ref_pd_gain               = hrp::to_DblSequence2(hcp.q_ref_pd_gain);
     return true;
 }
 
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index 7dd8e16dd27..646b0978b9e 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -16,8 +16,8 @@
 #include <time.h>
 
 #include "HapticControllerService_impl.h"
-#include "../Stabilizer/StabilizerService_impl.h"
-#include "../AutoBalancer/AutoBalancerService_impl.h"
+//#include "../Stabilizer/StabilizerService_impl.h"
+//#include "../AutoBalancer/AutoBalancerService_impl.h"
 //#include "wbms_core.h"
 #include "../AutoBalancer/FullbodyInverseKinematicsSolver.h"
 
@@ -99,75 +99,76 @@ class HapticController : public RTC::DataFlowComponentBase{
 //        std::map<std::string, RTC::TimedDoubleSeq> m_feedbackWrenches_filtered_dbg;
 //        RTC::OutPort<RTC::TimedDoubleSeq> m_dq_filtered_dbgOut;
 
-
-
         RTC::CorbaPort m_HapticControllerServicePort;
-
-        hrp::BodyPtr m_robot;
-
         HapticControllerService_impl m_service0;
 
     private:
         double m_dt;
         unsigned int loop;
         unsigned int m_debugLevel;
+        hrp::BodyPtr m_robot;
+
+        double output_ratio, q_ref_output_ratio;
+        interpolator *t_ip, *q_ref_ip;
+
+        hrp::InvDynStateBuffer idsb;
+        BiquadIIRFilterVec2 dqAct_filter;
+        hrp::dvector dqAct_filtered;
+        std::map<std::string, IKConstraint> ee_ikc_map;
+        std::map<std::string, BiquadIIRFilterVec2> ee_vel_filter;
+        std::map<std::string, BiquadIIRFilterVec2> wrench_lpf_for_hpf, wrench_lpf;
+        std::map<std::string, hrp::dvector6> wrench_shaped, wrench_used;
+        std::map<std::string, hrp::Pose3> ee_pose, ee_pose_old;
+        std::map<std::string, hrp::dvector6> ee_vel, ee_vel_filtered; // = twist
+        std::map<std::string, hrp::JointPath> jpath_ee;
+        std::map<std::string, hrp::dmatrix> J_ee;
+
+        ControlMode mode;
+
+        std::vector<std::string> ee_names, tgt_names;
 
         class HCParams {
             public:
-                double gravity_compensation_ratio;
-                double force_feedback_ratio;
                 double dqAct_filter_cutoff_hz;
                 double ee_vel_filter_cutoff_hz;
-                double wrench_filter_cutoff_hz;
+                double floor_height_from_base;
+                double foot_min_distance;
+                double force_feedback_ratio;
+                double gravity_compensation_ratio;
                 double q_friction_coeff;
-                hrp::Vector2 ee_friction_coeff;
+                double q_ref_output_ratio_goal;
                 double wrench_hpf_cutoff_hz;
                 double wrench_lpf_cutoff_hz;
                 double wrench_hpf_gain;
                 double wrench_lpf_gain;
+                hrp::Vector2 ee_pos_rot_friction_coeff;
+                hrp::Vector2 floor_pd_gain;
+                hrp::Vector2 foot_horizontal_pd_gain;
+                hrp::Vector2 q_ref_pd_gain;
             HCParams(){
-                gravity_compensation_ratio  = 0.9;
-                force_feedback_ratio        = 0.2;
                 dqAct_filter_cutoff_hz      = 100;// 10以下で確実に位相遅れによる振動
                 ee_vel_filter_cutoff_hz     = 100;// 10以下で確実に位相遅れによる振動
-                wrench_filter_cutoff_hz     = 100;
-                q_friction_coeff            = 1.0;
-                ee_friction_coeff           << 1, 0.1;
+                floor_height_from_base      = -1.1;
+                foot_min_distance           = 0.3;
+                force_feedback_ratio        = 0.2;
+                gravity_compensation_ratio  = 0.9;
+                q_friction_coeff            = 0;
+                q_ref_output_ratio_goal     = 0;
                 wrench_hpf_cutoff_hz        = 20;
                 wrench_lpf_cutoff_hz        = 0.3;
                 wrench_hpf_gain             = 1;
                 wrench_lpf_gain             = 0.2;
+                ee_pos_rot_friction_coeff   << 0, 0; // 1, 0.1
+                floor_pd_gain               << 1000, 10;
+                foot_horizontal_pd_gain     << 1000, 10;
+                q_ref_pd_gain               << 0, 0;
             }
         } hcp;
 
-
-        double output_ratio;
-        interpolator *t_ip,*q_ip;
-        hrp::dvector avg_q_vel, avg_q_acc;
-
-        hrp::InvDynStateBuffer idsb;
-        BiquadIIRFilterVec2 dqAct_filter;
-        hrp::dvector dqAct_filtered;
-        std::map<std::string, BiquadIIRFilterVec2> ee_vel_filter;
-        std::map<std::string, hrp::dvector6> ee_vel_filtered;
-        std::map<std::string, BiquadIIRFilterVec2> wrench_filter;
-        std::map<std::string, hrp::dvector6> wrench_filtered;
-
-        std::map<std::string, BiquadIIRFilterVec2> wrench_lpf_for_hpf;
-        std::map<std::string, BiquadIIRFilterVec2> wrench_lpf;
-        std::map<std::string, hrp::dvector6> wrench_shaped;
-        std::map<std::string, hrp::dvector6> wrench_used;
-
-        std::map<std::string, IKConstraint> ee_ikc_map;
-        ControlMode mode;
-
-        std::vector<std::string> ee_names;
-        std::vector<std::string> tgt_names;
-
         RTC::ReturnCode_t setupEEIKConstraintFromConf(std::map<std::string, IKConstraint>& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop);
+        void calcCurrentState();
         void calcTorque();
         void processTransition();
-//        void processHapticController(const HumanPose& pose_ref);
 };
 
 
diff --git a/rtc/HapticController/HapticControllerService_impl.cpp b/rtc/HapticController/HapticControllerService_impl.cpp
index 2c2f1761770..557080a74c9 100644
--- a/rtc/HapticController/HapticControllerService_impl.cpp
+++ b/rtc/HapticController/HapticControllerService_impl.cpp
@@ -42,13 +42,14 @@ void HapticControllerService_impl::setParams(const OpenHRP::HapticControllerServ
 void HapticControllerService_impl::getParams(OpenHRP::HapticControllerService::HapticControllerParam_out i_param)
 {
     //????????????????????????????????????????????????????????
-    OpenHRP::HapticControllerService::HapticControllerParam tmp;
-    m_hapticcontroller->getParams(tmp);
-    i_param = tmp;
+//    OpenHRP::HapticControllerService::HapticControllerParam tmp;
+//    m_hapticcontroller->getParams(tmp);
+//    i_param = tmp;
 
-    //今までこれでいけてた？
-//    i_param = new OpenHRP::HapticControllerService::HapticControllerParam();
-//    m_hapticcontroller->getParams(*i_param);
+    //今までこれでいけてた？ idlにsequence<double, 2>とか可変長を含むとき？
+    i_param = new OpenHRP::HapticControllerService::HapticControllerParam();
+    m_hapticcontroller->getParams(*i_param);
 
-//  m_hapticcontroller->getParams(i_param); // error: no matching function for call to ‘HapticController::getParams(OpenHRP::HapticControllerService::HapticControllerParam_out&)’
-};
+    // error: no matching function for call to ‘HapticController::getParams(OpenHRP::HapticControllerService::HapticControllerParam_out&)’
+//  m_hapticcontroller->getParams(i_param);
+ };

From 2026c3b9c8c720a113675462d5737bde6223b5c2 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 4 Sep 2019 21:18:41 +0900
Subject: [PATCH 169/217] add teleopOdom

---
 rtc/AutoBalancer/MyUtils.h                |   2 +
 rtc/HapticController/HapticController.cpp | 164 +++++++++++++---------
 rtc/HapticController/HapticController.h   |  14 +-
 3 files changed, 107 insertions(+), 73 deletions(-)

diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index a7fa754bcc8..5e886b548e3 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -26,6 +26,8 @@ static const double Q_BUTTERWORTH = 0.707106781;
 #define dbgn(var) std::cout<<#var"= "<<std::endl<<(var)<<std::endl
 #define dbgv(var) std::cout<<#var"= "<<(var.transpose())<<std::endl
 #define RTCOUT std::cerr << "[" << m_profile.instance_name << "] "
+#define RTC_INFO_STREAM(var) std::cout << "[" << m_profile.instance_name << "] "<< var << std::endl;
+#define RTC_WARN_STREAM(var) std::cerr << "\x1b[31m[" << m_profile.instance_name << "] " << var << "\x1b[39m" << std::endl;
 
 #define LIMIT_MIN(x,min) (x= ( x<min ? min:x ))
 #define LIMIT_MAX(x,max) (x= ( x<max ? x:max ))
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index d7db74620d4..60b254209f3 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -65,15 +65,20 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     setupEEIKConstraintFromConf(ee_ikc_map, m_robot, prop);
     RTCOUT << "setupEEIKConstraintFromConf finished" << std::endl;
 
-    const double tmp = 0;
+    const double tmp_init = 0;
     t_ip = new interpolator(1, m_dt, interpolator::LINEAR, 1);
     t_ip->clear();
-    t_ip->set(&tmp);
+    t_ip->set(&tmp_init);
     t_ip->get(&output_ratio, false);
     q_ref_ip = new interpolator(1, m_dt, interpolator::LINEAR, 1);
     q_ref_ip->clear();
-    q_ref_ip->set(&tmp);
+    q_ref_ip->set(&tmp_init);
     q_ref_ip->get(&q_ref_output_ratio, false);
+    const double floor_h_from_base_init = -1.5;
+    floor_h_ip = new interpolator(1, m_dt, interpolator::LINEAR, 1);
+    floor_h_ip->clear();
+    floor_h_ip->set(&floor_h_from_base_init);
+    floor_h_ip->get(&floor_h_from_base, false);
     RTCOUT << "setup interpolator finished" << std::endl;
 
     dqAct_filter = BiquadIIRFilterVec2(m_robot->numJoints());
@@ -93,9 +98,9 @@ RTC::ReturnCode_t HapticController::onInitialize(){
 
     for ( int i=0; i<ee_names.size(); i++) {
         std::string n = "slave_"+ee_names[i]+"_wrench_in";
-        m_feedbackWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_feedbackWrenches[ee_names[i]]));
-        registerInPort(n.c_str(), *m_feedbackWrenchesIn[ee_names[i]]);
-        m_feedbackWrenches[ee_names[i]].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
+        m_slaveEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee_names[i]]));
+        registerInPort(n.c_str(), *m_slaveEEWrenchesIn[ee_names[i]]);
+        m_slaveEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
         RTCOUT << " registerInPort " << n << std::endl;
     }
 
@@ -104,8 +109,8 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     tgt_names.push_back("head");
     for ( int i=0; i<tgt_names.size(); i++) {
         std::string n = "master_"+tgt_names[i]+"_pose_out";
-        m_eePosesOut[tgt_names[i]] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
-        registerOutPort(n.c_str(), *m_eePosesOut[tgt_names[i]]);
+        m_masterTgtPosesOut[tgt_names[i]] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
+        registerOutPort(n.c_str(), *m_masterTgtPosesOut[tgt_names[i]]);
         RTCOUT << " registerOutPort " << n << std::endl;
     }
 
@@ -117,21 +122,21 @@ RTC::ReturnCode_t HapticController::onInitialize(){
 }
 
 RTC::ReturnCode_t HapticController::setupEEIKConstraintFromConf(std::map<std::string, IKConstraint>& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop){
-    coil::vstring ee_conf_all = coil::split(_prop["end_effectors"], ",");
+    std::vector<std::string> ee_conf_all = coil::split(_prop["end_effectors"], ",");
     size_t prop_num = 10; // limbname + linkname + basename + pos(3) + angleaxis(4)
     if (ee_conf_all.size() > 0) {
         size_t ee_num = ee_conf_all.size()/prop_num;
         for (size_t i = 0; i < ee_num; i++) {
             std::string ee_name, target_link_name, base_name; // e.g. rleg, RLEG_JOINT5, WAIST
-            coil::stringTo(ee_name,             ee_conf_all[i*prop_num].c_str());
-            coil::stringTo(target_link_name,    ee_conf_all[i*prop_num+1].c_str());
-            coil::stringTo(base_name,           ee_conf_all[i*prop_num+2].c_str());
+            ee_name             = ee_conf_all[i*prop_num];
+            target_link_name    = ee_conf_all[i*prop_num+1];
+            base_name           = ee_conf_all[i*prop_num+2];
             ee_names.push_back(ee_name);
             _ee_ikc_map[ee_name].target_link_name = target_link_name;
-            for (size_t j = 0; j < XYZ; j++){ coil::stringTo(_ee_ikc_map[ee_name].localPos(j), ee_conf_all[i*prop_num+3+j].c_str()); }
-            double tmp_aa[4];
-            for (int j = 0; j < 4; j++ ){ coil::stringTo(tmp_aa[j], ee_conf_all[i*prop_num+6+j].c_str()); }
-            _ee_ikc_map[ee_name].localR = Eigen::AngleAxis<double>(tmp_aa[3], hrp::Vector3(tmp_aa[0], tmp_aa[1], tmp_aa[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
+            for (size_t j = 0; j < XYZ; j++){ _ee_ikc_map[ee_name].localPos(j) = std::atof( ee_conf_all[i*prop_num+3+j].c_str() ); }
+            hrp::Vector4 tmp_aa;
+            for (int j = 0; j < 4; j++ ){ tmp_aa(j) = std::atof( ee_conf_all[i*prop_num+6+j].c_str() ); }
+            _ee_ikc_map[ee_name].localR = Eigen::AngleAxis<double>(tmp_aa(3), tmp_aa.head(3)).toRotationMatrix(); // rotation in VRML is represented by axis + angle
             jpath_ee[ee_name] = hrp::JointPath(m_robot->rootLink(), m_robot->link(target_link_name));
             if(_robot->link(target_link_name)){
               RTCOUT << "End Effector [" << ee_name << "]" << std::endl;
@@ -153,18 +158,16 @@ RTC::ReturnCode_t HapticController::onExecute(RTC::UniqueId ec_id){
     if (m_qActIn.isNew())   { m_qActIn.read(); }
     if (m_dqActIn.isNew())  { m_dqActIn.read(); }
     for (int i=0; i<ee_names.size(); i++) {
-        if ( m_feedbackWrenchesIn[ee_names[i]]->isNew() ) { m_feedbackWrenchesIn[ee_names[i]]->read(); }
+        if ( m_slaveEEWrenchesIn[ee_names[i]]->isNew() ) { m_slaveEEWrenchesIn[ee_names[i]]->read(); }
     }
-
-    processTransition();
     mode.update();
+    processTransition();
 
     calcCurrentState();
     calcTorque();
 
     if (mode.isRunning()) {
         if(mode.isInitialize()){
-//            preProcessForHapticController();
             idsb.setInitState(m_robot, m_dt);//逆動力学初期化
         }
 
@@ -236,7 +239,7 @@ void HapticController::calcCurrentState(){
     }
     if(ee_pose_old.size()==0){ ee_pose_old = ee_pose; } //init
     for(int i=0; i<ee_names.size();i++){
-        ee_vel[ee_names[i]].head(3) = (ee_pose[ee_names[i]].p - ee_pose_old[ee_names[i]].p) / m_dt;
+        ee_vel[ee_names[i]].head(3) = (ee_pose[ee_names[i]].p - ee_pose_old[ee_names[i]].p) / m_dt;// rawは廃止したほうがいい？
         ee_vel[ee_names[i]].tail(3) = ee_pose_old[ee_names[i]].R * hrp::omegaFromRot(ee_pose_old[ee_names[i]].R.transpose() * ee_pose[ee_names[i]].R) / m_dt; // world angular velocity
         ee_vel_filtered[ee_names[i]] = ee_vel_filter[ee_names[i]].passFilter(ee_vel[ee_names[i]]);
     }
@@ -306,6 +309,7 @@ void HapticController::calcTorque(){
 //    }
 
 
+    std::map<std::string, bool> is_contact_to_floor;
     {
         std::vector<std::string> tgt;
         tgt.push_back("lleg");
@@ -329,7 +333,7 @@ void HapticController::calcTorque(){
         for (int i=0; i<tgt.size();i++){
             hrp::Vector3 diff_rot;
             rats::difference_rotation(diff_rot, ee_pose[tgt[i]].R, hrp::Matrix33::Identity());
-            const hrp::Vector3 diff_rot_vel = hrp::Vector3::Zero() - ee_vel[tgt[i]].tail(3);
+            const hrp::Vector3 diff_rot_vel = hrp::Vector3::Zero() - ee_vel_filtered[tgt[i]].tail(3);
             hrp::dvector6 wrench = (hrp::dvector6()<< 0,0,0, diff_rot * hcp.foot_horizontal_pd_gain(0) + diff_rot_vel * hcp.foot_horizontal_pd_gain(1)).finished();
             LIMIT_NORM(wrench.tail(3), 50);
             hrp::dvector tq_tmp = J_ee[ee_names[i]].transpose() * wrench;
@@ -342,17 +346,21 @@ void HapticController::calcTorque(){
             if(loop%1000==0){
                 dbg(current_z);
             }
-            if(current_z < hcp.floor_height_from_base){
-                hrp::dvector6 wrench = (hrp::dvector6()<< 0,0, (hcp.floor_height_from_base-current_z)*hcp.floor_pd_gain(0) + (0-ee_vel[tgt[i]](fz))*hcp.floor_pd_gain(1), 0,0,0).finished();
+            if(current_z < floor_h_from_base){
+                hrp::dvector6 wrench = (hrp::dvector6()<< 0,0, (floor_h_from_base - current_z)*hcp.floor_pd_gain(0) + (0-ee_vel_filtered[tgt[i]](fz))*hcp.floor_pd_gain(1), 0,0,0).finished();
                 LIMIT_NORM(wrench, 1000);
                 hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
                 for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j) * out2; }
+
+                is_contact_to_floor[tgt[i]] = true;
+            }else{
+                is_contact_to_floor[tgt[i]] = false;
             }
         }
 
         ///// virtual back wall
         for (int i=0; i<tgt.size();i++){
-            const double wall_x_rel_base = 0.0;
+            const double wall_x_rel_base = -0.1;
             const double current_x = ee_pose[tgt[i]].p(X) - m_robot->rootLink()->p(X);
             if(current_x < wall_x_rel_base){
                 hrp::dvector6 wrench = (hrp::dvector6()<< (wall_x_rel_base - current_x) * 1000,0,0, 0,0,0).finished();
@@ -372,12 +380,56 @@ void HapticController::calcTorque(){
                 for (int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j) * out2; }
             }
         }
+
+        ///// virtual floor, leg constraint
+        static hrp::Vector3 locked_rel_pos = ee_pose["rleg"].p - ee_pose["lleg"].p;
+        if(is_contact_to_floor["lleg"] && is_contact_to_floor["rleg"]){
+            for (int i=0; i<tgt.size();i++){
+                hrp::Vector3 cur_rel_pos = ee_pose["rleg"].p - ee_pose["lleg"].p;
+                hrp::Vector2 rleg_wrench = (locked_rel_pos - cur_rel_pos).head(XY) * 1000;
+                hrp::dvector6 wrench = (hrp::dvector6()<< (tgt[i]=="rleg" ? 1:-1) * rleg_wrench, 0,0,0,0).finished();
+                LIMIT_NORM(wrench, 1000);
+                hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
+                for (int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
+            }
+        }else{
+            locked_rel_pos = ee_pose["rleg"].p - ee_pose["lleg"].p;
+        }
+
+
+        static std::map<std::string, hrp::Vector2> leg_pos_from_floor_origin;
+        if(loop==0){
+            leg_pos_from_floor_origin["lleg"] << 0,  0.15;
+            leg_pos_from_floor_origin["rleg"] << 0, -0.15;
+        }
+        if(is_contact_to_floor["lleg"]){
+            // not update
+        }else{
+            leg_pos_from_floor_origin["lleg"] = (ee_pose["lleg"].p - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];
+        }
+        if(is_contact_to_floor["rleg"]){
+            // not update
+        }else{
+            leg_pos_from_floor_origin["rleg"] = (ee_pose["rleg"].p - ee_pose["lleg"].p).head(XY) + leg_pos_from_floor_origin["lleg"];
+        }
+
+        hrp::Vector2 floor_origin_from_baselink_2d = (ee_pose["rleg"].p - m_robot->rootLink()->p).head(XY) - leg_pos_from_floor_origin["rleg"];
+        hrp::Vector3 floor_origin_from_baselink = (hrp::Vector3() << floor_origin_from_baselink_2d, floor_h_from_base).finished();
+
+        m_teleopOdom.data = (RTC::Pose3D){floor_origin_from_baselink(X),floor_origin_from_baselink(Y),floor_origin_from_baselink(Z), 0,0,0};
+        m_teleopOdom.tm = m_qRef.tm;
+        m_teleopOdomOut.write();
+
+        m_robot->rootLink()->p = -floor_origin_from_baselink;//ヤバい
+
+
     }
 
 
+
     // calc real robot feedback force
     for (int i=0; i<ee_names.size(); i++){
-        const hrp::dvector6 wrench_raw = hrp::to_dvector(m_feedbackWrenches[ee_names[i]].data);
+        const hrp::dvector6 wrench_raw = hrp::to_dvector(m_slaveEEWrenches[ee_names[i]].data);
         const hrp::dvector6 w_hpf = wrench_raw - wrench_lpf_for_hpf[ee_names[i]].passFilter( wrench_raw );
         const hrp::dvector6 w_lpf = wrench_lpf[ee_names[i]].passFilter( wrench_raw );
         wrench_shaped[ee_names[i]] = w_hpf * hcp.wrench_hpf_gain + w_lpf * hcp.wrench_lpf_gain;
@@ -428,11 +480,9 @@ void HapticController::calcTorque(){
     }
 
     { // real robot joint friction damper
-//        const hrp::dvector q_friction_coeff_const = (hrp::dvector(m_robot->numJoints()) << 0,0,0,0,0,0, 0,0,0,0,0,0, 0, 3,3,2,2,1,0.5,0.5, 3,3,2,2,1,0.5,0.5).finished();
-//        hrp::dvector friction_tq = - q_friction_coeff_const.array() * dqAct_filtered.array();
         hrp::dvector friction_tq = - hcp.q_friction_coeff * dqAct_filtered;
         for (int i=0; i<m_robot->numJoints(); i++){
-         LIMIT_MINMAX(friction_tq(i), -5, 5);
+         LIMIT_MINMAX(friction_tq(i), -10, 10);
          m_robot->joint(i)->u += friction_tq(i);
         }
         if(loop%1000==0)dbgv(friction_tq);
@@ -474,49 +524,29 @@ void HapticController::calcTorque(){
         }
 //        m_masterTgtPoses[tgt_names[i]].tm = m_qRef.tm;
         m_masterTgtPoses[tgt_names[i]].tm = m_qAct.tm;
-        m_eePosesOut[tgt_names[i]]->write();
+        m_masterTgtPosesOut[tgt_names[i]]->write();
     }
 
 
-    m_teleopOdom.data = (RTC::Pose3D){1.0,2.0,3.0,0.0,0.0,1.57* loop * 0.001};
-    m_teleopOdom.tm = m_qRef.tm;
-    m_teleopOdomOut.write();
-
 }
 
 void HapticController::processTransition(){
-    switch(mode.now()){
-        case MODE_SYNC_TO_HC:
-            if(mode.pre() == MODE_IDLE){ double tmp = 1.0; t_ip->setGoal(&tmp, 10.0, true); }
-            if (!t_ip->isEmpty() ){
-                t_ip->get(&output_ratio, true);
-            }else{
-                mode.setNextMode(MODE_HC);
-            }
-            break;
-
-        case MODE_SYNC_TO_IDLE:
-            if(mode.pre() == MODE_HC || mode.pre() == MODE_PAUSE){ double tmp = 0.0; t_ip->setGoal(&tmp, 5.0, true); }
-            if (!t_ip->isEmpty()) {
-                t_ip->get(&output_ratio, true);
-            }else{
-                mode.setNextMode(MODE_IDLE);
-            }
-            break;
-    }
-    if (!q_ref_ip->isEmpty() ){
-        q_ref_ip->get(&q_ref_output_ratio, true);
-    }
+    if(!q_ref_ip->isEmpty()     ){ q_ref_ip->get(   &q_ref_output_ratio,    true); }
+    if(!floor_h_ip->isEmpty()   ){ floor_h_ip->get( &floor_h_from_base,     true); }
+    if(!t_ip->isEmpty()         ){ t_ip->get(       &output_ratio,          true); }
+    if(mode.now() == MODE_SYNC_TO_HC    && output_ratio == 1.0){ mode.setNextMode(MODE_HC); }
+    if(mode.now() == MODE_SYNC_TO_IDLE  && output_ratio == 0.0){ mode.setNextMode(MODE_IDLE); }
 }
 
 
 bool HapticController::startHapticController(){
     if(mode.now() == MODE_IDLE){
-        RTCOUT << "startHapticController" << std::endl;
+        RTC_INFO_STREAM("startHapticController");
         mode.setNextMode(MODE_SYNC_TO_HC);
+        const double next_goal = 1.0; t_ip->setGoal(&next_goal, 10.0, true);
         return true;
     }else{
-        RTCOUT << "Invalid context to startHapticController" << std::endl;
+        RTC_WARN_STREAM("Invalid context to startHapticController");
         return false;
     }
 }
@@ -524,11 +554,11 @@ bool HapticController::startHapticController(){
 
 bool HapticController::pauseHapticController(){
     if(mode.now() == MODE_HC){
-        RTCOUT << "pauseHapticController" << std::endl;
+        RTC_INFO_STREAM("pauseHapticController");
         mode.setNextMode(MODE_PAUSE);
         return true;
     }else{
-        RTCOUT << "Invalid context to pauseHapticController" << std::endl;
+        RTC_WARN_STREAM("Invalid context to pauseHapticController");
         return false;
     }
 }
@@ -536,11 +566,11 @@ bool HapticController::pauseHapticController(){
 
 bool HapticController::resumeHapticController(){
     if(mode.now() == MODE_PAUSE){
-        RTCOUT << "resumeHapticController" << std::endl;
+        RTC_INFO_STREAM("resumeHapticController");
         mode.setNextMode(MODE_HC);
         return true;
     }else{
-        RTCOUT << "Invalid context to resumeHapticController" << std::endl;
+        RTC_WARN_STREAM("Invalid context to resumeHapticController");
         return false;
     }
 }
@@ -548,11 +578,12 @@ bool HapticController::resumeHapticController(){
 
 bool HapticController::stopHapticController(){
     if(mode.now() == MODE_HC || mode.now() == MODE_PAUSE ){
-        RTCOUT << "stopHapticController" << std::endl;
+        RTC_INFO_STREAM("stopHapticController");
         mode.setNextMode(MODE_SYNC_TO_IDLE);
+        const double next_goal = 0.0; t_ip->setGoal(&next_goal, 5.0, true);
         return true;
     }else{
-        RTCOUT << "Invalid context to stopHapticController" << std::endl;
+        RTC_WARN_STREAM("Invalid context to stopHapticController");
         return false;
     }
 }
@@ -583,6 +614,7 @@ bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticC
     hcp.q_ref_pd_gain               = hrp::to_Vector2(i_param.q_ref_pd_gain);
     ///// update process if required
     q_ref_ip->setGoal(&hcp.q_ref_output_ratio_goal, 5.0, true);
+    floor_h_ip->setGoal(&hcp.floor_height_from_base, 5.0, true);
     dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
     dqAct_filter.reset(0);
     for(int i=0;i<ee_names.size();i++){
@@ -620,8 +652,8 @@ bool HapticController::getParams(OpenHRP::HapticControllerService::HapticControl
 }
 
 
-RTC::ReturnCode_t HapticController::onActivated(RTC::UniqueId ec_id){ RTCOUT << "onActivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; }
-RTC::ReturnCode_t HapticController::onDeactivated(RTC::UniqueId ec_id){ RTCOUT << "onDeactivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; }
+RTC::ReturnCode_t HapticController::onActivated(RTC::UniqueId ec_id){ RTC_INFO_STREAM("onActivated(" << ec_id << ")"); return RTC::RTC_OK; }
+RTC::ReturnCode_t HapticController::onDeactivated(RTC::UniqueId ec_id){ RTC_INFO_STREAM("onDeactivated(" << ec_id << ")"); return RTC::RTC_OK; }
 RTC::ReturnCode_t HapticController::onFinalize(){ return RTC::RTC_OK; }
 
 extern "C"{
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index 646b0978b9e..93628a3042c 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -79,9 +79,9 @@ class HapticController : public RTC::DataFlowComponentBase{
         RTC::InPort<RTC::TimedDoubleSeq> m_qActIn;
         RTC::TimedDoubleSeq m_dqAct;
         RTC::InPort<RTC::TimedDoubleSeq> m_dqActIn;
-        std::map<std::string, RTC::TimedDoubleSeq> m_feedbackWrenches;
+        std::map<std::string, RTC::TimedDoubleSeq> m_slaveEEWrenches;
         typedef boost::shared_ptr<RTC::InPort<RTC::TimedDoubleSeq> > ITDS_Ptr;
-        std::map<std::string, ITDS_Ptr> m_feedbackWrenchesIn;
+        std::map<std::string, ITDS_Ptr> m_slaveEEWrenchesIn;
 
         RTC::TimedDoubleSeq m_tau;
         RTC::OutPort<RTC::TimedDoubleSeq> m_tauOut;
@@ -90,7 +90,7 @@ class HapticController : public RTC::DataFlowComponentBase{
         RTC::OutPort<RTC::TimedDoubleSeq> m_qOut;
         std::map<std::string, RTC::TimedPose3D> m_masterTgtPoses;
         typedef boost::shared_ptr<RTC::OutPort<RTC::TimedPose3D> > OTP3_Ptr;
-        std::map<std::string, OTP3_Ptr> m_eePosesOut;
+        std::map<std::string, OTP3_Ptr> m_masterTgtPosesOut;
         RTC::TimedDoubleSeq m_debugData;
         RTC::OutPort<RTC::TimedDoubleSeq> m_debugDataOut;
 
@@ -108,8 +108,8 @@ class HapticController : public RTC::DataFlowComponentBase{
         unsigned int m_debugLevel;
         hrp::BodyPtr m_robot;
 
-        double output_ratio, q_ref_output_ratio;
-        interpolator *t_ip, *q_ref_ip;
+        double output_ratio, q_ref_output_ratio, floor_h_from_base;
+        interpolator *t_ip, *q_ref_ip, *floor_h_ip;
 
         hrp::InvDynStateBuffer idsb;
         BiquadIIRFilterVec2 dqAct_filter;
@@ -159,8 +159,8 @@ class HapticController : public RTC::DataFlowComponentBase{
                 wrench_hpf_gain             = 1;
                 wrench_lpf_gain             = 0.2;
                 ee_pos_rot_friction_coeff   << 0, 0; // 1, 0.1
-                floor_pd_gain               << 1000, 10;
-                foot_horizontal_pd_gain     << 1000, 10;
+                floor_pd_gain               << 1000, 1;
+                foot_horizontal_pd_gain     << 1000, 1;
                 q_ref_pd_gain               << 0, 0;
             }
         } hcp;

From e07b152eae272fca4d7137ee455461fa1c49f57b Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 5 Sep 2019 21:32:52 +0900
Subject: [PATCH 170/217] update foot contact

---
 idl/HapticControllerService.idl           |  2 +-
 rtc/HapticController/HapticController.cpp | 94 +++++++++++------------
 rtc/HapticController/HapticController.h   | 26 +++----
 3 files changed, 53 insertions(+), 69 deletions(-)

diff --git a/idl/HapticControllerService.idl b/idl/HapticControllerService.idl
index 249de476111..d5359ca3676 100644
--- a/idl/HapticControllerService.idl
+++ b/idl/HapticControllerService.idl
@@ -20,7 +20,7 @@ module OpenHRP
     {
         double dqAct_filter_cutoff_hz;
         double ee_vel_filter_cutoff_hz;
-        double floor_height_from_base;
+        double baselink_height_from_floor;
         double foot_min_distance;
         double force_feedback_ratio;
         double gravity_compensation_ratio;
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index 60b254209f3..50c19a37021 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -74,11 +74,12 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     q_ref_ip->clear();
     q_ref_ip->set(&tmp_init);
     q_ref_ip->get(&q_ref_output_ratio, false);
-    const double floor_h_from_base_init = -1.5;
-    floor_h_ip = new interpolator(1, m_dt, interpolator::LINEAR, 1);
-    floor_h_ip->clear();
-    floor_h_ip->set(&floor_h_from_base_init);
-    floor_h_ip->get(&floor_h_from_base, false);
+    default_baselink_h_from_floor = 1.5;
+    hcp.baselink_height_from_floor = default_baselink_h_from_floor;
+    baselink_h_ip = new interpolator(1, m_dt, interpolator::LINEAR, 1);
+    baselink_h_ip->clear();
+    baselink_h_ip->set(&default_baselink_h_from_floor);
+    baselink_h_ip->get(&baselink_h_from_floor, false);
     RTCOUT << "setup interpolator finished" << std::endl;
 
     dqAct_filter = BiquadIIRFilterVec2(m_robot->numJoints());
@@ -315,20 +316,6 @@ void HapticController::calcTorque(){
         tgt.push_back("lleg");
         tgt.push_back("rleg");
 
-        static double out1=1,out2=1,out3=1;
-//        if(mode.isRunning()){
-//            out1 += m_dt/5;
-//            LIMIT_MAX(out1,1);
-//            if(out1 >= 1){
-//                out2 += m_dt/5;
-//                LIMIT_MAX(out2,1);
-//                if(out2>= 1){
-//                    out3 += m_dt/5;
-//                    LIMIT_MAX(out3,1);
-//                }
-//            }
-//        }
-
         ///// fix ee rot horizontal
         for (int i=0; i<tgt.size();i++){
             hrp::Vector3 diff_rot;
@@ -337,20 +324,20 @@ void HapticController::calcTorque(){
             hrp::dvector6 wrench = (hrp::dvector6()<< 0,0,0, diff_rot * hcp.foot_horizontal_pd_gain(0) + diff_rot_vel * hcp.foot_horizontal_pd_gain(1)).finished();
             LIMIT_NORM(wrench.tail(3), 50);
             hrp::dvector tq_tmp = J_ee[ee_names[i]].transpose() * wrench;
-            for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j) * out1; }
+            for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
         }
 
         ///// virtual floor
         for (int i=0; i<tgt.size();i++){
-            const double current_z = ee_pose[tgt[i]].p(Z) - m_robot->rootLink()->p(Z);
+            const double foot_h_from_floor = ee_pose[tgt[i]].p(Z);
             if(loop%1000==0){
-                dbg(current_z);
+                dbg(foot_h_from_floor);
             }
-            if(current_z < floor_h_from_base){
-                hrp::dvector6 wrench = (hrp::dvector6()<< 0,0, (floor_h_from_base - current_z)*hcp.floor_pd_gain(0) + (0-ee_vel_filtered[tgt[i]](fz))*hcp.floor_pd_gain(1), 0,0,0).finished();
+            if(foot_h_from_floor < 0){
+                hrp::dvector6 wrench = hrp::dvector6::Unit(fz) * (-foot_h_from_floor*hcp.floor_pd_gain(0) + (0-ee_vel_filtered[tgt[i]](fz))*hcp.floor_pd_gain(1));
                 LIMIT_NORM(wrench, 1000);
-                hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
-                for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j) * out2; }
+                const hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
+                for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
 
                 is_contact_to_floor[tgt[i]] = true;
             }else{
@@ -366,7 +353,7 @@ void HapticController::calcTorque(){
                 hrp::dvector6 wrench = (hrp::dvector6()<< (wall_x_rel_base - current_x) * 1000,0,0, 0,0,0).finished();
                 LIMIT_NORM(wrench, 100);
                 hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
-                for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j) * out3; }
+                for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
             }
         }
 
@@ -377,7 +364,7 @@ void HapticController::calcTorque(){
                 hrp::dvector6 wrench = (hrp::dvector6()<< 0, (tgt[i]=="lleg" ? 1:-1) * (hcp.foot_min_distance - current_dist) * 1000, 0,0,0,0).finished();
                 LIMIT_NORM(wrench, 50);
                 hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
-                for (int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j) * out2; }
+                for (int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
             }
         }
 
@@ -398,30 +385,34 @@ void HapticController::calcTorque(){
 
 
         static std::map<std::string, hrp::Vector2> leg_pos_from_floor_origin;
+        hrp::Vector2 baselink_pos_from_floor_origin;
         if(loop==0){
-            leg_pos_from_floor_origin["lleg"] << 0,  0.15;
-            leg_pos_from_floor_origin["rleg"] << 0, -0.15;
+            leg_pos_from_floor_origin["lleg"] << 0, 0.15;
+            leg_pos_from_floor_origin["rleg"] << 0,- 0.15;
+            baselink_pos_from_floor_origin << 0,0;
         }
-        if(is_contact_to_floor["lleg"]){
-            // not update
-        }else{
-            leg_pos_from_floor_origin["lleg"] = (ee_pose["lleg"].p - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];
+        if(is_contact_to_floor["lleg"] && !is_contact_to_floor["rleg"] ){ // when lleg has contact to floor, each value can be calculated from lleg
+            leg_pos_from_floor_origin["rleg"]   = (ee_pose["rleg"].p        - ee_pose["lleg"].p).head(XY) + leg_pos_from_floor_origin["lleg"];
+            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - ee_pose["lleg"].p).head(XY) + leg_pos_from_floor_origin["lleg"];
         }
-        if(is_contact_to_floor["rleg"]){
-            // not update
-        }else{
-            leg_pos_from_floor_origin["rleg"] = (ee_pose["rleg"].p - ee_pose["lleg"].p).head(XY) + leg_pos_from_floor_origin["lleg"];
+        else if(!is_contact_to_floor["lleg"] && is_contact_to_floor["rleg"] ){ // when rleg has contact to floor, each value can be calculated from rleg
+            leg_pos_from_floor_origin["lleg"]   = (ee_pose["lleg"].p        - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];
+            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];
+        }
+        else if(is_contact_to_floor["lleg"] && is_contact_to_floor["rleg"] ){ // when both has contact to floor, baselink may be calculated from rleg or lleg
+            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];// for example
+        }
+        else{ // when both in the air, both feet pos won't be update, only baselink will be update
+            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];// for example
         }
 
-        hrp::Vector2 floor_origin_from_baselink_2d = (ee_pose["rleg"].p - m_robot->rootLink()->p).head(XY) - leg_pos_from_floor_origin["rleg"];
-        hrp::Vector3 floor_origin_from_baselink = (hrp::Vector3() << floor_origin_from_baselink_2d, floor_h_from_base).finished();
+        //update base link pos from world
+        m_robot->rootLink()->p << baselink_pos_from_floor_origin, baselink_h_from_floor;
 
-        m_teleopOdom.data = (RTC::Pose3D){floor_origin_from_baselink(X),floor_origin_from_baselink(Y),floor_origin_from_baselink(Z), 0,0,0};
+        m_teleopOdom.data = (RTC::Pose3D){m_robot->rootLink()->p(X),m_robot->rootLink()->p(Y),m_robot->rootLink()->p(Z), 0,0,0};
         m_teleopOdom.tm = m_qRef.tm;
         m_teleopOdomOut.write();
 
-        m_robot->rootLink()->p = -floor_origin_from_baselink;//ヤバい
-
 
     }
 
@@ -522,8 +513,8 @@ void HapticController::calcTorque(){
         }else{
             m_masterTgtPoses[tgt_names[i]].data = hrp::to_Pose3D(ee_ikc_map[ee_names[i]].getCurrentTargetPose(m_robot));//四肢揃ってないと危険
         }
-//        m_masterTgtPoses[tgt_names[i]].tm = m_qRef.tm;
-        m_masterTgtPoses[tgt_names[i]].tm = m_qAct.tm;
+        m_masterTgtPoses[tgt_names[i]].tm = m_qRef.tm;
+//        m_masterTgtPoses[tgt_names[i]].tm = m_qAct.tm;
         m_masterTgtPosesOut[tgt_names[i]]->write();
     }
 
@@ -531,9 +522,9 @@ void HapticController::calcTorque(){
 }
 
 void HapticController::processTransition(){
-    if(!q_ref_ip->isEmpty()     ){ q_ref_ip->get(   &q_ref_output_ratio,    true); }
-    if(!floor_h_ip->isEmpty()   ){ floor_h_ip->get( &floor_h_from_base,     true); }
-    if(!t_ip->isEmpty()         ){ t_ip->get(       &output_ratio,          true); }
+    if(!q_ref_ip        ->isEmpty()){q_ref_ip       ->get( &q_ref_output_ratio,     true); }
+    if(!baselink_h_ip   ->isEmpty()){baselink_h_ip  ->get( &baselink_h_from_floor,  true); }
+    if(!t_ip            ->isEmpty()){t_ip           ->get( &output_ratio,           true); }
     if(mode.now() == MODE_SYNC_TO_HC    && output_ratio == 1.0){ mode.setNextMode(MODE_HC); }
     if(mode.now() == MODE_SYNC_TO_IDLE  && output_ratio == 0.0){ mode.setNextMode(MODE_IDLE); }
 }
@@ -581,6 +572,7 @@ bool HapticController::stopHapticController(){
         RTC_INFO_STREAM("stopHapticController");
         mode.setNextMode(MODE_SYNC_TO_IDLE);
         const double next_goal = 0.0; t_ip->setGoal(&next_goal, 5.0, true);
+        baselink_h_ip->setGoal(&default_baselink_h_from_floor, 5.0, true);
         return true;
     }else{
         RTC_WARN_STREAM("Invalid context to stopHapticController");
@@ -596,9 +588,9 @@ namespace hrp{
 
 bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param){
     RTCOUT << "setHapticControllerParam" << std::endl;
+    hcp.baselink_height_from_floor  = i_param.baselink_height_from_floor;
     hcp.dqAct_filter_cutoff_hz      = i_param.dqAct_filter_cutoff_hz;
     hcp.ee_vel_filter_cutoff_hz     = i_param.ee_vel_filter_cutoff_hz;
-    hcp.floor_height_from_base      = i_param.floor_height_from_base;
     hcp.foot_min_distance           = i_param.foot_min_distance;
     hcp.force_feedback_ratio        = i_param.force_feedback_ratio;
     hcp.gravity_compensation_ratio  = i_param.gravity_compensation_ratio;
@@ -614,7 +606,7 @@ bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticC
     hcp.q_ref_pd_gain               = hrp::to_Vector2(i_param.q_ref_pd_gain);
     ///// update process if required
     q_ref_ip->setGoal(&hcp.q_ref_output_ratio_goal, 5.0, true);
-    floor_h_ip->setGoal(&hcp.floor_height_from_base, 5.0, true);
+    baselink_h_ip->setGoal(&hcp.baselink_height_from_floor, 5.0, true);
     dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
     dqAct_filter.reset(0);
     for(int i=0;i<ee_names.size();i++){
@@ -632,9 +624,9 @@ bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticC
 
 bool HapticController::getParams(OpenHRP::HapticControllerService::HapticControllerParam& i_param){
     RTCOUT << "getHapticControllerParam" << std::endl;
+    i_param.baselink_height_from_floor  = hcp.baselink_height_from_floor;
     i_param.dqAct_filter_cutoff_hz      = hcp.dqAct_filter_cutoff_hz;
     i_param.ee_vel_filter_cutoff_hz     = hcp.ee_vel_filter_cutoff_hz;
-    i_param.floor_height_from_base      = hcp.floor_height_from_base;
     i_param.foot_min_distance           = hcp.foot_min_distance;
     i_param.force_feedback_ratio        = hcp.force_feedback_ratio;
     i_param.gravity_compensation_ratio  = hcp.gravity_compensation_ratio;
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index 93628a3042c..4c921019a2b 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -72,7 +72,6 @@ class HapticController : public RTC::DataFlowComponentBase{
         bool getParams(OpenHRP::HapticControllerService::HapticControllerParam& i_param);
 
     protected:
-
         RTC::TimedDoubleSeq m_qRef;
         RTC::InPort<RTC::TimedDoubleSeq> m_qRefIn;
         RTC::TimedDoubleSeq m_qAct;
@@ -82,7 +81,6 @@ class HapticController : public RTC::DataFlowComponentBase{
         std::map<std::string, RTC::TimedDoubleSeq> m_slaveEEWrenches;
         typedef boost::shared_ptr<RTC::InPort<RTC::TimedDoubleSeq> > ITDS_Ptr;
         std::map<std::string, ITDS_Ptr> m_slaveEEWrenchesIn;
-
         RTC::TimedDoubleSeq m_tau;
         RTC::OutPort<RTC::TimedDoubleSeq> m_tauOut;
         RTC::TimedPose3D m_teleopOdom;
@@ -93,12 +91,6 @@ class HapticController : public RTC::DataFlowComponentBase{
         std::map<std::string, OTP3_Ptr> m_masterTgtPosesOut;
         RTC::TimedDoubleSeq m_debugData;
         RTC::OutPort<RTC::TimedDoubleSeq> m_debugDataOut;
-
-//        RTC::TimedDoubleSeq m_dq_filtered_dbg;
-//        RTC::OutPort<RTC::TimedDoubleSeq> m_dq_filtered_dbgOut;
-//        std::map<std::string, RTC::TimedDoubleSeq> m_feedbackWrenches_filtered_dbg;
-//        RTC::OutPort<RTC::TimedDoubleSeq> m_dq_filtered_dbgOut;
-
         RTC::CorbaPort m_HapticControllerServicePort;
         HapticControllerService_impl m_service0;
 
@@ -108,8 +100,8 @@ class HapticController : public RTC::DataFlowComponentBase{
         unsigned int m_debugLevel;
         hrp::BodyPtr m_robot;
 
-        double output_ratio, q_ref_output_ratio, floor_h_from_base;
-        interpolator *t_ip, *q_ref_ip, *floor_h_ip;
+        double output_ratio, q_ref_output_ratio, baselink_h_from_floor, default_baselink_h_from_floor;
+        interpolator *t_ip, *q_ref_ip, *baselink_h_ip;
 
         hrp::InvDynStateBuffer idsb;
         BiquadIIRFilterVec2 dqAct_filter;
@@ -126,12 +118,12 @@ class HapticController : public RTC::DataFlowComponentBase{
         ControlMode mode;
 
         std::vector<std::string> ee_names, tgt_names;
-
         class HCParams {
             public:
+                double baselink_height_from_floor;
+                double default_baselink_h_from_floor;
                 double dqAct_filter_cutoff_hz;
                 double ee_vel_filter_cutoff_hz;
-                double floor_height_from_base;
                 double foot_min_distance;
                 double force_feedback_ratio;
                 double gravity_compensation_ratio;
@@ -146,9 +138,9 @@ class HapticController : public RTC::DataFlowComponentBase{
                 hrp::Vector2 foot_horizontal_pd_gain;
                 hrp::Vector2 q_ref_pd_gain;
             HCParams(){
-                dqAct_filter_cutoff_hz      = 100;// 10以下で確実に位相遅れによる振動
-                ee_vel_filter_cutoff_hz     = 100;// 10以下で確実に位相遅れによる振動
-                floor_height_from_base      = -1.1;
+                baselink_height_from_floor  = 1.5;// will be overwrited
+                dqAct_filter_cutoff_hz      = 500;// 10以下で確実に位相遅れによる振動
+                ee_vel_filter_cutoff_hz     = 500;// 10以下で確実に位相遅れによる振動
                 foot_min_distance           = 0.3;
                 force_feedback_ratio        = 0.2;
                 gravity_compensation_ratio  = 0.9;
@@ -159,8 +151,8 @@ class HapticController : public RTC::DataFlowComponentBase{
                 wrench_hpf_gain             = 1;
                 wrench_lpf_gain             = 0.2;
                 ee_pos_rot_friction_coeff   << 0, 0; // 1, 0.1
-                floor_pd_gain               << 1000, 1;
-                foot_horizontal_pd_gain     << 1000, 1;
+                floor_pd_gain               << 5000, 100;
+                foot_horizontal_pd_gain     << 300, 30;
                 q_ref_pd_gain               << 0, 0;
             }
         } hcp;

From 4568fd0afe58f862fa3cb4cb082618248c8b1fe8 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 6 Sep 2019 14:50:43 +0900
Subject: [PATCH 171/217] refactor

---
 .../FullbodyInverseKinematicsSolver.h         |  2 +-
 rtc/AutoBalancer/MyUtils.h                    | 17 +++--
 rtc/HapticController/HapticController.cpp     | 49 +++++++------
 .../WholeBodyMasterSlave.cpp                  | 69 +++++++++----------
 4 files changed, 70 insertions(+), 67 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 9dff1948a21..ca151df28d9 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -153,7 +153,7 @@ class FullbodyInverseKinematicsSolver{
                     dp_part.head(3) = _ikc_list[i].targetPos - m_robot->calcCM();
                     dp_part.tail(3) = (_ikc_list[i].targetRpy - cur_momentum_around_COM_filtered) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける(+フィルタ)
                     m_robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
-                    m_robot->calcAngularMomentumJacobian(NULL, J_am);//world座標系での角運動量を生成するヤコビアン(なので本当はCOM周りには使えない)
+                    m_robot->calcAngularMomentumJacobian(NULL, J_am);//base=NULLの時は重心周りの角運動量っぽい？
                     J_part << J_com, J_am;
                 }
                 else{
diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index 5e886b548e3..fea79445ff9 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -25,15 +25,20 @@ static const double Q_BUTTERWORTH = 0.707106781;
 #define dbg(var) std::cout<<#var"= "<<(var)<<std::endl
 #define dbgn(var) std::cout<<#var"= "<<std::endl<<(var)<<std::endl
 #define dbgv(var) std::cout<<#var"= "<<(var.transpose())<<std::endl
-#define RTCOUT std::cerr << "[" << m_profile.instance_name << "] "
 #define RTC_INFO_STREAM(var) std::cout << "[" << m_profile.instance_name << "] "<< var << std::endl;
 #define RTC_WARN_STREAM(var) std::cerr << "\x1b[31m[" << m_profile.instance_name << "] " << var << "\x1b[39m" << std::endl;
 
-#define LIMIT_MIN(x,min) (x= ( x<min ? min:x ))
-#define LIMIT_MAX(x,max) (x= ( x<max ? x:max ))
-#define LIMIT_MINMAX(x,min,max) ((x= (x<min  ? min : x<max ? x : max)))
-#define eps_eq(a, b, c)  (fabs((a)-(b)) <= c)
-#define LIMIT_NORM(v,max) if(v.norm()>max){ v=v.normalized()*max; }
+#define eps_eq(a, b, c)             (fabs((a)-(b)) <= c)
+#define LIMIT_NORM(x,max)           (x= ( x<(-max) ? -max : (x>max ? max : x)))
+#define LIMIT_MIN(x,min)            (x= ( x<min ? min : x ))
+#define LIMIT_MAX(x,max)            (x= ( x>max ? max : x ))
+#define LIMIT_MINMAX(x,min,max)     (x= ( x<min ? min : ( x>max ? max : x )))
+#define LIMIT_NORM_V(v,max)         if(v.norm()>max){v=v.normalized()*max;}
+#define LIMIT_MIN_V(v,minv)         (v= v.cwiseMax(minv))
+#define LIMIT_MAX_V(v,maxv)         (v= v.cwiseMin(maxv))
+#define LIMIT_MINMAX_V(v,minv,maxv) (v= v.cwiseMin(minv).cwiseMax(maxv))
+
+
 
 namespace hrp{
     class Pose3{
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index 50c19a37021..8a26377f2c6 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -35,7 +35,7 @@ HapticController::HapticController(RTC::Manager* manager) : RTC::DataFlowCompone
 HapticController::~HapticController(){}
 
 RTC::ReturnCode_t HapticController::onInitialize(){
-    RTCOUT << "onInitialize()" << std::endl;
+    RTC_INFO_STREAM("onInitialize()");
     bindParameter("debugLevel", m_debugLevel, "0");
     addInPort("qRef", m_qRefIn);
     addInPort("qAct", m_qActIn);
@@ -58,12 +58,12 @@ RTC::ReturnCode_t HapticController::onInitialize(){
 
     m_robot = hrp::BodyPtr(new hrp::Body());
     if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
-        RTCOUT << "failed to load model[" << prop["model"] << "]" << std::endl;
+        RTC_WARN_STREAM("failed to load model[" << prop["model"] << "]");
         return RTC::RTC_ERROR;
     }
 
     setupEEIKConstraintFromConf(ee_ikc_map, m_robot, prop);
-    RTCOUT << "setupEEIKConstraintFromConf finished" << std::endl;
+    RTC_INFO_STREAM("setupEEIKConstraintFromConf finished");
 
     const double tmp_init = 0;
     t_ip = new interpolator(1, m_dt, interpolator::LINEAR, 1);
@@ -80,7 +80,7 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     baselink_h_ip->clear();
     baselink_h_ip->set(&default_baselink_h_from_floor);
     baselink_h_ip->get(&baselink_h_from_floor, false);
-    RTCOUT << "setup interpolator finished" << std::endl;
+    RTC_INFO_STREAM("setup interpolator finished");
 
     dqAct_filter = BiquadIIRFilterVec2(m_robot->numJoints());
     dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
@@ -102,7 +102,7 @@ RTC::ReturnCode_t HapticController::onInitialize(){
         m_slaveEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee_names[i]]));
         registerInPort(n.c_str(), *m_slaveEEWrenchesIn[ee_names[i]]);
         m_slaveEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
-        RTCOUT << " registerInPort " << n << std::endl;
+        RTC_INFO_STREAM(" registerInPort " << n );
     }
 
     tgt_names = ee_names;
@@ -112,13 +112,13 @@ RTC::ReturnCode_t HapticController::onInitialize(){
         std::string n = "master_"+tgt_names[i]+"_pose_out";
         m_masterTgtPosesOut[tgt_names[i]] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
         registerOutPort(n.c_str(), *m_masterTgtPosesOut[tgt_names[i]]);
-        RTCOUT << " registerOutPort " << n << std::endl;
+        RTC_INFO_STREAM(" registerOutPort " << n);
     }
 
     m_tau.data = hrp::to_DoubleSeq(hrp::dvector::Zero(m_robot->numJoints()));
 
     loop = 0;
-    RTCOUT << "onInitialize() OK" << std::endl;
+    RTC_INFO_STREAM("onInitialize() OK");
     return RTC::RTC_OK;
 }
 
@@ -140,13 +140,13 @@ RTC::ReturnCode_t HapticController::setupEEIKConstraintFromConf(std::map<std::st
             _ee_ikc_map[ee_name].localR = Eigen::AngleAxis<double>(tmp_aa(3), tmp_aa.head(3)).toRotationMatrix(); // rotation in VRML is represented by axis + angle
             jpath_ee[ee_name] = hrp::JointPath(m_robot->rootLink(), m_robot->link(target_link_name));
             if(_robot->link(target_link_name)){
-              RTCOUT << "End Effector [" << ee_name << "]" << std::endl;
-              RTCOUT << "   target_link_name = " << _ee_ikc_map[ee_name].target_link_name << ", base = " << base_name << std::endl;
-              RTCOUT << "   offset_pos = " << _ee_ikc_map[ee_name].localPos.transpose() << "[m]" << std::endl;
-              RTCOUT << "   has_toe_joint = " << "fix to false now" << std::endl;
+                RTC_INFO_STREAM("End Effector [" << ee_name << "]");
+                RTC_INFO_STREAM("   target_link_name = " << _ee_ikc_map[ee_name].target_link_name << ", base = " << base_name);
+                RTC_INFO_STREAM("   offset_pos = " << _ee_ikc_map[ee_name].localPos.transpose() << "[m]");
+                RTC_INFO_STREAM("   has_toe_joint = " << "fix to false now");
             }else{
-              RTCOUT << "Target link [" << target_link_name << "] not found !" << std::endl;
-              return RTC::RTC_ERROR;
+                RTC_WARN_STREAM("Target link [" << target_link_name << "] not found !");
+                return RTC::RTC_ERROR;
             }
         }
     }
@@ -154,7 +154,7 @@ RTC::ReturnCode_t HapticController::setupEEIKConstraintFromConf(std::map<std::st
 }
 
 RTC::ReturnCode_t HapticController::onExecute(RTC::UniqueId ec_id){
-    if(DEBUGP_ONCE) RTCOUT << "onExecute(" << ec_id << ") dt = " << m_dt << std::endl;
+    if(DEBUGP_ONCE) RTC_INFO_STREAM("onExecute(" << ec_id << ") dt = " << m_dt);
     if (m_qRefIn.isNew())   { m_qRefIn.read(); }
     if (m_qActIn.isNew())   { m_qActIn.read(); }
     if (m_dqActIn.isNew())  { m_dqActIn.read(); }
@@ -322,7 +322,7 @@ void HapticController::calcTorque(){
             rats::difference_rotation(diff_rot, ee_pose[tgt[i]].R, hrp::Matrix33::Identity());
             const hrp::Vector3 diff_rot_vel = hrp::Vector3::Zero() - ee_vel_filtered[tgt[i]].tail(3);
             hrp::dvector6 wrench = (hrp::dvector6()<< 0,0,0, diff_rot * hcp.foot_horizontal_pd_gain(0) + diff_rot_vel * hcp.foot_horizontal_pd_gain(1)).finished();
-            LIMIT_NORM(wrench.tail(3), 50);
+            LIMIT_NORM_V(wrench.tail(3), 50);
             hrp::dvector tq_tmp = J_ee[ee_names[i]].transpose() * wrench;
             for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
         }
@@ -335,7 +335,7 @@ void HapticController::calcTorque(){
             }
             if(foot_h_from_floor < 0){
                 hrp::dvector6 wrench = hrp::dvector6::Unit(fz) * (-foot_h_from_floor*hcp.floor_pd_gain(0) + (0-ee_vel_filtered[tgt[i]](fz))*hcp.floor_pd_gain(1));
-                LIMIT_NORM(wrench, 1000);
+                LIMIT_NORM_V(wrench, 1000);
                 const hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
                 for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
 
@@ -351,7 +351,7 @@ void HapticController::calcTorque(){
             const double current_x = ee_pose[tgt[i]].p(X) - m_robot->rootLink()->p(X);
             if(current_x < wall_x_rel_base){
                 hrp::dvector6 wrench = (hrp::dvector6()<< (wall_x_rel_base - current_x) * 1000,0,0, 0,0,0).finished();
-                LIMIT_NORM(wrench, 100);
+                LIMIT_NORM_V(wrench, 100);
                 hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
                 for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
             }
@@ -362,7 +362,7 @@ void HapticController::calcTorque(){
         if(current_dist < hcp.foot_min_distance){
             for (int i=0; i<tgt.size();i++){
                 hrp::dvector6 wrench = (hrp::dvector6()<< 0, (tgt[i]=="lleg" ? 1:-1) * (hcp.foot_min_distance - current_dist) * 1000, 0,0,0,0).finished();
-                LIMIT_NORM(wrench, 50);
+                LIMIT_NORM_V(wrench, 50);
                 hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
                 for (int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
             }
@@ -375,7 +375,7 @@ void HapticController::calcTorque(){
                 hrp::Vector3 cur_rel_pos = ee_pose["rleg"].p - ee_pose["lleg"].p;
                 hrp::Vector2 rleg_wrench = (locked_rel_pos - cur_rel_pos).head(XY) * 1000;
                 hrp::dvector6 wrench = (hrp::dvector6()<< (tgt[i]=="rleg" ? 1:-1) * rleg_wrench, 0,0,0,0).finished();
-                LIMIT_NORM(wrench, 1000);
+                LIMIT_NORM_V(wrench, 1000);
                 hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
                 for (int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
             }
@@ -403,7 +403,7 @@ void HapticController::calcTorque(){
             baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];// for example
         }
         else{ // when both in the air, both feet pos won't be update, only baselink will be update
-            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];// for example
+            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];// for example
         }
 
         //update base link pos from world
@@ -426,8 +426,8 @@ void HapticController::calcTorque(){
         wrench_shaped[ee_names[i]] = w_hpf * hcp.wrench_hpf_gain + w_lpf * hcp.wrench_lpf_gain;
 
         wrench_used[ee_names[i]] = wrench_shaped[ee_names[i]];
-        LIMIT_NORM(wrench_used[ee_names[i]].head(3),50);
-        LIMIT_NORM(wrench_used[ee_names[i]].tail(3),5);
+        LIMIT_NORM_V(wrench_used[ee_names[i]].head(3),50);
+        LIMIT_NORM_V(wrench_used[ee_names[i]].tail(3),5);
 
         hrp::Vector3 friction_f = - hcp.ee_pos_rot_friction_coeff(0) * ee_vel_filtered[ee_names[i]].head(3);
         hrp::Vector3 friction_t = - hcp.ee_pos_rot_friction_coeff(1) * ee_vel_filtered[ee_names[i]].tail(3);
@@ -587,7 +587,7 @@ namespace hrp{
 }
 
 bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param){
-    RTCOUT << "setHapticControllerParam" << std::endl;
+    RTC_INFO_STREAM("setHapticControllerParam");
     hcp.baselink_height_from_floor  = i_param.baselink_height_from_floor;
     hcp.dqAct_filter_cutoff_hz      = i_param.dqAct_filter_cutoff_hz;
     hcp.ee_vel_filter_cutoff_hz     = i_param.ee_vel_filter_cutoff_hz;
@@ -617,13 +617,12 @@ bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticC
         wrench_lpf[ee_names[i]].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
         wrench_lpf[ee_names[i]].reset(0);
     }
-
     return true;
 }
 
 
 bool HapticController::getParams(OpenHRP::HapticControllerService::HapticControllerParam& i_param){
-    RTCOUT << "getHapticControllerParam" << std::endl;
+    RTC_INFO_STREAM("getHapticControllerParam");
     i_param.baselink_height_from_floor  = hcp.baselink_height_from_floor;
     i_param.dqAct_filter_cutoff_hz      = hcp.dqAct_filter_cutoff_hz;
     i_param.ee_vel_filter_cutoff_hz     = hcp.ee_vel_filter_cutoff_hz;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 354d5b8ba5c..9e4fc20cfbc 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -45,7 +45,7 @@ WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlo
 WholeBodyMasterSlave::~WholeBodyMasterSlave(){}
 
 RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
-    RTCOUT << "onInitialize()" << std::endl;
+    RTC_INFO_STREAM("onInitialize()");
     bindParameter("debugLevel", m_debugLevel, "0");
     addInPort("qRef", m_qRefIn);// from sh
     addInPort("qAct", m_qActIn);
@@ -81,17 +81,17 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
 
     hrp::BodyPtr robot_for_ik = hrp::BodyPtr(new hrp::Body());
     if (!loadBodyFromModelLoader(robot_for_ik, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){
-        RTCOUT << "failed to load model[" << prop["model"] << "]" << std::endl;
+        RTC_WARN_STREAM("failed to load model[" << prop["model"] << "]");
         return RTC::RTC_ERROR;
     }
     m_robot_act = hrp::BodyPtr(new hrp::Body(*robot_for_ik)); //copy
     m_robot_vsafe = hrp::BodyPtr(new hrp::Body(*robot_for_ik)); //copy
-    RTCOUT << "setup robot model finished" << std::endl;
+    RTC_INFO_STREAM("setup robot model finished");
 
     fik = fikPtr(new FullbodyInverseKinematicsSolver(robot_for_ik, std::string(m_profile.instance_name), m_dt));
     setupEEIKConstraintFromConf(ee_ikc_map, robot_for_ik, prop);
     is_legged_robot = (ee_ikc_map.find("rleg") != ee_ikc_map.end() && ee_ikc_map.find("lleg") != ee_ikc_map.end());
-    RTCOUT << "setup fullbody ik finished" << std::endl;
+    RTC_INFO_STREAM("setup fullbody ik finished");
 
     wbms = boost::shared_ptr<WBMSCore>(new WBMSCore(m_dt));
     if(fik->m_robot->name() == "RHP4B"){
@@ -105,11 +105,11 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     }
 
     if(fik->m_robot->name().find("JAXON") != std::string::npos){ // for demo
-//        wbms->wp.use_targets.push_back("rleg");
-//        wbms->wp.use_targets.push_back("lleg");
+        wbms->wp.use_targets.push_back("rleg");
+        wbms->wp.use_targets.push_back("lleg");
         wbms->wp.use_targets.push_back("rarm");
         wbms->wp.use_targets.push_back("larm");
-//        wbms->wp.use_targets.push_back("com");
+        wbms->wp.use_targets.push_back("com");
         wbms->wp.use_targets.push_back("head");
     }else{
         wbms->wp.use_targets.push_back("rleg");
@@ -120,7 +120,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
         wbms->wp.use_targets.push_back("head");
     }
     sccp = boost::shared_ptr<CapsuleCollisionChecker>(new CapsuleCollisionChecker(fik->m_robot));
-    RTCOUT << "setup main function class finished" << std::endl;
+    RTC_INFO_STREAM("setup main function class finished");
 
     // allocate memory for outPorts
     m_qRef.data.length(fik->m_robot->numJoints()); // is really needed?
@@ -140,9 +140,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     avg_q_acc = hrp::dvector::Constant(fik->numStates(), 16.0); // all joint max avarage acc = 16.0 rad/s^2
     avg_q_vel.tail(6).fill(std::numeric_limits<double>::max()); // no limit for base link vel
     avg_q_acc.tail(6).fill(std::numeric_limits<double>::max()); // no limit for base link acc
-    RTCOUT << "setup interpolator finished" << std::endl;
+    RTC_INFO_STREAM("setup interpolator finished");
 
-    //    invdyn_zmp_filters.setParameter(25, 1/m_dt, Q_BUTTERWORTH);
     ref_zmp_filter.setParameter(5, 1/m_dt, Q_BUTTERWORTH);
 
     cp_flag = -1;
@@ -160,13 +159,13 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
         std::string n = "slave_"+ee_names[i]+"_wrench_out";
         m_slaveEEWrenchesOut[ee_names[i]] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee_names[i]]));
         registerOutPort(n.c_str(), *m_slaveEEWrenchesOut[ee_names[i]]);
-        RTCOUT << " registerOutPort " << n << std::endl;
+        RTC_INFO_STREAM(" registerOutPort " << n);
     }
     for ( int i=0; i<ee_names.size(); i++) {
         std::string n = "local_"+ee_names[i]+"_wrench_in";
         m_localEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_localEEWrenches[ee_names[i]]));
         registerInPort(n.c_str(), *m_localEEWrenchesIn[ee_names[i]]);
-        RTCOUT << " registerInPort " << n << std::endl;
+        RTC_INFO_STREAM(" registerInPort " << n);
     }
 
     tgt_names = ee_names;
@@ -176,10 +175,10 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
         std::string n = "master_"+tgt_names[i]+"_pose_in";
         m_masterTgtPosesIn[tgt_names[i]] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
         registerInPort(n.c_str(), *m_masterTgtPosesIn[tgt_names[i]]);
-        RTCOUT << " registerInPort " << n << std::endl;
+        RTC_INFO_STREAM(" registerInPort " << n);
     }
 
-    RTCOUT << "onInitialize() OK" << std::endl;
+    RTC_INFO_STREAM("onInitialize() OK");
     loop = 0;
     return RTC::RTC_OK;
 }
@@ -201,13 +200,13 @@ RTC::ReturnCode_t WholeBodyMasterSlave::setupEEIKConstraintFromConf(std::map<std
             for (int j = 0; j < 4; j++ ){ coil::stringTo(tmp_aa[j], ee_conf_all[i*prop_num+6+j].c_str()); }
             _ee_ikc_map[ee_name].localR = Eigen::AngleAxis<double>(tmp_aa[3], hrp::Vector3(tmp_aa[0], tmp_aa[1], tmp_aa[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
             if(_robot->link(target_link_name)){
-              RTCOUT << "End Effector [" << ee_name << "]" << std::endl;
-              RTCOUT << "   target_link_name = " << _ee_ikc_map[ee_name].target_link_name << ", base = " << base_name << std::endl;
-              RTCOUT << "   offset_pos = " << _ee_ikc_map[ee_name].localPos.transpose() << "[m]" << std::endl;
-              RTCOUT << "   has_toe_joint = " << "fix to false now" << std::endl;
+                RTC_INFO_STREAM("End Effector [" << ee_name << "]");
+                RTC_INFO_STREAM("   target_link_name = " << _ee_ikc_map[ee_name].target_link_name << ", base = " << base_name);
+                RTC_INFO_STREAM("   offset_pos = " << _ee_ikc_map[ee_name].localPos.transpose() << "[m]");
+                RTC_INFO_STREAM("   has_toe_joint = " << "fix to false now");
             }else{
-              RTCOUT << "Target link [" << target_link_name << "] not found !" << std::endl;
-              return RTC::RTC_ERROR;
+                RTC_WARN_STREAM("Target link [" << target_link_name << "] not found !");
+                return RTC::RTC_ERROR;
             }
             contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));////TODO:要移動? //used for check optional data order
         }
@@ -216,7 +215,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::setupEEIKConstraintFromConf(std::map<std
 }
 
 RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
-    if(DEBUGP_ONCE) RTCOUT << "onExecute(" << ec_id << ")" << std::endl;
+    if(DEBUGP_ONCE) RTC_INFO_STREAM("onExecute(" << ec_id << ")");
     time_report_str.clear();
     clock_gettime(CLOCK_REALTIME, &startT);
     if (m_qRefIn.isNew()) { m_qRefIn.read(); }
@@ -369,7 +368,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     m_zmpOut.write();
     m_optionalDataOut.write();
     addTimeReport("OutPort");
-    if(DEBUGP) RTCOUT << time_report_str << std::endl;
+    if(DEBUGP) RTC_INFO_STREAM(time_report_str);
     loop ++;
     return RTC::RTC_OK;
 }
@@ -770,11 +769,11 @@ void WholeBodyMasterSlave::smoothingJointAngles(hrp::BodyPtr _robot, hrp::BodyPt
 
 bool WholeBodyMasterSlave::startWholeBodyMasterSlave(){
     if(mode.now() == MODE_IDLE){
-        RTCOUT << "startWholeBodyMasterSlave" << std::endl;
+        RTC_INFO_STREAM("startWholeBodyMasterSlave");
         mode.setNextMode(MODE_SYNC_TO_HC);
         return true;
     }else{
-        RTCOUT << "Invalid context to startWholeBodyMasterSlave" << std::endl;
+        RTC_WARN_STREAM("Invalid context to startWholeBodyMasterSlave");
         return false;
     }
 }
@@ -782,11 +781,11 @@ bool WholeBodyMasterSlave::startWholeBodyMasterSlave(){
 
 bool WholeBodyMasterSlave::pauseWholeBodyMasterSlave(){
     if(mode.now() == MODE_WBMS){
-        RTCOUT << "pauseWholeBodyMasterSlave" << std::endl;
+        RTC_INFO_STREAM("pauseWholeBodyMasterSlave");
         mode.setNextMode(MODE_PAUSE);
         return true;
     }else{
-        RTCOUT << "Invalid context to pauseWholeBodyMasterSlave" << std::endl;
+        RTC_WARN_STREAM("Invalid context to pauseWholeBodyMasterSlave");
         return false;
     }
 }
@@ -794,11 +793,11 @@ bool WholeBodyMasterSlave::pauseWholeBodyMasterSlave(){
 
 bool WholeBodyMasterSlave::resumeWholeBodyMasterSlave(){
     if(mode.now() == MODE_PAUSE){
-        RTCOUT << "resumeWholeBodyMasterSlave" << std::endl;
+        RTC_INFO_STREAM("resumeWholeBodyMasterSlave");
         mode.setNextMode(MODE_WBMS);
         return true;
     }else{
-        RTCOUT << "Invalid context to resumeWholeBodyMasterSlave" << std::endl;
+        RTC_WARN_STREAM("Invalid context to resumeWholeBodyMasterSlave");
         return false;
     }
 }
@@ -806,18 +805,18 @@ bool WholeBodyMasterSlave::resumeWholeBodyMasterSlave(){
 
 bool WholeBodyMasterSlave::stopWholeBodyMasterSlave(){
     if(mode.now() == MODE_WBMS || mode.now() == MODE_PAUSE ){
-        RTCOUT << "stopWholeBodyMasterSlave" << std::endl;
+        RTC_INFO_STREAM("stopWholeBodyMasterSlave");
         mode.setNextMode(MODE_SYNC_TO_IDLE);
         return true;
     }else{
-        RTCOUT << "Invalid context to stopWholeBodyMasterSlave" << std::endl;
+        RTC_WARN_STREAM("Invalid context to stopWholeBodyMasterSlave");
         return false;
     }
 }
 
 
 bool WholeBodyMasterSlave::setParams(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
-    RTCOUT << "setWholeBodyMasterSlaveParam" << std::endl;
+    RTC_INFO_STREAM("setWholeBodyMasterSlaveParam");
     wbms->wp.auto_swing_foot_landing_threshold  = i_param.auto_swing_foot_landing_threshold;
     wbms->wp.human_to_robot_ratio               = i_param.human_to_robot_ratio;
     wbms->wp.set_com_height_fix                 = i_param.set_com_height_fix;
@@ -831,14 +830,14 @@ bool WholeBodyMasterSlave::setParams(const OpenHRP::WholeBodyMasterSlaveService:
         wbms->wp.use_joints                     = hrp::to_string_vector(i_param.use_joints);
         wbms->wp.use_targets                    = hrp::to_string_vector(i_param.use_targets);
     }else{
-      RTCOUT << "use_head, use_upper, use_lower can be changed in MODE_IDLE" << std::endl;
+        RTC_WARN_STREAM("use_head, use_upper, use_lower can be changed in MODE_IDLE");
     }
     return true;
 }
 
 
 bool WholeBodyMasterSlave::getParams(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
-    RTCOUT << "getWholeBodyMasterSlaveParam" << std::endl;
+    RTC_INFO_STREAM("getWholeBodyMasterSlaveParam");
     i_param.auto_swing_foot_landing_threshold   = wbms->wp.auto_swing_foot_landing_threshold;
     i_param.human_to_robot_ratio                = wbms->wp.human_to_robot_ratio;
     i_param.set_com_height_fix                  = wbms->wp.set_com_height_fix;
@@ -854,8 +853,8 @@ bool WholeBodyMasterSlave::getParams(OpenHRP::WholeBodyMasterSlaveService::Whole
 }
 
 
-RTC::ReturnCode_t WholeBodyMasterSlave::onActivated(RTC::UniqueId ec_id){ RTCOUT << "onActivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; }
-RTC::ReturnCode_t WholeBodyMasterSlave::onDeactivated(RTC::UniqueId ec_id){ RTCOUT << "onDeactivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; }
+RTC::ReturnCode_t WholeBodyMasterSlave::onActivated(RTC::UniqueId ec_id){ RTC_INFO_STREAM("onActivated(" << ec_id << ")"); return RTC::RTC_OK; }
+RTC::ReturnCode_t WholeBodyMasterSlave::onDeactivated(RTC::UniqueId ec_id){ RTC_INFO_STREAM("onDeactivated(" << ec_id << ")"); return RTC::RTC_OK; }
 RTC::ReturnCode_t WholeBodyMasterSlave::onFinalize(){ return RTC::RTC_OK; }
 
 extern "C"{

From 350d386077365f0784620260a9c22b48e9d81916 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 6 Sep 2019 19:43:28 +0900
Subject: [PATCH 172/217] up

---
 python/hrpsys_config.py                       |   8 +-
 .../FullbodyInverseKinematicsSolver.h         |  27 +--
 rtc/HapticController/HapticController.cpp     |   4 +-
 .../WholeBodyMasterSlave.cpp                  | 217 +++++-------------
 .../WholeBodyMasterSlave.h                    |   1 -
 5 files changed, 74 insertions(+), 183 deletions(-)

diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index ace7347409b..d9c0e50f633 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -397,10 +397,10 @@ def connectComps(self):
             connectPorts(self.sh.port("basePosOut"), self.wbms.port("basePosIn"))
             connectPorts(self.sh.port("baseRpyOut"), self.wbms.port("baseRpyIn"))
             connectPorts(self.sh.port("optionalDataOut"), self.wbms.port("optionalData"))
-            connectPorts(self.rmfo.port("off_lfsensor"), self.wbms.port("local_lleg_wrench_in"))
-            connectPorts(self.rmfo.port("off_rfsensor"), self.wbms.port("local_rleg_wrench_in"))
-            connectPorts(self.rmfo.port("off_lhsensor"), self.wbms.port("local_larm_wrench_in"))
-            connectPorts(self.rmfo.port("off_rhsensor"), self.wbms.port("local_rarm_wrench_in"))
+            connectPorts(self.rmfo.port("off_lfsensor"), self.wbms.port("local_lleg_wrench"))
+            connectPorts(self.rmfo.port("off_rfsensor"), self.wbms.port("local_rleg_wrench"))
+            connectPorts(self.rmfo.port("off_lhsensor"), self.wbms.port("local_larm_wrench"))
+            connectPorts(self.rmfo.port("off_rhsensor"), self.wbms.port("local_rarm_wrench"))
 
         # connection for st
         if rtm.findPort(self.rh.ref, "lfsensor") and rtm.findPort(
diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index ca151df28d9..75c51f5318e 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -147,7 +147,7 @@ class FullbodyInverseKinematicsSolver{
                     tgt_jpath.calcJacobian(J_jpath, _ikc_list[i].localPos);
                     for(int id_in_jpath=0; id_in_jpath<tgt_jpath.numJoints(); id_in_jpath++){ J_part.col(tgt_jpath.joint(id_in_jpath)->jointId) = J_jpath.col(id_in_jpath); } //ジョイントパスのJacobianを全身用に並び替え
                     J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF,  BASE_DOF );
-                    J_part.rightCols(BASE_DOF).topRightCorner(3,3) = hrp::hat(link_tgt_ptr->p - m_robot->rootLink()->p);
+                    J_part.rightCols(BASE_DOF).topRightCorner(3,3) = - hrp::hat(tgt_cur_pos - m_robot->rootLink()->p);
                 }
                 else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){ //重心限定
                     dp_part.head(3) = _ikc_list[i].targetPos - m_robot->calcCM();
@@ -164,7 +164,7 @@ class FullbodyInverseKinematicsSolver{
                 if(c_part_selection_mat.rows() != 0 && c_part_selection_mat.cols() != 0 ){
 
 
-                    const double dp_max = 0.1;
+                    const double dp_max = 0.1;// ???????
 
                     J_all.middleRows                (CURRENT_C_COUNT, c_part_selection_mat.rows()) = c_part_selection_mat * J_part * q_select_mat.transpose();
                     err_all.segment                 (CURRENT_C_COUNT, c_part_selection_mat.rows()) = c_part_selection_mat * dp_part.cwiseMin(hrp::dvector6::Constant(dp_max)).cwiseMax(hrp::dvector6::Constant(-dp_max));
@@ -221,8 +221,8 @@ class FullbodyInverseKinematicsSolver{
 
             // rtconf localhost:15005/wbms.rtc set debugLevel 1 とかにしたい
             static int count;
+#if 0
             if(count++ % 10000 == 0){
-//                if(true){
                 std::cout<<std::setprecision(2) << "J=\n"<<J_all<<std::setprecision(6)<<std::endl;
                 dbg(J_all.rows());
                 dbg(J_all.cols());
@@ -249,7 +249,7 @@ class FullbodyInverseKinematicsSolver{
                 dbgv(hrp::getRobotStateVec(m_robot));
                 std::cout<<std::endl;
             }
-
+#endif
             // update joint angles
             for(int i=0;i<dq_all.rows();i++){ if( isnan(dq_all(i)) || isinf(dq_all(i)) ){ std::cerr <<"[FullbodyIK] ERROR nan/inf is found" << std::endl; return;} }
             dq_all = q_select_mat.transpose() * dq_all;
@@ -258,25 +258,20 @@ class FullbodyInverseKinematicsSolver{
                 LIMIT_MINMAX(m_robot->joint(i)->q, m_robot->joint(i)->llimit, m_robot->joint(i)->ulimit);
             }
 
-            // update rootlink pos rot
-            m_robot->rootLink()->p += dq_all.tail(6).head(3);
+            // rootlink rpy limit ???
             for(int i=0;i<3;i++){
                 if(hrp::rpyFromRot(m_robot->rootLink()->R)(i) < rootlink_rpy_llimit(i) && dq_all.tail(6).tail(3)(i) < 0) dq_all.tail(6).tail(3)(i) = 0;
                 if(hrp::rpyFromRot(m_robot->rootLink()->R)(i) > rootlink_rpy_ulimit(i) && dq_all.tail(6).tail(3)(i) > 0) dq_all.tail(6).tail(3)(i) = 0;
             }
 
+            // update rootlink pos rot
+            m_robot->rootLink()->p += dq_all.tail(6).head(3);
             hrp::Matrix33 dR;
-            hrp::Vector3 omega = dq_all.tail(6).tail(3);
+            const hrp::Vector3 omega = dq_all.tail(6).tail(3);
             hrp::calcRodrigues(dR, omega.normalized(), omega.norm());
-            hrp::Matrix33 R_base_ans = m_robot->rootLink()->R * dR;
-            if(!R_base_ans.isUnitary()){
-                std::cerr <<"[FullbodyIK] WARN R_base_ans is not Unitary, normalize via Quaternion" << std::endl;
-                Eigen::Quaternion<double> quat(R_base_ans);
-                quat.normalize();
-                R_base_ans = quat.toRotationMatrix();
-                return;
-            }else{
-                m_robot->rootLink()->R = R_base_ans;
+            rats::rotm3times(m_robot->rootLink()->R, dR, m_robot->rootLink()->R); // safe rot operation with quartanion normalization
+            if(!m_robot->rootLink()->R.isUnitary()){
+                std::cerr <<"[FullbodyIK] WARN m_robot->rootLink()->R is not Unitary, something wrong !" << std::endl;
             }
             m_robot->calcForwardKinematics();
             #else
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index 8a26377f2c6..078d9c4ba04 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -98,7 +98,7 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     }
 
     for ( int i=0; i<ee_names.size(); i++) {
-        std::string n = "slave_"+ee_names[i]+"_wrench_in";
+        std::string n = "slave_"+ee_names[i]+"_wrench";
         m_slaveEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee_names[i]]));
         registerInPort(n.c_str(), *m_slaveEEWrenchesIn[ee_names[i]]);
         m_slaveEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
@@ -109,7 +109,7 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     tgt_names.push_back("com");
     tgt_names.push_back("head");
     for ( int i=0; i<tgt_names.size(); i++) {
-        std::string n = "master_"+tgt_names[i]+"_pose_out";
+        std::string n = "master_"+tgt_names[i]+"_pose";
         m_masterTgtPosesOut[tgt_names[i]] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
         registerOutPort(n.c_str(), *m_masterTgtPosesOut[tgt_names[i]]);
         RTC_INFO_STREAM(" registerOutPort " << n);
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 9e4fc20cfbc..37ce956b3ea 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -90,7 +90,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
 
     fik = fikPtr(new FullbodyInverseKinematicsSolver(robot_for_ik, std::string(m_profile.instance_name), m_dt));
     setupEEIKConstraintFromConf(ee_ikc_map, robot_for_ik, prop);
-    is_legged_robot = (ee_ikc_map.find("rleg") != ee_ikc_map.end() && ee_ikc_map.find("lleg") != ee_ikc_map.end());
     RTC_INFO_STREAM("setup fullbody ik finished");
 
     wbms = boost::shared_ptr<WBMSCore>(new WBMSCore(m_dt));
@@ -156,13 +155,13 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     to_enum["head"] = head;
 
     for ( int i=0; i<ee_names.size(); i++) {
-        std::string n = "slave_"+ee_names[i]+"_wrench_out";
+        std::string n = "slave_"+ee_names[i]+"_wrench";
         m_slaveEEWrenchesOut[ee_names[i]] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee_names[i]]));
         registerOutPort(n.c_str(), *m_slaveEEWrenchesOut[ee_names[i]]);
         RTC_INFO_STREAM(" registerOutPort " << n);
     }
     for ( int i=0; i<ee_names.size(); i++) {
-        std::string n = "local_"+ee_names[i]+"_wrench_in";
+        std::string n = "local_"+ee_names[i]+"_wrench";
         m_localEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_localEEWrenches[ee_names[i]]));
         registerInPort(n.c_str(), *m_localEEWrenchesIn[ee_names[i]]);
         RTC_INFO_STREAM(" registerInPort " << n);
@@ -172,7 +171,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     tgt_names.push_back("com");
     tgt_names.push_back("head");
     for ( int i=0; i<tgt_names.size(); i++) {
-        std::string n = "master_"+tgt_names[i]+"_pose_in";
+        std::string n = "master_"+tgt_names[i]+"_pose";
         m_masterTgtPosesIn[tgt_names[i]] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
         registerInPort(n.c_str(), *m_masterTgtPosesIn[tgt_names[i]]);
         RTC_INFO_STREAM(" registerInPort " << n);
@@ -275,35 +274,34 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     }
     addTimeReport("InPort");
 
-    if ( is_legged_robot ) {
-        processTransition();
-        mode.update();
+    processTransition();
+    mode.update();
 
-        if (mode.isRunning()) {
-            if(mode.isInitialize()){
-                preProcessForWholeBodyMasterSlave();
-                idsb.setInitState(fik->m_robot, m_dt);//逆動力学初期化
-            }
-            wbms->update();//////HumanSynchronizerの主要処理
-            if(DEBUGP)wbms->rp_ref_out.print();
-            addTimeReport("MainFunc");
-
-            solveFullbodyIK(wbms->rp_ref_out.tgt[com].abs, wbms->rp_ref_out.tgt[rf].abs, wbms->rp_ref_out.tgt[lf].abs, wbms->rp_ref_out.tgt[rh].abs, wbms->rp_ref_out.tgt[lh].abs, wbms->rp_ref_out.tgt[head].abs);
-            addTimeReport("IK");
-
-            smoothingJointAngles(fik->m_robot, m_robot_vsafe);
-
-            hrp::Vector3 com = m_robot_vsafe->calcCM();
-            static hrp::Vector3 com_old = com;
-            static hrp::Vector3 com_old_old = com_old;
-            hrp::Vector3 com_acc = (com - 2*com_old + com_old_old)/(m_dt*m_dt);
-            hrp::Vector3 ref_zmp; ref_zmp << com.head(XY)-(com(Z)/G)*com_acc.head(XY), 0;
-            if(mode.isInitialize()){ ref_zmp_filter.reset(ref_zmp); }
-            ref_zmp = ref_zmp_filter.passFilter(ref_zmp);
-            com_old_old = com_old;
-            com_old = com;
-            wbms->act_rs.com = com;
-            wbms->act_rs.zmp = ref_zmp;
+    if (mode.isRunning()) {
+        if(mode.isInitialize()){
+            preProcessForWholeBodyMasterSlave();
+            idsb.setInitState(fik->m_robot, m_dt);//逆動力学初期化
+        }
+        wbms->update();//////HumanSynchronizerの主要処理
+        if(DEBUGP)wbms->rp_ref_out.print();
+        addTimeReport("MainFunc");
+
+        solveFullbodyIK(wbms->rp_ref_out.tgt[com].abs, wbms->rp_ref_out.tgt[rf].abs, wbms->rp_ref_out.tgt[lf].abs, wbms->rp_ref_out.tgt[rh].abs, wbms->rp_ref_out.tgt[lh].abs, wbms->rp_ref_out.tgt[head].abs);
+        addTimeReport("IK");
+
+        smoothingJointAngles(fik->m_robot, m_robot_vsafe);
+
+        hrp::Vector3 com = m_robot_vsafe->calcCM();
+        static hrp::Vector3 com_old = com;
+        static hrp::Vector3 com_old_old = com_old;
+        hrp::Vector3 com_acc = (com - 2*com_old + com_old_old)/(m_dt*m_dt);
+        hrp::Vector3 ref_zmp; ref_zmp << com.head(XY)-(com(Z)/G)*com_acc.head(XY), 0;
+        if(mode.isInitialize()){ ref_zmp_filter.reset(ref_zmp); }
+        ref_zmp = ref_zmp_filter.passFilter(ref_zmp);
+        com_old_old = com_old;
+        com_old = com;
+        wbms->act_rs.com = com;
+        wbms->act_rs.zmp = ref_zmp;
 
 //            if(wbms->rp_ref_out.tgt[rf].is_contact && !wbms->rp_ref_out.tgt[lf].is_contact){
 //                ref_zmp.head(XY) = m_robot_vsafe->link(ee_ikc_map["rleg"].target_link_name)->p.head(XY);
@@ -314,34 +312,33 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 ////                ref_zmp(X) + 0.05;
 //            }
 
-            // qRef
-            for (int i = 0; i < m_qRef.data.length(); i++ ){
-              if(has(wbms->wp.use_joints, fik->m_robot->joint(i)->name)){
-                  m_qRef.data[i] = output_ratio * m_robot_vsafe->joint(i)->q  + (1 - output_ratio) * m_qRef.data[i];
-              }
-            }
-            // basePos
-            m_basePos.data = hrp::to_Point3D( output_ratio * m_robot_vsafe->rootLink()->p + (1 - output_ratio) * hrp::to_Vector3(m_basePos.data));
-            m_basePos.tm = m_qRef.tm;
-            // baseRpy
-            m_baseRpy.data = hrp::to_Orientation3D( output_ratio * hrp::rpyFromRot(m_robot_vsafe->rootLink()->R) + (1 - output_ratio) * hrp::to_Vector3(m_baseRpy.data));
-            m_baseRpy.tm = m_qRef.tm;
-            // zmp
-            hrp::Vector3 rel_ref_zmp = m_robot_vsafe->rootLink()->R.transpose() * (ref_zmp - m_robot_vsafe->rootLink()->p);
-            m_zmp.data = hrp::to_Point3D( output_ratio * rel_ref_zmp + (1 - output_ratio) * hrp::to_Vector3(m_zmp.data));
-            m_zmp.tm = m_qRef.tm;
-            // m_optionalData
-            if(m_optionalData.data.length() < optionalDataLength){
-                m_optionalData.data.length(optionalDataLength);//TODO:これいいのか？
-                for(int i=0;i<optionalDataLength;i++)m_optionalData.data[i] = 0;
-            }
-            m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = wbms->rp_ref_out.tgt[rf].is_contact;
-            m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = wbms->rp_ref_out.tgt[lf].is_contact;
-            addTimeReport("SetOutPut");
+        // qRef
+        for (int i = 0; i < m_qRef.data.length(); i++ ){
+          if(has(wbms->wp.use_joints, fik->m_robot->joint(i)->name)){
+              m_qRef.data[i] = output_ratio * m_robot_vsafe->joint(i)->q  + (1 - output_ratio) * m_qRef.data[i];
+          }
+        }
+        // basePos
+        m_basePos.data = hrp::to_Point3D( output_ratio * m_robot_vsafe->rootLink()->p + (1 - output_ratio) * hrp::to_Vector3(m_basePos.data));
+        m_basePos.tm = m_qRef.tm;
+        // baseRpy
+        m_baseRpy.data = hrp::to_Orientation3D( output_ratio * hrp::rpyFromRot(m_robot_vsafe->rootLink()->R) + (1 - output_ratio) * hrp::to_Vector3(m_baseRpy.data));
+        m_baseRpy.tm = m_qRef.tm;
+        // zmp
+        hrp::Vector3 rel_ref_zmp = m_robot_vsafe->rootLink()->R.transpose() * (ref_zmp - m_robot_vsafe->rootLink()->p);
+        m_zmp.data = hrp::to_Point3D( output_ratio * rel_ref_zmp + (1 - output_ratio) * hrp::to_Vector3(m_zmp.data));
+        m_zmp.tm = m_qRef.tm;
+        // m_optionalData
+        if(m_optionalData.data.length() < optionalDataLength){
+            m_optionalData.data.length(optionalDataLength);//TODO:これいいのか？
+            for(int i=0;i<optionalDataLength;i++)m_optionalData.data[i] = 0;
         }
-        wbms->baselinkpose.p = fik->m_robot->rootLink()->p;
-        wbms->baselinkpose.R = fik->m_robot->rootLink()->R;
+        m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = wbms->rp_ref_out.tgt[rf].is_contact;
+        m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = wbms->rp_ref_out.tgt[lf].is_contact;
+        addTimeReport("SetOutPut");
     }
+    wbms->baselinkpose.p = fik->m_robot->rootLink()->p;
+    wbms->baselinkpose.R = fik->m_robot->rootLink()->R;
 
 
     std::map<std::string, std::string> to_sname;
@@ -435,8 +432,8 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.localPos = hrp::Vector3::Zero();
         tmp.localR = hrp::Matrix33::Identity();
         tmp.targetPos = hrp::to_Vector3(m_basePos.data);// will be ignored by selection_vec
-        tmp.targetRpy = com_ref.rpy();// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
-        tmp.constraint_weight << 0,0,0,1,1,1;
+        tmp.targetRpy = com_ref.rpy();
+        tmp.constraint_weight << 0,0,0,1e-1,1e-1,1e-1;
         tmp.rot_precision = deg2rad(3);
         ikc_list.push_back(tmp);
     }else{
@@ -450,99 +447,6 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
       tmp.rot_precision = deg2rad(3);
       ikc_list.push_back(tmp);
     }
-//    hrp::Vector3 act_cp = fik->m_robot->rootLink()->p + fik->m_robot->rootLink()->R * rel_act_cp;
-//    act_cp(Z) = 0.3;
-////    act_cp = 0.9 * act_cp + 0.1 * (fik->m_robot->rootLink()->p + fik->m_robot->rootLink()->R * rel_act_cp);
-//    static hrp::Vector3 track_cp = act_cp;
-//
-//    if(!std::isnan(act_cp(X)) && !std::isnan(act_cp(Y)) && !std::isnan(act_cp(Z))){
-//        hrp::Vector3 direc = act_cp - track_cp;
-//        const double speed = 0.3;
-//        LIMIT_MINMAX(direc(X), -speed*m_dt, speed*m_dt);
-//        LIMIT_MINMAX(direc(Y), -speed*m_dt, speed*m_dt);
-//        LIMIT_MINMAX(direc(Z), -0.01*m_dt, 0.01*m_dt);
-////        if(direc.norm()>speed*m_dt){
-////            direc = direc.normalized() * speed*m_dt;
-////        }
-//        track_cp += direc;
-//    }
-//
-////    hrp::Vector3 act_zmp = fik->m_robot->rootLink()->p + fik->m_robot->rootLink()->R * rel_act_zmp;
-////    static int cp_count = 0;
-//    const double L = 100;
-//    const double sink = 0.0;
-//    const double fh = 0.05;
-//    const bool start_cp = false;
-////    const bool start_cp = ((act_cp - fik->m_robot->calcCM()).head(XY).norm()>0.025);
-////    dbgv(act_cp);
-////    dbgv(track_cp);
-////    dbgv(fik->m_robot->calcCM());
-//
-//
-//
-//    if (act_cp.head(XY).norm()>0.02)
-//    {
-//
-//
-//
-//
-//    try {
-//        OpenHRP::AutoBalancerService::FootstepsSequence fss;
-//        OpenHRP::AutoBalancerService::StepParamsSequence spss;
-//        fss.length(2);
-//        spss.length(2);
-//        fss[0].fs.length(1);
-//        fss[1].fs.length(1);
-//        spss[0].sps.length(1);
-//        spss[1].sps.length(1);
-//        fss[0].fs[0].leg = "rleg";
-//        fss[0].fs[0].pos[0] = 0;
-//        fss[0].fs[0].pos[1] = -0.1;
-//        fss[0].fs[0].pos[2] = 0;
-//        fss[0].fs[0].rot[0] = 1;
-//        fss[0].fs[0].rot[1] = 0;
-//        fss[0].fs[0].rot[2] = 0;
-//        fss[0].fs[0].rot[3] = 0;
-//        fss[1].fs[0].leg = "lleg";
-//        fss[1].fs[0].pos[0] = act_cp(Y)>0 ? 0.3*act_cp.head(XY).normalized()(X) : 0;
-//        fss[1].fs[0].pos[1] = act_cp(Y)>0 ? 0.3*act_cp.head(XY).normalized()(Y) : 0.1;
-//        fss[1].fs[0].pos[2] = 0;
-//        fss[1].fs[0].rot[0] = 1;
-//        fss[1].fs[0].rot[1] = 0;
-//        fss[1].fs[0].rot[2] = 0;
-//        fss[1].fs[0].rot[3] = 0;
-//        spss[0].sps[0].step_height = 0.1;
-//        spss[1].sps[0].step_height = 0.1;
-//        spss[0].sps[0].step_time = 0.01;
-//        spss[1].sps[0].step_time = 0.5;
-//        spss[0].sps[0].toe_angle = 0;
-//        spss[1].sps[0].toe_angle = 0;
-//        spss[0].sps[0].heel_angle = 0;
-//        spss[1].sps[0].heel_angle = 0;
-////        m_AutoBalancerServiceConsumer->setFootSteps(fss,1);
-//        m_AutoBalancerServiceConsumer->setFootStepsWithParam(fss,spss,1);
-//    } catch(CORBA::COMM_FAILURE& ex) {
-////        dbg("1");
-//    } catch(CORBA::MARSHAL& ex) {
-////        dbg("2");
-//    } catch(CORBA::BAD_PARAM& ex) {
-////        dbg("3");
-//    } catch(CORBA::OBJECT_NOT_EXIST& ex) {
-////        dbg("4");
-//    } catch(CORBA::SystemException& ex) {
-////        dbg("5");
-//    } catch(CORBA::Exception&) {
-////        dbg("6");
-//    } catch(omniORB::fatalException& fe) {
-////        dbg("7");
-//    }
-//    catch(...) {
-////        dbg("8");
-//    }
-////    dbg("ok");
-//    // サービスポートによる処理
-//}
-
     if(has(wbms->wp.use_targets, "rleg")){
         IKConstraint tmp;
         tmp.target_link_name = ee_ikc_map["rleg"].target_link_name;
@@ -674,14 +578,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.localR = hrp::Matrix33::Identity();
         tmp.targetPos = com_ref.p;// COM height will not be constraint
         tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
-//        tmp.constraint_weight << 3,3,0.01,0.1,0.1,0;
         tmp.constraint_weight << 3,3,0.01,0,0,0;
-//        if(fik_in->cur_momentum_around_COM.norm() > 1e9){
-//            tmp.constraint_weight << 10,10,1,1e-5,1e-5,0;
-//            tmp.rot_precision = 100;//angular momentum precision
-//        }else{
-//            tmp.constraint_weight << 10,10,1,0,0,0;
-//        }
         ikc_list.push_back(tmp);
     }
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index d05c1d09d0b..c4b0b2c43f5 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -136,7 +136,6 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         unsigned int loop;
         unsigned int m_debugLevel;
         int optionalDataLength;
-        bool is_legged_robot;
         hrp::BodyPtr m_robot_act; // actual
         hrp::BodyPtr m_robot_vsafe; // joint trajectory safe
         typedef boost::shared_ptr<FullbodyInverseKinematicsSolver> fikPtr;

From 543327fd4d72bc666ec88686ceb23f38713f3a44 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 9 Sep 2019 19:09:52 +0900
Subject: [PATCH 173/217] fix zmp height

---
 .../WholeBodyMasterSlave.cpp                     | 16 ++++------------
 1 file changed, 4 insertions(+), 12 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 37ce956b3ea..75ca861161d 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -396,25 +396,17 @@ void WholeBodyMasterSlave::processTransition(){
 
 
 void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(){
-    hrp::Vector3 basePos_heightChecked = hrp::to_Vector3(m_basePos.data);//ベースリンク高さ調整により足裏高さ0に
-    fik->m_robot->rootLink()->p = basePos_heightChecked;
-//    for ( int i = 0; i < fik->m_robot->numJoints(); i++ ){ fik->m_robot->joint(i)->q = m_qRef.data[i]; }
+    fik->m_robot->rootLink()->p = hrp::to_Vector3(m_basePos.data);
     hrp::setQAll(fik->m_robot, hrp::to_dvector(m_qRef.data));
     fik->m_robot->calcForwardKinematics();
-
-    //TODO
-//    hrp::Vector3 init_foot_mid_coord = (fik_in->getEndEffectorPos("rleg") + fik_in->getEndEffectorPos("lleg")) / 2;
-//    if( fabs((double)init_foot_mid_coord(Z)) > 1e-5 ){
-//        basePos_heightChecked(Z) -= init_foot_mid_coord(Z);
-//        init_foot_mid_coord(Z) = 0;
-//        std::cerr<<"["<<m_profile.instance_name<<"] Input basePos height is invalid. Auto modify "<<m_basePos.data.z<<" -> "<<basePos_heightChecked(Z)<<endl;
-//    }
+    double current_foot_height_from_world = std::min(ee_ikc_map["rleg"].getCurrentTargetPos(fik->m_robot)(Z), ee_ikc_map["lleg"].getCurrentTargetPos(fik->m_robot)(Z));
+    RTC_INFO_STREAM("current_foot_height_from_world = "<<current_foot_height_from_world<<" will be modified to 0");
 
     std::vector<hrp::BodyPtr> body_list;
     body_list.push_back(fik->m_robot);
     body_list.push_back(m_robot_vsafe);
     for(int i=0;i<body_list.size();i++){//初期姿勢でBodyをFK
-        hrp::setRobotStateVec(body_list[i], hrp::to_dvector(m_qRef.data), basePos_heightChecked, hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y));
+        hrp::setRobotStateVec(body_list[i], hrp::to_dvector(m_qRef.data), hrp::to_Vector3(m_basePos.data) - hrp::Vector3::UnitZ() * current_foot_height_from_world, hrp::rotFromRpy(hrp::to_Vector3(m_baseRpy.data)));
         body_list[i]->calcForwardKinematics();
     }
 

From 8bdcde7b0047dcf38bb98d16944b81338f84c5bc Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 13 Sep 2019 18:36:59 +0900
Subject: [PATCH 174/217] use act zmp and refactor

---
 .../WholeBodyMasterSlave.cpp                  |  10 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 147 +++++-------------
 2 files changed, 49 insertions(+), 108 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 75ca861161d..c822475cad3 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -132,7 +132,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     q_ip = new interpolator(fik->numStates(), m_dt, interpolator::CUBICSPLINE, 1); // or HOFFARBIB, QUINTICSPLINE
     q_ip->clear();
     if(fik->m_robot->name().find("JAXON") != std::string::npos){
-        avg_q_vel = hrp::dvector::Constant(fik->numStates(), 2.0); // all joint max avarage vel = 1.0 rad/s
+        avg_q_vel = hrp::dvector::Constant(fik->numStates(), 4.0); // all joint max avarage vel = 1.0 rad/s
     }else{
         avg_q_vel = hrp::dvector::Constant(fik->numStates(), 1.0); // all joint max avarage vel = 1.0 rad/s
     }
@@ -141,7 +141,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     avg_q_acc.tail(6).fill(std::numeric_limits<double>::max()); // no limit for base link acc
     RTC_INFO_STREAM("setup interpolator finished");
 
-    ref_zmp_filter.setParameter(5, 1/m_dt, Q_BUTTERWORTH);
+    ref_zmp_filter.setParameter(20, 1/m_dt, Q_BUTTERWORTH);
 
     cp_flag = -1;
     lt.fill(0);
@@ -292,6 +292,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         smoothingJointAngles(fik->m_robot, m_robot_vsafe);
 
         hrp::Vector3 com = m_robot_vsafe->calcCM();
+        com(Z) += 0.3;//hotfix
         static hrp::Vector3 com_old = com;
         static hrp::Vector3 com_old_old = com_old;
         hrp::Vector3 com_acc = (com - 2*com_old + com_old_old)/(m_dt*m_dt);
@@ -302,6 +303,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         com_old = com;
         wbms->act_rs.com = com;
         wbms->act_rs.zmp = ref_zmp;
+        wbms->act_rs.st_zmp = m_robot_vsafe->rootLink()->p + m_robot_vsafe->rootLink()->R * rel_act_zmp;
 
 //            if(wbms->rp_ref_out.tgt[rf].is_contact && !wbms->rp_ref_out.tgt[lf].is_contact){
 //                ref_zmp.head(XY) = m_robot_vsafe->link(ee_ikc_map["rleg"].target_link_name)->p.head(XY);
@@ -628,13 +630,13 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         }
     }
 
-    const int IK_MAX_LOOP = 2;
+    const int IK_MAX_LOOP = 1;
     int loop_result = fik->solveFullbodyIKLoop(ikc_list, IK_MAX_LOOP);
 }
 
 void WholeBodyMasterSlave::smoothingJointAngles(hrp::BodyPtr _robot, hrp::BodyPtr _robot_safe){
     double goal_time = 0.0;
-    const double min_goal_time_offset = 0.1;
+    const double min_goal_time_offset = 0.3;
 
     static hrp::dvector ans_state_vel = hrp::dvector::Zero(fik->numStates());
 
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 1535dac07ec..1b1769abe96 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -6,12 +6,6 @@
 #include "../SequencePlayer/interpolator.h"
 #include "../ImpedanceController/JointPathEx.h"
 #include "../AutoBalancer/FullbodyInverseKinematicsSolver.h"
-// geometry
-#include <opencv2/core/core.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <opencv2/imgproc/types_c.h>
-#include <opencv2/core/eigen.hpp>
-
 // geometry
 #include <boost/geometry.hpp>
 #include <boost/geometry/geometries/point_xy.hpp>
@@ -356,22 +350,21 @@ class WBMSCore{
         double HZ, DT;
         BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters, com_in_filter;
         hrp::Vector3 com_old, com_oldold, comacc, com_CP_ref_old;
-        std::vector<BiquadIIRFilterVec> tgt_pos_filters,tgt_rpy_filters;
         HumanPose rp_ref_out_old, hp_swap_checked;
         unsigned int loop;
         bool is_initial_loop;
         bool cp_force_go_contact[LR];
+        int zmp_force_go_contact_count[LR];
     public:
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
         double H_cur;
         HumanPose hp_wld_raw, hp_plot, rp_ref_out, rp_ref_vel_old;
-        FILE *sr_log, *cz_log, *id_log;
         hrp::Pose3 baselinkpose;
         hrp::Vector3 cp_dec, cp_acc;
         hrp::dmatrix foot_vert3d_act[LR], foot_vert3d_check[LR]; // A = [p0 p1 p2 p3]
         hrp::Vector2 com_forcp_ref,com_vel_forcp_ref;
         hrp::dvector6 invdyn_ft;
-        hrp::Vector4 foot_vert_act_fblr[LR], foot_vert_safe_fblr[LR], foot_vert_check_fblr[LR];
+        hrp::Vector4 foot_vert_act_fblr[LR], foot_vert_safe_fblr[LR];//, foot_vert_check_fblr[LR];
         struct WBMSparameters {
             double auto_swing_foot_landing_threshold;
             double human_to_robot_ratio;
@@ -386,7 +379,7 @@ class WBMSCore{
             std::vector<std::string> use_targets;
         } wp;
         struct ActualRobotState {
-            hrp::Vector3 com, zmp;
+            hrp::Vector3 com, zmp, st_zmp;
         } act_rs;
 
         WBMSCore(const double& dt){
@@ -397,30 +390,17 @@ class WBMSCore{
             com_old = com_oldold = comacc = hrp::Vector3::Zero();
             cp_dec = cp_acc = hrp::Vector3::Zero();
             invdyn_ft = hrp::dvector6::Zero();
-            tgt_pos_filters.resize(num_pose_tgt);
-            tgt_rpy_filters.resize(num_pose_tgt);
-            for(int i=0;i<tgt_pos_filters.size();i++)tgt_pos_filters[i].setParameter(1.0, HZ, Q_NOOVERSHOOT);//四肢拘束点用(position)
-            for(int i=0;i<tgt_rpy_filters.size();i++)tgt_rpy_filters[i].setParameter(1.0, HZ, Q_NOOVERSHOOT);//四肢拘束点用(Rotation)
-            tgt_pos_filters[com].setParameter(1.0, HZ, Q_NOOVERSHOOT);//重心pos用
-            tgt_rpy_filters[com].setParameter(0.6, HZ, Q_NOOVERSHOOT);//重心rot用
-            tgt_pos_filters[rf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, Q_NOOVERSHOOT);//右足pos用
-            tgt_pos_filters[lf].setParameter(hrp::Vector3(1.0,1.0,1.0), HZ, Q_NOOVERSHOOT);//左足pos用
             calcacc_v_filters.setParameter(5, HZ, Q_BUTTERWORTH);//加速度計算用
             acc4zmp_v_filters.setParameter(5, HZ, Q_BUTTERWORTH);//ZMP生成用ほぼこの値でいい
             com_in_filter.setParameter(1, HZ);
-            foot_vert_act_fblr[R] << 0.13, -0.10,  0.06, -0.08;
-            foot_vert_act_fblr[L] << 0.13, -0.10,  0.08, -0.06;
-            foot_vert_check_fblr[R] << 0.02+0.002, -0.01-0.002,  0.02+0.002,  0.01-0.002;
-            foot_vert_check_fblr[L] << 0.02+0.002, -0.01-0.002, -0.01+0.002, -0.02-0.002;
-            foot_vert_safe_fblr[R] << 0.02, -0.01,  0.02,  0.01;
-            foot_vert_safe_fblr[L] << 0.02, -0.01, -0.01, -0.02;
+            foot_vert_safe_fblr[R] << 0.02, -0.02,  0.02,  0.01;
+            foot_vert_safe_fblr[L] << 0.02, -0.02, -0.01, -0.02;
             for(int i=0;i<LR;i++){
                 foot_vert3d_act[i]      = make_rect_3d(foot_vert_act_fblr[i]);
-                foot_vert3d_check[i]    = make_rect_3d(foot_vert_check_fblr[i]);
+                foot_vert3d_check[i]    = make_rect_3d(offset_fblr(foot_vert_safe_fblr[i], 0.002));
             }
-            cp_force_go_contact[R] = false;
-            cp_force_go_contact[L] = false;
-
+            cp_force_go_contact[R] = cp_force_go_contact[L] = false;
+            zmp_force_go_contact_count[R] = zmp_force_go_contact_count[L] = 0;
             wp.auto_swing_foot_landing_threshold = 0.04;
             wp.human_to_robot_ratio = 1.0;//human 1.1m vs jaxon 1.06m
             wp.set_com_height_fix = false;
@@ -433,14 +413,10 @@ class WBMSCore{
             rp_ref_out_old.reset();
             rp_ref_out.reset();
             rp_ref_vel_old.reset();
-            if(DEBUG){
-                sr_log = fopen("/home/ishiguro/HumanSync_support_region.log","w+");
-                cz_log = fopen("/home/ishiguro/HumanSync_com_zmp.log","w+");
-                id_log = fopen("/home/ishiguro/HumanSync_invdyn.log","w+");
-            }
             is_initial_loop = true;
             cout<<"WBMSCore constructed"<<endl;
         }
+        ~WBMSCore(){ cout<<"WBMSCore destructed"<<endl; }
         hrp::dmatrix make_rect_3d(const hrp::Vector4& ForwardBackLeftRight){
             return (hrp::dmatrix(3,4) << // (XYZ,4)とかの#defineマクロ効かない
                 hrp::Vector3(ForwardBackLeftRight(0), ForwardBackLeftRight(2), 0),//左前
@@ -448,11 +424,8 @@ class WBMSCore{
                 hrp::Vector3(ForwardBackLeftRight(1), ForwardBackLeftRight(3), 0),//右後
                 hrp::Vector3(ForwardBackLeftRight(0), ForwardBackLeftRight(3), 0)).finished();//右前
         }
-        ~WBMSCore(){
-            if(DEBUG)fclose(sr_log);
-            if(DEBUG)fclose(cz_log);
-            if(DEBUG)fclose(id_log);
-            cout<<"WBMSCore destructed"<<endl;
+        hrp::Vector4 offset_fblr(const hrp::Vector4& in, const double offset){
+            return (hrp::Vector4()<< in(0)+offset, in(1)-offset, in(2)+offset, in(3)-offset).finished();//右前
         }
         void initializeHumanPoseFromCurrentInput(){
             std::string ns[7] = {"com","rf","lf","rh","lh","zmp","head"};
@@ -470,7 +443,7 @@ class WBMSCore{
                     rp_ref_out.tgt[human_l_names[i]].abs = rp_ref_out.tgt[human_l_names[i]].offs;
                 }
             }
-            rp_ref_out.tgt[com].offs.p = act_rs.com = act_rs.zmp = robot_in->calcCM();
+            rp_ref_out.tgt[com].offs.p = act_rs.com = act_rs.zmp = act_rs.st_zmp = robot_in->calcCM();
             com_CP_ref_old = rp_ref_out.tgt[com].offs.p;
             rp_ref_out.tgt[com].offs.R = robot_in->rootLink()->R;
             rp_ref_out.tgt[zmp].offs.p(X) = rp_ref_out.tgt[com].offs.p(X);
@@ -511,7 +484,6 @@ class WBMSCore{
             com_forcp_ref_old = com_forcp_ref;
             applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
             applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, rp_ref_out.tgt[com].abs.p);
-//            applyLPFilter_post                       (rp_ref_out);
             applyZMPCalcFromCOM                 (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);
             modifyFootRotAndXYForContact        (rp_ref_out);
             H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[lf].abs.p(Z));
@@ -539,12 +511,34 @@ class WBMSCore{
             out.tgt[zmp].abs = in.tgt[zmp].abs;//最近使わない
         }
         void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
-            hrp::Vector3 inside_vec_rf = old.tgt[rf].abs.R * hrp::Vector3(0,1,0);
-            hrp::Vector3 inside_vec_lf = old.tgt[lf].abs.R * hrp::Vector3(0,-1,0);
-            hrp::Vector2 rf2zmp = act_rs.zmp.head(XY) - old.tgt[rf].abs.p.head(XY);
-            hrp::Vector2 lf2zmp = act_rs.zmp.head(XY) - old.tgt[lf].abs.p.head(XY);
-            if( inside_vec_rf.head(XY).dot(rf2zmp) / inside_vec_rf.norm() > wp.auto_swing_foot_landing_threshold ){ out.tgt[lf].go_contact = true; }//2D,3D混ざって微妙
-            if( inside_vec_lf.head(XY).dot(lf2zmp) / inside_vec_lf.norm() > wp.auto_swing_foot_landing_threshold ){ out.tgt[rf].go_contact = true; }
+            hrp::Vector3 inside_vec_rf = old.tgt[rf].abs.R * hrp::Vector3::UnitY();
+            hrp::Vector3 inside_vec_lf = old.tgt[lf].abs.R * (-hrp::Vector3::UnitY());
+            {
+                hrp::Vector2 rf2zmp = act_rs.zmp.head(XY) - old.tgt[rf].abs.p.head(XY);
+                hrp::Vector2 lf2zmp = act_rs.zmp.head(XY) - old.tgt[lf].abs.p.head(XY);
+                if( inside_vec_rf.head(XY).dot(rf2zmp) / inside_vec_rf.norm() > wp.auto_swing_foot_landing_threshold ){ out.tgt[lf].go_contact = true; }//2D,3D混ざって微妙
+                if( inside_vec_lf.head(XY).dot(lf2zmp) / inside_vec_lf.norm() > wp.auto_swing_foot_landing_threshold ){ out.tgt[rf].go_contact = true; }
+            }
+            {
+                hrp::Vector2 rf2zmp = act_rs.st_zmp.head(XY) - old.tgt[rf].abs.p.head(XY);
+                hrp::Vector2 lf2zmp = act_rs.st_zmp.head(XY) - old.tgt[lf].abs.p.head(XY);
+                if( inside_vec_rf.head(XY).dot(rf2zmp) / inside_vec_rf.norm() > wp.auto_swing_foot_landing_threshold ){
+                    zmp_force_go_contact_count[L] = 1;
+                }else if(zmp_force_go_contact_count[L] > 0 && zmp_force_go_contact_count[L] < HZ*1 ){
+                    zmp_force_go_contact_count[L]++;
+                }else{
+                    zmp_force_go_contact_count[L] = 0;
+                }
+                if( inside_vec_lf.head(XY).dot(lf2zmp) / inside_vec_lf.norm() > wp.auto_swing_foot_landing_threshold ){
+                    zmp_force_go_contact_count[R] = 1;
+                }else if(zmp_force_go_contact_count[R] > 0 && zmp_force_go_contact_count[R] < HZ*1 ){
+                    zmp_force_go_contact_count[R]++;
+                }else{
+                    zmp_force_go_contact_count[R] = 0;
+                }
+                out.tgt[lf].go_contact |= (zmp_force_go_contact_count[L] > 0);
+                out.tgt[rf].go_contact |= (zmp_force_go_contact_count[R] > 0);
+            }
         }
         void limitEEWorkspace(HumanPose& out){
             const double MAX_FW = 0.25;
@@ -564,7 +558,7 @@ class WBMSCore{
                 sp2sw_vec = base_rot * sp2sw_vec_baserel;
                 swing_leg.abs.p.head(XY) = sp2sw_vec + support_leg.cnt.p.head(XY);
             }
-            const double base2hand_min = 0.4;// for jaxon demo
+            const double base2hand_min = 0.5;// for jaxon demo
             for(int i=0, l[2]={rh,lh}; i<2; i++){
                 hrp::Vector2 horizontal_dist = out.tgt[l[i]].abs.p.head(XY) - baselinkpose.p.head(XY);
                 if(horizontal_dist.norm() < base2hand_min){
@@ -636,10 +630,8 @@ class WBMSCore{
             com_ans = com_in;
             if (is_initial_loop)com_ans_old = com_in;
             hrp::Vector3 com_vel = (com_in - com_ans_old)/DT;
-            hrp::Vector4 marginDelta_for_dcp(+0.001,-0.001,0.001,-0.001);
-            hrp::Vector4 marginDelta_for_acp(+0.010,-0.010,0.010,-0.010);
-            hrp::dmatrix hull_dcp = createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R]+marginDelta_for_dcp, foot_vert_safe_fblr[L]+marginDelta_for_dcp);
-            hrp::dmatrix hull_acp = createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R]+marginDelta_for_acp, foot_vert_safe_fblr[L]+marginDelta_for_acp);
+            hrp::dmatrix hull_dcp = createSupportRegionByFootPos(rfin_abs, lfin_abs, offset_fblr(foot_vert_safe_fblr[R], 0.001),    offset_fblr(foot_vert_safe_fblr[L], 0.001));
+            hrp::dmatrix hull_acp = createSupportRegionByFootPos(rfin_abs, lfin_abs, offset_fblr(foot_vert_safe_fblr[R], 0.01),     offset_fblr(foot_vert_safe_fblr[L], 0.01));
             hrp::Vector2 com_vel_ans_2d;
             regulateCOMVelocityByCapturePointVec( com_ans_old.head(XY), com_vel.head(XY), hull_dcp, hull_acp, com_vel_ans_2d);
             com_ans.head(XY) = com_ans_old.head(XY) + com_vel_ans_2d * DT;
@@ -694,23 +686,6 @@ class WBMSCore{
             hrp::dmatrix both_sole_verts_abs = (hrp::dmatrix(3, rf_sole_verts_abs.cols()+lf_sole_verts_abs.cols()) << rf_sole_verts_abs, lf_sole_verts_abs).finished();
             return makeConvexHull2D(both_sole_verts_abs.topRows(XY));
         }
-        void calcWorldZMP(const hrp::Vector3& rfpos, const hrp::Vector3& lfpos, const hrp::dvector6& rfwin, const hrp::dvector6& lfwin, hrp::Vector3& zmp_ans){
-            hrp::Vector3 rfzmp,lfzmp;
-            const double F_H_OFFSET = 0.03;//地面から6軸センサ原点への高さ
-            if( rfwin(fz) > 1.0e-6 ){
-                rfzmp(0) = ( - rfwin(ty) - rfwin(fx) * F_H_OFFSET + rfwin(fz) * 0 ) / rfwin(fz) + rfpos(0);
-                rfzmp(1) = (   rfwin(tx) - rfwin(fy) * F_H_OFFSET + rfwin(fz) * 0 ) / rfwin(fz) + rfpos(1);
-            }
-            if( lfwin(fz) > 1.0e-6 ){
-                lfzmp(0) = ( - lfwin(ty) - lfwin(fx) * F_H_OFFSET + lfwin(fz) * 0 ) / lfwin(fz) + lfpos(0);
-                lfzmp(1) = (   lfwin(tx) - lfwin(fy) * F_H_OFFSET + lfwin(fz) * 0 ) / lfwin(fz) + lfpos(1);
-            }
-            if( rfwin(fz) > 1.0e-6 || lfwin(fz) > 1.0e-6 ){
-                zmp_ans(0) = ( rfzmp(0)*rfwin(fz) + lfzmp(0)*lfwin(fz) ) / ( rfwin(fz) + lfwin(fz));
-                zmp_ans(1) = ( rfzmp(1)*rfwin(fz) + lfzmp(1)*lfwin(fz) ) / ( rfwin(fz) + lfwin(fz));
-            }else{ zmp_ans(0) = 0; zmp_ans(1) = 0; }
-            zmp_ans(2) = 0;
-        }
         bool calcCrossPointOnHull(const hrp::Vector2& inside_start_pt, const hrp::Vector2& outside_goal_pt, const hrp::dmatrix& hull, hrp::Vector2& ans_cross_pt){
             if(hull.rows() != 2){ std::cerr << "Invalid input for calcCrossPointOnHull" << std::endl; dbgn(hull); }
             bg_linestring anchor_vec;
@@ -749,27 +724,11 @@ class WBMSCore{
             return ans_nearest_pt;
         }
         hrp::dmatrix makeConvexHull2D(const hrp::dmatrix& pts_2d){
-//            return makeConvexHull2D_OpenCV(pts_2d);
             return makeConvexHull2D_Boost(pts_2d);
         }
         bool isPointInHull2D(const hrp::Vector2& pt, const hrp::dmatrix& hull){
-//            return isPointInHull2D_OpenCV(pt, hull);
             return isPointInHull2D_Boost(pt, hull);
         }
-        hrp::dmatrix makeConvexHull2D_OpenCV(const hrp::dmatrix& pts_2d){
-            if(pts_2d.rows() != 2){ std::cerr << "Invalid input for makeConvexHull2D_OpenCV" << std::endl; dbgn(pts_2d); }
-            cv::Mat pts_2d_cv;
-            std::vector<cv::Point2f> points(pts_2d.cols()), hull_2d_cv;
-            for(int i=0; i<pts_2d.cols(); i++){
-                points[i] = cv::Point2f(pts_2d.col(i)(X), pts_2d.col(i)(Y));
-            }
-            cv::convexHull( cv::Mat(points), hull_2d_cv, true);
-            hrp::dmatrix hull_2d(2, hull_2d_cv.size());
-            for(int i=0; i<hull_2d_cv.size(); i++){
-                hull_2d.col(i) << hull_2d_cv[i].x,  hull_2d_cv[i].y;
-            }
-            return hull_2d;
-        }
         hrp::dmatrix makeConvexHull2D_Boost(const hrp::dmatrix& pts_2d){
             if(pts_2d.rows() != 2){ std::cerr << "Invalid input for makeConvexHull_Boost" << std::endl; dbgn(pts_2d); }
             bg_multi_point tmp;
@@ -785,30 +744,10 @@ class WBMSCore{
             }
             return hull_2d;
         }
-        bool isPointInHull2D_OpenCV(const hrp::Vector2& pt, const hrp::dmatrix& hull){
-            if(hull.rows() != 2){ std::cerr << "Invalid input for isPointInHull2dOpenCV" << std::endl; dbgn(hull); }
-            std::vector<cv::Point2f> hull_cv(hull.cols());
-            for(int i=0; i<hull.cols(); i++){
-                hull_cv[i] = cv::Point2f(hull.col(i)(X), hull.col(i)(Y));
-            }
-            return (cv::pointPolygonTest(cv::Mat(hull_cv), cv::Point2f(pt(X),pt(Y)), false) > 0);
-        }
         bool isPointInHull2D_Boost(const hrp::Vector2& pt, const hrp::dmatrix& hull){
             if(hull.rows() != 2){ std::cerr << "Invalid input for isPointInHull2D_Boost" << std::endl; dbgn(hull); }
             return bg::within(to_bg_point(pt), to_bg_hull(hull));
         }
-        void applyLPFilter_post(HumanPose& tgt){
-            if(is_initial_loop){
-                for(int i=0, l[1]={com}; i<1; i++){
-                    tgt_pos_filters[l[i]].reset(tgt.tgt[l[i]].abs.p);
-                    tgt_rpy_filters[l[i]].reset(tgt.tgt[l[i]].abs.rpy());
-                }
-            }
-            for(int i=0, l[1]={com}; i<1; i++){
-                tgt.tgt[l[i]].abs.p   = tgt_pos_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.p);
-                tgt.tgt[l[i]].abs.setRpy(tgt_rpy_filters[l[i]].passFilter(tgt.tgt[l[i]].abs.rpy()));
-            }
-        }
         void applyZMPCalcFromCOM(const hrp::Vector3& comin, hrp::Vector3& zmpout){
             comacc = (comin - 2 * com_old + com_oldold)/(DT*DT);
             comacc = acc4zmp_v_filters.passFilter(comacc);

From 2e5cc878ee67c5da168e75f2299a5acdc94209ac Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 16 Sep 2019 22:54:25 +0900
Subject: [PATCH 175/217] refactor

---
 rtc/AutoBalancer/MyUtils.h                    |   7 +-
 .../WholeBodyMasterSlave.cpp                  |  18 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 166 +++++++++---------
 3 files changed, 92 insertions(+), 99 deletions(-)

diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index fea79445ff9..bfbac6b6f32 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -18,6 +18,8 @@ enum { fx, fy, fz, tx, ty, tz, ft_xyz };
 enum { com, rf, lf, rh, lh, head, zmp, num_pose_tgt };
 enum { num_ee_tgt=4 };
 
+#define OPPOSITE(lr) (lr==R ? L : R)
+
 static const double G = 9.80665;
 static const double Q_NOOVERSHOOT = 0.5;
 static const double Q_BUTTERWORTH = 0.707106781;
@@ -39,7 +41,6 @@ static const double Q_BUTTERWORTH = 0.707106781;
 #define LIMIT_MINMAX_V(v,minv,maxv) (v= v.cwiseMin(minv).cwiseMax(maxv))
 
 
-
 namespace hrp{
     class Pose3{
         public:
@@ -47,14 +48,14 @@ namespace hrp{
             hrp::Vector3 p;
             hrp::Matrix33 R;
             Pose3()                                                                                                             { reset();}
-            Pose3(const double& _X, const double& _Y, const double& _Z, const double& _r, const double& _p, const double& _y)   { p << _X,_Y,_Z; R = hrp::rotFromRpy(_r,_p,_y); }
+            Pose3(const double _X, const double _Y, const double _Z, const double _r, const double _p, const double _y)         { p << _X,_Y,_Z; R = hrp::rotFromRpy(_r,_p,_y); }
             Pose3(const hrp::dvector6& _xyz_rpy)                                                                                { p = _xyz_rpy.head(3); R = hrp::rotFromRpy(_xyz_rpy.tail(3)); }
 //            Pose3(const hrp::Vector3& _xyz, const hrp::Vector3& _rpy)                                                           { p = _xyz; R = hrp::rotFromRpy(_rpy); }// ambiguous overload...
             Pose3(const hrp::Vector3& _xyz, const hrp::Matrix33& _R)                                                            { p = _xyz; R = _R; }
             void reset()                                                                                                        { p.fill(0); R.setIdentity(); }
             hrp::Vector3 rpy() const                                                                                            { return hrp::rpyFromRot(R); }
             hrp::dvector6 to_dvector6() const                                                                                   { return (hrp::dvector6() << p, hrp::rpyFromRot(R)).finished(); }
-            void setRpy(const double& _r, const double& _p, const double& _y)                                                   { R = rotFromRpy(_r,_p,_y); }
+            void setRpy(const double _r, const double _p, const double _y)                                                      { R = rotFromRpy(_r,_p,_y); }
             void setRpy(const hrp::Vector3& _rpy)                                                                               { R = rotFromRpy(_rpy); }
     };
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index c822475cad3..39218252dbb 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -266,7 +266,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         for(int i=0; i<tgt_names.size(); i++){
             if (m_masterTgtPosesIn[tgt_names[i]]->isNew()){
                 m_masterTgtPosesIn[tgt_names[i]]->read();
-                wbms->hp_wld_raw.tgt[to_enum[tgt_names[i]]].abs = hrp::to_Pose3(m_masterTgtPoses[tgt_names[i]].data);
+                wbms->hp_wld_raw.tgt_by_str(tgt_names[i]).abs = hrp::to_Pose3(m_masterTgtPoses[tgt_names[i]].data);
             }
         }
         if (m_actCPIn.isNew())  { m_actCPIn.read(); rel_act_cp = hrp::to_Vector3(m_actCP.data);}
@@ -335,8 +335,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
             m_optionalData.data.length(optionalDataLength);//TODO:これいいのか？
             for(int i=0;i<optionalDataLength;i++)m_optionalData.data[i] = 0;
         }
-        m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = wbms->rp_ref_out.tgt[rf].is_contact;
-        m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = wbms->rp_ref_out.tgt[lf].is_contact;
+        m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = wbms->rp_ref_out.tgt[rf].is_contact();
+        m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = wbms->rp_ref_out.tgt[lf].is_contact();
         addTimeReport("SetOutPut");
     }
     wbms->baselinkpose.p = fik->m_robot->rootLink()->p;
@@ -448,7 +448,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.localR = ee_ikc_map["rleg"].localR;
         tmp.targetPos = rf_ref.p;
         tmp.targetRpy = rf_ref.rpy();
-        if(wbms->rp_ref_out.tgt[rf].is_contact){
+        if(wbms->rp_ref_out.tgt[rf].is_contact()){
             tmp.constraint_weight = hrp::dvector6::Constant(3);
         }else{
             tmp.constraint_weight = hrp::dvector6::Constant(0.1);
@@ -462,7 +462,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.localR = ee_ikc_map["lleg"].localR;
         tmp.targetPos = lf_ref.p;
         tmp.targetRpy = lf_ref.rpy();
-        if(wbms->rp_ref_out.tgt[lf].is_contact){
+        if(wbms->rp_ref_out.tgt[lf].is_contact()){
             tmp.constraint_weight = hrp::dvector6::Constant(3);
         }else{
             tmp.constraint_weight = hrp::dvector6::Constant(0.1);
@@ -511,12 +511,12 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
             ikc_list.push_back(tmp);
         }
     }
-    if(wbms->rp_ref_out.tgt[rf].is_contact){
+    if(wbms->rp_ref_out.tgt[rf].is_contact()){
         sccp->avoid_priority.head(12).head(6).fill(4);
     }else{
         sccp->avoid_priority.head(12).head(6).fill(3);
     }
-    if(wbms->rp_ref_out.tgt[lf].is_contact){
+    if(wbms->rp_ref_out.tgt[lf].is_contact()){
         sccp->avoid_priority.head(12).tail(6).fill(4);
     }else{
         sccp->avoid_priority.head(12).tail(6).fill(3);
@@ -585,8 +585,8 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
     if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->ulimit = deg2rad(10);
     if( fik->m_robot->link("RARM_JOINT2") != NULL) fik->m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-45);//脇内側の干渉回避
     if( fik->m_robot->link("LARM_JOINT2") != NULL) fik->m_robot->link("LARM_JOINT2")->llimit = deg2rad(45);
-    if( fik->m_robot->link("RLEG_JOINT3") != NULL) fik->m_robot->link("RLEG_JOINT3")->llimit = deg2rad(10);//膝伸びきり防止のため
-    if( fik->m_robot->link("LLEG_JOINT3") != NULL) fik->m_robot->link("LLEG_JOINT3")->llimit = deg2rad(10);
+    if( fik->m_robot->link("RLEG_JOINT3") != NULL) fik->m_robot->link("RLEG_JOINT3")->llimit = deg2rad(15);//膝伸びきり防止のため
+    if( fik->m_robot->link("LLEG_JOINT3") != NULL) fik->m_robot->link("LLEG_JOINT3")->llimit = deg2rad(15);
 
     if( fik->m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_Y")->jointId) = 0.1;//K
     if( fik->m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_P")->jointId) = 0.1;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 1b1769abe96..88ed5f2a2b2 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -25,8 +25,6 @@ typedef bg::model::polygon<bg_point> bg_polygon;
 //EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d)
 
 
-#define F(x) (x == R ? rf : lf)
-
 
 #define DEBUG 0
 
@@ -84,10 +82,14 @@ class PoseTGT{
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
         hrp::Pose3 abs, offs, cnt;
         hrp::dvector6 w;
-        bool is_contact, go_contact;
+        bool go_contact;
         PoseTGT(){ reset(); }
         ~PoseTGT(){}
-        void reset(){ abs.reset(); offs.reset(); cnt.reset(); w.fill(0); is_contact = go_contact = false;}
+        void reset(){ abs.reset(); offs.reset(); cnt.reset(); w.fill(0); go_contact = false;}
+        bool is_contact(){
+            const double contact_threshold = 0.005;
+            return ((abs.p(Z) - offs.p(Z)) < contact_threshold);
+        }
 };
 
 class HumanPose{
@@ -103,6 +105,19 @@ class HumanPose{
             for(int i=0;i<num_pose_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",pcatgt[i].c_str(),in.tgt[i].abs.p(X),in.tgt[i].abs.p(Y),in.tgt[i].abs.p(Z)); }
             printf("\n");
         }
+        PoseTGT&                        foot    (const int lr)      { assert(lr == R || lr == L); return (lr == R) ? tgt[rf] : tgt[lf]; }
+        const PoseTGT&                  foot    (const int lr) const{ assert(lr == R || lr == L); return (lr == R) ? tgt[rf] : tgt[lf]; }
+        PoseTGT&        opposite_side_of_foot   (const int lr)      { assert(lr == R || lr == L); return (lr == R) ? tgt[lf] : tgt[rf]; }
+        const PoseTGT&  opposite_side_of_foot   (const int lr) const{ assert(lr == R || lr == L); return (lr == R) ? tgt[lf] : tgt[rf]; }
+        PoseTGT& tgt_by_str(const std::string ln){
+            assert(ln == "com" || ln == "head" || ln == "lleg" || ln == "rleg" || ln == "larm" || ln == "rarm" );
+            if      (ln == "lleg"){ return tgt[lf]; }
+            else if (ln == "rleg"){ return tgt[rf]; }
+            else if (ln == "larm"){ return tgt[lh]; }
+            else if (ln == "rarm"){ return tgt[rh]; }
+            else if (ln == "com" ){ return tgt[com]; }
+            else                  { return tgt[head]; }
+        }
         void print() const { hp_printf(*this); }
 };
 
@@ -350,7 +365,7 @@ class WBMSCore{
         double HZ, DT;
         BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters, com_in_filter;
         hrp::Vector3 com_old, com_oldold, comacc, com_CP_ref_old;
-        HumanPose rp_ref_out_old, hp_swap_checked;
+        HumanPose rp_ref_out_old;
         unsigned int loop;
         bool is_initial_loop;
         bool cp_force_go_contact[LR];
@@ -453,9 +468,6 @@ class WBMSCore{
             rp_ref_out.tgt[lf].cnt = rp_ref_out.tgt[lf].offs;
             baselinkpose.p = robot_in->rootLink()->p;
             baselinkpose.R = robot_in->rootLink()->R;
-            for(int i=0, l[LR]={rf,lf}; i<LR; i++){
-                rp_ref_out.tgt[l[i]].is_contact = true;
-            }
             H_cur = rp_ref_out.tgt[com].offs.p(Z) - std::min((double)rp_ref_out.tgt[rf].offs.p(Z), (double)rp_ref_out.tgt[rf].offs.p(Z));
         }
         void initializeRequest(hrp::BodyPtr robot_in, std::map<std::string, IKConstraint>& _ee_ikc_map){
@@ -466,8 +478,7 @@ class WBMSCore{
             rp_ref_out_old = rp_ref_vel_old = rp_ref_out;
         }
         void update(){//////////  メインループ  ////////////
-            autoLRSwapCheck                     (hp_wld_raw, hp_swap_checked);//入力の左右反転を常にチェック(＝手足の交差は不可能)(動作中に入れ替わると余計なお世話かも)
-            convertHumanToRobot                 (hp_swap_checked, rp_ref_out);
+            convertHumanToRobot                 (hp_wld_raw, rp_ref_out);
             hp_plot = rp_ref_out;
             if(wp.set_com_height_fix) rp_ref_out.tgt[com].abs.p(Z) = rp_ref_out.tgt[com].offs.p(Z) + wp.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
             lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
@@ -484,7 +495,7 @@ class WBMSCore{
             com_forcp_ref_old = com_forcp_ref;
             applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
             applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, rp_ref_out.tgt[com].abs.p);
-            applyZMPCalcFromCOM                 (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);
+            applyZMPCalcFromCOM                 (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);//結局STに送るZMPは最終段で計算するからこれ意味ない
             modifyFootRotAndXYForContact        (rp_ref_out);
             H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[lf].abs.p(Z));
             rp_ref_out_old = rp_ref_out;
@@ -493,12 +504,7 @@ class WBMSCore{
         }
 
     private:
-        void autoLRSwapCheck(const HumanPose& in, HumanPose& out){
-            out = in;
-            if(in.tgt[rh].offs.p(Y) > in.tgt[lh].offs.p(Y) ){ out.tgt[rh] = in.tgt[lh]; out.tgt[lh] = in.tgt[rh]; }
-            if(in.tgt[rf].offs.p(Y) > in.tgt[lf].offs.p(Y) ){ out.tgt[rf] = in.tgt[lf]; out.tgt[lf] = in.tgt[rf]; }
-        }
-        void convertHumanToRobot(const HumanPose& in, HumanPose& out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
+        void convertHumanToRobot(const HumanPose& in, HumanPose& out){//結局初期指定からの移動量(=Rel)で計算をcp_decしてゆく
             //      out = in;//ダメゼッタイ
             for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
                 out.tgt[l[i]].abs.p = wp.human_to_robot_ratio * (in.tgt[l[i]].abs.p - in.tgt[l[i]].offs.p) + out.tgt[l[i]].offs.p;
@@ -511,42 +517,37 @@ class WBMSCore{
             out.tgt[zmp].abs = in.tgt[zmp].abs;//最近使わない
         }
         void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
-            hrp::Vector3 inside_vec_rf = old.tgt[rf].abs.R * hrp::Vector3::UnitY();
-            hrp::Vector3 inside_vec_lf = old.tgt[lf].abs.R * (-hrp::Vector3::UnitY());
-            {
-                hrp::Vector2 rf2zmp = act_rs.zmp.head(XY) - old.tgt[rf].abs.p.head(XY);
-                hrp::Vector2 lf2zmp = act_rs.zmp.head(XY) - old.tgt[lf].abs.p.head(XY);
-                if( inside_vec_rf.head(XY).dot(rf2zmp) / inside_vec_rf.norm() > wp.auto_swing_foot_landing_threshold ){ out.tgt[lf].go_contact = true; }//2D,3D混ざって微妙
-                if( inside_vec_lf.head(XY).dot(lf2zmp) / inside_vec_lf.norm() > wp.auto_swing_foot_landing_threshold ){ out.tgt[rf].go_contact = true; }
-            }
-            {
-                hrp::Vector2 rf2zmp = act_rs.st_zmp.head(XY) - old.tgt[rf].abs.p.head(XY);
-                hrp::Vector2 lf2zmp = act_rs.st_zmp.head(XY) - old.tgt[lf].abs.p.head(XY);
-                if( inside_vec_rf.head(XY).dot(rf2zmp) / inside_vec_rf.norm() > wp.auto_swing_foot_landing_threshold ){
-                    zmp_force_go_contact_count[L] = 1;
-                }else if(zmp_force_go_contact_count[L] > 0 && zmp_force_go_contact_count[L] < HZ*1 ){
-                    zmp_force_go_contact_count[L]++;
+            hrp::Vector2 to_opposite_foot[LR], ref_zmp_from_foot[LR], act_zmp_from_foot[LR];
+            for(int lr=0; lr<LR; lr++){
+                to_opposite_foot[lr] = old.opposite_side_of_foot(lr).abs.p.head(XY) - old.foot(lr).abs.p.head(XY);
+                if(to_opposite_foot[lr].norm() < 1e-3){ std::cerr << "to_opposite_foot[lr].norm() < 1e-3 :" << to_opposite_foot[lr].transpose()<<std::endl; }
+
+                // lock by ref_zmp
+                ref_zmp_from_foot[lr] = act_rs.zmp.head(XY) - old.foot(lr).abs.p.head(XY);
+                out.opposite_side_of_foot(lr).go_contact |= ( ref_zmp_from_foot[lr].dot(to_opposite_foot[lr]) / to_opposite_foot[lr].norm() > wp.auto_swing_foot_landing_threshold );
+
+                // lock by acts_zmp
+                act_zmp_from_foot[lr] = act_rs.st_zmp.head(XY) - old.foot(lr).abs.p.head(XY);
+                if( act_zmp_from_foot[lr].dot(to_opposite_foot[lr]) / to_opposite_foot[lr].norm() > wp.auto_swing_foot_landing_threshold ){
+                    zmp_force_go_contact_count[OPPOSITE(lr)] = 1;
+                }else if(zmp_force_go_contact_count[OPPOSITE(lr)] > 0 && zmp_force_go_contact_count[OPPOSITE(lr)] < HZ*0.5 ){
+                    zmp_force_go_contact_count[OPPOSITE(lr)]++;
                 }else{
-                    zmp_force_go_contact_count[L] = 0;
+                    zmp_force_go_contact_count[OPPOSITE(lr)] = 0;
                 }
-                if( inside_vec_lf.head(XY).dot(lf2zmp) / inside_vec_lf.norm() > wp.auto_swing_foot_landing_threshold ){
-                    zmp_force_go_contact_count[R] = 1;
-                }else if(zmp_force_go_contact_count[R] > 0 && zmp_force_go_contact_count[R] < HZ*1 ){
-                    zmp_force_go_contact_count[R]++;
-                }else{
-                    zmp_force_go_contact_count[R] = 0;
-                }
-                out.tgt[lf].go_contact |= (zmp_force_go_contact_count[L] > 0);
-                out.tgt[rf].go_contact |= (zmp_force_go_contact_count[R] > 0);
+                out.opposite_side_of_foot(lr).go_contact |= (zmp_force_go_contact_count[OPPOSITE(lr)] > 0);
+
+                // lock by ref cp
+                out.opposite_side_of_foot(lr).go_contact |= cp_force_go_contact[OPPOSITE(lr)];
             }
         }
         void limitEEWorkspace(HumanPose& out){
             const double MAX_FW = 0.25;
             const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
-            for(int i=0, spl[LR]={rf,lf}, swl[LR]={lf,rf}; i<LR; i++){
-                PoseTGT& support_leg = out.tgt[spl[i]];
-                PoseTGT& swing_leg = out.tgt[swl[i]];
-                hrp::Vector2 inside_vec_baserel = ( spl[i]==rf ? hrp::Vector2(0,+1) : hrp::Vector2(0,-1) );
+            for(int lr=0; lr<LR; lr++){
+                PoseTGT& support_leg = out.foot(lr);
+                PoseTGT& swing_leg = out.opposite_side_of_foot(lr);
+                hrp::Vector2 inside_vec_baserel = ( lr==R ? hrp::Vector2(0,+1) : hrp::Vector2(0,-1) );
                 hrp::Vector2 sp2sw_vec = swing_leg.abs.p.head(XY) - support_leg.cnt.p.head(XY);
                 if(swing_leg.go_contact && sp2sw_vec.norm() > MAX_FW){ sp2sw_vec = sp2sw_vec.normalized() * MAX_FW; }//着地時に足を広げすぎないよう制限
                 Eigen::Matrix2d base_rot;
@@ -567,14 +568,14 @@ class WBMSCore{
             }
         }
         void setFootContactPoseByGoContact(HumanPose& out){
-            for(int i=0, l[LR]={rf,lf}; i<LR; i++){
-                if(out.tgt[l[i]].go_contact){
-                    out.tgt[l[i]].abs.p.head(XY) = out.tgt[l[i]].cnt.p.head(XY);
-                    out.tgt[l[i]].abs.setRpy(out.tgt[l[i]].abs.rpy()(r), out.tgt[l[i]].abs.rpy()(p), out.tgt[l[i]].cnt.rpy()(y));
+            for(int lr=0; lr<LR; lr++){
+                if(out.foot(lr).go_contact){
+                    out.foot(lr).abs.p.head(XY) = out.foot(lr).cnt.p.head(XY);
+                    out.foot(lr).abs.setRpy(out.foot(lr).abs.rpy()(r), out.foot(lr).abs.rpy()(p), out.foot(lr).cnt.rpy()(y));
 
                 }else{
-                    out.tgt[l[i]].cnt.p.head(XY) = out.tgt[l[i]].abs.p.head(XY);
-                    out.tgt[l[i]].cnt.setRpy(out.tgt[l[i]].cnt.rpy()(r), out.tgt[l[i]].cnt.rpy()(p), out.tgt[l[i]].abs.rpy()(y));
+                    out.foot(lr).cnt.p.head(XY) = out.foot(lr).abs.p.head(XY);
+                    out.foot(lr).cnt.setRpy(out.foot(lr).cnt.rpy()(r), out.foot(lr).cnt.rpy()(p), out.foot(lr).abs.rpy()(y));
                 }
             }
         }
@@ -586,44 +587,37 @@ class WBMSCore{
             return true;
         }
         void overwriteFootZFromFootLandOnCommand(HumanPose& out){
-            for(int i=0, l[LR]={rf,lf}; i<LR; i++){
-                if(out.tgt[l[i]].go_contact || cp_force_go_contact[i]){
-                    out.tgt[l[i]].abs.p(Z) = out.tgt[l[i]].offs.p(Z);
+            for(int lr=0; lr<LR; lr++){
+                if(out.foot(lr).go_contact){
+                    out.foot(lr).abs.p(Z) = out.foot(lr).offs.p(Z);
                 }else{
-                    LIMIT_MIN( out.tgt[l[i]].abs.p(Z), out.tgt[l[i]].offs.p(Z)+wp.swing_foot_height_offset);
+                    LIMIT_MIN( out.foot(lr).abs.p(Z), out.foot(lr).offs.p(Z)+wp.swing_foot_height_offset);
                 }
-                const double contact_threshold = 0.005;
-                out.tgt[l[i]].is_contact = ((out.tgt[l[i]].abs.p(Z)-out.tgt[l[i]].offs.p(Z)) <= contact_threshold);
             }
         }
         void setFootRotHorizontalIfGoLanding(HumanPose& out){
-            if( out.tgt[rf].go_contact || cp_force_go_contact[R]){
-                out.tgt[rf].abs.setRpy(0, 0, out.tgt[rf].abs.rpy()(y));
-            }
-            if( out.tgt[lf].go_contact || cp_force_go_contact[L]){
-                out.tgt[lf].abs.setRpy(0, 0, out.tgt[lf].abs.rpy()(y));
+            for(int lr=0; lr<LR; lr++){
+                if( out.foot(lr).go_contact){ out.foot(lr).abs.setRpy(0, 0, out.foot(lr).abs.rpy()(y)); }
             }
         }
         void limitFootVelNearGround(HumanPose& out){
-            for(int i=0, l[2]={rf,lf}; i<2; i++){
-                double fheight = rp_ref_out_old.tgt[l[i]].abs.p(Z) - rp_ref_out_old.tgt[l[i]].offs.p(Z);
-                double horizontal_max_vel = 0 + fheight * 20;
+            for(int lr=0; lr<LR; lr++){
+                const double fheight = rp_ref_out_old.foot(lr).abs.p(Z) - rp_ref_out_old.foot(lr).offs.p(Z);
+                const double horizontal_max_vel = 0 + fheight * 20;
                 for(int j=0; j<XY; j++){
-                    LIMIT_MINMAX( out.tgt[l[i]].abs.p(j), rp_ref_out_old.tgt[l[i]].abs.p(j) - horizontal_max_vel * DT, rp_ref_out_old.tgt[l[i]].abs.p(j) + horizontal_max_vel * DT);
+                    LIMIT_MINMAX( out.foot(lr).abs.p(j), rp_ref_out_old.foot(lr).abs.p(j) - horizontal_max_vel * DT, rp_ref_out_old.foot(lr).abs.p(j) + horizontal_max_vel * DT);
                 }
-                double vertical_max_vel = 0 + fheight * 10;
-                LIMIT_MIN( out.tgt[l[i]].abs.p(Z), rp_ref_out_old.tgt[l[i]].abs.p(Z) - vertical_max_vel * DT);
+                const double vertical_max_vel = 0 + fheight * 10;
+                LIMIT_MIN( out.foot(lr).abs.p(Z), rp_ref_out_old.foot(lr).abs.p(Z) - vertical_max_vel * DT);
             }
         }
         void modifyFootRotAndXYForContact(HumanPose& out){
-            avoidFootSinkIntoFloor(out.tgt[rf],foot_vert3d_act[R]);
-            avoidFootSinkIntoFloor(out.tgt[lf],foot_vert3d_act[L]);
-        }
-        void avoidFootSinkIntoFloor(PoseTGT& foot_in, const hrp::dmatrix& act_sole_vert){
-            hrp::dmatrix act_sole_vert_abs = (foot_in.abs.R * act_sole_vert).colwise() + foot_in.abs.p;// translate and rotate each cols
-            double min_height = act_sole_vert_abs.row(Z).minCoeff(); // pick up min Z height
-            if(min_height < foot_in.offs.p(Z)){
-                foot_in.abs.p(Z) += (foot_in.offs.p(Z) - min_height);
+            for(int lr=0; lr<LR; lr++){
+                hrp::dmatrix act_sole_vert_abs = (out.foot(lr).abs.R * foot_vert3d_act[lr]).colwise() + out.foot(lr).abs.p;// translate and rotate each cols
+                const double min_height = act_sole_vert_abs.row(Z).minCoeff(); // pick up min Z height
+                if(min_height < out.foot(lr).offs.p(Z)){
+                    out.foot(lr).abs.p(Z) += (out.foot(lr).offs.p(Z) - min_height);
+                }
             }
         }
         bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Pose3& rfin_abs, const hrp::Pose3& lfin_abs, hrp::Vector3& com_ans_old, hrp::Vector3& com_ans){
@@ -632,17 +626,16 @@ class WBMSCore{
             hrp::Vector3 com_vel = (com_in - com_ans_old)/DT;
             hrp::dmatrix hull_dcp = createSupportRegionByFootPos(rfin_abs, lfin_abs, offset_fblr(foot_vert_safe_fblr[R], 0.001),    offset_fblr(foot_vert_safe_fblr[L], 0.001));
             hrp::dmatrix hull_acp = createSupportRegionByFootPos(rfin_abs, lfin_abs, offset_fblr(foot_vert_safe_fblr[R], 0.01),     offset_fblr(foot_vert_safe_fblr[L], 0.01));
-            hrp::Vector2 com_vel_ans_2d;
-            regulateCOMVelocityByCapturePointVec( com_ans_old.head(XY), com_vel.head(XY), hull_dcp, hull_acp, com_vel_ans_2d);
+            hrp::Vector2 com_vel_ans_2d = regulateCOMVelocityByCapturePointVec( com_ans_old.head(XY), com_vel.head(XY), hull_dcp, hull_acp);
             com_ans.head(XY) = com_ans_old.head(XY) + com_vel_ans_2d * DT;
             com_ans_old = com_ans;
             cp_dec.head(XY) = com_ans.head(XY) + com_vel_ans_2d * sqrt( H_cur / G );
             cp_acc.head(XY) = com_ans.head(XY) - com_vel_ans_2d * sqrt( H_cur / G );
             return true;
         }
-        void regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const hrp::dmatrix& hull_d, const hrp::dmatrix& hull_a, hrp::Vector2& com_vel_ans){
+        hrp::Vector2 regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const hrp::dmatrix& hull_d, const hrp::dmatrix& hull_a){
             hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
-            com_vel_decel_ok = com_vel_accel_ok = com_vel_ans = com_vel;
+            com_vel_decel_ok = com_vel_accel_ok = com_vel;
 
             const double foot_ave_vel = 2.0;
             double lf_landing_delay = (rp_ref_out.tgt[lf].abs.p[Z] - rp_ref_out.tgt[lf].offs.p[Z]) / foot_ave_vel;
@@ -663,12 +656,11 @@ class WBMSCore{
 
             //ついでに減速CPによる強制遊脚着地も判定
             hrp::dmatrix foot_vert3d_check_wld[LR], one_foot_hull2d[LR];
-            for(int i=0; i<LR; i++){
-                foot_vert3d_check_wld[i] = (rp_ref_out.tgt[F(i)].abs.R * foot_vert3d_check[i]).colwise() + rp_ref_out.tgt[F(i)].abs.p; // rotate and translate in 3D
-                one_foot_hull2d[i] = makeConvexHull2D( foot_vert3d_check_wld[i].topRows(XY) ); // project into 2D and make convex hull
+            for(int lr=0; lr<LR; lr++){
+                foot_vert3d_check_wld[lr] = (rp_ref_out.foot(lr).abs.R * foot_vert3d_check[lr]).colwise() + rp_ref_out.foot(lr).abs.p; // rotate and translate in 3D
+                one_foot_hull2d[lr] = makeConvexHull2D( foot_vert3d_check_wld[lr].topRows(XY) ); // project into 2D and make convex hull
+                cp_force_go_contact[OPPOSITE(lr)] = !isPointInHull2D(cp_dec_ragulated, one_foot_hull2d[lr]); // if CapturePoint go out of R sole region, L foot must be go contact
             }
-            cp_force_go_contact[L] = !isPointInHull2D(cp_dec_ragulated, one_foot_hull2d[R]); // if CapturePoint go out of R sole region, L foot must be go contact
-            cp_force_go_contact[R] = !isPointInHull2D(cp_dec_ragulated, one_foot_hull2d[L]); // if CapturePoint go out of L sole region, R foot must be go contact
 
             //加速CP条件(ACP使用)
             hrp::Vector2 cp_acc_ragulated = com_pos - com_vel * sqrt( H_cur / G );
@@ -678,7 +670,7 @@ class WBMSCore{
             }
 
             //加速減速条件マージ com_vel進行方向をより減速させるものを採用？
-            com_vel_ans = (com_vel_decel_ok.dot(com_vel) < com_vel_accel_ok.dot(com_vel)) ? com_vel_decel_ok : com_vel_accel_ok; //normじゃダメ？
+            return (com_vel_decel_ok.dot(com_vel) < com_vel_accel_ok.dot(com_vel)) ? com_vel_decel_ok : com_vel_accel_ok; //normじゃダメ？
         }
         hrp::dmatrix createSupportRegionByFootPos(const hrp::Pose3& rfin_abs, const hrp::Pose3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn){
             hrp::dmatrix rf_sole_verts_abs = (rfin_abs.R * make_rect_3d(rf_mgn)).colwise() + rfin_abs.p;

From 771632565cbd98b9f0676439b5989b56e2ba5af6 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 19 Sep 2019 13:24:30 +0900
Subject: [PATCH 176/217] add params and auto com

---
 idl/WholeBodyMasterSlaveService.idl           |  26 +-
 .../FullbodyInverseKinematicsSolver.h         |   8 +-
 .../WholeBodyMasterSlave.cpp                  |  71 +++---
 rtc/WholeBodyMasterSlave/wbms_core.h          | 239 +++++++++---------
 4 files changed, 178 insertions(+), 166 deletions(-)

diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
index 04192f93e85..27322999a5d 100644
--- a/idl/WholeBodyMasterSlaveService.idl
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -8,26 +8,30 @@ module OpenHRP
   
   interface WholeBodyMasterSlaveService
   {
+    typedef sequence<double, 2> DblSequence2;
     typedef sequence<double, 3> DblSequence3;
-    //typedef sequence<double, 4> DblSequence4;
+    typedef sequence<double, 4> DblSequence4;
+    typedef double DblArray2[2];
     typedef double DblArray3[3];
     typedef double DblArray4[4];
     typedef sequence<string> StrSequence;
 
-    /**
-     * HumanSync
-     */
     struct WholeBodyMasterSlaveParam
     {
-        double auto_swing_foot_landing_threshold;
+        boolean auto_com_mode;
+        double additional_double_support_time;
+        double auto_com_foot_move_detect_height;
+        double base_to_hand_min_distance;
+        double capture_point_extend_ratio;
+        double foot_collision_avoidance_distance;
         double human_to_robot_ratio;
-        double set_com_height_fix_val;
-        double swing_foot_height_offset;
+        double max_double_support_width;
         double upper_body_rmc_ratio;
-        boolean set_com_height_fix;
-        boolean use_head;
-        boolean use_upper;
-        boolean use_lower;
+        double single_foot_zmp_safety_distance;
+        double swing_foot_height_offset;
+        DblSequence3 com_offset;
+        DblSequence4 actual_foot_vert_fbio;
+        DblSequence4 safety_foot_vert_fbio;
         StrSequence use_joints;
         StrSequence use_targets;
     };
diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 75c51f5318e..222797e54ff 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -266,9 +266,13 @@ class FullbodyInverseKinematicsSolver{
 
             // update rootlink pos rot
             m_robot->rootLink()->p += dq_all.tail(6).head(3);
-            hrp::Matrix33 dR;
             const hrp::Vector3 omega = dq_all.tail(6).tail(3);
-            hrp::calcRodrigues(dR, omega.normalized(), omega.norm());
+            hrp::Matrix33 dR;
+            if(omega.norm() > 1e-12){
+                hrp::calcRodrigues(dR, omega.normalized(), omega.norm());
+            }else{
+                dR = hrp::Matrix33::Identity();
+            }
             rats::rotm3times(m_robot->rootLink()->R, dR, m_robot->rootLink()->R); // safe rot operation with quartanion normalization
             if(!m_robot->rootLink()->R.isUnitary()){
                 std::cerr <<"[FullbodyIK] WARN m_robot->rootLink()->R is not Unitary, something wrong !" << std::endl;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 39218252dbb..b26c5bc4797 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -448,11 +448,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.localR = ee_ikc_map["rleg"].localR;
         tmp.targetPos = rf_ref.p;
         tmp.targetRpy = rf_ref.rpy();
-        if(wbms->rp_ref_out.tgt[rf].is_contact()){
-            tmp.constraint_weight = hrp::dvector6::Constant(3);
-        }else{
-            tmp.constraint_weight = hrp::dvector6::Constant(0.1);
-        }
+        tmp.constraint_weight = wbms->rp_ref_out.tgt[rf].is_contact() ? hrp::dvector6::Constant(3) : hrp::dvector6::Constant(0.1);
         ikc_list.push_back(tmp);
     }
     if(has(wbms->wp.use_targets, "lleg")){
@@ -462,11 +458,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.localR = ee_ikc_map["lleg"].localR;
         tmp.targetPos = lf_ref.p;
         tmp.targetRpy = lf_ref.rpy();
-        if(wbms->rp_ref_out.tgt[lf].is_contact()){
-            tmp.constraint_weight = hrp::dvector6::Constant(3);
-        }else{
-            tmp.constraint_weight = hrp::dvector6::Constant(0.1);
-        }
+        tmp.constraint_weight = wbms->rp_ref_out.tgt[lf].is_contact() ? hrp::dvector6::Constant(3) : hrp::dvector6::Constant(0.1);
         ikc_list.push_back(tmp);
     }
     if(has(wbms->wp.use_targets, "rarm")){
@@ -587,6 +579,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
     if( fik->m_robot->link("LARM_JOINT2") != NULL) fik->m_robot->link("LARM_JOINT2")->llimit = deg2rad(45);
     if( fik->m_robot->link("RLEG_JOINT3") != NULL) fik->m_robot->link("RLEG_JOINT3")->llimit = deg2rad(15);//膝伸びきり防止のため
     if( fik->m_robot->link("LLEG_JOINT3") != NULL) fik->m_robot->link("LLEG_JOINT3")->llimit = deg2rad(15);
+    if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 0;//実機修理中
 
     if( fik->m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_Y")->jointId) = 0.1;//K
     if( fik->m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_P")->jointId) = 0.1;
@@ -704,40 +697,56 @@ bool WholeBodyMasterSlave::stopWholeBodyMasterSlave(){
         return false;
     }
 }
-
+namespace hrp{
+//    hrp::Vector2 to_Vector2(const OpenHRP::WholeBodyMasterSlaveService::DblSequence2& in){ return (hrp::Vector2()<< in[0],in[1]).finished(); }
+//    hrp::Vector2 to_Vector3(const OpenHRP::WholeBodyMasterSlaveService::DblSequence3& in){ return hrp::dvector::Map(in.get_buffer(), in.length()); }
+//    hrp::Vector2 to_Vector4(const OpenHRP::WholeBodyMasterSlaveService::DblSequence4& in){ return (hrp::Vector4()<< in[0],in[1]).finished(); }
+    OpenHRP::WholeBodyMasterSlaveService::DblSequence2 to_DblSequence2(const hrp::Vector2& in){
+        OpenHRP::WholeBodyMasterSlaveService::DblSequence2 ret; ret.length(in.size()); hrp::Vector2::Map(ret.get_buffer()) = in; return ret; }
+    OpenHRP::WholeBodyMasterSlaveService::DblSequence3 to_DblSequence3(const hrp::Vector3& in){
+        OpenHRP::WholeBodyMasterSlaveService::DblSequence3 ret; ret.length(in.size()); hrp::Vector3::Map(ret.get_buffer()) = in; return ret; }
+    OpenHRP::WholeBodyMasterSlaveService::DblSequence4 to_DblSequence4(const hrp::Vector4& in){
+        OpenHRP::WholeBodyMasterSlaveService::DblSequence4 ret; ret.length(in.size()); hrp::Vector4::Map(ret.get_buffer()) = in; return ret; }
+}
 
 bool WholeBodyMasterSlave::setParams(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
     RTC_INFO_STREAM("setWholeBodyMasterSlaveParam");
-    wbms->wp.auto_swing_foot_landing_threshold  = i_param.auto_swing_foot_landing_threshold;
+    wbms->wp.auto_com_mode                      = i_param.auto_com_mode;
+    wbms->wp.additional_double_support_time     = i_param.additional_double_support_time;
+    wbms->wp.auto_com_foot_move_detect_height   = i_param.auto_com_foot_move_detect_height;
+    wbms->wp.base_to_hand_min_distance          = i_param.base_to_hand_min_distance;
+    wbms->wp.capture_point_extend_ratio         = i_param.capture_point_extend_ratio;
+    wbms->wp.foot_collision_avoidance_distance  = i_param.foot_collision_avoidance_distance;
     wbms->wp.human_to_robot_ratio               = i_param.human_to_robot_ratio;
-    wbms->wp.set_com_height_fix                 = i_param.set_com_height_fix;
-    wbms->wp.set_com_height_fix_val             = i_param.set_com_height_fix_val;
-    wbms->wp.swing_foot_height_offset           = i_param.swing_foot_height_offset;
+    wbms->wp.max_double_support_width           = i_param.max_double_support_width;
     wbms->wp.upper_body_rmc_ratio               = i_param.upper_body_rmc_ratio;
-    if(mode.now() == MODE_IDLE){
-        wbms->wp.use_head                       = i_param.use_head;
-        wbms->wp.use_upper                      = i_param.use_upper;
-        wbms->wp.use_lower                      = i_param.use_lower;
-        wbms->wp.use_joints                     = hrp::to_string_vector(i_param.use_joints);
-        wbms->wp.use_targets                    = hrp::to_string_vector(i_param.use_targets);
-    }else{
-        RTC_WARN_STREAM("use_head, use_upper, use_lower can be changed in MODE_IDLE");
-    }
+    wbms->wp.single_foot_zmp_safety_distance    = i_param.single_foot_zmp_safety_distance;
+    wbms->wp.swing_foot_height_offset           = i_param.swing_foot_height_offset;
+    wbms->wp.com_offset                         = hrp::Vector3::Map(i_param.com_offset.get_buffer());
+    wbms->wp.actual_foot_vert_fbio              = hrp::Vector4::Map(i_param.actual_foot_vert_fbio.get_buffer());
+    wbms->wp.safety_foot_vert_fbio              = hrp::Vector4::Map(i_param.safety_foot_vert_fbio.get_buffer());
+    wbms->wp.use_joints                         = hrp::to_string_vector(i_param.use_joints);
+    wbms->wp.use_targets                        = hrp::to_string_vector(i_param.use_targets);
     return true;
 }
 
 
 bool WholeBodyMasterSlave::getParams(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
     RTC_INFO_STREAM("getWholeBodyMasterSlaveParam");
-    i_param.auto_swing_foot_landing_threshold   = wbms->wp.auto_swing_foot_landing_threshold;
+    i_param.auto_com_mode                       = wbms->wp.auto_com_mode;
+    i_param.additional_double_support_time      = wbms->wp.additional_double_support_time;
+    i_param.auto_com_foot_move_detect_height    = wbms->wp.auto_com_foot_move_detect_height;
+    i_param.base_to_hand_min_distance           = wbms->wp.base_to_hand_min_distance;
+    i_param.capture_point_extend_ratio          = wbms->wp.capture_point_extend_ratio;
+    i_param.foot_collision_avoidance_distance   = wbms->wp.foot_collision_avoidance_distance;
     i_param.human_to_robot_ratio                = wbms->wp.human_to_robot_ratio;
-    i_param.set_com_height_fix                  = wbms->wp.set_com_height_fix;
-    i_param.set_com_height_fix_val              = wbms->wp.set_com_height_fix_val;
-    i_param.swing_foot_height_offset            = wbms->wp.swing_foot_height_offset;
+    i_param.max_double_support_width            = wbms->wp.max_double_support_width;
     i_param.upper_body_rmc_ratio                = wbms->wp.upper_body_rmc_ratio;
-    i_param.use_head                            = wbms->wp.use_head;
-    i_param.use_upper                           = wbms->wp.use_upper;
-    i_param.use_lower                           = wbms->wp.use_lower;
+    i_param.single_foot_zmp_safety_distance     = wbms->wp.single_foot_zmp_safety_distance;
+    i_param.swing_foot_height_offset            = wbms->wp.swing_foot_height_offset;
+    i_param.com_offset                          = hrp::to_DblSequence3(wbms->wp.com_offset);
+    i_param.actual_foot_vert_fbio               = hrp::to_DblSequence4(wbms->wp.actual_foot_vert_fbio);
+    i_param.safety_foot_vert_fbio               = hrp::to_DblSequence4(wbms->wp.safety_foot_vert_fbio);
     i_param.use_joints                          = hrp::to_StrSequence(wbms->wp.use_joints);
     i_param.use_targets                         = hrp::to_StrSequence(wbms->wp.use_targets);
     return true;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 88ed5f2a2b2..8f073e9efcc 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -107,8 +107,8 @@ class HumanPose{
         }
         PoseTGT&                        foot    (const int lr)      { assert(lr == R || lr == L); return (lr == R) ? tgt[rf] : tgt[lf]; }
         const PoseTGT&                  foot    (const int lr) const{ assert(lr == R || lr == L); return (lr == R) ? tgt[rf] : tgt[lf]; }
-        PoseTGT&        opposite_side_of_foot   (const int lr)      { assert(lr == R || lr == L); return (lr == R) ? tgt[lf] : tgt[rf]; }
-        const PoseTGT&  opposite_side_of_foot   (const int lr) const{ assert(lr == R || lr == L); return (lr == R) ? tgt[lf] : tgt[rf]; }
+        PoseTGT&                        hand    (const int lr)      { assert(lr == R || lr == L); return (lr == R) ? tgt[rh] : tgt[lh]; }
+        const PoseTGT&                  hand    (const int lr) const{ assert(lr == R || lr == L); return (lr == R) ? tgt[rh] : tgt[lh]; }
         PoseTGT& tgt_by_str(const std::string ln){
             assert(ln == "com" || ln == "head" || ln == "lleg" || ln == "rleg" || ln == "larm" || ln == "rarm" );
             if      (ln == "lleg"){ return tgt[lf]; }
@@ -363,7 +363,7 @@ class CapsuleCollisionChecker {
 class WBMSCore{
     private:
         double HZ, DT;
-        BiquadIIRFilterVec calcacc_v_filters, acc4zmp_v_filters, com_in_filter;
+        BiquadIIRFilterVec acc4zmp_v_filters, com_in_filter;
         hrp::Vector3 com_old, com_oldold, comacc, com_CP_ref_old;
         HumanPose rp_ref_out_old;
         unsigned int loop;
@@ -373,25 +373,45 @@ class WBMSCore{
     public:
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
         double H_cur;
-        HumanPose hp_wld_raw, hp_plot, rp_ref_out, rp_ref_vel_old;
+        HumanPose hp_wld_raw, hp_plot, rp_ref_out;
         hrp::Pose3 baselinkpose;
         hrp::Vector3 cp_dec, cp_acc;
-        hrp::dmatrix foot_vert3d_act[LR], foot_vert3d_check[LR]; // A = [p0 p1 p2 p3]
+        hrp::Vector4 act_foot_vert_fblr[LR], safe_foot_vert_fblr[LR];
         hrp::Vector2 com_forcp_ref,com_vel_forcp_ref;
-        hrp::dvector6 invdyn_ft;
-        hrp::Vector4 foot_vert_act_fblr[LR], foot_vert_safe_fblr[LR];//, foot_vert_check_fblr[LR];
-        struct WBMSparameters {
-            double auto_swing_foot_landing_threshold;
-            double human_to_robot_ratio;
-            bool set_com_height_fix;
-            double set_com_height_fix_val;
-            double swing_foot_height_offset;
-            double upper_body_rmc_ratio;
-            bool use_head;
-            bool use_upper;
-            bool use_lower;
-            std::vector<std::string> use_joints;
-            std::vector<std::string> use_targets;
+        class WBMSParams {
+            public:
+                bool auto_com_mode;
+                double additional_double_support_time;
+                double auto_com_foot_move_detect_height;
+                double base_to_hand_min_distance;
+                double capture_point_extend_ratio;
+                double foot_collision_avoidance_distance;
+                double human_to_robot_ratio;
+                double max_double_support_width;
+                double upper_body_rmc_ratio;
+                double single_foot_zmp_safety_distance;
+                double swing_foot_height_offset;
+                hrp::Vector3 com_offset;
+                hrp::Vector4 actual_foot_vert_fbio;
+                hrp::Vector4 safety_foot_vert_fbio;
+                std::vector<std::string> use_joints;
+                std::vector<std::string> use_targets;
+            WBMSParams(){
+                auto_com_mode                       = false;
+                additional_double_support_time      = 0.5;
+                auto_com_foot_move_detect_height    = 0.03;
+                base_to_hand_min_distance           = 0.5;
+                capture_point_extend_ratio          = 1.0;
+                foot_collision_avoidance_distance   = 0.16;
+                human_to_robot_ratio                = 1.0;//human 1.1m vs jaxon 1.06m
+                max_double_support_width            = 0.4;
+                upper_body_rmc_ratio                = 0.0;
+                single_foot_zmp_safety_distance     = 0.04;
+                swing_foot_height_offset            = 0.02;
+                com_offset                          << 0, 0, 0;
+                actual_foot_vert_fbio               << 0.13, -0.10,  0.06, -0.08;
+                safety_foot_vert_fbio               << 0.02, -0.02,  0.02,  0.01;
+            }
         } wp;
         struct ActualRobotState {
             hrp::Vector3 com, zmp, st_zmp;
@@ -401,38 +421,23 @@ class WBMSCore{
             DT = dt;
             HZ = (int)(1.0/DT);
             loop = 0;
-            //////////  hrp::Vector は初期化必要  ///////////
             com_old = com_oldold = comacc = hrp::Vector3::Zero();
             cp_dec = cp_acc = hrp::Vector3::Zero();
-            invdyn_ft = hrp::dvector6::Zero();
-            calcacc_v_filters.setParameter(5, HZ, Q_BUTTERWORTH);//加速度計算用
             acc4zmp_v_filters.setParameter(5, HZ, Q_BUTTERWORTH);//ZMP生成用ほぼこの値でいい
             com_in_filter.setParameter(1, HZ);
-            foot_vert_safe_fblr[R] << 0.02, -0.02,  0.02,  0.01;
-            foot_vert_safe_fblr[L] << 0.02, -0.02, -0.01, -0.02;
-            for(int i=0;i<LR;i++){
-                foot_vert3d_act[i]      = make_rect_3d(foot_vert_act_fblr[i]);
-                foot_vert3d_check[i]    = make_rect_3d(offset_fblr(foot_vert_safe_fblr[i], 0.002));
+            for(int lr=0; lr<LR; lr++){
+                act_foot_vert_fblr[lr]          = fbio2fblr(wp.actual_foot_vert_fbio, lr);
+                safe_foot_vert_fblr[lr]         = fbio2fblr(wp.safety_foot_vert_fbio, lr);
+                cp_force_go_contact[lr]         = false;
+                zmp_force_go_contact_count[lr]  = 0;
             }
-            cp_force_go_contact[R] = cp_force_go_contact[L] = false;
-            zmp_force_go_contact_count[R] = zmp_force_go_contact_count[L] = 0;
-            wp.auto_swing_foot_landing_threshold = 0.04;
-            wp.human_to_robot_ratio = 1.0;//human 1.1m vs jaxon 1.06m
-            wp.set_com_height_fix = false;
-            wp.set_com_height_fix_val = 0.02;
-            wp.swing_foot_height_offset = 0.02;
-            wp.upper_body_rmc_ratio = 0.0;
-            wp.use_head = true;
-            wp.use_upper = true;
-            wp.use_lower = true;
             rp_ref_out_old.reset();
             rp_ref_out.reset();
-            rp_ref_vel_old.reset();
             is_initial_loop = true;
             cout<<"WBMSCore constructed"<<endl;
         }
         ~WBMSCore(){ cout<<"WBMSCore destructed"<<endl; }
-        hrp::dmatrix make_rect_3d(const hrp::Vector4& ForwardBackLeftRight){
+        hrp::dmatrix make_rect_3d(const hrp::Vector4& ForwardBackLeftRight){ // A = [p0 p1 p2 p3]
             return (hrp::dmatrix(3,4) << // (XYZ,4)とかの#defineマクロ効かない
                 hrp::Vector3(ForwardBackLeftRight(0), ForwardBackLeftRight(2), 0),//左前
                 hrp::Vector3(ForwardBackLeftRight(1), ForwardBackLeftRight(2), 0),//左後
@@ -475,19 +480,26 @@ class WBMSCore{
             is_initial_loop = true;
             initializeHumanPoseFromCurrentInput();
             initializeRobotPoseFromHRPBody(robot_in, _ee_ikc_map);
-            rp_ref_out_old = rp_ref_vel_old = rp_ref_out;
+            rp_ref_out_old = rp_ref_out;
         }
         void update(){//////////  メインループ  ////////////
             convertHumanToRobot                 (hp_wld_raw, rp_ref_out);
             hp_plot = rp_ref_out;
-            if(wp.set_com_height_fix) rp_ref_out.tgt[com].abs.p(Z) = rp_ref_out.tgt[com].offs.p(Z) + wp.set_com_height_fix_val;//膝曲げトルクで落ちるときの応急措置
             lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
             limitEEWorkspace                    (rp_ref_out);
             setFootContactPoseByGoContact       (rp_ref_out);
-            applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, rp_ref_out.tgt[com].abs.p);
-            overwriteFootZFromFootLandOnCommand (rp_ref_out);
-            setFootRotHorizontalIfGoLanding     (rp_ref_out);
             limitFootVelNearGround              (rp_ref_out);
+            avoidFootSinkIntoFloor              (rp_ref_out);
+
+            if(wp.auto_com_mode){
+                const bool rf_is_up = (hp_wld_raw.tgt[rf].abs.p(Z) - hp_wld_raw.tgt[rf].offs.p(Z) > wp.auto_com_foot_move_detect_height);
+                const bool lf_is_up = (hp_wld_raw.tgt[lf].abs.p(Z) - hp_wld_raw.tgt[lf].offs.p(Z) > wp.auto_com_foot_move_detect_height);
+                if      ( rf_is_up && !lf_is_up){ rp_ref_out.tgt[com].abs.p.head(XY) = rp_ref_out.tgt[lf].cnt.p.head(XY); }
+                else if (!rf_is_up &&  lf_is_up){ rp_ref_out.tgt[com].abs.p.head(XY) = rp_ref_out.tgt[rf].cnt.p.head(XY); }
+                else                            { rp_ref_out.tgt[com].abs.p.head(XY) = (rp_ref_out.tgt[rf].cnt.p.head(XY) + rp_ref_out.tgt[lf].cnt.p.head(XY)) / 2; }
+            }
+
+            applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, rp_ref_out.tgt[com].abs.p);
             applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
             com_forcp_ref = rp_ref_out.tgt[com].abs.p.head(XY);
             static hrp::Vector2 com_forcp_ref_old;
@@ -496,7 +508,6 @@ class WBMSCore{
             applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
             applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, rp_ref_out.tgt[com].abs.p);
             applyZMPCalcFromCOM                 (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);//結局STに送るZMPは最終段で計算するからこれ意味ない
-            modifyFootRotAndXYForContact        (rp_ref_out);
             H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[lf].abs.p(Z));
             rp_ref_out_old = rp_ref_out;
             loop++;
@@ -514,92 +525,70 @@ class WBMSCore{
                 out.tgt[l[i]].w = in.tgt[l[i]].w;
                 out.tgt[l[i]].go_contact = in.tgt[l[i]].go_contact;
             }
+            out.tgt[com].abs.p += wp.com_offset;
             out.tgt[zmp].abs = in.tgt[zmp].abs;//最近使わない
         }
         void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
             hrp::Vector2 to_opposite_foot[LR], ref_zmp_from_foot[LR], act_zmp_from_foot[LR];
             for(int lr=0; lr<LR; lr++){
-                to_opposite_foot[lr] = old.opposite_side_of_foot(lr).abs.p.head(XY) - old.foot(lr).abs.p.head(XY);
+                to_opposite_foot[lr] = old.foot(OPPOSITE(lr)).abs.p.head(XY) - old.foot(lr).abs.p.head(XY);
                 if(to_opposite_foot[lr].norm() < 1e-3){ std::cerr << "to_opposite_foot[lr].norm() < 1e-3 :" << to_opposite_foot[lr].transpose()<<std::endl; }
-
-                // lock by ref_zmp
+                ///// lock by ref_zmp
                 ref_zmp_from_foot[lr] = act_rs.zmp.head(XY) - old.foot(lr).abs.p.head(XY);
-                out.opposite_side_of_foot(lr).go_contact |= ( ref_zmp_from_foot[lr].dot(to_opposite_foot[lr]) / to_opposite_foot[lr].norm() > wp.auto_swing_foot_landing_threshold );
-
-                // lock by acts_zmp
+                out.foot(OPPOSITE(lr)).go_contact |= ( ref_zmp_from_foot[lr].dot(to_opposite_foot[lr]) / to_opposite_foot[lr].norm() > wp.single_foot_zmp_safety_distance );
+                ///// lock by acts_zmp
                 act_zmp_from_foot[lr] = act_rs.st_zmp.head(XY) - old.foot(lr).abs.p.head(XY);
-                if( act_zmp_from_foot[lr].dot(to_opposite_foot[lr]) / to_opposite_foot[lr].norm() > wp.auto_swing_foot_landing_threshold ){
+                if( act_zmp_from_foot[lr].dot(to_opposite_foot[lr]) / to_opposite_foot[lr].norm() > wp.single_foot_zmp_safety_distance ){
                     zmp_force_go_contact_count[OPPOSITE(lr)] = 1;
-                }else if(zmp_force_go_contact_count[OPPOSITE(lr)] > 0 && zmp_force_go_contact_count[OPPOSITE(lr)] < HZ*0.5 ){
+                }else if(zmp_force_go_contact_count[OPPOSITE(lr)] > 0 && zmp_force_go_contact_count[OPPOSITE(lr)] < HZ * wp.additional_double_support_time ){
                     zmp_force_go_contact_count[OPPOSITE(lr)]++;
                 }else{
                     zmp_force_go_contact_count[OPPOSITE(lr)] = 0;
                 }
-                out.opposite_side_of_foot(lr).go_contact |= (zmp_force_go_contact_count[OPPOSITE(lr)] > 0);
-
-                // lock by ref cp
-                out.opposite_side_of_foot(lr).go_contact |= cp_force_go_contact[OPPOSITE(lr)];
+                out.foot(OPPOSITE(lr)).go_contact |= (zmp_force_go_contact_count[OPPOSITE(lr)] > 0);
+                ///// lock by ref cp
+                out.foot(OPPOSITE(lr)).go_contact |= cp_force_go_contact[OPPOSITE(lr)];
             }
         }
         void limitEEWorkspace(HumanPose& out){
-            const double MAX_FW = 0.25;
-            const double FOOT_2_FOOT_COLLISION_MARGIIN = 0.16;
             for(int lr=0; lr<LR; lr++){
                 PoseTGT& support_leg = out.foot(lr);
-                PoseTGT& swing_leg = out.opposite_side_of_foot(lr);
+                PoseTGT& swing_leg = out.foot(OPPOSITE(lr));
                 hrp::Vector2 inside_vec_baserel = ( lr==R ? hrp::Vector2(0,+1) : hrp::Vector2(0,-1) );
                 hrp::Vector2 sp2sw_vec = swing_leg.abs.p.head(XY) - support_leg.cnt.p.head(XY);
-                if(swing_leg.go_contact && sp2sw_vec.norm() > MAX_FW){ sp2sw_vec = sp2sw_vec.normalized() * MAX_FW; }//着地時に足を広げすぎないよう制限
                 Eigen::Matrix2d base_rot;
                 base_rot = Eigen::Rotation2Dd(baselinkpose.rpy()(y));
                 hrp::Vector2 sp2sw_vec_baserel = base_rot.transpose() * sp2sw_vec;
-                if( sp2sw_vec_baserel.dot(inside_vec_baserel) < FOOT_2_FOOT_COLLISION_MARGIIN){
-                    sp2sw_vec_baserel += inside_vec_baserel * (FOOT_2_FOOT_COLLISION_MARGIIN - sp2sw_vec_baserel.dot(inside_vec_baserel));
+                if( sp2sw_vec_baserel.dot(inside_vec_baserel) < wp.foot_collision_avoidance_distance){
+                    sp2sw_vec_baserel += inside_vec_baserel * (wp.foot_collision_avoidance_distance - sp2sw_vec_baserel.dot(inside_vec_baserel));
                 }
                 sp2sw_vec = base_rot * sp2sw_vec_baserel;
                 swing_leg.abs.p.head(XY) = sp2sw_vec + support_leg.cnt.p.head(XY);
             }
-            const double base2hand_min = 0.5;// for jaxon demo
-            for(int i=0, l[2]={rh,lh}; i<2; i++){
-                hrp::Vector2 horizontal_dist = out.tgt[l[i]].abs.p.head(XY) - baselinkpose.p.head(XY);
-                if(horizontal_dist.norm() < base2hand_min){
-                    out.tgt[l[i]].abs.p.head(XY) = baselinkpose.p.head(XY) + base2hand_min * horizontal_dist.normalized();
+            for(int lr=0; lr<LR; lr++){
+                hrp::Vector2 horizontal_dist = out.hand(lr).abs.p.head(XY) - baselinkpose.p.head(XY);
+                if(horizontal_dist.norm() < wp.base_to_hand_min_distance){
+                    out.hand(lr).abs.p.head(XY) = baselinkpose.p.head(XY) + wp.base_to_hand_min_distance * horizontal_dist.normalized();
                 }
             }
         }
         void setFootContactPoseByGoContact(HumanPose& out){
             for(int lr=0; lr<LR; lr++){
                 if(out.foot(lr).go_contact){
-                    out.foot(lr).abs.p.head(XY) = out.foot(lr).cnt.p.head(XY);
-                    out.foot(lr).abs.setRpy(out.foot(lr).abs.rpy()(r), out.foot(lr).abs.rpy()(p), out.foot(lr).cnt.rpy()(y));
-
+                    out.foot(lr).abs.p = out.foot(lr).cnt.p;
+                    out.foot(lr).abs.setRpy(0, 0, out.foot(lr).cnt.rpy()(y));
                 }else{
                     out.foot(lr).cnt.p.head(XY) = out.foot(lr).abs.p.head(XY);
-                    out.foot(lr).cnt.setRpy(out.foot(lr).cnt.rpy()(r), out.foot(lr).cnt.rpy()(p), out.foot(lr).abs.rpy()(y));
-                }
-            }
-        }
-        bool applyCOMToSupportRegionLimit(const hrp::Pose3& rfin_abs, const hrp::Pose3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
-            hrp::dmatrix hull_com = createSupportRegionByFootPos(rfin_abs, lfin_abs, foot_vert_safe_fblr[R], foot_vert_safe_fblr[L]);
-            if(!isPointInHull2D(comin_abs.head(XY), hull_com)){
-                comin_abs.head(XY) = calcNearestPointOnHull(comin_abs.head(XY), hull_com);
-            }//外に出たら最近傍点に頭打ち
-            return true;
-        }
-        void overwriteFootZFromFootLandOnCommand(HumanPose& out){
-            for(int lr=0; lr<LR; lr++){
-                if(out.foot(lr).go_contact){
-                    out.foot(lr).abs.p(Z) = out.foot(lr).offs.p(Z);
-                }else{
+                    out.foot(lr).cnt.setRpy(0, 0, out.foot(lr).abs.rpy()(y));
                     LIMIT_MIN( out.foot(lr).abs.p(Z), out.foot(lr).offs.p(Z)+wp.swing_foot_height_offset);
+                    /////着地時に足を広げすぎないよう制限
+                    const hrp::Vector2 spport_to_swing = out.foot(lr).cnt.p.head(XY) - out.foot(OPPOSITE(lr)).cnt.p.head(XY);
+                    if(spport_to_swing.norm() > wp.max_double_support_width){
+                        out.foot(lr).cnt.p.head(XY) = out.foot(OPPOSITE(lr)).cnt.p.head(XY) + spport_to_swing.normalized() * wp.max_double_support_width;
+                    }
                 }
             }
         }
-        void setFootRotHorizontalIfGoLanding(HumanPose& out){
-            for(int lr=0; lr<LR; lr++){
-                if( out.foot(lr).go_contact){ out.foot(lr).abs.setRpy(0, 0, out.foot(lr).abs.rpy()(y)); }
-            }
-        }
         void limitFootVelNearGround(HumanPose& out){
             for(int lr=0; lr<LR; lr++){
                 const double fheight = rp_ref_out_old.foot(lr).abs.p(Z) - rp_ref_out_old.foot(lr).offs.p(Z);
@@ -611,21 +600,28 @@ class WBMSCore{
                 LIMIT_MIN( out.foot(lr).abs.p(Z), rp_ref_out_old.foot(lr).abs.p(Z) - vertical_max_vel * DT);
             }
         }
-        void modifyFootRotAndXYForContact(HumanPose& out){
+        void avoidFootSinkIntoFloor(HumanPose& out){
             for(int lr=0; lr<LR; lr++){
-                hrp::dmatrix act_sole_vert_abs = (out.foot(lr).abs.R * foot_vert3d_act[lr]).colwise() + out.foot(lr).abs.p;// translate and rotate each cols
+                hrp::dmatrix act_sole_vert_abs = (out.foot(lr).abs.R * make_rect_3d(act_foot_vert_fblr[lr])).colwise() + out.foot(lr).abs.p;// translate and rotate each cols
                 const double min_height = act_sole_vert_abs.row(Z).minCoeff(); // pick up min Z height
                 if(min_height < out.foot(lr).offs.p(Z)){
                     out.foot(lr).abs.p(Z) += (out.foot(lr).offs.p(Z) - min_height);
                 }
             }
         }
+        bool applyCOMToSupportRegionLimit(const hrp::Pose3& rfin_abs, const hrp::Pose3& lfin_abs, hrp::Vector3& comin_abs){//boost::geometryがUbuntu12だとないから・・・
+            const hrp::dmatrix hull_com = createSupportRegionByFootPos(rfin_abs, lfin_abs, safe_foot_vert_fblr[R], safe_foot_vert_fblr[L]);
+            if(!isPointInHull2D(comin_abs.head(XY), hull_com)){
+                comin_abs.head(XY) = calcNearestPointOnHull(comin_abs.head(XY), hull_com);
+            }//外に出たら最近傍点に頭打ち
+            return true;
+        }
         bool applyCOMStateLimitByCapturePoint(const hrp::Vector3& com_in, const hrp::Pose3& rfin_abs, const hrp::Pose3& lfin_abs, hrp::Vector3& com_ans_old, hrp::Vector3& com_ans){
             com_ans = com_in;
             if (is_initial_loop)com_ans_old = com_in;
             hrp::Vector3 com_vel = (com_in - com_ans_old)/DT;
-            hrp::dmatrix hull_dcp = createSupportRegionByFootPos(rfin_abs, lfin_abs, offset_fblr(foot_vert_safe_fblr[R], 0.001),    offset_fblr(foot_vert_safe_fblr[L], 0.001));
-            hrp::dmatrix hull_acp = createSupportRegionByFootPos(rfin_abs, lfin_abs, offset_fblr(foot_vert_safe_fblr[R], 0.01),     offset_fblr(foot_vert_safe_fblr[L], 0.01));
+            hrp::dmatrix hull_dcp = createSupportRegionByFootPos(rfin_abs, lfin_abs, offset_fblr(safe_foot_vert_fblr[R], 0.001),    offset_fblr(safe_foot_vert_fblr[L], 0.001));
+            hrp::dmatrix hull_acp = createSupportRegionByFootPos(rfin_abs, lfin_abs, offset_fblr(safe_foot_vert_fblr[R], 0.01),     offset_fblr(safe_foot_vert_fblr[L], 0.01));
             hrp::Vector2 com_vel_ans_2d = regulateCOMVelocityByCapturePointVec( com_ans_old.head(XY), com_vel.head(XY), hull_dcp, hull_acp);
             com_ans.head(XY) = com_ans_old.head(XY) + com_vel_ans_2d * DT;
             com_ans_old = com_ans;
@@ -634,7 +630,7 @@ class WBMSCore{
             return true;
         }
         hrp::Vector2 regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const hrp::dmatrix& hull_d, const hrp::dmatrix& hull_a){
-            hrp::Vector2 com_vel_decel_ok,com_vel_accel_ok;
+            hrp::Vector2 com_vel_decel_ok, com_vel_accel_ok;
             com_vel_decel_ok = com_vel_accel_ok = com_vel;
 
             const double foot_ave_vel = 2.0;
@@ -649,27 +645,27 @@ class WBMSCore{
             //        foot_landing_delay = 0;
             //      }
             hrp::Vector2 cp_dec_ragulated = com_pos + com_vel * ( sqrt( H_cur / G ) + foot_landing_delay);
+            cp_dec_ragulated *= wp.capture_point_extend_ratio;
             if(!isPointInHull2D(cp_dec_ragulated, hull_d)){
                 calcCrossPointOnHull(com_pos, cp_dec_ragulated, hull_d, cp_dec_ragulated);
                 com_vel_decel_ok = (cp_dec_ragulated - com_pos) / ( sqrt( H_cur / G ) + foot_landing_delay);
             }
-
-            //ついでに減速CPによる強制遊脚着地も判定
-            hrp::dmatrix foot_vert3d_check_wld[LR], one_foot_hull2d[LR];
+            ///// ついでに減速CPによる強制遊脚着地も判定
+            hrp::dmatrix foot_vert3d_check_wld[LR], one_foot_hull2d[LR], one_foot_vert_3d_for_check[LR];
             for(int lr=0; lr<LR; lr++){
-                foot_vert3d_check_wld[lr] = (rp_ref_out.foot(lr).abs.R * foot_vert3d_check[lr]).colwise() + rp_ref_out.foot(lr).abs.p; // rotate and translate in 3D
-                one_foot_hull2d[lr] = makeConvexHull2D( foot_vert3d_check_wld[lr].topRows(XY) ); // project into 2D and make convex hull
+                one_foot_vert_3d_for_check[lr]  = make_rect_3d(offset_fblr(safe_foot_vert_fblr[lr], 0.002));// 数値誤差で内外判定ずれるので少し大きい凸包で判定
+                foot_vert3d_check_wld[lr]       = (rp_ref_out.foot(lr).abs.R * one_foot_vert_3d_for_check[lr]).colwise() + rp_ref_out.foot(lr).abs.p; // rotate and translate in 3D
+                one_foot_hull2d[lr]             = makeConvexHull2D( foot_vert3d_check_wld[lr].topRows(XY) ); // project into 2D and make convex hull
                 cp_force_go_contact[OPPOSITE(lr)] = !isPointInHull2D(cp_dec_ragulated, one_foot_hull2d[lr]); // if CapturePoint go out of R sole region, L foot must be go contact
             }
-
-            //加速CP条件(ACP使用)
+            ///// 加速CP条件(ACP使用)
             hrp::Vector2 cp_acc_ragulated = com_pos - com_vel * sqrt( H_cur / G );
+            cp_acc_ragulated *= wp.capture_point_extend_ratio;
             if(!isPointInHull2D(cp_acc_ragulated, hull_a)){
                 calcCrossPointOnHull(com_pos, cp_acc_ragulated, hull_a, cp_acc_ragulated);
                 com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
             }
-
-            //加速減速条件マージ com_vel進行方向をより減速させるものを採用？
+            ///// 加速減速条件マージ com_vel進行方向をより減速させるものを採用？
             return (com_vel_decel_ok.dot(com_vel) < com_vel_accel_ok.dot(com_vel)) ? com_vel_decel_ok : com_vel_accel_ok; //normじゃダメ？
         }
         hrp::dmatrix createSupportRegionByFootPos(const hrp::Pose3& rfin_abs, const hrp::Pose3& lfin_abs, const hrp::Vector4& rf_mgn, const hrp::Vector4& lf_mgn){
@@ -686,14 +682,9 @@ class WBMSCore{
             bg_multi_point bg_ans_cross_pt;
             bg::intersection(anchor_vec, to_bg_hull(hull), bg_ans_cross_pt);
             switch(bg_ans_cross_pt.size()){
-                case 0:
-                    return false;
-                case 1:
-                    ans_cross_pt = to_Vector2(bg_ans_cross_pt[0]);
-                    return true;
-                default:
-                    std::cerr << "Number of the cross point must be 0 or 1 (current = " << bg_ans_cross_pt.size() << ") something wrong!" << std::endl;
-                    return false;
+                case 0: return false;
+                case 1: ans_cross_pt = to_Vector2(bg_ans_cross_pt[0]); return true;
+                default: std::cerr << "Number of the cross point must be 0 or 1 (current = " << bg_ans_cross_pt.size() << ") something wrong!" << std::endl; return false;
             }
         }
         hrp::Vector2 calcNearestPointOnHull(const hrp::Vector2& tgt_pt, const hrp::dmatrix& hull){
@@ -715,13 +706,9 @@ class WBMSCore{
             }
             return ans_nearest_pt;
         }
-        hrp::dmatrix makeConvexHull2D(const hrp::dmatrix& pts_2d){
-            return makeConvexHull2D_Boost(pts_2d);
-        }
-        bool isPointInHull2D(const hrp::Vector2& pt, const hrp::dmatrix& hull){
-            return isPointInHull2D_Boost(pt, hull);
-        }
-        hrp::dmatrix makeConvexHull2D_Boost(const hrp::dmatrix& pts_2d){
+        hrp::dmatrix makeConvexHull2D(const hrp::dmatrix& pts_2d){ return makeConvexHull2D_Boost(pts_2d); }
+        bool isPointInHull2D(const hrp::Vector2& pt, const hrp::dmatrix& hull){ return isPointInHull2D_Boost(pt, hull); }
+        hrp::dmatrix makeConvexHull2D_Boost(const hrp::dmatrix& pts_2d){  // A = [p0 p1 p2 p3 ... pn p0]
             if(pts_2d.rows() != 2){ std::cerr << "Invalid input for makeConvexHull_Boost" << std::endl; dbgn(pts_2d); }
             bg_multi_point tmp;
             tmp.resize(pts_2d.cols());
@@ -747,6 +734,14 @@ class WBMSCore{
             com_oldold = com_old;
             com_old = comin;
         }
+        hrp::Vector4 fbio2fblr(const hrp::Vector4& fbio_in, const int lr){// convert front_back_in_out to front_back_left_right order
+            assert(lr == R || lr == L);
+            if(lr == R){
+                return fbio_in;
+            }else{
+                return (hrp::Vector4()<<fbio_in(0), fbio_in(1), -fbio_in(3), -fbio_in(2)).finished();
+            }
+        }
 };
 
 

From f83324008a5053f9a461d271ea8d3d1c6fc70603 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 19 Sep 2019 16:33:02 +0900
Subject: [PATCH 177/217] knee stretch

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index b26c5bc4797..e8b1ea0575b 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -577,8 +577,8 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
     if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->ulimit = deg2rad(10);
     if( fik->m_robot->link("RARM_JOINT2") != NULL) fik->m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-45);//脇内側の干渉回避
     if( fik->m_robot->link("LARM_JOINT2") != NULL) fik->m_robot->link("LARM_JOINT2")->llimit = deg2rad(45);
-    if( fik->m_robot->link("RLEG_JOINT3") != NULL) fik->m_robot->link("RLEG_JOINT3")->llimit = deg2rad(15);//膝伸びきり防止のため
-    if( fik->m_robot->link("LLEG_JOINT3") != NULL) fik->m_robot->link("LLEG_JOINT3")->llimit = deg2rad(15);
+    if( fik->m_robot->link("RLEG_JOINT3") != NULL) fik->m_robot->link("RLEG_JOINT3")->llimit = deg2rad(40);//膝伸びきり防止のため
+    if( fik->m_robot->link("LLEG_JOINT3") != NULL) fik->m_robot->link("LLEG_JOINT3")->llimit = deg2rad(40);
     if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 0;//実機修理中
 
     if( fik->m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_Y")->jointId) = 0.1;//K

From 9e99b2ff393d12525831c17a3aa9b1bcab90943d Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 2 Oct 2019 19:52:56 +0900
Subject: [PATCH 178/217] delete cnt

---
 .../WholeBodyMasterSlave.cpp                  |   3 +
 .../WholeBodyMasterSlave.h                    |   3 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 116 ++++++++----------
 3 files changed, 56 insertions(+), 66 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index e8b1ea0575b..f6a4e34438f 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -141,6 +141,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     avg_q_acc.tail(6).fill(std::numeric_limits<double>::max()); // no limit for base link acc
     RTC_INFO_STREAM("setup interpolator finished");
 
+    ref_zmp_filter.resize(XYZ);
     ref_zmp_filter.setParameter(20, 1/m_dt, Q_BUTTERWORTH);
 
     cp_flag = -1;
@@ -716,6 +717,7 @@ bool WholeBodyMasterSlave::setParams(const OpenHRP::WholeBodyMasterSlaveService:
     wbms->wp.auto_com_foot_move_detect_height   = i_param.auto_com_foot_move_detect_height;
     wbms->wp.base_to_hand_min_distance          = i_param.base_to_hand_min_distance;
     wbms->wp.capture_point_extend_ratio         = i_param.capture_point_extend_ratio;
+    wbms->wp.com_filter_cutoff_hz               = i_param.com_filter_cutoff_hz;
     wbms->wp.foot_collision_avoidance_distance  = i_param.foot_collision_avoidance_distance;
     wbms->wp.human_to_robot_ratio               = i_param.human_to_robot_ratio;
     wbms->wp.max_double_support_width           = i_param.max_double_support_width;
@@ -738,6 +740,7 @@ bool WholeBodyMasterSlave::getParams(OpenHRP::WholeBodyMasterSlaveService::Whole
     i_param.auto_com_foot_move_detect_height    = wbms->wp.auto_com_foot_move_detect_height;
     i_param.base_to_hand_min_distance           = wbms->wp.base_to_hand_min_distance;
     i_param.capture_point_extend_ratio          = wbms->wp.capture_point_extend_ratio;
+    i_param.com_filter_cutoff_hz                = wbms->wp.com_filter_cutoff_hz;
     i_param.foot_collision_avoidance_distance   = wbms->wp.foot_collision_avoidance_distance;
     i_param.human_to_robot_ratio                = wbms->wp.human_to_robot_ratio;
     i_param.max_double_support_width            = wbms->wp.max_double_support_width;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index c4b0b2c43f5..8539274df07 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -148,8 +148,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         hrp::dvector avg_q_vel, avg_q_acc;
 
         hrp::InvDynStateBuffer idsb;
-    //    BiquadIIRFilterVec invdyn_zmp_filters;
-        BiquadIIRFilterVec ref_zmp_filter;
+        BiquadIIRFilterVec2 ref_zmp_filter;
         IIRFilter ref_ee_vel_v_filter;
 
         HumanPose raw_pose;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 8f073e9efcc..eceb8ea7c93 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -55,37 +55,15 @@ class LIP_model{
 //        hrp::Vector3 ZMP(){ return (hrp::Vector3() << com[0].head(XY) - acc().head(XY) * ( com_height / G ), com[0](Z) - com_height).finished; }
 };
 
-
-class BiquadIIRFilterVec{
-    private:
-        IIRFilter filters[XYZ];
-        hrp::Vector3 ans;
-    public:
-        BiquadIIRFilterVec(){}
-        ~BiquadIIRFilterVec(){}
-        void setParameter(const hrp::Vector3& fc_in, const double& HZ, const double& Q = 0.5){ for(int i=0;i<XYZ;i++){ filters[i].setParameterAsBiquad((double)fc_in(i), Q, HZ); } }
-        void setParameter(const double& fc_in, const double& HZ, const double& Q = 0.5){
-            setParameter(hrp::Vector3(fc_in,fc_in,fc_in), HZ, Q);
-        }//overload
-        hrp::Vector3 passFilter(const hrp::Vector3& input){
-            for(int i=0;i<XYZ;i++){
-                ans(i) = filters[i].passFilter((double)input(i));
-            }
-            return ans;
-        }
-        void reset(const hrp::Vector3& initial_input){ for(int i=0;i<XYZ;i++){ filters[i].reset((double)initial_input(i));} }
-};
-
-
 class PoseTGT{
     public:
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-        hrp::Pose3 abs, offs, cnt;
+        hrp::Pose3 abs, offs;
         hrp::dvector6 w;
         bool go_contact;
         PoseTGT(){ reset(); }
         ~PoseTGT(){}
-        void reset(){ abs.reset(); offs.reset(); cnt.reset(); w.fill(0); go_contact = false;}
+        void reset(){ abs.reset(); offs.reset(); w.fill(0); go_contact = false;}
         bool is_contact(){
             const double contact_threshold = 0.005;
             return ((abs.p(Z) - offs.p(Z)) < contact_threshold);
@@ -363,17 +341,18 @@ class CapsuleCollisionChecker {
 class WBMSCore{
     private:
         double HZ, DT;
-        BiquadIIRFilterVec acc4zmp_v_filters, com_in_filter;
         hrp::Vector3 com_old, com_oldold, comacc, com_CP_ref_old;
         HumanPose rp_ref_out_old;
         unsigned int loop;
         bool is_initial_loop;
         bool cp_force_go_contact[LR];
         int zmp_force_go_contact_count[LR];
+        BiquadIIRFilterVec2 acc4zmp_v_filters, com_filter;
+        double com_filter_cutoff_hz_old;
     public:
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
         double H_cur;
-        HumanPose hp_wld_raw, hp_plot, rp_ref_out;
+        HumanPose hp_wld_raw, rp_ref_out;
         hrp::Pose3 baselinkpose;
         hrp::Vector3 cp_dec, cp_acc;
         hrp::Vector4 act_foot_vert_fblr[LR], safe_foot_vert_fblr[LR];
@@ -385,6 +364,7 @@ class WBMSCore{
                 double auto_com_foot_move_detect_height;
                 double base_to_hand_min_distance;
                 double capture_point_extend_ratio;
+                double com_filter_cutoff_hz;
                 double foot_collision_avoidance_distance;
                 double human_to_robot_ratio;
                 double max_double_support_width;
@@ -402,6 +382,7 @@ class WBMSCore{
                 auto_com_foot_move_detect_height    = 0.03;
                 base_to_hand_min_distance           = 0.5;
                 capture_point_extend_ratio          = 1.0;
+                com_filter_cutoff_hz                = 0.5;
                 foot_collision_avoidance_distance   = 0.16;
                 human_to_robot_ratio                = 1.0;//human 1.1m vs jaxon 1.06m
                 max_double_support_width            = 0.4;
@@ -423,8 +404,10 @@ class WBMSCore{
             loop = 0;
             com_old = com_oldold = comacc = hrp::Vector3::Zero();
             cp_dec = cp_acc = hrp::Vector3::Zero();
+            acc4zmp_v_filters.resize(XYZ);
             acc4zmp_v_filters.setParameter(5, HZ, Q_BUTTERWORTH);//ZMP生成用ほぼこの値でいい
-            com_in_filter.setParameter(1, HZ);
+            com_filter.resize(XYZ);
+            com_filter.setParameter(wp.com_filter_cutoff_hz, HZ, Q_NOOVERSHOOT);
             for(int lr=0; lr<LR; lr++){
                 act_foot_vert_fblr[lr]          = fbio2fblr(wp.actual_foot_vert_fbio, lr);
                 safe_foot_vert_fblr[lr]         = fbio2fblr(wp.safety_foot_vert_fbio, lr);
@@ -458,19 +441,14 @@ class WBMSCore{
             const int human_l_names[4] = {rf,lf,rh,lh};
             for(int i=0;i<4;i++){//HumanSynchronizerの初期姿勢オフセットをセット
                 if(_ee_ikc_map.count(robot_l_names[i])){
-                    rp_ref_out.tgt[human_l_names[i]].offs.p = _ee_ikc_map[robot_l_names[i]].getCurrentTargetPos(robot_in);//          fik_in->getEndEffectorPos(robot_l_names[i]);
-                    rp_ref_out.tgt[human_l_names[i]].offs.R = _ee_ikc_map[robot_l_names[i]].getCurrentTargetRot(robot_in);
-                    rp_ref_out.tgt[human_l_names[i]].abs = rp_ref_out.tgt[human_l_names[i]].offs;
+                    rp_ref_out.tgt[human_l_names[i]].abs = rp_ref_out.tgt[human_l_names[i]].offs = _ee_ikc_map[robot_l_names[i]].getCurrentTargetPose(robot_in);//          fik_in->getEndEffectorPos(robot_l_names[i]);
                 }
             }
             rp_ref_out.tgt[com].offs.p = act_rs.com = act_rs.zmp = act_rs.st_zmp = robot_in->calcCM();
             com_CP_ref_old = rp_ref_out.tgt[com].offs.p;
             rp_ref_out.tgt[com].offs.R = robot_in->rootLink()->R;
-            rp_ref_out.tgt[zmp].offs.p(X) = rp_ref_out.tgt[com].offs.p(X);
-            rp_ref_out.tgt[zmp].offs.p(Y) = rp_ref_out.tgt[com].offs.p(Y);
+            rp_ref_out.tgt[zmp].offs.p.head(XY) = rp_ref_out.tgt[com].offs.p.head(XY);
             rp_ref_out.tgt[zmp].offs.p(Z) = (rp_ref_out.tgt[rf].offs.p(Z) + rp_ref_out.tgt[lf].offs.p(Z)) / 2;
-            rp_ref_out.tgt[rf].cnt = rp_ref_out.tgt[rf].offs;
-            rp_ref_out.tgt[lf].cnt = rp_ref_out.tgt[lf].offs;
             baselinkpose.p = robot_in->rootLink()->p;
             baselinkpose.R = robot_in->rootLink()->R;
             H_cur = rp_ref_out.tgt[com].offs.p(Z) - std::min((double)rp_ref_out.tgt[rf].offs.p(Z), (double)rp_ref_out.tgt[rf].offs.p(Z));
@@ -484,20 +462,22 @@ class WBMSCore{
         }
         void update(){//////////  メインループ  ////////////
             convertHumanToRobot                 (hp_wld_raw, rp_ref_out);
-            hp_plot = rp_ref_out;
+            setAutoCOMMode                      (hp_wld_raw, rp_ref_out_old, rp_ref_out);
+
+            ///// COMだけはフィルターいるか・・・
+            if(is_initial_loop || wp.com_filter_cutoff_hz != com_filter_cutoff_hz_old){
+                com_filter.setParameter(wp.com_filter_cutoff_hz, HZ, Q_NOOVERSHOOT);
+                com_filter.reset(rp_ref_out.tgt[com].abs.p);
+                com_filter_cutoff_hz_old = wp.com_filter_cutoff_hz;
+            }
+            rp_ref_out.tgt[com].abs.p = com_filter.passFilter(rp_ref_out.tgt[com].abs.p);
+
             lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
-            limitEEWorkspace                    (rp_ref_out);
-            setFootContactPoseByGoContact       (rp_ref_out);
-            limitFootVelNearGround              (rp_ref_out);
+            limitEEWorkspace                    (rp_ref_out_old, rp_ref_out);
+            setFootContactPoseByGoContact       (rp_ref_out_old, rp_ref_out);
+            limitFootVelNearGround              (rp_ref_out_old, rp_ref_out);
             avoidFootSinkIntoFloor              (rp_ref_out);
 
-            if(wp.auto_com_mode){
-                const bool rf_is_up = (hp_wld_raw.tgt[rf].abs.p(Z) - hp_wld_raw.tgt[rf].offs.p(Z) > wp.auto_com_foot_move_detect_height);
-                const bool lf_is_up = (hp_wld_raw.tgt[lf].abs.p(Z) - hp_wld_raw.tgt[lf].offs.p(Z) > wp.auto_com_foot_move_detect_height);
-                if      ( rf_is_up && !lf_is_up){ rp_ref_out.tgt[com].abs.p.head(XY) = rp_ref_out.tgt[lf].cnt.p.head(XY); }
-                else if (!rf_is_up &&  lf_is_up){ rp_ref_out.tgt[com].abs.p.head(XY) = rp_ref_out.tgt[rf].cnt.p.head(XY); }
-                else                            { rp_ref_out.tgt[com].abs.p.head(XY) = (rp_ref_out.tgt[rf].cnt.p.head(XY) + rp_ref_out.tgt[lf].cnt.p.head(XY)) / 2; }
-            }
 
             applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, rp_ref_out.tgt[com].abs.p);
             applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
@@ -528,6 +508,15 @@ class WBMSCore{
             out.tgt[com].abs.p += wp.com_offset;
             out.tgt[zmp].abs = in.tgt[zmp].abs;//最近使わない
         }
+        void setAutoCOMMode(const HumanPose& human, const HumanPose& old, HumanPose& out){
+            if(wp.auto_com_mode){
+                const bool rf_is_up = (human.tgt[rf].abs.p(Z) - human.tgt[rf].offs.p(Z) > wp.auto_com_foot_move_detect_height);
+                const bool lf_is_up = (human.tgt[lf].abs.p(Z) - human.tgt[lf].offs.p(Z) > wp.auto_com_foot_move_detect_height);
+                if      ( rf_is_up && !lf_is_up){ out.tgt[com].abs.p.head(XY) = old.tgt[lf].abs.p.head(XY); }
+                else if (!rf_is_up &&  lf_is_up){ out.tgt[com].abs.p.head(XY) = old.tgt[rf].abs.p.head(XY); }
+                else                            { out.tgt[com].abs.p.head(XY) = (old.tgt[rf].abs.p.head(XY) + old.tgt[lf].abs.p.head(XY)) / 2; }
+            }
+        }
         void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
             hrp::Vector2 to_opposite_foot[LR], ref_zmp_from_foot[LR], act_zmp_from_foot[LR];
             for(int lr=0; lr<LR; lr++){
@@ -550,12 +539,12 @@ class WBMSCore{
                 out.foot(OPPOSITE(lr)).go_contact |= cp_force_go_contact[OPPOSITE(lr)];
             }
         }
-        void limitEEWorkspace(HumanPose& out){
+        void limitEEWorkspace(const HumanPose& old, HumanPose& out){
             for(int lr=0; lr<LR; lr++){
-                PoseTGT& support_leg = out.foot(lr);
-                PoseTGT& swing_leg = out.foot(OPPOSITE(lr));
+                const PoseTGT&  support_leg = old.foot(lr);
+                PoseTGT&        swing_leg   = out.foot(OPPOSITE(lr));
                 hrp::Vector2 inside_vec_baserel = ( lr==R ? hrp::Vector2(0,+1) : hrp::Vector2(0,-1) );
-                hrp::Vector2 sp2sw_vec = swing_leg.abs.p.head(XY) - support_leg.cnt.p.head(XY);
+                hrp::Vector2 sp2sw_vec = swing_leg.abs.p.head(XY) - support_leg.abs.p.head(XY);
                 Eigen::Matrix2d base_rot;
                 base_rot = Eigen::Rotation2Dd(baselinkpose.rpy()(y));
                 hrp::Vector2 sp2sw_vec_baserel = base_rot.transpose() * sp2sw_vec;
@@ -563,7 +552,7 @@ class WBMSCore{
                     sp2sw_vec_baserel += inside_vec_baserel * (wp.foot_collision_avoidance_distance - sp2sw_vec_baserel.dot(inside_vec_baserel));
                 }
                 sp2sw_vec = base_rot * sp2sw_vec_baserel;
-                swing_leg.abs.p.head(XY) = sp2sw_vec + support_leg.cnt.p.head(XY);
+                swing_leg.abs.p.head(XY) = sp2sw_vec + support_leg.abs.p.head(XY);
             }
             for(int lr=0; lr<LR; lr++){
                 hrp::Vector2 horizontal_dist = out.hand(lr).abs.p.head(XY) - baselinkpose.p.head(XY);
@@ -572,32 +561,31 @@ class WBMSCore{
                 }
             }
         }
-        void setFootContactPoseByGoContact(HumanPose& out){
+        void setFootContactPoseByGoContact(const HumanPose& old, HumanPose& out){
             for(int lr=0; lr<LR; lr++){
                 if(out.foot(lr).go_contact){
-                    out.foot(lr).abs.p = out.foot(lr).cnt.p;
-                    out.foot(lr).abs.setRpy(0, 0, out.foot(lr).cnt.rpy()(y));
-                }else{
-                    out.foot(lr).cnt.p.head(XY) = out.foot(lr).abs.p.head(XY);
-                    out.foot(lr).cnt.setRpy(0, 0, out.foot(lr).abs.rpy()(y));
-                    LIMIT_MIN( out.foot(lr).abs.p(Z), out.foot(lr).offs.p(Z)+wp.swing_foot_height_offset);
+                    out.foot(lr).abs.p.head(XY) = old.foot(lr).abs.p.head(XY);
+                    out.foot(lr).abs.p(Z) = out.foot(lr).offs.p(Z);
+                    out.foot(lr).abs.setRpy(0, 0, old.foot(lr).abs.rpy()(y));
                     /////着地時に足を広げすぎないよう制限
-                    const hrp::Vector2 spport_to_swing = out.foot(lr).cnt.p.head(XY) - out.foot(OPPOSITE(lr)).cnt.p.head(XY);
-                    if(spport_to_swing.norm() > wp.max_double_support_width){
-                        out.foot(lr).cnt.p.head(XY) = out.foot(OPPOSITE(lr)).cnt.p.head(XY) + spport_to_swing.normalized() * wp.max_double_support_width;
+                    const hrp::Vector2 support_to_swing = out.foot(lr).abs.p.head(XY) - old.foot(OPPOSITE(lr)).abs.p.head(XY);
+                    if(support_to_swing.norm() > wp.max_double_support_width){
+                        out.foot(lr).abs.p.head(XY) = old.foot(OPPOSITE(lr)).abs.p.head(XY) + support_to_swing.normalized() * wp.max_double_support_width;
                     }
+                }else{
+                    LIMIT_MIN( out.foot(lr).abs.p(Z), out.foot(lr).offs.p(Z)+wp.swing_foot_height_offset);
                 }
             }
         }
-        void limitFootVelNearGround(HumanPose& out){
+        void limitFootVelNearGround(const HumanPose& old, HumanPose& out){
             for(int lr=0; lr<LR; lr++){
-                const double fheight = rp_ref_out_old.foot(lr).abs.p(Z) - rp_ref_out_old.foot(lr).offs.p(Z);
+                const double fheight = old.foot(lr).abs.p(Z) - old.foot(lr).offs.p(Z);
                 const double horizontal_max_vel = 0 + fheight * 20;
                 for(int j=0; j<XY; j++){
-                    LIMIT_MINMAX( out.foot(lr).abs.p(j), rp_ref_out_old.foot(lr).abs.p(j) - horizontal_max_vel * DT, rp_ref_out_old.foot(lr).abs.p(j) + horizontal_max_vel * DT);
+                    LIMIT_MINMAX( out.foot(lr).abs.p(j), old.foot(lr).abs.p(j) - horizontal_max_vel * DT, old.foot(lr).abs.p(j) + horizontal_max_vel * DT);
                 }
                 const double vertical_max_vel = 0 + fheight * 10;
-                LIMIT_MIN( out.foot(lr).abs.p(Z), rp_ref_out_old.foot(lr).abs.p(Z) - vertical_max_vel * DT);
+                LIMIT_MIN( out.foot(lr).abs.p(Z), old.foot(lr).abs.p(Z) - vertical_max_vel * DT);
             }
         }
         void avoidFootSinkIntoFloor(HumanPose& out){

From f72666f5ba0f87655e6c7233b7edde260daf3e45 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 2 Oct 2019 20:00:43 +0900
Subject: [PATCH 179/217] up

---
 idl/WholeBodyMasterSlaveService.idl | 1 +
 1 file changed, 1 insertion(+)

diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
index 27322999a5d..8df144c903a 100644
--- a/idl/WholeBodyMasterSlaveService.idl
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -23,6 +23,7 @@ module OpenHRP
         double auto_com_foot_move_detect_height;
         double base_to_hand_min_distance;
         double capture_point_extend_ratio;
+        double com_filter_cutoff_hz;
         double foot_collision_avoidance_distance;
         double human_to_robot_ratio;
         double max_double_support_width;

From e3a10d3f10f1ec6bd40524dcf5dba2d1f9172797 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 2 Oct 2019 20:02:15 +0900
Subject: [PATCH 180/217] up

---
 rtc/AutoBalancer/MyUtils.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index bfbac6b6f32..05ec793a825 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -118,7 +118,7 @@ namespace hrp{
 }
 
 
-class BiquadIIRFilterVec2{
+class BiquadIIRFilterVec2{/// safe ver
     private:
         std::vector<IIRFilter> filters;
         hrp::dvector ans;

From f8767fab7116ab37dfbebec5e81f047507d65af5 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 7 Oct 2019 10:41:37 +0900
Subject: [PATCH 181/217] fix q weight bug, base rpy limit bug

---
 .../FullbodyInverseKinematicsSolver.h         | 121 ++++++++++--------
 rtc/AutoBalancer/MyUtils.h                    |  14 +-
 rtc/HapticController/HapticController.cpp     |   8 +-
 rtc/HapticController/HapticController.h       |   6 +-
 .../WholeBodyMasterSlave.cpp                  |  12 +-
 .../WholeBodyMasterSlave.h                    |   2 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          |   2 +-
 7 files changed, 90 insertions(+), 75 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index 222797e54ff..b27bb096974 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -15,33 +15,33 @@
 class IKConstraint {
     public:
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-        std::string target_link_name;
-        hrp::Vector3 targetPos;
-        hrp::Vector3 targetRpy;
-        hrp::Vector3 localPos;
-        hrp::Matrix33 localR;
-        hrp::dvector6 constraint_weight;
-        double pos_precision, rot_precision;
+        std::string     target_link_name;
+        hrp::Vector3    targetPos;
+        hrp::Vector3    targetRpy;
+        hrp::Vector3    localPos;
+        hrp::Matrix33   localR;
+        hrp::dvector6   constraint_weight;
+        double          pos_precision, rot_precision;
         IKConstraint ()
-        :targetPos(hrp::Vector3::Zero()),
-         targetRpy(hrp::Vector3::Zero()),
-         localPos(hrp::Vector3::Zero()),
-         localR(hrp::Matrix33::Identity()),
-         constraint_weight(hrp::dvector6::Ones()),
+        :targetPos(         hrp::Vector3::Zero()),
+         targetRpy(         hrp::Vector3::Zero()),
+         localPos(          hrp::Vector3::Zero()),
+         localR(            hrp::Matrix33::Identity()),
+         constraint_weight( hrp::dvector6::Ones()),
          pos_precision(1e-4), rot_precision(deg2rad(0.1)){}
-        hrp::Vector3 getCurrentTargetPos(const hrp::BodyPtr _robot){ return _robot->link(target_link_name)->p + _robot->link(target_link_name)->R * localPos; }
-        hrp::Matrix33 getCurrentTargetRot(const hrp::BodyPtr _robot){ return _robot->link(target_link_name)->R * localR; }
-        hrp::Pose3 getCurrentTargetPose(const hrp::BodyPtr _robot){ return hrp::Pose3(getCurrentTargetPos(_robot), getCurrentTargetRot(_robot)); }
+        hrp::Vector3    getCurrentTargetPos(const hrp::BodyPtr _robot){ return _robot->link(target_link_name)->p + _robot->link(target_link_name)->R * localPos; }
+        hrp::Matrix33   getCurrentTargetRot(const hrp::BodyPtr _robot){ return _robot->link(target_link_name)->R * localR; }
+        hrp::Pose3      getCurrentTargetPose(const hrp::BodyPtr _robot){ return hrp::Pose3(getCurrentTargetPos(_robot), getCurrentTargetRot(_robot)); }
 };
 
 
 class FullbodyInverseKinematicsSolver{
     protected:
-        hrp::dmatrix J_all, J_com, J_am;
-        hrp::dvector dq_all, err_all, constraint_weight_all, jlim_avoid_weight_old;
         const int WS_DOF, BASE_DOF, J_DOF, ALL_DOF;
-        std::vector<IIRFilter> am_filters;
         const double m_dt;
+        hrp::dmatrix J_all;
+        hrp::dvector dq_all, err_all, constraint_weight_all, jlim_avoid_weight_old;
+        //        BiquadIIRFilterVec am_filters;
     public:
         hrp::BodyPtr m_robot;
         hrp::dvector dq_weight_all, q_ref, q_ref_max_dq, q_ref_constraint_weight;
@@ -53,24 +53,24 @@ class FullbodyInverseKinematicsSolver{
           BASE_DOF(6),
           J_DOF(_robot->numJoints()),
           ALL_DOF(J_DOF+BASE_DOF){
-            dq_weight_all = hrp::dvector::Ones(ALL_DOF);
-            q_ref = q_ref_constraint_weight = hrp::dvector::Zero(ALL_DOF);
-            q_ref_max_dq = hrp::dvector::Constant(ALL_DOF, 1e-2);
-            cur_momentum_around_COM = cur_momentum_around_COM_filtered = hrp::Vector3::Zero();
-            jlim_avoid_weight_old = hrp::dvector::Constant(ALL_DOF, 0);
-            rootlink_rpy_llimit = hrp::Vector3::Constant(-DBL_MAX);
-            rootlink_rpy_ulimit = hrp::Vector3::Constant(DBL_MAX);
-            am_filters.resize(3);
-            for(int i=0;i<3;i++){
-              am_filters[i].setParameterAsBiquad(10, 1/std::sqrt(2), 1.0/m_dt);
-              am_filters[i].reset();
-            }
+            dq_weight_all                       = hrp::dvector::Ones(ALL_DOF);
+            q_ref                               = hrp::dvector::Zero(ALL_DOF);
+            q_ref_constraint_weight             = hrp::dvector::Zero(ALL_DOF);
+            q_ref_max_dq                        = hrp::dvector::Constant(ALL_DOF, 1e-2);
+            jlim_avoid_weight_old               = hrp::dvector::Zero(ALL_DOF);
+            cur_momentum_around_COM             = hrp::Vector3::Zero();
+            cur_momentum_around_COM_filtered    = hrp::Vector3::Zero();
+            rootlink_rpy_llimit                 = hrp::Vector3::Constant(-DBL_MAX);
+            rootlink_rpy_ulimit                 = hrp::Vector3::Constant(DBL_MAX);
+//            am_filters.resize(3);
+//            am_filters.setParameter(10, 1.0/m_dt, Q_BUTTERWORTH);
+//            am_filters.reset(0);
         };
         ~FullbodyInverseKinematicsSolver () {};
         int numStates(){ return ALL_DOF; };
         bool checkIKConvergence(const std::vector<IKConstraint>& _ikc_list){
             for ( int i=0; i<_ikc_list.size(); i++ ) {
-                hrp::Link* link_tgt_ptr = m_robot->link(_ikc_list[i].target_link_name);
+                const hrp::Link* link_tgt_ptr = m_robot->link(_ikc_list[i].target_link_name);
                 hrp::Vector3 pos_err, rot_err;
                 if(link_tgt_ptr){
                     pos_err = _ikc_list[i].targetPos - (link_tgt_ptr->p + link_tgt_ptr->R * _ikc_list[i].localPos);
@@ -90,10 +90,9 @@ class FullbodyInverseKinematicsSolver{
         }
         int solveFullbodyIKLoop (const std::vector<IKConstraint>& _ikc_list, const int _max_iteration) {
             #ifdef OPENHRP_PACKAGE_VERSION_320
-                hrp::Vector3 base_p_old = m_robot->rootLink()->p;
-                hrp::Matrix33 base_R_old = m_robot->rootLink()->R;
-                hrp::dvector q_old(J_DOF);
-                for(int i=0;i<J_DOF;i++){ q_old(i) = m_robot->joint(i)->q; }
+                const hrp::Vector3    base_p_old = m_robot->rootLink()->p;
+                const hrp::Matrix33   base_R_old = m_robot->rootLink()->R;
+                const hrp::dvector    q_old = hrp::getQAll(m_robot);
                 unsigned int loop;
                 for(loop=0; loop < _max_iteration;){
                     solveFullbodyIKOnce(_ikc_list);
@@ -106,9 +105,8 @@ class FullbodyInverseKinematicsSolver{
                     m_robot->calcTotalMomentum(tmp_P, tmp_L);//calcTotalMomentumは漸化的にWorld周りの並進＋回転運動量を出す
                     cur_momentum_around_COM = tmp_L - m_robot->calcCM().cross(tmp_P);
 //                    m_robot->calcTotalMomentumFromJacobian(tmp_P, cur_momentum_around_COM);//calcTotalMomentumFromJacobianは重心ヤコビアンと重心周り角運動量ヤコビアンを用いて重心周りの並進＋回転運動量を出す
-                    for(int i=0; i<3; i++){
-                        cur_momentum_around_COM_filtered(i) = am_filters[i].passFilter(cur_momentum_around_COM(i));
-                    }
+//                    cur_momentum_around_COM_filtered = am_filters.passFilter(cur_momentum_around_COM);//フィルター位相遅れのせいか常にウネウネしてる
+                    cur_momentum_around_COM_filtered = cur_momentum_around_COM;
                     loop++;
                     if(checkIKConvergence(_ikc_list)){ break; }
                 }
@@ -121,7 +119,7 @@ class FullbodyInverseKinematicsSolver{
             #ifdef OPENHRP_PACKAGE_VERSION_320
 
             // count up all valid constraint DOF and allocate Jacobian and vectors
-            hrp::dmatrix q_select_mat = hrp::to_SelectionMat(dq_weight_all);
+            const hrp::dmatrix q_select_mat = hrp::to_SelectionMat(dq_weight_all);
             const int VALID_Q_NUM = q_select_mat.rows();
             if(VALID_Q_NUM == 0){ return; }
             int VALID_C_NUM = 0; // count up valid constraint num
@@ -140,7 +138,7 @@ class FullbodyInverseKinematicsSolver{
                 if(link_tgt_ptr){//ベースリンク，通常リンク共通
                     hrp::Vector3 tgt_cur_pos = link_tgt_ptr->p + link_tgt_ptr->R * _ikc_list[i].localPos;
                     hrp::Matrix33 tgt_cur_rot = link_tgt_ptr->R * _ikc_list[i].localR;
-                    dp_part.head(3) =  _ikc_list[i].targetPos - tgt_cur_pos;
+                    dp_part.head(3) = _ikc_list[i].targetPos - tgt_cur_pos;
                     dp_part.tail(3) = tgt_cur_rot * hrp::omegaFromRotEx(tgt_cur_rot.transpose() * hrp::rotFromRpy(_ikc_list[i].targetRpy));
                     hrp::JointPath tgt_jpath(m_robot->rootLink(), link_tgt_ptr);
                     hrp::dmatrix J_jpath;
@@ -152,6 +150,7 @@ class FullbodyInverseKinematicsSolver{
                 else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){ //重心限定
                     dp_part.head(3) = _ikc_list[i].targetPos - m_robot->calcCM();
                     dp_part.tail(3) = (_ikc_list[i].targetRpy - cur_momentum_around_COM_filtered) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける(+フィルタ)
+                    hrp::dmatrix J_com, J_am;
                     m_robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる
                     m_robot->calcAngularMomentumJacobian(NULL, J_am);//base=NULLの時は重心周りの角運動量っぽい？
                     J_part << J_com, J_am;
@@ -163,33 +162,44 @@ class FullbodyInverseKinematicsSolver{
                 hrp::dmatrix c_part_selection_mat = hrp::to_SelectionMat(_ikc_list[i].constraint_weight);   // select only valid end effector constraint
                 if(c_part_selection_mat.rows() != 0 && c_part_selection_mat.cols() != 0 ){
 
-
-                    const double dp_max = 0.1;// ???????
+                    // 明らかに可到達でない条件の誤差は頭打ちしないと，いくら重み付きでも引きずられる?
+                    dp_part = dp_part.cwiseMin(hrp::dvector6::Constant(0.1)).cwiseMax(hrp::dvector6::Constant(-0.1));
 
                     J_all.middleRows                (CURRENT_C_COUNT, c_part_selection_mat.rows()) = c_part_selection_mat * J_part * q_select_mat.transpose();
-                    err_all.segment                 (CURRENT_C_COUNT, c_part_selection_mat.rows()) = c_part_selection_mat * dp_part.cwiseMin(hrp::dvector6::Constant(dp_max)).cwiseMax(hrp::dvector6::Constant(-dp_max));
+                    err_all.segment                 (CURRENT_C_COUNT, c_part_selection_mat.rows()) = c_part_selection_mat * dp_part;
                     constraint_weight_all.segment   (CURRENT_C_COUNT, c_part_selection_mat.rows()) = c_part_selection_mat * _ikc_list[i].constraint_weight;
                     CURRENT_C_COUNT += c_part_selection_mat.rows();
                 }
             }
 
             // set reference joint angle constraints into Jacobian
-            hrp::dmatrix q_ref_selection_mat = hrp::to_SelectionMat(q_ref_constraint_weight);
+            const hrp::dmatrix q_ref_selection_mat = hrp::to_SelectionMat(q_ref_constraint_weight);
             if(q_ref_selection_mat.rows() != 0 && q_ref_selection_mat.cols() != 0 ){
                 J_all.middleRows                (CURRENT_C_COUNT, q_ref_selection_mat.rows()) = q_ref_selection_mat * hrp::dmatrix::Identity(ALL_DOF,ALL_DOF) * q_select_mat.transpose();
                 err_all.segment                 (CURRENT_C_COUNT, q_ref_selection_mat.rows()) = q_ref_selection_mat * ((q_ref - hrp::getRobotStateVec(m_robot)).cwiseMin(q_ref_max_dq).cwiseMax(-q_ref_max_dq));
                 constraint_weight_all.segment   (CURRENT_C_COUNT, q_ref_selection_mat.rows()) = q_ref_selection_mat * q_ref_constraint_weight;
                 CURRENT_C_COUNT += q_ref_selection_mat.rows();
             }
-            assert(CURRENT_C_COUNT == VALID_C_NUM);
+            if(CURRENT_C_COUNT != VALID_C_NUM){ std::cerr <<"[FullbodyIK] CURRENT_C_COUNT != VALID_C_NUM, something wrong !" << std::endl; }
 
             // joint limit avoidance (copy from JointPathEx)
             hrp::dvector dq_weight_all_jlim = hrp::dvector::Ones(ALL_DOF);
             for ( int j = 0; j < ALL_DOF ; j++ ) {
-                double jang = (j<J_DOF) ? m_robot->joint(j)->q       :   hrp::rpyFromRot(m_robot->rootLink()->R)(j - J_DOF);
-                double jmax = (j<J_DOF) ? m_robot->joint(j)->ulimit  :   rootlink_rpy_ulimit(j - J_DOF);
-                double jmin = (j<J_DOF) ? m_robot->joint(j)->llimit  :   rootlink_rpy_llimit(j - J_DOF);
-                double e = deg2rad(1);
+                double jang, jmax, jmin;
+                if(j<J_DOF){
+                    jang = m_robot->joint(j)->q;
+                    jmax = m_robot->joint(j)->ulimit;
+                    jmin = m_robot->joint(j)->llimit;
+                }else if(j<J_DOF+3){
+                    continue;// do nothing about base link pos
+                }else{
+                    const int rpy_id = j - (J_DOF+3);
+                    if(!(0 <= rpy_id && rpy_id < 3)){ std::cerr <<"[FullbodyIK] !(0 <= rpy_id && rpy_id < 3), something wrong !" << std::endl; }
+                    jang = hrp::rpyFromRot(m_robot->rootLink()->R)(rpy_id);
+                    jmax = rootlink_rpy_ulimit(rpy_id);
+                    jmin = rootlink_rpy_llimit(rpy_id);
+                }
+                const double e = deg2rad(1);
                 if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) {
                 } else if ( eps_eq(jang, jmax,e) ) {
                     jang = jmax - e;
@@ -212,11 +222,16 @@ class FullbodyInverseKinematicsSolver{
             dq_weight_all_final = q_select_mat * dq_weight_all_final;
 
             // Solvability-unconcerned Inverse Kinematics by Levenberg-Marquardt Method [sugihara:RSJ2009]
+            // q = q + H^(-1) * g
+            // g = J^T * We * e
+            // H = J^T * We * J + Wn
+            // Wn = (e^T * We * e) * 1 + \bar{Wn}   // \bar{Wn} = \bar{wn} * E
+            //    = (e^T * We * e + \bar{wn}) * E
+            // Wn = Wn * Wq                         // modify to insert dq weight
             const double wn_const = 1e-6;
-            hrp::dmatrix Wn = (static_cast<double>(err_all.transpose() * constraint_weight_all.asDiagonal() * err_all) + wn_const) * hrp::dmatrix::Identity(dq_weight_all_final.size(), dq_weight_all_final.size());
-            Wn = dq_weight_all_final.asDiagonal() * Wn;
-            hrp::dmatrix H = J_all.transpose() * constraint_weight_all.asDiagonal() * J_all + Wn;
-            hrp::dvector g = J_all.transpose() * constraint_weight_all.asDiagonal() * err_all;
+            const hrp::dmatrix Wn = (static_cast<double>(err_all.transpose() * constraint_weight_all.asDiagonal() * err_all) + wn_const) * dq_weight_all_final.asDiagonal();
+            const hrp::dmatrix H = J_all.transpose() * constraint_weight_all.asDiagonal() * J_all + Wn;
+            const hrp::dvector g = J_all.transpose() * constraint_weight_all.asDiagonal() * err_all;
             dq_all = H.ldlt().solve(g); // dq_all = H.inverse() * g; is slow
 
             // rtconf localhost:15005/wbms.rtc set debugLevel 1 とかにしたい
diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index 05ec793a825..c6364fe3aa8 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -118,17 +118,17 @@ namespace hrp{
 }
 
 
-class BiquadIIRFilterVec2{/// safe ver
+class BiquadIIRFilterVec{/// safe ver
     private:
         std::vector<IIRFilter> filters;
         hrp::dvector ans;
     public:
-        BiquadIIRFilterVec2(){}
-        BiquadIIRFilterVec2(const int len){resize(len);}
+        BiquadIIRFilterVec(){}
+        BiquadIIRFilterVec(const int len){resize(len);}
         void resize(const int len){filters.resize(len); ans.resize(len);}
-        ~BiquadIIRFilterVec2(){}
-        void setParameter(const hrp::dvector& fc_in, const double& HZ, const double& Q = 0.5){ for(int i=0;i<filters.size();i++){ filters[i].setParameterAsBiquad(std::min(fc_in(i),HZ/2), Q, HZ); } }
-        void setParameter(const double& fc_in, const double& HZ, const double& Q = 0.5){ setParameter(hrp::dvector::Constant(filters.size(), fc_in), HZ, Q); }//overload
+        ~BiquadIIRFilterVec(){}
+        void setParameter(const hrp::dvector& fc_in, const double HZ, const double Q = 0.5){ for(int i=0;i<filters.size();i++){ filters[i].setParameterAsBiquad(std::min(fc_in(i),HZ/2), Q, HZ); } }
+        void setParameter(const double fc_in, const double HZ, const double Q = 0.5){ setParameter(hrp::dvector::Constant(filters.size(), fc_in), HZ, Q); }//overload
         hrp::dvector passFilter(const hrp::dvector& input){
             for(int i=0;i<filters.size();i++){
                 ans(i) = filters[i].passFilter((double)input(i));
@@ -136,7 +136,7 @@ class BiquadIIRFilterVec2{/// safe ver
             return ans;
         }
         void reset(const hrp::dvector& initial_input){ for(int i=0;i<filters.size();i++){ filters[i].reset((double)initial_input(i));} }
-        void reset(const double& initial_input){ for(int i=0;i<filters.size();i++){ filters[i].reset(initial_input);} }
+        void reset(const double initial_input){ for(int i=0;i<filters.size();i++){ filters[i].reset(initial_input);} }
 };
 
 inline bool has(const std::vector<std::string> v, const std::string& s){ return (std::find(v.begin(), v.end(), s) != v.end());}
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index 078d9c4ba04..fe0ce287354 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -82,17 +82,17 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     baselink_h_ip->get(&baselink_h_from_floor, false);
     RTC_INFO_STREAM("setup interpolator finished");
 
-    dqAct_filter = BiquadIIRFilterVec2(m_robot->numJoints());
+    dqAct_filter = BiquadIIRFilterVec(m_robot->numJoints());
     dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
     dqAct_filter.reset(0);
     for ( int i=0; i<ee_names.size(); i++) {
-        ee_vel_filter[ee_names[i]] = BiquadIIRFilterVec2(6);
+        ee_vel_filter[ee_names[i]] = BiquadIIRFilterVec(6);
         ee_vel_filter[ee_names[i]].setParameter(hcp.ee_vel_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
         ee_vel_filter[ee_names[i]].reset(0);
-        wrench_lpf_for_hpf[ee_names[i]] = BiquadIIRFilterVec2(6);
+        wrench_lpf_for_hpf[ee_names[i]] = BiquadIIRFilterVec(6);
         wrench_lpf_for_hpf[ee_names[i]].setParameter(hcp.wrench_hpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
         wrench_lpf_for_hpf[ee_names[i]].reset(0);
-        wrench_lpf[ee_names[i]] = BiquadIIRFilterVec2(6);
+        wrench_lpf[ee_names[i]] = BiquadIIRFilterVec(6);
         wrench_lpf[ee_names[i]].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
         wrench_lpf[ee_names[i]].reset(0);
     }
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index 4c921019a2b..e21dd91c04e 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -104,11 +104,11 @@ class HapticController : public RTC::DataFlowComponentBase{
         interpolator *t_ip, *q_ref_ip, *baselink_h_ip;
 
         hrp::InvDynStateBuffer idsb;
-        BiquadIIRFilterVec2 dqAct_filter;
+        BiquadIIRFilterVec dqAct_filter;
         hrp::dvector dqAct_filtered;
         std::map<std::string, IKConstraint> ee_ikc_map;
-        std::map<std::string, BiquadIIRFilterVec2> ee_vel_filter;
-        std::map<std::string, BiquadIIRFilterVec2> wrench_lpf_for_hpf, wrench_lpf;
+        std::map<std::string, BiquadIIRFilterVec> ee_vel_filter;
+        std::map<std::string, BiquadIIRFilterVec> wrench_lpf_for_hpf, wrench_lpf;
         std::map<std::string, hrp::dvector6> wrench_shaped, wrench_used;
         std::map<std::string, hrp::Pose3> ee_pose, ee_pose_old;
         std::map<std::string, hrp::dvector6> ee_vel, ee_vel_filtered; // = twist
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index f6a4e34438f..24f28b81fdc 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -569,9 +569,10 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         ikc_list.push_back(tmp);
     }
 
-    if( fik->m_robot->link("CHEST_JOINT0") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT0")->jointId) = 0.01;//JAXON
-    if( fik->m_robot->link("CHEST_JOINT1") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT1")->jointId) = 0.01;
-    if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 0.01;
+    if( fik->m_robot->link("CHEST_JOINT0") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT0")->jointId) = 1e3;//JAXON
+    if( fik->m_robot->link("CHEST_JOINT1") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT1")->jointId) = 1e3;
+//    if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 10;
+    if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 0;//実機修理中
     if( fik->m_robot->link("HEAD_JOINT0") != NULL) fik->m_robot->link("HEAD_JOINT0")->llimit = deg2rad(-30);
     if( fik->m_robot->link("HEAD_JOINT0") != NULL) fik->m_robot->link("HEAD_JOINT0")->ulimit = deg2rad(30);
     if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->llimit = deg2rad(-45);
@@ -580,10 +581,9 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
     if( fik->m_robot->link("LARM_JOINT2") != NULL) fik->m_robot->link("LARM_JOINT2")->llimit = deg2rad(45);
     if( fik->m_robot->link("RLEG_JOINT3") != NULL) fik->m_robot->link("RLEG_JOINT3")->llimit = deg2rad(40);//膝伸びきり防止のため
     if( fik->m_robot->link("LLEG_JOINT3") != NULL) fik->m_robot->link("LLEG_JOINT3")->llimit = deg2rad(40);
-    if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 0;//実機修理中
 
-    if( fik->m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_Y")->jointId) = 0.1;//K
-    if( fik->m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_P")->jointId) = 0.1;
+    if( fik->m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_Y")->jointId) = 10;//K
+    if( fik->m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_P")->jointId) = 10;
     if( fik->m_robot->link("R_KNEE_P") != NULL) fik->m_robot->link("R_KNEE_P")->llimit = deg2rad(15);
     if( fik->m_robot->link("L_KNEE_P") != NULL) fik->m_robot->link("L_KNEE_P")->llimit = deg2rad(15);
     if( fik->m_robot->link("R_WRIST_R") != NULL) fik->m_robot->link("R_WRIST_R")->llimit = deg2rad(-40);
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 8539274df07..8c532ac5a3b 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -148,7 +148,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         hrp::dvector avg_q_vel, avg_q_acc;
 
         hrp::InvDynStateBuffer idsb;
-        BiquadIIRFilterVec2 ref_zmp_filter;
+        BiquadIIRFilterVec ref_zmp_filter;
         IIRFilter ref_ee_vel_v_filter;
 
         HumanPose raw_pose;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index eceb8ea7c93..8dd8f5c6b3c 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -347,7 +347,7 @@ class WBMSCore{
         bool is_initial_loop;
         bool cp_force_go_contact[LR];
         int zmp_force_go_contact_count[LR];
-        BiquadIIRFilterVec2 acc4zmp_v_filters, com_filter;
+        BiquadIIRFilterVec acc4zmp_v_filters, com_filter;
         double com_filter_cutoff_hz_old;
     public:
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW

From e9a515d0d89d9208eecf0ac5b091a865d49e9389 Mon Sep 17 00:00:00 2001
From: leus <leus@example.com>
Date: Wed, 23 Oct 2019 13:18:44 +0900
Subject: [PATCH 182/217] contact tuneing

---
 rtc/HapticController/HapticController.cpp | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index 078d9c4ba04..42f85d33948 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -338,11 +338,8 @@ void HapticController::calcTorque(){
                 LIMIT_NORM_V(wrench, 1000);
                 const hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
                 for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
-
-                is_contact_to_floor[tgt[i]] = true;
-            }else{
-                is_contact_to_floor[tgt[i]] = false;
             }
+            is_contact_to_floor[tgt[i]] = (foot_h_from_floor < 0 + 0.05);
         }
 
         ///// virtual back wall
@@ -420,6 +417,7 @@ void HapticController::calcTorque(){
 
     // calc real robot feedback force
     for (int i=0; i<ee_names.size(); i++){
+        if(ee_names[i] == "lleg" || ee_names[i] == "rleg" ) continue; // skip leg force feedback
         const hrp::dvector6 wrench_raw = hrp::to_dvector(m_slaveEEWrenches[ee_names[i]].data);
         const hrp::dvector6 w_hpf = wrench_raw - wrench_lpf_for_hpf[ee_names[i]].passFilter( wrench_raw );
         const hrp::dvector6 w_lpf = wrench_lpf[ee_names[i]].passFilter( wrench_raw );

From aa4a0dc32611f395d046b5db0f41dcb1512d84e2 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 23 Oct 2019 16:27:16 +0900
Subject: [PATCH 183/217] add delay calc in wbms

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 9 +++++++++
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h   | 4 ++++
 2 files changed, 13 insertions(+)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 24f28b81fdc..4b703e384c4 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -33,6 +33,8 @@ WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlo
     m_actCPIn("actCapturePoint", m_actCP),
     m_actZMPIn("zmp", m_actZMP),
     m_exDataIn("exData", m_exData),
+    m_calcDelayInboundIn("calc_delay_inbound", m_calcDelayInbound),
+    m_calcDelayOutboundOut("calc_delay_outbound", m_calcDelayOutbound),
     m_exDataIndexIn("exDataIndex", m_exDataIndex),
     m_WholeBodyMasterSlaveServicePort("WholeBodyMasterSlaveService"),
     m_AutoBalancerServicePort("AutoBalancerService"),
@@ -60,6 +62,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     addOutPort("optionalDataOut", m_optionalDataOut);
     addInPort("actCapturePoint", m_actCPIn);
     addInPort("zmp", m_actZMPIn);
+    addInPort("calc_delay_inbound", m_calcDelayInboundIn);
+    addOutPort("calc_delay_outbound", m_calcDelayOutboundOut);
     addInPort("exData", m_exDataIn);
     addInPort("exDataIndex", m_exDataIndexIn);
     m_WholeBodyMasterSlaveServicePort.registerProvider("service0", "WholeBodyMasterSlaveService", m_service0);
@@ -223,6 +227,10 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if (m_basePosIn.isNew()) { m_basePosIn.read(); }
     if (m_baseRpyIn.isNew()) { m_baseRpyIn.read(); }
     if (m_zmpIn.isNew()) { m_zmpIn.read(); }
+    if (m_calcDelayInboundIn.isNew()) {
+        m_calcDelayInboundIn.read();
+        m_calcDelayOutbound = m_calcDelayInbound;
+    }
     if (m_optionalDataIn.isNew()) { m_optionalDataIn.read(); }
     for(int i=0; i<ee_names.size(); i++){ if (m_localEEWrenchesIn[ee_names[i]]->isNew()){ m_localEEWrenchesIn[ee_names[i]]->read(); } }
 
@@ -366,6 +374,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     m_basePosOut.write();
     m_baseRpyOut.write();
     m_zmpOut.write();
+    m_calcDelayOutboundOut.write();
     m_optionalDataOut.write();
     addTimeReport("OutPort");
     if(DEBUGP) RTC_INFO_STREAM(time_report_str);
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 8c532ac5a3b..cb259b49aad 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -104,6 +104,10 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         typedef boost::shared_ptr<RTC::InPort<RTC::TimedDoubleSeq> > ITDS_Ptr;
         std::map<std::string, ITDS_Ptr> m_localEEWrenchesIn;
 
+        RTC::Time m_calcDelayInbound, m_calcDelayOutbound;
+        RTC::InPort<RTC::Time> m_calcDelayInboundIn;
+        RTC::OutPort<RTC::Time> m_calcDelayOutboundOut;
+
         RTC::TimedDoubleSeq m_exData;
         RTC::InPort<RTC::TimedDoubleSeq> m_exDataIn;
         RTC::TimedStringSeq m_exDataIndex;

From 33533811ec9178e5944d88433e889f133c621f80 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 23 Oct 2019 18:58:11 +0900
Subject: [PATCH 184/217] up

---
 rtc/HapticController/HapticController.cpp     | 12 +++--
 rtc/HapticController/HapticController.h       |  4 ++
 .../WholeBodyMasterSlave.cpp                  | 53 +++++--------------
 .../WholeBodyMasterSlave.h                    |  6 +--
 4 files changed, 29 insertions(+), 46 deletions(-)

diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index 8f01d24b51c..5f6219b348b 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -26,6 +26,8 @@ HapticController::HapticController(RTC::Manager* manager) : RTC::DataFlowCompone
     m_tauOut("tau", m_tau),
     m_teleopOdomOut("teleopOdom", m_teleopOdom),
     m_debugDataOut("debugData", m_debugData),
+    m_delayCheckPacketInboundIn("delay_check_packet_inbound", m_delayCheckPacket),
+    m_delayCheckPacketOutboundOut("delay_check_packet_outbound", m_delayCheckPacket),
     m_HapticControllerServicePort("HapticControllerService"),
     m_debugLevel(0)
 {
@@ -44,6 +46,8 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     addOutPort("tau", m_tauOut);
     addOutPort("teleopOdom", m_teleopOdomOut);
     addOutPort("debugData", m_debugDataOut);
+    addInPort("delay_check_packet_inbound", m_delayCheckPacketInboundIn);
+    addOutPort("delay_check_packet_outbound", m_delayCheckPacketOutboundOut);
     m_HapticControllerServicePort.registerProvider("service0", "HapticControllerService", m_service0);
     addPort(m_HapticControllerServicePort);
 
@@ -155,9 +159,10 @@ RTC::ReturnCode_t HapticController::setupEEIKConstraintFromConf(std::map<std::st
 
 RTC::ReturnCode_t HapticController::onExecute(RTC::UniqueId ec_id){
     if(DEBUGP_ONCE) RTC_INFO_STREAM("onExecute(" << ec_id << ") dt = " << m_dt);
-    if (m_qRefIn.isNew())   { m_qRefIn.read(); }
-    if (m_qActIn.isNew())   { m_qActIn.read(); }
-    if (m_dqActIn.isNew())  { m_dqActIn.read(); }
+    if(m_qRefIn.isNew()                     ){ m_qRefIn.read(); }
+    if(m_qActIn.isNew()                     ){ m_qActIn.read(); }
+    if(m_dqActIn.isNew()                    ){ m_dqActIn.read(); }
+    if(m_delayCheckPacketInboundIn.isNew()  ){ m_delayCheckPacketInboundIn.read(); }
     for (int i=0; i<ee_names.size(); i++) {
         if ( m_slaveEEWrenchesIn[ee_names[i]]->isNew() ) { m_slaveEEWrenchesIn[ee_names[i]]->read(); }
     }
@@ -183,6 +188,7 @@ RTC::ReturnCode_t HapticController::onExecute(RTC::UniqueId ec_id){
     m_qOut.write();
     m_tauOut.write();
     m_teleopOdomOut.write();
+    m_delayCheckPacketOutboundOut.write();
 
     loop ++;
     return RTC::RTC_OK;
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index e21dd91c04e..b894b28e9cd 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -94,6 +94,10 @@ class HapticController : public RTC::DataFlowComponentBase{
         RTC::CorbaPort m_HapticControllerServicePort;
         HapticControllerService_impl m_service0;
 
+        RTC::Time m_delayCheckPacket;
+        RTC::InPort<RTC::Time> m_delayCheckPacketInboundIn;
+        RTC::OutPort<RTC::Time> m_delayCheckPacketOutboundOut;
+
     private:
         double m_dt;
         unsigned int loop;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 4b703e384c4..6e6c27759be 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -33,8 +33,8 @@ WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlo
     m_actCPIn("actCapturePoint", m_actCP),
     m_actZMPIn("zmp", m_actZMP),
     m_exDataIn("exData", m_exData),
-    m_calcDelayInboundIn("calc_delay_inbound", m_calcDelayInbound),
-    m_calcDelayOutboundOut("calc_delay_outbound", m_calcDelayOutbound),
+    m_delayCheckPacketInboundIn("delay_check_packet_inbound", m_delayCheckPacket),
+    m_delayCheckPacketOutboundOut("delay_check_packet_outbound", m_delayCheckPacket),
     m_exDataIndexIn("exDataIndex", m_exDataIndex),
     m_WholeBodyMasterSlaveServicePort("WholeBodyMasterSlaveService"),
     m_AutoBalancerServicePort("AutoBalancerService"),
@@ -62,8 +62,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     addOutPort("optionalDataOut", m_optionalDataOut);
     addInPort("actCapturePoint", m_actCPIn);
     addInPort("zmp", m_actZMPIn);
-    addInPort("calc_delay_inbound", m_calcDelayInboundIn);
-    addOutPort("calc_delay_outbound", m_calcDelayOutboundOut);
+    addInPort("delay_check_packet_inbound", m_delayCheckPacketInboundIn);
+    addOutPort("delay_check_packet_outbound", m_delayCheckPacketOutboundOut);
     addInPort("exData", m_exDataIn);
     addInPort("exDataIndex", m_exDataIndexIn);
     m_WholeBodyMasterSlaveServicePort.registerProvider("service0", "WholeBodyMasterSlaveService", m_service0);
@@ -222,17 +222,14 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if(DEBUGP_ONCE) RTC_INFO_STREAM("onExecute(" << ec_id << ")");
     time_report_str.clear();
     clock_gettime(CLOCK_REALTIME, &startT);
-    if (m_qRefIn.isNew()) { m_qRefIn.read(); }
-    if (m_qActIn.isNew()) { m_qActIn.read(); }
-    if (m_basePosIn.isNew()) { m_basePosIn.read(); }
-    if (m_baseRpyIn.isNew()) { m_baseRpyIn.read(); }
-    if (m_zmpIn.isNew()) { m_zmpIn.read(); }
-    if (m_calcDelayInboundIn.isNew()) {
-        m_calcDelayInboundIn.read();
-        m_calcDelayOutbound = m_calcDelayInbound;
-    }
-    if (m_optionalDataIn.isNew()) { m_optionalDataIn.read(); }
-    for(int i=0; i<ee_names.size(); i++){ if (m_localEEWrenchesIn[ee_names[i]]->isNew()){ m_localEEWrenchesIn[ee_names[i]]->read(); } }
+    if(m_qRefIn.isNew()                     ){ m_qRefIn.read(); }
+    if(m_qActIn.isNew()                     ){ m_qActIn.read(); }
+    if(m_basePosIn.isNew()                  ){ m_basePosIn.read(); }
+    if(m_baseRpyIn.isNew()                  ){ m_baseRpyIn.read(); }
+    if(m_zmpIn.isNew()                      ){ m_zmpIn.read(); }
+    if(m_optionalDataIn.isNew()             ){ m_optionalDataIn.read(); }
+    if(m_delayCheckPacketInboundIn.isNew()  ){ m_delayCheckPacketInboundIn.read(); }
+    for(int i=0; i<ee_names.size(); i++){ if(m_localEEWrenchesIn[ee_names[i]]->isNew()){ m_localEEWrenchesIn[ee_names[i]]->read(); } }
 
     // button func
     hrp::dvector ex_data;
@@ -246,30 +243,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         ex_data_index = hrp::to_string_vector(m_exDataIndex.data);
     }
 
-//    if(m_htlfw.data.force.y >= 1 && m_htrfw.data.force.y >= 1 && mode.now() == MODE_IDLE){
-//        std::cerr<<"startWholeBodyMasterSlave() called by button"<<std::endl;
-//        startWholeBodyMasterSlave();
-//    }else if(m_htlfw.data.force.y >= 1 && m_htrfw.data.force.y >= 1 && mode.now() == MODE_WBMS){
-//        std::cerr<<"stopWholeBodyMasterSlave() called by button"<<std::endl;
-//        stopWholeBodyMasterSlave();
-//    }
-//    static bool is_blocking_continuous_hits = false;
-//    if(m_htlfw.data.force.z >= 1 && m_htrfw.data.force.z >= 1 && mode.now() == MODE_PAUSE){
-//        if(!is_blocking_continuous_hits){
-//            std::cerr<<"resumeWholeBodyMasterSlave() called by button"<<std::endl;
-//            resumeWholeBodyMasterSlave();
-//            is_blocking_continuous_hits = true;
-//        }
-//    }else if(m_htlfw.data.force.z >= 1 && m_htrfw.data.force.z >= 1 && mode.now() == MODE_WBMS){
-//        if(!is_blocking_continuous_hits){
-//            std::cerr<<"pauseWholeBodyMasterSlave() called by button"<<std::endl;
-//            pauseWholeBodyMasterSlave();
-//            is_blocking_continuous_hits = true;
-//        }
-//    }else{
-//        is_blocking_continuous_hits = false;
-//    }
-
 
     if( mode.now() != MODE_PAUSE ){ // stop updating input when MODE_PAUSE
         for(int i=0; i<tgt_names.size(); i++){
@@ -374,7 +347,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     m_basePosOut.write();
     m_baseRpyOut.write();
     m_zmpOut.write();
-    m_calcDelayOutboundOut.write();
+    m_delayCheckPacketOutboundOut.write();
     m_optionalDataOut.write();
     addTimeReport("OutPort");
     if(DEBUGP) RTC_INFO_STREAM(time_report_str);
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index cb259b49aad..167759bb29a 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -104,9 +104,9 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         typedef boost::shared_ptr<RTC::InPort<RTC::TimedDoubleSeq> > ITDS_Ptr;
         std::map<std::string, ITDS_Ptr> m_localEEWrenchesIn;
 
-        RTC::Time m_calcDelayInbound, m_calcDelayOutbound;
-        RTC::InPort<RTC::Time> m_calcDelayInboundIn;
-        RTC::OutPort<RTC::Time> m_calcDelayOutboundOut;
+        RTC::Time m_delayCheckPacket;
+        RTC::InPort<RTC::Time> m_delayCheckPacketInboundIn;
+        RTC::OutPort<RTC::Time> m_delayCheckPacketOutboundOut;
 
         RTC::TimedDoubleSeq m_exData;
         RTC::InPort<RTC::TimedDoubleSeq> m_exDataIn;

From f6fde5db33184b650b8f34c605f5d3d4c7581be3 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 23 Oct 2019 20:33:52 +0900
Subject: [PATCH 185/217] delay_check echo back only if isNew

---
 rtc/HapticController/HapticController.cpp         | 3 +--
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 3 +--
 2 files changed, 2 insertions(+), 4 deletions(-)

diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index 5f6219b348b..9b123656612 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -162,7 +162,7 @@ RTC::ReturnCode_t HapticController::onExecute(RTC::UniqueId ec_id){
     if(m_qRefIn.isNew()                     ){ m_qRefIn.read(); }
     if(m_qActIn.isNew()                     ){ m_qActIn.read(); }
     if(m_dqActIn.isNew()                    ){ m_dqActIn.read(); }
-    if(m_delayCheckPacketInboundIn.isNew()  ){ m_delayCheckPacketInboundIn.read(); }
+    if(m_delayCheckPacketInboundIn.isNew()  ){ m_delayCheckPacketInboundIn.read(); m_delayCheckPacketOutboundOut.write();}
     for (int i=0; i<ee_names.size(); i++) {
         if ( m_slaveEEWrenchesIn[ee_names[i]]->isNew() ) { m_slaveEEWrenchesIn[ee_names[i]]->read(); }
     }
@@ -188,7 +188,6 @@ RTC::ReturnCode_t HapticController::onExecute(RTC::UniqueId ec_id){
     m_qOut.write();
     m_tauOut.write();
     m_teleopOdomOut.write();
-    m_delayCheckPacketOutboundOut.write();
 
     loop ++;
     return RTC::RTC_OK;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 6e6c27759be..ff7d4024eba 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -228,7 +228,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if(m_baseRpyIn.isNew()                  ){ m_baseRpyIn.read(); }
     if(m_zmpIn.isNew()                      ){ m_zmpIn.read(); }
     if(m_optionalDataIn.isNew()             ){ m_optionalDataIn.read(); }
-    if(m_delayCheckPacketInboundIn.isNew()  ){ m_delayCheckPacketInboundIn.read(); }
+    if(m_delayCheckPacketInboundIn.isNew()  ){ m_delayCheckPacketInboundIn.read(); m_delayCheckPacketOutboundOut.write();}
     for(int i=0; i<ee_names.size(); i++){ if(m_localEEWrenchesIn[ee_names[i]]->isNew()){ m_localEEWrenchesIn[ee_names[i]]->read(); } }
 
     // button func
@@ -347,7 +347,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     m_basePosOut.write();
     m_baseRpyOut.write();
     m_zmpOut.write();
-    m_delayCheckPacketOutboundOut.write();
     m_optionalDataOut.write();
     addTimeReport("OutPort");
     if(DEBUGP) RTC_INFO_STREAM(time_report_str);

From 1a68ab98ed6a2084a307b32105a40a350ae0f64f Mon Sep 17 00:00:00 2001
From: JAXONRED <jaxonred@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 4 Nov 2019 19:59:42 +0900
Subject: [PATCH 186/217] add slave tgt pose port and shift ee force

---
 python/hrpsys_config.py                       | 28 +++++++------------
 .../WholeBodyMasterSlave.cpp                  | 23 +++++++++++++--
 .../WholeBodyMasterSlave.h                    |  4 +++
 3 files changed, 34 insertions(+), 21 deletions(-)

diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index d9c0e50f633..d79d74f0900 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -937,24 +937,16 @@ def setupLogger(self, maxLength=4000):
             self.connectLoggerPort(self.wbms, 'baseRpyOut')
             self.connectLoggerPort(self.wbms, 'zmpOut')
             self.connectLoggerPort(self.wbms, 'baseTformOut')
-            self.connectLoggerPort(self.wbms, 'htcom_dbgOut')
-            self.connectLoggerPort(self.wbms, 'htrf_dbgOut')
-            self.connectLoggerPort(self.wbms, 'htlf_dbgOut')
-            self.connectLoggerPort(self.wbms, 'htrh_dbgOut')
-            self.connectLoggerPort(self.wbms, 'htlh_dbgOut')
-            self.connectLoggerPort(self.wbms, 'hthead_dbgOut')
-            self.connectLoggerPort(self.wbms, 'htzmp_dbgOut')
-            self.connectLoggerPort(self.wbms, 'htrfw_dbgOut')
-            self.connectLoggerPort(self.wbms, 'htlfw_dbgOut')
-            self.connectLoggerPort(self.wbms, 'rpcom_dbgOut')
-            self.connectLoggerPort(self.wbms, 'rprf_dbgOut')
-            self.connectLoggerPort(self.wbms, 'rplf_dbgOut')
-            self.connectLoggerPort(self.wbms, 'rprh_dbgOut')
-            self.connectLoggerPort(self.wbms, 'rplh_dbgOut')
-            self.connectLoggerPort(self.wbms, 'rphead_dbgOut')
-            self.connectLoggerPort(self.wbms, 'rpzmp_dbgOut')
-            self.connectLoggerPort(self.wbms, 'rpdcp_dbgOut')
-            self.connectLoggerPort(self.wbms, 'rpacp_dbgOut')
+            self.connectLoggerPort(self.wbms, 'slave_rleg_wrench')
+            self.connectLoggerPort(self.wbms, 'slave_lleg_wrench')
+            self.connectLoggerPort(self.wbms, 'slave_rarm_wrench')
+            self.connectLoggerPort(self.wbms, 'slave_larm_wrench')
+            self.connectLoggerPort(self.wbms, 'slave_rleg_pose')
+            self.connectLoggerPort(self.wbms, 'slave_lleg_pose')
+            self.connectLoggerPort(self.wbms, 'slave_rarm_pose')
+            self.connectLoggerPort(self.wbms, 'slave_larm_pose')
+            self.connectLoggerPort(self.wbms, 'slave_com_pose')
+            self.connectLoggerPort(self.wbms, 'slave_head_pose')
         if self.abc != None:
             self.connectLoggerPort(self.abc, 'zmpOut')
             self.connectLoggerPort(self.abc, 'baseTformOut')
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index ff7d4024eba..2ffc61556be 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -181,6 +181,12 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
         registerInPort(n.c_str(), *m_masterTgtPosesIn[tgt_names[i]]);
         RTC_INFO_STREAM(" registerInPort " << n);
     }
+    for ( int i=0; i<tgt_names.size(); i++) {
+        std::string n = "slave_"+tgt_names[i]+"_pose";
+        m_slaveTgtPosesOut[tgt_names[i]] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_slaveTgtPoses[tgt_names[i]]));
+        registerOutPort(n.c_str(), *m_slaveTgtPosesOut[tgt_names[i]]);
+        RTC_INFO_STREAM(" registerOutPort " << n);
+    }
 
     RTC_INFO_STREAM("onInitialize() OK");
     loop = 0;
@@ -336,12 +342,23 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         hrp::ForceSensor* sensor = m_robot_act->sensor<hrp::ForceSensor>(to_sname[ee_names[i]]);
         hrp::Matrix33 sensorR_wld = sensor->link->R * sensor->localR;
         hrp::Matrix33 sensorR_from_base = m_robot_act->rootLink()->R.transpose() * sensorR_wld;
-        const hrp::Vector3 f_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee_names[i]].data).head(3);
-        const hrp::Vector3 t_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee_names[i]].data).tail(3);
-        m_slaveEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq( (hrp::dvector6()<<f_wld,t_wld).finished());
+        const hrp::Vector3 f_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee_names[i]].data).head(3);
+        const hrp::Vector3 t_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee_names[i]].data).tail(3);
+
+        const hrp::Vector3 sensor_to_ee_vec_wld = ee_ikc_map[ee_names[i]].getCurrentTargetPos(m_robot_act) - sensor->link->p;
+
+        const hrp::Vector3 f_ee_wld = f_sensor_wld;
+        const hrp::Vector3 t_ee_wld = t_sensor_wld - sensor_to_ee_vec_wld.cross(f_sensor_wld);
+        m_slaveEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq( (hrp::dvector6()<<f_ee_wld,t_ee_wld).finished());
         m_slaveEEWrenches[ee_names[i]].tm = m_qRef.tm;
         m_slaveEEWrenchesOut[ee_names[i]]->write();
+        m_slaveTgtPoses[ee_names[i]].data = hrp::to_Pose3D(ee_ikc_map[ee_names[i]].getCurrentTargetPose(m_robot_act));
+        m_slaveTgtPoses[ee_names[i]].tm = m_qRef.tm;
+        m_slaveTgtPosesOut[ee_names[i]]->write();
     }
+    m_slaveTgtPoses["com"].data = hrp::to_Pose3D( (hrp::dvector6()<<m_robot_act->calcCM(),0,0,0).finished());
+    m_slaveTgtPoses["com"].tm = m_qRef.tm;
+    m_slaveTgtPosesOut["com"]->write();
     // write
     m_qOut.write();
     m_basePosOut.write();
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 167759bb29a..2799e32a8f6 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -100,6 +100,10 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         typedef boost::shared_ptr<RTC::OutPort<RTC::TimedDoubleSeq> > OTDS_Ptr;
         std::map<std::string, OTDS_Ptr> m_slaveEEWrenchesOut;
 
+        std::map<std::string, RTC::TimedPose3D> m_slaveTgtPoses;
+        typedef boost::shared_ptr<RTC::OutPort<RTC::TimedPose3D> > OTP3_Ptr;
+        std::map<std::string, OTP3_Ptr> m_slaveTgtPosesOut;
+
         std::map<std::string, RTC::TimedDoubleSeq> m_localEEWrenches;
         typedef boost::shared_ptr<RTC::InPort<RTC::TimedDoubleSeq> > ITDS_Ptr;
         std::map<std::string, ITDS_Ptr> m_localEEWrenchesIn;

From 20ddd25175a1774c2b7f9ebdc7b10d7c2a934e39 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 5 Nov 2019 23:54:50 +0900
Subject: [PATCH 187/217] rhp finger

---
 .../WholeBodyMasterSlave.cpp                  | 29 ++++++++++++++++++-
 1 file changed, 28 insertions(+), 1 deletion(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 2ffc61556be..ace88947989 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -175,6 +175,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     tgt_names = ee_names;
     tgt_names.push_back("com");
     tgt_names.push_back("head");
+    tgt_names.push_back("rhand");
+    tgt_names.push_back("lhand");
     for ( int i=0; i<tgt_names.size(); i++) {
         std::string n = "master_"+tgt_names[i]+"_pose";
         m_masterTgtPosesIn[tgt_names[i]] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
@@ -250,11 +252,16 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     }
 
 
+
     if( mode.now() != MODE_PAUSE ){ // stop updating input when MODE_PAUSE
         for(int i=0; i<tgt_names.size(); i++){
             if (m_masterTgtPosesIn[tgt_names[i]]->isNew()){
                 m_masterTgtPosesIn[tgt_names[i]]->read();
-                wbms->hp_wld_raw.tgt_by_str(tgt_names[i]).abs = hrp::to_Pose3(m_masterTgtPoses[tgt_names[i]].data);
+                if(tgt_names[i] == "lhand" || tgt_names[i] == "rhand"){
+                    // nothind
+                }else{
+                    wbms->hp_wld_raw.tgt_by_str(tgt_names[i]).abs = hrp::to_Pose3(m_masterTgtPoses[tgt_names[i]].data);
+                }
             }
         }
         if (m_actCPIn.isNew())  { m_actCPIn.read(); rel_act_cp = hrp::to_Vector3(m_actCP.data);}
@@ -277,6 +284,26 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         solveFullbodyIK(wbms->rp_ref_out.tgt[com].abs, wbms->rp_ref_out.tgt[rf].abs, wbms->rp_ref_out.tgt[lf].abs, wbms->rp_ref_out.tgt[rh].abs, wbms->rp_ref_out.tgt[lh].abs, wbms->rp_ref_out.tgt[head].abs);
         addTimeReport("IK");
 
+        // RHP finger
+        if(fik->m_robot->name() == "RHP4B"){
+            const double hand_max_vel = M_PI/0.4;//0.4s for 180deg
+            {
+                double trigger_in = m_masterTgtPoses["lhand"].data.position.y;
+                LIMIT_MINMAX(trigger_in, 0.0, 1.0);
+                double tgt_hand_q = (-29.0 + (124.0-(-29.0))*trigger_in ) /180.0*M_PI;
+                LIMIT_MINMAX(tgt_hand_q, fik->m_robot->link("L_HAND")->llimit, fik->m_robot->link("L_HAND")->ulimit);
+                fik->m_robot->link("L_HAND")->q = tgt_hand_q;
+                avg_q_vel(fik->m_robot->link("L_HAND")->jointId) = hand_max_vel;
+            }{
+                double trigger_in = m_masterTgtPoses["rhand"].data.position.y;
+                LIMIT_MINMAX(trigger_in, 0.0, 1.0);
+                double tgt_hand_q = (-29.0 + (124.0-(-29.0))*trigger_in ) /180.0*M_PI;
+                LIMIT_MINMAX(tgt_hand_q, fik->m_robot->link("R_HAND")->llimit, fik->m_robot->link("R_HAND")->ulimit);
+                fik->m_robot->link("R_HAND")->q = tgt_hand_q;
+                avg_q_vel(fik->m_robot->link("R_HAND")->jointId) = hand_max_vel;
+            }
+        }
+
         smoothingJointAngles(fik->m_robot, m_robot_vsafe);
 
         hrp::Vector3 com = m_robot_vsafe->calcCM();

From f0b4eab5d36f8dcfe14514d889c96ae9dc200ca5 Mon Sep 17 00:00:00 2001
From: leus <leus@example.com>
Date: Fri, 8 Nov 2019 00:02:30 +0900
Subject: [PATCH 188/217] tune for demo

---
 rtc/HapticController/HapticController.cpp | 12 ++++++++----
 rtc/HapticController/HapticController.h   |  4 ++--
 2 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index 9b123656612..545133b7523 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -344,7 +344,7 @@ void HapticController::calcTorque(){
                 const hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
                 for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
             }
-            is_contact_to_floor[tgt[i]] = (foot_h_from_floor < 0 + 0.05);
+            is_contact_to_floor[tgt[i]] = (foot_h_from_floor < 0 + 0.03);
         }
 
         ///// virtual back wall
@@ -482,11 +482,16 @@ void HapticController::calcTorque(){
         if(loop%1000==0)dbgv(friction_tq);
     }
 
+
+    const hrp::dvector max_torque = (hrp::dvector(m_robot->numJoints()) << 20,40,60,60,20,20, 20,40,60,60,20,20, 40, 15,15,10,10,6,4,4, 15,15,10,10,6,4,4).finished();
+    hrp::dvector j_power_coeff = max_torque/max_torque.maxCoeff(); // reduce gain of light inertia joint
+
     {// calc qref pd control torque
         hrp::dvector q_diff = hrp::to_dvector(m_qRef.data) - hrp::to_dvector(m_qAct.data);
         hrp::dvector dq_diff = - dqAct_filtered;
-        hrp::dvector torque = q_diff * hcp.q_ref_pd_gain(0) + dq_diff * hcp.q_ref_pd_gain(1);
+        hrp::dvector torque = (q_diff * hcp.q_ref_pd_gain(0) + dq_diff * hcp.q_ref_pd_gain(1)) * j_power_coeff;
         for (int i=0; i<m_robot->numJoints(); i++){
+            LIMIT_NORM(torque(i), max_torque(i)/5);// pd torque will be limited to 1/5 max torque
             m_robot->joint(i)->u += torque(i);
         }
     }
@@ -495,10 +500,9 @@ void HapticController::calcTorque(){
 
 
 
-    const hrp::dvector max_torque = (hrp::dvector(m_robot->numJoints()) << 30,80,80,80,30,30, 30,80,80,80,30,30, 80, 30,30,25,25,20,10,10, 30,30,25,25,20,10,10).finished();
 
     for(int i=0;i<m_robot->numJoints();i++){
-        LIMIT_MINMAX(m_robot->joint(i)->u, -max_torque(i), max_torque(i));
+        LIMIT_NORM(m_robot->joint(i)->u, max_torque(i)*2);// up to x2 max torque
 //        double tlimit = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst;////?????
     }
 
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index b894b28e9cd..016058452aa 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -155,9 +155,9 @@ class HapticController : public RTC::DataFlowComponentBase{
                 wrench_hpf_gain             = 1;
                 wrench_lpf_gain             = 0.2;
                 ee_pos_rot_friction_coeff   << 0, 0; // 1, 0.1
-                floor_pd_gain               << 5000, 100;
+                floor_pd_gain               << 10000, 500;
                 foot_horizontal_pd_gain     << 300, 30;
-                q_ref_pd_gain               << 0, 0;
+                q_ref_pd_gain               << 50, 0;
             }
         } hcp;
 

From 13ca45188c677a90ee02979e0b0ad7e63780dcbb Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 13 Nov 2019 12:12:11 +0900
Subject: [PATCH 189/217] zmp st tune

---
 .../WholeBodyMasterSlave.cpp                  | 23 +++++++++----------
 rtc/WholeBodyMasterSlave/wbms_core.h          | 20 ++++++++--------
 2 files changed, 21 insertions(+), 22 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index ace88947989..f83b2cd07f4 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -146,7 +146,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     RTC_INFO_STREAM("setup interpolator finished");
 
     ref_zmp_filter.resize(XYZ);
-    ref_zmp_filter.setParameter(20, 1/m_dt, Q_BUTTERWORTH);
+    ref_zmp_filter.setParameter(100, 1/m_dt, Q_BUTTERWORTH);
 
     cp_flag = -1;
     lt.fill(0);
@@ -307,13 +307,12 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         smoothingJointAngles(fik->m_robot, m_robot_vsafe);
 
         hrp::Vector3 com = m_robot_vsafe->calcCM();
-        com(Z) += 0.3;//hotfix
         static hrp::Vector3 com_old = com;
         static hrp::Vector3 com_old_old = com_old;
         hrp::Vector3 com_acc = (com - 2*com_old + com_old_old)/(m_dt*m_dt);
         hrp::Vector3 ref_zmp; ref_zmp << com.head(XY)-(com(Z)/G)*com_acc.head(XY), 0;
-        if(mode.isInitialize()){ ref_zmp_filter.reset(ref_zmp); }
-        ref_zmp = ref_zmp_filter.passFilter(ref_zmp);
+//        if(mode.isInitialize()){ ref_zmp_filter.reset(ref_zmp); }
+//        ref_zmp = ref_zmp_filter.passFilter(ref_zmp);
         com_old_old = com_old;
         com_old = com;
         wbms->act_rs.com = com;
@@ -598,10 +597,10 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
     if( fik->m_robot->link("CHEST_JOINT1") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT1")->jointId) = 1e3;
 //    if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 10;
     if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 0;//実機修理中
-    if( fik->m_robot->link("HEAD_JOINT0") != NULL) fik->m_robot->link("HEAD_JOINT0")->llimit = deg2rad(-30);
-    if( fik->m_robot->link("HEAD_JOINT0") != NULL) fik->m_robot->link("HEAD_JOINT0")->ulimit = deg2rad(30);
-    if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->llimit = deg2rad(-45);
-    if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->ulimit = deg2rad(10);
+//    if( fik->m_robot->link("HEAD_JOINT0") != NULL) fik->m_robot->link("HEAD_JOINT0")->llimit = deg2rad(-30);
+//    if( fik->m_robot->link("HEAD_JOINT0") != NULL) fik->m_robot->link("HEAD_JOINT0")->ulimit = deg2rad(30);
+//    if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->llimit = deg2rad(-45);
+//    if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->ulimit = deg2rad(10);
     if( fik->m_robot->link("RARM_JOINT2") != NULL) fik->m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-45);//脇内側の干渉回避
     if( fik->m_robot->link("LARM_JOINT2") != NULL) fik->m_robot->link("LARM_JOINT2")->llimit = deg2rad(45);
     if( fik->m_robot->link("RLEG_JOINT3") != NULL) fik->m_robot->link("RLEG_JOINT3")->llimit = deg2rad(40);//膝伸びきり防止のため
@@ -659,10 +658,10 @@ void WholeBodyMasterSlave::smoothingJointAngles(hrp::BodyPtr _robot, hrp::BodyPt
 
     static hrp::dvector ans_state_vel = hrp::dvector::Zero(fik->numStates());
 
-    hrp::dvector estimated_times_from_vel_limit = (hrp::getRobotStateVec(_robot) - hrp::getRobotStateVec(_robot_safe)).array().abs() / avg_q_vel.array();
-    hrp::dvector estimated_times_from_acc_limit = ans_state_vel.array().abs() / avg_q_acc.array();
-    double longest_estimated_time_from_vel_limit = estimated_times_from_vel_limit.maxCoeff();
-    double longest_estimated_time_from_acc_limit = estimated_times_from_acc_limit.maxCoeff();
+    const hrp::dvector estimated_times_from_vel_limit = (hrp::getRobotStateVec(_robot) - hrp::getRobotStateVec(_robot_safe)).array().abs() / avg_q_vel.array();
+    const hrp::dvector estimated_times_from_acc_limit = ans_state_vel.array().abs() / avg_q_acc.array();
+    const double longest_estimated_time_from_vel_limit = estimated_times_from_vel_limit.maxCoeff();
+    const double longest_estimated_time_from_acc_limit = estimated_times_from_acc_limit.maxCoeff();
     goal_time = hrp::Vector3(longest_estimated_time_from_vel_limit, longest_estimated_time_from_acc_limit, min_goal_time_offset).maxCoeff();
 
     q_ip->setGoal(hrp::getRobotStateVec(_robot).data(), goal_time, true);
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 8dd8f5c6b3c..d86b24e41ff 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -382,7 +382,7 @@ class WBMSCore{
                 auto_com_foot_move_detect_height    = 0.03;
                 base_to_hand_min_distance           = 0.5;
                 capture_point_extend_ratio          = 1.0;
-                com_filter_cutoff_hz                = 0.5;
+                com_filter_cutoff_hz                = 1.5;
                 foot_collision_avoidance_distance   = 0.16;
                 human_to_robot_ratio                = 1.0;//human 1.1m vs jaxon 1.06m
                 max_double_support_width            = 0.4;
@@ -464,21 +464,12 @@ class WBMSCore{
             convertHumanToRobot                 (hp_wld_raw, rp_ref_out);
             setAutoCOMMode                      (hp_wld_raw, rp_ref_out_old, rp_ref_out);
 
-            ///// COMだけはフィルターいるか・・・
-            if(is_initial_loop || wp.com_filter_cutoff_hz != com_filter_cutoff_hz_old){
-                com_filter.setParameter(wp.com_filter_cutoff_hz, HZ, Q_NOOVERSHOOT);
-                com_filter.reset(rp_ref_out.tgt[com].abs.p);
-                com_filter_cutoff_hz_old = wp.com_filter_cutoff_hz;
-            }
-            rp_ref_out.tgt[com].abs.p = com_filter.passFilter(rp_ref_out.tgt[com].abs.p);
-
             lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
             limitEEWorkspace                    (rp_ref_out_old, rp_ref_out);
             setFootContactPoseByGoContact       (rp_ref_out_old, rp_ref_out);
             limitFootVelNearGround              (rp_ref_out_old, rp_ref_out);
             avoidFootSinkIntoFloor              (rp_ref_out);
 
-
             applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, rp_ref_out.tgt[com].abs.p);
             applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
             com_forcp_ref = rp_ref_out.tgt[com].abs.p.head(XY);
@@ -490,6 +481,15 @@ class WBMSCore{
             applyZMPCalcFromCOM                 (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);//結局STに送るZMPは最終段で計算するからこれ意味ない
             H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[lf].abs.p(Z));
             rp_ref_out_old = rp_ref_out;
+
+            ///// COMだけはフィルターいるか・・・
+            if(is_initial_loop || wp.com_filter_cutoff_hz != com_filter_cutoff_hz_old){
+                com_filter.setParameter(wp.com_filter_cutoff_hz, HZ, Q_NOOVERSHOOT);
+                com_filter.reset(rp_ref_out.tgt[com].abs.p);
+                com_filter_cutoff_hz_old = wp.com_filter_cutoff_hz;
+            }
+            rp_ref_out.tgt[com].abs.p = com_filter.passFilter(rp_ref_out.tgt[com].abs.p);
+
             loop++;
             is_initial_loop = false;
         }

From 8e58f421f685452f5193784d5525f23c4f4e2e20 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 13 Nov 2019 14:35:28 +0900
Subject: [PATCH 190/217] add dbg port

---
 python/hrpsys_config.py                   | 11 ++++++++++
 rtc/HapticController/HapticController.cpp | 25 +++++++++++++++++++++--
 rtc/HapticController/HapticController.h   |  8 ++++++--
 3 files changed, 40 insertions(+), 4 deletions(-)

diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
index d79d74f0900..f58181da606 100755
--- a/python/hrpsys_config.py
+++ b/python/hrpsys_config.py
@@ -931,6 +931,17 @@ def setupLogger(self, maxLength=4000):
         if self.hc != None:
             self.connectLoggerPort(self.hc, 'tau')
             self.connectLoggerPort(self.hc, 'debugData')
+            self.connectLoggerPort(self.hc, 'master_rleg_pose')
+            self.connectLoggerPort(self.hc, 'master_lleg_pose')
+            self.connectLoggerPort(self.hc, 'master_rarm_pose')
+            self.connectLoggerPort(self.hc, 'master_larm_pose')
+            self.connectLoggerPort(self.hc, 'master_com_pose')
+            self.connectLoggerPort(self.hc, 'master_head_pose')
+            self.connectLoggerPort(self.hc, 'master_rleg_wrench_dbg')
+            self.connectLoggerPort(self.hc, 'master_lleg_wrench_dbg')
+            self.connectLoggerPort(self.hc, 'master_rarm_wrench_dbg')
+            self.connectLoggerPort(self.hc, 'master_larm_wrench_dbg')
+
         if self.wbms != None:
             self.connectLoggerPort(self.wbms, 'q')
             self.connectLoggerPort(self.wbms, 'basePosOut')
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index 9b123656612..b351652545f 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -119,6 +119,15 @@ RTC::ReturnCode_t HapticController::onInitialize(){
         RTC_INFO_STREAM(" registerOutPort " << n);
     }
 
+    //debug : This won't be send to slave
+    for ( int i=0; i<ee_names.size(); i++) {
+        std::string n = "master_"+ee_names[i]+"_wrench_dbg";
+        m_masterEEWrenchesOut[ee_names[i]] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_masterEEWrenches[ee_names[i]]));
+        registerOutPort(n.c_str(), *m_masterEEWrenchesOut[ee_names[i]]);
+        m_masterEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
+        RTC_INFO_STREAM(" registerOutPort " << n );
+    }
+
     m_tau.data = hrp::to_DoubleSeq(hrp::dvector::Zero(m_robot->numJoints()));
 
     loop = 0;
@@ -187,8 +196,6 @@ RTC::ReturnCode_t HapticController::onExecute(RTC::UniqueId ec_id){
     // write
     m_qOut.write();
     m_tauOut.write();
-    m_teleopOdomOut.write();
-
     loop ++;
     return RTC::RTC_OK;
 }
@@ -314,6 +321,8 @@ void HapticController::calcTorque(){
 //        if(loop%500==0)dbgv(hrp::getUAll(m_robot));
 //    }
 
+    std::map<std::string, hrp::dvector6> masterEEWrenches;
+    for (int i=0; i<ee_names.size(); i++){ masterEEWrenches[ee_names[i]].fill(0); }
 
     std::map<std::string, bool> is_contact_to_floor;
     {
@@ -330,6 +339,7 @@ void HapticController::calcTorque(){
             LIMIT_NORM_V(wrench.tail(3), 50);
             hrp::dvector tq_tmp = J_ee[ee_names[i]].transpose() * wrench;
             for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
+            masterEEWrenches[tgt[i]] += wrench;
         }
 
         ///// virtual floor
@@ -343,6 +353,7 @@ void HapticController::calcTorque(){
                 LIMIT_NORM_V(wrench, 1000);
                 const hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
                 for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
+                masterEEWrenches[tgt[i]] += wrench;
             }
             is_contact_to_floor[tgt[i]] = (foot_h_from_floor < 0 + 0.05);
         }
@@ -356,6 +367,7 @@ void HapticController::calcTorque(){
                 LIMIT_NORM_V(wrench, 100);
                 hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
                 for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
+                masterEEWrenches[tgt[i]] += wrench;
             }
         }
 
@@ -367,6 +379,7 @@ void HapticController::calcTorque(){
                 LIMIT_NORM_V(wrench, 50);
                 hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
                 for (int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
+                masterEEWrenches[tgt[i]] += wrench;
             }
         }
 
@@ -380,6 +393,7 @@ void HapticController::calcTorque(){
                 LIMIT_NORM_V(wrench, 1000);
                 hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
                 for (int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
+                masterEEWrenches[tgt[i]] += wrench;
             }
         }else{
             locked_rel_pos = ee_pose["rleg"].p - ee_pose["lleg"].p;
@@ -452,6 +466,7 @@ void HapticController::calcTorque(){
         hrp::dvector tq_from_feedbackWrench = J_base_to_ee.transpose() * wrench_used[ee_names[i]];
         for (int j = 0; j < jp.numJoints(); j++) jp.joint(j)->u += tq_from_feedbackWrench(j);
         if(loop%1000==0)dbgv(wrench_used[ee_names[i]]);
+        masterEEWrenches[ee_names[i]] += wrench_used[ee_names[i]];
     }
 
 
@@ -505,6 +520,12 @@ void HapticController::calcTorque(){
     if(loop%1000==0)dbgv(hrp::getUAll(m_robot));
 
 
+    // for debug plot
+    for (int i=0; i<ee_names.size(); i++){
+        m_masterEEWrenches[ee_names[i]].tm = m_qRef.tm;
+        m_masterEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq(masterEEWrenches[ee_names[i]]);
+        m_masterEEWrenchesOut[ee_names[i]]->write();
+    }
 
 
 
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index b894b28e9cd..2ef20a7705c 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -78,17 +78,21 @@ class HapticController : public RTC::DataFlowComponentBase{
         RTC::InPort<RTC::TimedDoubleSeq> m_qActIn;
         RTC::TimedDoubleSeq m_dqAct;
         RTC::InPort<RTC::TimedDoubleSeq> m_dqActIn;
-        std::map<std::string, RTC::TimedDoubleSeq> m_slaveEEWrenches;
         typedef boost::shared_ptr<RTC::InPort<RTC::TimedDoubleSeq> > ITDS_Ptr;
+        std::map<std::string, RTC::TimedDoubleSeq> m_slaveEEWrenches;
         std::map<std::string, ITDS_Ptr> m_slaveEEWrenchesIn;
         RTC::TimedDoubleSeq m_tau;
         RTC::OutPort<RTC::TimedDoubleSeq> m_tauOut;
         RTC::TimedPose3D m_teleopOdom;
         RTC::OutPort<RTC::TimedPose3D> m_teleopOdomOut;
         RTC::OutPort<RTC::TimedDoubleSeq> m_qOut;
-        std::map<std::string, RTC::TimedPose3D> m_masterTgtPoses;
         typedef boost::shared_ptr<RTC::OutPort<RTC::TimedPose3D> > OTP3_Ptr;
+        std::map<std::string, RTC::TimedPose3D> m_masterTgtPoses;
         std::map<std::string, OTP3_Ptr> m_masterTgtPosesOut;
+        //debug
+        typedef boost::shared_ptr<RTC::OutPort<RTC::TimedDoubleSeq> > OTDS_Ptr;
+        std::map<std::string, RTC::TimedDoubleSeq> m_masterEEWrenches;
+        std::map<std::string, OTDS_Ptr> m_masterEEWrenchesOut;
         RTC::TimedDoubleSeq m_debugData;
         RTC::OutPort<RTC::TimedDoubleSeq> m_debugDataOut;
         RTC::CorbaPort m_HapticControllerServicePort;

From 4289fc180f243748ab7f0bd84be3f2cbe19833c1 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 13 Nov 2019 21:33:41 +0900
Subject: [PATCH 191/217] add upper lower

---
 idl/HapticControllerService.idl           |   5 ++
 rtc/HapticController/HapticController.cpp | 104 +++++++++++++++-------
 rtc/HapticController/HapticController.h   |  41 +++++----
 3 files changed, 101 insertions(+), 49 deletions(-)

diff --git a/idl/HapticControllerService.idl b/idl/HapticControllerService.idl
index d5359ca3676..0f2dc182281 100644
--- a/idl/HapticControllerService.idl
+++ b/idl/HapticControllerService.idl
@@ -11,6 +11,8 @@ module OpenHRP
     typedef sequence<double, 2> DblSequence2;
     typedef sequence<double, 3> DblSequence3;
     typedef sequence<double, 4> DblSequence4;
+    typedef sequence<double, 6> DblSequence6;
+    typedef sequence<sequence<double, 6> , 4> DblSeqSeq6;
     typedef double DblArray2[2];
     typedef double DblArray3[3];
     typedef double DblArray4[4];
@@ -20,6 +22,8 @@ module OpenHRP
     {
         double dqAct_filter_cutoff_hz;
         double ee_vel_filter_cutoff_hz;
+        double ex_gravity_compensation_ratio_lower;
+        double ex_gravity_compensation_ratio_upper;
         double baselink_height_from_floor;
         double foot_min_distance;
         double force_feedback_ratio;
@@ -34,6 +38,7 @@ module OpenHRP
         DblSequence2 floor_pd_gain;
         DblSequence2 foot_horizontal_pd_gain;
         DblSequence2 q_ref_pd_gain;
+        DblSeqSeq6   ex_ee_ref_wrench;
     };
     boolean startHapticController();
     boolean stopHapticController();
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index e77521e1900..e38ea1c2112 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -191,7 +191,8 @@ RTC::ReturnCode_t HapticController::onExecute(RTC::UniqueId ec_id){
     }
 
 
-    m_tau.data = hrp::to_DoubleSeq(hrp::getUAll(m_robot) * output_ratio);
+    m_tau.tm    = m_qRef.tm;
+    m_tau.data  = hrp::to_DoubleSeq(hrp::getUAll(m_robot) * output_ratio);
 
     // write
     m_qOut.write();
@@ -245,6 +246,13 @@ void HapticController::calcCurrentState(){
     hrp::Vector3 f_base_wld, t_base_wld;
     calcRootLinkWrenchFromInverseDynamics(m_robot, idsb, f_base_wld, t_base_wld); // torque (m_robot->joint(i)->u) set
     updateInvDynStateBuffer(idsb);
+    for(int i=0; i<m_robot->numJoints(); i++){
+        if(m_robot->joint(i)->name.find("LEG_JOINT") != std::string::npos){
+            m_robot->joint(i)->u *= hcp.ex_gravity_compensation_ratio_lower;
+        }else if(m_robot->joint(i)->name.find("ARM_JOINT") != std::string::npos){
+            m_robot->joint(i)->u *= hcp.ex_gravity_compensation_ratio_upper;
+        }
+    }
 
     ///// calc end effector position and velocity
     for(int i=0; i<ee_names.size();i++){
@@ -399,6 +407,15 @@ void HapticController::calcTorque(){
             locked_rel_pos = ee_pose["rleg"].p - ee_pose["lleg"].p;
         }
 
+        ///// ex
+        for (int i=0; i<ee_names.size();i++){
+            hrp::dvector6 wrench = hcp.ex_ee_ref_wrench[ee_names[i]];
+            LIMIT_NORM_V(wrench, 500);
+            hrp::dvector tq_tmp = J_ee[ee_names[i]].transpose() * wrench;
+            for (int j=0; j<jpath_ee[ee_names[i]].numJoints(); j++){ jpath_ee[ee_names[i]].joint(j)->u += tq_tmp(j); }
+            masterEEWrenches[ee_names[i]] += wrench;
+        }
+
 
         static std::map<std::string, hrp::Vector2> leg_pos_from_floor_origin;
         hrp::Vector2 baselink_pos_from_floor_origin;
@@ -612,26 +629,37 @@ namespace hrp{
     hrp::Vector2 to_Vector2(const OpenHRP::HapticControllerService::DblSequence2& in){ return (hrp::Vector2()<< in[0],in[1]).finished(); }
     OpenHRP::HapticControllerService::DblSequence2 to_DblSequence2(const hrp::Vector2& in){
         OpenHRP::HapticControllerService::DblSequence2 ret; ret.length(2); ret[0] = in(0); ret[1] = in(1); return ret; }
+    OpenHRP::HapticControllerService::DblSequence6 to_DblSequence6(const hrp::dvector6& in){
+        OpenHRP::HapticControllerService::DblSequence6 ret;
+        ret.length(6);  for(int i=0; i<6; i++){ ret[i] = in(i); }   return ret;
+    }
 }
 
 bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param){
     RTC_INFO_STREAM("setHapticControllerParam");
-    hcp.baselink_height_from_floor  = i_param.baselink_height_from_floor;
-    hcp.dqAct_filter_cutoff_hz      = i_param.dqAct_filter_cutoff_hz;
-    hcp.ee_vel_filter_cutoff_hz     = i_param.ee_vel_filter_cutoff_hz;
-    hcp.foot_min_distance           = i_param.foot_min_distance;
-    hcp.force_feedback_ratio        = i_param.force_feedback_ratio;
-    hcp.gravity_compensation_ratio  = i_param.gravity_compensation_ratio;
-    hcp.q_friction_coeff            = i_param.q_friction_coeff;
-    hcp.q_ref_output_ratio_goal     = i_param.q_ref_output_ratio_goal;
-    hcp.wrench_hpf_cutoff_hz        = i_param.wrench_hpf_cutoff_hz;
-    hcp.wrench_lpf_cutoff_hz        = i_param.wrench_lpf_cutoff_hz;
-    hcp.wrench_hpf_gain             = i_param.wrench_hpf_gain;
-    hcp.wrench_lpf_gain             = i_param.wrench_lpf_gain;
-    hcp.ee_pos_rot_friction_coeff   = hrp::to_Vector2(i_param.ee_pos_rot_friction_coeff);
-    hcp.floor_pd_gain               = hrp::to_Vector2(i_param.floor_pd_gain);
-    hcp.foot_horizontal_pd_gain     = hrp::to_Vector2(i_param.foot_horizontal_pd_gain);
-    hcp.q_ref_pd_gain               = hrp::to_Vector2(i_param.q_ref_pd_gain);
+    hcp.baselink_height_from_floor          = i_param.baselink_height_from_floor;
+    hcp.dqAct_filter_cutoff_hz              = i_param.dqAct_filter_cutoff_hz;
+    hcp.ee_vel_filter_cutoff_hz             = i_param.ee_vel_filter_cutoff_hz;
+    hcp.ex_gravity_compensation_ratio_lower = i_param.ex_gravity_compensation_ratio_lower;
+    hcp.ex_gravity_compensation_ratio_upper = i_param.ex_gravity_compensation_ratio_upper;
+    hcp.foot_min_distance                   = i_param.foot_min_distance;
+    hcp.force_feedback_ratio                = i_param.force_feedback_ratio;
+    hcp.gravity_compensation_ratio          = i_param.gravity_compensation_ratio;
+    hcp.q_friction_coeff                    = i_param.q_friction_coeff;
+    hcp.q_ref_output_ratio_goal             = i_param.q_ref_output_ratio_goal;
+    hcp.wrench_hpf_cutoff_hz                = i_param.wrench_hpf_cutoff_hz;
+    hcp.wrench_lpf_cutoff_hz                = i_param.wrench_lpf_cutoff_hz;
+    hcp.wrench_hpf_gain                     = i_param.wrench_hpf_gain;
+    hcp.wrench_lpf_gain                     = i_param.wrench_lpf_gain;
+    hcp.ee_pos_rot_friction_coeff           = hrp::to_Vector2(i_param.ee_pos_rot_friction_coeff);
+    hcp.floor_pd_gain                       = hrp::to_Vector2(i_param.floor_pd_gain);
+    hcp.foot_horizontal_pd_gain             = hrp::to_Vector2(i_param.foot_horizontal_pd_gain);
+    hcp.q_ref_pd_gain                       = hrp::to_Vector2(i_param.q_ref_pd_gain);
+    for(int i=0;i<ee_names.size();i++){
+        for(int j=0;j<6;j++){
+            hcp.ex_ee_ref_wrench[ee_names[i]](j) =  i_param.ex_ee_ref_wrench[i][j];
+        }
+    }
     ///// update process if required
     q_ref_ip->setGoal(&hcp.q_ref_output_ratio_goal, 5.0, true);
     baselink_h_ip->setGoal(&hcp.baselink_height_from_floor, 5.0, true);
@@ -651,22 +679,32 @@ bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticC
 
 bool HapticController::getParams(OpenHRP::HapticControllerService::HapticControllerParam& i_param){
     RTC_INFO_STREAM("getHapticControllerParam");
-    i_param.baselink_height_from_floor  = hcp.baselink_height_from_floor;
-    i_param.dqAct_filter_cutoff_hz      = hcp.dqAct_filter_cutoff_hz;
-    i_param.ee_vel_filter_cutoff_hz     = hcp.ee_vel_filter_cutoff_hz;
-    i_param.foot_min_distance           = hcp.foot_min_distance;
-    i_param.force_feedback_ratio        = hcp.force_feedback_ratio;
-    i_param.gravity_compensation_ratio  = hcp.gravity_compensation_ratio;
-    i_param.q_friction_coeff            = hcp.q_friction_coeff;
-    i_param.q_ref_output_ratio_goal     = hcp.q_ref_output_ratio_goal;
-    i_param.wrench_hpf_cutoff_hz        = hcp.wrench_hpf_cutoff_hz;
-    i_param.wrench_lpf_cutoff_hz        = hcp.wrench_lpf_cutoff_hz;
-    i_param.wrench_hpf_gain             = hcp.wrench_hpf_gain;
-    i_param.wrench_lpf_gain             = hcp.wrench_lpf_gain;
-    i_param.ee_pos_rot_friction_coeff   = hrp::to_DblSequence2(hcp.ee_pos_rot_friction_coeff);
-    i_param.floor_pd_gain               = hrp::to_DblSequence2(hcp.floor_pd_gain);
-    i_param.foot_horizontal_pd_gain     = hrp::to_DblSequence2(hcp.foot_horizontal_pd_gain);
-    i_param.q_ref_pd_gain               = hrp::to_DblSequence2(hcp.q_ref_pd_gain);
+    i_param.baselink_height_from_floor          = hcp.baselink_height_from_floor;
+    i_param.dqAct_filter_cutoff_hz              = hcp.dqAct_filter_cutoff_hz;
+    i_param.ee_vel_filter_cutoff_hz             = hcp.ee_vel_filter_cutoff_hz;
+    i_param.ex_gravity_compensation_ratio_lower = hcp.ex_gravity_compensation_ratio_lower;
+    i_param.ex_gravity_compensation_ratio_upper = hcp.ex_gravity_compensation_ratio_upper;
+    i_param.foot_min_distance                   = hcp.foot_min_distance;
+    i_param.force_feedback_ratio                = hcp.force_feedback_ratio;
+    i_param.gravity_compensation_ratio          = hcp.gravity_compensation_ratio;
+    i_param.q_friction_coeff                    = hcp.q_friction_coeff;
+    i_param.q_ref_output_ratio_goal             = hcp.q_ref_output_ratio_goal;
+    i_param.wrench_hpf_cutoff_hz                = hcp.wrench_hpf_cutoff_hz;
+    i_param.wrench_lpf_cutoff_hz                = hcp.wrench_lpf_cutoff_hz;
+    i_param.wrench_hpf_gain                     = hcp.wrench_hpf_gain;
+    i_param.wrench_lpf_gain                     = hcp.wrench_lpf_gain;
+    i_param.ee_pos_rot_friction_coeff           = hrp::to_DblSequence2(hcp.ee_pos_rot_friction_coeff);
+    i_param.floor_pd_gain                       = hrp::to_DblSequence2(hcp.floor_pd_gain);
+    i_param.foot_horizontal_pd_gain             = hrp::to_DblSequence2(hcp.foot_horizontal_pd_gain);
+    i_param.q_ref_pd_gain                       = hrp::to_DblSequence2(hcp.q_ref_pd_gain);
+
+    i_param.ex_ee_ref_wrench.length(4);
+    for(int i=0;i<ee_names.size();i++){
+        i_param.ex_ee_ref_wrench[i].length(6);
+        for(int j=0;j<6;j++){
+            i_param.ex_ee_ref_wrench[i][j] = hcp.ex_ee_ref_wrench[ee_names[i]](j);
+        }
+    }
     return true;
 }
 
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index 22f7e74d072..f83c40dc737 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -132,6 +132,8 @@ class HapticController : public RTC::DataFlowComponentBase{
                 double default_baselink_h_from_floor;
                 double dqAct_filter_cutoff_hz;
                 double ee_vel_filter_cutoff_hz;
+                double ex_gravity_compensation_ratio_lower;
+                double ex_gravity_compensation_ratio_upper;
                 double foot_min_distance;
                 double force_feedback_ratio;
                 double gravity_compensation_ratio;
@@ -145,23 +147,30 @@ class HapticController : public RTC::DataFlowComponentBase{
                 hrp::Vector2 floor_pd_gain;
                 hrp::Vector2 foot_horizontal_pd_gain;
                 hrp::Vector2 q_ref_pd_gain;
+                std::map<std::string, hrp::dvector6> ex_ee_ref_wrench;
             HCParams(){
-                baselink_height_from_floor  = 1.5;// will be overwrited
-                dqAct_filter_cutoff_hz      = 500;// 10以下で確実に位相遅れによる振動
-                ee_vel_filter_cutoff_hz     = 500;// 10以下で確実に位相遅れによる振動
-                foot_min_distance           = 0.3;
-                force_feedback_ratio        = 0.2;
-                gravity_compensation_ratio  = 0.9;
-                q_friction_coeff            = 0;
-                q_ref_output_ratio_goal     = 0;
-                wrench_hpf_cutoff_hz        = 20;
-                wrench_lpf_cutoff_hz        = 0.3;
-                wrench_hpf_gain             = 1;
-                wrench_lpf_gain             = 0.2;
-                ee_pos_rot_friction_coeff   << 0, 0; // 1, 0.1
-                floor_pd_gain               << 10000, 500;
-                foot_horizontal_pd_gain     << 300, 30;
-                q_ref_pd_gain               << 50, 0;
+                baselink_height_from_floor          = 1.5;// will be overwrited
+                dqAct_filter_cutoff_hz              = 500;// 10以下で確実に位相遅れによる振動
+                ee_vel_filter_cutoff_hz             = 500;// 10以下で確実に位相遅れによる振動
+                ex_gravity_compensation_ratio_lower = 0.9;
+                ex_gravity_compensation_ratio_upper = 1.1;
+                foot_min_distance                   = 0.3;
+                force_feedback_ratio                = 0.2;
+                gravity_compensation_ratio          = 0.9;
+                q_friction_coeff                    = 0;
+                q_ref_output_ratio_goal             = 0;
+                wrench_hpf_cutoff_hz                = 20;
+                wrench_lpf_cutoff_hz                = 0.3;
+                wrench_hpf_gain                     = 1;
+                wrench_lpf_gain                     = 0.2;
+                ee_pos_rot_friction_coeff           << 0, 0; // 1, 0.1
+                floor_pd_gain                       << 10000, 500;
+                foot_horizontal_pd_gain             << 300, 30;
+                q_ref_pd_gain                       << 50, 0;
+                ex_ee_ref_wrench["rleg"]            = hrp::dvector6::Zero();
+                ex_ee_ref_wrench["lleg"]            = hrp::dvector6::Zero();
+                ex_ee_ref_wrench["rarm"]            = hrp::dvector6::Zero();
+                ex_ee_ref_wrench["larm"]            = hrp::dvector6::Zero();
             }
         } hcp;
 

From a45ac552bdacf6ea517e98e335d66246e9cb0cc9 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 13 Nov 2019 21:53:17 +0900
Subject: [PATCH 192/217] tune

---
 rtc/HapticController/HapticController.h | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index f83c40dc737..01a0981e240 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -152,11 +152,11 @@ class HapticController : public RTC::DataFlowComponentBase{
                 baselink_height_from_floor          = 1.5;// will be overwrited
                 dqAct_filter_cutoff_hz              = 500;// 10以下で確実に位相遅れによる振動
                 ee_vel_filter_cutoff_hz             = 500;// 10以下で確実に位相遅れによる振動
-                ex_gravity_compensation_ratio_lower = 0.9;
-                ex_gravity_compensation_ratio_upper = 1.1;
-                foot_min_distance                   = 0.3;
+                ex_gravity_compensation_ratio_lower = 1.1;
+                ex_gravity_compensation_ratio_upper = 0.9;
+                foot_min_distance                   = 0.25;
                 force_feedback_ratio                = 0.2;
-                gravity_compensation_ratio          = 0.9;
+                gravity_compensation_ratio          = 1.0;
                 q_friction_coeff                    = 0;
                 q_ref_output_ratio_goal             = 0;
                 wrench_hpf_cutoff_hz                = 20;

From a85545ad2213a4fed94bb406d25c834e02787d05 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 15 Nov 2019 14:37:24 +0900
Subject: [PATCH 193/217] add pose wrench to both side

---
 rtc/HapticController/HapticController.cpp     | 25 ++++++++++--------
 rtc/HapticController/HapticController.h       | 11 +++++---
 .../WholeBodyMasterSlave.cpp                  | 26 ++++++++++++-------
 .../WholeBodyMasterSlave.h                    | 11 +++++---
 4 files changed, 45 insertions(+), 28 deletions(-)

diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index e38ea1c2112..da77dd20f61 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -112,6 +112,8 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     tgt_names = ee_names;
     tgt_names.push_back("com");
     tgt_names.push_back("head");
+    tgt_names.push_back("rhand");
+    tgt_names.push_back("lhand");
     for ( int i=0; i<tgt_names.size(); i++) {
         std::string n = "master_"+tgt_names[i]+"_pose";
         m_masterTgtPosesOut[tgt_names[i]] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
@@ -119,14 +121,20 @@ RTC::ReturnCode_t HapticController::onInitialize(){
         RTC_INFO_STREAM(" registerOutPort " << n);
     }
 
-    //debug : This won't be send to slave
+    // sub usage
     for ( int i=0; i<ee_names.size(); i++) {
-        std::string n = "master_"+ee_names[i]+"_wrench_dbg";
+        std::string n = "master_"+ee_names[i]+"_wrench";
         m_masterEEWrenchesOut[ee_names[i]] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_masterEEWrenches[ee_names[i]]));
         registerOutPort(n.c_str(), *m_masterEEWrenchesOut[ee_names[i]]);
         m_masterEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
         RTC_INFO_STREAM(" registerOutPort " << n );
     }
+    for ( int i=0; i<tgt_names.size(); i++) {
+        std::string n = "slave_"+tgt_names[i]+"_pose";
+        m_slaveTgtPosesIn[tgt_names[i]] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_slaveTgtPoses[tgt_names[i]]));
+        registerInPort(n.c_str(), *m_slaveTgtPosesIn[tgt_names[i]]);
+        RTC_INFO_STREAM(" registerInPort " << n );
+    }
 
     m_tau.data = hrp::to_DoubleSeq(hrp::dvector::Zero(m_robot->numJoints()));
 
@@ -549,17 +557,12 @@ void HapticController::calcTorque(){
     }
 
 
-
+    m_masterTgtPoses["com"].data = hrp::to_Pose3D(hrp::Pose3(m_robot->calcCM(), hrp::Matrix33::Identity()));
+    for (int i=0; i<ee_names.size(); i++){
+        m_masterTgtPoses[ee_names[i]].data = hrp::to_Pose3D(ee_ikc_map[ee_names[i]].getCurrentTargetPose(m_robot));//四肢揃ってないと危険
+    }
     for (int i=0; i<tgt_names.size(); i++){
-        if(tgt_names[i] == "com"){
-            m_masterTgtPoses[tgt_names[i]].data = hrp::to_Pose3D(hrp::Pose3(m_robot->calcCM(), hrp::Matrix33::Identity()));
-        }else if(tgt_names[i] == "head"){
-            // dummy
-        }else{
-            m_masterTgtPoses[tgt_names[i]].data = hrp::to_Pose3D(ee_ikc_map[ee_names[i]].getCurrentTargetPose(m_robot));//四肢揃ってないと危険
-        }
         m_masterTgtPoses[tgt_names[i]].tm = m_qRef.tm;
-//        m_masterTgtPoses[tgt_names[i]].tm = m_qAct.tm;
         m_masterTgtPosesOut[tgt_names[i]]->write();
     }
 
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index 01a0981e240..a3254bde1cb 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -70,6 +70,10 @@ class HapticController : public RTC::DataFlowComponentBase{
         bool resumeHapticController();
         bool setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param);
         bool getParams(OpenHRP::HapticControllerService::HapticControllerParam& i_param);
+        typedef boost::shared_ptr<RTC::InPort   <RTC::TimedPose3D>      > ITP3_Ptr;
+        typedef boost::shared_ptr<RTC::InPort   <RTC::TimedDoubleSeq>   > ITDS_Ptr;
+        typedef boost::shared_ptr<RTC::OutPort  <RTC::TimedPose3D>      > OTP3_Ptr;
+        typedef boost::shared_ptr<RTC::OutPort  <RTC::TimedDoubleSeq>   > OTDS_Ptr;
 
     protected:
         RTC::TimedDoubleSeq m_qRef;
@@ -78,7 +82,6 @@ class HapticController : public RTC::DataFlowComponentBase{
         RTC::InPort<RTC::TimedDoubleSeq> m_qActIn;
         RTC::TimedDoubleSeq m_dqAct;
         RTC::InPort<RTC::TimedDoubleSeq> m_dqActIn;
-        typedef boost::shared_ptr<RTC::InPort<RTC::TimedDoubleSeq> > ITDS_Ptr;
         std::map<std::string, RTC::TimedDoubleSeq> m_slaveEEWrenches;
         std::map<std::string, ITDS_Ptr> m_slaveEEWrenchesIn;
         RTC::TimedDoubleSeq m_tau;
@@ -86,13 +89,13 @@ class HapticController : public RTC::DataFlowComponentBase{
         RTC::TimedPose3D m_teleopOdom;
         RTC::OutPort<RTC::TimedPose3D> m_teleopOdomOut;
         RTC::OutPort<RTC::TimedDoubleSeq> m_qOut;
-        typedef boost::shared_ptr<RTC::OutPort<RTC::TimedPose3D> > OTP3_Ptr;
         std::map<std::string, RTC::TimedPose3D> m_masterTgtPoses;
         std::map<std::string, OTP3_Ptr> m_masterTgtPosesOut;
-        //debug
-        typedef boost::shared_ptr<RTC::OutPort<RTC::TimedDoubleSeq> > OTDS_Ptr;
+        // sub usage
         std::map<std::string, RTC::TimedDoubleSeq> m_masterEEWrenches;
         std::map<std::string, OTDS_Ptr> m_masterEEWrenchesOut;
+        std::map<std::string, RTC::TimedPose3D> m_slaveTgtPoses;
+        std::map<std::string, ITP3_Ptr> m_slaveTgtPosesIn;
         RTC::TimedDoubleSeq m_debugData;
         RTC::OutPort<RTC::TimedDoubleSeq> m_debugDataOut;
         RTC::CorbaPort m_HapticControllerServicePort;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index f83b2cd07f4..870edd760ca 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -159,6 +159,12 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     to_enum["com"] =  com;
     to_enum["head"] = head;
 
+    tgt_names = ee_names;
+    tgt_names.push_back("com");
+    tgt_names.push_back("head");
+    tgt_names.push_back("rhand");
+    tgt_names.push_back("lhand");
+
     for ( int i=0; i<ee_names.size(); i++) {
         std::string n = "slave_"+ee_names[i]+"_wrench";
         m_slaveEEWrenchesOut[ee_names[i]] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee_names[i]]));
@@ -166,17 +172,12 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
         RTC_INFO_STREAM(" registerOutPort " << n);
     }
     for ( int i=0; i<ee_names.size(); i++) {
-        std::string n = "local_"+ee_names[i]+"_wrench";
-        m_localEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_localEEWrenches[ee_names[i]]));
-        registerInPort(n.c_str(), *m_localEEWrenchesIn[ee_names[i]]);
+        std::string n = "master_"+ee_names[i]+"_wrench";
+        m_masterEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_masterEEWrenches[ee_names[i]]));
+        registerInPort(n.c_str(), *m_masterEEWrenchesIn[ee_names[i]]);
         RTC_INFO_STREAM(" registerInPort " << n);
     }
-
-    tgt_names = ee_names;
-    tgt_names.push_back("com");
-    tgt_names.push_back("head");
-    tgt_names.push_back("rhand");
-    tgt_names.push_back("lhand");
+    ///////////////////
     for ( int i=0; i<tgt_names.size(); i++) {
         std::string n = "master_"+tgt_names[i]+"_pose";
         m_masterTgtPosesIn[tgt_names[i]] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
@@ -189,6 +190,13 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
         registerOutPort(n.c_str(), *m_slaveTgtPosesOut[tgt_names[i]]);
         RTC_INFO_STREAM(" registerOutPort " << n);
     }
+    //////////////
+    for ( int i=0; i<ee_names.size(); i++) {
+        std::string n = "local_"+ee_names[i]+"_wrench";
+        m_localEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_localEEWrenches[ee_names[i]]));
+        registerInPort(n.c_str(), *m_localEEWrenchesIn[ee_names[i]]);
+        RTC_INFO_STREAM(" registerInPort " << n);
+    }
 
     RTC_INFO_STREAM("onInitialize() OK");
     loop = 0;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 2799e32a8f6..bf8bd77bd4d 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -91,21 +91,24 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         RTC::InPort<RTC::TimedPoint3D> m_zmpIn;
         RTC::TimedDoubleSeq m_optionalData;
         RTC::InPort<RTC::TimedDoubleSeq> m_optionalDataIn;
+        typedef boost::shared_ptr<RTC::InPort   <RTC::TimedPose3D>      > ITP3_Ptr;
+        typedef boost::shared_ptr<RTC::InPort   <RTC::TimedDoubleSeq>   > ITDS_Ptr;
+        typedef boost::shared_ptr<RTC::OutPort  <RTC::TimedPose3D>      > OTP3_Ptr;
+        typedef boost::shared_ptr<RTC::OutPort  <RTC::TimedDoubleSeq>   > OTDS_Ptr;
 
         std::map<std::string, RTC::TimedPose3D> m_masterTgtPoses;
-        typedef boost::shared_ptr<RTC::InPort<RTC::TimedPose3D> > ITP3_Ptr;
         std::map<std::string, ITP3_Ptr> m_masterTgtPosesIn;
 
+        std::map<std::string, RTC::TimedDoubleSeq> m_masterEEWrenches;
+        std::map<std::string, ITDS_Ptr> m_masterEEWrenchesIn;
+
         std::map<std::string, RTC::TimedDoubleSeq> m_slaveEEWrenches;
-        typedef boost::shared_ptr<RTC::OutPort<RTC::TimedDoubleSeq> > OTDS_Ptr;
         std::map<std::string, OTDS_Ptr> m_slaveEEWrenchesOut;
 
         std::map<std::string, RTC::TimedPose3D> m_slaveTgtPoses;
-        typedef boost::shared_ptr<RTC::OutPort<RTC::TimedPose3D> > OTP3_Ptr;
         std::map<std::string, OTP3_Ptr> m_slaveTgtPosesOut;
 
         std::map<std::string, RTC::TimedDoubleSeq> m_localEEWrenches;
-        typedef boost::shared_ptr<RTC::InPort<RTC::TimedDoubleSeq> > ITDS_Ptr;
         std::map<std::string, ITDS_Ptr> m_localEEWrenchesIn;
 
         RTC::Time m_delayCheckPacket;

From 3ae33120688998e7bf6f128b613d414b08499e4c Mon Sep 17 00:00:00 2001
From: leus <leus@example.com>
Date: Fri, 15 Nov 2019 15:26:58 +0900
Subject: [PATCH 194/217] remove debug message

---
 rtc/HapticController/HapticController.cpp | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index da77dd20f61..ce0da9fe8e6 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -478,11 +478,6 @@ void HapticController::calcTorque(){
             m_debugData.data = hrp::to_DoubleSeq((hrp::dvector(6+6+6+6)<<wrench_raw,wrench_shaped[ee_names[i]],w_hpf,w_lpf).finished());
             m_debugData.tm = m_qRef.tm;
             m_debugDataOut.write();
-            if(loop%1000==0){
-                dbgv(ee_vel_filtered["rarm"]);
-                dbgv(hcp.ee_pos_rot_friction_coeff);
-                dbg(hcp.ee_vel_filter_cutoff_hz);
-            }
         }
 
         hrp::JointPath jp(m_robot->rootLink(), m_robot->link(ee_ikc_map[ee_names[i]].target_link_name));
@@ -519,7 +514,6 @@ void HapticController::calcTorque(){
          LIMIT_MINMAX(friction_tq(i), -10, 10);
          m_robot->joint(i)->u += friction_tq(i);
         }
-        if(loop%1000==0)dbgv(friction_tq);
     }
 
 

From e2c8cc411a59d7f0fa73443016b45672319a9f2c Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 6 Dec 2019 17:00:12 +0900
Subject: [PATCH 195/217] add auto_floor_h_mode

---
 idl/WholeBodyMasterSlaveService.idl           |  3 +
 .../WholeBodyMasterSlave.cpp                  | 12 +++-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 64 +++++++++++++++----
 3 files changed, 65 insertions(+), 14 deletions(-)

diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
index 8df144c903a..5eb4230b1b0 100644
--- a/idl/WholeBodyMasterSlaveService.idl
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -19,8 +19,11 @@ module OpenHRP
     struct WholeBodyMasterSlaveParam
     {
         boolean auto_com_mode;
+        boolean auto_foor_h_mode;
         double additional_double_support_time;
         double auto_com_foot_move_detect_height;
+        double auto_floor_h_detect_fz;
+        double auto_floor_h_reset_fz;
         double base_to_hand_min_distance;
         double capture_point_extend_ratio;
         double com_filter_cutoff_hz;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 870edd760ca..944b8615f06 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -246,6 +246,10 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if(m_optionalDataIn.isNew()             ){ m_optionalDataIn.read(); }
     if(m_delayCheckPacketInboundIn.isNew()  ){ m_delayCheckPacketInboundIn.read(); m_delayCheckPacketOutboundOut.write();}
     for(int i=0; i<ee_names.size(); i++){ if(m_localEEWrenchesIn[ee_names[i]]->isNew()){ m_localEEWrenchesIn[ee_names[i]]->read(); } }
+    wbms->act_rs.act_foot_wrench[0] = hrp::to_dvector(m_localEEWrenches["rleg"].data);
+    wbms->act_rs.act_foot_wrench[1] = hrp::to_dvector(m_localEEWrenches["lleg"].data);
+    wbms->act_rs.act_foot_pose[0] = ee_ikc_map["rleg"].getCurrentTargetPose(m_robot_vsafe);
+    wbms->act_rs.act_foot_pose[1] = ee_ikc_map["lleg"].getCurrentTargetPose(m_robot_vsafe);
 
     // button func
     hrp::dvector ex_data;
@@ -323,8 +327,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 //        ref_zmp = ref_zmp_filter.passFilter(ref_zmp);
         com_old_old = com_old;
         com_old = com;
-        wbms->act_rs.com = com;
-        wbms->act_rs.zmp = ref_zmp;
+        wbms->act_rs.ref_com = com;
+        wbms->act_rs.ref_zmp = ref_zmp;
         wbms->act_rs.st_zmp = m_robot_vsafe->rootLink()->p + m_robot_vsafe->rootLink()->R * rel_act_zmp;
 
 //            if(wbms->rp_ref_out.tgt[rf].is_contact && !wbms->rp_ref_out.tgt[lf].is_contact){
@@ -747,6 +751,8 @@ bool WholeBodyMasterSlave::setParams(const OpenHRP::WholeBodyMasterSlaveService:
     wbms->wp.auto_com_mode                      = i_param.auto_com_mode;
     wbms->wp.additional_double_support_time     = i_param.additional_double_support_time;
     wbms->wp.auto_com_foot_move_detect_height   = i_param.auto_com_foot_move_detect_height;
+    wbms->wp.auto_floor_h_detect_fz             = i_param.auto_floor_h_detect_fz;
+    wbms->wp.auto_floor_h_reset_fz              = i_param.auto_floor_h_reset_fz;
     wbms->wp.base_to_hand_min_distance          = i_param.base_to_hand_min_distance;
     wbms->wp.capture_point_extend_ratio         = i_param.capture_point_extend_ratio;
     wbms->wp.com_filter_cutoff_hz               = i_param.com_filter_cutoff_hz;
@@ -770,6 +776,8 @@ bool WholeBodyMasterSlave::getParams(OpenHRP::WholeBodyMasterSlaveService::Whole
     i_param.auto_com_mode                       = wbms->wp.auto_com_mode;
     i_param.additional_double_support_time      = wbms->wp.additional_double_support_time;
     i_param.auto_com_foot_move_detect_height    = wbms->wp.auto_com_foot_move_detect_height;
+    i_param.auto_floor_h_detect_fz              = wbms->wp.auto_floor_h_detect_fz;
+    i_param.auto_floor_h_reset_fz               = wbms->wp.auto_floor_h_reset_fz;
     i_param.base_to_hand_min_distance           = wbms->wp.base_to_hand_min_distance;
     i_param.capture_point_extend_ratio          = wbms->wp.capture_point_extend_ratio;
     i_param.com_filter_cutoff_hz                = wbms->wp.com_filter_cutoff_hz;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index d86b24e41ff..bb94f88736c 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -58,15 +58,16 @@ class LIP_model{
 class PoseTGT{
     public:
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-        hrp::Pose3 abs, offs;
+        hrp::Pose3 abs, offs, cnt;
         hrp::dvector6 w;
         bool go_contact;
         PoseTGT(){ reset(); }
         ~PoseTGT(){}
-        void reset(){ abs.reset(); offs.reset(); w.fill(0); go_contact = false;}
+        void reset(){ abs.reset(); offs.reset(); cnt.reset(); w.fill(0); go_contact = false;}
         bool is_contact(){
             const double contact_threshold = 0.005;
-            return ((abs.p(Z) - offs.p(Z)) < contact_threshold);
+            // return ((abs.p(Z) - offs.p(Z)) < contact_threshold);
+            return ((abs.p(Z) - cnt.p(Z)) < contact_threshold);
         }
 };
 
@@ -360,8 +361,11 @@ class WBMSCore{
         class WBMSParams {
             public:
                 bool auto_com_mode;
+                bool auto_foor_h_mode;
                 double additional_double_support_time;
                 double auto_com_foot_move_detect_height;
+                double auto_floor_h_detect_fz;
+                double auto_floor_h_reset_fz;
                 double base_to_hand_min_distance;
                 double capture_point_extend_ratio;
                 double com_filter_cutoff_hz;
@@ -378,11 +382,14 @@ class WBMSCore{
                 std::vector<std::string> use_targets;
             WBMSParams(){
                 auto_com_mode                       = false;
+                auto_foor_h_mode                         = true;
                 additional_double_support_time      = 0.5;
                 auto_com_foot_move_detect_height    = 0.03;
+                auto_floor_h_detect_fz              = 50;
+                auto_floor_h_reset_fz               = 30;
                 base_to_hand_min_distance           = 0.5;
                 capture_point_extend_ratio          = 1.0;
-                com_filter_cutoff_hz                = 1.5;
+                com_filter_cutoff_hz                = 1.2;
                 foot_collision_avoidance_distance   = 0.16;
                 human_to_robot_ratio                = 1.0;//human 1.1m vs jaxon 1.06m
                 max_double_support_width            = 0.4;
@@ -395,7 +402,9 @@ class WBMSCore{
             }
         } wp;
         struct ActualRobotState {
-            hrp::Vector3 com, zmp, st_zmp;
+            hrp::Vector3 ref_com, ref_zmp, st_zmp;
+            hrp::dvector6 act_foot_wrench[LR];
+            hrp::Pose3 act_foot_pose[LR];
         } act_rs;
 
         WBMSCore(const double& dt){
@@ -441,10 +450,13 @@ class WBMSCore{
             const int human_l_names[4] = {rf,lf,rh,lh};
             for(int i=0;i<4;i++){//HumanSynchronizerの初期姿勢オフセットをセット
                 if(_ee_ikc_map.count(robot_l_names[i])){
-                    rp_ref_out.tgt[human_l_names[i]].abs = rp_ref_out.tgt[human_l_names[i]].offs = _ee_ikc_map[robot_l_names[i]].getCurrentTargetPose(robot_in);//          fik_in->getEndEffectorPos(robot_l_names[i]);
+                    rp_ref_out.tgt[human_l_names[i]].abs =
+                    rp_ref_out.tgt[human_l_names[i]].offs = 
+                    rp_ref_out.tgt[human_l_names[i]].cnt = 
+                    _ee_ikc_map[robot_l_names[i]].getCurrentTargetPose(robot_in);//          fik_in->getEndEffectorPos(robot_l_names[i]);
                 }
             }
-            rp_ref_out.tgt[com].offs.p = act_rs.com = act_rs.zmp = act_rs.st_zmp = robot_in->calcCM();
+            rp_ref_out.tgt[com].offs.p = act_rs.ref_com = act_rs.ref_zmp = act_rs.st_zmp = robot_in->calcCM();
             com_CP_ref_old = rp_ref_out.tgt[com].offs.p;
             rp_ref_out.tgt[com].offs.R = robot_in->rootLink()->R;
             rp_ref_out.tgt[zmp].offs.p.head(XY) = rp_ref_out.tgt[com].offs.p.head(XY);
@@ -495,7 +507,7 @@ class WBMSCore{
         }
 
     private:
-        void convertHumanToRobot(const HumanPose& in, HumanPose& out){//結局初期指定からの移動量(=Rel)で計算をcp_decしてゆく
+        void convertHumanToRobot(const HumanPose& in, HumanPose& out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
             //      out = in;//ダメゼッタイ
             for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
                 out.tgt[l[i]].abs.p = wp.human_to_robot_ratio * (in.tgt[l[i]].abs.p - in.tgt[l[i]].offs.p) + out.tgt[l[i]].offs.p;
@@ -523,11 +535,11 @@ class WBMSCore{
                 to_opposite_foot[lr] = old.foot(OPPOSITE(lr)).abs.p.head(XY) - old.foot(lr).abs.p.head(XY);
                 if(to_opposite_foot[lr].norm() < 1e-3){ std::cerr << "to_opposite_foot[lr].norm() < 1e-3 :" << to_opposite_foot[lr].transpose()<<std::endl; }
                 ///// lock by ref_zmp
-                ref_zmp_from_foot[lr] = act_rs.zmp.head(XY) - old.foot(lr).abs.p.head(XY);
+                ref_zmp_from_foot[lr] = act_rs.ref_zmp.head(XY) - old.foot(lr).abs.p.head(XY);
                 out.foot(OPPOSITE(lr)).go_contact |= ( ref_zmp_from_foot[lr].dot(to_opposite_foot[lr]) / to_opposite_foot[lr].norm() > wp.single_foot_zmp_safety_distance );
                 ///// lock by acts_zmp
                 act_zmp_from_foot[lr] = act_rs.st_zmp.head(XY) - old.foot(lr).abs.p.head(XY);
-                if( act_zmp_from_foot[lr].dot(to_opposite_foot[lr]) / to_opposite_foot[lr].norm() > wp.single_foot_zmp_safety_distance ){
+                if( act_zmp_from_foot[lr].dot(to_opposite_foot[lr]) / to_opposite_foot[lr].norm() > wp.single_foot_zmp_safety_distance*9999 ){
                     zmp_force_go_contact_count[OPPOSITE(lr)] = 1;
                 }else if(zmp_force_go_contact_count[OPPOSITE(lr)] > 0 && zmp_force_go_contact_count[OPPOSITE(lr)] < HZ * wp.additional_double_support_time ){
                     zmp_force_go_contact_count[OPPOSITE(lr)]++;
@@ -562,10 +574,28 @@ class WBMSCore{
             }
         }
         void setFootContactPoseByGoContact(const HumanPose& old, HumanPose& out){
+            static bool is_locked[LR] = {false};
+            static int cnt_for_clear[LR] ={0};
             for(int lr=0; lr<LR; lr++){
+                if(wp.auto_foor_h_mode){
+                    // judge act contact height
+                    if(!is_locked[lr] && fabs(act_rs.act_foot_wrench[lr](fz)) > wp.auto_floor_h_detect_fz){
+                        out.foot(lr).cnt.p(Z) = act_rs.act_foot_pose[lr].p(Z);
+                        is_locked[lr] = true;
+                    }
+                    if(is_locked[lr] && fabs(act_rs.act_foot_wrench[lr](fz)) < wp.auto_floor_h_reset_fz){
+                        if(cnt_for_clear[lr]++ > 1000){
+                            out.foot(lr).cnt.p(Z) = out.foot(lr).offs.p(Z);
+                            is_locked[lr] = false;
+                            cnt_for_clear[lr] = 0;
+                        }
+                    }
+                }
+
                 if(out.foot(lr).go_contact){
                     out.foot(lr).abs.p.head(XY) = old.foot(lr).abs.p.head(XY);
-                    out.foot(lr).abs.p(Z) = out.foot(lr).offs.p(Z);
+                    // out.foot(lr).abs.p(Z) = out.foot(lr).offs.p(Z);
+                    out.foot(lr).abs.p(Z) = out.foot(lr).cnt.p(Z);
                     out.foot(lr).abs.setRpy(0, 0, old.foot(lr).abs.rpy()(y));
                     /////着地時に足を広げすぎないよう制限
                     const hrp::Vector2 support_to_swing = out.foot(lr).abs.p.head(XY) - old.foot(OPPOSITE(lr)).abs.p.head(XY);
@@ -573,7 +603,17 @@ class WBMSCore{
                         out.foot(lr).abs.p.head(XY) = old.foot(OPPOSITE(lr)).abs.p.head(XY) + support_to_swing.normalized() * wp.max_double_support_width;
                     }
                 }else{
-                    LIMIT_MIN( out.foot(lr).abs.p(Z), out.foot(lr).offs.p(Z)+wp.swing_foot_height_offset);
+                    out.foot(lr).cnt.p.head(XY) = out.foot(lr).abs.p.head(XY);  
+                    out.foot(lr).cnt.R = hrp::rotFromRpy(0, 0, hrp::rpyFromRot(out.foot(lr).abs.R)(y));  
+                    // LIMIT_MIN( out.foot(lr).abs.p(Z), out.foot(lr).offs.p(Z)+wp.swing_foot_height_offset);
+                    LIMIT_MIN( out.foot(lr).abs.p(Z), out.foot(lr).cnt.p(Z)+wp.swing_foot_height_offset);
+                }
+                if(loop%500){
+                    dbg(lr);
+                    dbg(is_locked[lr]);
+                    dbg(out.foot(lr).cnt.p(Z));
+                    dbg(out.foot(lr).abs.p(Z));
+                    dbg(act_rs.act_foot_pose[lr].p(Z));
                 }
             }
         }

From f555b0b580706ae5e3fea586a842a55b6ea34e0e Mon Sep 17 00:00:00 2001
From: JAXONRED <jaxonred@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 6 Dec 2019 17:03:50 +0900
Subject: [PATCH 196/217] use act base pose for ee pos calc

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 870edd760ca..78a338977ae 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -371,6 +371,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     to_sname["larm"] = "lhsensor";
     to_sname["rarm"] = "rhsensor";
     hrp::setQAll(m_robot_act, hrp::to_dvector(m_qAct.data));
+    m_robot_act->rootLink()->p = hrp::to_Vector3(m_basePos.data);
+    m_robot_act->rootLink()->R = hrp::rotFromRpy(hrp::to_Vector3(m_baseRpy.data));
     m_robot_act->calcForwardKinematics();
     for(int i=0;i<ee_names.size();i++){
         hrp::ForceSensor* sensor = m_robot_act->sensor<hrp::ForceSensor>(to_sname[ee_names[i]]);

From 88cc968803c05502da57cd55633a4133aa6dd699 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 9 Dec 2019 18:21:28 +0900
Subject: [PATCH 197/217] up c++11

---
 .../FullbodyInverseKinematicsSolver.h         |  4 +-
 rtc/WholeBodyMasterSlave/CMakeLists.txt       |  2 +
 .../WholeBodyMasterSlave.cpp                  | 78 ++++++++++---------
 .../WholeBodyMasterSlave.h                    |  4 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          |  2 +-
 5 files changed, 48 insertions(+), 42 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index b27bb096974..cdd04729022 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -211,7 +211,7 @@ class FullbodyInverseKinematicsSolver{
                     jlim_avoid_weight = DBL_MAX;
                 } else {
                     jlim_avoid_weight = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) / (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) );
-                    if (isnan(jlim_avoid_weight)) jlim_avoid_weight = 0;
+                    if (std::isnan(jlim_avoid_weight)) jlim_avoid_weight = 0;
                 }
                 if (( jlim_avoid_weight - jlim_avoid_weight_old(j) ) >= 0 ) { // add weight only if q approaching to the limit
                     dq_weight_all_jlim(j) += jlim_avoid_weight;
@@ -266,7 +266,7 @@ class FullbodyInverseKinematicsSolver{
             }
 #endif
             // update joint angles
-            for(int i=0;i<dq_all.rows();i++){ if( isnan(dq_all(i)) || isinf(dq_all(i)) ){ std::cerr <<"[FullbodyIK] ERROR nan/inf is found" << std::endl; return;} }
+            for(int i=0;i<dq_all.rows();i++){ if( std::isnan(dq_all(i)) || std::isinf(dq_all(i)) ){ std::cerr <<"[FullbodyIK] ERROR nan/inf is found" << std::endl; return;} }
             dq_all = q_select_mat.transpose() * dq_all;
             for(int i=0;i<J_DOF;i++){
                 m_robot->joint(i)->q += dq_all(i);
diff --git a/rtc/WholeBodyMasterSlave/CMakeLists.txt b/rtc/WholeBodyMasterSlave/CMakeLists.txt
index 9d6e063df99..7a6087fd119 100644
--- a/rtc/WholeBodyMasterSlave/CMakeLists.txt
+++ b/rtc/WholeBodyMasterSlave/CMakeLists.txt
@@ -1,5 +1,7 @@
 set(comp_sources WholeBodyMasterSlave.cpp WholeBodyMasterSlaveService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp ../TorqueFilter/IIRFilter.cpp)
 set(libs ${OpenCV_LIBRARIES} hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub)
+#set(CMAKE_CXX_STANDARD, 11)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 add_library(WholeBodyMasterSlave SHARED ${comp_sources})
 target_link_libraries(WholeBodyMasterSlave ${libs})
 set_target_properties(WholeBodyMasterSlave PROPERTIES PREFIX "")
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 944b8615f06..f1fd4c1e3f7 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -198,6 +198,11 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
         RTC_INFO_STREAM(" registerInPort " << n);
     }
 
+    for(auto een : ee_names){
+        ee_f_filter[een].resize(XYZ);
+        ee_f_filter[een].setParameter(1, 1/m_dt, Q_BUTTERWORTH);
+    }
+
     RTC_INFO_STREAM("onInitialize() OK");
     loop = 0;
     return RTC::RTC_OK;
@@ -251,6 +256,39 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     wbms->act_rs.act_foot_pose[0] = ee_ikc_map["rleg"].getCurrentTargetPose(m_robot_vsafe);
     wbms->act_rs.act_foot_pose[1] = ee_ikc_map["lleg"].getCurrentTargetPose(m_robot_vsafe);
 
+    // calc actual state
+    std::map<std::string, std::string> to_sname{{"lleg", "lfsensor"}, {"rleg", "rfsensor"}, {"larm", "lhsensor"}, {"rarm", "rhsensor"}};
+    hrp::setQAll(m_robot_act, hrp::to_dvector(m_qAct.data));
+    m_robot_act->calcForwardKinematics();
+    for(auto een : ee_names){
+        hrp::ForceSensor* sensor = m_robot_act->sensor<hrp::ForceSensor>(to_sname[een]);
+        hrp::Matrix33 sensorR_wld = sensor->link->R * sensor->localR;
+        hrp::Matrix33 sensorR_from_base = m_robot_act->rootLink()->R.transpose() * sensorR_wld;
+        const hrp::Vector3 f_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[een].data).head(3);
+        const hrp::Vector3 t_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[een].data).tail(3);
+        const hrp::Vector3 sensor_to_ee_vec_wld = ee_ikc_map[een].getCurrentTargetPos(m_robot_act) - sensor->link->p;
+        const hrp::Vector3 f_ee_wld = f_sensor_wld;
+        const hrp::Vector3 t_ee_wld = t_sensor_wld - sensor_to_ee_vec_wld.cross(f_sensor_wld);
+        m_slaveEEWrenches[een].data = hrp::to_DoubleSeq( (hrp::dvector6()<<f_ee_wld,t_ee_wld).finished());
+        m_slaveEEWrenches[een].tm = m_qRef.tm;
+        m_slaveEEWrenchesOut[een]->write();
+        m_slaveTgtPoses[een].data = hrp::to_Pose3D(ee_ikc_map[een].getCurrentTargetPose(m_robot_act));
+        m_slaveTgtPoses[een].tm = m_qRef.tm;
+        m_slaveTgtPosesOut[een]->write();
+    }
+
+    static_balancing_com_offset.fill(0);
+    for(std::string een : {"larm","rarm"}){
+        const hrp::Vector3 use_f = hrp::Vector3::UnitZ() * m_slaveEEWrenches[een].data[Z];
+        hrp::Vector3 use_f_filtered = ee_f_filter[een].passFilter(use_f);
+        const hrp::Vector3 ee_pos_from_com = ee_ikc_map[een].getCurrentTargetPos(m_robot_vsafe) - wbms->rp_ref_out.tgt[com].abs.p;
+        static_balancing_com_offset.head(XY) += - ee_pos_from_com.head(XY) * use_f_filtered(Z) / (-m_robot_vsafe->totalMass() * G);
+    }
+    if(loop%500==0)dbgv(static_balancing_com_offset);
+    m_slaveTgtPoses["com"].data = hrp::to_Pose3D( (hrp::dvector6()<<m_robot_act->calcCM(),0,0,0).finished());
+    m_slaveTgtPoses["com"].tm = m_qRef.tm;
+    m_slaveTgtPosesOut["com"]->write();
+
     // button func
     hrp::dvector ex_data;
     std::vector<std::string> ex_data_index;
@@ -323,6 +361,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         static hrp::Vector3 com_old_old = com_old;
         hrp::Vector3 com_acc = (com - 2*com_old + com_old_old)/(m_dt*m_dt);
         hrp::Vector3 ref_zmp; ref_zmp << com.head(XY)-(com(Z)/G)*com_acc.head(XY), 0;
+        ref_zmp.head(XY) -= static_balancing_com_offset.head(XY);
 //        if(mode.isInitialize()){ ref_zmp_filter.reset(ref_zmp); }
 //        ref_zmp = ref_zmp_filter.passFilter(ref_zmp);
         com_old_old = com_old;
@@ -331,15 +370,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         wbms->act_rs.ref_zmp = ref_zmp;
         wbms->act_rs.st_zmp = m_robot_vsafe->rootLink()->p + m_robot_vsafe->rootLink()->R * rel_act_zmp;
 
-//            if(wbms->rp_ref_out.tgt[rf].is_contact && !wbms->rp_ref_out.tgt[lf].is_contact){
-//                ref_zmp.head(XY) = m_robot_vsafe->link(ee_ikc_map["rleg"].target_link_name)->p.head(XY);
-////                ref_zmp(X) + 0.05;
-//            }
-//            if(wbms->rp_ref_out.tgt[lf].is_contact && !wbms->rp_ref_out.tgt[rf].is_contact){
-//                ref_zmp.head(XY) = m_robot_vsafe->link(ee_ikc_map["lleg"].target_link_name)->p.head(XY);
-////                ref_zmp(X) + 0.05;
-//            }
-
         // qRef
         for (int i = 0; i < m_qRef.data.length(); i++ ){
           if(has(wbms->wp.use_joints, fik->m_robot->joint(i)->name)){
@@ -369,34 +399,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     wbms->baselinkpose.R = fik->m_robot->rootLink()->R;
 
 
-    std::map<std::string, std::string> to_sname;
-    to_sname["lleg"] = "lfsensor";
-    to_sname["rleg"] = "rfsensor";
-    to_sname["larm"] = "lhsensor";
-    to_sname["rarm"] = "rhsensor";
-    hrp::setQAll(m_robot_act, hrp::to_dvector(m_qAct.data));
-    m_robot_act->calcForwardKinematics();
-    for(int i=0;i<ee_names.size();i++){
-        hrp::ForceSensor* sensor = m_robot_act->sensor<hrp::ForceSensor>(to_sname[ee_names[i]]);
-        hrp::Matrix33 sensorR_wld = sensor->link->R * sensor->localR;
-        hrp::Matrix33 sensorR_from_base = m_robot_act->rootLink()->R.transpose() * sensorR_wld;
-        const hrp::Vector3 f_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee_names[i]].data).head(3);
-        const hrp::Vector3 t_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee_names[i]].data).tail(3);
-
-        const hrp::Vector3 sensor_to_ee_vec_wld = ee_ikc_map[ee_names[i]].getCurrentTargetPos(m_robot_act) - sensor->link->p;
-
-        const hrp::Vector3 f_ee_wld = f_sensor_wld;
-        const hrp::Vector3 t_ee_wld = t_sensor_wld - sensor_to_ee_vec_wld.cross(f_sensor_wld);
-        m_slaveEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq( (hrp::dvector6()<<f_ee_wld,t_ee_wld).finished());
-        m_slaveEEWrenches[ee_names[i]].tm = m_qRef.tm;
-        m_slaveEEWrenchesOut[ee_names[i]]->write();
-        m_slaveTgtPoses[ee_names[i]].data = hrp::to_Pose3D(ee_ikc_map[ee_names[i]].getCurrentTargetPose(m_robot_act));
-        m_slaveTgtPoses[ee_names[i]].tm = m_qRef.tm;
-        m_slaveTgtPosesOut[ee_names[i]]->write();
-    }
-    m_slaveTgtPoses["com"].data = hrp::to_Pose3D( (hrp::dvector6()<<m_robot_act->calcCM(),0,0,0).finished());
-    m_slaveTgtPoses["com"].tm = m_qRef.tm;
-    m_slaveTgtPosesOut["com"]->write();
     // write
     m_qOut.write();
     m_basePosOut.write();
@@ -599,7 +601,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         tmp.target_link_name = "COM";
         tmp.localPos = hrp::Vector3::Zero();
         tmp.localR = hrp::Matrix33::Identity();
-        tmp.targetPos = com_ref.p;// COM height will not be constraint
+        tmp.targetPos = com_ref.p + static_balancing_com_offset;// COM height will not be constraint
         tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
         tmp.constraint_weight << 3,3,0.01,0,0,0;
         ikc_list.push_back(tmp);
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index bf8bd77bd4d..9b8a193aa8d 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -160,7 +160,7 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
 
         hrp::InvDynStateBuffer idsb;
         BiquadIIRFilterVec ref_zmp_filter;
-        IIRFilter ref_ee_vel_v_filter;
+        std::map<std::string, BiquadIIRFilterVec> ee_f_filter;
 
         HumanPose raw_pose;
 
@@ -178,6 +178,8 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         struct timespec startT, endT;
         std::string time_report_str;
 
+        hrp::Vector3 static_balancing_com_offset;
+
         std::vector<std::string> ee_names;
         std::vector<std::string> tgt_names;
 
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index bb94f88736c..4bdcdf33494 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -608,7 +608,7 @@ class WBMSCore{
                     // LIMIT_MIN( out.foot(lr).abs.p(Z), out.foot(lr).offs.p(Z)+wp.swing_foot_height_offset);
                     LIMIT_MIN( out.foot(lr).abs.p(Z), out.foot(lr).cnt.p(Z)+wp.swing_foot_height_offset);
                 }
-                if(loop%500){
+                if(loop%500==0){
                     dbg(lr);
                     dbg(is_locked[lr]);
                     dbg(out.foot(lr).cnt.p(Z));

From ee30bb629aa21ce947e7eaadc7d7fd74e06d7f64 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 9 Dec 2019 18:28:58 +0900
Subject: [PATCH 198/217] fix

---
 .../WholeBodyMasterSlave.cpp                  | 31 -------------------
 1 file changed, 31 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 0cb2b497722..536207840f9 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -398,37 +398,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     wbms->baselinkpose.p = fik->m_robot->rootLink()->p;
     wbms->baselinkpose.R = fik->m_robot->rootLink()->R;
 
-
-    std::map<std::string, std::string> to_sname;
-    to_sname["lleg"] = "lfsensor";
-    to_sname["rleg"] = "rfsensor";
-    to_sname["larm"] = "lhsensor";
-    to_sname["rarm"] = "rhsensor";
-    hrp::setQAll(m_robot_act, hrp::to_dvector(m_qAct.data));
-    m_robot_act->rootLink()->p = hrp::to_Vector3(m_basePos.data);
-    m_robot_act->rootLink()->R = hrp::rotFromRpy(hrp::to_Vector3(m_baseRpy.data));
-    m_robot_act->calcForwardKinematics();
-    for(int i=0;i<ee_names.size();i++){
-        hrp::ForceSensor* sensor = m_robot_act->sensor<hrp::ForceSensor>(to_sname[ee_names[i]]);
-        hrp::Matrix33 sensorR_wld = sensor->link->R * sensor->localR;
-        hrp::Matrix33 sensorR_from_base = m_robot_act->rootLink()->R.transpose() * sensorR_wld;
-        const hrp::Vector3 f_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee_names[i]].data).head(3);
-        const hrp::Vector3 t_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee_names[i]].data).tail(3);
-
-        const hrp::Vector3 sensor_to_ee_vec_wld = ee_ikc_map[ee_names[i]].getCurrentTargetPos(m_robot_act) - sensor->link->p;
-
-        const hrp::Vector3 f_ee_wld = f_sensor_wld;
-        const hrp::Vector3 t_ee_wld = t_sensor_wld - sensor_to_ee_vec_wld.cross(f_sensor_wld);
-        m_slaveEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq( (hrp::dvector6()<<f_ee_wld,t_ee_wld).finished());
-        m_slaveEEWrenches[ee_names[i]].tm = m_qRef.tm;
-        m_slaveEEWrenchesOut[ee_names[i]]->write();
-        m_slaveTgtPoses[ee_names[i]].data = hrp::to_Pose3D(ee_ikc_map[ee_names[i]].getCurrentTargetPose(m_robot_act));
-        m_slaveTgtPoses[ee_names[i]].tm = m_qRef.tm;
-        m_slaveTgtPosesOut[ee_names[i]]->write();
-    }
-    m_slaveTgtPoses["com"].data = hrp::to_Pose3D( (hrp::dvector6()<<m_robot_act->calcCM(),0,0,0).finished());
-    m_slaveTgtPoses["com"].tm = m_qRef.tm;
-    m_slaveTgtPosesOut["com"]->write();
     // write
     m_qOut.write();
     m_basePosOut.write();

From 719c06d6b90674ddbd21cc183e0502a4ef5b651b Mon Sep 17 00:00:00 2001
From: leus <leus@example.com>
Date: Mon, 16 Dec 2019 14:30:11 +0900
Subject: [PATCH 199/217] up

---
 idl/HapticControllerService.idl           |   3 +-
 rtc/HapticController/CMakeLists.txt       |   1 +
 rtc/HapticController/HapticController.cpp | 308 ++++++++++------------
 rtc/HapticController/HapticController.h   |  17 +-
 4 files changed, 158 insertions(+), 171 deletions(-)

diff --git a/idl/HapticControllerService.idl b/idl/HapticControllerService.idl
index 0f2dc182281..1635aeae3cf 100644
--- a/idl/HapticControllerService.idl
+++ b/idl/HapticControllerService.idl
@@ -29,7 +29,7 @@ module OpenHRP
         double force_feedback_ratio;
         double gravity_compensation_ratio;
         double q_friction_coeff;
-        double q_ref_output_ratio_goal;
+        double q_ref_max_torque_ratio;
         double wrench_hpf_cutoff_hz;
         double wrench_lpf_cutoff_hz;
         double wrench_hpf_gain;
@@ -37,6 +37,7 @@ module OpenHRP
         DblSequence2 ee_pos_rot_friction_coeff;
         DblSequence2 floor_pd_gain;
         DblSequence2 foot_horizontal_pd_gain;
+        DblSequence2 force_feedback_limit_ft;
         DblSequence2 q_ref_pd_gain;
         DblSeqSeq6   ex_ee_ref_wrench;
     };
diff --git a/rtc/HapticController/CMakeLists.txt b/rtc/HapticController/CMakeLists.txt
index 09861c5e331..b5a203ecc74 100644
--- a/rtc/HapticController/CMakeLists.txt
+++ b/rtc/HapticController/CMakeLists.txt
@@ -1,5 +1,6 @@
 set(comp_sources HapticController.cpp HapticControllerService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp ../TorqueFilter/IIRFilter.cpp)
 set(libs ${OpenCV_LIBRARIES} hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 add_library(HapticController SHARED ${comp_sources})
 target_link_libraries(HapticController ${libs})
 set_target_properties(HapticController PROPERTIES PREFIX "")
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index ce0da9fe8e6..8692ae25c13 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -89,23 +89,23 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     dqAct_filter = BiquadIIRFilterVec(m_robot->numJoints());
     dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
     dqAct_filter.reset(0);
-    for ( int i=0; i<ee_names.size(); i++) {
-        ee_vel_filter[ee_names[i]] = BiquadIIRFilterVec(6);
-        ee_vel_filter[ee_names[i]].setParameter(hcp.ee_vel_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        ee_vel_filter[ee_names[i]].reset(0);
-        wrench_lpf_for_hpf[ee_names[i]] = BiquadIIRFilterVec(6);
-        wrench_lpf_for_hpf[ee_names[i]].setParameter(hcp.wrench_hpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        wrench_lpf_for_hpf[ee_names[i]].reset(0);
-        wrench_lpf[ee_names[i]] = BiquadIIRFilterVec(6);
-        wrench_lpf[ee_names[i]].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        wrench_lpf[ee_names[i]].reset(0);
+    for (auto ee : ee_names){
+        ee_vel_filter[ee] = BiquadIIRFilterVec(6);
+        ee_vel_filter[ee].setParameter(hcp.ee_vel_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        ee_vel_filter[ee].reset(0);
+        wrench_lpf_for_hpf[ee] = BiquadIIRFilterVec(6);
+        wrench_lpf_for_hpf[ee].setParameter(hcp.wrench_hpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        wrench_lpf_for_hpf[ee].reset(0);
+        wrench_lpf[ee] = BiquadIIRFilterVec(6);
+        wrench_lpf[ee].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        wrench_lpf[ee].reset(0);
     }
 
-    for ( int i=0; i<ee_names.size(); i++) {
-        std::string n = "slave_"+ee_names[i]+"_wrench";
-        m_slaveEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee_names[i]]));
-        registerInPort(n.c_str(), *m_slaveEEWrenchesIn[ee_names[i]]);
-        m_slaveEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
+    for (auto ee : ee_names){
+        std::string n = "slave_"+ee+"_wrench";
+        m_slaveEEWrenchesIn[ee] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee]));
+        registerInPort(n.c_str(), *m_slaveEEWrenchesIn[ee]);
+        m_slaveEEWrenches[ee].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
         RTC_INFO_STREAM(" registerInPort " << n );
     }
 
@@ -114,25 +114,25 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     tgt_names.push_back("head");
     tgt_names.push_back("rhand");
     tgt_names.push_back("lhand");
-    for ( int i=0; i<tgt_names.size(); i++) {
-        std::string n = "master_"+tgt_names[i]+"_pose";
-        m_masterTgtPosesOut[tgt_names[i]] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
-        registerOutPort(n.c_str(), *m_masterTgtPosesOut[tgt_names[i]]);
+    for (auto tgt : tgt_names){
+        std::string n = "master_"+tgt+"_pose";
+        m_masterTgtPosesOut[tgt] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt]));
+        registerOutPort(n.c_str(), *m_masterTgtPosesOut[tgt]);
         RTC_INFO_STREAM(" registerOutPort " << n);
     }
 
     // sub usage
-    for ( int i=0; i<ee_names.size(); i++) {
-        std::string n = "master_"+ee_names[i]+"_wrench";
-        m_masterEEWrenchesOut[ee_names[i]] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_masterEEWrenches[ee_names[i]]));
-        registerOutPort(n.c_str(), *m_masterEEWrenchesOut[ee_names[i]]);
-        m_masterEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
+    for (auto ee : ee_names){
+        std::string n = "master_"+ee+"_wrench";
+        m_masterEEWrenchesOut[ee] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_masterEEWrenches[ee]));
+        registerOutPort(n.c_str(), *m_masterEEWrenchesOut[ee]);
+        m_masterEEWrenches[ee].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
         RTC_INFO_STREAM(" registerOutPort " << n );
     }
-    for ( int i=0; i<tgt_names.size(); i++) {
-        std::string n = "slave_"+tgt_names[i]+"_pose";
-        m_slaveTgtPosesIn[tgt_names[i]] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_slaveTgtPoses[tgt_names[i]]));
-        registerInPort(n.c_str(), *m_slaveTgtPosesIn[tgt_names[i]]);
+    for (auto tgt : tgt_names){
+        std::string n = "slave_"+tgt+"_pose";
+        m_slaveTgtPosesIn[tgt] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_slaveTgtPoses[tgt]));
+        registerInPort(n.c_str(), *m_slaveTgtPosesIn[tgt]);
         RTC_INFO_STREAM(" registerInPort " << n );
     }
 
@@ -180,9 +180,9 @@ RTC::ReturnCode_t HapticController::onExecute(RTC::UniqueId ec_id){
     if(m_qActIn.isNew()                     ){ m_qActIn.read(); }
     if(m_dqActIn.isNew()                    ){ m_dqActIn.read(); }
     if(m_delayCheckPacketInboundIn.isNew()  ){ m_delayCheckPacketInboundIn.read(); m_delayCheckPacketOutboundOut.write();}
-    for (int i=0; i<ee_names.size(); i++) {
-        if ( m_slaveEEWrenchesIn[ee_names[i]]->isNew() ) { m_slaveEEWrenchesIn[ee_names[i]]->read(); }
-    }
+    for(auto port : m_slaveEEWrenchesIn){   if( port.second->isNew() ) { port.second->read(); } }
+    for(auto port : m_slaveTgtPosesIn){     if( port.second->isNew() ) { port.second->read(); } }
+    
     mode.update();
     processTransition();
 
@@ -263,21 +263,26 @@ void HapticController::calcCurrentState(){
     }
 
     ///// calc end effector position and velocity
-    for(int i=0; i<ee_names.size();i++){
-        ee_pose[ee_names[i]] = ee_ikc_map[ee_names[i]].getCurrentTargetPose(m_robot);
+    for(auto ee : ee_names){
+        master_ee_pose[ee] = ee_ikc_map[ee].getCurrentTargetPose(m_robot);
+        slave_ee_pose[ee] = hrp::to_Pose3(m_slaveTgtPoses[ee].data);
     }
-    if(ee_pose_old.size()==0){ ee_pose_old = ee_pose; } //init
-    for(int i=0; i<ee_names.size();i++){
-        ee_vel[ee_names[i]].head(3) = (ee_pose[ee_names[i]].p - ee_pose_old[ee_names[i]].p) / m_dt;// rawは廃止したほうがいい？
-        ee_vel[ee_names[i]].tail(3) = ee_pose_old[ee_names[i]].R * hrp::omegaFromRot(ee_pose_old[ee_names[i]].R.transpose() * ee_pose[ee_names[i]].R) / m_dt; // world angular velocity
-        ee_vel_filtered[ee_names[i]] = ee_vel_filter[ee_names[i]].passFilter(ee_vel[ee_names[i]]);
+    if(master_ee_pose_old.size()==0 ){ master_ee_pose_old = master_ee_pose; } //init
+    if(slave_ee_pose_old.size()==0  ){ slave_ee_pose_old = slave_ee_pose; } //init
+    for(auto ee : ee_names){
+        master_ee_vel[ee].head(3) = (master_ee_pose[ee].p - master_ee_pose_old[ee].p) / m_dt;
+        master_ee_vel[ee].tail(3) = master_ee_pose_old[ee].R * hrp::omegaFromRot(master_ee_pose_old[ee].R.transpose() * master_ee_pose[ee].R) / m_dt; // world angular velocity
+        master_ee_vel_filtered[ee] = ee_vel_filter[ee].passFilter(master_ee_vel[ee]);
+        slave_ee_vel[ee].head(3) = (slave_ee_pose[ee].p - slave_ee_pose_old[ee].p) / m_dt;
+        slave_ee_vel[ee].tail(3) = slave_ee_pose_old[ee].R * hrp::omegaFromRot(slave_ee_pose_old[ee].R.transpose() * slave_ee_pose[ee].R) / m_dt; // world angular velocity
     }
-    ee_pose_old = ee_pose;
+    master_ee_pose_old  = master_ee_pose;
+    slave_ee_pose_old   = slave_ee_pose;
 
-    ///// calc base tp end effector Jacobian
-    for(int i=0; i<ee_names.size();i++){
-        hrp::JointPath jp(m_robot->rootLink(), m_robot->link(ee_ikc_map[ee_names[i]].target_link_name));
-        jp.calcJacobian(J_ee[ee_names[i]], ee_ikc_map[ee_names[i]].localPos);
+    ///// calc base to end effector Jacobian
+    for(auto ee : ee_names){
+        hrp::JointPath jp(m_robot->rootLink(), m_robot->link(ee_ikc_map[ee].target_link_name));
+        jp.calcJacobian(J_ee[ee], ee_ikc_map[ee].localPos);
     }
 }
 
@@ -338,93 +343,89 @@ void HapticController::calcTorque(){
 //    }
 
     std::map<std::string, hrp::dvector6> masterEEWrenches;
-    for (int i=0; i<ee_names.size(); i++){ masterEEWrenches[ee_names[i]].fill(0); }
+    for (auto ee : ee_names){ masterEEWrenches[ee].fill(0); }
 
     std::map<std::string, bool> is_contact_to_floor;
     {
-        std::vector<std::string> tgt;
-        tgt.push_back("lleg");
-        tgt.push_back("rleg");
+        const std::vector<std::string> legs = {"lleg", "rleg"};
 
         ///// fix ee rot horizontal
-        for (int i=0; i<tgt.size();i++){
+        for (auto leg : legs){
             hrp::Vector3 diff_rot;
-            rats::difference_rotation(diff_rot, ee_pose[tgt[i]].R, hrp::Matrix33::Identity());
-            const hrp::Vector3 diff_rot_vel = hrp::Vector3::Zero() - ee_vel_filtered[tgt[i]].tail(3);
+            rats::difference_rotation(diff_rot, master_ee_pose[leg].R, hrp::Matrix33::Identity());
+            const hrp::Vector3 diff_rot_vel = hrp::Vector3::Zero() - master_ee_vel_filtered[leg].tail(3);
             hrp::dvector6 wrench = (hrp::dvector6()<< 0,0,0, diff_rot * hcp.foot_horizontal_pd_gain(0) + diff_rot_vel * hcp.foot_horizontal_pd_gain(1)).finished();
             LIMIT_NORM_V(wrench.tail(3), 50);
-            hrp::dvector tq_tmp = J_ee[ee_names[i]].transpose() * wrench;
-            for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
-            masterEEWrenches[tgt[i]] += wrench;
+            hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
+            for(int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
+            masterEEWrenches[leg] += wrench;
         }
 
         ///// virtual floor
-        for (int i=0; i<tgt.size();i++){
-            const double foot_h_from_floor = ee_pose[tgt[i]].p(Z);
-            if(loop%1000==0){
-                dbg(foot_h_from_floor);
-            }
+        for (auto leg : legs){
+            const double foot_h_from_floor = master_ee_pose[leg].p(Z);
             if(foot_h_from_floor < 0){
-                hrp::dvector6 wrench = hrp::dvector6::Unit(fz) * (-foot_h_from_floor*hcp.floor_pd_gain(0) + (0-ee_vel_filtered[tgt[i]](fz))*hcp.floor_pd_gain(1));
+                hrp::dvector6 wrench = hrp::dvector6::Unit(fz) * (-foot_h_from_floor*hcp.floor_pd_gain(0) + (0-master_ee_vel_filtered[leg](fz))*hcp.floor_pd_gain(1));
                 LIMIT_NORM_V(wrench, 1000);
-                const hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
-                for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
-                masterEEWrenches[tgt[i]] += wrench;
+                const hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
+                for(int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
+                masterEEWrenches[leg] += wrench;
             }
-            is_contact_to_floor[tgt[i]] = (foot_h_from_floor < 0 + 0.03);
+            is_contact_to_floor[leg] = (foot_h_from_floor < 0 + 0.03);
+            if(loop%1000==0){ dbg(foot_h_from_floor); }
         }
 
         ///// virtual back wall
-        for (int i=0; i<tgt.size();i++){
+        for (auto leg : legs){
             const double wall_x_rel_base = -0.1;
-            const double current_x = ee_pose[tgt[i]].p(X) - m_robot->rootLink()->p(X);
+            const double current_x = master_ee_pose[leg].p(X) - m_robot->rootLink()->p(X);
             if(current_x < wall_x_rel_base){
                 hrp::dvector6 wrench = (hrp::dvector6()<< (wall_x_rel_base - current_x) * 1000,0,0, 0,0,0).finished();
                 LIMIT_NORM_V(wrench, 100);
-                hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
-                for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
-                masterEEWrenches[tgt[i]] += wrench;
+                hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
+                for(int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
+                masterEEWrenches[leg] += wrench;
             }
         }
 
         ///// keep apart each other
-        const double current_dist = ee_pose["lleg"].p(Y) - ee_pose["rleg"].p(Y);
+        const double current_dist = master_ee_pose["lleg"].p(Y) - master_ee_pose["rleg"].p(Y);
         if(current_dist < hcp.foot_min_distance){
-            for (int i=0; i<tgt.size();i++){
-                hrp::dvector6 wrench = (hrp::dvector6()<< 0, (tgt[i]=="lleg" ? 1:-1) * (hcp.foot_min_distance - current_dist) * 1000, 0,0,0,0).finished();
+            for (auto leg : legs){
+                hrp::dvector6 wrench = (hrp::dvector6()<< 0, (leg=="lleg" ? 1:-1) * (hcp.foot_min_distance - current_dist) * 1000, 0,0,0,0).finished();
                 LIMIT_NORM_V(wrench, 50);
-                hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
-                for (int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
-                masterEEWrenches[tgt[i]] += wrench;
+                hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
+                for (int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
+                masterEEWrenches[leg] += wrench;
             }
         }
 
         ///// virtual floor, leg constraint
-        static hrp::Vector3 locked_rel_pos = ee_pose["rleg"].p - ee_pose["lleg"].p;
+        static hrp::Vector3 locked_rel_pos = master_ee_pose["rleg"].p - master_ee_pose["lleg"].p;
         if(is_contact_to_floor["lleg"] && is_contact_to_floor["rleg"]){
-            for (int i=0; i<tgt.size();i++){
-                hrp::Vector3 cur_rel_pos = ee_pose["rleg"].p - ee_pose["lleg"].p;
+            for (auto leg : legs){
+                hrp::Vector3 cur_rel_pos = master_ee_pose["rleg"].p - master_ee_pose["lleg"].p;
                 hrp::Vector2 rleg_wrench = (locked_rel_pos - cur_rel_pos).head(XY) * 1000;
-                hrp::dvector6 wrench = (hrp::dvector6()<< (tgt[i]=="rleg" ? 1:-1) * rleg_wrench, 0,0,0,0).finished();
+                hrp::dvector6 wrench = (hrp::dvector6()<< (leg=="rleg" ? 1:-1) * rleg_wrench, 0,0,0,0).finished();
                 LIMIT_NORM_V(wrench, 1000);
-                hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
-                for (int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
-                masterEEWrenches[tgt[i]] += wrench;
+                hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
+                for (int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
+                masterEEWrenches[leg] += wrench;
             }
         }else{
-            locked_rel_pos = ee_pose["rleg"].p - ee_pose["lleg"].p;
+            locked_rel_pos = master_ee_pose["rleg"].p - master_ee_pose["lleg"].p;
         }
 
         ///// ex
-        for (int i=0; i<ee_names.size();i++){
-            hrp::dvector6 wrench = hcp.ex_ee_ref_wrench[ee_names[i]];
+        for (auto ee : ee_names){
+            hrp::dvector6 wrench = hcp.ex_ee_ref_wrench[ee];
             LIMIT_NORM_V(wrench, 500);
-            hrp::dvector tq_tmp = J_ee[ee_names[i]].transpose() * wrench;
-            for (int j=0; j<jpath_ee[ee_names[i]].numJoints(); j++){ jpath_ee[ee_names[i]].joint(j)->u += tq_tmp(j); }
-            masterEEWrenches[ee_names[i]] += wrench;
+            hrp::dvector tq_tmp = J_ee[ee].transpose() * wrench;
+            for (int j=0; j<jpath_ee[ee].numJoints(); j++){ jpath_ee[ee].joint(j)->u += tq_tmp(j); }
+            masterEEWrenches[ee] += wrench;
         }
 
-
+        ///// calc odometry
         static std::map<std::string, hrp::Vector2> leg_pos_from_floor_origin;
         hrp::Vector2 baselink_pos_from_floor_origin;
         if(loop==0){
@@ -433,77 +434,64 @@ void HapticController::calcTorque(){
             baselink_pos_from_floor_origin << 0,0;
         }
         if(is_contact_to_floor["lleg"] && !is_contact_to_floor["rleg"] ){ // when lleg has contact to floor, each value can be calculated from lleg
-            leg_pos_from_floor_origin["rleg"]   = (ee_pose["rleg"].p        - ee_pose["lleg"].p).head(XY) + leg_pos_from_floor_origin["lleg"];
-            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - ee_pose["lleg"].p).head(XY) + leg_pos_from_floor_origin["lleg"];
+            leg_pos_from_floor_origin["rleg"]   = (master_ee_pose["rleg"].p        - master_ee_pose["lleg"].p).head(XY) + leg_pos_from_floor_origin["lleg"];
+            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - master_ee_pose["lleg"].p).head(XY) + leg_pos_from_floor_origin["lleg"];
         }
         else if(!is_contact_to_floor["lleg"] && is_contact_to_floor["rleg"] ){ // when rleg has contact to floor, each value can be calculated from rleg
-            leg_pos_from_floor_origin["lleg"]   = (ee_pose["lleg"].p        - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];
-            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];
+            leg_pos_from_floor_origin["lleg"]   = (master_ee_pose["lleg"].p        - master_ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];
+            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - master_ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];
         }
         else if(is_contact_to_floor["lleg"] && is_contact_to_floor["rleg"] ){ // when both has contact to floor, baselink may be calculated from rleg or lleg
-            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];// for example
+            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - master_ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];// for example
         }
         else{ // when both in the air, both feet pos won't be update, only baselink will be update
-            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];// for example
+            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p - master_ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];// for example
         }
-
         //update base link pos from world
         m_robot->rootLink()->p << baselink_pos_from_floor_origin, baselink_h_from_floor;
-
         m_teleopOdom.data = (RTC::Pose3D){m_robot->rootLink()->p(X),m_robot->rootLink()->p(Y),m_robot->rootLink()->p(Z), 0,0,0};
         m_teleopOdom.tm = m_qRef.tm;
         m_teleopOdomOut.write();
-
-
     }
 
 
-
     // calc real robot feedback force
-    for (int i=0; i<ee_names.size(); i++){
-        if(ee_names[i] == "lleg" || ee_names[i] == "rleg" ) continue; // skip leg force feedback
-        const hrp::dvector6 wrench_raw = hrp::to_dvector(m_slaveEEWrenches[ee_names[i]].data);
-        const hrp::dvector6 w_hpf = wrench_raw - wrench_lpf_for_hpf[ee_names[i]].passFilter( wrench_raw );
-        const hrp::dvector6 w_lpf = wrench_lpf[ee_names[i]].passFilter( wrench_raw );
-        wrench_shaped[ee_names[i]] = w_hpf * hcp.wrench_hpf_gain + w_lpf * hcp.wrench_lpf_gain;
-
-        wrench_used[ee_names[i]] = wrench_shaped[ee_names[i]];
-        LIMIT_NORM_V(wrench_used[ee_names[i]].head(3),50);
-        LIMIT_NORM_V(wrench_used[ee_names[i]].tail(3),5);
-
-        hrp::Vector3 friction_f = - hcp.ee_pos_rot_friction_coeff(0) * ee_vel_filtered[ee_names[i]].head(3);
-        hrp::Vector3 friction_t = - hcp.ee_pos_rot_friction_coeff(1) * ee_vel_filtered[ee_names[i]].tail(3);
-
-        if(ee_names[i] == "rarm"){
-            m_debugData.data = hrp::to_DoubleSeq((hrp::dvector(6+6+6+6)<<wrench_raw,wrench_shaped[ee_names[i]],w_hpf,w_lpf).finished());
-            m_debugData.tm = m_qRef.tm;
-            m_debugDataOut.write();
-        }
-
-        hrp::JointPath jp(m_robot->rootLink(), m_robot->link(ee_ikc_map[ee_names[i]].target_link_name));
-        hrp::dmatrix J_base_to_ee;
-        jp.calcJacobian(J_base_to_ee, ee_ikc_map[ee_names[i]].localPos);
-        hrp::dvector tq_from_feedbackWrench = J_base_to_ee.transpose() * wrench_used[ee_names[i]];
+    for (auto ee : ee_names){
+        hrp::dvector6 slave_w_raw = hrp::to_dvector(m_slaveEEWrenches[ee].data);
+        if(ee == "lleg" || ee == "rleg" ){ slave_w_raw.tail(4).fill(0); }// disable Fz Tx Ty Tz
+        const hrp::dvector6 slave_w_hp = slave_w_raw - wrench_lpf_for_hpf[ee].passFilter( slave_w_raw );
+        const hrp::dvector6 slave_w_lp = wrench_lpf[ee].passFilter( slave_w_raw );
+        const hrp::dvector6 slave_w_shaped = slave_w_raw * hcp.force_feedback_ratio 
+                                            + slave_w_hp * hcp.wrench_hpf_gain 
+                                            + slave_w_lp * hcp.wrench_lpf_gain;
+
+        // F_master + v_master * a = F_slave + v_slave * a
+        hrp::dvector6 coeff_v;
+        coeff_v.head(XYZ).fill(hcp.ee_pos_rot_friction_coeff(0));
+        coeff_v.tail(XYZ).fill(hcp.ee_pos_rot_friction_coeff(1));
+        hrp::dvector6 master_w_ans = slave_w_shaped + slave_ee_vel[ee].cwiseProduct(coeff_v) - master_ee_vel[ee].cwiseProduct(coeff_v);
+        LIMIT_NORM_V(master_w_ans.head(3), hcp.force_feedback_limit_ft(0));
+        LIMIT_NORM_V(master_w_ans.tail(3), hcp.force_feedback_limit_ft(1));
+
+        hrp::JointPath jp(m_robot->rootLink(), m_robot->link(ee_ikc_map[ee].target_link_name));
+        hrp::dvector tq_from_feedbackWrench = J_ee[ee].transpose() * master_w_ans;
         for (int j = 0; j < jp.numJoints(); j++) jp.joint(j)->u += tq_from_feedbackWrench(j);
-        if(loop%1000==0)dbgv(wrench_used[ee_names[i]]);
-        masterEEWrenches[ee_names[i]] += wrench_used[ee_names[i]];
+        masterEEWrenches[ee] += master_w_ans;
+        if(loop%1000==0)dbg(ee);
+        if(loop%1000==0)dbgv(master_w_ans);
     }
 
 
     {
         // soft joint limit
-        m_robot->link("RARM_JOINT1")->ulimit = deg2rad(-10);
-        m_robot->link("LARM_JOINT1")->llimit = deg2rad(10);
+        m_robot->link("RARM_JOINT1")->ulimit = deg2rad(0);// hotfix
+        m_robot->link("LARM_JOINT1")->llimit = deg2rad(0);
         for(int i=0;i<m_robot->numJoints();i++){
             const double soft_ulimit = m_robot->joint(i)->ulimit - deg2rad(15);
             const double soft_llimit = m_robot->joint(i)->llimit + deg2rad(15);
             double soft_jlimit_tq = 0;
-            if(m_qAct.data[i] > soft_ulimit){
-                soft_jlimit_tq = 100 * (soft_ulimit - m_qAct.data[i]) + 0 * (0 - dqAct_filtered(i));
-            }
-            if(m_qAct.data[i] < soft_llimit){
-                soft_jlimit_tq = 100 * (soft_llimit - m_qAct.data[i]) + 0 * (0 - dqAct_filtered(i));
-            }
+            if(m_qAct.data[i] > soft_ulimit){ soft_jlimit_tq = 100 * (soft_ulimit - m_qAct.data[i]) + 0 * (0 - dqAct_filtered(i)); }
+            if(m_qAct.data[i] < soft_llimit){ soft_jlimit_tq = 100 * (soft_llimit - m_qAct.data[i]) + 0 * (0 - dqAct_filtered(i)); }
             m_robot->joint(i)->u += soft_jlimit_tq;
         }
     }
@@ -525,39 +513,34 @@ void HapticController::calcTorque(){
         hrp::dvector dq_diff = - dqAct_filtered;
         hrp::dvector torque = (q_diff * hcp.q_ref_pd_gain(0) + dq_diff * hcp.q_ref_pd_gain(1)) * j_power_coeff;
         for (int i=0; i<m_robot->numJoints(); i++){
-            LIMIT_NORM(torque(i), max_torque(i)/5);// pd torque will be limited to 1/5 max torque
+            LIMIT_NORM(torque(i), max_torque(i) * hcp.q_ref_max_torque_ratio);// pd torque will be limited to 1/5 max torque
             m_robot->joint(i)->u += torque(i);
         }
     }
 
-
-
-
-
-
+    // torque limit
     for(int i=0;i<m_robot->numJoints();i++){
         LIMIT_NORM(m_robot->joint(i)->u, max_torque(i)*2);// up to x2 max torque
 //        double tlimit = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst;////?????
     }
-
     if(loop%1000==0)dbgv(hrp::getUAll(m_robot));
 
 
     // for debug plot
-    for (int i=0; i<ee_names.size(); i++){
-        m_masterEEWrenches[ee_names[i]].tm = m_qRef.tm;
-        m_masterEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq(masterEEWrenches[ee_names[i]]);
-        m_masterEEWrenchesOut[ee_names[i]]->write();
+    for (auto ee : ee_names){
+        m_masterEEWrenches[ee].tm = m_qRef.tm;
+        m_masterEEWrenches[ee].data = hrp::to_DoubleSeq(masterEEWrenches[ee]);
+        m_masterEEWrenchesOut[ee]->write();
     }
 
 
     m_masterTgtPoses["com"].data = hrp::to_Pose3D(hrp::Pose3(m_robot->calcCM(), hrp::Matrix33::Identity()));
-    for (int i=0; i<ee_names.size(); i++){
-        m_masterTgtPoses[ee_names[i]].data = hrp::to_Pose3D(ee_ikc_map[ee_names[i]].getCurrentTargetPose(m_robot));//四肢揃ってないと危険
+    for (auto ee : ee_names){
+        m_masterTgtPoses[ee].data = hrp::to_Pose3D(ee_ikc_map[ee].getCurrentTargetPose(m_robot));//四肢揃ってないと危険
     }
-    for (int i=0; i<tgt_names.size(); i++){
-        m_masterTgtPoses[tgt_names[i]].tm = m_qRef.tm;
-        m_masterTgtPosesOut[tgt_names[i]]->write();
+    for (auto tgt : tgt_names){
+        m_masterTgtPoses[tgt].tm = m_qRef.tm;
+        m_masterTgtPosesOut[tgt]->write();
     }
 
 
@@ -643,7 +626,7 @@ bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticC
     hcp.force_feedback_ratio                = i_param.force_feedback_ratio;
     hcp.gravity_compensation_ratio          = i_param.gravity_compensation_ratio;
     hcp.q_friction_coeff                    = i_param.q_friction_coeff;
-    hcp.q_ref_output_ratio_goal             = i_param.q_ref_output_ratio_goal;
+    hcp.q_ref_max_torque_ratio              = i_param.q_ref_max_torque_ratio;
     hcp.wrench_hpf_cutoff_hz                = i_param.wrench_hpf_cutoff_hz;
     hcp.wrench_lpf_cutoff_hz                = i_param.wrench_lpf_cutoff_hz;
     hcp.wrench_hpf_gain                     = i_param.wrench_hpf_gain;
@@ -651,24 +634,24 @@ bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticC
     hcp.ee_pos_rot_friction_coeff           = hrp::to_Vector2(i_param.ee_pos_rot_friction_coeff);
     hcp.floor_pd_gain                       = hrp::to_Vector2(i_param.floor_pd_gain);
     hcp.foot_horizontal_pd_gain             = hrp::to_Vector2(i_param.foot_horizontal_pd_gain);
+    hcp.force_feedback_limit_ft             = hrp::to_Vector2(i_param.force_feedback_limit_ft);
     hcp.q_ref_pd_gain                       = hrp::to_Vector2(i_param.q_ref_pd_gain);
-    for(int i=0;i<ee_names.size();i++){
+    for (int i=0; i<ee_names.size(); i++){//順番気をつけないと危険．confファイルに書いたend_effectorの順
         for(int j=0;j<6;j++){
             hcp.ex_ee_ref_wrench[ee_names[i]](j) =  i_param.ex_ee_ref_wrench[i][j];
         }
     }
     ///// update process if required
-    q_ref_ip->setGoal(&hcp.q_ref_output_ratio_goal, 5.0, true);
     baselink_h_ip->setGoal(&hcp.baselink_height_from_floor, 5.0, true);
     dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
     dqAct_filter.reset(0);
-    for(int i=0;i<ee_names.size();i++){
-        ee_vel_filter[ee_names[i]].setParameter(hcp.ee_vel_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        ee_vel_filter[ee_names[i]].reset(0);
-        wrench_lpf_for_hpf[ee_names[i]].setParameter(hcp.wrench_hpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        wrench_lpf_for_hpf[ee_names[i]].reset(0);
-        wrench_lpf[ee_names[i]].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        wrench_lpf[ee_names[i]].reset(0);
+    for (auto ee : ee_names){
+        ee_vel_filter[ee].setParameter(hcp.ee_vel_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        ee_vel_filter[ee].reset(0);
+        wrench_lpf_for_hpf[ee].setParameter(hcp.wrench_hpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        wrench_lpf_for_hpf[ee].reset(0);
+        wrench_lpf[ee].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        wrench_lpf[ee].reset(0);
     }
     return true;
 }
@@ -685,7 +668,7 @@ bool HapticController::getParams(OpenHRP::HapticControllerService::HapticControl
     i_param.force_feedback_ratio                = hcp.force_feedback_ratio;
     i_param.gravity_compensation_ratio          = hcp.gravity_compensation_ratio;
     i_param.q_friction_coeff                    = hcp.q_friction_coeff;
-    i_param.q_ref_output_ratio_goal             = hcp.q_ref_output_ratio_goal;
+    i_param.q_ref_max_torque_ratio              = hcp.q_ref_max_torque_ratio;
     i_param.wrench_hpf_cutoff_hz                = hcp.wrench_hpf_cutoff_hz;
     i_param.wrench_lpf_cutoff_hz                = hcp.wrench_lpf_cutoff_hz;
     i_param.wrench_hpf_gain                     = hcp.wrench_hpf_gain;
@@ -693,12 +676,13 @@ bool HapticController::getParams(OpenHRP::HapticControllerService::HapticControl
     i_param.ee_pos_rot_friction_coeff           = hrp::to_DblSequence2(hcp.ee_pos_rot_friction_coeff);
     i_param.floor_pd_gain                       = hrp::to_DblSequence2(hcp.floor_pd_gain);
     i_param.foot_horizontal_pd_gain             = hrp::to_DblSequence2(hcp.foot_horizontal_pd_gain);
+    i_param.force_feedback_limit_ft             = hrp::to_DblSequence2(hcp.force_feedback_limit_ft);
     i_param.q_ref_pd_gain                       = hrp::to_DblSequence2(hcp.q_ref_pd_gain);
 
     i_param.ex_ee_ref_wrench.length(4);
-    for(int i=0;i<ee_names.size();i++){
+    for (int i=0; i<ee_names.size(); i++){//順番気をつけないと危険．confファイルに書いたend_effectorの順
         i_param.ex_ee_ref_wrench[i].length(6);
-        for(int j=0;j<6;j++){
+        for(int j=0;j<ft_xyz;j++){
             i_param.ex_ee_ref_wrench[i][j] = hcp.ex_ee_ref_wrench[ee_names[i]](j);
         }
     }
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index a3254bde1cb..d2a29fa187f 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -120,9 +120,8 @@ class HapticController : public RTC::DataFlowComponentBase{
         std::map<std::string, IKConstraint> ee_ikc_map;
         std::map<std::string, BiquadIIRFilterVec> ee_vel_filter;
         std::map<std::string, BiquadIIRFilterVec> wrench_lpf_for_hpf, wrench_lpf;
-        std::map<std::string, hrp::dvector6> wrench_shaped, wrench_used;
-        std::map<std::string, hrp::Pose3> ee_pose, ee_pose_old;
-        std::map<std::string, hrp::dvector6> ee_vel, ee_vel_filtered; // = twist
+        std::map<std::string, hrp::Pose3> master_ee_pose, master_ee_pose_old, slave_ee_pose, slave_ee_pose_old;
+        std::map<std::string, hrp::dvector6> master_ee_vel, master_ee_vel_filtered, slave_ee_vel; // = twist
         std::map<std::string, hrp::JointPath> jpath_ee;
         std::map<std::string, hrp::dmatrix> J_ee;
 
@@ -141,7 +140,7 @@ class HapticController : public RTC::DataFlowComponentBase{
                 double force_feedback_ratio;
                 double gravity_compensation_ratio;
                 double q_friction_coeff;
-                double q_ref_output_ratio_goal;
+                double q_ref_max_torque_ratio;
                 double wrench_hpf_cutoff_hz;
                 double wrench_lpf_cutoff_hz;
                 double wrench_hpf_gain;
@@ -149,6 +148,7 @@ class HapticController : public RTC::DataFlowComponentBase{
                 hrp::Vector2 ee_pos_rot_friction_coeff;
                 hrp::Vector2 floor_pd_gain;
                 hrp::Vector2 foot_horizontal_pd_gain;
+                hrp::Vector2 force_feedback_limit_ft;
                 hrp::Vector2 q_ref_pd_gain;
                 std::map<std::string, hrp::dvector6> ex_ee_ref_wrench;
             HCParams(){
@@ -161,14 +161,15 @@ class HapticController : public RTC::DataFlowComponentBase{
                 force_feedback_ratio                = 0.2;
                 gravity_compensation_ratio          = 1.0;
                 q_friction_coeff                    = 0;
-                q_ref_output_ratio_goal             = 0;
+                q_ref_max_torque_ratio              = 0.1;
                 wrench_hpf_cutoff_hz                = 20;
                 wrench_lpf_cutoff_hz                = 0.3;
-                wrench_hpf_gain                     = 1;
-                wrench_lpf_gain                     = 0.2;
-                ee_pos_rot_friction_coeff           << 0, 0; // 1, 0.1
+                wrench_hpf_gain                     = 0.8;
+                wrench_lpf_gain                     = 0;
+                ee_pos_rot_friction_coeff           << 10, 0.1;
                 floor_pd_gain                       << 10000, 500;
                 foot_horizontal_pd_gain             << 300, 30;
+                force_feedback_limit_ft             << 100, 10;
                 q_ref_pd_gain                       << 50, 0;
                 ex_ee_ref_wrench["rleg"]            = hrp::dvector6::Zero();
                 ex_ee_ref_wrench["lleg"]            = hrp::dvector6::Zero();

From 45cf8e3d6419bb758b582f1500b9a378f054d402 Mon Sep 17 00:00:00 2001
From: leus <leus@example.com>
Date: Mon, 16 Dec 2019 14:30:11 +0900
Subject: [PATCH 200/217] up

---
 idl/HapticControllerService.idl           |   3 +-
 rtc/HapticController/CMakeLists.txt       |   1 +
 rtc/HapticController/HapticController.cpp | 312 ++++++++++------------
 rtc/HapticController/HapticController.h   |  17 +-
 4 files changed, 160 insertions(+), 173 deletions(-)

diff --git a/idl/HapticControllerService.idl b/idl/HapticControllerService.idl
index 0f2dc182281..1635aeae3cf 100644
--- a/idl/HapticControllerService.idl
+++ b/idl/HapticControllerService.idl
@@ -29,7 +29,7 @@ module OpenHRP
         double force_feedback_ratio;
         double gravity_compensation_ratio;
         double q_friction_coeff;
-        double q_ref_output_ratio_goal;
+        double q_ref_max_torque_ratio;
         double wrench_hpf_cutoff_hz;
         double wrench_lpf_cutoff_hz;
         double wrench_hpf_gain;
@@ -37,6 +37,7 @@ module OpenHRP
         DblSequence2 ee_pos_rot_friction_coeff;
         DblSequence2 floor_pd_gain;
         DblSequence2 foot_horizontal_pd_gain;
+        DblSequence2 force_feedback_limit_ft;
         DblSequence2 q_ref_pd_gain;
         DblSeqSeq6   ex_ee_ref_wrench;
     };
diff --git a/rtc/HapticController/CMakeLists.txt b/rtc/HapticController/CMakeLists.txt
index 09861c5e331..b5a203ecc74 100644
--- a/rtc/HapticController/CMakeLists.txt
+++ b/rtc/HapticController/CMakeLists.txt
@@ -1,5 +1,6 @@
 set(comp_sources HapticController.cpp HapticControllerService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp ../TorqueFilter/IIRFilter.cpp)
 set(libs ${OpenCV_LIBRARIES} hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 add_library(HapticController SHARED ${comp_sources})
 target_link_libraries(HapticController ${libs})
 set_target_properties(HapticController PROPERTIES PREFIX "")
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index ce0da9fe8e6..1e0879d76a8 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -89,23 +89,23 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     dqAct_filter = BiquadIIRFilterVec(m_robot->numJoints());
     dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
     dqAct_filter.reset(0);
-    for ( int i=0; i<ee_names.size(); i++) {
-        ee_vel_filter[ee_names[i]] = BiquadIIRFilterVec(6);
-        ee_vel_filter[ee_names[i]].setParameter(hcp.ee_vel_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        ee_vel_filter[ee_names[i]].reset(0);
-        wrench_lpf_for_hpf[ee_names[i]] = BiquadIIRFilterVec(6);
-        wrench_lpf_for_hpf[ee_names[i]].setParameter(hcp.wrench_hpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        wrench_lpf_for_hpf[ee_names[i]].reset(0);
-        wrench_lpf[ee_names[i]] = BiquadIIRFilterVec(6);
-        wrench_lpf[ee_names[i]].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        wrench_lpf[ee_names[i]].reset(0);
+    for (auto ee : ee_names){
+        ee_vel_filter[ee] = BiquadIIRFilterVec(6);
+        ee_vel_filter[ee].setParameter(hcp.ee_vel_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        ee_vel_filter[ee].reset(0);
+        wrench_lpf_for_hpf[ee] = BiquadIIRFilterVec(6);
+        wrench_lpf_for_hpf[ee].setParameter(hcp.wrench_hpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        wrench_lpf_for_hpf[ee].reset(0);
+        wrench_lpf[ee] = BiquadIIRFilterVec(6);
+        wrench_lpf[ee].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        wrench_lpf[ee].reset(0);
     }
 
-    for ( int i=0; i<ee_names.size(); i++) {
-        std::string n = "slave_"+ee_names[i]+"_wrench";
-        m_slaveEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee_names[i]]));
-        registerInPort(n.c_str(), *m_slaveEEWrenchesIn[ee_names[i]]);
-        m_slaveEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
+    for (auto ee : ee_names){
+        std::string n = "slave_"+ee+"_wrench";
+        m_slaveEEWrenchesIn[ee] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee]));
+        registerInPort(n.c_str(), *m_slaveEEWrenchesIn[ee]);
+        m_slaveEEWrenches[ee].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
         RTC_INFO_STREAM(" registerInPort " << n );
     }
 
@@ -114,25 +114,25 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     tgt_names.push_back("head");
     tgt_names.push_back("rhand");
     tgt_names.push_back("lhand");
-    for ( int i=0; i<tgt_names.size(); i++) {
-        std::string n = "master_"+tgt_names[i]+"_pose";
-        m_masterTgtPosesOut[tgt_names[i]] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
-        registerOutPort(n.c_str(), *m_masterTgtPosesOut[tgt_names[i]]);
+    for (auto tgt : tgt_names){
+        std::string n = "master_"+tgt+"_pose";
+        m_masterTgtPosesOut[tgt] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt]));
+        registerOutPort(n.c_str(), *m_masterTgtPosesOut[tgt]);
         RTC_INFO_STREAM(" registerOutPort " << n);
     }
 
     // sub usage
-    for ( int i=0; i<ee_names.size(); i++) {
-        std::string n = "master_"+ee_names[i]+"_wrench";
-        m_masterEEWrenchesOut[ee_names[i]] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_masterEEWrenches[ee_names[i]]));
-        registerOutPort(n.c_str(), *m_masterEEWrenchesOut[ee_names[i]]);
-        m_masterEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
+    for (auto ee : ee_names){
+        std::string n = "master_"+ee+"_wrench";
+        m_masterEEWrenchesOut[ee] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_masterEEWrenches[ee]));
+        registerOutPort(n.c_str(), *m_masterEEWrenchesOut[ee]);
+        m_masterEEWrenches[ee].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
         RTC_INFO_STREAM(" registerOutPort " << n );
     }
-    for ( int i=0; i<tgt_names.size(); i++) {
-        std::string n = "slave_"+tgt_names[i]+"_pose";
-        m_slaveTgtPosesIn[tgt_names[i]] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_slaveTgtPoses[tgt_names[i]]));
-        registerInPort(n.c_str(), *m_slaveTgtPosesIn[tgt_names[i]]);
+    for (auto tgt : tgt_names){
+        std::string n = "slave_"+tgt+"_pose";
+        m_slaveTgtPosesIn[tgt] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_slaveTgtPoses[tgt]));
+        registerInPort(n.c_str(), *m_slaveTgtPosesIn[tgt]);
         RTC_INFO_STREAM(" registerInPort " << n );
     }
 
@@ -180,9 +180,9 @@ RTC::ReturnCode_t HapticController::onExecute(RTC::UniqueId ec_id){
     if(m_qActIn.isNew()                     ){ m_qActIn.read(); }
     if(m_dqActIn.isNew()                    ){ m_dqActIn.read(); }
     if(m_delayCheckPacketInboundIn.isNew()  ){ m_delayCheckPacketInboundIn.read(); m_delayCheckPacketOutboundOut.write();}
-    for (int i=0; i<ee_names.size(); i++) {
-        if ( m_slaveEEWrenchesIn[ee_names[i]]->isNew() ) { m_slaveEEWrenchesIn[ee_names[i]]->read(); }
-    }
+    for(auto port : m_slaveEEWrenchesIn){   if( port.second->isNew() ) { port.second->read(); } }
+    for(auto port : m_slaveTgtPosesIn){     if( port.second->isNew() ) { port.second->read(); } }
+    
     mode.update();
     processTransition();
 
@@ -263,21 +263,26 @@ void HapticController::calcCurrentState(){
     }
 
     ///// calc end effector position and velocity
-    for(int i=0; i<ee_names.size();i++){
-        ee_pose[ee_names[i]] = ee_ikc_map[ee_names[i]].getCurrentTargetPose(m_robot);
+    for(auto ee : ee_names){
+        master_ee_pose[ee] = ee_ikc_map[ee].getCurrentTargetPose(m_robot);
+        slave_ee_pose[ee] = hrp::to_Pose3(m_slaveTgtPoses[ee].data);
     }
-    if(ee_pose_old.size()==0){ ee_pose_old = ee_pose; } //init
-    for(int i=0; i<ee_names.size();i++){
-        ee_vel[ee_names[i]].head(3) = (ee_pose[ee_names[i]].p - ee_pose_old[ee_names[i]].p) / m_dt;// rawは廃止したほうがいい？
-        ee_vel[ee_names[i]].tail(3) = ee_pose_old[ee_names[i]].R * hrp::omegaFromRot(ee_pose_old[ee_names[i]].R.transpose() * ee_pose[ee_names[i]].R) / m_dt; // world angular velocity
-        ee_vel_filtered[ee_names[i]] = ee_vel_filter[ee_names[i]].passFilter(ee_vel[ee_names[i]]);
+    if(master_ee_pose_old.size()==0 ){ master_ee_pose_old = master_ee_pose; } //init
+    if(slave_ee_pose_old.size()==0  ){ slave_ee_pose_old = slave_ee_pose; } //init
+    for(auto ee : ee_names){
+        master_ee_vel[ee].head(3) = (master_ee_pose[ee].p - master_ee_pose_old[ee].p) / m_dt;
+        master_ee_vel[ee].tail(3) = master_ee_pose_old[ee].R * hrp::omegaFromRot(master_ee_pose_old[ee].R.transpose() * master_ee_pose[ee].R) / m_dt; // world angular velocity
+        master_ee_vel_filtered[ee] = ee_vel_filter[ee].passFilter(master_ee_vel[ee]);
+        slave_ee_vel[ee].head(3) = (slave_ee_pose[ee].p - slave_ee_pose_old[ee].p) / m_dt;
+        slave_ee_vel[ee].tail(3) = slave_ee_pose_old[ee].R * hrp::omegaFromRot(slave_ee_pose_old[ee].R.transpose() * slave_ee_pose[ee].R) / m_dt; // world angular velocity
     }
-    ee_pose_old = ee_pose;
+    master_ee_pose_old  = master_ee_pose;
+    slave_ee_pose_old   = slave_ee_pose;
 
-    ///// calc base tp end effector Jacobian
-    for(int i=0; i<ee_names.size();i++){
-        hrp::JointPath jp(m_robot->rootLink(), m_robot->link(ee_ikc_map[ee_names[i]].target_link_name));
-        jp.calcJacobian(J_ee[ee_names[i]], ee_ikc_map[ee_names[i]].localPos);
+    ///// calc base to end effector Jacobian
+    for(auto ee : ee_names){
+        hrp::JointPath jp(m_robot->rootLink(), m_robot->link(ee_ikc_map[ee].target_link_name));
+        jp.calcJacobian(J_ee[ee], ee_ikc_map[ee].localPos);
     }
 }
 
@@ -338,93 +343,89 @@ void HapticController::calcTorque(){
 //    }
 
     std::map<std::string, hrp::dvector6> masterEEWrenches;
-    for (int i=0; i<ee_names.size(); i++){ masterEEWrenches[ee_names[i]].fill(0); }
+    for (auto ee : ee_names){ masterEEWrenches[ee].fill(0); }
 
     std::map<std::string, bool> is_contact_to_floor;
     {
-        std::vector<std::string> tgt;
-        tgt.push_back("lleg");
-        tgt.push_back("rleg");
+        const std::vector<std::string> legs = {"lleg", "rleg"};
 
         ///// fix ee rot horizontal
-        for (int i=0; i<tgt.size();i++){
+        for (auto leg : legs){
             hrp::Vector3 diff_rot;
-            rats::difference_rotation(diff_rot, ee_pose[tgt[i]].R, hrp::Matrix33::Identity());
-            const hrp::Vector3 diff_rot_vel = hrp::Vector3::Zero() - ee_vel_filtered[tgt[i]].tail(3);
+            rats::difference_rotation(diff_rot, master_ee_pose[leg].R, hrp::Matrix33::Identity());
+            const hrp::Vector3 diff_rot_vel = hrp::Vector3::Zero() - master_ee_vel_filtered[leg].tail(3);
             hrp::dvector6 wrench = (hrp::dvector6()<< 0,0,0, diff_rot * hcp.foot_horizontal_pd_gain(0) + diff_rot_vel * hcp.foot_horizontal_pd_gain(1)).finished();
             LIMIT_NORM_V(wrench.tail(3), 50);
-            hrp::dvector tq_tmp = J_ee[ee_names[i]].transpose() * wrench;
-            for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
-            masterEEWrenches[tgt[i]] += wrench;
+            hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
+            for(int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
+            masterEEWrenches[leg] += wrench;
         }
 
         ///// virtual floor
-        for (int i=0; i<tgt.size();i++){
-            const double foot_h_from_floor = ee_pose[tgt[i]].p(Z);
-            if(loop%1000==0){
-                dbg(foot_h_from_floor);
-            }
+        for (auto leg : legs){
+            const double foot_h_from_floor = master_ee_pose[leg].p(Z);
             if(foot_h_from_floor < 0){
-                hrp::dvector6 wrench = hrp::dvector6::Unit(fz) * (-foot_h_from_floor*hcp.floor_pd_gain(0) + (0-ee_vel_filtered[tgt[i]](fz))*hcp.floor_pd_gain(1));
+                hrp::dvector6 wrench = hrp::dvector6::Unit(fz) * (-foot_h_from_floor*hcp.floor_pd_gain(0) + (0-master_ee_vel_filtered[leg](fz))*hcp.floor_pd_gain(1));
                 LIMIT_NORM_V(wrench, 1000);
-                const hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
-                for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
-                masterEEWrenches[tgt[i]] += wrench;
+                const hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
+                for(int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
+                masterEEWrenches[leg] += wrench;
             }
-            is_contact_to_floor[tgt[i]] = (foot_h_from_floor < 0 + 0.03);
+            is_contact_to_floor[leg] = (foot_h_from_floor < 0 + 0.03);
+            if(loop%1000==0){ dbg(foot_h_from_floor); }
         }
 
         ///// virtual back wall
-        for (int i=0; i<tgt.size();i++){
+        for (auto leg : legs){
             const double wall_x_rel_base = -0.1;
-            const double current_x = ee_pose[tgt[i]].p(X) - m_robot->rootLink()->p(X);
+            const double current_x = master_ee_pose[leg].p(X) - m_robot->rootLink()->p(X);
             if(current_x < wall_x_rel_base){
                 hrp::dvector6 wrench = (hrp::dvector6()<< (wall_x_rel_base - current_x) * 1000,0,0, 0,0,0).finished();
                 LIMIT_NORM_V(wrench, 100);
-                hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
-                for(int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
-                masterEEWrenches[tgt[i]] += wrench;
+                hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
+                for(int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
+                masterEEWrenches[leg] += wrench;
             }
         }
 
         ///// keep apart each other
-        const double current_dist = ee_pose["lleg"].p(Y) - ee_pose["rleg"].p(Y);
+        const double current_dist = master_ee_pose["lleg"].p(Y) - master_ee_pose["rleg"].p(Y);
         if(current_dist < hcp.foot_min_distance){
-            for (int i=0; i<tgt.size();i++){
-                hrp::dvector6 wrench = (hrp::dvector6()<< 0, (tgt[i]=="lleg" ? 1:-1) * (hcp.foot_min_distance - current_dist) * 1000, 0,0,0,0).finished();
+            for (auto leg : legs){
+                hrp::dvector6 wrench = (hrp::dvector6()<< 0, (leg=="lleg" ? 1:-1) * (hcp.foot_min_distance - current_dist) * 1000, 0,0,0,0).finished();
                 LIMIT_NORM_V(wrench, 50);
-                hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
-                for (int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
-                masterEEWrenches[tgt[i]] += wrench;
+                hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
+                for (int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
+                masterEEWrenches[leg] += wrench;
             }
         }
 
         ///// virtual floor, leg constraint
-        static hrp::Vector3 locked_rel_pos = ee_pose["rleg"].p - ee_pose["lleg"].p;
+        static hrp::Vector3 locked_rel_pos = master_ee_pose["rleg"].p - master_ee_pose["lleg"].p;
         if(is_contact_to_floor["lleg"] && is_contact_to_floor["rleg"]){
-            for (int i=0; i<tgt.size();i++){
-                hrp::Vector3 cur_rel_pos = ee_pose["rleg"].p - ee_pose["lleg"].p;
+            for (auto leg : legs){
+                hrp::Vector3 cur_rel_pos = master_ee_pose["rleg"].p - master_ee_pose["lleg"].p;
                 hrp::Vector2 rleg_wrench = (locked_rel_pos - cur_rel_pos).head(XY) * 1000;
-                hrp::dvector6 wrench = (hrp::dvector6()<< (tgt[i]=="rleg" ? 1:-1) * rleg_wrench, 0,0,0,0).finished();
+                hrp::dvector6 wrench = (hrp::dvector6()<< (leg=="rleg" ? 1:-1) * rleg_wrench, 0,0,0,0).finished();
                 LIMIT_NORM_V(wrench, 1000);
-                hrp::dvector tq_tmp = J_ee[tgt[i]].transpose() * wrench;
-                for (int j=0; j<jpath_ee[tgt[i]].numJoints(); j++){ jpath_ee[tgt[i]].joint(j)->u += tq_tmp(j); }
-                masterEEWrenches[tgt[i]] += wrench;
+                hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
+                for (int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
+                masterEEWrenches[leg] += wrench;
             }
         }else{
-            locked_rel_pos = ee_pose["rleg"].p - ee_pose["lleg"].p;
+            locked_rel_pos = master_ee_pose["rleg"].p - master_ee_pose["lleg"].p;
         }
 
         ///// ex
-        for (int i=0; i<ee_names.size();i++){
-            hrp::dvector6 wrench = hcp.ex_ee_ref_wrench[ee_names[i]];
+        for (auto ee : ee_names){
+            hrp::dvector6 wrench = hcp.ex_ee_ref_wrench[ee];
             LIMIT_NORM_V(wrench, 500);
-            hrp::dvector tq_tmp = J_ee[ee_names[i]].transpose() * wrench;
-            for (int j=0; j<jpath_ee[ee_names[i]].numJoints(); j++){ jpath_ee[ee_names[i]].joint(j)->u += tq_tmp(j); }
-            masterEEWrenches[ee_names[i]] += wrench;
+            hrp::dvector tq_tmp = J_ee[ee].transpose() * wrench;
+            for (int j=0; j<jpath_ee[ee].numJoints(); j++){ jpath_ee[ee].joint(j)->u += tq_tmp(j); }
+            masterEEWrenches[ee] += wrench;
         }
 
-
+        ///// calc odometry
         static std::map<std::string, hrp::Vector2> leg_pos_from_floor_origin;
         hrp::Vector2 baselink_pos_from_floor_origin;
         if(loop==0){
@@ -433,77 +434,64 @@ void HapticController::calcTorque(){
             baselink_pos_from_floor_origin << 0,0;
         }
         if(is_contact_to_floor["lleg"] && !is_contact_to_floor["rleg"] ){ // when lleg has contact to floor, each value can be calculated from lleg
-            leg_pos_from_floor_origin["rleg"]   = (ee_pose["rleg"].p        - ee_pose["lleg"].p).head(XY) + leg_pos_from_floor_origin["lleg"];
-            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - ee_pose["lleg"].p).head(XY) + leg_pos_from_floor_origin["lleg"];
+            leg_pos_from_floor_origin["rleg"]   = (master_ee_pose["rleg"].p        - master_ee_pose["lleg"].p).head(XY) + leg_pos_from_floor_origin["lleg"];
+            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - master_ee_pose["lleg"].p).head(XY) + leg_pos_from_floor_origin["lleg"];
         }
         else if(!is_contact_to_floor["lleg"] && is_contact_to_floor["rleg"] ){ // when rleg has contact to floor, each value can be calculated from rleg
-            leg_pos_from_floor_origin["lleg"]   = (ee_pose["lleg"].p        - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];
-            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];
+            leg_pos_from_floor_origin["lleg"]   = (master_ee_pose["lleg"].p        - master_ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];
+            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - master_ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];
         }
         else if(is_contact_to_floor["lleg"] && is_contact_to_floor["rleg"] ){ // when both has contact to floor, baselink may be calculated from rleg or lleg
-            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];// for example
+            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - master_ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];// for example
         }
         else{ // when both in the air, both feet pos won't be update, only baselink will be update
-            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p - ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];// for example
+            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p - master_ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];// for example
         }
-
         //update base link pos from world
         m_robot->rootLink()->p << baselink_pos_from_floor_origin, baselink_h_from_floor;
-
         m_teleopOdom.data = (RTC::Pose3D){m_robot->rootLink()->p(X),m_robot->rootLink()->p(Y),m_robot->rootLink()->p(Z), 0,0,0};
         m_teleopOdom.tm = m_qRef.tm;
         m_teleopOdomOut.write();
-
-
     }
 
 
-
     // calc real robot feedback force
-    for (int i=0; i<ee_names.size(); i++){
-        if(ee_names[i] == "lleg" || ee_names[i] == "rleg" ) continue; // skip leg force feedback
-        const hrp::dvector6 wrench_raw = hrp::to_dvector(m_slaveEEWrenches[ee_names[i]].data);
-        const hrp::dvector6 w_hpf = wrench_raw - wrench_lpf_for_hpf[ee_names[i]].passFilter( wrench_raw );
-        const hrp::dvector6 w_lpf = wrench_lpf[ee_names[i]].passFilter( wrench_raw );
-        wrench_shaped[ee_names[i]] = w_hpf * hcp.wrench_hpf_gain + w_lpf * hcp.wrench_lpf_gain;
-
-        wrench_used[ee_names[i]] = wrench_shaped[ee_names[i]];
-        LIMIT_NORM_V(wrench_used[ee_names[i]].head(3),50);
-        LIMIT_NORM_V(wrench_used[ee_names[i]].tail(3),5);
-
-        hrp::Vector3 friction_f = - hcp.ee_pos_rot_friction_coeff(0) * ee_vel_filtered[ee_names[i]].head(3);
-        hrp::Vector3 friction_t = - hcp.ee_pos_rot_friction_coeff(1) * ee_vel_filtered[ee_names[i]].tail(3);
-
-        if(ee_names[i] == "rarm"){
-            m_debugData.data = hrp::to_DoubleSeq((hrp::dvector(6+6+6+6)<<wrench_raw,wrench_shaped[ee_names[i]],w_hpf,w_lpf).finished());
-            m_debugData.tm = m_qRef.tm;
-            m_debugDataOut.write();
-        }
-
-        hrp::JointPath jp(m_robot->rootLink(), m_robot->link(ee_ikc_map[ee_names[i]].target_link_name));
-        hrp::dmatrix J_base_to_ee;
-        jp.calcJacobian(J_base_to_ee, ee_ikc_map[ee_names[i]].localPos);
-        hrp::dvector tq_from_feedbackWrench = J_base_to_ee.transpose() * wrench_used[ee_names[i]];
+    for (auto ee : ee_names){
+        hrp::dvector6 slave_w_raw = hrp::to_dvector(m_slaveEEWrenches[ee].data);
+        if(ee == "lleg" || ee == "rleg" ){ slave_w_raw.tail(4).fill(0); }// disable Fz Tx Ty Tz
+        const hrp::dvector6 slave_w_hp = slave_w_raw - wrench_lpf_for_hpf[ee].passFilter( slave_w_raw );
+        const hrp::dvector6 slave_w_lp = wrench_lpf[ee].passFilter( slave_w_raw );
+        const hrp::dvector6 slave_w_shaped = slave_w_raw * hcp.force_feedback_ratio 
+                                            + slave_w_hp * hcp.wrench_hpf_gain 
+                                            + slave_w_lp * hcp.wrench_lpf_gain;
+
+        // F_master + v_master * a = F_slave + v_slave * a
+        hrp::dvector6 coeff_v;
+        coeff_v.head(XYZ).fill(hcp.ee_pos_rot_friction_coeff(0));
+        coeff_v.tail(XYZ).fill(hcp.ee_pos_rot_friction_coeff(1));
+        hrp::dvector6 master_w_ans = slave_w_shaped + slave_ee_vel[ee].cwiseProduct(coeff_v) - master_ee_vel[ee].cwiseProduct(coeff_v);
+        LIMIT_NORM_V(master_w_ans.head(3), hcp.force_feedback_limit_ft(0));
+        LIMIT_NORM_V(master_w_ans.tail(3), hcp.force_feedback_limit_ft(1));
+
+        hrp::JointPath jp(m_robot->rootLink(), m_robot->link(ee_ikc_map[ee].target_link_name));
+        hrp::dvector tq_from_feedbackWrench = J_ee[ee].transpose() * master_w_ans;
         for (int j = 0; j < jp.numJoints(); j++) jp.joint(j)->u += tq_from_feedbackWrench(j);
-        if(loop%1000==0)dbgv(wrench_used[ee_names[i]]);
-        masterEEWrenches[ee_names[i]] += wrench_used[ee_names[i]];
+        masterEEWrenches[ee] += master_w_ans;
+        if(loop%1000==0)dbg(ee);
+        if(loop%1000==0)dbgv(master_w_ans);
     }
 
 
     {
         // soft joint limit
-        m_robot->link("RARM_JOINT1")->ulimit = deg2rad(-10);
-        m_robot->link("LARM_JOINT1")->llimit = deg2rad(10);
+        m_robot->link("RARM_JOINT1")->ulimit = deg2rad(0);// hotfix
+        m_robot->link("LARM_JOINT1")->llimit = deg2rad(0);
         for(int i=0;i<m_robot->numJoints();i++){
             const double soft_ulimit = m_robot->joint(i)->ulimit - deg2rad(15);
             const double soft_llimit = m_robot->joint(i)->llimit + deg2rad(15);
             double soft_jlimit_tq = 0;
-            if(m_qAct.data[i] > soft_ulimit){
-                soft_jlimit_tq = 100 * (soft_ulimit - m_qAct.data[i]) + 0 * (0 - dqAct_filtered(i));
-            }
-            if(m_qAct.data[i] < soft_llimit){
-                soft_jlimit_tq = 100 * (soft_llimit - m_qAct.data[i]) + 0 * (0 - dqAct_filtered(i));
-            }
+            if(m_qAct.data[i] > soft_ulimit){ soft_jlimit_tq = 100 * (soft_ulimit - m_qAct.data[i]) + 0 * (0 - dqAct_filtered(i)); }
+            if(m_qAct.data[i] < soft_llimit){ soft_jlimit_tq = 100 * (soft_llimit - m_qAct.data[i]) + 0 * (0 - dqAct_filtered(i)); }
             m_robot->joint(i)->u += soft_jlimit_tq;
         }
     }
@@ -521,43 +509,38 @@ void HapticController::calcTorque(){
     hrp::dvector j_power_coeff = max_torque/max_torque.maxCoeff(); // reduce gain of light inertia joint
 
     {// calc qref pd control torque
-        hrp::dvector q_diff = hrp::to_dvector(m_qRef.data) - hrp::to_dvector(m_qAct.data);
-        hrp::dvector dq_diff = - dqAct_filtered;
+        const hrp::dvector q_diff = hrp::to_dvector(m_qRef.data) - hrp::to_dvector(m_qAct.data);
+        const hrp::dvector dq_diff = - dqAct_filtered;
         hrp::dvector torque = (q_diff * hcp.q_ref_pd_gain(0) + dq_diff * hcp.q_ref_pd_gain(1)) * j_power_coeff;
         for (int i=0; i<m_robot->numJoints(); i++){
-            LIMIT_NORM(torque(i), max_torque(i)/5);// pd torque will be limited to 1/5 max torque
+            LIMIT_NORM(torque(i), max_torque(i) * hcp.q_ref_max_torque_ratio);// pd torque will be limited to 1/5 max torque
             m_robot->joint(i)->u += torque(i);
         }
     }
 
-
-
-
-
-
+    // torque limit
     for(int i=0;i<m_robot->numJoints();i++){
         LIMIT_NORM(m_robot->joint(i)->u, max_torque(i)*2);// up to x2 max torque
 //        double tlimit = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst;////?????
     }
-
     if(loop%1000==0)dbgv(hrp::getUAll(m_robot));
 
 
     // for debug plot
-    for (int i=0; i<ee_names.size(); i++){
-        m_masterEEWrenches[ee_names[i]].tm = m_qRef.tm;
-        m_masterEEWrenches[ee_names[i]].data = hrp::to_DoubleSeq(masterEEWrenches[ee_names[i]]);
-        m_masterEEWrenchesOut[ee_names[i]]->write();
+    for (auto ee : ee_names){
+        m_masterEEWrenches[ee].tm = m_qRef.tm;
+        m_masterEEWrenches[ee].data = hrp::to_DoubleSeq(masterEEWrenches[ee]);
+        m_masterEEWrenchesOut[ee]->write();
     }
 
 
     m_masterTgtPoses["com"].data = hrp::to_Pose3D(hrp::Pose3(m_robot->calcCM(), hrp::Matrix33::Identity()));
-    for (int i=0; i<ee_names.size(); i++){
-        m_masterTgtPoses[ee_names[i]].data = hrp::to_Pose3D(ee_ikc_map[ee_names[i]].getCurrentTargetPose(m_robot));//四肢揃ってないと危険
+    for (auto ee : ee_names){
+        m_masterTgtPoses[ee].data = hrp::to_Pose3D(ee_ikc_map[ee].getCurrentTargetPose(m_robot));//四肢揃ってないと危険
     }
-    for (int i=0; i<tgt_names.size(); i++){
-        m_masterTgtPoses[tgt_names[i]].tm = m_qRef.tm;
-        m_masterTgtPosesOut[tgt_names[i]]->write();
+    for (auto tgt : tgt_names){
+        m_masterTgtPoses[tgt].tm = m_qRef.tm;
+        m_masterTgtPosesOut[tgt]->write();
     }
 
 
@@ -643,7 +626,7 @@ bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticC
     hcp.force_feedback_ratio                = i_param.force_feedback_ratio;
     hcp.gravity_compensation_ratio          = i_param.gravity_compensation_ratio;
     hcp.q_friction_coeff                    = i_param.q_friction_coeff;
-    hcp.q_ref_output_ratio_goal             = i_param.q_ref_output_ratio_goal;
+    hcp.q_ref_max_torque_ratio              = i_param.q_ref_max_torque_ratio;
     hcp.wrench_hpf_cutoff_hz                = i_param.wrench_hpf_cutoff_hz;
     hcp.wrench_lpf_cutoff_hz                = i_param.wrench_lpf_cutoff_hz;
     hcp.wrench_hpf_gain                     = i_param.wrench_hpf_gain;
@@ -651,24 +634,24 @@ bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticC
     hcp.ee_pos_rot_friction_coeff           = hrp::to_Vector2(i_param.ee_pos_rot_friction_coeff);
     hcp.floor_pd_gain                       = hrp::to_Vector2(i_param.floor_pd_gain);
     hcp.foot_horizontal_pd_gain             = hrp::to_Vector2(i_param.foot_horizontal_pd_gain);
+    hcp.force_feedback_limit_ft             = hrp::to_Vector2(i_param.force_feedback_limit_ft);
     hcp.q_ref_pd_gain                       = hrp::to_Vector2(i_param.q_ref_pd_gain);
-    for(int i=0;i<ee_names.size();i++){
+    for (int i=0; i<ee_names.size(); i++){//順番気をつけないと危険．confファイルに書いたend_effectorの順
         for(int j=0;j<6;j++){
             hcp.ex_ee_ref_wrench[ee_names[i]](j) =  i_param.ex_ee_ref_wrench[i][j];
         }
     }
     ///// update process if required
-    q_ref_ip->setGoal(&hcp.q_ref_output_ratio_goal, 5.0, true);
     baselink_h_ip->setGoal(&hcp.baselink_height_from_floor, 5.0, true);
     dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
     dqAct_filter.reset(0);
-    for(int i=0;i<ee_names.size();i++){
-        ee_vel_filter[ee_names[i]].setParameter(hcp.ee_vel_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        ee_vel_filter[ee_names[i]].reset(0);
-        wrench_lpf_for_hpf[ee_names[i]].setParameter(hcp.wrench_hpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        wrench_lpf_for_hpf[ee_names[i]].reset(0);
-        wrench_lpf[ee_names[i]].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
-        wrench_lpf[ee_names[i]].reset(0);
+    for (auto ee : ee_names){
+        ee_vel_filter[ee].setParameter(hcp.ee_vel_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        ee_vel_filter[ee].reset(0);
+        wrench_lpf_for_hpf[ee].setParameter(hcp.wrench_hpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        wrench_lpf_for_hpf[ee].reset(0);
+        wrench_lpf[ee].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
+        wrench_lpf[ee].reset(0);
     }
     return true;
 }
@@ -685,7 +668,7 @@ bool HapticController::getParams(OpenHRP::HapticControllerService::HapticControl
     i_param.force_feedback_ratio                = hcp.force_feedback_ratio;
     i_param.gravity_compensation_ratio          = hcp.gravity_compensation_ratio;
     i_param.q_friction_coeff                    = hcp.q_friction_coeff;
-    i_param.q_ref_output_ratio_goal             = hcp.q_ref_output_ratio_goal;
+    i_param.q_ref_max_torque_ratio              = hcp.q_ref_max_torque_ratio;
     i_param.wrench_hpf_cutoff_hz                = hcp.wrench_hpf_cutoff_hz;
     i_param.wrench_lpf_cutoff_hz                = hcp.wrench_lpf_cutoff_hz;
     i_param.wrench_hpf_gain                     = hcp.wrench_hpf_gain;
@@ -693,12 +676,13 @@ bool HapticController::getParams(OpenHRP::HapticControllerService::HapticControl
     i_param.ee_pos_rot_friction_coeff           = hrp::to_DblSequence2(hcp.ee_pos_rot_friction_coeff);
     i_param.floor_pd_gain                       = hrp::to_DblSequence2(hcp.floor_pd_gain);
     i_param.foot_horizontal_pd_gain             = hrp::to_DblSequence2(hcp.foot_horizontal_pd_gain);
+    i_param.force_feedback_limit_ft             = hrp::to_DblSequence2(hcp.force_feedback_limit_ft);
     i_param.q_ref_pd_gain                       = hrp::to_DblSequence2(hcp.q_ref_pd_gain);
 
     i_param.ex_ee_ref_wrench.length(4);
-    for(int i=0;i<ee_names.size();i++){
+    for (int i=0; i<ee_names.size(); i++){//順番気をつけないと危険．confファイルに書いたend_effectorの順
         i_param.ex_ee_ref_wrench[i].length(6);
-        for(int j=0;j<6;j++){
+        for(int j=0;j<ft_xyz;j++){
             i_param.ex_ee_ref_wrench[i][j] = hcp.ex_ee_ref_wrench[ee_names[i]](j);
         }
     }
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index a3254bde1cb..d2a29fa187f 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -120,9 +120,8 @@ class HapticController : public RTC::DataFlowComponentBase{
         std::map<std::string, IKConstraint> ee_ikc_map;
         std::map<std::string, BiquadIIRFilterVec> ee_vel_filter;
         std::map<std::string, BiquadIIRFilterVec> wrench_lpf_for_hpf, wrench_lpf;
-        std::map<std::string, hrp::dvector6> wrench_shaped, wrench_used;
-        std::map<std::string, hrp::Pose3> ee_pose, ee_pose_old;
-        std::map<std::string, hrp::dvector6> ee_vel, ee_vel_filtered; // = twist
+        std::map<std::string, hrp::Pose3> master_ee_pose, master_ee_pose_old, slave_ee_pose, slave_ee_pose_old;
+        std::map<std::string, hrp::dvector6> master_ee_vel, master_ee_vel_filtered, slave_ee_vel; // = twist
         std::map<std::string, hrp::JointPath> jpath_ee;
         std::map<std::string, hrp::dmatrix> J_ee;
 
@@ -141,7 +140,7 @@ class HapticController : public RTC::DataFlowComponentBase{
                 double force_feedback_ratio;
                 double gravity_compensation_ratio;
                 double q_friction_coeff;
-                double q_ref_output_ratio_goal;
+                double q_ref_max_torque_ratio;
                 double wrench_hpf_cutoff_hz;
                 double wrench_lpf_cutoff_hz;
                 double wrench_hpf_gain;
@@ -149,6 +148,7 @@ class HapticController : public RTC::DataFlowComponentBase{
                 hrp::Vector2 ee_pos_rot_friction_coeff;
                 hrp::Vector2 floor_pd_gain;
                 hrp::Vector2 foot_horizontal_pd_gain;
+                hrp::Vector2 force_feedback_limit_ft;
                 hrp::Vector2 q_ref_pd_gain;
                 std::map<std::string, hrp::dvector6> ex_ee_ref_wrench;
             HCParams(){
@@ -161,14 +161,15 @@ class HapticController : public RTC::DataFlowComponentBase{
                 force_feedback_ratio                = 0.2;
                 gravity_compensation_ratio          = 1.0;
                 q_friction_coeff                    = 0;
-                q_ref_output_ratio_goal             = 0;
+                q_ref_max_torque_ratio              = 0.1;
                 wrench_hpf_cutoff_hz                = 20;
                 wrench_lpf_cutoff_hz                = 0.3;
-                wrench_hpf_gain                     = 1;
-                wrench_lpf_gain                     = 0.2;
-                ee_pos_rot_friction_coeff           << 0, 0; // 1, 0.1
+                wrench_hpf_gain                     = 0.8;
+                wrench_lpf_gain                     = 0;
+                ee_pos_rot_friction_coeff           << 10, 0.1;
                 floor_pd_gain                       << 10000, 500;
                 foot_horizontal_pd_gain             << 300, 30;
+                force_feedback_limit_ft             << 100, 10;
                 q_ref_pd_gain                       << 50, 0;
                 ex_ee_ref_wrench["rleg"]            = hrp::dvector6::Zero();
                 ex_ee_ref_wrench["lleg"]            = hrp::dvector6::Zero();

From ed0299249c99315a1b9c82ed264f291de806dd62 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 17 Dec 2019 22:04:36 +0900
Subject: [PATCH 201/217] add new Odom func

---
 idl/HapticControllerService.idl               |  1 +
 rtc/AutoBalancer/MyUtils.h                    | 11 +++
 rtc/HapticController/HapticController.cpp     | 93 ++++++++++---------
 rtc/HapticController/HapticController.h       |  6 +-
 .../HapticControllerService_impl.cpp          |  4 +
 .../HapticControllerService_impl.h            |  1 +
 6 files changed, 73 insertions(+), 43 deletions(-)

diff --git a/idl/HapticControllerService.idl b/idl/HapticControllerService.idl
index 1635aeae3cf..3587777c9af 100644
--- a/idl/HapticControllerService.idl
+++ b/idl/HapticControllerService.idl
@@ -45,6 +45,7 @@ module OpenHRP
     boolean stopHapticController();
     boolean pauseHapticController();
     boolean resumeHapticController();
+    void resetOdom();
     void setParams(in HapticControllerParam i_param);
     void getParams(out HapticControllerParam i_param);
   };
diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index c6364fe3aa8..5c7a8881a3f 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -58,6 +58,15 @@ namespace hrp{
             void setRpy(const double _r, const double _p, const double _y)                                                      { R = rotFromRpy(_r,_p,_y); }
             void setRpy(const hrp::Vector3& _rpy)                                                                               { R = rotFromRpy(_rpy); }
     };
+    inline Pose3 calcRelPose3(const Pose3& from, const Pose3& to){
+        return Pose3( from.R.transpose() * (to.p - from.p), from.R.transpose() * to.R );
+    }
+    inline Pose3 applyRelPose3(const Pose3& tgt, const Pose3& transform){
+        return Pose3( tgt.p + tgt.R * transform.p, tgt.R * transform.R);
+    }
+    inline Pose3 to_2DPlanePose3(const Pose3& in){
+        return Pose3(in.p(X), in.p(Y), 0, 0, 0, in.rpy()(y));
+    }
 
     inline hrp::Vector3         to_Vector3      (const RTC::Point3D& in)        { return hrp::Vector3(in.x, in.y, in.z); }
     inline hrp::Vector3         to_Vector3      (const RTC::Orientation3D& in)  { return hrp::Vector3(in.r, in.p, in.y); }
@@ -73,6 +82,8 @@ namespace hrp{
     inline OpenHRP::Wrench      to_Wrench       (const hrp::dvector6& in)       { return (OpenHRP::Wrench){in(X),in(Y),in(Z),in(r),in(p),in(y)}; }
     inline RTC::TimedDoubleSeq::_data_seq   to_DoubleSeq    (const hrp::dvector& in)    { RTC::TimedDoubleSeq::_data_seq out; out.length(in.size()); hrp::dvector::Map(out.get_buffer(), in.size()) = in; return out; }
 
+    inline hrp::Pose3   getLinkPose3    (const hrp::Link* _link)                { return hrp::Pose3(_link->p, _link->R);  }
+    inline void         setLinkPose3    (hrp::Link* _link, const hrp::Pose3& val){ _link->p = val.p; _link->R = val.R;     }
     inline hrp::dvector getQAll         (const hrp::BodyPtr _robot){ hrp::dvector tmp(_robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ tmp(i) = _robot->joint(i)->q; } return tmp; }
     inline void         setQAll         (hrp::BodyPtr       _robot, const hrp::dvector& in){ assert(in.size() <= _robot->numJoints()); for(int i=0;i<in.size();i++){ _robot->joint(i)->q = in(i); } }
     inline hrp::dvector getRobotStateVec(const hrp::BodyPtr _robot){ return (hrp::dvector(_robot->numJoints()+6) << getQAll(_robot), _robot->rootLink()->p, hrp::rpyFromRot(_robot->rootLink()->R)).finished(); }
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index 1e0879d76a8..5b9875a78b2 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -65,6 +65,7 @@ RTC::ReturnCode_t HapticController::onInitialize(){
         RTC_WARN_STREAM("failed to load model[" << prop["model"] << "]");
         return RTC::RTC_ERROR;
     }
+    m_robot_odom = hrp::BodyPtr(new hrp::Body(*m_robot)); //copy
 
     setupEEIKConstraintFromConf(ee_ikc_map, m_robot, prop);
     RTC_INFO_STREAM("setupEEIKConstraintFromConf finished");
@@ -139,6 +140,7 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     m_tau.data = hrp::to_DoubleSeq(hrp::dvector::Zero(m_robot->numJoints()));
 
     loop = 0;
+    resetOdom_request = false;
     RTC_INFO_STREAM("onInitialize() OK");
     return RTC::RTC_OK;
 }
@@ -188,6 +190,7 @@ RTC::ReturnCode_t HapticController::onExecute(RTC::UniqueId ec_id){
 
     calcCurrentState();
     calcTorque();
+    calcOdometry();
 
     if (mode.isRunning()) {
         if(mode.isInitialize()){
@@ -344,8 +347,6 @@ void HapticController::calcTorque(){
 
     std::map<std::string, hrp::dvector6> masterEEWrenches;
     for (auto ee : ee_names){ masterEEWrenches[ee].fill(0); }
-
-    std::map<std::string, bool> is_contact_to_floor;
     {
         const std::vector<std::string> legs = {"lleg", "rleg"};
 
@@ -355,6 +356,7 @@ void HapticController::calcTorque(){
             rats::difference_rotation(diff_rot, master_ee_pose[leg].R, hrp::Matrix33::Identity());
             const hrp::Vector3 diff_rot_vel = hrp::Vector3::Zero() - master_ee_vel_filtered[leg].tail(3);
             hrp::dvector6 wrench = (hrp::dvector6()<< 0,0,0, diff_rot * hcp.foot_horizontal_pd_gain(0) + diff_rot_vel * hcp.foot_horizontal_pd_gain(1)).finished();
+            wrench(tz) = 0;
             LIMIT_NORM_V(wrench.tail(3), 50);
             hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
             for(int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
@@ -363,7 +365,9 @@ void HapticController::calcTorque(){
 
         ///// virtual floor
         for (auto leg : legs){
-            const double foot_h_from_floor = master_ee_pose[leg].p(Z);
+            const double floor_h_wld = m_robot->rootLink()->p(Z) - baselink_h_from_floor;
+            const double foot_h_from_floor = master_ee_pose[leg].p(Z) - floor_h_wld;
+            // const double foot_h_from_floor = baselink_h_from_floor - (m_robot->rootLink()->p(Z) - master_ee_pose[leg].p(Z));
             if(foot_h_from_floor < 0){
                 hrp::dvector6 wrench = hrp::dvector6::Unit(fz) * (-foot_h_from_floor*hcp.floor_pd_gain(0) + (0-master_ee_vel_filtered[leg](fz))*hcp.floor_pd_gain(1));
                 LIMIT_NORM_V(wrench, 1000);
@@ -401,19 +405,22 @@ void HapticController::calcTorque(){
         }
 
         ///// virtual floor, leg constraint
-        static hrp::Vector3 locked_rel_pos = master_ee_pose["rleg"].p - master_ee_pose["lleg"].p;
+        static hrp::Pose3 locked_l2r_pose = hrp::calcRelPose3(master_ee_pose["lleg"], master_ee_pose["rleg"]);
         if(is_contact_to_floor["lleg"] && is_contact_to_floor["rleg"]){
+            hrp::Pose3 cur_l2r_pose = hrp::calcRelPose3(master_ee_pose["lleg"], master_ee_pose["rleg"]);
+            hrp::Vector3 diff_pos, diff_rot;
+            diff_pos = locked_l2r_pose.p - cur_l2r_pose.p;
+            rats::difference_rotation(diff_rot, cur_l2r_pose.R, locked_l2r_pose.R);
+            hrp::dvector6 rleg_wrench = (hrp::dvector6()<< diff_pos * 1000, diff_rot * 100).finished();
+            LIMIT_NORM_V(rleg_wrench, 1000);
             for (auto leg : legs){
-                hrp::Vector3 cur_rel_pos = master_ee_pose["rleg"].p - master_ee_pose["lleg"].p;
-                hrp::Vector2 rleg_wrench = (locked_rel_pos - cur_rel_pos).head(XY) * 1000;
-                hrp::dvector6 wrench = (hrp::dvector6()<< (leg=="rleg" ? 1:-1) * rleg_wrench, 0,0,0,0).finished();
-                LIMIT_NORM_V(wrench, 1000);
+                hrp::dvector6 wrench = (leg=="rleg" ? 1:-1) *  rleg_wrench;
                 hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
                 for (int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
                 masterEEWrenches[leg] += wrench;
             }
         }else{
-            locked_rel_pos = master_ee_pose["rleg"].p - master_ee_pose["lleg"].p;
+            locked_l2r_pose = hrp::calcRelPose3(master_ee_pose["lleg"], master_ee_pose["rleg"]);
         }
 
         ///// ex
@@ -424,37 +431,8 @@ void HapticController::calcTorque(){
             for (int j=0; j<jpath_ee[ee].numJoints(); j++){ jpath_ee[ee].joint(j)->u += tq_tmp(j); }
             masterEEWrenches[ee] += wrench;
         }
-
-        ///// calc odometry
-        static std::map<std::string, hrp::Vector2> leg_pos_from_floor_origin;
-        hrp::Vector2 baselink_pos_from_floor_origin;
-        if(loop==0){
-            leg_pos_from_floor_origin["lleg"] << 0, 0.15;
-            leg_pos_from_floor_origin["rleg"] << 0,- 0.15;
-            baselink_pos_from_floor_origin << 0,0;
-        }
-        if(is_contact_to_floor["lleg"] && !is_contact_to_floor["rleg"] ){ // when lleg has contact to floor, each value can be calculated from lleg
-            leg_pos_from_floor_origin["rleg"]   = (master_ee_pose["rleg"].p        - master_ee_pose["lleg"].p).head(XY) + leg_pos_from_floor_origin["lleg"];
-            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - master_ee_pose["lleg"].p).head(XY) + leg_pos_from_floor_origin["lleg"];
-        }
-        else if(!is_contact_to_floor["lleg"] && is_contact_to_floor["rleg"] ){ // when rleg has contact to floor, each value can be calculated from rleg
-            leg_pos_from_floor_origin["lleg"]   = (master_ee_pose["lleg"].p        - master_ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];
-            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - master_ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];
-        }
-        else if(is_contact_to_floor["lleg"] && is_contact_to_floor["rleg"] ){ // when both has contact to floor, baselink may be calculated from rleg or lleg
-            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p   - master_ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];// for example
-        }
-        else{ // when both in the air, both feet pos won't be update, only baselink will be update
-            baselink_pos_from_floor_origin      = (m_robot->rootLink()->p - master_ee_pose["rleg"].p).head(XY) + leg_pos_from_floor_origin["rleg"];// for example
-        }
-        //update base link pos from world
-        m_robot->rootLink()->p << baselink_pos_from_floor_origin, baselink_h_from_floor;
-        m_teleopOdom.data = (RTC::Pose3D){m_robot->rootLink()->p(X),m_robot->rootLink()->p(Y),m_robot->rootLink()->p(Z), 0,0,0};
-        m_teleopOdom.tm = m_qRef.tm;
-        m_teleopOdomOut.write();
     }
 
-
     // calc real robot feedback force
     for (auto ee : ee_names){
         hrp::dvector6 slave_w_raw = hrp::to_dvector(m_slaveEEWrenches[ee].data);
@@ -532,18 +510,43 @@ void HapticController::calcTorque(){
         m_masterEEWrenches[ee].data = hrp::to_DoubleSeq(masterEEWrenches[ee]);
         m_masterEEWrenchesOut[ee]->write();
     }
+}
+
 
 
-    m_masterTgtPoses["com"].data = hrp::to_Pose3D(hrp::Pose3(m_robot->calcCM(), hrp::Matrix33::Identity()));
+void HapticController::calcOdometry(){
+    // m_robotの軸足-ベースリンク間の相対姿勢を使って, m_robot_odomの世界座標でのベースリンク絶対姿勢を更新
+    // しゃがんでる時の運足はなかったことにしてもいいかなと思っている
+    static std::map<std::string, hrp::Pose3> leg_pose_from_floor_origin;
+    if(mode.now() != MODE_HC || resetOdom_request){ // reset odom
+        for (auto leg : {"rleg", "lleg"}){ leg_pose_from_floor_origin[leg] = hrp::to_2DPlanePose3(master_ee_pose[leg]); }
+        if(resetOdom_request){ resetOdom_request = false; }
+    }
+    const std::string   parent_leg          = (master_ee_pose["rleg"].p(Z) < master_ee_pose["lleg"].p(Z))   ? "rleg" : "lleg"; // choose lower height leg for odom parent
+    const std::string   child_leg           = (parent_leg == "rleg")                                        ? "lleg" : "rleg";
+    const hrp::Pose3    cur_base_pose       = hrp::getLinkPose3 (m_robot->rootLink());
+    const hrp::Pose3    pleg_to_base        = hrp::calcRelPose3 ( hrp::to_2DPlanePose3(master_ee_pose[parent_leg]),             hrp::to_2DPlanePose3(cur_base_pose)             );
+    const hrp::Pose3    pleg_to_cleg        = hrp::calcRelPose3 ( hrp::to_2DPlanePose3(master_ee_pose[parent_leg]),             hrp::to_2DPlanePose3(master_ee_pose[child_leg]) );
+    hrp::Pose3 base_pose_from_floor_origin  = hrp::applyRelPose3( hrp::to_2DPlanePose3(leg_pose_from_floor_origin[parent_leg]), hrp::to_2DPlanePose3(pleg_to_base)              );
+    base_pose_from_floor_origin.p(Z)        = m_robot->rootLink()->p(Z) - std::min(master_ee_pose["rleg"].p(Z), master_ee_pose["lleg"].p(Z)); // ajust lower foot on to the z=0 in the world.
+    leg_pose_from_floor_origin[child_leg]   = hrp::applyRelPose3( hrp::to_2DPlanePose3(leg_pose_from_floor_origin[parent_leg]), hrp::to_2DPlanePose3(pleg_to_cleg)              );
+
+    //update base link pos from world
+    hrp::setLinkPose3   (m_robot_odom->rootLink(),  base_pose_from_floor_origin);
+    hrp::setQAll        (m_robot_odom,              hrp::getQAll(m_robot));
+    m_robot_odom->calcForwardKinematics();
+
+    m_teleopOdom.data   = hrp::to_Pose3D(hrp::getLinkPose3(m_robot_odom->rootLink()));
+    m_teleopOdom.tm     = m_qRef.tm;
+    m_teleopOdomOut.write();
+    m_masterTgtPoses["com"].data = hrp::to_Pose3D(hrp::Pose3(m_robot_odom->calcCM(), hrp::Matrix33::Identity()));
     for (auto ee : ee_names){
-        m_masterTgtPoses[ee].data = hrp::to_Pose3D(ee_ikc_map[ee].getCurrentTargetPose(m_robot));//四肢揃ってないと危険
+        m_masterTgtPoses[ee].data = hrp::to_Pose3D(ee_ikc_map[ee].getCurrentTargetPose(m_robot_odom));//四肢揃ってないと危険
     }
     for (auto tgt : tgt_names){
         m_masterTgtPoses[tgt].tm = m_qRef.tm;
         m_masterTgtPosesOut[tgt]->write();
     }
-
-
 }
 
 void HapticController::processTransition(){
@@ -605,6 +608,12 @@ bool HapticController::stopHapticController(){
     }
 }
 
+
+void HapticController::resetOdom(){
+    resetOdom_request = true;
+    RTC_INFO_STREAM("resetOdom");
+}
+
 namespace hrp{
     hrp::Vector2 to_Vector2(const OpenHRP::HapticControllerService::DblSequence2& in){ return (hrp::Vector2()<< in[0],in[1]).finished(); }
     OpenHRP::HapticControllerService::DblSequence2 to_DblSequence2(const hrp::Vector2& in){
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index d2a29fa187f..6e3d9053579 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -68,6 +68,7 @@ class HapticController : public RTC::DataFlowComponentBase{
         bool stopHapticController();
         bool pauseHapticController();
         bool resumeHapticController();
+        void resetOdom();
         bool setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param);
         bool getParams(OpenHRP::HapticControllerService::HapticControllerParam& i_param);
         typedef boost::shared_ptr<RTC::InPort   <RTC::TimedPose3D>      > ITP3_Ptr;
@@ -109,7 +110,7 @@ class HapticController : public RTC::DataFlowComponentBase{
         double m_dt;
         unsigned int loop;
         unsigned int m_debugLevel;
-        hrp::BodyPtr m_robot;
+        hrp::BodyPtr m_robot, m_robot_odom;
 
         double output_ratio, q_ref_output_ratio, baselink_h_from_floor, default_baselink_h_from_floor;
         interpolator *t_ip, *q_ref_ip, *baselink_h_ip;
@@ -124,6 +125,8 @@ class HapticController : public RTC::DataFlowComponentBase{
         std::map<std::string, hrp::dvector6> master_ee_vel, master_ee_vel_filtered, slave_ee_vel; // = twist
         std::map<std::string, hrp::JointPath> jpath_ee;
         std::map<std::string, hrp::dmatrix> J_ee;
+        std::map<std::string, bool> is_contact_to_floor;
+        bool resetOdom_request;
 
         ControlMode mode;
 
@@ -181,6 +184,7 @@ class HapticController : public RTC::DataFlowComponentBase{
         RTC::ReturnCode_t setupEEIKConstraintFromConf(std::map<std::string, IKConstraint>& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop);
         void calcCurrentState();
         void calcTorque();
+        void calcOdometry();
         void processTransition();
 };
 
diff --git a/rtc/HapticController/HapticControllerService_impl.cpp b/rtc/HapticController/HapticControllerService_impl.cpp
index 557080a74c9..897f147bc9d 100644
--- a/rtc/HapticController/HapticControllerService_impl.cpp
+++ b/rtc/HapticController/HapticControllerService_impl.cpp
@@ -33,6 +33,10 @@ CORBA::Boolean HapticControllerService_impl::resumeHapticController()
 {
     return m_hapticcontroller->resumeHapticController();
 };
+void HapticControllerService_impl::resetOdom()
+{
+  m_hapticcontroller->resetOdom();
+};
 //
 void HapticControllerService_impl::setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param)
 {
diff --git a/rtc/HapticController/HapticControllerService_impl.h b/rtc/HapticController/HapticControllerService_impl.h
index 0be6cbf64a6..11b9d1f60d9 100644
--- a/rtc/HapticController/HapticControllerService_impl.h
+++ b/rtc/HapticController/HapticControllerService_impl.h
@@ -20,6 +20,7 @@ class HapticControllerService_impl
   CORBA::Boolean stopHapticController();
   CORBA::Boolean pauseHapticController();
   CORBA::Boolean resumeHapticController();
+  void resetOdom();
   void setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param);
   void getParams(OpenHRP::HapticControllerService::HapticControllerParam_out i_param);
   //

From 083dc0bb228877feeef70a999c405036cf61fe75 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 18 Dec 2019 15:28:20 +0900
Subject: [PATCH 202/217] refactor

---
 rtc/HapticController/HapticController.cpp | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index 5b9875a78b2..d0eddfcf4a6 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -367,7 +367,6 @@ void HapticController::calcTorque(){
         for (auto leg : legs){
             const double floor_h_wld = m_robot->rootLink()->p(Z) - baselink_h_from_floor;
             const double foot_h_from_floor = master_ee_pose[leg].p(Z) - floor_h_wld;
-            // const double foot_h_from_floor = baselink_h_from_floor - (m_robot->rootLink()->p(Z) - master_ee_pose[leg].p(Z));
             if(foot_h_from_floor < 0){
                 hrp::dvector6 wrench = hrp::dvector6::Unit(fz) * (-foot_h_from_floor*hcp.floor_pd_gain(0) + (0-master_ee_vel_filtered[leg](fz))*hcp.floor_pd_gain(1));
                 LIMIT_NORM_V(wrench, 1000);
@@ -384,7 +383,7 @@ void HapticController::calcTorque(){
             const double wall_x_rel_base = -0.1;
             const double current_x = master_ee_pose[leg].p(X) - m_robot->rootLink()->p(X);
             if(current_x < wall_x_rel_base){
-                hrp::dvector6 wrench = (hrp::dvector6()<< (wall_x_rel_base - current_x) * 1000,0,0, 0,0,0).finished();
+                hrp::dvector6 wrench = (wall_x_rel_base - current_x) * 1000 * hrp::dvector6::Unit(fx);
                 LIMIT_NORM_V(wrench, 100);
                 hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
                 for(int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
@@ -396,7 +395,7 @@ void HapticController::calcTorque(){
         const double current_dist = master_ee_pose["lleg"].p(Y) - master_ee_pose["rleg"].p(Y);
         if(current_dist < hcp.foot_min_distance){
             for (auto leg : legs){
-                hrp::dvector6 wrench = (hrp::dvector6()<< 0, (leg=="lleg" ? 1:-1) * (hcp.foot_min_distance - current_dist) * 1000, 0,0,0,0).finished();
+                hrp::dvector6 wrench = (leg=="lleg" ? 1:-1) * (hcp.foot_min_distance - current_dist) * 1000 * hrp::dvector6::Unit(fy);
                 LIMIT_NORM_V(wrench, 50);
                 hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
                 for (int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }

From 55a241c70ecfdf420bd96d9b25c06582330166ee Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 18 Dec 2019 16:22:31 +0900
Subject: [PATCH 203/217] up

---
 rtc/HapticController/HapticController.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index d0eddfcf4a6..95d2949722d 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -411,6 +411,7 @@ void HapticController::calcTorque(){
             diff_pos = locked_l2r_pose.p - cur_l2r_pose.p;
             rats::difference_rotation(diff_rot, cur_l2r_pose.R, locked_l2r_pose.R);
             hrp::dvector6 rleg_wrench = (hrp::dvector6()<< diff_pos * 1000, diff_rot * 100).finished();
+            rleg_wrench(fz) = 0;
             LIMIT_NORM_V(rleg_wrench, 1000);
             for (auto leg : legs){
                 hrp::dvector6 wrench = (leg=="rleg" ? 1:-1) *  rleg_wrench;

From 8b22ca9e54024c641cb428fde5311e14d3d3f938 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 7 Jan 2020 15:15:32 +0900
Subject: [PATCH 204/217] slave act floor height feedbach

---
 rtc/AutoBalancer/MyUtils.h                    |   3 +
 rtc/HapticController/HapticController.cpp     |  41 +++---
 rtc/HapticController/HapticController.h       |  18 +++
 .../WholeBodyMasterSlave.cpp                  | 121 +++++++++---------
 .../WholeBodyMasterSlave.h                    |   5 -
 rtc/WholeBodyMasterSlave/wbms_core.h          |  33 +++--
 6 files changed, 129 insertions(+), 92 deletions(-)

diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index 5c7a8881a3f..082ca45e545 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -39,6 +39,9 @@ static const double Q_BUTTERWORTH = 0.707106781;
 #define LIMIT_MIN_V(v,minv)         (v= v.cwiseMax(minv))
 #define LIMIT_MAX_V(v,maxv)         (v= v.cwiseMin(maxv))
 #define LIMIT_MINMAX_V(v,minv,maxv) (v= v.cwiseMin(minv).cwiseMax(maxv))
+#define MIN_LIMITED(x,min)          ( x<min ? min : x )
+#define MAX_LIMITED(x,max)          ( x>max ? max : x )
+#define MINMAX_LIMITED(x,min,max)   ( x<min ? min : ( x>max ? max : x ))
 
 
 namespace hrp{
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index 95d2949722d..ad4d1922066 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -102,6 +102,14 @@ RTC::ReturnCode_t HapticController::onInitialize(){
         wrench_lpf[ee].reset(0);
     }
 
+    tgt_names = ee_names;
+    tgt_names.push_back("com");
+    tgt_names.push_back("head");
+    tgt_names.push_back("rhand");
+    tgt_names.push_back("lhand");
+    tgt_names.push_back("rfloor");
+    tgt_names.push_back("lfloor");
+
     for (auto ee : ee_names){
         std::string n = "slave_"+ee+"_wrench";
         m_slaveEEWrenchesIn[ee] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee]));
@@ -109,12 +117,6 @@ RTC::ReturnCode_t HapticController::onInitialize(){
         m_slaveEEWrenches[ee].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input
         RTC_INFO_STREAM(" registerInPort " << n );
     }
-
-    tgt_names = ee_names;
-    tgt_names.push_back("com");
-    tgt_names.push_back("head");
-    tgt_names.push_back("rhand");
-    tgt_names.push_back("lhand");
     for (auto tgt : tgt_names){
         std::string n = "master_"+tgt+"_pose";
         m_masterTgtPosesOut[tgt] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt]));
@@ -182,8 +184,8 @@ RTC::ReturnCode_t HapticController::onExecute(RTC::UniqueId ec_id){
     if(m_qActIn.isNew()                     ){ m_qActIn.read(); }
     if(m_dqActIn.isNew()                    ){ m_dqActIn.read(); }
     if(m_delayCheckPacketInboundIn.isNew()  ){ m_delayCheckPacketInboundIn.read(); m_delayCheckPacketOutboundOut.write();}
-    for(auto port : m_slaveEEWrenchesIn){   if( port.second->isNew() ) { port.second->read(); } }
-    for(auto port : m_slaveTgtPosesIn){     if( port.second->isNew() ) { port.second->read(); } }
+    for(auto p : m_slaveEEWrenchesIn){   if( p.second->isNew() ) { p.second->read(); } }
+    for(auto p : m_slaveTgtPosesIn){     if( p.second->isNew() ) { p.second->read(); } }
     
     mode.update();
     processTransition();
@@ -287,6 +289,13 @@ void HapticController::calcCurrentState(){
         hrp::JointPath jp(m_robot->rootLink(), m_robot->link(ee_ikc_map[ee].target_link_name));
         jp.calcJacobian(J_ee[ee], ee_ikc_map[ee].localPos);
     }
+
+    for(auto leg : {"rleg","lleg"}){ //m_robot->rootLink()->p(Z)は0付近で更新されないのでworld座標系におけるfloor_hは-1.0とからへん
+        const double slave_act_floor_z = MINMAX_LIMITED(m_slaveTgtPoses[(leg=="lleg" ? "lfloor" : "rfloor")].data.position.z, 0, 0.3);
+        const double basic_floor_h_wld = m_robot->rootLink()->p(Z) - baselink_h_from_floor;
+        foot_h_from_floor[leg] = master_ee_pose[leg].p(Z) - (basic_floor_h_wld + slave_act_floor_z);
+    }
+
 }
 
 
@@ -365,17 +374,15 @@ void HapticController::calcTorque(){
 
         ///// virtual floor
         for (auto leg : legs){
-            const double floor_h_wld = m_robot->rootLink()->p(Z) - baselink_h_from_floor;
-            const double foot_h_from_floor = master_ee_pose[leg].p(Z) - floor_h_wld;
-            if(foot_h_from_floor < 0){
-                hrp::dvector6 wrench = hrp::dvector6::Unit(fz) * (-foot_h_from_floor*hcp.floor_pd_gain(0) + (0-master_ee_vel_filtered[leg](fz))*hcp.floor_pd_gain(1));
+            if(foot_h_from_floor[leg] < 0){
+                hrp::dvector6 wrench = hrp::dvector6::Unit(fz) * (-foot_h_from_floor[leg]*hcp.floor_pd_gain(0) + (0-master_ee_vel_filtered[leg](fz))*hcp.floor_pd_gain(1));
                 LIMIT_NORM_V(wrench, 1000);
                 const hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
                 for(int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
                 masterEEWrenches[leg] += wrench;
             }
-            is_contact_to_floor[leg] = (foot_h_from_floor < 0 + 0.03);
-            if(loop%1000==0){ dbg(foot_h_from_floor); }
+            is_contact_to_floor[leg] = (foot_h_from_floor[leg] < 0 + 0.03);
+            if(loop%1000==0){ dbg(foot_h_from_floor[leg]); }
         }
 
         ///// virtual back wall
@@ -528,7 +535,8 @@ void HapticController::calcOdometry(){
     const hrp::Pose3    pleg_to_base        = hrp::calcRelPose3 ( hrp::to_2DPlanePose3(master_ee_pose[parent_leg]),             hrp::to_2DPlanePose3(cur_base_pose)             );
     const hrp::Pose3    pleg_to_cleg        = hrp::calcRelPose3 ( hrp::to_2DPlanePose3(master_ee_pose[parent_leg]),             hrp::to_2DPlanePose3(master_ee_pose[child_leg]) );
     hrp::Pose3 base_pose_from_floor_origin  = hrp::applyRelPose3( hrp::to_2DPlanePose3(leg_pose_from_floor_origin[parent_leg]), hrp::to_2DPlanePose3(pleg_to_base)              );
-    base_pose_from_floor_origin.p(Z)        = m_robot->rootLink()->p(Z) - std::min(master_ee_pose["rleg"].p(Z), master_ee_pose["lleg"].p(Z)); // ajust lower foot on to the z=0 in the world.
+    const double floating_h                 = MIN_LIMITED(std::min(foot_h_from_floor["rleg"], foot_h_from_floor["lleg"]), 0); 
+    base_pose_from_floor_origin.p(Z)        = baselink_h_from_floor - floating_h; // ajust not to float with both feet.
     leg_pose_from_floor_origin[child_leg]   = hrp::applyRelPose3( hrp::to_2DPlanePose3(leg_pose_from_floor_origin[parent_leg]), hrp::to_2DPlanePose3(pleg_to_cleg)              );
 
     //update base link pos from world
@@ -539,7 +547,7 @@ void HapticController::calcOdometry(){
     m_teleopOdom.data   = hrp::to_Pose3D(hrp::getLinkPose3(m_robot_odom->rootLink()));
     m_teleopOdom.tm     = m_qRef.tm;
     m_teleopOdomOut.write();
-    m_masterTgtPoses["com"].data = hrp::to_Pose3D(hrp::Pose3(m_robot_odom->calcCM(), hrp::Matrix33::Identity()));
+    m_masterTgtPoses["com"].data = hrp::to_Pose3D(hrp::Pose3(m_robot_odom->calcCM(), m_robot_odom->rootLink()->R));
     for (auto ee : ee_names){
         m_masterTgtPoses[ee].data = hrp::to_Pose3D(ee_ikc_map[ee].getCurrentTargetPose(m_robot_odom));//四肢揃ってないと危険
     }
@@ -650,6 +658,7 @@ bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticC
             hcp.ex_ee_ref_wrench[ee_names[i]](j) =  i_param.ex_ee_ref_wrench[i][j];
         }
     }
+    hcp.CheckSafeLimit();
     ///// update process if required
     baselink_h_ip->setGoal(&hcp.baselink_height_from_floor, 5.0, true);
     dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index 6e3d9053579..67b1a7cd8e8 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -126,6 +126,7 @@ class HapticController : public RTC::DataFlowComponentBase{
         std::map<std::string, hrp::JointPath> jpath_ee;
         std::map<std::string, hrp::dmatrix> J_ee;
         std::map<std::string, bool> is_contact_to_floor;
+        std::map<std::string, double> foot_h_from_floor;
         bool resetOdom_request;
 
         ControlMode mode;
@@ -178,6 +179,23 @@ class HapticController : public RTC::DataFlowComponentBase{
                 ex_ee_ref_wrench["lleg"]            = hrp::dvector6::Zero();
                 ex_ee_ref_wrench["rarm"]            = hrp::dvector6::Zero();
                 ex_ee_ref_wrench["larm"]            = hrp::dvector6::Zero();
+                CheckSafeLimit();
+            }
+            void CheckSafeLimit(){
+                LIMIT_MINMAX(baselink_height_from_floor          , 0.5, 3);
+                LIMIT_MINMAX(dqAct_filter_cutoff_hz              , 0, 500);
+                LIMIT_MINMAX(ee_vel_filter_cutoff_hz             , 0, 500);
+                LIMIT_MINMAX(ex_gravity_compensation_ratio_lower , -1, 2);
+                LIMIT_MINMAX(ex_gravity_compensation_ratio_upper , -1, 2);
+                LIMIT_MINMAX(foot_min_distance                   , 0, 1);
+                LIMIT_MINMAX(force_feedback_ratio                , 0, 2);
+                LIMIT_MINMAX(gravity_compensation_ratio          , 0, 2);
+                LIMIT_MINMAX(q_friction_coeff                    , 0, 0.1);
+                LIMIT_MINMAX(q_ref_max_torque_ratio              , 0, 1);
+                LIMIT_MINMAX(wrench_hpf_cutoff_hz                , 0, 500);
+                LIMIT_MINMAX(wrench_lpf_cutoff_hz                , 0, 500);
+                LIMIT_MINMAX(wrench_hpf_gain                     , 0, 2);
+                LIMIT_MINMAX(wrench_lpf_gain                     , 0, 2);
             }
         } hcp;
 
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 536207840f9..b4cecc7332d 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -148,59 +148,50 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     ref_zmp_filter.resize(XYZ);
     ref_zmp_filter.setParameter(100, 1/m_dt, Q_BUTTERWORTH);
 
-    cp_flag = -1;
-    lt.fill(0);
-    rt.fill(0);
-
-    to_enum["lleg"] = lf;
-    to_enum["rleg"] = rf;
-    to_enum["larm"] = lh;
-    to_enum["rarm"] = rh;
-    to_enum["com"] =  com;
-    to_enum["head"] = head;
-
     tgt_names = ee_names;
     tgt_names.push_back("com");
     tgt_names.push_back("head");
     tgt_names.push_back("rhand");
     tgt_names.push_back("lhand");
+    tgt_names.push_back("rfloor");
+    tgt_names.push_back("lfloor");
 
-    for ( int i=0; i<ee_names.size(); i++) {
-        std::string n = "slave_"+ee_names[i]+"_wrench";
-        m_slaveEEWrenchesOut[ee_names[i]] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee_names[i]]));
-        registerOutPort(n.c_str(), *m_slaveEEWrenchesOut[ee_names[i]]);
+    for (auto ee : ee_names) {
+        const std::string n = "slave_"+ee+"_wrench";
+        m_slaveEEWrenchesOut[ee] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee]));
+        registerOutPort(n.c_str(), *m_slaveEEWrenchesOut[ee]);
         RTC_INFO_STREAM(" registerOutPort " << n);
     }
-    for ( int i=0; i<ee_names.size(); i++) {
-        std::string n = "master_"+ee_names[i]+"_wrench";
-        m_masterEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_masterEEWrenches[ee_names[i]]));
-        registerInPort(n.c_str(), *m_masterEEWrenchesIn[ee_names[i]]);
+    for (auto ee : ee_names) {
+        const std::string n = "master_"+ee+"_wrench";
+        m_masterEEWrenchesIn[ee] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_masterEEWrenches[ee]));
+        registerInPort(n.c_str(), *m_masterEEWrenchesIn[ee]);
         RTC_INFO_STREAM(" registerInPort " << n);
     }
     ///////////////////
-    for ( int i=0; i<tgt_names.size(); i++) {
-        std::string n = "master_"+tgt_names[i]+"_pose";
-        m_masterTgtPosesIn[tgt_names[i]] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
-        registerInPort(n.c_str(), *m_masterTgtPosesIn[tgt_names[i]]);
+    for (auto tgt : tgt_names) {
+        const std::string n = "master_"+tgt+"_pose";
+        m_masterTgtPosesIn[tgt] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt]));
+        registerInPort(n.c_str(), *m_masterTgtPosesIn[tgt]);
         RTC_INFO_STREAM(" registerInPort " << n);
     }
-    for ( int i=0; i<tgt_names.size(); i++) {
-        std::string n = "slave_"+tgt_names[i]+"_pose";
-        m_slaveTgtPosesOut[tgt_names[i]] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_slaveTgtPoses[tgt_names[i]]));
-        registerOutPort(n.c_str(), *m_slaveTgtPosesOut[tgt_names[i]]);
+    for (auto tgt : tgt_names) {
+        const std::string n = "slave_"+tgt+"_pose";
+        m_slaveTgtPosesOut[tgt] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_slaveTgtPoses[tgt]));
+        registerOutPort(n.c_str(), *m_slaveTgtPosesOut[tgt]);
         RTC_INFO_STREAM(" registerOutPort " << n);
     }
     //////////////
-    for ( int i=0; i<ee_names.size(); i++) {
-        std::string n = "local_"+ee_names[i]+"_wrench";
-        m_localEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_localEEWrenches[ee_names[i]]));
-        registerInPort(n.c_str(), *m_localEEWrenchesIn[ee_names[i]]);
+    for (auto ee : ee_names) {
+        const std::string n = "local_"+ee+"_wrench";
+        m_localEEWrenchesIn[ee] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_localEEWrenches[ee]));
+        registerInPort(n.c_str(), *m_localEEWrenchesIn[ee]);
         RTC_INFO_STREAM(" registerInPort " << n);
     }
 
-    for(auto een : ee_names){
-        ee_f_filter[een].resize(XYZ);
-        ee_f_filter[een].setParameter(1, 1/m_dt, Q_BUTTERWORTH);
+    for(auto ee : ee_names){
+        ee_f_filter[ee].resize(XYZ);
+        ee_f_filter[ee].setParameter(1, 1/m_dt, Q_BUTTERWORTH);
     }
 
     RTC_INFO_STREAM("onInitialize() OK");
@@ -250,7 +241,9 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     if(m_zmpIn.isNew()                      ){ m_zmpIn.read(); }
     if(m_optionalDataIn.isNew()             ){ m_optionalDataIn.read(); }
     if(m_delayCheckPacketInboundIn.isNew()  ){ m_delayCheckPacketInboundIn.read(); m_delayCheckPacketOutboundOut.write();}
-    for(int i=0; i<ee_names.size(); i++){ if(m_localEEWrenchesIn[ee_names[i]]->isNew()){ m_localEEWrenchesIn[ee_names[i]]->read(); } }
+    for (auto ee : ee_names) {
+        if(m_localEEWrenchesIn[ee]->isNew()){ m_localEEWrenchesIn[ee]->read(); }
+    }
     wbms->act_rs.act_foot_wrench[0] = hrp::to_dvector(m_localEEWrenches["rleg"].data);
     wbms->act_rs.act_foot_wrench[1] = hrp::to_dvector(m_localEEWrenches["lleg"].data);
     wbms->act_rs.act_foot_pose[0] = ee_ikc_map["rleg"].getCurrentTargetPose(m_robot_vsafe);
@@ -260,28 +253,28 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     std::map<std::string, std::string> to_sname{{"lleg", "lfsensor"}, {"rleg", "rfsensor"}, {"larm", "lhsensor"}, {"rarm", "rhsensor"}};
     hrp::setQAll(m_robot_act, hrp::to_dvector(m_qAct.data));
     m_robot_act->calcForwardKinematics();
-    for(auto een : ee_names){
-        hrp::ForceSensor* sensor = m_robot_act->sensor<hrp::ForceSensor>(to_sname[een]);
+    for(auto ee : ee_names){
+        hrp::ForceSensor* sensor = m_robot_act->sensor<hrp::ForceSensor>(to_sname[ee]);
         hrp::Matrix33 sensorR_wld = sensor->link->R * sensor->localR;
         hrp::Matrix33 sensorR_from_base = m_robot_act->rootLink()->R.transpose() * sensorR_wld;
-        const hrp::Vector3 f_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[een].data).head(3);
-        const hrp::Vector3 t_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[een].data).tail(3);
-        const hrp::Vector3 sensor_to_ee_vec_wld = ee_ikc_map[een].getCurrentTargetPos(m_robot_act) - sensor->link->p;
+        const hrp::Vector3 f_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee].data).head(3);
+        const hrp::Vector3 t_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee].data).tail(3);
+        const hrp::Vector3 sensor_to_ee_vec_wld = ee_ikc_map[ee].getCurrentTargetPos(m_robot_act) - sensor->link->p;
         const hrp::Vector3 f_ee_wld = f_sensor_wld;
         const hrp::Vector3 t_ee_wld = t_sensor_wld - sensor_to_ee_vec_wld.cross(f_sensor_wld);
-        m_slaveEEWrenches[een].data = hrp::to_DoubleSeq( (hrp::dvector6()<<f_ee_wld,t_ee_wld).finished());
-        m_slaveEEWrenches[een].tm = m_qRef.tm;
-        m_slaveEEWrenchesOut[een]->write();
-        m_slaveTgtPoses[een].data = hrp::to_Pose3D(ee_ikc_map[een].getCurrentTargetPose(m_robot_act));
-        m_slaveTgtPoses[een].tm = m_qRef.tm;
-        m_slaveTgtPosesOut[een]->write();
+        m_slaveEEWrenches[ee].data = hrp::to_DoubleSeq( (hrp::dvector6()<<f_ee_wld,t_ee_wld).finished());
+        m_slaveEEWrenches[ee].tm = m_qRef.tm;
+        m_slaveEEWrenchesOut[ee]->write();
+        m_slaveTgtPoses[ee].data = hrp::to_Pose3D(ee_ikc_map[ee].getCurrentTargetPose(m_robot_act));
+        m_slaveTgtPoses[ee].tm = m_qRef.tm;
+        m_slaveTgtPosesOut[ee]->write();
     }
 
     static_balancing_com_offset.fill(0);
-    for(std::string een : {"larm","rarm"}){
-        const hrp::Vector3 use_f = hrp::Vector3::UnitZ() * m_slaveEEWrenches[een].data[Z];
-        hrp::Vector3 use_f_filtered = ee_f_filter[een].passFilter(use_f);
-        const hrp::Vector3 ee_pos_from_com = ee_ikc_map[een].getCurrentTargetPos(m_robot_vsafe) - wbms->rp_ref_out.tgt[com].abs.p;
+    for(auto ee : {"larm","rarm"}){
+        const hrp::Vector3 use_f = hrp::Vector3::UnitZ() * m_slaveEEWrenches[ee].data[Z];
+        hrp::Vector3 use_f_filtered = ee_f_filter[ee].passFilter(use_f);
+        const hrp::Vector3 ee_pos_from_com = ee_ikc_map[ee].getCurrentTargetPos(m_robot_vsafe) - wbms->rp_ref_out.tgt[com].abs.p;
         static_balancing_com_offset.head(XY) += - ee_pos_from_com.head(XY) * use_f_filtered(Z) / (-m_robot_vsafe->totalMass() * G);
     }
     if(loop%500==0)dbgv(static_balancing_com_offset);
@@ -301,19 +294,23 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         ex_data_index = hrp::to_string_vector(m_exDataIndex.data);
     }
 
-
-
     if( mode.now() != MODE_PAUSE ){ // stop updating input when MODE_PAUSE
-        for(int i=0; i<tgt_names.size(); i++){
-            if (m_masterTgtPosesIn[tgt_names[i]]->isNew()){
-                m_masterTgtPosesIn[tgt_names[i]]->read();
-                if(tgt_names[i] == "lhand" || tgt_names[i] == "rhand"){
-                    // nothind
+        for (auto tgt : tgt_names){
+            if (m_masterTgtPosesIn[tgt]->isNew()){
+                m_masterTgtPosesIn[tgt]->read();
+                if(tgt == "lhand" || tgt == "rhand" || tgt == "lfloor" || tgt == "rfloor" ){
+                    // nothing
                 }else{
-                    wbms->hp_wld_raw.tgt_by_str(tgt_names[i]).abs = hrp::to_Pose3(m_masterTgtPoses[tgt_names[i]].data);
+                    wbms->hp_wld_raw.tgt_by_str(tgt).abs = hrp::to_Pose3(m_masterTgtPoses[tgt].data);
                 }
             }
         }
+        for (auto ee : ee_names){
+            if (m_masterEEWrenchesIn[ee]->isNew()){
+                m_masterEEWrenchesIn[ee]->read();
+                wbms->hp_wld_raw.tgt_by_str(ee).w = hrp::to_dvector(m_masterEEWrenches[ee].data);
+            }
+        }
         if (m_actCPIn.isNew())  { m_actCPIn.read(); rel_act_cp = hrp::to_Vector3(m_actCP.data);}
         if (m_actZMPIn.isNew())  { m_actZMPIn.read(); rel_act_zmp = hrp::to_Vector3(m_actZMP.data);}
     }
@@ -394,6 +391,14 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = wbms->rp_ref_out.tgt[rf].is_contact();
         m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = wbms->rp_ref_out.tgt[lf].is_contact();
         addTimeReport("SetOutPut");
+
+        // send back aut detected floor height
+        for(std::string lr : {"l","r"}){
+            const hrp::Vector3 floor_pos = wbms->rp_ref_out.tgt_by_str(lr+"leg").cnt.p - wbms->rp_ref_out.tgt_by_str(lr+"leg").offs.p;
+            m_slaveTgtPoses[lr+"floor"].data    = hrp::to_Pose3D( (hrp::dvector6()<<floor_pos,0,0,0).finished());
+            m_slaveTgtPoses[lr+"floor"].tm      = m_qRef.tm;
+            m_slaveTgtPosesOut[lr+"floor"]->write();
+        }
     }
     wbms->baselinkpose.p = fik->m_robot->rootLink()->p;
     wbms->baselinkpose.R = fik->m_robot->rootLink()->R;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index 9b8a193aa8d..de1b622b8ae 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -134,8 +134,6 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
 
         WholeBodyMasterSlaveService_impl m_service0;
 
-        std::map<std::string, int> to_enum;
-
         RTC::CorbaPort m_AutoBalancerServicePort;
         RTC::CorbaPort m_StabilizerServicePort;
         RTC::CorbaConsumer<OpenHRP::AutoBalancerService> m_AutoBalancerServiceConsumer;
@@ -172,9 +170,6 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
 
         hrp::Vector3 rel_act_cp;
         hrp::Vector3 rel_act_zmp;
-        int cp_flag;
-        hrp::Vector3 lt,rt;
-        enum cp_enum{ CP_IDLE, CP_LF, CP_RF, CP_STATIC};
         struct timespec startT, endT;
         std::string time_report_str;
 
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 4bdcdf33494..f0d53df92b6 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -381,8 +381,8 @@ class WBMSCore{
                 std::vector<std::string> use_joints;
                 std::vector<std::string> use_targets;
             WBMSParams(){
-                auto_com_mode                       = false;
-                auto_foor_h_mode                         = true;
+                auto_com_mode                       = true;
+                auto_foor_h_mode                    = true;
                 additional_double_support_time      = 0.5;
                 auto_com_foot_move_detect_height    = 0.03;
                 auto_floor_h_detect_fz              = 50;
@@ -509,24 +509,31 @@ class WBMSCore{
     private:
         void convertHumanToRobot(const HumanPose& in, HumanPose& out){//結局初期指定からの移動量(=Rel)で計算をしてゆく
             //      out = in;//ダメゼッタイ
-            for(int i=0, l[6]={com,rf,lf,rh,lh,head}; i<6; i++){
-                out.tgt[l[i]].abs.p = wp.human_to_robot_ratio * (in.tgt[l[i]].abs.p - in.tgt[l[i]].offs.p) + out.tgt[l[i]].offs.p;
-                out.tgt[l[i]].abs.R = in.tgt[l[i]].abs.R * in.tgt[l[i]].offs.R.transpose() * out.tgt[l[i]].offs.R;
+            for(auto l : {com,rf,lf,rh,lh,head}){
+                out.tgt[l].abs.p = wp.human_to_robot_ratio * (in.tgt[l].abs.p - in.tgt[l].offs.p) + out.tgt[l].offs.p;
+                out.tgt[l].abs.R = in.tgt[l].abs.R * in.tgt[l].offs.R.transpose() * out.tgt[l].offs.R;
             }
-            for(int i=0, l[4]={rf,lf,rh,lh}; i<4; i++){
-                out.tgt[l[i]].w = in.tgt[l[i]].w;
-                out.tgt[l[i]].go_contact = in.tgt[l[i]].go_contact;
+            for(auto l : {rf,lf,rh,lh}){
+                out.tgt[l].w = in.tgt[l].w;
+                out.tgt[l].go_contact = in.tgt[l].go_contact;
             }
             out.tgt[com].abs.p += wp.com_offset;
             out.tgt[zmp].abs = in.tgt[zmp].abs;//最近使わない
         }
         void setAutoCOMMode(const HumanPose& human, const HumanPose& old, HumanPose& out){
+            // if(wp.auto_com_mode){
+            //     const bool rf_is_up = (human.tgt[rf].abs.p(Z) - human.tgt[rf].offs.p(Z) > wp.auto_com_foot_move_detect_height);
+            //     const bool lf_is_up = (human.tgt[lf].abs.p(Z) - human.tgt[lf].offs.p(Z) > wp.auto_com_foot_move_detect_height);
+            //     if      ( rf_is_up && !lf_is_up){ out.tgt[com].abs.p.head(XY) = old.tgt[lf].abs.p.head(XY); }
+            //     else if (!rf_is_up &&  lf_is_up){ out.tgt[com].abs.p.head(XY) = old.tgt[rf].abs.p.head(XY); }
+            //     else                            { out.tgt[com].abs.p.head(XY) = (old.tgt[rf].abs.p.head(XY) + old.tgt[lf].abs.p.head(XY)) / 2; }
+            // }
             if(wp.auto_com_mode){
-                const bool rf_is_up = (human.tgt[rf].abs.p(Z) - human.tgt[rf].offs.p(Z) > wp.auto_com_foot_move_detect_height);
-                const bool lf_is_up = (human.tgt[lf].abs.p(Z) - human.tgt[lf].offs.p(Z) > wp.auto_com_foot_move_detect_height);
-                if      ( rf_is_up && !lf_is_up){ out.tgt[com].abs.p.head(XY) = old.tgt[lf].abs.p.head(XY); }
-                else if (!rf_is_up &&  lf_is_up){ out.tgt[com].abs.p.head(XY) = old.tgt[rf].abs.p.head(XY); }
-                else                            { out.tgt[com].abs.p.head(XY) = (old.tgt[rf].abs.p.head(XY) + old.tgt[lf].abs.p.head(XY)) / 2; }
+                const double rf_h_from_floor = human.tgt[rf].abs.p(Z) - old.tgt[rf].cnt.p(Z);
+                const double lf_h_from_floor = human.tgt[lf].abs.p(Z) - old.tgt[lf].cnt.p(Z);
+                if      ( rf_h_from_floor - lf_h_from_floor >  wp.auto_com_foot_move_detect_height){ out.tgt[com].abs.p.head(XY) = old.tgt[lf].abs.p.head(XY); }
+                else if ( rf_h_from_floor - lf_h_from_floor < -wp.auto_com_foot_move_detect_height){ out.tgt[com].abs.p.head(XY) = old.tgt[rf].abs.p.head(XY); }
+                else                       { out.tgt[com].abs.p.head(XY) = (old.tgt[rf].abs.p.head(XY) + old.tgt[lf].abs.p.head(XY)) / 2; }
             }
         }
         void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){

From 3287138965b908de4a812ae62de7f9d795c71253 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 7 Jan 2020 23:01:20 +0900
Subject: [PATCH 205/217] up

---
 idl/WholeBodyMasterSlaveService.idl           |  2 ++
 .../WholeBodyMasterSlave.cpp                  | 20 ++++++++++------
 rtc/WholeBodyMasterSlave/wbms_core.h          | 24 ++++++++++++-------
 3 files changed, 30 insertions(+), 16 deletions(-)

diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
index 5eb4230b1b0..0a5d1702d06 100644
--- a/idl/WholeBodyMasterSlaveService.idl
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -20,6 +20,8 @@ module OpenHRP
     {
         boolean auto_com_mode;
         boolean auto_foor_h_mode;
+        boolean auto_foot_landing_by_act_cp;
+        boolean auto_foot_landing_by_act_zmp;
         double additional_double_support_time;
         double auto_com_foot_move_detect_height;
         double auto_floor_h_detect_fz;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index b4cecc7332d..0bb5529d1a4 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -392,16 +392,16 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = wbms->rp_ref_out.tgt[lf].is_contact();
         addTimeReport("SetOutPut");
 
-        // send back aut detected floor height
-        for(std::string lr : {"l","r"}){
-            const hrp::Vector3 floor_pos = wbms->rp_ref_out.tgt_by_str(lr+"leg").cnt.p - wbms->rp_ref_out.tgt_by_str(lr+"leg").offs.p;
-            m_slaveTgtPoses[lr+"floor"].data    = hrp::to_Pose3D( (hrp::dvector6()<<floor_pos,0,0,0).finished());
-            m_slaveTgtPoses[lr+"floor"].tm      = m_qRef.tm;
-            m_slaveTgtPosesOut[lr+"floor"]->write();
-        }
     }
     wbms->baselinkpose.p = fik->m_robot->rootLink()->p;
     wbms->baselinkpose.R = fik->m_robot->rootLink()->R;
+    // send back auto detected floor height
+    for(std::string lr : {"l","r"}){
+        const hrp::Vector3 floor_pos = wbms->rp_ref_out.tgt_by_str(lr+"leg").cnt.p - wbms->rp_ref_out.tgt_by_str(lr+"leg").offs.p;
+        m_slaveTgtPoses[lr+"floor"].data    = hrp::to_Pose3D( (hrp::dvector6()<<floor_pos,0,0,0).finished());
+        m_slaveTgtPoses[lr+"floor"].tm      = m_qRef.tm;
+        m_slaveTgtPosesOut[lr+"floor"]->write();
+    }
 
     // write
     m_qOut.write();
@@ -755,6 +755,9 @@ namespace hrp{
 bool WholeBodyMasterSlave::setParams(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
     RTC_INFO_STREAM("setWholeBodyMasterSlaveParam");
     wbms->wp.auto_com_mode                      = i_param.auto_com_mode;
+    wbms->wp.auto_foor_h_mode                   = i_param.auto_foor_h_mode;
+    wbms->wp.auto_foot_landing_by_act_cp        = i_param.auto_foot_landing_by_act_cp;
+    wbms->wp.auto_foot_landing_by_act_zmp       = i_param.auto_foot_landing_by_act_zmp;
     wbms->wp.additional_double_support_time     = i_param.additional_double_support_time;
     wbms->wp.auto_com_foot_move_detect_height   = i_param.auto_com_foot_move_detect_height;
     wbms->wp.auto_floor_h_detect_fz             = i_param.auto_floor_h_detect_fz;
@@ -780,6 +783,9 @@ bool WholeBodyMasterSlave::setParams(const OpenHRP::WholeBodyMasterSlaveService:
 bool WholeBodyMasterSlave::getParams(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
     RTC_INFO_STREAM("getWholeBodyMasterSlaveParam");
     i_param.auto_com_mode                       = wbms->wp.auto_com_mode;
+    i_param.auto_foor_h_mode                    = wbms->wp.auto_foor_h_mode;
+    i_param.auto_foot_landing_by_act_cp         = wbms->wp.auto_foot_landing_by_act_cp;
+    i_param.auto_foot_landing_by_act_zmp        = wbms->wp.auto_foot_landing_by_act_zmp;
     i_param.additional_double_support_time      = wbms->wp.additional_double_support_time;
     i_param.auto_com_foot_move_detect_height    = wbms->wp.auto_com_foot_move_detect_height;
     i_param.auto_floor_h_detect_fz              = wbms->wp.auto_floor_h_detect_fz;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index f0d53df92b6..fd0acd65a68 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -362,6 +362,8 @@ class WBMSCore{
             public:
                 bool auto_com_mode;
                 bool auto_foor_h_mode;
+                bool auto_foot_landing_by_act_cp;
+                bool auto_foot_landing_by_act_zmp;
                 double additional_double_support_time;
                 double auto_com_foot_move_detect_height;
                 double auto_floor_h_detect_fz;
@@ -383,6 +385,8 @@ class WBMSCore{
             WBMSParams(){
                 auto_com_mode                       = true;
                 auto_foor_h_mode                    = true;
+                auto_foot_landing_by_act_cp         = false;
+                auto_foot_landing_by_act_zmp        = true;
                 additional_double_support_time      = 0.5;
                 auto_com_foot_move_detect_height    = 0.03;
                 auto_floor_h_detect_fz              = 50;
@@ -544,16 +548,18 @@ class WBMSCore{
                 ///// lock by ref_zmp
                 ref_zmp_from_foot[lr] = act_rs.ref_zmp.head(XY) - old.foot(lr).abs.p.head(XY);
                 out.foot(OPPOSITE(lr)).go_contact |= ( ref_zmp_from_foot[lr].dot(to_opposite_foot[lr]) / to_opposite_foot[lr].norm() > wp.single_foot_zmp_safety_distance );
-                ///// lock by acts_zmp
-                act_zmp_from_foot[lr] = act_rs.st_zmp.head(XY) - old.foot(lr).abs.p.head(XY);
-                if( act_zmp_from_foot[lr].dot(to_opposite_foot[lr]) / to_opposite_foot[lr].norm() > wp.single_foot_zmp_safety_distance*9999 ){
-                    zmp_force_go_contact_count[OPPOSITE(lr)] = 1;
-                }else if(zmp_force_go_contact_count[OPPOSITE(lr)] > 0 && zmp_force_go_contact_count[OPPOSITE(lr)] < HZ * wp.additional_double_support_time ){
-                    zmp_force_go_contact_count[OPPOSITE(lr)]++;
-                }else{
-                    zmp_force_go_contact_count[OPPOSITE(lr)] = 0;
+                ///// lock by act_zmp
+                if(wp.auto_foot_landing_by_act_zmp){
+                    act_zmp_from_foot[lr] = act_rs.st_zmp.head(XY) - old.foot(lr).abs.p.head(XY);
+                    if( act_zmp_from_foot[lr].dot(to_opposite_foot[lr]) / to_opposite_foot[lr].norm() > wp.single_foot_zmp_safety_distance ){
+                        zmp_force_go_contact_count[OPPOSITE(lr)] = 1;
+                    }else if(zmp_force_go_contact_count[OPPOSITE(lr)] > 0 && zmp_force_go_contact_count[OPPOSITE(lr)] < HZ * wp.additional_double_support_time ){
+                        zmp_force_go_contact_count[OPPOSITE(lr)]++;
+                    }else{
+                        zmp_force_go_contact_count[OPPOSITE(lr)] = 0;
+                    }
+                    out.foot(OPPOSITE(lr)).go_contact |= (zmp_force_go_contact_count[OPPOSITE(lr)] > 0);
                 }
-                out.foot(OPPOSITE(lr)).go_contact |= (zmp_force_go_contact_count[OPPOSITE(lr)] > 0);
                 ///// lock by ref cp
                 out.foot(OPPOSITE(lr)).go_contact |= cp_force_go_contact[OPPOSITE(lr)];
             }

From 4c33e9eb166f89001eabee9a2f37211715960f6d Mon Sep 17 00:00:00 2001
From: JAXONRED <jaxonred@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 7 Jan 2020 23:03:17 +0900
Subject: [PATCH 206/217] tune neck limit

---
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 0bb5529d1a4..1e1a2ecbc04 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -615,10 +615,10 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
     if( fik->m_robot->link("CHEST_JOINT1") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT1")->jointId) = 1e3;
 //    if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 10;
     if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 0;//実機修理中
-//    if( fik->m_robot->link("HEAD_JOINT0") != NULL) fik->m_robot->link("HEAD_JOINT0")->llimit = deg2rad(-30);
-//    if( fik->m_robot->link("HEAD_JOINT0") != NULL) fik->m_robot->link("HEAD_JOINT0")->ulimit = deg2rad(30);
-//    if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->llimit = deg2rad(-45);
-//    if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->ulimit = deg2rad(10);
+    if( fik->m_robot->link("HEAD_JOINT0") != NULL) fik->m_robot->link("HEAD_JOINT0")->llimit = deg2rad(-20);
+    if( fik->m_robot->link("HEAD_JOINT0") != NULL) fik->m_robot->link("HEAD_JOINT0")->ulimit = deg2rad(20);
+    if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->llimit = deg2rad(-15);
+    if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->ulimit = deg2rad(35);
     if( fik->m_robot->link("RARM_JOINT2") != NULL) fik->m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-45);//脇内側の干渉回避
     if( fik->m_robot->link("LARM_JOINT2") != NULL) fik->m_robot->link("LARM_JOINT2")->llimit = deg2rad(45);
     if( fik->m_robot->link("RLEG_JOINT3") != NULL) fik->m_robot->link("RLEG_JOINT3")->llimit = deg2rad(40);//膝伸びきり防止のため

From af3c8b5b1c24e3b8cf51c4409018c262c0a21995 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 8 Jan 2020 18:57:04 +0900
Subject: [PATCH 207/217] up

---
 .../WholeBodyMasterSlave.cpp                  | 23 ++++++++++++++++++-
 1 file changed, 22 insertions(+), 1 deletion(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 1e1a2ecbc04..77585d09772 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -136,7 +136,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     q_ip = new interpolator(fik->numStates(), m_dt, interpolator::CUBICSPLINE, 1); // or HOFFARBIB, QUINTICSPLINE
     q_ip->clear();
     if(fik->m_robot->name().find("JAXON") != std::string::npos){
-        avg_q_vel = hrp::dvector::Constant(fik->numStates(), 4.0); // all joint max avarage vel = 1.0 rad/s
+        avg_q_vel = hrp::dvector::Constant(fik->numStates(), 2.0); // all joint max avarage vel = 2.0 rad/s
+        avg_q_vel.head(12).fill(4.0); // leg
     }else{
         avg_q_vel = hrp::dvector::Constant(fik->numStates(), 1.0); // all joint max avarage vel = 1.0 rad/s
     }
@@ -615,10 +616,27 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
     if( fik->m_robot->link("CHEST_JOINT1") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT1")->jointId) = 1e3;
 //    if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 10;
     if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 0;//実機修理中
+
+    if( fik->m_robot->link("CHEST_JOINT0") != NULL) fik->m_robot->link("CHEST_JOINT0")->llimit = deg2rad(-8);
+    if( fik->m_robot->link("CHEST_JOINT0") != NULL) fik->m_robot->link("CHEST_JOINT0")->ulimit = deg2rad(8);
+    if( fik->m_robot->link("CHEST_JOINT1") != NULL) fik->m_robot->link("CHEST_JOINT1")->llimit = deg2rad(1);
+    if( fik->m_robot->link("CHEST_JOINT1") != NULL) fik->m_robot->link("CHEST_JOINT1")->ulimit = deg2rad(32);
+
     if( fik->m_robot->link("HEAD_JOINT0") != NULL) fik->m_robot->link("HEAD_JOINT0")->llimit = deg2rad(-20);
     if( fik->m_robot->link("HEAD_JOINT0") != NULL) fik->m_robot->link("HEAD_JOINT0")->ulimit = deg2rad(20);
     if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->llimit = deg2rad(-15);
     if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->ulimit = deg2rad(35);
+
+    if( fik->m_robot->link("RARM_JOINT6") != NULL) fik->m_robot->link("RARM_JOINT6")->llimit = deg2rad(-59);
+    if( fik->m_robot->link("RARM_JOINT6") != NULL) fik->m_robot->link("RARM_JOINT6")->ulimit = deg2rad(59);
+    if( fik->m_robot->link("RARM_JOINT7") != NULL) fik->m_robot->link("RARM_JOINT7")->llimit = deg2rad(-61);
+    if( fik->m_robot->link("RARM_JOINT7") != NULL) fik->m_robot->link("RARM_JOINT7")->ulimit = deg2rad(58);
+
+    if( fik->m_robot->link("LARM_JOINT6") != NULL) fik->m_robot->link("LARM_JOINT6")->llimit = deg2rad(-59);
+    if( fik->m_robot->link("LARM_JOINT6") != NULL) fik->m_robot->link("LARM_JOINT6")->ulimit = deg2rad(59);
+    if( fik->m_robot->link("LARM_JOINT7") != NULL) fik->m_robot->link("LARM_JOINT7")->llimit = deg2rad(-61);
+    if( fik->m_robot->link("LARM_JOINT7") != NULL) fik->m_robot->link("LARM_JOINT7")->ulimit = deg2rad(58);
+
     if( fik->m_robot->link("RARM_JOINT2") != NULL) fik->m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-45);//脇内側の干渉回避
     if( fik->m_robot->link("LARM_JOINT2") != NULL) fik->m_robot->link("LARM_JOINT2")->llimit = deg2rad(45);
     if( fik->m_robot->link("RLEG_JOINT3") != NULL) fik->m_robot->link("RLEG_JOINT3")->llimit = deg2rad(40);//膝伸びきり防止のため
@@ -689,6 +707,9 @@ void WholeBodyMasterSlave::smoothingJointAngles(hrp::BodyPtr _robot, hrp::BodyPt
     ans_state_vel = Eigen::Map<hrp::dvector>(tmpv, fik->numStates());
 
     hrp::setRobotStateVec(_robot_safe, ans_state);
+    for(int i=0; i<_robot_safe->numJoints(); i++){
+        LIMIT_MINMAX(_robot_safe->joint(i)->q, _robot_safe->joint(i)->llimit, _robot_safe->joint(i)->ulimit);
+    }
 
     _robot_safe->calcForwardKinematics();
 }

From f8ad0bc3449eec5479540dd0be33a404a2beb207 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 9 Jan 2020 15:07:57 +0900
Subject: [PATCH 208/217] foot_landing_vel

---
 idl/WholeBodyMasterSlaveService.idl           |  1 +
 .../WholeBodyMasterSlave.cpp                  |  3 +
 rtc/WholeBodyMasterSlave/wbms_core.h          | 80 ++++++++++---------
 3 files changed, 46 insertions(+), 38 deletions(-)

diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
index 0a5d1702d06..a9798cb8177 100644
--- a/idl/WholeBodyMasterSlaveService.idl
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -30,6 +30,7 @@ module OpenHRP
         double capture_point_extend_ratio;
         double com_filter_cutoff_hz;
         double foot_collision_avoidance_distance;
+        double foot_landing_vel;
         double human_to_robot_ratio;
         double max_double_support_width;
         double upper_body_rmc_ratio;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 77585d09772..70a7054152e 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -787,6 +787,7 @@ bool WholeBodyMasterSlave::setParams(const OpenHRP::WholeBodyMasterSlaveService:
     wbms->wp.capture_point_extend_ratio         = i_param.capture_point_extend_ratio;
     wbms->wp.com_filter_cutoff_hz               = i_param.com_filter_cutoff_hz;
     wbms->wp.foot_collision_avoidance_distance  = i_param.foot_collision_avoidance_distance;
+    wbms->wp.foot_landing_vel                   = i_param.foot_landing_vel;
     wbms->wp.human_to_robot_ratio               = i_param.human_to_robot_ratio;
     wbms->wp.max_double_support_width           = i_param.max_double_support_width;
     wbms->wp.upper_body_rmc_ratio               = i_param.upper_body_rmc_ratio;
@@ -797,6 +798,7 @@ bool WholeBodyMasterSlave::setParams(const OpenHRP::WholeBodyMasterSlaveService:
     wbms->wp.safety_foot_vert_fbio              = hrp::Vector4::Map(i_param.safety_foot_vert_fbio.get_buffer());
     wbms->wp.use_joints                         = hrp::to_string_vector(i_param.use_joints);
     wbms->wp.use_targets                        = hrp::to_string_vector(i_param.use_targets);
+    wbms->wp.CheckSafeLimit();
     return true;
 }
 
@@ -815,6 +817,7 @@ bool WholeBodyMasterSlave::getParams(OpenHRP::WholeBodyMasterSlaveService::Whole
     i_param.capture_point_extend_ratio          = wbms->wp.capture_point_extend_ratio;
     i_param.com_filter_cutoff_hz                = wbms->wp.com_filter_cutoff_hz;
     i_param.foot_collision_avoidance_distance   = wbms->wp.foot_collision_avoidance_distance;
+    i_param.foot_landing_vel                    = wbms->wp.foot_landing_vel;
     i_param.human_to_robot_ratio                = wbms->wp.human_to_robot_ratio;
     i_param.max_double_support_width            = wbms->wp.max_double_support_width;
     i_param.upper_body_rmc_ratio                = wbms->wp.upper_body_rmc_ratio;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index fd0acd65a68..882a97b36cf 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -372,6 +372,7 @@ class WBMSCore{
                 double capture_point_extend_ratio;
                 double com_filter_cutoff_hz;
                 double foot_collision_avoidance_distance;
+                double foot_landing_vel;
                 double human_to_robot_ratio;
                 double max_double_support_width;
                 double upper_body_rmc_ratio;
@@ -387,7 +388,7 @@ class WBMSCore{
                 auto_foor_h_mode                    = true;
                 auto_foot_landing_by_act_cp         = false;
                 auto_foot_landing_by_act_zmp        = true;
-                additional_double_support_time      = 0.5;
+                additional_double_support_time      = 0.2;
                 auto_com_foot_move_detect_height    = 0.03;
                 auto_floor_h_detect_fz              = 50;
                 auto_floor_h_reset_fz               = 30;
@@ -395,6 +396,7 @@ class WBMSCore{
                 capture_point_extend_ratio          = 1.0;
                 com_filter_cutoff_hz                = 1.2;
                 foot_collision_avoidance_distance   = 0.16;
+                foot_landing_vel                    = 0.4;
                 human_to_robot_ratio                = 1.0;//human 1.1m vs jaxon 1.06m
                 max_double_support_width            = 0.4;
                 upper_body_rmc_ratio                = 0.0;
@@ -403,6 +405,23 @@ class WBMSCore{
                 com_offset                          << 0, 0, 0;
                 actual_foot_vert_fbio               << 0.13, -0.10,  0.06, -0.08;
                 safety_foot_vert_fbio               << 0.02, -0.02,  0.02,  0.01;
+                CheckSafeLimit();
+            }
+            void CheckSafeLimit(){
+                LIMIT_MINMAX(additional_double_support_time      , 0, 9);
+                LIMIT_MINMAX(auto_com_foot_move_detect_height    , 0, 1);
+                LIMIT_MINMAX(auto_floor_h_detect_fz              , 0, 1000);
+                LIMIT_MINMAX(auto_floor_h_reset_fz               , 0, 1000);
+                LIMIT_MINMAX(base_to_hand_min_distance           , 0, 1);
+                LIMIT_MINMAX(capture_point_extend_ratio          , 0, 10);
+                LIMIT_MINMAX(com_filter_cutoff_hz                , 0, 500);
+                LIMIT_MINMAX(foot_collision_avoidance_distance   , 0, 1);
+                LIMIT_MINMAX(foot_landing_vel                    , 0.01, 2);
+                LIMIT_MINMAX(human_to_robot_ratio                , 0, 10);
+                LIMIT_MINMAX(max_double_support_width            , 0, 2);
+                LIMIT_MINMAX(upper_body_rmc_ratio                , 0, 1);
+                LIMIT_MINMAX(single_foot_zmp_safety_distance     , 0, 0.2);
+                LIMIT_MINMAX(swing_foot_height_offset            , 0, 0.1);
             }
         } wp;
         struct ActualRobotState {
@@ -525,13 +544,6 @@ class WBMSCore{
             out.tgt[zmp].abs = in.tgt[zmp].abs;//最近使わない
         }
         void setAutoCOMMode(const HumanPose& human, const HumanPose& old, HumanPose& out){
-            // if(wp.auto_com_mode){
-            //     const bool rf_is_up = (human.tgt[rf].abs.p(Z) - human.tgt[rf].offs.p(Z) > wp.auto_com_foot_move_detect_height);
-            //     const bool lf_is_up = (human.tgt[lf].abs.p(Z) - human.tgt[lf].offs.p(Z) > wp.auto_com_foot_move_detect_height);
-            //     if      ( rf_is_up && !lf_is_up){ out.tgt[com].abs.p.head(XY) = old.tgt[lf].abs.p.head(XY); }
-            //     else if (!rf_is_up &&  lf_is_up){ out.tgt[com].abs.p.head(XY) = old.tgt[rf].abs.p.head(XY); }
-            //     else                            { out.tgt[com].abs.p.head(XY) = (old.tgt[rf].abs.p.head(XY) + old.tgt[lf].abs.p.head(XY)) / 2; }
-            // }
             if(wp.auto_com_mode){
                 const double rf_h_from_floor = human.tgt[rf].abs.p(Z) - old.tgt[rf].cnt.p(Z);
                 const double lf_h_from_floor = human.tgt[lf].abs.p(Z) - old.tgt[lf].cnt.p(Z);
@@ -618,7 +630,6 @@ class WBMSCore{
                 }else{
                     out.foot(lr).cnt.p.head(XY) = out.foot(lr).abs.p.head(XY);  
                     out.foot(lr).cnt.R = hrp::rotFromRpy(0, 0, hrp::rpyFromRot(out.foot(lr).abs.R)(y));  
-                    // LIMIT_MIN( out.foot(lr).abs.p(Z), out.foot(lr).offs.p(Z)+wp.swing_foot_height_offset);
                     LIMIT_MIN( out.foot(lr).abs.p(Z), out.foot(lr).cnt.p(Z)+wp.swing_foot_height_offset);
                 }
                 if(loop%500==0){
@@ -632,21 +643,21 @@ class WBMSCore{
         }
         void limitFootVelNearGround(const HumanPose& old, HumanPose& out){
             for(int lr=0; lr<LR; lr++){
-                const double fheight = old.foot(lr).abs.p(Z) - old.foot(lr).offs.p(Z);
+                const double fheight = old.foot(lr).abs.p(Z) - old.foot(lr).cnt.p(Z);
                 const double horizontal_max_vel = 0 + fheight * 20;
                 for(int j=0; j<XY; j++){
                     LIMIT_MINMAX( out.foot(lr).abs.p(j), old.foot(lr).abs.p(j) - horizontal_max_vel * DT, old.foot(lr).abs.p(j) + horizontal_max_vel * DT);
                 }
-                const double vertical_max_vel = 0 + fheight * 10;
-                LIMIT_MIN( out.foot(lr).abs.p(Z), old.foot(lr).abs.p(Z) - vertical_max_vel * DT);
+                // const double vertical_max_vel = 0 + fheight * 10;
+                LIMIT_MIN( out.foot(lr).abs.p(Z), old.foot(lr).abs.p(Z) - wp.foot_landing_vel * DT);
             }
         }
         void avoidFootSinkIntoFloor(HumanPose& out){
             for(int lr=0; lr<LR; lr++){
                 hrp::dmatrix act_sole_vert_abs = (out.foot(lr).abs.R * make_rect_3d(act_foot_vert_fblr[lr])).colwise() + out.foot(lr).abs.p;// translate and rotate each cols
                 const double min_height = act_sole_vert_abs.row(Z).minCoeff(); // pick up min Z height
-                if(min_height < out.foot(lr).offs.p(Z)){
-                    out.foot(lr).abs.p(Z) += (out.foot(lr).offs.p(Z) - min_height);
+                if(min_height < out.foot(lr).cnt.p(Z)){
+                    out.foot(lr).abs.p(Z) += (out.foot(lr).cnt.p(Z) - min_height);
                 }
             }
         }
@@ -661,35 +672,29 @@ class WBMSCore{
             com_ans = com_in;
             if (is_initial_loop)com_ans_old = com_in;
             hrp::Vector3 com_vel = (com_in - com_ans_old)/DT;
-            hrp::dmatrix hull_dcp = createSupportRegionByFootPos(rfin_abs, lfin_abs, offset_fblr(safe_foot_vert_fblr[R], 0.001),    offset_fblr(safe_foot_vert_fblr[L], 0.001));
-            hrp::dmatrix hull_acp = createSupportRegionByFootPos(rfin_abs, lfin_abs, offset_fblr(safe_foot_vert_fblr[R], 0.01),     offset_fblr(safe_foot_vert_fblr[L], 0.01));
-            hrp::Vector2 com_vel_ans_2d = regulateCOMVelocityByCapturePointVec( com_ans_old.head(XY), com_vel.head(XY), hull_dcp, hull_acp);
+            hrp::dmatrix hull_dcm = createSupportRegionByFootPos(rfin_abs, lfin_abs, offset_fblr(safe_foot_vert_fblr[R], 0.001),    offset_fblr(safe_foot_vert_fblr[L], 0.001));
+            hrp::dmatrix hull_ccm = createSupportRegionByFootPos(rfin_abs, lfin_abs, offset_fblr(safe_foot_vert_fblr[R], 0.01),     offset_fblr(safe_foot_vert_fblr[L], 0.01));
+            hrp::Vector2 com_vel_ans_2d = regulateCOMVelocityByCapturePointVec( com_ans_old.head(XY), com_vel.head(XY), hull_dcm, hull_ccm);
             com_ans.head(XY) = com_ans_old.head(XY) + com_vel_ans_2d * DT;
             com_ans_old = com_ans;
             cp_dec.head(XY) = com_ans.head(XY) + com_vel_ans_2d * sqrt( H_cur / G );
             cp_acc.head(XY) = com_ans.head(XY) - com_vel_ans_2d * sqrt( H_cur / G );
             return true;
         }
-        hrp::Vector2 regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const hrp::dmatrix& hull_d, const hrp::dmatrix& hull_a){
+        hrp::Vector2 regulateCOMVelocityByCapturePointVec(const hrp::Vector2& com_pos, const hrp::Vector2& com_vel, const hrp::dmatrix& hull_d, const hrp::dmatrix& hull_ccm){
             hrp::Vector2 com_vel_decel_ok, com_vel_accel_ok;
             com_vel_decel_ok = com_vel_accel_ok = com_vel;
 
-            const double foot_ave_vel = 2.0;
-            double lf_landing_delay = (rp_ref_out.tgt[lf].abs.p[Z] - rp_ref_out.tgt[lf].offs.p[Z]) / foot_ave_vel;
-            double rf_landing_delay = (rp_ref_out.tgt[rf].abs.p[Z] - rp_ref_out.tgt[rf].offs.p[Z]) / foot_ave_vel;
-            if(com_vel(Y)>0 ){ rf_landing_delay = 0; }
+            double lf_landing_delay = (rp_ref_out.tgt[lf].abs.p[Z] - rp_ref_out.tgt[lf].cnt.p[Z]) / wp.foot_landing_vel;
+            double rf_landing_delay = (rp_ref_out.tgt[rf].abs.p[Z] - rp_ref_out.tgt[rf].cnt.p[Z]) / wp.foot_landing_vel;
+            if(com_vel(Y)>0 ){ rf_landing_delay = 0; }//怪しい
             if(com_vel(Y)<0 ){ lf_landing_delay = 0; }
             double foot_landing_delay = std::max(rf_landing_delay, lf_landing_delay);
             //減速CP条件(現在のCPを常に両足裏で頭打ち)
-            //      hrp::Vector2 inside_vec_rf(0,1),inside_vec_lf(0,-1);//TODO implement
-            //      if(com_vel(Y)>1e-5 || fabs(com_vel(Y))<=1e-5 ){
-            //        foot_landing_delay = 0;
-            //      }
-            hrp::Vector2 cp_dec_ragulated = com_pos + com_vel * ( sqrt( H_cur / G ) + foot_landing_delay);
-            cp_dec_ragulated *= wp.capture_point_extend_ratio;
-            if(!isPointInHull2D(cp_dec_ragulated, hull_d)){
-                calcCrossPointOnHull(com_pos, cp_dec_ragulated, hull_d, cp_dec_ragulated);
-                com_vel_decel_ok = (cp_dec_ragulated - com_pos) / ( sqrt( H_cur / G ) + foot_landing_delay);
+            hrp::Vector2  dcm_ragulated = (com_pos + com_vel * ( sqrt( H_cur / G ) + foot_landing_delay)) * wp.capture_point_extend_ratio;
+            if(!isPointInHull2D(dcm_ragulated, hull_d)){
+                calcCrossPointOnHull(com_pos, dcm_ragulated, hull_d, dcm_ragulated);
+                com_vel_decel_ok = (dcm_ragulated - com_pos) / ( sqrt( H_cur / G ) + foot_landing_delay);
             }
             ///// ついでに減速CPによる強制遊脚着地も判定
             hrp::dmatrix foot_vert3d_check_wld[LR], one_foot_hull2d[LR], one_foot_vert_3d_for_check[LR];
@@ -697,14 +702,13 @@ class WBMSCore{
                 one_foot_vert_3d_for_check[lr]  = make_rect_3d(offset_fblr(safe_foot_vert_fblr[lr], 0.002));// 数値誤差で内外判定ずれるので少し大きい凸包で判定
                 foot_vert3d_check_wld[lr]       = (rp_ref_out.foot(lr).abs.R * one_foot_vert_3d_for_check[lr]).colwise() + rp_ref_out.foot(lr).abs.p; // rotate and translate in 3D
                 one_foot_hull2d[lr]             = makeConvexHull2D( foot_vert3d_check_wld[lr].topRows(XY) ); // project into 2D and make convex hull
-                cp_force_go_contact[OPPOSITE(lr)] = !isPointInHull2D(cp_dec_ragulated, one_foot_hull2d[lr]); // if CapturePoint go out of R sole region, L foot must be go contact
+                cp_force_go_contact[OPPOSITE(lr)] = !isPointInHull2D(dcm_ragulated, one_foot_hull2d[lr]); // if CapturePoint go out of R sole region, L foot must be go contact
             }
-            ///// 加速CP条件(ACP使用)
-            hrp::Vector2 cp_acc_ragulated = com_pos - com_vel * sqrt( H_cur / G );
-            cp_acc_ragulated *= wp.capture_point_extend_ratio;
-            if(!isPointInHull2D(cp_acc_ragulated, hull_a)){
-                calcCrossPointOnHull(com_pos, cp_acc_ragulated, hull_a, cp_acc_ragulated);
-                com_vel_accel_ok = (-cp_acc_ragulated + com_pos ) / sqrt( H_cur / G );
+            ///// 加速CP条件(CCM使用)
+            hrp::Vector2 ccm_ragulated = (com_pos - com_vel * sqrt( H_cur / G )) * wp.capture_point_extend_ratio;;
+            if(!isPointInHull2D(ccm_ragulated, hull_ccm)){
+                calcCrossPointOnHull(com_pos, ccm_ragulated, hull_ccm, ccm_ragulated);
+                com_vel_accel_ok = (-ccm_ragulated + com_pos ) / sqrt( H_cur / G );
             }
             ///// 加速減速条件マージ com_vel進行方向をより減速させるものを採用？
             return (com_vel_decel_ok.dot(com_vel) < com_vel_accel_ok.dot(com_vel)) ? com_vel_decel_ok : com_vel_accel_ok; //normじゃダメ？

From 22b3743fc634629d784675a6dccae68873cd55b9 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 11 Jan 2020 19:13:42 +0900
Subject: [PATCH 209/217] add button

---
 idl/WholeBodyMasterSlaveService.idl           |  3 +-
 .../WholeBodyMasterSlave.cpp                  | 66 +++++++++++++------
 rtc/WholeBodyMasterSlave/wbms_core.h          | 17 +++--
 3 files changed, 60 insertions(+), 26 deletions(-)

diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl
index a9798cb8177..6c52798c09d 100644
--- a/idl/WholeBodyMasterSlaveService.idl
+++ b/idl/WholeBodyMasterSlaveService.idl
@@ -19,7 +19,7 @@ module OpenHRP
     struct WholeBodyMasterSlaveParam
     {
         boolean auto_com_mode;
-        boolean auto_foor_h_mode;
+        boolean auto_floor_h_mode;
         boolean auto_foot_landing_by_act_cp;
         boolean auto_foot_landing_by_act_zmp;
         double additional_double_support_time;
@@ -31,6 +31,7 @@ module OpenHRP
         double com_filter_cutoff_hz;
         double foot_collision_avoidance_distance;
         double foot_landing_vel;
+        double force_double_support_com_h;
         double human_to_robot_ratio;
         double max_double_support_width;
         double upper_body_rmc_ratio;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 70a7054152e..9a9d158d982 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -136,7 +136,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     q_ip = new interpolator(fik->numStates(), m_dt, interpolator::CUBICSPLINE, 1); // or HOFFARBIB, QUINTICSPLINE
     q_ip->clear();
     if(fik->m_robot->name().find("JAXON") != std::string::npos){
-        avg_q_vel = hrp::dvector::Constant(fik->numStates(), 2.0); // all joint max avarage vel = 2.0 rad/s
+        avg_q_vel = hrp::dvector::Constant(fik->numStates(), 1.0);
         avg_q_vel.head(12).fill(4.0); // leg
     }else{
         avg_q_vel = hrp::dvector::Constant(fik->numStates(), 1.0); // all joint max avarage vel = 1.0 rad/s
@@ -312,11 +312,42 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
                 wbms->hp_wld_raw.tgt_by_str(ee).w = hrp::to_dvector(m_masterEEWrenches[ee].data);
             }
         }
-        if (m_actCPIn.isNew())  { m_actCPIn.read(); rel_act_cp = hrp::to_Vector3(m_actCP.data);}
-        if (m_actZMPIn.isNew())  { m_actZMPIn.read(); rel_act_zmp = hrp::to_Vector3(m_actZMP.data);}
+        if (m_actCPIn.isNew()){     m_actCPIn.read();   rel_act_cp  = hrp::to_Vector3(m_actCP.data);}
+        if (m_actZMPIn.isNew()){    m_actZMPIn.read();  rel_act_zmp = hrp::to_Vector3(m_actZMP.data);}
+    }else{
+        for (auto tgt : {"lhand", "rhand"}){ // PAUSE中もボタン読み込みのためにこれらだけは更新・・・
+            if (m_masterTgtPosesIn[tgt]->isNew()){ m_masterTgtPosesIn[tgt]->read(); }
+        }
     }
     addTimeReport("InPort");
 
+    ///// Button start
+    static int button1_flag_count = 0, button2_flag_count = 0;
+    if(m_masterTgtPoses["rhand"].data.position.x > 0 && m_masterTgtPoses["lhand"].data.position.x > 0  ){ button1_flag_count++; }else{ button1_flag_count = 0; }
+    if(m_masterTgtPoses["rhand"].data.position.x > 0 && m_masterTgtPoses["lhand"].data.position.x <= 0 ){ button2_flag_count++; }else{ button2_flag_count = 0; }
+    if(button1_flag_count > 2.0 / m_dt){// keep pless both button for 2s to start
+        if(mode.now() == MODE_IDLE){
+            RTC_INFO_STREAM("startWholeBodyMasterSlave by Button");
+            startWholeBodyMasterSlave();
+        }
+        if(mode.now() == MODE_WBMS || mode.now() == MODE_PAUSE ){
+            RTC_INFO_STREAM("stopWholeBodyMasterSlave by Button");
+            stopWholeBodyMasterSlave();
+        }
+        button1_flag_count = 0;
+    }
+    if(button2_flag_count > 2.0 / m_dt){// keep pless both button for 2s to start
+        if(mode.now() == MODE_WBMS){
+            RTC_INFO_STREAM("pauseWholeBodyMasterSlave by Button");
+            pauseWholeBodyMasterSlave();
+        }
+        if(mode.now() == MODE_PAUSE ){
+            RTC_INFO_STREAM("resumeWholeBodyMasterSlave by Button");
+            resumeWholeBodyMasterSlave();
+        }
+        button2_flag_count = 0;
+    }
+
     processTransition();
     mode.update();
 
@@ -336,18 +367,14 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         if(fik->m_robot->name() == "RHP4B"){
             const double hand_max_vel = M_PI/0.4;//0.4s for 180deg
             {
-                double trigger_in = m_masterTgtPoses["lhand"].data.position.y;
-                LIMIT_MINMAX(trigger_in, 0.0, 1.0);
+                const double trigger_in = MINMAX_LIMITED(m_masterTgtPoses["lhand"].data.position.y, 0, 1);
                 double tgt_hand_q = (-29.0 + (124.0-(-29.0))*trigger_in ) /180.0*M_PI;
-                LIMIT_MINMAX(tgt_hand_q, fik->m_robot->link("L_HAND")->llimit, fik->m_robot->link("L_HAND")->ulimit);
-                fik->m_robot->link("L_HAND")->q = tgt_hand_q;
+                fik->m_robot->link("L_HAND")->q = MINMAX_LIMITED(tgt_hand_q, fik->m_robot->link("L_HAND")->llimit, fik->m_robot->link("L_HAND")->ulimit);
                 avg_q_vel(fik->m_robot->link("L_HAND")->jointId) = hand_max_vel;
             }{
-                double trigger_in = m_masterTgtPoses["rhand"].data.position.y;
-                LIMIT_MINMAX(trigger_in, 0.0, 1.0);
+                const double trigger_in = MINMAX_LIMITED(m_masterTgtPoses["rhand"].data.position.y, 0, 1);
                 double tgt_hand_q = (-29.0 + (124.0-(-29.0))*trigger_in ) /180.0*M_PI;
-                LIMIT_MINMAX(tgt_hand_q, fik->m_robot->link("R_HAND")->llimit, fik->m_robot->link("R_HAND")->ulimit);
-                fik->m_robot->link("R_HAND")->q = tgt_hand_q;
+                fik->m_robot->link("R_HAND")->q = MINMAX_LIMITED(tgt_hand_q, fik->m_robot->link("R_HAND")->llimit, fik->m_robot->link("R_HAND")->ulimit);
                 avg_q_vel(fik->m_robot->link("R_HAND")->jointId) = hand_max_vel;
             }
         }
@@ -447,15 +474,10 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(){
     fik->m_robot->calcForwardKinematics();
     double current_foot_height_from_world = std::min(ee_ikc_map["rleg"].getCurrentTargetPos(fik->m_robot)(Z), ee_ikc_map["lleg"].getCurrentTargetPos(fik->m_robot)(Z));
     RTC_INFO_STREAM("current_foot_height_from_world = "<<current_foot_height_from_world<<" will be modified to 0");
-
-    std::vector<hrp::BodyPtr> body_list;
-    body_list.push_back(fik->m_robot);
-    body_list.push_back(m_robot_vsafe);
-    for(int i=0;i<body_list.size();i++){//初期姿勢でBodyをFK
-        hrp::setRobotStateVec(body_list[i], hrp::to_dvector(m_qRef.data), hrp::to_Vector3(m_basePos.data) - hrp::Vector3::UnitZ() * current_foot_height_from_world, hrp::rotFromRpy(hrp::to_Vector3(m_baseRpy.data)));
-        body_list[i]->calcForwardKinematics();
+    for(auto b : {fik->m_robot, m_robot_vsafe}){//初期姿勢でBodyをFK
+        hrp::setRobotStateVec(b, hrp::to_dvector(m_qRef.data), hrp::to_Vector3(m_basePos.data) - hrp::Vector3::UnitZ() * current_foot_height_from_world, hrp::rotFromRpy(hrp::to_Vector3(m_baseRpy.data)));
+        b->calcForwardKinematics();
     }
-
     fik->q_ref = hrp::getRobotStateVec(fik->m_robot);
     q_ip->set(fik->q_ref.data());
     wbms->initializeRequest(fik->m_robot, ee_ikc_map);
@@ -776,7 +798,7 @@ namespace hrp{
 bool WholeBodyMasterSlave::setParams(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
     RTC_INFO_STREAM("setWholeBodyMasterSlaveParam");
     wbms->wp.auto_com_mode                      = i_param.auto_com_mode;
-    wbms->wp.auto_foor_h_mode                   = i_param.auto_foor_h_mode;
+    wbms->wp.auto_floor_h_mode                  = i_param.auto_floor_h_mode;
     wbms->wp.auto_foot_landing_by_act_cp        = i_param.auto_foot_landing_by_act_cp;
     wbms->wp.auto_foot_landing_by_act_zmp       = i_param.auto_foot_landing_by_act_zmp;
     wbms->wp.additional_double_support_time     = i_param.additional_double_support_time;
@@ -788,6 +810,7 @@ bool WholeBodyMasterSlave::setParams(const OpenHRP::WholeBodyMasterSlaveService:
     wbms->wp.com_filter_cutoff_hz               = i_param.com_filter_cutoff_hz;
     wbms->wp.foot_collision_avoidance_distance  = i_param.foot_collision_avoidance_distance;
     wbms->wp.foot_landing_vel                   = i_param.foot_landing_vel;
+    wbms->wp.force_double_support_com_h         = i_param.force_double_support_com_h;
     wbms->wp.human_to_robot_ratio               = i_param.human_to_robot_ratio;
     wbms->wp.max_double_support_width           = i_param.max_double_support_width;
     wbms->wp.upper_body_rmc_ratio               = i_param.upper_body_rmc_ratio;
@@ -806,7 +829,7 @@ bool WholeBodyMasterSlave::setParams(const OpenHRP::WholeBodyMasterSlaveService:
 bool WholeBodyMasterSlave::getParams(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){
     RTC_INFO_STREAM("getWholeBodyMasterSlaveParam");
     i_param.auto_com_mode                       = wbms->wp.auto_com_mode;
-    i_param.auto_foor_h_mode                    = wbms->wp.auto_foor_h_mode;
+    i_param.auto_floor_h_mode                   = wbms->wp.auto_floor_h_mode;
     i_param.auto_foot_landing_by_act_cp         = wbms->wp.auto_foot_landing_by_act_cp;
     i_param.auto_foot_landing_by_act_zmp        = wbms->wp.auto_foot_landing_by_act_zmp;
     i_param.additional_double_support_time      = wbms->wp.additional_double_support_time;
@@ -818,6 +841,7 @@ bool WholeBodyMasterSlave::getParams(OpenHRP::WholeBodyMasterSlaveService::Whole
     i_param.com_filter_cutoff_hz                = wbms->wp.com_filter_cutoff_hz;
     i_param.foot_collision_avoidance_distance   = wbms->wp.foot_collision_avoidance_distance;
     i_param.foot_landing_vel                    = wbms->wp.foot_landing_vel;
+    i_param.force_double_support_com_h          = wbms->wp.force_double_support_com_h;
     i_param.human_to_robot_ratio                = wbms->wp.human_to_robot_ratio;
     i_param.max_double_support_width            = wbms->wp.max_double_support_width;
     i_param.upper_body_rmc_ratio                = wbms->wp.upper_body_rmc_ratio;
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 882a97b36cf..8a3e8852d92 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -361,7 +361,7 @@ class WBMSCore{
         class WBMSParams {
             public:
                 bool auto_com_mode;
-                bool auto_foor_h_mode;
+                bool auto_floor_h_mode;
                 bool auto_foot_landing_by_act_cp;
                 bool auto_foot_landing_by_act_zmp;
                 double additional_double_support_time;
@@ -373,6 +373,7 @@ class WBMSCore{
                 double com_filter_cutoff_hz;
                 double foot_collision_avoidance_distance;
                 double foot_landing_vel;
+                double force_double_support_com_h;
                 double human_to_robot_ratio;
                 double max_double_support_width;
                 double upper_body_rmc_ratio;
@@ -385,7 +386,7 @@ class WBMSCore{
                 std::vector<std::string> use_targets;
             WBMSParams(){
                 auto_com_mode                       = true;
-                auto_foor_h_mode                    = true;
+                auto_floor_h_mode                    = true;
                 auto_foot_landing_by_act_cp         = false;
                 auto_foot_landing_by_act_zmp        = true;
                 additional_double_support_time      = 0.2;
@@ -397,6 +398,7 @@ class WBMSCore{
                 com_filter_cutoff_hz                = 1.2;
                 foot_collision_avoidance_distance   = 0.16;
                 foot_landing_vel                    = 0.4;
+                force_double_support_com_h          = 0.9;
                 human_to_robot_ratio                = 1.0;//human 1.1m vs jaxon 1.06m
                 max_double_support_width            = 0.4;
                 upper_body_rmc_ratio                = 0.0;
@@ -417,6 +419,7 @@ class WBMSCore{
                 LIMIT_MINMAX(com_filter_cutoff_hz                , 0, 500);
                 LIMIT_MINMAX(foot_collision_avoidance_distance   , 0, 1);
                 LIMIT_MINMAX(foot_landing_vel                    , 0.01, 2);
+                LIMIT_MINMAX(force_double_support_com_h          , 0, 999);
                 LIMIT_MINMAX(human_to_robot_ratio                , 0, 10);
                 LIMIT_MINMAX(max_double_support_width            , 0, 2);
                 LIMIT_MINMAX(upper_body_rmc_ratio                , 0, 1);
@@ -550,6 +553,12 @@ class WBMSCore{
                 if      ( rf_h_from_floor - lf_h_from_floor >  wp.auto_com_foot_move_detect_height){ out.tgt[com].abs.p.head(XY) = old.tgt[lf].abs.p.head(XY); }
                 else if ( rf_h_from_floor - lf_h_from_floor < -wp.auto_com_foot_move_detect_height){ out.tgt[com].abs.p.head(XY) = old.tgt[rf].abs.p.head(XY); }
                 else                       { out.tgt[com].abs.p.head(XY) = (old.tgt[rf].abs.p.head(XY) + old.tgt[lf].abs.p.head(XY)) / 2; }
+                ///// lock if com height is low = half sitting
+                for(int lr=0; lr<LR; lr++){
+                    if(act_rs.ref_com(Z) - old.foot(lr).cnt.p(Z) < wp.force_double_support_com_h){
+                        out.tgt[com].abs.p.head(XY) = (old.tgt[rf].abs.p.head(XY) + old.tgt[lf].abs.p.head(XY)) / 2;
+                    }
+                }
             }
         }
         void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
@@ -602,14 +611,14 @@ class WBMSCore{
             static bool is_locked[LR] = {false};
             static int cnt_for_clear[LR] ={0};
             for(int lr=0; lr<LR; lr++){
-                if(wp.auto_foor_h_mode){
+                if(wp.auto_floor_h_mode){
                     // judge act contact height
                     if(!is_locked[lr] && fabs(act_rs.act_foot_wrench[lr](fz)) > wp.auto_floor_h_detect_fz){
                         out.foot(lr).cnt.p(Z) = act_rs.act_foot_pose[lr].p(Z);
                         is_locked[lr] = true;
                     }
                     if(is_locked[lr] && fabs(act_rs.act_foot_wrench[lr](fz)) < wp.auto_floor_h_reset_fz){
-                        if(cnt_for_clear[lr]++ > 1000){
+                        if(cnt_for_clear[lr]++ > HZ * 0.5){
                             out.foot(lr).cnt.p(Z) = out.foot(lr).offs.p(Z);
                             is_locked[lr] = false;
                             cnt_for_clear[lr] = 0;

From f7a23742c5a0fed9d3d42b4c49a5cff93863395e Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 13 Jan 2020 12:13:36 +0900
Subject: [PATCH 210/217] fix minor bugs

---
 .../WholeBodyMasterSlave.cpp                  | 36 ++++++++++++-------
 rtc/WholeBodyMasterSlave/wbms_core.h          | 18 +++++-----
 2 files changed, 32 insertions(+), 22 deletions(-)

diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index 9a9d158d982..e7d9c88fb1d 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -253,6 +253,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     // calc actual state
     std::map<std::string, std::string> to_sname{{"lleg", "lfsensor"}, {"rleg", "rfsensor"}, {"larm", "lhsensor"}, {"rarm", "rhsensor"}};
     hrp::setQAll(m_robot_act, hrp::to_dvector(m_qAct.data));
+    m_robot_act->rootLink()->p = m_robot_vsafe->rootLink()->p;
+    m_robot_act->rootLink()->R = m_robot_vsafe->rootLink()->R;
     m_robot_act->calcForwardKinematics();
     for(auto ee : ee_names){
         hrp::ForceSensor* sensor = m_robot_act->sensor<hrp::ForceSensor>(to_sname[ee]);
@@ -270,6 +272,9 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         m_slaveTgtPoses[ee].tm = m_qRef.tm;
         m_slaveTgtPosesOut[ee]->write();
     }
+    m_slaveTgtPoses["com"].data = hrp::to_Pose3D( (hrp::dvector6()<<m_robot_act->calcCM(),0,0,0).finished());
+    m_slaveTgtPoses["com"].tm = m_qRef.tm;
+    m_slaveTgtPosesOut["com"]->write();
 
     static_balancing_com_offset.fill(0);
     for(auto ee : {"larm","rarm"}){
@@ -279,9 +284,6 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
         static_balancing_com_offset.head(XY) += - ee_pos_from_com.head(XY) * use_f_filtered(Z) / (-m_robot_vsafe->totalMass() * G);
     }
     if(loop%500==0)dbgv(static_balancing_com_offset);
-    m_slaveTgtPoses["com"].data = hrp::to_Pose3D( (hrp::dvector6()<<m_robot_act->calcCM(),0,0,0).finished());
-    m_slaveTgtPoses["com"].tm = m_qRef.tm;
-    m_slaveTgtPosesOut["com"]->write();
 
     // button func
     hrp::dvector ex_data;
@@ -661,21 +663,29 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
 
     if( fik->m_robot->link("RARM_JOINT2") != NULL) fik->m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-45);//脇内側の干渉回避
     if( fik->m_robot->link("LARM_JOINT2") != NULL) fik->m_robot->link("LARM_JOINT2")->llimit = deg2rad(45);
+    if( fik->m_robot->link("RARM_JOINT2") != NULL) fik->m_robot->link("RARM_JOINT2")->llimit = deg2rad(-89);//肩グルン防止
+    if( fik->m_robot->link("LARM_JOINT2") != NULL) fik->m_robot->link("LARM_JOINT2")->ulimit = deg2rad(89);
+    if( fik->m_robot->link("RARM_JOINT4") != NULL) fik->m_robot->link("RARM_JOINT4")->ulimit = deg2rad(1);//肘逆折れ
+    if( fik->m_robot->link("LARM_JOINT4") != NULL) fik->m_robot->link("LARM_JOINT4")->ulimit = deg2rad(1);
     if( fik->m_robot->link("RLEG_JOINT3") != NULL) fik->m_robot->link("RLEG_JOINT3")->llimit = deg2rad(40);//膝伸びきり防止のため
     if( fik->m_robot->link("LLEG_JOINT3") != NULL) fik->m_robot->link("LLEG_JOINT3")->llimit = deg2rad(40);
 
     if( fik->m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_Y")->jointId) = 10;//K
     if( fik->m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_P")->jointId) = 10;
-    if( fik->m_robot->link("R_KNEE_P") != NULL) fik->m_robot->link("R_KNEE_P")->llimit = deg2rad(15);
-    if( fik->m_robot->link("L_KNEE_P") != NULL) fik->m_robot->link("L_KNEE_P")->llimit = deg2rad(15);
-    if( fik->m_robot->link("R_WRIST_R") != NULL) fik->m_robot->link("R_WRIST_R")->llimit = deg2rad(-40);
-    if( fik->m_robot->link("L_WRIST_R") != NULL) fik->m_robot->link("L_WRIST_R")->llimit = deg2rad(-40);
-    if( fik->m_robot->link("R_WRIST_R") != NULL) fik->m_robot->link("R_WRIST_R")->ulimit = deg2rad(40);
-    if( fik->m_robot->link("L_WRIST_R") != NULL) fik->m_robot->link("L_WRIST_R")->ulimit = deg2rad(40);
-    if( fik->m_robot->link("R_WRIST_P") != NULL) fik->m_robot->link("R_WRIST_P")->llimit = deg2rad(-40);
-    if( fik->m_robot->link("L_WRIST_P") != NULL) fik->m_robot->link("L_WRIST_P")->llimit = deg2rad(-40);
-    if( fik->m_robot->link("R_WRIST_P") != NULL) fik->m_robot->link("R_WRIST_P")->ulimit = deg2rad(20);
-    if( fik->m_robot->link("L_WRIST_P") != NULL) fik->m_robot->link("L_WRIST_P")->ulimit = deg2rad(20);
+    if( fik->m_robot->link("R_KNEE_P") != NULL) fik->m_robot->link("R_KNEE_P")->llimit = deg2rad(30);//膝伸びきり防止
+    if( fik->m_robot->link("L_KNEE_P") != NULL) fik->m_robot->link("L_KNEE_P")->llimit = deg2rad(30);
+    // if( fik->m_robot->link("R_WRIST_R") != NULL) fik->m_robot->link("R_WRIST_R")->llimit = deg2rad(-40);
+    // if( fik->m_robot->link("L_WRIST_R") != NULL) fik->m_robot->link("L_WRIST_R")->llimit = deg2rad(-40);
+    // if( fik->m_robot->link("R_WRIST_R") != NULL) fik->m_robot->link("R_WRIST_R")->ulimit = deg2rad(40);
+    // if( fik->m_robot->link("L_WRIST_R") != NULL) fik->m_robot->link("L_WRIST_R")->ulimit = deg2rad(40);
+    // if( fik->m_robot->link("R_WRIST_P") != NULL) fik->m_robot->link("R_WRIST_P")->llimit = deg2rad(-40);
+    // if( fik->m_robot->link("L_WRIST_P") != NULL) fik->m_robot->link("L_WRIST_P")->llimit = deg2rad(-40);
+    // if( fik->m_robot->link("R_WRIST_P") != NULL) fik->m_robot->link("R_WRIST_P")->ulimit = deg2rad(20);
+    // if( fik->m_robot->link("L_WRIST_P") != NULL) fik->m_robot->link("L_WRIST_P")->ulimit = deg2rad(20);
+    if( fik->m_robot->link("R_WRIST_P") != NULL) fik->m_robot->link("R_WRIST_P")->llimit = deg2rad(-80);
+    if( fik->m_robot->link("L_WRIST_P") != NULL) fik->m_robot->link("L_WRIST_P")->llimit = deg2rad(-80);
+    if( fik->m_robot->link("R_WRIST_P") != NULL) fik->m_robot->link("R_WRIST_P")->ulimit = deg2rad(45);
+    if( fik->m_robot->link("L_WRIST_P") != NULL) fik->m_robot->link("L_WRIST_P")->ulimit = deg2rad(45);
     if( fik->m_robot->link("CHEST_Y") != NULL) fik->m_robot->link("CHEST_Y")->llimit = deg2rad(-20);
     if( fik->m_robot->link("CHEST_Y") != NULL) fik->m_robot->link("CHEST_Y")->ulimit = deg2rad(20);
     if( fik->m_robot->link("CHEST_P") != NULL) fik->m_robot->link("CHEST_P")->llimit = deg2rad(0);
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 8a3e8852d92..7e708301a0f 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -386,16 +386,16 @@ class WBMSCore{
                 std::vector<std::string> use_targets;
             WBMSParams(){
                 auto_com_mode                       = true;
-                auto_floor_h_mode                    = true;
+                auto_floor_h_mode                   = false;
                 auto_foot_landing_by_act_cp         = false;
                 auto_foot_landing_by_act_zmp        = true;
-                additional_double_support_time      = 0.2;
+                additional_double_support_time      = 0.5;//0.4s以上は必須な気がする
                 auto_com_foot_move_detect_height    = 0.03;
                 auto_floor_h_detect_fz              = 50;
                 auto_floor_h_reset_fz               = 30;
                 base_to_hand_min_distance           = 0.5;
                 capture_point_extend_ratio          = 1.0;
-                com_filter_cutoff_hz                = 1.2;
+                com_filter_cutoff_hz                = 1.0;
                 foot_collision_avoidance_distance   = 0.16;
                 foot_landing_vel                    = 0.4;
                 force_double_support_com_h          = 0.9;
@@ -694,13 +694,13 @@ class WBMSCore{
             hrp::Vector2 com_vel_decel_ok, com_vel_accel_ok;
             com_vel_decel_ok = com_vel_accel_ok = com_vel;
 
-            double lf_landing_delay = (rp_ref_out.tgt[lf].abs.p[Z] - rp_ref_out.tgt[lf].cnt.p[Z]) / wp.foot_landing_vel;
-            double rf_landing_delay = (rp_ref_out.tgt[rf].abs.p[Z] - rp_ref_out.tgt[rf].cnt.p[Z]) / wp.foot_landing_vel;
-            if(com_vel(Y)>0 ){ rf_landing_delay = 0; }//怪しい
-            if(com_vel(Y)<0 ){ lf_landing_delay = 0; }
+            double lf_landing_delay = MIN_LIMITED((rp_ref_out.tgt[lf].abs.p[Z] - rp_ref_out.tgt[lf].cnt.p[Z]) / wp.foot_landing_vel, 0);
+            double rf_landing_delay = MIN_LIMITED((rp_ref_out.tgt[rf].abs.p[Z] - rp_ref_out.tgt[rf].cnt.p[Z]) / wp.foot_landing_vel, 0);
+            // if(com_vel(Y)>0 ){ rf_landing_delay = 0; }//怪しい
+            // if(com_vel(Y)<0 ){ lf_landing_delay = 0; }
             double foot_landing_delay = std::max(rf_landing_delay, lf_landing_delay);
             //減速CP条件(現在のCPを常に両足裏で頭打ち)
-            hrp::Vector2  dcm_ragulated = (com_pos + com_vel * ( sqrt( H_cur / G ) + foot_landing_delay)) * wp.capture_point_extend_ratio;
+            hrp::Vector2  dcm_ragulated = (com_pos + com_vel * ( sqrt( H_cur / G ) + foot_landing_delay) * wp.capture_point_extend_ratio );
             if(!isPointInHull2D(dcm_ragulated, hull_d)){
                 calcCrossPointOnHull(com_pos, dcm_ragulated, hull_d, dcm_ragulated);
                 com_vel_decel_ok = (dcm_ragulated - com_pos) / ( sqrt( H_cur / G ) + foot_landing_delay);
@@ -714,7 +714,7 @@ class WBMSCore{
                 cp_force_go_contact[OPPOSITE(lr)] = !isPointInHull2D(dcm_ragulated, one_foot_hull2d[lr]); // if CapturePoint go out of R sole region, L foot must be go contact
             }
             ///// 加速CP条件(CCM使用)
-            hrp::Vector2 ccm_ragulated = (com_pos - com_vel * sqrt( H_cur / G )) * wp.capture_point_extend_ratio;;
+            hrp::Vector2 ccm_ragulated = (com_pos - com_vel * sqrt( H_cur / G ) * wp.capture_point_extend_ratio );
             if(!isPointInHull2D(ccm_ragulated, hull_ccm)){
                 calcCrossPointOnHull(com_pos, ccm_ragulated, hull_ccm, ccm_ragulated);
                 com_vel_accel_ok = (-ccm_ragulated + com_pos ) / sqrt( H_cur / G );

From cb2150ebd5aea69f34bdd23a885181829171b96b Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 15 Jan 2020 14:14:51 +0900
Subject: [PATCH 211/217] tune

---
 rtc/HapticController/HapticController.cpp |  2 ++
 rtc/HapticController/HapticController.h   | 11 +++++++----
 2 files changed, 9 insertions(+), 4 deletions(-)

diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index ad4d1922066..ba5b0d5d500 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -644,6 +644,7 @@ bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticC
     hcp.gravity_compensation_ratio          = i_param.gravity_compensation_ratio;
     hcp.q_friction_coeff                    = i_param.q_friction_coeff;
     hcp.q_ref_max_torque_ratio              = i_param.q_ref_max_torque_ratio;
+    hcp.torque_feedback_ratio               = i_param.torque_feedback_ratio;
     hcp.wrench_hpf_cutoff_hz                = i_param.wrench_hpf_cutoff_hz;
     hcp.wrench_lpf_cutoff_hz                = i_param.wrench_lpf_cutoff_hz;
     hcp.wrench_hpf_gain                     = i_param.wrench_hpf_gain;
@@ -687,6 +688,7 @@ bool HapticController::getParams(OpenHRP::HapticControllerService::HapticControl
     i_param.gravity_compensation_ratio          = hcp.gravity_compensation_ratio;
     i_param.q_friction_coeff                    = hcp.q_friction_coeff;
     i_param.q_ref_max_torque_ratio              = hcp.q_ref_max_torque_ratio;
+    i_param.torque_feedback_ratio               = hcp.torque_feedback_ratio;
     i_param.wrench_hpf_cutoff_hz                = hcp.wrench_hpf_cutoff_hz;
     i_param.wrench_lpf_cutoff_hz                = hcp.wrench_lpf_cutoff_hz;
     i_param.wrench_hpf_gain                     = hcp.wrench_hpf_gain;
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index 67b1a7cd8e8..a6968019294 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -145,6 +145,7 @@ class HapticController : public RTC::DataFlowComponentBase{
                 double gravity_compensation_ratio;
                 double q_friction_coeff;
                 double q_ref_max_torque_ratio;
+                double torque_feedback_ratio;
                 double wrench_hpf_cutoff_hz;
                 double wrench_lpf_cutoff_hz;
                 double wrench_hpf_gain;
@@ -159,16 +160,17 @@ class HapticController : public RTC::DataFlowComponentBase{
                 baselink_height_from_floor          = 1.5;// will be overwrited
                 dqAct_filter_cutoff_hz              = 500;// 10以下で確実に位相遅れによる振動
                 ee_vel_filter_cutoff_hz             = 500;// 10以下で確実に位相遅れによる振動
-                ex_gravity_compensation_ratio_lower = 1.1;
+                ex_gravity_compensation_ratio_lower = 1.0;
                 ex_gravity_compensation_ratio_upper = 0.9;
                 foot_min_distance                   = 0.25;
-                force_feedback_ratio                = 0.2;
+                force_feedback_ratio                = 0.1;
                 gravity_compensation_ratio          = 1.0;
                 q_friction_coeff                    = 0;
-                q_ref_max_torque_ratio              = 0.1;
+                q_ref_max_torque_ratio              = 0.01;
+                torque_feedback_ratio               = 0.01;
                 wrench_hpf_cutoff_hz                = 20;
                 wrench_lpf_cutoff_hz                = 0.3;
-                wrench_hpf_gain                     = 0.8;
+                wrench_hpf_gain                     = 0.1;
                 wrench_lpf_gain                     = 0;
                 ee_pos_rot_friction_coeff           << 10, 0.1;
                 floor_pd_gain                       << 10000, 500;
@@ -192,6 +194,7 @@ class HapticController : public RTC::DataFlowComponentBase{
                 LIMIT_MINMAX(gravity_compensation_ratio          , 0, 2);
                 LIMIT_MINMAX(q_friction_coeff                    , 0, 0.1);
                 LIMIT_MINMAX(q_ref_max_torque_ratio              , 0, 1);
+                LIMIT_MINMAX(torque_feedback_ratio               , 0, 1);
                 LIMIT_MINMAX(wrench_hpf_cutoff_hz                , 0, 500);
                 LIMIT_MINMAX(wrench_lpf_cutoff_hz                , 0, 500);
                 LIMIT_MINMAX(wrench_hpf_gain                     , 0, 2);

From 8d3407bd563146a8f5f4c32cc1062004bb13333f Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 15 Jan 2020 14:14:51 +0900
Subject: [PATCH 212/217] tune

---
 idl/HapticControllerService.idl           |  1 +
 rtc/HapticController/HapticController.cpp |  2 ++
 rtc/HapticController/HapticController.h   | 11 +++++++----
 3 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/idl/HapticControllerService.idl b/idl/HapticControllerService.idl
index 3587777c9af..e3402e45b28 100644
--- a/idl/HapticControllerService.idl
+++ b/idl/HapticControllerService.idl
@@ -30,6 +30,7 @@ module OpenHRP
         double gravity_compensation_ratio;
         double q_friction_coeff;
         double q_ref_max_torque_ratio;
+        double torque_feedback_ratio;
         double wrench_hpf_cutoff_hz;
         double wrench_lpf_cutoff_hz;
         double wrench_hpf_gain;
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index ad4d1922066..ba5b0d5d500 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -644,6 +644,7 @@ bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticC
     hcp.gravity_compensation_ratio          = i_param.gravity_compensation_ratio;
     hcp.q_friction_coeff                    = i_param.q_friction_coeff;
     hcp.q_ref_max_torque_ratio              = i_param.q_ref_max_torque_ratio;
+    hcp.torque_feedback_ratio               = i_param.torque_feedback_ratio;
     hcp.wrench_hpf_cutoff_hz                = i_param.wrench_hpf_cutoff_hz;
     hcp.wrench_lpf_cutoff_hz                = i_param.wrench_lpf_cutoff_hz;
     hcp.wrench_hpf_gain                     = i_param.wrench_hpf_gain;
@@ -687,6 +688,7 @@ bool HapticController::getParams(OpenHRP::HapticControllerService::HapticControl
     i_param.gravity_compensation_ratio          = hcp.gravity_compensation_ratio;
     i_param.q_friction_coeff                    = hcp.q_friction_coeff;
     i_param.q_ref_max_torque_ratio              = hcp.q_ref_max_torque_ratio;
+    i_param.torque_feedback_ratio               = hcp.torque_feedback_ratio;
     i_param.wrench_hpf_cutoff_hz                = hcp.wrench_hpf_cutoff_hz;
     i_param.wrench_lpf_cutoff_hz                = hcp.wrench_lpf_cutoff_hz;
     i_param.wrench_hpf_gain                     = hcp.wrench_hpf_gain;
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index 67b1a7cd8e8..a6968019294 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -145,6 +145,7 @@ class HapticController : public RTC::DataFlowComponentBase{
                 double gravity_compensation_ratio;
                 double q_friction_coeff;
                 double q_ref_max_torque_ratio;
+                double torque_feedback_ratio;
                 double wrench_hpf_cutoff_hz;
                 double wrench_lpf_cutoff_hz;
                 double wrench_hpf_gain;
@@ -159,16 +160,17 @@ class HapticController : public RTC::DataFlowComponentBase{
                 baselink_height_from_floor          = 1.5;// will be overwrited
                 dqAct_filter_cutoff_hz              = 500;// 10以下で確実に位相遅れによる振動
                 ee_vel_filter_cutoff_hz             = 500;// 10以下で確実に位相遅れによる振動
-                ex_gravity_compensation_ratio_lower = 1.1;
+                ex_gravity_compensation_ratio_lower = 1.0;
                 ex_gravity_compensation_ratio_upper = 0.9;
                 foot_min_distance                   = 0.25;
-                force_feedback_ratio                = 0.2;
+                force_feedback_ratio                = 0.1;
                 gravity_compensation_ratio          = 1.0;
                 q_friction_coeff                    = 0;
-                q_ref_max_torque_ratio              = 0.1;
+                q_ref_max_torque_ratio              = 0.01;
+                torque_feedback_ratio               = 0.01;
                 wrench_hpf_cutoff_hz                = 20;
                 wrench_lpf_cutoff_hz                = 0.3;
-                wrench_hpf_gain                     = 0.8;
+                wrench_hpf_gain                     = 0.1;
                 wrench_lpf_gain                     = 0;
                 ee_pos_rot_friction_coeff           << 10, 0.1;
                 floor_pd_gain                       << 10000, 500;
@@ -192,6 +194,7 @@ class HapticController : public RTC::DataFlowComponentBase{
                 LIMIT_MINMAX(gravity_compensation_ratio          , 0, 2);
                 LIMIT_MINMAX(q_friction_coeff                    , 0, 0.1);
                 LIMIT_MINMAX(q_ref_max_torque_ratio              , 0, 1);
+                LIMIT_MINMAX(torque_feedback_ratio               , 0, 1);
                 LIMIT_MINMAX(wrench_hpf_cutoff_hz                , 0, 500);
                 LIMIT_MINMAX(wrench_lpf_cutoff_hz                , 0, 500);
                 LIMIT_MINMAX(wrench_hpf_gain                     , 0, 2);

From fba53f1f3a1f2a3177d7bc64ab02d1c8ec3e240f Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 7 Mar 2020 00:19:36 +0900
Subject: [PATCH 213/217] refactor

---
 rtc/AutoBalancer/MyUtils.h                    |  1 +
 rtc/HapticController/HapticController.cpp     |  2 +-
 .../WholeBodyMasterSlave.cpp                  |  3 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 99 +++++++++++++------
 4 files changed, 72 insertions(+), 33 deletions(-)

diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index 082ca45e545..99e9f1b153f 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -18,6 +18,7 @@ enum { fx, fy, fz, tx, ty, tz, ft_xyz };
 enum { com, rf, lf, rh, lh, head, zmp, num_pose_tgt };
 enum { num_ee_tgt=4 };
 
+#define LR_STR(lr) (lr==R ? "R" : "L")
 #define OPPOSITE(lr) (lr==R ? L : R)
 
 static const double G = 9.80665;
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index ba5b0d5d500..ee2b55f1ed4 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -365,7 +365,7 @@ void HapticController::calcTorque(){
             rats::difference_rotation(diff_rot, master_ee_pose[leg].R, hrp::Matrix33::Identity());
             const hrp::Vector3 diff_rot_vel = hrp::Vector3::Zero() - master_ee_vel_filtered[leg].tail(3);
             hrp::dvector6 wrench = (hrp::dvector6()<< 0,0,0, diff_rot * hcp.foot_horizontal_pd_gain(0) + diff_rot_vel * hcp.foot_horizontal_pd_gain(1)).finished();
-            wrench(tz) = 0;
+            // wrench(tz) = 0;
             LIMIT_NORM_V(wrench.tail(3), 50);
             hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
             for(int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index e7d9c88fb1d..acaa4819168 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -440,7 +440,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     m_zmpOut.write();
     m_optionalDataOut.write();
     addTimeReport("OutPort");
-    if(DEBUGP) RTC_INFO_STREAM(time_report_str);
+    if(DEBUGP)RTC_INFO_STREAM(time_report_str);
+    if(DEBUGP)RTC_INFO_STREAM(wbms->ws.str());
     loop ++;
     return RTC::RTC_OK;
 }
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 7e708301a0f..1dba90c3ec2 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -346,7 +346,6 @@ class WBMSCore{
         HumanPose rp_ref_out_old;
         unsigned int loop;
         bool is_initial_loop;
-        bool cp_force_go_contact[LR];
         int zmp_force_go_contact_count[LR];
         BiquadIIRFilterVec acc4zmp_v_filters, com_filter;
         double com_filter_cutoff_hz_old;
@@ -432,6 +431,42 @@ class WBMSCore{
             hrp::dvector6 act_foot_wrench[LR];
             hrp::Pose3 act_foot_pose[LR];
         } act_rs;
+        class WBMSStates {
+            public:
+                bool lock_foot_by_ref_zmp[LR];
+                bool lock_foot_by_act_zmp[LR];
+                bool lock_foot_by_ref_cp[LR];
+                bool lock_foot_by_act_cp[LR];
+                bool lock_both_feet_by_com_h;
+                int lock_foot_by_act_zmp_count[LR];
+                string auto_com_pos_tgt;
+            WBMSStates(){
+                for(int lr=0; lr<LR; lr++){
+                    lock_foot_by_ref_zmp[lr] = false;
+                    lock_foot_by_act_zmp[lr] = false;
+                    lock_foot_by_ref_cp[lr] = false;
+                    lock_foot_by_act_cp[lr] = false;
+                    lock_foot_by_act_zmp_count[lr] = 0;
+                }
+                lock_both_feet_by_com_h = false;
+            }
+            std::string LR_BOOL_TO_STR(const bool in[LR]){
+                std::string ret;
+                ret += (in[L] ? "L":"");
+                ret += (in[R] ? "R":"");
+                return ret;
+            }
+            std::string str(){
+                std::stringstream ret;
+                ret << "LOCK_FOOT_BY_REF_ZMP(" << LR_BOOL_TO_STR(lock_foot_by_ref_zmp) <<") ";
+                ret << "LOCK_FOOT_BY_ACT_ZMP(" << LR_BOOL_TO_STR(lock_foot_by_act_zmp) <<") ";
+                ret << "LOCK_FOOT_BY_REF_CP("  << LR_BOOL_TO_STR(lock_foot_by_ref_cp) <<") ";
+                ret << "LOCK_FOOT_BY_ACT_CP("  << LR_BOOL_TO_STR(lock_foot_by_act_cp) <<") ";
+                ret << (lock_both_feet_by_com_h ? "LOCK_BOTH_FEET_BY_COM_H " : "");
+                ret << "AUTO_COM_POS_TGT(" << auto_com_pos_tgt << ") ";
+                return ret.str();
+            }
+        } ws;
 
         WBMSCore(const double& dt){
             DT = dt;
@@ -446,7 +481,6 @@ class WBMSCore{
             for(int lr=0; lr<LR; lr++){
                 act_foot_vert_fblr[lr]          = fbio2fblr(wp.actual_foot_vert_fbio, lr);
                 safe_foot_vert_fblr[lr]         = fbio2fblr(wp.safety_foot_vert_fbio, lr);
-                cp_force_go_contact[lr]         = false;
                 zmp_force_go_contact_count[lr]  = 0;
             }
             rp_ref_out_old.reset();
@@ -527,7 +561,6 @@ class WBMSCore{
                 com_filter_cutoff_hz_old = wp.com_filter_cutoff_hz;
             }
             rp_ref_out.tgt[com].abs.p = com_filter.passFilter(rp_ref_out.tgt[com].abs.p);
-
             loop++;
             is_initial_loop = false;
         }
@@ -550,15 +583,16 @@ class WBMSCore{
             if(wp.auto_com_mode){
                 const double rf_h_from_floor = human.tgt[rf].abs.p(Z) - old.tgt[rf].cnt.p(Z);
                 const double lf_h_from_floor = human.tgt[lf].abs.p(Z) - old.tgt[lf].cnt.p(Z);
-                if      ( rf_h_from_floor - lf_h_from_floor >  wp.auto_com_foot_move_detect_height){ out.tgt[com].abs.p.head(XY) = old.tgt[lf].abs.p.head(XY); }
-                else if ( rf_h_from_floor - lf_h_from_floor < -wp.auto_com_foot_move_detect_height){ out.tgt[com].abs.p.head(XY) = old.tgt[rf].abs.p.head(XY); }
-                else                       { out.tgt[com].abs.p.head(XY) = (old.tgt[rf].abs.p.head(XY) + old.tgt[lf].abs.p.head(XY)) / 2; }
-                ///// lock if com height is low = half sitting
-                for(int lr=0; lr<LR; lr++){
-                    if(act_rs.ref_com(Z) - old.foot(lr).cnt.p(Z) < wp.force_double_support_com_h){
-                        out.tgt[com].abs.p.head(XY) = (old.tgt[rf].abs.p.head(XY) + old.tgt[lf].abs.p.head(XY)) / 2;
-                    }
+                if      ( rf_h_from_floor - lf_h_from_floor >  wp.auto_com_foot_move_detect_height) { ws.auto_com_pos_tgt = "LF";  }
+                else if ( rf_h_from_floor - lf_h_from_floor < -wp.auto_com_foot_move_detect_height) { ws.auto_com_pos_tgt = "RF";  }
+                else                                                                                { ws.auto_com_pos_tgt = "MID"; }
+                for(int lr=0; lr<LR; lr++){///// lock if com height is low = half sitting
+                    if(act_rs.ref_com(Z) - old.foot(lr).cnt.p(Z) < wp.force_double_support_com_h)   { ws.auto_com_pos_tgt = "MID"; ws.lock_both_feet_by_com_h = true; }
                 }
+                if      (ws.auto_com_pos_tgt == "LF")   { out.tgt[com].abs.p.head(XY) =  old.tgt[lf].abs.p.head(XY);                                    }
+                else if (ws.auto_com_pos_tgt == "RF")   { out.tgt[com].abs.p.head(XY) =  old.tgt[rf].abs.p.head(XY);                                    }
+                else if (ws.auto_com_pos_tgt == "MID")  { out.tgt[com].abs.p.head(XY) = (old.tgt[rf].abs.p.head(XY) + old.tgt[lf].abs.p.head(XY)) / 2;  }
+                else                                    { std::cerr<< "setAutoCOMMode something wrong!" << std::endl;                                   }
             }
         }
         void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){
@@ -568,21 +602,25 @@ class WBMSCore{
                 if(to_opposite_foot[lr].norm() < 1e-3){ std::cerr << "to_opposite_foot[lr].norm() < 1e-3 :" << to_opposite_foot[lr].transpose()<<std::endl; }
                 ///// lock by ref_zmp
                 ref_zmp_from_foot[lr] = act_rs.ref_zmp.head(XY) - old.foot(lr).abs.p.head(XY);
-                out.foot(OPPOSITE(lr)).go_contact |= ( ref_zmp_from_foot[lr].dot(to_opposite_foot[lr]) / to_opposite_foot[lr].norm() > wp.single_foot_zmp_safety_distance );
+                ws.lock_foot_by_ref_zmp[OPPOSITE(lr)] = ( ref_zmp_from_foot[lr].dot(to_opposite_foot[lr].normalized()) > wp.single_foot_zmp_safety_distance);
                 ///// lock by act_zmp
                 if(wp.auto_foot_landing_by_act_zmp){
                     act_zmp_from_foot[lr] = act_rs.st_zmp.head(XY) - old.foot(lr).abs.p.head(XY);
-                    if( act_zmp_from_foot[lr].dot(to_opposite_foot[lr]) / to_opposite_foot[lr].norm() > wp.single_foot_zmp_safety_distance ){
-                        zmp_force_go_contact_count[OPPOSITE(lr)] = 1;
-                    }else if(zmp_force_go_contact_count[OPPOSITE(lr)] > 0 && zmp_force_go_contact_count[OPPOSITE(lr)] < HZ * wp.additional_double_support_time ){
-                        zmp_force_go_contact_count[OPPOSITE(lr)]++;
+                    if(act_zmp_from_foot[lr].dot(to_opposite_foot[lr].normalized()) > wp.single_foot_zmp_safety_distance){
+                        ws.lock_foot_by_act_zmp_count[OPPOSITE(lr)] = 0;// reset count
+                    }
+                    if(ws.lock_foot_by_act_zmp_count[OPPOSITE(lr)] < HZ * wp.additional_double_support_time){
+                        ws.lock_foot_by_act_zmp_count[OPPOSITE(lr)]++;// count up till additional_double_support_time
+                        ws.lock_foot_by_act_zmp[OPPOSITE(lr)] = true;// lock swing foot till reach count limit
                     }else{
-                        zmp_force_go_contact_count[OPPOSITE(lr)] = 0;
+                        ws.lock_foot_by_act_zmp[OPPOSITE(lr)] = false;// release swing foot lock after reach count limit
                     }
-                    out.foot(OPPOSITE(lr)).go_contact |= (zmp_force_go_contact_count[OPPOSITE(lr)] > 0);
+                }else{
+                    ws.lock_foot_by_act_zmp[OPPOSITE(lr)] = false;
                 }
-                ///// lock by ref cp
-                out.foot(OPPOSITE(lr)).go_contact |= cp_force_go_contact[OPPOSITE(lr)];
+            }
+            for(int lr=0; lr<LR; lr++){ // merge all swing foot lock flags
+                out.foot(lr).go_contact |= (ws.lock_foot_by_ref_zmp[lr] | ws.lock_foot_by_act_zmp[lr] | ws.lock_foot_by_ref_cp[lr] | ws.lock_foot_by_act_cp[lr]);
             }
         }
         void limitEEWorkspace(const HumanPose& old, HumanPose& out){
@@ -612,20 +650,19 @@ class WBMSCore{
             static int cnt_for_clear[LR] ={0};
             for(int lr=0; lr<LR; lr++){
                 if(wp.auto_floor_h_mode){
-                    // judge act contact height
-                    if(!is_locked[lr] && fabs(act_rs.act_foot_wrench[lr](fz)) > wp.auto_floor_h_detect_fz){
-                        out.foot(lr).cnt.p(Z) = act_rs.act_foot_pose[lr].p(Z);
-                        is_locked[lr] = true;
-                    }
-                    if(is_locked[lr] && fabs(act_rs.act_foot_wrench[lr](fz)) < wp.auto_floor_h_reset_fz){
-                        if(cnt_for_clear[lr]++ > HZ * 0.5){
+                    if(fabs(act_rs.act_foot_wrench[lr](fz)) > wp.auto_floor_h_detect_fz){
+                        if( cnt_for_clear[lr] >= HZ * 1.0){ // first contact after detected floor height cleared 
+                            out.foot(lr).cnt.p(Z) = act_rs.act_foot_pose[lr].p(Z);// get act contact height as detected floor height
+                        }
+                        cnt_for_clear[lr] = 0;// keep resetting count during act foot force exists
+                    }else{
+                        if(cnt_for_clear[lr] < HZ * 1.0){ // keep current detected floor height until reach count limit
+                            cnt_for_clear[lr]++;
+                        }else{ // clear detected floor height to zero after finish count up
                             out.foot(lr).cnt.p(Z) = out.foot(lr).offs.p(Z);
-                            is_locked[lr] = false;
-                            cnt_for_clear[lr] = 0;
                         }
                     }
                 }
-
                 if(out.foot(lr).go_contact){
                     out.foot(lr).abs.p.head(XY) = old.foot(lr).abs.p.head(XY);
                     // out.foot(lr).abs.p(Z) = out.foot(lr).offs.p(Z);
@@ -711,7 +748,7 @@ class WBMSCore{
                 one_foot_vert_3d_for_check[lr]  = make_rect_3d(offset_fblr(safe_foot_vert_fblr[lr], 0.002));// 数値誤差で内外判定ずれるので少し大きい凸包で判定
                 foot_vert3d_check_wld[lr]       = (rp_ref_out.foot(lr).abs.R * one_foot_vert_3d_for_check[lr]).colwise() + rp_ref_out.foot(lr).abs.p; // rotate and translate in 3D
                 one_foot_hull2d[lr]             = makeConvexHull2D( foot_vert3d_check_wld[lr].topRows(XY) ); // project into 2D and make convex hull
-                cp_force_go_contact[OPPOSITE(lr)] = !isPointInHull2D(dcm_ragulated, one_foot_hull2d[lr]); // if CapturePoint go out of R sole region, L foot must be go contact
+                ws.lock_foot_by_ref_cp[OPPOSITE(lr)] = !isPointInHull2D(dcm_ragulated, one_foot_hull2d[lr]); // if CapturePoint go out of R sole region, L foot must be go contact
             }
             ///// 加速CP条件(CCM使用)
             hrp::Vector2 ccm_ragulated = (com_pos - com_vel * sqrt( H_cur / G ) * wp.capture_point_extend_ratio );

From aa5f497d7373449e721f6ad5c44e2f97269025b7 Mon Sep 17 00:00:00 2001
From: ishiguro <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 9 Mar 2020 15:16:56 +0900
Subject: [PATCH 214/217] tune

---
 rtc/AutoBalancer/MyUtils.h                        |  1 +
 rtc/HapticController/HapticController.cpp         | 13 +++++++++----
 rtc/HapticController/HapticController.h           |  2 ++
 rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp |  2 +-
 rtc/WholeBodyMasterSlave/wbms_core.h              |  8 +++-----
 5 files changed, 16 insertions(+), 10 deletions(-)

diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index 99e9f1b153f..44b00b5a5fb 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -43,6 +43,7 @@ static const double Q_BUTTERWORTH = 0.707106781;
 #define MIN_LIMITED(x,min)          ( x<min ? min : x )
 #define MAX_LIMITED(x,max)          ( x>max ? max : x )
 #define MINMAX_LIMITED(x,min,max)   ( x<min ? min : ( x>max ? max : x ))
+#define NORM_LIMITED(x,max)         ( x<(-max) ? -max : (x>max ? max : x))
 
 
 namespace hrp{
diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index ee2b55f1ed4..8b4476b4c32 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -87,6 +87,8 @@ RTC::ReturnCode_t HapticController::onInitialize(){
     baselink_h_ip->get(&baselink_h_from_floor, false);
     RTC_INFO_STREAM("setup interpolator finished");
 
+    current_adjust_floor_h = 0;
+
     dqAct_filter = BiquadIIRFilterVec(m_robot->numJoints());
     dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH);
     dqAct_filter.reset(0);
@@ -290,11 +292,14 @@ void HapticController::calcCurrentState(){
         jp.calcJacobian(J_ee[ee], ee_ikc_map[ee].localPos);
     }
 
-    for(auto leg : {"rleg","lleg"}){ //m_robot->rootLink()->p(Z)は0付近で更新されないのでworld座標系におけるfloor_hは-1.0とからへん
-        const double slave_act_floor_z = MINMAX_LIMITED(m_slaveTgtPoses[(leg=="lleg" ? "lfloor" : "rfloor")].data.position.z, 0, 0.3);
+    for(std::string lr : {"l","r"}){ //m_robot->rootLink()->p(Z)は0付近で更新されないのでworld座標系におけるfloor_hは-1.0とからへん
+        // const double slave_act_floor_z = MINMAX_LIMITED(m_slaveTgtPoses[lr+"floor"].data.position.z, 0, 0.3);
+        const double slave_act_floor_z = m_slaveTgtPoses[lr+"floor"].data.position.z;
         const double basic_floor_h_wld = m_robot->rootLink()->p(Z) - baselink_h_from_floor;
-        foot_h_from_floor[leg] = master_ee_pose[leg].p(Z) - (basic_floor_h_wld + slave_act_floor_z);
+        foot_h_from_floor[lr+"leg"] = master_ee_pose[lr+"leg"].p(Z) - (basic_floor_h_wld + slave_act_floor_z - current_adjust_floor_h);
     }
+    const double target_adjust_floor_h = std::min(m_slaveTgtPoses["lfloor"].data.position.z, m_slaveTgtPoses["rfloor"].data.position.z);
+    current_adjust_floor_h += NORM_LIMITED(target_adjust_floor_h - current_adjust_floor_h, 0.01*m_dt);
 
 }
 
@@ -536,7 +541,7 @@ void HapticController::calcOdometry(){
     const hrp::Pose3    pleg_to_cleg        = hrp::calcRelPose3 ( hrp::to_2DPlanePose3(master_ee_pose[parent_leg]),             hrp::to_2DPlanePose3(master_ee_pose[child_leg]) );
     hrp::Pose3 base_pose_from_floor_origin  = hrp::applyRelPose3( hrp::to_2DPlanePose3(leg_pose_from_floor_origin[parent_leg]), hrp::to_2DPlanePose3(pleg_to_base)              );
     const double floating_h                 = MIN_LIMITED(std::min(foot_h_from_floor["rleg"], foot_h_from_floor["lleg"]), 0); 
-    base_pose_from_floor_origin.p(Z)        = baselink_h_from_floor - floating_h; // ajust not to float with both feet.
+    base_pose_from_floor_origin.p(Z)        = baselink_h_from_floor - floating_h + current_adjust_floor_h; // adjust not to float with both feet.
     leg_pose_from_floor_origin[child_leg]   = hrp::applyRelPose3( hrp::to_2DPlanePose3(leg_pose_from_floor_origin[parent_leg]), hrp::to_2DPlanePose3(pleg_to_cleg)              );
 
     //update base link pos from world
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index a6968019294..231a2e15c8b 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -129,6 +129,8 @@ class HapticController : public RTC::DataFlowComponentBase{
         std::map<std::string, double> foot_h_from_floor;
         bool resetOdom_request;
 
+        double current_adjust_floor_h;
+
         ControlMode mode;
 
         std::vector<std::string> ee_names, tgt_names;
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index acaa4819168..d6163da713f 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -505,7 +505,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
       tmp.localPos = hrp::Vector3::Zero();
       tmp.localR = hrp::Matrix33::Identity();
       tmp.targetPos = hrp::to_Vector3(m_basePos.data);// will be ignored by selection_vec
-      tmp.targetRpy = com_ref.rpy();// ベースリンクの回転をフリーにはしないほうがいい(omegaの積分誤差で暴れる)
+      tmp.targetRpy = com_ref.rpy();
       tmp.constraint_weight << 1,1,1,1,1,1;
       tmp.rot_precision = deg2rad(3);
       ikc_list.push_back(tmp);
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 1dba90c3ec2..44ac4707b84 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -468,7 +468,7 @@ class WBMSCore{
             }
         } ws;
 
-        WBMSCore(const double& dt){
+        WBMSCore(const double dt){
             DT = dt;
             HZ = (int)(1.0/DT);
             loop = 0;
@@ -586,8 +586,8 @@ class WBMSCore{
                 if      ( rf_h_from_floor - lf_h_from_floor >  wp.auto_com_foot_move_detect_height) { ws.auto_com_pos_tgt = "LF";  }
                 else if ( rf_h_from_floor - lf_h_from_floor < -wp.auto_com_foot_move_detect_height) { ws.auto_com_pos_tgt = "RF";  }
                 else                                                                                { ws.auto_com_pos_tgt = "MID"; }
-                for(int lr=0; lr<LR; lr++){///// lock if com height is low = half sitting
-                    if(act_rs.ref_com(Z) - old.foot(lr).cnt.p(Z) < wp.force_double_support_com_h)   { ws.auto_com_pos_tgt = "MID"; ws.lock_both_feet_by_com_h = true; }
+                for(auto f : {rf,lf}){///// lock if com height is low = half sitting
+                    if(act_rs.ref_com(Z) - old.tgt[f].cnt.p(Z) < wp.force_double_support_com_h)     { ws.auto_com_pos_tgt = "MID"; ws.lock_both_feet_by_com_h = true; }
                 }
                 if      (ws.auto_com_pos_tgt == "LF")   { out.tgt[com].abs.p.head(XY) =  old.tgt[lf].abs.p.head(XY);                                    }
                 else if (ws.auto_com_pos_tgt == "RF")   { out.tgt[com].abs.p.head(XY) =  old.tgt[rf].abs.p.head(XY);                                    }
@@ -646,7 +646,6 @@ class WBMSCore{
             }
         }
         void setFootContactPoseByGoContact(const HumanPose& old, HumanPose& out){
-            static bool is_locked[LR] = {false};
             static int cnt_for_clear[LR] ={0};
             for(int lr=0; lr<LR; lr++){
                 if(wp.auto_floor_h_mode){
@@ -680,7 +679,6 @@ class WBMSCore{
                 }
                 if(loop%500==0){
                     dbg(lr);
-                    dbg(is_locked[lr]);
                     dbg(out.foot(lr).cnt.p(Z));
                     dbg(out.foot(lr).abs.p(Z));
                     dbg(act_rs.act_foot_pose[lr].p(Z));

From dba674a7ab83489ea7ff61f4225d16f7fdb1e14f Mon Sep 17 00:00:00 2001
From: leus <leus@example.com>
Date: Mon, 9 Mar 2020 17:15:31 +0900
Subject: [PATCH 215/217] tune

---
 rtc/HapticController/HapticController.cpp | 6 +++---
 rtc/HapticController/HapticController.h   | 2 +-
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp
index 8b4476b4c32..e558e214ced 100644
--- a/rtc/HapticController/HapticController.cpp
+++ b/rtc/HapticController/HapticController.cpp
@@ -370,7 +370,7 @@ void HapticController::calcTorque(){
             rats::difference_rotation(diff_rot, master_ee_pose[leg].R, hrp::Matrix33::Identity());
             const hrp::Vector3 diff_rot_vel = hrp::Vector3::Zero() - master_ee_vel_filtered[leg].tail(3);
             hrp::dvector6 wrench = (hrp::dvector6()<< 0,0,0, diff_rot * hcp.foot_horizontal_pd_gain(0) + diff_rot_vel * hcp.foot_horizontal_pd_gain(1)).finished();
-            // wrench(tz) = 0;
+            //wrench(tz) = 0; // unlock foot yaw
             LIMIT_NORM_V(wrench.tail(3), 50);
             hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
             for(int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
@@ -381,7 +381,7 @@ void HapticController::calcTorque(){
         for (auto leg : legs){
             if(foot_h_from_floor[leg] < 0){
                 hrp::dvector6 wrench = hrp::dvector6::Unit(fz) * (-foot_h_from_floor[leg]*hcp.floor_pd_gain(0) + (0-master_ee_vel_filtered[leg](fz))*hcp.floor_pd_gain(1));
-                LIMIT_NORM_V(wrench, 1000);
+                LIMIT_NORM_V(wrench, 200);
                 const hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
                 for(int j=0; j<jpath_ee[leg].numJoints(); j++){ jpath_ee[leg].joint(j)->u += tq_tmp(j); }
                 masterEEWrenches[leg] += wrench;
@@ -424,7 +424,7 @@ void HapticController::calcTorque(){
             rats::difference_rotation(diff_rot, cur_l2r_pose.R, locked_l2r_pose.R);
             hrp::dvector6 rleg_wrench = (hrp::dvector6()<< diff_pos * 1000, diff_rot * 100).finished();
             rleg_wrench(fz) = 0;
-            LIMIT_NORM_V(rleg_wrench, 1000);
+            LIMIT_NORM_V(rleg_wrench, 200);
             for (auto leg : legs){
                 hrp::dvector6 wrench = (leg=="rleg" ? 1:-1) *  rleg_wrench;
                 hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench;
diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h
index 231a2e15c8b..c5f9102130b 100644
--- a/rtc/HapticController/HapticController.h
+++ b/rtc/HapticController/HapticController.h
@@ -195,7 +195,7 @@ class HapticController : public RTC::DataFlowComponentBase{
                 LIMIT_MINMAX(force_feedback_ratio                , 0, 2);
                 LIMIT_MINMAX(gravity_compensation_ratio          , 0, 2);
                 LIMIT_MINMAX(q_friction_coeff                    , 0, 0.1);
-                LIMIT_MINMAX(q_ref_max_torque_ratio              , 0, 1);
+                LIMIT_MINMAX(q_ref_max_torque_ratio              , 0, 2);
                 LIMIT_MINMAX(torque_feedback_ratio               , 0, 1);
                 LIMIT_MINMAX(wrench_hpf_cutoff_hz                , 0, 500);
                 LIMIT_MINMAX(wrench_lpf_cutoff_hz                , 0, 500);

From a6ad7229d104adc80baa98950df3beebc0bae977 Mon Sep 17 00:00:00 2001
From: ishiguroJSK <ishiguro@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 10 May 2020 00:56:23 +0900
Subject: [PATCH 216/217] refactor for non-biped

---
 .../FullbodyInverseKinematicsSolver.h         |   2 +-
 rtc/AutoBalancer/MyUtils.h                    |   8 +-
 .../WholeBodyMasterSlave.cpp                  | 239 ++++++++++--------
 .../WholeBodyMasterSlave.h                    |  16 +-
 rtc/WholeBodyMasterSlave/wbms_core.h          | 113 +++++----
 5 files changed, 202 insertions(+), 176 deletions(-)

diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
index cdd04729022..a4201b892fd 100644
--- a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
+++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h
@@ -156,7 +156,7 @@ class FullbodyInverseKinematicsSolver{
                     J_part << J_com, J_am;
                 }
                 else{
-                    std::cerr<<"Unknown Link Target !!"<<std::endl; continue;  //不明なリンク指定
+                    std::cerr<<"Unknown Link Target !! "<< _ikc_list[i].target_link_name << std::endl; continue;  //不明なリンク指定
                 }
                 // set one of the end effector constraints into Jacobian
                 hrp::dmatrix c_part_selection_mat = hrp::to_SelectionMat(_ikc_list[i].constraint_weight);   // select only valid end effector constraint
diff --git a/rtc/AutoBalancer/MyUtils.h b/rtc/AutoBalancer/MyUtils.h
index 44b00b5a5fb..408ccd3a356 100644
--- a/rtc/AutoBalancer/MyUtils.h
+++ b/rtc/AutoBalancer/MyUtils.h
@@ -25,10 +25,10 @@ static const double G = 9.80665;
 static const double Q_NOOVERSHOOT = 0.5;
 static const double Q_BUTTERWORTH = 0.707106781;
 
-#define dbg(var) std::cout<<#var"= "<<(var)<<std::endl
-#define dbgn(var) std::cout<<#var"= "<<std::endl<<(var)<<std::endl
-#define dbgv(var) std::cout<<#var"= "<<(var.transpose())<<std::endl
-#define RTC_INFO_STREAM(var) std::cout << "[" << m_profile.instance_name << "] "<< var << std::endl;
+#define dbg(var) std::cerr<<#var"= "<<(var)<<std::endl
+#define dbgn(var) std::cerr<<#var"= "<<std::endl<<(var)<<std::endl
+#define dbgv(var) std::cerr<<#var"= "<<(var.transpose())<<std::endl
+#define RTC_INFO_STREAM(var) std::cerr << "[" << m_profile.instance_name << "] "<< var << std::endl;
 #define RTC_WARN_STREAM(var) std::cerr << "\x1b[31m[" << m_profile.instance_name << "] " << var << "\x1b[39m" << std::endl;
 
 #define eps_eq(a, b, c)             (fabs((a)-(b)) <= c)
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
index d6163da713f..6cd39422360 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp
@@ -122,11 +122,26 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
         wbms->wp.use_targets.push_back("com");
         wbms->wp.use_targets.push_back("head");
     }
+    wbms->legged = ( has(ee_names,"lleg") || has(ee_names,"rleg") );
+    RTC_INFO_STREAM("setup mode as legged robot ? = " << wbms->legged);
+
+    for (int st; st<robot_for_ik->numSensorTypes(); st++){
+        RTC_INFO_STREAM("SensorType : "<<st);
+        for (int s; s<robot_for_ik->numSensors(st); s++){
+            RTC_INFO_STREAM("Sensor id : "<<s);
+            robot_for_ik->sensor(st,s)->putInformation(std::cerr);
+        }
+    }
+    
     sccp = boost::shared_ptr<CapsuleCollisionChecker>(new CapsuleCollisionChecker(fik->m_robot));
     RTC_INFO_STREAM("setup main function class finished");
 
     // allocate memory for outPorts
     m_qRef.data.length(fik->m_robot->numJoints()); // is really needed?
+    if(prop["seq_optional_data_dim"].empty()){
+        RTC_WARN_STREAM("No seq_optional_data_dim is set ! Exit !!!");
+        return RTC::RTC_ERROR;
+    }
     coil::stringTo(optionalDataLength, prop["seq_optional_data_dim"].c_str());
 
     output_ratio = 0.0;
@@ -141,6 +156,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){
     }else{
         avg_q_vel = hrp::dvector::Constant(fik->numStates(), 1.0); // all joint max avarage vel = 1.0 rad/s
     }
+    if(!wbms->legged){ avg_q_vel = hrp::dvector::Constant(fik->numStates(), 10.0); } // rapid manip 
     avg_q_acc = hrp::dvector::Constant(fik->numStates(), 16.0); // all joint max avarage acc = 16.0 rad/s^2
     avg_q_vel.tail(6).fill(std::numeric_limits<double>::max()); // no limit for base link vel
     avg_q_acc.tail(6).fill(std::numeric_limits<double>::max()); // no limit for base link acc
@@ -223,6 +239,8 @@ RTC::ReturnCode_t WholeBodyMasterSlave::setupEEIKConstraintFromConf(std::map<std
                 RTC_INFO_STREAM("   has_toe_joint = " << "fix to false now");
             }else{
                 RTC_WARN_STREAM("Target link [" << target_link_name << "] not found !");
+                for (int i; i<_robot->numJoints(); i++){    RTC_WARN_STREAM( i << " : " << _robot->joint(i)->name); }
+                for (int i; i<_robot->numLinks(); i++){     RTC_WARN_STREAM( i << " : " << _robot->link(i)->name); }
                 return RTC::RTC_ERROR;
             }
             contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));////TODO:要移動? //used for check optional data order
@@ -245,11 +263,12 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     for (auto ee : ee_names) {
         if(m_localEEWrenchesIn[ee]->isNew()){ m_localEEWrenchesIn[ee]->read(); }
     }
-    wbms->act_rs.act_foot_wrench[0] = hrp::to_dvector(m_localEEWrenches["rleg"].data);
-    wbms->act_rs.act_foot_wrench[1] = hrp::to_dvector(m_localEEWrenches["lleg"].data);
-    wbms->act_rs.act_foot_pose[0] = ee_ikc_map["rleg"].getCurrentTargetPose(m_robot_vsafe);
-    wbms->act_rs.act_foot_pose[1] = ee_ikc_map["lleg"].getCurrentTargetPose(m_robot_vsafe);
-
+    if(wbms->legged){
+        wbms->act_rs.act_foot_wrench[0] = hrp::to_dvector(m_localEEWrenches["rleg"].data);
+        wbms->act_rs.act_foot_wrench[1] = hrp::to_dvector(m_localEEWrenches["lleg"].data);
+        wbms->act_rs.act_foot_pose[0] = ee_ikc_map["rleg"].getCurrentTargetPose(m_robot_vsafe);
+        wbms->act_rs.act_foot_pose[1] = ee_ikc_map["lleg"].getCurrentTargetPose(m_robot_vsafe);
+    }
     // calc actual state
     std::map<std::string, std::string> to_sname{{"lleg", "lfsensor"}, {"rleg", "rfsensor"}, {"larm", "lhsensor"}, {"rarm", "rhsensor"}};
     hrp::setQAll(m_robot_act, hrp::to_dvector(m_qAct.data));
@@ -258,14 +277,16 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     m_robot_act->calcForwardKinematics();
     for(auto ee : ee_names){
         hrp::ForceSensor* sensor = m_robot_act->sensor<hrp::ForceSensor>(to_sname[ee]);
-        hrp::Matrix33 sensorR_wld = sensor->link->R * sensor->localR;
-        hrp::Matrix33 sensorR_from_base = m_robot_act->rootLink()->R.transpose() * sensorR_wld;
-        const hrp::Vector3 f_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee].data).head(3);
-        const hrp::Vector3 t_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee].data).tail(3);
-        const hrp::Vector3 sensor_to_ee_vec_wld = ee_ikc_map[ee].getCurrentTargetPos(m_robot_act) - sensor->link->p;
-        const hrp::Vector3 f_ee_wld = f_sensor_wld;
-        const hrp::Vector3 t_ee_wld = t_sensor_wld - sensor_to_ee_vec_wld.cross(f_sensor_wld);
-        m_slaveEEWrenches[ee].data = hrp::to_DoubleSeq( (hrp::dvector6()<<f_ee_wld,t_ee_wld).finished());
+        hrp::dvector6 w_ee_wld = hrp::dvector6::Zero();
+        if(sensor){
+            hrp::Matrix33 sensorR_wld = sensor->link->R * sensor->localR;
+            hrp::Matrix33 sensorR_from_base = m_robot_act->rootLink()->R.transpose() * sensorR_wld;
+            const hrp::Vector3 f_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee].data).head(3);
+            const hrp::Vector3 t_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee].data).tail(3);
+            const hrp::Vector3 sensor_to_ee_vec_wld = ee_ikc_map[ee].getCurrentTargetPos(m_robot_act) - sensor->link->p;
+            w_ee_wld << f_sensor_wld, t_sensor_wld - sensor_to_ee_vec_wld.cross(f_sensor_wld);
+        }
+        m_slaveEEWrenches[ee].data = hrp::to_DoubleSeq( w_ee_wld );
         m_slaveEEWrenches[ee].tm = m_qRef.tm;
         m_slaveEEWrenchesOut[ee]->write();
         m_slaveTgtPoses[ee].data = hrp::to_Pose3D(ee_ikc_map[ee].getCurrentTargetPose(m_robot_act));
@@ -304,14 +325,14 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
                 if(tgt == "lhand" || tgt == "rhand" || tgt == "lfloor" || tgt == "rfloor" ){
                     // nothing
                 }else{
-                    wbms->hp_wld_raw.tgt_by_str(tgt).abs = hrp::to_Pose3(m_masterTgtPoses[tgt].data);
+                    wbms->hp_wld_raw.stgt(tgt).abs = hrp::to_Pose3(m_masterTgtPoses[tgt].data);
                 }
             }
         }
         for (auto ee : ee_names){
             if (m_masterEEWrenchesIn[ee]->isNew()){
                 m_masterEEWrenchesIn[ee]->read();
-                wbms->hp_wld_raw.tgt_by_str(ee).w = hrp::to_dvector(m_masterEEWrenches[ee].data);
+                wbms->hp_wld_raw.stgt(ee).w = hrp::to_dvector(m_masterEEWrenches[ee].data);
             }
         }
         if (m_actCPIn.isNew()){     m_actCPIn.read();   rel_act_cp  = hrp::to_Vector3(m_actCP.data);}
@@ -359,10 +380,10 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
             idsb.setInitState(fik->m_robot, m_dt);//逆動力学初期化
         }
         wbms->update();//////HumanSynchronizerの主要処理
-        if(DEBUGP)wbms->rp_ref_out.print();
+        if(DEBUGP)RTC_INFO_STREAM(wbms->rp_ref_out);
         addTimeReport("MainFunc");
 
-        solveFullbodyIK(wbms->rp_ref_out.tgt[com].abs, wbms->rp_ref_out.tgt[rf].abs, wbms->rp_ref_out.tgt[lf].abs, wbms->rp_ref_out.tgt[rh].abs, wbms->rp_ref_out.tgt[lh].abs, wbms->rp_ref_out.tgt[head].abs);
+        solveFullbodyIK(wbms->rp_ref_out);
         addTimeReport("IK");
 
         // RHP finger
@@ -418,19 +439,24 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
             m_optionalData.data.length(optionalDataLength);//TODO:これいいのか？
             for(int i=0;i<optionalDataLength;i++)m_optionalData.data[i] = 0;
         }
-        m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = wbms->rp_ref_out.tgt[rf].is_contact();
-        m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = wbms->rp_ref_out.tgt[lf].is_contact();
+        if(wbms->legged){
+            m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = wbms->rp_ref_out.tgt[rf].is_contact();
+            m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = wbms->rp_ref_out.tgt[lf].is_contact();
+        }
         addTimeReport("SetOutPut");
 
     }
     wbms->baselinkpose.p = fik->m_robot->rootLink()->p;
     wbms->baselinkpose.R = fik->m_robot->rootLink()->R;
     // send back auto detected floor height
-    for(std::string lr : {"l","r"}){
-        const hrp::Vector3 floor_pos = wbms->rp_ref_out.tgt_by_str(lr+"leg").cnt.p - wbms->rp_ref_out.tgt_by_str(lr+"leg").offs.p;
-        m_slaveTgtPoses[lr+"floor"].data    = hrp::to_Pose3D( (hrp::dvector6()<<floor_pos,0,0,0).finished());
-        m_slaveTgtPoses[lr+"floor"].tm      = m_qRef.tm;
-        m_slaveTgtPosesOut[lr+"floor"]->write();
+    
+    if(wbms->legged){
+        for(std::string lr : {"l","r"}){
+            const hrp::Vector3 floor_pos = wbms->rp_ref_out.stgt(lr+"leg").cnt.p - wbms->rp_ref_out.stgt(lr+"leg").offs.p;
+            m_slaveTgtPoses[lr+"floor"].data    = hrp::to_Pose3D( (hrp::dvector6()<<floor_pos,0,0,0).finished());
+            m_slaveTgtPoses[lr+"floor"].tm      = m_qRef.tm;
+            m_slaveTgtPosesOut[lr+"floor"]->write();
+        }
     }
 
     // write
@@ -441,7 +467,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
     m_optionalDataOut.write();
     addTimeReport("OutPort");
     if(DEBUGP)RTC_INFO_STREAM(time_report_str);
-    if(DEBUGP)RTC_INFO_STREAM(wbms->ws.str());
+    if(DEBUGP)RTC_INFO_STREAM(wbms->ws);
     loop ++;
     return RTC::RTC_OK;
 }
@@ -450,7 +476,7 @@ RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){
 void WholeBodyMasterSlave::processTransition(){
     switch(mode.now()){
 
-        case MODE_SYNC_TO_HC:
+        case MODE_SYNC_TO_WBMS:
             if(mode.pre() == MODE_IDLE){ double tmp = 1.0; t_ip->setGoal(&tmp, 3.0, true); }
             if (!t_ip->isEmpty() ){
                 t_ip->get(&output_ratio, true);
@@ -475,7 +501,7 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(){
     fik->m_robot->rootLink()->p = hrp::to_Vector3(m_basePos.data);
     hrp::setQAll(fik->m_robot, hrp::to_dvector(m_qRef.data));
     fik->m_robot->calcForwardKinematics();
-    double current_foot_height_from_world = std::min(ee_ikc_map["rleg"].getCurrentTargetPos(fik->m_robot)(Z), ee_ikc_map["lleg"].getCurrentTargetPos(fik->m_robot)(Z));
+    double current_foot_height_from_world = wbms->legged ? ::min(ee_ikc_map["rleg"].getCurrentTargetPos(fik->m_robot)(Z), ee_ikc_map["lleg"].getCurrentTargetPos(fik->m_robot)(Z)) : 0;
     RTC_INFO_STREAM("current_foot_height_from_world = "<<current_foot_height_from_world<<" will be modified to 0");
     for(auto b : {fik->m_robot, m_robot_vsafe}){//初期姿勢でBodyをFK
         hrp::setRobotStateVec(b, hrp::to_dvector(m_qRef.data), hrp::to_Vector3(m_basePos.data) - hrp::Vector3::UnitZ() * current_foot_height_from_world, hrp::rotFromRpy(hrp::to_Vector3(m_baseRpy.data)));
@@ -487,78 +513,89 @@ void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(){
 }
 
 
-void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp::Pose3& rf_ref, const hrp::Pose3& lf_ref, const hrp::Pose3& rh_ref, const hrp::Pose3& lh_ref, const hrp::Pose3& head_ref){
+void WholeBodyMasterSlave::solveFullbodyIK(HumanPose& ref){
     std::vector<IKConstraint> ikc_list;
-    if(has(wbms->wp.use_targets, "com")){
-        IKConstraint tmp;
-        tmp.target_link_name = "WAIST";
-        tmp.localPos = hrp::Vector3::Zero();
-        tmp.localR = hrp::Matrix33::Identity();
-        tmp.targetPos = hrp::to_Vector3(m_basePos.data);// will be ignored by selection_vec
-        tmp.targetRpy = com_ref.rpy();
-        tmp.constraint_weight << 0,0,0,1e-1,1e-1,1e-1;
-        tmp.rot_precision = deg2rad(3);
-        ikc_list.push_back(tmp);
-    }else{
-      IKConstraint tmp;
-      tmp.target_link_name = "WAIST";
-      tmp.localPos = hrp::Vector3::Zero();
-      tmp.localR = hrp::Matrix33::Identity();
-      tmp.targetPos = hrp::to_Vector3(m_basePos.data);// will be ignored by selection_vec
-      tmp.targetRpy = com_ref.rpy();
-      tmp.constraint_weight << 1,1,1,1,1,1;
-      tmp.rot_precision = deg2rad(3);
-      ikc_list.push_back(tmp);
-    }
-    if(has(wbms->wp.use_targets, "rleg")){
-        IKConstraint tmp;
-        tmp.target_link_name = ee_ikc_map["rleg"].target_link_name;
-        tmp.localPos = ee_ikc_map["rleg"].localPos;
-        tmp.localR = ee_ikc_map["rleg"].localR;
-        tmp.targetPos = rf_ref.p;
-        tmp.targetRpy = rf_ref.rpy();
-        tmp.constraint_weight = wbms->rp_ref_out.tgt[rf].is_contact() ? hrp::dvector6::Constant(3) : hrp::dvector6::Constant(0.1);
-        ikc_list.push_back(tmp);
-    }
-    if(has(wbms->wp.use_targets, "lleg")){
-        IKConstraint tmp;
-        tmp.target_link_name = ee_ikc_map["lleg"].target_link_name;
-        tmp.localPos = ee_ikc_map["lleg"].localPos;
-        tmp.localR = ee_ikc_map["lleg"].localR;
-        tmp.targetPos = lf_ref.p;
-        tmp.targetRpy = lf_ref.rpy();
-        tmp.constraint_weight = wbms->rp_ref_out.tgt[lf].is_contact() ? hrp::dvector6::Constant(3) : hrp::dvector6::Constant(0.1);
-        ikc_list.push_back(tmp);
-    }
-    if(has(wbms->wp.use_targets, "rarm")){
-        IKConstraint tmp;
-        tmp.target_link_name = ee_ikc_map["rarm"].target_link_name;
-        tmp.localPos = ee_ikc_map["rarm"].localPos;
-        tmp.localR = ee_ikc_map["rarm"].localR;
-        tmp.targetPos = rh_ref.p;
-        tmp.targetRpy = rh_ref.rpy();
-        tmp.constraint_weight = hrp::dvector6::Constant(0.1);
-        tmp.pos_precision = 3e-3;
-        tmp.rot_precision = deg2rad(3);
-        ikc_list.push_back(tmp);
-    }
-    if(has(wbms->wp.use_targets, "larm")){
-        IKConstraint tmp;
-        tmp.target_link_name = ee_ikc_map["larm"].target_link_name;
-        tmp.localPos = ee_ikc_map["larm"].localPos;
-        tmp.localR = ee_ikc_map["larm"].localR;
-        tmp.targetPos = lh_ref.p;
-        tmp.targetRpy = lh_ref.rpy();
-        tmp.constraint_weight = hrp::dvector6::Constant(0.1);
-        tmp.pos_precision = 3e-3;
-        tmp.rot_precision = deg2rad(3);
-        ikc_list.push_back(tmp);
+    if(wbms->legged){ // free baselink, lleg, rleg, larm, rarm setting
+        {
+            IKConstraint tmp;
+            tmp.target_link_name    = fik->m_robot->rootLink()->name;
+            tmp.localPos            = hrp::Vector3::Zero();
+            tmp.localR              = hrp::Matrix33::Identity();
+            tmp.targetPos           = hrp::to_Vector3(m_basePos.data);// will be ignored by selection_vec
+            tmp.targetRpy           = ref.stgt("com").abs.rpy();
+            tmp.constraint_weight   << 0, 0, 0, 0.1, 0.1, 0.1;
+            tmp.rot_precision       = deg2rad(3);
+            ikc_list.push_back(tmp);
+        }
+        for(auto leg : {"lleg","rleg"}){
+            if(has(wbms->wp.use_targets, leg)){
+                IKConstraint tmp;
+                tmp.target_link_name    = ee_ikc_map[leg].target_link_name;
+                tmp.localPos            = ee_ikc_map[leg].localPos;
+                tmp.localR              = ee_ikc_map[leg].localR;
+                tmp.targetPos           = ref.stgt(leg).abs.p;
+                tmp.targetRpy           = ref.stgt(leg).abs.rpy();
+                tmp.constraint_weight   = wbms->rp_ref_out.tgt[rf].is_contact() ? hrp::dvector6::Constant(3) : hrp::dvector6::Constant(0.1);
+                ikc_list.push_back(tmp);
+            }
+        }
+        for(auto arm : {"larm","rarm"}){
+            if(has(wbms->wp.use_targets, arm)){
+                IKConstraint tmp;
+                tmp.target_link_name    = ee_ikc_map[arm].target_link_name;
+                tmp.localPos            = ee_ikc_map[arm].localPos;
+                tmp.localR              = ee_ikc_map[arm].localR;
+                tmp.targetPos           = ref.stgt(arm).abs.p;
+                tmp.targetRpy           = ref.stgt(arm).abs.rpy();
+                tmp.constraint_weight   = hrp::dvector6::Constant(0.1);
+                tmp.pos_precision       = 3e-3;
+                tmp.rot_precision       = deg2rad(3);
+                ikc_list.push_back(tmp);
+            }
+        }
+        if(has(wbms->wp.use_targets, "com")){
+            IKConstraint tmp;
+            tmp.target_link_name    = "COM";
+            tmp.localPos            = hrp::Vector3::Zero();
+            tmp.localR              = hrp::Matrix33::Identity();
+            tmp.targetPos           = ref.stgt("com").abs.p + static_balancing_com_offset;// COM height will not be constraint
+            tmp.targetRpy           = hrp::Vector3::Zero();//reference angular momentum
+            tmp.constraint_weight   << 3,3,0.01,0,0,0;
+            ikc_list.push_back(tmp);
+        }
+    }else{ // fixed baselink, larm, rarm setting
+        {
+            IKConstraint tmp;
+            tmp.target_link_name    = fik->m_robot->rootLink()->name;
+            tmp.localPos            = hrp::Vector3::Zero();
+            tmp.localR              = hrp::Matrix33::Identity();
+            tmp.targetPos           = hrp::to_Vector3(m_basePos.data);// will be ignored by selection_vec
+            tmp.targetRpy           = hrp::Vector3::Zero();
+            tmp.constraint_weight   << hrp::dvector6::Constant(1);
+            tmp.rot_precision       = deg2rad(3);
+            ikc_list.push_back(tmp);
+        }
+        for(auto arm : {"larm","rarm"}){
+            if(has(wbms->wp.use_targets, arm)){
+                IKConstraint tmp;
+                tmp.target_link_name    = ee_ikc_map[arm].target_link_name;
+                tmp.localPos            = ee_ikc_map[arm].localPos;
+                tmp.localR              = ee_ikc_map[arm].localR;
+                tmp.targetPos           = ref.stgt(arm).abs.p;
+                tmp.targetRpy           = ref.stgt(arm).abs.rpy();
+                tmp.constraint_weight   << 1, 1, 1, 0.1, 0.1, 0.1;
+                tmp.pos_precision       = 3e-3;
+                tmp.rot_precision       = deg2rad(3);
+                ikc_list.push_back(tmp);
+            }
+        }
     }
+    // common head setting
     if(has(wbms->wp.use_targets, "head")){
         if(fik->m_robot->link("HEAD_JOINT1") != NULL){
             IKConstraint tmp;
             tmp.target_link_name = "HEAD_JOINT1";
-            tmp.targetRpy = head_ref.rpy();
+            tmp.targetRpy = ref.stgt("head").abs.rpy();
             tmp.constraint_weight << 0,0,0,0,0.1,0.1;
             tmp.rot_precision = deg2rad(1);
             ikc_list.push_back(tmp);
@@ -566,7 +603,7 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         if(fik->m_robot->link("HEAD_P") != NULL){
             IKConstraint tmp;
             tmp.target_link_name = "HEAD_P";
-            tmp.targetRpy = head_ref.rpy();
+            tmp.targetRpy = ref.stgt("head").abs.rpy();
             tmp.constraint_weight << 0,0,0,0,0.1,0.1;
             tmp.rot_precision = deg2rad(1);
             ikc_list.push_back(tmp);
@@ -625,18 +662,6 @@ void WholeBodyMasterSlave::solveFullbodyIK(const hrp::Pose3& com_ref, const hrp:
         }
     }
 
-
-    if(has(wbms->wp.use_targets, "com")){
-        IKConstraint tmp;
-        tmp.target_link_name = "COM";
-        tmp.localPos = hrp::Vector3::Zero();
-        tmp.localR = hrp::Matrix33::Identity();
-        tmp.targetPos = com_ref.p + static_balancing_com_offset;// COM height will not be constraint
-        tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum
-        tmp.constraint_weight << 3,3,0.01,0,0,0;
-        ikc_list.push_back(tmp);
-    }
-
     if( fik->m_robot->link("CHEST_JOINT0") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT0")->jointId) = 1e3;//JAXON
     if( fik->m_robot->link("CHEST_JOINT1") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT1")->jointId) = 1e3;
 //    if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 10;
@@ -751,7 +776,7 @@ void WholeBodyMasterSlave::smoothingJointAngles(hrp::BodyPtr _robot, hrp::BodyPt
 bool WholeBodyMasterSlave::startWholeBodyMasterSlave(){
     if(mode.now() == MODE_IDLE){
         RTC_INFO_STREAM("startWholeBodyMasterSlave");
-        mode.setNextMode(MODE_SYNC_TO_HC);
+        mode.setNextMode(MODE_SYNC_TO_WBMS);
         return true;
     }else{
         RTC_WARN_STREAM("Invalid context to startWholeBodyMasterSlave");
diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
index de1b622b8ae..b2306fbf34c 100644
--- a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
+++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h
@@ -22,7 +22,7 @@
 
 //#define USE_DEBUG_PORT
 
-enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_HC, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE};
+enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE};
 
 class ControlMode{
     private:
@@ -32,10 +32,10 @@ class ControlMode{
         ~ControlMode(){}
         bool setNextMode(const mode_enum _request){
             switch(_request){
-                case MODE_SYNC_TO_HC:
-                    if(current == MODE_IDLE){ next = MODE_SYNC_TO_HC; return true; }else{ return false; }
+                case MODE_SYNC_TO_WBMS:
+                    if(current == MODE_IDLE){ next = MODE_SYNC_TO_WBMS; return true; }else{ return false; }
                 case MODE_WBMS:
-                    if(current == MODE_SYNC_TO_HC || current == MODE_PAUSE ){ next = MODE_WBMS; return true; }else{ return false; }
+                    if(current == MODE_SYNC_TO_WBMS || current == MODE_PAUSE ){ next = MODE_WBMS; return true; }else{ return false; }
                 case MODE_PAUSE:
                     if(current == MODE_WBMS){ next = MODE_PAUSE; return true; }else{ return false; }
                 case MODE_SYNC_TO_IDLE:
@@ -49,8 +49,8 @@ class ControlMode{
         void update(){ previous = current; current = next; }
         mode_enum now(){ return current; }
         mode_enum pre(){ return previous; }
-        bool isRunning(){ return (current==MODE_SYNC_TO_HC) || (current==MODE_WBMS) || (current==MODE_PAUSE) || (current==MODE_SYNC_TO_IDLE) ;}
-        bool isInitialize(){ return (previous==MODE_IDLE) && (current==MODE_SYNC_TO_HC) ;}
+        bool isRunning(){ return (current==MODE_SYNC_TO_WBMS) || (current==MODE_WBMS) || (current==MODE_PAUSE) || (current==MODE_SYNC_TO_IDLE) ;}
+        bool isInitialize(){ return (previous==MODE_IDLE) && (current==MODE_SYNC_TO_WBMS) ;}
 };
 
 namespace hrp{
@@ -179,10 +179,10 @@ class WholeBodyMasterSlave : public RTC::DataFlowComponentBase{
         std::vector<std::string> tgt_names;
 
         RTC::ReturnCode_t setupEEIKConstraintFromConf(std::map<std::string, IKConstraint>& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop);
-        void solveFullbodyIK(const hrp::Pose3& com_ref, const hrp::Pose3& rf_ref, const hrp::Pose3& lf_ref, const hrp::Pose3& rh_ref, const hrp::Pose3& lh_ref, const hrp::Pose3& head_ref);
+        void solveFullbodyIK(HumanPose& ref);
         void processTransition();
         void preProcessForWholeBodyMasterSlave();
-        void processWholeBodyMasterSlave(const HumanPose& pose_ref);
+        void processWholeBodyMasterSlave(const HumanPose& ref);
         void smoothingJointAngles(hrp::BodyPtr _robot, hrp::BodyPtr _robot_safe);
         bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; }
         void addTimeReport(const std::string& prefix){
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 44ac4707b84..6f16e91b473 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -76,19 +76,18 @@ class HumanPose{
         std::vector<PoseTGT> tgt;
         HumanPose(){ tgt.resize(num_pose_tgt); }
         ~HumanPose(){cerr<<"HumanPose destructed"<<endl;}
-        void reset(){
-            for(std::vector<PoseTGT>::iterator it = tgt.begin(); it != tgt.end(); it++){ it->reset(); }
-        }
-        static void hp_printf(const HumanPose& in) {
-            const std::string pcatgt[] = {"c","rf","lf","rh","lh","hd","z"}, wcatgt[] = {"rw","lw"};
-            for(int i=0;i<num_pose_tgt;i++){ fprintf(stderr,"\x1b[31m%s\x1b[39m%+05.2f %+05.2f %+05.2f ",pcatgt[i].c_str(),in.tgt[i].abs.p(X),in.tgt[i].abs.p(Y),in.tgt[i].abs.p(Z)); }
-            printf("\n");
-        }
-        PoseTGT&                        foot    (const int lr)      { assert(lr == R || lr == L); return (lr == R) ? tgt[rf] : tgt[lf]; }
-        const PoseTGT&                  foot    (const int lr) const{ assert(lr == R || lr == L); return (lr == R) ? tgt[rf] : tgt[lf]; }
-        PoseTGT&                        hand    (const int lr)      { assert(lr == R || lr == L); return (lr == R) ? tgt[rh] : tgt[lh]; }
-        const PoseTGT&                  hand    (const int lr) const{ assert(lr == R || lr == L); return (lr == R) ? tgt[rh] : tgt[lh]; }
-        PoseTGT& tgt_by_str(const std::string ln){
+        void reset(){ for(std::vector<PoseTGT>::iterator it = tgt.begin(); it != tgt.end(); it++){ it->reset(); }  }
+        friend ostream& operator<<(ostream& os, const HumanPose& in){
+            const std::vector<std::string> short_ns = {"c","rf","lf","rh","lh","hd","z"};
+            os << std::fixed << std::setprecision(1);
+            for (int i=0; i<in.tgt.size(); i++){ os << "\x1b[31m" << short_ns[i] << "\x1b[39m " << in.tgt[i].abs.p.transpose() <<" "; }
+            return os;
+        }
+        PoseTGT&        foot(const int lr)      { assert(lr == R || lr == L); return (lr == R) ? tgt[rf] : tgt[lf]; }
+        const PoseTGT&  foot(const int lr) const{ assert(lr == R || lr == L); return (lr == R) ? tgt[rf] : tgt[lf]; }
+        PoseTGT&        hand(const int lr)      { assert(lr == R || lr == L); return (lr == R) ? tgt[rh] : tgt[lh]; }
+        const PoseTGT&  hand(const int lr) const{ assert(lr == R || lr == L); return (lr == R) ? tgt[rh] : tgt[lh]; }
+        PoseTGT&  stgt(const std::string ln){
             assert(ln == "com" || ln == "head" || ln == "lleg" || ln == "rleg" || ln == "larm" || ln == "rarm" );
             if      (ln == "lleg"){ return tgt[lf]; }
             else if (ln == "rleg"){ return tgt[rf]; }
@@ -97,7 +96,6 @@ class HumanPose{
             else if (ln == "com" ){ return tgt[com]; }
             else                  { return tgt[head]; }
         }
-        void print() const { hp_printf(*this); }
 };
 
 namespace hrp{
@@ -351,6 +349,7 @@ class WBMSCore{
         double com_filter_cutoff_hz_old;
     public:
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+        bool legged;
         double H_cur;
         HumanPose hp_wld_raw, rp_ref_out;
         hrp::Pose3 baselinkpose;
@@ -450,21 +449,20 @@ class WBMSCore{
                 }
                 lock_both_feet_by_com_h = false;
             }
-            std::string LR_BOOL_TO_STR(const bool in[LR]){
+            static std::string LR_BOOL_TO_STR(const bool in[LR]){
                 std::string ret;
                 ret += (in[L] ? "L":"");
                 ret += (in[R] ? "R":"");
                 return ret;
             }
-            std::string str(){
-                std::stringstream ret;
-                ret << "LOCK_FOOT_BY_REF_ZMP(" << LR_BOOL_TO_STR(lock_foot_by_ref_zmp) <<") ";
-                ret << "LOCK_FOOT_BY_ACT_ZMP(" << LR_BOOL_TO_STR(lock_foot_by_act_zmp) <<") ";
-                ret << "LOCK_FOOT_BY_REF_CP("  << LR_BOOL_TO_STR(lock_foot_by_ref_cp) <<") ";
-                ret << "LOCK_FOOT_BY_ACT_CP("  << LR_BOOL_TO_STR(lock_foot_by_act_cp) <<") ";
-                ret << (lock_both_feet_by_com_h ? "LOCK_BOTH_FEET_BY_COM_H " : "");
-                ret << "AUTO_COM_POS_TGT(" << auto_com_pos_tgt << ") ";
-                return ret.str();
+            friend ostream& operator<<(ostream& os, const WBMSStates& in){
+                os << "LOCK_FOOT_BY_REF_ZMP(\x1b[31m" << LR_BOOL_TO_STR(in.lock_foot_by_ref_zmp) <<"\x1b[39m) ";
+                os << "LOCK_FOOT_BY_ACT_ZMP(\x1b[31m" << LR_BOOL_TO_STR(in.lock_foot_by_act_zmp) <<"\x1b[39m) ";
+                os << "LOCK_FOOT_BY_REF_CP(\x1b[31m"  << LR_BOOL_TO_STR(in.lock_foot_by_ref_cp)  <<"\x1b[39m) ";
+                os << "LOCK_FOOT_BY_ACT_CP(\x1b[31m"  << LR_BOOL_TO_STR(in.lock_foot_by_act_cp)  <<"\x1b[39m) ";
+                os << (in.lock_both_feet_by_com_h ? "LOCK_BOTH_FEET_BY_COM_H " : "");
+                os << "AUTO_COM_POS_TGT(\x1b[31m" << in.auto_com_pos_tgt << "\x1b[39m) ";
+                return os;
             }
         } ws;
 
@@ -472,6 +470,7 @@ class WBMSCore{
             DT = dt;
             HZ = (int)(1.0/DT);
             loop = 0;
+            legged = true;
             com_old = com_oldold = comacc = hrp::Vector3::Zero();
             cp_dec = cp_acc = hrp::Vector3::Zero();
             acc4zmp_v_filters.resize(XYZ);
@@ -534,33 +533,35 @@ class WBMSCore{
         }
         void update(){//////////  メインループ  ////////////
             convertHumanToRobot                 (hp_wld_raw, rp_ref_out);
-            setAutoCOMMode                      (hp_wld_raw, rp_ref_out_old, rp_ref_out);
-
-            lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
-            limitEEWorkspace                    (rp_ref_out_old, rp_ref_out);
-            setFootContactPoseByGoContact       (rp_ref_out_old, rp_ref_out);
-            limitFootVelNearGround              (rp_ref_out_old, rp_ref_out);
-            avoidFootSinkIntoFloor              (rp_ref_out);
-
-            applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, rp_ref_out.tgt[com].abs.p);
-            applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
-            com_forcp_ref = rp_ref_out.tgt[com].abs.p.head(XY);
-            static hrp::Vector2 com_forcp_ref_old;
-            com_vel_forcp_ref = (com_forcp_ref - com_forcp_ref_old)/DT;
-            com_forcp_ref_old = com_forcp_ref;
-            applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
-            applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, rp_ref_out.tgt[com].abs.p);
-            applyZMPCalcFromCOM                 (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);//結局STに送るZMPは最終段で計算するからこれ意味ない
-            H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[lf].abs.p(Z));
-            rp_ref_out_old = rp_ref_out;
-
-            ///// COMだけはフィルターいるか・・・
-            if(is_initial_loop || wp.com_filter_cutoff_hz != com_filter_cutoff_hz_old){
-                com_filter.setParameter(wp.com_filter_cutoff_hz, HZ, Q_NOOVERSHOOT);
-                com_filter.reset(rp_ref_out.tgt[com].abs.p);
-                com_filter_cutoff_hz_old = wp.com_filter_cutoff_hz;
+            if(legged){
+                setAutoCOMMode                      (hp_wld_raw, rp_ref_out_old, rp_ref_out);
+
+                lockSwingFootIfZMPOutOfSupportFoot  (rp_ref_out_old, rp_ref_out);//
+                limitEEWorkspace                    (rp_ref_out_old, rp_ref_out);
+                setFootContactPoseByGoContact       (rp_ref_out_old, rp_ref_out);
+                limitFootVelNearGround              (rp_ref_out_old, rp_ref_out);
+                avoidFootSinkIntoFloor              (rp_ref_out);
+
+                applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, rp_ref_out.tgt[com].abs.p);
+                applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから
+                com_forcp_ref = rp_ref_out.tgt[com].abs.p.head(XY);
+                static hrp::Vector2 com_forcp_ref_old;
+                com_vel_forcp_ref = (com_forcp_ref - com_forcp_ref_old)/DT;
+                com_forcp_ref_old = com_forcp_ref;
+                applyCOMStateLimitByCapturePoint    (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, com_CP_ref_old, rp_ref_out.tgt[com].abs.p);
+                applyCOMToSupportRegionLimit        (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, rp_ref_out.tgt[com].abs.p);
+                applyZMPCalcFromCOM                 (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);//結局STに送るZMPは最終段で計算するからこれ意味ない
+                H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[lf].abs.p(Z));
+                rp_ref_out_old = rp_ref_out;
+
+                ///// COMだけはフィルターいるか・・・
+                if(is_initial_loop || wp.com_filter_cutoff_hz != com_filter_cutoff_hz_old){
+                    com_filter.setParameter(wp.com_filter_cutoff_hz, HZ, Q_NOOVERSHOOT);
+                    com_filter.reset(rp_ref_out.tgt[com].abs.p);
+                    com_filter_cutoff_hz_old = wp.com_filter_cutoff_hz;
+                }
+                rp_ref_out.tgt[com].abs.p = com_filter.passFilter(rp_ref_out.tgt[com].abs.p);
             }
-            rp_ref_out.tgt[com].abs.p = com_filter.passFilter(rp_ref_out.tgt[com].abs.p);
             loop++;
             is_initial_loop = false;
         }
@@ -677,12 +678,12 @@ class WBMSCore{
                     out.foot(lr).cnt.R = hrp::rotFromRpy(0, 0, hrp::rpyFromRot(out.foot(lr).abs.R)(y));  
                     LIMIT_MIN( out.foot(lr).abs.p(Z), out.foot(lr).cnt.p(Z)+wp.swing_foot_height_offset);
                 }
-                if(loop%500==0){
-                    dbg(lr);
-                    dbg(out.foot(lr).cnt.p(Z));
-                    dbg(out.foot(lr).abs.p(Z));
-                    dbg(act_rs.act_foot_pose[lr].p(Z));
-                }
+                // if(loop%500==0){
+                //     dbg(lr);
+                //     dbg(out.foot(lr).cnt.p(Z));
+                //     dbg(out.foot(lr).abs.p(Z));
+                //     dbg(act_rs.act_foot_pose[lr].p(Z));
+                // }
             }
         }
         void limitFootVelNearGround(const HumanPose& old, HumanPose& out){

From 028957ef966a15df202bcd036251218b0d199456 Mon Sep 17 00:00:00 2001
From: JAXONRED <jaxonred@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 26 Mar 2021 21:06:22 +0900
Subject: [PATCH 217/217] write_power_command sleep and floor height detection
 clear count

---
 rtc/RobotHardware/robot.cpp          | 5 +++++
 rtc/WholeBodyMasterSlave/wbms_core.h | 2 +-
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp
index 2027dd405db..8f982efec15 100644
--- a/rtc/RobotHardware/robot.cpp
+++ b/rtc/RobotHardware/robot.cpp
@@ -466,7 +466,12 @@ bool robot::power(int jid, bool turnon)
             }
         } else       
             for (unsigned int i=0; i<numJoints(); i++)
+            {
                 write_power_command(i, com);
+                //usleep(100000);// OK
+                usleep(10000);// OK
+                //usleep(1000);// OK
+            }
     } else {
         if (com == OFF) {
             write_pgain(jid, 0);
diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h
index 7e708301a0f..5219d099e7f 100644
--- a/rtc/WholeBodyMasterSlave/wbms_core.h
+++ b/rtc/WholeBodyMasterSlave/wbms_core.h
@@ -618,7 +618,7 @@ class WBMSCore{
                         is_locked[lr] = true;
                     }
                     if(is_locked[lr] && fabs(act_rs.act_foot_wrench[lr](fz)) < wp.auto_floor_h_reset_fz){
-                        if(cnt_for_clear[lr]++ > HZ * 0.5){
+                        if(cnt_for_clear[lr]++ > HZ * 2.0){
                             out.foot(lr).cnt.p(Z) = out.foot(lr).offs.p(Z);
                             is_locked[lr] = false;
                             cnt_for_clear[lr] = 0;